3592 files changed, 268181 insertions, 171584 deletions

diff --git a/drivers/Kconfig b/drivers/Kconfig
index d236aef7e59..bfc918633fd 100644
--- a/drivers/Kconfig
+++ b/drivers/Kconfig
@@ -140,4 +140,12 @@ source "drivers/virt/Kconfig"
 
 source "drivers/devfreq/Kconfig"
 
+source "drivers/extcon/Kconfig"
+
+source "drivers/memory/Kconfig"
+
+source "drivers/iio/Kconfig"
+
+source "drivers/vme/Kconfig"
+
 endmenu
diff --git a/drivers/Makefile b/drivers/Makefile
index 95952c82bf1..2ba29ffef2c 100644
--- a/drivers/Makefile
+++ b/drivers/Makefile
@@ -18,7 +18,7 @@ obj-$(CONFIG_SFI)		+= sfi/
 # PnP must come after ACPI since it will eventually need to check if acpi
 # was used and do nothing if so
 obj-$(CONFIG_PNP)		+= pnp/
-obj-$(CONFIG_ARM_AMBA)		+= amba/
+obj-y				+= amba/
 # Many drivers will want to use DMA so this has to be made available
 # really early.
 obj-$(CONFIG_DMA_ENGINE)	+= dma/
@@ -92,7 +92,6 @@ obj-$(CONFIG_BT)		+= bluetooth/
 obj-$(CONFIG_ACCESSIBILITY)	+= accessibility/
 obj-$(CONFIG_ISDN)		+= isdn/
 obj-$(CONFIG_EDAC)		+= edac/
-obj-$(CONFIG_MCA)		+= mca/
 obj-$(CONFIG_EISA)		+= eisa/
 obj-y				+= lguest/
 obj-$(CONFIG_CPU_FREQ)		+= cpufreq/
@@ -134,3 +133,7 @@ obj-$(CONFIG_VIRT_DRIVERS)	+= virt/
 obj-$(CONFIG_HYPERV)		+= hv/
 
 obj-$(CONFIG_PM_DEVFREQ)	+= devfreq/
+obj-$(CONFIG_EXTCON)		+= extcon/
+obj-$(CONFIG_MEMORY)		+= memory/
+obj-$(CONFIG_IIO)		+= iio/
+obj-$(CONFIG_VME_BUS)		+= vme/
diff --git a/drivers/acpi/bus.c b/drivers/acpi/bus.c
index 3263b68cdfa..3188da3df8d 100644
--- a/drivers/acpi/bus.c
+++ b/drivers/acpi/bus.c
@@ -250,6 +250,10 @@ static int __acpi_bus_set_power(struct acpi_device *device, int state)
 		return -ENODEV;
 	}
 
+	/* For D3cold we should execute _PS3, not _PS4. */
+	if (state == ACPI_STATE_D3_COLD)
+		object_name[3] = '3';
+
 	/*
 	 * Transition Power
 	 * ----------------
diff --git a/drivers/acpi/glue.c b/drivers/acpi/glue.c
index 29a4a5c8ee0..1564e0927c2 100644
--- a/drivers/acpi/glue.c
+++ b/drivers/acpi/glue.c
@@ -69,6 +69,7 @@ static struct acpi_bus_type *acpi_get_bus_type(struct bus_type *type)
 	up_read(&bus_type_sem);
 	return ret;
 }
+EXPORT_SYMBOL_GPL(register_acpi_bus_type);
 
 static int acpi_find_bridge_device(struct device *dev, acpi_handle * handle)
 {
@@ -85,6 +86,7 @@ static int acpi_find_bridge_device(struct device *dev, acpi_handle * handle)
 	up_read(&bus_type_sem);
 	return ret;
 }
+EXPORT_SYMBOL_GPL(unregister_acpi_bus_type);
 
 /* Get device's handler per its address under its parent */
 struct acpi_find_child {
diff --git a/drivers/acpi/pci_link.c b/drivers/acpi/pci_link.c
index 4a29763b8eb..a12808259df 100644
--- a/drivers/acpi/pci_link.c
+++ b/drivers/acpi/pci_link.c
@@ -720,21 +720,21 @@ static int acpi_pci_link_add(struct acpi_device *device)
 	       acpi_device_bid(device));
 	for (i = 0; i < link->irq.possible_count; i++) {
 		if (link->irq.active == link->irq.possible[i]) {
-			printk(" *%d", link->irq.possible[i]);
+			printk(KERN_CONT " *%d", link->irq.possible[i]);
 			found = 1;
 		} else
-			printk(" %d", link->irq.possible[i]);
+			printk(KERN_CONT " %d", link->irq.possible[i]);
 	}
 
-	printk(")");
+	printk(KERN_CONT ")");
 
 	if (!found)
-		printk(" *%d", link->irq.active);
+		printk(KERN_CONT " *%d", link->irq.active);
 
 	if (!link->device->status.enabled)
-		printk(", disabled.");
+		printk(KERN_CONT ", disabled.");
 
-	printk("\n");
+	printk(KERN_CONT "\n");
 
 	list_add_tail(&link->list, &acpi_link_list);
 
diff --git a/drivers/acpi/power.c b/drivers/acpi/power.c
index 330bb4d7585..0500f719f63 100644
--- a/drivers/acpi/power.c
+++ b/drivers/acpi/power.c
@@ -660,7 +660,7 @@ int acpi_power_on_resources(struct acpi_device *device, int state)
 
 int acpi_power_transition(struct acpi_device *device, int state)
 {
-	int result;
+	int result = 0;
 
 	if (!device || (state < ACPI_STATE_D0) || (state > ACPI_STATE_D3_COLD))
 		return -EINVAL;
@@ -679,8 +679,11 @@ int acpi_power_transition(struct acpi_device *device, int state)
 	 * (e.g. so the device doesn't lose power while transitioning).  Then,
 	 * we dereference all power resources used in the current list.
 	 */
-	result = acpi_power_on_list(&device->power.states[state].resources);
-	if (!result)
+	if (state < ACPI_STATE_D3_COLD)
+		result = acpi_power_on_list(
+			&device->power.states[state].resources);
+
+	if (!result && device->power.state < ACPI_STATE_D3_COLD)
 		acpi_power_off_list(
 			&device->power.states[device->power.state].resources);
 
diff --git a/drivers/acpi/scan.c b/drivers/acpi/scan.c
index 7417267e88f..85cbfdccc97 100644
--- a/drivers/acpi/scan.c
+++ b/drivers/acpi/scan.c
@@ -908,6 +908,10 @@ static int acpi_bus_get_power_flags(struct acpi_device *device)
 	device->power.states[ACPI_STATE_D3].flags.valid = 1;
 	device->power.states[ACPI_STATE_D3].power = 0;
 
+	/* Set D3cold's explicit_set flag if _PS3 exists. */
+	if (device->power.states[ACPI_STATE_D3_HOT].flags.explicit_set)
+		device->power.states[ACPI_STATE_D3_COLD].flags.explicit_set = 1;
+
 	acpi_bus_init_power(device);
 
 	return 0;
diff --git a/drivers/acpi/sleep.c b/drivers/acpi/sleep.c
index 06139005c4d..ebaa0459323 100644
--- a/drivers/acpi/sleep.c
+++ b/drivers/acpi/sleep.c
@@ -889,7 +889,7 @@ int __init acpi_sleep_init(void)
 		status = acpi_get_sleep_type_data(i, &type_a, &type_b);
 		if (ACPI_SUCCESS(status)) {
 			sleep_states[i] = 1;
-			printk(" S%d", i);
+			printk(KERN_CONT " S%d", i);
 		}
 	}
 
@@ -903,7 +903,7 @@ int __init acpi_sleep_init(void)
 		hibernation_set_ops(old_suspend_ordering ?
 			&acpi_hibernation_ops_old : &acpi_hibernation_ops);
 		sleep_states[ACPI_STATE_S4] = 1;
-		printk(" S4");
+		printk(KERN_CONT " S4");
 		if (!nosigcheck) {
 			acpi_get_table(ACPI_SIG_FACS, 1,
 				(struct acpi_table_header **)&facs);
@@ -916,11 +916,11 @@ int __init acpi_sleep_init(void)
 	status = acpi_get_sleep_type_data(ACPI_STATE_S5, &type_a, &type_b);
 	if (ACPI_SUCCESS(status)) {
 		sleep_states[ACPI_STATE_S5] = 1;
-		printk(" S5");
+		printk(KERN_CONT " S5");
 		pm_power_off_prepare = acpi_power_off_prepare;
 		pm_power_off = acpi_power_off;
 	}
-	printk(")\n");
+	printk(KERN_CONT ")\n");
 	/*
 	 * Register the tts_notifier to reboot notifier list so that the _TTS
 	 * object can also be evaluated when the system enters S5.
diff --git a/drivers/acpi/utils.c b/drivers/acpi/utils.c
index b002a471c5d..adbbc1c80a2 100644
--- a/drivers/acpi/utils.c
+++ b/drivers/acpi/utils.c
@@ -382,3 +382,33 @@ acpi_evaluate_reference(acpi_handle handle,
 }
 
 EXPORT_SYMBOL(acpi_evaluate_reference);
+
+acpi_status
+acpi_get_physical_device_location(acpi_handle handle, struct acpi_pld *pld)
+{
+	acpi_status status;
+	struct acpi_buffer buffer = { ACPI_ALLOCATE_BUFFER, NULL };
+	union acpi_object *output;
+
+	status = acpi_evaluate_object(handle, "_PLD", NULL, &buffer);
+
+	if (ACPI_FAILURE(status))
+		return status;
+
+	output = buffer.pointer;
+
+	if (!output || output->type != ACPI_TYPE_PACKAGE
+	    || !output->package.count
+	    || output->package.elements[0].type != ACPI_TYPE_BUFFER
+	    || output->package.elements[0].buffer.length > sizeof(*pld)) {
+		status = AE_TYPE;
+		goto out;
+	}
+
+	memcpy(pld, output->package.elements[0].buffer.pointer,
+	       output->package.elements[0].buffer.length);
+out:
+	kfree(buffer.pointer);
+	return status;
+}
+EXPORT_SYMBOL(acpi_get_physical_device_location);
diff --git a/drivers/amba/Makefile b/drivers/amba/Makefile
index 40fe74097be..66e81c2f1e3 100644
--- a/drivers/amba/Makefile
+++ b/drivers/amba/Makefile
@@ -1,2 +1,2 @@
-obj-y		+= bus.o
-
+obj-$(CONFIG_ARM_AMBA)		+= bus.o
+obj-$(CONFIG_TEGRA_AHB)		+= tegra-ahb.o
diff --git a/drivers/amba/bus.c b/drivers/amba/bus.c
index cc273226dbd..b7e72851728 100644
--- a/drivers/amba/bus.c
+++ b/drivers/amba/bus.c
@@ -527,9 +527,9 @@ int amba_device_add(struct amba_device *dev, struct resource *parent)
 	if (ret)
 		goto err_release;
 
-	if (dev->irq[0] && dev->irq[0] != NO_IRQ)
+	if (dev->irq[0])
 		ret = device_create_file(&dev->dev, &dev_attr_irq0);
-	if (ret == 0 && dev->irq[1] && dev->irq[1] != NO_IRQ)
+	if (ret == 0 && dev->irq[1])
 		ret = device_create_file(&dev->dev, &dev_attr_irq1);
 	if (ret == 0)
 		return ret;
@@ -543,6 +543,55 @@ int amba_device_add(struct amba_device *dev, struct resource *parent)
 }
 EXPORT_SYMBOL_GPL(amba_device_add);
 
+static struct amba_device *
+amba_aphb_device_add(struct device *parent, const char *name,
+		     resource_size_t base, size_t size, int irq1, int irq2,
+		     void *pdata, unsigned int periphid, u64 dma_mask)
+{
+	struct amba_device *dev;
+	int ret;
+
+	dev = amba_device_alloc(name, base, size);
+	if (!dev)
+		return ERR_PTR(-ENOMEM);
+
+	dev->dma_mask = dma_mask;
+	dev->dev.coherent_dma_mask = dma_mask;
+	dev->irq[0] = irq1;
+	dev->irq[1] = irq2;
+	dev->periphid = periphid;
+	dev->dev.platform_data = pdata;
+	dev->dev.parent = parent;
+
+	ret = amba_device_add(dev, &iomem_resource);
+	if (ret) {
+		amba_device_put(dev);
+		return ERR_PTR(ret);
+	}
+
+	return dev;
+}
+
+struct amba_device *
+amba_apb_device_add(struct device *parent, const char *name,
+		    resource_size_t base, size_t size, int irq1, int irq2,
+		    void *pdata, unsigned int periphid)
+{
+	return amba_aphb_device_add(parent, name, base, size, irq1, irq2, pdata,
+				    periphid, 0);
+}
+EXPORT_SYMBOL_GPL(amba_apb_device_add);
+
+struct amba_device *
+amba_ahb_device_add(struct device *parent, const char *name,
+		    resource_size_t base, size_t size, int irq1, int irq2,
+		    void *pdata, unsigned int periphid)
+{
+	return amba_aphb_device_add(parent, name, base, size, irq1, irq2, pdata,
+				    periphid, ~0ULL);
+}
+EXPORT_SYMBOL_GPL(amba_ahb_device_add);
+
 static void amba_device_initialize(struct amba_device *dev, const char *name)
 {
 	device_initialize(&dev->dev);
diff --git a/drivers/amba/tegra-ahb.c b/drivers/amba/tegra-ahb.c
new file mode 100644
index 00000000000..aa0b1f16052
--- /dev/null
+++ b/drivers/amba/tegra-ahb.c
@@ -0,0 +1,293 @@
+/*
+ * Copyright (c) 2012, NVIDIA CORPORATION.  All rights reserved.
+ * Copyright (C) 2011 Google, Inc.
+ *
+ * Author:
+ *	Jay Cheng <jacheng@nvidia.com>
+ *	James Wylder <james.wylder@motorola.com>
+ *	Benoit Goby <benoit@android.com>
+ *	Colin Cross <ccross@android.com>
+ *	Hiroshi DOYU <hdoyu@nvidia.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/kernel.h>
+#include <linux/module.h>
+#include <linux/platform_device.h>
+#include <linux/io.h>
+
+#define DRV_NAME "tegra-ahb"
+
+#define AHB_ARBITRATION_DISABLE		0x00
+#define AHB_ARBITRATION_PRIORITY_CTRL	0x04
+#define   AHB_PRIORITY_WEIGHT(x)	(((x) & 0x7) << 29)
+#define   PRIORITY_SELECT_USB BIT(6)
+#define   PRIORITY_SELECT_USB2 BIT(18)
+#define   PRIORITY_SELECT_USB3 BIT(17)
+
+#define AHB_GIZMO_AHB_MEM		0x0c
+#define   ENB_FAST_REARBITRATE BIT(2)
+#define   DONT_SPLIT_AHB_WR     BIT(7)
+
+#define AHB_GIZMO_APB_DMA		0x10
+#define AHB_GIZMO_IDE			0x18
+#define AHB_GIZMO_USB			0x1c
+#define AHB_GIZMO_AHB_XBAR_BRIDGE	0x20
+#define AHB_GIZMO_CPU_AHB_BRIDGE	0x24
+#define AHB_GIZMO_COP_AHB_BRIDGE	0x28
+#define AHB_GIZMO_XBAR_APB_CTLR		0x2c
+#define AHB_GIZMO_VCP_AHB_BRIDGE	0x30
+#define AHB_GIZMO_NAND			0x3c
+#define AHB_GIZMO_SDMMC4		0x44
+#define AHB_GIZMO_XIO			0x48
+#define AHB_GIZMO_BSEV			0x60
+#define AHB_GIZMO_BSEA			0x70
+#define AHB_GIZMO_NOR			0x74
+#define AHB_GIZMO_USB2			0x78
+#define AHB_GIZMO_USB3			0x7c
+#define   IMMEDIATE	BIT(18)
+
+#define AHB_GIZMO_SDMMC1		0x80
+#define AHB_GIZMO_SDMMC2		0x84
+#define AHB_GIZMO_SDMMC3		0x88
+#define AHB_MEM_PREFETCH_CFG_X		0xd8
+#define AHB_ARBITRATION_XBAR_CTRL	0xdc
+#define AHB_MEM_PREFETCH_CFG3		0xe0
+#define AHB_MEM_PREFETCH_CFG4		0xe4
+#define AHB_MEM_PREFETCH_CFG1		0xec
+#define AHB_MEM_PREFETCH_CFG2		0xf0
+#define   PREFETCH_ENB	BIT(31)
+#define   MST_ID(x)	(((x) & 0x1f) << 26)
+#define   AHBDMA_MST_ID	MST_ID(5)
+#define   USB_MST_ID	MST_ID(6)
+#define   USB2_MST_ID	MST_ID(18)
+#define   USB3_MST_ID	MST_ID(17)
+#define   ADDR_BNDRY(x)	(((x) & 0xf) << 21)
+#define   INACTIVITY_TIMEOUT(x)	(((x) & 0xffff) << 0)
+
+#define AHB_ARBITRATION_AHB_MEM_WRQUE_MST_ID	0xf8
+
+#define AHB_ARBITRATION_XBAR_CTRL_SMMU_INIT_DONE BIT(17)
+
+static struct platform_driver tegra_ahb_driver;
+
+static const u32 tegra_ahb_gizmo[] = {
+	AHB_ARBITRATION_DISABLE,
+	AHB_ARBITRATION_PRIORITY_CTRL,
+	AHB_GIZMO_AHB_MEM,
+	AHB_GIZMO_APB_DMA,
+	AHB_GIZMO_IDE,
+	AHB_GIZMO_USB,
+	AHB_GIZMO_AHB_XBAR_BRIDGE,
+	AHB_GIZMO_CPU_AHB_BRIDGE,
+	AHB_GIZMO_COP_AHB_BRIDGE,
+	AHB_GIZMO_XBAR_APB_CTLR,
+	AHB_GIZMO_VCP_AHB_BRIDGE,
+	AHB_GIZMO_NAND,
+	AHB_GIZMO_SDMMC4,
+	AHB_GIZMO_XIO,
+	AHB_GIZMO_BSEV,
+	AHB_GIZMO_BSEA,
+	AHB_GIZMO_NOR,
+	AHB_GIZMO_USB2,
+	AHB_GIZMO_USB3,
+	AHB_GIZMO_SDMMC1,
+	AHB_GIZMO_SDMMC2,
+	AHB_GIZMO_SDMMC3,
+	AHB_MEM_PREFETCH_CFG_X,
+	AHB_ARBITRATION_XBAR_CTRL,
+	AHB_MEM_PREFETCH_CFG3,
+	AHB_MEM_PREFETCH_CFG4,
+	AHB_MEM_PREFETCH_CFG1,
+	AHB_MEM_PREFETCH_CFG2,
+	AHB_ARBITRATION_AHB_MEM_WRQUE_MST_ID,
+};
+
+struct tegra_ahb {
+	void __iomem	*regs;
+	struct device	*dev;
+	u32		ctx[0];
+};
+
+static inline u32 gizmo_readl(struct tegra_ahb *ahb, u32 offset)
+{
+	return readl(ahb->regs + offset);
+}
+
+static inline void gizmo_writel(struct tegra_ahb *ahb, u32 value, u32 offset)
+{
+	writel(value, ahb->regs + offset);
+}
+
+#ifdef CONFIG_ARCH_TEGRA_3x_SOC
+static int tegra_ahb_match_by_smmu(struct device *dev, void *data)
+{
+	struct tegra_ahb *ahb = dev_get_drvdata(dev);
+	struct device_node *dn = data;
+
+	return (ahb->dev->of_node == dn) ? 1 : 0;
+}
+
+int tegra_ahb_enable_smmu(struct device_node *dn)
+{
+	struct device *dev;
+	u32 val;
+	struct tegra_ahb *ahb;
+
+	dev = driver_find_device(&tegra_ahb_driver.driver, NULL, dn,
+				 tegra_ahb_match_by_smmu);
+	if (!dev)
+		return -EPROBE_DEFER;
+	ahb = dev_get_drvdata(dev);
+	val = gizmo_readl(ahb, AHB_ARBITRATION_XBAR_CTRL);
+	val |= AHB_ARBITRATION_XBAR_CTRL_SMMU_INIT_DONE;
+	gizmo_writel(ahb, val, AHB_ARBITRATION_XBAR_CTRL);
+	return 0;
+}
+EXPORT_SYMBOL(tegra_ahb_enable_smmu);
+#endif
+
+static int tegra_ahb_suspend(struct device *dev)
+{
+	int i;
+	struct tegra_ahb *ahb = dev_get_drvdata(dev);
+
+	for (i = 0; i < ARRAY_SIZE(tegra_ahb_gizmo); i++)
+		ahb->ctx[i] = gizmo_readl(ahb, tegra_ahb_gizmo[i]);
+	return 0;
+}
+
+static int tegra_ahb_resume(struct device *dev)
+{
+	int i;
+	struct tegra_ahb *ahb = dev_get_drvdata(dev);
+
+	for (i = 0; i < ARRAY_SIZE(tegra_ahb_gizmo); i++)
+		gizmo_writel(ahb, ahb->ctx[i], tegra_ahb_gizmo[i]);
+	return 0;
+}
+
+static UNIVERSAL_DEV_PM_OPS(tegra_ahb_pm,
+			    tegra_ahb_suspend,
+			    tegra_ahb_resume, NULL);
+
+static void tegra_ahb_gizmo_init(struct tegra_ahb *ahb)
+{
+	u32 val;
+
+	val = gizmo_readl(ahb, AHB_GIZMO_AHB_MEM);
+	val |= ENB_FAST_REARBITRATE | IMMEDIATE | DONT_SPLIT_AHB_WR;
+	gizmo_writel(ahb, val, AHB_GIZMO_AHB_MEM);
+
+	val = gizmo_readl(ahb, AHB_GIZMO_USB);
+	val |= IMMEDIATE;
+	gizmo_writel(ahb, val, AHB_GIZMO_USB);
+
+	val = gizmo_readl(ahb, AHB_GIZMO_USB2);
+	val |= IMMEDIATE;
+	gizmo_writel(ahb, val, AHB_GIZMO_USB2);
+
+	val = gizmo_readl(ahb, AHB_GIZMO_USB3);
+	val |= IMMEDIATE;
+	gizmo_writel(ahb, val, AHB_GIZMO_USB3);
+
+	val = gizmo_readl(ahb, AHB_ARBITRATION_PRIORITY_CTRL);
+	val |= PRIORITY_SELECT_USB |
+		PRIORITY_SELECT_USB2 |
+		PRIORITY_SELECT_USB3 |
+		AHB_PRIORITY_WEIGHT(7);
+	gizmo_writel(ahb, val, AHB_ARBITRATION_PRIORITY_CTRL);
+
+	val = gizmo_readl(ahb, AHB_MEM_PREFETCH_CFG1);
+	val &= ~MST_ID(~0);
+	val |= PREFETCH_ENB |
+		AHBDMA_MST_ID |
+		ADDR_BNDRY(0xc) |
+		INACTIVITY_TIMEOUT(0x1000);
+	gizmo_writel(ahb, val, AHB_MEM_PREFETCH_CFG1);
+
+	val = gizmo_readl(ahb, AHB_MEM_PREFETCH_CFG2);
+	val &= ~MST_ID(~0);
+	val |= PREFETCH_ENB |
+		USB_MST_ID |
+		ADDR_BNDRY(0xc) |
+		INACTIVITY_TIMEOUT(0x1000);
+	gizmo_writel(ahb, val, AHB_MEM_PREFETCH_CFG2);
+
+	val = gizmo_readl(ahb, AHB_MEM_PREFETCH_CFG3);
+	val &= ~MST_ID(~0);
+	val |= PREFETCH_ENB |
+		USB3_MST_ID |
+		ADDR_BNDRY(0xc) |
+		INACTIVITY_TIMEOUT(0x1000);
+	gizmo_writel(ahb, val, AHB_MEM_PREFETCH_CFG3);
+
+	val = gizmo_readl(ahb, AHB_MEM_PREFETCH_CFG4);
+	val &= ~MST_ID(~0);
+	val |= PREFETCH_ENB |
+		USB2_MST_ID |
+		ADDR_BNDRY(0xc) |
+		INACTIVITY_TIMEOUT(0x1000);
+	gizmo_writel(ahb, val, AHB_MEM_PREFETCH_CFG4);
+}
+
+static int __devinit tegra_ahb_probe(struct platform_device *pdev)
+{
+	struct resource *res;
+	struct tegra_ahb *ahb;
+	size_t bytes;
+
+	bytes = sizeof(*ahb) + sizeof(u32) * ARRAY_SIZE(tegra_ahb_gizmo);
+	ahb = devm_kzalloc(&pdev->dev, bytes, GFP_KERNEL);
+	if (!ahb)
+		return -ENOMEM;
+
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	if (!res)
+		return -ENODEV;
+	ahb->regs = devm_request_and_ioremap(&pdev->dev, res);
+	if (!ahb->regs)
+		return -EBUSY;
+
+	ahb->dev = &pdev->dev;
+	platform_set_drvdata(pdev, ahb);
+	tegra_ahb_gizmo_init(ahb);
+	return 0;
+}
+
+static int __devexit tegra_ahb_remove(struct platform_device *pdev)
+{
+	return 0;
+}
+
+static const struct of_device_id tegra_ahb_of_match[] __devinitconst = {
+	{ .compatible = "nvidia,tegra30-ahb", },
+	{ .compatible = "nvidia,tegra20-ahb", },
+	{},
+};
+
+static struct platform_driver tegra_ahb_driver = {
+	.probe = tegra_ahb_probe,
+	.remove = __devexit_p(tegra_ahb_remove),
+	.driver = {
+		.name = DRV_NAME,
+		.owner = THIS_MODULE,
+		.of_match_table = tegra_ahb_of_match,
+		.pm = &tegra_ahb_pm,
+	},
+};
+module_platform_driver(tegra_ahb_driver);
+
+MODULE_AUTHOR("Hiroshi DOYU <hdoyu@nvidia.com>");
+MODULE_DESCRIPTION("Tegra AHB driver");
+MODULE_LICENSE("GPL v2");
+MODULE_ALIAS("platform:" DRV_NAME);
diff --git a/drivers/ata/Kconfig b/drivers/ata/Kconfig
index 6bdedd7cca2..2be8ef1d309 100644
--- a/drivers/ata/Kconfig
+++ b/drivers/ata/Kconfig
@@ -416,6 +416,15 @@ config PATA_EFAR
 
 	  If unsure, say N.
 
+config PATA_EP93XX
+	tristate "Cirrus Logic EP93xx PATA support"
+	depends on ARCH_EP93XX
+	help
+	  This option enables support for the PATA controller in
+	  the Cirrus Logic EP9312 and EP9315 ARM CPU.
+
+	  If unsure, say N.
+
 config PATA_HPT366
 	tristate "HPT 366/368 PATA support"
 	depends on PCI
diff --git a/drivers/ata/Makefile b/drivers/ata/Makefile
index 6ece5b7231a..a454a139b1d 100644
--- a/drivers/ata/Makefile
+++ b/drivers/ata/Makefile
@@ -43,6 +43,7 @@ obj-$(CONFIG_PATA_CS5535)	+= pata_cs5535.o
 obj-$(CONFIG_PATA_CS5536)	+= pata_cs5536.o
 obj-$(CONFIG_PATA_CYPRESS)	+= pata_cypress.o
 obj-$(CONFIG_PATA_EFAR)		+= pata_efar.o
+obj-$(CONFIG_PATA_EP93XX)	+= pata_ep93xx.o
 obj-$(CONFIG_PATA_HPT366)	+= pata_hpt366.o
 obj-$(CONFIG_PATA_HPT37X)	+= pata_hpt37x.o
 obj-$(CONFIG_PATA_HPT3X2N)	+= pata_hpt3x2n.o
diff --git a/drivers/ata/ata_generic.c b/drivers/ata/ata_generic.c
index 7df56ec3181..aae115600b7 100644
--- a/drivers/ata/ata_generic.c
+++ b/drivers/ata/ata_generic.c
@@ -177,7 +177,7 @@ static int ata_generic_init_one(struct pci_dev *dev, const struct pci_device_id
 	if ((id->driver_data & ATA_GEN_CLASS_MATCH) && all_generic_ide == 0)
 		return -ENODEV;
 
-	if (id->driver_data & ATA_GEN_INTEL_IDER)
+	if ((id->driver_data & ATA_GEN_INTEL_IDER) && !all_generic_ide)
 		if (!is_intel_ider(dev))
 			return -ENODEV;
 
diff --git a/drivers/ata/ata_piix.c b/drivers/ata/ata_piix.c
index 7857e8fd0a3..3c809bfbccf 100644
--- a/drivers/ata/ata_piix.c
+++ b/drivers/ata/ata_piix.c
@@ -1554,6 +1554,39 @@ static bool piix_broken_system_poweroff(struct pci_dev *pdev)
 	return false;
 }
 
+static int prefer_ms_hyperv = 1;
+module_param(prefer_ms_hyperv, int, 0);
+
+static void piix_ignore_devices_quirk(struct ata_host *host)
+{
+#if IS_ENABLED(CONFIG_HYPERV_STORAGE)
+	static const struct dmi_system_id ignore_hyperv[] = {
+		{
+			/* On Hyper-V hypervisors the disks are exposed on
+			 * both the emulated SATA controller and on the
+			 * paravirtualised drivers.  The CD/DVD devices
+			 * are only exposed on the emulated controller.
+			 * Request we ignore ATA devices on this host.
+			 */
+			.ident = "Hyper-V Virtual Machine",
+			.matches = {
+				DMI_MATCH(DMI_SYS_VENDOR,
+						"Microsoft Corporation"),
+				DMI_MATCH(DMI_PRODUCT_NAME, "Virtual Machine"),
+			},
+		},
+		{ }	/* terminate list */
+	};
+	const struct dmi_system_id *dmi = dmi_first_match(ignore_hyperv);
+
+	if (dmi && prefer_ms_hyperv) {
+		host->flags |= ATA_HOST_IGNORE_ATA;
+		dev_info(host->dev, "%s detected, ATA device ignore set\n",
+			dmi->ident);
+	}
+#endif
+}
+
 /**
  *	piix_init_one - Register PIIX ATA PCI device with kernel services
  *	@pdev: PCI device to register
@@ -1669,6 +1702,9 @@ static int __devinit piix_init_one(struct pci_dev *pdev,
 	}
 	host->flags |= ATA_HOST_PARALLEL_SCAN;
 
+	/* Allow hosts to specify device types to ignore when scanning. */
+	piix_ignore_devices_quirk(host);
+
 	pci_set_master(pdev);
 	return ata_pci_sff_activate_host(host, ata_bmdma_interrupt, sht);
 }
diff --git a/drivers/ata/libata-core.c b/drivers/ata/libata-core.c
index 23763a1ec57..cece3a4d11e 100644
--- a/drivers/ata/libata-core.c
+++ b/drivers/ata/libata-core.c
@@ -1973,6 +1973,12 @@ retry:
 	if (class == ATA_DEV_ATA) {
 		if (!ata_id_is_ata(id) && !ata_id_is_cfa(id))
 			goto err_out;
+		if (ap->host->flags & ATA_HOST_IGNORE_ATA &&
+							ata_id_is_ata(id)) {
+			ata_dev_dbg(dev,
+				"host indicates ignore ATA devices, ignored\n");
+			return -ENOENT;
+		}
 	} else {
 		if (ata_id_is_ata(id))
 			goto err_out;
@@ -4051,6 +4057,7 @@ static const struct ata_blacklist_entry ata_device_blacklist [] = {
 	{ "_NEC DV5800A", 	NULL,		ATA_HORKAGE_NODMA },
 	{ "SAMSUNG CD-ROM SN-124", "N001",	ATA_HORKAGE_NODMA },
 	{ "Seagate STT20000A", NULL,		ATA_HORKAGE_NODMA },
+	{ "2GB ATA Flash Disk", "ADMA428M",	ATA_HORKAGE_NODMA },
 	/* Odd clown on sil3726/4726 PMPs */
 	{ "Config  Disk",	NULL,		ATA_HORKAGE_DISABLE },
 
diff --git a/drivers/ata/libata-eh.c b/drivers/ata/libata-eh.c
index d1fbd59ead1..6d53cf9b3b6 100644
--- a/drivers/ata/libata-eh.c
+++ b/drivers/ata/libata-eh.c
@@ -2047,6 +2047,26 @@ static unsigned int ata_eh_speed_down(struct ata_device *dev,
 }
 
 /**
+ *	ata_eh_worth_retry - analyze error and decide whether to retry
+ *	@qc: qc to possibly retry
+ *
+ *	Look at the cause of the error and decide if a retry
+ * 	might be useful or not.  We don't want to retry media errors
+ *	because the drive itself has probably already taken 10-30 seconds
+ *	doing its own internal retries before reporting the failure.
+ */
+static inline int ata_eh_worth_retry(struct ata_queued_cmd *qc)
+{
+	if (qc->flags & AC_ERR_MEDIA)
+		return 0;	/* don't retry media errors */
+	if (qc->flags & ATA_QCFLAG_IO)
+		return 1;	/* otherwise retry anything from fs stack */
+	if (qc->err_mask & AC_ERR_INVALID)
+		return 0;	/* don't retry these */
+	return qc->err_mask != AC_ERR_DEV;  /* retry if not dev error */
+}
+
+/**
  *	ata_eh_link_autopsy - analyze error and determine recovery action
  *	@link: host link to perform autopsy on
  *
@@ -2120,9 +2140,7 @@ static void ata_eh_link_autopsy(struct ata_link *link)
 			qc->err_mask &= ~(AC_ERR_DEV | AC_ERR_OTHER);
 
 		/* determine whether the command is worth retrying */
-		if (qc->flags & ATA_QCFLAG_IO ||
-		    (!(qc->err_mask & AC_ERR_INVALID) &&
-		     qc->err_mask != AC_ERR_DEV))
+		if (ata_eh_worth_retry(qc))
 			qc->flags |= ATA_QCFLAG_RETRY;
 
 		/* accumulate error info */
diff --git a/drivers/ata/pata_ep93xx.c b/drivers/ata/pata_ep93xx.c
new file mode 100644
index 00000000000..6ef2e3741f7
--- /dev/null
+++ b/drivers/ata/pata_ep93xx.c
@@ -0,0 +1,1044 @@
+/*
+ * EP93XX PATA controller driver.
+ *
+ * Copyright (c) 2012, Metasoft s.c.
+ *	Rafal Prylowski <prylowski@metasoft.pl>
+ *
+ * Based on pata_scc.c, pata_icside.c and on earlier version of EP93XX
+ * PATA driver by Lennert Buytenhek and Alessandro Zummo.
+ * Read/Write timings, resource management and other improvements
+ * from driver by Joao Ramos and Bartlomiej Zolnierkiewicz.
+ * DMA engine support based on spi-ep93xx.c by Mika Westerberg.
+ *
+ * Original copyrights:
+ *
+ * Support for Cirrus Logic's EP93xx (EP9312, EP9315) CPUs
+ * PATA host controller driver.
+ *
+ * Copyright (c) 2009, Bartlomiej Zolnierkiewicz
+ *
+ * Heavily based on the ep93xx-ide.c driver:
+ *
+ * Copyright (c) 2009, Joao Ramos <joao.ramos@inov.pt>
+ *		      INESC Inovacao (INOV)
+ *
+ * EP93XX PATA controller driver.
+ * Copyright (C) 2007 Lennert Buytenhek <buytenh@wantstofly.org>
+ *
+ * An ATA driver for the Cirrus Logic EP93xx PATA controller.
+ *
+ * Based on an earlier version by Alessandro Zummo, which is:
+ *   Copyright (C) 2006 Tower Technologies
+ */
+
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/init.h>
+#include <linux/blkdev.h>
+#include <scsi/scsi_host.h>
+#include <linux/ata.h>
+#include <linux/libata.h>
+#include <linux/platform_device.h>
+#include <linux/delay.h>
+#include <linux/dmaengine.h>
+#include <linux/ktime.h>
+
+#include <mach/dma.h>
+#include <mach/platform.h>
+
+#define DRV_NAME	"ep93xx-ide"
+#define DRV_VERSION	"1.0"
+
+enum {
+	/* IDE Control Register */
+	IDECTRL				= 0x00,
+	IDECTRL_CS0N			= (1 << 0),
+	IDECTRL_CS1N			= (1 << 1),
+	IDECTRL_DIORN			= (1 << 5),
+	IDECTRL_DIOWN			= (1 << 6),
+	IDECTRL_INTRQ			= (1 << 9),
+	IDECTRL_IORDY			= (1 << 10),
+	/*
+	 * the device IDE register to be accessed is selected through
+	 * IDECTRL register's specific bitfields 'DA', 'CS1N' and 'CS0N':
+	 *   b4   b3   b2    b1     b0
+	 *   A2   A1   A0   CS1N   CS0N
+	 * the values filled in this structure allows the value to be directly
+	 * ORed to the IDECTRL register, hence giving directly the A[2:0] and
+	 * CS1N/CS0N values for each IDE register.
+	 * The values correspond to the transformation:
+	 *   ((real IDE address) << 2) | CS1N value << 1 | CS0N value
+	 */
+	IDECTRL_ADDR_CMD		= 0 + 2, /* CS1 */
+	IDECTRL_ADDR_DATA		= (ATA_REG_DATA << 2) + 2,
+	IDECTRL_ADDR_ERROR		= (ATA_REG_ERR << 2) + 2,
+	IDECTRL_ADDR_FEATURE		= (ATA_REG_FEATURE << 2) + 2,
+	IDECTRL_ADDR_NSECT		= (ATA_REG_NSECT << 2) + 2,
+	IDECTRL_ADDR_LBAL		= (ATA_REG_LBAL << 2) + 2,
+	IDECTRL_ADDR_LBAM		= (ATA_REG_LBAM << 2) + 2,
+	IDECTRL_ADDR_LBAH		= (ATA_REG_LBAH << 2) + 2,
+	IDECTRL_ADDR_DEVICE		= (ATA_REG_DEVICE << 2) + 2,
+	IDECTRL_ADDR_STATUS		= (ATA_REG_STATUS << 2) + 2,
+	IDECTRL_ADDR_COMMAND		= (ATA_REG_CMD << 2) + 2,
+	IDECTRL_ADDR_ALTSTATUS		= (0x06 << 2) + 1, /* CS0 */
+	IDECTRL_ADDR_CTL		= (0x06 << 2) + 1, /* CS0 */
+
+	/* IDE Configuration Register */
+	IDECFG				= 0x04,
+	IDECFG_IDEEN			= (1 << 0),
+	IDECFG_PIO			= (1 << 1),
+	IDECFG_MDMA			= (1 << 2),
+	IDECFG_UDMA			= (1 << 3),
+	IDECFG_MODE_SHIFT		= 4,
+	IDECFG_MODE_MASK		= (0xf << 4),
+	IDECFG_WST_SHIFT		= 8,
+	IDECFG_WST_MASK			= (0x3 << 8),
+
+	/* MDMA Operation Register */
+	IDEMDMAOP			= 0x08,
+
+	/* UDMA Operation Register */
+	IDEUDMAOP			= 0x0c,
+	IDEUDMAOP_UEN			= (1 << 0),
+	IDEUDMAOP_RWOP			= (1 << 1),
+
+	/* PIO/MDMA/UDMA Data Registers */
+	IDEDATAOUT			= 0x10,
+	IDEDATAIN			= 0x14,
+	IDEMDMADATAOUT			= 0x18,
+	IDEMDMADATAIN			= 0x1c,
+	IDEUDMADATAOUT			= 0x20,
+	IDEUDMADATAIN			= 0x24,
+
+	/* UDMA Status Register */
+	IDEUDMASTS			= 0x28,
+	IDEUDMASTS_DMAIDE		= (1 << 16),
+	IDEUDMASTS_INTIDE		= (1 << 17),
+	IDEUDMASTS_SBUSY		= (1 << 18),
+	IDEUDMASTS_NDO			= (1 << 24),
+	IDEUDMASTS_NDI			= (1 << 25),
+	IDEUDMASTS_N4X			= (1 << 26),
+
+	/* UDMA Debug Status Register */
+	IDEUDMADEBUG			= 0x2c,
+};
+
+struct ep93xx_pata_data {
+	const struct platform_device *pdev;
+	void __iomem *ide_base;
+	struct ata_timing t;
+	bool iordy;
+
+	unsigned long udma_in_phys;
+	unsigned long udma_out_phys;
+
+	struct dma_chan *dma_rx_channel;
+	struct ep93xx_dma_data dma_rx_data;
+	struct dma_chan *dma_tx_channel;
+	struct ep93xx_dma_data dma_tx_data;
+};
+
+static void ep93xx_pata_clear_regs(void __iomem *base)
+{
+	writel(IDECTRL_CS0N | IDECTRL_CS1N | IDECTRL_DIORN |
+		IDECTRL_DIOWN, base + IDECTRL);
+
+	writel(0, base + IDECFG);
+	writel(0, base + IDEMDMAOP);
+	writel(0, base + IDEUDMAOP);
+	writel(0, base + IDEDATAOUT);
+	writel(0, base + IDEDATAIN);
+	writel(0, base + IDEMDMADATAOUT);
+	writel(0, base + IDEMDMADATAIN);
+	writel(0, base + IDEUDMADATAOUT);
+	writel(0, base + IDEUDMADATAIN);
+	writel(0, base + IDEUDMADEBUG);
+}
+
+static bool ep93xx_pata_check_iordy(void __iomem *base)
+{
+	return !!(readl(base + IDECTRL) & IDECTRL_IORDY);
+}
+
+/*
+ * According to EP93xx User's Guide, WST field of IDECFG specifies number
+ * of HCLK cycles to hold the data bus after a PIO write operation.
+ * It should be programmed to guarantee following delays:
+ *
+ * PIO Mode   [ns]
+ * 0          30
+ * 1          20
+ * 2          15
+ * 3          10
+ * 4          5
+ *
+ * Maximum possible value for HCLK is 100MHz.
+ */
+static int ep93xx_pata_get_wst(int pio_mode)
+{
+	int val;
+
+	if (pio_mode == 0)
+		val = 3;
+	else if (pio_mode < 3)
+		val = 2;
+	else
+		val = 1;
+
+	return val << IDECFG_WST_SHIFT;
+}
+
+static void ep93xx_pata_enable_pio(void __iomem *base, int pio_mode)
+{
+	writel(IDECFG_IDEEN | IDECFG_PIO |
+		ep93xx_pata_get_wst(pio_mode) |
+		(pio_mode << IDECFG_MODE_SHIFT), base + IDECFG);
+}
+
+/*
+ * Based on delay loop found in mach-pxa/mp900.c.
+ *
+ * Single iteration should take 5 cpu cycles. This is 25ns assuming the
+ * fastest ep93xx cpu speed (200MHz) and is better optimized for PIO4 timings
+ * than eg. 20ns.
+ */
+static void ep93xx_pata_delay(unsigned long count)
+{
+	__asm__ volatile (
+		"0:\n"
+		"mov r0, r0\n"
+		"subs %0, %1, #1\n"
+		"bge 0b\n"
+		: "=r" (count)
+		: "0" (count)
+	);
+}
+
+static unsigned long ep93xx_pata_wait_for_iordy(void __iomem *base,
+						unsigned long t2)
+{
+	/*
+	 * According to ATA specification, IORDY pin can be first sampled
+	 * tA = 35ns after activation of DIOR-/DIOW-. Maximum IORDY pulse
+	 * width is tB = 1250ns.
+	 *
+	 * We are already t2 delay loop iterations after activation of
+	 * DIOR-/DIOW-, so we set timeout to (1250 + 35) / 25 - t2 additional
+	 * delay loop iterations.
+	 */
+	unsigned long start = (1250 + 35) / 25 - t2;
+	unsigned long counter = start;
+
+	while (!ep93xx_pata_check_iordy(base) && counter--)
+		ep93xx_pata_delay(1);
+	return start - counter;
+}
+
+/* common part at start of ep93xx_pata_read/write() */
+static void ep93xx_pata_rw_begin(void __iomem *base, unsigned long addr,
+				 unsigned long t1)
+{
+	writel(IDECTRL_DIOWN | IDECTRL_DIORN | addr, base + IDECTRL);
+	ep93xx_pata_delay(t1);
+}
+
+/* common part at end of ep93xx_pata_read/write() */
+static void ep93xx_pata_rw_end(void __iomem *base, unsigned long addr,
+			       bool iordy, unsigned long t0, unsigned long t2,
+			       unsigned long t2i)
+{
+	ep93xx_pata_delay(t2);
+	/* lengthen t2 if needed */
+	if (iordy)
+		t2 += ep93xx_pata_wait_for_iordy(base, t2);
+	writel(IDECTRL_DIOWN | IDECTRL_DIORN | addr, base + IDECTRL);
+	if (t0 > t2 && t0 - t2 > t2i)
+		ep93xx_pata_delay(t0 - t2);
+	else
+		ep93xx_pata_delay(t2i);
+}
+
+static u16 ep93xx_pata_read(struct ep93xx_pata_data *drv_data,
+			    unsigned long addr,
+			    bool reg)
+{
+	void __iomem *base = drv_data->ide_base;
+	const struct ata_timing *t = &drv_data->t;
+	unsigned long t0 = reg ? t->cyc8b : t->cycle;
+	unsigned long t2 = reg ? t->act8b : t->active;
+	unsigned long t2i = reg ? t->rec8b : t->recover;
+
+	ep93xx_pata_rw_begin(base, addr, t->setup);
+	writel(IDECTRL_DIOWN | addr, base + IDECTRL);
+	/*
+	 * The IDEDATAIN register is loaded from the DD pins at the positive
+	 * edge of the DIORN signal. (EP93xx UG p27-14)
+	 */
+	ep93xx_pata_rw_end(base, addr, drv_data->iordy, t0, t2, t2i);
+	return readl(base + IDEDATAIN);
+}
+
+/* IDE register read */
+static u16 ep93xx_pata_read_reg(struct ep93xx_pata_data *drv_data,
+				unsigned long addr)
+{
+	return ep93xx_pata_read(drv_data, addr, true);
+}
+
+/* PIO data read */
+static u16 ep93xx_pata_read_data(struct ep93xx_pata_data *drv_data,
+				 unsigned long addr)
+{
+	return ep93xx_pata_read(drv_data, addr, false);
+}
+
+static void ep93xx_pata_write(struct ep93xx_pata_data *drv_data,
+			      u16 value, unsigned long addr,
+			      bool reg)
+{
+	void __iomem *base = drv_data->ide_base;
+	const struct ata_timing *t = &drv_data->t;
+	unsigned long t0 = reg ? t->cyc8b : t->cycle;
+	unsigned long t2 = reg ? t->act8b : t->active;
+	unsigned long t2i = reg ? t->rec8b : t->recover;
+
+	ep93xx_pata_rw_begin(base, addr, t->setup);
+	/*
+	 * Value from IDEDATAOUT register is driven onto the DD pins when
+	 * DIOWN is low. (EP93xx UG p27-13)
+	 */
+	writel(value, base + IDEDATAOUT);
+	writel(IDECTRL_DIORN | addr, base + IDECTRL);
+	ep93xx_pata_rw_end(base, addr, drv_data->iordy, t0, t2, t2i);
+}
+
+/* IDE register write */
+static void ep93xx_pata_write_reg(struct ep93xx_pata_data *drv_data,
+				  u16 value, unsigned long addr)
+{
+	ep93xx_pata_write(drv_data, value, addr, true);
+}
+
+/* PIO data write */
+static void ep93xx_pata_write_data(struct ep93xx_pata_data *drv_data,
+				   u16 value, unsigned long addr)
+{
+	ep93xx_pata_write(drv_data, value, addr, false);
+}
+
+static void ep93xx_pata_set_piomode(struct ata_port *ap,
+				    struct ata_device *adev)
+{
+	struct ep93xx_pata_data *drv_data = ap->host->private_data;
+	struct ata_device *pair = ata_dev_pair(adev);
+	/*
+	 * Calculate timings for the delay loop, assuming ep93xx cpu speed
+	 * is 200MHz (maximum possible for ep93xx). If actual cpu speed is
+	 * slower, we will wait a bit longer in each delay.
+	 * Additional division of cpu speed by 5, because single iteration
+	 * of our delay loop takes 5 cpu cycles (25ns).
+	 */
+	unsigned long T = 1000000 / (200 / 5);
+
+	ata_timing_compute(adev, adev->pio_mode, &drv_data->t, T, 0);
+	if (pair && pair->pio_mode) {
+		struct ata_timing t;
+		ata_timing_compute(pair, pair->pio_mode, &t, T, 0);
+		ata_timing_merge(&t, &drv_data->t, &drv_data->t,
+			ATA_TIMING_SETUP | ATA_TIMING_8BIT);
+	}
+	drv_data->iordy = ata_pio_need_iordy(adev);
+
+	ep93xx_pata_enable_pio(drv_data->ide_base,
+			       adev->pio_mode - XFER_PIO_0);
+}
+
+/* Note: original code is ata_sff_check_status */
+static u8 ep93xx_pata_check_status(struct ata_port *ap)
+{
+	struct ep93xx_pata_data *drv_data = ap->host->private_data;
+
+	return ep93xx_pata_read_reg(drv_data, IDECTRL_ADDR_STATUS);
+}
+
+static u8 ep93xx_pata_check_altstatus(struct ata_port *ap)
+{
+	struct ep93xx_pata_data *drv_data = ap->host->private_data;
+
+	return ep93xx_pata_read_reg(drv_data, IDECTRL_ADDR_ALTSTATUS);
+}
+
+/* Note: original code is ata_sff_tf_load */
+static void ep93xx_pata_tf_load(struct ata_port *ap,
+				const struct ata_taskfile *tf)
+{
+	struct ep93xx_pata_data *drv_data = ap->host->private_data;
+	unsigned int is_addr = tf->flags & ATA_TFLAG_ISADDR;
+
+	if (tf->ctl != ap->last_ctl) {
+		ep93xx_pata_write_reg(drv_data, tf->ctl, IDECTRL_ADDR_CTL);
+		ap->last_ctl = tf->ctl;
+		ata_wait_idle(ap);
+	}
+
+	if (is_addr && (tf->flags & ATA_TFLAG_LBA48)) {
+		ep93xx_pata_write_reg(drv_data, tf->hob_feature,
+			IDECTRL_ADDR_FEATURE);
+		ep93xx_pata_write_reg(drv_data, tf->hob_nsect,
+			IDECTRL_ADDR_NSECT);
+		ep93xx_pata_write_reg(drv_data, tf->hob_lbal,
+			IDECTRL_ADDR_LBAL);
+		ep93xx_pata_write_reg(drv_data, tf->hob_lbam,
+			IDECTRL_ADDR_LBAM);
+		ep93xx_pata_write_reg(drv_data, tf->hob_lbah,
+			IDECTRL_ADDR_LBAH);
+	}
+
+	if (is_addr) {
+		ep93xx_pata_write_reg(drv_data, tf->feature,
+			IDECTRL_ADDR_FEATURE);
+		ep93xx_pata_write_reg(drv_data, tf->nsect, IDECTRL_ADDR_NSECT);
+		ep93xx_pata_write_reg(drv_data, tf->lbal, IDECTRL_ADDR_LBAL);
+		ep93xx_pata_write_reg(drv_data, tf->lbam, IDECTRL_ADDR_LBAM);
+		ep93xx_pata_write_reg(drv_data, tf->lbah, IDECTRL_ADDR_LBAH);
+	}
+
+	if (tf->flags & ATA_TFLAG_DEVICE)
+		ep93xx_pata_write_reg(drv_data, tf->device,
+			IDECTRL_ADDR_DEVICE);
+
+	ata_wait_idle(ap);
+}
+
+/* Note: original code is ata_sff_tf_read */
+static void ep93xx_pata_tf_read(struct ata_port *ap, struct ata_taskfile *tf)
+{
+	struct ep93xx_pata_data *drv_data = ap->host->private_data;
+
+	tf->command = ep93xx_pata_check_status(ap);
+	tf->feature = ep93xx_pata_read_reg(drv_data, IDECTRL_ADDR_FEATURE);
+	tf->nsect = ep93xx_pata_read_reg(drv_data, IDECTRL_ADDR_NSECT);
+	tf->lbal = ep93xx_pata_read_reg(drv_data, IDECTRL_ADDR_LBAL);
+	tf->lbam = ep93xx_pata_read_reg(drv_data, IDECTRL_ADDR_LBAM);
+	tf->lbah = ep93xx_pata_read_reg(drv_data, IDECTRL_ADDR_LBAH);
+	tf->device = ep93xx_pata_read_reg(drv_data, IDECTRL_ADDR_DEVICE);
+
+	if (tf->flags & ATA_TFLAG_LBA48) {
+		ep93xx_pata_write_reg(drv_data, tf->ctl | ATA_HOB,
+			IDECTRL_ADDR_CTL);
+		tf->hob_feature = ep93xx_pata_read_reg(drv_data,
+			IDECTRL_ADDR_FEATURE);
+		tf->hob_nsect = ep93xx_pata_read_reg(drv_data,
+			IDECTRL_ADDR_NSECT);
+		tf->hob_lbal = ep93xx_pata_read_reg(drv_data,
+			IDECTRL_ADDR_LBAL);
+		tf->hob_lbam = ep93xx_pata_read_reg(drv_data,
+			IDECTRL_ADDR_LBAM);
+		tf->hob_lbah = ep93xx_pata_read_reg(drv_data,
+			IDECTRL_ADDR_LBAH);
+		ep93xx_pata_write_reg(drv_data, tf->ctl, IDECTRL_ADDR_CTL);
+		ap->last_ctl = tf->ctl;
+	}
+}
+
+/* Note: original code is ata_sff_exec_command */
+static void ep93xx_pata_exec_command(struct ata_port *ap,
+				     const struct ata_taskfile *tf)
+{
+	struct ep93xx_pata_data *drv_data = ap->host->private_data;
+
+	ep93xx_pata_write_reg(drv_data, tf->command,
+			  IDECTRL_ADDR_COMMAND);
+	ata_sff_pause(ap);
+}
+
+/* Note: original code is ata_sff_dev_select */
+static void ep93xx_pata_dev_select(struct ata_port *ap, unsigned int device)
+{
+	struct ep93xx_pata_data *drv_data = ap->host->private_data;
+	u8 tmp = ATA_DEVICE_OBS;
+
+	if (device != 0)
+		tmp |= ATA_DEV1;
+
+	ep93xx_pata_write_reg(drv_data, tmp, IDECTRL_ADDR_DEVICE);
+	ata_sff_pause(ap);	/* needed; also flushes, for mmio */
+}
+
+/* Note: original code is ata_sff_set_devctl */
+static void ep93xx_pata_set_devctl(struct ata_port *ap, u8 ctl)
+{
+	struct ep93xx_pata_data *drv_data = ap->host->private_data;
+
+	ep93xx_pata_write_reg(drv_data, ctl, IDECTRL_ADDR_CTL);
+}
+
+/* Note: original code is ata_sff_data_xfer */
+static unsigned int ep93xx_pata_data_xfer(struct ata_device *adev,
+					  unsigned char *buf,
+					  unsigned int buflen, int rw)
+{
+	struct ata_port *ap = adev->link->ap;
+	struct ep93xx_pata_data *drv_data = ap->host->private_data;
+	u16 *data = (u16 *)buf;
+	unsigned int words = buflen >> 1;
+
+	/* Transfer multiple of 2 bytes */
+	while (words--)
+		if (rw == READ)
+			*data++ = cpu_to_le16(
+				ep93xx_pata_read_data(
+					drv_data, IDECTRL_ADDR_DATA));
+		else
+			ep93xx_pata_write_data(drv_data, le16_to_cpu(*data++),
+				IDECTRL_ADDR_DATA);
+
+	/* Transfer trailing 1 byte, if any. */
+	if (unlikely(buflen & 0x01)) {
+		unsigned char pad[2] = { };
+
+		buf += buflen - 1;
+
+		if (rw == READ) {
+			*pad = cpu_to_le16(
+				ep93xx_pata_read_data(
+					drv_data, IDECTRL_ADDR_DATA));
+			*buf = pad[0];
+		} else {
+			pad[0] = *buf;
+			ep93xx_pata_write_data(drv_data, le16_to_cpu(*pad),
+					  IDECTRL_ADDR_DATA);
+		}
+		words++;
+	}
+
+	return words << 1;
+}
+
+/* Note: original code is ata_devchk */
+static bool ep93xx_pata_device_is_present(struct ata_port *ap,
+					  unsigned int device)
+{
+	struct ep93xx_pata_data *drv_data = ap->host->private_data;
+	u8 nsect, lbal;
+
+	ap->ops->sff_dev_select(ap, device);
+
+	ep93xx_pata_write_reg(drv_data, 0x55, IDECTRL_ADDR_NSECT);
+	ep93xx_pata_write_reg(drv_data, 0xaa, IDECTRL_ADDR_LBAL);
+
+	ep93xx_pata_write_reg(drv_data, 0xaa, IDECTRL_ADDR_NSECT);
+	ep93xx_pata_write_reg(drv_data, 0x55, IDECTRL_ADDR_LBAL);
+
+	ep93xx_pata_write_reg(drv_data, 0x55, IDECTRL_ADDR_NSECT);
+	ep93xx_pata_write_reg(drv_data, 0xaa, IDECTRL_ADDR_LBAL);
+
+	nsect = ep93xx_pata_read_reg(drv_data, IDECTRL_ADDR_NSECT);
+	lbal = ep93xx_pata_read_reg(drv_data, IDECTRL_ADDR_LBAL);
+
+	if ((nsect == 0x55) && (lbal == 0xaa))
+		return true;
+
+	return false;
+}
+
+/* Note: original code is ata_sff_wait_after_reset */
+static int ep93xx_pata_wait_after_reset(struct ata_link *link,
+					unsigned int devmask,
+					unsigned long deadline)
+{
+	struct ata_port *ap = link->ap;
+	struct ep93xx_pata_data *drv_data = ap->host->private_data;
+	unsigned int dev0 = devmask & (1 << 0);
+	unsigned int dev1 = devmask & (1 << 1);
+	int rc, ret = 0;
+
+	ata_msleep(ap, ATA_WAIT_AFTER_RESET);
+
+	/* always check readiness of the master device */
+	rc = ata_sff_wait_ready(link, deadline);
+	/*
+	 * -ENODEV means the odd clown forgot the D7 pulldown resistor
+	 * and TF status is 0xff, bail out on it too.
+	 */
+	if (rc)
+		return rc;
+
+	/*
+	 * if device 1 was found in ata_devchk, wait for register
+	 * access briefly, then wait for BSY to clear.
+	 */
+	if (dev1) {
+		int i;
+
+		ap->ops->sff_dev_select(ap, 1);
+
+		/*
+		 * Wait for register access.  Some ATAPI devices fail
+		 * to set nsect/lbal after reset, so don't waste too
+		 * much time on it.  We're gonna wait for !BSY anyway.
+		 */
+		for (i = 0; i < 2; i++) {
+			u8 nsect, lbal;
+
+			nsect = ep93xx_pata_read_reg(drv_data,
+				IDECTRL_ADDR_NSECT);
+			lbal = ep93xx_pata_read_reg(drv_data,
+				IDECTRL_ADDR_LBAL);
+			if (nsect == 1 && lbal == 1)
+				break;
+			msleep(50);	/* give drive a breather */
+		}
+
+		rc = ata_sff_wait_ready(link, deadline);
+		if (rc) {
+			if (rc != -ENODEV)
+				return rc;
+			ret = rc;
+		}
+	}
+	/* is all this really necessary? */
+	ap->ops->sff_dev_select(ap, 0);
+	if (dev1)
+		ap->ops->sff_dev_select(ap, 1);
+	if (dev0)
+		ap->ops->sff_dev_select(ap, 0);
+
+	return ret;
+}
+
+/* Note: original code is ata_bus_softreset */
+static int ep93xx_pata_bus_softreset(struct ata_port *ap, unsigned int devmask,
+				     unsigned long deadline)
+{
+	struct ep93xx_pata_data *drv_data = ap->host->private_data;
+
+	ep93xx_pata_write_reg(drv_data, ap->ctl, IDECTRL_ADDR_CTL);
+	udelay(20);		/* FIXME: flush */
+	ep93xx_pata_write_reg(drv_data, ap->ctl | ATA_SRST, IDECTRL_ADDR_CTL);
+	udelay(20);		/* FIXME: flush */
+	ep93xx_pata_write_reg(drv_data, ap->ctl, IDECTRL_ADDR_CTL);
+	ap->last_ctl = ap->ctl;
+
+	return ep93xx_pata_wait_after_reset(&ap->link, devmask, deadline);
+}
+
+static void ep93xx_pata_release_dma(struct ep93xx_pata_data *drv_data)
+{
+	if (drv_data->dma_rx_channel) {
+		dma_release_channel(drv_data->dma_rx_channel);
+		drv_data->dma_rx_channel = NULL;
+	}
+	if (drv_data->dma_tx_channel) {
+		dma_release_channel(drv_data->dma_tx_channel);
+		drv_data->dma_tx_channel = NULL;
+	}
+}
+
+static bool ep93xx_pata_dma_filter(struct dma_chan *chan, void *filter_param)
+{
+	if (ep93xx_dma_chan_is_m2p(chan))
+		return false;
+
+	chan->private = filter_param;
+	return true;
+}
+
+static void ep93xx_pata_dma_init(struct ep93xx_pata_data *drv_data)
+{
+	const struct platform_device *pdev = drv_data->pdev;
+	dma_cap_mask_t mask;
+	struct dma_slave_config conf;
+
+	dma_cap_zero(mask);
+	dma_cap_set(DMA_SLAVE, mask);
+
+	/*
+	 * Request two channels for IDE. Another possibility would be
+	 * to request only one channel, and reprogram it's direction at
+	 * start of new transfer.
+	 */
+	drv_data->dma_rx_data.port = EP93XX_DMA_IDE;
+	drv_data->dma_rx_data.direction = DMA_FROM_DEVICE;
+	drv_data->dma_rx_data.name = "ep93xx-pata-rx";
+	drv_data->dma_rx_channel = dma_request_channel(mask,
+		ep93xx_pata_dma_filter, &drv_data->dma_rx_data);
+	if (!drv_data->dma_rx_channel)
+		return;
+
+	drv_data->dma_tx_data.port = EP93XX_DMA_IDE;
+	drv_data->dma_tx_data.direction = DMA_TO_DEVICE;
+	drv_data->dma_tx_data.name = "ep93xx-pata-tx";
+	drv_data->dma_tx_channel = dma_request_channel(mask,
+		ep93xx_pata_dma_filter, &drv_data->dma_tx_data);
+	if (!drv_data->dma_tx_channel) {
+		dma_release_channel(drv_data->dma_rx_channel);
+		return;
+	}
+
+	/* Configure receive channel direction and source address */
+	memset(&conf, 0, sizeof(conf));
+	conf.direction = DMA_FROM_DEVICE;
+	conf.src_addr = drv_data->udma_in_phys;
+	conf.src_addr_width = DMA_SLAVE_BUSWIDTH_4_BYTES;
+	if (dmaengine_slave_config(drv_data->dma_rx_channel, &conf)) {
+		dev_err(&pdev->dev, "failed to configure rx dma channel\n");
+		ep93xx_pata_release_dma(drv_data);
+		return;
+	}
+
+	/* Configure transmit channel direction and destination address */
+	memset(&conf, 0, sizeof(conf));
+	conf.direction = DMA_TO_DEVICE;
+	conf.dst_addr = drv_data->udma_out_phys;
+	conf.dst_addr_width = DMA_SLAVE_BUSWIDTH_4_BYTES;
+	if (dmaengine_slave_config(drv_data->dma_tx_channel, &conf)) {
+		dev_err(&pdev->dev, "failed to configure tx dma channel\n");
+		ep93xx_pata_release_dma(drv_data);
+	}
+}
+
+static void ep93xx_pata_dma_start(struct ata_queued_cmd *qc)
+{
+	struct dma_async_tx_descriptor *txd;
+	struct ep93xx_pata_data *drv_data = qc->ap->host->private_data;
+	void __iomem *base = drv_data->ide_base;
+	struct ata_device *adev = qc->dev;
+	u32 v = qc->dma_dir == DMA_TO_DEVICE ? IDEUDMAOP_RWOP : 0;
+	struct dma_chan *channel = qc->dma_dir == DMA_TO_DEVICE
+		? drv_data->dma_tx_channel : drv_data->dma_rx_channel;
+
+	txd = channel->device->device_prep_slave_sg(channel, qc->sg,
+		 qc->n_elem, qc->dma_dir, DMA_CTRL_ACK, NULL);
+	if (!txd) {
+		dev_err(qc->ap->dev, "failed to prepare slave for sg dma\n");
+		return;
+	}
+	txd->callback = NULL;
+	txd->callback_param = NULL;
+
+	if (dmaengine_submit(txd) < 0) {
+		dev_err(qc->ap->dev, "failed to submit dma transfer\n");
+		return;
+	}
+	dma_async_issue_pending(channel);
+
+	/*
+	 * When enabling UDMA operation, IDEUDMAOP register needs to be
+	 * programmed in three step sequence:
+	 * 1) set or clear the RWOP bit,
+	 * 2) perform dummy read of the register,
+	 * 3) set the UEN bit.
+	 */
+	writel(v, base + IDEUDMAOP);
+	readl(base + IDEUDMAOP);
+	writel(v | IDEUDMAOP_UEN, base + IDEUDMAOP);
+
+	writel(IDECFG_IDEEN | IDECFG_UDMA |
+		((adev->xfer_mode - XFER_UDMA_0) << IDECFG_MODE_SHIFT),
+		base + IDECFG);
+}
+
+static void ep93xx_pata_dma_stop(struct ata_queued_cmd *qc)
+{
+	struct ep93xx_pata_data *drv_data = qc->ap->host->private_data;
+	void __iomem *base = drv_data->ide_base;
+
+	/* terminate all dma transfers, if not yet finished */
+	dmaengine_terminate_all(drv_data->dma_rx_channel);
+	dmaengine_terminate_all(drv_data->dma_tx_channel);
+
+	/*
+	 * To properly stop IDE-DMA, IDEUDMAOP register must to be cleared
+	 * and IDECTRL register must be set to default value.
+	 */
+	writel(0, base + IDEUDMAOP);
+	writel(readl(base + IDECTRL) | IDECTRL_DIOWN | IDECTRL_DIORN |
+		IDECTRL_CS0N | IDECTRL_CS1N, base + IDECTRL);
+
+	ep93xx_pata_enable_pio(drv_data->ide_base,
+		qc->dev->pio_mode - XFER_PIO_0);
+
+	ata_sff_dma_pause(qc->ap);
+}
+
+static void ep93xx_pata_dma_setup(struct ata_queued_cmd *qc)
+{
+	qc->ap->ops->sff_exec_command(qc->ap, &qc->tf);
+}
+
+static u8 ep93xx_pata_dma_status(struct ata_port *ap)
+{
+	struct ep93xx_pata_data *drv_data = ap->host->private_data;
+	u32 val = readl(drv_data->ide_base + IDEUDMASTS);
+
+	/*
+	 * UDMA Status Register bits:
+	 *
+	 * DMAIDE - DMA request signal from UDMA state machine,
+	 * INTIDE - INT line generated by UDMA because of errors in the
+	 *          state machine,
+	 * SBUSY - UDMA state machine busy, not in idle state,
+	 * NDO   - error for data-out not completed,
+	 * NDI   - error for data-in not completed,
+	 * N4X   - error for data transferred not multiplies of four
+	 *         32-bit words.
+	 * (EP93xx UG p27-17)
+	 */
+	if (val & IDEUDMASTS_NDO || val & IDEUDMASTS_NDI ||
+	    val & IDEUDMASTS_N4X || val & IDEUDMASTS_INTIDE)
+		return ATA_DMA_ERR;
+
+	/* read INTRQ (INT[3]) pin input state */
+	if (readl(drv_data->ide_base + IDECTRL) & IDECTRL_INTRQ)
+		return ATA_DMA_INTR;
+
+	if (val & IDEUDMASTS_SBUSY || val & IDEUDMASTS_DMAIDE)
+		return ATA_DMA_ACTIVE;
+
+	return 0;
+}
+
+/* Note: original code is ata_sff_softreset */
+static int ep93xx_pata_softreset(struct ata_link *al, unsigned int *classes,
+				 unsigned long deadline)
+{
+	struct ata_port *ap = al->ap;
+	unsigned int slave_possible = ap->flags & ATA_FLAG_SLAVE_POSS;
+	unsigned int devmask = 0;
+	int rc;
+	u8 err;
+
+	/* determine if device 0/1 are present */
+	if (ep93xx_pata_device_is_present(ap, 0))
+		devmask |= (1 << 0);
+	if (slave_possible && ep93xx_pata_device_is_present(ap, 1))
+		devmask |= (1 << 1);
+
+	/* select device 0 again */
+	ap->ops->sff_dev_select(al->ap, 0);
+
+	/* issue bus reset */
+	rc = ep93xx_pata_bus_softreset(ap, devmask, deadline);
+	/* if link is ocuppied, -ENODEV too is an error */
+	if (rc && (rc != -ENODEV || sata_scr_valid(al))) {
+		ata_link_printk(al, KERN_ERR, "SRST failed (errno=%d)\n",
+				rc);
+		return rc;
+	}
+
+	/* determine by signature whether we have ATA or ATAPI devices */
+	classes[0] = ata_sff_dev_classify(&al->device[0], devmask & (1 << 0),
+					  &err);
+	if (slave_possible && err != 0x81)
+		classes[1] = ata_sff_dev_classify(&al->device[1],
+						  devmask & (1 << 1), &err);
+
+	return 0;
+}
+
+/* Note: original code is ata_sff_drain_fifo */
+static void ep93xx_pata_drain_fifo(struct ata_queued_cmd *qc)
+{
+	int count;
+	struct ata_port *ap;
+	struct ep93xx_pata_data *drv_data;
+
+	/* We only need to flush incoming data when a command was running */
+	if (qc == NULL || qc->dma_dir == DMA_TO_DEVICE)
+		return;
+
+	ap = qc->ap;
+	drv_data = ap->host->private_data;
+	/* Drain up to 64K of data before we give up this recovery method */
+	for (count = 0; (ap->ops->sff_check_status(ap) & ATA_DRQ)
+		     && count < 65536; count += 2)
+		ep93xx_pata_read_reg(drv_data, IDECTRL_ADDR_DATA);
+
+	/* Can become DEBUG later */
+	if (count)
+		ata_port_printk(ap, KERN_DEBUG,
+				"drained %d bytes to clear DRQ.\n", count);
+
+}
+
+static int ep93xx_pata_port_start(struct ata_port *ap)
+{
+	struct ep93xx_pata_data *drv_data = ap->host->private_data;
+
+	/*
+	 * Set timings to safe values at startup (= number of ns from ATA
+	 * specification), we'll switch to properly calculated values later.
+	 */
+	drv_data->t = *ata_timing_find_mode(XFER_PIO_0);
+	return 0;
+}
+
+static struct scsi_host_template ep93xx_pata_sht = {
+	ATA_BASE_SHT(DRV_NAME),
+	/* ep93xx dma implementation limit */
+	.sg_tablesize		= 32,
+	/* ep93xx dma can't transfer 65536 bytes at once */
+	.dma_boundary		= 0x7fff,
+};
+
+static struct ata_port_operations ep93xx_pata_port_ops = {
+	.inherits		= &ata_bmdma_port_ops,
+
+	.qc_prep		= ata_noop_qc_prep,
+
+	.softreset		= ep93xx_pata_softreset,
+	.hardreset		= ATA_OP_NULL,
+
+	.sff_dev_select		= ep93xx_pata_dev_select,
+	.sff_set_devctl		= ep93xx_pata_set_devctl,
+	.sff_check_status	= ep93xx_pata_check_status,
+	.sff_check_altstatus	= ep93xx_pata_check_altstatus,
+	.sff_tf_load		= ep93xx_pata_tf_load,
+	.sff_tf_read		= ep93xx_pata_tf_read,
+	.sff_exec_command	= ep93xx_pata_exec_command,
+	.sff_data_xfer		= ep93xx_pata_data_xfer,
+	.sff_drain_fifo		= ep93xx_pata_drain_fifo,
+	.sff_irq_clear		= ATA_OP_NULL,
+
+	.set_piomode		= ep93xx_pata_set_piomode,
+
+	.bmdma_setup		= ep93xx_pata_dma_setup,
+	.bmdma_start		= ep93xx_pata_dma_start,
+	.bmdma_stop		= ep93xx_pata_dma_stop,
+	.bmdma_status		= ep93xx_pata_dma_status,
+
+	.cable_detect		= ata_cable_unknown,
+	.port_start		= ep93xx_pata_port_start,
+};
+
+static int __devinit ep93xx_pata_probe(struct platform_device *pdev)
+{
+	struct ep93xx_pata_data *drv_data;
+	struct ata_host *host;
+	struct ata_port *ap;
+	unsigned int irq;
+	struct resource *mem_res;
+	void __iomem *ide_base;
+	int err;
+
+	err = ep93xx_ide_acquire_gpio(pdev);
+	if (err)
+		return err;
+
+	/* INT[3] (IRQ_EP93XX_EXT3) line connected as pull down */
+	irq = platform_get_irq(pdev, 0);
+	if (irq < 0) {
+		err = -ENXIO;
+		goto err_rel_gpio;
+	}
+
+	mem_res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	if (!mem_res) {
+		err = -ENXIO;
+		goto err_rel_gpio;
+	}
+
+	ide_base = devm_request_and_ioremap(&pdev->dev, mem_res);
+	if (!ide_base) {
+		err = -ENXIO;
+		goto err_rel_gpio;
+	}
+
+	drv_data = devm_kzalloc(&pdev->dev, sizeof(*drv_data), GFP_KERNEL);
+	if (!drv_data) {
+		err = -ENXIO;
+		goto err_rel_gpio;
+	}
+
+	platform_set_drvdata(pdev, drv_data);
+	drv_data->pdev = pdev;
+	drv_data->ide_base = ide_base;
+	drv_data->udma_in_phys = mem_res->start + IDEUDMADATAIN;
+	drv_data->udma_out_phys = mem_res->start + IDEUDMADATAOUT;
+	ep93xx_pata_dma_init(drv_data);
+
+	/* allocate host */
+	host = ata_host_alloc(&pdev->dev, 1);
+	if (!host) {
+		err = -ENXIO;
+		goto err_rel_dma;
+	}
+
+	ep93xx_pata_clear_regs(ide_base);
+
+	host->private_data = drv_data;
+
+	ap = host->ports[0];
+	ap->dev = &pdev->dev;
+	ap->ops = &ep93xx_pata_port_ops;
+	ap->flags |= ATA_FLAG_SLAVE_POSS;
+	ap->pio_mask = ATA_PIO4;
+
+	/*
+	 * Maximum UDMA modes:
+	 * EP931x rev.E0 - UDMA2
+	 * EP931x rev.E1 - UDMA3
+	 * EP931x rev.E2 - UDMA4
+	 *
+	 * MWDMA support was removed from EP931x rev.E2,
+	 * so this driver supports only UDMA modes.
+	 */
+	if (drv_data->dma_rx_channel && drv_data->dma_tx_channel) {
+		int chip_rev = ep93xx_chip_revision();
+
+		if (chip_rev == EP93XX_CHIP_REV_E1)
+			ap->udma_mask = ATA_UDMA3;
+		else if (chip_rev == EP93XX_CHIP_REV_E2)
+			ap->udma_mask = ATA_UDMA4;
+		else
+			ap->udma_mask = ATA_UDMA2;
+	}
+
+	/* defaults, pio 0 */
+	ep93xx_pata_enable_pio(ide_base, 0);
+
+	dev_info(&pdev->dev, "version " DRV_VERSION "\n");
+
+	/* activate host */
+	err = ata_host_activate(host, irq, ata_bmdma_interrupt, 0,
+		&ep93xx_pata_sht);
+	if (err == 0)
+		return 0;
+
+err_rel_dma:
+	ep93xx_pata_release_dma(drv_data);
+err_rel_gpio:
+	ep93xx_ide_release_gpio(pdev);
+	return err;
+}
+
+static int __devexit ep93xx_pata_remove(struct platform_device *pdev)
+{
+	struct ata_host *host = platform_get_drvdata(pdev);
+	struct ep93xx_pata_data *drv_data = host->private_data;
+
+	ata_host_detach(host);
+	ep93xx_pata_release_dma(drv_data);
+	ep93xx_pata_clear_regs(drv_data->ide_base);
+	ep93xx_ide_release_gpio(pdev);
+	return 0;
+}
+
+static struct platform_driver ep93xx_pata_platform_driver = {
+	.driver = {
+		.name = DRV_NAME,
+		.owner = THIS_MODULE,
+	},
+	.probe = ep93xx_pata_probe,
+	.remove = __devexit_p(ep93xx_pata_remove),
+};
+
+module_platform_driver(ep93xx_pata_platform_driver);
+
+MODULE_AUTHOR("Alessandro Zummo, Lennert Buytenhek, Joao Ramos, "
+		"Bartlomiej Zolnierkiewicz, Rafal Prylowski");
+MODULE_DESCRIPTION("low-level driver for cirrus ep93xx IDE controller");
+MODULE_LICENSE("GPL");
+MODULE_VERSION(DRV_VERSION);
+MODULE_ALIAS("platform:pata_ep93xx");
diff --git a/drivers/ata/sata_mv.c b/drivers/ata/sata_mv.c
index 7336d4a7ab3..24712adf69d 100644
--- a/drivers/ata/sata_mv.c
+++ b/drivers/ata/sata_mv.c
@@ -553,6 +553,7 @@ struct mv_host_priv {
 
 #if defined(CONFIG_HAVE_CLK)
 	struct clk		*clk;
+	struct clk              **port_clks;
 #endif
 	/*
 	 * These consistent DMA memory pools give us guaranteed
@@ -4027,6 +4028,9 @@ static int mv_platform_probe(struct platform_device *pdev)
 	struct resource *res;
 	int n_ports = 0;
 	int rc;
+#if defined(CONFIG_HAVE_CLK)
+	int port;
+#endif
 
 	ata_print_version_once(&pdev->dev, DRV_VERSION);
 
@@ -4054,6 +4058,13 @@ static int mv_platform_probe(struct platform_device *pdev)
 
 	if (!host || !hpriv)
 		return -ENOMEM;
+#if defined(CONFIG_HAVE_CLK)
+	hpriv->port_clks = devm_kzalloc(&pdev->dev,
+					sizeof(struct clk *) * n_ports,
+					GFP_KERNEL);
+	if (!hpriv->port_clks)
+		return -ENOMEM;
+#endif
 	host->private_data = hpriv;
 	hpriv->n_ports = n_ports;
 	hpriv->board_idx = chip_soc;
@@ -4066,9 +4077,17 @@ static int mv_platform_probe(struct platform_device *pdev)
 #if defined(CONFIG_HAVE_CLK)
 	hpriv->clk = clk_get(&pdev->dev, NULL);
 	if (IS_ERR(hpriv->clk))
-		dev_notice(&pdev->dev, "cannot get clkdev\n");
+		dev_notice(&pdev->dev, "cannot get optional clkdev\n");
 	else
-		clk_enable(hpriv->clk);
+		clk_prepare_enable(hpriv->clk);
+
+	for (port = 0; port < n_ports; port++) {
+		char port_number[16];
+		sprintf(port_number, "%d", port);
+		hpriv->port_clks[port] = clk_get(&pdev->dev, port_number);
+		if (!IS_ERR(hpriv->port_clks[port]))
+			clk_prepare_enable(hpriv->port_clks[port]);
+	}
 #endif
 
 	/*
@@ -4098,9 +4117,15 @@ static int mv_platform_probe(struct platform_device *pdev)
 err:
 #if defined(CONFIG_HAVE_CLK)
 	if (!IS_ERR(hpriv->clk)) {
-		clk_disable(hpriv->clk);
+		clk_disable_unprepare(hpriv->clk);
 		clk_put(hpriv->clk);
 	}
+	for (port = 0; port < n_ports; port++) {
+		if (!IS_ERR(hpriv->port_clks[port])) {
+			clk_disable_unprepare(hpriv->port_clks[port]);
+			clk_put(hpriv->port_clks[port]);
+		}
+	}
 #endif
 
 	return rc;
@@ -4119,14 +4144,21 @@ static int __devexit mv_platform_remove(struct platform_device *pdev)
 	struct ata_host *host = platform_get_drvdata(pdev);
 #if defined(CONFIG_HAVE_CLK)
 	struct mv_host_priv *hpriv = host->private_data;
+	int port;
 #endif
 	ata_host_detach(host);
 
 #if defined(CONFIG_HAVE_CLK)
 	if (!IS_ERR(hpriv->clk)) {
-		clk_disable(hpriv->clk);
+		clk_disable_unprepare(hpriv->clk);
 		clk_put(hpriv->clk);
 	}
+	for (port = 0; port < host->n_ports; port++) {
+		if (!IS_ERR(hpriv->port_clks[port])) {
+			clk_disable_unprepare(hpriv->port_clks[port]);
+			clk_put(hpriv->port_clks[port]);
+		}
+	}
 #endif
 	return 0;
 }
diff --git a/drivers/atm/ambassador.c b/drivers/atm/ambassador.c
index f8f41e0e8a8..89b30f32ba6 100644
--- a/drivers/atm/ambassador.c
+++ b/drivers/atm/ambassador.c
@@ -802,7 +802,7 @@ static void fill_rx_pool (amb_dev * dev, unsigned char pool,
     }
     // cast needed as there is no %? for pointer differences
     PRINTD (DBG_SKB, "allocated skb at %p, head %p, area %li",
-	    skb, skb->head, (long) (skb_end_pointer(skb) - skb->head));
+	    skb, skb->head, (long) skb_end_offset(skb));
     rx.handle = virt_to_bus (skb);
     rx.host_address = cpu_to_be32 (virt_to_bus (skb->data));
     if (rx_give (dev, &rx, pool))
diff --git a/drivers/atm/horizon.c b/drivers/atm/horizon.c
index 75fd691cd43..7d01c2a7525 100644
--- a/drivers/atm/horizon.c
+++ b/drivers/atm/horizon.c
@@ -2182,7 +2182,6 @@ static int hrz_open (struct atm_vcc *atm_vcc)
     default:
       PRINTD (DBG_QOS|DBG_VCC, "Bad AAL!");
       return -EINVAL;
-      break;
   }
   
   // TX traffic parameters
@@ -2357,7 +2356,6 @@ static int hrz_open (struct atm_vcc *atm_vcc)
       default: {
 	PRINTD (DBG_QOS, "unsupported TX traffic class");
 	return -EINVAL;
-	break;
       }
     }
   }
@@ -2433,7 +2431,6 @@ static int hrz_open (struct atm_vcc *atm_vcc)
       default: {
 	PRINTD (DBG_QOS, "unsupported RX traffic class");
 	return -EINVAL;
-	break;
       }
     }
   }
@@ -2581,7 +2578,6 @@ static int hrz_getsockopt (struct atm_vcc * atm_vcc, int level, int optname,
 //	  break;
 	default:
 	  return -ENOPROTOOPT;
-	  break;
       };
       break;
   }
@@ -2601,7 +2597,6 @@ static int hrz_setsockopt (struct atm_vcc * atm_vcc, int level, int optname,
 //	  break;
 	default:
 	  return -ENOPROTOOPT;
-	  break;
       };
       break;
   }
diff --git a/drivers/atm/idt77252.c b/drivers/atm/idt77252.c
index 1c052127548..8974bd2b961 100644
--- a/drivers/atm/idt77252.c
+++ b/drivers/atm/idt77252.c
@@ -1258,7 +1258,7 @@ idt77252_rx_raw(struct idt77252_dev *card)
 	tail = readl(SAR_REG_RAWCT);
 
 	pci_dma_sync_single_for_cpu(card->pcidev, IDT77252_PRV_PADDR(queue),
-				    skb_end_pointer(queue) - queue->head - 16,
+				    skb_end_offset(queue) - 16,
 				    PCI_DMA_FROMDEVICE);
 
 	while (head != tail) {
diff --git a/drivers/base/Kconfig b/drivers/base/Kconfig
index 9aa618acfe9..9b21469482a 100644
--- a/drivers/base/Kconfig
+++ b/drivers/base/Kconfig
@@ -192,4 +192,93 @@ config DMA_SHARED_BUFFER
 	  APIs extension; the file's descriptor can then be passed on to other
 	  driver.
 
+config CMA
+	bool "Contiguous Memory Allocator (EXPERIMENTAL)"
+	depends on HAVE_DMA_CONTIGUOUS && HAVE_MEMBLOCK && EXPERIMENTAL
+	select MIGRATION
+	help
+	  This enables the Contiguous Memory Allocator which allows drivers
+	  to allocate big physically-contiguous blocks of memory for use with
+	  hardware components that do not support I/O map nor scatter-gather.
+
+	  For more information see <include/linux/dma-contiguous.h>.
+	  If unsure, say "n".
+
+if CMA
+
+config CMA_DEBUG
+	bool "CMA debug messages (DEVELOPMENT)"
+	depends on DEBUG_KERNEL
+	help
+	  Turns on debug messages in CMA.  This produces KERN_DEBUG
+	  messages for every CMA call as well as various messages while
+	  processing calls such as dma_alloc_from_contiguous().
+	  This option does not affect warning and error messages.
+
+comment "Default contiguous memory area size:"
+
+config CMA_SIZE_MBYTES
+	int "Size in Mega Bytes"
+	depends on !CMA_SIZE_SEL_PERCENTAGE
+	default 16
+	help
+	  Defines the size (in MiB) of the default memory area for Contiguous
+	  Memory Allocator.
+
+config CMA_SIZE_PERCENTAGE
+	int "Percentage of total memory"
+	depends on !CMA_SIZE_SEL_MBYTES
+	default 10
+	help
+	  Defines the size of the default memory area for Contiguous Memory
+	  Allocator as a percentage of the total memory in the system.
+
+choice
+	prompt "Selected region size"
+	default CMA_SIZE_SEL_ABSOLUTE
+
+config CMA_SIZE_SEL_MBYTES
+	bool "Use mega bytes value only"
+
+config CMA_SIZE_SEL_PERCENTAGE
+	bool "Use percentage value only"
+
+config CMA_SIZE_SEL_MIN
+	bool "Use lower value (minimum)"
+
+config CMA_SIZE_SEL_MAX
+	bool "Use higher value (maximum)"
+
+endchoice
+
+config CMA_ALIGNMENT
+	int "Maximum PAGE_SIZE order of alignment for contiguous buffers"
+	range 4 9
+	default 8
+	help
+	  DMA mapping framework by default aligns all buffers to the smallest
+	  PAGE_SIZE order which is greater than or equal to the requested buffer
+	  size. This works well for buffers up to a few hundreds kilobytes, but
+	  for larger buffers it just a memory waste. With this parameter you can
+	  specify the maximum PAGE_SIZE order for contiguous buffers. Larger
+	  buffers will be aligned only to this specified order. The order is
+	  expressed as a power of two multiplied by the PAGE_SIZE.
+
+	  For example, if your system defaults to 4KiB pages, the order value
+	  of 8 means that the buffers will be aligned up to 1MiB only.
+
+	  If unsure, leave the default value "8".
+
+config CMA_AREAS
+	int "Maximum count of the CMA device-private areas"
+	default 7
+	help
+	  CMA allows to create CMA areas for particular devices. This parameter
+	  sets the maximum number of such device private CMA areas in the
+	  system.
+
+	  If unsure, leave the default value "7".
+
+endif
+
 endmenu
diff --git a/drivers/base/Makefile b/drivers/base/Makefile
index b6d1b9c4200..5aa2d703d19 100644
--- a/drivers/base/Makefile
+++ b/drivers/base/Makefile
@@ -6,6 +6,7 @@ obj-y			:= core.o bus.o dd.o syscore.o \
 			   attribute_container.o transport_class.o \
 			   topology.o
 obj-$(CONFIG_DEVTMPFS)	+= devtmpfs.o
+obj-$(CONFIG_CMA) += dma-contiguous.o
 obj-y			+= power/
 obj-$(CONFIG_HAS_DMA)	+= dma-mapping.o
 obj-$(CONFIG_HAVE_GENERIC_DMA_COHERENT) += dma-coherent.o
diff --git a/drivers/base/bus.c b/drivers/base/bus.c
index 26a06b801b5..2bcef657a60 100644
--- a/drivers/base/bus.c
+++ b/drivers/base/bus.c
@@ -21,8 +21,7 @@
 #include "power/power.h"
 
 /* /sys/devices/system */
-/* FIXME: make static after drivers/base/sys.c is deleted */
-struct kset *system_kset;
+static struct kset *system_kset;
 
 #define to_bus_attr(_attr) container_of(_attr, struct bus_attribute, attr)
 
diff --git a/drivers/base/core.c b/drivers/base/core.c
index e28ce9898af..346be8b78b2 100644
--- a/drivers/base/core.c
+++ b/drivers/base/core.c
@@ -25,6 +25,7 @@
 #include <linux/mutex.h>
 #include <linux/async.h>
 #include <linux/pm_runtime.h>
+#include <linux/netdevice.h>
 
 #include "base.h"
 #include "power/power.h"
@@ -65,7 +66,7 @@ static inline int device_is_not_partition(struct device *dev)
  * @dev: struct device to get the name of
  *
  * Will return the device's driver's name if it is bound to a device.  If
- * the device is not bound to a device, it will return the name of the bus
+ * the device is not bound to a driver, it will return the name of the bus
  * it is attached to.  If it is not attached to a bus either, an empty
  * string will be returned.
  */
@@ -878,8 +879,8 @@ EXPORT_SYMBOL_GPL(dev_set_name);
  * to NULL prevents an entry from being created.  class->dev_kobj must
  * be set (or cleared) before any devices are registered to the class
  * otherwise device_create_sys_dev_entry() and
- * device_remove_sys_dev_entry() will disagree about the the presence
- * of the link.
+ * device_remove_sys_dev_entry() will disagree about the presence of
+ * the link.
  */
 static struct kobject *device_to_dev_kobj(struct device *dev)
 {
@@ -1843,15 +1844,60 @@ void device_shutdown(void)
  */
 
 #ifdef CONFIG_PRINTK
-
 int __dev_printk(const char *level, const struct device *dev,
 		 struct va_format *vaf)
 {
+	char dict[128];
+	size_t dictlen = 0;
+	const char *subsys;
+
 	if (!dev)
 		return printk("%s(NULL device *): %pV", level, vaf);
 
-	return printk("%s%s %s: %pV",
-		      level, dev_driver_string(dev), dev_name(dev), vaf);
+	if (dev->class)
+		subsys = dev->class->name;
+	else if (dev->bus)
+		subsys = dev->bus->name;
+	else
+		goto skip;
+
+	dictlen += snprintf(dict + dictlen, sizeof(dict) - dictlen,
+			    "SUBSYSTEM=%s", subsys);
+
+	/*
+	 * Add device identifier DEVICE=:
+	 *   b12:8         block dev_t
+	 *   c127:3        char dev_t
+	 *   n8            netdev ifindex
+	 *   +sound:card0  subsystem:devname
+	 */
+	if (MAJOR(dev->devt)) {
+		char c;
+
+		if (strcmp(subsys, "block") == 0)
+			c = 'b';
+		else
+			c = 'c';
+		dictlen++;
+		dictlen += snprintf(dict + dictlen, sizeof(dict) - dictlen,
+				   "DEVICE=%c%u:%u",
+				   c, MAJOR(dev->devt), MINOR(dev->devt));
+	} else if (strcmp(subsys, "net") == 0) {
+		struct net_device *net = to_net_dev(dev);
+
+		dictlen++;
+		dictlen += snprintf(dict + dictlen, sizeof(dict) - dictlen,
+				    "DEVICE=n%u", net->ifindex);
+	} else {
+		dictlen++;
+		dictlen += snprintf(dict + dictlen, sizeof(dict) - dictlen,
+				    "DEVICE=+%s:%s", subsys, dev_name(dev));
+	}
+skip:
+	return printk_emit(0, level[1] - '0',
+			   dictlen ? dict : NULL, dictlen,
+			   "%s %s: %pV",
+			   dev_driver_string(dev), dev_name(dev), vaf);
 }
 EXPORT_SYMBOL(__dev_printk);
 
diff --git a/drivers/base/cpu.c b/drivers/base/cpu.c
index adf937bf409..63452943abd 100644
--- a/drivers/base/cpu.c
+++ b/drivers/base/cpu.c
@@ -330,8 +330,4 @@ void __init cpu_dev_init(void)
 		panic("Failed to register CPU subsystem");
 
 	cpu_dev_register_generic();
-
-#if defined(CONFIG_SCHED_MC) || defined(CONFIG_SCHED_SMT)
-	sched_create_sysfs_power_savings_entries(cpu_subsys.dev_root);
-#endif
 }
diff --git a/drivers/base/devres.c b/drivers/base/devres.c
index 524bf96c289..2360adb7a58 100644
--- a/drivers/base/devres.c
+++ b/drivers/base/devres.c
@@ -309,6 +309,10 @@ EXPORT_SYMBOL_GPL(devres_remove);
  * which @match returns 1.  If @match is NULL, it's considered to
  * match all.  If found, the resource is removed atomically and freed.
  *
+ * Note that the release function for the resource will not be called,
+ * only the devres-allocated data will be freed.  The caller becomes
+ * responsible for freeing any other data.
+ *
  * RETURNS:
  * 0 if devres is found and freed, -ENOENT if not found.
  */
@@ -326,6 +330,37 @@ int devres_destroy(struct device *dev, dr_release_t release,
 }
 EXPORT_SYMBOL_GPL(devres_destroy);
 
+
+/**
+ * devres_release - Find a device resource and destroy it, calling release
+ * @dev: Device to find resource from
+ * @release: Look for resources associated with this release function
+ * @match: Match function (optional)
+ * @match_data: Data for the match function
+ *
+ * Find the latest devres of @dev associated with @release and for
+ * which @match returns 1.  If @match is NULL, it's considered to
+ * match all.  If found, the resource is removed atomically, the
+ * release function called and the resource freed.
+ *
+ * RETURNS:
+ * 0 if devres is found and freed, -ENOENT if not found.
+ */
+int devres_release(struct device *dev, dr_release_t release,
+		   dr_match_t match, void *match_data)
+{
+	void *res;
+
+	res = devres_remove(dev, release, match, match_data);
+	if (unlikely(!res))
+		return -ENOENT;
+
+	(*release)(dev, res);
+	devres_free(res);
+	return 0;
+}
+EXPORT_SYMBOL_GPL(devres_release);
+
 static int remove_nodes(struct device *dev,
 			struct list_head *first, struct list_head *end,
 			struct list_head *todo)
diff --git a/drivers/base/devtmpfs.c b/drivers/base/devtmpfs.c
index 8493536ea55..765c3a28077 100644
--- a/drivers/base/devtmpfs.c
+++ b/drivers/base/devtmpfs.c
@@ -7,9 +7,9 @@
  * devtmpfs, a tmpfs-based filesystem is created. Every driver-core
  * device which requests a device node, will add a node in this
  * filesystem.
- * By default, all devices are named after the the name of the
- * device, owned by root and have a default mode of 0600. Subsystems
- * can overwrite the default setting if needed.
+ * By default, all devices are named after the name of the device,
+ * owned by root and have a default mode of 0600. Subsystems can
+ * overwrite the default setting if needed.
  */
 
 #include <linux/kernel.h>
diff --git a/drivers/base/dma-buf.c b/drivers/base/dma-buf.c
index 07cbbc6fddb..24e88fe29ec 100644
--- a/drivers/base/dma-buf.c
+++ b/drivers/base/dma-buf.c
@@ -44,8 +44,26 @@ static int dma_buf_release(struct inode *inode, struct file *file)
 	return 0;
 }
 
+static int dma_buf_mmap_internal(struct file *file, struct vm_area_struct *vma)
+{
+	struct dma_buf *dmabuf;
+
+	if (!is_dma_buf_file(file))
+		return -EINVAL;
+
+	dmabuf = file->private_data;
+
+	/* check for overflowing the buffer's size */
+	if (vma->vm_pgoff + ((vma->vm_end - vma->vm_start) >> PAGE_SHIFT) >
+	    dmabuf->size >> PAGE_SHIFT)
+		return -EINVAL;
+
+	return dmabuf->ops->mmap(dmabuf, vma);
+}
+
 static const struct file_operations dma_buf_fops = {
 	.release	= dma_buf_release,
+	.mmap		= dma_buf_mmap_internal,
 };
 
 /*
@@ -82,7 +100,8 @@ struct dma_buf *dma_buf_export(void *priv, const struct dma_buf_ops *ops,
 			  || !ops->unmap_dma_buf
 			  || !ops->release
 			  || !ops->kmap_atomic
-			  || !ops->kmap)) {
+			  || !ops->kmap
+			  || !ops->mmap)) {
 		return ERR_PTR(-EINVAL);
 	}
 
@@ -293,7 +312,7 @@ EXPORT_SYMBOL_GPL(dma_buf_unmap_attachment);
  * cpu in the kernel context. Calls begin_cpu_access to allow exporter-specific
  * preparations. Coherency is only guaranteed in the specified range for the
  * specified access direction.
- * @dma_buf:	[in]	buffer to prepare cpu access for.
+ * @dmabuf:	[in]	buffer to prepare cpu access for.
  * @start:	[in]	start of range for cpu access.
  * @len:	[in]	length of range for cpu access.
  * @direction:	[in]	length of range for cpu access.
@@ -320,7 +339,7 @@ EXPORT_SYMBOL_GPL(dma_buf_begin_cpu_access);
  * cpu in the kernel context. Calls end_cpu_access to allow exporter-specific
  * actions. Coherency is only guaranteed in the specified range for the
  * specified access direction.
- * @dma_buf:	[in]	buffer to complete cpu access for.
+ * @dmabuf:	[in]	buffer to complete cpu access for.
  * @start:	[in]	start of range for cpu access.
  * @len:	[in]	length of range for cpu access.
  * @direction:	[in]	length of range for cpu access.
@@ -340,7 +359,7 @@ EXPORT_SYMBOL_GPL(dma_buf_end_cpu_access);
 /**
  * dma_buf_kmap_atomic - Map a page of the buffer object into kernel address
  * space. The same restrictions as for kmap_atomic and friends apply.
- * @dma_buf:	[in]	buffer to map page from.
+ * @dmabuf:	[in]	buffer to map page from.
  * @page_num:	[in]	page in PAGE_SIZE units to map.
  *
  * This call must always succeed, any necessary preparations that might fail
@@ -356,7 +375,7 @@ EXPORT_SYMBOL_GPL(dma_buf_kmap_atomic);
 
 /**
  * dma_buf_kunmap_atomic - Unmap a page obtained by dma_buf_kmap_atomic.
- * @dma_buf:	[in]	buffer to unmap page from.
+ * @dmabuf:	[in]	buffer to unmap page from.
  * @page_num:	[in]	page in PAGE_SIZE units to unmap.
  * @vaddr:	[in]	kernel space pointer obtained from dma_buf_kmap_atomic.
  *
@@ -375,7 +394,7 @@ EXPORT_SYMBOL_GPL(dma_buf_kunmap_atomic);
 /**
  * dma_buf_kmap - Map a page of the buffer object into kernel address space. The
  * same restrictions as for kmap and friends apply.
- * @dma_buf:	[in]	buffer to map page from.
+ * @dmabuf:	[in]	buffer to map page from.
  * @page_num:	[in]	page in PAGE_SIZE units to map.
  *
  * This call must always succeed, any necessary preparations that might fail
@@ -391,7 +410,7 @@ EXPORT_SYMBOL_GPL(dma_buf_kmap);
 
 /**
  * dma_buf_kunmap - Unmap a page obtained by dma_buf_kmap.
- * @dma_buf:	[in]	buffer to unmap page from.
+ * @dmabuf:	[in]	buffer to unmap page from.
  * @page_num:	[in]	page in PAGE_SIZE units to unmap.
  * @vaddr:	[in]	kernel space pointer obtained from dma_buf_kmap.
  *
@@ -406,3 +425,81 @@ void dma_buf_kunmap(struct dma_buf *dmabuf, unsigned long page_num,
 		dmabuf->ops->kunmap(dmabuf, page_num, vaddr);
 }
 EXPORT_SYMBOL_GPL(dma_buf_kunmap);
+
+
+/**
+ * dma_buf_mmap - Setup up a userspace mmap with the given vma
+ * @dmabuf:	[in]	buffer that should back the vma
+ * @vma:	[in]	vma for the mmap
+ * @pgoff:	[in]	offset in pages where this mmap should start within the
+ * 			dma-buf buffer.
+ *
+ * This function adjusts the passed in vma so that it points at the file of the
+ * dma_buf operation. It alsog adjusts the starting pgoff and does bounds
+ * checking on the size of the vma. Then it calls the exporters mmap function to
+ * set up the mapping.
+ *
+ * Can return negative error values, returns 0 on success.
+ */
+int dma_buf_mmap(struct dma_buf *dmabuf, struct vm_area_struct *vma,
+		 unsigned long pgoff)
+{
+	if (WARN_ON(!dmabuf || !vma))
+		return -EINVAL;
+
+	/* check for offset overflow */
+	if (pgoff + ((vma->vm_end - vma->vm_start) >> PAGE_SHIFT) < pgoff)
+		return -EOVERFLOW;
+
+	/* check for overflowing the buffer's size */
+	if (pgoff + ((vma->vm_end - vma->vm_start) >> PAGE_SHIFT) >
+	    dmabuf->size >> PAGE_SHIFT)
+		return -EINVAL;
+
+	/* readjust the vma */
+	if (vma->vm_file)
+		fput(vma->vm_file);
+
+	vma->vm_file = dmabuf->file;
+	get_file(vma->vm_file);
+
+	vma->vm_pgoff = pgoff;
+
+	return dmabuf->ops->mmap(dmabuf, vma);
+}
+EXPORT_SYMBOL_GPL(dma_buf_mmap);
+
+/**
+ * dma_buf_vmap - Create virtual mapping for the buffer object into kernel
+ * address space. Same restrictions as for vmap and friends apply.
+ * @dmabuf:	[in]	buffer to vmap
+ *
+ * This call may fail due to lack of virtual mapping address space.
+ * These calls are optional in drivers. The intended use for them
+ * is for mapping objects linear in kernel space for high use objects.
+ * Please attempt to use kmap/kunmap before thinking about these interfaces.
+ */
+void *dma_buf_vmap(struct dma_buf *dmabuf)
+{
+	if (WARN_ON(!dmabuf))
+		return NULL;
+
+	if (dmabuf->ops->vmap)
+		return dmabuf->ops->vmap(dmabuf);
+	return NULL;
+}
+EXPORT_SYMBOL_GPL(dma_buf_vmap);
+
+/**
+ * dma_buf_vunmap - Unmap a vmap obtained by dma_buf_vmap.
+ * @dmabuf:	[in]	buffer to vunmap
+ */
+void dma_buf_vunmap(struct dma_buf *dmabuf, void *vaddr)
+{
+	if (WARN_ON(!dmabuf))
+		return;
+
+	if (dmabuf->ops->vunmap)
+		dmabuf->ops->vunmap(dmabuf, vaddr);
+}
+EXPORT_SYMBOL_GPL(dma_buf_vunmap);
diff --git a/drivers/base/dma-coherent.c b/drivers/base/dma-coherent.c
index bb0025c510b..1b85949e3d2 100644
--- a/drivers/base/dma-coherent.c
+++ b/drivers/base/dma-coherent.c
@@ -10,6 +10,7 @@
 struct dma_coherent_mem {
 	void		*virt_base;
 	dma_addr_t	device_base;
+	phys_addr_t	pfn_base;
 	int		size;
 	int		flags;
 	unsigned long	*bitmap;
@@ -44,6 +45,7 @@ int dma_declare_coherent_memory(struct device *dev, dma_addr_t bus_addr,
 
 	dev->dma_mem->virt_base = mem_base;
 	dev->dma_mem->device_base = device_addr;
+	dev->dma_mem->pfn_base = PFN_DOWN(bus_addr);
 	dev->dma_mem->size = pages;
 	dev->dma_mem->flags = flags;
 
@@ -176,3 +178,43 @@ int dma_release_from_coherent(struct device *dev, int order, void *vaddr)
 	return 0;
 }
 EXPORT_SYMBOL(dma_release_from_coherent);
+
+/**
+ * dma_mmap_from_coherent() - try to mmap the memory allocated from
+ * per-device coherent memory pool to userspace
+ * @dev:	device from which the memory was allocated
+ * @vma:	vm_area for the userspace memory
+ * @vaddr:	cpu address returned by dma_alloc_from_coherent
+ * @size:	size of the memory buffer allocated by dma_alloc_from_coherent
+ *
+ * This checks whether the memory was allocated from the per-device
+ * coherent memory pool and if so, maps that memory to the provided vma.
+ *
+ * Returns 1 if we correctly mapped the memory, or 0 if
+ * dma_release_coherent() should proceed with mapping memory from
+ * generic pools.
+ */
+int dma_mmap_from_coherent(struct device *dev, struct vm_area_struct *vma,
+			   void *vaddr, size_t size, int *ret)
+{
+	struct dma_coherent_mem *mem = dev ? dev->dma_mem : NULL;
+
+	if (mem && vaddr >= mem->virt_base && vaddr + size <=
+		   (mem->virt_base + (mem->size << PAGE_SHIFT))) {
+		unsigned long off = vma->vm_pgoff;
+		int start = (vaddr - mem->virt_base) >> PAGE_SHIFT;
+		int user_count = (vma->vm_end - vma->vm_start) >> PAGE_SHIFT;
+		int count = size >> PAGE_SHIFT;
+
+		*ret = -ENXIO;
+		if (off < count && user_count <= count - off) {
+			unsigned pfn = mem->pfn_base + start + off;
+			*ret = remap_pfn_range(vma, vma->vm_start, pfn,
+					       user_count << PAGE_SHIFT,
+					       vma->vm_page_prot);
+		}
+		return 1;
+	}
+	return 0;
+}
+EXPORT_SYMBOL(dma_mmap_from_coherent);
diff --git a/drivers/base/dma-contiguous.c b/drivers/base/dma-contiguous.c
new file mode 100644
index 00000000000..78efb0306a4
--- /dev/null
+++ b/drivers/base/dma-contiguous.c
@@ -0,0 +1,401 @@
+/*
+ * Contiguous Memory Allocator for DMA mapping framework
+ * Copyright (c) 2010-2011 by Samsung Electronics.
+ * Written by:
+ *	Marek Szyprowski <m.szyprowski@samsung.com>
+ *	Michal Nazarewicz <mina86@mina86.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 optional) any later version of the license.
+ */
+
+#define pr_fmt(fmt) "cma: " fmt
+
+#ifdef CONFIG_CMA_DEBUG
+#ifndef DEBUG
+#  define DEBUG
+#endif
+#endif
+
+#include <asm/page.h>
+#include <asm/dma-contiguous.h>
+
+#include <linux/memblock.h>
+#include <linux/err.h>
+#include <linux/mm.h>
+#include <linux/mutex.h>
+#include <linux/page-isolation.h>
+#include <linux/slab.h>
+#include <linux/swap.h>
+#include <linux/mm_types.h>
+#include <linux/dma-contiguous.h>
+
+#ifndef SZ_1M
+#define SZ_1M (1 << 20)
+#endif
+
+struct cma {
+	unsigned long	base_pfn;
+	unsigned long	count;
+	unsigned long	*bitmap;
+};
+
+struct cma *dma_contiguous_default_area;
+
+#ifdef CONFIG_CMA_SIZE_MBYTES
+#define CMA_SIZE_MBYTES CONFIG_CMA_SIZE_MBYTES
+#else
+#define CMA_SIZE_MBYTES 0
+#endif
+
+/*
+ * Default global CMA area size can be defined in kernel's .config.
+ * This is usefull mainly for distro maintainers to create a kernel
+ * that works correctly for most supported systems.
+ * The size can be set in bytes or as a percentage of the total memory
+ * in the system.
+ *
+ * Users, who want to set the size of global CMA area for their system
+ * should use cma= kernel parameter.
+ */
+static const unsigned long size_bytes = CMA_SIZE_MBYTES * SZ_1M;
+static long size_cmdline = -1;
+
+static int __init early_cma(char *p)
+{
+	pr_debug("%s(%s)\n", __func__, p);
+	size_cmdline = memparse(p, &p);
+	return 0;
+}
+early_param("cma", early_cma);
+
+#ifdef CONFIG_CMA_SIZE_PERCENTAGE
+
+static unsigned long __init __maybe_unused cma_early_percent_memory(void)
+{
+	struct memblock_region *reg;
+	unsigned long total_pages = 0;
+
+	/*
+	 * We cannot use memblock_phys_mem_size() here, because
+	 * memblock_analyze() has not been called yet.
+	 */
+	for_each_memblock(memory, reg)
+		total_pages += memblock_region_memory_end_pfn(reg) -
+			       memblock_region_memory_base_pfn(reg);
+
+	return (total_pages * CONFIG_CMA_SIZE_PERCENTAGE / 100) << PAGE_SHIFT;
+}
+
+#else
+
+static inline __maybe_unused unsigned long cma_early_percent_memory(void)
+{
+	return 0;
+}
+
+#endif
+
+/**
+ * dma_contiguous_reserve() - reserve area for contiguous memory handling
+ * @limit: End address of the reserved memory (optional, 0 for any).
+ *
+ * This function reserves memory from early allocator. It should be
+ * called by arch specific code once the early allocator (memblock or bootmem)
+ * has been activated and all other subsystems have already allocated/reserved
+ * memory.
+ */
+void __init dma_contiguous_reserve(phys_addr_t limit)
+{
+	unsigned long selected_size = 0;
+
+	pr_debug("%s(limit %08lx)\n", __func__, (unsigned long)limit);
+
+	if (size_cmdline != -1) {
+		selected_size = size_cmdline;
+	} else {
+#ifdef CONFIG_CMA_SIZE_SEL_MBYTES
+		selected_size = size_bytes;
+#elif defined(CONFIG_CMA_SIZE_SEL_PERCENTAGE)
+		selected_size = cma_early_percent_memory();
+#elif defined(CONFIG_CMA_SIZE_SEL_MIN)
+		selected_size = min(size_bytes, cma_early_percent_memory());
+#elif defined(CONFIG_CMA_SIZE_SEL_MAX)
+		selected_size = max(size_bytes, cma_early_percent_memory());
+#endif
+	}
+
+	if (selected_size) {
+		pr_debug("%s: reserving %ld MiB for global area\n", __func__,
+			 selected_size / SZ_1M);
+
+		dma_declare_contiguous(NULL, selected_size, 0, limit);
+	}
+};
+
+static DEFINE_MUTEX(cma_mutex);
+
+static __init int cma_activate_area(unsigned long base_pfn, unsigned long count)
+{
+	unsigned long pfn = base_pfn;
+	unsigned i = count >> pageblock_order;
+	struct zone *zone;
+
+	WARN_ON_ONCE(!pfn_valid(pfn));
+	zone = page_zone(pfn_to_page(pfn));
+
+	do {
+		unsigned j;
+		base_pfn = pfn;
+		for (j = pageblock_nr_pages; j; --j, pfn++) {
+			WARN_ON_ONCE(!pfn_valid(pfn));
+			if (page_zone(pfn_to_page(pfn)) != zone)
+				return -EINVAL;
+		}
+		init_cma_reserved_pageblock(pfn_to_page(base_pfn));
+	} while (--i);
+	return 0;
+}
+
+static __init struct cma *cma_create_area(unsigned long base_pfn,
+				     unsigned long count)
+{
+	int bitmap_size = BITS_TO_LONGS(count) * sizeof(long);
+	struct cma *cma;
+	int ret = -ENOMEM;
+
+	pr_debug("%s(base %08lx, count %lx)\n", __func__, base_pfn, count);
+
+	cma = kmalloc(sizeof *cma, GFP_KERNEL);
+	if (!cma)
+		return ERR_PTR(-ENOMEM);
+
+	cma->base_pfn = base_pfn;
+	cma->count = count;
+	cma->bitmap = kzalloc(bitmap_size, GFP_KERNEL);
+
+	if (!cma->bitmap)
+		goto no_mem;
+
+	ret = cma_activate_area(base_pfn, count);
+	if (ret)
+		goto error;
+
+	pr_debug("%s: returned %p\n", __func__, (void *)cma);
+	return cma;
+
+error:
+	kfree(cma->bitmap);
+no_mem:
+	kfree(cma);
+	return ERR_PTR(ret);
+}
+
+static struct cma_reserved {
+	phys_addr_t start;
+	unsigned long size;
+	struct device *dev;
+} cma_reserved[MAX_CMA_AREAS] __initdata;
+static unsigned cma_reserved_count __initdata;
+
+static int __init cma_init_reserved_areas(void)
+{
+	struct cma_reserved *r = cma_reserved;
+	unsigned i = cma_reserved_count;
+
+	pr_debug("%s()\n", __func__);
+
+	for (; i; --i, ++r) {
+		struct cma *cma;
+		cma = cma_create_area(PFN_DOWN(r->start),
+				      r->size >> PAGE_SHIFT);
+		if (!IS_ERR(cma))
+			dev_set_cma_area(r->dev, cma);
+	}
+	return 0;
+}
+core_initcall(cma_init_reserved_areas);
+
+/**
+ * dma_declare_contiguous() - reserve area for contiguous memory handling
+ *			      for particular device
+ * @dev:   Pointer to device structure.
+ * @size:  Size of the reserved memory.
+ * @base:  Start address of the reserved memory (optional, 0 for any).
+ * @limit: End address of the reserved memory (optional, 0 for any).
+ *
+ * This function reserves memory for specified device. It should be
+ * called by board specific code when early allocator (memblock or bootmem)
+ * is still activate.
+ */
+int __init dma_declare_contiguous(struct device *dev, unsigned long size,
+				  phys_addr_t base, phys_addr_t limit)
+{
+	struct cma_reserved *r = &cma_reserved[cma_reserved_count];
+	unsigned long alignment;
+
+	pr_debug("%s(size %lx, base %08lx, limit %08lx)\n", __func__,
+		 (unsigned long)size, (unsigned long)base,
+		 (unsigned long)limit);
+
+	/* Sanity checks */
+	if (cma_reserved_count == ARRAY_SIZE(cma_reserved)) {
+		pr_err("Not enough slots for CMA reserved regions!\n");
+		return -ENOSPC;
+	}
+
+	if (!size)
+		return -EINVAL;
+
+	/* Sanitise input arguments */
+	alignment = PAGE_SIZE << max(MAX_ORDER, pageblock_order);
+	base = ALIGN(base, alignment);
+	size = ALIGN(size, alignment);
+	limit &= ~(alignment - 1);
+
+	/* Reserve memory */
+	if (base) {
+		if (memblock_is_region_reserved(base, size) ||
+		    memblock_reserve(base, size) < 0) {
+			base = -EBUSY;
+			goto err;
+		}
+	} else {
+		/*
+		 * Use __memblock_alloc_base() since
+		 * memblock_alloc_base() panic()s.
+		 */
+		phys_addr_t addr = __memblock_alloc_base(size, alignment, limit);
+		if (!addr) {
+			base = -ENOMEM;
+			goto err;
+		} else if (addr + size > ~(unsigned long)0) {
+			memblock_free(addr, size);
+			base = -EINVAL;
+			goto err;
+		} else {
+			base = addr;
+		}
+	}
+
+	/*
+	 * Each reserved area must be initialised later, when more kernel
+	 * subsystems (like slab allocator) are available.
+	 */
+	r->start = base;
+	r->size = size;
+	r->dev = dev;
+	cma_reserved_count++;
+	pr_info("CMA: reserved %ld MiB at %08lx\n", size / SZ_1M,
+		(unsigned long)base);
+
+	/* Architecture specific contiguous memory fixup. */
+	dma_contiguous_early_fixup(base, size);
+	return 0;
+err:
+	pr_err("CMA: failed to reserve %ld MiB\n", size / SZ_1M);
+	return base;
+}
+
+/**
+ * dma_alloc_from_contiguous() - allocate pages from contiguous area
+ * @dev:   Pointer to device for which the allocation is performed.
+ * @count: Requested number of pages.
+ * @align: Requested alignment of pages (in PAGE_SIZE order).
+ *
+ * This function allocates memory buffer for specified device. It uses
+ * device specific contiguous memory area if available or the default
+ * global one. Requires architecture specific get_dev_cma_area() helper
+ * function.
+ */
+struct page *dma_alloc_from_contiguous(struct device *dev, int count,
+				       unsigned int align)
+{
+	unsigned long mask, pfn, pageno, start = 0;
+	struct cma *cma = dev_get_cma_area(dev);
+	int ret;
+
+	if (!cma || !cma->count)
+		return NULL;
+
+	if (align > CONFIG_CMA_ALIGNMENT)
+		align = CONFIG_CMA_ALIGNMENT;
+
+	pr_debug("%s(cma %p, count %d, align %d)\n", __func__, (void *)cma,
+		 count, align);
+
+	if (!count)
+		return NULL;
+
+	mask = (1 << align) - 1;
+
+	mutex_lock(&cma_mutex);
+
+	for (;;) {
+		pageno = bitmap_find_next_zero_area(cma->bitmap, cma->count,
+						    start, count, mask);
+		if (pageno >= cma->count) {
+			ret = -ENOMEM;
+			goto error;
+		}
+
+		pfn = cma->base_pfn + pageno;
+		ret = alloc_contig_range(pfn, pfn + count, MIGRATE_CMA);
+		if (ret == 0) {
+			bitmap_set(cma->bitmap, pageno, count);
+			break;
+		} else if (ret != -EBUSY) {
+			goto error;
+		}
+		pr_debug("%s(): memory range at %p is busy, retrying\n",
+			 __func__, pfn_to_page(pfn));
+		/* try again with a bit different memory target */
+		start = pageno + mask + 1;
+	}
+
+	mutex_unlock(&cma_mutex);
+
+	pr_debug("%s(): returned %p\n", __func__, pfn_to_page(pfn));
+	return pfn_to_page(pfn);
+error:
+	mutex_unlock(&cma_mutex);
+	return NULL;
+}
+
+/**
+ * dma_release_from_contiguous() - release allocated pages
+ * @dev:   Pointer to device for which the pages were allocated.
+ * @pages: Allocated pages.
+ * @count: Number of allocated pages.
+ *
+ * This function releases memory allocated by dma_alloc_from_contiguous().
+ * It returns false when provided pages do not belong to contiguous area and
+ * true otherwise.
+ */
+bool dma_release_from_contiguous(struct device *dev, struct page *pages,
+				 int count)
+{
+	struct cma *cma = dev_get_cma_area(dev);
+	unsigned long pfn;
+
+	if (!cma || !pages)
+		return false;
+
+	pr_debug("%s(page %p)\n", __func__, (void *)pages);
+
+	pfn = page_to_pfn(pages);
+
+	if (pfn < cma->base_pfn || pfn >= cma->base_pfn + cma->count)
+		return false;
+
+	VM_BUG_ON(pfn + count > cma->base_pfn + cma->count);
+
+	mutex_lock(&cma_mutex);
+	bitmap_clear(cma->bitmap, pfn - cma->base_pfn, count);
+	free_contig_range(pfn, count);
+	mutex_unlock(&cma_mutex);
+
+	return true;
+}
diff --git a/drivers/base/driver.c b/drivers/base/driver.c
index 3ec3896c83a..207c27ddf82 100644
--- a/drivers/base/driver.c
+++ b/drivers/base/driver.c
@@ -80,7 +80,7 @@ struct device *driver_find_device(struct device_driver *drv,
 	struct klist_iter i;
 	struct device *dev;
 
-	if (!drv)
+	if (!drv || !drv->p)
 		return NULL;
 
 	klist_iter_init_node(&drv->p->klist_devices, &i,
diff --git a/drivers/base/node.c b/drivers/base/node.c
index 90aa2a11a93..af1a177216f 100644
--- a/drivers/base/node.c
+++ b/drivers/base/node.c
@@ -592,11 +592,9 @@ static ssize_t print_nodes_state(enum node_states state, char *buf)
 {
 	int n;
 
-	n = nodelist_scnprintf(buf, PAGE_SIZE, node_states[state]);
-	if (n > 0 && PAGE_SIZE > n + 1) {
-		*(buf + n++) = '\n';
-		*(buf + n++) = '\0';
-	}
+	n = nodelist_scnprintf(buf, PAGE_SIZE-2, node_states[state]);
+	buf[n++] = '\n';
+	buf[n] = '\0';
 	return n;
 }
 
diff --git a/drivers/base/power/domain.c b/drivers/base/power/domain.c
index 73ce9fbe983..83aa694a8ef 100644
--- a/drivers/base/power/domain.c
+++ b/drivers/base/power/domain.c
@@ -11,6 +11,7 @@
 #include <linux/io.h>
 #include <linux/pm_runtime.h>
 #include <linux/pm_domain.h>
+#include <linux/pm_qos.h>
 #include <linux/slab.h>
 #include <linux/err.h>
 #include <linux/sched.h>
@@ -38,11 +39,13 @@
 	ktime_t __start = ktime_get();						\
 	type __retval = GENPD_DEV_CALLBACK(genpd, type, callback, dev);		\
 	s64 __elapsed = ktime_to_ns(ktime_sub(ktime_get(), __start));		\
-	struct generic_pm_domain_data *__gpd_data = dev_gpd_data(dev);		\
-	if (__elapsed > __gpd_data->td.field) {					\
-		__gpd_data->td.field = __elapsed;				\
+	struct gpd_timing_data *__td = &dev_gpd_data(dev)->td;			\
+	if (!__retval && __elapsed > __td->field) {				\
+		__td->field = __elapsed;					\
 		dev_warn(dev, name " latency exceeded, new value %lld ns\n",	\
 			__elapsed);						\
+		genpd->max_off_time_changed = true;				\
+		__td->constraint_changed = true;				\
 	}									\
 	__retval;								\
 })
@@ -211,6 +214,7 @@ int __pm_genpd_poweron(struct generic_pm_domain *genpd)
 		elapsed_ns = ktime_to_ns(ktime_sub(ktime_get(), time_start));
 		if (elapsed_ns > genpd->power_on_latency_ns) {
 			genpd->power_on_latency_ns = elapsed_ns;
+			genpd->max_off_time_changed = true;
 			if (genpd->name)
 				pr_warning("%s: Power-on latency exceeded, "
 					"new value %lld ns\n", genpd->name,
@@ -247,6 +251,53 @@ int pm_genpd_poweron(struct generic_pm_domain *genpd)
 
 #ifdef CONFIG_PM_RUNTIME
 
+static int genpd_dev_pm_qos_notifier(struct notifier_block *nb,
+				     unsigned long val, void *ptr)
+{
+	struct generic_pm_domain_data *gpd_data;
+	struct device *dev;
+
+	gpd_data = container_of(nb, struct generic_pm_domain_data, nb);
+
+	mutex_lock(&gpd_data->lock);
+	dev = gpd_data->base.dev;
+	if (!dev) {
+		mutex_unlock(&gpd_data->lock);
+		return NOTIFY_DONE;
+	}
+	mutex_unlock(&gpd_data->lock);
+
+	for (;;) {
+		struct generic_pm_domain *genpd;
+		struct pm_domain_data *pdd;
+
+		spin_lock_irq(&dev->power.lock);
+
+		pdd = dev->power.subsys_data ?
+				dev->power.subsys_data->domain_data : NULL;
+		if (pdd) {
+			to_gpd_data(pdd)->td.constraint_changed = true;
+			genpd = dev_to_genpd(dev);
+		} else {
+			genpd = ERR_PTR(-ENODATA);
+		}
+
+		spin_unlock_irq(&dev->power.lock);
+
+		if (!IS_ERR(genpd)) {
+			mutex_lock(&genpd->lock);
+			genpd->max_off_time_changed = true;
+			mutex_unlock(&genpd->lock);
+		}
+
+		dev = dev->parent;
+		if (!dev || dev->power.ignore_children)
+			break;
+	}
+
+	return NOTIFY_DONE;
+}
+
 /**
  * __pm_genpd_save_device - Save the pre-suspend state of a device.
  * @pdd: Domain data of the device to save the state of.
@@ -435,6 +486,7 @@ static int pm_genpd_poweroff(struct generic_pm_domain *genpd)
 		elapsed_ns = ktime_to_ns(ktime_sub(ktime_get(), time_start));
 		if (elapsed_ns > genpd->power_off_latency_ns) {
 			genpd->power_off_latency_ns = elapsed_ns;
+			genpd->max_off_time_changed = true;
 			if (genpd->name)
 				pr_warning("%s: Power-off latency exceeded, "
 					"new value %lld ns\n", genpd->name,
@@ -443,17 +495,6 @@ static int pm_genpd_poweroff(struct generic_pm_domain *genpd)
 	}
 
 	genpd->status = GPD_STATE_POWER_OFF;
-	genpd->power_off_time = ktime_get();
-
-	/* Update PM QoS information for devices in the domain. */
-	list_for_each_entry_reverse(pdd, &genpd->dev_list, list_node) {
-		struct gpd_timing_data *td = &to_gpd_data(pdd)->td;
-
-		pm_runtime_update_max_time_suspended(pdd->dev,
-					td->start_latency_ns +
-					td->restore_state_latency_ns +
-					genpd->power_on_latency_ns);
-	}
 
 	list_for_each_entry(link, &genpd->slave_links, slave_node) {
 		genpd_sd_counter_dec(link->master);
@@ -514,9 +555,6 @@ static int pm_genpd_runtime_suspend(struct device *dev)
 	if (ret)
 		return ret;
 
-	pm_runtime_update_max_time_suspended(dev,
-				dev_gpd_data(dev)->td.start_latency_ns);
-
 	/*
 	 * If power.irq_safe is set, this routine will be run with interrupts
 	 * off, so it can't use mutexes.
@@ -613,6 +651,12 @@ void pm_genpd_poweroff_unused(void)
 
 #else
 
+static inline int genpd_dev_pm_qos_notifier(struct notifier_block *nb,
+					    unsigned long val, void *ptr)
+{
+	return NOTIFY_DONE;
+}
+
 static inline void genpd_power_off_work_fn(struct work_struct *work) {}
 
 #define pm_genpd_runtime_suspend	NULL
@@ -1209,12 +1253,15 @@ int __pm_genpd_add_device(struct generic_pm_domain *genpd, struct device *dev,
 	if (IS_ERR_OR_NULL(genpd) || IS_ERR_OR_NULL(dev))
 		return -EINVAL;
 
-	genpd_acquire_lock(genpd);
+	gpd_data = kzalloc(sizeof(*gpd_data), GFP_KERNEL);
+	if (!gpd_data)
+		return -ENOMEM;
 
-	if (genpd->status == GPD_STATE_POWER_OFF) {
-		ret = -EINVAL;
-		goto out;
-	}
+	mutex_init(&gpd_data->lock);
+	gpd_data->nb.notifier_call = genpd_dev_pm_qos_notifier;
+	dev_pm_qos_add_notifier(dev, &gpd_data->nb);
+
+	genpd_acquire_lock(genpd);
 
 	if (genpd->prepared_count > 0) {
 		ret = -EAGAIN;
@@ -1227,26 +1274,35 @@ int __pm_genpd_add_device(struct generic_pm_domain *genpd, struct device *dev,
 			goto out;
 		}
 
-	gpd_data = kzalloc(sizeof(*gpd_data), GFP_KERNEL);
-	if (!gpd_data) {
-		ret = -ENOMEM;
-		goto out;
-	}
-
 	genpd->device_count++;
+	genpd->max_off_time_changed = true;
 
-	dev->pm_domain = &genpd->domain;
 	dev_pm_get_subsys_data(dev);
+
+	mutex_lock(&gpd_data->lock);
+	spin_lock_irq(&dev->power.lock);
+	dev->pm_domain = &genpd->domain;
 	dev->power.subsys_data->domain_data = &gpd_data->base;
 	gpd_data->base.dev = dev;
-	gpd_data->need_restore = false;
 	list_add_tail(&gpd_data->base.list_node, &genpd->dev_list);
+	gpd_data->need_restore = genpd->status == GPD_STATE_POWER_OFF;
 	if (td)
 		gpd_data->td = *td;
 
+	gpd_data->td.constraint_changed = true;
+	gpd_data->td.effective_constraint_ns = -1;
+	spin_unlock_irq(&dev->power.lock);
+	mutex_unlock(&gpd_data->lock);
+
+	genpd_release_lock(genpd);
+
+	return 0;
+
  out:
 	genpd_release_lock(genpd);
 
+	dev_pm_qos_remove_notifier(dev, &gpd_data->nb);
+	kfree(gpd_data);
 	return ret;
 }
 
@@ -1290,12 +1346,15 @@ int __pm_genpd_of_add_device(struct device_node *genpd_node, struct device *dev,
 int pm_genpd_remove_device(struct generic_pm_domain *genpd,
 			   struct device *dev)
 {
+	struct generic_pm_domain_data *gpd_data;
 	struct pm_domain_data *pdd;
-	int ret = -EINVAL;
+	int ret = 0;
 
 	dev_dbg(dev, "%s()\n", __func__);
 
-	if (IS_ERR_OR_NULL(genpd) || IS_ERR_OR_NULL(dev))
+	if (IS_ERR_OR_NULL(genpd) || IS_ERR_OR_NULL(dev)
+	    ||  IS_ERR_OR_NULL(dev->pm_domain)
+	    ||  pd_to_genpd(dev->pm_domain) != genpd)
 		return -EINVAL;
 
 	genpd_acquire_lock(genpd);
@@ -1305,21 +1364,27 @@ int pm_genpd_remove_device(struct generic_pm_domain *genpd,
 		goto out;
 	}
 
-	list_for_each_entry(pdd, &genpd->dev_list, list_node) {
-		if (pdd->dev != dev)
-			continue;
+	genpd->device_count--;
+	genpd->max_off_time_changed = true;
 
-		list_del_init(&pdd->list_node);
-		pdd->dev = NULL;
-		dev_pm_put_subsys_data(dev);
-		dev->pm_domain = NULL;
-		kfree(to_gpd_data(pdd));
+	spin_lock_irq(&dev->power.lock);
+	dev->pm_domain = NULL;
+	pdd = dev->power.subsys_data->domain_data;
+	list_del_init(&pdd->list_node);
+	dev->power.subsys_data->domain_data = NULL;
+	spin_unlock_irq(&dev->power.lock);
 
-		genpd->device_count--;
+	gpd_data = to_gpd_data(pdd);
+	mutex_lock(&gpd_data->lock);
+	pdd->dev = NULL;
+	mutex_unlock(&gpd_data->lock);
 
-		ret = 0;
-		break;
-	}
+	genpd_release_lock(genpd);
+
+	dev_pm_qos_remove_notifier(dev, &gpd_data->nb);
+	kfree(gpd_data);
+	dev_pm_put_subsys_data(dev);
+	return 0;
 
  out:
 	genpd_release_lock(genpd);
@@ -1348,6 +1413,26 @@ void pm_genpd_dev_always_on(struct device *dev, bool val)
 EXPORT_SYMBOL_GPL(pm_genpd_dev_always_on);
 
 /**
+ * pm_genpd_dev_need_restore - Set/unset the device's "need restore" flag.
+ * @dev: Device to set/unset the flag for.
+ * @val: The new value of the device's "need restore" flag.
+ */
+void pm_genpd_dev_need_restore(struct device *dev, bool val)
+{
+	struct pm_subsys_data *psd;
+	unsigned long flags;
+
+	spin_lock_irqsave(&dev->power.lock, flags);
+
+	psd = dev_to_psd(dev);
+	if (psd && psd->domain_data)
+		to_gpd_data(psd->domain_data)->need_restore = val;
+
+	spin_unlock_irqrestore(&dev->power.lock, flags);
+}
+EXPORT_SYMBOL_GPL(pm_genpd_dev_need_restore);
+
+/**
  * pm_genpd_add_subdomain - Add a subdomain to an I/O PM domain.
  * @genpd: Master PM domain to add the subdomain to.
  * @subdomain: Subdomain to be added.
@@ -1378,7 +1463,7 @@ int pm_genpd_add_subdomain(struct generic_pm_domain *genpd,
 		goto out;
 	}
 
-	list_for_each_entry(link, &genpd->slave_links, slave_node) {
+	list_for_each_entry(link, &genpd->master_links, master_node) {
 		if (link->slave == subdomain && link->master == genpd) {
 			ret = -EINVAL;
 			goto out;
@@ -1690,6 +1775,7 @@ void pm_genpd_init(struct generic_pm_domain *genpd,
 	genpd->resume_count = 0;
 	genpd->device_count = 0;
 	genpd->max_off_time_ns = -1;
+	genpd->max_off_time_changed = true;
 	genpd->domain.ops.runtime_suspend = pm_genpd_runtime_suspend;
 	genpd->domain.ops.runtime_resume = pm_genpd_runtime_resume;
 	genpd->domain.ops.runtime_idle = pm_generic_runtime_idle;
diff --git a/drivers/base/power/domain_governor.c b/drivers/base/power/domain_governor.c
index 66a265bf586..28dee3053f1 100644
--- a/drivers/base/power/domain_governor.c
+++ b/drivers/base/power/domain_governor.c
@@ -14,6 +14,31 @@
 
 #ifdef CONFIG_PM_RUNTIME
 
+static int dev_update_qos_constraint(struct device *dev, void *data)
+{
+	s64 *constraint_ns_p = data;
+	s32 constraint_ns = -1;
+
+	if (dev->power.subsys_data && dev->power.subsys_data->domain_data)
+		constraint_ns = dev_gpd_data(dev)->td.effective_constraint_ns;
+
+	if (constraint_ns < 0) {
+		constraint_ns = dev_pm_qos_read_value(dev);
+		constraint_ns *= NSEC_PER_USEC;
+	}
+	if (constraint_ns == 0)
+		return 0;
+
+	/*
+	 * constraint_ns cannot be negative here, because the device has been
+	 * suspended.
+	 */
+	if (constraint_ns < *constraint_ns_p || *constraint_ns_p == 0)
+		*constraint_ns_p = constraint_ns;
+
+	return 0;
+}
+
 /**
  * default_stop_ok - Default PM domain governor routine for stopping devices.
  * @dev: Device to check.
@@ -21,14 +46,52 @@
 bool default_stop_ok(struct device *dev)
 {
 	struct gpd_timing_data *td = &dev_gpd_data(dev)->td;
+	unsigned long flags;
+	s64 constraint_ns;
 
 	dev_dbg(dev, "%s()\n", __func__);
 
-	if (dev->power.max_time_suspended_ns < 0 || td->break_even_ns == 0)
-		return true;
+	spin_lock_irqsave(&dev->power.lock, flags);
+
+	if (!td->constraint_changed) {
+		bool ret = td->cached_stop_ok;
 
-	return td->stop_latency_ns + td->start_latency_ns < td->break_even_ns
-		&& td->break_even_ns < dev->power.max_time_suspended_ns;
+		spin_unlock_irqrestore(&dev->power.lock, flags);
+		return ret;
+	}
+	td->constraint_changed = false;
+	td->cached_stop_ok = false;
+	td->effective_constraint_ns = -1;
+	constraint_ns = __dev_pm_qos_read_value(dev);
+
+	spin_unlock_irqrestore(&dev->power.lock, flags);
+
+	if (constraint_ns < 0)
+		return false;
+
+	constraint_ns *= NSEC_PER_USEC;
+	/*
+	 * We can walk the children without any additional locking, because
+	 * they all have been suspended at this point and their
+	 * effective_constraint_ns fields won't be modified in parallel with us.
+	 */
+	if (!dev->power.ignore_children)
+		device_for_each_child(dev, &constraint_ns,
+				      dev_update_qos_constraint);
+
+	if (constraint_ns > 0) {
+		constraint_ns -= td->start_latency_ns;
+		if (constraint_ns == 0)
+			return false;
+	}
+	td->effective_constraint_ns = constraint_ns;
+	td->cached_stop_ok = constraint_ns > td->stop_latency_ns ||
+				constraint_ns == 0;
+	/*
+	 * The children have been suspended already, so we don't need to take
+	 * their stop latencies into account here.
+	 */
+	return td->cached_stop_ok;
 }
 
 /**
@@ -42,9 +105,27 @@ static bool default_power_down_ok(struct dev_pm_domain *pd)
 	struct generic_pm_domain *genpd = pd_to_genpd(pd);
 	struct gpd_link *link;
 	struct pm_domain_data *pdd;
-	s64 min_dev_off_time_ns;
+	s64 min_off_time_ns;
 	s64 off_on_time_ns;
-	ktime_t time_now = ktime_get();
+
+	if (genpd->max_off_time_changed) {
+		struct gpd_link *link;
+
+		/*
+		 * We have to invalidate the cached results for the masters, so
+		 * use the observation that default_power_down_ok() is not
+		 * going to be called for any master until this instance
+		 * returns.
+		 */
+		list_for_each_entry(link, &genpd->slave_links, slave_node)
+			link->master->max_off_time_changed = true;
+
+		genpd->max_off_time_changed = false;
+		genpd->cached_power_down_ok = false;
+		genpd->max_off_time_ns = -1;
+	} else {
+		return genpd->cached_power_down_ok;
+	}
 
 	off_on_time_ns = genpd->power_off_latency_ns +
 				genpd->power_on_latency_ns;
@@ -61,6 +142,7 @@ static bool default_power_down_ok(struct dev_pm_domain *pd)
 				to_gpd_data(pdd)->td.save_state_latency_ns;
 	}
 
+	min_off_time_ns = -1;
 	/*
 	 * Check if subdomains can be off for enough time.
 	 *
@@ -73,8 +155,6 @@ static bool default_power_down_ok(struct dev_pm_domain *pd)
 		if (sd_max_off_ns < 0)
 			continue;
 
-		sd_max_off_ns -= ktime_to_ns(ktime_sub(time_now,
-						       sd->power_off_time));
 		/*
 		 * Check if the subdomain is allowed to be off long enough for
 		 * the current domain to turn off and on (that's how much time
@@ -82,60 +162,64 @@ static bool default_power_down_ok(struct dev_pm_domain *pd)
 		 */
 		if (sd_max_off_ns <= off_on_time_ns)
 			return false;
+
+		if (min_off_time_ns > sd_max_off_ns || min_off_time_ns < 0)
+			min_off_time_ns = sd_max_off_ns;
 	}
 
 	/*
 	 * Check if the devices in the domain can be off enough time.
 	 */
-	min_dev_off_time_ns = -1;
 	list_for_each_entry(pdd, &genpd->dev_list, list_node) {
 		struct gpd_timing_data *td;
-		struct device *dev = pdd->dev;
-		s64 dev_off_time_ns;
+		s64 constraint_ns;
 
-		if (!dev->driver || dev->power.max_time_suspended_ns < 0)
+		if (!pdd->dev->driver)
 			continue;
 
+		/*
+		 * Check if the device is allowed to be off long enough for the
+		 * domain to turn off and on (that's how much time it will
+		 * have to wait worst case).
+		 */
 		td = &to_gpd_data(pdd)->td;
-		dev_off_time_ns = dev->power.max_time_suspended_ns -
-			(td->start_latency_ns + td->restore_state_latency_ns +
-				ktime_to_ns(ktime_sub(time_now,
-						dev->power.suspend_time)));
-		if (dev_off_time_ns <= off_on_time_ns)
-			return false;
-
-		if (min_dev_off_time_ns > dev_off_time_ns
-		    || min_dev_off_time_ns < 0)
-			min_dev_off_time_ns = dev_off_time_ns;
-	}
+		constraint_ns = td->effective_constraint_ns;
+		/* default_stop_ok() need not be called before us. */
+		if (constraint_ns < 0) {
+			constraint_ns = dev_pm_qos_read_value(pdd->dev);
+			constraint_ns *= NSEC_PER_USEC;
+		}
+		if (constraint_ns == 0)
+			continue;
 
-	if (min_dev_off_time_ns < 0) {
 		/*
-		 * There are no latency constraints, so the domain can spend
-		 * arbitrary time in the "off" state.
+		 * constraint_ns cannot be negative here, because the device has
+		 * been suspended.
 		 */
-		genpd->max_off_time_ns = -1;
-		return true;
+		constraint_ns -= td->restore_state_latency_ns;
+		if (constraint_ns <= off_on_time_ns)
+			return false;
+
+		if (min_off_time_ns > constraint_ns || min_off_time_ns < 0)
+			min_off_time_ns = constraint_ns;
 	}
 
+	genpd->cached_power_down_ok = true;
+
 	/*
-	 * The difference between the computed minimum delta and the time needed
-	 * to turn the domain on is the maximum theoretical time this domain can
-	 * spend in the "off" state.
+	 * If the computed minimum device off time is negative, there are no
+	 * latency constraints, so the domain can spend arbitrary time in the
+	 * "off" state.
 	 */
-	min_dev_off_time_ns -= genpd->power_on_latency_ns;
+	if (min_off_time_ns < 0)
+		return true;
 
 	/*
-	 * If the difference between the computed minimum delta and the time
-	 * needed to turn the domain off and back on on is smaller than the
-	 * domain's power break even time, removing power from the domain is not
-	 * worth it.
+	 * The difference between the computed minimum subdomain or device off
+	 * time and the time needed to turn the domain on is the maximum
+	 * theoretical time this domain can spend in the "off" state.
 	 */
-	if (genpd->break_even_ns >
-	    min_dev_off_time_ns - genpd->power_off_latency_ns)
-		return false;
-
-	genpd->max_off_time_ns = min_dev_off_time_ns;
+	genpd->max_off_time_ns = min_off_time_ns - genpd->power_on_latency_ns;
 	return true;
 }
 
diff --git a/drivers/base/power/main.c b/drivers/base/power/main.c
index b462c0e341c..e0fb5b0435a 100644
--- a/drivers/base/power/main.c
+++ b/drivers/base/power/main.c
@@ -889,6 +889,11 @@ static int dpm_suspend_noirq(pm_message_t state)
 		if (!list_empty(&dev->power.entry))
 			list_move(&dev->power.entry, &dpm_noirq_list);
 		put_device(dev);
+
+		if (pm_wakeup_pending()) {
+			error = -EBUSY;
+			break;
+		}
 	}
 	mutex_unlock(&dpm_list_mtx);
 	if (error)
@@ -962,6 +967,11 @@ static int dpm_suspend_late(pm_message_t state)
 		if (!list_empty(&dev->power.entry))
 			list_move(&dev->power.entry, &dpm_late_early_list);
 		put_device(dev);
+
+		if (pm_wakeup_pending()) {
+			error = -EBUSY;
+			break;
+		}
 	}
 	mutex_unlock(&dpm_list_mtx);
 	if (error)
diff --git a/drivers/base/power/qos.c b/drivers/base/power/qos.c
index 71855570922..fd849a2c4fa 100644
--- a/drivers/base/power/qos.c
+++ b/drivers/base/power/qos.c
@@ -352,21 +352,26 @@ EXPORT_SYMBOL_GPL(dev_pm_qos_remove_request);
  *
  * Will register the notifier into a notification chain that gets called
  * upon changes to the target value for the device.
+ *
+ * If the device's constraints object doesn't exist when this routine is called,
+ * it will be created (or error code will be returned if that fails).
  */
 int dev_pm_qos_add_notifier(struct device *dev, struct notifier_block *notifier)
 {
-	int retval = 0;
+	int ret = 0;
 
 	mutex_lock(&dev_pm_qos_mtx);
 
-	/* Silently return if the constraints object is not present. */
-	if (dev->power.constraints)
-		retval = blocking_notifier_chain_register(
-				dev->power.constraints->notifiers,
-				notifier);
+	if (!dev->power.constraints)
+		ret = dev->power.power_state.event != PM_EVENT_INVALID ?
+			dev_pm_qos_constraints_allocate(dev) : -ENODEV;
+
+	if (!ret)
+		ret = blocking_notifier_chain_register(
+				dev->power.constraints->notifiers, notifier);
 
 	mutex_unlock(&dev_pm_qos_mtx);
-	return retval;
+	return ret;
 }
 EXPORT_SYMBOL_GPL(dev_pm_qos_add_notifier);
 
diff --git a/drivers/base/power/runtime.c b/drivers/base/power/runtime.c
index bd0f3949bcf..59894873a3b 100644
--- a/drivers/base/power/runtime.c
+++ b/drivers/base/power/runtime.c
@@ -282,47 +282,6 @@ static int rpm_callback(int (*cb)(struct device *), struct device *dev)
 	return retval != -EACCES ? retval : -EIO;
 }
 
-struct rpm_qos_data {
-	ktime_t time_now;
-	s64 constraint_ns;
-};
-
-/**
- * rpm_update_qos_constraint - Update a given PM QoS constraint data.
- * @dev: Device whose timing data to use.
- * @data: PM QoS constraint data to update.
- *
- * Use the suspend timing data of @dev to update PM QoS constraint data pointed
- * to by @data.
- */
-static int rpm_update_qos_constraint(struct device *dev, void *data)
-{
-	struct rpm_qos_data *qos = data;
-	unsigned long flags;
-	s64 delta_ns;
-	int ret = 0;
-
-	spin_lock_irqsave(&dev->power.lock, flags);
-
-	if (dev->power.max_time_suspended_ns < 0)
-		goto out;
-
-	delta_ns = dev->power.max_time_suspended_ns -
-		ktime_to_ns(ktime_sub(qos->time_now, dev->power.suspend_time));
-	if (delta_ns <= 0) {
-		ret = -EBUSY;
-		goto out;
-	}
-
-	if (qos->constraint_ns > delta_ns || qos->constraint_ns == 0)
-		qos->constraint_ns = delta_ns;
-
- out:
-	spin_unlock_irqrestore(&dev->power.lock, flags);
-
-	return ret;
-}
-
 /**
  * rpm_suspend - Carry out runtime suspend of given device.
  * @dev: Device to suspend.
@@ -349,7 +308,6 @@ static int rpm_suspend(struct device *dev, int rpmflags)
 {
 	int (*callback)(struct device *);
 	struct device *parent = NULL;
-	struct rpm_qos_data qos;
 	int retval;
 
 	trace_rpm_suspend(dev, rpmflags);
@@ -445,38 +403,14 @@ static int rpm_suspend(struct device *dev, int rpmflags)
 		goto out;
 	}
 
-	qos.constraint_ns = __dev_pm_qos_read_value(dev);
-	if (qos.constraint_ns < 0) {
-		/* Negative constraint means "never suspend". */
+	if (__dev_pm_qos_read_value(dev) < 0) {
+		/* Negative PM QoS constraint means "never suspend". */
 		retval = -EPERM;
 		goto out;
 	}
-	qos.constraint_ns *= NSEC_PER_USEC;
-	qos.time_now = ktime_get();
 
 	__update_runtime_status(dev, RPM_SUSPENDING);
 
-	if (!dev->power.ignore_children) {
-		if (dev->power.irq_safe)
-			spin_unlock(&dev->power.lock);
-		else
-			spin_unlock_irq(&dev->power.lock);
-
-		retval = device_for_each_child(dev, &qos,
-					       rpm_update_qos_constraint);
-
-		if (dev->power.irq_safe)
-			spin_lock(&dev->power.lock);
-		else
-			spin_lock_irq(&dev->power.lock);
-
-		if (retval)
-			goto fail;
-	}
-
-	dev->power.suspend_time = qos.time_now;
-	dev->power.max_time_suspended_ns = qos.constraint_ns ? : -1;
-
 	if (dev->pm_domain)
 		callback = dev->pm_domain->ops.runtime_suspend;
 	else if (dev->type && dev->type->pm)
@@ -529,8 +463,6 @@ static int rpm_suspend(struct device *dev, int rpmflags)
 
  fail:
 	__update_runtime_status(dev, RPM_ACTIVE);
-	dev->power.suspend_time = ktime_set(0, 0);
-	dev->power.max_time_suspended_ns = -1;
 	dev->power.deferred_resume = false;
 	wake_up_all(&dev->power.wait_queue);
 
@@ -704,9 +636,6 @@ static int rpm_resume(struct device *dev, int rpmflags)
 	if (dev->power.no_callbacks)
 		goto no_callback;	/* Assume success. */
 
-	dev->power.suspend_time = ktime_set(0, 0);
-	dev->power.max_time_suspended_ns = -1;
-
 	__update_runtime_status(dev, RPM_RESUMING);
 
 	if (dev->pm_domain)
@@ -1369,9 +1298,6 @@ void pm_runtime_init(struct device *dev)
 	setup_timer(&dev->power.suspend_timer, pm_suspend_timer_fn,
 			(unsigned long)dev);
 
-	dev->power.suspend_time = ktime_set(0, 0);
-	dev->power.max_time_suspended_ns = -1;
-
 	init_waitqueue_head(&dev->power.wait_queue);
 }
 
@@ -1389,28 +1315,3 @@ void pm_runtime_remove(struct device *dev)
 	if (dev->power.irq_safe && dev->parent)
 		pm_runtime_put_sync(dev->parent);
 }
-
-/**
- * pm_runtime_update_max_time_suspended - Update device's suspend time data.
- * @dev: Device to handle.
- * @delta_ns: Value to subtract from the device's max_time_suspended_ns field.
- *
- * Update the device's power.max_time_suspended_ns field by subtracting
- * @delta_ns from it.  The resulting value of power.max_time_suspended_ns is
- * never negative.
- */
-void pm_runtime_update_max_time_suspended(struct device *dev, s64 delta_ns)
-{
-	unsigned long flags;
-
-	spin_lock_irqsave(&dev->power.lock, flags);
-
-	if (delta_ns > 0 && dev->power.max_time_suspended_ns > 0) {
-		if (dev->power.max_time_suspended_ns > delta_ns)
-			dev->power.max_time_suspended_ns -= delta_ns;
-		else
-			dev->power.max_time_suspended_ns = 0;
-	}
-
-	spin_unlock_irqrestore(&dev->power.lock, flags);
-}
diff --git a/drivers/base/power/sysfs.c b/drivers/base/power/sysfs.c
index 95c12f6cb5b..48be2ad4dd2 100644
--- a/drivers/base/power/sysfs.c
+++ b/drivers/base/power/sysfs.c
@@ -314,22 +314,41 @@ static ssize_t wakeup_active_count_show(struct device *dev,
 
 static DEVICE_ATTR(wakeup_active_count, 0444, wakeup_active_count_show, NULL);
 
-static ssize_t wakeup_hit_count_show(struct device *dev,
-				struct device_attribute *attr, char *buf)
+static ssize_t wakeup_abort_count_show(struct device *dev,
+					struct device_attribute *attr,
+					char *buf)
+{
+	unsigned long count = 0;
+	bool enabled = false;
+
+	spin_lock_irq(&dev->power.lock);
+	if (dev->power.wakeup) {
+		count = dev->power.wakeup->wakeup_count;
+		enabled = true;
+	}
+	spin_unlock_irq(&dev->power.lock);
+	return enabled ? sprintf(buf, "%lu\n", count) : sprintf(buf, "\n");
+}
+
+static DEVICE_ATTR(wakeup_abort_count, 0444, wakeup_abort_count_show, NULL);
+
+static ssize_t wakeup_expire_count_show(struct device *dev,
+					struct device_attribute *attr,
+					char *buf)
 {
 	unsigned long count = 0;
 	bool enabled = false;
 
 	spin_lock_irq(&dev->power.lock);
 	if (dev->power.wakeup) {
-		count = dev->power.wakeup->hit_count;
+		count = dev->power.wakeup->expire_count;
 		enabled = true;
 	}
 	spin_unlock_irq(&dev->power.lock);
 	return enabled ? sprintf(buf, "%lu\n", count) : sprintf(buf, "\n");
 }
 
-static DEVICE_ATTR(wakeup_hit_count, 0444, wakeup_hit_count_show, NULL);
+static DEVICE_ATTR(wakeup_expire_count, 0444, wakeup_expire_count_show, NULL);
 
 static ssize_t wakeup_active_show(struct device *dev,
 				struct device_attribute *attr, char *buf)
@@ -398,6 +417,27 @@ static ssize_t wakeup_last_time_show(struct device *dev,
 }
 
 static DEVICE_ATTR(wakeup_last_time_ms, 0444, wakeup_last_time_show, NULL);
+
+#ifdef CONFIG_PM_AUTOSLEEP
+static ssize_t wakeup_prevent_sleep_time_show(struct device *dev,
+					      struct device_attribute *attr,
+					      char *buf)
+{
+	s64 msec = 0;
+	bool enabled = false;
+
+	spin_lock_irq(&dev->power.lock);
+	if (dev->power.wakeup) {
+		msec = ktime_to_ms(dev->power.wakeup->prevent_sleep_time);
+		enabled = true;
+	}
+	spin_unlock_irq(&dev->power.lock);
+	return enabled ? sprintf(buf, "%lld\n", msec) : sprintf(buf, "\n");
+}
+
+static DEVICE_ATTR(wakeup_prevent_sleep_time_ms, 0444,
+		   wakeup_prevent_sleep_time_show, NULL);
+#endif /* CONFIG_PM_AUTOSLEEP */
 #endif /* CONFIG_PM_SLEEP */
 
 #ifdef CONFIG_PM_ADVANCED_DEBUG
@@ -486,11 +526,15 @@ static struct attribute *wakeup_attrs[] = {
 	&dev_attr_wakeup.attr,
 	&dev_attr_wakeup_count.attr,
 	&dev_attr_wakeup_active_count.attr,
-	&dev_attr_wakeup_hit_count.attr,
+	&dev_attr_wakeup_abort_count.attr,
+	&dev_attr_wakeup_expire_count.attr,
 	&dev_attr_wakeup_active.attr,
 	&dev_attr_wakeup_total_time_ms.attr,
 	&dev_attr_wakeup_max_time_ms.attr,
 	&dev_attr_wakeup_last_time_ms.attr,
+#ifdef CONFIG_PM_AUTOSLEEP
+	&dev_attr_wakeup_prevent_sleep_time_ms.attr,
+#endif
 #endif
 	NULL,
 };
diff --git a/drivers/base/power/wakeup.c b/drivers/base/power/wakeup.c
index 2a3e581b8dc..cbb463b3a75 100644
--- a/drivers/base/power/wakeup.c
+++ b/drivers/base/power/wakeup.c
@@ -14,16 +14,15 @@
 #include <linux/suspend.h>
 #include <linux/seq_file.h>
 #include <linux/debugfs.h>
+#include <trace/events/power.h>
 
 #include "power.h"
 
-#define TIMEOUT		100
-
 /*
  * If set, the suspend/hibernate code will abort transitions to a sleep state
  * if wakeup events are registered during or immediately before the transition.
  */
-bool events_check_enabled;
+bool events_check_enabled __read_mostly;
 
 /*
  * Combined counters of registered wakeup events and wakeup events in progress.
@@ -52,6 +51,8 @@ static void pm_wakeup_timer_fn(unsigned long data);
 
 static LIST_HEAD(wakeup_sources);
 
+static DECLARE_WAIT_QUEUE_HEAD(wakeup_count_wait_queue);
+
 /**
  * wakeup_source_prepare - Prepare a new wakeup source for initialization.
  * @ws: Wakeup source to prepare.
@@ -132,6 +133,7 @@ void wakeup_source_add(struct wakeup_source *ws)
 	spin_lock_init(&ws->lock);
 	setup_timer(&ws->timer, pm_wakeup_timer_fn, (unsigned long)ws);
 	ws->active = false;
+	ws->last_time = ktime_get();
 
 	spin_lock_irq(&events_lock);
 	list_add_rcu(&ws->entry, &wakeup_sources);
@@ -374,12 +376,33 @@ EXPORT_SYMBOL_GPL(device_set_wakeup_enable);
  */
 static void wakeup_source_activate(struct wakeup_source *ws)
 {
+	unsigned int cec;
+
 	ws->active = true;
 	ws->active_count++;
 	ws->last_time = ktime_get();
+	if (ws->autosleep_enabled)
+		ws->start_prevent_time = ws->last_time;
 
 	/* Increment the counter of events in progress. */
-	atomic_inc(&combined_event_count);
+	cec = atomic_inc_return(&combined_event_count);
+
+	trace_wakeup_source_activate(ws->name, cec);
+}
+
+/**
+ * wakeup_source_report_event - Report wakeup event using the given source.
+ * @ws: Wakeup source to report the event for.
+ */
+static void wakeup_source_report_event(struct wakeup_source *ws)
+{
+	ws->event_count++;
+	/* This is racy, but the counter is approximate anyway. */
+	if (events_check_enabled)
+		ws->wakeup_count++;
+
+	if (!ws->active)
+		wakeup_source_activate(ws);
 }
 
 /**
@@ -397,10 +420,7 @@ void __pm_stay_awake(struct wakeup_source *ws)
 
 	spin_lock_irqsave(&ws->lock, flags);
 
-	ws->event_count++;
-	if (!ws->active)
-		wakeup_source_activate(ws);
-
+	wakeup_source_report_event(ws);
 	del_timer(&ws->timer);
 	ws->timer_expires = 0;
 
@@ -432,6 +452,17 @@ void pm_stay_awake(struct device *dev)
 }
 EXPORT_SYMBOL_GPL(pm_stay_awake);
 
+#ifdef CONFIG_PM_AUTOSLEEP
+static void update_prevent_sleep_time(struct wakeup_source *ws, ktime_t now)
+{
+	ktime_t delta = ktime_sub(now, ws->start_prevent_time);
+	ws->prevent_sleep_time = ktime_add(ws->prevent_sleep_time, delta);
+}
+#else
+static inline void update_prevent_sleep_time(struct wakeup_source *ws,
+					     ktime_t now) {}
+#endif
+
 /**
  * wakup_source_deactivate - Mark given wakeup source as inactive.
  * @ws: Wakeup source to handle.
@@ -442,6 +473,7 @@ EXPORT_SYMBOL_GPL(pm_stay_awake);
  */
 static void wakeup_source_deactivate(struct wakeup_source *ws)
 {
+	unsigned int cnt, inpr, cec;
 	ktime_t duration;
 	ktime_t now;
 
@@ -468,14 +500,23 @@ static void wakeup_source_deactivate(struct wakeup_source *ws)
 	if (ktime_to_ns(duration) > ktime_to_ns(ws->max_time))
 		ws->max_time = duration;
 
+	ws->last_time = now;
 	del_timer(&ws->timer);
 	ws->timer_expires = 0;
 
+	if (ws->autosleep_enabled)
+		update_prevent_sleep_time(ws, now);
+
 	/*
 	 * Increment the counter of registered wakeup events and decrement the
 	 * couter of wakeup events in progress simultaneously.
 	 */
-	atomic_add(MAX_IN_PROGRESS, &combined_event_count);
+	cec = atomic_add_return(MAX_IN_PROGRESS, &combined_event_count);
+	trace_wakeup_source_deactivate(ws->name, cec);
+
+	split_counters(&cnt, &inpr);
+	if (!inpr && waitqueue_active(&wakeup_count_wait_queue))
+		wake_up(&wakeup_count_wait_queue);
 }
 
 /**
@@ -536,8 +577,10 @@ static void pm_wakeup_timer_fn(unsigned long data)
 	spin_lock_irqsave(&ws->lock, flags);
 
 	if (ws->active && ws->timer_expires
-	    && time_after_eq(jiffies, ws->timer_expires))
+	    && time_after_eq(jiffies, ws->timer_expires)) {
 		wakeup_source_deactivate(ws);
+		ws->expire_count++;
+	}
 
 	spin_unlock_irqrestore(&ws->lock, flags);
 }
@@ -564,9 +607,7 @@ void __pm_wakeup_event(struct wakeup_source *ws, unsigned int msec)
 
 	spin_lock_irqsave(&ws->lock, flags);
 
-	ws->event_count++;
-	if (!ws->active)
-		wakeup_source_activate(ws);
+	wakeup_source_report_event(ws);
 
 	if (!msec) {
 		wakeup_source_deactivate(ws);
@@ -609,24 +650,6 @@ void pm_wakeup_event(struct device *dev, unsigned int msec)
 EXPORT_SYMBOL_GPL(pm_wakeup_event);
 
 /**
- * pm_wakeup_update_hit_counts - Update hit counts of all active wakeup sources.
- */
-static void pm_wakeup_update_hit_counts(void)
-{
-	unsigned long flags;
-	struct wakeup_source *ws;
-
-	rcu_read_lock();
-	list_for_each_entry_rcu(ws, &wakeup_sources, entry) {
-		spin_lock_irqsave(&ws->lock, flags);
-		if (ws->active)
-			ws->hit_count++;
-		spin_unlock_irqrestore(&ws->lock, flags);
-	}
-	rcu_read_unlock();
-}
-
-/**
  * pm_wakeup_pending - Check if power transition in progress should be aborted.
  *
  * Compare the current number of registered wakeup events with its preserved
@@ -648,32 +671,38 @@ bool pm_wakeup_pending(void)
 		events_check_enabled = !ret;
 	}
 	spin_unlock_irqrestore(&events_lock, flags);
-	if (ret)
-		pm_wakeup_update_hit_counts();
 	return ret;
 }
 
 /**
  * pm_get_wakeup_count - Read the number of registered wakeup events.
  * @count: Address to store the value at.
+ * @block: Whether or not to block.
  *
- * Store the number of registered wakeup events at the address in @count.  Block
- * if the current number of wakeup events being processed is nonzero.
+ * Store the number of registered wakeup events at the address in @count.  If
+ * @block is set, block until the current number of wakeup events being
+ * processed is zero.
  *
- * Return 'false' if the wait for the number of wakeup events being processed to
- * drop down to zero has been interrupted by a signal (and the current number
- * of wakeup events being processed is still nonzero).  Otherwise return 'true'.
+ * Return 'false' if the current number of wakeup events being processed is
+ * nonzero.  Otherwise return 'true'.
  */
-bool pm_get_wakeup_count(unsigned int *count)
+bool pm_get_wakeup_count(unsigned int *count, bool block)
 {
 	unsigned int cnt, inpr;
 
-	for (;;) {
-		split_counters(&cnt, &inpr);
-		if (inpr == 0 || signal_pending(current))
-			break;
-		pm_wakeup_update_hit_counts();
-		schedule_timeout_interruptible(msecs_to_jiffies(TIMEOUT));
+	if (block) {
+		DEFINE_WAIT(wait);
+
+		for (;;) {
+			prepare_to_wait(&wakeup_count_wait_queue, &wait,
+					TASK_INTERRUPTIBLE);
+			split_counters(&cnt, &inpr);
+			if (inpr == 0 || signal_pending(current))
+				break;
+
+			schedule();
+		}
+		finish_wait(&wakeup_count_wait_queue, &wait);
 	}
 
 	split_counters(&cnt, &inpr);
@@ -703,11 +732,37 @@ bool pm_save_wakeup_count(unsigned int count)
 		events_check_enabled = true;
 	}
 	spin_unlock_irq(&events_lock);
-	if (!events_check_enabled)
-		pm_wakeup_update_hit_counts();
 	return events_check_enabled;
 }
 
+#ifdef CONFIG_PM_AUTOSLEEP
+/**
+ * pm_wakep_autosleep_enabled - Modify autosleep_enabled for all wakeup sources.
+ * @enabled: Whether to set or to clear the autosleep_enabled flags.
+ */
+void pm_wakep_autosleep_enabled(bool set)
+{
+	struct wakeup_source *ws;
+	ktime_t now = ktime_get();
+
+	rcu_read_lock();
+	list_for_each_entry_rcu(ws, &wakeup_sources, entry) {
+		spin_lock_irq(&ws->lock);
+		if (ws->autosleep_enabled != set) {
+			ws->autosleep_enabled = set;
+			if (ws->active) {
+				if (set)
+					ws->start_prevent_time = now;
+				else
+					update_prevent_sleep_time(ws, now);
+			}
+		}
+		spin_unlock_irq(&ws->lock);
+	}
+	rcu_read_unlock();
+}
+#endif /* CONFIG_PM_AUTOSLEEP */
+
 static struct dentry *wakeup_sources_stats_dentry;
 
 /**
@@ -723,27 +778,37 @@ static int print_wakeup_source_stats(struct seq_file *m,
 	ktime_t max_time;
 	unsigned long active_count;
 	ktime_t active_time;
+	ktime_t prevent_sleep_time;
 	int ret;
 
 	spin_lock_irqsave(&ws->lock, flags);
 
 	total_time = ws->total_time;
 	max_time = ws->max_time;
+	prevent_sleep_time = ws->prevent_sleep_time;
 	active_count = ws->active_count;
 	if (ws->active) {
-		active_time = ktime_sub(ktime_get(), ws->last_time);
+		ktime_t now = ktime_get();
+
+		active_time = ktime_sub(now, ws->last_time);
 		total_time = ktime_add(total_time, active_time);
 		if (active_time.tv64 > max_time.tv64)
 			max_time = active_time;
+
+		if (ws->autosleep_enabled)
+			prevent_sleep_time = ktime_add(prevent_sleep_time,
+				ktime_sub(now, ws->start_prevent_time));
 	} else {
 		active_time = ktime_set(0, 0);
 	}
 
-	ret = seq_printf(m, "%-12s\t%lu\t\t%lu\t\t%lu\t\t"
-			"%lld\t\t%lld\t\t%lld\t\t%lld\n",
-			ws->name, active_count, ws->event_count, ws->hit_count,
+	ret = seq_printf(m, "%-12s\t%lu\t\t%lu\t\t%lu\t\t%lu\t\t"
+			"%lld\t\t%lld\t\t%lld\t\t%lld\t\t%lld\n",
+			ws->name, active_count, ws->event_count,
+			ws->wakeup_count, ws->expire_count,
 			ktime_to_ms(active_time), ktime_to_ms(total_time),
-			ktime_to_ms(max_time), ktime_to_ms(ws->last_time));
+			ktime_to_ms(max_time), ktime_to_ms(ws->last_time),
+			ktime_to_ms(prevent_sleep_time));
 
 	spin_unlock_irqrestore(&ws->lock, flags);
 
@@ -758,8 +823,9 @@ static int wakeup_sources_stats_show(struct seq_file *m, void *unused)
 {
 	struct wakeup_source *ws;
 
-	seq_puts(m, "name\t\tactive_count\tevent_count\thit_count\t"
-		"active_since\ttotal_time\tmax_time\tlast_change\n");
+	seq_puts(m, "name\t\tactive_count\tevent_count\twakeup_count\t"
+		"expire_count\tactive_since\ttotal_time\tmax_time\t"
+		"last_change\tprevent_suspend_time\n");
 
 	rcu_read_lock();
 	list_for_each_entry_rcu(ws, &wakeup_sources, entry)
diff --git a/drivers/base/regmap/Kconfig b/drivers/base/regmap/Kconfig
index 0f6c7fb418e..6be390bd8bd 100644
--- a/drivers/base/regmap/Kconfig
+++ b/drivers/base/regmap/Kconfig
@@ -6,6 +6,7 @@ config REGMAP
 	default y if (REGMAP_I2C || REGMAP_SPI)
 	select LZO_COMPRESS
 	select LZO_DECOMPRESS
+	select IRQ_DOMAIN if REGMAP_IRQ
 	bool
 
 config REGMAP_I2C
@@ -14,5 +15,8 @@ config REGMAP_I2C
 config REGMAP_SPI
 	tristate
 
+config REGMAP_MMIO
+	tristate
+
 config REGMAP_IRQ
 	bool
diff --git a/drivers/base/regmap/Makefile b/drivers/base/regmap/Makefile
index defd57963c8..5e75d1b683e 100644
--- a/drivers/base/regmap/Makefile
+++ b/drivers/base/regmap/Makefile
@@ -3,4 +3,5 @@ obj-$(CONFIG_REGMAP) += regcache-rbtree.o regcache-lzo.o
 obj-$(CONFIG_DEBUG_FS) += regmap-debugfs.o
 obj-$(CONFIG_REGMAP_I2C) += regmap-i2c.o
 obj-$(CONFIG_REGMAP_SPI) += regmap-spi.o
+obj-$(CONFIG_REGMAP_MMIO) += regmap-mmio.o
 obj-$(CONFIG_REGMAP_IRQ) += regmap-irq.o
diff --git a/drivers/base/regmap/internal.h b/drivers/base/regmap/internal.h
index fcafc5b2e65..b986b8660b0 100644
--- a/drivers/base/regmap/internal.h
+++ b/drivers/base/regmap/internal.h
@@ -26,21 +26,30 @@ struct regmap_format {
 	size_t val_bytes;
 	void (*format_write)(struct regmap *map,
 			     unsigned int reg, unsigned int val);
-	void (*format_reg)(void *buf, unsigned int reg);
-	void (*format_val)(void *buf, unsigned int val);
+	void (*format_reg)(void *buf, unsigned int reg, unsigned int shift);
+	void (*format_val)(void *buf, unsigned int val, unsigned int shift);
 	unsigned int (*parse_val)(void *buf);
 };
 
+typedef void (*regmap_lock)(struct regmap *map);
+typedef void (*regmap_unlock)(struct regmap *map);
+
 struct regmap {
-	struct mutex lock;
+	struct mutex mutex;
+	spinlock_t spinlock;
+	regmap_lock lock;
+	regmap_unlock unlock;
 
 	struct device *dev; /* Device we do I/O on */
 	void *work_buf;     /* Scratch buffer used to format I/O */
 	struct regmap_format format;  /* Buffer format */
 	const struct regmap_bus *bus;
+	void *bus_context;
+	const char *name;
 
 #ifdef CONFIG_DEBUG_FS
 	struct dentry *debugfs;
+	const char *debugfs_name;
 #endif
 
 	unsigned int max_register;
@@ -52,6 +61,10 @@ struct regmap {
 	u8 read_flag_mask;
 	u8 write_flag_mask;
 
+	/* number of bits to (left) shift the reg value when formatting*/
+	int reg_shift;
+	int reg_stride;
+
 	/* regcache specific members */
 	const struct regcache_ops *cache_ops;
 	enum regcache_type cache_type;
@@ -79,6 +92,9 @@ struct regmap {
 
 	struct reg_default *patch;
 	int patch_regs;
+
+	/* if set, converts bulk rw to single rw */
+	bool use_single_rw;
 };
 
 struct regcache_ops {
@@ -101,11 +117,11 @@ int _regmap_write(struct regmap *map, unsigned int reg,
 
 #ifdef CONFIG_DEBUG_FS
 extern void regmap_debugfs_initcall(void);
-extern void regmap_debugfs_init(struct regmap *map);
+extern void regmap_debugfs_init(struct regmap *map, const char *name);
 extern void regmap_debugfs_exit(struct regmap *map);
 #else
 static inline void regmap_debugfs_initcall(void) { }
-static inline void regmap_debugfs_init(struct regmap *map) { }
+static inline void regmap_debugfs_init(struct regmap *map, const char *name) { }
 static inline void regmap_debugfs_exit(struct regmap *map) { }
 #endif
 
diff --git a/drivers/base/regmap/regcache-lzo.c b/drivers/base/regmap/regcache-lzo.c
index 483b06d4a38..afd6aa91a0d 100644
--- a/drivers/base/regmap/regcache-lzo.c
+++ b/drivers/base/regmap/regcache-lzo.c
@@ -108,7 +108,7 @@ static int regcache_lzo_decompress_cache_block(struct regmap *map,
 static inline int regcache_lzo_get_blkindex(struct regmap *map,
 					    unsigned int reg)
 {
-	return (reg * map->cache_word_size) /
+	return ((reg / map->reg_stride) * map->cache_word_size) /
 		DIV_ROUND_UP(map->cache_size_raw,
 			     regcache_lzo_block_count(map));
 }
@@ -116,9 +116,10 @@ static inline int regcache_lzo_get_blkindex(struct regmap *map,
 static inline int regcache_lzo_get_blkpos(struct regmap *map,
 					  unsigned int reg)
 {
-	return reg % (DIV_ROUND_UP(map->cache_size_raw,
-				   regcache_lzo_block_count(map)) /
-		      map->cache_word_size);
+	return (reg / map->reg_stride) %
+		    (DIV_ROUND_UP(map->cache_size_raw,
+				  regcache_lzo_block_count(map)) /
+		     map->cache_word_size);
 }
 
 static inline int regcache_lzo_get_blksize(struct regmap *map)
@@ -322,7 +323,7 @@ static int regcache_lzo_write(struct regmap *map,
 	}
 
 	/* set the bit so we know we have to sync this register */
-	set_bit(reg, lzo_block->sync_bmp);
+	set_bit(reg / map->reg_stride, lzo_block->sync_bmp);
 	kfree(tmp_dst);
 	kfree(lzo_block->src);
 	return 0;
diff --git a/drivers/base/regmap/regcache-rbtree.c b/drivers/base/regmap/regcache-rbtree.c
index 92b779ee002..e6732cf7c06 100644
--- a/drivers/base/regmap/regcache-rbtree.c
+++ b/drivers/base/regmap/regcache-rbtree.c
@@ -39,11 +39,12 @@ struct regcache_rbtree_ctx {
 };
 
 static inline void regcache_rbtree_get_base_top_reg(
+	struct regmap *map,
 	struct regcache_rbtree_node *rbnode,
 	unsigned int *base, unsigned int *top)
 {
 	*base = rbnode->base_reg;
-	*top = rbnode->base_reg + rbnode->blklen - 1;
+	*top = rbnode->base_reg + ((rbnode->blklen - 1) * map->reg_stride);
 }
 
 static unsigned int regcache_rbtree_get_register(
@@ -70,7 +71,8 @@ static struct regcache_rbtree_node *regcache_rbtree_lookup(struct regmap *map,
 
 	rbnode = rbtree_ctx->cached_rbnode;
 	if (rbnode) {
-		regcache_rbtree_get_base_top_reg(rbnode, &base_reg, &top_reg);
+		regcache_rbtree_get_base_top_reg(map, rbnode, &base_reg,
+						 &top_reg);
 		if (reg >= base_reg && reg <= top_reg)
 			return rbnode;
 	}
@@ -78,7 +80,8 @@ static struct regcache_rbtree_node *regcache_rbtree_lookup(struct regmap *map,
 	node = rbtree_ctx->root.rb_node;
 	while (node) {
 		rbnode = container_of(node, struct regcache_rbtree_node, node);
-		regcache_rbtree_get_base_top_reg(rbnode, &base_reg, &top_reg);
+		regcache_rbtree_get_base_top_reg(map, rbnode, &base_reg,
+						 &top_reg);
 		if (reg >= base_reg && reg <= top_reg) {
 			rbtree_ctx->cached_rbnode = rbnode;
 			return rbnode;
@@ -92,7 +95,7 @@ static struct regcache_rbtree_node *regcache_rbtree_lookup(struct regmap *map,
 	return NULL;
 }
 
-static int regcache_rbtree_insert(struct rb_root *root,
+static int regcache_rbtree_insert(struct regmap *map, struct rb_root *root,
 				  struct regcache_rbtree_node *rbnode)
 {
 	struct rb_node **new, *parent;
@@ -106,7 +109,7 @@ static int regcache_rbtree_insert(struct rb_root *root,
 		rbnode_tmp = container_of(*new, struct regcache_rbtree_node,
 					  node);
 		/* base and top registers of the current rbnode */
-		regcache_rbtree_get_base_top_reg(rbnode_tmp, &base_reg_tmp,
+		regcache_rbtree_get_base_top_reg(map, rbnode_tmp, &base_reg_tmp,
 						 &top_reg_tmp);
 		/* base register of the rbnode to be added */
 		base_reg = rbnode->base_reg;
@@ -138,19 +141,20 @@ static int rbtree_show(struct seq_file *s, void *ignored)
 	unsigned int base, top;
 	int nodes = 0;
 	int registers = 0;
-	int average;
+	int this_registers, average;
 
-	mutex_lock(&map->lock);
+	map->lock(map);
 
 	for (node = rb_first(&rbtree_ctx->root); node != NULL;
 	     node = rb_next(node)) {
 		n = container_of(node, struct regcache_rbtree_node, node);
 
-		regcache_rbtree_get_base_top_reg(n, &base, &top);
-		seq_printf(s, "%x-%x (%d)\n", base, top, top - base + 1);
+		regcache_rbtree_get_base_top_reg(map, n, &base, &top);
+		this_registers = ((top - base) / map->reg_stride) + 1;
+		seq_printf(s, "%x-%x (%d)\n", base, top, this_registers);
 
 		nodes++;
-		registers += top - base + 1;
+		registers += this_registers;
 	}
 
 	if (nodes)
@@ -161,7 +165,7 @@ static int rbtree_show(struct seq_file *s, void *ignored)
 	seq_printf(s, "%d nodes, %d registers, average %d registers\n",
 		   nodes, registers, average);
 
-	mutex_unlock(&map->lock);
+	map->unlock(map);
 
 	return 0;
 }
@@ -255,7 +259,7 @@ static int regcache_rbtree_read(struct regmap *map,
 
 	rbnode = regcache_rbtree_lookup(map, reg);
 	if (rbnode) {
-		reg_tmp = reg - rbnode->base_reg;
+		reg_tmp = (reg - rbnode->base_reg) / map->reg_stride;
 		*value = regcache_rbtree_get_register(rbnode, reg_tmp,
 						      map->cache_word_size);
 	} else {
@@ -310,7 +314,7 @@ static int regcache_rbtree_write(struct regmap *map, unsigned int reg,
 	 */
 	rbnode = regcache_rbtree_lookup(map, reg);
 	if (rbnode) {
-		reg_tmp = reg - rbnode->base_reg;
+		reg_tmp = (reg - rbnode->base_reg) / map->reg_stride;
 		val = regcache_rbtree_get_register(rbnode, reg_tmp,
 						   map->cache_word_size);
 		if (val == value)
@@ -321,13 +325,15 @@ static int regcache_rbtree_write(struct regmap *map, unsigned int reg,
 		/* look for an adjacent register to the one we are about to add */
 		for (node = rb_first(&rbtree_ctx->root); node;
 		     node = rb_next(node)) {
-			rbnode_tmp = rb_entry(node, struct regcache_rbtree_node, node);
+			rbnode_tmp = rb_entry(node, struct regcache_rbtree_node,
+					      node);
 			for (i = 0; i < rbnode_tmp->blklen; i++) {
-				reg_tmp = rbnode_tmp->base_reg + i;
-				if (abs(reg_tmp - reg) != 1)
+				reg_tmp = rbnode_tmp->base_reg +
+						(i * map->reg_stride);
+				if (abs(reg_tmp - reg) != map->reg_stride)
 					continue;
 				/* decide where in the block to place our register */
-				if (reg_tmp + 1 == reg)
+				if (reg_tmp + map->reg_stride == reg)
 					pos = i + 1;
 				else
 					pos = i;
@@ -357,7 +363,7 @@ static int regcache_rbtree_write(struct regmap *map, unsigned int reg,
 			return -ENOMEM;
 		}
 		regcache_rbtree_set_register(rbnode, 0, value, map->cache_word_size);
-		regcache_rbtree_insert(&rbtree_ctx->root, rbnode);
+		regcache_rbtree_insert(map, &rbtree_ctx->root, rbnode);
 		rbtree_ctx->cached_rbnode = rbnode;
 	}
 
@@ -397,7 +403,7 @@ static int regcache_rbtree_sync(struct regmap *map, unsigned int min,
 			end = rbnode->blklen;
 
 		for (i = base; i < end; i++) {
-			regtmp = rbnode->base_reg + i;
+			regtmp = rbnode->base_reg + (i * map->reg_stride);
 			val = regcache_rbtree_get_register(rbnode, i,
 							   map->cache_word_size);
 
diff --git a/drivers/base/regmap/regcache.c b/drivers/base/regmap/regcache.c
index 74b69095def..835883bda97 100644
--- a/drivers/base/regmap/regcache.c
+++ b/drivers/base/regmap/regcache.c
@@ -59,7 +59,7 @@ static int regcache_hw_init(struct regmap *map)
 	for (count = 0, i = 0; i < map->num_reg_defaults_raw; i++) {
 		val = regcache_get_val(map->reg_defaults_raw,
 				       i, map->cache_word_size);
-		if (regmap_volatile(map, i))
+		if (regmap_volatile(map, i * map->reg_stride))
 			continue;
 		count++;
 	}
@@ -76,9 +76,9 @@ static int regcache_hw_init(struct regmap *map)
 	for (i = 0, j = 0; i < map->num_reg_defaults_raw; i++) {
 		val = regcache_get_val(map->reg_defaults_raw,
 				       i, map->cache_word_size);
-		if (regmap_volatile(map, i))
+		if (regmap_volatile(map, i * map->reg_stride))
 			continue;
-		map->reg_defaults[j].reg = i;
+		map->reg_defaults[j].reg = i * map->reg_stride;
 		map->reg_defaults[j].def = val;
 		j++;
 	}
@@ -98,6 +98,10 @@ int regcache_init(struct regmap *map, const struct regmap_config *config)
 	int i;
 	void *tmp_buf;
 
+	for (i = 0; i < config->num_reg_defaults; i++)
+		if (config->reg_defaults[i].reg % map->reg_stride)
+			return -EINVAL;
+
 	if (map->cache_type == REGCACHE_NONE) {
 		map->cache_bypass = true;
 		return 0;
@@ -264,7 +268,7 @@ int regcache_sync(struct regmap *map)
 
 	BUG_ON(!map->cache_ops || !map->cache_ops->sync);
 
-	mutex_lock(&map->lock);
+	map->lock(map);
 	/* Remember the initial bypass state */
 	bypass = map->cache_bypass;
 	dev_dbg(map->dev, "Syncing %s cache\n",
@@ -278,6 +282,10 @@ int regcache_sync(struct regmap *map)
 	/* Apply any patch first */
 	map->cache_bypass = 1;
 	for (i = 0; i < map->patch_regs; i++) {
+		if (map->patch[i].reg % map->reg_stride) {
+			ret = -EINVAL;
+			goto out;
+		}
 		ret = _regmap_write(map, map->patch[i].reg, map->patch[i].def);
 		if (ret != 0) {
 			dev_err(map->dev, "Failed to write %x = %x: %d\n",
@@ -296,7 +304,7 @@ out:
 	trace_regcache_sync(map->dev, name, "stop");
 	/* Restore the bypass state */
 	map->cache_bypass = bypass;
-	mutex_unlock(&map->lock);
+	map->unlock(map);
 
 	return ret;
 }
@@ -323,7 +331,7 @@ int regcache_sync_region(struct regmap *map, unsigned int min,
 
 	BUG_ON(!map->cache_ops || !map->cache_ops->sync);
 
-	mutex_lock(&map->lock);
+	map->lock(map);
 
 	/* Remember the initial bypass state */
 	bypass = map->cache_bypass;
@@ -342,7 +350,7 @@ out:
 	trace_regcache_sync(map->dev, name, "stop region");
 	/* Restore the bypass state */
 	map->cache_bypass = bypass;
-	mutex_unlock(&map->lock);
+	map->unlock(map);
 
 	return ret;
 }
@@ -362,11 +370,11 @@ EXPORT_SYMBOL_GPL(regcache_sync_region);
  */
 void regcache_cache_only(struct regmap *map, bool enable)
 {
-	mutex_lock(&map->lock);
+	map->lock(map);
 	WARN_ON(map->cache_bypass && enable);
 	map->cache_only = enable;
 	trace_regmap_cache_only(map->dev, enable);
-	mutex_unlock(&map->lock);
+	map->unlock(map);
 }
 EXPORT_SYMBOL_GPL(regcache_cache_only);
 
@@ -381,9 +389,9 @@ EXPORT_SYMBOL_GPL(regcache_cache_only);
  */
 void regcache_mark_dirty(struct regmap *map)
 {
-	mutex_lock(&map->lock);
+	map->lock(map);
 	map->cache_dirty = true;
-	mutex_unlock(&map->lock);
+	map->unlock(map);
 }
 EXPORT_SYMBOL_GPL(regcache_mark_dirty);
 
@@ -400,11 +408,11 @@ EXPORT_SYMBOL_GPL(regcache_mark_dirty);
  */
 void regcache_cache_bypass(struct regmap *map, bool enable)
 {
-	mutex_lock(&map->lock);
+	map->lock(map);
 	WARN_ON(map->cache_only && enable);
 	map->cache_bypass = enable;
 	trace_regmap_cache_bypass(map->dev, enable);
-	mutex_unlock(&map->lock);
+	map->unlock(map);
 }
 EXPORT_SYMBOL_GPL(regcache_cache_bypass);
 
diff --git a/drivers/base/regmap/regmap-debugfs.c b/drivers/base/regmap/regmap-debugfs.c
index 251eb70f83e..bb1ff175b96 100644
--- a/drivers/base/regmap/regmap-debugfs.c
+++ b/drivers/base/regmap/regmap-debugfs.c
@@ -80,7 +80,7 @@ static ssize_t regmap_map_read_file(struct file *file, char __user *user_buf,
 	val_len = 2 * map->format.val_bytes;
 	tot_len = reg_len + val_len + 3;      /* : \n */
 
-	for (i = 0; i < map->max_register + 1; i++) {
+	for (i = 0; i <= map->max_register; i += map->reg_stride) {
 		if (!regmap_readable(map, i))
 			continue;
 
@@ -197,7 +197,7 @@ static ssize_t regmap_access_read_file(struct file *file,
 	reg_len = regmap_calc_reg_len(map->max_register, buf, count);
 	tot_len = reg_len + 10; /* ': R W V P\n' */
 
-	for (i = 0; i < map->max_register + 1; i++) {
+	for (i = 0; i <= map->max_register; i += map->reg_stride) {
 		/* Ignore registers which are neither readable nor writable */
 		if (!regmap_readable(map, i) && !regmap_writeable(map, i))
 			continue;
@@ -242,10 +242,17 @@ static const struct file_operations regmap_access_fops = {
 	.llseek = default_llseek,
 };
 
-void regmap_debugfs_init(struct regmap *map)
+void regmap_debugfs_init(struct regmap *map, const char *name)
 {
-	map->debugfs = debugfs_create_dir(dev_name(map->dev),
-					  regmap_debugfs_root);
+	if (name) {
+		map->debugfs_name = kasprintf(GFP_KERNEL, "%s-%s",
+					      dev_name(map->dev), name);
+		name = map->debugfs_name;
+	} else {
+		name = dev_name(map->dev);
+	}
+
+	map->debugfs = debugfs_create_dir(name, regmap_debugfs_root);
 	if (!map->debugfs) {
 		dev_warn(map->dev, "Failed to create debugfs directory\n");
 		return;
@@ -274,6 +281,7 @@ void regmap_debugfs_init(struct regmap *map)
 void regmap_debugfs_exit(struct regmap *map)
 {
 	debugfs_remove_recursive(map->debugfs);
+	kfree(map->debugfs_name);
 }
 
 void regmap_debugfs_initcall(void)
diff --git a/drivers/base/regmap/regmap-i2c.c b/drivers/base/regmap/regmap-i2c.c
index 9a3a8c56438..5f6b2478bf1 100644
--- a/drivers/base/regmap/regmap-i2c.c
+++ b/drivers/base/regmap/regmap-i2c.c
@@ -15,8 +15,9 @@
 #include <linux/module.h>
 #include <linux/init.h>
 
-static int regmap_i2c_write(struct device *dev, const void *data, size_t count)
+static int regmap_i2c_write(void *context, const void *data, size_t count)
 {
+	struct device *dev = context;
 	struct i2c_client *i2c = to_i2c_client(dev);
 	int ret;
 
@@ -29,10 +30,11 @@ static int regmap_i2c_write(struct device *dev, const void *data, size_t count)
 		return -EIO;
 }
 
-static int regmap_i2c_gather_write(struct device *dev,
+static int regmap_i2c_gather_write(void *context,
 				   const void *reg, size_t reg_size,
 				   const void *val, size_t val_size)
 {
+	struct device *dev = context;
 	struct i2c_client *i2c = to_i2c_client(dev);
 	struct i2c_msg xfer[2];
 	int ret;
@@ -62,10 +64,11 @@ static int regmap_i2c_gather_write(struct device *dev,
 		return -EIO;
 }
 
-static int regmap_i2c_read(struct device *dev,
+static int regmap_i2c_read(void *context,
 			   const void *reg, size_t reg_size,
 			   void *val, size_t val_size)
 {
+	struct device *dev = context;
 	struct i2c_client *i2c = to_i2c_client(dev);
 	struct i2c_msg xfer[2];
 	int ret;
@@ -107,7 +110,7 @@ static struct regmap_bus regmap_i2c = {
 struct regmap *regmap_init_i2c(struct i2c_client *i2c,
 			       const struct regmap_config *config)
 {
-	return regmap_init(&i2c->dev, &regmap_i2c, config);
+	return regmap_init(&i2c->dev, &regmap_i2c, &i2c->dev, config);
 }
 EXPORT_SYMBOL_GPL(regmap_init_i2c);
 
@@ -124,7 +127,7 @@ EXPORT_SYMBOL_GPL(regmap_init_i2c);
 struct regmap *devm_regmap_init_i2c(struct i2c_client *i2c,
 				    const struct regmap_config *config)
 {
-	return devm_regmap_init(&i2c->dev, &regmap_i2c, config);
+	return devm_regmap_init(&i2c->dev, &regmap_i2c, &i2c->dev, config);
 }
 EXPORT_SYMBOL_GPL(devm_regmap_init_i2c);
 
diff --git a/drivers/base/regmap/regmap-irq.c b/drivers/base/regmap/regmap-irq.c
index 1befaa7a31c..4fac4b9be88 100644
--- a/drivers/base/regmap/regmap-irq.c
+++ b/drivers/base/regmap/regmap-irq.c
@@ -15,6 +15,7 @@
 #include <linux/regmap.h>
 #include <linux/irq.h>
 #include <linux/interrupt.h>
+#include <linux/irqdomain.h>
 #include <linux/slab.h>
 
 #include "internal.h"
@@ -26,18 +27,20 @@ struct regmap_irq_chip_data {
 	struct regmap_irq_chip *chip;
 
 	int irq_base;
+	struct irq_domain *domain;
 
-	void *status_reg_buf;
 	unsigned int *status_buf;
 	unsigned int *mask_buf;
 	unsigned int *mask_buf_def;
+
+	unsigned int irq_reg_stride;
 };
 
 static inline const
 struct regmap_irq *irq_to_regmap_irq(struct regmap_irq_chip_data *data,
 				     int irq)
 {
-	return &data->chip->irqs[irq - data->irq_base];
+	return &data->chip->irqs[irq];
 }
 
 static void regmap_irq_lock(struct irq_data *data)
@@ -50,6 +53,7 @@ static void regmap_irq_lock(struct irq_data *data)
 static void regmap_irq_sync_unlock(struct irq_data *data)
 {
 	struct regmap_irq_chip_data *d = irq_data_get_irq_chip_data(data);
+	struct regmap *map = d->map;
 	int i, ret;
 
 	/*
@@ -58,11 +62,13 @@ static void regmap_irq_sync_unlock(struct irq_data *data)
 	 * suppress pointless writes.
 	 */
 	for (i = 0; i < d->chip->num_regs; i++) {
-		ret = regmap_update_bits(d->map, d->chip->mask_base + i,
+		ret = regmap_update_bits(d->map, d->chip->mask_base +
+						(i * map->reg_stride *
+						d->irq_reg_stride),
 					 d->mask_buf_def[i], d->mask_buf[i]);
 		if (ret != 0)
 			dev_err(d->map->dev, "Failed to sync masks in %x\n",
-				d->chip->mask_base + i);
+				d->chip->mask_base + (i * map->reg_stride));
 	}
 
 	mutex_unlock(&d->lock);
@@ -71,17 +77,19 @@ static void regmap_irq_sync_unlock(struct irq_data *data)
 static void regmap_irq_enable(struct irq_data *data)
 {
 	struct regmap_irq_chip_data *d = irq_data_get_irq_chip_data(data);
-	const struct regmap_irq *irq_data = irq_to_regmap_irq(d, data->irq);
+	struct regmap *map = d->map;
+	const struct regmap_irq *irq_data = irq_to_regmap_irq(d, data->hwirq);
 
-	d->mask_buf[irq_data->reg_offset] &= ~irq_data->mask;
+	d->mask_buf[irq_data->reg_offset / map->reg_stride] &= ~irq_data->mask;
 }
 
 static void regmap_irq_disable(struct irq_data *data)
 {
 	struct regmap_irq_chip_data *d = irq_data_get_irq_chip_data(data);
-	const struct regmap_irq *irq_data = irq_to_regmap_irq(d, data->irq);
+	struct regmap *map = d->map;
+	const struct regmap_irq *irq_data = irq_to_regmap_irq(d, data->hwirq);
 
-	d->mask_buf[irq_data->reg_offset] |= irq_data->mask;
+	d->mask_buf[irq_data->reg_offset / map->reg_stride] |= irq_data->mask;
 }
 
 static struct irq_chip regmap_irq_chip = {
@@ -98,18 +106,8 @@ static irqreturn_t regmap_irq_thread(int irq, void *d)
 	struct regmap_irq_chip *chip = data->chip;
 	struct regmap *map = data->map;
 	int ret, i;
-	u8 *buf8 = data->status_reg_buf;
-	u16 *buf16 = data->status_reg_buf;
-	u32 *buf32 = data->status_reg_buf;
 	bool handled = false;
 
-	ret = regmap_bulk_read(map, chip->status_base, data->status_reg_buf,
-			       chip->num_regs);
-	if (ret != 0) {
-		dev_err(map->dev, "Failed to read IRQ status: %d\n", ret);
-		return IRQ_NONE;
-	}
-
 	/*
 	 * Ignore masked IRQs and ack if we need to; we ack early so
 	 * there is no race between handling and acknowleding the
@@ -118,36 +116,34 @@ static irqreturn_t regmap_irq_thread(int irq, void *d)
 	 * doing a write per register.
 	 */
 	for (i = 0; i < data->chip->num_regs; i++) {
-		switch (map->format.val_bytes) {
-		case 1:
-			data->status_buf[i] = buf8[i];
-			break;
-		case 2:
-			data->status_buf[i] = buf16[i];
-			break;
-		case 4:
-			data->status_buf[i] = buf32[i];
-			break;
-		default:
-			BUG();
+		ret = regmap_read(map, chip->status_base + (i * map->reg_stride
+				   * data->irq_reg_stride),
+				   &data->status_buf[i]);
+
+		if (ret != 0) {
+			dev_err(map->dev, "Failed to read IRQ status: %d\n",
+					ret);
 			return IRQ_NONE;
 		}
 
 		data->status_buf[i] &= ~data->mask_buf[i];
 
 		if (data->status_buf[i] && chip->ack_base) {
-			ret = regmap_write(map, chip->ack_base + i,
+			ret = regmap_write(map, chip->ack_base +
+						(i * map->reg_stride *
+						data->irq_reg_stride),
 					   data->status_buf[i]);
 			if (ret != 0)
 				dev_err(map->dev, "Failed to ack 0x%x: %d\n",
-					chip->ack_base + i, ret);
+					chip->ack_base + (i * map->reg_stride),
+					ret);
 		}
 	}
 
 	for (i = 0; i < chip->num_irqs; i++) {
-		if (data->status_buf[chip->irqs[i].reg_offset] &
-		    chip->irqs[i].mask) {
-			handle_nested_irq(data->irq_base + i);
+		if (data->status_buf[chip->irqs[i].reg_offset /
+				     map->reg_stride] & chip->irqs[i].mask) {
+			handle_nested_irq(irq_find_mapping(data->domain, i));
 			handled = true;
 		}
 	}
@@ -158,6 +154,31 @@ static irqreturn_t regmap_irq_thread(int irq, void *d)
 		return IRQ_NONE;
 }
 
+static int regmap_irq_map(struct irq_domain *h, unsigned int virq,
+			  irq_hw_number_t hw)
+{
+	struct regmap_irq_chip_data *data = h->host_data;
+
+	irq_set_chip_data(virq, data);
+	irq_set_chip_and_handler(virq, &regmap_irq_chip, handle_edge_irq);
+	irq_set_nested_thread(virq, 1);
+
+	/* ARM needs us to explicitly flag the IRQ as valid
+	 * and will set them noprobe when we do so. */
+#ifdef CONFIG_ARM
+	set_irq_flags(virq, IRQF_VALID);
+#else
+	irq_set_noprobe(virq);
+#endif
+
+	return 0;
+}
+
+static struct irq_domain_ops regmap_domain_ops = {
+	.map	= regmap_irq_map,
+	.xlate	= irq_domain_xlate_twocell,
+};
+
 /**
  * regmap_add_irq_chip(): Use standard regmap IRQ controller handling
  *
@@ -178,30 +199,37 @@ int regmap_add_irq_chip(struct regmap *map, int irq, int irq_flags,
 			struct regmap_irq_chip_data **data)
 {
 	struct regmap_irq_chip_data *d;
-	int cur_irq, i;
+	int i;
 	int ret = -ENOMEM;
 
-	irq_base = irq_alloc_descs(irq_base, 0, chip->num_irqs, 0);
-	if (irq_base < 0) {
-		dev_warn(map->dev, "Failed to allocate IRQs: %d\n",
-			 irq_base);
-		return irq_base;
+	for (i = 0; i < chip->num_irqs; i++) {
+		if (chip->irqs[i].reg_offset % map->reg_stride)
+			return -EINVAL;
+		if (chip->irqs[i].reg_offset / map->reg_stride >=
+		    chip->num_regs)
+			return -EINVAL;
+	}
+
+	if (irq_base) {
+		irq_base = irq_alloc_descs(irq_base, 0, chip->num_irqs, 0);
+		if (irq_base < 0) {
+			dev_warn(map->dev, "Failed to allocate IRQs: %d\n",
+				 irq_base);
+			return irq_base;
+		}
 	}
 
 	d = kzalloc(sizeof(*d), GFP_KERNEL);
 	if (!d)
 		return -ENOMEM;
 
+	*data = d;
+
 	d->status_buf = kzalloc(sizeof(unsigned int) * chip->num_regs,
 				GFP_KERNEL);
 	if (!d->status_buf)
 		goto err_alloc;
 
-	d->status_reg_buf = kzalloc(map->format.val_bytes * chip->num_regs,
-				    GFP_KERNEL);
-	if (!d->status_reg_buf)
-		goto err_alloc;
-
 	d->mask_buf = kzalloc(sizeof(unsigned int) * chip->num_regs,
 			      GFP_KERNEL);
 	if (!d->mask_buf)
@@ -215,54 +243,59 @@ int regmap_add_irq_chip(struct regmap *map, int irq, int irq_flags,
 	d->map = map;
 	d->chip = chip;
 	d->irq_base = irq_base;
+
+	if (chip->irq_reg_stride)
+		d->irq_reg_stride = chip->irq_reg_stride;
+	else
+		d->irq_reg_stride = 1;
+
 	mutex_init(&d->lock);
 
 	for (i = 0; i < chip->num_irqs; i++)
-		d->mask_buf_def[chip->irqs[i].reg_offset]
+		d->mask_buf_def[chip->irqs[i].reg_offset / map->reg_stride]
 			|= chip->irqs[i].mask;
 
 	/* Mask all the interrupts by default */
 	for (i = 0; i < chip->num_regs; i++) {
 		d->mask_buf[i] = d->mask_buf_def[i];
-		ret = regmap_write(map, chip->mask_base + i, d->mask_buf[i]);
+		ret = regmap_write(map, chip->mask_base + (i * map->reg_stride
+				   * d->irq_reg_stride),
+				   d->mask_buf[i]);
 		if (ret != 0) {
 			dev_err(map->dev, "Failed to set masks in 0x%x: %d\n",
-				chip->mask_base + i, ret);
+				chip->mask_base + (i * map->reg_stride), ret);
 			goto err_alloc;
 		}
 	}
 
-	/* Register them with genirq */
-	for (cur_irq = irq_base;
-	     cur_irq < chip->num_irqs + irq_base;
-	     cur_irq++) {
-		irq_set_chip_data(cur_irq, d);
-		irq_set_chip_and_handler(cur_irq, &regmap_irq_chip,
-					 handle_edge_irq);
-		irq_set_nested_thread(cur_irq, 1);
-
-		/* ARM needs us to explicitly flag the IRQ as valid
-		 * and will set them noprobe when we do so. */
-#ifdef CONFIG_ARM
-		set_irq_flags(cur_irq, IRQF_VALID);
-#else
-		irq_set_noprobe(cur_irq);
-#endif
+	if (irq_base)
+		d->domain = irq_domain_add_legacy(map->dev->of_node,
+						  chip->num_irqs, irq_base, 0,
+						  &regmap_domain_ops, d);
+	else
+		d->domain = irq_domain_add_linear(map->dev->of_node,
+						  chip->num_irqs,
+						  &regmap_domain_ops, d);
+	if (!d->domain) {
+		dev_err(map->dev, "Failed to create IRQ domain\n");
+		ret = -ENOMEM;
+		goto err_alloc;
 	}
 
 	ret = request_threaded_irq(irq, NULL, regmap_irq_thread, irq_flags,
 				   chip->name, d);
 	if (ret != 0) {
 		dev_err(map->dev, "Failed to request IRQ %d: %d\n", irq, ret);
-		goto err_alloc;
+		goto err_domain;
 	}
 
 	return 0;
 
+err_domain:
+	/* Should really dispose of the domain but... */
 err_alloc:
 	kfree(d->mask_buf_def);
 	kfree(d->mask_buf);
-	kfree(d->status_reg_buf);
 	kfree(d->status_buf);
 	kfree(d);
 	return ret;
@@ -281,9 +314,9 @@ void regmap_del_irq_chip(int irq, struct regmap_irq_chip_data *d)
 		return;
 
 	free_irq(irq, d);
+	/* We should unmap the domain but... */
 	kfree(d->mask_buf_def);
 	kfree(d->mask_buf);
-	kfree(d->status_reg_buf);
 	kfree(d->status_buf);
 	kfree(d);
 }
@@ -298,6 +331,21 @@ EXPORT_SYMBOL_GPL(regmap_del_irq_chip);
  */
 int regmap_irq_chip_get_base(struct regmap_irq_chip_data *data)
 {
+	WARN_ON(!data->irq_base);
 	return data->irq_base;
 }
 EXPORT_SYMBOL_GPL(regmap_irq_chip_get_base);
+
+/**
+ * regmap_irq_get_virq(): Map an interrupt on a chip to a virtual IRQ
+ *
+ * Useful for drivers to request their own IRQs.
+ *
+ * @data: regmap_irq controller to operate on.
+ * @irq: index of the interrupt requested in the chip IRQs
+ */
+int regmap_irq_get_virq(struct regmap_irq_chip_data *data, int irq)
+{
+	return irq_create_mapping(data->domain, irq);
+}
+EXPORT_SYMBOL_GPL(regmap_irq_get_virq);
diff --git a/drivers/base/regmap/regmap-mmio.c b/drivers/base/regmap/regmap-mmio.c
new file mode 100644
index 00000000000..febd6de6c8a
--- /dev/null
+++ b/drivers/base/regmap/regmap-mmio.c
@@ -0,0 +1,224 @@
+/*
+ * Register map access API - MMIO support
+ *
+ * Copyright (c) 2012, 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/io.h>
+#include <linux/module.h>
+#include <linux/regmap.h>
+#include <linux/slab.h>
+
+struct regmap_mmio_context {
+	void __iomem *regs;
+	unsigned val_bytes;
+};
+
+static int regmap_mmio_gather_write(void *context,
+				    const void *reg, size_t reg_size,
+				    const void *val, size_t val_size)
+{
+	struct regmap_mmio_context *ctx = context;
+	u32 offset;
+
+	BUG_ON(reg_size != 4);
+
+	offset = be32_to_cpup(reg);
+
+	while (val_size) {
+		switch (ctx->val_bytes) {
+		case 1:
+			writeb(*(u8 *)val, ctx->regs + offset);
+			break;
+		case 2:
+			writew(be16_to_cpup(val), ctx->regs + offset);
+			break;
+		case 4:
+			writel(be32_to_cpup(val), ctx->regs + offset);
+			break;
+#ifdef CONFIG_64BIT
+		case 8:
+			writeq(be64_to_cpup(val), ctx->regs + offset);
+			break;
+#endif
+		default:
+			/* Should be caught by regmap_mmio_check_config */
+			BUG();
+		}
+		val_size -= ctx->val_bytes;
+		val += ctx->val_bytes;
+		offset += ctx->val_bytes;
+	}
+
+	return 0;
+}
+
+static int regmap_mmio_write(void *context, const void *data, size_t count)
+{
+	BUG_ON(count < 4);
+
+	return regmap_mmio_gather_write(context, data, 4, data + 4, count - 4);
+}
+
+static int regmap_mmio_read(void *context,
+			    const void *reg, size_t reg_size,
+			    void *val, size_t val_size)
+{
+	struct regmap_mmio_context *ctx = context;
+	u32 offset;
+
+	BUG_ON(reg_size != 4);
+
+	offset = be32_to_cpup(reg);
+
+	while (val_size) {
+		switch (ctx->val_bytes) {
+		case 1:
+			*(u8 *)val = readb(ctx->regs + offset);
+			break;
+		case 2:
+			*(u16 *)val = cpu_to_be16(readw(ctx->regs + offset));
+			break;
+		case 4:
+			*(u32 *)val = cpu_to_be32(readl(ctx->regs + offset));
+			break;
+#ifdef CONFIG_64BIT
+		case 8:
+			*(u64 *)val = cpu_to_be32(readq(ctx->regs + offset));
+			break;
+#endif
+		default:
+			/* Should be caught by regmap_mmio_check_config */
+			BUG();
+		}
+		val_size -= ctx->val_bytes;
+		val += ctx->val_bytes;
+		offset += ctx->val_bytes;
+	}
+
+	return 0;
+}
+
+static void regmap_mmio_free_context(void *context)
+{
+	kfree(context);
+}
+
+static struct regmap_bus regmap_mmio = {
+	.fast_io = true,
+	.write = regmap_mmio_write,
+	.gather_write = regmap_mmio_gather_write,
+	.read = regmap_mmio_read,
+	.free_context = regmap_mmio_free_context,
+};
+
+struct regmap_mmio_context *regmap_mmio_gen_context(void __iomem *regs,
+					const struct regmap_config *config)
+{
+	struct regmap_mmio_context *ctx;
+	int min_stride;
+
+	if (config->reg_bits != 32)
+		return ERR_PTR(-EINVAL);
+
+	if (config->pad_bits)
+		return ERR_PTR(-EINVAL);
+
+	switch (config->val_bits) {
+	case 8:
+		/* The core treats 0 as 1 */
+		min_stride = 0;
+		break;
+	case 16:
+		min_stride = 2;
+		break;
+	case 32:
+		min_stride = 4;
+		break;
+#ifdef CONFIG_64BIT
+	case 64:
+		min_stride = 8;
+		break;
+#endif
+		break;
+	default:
+		return ERR_PTR(-EINVAL);
+	}
+
+	if (config->reg_stride < min_stride)
+		return ERR_PTR(-EINVAL);
+
+	ctx = kzalloc(GFP_KERNEL, sizeof(*ctx));
+	if (!ctx)
+		return ERR_PTR(-ENOMEM);
+
+	ctx->regs = regs;
+	ctx->val_bytes = config->val_bits / 8;
+
+	return ctx;
+}
+
+/**
+ * regmap_init_mmio(): Initialise register map
+ *
+ * @dev: Device that will be interacted with
+ * @regs: Pointer to memory-mapped IO region
+ * @config: Configuration for register map
+ *
+ * The return value will be an ERR_PTR() on error or a valid pointer to
+ * a struct regmap.
+ */
+struct regmap *regmap_init_mmio(struct device *dev,
+				void __iomem *regs,
+				const struct regmap_config *config)
+{
+	struct regmap_mmio_context *ctx;
+
+	ctx = regmap_mmio_gen_context(regs, config);
+	if (IS_ERR(ctx))
+		return ERR_CAST(ctx);
+
+	return regmap_init(dev, &regmap_mmio, ctx, config);
+}
+EXPORT_SYMBOL_GPL(regmap_init_mmio);
+
+/**
+ * devm_regmap_init_mmio(): Initialise managed register map
+ *
+ * @dev: Device that will be interacted with
+ * @regs: Pointer to memory-mapped IO region
+ * @config: Configuration for register map
+ *
+ * The return value will be an ERR_PTR() on error or a valid pointer
+ * to a struct regmap.  The regmap will be automatically freed by the
+ * device management code.
+ */
+struct regmap *devm_regmap_init_mmio(struct device *dev,
+				     void __iomem *regs,
+				     const struct regmap_config *config)
+{
+	struct regmap_mmio_context *ctx;
+
+	ctx = regmap_mmio_gen_context(regs, config);
+	if (IS_ERR(ctx))
+		return ERR_CAST(ctx);
+
+	return devm_regmap_init(dev, &regmap_mmio, ctx, config);
+}
+EXPORT_SYMBOL_GPL(devm_regmap_init_mmio);
+
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/base/regmap/regmap-spi.c b/drivers/base/regmap/regmap-spi.c
index 7c0c35a39c3..ffa46a92ad3 100644
--- a/drivers/base/regmap/regmap-spi.c
+++ b/drivers/base/regmap/regmap-spi.c
@@ -15,17 +15,19 @@
 #include <linux/init.h>
 #include <linux/module.h>
 
-static int regmap_spi_write(struct device *dev, const void *data, size_t count)
+static int regmap_spi_write(void *context, const void *data, size_t count)
 {
+	struct device *dev = context;
 	struct spi_device *spi = to_spi_device(dev);
 
 	return spi_write(spi, data, count);
 }
 
-static int regmap_spi_gather_write(struct device *dev,
+static int regmap_spi_gather_write(void *context,
 				   const void *reg, size_t reg_len,
 				   const void *val, size_t val_len)
 {
+	struct device *dev = context;
 	struct spi_device *spi = to_spi_device(dev);
 	struct spi_message m;
 	struct spi_transfer t[2] = { { .tx_buf = reg, .len = reg_len, },
@@ -38,10 +40,11 @@ static int regmap_spi_gather_write(struct device *dev,
 	return spi_sync(spi, &m);
 }
 
-static int regmap_spi_read(struct device *dev,
+static int regmap_spi_read(void *context,
 			   const void *reg, size_t reg_size,
 			   void *val, size_t val_size)
 {
+	struct device *dev = context;
 	struct spi_device *spi = to_spi_device(dev);
 
 	return spi_write_then_read(spi, reg, reg_size, val, val_size);
@@ -66,7 +69,7 @@ static struct regmap_bus regmap_spi = {
 struct regmap *regmap_init_spi(struct spi_device *spi,
 			       const struct regmap_config *config)
 {
-	return regmap_init(&spi->dev, &regmap_spi, config);
+	return regmap_init(&spi->dev, &regmap_spi, &spi->dev, config);
 }
 EXPORT_SYMBOL_GPL(regmap_init_spi);
 
@@ -83,7 +86,7 @@ EXPORT_SYMBOL_GPL(regmap_init_spi);
 struct regmap *devm_regmap_init_spi(struct spi_device *spi,
 				    const struct regmap_config *config)
 {
-	return devm_regmap_init(&spi->dev, &regmap_spi, config);
+	return devm_regmap_init(&spi->dev, &regmap_spi, &spi->dev, config);
 }
 EXPORT_SYMBOL_GPL(devm_regmap_init_spi);
 
diff --git a/drivers/base/regmap/regmap.c b/drivers/base/regmap/regmap.c
index 7a3f535e481..0bcda488f11 100644
--- a/drivers/base/regmap/regmap.c
+++ b/drivers/base/regmap/regmap.c
@@ -112,25 +112,36 @@ static void regmap_format_10_14_write(struct regmap *map,
 	out[0] = reg >> 2;
 }
 
-static void regmap_format_8(void *buf, unsigned int val)
+static void regmap_format_8(void *buf, unsigned int val, unsigned int shift)
 {
 	u8 *b = buf;
 
-	b[0] = val;
+	b[0] = val << shift;
 }
 
-static void regmap_format_16(void *buf, unsigned int val)
+static void regmap_format_16(void *buf, unsigned int val, unsigned int shift)
 {
 	__be16 *b = buf;
 
-	b[0] = cpu_to_be16(val);
+	b[0] = cpu_to_be16(val << shift);
 }
 
-static void regmap_format_32(void *buf, unsigned int val)
+static void regmap_format_24(void *buf, unsigned int val, unsigned int shift)
+{
+	u8 *b = buf;
+
+	val <<= shift;
+
+	b[0] = val >> 16;
+	b[1] = val >> 8;
+	b[2] = val;
+}
+
+static void regmap_format_32(void *buf, unsigned int val, unsigned int shift)
 {
 	__be32 *b = buf;
 
-	b[0] = cpu_to_be32(val);
+	b[0] = cpu_to_be32(val << shift);
 }
 
 static unsigned int regmap_parse_8(void *buf)
@@ -149,6 +160,16 @@ static unsigned int regmap_parse_16(void *buf)
 	return b[0];
 }
 
+static unsigned int regmap_parse_24(void *buf)
+{
+	u8 *b = buf;
+	unsigned int ret = b[2];
+	ret |= ((unsigned int)b[1]) << 8;
+	ret |= ((unsigned int)b[0]) << 16;
+
+	return ret;
+}
+
 static unsigned int regmap_parse_32(void *buf)
 {
 	__be32 *b = buf;
@@ -158,11 +179,41 @@ static unsigned int regmap_parse_32(void *buf)
 	return b[0];
 }
 
+static void regmap_lock_mutex(struct regmap *map)
+{
+	mutex_lock(&map->mutex);
+}
+
+static void regmap_unlock_mutex(struct regmap *map)
+{
+	mutex_unlock(&map->mutex);
+}
+
+static void regmap_lock_spinlock(struct regmap *map)
+{
+	spin_lock(&map->spinlock);
+}
+
+static void regmap_unlock_spinlock(struct regmap *map)
+{
+	spin_unlock(&map->spinlock);
+}
+
+static void dev_get_regmap_release(struct device *dev, void *res)
+{
+	/*
+	 * We don't actually have anything to do here; the goal here
+	 * is not to manage the regmap but to provide a simple way to
+	 * get the regmap back given a struct device.
+	 */
+}
+
 /**
  * regmap_init(): Initialise register map
  *
  * @dev: Device that will be interacted with
  * @bus: Bus-specific callbacks to use with device
+ * @bus_context: Data passed to bus-specific callbacks
  * @config: Configuration for register map
  *
  * The return value will be an ERR_PTR() on error or a valid pointer to
@@ -171,9 +222,10 @@ static unsigned int regmap_parse_32(void *buf)
  */
 struct regmap *regmap_init(struct device *dev,
 			   const struct regmap_bus *bus,
+			   void *bus_context,
 			   const struct regmap_config *config)
 {
-	struct regmap *map;
+	struct regmap *map, **m;
 	int ret = -EINVAL;
 
 	if (!bus || !config)
@@ -185,20 +237,36 @@ struct regmap *regmap_init(struct device *dev,
 		goto err;
 	}
 
-	mutex_init(&map->lock);
+	if (bus->fast_io) {
+		spin_lock_init(&map->spinlock);
+		map->lock = regmap_lock_spinlock;
+		map->unlock = regmap_unlock_spinlock;
+	} else {
+		mutex_init(&map->mutex);
+		map->lock = regmap_lock_mutex;
+		map->unlock = regmap_unlock_mutex;
+	}
 	map->format.buf_size = (config->reg_bits + config->val_bits) / 8;
 	map->format.reg_bytes = DIV_ROUND_UP(config->reg_bits, 8);
 	map->format.pad_bytes = config->pad_bits / 8;
 	map->format.val_bytes = DIV_ROUND_UP(config->val_bits, 8);
 	map->format.buf_size += map->format.pad_bytes;
+	map->reg_shift = config->pad_bits % 8;
+	if (config->reg_stride)
+		map->reg_stride = config->reg_stride;
+	else
+		map->reg_stride = 1;
+	map->use_single_rw = config->use_single_rw;
 	map->dev = dev;
 	map->bus = bus;
+	map->bus_context = bus_context;
 	map->max_register = config->max_register;
 	map->writeable_reg = config->writeable_reg;
 	map->readable_reg = config->readable_reg;
 	map->volatile_reg = config->volatile_reg;
 	map->precious_reg = config->precious_reg;
 	map->cache_type = config->cache_type;
+	map->name = config->name;
 
 	if (config->read_flag_mask || config->write_flag_mask) {
 		map->read_flag_mask = config->read_flag_mask;
@@ -207,7 +275,7 @@ struct regmap *regmap_init(struct device *dev,
 		map->read_flag_mask = bus->read_flag_mask;
 	}
 
-	switch (config->reg_bits) {
+	switch (config->reg_bits + map->reg_shift) {
 	case 2:
 		switch (config->val_bits) {
 		case 6:
@@ -273,12 +341,19 @@ struct regmap *regmap_init(struct device *dev,
 		map->format.format_val = regmap_format_16;
 		map->format.parse_val = regmap_parse_16;
 		break;
+	case 24:
+		map->format.format_val = regmap_format_24;
+		map->format.parse_val = regmap_parse_24;
+		break;
 	case 32:
 		map->format.format_val = regmap_format_32;
 		map->format.parse_val = regmap_parse_32;
 		break;
 	}
 
+	if (map->format.format_write)
+		map->use_single_rw = true;
+
 	if (!map->format.format_write &&
 	    !(map->format.format_reg && map->format.format_val))
 		goto err_map;
@@ -289,14 +364,25 @@ struct regmap *regmap_init(struct device *dev,
 		goto err_map;
 	}
 
-	regmap_debugfs_init(map);
+	regmap_debugfs_init(map, config->name);
 
 	ret = regcache_init(map, config);
 	if (ret < 0)
 		goto err_free_workbuf;
 
+	/* Add a devres resource for dev_get_regmap() */
+	m = devres_alloc(dev_get_regmap_release, sizeof(*m), GFP_KERNEL);
+	if (!m) {
+		ret = -ENOMEM;
+		goto err_cache;
+	}
+	*m = map;
+	devres_add(dev, m);
+
 	return map;
 
+err_cache:
+	regcache_exit(map);
 err_free_workbuf:
 	kfree(map->work_buf);
 err_map:
@@ -316,6 +402,7 @@ static void devm_regmap_release(struct device *dev, void *res)
  *
  * @dev: Device that will be interacted with
  * @bus: Bus-specific callbacks to use with device
+ * @bus_context: Data passed to bus-specific callbacks
  * @config: Configuration for register map
  *
  * The return value will be an ERR_PTR() on error or a valid pointer
@@ -325,6 +412,7 @@ static void devm_regmap_release(struct device *dev, void *res)
  */
 struct regmap *devm_regmap_init(struct device *dev,
 				const struct regmap_bus *bus,
+				void *bus_context,
 				const struct regmap_config *config)
 {
 	struct regmap **ptr, *regmap;
@@ -333,7 +421,7 @@ struct regmap *devm_regmap_init(struct device *dev,
 	if (!ptr)
 		return ERR_PTR(-ENOMEM);
 
-	regmap = regmap_init(dev, bus, config);
+	regmap = regmap_init(dev, bus, bus_context, config);
 	if (!IS_ERR(regmap)) {
 		*ptr = regmap;
 		devres_add(dev, ptr);
@@ -360,7 +448,7 @@ int regmap_reinit_cache(struct regmap *map, const struct regmap_config *config)
 {
 	int ret;
 
-	mutex_lock(&map->lock);
+	map->lock(map);
 
 	regcache_exit(map);
 	regmap_debugfs_exit(map);
@@ -372,14 +460,14 @@ int regmap_reinit_cache(struct regmap *map, const struct regmap_config *config)
 	map->precious_reg = config->precious_reg;
 	map->cache_type = config->cache_type;
 
-	regmap_debugfs_init(map);
+	regmap_debugfs_init(map, config->name);
 
 	map->cache_bypass = false;
 	map->cache_only = false;
 
 	ret = regcache_init(map, config);
 
-	mutex_unlock(&map->lock);
+	map->unlock(map);
 
 	return ret;
 }
@@ -391,11 +479,51 @@ void regmap_exit(struct regmap *map)
 {
 	regcache_exit(map);
 	regmap_debugfs_exit(map);
+	if (map->bus->free_context)
+		map->bus->free_context(map->bus_context);
 	kfree(map->work_buf);
 	kfree(map);
 }
 EXPORT_SYMBOL_GPL(regmap_exit);
 
+static int dev_get_regmap_match(struct device *dev, void *res, void *data)
+{
+	struct regmap **r = res;
+	if (!r || !*r) {
+		WARN_ON(!r || !*r);
+		return 0;
+	}
+
+	/* If the user didn't specify a name match any */
+	if (data)
+		return (*r)->name == data;
+	else
+		return 1;
+}
+
+/**
+ * dev_get_regmap(): Obtain the regmap (if any) for a device
+ *
+ * @dev: Device to retrieve the map for
+ * @name: Optional name for the register map, usually NULL.
+ *
+ * Returns the regmap for the device if one is present, or NULL.  If
+ * name is specified then it must match the name specified when
+ * registering the device, if it is NULL then the first regmap found
+ * will be used.  Devices with multiple register maps are very rare,
+ * generic code should normally not need to specify a name.
+ */
+struct regmap *dev_get_regmap(struct device *dev, const char *name)
+{
+	struct regmap **r = devres_find(dev, dev_get_regmap_release,
+					dev_get_regmap_match, (void *)name);
+
+	if (!r)
+		return NULL;
+	return *r;
+}
+EXPORT_SYMBOL_GPL(dev_get_regmap);
+
 static int _regmap_raw_write(struct regmap *map, unsigned int reg,
 			     const void *val, size_t val_len)
 {
@@ -408,7 +536,8 @@ static int _regmap_raw_write(struct regmap *map, unsigned int reg,
 	/* Check for unwritable registers before we start */
 	if (map->writeable_reg)
 		for (i = 0; i < val_len / map->format.val_bytes; i++)
-			if (!map->writeable_reg(map->dev, reg + i))
+			if (!map->writeable_reg(map->dev,
+						reg + (i * map->reg_stride)))
 				return -EINVAL;
 
 	if (!map->cache_bypass && map->format.parse_val) {
@@ -417,7 +546,8 @@ static int _regmap_raw_write(struct regmap *map, unsigned int reg,
 		for (i = 0; i < val_len / val_bytes; i++) {
 			memcpy(map->work_buf, val + (i * val_bytes), val_bytes);
 			ival = map->format.parse_val(map->work_buf);
-			ret = regcache_write(map, reg + i, ival);
+			ret = regcache_write(map, reg + (i * map->reg_stride),
+					     ival);
 			if (ret) {
 				dev_err(map->dev,
 				   "Error in caching of register: %u ret: %d\n",
@@ -431,7 +561,7 @@ static int _regmap_raw_write(struct regmap *map, unsigned int reg,
 		}
 	}
 
-	map->format.format_reg(map->work_buf, reg);
+	map->format.format_reg(map->work_buf, reg, map->reg_shift);
 
 	u8[0] |= map->write_flag_mask;
 
@@ -444,12 +574,12 @@ static int _regmap_raw_write(struct regmap *map, unsigned int reg,
 	 */
 	if (val == (map->work_buf + map->format.pad_bytes +
 		    map->format.reg_bytes))
-		ret = map->bus->write(map->dev, map->work_buf,
+		ret = map->bus->write(map->bus_context, map->work_buf,
 				      map->format.reg_bytes +
 				      map->format.pad_bytes +
 				      val_len);
 	else if (map->bus->gather_write)
-		ret = map->bus->gather_write(map->dev, map->work_buf,
+		ret = map->bus->gather_write(map->bus_context, map->work_buf,
 					     map->format.reg_bytes +
 					     map->format.pad_bytes,
 					     val, val_len);
@@ -464,7 +594,7 @@ static int _regmap_raw_write(struct regmap *map, unsigned int reg,
 		memcpy(buf, map->work_buf, map->format.reg_bytes);
 		memcpy(buf + map->format.reg_bytes + map->format.pad_bytes,
 		       val, val_len);
-		ret = map->bus->write(map->dev, buf, len);
+		ret = map->bus->write(map->bus_context, buf, len);
 
 		kfree(buf);
 	}
@@ -498,7 +628,7 @@ int _regmap_write(struct regmap *map, unsigned int reg,
 
 		trace_regmap_hw_write_start(map->dev, reg, 1);
 
-		ret = map->bus->write(map->dev, map->work_buf,
+		ret = map->bus->write(map->bus_context, map->work_buf,
 				      map->format.buf_size);
 
 		trace_regmap_hw_write_done(map->dev, reg, 1);
@@ -506,7 +636,7 @@ int _regmap_write(struct regmap *map, unsigned int reg,
 		return ret;
 	} else {
 		map->format.format_val(map->work_buf + map->format.reg_bytes
-				       + map->format.pad_bytes, val);
+				       + map->format.pad_bytes, val, 0);
 		return _regmap_raw_write(map, reg,
 					 map->work_buf +
 					 map->format.reg_bytes +
@@ -529,11 +659,14 @@ int regmap_write(struct regmap *map, unsigned int reg, unsigned int val)
 {
 	int ret;
 
-	mutex_lock(&map->lock);
+	if (reg % map->reg_stride)
+		return -EINVAL;
+
+	map->lock(map);
 
 	ret = _regmap_write(map, reg, val);
 
-	mutex_unlock(&map->lock);
+	map->unlock(map);
 
 	return ret;
 }
@@ -560,11 +693,16 @@ int regmap_raw_write(struct regmap *map, unsigned int reg,
 {
 	int ret;
 
-	mutex_lock(&map->lock);
+	if (val_len % map->format.val_bytes)
+		return -EINVAL;
+	if (reg % map->reg_stride)
+		return -EINVAL;
+
+	map->lock(map);
 
 	ret = _regmap_raw_write(map, reg, val, val_len);
 
-	mutex_unlock(&map->lock);
+	map->unlock(map);
 
 	return ret;
 }
@@ -593,8 +731,10 @@ int regmap_bulk_write(struct regmap *map, unsigned int reg, const void *val,
 
 	if (!map->format.parse_val)
 		return -EINVAL;
+	if (reg % map->reg_stride)
+		return -EINVAL;
 
-	mutex_lock(&map->lock);
+	map->lock(map);
 
 	/* No formatting is require if val_byte is 1 */
 	if (val_bytes == 1) {
@@ -609,13 +749,28 @@ int regmap_bulk_write(struct regmap *map, unsigned int reg, const void *val,
 		for (i = 0; i < val_count * val_bytes; i += val_bytes)
 			map->format.parse_val(wval + i);
 	}
-	ret = _regmap_raw_write(map, reg, wval, val_bytes * val_count);
+	/*
+	 * Some devices does not support bulk write, for
+	 * them we have a series of single write operations.
+	 */
+	if (map->use_single_rw) {
+		for (i = 0; i < val_count; i++) {
+			ret = regmap_raw_write(map,
+						reg + (i * map->reg_stride),
+						val + (i * val_bytes),
+						val_bytes);
+			if (ret != 0)
+				return ret;
+		}
+	} else {
+		ret = _regmap_raw_write(map, reg, wval, val_bytes * val_count);
+	}
 
 	if (val_bytes != 1)
 		kfree(wval);
 
 out:
-	mutex_unlock(&map->lock);
+	map->unlock(map);
 	return ret;
 }
 EXPORT_SYMBOL_GPL(regmap_bulk_write);
@@ -626,7 +781,7 @@ static int _regmap_raw_read(struct regmap *map, unsigned int reg, void *val,
 	u8 *u8 = map->work_buf;
 	int ret;
 
-	map->format.format_reg(map->work_buf, reg);
+	map->format.format_reg(map->work_buf, reg, map->reg_shift);
 
 	/*
 	 * Some buses or devices flag reads by setting the high bits in the
@@ -639,7 +794,7 @@ static int _regmap_raw_read(struct regmap *map, unsigned int reg, void *val,
 	trace_regmap_hw_read_start(map->dev, reg,
 				   val_len / map->format.val_bytes);
 
-	ret = map->bus->read(map->dev, map->work_buf,
+	ret = map->bus->read(map->bus_context, map->work_buf,
 			     map->format.reg_bytes + map->format.pad_bytes,
 			     val, val_len);
 
@@ -672,6 +827,9 @@ static int _regmap_read(struct regmap *map, unsigned int reg,
 		trace_regmap_reg_read(map->dev, reg, *val);
 	}
 
+	if (ret == 0 && !map->cache_bypass)
+		regcache_write(map, reg, *val);
+
 	return ret;
 }
 
@@ -689,11 +847,14 @@ int regmap_read(struct regmap *map, unsigned int reg, unsigned int *val)
 {
 	int ret;
 
-	mutex_lock(&map->lock);
+	if (reg % map->reg_stride)
+		return -EINVAL;
+
+	map->lock(map);
 
 	ret = _regmap_read(map, reg, val);
 
-	mutex_unlock(&map->lock);
+	map->unlock(map);
 
 	return ret;
 }
@@ -718,7 +879,12 @@ int regmap_raw_read(struct regmap *map, unsigned int reg, void *val,
 	unsigned int v;
 	int ret, i;
 
-	mutex_lock(&map->lock);
+	if (val_len % map->format.val_bytes)
+		return -EINVAL;
+	if (reg % map->reg_stride)
+		return -EINVAL;
+
+	map->lock(map);
 
 	if (regmap_volatile_range(map, reg, val_count) || map->cache_bypass ||
 	    map->cache_type == REGCACHE_NONE) {
@@ -730,16 +896,17 @@ int regmap_raw_read(struct regmap *map, unsigned int reg, void *val,
 		 * cost as we expect to hit the cache.
 		 */
 		for (i = 0; i < val_count; i++) {
-			ret = _regmap_read(map, reg + i, &v);
+			ret = _regmap_read(map, reg + (i * map->reg_stride),
+					   &v);
 			if (ret != 0)
 				goto out;
 
-			map->format.format_val(val + (i * val_bytes), v);
+			map->format.format_val(val + (i * val_bytes), v, 0);
 		}
 	}
 
  out:
-	mutex_unlock(&map->lock);
+	map->unlock(map);
 
 	return ret;
 }
@@ -765,19 +932,40 @@ int regmap_bulk_read(struct regmap *map, unsigned int reg, void *val,
 
 	if (!map->format.parse_val)
 		return -EINVAL;
+	if (reg % map->reg_stride)
+		return -EINVAL;
 
 	if (vol || map->cache_type == REGCACHE_NONE) {
-		ret = regmap_raw_read(map, reg, val, val_bytes * val_count);
-		if (ret != 0)
-			return ret;
+		/*
+		 * Some devices does not support bulk read, for
+		 * them we have a series of single read operations.
+		 */
+		if (map->use_single_rw) {
+			for (i = 0; i < val_count; i++) {
+				ret = regmap_raw_read(map,
+						reg + (i * map->reg_stride),
+						val + (i * val_bytes),
+						val_bytes);
+				if (ret != 0)
+					return ret;
+			}
+		} else {
+			ret = regmap_raw_read(map, reg, val,
+					      val_bytes * val_count);
+			if (ret != 0)
+				return ret;
+		}
 
 		for (i = 0; i < val_count * val_bytes; i += val_bytes)
 			map->format.parse_val(val + i);
 	} else {
 		for (i = 0; i < val_count; i++) {
-			ret = regmap_read(map, reg + i, val + (i * val_bytes));
+			unsigned int ival;
+			ret = regmap_read(map, reg + (i * map->reg_stride),
+					  &ival);
 			if (ret != 0)
 				return ret;
+			memcpy(val + (i * val_bytes), &ival, val_bytes);
 		}
 	}
 
@@ -792,7 +980,7 @@ static int _regmap_update_bits(struct regmap *map, unsigned int reg,
 	int ret;
 	unsigned int tmp, orig;
 
-	mutex_lock(&map->lock);
+	map->lock(map);
 
 	ret = _regmap_read(map, reg, &orig);
 	if (ret != 0)
@@ -809,7 +997,7 @@ static int _regmap_update_bits(struct regmap *map, unsigned int reg,
 	}
 
 out:
-	mutex_unlock(&map->lock);
+	map->unlock(map);
 
 	return ret;
 }
@@ -876,7 +1064,7 @@ int regmap_register_patch(struct regmap *map, const struct reg_default *regs,
 	if (map->patch)
 		return -EBUSY;
 
-	mutex_lock(&map->lock);
+	map->lock(map);
 
 	bypass = map->cache_bypass;
 
@@ -904,7 +1092,7 @@ int regmap_register_patch(struct regmap *map, const struct reg_default *regs,
 out:
 	map->cache_bypass = bypass;
 
-	mutex_unlock(&map->lock);
+	map->unlock(map);
 
 	return ret;
 }
diff --git a/drivers/bcma/core.c b/drivers/bcma/core.c
index 893f6e0c759..bc6e89212ad 100644
--- a/drivers/bcma/core.c
+++ b/drivers/bcma/core.c
@@ -30,6 +30,7 @@ void bcma_core_disable(struct bcma_device *core, u32 flags)
 	udelay(10);
 
 	bcma_awrite32(core, BCMA_RESET_CTL, BCMA_RESET_CTL_RESET);
+	bcma_aread32(core, BCMA_RESET_CTL);
 	udelay(1);
 }
 EXPORT_SYMBOL_GPL(bcma_core_disable);
@@ -77,7 +78,7 @@ void bcma_core_set_clockmode(struct bcma_device *core,
 			pr_err("HT force timeout\n");
 		break;
 	case BCMA_CLKMODE_DYNAMIC:
-		pr_warn("Dynamic clockmode not supported yet!\n");
+		bcma_set32(core, BCMA_CLKCTLST, ~BCMA_CLKCTLST_FORCEHT);
 		break;
 	}
 }
diff --git a/drivers/bcma/driver_pci.c b/drivers/bcma/driver_pci.c
index 4d38ae179b4..9a96f14c8f4 100644
--- a/drivers/bcma/driver_pci.c
+++ b/drivers/bcma/driver_pci.c
@@ -24,14 +24,12 @@ u32 bcma_pcie_read(struct bcma_drv_pci *pc, u32 address)
 	return pcicore_read32(pc, BCMA_CORE_PCI_PCIEIND_DATA);
 }
 
-#if 0
 static void bcma_pcie_write(struct bcma_drv_pci *pc, u32 address, u32 data)
 {
 	pcicore_write32(pc, BCMA_CORE_PCI_PCIEIND_ADDR, address);
 	pcicore_read32(pc, BCMA_CORE_PCI_PCIEIND_ADDR);
 	pcicore_write32(pc, BCMA_CORE_PCI_PCIEIND_DATA, data);
 }
-#endif
 
 static void bcma_pcie_mdio_set_phy(struct bcma_drv_pci *pc, u8 phy)
 {
@@ -170,13 +168,50 @@ static void bcma_pcicore_serdes_workaround(struct bcma_drv_pci *pc)
 		                     tmp & ~BCMA_CORE_PCI_PLL_CTRL_FREQDET_EN);
 }
 
+static void bcma_core_pci_fixcfg(struct bcma_drv_pci *pc)
+{
+	struct bcma_device *core = pc->core;
+	u16 val16, core_index;
+	uint regoff;
+
+	regoff = BCMA_CORE_PCI_SPROM(BCMA_CORE_PCI_SPROM_PI_OFFSET);
+	core_index = (u16)core->core_index;
+
+	val16 = pcicore_read16(pc, regoff);
+	if (((val16 & BCMA_CORE_PCI_SPROM_PI_MASK) >> BCMA_CORE_PCI_SPROM_PI_SHIFT)
+	     != core_index) {
+		val16 = (core_index << BCMA_CORE_PCI_SPROM_PI_SHIFT) |
+			(val16 & ~BCMA_CORE_PCI_SPROM_PI_MASK);
+		pcicore_write16(pc, regoff, val16);
+	}
+}
+
+/* Fix MISC config to allow coming out of L2/L3-Ready state w/o PRST */
+/* Needs to happen when coming out of 'standby'/'hibernate' */
+static void bcma_core_pci_config_fixup(struct bcma_drv_pci *pc)
+{
+	u16 val16;
+	uint regoff;
+
+	regoff = BCMA_CORE_PCI_SPROM(BCMA_CORE_PCI_SPROM_MISC_CONFIG);
+
+	val16 = pcicore_read16(pc, regoff);
+
+	if (!(val16 & BCMA_CORE_PCI_SPROM_L23READY_EXIT_NOPERST)) {
+		val16 |= BCMA_CORE_PCI_SPROM_L23READY_EXIT_NOPERST;
+		pcicore_write16(pc, regoff, val16);
+	}
+}
+
 /**************************************************
  * Init.
  **************************************************/
 
 static void __devinit bcma_core_pci_clientmode_init(struct bcma_drv_pci *pc)
 {
+	bcma_core_pci_fixcfg(pc);
 	bcma_pcicore_serdes_workaround(pc);
+	bcma_core_pci_config_fixup(pc);
 }
 
 void __devinit bcma_core_pci_init(struct bcma_drv_pci *pc)
@@ -224,3 +259,17 @@ out:
 	return err;
 }
 EXPORT_SYMBOL_GPL(bcma_core_pci_irq_ctl);
+
+void bcma_core_pci_extend_L1timer(struct bcma_drv_pci *pc, bool extend)
+{
+	u32 w;
+
+	w = bcma_pcie_read(pc, BCMA_CORE_PCI_DLLP_PMTHRESHREG);
+	if (extend)
+		w |= BCMA_CORE_PCI_ASPMTIMER_EXTEND;
+	else
+		w &= ~BCMA_CORE_PCI_ASPMTIMER_EXTEND;
+	bcma_pcie_write(pc, BCMA_CORE_PCI_DLLP_PMTHRESHREG, w);
+	bcma_pcie_read(pc, BCMA_CORE_PCI_DLLP_PMTHRESHREG);
+}
+EXPORT_SYMBOL_GPL(bcma_core_pci_extend_L1timer);
diff --git a/drivers/bcma/driver_pci_host.c b/drivers/bcma/driver_pci_host.c
index d2097a11c3c..b9a86edfec3 100644
--- a/drivers/bcma/driver_pci_host.c
+++ b/drivers/bcma/driver_pci_host.c
@@ -119,7 +119,7 @@ static int bcma_extpci_read_config(struct bcma_drv_pci *pc, unsigned int dev,
 		if (unlikely(!addr))
 			goto out;
 		err = -ENOMEM;
-		mmio = ioremap_nocache(addr, len);
+		mmio = ioremap_nocache(addr, sizeof(val));
 		if (!mmio)
 			goto out;
 
@@ -171,7 +171,7 @@ static int bcma_extpci_write_config(struct bcma_drv_pci *pc, unsigned int dev,
 			addr = pc->core->addr + BCMA_CORE_PCI_PCICFG0;
 			addr |= (func << 8);
 			addr |= (off & 0xfc);
-			mmio = ioremap_nocache(addr, len);
+			mmio = ioremap_nocache(addr, sizeof(val));
 			if (!mmio)
 				goto out;
 		}
@@ -180,7 +180,7 @@ static int bcma_extpci_write_config(struct bcma_drv_pci *pc, unsigned int dev,
 		if (unlikely(!addr))
 			goto out;
 		err = -ENOMEM;
-		mmio = ioremap_nocache(addr, len);
+		mmio = ioremap_nocache(addr, sizeof(val));
 		if (!mmio)
 			goto out;
 
@@ -491,8 +491,8 @@ void __devinit bcma_core_pci_hostmode_init(struct bcma_drv_pci *pc)
 	/* Ok, ready to run, register it to the system.
 	 * The following needs change, if we want to port hostmode
 	 * to non-MIPS platform. */
-	io_map_base = (unsigned long)ioremap_nocache(BCMA_SOC_PCI_MEM,
-						     0x04000000);
+	io_map_base = (unsigned long)ioremap_nocache(pc_host->mem_resource.start,
+						     resource_size(&pc_host->mem_resource));
 	pc_host->pci_controller.io_map_base = io_map_base;
 	set_io_port_base(pc_host->pci_controller.io_map_base);
 	/* Give some time to the PCI controller to configure itself with the new
diff --git a/drivers/bcma/host_pci.c b/drivers/bcma/host_pci.c
index e3928d68802..6c05cf470f9 100644
--- a/drivers/bcma/host_pci.c
+++ b/drivers/bcma/host_pci.c
@@ -201,6 +201,9 @@ static int __devinit bcma_host_pci_probe(struct pci_dev *dev,
 	bus->hosttype = BCMA_HOSTTYPE_PCI;
 	bus->ops = &bcma_host_pci_ops;
 
+	bus->boardinfo.vendor = bus->host_pci->subsystem_vendor;
+	bus->boardinfo.type = bus->host_pci->subsystem_device;
+
 	/* Register */
 	err = bcma_bus_register(bus);
 	if (err)
@@ -222,7 +225,7 @@ err_kfree_bus:
 	return err;
 }
 
-static void bcma_host_pci_remove(struct pci_dev *dev)
+static void __devexit bcma_host_pci_remove(struct pci_dev *dev)
 {
 	struct bcma_bus *bus = pci_get_drvdata(dev);
 
@@ -277,7 +280,7 @@ static struct pci_driver bcma_pci_bridge_driver = {
 	.name = "bcma-pci-bridge",
 	.id_table = bcma_pci_bridge_tbl,
 	.probe = bcma_host_pci_probe,
-	.remove = bcma_host_pci_remove,
+	.remove = __devexit_p(bcma_host_pci_remove),
 	.driver.pm = BCMA_PM_OPS,
 };
 
diff --git a/drivers/bcma/scan.c b/drivers/bcma/scan.c
index f94cccccfa5..5ed0718fc66 100644
--- a/drivers/bcma/scan.c
+++ b/drivers/bcma/scan.c
@@ -19,7 +19,14 @@ struct bcma_device_id_name {
 	u16 id;
 	const char *name;
 };
-struct bcma_device_id_name bcma_device_names[] = {
+
+static const struct bcma_device_id_name bcma_arm_device_names[] = {
+	{ BCMA_CORE_ARM_1176, "ARM 1176" },
+	{ BCMA_CORE_ARM_7TDMI, "ARM 7TDMI" },
+	{ BCMA_CORE_ARM_CM3, "ARM CM3" },
+};
+
+static const struct bcma_device_id_name bcma_bcm_device_names[] = {
 	{ BCMA_CORE_OOB_ROUTER, "OOB Router" },
 	{ BCMA_CORE_INVALID, "Invalid" },
 	{ BCMA_CORE_CHIPCOMMON, "ChipCommon" },
@@ -27,7 +34,6 @@ struct bcma_device_id_name bcma_device_names[] = {
 	{ BCMA_CORE_SRAM, "SRAM" },
 	{ BCMA_CORE_SDRAM, "SDRAM" },
 	{ BCMA_CORE_PCI, "PCI" },
-	{ BCMA_CORE_MIPS, "MIPS" },
 	{ BCMA_CORE_ETHERNET, "Fast Ethernet" },
 	{ BCMA_CORE_V90, "V90" },
 	{ BCMA_CORE_USB11_HOSTDEV, "USB 1.1 Hostdev" },
@@ -44,7 +50,6 @@ struct bcma_device_id_name bcma_device_names[] = {
 	{ BCMA_CORE_PHY_A, "PHY A" },
 	{ BCMA_CORE_PHY_B, "PHY B" },
 	{ BCMA_CORE_PHY_G, "PHY G" },
-	{ BCMA_CORE_MIPS_3302, "MIPS 3302" },
 	{ BCMA_CORE_USB11_HOST, "USB 1.1 Host" },
 	{ BCMA_CORE_USB11_DEV, "USB 1.1 Device" },
 	{ BCMA_CORE_USB20_HOST, "USB 2.0 Host" },
@@ -58,15 +63,11 @@ struct bcma_device_id_name bcma_device_names[] = {
 	{ BCMA_CORE_PHY_N, "PHY N" },
 	{ BCMA_CORE_SRAM_CTL, "SRAM Controller" },
 	{ BCMA_CORE_MINI_MACPHY, "Mini MACPHY" },
-	{ BCMA_CORE_ARM_1176, "ARM 1176" },
-	{ BCMA_CORE_ARM_7TDMI, "ARM 7TDMI" },
 	{ BCMA_CORE_PHY_LP, "PHY LP" },
 	{ BCMA_CORE_PMU, "PMU" },
 	{ BCMA_CORE_PHY_SSN, "PHY SSN" },
 	{ BCMA_CORE_SDIO_DEV, "SDIO Device" },
-	{ BCMA_CORE_ARM_CM3, "ARM CM3" },
 	{ BCMA_CORE_PHY_HT, "PHY HT" },
-	{ BCMA_CORE_MIPS_74K, "MIPS 74K" },
 	{ BCMA_CORE_MAC_GBIT, "GBit MAC" },
 	{ BCMA_CORE_DDR12_MEM_CTL, "DDR1/DDR2 Memory Controller" },
 	{ BCMA_CORE_PCIE_RC, "PCIe Root Complex" },
@@ -79,16 +80,41 @@ struct bcma_device_id_name bcma_device_names[] = {
 	{ BCMA_CORE_SHIM, "SHIM" },
 	{ BCMA_CORE_DEFAULT, "Default" },
 };
-const char *bcma_device_name(struct bcma_device_id *id)
+
+static const struct bcma_device_id_name bcma_mips_device_names[] = {
+	{ BCMA_CORE_MIPS, "MIPS" },
+	{ BCMA_CORE_MIPS_3302, "MIPS 3302" },
+	{ BCMA_CORE_MIPS_74K, "MIPS 74K" },
+};
+
+static const char *bcma_device_name(const struct bcma_device_id *id)
 {
-	int i;
+	const struct bcma_device_id_name *names;
+	int size, i;
+
+	/* search manufacturer specific names */
+	switch (id->manuf) {
+	case BCMA_MANUF_ARM:
+		names = bcma_arm_device_names;
+		size = ARRAY_SIZE(bcma_arm_device_names);
+		break;
+	case BCMA_MANUF_BCM:
+		names = bcma_bcm_device_names;
+		size = ARRAY_SIZE(bcma_bcm_device_names);
+		break;
+	case BCMA_MANUF_MIPS:
+		names = bcma_mips_device_names;
+		size = ARRAY_SIZE(bcma_mips_device_names);
+		break;
+	default:
+		return "UNKNOWN";
+	}
 
-	if (id->manuf == BCMA_MANUF_BCM) {
-		for (i = 0; i < ARRAY_SIZE(bcma_device_names); i++) {
-			if (bcma_device_names[i].id == id->id)
-				return bcma_device_names[i].name;
-		}
+	for (i = 0; i < size; i++) {
+		if (names[i].id == id->id)
+			return names[i].name;
 	}
+
 	return "UNKNOWN";
 }
 
@@ -297,6 +323,23 @@ static int bcma_get_next_core(struct bcma_bus *bus, u32 __iomem **eromptr,
 			return -EILSEQ;
 	}
 
+	/* First Slave Address Descriptor should be port 0:
+	 * the main register space for the core
+	 */
+	tmp = bcma_erom_get_addr_desc(bus, eromptr, SCAN_ADDR_TYPE_SLAVE, 0);
+	if (tmp <= 0) {
+		/* Try again to see if it is a bridge */
+		tmp = bcma_erom_get_addr_desc(bus, eromptr,
+					      SCAN_ADDR_TYPE_BRIDGE, 0);
+		if (tmp <= 0) {
+			return -EILSEQ;
+		} else {
+			pr_info("Bridge found\n");
+			return -ENXIO;
+		}
+	}
+	core->addr = tmp;
+
 	/* get & parse slave ports */
 	for (i = 0; i < ports[1]; i++) {
 		for (j = 0; ; j++) {
@@ -309,7 +352,7 @@ static int bcma_get_next_core(struct bcma_bus *bus, u32 __iomem **eromptr,
 				break;
 			} else {
 				if (i == 0 && j == 0)
-					core->addr = tmp;
+					core->addr1 = tmp;
 			}
 		}
 	}
diff --git a/drivers/bcma/sprom.c b/drivers/bcma/sprom.c
index 3e2a6002aae..c7f93359acb 100644
--- a/drivers/bcma/sprom.c
+++ b/drivers/bcma/sprom.c
@@ -181,6 +181,22 @@ static int bcma_sprom_valid(const u16 *sprom)
 #define SPEX(_field, _offset, _mask, _shift)	\
 	bus->sprom._field = ((sprom[SPOFF(_offset)] & (_mask)) >> (_shift))
 
+#define SPEX32(_field, _offset, _mask, _shift)	\
+	bus->sprom._field = ((((u32)sprom[SPOFF((_offset)+2)] << 16 | \
+				sprom[SPOFF(_offset)]) & (_mask)) >> (_shift))
+
+#define SPEX_ARRAY8(_field, _offset, _mask, _shift)	\
+	do {	\
+		SPEX(_field[0], _offset +  0, _mask, _shift);	\
+		SPEX(_field[1], _offset +  2, _mask, _shift);	\
+		SPEX(_field[2], _offset +  4, _mask, _shift);	\
+		SPEX(_field[3], _offset +  6, _mask, _shift);	\
+		SPEX(_field[4], _offset +  8, _mask, _shift);	\
+		SPEX(_field[5], _offset + 10, _mask, _shift);	\
+		SPEX(_field[6], _offset + 12, _mask, _shift);	\
+		SPEX(_field[7], _offset + 14, _mask, _shift);	\
+	} while (0)
+
 static void bcma_sprom_extract_r8(struct bcma_bus *bus, const u16 *sprom)
 {
 	u16 v, o;
@@ -243,7 +259,8 @@ static void bcma_sprom_extract_r8(struct bcma_bus *bus, const u16 *sprom)
 	SPEX(boardflags2_lo, SSB_SPROM8_BFL2LO, ~0, 0);
 	SPEX(boardflags2_hi, SSB_SPROM8_BFL2HI, ~0, 0);
 
-	SPEX(country_code, SSB_SPROM8_CCODE, ~0, 0);
+	SPEX(alpha2[0], SSB_SPROM8_CCODE, 0xff00, 8);
+	SPEX(alpha2[1], SSB_SPROM8_CCODE, 0x00ff, 0);
 
 	/* Extract cores power info info */
 	for (i = 0; i < ARRAY_SIZE(pwr_info_offset); i++) {
@@ -298,6 +315,136 @@ static void bcma_sprom_extract_r8(struct bcma_bus *bus, const u16 *sprom)
 	     SSB_SROM8_FEM_TR_ISO_SHIFT);
 	SPEX(fem.ghz5.antswlut, SSB_SPROM8_FEM5G, SSB_SROM8_FEM_ANTSWLUT,
 	     SSB_SROM8_FEM_ANTSWLUT_SHIFT);
+
+	SPEX(ant_available_a, SSB_SPROM8_ANTAVAIL, SSB_SPROM8_ANTAVAIL_A,
+	     SSB_SPROM8_ANTAVAIL_A_SHIFT);
+	SPEX(ant_available_bg, SSB_SPROM8_ANTAVAIL, SSB_SPROM8_ANTAVAIL_BG,
+	     SSB_SPROM8_ANTAVAIL_BG_SHIFT);
+	SPEX(maxpwr_bg, SSB_SPROM8_MAXP_BG, SSB_SPROM8_MAXP_BG_MASK, 0);
+	SPEX(itssi_bg, SSB_SPROM8_MAXP_BG, SSB_SPROM8_ITSSI_BG,
+	     SSB_SPROM8_ITSSI_BG_SHIFT);
+	SPEX(maxpwr_a, SSB_SPROM8_MAXP_A, SSB_SPROM8_MAXP_A_MASK, 0);
+	SPEX(itssi_a, SSB_SPROM8_MAXP_A, SSB_SPROM8_ITSSI_A,
+	     SSB_SPROM8_ITSSI_A_SHIFT);
+	SPEX(maxpwr_ah, SSB_SPROM8_MAXP_AHL, SSB_SPROM8_MAXP_AH_MASK, 0);
+	SPEX(maxpwr_al, SSB_SPROM8_MAXP_AHL, SSB_SPROM8_MAXP_AL_MASK,
+	     SSB_SPROM8_MAXP_AL_SHIFT);
+	SPEX(gpio0, SSB_SPROM8_GPIOA, SSB_SPROM8_GPIOA_P0, 0);
+	SPEX(gpio1, SSB_SPROM8_GPIOA, SSB_SPROM8_GPIOA_P1,
+	     SSB_SPROM8_GPIOA_P1_SHIFT);
+	SPEX(gpio2, SSB_SPROM8_GPIOB, SSB_SPROM8_GPIOB_P2, 0);
+	SPEX(gpio3, SSB_SPROM8_GPIOB, SSB_SPROM8_GPIOB_P3,
+	     SSB_SPROM8_GPIOB_P3_SHIFT);
+	SPEX(tri2g, SSB_SPROM8_TRI25G, SSB_SPROM8_TRI2G, 0);
+	SPEX(tri5g, SSB_SPROM8_TRI25G, SSB_SPROM8_TRI5G,
+	     SSB_SPROM8_TRI5G_SHIFT);
+	SPEX(tri5gl, SSB_SPROM8_TRI5GHL, SSB_SPROM8_TRI5GL, 0);
+	SPEX(tri5gh, SSB_SPROM8_TRI5GHL, SSB_SPROM8_TRI5GH,
+	     SSB_SPROM8_TRI5GH_SHIFT);
+	SPEX(rxpo2g, SSB_SPROM8_RXPO, SSB_SPROM8_RXPO2G,
+	     SSB_SPROM8_RXPO2G_SHIFT);
+	SPEX(rxpo5g, SSB_SPROM8_RXPO, SSB_SPROM8_RXPO5G,
+	     SSB_SPROM8_RXPO5G_SHIFT);
+	SPEX(rssismf2g, SSB_SPROM8_RSSIPARM2G, SSB_SPROM8_RSSISMF2G, 0);
+	SPEX(rssismc2g, SSB_SPROM8_RSSIPARM2G, SSB_SPROM8_RSSISMC2G,
+	     SSB_SPROM8_RSSISMC2G_SHIFT);
+	SPEX(rssisav2g, SSB_SPROM8_RSSIPARM2G, SSB_SPROM8_RSSISAV2G,
+	     SSB_SPROM8_RSSISAV2G_SHIFT);
+	SPEX(bxa2g, SSB_SPROM8_RSSIPARM2G, SSB_SPROM8_BXA2G,
+	     SSB_SPROM8_BXA2G_SHIFT);
+	SPEX(rssismf5g, SSB_SPROM8_RSSIPARM5G, SSB_SPROM8_RSSISMF5G, 0);
+	SPEX(rssismc5g, SSB_SPROM8_RSSIPARM5G, SSB_SPROM8_RSSISMC5G,
+	     SSB_SPROM8_RSSISMC5G_SHIFT);
+	SPEX(rssisav5g, SSB_SPROM8_RSSIPARM5G, SSB_SPROM8_RSSISAV5G,
+	     SSB_SPROM8_RSSISAV5G_SHIFT);
+	SPEX(bxa5g, SSB_SPROM8_RSSIPARM5G, SSB_SPROM8_BXA5G,
+	     SSB_SPROM8_BXA5G_SHIFT);
+
+	SPEX(pa0b0, SSB_SPROM8_PA0B0, ~0, 0);
+	SPEX(pa0b1, SSB_SPROM8_PA0B1, ~0, 0);
+	SPEX(pa0b2, SSB_SPROM8_PA0B2, ~0, 0);
+	SPEX(pa1b0, SSB_SPROM8_PA1B0, ~0, 0);
+	SPEX(pa1b1, SSB_SPROM8_PA1B1, ~0, 0);
+	SPEX(pa1b2, SSB_SPROM8_PA1B2, ~0, 0);
+	SPEX(pa1lob0, SSB_SPROM8_PA1LOB0, ~0, 0);
+	SPEX(pa1lob1, SSB_SPROM8_PA1LOB1, ~0, 0);
+	SPEX(pa1lob2, SSB_SPROM8_PA1LOB2, ~0, 0);
+	SPEX(pa1hib0, SSB_SPROM8_PA1HIB0, ~0, 0);
+	SPEX(pa1hib1, SSB_SPROM8_PA1HIB1, ~0, 0);
+	SPEX(pa1hib2, SSB_SPROM8_PA1HIB2, ~0, 0);
+	SPEX(cck2gpo, SSB_SPROM8_CCK2GPO, ~0, 0);
+	SPEX32(ofdm2gpo, SSB_SPROM8_OFDM2GPO, ~0, 0);
+	SPEX32(ofdm5glpo, SSB_SPROM8_OFDM5GLPO, ~0, 0);
+	SPEX32(ofdm5gpo, SSB_SPROM8_OFDM5GPO, ~0, 0);
+	SPEX32(ofdm5ghpo, SSB_SPROM8_OFDM5GHPO, ~0, 0);
+
+	/* Extract the antenna gain values. */
+	SPEX(antenna_gain.a0, SSB_SPROM8_AGAIN01,
+	     SSB_SPROM8_AGAIN0, SSB_SPROM8_AGAIN0_SHIFT);
+	SPEX(antenna_gain.a1, SSB_SPROM8_AGAIN01,
+	     SSB_SPROM8_AGAIN1, SSB_SPROM8_AGAIN1_SHIFT);
+	SPEX(antenna_gain.a2, SSB_SPROM8_AGAIN23,
+	     SSB_SPROM8_AGAIN2, SSB_SPROM8_AGAIN2_SHIFT);
+	SPEX(antenna_gain.a3, SSB_SPROM8_AGAIN23,
+	     SSB_SPROM8_AGAIN3, SSB_SPROM8_AGAIN3_SHIFT);
+
+	SPEX(leddc_on_time, SSB_SPROM8_LEDDC, SSB_SPROM8_LEDDC_ON,
+	     SSB_SPROM8_LEDDC_ON_SHIFT);
+	SPEX(leddc_off_time, SSB_SPROM8_LEDDC, SSB_SPROM8_LEDDC_OFF,
+	     SSB_SPROM8_LEDDC_OFF_SHIFT);
+
+	SPEX(txchain, SSB_SPROM8_TXRXC, SSB_SPROM8_TXRXC_TXCHAIN,
+	     SSB_SPROM8_TXRXC_TXCHAIN_SHIFT);
+	SPEX(rxchain, SSB_SPROM8_TXRXC, SSB_SPROM8_TXRXC_RXCHAIN,
+	     SSB_SPROM8_TXRXC_RXCHAIN_SHIFT);
+	SPEX(antswitch, SSB_SPROM8_TXRXC, SSB_SPROM8_TXRXC_SWITCH,
+	     SSB_SPROM8_TXRXC_SWITCH_SHIFT);
+
+	SPEX(opo, SSB_SPROM8_OFDM2GPO, 0x00ff, 0);
+
+	SPEX_ARRAY8(mcs2gpo, SSB_SPROM8_2G_MCSPO, ~0, 0);
+	SPEX_ARRAY8(mcs5gpo, SSB_SPROM8_5G_MCSPO, ~0, 0);
+	SPEX_ARRAY8(mcs5glpo, SSB_SPROM8_5GL_MCSPO, ~0, 0);
+	SPEX_ARRAY8(mcs5ghpo, SSB_SPROM8_5GH_MCSPO, ~0, 0);
+
+	SPEX(rawtempsense, SSB_SPROM8_RAWTS, SSB_SPROM8_RAWTS_RAWTEMP,
+	     SSB_SPROM8_RAWTS_RAWTEMP_SHIFT);
+	SPEX(measpower, SSB_SPROM8_RAWTS, SSB_SPROM8_RAWTS_MEASPOWER,
+	     SSB_SPROM8_RAWTS_MEASPOWER_SHIFT);
+	SPEX(tempsense_slope, SSB_SPROM8_OPT_CORRX,
+	     SSB_SPROM8_OPT_CORRX_TEMP_SLOPE,
+	     SSB_SPROM8_OPT_CORRX_TEMP_SLOPE_SHIFT);
+	SPEX(tempcorrx, SSB_SPROM8_OPT_CORRX, SSB_SPROM8_OPT_CORRX_TEMPCORRX,
+	     SSB_SPROM8_OPT_CORRX_TEMPCORRX_SHIFT);
+	SPEX(tempsense_option, SSB_SPROM8_OPT_CORRX,
+	     SSB_SPROM8_OPT_CORRX_TEMP_OPTION,
+	     SSB_SPROM8_OPT_CORRX_TEMP_OPTION_SHIFT);
+	SPEX(freqoffset_corr, SSB_SPROM8_HWIQ_IQSWP,
+	     SSB_SPROM8_HWIQ_IQSWP_FREQ_CORR,
+	     SSB_SPROM8_HWIQ_IQSWP_FREQ_CORR_SHIFT);
+	SPEX(iqcal_swp_dis, SSB_SPROM8_HWIQ_IQSWP,
+	     SSB_SPROM8_HWIQ_IQSWP_IQCAL_SWP,
+	     SSB_SPROM8_HWIQ_IQSWP_IQCAL_SWP_SHIFT);
+	SPEX(hw_iqcal_en, SSB_SPROM8_HWIQ_IQSWP, SSB_SPROM8_HWIQ_IQSWP_HW_IQCAL,
+	     SSB_SPROM8_HWIQ_IQSWP_HW_IQCAL_SHIFT);
+
+	SPEX(bw40po, SSB_SPROM8_BW40PO, ~0, 0);
+	SPEX(cddpo, SSB_SPROM8_CDDPO, ~0, 0);
+	SPEX(stbcpo, SSB_SPROM8_STBCPO, ~0, 0);
+	SPEX(bwduppo, SSB_SPROM8_BWDUPPO, ~0, 0);
+
+	SPEX(tempthresh, SSB_SPROM8_THERMAL, SSB_SPROM8_THERMAL_TRESH,
+	     SSB_SPROM8_THERMAL_TRESH_SHIFT);
+	SPEX(tempoffset, SSB_SPROM8_THERMAL, SSB_SPROM8_THERMAL_OFFSET,
+	     SSB_SPROM8_THERMAL_OFFSET_SHIFT);
+	SPEX(phycal_tempdelta, SSB_SPROM8_TEMPDELTA,
+	     SSB_SPROM8_TEMPDELTA_PHYCAL,
+	     SSB_SPROM8_TEMPDELTA_PHYCAL_SHIFT);
+	SPEX(temps_period, SSB_SPROM8_TEMPDELTA, SSB_SPROM8_TEMPDELTA_PERIOD,
+	     SSB_SPROM8_TEMPDELTA_PERIOD_SHIFT);
+	SPEX(temps_hysteresis, SSB_SPROM8_TEMPDELTA,
+	     SSB_SPROM8_TEMPDELTA_HYSTERESIS,
+	     SSB_SPROM8_TEMPDELTA_HYSTERESIS_SHIFT);
 }
 
 /*
diff --git a/drivers/block/DAC960.c b/drivers/block/DAC960.c
index 8db9089127c..9a13e889837 100644
--- a/drivers/block/DAC960.c
+++ b/drivers/block/DAC960.c
@@ -6580,24 +6580,21 @@ static const struct file_operations dac960_user_command_proc_fops = {
 
 static void DAC960_CreateProcEntries(DAC960_Controller_T *Controller)
 {
-	struct proc_dir_entry *StatusProcEntry;
 	struct proc_dir_entry *ControllerProcEntry;
-	struct proc_dir_entry *UserCommandProcEntry;
 
 	if (DAC960_ProcDirectoryEntry == NULL) {
-  		DAC960_ProcDirectoryEntry = proc_mkdir("rd", NULL);
-  		StatusProcEntry = proc_create("status", 0,
-					   DAC960_ProcDirectoryEntry,
-					   &dac960_proc_fops);
+		DAC960_ProcDirectoryEntry = proc_mkdir("rd", NULL);
+		proc_create("status", 0, DAC960_ProcDirectoryEntry,
+			    &dac960_proc_fops);
 	}
 
-      sprintf(Controller->ControllerName, "c%d", Controller->ControllerNumber);
-      ControllerProcEntry = proc_mkdir(Controller->ControllerName,
-				       DAC960_ProcDirectoryEntry);
-      proc_create_data("initial_status", 0, ControllerProcEntry, &dac960_initial_status_proc_fops, Controller);
-      proc_create_data("current_status", 0, ControllerProcEntry, &dac960_current_status_proc_fops, Controller);
-      UserCommandProcEntry = proc_create_data("user_command", S_IWUSR | S_IRUSR, ControllerProcEntry, &dac960_user_command_proc_fops, Controller);
-      Controller->ControllerProcEntry = ControllerProcEntry;
+	sprintf(Controller->ControllerName, "c%d", Controller->ControllerNumber);
+	ControllerProcEntry = proc_mkdir(Controller->ControllerName,
+					 DAC960_ProcDirectoryEntry);
+	proc_create_data("initial_status", 0, ControllerProcEntry, &dac960_initial_status_proc_fops, Controller);
+	proc_create_data("current_status", 0, ControllerProcEntry, &dac960_current_status_proc_fops, Controller);
+	proc_create_data("user_command", S_IWUSR | S_IRUSR, ControllerProcEntry, &dac960_user_command_proc_fops, Controller);
+	Controller->ControllerProcEntry = ControllerProcEntry;
 }
 
 
diff --git a/drivers/block/drbd/drbd_nl.c b/drivers/block/drbd/drbd_nl.c
index abfaacaaf34..946166e1395 100644
--- a/drivers/block/drbd/drbd_nl.c
+++ b/drivers/block/drbd/drbd_nl.c
@@ -2297,7 +2297,7 @@ static void drbd_connector_callback(struct cn_msg *req, struct netlink_skb_parms
 		return;
 	}
 
-	if (!cap_raised(current_cap(), CAP_SYS_ADMIN)) {
+	if (!capable(CAP_SYS_ADMIN)) {
 		retcode = ERR_PERM;
 		goto fail;
 	}
diff --git a/drivers/block/drbd/drbd_receiver.c b/drivers/block/drbd/drbd_receiver.c
index 43beaca5317..436f519bed1 100644
--- a/drivers/block/drbd/drbd_receiver.c
+++ b/drivers/block/drbd/drbd_receiver.c
@@ -664,7 +664,7 @@ static struct socket *drbd_wait_for_connect(struct drbd_conf *mdev)
 	timeo = mdev->net_conf->try_connect_int * HZ;
 	timeo += (random32() & 1) ? timeo / 7 : -timeo / 7; /* 28.5% random jitter */
 
-	s_listen->sk->sk_reuse    = 1; /* SO_REUSEADDR */
+	s_listen->sk->sk_reuse    = SK_CAN_REUSE; /* SO_REUSEADDR */
 	s_listen->sk->sk_rcvtimeo = timeo;
 	s_listen->sk->sk_sndtimeo = timeo;
 	drbd_setbufsize(s_listen, mdev->net_conf->sndbuf_size,
@@ -841,8 +841,8 @@ retry:
 		}
 	} while (1);
 
-	msock->sk->sk_reuse = 1; /* SO_REUSEADDR */
-	sock->sk->sk_reuse = 1; /* SO_REUSEADDR */
+	msock->sk->sk_reuse = SK_CAN_REUSE; /* SO_REUSEADDR */
+	sock->sk->sk_reuse = SK_CAN_REUSE; /* SO_REUSEADDR */
 
 	sock->sk->sk_allocation = GFP_NOIO;
 	msock->sk->sk_allocation = GFP_NOIO;
diff --git a/drivers/block/mtip32xx/mtip32xx.c b/drivers/block/mtip32xx/mtip32xx.c
index 00f9fc99209..304000c3d43 100644
--- a/drivers/block/mtip32xx/mtip32xx.c
+++ b/drivers/block/mtip32xx/mtip32xx.c
@@ -2510,8 +2510,10 @@ static struct scatterlist *mtip_hw_get_scatterlist(struct driver_data *dd,
 		up(&dd->port->cmd_slot);
 		return NULL;
 	}
-	if (unlikely(*tag < 0))
+	if (unlikely(*tag < 0)) {
+		up(&dd->port->cmd_slot);
 		return NULL;
+	}
 
 	return dd->port->commands[*tag].sg;
 }
diff --git a/drivers/block/virtio_blk.c b/drivers/block/virtio_blk.c
index 0d39f2f4294..693187df760 100644
--- a/drivers/block/virtio_blk.c
+++ b/drivers/block/virtio_blk.c
@@ -29,9 +29,6 @@ struct virtio_blk
 	/* The disk structure for the kernel. */
 	struct gendisk *disk;
 
-	/* Request tracking. */
-	struct list_head reqs;
-
 	mempool_t *pool;
 
 	/* Process context for config space updates */
@@ -55,7 +52,6 @@ struct virtio_blk
 
 struct virtblk_req
 {
-	struct list_head list;
 	struct request *req;
 	struct virtio_blk_outhdr out_hdr;
 	struct virtio_scsi_inhdr in_hdr;
@@ -99,7 +95,6 @@ static void blk_done(struct virtqueue *vq)
 		}
 
 		__blk_end_request_all(vbr->req, error);
-		list_del(&vbr->list);
 		mempool_free(vbr, vblk->pool);
 	}
 	/* In case queue is stopped waiting for more buffers. */
@@ -184,7 +179,6 @@ static bool do_req(struct request_queue *q, struct virtio_blk *vblk,
 		return false;
 	}
 
-	list_add_tail(&vbr->list, &vblk->reqs);
 	return true;
 }
 
@@ -437,7 +431,6 @@ static int __devinit virtblk_probe(struct virtio_device *vdev)
 		goto out_free_index;
 	}
 
-	INIT_LIST_HEAD(&vblk->reqs);
 	spin_lock_init(&vblk->lock);
 	vblk->vdev = vdev;
 	vblk->sg_elems = sg_elems;
@@ -583,21 +576,29 @@ static void __devexit virtblk_remove(struct virtio_device *vdev)
 {
 	struct virtio_blk *vblk = vdev->priv;
 	int index = vblk->index;
+	struct virtblk_req *vbr;
+	unsigned long flags;
 
 	/* Prevent config work handler from accessing the device. */
 	mutex_lock(&vblk->config_lock);
 	vblk->config_enable = false;
 	mutex_unlock(&vblk->config_lock);
 
-	/* Nothing should be pending. */
-	BUG_ON(!list_empty(&vblk->reqs));
-
 	/* Stop all the virtqueues. */
 	vdev->config->reset(vdev);
 
 	flush_work(&vblk->config_work);
 
 	del_gendisk(vblk->disk);
+
+	/* Abort requests dispatched to driver. */
+	spin_lock_irqsave(&vblk->lock, flags);
+	while ((vbr = virtqueue_detach_unused_buf(vblk->vq))) {
+		__blk_end_request_all(vbr->req, -EIO);
+		mempool_free(vbr, vblk->pool);
+	}
+	spin_unlock_irqrestore(&vblk->lock, flags);
+
 	blk_cleanup_queue(vblk->disk->queue);
 	put_disk(vblk->disk);
 	mempool_destroy(vblk->pool);
diff --git a/drivers/bluetooth/ath3k.c b/drivers/bluetooth/ath3k.c
index 57fd867553d..ad591bd240e 100644
--- a/drivers/bluetooth/ath3k.c
+++ b/drivers/bluetooth/ath3k.c
@@ -81,6 +81,9 @@ static struct usb_device_id ath3k_table[] = {
 	/* Atheros AR5BBU12 with sflash firmware */
 	{ USB_DEVICE(0x0489, 0xE02C) },
 
+	/* Atheros AR5BBU22 with sflash firmware */
+	{ USB_DEVICE(0x0489, 0xE03C) },
+
 	{ }	/* Terminating entry */
 };
 
@@ -99,6 +102,9 @@ static struct usb_device_id ath3k_blist_tbl[] = {
 	{ USB_DEVICE(0x13d3, 0x3362), .driver_info = BTUSB_ATH3012 },
 	{ USB_DEVICE(0x0cf3, 0xe004), .driver_info = BTUSB_ATH3012 },
 
+	/* Atheros AR5BBU22 with sflash firmware */
+	{ USB_DEVICE(0x0489, 0xE03C), .driver_info = BTUSB_ATH3012 },
+
 	{ }	/* Terminating entry */
 };
 
@@ -439,6 +445,7 @@ static struct usb_driver ath3k_driver = {
 	.probe		= ath3k_probe,
 	.disconnect	= ath3k_disconnect,
 	.id_table	= ath3k_table,
+	.disable_hub_initiated_lpm = 1,
 };
 
 module_usb_driver(ath3k_driver);
diff --git a/drivers/bluetooth/bcm203x.c b/drivers/bluetooth/bcm203x.c
index 1e742a50e2c..37ae175162f 100644
--- a/drivers/bluetooth/bcm203x.c
+++ b/drivers/bluetooth/bcm203x.c
@@ -279,6 +279,7 @@ static struct usb_driver bcm203x_driver = {
 	.probe		= bcm203x_probe,
 	.disconnect	= bcm203x_disconnect,
 	.id_table	= bcm203x_table,
+	.disable_hub_initiated_lpm = 1,
 };
 
 module_usb_driver(bcm203x_driver);
diff --git a/drivers/bluetooth/bfusb.c b/drivers/bluetooth/bfusb.c
index b8ac1c549a1..32e825144fe 100644
--- a/drivers/bluetooth/bfusb.c
+++ b/drivers/bluetooth/bfusb.c
@@ -749,6 +749,7 @@ static struct usb_driver bfusb_driver = {
 	.probe		= bfusb_probe,
 	.disconnect	= bfusb_disconnect,
 	.id_table	= bfusb_table,
+	.disable_hub_initiated_lpm = 1,
 };
 
 module_usb_driver(bfusb_driver);
diff --git a/drivers/bluetooth/bpa10x.c b/drivers/bluetooth/bpa10x.c
index d894340a760..609861a53c2 100644
--- a/drivers/bluetooth/bpa10x.c
+++ b/drivers/bluetooth/bpa10x.c
@@ -508,6 +508,7 @@ static struct usb_driver bpa10x_driver = {
 	.probe		= bpa10x_probe,
 	.disconnect	= bpa10x_disconnect,
 	.id_table	= bpa10x_table,
+	.disable_hub_initiated_lpm = 1,
 };
 
 module_usb_driver(bpa10x_driver);
diff --git a/drivers/bluetooth/btmrvl_drv.h b/drivers/bluetooth/btmrvl_drv.h
index 90bda50dc44..94f2d65131c 100644
--- a/drivers/bluetooth/btmrvl_drv.h
+++ b/drivers/bluetooth/btmrvl_drv.h
@@ -67,6 +67,7 @@ struct btmrvl_adapter {
 	u8 wakeup_tries;
 	wait_queue_head_t cmd_wait_q;
 	u8 cmd_complete;
+	bool is_suspended;
 };
 
 struct btmrvl_private {
@@ -139,8 +140,10 @@ void btmrvl_check_evtpkt(struct btmrvl_private *priv, struct sk_buff *skb);
 int btmrvl_process_event(struct btmrvl_private *priv, struct sk_buff *skb);
 
 int btmrvl_send_module_cfg_cmd(struct btmrvl_private *priv, int subcmd);
+int btmrvl_send_hscfg_cmd(struct btmrvl_private *priv);
 int btmrvl_enable_ps(struct btmrvl_private *priv);
 int btmrvl_prepare_command(struct btmrvl_private *priv);
+int btmrvl_enable_hs(struct btmrvl_private *priv);
 
 #ifdef CONFIG_DEBUG_FS
 void btmrvl_debugfs_init(struct hci_dev *hdev);
diff --git a/drivers/bluetooth/btmrvl_main.c b/drivers/bluetooth/btmrvl_main.c
index d1209adc882..681ca9d18e1 100644
--- a/drivers/bluetooth/btmrvl_main.c
+++ b/drivers/bluetooth/btmrvl_main.c
@@ -200,6 +200,36 @@ int btmrvl_send_module_cfg_cmd(struct btmrvl_private *priv, int subcmd)
 }
 EXPORT_SYMBOL_GPL(btmrvl_send_module_cfg_cmd);
 
+int btmrvl_send_hscfg_cmd(struct btmrvl_private *priv)
+{
+	struct sk_buff *skb;
+	struct btmrvl_cmd *cmd;
+
+	skb = bt_skb_alloc(sizeof(*cmd), GFP_ATOMIC);
+	if (!skb) {
+		BT_ERR("No free skb");
+		return -ENOMEM;
+	}
+
+	cmd = (struct btmrvl_cmd *) skb_put(skb, sizeof(*cmd));
+	cmd->ocf_ogf = cpu_to_le16(hci_opcode_pack(OGF,
+						   BT_CMD_HOST_SLEEP_CONFIG));
+	cmd->length = 2;
+	cmd->data[0] = (priv->btmrvl_dev.gpio_gap & 0xff00) >> 8;
+	cmd->data[1] = (u8) (priv->btmrvl_dev.gpio_gap & 0x00ff);
+
+	bt_cb(skb)->pkt_type = MRVL_VENDOR_PKT;
+
+	skb->dev = (void *) priv->btmrvl_dev.hcidev;
+	skb_queue_head(&priv->adapter->tx_queue, skb);
+
+	BT_DBG("Queue HSCFG Command, gpio=0x%x, gap=0x%x", cmd->data[0],
+	       cmd->data[1]);
+
+	return 0;
+}
+EXPORT_SYMBOL_GPL(btmrvl_send_hscfg_cmd);
+
 int btmrvl_enable_ps(struct btmrvl_private *priv)
 {
 	struct sk_buff *skb;
@@ -232,7 +262,7 @@ int btmrvl_enable_ps(struct btmrvl_private *priv)
 }
 EXPORT_SYMBOL_GPL(btmrvl_enable_ps);
 
-static int btmrvl_enable_hs(struct btmrvl_private *priv)
+int btmrvl_enable_hs(struct btmrvl_private *priv)
 {
 	struct sk_buff *skb;
 	struct btmrvl_cmd *cmd;
@@ -268,35 +298,15 @@ static int btmrvl_enable_hs(struct btmrvl_private *priv)
 
 	return ret;
 }
+EXPORT_SYMBOL_GPL(btmrvl_enable_hs);
 
 int btmrvl_prepare_command(struct btmrvl_private *priv)
 {
-	struct sk_buff *skb = NULL;
-	struct btmrvl_cmd *cmd;
 	int ret = 0;
 
 	if (priv->btmrvl_dev.hscfgcmd) {
 		priv->btmrvl_dev.hscfgcmd = 0;
-
-		skb = bt_skb_alloc(sizeof(*cmd), GFP_ATOMIC);
-		if (skb == NULL) {
-			BT_ERR("No free skb");
-			return -ENOMEM;
-		}
-
-		cmd = (struct btmrvl_cmd *) skb_put(skb, sizeof(*cmd));
-		cmd->ocf_ogf = cpu_to_le16(hci_opcode_pack(OGF, BT_CMD_HOST_SLEEP_CONFIG));
-		cmd->length = 2;
-		cmd->data[0] = (priv->btmrvl_dev.gpio_gap & 0xff00) >> 8;
-		cmd->data[1] = (u8) (priv->btmrvl_dev.gpio_gap & 0x00ff);
-
-		bt_cb(skb)->pkt_type = MRVL_VENDOR_PKT;
-
-		skb->dev = (void *) priv->btmrvl_dev.hcidev;
-		skb_queue_head(&priv->adapter->tx_queue, skb);
-
-		BT_DBG("Queue HSCFG Command, gpio=0x%x, gap=0x%x",
-						cmd->data[0], cmd->data[1]);
+		btmrvl_send_hscfg_cmd(priv);
 	}
 
 	if (priv->btmrvl_dev.pscmd) {
diff --git a/drivers/bluetooth/btmrvl_sdio.c b/drivers/bluetooth/btmrvl_sdio.c
index 27b74b0d547..a853244e7fd 100644
--- a/drivers/bluetooth/btmrvl_sdio.c
+++ b/drivers/bluetooth/btmrvl_sdio.c
@@ -339,9 +339,7 @@ static int btmrvl_sdio_download_helper(struct btmrvl_sdio_card *card)
 
 done:
 	kfree(tmphlprbuf);
-	if (fw_helper)
-		release_firmware(fw_helper);
-
+	release_firmware(fw_helper);
 	return ret;
 }
 
@@ -484,10 +482,7 @@ static int btmrvl_sdio_download_fw_w_helper(struct btmrvl_sdio_card *card)
 
 done:
 	kfree(tmpfwbuf);
-
-	if (fw_firmware)
-		release_firmware(fw_firmware);
-
+	release_firmware(fw_firmware);
 	return ret;
 }
 
@@ -1013,6 +1008,9 @@ static int btmrvl_sdio_probe(struct sdio_func *func,
 	priv->btmrvl_dev.psmode = 1;
 	btmrvl_enable_ps(priv);
 
+	priv->btmrvl_dev.gpio_gap = 0xffff;
+	btmrvl_send_hscfg_cmd(priv);
+
 	return 0;
 
 disable_host_int:
@@ -1048,11 +1046,111 @@ static void btmrvl_sdio_remove(struct sdio_func *func)
 	}
 }
 
+static int btmrvl_sdio_suspend(struct device *dev)
+{
+	struct sdio_func *func = dev_to_sdio_func(dev);
+	struct btmrvl_sdio_card *card;
+	struct btmrvl_private *priv;
+	mmc_pm_flag_t pm_flags;
+	struct hci_dev *hcidev;
+
+	if (func) {
+		pm_flags = sdio_get_host_pm_caps(func);
+		BT_DBG("%s: suspend: PM flags = 0x%x", sdio_func_id(func),
+		       pm_flags);
+		if (!(pm_flags & MMC_PM_KEEP_POWER)) {
+			BT_ERR("%s: cannot remain alive while suspended",
+			       sdio_func_id(func));
+			return -ENOSYS;
+		}
+		card = sdio_get_drvdata(func);
+		if (!card || !card->priv) {
+			BT_ERR("card or priv structure is not valid");
+			return 0;
+		}
+	} else {
+		BT_ERR("sdio_func is not specified");
+		return 0;
+	}
+
+	priv = card->priv;
+
+	if (priv->adapter->hs_state != HS_ACTIVATED) {
+		if (btmrvl_enable_hs(priv)) {
+			BT_ERR("HS not actived, suspend failed!");
+			return -EBUSY;
+		}
+	}
+	hcidev = priv->btmrvl_dev.hcidev;
+	BT_DBG("%s: SDIO suspend", hcidev->name);
+	hci_suspend_dev(hcidev);
+	skb_queue_purge(&priv->adapter->tx_queue);
+
+	priv->adapter->is_suspended = true;
+
+	/* We will keep the power when hs enabled successfully */
+	if (priv->adapter->hs_state == HS_ACTIVATED) {
+		BT_DBG("suspend with MMC_PM_KEEP_POWER");
+		return sdio_set_host_pm_flags(func, MMC_PM_KEEP_POWER);
+	} else {
+		BT_DBG("suspend without MMC_PM_KEEP_POWER");
+		return 0;
+	}
+}
+
+static int btmrvl_sdio_resume(struct device *dev)
+{
+	struct sdio_func *func = dev_to_sdio_func(dev);
+	struct btmrvl_sdio_card *card;
+	struct btmrvl_private *priv;
+	mmc_pm_flag_t pm_flags;
+	struct hci_dev *hcidev;
+
+	if (func) {
+		pm_flags = sdio_get_host_pm_caps(func);
+		BT_DBG("%s: resume: PM flags = 0x%x", sdio_func_id(func),
+		       pm_flags);
+		card = sdio_get_drvdata(func);
+		if (!card || !card->priv) {
+			BT_ERR("card or priv structure is not valid");
+			return 0;
+		}
+	} else {
+		BT_ERR("sdio_func is not specified");
+		return 0;
+	}
+	priv = card->priv;
+
+	if (!priv->adapter->is_suspended) {
+		BT_DBG("device already resumed");
+		return 0;
+	}
+
+	priv->adapter->is_suspended = false;
+	hcidev = priv->btmrvl_dev.hcidev;
+	BT_DBG("%s: SDIO resume", hcidev->name);
+	hci_resume_dev(hcidev);
+	priv->hw_wakeup_firmware(priv);
+	priv->adapter->hs_state = HS_DEACTIVATED;
+	BT_DBG("%s: HS DEACTIVATED in resume!", hcidev->name);
+
+	return 0;
+}
+
+static const struct dev_pm_ops btmrvl_sdio_pm_ops = {
+	.suspend	= btmrvl_sdio_suspend,
+	.resume		= btmrvl_sdio_resume,
+};
+
 static struct sdio_driver bt_mrvl_sdio = {
 	.name		= "btmrvl_sdio",
 	.id_table	= btmrvl_sdio_ids,
 	.probe		= btmrvl_sdio_probe,
 	.remove		= btmrvl_sdio_remove,
+	.drv = {
+		.owner = THIS_MODULE,
+		.pm = &btmrvl_sdio_pm_ops,
+	}
 };
 
 static int __init btmrvl_sdio_init_module(void)
diff --git a/drivers/bluetooth/btusb.c b/drivers/bluetooth/btusb.c
index 9217121362e..c9463af8e56 100644
--- a/drivers/bluetooth/btusb.c
+++ b/drivers/bluetooth/btusb.c
@@ -143,6 +143,9 @@ static struct usb_device_id blacklist_table[] = {
 	/* Atheros AR5BBU12 with sflash firmware */
 	{ USB_DEVICE(0x0489, 0xe02c), .driver_info = BTUSB_IGNORE },
 
+	/* Atheros AR5BBU12 with sflash firmware */
+	{ USB_DEVICE(0x0489, 0xe03c), .driver_info = BTUSB_ATH3012 },
+
 	/* Broadcom BCM2035 */
 	{ USB_DEVICE(0x0a5c, 0x2035), .driver_info = BTUSB_WRONG_SCO_MTU },
 	{ USB_DEVICE(0x0a5c, 0x200a), .driver_info = BTUSB_WRONG_SCO_MTU },
@@ -855,6 +858,7 @@ static void btusb_work(struct work_struct *work)
 {
 	struct btusb_data *data = container_of(work, struct btusb_data, work);
 	struct hci_dev *hdev = data->hdev;
+	int new_alts;
 	int err;
 
 	if (hdev->conn_hash.sco_num > 0) {
@@ -868,11 +872,19 @@ static void btusb_work(struct work_struct *work)
 
 			set_bit(BTUSB_DID_ISO_RESUME, &data->flags);
 		}
-		if (data->isoc_altsetting != 2) {
+
+		if (hdev->voice_setting & 0x0020) {
+			static const int alts[3] = { 2, 4, 5 };
+			new_alts = alts[hdev->conn_hash.sco_num - 1];
+		} else {
+			new_alts = hdev->conn_hash.sco_num;
+		}
+
+		if (data->isoc_altsetting != new_alts) {
 			clear_bit(BTUSB_ISOC_RUNNING, &data->flags);
 			usb_kill_anchored_urbs(&data->isoc_anchor);
 
-			if (__set_isoc_interface(hdev, 2) < 0)
+			if (__set_isoc_interface(hdev, new_alts) < 0)
 				return;
 		}
 
@@ -1218,6 +1230,7 @@ static struct usb_driver btusb_driver = {
 #endif
 	.id_table	= btusb_table,
 	.supports_autosuspend = 1,
+	.disable_hub_initiated_lpm = 1,
 };
 
 module_usb_driver(btusb_driver);
diff --git a/drivers/bluetooth/hci_ldisc.c b/drivers/bluetooth/hci_ldisc.c
index 98a8c05d4f2..e564579a611 100644
--- a/drivers/bluetooth/hci_ldisc.c
+++ b/drivers/bluetooth/hci_ldisc.c
@@ -388,7 +388,7 @@ static int hci_uart_register_dev(struct hci_uart *hu)
 	hdev->close = hci_uart_close;
 	hdev->flush = hci_uart_flush;
 	hdev->send  = hci_uart_send_frame;
-	hdev->parent = hu->tty->dev;
+	SET_HCIDEV_DEV(hdev, hu->tty->dev);
 
 	if (test_bit(HCI_UART_RAW_DEVICE, &hu->hdev_flags))
 		set_bit(HCI_QUIRK_RAW_DEVICE, &hdev->quirks);
diff --git a/drivers/bluetooth/hci_vhci.c b/drivers/bluetooth/hci_vhci.c
index 158bfe507da..3f72595a601 100644
--- a/drivers/bluetooth/hci_vhci.c
+++ b/drivers/bluetooth/hci_vhci.c
@@ -252,8 +252,9 @@ static int vhci_open(struct inode *inode, struct file *file)
 	}
 
 	file->private_data = data;
+	nonseekable_open(inode, file);
 
-	return nonseekable_open(inode, file);
+	return 0;
 }
 
 static int vhci_release(struct inode *inode, struct file *file)
diff --git a/drivers/char/Kconfig b/drivers/char/Kconfig
index ee946865d6c..ea6f6325f9b 100644
--- a/drivers/char/Kconfig
+++ b/drivers/char/Kconfig
@@ -585,14 +585,6 @@ config DEVPORT
 
 source "drivers/s390/char/Kconfig"
 
-config RAMOOPS
-	tristate "Log panic/oops to a RAM buffer"
-	depends on HAS_IOMEM
-	default n
-	help
-	  This enables panic and oops messages to be logged to a circular
-	  buffer in RAM where it can be read back at some later point.
-
 config MSM_SMD_PKT
 	bool "Enable device interface for some SMD packet ports"
 	default n
diff --git a/drivers/char/Makefile b/drivers/char/Makefile
index 0dc5d7ce486..d0b27a39f1d 100644
--- a/drivers/char/Makefile
+++ b/drivers/char/Makefile
@@ -58,7 +58,6 @@ obj-$(CONFIG_HANGCHECK_TIMER)	+= hangcheck-timer.o
 obj-$(CONFIG_TCG_TPM)		+= tpm/
 
 obj-$(CONFIG_PS3_FLASH)		+= ps3flash.o
-obj-$(CONFIG_RAMOOPS)		+= ramoops.o
 
 obj-$(CONFIG_JS_RTC)		+= js-rtc.o
 js-rtc-y = rtc.o
diff --git a/drivers/char/agp/generic.c b/drivers/char/agp/generic.c
index 17e05d1076b..a0df182f6f7 100644
--- a/drivers/char/agp/generic.c
+++ b/drivers/char/agp/generic.c
@@ -958,7 +958,7 @@ int agp_generic_create_gatt_table(struct agp_bridge_data *bridge)
 	if (set_memory_uc((unsigned long)table, 1 << page_order))
 		printk(KERN_WARNING "Could not set GATT table memory to UC!\n");
 
-	bridge->gatt_table = (void *)table;
+	bridge->gatt_table = (u32 __iomem *)table;
 #else
 	bridge->gatt_table = ioremap_nocache(virt_to_phys(table),
 					(PAGE_SIZE * (1 << page_order)));
@@ -1010,7 +1010,6 @@ int agp_generic_free_gatt_table(struct agp_bridge_data *bridge)
 	case LVL2_APER_SIZE:
 		/* The generic routines can't deal with 2 level gatt's */
 		return -EINVAL;
-		break;
 	default:
 		page_order = 0;
 		break;
@@ -1077,7 +1076,6 @@ int agp_generic_insert_memory(struct agp_memory * mem, off_t pg_start, int type)
 	case LVL2_APER_SIZE:
 		/* The generic routines can't deal with 2 level gatt's */
 		return -EINVAL;
-		break;
 	default:
 		num_entries = 0;
 		break;
diff --git a/drivers/char/agp/intel-agp.c b/drivers/char/agp/intel-agp.c
index 962e75dc478..764f70c5e69 100644
--- a/drivers/char/agp/intel-agp.c
+++ b/drivers/char/agp/intel-agp.c
@@ -907,6 +907,11 @@ static struct pci_device_id agp_intel_pci_table[] = {
 	ID(PCI_DEVICE_ID_INTEL_IVYBRIDGE_HB),
 	ID(PCI_DEVICE_ID_INTEL_IVYBRIDGE_M_HB),
 	ID(PCI_DEVICE_ID_INTEL_IVYBRIDGE_S_HB),
+	ID(PCI_DEVICE_ID_INTEL_VALLEYVIEW_HB),
+	ID(PCI_DEVICE_ID_INTEL_HASWELL_HB),
+	ID(PCI_DEVICE_ID_INTEL_HASWELL_M_HB),
+	ID(PCI_DEVICE_ID_INTEL_HASWELL_S_HB),
+	ID(PCI_DEVICE_ID_INTEL_HASWELL_E_HB),
 	{ }
 };
 
diff --git a/drivers/char/agp/intel-agp.h b/drivers/char/agp/intel-agp.h
index 7ea18a5fe71..c0091753a0d 100644
--- a/drivers/char/agp/intel-agp.h
+++ b/drivers/char/agp/intel-agp.h
@@ -96,6 +96,7 @@
 #define G4x_GMCH_SIZE_VT_2M	(G4x_GMCH_SIZE_2M | G4x_GMCH_SIZE_VT_EN)
 
 #define GFX_FLSH_CNTL		0x2170 /* 915+ */
+#define GFX_FLSH_CNTL_VLV	0x101008
 
 #define I810_DRAM_CTL		0x3000
 #define I810_DRAM_ROW_0		0x00000001
@@ -235,6 +236,19 @@
 #define PCI_DEVICE_ID_INTEL_IVYBRIDGE_S_HB		0x0158  /* Server */
 #define PCI_DEVICE_ID_INTEL_IVYBRIDGE_S_GT1_IG		0x015A
 #define PCI_DEVICE_ID_INTEL_IVYBRIDGE_S_GT2_IG		0x016A
+#define PCI_DEVICE_ID_INTEL_VALLEYVIEW_HB		0x0F00 /* VLV1 */
+#define PCI_DEVICE_ID_INTEL_VALLEYVIEW_IG		0x0F30
+#define PCI_DEVICE_ID_INTEL_HASWELL_HB				0x0400 /* Desktop */
+#define PCI_DEVICE_ID_INTEL_HASWELL_D_GT1_IG		0x0402
+#define PCI_DEVICE_ID_INTEL_HASWELL_D_GT2_IG		0x0412
+#define PCI_DEVICE_ID_INTEL_HASWELL_M_HB			0x0404 /* Mobile */
+#define PCI_DEVICE_ID_INTEL_HASWELL_M_GT1_IG		0x0406
+#define PCI_DEVICE_ID_INTEL_HASWELL_M_GT2_IG		0x0416
+#define PCI_DEVICE_ID_INTEL_HASWELL_S_HB			0x0408 /* Server */
+#define PCI_DEVICE_ID_INTEL_HASWELL_S_GT1_IG		0x040a
+#define PCI_DEVICE_ID_INTEL_HASWELL_S_GT2_IG		0x041a
+#define PCI_DEVICE_ID_INTEL_HASWELL_SDV		0x0c16 /* SDV */
+#define PCI_DEVICE_ID_INTEL_HASWELL_E_HB			0x0c04
 
 int intel_gmch_probe(struct pci_dev *pdev,
 			       struct agp_bridge_data *bridge);
diff --git a/drivers/char/agp/intel-gtt.c b/drivers/char/agp/intel-gtt.c
index 7f025fb620d..1237e7575c3 100644
--- a/drivers/char/agp/intel-gtt.c
+++ b/drivers/char/agp/intel-gtt.c
@@ -1179,6 +1179,20 @@ static void gen6_write_entry(dma_addr_t addr, unsigned int entry,
 	writel(addr | pte_flags, intel_private.gtt + entry);
 }
 
+static void valleyview_write_entry(dma_addr_t addr, unsigned int entry,
+				   unsigned int flags)
+{
+	u32 pte_flags;
+
+	pte_flags = GEN6_PTE_UNCACHED | I810_PTE_VALID;
+
+	/* gen6 has bit11-4 for physical addr bit39-32 */
+	addr |= (addr >> 28) & 0xff0;
+	writel(addr | pte_flags, intel_private.gtt + entry);
+
+	writel(1, intel_private.registers + GFX_FLSH_CNTL_VLV);
+}
+
 static void gen6_cleanup(void)
 {
 }
@@ -1205,12 +1219,16 @@ static inline int needs_idle_maps(void)
 static int i9xx_setup(void)
 {
 	u32 reg_addr;
+	int size = KB(512);
 
 	pci_read_config_dword(intel_private.pcidev, I915_MMADDR, &reg_addr);
 
 	reg_addr &= 0xfff80000;
 
-	intel_private.registers = ioremap(reg_addr, 128 * 4096);
+	if (INTEL_GTT_GEN >= 7)
+		size = MB(2);
+
+	intel_private.registers = ioremap(reg_addr, size);
 	if (!intel_private.registers)
 		return -ENOMEM;
 
@@ -1354,6 +1372,15 @@ static const struct intel_gtt_driver sandybridge_gtt_driver = {
 	.check_flags = gen6_check_flags,
 	.chipset_flush = i9xx_chipset_flush,
 };
+static const struct intel_gtt_driver valleyview_gtt_driver = {
+	.gen = 7,
+	.setup = i9xx_setup,
+	.cleanup = gen6_cleanup,
+	.write_entry = valleyview_write_entry,
+	.dma_mask_size = 40,
+	.check_flags = gen6_check_flags,
+	.chipset_flush = i9xx_chipset_flush,
+};
 
 /* Table to describe Intel GMCH and AGP/PCIE GART drivers.  At least one of
  * driver and gmch_driver must be non-null, and find_gmch will determine
@@ -1460,6 +1487,22 @@ static const struct intel_gtt_driver_description {
 	    "Ivybridge", &sandybridge_gtt_driver },
 	{ PCI_DEVICE_ID_INTEL_IVYBRIDGE_S_GT2_IG,
 	    "Ivybridge", &sandybridge_gtt_driver },
+	{ PCI_DEVICE_ID_INTEL_VALLEYVIEW_IG,
+	    "ValleyView", &valleyview_gtt_driver },
+	{ PCI_DEVICE_ID_INTEL_HASWELL_D_GT1_IG,
+	    "Haswell", &sandybridge_gtt_driver },
+	{ PCI_DEVICE_ID_INTEL_HASWELL_D_GT2_IG,
+	    "Haswell", &sandybridge_gtt_driver },
+	{ PCI_DEVICE_ID_INTEL_HASWELL_M_GT1_IG,
+	    "Haswell", &sandybridge_gtt_driver },
+	{ PCI_DEVICE_ID_INTEL_HASWELL_M_GT2_IG,
+	    "Haswell", &sandybridge_gtt_driver },
+	{ PCI_DEVICE_ID_INTEL_HASWELL_S_GT1_IG,
+	    "Haswell", &sandybridge_gtt_driver },
+	{ PCI_DEVICE_ID_INTEL_HASWELL_S_GT2_IG,
+	    "Haswell", &sandybridge_gtt_driver },
+	{ PCI_DEVICE_ID_INTEL_HASWELL_SDV,
+	    "Haswell", &sandybridge_gtt_driver },
 	{ 0, NULL, NULL }
 };
 
diff --git a/drivers/char/agp/sgi-agp.c b/drivers/char/agp/sgi-agp.c
index ffa888cd1c8..19200037773 100644
--- a/drivers/char/agp/sgi-agp.c
+++ b/drivers/char/agp/sgi-agp.c
@@ -158,7 +158,6 @@ static int sgi_tioca_insert_memory(struct agp_memory *mem, off_t pg_start,
 		break;
 	case LVL2_APER_SIZE:
 		return -EINVAL;
-		break;
 	default:
 		num_entries = 0;
 		break;
diff --git a/drivers/char/hw_random/Kconfig b/drivers/char/hw_random/Kconfig
index 0689bf6b018..f45dad39a18 100644
--- a/drivers/char/hw_random/Kconfig
+++ b/drivers/char/hw_random/Kconfig
@@ -62,8 +62,8 @@ config HW_RANDOM_AMD
 
 config HW_RANDOM_ATMEL
 	tristate "Atmel Random Number Generator support"
-	depends on HW_RANDOM && ARCH_AT91SAM9G45
-	default HW_RANDOM
+	depends on HW_RANDOM && HAVE_CLK
+	default (HW_RANDOM && ARCH_AT91)
 	---help---
 	  This driver provides kernel-side support for the Random Number
 	  Generator hardware found on Atmel AT91 devices.
@@ -250,3 +250,16 @@ config UML_RANDOM
 	  (check your distro, or download from
 	  http://sourceforge.net/projects/gkernel/).  rngd periodically reads
 	  /dev/hwrng and injects the entropy into /dev/random.
+
+config HW_RANDOM_PSERIES
+	tristate "pSeries HW Random Number Generator support"
+	depends on HW_RANDOM && PPC64 && IBMVIO
+	default HW_RANDOM
+	---help---
+	  This driver provides kernel-side support for the Random Number
+	  Generator hardware found on POWER7+ machines and above
+
+	  To compile this driver as a module, choose M here: the
+	  module will be called pseries-rng.
+
+	  If unsure, say Y.
diff --git a/drivers/char/hw_random/Makefile b/drivers/char/hw_random/Makefile
index b2ff5265a99..d901dfa3032 100644
--- a/drivers/char/hw_random/Makefile
+++ b/drivers/char/hw_random/Makefile
@@ -22,3 +22,4 @@ obj-$(CONFIG_HW_RANDOM_OCTEON) += octeon-rng.o
 obj-$(CONFIG_HW_RANDOM_NOMADIK) += nomadik-rng.o
 obj-$(CONFIG_HW_RANDOM_PICOXCELL) += picoxcell-rng.o
 obj-$(CONFIG_HW_RANDOM_PPC4XX) += ppc4xx-rng.o
+obj-$(CONFIG_HW_RANDOM_PSERIES) += pseries-rng.o
diff --git a/drivers/char/hw_random/omap-rng.c b/drivers/char/hw_random/omap-rng.c
index a07a5caa599..1412565c01a 100644
--- a/drivers/char/hw_random/omap-rng.c
+++ b/drivers/char/hw_random/omap-rng.c
@@ -115,22 +115,12 @@ static int __devinit omap_rng_probe(struct platform_device *pdev)
 
 	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
 
-	if (!res) {
-		ret = -ENOENT;
-		goto err_region;
-	}
-
-	if (!request_mem_region(res->start, resource_size(res), pdev->name)) {
-		ret = -EBUSY;
-		goto err_region;
-	}
-
-	dev_set_drvdata(&pdev->dev, res);
-	rng_base = ioremap(res->start, resource_size(res));
+	rng_base = devm_request_and_ioremap(&pdev->dev, res);
 	if (!rng_base) {
 		ret = -ENOMEM;
 		goto err_ioremap;
 	}
+	dev_set_drvdata(&pdev->dev, res);
 
 	ret = hwrng_register(&omap_rng_ops);
 	if (ret)
@@ -145,11 +135,8 @@ static int __devinit omap_rng_probe(struct platform_device *pdev)
 	return 0;
 
 err_register:
-	iounmap(rng_base);
 	rng_base = NULL;
 err_ioremap:
-	release_mem_region(res->start, resource_size(res));
-err_region:
 	if (cpu_is_omap24xx()) {
 		clk_disable(rng_ick);
 		clk_put(rng_ick);
@@ -159,20 +146,15 @@ err_region:
 
 static int __exit omap_rng_remove(struct platform_device *pdev)
 {
-	struct resource *res = dev_get_drvdata(&pdev->dev);
-
 	hwrng_unregister(&omap_rng_ops);
 
 	omap_rng_write_reg(RNG_MASK_REG, 0x0);
 
-	iounmap(rng_base);
-
 	if (cpu_is_omap24xx()) {
 		clk_disable(rng_ick);
 		clk_put(rng_ick);
 	}
 
-	release_mem_region(res->start, resource_size(res));
 	rng_base = NULL;
 
 	return 0;
diff --git a/drivers/char/hw_random/pseries-rng.c b/drivers/char/hw_random/pseries-rng.c
new file mode 100644
index 00000000000..5f1197929f0
--- /dev/null
+++ b/drivers/char/hw_random/pseries-rng.c
@@ -0,0 +1,96 @@
+/*
+ * Copyright (C) 2010 Michael Neuling IBM Corporation
+ *
+ * Driver for the pseries hardware RNG for POWER7+ and above
+ *
+ * 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/hw_random.h>
+#include <asm/vio.h>
+
+#define MODULE_NAME "pseries-rng"
+
+static int pseries_rng_data_read(struct hwrng *rng, u32 *data)
+{
+	if (plpar_hcall(H_RANDOM, (unsigned long *)data) != H_SUCCESS) {
+		printk(KERN_ERR "pseries rng hcall error\n");
+		return 0;
+	}
+	return 8;
+}
+
+/**
+ * pseries_rng_get_desired_dma - Return desired DMA allocate for CMO operations
+ *
+ * This is a required function for a driver to operate in a CMO environment
+ * but this device does not make use of DMA allocations, return 0.
+ *
+ * Return value:
+ *	Number of bytes of IO data the driver will need to perform well -> 0
+ */
+static unsigned long pseries_rng_get_desired_dma(struct vio_dev *vdev)
+{
+	return 0;
+};
+
+static struct hwrng pseries_rng = {
+	.name		= MODULE_NAME,
+	.data_read	= pseries_rng_data_read,
+};
+
+static int __init pseries_rng_probe(struct vio_dev *dev,
+		const struct vio_device_id *id)
+{
+	return hwrng_register(&pseries_rng);
+}
+
+static int __exit pseries_rng_remove(struct vio_dev *dev)
+{
+	hwrng_unregister(&pseries_rng);
+	return 0;
+}
+
+static struct vio_device_id pseries_rng_driver_ids[] = {
+	{ "ibm,random-v1", "ibm,random"},
+	{ "", "" }
+};
+MODULE_DEVICE_TABLE(vio, pseries_rng_driver_ids);
+
+static struct vio_driver pseries_rng_driver = {
+	.name = MODULE_NAME,
+	.probe = pseries_rng_probe,
+	.remove = pseries_rng_remove,
+	.get_desired_dma = pseries_rng_get_desired_dma,
+	.id_table = pseries_rng_driver_ids
+};
+
+static int __init rng_init(void)
+{
+	printk(KERN_INFO "Registering IBM pSeries RNG driver\n");
+	return vio_register_driver(&pseries_rng_driver);
+}
+
+module_init(rng_init);
+
+static void __exit rng_exit(void)
+{
+	vio_unregister_driver(&pseries_rng_driver);
+}
+module_exit(rng_exit);
+
+MODULE_LICENSE("GPL");
+MODULE_AUTHOR("Michael Neuling <mikey@neuling.org>");
+MODULE_DESCRIPTION("H/W RNG driver for IBM pSeries processors");
diff --git a/drivers/char/mem.c b/drivers/char/mem.c
index d6e9d081c8b..67c3371723c 100644
--- a/drivers/char/mem.c
+++ b/drivers/char/mem.c
@@ -807,44 +807,6 @@ static const struct file_operations oldmem_fops = {
 };
 #endif
 
-static ssize_t kmsg_writev(struct kiocb *iocb, const struct iovec *iv,
-			   unsigned long count, loff_t pos)
-{
-	char *line, *p;
-	int i;
-	ssize_t ret = -EFAULT;
-	size_t len = iov_length(iv, count);
-
-	line = kmalloc(len + 1, GFP_KERNEL);
-	if (line == NULL)
-		return -ENOMEM;
-
-	/*
-	 * copy all vectors into a single string, to ensure we do
-	 * not interleave our log line with other printk calls
-	 */
-	p = line;
-	for (i = 0; i < count; i++) {
-		if (copy_from_user(p, iv[i].iov_base, iv[i].iov_len))
-			goto out;
-		p += iv[i].iov_len;
-	}
-	p[0] = '\0';
-
-	ret = printk("%s", line);
-	/* printk can add a prefix */
-	if (ret > len)
-		ret = len;
-out:
-	kfree(line);
-	return ret;
-}
-
-static const struct file_operations kmsg_fops = {
-	.aio_write = kmsg_writev,
-	.llseek = noop_llseek,
-};
-
 static const struct memdev {
 	const char *name;
 	umode_t mode;
@@ -863,7 +825,9 @@ static const struct memdev {
 	 [7] = { "full", 0666, &full_fops, NULL },
 	 [8] = { "random", 0666, &random_fops, NULL },
 	 [9] = { "urandom", 0666, &urandom_fops, NULL },
-	[11] = { "kmsg", 0, &kmsg_fops, NULL },
+#ifdef CONFIG_PRINTK
+	[11] = { "kmsg", 0644, &kmsg_fops, NULL },
+#endif
 #ifdef CONFIG_CRASH_DUMP
 	[12] = { "oldmem", 0, &oldmem_fops, NULL },
 #endif
diff --git a/drivers/char/ramoops.c b/drivers/char/ramoops.c
deleted file mode 100644
index 2a5e45d2a9f..00000000000
--- a/drivers/char/ramoops.c
+++ /dev/null
@@ -1,250 +0,0 @@
-/*
- * RAM Oops/Panic logger
- *
- * Copyright (C) 2010 Marco Stornelli <marco.stornelli@gmail.com>
- *
- * This program is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * version 2 as published by the Free Software Foundation.
- *
- * This program is distributed in the hope that it will be useful, but
- * WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
- * General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program; if not, write to the Free Software
- * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA
- * 02110-1301 USA
- *
- */
-
-#define pr_fmt(fmt) KBUILD_MODNAME ": " fmt
-
-#include <linux/kernel.h>
-#include <linux/err.h>
-#include <linux/module.h>
-#include <linux/kmsg_dump.h>
-#include <linux/time.h>
-#include <linux/io.h>
-#include <linux/ioport.h>
-#include <linux/platform_device.h>
-#include <linux/slab.h>
-#include <linux/ramoops.h>
-
-#define RAMOOPS_KERNMSG_HDR "===="
-#define MIN_MEM_SIZE 4096UL
-
-static ulong record_size = MIN_MEM_SIZE;
-module_param(record_size, ulong, 0400);
-MODULE_PARM_DESC(record_size,
-		"size of each dump done on oops/panic");
-
-static ulong mem_address;
-module_param(mem_address, ulong, 0400);
-MODULE_PARM_DESC(mem_address,
-		"start of reserved RAM used to store oops/panic logs");
-
-static ulong mem_size;
-module_param(mem_size, ulong, 0400);
-MODULE_PARM_DESC(mem_size,
-		"size of reserved RAM used to store oops/panic logs");
-
-static int dump_oops = 1;
-module_param(dump_oops, int, 0600);
-MODULE_PARM_DESC(dump_oops,
-		"set to 1 to dump oopses, 0 to only dump panics (default 1)");
-
-static struct ramoops_context {
-	struct kmsg_dumper dump;
-	void *virt_addr;
-	phys_addr_t phys_addr;
-	unsigned long size;
-	unsigned long record_size;
-	int dump_oops;
-	int count;
-	int max_count;
-} oops_cxt;
-
-static struct platform_device *dummy;
-static struct ramoops_platform_data *dummy_data;
-
-static void ramoops_do_dump(struct kmsg_dumper *dumper,
-		enum kmsg_dump_reason reason, const char *s1, unsigned long l1,
-		const char *s2, unsigned long l2)
-{
-	struct ramoops_context *cxt = container_of(dumper,
-			struct ramoops_context, dump);
-	unsigned long s1_start, s2_start;
-	unsigned long l1_cpy, l2_cpy;
-	int res, hdr_size;
-	char *buf, *buf_orig;
-	struct timeval timestamp;
-
-	if (reason != KMSG_DUMP_OOPS &&
-	    reason != KMSG_DUMP_PANIC)
-		return;
-
-	/* Only dump oopses if dump_oops is set */
-	if (reason == KMSG_DUMP_OOPS && !cxt->dump_oops)
-		return;
-
-	buf = cxt->virt_addr + (cxt->count * cxt->record_size);
-	buf_orig = buf;
-
-	memset(buf, '\0', cxt->record_size);
-	res = sprintf(buf, "%s", RAMOOPS_KERNMSG_HDR);
-	buf += res;
-	do_gettimeofday(&timestamp);
-	res = sprintf(buf, "%lu.%lu\n", (long)timestamp.tv_sec, (long)timestamp.tv_usec);
-	buf += res;
-
-	hdr_size = buf - buf_orig;
-	l2_cpy = min(l2, cxt->record_size - hdr_size);
-	l1_cpy = min(l1, cxt->record_size - hdr_size - l2_cpy);
-
-	s2_start = l2 - l2_cpy;
-	s1_start = l1 - l1_cpy;
-
-	memcpy(buf, s1 + s1_start, l1_cpy);
-	memcpy(buf + l1_cpy, s2 + s2_start, l2_cpy);
-
-	cxt->count = (cxt->count + 1) % cxt->max_count;
-}
-
-static int __init ramoops_probe(struct platform_device *pdev)
-{
-	struct ramoops_platform_data *pdata = pdev->dev.platform_data;
-	struct ramoops_context *cxt = &oops_cxt;
-	int err = -EINVAL;
-
-	if (!pdata->mem_size || !pdata->record_size) {
-		pr_err("The memory size and the record size must be "
-			"non-zero\n");
-		goto fail3;
-	}
-
-	pdata->mem_size = rounddown_pow_of_two(pdata->mem_size);
-	pdata->record_size = rounddown_pow_of_two(pdata->record_size);
-
-	/* Check for the minimum memory size */
-	if (pdata->mem_size < MIN_MEM_SIZE &&
-			pdata->record_size < MIN_MEM_SIZE) {
-		pr_err("memory size too small, minium is %lu\n", MIN_MEM_SIZE);
-		goto fail3;
-	}
-
-	if (pdata->mem_size < pdata->record_size) {
-		pr_err("The memory size must be larger than the "
-			"records size\n");
-		goto fail3;
-	}
-
-	cxt->max_count = pdata->mem_size / pdata->record_size;
-	cxt->count = 0;
-	cxt->size = pdata->mem_size;
-	cxt->phys_addr = pdata->mem_address;
-	cxt->record_size = pdata->record_size;
-	cxt->dump_oops = pdata->dump_oops;
-
-	if (!request_mem_region(cxt->phys_addr, cxt->size, "ramoops")) {
-		pr_err("request mem region failed\n");
-		err = -EINVAL;
-		goto fail3;
-	}
-
-	cxt->virt_addr = ioremap(cxt->phys_addr,  cxt->size);
-	if (!cxt->virt_addr) {
-		pr_err("ioremap failed\n");
-		goto fail2;
-	}
-
-	cxt->dump.dump = ramoops_do_dump;
-	err = kmsg_dump_register(&cxt->dump);
-	if (err) {
-		pr_err("registering kmsg dumper failed\n");
-		goto fail1;
-	}
-
-	/*
-	 * Update the module parameter variables as well so they are visible
-	 * through /sys/module/ramoops/parameters/
-	 */
-	mem_size = pdata->mem_size;
-	mem_address = pdata->mem_address;
-	record_size = pdata->record_size;
-	dump_oops = pdata->dump_oops;
-
-	return 0;
-
-fail1:
-	iounmap(cxt->virt_addr);
-fail2:
-	release_mem_region(cxt->phys_addr, cxt->size);
-fail3:
-	return err;
-}
-
-static int __exit ramoops_remove(struct platform_device *pdev)
-{
-	struct ramoops_context *cxt = &oops_cxt;
-
-	if (kmsg_dump_unregister(&cxt->dump) < 0)
-		pr_warn("could not unregister kmsg_dumper\n");
-
-	iounmap(cxt->virt_addr);
-	release_mem_region(cxt->phys_addr, cxt->size);
-	return 0;
-}
-
-static struct platform_driver ramoops_driver = {
-	.remove		= __exit_p(ramoops_remove),
-	.driver		= {
-		.name	= "ramoops",
-		.owner	= THIS_MODULE,
-	},
-};
-
-static int __init ramoops_init(void)
-{
-	int ret;
-	ret = platform_driver_probe(&ramoops_driver, ramoops_probe);
-	if (ret == -ENODEV) {
-		/*
-		 * If we didn't find a platform device, we use module parameters
-		 * building platform data on the fly.
-		 */
-		pr_info("platform device not found, using module parameters\n");
-		dummy_data = kzalloc(sizeof(struct ramoops_platform_data),
-				     GFP_KERNEL);
-		if (!dummy_data)
-			return -ENOMEM;
-		dummy_data->mem_size = mem_size;
-		dummy_data->mem_address = mem_address;
-		dummy_data->record_size = record_size;
-		dummy_data->dump_oops = dump_oops;
-		dummy = platform_create_bundle(&ramoops_driver, ramoops_probe,
-			NULL, 0, dummy_data,
-			sizeof(struct ramoops_platform_data));
-
-		if (IS_ERR(dummy))
-			ret = PTR_ERR(dummy);
-		else
-			ret = 0;
-	}
-
-	return ret;
-}
-
-static void __exit ramoops_exit(void)
-{
-	platform_driver_unregister(&ramoops_driver);
-	kfree(dummy_data);
-}
-
-module_init(ramoops_init);
-module_exit(ramoops_exit);
-
-MODULE_LICENSE("GPL");
-MODULE_AUTHOR("Marco Stornelli <marco.stornelli@gmail.com>");
-MODULE_DESCRIPTION("RAM Oops/Panic logger/driver");
diff --git a/drivers/char/virtio_console.c b/drivers/char/virtio_console.c
index ddf86b6500b..cdf2f5451c7 100644
--- a/drivers/char/virtio_console.c
+++ b/drivers/char/virtio_console.c
@@ -1895,6 +1895,13 @@ static int virtcons_restore(struct virtio_device *vdev)
 
 		/* Get port open/close status on the host */
 		send_control_msg(port, VIRTIO_CONSOLE_PORT_READY, 1);
+
+		/*
+		 * If a port was open at the time of suspending, we
+		 * have to let the host know that it's still open.
+		 */
+		if (port->guest_connected)
+			send_control_msg(port, VIRTIO_CONSOLE_PORT_OPEN, 1);
 	}
 	return 0;
 }
diff --git a/drivers/char/xilinx_hwicap/xilinx_hwicap.c b/drivers/char/xilinx_hwicap/xilinx_hwicap.c
index 31ba11ca75e..2c5d15beea3 100644
--- a/drivers/char/xilinx_hwicap/xilinx_hwicap.c
+++ b/drivers/char/xilinx_hwicap/xilinx_hwicap.c
@@ -167,6 +167,7 @@ static const struct config_registers v4_config_registers = {
 	.BOOTSTS = UNIMPLEMENTED,
 	.CTL_1 = UNIMPLEMENTED,
 };
+
 static const struct config_registers v5_config_registers = {
 	.CRC = 0,
 	.FAR = 1,
@@ -192,6 +193,31 @@ static const struct config_registers v5_config_registers = {
 	.CTL_1 = 19,
 };
 
+static const struct config_registers v6_config_registers = {
+	.CRC = 0,
+	.FAR = 1,
+	.FDRI = 2,
+	.FDRO = 3,
+	.CMD = 4,
+	.CTL = 5,
+	.MASK = 6,
+	.STAT = 7,
+	.LOUT = 8,
+	.COR = 9,
+	.MFWR = 10,
+	.FLR = UNIMPLEMENTED,
+	.KEY = UNIMPLEMENTED,
+	.CBC = 11,
+	.IDCODE = 12,
+	.AXSS = 13,
+	.C0R_1 = 14,
+	.CSOB = 15,
+	.WBSTAR = 16,
+	.TIMER = 17,
+	.BOOTSTS = 22,
+	.CTL_1 = 24,
+};
+
 /**
  * hwicap_command_desync - Send a DESYNC command to the ICAP port.
  * @drvdata: a pointer to the drvdata.
@@ -744,6 +770,8 @@ static int __devinit hwicap_of_probe(struct platform_device *op,
 			regs = &v4_config_registers;
 		} else if (!strcmp(family, "virtex5")) {
 			regs = &v5_config_registers;
+		} else if (!strcmp(family, "virtex6")) {
+			regs = &v6_config_registers;
 		}
 	}
 	return hwicap_setup(&op->dev, id ? *id : -1, &res, config,
@@ -785,6 +813,8 @@ static int __devinit hwicap_drv_probe(struct platform_device *pdev)
 			regs = &v4_config_registers;
 		} else if (!strcmp(family, "virtex5")) {
 			regs = &v5_config_registers;
+		} else if (!strcmp(family, "virtex6")) {
+			regs = &v6_config_registers;
 		}
 	}
 
diff --git a/drivers/char/xilinx_hwicap/xilinx_hwicap.h b/drivers/char/xilinx_hwicap/xilinx_hwicap.h
index 8cca11981c5..d31ee23c9f1 100644
--- a/drivers/char/xilinx_hwicap/xilinx_hwicap.h
+++ b/drivers/char/xilinx_hwicap/xilinx_hwicap.h
@@ -86,7 +86,7 @@ struct hwicap_driver_config {
 };
 
 /* Number of times to poll the done regsiter */
-#define XHI_MAX_RETRIES     10
+#define XHI_MAX_RETRIES     5000
 
 /************ Constant Definitions *************/
 
diff --git a/drivers/clk/Kconfig b/drivers/clk/Kconfig
index 165e1febae5..4864407e3fc 100644
--- a/drivers/clk/Kconfig
+++ b/drivers/clk/Kconfig
@@ -12,6 +12,7 @@ config HAVE_MACH_CLKDEV
 config COMMON_CLK
 	bool
 	select HAVE_CLK_PREPARE
+	select CLKDEV_LOOKUP
 	---help---
 	  The common clock framework is a single definition of struct
 	  clk, useful across many platforms, as well as an
@@ -22,17 +23,6 @@ config COMMON_CLK
 menu "Common Clock Framework"
 	depends on COMMON_CLK
 
-config COMMON_CLK_DISABLE_UNUSED
-	bool "Disabled unused clocks at boot"
-	depends on COMMON_CLK
-	---help---
-	  Traverses the entire clock tree and disables any clocks that are
-	  enabled in hardware but have not been enabled by any device drivers.
-	  This saves power and keeps the software model of the clock in line
-	  with reality.
-
-	  If in doubt, say "N".
-
 config COMMON_CLK_DEBUG
 	bool "DebugFS representation of clock tree"
 	depends on COMMON_CLK
diff --git a/drivers/clk/Makefile b/drivers/clk/Makefile
index 1f736bc11c4..b9a5158a30b 100644
--- a/drivers/clk/Makefile
+++ b/drivers/clk/Makefile
@@ -1,4 +1,7 @@
 
 obj-$(CONFIG_CLKDEV_LOOKUP)	+= clkdev.o
 obj-$(CONFIG_COMMON_CLK)	+= clk.o clk-fixed-rate.o clk-gate.o \
-				   clk-mux.o clk-divider.o
+				   clk-mux.o clk-divider.o clk-fixed-factor.o
+# SoCs specific
+obj-$(CONFIG_ARCH_MXS)		+= mxs/
+obj-$(CONFIG_PLAT_SPEAR)	+= spear/
diff --git a/drivers/clk/clk-divider.c b/drivers/clk/clk-divider.c
index d5ac6a75ea5..8ea11b44452 100644
--- a/drivers/clk/clk-divider.c
+++ b/drivers/clk/clk-divider.c
@@ -45,7 +45,6 @@ static unsigned long clk_divider_recalc_rate(struct clk_hw *hw,
 
 	return parent_rate / div;
 }
-EXPORT_SYMBOL_GPL(clk_divider_recalc_rate);
 
 /*
  * The reverse of DIV_ROUND_UP: The maximum number which
@@ -68,8 +67,8 @@ static int clk_divider_bestdiv(struct clk_hw *hw, unsigned long rate,
 	if (divider->flags & CLK_DIVIDER_ONE_BASED)
 		maxdiv--;
 
-	if (!best_parent_rate) {
-		parent_rate = __clk_get_rate(__clk_get_parent(hw->clk));
+	if (!(__clk_get_flags(hw->clk) & CLK_SET_RATE_PARENT)) {
+		parent_rate = *best_parent_rate;
 		bestdiv = DIV_ROUND_UP(parent_rate, rate);
 		bestdiv = bestdiv == 0 ? 1 : bestdiv;
 		bestdiv = bestdiv > maxdiv ? maxdiv : bestdiv;
@@ -109,24 +108,18 @@ static long clk_divider_round_rate(struct clk_hw *hw, unsigned long rate,
 	int div;
 	div = clk_divider_bestdiv(hw, rate, prate);
 
-	if (prate)
-		return *prate / div;
-	else {
-		unsigned long r;
-		r = __clk_get_rate(__clk_get_parent(hw->clk));
-		return r / div;
-	}
+	return *prate / div;
 }
-EXPORT_SYMBOL_GPL(clk_divider_round_rate);
 
-static int clk_divider_set_rate(struct clk_hw *hw, unsigned long rate)
+static int clk_divider_set_rate(struct clk_hw *hw, unsigned long rate,
+				unsigned long parent_rate)
 {
 	struct clk_divider *divider = to_clk_divider(hw);
 	unsigned int div;
 	unsigned long flags = 0;
 	u32 val;
 
-	div = __clk_get_rate(__clk_get_parent(hw->clk)) / rate;
+	div = parent_rate / rate;
 
 	if (!(divider->flags & CLK_DIVIDER_ONE_BASED))
 		div--;
@@ -147,15 +140,26 @@ static int clk_divider_set_rate(struct clk_hw *hw, unsigned long rate)
 
 	return 0;
 }
-EXPORT_SYMBOL_GPL(clk_divider_set_rate);
 
-struct clk_ops clk_divider_ops = {
+const struct clk_ops clk_divider_ops = {
 	.recalc_rate = clk_divider_recalc_rate,
 	.round_rate = clk_divider_round_rate,
 	.set_rate = clk_divider_set_rate,
 };
 EXPORT_SYMBOL_GPL(clk_divider_ops);
 
+/**
+ * clk_register_divider - register a divider clock with the clock framework
+ * @dev: device registering this clock
+ * @name: name of this clock
+ * @parent_name: name of clock's parent
+ * @flags: framework-specific flags
+ * @reg: register address to adjust divider
+ * @shift: number of bits to shift the bitfield
+ * @width: width of the bitfield
+ * @clk_divider_flags: divider-specific flags for this clock
+ * @lock: shared register lock for this clock
+ */
 struct clk *clk_register_divider(struct device *dev, const char *name,
 		const char *parent_name, unsigned long flags,
 		void __iomem *reg, u8 shift, u8 width,
@@ -163,38 +167,34 @@ struct clk *clk_register_divider(struct device *dev, const char *name,
 {
 	struct clk_divider *div;
 	struct clk *clk;
+	struct clk_init_data init;
 
+	/* allocate the divider */
 	div = kzalloc(sizeof(struct clk_divider), GFP_KERNEL);
-
 	if (!div) {
 		pr_err("%s: could not allocate divider clk\n", __func__);
-		return NULL;
+		return ERR_PTR(-ENOMEM);
 	}
 
+	init.name = name;
+	init.ops = &clk_divider_ops;
+	init.flags = flags;
+	init.parent_names = (parent_name ? &parent_name: NULL);
+	init.num_parents = (parent_name ? 1 : 0);
+
 	/* struct clk_divider assignments */
 	div->reg = reg;
 	div->shift = shift;
 	div->width = width;
 	div->flags = clk_divider_flags;
 	div->lock = lock;
+	div->hw.init = &init;
 
-	if (parent_name) {
-		div->parent[0] = kstrdup(parent_name, GFP_KERNEL);
-		if (!div->parent[0])
-			goto out;
-	}
-
-	clk = clk_register(dev, name,
-			&clk_divider_ops, &div->hw,
-			div->parent,
-			(parent_name ? 1 : 0),
-			flags);
-	if (clk)
-		return clk;
+	/* register the clock */
+	clk = clk_register(dev, &div->hw);
 
-out:
-	kfree(div->parent[0]);
-	kfree(div);
+	if (IS_ERR(clk))
+		kfree(div);
 
-	return NULL;
+	return clk;
 }
diff --git a/drivers/clk/clk-fixed-factor.c b/drivers/clk/clk-fixed-factor.c
new file mode 100644
index 00000000000..c8c003e217a
--- /dev/null
+++ b/drivers/clk/clk-fixed-factor.c
@@ -0,0 +1,95 @@
+/*
+ * Copyright (C) 2011 Sascha Hauer, Pengutronix <s.hauer@pengutronix.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.
+ *
+ * Standard functionality for the common clock API.
+ */
+#include <linux/module.h>
+#include <linux/clk-provider.h>
+#include <linux/slab.h>
+#include <linux/err.h>
+
+/*
+ * DOC: basic fixed multiplier and divider clock that cannot gate
+ *
+ * Traits of this clock:
+ * prepare - clk_prepare only ensures that parents are prepared
+ * enable - clk_enable only ensures that parents are enabled
+ * rate - rate is fixed.  clk->rate = parent->rate / div * mult
+ * parent - fixed parent.  No clk_set_parent support
+ */
+
+#define to_clk_fixed_factor(_hw) container_of(_hw, struct clk_fixed_factor, hw)
+
+static unsigned long clk_factor_recalc_rate(struct clk_hw *hw,
+		unsigned long parent_rate)
+{
+	struct clk_fixed_factor *fix = to_clk_fixed_factor(hw);
+
+	return parent_rate * fix->mult / fix->div;
+}
+
+static long clk_factor_round_rate(struct clk_hw *hw, unsigned long rate,
+				unsigned long *prate)
+{
+	struct clk_fixed_factor *fix = to_clk_fixed_factor(hw);
+
+	if (__clk_get_flags(hw->clk) & CLK_SET_RATE_PARENT) {
+		unsigned long best_parent;
+
+		best_parent = (rate / fix->mult) * fix->div;
+		*prate = __clk_round_rate(__clk_get_parent(hw->clk),
+				best_parent);
+	}
+
+	return (*prate / fix->div) * fix->mult;
+}
+
+static int clk_factor_set_rate(struct clk_hw *hw, unsigned long rate,
+				unsigned long parent_rate)
+{
+	return 0;
+}
+
+struct clk_ops clk_fixed_factor_ops = {
+	.round_rate = clk_factor_round_rate,
+	.set_rate = clk_factor_set_rate,
+	.recalc_rate = clk_factor_recalc_rate,
+};
+EXPORT_SYMBOL_GPL(clk_fixed_factor_ops);
+
+struct clk *clk_register_fixed_factor(struct device *dev, const char *name,
+		const char *parent_name, unsigned long flags,
+		unsigned int mult, unsigned int div)
+{
+	struct clk_fixed_factor *fix;
+	struct clk_init_data init;
+	struct clk *clk;
+
+	fix = kmalloc(sizeof(*fix), GFP_KERNEL);
+	if (!fix) {
+		pr_err("%s: could not allocate fixed factor clk\n", __func__);
+		return ERR_PTR(-ENOMEM);
+	}
+
+	/* struct clk_fixed_factor assignments */
+	fix->mult = mult;
+	fix->div = div;
+	fix->hw.init = &init;
+
+	init.name = name;
+	init.ops = &clk_fixed_factor_ops;
+	init.flags = flags;
+	init.parent_names = &parent_name;
+	init.num_parents = 1;
+
+	clk = clk_register(dev, &fix->hw);
+
+	if (IS_ERR(clk))
+		kfree(fix);
+
+	return clk;
+}
diff --git a/drivers/clk/clk-fixed-rate.c b/drivers/clk/clk-fixed-rate.c
index 90c79fb5d1b..cbd24622978 100644
--- a/drivers/clk/clk-fixed-rate.c
+++ b/drivers/clk/clk-fixed-rate.c
@@ -32,51 +32,50 @@ static unsigned long clk_fixed_rate_recalc_rate(struct clk_hw *hw,
 {
 	return to_clk_fixed_rate(hw)->fixed_rate;
 }
-EXPORT_SYMBOL_GPL(clk_fixed_rate_recalc_rate);
 
-struct clk_ops clk_fixed_rate_ops = {
+const struct clk_ops clk_fixed_rate_ops = {
 	.recalc_rate = clk_fixed_rate_recalc_rate,
 };
 EXPORT_SYMBOL_GPL(clk_fixed_rate_ops);
 
+/**
+ * clk_register_fixed_rate - register fixed-rate clock with the clock framework
+ * @dev: device that is registering this clock
+ * @name: name of this clock
+ * @parent_name: name of clock's parent
+ * @flags: framework-specific flags
+ * @fixed_rate: non-adjustable clock rate
+ */
 struct clk *clk_register_fixed_rate(struct device *dev, const char *name,
 		const char *parent_name, unsigned long flags,
 		unsigned long fixed_rate)
 {
 	struct clk_fixed_rate *fixed;
-	char **parent_names = NULL;
-	u8 len;
+	struct clk *clk;
+	struct clk_init_data init;
 
+	/* allocate fixed-rate clock */
 	fixed = kzalloc(sizeof(struct clk_fixed_rate), GFP_KERNEL);
-
 	if (!fixed) {
 		pr_err("%s: could not allocate fixed clk\n", __func__);
 		return ERR_PTR(-ENOMEM);
 	}
 
+	init.name = name;
+	init.ops = &clk_fixed_rate_ops;
+	init.flags = flags;
+	init.parent_names = (parent_name ? &parent_name: NULL);
+	init.num_parents = (parent_name ? 1 : 0);
+
 	/* struct clk_fixed_rate assignments */
 	fixed->fixed_rate = fixed_rate;
+	fixed->hw.init = &init;
 
-	if (parent_name) {
-		parent_names = kmalloc(sizeof(char *), GFP_KERNEL);
-
-		if (! parent_names)
-			goto out;
+	/* register the clock */
+	clk = clk_register(dev, &fixed->hw);
 
-		len = sizeof(char) * strlen(parent_name);
-
-		parent_names[0] = kmalloc(len, GFP_KERNEL);
-
-		if (!parent_names[0])
-			goto out;
-
-		strncpy(parent_names[0], parent_name, len);
-	}
+	if (IS_ERR(clk))
+		kfree(fixed);
 
-out:
-	return clk_register(dev, name,
-			&clk_fixed_rate_ops, &fixed->hw,
-			parent_names,
-			(parent_name ? 1 : 0),
-			flags);
+	return clk;
 }
diff --git a/drivers/clk/clk-gate.c b/drivers/clk/clk-gate.c
index b5902e2ef2f..578465e04be 100644
--- a/drivers/clk/clk-gate.c
+++ b/drivers/clk/clk-gate.c
@@ -28,32 +28,38 @@
 
 #define to_clk_gate(_hw) container_of(_hw, struct clk_gate, hw)
 
-static void clk_gate_set_bit(struct clk_gate *gate)
+/*
+ * It works on following logic:
+ *
+ * For enabling clock, enable = 1
+ *	set2dis = 1	-> clear bit	-> set = 0
+ *	set2dis = 0	-> set bit	-> set = 1
+ *
+ * For disabling clock, enable = 0
+ *	set2dis = 1	-> set bit	-> set = 1
+ *	set2dis = 0	-> clear bit	-> set = 0
+ *
+ * So, result is always: enable xor set2dis.
+ */
+static void clk_gate_endisable(struct clk_hw *hw, int enable)
 {
-	u32 reg;
+	struct clk_gate *gate = to_clk_gate(hw);
+	int set = gate->flags & CLK_GATE_SET_TO_DISABLE ? 1 : 0;
 	unsigned long flags = 0;
+	u32 reg;
+
+	set ^= enable;
 
 	if (gate->lock)
 		spin_lock_irqsave(gate->lock, flags);
 
 	reg = readl(gate->reg);
-	reg |= BIT(gate->bit_idx);
-	writel(reg, gate->reg);
-
-	if (gate->lock)
-		spin_unlock_irqrestore(gate->lock, flags);
-}
-
-static void clk_gate_clear_bit(struct clk_gate *gate)
-{
-	u32 reg;
-	unsigned long flags = 0;
 
-	if (gate->lock)
-		spin_lock_irqsave(gate->lock, flags);
+	if (set)
+		reg |= BIT(gate->bit_idx);
+	else
+		reg &= ~BIT(gate->bit_idx);
 
-	reg = readl(gate->reg);
-	reg &= ~BIT(gate->bit_idx);
 	writel(reg, gate->reg);
 
 	if (gate->lock)
@@ -62,27 +68,15 @@ static void clk_gate_clear_bit(struct clk_gate *gate)
 
 static int clk_gate_enable(struct clk_hw *hw)
 {
-	struct clk_gate *gate = to_clk_gate(hw);
-
-	if (gate->flags & CLK_GATE_SET_TO_DISABLE)
-		clk_gate_clear_bit(gate);
-	else
-		clk_gate_set_bit(gate);
+	clk_gate_endisable(hw, 1);
 
 	return 0;
 }
-EXPORT_SYMBOL_GPL(clk_gate_enable);
 
 static void clk_gate_disable(struct clk_hw *hw)
 {
-	struct clk_gate *gate = to_clk_gate(hw);
-
-	if (gate->flags & CLK_GATE_SET_TO_DISABLE)
-		clk_gate_set_bit(gate);
-	else
-		clk_gate_clear_bit(gate);
+	clk_gate_endisable(hw, 0);
 }
-EXPORT_SYMBOL_GPL(clk_gate_disable);
 
 static int clk_gate_is_enabled(struct clk_hw *hw)
 {
@@ -99,15 +93,25 @@ static int clk_gate_is_enabled(struct clk_hw *hw)
 
 	return reg ? 1 : 0;
 }
-EXPORT_SYMBOL_GPL(clk_gate_is_enabled);
 
-struct clk_ops clk_gate_ops = {
+const struct clk_ops clk_gate_ops = {
 	.enable = clk_gate_enable,
 	.disable = clk_gate_disable,
 	.is_enabled = clk_gate_is_enabled,
 };
 EXPORT_SYMBOL_GPL(clk_gate_ops);
 
+/**
+ * clk_register_gate - register a gate clock with the clock framework
+ * @dev: device that is registering this clock
+ * @name: name of this clock
+ * @parent_name: name of this clock's parent
+ * @flags: framework-specific flags for this clock
+ * @reg: register address to control gating of this clock
+ * @bit_idx: which bit in the register controls gating of this clock
+ * @clk_gate_flags: gate-specific flags for this clock
+ * @lock: shared register lock for this clock
+ */
 struct clk *clk_register_gate(struct device *dev, const char *name,
 		const char *parent_name, unsigned long flags,
 		void __iomem *reg, u8 bit_idx,
@@ -115,36 +119,32 @@ struct clk *clk_register_gate(struct device *dev, const char *name,
 {
 	struct clk_gate *gate;
 	struct clk *clk;
+	struct clk_init_data init;
 
+	/* allocate the gate */
 	gate = kzalloc(sizeof(struct clk_gate), GFP_KERNEL);
-
 	if (!gate) {
 		pr_err("%s: could not allocate gated clk\n", __func__);
-		return NULL;
+		return ERR_PTR(-ENOMEM);
 	}
 
+	init.name = name;
+	init.ops = &clk_gate_ops;
+	init.flags = flags;
+	init.parent_names = (parent_name ? &parent_name: NULL);
+	init.num_parents = (parent_name ? 1 : 0);
+
 	/* struct clk_gate assignments */
 	gate->reg = reg;
 	gate->bit_idx = bit_idx;
 	gate->flags = clk_gate_flags;
 	gate->lock = lock;
+	gate->hw.init = &init;
 
-	if (parent_name) {
-		gate->parent[0] = kstrdup(parent_name, GFP_KERNEL);
-		if (!gate->parent[0])
-			goto out;
-	}
+	clk = clk_register(dev, &gate->hw);
+
+	if (IS_ERR(clk))
+		kfree(gate);
 
-	clk = clk_register(dev, name,
-			&clk_gate_ops, &gate->hw,
-			gate->parent,
-			(parent_name ? 1 : 0),
-			flags);
-	if (clk)
-		return clk;
-out:
-	kfree(gate->parent[0]);
-	kfree(gate);
-
-	return NULL;
+	return clk;
 }
diff --git a/drivers/clk/clk-mux.c b/drivers/clk/clk-mux.c
index c71ad1f41a9..fd36a8ea73d 100644
--- a/drivers/clk/clk-mux.c
+++ b/drivers/clk/clk-mux.c
@@ -55,7 +55,6 @@ static u8 clk_mux_get_parent(struct clk_hw *hw)
 
 	return val;
 }
-EXPORT_SYMBOL_GPL(clk_mux_get_parent);
 
 static int clk_mux_set_parent(struct clk_hw *hw, u8 index)
 {
@@ -82,35 +81,47 @@ static int clk_mux_set_parent(struct clk_hw *hw, u8 index)
 
 	return 0;
 }
-EXPORT_SYMBOL_GPL(clk_mux_set_parent);
 
-struct clk_ops clk_mux_ops = {
+const struct clk_ops clk_mux_ops = {
 	.get_parent = clk_mux_get_parent,
 	.set_parent = clk_mux_set_parent,
 };
 EXPORT_SYMBOL_GPL(clk_mux_ops);
 
 struct clk *clk_register_mux(struct device *dev, const char *name,
-		char **parent_names, u8 num_parents, unsigned long flags,
+		const char **parent_names, u8 num_parents, unsigned long flags,
 		void __iomem *reg, u8 shift, u8 width,
 		u8 clk_mux_flags, spinlock_t *lock)
 {
 	struct clk_mux *mux;
+	struct clk *clk;
+	struct clk_init_data init;
 
-	mux = kmalloc(sizeof(struct clk_mux), GFP_KERNEL);
-
+	/* allocate the mux */
+	mux = kzalloc(sizeof(struct clk_mux), GFP_KERNEL);
 	if (!mux) {
 		pr_err("%s: could not allocate mux clk\n", __func__);
 		return ERR_PTR(-ENOMEM);
 	}
 
+	init.name = name;
+	init.ops = &clk_mux_ops;
+	init.flags = flags;
+	init.parent_names = parent_names;
+	init.num_parents = num_parents;
+
 	/* struct clk_mux assignments */
 	mux->reg = reg;
 	mux->shift = shift;
 	mux->width = width;
 	mux->flags = clk_mux_flags;
 	mux->lock = lock;
+	mux->hw.init = &init;
+
+	clk = clk_register(dev, &mux->hw);
+
+	if (IS_ERR(clk))
+		kfree(mux);
 
-	return clk_register(dev, name, &clk_mux_ops, &mux->hw,
-			parent_names, num_parents, flags);
+	return clk;
 }
diff --git a/drivers/clk/clk.c b/drivers/clk/clk.c
index 9cf6f59e3e1..687b00d67c8 100644
--- a/drivers/clk/clk.c
+++ b/drivers/clk/clk.c
@@ -194,9 +194,8 @@ static int __init clk_debug_init(void)
 late_initcall(clk_debug_init);
 #else
 static inline int clk_debug_register(struct clk *clk) { return 0; }
-#endif /* CONFIG_COMMON_CLK_DEBUG */
+#endif
 
-#ifdef CONFIG_COMMON_CLK_DISABLE_UNUSED
 /* caller must hold prepare_lock */
 static void clk_disable_unused_subtree(struct clk *clk)
 {
@@ -246,9 +245,6 @@ static int clk_disable_unused(void)
 	return 0;
 }
 late_initcall(clk_disable_unused);
-#else
-static inline int clk_disable_unused(struct clk *clk) { return 0; }
-#endif /* CONFIG_COMMON_CLK_DISABLE_UNUSED */
 
 /***    helper functions   ***/
 
@@ -287,7 +283,7 @@ unsigned long __clk_get_rate(struct clk *clk)
 	unsigned long ret;
 
 	if (!clk) {
-		ret = -EINVAL;
+		ret = 0;
 		goto out;
 	}
 
@@ -297,7 +293,7 @@ unsigned long __clk_get_rate(struct clk *clk)
 		goto out;
 
 	if (!clk->parent)
-		ret = -ENODEV;
+		ret = 0;
 
 out:
 	return ret;
@@ -562,7 +558,7 @@ EXPORT_SYMBOL_GPL(clk_enable);
  * @clk: the clk whose rate is being returned
  *
  * Simply returns the cached rate of the clk.  Does not query the hardware.  If
- * clk is NULL then returns -EINVAL.
+ * clk is NULL then returns 0.
  */
 unsigned long clk_get_rate(struct clk *clk)
 {
@@ -584,18 +580,22 @@ EXPORT_SYMBOL_GPL(clk_get_rate);
  */
 unsigned long __clk_round_rate(struct clk *clk, unsigned long rate)
 {
-	unsigned long unused;
+	unsigned long parent_rate = 0;
 
 	if (!clk)
 		return -EINVAL;
 
-	if (!clk->ops->round_rate)
-		return clk->rate;
+	if (!clk->ops->round_rate) {
+		if (clk->flags & CLK_SET_RATE_PARENT)
+			return __clk_round_rate(clk->parent, rate);
+		else
+			return clk->rate;
+	}
 
-	if (clk->flags & CLK_SET_RATE_PARENT)
-		return clk->ops->round_rate(clk->hw, rate, &unused);
-	else
-		return clk->ops->round_rate(clk->hw, rate, NULL);
+	if (clk->parent)
+		parent_rate = clk->parent->rate;
+
+	return clk->ops->round_rate(clk->hw, rate, &parent_rate);
 }
 
 /**
@@ -765,25 +765,41 @@ static void clk_calc_subtree(struct clk *clk, unsigned long new_rate)
 static struct clk *clk_calc_new_rates(struct clk *clk, unsigned long rate)
 {
 	struct clk *top = clk;
-	unsigned long best_parent_rate = clk->parent->rate;
+	unsigned long best_parent_rate = 0;
 	unsigned long new_rate;
 
-	if (!clk->ops->round_rate && !(clk->flags & CLK_SET_RATE_PARENT)) {
-		clk->new_rate = clk->rate;
+	/* sanity */
+	if (IS_ERR_OR_NULL(clk))
+		return NULL;
+
+	/* save parent rate, if it exists */
+	if (clk->parent)
+		best_parent_rate = clk->parent->rate;
+
+	/* never propagate up to the parent */
+	if (!(clk->flags & CLK_SET_RATE_PARENT)) {
+		if (!clk->ops->round_rate) {
+			clk->new_rate = clk->rate;
+			return NULL;
+		}
+		new_rate = clk->ops->round_rate(clk->hw, rate, &best_parent_rate);
+		goto out;
+	}
+
+	/* need clk->parent from here on out */
+	if (!clk->parent) {
+		pr_debug("%s: %s has NULL parent\n", __func__, clk->name);
 		return NULL;
 	}
 
-	if (!clk->ops->round_rate && (clk->flags & CLK_SET_RATE_PARENT)) {
+	if (!clk->ops->round_rate) {
 		top = clk_calc_new_rates(clk->parent, rate);
-		new_rate = clk->new_rate = clk->parent->new_rate;
+		new_rate = clk->parent->new_rate;
 
 		goto out;
 	}
 
-	if (clk->flags & CLK_SET_RATE_PARENT)
-		new_rate = clk->ops->round_rate(clk->hw, rate, &best_parent_rate);
-	else
-		new_rate = clk->ops->round_rate(clk->hw, rate, NULL);
+	new_rate = clk->ops->round_rate(clk->hw, rate, &best_parent_rate);
 
 	if (best_parent_rate != clk->parent->rate) {
 		top = clk_calc_new_rates(clk->parent, best_parent_rate);
@@ -839,7 +855,7 @@ static void clk_change_rate(struct clk *clk)
 	old_rate = clk->rate;
 
 	if (clk->ops->set_rate)
-		clk->ops->set_rate(clk->hw, clk->new_rate);
+		clk->ops->set_rate(clk->hw, clk->new_rate, clk->parent->rate);
 
 	if (clk->ops->recalc_rate)
 		clk->rate = clk->ops->recalc_rate(clk->hw,
@@ -859,38 +875,19 @@ static void clk_change_rate(struct clk *clk)
  * @clk: the clk whose rate is being changed
  * @rate: the new rate for clk
  *
- * In the simplest case clk_set_rate will only change the rate of clk.
- *
- * If clk has the CLK_SET_RATE_GATE flag set and it is enabled this call
- * will fail; only when the clk is disabled will it be able to change
- * its rate.
+ * In the simplest case clk_set_rate will only adjust the rate of clk.
  *
- * Setting the CLK_SET_RATE_PARENT flag allows clk_set_rate to
- * recursively propagate up to clk's parent; whether or not this happens
- * depends on the outcome of clk's .round_rate implementation.  If
- * *parent_rate is 0 after calling .round_rate then upstream parent
- * propagation is ignored.  If *parent_rate comes back with a new rate
- * for clk's parent then we propagate up to clk's parent and set it's
- * rate.  Upward propagation will continue until either a clk does not
- * support the CLK_SET_RATE_PARENT flag or .round_rate stops requesting
- * changes to clk's parent_rate.  If there is a failure during upstream
- * propagation then clk_set_rate will unwind and restore each clk's rate
- * that had been successfully changed.  Afterwards a rate change abort
- * notification will be propagated downstream, starting from the clk
- * that failed.
+ * Setting the CLK_SET_RATE_PARENT flag allows the rate change operation to
+ * propagate up to clk's parent; whether or not this happens depends on the
+ * outcome of clk's .round_rate implementation.  If *parent_rate is unchanged
+ * after calling .round_rate then upstream parent propagation is ignored.  If
+ * *parent_rate comes back with a new rate for clk's parent then we propagate
+ * up to clk's parent and set it's rate.  Upward propagation will continue
+ * until either a clk does not support the CLK_SET_RATE_PARENT flag or
+ * .round_rate stops requesting changes to clk's parent_rate.
  *
- * At the end of all of the rate setting, clk_set_rate internally calls
- * __clk_recalc_rates and propagates the rate changes downstream,
- * starting from the highest clk whose rate was changed.  This has the
- * added benefit of propagating post-rate change notifiers.
- *
- * Note that while post-rate change and rate change abort notifications
- * are guaranteed to be sent to a clk only once per call to
- * clk_set_rate, pre-change notifications will be sent for every clk
- * whose rate is changed.  Stacking pre-change notifications is noisy
- * for the drivers subscribed to them, but this allows drivers to react
- * to intermediate clk rate changes up until the point where the final
- * rate is achieved at the end of upstream propagation.
+ * Rate changes are accomplished via tree traversal that also recalculates the
+ * rates for the clocks and fires off POST_RATE_CHANGE notifiers.
  *
  * Returns 0 on success, -EERROR otherwise.
  */
@@ -906,6 +903,11 @@ int clk_set_rate(struct clk *clk, unsigned long rate)
 	if (rate == clk->rate)
 		goto out;
 
+	if ((clk->flags & CLK_SET_RATE_GATE) && clk->prepare_count) {
+		ret = -EBUSY;
+		goto out;
+	}
+
 	/* calculate new rates and get the topmost changed clock */
 	top = clk_calc_new_rates(clk, rate);
 	if (!top) {
@@ -1175,40 +1177,41 @@ EXPORT_SYMBOL_GPL(clk_set_parent);
  *
  * Initializes the lists in struct clk, queries the hardware for the
  * parent and rate and sets them both.
- *
- * Any struct clk passed into __clk_init must have the following members
- * populated:
- * 	.name
- * 	.ops
- * 	.hw
- * 	.parent_names
- * 	.num_parents
- * 	.flags
- *
- * Essentially, everything that would normally be passed into clk_register is
- * assumed to be initialized already in __clk_init.  The other members may be
- * populated, but are optional.
- *
- * __clk_init is only exposed via clk-private.h and is intended for use with
- * very large numbers of clocks that need to be statically initialized.  It is
- * a layering violation to include clk-private.h from any code which implements
- * a clock's .ops; as such any statically initialized clock data MUST be in a
- * separate C file from the logic that implements it's operations.
  */
-void __clk_init(struct device *dev, struct clk *clk)
+int __clk_init(struct device *dev, struct clk *clk)
 {
-	int i;
+	int i, ret = 0;
 	struct clk *orphan;
 	struct hlist_node *tmp, *tmp2;
 
 	if (!clk)
-		return;
+		return -EINVAL;
 
 	mutex_lock(&prepare_lock);
 
 	/* check to see if a clock with this name is already registered */
-	if (__clk_lookup(clk->name))
+	if (__clk_lookup(clk->name)) {
+		pr_debug("%s: clk %s already initialized\n",
+				__func__, clk->name);
+		ret = -EEXIST;
+		goto out;
+	}
+
+	/* check that clk_ops are sane.  See Documentation/clk.txt */
+	if (clk->ops->set_rate &&
+			!(clk->ops->round_rate && clk->ops->recalc_rate)) {
+		pr_warning("%s: %s must implement .round_rate & .recalc_rate\n",
+				__func__, clk->name);
+		ret = -EINVAL;
+		goto out;
+	}
+
+	if (clk->ops->set_parent && !clk->ops->get_parent) {
+		pr_warning("%s: %s must implement .get_parent & .set_parent\n",
+				__func__, clk->name);
+		ret = -EINVAL;
 		goto out;
+	}
 
 	/* throw a WARN if any entries in parent_names are NULL */
 	for (i = 0; i < clk->num_parents; i++)
@@ -1302,48 +1305,130 @@ void __clk_init(struct device *dev, struct clk *clk)
 out:
 	mutex_unlock(&prepare_lock);
 
-	return;
+	return ret;
 }
 
 /**
+ * __clk_register - register a clock and return a cookie.
+ *
+ * Same as clk_register, except that the .clk field inside hw shall point to a
+ * preallocated (generally statically allocated) struct clk. None of the fields
+ * of the struct clk need to be initialized.
+ *
+ * The data pointed to by .init and .clk field shall NOT be marked as init
+ * data.
+ *
+ * __clk_register is only exposed via clk-private.h and is intended for use with
+ * very large numbers of clocks that need to be statically initialized.  It is
+ * a layering violation to include clk-private.h from any code which implements
+ * a clock's .ops; as such any statically initialized clock data MUST be in a
+ * separate C file from the logic that implements it's operations.  Returns 0
+ * on success, otherwise an error code.
+ */
+struct clk *__clk_register(struct device *dev, struct clk_hw *hw)
+{
+	int ret;
+	struct clk *clk;
+
+	clk = hw->clk;
+	clk->name = hw->init->name;
+	clk->ops = hw->init->ops;
+	clk->hw = hw;
+	clk->flags = hw->init->flags;
+	clk->parent_names = hw->init->parent_names;
+	clk->num_parents = hw->init->num_parents;
+
+	ret = __clk_init(dev, clk);
+	if (ret)
+		return ERR_PTR(ret);
+
+	return clk;
+}
+EXPORT_SYMBOL_GPL(__clk_register);
+
+/**
  * clk_register - allocate a new clock, register it and return an opaque cookie
  * @dev: device that is registering this clock
- * @name: clock name
- * @ops: operations this clock supports
  * @hw: link to hardware-specific clock data
- * @parent_names: array of string names for all possible parents
- * @num_parents: number of possible parents
- * @flags: framework-level hints and quirks
  *
  * clk_register is the primary interface for populating the clock tree with new
  * clock nodes.  It returns a pointer to the newly allocated struct clk which
  * cannot be dereferenced by driver code but may be used in conjuction with the
- * rest of the clock API.
+ * rest of the clock API.  In the event of an error clk_register will return an
+ * error code; drivers must test for an error code after calling clk_register.
  */
-struct clk *clk_register(struct device *dev, const char *name,
-		const struct clk_ops *ops, struct clk_hw *hw,
-		char **parent_names, u8 num_parents, unsigned long flags)
+struct clk *clk_register(struct device *dev, struct clk_hw *hw)
 {
+	int i, ret;
 	struct clk *clk;
 
 	clk = kzalloc(sizeof(*clk), GFP_KERNEL);
-	if (!clk)
-		return NULL;
+	if (!clk) {
+		pr_err("%s: could not allocate clk\n", __func__);
+		ret = -ENOMEM;
+		goto fail_out;
+	}
 
-	clk->name = name;
-	clk->ops = ops;
+	clk->name = kstrdup(hw->init->name, GFP_KERNEL);
+	if (!clk->name) {
+		pr_err("%s: could not allocate clk->name\n", __func__);
+		ret = -ENOMEM;
+		goto fail_name;
+	}
+	clk->ops = hw->init->ops;
 	clk->hw = hw;
-	clk->flags = flags;
-	clk->parent_names = parent_names;
-	clk->num_parents = num_parents;
+	clk->flags = hw->init->flags;
+	clk->num_parents = hw->init->num_parents;
 	hw->clk = clk;
 
-	__clk_init(dev, clk);
+	/* allocate local copy in case parent_names is __initdata */
+	clk->parent_names = kzalloc((sizeof(char*) * clk->num_parents),
+			GFP_KERNEL);
 
-	return clk;
+	if (!clk->parent_names) {
+		pr_err("%s: could not allocate clk->parent_names\n", __func__);
+		ret = -ENOMEM;
+		goto fail_parent_names;
+	}
+
+
+	/* copy each string name in case parent_names is __initdata */
+	for (i = 0; i < clk->num_parents; i++) {
+		clk->parent_names[i] = kstrdup(hw->init->parent_names[i],
+						GFP_KERNEL);
+		if (!clk->parent_names[i]) {
+			pr_err("%s: could not copy parent_names\n", __func__);
+			ret = -ENOMEM;
+			goto fail_parent_names_copy;
+		}
+	}
+
+	ret = __clk_init(dev, clk);
+	if (!ret)
+		return clk;
+
+fail_parent_names_copy:
+	while (--i >= 0)
+		kfree(clk->parent_names[i]);
+	kfree(clk->parent_names);
+fail_parent_names:
+	kfree(clk->name);
+fail_name:
+	kfree(clk);
+fail_out:
+	return ERR_PTR(ret);
 }
 EXPORT_SYMBOL_GPL(clk_register);
 
+/**
+ * clk_unregister - unregister a currently registered clock
+ * @clk: clock to unregister
+ *
+ * Currently unimplemented.
+ */
+void clk_unregister(struct clk *clk) {}
+EXPORT_SYMBOL_GPL(clk_unregister);
+
 /***        clk rate change notifiers        ***/
 
 /**
diff --git a/drivers/clk/clkdev.c b/drivers/clk/clkdev.c
index 6db161f64ae..c535cf8c577 100644
--- a/drivers/clk/clkdev.c
+++ b/drivers/clk/clkdev.c
@@ -35,7 +35,12 @@ static DEFINE_MUTEX(clocks_mutex);
 static struct clk_lookup *clk_find(const char *dev_id, const char *con_id)
 {
 	struct clk_lookup *p, *cl = NULL;
-	int match, best = 0;
+	int match, best_found = 0, best_possible = 0;
+
+	if (dev_id)
+		best_possible += 2;
+	if (con_id)
+		best_possible += 1;
 
 	list_for_each_entry(p, &clocks, node) {
 		match = 0;
@@ -50,10 +55,10 @@ static struct clk_lookup *clk_find(const char *dev_id, const char *con_id)
 			match += 1;
 		}
 
-		if (match > best) {
+		if (match > best_found) {
 			cl = p;
-			if (match != 3)
-				best = match;
+			if (match != best_possible)
+				best_found = match;
 			else
 				break;
 		}
@@ -89,6 +94,51 @@ void clk_put(struct clk *clk)
 }
 EXPORT_SYMBOL(clk_put);
 
+static void devm_clk_release(struct device *dev, void *res)
+{
+	clk_put(*(struct clk **)res);
+}
+
+struct clk *devm_clk_get(struct device *dev, const char *id)
+{
+	struct clk **ptr, *clk;
+
+	ptr = devres_alloc(devm_clk_release, sizeof(*ptr), GFP_KERNEL);
+	if (!ptr)
+		return ERR_PTR(-ENOMEM);
+
+	clk = clk_get(dev, id);
+	if (!IS_ERR(clk)) {
+		*ptr = clk;
+		devres_add(dev, ptr);
+	} else {
+		devres_free(ptr);
+	}
+
+	return clk;
+}
+EXPORT_SYMBOL(devm_clk_get);
+
+static int devm_clk_match(struct device *dev, void *res, void *data)
+{
+	struct clk **c = res;
+	if (!c || !*c) {
+		WARN_ON(!c || !*c);
+		return 0;
+	}
+	return *c == data;
+}
+
+void devm_clk_put(struct device *dev, struct clk *clk)
+{
+	int ret;
+
+	ret = devres_destroy(dev, devm_clk_release, devm_clk_match, clk);
+
+	WARN_ON(ret);
+}
+EXPORT_SYMBOL(devm_clk_put);
+
 void clkdev_add(struct clk_lookup *cl)
 {
 	mutex_lock(&clocks_mutex);
@@ -116,8 +166,9 @@ struct clk_lookup_alloc {
 	char	con_id[MAX_CON_ID];
 };
 
-struct clk_lookup * __init_refok
-clkdev_alloc(struct clk *clk, const char *con_id, const char *dev_fmt, ...)
+static struct clk_lookup * __init_refok
+vclkdev_alloc(struct clk *clk, const char *con_id, const char *dev_fmt,
+	va_list ap)
 {
 	struct clk_lookup_alloc *cla;
 
@@ -132,16 +183,25 @@ clkdev_alloc(struct clk *clk, const char *con_id, const char *dev_fmt, ...)
 	}
 
 	if (dev_fmt) {
-		va_list ap;
-
-		va_start(ap, dev_fmt);
 		vscnprintf(cla->dev_id, sizeof(cla->dev_id), dev_fmt, ap);
 		cla->cl.dev_id = cla->dev_id;
-		va_end(ap);
 	}
 
 	return &cla->cl;
 }
+
+struct clk_lookup * __init_refok
+clkdev_alloc(struct clk *clk, const char *con_id, const char *dev_fmt, ...)
+{
+	struct clk_lookup *cl;
+	va_list ap;
+
+	va_start(ap, dev_fmt);
+	cl = vclkdev_alloc(clk, con_id, dev_fmt, ap);
+	va_end(ap);
+
+	return cl;
+}
 EXPORT_SYMBOL(clkdev_alloc);
 
 int clk_add_alias(const char *alias, const char *alias_dev_name, char *id,
@@ -173,3 +233,65 @@ void clkdev_drop(struct clk_lookup *cl)
 	kfree(cl);
 }
 EXPORT_SYMBOL(clkdev_drop);
+
+/**
+ * clk_register_clkdev - register one clock lookup for a struct clk
+ * @clk: struct clk to associate with all clk_lookups
+ * @con_id: connection ID string on device
+ * @dev_id: format string describing device name
+ *
+ * con_id or dev_id may be NULL as a wildcard, just as in the rest of
+ * clkdev.
+ *
+ * To make things easier for mass registration, we detect error clks
+ * from a previous clk_register() call, and return the error code for
+ * those.  This is to permit this function to be called immediately
+ * after clk_register().
+ */
+int clk_register_clkdev(struct clk *clk, const char *con_id,
+	const char *dev_fmt, ...)
+{
+	struct clk_lookup *cl;
+	va_list ap;
+
+	if (IS_ERR(clk))
+		return PTR_ERR(clk);
+
+	va_start(ap, dev_fmt);
+	cl = vclkdev_alloc(clk, con_id, dev_fmt, ap);
+	va_end(ap);
+
+	if (!cl)
+		return -ENOMEM;
+
+	clkdev_add(cl);
+
+	return 0;
+}
+
+/**
+ * clk_register_clkdevs - register a set of clk_lookup for a struct clk
+ * @clk: struct clk to associate with all clk_lookups
+ * @cl: array of clk_lookup structures with con_id and dev_id pre-initialized
+ * @num: number of clk_lookup structures to register
+ *
+ * To make things easier for mass registration, we detect error clks
+ * from a previous clk_register() call, and return the error code for
+ * those.  This is to permit this function to be called immediately
+ * after clk_register().
+ */
+int clk_register_clkdevs(struct clk *clk, struct clk_lookup *cl, size_t num)
+{
+	unsigned i;
+
+	if (IS_ERR(clk))
+		return PTR_ERR(clk);
+
+	for (i = 0; i < num; i++, cl++) {
+		cl->clk = clk;
+		clkdev_add(cl);
+	}
+
+	return 0;
+}
+EXPORT_SYMBOL(clk_register_clkdevs);
diff --git a/drivers/clk/mxs/Makefile b/drivers/clk/mxs/Makefile
new file mode 100644
index 00000000000..7bedeec0852
--- /dev/null
+++ b/drivers/clk/mxs/Makefile
@@ -0,0 +1,8 @@
+#
+# Makefile for mxs specific clk
+#
+
+obj-y += clk.o clk-pll.o clk-ref.o clk-div.o clk-frac.o
+
+obj-$(CONFIG_SOC_IMX23) += clk-imx23.o
+obj-$(CONFIG_SOC_IMX28) += clk-imx28.o
diff --git a/drivers/clk/mxs/clk-div.c b/drivers/clk/mxs/clk-div.c
new file mode 100644
index 00000000000..90e1da93877
--- /dev/null
+++ b/drivers/clk/mxs/clk-div.c
@@ -0,0 +1,110 @@
+/*
+ * Copyright 2012 Freescale Semiconductor, Inc.
+ *
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/gpl-license.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+
+#include <linux/clk.h>
+#include <linux/clk-provider.h>
+#include <linux/err.h>
+#include <linux/slab.h>
+#include "clk.h"
+
+/**
+ * struct clk_div - mxs integer divider clock
+ * @divider: the parent class
+ * @ops: pointer to clk_ops of parent class
+ * @reg: register address
+ * @busy: busy bit shift
+ *
+ * The mxs divider clock is a subclass of basic clk_divider with an
+ * addtional busy bit.
+ */
+struct clk_div {
+	struct clk_divider divider;
+	const struct clk_ops *ops;
+	void __iomem *reg;
+	u8 busy;
+};
+
+static inline struct clk_div *to_clk_div(struct clk_hw *hw)
+{
+	struct clk_divider *divider = container_of(hw, struct clk_divider, hw);
+
+	return container_of(divider, struct clk_div, divider);
+}
+
+static unsigned long clk_div_recalc_rate(struct clk_hw *hw,
+					 unsigned long parent_rate)
+{
+	struct clk_div *div = to_clk_div(hw);
+
+	return div->ops->recalc_rate(&div->divider.hw, parent_rate);
+}
+
+static long clk_div_round_rate(struct clk_hw *hw, unsigned long rate,
+			       unsigned long *prate)
+{
+	struct clk_div *div = to_clk_div(hw);
+
+	return div->ops->round_rate(&div->divider.hw, rate, prate);
+}
+
+static int clk_div_set_rate(struct clk_hw *hw, unsigned long rate,
+			    unsigned long parent_rate)
+{
+	struct clk_div *div = to_clk_div(hw);
+	int ret;
+
+	ret = div->ops->set_rate(&div->divider.hw, rate, parent_rate);
+	if (!ret)
+		ret = mxs_clk_wait(div->reg, div->busy);
+
+	return ret;
+}
+
+static struct clk_ops clk_div_ops = {
+	.recalc_rate = clk_div_recalc_rate,
+	.round_rate = clk_div_round_rate,
+	.set_rate = clk_div_set_rate,
+};
+
+struct clk *mxs_clk_div(const char *name, const char *parent_name,
+			void __iomem *reg, u8 shift, u8 width, u8 busy)
+{
+	struct clk_div *div;
+	struct clk *clk;
+	struct clk_init_data init;
+
+	div = kzalloc(sizeof(*div), GFP_KERNEL);
+	if (!div)
+		return ERR_PTR(-ENOMEM);
+
+	init.name = name;
+	init.ops = &clk_div_ops;
+	init.flags = CLK_SET_RATE_PARENT;
+	init.parent_names = (parent_name ? &parent_name: NULL);
+	init.num_parents = (parent_name ? 1 : 0);
+
+	div->reg = reg;
+	div->busy = busy;
+
+	div->divider.reg = reg;
+	div->divider.shift = shift;
+	div->divider.width = width;
+	div->divider.flags = CLK_DIVIDER_ONE_BASED;
+	div->divider.lock = &mxs_lock;
+	div->divider.hw.init = &init;
+	div->ops = &clk_divider_ops;
+
+	clk = clk_register(NULL, &div->divider.hw);
+	if (IS_ERR(clk))
+		kfree(div);
+
+	return clk;
+}
diff --git a/drivers/clk/mxs/clk-frac.c b/drivers/clk/mxs/clk-frac.c
new file mode 100644
index 00000000000..e6aa6b567d6
--- /dev/null
+++ b/drivers/clk/mxs/clk-frac.c
@@ -0,0 +1,139 @@
+/*
+ * Copyright 2012 Freescale Semiconductor, Inc.
+ *
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/gpl-license.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+
+#include <linux/clk.h>
+#include <linux/clk-provider.h>
+#include <linux/err.h>
+#include <linux/io.h>
+#include <linux/slab.h>
+#include "clk.h"
+
+/**
+ * struct clk_frac - mxs fractional divider clock
+ * @hw: clk_hw for the fractional divider clock
+ * @reg: register address
+ * @shift: the divider bit shift
+ * @width: the divider bit width
+ * @busy: busy bit shift
+ *
+ * The clock is an adjustable fractional divider with a busy bit to wait
+ * when the divider is adjusted.
+ */
+struct clk_frac {
+	struct clk_hw hw;
+	void __iomem *reg;
+	u8 shift;
+	u8 width;
+	u8 busy;
+};
+
+#define to_clk_frac(_hw) container_of(_hw, struct clk_frac, hw)
+
+static unsigned long clk_frac_recalc_rate(struct clk_hw *hw,
+					  unsigned long parent_rate)
+{
+	struct clk_frac *frac = to_clk_frac(hw);
+	u32 div;
+
+	div = readl_relaxed(frac->reg) >> frac->shift;
+	div &= (1 << frac->width) - 1;
+
+	return (parent_rate >> frac->width) * div;
+}
+
+static long clk_frac_round_rate(struct clk_hw *hw, unsigned long rate,
+				unsigned long *prate)
+{
+	struct clk_frac *frac = to_clk_frac(hw);
+	unsigned long parent_rate = *prate;
+	u32 div;
+	u64 tmp;
+
+	if (rate > parent_rate)
+		return -EINVAL;
+
+	tmp = rate;
+	tmp <<= frac->width;
+	do_div(tmp, parent_rate);
+	div = tmp;
+
+	if (!div)
+		return -EINVAL;
+
+	return (parent_rate >> frac->width) * div;
+}
+
+static int clk_frac_set_rate(struct clk_hw *hw, unsigned long rate,
+			     unsigned long parent_rate)
+{
+	struct clk_frac *frac = to_clk_frac(hw);
+	unsigned long flags;
+	u32 div, val;
+	u64 tmp;
+
+	if (rate > parent_rate)
+		return -EINVAL;
+
+	tmp = rate;
+	tmp <<= frac->width;
+	do_div(tmp, parent_rate);
+	div = tmp;
+
+	if (!div)
+		return -EINVAL;
+
+	spin_lock_irqsave(&mxs_lock, flags);
+
+	val = readl_relaxed(frac->reg);
+	val &= ~(((1 << frac->width) - 1) << frac->shift);
+	val |= div << frac->shift;
+	writel_relaxed(val, frac->reg);
+
+	spin_unlock_irqrestore(&mxs_lock, flags);
+
+	return mxs_clk_wait(frac->reg, frac->busy);
+}
+
+static struct clk_ops clk_frac_ops = {
+	.recalc_rate = clk_frac_recalc_rate,
+	.round_rate = clk_frac_round_rate,
+	.set_rate = clk_frac_set_rate,
+};
+
+struct clk *mxs_clk_frac(const char *name, const char *parent_name,
+			 void __iomem *reg, u8 shift, u8 width, u8 busy)
+{
+	struct clk_frac *frac;
+	struct clk *clk;
+	struct clk_init_data init;
+
+	frac = kzalloc(sizeof(*frac), GFP_KERNEL);
+	if (!frac)
+		return ERR_PTR(-ENOMEM);
+
+	init.name = name;
+	init.ops = &clk_frac_ops;
+	init.flags = CLK_SET_RATE_PARENT;
+	init.parent_names = (parent_name ? &parent_name: NULL);
+	init.num_parents = (parent_name ? 1 : 0);
+
+	frac->reg = reg;
+	frac->shift = shift;
+	frac->width = width;
+	frac->busy = busy;
+	frac->hw.init = &init;
+
+	clk = clk_register(NULL, &frac->hw);
+	if (IS_ERR(clk))
+		kfree(frac);
+
+	return clk;
+}
diff --git a/drivers/clk/mxs/clk-imx23.c b/drivers/clk/mxs/clk-imx23.c
new file mode 100644
index 00000000000..f7be225f544
--- /dev/null
+++ b/drivers/clk/mxs/clk-imx23.c
@@ -0,0 +1,205 @@
+/*
+ * Copyright 2012 Freescale Semiconductor, Inc.
+ *
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/gpl-license.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+
+#include <linux/clk.h>
+#include <linux/clkdev.h>
+#include <linux/err.h>
+#include <linux/init.h>
+#include <linux/io.h>
+#include <mach/common.h>
+#include <mach/mx23.h>
+#include "clk.h"
+
+#define DIGCTRL			MX23_IO_ADDRESS(MX23_DIGCTL_BASE_ADDR)
+#define CLKCTRL			MX23_IO_ADDRESS(MX23_CLKCTRL_BASE_ADDR)
+#define PLLCTRL0		(CLKCTRL + 0x0000)
+#define CPU			(CLKCTRL + 0x0020)
+#define HBUS			(CLKCTRL + 0x0030)
+#define XBUS			(CLKCTRL + 0x0040)
+#define XTAL			(CLKCTRL + 0x0050)
+#define PIX			(CLKCTRL + 0x0060)
+#define SSP			(CLKCTRL + 0x0070)
+#define GPMI			(CLKCTRL + 0x0080)
+#define SPDIF			(CLKCTRL + 0x0090)
+#define EMI			(CLKCTRL + 0x00a0)
+#define SAIF			(CLKCTRL + 0x00c0)
+#define TV			(CLKCTRL + 0x00d0)
+#define ETM			(CLKCTRL + 0x00e0)
+#define FRAC			(CLKCTRL + 0x00f0)
+#define CLKSEQ			(CLKCTRL + 0x0110)
+
+#define BP_CPU_INTERRUPT_WAIT	12
+#define BP_CLKSEQ_BYPASS_SAIF	0
+#define BP_CLKSEQ_BYPASS_SSP	5
+#define BP_SAIF_DIV_FRAC_EN	16
+#define BP_FRAC_IOFRAC		24
+
+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);
+
+	/* Clear BYPASS for SAIF */
+	__mxs_clrl(1 << BP_CLKSEQ_BYPASS_SAIF, CLKSEQ);
+
+	/* SAIF has to use frac div for functional operation */
+	val = readl_relaxed(SAIF);
+	val |= 1 << BP_SAIF_DIV_FRAC_EN;
+	writel_relaxed(val, SAIF);
+
+	/*
+	 * 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);
+
+	/*
+	 * 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);
+}
+
+static struct clk_lookup uart_lookups[] __initdata = {
+	{ .dev_id = "duart", },
+	{ .dev_id = "mxs-auart.0", },
+	{ .dev_id = "mxs-auart.1", },
+	{ .dev_id = "8006c000.serial", },
+	{ .dev_id = "8006e000.serial", },
+	{ .dev_id = "80070000.serial", },
+};
+
+static struct clk_lookup hbus_lookups[] __initdata = {
+	{ .dev_id = "imx23-dma-apbh", },
+	{ .dev_id = "80004000.dma-apbh", },
+};
+
+static struct clk_lookup xbus_lookups[] __initdata = {
+	{ .dev_id = "duart", .con_id = "apb_pclk"},
+	{ .dev_id = "80070000.serial", .con_id = "apb_pclk"},
+	{ .dev_id = "imx23-dma-apbx", },
+	{ .dev_id = "80024000.dma-apbx", },
+};
+
+static struct clk_lookup ssp_lookups[] __initdata = {
+	{ .dev_id = "imx23-mmc.0", },
+	{ .dev_id = "imx23-mmc.1", },
+	{ .dev_id = "80010000.ssp", },
+	{ .dev_id = "80034000.ssp", },
+};
+
+static struct clk_lookup lcdif_lookups[] __initdata = {
+	{ .dev_id = "imx23-fb", },
+	{ .dev_id = "80030000.lcdif", },
+};
+
+static struct clk_lookup gpmi_lookups[] __initdata = {
+	{ .dev_id = "imx23-gpmi-nand", },
+	{ .dev_id = "8000c000.gpmi", },
+};
+
+static const char *sel_pll[]  __initconst = { "pll", "ref_xtal", };
+static const char *sel_cpu[]  __initconst = { "ref_cpu", "ref_xtal", };
+static const char *sel_pix[]  __initconst = { "ref_pix", "ref_xtal", };
+static const char *sel_io[]   __initconst = { "ref_io", "ref_xtal", };
+static const char *cpu_sels[] __initconst = { "cpu_pll", "cpu_xtal", };
+static const char *emi_sels[] __initconst = { "emi_pll", "emi_xtal", };
+
+enum imx23_clk {
+	ref_xtal, pll, ref_cpu, ref_emi, ref_pix, ref_io, saif_sel,
+	lcdif_sel, gpmi_sel, ssp_sel, emi_sel, cpu, etm_sel, cpu_pll,
+	cpu_xtal, hbus, xbus, lcdif_div, ssp_div, gpmi_div, emi_pll,
+	emi_xtal, etm_div, saif_div, clk32k_div, rtc, adc, spdif_div,
+	clk32k, dri, pwm, filt, uart, ssp, gpmi, spdif, emi, saif,
+	lcdif, etm, usb, usb_pwr,
+	clk_max
+};
+
+static struct clk *clks[clk_max];
+
+static enum imx23_clk clks_init_on[] __initdata = {
+	cpu, hbus, xbus, emi, uart,
+};
+
+int __init mx23_clocks_init(void)
+{
+	int i;
+
+	clk_misc_init();
+
+	clks[ref_xtal] = mxs_clk_fixed("ref_xtal", 24000000);
+	clks[pll] = mxs_clk_pll("pll", "ref_xtal", PLLCTRL0, 16, 480000000);
+	clks[ref_cpu] = mxs_clk_ref("ref_cpu", "pll", FRAC, 0);
+	clks[ref_emi] = mxs_clk_ref("ref_emi", "pll", FRAC, 1);
+	clks[ref_pix] = mxs_clk_ref("ref_pix", "pll", FRAC, 2);
+	clks[ref_io] = mxs_clk_ref("ref_io", "pll", FRAC, 3);
+	clks[saif_sel] = mxs_clk_mux("saif_sel", CLKSEQ, 0, 1, sel_pll, ARRAY_SIZE(sel_pll));
+	clks[lcdif_sel] = mxs_clk_mux("lcdif_sel", CLKSEQ, 1, 1, sel_pix, ARRAY_SIZE(sel_pix));
+	clks[gpmi_sel] = mxs_clk_mux("gpmi_sel", CLKSEQ, 4, 1, sel_io, ARRAY_SIZE(sel_io));
+	clks[ssp_sel] = mxs_clk_mux("ssp_sel", CLKSEQ, 5, 1, sel_io, ARRAY_SIZE(sel_io));
+	clks[emi_sel] = mxs_clk_mux("emi_sel", CLKSEQ, 6, 1, emi_sels, ARRAY_SIZE(emi_sels));
+	clks[cpu] = mxs_clk_mux("cpu", CLKSEQ, 7, 1, cpu_sels, ARRAY_SIZE(cpu_sels));
+	clks[etm_sel] = mxs_clk_mux("etm_sel", CLKSEQ, 8, 1, sel_cpu, ARRAY_SIZE(sel_cpu));
+	clks[cpu_pll] = mxs_clk_div("cpu_pll", "ref_cpu", CPU, 0, 6, 28);
+	clks[cpu_xtal] = mxs_clk_div("cpu_xtal", "ref_xtal", CPU, 16, 10, 29);
+	clks[hbus] = mxs_clk_div("hbus", "cpu", HBUS, 0, 5, 29);
+	clks[xbus] = mxs_clk_div("xbus", "ref_xtal", XBUS, 0, 10, 31);
+	clks[lcdif_div] = mxs_clk_div("lcdif_div", "lcdif_sel", PIX, 0, 12, 29);
+	clks[ssp_div] = mxs_clk_div("ssp_div", "ssp_sel", SSP, 0, 9, 29);
+	clks[gpmi_div] = mxs_clk_div("gpmi_div", "gpmi_sel", GPMI, 0, 10, 29);
+	clks[emi_pll] = mxs_clk_div("emi_pll", "ref_emi", EMI, 0, 6, 28);
+	clks[emi_xtal] = mxs_clk_div("emi_xtal", "ref_xtal", EMI, 8, 4, 29);
+	clks[etm_div] = mxs_clk_div("etm_div", "etm_sel", ETM, 0, 6, 29);
+	clks[saif_div] = mxs_clk_frac("saif_div", "saif_sel", SAIF, 0, 16, 29);
+	clks[clk32k_div] = mxs_clk_fixed_factor("clk32k_div", "ref_xtal", 1, 750);
+	clks[rtc] = mxs_clk_fixed_factor("rtc", "ref_xtal", 1, 768);
+	clks[adc] = mxs_clk_fixed_factor("adc", "clk32k", 1, 16);
+	clks[spdif_div] = mxs_clk_fixed_factor("spdif_div", "pll", 1, 4);
+	clks[clk32k] = mxs_clk_gate("clk32k", "clk32k_div", XTAL, 26);
+	clks[dri] = mxs_clk_gate("dri", "ref_xtal", XTAL, 28);
+	clks[pwm] = mxs_clk_gate("pwm", "ref_xtal", XTAL, 29);
+	clks[filt] = mxs_clk_gate("filt", "ref_xtal", XTAL, 30);
+	clks[uart] = mxs_clk_gate("uart", "ref_xtal", XTAL, 31);
+	clks[ssp] = mxs_clk_gate("ssp", "ssp_div", SSP, 31);
+	clks[gpmi] = mxs_clk_gate("gpmi", "gpmi_div", GPMI, 31);
+	clks[spdif] = mxs_clk_gate("spdif", "spdif_div", SPDIF, 31);
+	clks[emi] = mxs_clk_gate("emi", "emi_sel", EMI, 31);
+	clks[saif] = mxs_clk_gate("saif", "saif_div", SAIF, 31);
+	clks[lcdif] = mxs_clk_gate("lcdif", "lcdif_div", PIX, 31);
+	clks[etm] = mxs_clk_gate("etm", "etm_div", ETM, 31);
+	clks[usb] = mxs_clk_gate("usb", "usb_pwr", DIGCTRL, 2);
+	clks[usb_pwr] = clk_register_gate(NULL, "usb_pwr", "pll", 0, PLLCTRL0, 18, 0, &mxs_lock);
+
+	for (i = 0; i < ARRAY_SIZE(clks); i++)
+		if (IS_ERR(clks[i])) {
+			pr_err("i.MX23 clk %d: register failed with %ld\n",
+				i, PTR_ERR(clks[i]));
+			return PTR_ERR(clks[i]);
+		}
+
+	clk_register_clkdev(clks[clk32k], NULL, "timrot");
+	clk_register_clkdevs(clks[hbus], hbus_lookups, ARRAY_SIZE(hbus_lookups));
+	clk_register_clkdevs(clks[xbus], xbus_lookups, ARRAY_SIZE(xbus_lookups));
+	clk_register_clkdevs(clks[uart], uart_lookups, ARRAY_SIZE(uart_lookups));
+	clk_register_clkdevs(clks[ssp], ssp_lookups, ARRAY_SIZE(ssp_lookups));
+	clk_register_clkdevs(clks[gpmi], gpmi_lookups, ARRAY_SIZE(gpmi_lookups));
+	clk_register_clkdevs(clks[lcdif], lcdif_lookups, ARRAY_SIZE(lcdif_lookups));
+
+	for (i = 0; i < ARRAY_SIZE(clks_init_on); i++)
+		clk_prepare_enable(clks[clks_init_on[i]]);
+
+	mxs_timer_init(MX23_INT_TIMER0);
+
+	return 0;
+}
diff --git a/drivers/clk/mxs/clk-imx28.c b/drivers/clk/mxs/clk-imx28.c
new file mode 100644
index 00000000000..2826a2606a2
--- /dev/null
+++ b/drivers/clk/mxs/clk-imx28.c
@@ -0,0 +1,338 @@
+/*
+ * Copyright 2012 Freescale Semiconductor, Inc.
+ *
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/gpl-license.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+
+#include <linux/clk.h>
+#include <linux/clkdev.h>
+#include <linux/err.h>
+#include <linux/init.h>
+#include <linux/io.h>
+#include <mach/common.h>
+#include <mach/mx28.h>
+#include "clk.h"
+
+#define CLKCTRL			MX28_IO_ADDRESS(MX28_CLKCTRL_BASE_ADDR)
+#define PLL0CTRL0		(CLKCTRL + 0x0000)
+#define PLL1CTRL0		(CLKCTRL + 0x0020)
+#define PLL2CTRL0		(CLKCTRL + 0x0040)
+#define CPU			(CLKCTRL + 0x0050)
+#define HBUS			(CLKCTRL + 0x0060)
+#define XBUS			(CLKCTRL + 0x0070)
+#define XTAL			(CLKCTRL + 0x0080)
+#define SSP0			(CLKCTRL + 0x0090)
+#define SSP1			(CLKCTRL + 0x00a0)
+#define SSP2			(CLKCTRL + 0x00b0)
+#define SSP3			(CLKCTRL + 0x00c0)
+#define GPMI			(CLKCTRL + 0x00d0)
+#define SPDIF			(CLKCTRL + 0x00e0)
+#define EMI			(CLKCTRL + 0x00f0)
+#define SAIF0			(CLKCTRL + 0x0100)
+#define SAIF1			(CLKCTRL + 0x0110)
+#define LCDIF			(CLKCTRL + 0x0120)
+#define ETM			(CLKCTRL + 0x0130)
+#define ENET			(CLKCTRL + 0x0140)
+#define FLEXCAN			(CLKCTRL + 0x0160)
+#define FRAC0			(CLKCTRL + 0x01b0)
+#define FRAC1			(CLKCTRL + 0x01c0)
+#define CLKSEQ			(CLKCTRL + 0x01d0)
+
+#define BP_CPU_INTERRUPT_WAIT	12
+#define BP_SAIF_DIV_FRAC_EN	16
+#define BP_ENET_DIV_TIME	21
+#define BP_ENET_SLEEP		31
+#define BP_CLKSEQ_BYPASS_SAIF0	0
+#define BP_CLKSEQ_BYPASS_SSP0	3
+#define BP_FRAC0_IO1FRAC	16
+#define BP_FRAC0_IO0FRAC	24
+
+#define DIGCTRL			MX28_IO_ADDRESS(MX28_DIGCTL_BASE_ADDR)
+#define BP_SAIF_CLKMUX		10
+
+/*
+ * HW_SAIF_CLKMUX_SEL:
+ *  DIRECT(0x0): SAIF0 clock pins selected for SAIF0 input clocks, and SAIF1
+ *		clock pins selected for SAIF1 input clocks.
+ *  CROSSINPUT(0x1): SAIF1 clock inputs selected for SAIF0 input clocks, and
+ *		SAIF0 clock inputs selected for SAIF1 input clocks.
+ *  EXTMSTR0(0x2): SAIF0 clock pin selected for both SAIF0 and SAIF1 input
+ *		clocks.
+ *  EXTMSTR1(0x3): SAIF1 clock pin selected for both SAIF0 and SAIF1 input
+ *		clocks.
+ */
+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);
+
+	return 0;
+}
+
+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);
+
+	/* 0 is a bad default value for a divider */
+	__mxs_setl(1 << BP_ENET_DIV_TIME, ENET);
+
+	/* Clear BYPASS for SAIF */
+	__mxs_clrl(0x3 << BP_CLKSEQ_BYPASS_SAIF0, CLKSEQ);
+
+	/* SAIF has to use frac div for functional operation */
+	val = readl_relaxed(SAIF0);
+	val |= 1 << BP_SAIF_DIV_FRAC_EN;
+	writel_relaxed(val, SAIF0);
+
+	val = readl_relaxed(SAIF1);
+	val |= 1 << BP_SAIF_DIV_FRAC_EN;
+	writel_relaxed(val, SAIF1);
+
+	/* Extra fec clock setting */
+	val = readl_relaxed(ENET);
+	val &= ~(1 << BP_ENET_SLEEP);
+	writel_relaxed(val, ENET);
+
+	/*
+	 * 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);
+
+	/*
+	 * 480 MHz seems too high to be ssp clock source directly,
+	 * so set frac0 to get a 288 MHz ref_io0.
+	 */
+	val = readl_relaxed(FRAC0);
+	val &= ~(0x3f << BP_FRAC0_IO0FRAC);
+	val |= 30 << BP_FRAC0_IO0FRAC;
+	writel_relaxed(val, FRAC0);
+}
+
+static struct clk_lookup uart_lookups[] __initdata = {
+	{ .dev_id = "duart", },
+	{ .dev_id = "mxs-auart.0", },
+	{ .dev_id = "mxs-auart.1", },
+	{ .dev_id = "mxs-auart.2", },
+	{ .dev_id = "mxs-auart.3", },
+	{ .dev_id = "mxs-auart.4", },
+	{ .dev_id = "8006a000.serial", },
+	{ .dev_id = "8006c000.serial", },
+	{ .dev_id = "8006e000.serial", },
+	{ .dev_id = "80070000.serial", },
+	{ .dev_id = "80072000.serial", },
+	{ .dev_id = "80074000.serial", },
+};
+
+static struct clk_lookup hbus_lookups[] __initdata = {
+	{ .dev_id = "imx28-dma-apbh", },
+	{ .dev_id = "80004000.dma-apbh", },
+};
+
+static struct clk_lookup xbus_lookups[] __initdata = {
+	{ .dev_id = "duart", .con_id = "apb_pclk"},
+	{ .dev_id = "80074000.serial", .con_id = "apb_pclk"},
+	{ .dev_id = "imx28-dma-apbx", },
+	{ .dev_id = "80024000.dma-apbx", },
+};
+
+static struct clk_lookup ssp0_lookups[] __initdata = {
+	{ .dev_id = "imx28-mmc.0", },
+	{ .dev_id = "80010000.ssp", },
+};
+
+static struct clk_lookup ssp1_lookups[] __initdata = {
+	{ .dev_id = "imx28-mmc.1", },
+	{ .dev_id = "80012000.ssp", },
+};
+
+static struct clk_lookup ssp2_lookups[] __initdata = {
+	{ .dev_id = "imx28-mmc.2", },
+	{ .dev_id = "80014000.ssp", },
+};
+
+static struct clk_lookup ssp3_lookups[] __initdata = {
+	{ .dev_id = "imx28-mmc.3", },
+	{ .dev_id = "80016000.ssp", },
+};
+
+static struct clk_lookup lcdif_lookups[] __initdata = {
+	{ .dev_id = "imx28-fb", },
+	{ .dev_id = "80030000.lcdif", },
+};
+
+static struct clk_lookup gpmi_lookups[] __initdata = {
+	{ .dev_id = "imx28-gpmi-nand", },
+	{ .dev_id = "8000c000.gpmi", },
+};
+
+static struct clk_lookup fec_lookups[] __initdata = {
+	{ .dev_id = "imx28-fec.0", },
+	{ .dev_id = "imx28-fec.1", },
+	{ .dev_id = "800f0000.ethernet", },
+	{ .dev_id = "800f4000.ethernet", },
+};
+
+static struct clk_lookup can0_lookups[] __initdata = {
+	{ .dev_id = "flexcan.0", },
+	{ .dev_id = "80032000.can", },
+};
+
+static struct clk_lookup can1_lookups[] __initdata = {
+	{ .dev_id = "flexcan.1", },
+	{ .dev_id = "80034000.can", },
+};
+
+static struct clk_lookup saif0_lookups[] __initdata = {
+	{ .dev_id = "mxs-saif.0", },
+	{ .dev_id = "80042000.saif", },
+};
+
+static struct clk_lookup saif1_lookups[] __initdata = {
+	{ .dev_id = "mxs-saif.1", },
+	{ .dev_id = "80046000.saif", },
+};
+
+static const char *sel_cpu[]  __initconst = { "ref_cpu", "ref_xtal", };
+static const char *sel_io0[]  __initconst = { "ref_io0", "ref_xtal", };
+static const char *sel_io1[]  __initconst = { "ref_io1", "ref_xtal", };
+static const char *sel_pix[]  __initconst = { "ref_pix", "ref_xtal", };
+static const char *sel_gpmi[] __initconst = { "ref_gpmi", "ref_xtal", };
+static const char *sel_pll0[] __initconst = { "pll0", "ref_xtal", };
+static const char *cpu_sels[] __initconst = { "cpu_pll", "cpu_xtal", };
+static const char *emi_sels[] __initconst = { "emi_pll", "emi_xtal", };
+static const char *ptp_sels[] __initconst = { "ref_xtal", "pll0", };
+
+enum imx28_clk {
+	ref_xtal, pll0, pll1, pll2, ref_cpu, ref_emi, ref_io0, ref_io1,
+	ref_pix, ref_hsadc, ref_gpmi, saif0_sel, saif1_sel, gpmi_sel,
+	ssp0_sel, ssp1_sel, ssp2_sel, ssp3_sel, emi_sel, etm_sel,
+	lcdif_sel, cpu, ptp_sel, cpu_pll, cpu_xtal, hbus, xbus,
+	ssp0_div, ssp1_div, ssp2_div, ssp3_div, gpmi_div, emi_pll,
+	emi_xtal, lcdif_div, etm_div, ptp, saif0_div, saif1_div,
+	clk32k_div, rtc, lradc, spdif_div, clk32k, pwm, uart, ssp0,
+	ssp1, ssp2, ssp3, gpmi, spdif, emi, saif0, saif1, lcdif, etm,
+	fec, can0, can1, usb0, usb1, usb0_pwr, usb1_pwr, enet_out,
+	clk_max
+};
+
+static struct clk *clks[clk_max];
+
+static enum imx28_clk clks_init_on[] __initdata = {
+	cpu, hbus, xbus, emi, uart,
+};
+
+int __init mx28_clocks_init(void)
+{
+	int i;
+
+	clk_misc_init();
+
+	clks[ref_xtal] = mxs_clk_fixed("ref_xtal", 24000000);
+	clks[pll0] = mxs_clk_pll("pll0", "ref_xtal", PLL0CTRL0, 17, 480000000);
+	clks[pll1] = mxs_clk_pll("pll1", "ref_xtal", PLL1CTRL0, 17, 480000000);
+	clks[pll2] = mxs_clk_pll("pll2", "ref_xtal", PLL2CTRL0, 23, 50000000);
+	clks[ref_cpu] = mxs_clk_ref("ref_cpu", "pll0", FRAC0, 0);
+	clks[ref_emi] = mxs_clk_ref("ref_emi", "pll0", FRAC0, 1);
+	clks[ref_io0] = mxs_clk_ref("ref_io0", "pll0", FRAC0, 2);
+	clks[ref_io1] = mxs_clk_ref("ref_io1", "pll0", FRAC0, 3);
+	clks[ref_pix] = mxs_clk_ref("ref_pix", "pll0", FRAC1, 0);
+	clks[ref_hsadc] = mxs_clk_ref("ref_hsadc", "pll0", FRAC1, 1);
+	clks[ref_gpmi] = mxs_clk_ref("ref_gpmi", "pll0", FRAC1, 2);
+	clks[saif0_sel] = mxs_clk_mux("saif0_sel", CLKSEQ, 0, 1, sel_pll0, ARRAY_SIZE(sel_pll0));
+	clks[saif1_sel] = mxs_clk_mux("saif1_sel", CLKSEQ, 1, 1, sel_pll0, ARRAY_SIZE(sel_pll0));
+	clks[gpmi_sel] = mxs_clk_mux("gpmi_sel", CLKSEQ, 2, 1, sel_gpmi, ARRAY_SIZE(sel_gpmi));
+	clks[ssp0_sel] = mxs_clk_mux("ssp0_sel", CLKSEQ, 3, 1, sel_io0, ARRAY_SIZE(sel_io0));
+	clks[ssp1_sel] = mxs_clk_mux("ssp1_sel", CLKSEQ, 4, 1, sel_io0, ARRAY_SIZE(sel_io0));
+	clks[ssp2_sel] = mxs_clk_mux("ssp2_sel", CLKSEQ, 5, 1, sel_io1, ARRAY_SIZE(sel_io1));
+	clks[ssp3_sel] = mxs_clk_mux("ssp3_sel", CLKSEQ, 6, 1, sel_io1, ARRAY_SIZE(sel_io1));
+	clks[emi_sel] = mxs_clk_mux("emi_sel", CLKSEQ, 7, 1, emi_sels, ARRAY_SIZE(emi_sels));
+	clks[etm_sel] = mxs_clk_mux("etm_sel", CLKSEQ, 8, 1, sel_cpu, ARRAY_SIZE(sel_cpu));
+	clks[lcdif_sel] = mxs_clk_mux("lcdif_sel", CLKSEQ, 14, 1, sel_pix, ARRAY_SIZE(sel_pix));
+	clks[cpu] = mxs_clk_mux("cpu", CLKSEQ, 18, 1, cpu_sels, ARRAY_SIZE(cpu_sels));
+	clks[ptp_sel] = mxs_clk_mux("ptp_sel", ENET, 19, 1, ptp_sels, ARRAY_SIZE(ptp_sels));
+	clks[cpu_pll] = mxs_clk_div("cpu_pll", "ref_cpu", CPU, 0, 6, 28);
+	clks[cpu_xtal] = mxs_clk_div("cpu_xtal", "ref_xtal", CPU, 16, 10, 29);
+	clks[hbus] = mxs_clk_div("hbus", "cpu", HBUS, 0, 5, 31);
+	clks[xbus] = mxs_clk_div("xbus", "ref_xtal", XBUS, 0, 10, 31);
+	clks[ssp0_div] = mxs_clk_div("ssp0_div", "ssp0_sel", SSP0, 0, 9, 29);
+	clks[ssp1_div] = mxs_clk_div("ssp1_div", "ssp1_sel", SSP1, 0, 9, 29);
+	clks[ssp2_div] = mxs_clk_div("ssp2_div", "ssp2_sel", SSP2, 0, 9, 29);
+	clks[ssp3_div] = mxs_clk_div("ssp3_div", "ssp3_sel", SSP3, 0, 9, 29);
+	clks[gpmi_div] = mxs_clk_div("gpmi_div", "gpmi_sel", GPMI, 0, 10, 29);
+	clks[emi_pll] = mxs_clk_div("emi_pll", "ref_emi", EMI, 0, 6, 28);
+	clks[emi_xtal] = mxs_clk_div("emi_xtal", "ref_xtal", EMI, 8, 4, 29);
+	clks[lcdif_div] = mxs_clk_div("lcdif_div", "lcdif_sel", LCDIF, 0, 13, 29);
+	clks[etm_div] = mxs_clk_div("etm_div", "etm_sel", ETM, 0, 7, 29);
+	clks[ptp] = mxs_clk_div("ptp", "ptp_sel", ENET, 21, 6, 27);
+	clks[saif0_div] = mxs_clk_frac("saif0_div", "saif0_sel", SAIF0, 0, 16, 29);
+	clks[saif1_div] = mxs_clk_frac("saif1_div", "saif1_sel", SAIF1, 0, 16, 29);
+	clks[clk32k_div] = mxs_clk_fixed_factor("clk32k_div", "ref_xtal", 1, 750);
+	clks[rtc] = mxs_clk_fixed_factor("rtc", "ref_xtal", 1, 768);
+	clks[lradc] = mxs_clk_fixed_factor("lradc", "clk32k", 1, 16);
+	clks[spdif_div] = mxs_clk_fixed_factor("spdif_div", "pll0", 1, 4);
+	clks[clk32k] = mxs_clk_gate("clk32k", "clk32k_div", XTAL, 26);
+	clks[pwm] = mxs_clk_gate("pwm", "ref_xtal", XTAL, 29);
+	clks[uart] = mxs_clk_gate("uart", "ref_xtal", XTAL, 31);
+	clks[ssp0] = mxs_clk_gate("ssp0", "ssp0_div", SSP0, 31);
+	clks[ssp1] = mxs_clk_gate("ssp1", "ssp1_div", SSP1, 31);
+	clks[ssp2] = mxs_clk_gate("ssp2", "ssp2_div", SSP2, 31);
+	clks[ssp3] = mxs_clk_gate("ssp3", "ssp3_div", SSP3, 31);
+	clks[gpmi] = mxs_clk_gate("gpmi", "gpmi_div", GPMI, 31);
+	clks[spdif] = mxs_clk_gate("spdif", "spdif_div", SPDIF, 31);
+	clks[emi] = mxs_clk_gate("emi", "emi_sel", EMI, 31);
+	clks[saif0] = mxs_clk_gate("saif0", "saif0_div", SAIF0, 31);
+	clks[saif1] = mxs_clk_gate("saif1", "saif1_div", SAIF1, 31);
+	clks[lcdif] = mxs_clk_gate("lcdif", "lcdif_div", LCDIF, 31);
+	clks[etm] = mxs_clk_gate("etm", "etm_div", ETM, 31);
+	clks[fec] = mxs_clk_gate("fec", "hbus", ENET, 30);
+	clks[can0] = mxs_clk_gate("can0", "ref_xtal", FLEXCAN, 30);
+	clks[can1] = mxs_clk_gate("can1", "ref_xtal", FLEXCAN, 28);
+	clks[usb0] = mxs_clk_gate("usb0", "usb0_pwr", DIGCTRL, 2);
+	clks[usb1] = mxs_clk_gate("usb1", "usb1_pwr", DIGCTRL, 16);
+	clks[usb0_pwr] = clk_register_gate(NULL, "usb0_pwr", "pll0", 0, PLL0CTRL0, 18, 0, &mxs_lock);
+	clks[usb1_pwr] = clk_register_gate(NULL, "usb1_pwr", "pll1", 0, PLL1CTRL0, 18, 0, &mxs_lock);
+	clks[enet_out] = clk_register_gate(NULL, "enet_out", "pll2", 0, ENET, 18, 0, &mxs_lock);
+
+	for (i = 0; i < ARRAY_SIZE(clks); i++)
+		if (IS_ERR(clks[i])) {
+			pr_err("i.MX28 clk %d: register failed with %ld\n",
+				i, PTR_ERR(clks[i]));
+			return PTR_ERR(clks[i]);
+		}
+
+	clk_register_clkdev(clks[clk32k], NULL, "timrot");
+	clk_register_clkdev(clks[enet_out], NULL, "enet_out");
+	clk_register_clkdevs(clks[hbus], hbus_lookups, ARRAY_SIZE(hbus_lookups));
+	clk_register_clkdevs(clks[xbus], xbus_lookups, ARRAY_SIZE(xbus_lookups));
+	clk_register_clkdevs(clks[uart], uart_lookups, ARRAY_SIZE(uart_lookups));
+	clk_register_clkdevs(clks[ssp0], ssp0_lookups, ARRAY_SIZE(ssp0_lookups));
+	clk_register_clkdevs(clks[ssp1], ssp1_lookups, ARRAY_SIZE(ssp1_lookups));
+	clk_register_clkdevs(clks[ssp2], ssp2_lookups, ARRAY_SIZE(ssp2_lookups));
+	clk_register_clkdevs(clks[ssp3], ssp3_lookups, ARRAY_SIZE(ssp3_lookups));
+	clk_register_clkdevs(clks[gpmi], gpmi_lookups, ARRAY_SIZE(gpmi_lookups));
+	clk_register_clkdevs(clks[saif0], saif0_lookups, ARRAY_SIZE(saif0_lookups));
+	clk_register_clkdevs(clks[saif1], saif1_lookups, ARRAY_SIZE(saif1_lookups));
+	clk_register_clkdevs(clks[lcdif], lcdif_lookups, ARRAY_SIZE(lcdif_lookups));
+	clk_register_clkdevs(clks[fec], fec_lookups, ARRAY_SIZE(fec_lookups));
+	clk_register_clkdevs(clks[can0], can0_lookups, ARRAY_SIZE(can0_lookups));
+	clk_register_clkdevs(clks[can1], can1_lookups, ARRAY_SIZE(can1_lookups));
+
+	for (i = 0; i < ARRAY_SIZE(clks_init_on); i++)
+		clk_prepare_enable(clks[clks_init_on[i]]);
+
+	mxs_timer_init(MX28_INT_TIMER0);
+
+	return 0;
+}
diff --git a/drivers/clk/mxs/clk-pll.c b/drivers/clk/mxs/clk-pll.c
new file mode 100644
index 00000000000..fadae41833e
--- /dev/null
+++ b/drivers/clk/mxs/clk-pll.c
@@ -0,0 +1,116 @@
+/*
+ * Copyright 2012 Freescale Semiconductor, Inc.
+ *
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/gpl-license.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+
+#include <linux/clk.h>
+#include <linux/clk-provider.h>
+#include <linux/delay.h>
+#include <linux/err.h>
+#include <linux/io.h>
+#include <linux/slab.h>
+#include "clk.h"
+
+/**
+ * struct clk_pll - mxs pll clock
+ * @hw: clk_hw for the pll
+ * @base: base address of the pll
+ * @power: the shift of power bit
+ * @rate: the clock rate of the pll
+ *
+ * The mxs pll is a fixed rate clock with power and gate control,
+ * and the shift of gate bit is always 31.
+ */
+struct clk_pll {
+	struct clk_hw hw;
+	void __iomem *base;
+	u8 power;
+	unsigned long rate;
+};
+
+#define to_clk_pll(_hw) container_of(_hw, struct clk_pll, hw)
+
+static int clk_pll_prepare(struct clk_hw *hw)
+{
+	struct clk_pll *pll = to_clk_pll(hw);
+
+	writel_relaxed(1 << pll->power, pll->base + SET);
+
+	udelay(10);
+
+	return 0;
+}
+
+static void clk_pll_unprepare(struct clk_hw *hw)
+{
+	struct clk_pll *pll = to_clk_pll(hw);
+
+	writel_relaxed(1 << pll->power, pll->base + CLR);
+}
+
+static int clk_pll_enable(struct clk_hw *hw)
+{
+	struct clk_pll *pll = to_clk_pll(hw);
+
+	writel_relaxed(1 << 31, pll->base + CLR);
+
+	return 0;
+}
+
+static void clk_pll_disable(struct clk_hw *hw)
+{
+	struct clk_pll *pll = to_clk_pll(hw);
+
+	writel_relaxed(1 << 31, pll->base + SET);
+}
+
+static unsigned long clk_pll_recalc_rate(struct clk_hw *hw,
+					 unsigned long parent_rate)
+{
+	struct clk_pll *pll = to_clk_pll(hw);
+
+	return pll->rate;
+}
+
+static const struct clk_ops clk_pll_ops = {
+	.prepare = clk_pll_prepare,
+	.unprepare = clk_pll_unprepare,
+	.enable = clk_pll_enable,
+	.disable = clk_pll_disable,
+	.recalc_rate = clk_pll_recalc_rate,
+};
+
+struct clk *mxs_clk_pll(const char *name, const char *parent_name,
+			void __iomem *base, u8 power, unsigned long rate)
+{
+	struct 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 = &clk_pll_ops;
+	init.flags = 0;
+	init.parent_names = (parent_name ? &parent_name: NULL);
+	init.num_parents = (parent_name ? 1 : 0);
+
+	pll->base = base;
+	pll->rate = rate;
+	pll->power = power;
+	pll->hw.init = &init;
+
+	clk = clk_register(NULL, &pll->hw);
+	if (IS_ERR(clk))
+		kfree(pll);
+
+	return clk;
+}
diff --git a/drivers/clk/mxs/clk-ref.c b/drivers/clk/mxs/clk-ref.c
new file mode 100644
index 00000000000..4adeed6c2f9
--- /dev/null
+++ b/drivers/clk/mxs/clk-ref.c
@@ -0,0 +1,154 @@
+/*
+ * Copyright 2012 Freescale Semiconductor, Inc.
+ *
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/gpl-license.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+
+#include <linux/clk.h>
+#include <linux/clk-provider.h>
+#include <linux/err.h>
+#include <linux/io.h>
+#include <linux/slab.h>
+#include "clk.h"
+
+/**
+ * struct clk_ref - mxs reference clock
+ * @hw: clk_hw for the reference clock
+ * @reg: register address
+ * @idx: the index of the reference clock within the same register
+ *
+ * The mxs reference clock sources from pll.  Every 4 reference clocks share
+ * one register space, and @idx is used to identify them.  Each reference
+ * clock has a gate control and a fractional * divider.  The rate is calculated
+ * as pll rate  * (18 / FRAC), where FRAC = 18 ~ 35.
+ */
+struct clk_ref {
+	struct clk_hw hw;
+	void __iomem *reg;
+	u8 idx;
+};
+
+#define to_clk_ref(_hw) container_of(_hw, struct clk_ref, hw)
+
+static int clk_ref_enable(struct clk_hw *hw)
+{
+	struct clk_ref *ref = to_clk_ref(hw);
+
+	writel_relaxed(1 << ((ref->idx + 1) * 8 - 1), ref->reg + CLR);
+
+	return 0;
+}
+
+static void clk_ref_disable(struct clk_hw *hw)
+{
+	struct clk_ref *ref = to_clk_ref(hw);
+
+	writel_relaxed(1 << ((ref->idx + 1) * 8 - 1), ref->reg + SET);
+}
+
+static unsigned long clk_ref_recalc_rate(struct clk_hw *hw,
+					 unsigned long parent_rate)
+{
+	struct clk_ref *ref = to_clk_ref(hw);
+	u64 tmp = parent_rate;
+	u8 frac = (readl_relaxed(ref->reg) >> (ref->idx * 8)) & 0x3f;
+
+	tmp *= 18;
+	do_div(tmp, frac);
+
+	return tmp;
+}
+
+static long clk_ref_round_rate(struct clk_hw *hw, unsigned long rate,
+			       unsigned long *prate)
+{
+	unsigned long parent_rate = *prate;
+	u64 tmp = parent_rate;
+	u8 frac;
+
+	tmp = tmp * 18 + rate / 2;
+	do_div(tmp, rate);
+	frac = tmp;
+
+	if (frac < 18)
+		frac = 18;
+	else if (frac > 35)
+		frac = 35;
+
+	tmp = parent_rate;
+	tmp *= 18;
+	do_div(tmp, frac);
+
+	return tmp;
+}
+
+static int clk_ref_set_rate(struct clk_hw *hw, unsigned long rate,
+			    unsigned long parent_rate)
+{
+	struct clk_ref *ref = to_clk_ref(hw);
+	unsigned long flags;
+	u64 tmp = parent_rate;
+	u32 val;
+	u8 frac, shift = ref->idx * 8;
+
+	tmp = tmp * 18 + rate / 2;
+	do_div(tmp, rate);
+	frac = tmp;
+
+	if (frac < 18)
+		frac = 18;
+	else if (frac > 35)
+		frac = 35;
+
+	spin_lock_irqsave(&mxs_lock, flags);
+
+	val = readl_relaxed(ref->reg);
+	val &= ~(0x3f << shift);
+	val |= frac << shift;
+	writel_relaxed(val, ref->reg);
+
+	spin_unlock_irqrestore(&mxs_lock, flags);
+
+	return 0;
+}
+
+static const struct clk_ops clk_ref_ops = {
+	.enable		= clk_ref_enable,
+	.disable	= clk_ref_disable,
+	.recalc_rate	= clk_ref_recalc_rate,
+	.round_rate	= clk_ref_round_rate,
+	.set_rate	= clk_ref_set_rate,
+};
+
+struct clk *mxs_clk_ref(const char *name, const char *parent_name,
+			void __iomem *reg, u8 idx)
+{
+	struct clk_ref *ref;
+	struct clk *clk;
+	struct clk_init_data init;
+
+	ref = kzalloc(sizeof(*ref), GFP_KERNEL);
+	if (!ref)
+		return ERR_PTR(-ENOMEM);
+
+	init.name = name;
+	init.ops = &clk_ref_ops;
+	init.flags = 0;
+	init.parent_names = (parent_name ? &parent_name: NULL);
+	init.num_parents = (parent_name ? 1 : 0);
+
+	ref->reg = reg;
+	ref->idx = idx;
+	ref->hw.init = &init;
+
+	clk = clk_register(NULL, &ref->hw);
+	if (IS_ERR(clk))
+		kfree(ref);
+
+	return clk;
+}
diff --git a/drivers/clk/mxs/clk.c b/drivers/clk/mxs/clk.c
new file mode 100644
index 00000000000..b24d56067c8
--- /dev/null
+++ b/drivers/clk/mxs/clk.c
@@ -0,0 +1,28 @@
+/*
+ * Copyright 2012 Freescale Semiconductor, Inc.
+ *
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/gpl-license.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+
+#include <linux/err.h>
+#include <linux/io.h>
+#include <linux/jiffies.h>
+#include <linux/spinlock.h>
+
+DEFINE_SPINLOCK(mxs_lock);
+
+int mxs_clk_wait(void __iomem *reg, u8 shift)
+{
+	unsigned long timeout = jiffies + msecs_to_jiffies(10);
+
+	while (readl_relaxed(reg) & (1 << shift))
+		if (time_after(jiffies, timeout))
+			return -ETIMEDOUT;
+
+	return 0;
+}
diff --git a/drivers/clk/mxs/clk.h b/drivers/clk/mxs/clk.h
new file mode 100644
index 00000000000..81421e28e69
--- /dev/null
+++ b/drivers/clk/mxs/clk.h
@@ -0,0 +1,66 @@
+/*
+ * Copyright 2012 Freescale Semiconductor, Inc.
+ *
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/gpl-license.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+
+#ifndef __MXS_CLK_H
+#define __MXS_CLK_H
+
+#include <linux/clk.h>
+#include <linux/clk-provider.h>
+#include <linux/spinlock.h>
+
+#define SET	0x4
+#define CLR	0x8
+
+extern spinlock_t mxs_lock;
+
+int mxs_clk_wait(void __iomem *reg, u8 shift);
+
+struct clk *mxs_clk_pll(const char *name, const char *parent_name,
+			void __iomem *base, u8 power, unsigned long rate);
+
+struct clk *mxs_clk_ref(const char *name, const char *parent_name,
+			void __iomem *reg, u8 idx);
+
+struct clk *mxs_clk_div(const char *name, const char *parent_name,
+			void __iomem *reg, u8 shift, u8 width, u8 busy);
+
+struct clk *mxs_clk_frac(const char *name, const char *parent_name,
+			 void __iomem *reg, u8 shift, u8 width, u8 busy);
+
+static inline struct clk *mxs_clk_fixed(const char *name, int rate)
+{
+	return clk_register_fixed_rate(NULL, name, NULL, CLK_IS_ROOT, rate);
+}
+
+static inline struct clk *mxs_clk_gate(const char *name,
+			const char *parent_name, void __iomem *reg, u8 shift)
+{
+	return clk_register_gate(NULL, name, parent_name, CLK_SET_RATE_PARENT,
+				 reg, shift, CLK_GATE_SET_TO_DISABLE,
+				 &mxs_lock);
+}
+
+static inline struct clk *mxs_clk_mux(const char *name, void __iomem *reg,
+		u8 shift, u8 width, const char **parent_names, int num_parents)
+{
+	return clk_register_mux(NULL, name, parent_names, num_parents,
+				CLK_SET_RATE_PARENT, reg, shift, width,
+				0, &mxs_lock);
+}
+
+static inline struct clk *mxs_clk_fixed_factor(const char *name,
+		const char *parent_name, unsigned int mult, unsigned int div)
+{
+	return clk_register_fixed_factor(NULL, name, parent_name,
+					 CLK_SET_RATE_PARENT, mult, div);
+}
+
+#endif /* __MXS_CLK_H */
diff --git a/drivers/clk/spear/Makefile b/drivers/clk/spear/Makefile
new file mode 100644
index 00000000000..cdb425d3b8e
--- /dev/null
+++ b/drivers/clk/spear/Makefile
@@ -0,0 +1,10 @@
+#
+# SPEAr Clock specific Makefile
+#
+
+obj-y	+= clk.o clk-aux-synth.o clk-frac-synth.o clk-gpt-synth.o clk-vco-pll.o
+
+obj-$(CONFIG_ARCH_SPEAR3XX)	+= spear3xx_clock.o
+obj-$(CONFIG_ARCH_SPEAR6XX)	+= spear6xx_clock.o
+obj-$(CONFIG_MACH_SPEAR1310)	+= spear1310_clock.o
+obj-$(CONFIG_MACH_SPEAR1340)	+= spear1340_clock.o
diff --git a/drivers/clk/spear/clk-aux-synth.c b/drivers/clk/spear/clk-aux-synth.c
new file mode 100644
index 00000000000..af34074e702
--- /dev/null
+++ b/drivers/clk/spear/clk-aux-synth.c
@@ -0,0 +1,198 @@
+/*
+ * Copyright (C) 2012 ST Microelectronics
+ * Viresh Kumar <viresh.kumar@st.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.
+ *
+ * Auxiliary Synthesizer clock implementation
+ */
+
+#define pr_fmt(fmt) "clk-aux-synth: " fmt
+
+#include <linux/clk-provider.h>
+#include <linux/slab.h>
+#include <linux/io.h>
+#include <linux/err.h>
+#include "clk.h"
+
+/*
+ * DOC: Auxiliary Synthesizer clock
+ *
+ * Aux synth gives rate for different values of eq, x and y
+ *
+ * Fout from synthesizer can be given from two equations:
+ * Fout1 = (Fin * X/Y)/2		EQ1
+ * Fout2 = Fin * X/Y			EQ2
+ */
+
+#define to_clk_aux(_hw) container_of(_hw, struct clk_aux, hw)
+
+static struct aux_clk_masks default_aux_masks = {
+	.eq_sel_mask = AUX_EQ_SEL_MASK,
+	.eq_sel_shift = AUX_EQ_SEL_SHIFT,
+	.eq1_mask = AUX_EQ1_SEL,
+	.eq2_mask = AUX_EQ2_SEL,
+	.xscale_sel_mask = AUX_XSCALE_MASK,
+	.xscale_sel_shift = AUX_XSCALE_SHIFT,
+	.yscale_sel_mask = AUX_YSCALE_MASK,
+	.yscale_sel_shift = AUX_YSCALE_SHIFT,
+	.enable_bit = AUX_SYNT_ENB,
+};
+
+static unsigned long aux_calc_rate(struct clk_hw *hw, unsigned long prate,
+		int index)
+{
+	struct clk_aux *aux = to_clk_aux(hw);
+	struct aux_rate_tbl *rtbl = aux->rtbl;
+	u8 eq = rtbl[index].eq ? 1 : 2;
+
+	return (((prate / 10000) * rtbl[index].xscale) /
+			(rtbl[index].yscale * eq)) * 10000;
+}
+
+static long clk_aux_round_rate(struct clk_hw *hw, unsigned long drate,
+		unsigned long *prate)
+{
+	struct clk_aux *aux = to_clk_aux(hw);
+	int unused;
+
+	return clk_round_rate_index(hw, drate, *prate, aux_calc_rate,
+			aux->rtbl_cnt, &unused);
+}
+
+static unsigned long clk_aux_recalc_rate(struct clk_hw *hw,
+		unsigned long parent_rate)
+{
+	struct clk_aux *aux = to_clk_aux(hw);
+	unsigned int num = 1, den = 1, val, eqn;
+	unsigned long flags = 0;
+
+	if (aux->lock)
+		spin_lock_irqsave(aux->lock, flags);
+
+	val = readl_relaxed(aux->reg);
+
+	if (aux->lock)
+		spin_unlock_irqrestore(aux->lock, flags);
+
+	eqn = (val >> aux->masks->eq_sel_shift) & aux->masks->eq_sel_mask;
+	if (eqn == aux->masks->eq1_mask)
+		den = 2;
+
+	/* calculate numerator */
+	num = (val >> aux->masks->xscale_sel_shift) &
+		aux->masks->xscale_sel_mask;
+
+	/* calculate denominator */
+	den *= (val >> aux->masks->yscale_sel_shift) &
+		aux->masks->yscale_sel_mask;
+
+	if (!den)
+		return 0;
+
+	return (((parent_rate / 10000) * num) / den) * 10000;
+}
+
+/* Configures new clock rate of aux */
+static int clk_aux_set_rate(struct clk_hw *hw, unsigned long drate,
+				unsigned long prate)
+{
+	struct clk_aux *aux = to_clk_aux(hw);
+	struct aux_rate_tbl *rtbl = aux->rtbl;
+	unsigned long val, flags = 0;
+	int i;
+
+	clk_round_rate_index(hw, drate, prate, aux_calc_rate, aux->rtbl_cnt,
+			&i);
+
+	if (aux->lock)
+		spin_lock_irqsave(aux->lock, flags);
+
+	val = readl_relaxed(aux->reg) &
+		~(aux->masks->eq_sel_mask << aux->masks->eq_sel_shift);
+	val |= (rtbl[i].eq & aux->masks->eq_sel_mask) <<
+		aux->masks->eq_sel_shift;
+	val &= ~(aux->masks->xscale_sel_mask << aux->masks->xscale_sel_shift);
+	val |= (rtbl[i].xscale & aux->masks->xscale_sel_mask) <<
+		aux->masks->xscale_sel_shift;
+	val &= ~(aux->masks->yscale_sel_mask << aux->masks->yscale_sel_shift);
+	val |= (rtbl[i].yscale & aux->masks->yscale_sel_mask) <<
+		aux->masks->yscale_sel_shift;
+	writel_relaxed(val, aux->reg);
+
+	if (aux->lock)
+		spin_unlock_irqrestore(aux->lock, flags);
+
+	return 0;
+}
+
+static struct clk_ops clk_aux_ops = {
+	.recalc_rate = clk_aux_recalc_rate,
+	.round_rate = clk_aux_round_rate,
+	.set_rate = clk_aux_set_rate,
+};
+
+struct clk *clk_register_aux(const char *aux_name, const char *gate_name,
+		const char *parent_name, unsigned long flags, void __iomem *reg,
+		struct aux_clk_masks *masks, struct aux_rate_tbl *rtbl,
+		u8 rtbl_cnt, spinlock_t *lock, struct clk **gate_clk)
+{
+	struct clk_aux *aux;
+	struct clk_init_data init;
+	struct clk *clk;
+
+	if (!aux_name || !parent_name || !reg || !rtbl || !rtbl_cnt) {
+		pr_err("Invalid arguments passed");
+		return ERR_PTR(-EINVAL);
+	}
+
+	aux = kzalloc(sizeof(*aux), GFP_KERNEL);
+	if (!aux) {
+		pr_err("could not allocate aux clk\n");
+		return ERR_PTR(-ENOMEM);
+	}
+
+	/* struct clk_aux assignments */
+	if (!masks)
+		aux->masks = &default_aux_masks;
+	else
+		aux->masks = masks;
+
+	aux->reg = reg;
+	aux->rtbl = rtbl;
+	aux->rtbl_cnt = rtbl_cnt;
+	aux->lock = lock;
+	aux->hw.init = &init;
+
+	init.name = aux_name;
+	init.ops = &clk_aux_ops;
+	init.flags = flags;
+	init.parent_names = &parent_name;
+	init.num_parents = 1;
+
+	clk = clk_register(NULL, &aux->hw);
+	if (IS_ERR_OR_NULL(clk))
+		goto free_aux;
+
+	if (gate_name) {
+		struct clk *tgate_clk;
+
+		tgate_clk = clk_register_gate(NULL, gate_name, aux_name, 0, reg,
+				aux->masks->enable_bit, 0, lock);
+		if (IS_ERR_OR_NULL(tgate_clk))
+			goto free_aux;
+
+		if (gate_clk)
+			*gate_clk = tgate_clk;
+	}
+
+	return clk;
+
+free_aux:
+	kfree(aux);
+	pr_err("clk register failed\n");
+
+	return NULL;
+}
diff --git a/drivers/clk/spear/clk-frac-synth.c b/drivers/clk/spear/clk-frac-synth.c
new file mode 100644
index 00000000000..4dbdb3fe18e
--- /dev/null
+++ b/drivers/clk/spear/clk-frac-synth.c
@@ -0,0 +1,165 @@
+/*
+ * Copyright (C) 2012 ST Microelectronics
+ * Viresh Kumar <viresh.kumar@st.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.
+ *
+ * Fractional Synthesizer clock implementation
+ */
+
+#define pr_fmt(fmt) "clk-frac-synth: " fmt
+
+#include <linux/clk-provider.h>
+#include <linux/slab.h>
+#include <linux/io.h>
+#include <linux/err.h>
+#include "clk.h"
+
+#define DIV_FACTOR_MASK		0x1FFFF
+
+/*
+ * DOC: Fractional Synthesizer clock
+ *
+ * Fout from synthesizer can be given from below equation:
+ *
+ * Fout= Fin/2*div (division factor)
+ * div is 17 bits:-
+ *	0-13 (fractional part)
+ *	14-16 (integer part)
+ *	div is (16-14 bits).(13-0 bits) (in binary)
+ *
+ *	Fout = Fin/(2 * div)
+ *	Fout = ((Fin / 10000)/(2 * div)) * 10000
+ *	Fout = (2^14 * (Fin / 10000)/(2^14 * (2 * div))) * 10000
+ *	Fout = (((Fin / 10000) << 14)/(2 * (div << 14))) * 10000
+ *
+ * div << 14 simply 17 bit value written at register.
+ * Max error due to scaling down by 10000 is 10 KHz
+ */
+
+#define to_clk_frac(_hw) container_of(_hw, struct clk_frac, hw)
+
+static unsigned long frac_calc_rate(struct clk_hw *hw, unsigned long prate,
+		int index)
+{
+	struct clk_frac *frac = to_clk_frac(hw);
+	struct frac_rate_tbl *rtbl = frac->rtbl;
+
+	prate /= 10000;
+	prate <<= 14;
+	prate /= (2 * rtbl[index].div);
+	prate *= 10000;
+
+	return prate;
+}
+
+static long clk_frac_round_rate(struct clk_hw *hw, unsigned long drate,
+		unsigned long *prate)
+{
+	struct clk_frac *frac = to_clk_frac(hw);
+	int unused;
+
+	return clk_round_rate_index(hw, drate, *prate, frac_calc_rate,
+			frac->rtbl_cnt, &unused);
+}
+
+static unsigned long clk_frac_recalc_rate(struct clk_hw *hw,
+		unsigned long parent_rate)
+{
+	struct clk_frac *frac = to_clk_frac(hw);
+	unsigned long flags = 0;
+	unsigned int div = 1, val;
+
+	if (frac->lock)
+		spin_lock_irqsave(frac->lock, flags);
+
+	val = readl_relaxed(frac->reg);
+
+	if (frac->lock)
+		spin_unlock_irqrestore(frac->lock, flags);
+
+	div = val & DIV_FACTOR_MASK;
+
+	if (!div)
+		return 0;
+
+	parent_rate = parent_rate / 10000;
+
+	parent_rate = (parent_rate << 14) / (2 * div);
+	return parent_rate * 10000;
+}
+
+/* Configures new clock rate of frac */
+static int clk_frac_set_rate(struct clk_hw *hw, unsigned long drate,
+				unsigned long prate)
+{
+	struct clk_frac *frac = to_clk_frac(hw);
+	struct frac_rate_tbl *rtbl = frac->rtbl;
+	unsigned long flags = 0, val;
+	int i;
+
+	clk_round_rate_index(hw, drate, prate, frac_calc_rate, frac->rtbl_cnt,
+			&i);
+
+	if (frac->lock)
+		spin_lock_irqsave(frac->lock, flags);
+
+	val = readl_relaxed(frac->reg) & ~DIV_FACTOR_MASK;
+	val |= rtbl[i].div & DIV_FACTOR_MASK;
+	writel_relaxed(val, frac->reg);
+
+	if (frac->lock)
+		spin_unlock_irqrestore(frac->lock, flags);
+
+	return 0;
+}
+
+struct clk_ops clk_frac_ops = {
+	.recalc_rate = clk_frac_recalc_rate,
+	.round_rate = clk_frac_round_rate,
+	.set_rate = clk_frac_set_rate,
+};
+
+struct clk *clk_register_frac(const char *name, const char *parent_name,
+		unsigned long flags, void __iomem *reg,
+		struct frac_rate_tbl *rtbl, u8 rtbl_cnt, spinlock_t *lock)
+{
+	struct clk_init_data init;
+	struct clk_frac *frac;
+	struct clk *clk;
+
+	if (!name || !parent_name || !reg || !rtbl || !rtbl_cnt) {
+		pr_err("Invalid arguments passed");
+		return ERR_PTR(-EINVAL);
+	}
+
+	frac = kzalloc(sizeof(*frac), GFP_KERNEL);
+	if (!frac) {
+		pr_err("could not allocate frac clk\n");
+		return ERR_PTR(-ENOMEM);
+	}
+
+	/* struct clk_frac assignments */
+	frac->reg = reg;
+	frac->rtbl = rtbl;
+	frac->rtbl_cnt = rtbl_cnt;
+	frac->lock = lock;
+	frac->hw.init = &init;
+
+	init.name = name;
+	init.ops = &clk_frac_ops;
+	init.flags = flags;
+	init.parent_names = &parent_name;
+	init.num_parents = 1;
+
+	clk = clk_register(NULL, &frac->hw);
+	if (!IS_ERR_OR_NULL(clk))
+		return clk;
+
+	pr_err("clk register failed\n");
+	kfree(frac);
+
+	return NULL;
+}
diff --git a/drivers/clk/spear/clk-gpt-synth.c b/drivers/clk/spear/clk-gpt-synth.c
new file mode 100644
index 00000000000..b471c9762a9
--- /dev/null
+++ b/drivers/clk/spear/clk-gpt-synth.c
@@ -0,0 +1,154 @@
+/*
+ * Copyright (C) 2012 ST Microelectronics
+ * Viresh Kumar <viresh.kumar@st.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.
+ *
+ * General Purpose Timer Synthesizer clock implementation
+ */
+
+#define pr_fmt(fmt) "clk-gpt-synth: " fmt
+
+#include <linux/clk-provider.h>
+#include <linux/slab.h>
+#include <linux/io.h>
+#include <linux/err.h>
+#include "clk.h"
+
+#define GPT_MSCALE_MASK		0xFFF
+#define GPT_NSCALE_SHIFT	12
+#define GPT_NSCALE_MASK		0xF
+
+/*
+ * DOC: General Purpose Timer Synthesizer clock
+ *
+ * Calculates gpt synth clk rate for different values of mscale and nscale
+ *
+ * Fout= Fin/((2 ^ (N+1)) * (M+1))
+ */
+
+#define to_clk_gpt(_hw) container_of(_hw, struct clk_gpt, hw)
+
+static unsigned long gpt_calc_rate(struct clk_hw *hw, unsigned long prate,
+		int index)
+{
+	struct clk_gpt *gpt = to_clk_gpt(hw);
+	struct gpt_rate_tbl *rtbl = gpt->rtbl;
+
+	prate /= ((1 << (rtbl[index].nscale + 1)) * (rtbl[index].mscale + 1));
+
+	return prate;
+}
+
+static long clk_gpt_round_rate(struct clk_hw *hw, unsigned long drate,
+		unsigned long *prate)
+{
+	struct clk_gpt *gpt = to_clk_gpt(hw);
+	int unused;
+
+	return clk_round_rate_index(hw, drate, *prate, gpt_calc_rate,
+			gpt->rtbl_cnt, &unused);
+}
+
+static unsigned long clk_gpt_recalc_rate(struct clk_hw *hw,
+		unsigned long parent_rate)
+{
+	struct clk_gpt *gpt = to_clk_gpt(hw);
+	unsigned long flags = 0;
+	unsigned int div = 1, val;
+
+	if (gpt->lock)
+		spin_lock_irqsave(gpt->lock, flags);
+
+	val = readl_relaxed(gpt->reg);
+
+	if (gpt->lock)
+		spin_unlock_irqrestore(gpt->lock, flags);
+
+	div += val & GPT_MSCALE_MASK;
+	div *= 1 << (((val >> GPT_NSCALE_SHIFT) & GPT_NSCALE_MASK) + 1);
+
+	if (!div)
+		return 0;
+
+	return parent_rate / div;
+}
+
+/* Configures new clock rate of gpt */
+static int clk_gpt_set_rate(struct clk_hw *hw, unsigned long drate,
+				unsigned long prate)
+{
+	struct clk_gpt *gpt = to_clk_gpt(hw);
+	struct gpt_rate_tbl *rtbl = gpt->rtbl;
+	unsigned long flags = 0, val;
+	int i;
+
+	clk_round_rate_index(hw, drate, prate, gpt_calc_rate, gpt->rtbl_cnt,
+			&i);
+
+	if (gpt->lock)
+		spin_lock_irqsave(gpt->lock, flags);
+
+	val = readl(gpt->reg) & ~GPT_MSCALE_MASK;
+	val &= ~(GPT_NSCALE_MASK << GPT_NSCALE_SHIFT);
+
+	val |= rtbl[i].mscale & GPT_MSCALE_MASK;
+	val |= (rtbl[i].nscale & GPT_NSCALE_MASK) << GPT_NSCALE_SHIFT;
+
+	writel_relaxed(val, gpt->reg);
+
+	if (gpt->lock)
+		spin_unlock_irqrestore(gpt->lock, flags);
+
+	return 0;
+}
+
+static struct clk_ops clk_gpt_ops = {
+	.recalc_rate = clk_gpt_recalc_rate,
+	.round_rate = clk_gpt_round_rate,
+	.set_rate = clk_gpt_set_rate,
+};
+
+struct clk *clk_register_gpt(const char *name, const char *parent_name, unsigned
+		long flags, void __iomem *reg, struct gpt_rate_tbl *rtbl, u8
+		rtbl_cnt, spinlock_t *lock)
+{
+	struct clk_init_data init;
+	struct clk_gpt *gpt;
+	struct clk *clk;
+
+	if (!name || !parent_name || !reg || !rtbl || !rtbl_cnt) {
+		pr_err("Invalid arguments passed");
+		return ERR_PTR(-EINVAL);
+	}
+
+	gpt = kzalloc(sizeof(*gpt), GFP_KERNEL);
+	if (!gpt) {
+		pr_err("could not allocate gpt clk\n");
+		return ERR_PTR(-ENOMEM);
+	}
+
+	/* struct clk_gpt assignments */
+	gpt->reg = reg;
+	gpt->rtbl = rtbl;
+	gpt->rtbl_cnt = rtbl_cnt;
+	gpt->lock = lock;
+	gpt->hw.init = &init;
+
+	init.name = name;
+	init.ops = &clk_gpt_ops;
+	init.flags = flags;
+	init.parent_names = &parent_name;
+	init.num_parents = 1;
+
+	clk = clk_register(NULL, &gpt->hw);
+	if (!IS_ERR_OR_NULL(clk))
+		return clk;
+
+	pr_err("clk register failed\n");
+	kfree(gpt);
+
+	return NULL;
+}
diff --git a/drivers/clk/spear/clk-vco-pll.c b/drivers/clk/spear/clk-vco-pll.c
new file mode 100644
index 00000000000..dcd4bdf4b0d
--- /dev/null
+++ b/drivers/clk/spear/clk-vco-pll.c
@@ -0,0 +1,363 @@
+/*
+ * Copyright (C) 2012 ST Microelectronics
+ * Viresh Kumar <viresh.kumar@st.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.
+ *
+ * VCO-PLL clock implementation
+ */
+
+#define pr_fmt(fmt) "clk-vco-pll: " fmt
+
+#include <linux/clk-provider.h>
+#include <linux/slab.h>
+#include <linux/io.h>
+#include <linux/err.h>
+#include "clk.h"
+
+/*
+ * DOC: VCO-PLL clock
+ *
+ * VCO and PLL rate are derived from following equations:
+ *
+ * In normal mode
+ * vco = (2 * M[15:8] * Fin)/N
+ *
+ * In Dithered mode
+ * vco = (2 * M[15:0] * Fin)/(256 * N)
+ *
+ * pll_rate = pll/2^p
+ *
+ * vco and pll are very closely bound to each other, "vco needs to program:
+ * mode, m & n" and "pll needs to program p", both share common enable/disable
+ * logic.
+ *
+ * clk_register_vco_pll() registers instances of both vco & pll.
+ * CLK_SET_RATE_PARENT flag is forced for pll, as it will always pass its
+ * set_rate to vco. A single rate table exists for both the clocks, which
+ * configures m, n and p.
+ */
+
+/* PLL_CTR register masks */
+#define PLL_MODE_NORMAL		0
+#define PLL_MODE_FRACTION	1
+#define PLL_MODE_DITH_DSM	2
+#define PLL_MODE_DITH_SSM	3
+#define PLL_MODE_MASK		3
+#define PLL_MODE_SHIFT		3
+#define PLL_ENABLE		2
+
+#define PLL_LOCK_SHIFT		0
+#define PLL_LOCK_MASK		1
+
+/* PLL FRQ register masks */
+#define PLL_NORM_FDBK_M_MASK	0xFF
+#define PLL_NORM_FDBK_M_SHIFT	24
+#define PLL_DITH_FDBK_M_MASK	0xFFFF
+#define PLL_DITH_FDBK_M_SHIFT	16
+#define PLL_DIV_P_MASK		0x7
+#define PLL_DIV_P_SHIFT		8
+#define PLL_DIV_N_MASK		0xFF
+#define PLL_DIV_N_SHIFT		0
+
+#define to_clk_vco(_hw) container_of(_hw, struct clk_vco, hw)
+#define to_clk_pll(_hw) container_of(_hw, struct clk_pll, hw)
+
+/* Calculates pll clk rate for specific value of mode, m, n and p */
+static unsigned long pll_calc_rate(struct pll_rate_tbl *rtbl,
+		unsigned long prate, int index, unsigned long *pll_rate)
+{
+	unsigned long rate = prate;
+	unsigned int mode;
+
+	mode = rtbl[index].mode ? 256 : 1;
+	rate = (((2 * rate / 10000) * rtbl[index].m) / (mode * rtbl[index].n));
+
+	if (pll_rate)
+		*pll_rate = (rate / (1 << rtbl[index].p)) * 10000;
+
+	return rate * 10000;
+}
+
+static long clk_pll_round_rate_index(struct clk_hw *hw, unsigned long drate,
+				unsigned long *prate, int *index)
+{
+	struct clk_pll *pll = to_clk_pll(hw);
+	unsigned long prev_rate, vco_prev_rate, rate = 0;
+	unsigned long vco_parent_rate =
+		__clk_get_rate(__clk_get_parent(__clk_get_parent(hw->clk)));
+
+	if (!prate) {
+		pr_err("%s: prate is must for pll clk\n", __func__);
+		return -EINVAL;
+	}
+
+	for (*index = 0; *index < pll->vco->rtbl_cnt; (*index)++) {
+		prev_rate = rate;
+		vco_prev_rate = *prate;
+		*prate = pll_calc_rate(pll->vco->rtbl, vco_parent_rate, *index,
+				&rate);
+		if (drate < rate) {
+			/* previous clock was best */
+			if (*index) {
+				rate = prev_rate;
+				*prate = vco_prev_rate;
+				(*index)--;
+			}
+			break;
+		}
+	}
+
+	return rate;
+}
+
+static long clk_pll_round_rate(struct clk_hw *hw, unsigned long drate,
+				unsigned long *prate)
+{
+	int unused;
+
+	return clk_pll_round_rate_index(hw, drate, prate, &unused);
+}
+
+static unsigned long clk_pll_recalc_rate(struct clk_hw *hw, unsigned long
+		parent_rate)
+{
+	struct clk_pll *pll = to_clk_pll(hw);
+	unsigned long flags = 0;
+	unsigned int p;
+
+	if (pll->vco->lock)
+		spin_lock_irqsave(pll->vco->lock, flags);
+
+	p = readl_relaxed(pll->vco->cfg_reg);
+
+	if (pll->vco->lock)
+		spin_unlock_irqrestore(pll->vco->lock, flags);
+
+	p = (p >> PLL_DIV_P_SHIFT) & PLL_DIV_P_MASK;
+
+	return parent_rate / (1 << p);
+}
+
+static int clk_pll_set_rate(struct clk_hw *hw, unsigned long drate,
+				unsigned long prate)
+{
+	struct clk_pll *pll = to_clk_pll(hw);
+	struct pll_rate_tbl *rtbl = pll->vco->rtbl;
+	unsigned long flags = 0, val;
+	int i;
+
+	clk_pll_round_rate_index(hw, drate, NULL, &i);
+
+	if (pll->vco->lock)
+		spin_lock_irqsave(pll->vco->lock, flags);
+
+	val = readl_relaxed(pll->vco->cfg_reg);
+	val &= ~(PLL_DIV_P_MASK << PLL_DIV_P_SHIFT);
+	val |= (rtbl[i].p & PLL_DIV_P_MASK) << PLL_DIV_P_SHIFT;
+	writel_relaxed(val, pll->vco->cfg_reg);
+
+	if (pll->vco->lock)
+		spin_unlock_irqrestore(pll->vco->lock, flags);
+
+	return 0;
+}
+
+static struct clk_ops clk_pll_ops = {
+	.recalc_rate = clk_pll_recalc_rate,
+	.round_rate = clk_pll_round_rate,
+	.set_rate = clk_pll_set_rate,
+};
+
+static inline unsigned long vco_calc_rate(struct clk_hw *hw,
+		unsigned long prate, int index)
+{
+	struct clk_vco *vco = to_clk_vco(hw);
+
+	return pll_calc_rate(vco->rtbl, prate, index, NULL);
+}
+
+static long clk_vco_round_rate(struct clk_hw *hw, unsigned long drate,
+		unsigned long *prate)
+{
+	struct clk_vco *vco = to_clk_vco(hw);
+	int unused;
+
+	return clk_round_rate_index(hw, drate, *prate, vco_calc_rate,
+			vco->rtbl_cnt, &unused);
+}
+
+static unsigned long clk_vco_recalc_rate(struct clk_hw *hw,
+		unsigned long parent_rate)
+{
+	struct clk_vco *vco = to_clk_vco(hw);
+	unsigned long flags = 0;
+	unsigned int num = 2, den = 0, val, mode = 0;
+
+	if (vco->lock)
+		spin_lock_irqsave(vco->lock, flags);
+
+	mode = (readl_relaxed(vco->mode_reg) >> PLL_MODE_SHIFT) & PLL_MODE_MASK;
+
+	val = readl_relaxed(vco->cfg_reg);
+
+	if (vco->lock)
+		spin_unlock_irqrestore(vco->lock, flags);
+
+	den = (val >> PLL_DIV_N_SHIFT) & PLL_DIV_N_MASK;
+
+	/* calculate numerator & denominator */
+	if (!mode) {
+		/* Normal mode */
+		num *= (val >> PLL_NORM_FDBK_M_SHIFT) & PLL_NORM_FDBK_M_MASK;
+	} else {
+		/* Dithered mode */
+		num *= (val >> PLL_DITH_FDBK_M_SHIFT) & PLL_DITH_FDBK_M_MASK;
+		den *= 256;
+	}
+
+	if (!den) {
+		WARN(1, "%s: denominator can't be zero\n", __func__);
+		return 0;
+	}
+
+	return (((parent_rate / 10000) * num) / den) * 10000;
+}
+
+/* Configures new clock rate of vco */
+static int clk_vco_set_rate(struct clk_hw *hw, unsigned long drate,
+				unsigned long prate)
+{
+	struct clk_vco *vco = to_clk_vco(hw);
+	struct pll_rate_tbl *rtbl = vco->rtbl;
+	unsigned long flags = 0, val;
+	int i;
+
+	clk_round_rate_index(hw, drate, prate, vco_calc_rate, vco->rtbl_cnt,
+			&i);
+
+	if (vco->lock)
+		spin_lock_irqsave(vco->lock, flags);
+
+	val = readl_relaxed(vco->mode_reg);
+	val &= ~(PLL_MODE_MASK << PLL_MODE_SHIFT);
+	val |= (rtbl[i].mode & PLL_MODE_MASK) << PLL_MODE_SHIFT;
+	writel_relaxed(val, vco->mode_reg);
+
+	val = readl_relaxed(vco->cfg_reg);
+	val &= ~(PLL_DIV_N_MASK << PLL_DIV_N_SHIFT);
+	val |= (rtbl[i].n & PLL_DIV_N_MASK) << PLL_DIV_N_SHIFT;
+
+	val &= ~(PLL_DITH_FDBK_M_MASK << PLL_DITH_FDBK_M_SHIFT);
+	if (rtbl[i].mode)
+		val |= (rtbl[i].m & PLL_DITH_FDBK_M_MASK) <<
+			PLL_DITH_FDBK_M_SHIFT;
+	else
+		val |= (rtbl[i].m & PLL_NORM_FDBK_M_MASK) <<
+			PLL_NORM_FDBK_M_SHIFT;
+
+	writel_relaxed(val, vco->cfg_reg);
+
+	if (vco->lock)
+		spin_unlock_irqrestore(vco->lock, flags);
+
+	return 0;
+}
+
+static struct clk_ops clk_vco_ops = {
+	.recalc_rate = clk_vco_recalc_rate,
+	.round_rate = clk_vco_round_rate,
+	.set_rate = clk_vco_set_rate,
+};
+
+struct clk *clk_register_vco_pll(const char *vco_name, const char *pll_name,
+		const char *vco_gate_name, const char *parent_name,
+		unsigned long flags, void __iomem *mode_reg, void __iomem
+		*cfg_reg, struct pll_rate_tbl *rtbl, u8 rtbl_cnt,
+		spinlock_t *lock, struct clk **pll_clk,
+		struct clk **vco_gate_clk)
+{
+	struct clk_vco *vco;
+	struct clk_pll *pll;
+	struct clk *vco_clk, *tpll_clk, *tvco_gate_clk;
+	struct clk_init_data vco_init, pll_init;
+	const char **vco_parent_name;
+
+	if (!vco_name || !pll_name || !parent_name || !mode_reg || !cfg_reg ||
+			!rtbl || !rtbl_cnt) {
+		pr_err("Invalid arguments passed");
+		return ERR_PTR(-EINVAL);
+	}
+
+	vco = kzalloc(sizeof(*vco), GFP_KERNEL);
+	if (!vco) {
+		pr_err("could not allocate vco clk\n");
+		return ERR_PTR(-ENOMEM);
+	}
+
+	pll = kzalloc(sizeof(*pll), GFP_KERNEL);
+	if (!pll) {
+		pr_err("could not allocate pll clk\n");
+		goto free_vco;
+	}
+
+	/* struct clk_vco assignments */
+	vco->mode_reg = mode_reg;
+	vco->cfg_reg = cfg_reg;
+	vco->rtbl = rtbl;
+	vco->rtbl_cnt = rtbl_cnt;
+	vco->lock = lock;
+	vco->hw.init = &vco_init;
+
+	pll->vco = vco;
+	pll->hw.init = &pll_init;
+
+	if (vco_gate_name) {
+		tvco_gate_clk = clk_register_gate(NULL, vco_gate_name,
+				parent_name, 0, mode_reg, PLL_ENABLE, 0, lock);
+		if (IS_ERR_OR_NULL(tvco_gate_clk))
+			goto free_pll;
+
+		if (vco_gate_clk)
+			*vco_gate_clk = tvco_gate_clk;
+		vco_parent_name = &vco_gate_name;
+	} else {
+		vco_parent_name = &parent_name;
+	}
+
+	vco_init.name = vco_name;
+	vco_init.ops = &clk_vco_ops;
+	vco_init.flags = flags;
+	vco_init.parent_names = vco_parent_name;
+	vco_init.num_parents = 1;
+
+	pll_init.name = pll_name;
+	pll_init.ops = &clk_pll_ops;
+	pll_init.flags = CLK_SET_RATE_PARENT;
+	pll_init.parent_names = &vco_name;
+	pll_init.num_parents = 1;
+
+	vco_clk = clk_register(NULL, &vco->hw);
+	if (IS_ERR_OR_NULL(vco_clk))
+		goto free_pll;
+
+	tpll_clk = clk_register(NULL, &pll->hw);
+	if (IS_ERR_OR_NULL(tpll_clk))
+		goto free_pll;
+
+	if (pll_clk)
+		*pll_clk = tpll_clk;
+
+	return vco_clk;
+
+free_pll:
+	kfree(pll);
+free_vco:
+	kfree(vco);
+
+	pr_err("Failed to register vco pll clock\n");
+
+	return ERR_PTR(-ENOMEM);
+}
diff --git a/drivers/clk/spear/clk.c b/drivers/clk/spear/clk.c
new file mode 100644
index 00000000000..376d4e5ff32
--- /dev/null
+++ b/drivers/clk/spear/clk.c
@@ -0,0 +1,36 @@
+/*
+ * Copyright (C) 2012 ST Microelectronics
+ * Viresh Kumar <viresh.kumar@st.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.
+ *
+ * SPEAr clk - Common routines
+ */
+
+#include <linux/clk-provider.h>
+#include <linux/types.h>
+#include "clk.h"
+
+long clk_round_rate_index(struct clk_hw *hw, unsigned long drate,
+		unsigned long parent_rate, clk_calc_rate calc_rate, u8 rtbl_cnt,
+		int *index)
+{
+	unsigned long prev_rate, rate = 0;
+
+	for (*index = 0; *index < rtbl_cnt; (*index)++) {
+		prev_rate = rate;
+		rate = calc_rate(hw, parent_rate, *index);
+		if (drate < rate) {
+			/* previous clock was best */
+			if (*index) {
+				rate = prev_rate;
+				(*index)--;
+			}
+			break;
+		}
+	}
+
+	return rate;
+}
diff --git a/drivers/clk/spear/clk.h b/drivers/clk/spear/clk.h
new file mode 100644
index 00000000000..3321c46a071
--- /dev/null
+++ b/drivers/clk/spear/clk.h
@@ -0,0 +1,134 @@
+/*
+ * Clock framework definitions for SPEAr platform
+ *
+ * Copyright (C) 2012 ST Microelectronics
+ * Viresh Kumar <viresh.kumar@st.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.
+ */
+
+#ifndef __SPEAR_CLK_H
+#define __SPEAR_CLK_H
+
+#include <linux/clk-provider.h>
+#include <linux/spinlock_types.h>
+#include <linux/types.h>
+
+/* Auxiliary Synth clk */
+/* Default masks */
+#define AUX_EQ_SEL_SHIFT	30
+#define AUX_EQ_SEL_MASK		1
+#define AUX_EQ1_SEL		0
+#define AUX_EQ2_SEL		1
+#define AUX_XSCALE_SHIFT	16
+#define AUX_XSCALE_MASK		0xFFF
+#define AUX_YSCALE_SHIFT	0
+#define AUX_YSCALE_MASK		0xFFF
+#define AUX_SYNT_ENB		31
+
+struct aux_clk_masks {
+	u32 eq_sel_mask;
+	u32 eq_sel_shift;
+	u32 eq1_mask;
+	u32 eq2_mask;
+	u32 xscale_sel_mask;
+	u32 xscale_sel_shift;
+	u32 yscale_sel_mask;
+	u32 yscale_sel_shift;
+	u32 enable_bit;
+};
+
+struct aux_rate_tbl {
+	u16 xscale;
+	u16 yscale;
+	u8 eq;
+};
+
+struct clk_aux {
+	struct			clk_hw hw;
+	void __iomem		*reg;
+	struct aux_clk_masks	*masks;
+	struct aux_rate_tbl	*rtbl;
+	u8			rtbl_cnt;
+	spinlock_t		*lock;
+};
+
+/* Fractional Synth clk */
+struct frac_rate_tbl {
+	u32 div;
+};
+
+struct clk_frac {
+	struct			clk_hw hw;
+	void __iomem		*reg;
+	struct frac_rate_tbl	*rtbl;
+	u8			rtbl_cnt;
+	spinlock_t		*lock;
+};
+
+/* GPT clk */
+struct gpt_rate_tbl {
+	u16 mscale;
+	u16 nscale;
+};
+
+struct clk_gpt {
+	struct			clk_hw hw;
+	void __iomem		*reg;
+	struct gpt_rate_tbl	*rtbl;
+	u8			rtbl_cnt;
+	spinlock_t		*lock;
+};
+
+/* VCO-PLL clk */
+struct pll_rate_tbl {
+	u8 mode;
+	u16 m;
+	u8 n;
+	u8 p;
+};
+
+struct clk_vco {
+	struct			clk_hw hw;
+	void __iomem		*mode_reg;
+	void __iomem		*cfg_reg;
+	struct pll_rate_tbl	*rtbl;
+	u8			rtbl_cnt;
+	spinlock_t		*lock;
+};
+
+struct clk_pll {
+	struct			clk_hw hw;
+	struct clk_vco		*vco;
+	const char		*parent[1];
+	spinlock_t		*lock;
+};
+
+typedef unsigned long (*clk_calc_rate)(struct clk_hw *hw, unsigned long prate,
+		int index);
+
+/* clk register routines */
+struct clk *clk_register_aux(const char *aux_name, const char *gate_name,
+		const char *parent_name, unsigned long flags, void __iomem *reg,
+		struct aux_clk_masks *masks, struct aux_rate_tbl *rtbl,
+		u8 rtbl_cnt, spinlock_t *lock, struct clk **gate_clk);
+struct clk *clk_register_frac(const char *name, const char *parent_name,
+		unsigned long flags, void __iomem *reg,
+		struct frac_rate_tbl *rtbl, u8 rtbl_cnt, spinlock_t *lock);
+struct clk *clk_register_gpt(const char *name, const char *parent_name, unsigned
+		long flags, void __iomem *reg, struct gpt_rate_tbl *rtbl, u8
+		rtbl_cnt, spinlock_t *lock);
+struct clk *clk_register_vco_pll(const char *vco_name, const char *pll_name,
+		const char *vco_gate_name, const char *parent_name,
+		unsigned long flags, void __iomem *mode_reg, void __iomem
+		*cfg_reg, struct pll_rate_tbl *rtbl, u8 rtbl_cnt,
+		spinlock_t *lock, struct clk **pll_clk,
+		struct clk **vco_gate_clk);
+
+long clk_round_rate_index(struct clk_hw *hw, unsigned long drate,
+		unsigned long parent_rate, clk_calc_rate calc_rate, u8 rtbl_cnt,
+		int *index);
+
+#endif /* __SPEAR_CLK_H */
diff --git a/drivers/clk/spear/spear1310_clock.c b/drivers/clk/spear/spear1310_clock.c
new file mode 100644
index 00000000000..42b68df9aee
--- /dev/null
+++ b/drivers/clk/spear/spear1310_clock.c
@@ -0,0 +1,1106 @@
+/*
+ * arch/arm/mach-spear13xx/spear1310_clock.c
+ *
+ * SPEAr1310 machine clock framework source file
+ *
+ * Copyright (C) 2012 ST Microelectronics
+ * Viresh Kumar <viresh.kumar@st.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/clk.h>
+#include <linux/clkdev.h>
+#include <linux/err.h>
+#include <linux/io.h>
+#include <linux/of_platform.h>
+#include <linux/spinlock_types.h>
+#include <mach/spear.h>
+#include "clk.h"
+
+/* PLL related registers and bit values */
+#define SPEAR1310_PLL_CFG			(VA_MISC_BASE + 0x210)
+	/* PLL_CFG bit values */
+	#define SPEAR1310_CLCD_SYNT_CLK_MASK		1
+	#define SPEAR1310_CLCD_SYNT_CLK_SHIFT		31
+	#define SPEAR1310_RAS_SYNT2_3_CLK_MASK		2
+	#define SPEAR1310_RAS_SYNT2_3_CLK_SHIFT		29
+	#define SPEAR1310_RAS_SYNT_CLK_MASK		2
+	#define SPEAR1310_RAS_SYNT0_1_CLK_SHIFT		27
+	#define SPEAR1310_PLL_CLK_MASK			2
+	#define SPEAR1310_PLL3_CLK_SHIFT		24
+	#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)
+	/* PERIP_CLK_CFG bit values */
+	#define SPEAR1310_GPT_OSC24_VAL			0
+	#define SPEAR1310_GPT_APB_VAL			1
+	#define SPEAR1310_GPT_CLK_MASK			1
+	#define SPEAR1310_GPT3_CLK_SHIFT		11
+	#define SPEAR1310_GPT2_CLK_SHIFT		10
+	#define SPEAR1310_GPT1_CLK_SHIFT		9
+	#define SPEAR1310_GPT0_CLK_SHIFT		8
+	#define SPEAR1310_UART_CLK_PLL5_VAL		0
+	#define SPEAR1310_UART_CLK_OSC24_VAL		1
+	#define SPEAR1310_UART_CLK_SYNT_VAL		2
+	#define SPEAR1310_UART_CLK_MASK			2
+	#define SPEAR1310_UART_CLK_SHIFT		4
+
+	#define SPEAR1310_AUX_CLK_PLL5_VAL		0
+	#define SPEAR1310_AUX_CLK_SYNT_VAL		1
+	#define SPEAR1310_CLCD_CLK_MASK			2
+	#define SPEAR1310_CLCD_CLK_SHIFT		2
+	#define SPEAR1310_C3_CLK_MASK			1
+	#define SPEAR1310_C3_CLK_SHIFT			1
+
+#define SPEAR1310_GMAC_CLK_CFG			(VA_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
+	#define SPEAR1310_GMAC_PHY_CLK_SHIFT		3
+	#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)
+	/* I2S_CLK_CFG register mask */
+	#define SPEAR1310_I2S_SCLK_X_MASK		0x1F
+	#define SPEAR1310_I2S_SCLK_X_SHIFT		27
+	#define SPEAR1310_I2S_SCLK_Y_MASK		0x1F
+	#define SPEAR1310_I2S_SCLK_Y_SHIFT		22
+	#define SPEAR1310_I2S_SCLK_EQ_SEL_SHIFT		21
+	#define SPEAR1310_I2S_SCLK_SYNTH_ENB		20
+	#define SPEAR1310_I2S_PRS1_CLK_X_MASK		0xFF
+	#define SPEAR1310_I2S_PRS1_CLK_X_SHIFT		12
+	#define SPEAR1310_I2S_PRS1_CLK_Y_MASK		0xFF
+	#define SPEAR1310_I2S_PRS1_CLK_Y_SHIFT		4
+	#define SPEAR1310_I2S_PRS1_EQ_SEL_SHIFT		3
+	#define SPEAR1310_I2S_REF_SEL_MASK		1
+	#define SPEAR1310_I2S_REF_SHIFT			2
+	#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)
+	/* Check Fractional synthesizer reg masks */
+
+#define SPEAR1310_PERIP1_CLK_ENB		(VA_MISC_BASE + 0x300)
+	/* PERIP1_CLK_ENB register masks */
+	#define SPEAR1310_RTC_CLK_ENB			31
+	#define SPEAR1310_ADC_CLK_ENB			30
+	#define SPEAR1310_C3_CLK_ENB			29
+	#define SPEAR1310_JPEG_CLK_ENB			28
+	#define SPEAR1310_CLCD_CLK_ENB			27
+	#define SPEAR1310_DMA_CLK_ENB			25
+	#define SPEAR1310_GPIO1_CLK_ENB			24
+	#define SPEAR1310_GPIO0_CLK_ENB			23
+	#define SPEAR1310_GPT1_CLK_ENB			22
+	#define SPEAR1310_GPT0_CLK_ENB			21
+	#define SPEAR1310_I2S0_CLK_ENB			20
+	#define SPEAR1310_I2S1_CLK_ENB			19
+	#define SPEAR1310_I2C0_CLK_ENB			18
+	#define SPEAR1310_SSP_CLK_ENB			17
+	#define SPEAR1310_UART_CLK_ENB			15
+	#define SPEAR1310_PCIE_SATA_2_CLK_ENB		14
+	#define SPEAR1310_PCIE_SATA_1_CLK_ENB		13
+	#define SPEAR1310_PCIE_SATA_0_CLK_ENB		12
+	#define SPEAR1310_UOC_CLK_ENB			11
+	#define SPEAR1310_UHC1_CLK_ENB			10
+	#define SPEAR1310_UHC0_CLK_ENB			9
+	#define SPEAR1310_GMAC_CLK_ENB			8
+	#define SPEAR1310_CFXD_CLK_ENB			7
+	#define SPEAR1310_SDHCI_CLK_ENB			6
+	#define SPEAR1310_SMI_CLK_ENB			5
+	#define SPEAR1310_FSMC_CLK_ENB			4
+	#define SPEAR1310_SYSRAM0_CLK_ENB		3
+	#define SPEAR1310_SYSRAM1_CLK_ENB		2
+	#define SPEAR1310_SYSROM_CLK_ENB		1
+	#define SPEAR1310_BUS_CLK_ENB			0
+
+#define SPEAR1310_PERIP2_CLK_ENB		(VA_MISC_BASE + 0x304)
+	/* PERIP2_CLK_ENB register masks */
+	#define SPEAR1310_THSENS_CLK_ENB		8
+	#define SPEAR1310_I2S_REF_PAD_CLK_ENB		7
+	#define SPEAR1310_ACP_CLK_ENB			6
+	#define SPEAR1310_GPT3_CLK_ENB			5
+	#define SPEAR1310_GPT2_CLK_ENB			4
+	#define SPEAR1310_KBD_CLK_ENB			3
+	#define SPEAR1310_CPU_DBG_CLK_ENB		2
+	#define SPEAR1310_DDR_CORE_CLK_ENB		1
+	#define SPEAR1310_DDR_CTRL_CLK_ENB		0
+
+#define SPEAR1310_RAS_CLK_ENB			(VA_MISC_BASE + 0x310)
+	/* RAS_CLK_ENB register masks */
+	#define SPEAR1310_SYNT3_CLK_ENB			17
+	#define SPEAR1310_SYNT2_CLK_ENB			16
+	#define SPEAR1310_SYNT1_CLK_ENB			15
+	#define SPEAR1310_SYNT0_CLK_ENB			14
+	#define SPEAR1310_PCLK3_CLK_ENB			13
+	#define SPEAR1310_PCLK2_CLK_ENB			12
+	#define SPEAR1310_PCLK1_CLK_ENB			11
+	#define SPEAR1310_PCLK0_CLK_ENB			10
+	#define SPEAR1310_PLL3_CLK_ENB			9
+	#define SPEAR1310_PLL2_CLK_ENB			8
+	#define SPEAR1310_C125M_PAD_CLK_ENB		7
+	#define SPEAR1310_C30M_CLK_ENB			6
+	#define SPEAR1310_C48M_CLK_ENB			5
+	#define SPEAR1310_OSC_25M_CLK_ENB		4
+	#define SPEAR1310_OSC_32K_CLK_ENB		3
+	#define SPEAR1310_OSC_24M_CLK_ENB		2
+	#define SPEAR1310_PCLK_CLK_ENB			1
+	#define SPEAR1310_ACLK_CLK_ENB			0
+
+/* RAS Area Control Register */
+#define SPEAR1310_RAS_CTRL_REG0			(VA_SPEAR1310_RAS_BASE + 0x000)
+	#define SPEAR1310_SSP1_CLK_MASK			3
+	#define SPEAR1310_SSP1_CLK_SHIFT		26
+	#define SPEAR1310_TDM_CLK_MASK			1
+	#define SPEAR1310_TDM2_CLK_SHIFT		24
+	#define SPEAR1310_TDM1_CLK_SHIFT		23
+	#define SPEAR1310_I2C_CLK_MASK			1
+	#define SPEAR1310_I2C7_CLK_SHIFT		22
+	#define SPEAR1310_I2C6_CLK_SHIFT		21
+	#define SPEAR1310_I2C5_CLK_SHIFT		20
+	#define SPEAR1310_I2C4_CLK_SHIFT		19
+	#define SPEAR1310_I2C3_CLK_SHIFT		18
+	#define SPEAR1310_I2C2_CLK_SHIFT		17
+	#define SPEAR1310_I2C1_CLK_SHIFT		16
+	#define SPEAR1310_GPT64_CLK_MASK		1
+	#define SPEAR1310_GPT64_CLK_SHIFT		15
+	#define SPEAR1310_RAS_UART_CLK_MASK		1
+	#define SPEAR1310_UART5_CLK_SHIFT		14
+	#define SPEAR1310_UART4_CLK_SHIFT		13
+	#define SPEAR1310_UART3_CLK_SHIFT		12
+	#define SPEAR1310_UART2_CLK_SHIFT		11
+	#define SPEAR1310_UART1_CLK_SHIFT		10
+	#define SPEAR1310_PCI_CLK_MASK			1
+	#define SPEAR1310_PCI_CLK_SHIFT			0
+
+#define SPEAR1310_RAS_CTRL_REG1			(VA_SPEAR1310_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_CAN1_CLK_ENB			25
+	#define SPEAR1310_CAN0_CLK_ENB			24
+	#define SPEAR1310_GPT64_CLK_ENB			23
+	#define SPEAR1310_SSP1_CLK_ENB			22
+	#define SPEAR1310_I2C7_CLK_ENB			21
+	#define SPEAR1310_I2C6_CLK_ENB			20
+	#define SPEAR1310_I2C5_CLK_ENB			19
+	#define SPEAR1310_I2C4_CLK_ENB			18
+	#define SPEAR1310_I2C3_CLK_ENB			17
+	#define SPEAR1310_I2C2_CLK_ENB			16
+	#define SPEAR1310_I2C1_CLK_ENB			15
+	#define SPEAR1310_UART5_CLK_ENB			14
+	#define SPEAR1310_UART4_CLK_ENB			13
+	#define SPEAR1310_UART3_CLK_ENB			12
+	#define SPEAR1310_UART2_CLK_ENB			11
+	#define SPEAR1310_UART1_CLK_ENB			10
+	#define SPEAR1310_RS485_1_CLK_ENB		9
+	#define SPEAR1310_RS485_0_CLK_ENB		8
+	#define SPEAR1310_TDM2_CLK_ENB			7
+	#define SPEAR1310_TDM1_CLK_ENB			6
+	#define SPEAR1310_PCI_CLK_ENB			5
+	#define SPEAR1310_GMII_CLK_ENB			4
+	#define SPEAR1310_MII2_CLK_ENB			3
+	#define SPEAR1310_MII1_CLK_ENB			2
+	#define SPEAR1310_MII0_CLK_ENB			1
+	#define SPEAR1310_ESRAM_CLK_ENB			0
+
+static DEFINE_SPINLOCK(_lock);
+
+/* pll rate configuration table, in ascending order of rates */
+static struct pll_rate_tbl pll_rtbl[] = {
+	/* PCLK 24MHz */
+	{.mode = 0, .m = 0x83, .n = 0x04, .p = 0x5}, /* vco 1572, pll 49.125 MHz */
+	{.mode = 0, .m = 0x7D, .n = 0x06, .p = 0x3}, /* vco 1000, pll 125 MHz */
+	{.mode = 0, .m = 0x64, .n = 0x06, .p = 0x1}, /* vco 800, pll 400 MHz */
+	{.mode = 0, .m = 0x7D, .n = 0x06, .p = 0x1}, /* vco 1000, pll 500 MHz */
+	{.mode = 0, .m = 0xA6, .n = 0x06, .p = 0x1}, /* vco 1328, pll 664 MHz */
+	{.mode = 0, .m = 0xC8, .n = 0x06, .p = 0x1}, /* vco 1600, pll 800 MHz */
+	{.mode = 0, .m = 0x7D, .n = 0x06, .p = 0x0}, /* vco 1, pll 1 GHz */
+};
+
+/* vco-pll4 rate configuration table, in ascending order of rates */
+static struct pll_rate_tbl pll4_rtbl[] = {
+	{.mode = 0, .m = 0x7D, .n = 0x06, .p = 0x2}, /* vco 1000, pll 250 MHz */
+	{.mode = 0, .m = 0xA6, .n = 0x06, .p = 0x2}, /* vco 1328, pll 332 MHz */
+	{.mode = 0, .m = 0xC8, .n = 0x06, .p = 0x2}, /* vco 1600, pll 400 MHz */
+	{.mode = 0, .m = 0x7D, .n = 0x06, .p = 0x0}, /* vco 1, pll 1 GHz */
+};
+
+/* aux rate configuration table, in ascending order of rates */
+static struct aux_rate_tbl aux_rtbl[] = {
+	/* For VCO1div2 = 500 MHz */
+	{.xscale = 10, .yscale = 204, .eq = 0}, /* 12.29 MHz */
+	{.xscale = 4, .yscale = 21, .eq = 0}, /* 48 MHz */
+	{.xscale = 2, .yscale = 6, .eq = 0}, /* 83 MHz */
+	{.xscale = 2, .yscale = 4, .eq = 0}, /* 125 MHz */
+	{.xscale = 1, .yscale = 3, .eq = 1}, /* 166 MHz */
+	{.xscale = 1, .yscale = 2, .eq = 1}, /* 250 MHz */
+};
+
+/* gmac rate configuration table, in ascending order of rates */
+static struct aux_rate_tbl gmac_rtbl[] = {
+	/* For gmac phy input clk */
+	{.xscale = 2, .yscale = 6, .eq = 0}, /* divided by 6 */
+	{.xscale = 2, .yscale = 4, .eq = 0}, /* divided by 4 */
+	{.xscale = 1, .yscale = 3, .eq = 1}, /* divided by 3 */
+	{.xscale = 1, .yscale = 2, .eq = 1}, /* divided by 2 */
+};
+
+/* clcd rate configuration table, in ascending order of rates */
+static struct frac_rate_tbl clcd_rtbl[] = {
+	{.div = 0x14000}, /* 25 Mhz , for vc01div4 = 250 MHz*/
+	{.div = 0x1284B}, /* 27 Mhz , for vc01div4 = 250 MHz*/
+	{.div = 0x0D8D3}, /* 58 Mhz , for vco1div4 = 393 MHz */
+	{.div = 0x0B72C}, /* 58 Mhz , for vco1div4 = 332 MHz */
+	{.div = 0x089EE}, /* 58 Mhz , for vc01div4 = 250 MHz*/
+	{.div = 0x06f1C}, /* 72 Mhz , for vc01div4 = 250 MHz*/
+	{.div = 0x06E58}, /* 58 Mhz , for vco1div4 = 200 MHz */
+	{.div = 0x06c1B}, /* 74 Mhz , for vc01div4 = 250 MHz*/
+	{.div = 0x04A12}, /* 108 Mhz , for vc01div4 = 250 MHz*/
+	{.div = 0x0378E}, /* 144 Mhz , for vc01div4 = 250 MHz*/
+};
+
+/* i2s prescaler1 masks */
+static struct aux_clk_masks i2s_prs1_masks = {
+	.eq_sel_mask = AUX_EQ_SEL_MASK,
+	.eq_sel_shift = SPEAR1310_I2S_PRS1_EQ_SEL_SHIFT,
+	.eq1_mask = AUX_EQ1_SEL,
+	.eq2_mask = AUX_EQ2_SEL,
+	.xscale_sel_mask = SPEAR1310_I2S_PRS1_CLK_X_MASK,
+	.xscale_sel_shift = SPEAR1310_I2S_PRS1_CLK_X_SHIFT,
+	.yscale_sel_mask = SPEAR1310_I2S_PRS1_CLK_Y_MASK,
+	.yscale_sel_shift = SPEAR1310_I2S_PRS1_CLK_Y_SHIFT,
+};
+
+/* i2s sclk (bit clock) syynthesizers masks */
+static struct aux_clk_masks i2s_sclk_masks = {
+	.eq_sel_mask = AUX_EQ_SEL_MASK,
+	.eq_sel_shift = SPEAR1310_I2S_SCLK_EQ_SEL_SHIFT,
+	.eq1_mask = AUX_EQ1_SEL,
+	.eq2_mask = AUX_EQ2_SEL,
+	.xscale_sel_mask = SPEAR1310_I2S_SCLK_X_MASK,
+	.xscale_sel_shift = SPEAR1310_I2S_SCLK_X_SHIFT,
+	.yscale_sel_mask = SPEAR1310_I2S_SCLK_Y_MASK,
+	.yscale_sel_shift = SPEAR1310_I2S_SCLK_Y_SHIFT,
+	.enable_bit = SPEAR1310_I2S_SCLK_SYNTH_ENB,
+};
+
+/* i2s prs1 aux rate configuration table, in ascending order of rates */
+static struct aux_rate_tbl i2s_prs1_rtbl[] = {
+	/* For parent clk = 49.152 MHz */
+	{.xscale = 1, .yscale = 2, .eq = 0}, /* 12.288 MHz */
+};
+
+/* i2s sclk aux rate configuration table, in ascending order of rates */
+static struct aux_rate_tbl i2s_sclk_rtbl[] = {
+	/* For i2s_ref_clk = 12.288MHz */
+	{.xscale = 1, .yscale = 4, .eq = 0}, /* 1.53 MHz */
+	{.xscale = 1, .yscale = 2, .eq = 0}, /* 3.07 Mhz */
+};
+
+/* adc rate configuration table, in ascending order of rates */
+/* possible adc range is 2.5 MHz to 20 MHz. */
+static struct aux_rate_tbl adc_rtbl[] = {
+	/* For ahb = 166.67 MHz */
+	{.xscale = 1, .yscale = 31, .eq = 0}, /* 2.68 MHz */
+	{.xscale = 2, .yscale = 21, .eq = 0}, /* 7.94 MHz */
+	{.xscale = 4, .yscale = 21, .eq = 0}, /* 15.87 MHz */
+	{.xscale = 10, .yscale = 42, .eq = 0}, /* 19.84 MHz */
+};
+
+/* General synth rate configuration table, in ascending order of rates */
+static struct frac_rate_tbl gen_rtbl[] = {
+	/* For vco1div4 = 250 MHz */
+	{.div = 0x14000}, /* 25 MHz */
+	{.div = 0x0A000}, /* 50 MHz */
+	{.div = 0x05000}, /* 100 MHz */
+	{.div = 0x02000}, /* 250 MHz */
+};
+
+/* clock parents */
+static const char *vco_parents[] = { "osc_24m_clk", "osc_25m_clk", };
+static const char *gpt_parents[] = { "osc_24m_clk", "apb_clk", };
+static const char *uart0_parents[] = { "pll5_clk", "uart_synth_gate_clk", };
+static const char *c3_parents[] = { "pll5_clk", "c3_synth_gate_clk", };
+static const char *gmac_phy_input_parents[] = { "gmii_125m_pad_clk", "pll2_clk",
+	"osc_25m_clk", };
+static const char *gmac_phy_parents[] = { "gmac_phy_input_mux_clk",
+	"gmac_phy_synth_gate_clk", };
+static const char *clcd_synth_parents[] = { "vco1div4_clk", "pll2_clk", };
+static const char *clcd_pixel_parents[] = { "pll5_clk", "clcd_synth_clk", };
+static const char *i2s_src_parents[] = { "vco1div2_clk", "none", "pll3_clk",
+	"i2s_src_pad_clk", };
+static const char *i2s_ref_parents[] = { "i2s_src_mux_clk", "i2s_prs1_clk", };
+static const char *gen_synth0_1_parents[] = { "vco1div4_clk", "vco3div2_clk",
+	"pll3_clk", };
+static const char *gen_synth2_3_parents[] = { "vco1div4_clk", "vco3div2_clk",
+	"pll2_clk", };
+static const char *rmii_phy_parents[] = { "ras_tx50_clk", "none",
+	"ras_pll2_clk", "ras_synth0_clk", };
+static const char *smii_rgmii_phy_parents[] = { "none", "ras_tx125_clk",
+	"ras_pll2_clk", "ras_synth0_clk", };
+static const char *uart_parents[] = { "ras_apb_clk", "gen_synth3_clk", };
+static const char *i2c_parents[] = { "ras_apb_clk", "gen_synth1_clk", };
+static const char *ssp1_parents[] = { "ras_apb_clk", "gen_synth1_clk",
+	"ras_plclk0_clk", };
+static const char *pci_parents[] = { "ras_pll3_clk", "gen_synth2_clk", };
+static const char *tdm_parents[] = { "ras_pll3_clk", "gen_synth1_clk", };
+
+void __init spear1310_clk_init(void)
+{
+	struct clk *clk, *clk1;
+
+	clk = clk_register_fixed_rate(NULL, "apb_pclk", NULL, CLK_IS_ROOT, 0);
+	clk_register_clkdev(clk, "apb_pclk", NULL);
+
+	clk = clk_register_fixed_rate(NULL, "osc_32k_clk", NULL, CLK_IS_ROOT,
+			32000);
+	clk_register_clkdev(clk, "osc_32k_clk", NULL);
+
+	clk = clk_register_fixed_rate(NULL, "osc_24m_clk", NULL, CLK_IS_ROOT,
+			24000000);
+	clk_register_clkdev(clk, "osc_24m_clk", NULL);
+
+	clk = clk_register_fixed_rate(NULL, "osc_25m_clk", NULL, CLK_IS_ROOT,
+			25000000);
+	clk_register_clkdev(clk, "osc_25m_clk", NULL);
+
+	clk = clk_register_fixed_rate(NULL, "gmii_125m_pad_clk", NULL,
+			CLK_IS_ROOT, 125000000);
+	clk_register_clkdev(clk, "gmii_125m_pad_clk", NULL);
+
+	clk = clk_register_fixed_rate(NULL, "i2s_src_pad_clk", NULL,
+			CLK_IS_ROOT, 12288000);
+	clk_register_clkdev(clk, "i2s_src_pad_clk", NULL);
+
+	/* clock derived from 32 KHz osc clk */
+	clk = clk_register_gate(NULL, "rtc-spear", "osc_32k_clk", 0,
+			SPEAR1310_PERIP1_CLK_ENB, SPEAR1310_RTC_CLK_ENB, 0,
+			&_lock);
+	clk_register_clkdev(clk, NULL, "fc900000.rtc");
+
+	/* clock derived from 24 or 25 MHz osc clk */
+	/* vco-pll */
+	clk = clk_register_mux(NULL, "vco1_mux_clk", vco_parents,
+			ARRAY_SIZE(vco_parents), 0, SPEAR1310_PLL_CFG,
+			SPEAR1310_PLL1_CLK_SHIFT, SPEAR1310_PLL_CLK_MASK, 0,
+			&_lock);
+	clk_register_clkdev(clk, "vco1_mux_clk", NULL);
+	clk = clk_register_vco_pll("vco1_clk", "pll1_clk", NULL, "vco1_mux_clk",
+			0, SPEAR1310_PLL1_CTR, SPEAR1310_PLL1_FRQ, pll_rtbl,
+			ARRAY_SIZE(pll_rtbl), &_lock, &clk1, NULL);
+	clk_register_clkdev(clk, "vco1_clk", NULL);
+	clk_register_clkdev(clk1, "pll1_clk", NULL);
+
+	clk = clk_register_mux(NULL, "vco2_mux_clk", vco_parents,
+			ARRAY_SIZE(vco_parents), 0, SPEAR1310_PLL_CFG,
+			SPEAR1310_PLL2_CLK_SHIFT, SPEAR1310_PLL_CLK_MASK, 0,
+			&_lock);
+	clk_register_clkdev(clk, "vco2_mux_clk", NULL);
+	clk = clk_register_vco_pll("vco2_clk", "pll2_clk", NULL, "vco2_mux_clk",
+			0, SPEAR1310_PLL2_CTR, SPEAR1310_PLL2_FRQ, pll_rtbl,
+			ARRAY_SIZE(pll_rtbl), &_lock, &clk1, NULL);
+	clk_register_clkdev(clk, "vco2_clk", NULL);
+	clk_register_clkdev(clk1, "pll2_clk", NULL);
+
+	clk = clk_register_mux(NULL, "vco3_mux_clk", vco_parents,
+			ARRAY_SIZE(vco_parents), 0, SPEAR1310_PLL_CFG,
+			SPEAR1310_PLL3_CLK_SHIFT, SPEAR1310_PLL_CLK_MASK, 0,
+			&_lock);
+	clk_register_clkdev(clk, "vco3_mux_clk", NULL);
+	clk = clk_register_vco_pll("vco3_clk", "pll3_clk", NULL, "vco3_mux_clk",
+			0, SPEAR1310_PLL3_CTR, SPEAR1310_PLL3_FRQ, pll_rtbl,
+			ARRAY_SIZE(pll_rtbl), &_lock, &clk1, NULL);
+	clk_register_clkdev(clk, "vco3_clk", NULL);
+	clk_register_clkdev(clk1, "pll3_clk", NULL);
+
+	clk = clk_register_vco_pll("vco4_clk", "pll4_clk", NULL, "osc_24m_clk",
+			0, SPEAR1310_PLL4_CTR, SPEAR1310_PLL4_FRQ, pll4_rtbl,
+			ARRAY_SIZE(pll4_rtbl), &_lock, &clk1, NULL);
+	clk_register_clkdev(clk, "vco4_clk", NULL);
+	clk_register_clkdev(clk1, "pll4_clk", NULL);
+
+	clk = clk_register_fixed_rate(NULL, "pll5_clk", "osc_24m_clk", 0,
+			48000000);
+	clk_register_clkdev(clk, "pll5_clk", NULL);
+
+	clk = clk_register_fixed_rate(NULL, "pll6_clk", "osc_25m_clk", 0,
+			25000000);
+	clk_register_clkdev(clk, "pll6_clk", NULL);
+
+	/* vco div n clocks */
+	clk = clk_register_fixed_factor(NULL, "vco1div2_clk", "vco1_clk", 0, 1,
+			2);
+	clk_register_clkdev(clk, "vco1div2_clk", NULL);
+
+	clk = clk_register_fixed_factor(NULL, "vco1div4_clk", "vco1_clk", 0, 1,
+			4);
+	clk_register_clkdev(clk, "vco1div4_clk", NULL);
+
+	clk = clk_register_fixed_factor(NULL, "vco2div2_clk", "vco2_clk", 0, 1,
+			2);
+	clk_register_clkdev(clk, "vco2div2_clk", NULL);
+
+	clk = clk_register_fixed_factor(NULL, "vco3div2_clk", "vco3_clk", 0, 1,
+			2);
+	clk_register_clkdev(clk, "vco3div2_clk", NULL);
+
+	/* peripherals */
+	clk_register_fixed_factor(NULL, "thermal_clk", "osc_24m_clk", 0, 1,
+			128);
+	clk = clk_register_gate(NULL, "thermal_gate_clk", "thermal_clk", 0,
+			SPEAR1310_PERIP2_CLK_ENB, SPEAR1310_THSENS_CLK_ENB, 0,
+			&_lock);
+	clk_register_clkdev(clk, NULL, "spear_thermal");
+
+	/* clock derived from pll4 clk */
+	clk = clk_register_fixed_factor(NULL, "ddr_clk", "pll4_clk", 0, 1,
+			1);
+	clk_register_clkdev(clk, "ddr_clk", NULL);
+
+	/* clock derived from pll1 clk */
+	clk = clk_register_fixed_factor(NULL, "cpu_clk", "pll1_clk", 0, 1, 2);
+	clk_register_clkdev(clk, "cpu_clk", NULL);
+
+	clk = clk_register_fixed_factor(NULL, "wdt_clk", "cpu_clk", 0, 1,
+			2);
+	clk_register_clkdev(clk, NULL, "ec800620.wdt");
+
+	clk = clk_register_fixed_factor(NULL, "ahb_clk", "pll1_clk", 0, 1,
+			6);
+	clk_register_clkdev(clk, "ahb_clk", NULL);
+
+	clk = clk_register_fixed_factor(NULL, "apb_clk", "pll1_clk", 0, 1,
+			12);
+	clk_register_clkdev(clk, "apb_clk", NULL);
+
+	/* gpt clocks */
+	clk = clk_register_mux(NULL, "gpt0_mux_clk", gpt_parents,
+			ARRAY_SIZE(gpt_parents), 0, SPEAR1310_PERIP_CLK_CFG,
+			SPEAR1310_GPT0_CLK_SHIFT, SPEAR1310_GPT_CLK_MASK, 0,
+			&_lock);
+	clk_register_clkdev(clk, "gpt0_mux_clk", NULL);
+	clk = clk_register_gate(NULL, "gpt0_clk", "gpt0_mux_clk", 0,
+			SPEAR1310_PERIP1_CLK_ENB, SPEAR1310_GPT0_CLK_ENB, 0,
+			&_lock);
+	clk_register_clkdev(clk, NULL, "gpt0");
+
+	clk = clk_register_mux(NULL, "gpt1_mux_clk", gpt_parents,
+			ARRAY_SIZE(gpt_parents), 0, SPEAR1310_PERIP_CLK_CFG,
+			SPEAR1310_GPT1_CLK_SHIFT, SPEAR1310_GPT_CLK_MASK, 0,
+			&_lock);
+	clk_register_clkdev(clk, "gpt1_mux_clk", NULL);
+	clk = clk_register_gate(NULL, "gpt1_clk", "gpt1_mux_clk", 0,
+			SPEAR1310_PERIP1_CLK_ENB, SPEAR1310_GPT1_CLK_ENB, 0,
+			&_lock);
+	clk_register_clkdev(clk, NULL, "gpt1");
+
+	clk = clk_register_mux(NULL, "gpt2_mux_clk", gpt_parents,
+			ARRAY_SIZE(gpt_parents), 0, SPEAR1310_PERIP_CLK_CFG,
+			SPEAR1310_GPT2_CLK_SHIFT, SPEAR1310_GPT_CLK_MASK, 0,
+			&_lock);
+	clk_register_clkdev(clk, "gpt2_mux_clk", NULL);
+	clk = clk_register_gate(NULL, "gpt2_clk", "gpt2_mux_clk", 0,
+			SPEAR1310_PERIP2_CLK_ENB, SPEAR1310_GPT2_CLK_ENB, 0,
+			&_lock);
+	clk_register_clkdev(clk, NULL, "gpt2");
+
+	clk = clk_register_mux(NULL, "gpt3_mux_clk", gpt_parents,
+			ARRAY_SIZE(gpt_parents), 0, SPEAR1310_PERIP_CLK_CFG,
+			SPEAR1310_GPT3_CLK_SHIFT, SPEAR1310_GPT_CLK_MASK, 0,
+			&_lock);
+	clk_register_clkdev(clk, "gpt3_mux_clk", NULL);
+	clk = clk_register_gate(NULL, "gpt3_clk", "gpt3_mux_clk", 0,
+			SPEAR1310_PERIP2_CLK_ENB, SPEAR1310_GPT3_CLK_ENB, 0,
+			&_lock);
+	clk_register_clkdev(clk, NULL, "gpt3");
+
+	/* others */
+	clk = clk_register_aux("uart_synth_clk", "uart_synth_gate_clk",
+			"vco1div2_clk", 0, SPEAR1310_UART_CLK_SYNT, NULL,
+			aux_rtbl, ARRAY_SIZE(aux_rtbl), &_lock, &clk1);
+	clk_register_clkdev(clk, "uart_synth_clk", NULL);
+	clk_register_clkdev(clk1, "uart_synth_gate_clk", NULL);
+
+	clk = clk_register_mux(NULL, "uart0_mux_clk", uart0_parents,
+			ARRAY_SIZE(uart0_parents), 0, SPEAR1310_PERIP_CLK_CFG,
+			SPEAR1310_UART_CLK_SHIFT, SPEAR1310_UART_CLK_MASK, 0,
+			&_lock);
+	clk_register_clkdev(clk, "uart0_mux_clk", NULL);
+
+	clk = clk_register_gate(NULL, "uart0_clk", "uart0_mux_clk", 0,
+			SPEAR1310_PERIP1_CLK_ENB, SPEAR1310_UART_CLK_ENB, 0,
+			&_lock);
+	clk_register_clkdev(clk, NULL, "e0000000.serial");
+
+	clk = clk_register_aux("sdhci_synth_clk", "sdhci_synth_gate_clk",
+			"vco1div2_clk", 0, SPEAR1310_SDHCI_CLK_SYNT, NULL,
+			aux_rtbl, ARRAY_SIZE(aux_rtbl), &_lock, &clk1);
+	clk_register_clkdev(clk, "sdhci_synth_clk", NULL);
+	clk_register_clkdev(clk1, "sdhci_synth_gate_clk", NULL);
+
+	clk = clk_register_gate(NULL, "sdhci_clk", "sdhci_synth_gate_clk", 0,
+			SPEAR1310_PERIP1_CLK_ENB, SPEAR1310_SDHCI_CLK_ENB, 0,
+			&_lock);
+	clk_register_clkdev(clk, NULL, "b3000000.sdhci");
+
+	clk = clk_register_aux("cfxd_synth_clk", "cfxd_synth_gate_clk",
+			"vco1div2_clk", 0, SPEAR1310_CFXD_CLK_SYNT, NULL,
+			aux_rtbl, ARRAY_SIZE(aux_rtbl), &_lock, &clk1);
+	clk_register_clkdev(clk, "cfxd_synth_clk", NULL);
+	clk_register_clkdev(clk1, "cfxd_synth_gate_clk", NULL);
+
+	clk = clk_register_gate(NULL, "cfxd_clk", "cfxd_synth_gate_clk", 0,
+			SPEAR1310_PERIP1_CLK_ENB, SPEAR1310_CFXD_CLK_ENB, 0,
+			&_lock);
+	clk_register_clkdev(clk, NULL, "b2800000.cf");
+	clk_register_clkdev(clk, NULL, "arasan_xd");
+
+	clk = clk_register_aux("c3_synth_clk", "c3_synth_gate_clk",
+			"vco1div2_clk", 0, SPEAR1310_C3_CLK_SYNT, NULL,
+			aux_rtbl, ARRAY_SIZE(aux_rtbl), &_lock, &clk1);
+	clk_register_clkdev(clk, "c3_synth_clk", NULL);
+	clk_register_clkdev(clk1, "c3_synth_gate_clk", NULL);
+
+	clk = clk_register_mux(NULL, "c3_mux_clk", c3_parents,
+			ARRAY_SIZE(c3_parents), 0, SPEAR1310_PERIP_CLK_CFG,
+			SPEAR1310_C3_CLK_SHIFT, SPEAR1310_C3_CLK_MASK, 0,
+			&_lock);
+	clk_register_clkdev(clk, "c3_mux_clk", NULL);
+
+	clk = clk_register_gate(NULL, "c3_clk", "c3_mux_clk", 0,
+			SPEAR1310_PERIP1_CLK_ENB, SPEAR1310_C3_CLK_ENB, 0,
+			&_lock);
+	clk_register_clkdev(clk, NULL, "c3");
+
+	/* gmac */
+	clk = clk_register_mux(NULL, "gmac_phy_input_mux_clk",
+			gmac_phy_input_parents,
+			ARRAY_SIZE(gmac_phy_input_parents), 0,
+			SPEAR1310_GMAC_CLK_CFG,
+			SPEAR1310_GMAC_PHY_INPUT_CLK_SHIFT,
+			SPEAR1310_GMAC_PHY_INPUT_CLK_MASK, 0, &_lock);
+	clk_register_clkdev(clk, "gmac_phy_input_mux_clk", NULL);
+
+	clk = clk_register_aux("gmac_phy_synth_clk", "gmac_phy_synth_gate_clk",
+			"gmac_phy_input_mux_clk", 0, SPEAR1310_GMAC_CLK_SYNT,
+			NULL, gmac_rtbl, ARRAY_SIZE(gmac_rtbl), &_lock, &clk1);
+	clk_register_clkdev(clk, "gmac_phy_synth_clk", NULL);
+	clk_register_clkdev(clk1, "gmac_phy_synth_gate_clk", NULL);
+
+	clk = clk_register_mux(NULL, "gmac_phy_mux_clk", gmac_phy_parents,
+			ARRAY_SIZE(gmac_phy_parents), 0,
+			SPEAR1310_PERIP_CLK_CFG, SPEAR1310_GMAC_PHY_CLK_SHIFT,
+			SPEAR1310_GMAC_PHY_CLK_MASK, 0, &_lock);
+	clk_register_clkdev(clk, NULL, "stmmacphy.0");
+
+	/* clcd */
+	clk = clk_register_mux(NULL, "clcd_synth_mux_clk", clcd_synth_parents,
+			ARRAY_SIZE(clcd_synth_parents), 0,
+			SPEAR1310_CLCD_CLK_SYNT, SPEAR1310_CLCD_SYNT_CLK_SHIFT,
+			SPEAR1310_CLCD_SYNT_CLK_MASK, 0, &_lock);
+	clk_register_clkdev(clk, "clcd_synth_mux_clk", NULL);
+
+	clk = clk_register_frac("clcd_synth_clk", "clcd_synth_mux_clk", 0,
+			SPEAR1310_CLCD_CLK_SYNT, clcd_rtbl,
+			ARRAY_SIZE(clcd_rtbl), &_lock);
+	clk_register_clkdev(clk, "clcd_synth_clk", NULL);
+
+	clk = clk_register_mux(NULL, "clcd_pixel_mux_clk", clcd_pixel_parents,
+			ARRAY_SIZE(clcd_pixel_parents), 0,
+			SPEAR1310_PERIP_CLK_CFG, SPEAR1310_CLCD_CLK_SHIFT,
+			SPEAR1310_CLCD_CLK_MASK, 0, &_lock);
+	clk_register_clkdev(clk, "clcd_pixel_clk", NULL);
+
+	clk = clk_register_gate(NULL, "clcd_clk", "clcd_pixel_mux_clk", 0,
+			SPEAR1310_PERIP1_CLK_ENB, SPEAR1310_CLCD_CLK_ENB, 0,
+			&_lock);
+	clk_register_clkdev(clk, "clcd_clk", NULL);
+
+	/* i2s */
+	clk = clk_register_mux(NULL, "i2s_src_mux_clk", i2s_src_parents,
+			ARRAY_SIZE(i2s_src_parents), 0, SPEAR1310_I2S_CLK_CFG,
+			SPEAR1310_I2S_SRC_CLK_SHIFT, SPEAR1310_I2S_SRC_CLK_MASK,
+			0, &_lock);
+	clk_register_clkdev(clk, "i2s_src_clk", NULL);
+
+	clk = clk_register_aux("i2s_prs1_clk", NULL, "i2s_src_mux_clk", 0,
+			SPEAR1310_I2S_CLK_CFG, &i2s_prs1_masks, i2s_prs1_rtbl,
+			ARRAY_SIZE(i2s_prs1_rtbl), &_lock, NULL);
+	clk_register_clkdev(clk, "i2s_prs1_clk", NULL);
+
+	clk = clk_register_mux(NULL, "i2s_ref_mux_clk", i2s_ref_parents,
+			ARRAY_SIZE(i2s_ref_parents), 0, SPEAR1310_I2S_CLK_CFG,
+			SPEAR1310_I2S_REF_SHIFT, SPEAR1310_I2S_REF_SEL_MASK, 0,
+			&_lock);
+	clk_register_clkdev(clk, "i2s_ref_clk", NULL);
+
+	clk = clk_register_gate(NULL, "i2s_ref_pad_clk", "i2s_ref_mux_clk", 0,
+			SPEAR1310_PERIP2_CLK_ENB, SPEAR1310_I2S_REF_PAD_CLK_ENB,
+			0, &_lock);
+	clk_register_clkdev(clk, "i2s_ref_pad_clk", NULL);
+
+	clk = clk_register_aux("i2s_sclk_clk", "i2s_sclk_gate_clk",
+			"i2s_ref_pad_clk", 0, SPEAR1310_I2S_CLK_CFG,
+			&i2s_sclk_masks, i2s_sclk_rtbl,
+			ARRAY_SIZE(i2s_sclk_rtbl), &_lock, &clk1);
+	clk_register_clkdev(clk, "i2s_sclk_clk", NULL);
+	clk_register_clkdev(clk1, "i2s_sclk_gate_clk", NULL);
+
+	/* clock derived from ahb clk */
+	clk = clk_register_gate(NULL, "i2c0_clk", "ahb_clk", 0,
+			SPEAR1310_PERIP1_CLK_ENB, SPEAR1310_I2C0_CLK_ENB, 0,
+			&_lock);
+	clk_register_clkdev(clk, NULL, "e0280000.i2c");
+
+	clk = clk_register_gate(NULL, "dma_clk", "ahb_clk", 0,
+			SPEAR1310_PERIP1_CLK_ENB, SPEAR1310_DMA_CLK_ENB, 0,
+			&_lock);
+	clk_register_clkdev(clk, NULL, "ea800000.dma");
+	clk_register_clkdev(clk, NULL, "eb000000.dma");
+
+	clk = clk_register_gate(NULL, "jpeg_clk", "ahb_clk", 0,
+			SPEAR1310_PERIP1_CLK_ENB, SPEAR1310_JPEG_CLK_ENB, 0,
+			&_lock);
+	clk_register_clkdev(clk, NULL, "b2000000.jpeg");
+
+	clk = clk_register_gate(NULL, "gmac_clk", "ahb_clk", 0,
+			SPEAR1310_PERIP1_CLK_ENB, SPEAR1310_GMAC_CLK_ENB, 0,
+			&_lock);
+	clk_register_clkdev(clk, NULL, "e2000000.eth");
+
+	clk = clk_register_gate(NULL, "fsmc_clk", "ahb_clk", 0,
+			SPEAR1310_PERIP1_CLK_ENB, SPEAR1310_FSMC_CLK_ENB, 0,
+			&_lock);
+	clk_register_clkdev(clk, NULL, "b0000000.flash");
+
+	clk = clk_register_gate(NULL, "smi_clk", "ahb_clk", 0,
+			SPEAR1310_PERIP1_CLK_ENB, SPEAR1310_SMI_CLK_ENB, 0,
+			&_lock);
+	clk_register_clkdev(clk, NULL, "ea000000.flash");
+
+	clk = clk_register_gate(NULL, "usbh0_clk", "ahb_clk", 0,
+			SPEAR1310_PERIP1_CLK_ENB, SPEAR1310_UHC0_CLK_ENB, 0,
+			&_lock);
+	clk_register_clkdev(clk, "usbh.0_clk", NULL);
+
+	clk = clk_register_gate(NULL, "usbh1_clk", "ahb_clk", 0,
+			SPEAR1310_PERIP1_CLK_ENB, SPEAR1310_UHC1_CLK_ENB, 0,
+			&_lock);
+	clk_register_clkdev(clk, "usbh.1_clk", NULL);
+
+	clk = clk_register_gate(NULL, "uoc_clk", "ahb_clk", 0,
+			SPEAR1310_PERIP1_CLK_ENB, SPEAR1310_UOC_CLK_ENB, 0,
+			&_lock);
+	clk_register_clkdev(clk, NULL, "uoc");
+
+	clk = clk_register_gate(NULL, "pcie_sata_0_clk", "ahb_clk", 0,
+			SPEAR1310_PERIP1_CLK_ENB, SPEAR1310_PCIE_SATA_0_CLK_ENB,
+			0, &_lock);
+	clk_register_clkdev(clk, NULL, "dw_pcie.0");
+	clk_register_clkdev(clk, NULL, "ahci.0");
+
+	clk = clk_register_gate(NULL, "pcie_sata_1_clk", "ahb_clk", 0,
+			SPEAR1310_PERIP1_CLK_ENB, SPEAR1310_PCIE_SATA_1_CLK_ENB,
+			0, &_lock);
+	clk_register_clkdev(clk, NULL, "dw_pcie.1");
+	clk_register_clkdev(clk, NULL, "ahci.1");
+
+	clk = clk_register_gate(NULL, "pcie_sata_2_clk", "ahb_clk", 0,
+			SPEAR1310_PERIP1_CLK_ENB, SPEAR1310_PCIE_SATA_2_CLK_ENB,
+			0, &_lock);
+	clk_register_clkdev(clk, NULL, "dw_pcie.2");
+	clk_register_clkdev(clk, NULL, "ahci.2");
+
+	clk = clk_register_gate(NULL, "sysram0_clk", "ahb_clk", 0,
+			SPEAR1310_PERIP1_CLK_ENB, SPEAR1310_SYSRAM0_CLK_ENB, 0,
+			&_lock);
+	clk_register_clkdev(clk, "sysram0_clk", NULL);
+
+	clk = clk_register_gate(NULL, "sysram1_clk", "ahb_clk", 0,
+			SPEAR1310_PERIP1_CLK_ENB, SPEAR1310_SYSRAM1_CLK_ENB, 0,
+			&_lock);
+	clk_register_clkdev(clk, "sysram1_clk", NULL);
+
+	clk = clk_register_aux("adc_synth_clk", "adc_synth_gate_clk", "ahb_clk",
+			0, SPEAR1310_ADC_CLK_SYNT, NULL, adc_rtbl,
+			ARRAY_SIZE(adc_rtbl), &_lock, &clk1);
+	clk_register_clkdev(clk, "adc_synth_clk", NULL);
+	clk_register_clkdev(clk1, "adc_synth_gate_clk", NULL);
+
+	clk = clk_register_gate(NULL, "adc_clk", "adc_synth_gate_clk", 0,
+			SPEAR1310_PERIP1_CLK_ENB, SPEAR1310_ADC_CLK_ENB, 0,
+			&_lock);
+	clk_register_clkdev(clk, NULL, "adc_clk");
+
+	/* clock derived from apb clk */
+	clk = clk_register_gate(NULL, "ssp0_clk", "apb_clk", 0,
+			SPEAR1310_PERIP1_CLK_ENB, SPEAR1310_SSP_CLK_ENB, 0,
+			&_lock);
+	clk_register_clkdev(clk, NULL, "e0100000.spi");
+
+	clk = clk_register_gate(NULL, "gpio0_clk", "apb_clk", 0,
+			SPEAR1310_PERIP1_CLK_ENB, SPEAR1310_GPIO0_CLK_ENB, 0,
+			&_lock);
+	clk_register_clkdev(clk, NULL, "e0600000.gpio");
+
+	clk = clk_register_gate(NULL, "gpio1_clk", "apb_clk", 0,
+			SPEAR1310_PERIP1_CLK_ENB, SPEAR1310_GPIO1_CLK_ENB, 0,
+			&_lock);
+	clk_register_clkdev(clk, NULL, "e0680000.gpio");
+
+	clk = clk_register_gate(NULL, "i2s0_clk", "apb_clk", 0,
+			SPEAR1310_PERIP1_CLK_ENB, SPEAR1310_I2S0_CLK_ENB, 0,
+			&_lock);
+	clk_register_clkdev(clk, NULL, "e0180000.i2s");
+
+	clk = clk_register_gate(NULL, "i2s1_clk", "apb_clk", 0,
+			SPEAR1310_PERIP1_CLK_ENB, SPEAR1310_I2S1_CLK_ENB, 0,
+			&_lock);
+	clk_register_clkdev(clk, NULL, "e0200000.i2s");
+
+	clk = clk_register_gate(NULL, "kbd_clk", "apb_clk", 0,
+			SPEAR1310_PERIP2_CLK_ENB, SPEAR1310_KBD_CLK_ENB, 0,
+			&_lock);
+	clk_register_clkdev(clk, NULL, "e0300000.kbd");
+
+	/* RAS clks */
+	clk = clk_register_mux(NULL, "gen_synth0_1_mux_clk",
+			gen_synth0_1_parents, ARRAY_SIZE(gen_synth0_1_parents),
+			0, SPEAR1310_PLL_CFG, SPEAR1310_RAS_SYNT0_1_CLK_SHIFT,
+			SPEAR1310_RAS_SYNT_CLK_MASK, 0, &_lock);
+	clk_register_clkdev(clk, "gen_synth0_1_clk", NULL);
+
+	clk = clk_register_mux(NULL, "gen_synth2_3_mux_clk",
+			gen_synth2_3_parents, ARRAY_SIZE(gen_synth2_3_parents),
+			0, SPEAR1310_PLL_CFG, SPEAR1310_RAS_SYNT2_3_CLK_SHIFT,
+			SPEAR1310_RAS_SYNT_CLK_MASK, 0, &_lock);
+	clk_register_clkdev(clk, "gen_synth2_3_clk", NULL);
+
+	clk = clk_register_frac("gen_synth0_clk", "gen_synth0_1_clk", 0,
+			SPEAR1310_RAS_CLK_SYNT0, gen_rtbl, ARRAY_SIZE(gen_rtbl),
+			&_lock);
+	clk_register_clkdev(clk, "gen_synth0_clk", NULL);
+
+	clk = clk_register_frac("gen_synth1_clk", "gen_synth0_1_clk", 0,
+			SPEAR1310_RAS_CLK_SYNT1, gen_rtbl, ARRAY_SIZE(gen_rtbl),
+			&_lock);
+	clk_register_clkdev(clk, "gen_synth1_clk", NULL);
+
+	clk = clk_register_frac("gen_synth2_clk", "gen_synth2_3_clk", 0,
+			SPEAR1310_RAS_CLK_SYNT2, gen_rtbl, ARRAY_SIZE(gen_rtbl),
+			&_lock);
+	clk_register_clkdev(clk, "gen_synth2_clk", NULL);
+
+	clk = clk_register_frac("gen_synth3_clk", "gen_synth2_3_clk", 0,
+			SPEAR1310_RAS_CLK_SYNT3, gen_rtbl, ARRAY_SIZE(gen_rtbl),
+			&_lock);
+	clk_register_clkdev(clk, "gen_synth3_clk", NULL);
+
+	clk = clk_register_gate(NULL, "ras_osc_24m_clk", "osc_24m_clk", 0,
+			SPEAR1310_RAS_CLK_ENB, SPEAR1310_OSC_24M_CLK_ENB, 0,
+			&_lock);
+	clk_register_clkdev(clk, "ras_osc_24m_clk", NULL);
+
+	clk = clk_register_gate(NULL, "ras_osc_25m_clk", "osc_25m_clk", 0,
+			SPEAR1310_RAS_CLK_ENB, SPEAR1310_OSC_25M_CLK_ENB, 0,
+			&_lock);
+	clk_register_clkdev(clk, "ras_osc_25m_clk", NULL);
+
+	clk = clk_register_gate(NULL, "ras_osc_32k_clk", "osc_32k_clk", 0,
+			SPEAR1310_RAS_CLK_ENB, SPEAR1310_OSC_32K_CLK_ENB, 0,
+			&_lock);
+	clk_register_clkdev(clk, "ras_osc_32k_clk", NULL);
+
+	clk = clk_register_gate(NULL, "ras_pll2_clk", "pll2_clk", 0,
+			SPEAR1310_RAS_CLK_ENB, SPEAR1310_PLL2_CLK_ENB, 0,
+			&_lock);
+	clk_register_clkdev(clk, "ras_pll2_clk", NULL);
+
+	clk = clk_register_gate(NULL, "ras_pll3_clk", "pll3_clk", 0,
+			SPEAR1310_RAS_CLK_ENB, SPEAR1310_PLL3_CLK_ENB, 0,
+			&_lock);
+	clk_register_clkdev(clk, "ras_pll3_clk", NULL);
+
+	clk = clk_register_gate(NULL, "ras_tx125_clk", "gmii_125m_pad_clk", 0,
+			SPEAR1310_RAS_CLK_ENB, SPEAR1310_C125M_PAD_CLK_ENB, 0,
+			&_lock);
+	clk_register_clkdev(clk, "ras_tx125_clk", NULL);
+
+	clk = clk_register_fixed_rate(NULL, "ras_30m_fixed_clk", "pll5_clk", 0,
+			30000000);
+	clk = clk_register_gate(NULL, "ras_30m_clk", "ras_30m_fixed_clk", 0,
+			SPEAR1310_RAS_CLK_ENB, SPEAR1310_C30M_CLK_ENB, 0,
+			&_lock);
+	clk_register_clkdev(clk, "ras_30m_clk", NULL);
+
+	clk = clk_register_fixed_rate(NULL, "ras_48m_fixed_clk", "pll5_clk", 0,
+			48000000);
+	clk = clk_register_gate(NULL, "ras_48m_clk", "ras_48m_fixed_clk", 0,
+			SPEAR1310_RAS_CLK_ENB, SPEAR1310_C48M_CLK_ENB, 0,
+			&_lock);
+	clk_register_clkdev(clk, "ras_48m_clk", NULL);
+
+	clk = clk_register_gate(NULL, "ras_ahb_clk", "ahb_clk", 0,
+			SPEAR1310_RAS_CLK_ENB, SPEAR1310_ACLK_CLK_ENB, 0,
+			&_lock);
+	clk_register_clkdev(clk, "ras_ahb_clk", NULL);
+
+	clk = clk_register_gate(NULL, "ras_apb_clk", "apb_clk", 0,
+			SPEAR1310_RAS_CLK_ENB, SPEAR1310_PCLK_CLK_ENB, 0,
+			&_lock);
+	clk_register_clkdev(clk, "ras_apb_clk", NULL);
+
+	clk = clk_register_fixed_rate(NULL, "ras_plclk0_clk", NULL, CLK_IS_ROOT,
+			50000000);
+
+	clk = clk_register_fixed_rate(NULL, "ras_tx50_clk", NULL, CLK_IS_ROOT,
+			50000000);
+
+	clk = clk_register_gate(NULL, "can0_clk", "apb_clk", 0,
+			SPEAR1310_RAS_SW_CLK_CTRL, SPEAR1310_CAN0_CLK_ENB, 0,
+			&_lock);
+	clk_register_clkdev(clk, NULL, "c_can_platform.0");
+
+	clk = clk_register_gate(NULL, "can1_clk", "apb_clk", 0,
+			SPEAR1310_RAS_SW_CLK_CTRL, SPEAR1310_CAN1_CLK_ENB, 0,
+			&_lock);
+	clk_register_clkdev(clk, NULL, "c_can_platform.1");
+
+	clk = clk_register_gate(NULL, "ras_smii0_clk", "ras_ahb_clk", 0,
+			SPEAR1310_RAS_SW_CLK_CTRL, SPEAR1310_MII0_CLK_ENB, 0,
+			&_lock);
+	clk_register_clkdev(clk, NULL, "5c400000.eth");
+
+	clk = clk_register_gate(NULL, "ras_smii1_clk", "ras_ahb_clk", 0,
+			SPEAR1310_RAS_SW_CLK_CTRL, SPEAR1310_MII1_CLK_ENB, 0,
+			&_lock);
+	clk_register_clkdev(clk, NULL, "5c500000.eth");
+
+	clk = clk_register_gate(NULL, "ras_smii2_clk", "ras_ahb_clk", 0,
+			SPEAR1310_RAS_SW_CLK_CTRL, SPEAR1310_MII2_CLK_ENB, 0,
+			&_lock);
+	clk_register_clkdev(clk, NULL, "5c600000.eth");
+
+	clk = clk_register_gate(NULL, "ras_rgmii_clk", "ras_ahb_clk", 0,
+			SPEAR1310_RAS_SW_CLK_CTRL, SPEAR1310_GMII_CLK_ENB, 0,
+			&_lock);
+	clk_register_clkdev(clk, NULL, "5c700000.eth");
+
+	clk = clk_register_mux(NULL, "smii_rgmii_phy_mux_clk",
+			smii_rgmii_phy_parents,
+			ARRAY_SIZE(smii_rgmii_phy_parents), 0,
+			SPEAR1310_RAS_CTRL_REG1,
+			SPEAR1310_SMII_RGMII_PHY_CLK_SHIFT,
+			SPEAR1310_PHY_CLK_MASK, 0, &_lock);
+	clk_register_clkdev(clk, NULL, "stmmacphy.1");
+	clk_register_clkdev(clk, NULL, "stmmacphy.2");
+	clk_register_clkdev(clk, NULL, "stmmacphy.4");
+
+	clk = clk_register_mux(NULL, "rmii_phy_mux_clk", rmii_phy_parents,
+			ARRAY_SIZE(rmii_phy_parents), 0,
+			SPEAR1310_RAS_CTRL_REG1, SPEAR1310_RMII_PHY_CLK_SHIFT,
+			SPEAR1310_PHY_CLK_MASK, 0, &_lock);
+	clk_register_clkdev(clk, NULL, "stmmacphy.3");
+
+	clk = clk_register_mux(NULL, "uart1_mux_clk", uart_parents,
+			ARRAY_SIZE(uart_parents), 0, SPEAR1310_RAS_CTRL_REG0,
+			SPEAR1310_UART1_CLK_SHIFT, SPEAR1310_RAS_UART_CLK_MASK,
+			0, &_lock);
+	clk_register_clkdev(clk, "uart1_mux_clk", NULL);
+
+	clk = clk_register_gate(NULL, "uart1_clk", "uart1_mux_clk", 0,
+			SPEAR1310_RAS_SW_CLK_CTRL, SPEAR1310_UART1_CLK_ENB, 0,
+			&_lock);
+	clk_register_clkdev(clk, NULL, "5c800000.serial");
+
+	clk = clk_register_mux(NULL, "uart2_mux_clk", uart_parents,
+			ARRAY_SIZE(uart_parents), 0, SPEAR1310_RAS_CTRL_REG0,
+			SPEAR1310_UART2_CLK_SHIFT, SPEAR1310_RAS_UART_CLK_MASK,
+			0, &_lock);
+	clk_register_clkdev(clk, "uart2_mux_clk", NULL);
+
+	clk = clk_register_gate(NULL, "uart2_clk", "uart2_mux_clk", 0,
+			SPEAR1310_RAS_SW_CLK_CTRL, SPEAR1310_UART2_CLK_ENB, 0,
+			&_lock);
+	clk_register_clkdev(clk, NULL, "5c900000.serial");
+
+	clk = clk_register_mux(NULL, "uart3_mux_clk", uart_parents,
+			ARRAY_SIZE(uart_parents), 0, SPEAR1310_RAS_CTRL_REG0,
+			SPEAR1310_UART3_CLK_SHIFT, SPEAR1310_RAS_UART_CLK_MASK,
+			0, &_lock);
+	clk_register_clkdev(clk, "uart3_mux_clk", NULL);
+
+	clk = clk_register_gate(NULL, "uart3_clk", "uart3_mux_clk", 0,
+			SPEAR1310_RAS_SW_CLK_CTRL, SPEAR1310_UART3_CLK_ENB, 0,
+			&_lock);
+	clk_register_clkdev(clk, NULL, "5ca00000.serial");
+
+	clk = clk_register_mux(NULL, "uart4_mux_clk", uart_parents,
+			ARRAY_SIZE(uart_parents), 0, SPEAR1310_RAS_CTRL_REG0,
+			SPEAR1310_UART4_CLK_SHIFT, SPEAR1310_RAS_UART_CLK_MASK,
+			0, &_lock);
+	clk_register_clkdev(clk, "uart4_mux_clk", NULL);
+
+	clk = clk_register_gate(NULL, "uart4_clk", "uart4_mux_clk", 0,
+			SPEAR1310_RAS_SW_CLK_CTRL, SPEAR1310_UART4_CLK_ENB, 0,
+			&_lock);
+	clk_register_clkdev(clk, NULL, "5cb00000.serial");
+
+	clk = clk_register_mux(NULL, "uart5_mux_clk", uart_parents,
+			ARRAY_SIZE(uart_parents), 0, SPEAR1310_RAS_CTRL_REG0,
+			SPEAR1310_UART5_CLK_SHIFT, SPEAR1310_RAS_UART_CLK_MASK,
+			0, &_lock);
+	clk_register_clkdev(clk, "uart5_mux_clk", NULL);
+
+	clk = clk_register_gate(NULL, "uart5_clk", "uart5_mux_clk", 0,
+			SPEAR1310_RAS_SW_CLK_CTRL, SPEAR1310_UART5_CLK_ENB, 0,
+			&_lock);
+	clk_register_clkdev(clk, NULL, "5cc00000.serial");
+
+	clk = clk_register_mux(NULL, "i2c1_mux_clk", i2c_parents,
+			ARRAY_SIZE(i2c_parents), 0, SPEAR1310_RAS_CTRL_REG0,
+			SPEAR1310_I2C1_CLK_SHIFT, SPEAR1310_I2C_CLK_MASK, 0,
+			&_lock);
+	clk_register_clkdev(clk, "i2c1_mux_clk", NULL);
+
+	clk = clk_register_gate(NULL, "i2c1_clk", "i2c1_mux_clk", 0,
+			SPEAR1310_RAS_SW_CLK_CTRL, SPEAR1310_I2C1_CLK_ENB, 0,
+			&_lock);
+	clk_register_clkdev(clk, NULL, "5cd00000.i2c");
+
+	clk = clk_register_mux(NULL, "i2c2_mux_clk", i2c_parents,
+			ARRAY_SIZE(i2c_parents), 0, SPEAR1310_RAS_CTRL_REG0,
+			SPEAR1310_I2C2_CLK_SHIFT, SPEAR1310_I2C_CLK_MASK, 0,
+			&_lock);
+	clk_register_clkdev(clk, "i2c2_mux_clk", NULL);
+
+	clk = clk_register_gate(NULL, "i2c2_clk", "i2c2_mux_clk", 0,
+			SPEAR1310_RAS_SW_CLK_CTRL, SPEAR1310_I2C2_CLK_ENB, 0,
+			&_lock);
+	clk_register_clkdev(clk, NULL, "5ce00000.i2c");
+
+	clk = clk_register_mux(NULL, "i2c3_mux_clk", i2c_parents,
+			ARRAY_SIZE(i2c_parents), 0, SPEAR1310_RAS_CTRL_REG0,
+			SPEAR1310_I2C3_CLK_SHIFT, SPEAR1310_I2C_CLK_MASK, 0,
+			&_lock);
+	clk_register_clkdev(clk, "i2c3_mux_clk", NULL);
+
+	clk = clk_register_gate(NULL, "i2c3_clk", "i2c3_mux_clk", 0,
+			SPEAR1310_RAS_SW_CLK_CTRL, SPEAR1310_I2C3_CLK_ENB, 0,
+			&_lock);
+	clk_register_clkdev(clk, NULL, "5cf00000.i2c");
+
+	clk = clk_register_mux(NULL, "i2c4_mux_clk", i2c_parents,
+			ARRAY_SIZE(i2c_parents), 0, SPEAR1310_RAS_CTRL_REG0,
+			SPEAR1310_I2C4_CLK_SHIFT, SPEAR1310_I2C_CLK_MASK, 0,
+			&_lock);
+	clk_register_clkdev(clk, "i2c4_mux_clk", NULL);
+
+	clk = clk_register_gate(NULL, "i2c4_clk", "i2c4_mux_clk", 0,
+			SPEAR1310_RAS_SW_CLK_CTRL, SPEAR1310_I2C4_CLK_ENB, 0,
+			&_lock);
+	clk_register_clkdev(clk, NULL, "5d000000.i2c");
+
+	clk = clk_register_mux(NULL, "i2c5_mux_clk", i2c_parents,
+			ARRAY_SIZE(i2c_parents), 0, SPEAR1310_RAS_CTRL_REG0,
+			SPEAR1310_I2C5_CLK_SHIFT, SPEAR1310_I2C_CLK_MASK, 0,
+			&_lock);
+	clk_register_clkdev(clk, "i2c5_mux_clk", NULL);
+
+	clk = clk_register_gate(NULL, "i2c5_clk", "i2c5_mux_clk", 0,
+			SPEAR1310_RAS_SW_CLK_CTRL, SPEAR1310_I2C5_CLK_ENB, 0,
+			&_lock);
+	clk_register_clkdev(clk, NULL, "5d100000.i2c");
+
+	clk = clk_register_mux(NULL, "i2c6_mux_clk", i2c_parents,
+			ARRAY_SIZE(i2c_parents), 0, SPEAR1310_RAS_CTRL_REG0,
+			SPEAR1310_I2C6_CLK_SHIFT, SPEAR1310_I2C_CLK_MASK, 0,
+			&_lock);
+	clk_register_clkdev(clk, "i2c6_mux_clk", NULL);
+
+	clk = clk_register_gate(NULL, "i2c6_clk", "i2c6_mux_clk", 0,
+			SPEAR1310_RAS_SW_CLK_CTRL, SPEAR1310_I2C6_CLK_ENB, 0,
+			&_lock);
+	clk_register_clkdev(clk, NULL, "5d200000.i2c");
+
+	clk = clk_register_mux(NULL, "i2c7_mux_clk", i2c_parents,
+			ARRAY_SIZE(i2c_parents), 0, SPEAR1310_RAS_CTRL_REG0,
+			SPEAR1310_I2C7_CLK_SHIFT, SPEAR1310_I2C_CLK_MASK, 0,
+			&_lock);
+	clk_register_clkdev(clk, "i2c7_mux_clk", NULL);
+
+	clk = clk_register_gate(NULL, "i2c7_clk", "i2c7_mux_clk", 0,
+			SPEAR1310_RAS_SW_CLK_CTRL, SPEAR1310_I2C7_CLK_ENB, 0,
+			&_lock);
+	clk_register_clkdev(clk, NULL, "5d300000.i2c");
+
+	clk = clk_register_mux(NULL, "ssp1_mux_clk", ssp1_parents,
+			ARRAY_SIZE(ssp1_parents), 0, SPEAR1310_RAS_CTRL_REG0,
+			SPEAR1310_SSP1_CLK_SHIFT, SPEAR1310_SSP1_CLK_MASK, 0,
+			&_lock);
+	clk_register_clkdev(clk, "ssp1_mux_clk", NULL);
+
+	clk = clk_register_gate(NULL, "ssp1_clk", "ssp1_mux_clk", 0,
+			SPEAR1310_RAS_SW_CLK_CTRL, SPEAR1310_SSP1_CLK_ENB, 0,
+			&_lock);
+	clk_register_clkdev(clk, NULL, "5d400000.spi");
+
+	clk = clk_register_mux(NULL, "pci_mux_clk", pci_parents,
+			ARRAY_SIZE(pci_parents), 0, SPEAR1310_RAS_CTRL_REG0,
+			SPEAR1310_PCI_CLK_SHIFT, SPEAR1310_PCI_CLK_MASK, 0,
+			&_lock);
+	clk_register_clkdev(clk, "pci_mux_clk", NULL);
+
+	clk = clk_register_gate(NULL, "pci_clk", "pci_mux_clk", 0,
+			SPEAR1310_RAS_SW_CLK_CTRL, SPEAR1310_PCI_CLK_ENB, 0,
+			&_lock);
+	clk_register_clkdev(clk, NULL, "pci");
+
+	clk = clk_register_mux(NULL, "tdm1_mux_clk", tdm_parents,
+			ARRAY_SIZE(tdm_parents), 0, SPEAR1310_RAS_CTRL_REG0,
+			SPEAR1310_TDM1_CLK_SHIFT, SPEAR1310_TDM_CLK_MASK, 0,
+			&_lock);
+	clk_register_clkdev(clk, "tdm1_mux_clk", NULL);
+
+	clk = clk_register_gate(NULL, "tdm1_clk", "tdm1_mux_clk", 0,
+			SPEAR1310_RAS_SW_CLK_CTRL, SPEAR1310_TDM1_CLK_ENB, 0,
+			&_lock);
+	clk_register_clkdev(clk, NULL, "tdm_hdlc.0");
+
+	clk = clk_register_mux(NULL, "tdm2_mux_clk", tdm_parents,
+			ARRAY_SIZE(tdm_parents), 0, SPEAR1310_RAS_CTRL_REG0,
+			SPEAR1310_TDM2_CLK_SHIFT, SPEAR1310_TDM_CLK_MASK, 0,
+			&_lock);
+	clk_register_clkdev(clk, "tdm2_mux_clk", NULL);
+
+	clk = clk_register_gate(NULL, "tdm2_clk", "tdm2_mux_clk", 0,
+			SPEAR1310_RAS_SW_CLK_CTRL, SPEAR1310_TDM2_CLK_ENB, 0,
+			&_lock);
+	clk_register_clkdev(clk, NULL, "tdm_hdlc.1");
+}
diff --git a/drivers/clk/spear/spear1340_clock.c b/drivers/clk/spear/spear1340_clock.c
new file mode 100644
index 00000000000..f130919d5bf
--- /dev/null
+++ b/drivers/clk/spear/spear1340_clock.c
@@ -0,0 +1,964 @@
+/*
+ * arch/arm/mach-spear13xx/spear1340_clock.c
+ *
+ * SPEAr1340 machine clock framework source file
+ *
+ * Copyright (C) 2012 ST Microelectronics
+ * Viresh Kumar <viresh.kumar@st.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/clk.h>
+#include <linux/clkdev.h>
+#include <linux/err.h>
+#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_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)
+	/* PLL_CFG bit values */
+	#define SPEAR1340_CLCD_SYNT_CLK_MASK		1
+	#define SPEAR1340_CLCD_SYNT_CLK_SHIFT		31
+	#define SPEAR1340_GEN_SYNT2_3_CLK_SHIFT		29
+	#define SPEAR1340_GEN_SYNT_CLK_MASK		2
+	#define SPEAR1340_GEN_SYNT0_1_CLK_SHIFT		27
+	#define SPEAR1340_PLL_CLK_MASK			2
+	#define SPEAR1340_PLL3_CLK_SHIFT		24
+	#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)
+	/* PERIP_CLK_CFG bit values */
+	#define SPEAR1340_SPDIF_CLK_MASK		1
+	#define SPEAR1340_SPDIF_OUT_CLK_SHIFT		15
+	#define SPEAR1340_SPDIF_IN_CLK_SHIFT		14
+	#define SPEAR1340_GPT3_CLK_SHIFT		13
+	#define SPEAR1340_GPT2_CLK_SHIFT		12
+	#define SPEAR1340_GPT_CLK_MASK			1
+	#define SPEAR1340_GPT1_CLK_SHIFT		9
+	#define SPEAR1340_GPT0_CLK_SHIFT		8
+	#define SPEAR1340_UART_CLK_MASK			2
+	#define SPEAR1340_UART1_CLK_SHIFT		6
+	#define SPEAR1340_UART0_CLK_SHIFT		4
+	#define SPEAR1340_CLCD_CLK_MASK			2
+	#define SPEAR1340_CLCD_CLK_SHIFT		2
+	#define SPEAR1340_C3_CLK_MASK			1
+	#define SPEAR1340_C3_CLK_SHIFT			1
+
+#define SPEAR1340_GMAC_CLK_CFG			(VA_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)
+	/* I2S_CLK_CFG register mask */
+	#define SPEAR1340_I2S_SCLK_X_MASK		0x1F
+	#define SPEAR1340_I2S_SCLK_X_SHIFT		27
+	#define SPEAR1340_I2S_SCLK_Y_MASK		0x1F
+	#define SPEAR1340_I2S_SCLK_Y_SHIFT		22
+	#define SPEAR1340_I2S_SCLK_EQ_SEL_SHIFT		21
+	#define SPEAR1340_I2S_SCLK_SYNTH_ENB		20
+	#define SPEAR1340_I2S_PRS1_CLK_X_MASK		0xFF
+	#define SPEAR1340_I2S_PRS1_CLK_X_SHIFT		12
+	#define SPEAR1340_I2S_PRS1_CLK_Y_MASK		0xFF
+	#define SPEAR1340_I2S_PRS1_CLK_Y_SHIFT		4
+	#define SPEAR1340_I2S_PRS1_EQ_SEL_SHIFT		3
+	#define SPEAR1340_I2S_REF_SEL_MASK		1
+	#define SPEAR1340_I2S_REF_SHIFT			2
+	#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_RTC_CLK_ENB			31
+	#define SPEAR1340_ADC_CLK_ENB			30
+	#define SPEAR1340_C3_CLK_ENB			29
+	#define SPEAR1340_CLCD_CLK_ENB			27
+	#define SPEAR1340_DMA_CLK_ENB			25
+	#define SPEAR1340_GPIO1_CLK_ENB			24
+	#define SPEAR1340_GPIO0_CLK_ENB			23
+	#define SPEAR1340_GPT1_CLK_ENB			22
+	#define SPEAR1340_GPT0_CLK_ENB			21
+	#define SPEAR1340_I2S_PLAY_CLK_ENB		20
+	#define SPEAR1340_I2S_REC_CLK_ENB		19
+	#define SPEAR1340_I2C0_CLK_ENB			18
+	#define SPEAR1340_SSP_CLK_ENB			17
+	#define SPEAR1340_UART0_CLK_ENB			15
+	#define SPEAR1340_PCIE_SATA_CLK_ENB		12
+	#define SPEAR1340_UOC_CLK_ENB			11
+	#define SPEAR1340_UHC1_CLK_ENB			10
+	#define SPEAR1340_UHC0_CLK_ENB			9
+	#define SPEAR1340_GMAC_CLK_ENB			8
+	#define SPEAR1340_CFXD_CLK_ENB			7
+	#define SPEAR1340_SDHCI_CLK_ENB			6
+	#define SPEAR1340_SMI_CLK_ENB			5
+	#define SPEAR1340_FSMC_CLK_ENB			4
+	#define SPEAR1340_SYSRAM0_CLK_ENB		3
+	#define SPEAR1340_SYSRAM1_CLK_ENB		2
+	#define SPEAR1340_SYSROM_CLK_ENB		1
+	#define SPEAR1340_BUS_CLK_ENB			0
+
+#define SPEAR1340_PERIP2_CLK_ENB		(VA_MISC_BASE + 0x310)
+	#define SPEAR1340_THSENS_CLK_ENB		8
+	#define SPEAR1340_I2S_REF_PAD_CLK_ENB		7
+	#define SPEAR1340_ACP_CLK_ENB			6
+	#define SPEAR1340_GPT3_CLK_ENB			5
+	#define SPEAR1340_GPT2_CLK_ENB			4
+	#define SPEAR1340_KBD_CLK_ENB			3
+	#define SPEAR1340_CPU_DBG_CLK_ENB		2
+	#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_PLGPIO_CLK_ENB		18
+	#define SPEAR1340_VIDEO_DEC_CLK_ENB		16
+	#define SPEAR1340_VIDEO_ENC_CLK_ENB		15
+	#define SPEAR1340_SPDIF_OUT_CLK_ENB		13
+	#define SPEAR1340_SPDIF_IN_CLK_ENB		12
+	#define SPEAR1340_VIDEO_IN_CLK_ENB		11
+	#define SPEAR1340_CAM0_CLK_ENB			10
+	#define SPEAR1340_CAM1_CLK_ENB			9
+	#define SPEAR1340_CAM2_CLK_ENB			8
+	#define SPEAR1340_CAM3_CLK_ENB			7
+	#define SPEAR1340_MALI_CLK_ENB			6
+	#define SPEAR1340_CEC0_CLK_ENB			5
+	#define SPEAR1340_CEC1_CLK_ENB			4
+	#define SPEAR1340_PWM_CLK_ENB			3
+	#define SPEAR1340_I2C1_CLK_ENB			2
+	#define SPEAR1340_UART1_CLK_ENB			1
+
+static DEFINE_SPINLOCK(_lock);
+
+/* pll rate configuration table, in ascending order of rates */
+static struct pll_rate_tbl pll_rtbl[] = {
+	/* PCLK 24MHz */
+	{.mode = 0, .m = 0x83, .n = 0x04, .p = 0x5}, /* vco 1572, pll 49.125 MHz */
+	{.mode = 0, .m = 0x7D, .n = 0x06, .p = 0x3}, /* vco 1000, pll 125 MHz */
+	{.mode = 0, .m = 0x64, .n = 0x06, .p = 0x1}, /* vco 800, pll 400 MHz */
+	{.mode = 0, .m = 0x7D, .n = 0x06, .p = 0x1}, /* vco 1000, pll 500 MHz */
+	{.mode = 0, .m = 0xA6, .n = 0x06, .p = 0x1}, /* vco 1328, pll 664 MHz */
+	{.mode = 0, .m = 0xC8, .n = 0x06, .p = 0x1}, /* vco 1600, pll 800 MHz */
+	{.mode = 0, .m = 0x7D, .n = 0x06, .p = 0x0}, /* vco 1, pll 1 GHz */
+	{.mode = 0, .m = 0x96, .n = 0x06, .p = 0x0}, /* vco 1200, pll 1200 MHz */
+};
+
+/* vco-pll4 rate configuration table, in ascending order of rates */
+static struct pll_rate_tbl pll4_rtbl[] = {
+	{.mode = 0, .m = 0x7D, .n = 0x06, .p = 0x2}, /* vco 1000, pll 250 MHz */
+	{.mode = 0, .m = 0xA6, .n = 0x06, .p = 0x2}, /* vco 1328, pll 332 MHz */
+	{.mode = 0, .m = 0xC8, .n = 0x06, .p = 0x2}, /* vco 1600, pll 400 MHz */
+	{.mode = 0, .m = 0x7D, .n = 0x06, .p = 0x0}, /* vco 1, pll 1 GHz */
+};
+
+/*
+ * All below entries generate 166 MHz for
+ * different values of vco1div2
+ */
+static struct frac_rate_tbl amba_synth_rtbl[] = {
+	{.div = 0x06062}, /* for vco1div2 = 500 MHz */
+	{.div = 0x04D1B}, /* for vco1div2 = 400 MHz */
+	{.div = 0x04000}, /* for vco1div2 = 332 MHz */
+	{.div = 0x03031}, /* for vco1div2 = 250 MHz */
+	{.div = 0x0268D}, /* for vco1div2 = 200 MHz */
+};
+
+/*
+ * Synthesizer Clock derived from vcodiv2. This clock is one of the
+ * possible clocks to feed cpu directly.
+ * We can program this synthesizer to make cpu run on different clock
+ * frequencies.
+ * Following table provides configuration values to let cpu run on 200,
+ * 250, 332, 400 or 500 MHz considering different possibilites of input
+ * (vco1div2) clock.
+ *
+ * --------------------------------------------------------------------
+ * vco1div2(Mhz)	fout(Mhz)	cpuclk = fout/2		div
+ * --------------------------------------------------------------------
+ * 400			200		100			0x04000
+ * 400			250		125			0x03333
+ * 400			332		166			0x0268D
+ * 400			400		200			0x02000
+ * --------------------------------------------------------------------
+ * 500			200		100			0x05000
+ * 500			250		125			0x04000
+ * 500			332		166			0x03031
+ * 500			400		200			0x02800
+ * 500			500		250			0x02000
+ * --------------------------------------------------------------------
+ * 664			200		100			0x06a38
+ * 664			250		125			0x054FD
+ * 664			332		166			0x04000
+ * 664			400		200			0x0351E
+ * 664			500		250			0x02A7E
+ * --------------------------------------------------------------------
+ * 800			200		100			0x08000
+ * 800			250		125			0x06666
+ * 800			332		166			0x04D18
+ * 800			400		200			0x04000
+ * 800			500		250			0x03333
+ * --------------------------------------------------------------------
+ * sys rate configuration table is in descending order of divisor.
+ */
+static struct frac_rate_tbl sys_synth_rtbl[] = {
+	{.div = 0x08000},
+	{.div = 0x06a38},
+	{.div = 0x06666},
+	{.div = 0x054FD},
+	{.div = 0x05000},
+	{.div = 0x04D18},
+	{.div = 0x04000},
+	{.div = 0x0351E},
+	{.div = 0x03333},
+	{.div = 0x03031},
+	{.div = 0x02A7E},
+	{.div = 0x02800},
+	{.div = 0x0268D},
+	{.div = 0x02000},
+};
+
+/* aux rate configuration table, in ascending order of rates */
+static struct aux_rate_tbl aux_rtbl[] = {
+	/* For VCO1div2 = 500 MHz */
+	{.xscale = 10, .yscale = 204, .eq = 0}, /* 12.29 MHz */
+	{.xscale = 4, .yscale = 21, .eq = 0}, /* 48 MHz */
+	{.xscale = 2, .yscale = 6, .eq = 0}, /* 83 MHz */
+	{.xscale = 2, .yscale = 4, .eq = 0}, /* 125 MHz */
+	{.xscale = 1, .yscale = 3, .eq = 1}, /* 166 MHz */
+	{.xscale = 1, .yscale = 2, .eq = 1}, /* 250 MHz */
+};
+
+/* gmac rate configuration table, in ascending order of rates */
+static struct aux_rate_tbl gmac_rtbl[] = {
+	/* For gmac phy input clk */
+	{.xscale = 2, .yscale = 6, .eq = 0}, /* divided by 6 */
+	{.xscale = 2, .yscale = 4, .eq = 0}, /* divided by 4 */
+	{.xscale = 1, .yscale = 3, .eq = 1}, /* divided by 3 */
+	{.xscale = 1, .yscale = 2, .eq = 1}, /* divided by 2 */
+};
+
+/* clcd rate configuration table, in ascending order of rates */
+static struct frac_rate_tbl clcd_rtbl[] = {
+	{.div = 0x14000}, /* 25 Mhz , for vc01div4 = 250 MHz*/
+	{.div = 0x1284B}, /* 27 Mhz , for vc01div4 = 250 MHz*/
+	{.div = 0x0D8D3}, /* 58 Mhz , for vco1div4 = 393 MHz */
+	{.div = 0x0B72C}, /* 58 Mhz , for vco1div4 = 332 MHz */
+	{.div = 0x089EE}, /* 58 Mhz , for vc01div4 = 250 MHz*/
+	{.div = 0x07BA0}, /* 65 Mhz , for vc01div4 = 250 MHz*/
+	{.div = 0x06f1C}, /* 72 Mhz , for vc01div4 = 250 MHz*/
+	{.div = 0x06E58}, /* 58 Mhz , for vco1div4 = 200 MHz */
+	{.div = 0x06c1B}, /* 74 Mhz , for vc01div4 = 250 MHz*/
+	{.div = 0x04A12}, /* 108 Mhz , for vc01div4 = 250 MHz*/
+	{.div = 0x0378E}, /* 144 Mhz , for vc01div4 = 250 MHz*/
+	{.div = 0x0360D}, /* 148 Mhz , for vc01div4 = 250 MHz*/
+	{.div = 0x035E0}, /* 148.5 MHz, for vc01div4 = 250 MHz*/
+};
+
+/* i2s prescaler1 masks */
+static struct aux_clk_masks i2s_prs1_masks = {
+	.eq_sel_mask = AUX_EQ_SEL_MASK,
+	.eq_sel_shift = SPEAR1340_I2S_PRS1_EQ_SEL_SHIFT,
+	.eq1_mask = AUX_EQ1_SEL,
+	.eq2_mask = AUX_EQ2_SEL,
+	.xscale_sel_mask = SPEAR1340_I2S_PRS1_CLK_X_MASK,
+	.xscale_sel_shift = SPEAR1340_I2S_PRS1_CLK_X_SHIFT,
+	.yscale_sel_mask = SPEAR1340_I2S_PRS1_CLK_Y_MASK,
+	.yscale_sel_shift = SPEAR1340_I2S_PRS1_CLK_Y_SHIFT,
+};
+
+/* i2s sclk (bit clock) syynthesizers masks */
+static struct aux_clk_masks i2s_sclk_masks = {
+	.eq_sel_mask = AUX_EQ_SEL_MASK,
+	.eq_sel_shift = SPEAR1340_I2S_SCLK_EQ_SEL_SHIFT,
+	.eq1_mask = AUX_EQ1_SEL,
+	.eq2_mask = AUX_EQ2_SEL,
+	.xscale_sel_mask = SPEAR1340_I2S_SCLK_X_MASK,
+	.xscale_sel_shift = SPEAR1340_I2S_SCLK_X_SHIFT,
+	.yscale_sel_mask = SPEAR1340_I2S_SCLK_Y_MASK,
+	.yscale_sel_shift = SPEAR1340_I2S_SCLK_Y_SHIFT,
+	.enable_bit = SPEAR1340_I2S_SCLK_SYNTH_ENB,
+};
+
+/* i2s prs1 aux rate configuration table, in ascending order of rates */
+static struct aux_rate_tbl i2s_prs1_rtbl[] = {
+	/* For parent clk = 49.152 MHz */
+	{.xscale = 1, .yscale = 12, .eq = 0}, /* 2.048 MHz, smp freq = 8Khz */
+	{.xscale = 11, .yscale = 96, .eq = 0}, /* 2.816 MHz, smp freq = 11Khz */
+	{.xscale = 1, .yscale = 6, .eq = 0}, /* 4.096 MHz, smp freq = 16Khz */
+	{.xscale = 11, .yscale = 48, .eq = 0}, /* 5.632 MHz, smp freq = 22Khz */
+
+	/*
+	 * with parent clk = 49.152, freq gen is 8.192 MHz, smp freq = 32Khz
+	 * with parent clk = 12.288, freq gen is 2.048 MHz, smp freq = 8Khz
+	 */
+	{.xscale = 1, .yscale = 3, .eq = 0},
+
+	/* For parent clk = 49.152 MHz */
+	{.xscale = 17, .yscale = 37, .eq = 0}, /* 11.289 MHz, smp freq = 44Khz*/
+	{.xscale = 1, .yscale = 2, .eq = 0}, /* 12.288 MHz, smp freq = 48Khz*/
+};
+
+/* i2s sclk aux rate configuration table, in ascending order of rates */
+static struct aux_rate_tbl i2s_sclk_rtbl[] = {
+	/* For sclk = ref_clk * x/2/y */
+	{.xscale = 1, .yscale = 4, .eq = 0},
+	{.xscale = 1, .yscale = 2, .eq = 0},
+};
+
+/* adc rate configuration table, in ascending order of rates */
+/* possible adc range is 2.5 MHz to 20 MHz. */
+static struct aux_rate_tbl adc_rtbl[] = {
+	/* For ahb = 166.67 MHz */
+	{.xscale = 1, .yscale = 31, .eq = 0}, /* 2.68 MHz */
+	{.xscale = 2, .yscale = 21, .eq = 0}, /* 7.94 MHz */
+	{.xscale = 4, .yscale = 21, .eq = 0}, /* 15.87 MHz */
+	{.xscale = 10, .yscale = 42, .eq = 0}, /* 19.84 MHz */
+};
+
+/* General synth rate configuration table, in ascending order of rates */
+static struct frac_rate_tbl gen_rtbl[] = {
+	/* For vco1div4 = 250 MHz */
+	{.div = 0x1624E}, /* 22.5792 MHz */
+	{.div = 0x14585}, /* 24.576 MHz */
+	{.div = 0x14000}, /* 25 MHz */
+	{.div = 0x0B127}, /* 45.1584 MHz */
+	{.div = 0x0A000}, /* 50 MHz */
+	{.div = 0x061A8}, /* 81.92 MHz */
+	{.div = 0x05000}, /* 100 MHz */
+	{.div = 0x02800}, /* 200 MHz */
+	{.div = 0x02620}, /* 210 MHz */
+	{.div = 0x02460}, /* 220 MHz */
+	{.div = 0x022C0}, /* 230 MHz */
+	{.div = 0x02160}, /* 240 MHz */
+	{.div = 0x02000}, /* 250 MHz */
+};
+
+/* clock parents */
+static const char *vco_parents[] = { "osc_24m_clk", "osc_25m_clk", };
+static const char *sys_parents[] = { "none", "pll1_clk", "none", "none",
+	"sys_synth_clk", "none", "pll2_clk", "pll3_clk", };
+static const char *ahb_parents[] = { "cpu_div3_clk", "amba_synth_clk", };
+static const char *gpt_parents[] = { "osc_24m_clk", "apb_clk", };
+static const char *uart0_parents[] = { "pll5_clk", "osc_24m_clk",
+	"uart0_synth_gate_clk", };
+static const char *uart1_parents[] = { "pll5_clk", "osc_24m_clk",
+	"uart1_synth_gate_clk", };
+static const char *c3_parents[] = { "pll5_clk", "c3_synth_gate_clk", };
+static const char *gmac_phy_input_parents[] = { "gmii_125m_pad_clk", "pll2_clk",
+	"osc_25m_clk", };
+static const char *gmac_phy_parents[] = { "gmac_phy_input_mux_clk",
+	"gmac_phy_synth_gate_clk", };
+static const char *clcd_synth_parents[] = { "vco1div4_clk", "pll2_clk", };
+static const char *clcd_pixel_parents[] = { "pll5_clk", "clcd_synth_clk", };
+static const char *i2s_src_parents[] = { "vco1div2_clk", "pll2_clk", "pll3_clk",
+	"i2s_src_pad_clk", };
+static const char *i2s_ref_parents[] = { "i2s_src_mux_clk", "i2s_prs1_clk", };
+static const char *spdif_out_parents[] = { "i2s_src_pad_clk", "gen_synth2_clk",
+};
+static const char *spdif_in_parents[] = { "pll2_clk", "gen_synth3_clk", };
+
+static const char *gen_synth0_1_parents[] = { "vco1div4_clk", "vco3div2_clk",
+	"pll3_clk", };
+static const char *gen_synth2_3_parents[] = { "vco1div4_clk", "vco3div2_clk",
+	"pll2_clk", };
+
+void __init spear1340_clk_init(void)
+{
+	struct clk *clk, *clk1;
+
+	clk = clk_register_fixed_rate(NULL, "apb_pclk", NULL, CLK_IS_ROOT, 0);
+	clk_register_clkdev(clk, "apb_pclk", NULL);
+
+	clk = clk_register_fixed_rate(NULL, "osc_32k_clk", NULL, CLK_IS_ROOT,
+			32000);
+	clk_register_clkdev(clk, "osc_32k_clk", NULL);
+
+	clk = clk_register_fixed_rate(NULL, "osc_24m_clk", NULL, CLK_IS_ROOT,
+			24000000);
+	clk_register_clkdev(clk, "osc_24m_clk", NULL);
+
+	clk = clk_register_fixed_rate(NULL, "osc_25m_clk", NULL, CLK_IS_ROOT,
+			25000000);
+	clk_register_clkdev(clk, "osc_25m_clk", NULL);
+
+	clk = clk_register_fixed_rate(NULL, "gmii_125m_pad_clk", NULL,
+			CLK_IS_ROOT, 125000000);
+	clk_register_clkdev(clk, "gmii_125m_pad_clk", NULL);
+
+	clk = clk_register_fixed_rate(NULL, "i2s_src_pad_clk", NULL,
+			CLK_IS_ROOT, 12288000);
+	clk_register_clkdev(clk, "i2s_src_pad_clk", NULL);
+
+	/* clock derived from 32 KHz osc clk */
+	clk = clk_register_gate(NULL, "rtc-spear", "osc_32k_clk", 0,
+			SPEAR1340_PERIP1_CLK_ENB, SPEAR1340_RTC_CLK_ENB, 0,
+			&_lock);
+	clk_register_clkdev(clk, NULL, "fc900000.rtc");
+
+	/* clock derived from 24 or 25 MHz osc clk */
+	/* vco-pll */
+	clk = clk_register_mux(NULL, "vco1_mux_clk", vco_parents,
+			ARRAY_SIZE(vco_parents), 0, SPEAR1340_PLL_CFG,
+			SPEAR1340_PLL1_CLK_SHIFT, SPEAR1340_PLL_CLK_MASK, 0,
+			&_lock);
+	clk_register_clkdev(clk, "vco1_mux_clk", NULL);
+	clk = clk_register_vco_pll("vco1_clk", "pll1_clk", NULL, "vco1_mux_clk",
+			0, SPEAR1340_PLL1_CTR, SPEAR1340_PLL1_FRQ, pll_rtbl,
+			ARRAY_SIZE(pll_rtbl), &_lock, &clk1, NULL);
+	clk_register_clkdev(clk, "vco1_clk", NULL);
+	clk_register_clkdev(clk1, "pll1_clk", NULL);
+
+	clk = clk_register_mux(NULL, "vco2_mux_clk", vco_parents,
+			ARRAY_SIZE(vco_parents), 0, SPEAR1340_PLL_CFG,
+			SPEAR1340_PLL2_CLK_SHIFT, SPEAR1340_PLL_CLK_MASK, 0,
+			&_lock);
+	clk_register_clkdev(clk, "vco2_mux_clk", NULL);
+	clk = clk_register_vco_pll("vco2_clk", "pll2_clk", NULL, "vco2_mux_clk",
+			0, SPEAR1340_PLL2_CTR, SPEAR1340_PLL2_FRQ, pll_rtbl,
+			ARRAY_SIZE(pll_rtbl), &_lock, &clk1, NULL);
+	clk_register_clkdev(clk, "vco2_clk", NULL);
+	clk_register_clkdev(clk1, "pll2_clk", NULL);
+
+	clk = clk_register_mux(NULL, "vco3_mux_clk", vco_parents,
+			ARRAY_SIZE(vco_parents), 0, SPEAR1340_PLL_CFG,
+			SPEAR1340_PLL3_CLK_SHIFT, SPEAR1340_PLL_CLK_MASK, 0,
+			&_lock);
+	clk_register_clkdev(clk, "vco3_mux_clk", NULL);
+	clk = clk_register_vco_pll("vco3_clk", "pll3_clk", NULL, "vco3_mux_clk",
+			0, SPEAR1340_PLL3_CTR, SPEAR1340_PLL3_FRQ, pll_rtbl,
+			ARRAY_SIZE(pll_rtbl), &_lock, &clk1, NULL);
+	clk_register_clkdev(clk, "vco3_clk", NULL);
+	clk_register_clkdev(clk1, "pll3_clk", NULL);
+
+	clk = clk_register_vco_pll("vco4_clk", "pll4_clk", NULL, "osc_24m_clk",
+			0, SPEAR1340_PLL4_CTR, SPEAR1340_PLL4_FRQ, pll4_rtbl,
+			ARRAY_SIZE(pll4_rtbl), &_lock, &clk1, NULL);
+	clk_register_clkdev(clk, "vco4_clk", NULL);
+	clk_register_clkdev(clk1, "pll4_clk", NULL);
+
+	clk = clk_register_fixed_rate(NULL, "pll5_clk", "osc_24m_clk", 0,
+			48000000);
+	clk_register_clkdev(clk, "pll5_clk", NULL);
+
+	clk = clk_register_fixed_rate(NULL, "pll6_clk", "osc_25m_clk", 0,
+			25000000);
+	clk_register_clkdev(clk, "pll6_clk", NULL);
+
+	/* vco div n clocks */
+	clk = clk_register_fixed_factor(NULL, "vco1div2_clk", "vco1_clk", 0, 1,
+			2);
+	clk_register_clkdev(clk, "vco1div2_clk", NULL);
+
+	clk = clk_register_fixed_factor(NULL, "vco1div4_clk", "vco1_clk", 0, 1,
+			4);
+	clk_register_clkdev(clk, "vco1div4_clk", NULL);
+
+	clk = clk_register_fixed_factor(NULL, "vco2div2_clk", "vco2_clk", 0, 1,
+			2);
+	clk_register_clkdev(clk, "vco2div2_clk", NULL);
+
+	clk = clk_register_fixed_factor(NULL, "vco3div2_clk", "vco3_clk", 0, 1,
+			2);
+	clk_register_clkdev(clk, "vco3div2_clk", NULL);
+
+	/* peripherals */
+	clk_register_fixed_factor(NULL, "thermal_clk", "osc_24m_clk", 0, 1,
+			128);
+	clk = clk_register_gate(NULL, "thermal_gate_clk", "thermal_clk", 0,
+			SPEAR1340_PERIP2_CLK_ENB, SPEAR1340_THSENS_CLK_ENB, 0,
+			&_lock);
+	clk_register_clkdev(clk, NULL, "spear_thermal");
+
+	/* clock derived from pll4 clk */
+	clk = clk_register_fixed_factor(NULL, "ddr_clk", "pll4_clk", 0, 1,
+			1);
+	clk_register_clkdev(clk, "ddr_clk", NULL);
+
+	/* clock derived from pll1 clk */
+	clk = clk_register_frac("sys_synth_clk", "vco1div2_clk", 0,
+			SPEAR1340_SYS_CLK_SYNT, sys_synth_rtbl,
+			ARRAY_SIZE(sys_synth_rtbl), &_lock);
+	clk_register_clkdev(clk, "sys_synth_clk", NULL);
+
+	clk = clk_register_frac("amba_synth_clk", "vco1div2_clk", 0,
+			SPEAR1340_AMBA_CLK_SYNT, amba_synth_rtbl,
+			ARRAY_SIZE(amba_synth_rtbl), &_lock);
+	clk_register_clkdev(clk, "amba_synth_clk", NULL);
+
+	clk = clk_register_mux(NULL, "sys_mux_clk", sys_parents,
+			ARRAY_SIZE(sys_parents), 0, SPEAR1340_SYS_CLK_CTRL,
+			SPEAR1340_SCLK_SRC_SEL_SHIFT,
+			SPEAR1340_SCLK_SRC_SEL_MASK, 0, &_lock);
+	clk_register_clkdev(clk, "sys_clk", NULL);
+
+	clk = clk_register_fixed_factor(NULL, "cpu_clk", "sys_mux_clk", 0, 1,
+			2);
+	clk_register_clkdev(clk, "cpu_clk", NULL);
+
+	clk = clk_register_fixed_factor(NULL, "cpu_div3_clk", "cpu_clk", 0, 1,
+			3);
+	clk_register_clkdev(clk, "cpu_div3_clk", NULL);
+
+	clk = clk_register_fixed_factor(NULL, "wdt_clk", "cpu_clk", 0, 1,
+			2);
+	clk_register_clkdev(clk, NULL, "ec800620.wdt");
+
+	clk = clk_register_mux(NULL, "ahb_clk", ahb_parents,
+			ARRAY_SIZE(ahb_parents), 0, SPEAR1340_SYS_CLK_CTRL,
+			SPEAR1340_HCLK_SRC_SEL_SHIFT,
+			SPEAR1340_HCLK_SRC_SEL_MASK, 0, &_lock);
+	clk_register_clkdev(clk, "ahb_clk", NULL);
+
+	clk = clk_register_fixed_factor(NULL, "apb_clk", "ahb_clk", 0, 1,
+			2);
+	clk_register_clkdev(clk, "apb_clk", NULL);
+
+	/* gpt clocks */
+	clk = clk_register_mux(NULL, "gpt0_mux_clk", gpt_parents,
+			ARRAY_SIZE(gpt_parents), 0, SPEAR1340_PERIP_CLK_CFG,
+			SPEAR1340_GPT0_CLK_SHIFT, SPEAR1340_GPT_CLK_MASK, 0,
+			&_lock);
+	clk_register_clkdev(clk, "gpt0_mux_clk", NULL);
+	clk = clk_register_gate(NULL, "gpt0_clk", "gpt0_mux_clk", 0,
+			SPEAR1340_PERIP1_CLK_ENB, SPEAR1340_GPT0_CLK_ENB, 0,
+			&_lock);
+	clk_register_clkdev(clk, NULL, "gpt0");
+
+	clk = clk_register_mux(NULL, "gpt1_mux_clk", gpt_parents,
+			ARRAY_SIZE(gpt_parents), 0, SPEAR1340_PERIP_CLK_CFG,
+			SPEAR1340_GPT1_CLK_SHIFT, SPEAR1340_GPT_CLK_MASK, 0,
+			&_lock);
+	clk_register_clkdev(clk, "gpt1_mux_clk", NULL);
+	clk = clk_register_gate(NULL, "gpt1_clk", "gpt1_mux_clk", 0,
+			SPEAR1340_PERIP1_CLK_ENB, SPEAR1340_GPT1_CLK_ENB, 0,
+			&_lock);
+	clk_register_clkdev(clk, NULL, "gpt1");
+
+	clk = clk_register_mux(NULL, "gpt2_mux_clk", gpt_parents,
+			ARRAY_SIZE(gpt_parents), 0, SPEAR1340_PERIP_CLK_CFG,
+			SPEAR1340_GPT2_CLK_SHIFT, SPEAR1340_GPT_CLK_MASK, 0,
+			&_lock);
+	clk_register_clkdev(clk, "gpt2_mux_clk", NULL);
+	clk = clk_register_gate(NULL, "gpt2_clk", "gpt2_mux_clk", 0,
+			SPEAR1340_PERIP2_CLK_ENB, SPEAR1340_GPT2_CLK_ENB, 0,
+			&_lock);
+	clk_register_clkdev(clk, NULL, "gpt2");
+
+	clk = clk_register_mux(NULL, "gpt3_mux_clk", gpt_parents,
+			ARRAY_SIZE(gpt_parents), 0, SPEAR1340_PERIP_CLK_CFG,
+			SPEAR1340_GPT3_CLK_SHIFT, SPEAR1340_GPT_CLK_MASK, 0,
+			&_lock);
+	clk_register_clkdev(clk, "gpt3_mux_clk", NULL);
+	clk = clk_register_gate(NULL, "gpt3_clk", "gpt3_mux_clk", 0,
+			SPEAR1340_PERIP2_CLK_ENB, SPEAR1340_GPT3_CLK_ENB, 0,
+			&_lock);
+	clk_register_clkdev(clk, NULL, "gpt3");
+
+	/* others */
+	clk = clk_register_aux("uart0_synth_clk", "uart0_synth_gate_clk",
+			"vco1div2_clk", 0, SPEAR1340_UART0_CLK_SYNT, NULL,
+			aux_rtbl, ARRAY_SIZE(aux_rtbl), &_lock, &clk1);
+	clk_register_clkdev(clk, "uart0_synth_clk", NULL);
+	clk_register_clkdev(clk1, "uart0_synth_gate_clk", NULL);
+
+	clk = clk_register_mux(NULL, "uart0_mux_clk", uart0_parents,
+			ARRAY_SIZE(uart0_parents), 0, SPEAR1340_PERIP_CLK_CFG,
+			SPEAR1340_UART0_CLK_SHIFT, SPEAR1340_UART_CLK_MASK, 0,
+			&_lock);
+	clk_register_clkdev(clk, "uart0_mux_clk", NULL);
+
+	clk = clk_register_gate(NULL, "uart0_clk", "uart0_mux_clk", 0,
+			SPEAR1340_PERIP1_CLK_ENB, SPEAR1340_UART0_CLK_ENB, 0,
+			&_lock);
+	clk_register_clkdev(clk, NULL, "e0000000.serial");
+
+	clk = clk_register_aux("uart1_synth_clk", "uart1_synth_gate_clk",
+			"vco1div2_clk", 0, SPEAR1340_UART1_CLK_SYNT, NULL,
+			aux_rtbl, ARRAY_SIZE(aux_rtbl), &_lock, &clk1);
+	clk_register_clkdev(clk, "uart1_synth_clk", NULL);
+	clk_register_clkdev(clk1, "uart1_synth_gate_clk", NULL);
+
+	clk = clk_register_mux(NULL, "uart1_mux_clk", uart1_parents,
+			ARRAY_SIZE(uart1_parents), 0, SPEAR1340_PERIP_CLK_CFG,
+			SPEAR1340_UART1_CLK_SHIFT, SPEAR1340_UART_CLK_MASK, 0,
+			&_lock);
+	clk_register_clkdev(clk, "uart1_mux_clk", NULL);
+
+	clk = clk_register_gate(NULL, "uart1_clk", "uart1_mux_clk", 0,
+			SPEAR1340_PERIP1_CLK_ENB, SPEAR1340_UART1_CLK_ENB, 0,
+			&_lock);
+	clk_register_clkdev(clk, NULL, "b4100000.serial");
+
+	clk = clk_register_aux("sdhci_synth_clk", "sdhci_synth_gate_clk",
+			"vco1div2_clk", 0, SPEAR1340_SDHCI_CLK_SYNT, NULL,
+			aux_rtbl, ARRAY_SIZE(aux_rtbl), &_lock, &clk1);
+	clk_register_clkdev(clk, "sdhci_synth_clk", NULL);
+	clk_register_clkdev(clk1, "sdhci_synth_gate_clk", NULL);
+
+	clk = clk_register_gate(NULL, "sdhci_clk", "sdhci_synth_gate_clk", 0,
+			SPEAR1340_PERIP1_CLK_ENB, SPEAR1340_SDHCI_CLK_ENB, 0,
+			&_lock);
+	clk_register_clkdev(clk, NULL, "b3000000.sdhci");
+
+	clk = clk_register_aux("cfxd_synth_clk", "cfxd_synth_gate_clk",
+			"vco1div2_clk", 0, SPEAR1340_CFXD_CLK_SYNT, NULL,
+			aux_rtbl, ARRAY_SIZE(aux_rtbl), &_lock, &clk1);
+	clk_register_clkdev(clk, "cfxd_synth_clk", NULL);
+	clk_register_clkdev(clk1, "cfxd_synth_gate_clk", NULL);
+
+	clk = clk_register_gate(NULL, "cfxd_clk", "cfxd_synth_gate_clk", 0,
+			SPEAR1340_PERIP1_CLK_ENB, SPEAR1340_CFXD_CLK_ENB, 0,
+			&_lock);
+	clk_register_clkdev(clk, NULL, "b2800000.cf");
+	clk_register_clkdev(clk, NULL, "arasan_xd");
+
+	clk = clk_register_aux("c3_synth_clk", "c3_synth_gate_clk",
+			"vco1div2_clk", 0, SPEAR1340_C3_CLK_SYNT, NULL,
+			aux_rtbl, ARRAY_SIZE(aux_rtbl), &_lock, &clk1);
+	clk_register_clkdev(clk, "c3_synth_clk", NULL);
+	clk_register_clkdev(clk1, "c3_synth_gate_clk", NULL);
+
+	clk = clk_register_mux(NULL, "c3_mux_clk", c3_parents,
+			ARRAY_SIZE(c3_parents), 0, SPEAR1340_PERIP_CLK_CFG,
+			SPEAR1340_C3_CLK_SHIFT, SPEAR1340_C3_CLK_MASK, 0,
+			&_lock);
+	clk_register_clkdev(clk, "c3_mux_clk", NULL);
+
+	clk = clk_register_gate(NULL, "c3_clk", "c3_mux_clk", 0,
+			SPEAR1340_PERIP1_CLK_ENB, SPEAR1340_C3_CLK_ENB, 0,
+			&_lock);
+	clk_register_clkdev(clk, NULL, "c3");
+
+	/* gmac */
+	clk = clk_register_mux(NULL, "gmac_phy_input_mux_clk",
+			gmac_phy_input_parents,
+			ARRAY_SIZE(gmac_phy_input_parents), 0,
+			SPEAR1340_GMAC_CLK_CFG,
+			SPEAR1340_GMAC_PHY_INPUT_CLK_SHIFT,
+			SPEAR1340_GMAC_PHY_INPUT_CLK_MASK, 0, &_lock);
+	clk_register_clkdev(clk, "gmac_phy_input_mux_clk", NULL);
+
+	clk = clk_register_aux("gmac_phy_synth_clk", "gmac_phy_synth_gate_clk",
+			"gmac_phy_input_mux_clk", 0, SPEAR1340_GMAC_CLK_SYNT,
+			NULL, gmac_rtbl, ARRAY_SIZE(gmac_rtbl), &_lock, &clk1);
+	clk_register_clkdev(clk, "gmac_phy_synth_clk", NULL);
+	clk_register_clkdev(clk1, "gmac_phy_synth_gate_clk", NULL);
+
+	clk = clk_register_mux(NULL, "gmac_phy_mux_clk", gmac_phy_parents,
+			ARRAY_SIZE(gmac_phy_parents), 0,
+			SPEAR1340_PERIP_CLK_CFG, SPEAR1340_GMAC_PHY_CLK_SHIFT,
+			SPEAR1340_GMAC_PHY_CLK_MASK, 0, &_lock);
+	clk_register_clkdev(clk, NULL, "stmmacphy.0");
+
+	/* clcd */
+	clk = clk_register_mux(NULL, "clcd_synth_mux_clk", clcd_synth_parents,
+			ARRAY_SIZE(clcd_synth_parents), 0,
+			SPEAR1340_CLCD_CLK_SYNT, SPEAR1340_CLCD_SYNT_CLK_SHIFT,
+			SPEAR1340_CLCD_SYNT_CLK_MASK, 0, &_lock);
+	clk_register_clkdev(clk, "clcd_synth_mux_clk", NULL);
+
+	clk = clk_register_frac("clcd_synth_clk", "clcd_synth_mux_clk", 0,
+			SPEAR1340_CLCD_CLK_SYNT, clcd_rtbl,
+			ARRAY_SIZE(clcd_rtbl), &_lock);
+	clk_register_clkdev(clk, "clcd_synth_clk", NULL);
+
+	clk = clk_register_mux(NULL, "clcd_pixel_mux_clk", clcd_pixel_parents,
+			ARRAY_SIZE(clcd_pixel_parents), 0,
+			SPEAR1340_PERIP_CLK_CFG, SPEAR1340_CLCD_CLK_SHIFT,
+			SPEAR1340_CLCD_CLK_MASK, 0, &_lock);
+	clk_register_clkdev(clk, "clcd_pixel_clk", NULL);
+
+	clk = clk_register_gate(NULL, "clcd_clk", "clcd_pixel_mux_clk", 0,
+			SPEAR1340_PERIP1_CLK_ENB, SPEAR1340_CLCD_CLK_ENB, 0,
+			&_lock);
+	clk_register_clkdev(clk, "clcd_clk", NULL);
+
+	/* i2s */
+	clk = clk_register_mux(NULL, "i2s_src_mux_clk", i2s_src_parents,
+			ARRAY_SIZE(i2s_src_parents), 0, SPEAR1340_I2S_CLK_CFG,
+			SPEAR1340_I2S_SRC_CLK_SHIFT, SPEAR1340_I2S_SRC_CLK_MASK,
+			0, &_lock);
+	clk_register_clkdev(clk, "i2s_src_clk", NULL);
+
+	clk = clk_register_aux("i2s_prs1_clk", NULL, "i2s_src_mux_clk", 0,
+			SPEAR1340_I2S_CLK_CFG, &i2s_prs1_masks, i2s_prs1_rtbl,
+			ARRAY_SIZE(i2s_prs1_rtbl), &_lock, NULL);
+	clk_register_clkdev(clk, "i2s_prs1_clk", NULL);
+
+	clk = clk_register_mux(NULL, "i2s_ref_mux_clk", i2s_ref_parents,
+			ARRAY_SIZE(i2s_ref_parents), 0, SPEAR1340_I2S_CLK_CFG,
+			SPEAR1340_I2S_REF_SHIFT, SPEAR1340_I2S_REF_SEL_MASK, 0,
+			&_lock);
+	clk_register_clkdev(clk, "i2s_ref_clk", NULL);
+
+	clk = clk_register_gate(NULL, "i2s_ref_pad_clk", "i2s_ref_mux_clk", 0,
+			SPEAR1340_PERIP2_CLK_ENB, SPEAR1340_I2S_REF_PAD_CLK_ENB,
+			0, &_lock);
+	clk_register_clkdev(clk, "i2s_ref_pad_clk", NULL);
+
+	clk = clk_register_aux("i2s_sclk_clk", "i2s_sclk_gate_clk",
+			"i2s_ref_mux_clk", 0, SPEAR1340_I2S_CLK_CFG,
+			&i2s_sclk_masks, i2s_sclk_rtbl,
+			ARRAY_SIZE(i2s_sclk_rtbl), &_lock, &clk1);
+	clk_register_clkdev(clk, "i2s_sclk_clk", NULL);
+	clk_register_clkdev(clk1, "i2s_sclk_gate_clk", NULL);
+
+	/* clock derived from ahb clk */
+	clk = clk_register_gate(NULL, "i2c0_clk", "ahb_clk", 0,
+			SPEAR1340_PERIP1_CLK_ENB, SPEAR1340_I2C0_CLK_ENB, 0,
+			&_lock);
+	clk_register_clkdev(clk, NULL, "e0280000.i2c");
+
+	clk = clk_register_gate(NULL, "i2c1_clk", "ahb_clk", 0,
+			SPEAR1340_PERIP1_CLK_ENB, SPEAR1340_I2C1_CLK_ENB, 0,
+			&_lock);
+	clk_register_clkdev(clk, NULL, "b4000000.i2c");
+
+	clk = clk_register_gate(NULL, "dma_clk", "ahb_clk", 0,
+			SPEAR1340_PERIP1_CLK_ENB, SPEAR1340_DMA_CLK_ENB, 0,
+			&_lock);
+	clk_register_clkdev(clk, NULL, "ea800000.dma");
+	clk_register_clkdev(clk, NULL, "eb000000.dma");
+
+	clk = clk_register_gate(NULL, "gmac_clk", "ahb_clk", 0,
+			SPEAR1340_PERIP1_CLK_ENB, SPEAR1340_GMAC_CLK_ENB, 0,
+			&_lock);
+	clk_register_clkdev(clk, NULL, "e2000000.eth");
+
+	clk = clk_register_gate(NULL, "fsmc_clk", "ahb_clk", 0,
+			SPEAR1340_PERIP1_CLK_ENB, SPEAR1340_FSMC_CLK_ENB, 0,
+			&_lock);
+	clk_register_clkdev(clk, NULL, "b0000000.flash");
+
+	clk = clk_register_gate(NULL, "smi_clk", "ahb_clk", 0,
+			SPEAR1340_PERIP1_CLK_ENB, SPEAR1340_SMI_CLK_ENB, 0,
+			&_lock);
+	clk_register_clkdev(clk, NULL, "ea000000.flash");
+
+	clk = clk_register_gate(NULL, "usbh0_clk", "ahb_clk", 0,
+			SPEAR1340_PERIP1_CLK_ENB, SPEAR1340_UHC0_CLK_ENB, 0,
+			&_lock);
+	clk_register_clkdev(clk, "usbh.0_clk", NULL);
+
+	clk = clk_register_gate(NULL, "usbh1_clk", "ahb_clk", 0,
+			SPEAR1340_PERIP1_CLK_ENB, SPEAR1340_UHC1_CLK_ENB, 0,
+			&_lock);
+	clk_register_clkdev(clk, "usbh.1_clk", NULL);
+
+	clk = clk_register_gate(NULL, "uoc_clk", "ahb_clk", 0,
+			SPEAR1340_PERIP1_CLK_ENB, SPEAR1340_UOC_CLK_ENB, 0,
+			&_lock);
+	clk_register_clkdev(clk, NULL, "uoc");
+
+	clk = clk_register_gate(NULL, "pcie_sata_clk", "ahb_clk", 0,
+			SPEAR1340_PERIP1_CLK_ENB, SPEAR1340_PCIE_SATA_CLK_ENB,
+			0, &_lock);
+	clk_register_clkdev(clk, NULL, "dw_pcie");
+	clk_register_clkdev(clk, NULL, "ahci");
+
+	clk = clk_register_gate(NULL, "sysram0_clk", "ahb_clk", 0,
+			SPEAR1340_PERIP1_CLK_ENB, SPEAR1340_SYSRAM0_CLK_ENB, 0,
+			&_lock);
+	clk_register_clkdev(clk, "sysram0_clk", NULL);
+
+	clk = clk_register_gate(NULL, "sysram1_clk", "ahb_clk", 0,
+			SPEAR1340_PERIP1_CLK_ENB, SPEAR1340_SYSRAM1_CLK_ENB, 0,
+			&_lock);
+	clk_register_clkdev(clk, "sysram1_clk", NULL);
+
+	clk = clk_register_aux("adc_synth_clk", "adc_synth_gate_clk", "ahb_clk",
+			0, SPEAR1340_ADC_CLK_SYNT, NULL, adc_rtbl,
+			ARRAY_SIZE(adc_rtbl), &_lock, &clk1);
+	clk_register_clkdev(clk, "adc_synth_clk", NULL);
+	clk_register_clkdev(clk1, "adc_synth_gate_clk", NULL);
+
+	clk = clk_register_gate(NULL, "adc_clk", "adc_synth_gate_clk", 0,
+			SPEAR1340_PERIP1_CLK_ENB, SPEAR1340_ADC_CLK_ENB, 0,
+			&_lock);
+	clk_register_clkdev(clk, NULL, "adc_clk");
+
+	/* clock derived from apb clk */
+	clk = clk_register_gate(NULL, "ssp_clk", "apb_clk", 0,
+			SPEAR1340_PERIP1_CLK_ENB, SPEAR1340_SSP_CLK_ENB, 0,
+			&_lock);
+	clk_register_clkdev(clk, NULL, "e0100000.spi");
+
+	clk = clk_register_gate(NULL, "gpio0_clk", "apb_clk", 0,
+			SPEAR1340_PERIP1_CLK_ENB, SPEAR1340_GPIO0_CLK_ENB, 0,
+			&_lock);
+	clk_register_clkdev(clk, NULL, "e0600000.gpio");
+
+	clk = clk_register_gate(NULL, "gpio1_clk", "apb_clk", 0,
+			SPEAR1340_PERIP1_CLK_ENB, SPEAR1340_GPIO1_CLK_ENB, 0,
+			&_lock);
+	clk_register_clkdev(clk, NULL, "e0680000.gpio");
+
+	clk = clk_register_gate(NULL, "i2s_play_clk", "apb_clk", 0,
+			SPEAR1340_PERIP1_CLK_ENB, SPEAR1340_I2S_PLAY_CLK_ENB, 0,
+			&_lock);
+	clk_register_clkdev(clk, NULL, "b2400000.i2s");
+
+	clk = clk_register_gate(NULL, "i2s_rec_clk", "apb_clk", 0,
+			SPEAR1340_PERIP1_CLK_ENB, SPEAR1340_I2S_REC_CLK_ENB, 0,
+			&_lock);
+	clk_register_clkdev(clk, NULL, "b2000000.i2s");
+
+	clk = clk_register_gate(NULL, "kbd_clk", "apb_clk", 0,
+			SPEAR1340_PERIP2_CLK_ENB, SPEAR1340_KBD_CLK_ENB, 0,
+			&_lock);
+	clk_register_clkdev(clk, NULL, "e0300000.kbd");
+
+	/* RAS clks */
+	clk = clk_register_mux(NULL, "gen_synth0_1_mux_clk",
+			gen_synth0_1_parents, ARRAY_SIZE(gen_synth0_1_parents),
+			0, SPEAR1340_PLL_CFG, SPEAR1340_GEN_SYNT0_1_CLK_SHIFT,
+			SPEAR1340_GEN_SYNT_CLK_MASK, 0, &_lock);
+	clk_register_clkdev(clk, "gen_synth0_1_clk", NULL);
+
+	clk = clk_register_mux(NULL, "gen_synth2_3_mux_clk",
+			gen_synth2_3_parents, ARRAY_SIZE(gen_synth2_3_parents),
+			0, SPEAR1340_PLL_CFG, SPEAR1340_GEN_SYNT2_3_CLK_SHIFT,
+			SPEAR1340_GEN_SYNT_CLK_MASK, 0, &_lock);
+	clk_register_clkdev(clk, "gen_synth2_3_clk", NULL);
+
+	clk = clk_register_frac("gen_synth0_clk", "gen_synth0_1_clk", 0,
+			SPEAR1340_GEN_CLK_SYNT0, gen_rtbl, ARRAY_SIZE(gen_rtbl),
+			&_lock);
+	clk_register_clkdev(clk, "gen_synth0_clk", NULL);
+
+	clk = clk_register_frac("gen_synth1_clk", "gen_synth0_1_clk", 0,
+			SPEAR1340_GEN_CLK_SYNT1, gen_rtbl, ARRAY_SIZE(gen_rtbl),
+			&_lock);
+	clk_register_clkdev(clk, "gen_synth1_clk", NULL);
+
+	clk = clk_register_frac("gen_synth2_clk", "gen_synth2_3_clk", 0,
+			SPEAR1340_GEN_CLK_SYNT2, gen_rtbl, ARRAY_SIZE(gen_rtbl),
+			&_lock);
+	clk_register_clkdev(clk, "gen_synth2_clk", NULL);
+
+	clk = clk_register_frac("gen_synth3_clk", "gen_synth2_3_clk", 0,
+			SPEAR1340_GEN_CLK_SYNT3, gen_rtbl, ARRAY_SIZE(gen_rtbl),
+			&_lock);
+	clk_register_clkdev(clk, "gen_synth3_clk", NULL);
+
+	clk = clk_register_gate(NULL, "mali_clk", "gen_synth3_clk", 0,
+			SPEAR1340_PERIP3_CLK_ENB, SPEAR1340_MALI_CLK_ENB, 0,
+			&_lock);
+	clk_register_clkdev(clk, NULL, "mali");
+
+	clk = clk_register_gate(NULL, "cec0_clk", "ahb_clk", 0,
+			SPEAR1340_PERIP3_CLK_ENB, SPEAR1340_CEC0_CLK_ENB, 0,
+			&_lock);
+	clk_register_clkdev(clk, NULL, "spear_cec.0");
+
+	clk = clk_register_gate(NULL, "cec1_clk", "ahb_clk", 0,
+			SPEAR1340_PERIP3_CLK_ENB, SPEAR1340_CEC1_CLK_ENB, 0,
+			&_lock);
+	clk_register_clkdev(clk, NULL, "spear_cec.1");
+
+	clk = clk_register_mux(NULL, "spdif_out_mux_clk", spdif_out_parents,
+			ARRAY_SIZE(spdif_out_parents), 0,
+			SPEAR1340_PERIP_CLK_CFG, SPEAR1340_SPDIF_OUT_CLK_SHIFT,
+			SPEAR1340_SPDIF_CLK_MASK, 0, &_lock);
+	clk_register_clkdev(clk, "spdif_out_mux_clk", NULL);
+
+	clk = clk_register_gate(NULL, "spdif_out_clk", "spdif_out_mux_clk", 0,
+			SPEAR1340_PERIP3_CLK_ENB, SPEAR1340_SPDIF_OUT_CLK_ENB,
+			0, &_lock);
+	clk_register_clkdev(clk, NULL, "spdif-out");
+
+	clk = clk_register_mux(NULL, "spdif_in_mux_clk", spdif_in_parents,
+			ARRAY_SIZE(spdif_in_parents), 0,
+			SPEAR1340_PERIP_CLK_CFG, SPEAR1340_SPDIF_IN_CLK_SHIFT,
+			SPEAR1340_SPDIF_CLK_MASK, 0, &_lock);
+	clk_register_clkdev(clk, "spdif_in_mux_clk", NULL);
+
+	clk = clk_register_gate(NULL, "spdif_in_clk", "spdif_in_mux_clk", 0,
+			SPEAR1340_PERIP3_CLK_ENB, SPEAR1340_SPDIF_IN_CLK_ENB, 0,
+			&_lock);
+	clk_register_clkdev(clk, NULL, "spdif-in");
+
+	clk = clk_register_gate(NULL, "acp_clk", "acp_mux_clk", 0,
+			SPEAR1340_PERIP2_CLK_ENB, SPEAR1340_ACP_CLK_ENB, 0,
+			&_lock);
+	clk_register_clkdev(clk, NULL, "acp_clk");
+
+	clk = clk_register_gate(NULL, "plgpio_clk", "plgpio_mux_clk", 0,
+			SPEAR1340_PERIP3_CLK_ENB, SPEAR1340_PLGPIO_CLK_ENB, 0,
+			&_lock);
+	clk_register_clkdev(clk, NULL, "plgpio");
+
+	clk = clk_register_gate(NULL, "video_dec_clk", "video_dec_mux_clk", 0,
+			SPEAR1340_PERIP3_CLK_ENB, SPEAR1340_VIDEO_DEC_CLK_ENB,
+			0, &_lock);
+	clk_register_clkdev(clk, NULL, "video_dec");
+
+	clk = clk_register_gate(NULL, "video_enc_clk", "video_enc_mux_clk", 0,
+			SPEAR1340_PERIP3_CLK_ENB, SPEAR1340_VIDEO_ENC_CLK_ENB,
+			0, &_lock);
+	clk_register_clkdev(clk, NULL, "video_enc");
+
+	clk = clk_register_gate(NULL, "video_in_clk", "video_in_mux_clk", 0,
+			SPEAR1340_PERIP3_CLK_ENB, SPEAR1340_VIDEO_IN_CLK_ENB, 0,
+			&_lock);
+	clk_register_clkdev(clk, NULL, "spear_vip");
+
+	clk = clk_register_gate(NULL, "cam0_clk", "cam0_mux_clk", 0,
+			SPEAR1340_PERIP3_CLK_ENB, SPEAR1340_CAM0_CLK_ENB, 0,
+			&_lock);
+	clk_register_clkdev(clk, NULL, "spear_camif.0");
+
+	clk = clk_register_gate(NULL, "cam1_clk", "cam1_mux_clk", 0,
+			SPEAR1340_PERIP3_CLK_ENB, SPEAR1340_CAM1_CLK_ENB, 0,
+			&_lock);
+	clk_register_clkdev(clk, NULL, "spear_camif.1");
+
+	clk = clk_register_gate(NULL, "cam2_clk", "cam2_mux_clk", 0,
+			SPEAR1340_PERIP3_CLK_ENB, SPEAR1340_CAM2_CLK_ENB, 0,
+			&_lock);
+	clk_register_clkdev(clk, NULL, "spear_camif.2");
+
+	clk = clk_register_gate(NULL, "cam3_clk", "cam3_mux_clk", 0,
+			SPEAR1340_PERIP3_CLK_ENB, SPEAR1340_CAM3_CLK_ENB, 0,
+			&_lock);
+	clk_register_clkdev(clk, NULL, "spear_camif.3");
+
+	clk = clk_register_gate(NULL, "pwm_clk", "pwm_mux_clk", 0,
+			SPEAR1340_PERIP3_CLK_ENB, SPEAR1340_PWM_CLK_ENB, 0,
+			&_lock);
+	clk_register_clkdev(clk, NULL, "pwm");
+}
diff --git a/drivers/clk/spear/spear3xx_clock.c b/drivers/clk/spear/spear3xx_clock.c
new file mode 100644
index 00000000000..440bb3e4c97
--- /dev/null
+++ b/drivers/clk/spear/spear3xx_clock.c
@@ -0,0 +1,612 @@
+/*
+ * SPEAr3xx machines clock framework source file
+ *
+ * Copyright (C) 2012 ST Microelectronics
+ * Viresh Kumar <viresh.kumar@st.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/clk.h>
+#include <linux/clkdev.h>
+#include <linux/err.h>
+#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)
+	/* PLL_CLK_CFG register masks */
+	#define MCTR_CLK_SHIFT		28
+	#define MCTR_CLK_MASK		3
+
+#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
+
+	#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)
+	/* PERIP_CLK_CFG register masks */
+	#define UART_CLK_SHIFT		4
+	#define UART_CLK_MASK		1
+	#define FIRDA_CLK_SHIFT		5
+	#define FIRDA_CLK_MASK		2
+	#define GPT0_CLK_SHIFT		8
+	#define GPT1_CLK_SHIFT		11
+	#define GPT2_CLK_SHIFT		12
+	#define GPT_CLK_MASK		1
+
+#define PERIP1_CLK_ENB			(MISC_BASE + 0x02C)
+	/* PERIP1_CLK_ENB register masks */
+	#define UART_CLK_ENB		3
+	#define SSP_CLK_ENB		5
+	#define I2C_CLK_ENB		7
+	#define JPEG_CLK_ENB		8
+	#define FIRDA_CLK_ENB		10
+	#define GPT1_CLK_ENB		11
+	#define GPT2_CLK_ENB		12
+	#define ADC_CLK_ENB		15
+	#define RTC_CLK_ENB		17
+	#define GPIO_CLK_ENB		18
+	#define DMA_CLK_ENB		19
+	#define SMI_CLK_ENB		21
+	#define GMAC_CLK_ENB		23
+	#define USBD_CLK_ENB		24
+	#define USBH_CLK_ENB		25
+	#define C3_CLK_ENB		31
+
+#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
+	#define RAS_32K_CLK_ENB		3
+	#define RAS_24M_CLK_ENB		4
+	#define RAS_48M_CLK_ENB		5
+	#define RAS_PLL2_CLK_ENB	7
+	#define RAS_SYNT0_CLK_ENB	8
+	#define RAS_SYNT1_CLK_ENB	9
+	#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 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)
+
+/* pll rate configuration table, in ascending order of rates */
+static struct pll_rate_tbl pll_rtbl[] = {
+	{.mode = 0, .m = 0x53, .n = 0x0C, .p = 0x1}, /* vco 332 & pll 166 MHz */
+	{.mode = 0, .m = 0x85, .n = 0x0C, .p = 0x1}, /* vco 532 & pll 266 MHz */
+	{.mode = 0, .m = 0xA6, .n = 0x0C, .p = 0x1}, /* vco 664 & pll 332 MHz */
+};
+
+/* aux rate configuration table, in ascending order of rates */
+static struct aux_rate_tbl aux_rtbl[] = {
+	/* For PLL1 = 332 MHz */
+	{.xscale = 2, .yscale = 27, .eq = 0}, /* 12.296 MHz */
+	{.xscale = 2, .yscale = 8, .eq = 0}, /* 41.5 MHz */
+	{.xscale = 2, .yscale = 4, .eq = 0}, /* 83 MHz */
+	{.xscale = 1, .yscale = 2, .eq = 1}, /* 166 MHz */
+};
+
+/* gpt rate configuration table, in ascending order of rates */
+static struct gpt_rate_tbl gpt_rtbl[] = {
+	/* For pll1 = 332 MHz */
+	{.mscale = 4, .nscale = 0}, /* 41.5 MHz */
+	{.mscale = 2, .nscale = 0}, /* 55.3 MHz */
+	{.mscale = 1, .nscale = 0}, /* 83 MHz */
+};
+
+/* clock parents */
+static const char *uart0_parents[] = { "pll3_48m_clk", "uart_synth_gate_clk", };
+static const char *firda_parents[] = { "pll3_48m_clk", "firda_synth_gate_clk",
+};
+static const char *gpt0_parents[] = { "pll3_48m_clk", "gpt0_synth_clk", };
+static const char *gpt1_parents[] = { "pll3_48m_clk", "gpt1_synth_clk", };
+static const char *gpt2_parents[] = { "pll3_48m_clk", "gpt2_synth_clk", };
+static const char *gen2_3_parents[] = { "pll1_clk", "pll2_clk", };
+static const char *ddr_parents[] = { "ahb_clk", "ahbmult2_clk", "none",
+	"pll2_clk", };
+
+#ifdef CONFIG_MACH_SPEAR300
+static void __init spear300_clk_init(void)
+{
+	struct clk *clk;
+
+	clk = clk_register_fixed_factor(NULL, "clcd_clk", "ras_pll3_48m_clk", 0,
+			1, 1);
+	clk_register_clkdev(clk, NULL, "60000000.clcd");
+
+	clk = clk_register_fixed_factor(NULL, "fsmc_clk", "ras_ahb_clk", 0, 1,
+			1);
+	clk_register_clkdev(clk, NULL, "94000000.flash");
+
+	clk = clk_register_fixed_factor(NULL, "sdhci_clk", "ras_ahb_clk", 0, 1,
+			1);
+	clk_register_clkdev(clk, NULL, "70000000.sdhci");
+
+	clk = clk_register_fixed_factor(NULL, "gpio1_clk", "ras_apb_clk", 0, 1,
+			1);
+	clk_register_clkdev(clk, NULL, "a9000000.gpio");
+
+	clk = clk_register_fixed_factor(NULL, "kbd_clk", "ras_apb_clk", 0, 1,
+			1);
+	clk_register_clkdev(clk, NULL, "a0000000.kbd");
+}
+#endif
+
+/* array of all spear 310 clock lookups */
+#ifdef CONFIG_MACH_SPEAR310
+static void __init spear310_clk_init(void)
+{
+	struct clk *clk;
+
+	clk = clk_register_fixed_factor(NULL, "emi_clk", "ras_ahb_clk", 0, 1,
+			1);
+	clk_register_clkdev(clk, "emi", NULL);
+
+	clk = clk_register_fixed_factor(NULL, "fsmc_clk", "ras_ahb_clk", 0, 1,
+			1);
+	clk_register_clkdev(clk, NULL, "44000000.flash");
+
+	clk = clk_register_fixed_factor(NULL, "tdm_clk", "ras_ahb_clk", 0, 1,
+			1);
+	clk_register_clkdev(clk, NULL, "tdm");
+
+	clk = clk_register_fixed_factor(NULL, "uart1_clk", "ras_apb_clk", 0, 1,
+			1);
+	clk_register_clkdev(clk, NULL, "b2000000.serial");
+
+	clk = clk_register_fixed_factor(NULL, "uart2_clk", "ras_apb_clk", 0, 1,
+			1);
+	clk_register_clkdev(clk, NULL, "b2080000.serial");
+
+	clk = clk_register_fixed_factor(NULL, "uart3_clk", "ras_apb_clk", 0, 1,
+			1);
+	clk_register_clkdev(clk, NULL, "b2100000.serial");
+
+	clk = clk_register_fixed_factor(NULL, "uart4_clk", "ras_apb_clk", 0, 1,
+			1);
+	clk_register_clkdev(clk, NULL, "b2180000.serial");
+
+	clk = clk_register_fixed_factor(NULL, "uart5_clk", "ras_apb_clk", 0, 1,
+			1);
+	clk_register_clkdev(clk, NULL, "b2200000.serial");
+}
+#endif
+
+/* array of all spear 320 clock lookups */
+#ifdef CONFIG_MACH_SPEAR320
+	#define SMII_PCLK_SHIFT				18
+	#define SMII_PCLK_MASK				2
+	#define SMII_PCLK_VAL_PAD			0x0
+	#define SMII_PCLK_VAL_PLL2			0x1
+	#define SMII_PCLK_VAL_SYNTH0			0x2
+	#define SDHCI_PCLK_SHIFT			15
+	#define SDHCI_PCLK_MASK				1
+	#define SDHCI_PCLK_VAL_48M			0x0
+	#define SDHCI_PCLK_VAL_SYNTH3			0x1
+	#define I2S_REF_PCLK_SHIFT			8
+	#define I2S_REF_PCLK_MASK			1
+	#define I2S_REF_PCLK_SYNTH_VAL			0x1
+	#define I2S_REF_PCLK_PLL2_VAL			0x0
+	#define UART1_PCLK_SHIFT			6
+	#define UART1_PCLK_MASK				1
+	#define SPEAR320_UARTX_PCLK_VAL_SYNTH1		0x0
+	#define SPEAR320_UARTX_PCLK_VAL_APB		0x1
+
+static const char *i2s_ref_parents[] = { "ras_pll2_clk",
+	"ras_gen2_synth_gate_clk", };
+static const char *sdhci_parents[] = { "ras_pll3_48m_clk",
+	"ras_gen3_synth_gate_clk",
+};
+static const char *smii0_parents[] = { "smii_125m_pad", "ras_pll2_clk",
+	"ras_gen0_synth_gate_clk", };
+static const char *uartx_parents[] = { "ras_gen1_synth_gate_clk", "ras_apb_clk",
+};
+
+static void __init spear320_clk_init(void)
+{
+	struct clk *clk;
+
+	clk = clk_register_fixed_rate(NULL, "smii_125m_pad_clk", NULL,
+			CLK_IS_ROOT, 125000000);
+	clk_register_clkdev(clk, "smii_125m_pad", NULL);
+
+	clk = clk_register_fixed_factor(NULL, "clcd_clk", "ras_pll3_48m_clk", 0,
+			1, 1);
+	clk_register_clkdev(clk, NULL, "90000000.clcd");
+
+	clk = clk_register_fixed_factor(NULL, "emi_clk", "ras_ahb_clk", 0, 1,
+			1);
+	clk_register_clkdev(clk, "emi", NULL);
+
+	clk = clk_register_fixed_factor(NULL, "fsmc_clk", "ras_ahb_clk", 0, 1,
+			1);
+	clk_register_clkdev(clk, NULL, "4c000000.flash");
+
+	clk = clk_register_fixed_factor(NULL, "i2c1_clk", "ras_ahb_clk", 0, 1,
+			1);
+	clk_register_clkdev(clk, NULL, "a7000000.i2c");
+
+	clk = clk_register_fixed_factor(NULL, "pwm_clk", "ras_ahb_clk", 0, 1,
+			1);
+	clk_register_clkdev(clk, "pwm", NULL);
+
+	clk = clk_register_fixed_factor(NULL, "ssp1_clk", "ras_ahb_clk", 0, 1,
+			1);
+	clk_register_clkdev(clk, NULL, "a5000000.spi");
+
+	clk = clk_register_fixed_factor(NULL, "ssp2_clk", "ras_ahb_clk", 0, 1,
+			1);
+	clk_register_clkdev(clk, NULL, "a6000000.spi");
+
+	clk = clk_register_fixed_factor(NULL, "can0_clk", "ras_apb_clk", 0, 1,
+			1);
+	clk_register_clkdev(clk, NULL, "c_can_platform.0");
+
+	clk = clk_register_fixed_factor(NULL, "can1_clk", "ras_apb_clk", 0, 1,
+			1);
+	clk_register_clkdev(clk, NULL, "c_can_platform.1");
+
+	clk = clk_register_fixed_factor(NULL, "i2s_clk", "ras_apb_clk", 0, 1,
+			1);
+	clk_register_clkdev(clk, NULL, "i2s");
+
+	clk = clk_register_mux(NULL, "i2s_ref_clk", i2s_ref_parents,
+			ARRAY_SIZE(i2s_ref_parents), 0, SPEAR320_CONTROL_REG,
+			I2S_REF_PCLK_SHIFT, I2S_REF_PCLK_MASK, 0, &_lock);
+	clk_register_clkdev(clk, "i2s_ref_clk", NULL);
+
+	clk = clk_register_fixed_factor(NULL, "i2s_sclk", "i2s_ref_clk", 0, 1,
+			4);
+	clk_register_clkdev(clk, "i2s_sclk", NULL);
+
+	clk = clk_register_mux(NULL, "rs485_clk", uartx_parents,
+			ARRAY_SIZE(uartx_parents), 0, SPEAR320_EXT_CTRL_REG,
+			SPEAR320_RS485_PCLK_SHIFT, SPEAR320_UARTX_PCLK_MASK, 0,
+			&_lock);
+	clk_register_clkdev(clk, NULL, "a9300000.serial");
+
+	clk = clk_register_mux(NULL, "sdhci_clk", sdhci_parents,
+			ARRAY_SIZE(sdhci_parents), 0, SPEAR320_CONTROL_REG,
+			SDHCI_PCLK_SHIFT, SDHCI_PCLK_MASK, 0, &_lock);
+	clk_register_clkdev(clk, NULL, "70000000.sdhci");
+
+	clk = clk_register_mux(NULL, "smii_pclk", smii0_parents,
+			ARRAY_SIZE(smii0_parents), 0, SPEAR320_CONTROL_REG,
+			SMII_PCLK_SHIFT, SMII_PCLK_MASK, 0, &_lock);
+	clk_register_clkdev(clk, NULL, "smii_pclk");
+
+	clk = clk_register_fixed_factor(NULL, "smii_clk", "smii_pclk", 0, 1, 1);
+	clk_register_clkdev(clk, NULL, "smii");
+
+	clk = clk_register_mux(NULL, "uart1_clk", uartx_parents,
+			ARRAY_SIZE(uartx_parents), 0, SPEAR320_CONTROL_REG,
+			UART1_PCLK_SHIFT, UART1_PCLK_MASK, 0, &_lock);
+	clk_register_clkdev(clk, NULL, "a3000000.serial");
+
+	clk = clk_register_mux(NULL, "uart2_clk", uartx_parents,
+			ARRAY_SIZE(uartx_parents), 0, SPEAR320_EXT_CTRL_REG,
+			SPEAR320_UART2_PCLK_SHIFT, SPEAR320_UARTX_PCLK_MASK, 0,
+			&_lock);
+	clk_register_clkdev(clk, NULL, "a4000000.serial");
+
+	clk = clk_register_mux(NULL, "uart3_clk", uartx_parents,
+			ARRAY_SIZE(uartx_parents), 0, SPEAR320_EXT_CTRL_REG,
+			SPEAR320_UART3_PCLK_SHIFT, SPEAR320_UARTX_PCLK_MASK, 0,
+			&_lock);
+	clk_register_clkdev(clk, NULL, "a9100000.serial");
+
+	clk = clk_register_mux(NULL, "uart4_clk", uartx_parents,
+			ARRAY_SIZE(uartx_parents), 0, SPEAR320_EXT_CTRL_REG,
+			SPEAR320_UART4_PCLK_SHIFT, SPEAR320_UARTX_PCLK_MASK, 0,
+			&_lock);
+	clk_register_clkdev(clk, NULL, "a9200000.serial");
+
+	clk = clk_register_mux(NULL, "uart5_clk", uartx_parents,
+			ARRAY_SIZE(uartx_parents), 0, SPEAR320_EXT_CTRL_REG,
+			SPEAR320_UART5_PCLK_SHIFT, SPEAR320_UARTX_PCLK_MASK, 0,
+			&_lock);
+	clk_register_clkdev(clk, NULL, "60000000.serial");
+
+	clk = clk_register_mux(NULL, "uart6_clk", uartx_parents,
+			ARRAY_SIZE(uartx_parents), 0, SPEAR320_EXT_CTRL_REG,
+			SPEAR320_UART6_PCLK_SHIFT, SPEAR320_UARTX_PCLK_MASK, 0,
+			&_lock);
+	clk_register_clkdev(clk, NULL, "60100000.serial");
+}
+#endif
+
+void __init spear3xx_clk_init(void)
+{
+	struct clk *clk, *clk1;
+
+	clk = clk_register_fixed_rate(NULL, "apb_pclk", NULL, CLK_IS_ROOT, 0);
+	clk_register_clkdev(clk, "apb_pclk", NULL);
+
+	clk = clk_register_fixed_rate(NULL, "osc_32k_clk", NULL, CLK_IS_ROOT,
+			32000);
+	clk_register_clkdev(clk, "osc_32k_clk", NULL);
+
+	clk = clk_register_fixed_rate(NULL, "osc_24m_clk", NULL, CLK_IS_ROOT,
+			24000000);
+	clk_register_clkdev(clk, "osc_24m_clk", NULL);
+
+	/* clock derived from 32 KHz osc clk */
+	clk = clk_register_gate(NULL, "rtc-spear", "osc_32k_clk", 0,
+			PERIP1_CLK_ENB, RTC_CLK_ENB, 0, &_lock);
+	clk_register_clkdev(clk, NULL, "fc900000.rtc");
+
+	/* clock derived from 24 MHz osc clk */
+	clk = clk_register_fixed_rate(NULL, "pll3_48m_clk", "osc_24m_clk", 0,
+			48000000);
+	clk_register_clkdev(clk, "pll3_48m_clk", NULL);
+
+	clk = clk_register_fixed_factor(NULL, "wdt_clk", "osc_24m_clk", 0, 1,
+			1);
+	clk_register_clkdev(clk, NULL, "fc880000.wdt");
+
+	clk = clk_register_vco_pll("vco1_clk", "pll1_clk", NULL,
+			"osc_24m_clk", 0, PLL1_CTR, PLL1_FRQ, pll_rtbl,
+			ARRAY_SIZE(pll_rtbl), &_lock, &clk1, NULL);
+	clk_register_clkdev(clk, "vco1_clk", NULL);
+	clk_register_clkdev(clk1, "pll1_clk", NULL);
+
+	clk = clk_register_vco_pll("vco2_clk", "pll2_clk", NULL,
+			"osc_24m_clk", 0, PLL2_CTR, PLL2_FRQ, pll_rtbl,
+			ARRAY_SIZE(pll_rtbl), &_lock, &clk1, NULL);
+	clk_register_clkdev(clk, "vco2_clk", NULL);
+	clk_register_clkdev(clk1, "pll2_clk", NULL);
+
+	/* clock derived from pll1 clk */
+	clk = clk_register_fixed_factor(NULL, "cpu_clk", "pll1_clk", 0, 1, 1);
+	clk_register_clkdev(clk, "cpu_clk", NULL);
+
+	clk = clk_register_divider(NULL, "ahb_clk", "pll1_clk",
+			CLK_SET_RATE_PARENT, CORE_CLK_CFG, HCLK_RATIO_SHIFT,
+			HCLK_RATIO_MASK, 0, &_lock);
+	clk_register_clkdev(clk, "ahb_clk", NULL);
+
+	clk = clk_register_aux("uart_synth_clk", "uart_synth_gate_clk",
+			"pll1_clk", 0, UART_CLK_SYNT, NULL, aux_rtbl,
+			ARRAY_SIZE(aux_rtbl), &_lock, &clk1);
+	clk_register_clkdev(clk, "uart_synth_clk", NULL);
+	clk_register_clkdev(clk1, "uart_synth_gate_clk", NULL);
+
+	clk = clk_register_mux(NULL, "uart0_mux_clk", uart0_parents,
+			ARRAY_SIZE(uart0_parents), 0, PERIP_CLK_CFG,
+			UART_CLK_SHIFT, UART_CLK_MASK, 0, &_lock);
+	clk_register_clkdev(clk, "uart0_mux_clk", NULL);
+
+	clk = clk_register_gate(NULL, "uart0", "uart0_mux_clk", 0,
+			PERIP1_CLK_ENB, UART_CLK_ENB, 0, &_lock);
+	clk_register_clkdev(clk, NULL, "d0000000.serial");
+
+	clk = clk_register_aux("firda_synth_clk", "firda_synth_gate_clk",
+			"pll1_clk", 0, FIRDA_CLK_SYNT, NULL, aux_rtbl,
+			ARRAY_SIZE(aux_rtbl), &_lock, &clk1);
+	clk_register_clkdev(clk, "firda_synth_clk", NULL);
+	clk_register_clkdev(clk1, "firda_synth_gate_clk", NULL);
+
+	clk = clk_register_mux(NULL, "firda_mux_clk", firda_parents,
+			ARRAY_SIZE(firda_parents), 0, PERIP_CLK_CFG,
+			FIRDA_CLK_SHIFT, FIRDA_CLK_MASK, 0, &_lock);
+	clk_register_clkdev(clk, "firda_mux_clk", NULL);
+
+	clk = clk_register_gate(NULL, "firda_clk", "firda_mux_clk", 0,
+			PERIP1_CLK_ENB, FIRDA_CLK_ENB, 0, &_lock);
+	clk_register_clkdev(clk, NULL, "firda");
+
+	/* gpt clocks */
+	clk_register_gpt("gpt0_synth_clk", "pll1_clk", 0, PRSC0_CLK_CFG,
+			gpt_rtbl, ARRAY_SIZE(gpt_rtbl), &_lock);
+	clk = clk_register_mux(NULL, "gpt0_clk", gpt0_parents,
+			ARRAY_SIZE(gpt0_parents), 0, PERIP_CLK_CFG,
+			GPT0_CLK_SHIFT, GPT_CLK_MASK, 0, &_lock);
+	clk_register_clkdev(clk, NULL, "gpt0");
+
+	clk_register_gpt("gpt1_synth_clk", "pll1_clk", 0, PRSC1_CLK_CFG,
+			gpt_rtbl, ARRAY_SIZE(gpt_rtbl), &_lock);
+	clk = clk_register_mux(NULL, "gpt1_mux_clk", gpt1_parents,
+			ARRAY_SIZE(gpt1_parents), 0, PERIP_CLK_CFG,
+			GPT1_CLK_SHIFT, GPT_CLK_MASK, 0, &_lock);
+	clk_register_clkdev(clk, "gpt1_mux_clk", NULL);
+	clk = clk_register_gate(NULL, "gpt1_clk", "gpt1_mux_clk", 0,
+			PERIP1_CLK_ENB, GPT1_CLK_ENB, 0, &_lock);
+	clk_register_clkdev(clk, NULL, "gpt1");
+
+	clk_register_gpt("gpt2_synth_clk", "pll1_clk", 0, PRSC2_CLK_CFG,
+			gpt_rtbl, ARRAY_SIZE(gpt_rtbl), &_lock);
+	clk = clk_register_mux(NULL, "gpt2_mux_clk", gpt2_parents,
+			ARRAY_SIZE(gpt2_parents), 0, PERIP_CLK_CFG,
+			GPT2_CLK_SHIFT, GPT_CLK_MASK, 0, &_lock);
+	clk_register_clkdev(clk, "gpt2_mux_clk", NULL);
+	clk = clk_register_gate(NULL, "gpt2_clk", "gpt2_mux_clk", 0,
+			PERIP1_CLK_ENB, GPT2_CLK_ENB, 0, &_lock);
+	clk_register_clkdev(clk, NULL, "gpt2");
+
+	/* general synths clocks */
+	clk = clk_register_aux("gen0_synth_clk", "gen0_synth_gate_clk",
+			"pll1_clk", 0, GEN0_CLK_SYNT, NULL, aux_rtbl,
+			ARRAY_SIZE(aux_rtbl), &_lock, &clk1);
+	clk_register_clkdev(clk, "gen0_synth_clk", NULL);
+	clk_register_clkdev(clk1, "gen0_synth_gate_clk", NULL);
+
+	clk = clk_register_aux("gen1_synth_clk", "gen1_synth_gate_clk",
+			"pll1_clk", 0, GEN1_CLK_SYNT, NULL, aux_rtbl,
+			ARRAY_SIZE(aux_rtbl), &_lock, &clk1);
+	clk_register_clkdev(clk, "gen1_synth_clk", NULL);
+	clk_register_clkdev(clk1, "gen1_synth_gate_clk", NULL);
+
+	clk = clk_register_mux(NULL, "gen2_3_parent_clk", gen2_3_parents,
+			ARRAY_SIZE(gen2_3_parents), 0, CORE_CLK_CFG,
+			GEN_SYNTH2_3_CLK_SHIFT, GEN_SYNTH2_3_CLK_MASK, 0,
+			&_lock);
+	clk_register_clkdev(clk, "gen2_3_parent_clk", NULL);
+
+	clk = clk_register_aux("gen2_synth_clk", "gen2_synth_gate_clk",
+			"gen2_3_parent_clk", 0, GEN2_CLK_SYNT, NULL, aux_rtbl,
+			ARRAY_SIZE(aux_rtbl), &_lock, &clk1);
+	clk_register_clkdev(clk, "gen2_synth_clk", NULL);
+	clk_register_clkdev(clk1, "gen2_synth_gate_clk", NULL);
+
+	clk = clk_register_aux("gen3_synth_clk", "gen3_synth_gate_clk",
+			"gen2_3_parent_clk", 0, GEN3_CLK_SYNT, NULL, aux_rtbl,
+			ARRAY_SIZE(aux_rtbl), &_lock, &clk1);
+	clk_register_clkdev(clk, "gen3_synth_clk", NULL);
+	clk_register_clkdev(clk1, "gen3_synth_gate_clk", NULL);
+
+	/* clock derived from pll3 clk */
+	clk = clk_register_gate(NULL, "usbh_clk", "pll3_48m_clk", 0,
+			PERIP1_CLK_ENB, USBH_CLK_ENB, 0, &_lock);
+	clk_register_clkdev(clk, "usbh_clk", NULL);
+
+	clk = clk_register_fixed_factor(NULL, "usbh.0_clk", "usbh_clk", 0, 1,
+			1);
+	clk_register_clkdev(clk, "usbh.0_clk", NULL);
+
+	clk = clk_register_fixed_factor(NULL, "usbh.1_clk", "usbh_clk", 0, 1,
+			1);
+	clk_register_clkdev(clk, "usbh.1_clk", NULL);
+
+	clk = clk_register_gate(NULL, "usbd_clk", "pll3_48m_clk", 0,
+			PERIP1_CLK_ENB, USBD_CLK_ENB, 0, &_lock);
+	clk_register_clkdev(clk, NULL, "designware_udc");
+
+	/* clock derived from ahb clk */
+	clk = clk_register_fixed_factor(NULL, "ahbmult2_clk", "ahb_clk", 0, 2,
+			1);
+	clk_register_clkdev(clk, "ahbmult2_clk", NULL);
+
+	clk = clk_register_mux(NULL, "ddr_clk", ddr_parents,
+			ARRAY_SIZE(ddr_parents), 0, PLL_CLK_CFG, MCTR_CLK_SHIFT,
+			MCTR_CLK_MASK, 0, &_lock);
+	clk_register_clkdev(clk, "ddr_clk", NULL);
+
+	clk = clk_register_divider(NULL, "apb_clk", "ahb_clk",
+			CLK_SET_RATE_PARENT, CORE_CLK_CFG, PCLK_RATIO_SHIFT,
+			PCLK_RATIO_MASK, 0, &_lock);
+	clk_register_clkdev(clk, "apb_clk", NULL);
+
+	clk = clk_register_gate(NULL, "amem_clk", "ahb_clk", 0, AMEM_CLK_CFG,
+			AMEM_CLK_ENB, 0, &_lock);
+	clk_register_clkdev(clk, "amem_clk", NULL);
+
+	clk = clk_register_gate(NULL, "c3_clk", "ahb_clk", 0, PERIP1_CLK_ENB,
+			C3_CLK_ENB, 0, &_lock);
+	clk_register_clkdev(clk, NULL, "c3_clk");
+
+	clk = clk_register_gate(NULL, "dma_clk", "ahb_clk", 0, PERIP1_CLK_ENB,
+			DMA_CLK_ENB, 0, &_lock);
+	clk_register_clkdev(clk, NULL, "fc400000.dma");
+
+	clk = clk_register_gate(NULL, "gmac_clk", "ahb_clk", 0, PERIP1_CLK_ENB,
+			GMAC_CLK_ENB, 0, &_lock);
+	clk_register_clkdev(clk, NULL, "e0800000.eth");
+
+	clk = clk_register_gate(NULL, "i2c0_clk", "ahb_clk", 0, PERIP1_CLK_ENB,
+			I2C_CLK_ENB, 0, &_lock);
+	clk_register_clkdev(clk, NULL, "d0180000.i2c");
+
+	clk = clk_register_gate(NULL, "jpeg_clk", "ahb_clk", 0, PERIP1_CLK_ENB,
+			JPEG_CLK_ENB, 0, &_lock);
+	clk_register_clkdev(clk, NULL, "jpeg");
+
+	clk = clk_register_gate(NULL, "smi_clk", "ahb_clk", 0, PERIP1_CLK_ENB,
+			SMI_CLK_ENB, 0, &_lock);
+	clk_register_clkdev(clk, NULL, "fc000000.flash");
+
+	/* clock derived from apb clk */
+	clk = clk_register_gate(NULL, "adc_clk", "apb_clk", 0, PERIP1_CLK_ENB,
+			ADC_CLK_ENB, 0, &_lock);
+	clk_register_clkdev(clk, NULL, "adc");
+
+	clk = clk_register_gate(NULL, "gpio0_clk", "apb_clk", 0, PERIP1_CLK_ENB,
+			GPIO_CLK_ENB, 0, &_lock);
+	clk_register_clkdev(clk, NULL, "fc980000.gpio");
+
+	clk = clk_register_gate(NULL, "ssp0_clk", "apb_clk", 0, PERIP1_CLK_ENB,
+			SSP_CLK_ENB, 0, &_lock);
+	clk_register_clkdev(clk, NULL, "d0100000.spi");
+
+	/* RAS clk enable */
+	clk = clk_register_gate(NULL, "ras_ahb_clk", "ahb_clk", 0, RAS_CLK_ENB,
+			RAS_AHB_CLK_ENB, 0, &_lock);
+	clk_register_clkdev(clk, "ras_ahb_clk", NULL);
+
+	clk = clk_register_gate(NULL, "ras_apb_clk", "apb_clk", 0, RAS_CLK_ENB,
+			RAS_APB_CLK_ENB, 0, &_lock);
+	clk_register_clkdev(clk, "ras_apb_clk", NULL);
+
+	clk = clk_register_gate(NULL, "ras_32k_clk", "osc_32k_clk", 0,
+			RAS_CLK_ENB, RAS_32K_CLK_ENB, 0, &_lock);
+	clk_register_clkdev(clk, "ras_32k_clk", NULL);
+
+	clk = clk_register_gate(NULL, "ras_24m_clk", "osc_24m_clk", 0,
+			RAS_CLK_ENB, RAS_24M_CLK_ENB, 0, &_lock);
+	clk_register_clkdev(clk, "ras_24m_clk", NULL);
+
+	clk = clk_register_gate(NULL, "ras_pll1_clk", "pll1_clk", 0,
+			RAS_CLK_ENB, RAS_PLL1_CLK_ENB, 0, &_lock);
+	clk_register_clkdev(clk, "ras_pll1_clk", NULL);
+
+	clk = clk_register_gate(NULL, "ras_pll2_clk", "pll2_clk", 0,
+			RAS_CLK_ENB, RAS_PLL2_CLK_ENB, 0, &_lock);
+	clk_register_clkdev(clk, "ras_pll2_clk", NULL);
+
+	clk = clk_register_gate(NULL, "ras_pll3_48m_clk", "pll3_48m_clk", 0,
+			RAS_CLK_ENB, RAS_48M_CLK_ENB, 0, &_lock);
+	clk_register_clkdev(clk, "ras_pll3_48m_clk", NULL);
+
+	clk = clk_register_gate(NULL, "ras_gen0_synth_gate_clk",
+			"gen0_synth_gate_clk", 0, RAS_CLK_ENB,
+			RAS_SYNT0_CLK_ENB, 0, &_lock);
+	clk_register_clkdev(clk, "ras_gen0_synth_gate_clk", NULL);
+
+	clk = clk_register_gate(NULL, "ras_gen1_synth_gate_clk",
+			"gen1_synth_gate_clk", 0, RAS_CLK_ENB,
+			RAS_SYNT1_CLK_ENB, 0, &_lock);
+	clk_register_clkdev(clk, "ras_gen1_synth_gate_clk", NULL);
+
+	clk = clk_register_gate(NULL, "ras_gen2_synth_gate_clk",
+			"gen2_synth_gate_clk", 0, RAS_CLK_ENB,
+			RAS_SYNT2_CLK_ENB, 0, &_lock);
+	clk_register_clkdev(clk, "ras_gen2_synth_gate_clk", NULL);
+
+	clk = clk_register_gate(NULL, "ras_gen3_synth_gate_clk",
+			"gen3_synth_gate_clk", 0, RAS_CLK_ENB,
+			RAS_SYNT3_CLK_ENB, 0, &_lock);
+	clk_register_clkdev(clk, "ras_gen3_synth_gate_clk", NULL);
+
+	if (of_machine_is_compatible("st,spear300"))
+		spear300_clk_init();
+	else if (of_machine_is_compatible("st,spear310"))
+		spear310_clk_init();
+	else if (of_machine_is_compatible("st,spear320"))
+		spear320_clk_init();
+}
diff --git a/drivers/clk/spear/spear6xx_clock.c b/drivers/clk/spear/spear6xx_clock.c
new file mode 100644
index 00000000000..f9a20b38230
--- /dev/null
+++ b/drivers/clk/spear/spear6xx_clock.c
@@ -0,0 +1,342 @@
+/*
+ * SPEAr6xx machines clock framework source file
+ *
+ * Copyright (C) 2012 ST Microelectronics
+ * Viresh Kumar <viresh.kumar@st.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/clk.h>
+#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)
+	/* PLL_CLK_CFG register masks */
+	#define MCTR_CLK_SHIFT		28
+	#define MCTR_CLK_MASK		3
+
+#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)
+	/* PERIP_CLK_CFG register masks */
+	#define CLCD_CLK_SHIFT		2
+	#define CLCD_CLK_MASK		2
+	#define UART_CLK_SHIFT		4
+	#define UART_CLK_MASK		1
+	#define FIRDA_CLK_SHIFT		5
+	#define FIRDA_CLK_MASK		2
+	#define GPT0_CLK_SHIFT		8
+	#define GPT1_CLK_SHIFT		10
+	#define GPT2_CLK_SHIFT		11
+	#define GPT3_CLK_SHIFT		12
+	#define GPT_CLK_MASK		1
+
+#define PERIP1_CLK_ENB			(MISC_BASE + 0x02C)
+	/* PERIP1_CLK_ENB register masks */
+	#define UART0_CLK_ENB		3
+	#define UART1_CLK_ENB		4
+	#define SSP0_CLK_ENB		5
+	#define SSP1_CLK_ENB		6
+	#define I2C_CLK_ENB		7
+	#define JPEG_CLK_ENB		8
+	#define FSMC_CLK_ENB		9
+	#define FIRDA_CLK_ENB		10
+	#define GPT2_CLK_ENB		11
+	#define GPT3_CLK_ENB		12
+	#define GPIO2_CLK_ENB		13
+	#define SSP2_CLK_ENB		14
+	#define ADC_CLK_ENB		15
+	#define GPT1_CLK_ENB		11
+	#define RTC_CLK_ENB		17
+	#define GPIO1_CLK_ENB		18
+	#define DMA_CLK_ENB		19
+	#define SMI_CLK_ENB		21
+	#define CLCD_CLK_ENB		22
+	#define GMAC_CLK_ENB		23
+	#define USBD_CLK_ENB		24
+	#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 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[] = {
+	{.mode = 0, .m = 0x53, .n = 0x0F, .p = 0x1}, /* vco 332 & pll 166 MHz */
+	{.mode = 0, .m = 0x85, .n = 0x0F, .p = 0x1}, /* vco 532 & pll 266 MHz */
+	{.mode = 0, .m = 0xA6, .n = 0x0F, .p = 0x1}, /* vco 664 & pll 332 MHz */
+};
+
+/* aux rate configuration table, in ascending order of rates */
+static struct aux_rate_tbl aux_rtbl[] = {
+	/* For PLL1 = 332 MHz */
+	{.xscale = 2, .yscale = 8, .eq = 0}, /* 41.5 MHz */
+	{.xscale = 2, .yscale = 4, .eq = 0}, /* 83 MHz */
+	{.xscale = 1, .yscale = 2, .eq = 1}, /* 166 MHz */
+};
+
+static const char *clcd_parents[] = { "pll3_48m_clk", "clcd_synth_gate_clk", };
+static const char *firda_parents[] = { "pll3_48m_clk", "firda_synth_gate_clk",
+};
+static const char *uart_parents[] = { "pll3_48m_clk", "uart_synth_gate_clk", };
+static const char *gpt0_1_parents[] = { "pll3_48m_clk", "gpt0_1_synth_clk", };
+static const char *gpt2_parents[] = { "pll3_48m_clk", "gpt2_synth_clk", };
+static const char *gpt3_parents[] = { "pll3_48m_clk", "gpt3_synth_clk", };
+static const char *ddr_parents[] = { "ahb_clk", "ahbmult2_clk", "none",
+	"pll2_clk", };
+
+/* gpt rate configuration table, in ascending order of rates */
+static struct gpt_rate_tbl gpt_rtbl[] = {
+	/* For pll1 = 332 MHz */
+	{.mscale = 4, .nscale = 0}, /* 41.5 MHz */
+	{.mscale = 2, .nscale = 0}, /* 55.3 MHz */
+	{.mscale = 1, .nscale = 0}, /* 83 MHz */
+};
+
+void __init spear6xx_clk_init(void)
+{
+	struct clk *clk, *clk1;
+
+	clk = clk_register_fixed_rate(NULL, "apb_pclk", NULL, CLK_IS_ROOT, 0);
+	clk_register_clkdev(clk, "apb_pclk", NULL);
+
+	clk = clk_register_fixed_rate(NULL, "osc_32k_clk", NULL, CLK_IS_ROOT,
+			32000);
+	clk_register_clkdev(clk, "osc_32k_clk", NULL);
+
+	clk = clk_register_fixed_rate(NULL, "osc_30m_clk", NULL, CLK_IS_ROOT,
+			30000000);
+	clk_register_clkdev(clk, "osc_30m_clk", NULL);
+
+	/* clock derived from 32 KHz osc clk */
+	clk = clk_register_gate(NULL, "rtc_spear", "osc_32k_clk", 0,
+			PERIP1_CLK_ENB, RTC_CLK_ENB, 0, &_lock);
+	clk_register_clkdev(clk, NULL, "rtc-spear");
+
+	/* clock derived from 30 MHz osc clk */
+	clk = clk_register_fixed_rate(NULL, "pll3_48m_clk", "osc_24m_clk", 0,
+			48000000);
+	clk_register_clkdev(clk, "pll3_48m_clk", NULL);
+
+	clk = clk_register_vco_pll("vco1_clk", "pll1_clk", NULL, "osc_30m_clk",
+			0, PLL1_CTR, PLL1_FRQ, pll_rtbl, ARRAY_SIZE(pll_rtbl),
+			&_lock, &clk1, NULL);
+	clk_register_clkdev(clk, "vco1_clk", NULL);
+	clk_register_clkdev(clk1, "pll1_clk", NULL);
+
+	clk = clk_register_vco_pll("vco2_clk", "pll2_clk", NULL,
+			"osc_30m_clk", 0, PLL2_CTR, PLL2_FRQ, pll_rtbl,
+			ARRAY_SIZE(pll_rtbl), &_lock, &clk1, NULL);
+	clk_register_clkdev(clk, "vco2_clk", NULL);
+	clk_register_clkdev(clk1, "pll2_clk", NULL);
+
+	clk = clk_register_fixed_factor(NULL, "wdt_clk", "osc_30m_clk", 0, 1,
+			1);
+	clk_register_clkdev(clk, NULL, "wdt");
+
+	/* clock derived from pll1 clk */
+	clk = clk_register_fixed_factor(NULL, "cpu_clk", "pll1_clk", 0, 1, 1);
+	clk_register_clkdev(clk, "cpu_clk", NULL);
+
+	clk = clk_register_divider(NULL, "ahb_clk", "pll1_clk",
+			CLK_SET_RATE_PARENT, CORE_CLK_CFG, HCLK_RATIO_SHIFT,
+			HCLK_RATIO_MASK, 0, &_lock);
+	clk_register_clkdev(clk, "ahb_clk", NULL);
+
+	clk = clk_register_aux("uart_synth_clk", "uart_synth_gate_clk",
+			"pll1_clk", 0, UART_CLK_SYNT, NULL, aux_rtbl,
+			ARRAY_SIZE(aux_rtbl), &_lock, &clk1);
+	clk_register_clkdev(clk, "uart_synth_clk", NULL);
+	clk_register_clkdev(clk1, "uart_synth_gate_clk", NULL);
+
+	clk = clk_register_mux(NULL, "uart_mux_clk", uart_parents,
+			ARRAY_SIZE(uart_parents), 0, PERIP_CLK_CFG,
+			UART_CLK_SHIFT, UART_CLK_MASK, 0, &_lock);
+	clk_register_clkdev(clk, "uart_mux_clk", NULL);
+
+	clk = clk_register_gate(NULL, "uart0", "uart_mux_clk", 0,
+			PERIP1_CLK_ENB, UART0_CLK_ENB, 0, &_lock);
+	clk_register_clkdev(clk, NULL, "d0000000.serial");
+
+	clk = clk_register_gate(NULL, "uart1", "uart_mux_clk", 0,
+			PERIP1_CLK_ENB, UART1_CLK_ENB, 0, &_lock);
+	clk_register_clkdev(clk, NULL, "d0080000.serial");
+
+	clk = clk_register_aux("firda_synth_clk", "firda_synth_gate_clk",
+			"pll1_clk", 0, FIRDA_CLK_SYNT, NULL, aux_rtbl,
+			ARRAY_SIZE(aux_rtbl), &_lock, &clk1);
+	clk_register_clkdev(clk, "firda_synth_clk", NULL);
+	clk_register_clkdev(clk1, "firda_synth_gate_clk", NULL);
+
+	clk = clk_register_mux(NULL, "firda_mux_clk", firda_parents,
+			ARRAY_SIZE(firda_parents), 0, PERIP_CLK_CFG,
+			FIRDA_CLK_SHIFT, FIRDA_CLK_MASK, 0, &_lock);
+	clk_register_clkdev(clk, "firda_mux_clk", NULL);
+
+	clk = clk_register_gate(NULL, "firda_clk", "firda_mux_clk", 0,
+			PERIP1_CLK_ENB, FIRDA_CLK_ENB, 0, &_lock);
+	clk_register_clkdev(clk, NULL, "firda");
+
+	clk = clk_register_aux("clcd_synth_clk", "clcd_synth_gate_clk",
+			"pll1_clk", 0, CLCD_CLK_SYNT, NULL, aux_rtbl,
+			ARRAY_SIZE(aux_rtbl), &_lock, &clk1);
+	clk_register_clkdev(clk, "clcd_synth_clk", NULL);
+	clk_register_clkdev(clk1, "clcd_synth_gate_clk", NULL);
+
+	clk = clk_register_mux(NULL, "clcd_mux_clk", clcd_parents,
+			ARRAY_SIZE(clcd_parents), 0, PERIP_CLK_CFG,
+			CLCD_CLK_SHIFT, CLCD_CLK_MASK, 0, &_lock);
+	clk_register_clkdev(clk, "clcd_mux_clk", NULL);
+
+	clk = clk_register_gate(NULL, "clcd_clk", "clcd_mux_clk", 0,
+			PERIP1_CLK_ENB, CLCD_CLK_ENB, 0, &_lock);
+	clk_register_clkdev(clk, NULL, "clcd");
+
+	/* gpt clocks */
+	clk = clk_register_gpt("gpt0_1_synth_clk", "pll1_clk", 0, PRSC0_CLK_CFG,
+			gpt_rtbl, ARRAY_SIZE(gpt_rtbl), &_lock);
+	clk_register_clkdev(clk, "gpt0_1_synth_clk", NULL);
+
+	clk = clk_register_mux(NULL, "gpt0_mux_clk", gpt0_1_parents,
+			ARRAY_SIZE(gpt0_1_parents), 0, PERIP_CLK_CFG,
+			GPT0_CLK_SHIFT, GPT_CLK_MASK, 0, &_lock);
+	clk_register_clkdev(clk, NULL, "gpt0");
+
+	clk = clk_register_mux(NULL, "gpt1_mux_clk", gpt0_1_parents,
+			ARRAY_SIZE(gpt0_1_parents), 0, PERIP_CLK_CFG,
+			GPT1_CLK_SHIFT, GPT_CLK_MASK, 0, &_lock);
+	clk_register_clkdev(clk, "gpt1_mux_clk", NULL);
+
+	clk = clk_register_gate(NULL, "gpt1_clk", "gpt1_mux_clk", 0,
+			PERIP1_CLK_ENB, GPT1_CLK_ENB, 0, &_lock);
+	clk_register_clkdev(clk, NULL, "gpt1");
+
+	clk = clk_register_gpt("gpt2_synth_clk", "pll1_clk", 0, PRSC1_CLK_CFG,
+			gpt_rtbl, ARRAY_SIZE(gpt_rtbl), &_lock);
+	clk_register_clkdev(clk, "gpt2_synth_clk", NULL);
+
+	clk = clk_register_mux(NULL, "gpt2_mux_clk", gpt2_parents,
+			ARRAY_SIZE(gpt2_parents), 0, PERIP_CLK_CFG,
+			GPT2_CLK_SHIFT, GPT_CLK_MASK, 0, &_lock);
+	clk_register_clkdev(clk, "gpt2_mux_clk", NULL);
+
+	clk = clk_register_gate(NULL, "gpt2_clk", "gpt2_mux_clk", 0,
+			PERIP1_CLK_ENB, GPT2_CLK_ENB, 0, &_lock);
+	clk_register_clkdev(clk, NULL, "gpt2");
+
+	clk = clk_register_gpt("gpt3_synth_clk", "pll1_clk", 0, PRSC2_CLK_CFG,
+			gpt_rtbl, ARRAY_SIZE(gpt_rtbl), &_lock);
+	clk_register_clkdev(clk, "gpt3_synth_clk", NULL);
+
+	clk = clk_register_mux(NULL, "gpt3_mux_clk", gpt3_parents,
+			ARRAY_SIZE(gpt3_parents), 0, PERIP_CLK_CFG,
+			GPT3_CLK_SHIFT, GPT_CLK_MASK, 0, &_lock);
+	clk_register_clkdev(clk, "gpt3_mux_clk", NULL);
+
+	clk = clk_register_gate(NULL, "gpt3_clk", "gpt3_mux_clk", 0,
+			PERIP1_CLK_ENB, GPT3_CLK_ENB, 0, &_lock);
+	clk_register_clkdev(clk, NULL, "gpt3");
+
+	/* clock derived from pll3 clk */
+	clk = clk_register_gate(NULL, "usbh0_clk", "pll3_48m_clk", 0,
+			PERIP1_CLK_ENB, USBH0_CLK_ENB, 0, &_lock);
+	clk_register_clkdev(clk, NULL, "usbh.0_clk");
+
+	clk = clk_register_gate(NULL, "usbh1_clk", "pll3_48m_clk", 0,
+			PERIP1_CLK_ENB, USBH1_CLK_ENB, 0, &_lock);
+	clk_register_clkdev(clk, NULL, "usbh.1_clk");
+
+	clk = clk_register_gate(NULL, "usbd_clk", "pll3_48m_clk", 0,
+			PERIP1_CLK_ENB, USBD_CLK_ENB, 0, &_lock);
+	clk_register_clkdev(clk, NULL, "designware_udc");
+
+	/* clock derived from ahb clk */
+	clk = clk_register_fixed_factor(NULL, "ahbmult2_clk", "ahb_clk", 0, 2,
+			1);
+	clk_register_clkdev(clk, "ahbmult2_clk", NULL);
+
+	clk = clk_register_mux(NULL, "ddr_clk", ddr_parents,
+			ARRAY_SIZE(ddr_parents),
+			0, PLL_CLK_CFG, MCTR_CLK_SHIFT, MCTR_CLK_MASK, 0,
+			&_lock);
+	clk_register_clkdev(clk, "ddr_clk", NULL);
+
+	clk = clk_register_divider(NULL, "apb_clk", "ahb_clk",
+			CLK_SET_RATE_PARENT, CORE_CLK_CFG, PCLK_RATIO_SHIFT,
+			PCLK_RATIO_MASK, 0, &_lock);
+	clk_register_clkdev(clk, "apb_clk", NULL);
+
+	clk = clk_register_gate(NULL, "dma_clk", "ahb_clk", 0, PERIP1_CLK_ENB,
+			DMA_CLK_ENB, 0, &_lock);
+	clk_register_clkdev(clk, NULL, "fc400000.dma");
+
+	clk = clk_register_gate(NULL, "fsmc_clk", "ahb_clk", 0, PERIP1_CLK_ENB,
+			FSMC_CLK_ENB, 0, &_lock);
+	clk_register_clkdev(clk, NULL, "d1800000.flash");
+
+	clk = clk_register_gate(NULL, "gmac_clk", "ahb_clk", 0, PERIP1_CLK_ENB,
+			GMAC_CLK_ENB, 0, &_lock);
+	clk_register_clkdev(clk, NULL, "gmac");
+
+	clk = clk_register_gate(NULL, "i2c_clk", "ahb_clk", 0, PERIP1_CLK_ENB,
+			I2C_CLK_ENB, 0, &_lock);
+	clk_register_clkdev(clk, NULL, "d0200000.i2c");
+
+	clk = clk_register_gate(NULL, "jpeg_clk", "ahb_clk", 0, PERIP1_CLK_ENB,
+			JPEG_CLK_ENB, 0, &_lock);
+	clk_register_clkdev(clk, NULL, "jpeg");
+
+	clk = clk_register_gate(NULL, "smi_clk", "ahb_clk", 0, PERIP1_CLK_ENB,
+			SMI_CLK_ENB, 0, &_lock);
+	clk_register_clkdev(clk, NULL, "fc000000.flash");
+
+	/* clock derived from apb clk */
+	clk = clk_register_gate(NULL, "adc_clk", "apb_clk", 0, PERIP1_CLK_ENB,
+			ADC_CLK_ENB, 0, &_lock);
+	clk_register_clkdev(clk, NULL, "adc");
+
+	clk = clk_register_fixed_factor(NULL, "gpio0_clk", "apb_clk", 0, 1, 1);
+	clk_register_clkdev(clk, NULL, "f0100000.gpio");
+
+	clk = clk_register_gate(NULL, "gpio1_clk", "apb_clk", 0, PERIP1_CLK_ENB,
+			GPIO1_CLK_ENB, 0, &_lock);
+	clk_register_clkdev(clk, NULL, "fc980000.gpio");
+
+	clk = clk_register_gate(NULL, "gpio2_clk", "apb_clk", 0, PERIP1_CLK_ENB,
+			GPIO2_CLK_ENB, 0, &_lock);
+	clk_register_clkdev(clk, NULL, "d8100000.gpio");
+
+	clk = clk_register_gate(NULL, "ssp0_clk", "apb_clk", 0, PERIP1_CLK_ENB,
+			SSP0_CLK_ENB, 0, &_lock);
+	clk_register_clkdev(clk, NULL, "ssp-pl022.0");
+
+	clk = clk_register_gate(NULL, "ssp1_clk", "apb_clk", 0, PERIP1_CLK_ENB,
+			SSP1_CLK_ENB, 0, &_lock);
+	clk_register_clkdev(clk, NULL, "ssp-pl022.1");
+
+	clk = clk_register_gate(NULL, "ssp2_clk", "apb_clk", 0, PERIP1_CLK_ENB,
+			SSP2_CLK_ENB, 0, &_lock);
+	clk_register_clkdev(clk, NULL, "ssp-pl022.2");
+}
diff --git a/drivers/clocksource/Kconfig b/drivers/clocksource/Kconfig
index 5138927a416..99c6b203e6c 100644
--- a/drivers/clocksource/Kconfig
+++ b/drivers/clocksource/Kconfig
@@ -18,7 +18,7 @@ config DW_APB_TIMER
 
 config CLKSRC_DBX500_PRCMU
 	bool "Clocksource PRCMU Timer"
-	depends on UX500_SOC_DB5500 || UX500_SOC_DB8500
+	depends on UX500_SOC_DB8500
 	default y
 	help
 	  Use the always on PRCMU Timer as clocksource
diff --git a/drivers/cpufreq/db8500-cpufreq.c b/drivers/cpufreq/db8500-cpufreq.c
index 0bf1b8910ee..74b830b635a 100644
--- a/drivers/cpufreq/db8500-cpufreq.c
+++ b/drivers/cpufreq/db8500-cpufreq.c
@@ -161,7 +161,7 @@ static struct cpufreq_driver db8500_cpufreq_driver = {
 
 static int __init db8500_cpufreq_register(void)
 {
-	if (!cpu_is_u8500v20_or_later())
+	if (!cpu_is_u8500_family())
 		return -ENODEV;
 
 	pr_info("cpufreq for DB8500 started\n");
diff --git a/drivers/cpuidle/cpuidle.c b/drivers/cpuidle/cpuidle.c
index 2f0083a51a9..d90519cec88 100644
--- a/drivers/cpuidle/cpuidle.c
+++ b/drivers/cpuidle/cpuidle.c
@@ -40,17 +40,6 @@ void disable_cpuidle(void)
 	off = 1;
 }
 
-#if defined(CONFIG_ARCH_HAS_CPU_IDLE_WAIT)
-static void cpuidle_kick_cpus(void)
-{
-	cpu_idle_wait();
-}
-#elif defined(CONFIG_SMP)
-# error "Arch needs cpu_idle_wait() equivalent here"
-#else /* !CONFIG_ARCH_HAS_CPU_IDLE_WAIT && !CONFIG_SMP */
-static void cpuidle_kick_cpus(void) {}
-#endif
-
 static int __cpuidle_register_device(struct cpuidle_device *dev);
 
 static inline int cpuidle_enter(struct cpuidle_device *dev,
@@ -186,7 +175,7 @@ void cpuidle_uninstall_idle_handler(void)
 {
 	if (enabled_devices) {
 		initialized = 0;
-		cpuidle_kick_cpus();
+		kick_all_cpus_sync();
 	}
 }
 
diff --git a/drivers/crypto/Kconfig b/drivers/crypto/Kconfig
index ab9abb46d01..1092a770482 100644
--- a/drivers/crypto/Kconfig
+++ b/drivers/crypto/Kconfig
@@ -111,6 +111,7 @@ config CRYPTO_DES_S390
 	depends on S390
 	select CRYPTO_ALGAPI
 	select CRYPTO_BLKCIPHER
+	select CRYPTO_DES
 	help
 	  This is the s390 hardware accelerated implementation of the
 	  DES cipher algorithm (FIPS 46-2), and Triple DES EDE (FIPS 46-3).
@@ -164,6 +165,7 @@ config CRYPTO_DEV_MV_CESA
 	select CRYPTO_ALGAPI
 	select CRYPTO_AES
 	select CRYPTO_BLKCIPHER2
+	select CRYPTO_HASH
 	help
 	  This driver allows you to utilize the Cryptographic Engines and
 	  Security Accelerator (CESA) which can be found on the Marvell Orion
@@ -295,4 +297,32 @@ config CRYPTO_DEV_TEGRA_AES
 	  To compile this driver as a module, choose M here: the module
 	  will be called tegra-aes.
 
+config CRYPTO_DEV_NX
+	tristate "Support for Power7+ in-Nest cryptographic accleration"
+	depends on PPC64 && IBMVIO
+	select CRYPTO_AES
+	select CRYPTO_CBC
+	select CRYPTO_ECB
+	select CRYPTO_CCM
+	select CRYPTO_GCM
+	select CRYPTO_AUTHENC
+	select CRYPTO_XCBC
+	select CRYPTO_SHA256
+	select CRYPTO_SHA512
+	help
+	  Support for Power7+ in-Nest cryptographic acceleration. This
+	  module supports acceleration for AES and SHA2 algorithms. If you
+	  choose 'M' here, this module will be called nx_crypto.
+
+config CRYPTO_DEV_UX500
+	tristate "Driver for ST-Ericsson UX500 crypto hardware acceleration"
+	depends on ARCH_U8500
+	select CRYPTO_ALGAPI
+	help
+	  Driver for ST-Ericsson UX500 crypto engine.
+
+if CRYPTO_DEV_UX500
+	source "drivers/crypto/ux500/Kconfig"
+endif # if CRYPTO_DEV_UX500
+
 endif # CRYPTO_HW
diff --git a/drivers/crypto/Makefile b/drivers/crypto/Makefile
index f3e64eadd7a..01390325d72 100644
--- a/drivers/crypto/Makefile
+++ b/drivers/crypto/Makefile
@@ -14,3 +14,4 @@ obj-$(CONFIG_CRYPTO_DEV_OMAP_AES) += omap-aes.o
 obj-$(CONFIG_CRYPTO_DEV_PICOXCELL) += picoxcell_crypto.o
 obj-$(CONFIG_CRYPTO_DEV_S5P) += s5p-sss.o
 obj-$(CONFIG_CRYPTO_DEV_TEGRA_AES) += tegra-aes.o
+obj-$(CONFIG_CRYPTO_DEV_UX500) += ux500/
\ No newline at end of file
diff --git a/drivers/crypto/amcc/crypto4xx_core.c b/drivers/crypto/amcc/crypto4xx_core.c
index 13f8e1a1498..802e85102c3 100644
--- a/drivers/crypto/amcc/crypto4xx_core.c
+++ b/drivers/crypto/amcc/crypto4xx_core.c
@@ -1244,9 +1244,9 @@ err_start_dev:
 	iounmap(core_dev->dev->ce_base);
 err_iomap:
 	free_irq(core_dev->irq, dev);
+err_request_irq:
 	irq_dispose_mapping(core_dev->irq);
 	tasklet_kill(&core_dev->tasklet);
-err_request_irq:
 	crypto4xx_destroy_sdr(core_dev->dev);
 err_build_sdr:
 	crypto4xx_destroy_gdr(core_dev->dev);
diff --git a/drivers/crypto/caam/caamalg.c b/drivers/crypto/caam/caamalg.c
index 534a36469d5..4eec389184d 100644
--- a/drivers/crypto/caam/caamalg.c
+++ b/drivers/crypto/caam/caamalg.c
@@ -2267,8 +2267,11 @@ static void __exit caam_algapi_exit(void)
 	int i, err;
 
 	dev_node = of_find_compatible_node(NULL, NULL, "fsl,sec-v4.0");
-	if (!dev_node)
-		return;
+	if (!dev_node) {
+		dev_node = of_find_compatible_node(NULL, NULL, "fsl,sec4.0");
+		if (!dev_node)
+			return;
+	}
 
 	pdev = of_find_device_by_node(dev_node);
 	if (!pdev)
@@ -2350,8 +2353,11 @@ static int __init caam_algapi_init(void)
 	int i = 0, err = 0;
 
 	dev_node = of_find_compatible_node(NULL, NULL, "fsl,sec-v4.0");
-	if (!dev_node)
-		return -ENODEV;
+	if (!dev_node) {
+		dev_node = of_find_compatible_node(NULL, NULL, "fsl,sec4.0");
+		if (!dev_node)
+			return -ENODEV;
+	}
 
 	pdev = of_find_device_by_node(dev_node);
 	if (!pdev)
diff --git a/drivers/crypto/caam/ctrl.c b/drivers/crypto/caam/ctrl.c
index c5f61c55d92..77557ebcd33 100644
--- a/drivers/crypto/caam/ctrl.c
+++ b/drivers/crypto/caam/ctrl.c
@@ -98,6 +98,12 @@ static int caam_probe(struct platform_device *pdev)
 	rspec = 0;
 	for_each_compatible_node(np, NULL, "fsl,sec-v4.0-job-ring")
 		rspec++;
+	if (!rspec) {
+		/* for backward compatible with device trees */
+		for_each_compatible_node(np, NULL, "fsl,sec4.0-job-ring")
+			rspec++;
+	}
+
 	ctrlpriv->jrdev = kzalloc(sizeof(struct device *) * rspec, GFP_KERNEL);
 	if (ctrlpriv->jrdev == NULL) {
 		iounmap(&topregs->ctrl);
@@ -111,6 +117,13 @@ static int caam_probe(struct platform_device *pdev)
 		ctrlpriv->total_jobrs++;
 		ring++;
 	}
+	if (!ring) {
+		for_each_compatible_node(np, NULL, "fsl,sec4.0-job-ring") {
+			caam_jr_probe(pdev, np, ring);
+			ctrlpriv->total_jobrs++;
+			ring++;
+		}
+	}
 
 	/* Check to see if QI present. If so, enable */
 	ctrlpriv->qi_present = !!(rd_reg64(&topregs->ctrl.perfmon.comp_parms) &
@@ -226,6 +239,9 @@ static struct of_device_id caam_match[] = {
 	{
 		.compatible = "fsl,sec-v4.0",
 	},
+	{
+		.compatible = "fsl,sec4.0",
+	},
 	{},
 };
 MODULE_DEVICE_TABLE(of, caam_match);
diff --git a/drivers/crypto/mv_cesa.c b/drivers/crypto/mv_cesa.c
index e6ecc5f2394..1cc6b3f3e26 100644
--- a/drivers/crypto/mv_cesa.c
+++ b/drivers/crypto/mv_cesa.c
@@ -16,6 +16,7 @@
 #include <linux/scatterlist.h>
 #include <linux/slab.h>
 #include <linux/module.h>
+#include <linux/clk.h>
 #include <crypto/internal/hash.h>
 #include <crypto/sha.h>
 
@@ -79,6 +80,7 @@ struct crypto_priv {
 	void __iomem *reg;
 	void __iomem *sram;
 	int irq;
+	struct clk *clk;
 	struct task_struct *queue_th;
 
 	/* the lock protects queue and eng_st */
@@ -1053,6 +1055,12 @@ static int mv_probe(struct platform_device *pdev)
 	if (ret)
 		goto err_thread;
 
+	/* Not all platforms can gate the clock, so it is not
+	   an error if the clock does not exists. */
+	cp->clk = clk_get(&pdev->dev, NULL);
+	if (!IS_ERR(cp->clk))
+		clk_prepare_enable(cp->clk);
+
 	writel(SEC_INT_ACCEL0_DONE, cpg->reg + SEC_ACCEL_INT_MASK);
 	writel(SEC_CFG_STOP_DIG_ERR, cpg->reg + SEC_ACCEL_CFG);
 	writel(SRAM_CONFIG, cpg->reg + SEC_ACCEL_DESC_P0);
@@ -1118,6 +1126,12 @@ static int mv_remove(struct platform_device *pdev)
 	memset(cp->sram, 0, cp->sram_size);
 	iounmap(cp->sram);
 	iounmap(cp->reg);
+
+	if (!IS_ERR(cp->clk)) {
+		clk_disable_unprepare(cp->clk);
+		clk_put(cp->clk);
+	}
+
 	kfree(cp);
 	cpg = NULL;
 	return 0;
diff --git a/drivers/crypto/nx/Makefile b/drivers/crypto/nx/Makefile
new file mode 100644
index 00000000000..411ce59c80d
--- /dev/null
+++ b/drivers/crypto/nx/Makefile
@@ -0,0 +1,11 @@
+obj-$(CONFIG_CRYPTO_DEV_NX) += nx-crypto.o
+nx-crypto-objs := nx.o \
+		  nx_debugfs.o \
+		  nx-aes-cbc.o \
+		  nx-aes-ecb.o \
+		  nx-aes-gcm.o \
+		  nx-aes-ccm.o \
+		  nx-aes-ctr.o \
+		  nx-aes-xcbc.o \
+		  nx-sha256.o \
+		  nx-sha512.o
diff --git a/drivers/crypto/nx/nx-aes-cbc.c b/drivers/crypto/nx/nx-aes-cbc.c
new file mode 100644
index 00000000000..69ed796ee32
--- /dev/null
+++ b/drivers/crypto/nx/nx-aes-cbc.c
@@ -0,0 +1,141 @@
+/**
+ * AES CBC routines supporting the Power 7+ Nest Accelerators driver
+ *
+ * Copyright (C) 2011-2012 International Business Machines 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; version 2 only.
+ *
+ * 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.
+ *
+ * Author: Kent Yoder <yoder1@us.ibm.com>
+ */
+
+#include <crypto/aes.h>
+#include <crypto/algapi.h>
+#include <linux/module.h>
+#include <linux/types.h>
+#include <linux/crypto.h>
+#include <asm/vio.h>
+
+#include "nx_csbcpb.h"
+#include "nx.h"
+
+
+static int cbc_aes_nx_set_key(struct crypto_tfm *tfm,
+			      const u8          *in_key,
+			      unsigned int       key_len)
+{
+	struct nx_crypto_ctx *nx_ctx = crypto_tfm_ctx(tfm);
+	struct nx_csbcpb *csbcpb = nx_ctx->csbcpb;
+
+	nx_ctx_init(nx_ctx, HCOP_FC_AES);
+
+	switch (key_len) {
+	case AES_KEYSIZE_128:
+		NX_CPB_SET_KEY_SIZE(csbcpb, NX_KS_AES_128);
+		nx_ctx->ap = &nx_ctx->props[NX_PROPS_AES_128];
+		break;
+	case AES_KEYSIZE_192:
+		NX_CPB_SET_KEY_SIZE(csbcpb, NX_KS_AES_192);
+		nx_ctx->ap = &nx_ctx->props[NX_PROPS_AES_192];
+		break;
+	case AES_KEYSIZE_256:
+		NX_CPB_SET_KEY_SIZE(csbcpb, NX_KS_AES_256);
+		nx_ctx->ap = &nx_ctx->props[NX_PROPS_AES_256];
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	csbcpb->cpb.hdr.mode = NX_MODE_AES_CBC;
+	memcpy(csbcpb->cpb.aes_cbc.key, in_key, key_len);
+
+	return 0;
+}
+
+static int cbc_aes_nx_crypt(struct blkcipher_desc *desc,
+			    struct scatterlist    *dst,
+			    struct scatterlist    *src,
+			    unsigned int           nbytes,
+			    int                    enc)
+{
+	struct nx_crypto_ctx *nx_ctx = crypto_blkcipher_ctx(desc->tfm);
+	struct nx_csbcpb *csbcpb = nx_ctx->csbcpb;
+	int rc;
+
+	if (nbytes > nx_ctx->ap->databytelen)
+		return -EINVAL;
+
+	if (enc)
+		NX_CPB_FDM(csbcpb) |= NX_FDM_ENDE_ENCRYPT;
+	else
+		NX_CPB_FDM(csbcpb) &= ~NX_FDM_ENDE_ENCRYPT;
+
+	rc = nx_build_sg_lists(nx_ctx, desc, dst, src, nbytes,
+			       csbcpb->cpb.aes_cbc.iv);
+	if (rc)
+		goto out;
+
+	if (!nx_ctx->op.inlen || !nx_ctx->op.outlen) {
+		rc = -EINVAL;
+		goto out;
+	}
+
+	rc = nx_hcall_sync(nx_ctx, &nx_ctx->op,
+			   desc->flags & CRYPTO_TFM_REQ_MAY_SLEEP);
+	if (rc)
+		goto out;
+
+	atomic_inc(&(nx_ctx->stats->aes_ops));
+	atomic64_add(csbcpb->csb.processed_byte_count,
+		     &(nx_ctx->stats->aes_bytes));
+out:
+	return rc;
+}
+
+static int cbc_aes_nx_encrypt(struct blkcipher_desc *desc,
+			      struct scatterlist    *dst,
+			      struct scatterlist    *src,
+			      unsigned int           nbytes)
+{
+	return cbc_aes_nx_crypt(desc, dst, src, nbytes, 1);
+}
+
+static int cbc_aes_nx_decrypt(struct blkcipher_desc *desc,
+			      struct scatterlist    *dst,
+			      struct scatterlist    *src,
+			      unsigned int           nbytes)
+{
+	return cbc_aes_nx_crypt(desc, dst, src, nbytes, 0);
+}
+
+struct crypto_alg nx_cbc_aes_alg = {
+	.cra_name        = "cbc(aes)",
+	.cra_driver_name = "cbc-aes-nx",
+	.cra_priority    = 300,
+	.cra_flags       = CRYPTO_ALG_TYPE_BLKCIPHER,
+	.cra_blocksize   = AES_BLOCK_SIZE,
+	.cra_ctxsize     = sizeof(struct nx_crypto_ctx),
+	.cra_type        = &crypto_blkcipher_type,
+	.cra_module      = THIS_MODULE,
+	.cra_list        = LIST_HEAD_INIT(nx_cbc_aes_alg.cra_list),
+	.cra_init        = nx_crypto_ctx_aes_cbc_init,
+	.cra_exit        = nx_crypto_ctx_exit,
+	.cra_blkcipher = {
+		.min_keysize = AES_MIN_KEY_SIZE,
+		.max_keysize = AES_MAX_KEY_SIZE,
+		.ivsize      = AES_BLOCK_SIZE,
+		.setkey      = cbc_aes_nx_set_key,
+		.encrypt     = cbc_aes_nx_encrypt,
+		.decrypt     = cbc_aes_nx_decrypt,
+	}
+};
diff --git a/drivers/crypto/nx/nx-aes-ccm.c b/drivers/crypto/nx/nx-aes-ccm.c
new file mode 100644
index 00000000000..7aeac678b9c
--- /dev/null
+++ b/drivers/crypto/nx/nx-aes-ccm.c
@@ -0,0 +1,468 @@
+/**
+ * AES CCM routines supporting the Power 7+ Nest Accelerators driver
+ *
+ * Copyright (C) 2012 International Business Machines 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; version 2 only.
+ *
+ * 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.
+ *
+ * Author: Kent Yoder <yoder1@us.ibm.com>
+ */
+
+#include <crypto/internal/aead.h>
+#include <crypto/aes.h>
+#include <crypto/algapi.h>
+#include <crypto/scatterwalk.h>
+#include <linux/module.h>
+#include <linux/types.h>
+#include <linux/crypto.h>
+#include <asm/vio.h>
+
+#include "nx_csbcpb.h"
+#include "nx.h"
+
+
+static int ccm_aes_nx_set_key(struct crypto_aead *tfm,
+			      const u8           *in_key,
+			      unsigned int        key_len)
+{
+	struct nx_crypto_ctx *nx_ctx = crypto_tfm_ctx(&tfm->base);
+	struct nx_csbcpb *csbcpb = nx_ctx->csbcpb;
+	struct nx_csbcpb *csbcpb_aead = nx_ctx->csbcpb_aead;
+
+	nx_ctx_init(nx_ctx, HCOP_FC_AES);
+
+	switch (key_len) {
+	case AES_KEYSIZE_128:
+		NX_CPB_SET_KEY_SIZE(csbcpb, NX_KS_AES_128);
+		NX_CPB_SET_KEY_SIZE(csbcpb_aead, NX_KS_AES_128);
+		nx_ctx->ap = &nx_ctx->props[NX_PROPS_AES_128];
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	csbcpb->cpb.hdr.mode = NX_MODE_AES_CCM;
+	memcpy(csbcpb->cpb.aes_ccm.key, in_key, key_len);
+
+	csbcpb_aead->cpb.hdr.mode = NX_MODE_AES_CCA;
+	memcpy(csbcpb_aead->cpb.aes_cca.key, in_key, key_len);
+
+	return 0;
+
+}
+
+static int ccm4309_aes_nx_set_key(struct crypto_aead *tfm,
+				  const u8           *in_key,
+				  unsigned int        key_len)
+{
+	struct nx_crypto_ctx *nx_ctx = crypto_tfm_ctx(&tfm->base);
+
+	if (key_len < 3)
+		return -EINVAL;
+
+	key_len -= 3;
+
+	memcpy(nx_ctx->priv.ccm.nonce, in_key + key_len, 3);
+
+	return ccm_aes_nx_set_key(tfm, in_key, key_len);
+}
+
+static int ccm_aes_nx_setauthsize(struct crypto_aead *tfm,
+				  unsigned int authsize)
+{
+	switch (authsize) {
+	case 4:
+	case 6:
+	case 8:
+	case 10:
+	case 12:
+	case 14:
+	case 16:
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	crypto_aead_crt(tfm)->authsize = authsize;
+
+	return 0;
+}
+
+static int ccm4309_aes_nx_setauthsize(struct crypto_aead *tfm,
+				      unsigned int authsize)
+{
+	switch (authsize) {
+	case 8:
+	case 12:
+	case 16:
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	crypto_aead_crt(tfm)->authsize = authsize;
+
+	return 0;
+}
+
+/* taken from crypto/ccm.c */
+static int set_msg_len(u8 *block, unsigned int msglen, int csize)
+{
+	__be32 data;
+
+	memset(block, 0, csize);
+	block += csize;
+
+	if (csize >= 4)
+		csize = 4;
+	else if (msglen > (unsigned int)(1 << (8 * csize)))
+		return -EOVERFLOW;
+
+	data = cpu_to_be32(msglen);
+	memcpy(block - csize, (u8 *)&data + 4 - csize, csize);
+
+	return 0;
+}
+
+/* taken from crypto/ccm.c */
+static inline int crypto_ccm_check_iv(const u8 *iv)
+{
+	/* 2 <= L <= 8, so 1 <= L' <= 7. */
+	if (1 > iv[0] || iv[0] > 7)
+		return -EINVAL;
+
+	return 0;
+}
+
+/* based on code from crypto/ccm.c */
+static int generate_b0(u8 *iv, unsigned int assoclen, unsigned int authsize,
+		       unsigned int cryptlen, u8 *b0)
+{
+	unsigned int l, lp, m = authsize;
+	int rc;
+
+	memcpy(b0, iv, 16);
+
+	lp = b0[0];
+	l = lp + 1;
+
+	/* set m, bits 3-5 */
+	*b0 |= (8 * ((m - 2) / 2));
+
+	/* set adata, bit 6, if associated data is used */
+	if (assoclen)
+		*b0 |= 64;
+
+	rc = set_msg_len(b0 + 16 - l, cryptlen, l);
+
+	return rc;
+}
+
+static int generate_pat(u8                   *iv,
+			struct aead_request  *req,
+			struct nx_crypto_ctx *nx_ctx,
+			unsigned int          authsize,
+			unsigned int          nbytes,
+			u8                   *out)
+{
+	struct nx_sg *nx_insg = nx_ctx->in_sg;
+	struct nx_sg *nx_outsg = nx_ctx->out_sg;
+	unsigned int iauth_len = 0;
+	struct vio_pfo_op *op = NULL;
+	u8 tmp[16], *b1 = NULL, *b0 = NULL, *result = NULL;
+	int rc;
+
+	/* zero the ctr value */
+	memset(iv + 15 - iv[0], 0, iv[0] + 1);
+
+	if (!req->assoclen) {
+		b0 = nx_ctx->csbcpb->cpb.aes_ccm.in_pat_or_b0;
+	} else if (req->assoclen <= 14) {
+		/* if associated data is 14 bytes or less, we do 1 GCM
+		 * operation on 2 AES blocks, B0 (stored in the csbcpb) and B1,
+		 * which is fed in through the source buffers here */
+		b0 = nx_ctx->csbcpb->cpb.aes_ccm.in_pat_or_b0;
+		b1 = nx_ctx->priv.ccm.iauth_tag;
+		iauth_len = req->assoclen;
+
+		nx_insg = nx_build_sg_list(nx_insg, b1, 16, nx_ctx->ap->sglen);
+		nx_outsg = nx_build_sg_list(nx_outsg, tmp, 16,
+					    nx_ctx->ap->sglen);
+
+		/* inlen should be negative, indicating to phyp that its a
+		 * pointer to an sg list */
+		nx_ctx->op.inlen = (nx_ctx->in_sg - nx_insg) *
+					sizeof(struct nx_sg);
+		nx_ctx->op.outlen = (nx_ctx->out_sg - nx_outsg) *
+					sizeof(struct nx_sg);
+
+		NX_CPB_FDM(nx_ctx->csbcpb) |= NX_FDM_ENDE_ENCRYPT;
+		NX_CPB_FDM(nx_ctx->csbcpb) |= NX_FDM_INTERMEDIATE;
+
+		op = &nx_ctx->op;
+		result = nx_ctx->csbcpb->cpb.aes_ccm.out_pat_or_mac;
+	} else if (req->assoclen <= 65280) {
+		/* if associated data is less than (2^16 - 2^8), we construct
+		 * B1 differently and feed in the associated data to a CCA
+		 * operation */
+		b0 = nx_ctx->csbcpb_aead->cpb.aes_cca.b0;
+		b1 = nx_ctx->csbcpb_aead->cpb.aes_cca.b1;
+		iauth_len = 14;
+
+		/* remaining assoc data must have scatterlist built for it */
+		nx_insg = nx_walk_and_build(nx_insg, nx_ctx->ap->sglen,
+					    req->assoc, iauth_len,
+					    req->assoclen - iauth_len);
+		nx_ctx->op_aead.inlen = (nx_ctx->in_sg - nx_insg) *
+						sizeof(struct nx_sg);
+
+		op = &nx_ctx->op_aead;
+		result = nx_ctx->csbcpb_aead->cpb.aes_cca.out_pat_or_b0;
+	} else {
+		/* if associated data is less than (2^32), we construct B1
+		 * differently yet again and feed in the associated data to a
+		 * CCA operation */
+		pr_err("associated data len is %u bytes (returning -EINVAL)\n",
+		       req->assoclen);
+		rc = -EINVAL;
+	}
+
+	rc = generate_b0(iv, req->assoclen, authsize, nbytes, b0);
+	if (rc)
+		goto done;
+
+	if (b1) {
+		memset(b1, 0, 16);
+		*(u16 *)b1 = (u16)req->assoclen;
+
+		scatterwalk_map_and_copy(b1 + 2, req->assoc, 0,
+					 iauth_len, SCATTERWALK_FROM_SG);
+
+		rc = nx_hcall_sync(nx_ctx, op,
+				   req->base.flags & CRYPTO_TFM_REQ_MAY_SLEEP);
+		if (rc)
+			goto done;
+
+		atomic_inc(&(nx_ctx->stats->aes_ops));
+		atomic64_add(req->assoclen, &(nx_ctx->stats->aes_bytes));
+
+		memcpy(out, result, AES_BLOCK_SIZE);
+	}
+done:
+	return rc;
+}
+
+static int ccm_nx_decrypt(struct aead_request   *req,
+			  struct blkcipher_desc *desc)
+{
+	struct nx_crypto_ctx *nx_ctx = crypto_tfm_ctx(req->base.tfm);
+	struct nx_csbcpb *csbcpb = nx_ctx->csbcpb;
+	unsigned int nbytes = req->cryptlen;
+	unsigned int authsize = crypto_aead_authsize(crypto_aead_reqtfm(req));
+	struct nx_ccm_priv *priv = &nx_ctx->priv.ccm;
+	int rc = -1;
+
+	if (nbytes > nx_ctx->ap->databytelen)
+		return -EINVAL;
+
+	nbytes -= authsize;
+
+	/* copy out the auth tag to compare with later */
+	scatterwalk_map_and_copy(priv->oauth_tag,
+				 req->src, nbytes, authsize,
+				 SCATTERWALK_FROM_SG);
+
+	rc = generate_pat(desc->info, req, nx_ctx, authsize, nbytes,
+			  csbcpb->cpb.aes_ccm.in_pat_or_b0);
+	if (rc)
+		goto out;
+
+	rc = nx_build_sg_lists(nx_ctx, desc, req->dst, req->src, nbytes,
+			       csbcpb->cpb.aes_ccm.iv_or_ctr);
+	if (rc)
+		goto out;
+
+	NX_CPB_FDM(nx_ctx->csbcpb) &= ~NX_FDM_ENDE_ENCRYPT;
+	NX_CPB_FDM(nx_ctx->csbcpb) &= ~NX_FDM_INTERMEDIATE;
+
+	rc = nx_hcall_sync(nx_ctx, &nx_ctx->op,
+			   req->base.flags & CRYPTO_TFM_REQ_MAY_SLEEP);
+	if (rc)
+		goto out;
+
+	atomic_inc(&(nx_ctx->stats->aes_ops));
+	atomic64_add(csbcpb->csb.processed_byte_count,
+		     &(nx_ctx->stats->aes_bytes));
+
+	rc = memcmp(csbcpb->cpb.aes_ccm.out_pat_or_mac, priv->oauth_tag,
+		    authsize) ? -EBADMSG : 0;
+out:
+	return rc;
+}
+
+static int ccm_nx_encrypt(struct aead_request   *req,
+			  struct blkcipher_desc *desc)
+{
+	struct nx_crypto_ctx *nx_ctx = crypto_tfm_ctx(req->base.tfm);
+	struct nx_csbcpb *csbcpb = nx_ctx->csbcpb;
+	unsigned int nbytes = req->cryptlen;
+	unsigned int authsize = crypto_aead_authsize(crypto_aead_reqtfm(req));
+	int rc = -1;
+
+	if (nbytes > nx_ctx->ap->databytelen)
+		return -EINVAL;
+
+	rc = generate_pat(desc->info, req, nx_ctx, authsize, nbytes,
+			  csbcpb->cpb.aes_ccm.in_pat_or_b0);
+	if (rc)
+		goto out;
+
+	rc = nx_build_sg_lists(nx_ctx, desc, req->dst, req->src, nbytes,
+			       csbcpb->cpb.aes_ccm.iv_or_ctr);
+	if (rc)
+		goto out;
+
+	NX_CPB_FDM(csbcpb) |= NX_FDM_ENDE_ENCRYPT;
+	NX_CPB_FDM(csbcpb) &= ~NX_FDM_INTERMEDIATE;
+
+	rc = nx_hcall_sync(nx_ctx, &nx_ctx->op,
+			   req->base.flags & CRYPTO_TFM_REQ_MAY_SLEEP);
+	if (rc)
+		goto out;
+
+	atomic_inc(&(nx_ctx->stats->aes_ops));
+	atomic64_add(csbcpb->csb.processed_byte_count,
+		     &(nx_ctx->stats->aes_bytes));
+
+	/* copy out the auth tag */
+	scatterwalk_map_and_copy(csbcpb->cpb.aes_ccm.out_pat_or_mac,
+				 req->dst, nbytes, authsize,
+				 SCATTERWALK_TO_SG);
+out:
+	return rc;
+}
+
+static int ccm4309_aes_nx_encrypt(struct aead_request *req)
+{
+	struct nx_crypto_ctx *nx_ctx = crypto_tfm_ctx(req->base.tfm);
+	struct blkcipher_desc desc;
+	u8 *iv = nx_ctx->priv.ccm.iv;
+
+	iv[0] = 3;
+	memcpy(iv + 1, nx_ctx->priv.ccm.nonce, 3);
+	memcpy(iv + 4, req->iv, 8);
+
+	desc.info = iv;
+	desc.tfm = (struct crypto_blkcipher *)req->base.tfm;
+
+	return ccm_nx_encrypt(req, &desc);
+}
+
+static int ccm_aes_nx_encrypt(struct aead_request *req)
+{
+	struct blkcipher_desc desc;
+	int rc;
+
+	desc.info = req->iv;
+	desc.tfm = (struct crypto_blkcipher *)req->base.tfm;
+
+	rc = crypto_ccm_check_iv(desc.info);
+	if (rc)
+		return rc;
+
+	return ccm_nx_encrypt(req, &desc);
+}
+
+static int ccm4309_aes_nx_decrypt(struct aead_request *req)
+{
+	struct nx_crypto_ctx *nx_ctx = crypto_tfm_ctx(req->base.tfm);
+	struct blkcipher_desc desc;
+	u8 *iv = nx_ctx->priv.ccm.iv;
+
+	iv[0] = 3;
+	memcpy(iv + 1, nx_ctx->priv.ccm.nonce, 3);
+	memcpy(iv + 4, req->iv, 8);
+
+	desc.info = iv;
+	desc.tfm = (struct crypto_blkcipher *)req->base.tfm;
+
+	return ccm_nx_decrypt(req, &desc);
+}
+
+static int ccm_aes_nx_decrypt(struct aead_request *req)
+{
+	struct blkcipher_desc desc;
+	int rc;
+
+	desc.info = req->iv;
+	desc.tfm = (struct crypto_blkcipher *)req->base.tfm;
+
+	rc = crypto_ccm_check_iv(desc.info);
+	if (rc)
+		return rc;
+
+	return ccm_nx_decrypt(req, &desc);
+}
+
+/* tell the block cipher walk routines that this is a stream cipher by
+ * setting cra_blocksize to 1. Even using blkcipher_walk_virt_block
+ * during encrypt/decrypt doesn't solve this problem, because it calls
+ * blkcipher_walk_done under the covers, which doesn't use walk->blocksize,
+ * but instead uses this tfm->blocksize. */
+struct crypto_alg nx_ccm_aes_alg = {
+	.cra_name        = "ccm(aes)",
+	.cra_driver_name = "ccm-aes-nx",
+	.cra_priority    = 300,
+	.cra_flags       = CRYPTO_ALG_TYPE_AEAD |
+			   CRYPTO_ALG_NEED_FALLBACK,
+	.cra_blocksize   = 1,
+	.cra_ctxsize     = sizeof(struct nx_crypto_ctx),
+	.cra_type        = &crypto_aead_type,
+	.cra_module      = THIS_MODULE,
+	.cra_list        = LIST_HEAD_INIT(nx_ccm_aes_alg.cra_list),
+	.cra_init        = nx_crypto_ctx_aes_ccm_init,
+	.cra_exit        = nx_crypto_ctx_exit,
+	.cra_aead = {
+		.ivsize      = AES_BLOCK_SIZE,
+		.maxauthsize = AES_BLOCK_SIZE,
+		.setkey      = ccm_aes_nx_set_key,
+		.setauthsize = ccm_aes_nx_setauthsize,
+		.encrypt     = ccm_aes_nx_encrypt,
+		.decrypt     = ccm_aes_nx_decrypt,
+	}
+};
+
+struct crypto_alg nx_ccm4309_aes_alg = {
+	.cra_name        = "rfc4309(ccm(aes))",
+	.cra_driver_name = "rfc4309-ccm-aes-nx",
+	.cra_priority    = 300,
+	.cra_flags       = CRYPTO_ALG_TYPE_AEAD |
+			   CRYPTO_ALG_NEED_FALLBACK,
+	.cra_blocksize   = 1,
+	.cra_ctxsize     = sizeof(struct nx_crypto_ctx),
+	.cra_type        = &crypto_nivaead_type,
+	.cra_module      = THIS_MODULE,
+	.cra_list        = LIST_HEAD_INIT(nx_ccm4309_aes_alg.cra_list),
+	.cra_init        = nx_crypto_ctx_aes_ccm_init,
+	.cra_exit        = nx_crypto_ctx_exit,
+	.cra_aead = {
+		.ivsize      = 8,
+		.maxauthsize = AES_BLOCK_SIZE,
+		.setkey      = ccm4309_aes_nx_set_key,
+		.setauthsize = ccm4309_aes_nx_setauthsize,
+		.encrypt     = ccm4309_aes_nx_encrypt,
+		.decrypt     = ccm4309_aes_nx_decrypt,
+		.geniv       = "seqiv",
+	}
+};
diff --git a/drivers/crypto/nx/nx-aes-ctr.c b/drivers/crypto/nx/nx-aes-ctr.c
new file mode 100644
index 00000000000..52d4eb05e8f
--- /dev/null
+++ b/drivers/crypto/nx/nx-aes-ctr.c
@@ -0,0 +1,178 @@
+/**
+ * AES CTR routines supporting the Power 7+ Nest Accelerators driver
+ *
+ * Copyright (C) 2011-2012 International Business Machines 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; version 2 only.
+ *
+ * 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.
+ *
+ * Author: Kent Yoder <yoder1@us.ibm.com>
+ */
+
+#include <crypto/aes.h>
+#include <crypto/ctr.h>
+#include <crypto/algapi.h>
+#include <linux/module.h>
+#include <linux/types.h>
+#include <linux/crypto.h>
+#include <asm/vio.h>
+
+#include "nx_csbcpb.h"
+#include "nx.h"
+
+
+static int ctr_aes_nx_set_key(struct crypto_tfm *tfm,
+			      const u8          *in_key,
+			      unsigned int       key_len)
+{
+	struct nx_crypto_ctx *nx_ctx = crypto_tfm_ctx(tfm);
+	struct nx_csbcpb *csbcpb = nx_ctx->csbcpb;
+
+	nx_ctx_init(nx_ctx, HCOP_FC_AES);
+
+	switch (key_len) {
+	case AES_KEYSIZE_128:
+		NX_CPB_SET_KEY_SIZE(csbcpb, NX_KS_AES_128);
+		nx_ctx->ap = &nx_ctx->props[NX_PROPS_AES_128];
+		break;
+	case AES_KEYSIZE_192:
+		NX_CPB_SET_KEY_SIZE(csbcpb, NX_KS_AES_192);
+		nx_ctx->ap = &nx_ctx->props[NX_PROPS_AES_192];
+		break;
+	case AES_KEYSIZE_256:
+		NX_CPB_SET_KEY_SIZE(csbcpb, NX_KS_AES_256);
+		nx_ctx->ap = &nx_ctx->props[NX_PROPS_AES_256];
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	csbcpb->cpb.hdr.mode = NX_MODE_AES_CTR;
+	memcpy(csbcpb->cpb.aes_ctr.key, in_key, key_len);
+
+	return 0;
+}
+
+static int ctr3686_aes_nx_set_key(struct crypto_tfm *tfm,
+				  const u8          *in_key,
+				  unsigned int       key_len)
+{
+	struct nx_crypto_ctx *nx_ctx = crypto_tfm_ctx(tfm);
+
+	if (key_len < CTR_RFC3686_NONCE_SIZE)
+		return -EINVAL;
+
+	memcpy(nx_ctx->priv.ctr.iv,
+	       in_key + key_len - CTR_RFC3686_NONCE_SIZE,
+	       CTR_RFC3686_NONCE_SIZE);
+
+	key_len -= CTR_RFC3686_NONCE_SIZE;
+
+	return ctr_aes_nx_set_key(tfm, in_key, key_len);
+}
+
+static int ctr_aes_nx_crypt(struct blkcipher_desc *desc,
+			    struct scatterlist    *dst,
+			    struct scatterlist    *src,
+			    unsigned int           nbytes)
+{
+	struct nx_crypto_ctx *nx_ctx = crypto_blkcipher_ctx(desc->tfm);
+	struct nx_csbcpb *csbcpb = nx_ctx->csbcpb;
+	int rc;
+
+	if (nbytes > nx_ctx->ap->databytelen)
+		return -EINVAL;
+
+	rc = nx_build_sg_lists(nx_ctx, desc, dst, src, nbytes,
+			       csbcpb->cpb.aes_ctr.iv);
+	if (rc)
+		goto out;
+
+	if (!nx_ctx->op.inlen || !nx_ctx->op.outlen) {
+		rc = -EINVAL;
+		goto out;
+	}
+
+	rc = nx_hcall_sync(nx_ctx, &nx_ctx->op,
+			   desc->flags & CRYPTO_TFM_REQ_MAY_SLEEP);
+	if (rc)
+		goto out;
+
+	atomic_inc(&(nx_ctx->stats->aes_ops));
+	atomic64_add(csbcpb->csb.processed_byte_count,
+		     &(nx_ctx->stats->aes_bytes));
+out:
+	return rc;
+}
+
+static int ctr3686_aes_nx_crypt(struct blkcipher_desc *desc,
+				struct scatterlist    *dst,
+				struct scatterlist    *src,
+				unsigned int           nbytes)
+{
+	struct nx_crypto_ctx *nx_ctx = crypto_blkcipher_ctx(desc->tfm);
+	u8 *iv = nx_ctx->priv.ctr.iv;
+
+	memcpy(iv + CTR_RFC3686_NONCE_SIZE,
+	       desc->info, CTR_RFC3686_IV_SIZE);
+	iv[15] = 1;
+
+	desc->info = nx_ctx->priv.ctr.iv;
+
+	return ctr_aes_nx_crypt(desc, dst, src, nbytes);
+}
+
+struct crypto_alg nx_ctr_aes_alg = {
+	.cra_name        = "ctr(aes)",
+	.cra_driver_name = "ctr-aes-nx",
+	.cra_priority    = 300,
+	.cra_flags       = CRYPTO_ALG_TYPE_BLKCIPHER,
+	.cra_blocksize   = 1,
+	.cra_ctxsize     = sizeof(struct nx_crypto_ctx),
+	.cra_type        = &crypto_blkcipher_type,
+	.cra_module      = THIS_MODULE,
+	.cra_list        = LIST_HEAD_INIT(nx_ctr_aes_alg.cra_list),
+	.cra_init        = nx_crypto_ctx_aes_ctr_init,
+	.cra_exit        = nx_crypto_ctx_exit,
+	.cra_blkcipher = {
+		.min_keysize = AES_MIN_KEY_SIZE,
+		.max_keysize = AES_MAX_KEY_SIZE,
+		.ivsize      = AES_BLOCK_SIZE,
+		.setkey      = ctr_aes_nx_set_key,
+		.encrypt     = ctr_aes_nx_crypt,
+		.decrypt     = ctr_aes_nx_crypt,
+	}
+};
+
+struct crypto_alg nx_ctr3686_aes_alg = {
+	.cra_name        = "rfc3686(ctr(aes))",
+	.cra_driver_name = "rfc3686-ctr-aes-nx",
+	.cra_priority    = 300,
+	.cra_flags       = CRYPTO_ALG_TYPE_BLKCIPHER,
+	.cra_blocksize   = 1,
+	.cra_ctxsize     = sizeof(struct nx_crypto_ctx),
+	.cra_type        = &crypto_blkcipher_type,
+	.cra_module      = THIS_MODULE,
+	.cra_list        = LIST_HEAD_INIT(nx_ctr3686_aes_alg.cra_list),
+	.cra_init        = nx_crypto_ctx_aes_ctr_init,
+	.cra_exit        = nx_crypto_ctx_exit,
+	.cra_blkcipher = {
+		.min_keysize = AES_MIN_KEY_SIZE + CTR_RFC3686_NONCE_SIZE,
+		.max_keysize = AES_MAX_KEY_SIZE + CTR_RFC3686_NONCE_SIZE,
+		.ivsize      = CTR_RFC3686_IV_SIZE,
+		.geniv       = "seqiv",
+		.setkey      = ctr3686_aes_nx_set_key,
+		.encrypt     = ctr3686_aes_nx_crypt,
+		.decrypt     = ctr3686_aes_nx_crypt,
+	}
+};
diff --git a/drivers/crypto/nx/nx-aes-ecb.c b/drivers/crypto/nx/nx-aes-ecb.c
new file mode 100644
index 00000000000..7b77bc2d1df
--- /dev/null
+++ b/drivers/crypto/nx/nx-aes-ecb.c
@@ -0,0 +1,139 @@
+/**
+ * AES ECB routines supporting the Power 7+ Nest Accelerators driver
+ *
+ * Copyright (C) 2011-2012 International Business Machines 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; version 2 only.
+ *
+ * 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.
+ *
+ * Author: Kent Yoder <yoder1@us.ibm.com>
+ */
+
+#include <crypto/aes.h>
+#include <crypto/algapi.h>
+#include <linux/module.h>
+#include <linux/types.h>
+#include <linux/crypto.h>
+#include <asm/vio.h>
+
+#include "nx_csbcpb.h"
+#include "nx.h"
+
+
+static int ecb_aes_nx_set_key(struct crypto_tfm *tfm,
+			      const u8          *in_key,
+			      unsigned int       key_len)
+{
+	struct nx_crypto_ctx *nx_ctx = crypto_tfm_ctx(tfm);
+	struct nx_csbcpb *csbcpb = (struct nx_csbcpb *)nx_ctx->csbcpb;
+
+	nx_ctx_init(nx_ctx, HCOP_FC_AES);
+
+	switch (key_len) {
+	case AES_KEYSIZE_128:
+		NX_CPB_SET_KEY_SIZE(csbcpb, NX_KS_AES_128);
+		nx_ctx->ap = &nx_ctx->props[NX_PROPS_AES_128];
+		break;
+	case AES_KEYSIZE_192:
+		NX_CPB_SET_KEY_SIZE(csbcpb, NX_KS_AES_192);
+		nx_ctx->ap = &nx_ctx->props[NX_PROPS_AES_192];
+		break;
+	case AES_KEYSIZE_256:
+		NX_CPB_SET_KEY_SIZE(csbcpb, NX_KS_AES_256);
+		nx_ctx->ap = &nx_ctx->props[NX_PROPS_AES_256];
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	csbcpb->cpb.hdr.mode = NX_MODE_AES_ECB;
+	memcpy(csbcpb->cpb.aes_ecb.key, in_key, key_len);
+
+	return 0;
+}
+
+static int ecb_aes_nx_crypt(struct blkcipher_desc *desc,
+			    struct scatterlist    *dst,
+			    struct scatterlist    *src,
+			    unsigned int           nbytes,
+			    int                    enc)
+{
+	struct nx_crypto_ctx *nx_ctx = crypto_blkcipher_ctx(desc->tfm);
+	struct nx_csbcpb *csbcpb = nx_ctx->csbcpb;
+	int rc;
+
+	if (nbytes > nx_ctx->ap->databytelen)
+		return -EINVAL;
+
+	if (enc)
+		NX_CPB_FDM(csbcpb) |= NX_FDM_ENDE_ENCRYPT;
+	else
+		NX_CPB_FDM(csbcpb) &= ~NX_FDM_ENDE_ENCRYPT;
+
+	rc = nx_build_sg_lists(nx_ctx, desc, dst, src, nbytes, NULL);
+	if (rc)
+		goto out;
+
+	if (!nx_ctx->op.inlen || !nx_ctx->op.outlen) {
+		rc = -EINVAL;
+		goto out;
+	}
+
+	rc = nx_hcall_sync(nx_ctx, &nx_ctx->op,
+			   desc->flags & CRYPTO_TFM_REQ_MAY_SLEEP);
+	if (rc)
+		goto out;
+
+	atomic_inc(&(nx_ctx->stats->aes_ops));
+	atomic64_add(csbcpb->csb.processed_byte_count,
+		     &(nx_ctx->stats->aes_bytes));
+out:
+	return rc;
+}
+
+static int ecb_aes_nx_encrypt(struct blkcipher_desc *desc,
+			      struct scatterlist    *dst,
+			      struct scatterlist    *src,
+			      unsigned int           nbytes)
+{
+	return ecb_aes_nx_crypt(desc, dst, src, nbytes, 1);
+}
+
+static int ecb_aes_nx_decrypt(struct blkcipher_desc *desc,
+			      struct scatterlist    *dst,
+			      struct scatterlist    *src,
+			      unsigned int           nbytes)
+{
+	return ecb_aes_nx_crypt(desc, dst, src, nbytes, 0);
+}
+
+struct crypto_alg nx_ecb_aes_alg = {
+	.cra_name        = "ecb(aes)",
+	.cra_driver_name = "ecb-aes-nx",
+	.cra_priority    = 300,
+	.cra_flags       = CRYPTO_ALG_TYPE_BLKCIPHER,
+	.cra_blocksize   = AES_BLOCK_SIZE,
+	.cra_ctxsize     = sizeof(struct nx_crypto_ctx),
+	.cra_type        = &crypto_blkcipher_type,
+	.cra_module      = THIS_MODULE,
+	.cra_list        = LIST_HEAD_INIT(nx_ecb_aes_alg.cra_list),
+	.cra_init        = nx_crypto_ctx_aes_ecb_init,
+	.cra_exit        = nx_crypto_ctx_exit,
+	.cra_blkcipher = {
+		.min_keysize = AES_MIN_KEY_SIZE,
+		.max_keysize = AES_MAX_KEY_SIZE,
+		.setkey      = ecb_aes_nx_set_key,
+		.encrypt     = ecb_aes_nx_encrypt,
+		.decrypt     = ecb_aes_nx_decrypt,
+	}
+};
diff --git a/drivers/crypto/nx/nx-aes-gcm.c b/drivers/crypto/nx/nx-aes-gcm.c
new file mode 100644
index 00000000000..9ab1c7341da
--- /dev/null
+++ b/drivers/crypto/nx/nx-aes-gcm.c
@@ -0,0 +1,353 @@
+/**
+ * AES GCM routines supporting the Power 7+ Nest Accelerators driver
+ *
+ * Copyright (C) 2012 International Business Machines 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; version 2 only.
+ *
+ * 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.
+ *
+ * Author: Kent Yoder <yoder1@us.ibm.com>
+ */
+
+#include <crypto/internal/aead.h>
+#include <crypto/aes.h>
+#include <crypto/algapi.h>
+#include <crypto/scatterwalk.h>
+#include <linux/module.h>
+#include <linux/types.h>
+#include <linux/crypto.h>
+#include <asm/vio.h>
+
+#include "nx_csbcpb.h"
+#include "nx.h"
+
+
+static int gcm_aes_nx_set_key(struct crypto_aead *tfm,
+			      const u8           *in_key,
+			      unsigned int        key_len)
+{
+	struct nx_crypto_ctx *nx_ctx = crypto_tfm_ctx(&tfm->base);
+	struct nx_csbcpb *csbcpb = nx_ctx->csbcpb;
+	struct nx_csbcpb *csbcpb_aead = nx_ctx->csbcpb_aead;
+
+	nx_ctx_init(nx_ctx, HCOP_FC_AES);
+
+	switch (key_len) {
+	case AES_KEYSIZE_128:
+		NX_CPB_SET_KEY_SIZE(csbcpb, NX_KS_AES_128);
+		NX_CPB_SET_KEY_SIZE(csbcpb_aead, NX_KS_AES_128);
+		nx_ctx->ap = &nx_ctx->props[NX_PROPS_AES_128];
+		break;
+	case AES_KEYSIZE_192:
+		NX_CPB_SET_KEY_SIZE(csbcpb, NX_KS_AES_192);
+		NX_CPB_SET_KEY_SIZE(csbcpb_aead, NX_KS_AES_192);
+		nx_ctx->ap = &nx_ctx->props[NX_PROPS_AES_192];
+		break;
+	case AES_KEYSIZE_256:
+		NX_CPB_SET_KEY_SIZE(csbcpb, NX_KS_AES_256);
+		NX_CPB_SET_KEY_SIZE(csbcpb_aead, NX_KS_AES_256);
+		nx_ctx->ap = &nx_ctx->props[NX_PROPS_AES_256];
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	csbcpb->cpb.hdr.mode = NX_MODE_AES_GCM;
+	memcpy(csbcpb->cpb.aes_gcm.key, in_key, key_len);
+
+	csbcpb_aead->cpb.hdr.mode = NX_MODE_AES_GCA;
+	memcpy(csbcpb_aead->cpb.aes_gca.key, in_key, key_len);
+
+	return 0;
+}
+
+static int gcm4106_aes_nx_set_key(struct crypto_aead *tfm,
+				  const u8           *in_key,
+				  unsigned int        key_len)
+{
+	struct nx_crypto_ctx *nx_ctx = crypto_tfm_ctx(&tfm->base);
+	char *nonce = nx_ctx->priv.gcm.nonce;
+	int rc;
+
+	if (key_len < 4)
+		return -EINVAL;
+
+	key_len -= 4;
+
+	rc = gcm_aes_nx_set_key(tfm, in_key, key_len);
+	if (rc)
+		goto out;
+
+	memcpy(nonce, in_key + key_len, 4);
+out:
+	return rc;
+}
+
+static int gcm_aes_nx_setauthsize(struct crypto_aead *tfm,
+				  unsigned int authsize)
+{
+	if (authsize > crypto_aead_alg(tfm)->maxauthsize)
+		return -EINVAL;
+
+	crypto_aead_crt(tfm)->authsize = authsize;
+
+	return 0;
+}
+
+static int gcm4106_aes_nx_setauthsize(struct crypto_aead *tfm,
+				      unsigned int authsize)
+{
+	switch (authsize) {
+	case 8:
+	case 12:
+	case 16:
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	crypto_aead_crt(tfm)->authsize = authsize;
+
+	return 0;
+}
+
+static int nx_gca(struct nx_crypto_ctx  *nx_ctx,
+		  struct aead_request   *req,
+		  u8                    *out)
+{
+	struct nx_csbcpb *csbcpb_aead = nx_ctx->csbcpb_aead;
+	int rc = -EINVAL;
+	struct scatter_walk walk;
+	struct nx_sg *nx_sg = nx_ctx->in_sg;
+
+	if (req->assoclen > nx_ctx->ap->databytelen)
+		goto out;
+
+	if (req->assoclen <= AES_BLOCK_SIZE) {
+		scatterwalk_start(&walk, req->assoc);
+		scatterwalk_copychunks(out, &walk, req->assoclen,
+				       SCATTERWALK_FROM_SG);
+		scatterwalk_done(&walk, SCATTERWALK_FROM_SG, 0);
+
+		rc = 0;
+		goto out;
+	}
+
+	nx_sg = nx_walk_and_build(nx_sg, nx_ctx->ap->sglen, req->assoc, 0,
+				  req->assoclen);
+	nx_ctx->op_aead.inlen = (nx_ctx->in_sg - nx_sg) * sizeof(struct nx_sg);
+
+	rc = nx_hcall_sync(nx_ctx, &nx_ctx->op_aead,
+			   req->base.flags & CRYPTO_TFM_REQ_MAY_SLEEP);
+	if (rc)
+		goto out;
+
+	atomic_inc(&(nx_ctx->stats->aes_ops));
+	atomic64_add(req->assoclen, &(nx_ctx->stats->aes_bytes));
+
+	memcpy(out, csbcpb_aead->cpb.aes_gca.out_pat, AES_BLOCK_SIZE);
+out:
+	return rc;
+}
+
+static int gcm_aes_nx_crypt(struct aead_request *req, int enc)
+{
+	struct nx_crypto_ctx *nx_ctx = crypto_tfm_ctx(req->base.tfm);
+	struct nx_csbcpb *csbcpb = nx_ctx->csbcpb;
+	struct blkcipher_desc desc;
+	unsigned int nbytes = req->cryptlen;
+	int rc = -EINVAL;
+
+	if (nbytes > nx_ctx->ap->databytelen)
+		goto out;
+
+	desc.info = nx_ctx->priv.gcm.iv;
+	/* initialize the counter */
+	*(u32 *)(desc.info + NX_GCM_CTR_OFFSET) = 1;
+
+	/* For scenarios where the input message is zero length, AES CTR mode
+	 * may be used. Set the source data to be a single block (16B) of all
+	 * zeros, and set the input IV value to be the same as the GMAC IV
+	 * value. - nx_wb 4.8.1.3 */
+	if (nbytes == 0) {
+		char src[AES_BLOCK_SIZE] = {};
+		struct scatterlist sg;
+
+		desc.tfm = crypto_alloc_blkcipher("ctr(aes)", 0, 0);
+		if (IS_ERR(desc.tfm)) {
+			rc = -ENOMEM;
+			goto out;
+		}
+
+		crypto_blkcipher_setkey(desc.tfm, csbcpb->cpb.aes_gcm.key,
+			NX_CPB_KEY_SIZE(csbcpb) == NX_KS_AES_128 ? 16 :
+			NX_CPB_KEY_SIZE(csbcpb) == NX_KS_AES_192 ? 24 : 32);
+
+		sg_init_one(&sg, src, AES_BLOCK_SIZE);
+		if (enc)
+			crypto_blkcipher_encrypt_iv(&desc, req->dst, &sg,
+						    AES_BLOCK_SIZE);
+		else
+			crypto_blkcipher_decrypt_iv(&desc, req->dst, &sg,
+						    AES_BLOCK_SIZE);
+		crypto_free_blkcipher(desc.tfm);
+
+		rc = 0;
+		goto out;
+	}
+
+	desc.tfm = (struct crypto_blkcipher *)req->base.tfm;
+
+	csbcpb->cpb.aes_gcm.bit_length_aad = req->assoclen * 8;
+
+	if (req->assoclen) {
+		rc = nx_gca(nx_ctx, req, csbcpb->cpb.aes_gcm.in_pat_or_aad);
+		if (rc)
+			goto out;
+	}
+
+	if (enc)
+		NX_CPB_FDM(csbcpb) |= NX_FDM_ENDE_ENCRYPT;
+	else
+		nbytes -= AES_BLOCK_SIZE;
+
+	csbcpb->cpb.aes_gcm.bit_length_data = nbytes * 8;
+
+	rc = nx_build_sg_lists(nx_ctx, &desc, req->dst, req->src, nbytes,
+			       csbcpb->cpb.aes_gcm.iv_or_cnt);
+	if (rc)
+		goto out;
+
+	rc = nx_hcall_sync(nx_ctx, &nx_ctx->op,
+			   req->base.flags & CRYPTO_TFM_REQ_MAY_SLEEP);
+	if (rc)
+		goto out;
+
+	atomic_inc(&(nx_ctx->stats->aes_ops));
+	atomic64_add(csbcpb->csb.processed_byte_count,
+		     &(nx_ctx->stats->aes_bytes));
+
+	if (enc) {
+		/* copy out the auth tag */
+		scatterwalk_map_and_copy(csbcpb->cpb.aes_gcm.out_pat_or_mac,
+				 req->dst, nbytes,
+				 crypto_aead_authsize(crypto_aead_reqtfm(req)),
+				 SCATTERWALK_TO_SG);
+	} else if (req->assoclen) {
+		u8 *itag = nx_ctx->priv.gcm.iauth_tag;
+		u8 *otag = csbcpb->cpb.aes_gcm.out_pat_or_mac;
+
+		scatterwalk_map_and_copy(itag, req->dst, nbytes,
+				 crypto_aead_authsize(crypto_aead_reqtfm(req)),
+				 SCATTERWALK_FROM_SG);
+		rc = memcmp(itag, otag,
+			    crypto_aead_authsize(crypto_aead_reqtfm(req))) ?
+		     -EBADMSG : 0;
+	}
+out:
+	return rc;
+}
+
+static int gcm_aes_nx_encrypt(struct aead_request *req)
+{
+	struct nx_crypto_ctx *nx_ctx = crypto_tfm_ctx(req->base.tfm);
+	char *iv = nx_ctx->priv.gcm.iv;
+
+	memcpy(iv, req->iv, 12);
+
+	return gcm_aes_nx_crypt(req, 1);
+}
+
+static int gcm_aes_nx_decrypt(struct aead_request *req)
+{
+	struct nx_crypto_ctx *nx_ctx = crypto_tfm_ctx(req->base.tfm);
+	char *iv = nx_ctx->priv.gcm.iv;
+
+	memcpy(iv, req->iv, 12);
+
+	return gcm_aes_nx_crypt(req, 0);
+}
+
+static int gcm4106_aes_nx_encrypt(struct aead_request *req)
+{
+	struct nx_crypto_ctx *nx_ctx = crypto_tfm_ctx(req->base.tfm);
+	char *iv = nx_ctx->priv.gcm.iv;
+	char *nonce = nx_ctx->priv.gcm.nonce;
+
+	memcpy(iv, nonce, NX_GCM4106_NONCE_LEN);
+	memcpy(iv + NX_GCM4106_NONCE_LEN, req->iv, 8);
+
+	return gcm_aes_nx_crypt(req, 1);
+}
+
+static int gcm4106_aes_nx_decrypt(struct aead_request *req)
+{
+	struct nx_crypto_ctx *nx_ctx = crypto_tfm_ctx(req->base.tfm);
+	char *iv = nx_ctx->priv.gcm.iv;
+	char *nonce = nx_ctx->priv.gcm.nonce;
+
+	memcpy(iv, nonce, NX_GCM4106_NONCE_LEN);
+	memcpy(iv + NX_GCM4106_NONCE_LEN, req->iv, 8);
+
+	return gcm_aes_nx_crypt(req, 0);
+}
+
+/* tell the block cipher walk routines that this is a stream cipher by
+ * setting cra_blocksize to 1. Even using blkcipher_walk_virt_block
+ * during encrypt/decrypt doesn't solve this problem, because it calls
+ * blkcipher_walk_done under the covers, which doesn't use walk->blocksize,
+ * but instead uses this tfm->blocksize. */
+struct crypto_alg nx_gcm_aes_alg = {
+	.cra_name        = "gcm(aes)",
+	.cra_driver_name = "gcm-aes-nx",
+	.cra_priority    = 300,
+	.cra_flags       = CRYPTO_ALG_TYPE_AEAD,
+	.cra_blocksize   = 1,
+	.cra_ctxsize     = sizeof(struct nx_crypto_ctx),
+	.cra_type        = &crypto_aead_type,
+	.cra_module      = THIS_MODULE,
+	.cra_list        = LIST_HEAD_INIT(nx_gcm_aes_alg.cra_list),
+	.cra_init        = nx_crypto_ctx_aes_gcm_init,
+	.cra_exit        = nx_crypto_ctx_exit,
+	.cra_aead = {
+		.ivsize      = AES_BLOCK_SIZE,
+		.maxauthsize = AES_BLOCK_SIZE,
+		.setkey      = gcm_aes_nx_set_key,
+		.setauthsize = gcm_aes_nx_setauthsize,
+		.encrypt     = gcm_aes_nx_encrypt,
+		.decrypt     = gcm_aes_nx_decrypt,
+	}
+};
+
+struct crypto_alg nx_gcm4106_aes_alg = {
+	.cra_name        = "rfc4106(gcm(aes))",
+	.cra_driver_name = "rfc4106-gcm-aes-nx",
+	.cra_priority    = 300,
+	.cra_flags       = CRYPTO_ALG_TYPE_AEAD,
+	.cra_blocksize   = 1,
+	.cra_ctxsize     = sizeof(struct nx_crypto_ctx),
+	.cra_type        = &crypto_nivaead_type,
+	.cra_module      = THIS_MODULE,
+	.cra_list        = LIST_HEAD_INIT(nx_gcm4106_aes_alg.cra_list),
+	.cra_init        = nx_crypto_ctx_aes_gcm_init,
+	.cra_exit        = nx_crypto_ctx_exit,
+	.cra_aead = {
+		.ivsize      = 8,
+		.maxauthsize = AES_BLOCK_SIZE,
+		.geniv       = "seqiv",
+		.setkey      = gcm4106_aes_nx_set_key,
+		.setauthsize = gcm4106_aes_nx_setauthsize,
+		.encrypt     = gcm4106_aes_nx_encrypt,
+		.decrypt     = gcm4106_aes_nx_decrypt,
+	}
+};
diff --git a/drivers/crypto/nx/nx-aes-xcbc.c b/drivers/crypto/nx/nx-aes-xcbc.c
new file mode 100644
index 00000000000..93923e4628c
--- /dev/null
+++ b/drivers/crypto/nx/nx-aes-xcbc.c
@@ -0,0 +1,236 @@
+/**
+ * AES XCBC routines supporting the Power 7+ Nest Accelerators driver
+ *
+ * Copyright (C) 2011-2012 International Business Machines 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; version 2 only.
+ *
+ * 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.
+ *
+ * Author: Kent Yoder <yoder1@us.ibm.com>
+ */
+
+#include <crypto/internal/hash.h>
+#include <crypto/aes.h>
+#include <crypto/algapi.h>
+#include <linux/module.h>
+#include <linux/types.h>
+#include <linux/crypto.h>
+#include <asm/vio.h>
+
+#include "nx_csbcpb.h"
+#include "nx.h"
+
+
+struct xcbc_state {
+	u8 state[AES_BLOCK_SIZE];
+	unsigned int count;
+	u8 buffer[AES_BLOCK_SIZE];
+};
+
+static int nx_xcbc_set_key(struct crypto_shash *desc,
+			   const u8            *in_key,
+			   unsigned int         key_len)
+{
+	struct nx_crypto_ctx *nx_ctx = crypto_shash_ctx(desc);
+
+	switch (key_len) {
+	case AES_KEYSIZE_128:
+		nx_ctx->ap = &nx_ctx->props[NX_PROPS_AES_128];
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	memcpy(nx_ctx->priv.xcbc.key, in_key, key_len);
+
+	return 0;
+}
+
+static int nx_xcbc_init(struct shash_desc *desc)
+{
+	struct xcbc_state *sctx = shash_desc_ctx(desc);
+	struct nx_crypto_ctx *nx_ctx = crypto_tfm_ctx(&desc->tfm->base);
+	struct nx_csbcpb *csbcpb = nx_ctx->csbcpb;
+	struct nx_sg *out_sg;
+
+	nx_ctx_init(nx_ctx, HCOP_FC_AES);
+
+	memset(sctx, 0, sizeof *sctx);
+
+	NX_CPB_SET_KEY_SIZE(csbcpb, NX_KS_AES_128);
+	csbcpb->cpb.hdr.mode = NX_MODE_AES_XCBC_MAC;
+
+	memcpy(csbcpb->cpb.aes_xcbc.key, nx_ctx->priv.xcbc.key, AES_BLOCK_SIZE);
+	memset(nx_ctx->priv.xcbc.key, 0, sizeof *nx_ctx->priv.xcbc.key);
+
+	out_sg = nx_build_sg_list(nx_ctx->out_sg, (u8 *)sctx->state,
+				  AES_BLOCK_SIZE, nx_ctx->ap->sglen);
+	nx_ctx->op.outlen = (nx_ctx->out_sg - out_sg) * sizeof(struct nx_sg);
+
+	return 0;
+}
+
+static int nx_xcbc_update(struct shash_desc *desc,
+			  const u8          *data,
+			  unsigned int       len)
+{
+	struct xcbc_state *sctx = shash_desc_ctx(desc);
+	struct nx_crypto_ctx *nx_ctx = crypto_tfm_ctx(&desc->tfm->base);
+	struct nx_csbcpb *csbcpb = nx_ctx->csbcpb;
+	struct nx_sg *in_sg;
+	u32 to_process, leftover;
+	int rc = 0;
+
+	if (NX_CPB_FDM(csbcpb) & NX_FDM_CONTINUATION) {
+		/* we've hit the nx chip previously and we're updating again,
+		 * so copy over the partial digest */
+		memcpy(csbcpb->cpb.aes_xcbc.cv,
+		       csbcpb->cpb.aes_xcbc.out_cv_mac, AES_BLOCK_SIZE);
+	}
+
+	/* 2 cases for total data len:
+	 *  1: <= AES_BLOCK_SIZE: copy into state, return 0
+	 *  2: > AES_BLOCK_SIZE: process X blocks, copy in leftover
+	 */
+	if (len + sctx->count <= AES_BLOCK_SIZE) {
+		memcpy(sctx->buffer + sctx->count, data, len);
+		sctx->count += len;
+		goto out;
+	}
+
+	/* to_process: the AES_BLOCK_SIZE data chunk to process in this
+	 * update */
+	to_process = (sctx->count + len) & ~(AES_BLOCK_SIZE - 1);
+	leftover = (sctx->count + len) & (AES_BLOCK_SIZE - 1);
+
+	/* the hardware will not accept a 0 byte operation for this algorithm
+	 * and the operation MUST be finalized to be correct. So if we happen
+	 * to get an update that falls on a block sized boundary, we must
+	 * save off the last block to finalize with later. */
+	if (!leftover) {
+		to_process -= AES_BLOCK_SIZE;
+		leftover = AES_BLOCK_SIZE;
+	}
+
+	if (sctx->count) {
+		in_sg = nx_build_sg_list(nx_ctx->in_sg, sctx->buffer,
+					 sctx->count, nx_ctx->ap->sglen);
+		in_sg = nx_build_sg_list(in_sg, (u8 *)data,
+					 to_process - sctx->count,
+					 nx_ctx->ap->sglen);
+		nx_ctx->op.inlen = (nx_ctx->in_sg - in_sg) *
+					sizeof(struct nx_sg);
+	} else {
+		in_sg = nx_build_sg_list(nx_ctx->in_sg, (u8 *)data, to_process,
+					 nx_ctx->ap->sglen);
+		nx_ctx->op.inlen = (nx_ctx->in_sg - in_sg) *
+					sizeof(struct nx_sg);
+	}
+
+	NX_CPB_FDM(csbcpb) |= NX_FDM_INTERMEDIATE;
+
+	if (!nx_ctx->op.inlen || !nx_ctx->op.outlen) {
+		rc = -EINVAL;
+		goto out;
+	}
+
+	rc = nx_hcall_sync(nx_ctx, &nx_ctx->op,
+			   desc->flags & CRYPTO_TFM_REQ_MAY_SLEEP);
+	if (rc)
+		goto out;
+
+	atomic_inc(&(nx_ctx->stats->aes_ops));
+
+	/* copy the leftover back into the state struct */
+	memcpy(sctx->buffer, data + len - leftover, leftover);
+	sctx->count = leftover;
+
+	/* everything after the first update is continuation */
+	NX_CPB_FDM(csbcpb) |= NX_FDM_CONTINUATION;
+out:
+	return rc;
+}
+
+static int nx_xcbc_final(struct shash_desc *desc, u8 *out)
+{
+	struct xcbc_state *sctx = shash_desc_ctx(desc);
+	struct nx_crypto_ctx *nx_ctx = crypto_tfm_ctx(&desc->tfm->base);
+	struct nx_csbcpb *csbcpb = nx_ctx->csbcpb;
+	struct nx_sg *in_sg, *out_sg;
+	int rc = 0;
+
+	if (NX_CPB_FDM(csbcpb) & NX_FDM_CONTINUATION) {
+		/* we've hit the nx chip previously, now we're finalizing,
+		 * so copy over the partial digest */
+		memcpy(csbcpb->cpb.aes_xcbc.cv,
+		       csbcpb->cpb.aes_xcbc.out_cv_mac, AES_BLOCK_SIZE);
+	} else if (sctx->count == 0) {
+		/* we've never seen an update, so this is a 0 byte op. The
+		 * hardware cannot handle a 0 byte op, so just copy out the
+		 * known 0 byte result. This is cheaper than allocating a
+		 * software context to do a 0 byte op */
+		u8 data[] = { 0x75, 0xf0, 0x25, 0x1d, 0x52, 0x8a, 0xc0, 0x1c,
+			      0x45, 0x73, 0xdf, 0xd5, 0x84, 0xd7, 0x9f, 0x29 };
+		memcpy(out, data, sizeof(data));
+		goto out;
+	}
+
+	/* final is represented by continuing the operation and indicating that
+	 * this is not an intermediate operation */
+	NX_CPB_FDM(csbcpb) &= ~NX_FDM_INTERMEDIATE;
+
+	in_sg = nx_build_sg_list(nx_ctx->in_sg, (u8 *)sctx->buffer,
+				 sctx->count, nx_ctx->ap->sglen);
+	out_sg = nx_build_sg_list(nx_ctx->out_sg, out, AES_BLOCK_SIZE,
+				  nx_ctx->ap->sglen);
+
+	nx_ctx->op.inlen = (nx_ctx->in_sg - in_sg) * sizeof(struct nx_sg);
+	nx_ctx->op.outlen = (nx_ctx->out_sg - out_sg) * sizeof(struct nx_sg);
+
+	if (!nx_ctx->op.outlen) {
+		rc = -EINVAL;
+		goto out;
+	}
+
+	rc = nx_hcall_sync(nx_ctx, &nx_ctx->op,
+			   desc->flags & CRYPTO_TFM_REQ_MAY_SLEEP);
+	if (rc)
+		goto out;
+
+	atomic_inc(&(nx_ctx->stats->aes_ops));
+
+	memcpy(out, csbcpb->cpb.aes_xcbc.out_cv_mac, AES_BLOCK_SIZE);
+out:
+	return rc;
+}
+
+struct shash_alg nx_shash_aes_xcbc_alg = {
+	.digestsize = AES_BLOCK_SIZE,
+	.init       = nx_xcbc_init,
+	.update     = nx_xcbc_update,
+	.final      = nx_xcbc_final,
+	.setkey     = nx_xcbc_set_key,
+	.descsize   = sizeof(struct xcbc_state),
+	.statesize  = sizeof(struct xcbc_state),
+	.base       = {
+		.cra_name        = "xcbc(aes)",
+		.cra_driver_name = "xcbc-aes-nx",
+		.cra_priority    = 300,
+		.cra_flags       = CRYPTO_ALG_TYPE_SHASH,
+		.cra_blocksize   = AES_BLOCK_SIZE,
+		.cra_module      = THIS_MODULE,
+		.cra_ctxsize     = sizeof(struct nx_crypto_ctx),
+		.cra_init        = nx_crypto_ctx_aes_xcbc_init,
+		.cra_exit        = nx_crypto_ctx_exit,
+	}
+};
diff --git a/drivers/crypto/nx/nx-sha256.c b/drivers/crypto/nx/nx-sha256.c
new file mode 100644
index 00000000000..9767315f8c0
--- /dev/null
+++ b/drivers/crypto/nx/nx-sha256.c
@@ -0,0 +1,246 @@
+/**
+ * SHA-256 routines supporting the Power 7+ Nest Accelerators driver
+ *
+ * Copyright (C) 2011-2012 International Business Machines 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; version 2 only.
+ *
+ * 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.
+ *
+ * Author: Kent Yoder <yoder1@us.ibm.com>
+ */
+
+#include <crypto/internal/hash.h>
+#include <crypto/sha.h>
+#include <linux/module.h>
+#include <asm/vio.h>
+
+#include "nx_csbcpb.h"
+#include "nx.h"
+
+
+static int nx_sha256_init(struct shash_desc *desc)
+{
+	struct sha256_state *sctx = shash_desc_ctx(desc);
+	struct nx_crypto_ctx *nx_ctx = crypto_tfm_ctx(&desc->tfm->base);
+	struct nx_sg *out_sg;
+
+	nx_ctx_init(nx_ctx, HCOP_FC_SHA);
+
+	memset(sctx, 0, sizeof *sctx);
+
+	nx_ctx->ap = &nx_ctx->props[NX_PROPS_SHA256];
+
+	NX_CPB_SET_DIGEST_SIZE(nx_ctx->csbcpb, NX_DS_SHA256);
+	out_sg = nx_build_sg_list(nx_ctx->out_sg, (u8 *)sctx->state,
+				  SHA256_DIGEST_SIZE, nx_ctx->ap->sglen);
+	nx_ctx->op.outlen = (nx_ctx->out_sg - out_sg) * sizeof(struct nx_sg);
+
+	return 0;
+}
+
+static int nx_sha256_update(struct shash_desc *desc, const u8 *data,
+			    unsigned int len)
+{
+	struct sha256_state *sctx = shash_desc_ctx(desc);
+	struct nx_crypto_ctx *nx_ctx = crypto_tfm_ctx(&desc->tfm->base);
+	struct nx_csbcpb *csbcpb = (struct nx_csbcpb *)nx_ctx->csbcpb;
+	struct nx_sg *in_sg;
+	u64 to_process, leftover;
+	int rc = 0;
+
+	if (NX_CPB_FDM(csbcpb) & NX_FDM_CONTINUATION) {
+		/* we've hit the nx chip previously and we're updating again,
+		 * so copy over the partial digest */
+		memcpy(csbcpb->cpb.sha256.input_partial_digest,
+		       csbcpb->cpb.sha256.message_digest, SHA256_DIGEST_SIZE);
+	}
+
+	/* 2 cases for total data len:
+	 *  1: <= SHA256_BLOCK_SIZE: copy into state, return 0
+	 *  2: > SHA256_BLOCK_SIZE: process X blocks, copy in leftover
+	 */
+	if (len + sctx->count <= SHA256_BLOCK_SIZE) {
+		memcpy(sctx->buf + sctx->count, data, len);
+		sctx->count += len;
+		goto out;
+	}
+
+	/* to_process: the SHA256_BLOCK_SIZE data chunk to process in this
+	 * update */
+	to_process = (sctx->count + len) & ~(SHA256_BLOCK_SIZE - 1);
+	leftover = (sctx->count + len) & (SHA256_BLOCK_SIZE - 1);
+
+	if (sctx->count) {
+		in_sg = nx_build_sg_list(nx_ctx->in_sg, (u8 *)sctx->buf,
+					 sctx->count, nx_ctx->ap->sglen);
+		in_sg = nx_build_sg_list(in_sg, (u8 *)data,
+					 to_process - sctx->count,
+					 nx_ctx->ap->sglen);
+		nx_ctx->op.inlen = (nx_ctx->in_sg - in_sg) *
+					sizeof(struct nx_sg);
+	} else {
+		in_sg = nx_build_sg_list(nx_ctx->in_sg, (u8 *)data,
+					 to_process, nx_ctx->ap->sglen);
+		nx_ctx->op.inlen = (nx_ctx->in_sg - in_sg) *
+					sizeof(struct nx_sg);
+	}
+
+	NX_CPB_FDM(csbcpb) |= NX_FDM_INTERMEDIATE;
+
+	if (!nx_ctx->op.inlen || !nx_ctx->op.outlen) {
+		rc = -EINVAL;
+		goto out;
+	}
+
+	rc = nx_hcall_sync(nx_ctx, &nx_ctx->op,
+			   desc->flags & CRYPTO_TFM_REQ_MAY_SLEEP);
+	if (rc)
+		goto out;
+
+	atomic_inc(&(nx_ctx->stats->sha256_ops));
+
+	/* copy the leftover back into the state struct */
+	memcpy(sctx->buf, data + len - leftover, leftover);
+	sctx->count = leftover;
+
+	csbcpb->cpb.sha256.message_bit_length += (u64)
+		(csbcpb->cpb.sha256.spbc * 8);
+
+	/* everything after the first update is continuation */
+	NX_CPB_FDM(csbcpb) |= NX_FDM_CONTINUATION;
+out:
+	return rc;
+}
+
+static int nx_sha256_final(struct shash_desc *desc, u8 *out)
+{
+	struct sha256_state *sctx = shash_desc_ctx(desc);
+	struct nx_crypto_ctx *nx_ctx = crypto_tfm_ctx(&desc->tfm->base);
+	struct nx_csbcpb *csbcpb = (struct nx_csbcpb *)nx_ctx->csbcpb;
+	struct nx_sg *in_sg, *out_sg;
+	int rc;
+
+	if (NX_CPB_FDM(csbcpb) & NX_FDM_CONTINUATION) {
+		/* we've hit the nx chip previously, now we're finalizing,
+		 * so copy over the partial digest */
+		memcpy(csbcpb->cpb.sha256.input_partial_digest,
+		       csbcpb->cpb.sha256.message_digest, SHA256_DIGEST_SIZE);
+	}
+
+	/* final is represented by continuing the operation and indicating that
+	 * this is not an intermediate operation */
+	NX_CPB_FDM(csbcpb) &= ~NX_FDM_INTERMEDIATE;
+
+	csbcpb->cpb.sha256.message_bit_length += (u64)(sctx->count * 8);
+
+	in_sg = nx_build_sg_list(nx_ctx->in_sg, (u8 *)sctx->buf,
+				 sctx->count, nx_ctx->ap->sglen);
+	out_sg = nx_build_sg_list(nx_ctx->out_sg, out, SHA256_DIGEST_SIZE,
+				  nx_ctx->ap->sglen);
+	nx_ctx->op.inlen = (nx_ctx->in_sg - in_sg) * sizeof(struct nx_sg);
+	nx_ctx->op.outlen = (nx_ctx->out_sg - out_sg) * sizeof(struct nx_sg);
+
+	if (!nx_ctx->op.outlen) {
+		rc = -EINVAL;
+		goto out;
+	}
+
+	rc = nx_hcall_sync(nx_ctx, &nx_ctx->op,
+			   desc->flags & CRYPTO_TFM_REQ_MAY_SLEEP);
+	if (rc)
+		goto out;
+
+	atomic_inc(&(nx_ctx->stats->sha256_ops));
+
+	atomic64_add(csbcpb->cpb.sha256.message_bit_length,
+		     &(nx_ctx->stats->sha256_bytes));
+	memcpy(out, csbcpb->cpb.sha256.message_digest, SHA256_DIGEST_SIZE);
+out:
+	return rc;
+}
+
+static int nx_sha256_export(struct shash_desc *desc, void *out)
+{
+	struct sha256_state *sctx = shash_desc_ctx(desc);
+	struct nx_crypto_ctx *nx_ctx = crypto_tfm_ctx(&desc->tfm->base);
+	struct nx_csbcpb *csbcpb = (struct nx_csbcpb *)nx_ctx->csbcpb;
+	struct sha256_state *octx = out;
+
+	octx->count = sctx->count +
+		      (csbcpb->cpb.sha256.message_bit_length / 8);
+	memcpy(octx->buf, sctx->buf, sizeof(octx->buf));
+
+	/* if no data has been processed yet, we need to export SHA256's
+	 * initial data, in case this context gets imported into a software
+	 * context */
+	if (csbcpb->cpb.sha256.message_bit_length)
+		memcpy(octx->state, csbcpb->cpb.sha256.message_digest,
+		       SHA256_DIGEST_SIZE);
+	else {
+		octx->state[0] = SHA256_H0;
+		octx->state[1] = SHA256_H1;
+		octx->state[2] = SHA256_H2;
+		octx->state[3] = SHA256_H3;
+		octx->state[4] = SHA256_H4;
+		octx->state[5] = SHA256_H5;
+		octx->state[6] = SHA256_H6;
+		octx->state[7] = SHA256_H7;
+	}
+
+	return 0;
+}
+
+static int nx_sha256_import(struct shash_desc *desc, const void *in)
+{
+	struct sha256_state *sctx = shash_desc_ctx(desc);
+	struct nx_crypto_ctx *nx_ctx = crypto_tfm_ctx(&desc->tfm->base);
+	struct nx_csbcpb *csbcpb = (struct nx_csbcpb *)nx_ctx->csbcpb;
+	const struct sha256_state *ictx = in;
+
+	memcpy(sctx->buf, ictx->buf, sizeof(ictx->buf));
+
+	sctx->count = ictx->count & 0x3f;
+	csbcpb->cpb.sha256.message_bit_length = (ictx->count & ~0x3f) * 8;
+
+	if (csbcpb->cpb.sha256.message_bit_length) {
+		memcpy(csbcpb->cpb.sha256.message_digest, ictx->state,
+		       SHA256_DIGEST_SIZE);
+
+		NX_CPB_FDM(csbcpb) |= NX_FDM_CONTINUATION;
+		NX_CPB_FDM(csbcpb) |= NX_FDM_INTERMEDIATE;
+	}
+
+	return 0;
+}
+
+struct shash_alg nx_shash_sha256_alg = {
+	.digestsize = SHA256_DIGEST_SIZE,
+	.init       = nx_sha256_init,
+	.update     = nx_sha256_update,
+	.final      = nx_sha256_final,
+	.export     = nx_sha256_export,
+	.import     = nx_sha256_import,
+	.descsize   = sizeof(struct sha256_state),
+	.statesize  = sizeof(struct sha256_state),
+	.base       = {
+		.cra_name        = "sha256",
+		.cra_driver_name = "sha256-nx",
+		.cra_priority    = 300,
+		.cra_flags       = CRYPTO_ALG_TYPE_SHASH,
+		.cra_blocksize   = SHA256_BLOCK_SIZE,
+		.cra_module      = THIS_MODULE,
+		.cra_ctxsize     = sizeof(struct nx_crypto_ctx),
+		.cra_init        = nx_crypto_ctx_sha_init,
+		.cra_exit        = nx_crypto_ctx_exit,
+	}
+};
diff --git a/drivers/crypto/nx/nx-sha512.c b/drivers/crypto/nx/nx-sha512.c
new file mode 100644
index 00000000000..3177b8c3d5f
--- /dev/null
+++ b/drivers/crypto/nx/nx-sha512.c
@@ -0,0 +1,265 @@
+/**
+ * SHA-512 routines supporting the Power 7+ Nest Accelerators driver
+ *
+ * Copyright (C) 2011-2012 International Business Machines 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; version 2 only.
+ *
+ * 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.
+ *
+ * Author: Kent Yoder <yoder1@us.ibm.com>
+ */
+
+#include <crypto/internal/hash.h>
+#include <crypto/sha.h>
+#include <linux/module.h>
+#include <asm/vio.h>
+
+#include "nx_csbcpb.h"
+#include "nx.h"
+
+
+static int nx_sha512_init(struct shash_desc *desc)
+{
+	struct sha512_state *sctx = shash_desc_ctx(desc);
+	struct nx_crypto_ctx *nx_ctx = crypto_tfm_ctx(&desc->tfm->base);
+	struct nx_sg *out_sg;
+
+	nx_ctx_init(nx_ctx, HCOP_FC_SHA);
+
+	memset(sctx, 0, sizeof *sctx);
+
+	nx_ctx->ap = &nx_ctx->props[NX_PROPS_SHA512];
+
+	NX_CPB_SET_DIGEST_SIZE(nx_ctx->csbcpb, NX_DS_SHA512);
+	out_sg = nx_build_sg_list(nx_ctx->out_sg, (u8 *)sctx->state,
+				  SHA512_DIGEST_SIZE, nx_ctx->ap->sglen);
+	nx_ctx->op.outlen = (nx_ctx->out_sg - out_sg) * sizeof(struct nx_sg);
+
+	return 0;
+}
+
+static int nx_sha512_update(struct shash_desc *desc, const u8 *data,
+			    unsigned int len)
+{
+	struct sha512_state *sctx = shash_desc_ctx(desc);
+	struct nx_crypto_ctx *nx_ctx = crypto_tfm_ctx(&desc->tfm->base);
+	struct nx_csbcpb *csbcpb = (struct nx_csbcpb *)nx_ctx->csbcpb;
+	struct nx_sg *in_sg;
+	u64 to_process, leftover, spbc_bits;
+	int rc = 0;
+
+	if (NX_CPB_FDM(csbcpb) & NX_FDM_CONTINUATION) {
+		/* we've hit the nx chip previously and we're updating again,
+		 * so copy over the partial digest */
+		memcpy(csbcpb->cpb.sha512.input_partial_digest,
+		       csbcpb->cpb.sha512.message_digest, SHA512_DIGEST_SIZE);
+	}
+
+	/* 2 cases for total data len:
+	 *  1: <= SHA512_BLOCK_SIZE: copy into state, return 0
+	 *  2: > SHA512_BLOCK_SIZE: process X blocks, copy in leftover
+	 */
+	if ((u64)len + sctx->count[0] <= SHA512_BLOCK_SIZE) {
+		memcpy(sctx->buf + sctx->count[0], data, len);
+		sctx->count[0] += len;
+		goto out;
+	}
+
+	/* to_process: the SHA512_BLOCK_SIZE data chunk to process in this
+	 * update */
+	to_process = (sctx->count[0] + len) & ~(SHA512_BLOCK_SIZE - 1);
+	leftover = (sctx->count[0] + len) & (SHA512_BLOCK_SIZE - 1);
+
+	if (sctx->count[0]) {
+		in_sg = nx_build_sg_list(nx_ctx->in_sg, (u8 *)sctx->buf,
+					 sctx->count[0], nx_ctx->ap->sglen);
+		in_sg = nx_build_sg_list(in_sg, (u8 *)data,
+					 to_process - sctx->count[0],
+					 nx_ctx->ap->sglen);
+		nx_ctx->op.inlen = (nx_ctx->in_sg - in_sg) *
+					sizeof(struct nx_sg);
+	} else {
+		in_sg = nx_build_sg_list(nx_ctx->in_sg, (u8 *)data,
+					 to_process, nx_ctx->ap->sglen);
+		nx_ctx->op.inlen = (nx_ctx->in_sg - in_sg) *
+					sizeof(struct nx_sg);
+	}
+
+	NX_CPB_FDM(csbcpb) |= NX_FDM_INTERMEDIATE;
+
+	if (!nx_ctx->op.inlen || !nx_ctx->op.outlen) {
+		rc = -EINVAL;
+		goto out;
+	}
+
+	rc = nx_hcall_sync(nx_ctx, &nx_ctx->op,
+			   desc->flags & CRYPTO_TFM_REQ_MAY_SLEEP);
+	if (rc)
+		goto out;
+
+	atomic_inc(&(nx_ctx->stats->sha512_ops));
+
+	/* copy the leftover back into the state struct */
+	memcpy(sctx->buf, data + len - leftover, leftover);
+	sctx->count[0] = leftover;
+
+	spbc_bits = csbcpb->cpb.sha512.spbc * 8;
+	csbcpb->cpb.sha512.message_bit_length_lo += spbc_bits;
+	if (csbcpb->cpb.sha512.message_bit_length_lo < spbc_bits)
+		csbcpb->cpb.sha512.message_bit_length_hi++;
+
+	/* everything after the first update is continuation */
+	NX_CPB_FDM(csbcpb) |= NX_FDM_CONTINUATION;
+out:
+	return rc;
+}
+
+static int nx_sha512_final(struct shash_desc *desc, u8 *out)
+{
+	struct sha512_state *sctx = shash_desc_ctx(desc);
+	struct nx_crypto_ctx *nx_ctx = crypto_tfm_ctx(&desc->tfm->base);
+	struct nx_csbcpb *csbcpb = (struct nx_csbcpb *)nx_ctx->csbcpb;
+	struct nx_sg *in_sg, *out_sg;
+	u64 count0;
+	int rc;
+
+	if (NX_CPB_FDM(csbcpb) & NX_FDM_CONTINUATION) {
+		/* we've hit the nx chip previously, now we're finalizing,
+		 * so copy over the partial digest */
+		memcpy(csbcpb->cpb.sha512.input_partial_digest,
+		       csbcpb->cpb.sha512.message_digest, SHA512_DIGEST_SIZE);
+	}
+
+	/* final is represented by continuing the operation and indicating that
+	 * this is not an intermediate operation */
+	NX_CPB_FDM(csbcpb) &= ~NX_FDM_INTERMEDIATE;
+
+	count0 = sctx->count[0] * 8;
+
+	csbcpb->cpb.sha512.message_bit_length_lo += count0;
+	if (csbcpb->cpb.sha512.message_bit_length_lo < count0)
+		csbcpb->cpb.sha512.message_bit_length_hi++;
+
+	in_sg = nx_build_sg_list(nx_ctx->in_sg, sctx->buf, sctx->count[0],
+				 nx_ctx->ap->sglen);
+	out_sg = nx_build_sg_list(nx_ctx->out_sg, out, SHA512_DIGEST_SIZE,
+				  nx_ctx->ap->sglen);
+	nx_ctx->op.inlen = (nx_ctx->in_sg - in_sg) * sizeof(struct nx_sg);
+	nx_ctx->op.outlen = (nx_ctx->out_sg - out_sg) * sizeof(struct nx_sg);
+
+	if (!nx_ctx->op.outlen) {
+		rc = -EINVAL;
+		goto out;
+	}
+
+	rc = nx_hcall_sync(nx_ctx, &nx_ctx->op,
+			   desc->flags & CRYPTO_TFM_REQ_MAY_SLEEP);
+	if (rc)
+		goto out;
+
+	atomic_inc(&(nx_ctx->stats->sha512_ops));
+	atomic64_add(csbcpb->cpb.sha512.message_bit_length_lo,
+		     &(nx_ctx->stats->sha512_bytes));
+
+	memcpy(out, csbcpb->cpb.sha512.message_digest, SHA512_DIGEST_SIZE);
+out:
+	return rc;
+}
+
+static int nx_sha512_export(struct shash_desc *desc, void *out)
+{
+	struct sha512_state *sctx = shash_desc_ctx(desc);
+	struct nx_crypto_ctx *nx_ctx = crypto_tfm_ctx(&desc->tfm->base);
+	struct nx_csbcpb *csbcpb = (struct nx_csbcpb *)nx_ctx->csbcpb;
+	struct sha512_state *octx = out;
+
+	/* move message_bit_length (128 bits) into count and convert its value
+	 * to bytes */
+	octx->count[0] = csbcpb->cpb.sha512.message_bit_length_lo >> 3 |
+			 ((csbcpb->cpb.sha512.message_bit_length_hi & 7) << 61);
+	octx->count[1] = csbcpb->cpb.sha512.message_bit_length_hi >> 3;
+
+	octx->count[0] += sctx->count[0];
+	if (octx->count[0] < sctx->count[0])
+		octx->count[1]++;
+
+	memcpy(octx->buf, sctx->buf, sizeof(octx->buf));
+
+	/* if no data has been processed yet, we need to export SHA512's
+	 * initial data, in case this context gets imported into a software
+	 * context */
+	if (csbcpb->cpb.sha512.message_bit_length_hi ||
+	    csbcpb->cpb.sha512.message_bit_length_lo)
+		memcpy(octx->state, csbcpb->cpb.sha512.message_digest,
+		       SHA512_DIGEST_SIZE);
+	else {
+		octx->state[0] = SHA512_H0;
+		octx->state[1] = SHA512_H1;
+		octx->state[2] = SHA512_H2;
+		octx->state[3] = SHA512_H3;
+		octx->state[4] = SHA512_H4;
+		octx->state[5] = SHA512_H5;
+		octx->state[6] = SHA512_H6;
+		octx->state[7] = SHA512_H7;
+	}
+
+	return 0;
+}
+
+static int nx_sha512_import(struct shash_desc *desc, const void *in)
+{
+	struct sha512_state *sctx = shash_desc_ctx(desc);
+	struct nx_crypto_ctx *nx_ctx = crypto_tfm_ctx(&desc->tfm->base);
+	struct nx_csbcpb *csbcpb = (struct nx_csbcpb *)nx_ctx->csbcpb;
+	const struct sha512_state *ictx = in;
+
+	memcpy(sctx->buf, ictx->buf, sizeof(ictx->buf));
+	sctx->count[0] = ictx->count[0] & 0x3f;
+	csbcpb->cpb.sha512.message_bit_length_lo = (ictx->count[0] & ~0x3f)
+							<< 3;
+	csbcpb->cpb.sha512.message_bit_length_hi = ictx->count[1] << 3 |
+						   ictx->count[0] >> 61;
+
+	if (csbcpb->cpb.sha512.message_bit_length_hi ||
+	    csbcpb->cpb.sha512.message_bit_length_lo) {
+		memcpy(csbcpb->cpb.sha512.message_digest, ictx->state,
+		       SHA512_DIGEST_SIZE);
+
+		NX_CPB_FDM(csbcpb) |= NX_FDM_CONTINUATION;
+		NX_CPB_FDM(csbcpb) |= NX_FDM_INTERMEDIATE;
+	}
+
+	return 0;
+}
+
+struct shash_alg nx_shash_sha512_alg = {
+	.digestsize = SHA512_DIGEST_SIZE,
+	.init       = nx_sha512_init,
+	.update     = nx_sha512_update,
+	.final      = nx_sha512_final,
+	.export     = nx_sha512_export,
+	.import     = nx_sha512_import,
+	.descsize   = sizeof(struct sha512_state),
+	.statesize  = sizeof(struct sha512_state),
+	.base       = {
+		.cra_name        = "sha512",
+		.cra_driver_name = "sha512-nx",
+		.cra_priority    = 300,
+		.cra_flags       = CRYPTO_ALG_TYPE_SHASH,
+		.cra_blocksize   = SHA512_BLOCK_SIZE,
+		.cra_module      = THIS_MODULE,
+		.cra_ctxsize     = sizeof(struct nx_crypto_ctx),
+		.cra_init        = nx_crypto_ctx_sha_init,
+		.cra_exit        = nx_crypto_ctx_exit,
+	}
+};
diff --git a/drivers/crypto/nx/nx.c b/drivers/crypto/nx/nx.c
new file mode 100644
index 00000000000..d7f179cc2e9
--- /dev/null
+++ b/drivers/crypto/nx/nx.c
@@ -0,0 +1,716 @@
+/**
+ * Routines supporting the Power 7+ Nest Accelerators driver
+ *
+ * Copyright (C) 2011-2012 International Business Machines 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; version 2 only.
+ *
+ * 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.
+ *
+ * Author: Kent Yoder <yoder1@us.ibm.com>
+ */
+
+#include <crypto/internal/hash.h>
+#include <crypto/hash.h>
+#include <crypto/aes.h>
+#include <crypto/sha.h>
+#include <crypto/algapi.h>
+#include <crypto/scatterwalk.h>
+#include <linux/module.h>
+#include <linux/moduleparam.h>
+#include <linux/types.h>
+#include <linux/mm.h>
+#include <linux/crypto.h>
+#include <linux/scatterlist.h>
+#include <linux/device.h>
+#include <linux/of.h>
+#include <asm/pSeries_reconfig.h>
+#include <asm/abs_addr.h>
+#include <asm/hvcall.h>
+#include <asm/vio.h>
+
+#include "nx_csbcpb.h"
+#include "nx.h"
+
+
+/**
+ * nx_hcall_sync - make an H_COP_OP hcall for the passed in op structure
+ *
+ * @nx_ctx: the crypto context handle
+ * @op: PFO operation struct to pass in
+ * @may_sleep: flag indicating the request can sleep
+ *
+ * Make the hcall, retrying while the hardware is busy. If we cannot yield
+ * the thread, limit the number of retries to 10 here.
+ */
+int nx_hcall_sync(struct nx_crypto_ctx *nx_ctx,
+		  struct vio_pfo_op    *op,
+		  u32                   may_sleep)
+{
+	int rc, retries = 10;
+	struct vio_dev *viodev = nx_driver.viodev;
+
+	atomic_inc(&(nx_ctx->stats->sync_ops));
+
+	do {
+		rc = vio_h_cop_sync(viodev, op);
+	} while ((rc == -EBUSY && !may_sleep && retries--) ||
+	         (rc == -EBUSY && may_sleep && cond_resched()));
+
+	if (rc) {
+		dev_dbg(&viodev->dev, "vio_h_cop_sync failed: rc: %d "
+			"hcall rc: %ld\n", rc, op->hcall_err);
+		atomic_inc(&(nx_ctx->stats->errors));
+		atomic_set(&(nx_ctx->stats->last_error), op->hcall_err);
+		atomic_set(&(nx_ctx->stats->last_error_pid), current->pid);
+	}
+
+	return rc;
+}
+
+/**
+ * nx_build_sg_list - build an NX scatter list describing a single  buffer
+ *
+ * @sg_head: pointer to the first scatter list element to build
+ * @start_addr: pointer to the linear buffer
+ * @len: length of the data at @start_addr
+ * @sgmax: the largest number of scatter list elements we're allowed to create
+ *
+ * This function will start writing nx_sg elements at @sg_head and keep
+ * writing them until all of the data from @start_addr is described or
+ * until sgmax elements have been written. Scatter list elements will be
+ * created such that none of the elements describes a buffer that crosses a 4K
+ * boundary.
+ */
+struct nx_sg *nx_build_sg_list(struct nx_sg *sg_head,
+			       u8           *start_addr,
+			       unsigned int  len,
+			       u32           sgmax)
+{
+	unsigned int sg_len = 0;
+	struct nx_sg *sg;
+	u64 sg_addr = (u64)start_addr;
+	u64 end_addr;
+
+	/* determine the start and end for this address range - slightly
+	 * different if this is in VMALLOC_REGION */
+	if (is_vmalloc_addr(start_addr))
+		sg_addr = phys_to_abs(page_to_phys(vmalloc_to_page(start_addr)))
+			  + offset_in_page(sg_addr);
+	else
+		sg_addr = virt_to_abs(sg_addr);
+
+	end_addr = sg_addr + len;
+
+	/* each iteration will write one struct nx_sg element and add the
+	 * length of data described by that element to sg_len. Once @len bytes
+	 * have been described (or @sgmax elements have been written), the
+	 * loop ends. min_t is used to ensure @end_addr falls on the same page
+	 * as sg_addr, if not, we need to create another nx_sg element for the
+	 * data on the next page */
+	for (sg = sg_head; sg_len < len; sg++) {
+		sg->addr = sg_addr;
+		sg_addr = min_t(u64, NX_PAGE_NUM(sg_addr + NX_PAGE_SIZE), end_addr);
+		sg->len = sg_addr - sg->addr;
+		sg_len += sg->len;
+
+		if ((sg - sg_head) == sgmax) {
+			pr_err("nx: scatter/gather list overflow, pid: %d\n",
+			       current->pid);
+			return NULL;
+		}
+	}
+
+	/* return the moved sg_head pointer */
+	return sg;
+}
+
+/**
+ * nx_walk_and_build - walk a linux scatterlist and build an nx scatterlist
+ *
+ * @nx_dst: pointer to the first nx_sg element to write
+ * @sglen: max number of nx_sg entries we're allowed to write
+ * @sg_src: pointer to the source linux scatterlist to walk
+ * @start: number of bytes to fast-forward past at the beginning of @sg_src
+ * @src_len: number of bytes to walk in @sg_src
+ */
+struct nx_sg *nx_walk_and_build(struct nx_sg       *nx_dst,
+				unsigned int        sglen,
+				struct scatterlist *sg_src,
+				unsigned int        start,
+				unsigned int        src_len)
+{
+	struct scatter_walk walk;
+	struct nx_sg *nx_sg = nx_dst;
+	unsigned int n, offset = 0, len = src_len;
+	char *dst;
+
+	/* we need to fast forward through @start bytes first */
+	for (;;) {
+		scatterwalk_start(&walk, sg_src);
+
+		if (start < offset + sg_src->length)
+			break;
+
+		offset += sg_src->length;
+		sg_src = scatterwalk_sg_next(sg_src);
+	}
+
+	/* start - offset is the number of bytes to advance in the scatterlist
+	 * element we're currently looking at */
+	scatterwalk_advance(&walk, start - offset);
+
+	while (len && nx_sg) {
+		n = scatterwalk_clamp(&walk, len);
+		if (!n) {
+			scatterwalk_start(&walk, sg_next(walk.sg));
+			n = scatterwalk_clamp(&walk, len);
+		}
+		dst = scatterwalk_map(&walk);
+
+		nx_sg = nx_build_sg_list(nx_sg, dst, n, sglen);
+		len -= n;
+
+		scatterwalk_unmap(dst);
+		scatterwalk_advance(&walk, n);
+		scatterwalk_done(&walk, SCATTERWALK_FROM_SG, len);
+	}
+
+	/* return the moved destination pointer */
+	return nx_sg;
+}
+
+/**
+ * nx_build_sg_lists - walk the input scatterlists and build arrays of NX
+ *                     scatterlists based on them.
+ *
+ * @nx_ctx: NX crypto context for the lists we're building
+ * @desc: the block cipher descriptor for the operation
+ * @dst: destination scatterlist
+ * @src: source scatterlist
+ * @nbytes: length of data described in the scatterlists
+ * @iv: destination for the iv data, if the algorithm requires it
+ *
+ * This is common code shared by all the AES algorithms. It uses the block
+ * cipher walk routines to traverse input and output scatterlists, building
+ * corresponding NX scatterlists
+ */
+int nx_build_sg_lists(struct nx_crypto_ctx  *nx_ctx,
+		      struct blkcipher_desc *desc,
+		      struct scatterlist    *dst,
+		      struct scatterlist    *src,
+		      unsigned int           nbytes,
+		      u8                    *iv)
+{
+	struct nx_sg *nx_insg = nx_ctx->in_sg;
+	struct nx_sg *nx_outsg = nx_ctx->out_sg;
+	struct blkcipher_walk walk;
+	int rc;
+
+	blkcipher_walk_init(&walk, dst, src, nbytes);
+	rc = blkcipher_walk_virt_block(desc, &walk, AES_BLOCK_SIZE);
+	if (rc)
+		goto out;
+
+	if (iv)
+		memcpy(iv, walk.iv, AES_BLOCK_SIZE);
+
+	while (walk.nbytes) {
+		nx_insg = nx_build_sg_list(nx_insg, walk.src.virt.addr,
+					   walk.nbytes, nx_ctx->ap->sglen);
+		nx_outsg = nx_build_sg_list(nx_outsg, walk.dst.virt.addr,
+					    walk.nbytes, nx_ctx->ap->sglen);
+
+		rc = blkcipher_walk_done(desc, &walk, 0);
+		if (rc)
+			break;
+	}
+
+	if (walk.nbytes) {
+		nx_insg = nx_build_sg_list(nx_insg, walk.src.virt.addr,
+					   walk.nbytes, nx_ctx->ap->sglen);
+		nx_outsg = nx_build_sg_list(nx_outsg, walk.dst.virt.addr,
+					    walk.nbytes, nx_ctx->ap->sglen);
+
+		rc = 0;
+	}
+
+	/* these lengths should be negative, which will indicate to phyp that
+	 * the input and output parameters are scatterlists, not linear
+	 * buffers */
+	nx_ctx->op.inlen = (nx_ctx->in_sg - nx_insg) * sizeof(struct nx_sg);
+	nx_ctx->op.outlen = (nx_ctx->out_sg - nx_outsg) * sizeof(struct nx_sg);
+out:
+	return rc;
+}
+
+/**
+ * nx_ctx_init - initialize an nx_ctx's vio_pfo_op struct
+ *
+ * @nx_ctx: the nx context to initialize
+ * @function: the function code for the op
+ */
+void nx_ctx_init(struct nx_crypto_ctx *nx_ctx, unsigned int function)
+{
+	memset(nx_ctx->kmem, 0, nx_ctx->kmem_len);
+	nx_ctx->csbcpb->csb.valid |= NX_CSB_VALID_BIT;
+
+	nx_ctx->op.flags = function;
+	nx_ctx->op.csbcpb = virt_to_abs(nx_ctx->csbcpb);
+	nx_ctx->op.in = virt_to_abs(nx_ctx->in_sg);
+	nx_ctx->op.out = virt_to_abs(nx_ctx->out_sg);
+
+	if (nx_ctx->csbcpb_aead) {
+		nx_ctx->csbcpb_aead->csb.valid |= NX_CSB_VALID_BIT;
+
+		nx_ctx->op_aead.flags = function;
+		nx_ctx->op_aead.csbcpb = virt_to_abs(nx_ctx->csbcpb_aead);
+		nx_ctx->op_aead.in = virt_to_abs(nx_ctx->in_sg);
+		nx_ctx->op_aead.out = virt_to_abs(nx_ctx->out_sg);
+	}
+}
+
+static void nx_of_update_status(struct device   *dev,
+			       struct property *p,
+			       struct nx_of    *props)
+{
+	if (!strncmp(p->value, "okay", p->length)) {
+		props->status = NX_WAITING;
+		props->flags |= NX_OF_FLAG_STATUS_SET;
+	} else {
+		dev_info(dev, "%s: status '%s' is not 'okay'\n", __func__,
+			 (char *)p->value);
+	}
+}
+
+static void nx_of_update_sglen(struct device   *dev,
+			       struct property *p,
+			       struct nx_of    *props)
+{
+	if (p->length != sizeof(props->max_sg_len)) {
+		dev_err(dev, "%s: unexpected format for "
+			"ibm,max-sg-len property\n", __func__);
+		dev_dbg(dev, "%s: ibm,max-sg-len is %d bytes "
+			"long, expected %zd bytes\n", __func__,
+			p->length, sizeof(props->max_sg_len));
+		return;
+	}
+
+	props->max_sg_len = *(u32 *)p->value;
+	props->flags |= NX_OF_FLAG_MAXSGLEN_SET;
+}
+
+static void nx_of_update_msc(struct device   *dev,
+			     struct property *p,
+			     struct nx_of    *props)
+{
+	struct msc_triplet *trip;
+	struct max_sync_cop *msc;
+	unsigned int bytes_so_far, i, lenp;
+
+	msc = (struct max_sync_cop *)p->value;
+	lenp = p->length;
+
+	/* You can't tell if the data read in for this property is sane by its
+	 * size alone. This is because there are sizes embedded in the data
+	 * structure. The best we can do is check lengths as we parse and bail
+	 * as soon as a length error is detected. */
+	bytes_so_far = 0;
+
+	while ((bytes_so_far + sizeof(struct max_sync_cop)) <= lenp) {
+		bytes_so_far += sizeof(struct max_sync_cop);
+
+		trip = msc->trip;
+
+		for (i = 0;
+		     ((bytes_so_far + sizeof(struct msc_triplet)) <= lenp) &&
+		     i < msc->triplets;
+		     i++) {
+			if (msc->fc > NX_MAX_FC || msc->mode > NX_MAX_MODE) {
+				dev_err(dev, "unknown function code/mode "
+					"combo: %d/%d (ignored)\n", msc->fc,
+					msc->mode);
+				goto next_loop;
+			}
+
+			switch (trip->keybitlen) {
+			case 128:
+			case 160:
+				props->ap[msc->fc][msc->mode][0].databytelen =
+					trip->databytelen;
+				props->ap[msc->fc][msc->mode][0].sglen =
+					trip->sglen;
+				break;
+			case 192:
+				props->ap[msc->fc][msc->mode][1].databytelen =
+					trip->databytelen;
+				props->ap[msc->fc][msc->mode][1].sglen =
+					trip->sglen;
+				break;
+			case 256:
+				if (msc->fc == NX_FC_AES) {
+					props->ap[msc->fc][msc->mode][2].
+						databytelen = trip->databytelen;
+					props->ap[msc->fc][msc->mode][2].sglen =
+						trip->sglen;
+				} else if (msc->fc == NX_FC_AES_HMAC ||
+					   msc->fc == NX_FC_SHA) {
+					props->ap[msc->fc][msc->mode][1].
+						databytelen = trip->databytelen;
+					props->ap[msc->fc][msc->mode][1].sglen =
+						trip->sglen;
+				} else {
+					dev_warn(dev, "unknown function "
+						"code/key bit len combo"
+						": (%u/256)\n", msc->fc);
+				}
+				break;
+			case 512:
+				props->ap[msc->fc][msc->mode][2].databytelen =
+					trip->databytelen;
+				props->ap[msc->fc][msc->mode][2].sglen =
+					trip->sglen;
+				break;
+			default:
+				dev_warn(dev, "unknown function code/key bit "
+					 "len combo: (%u/%u)\n", msc->fc,
+					 trip->keybitlen);
+				break;
+			}
+next_loop:
+			bytes_so_far += sizeof(struct msc_triplet);
+			trip++;
+		}
+
+		msc = (struct max_sync_cop *)trip;
+	}
+
+	props->flags |= NX_OF_FLAG_MAXSYNCCOP_SET;
+}
+
+/**
+ * nx_of_init - read openFirmware values from the device tree
+ *
+ * @dev: device handle
+ * @props: pointer to struct to hold the properties values
+ *
+ * Called once at driver probe time, this function will read out the
+ * openFirmware properties we use at runtime. If all the OF properties are
+ * acceptable, when we exit this function props->flags will indicate that
+ * we're ready to register our crypto algorithms.
+ */
+static void nx_of_init(struct device *dev, struct nx_of *props)
+{
+	struct device_node *base_node = dev->of_node;
+	struct property *p;
+
+	p = of_find_property(base_node, "status", NULL);
+	if (!p)
+		dev_info(dev, "%s: property 'status' not found\n", __func__);
+	else
+		nx_of_update_status(dev, p, props);
+
+	p = of_find_property(base_node, "ibm,max-sg-len", NULL);
+	if (!p)
+		dev_info(dev, "%s: property 'ibm,max-sg-len' not found\n",
+			 __func__);
+	else
+		nx_of_update_sglen(dev, p, props);
+
+	p = of_find_property(base_node, "ibm,max-sync-cop", NULL);
+	if (!p)
+		dev_info(dev, "%s: property 'ibm,max-sync-cop' not found\n",
+			 __func__);
+	else
+		nx_of_update_msc(dev, p, props);
+}
+
+/**
+ * nx_register_algs - register algorithms with the crypto API
+ *
+ * Called from nx_probe()
+ *
+ * If all OF properties are in an acceptable state, the driver flags will
+ * indicate that we're ready and we'll create our debugfs files and register
+ * out crypto algorithms.
+ */
+static int nx_register_algs(void)
+{
+	int rc = -1;
+
+	if (nx_driver.of.flags != NX_OF_FLAG_MASK_READY)
+		goto out;
+
+	memset(&nx_driver.stats, 0, sizeof(struct nx_stats));
+
+	rc = NX_DEBUGFS_INIT(&nx_driver);
+	if (rc)
+		goto out;
+
+	rc = crypto_register_alg(&nx_ecb_aes_alg);
+	if (rc)
+		goto out;
+
+	rc = crypto_register_alg(&nx_cbc_aes_alg);
+	if (rc)
+		goto out_unreg_ecb;
+
+	rc = crypto_register_alg(&nx_ctr_aes_alg);
+	if (rc)
+		goto out_unreg_cbc;
+
+	rc = crypto_register_alg(&nx_ctr3686_aes_alg);
+	if (rc)
+		goto out_unreg_ctr;
+
+	rc = crypto_register_alg(&nx_gcm_aes_alg);
+	if (rc)
+		goto out_unreg_ctr3686;
+
+	rc = crypto_register_alg(&nx_gcm4106_aes_alg);
+	if (rc)
+		goto out_unreg_gcm;
+
+	rc = crypto_register_alg(&nx_ccm_aes_alg);
+	if (rc)
+		goto out_unreg_gcm4106;
+
+	rc = crypto_register_alg(&nx_ccm4309_aes_alg);
+	if (rc)
+		goto out_unreg_ccm;
+
+	rc = crypto_register_shash(&nx_shash_sha256_alg);
+	if (rc)
+		goto out_unreg_ccm4309;
+
+	rc = crypto_register_shash(&nx_shash_sha512_alg);
+	if (rc)
+		goto out_unreg_s256;
+
+	rc = crypto_register_shash(&nx_shash_aes_xcbc_alg);
+	if (rc)
+		goto out_unreg_s512;
+
+	nx_driver.of.status = NX_OKAY;
+
+	goto out;
+
+out_unreg_s512:
+	crypto_unregister_shash(&nx_shash_sha512_alg);
+out_unreg_s256:
+	crypto_unregister_shash(&nx_shash_sha256_alg);
+out_unreg_ccm4309:
+	crypto_unregister_alg(&nx_ccm4309_aes_alg);
+out_unreg_ccm:
+	crypto_unregister_alg(&nx_ccm_aes_alg);
+out_unreg_gcm4106:
+	crypto_unregister_alg(&nx_gcm4106_aes_alg);
+out_unreg_gcm:
+	crypto_unregister_alg(&nx_gcm_aes_alg);
+out_unreg_ctr3686:
+	crypto_unregister_alg(&nx_ctr3686_aes_alg);
+out_unreg_ctr:
+	crypto_unregister_alg(&nx_ctr_aes_alg);
+out_unreg_cbc:
+	crypto_unregister_alg(&nx_cbc_aes_alg);
+out_unreg_ecb:
+	crypto_unregister_alg(&nx_ecb_aes_alg);
+out:
+	return rc;
+}
+
+/**
+ * nx_crypto_ctx_init - create and initialize a crypto api context
+ *
+ * @nx_ctx: the crypto api context
+ * @fc: function code for the context
+ * @mode: the function code specific mode for this context
+ */
+static int nx_crypto_ctx_init(struct nx_crypto_ctx *nx_ctx, u32 fc, u32 mode)
+{
+	if (nx_driver.of.status != NX_OKAY) {
+		pr_err("Attempt to initialize NX crypto context while device "
+		       "is not available!\n");
+		return -ENODEV;
+	}
+
+	/* we need an extra page for csbcpb_aead for these modes */
+	if (mode == NX_MODE_AES_GCM || mode == NX_MODE_AES_CCM)
+		nx_ctx->kmem_len = (4 * NX_PAGE_SIZE) +
+				   sizeof(struct nx_csbcpb);
+	else
+		nx_ctx->kmem_len = (3 * NX_PAGE_SIZE) +
+				   sizeof(struct nx_csbcpb);
+
+	nx_ctx->kmem = kmalloc(nx_ctx->kmem_len, GFP_KERNEL);
+	if (!nx_ctx->kmem)
+		return -ENOMEM;
+
+	/* the csbcpb and scatterlists must be 4K aligned pages */
+	nx_ctx->csbcpb = (struct nx_csbcpb *)(round_up((u64)nx_ctx->kmem,
+						       (u64)NX_PAGE_SIZE));
+	nx_ctx->in_sg = (struct nx_sg *)((u8 *)nx_ctx->csbcpb + NX_PAGE_SIZE);
+	nx_ctx->out_sg = (struct nx_sg *)((u8 *)nx_ctx->in_sg + NX_PAGE_SIZE);
+
+	if (mode == NX_MODE_AES_GCM || mode == NX_MODE_AES_CCM)
+		nx_ctx->csbcpb_aead =
+			(struct nx_csbcpb *)((u8 *)nx_ctx->out_sg +
+					     NX_PAGE_SIZE);
+
+	/* give each context a pointer to global stats and their OF
+	 * properties */
+	nx_ctx->stats = &nx_driver.stats;
+	memcpy(nx_ctx->props, nx_driver.of.ap[fc][mode],
+	       sizeof(struct alg_props) * 3);
+
+	return 0;
+}
+
+/* entry points from the crypto tfm initializers */
+int nx_crypto_ctx_aes_ccm_init(struct crypto_tfm *tfm)
+{
+	return nx_crypto_ctx_init(crypto_tfm_ctx(tfm), NX_FC_AES,
+				  NX_MODE_AES_CCM);
+}
+
+int nx_crypto_ctx_aes_gcm_init(struct crypto_tfm *tfm)
+{
+	return nx_crypto_ctx_init(crypto_tfm_ctx(tfm), NX_FC_AES,
+				  NX_MODE_AES_GCM);
+}
+
+int nx_crypto_ctx_aes_ctr_init(struct crypto_tfm *tfm)
+{
+	return nx_crypto_ctx_init(crypto_tfm_ctx(tfm), NX_FC_AES,
+				  NX_MODE_AES_CTR);
+}
+
+int nx_crypto_ctx_aes_cbc_init(struct crypto_tfm *tfm)
+{
+	return nx_crypto_ctx_init(crypto_tfm_ctx(tfm), NX_FC_AES,
+				  NX_MODE_AES_CBC);
+}
+
+int nx_crypto_ctx_aes_ecb_init(struct crypto_tfm *tfm)
+{
+	return nx_crypto_ctx_init(crypto_tfm_ctx(tfm), NX_FC_AES,
+				  NX_MODE_AES_ECB);
+}
+
+int nx_crypto_ctx_sha_init(struct crypto_tfm *tfm)
+{
+	return nx_crypto_ctx_init(crypto_tfm_ctx(tfm), NX_FC_SHA, NX_MODE_SHA);
+}
+
+int nx_crypto_ctx_aes_xcbc_init(struct crypto_tfm *tfm)
+{
+	return nx_crypto_ctx_init(crypto_tfm_ctx(tfm), NX_FC_AES,
+				  NX_MODE_AES_XCBC_MAC);
+}
+
+/**
+ * nx_crypto_ctx_exit - destroy a crypto api context
+ *
+ * @tfm: the crypto transform pointer for the context
+ *
+ * As crypto API contexts are destroyed, this exit hook is called to free the
+ * memory associated with it.
+ */
+void nx_crypto_ctx_exit(struct crypto_tfm *tfm)
+{
+	struct nx_crypto_ctx *nx_ctx = crypto_tfm_ctx(tfm);
+
+	kzfree(nx_ctx->kmem);
+	nx_ctx->csbcpb = NULL;
+	nx_ctx->csbcpb_aead = NULL;
+	nx_ctx->in_sg = NULL;
+	nx_ctx->out_sg = NULL;
+}
+
+static int __devinit nx_probe(struct vio_dev *viodev,
+			      const struct vio_device_id *id)
+{
+	dev_dbg(&viodev->dev, "driver probed: %s resource id: 0x%x\n",
+		viodev->name, viodev->resource_id);
+
+	if (nx_driver.viodev) {
+		dev_err(&viodev->dev, "%s: Attempt to register more than one "
+			"instance of the hardware\n", __func__);
+		return -EINVAL;
+	}
+
+	nx_driver.viodev = viodev;
+
+	nx_of_init(&viodev->dev, &nx_driver.of);
+
+	return nx_register_algs();
+}
+
+static int __devexit nx_remove(struct vio_dev *viodev)
+{
+	dev_dbg(&viodev->dev, "entering nx_remove for UA 0x%x\n",
+		viodev->unit_address);
+
+	if (nx_driver.of.status == NX_OKAY) {
+		NX_DEBUGFS_FINI(&nx_driver);
+
+		crypto_unregister_alg(&nx_ccm_aes_alg);
+		crypto_unregister_alg(&nx_ccm4309_aes_alg);
+		crypto_unregister_alg(&nx_gcm_aes_alg);
+		crypto_unregister_alg(&nx_gcm4106_aes_alg);
+		crypto_unregister_alg(&nx_ctr_aes_alg);
+		crypto_unregister_alg(&nx_ctr3686_aes_alg);
+		crypto_unregister_alg(&nx_cbc_aes_alg);
+		crypto_unregister_alg(&nx_ecb_aes_alg);
+		crypto_unregister_shash(&nx_shash_sha256_alg);
+		crypto_unregister_shash(&nx_shash_sha512_alg);
+		crypto_unregister_shash(&nx_shash_aes_xcbc_alg);
+	}
+
+	return 0;
+}
+
+
+/* module wide initialization/cleanup */
+static int __init nx_init(void)
+{
+	return vio_register_driver(&nx_driver.viodriver);
+}
+
+static void __exit nx_fini(void)
+{
+	vio_unregister_driver(&nx_driver.viodriver);
+}
+
+static struct vio_device_id nx_crypto_driver_ids[] __devinitdata = {
+	{ "ibm,sym-encryption-v1", "ibm,sym-encryption" },
+	{ "", "" }
+};
+MODULE_DEVICE_TABLE(vio, nx_crypto_driver_ids);
+
+/* driver state structure */
+struct nx_crypto_driver nx_driver = {
+	.viodriver = {
+		.id_table = nx_crypto_driver_ids,
+		.probe = nx_probe,
+		.remove = nx_remove,
+		.name  = NX_NAME,
+	},
+};
+
+module_init(nx_init);
+module_exit(nx_fini);
+
+MODULE_AUTHOR("Kent Yoder <yoder1@us.ibm.com>");
+MODULE_DESCRIPTION(NX_STRING);
+MODULE_LICENSE("GPL");
+MODULE_VERSION(NX_VERSION);
diff --git a/drivers/crypto/nx/nx.h b/drivers/crypto/nx/nx.h
new file mode 100644
index 00000000000..3232b182dd2
--- /dev/null
+++ b/drivers/crypto/nx/nx.h
@@ -0,0 +1,193 @@
+
+#ifndef __NX_H__
+#define __NX_H__
+
+#define NX_NAME		"nx-crypto"
+#define NX_STRING	"IBM Power7+ Nest Accelerator Crypto Driver"
+#define NX_VERSION	"1.0"
+
+static const char nx_driver_string[] = NX_STRING;
+static const char nx_driver_version[] = NX_VERSION;
+
+/* a scatterlist in the format PHYP is expecting */
+struct nx_sg {
+	u64 addr;
+	u32 rsvd;
+	u32 len;
+} __attribute((packed));
+
+#define NX_PAGE_SIZE		(4096)
+#define NX_MAX_SG_ENTRIES	(NX_PAGE_SIZE/(sizeof(struct nx_sg)))
+
+enum nx_status {
+	NX_DISABLED,
+	NX_WAITING,
+	NX_OKAY
+};
+
+/* msc_triplet and max_sync_cop are used only to assist in parsing the
+ * openFirmware property */
+struct msc_triplet {
+	u32 keybitlen;
+	u32 databytelen;
+	u32 sglen;
+} __packed;
+
+struct max_sync_cop {
+	u32 fc;
+	u32 mode;
+	u32 triplets;
+	struct msc_triplet trip[0];
+} __packed;
+
+struct alg_props {
+	u32 databytelen;
+	u32 sglen;
+};
+
+#define NX_OF_FLAG_MAXSGLEN_SET		(1)
+#define NX_OF_FLAG_STATUS_SET		(2)
+#define NX_OF_FLAG_MAXSYNCCOP_SET	(4)
+#define NX_OF_FLAG_MASK_READY		(NX_OF_FLAG_MAXSGLEN_SET | \
+					 NX_OF_FLAG_STATUS_SET |   \
+					 NX_OF_FLAG_MAXSYNCCOP_SET)
+struct nx_of {
+	u32 flags;
+	u32 max_sg_len;
+	enum nx_status status;
+	struct alg_props ap[NX_MAX_FC][NX_MAX_MODE][3];
+};
+
+struct nx_stats {
+	atomic_t aes_ops;
+	atomic64_t aes_bytes;
+	atomic_t sha256_ops;
+	atomic64_t sha256_bytes;
+	atomic_t sha512_ops;
+	atomic64_t sha512_bytes;
+
+	atomic_t sync_ops;
+
+	atomic_t errors;
+	atomic_t last_error;
+	atomic_t last_error_pid;
+};
+
+struct nx_debugfs {
+	struct dentry *dfs_root;
+	struct dentry *dfs_aes_ops, *dfs_aes_bytes;
+	struct dentry *dfs_sha256_ops, *dfs_sha256_bytes;
+	struct dentry *dfs_sha512_ops, *dfs_sha512_bytes;
+	struct dentry *dfs_errors, *dfs_last_error, *dfs_last_error_pid;
+};
+
+struct nx_crypto_driver {
+	struct nx_stats    stats;
+	struct nx_of       of;
+	struct vio_dev    *viodev;
+	struct vio_driver  viodriver;
+	struct nx_debugfs  dfs;
+};
+
+#define NX_GCM4106_NONCE_LEN		(4)
+#define NX_GCM_CTR_OFFSET		(12)
+struct nx_gcm_priv {
+	u8 iv[16];
+	u8 iauth_tag[16];
+	u8 nonce[NX_GCM4106_NONCE_LEN];
+};
+
+#define NX_CCM_AES_KEY_LEN		(16)
+#define NX_CCM4309_AES_KEY_LEN		(19)
+#define NX_CCM4309_NONCE_LEN		(3)
+struct nx_ccm_priv {
+	u8 iv[16];
+	u8 b0[16];
+	u8 iauth_tag[16];
+	u8 oauth_tag[16];
+	u8 nonce[NX_CCM4309_NONCE_LEN];
+};
+
+struct nx_xcbc_priv {
+	u8 key[16];
+};
+
+struct nx_ctr_priv {
+	u8 iv[16];
+};
+
+struct nx_crypto_ctx {
+	void *kmem;		  /* unaligned, kmalloc'd buffer */
+	size_t kmem_len;	  /* length of kmem */
+	struct nx_csbcpb *csbcpb; /* aligned page given to phyp @ hcall time */
+	struct vio_pfo_op op;     /* operation struct with hcall parameters */
+	struct nx_csbcpb *csbcpb_aead; /* secondary csbcpb used by AEAD algs */
+	struct vio_pfo_op op_aead;/* operation struct for csbcpb_aead */
+
+	struct nx_sg *in_sg;      /* aligned pointer into kmem to an sg list */
+	struct nx_sg *out_sg;     /* aligned pointer into kmem to an sg list */
+
+	struct alg_props *ap;	  /* pointer into props based on our key size */
+	struct alg_props props[3];/* openFirmware properties for requests */
+	struct nx_stats *stats;   /* pointer into an nx_crypto_driver for stats
+				     reporting */
+
+	union {
+		struct nx_gcm_priv gcm;
+		struct nx_ccm_priv ccm;
+		struct nx_xcbc_priv xcbc;
+		struct nx_ctr_priv ctr;
+	} priv;
+};
+
+/* prototypes */
+int nx_crypto_ctx_aes_ccm_init(struct crypto_tfm *tfm);
+int nx_crypto_ctx_aes_gcm_init(struct crypto_tfm *tfm);
+int nx_crypto_ctx_aes_xcbc_init(struct crypto_tfm *tfm);
+int nx_crypto_ctx_aes_ctr_init(struct crypto_tfm *tfm);
+int nx_crypto_ctx_aes_cbc_init(struct crypto_tfm *tfm);
+int nx_crypto_ctx_aes_ecb_init(struct crypto_tfm *tfm);
+int nx_crypto_ctx_sha_init(struct crypto_tfm *tfm);
+void nx_crypto_ctx_exit(struct crypto_tfm *tfm);
+void nx_ctx_init(struct nx_crypto_ctx *nx_ctx, unsigned int function);
+int nx_hcall_sync(struct nx_crypto_ctx *ctx, struct vio_pfo_op *op,
+		  u32 may_sleep);
+struct nx_sg *nx_build_sg_list(struct nx_sg *, u8 *, unsigned int, u32);
+int nx_build_sg_lists(struct nx_crypto_ctx *, struct blkcipher_desc *,
+		      struct scatterlist *, struct scatterlist *, unsigned int,
+		      u8 *);
+struct nx_sg *nx_walk_and_build(struct nx_sg *, unsigned int,
+				struct scatterlist *, unsigned int,
+				unsigned int);
+
+#ifdef CONFIG_DEBUG_FS
+#define NX_DEBUGFS_INIT(drv)	nx_debugfs_init(drv)
+#define NX_DEBUGFS_FINI(drv)	nx_debugfs_fini(drv)
+
+int nx_debugfs_init(struct nx_crypto_driver *);
+void nx_debugfs_fini(struct nx_crypto_driver *);
+#else
+#define NX_DEBUGFS_INIT(drv)	(0)
+#define NX_DEBUGFS_FINI(drv)	(0)
+#endif
+
+#define NX_PAGE_NUM(x)		((u64)(x) & 0xfffffffffffff000ULL)
+
+extern struct crypto_alg nx_cbc_aes_alg;
+extern struct crypto_alg nx_ecb_aes_alg;
+extern struct crypto_alg nx_gcm_aes_alg;
+extern struct crypto_alg nx_gcm4106_aes_alg;
+extern struct crypto_alg nx_ctr_aes_alg;
+extern struct crypto_alg nx_ctr3686_aes_alg;
+extern struct crypto_alg nx_ccm_aes_alg;
+extern struct crypto_alg nx_ccm4309_aes_alg;
+extern struct shash_alg nx_shash_aes_xcbc_alg;
+extern struct shash_alg nx_shash_sha512_alg;
+extern struct shash_alg nx_shash_sha256_alg;
+
+extern struct nx_crypto_driver nx_driver;
+
+#define SCATTERWALK_TO_SG	1
+#define SCATTERWALK_FROM_SG	0
+
+#endif
diff --git a/drivers/crypto/nx/nx_csbcpb.h b/drivers/crypto/nx/nx_csbcpb.h
new file mode 100644
index 00000000000..a304f956d6f
--- /dev/null
+++ b/drivers/crypto/nx/nx_csbcpb.h
@@ -0,0 +1,205 @@
+
+#ifndef __NX_CSBCPB_H__
+#define __NX_CSBCPB_H__
+
+struct cop_symcpb_aes_ecb {
+	u8 key[32];
+	u8 __rsvd[80];
+} __packed;
+
+struct cop_symcpb_aes_cbc {
+	u8 iv[16];
+	u8 key[32];
+	u8 cv[16];
+	u32 spbc;
+	u8 __rsvd[44];
+} __packed;
+
+struct cop_symcpb_aes_gca {
+	u8 in_pat[16];
+	u8 key[32];
+	u8 out_pat[16];
+	u32 spbc;
+	u8 __rsvd[44];
+} __packed;
+
+struct cop_symcpb_aes_gcm {
+	u8 in_pat_or_aad[16];
+	u8 iv_or_cnt[16];
+	u64 bit_length_aad;
+	u64 bit_length_data;
+	u8 in_s0[16];
+	u8 key[32];
+	u8 __rsvd1[16];
+	u8 out_pat_or_mac[16];
+	u8 out_s0[16];
+	u8 out_cnt[16];
+	u32 spbc;
+	u8 __rsvd2[12];
+} __packed;
+
+struct cop_symcpb_aes_ctr {
+	u8 iv[16];
+	u8 key[32];
+	u8 cv[16];
+	u32 spbc;
+	u8 __rsvd2[44];
+} __packed;
+
+struct cop_symcpb_aes_cca {
+	u8 b0[16];
+	u8 b1[16];
+	u8 key[16];
+	u8 out_pat_or_b0[16];
+	u32 spbc;
+	u8 __rsvd[44];
+} __packed;
+
+struct cop_symcpb_aes_ccm {
+	u8 in_pat_or_b0[16];
+	u8 iv_or_ctr[16];
+	u8 in_s0[16];
+	u8 key[16];
+	u8 __rsvd1[48];
+	u8 out_pat_or_mac[16];
+	u8 out_s0[16];
+	u8 out_ctr[16];
+	u32 spbc;
+	u8 __rsvd2[12];
+} __packed;
+
+struct cop_symcpb_aes_xcbc {
+	u8 cv[16];
+	u8 key[16];
+	u8 __rsvd1[16];
+	u8 out_cv_mac[16];
+	u32 spbc;
+	u8 __rsvd2[44];
+} __packed;
+
+struct cop_symcpb_sha256 {
+	u64 message_bit_length;
+	u64 __rsvd1;
+	u8 input_partial_digest[32];
+	u8 message_digest[32];
+	u32 spbc;
+	u8 __rsvd2[44];
+} __packed;
+
+struct cop_symcpb_sha512 {
+	u64 message_bit_length_hi;
+	u64 message_bit_length_lo;
+	u8 input_partial_digest[64];
+	u8 __rsvd1[32];
+	u8 message_digest[64];
+	u32 spbc;
+	u8 __rsvd2[76];
+} __packed;
+
+#define NX_FDM_INTERMEDIATE		0x01
+#define NX_FDM_CONTINUATION		0x02
+#define NX_FDM_ENDE_ENCRYPT		0x80
+
+#define NX_CPB_FDM(c)			((c)->cpb.hdr.fdm)
+#define NX_CPB_KS_DS(c)			((c)->cpb.hdr.ks_ds)
+
+#define NX_CPB_KEY_SIZE(c)		(NX_CPB_KS_DS(c) >> 4)
+#define NX_CPB_SET_KEY_SIZE(c, x)	NX_CPB_KS_DS(c) |= ((x) << 4)
+#define NX_CPB_SET_DIGEST_SIZE(c, x)	NX_CPB_KS_DS(c) |= (x)
+
+struct cop_symcpb_header {
+	u8 mode;
+	u8 fdm;
+	u8 ks_ds;
+	u8 pad_byte;
+	u8 __rsvd[12];
+} __packed;
+
+struct cop_parameter_block {
+	struct cop_symcpb_header hdr;
+	union {
+		struct cop_symcpb_aes_ecb  aes_ecb;
+		struct cop_symcpb_aes_cbc  aes_cbc;
+		struct cop_symcpb_aes_gca  aes_gca;
+		struct cop_symcpb_aes_gcm  aes_gcm;
+		struct cop_symcpb_aes_cca  aes_cca;
+		struct cop_symcpb_aes_ccm  aes_ccm;
+		struct cop_symcpb_aes_ctr  aes_ctr;
+		struct cop_symcpb_aes_xcbc aes_xcbc;
+		struct cop_symcpb_sha256   sha256;
+		struct cop_symcpb_sha512   sha512;
+	};
+} __packed;
+
+#define NX_CSB_VALID_BIT	0x80
+
+/* co-processor status block */
+struct cop_status_block {
+	u8 valid;
+	u8 crb_seq_number;
+	u8 completion_code;
+	u8 completion_extension;
+	u32 processed_byte_count;
+	u64 address;
+} __packed;
+
+/* Nest accelerator workbook section 4.4 */
+struct nx_csbcpb {
+	unsigned char __rsvd[112];
+	struct cop_status_block csb;
+	struct cop_parameter_block cpb;
+} __packed;
+
+/* nx_csbcpb related definitions */
+#define NX_MODE_AES_ECB			0
+#define NX_MODE_AES_CBC			1
+#define NX_MODE_AES_GMAC		2
+#define NX_MODE_AES_GCA			3
+#define NX_MODE_AES_GCM			4
+#define NX_MODE_AES_CCA			5
+#define NX_MODE_AES_CCM			6
+#define NX_MODE_AES_CTR			7
+#define NX_MODE_AES_XCBC_MAC		20
+#define NX_MODE_SHA			0
+#define NX_MODE_SHA_HMAC		1
+#define NX_MODE_AES_CBC_HMAC_ETA	8
+#define NX_MODE_AES_CBC_HMAC_ATE	9
+#define NX_MODE_AES_CBC_HMAC_EAA	10
+#define NX_MODE_AES_CTR_HMAC_ETA	12
+#define NX_MODE_AES_CTR_HMAC_ATE	13
+#define NX_MODE_AES_CTR_HMAC_EAA	14
+
+#define NX_FDM_CI_FULL		0
+#define NX_FDM_CI_FIRST		1
+#define NX_FDM_CI_LAST		2
+#define NX_FDM_CI_MIDDLE	3
+
+#define NX_FDM_PR_NONE		0
+#define NX_FDM_PR_PAD		1
+
+#define NX_KS_AES_128		1
+#define NX_KS_AES_192		2
+#define NX_KS_AES_256		3
+
+#define NX_DS_SHA256		2
+#define NX_DS_SHA512		3
+
+#define NX_FC_AES		0
+#define NX_FC_SHA		2
+#define NX_FC_AES_HMAC		6
+
+#define NX_MAX_FC		(NX_FC_AES_HMAC + 1)
+#define NX_MAX_MODE		(NX_MODE_AES_XCBC_MAC + 1)
+
+#define HCOP_FC_AES          NX_FC_AES
+#define HCOP_FC_SHA          NX_FC_SHA
+#define HCOP_FC_AES_HMAC     NX_FC_AES_HMAC
+
+/* indices into the array of algorithm properties */
+#define NX_PROPS_AES_128		0
+#define NX_PROPS_AES_192		1
+#define NX_PROPS_AES_256		2
+#define NX_PROPS_SHA256			1
+#define NX_PROPS_SHA512			2
+
+#endif
diff --git a/drivers/crypto/nx/nx_debugfs.c b/drivers/crypto/nx/nx_debugfs.c
new file mode 100644
index 00000000000..7ab2e8dcd9b
--- /dev/null
+++ b/drivers/crypto/nx/nx_debugfs.c
@@ -0,0 +1,103 @@
+/**
+ * debugfs routines supporting the Power 7+ Nest Accelerators driver
+ *
+ * Copyright (C) 2011-2012 International Business Machines 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; version 2 only.
+ *
+ * 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.
+ *
+ * Author: Kent Yoder <yoder1@us.ibm.com>
+ */
+
+#include <linux/device.h>
+#include <linux/kobject.h>
+#include <linux/string.h>
+#include <linux/debugfs.h>
+#include <linux/module.h>
+#include <linux/init.h>
+#include <linux/crypto.h>
+#include <crypto/hash.h>
+#include <asm/vio.h>
+
+#include "nx_csbcpb.h"
+#include "nx.h"
+
+#ifdef CONFIG_DEBUG_FS
+
+/*
+ * debugfs
+ *
+ * For documentation on these attributes, please see:
+ *
+ * Documentation/ABI/testing/debugfs-pfo-nx-crypto
+ */
+
+int nx_debugfs_init(struct nx_crypto_driver *drv)
+{
+	struct nx_debugfs *dfs = &drv->dfs;
+
+	dfs->dfs_root = debugfs_create_dir(NX_NAME, NULL);
+
+	dfs->dfs_aes_ops =
+		debugfs_create_u32("aes_ops",
+				   S_IRUSR | S_IRGRP | S_IROTH,
+				   dfs->dfs_root, (u32 *)&drv->stats.aes_ops);
+	dfs->dfs_sha256_ops =
+		debugfs_create_u32("sha256_ops",
+				   S_IRUSR | S_IRGRP | S_IROTH,
+				   dfs->dfs_root,
+				   (u32 *)&drv->stats.sha256_ops);
+	dfs->dfs_sha512_ops =
+		debugfs_create_u32("sha512_ops",
+				   S_IRUSR | S_IRGRP | S_IROTH,
+				   dfs->dfs_root,
+				   (u32 *)&drv->stats.sha512_ops);
+	dfs->dfs_aes_bytes =
+		debugfs_create_u64("aes_bytes",
+				   S_IRUSR | S_IRGRP | S_IROTH,
+				   dfs->dfs_root,
+				   (u64 *)&drv->stats.aes_bytes);
+	dfs->dfs_sha256_bytes =
+		debugfs_create_u64("sha256_bytes",
+				   S_IRUSR | S_IRGRP | S_IROTH,
+				   dfs->dfs_root,
+				   (u64 *)&drv->stats.sha256_bytes);
+	dfs->dfs_sha512_bytes =
+		debugfs_create_u64("sha512_bytes",
+				   S_IRUSR | S_IRGRP | S_IROTH,
+				   dfs->dfs_root,
+				   (u64 *)&drv->stats.sha512_bytes);
+	dfs->dfs_errors =
+		debugfs_create_u32("errors",
+				   S_IRUSR | S_IRGRP | S_IROTH,
+				   dfs->dfs_root, (u32 *)&drv->stats.errors);
+	dfs->dfs_last_error =
+		debugfs_create_u32("last_error",
+				   S_IRUSR | S_IRGRP | S_IROTH,
+				   dfs->dfs_root,
+				   (u32 *)&drv->stats.last_error);
+	dfs->dfs_last_error_pid =
+		debugfs_create_u32("last_error_pid",
+				   S_IRUSR | S_IRGRP | S_IROTH,
+				   dfs->dfs_root,
+				   (u32 *)&drv->stats.last_error_pid);
+	return 0;
+}
+
+void
+nx_debugfs_fini(struct nx_crypto_driver *drv)
+{
+	debugfs_remove_recursive(drv->dfs.dfs_root);
+}
+
+#endif
diff --git a/drivers/crypto/ux500/Kconfig b/drivers/crypto/ux500/Kconfig
new file mode 100644
index 00000000000..b35e5c4b025
--- /dev/null
+++ b/drivers/crypto/ux500/Kconfig
@@ -0,0 +1,30 @@
+#
+# Copyright (C) ST-Ericsson SA 2010
+# Author: Shujuan Chen (shujuan.chen@stericsson.com)
+# License terms: GNU General Public License (GPL) version 2
+#
+
+config CRYPTO_DEV_UX500_CRYP
+	tristate "UX500 crypto driver for CRYP block"
+	depends on CRYPTO_DEV_UX500
+	select CRYPTO_DES
+	help
+        This selects the crypto driver for the UX500_CRYP hardware. It supports
+        AES-ECB, CBC and CTR with keys sizes of 128, 192 and 256 bit sizes.
+
+config CRYPTO_DEV_UX500_HASH
+        tristate "UX500 crypto driver for HASH block"
+        depends on CRYPTO_DEV_UX500
+        select CRYPTO_HASH
+        select CRYPTO_HMAC
+        help
+          This selects the hash driver for the UX500_HASH hardware.
+          Depends on UX500/STM DMA if running in DMA mode.
+
+config CRYPTO_DEV_UX500_DEBUG
+	bool "Activate ux500 platform debug-mode for crypto and hash block"
+	depends on CRYPTO_DEV_UX500_CRYP || CRYPTO_DEV_UX500_HASH
+	default n
+	help
+	  Say Y if you want to add debug prints to ux500_hash and
+	  ux500_cryp devices.
diff --git a/drivers/crypto/ux500/Makefile b/drivers/crypto/ux500/Makefile
new file mode 100644
index 00000000000..b9a365bade8
--- /dev/null
+++ b/drivers/crypto/ux500/Makefile
@@ -0,0 +1,8 @@
+#
+# Copyright (C) ST-Ericsson SA 2010
+# Author: Shujuan Chen (shujuan.chen@stericsson.com)
+# License terms: GNU General Public License (GPL) version 2
+#
+
+obj-$(CONFIG_CRYPTO_DEV_UX500_HASH) += hash/
+obj-$(CONFIG_CRYPTO_DEV_UX500_CRYP) += cryp/
diff --git a/drivers/crypto/ux500/cryp/Makefile b/drivers/crypto/ux500/cryp/Makefile
new file mode 100644
index 00000000000..e5d362a6f68
--- /dev/null
+++ b/drivers/crypto/ux500/cryp/Makefile
@@ -0,0 +1,13 @@
+#/*
+# * Copyright (C) ST-Ericsson SA 2010
+# * Author: shujuan.chen@stericsson.com for ST-Ericsson.
+# * License terms: GNU General Public License (GPL) version 2  */
+
+ifdef CONFIG_CRYPTO_DEV_UX500_DEBUG
+CFLAGS_cryp_core.o := -DDEBUG -O0
+CFLAGS_cryp.o := -DDEBUG -O0
+CFLAGS_cryp_irq.o := -DDEBUG -O0
+endif
+
+obj-$(CONFIG_CRYPTO_DEV_UX500_CRYP) += ux500_cryp.o
+ux500_cryp-objs :=  cryp.o cryp_irq.o cryp_core.o
diff --git a/drivers/crypto/ux500/cryp/cryp.c b/drivers/crypto/ux500/cryp/cryp.c
new file mode 100644
index 00000000000..e208ceaf81c
--- /dev/null
+++ b/drivers/crypto/ux500/cryp/cryp.c
@@ -0,0 +1,389 @@
+/**
+ * Copyright (C) ST-Ericsson SA 2010
+ * Author: Shujuan Chen <shujuan.chen@stericsson.com> for ST-Ericsson.
+ * Author: Jonas Linde <jonas.linde@stericsson.com> for ST-Ericsson.
+ * Author: Niklas Hernaeus <niklas.hernaeus@stericsson.com> for ST-Ericsson.
+ * Author: Joakim Bech <joakim.xx.bech@stericsson.com> for ST-Ericsson.
+ * Author: Berne Hebark <berne.herbark@stericsson.com> for ST-Ericsson.
+ * License terms: GNU General Public License (GPL) version 2
+ */
+
+#include <linux/errno.h>
+#include <linux/kernel.h>
+#include <linux/types.h>
+
+#include <mach/hardware.h>
+
+#include "cryp_p.h"
+#include "cryp.h"
+
+/**
+ * cryp_wait_until_done - wait until the device logic is not busy
+ */
+void cryp_wait_until_done(struct cryp_device_data *device_data)
+{
+	while (cryp_is_logic_busy(device_data))
+		cpu_relax();
+}
+
+/**
+ * cryp_check - This routine checks Peripheral and PCell Id
+ * @device_data: Pointer to the device data struct for base address.
+ */
+int cryp_check(struct cryp_device_data *device_data)
+{
+	int peripheralid2 = 0;
+
+	if (NULL == device_data)
+		return -EINVAL;
+
+	peripheralid2 = readl_relaxed(&device_data->base->periphId2);
+
+	if (peripheralid2 != CRYP_PERIPHERAL_ID2_DB8500)
+		return -EPERM;
+
+	/* Check Peripheral and Pcell Id Register for CRYP */
+	if ((CRYP_PERIPHERAL_ID0 ==
+		readl_relaxed(&device_data->base->periphId0))
+	    && (CRYP_PERIPHERAL_ID1 ==
+		    readl_relaxed(&device_data->base->periphId1))
+	    && (CRYP_PERIPHERAL_ID3 ==
+		    readl_relaxed(&device_data->base->periphId3))
+	    && (CRYP_PCELL_ID0 ==
+		    readl_relaxed(&device_data->base->pcellId0))
+	    && (CRYP_PCELL_ID1 ==
+		    readl_relaxed(&device_data->base->pcellId1))
+	    && (CRYP_PCELL_ID2 ==
+		    readl_relaxed(&device_data->base->pcellId2))
+	    && (CRYP_PCELL_ID3 ==
+		    readl_relaxed(&device_data->base->pcellId3))) {
+		return 0;
+	}
+
+	return -EPERM;
+}
+
+/**
+ * cryp_activity - This routine enables/disable the cryptography function.
+ * @device_data: Pointer to the device data struct for base address.
+ * @cryp_crypen: Enable/Disable functionality
+ */
+void cryp_activity(struct cryp_device_data *device_data,
+		   enum cryp_crypen cryp_crypen)
+{
+	CRYP_PUT_BITS(&device_data->base->cr,
+		      cryp_crypen,
+		      CRYP_CR_CRYPEN_POS,
+		      CRYP_CR_CRYPEN_MASK);
+}
+
+/**
+ * cryp_flush_inoutfifo - Resets both the input and the output FIFOs
+ * @device_data: Pointer to the device data struct for base address.
+ */
+void cryp_flush_inoutfifo(struct cryp_device_data *device_data)
+{
+	/*
+	 * We always need to disble the hardware before trying to flush the
+	 * FIFO. This is something that isn't written in the design
+	 * specification, but we have been informed by the hardware designers
+	 * that this must be done.
+	 */
+	cryp_activity(device_data, CRYP_CRYPEN_DISABLE);
+	cryp_wait_until_done(device_data);
+
+	CRYP_SET_BITS(&device_data->base->cr, CRYP_CR_FFLUSH_MASK);
+	/*
+	 * CRYP_SR_INFIFO_READY_MASK is the expected value on the status
+	 * register when starting a new calculation, which means Input FIFO is
+	 * not full and input FIFO is empty.
+	 */
+	while (readl_relaxed(&device_data->base->sr) !=
+	       CRYP_SR_INFIFO_READY_MASK)
+		cpu_relax();
+}
+
+/**
+ * cryp_set_configuration - This routine set the cr CRYP IP
+ * @device_data: Pointer to the device data struct for base address.
+ * @cryp_config: Pointer to the configuration parameter
+ * @control_register: The control register to be written later on.
+ */
+int cryp_set_configuration(struct cryp_device_data *device_data,
+			   struct cryp_config *cryp_config,
+			   u32 *control_register)
+{
+	u32 cr_for_kse;
+
+	if (NULL == device_data || NULL == cryp_config)
+		return -EINVAL;
+
+	*control_register |= (cryp_config->keysize << CRYP_CR_KEYSIZE_POS);
+
+	/* Prepare key for decryption in AES_ECB and AES_CBC mode. */
+	if ((CRYP_ALGORITHM_DECRYPT == cryp_config->algodir) &&
+	    ((CRYP_ALGO_AES_ECB == cryp_config->algomode) ||
+	     (CRYP_ALGO_AES_CBC == cryp_config->algomode))) {
+		cr_for_kse = *control_register;
+		/*
+		 * This seems a bit odd, but it is indeed needed to set this to
+		 * encrypt even though it is a decryption that we are doing. It
+		 * also mentioned in the design spec that you need to do this.
+		 * After the keyprepartion for decrypting is done you should set
+		 * algodir back to decryption, which is done outside this if
+		 * statement.
+		 *
+		 * According to design specification we should set mode ECB
+		 * during key preparation even though we might be running CBC
+		 * when enter this function.
+		 *
+		 * Writing to KSE_ENABLED will drop CRYPEN when key preparation
+		 * is done. Therefore we need to set CRYPEN again outside this
+		 * if statement when running decryption.
+		 */
+		cr_for_kse |= ((CRYP_ALGORITHM_ENCRYPT << CRYP_CR_ALGODIR_POS) |
+			       (CRYP_ALGO_AES_ECB << CRYP_CR_ALGOMODE_POS) |
+			       (CRYP_CRYPEN_ENABLE << CRYP_CR_CRYPEN_POS) |
+			       (KSE_ENABLED << CRYP_CR_KSE_POS));
+
+		writel_relaxed(cr_for_kse, &device_data->base->cr);
+		cryp_wait_until_done(device_data);
+	}
+
+	*control_register |=
+		((cryp_config->algomode << CRYP_CR_ALGOMODE_POS) |
+		 (cryp_config->algodir << CRYP_CR_ALGODIR_POS));
+
+	return 0;
+}
+
+/**
+ * cryp_configure_protection - set the protection bits in the CRYP logic.
+ * @device_data: Pointer to the device data struct for base address.
+ * @p_protect_config:	Pointer to the protection mode and
+ *			secure mode configuration
+ */
+int cryp_configure_protection(struct cryp_device_data *device_data,
+			      struct cryp_protection_config *p_protect_config)
+{
+	if (NULL == p_protect_config)
+		return -EINVAL;
+
+	CRYP_WRITE_BIT(&device_data->base->cr,
+		       (u32) p_protect_config->secure_access,
+		       CRYP_CR_SECURE_MASK);
+	CRYP_PUT_BITS(&device_data->base->cr,
+		      p_protect_config->privilege_access,
+		      CRYP_CR_PRLG_POS,
+		      CRYP_CR_PRLG_MASK);
+
+	return 0;
+}
+
+/**
+ * cryp_is_logic_busy - returns the busy status of the CRYP logic
+ * @device_data: Pointer to the device data struct for base address.
+ */
+int cryp_is_logic_busy(struct cryp_device_data *device_data)
+{
+	return CRYP_TEST_BITS(&device_data->base->sr,
+			      CRYP_SR_BUSY_MASK);
+}
+
+/**
+ * cryp_configure_for_dma - configures the CRYP IP for DMA operation
+ * @device_data: Pointer to the device data struct for base address.
+ * @dma_req: Specifies the DMA request type value.
+ */
+void cryp_configure_for_dma(struct cryp_device_data *device_data,
+			    enum cryp_dma_req_type dma_req)
+{
+	CRYP_SET_BITS(&device_data->base->dmacr,
+		      (u32) dma_req);
+}
+
+/**
+ * cryp_configure_key_values - configures the key values for CRYP operations
+ * @device_data: Pointer to the device data struct for base address.
+ * @key_reg_index: Key value index register
+ * @key_value: The key value struct
+ */
+int cryp_configure_key_values(struct cryp_device_data *device_data,
+			      enum cryp_key_reg_index key_reg_index,
+			      struct cryp_key_value key_value)
+{
+	while (cryp_is_logic_busy(device_data))
+		cpu_relax();
+
+	switch (key_reg_index) {
+	case CRYP_KEY_REG_1:
+		writel_relaxed(key_value.key_value_left,
+				&device_data->base->key_1_l);
+		writel_relaxed(key_value.key_value_right,
+				&device_data->base->key_1_r);
+		break;
+	case CRYP_KEY_REG_2:
+		writel_relaxed(key_value.key_value_left,
+				&device_data->base->key_2_l);
+		writel_relaxed(key_value.key_value_right,
+				&device_data->base->key_2_r);
+		break;
+	case CRYP_KEY_REG_3:
+		writel_relaxed(key_value.key_value_left,
+				&device_data->base->key_3_l);
+		writel_relaxed(key_value.key_value_right,
+				&device_data->base->key_3_r);
+		break;
+	case CRYP_KEY_REG_4:
+		writel_relaxed(key_value.key_value_left,
+				&device_data->base->key_4_l);
+		writel_relaxed(key_value.key_value_right,
+				&device_data->base->key_4_r);
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+/**
+ * cryp_configure_init_vector - configures the initialization vector register
+ * @device_data: Pointer to the device data struct for base address.
+ * @init_vector_index: Specifies the index of the init vector.
+ * @init_vector_value: Specifies the value for the init vector.
+ */
+int cryp_configure_init_vector(struct cryp_device_data *device_data,
+			       enum cryp_init_vector_index
+			       init_vector_index,
+			       struct cryp_init_vector_value
+			       init_vector_value)
+{
+	while (cryp_is_logic_busy(device_data))
+		cpu_relax();
+
+	switch (init_vector_index) {
+	case CRYP_INIT_VECTOR_INDEX_0:
+		writel_relaxed(init_vector_value.init_value_left,
+		       &device_data->base->init_vect_0_l);
+		writel_relaxed(init_vector_value.init_value_right,
+		       &device_data->base->init_vect_0_r);
+		break;
+	case CRYP_INIT_VECTOR_INDEX_1:
+		writel_relaxed(init_vector_value.init_value_left,
+		       &device_data->base->init_vect_1_l);
+		writel_relaxed(init_vector_value.init_value_right,
+		       &device_data->base->init_vect_1_r);
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+/**
+ * cryp_save_device_context -	Store hardware registers and
+ *				other device context parameter
+ * @device_data: Pointer to the device data struct for base address.
+ * @ctx: Crypto device context
+ */
+void cryp_save_device_context(struct cryp_device_data *device_data,
+			      struct cryp_device_context *ctx,
+			      int cryp_mode)
+{
+	enum cryp_algo_mode algomode;
+	struct cryp_register *src_reg = device_data->base;
+	struct cryp_config *config =
+		(struct cryp_config *)device_data->current_ctx;
+
+	/*
+	 * Always start by disable the hardware and wait for it to finish the
+	 * ongoing calculations before trying to reprogram it.
+	 */
+	cryp_activity(device_data, CRYP_CRYPEN_DISABLE);
+	cryp_wait_until_done(device_data);
+
+	if (cryp_mode == CRYP_MODE_DMA)
+		cryp_configure_for_dma(device_data, CRYP_DMA_DISABLE_BOTH);
+
+	if (CRYP_TEST_BITS(&src_reg->sr, CRYP_SR_IFEM_MASK) == 0)
+		ctx->din = readl_relaxed(&src_reg->din);
+
+	ctx->cr = readl_relaxed(&src_reg->cr) & CRYP_CR_CONTEXT_SAVE_MASK;
+
+	switch (config->keysize) {
+	case CRYP_KEY_SIZE_256:
+		ctx->key_4_l = readl_relaxed(&src_reg->key_4_l);
+		ctx->key_4_r = readl_relaxed(&src_reg->key_4_r);
+
+	case CRYP_KEY_SIZE_192:
+		ctx->key_3_l = readl_relaxed(&src_reg->key_3_l);
+		ctx->key_3_r = readl_relaxed(&src_reg->key_3_r);
+
+	case CRYP_KEY_SIZE_128:
+		ctx->key_2_l = readl_relaxed(&src_reg->key_2_l);
+		ctx->key_2_r = readl_relaxed(&src_reg->key_2_r);
+
+	default:
+		ctx->key_1_l = readl_relaxed(&src_reg->key_1_l);
+		ctx->key_1_r = readl_relaxed(&src_reg->key_1_r);
+	}
+
+	/* Save IV for CBC mode for both AES and DES. */
+	algomode = ((ctx->cr & CRYP_CR_ALGOMODE_MASK) >> CRYP_CR_ALGOMODE_POS);
+	if (algomode == CRYP_ALGO_TDES_CBC ||
+	    algomode == CRYP_ALGO_DES_CBC ||
+	    algomode == CRYP_ALGO_AES_CBC) {
+		ctx->init_vect_0_l = readl_relaxed(&src_reg->init_vect_0_l);
+		ctx->init_vect_0_r = readl_relaxed(&src_reg->init_vect_0_r);
+		ctx->init_vect_1_l = readl_relaxed(&src_reg->init_vect_1_l);
+		ctx->init_vect_1_r = readl_relaxed(&src_reg->init_vect_1_r);
+	}
+}
+
+/**
+ * cryp_restore_device_context -	Restore hardware registers and
+ *					other device context parameter
+ * @device_data: Pointer to the device data struct for base address.
+ * @ctx: Crypto device context
+ */
+void cryp_restore_device_context(struct cryp_device_data *device_data,
+				 struct cryp_device_context *ctx)
+{
+	struct cryp_register *reg = device_data->base;
+	struct cryp_config *config =
+		(struct cryp_config *)device_data->current_ctx;
+
+	/*
+	 * Fall through for all items in switch statement. DES is captured in
+	 * the default.
+	 */
+	switch (config->keysize) {
+	case CRYP_KEY_SIZE_256:
+		writel_relaxed(ctx->key_4_l, &reg->key_4_l);
+		writel_relaxed(ctx->key_4_r, &reg->key_4_r);
+
+	case CRYP_KEY_SIZE_192:
+		writel_relaxed(ctx->key_3_l, &reg->key_3_l);
+		writel_relaxed(ctx->key_3_r, &reg->key_3_r);
+
+	case CRYP_KEY_SIZE_128:
+		writel_relaxed(ctx->key_2_l, &reg->key_2_l);
+		writel_relaxed(ctx->key_2_r, &reg->key_2_r);
+
+	default:
+		writel_relaxed(ctx->key_1_l, &reg->key_1_l);
+		writel_relaxed(ctx->key_1_r, &reg->key_1_r);
+	}
+
+	/* Restore IV for CBC mode for AES and DES. */
+	if (config->algomode == CRYP_ALGO_TDES_CBC ||
+	    config->algomode == CRYP_ALGO_DES_CBC ||
+	    config->algomode == CRYP_ALGO_AES_CBC) {
+		writel_relaxed(ctx->init_vect_0_l, &reg->init_vect_0_l);
+		writel_relaxed(ctx->init_vect_0_r, &reg->init_vect_0_r);
+		writel_relaxed(ctx->init_vect_1_l, &reg->init_vect_1_l);
+		writel_relaxed(ctx->init_vect_1_r, &reg->init_vect_1_r);
+	}
+}
diff --git a/drivers/crypto/ux500/cryp/cryp.h b/drivers/crypto/ux500/cryp/cryp.h
new file mode 100644
index 00000000000..14cfd05b777
--- /dev/null
+++ b/drivers/crypto/ux500/cryp/cryp.h
@@ -0,0 +1,308 @@
+/**
+ * Copyright (C) ST-Ericsson SA 2010
+ * Author: Shujuan Chen <shujuan.chen@stericsson.com> for ST-Ericsson.
+ * Author: Jonas Linde <jonas.linde@stericsson.com> for ST-Ericsson.
+ * Author: Joakim Bech <joakim.xx.bech@stericsson.com> for ST-Ericsson.
+ * Author: Berne Hebark <berne.herbark@stericsson.com> for ST-Ericsson.
+ * Author: Niklas Hernaeus <niklas.hernaeus@stericsson.com> for ST-Ericsson.
+ * License terms: GNU General Public License (GPL) version 2
+ */
+
+#ifndef _CRYP_H_
+#define _CRYP_H_
+
+#include <linux/completion.h>
+#include <linux/dmaengine.h>
+#include <linux/klist.h>
+#include <linux/mutex.h>
+
+#define DEV_DBG_NAME "crypX crypX:"
+
+/* CRYP enable/disable */
+enum cryp_crypen {
+	CRYP_CRYPEN_DISABLE = 0,
+	CRYP_CRYPEN_ENABLE = 1
+};
+
+/* CRYP Start Computation enable/disable */
+enum cryp_start {
+	CRYP_START_DISABLE = 0,
+	CRYP_START_ENABLE = 1
+};
+
+/* CRYP Init Signal enable/disable */
+enum cryp_init {
+	CRYP_INIT_DISABLE = 0,
+	CRYP_INIT_ENABLE = 1
+};
+
+/* Cryp State enable/disable */
+enum cryp_state {
+	CRYP_STATE_DISABLE = 0,
+	CRYP_STATE_ENABLE = 1
+};
+
+/* Key preparation bit enable */
+enum cryp_key_prep {
+	KSE_DISABLED = 0,
+	KSE_ENABLED = 1
+};
+
+/* Key size for AES */
+#define	CRYP_KEY_SIZE_128 (0)
+#define	CRYP_KEY_SIZE_192 (1)
+#define	CRYP_KEY_SIZE_256 (2)
+
+/* AES modes */
+enum cryp_algo_mode {
+	CRYP_ALGO_TDES_ECB,
+	CRYP_ALGO_TDES_CBC,
+	CRYP_ALGO_DES_ECB,
+	CRYP_ALGO_DES_CBC,
+	CRYP_ALGO_AES_ECB,
+	CRYP_ALGO_AES_CBC,
+	CRYP_ALGO_AES_CTR,
+	CRYP_ALGO_AES_XTS
+};
+
+/* Cryp Encryption or Decryption */
+enum cryp_algorithm_dir {
+	CRYP_ALGORITHM_ENCRYPT,
+	CRYP_ALGORITHM_DECRYPT
+};
+
+/* Hardware access method */
+enum cryp_mode {
+	CRYP_MODE_POLLING,
+	CRYP_MODE_INTERRUPT,
+	CRYP_MODE_DMA
+};
+
+/**
+ * struct cryp_config -
+ * @keysize: Key size for AES
+ * @algomode: AES modes
+ * @algodir: Cryp Encryption or Decryption
+ *
+ * CRYP configuration structure to be passed to set configuration
+ */
+struct cryp_config {
+	int keysize;
+	enum cryp_algo_mode algomode;
+	enum cryp_algorithm_dir algodir;
+};
+
+/**
+ * struct cryp_protection_config -
+ * @privilege_access: Privileged cryp state enable/disable
+ * @secure_access: Secure cryp state enable/disable
+ *
+ * Protection configuration structure for setting privilage access
+ */
+struct cryp_protection_config {
+	enum cryp_state privilege_access;
+	enum cryp_state secure_access;
+};
+
+/* Cryp status */
+enum cryp_status_id {
+	CRYP_STATUS_BUSY = 0x10,
+	CRYP_STATUS_OUTPUT_FIFO_FULL = 0x08,
+	CRYP_STATUS_OUTPUT_FIFO_NOT_EMPTY = 0x04,
+	CRYP_STATUS_INPUT_FIFO_NOT_FULL = 0x02,
+	CRYP_STATUS_INPUT_FIFO_EMPTY = 0x01
+};
+
+/* Cryp DMA interface */
+enum cryp_dma_req_type {
+	CRYP_DMA_DISABLE_BOTH,
+	CRYP_DMA_ENABLE_IN_DATA,
+	CRYP_DMA_ENABLE_OUT_DATA,
+	CRYP_DMA_ENABLE_BOTH_DIRECTIONS
+};
+
+enum cryp_dma_channel {
+	CRYP_DMA_RX = 0,
+	CRYP_DMA_TX
+};
+
+/* Key registers */
+enum cryp_key_reg_index {
+	CRYP_KEY_REG_1,
+	CRYP_KEY_REG_2,
+	CRYP_KEY_REG_3,
+	CRYP_KEY_REG_4
+};
+
+/* Key register left and right */
+struct cryp_key_value {
+	u32 key_value_left;
+	u32 key_value_right;
+};
+
+/* Cryp Initialization structure */
+enum cryp_init_vector_index {
+	CRYP_INIT_VECTOR_INDEX_0,
+	CRYP_INIT_VECTOR_INDEX_1
+};
+
+/* struct cryp_init_vector_value -
+ * @init_value_left
+ * @init_value_right
+ * */
+struct cryp_init_vector_value {
+	u32 init_value_left;
+	u32 init_value_right;
+};
+
+/**
+ * struct cryp_device_context - structure for a cryp context.
+ * @cr: control register
+ * @dmacr: DMA control register
+ * @imsc: Interrupt mask set/clear register
+ * @key_1_l: Key 1l register
+ * @key_1_r: Key 1r register
+ * @key_2_l: Key 2l register
+ * @key_2_r: Key 2r register
+ * @key_3_l: Key 3l register
+ * @key_3_r: Key 3r register
+ * @key_4_l: Key 4l register
+ * @key_4_r: Key 4r register
+ * @init_vect_0_l: Initialization vector 0l register
+ * @init_vect_0_r: Initialization vector 0r register
+ * @init_vect_1_l: Initialization vector 1l register
+ * @init_vect_1_r: Initialization vector 0r register
+ * @din: Data in register
+ * @dout: Data out register
+ *
+ * CRYP power management specifc structure.
+ */
+struct cryp_device_context {
+	u32 cr;
+	u32 dmacr;
+	u32 imsc;
+
+	u32 key_1_l;
+	u32 key_1_r;
+	u32 key_2_l;
+	u32 key_2_r;
+	u32 key_3_l;
+	u32 key_3_r;
+	u32 key_4_l;
+	u32 key_4_r;
+
+	u32 init_vect_0_l;
+	u32 init_vect_0_r;
+	u32 init_vect_1_l;
+	u32 init_vect_1_r;
+
+	u32 din;
+	u32 dout;
+};
+
+struct cryp_dma {
+	dma_cap_mask_t mask;
+	struct completion cryp_dma_complete;
+	struct dma_chan *chan_cryp2mem;
+	struct dma_chan *chan_mem2cryp;
+	struct stedma40_chan_cfg *cfg_cryp2mem;
+	struct stedma40_chan_cfg *cfg_mem2cryp;
+	int sg_src_len;
+	int sg_dst_len;
+	struct scatterlist *sg_src;
+	struct scatterlist *sg_dst;
+	int nents_src;
+	int nents_dst;
+};
+
+/**
+ * struct cryp_device_data - structure for a cryp device.
+ * @base: Pointer to the hardware base address.
+ * @dev: Pointer to the devices dev structure.
+ * @clk: Pointer to the device's clock control.
+ * @pwr_regulator: Pointer to the device's power control.
+ * @power_status: Current status of the power.
+ * @ctx_lock: Lock for current_ctx.
+ * @current_ctx: Pointer to the currently allocated context.
+ * @list_node: For inclusion into a klist.
+ * @dma: The dma structure holding channel configuration.
+ * @power_state: TRUE = power state on, FALSE = power state off.
+ * @power_state_spinlock: Spinlock for power_state.
+ * @restore_dev_ctx: TRUE = saved ctx, FALSE = no saved ctx.
+ */
+struct cryp_device_data {
+	struct cryp_register __iomem *base;
+	struct device *dev;
+	struct clk *clk;
+	struct regulator *pwr_regulator;
+	int power_status;
+	struct spinlock ctx_lock;
+	struct cryp_ctx *current_ctx;
+	struct klist_node list_node;
+	struct cryp_dma dma;
+	bool power_state;
+	struct spinlock power_state_spinlock;
+	bool restore_dev_ctx;
+};
+
+void cryp_wait_until_done(struct cryp_device_data *device_data);
+
+/* Initialization functions */
+
+int cryp_check(struct cryp_device_data *device_data);
+
+void cryp_activity(struct cryp_device_data *device_data,
+		   enum cryp_crypen cryp_crypen);
+
+void cryp_flush_inoutfifo(struct cryp_device_data *device_data);
+
+int cryp_set_configuration(struct cryp_device_data *device_data,
+			   struct cryp_config *cryp_config,
+			   u32 *control_register);
+
+void cryp_configure_for_dma(struct cryp_device_data *device_data,
+			    enum cryp_dma_req_type dma_req);
+
+int cryp_configure_key_values(struct cryp_device_data *device_data,
+			      enum cryp_key_reg_index key_reg_index,
+			      struct cryp_key_value key_value);
+
+int cryp_configure_init_vector(struct cryp_device_data *device_data,
+			       enum cryp_init_vector_index
+			       init_vector_index,
+			       struct cryp_init_vector_value
+			       init_vector_value);
+
+int cryp_configure_protection(struct cryp_device_data *device_data,
+			      struct cryp_protection_config *p_protect_config);
+
+/* Power management funtions */
+void cryp_save_device_context(struct cryp_device_data *device_data,
+			      struct cryp_device_context *ctx,
+			      int cryp_mode);
+
+void cryp_restore_device_context(struct cryp_device_data *device_data,
+				 struct cryp_device_context *ctx);
+
+/* Data transfer and status bits. */
+int cryp_is_logic_busy(struct cryp_device_data *device_data);
+
+int cryp_get_status(struct cryp_device_data *device_data);
+
+/**
+ * cryp_write_indata - This routine writes 32 bit data into the data input
+ *		       register of the cryptography IP.
+ * @device_data: Pointer to the device data struct for base address.
+ * @write_data: Data to write.
+ */
+int cryp_write_indata(struct cryp_device_data *device_data, u32 write_data);
+
+/**
+ * cryp_read_outdata - This routine reads the data from the data output
+ *		       register of the CRYP logic
+ * @device_data: Pointer to the device data struct for base address.
+ * @read_data: Read the data from the output FIFO.
+ */
+int cryp_read_outdata(struct cryp_device_data *device_data, u32 *read_data);
+
+#endif /* _CRYP_H_ */
diff --git a/drivers/crypto/ux500/cryp/cryp_core.c b/drivers/crypto/ux500/cryp/cryp_core.c
new file mode 100644
index 00000000000..7cac12793a4
--- /dev/null
+++ b/drivers/crypto/ux500/cryp/cryp_core.c
@@ -0,0 +1,1784 @@
+/**
+ * Copyright (C) ST-Ericsson SA 2010
+ * Author: Shujuan Chen <shujuan.chen@stericsson.com> for ST-Ericsson.
+ * Author: Joakim Bech <joakim.xx.bech@stericsson.com> for ST-Ericsson.
+ * Author: Berne Hebark <berne.herbark@stericsson.com> for ST-Ericsson.
+ * Author: Niklas Hernaeus <niklas.hernaeus@stericsson.com> for ST-Ericsson.
+ * Author: Jonas Linde <jonas.linde@stericsson.com> for ST-Ericsson.
+ * Author: Andreas Westin <andreas.westin@stericsson.com> for ST-Ericsson.
+ * License terms: GNU General Public License (GPL) version 2
+ */
+
+#include <linux/clk.h>
+#include <linux/completion.h>
+#include <linux/crypto.h>
+#include <linux/dmaengine.h>
+#include <linux/err.h>
+#include <linux/errno.h>
+#include <linux/interrupt.h>
+#include <linux/io.h>
+#include <linux/irqreturn.h>
+#include <linux/klist.h>
+#include <linux/module.h>
+#include <linux/platform_device.h>
+#include <linux/regulator/consumer.h>
+#include <linux/semaphore.h>
+
+#include <crypto/aes.h>
+#include <crypto/algapi.h>
+#include <crypto/ctr.h>
+#include <crypto/des.h>
+#include <crypto/scatterwalk.h>
+
+#include <plat/ste_dma40.h>
+
+#include <mach/crypto-ux500.h>
+#include <mach/hardware.h>
+
+#include "cryp_p.h"
+#include "cryp.h"
+
+#define CRYP_MAX_KEY_SIZE	32
+#define BYTES_PER_WORD		4
+
+static int cryp_mode;
+static atomic_t session_id;
+
+static struct stedma40_chan_cfg *mem_to_engine;
+static struct stedma40_chan_cfg *engine_to_mem;
+
+/**
+ * struct cryp_driver_data - data specific to the driver.
+ *
+ * @device_list: A list of registered devices to choose from.
+ * @device_allocation: A semaphore initialized with number of devices.
+ */
+struct cryp_driver_data {
+	struct klist device_list;
+	struct semaphore device_allocation;
+};
+
+/**
+ * struct cryp_ctx - Crypto context
+ * @config: Crypto mode.
+ * @key[CRYP_MAX_KEY_SIZE]: Key.
+ * @keylen: Length of key.
+ * @iv: Pointer to initialization vector.
+ * @indata: Pointer to indata.
+ * @outdata: Pointer to outdata.
+ * @datalen: Length of indata.
+ * @outlen: Length of outdata.
+ * @blocksize: Size of blocks.
+ * @updated: Updated flag.
+ * @dev_ctx: Device dependent context.
+ * @device: Pointer to the device.
+ */
+struct cryp_ctx {
+	struct cryp_config config;
+	u8 key[CRYP_MAX_KEY_SIZE];
+	u32 keylen;
+	u8 *iv;
+	const u8 *indata;
+	u8 *outdata;
+	u32 datalen;
+	u32 outlen;
+	u32 blocksize;
+	u8 updated;
+	struct cryp_device_context dev_ctx;
+	struct cryp_device_data *device;
+	u32 session_id;
+};
+
+static struct cryp_driver_data driver_data;
+
+/**
+ * uint8p_to_uint32_be - 4*uint8 to uint32 big endian
+ * @in: Data to convert.
+ */
+static inline u32 uint8p_to_uint32_be(u8 *in)
+{
+	u32 *data = (u32 *)in;
+
+	return cpu_to_be32p(data);
+}
+
+/**
+ * swap_bits_in_byte - mirror the bits in a byte
+ * @b: the byte to be mirrored
+ *
+ * The bits are swapped the following way:
+ *  Byte b include bits 0-7, nibble 1 (n1) include bits 0-3 and
+ *  nibble 2 (n2) bits 4-7.
+ *
+ *  Nibble 1 (n1):
+ *  (The "old" (moved) bit is replaced with a zero)
+ *  1. Move bit 6 and 7, 4 positions to the left.
+ *  2. Move bit 3 and 5, 2 positions to the left.
+ *  3. Move bit 1-4, 1 position to the left.
+ *
+ *  Nibble 2 (n2):
+ *  1. Move bit 0 and 1, 4 positions to the right.
+ *  2. Move bit 2 and 4, 2 positions to the right.
+ *  3. Move bit 3-6, 1 position to the right.
+ *
+ *  Combine the two nibbles to a complete and swapped byte.
+ */
+
+static inline u8 swap_bits_in_byte(u8 b)
+{
+#define R_SHIFT_4_MASK  0xc0 /* Bits 6 and 7, right shift 4 */
+#define R_SHIFT_2_MASK  0x28 /* (After right shift 4) Bits 3 and 5,
+				  right shift 2 */
+#define R_SHIFT_1_MASK  0x1e /* (After right shift 2) Bits 1-4,
+				  right shift 1 */
+#define L_SHIFT_4_MASK  0x03 /* Bits 0 and 1, left shift 4 */
+#define L_SHIFT_2_MASK  0x14 /* (After left shift 4) Bits 2 and 4,
+				  left shift 2 */
+#define L_SHIFT_1_MASK  0x78 /* (After left shift 1) Bits 3-6,
+				  left shift 1 */
+
+	u8 n1;
+	u8 n2;
+
+	/* Swap most significant nibble */
+	/* Right shift 4, bits 6 and 7 */
+	n1 = ((b  & R_SHIFT_4_MASK) >> 4) | (b  & ~(R_SHIFT_4_MASK >> 4));
+	/* Right shift 2, bits 3 and 5 */
+	n1 = ((n1 & R_SHIFT_2_MASK) >> 2) | (n1 & ~(R_SHIFT_2_MASK >> 2));
+	/* Right shift 1, bits 1-4 */
+	n1 = (n1  & R_SHIFT_1_MASK) >> 1;
+
+	/* Swap least significant nibble */
+	/* Left shift 4, bits 0 and 1 */
+	n2 = ((b  & L_SHIFT_4_MASK) << 4) | (b  & ~(L_SHIFT_4_MASK << 4));
+	/* Left shift 2, bits 2 and 4 */
+	n2 = ((n2 & L_SHIFT_2_MASK) << 2) | (n2 & ~(L_SHIFT_2_MASK << 2));
+	/* Left shift 1, bits 3-6 */
+	n2 = (n2  & L_SHIFT_1_MASK) << 1;
+
+	return n1 | n2;
+}
+
+static inline void swap_words_in_key_and_bits_in_byte(const u8 *in,
+						      u8 *out, u32 len)
+{
+	unsigned int i = 0;
+	int j;
+	int index = 0;
+
+	j = len - BYTES_PER_WORD;
+	while (j >= 0) {
+		for (i = 0; i < BYTES_PER_WORD; i++) {
+			index = len - j - BYTES_PER_WORD + i;
+			out[j + i] =
+				swap_bits_in_byte(in[index]);
+		}
+		j -= BYTES_PER_WORD;
+	}
+}
+
+static void add_session_id(struct cryp_ctx *ctx)
+{
+	/*
+	 * We never want 0 to be a valid value, since this is the default value
+	 * for the software context.
+	 */
+	if (unlikely(atomic_inc_and_test(&session_id)))
+		atomic_inc(&session_id);
+
+	ctx->session_id = atomic_read(&session_id);
+}
+
+static irqreturn_t cryp_interrupt_handler(int irq, void *param)
+{
+	struct cryp_ctx *ctx;
+	int i;
+	struct cryp_device_data *device_data;
+
+	if (param == NULL) {
+		BUG_ON(!param);
+		return IRQ_HANDLED;
+	}
+
+	/* The device is coming from the one found in hw_crypt_noxts. */
+	device_data = (struct cryp_device_data *)param;
+
+	ctx = device_data->current_ctx;
+
+	if (ctx == NULL) {
+		BUG_ON(!ctx);
+		return IRQ_HANDLED;
+	}
+
+	dev_dbg(ctx->device->dev, "[%s] (len: %d) %s, ", __func__, ctx->outlen,
+		cryp_pending_irq_src(device_data, CRYP_IRQ_SRC_OUTPUT_FIFO) ?
+		"out" : "in");
+
+	if (cryp_pending_irq_src(device_data,
+				 CRYP_IRQ_SRC_OUTPUT_FIFO)) {
+		if (ctx->outlen / ctx->blocksize > 0) {
+			for (i = 0; i < ctx->blocksize / 4; i++) {
+				*(ctx->outdata) = readl_relaxed(
+						&device_data->base->dout);
+				ctx->outdata += 4;
+				ctx->outlen -= 4;
+			}
+
+			if (ctx->outlen == 0) {
+				cryp_disable_irq_src(device_data,
+						     CRYP_IRQ_SRC_OUTPUT_FIFO);
+			}
+		}
+	} else if (cryp_pending_irq_src(device_data,
+					CRYP_IRQ_SRC_INPUT_FIFO)) {
+		if (ctx->datalen / ctx->blocksize > 0) {
+			for (i = 0 ; i < ctx->blocksize / 4; i++) {
+				writel_relaxed(ctx->indata,
+						&device_data->base->din);
+				ctx->indata += 4;
+				ctx->datalen -= 4;
+			}
+
+			if (ctx->datalen == 0)
+				cryp_disable_irq_src(device_data,
+						   CRYP_IRQ_SRC_INPUT_FIFO);
+
+			if (ctx->config.algomode == CRYP_ALGO_AES_XTS) {
+				CRYP_PUT_BITS(&device_data->base->cr,
+					      CRYP_START_ENABLE,
+					      CRYP_CR_START_POS,
+					      CRYP_CR_START_MASK);
+
+				cryp_wait_until_done(device_data);
+			}
+		}
+	}
+
+	return IRQ_HANDLED;
+}
+
+static int mode_is_aes(enum cryp_algo_mode mode)
+{
+	return	CRYP_ALGO_AES_ECB == mode ||
+		CRYP_ALGO_AES_CBC == mode ||
+		CRYP_ALGO_AES_CTR == mode ||
+		CRYP_ALGO_AES_XTS == mode;
+}
+
+static int cfg_iv(struct cryp_device_data *device_data, u32 left, u32 right,
+		  enum cryp_init_vector_index index)
+{
+	struct cryp_init_vector_value vector_value;
+
+	dev_dbg(device_data->dev, "[%s]", __func__);
+
+	vector_value.init_value_left = left;
+	vector_value.init_value_right = right;
+
+	return cryp_configure_init_vector(device_data,
+					  index,
+					  vector_value);
+}
+
+static int cfg_ivs(struct cryp_device_data *device_data, struct cryp_ctx *ctx)
+{
+	int i;
+	int status = 0;
+	int num_of_regs = ctx->blocksize / 8;
+	u32 iv[AES_BLOCK_SIZE / 4];
+
+	dev_dbg(device_data->dev, "[%s]", __func__);
+
+	/*
+	 * Since we loop on num_of_regs we need to have a check in case
+	 * someone provides an incorrect blocksize which would force calling
+	 * cfg_iv with i greater than 2 which is an error.
+	 */
+	if (num_of_regs > 2) {
+		dev_err(device_data->dev, "[%s] Incorrect blocksize %d",
+			__func__, ctx->blocksize);
+		return -EINVAL;
+	}
+
+	for (i = 0; i < ctx->blocksize / 4; i++)
+		iv[i] = uint8p_to_uint32_be(ctx->iv + i*4);
+
+	for (i = 0; i < num_of_regs; i++) {
+		status = cfg_iv(device_data, iv[i*2], iv[i*2+1],
+				(enum cryp_init_vector_index) i);
+		if (status != 0)
+			return status;
+	}
+	return status;
+}
+
+static int set_key(struct cryp_device_data *device_data,
+		   u32 left_key,
+		   u32 right_key,
+		   enum cryp_key_reg_index index)
+{
+	struct cryp_key_value key_value;
+	int cryp_error;
+
+	dev_dbg(device_data->dev, "[%s]", __func__);
+
+	key_value.key_value_left = left_key;
+	key_value.key_value_right = right_key;
+
+	cryp_error = cryp_configure_key_values(device_data,
+					       index,
+					       key_value);
+	if (cryp_error != 0)
+		dev_err(device_data->dev, "[%s]: "
+			"cryp_configure_key_values() failed!", __func__);
+
+	return cryp_error;
+}
+
+static int cfg_keys(struct cryp_ctx *ctx)
+{
+	int i;
+	int num_of_regs = ctx->keylen / 8;
+	u32 swapped_key[CRYP_MAX_KEY_SIZE / 4];
+	int cryp_error = 0;
+
+	dev_dbg(ctx->device->dev, "[%s]", __func__);
+
+	if (mode_is_aes(ctx->config.algomode)) {
+		swap_words_in_key_and_bits_in_byte((u8 *)ctx->key,
+						   (u8 *)swapped_key,
+						   ctx->keylen);
+	} else {
+		for (i = 0; i < ctx->keylen / 4; i++)
+			swapped_key[i] = uint8p_to_uint32_be(ctx->key + i*4);
+	}
+
+	for (i = 0; i < num_of_regs; i++) {
+		cryp_error = set_key(ctx->device,
+				     *(((u32 *)swapped_key)+i*2),
+				     *(((u32 *)swapped_key)+i*2+1),
+				     (enum cryp_key_reg_index) i);
+
+		if (cryp_error != 0) {
+			dev_err(ctx->device->dev, "[%s]: set_key() failed!",
+					__func__);
+			return cryp_error;
+		}
+	}
+	return cryp_error;
+}
+
+static int cryp_setup_context(struct cryp_ctx *ctx,
+			      struct cryp_device_data *device_data)
+{
+	u32 control_register = CRYP_CR_DEFAULT;
+
+	switch (cryp_mode) {
+	case CRYP_MODE_INTERRUPT:
+		writel_relaxed(CRYP_IMSC_DEFAULT, &device_data->base->imsc);
+		break;
+
+	case CRYP_MODE_DMA:
+		writel_relaxed(CRYP_DMACR_DEFAULT, &device_data->base->dmacr);
+		break;
+
+	default:
+		break;
+	}
+
+	if (ctx->updated == 0) {
+		cryp_flush_inoutfifo(device_data);
+		if (cfg_keys(ctx) != 0) {
+			dev_err(ctx->device->dev, "[%s]: cfg_keys failed!",
+				__func__);
+			return -EINVAL;
+		}
+
+		if (ctx->iv &&
+		    CRYP_ALGO_AES_ECB != ctx->config.algomode &&
+		    CRYP_ALGO_DES_ECB != ctx->config.algomode &&
+		    CRYP_ALGO_TDES_ECB != ctx->config.algomode) {
+			if (cfg_ivs(device_data, ctx) != 0)
+				return -EPERM;
+		}
+
+		cryp_set_configuration(device_data, &ctx->config,
+				       &control_register);
+		add_session_id(ctx);
+	} else if (ctx->updated == 1 &&
+		   ctx->session_id != atomic_read(&session_id)) {
+		cryp_flush_inoutfifo(device_data);
+		cryp_restore_device_context(device_data, &ctx->dev_ctx);
+
+		add_session_id(ctx);
+		control_register = ctx->dev_ctx.cr;
+	} else
+		control_register = ctx->dev_ctx.cr;
+
+	writel(control_register |
+	       (CRYP_CRYPEN_ENABLE << CRYP_CR_CRYPEN_POS),
+	       &device_data->base->cr);
+
+	return 0;
+}
+
+static int cryp_get_device_data(struct cryp_ctx *ctx,
+				struct cryp_device_data **device_data)
+{
+	int ret;
+	struct klist_iter device_iterator;
+	struct klist_node *device_node;
+	struct cryp_device_data *local_device_data = NULL;
+	pr_debug(DEV_DBG_NAME " [%s]", __func__);
+
+	/* Wait until a device is available */
+	ret = down_interruptible(&driver_data.device_allocation);
+	if (ret)
+		return ret;  /* Interrupted */
+
+	/* Select a device */
+	klist_iter_init(&driver_data.device_list, &device_iterator);
+
+	device_node = klist_next(&device_iterator);
+	while (device_node) {
+		local_device_data = container_of(device_node,
+					   struct cryp_device_data, list_node);
+		spin_lock(&local_device_data->ctx_lock);
+		/* current_ctx allocates a device, NULL = unallocated */
+		if (local_device_data->current_ctx) {
+			device_node = klist_next(&device_iterator);
+		} else {
+			local_device_data->current_ctx = ctx;
+			ctx->device = local_device_data;
+			spin_unlock(&local_device_data->ctx_lock);
+			break;
+		}
+		spin_unlock(&local_device_data->ctx_lock);
+	}
+	klist_iter_exit(&device_iterator);
+
+	if (!device_node) {
+		/**
+		 * No free device found.
+		 * Since we allocated a device with down_interruptible, this
+		 * should not be able to happen.
+		 * Number of available devices, which are contained in
+		 * device_allocation, is therefore decremented by not doing
+		 * an up(device_allocation).
+		 */
+		return -EBUSY;
+	}
+
+	*device_data = local_device_data;
+
+	return 0;
+}
+
+static void cryp_dma_setup_channel(struct cryp_device_data *device_data,
+				   struct device *dev)
+{
+	dma_cap_zero(device_data->dma.mask);
+	dma_cap_set(DMA_SLAVE, device_data->dma.mask);
+
+	device_data->dma.cfg_mem2cryp = mem_to_engine;
+	device_data->dma.chan_mem2cryp =
+		dma_request_channel(device_data->dma.mask,
+				    stedma40_filter,
+				    device_data->dma.cfg_mem2cryp);
+
+	device_data->dma.cfg_cryp2mem = engine_to_mem;
+	device_data->dma.chan_cryp2mem =
+		dma_request_channel(device_data->dma.mask,
+				    stedma40_filter,
+				    device_data->dma.cfg_cryp2mem);
+
+	init_completion(&device_data->dma.cryp_dma_complete);
+}
+
+static void cryp_dma_out_callback(void *data)
+{
+	struct cryp_ctx *ctx = (struct cryp_ctx *) data;
+	dev_dbg(ctx->device->dev, "[%s]: ", __func__);
+
+	complete(&ctx->device->dma.cryp_dma_complete);
+}
+
+static int cryp_set_dma_transfer(struct cryp_ctx *ctx,
+				 struct scatterlist *sg,
+				 int len,
+				 enum dma_data_direction direction)
+{
+	struct dma_async_tx_descriptor *desc;
+	struct dma_chan *channel = NULL;
+	dma_cookie_t cookie;
+
+	dev_dbg(ctx->device->dev, "[%s]: ", __func__);
+
+	if (unlikely(!IS_ALIGNED((u32)sg, 4))) {
+		dev_err(ctx->device->dev, "[%s]: Data in sg list isn't "
+			"aligned! Addr: 0x%08x", __func__, (u32)sg);
+		return -EFAULT;
+	}
+
+	switch (direction) {
+	case DMA_TO_DEVICE:
+		channel = ctx->device->dma.chan_mem2cryp;
+		ctx->device->dma.sg_src = sg;
+		ctx->device->dma.sg_src_len = dma_map_sg(channel->device->dev,
+						 ctx->device->dma.sg_src,
+						 ctx->device->dma.nents_src,
+						 direction);
+
+		if (!ctx->device->dma.sg_src_len) {
+			dev_dbg(ctx->device->dev,
+				"[%s]: Could not map the sg list (TO_DEVICE)",
+				__func__);
+			return -EFAULT;
+		}
+
+		dev_dbg(ctx->device->dev, "[%s]: Setting up DMA for buffer "
+			"(TO_DEVICE)", __func__);
+
+		desc = channel->device->device_prep_slave_sg(channel,
+					     ctx->device->dma.sg_src,
+					     ctx->device->dma.sg_src_len,
+					     direction, DMA_CTRL_ACK, NULL);
+		break;
+
+	case DMA_FROM_DEVICE:
+		channel = ctx->device->dma.chan_cryp2mem;
+		ctx->device->dma.sg_dst = sg;
+		ctx->device->dma.sg_dst_len = dma_map_sg(channel->device->dev,
+						 ctx->device->dma.sg_dst,
+						 ctx->device->dma.nents_dst,
+						 direction);
+
+		if (!ctx->device->dma.sg_dst_len) {
+			dev_dbg(ctx->device->dev,
+				"[%s]: Could not map the sg list (FROM_DEVICE)",
+				__func__);
+			return -EFAULT;
+		}
+
+		dev_dbg(ctx->device->dev, "[%s]: Setting up DMA for buffer "
+			"(FROM_DEVICE)", __func__);
+
+		desc = channel->device->device_prep_slave_sg(channel,
+					     ctx->device->dma.sg_dst,
+					     ctx->device->dma.sg_dst_len,
+					     direction,
+					     DMA_CTRL_ACK |
+					     DMA_PREP_INTERRUPT, NULL);
+
+		desc->callback = cryp_dma_out_callback;
+		desc->callback_param = ctx;
+		break;
+
+	default:
+		dev_dbg(ctx->device->dev, "[%s]: Invalid DMA direction",
+			__func__);
+		return -EFAULT;
+	}
+
+	cookie = desc->tx_submit(desc);
+	dma_async_issue_pending(channel);
+
+	return 0;
+}
+
+static void cryp_dma_done(struct cryp_ctx *ctx)
+{
+	struct dma_chan *chan;
+
+	dev_dbg(ctx->device->dev, "[%s]: ", __func__);
+
+	chan = ctx->device->dma.chan_mem2cryp;
+	chan->device->device_control(chan, DMA_TERMINATE_ALL, 0);
+	dma_unmap_sg(chan->device->dev, ctx->device->dma.sg_src,
+		     ctx->device->dma.sg_src_len, DMA_TO_DEVICE);
+
+	chan = ctx->device->dma.chan_cryp2mem;
+	chan->device->device_control(chan, DMA_TERMINATE_ALL, 0);
+	dma_unmap_sg(chan->device->dev, ctx->device->dma.sg_dst,
+		     ctx->device->dma.sg_dst_len, DMA_FROM_DEVICE);
+}
+
+static int cryp_dma_write(struct cryp_ctx *ctx, struct scatterlist *sg,
+			  int len)
+{
+	int error = cryp_set_dma_transfer(ctx, sg, len, DMA_TO_DEVICE);
+	dev_dbg(ctx->device->dev, "[%s]: ", __func__);
+
+	if (error) {
+		dev_dbg(ctx->device->dev, "[%s]: cryp_set_dma_transfer() "
+			"failed", __func__);
+		return error;
+	}
+
+	return len;
+}
+
+static int cryp_dma_read(struct cryp_ctx *ctx, struct scatterlist *sg, int len)
+{
+	int error = cryp_set_dma_transfer(ctx, sg, len, DMA_FROM_DEVICE);
+	if (error) {
+		dev_dbg(ctx->device->dev, "[%s]: cryp_set_dma_transfer() "
+			"failed", __func__);
+		return error;
+	}
+
+	return len;
+}
+
+static void cryp_polling_mode(struct cryp_ctx *ctx,
+			      struct cryp_device_data *device_data)
+{
+	int len = ctx->blocksize / BYTES_PER_WORD;
+	int remaining_length = ctx->datalen;
+	u32 *indata = (u32 *)ctx->indata;
+	u32 *outdata = (u32 *)ctx->outdata;
+
+	while (remaining_length > 0) {
+		writesl(&device_data->base->din, indata, len);
+		indata += len;
+		remaining_length -= (len * BYTES_PER_WORD);
+		cryp_wait_until_done(device_data);
+
+		readsl(&device_data->base->dout, outdata, len);
+		outdata += len;
+		cryp_wait_until_done(device_data);
+	}
+}
+
+static int cryp_disable_power(struct device *dev,
+			      struct cryp_device_data *device_data,
+			      bool save_device_context)
+{
+	int ret = 0;
+
+	dev_dbg(dev, "[%s]", __func__);
+
+	spin_lock(&device_data->power_state_spinlock);
+	if (!device_data->power_state)
+		goto out;
+
+	spin_lock(&device_data->ctx_lock);
+	if (save_device_context && device_data->current_ctx) {
+		cryp_save_device_context(device_data,
+				&device_data->current_ctx->dev_ctx,
+				cryp_mode);
+		device_data->restore_dev_ctx = true;
+	}
+	spin_unlock(&device_data->ctx_lock);
+
+	clk_disable(device_data->clk);
+	ret = regulator_disable(device_data->pwr_regulator);
+	if (ret)
+		dev_err(dev, "[%s]: "
+				"regulator_disable() failed!",
+				__func__);
+
+	device_data->power_state = false;
+
+out:
+	spin_unlock(&device_data->power_state_spinlock);
+
+	return ret;
+}
+
+static int cryp_enable_power(
+		struct device *dev,
+		struct cryp_device_data *device_data,
+		bool restore_device_context)
+{
+	int ret = 0;
+
+	dev_dbg(dev, "[%s]", __func__);
+
+	spin_lock(&device_data->power_state_spinlock);
+	if (!device_data->power_state) {
+		ret = regulator_enable(device_data->pwr_regulator);
+		if (ret) {
+			dev_err(dev, "[%s]: regulator_enable() failed!",
+					__func__);
+			goto out;
+		}
+
+		ret = clk_enable(device_data->clk);
+		if (ret) {
+			dev_err(dev, "[%s]: clk_enable() failed!",
+					__func__);
+			regulator_disable(device_data->pwr_regulator);
+			goto out;
+		}
+		device_data->power_state = true;
+	}
+
+	if (device_data->restore_dev_ctx) {
+		spin_lock(&device_data->ctx_lock);
+		if (restore_device_context && device_data->current_ctx) {
+			device_data->restore_dev_ctx = false;
+			cryp_restore_device_context(device_data,
+					&device_data->current_ctx->dev_ctx);
+		}
+		spin_unlock(&device_data->ctx_lock);
+	}
+out:
+	spin_unlock(&device_data->power_state_spinlock);
+
+	return ret;
+}
+
+static int hw_crypt_noxts(struct cryp_ctx *ctx,
+			  struct cryp_device_data *device_data)
+{
+	int ret = 0;
+
+	const u8 *indata = ctx->indata;
+	u8 *outdata = ctx->outdata;
+	u32 datalen = ctx->datalen;
+	u32 outlen = datalen;
+
+	pr_debug(DEV_DBG_NAME " [%s]", __func__);
+
+	ctx->outlen = ctx->datalen;
+
+	if (unlikely(!IS_ALIGNED((u32)indata, 4))) {
+		pr_debug(DEV_DBG_NAME " [%s]: Data isn't aligned! Addr: "
+			 "0x%08x", __func__, (u32)indata);
+		return -EINVAL;
+	}
+
+	ret = cryp_setup_context(ctx, device_data);
+
+	if (ret)
+		goto out;
+
+	if (cryp_mode == CRYP_MODE_INTERRUPT) {
+		cryp_enable_irq_src(device_data, CRYP_IRQ_SRC_INPUT_FIFO |
+				    CRYP_IRQ_SRC_OUTPUT_FIFO);
+
+		/*
+		 * ctx->outlen is decremented in the cryp_interrupt_handler
+		 * function. We had to add cpu_relax() (barrier) to make sure
+		 * that gcc didn't optimze away this variable.
+		 */
+		while (ctx->outlen > 0)
+			cpu_relax();
+	} else if (cryp_mode == CRYP_MODE_POLLING ||
+		   cryp_mode == CRYP_MODE_DMA) {
+		/*
+		 * The reason for having DMA in this if case is that if we are
+		 * running cryp_mode = 2, then we separate DMA routines for
+		 * handling cipher/plaintext > blocksize, except when
+		 * running the normal CRYPTO_ALG_TYPE_CIPHER, then we still use
+		 * the polling mode. Overhead of doing DMA setup eats up the
+		 * benefits using it.
+		 */
+		cryp_polling_mode(ctx, device_data);
+	} else {
+		dev_err(ctx->device->dev, "[%s]: Invalid operation mode!",
+			__func__);
+		ret = -EPERM;
+		goto out;
+	}
+
+	cryp_save_device_context(device_data, &ctx->dev_ctx, cryp_mode);
+	ctx->updated = 1;
+
+out:
+	ctx->indata = indata;
+	ctx->outdata = outdata;
+	ctx->datalen = datalen;
+	ctx->outlen = outlen;
+
+	return ret;
+}
+
+static int get_nents(struct scatterlist *sg, int nbytes)
+{
+	int nents = 0;
+
+	while (nbytes > 0) {
+		nbytes -= sg->length;
+		sg = scatterwalk_sg_next(sg);
+		nents++;
+	}
+
+	return nents;
+}
+
+static int ablk_dma_crypt(struct ablkcipher_request *areq)
+{
+	struct crypto_ablkcipher *cipher = crypto_ablkcipher_reqtfm(areq);
+	struct cryp_ctx *ctx = crypto_ablkcipher_ctx(cipher);
+	struct cryp_device_data *device_data;
+
+	int bytes_written = 0;
+	int bytes_read = 0;
+	int ret;
+
+	pr_debug(DEV_DBG_NAME " [%s]", __func__);
+
+	ctx->datalen = areq->nbytes;
+	ctx->outlen = areq->nbytes;
+
+	ret = cryp_get_device_data(ctx, &device_data);
+	if (ret)
+		return ret;
+
+	ret = cryp_setup_context(ctx, device_data);
+	if (ret)
+		goto out;
+
+	/* We have the device now, so store the nents in the dma struct. */
+	ctx->device->dma.nents_src = get_nents(areq->src, ctx->datalen);
+	ctx->device->dma.nents_dst = get_nents(areq->dst, ctx->outlen);
+
+	/* Enable DMA in- and output. */
+	cryp_configure_for_dma(device_data, CRYP_DMA_ENABLE_BOTH_DIRECTIONS);
+
+	bytes_written = cryp_dma_write(ctx, areq->src, ctx->datalen);
+	bytes_read = cryp_dma_read(ctx, areq->dst, bytes_written);
+
+	wait_for_completion(&ctx->device->dma.cryp_dma_complete);
+	cryp_dma_done(ctx);
+
+	cryp_save_device_context(device_data, &ctx->dev_ctx, cryp_mode);
+	ctx->updated = 1;
+
+out:
+	spin_lock(&device_data->ctx_lock);
+	device_data->current_ctx = NULL;
+	ctx->device = NULL;
+	spin_unlock(&device_data->ctx_lock);
+
+	/*
+	 * The down_interruptible part for this semaphore is called in
+	 * cryp_get_device_data.
+	 */
+	up(&driver_data.device_allocation);
+
+	if (unlikely(bytes_written != bytes_read))
+		return -EPERM;
+
+	return 0;
+}
+
+static int ablk_crypt(struct ablkcipher_request *areq)
+{
+	struct ablkcipher_walk walk;
+	struct crypto_ablkcipher *cipher = crypto_ablkcipher_reqtfm(areq);
+	struct cryp_ctx *ctx = crypto_ablkcipher_ctx(cipher);
+	struct cryp_device_data *device_data;
+	unsigned long src_paddr;
+	unsigned long dst_paddr;
+	int ret;
+	int nbytes;
+
+	pr_debug(DEV_DBG_NAME " [%s]", __func__);
+
+	ret = cryp_get_device_data(ctx, &device_data);
+	if (ret)
+		goto out;
+
+	ablkcipher_walk_init(&walk, areq->dst, areq->src, areq->nbytes);
+	ret = ablkcipher_walk_phys(areq, &walk);
+
+	if (ret) {
+		pr_err(DEV_DBG_NAME "[%s]: ablkcipher_walk_phys() failed!",
+			__func__);
+		goto out;
+	}
+
+	while ((nbytes = walk.nbytes) > 0) {
+		ctx->iv = walk.iv;
+		src_paddr = (page_to_phys(walk.src.page) + walk.src.offset);
+		ctx->indata = phys_to_virt(src_paddr);
+
+		dst_paddr = (page_to_phys(walk.dst.page) + walk.dst.offset);
+		ctx->outdata = phys_to_virt(dst_paddr);
+
+		ctx->datalen = nbytes - (nbytes % ctx->blocksize);
+
+		ret = hw_crypt_noxts(ctx, device_data);
+		if (ret)
+			goto out;
+
+		nbytes -= ctx->datalen;
+		ret = ablkcipher_walk_done(areq, &walk, nbytes);
+		if (ret)
+			goto out;
+	}
+	ablkcipher_walk_complete(&walk);
+
+out:
+	/* Release the device */
+	spin_lock(&device_data->ctx_lock);
+	device_data->current_ctx = NULL;
+	ctx->device = NULL;
+	spin_unlock(&device_data->ctx_lock);
+
+	/*
+	 * The down_interruptible part for this semaphore is called in
+	 * cryp_get_device_data.
+	 */
+	up(&driver_data.device_allocation);
+
+	return ret;
+}
+
+static int aes_ablkcipher_setkey(struct crypto_ablkcipher *cipher,
+				 const u8 *key, unsigned int keylen)
+{
+	struct cryp_ctx *ctx = crypto_ablkcipher_ctx(cipher);
+	u32 *flags = &cipher->base.crt_flags;
+
+	pr_debug(DEV_DBG_NAME " [%s]", __func__);
+
+	switch (keylen) {
+	case AES_KEYSIZE_128:
+		ctx->config.keysize = CRYP_KEY_SIZE_128;
+		break;
+
+	case AES_KEYSIZE_192:
+		ctx->config.keysize = CRYP_KEY_SIZE_192;
+		break;
+
+	case AES_KEYSIZE_256:
+		ctx->config.keysize = CRYP_KEY_SIZE_256;
+		break;
+
+	default:
+		pr_err(DEV_DBG_NAME "[%s]: Unknown keylen!", __func__);
+		*flags |= CRYPTO_TFM_RES_BAD_KEY_LEN;
+		return -EINVAL;
+	}
+
+	memcpy(ctx->key, key, keylen);
+	ctx->keylen = keylen;
+
+	ctx->updated = 0;
+
+	return 0;
+}
+
+static int des_ablkcipher_setkey(struct crypto_ablkcipher *cipher,
+				 const u8 *key, unsigned int keylen)
+{
+	struct cryp_ctx *ctx = crypto_ablkcipher_ctx(cipher);
+	u32 *flags = &cipher->base.crt_flags;
+	u32 tmp[DES_EXPKEY_WORDS];
+	int ret;
+
+	pr_debug(DEV_DBG_NAME " [%s]", __func__);
+	if (keylen != DES_KEY_SIZE) {
+		*flags |= CRYPTO_TFM_RES_BAD_KEY_LEN;
+		pr_debug(DEV_DBG_NAME " [%s]: CRYPTO_TFM_RES_BAD_KEY_LEN",
+				__func__);
+		return -EINVAL;
+	}
+
+	ret = des_ekey(tmp, key);
+	if (unlikely(ret == 0) && (*flags & CRYPTO_TFM_REQ_WEAK_KEY)) {
+		*flags |= CRYPTO_TFM_RES_WEAK_KEY;
+		pr_debug(DEV_DBG_NAME " [%s]: CRYPTO_TFM_REQ_WEAK_KEY",
+				__func__);
+		return -EINVAL;
+	}
+
+	memcpy(ctx->key, key, keylen);
+	ctx->keylen = keylen;
+
+	ctx->updated = 0;
+	return 0;
+}
+
+static int des3_ablkcipher_setkey(struct crypto_ablkcipher *cipher,
+				  const u8 *key, unsigned int keylen)
+{
+	struct cryp_ctx *ctx = crypto_ablkcipher_ctx(cipher);
+	u32 *flags = &cipher->base.crt_flags;
+	const u32 *K = (const u32 *)key;
+	u32 tmp[DES3_EDE_EXPKEY_WORDS];
+	int i, ret;
+
+	pr_debug(DEV_DBG_NAME " [%s]", __func__);
+	if (keylen != DES3_EDE_KEY_SIZE) {
+		*flags |= CRYPTO_TFM_RES_BAD_KEY_LEN;
+		pr_debug(DEV_DBG_NAME " [%s]: CRYPTO_TFM_RES_BAD_KEY_LEN",
+				__func__);
+		return -EINVAL;
+	}
+
+	/* Checking key interdependency for weak key detection. */
+	if (unlikely(!((K[0] ^ K[2]) | (K[1] ^ K[3])) ||
+				!((K[2] ^ K[4]) | (K[3] ^ K[5]))) &&
+			(*flags & CRYPTO_TFM_REQ_WEAK_KEY)) {
+		*flags |= CRYPTO_TFM_RES_WEAK_KEY;
+		pr_debug(DEV_DBG_NAME " [%s]: CRYPTO_TFM_REQ_WEAK_KEY",
+				__func__);
+		return -EINVAL;
+	}
+	for (i = 0; i < 3; i++) {
+		ret = des_ekey(tmp, key + i*DES_KEY_SIZE);
+		if (unlikely(ret == 0) && (*flags & CRYPTO_TFM_REQ_WEAK_KEY)) {
+			*flags |= CRYPTO_TFM_RES_WEAK_KEY;
+			pr_debug(DEV_DBG_NAME " [%s]: "
+					"CRYPTO_TFM_REQ_WEAK_KEY", __func__);
+			return -EINVAL;
+		}
+	}
+
+	memcpy(ctx->key, key, keylen);
+	ctx->keylen = keylen;
+
+	ctx->updated = 0;
+	return 0;
+}
+
+static int cryp_blk_encrypt(struct ablkcipher_request *areq)
+{
+	struct crypto_ablkcipher *cipher = crypto_ablkcipher_reqtfm(areq);
+	struct cryp_ctx *ctx = crypto_ablkcipher_ctx(cipher);
+
+	pr_debug(DEV_DBG_NAME " [%s]", __func__);
+
+	ctx->config.algodir = CRYP_ALGORITHM_ENCRYPT;
+
+	/*
+	 * DMA does not work for DES due to a hw bug */
+	if (cryp_mode == CRYP_MODE_DMA && mode_is_aes(ctx->config.algomode))
+		return ablk_dma_crypt(areq);
+
+	/* For everything except DMA, we run the non DMA version. */
+	return ablk_crypt(areq);
+}
+
+static int cryp_blk_decrypt(struct ablkcipher_request *areq)
+{
+	struct crypto_ablkcipher *cipher = crypto_ablkcipher_reqtfm(areq);
+	struct cryp_ctx *ctx = crypto_ablkcipher_ctx(cipher);
+
+	pr_debug(DEV_DBG_NAME " [%s]", __func__);
+
+	ctx->config.algodir = CRYP_ALGORITHM_DECRYPT;
+
+	/* DMA does not work for DES due to a hw bug */
+	if (cryp_mode == CRYP_MODE_DMA && mode_is_aes(ctx->config.algomode))
+		return ablk_dma_crypt(areq);
+
+	/* For everything except DMA, we run the non DMA version. */
+	return ablk_crypt(areq);
+}
+
+struct cryp_algo_template {
+	enum cryp_algo_mode algomode;
+	struct crypto_alg crypto;
+};
+
+static int cryp_cra_init(struct crypto_tfm *tfm)
+{
+	struct cryp_ctx *ctx = crypto_tfm_ctx(tfm);
+	struct crypto_alg *alg = tfm->__crt_alg;
+	struct cryp_algo_template *cryp_alg = container_of(alg,
+			struct cryp_algo_template,
+			crypto);
+
+	ctx->config.algomode = cryp_alg->algomode;
+	ctx->blocksize = crypto_tfm_alg_blocksize(tfm);
+
+	return 0;
+}
+
+static struct cryp_algo_template cryp_algs[] = {
+	{
+		.algomode = CRYP_ALGO_AES_ECB,
+		.crypto = {
+			.cra_name = "aes",
+			.cra_driver_name = "aes-ux500",
+			.cra_priority =	300,
+			.cra_flags = CRYPTO_ALG_TYPE_ABLKCIPHER |
+					CRYPTO_ALG_ASYNC,
+			.cra_blocksize = AES_BLOCK_SIZE,
+			.cra_ctxsize = sizeof(struct cryp_ctx),
+			.cra_alignmask = 3,
+			.cra_type = &crypto_ablkcipher_type,
+			.cra_init = cryp_cra_init,
+			.cra_module = THIS_MODULE,
+			.cra_u = {
+				.ablkcipher = {
+					.min_keysize = AES_MIN_KEY_SIZE,
+					.max_keysize = AES_MAX_KEY_SIZE,
+					.setkey = aes_ablkcipher_setkey,
+					.encrypt = cryp_blk_encrypt,
+					.decrypt = cryp_blk_decrypt
+				}
+			}
+		}
+	},
+	{
+		.algomode = CRYP_ALGO_AES_ECB,
+		.crypto = {
+			.cra_name = "ecb(aes)",
+			.cra_driver_name = "ecb-aes-ux500",
+			.cra_priority = 300,
+			.cra_flags = CRYPTO_ALG_TYPE_ABLKCIPHER |
+					CRYPTO_ALG_ASYNC,
+			.cra_blocksize = AES_BLOCK_SIZE,
+			.cra_ctxsize = sizeof(struct cryp_ctx),
+			.cra_alignmask = 3,
+			.cra_type = &crypto_ablkcipher_type,
+			.cra_init = cryp_cra_init,
+			.cra_module = THIS_MODULE,
+			.cra_u = {
+				.ablkcipher = {
+					.min_keysize = AES_MIN_KEY_SIZE,
+					.max_keysize = AES_MAX_KEY_SIZE,
+					.setkey = aes_ablkcipher_setkey,
+					.encrypt = cryp_blk_encrypt,
+					.decrypt = cryp_blk_decrypt,
+				}
+			}
+		}
+	},
+	{
+		.algomode = CRYP_ALGO_AES_CBC,
+		.crypto = {
+			.cra_name = "cbc(aes)",
+			.cra_driver_name = "cbc-aes-ux500",
+			.cra_priority = 300,
+			.cra_flags = CRYPTO_ALG_TYPE_ABLKCIPHER |
+					CRYPTO_ALG_ASYNC,
+			.cra_blocksize = AES_BLOCK_SIZE,
+			.cra_ctxsize = sizeof(struct cryp_ctx),
+			.cra_alignmask = 3,
+			.cra_type = &crypto_ablkcipher_type,
+			.cra_init = cryp_cra_init,
+			.cra_module = THIS_MODULE,
+			.cra_u = {
+				.ablkcipher = {
+					.min_keysize = AES_MIN_KEY_SIZE,
+					.max_keysize = AES_MAX_KEY_SIZE,
+					.setkey = aes_ablkcipher_setkey,
+					.encrypt = cryp_blk_encrypt,
+					.decrypt = cryp_blk_decrypt,
+					.ivsize = AES_BLOCK_SIZE,
+				}
+			}
+		}
+	},
+	{
+		.algomode = CRYP_ALGO_AES_CTR,
+		.crypto = {
+			.cra_name = "ctr(aes)",
+			.cra_driver_name = "ctr-aes-ux500",
+			.cra_priority = 300,
+			.cra_flags = CRYPTO_ALG_TYPE_ABLKCIPHER |
+						CRYPTO_ALG_ASYNC,
+			.cra_blocksize = AES_BLOCK_SIZE,
+			.cra_ctxsize = sizeof(struct cryp_ctx),
+			.cra_alignmask = 3,
+			.cra_type = &crypto_ablkcipher_type,
+			.cra_init = cryp_cra_init,
+			.cra_module = THIS_MODULE,
+			.cra_u = {
+				.ablkcipher = {
+					.min_keysize = AES_MIN_KEY_SIZE,
+					.max_keysize = AES_MAX_KEY_SIZE,
+					.setkey = aes_ablkcipher_setkey,
+					.encrypt = cryp_blk_encrypt,
+					.decrypt = cryp_blk_decrypt,
+					.ivsize = AES_BLOCK_SIZE,
+				}
+			}
+		}
+	},
+	{
+		.algomode = CRYP_ALGO_DES_ECB,
+		.crypto = {
+			.cra_name = "des",
+			.cra_driver_name = "des-ux500",
+			.cra_priority = 300,
+			.cra_flags = CRYPTO_ALG_TYPE_ABLKCIPHER |
+						CRYPTO_ALG_ASYNC,
+			.cra_blocksize = DES_BLOCK_SIZE,
+			.cra_ctxsize = sizeof(struct cryp_ctx),
+			.cra_alignmask = 3,
+			.cra_type = &crypto_ablkcipher_type,
+			.cra_init = cryp_cra_init,
+			.cra_module = THIS_MODULE,
+			.cra_u = {
+				.ablkcipher = {
+					.min_keysize = DES_KEY_SIZE,
+					.max_keysize = DES_KEY_SIZE,
+					.setkey = des_ablkcipher_setkey,
+					.encrypt = cryp_blk_encrypt,
+					.decrypt = cryp_blk_decrypt
+				}
+			}
+		}
+
+	},
+	{
+		.algomode = CRYP_ALGO_TDES_ECB,
+		.crypto = {
+			.cra_name = "des3_ede",
+			.cra_driver_name = "des3_ede-ux500",
+			.cra_priority = 300,
+			.cra_flags = CRYPTO_ALG_TYPE_ABLKCIPHER |
+						CRYPTO_ALG_ASYNC,
+			.cra_blocksize = DES3_EDE_BLOCK_SIZE,
+			.cra_ctxsize = sizeof(struct cryp_ctx),
+			.cra_alignmask = 3,
+			.cra_type = &crypto_ablkcipher_type,
+			.cra_init = cryp_cra_init,
+			.cra_module = THIS_MODULE,
+			.cra_u = {
+				.ablkcipher = {
+					.min_keysize = DES3_EDE_KEY_SIZE,
+					.max_keysize = DES3_EDE_KEY_SIZE,
+					.setkey = des_ablkcipher_setkey,
+					.encrypt = cryp_blk_encrypt,
+					.decrypt = cryp_blk_decrypt
+				}
+			}
+		}
+	},
+	{
+		.algomode = CRYP_ALGO_DES_ECB,
+		.crypto = {
+			.cra_name = "ecb(des)",
+			.cra_driver_name = "ecb-des-ux500",
+			.cra_priority = 300,
+			.cra_flags = CRYPTO_ALG_TYPE_ABLKCIPHER |
+					CRYPTO_ALG_ASYNC,
+			.cra_blocksize = DES_BLOCK_SIZE,
+			.cra_ctxsize = sizeof(struct cryp_ctx),
+			.cra_alignmask = 3,
+			.cra_type = &crypto_ablkcipher_type,
+			.cra_init = cryp_cra_init,
+			.cra_module = THIS_MODULE,
+			.cra_u = {
+				.ablkcipher = {
+					.min_keysize = DES_KEY_SIZE,
+					.max_keysize = DES_KEY_SIZE,
+					.setkey = des_ablkcipher_setkey,
+					.encrypt = cryp_blk_encrypt,
+					.decrypt = cryp_blk_decrypt,
+				}
+			}
+		}
+	},
+	{
+		.algomode = CRYP_ALGO_TDES_ECB,
+		.crypto = {
+			.cra_name = "ecb(des3_ede)",
+			.cra_driver_name = "ecb-des3_ede-ux500",
+			.cra_priority = 300,
+			.cra_flags = CRYPTO_ALG_TYPE_ABLKCIPHER |
+					CRYPTO_ALG_ASYNC,
+			.cra_blocksize = DES3_EDE_BLOCK_SIZE,
+			.cra_ctxsize = sizeof(struct cryp_ctx),
+			.cra_alignmask = 3,
+			.cra_type = &crypto_ablkcipher_type,
+			.cra_init = cryp_cra_init,
+			.cra_module = THIS_MODULE,
+			.cra_u = {
+				.ablkcipher = {
+					.min_keysize = DES3_EDE_KEY_SIZE,
+					.max_keysize = DES3_EDE_KEY_SIZE,
+					.setkey = des3_ablkcipher_setkey,
+					.encrypt = cryp_blk_encrypt,
+					.decrypt = cryp_blk_decrypt,
+				}
+			}
+		}
+	},
+	{
+		.algomode = CRYP_ALGO_DES_CBC,
+		.crypto = {
+			.cra_name = "cbc(des)",
+			.cra_driver_name = "cbc-des-ux500",
+			.cra_priority = 300,
+			.cra_flags = CRYPTO_ALG_TYPE_ABLKCIPHER |
+					CRYPTO_ALG_ASYNC,
+			.cra_blocksize = DES_BLOCK_SIZE,
+			.cra_ctxsize = sizeof(struct cryp_ctx),
+			.cra_alignmask = 3,
+			.cra_type = &crypto_ablkcipher_type,
+			.cra_init = cryp_cra_init,
+			.cra_module = THIS_MODULE,
+			.cra_u = {
+				.ablkcipher = {
+					.min_keysize = DES_KEY_SIZE,
+					.max_keysize = DES_KEY_SIZE,
+					.setkey = des_ablkcipher_setkey,
+					.encrypt = cryp_blk_encrypt,
+					.decrypt = cryp_blk_decrypt,
+				}
+			}
+		}
+	},
+	{
+		.algomode = CRYP_ALGO_TDES_CBC,
+		.crypto = {
+			.cra_name = "cbc(des3_ede)",
+			.cra_driver_name = "cbc-des3_ede-ux500",
+			.cra_priority = 300,
+			.cra_flags = CRYPTO_ALG_TYPE_ABLKCIPHER |
+					CRYPTO_ALG_ASYNC,
+			.cra_blocksize = DES3_EDE_BLOCK_SIZE,
+			.cra_ctxsize = sizeof(struct cryp_ctx),
+			.cra_alignmask = 3,
+			.cra_type = &crypto_ablkcipher_type,
+			.cra_init = cryp_cra_init,
+			.cra_module = THIS_MODULE,
+			.cra_u = {
+				.ablkcipher = {
+					.min_keysize = DES3_EDE_KEY_SIZE,
+					.max_keysize = DES3_EDE_KEY_SIZE,
+					.setkey = des3_ablkcipher_setkey,
+					.encrypt = cryp_blk_encrypt,
+					.decrypt = cryp_blk_decrypt,
+					.ivsize = DES3_EDE_BLOCK_SIZE,
+				}
+			}
+		}
+	}
+};
+
+/**
+ * cryp_algs_register_all -
+ */
+static int cryp_algs_register_all(void)
+{
+	int ret;
+	int i;
+	int count;
+
+	pr_debug("[%s]", __func__);
+
+	for (i = 0; i < ARRAY_SIZE(cryp_algs); i++) {
+		ret = crypto_register_alg(&cryp_algs[i].crypto);
+		if (ret) {
+			count = i;
+			pr_err("[%s] alg registration failed",
+					cryp_algs[i].crypto.cra_driver_name);
+			goto unreg;
+		}
+	}
+	return 0;
+unreg:
+	for (i = 0; i < count; i++)
+		crypto_unregister_alg(&cryp_algs[i].crypto);
+	return ret;
+}
+
+/**
+ * cryp_algs_unregister_all -
+ */
+static void cryp_algs_unregister_all(void)
+{
+	int i;
+
+	pr_debug(DEV_DBG_NAME " [%s]", __func__);
+
+	for (i = 0; i < ARRAY_SIZE(cryp_algs); i++)
+		crypto_unregister_alg(&cryp_algs[i].crypto);
+}
+
+static int ux500_cryp_probe(struct platform_device *pdev)
+{
+	int ret;
+	int cryp_error = 0;
+	struct resource *res = NULL;
+	struct resource *res_irq = NULL;
+	struct cryp_device_data *device_data;
+	struct cryp_protection_config prot = {
+		.privilege_access = CRYP_STATE_ENABLE
+	};
+	struct device *dev = &pdev->dev;
+
+	dev_dbg(dev, "[%s]", __func__);
+	device_data = kzalloc(sizeof(struct cryp_device_data), GFP_ATOMIC);
+	if (!device_data) {
+		dev_err(dev, "[%s]: kzalloc() failed!", __func__);
+		ret = -ENOMEM;
+		goto out;
+	}
+
+	device_data->dev = dev;
+	device_data->current_ctx = NULL;
+
+	/* Grab the DMA configuration from platform data. */
+	mem_to_engine = &((struct cryp_platform_data *)
+			 dev->platform_data)->mem_to_engine;
+	engine_to_mem = &((struct cryp_platform_data *)
+			 dev->platform_data)->engine_to_mem;
+
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	if (!res) {
+		dev_err(dev, "[%s]: platform_get_resource() failed",
+				__func__);
+		ret = -ENODEV;
+		goto out_kfree;
+	}
+
+	res = request_mem_region(res->start, resource_size(res), pdev->name);
+	if (res == NULL) {
+		dev_err(dev, "[%s]: request_mem_region() failed",
+				__func__);
+		ret = -EBUSY;
+		goto out_kfree;
+	}
+
+	device_data->base = ioremap(res->start, resource_size(res));
+	if (!device_data->base) {
+		dev_err(dev, "[%s]: ioremap failed!", __func__);
+		ret = -ENOMEM;
+		goto out_free_mem;
+	}
+
+	spin_lock_init(&device_data->ctx_lock);
+	spin_lock_init(&device_data->power_state_spinlock);
+
+	/* Enable power for CRYP hardware block */
+	device_data->pwr_regulator = regulator_get(&pdev->dev, "v-ape");
+	if (IS_ERR(device_data->pwr_regulator)) {
+		dev_err(dev, "[%s]: could not get cryp regulator", __func__);
+		ret = PTR_ERR(device_data->pwr_regulator);
+		device_data->pwr_regulator = NULL;
+		goto out_unmap;
+	}
+
+	/* Enable the clk for CRYP hardware block */
+	device_data->clk = clk_get(&pdev->dev, NULL);
+	if (IS_ERR(device_data->clk)) {
+		dev_err(dev, "[%s]: clk_get() failed!", __func__);
+		ret = PTR_ERR(device_data->clk);
+		goto out_regulator;
+	}
+
+	/* Enable device power (and clock) */
+	ret = cryp_enable_power(device_data->dev, device_data, false);
+	if (ret) {
+		dev_err(dev, "[%s]: cryp_enable_power() failed!", __func__);
+		goto out_clk;
+	}
+
+	cryp_error = cryp_check(device_data);
+	if (cryp_error != 0) {
+		dev_err(dev, "[%s]: cryp_init() failed!", __func__);
+		ret = -EINVAL;
+		goto out_power;
+	}
+
+	cryp_error = cryp_configure_protection(device_data, &prot);
+	if (cryp_error != 0) {
+		dev_err(dev, "[%s]: cryp_configure_protection() failed!",
+			__func__);
+		ret = -EINVAL;
+		goto out_power;
+	}
+
+	res_irq = platform_get_resource(pdev, IORESOURCE_IRQ, 0);
+	if (!res_irq) {
+		dev_err(dev, "[%s]: IORESOURCE_IRQ unavailable",
+			__func__);
+		goto out_power;
+	}
+
+	ret = request_irq(res_irq->start,
+			  cryp_interrupt_handler,
+			  0,
+			  "cryp1",
+			  device_data);
+	if (ret) {
+		dev_err(dev, "[%s]: Unable to request IRQ", __func__);
+		goto out_power;
+	}
+
+	if (cryp_mode == CRYP_MODE_DMA)
+		cryp_dma_setup_channel(device_data, dev);
+
+	platform_set_drvdata(pdev, device_data);
+
+	/* Put the new device into the device list... */
+	klist_add_tail(&device_data->list_node, &driver_data.device_list);
+
+	/* ... and signal that a new device is available. */
+	up(&driver_data.device_allocation);
+
+	atomic_set(&session_id, 1);
+
+	ret = cryp_algs_register_all();
+	if (ret) {
+		dev_err(dev, "[%s]: cryp_algs_register_all() failed!",
+			__func__);
+		goto out_power;
+	}
+
+	return 0;
+
+out_power:
+	cryp_disable_power(device_data->dev, device_data, false);
+
+out_clk:
+	clk_put(device_data->clk);
+
+out_regulator:
+	regulator_put(device_data->pwr_regulator);
+
+out_unmap:
+	iounmap(device_data->base);
+
+out_free_mem:
+	release_mem_region(res->start, resource_size(res));
+
+out_kfree:
+	kfree(device_data);
+out:
+	return ret;
+}
+
+static int ux500_cryp_remove(struct platform_device *pdev)
+{
+	struct resource *res = NULL;
+	struct resource *res_irq = NULL;
+	struct cryp_device_data *device_data;
+
+	dev_dbg(&pdev->dev, "[%s]", __func__);
+	device_data = platform_get_drvdata(pdev);
+	if (!device_data) {
+		dev_err(&pdev->dev, "[%s]: platform_get_drvdata() failed!",
+			__func__);
+		return -ENOMEM;
+	}
+
+	/* Try to decrease the number of available devices. */
+	if (down_trylock(&driver_data.device_allocation))
+		return -EBUSY;
+
+	/* Check that the device is free */
+	spin_lock(&device_data->ctx_lock);
+	/* current_ctx allocates a device, NULL = unallocated */
+	if (device_data->current_ctx) {
+		/* The device is busy */
+		spin_unlock(&device_data->ctx_lock);
+		/* Return the device to the pool. */
+		up(&driver_data.device_allocation);
+		return -EBUSY;
+	}
+
+	spin_unlock(&device_data->ctx_lock);
+
+	/* Remove the device from the list */
+	if (klist_node_attached(&device_data->list_node))
+		klist_remove(&device_data->list_node);
+
+	/* If this was the last device, remove the services */
+	if (list_empty(&driver_data.device_list.k_list))
+		cryp_algs_unregister_all();
+
+	res_irq = platform_get_resource(pdev, IORESOURCE_IRQ, 0);
+	if (!res_irq)
+		dev_err(&pdev->dev, "[%s]: IORESOURCE_IRQ, unavailable",
+			__func__);
+	else {
+		disable_irq(res_irq->start);
+		free_irq(res_irq->start, device_data);
+	}
+
+	if (cryp_disable_power(&pdev->dev, device_data, false))
+		dev_err(&pdev->dev, "[%s]: cryp_disable_power() failed",
+			__func__);
+
+	clk_put(device_data->clk);
+	regulator_put(device_data->pwr_regulator);
+
+	iounmap(device_data->base);
+
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	if (res)
+		release_mem_region(res->start, res->end - res->start + 1);
+
+	kfree(device_data);
+
+	return 0;
+}
+
+static void ux500_cryp_shutdown(struct platform_device *pdev)
+{
+	struct resource *res_irq = NULL;
+	struct cryp_device_data *device_data;
+
+	dev_dbg(&pdev->dev, "[%s]", __func__);
+
+	device_data = platform_get_drvdata(pdev);
+	if (!device_data) {
+		dev_err(&pdev->dev, "[%s]: platform_get_drvdata() failed!",
+			__func__);
+		return;
+	}
+
+	/* Check that the device is free */
+	spin_lock(&device_data->ctx_lock);
+	/* current_ctx allocates a device, NULL = unallocated */
+	if (!device_data->current_ctx) {
+		if (down_trylock(&driver_data.device_allocation))
+			dev_dbg(&pdev->dev, "[%s]: Cryp still in use!"
+				"Shutting down anyway...", __func__);
+		/**
+		 * (Allocate the device)
+		 * Need to set this to non-null (dummy) value,
+		 * to avoid usage if context switching.
+		 */
+		device_data->current_ctx++;
+	}
+	spin_unlock(&device_data->ctx_lock);
+
+	/* Remove the device from the list */
+	if (klist_node_attached(&device_data->list_node))
+		klist_remove(&device_data->list_node);
+
+	/* If this was the last device, remove the services */
+	if (list_empty(&driver_data.device_list.k_list))
+		cryp_algs_unregister_all();
+
+	res_irq = platform_get_resource(pdev, IORESOURCE_IRQ, 0);
+	if (!res_irq)
+		dev_err(&pdev->dev, "[%s]: IORESOURCE_IRQ, unavailable",
+			__func__);
+	else {
+		disable_irq(res_irq->start);
+		free_irq(res_irq->start, device_data);
+	}
+
+	if (cryp_disable_power(&pdev->dev, device_data, false))
+		dev_err(&pdev->dev, "[%s]: cryp_disable_power() failed",
+			__func__);
+
+}
+
+static int ux500_cryp_suspend(struct platform_device *pdev, pm_message_t state)
+{
+	int ret;
+	struct cryp_device_data *device_data;
+	struct resource *res_irq;
+	struct cryp_ctx *temp_ctx = NULL;
+
+	dev_dbg(&pdev->dev, "[%s]", __func__);
+
+	/* Handle state? */
+	device_data = platform_get_drvdata(pdev);
+	if (!device_data) {
+		dev_err(&pdev->dev, "[%s]: platform_get_drvdata() failed!",
+			__func__);
+		return -ENOMEM;
+	}
+
+	res_irq = platform_get_resource(pdev, IORESOURCE_IRQ, 0);
+	if (!res_irq)
+		dev_err(&pdev->dev, "[%s]: IORESOURCE_IRQ, unavailable",
+			__func__);
+	else
+		disable_irq(res_irq->start);
+
+	spin_lock(&device_data->ctx_lock);
+	if (!device_data->current_ctx)
+		device_data->current_ctx++;
+	spin_unlock(&device_data->ctx_lock);
+
+	if (device_data->current_ctx == ++temp_ctx) {
+		if (down_interruptible(&driver_data.device_allocation))
+			dev_dbg(&pdev->dev, "[%s]: down_interruptible() "
+					"failed", __func__);
+		ret = cryp_disable_power(&pdev->dev, device_data, false);
+
+	} else
+		ret = cryp_disable_power(&pdev->dev, device_data, true);
+
+	if (ret)
+		dev_err(&pdev->dev, "[%s]: cryp_disable_power()", __func__);
+
+	return ret;
+}
+
+static int ux500_cryp_resume(struct platform_device *pdev)
+{
+	int ret = 0;
+	struct cryp_device_data *device_data;
+	struct resource *res_irq;
+	struct cryp_ctx *temp_ctx = NULL;
+
+	dev_dbg(&pdev->dev, "[%s]", __func__);
+
+	device_data = platform_get_drvdata(pdev);
+	if (!device_data) {
+		dev_err(&pdev->dev, "[%s]: platform_get_drvdata() failed!",
+			__func__);
+		return -ENOMEM;
+	}
+
+	spin_lock(&device_data->ctx_lock);
+	if (device_data->current_ctx == ++temp_ctx)
+		device_data->current_ctx = NULL;
+	spin_unlock(&device_data->ctx_lock);
+
+
+	if (!device_data->current_ctx)
+		up(&driver_data.device_allocation);
+	else
+		ret = cryp_enable_power(&pdev->dev, device_data, true);
+
+	if (ret)
+		dev_err(&pdev->dev, "[%s]: cryp_enable_power() failed!",
+			__func__);
+	else {
+		res_irq = platform_get_resource(pdev, IORESOURCE_IRQ, 0);
+		if (res_irq)
+			enable_irq(res_irq->start);
+	}
+
+	return ret;
+}
+
+static struct platform_driver cryp_driver = {
+	.probe  = ux500_cryp_probe,
+	.remove = ux500_cryp_remove,
+	.shutdown = ux500_cryp_shutdown,
+	.suspend  = ux500_cryp_suspend,
+	.resume   = ux500_cryp_resume,
+	.driver = {
+		.owner = THIS_MODULE,
+		.name  = "cryp1"
+	}
+};
+
+static int __init ux500_cryp_mod_init(void)
+{
+	pr_debug("[%s] is called!", __func__);
+	klist_init(&driver_data.device_list, NULL, NULL);
+	/* Initialize the semaphore to 0 devices (locked state) */
+	sema_init(&driver_data.device_allocation, 0);
+	return platform_driver_register(&cryp_driver);
+}
+
+static void __exit ux500_cryp_mod_fini(void)
+{
+	pr_debug("[%s] is called!", __func__);
+	platform_driver_unregister(&cryp_driver);
+	return;
+}
+
+module_init(ux500_cryp_mod_init);
+module_exit(ux500_cryp_mod_fini);
+
+module_param(cryp_mode, int, 0);
+
+MODULE_DESCRIPTION("Driver for ST-Ericsson UX500 CRYP crypto engine.");
+MODULE_ALIAS("aes-all");
+MODULE_ALIAS("des-all");
+
+MODULE_LICENSE("GPL");
diff --git a/drivers/crypto/ux500/cryp/cryp_irq.c b/drivers/crypto/ux500/cryp/cryp_irq.c
new file mode 100644
index 00000000000..08d291cdbe6
--- /dev/null
+++ b/drivers/crypto/ux500/cryp/cryp_irq.c
@@ -0,0 +1,45 @@
+/**
+ * Copyright (C) ST-Ericsson SA 2010
+ * Author: Shujuan Chen <shujuan.chen@stericsson.com> for ST-Ericsson.
+ * Author: Jonas Linde <jonas.linde@stericsson.com> for ST-Ericsson.
+ * Author: Joakim Bech <joakim.xx.bech@stericsson.com> for ST-Ericsson.
+ * Author: Berne Hebark <berne.herbark@stericsson.com> for ST-Ericsson.
+ * Author: Niklas Hernaeus <niklas.hernaeus@stericsson.com> for ST-Ericsson.
+ * License terms: GNU General Public License (GPL) version 2.
+ */
+
+#include <linux/kernel.h>
+#include <linux/bitmap.h>
+#include <linux/device.h>
+
+#include "cryp.h"
+#include "cryp_p.h"
+#include "cryp_irq.h"
+#include "cryp_irqp.h"
+
+void cryp_enable_irq_src(struct cryp_device_data *device_data, u32 irq_src)
+{
+	u32 i;
+
+	dev_dbg(device_data->dev, "[%s]", __func__);
+
+	i = readl_relaxed(&device_data->base->imsc);
+	i = i | irq_src;
+	writel_relaxed(i, &device_data->base->imsc);
+}
+
+void cryp_disable_irq_src(struct cryp_device_data *device_data, u32 irq_src)
+{
+	u32 i;
+
+	dev_dbg(device_data->dev, "[%s]", __func__);
+
+	i = readl_relaxed(&device_data->base->imsc);
+	i = i & ~irq_src;
+	writel_relaxed(i, &device_data->base->imsc);
+}
+
+bool cryp_pending_irq_src(struct cryp_device_data *device_data, u32 irq_src)
+{
+	return (readl_relaxed(&device_data->base->mis) & irq_src) > 0;
+}
diff --git a/drivers/crypto/ux500/cryp/cryp_irq.h b/drivers/crypto/ux500/cryp/cryp_irq.h
new file mode 100644
index 00000000000..5a7837f1b8f
--- /dev/null
+++ b/drivers/crypto/ux500/cryp/cryp_irq.h
@@ -0,0 +1,31 @@
+/**
+ * Copyright (C) ST-Ericsson SA 2010
+ * Author: Shujuan Chen <shujuan.chen@stericsson.com> for ST-Ericsson.
+ * Author: Jonas Linde <jonas.linde@stericsson.com> for ST-Ericsson.
+ * Author: Joakim Bech <joakim.xx.bech@stericsson.com> for ST-Ericsson.
+ * Author: Berne Hebark <berne.herbark@stericsson.com> for ST-Ericsson.
+ * Author: Niklas Hernaeus <niklas.hernaeus@stericsson.com> for ST-Ericsson.
+ * License terms: GNU General Public License (GPL) version 2
+ */
+
+#ifndef _CRYP_IRQ_H_
+#define _CRYP_IRQ_H_
+
+#include "cryp.h"
+
+enum cryp_irq_src_id {
+	CRYP_IRQ_SRC_INPUT_FIFO = 0x1,
+	CRYP_IRQ_SRC_OUTPUT_FIFO = 0x2,
+	CRYP_IRQ_SRC_ALL = 0x3
+};
+
+/**
+ * M0 Funtions
+ */
+void cryp_enable_irq_src(struct cryp_device_data *device_data, u32 irq_src);
+
+void cryp_disable_irq_src(struct cryp_device_data *device_data, u32 irq_src);
+
+bool cryp_pending_irq_src(struct cryp_device_data *device_data, u32 irq_src);
+
+#endif				/* _CRYP_IRQ_H_ */
diff --git a/drivers/crypto/ux500/cryp/cryp_irqp.h b/drivers/crypto/ux500/cryp/cryp_irqp.h
new file mode 100644
index 00000000000..8b339cc34bf
--- /dev/null
+++ b/drivers/crypto/ux500/cryp/cryp_irqp.h
@@ -0,0 +1,125 @@
+/**
+ * Copyright (C) ST-Ericsson SA 2010
+ * Author: Shujuan Chen <shujuan.chen@stericsson.com> for ST-Ericsson.
+ * Author: Jonas Linde <jonas.linde@stericsson.com> for ST-Ericsson.
+ * Author: Joakim Bech <joakim.xx.bech@stericsson.com> for ST-Ericsson.
+ * Author: Berne Hebark <berne.herbark@stericsson.com> for ST-Ericsson.
+ * Author: Niklas Hernaeus <niklas.hernaeus@stericsson.com> for ST-Ericsson.
+ * License terms: GNU General Public License (GPL) version 2
+ */
+
+#ifndef __CRYP_IRQP_H_
+#define __CRYP_IRQP_H_
+
+#include "cryp_irq.h"
+
+/**
+ *
+ * CRYP Registers - Offset mapping
+ *     +-----------------+
+ * 00h | CRYP_CR         |  Configuration register
+ *     +-----------------+
+ * 04h | CRYP_SR         |  Status register
+ *     +-----------------+
+ * 08h | CRYP_DIN        |  Data In register
+ *     +-----------------+
+ * 0ch | CRYP_DOUT       |  Data out register
+ *     +-----------------+
+ * 10h | CRYP_DMACR      |  DMA control register
+ *     +-----------------+
+ * 14h | CRYP_IMSC       |  IMSC
+ *     +-----------------+
+ * 18h | CRYP_RIS        |  Raw interrupt status
+ *     +-----------------+
+ * 1ch | CRYP_MIS        |  Masked interrupt status.
+ *     +-----------------+
+ *       Key registers
+ *       IVR registers
+ *       Peripheral
+ *       Cell IDs
+ *
+ *       Refer data structure for other register map
+ */
+
+/**
+ * struct cryp_register
+ * @cr			- Configuration register
+ * @status		- Status register
+ * @din			- Data input register
+ * @din_size		- Data input size register
+ * @dout		- Data output register
+ * @dout_size		- Data output size register
+ * @dmacr		- Dma control register
+ * @imsc		- Interrupt mask set/clear register
+ * @ris			- Raw interrupt status
+ * @mis			- Masked interrupt statu register
+ * @key_1_l		- Key register 1 L
+ * @key_1_r		- Key register 1 R
+ * @key_2_l		- Key register 2 L
+ * @key_2_r		- Key register 2 R
+ * @key_3_l		- Key register 3 L
+ * @key_3_r		- Key register 3 R
+ * @key_4_l		- Key register 4 L
+ * @key_4_r		- Key register 4 R
+ * @init_vect_0_l	- init vector 0 L
+ * @init_vect_0_r	- init vector 0 R
+ * @init_vect_1_l	- init vector 1 L
+ * @init_vect_1_r	- init vector 1 R
+ * @cryp_unused1	- unused registers
+ * @itcr		- Integration test control register
+ * @itip		- Integration test input register
+ * @itop		- Integration test output register
+ * @cryp_unused2	- unused registers
+ * @periphId0		- FE0 CRYP Peripheral Identication Register
+ * @periphId1		- FE4
+ * @periphId2		- FE8
+ * @periphId3		- FEC
+ * @pcellId0		- FF0  CRYP PCell Identication Register
+ * @pcellId1		- FF4
+ * @pcellId2		- FF8
+ * @pcellId3		- FFC
+ */
+struct cryp_register {
+	u32 cr;			/* Configuration register   */
+	u32 sr;			/* Status register          */
+	u32 din;		/* Data input register      */
+	u32 din_size;		/* Data input size register */
+	u32 dout;		/* Data output register     */
+	u32 dout_size;		/* Data output size register */
+	u32 dmacr;		/* Dma control register     */
+	u32 imsc;		/* Interrupt mask set/clear register */
+	u32 ris;		/* Raw interrupt status             */
+	u32 mis;		/* Masked interrupt statu register  */
+
+	u32 key_1_l;		/*Key register 1 L */
+	u32 key_1_r;		/*Key register 1 R */
+	u32 key_2_l;		/*Key register 2 L */
+	u32 key_2_r;		/*Key register 2 R */
+	u32 key_3_l;		/*Key register 3 L */
+	u32 key_3_r;		/*Key register 3 R */
+	u32 key_4_l;		/*Key register 4 L */
+	u32 key_4_r;		/*Key register 4 R */
+
+	u32 init_vect_0_l;	/*init vector 0 L */
+	u32 init_vect_0_r;	/*init vector 0 R */
+	u32 init_vect_1_l;	/*init vector 1 L */
+	u32 init_vect_1_r;	/*init vector 1 R */
+
+	u32 cryp_unused1[(0x80 - 0x58) / sizeof(u32)];	/* unused registers */
+	u32 itcr;		/*Integration test control register */
+	u32 itip;		/*Integration test input register */
+	u32 itop;		/*Integration test output register */
+	u32 cryp_unused2[(0xFE0 - 0x8C) / sizeof(u32)];	/* unused registers */
+
+	u32 periphId0;		/* FE0  CRYP Peripheral Identication Register */
+	u32 periphId1;		/* FE4 */
+	u32 periphId2;		/* FE8 */
+	u32 periphId3;		/* FEC */
+
+	u32 pcellId0;		/* FF0  CRYP PCell Identication Register */
+	u32 pcellId1;		/* FF4 */
+	u32 pcellId2;		/* FF8 */
+	u32 pcellId3;		/* FFC */
+};
+
+#endif
diff --git a/drivers/crypto/ux500/cryp/cryp_p.h b/drivers/crypto/ux500/cryp/cryp_p.h
new file mode 100644
index 00000000000..6dcffe15c2b
--- /dev/null
+++ b/drivers/crypto/ux500/cryp/cryp_p.h
@@ -0,0 +1,123 @@
+/**
+ * Copyright (C) ST-Ericsson SA 2010
+ * Author: Shujuan Chen <shujuan.chen@stericsson.com> for ST-Ericsson.
+ * Author: Jonas Linde <jonas.linde@stericsson.com> for ST-Ericsson.
+ * Author: Joakim Bech <joakim.xx.bech@stericsson.com> for ST-Ericsson.
+ * Author: Berne Hebark <berne.herbark@stericsson.com> for ST-Ericsson.
+ * Author: Niklas Hernaeus <niklas.hernaeus@stericsson.com> for ST-Ericsson.
+ * License terms: GNU General Public License (GPL) version 2
+ */
+
+#ifndef _CRYP_P_H_
+#define _CRYP_P_H_
+
+#include <linux/io.h>
+#include <linux/bitops.h>
+
+#include "cryp.h"
+#include "cryp_irqp.h"
+
+/**
+ * Generic Macros
+ */
+#define CRYP_SET_BITS(reg_name, mask) \
+	writel_relaxed((readl_relaxed(reg_name) | mask), reg_name)
+
+#define CRYP_WRITE_BIT(reg_name, val, mask) \
+	writel_relaxed(((readl_relaxed(reg_name) & ~(mask)) |\
+			((val) & (mask))), reg_name)
+
+#define CRYP_TEST_BITS(reg_name, val) \
+	(readl_relaxed(reg_name) & (val))
+
+#define CRYP_PUT_BITS(reg, val, shift, mask) \
+	writel_relaxed(((readl_relaxed(reg) & ~(mask)) | \
+		(((u32)val << shift) & (mask))), reg)
+
+/**
+ * CRYP specific Macros
+ */
+#define CRYP_PERIPHERAL_ID0		0xE3
+#define CRYP_PERIPHERAL_ID1		0x05
+
+#define CRYP_PERIPHERAL_ID2_DB8500	0x28
+#define CRYP_PERIPHERAL_ID3		0x00
+
+#define CRYP_PCELL_ID0			0x0D
+#define CRYP_PCELL_ID1			0xF0
+#define CRYP_PCELL_ID2			0x05
+#define CRYP_PCELL_ID3			0xB1
+
+/**
+ * CRYP register default values
+ */
+#define MAX_DEVICE_SUPPORT		2
+
+/* Priv set, keyrden set and datatype 8bits swapped set as default. */
+#define CRYP_CR_DEFAULT			0x0482
+#define CRYP_DMACR_DEFAULT		0x0
+#define CRYP_IMSC_DEFAULT		0x0
+#define CRYP_DIN_DEFAULT		0x0
+#define CRYP_DOUT_DEFAULT		0x0
+#define CRYP_KEY_DEFAULT		0x0
+#define CRYP_INIT_VECT_DEFAULT		0x0
+
+/**
+ * CRYP Control register specific mask
+ */
+#define CRYP_CR_SECURE_MASK		BIT(0)
+#define CRYP_CR_PRLG_MASK		BIT(1)
+#define CRYP_CR_ALGODIR_MASK		BIT(2)
+#define CRYP_CR_ALGOMODE_MASK		(BIT(5) | BIT(4) | BIT(3))
+#define CRYP_CR_DATATYPE_MASK		(BIT(7) | BIT(6))
+#define CRYP_CR_KEYSIZE_MASK		(BIT(9) | BIT(8))
+#define CRYP_CR_KEYRDEN_MASK		BIT(10)
+#define CRYP_CR_KSE_MASK		BIT(11)
+#define CRYP_CR_START_MASK		BIT(12)
+#define CRYP_CR_INIT_MASK		BIT(13)
+#define CRYP_CR_FFLUSH_MASK		BIT(14)
+#define CRYP_CR_CRYPEN_MASK		BIT(15)
+#define CRYP_CR_CONTEXT_SAVE_MASK	(CRYP_CR_SECURE_MASK |\
+					 CRYP_CR_PRLG_MASK |\
+					 CRYP_CR_ALGODIR_MASK |\
+					 CRYP_CR_ALGOMODE_MASK |\
+					 CRYP_CR_DATATYPE_MASK |\
+					 CRYP_CR_KEYSIZE_MASK |\
+					 CRYP_CR_KEYRDEN_MASK |\
+					 CRYP_CR_DATATYPE_MASK)
+
+
+#define CRYP_SR_INFIFO_READY_MASK	(BIT(0) | BIT(1))
+#define CRYP_SR_IFEM_MASK		BIT(0)
+#define CRYP_SR_BUSY_MASK		BIT(4)
+
+/**
+ * Bit position used while setting bits in register
+ */
+#define CRYP_CR_PRLG_POS		1
+#define CRYP_CR_ALGODIR_POS		2
+#define CRYP_CR_ALGOMODE_POS		3
+#define CRYP_CR_DATATYPE_POS		6
+#define CRYP_CR_KEYSIZE_POS		8
+#define CRYP_CR_KEYRDEN_POS		10
+#define CRYP_CR_KSE_POS			11
+#define CRYP_CR_START_POS		12
+#define CRYP_CR_INIT_POS		13
+#define CRYP_CR_CRYPEN_POS		15
+
+#define CRYP_SR_BUSY_POS		4
+
+/**
+ * CRYP PCRs------PC_NAND control register
+ * BIT_MASK
+ */
+#define CRYP_DMA_REQ_MASK		(BIT(1) | BIT(0))
+#define CRYP_DMA_REQ_MASK_POS		0
+
+
+struct cryp_system_context {
+	/* CRYP Register structure */
+	struct cryp_register *p_cryp_reg[MAX_DEVICE_SUPPORT];
+};
+
+#endif
diff --git a/drivers/crypto/ux500/hash/Makefile b/drivers/crypto/ux500/hash/Makefile
new file mode 100644
index 00000000000..b2f90d9bac7
--- /dev/null
+++ b/drivers/crypto/ux500/hash/Makefile
@@ -0,0 +1,11 @@
+#
+# Copyright (C) ST-Ericsson SA 2010
+# Author: Shujuan Chen (shujuan.chen@stericsson.com)
+# License terms: GNU General Public License (GPL) version 2
+#
+ifdef CONFIG_CRYPTO_DEV_UX500_DEBUG
+CFLAGS_hash_core.o := -DDEBUG -O0
+endif
+
+obj-$(CONFIG_CRYPTO_DEV_UX500_HASH) += ux500_hash.o
+ux500_hash-objs :=  hash_core.o
diff --git a/drivers/crypto/ux500/hash/hash_alg.h b/drivers/crypto/ux500/hash/hash_alg.h
new file mode 100644
index 00000000000..cd9351cb24d
--- /dev/null
+++ b/drivers/crypto/ux500/hash/hash_alg.h
@@ -0,0 +1,395 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ * Author: Shujuan Chen (shujuan.chen@stericsson.com)
+ * Author: Joakim Bech (joakim.xx.bech@stericsson.com)
+ * Author: Berne Hebark (berne.hebark@stericsson.com))
+ * License terms: GNU General Public License (GPL) version 2
+ */
+#ifndef _HASH_ALG_H
+#define _HASH_ALG_H
+
+#include <linux/bitops.h>
+
+#define HASH_BLOCK_SIZE			64
+#define HASH_DMA_ALIGN_SIZE		4
+#define HASH_DMA_PERFORMANCE_MIN_SIZE	1024
+#define HASH_BYTES_PER_WORD		4
+
+/* Maximum value of the length's high word */
+#define HASH_HIGH_WORD_MAX_VAL		0xFFFFFFFFUL
+
+/* Power on Reset values HASH registers */
+#define HASH_RESET_CR_VALUE		0x0
+#define HASH_RESET_STR_VALUE		0x0
+
+/* Number of context swap registers */
+#define HASH_CSR_COUNT			52
+
+#define HASH_RESET_CSRX_REG_VALUE	0x0
+#define HASH_RESET_CSFULL_REG_VALUE	0x0
+#define HASH_RESET_CSDATAIN_REG_VALUE	0x0
+
+#define HASH_RESET_INDEX_VAL		0x0
+#define HASH_RESET_BIT_INDEX_VAL	0x0
+#define HASH_RESET_BUFFER_VAL		0x0
+#define HASH_RESET_LEN_HIGH_VAL		0x0
+#define HASH_RESET_LEN_LOW_VAL		0x0
+
+/* Control register bitfields */
+#define HASH_CR_RESUME_MASK	0x11FCF
+
+#define HASH_CR_SWITCHON_POS	31
+#define HASH_CR_SWITCHON_MASK	BIT(31)
+
+#define HASH_CR_EMPTYMSG_POS	20
+#define HASH_CR_EMPTYMSG_MASK	BIT(20)
+
+#define HASH_CR_DINF_POS	12
+#define HASH_CR_DINF_MASK	BIT(12)
+
+#define HASH_CR_NBW_POS		8
+#define HASH_CR_NBW_MASK	0x00000F00UL
+
+#define HASH_CR_LKEY_POS	16
+#define HASH_CR_LKEY_MASK	BIT(16)
+
+#define HASH_CR_ALGO_POS	7
+#define HASH_CR_ALGO_MASK	BIT(7)
+
+#define HASH_CR_MODE_POS	6
+#define HASH_CR_MODE_MASK	BIT(6)
+
+#define HASH_CR_DATAFORM_POS	4
+#define HASH_CR_DATAFORM_MASK	(BIT(4) | BIT(5))
+
+#define HASH_CR_DMAE_POS	3
+#define HASH_CR_DMAE_MASK	BIT(3)
+
+#define HASH_CR_INIT_POS	2
+#define HASH_CR_INIT_MASK	BIT(2)
+
+#define HASH_CR_PRIVN_POS	1
+#define HASH_CR_PRIVN_MASK	BIT(1)
+
+#define HASH_CR_SECN_POS	0
+#define HASH_CR_SECN_MASK	BIT(0)
+
+/* Start register bitfields */
+#define HASH_STR_DCAL_POS	8
+#define HASH_STR_DCAL_MASK	BIT(8)
+#define HASH_STR_DEFAULT	0x0
+
+#define HASH_STR_NBLW_POS	0
+#define HASH_STR_NBLW_MASK	0x0000001FUL
+
+#define HASH_NBLW_MAX_VAL	0x1F
+
+/* PrimeCell IDs */
+#define HASH_P_ID0		0xE0
+#define HASH_P_ID1		0x05
+#define HASH_P_ID2		0x38
+#define HASH_P_ID3		0x00
+#define HASH_CELL_ID0		0x0D
+#define HASH_CELL_ID1		0xF0
+#define HASH_CELL_ID2		0x05
+#define HASH_CELL_ID3		0xB1
+
+#define HASH_SET_BITS(reg_name, mask)	\
+	writel_relaxed((readl_relaxed(reg_name) | mask), reg_name)
+
+#define HASH_CLEAR_BITS(reg_name, mask)	\
+	writel_relaxed((readl_relaxed(reg_name) & ~mask), reg_name)
+
+#define HASH_PUT_BITS(reg, val, shift, mask)	\
+	writel_relaxed(((readl(reg) & ~(mask)) |	\
+		(((u32)val << shift) & (mask))), reg)
+
+#define HASH_SET_DIN(val, len)	writesl(&device_data->base->din, (val), (len))
+
+#define HASH_INITIALIZE			\
+	HASH_PUT_BITS(			\
+		&device_data->base->cr,	\
+		0x01, HASH_CR_INIT_POS,	\
+		HASH_CR_INIT_MASK)
+
+#define HASH_SET_DATA_FORMAT(data_format)				\
+		HASH_PUT_BITS(						\
+			&device_data->base->cr,				\
+			(u32) (data_format), HASH_CR_DATAFORM_POS,	\
+			HASH_CR_DATAFORM_MASK)
+#define HASH_SET_NBLW(val)					\
+		HASH_PUT_BITS(					\
+			&device_data->base->str,		\
+			(u32) (val), HASH_STR_NBLW_POS,		\
+			HASH_STR_NBLW_MASK)
+#define HASH_SET_DCAL					\
+		HASH_PUT_BITS(				\
+			&device_data->base->str,	\
+			0x01, HASH_STR_DCAL_POS,	\
+			HASH_STR_DCAL_MASK)
+
+/* Hardware access method */
+enum hash_mode {
+	HASH_MODE_CPU,
+	HASH_MODE_DMA
+};
+
+/**
+ * struct uint64 - Structure to handle 64 bits integers.
+ * @high_word:	Most significant bits.
+ * @low_word:	Least significant bits.
+ *
+ * Used to handle 64 bits integers.
+ */
+struct uint64 {
+	u32 high_word;
+	u32 low_word;
+};
+
+/**
+ * struct hash_register - Contains all registers in ux500 hash hardware.
+ * @cr:		HASH control register (0x000).
+ * @din:	HASH data input register (0x004).
+ * @str:	HASH start register (0x008).
+ * @hx:		HASH digest register 0..7 (0x00c-0x01C).
+ * @padding0:	Reserved (0x02C).
+ * @itcr:	Integration test control register (0x080).
+ * @itip:	Integration test input register (0x084).
+ * @itop:	Integration test output register (0x088).
+ * @padding1:	Reserved (0x08C).
+ * @csfull:	HASH context full register (0x0F8).
+ * @csdatain:	HASH context swap data input register (0x0FC).
+ * @csrx:	HASH context swap register 0..51 (0x100-0x1CC).
+ * @padding2:	Reserved (0x1D0).
+ * @periphid0:	HASH peripheral identification register 0 (0xFE0).
+ * @periphid1:	HASH peripheral identification register 1 (0xFE4).
+ * @periphid2:	HASH peripheral identification register 2 (0xFE8).
+ * @periphid3:	HASH peripheral identification register 3 (0xFEC).
+ * @cellid0:	HASH PCell identification register 0 (0xFF0).
+ * @cellid1:	HASH PCell identification register 1 (0xFF4).
+ * @cellid2:	HASH PCell identification register 2 (0xFF8).
+ * @cellid3:	HASH PCell identification register 3 (0xFFC).
+ *
+ * The device communicates to the HASH via 32-bit-wide control registers
+ * accessible via the 32-bit width AMBA rev. 2.0 AHB Bus. Below is a structure
+ * with the registers used.
+ */
+struct hash_register {
+	u32 cr;
+	u32 din;
+	u32 str;
+	u32 hx[8];
+
+	u32 padding0[(0x080 - 0x02C) / sizeof(u32)];
+
+	u32 itcr;
+	u32 itip;
+	u32 itop;
+
+	u32 padding1[(0x0F8 - 0x08C) / sizeof(u32)];
+
+	u32 csfull;
+	u32 csdatain;
+	u32 csrx[HASH_CSR_COUNT];
+
+	u32 padding2[(0xFE0 - 0x1D0) / sizeof(u32)];
+
+	u32 periphid0;
+	u32 periphid1;
+	u32 periphid2;
+	u32 periphid3;
+
+	u32 cellid0;
+	u32 cellid1;
+	u32 cellid2;
+	u32 cellid3;
+};
+
+/**
+ * struct hash_state - Hash context state.
+ * @temp_cr:	Temporary HASH Control Register.
+ * @str_reg:	HASH Start Register.
+ * @din_reg:	HASH Data Input Register.
+ * @csr[52]:	HASH Context Swap Registers 0-39.
+ * @csfull:	HASH Context Swap Registers 40 ie Status flags.
+ * @csdatain:	HASH Context Swap Registers 41 ie Input data.
+ * @buffer:	Working buffer for messages going to the hardware.
+ * @length:	Length of the part of message hashed so far (floor(N/64) * 64).
+ * @index:	Valid number of bytes in buffer (N % 64).
+ * @bit_index:	Valid number of bits in buffer (N % 8).
+ *
+ * This structure is used between context switches, i.e. when ongoing jobs are
+ * interupted with new jobs. When this happens we need to store intermediate
+ * results in software.
+ *
+ * WARNING: "index" is the  member of the structure, to be sure  that "buffer"
+ * is aligned on a 4-bytes boundary. This is highly implementation dependent
+ * and MUST be checked whenever this code is ported on new platforms.
+ */
+struct hash_state {
+	u32		temp_cr;
+	u32		str_reg;
+	u32		din_reg;
+	u32		csr[52];
+	u32		csfull;
+	u32		csdatain;
+	u32		buffer[HASH_BLOCK_SIZE / sizeof(u32)];
+	struct uint64	length;
+	u8		index;
+	u8		bit_index;
+};
+
+/**
+ * enum hash_device_id - HASH device ID.
+ * @HASH_DEVICE_ID_0: Hash hardware with ID 0
+ * @HASH_DEVICE_ID_1: Hash hardware with ID 1
+ */
+enum hash_device_id {
+	HASH_DEVICE_ID_0 = 0,
+	HASH_DEVICE_ID_1 = 1
+};
+
+/**
+ * enum hash_data_format - HASH data format.
+ * @HASH_DATA_32_BITS:	32 bits data format
+ * @HASH_DATA_16_BITS:	16 bits data format
+ * @HASH_DATA_8_BITS:	8 bits data format.
+ * @HASH_DATA_1_BITS:	1 bit data format.
+ */
+enum hash_data_format {
+	HASH_DATA_32_BITS	= 0x0,
+	HASH_DATA_16_BITS	= 0x1,
+	HASH_DATA_8_BITS	= 0x2,
+	HASH_DATA_1_BIT		= 0x3
+};
+
+/**
+ * enum hash_algo - Enumeration for selecting between SHA1 or SHA2 algorithm.
+ * @HASH_ALGO_SHA1: Indicates that SHA1 is used.
+ * @HASH_ALGO_SHA2: Indicates that SHA2 (SHA256) is used.
+ */
+enum hash_algo {
+	HASH_ALGO_SHA1		= 0x0,
+	HASH_ALGO_SHA256	= 0x1
+};
+
+/**
+ * enum hash_op - Enumeration for selecting between HASH or HMAC mode.
+ * @HASH_OPER_MODE_HASH: Indicates usage of normal HASH mode.
+ * @HASH_OPER_MODE_HMAC: Indicates usage of HMAC.
+ */
+enum hash_op {
+	HASH_OPER_MODE_HASH = 0x0,
+	HASH_OPER_MODE_HMAC = 0x1
+};
+
+/**
+ * struct hash_config - Configuration data for the hardware.
+ * @data_format:	Format of data entered into the hash data in register.
+ * @algorithm:		Algorithm selection bit.
+ * @oper_mode:		Operating mode selection bit.
+ */
+struct hash_config {
+	int data_format;
+	int algorithm;
+	int oper_mode;
+};
+
+/**
+ * struct hash_dma - Structure used for dma.
+ * @mask:		DMA capabilities bitmap mask.
+ * @complete:		Used to maintain state for a "completion".
+ * @chan_mem2hash:	DMA channel.
+ * @cfg_mem2hash:	DMA channel configuration.
+ * @sg_len:		Scatterlist length.
+ * @sg:			Scatterlist.
+ * @nents:		Number of sg entries.
+ */
+struct hash_dma {
+	dma_cap_mask_t		mask;
+	struct completion	complete;
+	struct dma_chan		*chan_mem2hash;
+	void			*cfg_mem2hash;
+	int			sg_len;
+	struct scatterlist	*sg;
+	int			nents;
+};
+
+/**
+ * struct hash_ctx - The context used for hash calculations.
+ * @key:	The key used in the operation.
+ * @keylen:	The length of the key.
+ * @state:	The state of the current calculations.
+ * @config:	The current configuration.
+ * @digestsize:	The size of current digest.
+ * @device:	Pointer to the device structure.
+ */
+struct hash_ctx {
+	u8			*key;
+	u32			keylen;
+	struct hash_config	config;
+	int			digestsize;
+	struct hash_device_data	*device;
+};
+
+/**
+ * struct hash_ctx - The request context used for hash calculations.
+ * @state:	The state of the current calculations.
+ * @dma_mode:	Used in special cases (workaround), e.g. need to change to
+ *		cpu mode, if not supported/working in dma mode.
+ * @updated:	Indicates if hardware is initialized for new operations.
+ */
+struct hash_req_ctx {
+	struct hash_state	state;
+	bool			dma_mode;
+	u8			updated;
+};
+
+/**
+ * struct hash_device_data - structure for a hash device.
+ * @base:		Pointer to the hardware base address.
+ * @list_node:		For inclusion in klist.
+ * @dev:		Pointer to the device dev structure.
+ * @ctx_lock:		Spinlock for current_ctx.
+ * @current_ctx:	Pointer to the currently allocated context.
+ * @power_state:	TRUE = power state on, FALSE = power state off.
+ * @power_state_lock:	Spinlock for power_state.
+ * @regulator:		Pointer to the device's power control.
+ * @clk:		Pointer to the device's clock control.
+ * @restore_dev_state:	TRUE = saved state, FALSE = no saved state.
+ * @dma:		Structure used for dma.
+ */
+struct hash_device_data {
+	struct hash_register __iomem	*base;
+	struct klist_node	list_node;
+	struct device		*dev;
+	struct spinlock		ctx_lock;
+	struct hash_ctx		*current_ctx;
+	bool			power_state;
+	struct spinlock		power_state_lock;
+	struct regulator	*regulator;
+	struct clk		*clk;
+	bool			restore_dev_state;
+	struct hash_state	state; /* Used for saving and resuming state */
+	struct hash_dma		dma;
+};
+
+int hash_check_hw(struct hash_device_data *device_data);
+
+int hash_setconfiguration(struct hash_device_data *device_data,
+		struct hash_config *config);
+
+void hash_begin(struct hash_device_data *device_data, struct hash_ctx *ctx);
+
+void hash_get_digest(struct hash_device_data *device_data,
+		u8 *digest, int algorithm);
+
+int hash_hw_update(struct ahash_request *req);
+
+int hash_save_state(struct hash_device_data *device_data,
+		struct hash_state *state);
+
+int hash_resume_state(struct hash_device_data *device_data,
+		const struct hash_state *state);
+
+#endif
diff --git a/drivers/crypto/ux500/hash/hash_core.c b/drivers/crypto/ux500/hash/hash_core.c
new file mode 100644
index 00000000000..6dbb9ec709a
--- /dev/null
+++ b/drivers/crypto/ux500/hash/hash_core.c
@@ -0,0 +1,2009 @@
+/*
+ * Cryptographic API.
+ * Support for Nomadik hardware crypto engine.
+
+ * Copyright (C) ST-Ericsson SA 2010
+ * Author: Shujuan Chen <shujuan.chen@stericsson.com> for ST-Ericsson
+ * Author: Joakim Bech <joakim.xx.bech@stericsson.com> for ST-Ericsson
+ * Author: Berne Hebark <berne.herbark@stericsson.com> for ST-Ericsson.
+ * Author: Niklas Hernaeus <niklas.hernaeus@stericsson.com> for ST-Ericsson.
+ * Author: Andreas Westin <andreas.westin@stericsson.com> for ST-Ericsson.
+ * License terms: GNU General Public License (GPL) version 2
+ */
+
+#include <linux/clk.h>
+#include <linux/device.h>
+#include <linux/err.h>
+#include <linux/init.h>
+#include <linux/io.h>
+#include <linux/klist.h>
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/platform_device.h>
+#include <linux/crypto.h>
+
+#include <linux/regulator/consumer.h>
+#include <linux/dmaengine.h>
+#include <linux/bitops.h>
+
+#include <crypto/internal/hash.h>
+#include <crypto/sha.h>
+#include <crypto/scatterwalk.h>
+#include <crypto/algapi.h>
+
+#include <mach/crypto-ux500.h>
+#include <mach/hardware.h>
+
+#include "hash_alg.h"
+
+#define DEV_DBG_NAME "hashX hashX:"
+
+static int hash_mode;
+module_param(hash_mode, int, 0);
+MODULE_PARM_DESC(hash_mode, "CPU or DMA mode. CPU = 0 (default), DMA = 1");
+
+/**
+ * Pre-calculated empty message digests.
+ */
+static u8 zero_message_hash_sha1[SHA1_DIGEST_SIZE] = {
+	0xda, 0x39, 0xa3, 0xee, 0x5e, 0x6b, 0x4b, 0x0d,
+	0x32, 0x55, 0xbf, 0xef, 0x95, 0x60, 0x18, 0x90,
+	0xaf, 0xd8, 0x07, 0x09
+};
+
+static u8 zero_message_hash_sha256[SHA256_DIGEST_SIZE] = {
+	0xe3, 0xb0, 0xc4, 0x42, 0x98, 0xfc, 0x1c, 0x14,
+	0x9a, 0xfb, 0xf4, 0xc8, 0x99, 0x6f, 0xb9, 0x24,
+	0x27, 0xae, 0x41, 0xe4, 0x64, 0x9b, 0x93, 0x4c,
+	0xa4, 0x95, 0x99, 0x1b, 0x78, 0x52, 0xb8, 0x55
+};
+
+/* HMAC-SHA1, no key */
+static u8 zero_message_hmac_sha1[SHA1_DIGEST_SIZE] = {
+	0xfb, 0xdb, 0x1d, 0x1b, 0x18, 0xaa, 0x6c, 0x08,
+	0x32, 0x4b, 0x7d, 0x64, 0xb7, 0x1f, 0xb7, 0x63,
+	0x70, 0x69, 0x0e, 0x1d
+};
+
+/* HMAC-SHA256, no key */
+static u8 zero_message_hmac_sha256[SHA256_DIGEST_SIZE] = {
+	0xb6, 0x13, 0x67, 0x9a, 0x08, 0x14, 0xd9, 0xec,
+	0x77, 0x2f, 0x95, 0xd7, 0x78, 0xc3, 0x5f, 0xc5,
+	0xff, 0x16, 0x97, 0xc4, 0x93, 0x71, 0x56, 0x53,
+	0xc6, 0xc7, 0x12, 0x14, 0x42, 0x92, 0xc5, 0xad
+};
+
+/**
+ * struct hash_driver_data - data specific to the driver.
+ *
+ * @device_list:	A list of registered devices to choose from.
+ * @device_allocation:	A semaphore initialized with number of devices.
+ */
+struct hash_driver_data {
+	struct klist		device_list;
+	struct semaphore	device_allocation;
+};
+
+static struct hash_driver_data	driver_data;
+
+/* Declaration of functions */
+/**
+ * hash_messagepad - Pads a message and write the nblw bits.
+ * @device_data:	Structure for the hash device.
+ * @message:		Last word of a message
+ * @index_bytes:	The number of bytes in the last message
+ *
+ * This function manages the final part of the digest calculation, when less
+ * than 512 bits (64 bytes) remain in message. This means index_bytes < 64.
+ *
+ */
+static void hash_messagepad(struct hash_device_data *device_data,
+		const u32 *message, u8 index_bytes);
+
+/**
+ * release_hash_device - Releases a previously allocated hash device.
+ * @device_data:	Structure for the hash device.
+ *
+ */
+static void release_hash_device(struct hash_device_data *device_data)
+{
+	spin_lock(&device_data->ctx_lock);
+	device_data->current_ctx->device = NULL;
+	device_data->current_ctx = NULL;
+	spin_unlock(&device_data->ctx_lock);
+
+	/*
+	 * The down_interruptible part for this semaphore is called in
+	 * cryp_get_device_data.
+	 */
+	up(&driver_data.device_allocation);
+}
+
+static void hash_dma_setup_channel(struct hash_device_data *device_data,
+				struct device *dev)
+{
+	struct hash_platform_data *platform_data = dev->platform_data;
+	dma_cap_zero(device_data->dma.mask);
+	dma_cap_set(DMA_SLAVE, device_data->dma.mask);
+
+	device_data->dma.cfg_mem2hash = platform_data->mem_to_engine;
+	device_data->dma.chan_mem2hash =
+		dma_request_channel(device_data->dma.mask,
+				platform_data->dma_filter,
+				device_data->dma.cfg_mem2hash);
+
+	init_completion(&device_data->dma.complete);
+}
+
+static void hash_dma_callback(void *data)
+{
+	struct hash_ctx *ctx = (struct hash_ctx *) data;
+
+	complete(&ctx->device->dma.complete);
+}
+
+static int hash_set_dma_transfer(struct hash_ctx *ctx, struct scatterlist *sg,
+		int len, enum dma_data_direction direction)
+{
+	struct dma_async_tx_descriptor *desc = NULL;
+	struct dma_chan *channel = NULL;
+	dma_cookie_t cookie;
+
+	if (direction != DMA_TO_DEVICE) {
+		dev_err(ctx->device->dev, "[%s] Invalid DMA direction",
+				__func__);
+		return -EFAULT;
+	}
+
+	sg->length = ALIGN(sg->length, HASH_DMA_ALIGN_SIZE);
+
+	channel = ctx->device->dma.chan_mem2hash;
+	ctx->device->dma.sg = sg;
+	ctx->device->dma.sg_len = dma_map_sg(channel->device->dev,
+			ctx->device->dma.sg, ctx->device->dma.nents,
+			direction);
+
+	if (!ctx->device->dma.sg_len) {
+		dev_err(ctx->device->dev,
+				"[%s]: Could not map the sg list (TO_DEVICE)",
+				__func__);
+		return -EFAULT;
+	}
+
+	dev_dbg(ctx->device->dev, "[%s]: Setting up DMA for buffer "
+			"(TO_DEVICE)", __func__);
+	desc = channel->device->device_prep_slave_sg(channel,
+			ctx->device->dma.sg, ctx->device->dma.sg_len,
+			direction, DMA_CTRL_ACK | DMA_PREP_INTERRUPT, NULL);
+	if (!desc) {
+		dev_err(ctx->device->dev,
+			"[%s]: device_prep_slave_sg() failed!", __func__);
+		return -EFAULT;
+	}
+
+	desc->callback = hash_dma_callback;
+	desc->callback_param = ctx;
+
+	cookie = desc->tx_submit(desc);
+	dma_async_issue_pending(channel);
+
+	return 0;
+}
+
+static void hash_dma_done(struct hash_ctx *ctx)
+{
+	struct dma_chan *chan;
+
+	chan = ctx->device->dma.chan_mem2hash;
+	chan->device->device_control(chan, DMA_TERMINATE_ALL, 0);
+	dma_unmap_sg(chan->device->dev, ctx->device->dma.sg,
+			ctx->device->dma.sg_len, DMA_TO_DEVICE);
+
+}
+
+static int hash_dma_write(struct hash_ctx *ctx,
+		struct scatterlist *sg, int len)
+{
+	int error = hash_set_dma_transfer(ctx, sg, len, DMA_TO_DEVICE);
+	if (error) {
+		dev_dbg(ctx->device->dev, "[%s]: hash_set_dma_transfer() "
+			"failed", __func__);
+		return error;
+	}
+
+	return len;
+}
+
+/**
+ * get_empty_message_digest - Returns a pre-calculated digest for
+ * the empty message.
+ * @device_data:	Structure for the hash device.
+ * @zero_hash:		Buffer to return the empty message digest.
+ * @zero_hash_size:	Hash size of the empty message digest.
+ * @zero_digest:	True if zero_digest returned.
+ */
+static int get_empty_message_digest(
+		struct hash_device_data *device_data,
+		u8 *zero_hash, u32 *zero_hash_size, bool *zero_digest)
+{
+	int ret = 0;
+	struct hash_ctx *ctx = device_data->current_ctx;
+	*zero_digest = false;
+
+	/**
+	 * Caller responsible for ctx != NULL.
+	 */
+
+	if (HASH_OPER_MODE_HASH == ctx->config.oper_mode) {
+		if (HASH_ALGO_SHA1 == ctx->config.algorithm) {
+			memcpy(zero_hash, &zero_message_hash_sha1[0],
+					SHA1_DIGEST_SIZE);
+			*zero_hash_size = SHA1_DIGEST_SIZE;
+			*zero_digest = true;
+		} else if (HASH_ALGO_SHA256 ==
+				ctx->config.algorithm) {
+			memcpy(zero_hash, &zero_message_hash_sha256[0],
+					SHA256_DIGEST_SIZE);
+			*zero_hash_size = SHA256_DIGEST_SIZE;
+			*zero_digest = true;
+		} else {
+			dev_err(device_data->dev, "[%s] "
+					"Incorrect algorithm!"
+					, __func__);
+			ret = -EINVAL;
+			goto out;
+		}
+	} else if (HASH_OPER_MODE_HMAC == ctx->config.oper_mode) {
+		if (!ctx->keylen) {
+			if (HASH_ALGO_SHA1 == ctx->config.algorithm) {
+				memcpy(zero_hash, &zero_message_hmac_sha1[0],
+						SHA1_DIGEST_SIZE);
+				*zero_hash_size = SHA1_DIGEST_SIZE;
+				*zero_digest = true;
+			} else if (HASH_ALGO_SHA256 == ctx->config.algorithm) {
+				memcpy(zero_hash, &zero_message_hmac_sha256[0],
+						SHA256_DIGEST_SIZE);
+				*zero_hash_size = SHA256_DIGEST_SIZE;
+				*zero_digest = true;
+			} else {
+				dev_err(device_data->dev, "[%s] "
+						"Incorrect algorithm!"
+						, __func__);
+				ret = -EINVAL;
+				goto out;
+			}
+		} else {
+			dev_dbg(device_data->dev, "[%s] Continue hash "
+					"calculation, since hmac key avalable",
+					__func__);
+		}
+	}
+out:
+
+	return ret;
+}
+
+/**
+ * hash_disable_power - Request to disable power and clock.
+ * @device_data:	Structure for the hash device.
+ * @save_device_state:	If true, saves the current hw state.
+ *
+ * This function request for disabling power (regulator) and clock,
+ * and could also save current hw state.
+ */
+static int hash_disable_power(
+		struct hash_device_data *device_data,
+		bool			save_device_state)
+{
+	int ret = 0;
+	struct device *dev = device_data->dev;
+
+	spin_lock(&device_data->power_state_lock);
+	if (!device_data->power_state)
+		goto out;
+
+	if (save_device_state) {
+		hash_save_state(device_data,
+				&device_data->state);
+		device_data->restore_dev_state = true;
+	}
+
+	clk_disable(device_data->clk);
+	ret = regulator_disable(device_data->regulator);
+	if (ret)
+		dev_err(dev, "[%s] regulator_disable() failed!", __func__);
+
+	device_data->power_state = false;
+
+out:
+	spin_unlock(&device_data->power_state_lock);
+
+	return ret;
+}
+
+/**
+ * hash_enable_power - Request to enable power and clock.
+ * @device_data:		Structure for the hash device.
+ * @restore_device_state:	If true, restores a previous saved hw state.
+ *
+ * This function request for enabling power (regulator) and clock,
+ * and could also restore a previously saved hw state.
+ */
+static int hash_enable_power(
+		struct hash_device_data *device_data,
+		bool			restore_device_state)
+{
+	int ret = 0;
+	struct device *dev = device_data->dev;
+
+	spin_lock(&device_data->power_state_lock);
+	if (!device_data->power_state) {
+		ret = regulator_enable(device_data->regulator);
+		if (ret) {
+			dev_err(dev, "[%s]: regulator_enable() failed!",
+					__func__);
+			goto out;
+		}
+		ret = clk_enable(device_data->clk);
+		if (ret) {
+			dev_err(dev, "[%s]: clk_enable() failed!",
+					__func__);
+			ret = regulator_disable(
+					device_data->regulator);
+			goto out;
+		}
+		device_data->power_state = true;
+	}
+
+	if (device_data->restore_dev_state) {
+		if (restore_device_state) {
+			device_data->restore_dev_state = false;
+			hash_resume_state(device_data,
+				&device_data->state);
+		}
+	}
+out:
+	spin_unlock(&device_data->power_state_lock);
+
+	return ret;
+}
+
+/**
+ * hash_get_device_data - Checks for an available hash device and return it.
+ * @hash_ctx:		Structure for the hash context.
+ * @device_data:	Structure for the hash device.
+ *
+ * This function check for an available hash device and return it to
+ * the caller.
+ * Note! Caller need to release the device, calling up().
+ */
+static int hash_get_device_data(struct hash_ctx *ctx,
+				struct hash_device_data **device_data)
+{
+	int			ret;
+	struct klist_iter	device_iterator;
+	struct klist_node	*device_node;
+	struct hash_device_data *local_device_data = NULL;
+
+	/* Wait until a device is available */
+	ret = down_interruptible(&driver_data.device_allocation);
+	if (ret)
+		return ret;  /* Interrupted */
+
+	/* Select a device */
+	klist_iter_init(&driver_data.device_list, &device_iterator);
+	device_node = klist_next(&device_iterator);
+	while (device_node) {
+		local_device_data = container_of(device_node,
+					   struct hash_device_data, list_node);
+		spin_lock(&local_device_data->ctx_lock);
+		/* current_ctx allocates a device, NULL = unallocated */
+		if (local_device_data->current_ctx) {
+			device_node = klist_next(&device_iterator);
+		} else {
+			local_device_data->current_ctx = ctx;
+			ctx->device = local_device_data;
+			spin_unlock(&local_device_data->ctx_lock);
+			break;
+		}
+		spin_unlock(&local_device_data->ctx_lock);
+	}
+	klist_iter_exit(&device_iterator);
+
+	if (!device_node) {
+		/**
+		 * No free device found.
+		 * Since we allocated a device with down_interruptible, this
+		 * should not be able to happen.
+		 * Number of available devices, which are contained in
+		 * device_allocation, is therefore decremented by not doing
+		 * an up(device_allocation).
+		 */
+		return -EBUSY;
+	}
+
+	*device_data = local_device_data;
+
+	return 0;
+}
+
+/**
+ * hash_hw_write_key - Writes the key to the hardware registries.
+ *
+ * @device_data:	Structure for the hash device.
+ * @key:		Key to be written.
+ * @keylen:		The lengt of the key.
+ *
+ * Note! This function DOES NOT write to the NBLW registry, even though
+ * specified in the the hw design spec. Either due to incorrect info in the
+ * spec or due to a bug in the hw.
+ */
+static void hash_hw_write_key(struct hash_device_data *device_data,
+		const u8 *key, unsigned int keylen)
+{
+	u32 word = 0;
+	int nwords = 1;
+
+	HASH_CLEAR_BITS(&device_data->base->str, HASH_STR_NBLW_MASK);
+
+	while (keylen >= 4) {
+		u32 *key_word = (u32 *)key;
+
+		HASH_SET_DIN(key_word, nwords);
+		keylen -= 4;
+		key += 4;
+	}
+
+	/* Take care of the remaining bytes in the last word */
+	if (keylen) {
+		word = 0;
+		while (keylen) {
+			word |= (key[keylen - 1] << (8 * (keylen - 1)));
+			keylen--;
+		}
+
+		HASH_SET_DIN(&word, nwords);
+	}
+
+	while (device_data->base->str & HASH_STR_DCAL_MASK)
+		cpu_relax();
+
+	HASH_SET_DCAL;
+
+	while (device_data->base->str & HASH_STR_DCAL_MASK)
+		cpu_relax();
+}
+
+/**
+ * init_hash_hw - Initialise the hash hardware for a new calculation.
+ * @device_data:	Structure for the hash device.
+ * @ctx:		The hash context.
+ *
+ * This function will enable the bits needed to clear and start a new
+ * calculation.
+ */
+static int init_hash_hw(struct hash_device_data *device_data,
+		struct hash_ctx *ctx)
+{
+	int ret = 0;
+
+	ret = hash_setconfiguration(device_data, &ctx->config);
+	if (ret) {
+		dev_err(device_data->dev, "[%s] hash_setconfiguration() "
+				"failed!", __func__);
+		return ret;
+	}
+
+	hash_begin(device_data, ctx);
+
+	if (ctx->config.oper_mode == HASH_OPER_MODE_HMAC)
+		hash_hw_write_key(device_data, ctx->key, ctx->keylen);
+
+	return ret;
+}
+
+/**
+ * hash_get_nents - Return number of entries (nents) in scatterlist (sg).
+ *
+ * @sg:		Scatterlist.
+ * @size:	Size in bytes.
+ * @aligned:	True if sg data aligned to work in DMA mode.
+ *
+ */
+static int hash_get_nents(struct scatterlist *sg, int size, bool *aligned)
+{
+	int nents = 0;
+	bool aligned_data = true;
+
+	while (size > 0 && sg) {
+		nents++;
+		size -= sg->length;
+
+		/* hash_set_dma_transfer will align last nent */
+		if ((aligned && !IS_ALIGNED(sg->offset, HASH_DMA_ALIGN_SIZE))
+			|| (!IS_ALIGNED(sg->length, HASH_DMA_ALIGN_SIZE) &&
+				size > 0))
+			aligned_data = false;
+
+		sg = sg_next(sg);
+	}
+
+	if (aligned)
+		*aligned = aligned_data;
+
+	if (size != 0)
+		return -EFAULT;
+
+	return nents;
+}
+
+/**
+ * hash_dma_valid_data - checks for dma valid sg data.
+ * @sg:		Scatterlist.
+ * @datasize:	Datasize in bytes.
+ *
+ * NOTE! This function checks for dma valid sg data, since dma
+ * only accept datasizes of even wordsize.
+ */
+static bool hash_dma_valid_data(struct scatterlist *sg, int datasize)
+{
+	bool aligned;
+
+	/* Need to include at least one nent, else error */
+	if (hash_get_nents(sg, datasize, &aligned) < 1)
+		return false;
+
+	return aligned;
+}
+
+/**
+ * hash_init - Common hash init function for SHA1/SHA2 (SHA256).
+ * @req: The hash request for the job.
+ *
+ * Initialize structures.
+ */
+static int hash_init(struct ahash_request *req)
+{
+	struct crypto_ahash *tfm = crypto_ahash_reqtfm(req);
+	struct hash_ctx *ctx = crypto_ahash_ctx(tfm);
+	struct hash_req_ctx *req_ctx = ahash_request_ctx(req);
+
+	if (!ctx->key)
+		ctx->keylen = 0;
+
+	memset(&req_ctx->state, 0, sizeof(struct hash_state));
+	req_ctx->updated = 0;
+	if (hash_mode == HASH_MODE_DMA) {
+		if (req->nbytes < HASH_DMA_ALIGN_SIZE) {
+			req_ctx->dma_mode = false; /* Don't use DMA */
+
+			pr_debug(DEV_DBG_NAME " [%s] DMA mode, but direct "
+					"to CPU mode for data size < %d",
+					__func__, HASH_DMA_ALIGN_SIZE);
+		} else {
+			if (req->nbytes >= HASH_DMA_PERFORMANCE_MIN_SIZE &&
+					hash_dma_valid_data(req->src,
+						req->nbytes)) {
+				req_ctx->dma_mode = true;
+			} else {
+				req_ctx->dma_mode = false;
+				pr_debug(DEV_DBG_NAME " [%s] DMA mode, but use"
+						" CPU mode for datalength < %d"
+						" or non-aligned data, except "
+						"in last nent", __func__,
+						HASH_DMA_PERFORMANCE_MIN_SIZE);
+			}
+		}
+	}
+	return 0;
+}
+
+/**
+ * hash_processblock - This function processes a single block of 512 bits (64
+ *                     bytes), word aligned, starting at message.
+ * @device_data:	Structure for the hash device.
+ * @message:		Block (512 bits) of message to be written to
+ *			the HASH hardware.
+ *
+ */
+static void hash_processblock(
+		struct hash_device_data *device_data,
+		const u32 *message, int length)
+{
+	int len = length / HASH_BYTES_PER_WORD;
+	/*
+	 * NBLW bits. Reset the number of bits in last word (NBLW).
+	 */
+	HASH_CLEAR_BITS(&device_data->base->str, HASH_STR_NBLW_MASK);
+
+	/*
+	 * Write message data to the HASH_DIN register.
+	 */
+	HASH_SET_DIN(message, len);
+}
+
+/**
+ * hash_messagepad - Pads a message and write the nblw bits.
+ * @device_data:	Structure for the hash device.
+ * @message:		Last word of a message.
+ * @index_bytes:	The number of bytes in the last message.
+ *
+ * This function manages the final part of the digest calculation, when less
+ * than 512 bits (64 bytes) remain in message. This means index_bytes < 64.
+ *
+ */
+static void hash_messagepad(struct hash_device_data *device_data,
+		const u32 *message, u8 index_bytes)
+{
+	int nwords = 1;
+
+	/*
+	 * Clear hash str register, only clear NBLW
+	 * since DCAL will be reset by hardware.
+	 */
+	HASH_CLEAR_BITS(&device_data->base->str, HASH_STR_NBLW_MASK);
+
+	/* Main loop */
+	while (index_bytes >= 4) {
+		HASH_SET_DIN(message, nwords);
+		index_bytes -= 4;
+		message++;
+	}
+
+	if (index_bytes)
+		HASH_SET_DIN(message, nwords);
+
+	while (device_data->base->str & HASH_STR_DCAL_MASK)
+		cpu_relax();
+
+	/* num_of_bytes == 0 => NBLW <- 0 (32 bits valid in DATAIN) */
+	HASH_SET_NBLW(index_bytes * 8);
+	dev_dbg(device_data->dev, "[%s] DIN=0x%08x NBLW=%d", __func__,
+			readl_relaxed(&device_data->base->din),
+			(int)(readl_relaxed(&device_data->base->str) &
+				HASH_STR_NBLW_MASK));
+	HASH_SET_DCAL;
+	dev_dbg(device_data->dev, "[%s] after dcal -> DIN=0x%08x NBLW=%d",
+			__func__, readl_relaxed(&device_data->base->din),
+			(int)(readl_relaxed(&device_data->base->str) &
+				HASH_STR_NBLW_MASK));
+
+	while (device_data->base->str & HASH_STR_DCAL_MASK)
+		cpu_relax();
+}
+
+/**
+ * hash_incrementlength - Increments the length of the current message.
+ * @ctx: Hash context
+ * @incr: Length of message processed already
+ *
+ * Overflow cannot occur, because conditions for overflow are checked in
+ * hash_hw_update.
+ */
+static void hash_incrementlength(struct hash_req_ctx *ctx, u32 incr)
+{
+	ctx->state.length.low_word += incr;
+
+	/* Check for wrap-around */
+	if (ctx->state.length.low_word < incr)
+		ctx->state.length.high_word++;
+}
+
+/**
+ * hash_setconfiguration - Sets the required configuration for the hash
+ *                         hardware.
+ * @device_data:	Structure for the hash device.
+ * @config:		Pointer to a configuration structure.
+ */
+int hash_setconfiguration(struct hash_device_data *device_data,
+		struct hash_config *config)
+{
+	int ret = 0;
+
+	if (config->algorithm != HASH_ALGO_SHA1 &&
+	    config->algorithm != HASH_ALGO_SHA256)
+		return -EPERM;
+
+	/*
+	 * DATAFORM bits. Set the DATAFORM bits to 0b11, which means the data
+	 * to be written to HASH_DIN is considered as 32 bits.
+	 */
+	HASH_SET_DATA_FORMAT(config->data_format);
+
+	/*
+	 * ALGO bit. Set to 0b1 for SHA-1 and 0b0 for SHA-256
+	 */
+	switch (config->algorithm) {
+	case HASH_ALGO_SHA1:
+		HASH_SET_BITS(&device_data->base->cr, HASH_CR_ALGO_MASK);
+		break;
+
+	case HASH_ALGO_SHA256:
+		HASH_CLEAR_BITS(&device_data->base->cr, HASH_CR_ALGO_MASK);
+		break;
+
+	default:
+		dev_err(device_data->dev, "[%s] Incorrect algorithm.",
+				__func__);
+		return -EPERM;
+	}
+
+	/*
+	 * MODE bit. This bit selects between HASH or HMAC mode for the
+	 * selected algorithm. 0b0 = HASH and 0b1 = HMAC.
+	 */
+	if (HASH_OPER_MODE_HASH == config->oper_mode)
+		HASH_CLEAR_BITS(&device_data->base->cr,
+				HASH_CR_MODE_MASK);
+	else if (HASH_OPER_MODE_HMAC == config->oper_mode) {
+		HASH_SET_BITS(&device_data->base->cr,
+				HASH_CR_MODE_MASK);
+		if (device_data->current_ctx->keylen > HASH_BLOCK_SIZE) {
+			/* Truncate key to blocksize */
+			dev_dbg(device_data->dev, "[%s] LKEY set", __func__);
+			HASH_SET_BITS(&device_data->base->cr,
+					HASH_CR_LKEY_MASK);
+		} else {
+			dev_dbg(device_data->dev, "[%s] LKEY cleared",
+					__func__);
+			HASH_CLEAR_BITS(&device_data->base->cr,
+					HASH_CR_LKEY_MASK);
+		}
+	} else {	/* Wrong hash mode */
+		ret = -EPERM;
+		dev_err(device_data->dev, "[%s] HASH_INVALID_PARAMETER!",
+				__func__);
+	}
+	return ret;
+}
+
+/**
+ * hash_begin - This routine resets some globals and initializes the hash
+ *              hardware.
+ * @device_data:	Structure for the hash device.
+ * @ctx:		Hash context.
+ */
+void hash_begin(struct hash_device_data *device_data, struct hash_ctx *ctx)
+{
+	/* HW and SW initializations */
+	/* Note: there is no need to initialize buffer and digest members */
+
+	while (device_data->base->str & HASH_STR_DCAL_MASK)
+		cpu_relax();
+
+	/*
+	 * INIT bit. Set this bit to 0b1 to reset the HASH processor core and
+	 * prepare the initialize the HASH accelerator to compute the message
+	 * digest of a new message.
+	 */
+	HASH_INITIALIZE;
+
+	/*
+	 * NBLW bits. Reset the number of bits in last word (NBLW).
+	 */
+	HASH_CLEAR_BITS(&device_data->base->str, HASH_STR_NBLW_MASK);
+}
+
+int hash_process_data(
+		struct hash_device_data *device_data,
+		struct hash_ctx *ctx, struct hash_req_ctx *req_ctx,
+		int msg_length, u8 *data_buffer, u8 *buffer, u8 *index)
+{
+	int ret = 0;
+	u32 count;
+
+	do {
+		if ((*index + msg_length) < HASH_BLOCK_SIZE) {
+			for (count = 0; count < msg_length; count++) {
+				buffer[*index + count] =
+					*(data_buffer + count);
+			}
+			*index += msg_length;
+			msg_length = 0;
+		} else {
+			if (req_ctx->updated) {
+
+				ret = hash_resume_state(device_data,
+						&device_data->state);
+				memmove(req_ctx->state.buffer,
+						device_data->state.buffer,
+						HASH_BLOCK_SIZE / sizeof(u32));
+				if (ret) {
+					dev_err(device_data->dev, "[%s] "
+							"hash_resume_state()"
+							" failed!", __func__);
+					goto out;
+				}
+			} else {
+				ret = init_hash_hw(device_data, ctx);
+				if (ret) {
+					dev_err(device_data->dev, "[%s] "
+							"init_hash_hw()"
+							" failed!", __func__);
+					goto out;
+				}
+				req_ctx->updated = 1;
+			}
+			/*
+			 * If 'data_buffer' is four byte aligned and
+			 * local buffer does not have any data, we can
+			 * write data directly from 'data_buffer' to
+			 * HW peripheral, otherwise we first copy data
+			 * to a local buffer
+			 */
+			if ((0 == (((u32)data_buffer) % 4))
+					&& (0 == *index))
+				hash_processblock(device_data,
+						(const u32 *)
+						data_buffer, HASH_BLOCK_SIZE);
+			else {
+				for (count = 0; count <
+						(u32)(HASH_BLOCK_SIZE -
+							*index);
+						count++) {
+					buffer[*index + count] =
+						*(data_buffer + count);
+				}
+				hash_processblock(device_data,
+						(const u32 *)buffer,
+						HASH_BLOCK_SIZE);
+			}
+			hash_incrementlength(req_ctx, HASH_BLOCK_SIZE);
+			data_buffer += (HASH_BLOCK_SIZE - *index);
+
+			msg_length -= (HASH_BLOCK_SIZE - *index);
+			*index = 0;
+
+			ret = hash_save_state(device_data,
+					&device_data->state);
+
+			memmove(device_data->state.buffer,
+					req_ctx->state.buffer,
+					HASH_BLOCK_SIZE / sizeof(u32));
+			if (ret) {
+				dev_err(device_data->dev, "[%s] "
+						"hash_save_state()"
+						" failed!", __func__);
+				goto out;
+			}
+		}
+	} while (msg_length != 0);
+out:
+
+	return ret;
+}
+
+/**
+ * hash_dma_final - The hash dma final function for SHA1/SHA256.
+ * @req:	The hash request for the job.
+ */
+static int hash_dma_final(struct ahash_request *req)
+{
+	int ret = 0;
+	struct crypto_ahash *tfm = crypto_ahash_reqtfm(req);
+	struct hash_ctx *ctx = crypto_ahash_ctx(tfm);
+	struct hash_req_ctx *req_ctx = ahash_request_ctx(req);
+	struct hash_device_data *device_data;
+	u8 digest[SHA256_DIGEST_SIZE];
+	int bytes_written = 0;
+
+	ret = hash_get_device_data(ctx, &device_data);
+	if (ret)
+		return ret;
+
+	dev_dbg(device_data->dev, "[%s] (ctx=0x%x)!", __func__, (u32) ctx);
+
+	if (req_ctx->updated) {
+		ret = hash_resume_state(device_data, &device_data->state);
+
+		if (ret) {
+			dev_err(device_data->dev, "[%s] hash_resume_state() "
+					"failed!", __func__);
+			goto out;
+		}
+
+	}
+
+	if (!req_ctx->updated) {
+		ret = hash_setconfiguration(device_data, &ctx->config);
+		if (ret) {
+			dev_err(device_data->dev, "[%s] "
+					"hash_setconfiguration() failed!",
+					__func__);
+			goto out;
+		}
+
+		/* Enable DMA input */
+		if (hash_mode != HASH_MODE_DMA || !req_ctx->dma_mode) {
+			HASH_CLEAR_BITS(&device_data->base->cr,
+					HASH_CR_DMAE_MASK);
+		} else {
+			HASH_SET_BITS(&device_data->base->cr,
+					HASH_CR_DMAE_MASK);
+			HASH_SET_BITS(&device_data->base->cr,
+					HASH_CR_PRIVN_MASK);
+		}
+
+		HASH_INITIALIZE;
+
+		if (ctx->config.oper_mode == HASH_OPER_MODE_HMAC)
+			hash_hw_write_key(device_data, ctx->key, ctx->keylen);
+
+		/* Number of bits in last word = (nbytes * 8) % 32 */
+		HASH_SET_NBLW((req->nbytes * 8) % 32);
+		req_ctx->updated = 1;
+	}
+
+	/* Store the nents in the dma struct. */
+	ctx->device->dma.nents = hash_get_nents(req->src, req->nbytes, NULL);
+	if (!ctx->device->dma.nents) {
+		dev_err(device_data->dev, "[%s] "
+				"ctx->device->dma.nents = 0", __func__);
+		goto out;
+	}
+
+	bytes_written = hash_dma_write(ctx, req->src, req->nbytes);
+	if (bytes_written != req->nbytes) {
+		dev_err(device_data->dev, "[%s] "
+				"hash_dma_write() failed!", __func__);
+		goto out;
+	}
+
+	wait_for_completion(&ctx->device->dma.complete);
+	hash_dma_done(ctx);
+
+	while (device_data->base->str & HASH_STR_DCAL_MASK)
+		cpu_relax();
+
+	if (ctx->config.oper_mode == HASH_OPER_MODE_HMAC && ctx->key) {
+		unsigned int keylen = ctx->keylen;
+		u8 *key = ctx->key;
+
+		dev_dbg(device_data->dev, "[%s] keylen: %d", __func__,
+				ctx->keylen);
+		hash_hw_write_key(device_data, key, keylen);
+	}
+
+	hash_get_digest(device_data, digest, ctx->config.algorithm);
+	memcpy(req->result, digest, ctx->digestsize);
+
+out:
+	release_hash_device(device_data);
+
+	/**
+	 * Allocated in setkey, and only used in HMAC.
+	 */
+	kfree(ctx->key);
+
+	return ret;
+}
+
+/**
+ * hash_hw_final - The final hash calculation function
+ * @req:	The hash request for the job.
+ */
+int hash_hw_final(struct ahash_request *req)
+{
+	int ret = 0;
+	struct crypto_ahash *tfm = crypto_ahash_reqtfm(req);
+	struct hash_ctx *ctx = crypto_ahash_ctx(tfm);
+	struct hash_req_ctx *req_ctx = ahash_request_ctx(req);
+	struct hash_device_data *device_data;
+	u8 digest[SHA256_DIGEST_SIZE];
+
+	ret = hash_get_device_data(ctx, &device_data);
+	if (ret)
+		return ret;
+
+	dev_dbg(device_data->dev, "[%s] (ctx=0x%x)!", __func__, (u32) ctx);
+
+	if (req_ctx->updated) {
+		ret = hash_resume_state(device_data, &device_data->state);
+
+		if (ret) {
+			dev_err(device_data->dev, "[%s] hash_resume_state() "
+					"failed!", __func__);
+			goto out;
+		}
+	} else if (req->nbytes == 0 && ctx->keylen == 0) {
+		u8 zero_hash[SHA256_DIGEST_SIZE];
+		u32 zero_hash_size = 0;
+		bool zero_digest = false;
+		/**
+		 * Use a pre-calculated empty message digest
+		 * (workaround since hw return zeroes, hw bug!?)
+		 */
+		ret = get_empty_message_digest(device_data, &zero_hash[0],
+				&zero_hash_size, &zero_digest);
+		if (!ret && likely(zero_hash_size == ctx->digestsize) &&
+				zero_digest) {
+			memcpy(req->result, &zero_hash[0], ctx->digestsize);
+			goto out;
+		} else if (!ret && !zero_digest) {
+			dev_dbg(device_data->dev, "[%s] HMAC zero msg with "
+					"key, continue...", __func__);
+		} else {
+			dev_err(device_data->dev, "[%s] ret=%d, or wrong "
+					"digest size? %s", __func__, ret,
+					(zero_hash_size == ctx->digestsize) ?
+					"true" : "false");
+			/* Return error */
+			goto out;
+		}
+	} else if (req->nbytes == 0 && ctx->keylen > 0) {
+		dev_err(device_data->dev, "[%s] Empty message with "
+				"keylength > 0, NOT supported.", __func__);
+		goto out;
+	}
+
+	if (!req_ctx->updated) {
+		ret = init_hash_hw(device_data, ctx);
+		if (ret) {
+			dev_err(device_data->dev, "[%s] init_hash_hw() "
+					"failed!", __func__);
+			goto out;
+		}
+	}
+
+	if (req_ctx->state.index) {
+		hash_messagepad(device_data, req_ctx->state.buffer,
+				req_ctx->state.index);
+	} else {
+		HASH_SET_DCAL;
+		while (device_data->base->str & HASH_STR_DCAL_MASK)
+			cpu_relax();
+	}
+
+	if (ctx->config.oper_mode == HASH_OPER_MODE_HMAC && ctx->key) {
+		unsigned int keylen = ctx->keylen;
+		u8 *key = ctx->key;
+
+		dev_dbg(device_data->dev, "[%s] keylen: %d", __func__,
+				ctx->keylen);
+		hash_hw_write_key(device_data, key, keylen);
+	}
+
+	hash_get_digest(device_data, digest, ctx->config.algorithm);
+	memcpy(req->result, digest, ctx->digestsize);
+
+out:
+	release_hash_device(device_data);
+
+	/**
+	 * Allocated in setkey, and only used in HMAC.
+	 */
+	kfree(ctx->key);
+
+	return ret;
+}
+
+/**
+ * hash_hw_update - Updates current HASH computation hashing another part of
+ *                  the message.
+ * @req:	Byte array containing the message to be hashed (caller
+ *		allocated).
+ */
+int hash_hw_update(struct ahash_request *req)
+{
+	int ret = 0;
+	u8 index = 0;
+	u8 *buffer;
+	struct hash_device_data *device_data;
+	u8 *data_buffer;
+	struct crypto_ahash *tfm = crypto_ahash_reqtfm(req);
+	struct hash_ctx *ctx = crypto_ahash_ctx(tfm);
+	struct hash_req_ctx *req_ctx = ahash_request_ctx(req);
+	struct crypto_hash_walk walk;
+	int msg_length = crypto_hash_walk_first(req, &walk);
+
+	/* Empty message ("") is correct indata */
+	if (msg_length == 0)
+		return ret;
+
+	index = req_ctx->state.index;
+	buffer = (u8 *)req_ctx->state.buffer;
+
+	/* Check if ctx->state.length + msg_length
+	   overflows */
+	if (msg_length > (req_ctx->state.length.low_word + msg_length) &&
+			HASH_HIGH_WORD_MAX_VAL ==
+			req_ctx->state.length.high_word) {
+		pr_err(DEV_DBG_NAME " [%s] HASH_MSG_LENGTH_OVERFLOW!",
+				__func__);
+		return -EPERM;
+	}
+
+	ret = hash_get_device_data(ctx, &device_data);
+	if (ret)
+		return ret;
+
+	/* Main loop */
+	while (0 != msg_length) {
+		data_buffer = walk.data;
+		ret = hash_process_data(device_data, ctx, req_ctx, msg_length,
+				data_buffer, buffer, &index);
+
+		if (ret) {
+			dev_err(device_data->dev, "[%s] hash_internal_hw_"
+					"update() failed!", __func__);
+			goto out;
+		}
+
+		msg_length = crypto_hash_walk_done(&walk, 0);
+	}
+
+	req_ctx->state.index = index;
+	dev_dbg(device_data->dev, "[%s] indata length=%d, bin=%d))",
+			__func__, req_ctx->state.index,
+			req_ctx->state.bit_index);
+
+out:
+	release_hash_device(device_data);
+
+	return ret;
+}
+
+/**
+ * hash_resume_state - Function that resumes the state of an calculation.
+ * @device_data:	Pointer to the device structure.
+ * @device_state:	The state to be restored in the hash hardware
+ */
+int hash_resume_state(struct hash_device_data *device_data,
+		const struct hash_state *device_state)
+{
+	u32 temp_cr;
+	s32 count;
+	int hash_mode = HASH_OPER_MODE_HASH;
+
+	if (NULL == device_state) {
+		dev_err(device_data->dev, "[%s] HASH_INVALID_PARAMETER!",
+				__func__);
+		return -EPERM;
+	}
+
+	/* Check correctness of index and length members */
+	if (device_state->index > HASH_BLOCK_SIZE
+	    || (device_state->length.low_word % HASH_BLOCK_SIZE) != 0) {
+		dev_err(device_data->dev, "[%s] HASH_INVALID_PARAMETER!",
+				__func__);
+		return -EPERM;
+	}
+
+	/*
+	 * INIT bit. Set this bit to 0b1 to reset the HASH processor core and
+	 * prepare the initialize the HASH accelerator to compute the message
+	 * digest of a new message.
+	 */
+	HASH_INITIALIZE;
+
+	temp_cr = device_state->temp_cr;
+	writel_relaxed(temp_cr & HASH_CR_RESUME_MASK, &device_data->base->cr);
+
+	if (device_data->base->cr & HASH_CR_MODE_MASK)
+		hash_mode = HASH_OPER_MODE_HMAC;
+	else
+		hash_mode = HASH_OPER_MODE_HASH;
+
+	for (count = 0; count < HASH_CSR_COUNT; count++) {
+		if ((count >= 36) && (hash_mode == HASH_OPER_MODE_HASH))
+			break;
+
+		writel_relaxed(device_state->csr[count],
+				&device_data->base->csrx[count]);
+	}
+
+	writel_relaxed(device_state->csfull, &device_data->base->csfull);
+	writel_relaxed(device_state->csdatain, &device_data->base->csdatain);
+
+	writel_relaxed(device_state->str_reg, &device_data->base->str);
+	writel_relaxed(temp_cr, &device_data->base->cr);
+
+	return 0;
+}
+
+/**
+ * hash_save_state - Function that saves the state of hardware.
+ * @device_data:	Pointer to the device structure.
+ * @device_state:	The strucure where the hardware state should be saved.
+ */
+int hash_save_state(struct hash_device_data *device_data,
+		struct hash_state *device_state)
+{
+	u32 temp_cr;
+	u32 count;
+	int hash_mode = HASH_OPER_MODE_HASH;
+
+	if (NULL == device_state) {
+		dev_err(device_data->dev, "[%s] HASH_INVALID_PARAMETER!",
+				__func__);
+		return -ENOTSUPP;
+	}
+
+	/* Write dummy value to force digest intermediate calculation. This
+	 * actually makes sure that there isn't any ongoing calculation in the
+	 * hardware.
+	 */
+	while (device_data->base->str & HASH_STR_DCAL_MASK)
+		cpu_relax();
+
+	temp_cr = readl_relaxed(&device_data->base->cr);
+
+	device_state->str_reg = readl_relaxed(&device_data->base->str);
+
+	device_state->din_reg = readl_relaxed(&device_data->base->din);
+
+	if (device_data->base->cr & HASH_CR_MODE_MASK)
+		hash_mode = HASH_OPER_MODE_HMAC;
+	else
+		hash_mode = HASH_OPER_MODE_HASH;
+
+	for (count = 0; count < HASH_CSR_COUNT; count++) {
+		if ((count >= 36) && (hash_mode == HASH_OPER_MODE_HASH))
+			break;
+
+		device_state->csr[count] =
+			readl_relaxed(&device_data->base->csrx[count]);
+	}
+
+	device_state->csfull = readl_relaxed(&device_data->base->csfull);
+	device_state->csdatain = readl_relaxed(&device_data->base->csdatain);
+
+	device_state->temp_cr = temp_cr;
+
+	return 0;
+}
+
+/**
+ * hash_check_hw - This routine checks for peripheral Ids and PCell Ids.
+ * @device_data:
+ *
+ */
+int hash_check_hw(struct hash_device_data *device_data)
+{
+	/* Checking Peripheral Ids  */
+	if (HASH_P_ID0 == readl_relaxed(&device_data->base->periphid0)
+		&& HASH_P_ID1 == readl_relaxed(&device_data->base->periphid1)
+		&& HASH_P_ID2 == readl_relaxed(&device_data->base->periphid2)
+		&& HASH_P_ID3 == readl_relaxed(&device_data->base->periphid3)
+		&& HASH_CELL_ID0 == readl_relaxed(&device_data->base->cellid0)
+		&& HASH_CELL_ID1 == readl_relaxed(&device_data->base->cellid1)
+		&& HASH_CELL_ID2 == readl_relaxed(&device_data->base->cellid2)
+		&& HASH_CELL_ID3 == readl_relaxed(&device_data->base->cellid3)
+	   ) {
+		return 0;
+	}
+
+	dev_err(device_data->dev, "[%s] HASH_UNSUPPORTED_HW!",
+			__func__);
+	return -ENOTSUPP;
+}
+
+/**
+ * hash_get_digest - Gets the digest.
+ * @device_data:	Pointer to the device structure.
+ * @digest:		User allocated byte array for the calculated digest.
+ * @algorithm:		The algorithm in use.
+ */
+void hash_get_digest(struct hash_device_data *device_data,
+		u8 *digest, int algorithm)
+{
+	u32 temp_hx_val, count;
+	int loop_ctr;
+
+	if (algorithm != HASH_ALGO_SHA1 && algorithm != HASH_ALGO_SHA256) {
+		dev_err(device_data->dev, "[%s] Incorrect algorithm %d",
+				__func__, algorithm);
+		return;
+	}
+
+	if (algorithm == HASH_ALGO_SHA1)
+		loop_ctr = SHA1_DIGEST_SIZE / sizeof(u32);
+	else
+		loop_ctr = SHA256_DIGEST_SIZE / sizeof(u32);
+
+	dev_dbg(device_data->dev, "[%s] digest array:(0x%x)",
+			__func__, (u32) digest);
+
+	/* Copy result into digest array */
+	for (count = 0; count < loop_ctr; count++) {
+		temp_hx_val = readl_relaxed(&device_data->base->hx[count]);
+		digest[count * 4] = (u8) ((temp_hx_val >> 24) & 0xFF);
+		digest[count * 4 + 1] = (u8) ((temp_hx_val >> 16) & 0xFF);
+		digest[count * 4 + 2] = (u8) ((temp_hx_val >> 8) & 0xFF);
+		digest[count * 4 + 3] = (u8) ((temp_hx_val >> 0) & 0xFF);
+	}
+}
+
+/**
+ * hash_update - The hash update function for SHA1/SHA2 (SHA256).
+ * @req: The hash request for the job.
+ */
+static int ahash_update(struct ahash_request *req)
+{
+	int ret = 0;
+	struct hash_req_ctx *req_ctx = ahash_request_ctx(req);
+
+	if (hash_mode != HASH_MODE_DMA || !req_ctx->dma_mode)
+		ret = hash_hw_update(req);
+	/* Skip update for DMA, all data will be passed to DMA in final */
+
+	if (ret) {
+		pr_err(DEV_DBG_NAME " [%s] hash_hw_update() failed!",
+				__func__);
+	}
+
+	return ret;
+}
+
+/**
+ * hash_final - The hash final function for SHA1/SHA2 (SHA256).
+ * @req:	The hash request for the job.
+ */
+static int ahash_final(struct ahash_request *req)
+{
+	int ret = 0;
+	struct hash_req_ctx *req_ctx = ahash_request_ctx(req);
+
+	pr_debug(DEV_DBG_NAME " [%s] data size: %d", __func__, req->nbytes);
+
+	if ((hash_mode == HASH_MODE_DMA) && req_ctx->dma_mode)
+		ret = hash_dma_final(req);
+	else
+		ret = hash_hw_final(req);
+
+	if (ret) {
+		pr_err(DEV_DBG_NAME " [%s] hash_hw/dma_final() failed",
+				__func__);
+	}
+
+	return ret;
+}
+
+static int hash_setkey(struct crypto_ahash *tfm,
+		const u8 *key, unsigned int keylen, int alg)
+{
+	int ret = 0;
+	struct hash_ctx *ctx = crypto_ahash_ctx(tfm);
+
+	/**
+	 * Freed in final.
+	 */
+	ctx->key = kmalloc(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;
+}
+
+static int ahash_sha1_init(struct ahash_request *req)
+{
+	struct crypto_ahash *tfm = crypto_ahash_reqtfm(req);
+	struct hash_ctx *ctx = crypto_ahash_ctx(tfm);
+
+	ctx->config.data_format = HASH_DATA_8_BITS;
+	ctx->config.algorithm = HASH_ALGO_SHA1;
+	ctx->config.oper_mode = HASH_OPER_MODE_HASH;
+	ctx->digestsize = SHA1_DIGEST_SIZE;
+
+	return hash_init(req);
+}
+
+static int ahash_sha256_init(struct ahash_request *req)
+{
+	struct crypto_ahash *tfm = crypto_ahash_reqtfm(req);
+	struct hash_ctx *ctx = crypto_ahash_ctx(tfm);
+
+	ctx->config.data_format = HASH_DATA_8_BITS;
+	ctx->config.algorithm = HASH_ALGO_SHA256;
+	ctx->config.oper_mode = HASH_OPER_MODE_HASH;
+	ctx->digestsize = SHA256_DIGEST_SIZE;
+
+	return hash_init(req);
+}
+
+static int ahash_sha1_digest(struct ahash_request *req)
+{
+	int ret2, ret1;
+
+	ret1 = ahash_sha1_init(req);
+	if (ret1)
+		goto out;
+
+	ret1 = ahash_update(req);
+	ret2 = ahash_final(req);
+
+out:
+	return ret1 ? ret1 : ret2;
+}
+
+static int ahash_sha256_digest(struct ahash_request *req)
+{
+	int ret2, ret1;
+
+	ret1 = ahash_sha256_init(req);
+	if (ret1)
+		goto out;
+
+	ret1 = ahash_update(req);
+	ret2 = ahash_final(req);
+
+out:
+	return ret1 ? ret1 : ret2;
+}
+
+static int hmac_sha1_init(struct ahash_request *req)
+{
+	struct crypto_ahash *tfm = crypto_ahash_reqtfm(req);
+	struct hash_ctx *ctx = crypto_ahash_ctx(tfm);
+
+	ctx->config.data_format	= HASH_DATA_8_BITS;
+	ctx->config.algorithm	= HASH_ALGO_SHA1;
+	ctx->config.oper_mode	= HASH_OPER_MODE_HMAC;
+	ctx->digestsize		= SHA1_DIGEST_SIZE;
+
+	return hash_init(req);
+}
+
+static int hmac_sha256_init(struct ahash_request *req)
+{
+	struct crypto_ahash *tfm = crypto_ahash_reqtfm(req);
+	struct hash_ctx *ctx = crypto_ahash_ctx(tfm);
+
+	ctx->config.data_format	= HASH_DATA_8_BITS;
+	ctx->config.algorithm	= HASH_ALGO_SHA256;
+	ctx->config.oper_mode	= HASH_OPER_MODE_HMAC;
+	ctx->digestsize		= SHA256_DIGEST_SIZE;
+
+	return hash_init(req);
+}
+
+static int hmac_sha1_digest(struct ahash_request *req)
+{
+	int ret2, ret1;
+
+	ret1 = hmac_sha1_init(req);
+	if (ret1)
+		goto out;
+
+	ret1 = ahash_update(req);
+	ret2 = ahash_final(req);
+
+out:
+	return ret1 ? ret1 : ret2;
+}
+
+static int hmac_sha256_digest(struct ahash_request *req)
+{
+	int ret2, ret1;
+
+	ret1 = hmac_sha256_init(req);
+	if (ret1)
+		goto out;
+
+	ret1 = ahash_update(req);
+	ret2 = ahash_final(req);
+
+out:
+	return ret1 ? ret1 : ret2;
+}
+
+static int hmac_sha1_setkey(struct crypto_ahash *tfm,
+		const u8 *key, unsigned int keylen)
+{
+	return hash_setkey(tfm, key, keylen, HASH_ALGO_SHA1);
+}
+
+static int hmac_sha256_setkey(struct crypto_ahash *tfm,
+		const u8 *key, unsigned int keylen)
+{
+	return hash_setkey(tfm, key, keylen, HASH_ALGO_SHA256);
+}
+
+struct hash_algo_template {
+	struct hash_config conf;
+	struct ahash_alg hash;
+};
+
+static int hash_cra_init(struct crypto_tfm *tfm)
+{
+	struct hash_ctx *ctx = crypto_tfm_ctx(tfm);
+	struct crypto_alg *alg = tfm->__crt_alg;
+	struct hash_algo_template *hash_alg;
+
+	hash_alg = container_of(__crypto_ahash_alg(alg),
+			struct hash_algo_template,
+			hash);
+
+	crypto_ahash_set_reqsize(__crypto_ahash_cast(tfm),
+			sizeof(struct hash_req_ctx));
+
+	ctx->config.data_format = HASH_DATA_8_BITS;
+	ctx->config.algorithm = hash_alg->conf.algorithm;
+	ctx->config.oper_mode = hash_alg->conf.oper_mode;
+
+	ctx->digestsize = hash_alg->hash.halg.digestsize;
+
+	return 0;
+}
+
+static struct hash_algo_template hash_algs[] = {
+	{
+			.conf.algorithm	= HASH_ALGO_SHA1,
+			.conf.oper_mode	= HASH_OPER_MODE_HASH,
+			.hash = {
+				.init = hash_init,
+				.update = ahash_update,
+				.final = ahash_final,
+				.digest = ahash_sha1_digest,
+				.halg.digestsize = SHA1_DIGEST_SIZE,
+				.halg.statesize = sizeof(struct hash_ctx),
+				.halg.base = {
+					.cra_name = "sha1",
+					.cra_driver_name = "sha1-ux500",
+					.cra_flags = CRYPTO_ALG_TYPE_AHASH |
+							CRYPTO_ALG_ASYNC,
+					.cra_blocksize = SHA1_BLOCK_SIZE,
+					.cra_ctxsize = sizeof(struct hash_ctx),
+					.cra_init = hash_cra_init,
+					.cra_module = THIS_MODULE,
+			}
+		}
+	},
+	{
+			.conf.algorithm		= HASH_ALGO_SHA256,
+			.conf.oper_mode		= HASH_OPER_MODE_HASH,
+			.hash = {
+				.init = hash_init,
+				.update	= ahash_update,
+				.final = ahash_final,
+				.digest = ahash_sha256_digest,
+				.halg.digestsize = SHA256_DIGEST_SIZE,
+				.halg.statesize = sizeof(struct hash_ctx),
+				.halg.base = {
+					.cra_name = "sha256",
+					.cra_driver_name = "sha256-ux500",
+					.cra_flags = CRYPTO_ALG_TYPE_AHASH |
+							CRYPTO_ALG_ASYNC,
+					.cra_blocksize = SHA256_BLOCK_SIZE,
+					.cra_ctxsize = sizeof(struct hash_ctx),
+					.cra_type = &crypto_ahash_type,
+					.cra_init = hash_cra_init,
+					.cra_module = THIS_MODULE,
+				}
+			}
+
+	},
+	{
+			.conf.algorithm		= HASH_ALGO_SHA1,
+			.conf.oper_mode		= HASH_OPER_MODE_HMAC,
+			.hash = {
+				.init = hash_init,
+				.update = ahash_update,
+				.final = ahash_final,
+				.digest = hmac_sha1_digest,
+				.setkey = hmac_sha1_setkey,
+				.halg.digestsize = SHA1_DIGEST_SIZE,
+				.halg.statesize = sizeof(struct hash_ctx),
+				.halg.base = {
+					.cra_name = "hmac(sha1)",
+					.cra_driver_name = "hmac-sha1-ux500",
+					.cra_flags = CRYPTO_ALG_TYPE_AHASH |
+							CRYPTO_ALG_ASYNC,
+					.cra_blocksize = SHA1_BLOCK_SIZE,
+					.cra_ctxsize = sizeof(struct hash_ctx),
+					.cra_type = &crypto_ahash_type,
+					.cra_init = hash_cra_init,
+					.cra_module = THIS_MODULE,
+				}
+			}
+	},
+	{
+			.conf.algorithm		= HASH_ALGO_SHA256,
+			.conf.oper_mode		= HASH_OPER_MODE_HMAC,
+			.hash = {
+				.init = hash_init,
+				.update = ahash_update,
+				.final = ahash_final,
+				.digest = hmac_sha256_digest,
+				.setkey = hmac_sha256_setkey,
+				.halg.digestsize = SHA256_DIGEST_SIZE,
+				.halg.statesize = sizeof(struct hash_ctx),
+				.halg.base = {
+					.cra_name = "hmac(sha256)",
+					.cra_driver_name = "hmac-sha256-ux500",
+					.cra_flags = CRYPTO_ALG_TYPE_AHASH |
+							CRYPTO_ALG_ASYNC,
+					.cra_blocksize = SHA256_BLOCK_SIZE,
+					.cra_ctxsize = sizeof(struct hash_ctx),
+					.cra_type = &crypto_ahash_type,
+					.cra_init = hash_cra_init,
+					.cra_module = THIS_MODULE,
+				}
+			}
+	}
+};
+
+/**
+ * hash_algs_register_all -
+ */
+static int ahash_algs_register_all(struct hash_device_data *device_data)
+{
+	int ret;
+	int i;
+	int count;
+
+	for (i = 0; i < ARRAY_SIZE(hash_algs); i++) {
+		ret = crypto_register_ahash(&hash_algs[i].hash);
+		if (ret) {
+			count = i;
+			dev_err(device_data->dev, "[%s] alg registration failed",
+				hash_algs[i].hash.halg.base.cra_driver_name);
+			goto unreg;
+		}
+	}
+	return 0;
+unreg:
+	for (i = 0; i < count; i++)
+		crypto_unregister_ahash(&hash_algs[i].hash);
+	return ret;
+}
+
+/**
+ * hash_algs_unregister_all -
+ */
+static void ahash_algs_unregister_all(struct hash_device_data *device_data)
+{
+	int i;
+
+	for (i = 0; i < ARRAY_SIZE(hash_algs); i++)
+		crypto_unregister_ahash(&hash_algs[i].hash);
+}
+
+/**
+ * ux500_hash_probe - Function that probes the hash hardware.
+ * @pdev: The platform device.
+ */
+static int ux500_hash_probe(struct platform_device *pdev)
+{
+	int			ret = 0;
+	struct resource		*res = NULL;
+	struct hash_device_data *device_data;
+	struct device		*dev = &pdev->dev;
+
+	device_data = kzalloc(sizeof(struct hash_device_data), GFP_ATOMIC);
+	if (!device_data) {
+		dev_dbg(dev, "[%s] kzalloc() failed!", __func__);
+		ret = -ENOMEM;
+		goto out;
+	}
+
+	device_data->dev = dev;
+	device_data->current_ctx = NULL;
+
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	if (!res) {
+		dev_dbg(dev, "[%s] platform_get_resource() failed!", __func__);
+		ret = -ENODEV;
+		goto out_kfree;
+	}
+
+	res = request_mem_region(res->start, resource_size(res), pdev->name);
+	if (res == NULL) {
+		dev_dbg(dev, "[%s] request_mem_region() failed!", __func__);
+		ret = -EBUSY;
+		goto out_kfree;
+	}
+
+	device_data->base = ioremap(res->start, resource_size(res));
+	if (!device_data->base) {
+		dev_err(dev, "[%s] ioremap() failed!",
+				__func__);
+		ret = -ENOMEM;
+		goto out_free_mem;
+	}
+	spin_lock_init(&device_data->ctx_lock);
+	spin_lock_init(&device_data->power_state_lock);
+
+	/* Enable power for HASH1 hardware block */
+	device_data->regulator = regulator_get(dev, "v-ape");
+	if (IS_ERR(device_data->regulator)) {
+		dev_err(dev, "[%s] regulator_get() failed!", __func__);
+		ret = PTR_ERR(device_data->regulator);
+		device_data->regulator = NULL;
+		goto out_unmap;
+	}
+
+	/* Enable the clock for HASH1 hardware block */
+	device_data->clk = clk_get(dev, NULL);
+	if (IS_ERR(device_data->clk)) {
+		dev_err(dev, "[%s] clk_get() failed!", __func__);
+		ret = PTR_ERR(device_data->clk);
+		goto out_regulator;
+	}
+
+	/* Enable device power (and clock) */
+	ret = hash_enable_power(device_data, false);
+	if (ret) {
+		dev_err(dev, "[%s]: hash_enable_power() failed!", __func__);
+		goto out_clk;
+	}
+
+	ret = hash_check_hw(device_data);
+	if (ret) {
+		dev_err(dev, "[%s] hash_check_hw() failed!", __func__);
+		goto out_power;
+	}
+
+	if (hash_mode == HASH_MODE_DMA)
+		hash_dma_setup_channel(device_data, dev);
+
+	platform_set_drvdata(pdev, device_data);
+
+	/* Put the new device into the device list... */
+	klist_add_tail(&device_data->list_node, &driver_data.device_list);
+	/* ... and signal that a new device is available. */
+	up(&driver_data.device_allocation);
+
+	ret = ahash_algs_register_all(device_data);
+	if (ret) {
+		dev_err(dev, "[%s] ahash_algs_register_all() "
+				"failed!", __func__);
+		goto out_power;
+	}
+
+	dev_info(dev, "[%s] successfully probed\n", __func__);
+	return 0;
+
+out_power:
+	hash_disable_power(device_data, false);
+
+out_clk:
+	clk_put(device_data->clk);
+
+out_regulator:
+	regulator_put(device_data->regulator);
+
+out_unmap:
+	iounmap(device_data->base);
+
+out_free_mem:
+	release_mem_region(res->start, resource_size(res));
+
+out_kfree:
+	kfree(device_data);
+out:
+	return ret;
+}
+
+/**
+ * ux500_hash_remove - Function that removes the hash device from the platform.
+ * @pdev: The platform device.
+ */
+static int ux500_hash_remove(struct platform_device *pdev)
+{
+	struct resource		*res;
+	struct hash_device_data *device_data;
+	struct device		*dev = &pdev->dev;
+
+	device_data = platform_get_drvdata(pdev);
+	if (!device_data) {
+		dev_err(dev, "[%s]: platform_get_drvdata() failed!",
+			__func__);
+		return -ENOMEM;
+	}
+
+	/* Try to decrease the number of available devices. */
+	if (down_trylock(&driver_data.device_allocation))
+		return -EBUSY;
+
+	/* Check that the device is free */
+	spin_lock(&device_data->ctx_lock);
+	/* current_ctx allocates a device, NULL = unallocated */
+	if (device_data->current_ctx) {
+		/* The device is busy */
+		spin_unlock(&device_data->ctx_lock);
+		/* Return the device to the pool. */
+		up(&driver_data.device_allocation);
+		return -EBUSY;
+	}
+
+	spin_unlock(&device_data->ctx_lock);
+
+	/* Remove the device from the list */
+	if (klist_node_attached(&device_data->list_node))
+		klist_remove(&device_data->list_node);
+
+	/* If this was the last device, remove the services */
+	if (list_empty(&driver_data.device_list.k_list))
+		ahash_algs_unregister_all(device_data);
+
+	if (hash_disable_power(device_data, false))
+		dev_err(dev, "[%s]: hash_disable_power() failed",
+			__func__);
+
+	clk_put(device_data->clk);
+	regulator_put(device_data->regulator);
+
+	iounmap(device_data->base);
+
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	if (res)
+		release_mem_region(res->start, resource_size(res));
+
+	kfree(device_data);
+
+	return 0;
+}
+
+/**
+ * ux500_hash_shutdown - Function that shutdown the hash device.
+ * @pdev: The platform device
+ */
+static void ux500_hash_shutdown(struct platform_device *pdev)
+{
+	struct resource *res = NULL;
+	struct hash_device_data *device_data;
+
+	device_data = platform_get_drvdata(pdev);
+	if (!device_data) {
+		dev_err(&pdev->dev, "[%s] platform_get_drvdata() failed!",
+				__func__);
+		return;
+	}
+
+	/* Check that the device is free */
+	spin_lock(&device_data->ctx_lock);
+	/* current_ctx allocates a device, NULL = unallocated */
+	if (!device_data->current_ctx) {
+		if (down_trylock(&driver_data.device_allocation))
+			dev_dbg(&pdev->dev, "[%s]: Cryp still in use!"
+				"Shutting down anyway...", __func__);
+		/**
+		 * (Allocate the device)
+		 * Need to set this to non-null (dummy) value,
+		 * to avoid usage if context switching.
+		 */
+		device_data->current_ctx++;
+	}
+	spin_unlock(&device_data->ctx_lock);
+
+	/* Remove the device from the list */
+	if (klist_node_attached(&device_data->list_node))
+		klist_remove(&device_data->list_node);
+
+	/* If this was the last device, remove the services */
+	if (list_empty(&driver_data.device_list.k_list))
+		ahash_algs_unregister_all(device_data);
+
+	iounmap(device_data->base);
+
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	if (res)
+		release_mem_region(res->start, resource_size(res));
+
+	if (hash_disable_power(device_data, false))
+		dev_err(&pdev->dev, "[%s] hash_disable_power() failed",
+				__func__);
+}
+
+/**
+ * ux500_hash_suspend - Function that suspends the hash device.
+ * @pdev:	The platform device.
+ * @state:	-
+ */
+static int ux500_hash_suspend(struct platform_device *pdev, pm_message_t state)
+{
+	int ret;
+	struct hash_device_data *device_data;
+	struct hash_ctx *temp_ctx = NULL;
+
+	device_data = platform_get_drvdata(pdev);
+	if (!device_data) {
+		dev_err(&pdev->dev, "[%s] platform_get_drvdata() failed!",
+				__func__);
+		return -ENOMEM;
+	}
+
+	spin_lock(&device_data->ctx_lock);
+	if (!device_data->current_ctx)
+		device_data->current_ctx++;
+	spin_unlock(&device_data->ctx_lock);
+
+	if (device_data->current_ctx == ++temp_ctx) {
+		if (down_interruptible(&driver_data.device_allocation))
+			dev_dbg(&pdev->dev, "[%s]: down_interruptible() "
+					"failed", __func__);
+		ret = hash_disable_power(device_data, false);
+
+	} else
+		ret = hash_disable_power(device_data, true);
+
+	if (ret)
+		dev_err(&pdev->dev, "[%s]: hash_disable_power()", __func__);
+
+	return ret;
+}
+
+/**
+ * ux500_hash_resume - Function that resume the hash device.
+ * @pdev:	The platform device.
+ */
+static int ux500_hash_resume(struct platform_device *pdev)
+{
+	int ret = 0;
+	struct hash_device_data *device_data;
+	struct hash_ctx *temp_ctx = NULL;
+
+	device_data = platform_get_drvdata(pdev);
+	if (!device_data) {
+		dev_err(&pdev->dev, "[%s] platform_get_drvdata() failed!",
+				__func__);
+		return -ENOMEM;
+	}
+
+	spin_lock(&device_data->ctx_lock);
+	if (device_data->current_ctx == ++temp_ctx)
+		device_data->current_ctx = NULL;
+	spin_unlock(&device_data->ctx_lock);
+
+	if (!device_data->current_ctx)
+		up(&driver_data.device_allocation);
+	else
+		ret = hash_enable_power(device_data, true);
+
+	if (ret)
+		dev_err(&pdev->dev, "[%s]: hash_enable_power() failed!",
+			__func__);
+
+	return ret;
+}
+
+static struct platform_driver hash_driver = {
+	.probe  = ux500_hash_probe,
+	.remove = ux500_hash_remove,
+	.shutdown = ux500_hash_shutdown,
+	.suspend  = ux500_hash_suspend,
+	.resume   = ux500_hash_resume,
+	.driver = {
+		.owner = THIS_MODULE,
+		.name  = "hash1",
+	}
+};
+
+/**
+ * ux500_hash_mod_init - The kernel module init function.
+ */
+static int __init ux500_hash_mod_init(void)
+{
+	klist_init(&driver_data.device_list, NULL, NULL);
+	/* Initialize the semaphore to 0 devices (locked state) */
+	sema_init(&driver_data.device_allocation, 0);
+
+	return platform_driver_register(&hash_driver);
+}
+
+/**
+ * ux500_hash_mod_fini - The kernel module exit function.
+ */
+static void __exit ux500_hash_mod_fini(void)
+{
+	platform_driver_unregister(&hash_driver);
+	return;
+}
+
+module_init(ux500_hash_mod_init);
+module_exit(ux500_hash_mod_fini);
+
+MODULE_DESCRIPTION("Driver for ST-Ericsson UX500 HASH engine.");
+MODULE_LICENSE("GPL");
+
+MODULE_ALIAS("sha1-all");
+MODULE_ALIAS("sha256-all");
+MODULE_ALIAS("hmac-sha1-all");
+MODULE_ALIAS("hmac-sha256-all");
diff --git a/drivers/devfreq/Kconfig b/drivers/devfreq/Kconfig
index 464fa2147df..f6b0a6e2ea5 100644
--- a/drivers/devfreq/Kconfig
+++ b/drivers/devfreq/Kconfig
@@ -16,7 +16,7 @@ menuconfig PM_DEVFREQ
 	  is attached to a single device and returns a "representative"
 	  clock frequency of the device, which is also attached
 	  to a device by 1-to-1. The device registering devfreq takes the
-	  responsiblity to "interpret" the representative frequency and
+	  responsibility to "interpret" the representative frequency and
 	  to set its every clock accordingly with the "target" callback
 	  given to devfreq.
 
diff --git a/drivers/devfreq/governor_performance.c b/drivers/devfreq/governor_performance.c
index 574a06b1b1d..af75ddd4f15 100644
--- a/drivers/devfreq/governor_performance.c
+++ b/drivers/devfreq/governor_performance.c
@@ -10,6 +10,7 @@
  */
 
 #include <linux/devfreq.h>
+#include "governor.h"
 
 static int devfreq_performance_func(struct devfreq *df,
 				    unsigned long *freq)
@@ -25,8 +26,14 @@ static int devfreq_performance_func(struct devfreq *df,
 	return 0;
 }
 
+static int performance_init(struct devfreq *devfreq)
+{
+	return update_devfreq(devfreq);
+}
+
 const struct devfreq_governor devfreq_performance = {
 	.name = "performance",
+	.init = performance_init,
 	.get_target_freq = devfreq_performance_func,
 	.no_central_polling = true,
 };
diff --git a/drivers/devfreq/governor_powersave.c b/drivers/devfreq/governor_powersave.c
index d742d4a82d6..fec0cdbd247 100644
--- a/drivers/devfreq/governor_powersave.c
+++ b/drivers/devfreq/governor_powersave.c
@@ -10,6 +10,7 @@
  */
 
 #include <linux/devfreq.h>
+#include "governor.h"
 
 static int devfreq_powersave_func(struct devfreq *df,
 				  unsigned long *freq)
@@ -22,8 +23,14 @@ static int devfreq_powersave_func(struct devfreq *df,
 	return 0;
 }
 
+static int powersave_init(struct devfreq *devfreq)
+{
+	return update_devfreq(devfreq);
+}
+
 const struct devfreq_governor devfreq_powersave = {
 	.name = "powersave",
+	.init = powersave_init,
 	.get_target_freq = devfreq_powersave_func,
 	.no_central_polling = true,
 };
diff --git a/drivers/dma/Kconfig b/drivers/dma/Kconfig
index ef378b5b17e..aadeb5be9db 100644
--- a/drivers/dma/Kconfig
+++ b/drivers/dma/Kconfig
@@ -238,6 +238,7 @@ config IMX_DMA
 config MXS_DMA
 	bool "MXS DMA support"
 	depends on SOC_IMX23 || SOC_IMX28
+	select STMP_DEVICE
 	select DMA_ENGINE
 	help
 	  Support the MXS DMA engine. This engine including APBH-DMA
diff --git a/drivers/dma/amba-pl08x.c b/drivers/dma/amba-pl08x.c
index 3d704abd791..49ecbbb8932 100644
--- a/drivers/dma/amba-pl08x.c
+++ b/drivers/dma/amba-pl08x.c
@@ -95,10 +95,14 @@ static struct amba_driver pl08x_amba_driver;
  * struct vendor_data - vendor-specific config parameters for PL08x derivatives
  * @channels: the number of channels available in this variant
  * @dualmaster: whether this version supports dual AHB masters or not.
+ * @nomadik: whether the channels have Nomadik security extension bits
+ *	that need to be checked for permission before use and some registers are
+ *	missing
  */
 struct vendor_data {
 	u8 channels;
 	bool dualmaster;
+	bool nomadik;
 };
 
 /*
@@ -385,7 +389,7 @@ pl08x_get_phy_channel(struct pl08x_driver_data *pl08x,
 
 		spin_lock_irqsave(&ch->lock, flags);
 
-		if (!ch->serving) {
+		if (!ch->locked && !ch->serving) {
 			ch->serving = virt_chan;
 			ch->signal = -1;
 			spin_unlock_irqrestore(&ch->lock, flags);
@@ -1324,7 +1328,7 @@ static struct dma_async_tx_descriptor *pl08x_prep_slave_sg(
 	int ret, tmp;
 
 	dev_dbg(&pl08x->adev->dev, "%s prepare transaction of %d bytes from %s\n",
-			__func__, sgl->length, plchan->name);
+			__func__, sg_dma_len(sgl), plchan->name);
 
 	txd = pl08x_get_txd(plchan, flags);
 	if (!txd) {
@@ -1378,11 +1382,11 @@ static struct dma_async_tx_descriptor *pl08x_prep_slave_sg(
 
 		dsg->len = sg_dma_len(sg);
 		if (direction == DMA_MEM_TO_DEV) {
-			dsg->src_addr = sg_phys(sg);
+			dsg->src_addr = sg_dma_address(sg);
 			dsg->dst_addr = slave_addr;
 		} else {
 			dsg->src_addr = slave_addr;
-			dsg->dst_addr = sg_phys(sg);
+			dsg->dst_addr = sg_dma_address(sg);
 		}
 	}
 
@@ -1484,6 +1488,9 @@ bool pl08x_filter_id(struct dma_chan *chan, void *chan_id)
  */
 static void pl08x_ensure_on(struct pl08x_driver_data *pl08x)
 {
+	/* The Nomadik variant does not have the config register */
+	if (pl08x->vd->nomadik)
+		return;
 	writel(PL080_CONFIG_ENABLE, pl08x->base + PL080_CONFIG);
 }
 
@@ -1616,7 +1623,7 @@ static irqreturn_t pl08x_irq(int irq, void *dev)
 			__func__, err);
 		writel(err, pl08x->base + PL080_ERR_CLEAR);
 	}
-	tc = readl(pl08x->base + PL080_INT_STATUS);
+	tc = readl(pl08x->base + PL080_TC_STATUS);
 	if (tc)
 		writel(tc, pl08x->base + PL080_TC_CLEAR);
 
@@ -1773,8 +1780,10 @@ static int pl08x_debugfs_show(struct seq_file *s, void *data)
 		spin_lock_irqsave(&ch->lock, flags);
 		virt_chan = ch->serving;
 
-		seq_printf(s, "%d\t\t%s\n",
-			   ch->id, virt_chan ? virt_chan->name : "(none)");
+		seq_printf(s, "%d\t\t%s%s\n",
+			   ch->id,
+			   virt_chan ? virt_chan->name : "(none)",
+			   ch->locked ? " LOCKED" : "");
 
 		spin_unlock_irqrestore(&ch->lock, flags);
 	}
@@ -1918,7 +1927,7 @@ static int pl08x_probe(struct amba_device *adev, const struct amba_id *id)
 	}
 
 	/* Initialize physical channels */
-	pl08x->phy_chans = kmalloc((vd->channels * sizeof(*pl08x->phy_chans)),
+	pl08x->phy_chans = kzalloc((vd->channels * sizeof(*pl08x->phy_chans)),
 			GFP_KERNEL);
 	if (!pl08x->phy_chans) {
 		dev_err(&adev->dev, "%s failed to allocate "
@@ -1933,8 +1942,23 @@ static int pl08x_probe(struct amba_device *adev, const struct amba_id *id)
 		ch->id = i;
 		ch->base = pl08x->base + PL080_Cx_BASE(i);
 		spin_lock_init(&ch->lock);
-		ch->serving = NULL;
 		ch->signal = -1;
+
+		/*
+		 * Nomadik variants can have channels that are locked
+		 * down for the secure world only. Lock up these channels
+		 * by perpetually serving a dummy virtual channel.
+		 */
+		if (vd->nomadik) {
+			u32 val;
+
+			val = readl(ch->base + PL080_CH_CONFIG);
+			if (val & (PL080N_CONFIG_ITPROT | PL080N_CONFIG_SECPROT)) {
+				dev_info(&adev->dev, "physical channel %d reserved for secure access only\n", i);
+				ch->locked = true;
+			}
+		}
+
 		dev_dbg(&adev->dev, "physical channel %d is %s\n",
 			i, pl08x_phy_channel_busy(ch) ? "BUSY" : "FREE");
 	}
@@ -2017,6 +2041,12 @@ static struct vendor_data vendor_pl080 = {
 	.dualmaster = true,
 };
 
+static struct vendor_data vendor_nomadik = {
+	.channels = 8,
+	.dualmaster = true,
+	.nomadik = true,
+};
+
 static struct vendor_data vendor_pl081 = {
 	.channels = 2,
 	.dualmaster = false,
@@ -2037,9 +2067,9 @@ static struct amba_id pl08x_ids[] = {
 	},
 	/* Nomadik 8815 PL080 variant */
 	{
-		.id	= 0x00280880,
+		.id	= 0x00280080,
 		.mask	= 0x00ffffff,
-		.data	= &vendor_pl080,
+		.data	= &vendor_nomadik,
 	},
 	{ 0, 0 },
 };
diff --git a/drivers/dma/at_hdmac.c b/drivers/dma/at_hdmac.c
index 445fdf81169..7292aa87b2d 100644
--- a/drivers/dma/at_hdmac.c
+++ b/drivers/dma/at_hdmac.c
@@ -39,7 +39,6 @@
  */
 
 #define	ATC_DEFAULT_CFG		(ATC_FIFOCFG_HALFFIFO)
-#define	ATC_DEFAULT_CTRLA	(0)
 #define	ATC_DEFAULT_CTRLB	(ATC_SIF(AT_DMA_MEM_IF) \
 				|ATC_DIF(AT_DMA_MEM_IF))
 
@@ -245,7 +244,9 @@ atc_chain_complete(struct at_dma_chan *atchan, struct at_desc *desc)
 	dev_vdbg(chan2dev(&atchan->chan_common),
 		"descriptor %u complete\n", txd->cookie);
 
-	dma_cookie_complete(txd);
+	/* mark the descriptor as complete for non cyclic cases only */
+	if (!atc_chan_is_cyclic(atchan))
+		dma_cookie_complete(txd);
 
 	/* move children to free_list */
 	list_splice_init(&desc->tx_list, &atchan->free_list);
@@ -572,7 +573,6 @@ atc_prep_dma_memcpy(struct dma_chan *chan, dma_addr_t dest, dma_addr_t src,
 		return NULL;
 	}
 
-	ctrla =   ATC_DEFAULT_CTRLA;
 	ctrlb =   ATC_DEFAULT_CTRLB | ATC_IEN
 		| ATC_SRC_ADDR_MODE_INCR
 		| ATC_DST_ADDR_MODE_INCR
@@ -583,13 +583,13 @@ atc_prep_dma_memcpy(struct dma_chan *chan, dma_addr_t dest, dma_addr_t src,
 	 * of the most common optimization.
 	 */
 	if (!((src | dest  | len) & 3)) {
-		ctrla |= ATC_SRC_WIDTH_WORD | ATC_DST_WIDTH_WORD;
+		ctrla = ATC_SRC_WIDTH_WORD | ATC_DST_WIDTH_WORD;
 		src_width = dst_width = 2;
 	} else if (!((src | dest | len) & 1)) {
-		ctrla |= ATC_SRC_WIDTH_HALFWORD | ATC_DST_WIDTH_HALFWORD;
+		ctrla = ATC_SRC_WIDTH_HALFWORD | ATC_DST_WIDTH_HALFWORD;
 		src_width = dst_width = 1;
 	} else {
-		ctrla |= ATC_SRC_WIDTH_BYTE | ATC_DST_WIDTH_BYTE;
+		ctrla = ATC_SRC_WIDTH_BYTE | ATC_DST_WIDTH_BYTE;
 		src_width = dst_width = 0;
 	}
 
@@ -666,7 +666,8 @@ atc_prep_slave_sg(struct dma_chan *chan, struct scatterlist *sgl,
 		return NULL;
 	}
 
-	ctrla = ATC_DEFAULT_CTRLA | atslave->ctrla;
+	ctrla =   ATC_SCSIZE(sconfig->src_maxburst)
+		| ATC_DCSIZE(sconfig->dst_maxburst);
 	ctrlb = ATC_IEN;
 
 	switch (direction) {
@@ -794,12 +795,12 @@ atc_dma_cyclic_fill_desc(struct dma_chan *chan, struct at_desc *desc,
 		enum dma_transfer_direction direction)
 {
 	struct at_dma_chan	*atchan = to_at_dma_chan(chan);
-	struct at_dma_slave	*atslave = chan->private;
 	struct dma_slave_config	*sconfig = &atchan->dma_sconfig;
 	u32			ctrla;
 
 	/* prepare common CRTLA value */
-	ctrla =   ATC_DEFAULT_CTRLA | atslave->ctrla
+	ctrla =   ATC_SCSIZE(sconfig->src_maxburst)
+		| ATC_DCSIZE(sconfig->dst_maxburst)
 		| ATC_DST_WIDTH(reg_width)
 		| ATC_SRC_WIDTH(reg_width)
 		| period_len >> reg_width;
diff --git a/drivers/dma/at_hdmac_regs.h b/drivers/dma/at_hdmac_regs.h
index 897a8bcaec9..8a6c8e8b294 100644
--- a/drivers/dma/at_hdmac_regs.h
+++ b/drivers/dma/at_hdmac_regs.h
@@ -87,7 +87,26 @@
 /* Bitfields in CTRLA */
 #define	ATC_BTSIZE_MAX		0xFFFFUL	/* Maximum Buffer Transfer Size */
 #define	ATC_BTSIZE(x)		(ATC_BTSIZE_MAX & (x)) /* Buffer Transfer Size */
-/* Chunck Tranfer size definitions are in at_hdmac.h */
+#define	ATC_SCSIZE_MASK		(0x7 << 16)	/* Source Chunk Transfer Size */
+#define		ATC_SCSIZE(x)		(ATC_SCSIZE_MASK & ((x) << 16))
+#define		ATC_SCSIZE_1		(0x0 << 16)
+#define		ATC_SCSIZE_4		(0x1 << 16)
+#define		ATC_SCSIZE_8		(0x2 << 16)
+#define		ATC_SCSIZE_16		(0x3 << 16)
+#define		ATC_SCSIZE_32		(0x4 << 16)
+#define		ATC_SCSIZE_64		(0x5 << 16)
+#define		ATC_SCSIZE_128		(0x6 << 16)
+#define		ATC_SCSIZE_256		(0x7 << 16)
+#define	ATC_DCSIZE_MASK		(0x7 << 20)	/* Destination Chunk Transfer Size */
+#define		ATC_DCSIZE(x)		(ATC_DCSIZE_MASK & ((x) << 20))
+#define		ATC_DCSIZE_1		(0x0 << 20)
+#define		ATC_DCSIZE_4		(0x1 << 20)
+#define		ATC_DCSIZE_8		(0x2 << 20)
+#define		ATC_DCSIZE_16		(0x3 << 20)
+#define		ATC_DCSIZE_32		(0x4 << 20)
+#define		ATC_DCSIZE_64		(0x5 << 20)
+#define		ATC_DCSIZE_128		(0x6 << 20)
+#define		ATC_DCSIZE_256		(0x7 << 20)
 #define	ATC_SRC_WIDTH_MASK	(0x3 << 24)	/* Source Single Transfer Size */
 #define		ATC_SRC_WIDTH(x)	((x) << 24)
 #define		ATC_SRC_WIDTH_BYTE	(0x0 << 24)
diff --git a/drivers/dma/coh901318.c b/drivers/dma/coh901318.c
index 750925f9638..e67b4e06a91 100644
--- a/drivers/dma/coh901318.c
+++ b/drivers/dma/coh901318.c
@@ -1033,7 +1033,7 @@ coh901318_prep_slave_sg(struct dma_chan *chan, struct scatterlist *sgl,
 
 	if (!sgl)
 		goto out;
-	if (sgl->length == 0)
+	if (sg_dma_len(sgl) == 0)
 		goto out;
 
 	spin_lock_irqsave(&cohc->lock, flg);
diff --git a/drivers/dma/coh901318_lli.c b/drivers/dma/coh901318_lli.c
index 6c0e2d4c668..780e0429b38 100644
--- a/drivers/dma/coh901318_lli.c
+++ b/drivers/dma/coh901318_lli.c
@@ -270,10 +270,10 @@ coh901318_lli_fill_sg(struct coh901318_pool *pool,
 
 		if (dir == DMA_MEM_TO_DEV)
 			/* increment source address */
-			src = sg_phys(sg);
+			src = sg_dma_address(sg);
 		else
 			/* increment destination address */
-			dst =  sg_phys(sg);
+			dst = sg_dma_address(sg);
 
 		bytes_to_transfer = sg_dma_len(sg);
 
diff --git a/drivers/dma/dw_dmac.c b/drivers/dma/dw_dmac.c
index 7439079f5ee..e23dc82d43a 100644
--- a/drivers/dma/dw_dmac.c
+++ b/drivers/dma/dw_dmac.c
@@ -17,6 +17,7 @@
 #include <linux/init.h>
 #include <linux/interrupt.h>
 #include <linux/io.h>
+#include <linux/of.h>
 #include <linux/mm.h>
 #include <linux/module.h>
 #include <linux/platform_device.h>
@@ -742,7 +743,7 @@ dwc_prep_slave_sg(struct dma_chan *chan, struct scatterlist *sgl,
 			struct dw_desc	*desc;
 			u32		len, dlen, mem;
 
-			mem = sg_phys(sg);
+			mem = sg_dma_address(sg);
 			len = sg_dma_len(sg);
 
 			if (!((mem | len) & 7))
@@ -809,7 +810,7 @@ slave_sg_todev_fill_desc:
 			struct dw_desc	*desc;
 			u32		len, dlen, mem;
 
-			mem = sg_phys(sg);
+			mem = sg_dma_address(sg);
 			len = sg_dma_len(sg);
 
 			if (!((mem | len) & 7))
@@ -1429,7 +1430,7 @@ static int __init dw_probe(struct platform_device *pdev)
 		err = PTR_ERR(dw->clk);
 		goto err_clk;
 	}
-	clk_enable(dw->clk);
+	clk_prepare_enable(dw->clk);
 
 	/* force dma off, just in case */
 	dw_dma_off(dw);
@@ -1510,7 +1511,7 @@ static int __init dw_probe(struct platform_device *pdev)
 	return 0;
 
 err_irq:
-	clk_disable(dw->clk);
+	clk_disable_unprepare(dw->clk);
 	clk_put(dw->clk);
 err_clk:
 	iounmap(dw->regs);
@@ -1540,7 +1541,7 @@ static int __exit dw_remove(struct platform_device *pdev)
 		channel_clear_bit(dw, CH_EN, dwc->mask);
 	}
 
-	clk_disable(dw->clk);
+	clk_disable_unprepare(dw->clk);
 	clk_put(dw->clk);
 
 	iounmap(dw->regs);
@@ -1559,7 +1560,7 @@ static void dw_shutdown(struct platform_device *pdev)
 	struct dw_dma	*dw = platform_get_drvdata(pdev);
 
 	dw_dma_off(platform_get_drvdata(pdev));
-	clk_disable(dw->clk);
+	clk_disable_unprepare(dw->clk);
 }
 
 static int dw_suspend_noirq(struct device *dev)
@@ -1568,7 +1569,7 @@ static int dw_suspend_noirq(struct device *dev)
 	struct dw_dma	*dw = platform_get_drvdata(pdev);
 
 	dw_dma_off(platform_get_drvdata(pdev));
-	clk_disable(dw->clk);
+	clk_disable_unprepare(dw->clk);
 
 	return 0;
 }
@@ -1578,7 +1579,7 @@ static int dw_resume_noirq(struct device *dev)
 	struct platform_device *pdev = to_platform_device(dev);
 	struct dw_dma	*dw = platform_get_drvdata(pdev);
 
-	clk_enable(dw->clk);
+	clk_prepare_enable(dw->clk);
 	dma_writel(dw, CFG, DW_CFG_DMA_EN);
 	return 0;
 }
@@ -1592,12 +1593,21 @@ static const struct dev_pm_ops dw_dev_pm_ops = {
 	.poweroff_noirq = dw_suspend_noirq,
 };
 
+#ifdef CONFIG_OF
+static const struct of_device_id dw_dma_id_table[] = {
+	{ .compatible = "snps,dma-spear1340" },
+	{}
+};
+MODULE_DEVICE_TABLE(of, dw_dma_id_table);
+#endif
+
 static struct platform_driver dw_driver = {
 	.remove		= __exit_p(dw_remove),
 	.shutdown	= dw_shutdown,
 	.driver = {
 		.name	= "dw_dmac",
 		.pm	= &dw_dev_pm_ops,
+		.of_match_table = of_match_ptr(dw_dma_id_table),
 	},
 };
 
diff --git a/drivers/dma/ep93xx_dma.c b/drivers/dma/ep93xx_dma.c
index e6f133b78dc..c64917ec313 100644
--- a/drivers/dma/ep93xx_dma.c
+++ b/drivers/dma/ep93xx_dma.c
@@ -71,6 +71,7 @@
 #define M2M_CONTROL_TM_SHIFT		13
 #define M2M_CONTROL_TM_TX		(1 << M2M_CONTROL_TM_SHIFT)
 #define M2M_CONTROL_TM_RX		(2 << M2M_CONTROL_TM_SHIFT)
+#define M2M_CONTROL_NFBINT		BIT(21)
 #define M2M_CONTROL_RSS_SHIFT		22
 #define M2M_CONTROL_RSS_SSPRX		(1 << M2M_CONTROL_RSS_SHIFT)
 #define M2M_CONTROL_RSS_SSPTX		(2 << M2M_CONTROL_RSS_SHIFT)
@@ -79,7 +80,22 @@
 #define M2M_CONTROL_PWSC_SHIFT		25
 
 #define M2M_INTERRUPT			0x0004
-#define M2M_INTERRUPT_DONEINT		BIT(1)
+#define M2M_INTERRUPT_MASK		6
+
+#define M2M_STATUS			0x000c
+#define M2M_STATUS_CTL_SHIFT		1
+#define M2M_STATUS_CTL_IDLE		(0 << M2M_STATUS_CTL_SHIFT)
+#define M2M_STATUS_CTL_STALL		(1 << M2M_STATUS_CTL_SHIFT)
+#define M2M_STATUS_CTL_MEMRD		(2 << M2M_STATUS_CTL_SHIFT)
+#define M2M_STATUS_CTL_MEMWR		(3 << M2M_STATUS_CTL_SHIFT)
+#define M2M_STATUS_CTL_BWCWAIT		(4 << M2M_STATUS_CTL_SHIFT)
+#define M2M_STATUS_CTL_MASK		(7 << M2M_STATUS_CTL_SHIFT)
+#define M2M_STATUS_BUF_SHIFT		4
+#define M2M_STATUS_BUF_NO		(0 << M2M_STATUS_BUF_SHIFT)
+#define M2M_STATUS_BUF_ON		(1 << M2M_STATUS_BUF_SHIFT)
+#define M2M_STATUS_BUF_NEXT		(2 << M2M_STATUS_BUF_SHIFT)
+#define M2M_STATUS_BUF_MASK		(3 << M2M_STATUS_BUF_SHIFT)
+#define M2M_STATUS_DONE			BIT(6)
 
 #define M2M_BCR0			0x0010
 #define M2M_BCR1			0x0014
@@ -426,15 +442,6 @@ static int m2p_hw_interrupt(struct ep93xx_dma_chan *edmac)
 
 /*
  * M2M DMA implementation
- *
- * For the M2M transfers we don't use NFB at all. This is because it simply
- * doesn't work well with memcpy transfers. When you submit both buffers it is
- * extremely unlikely that you get an NFB interrupt, but it instead reports
- * DONE interrupt and both buffers are already transferred which means that we
- * weren't able to update the next buffer.
- *
- * So for now we "simulate" NFB by just submitting buffer after buffer
- * without double buffering.
  */
 
 static int m2m_hw_setup(struct ep93xx_dma_chan *edmac)
@@ -543,6 +550,11 @@ static void m2m_hw_submit(struct ep93xx_dma_chan *edmac)
 	m2m_fill_desc(edmac);
 	control |= M2M_CONTROL_DONEINT;
 
+	if (ep93xx_dma_advance_active(edmac)) {
+		m2m_fill_desc(edmac);
+		control |= M2M_CONTROL_NFBINT;
+	}
+
 	/*
 	 * Now we can finally enable the channel. For M2M channel this must be
 	 * done _after_ the BCRx registers are programmed.
@@ -560,32 +572,89 @@ static void m2m_hw_submit(struct ep93xx_dma_chan *edmac)
 	}
 }
 
+/*
+ * According to EP93xx User's Guide, we should receive DONE interrupt when all
+ * M2M DMA controller transactions complete normally. This is not always the
+ * case - sometimes EP93xx M2M DMA asserts DONE interrupt when the DMA channel
+ * is still running (channel Buffer FSM in DMA_BUF_ON state, and channel
+ * Control FSM in DMA_MEM_RD state, observed at least in IDE-DMA operation).
+ * In effect, disabling the channel when only DONE bit is set could stop
+ * currently running DMA transfer. To avoid this, we use Buffer FSM and
+ * Control FSM to check current state of DMA channel.
+ */
 static int m2m_hw_interrupt(struct ep93xx_dma_chan *edmac)
 {
+	u32 status = readl(edmac->regs + M2M_STATUS);
+	u32 ctl_fsm = status & M2M_STATUS_CTL_MASK;
+	u32 buf_fsm = status & M2M_STATUS_BUF_MASK;
+	bool done = status & M2M_STATUS_DONE;
+	bool last_done;
 	u32 control;
+	struct ep93xx_dma_desc *desc;
 
-	if (!(readl(edmac->regs + M2M_INTERRUPT) & M2M_INTERRUPT_DONEINT))
+	/* Accept only DONE and NFB interrupts */
+	if (!(readl(edmac->regs + M2M_INTERRUPT) & M2M_INTERRUPT_MASK))
 		return INTERRUPT_UNKNOWN;
 
-	/* Clear the DONE bit */
-	writel(0, edmac->regs + M2M_INTERRUPT);
+	if (done) {
+		/* Clear the DONE bit */
+		writel(0, edmac->regs + M2M_INTERRUPT);
+	}
 
-	/* Disable interrupts and the channel */
-	control = readl(edmac->regs + M2M_CONTROL);
-	control &= ~(M2M_CONTROL_DONEINT | M2M_CONTROL_ENABLE);
-	writel(control, edmac->regs + M2M_CONTROL);
+	/*
+	 * Check whether we are done with descriptors or not. This, together
+	 * with DMA channel state, determines action to take in interrupt.
+	 */
+	desc = ep93xx_dma_get_active(edmac);
+	last_done = !desc || desc->txd.cookie;
 
 	/*
-	 * Since we only get DONE interrupt we have to find out ourselves
-	 * whether there still is something to process. So we try to advance
-	 * the chain an see whether it succeeds.
+	 * Use M2M DMA Buffer FSM and Control FSM to check current state of
+	 * DMA channel. Using DONE and NFB bits from channel status register
+	 * or bits from channel interrupt register is not reliable.
 	 */
-	if (ep93xx_dma_advance_active(edmac)) {
-		edmac->edma->hw_submit(edmac);
-		return INTERRUPT_NEXT_BUFFER;
+	if (!last_done &&
+	    (buf_fsm == M2M_STATUS_BUF_NO ||
+	     buf_fsm == M2M_STATUS_BUF_ON)) {
+		/*
+		 * Two buffers are ready for update when Buffer FSM is in
+		 * DMA_NO_BUF state. Only one buffer can be prepared without
+		 * disabling the channel or polling the DONE bit.
+		 * To simplify things, always prepare only one buffer.
+		 */
+		if (ep93xx_dma_advance_active(edmac)) {
+			m2m_fill_desc(edmac);
+			if (done && !edmac->chan.private) {
+				/* Software trigger for memcpy channel */
+				control = readl(edmac->regs + M2M_CONTROL);
+				control |= M2M_CONTROL_START;
+				writel(control, edmac->regs + M2M_CONTROL);
+			}
+			return INTERRUPT_NEXT_BUFFER;
+		} else {
+			last_done = true;
+		}
+	}
+
+	/*
+	 * Disable the channel only when Buffer FSM is in DMA_NO_BUF state
+	 * and Control FSM is in DMA_STALL state.
+	 */
+	if (last_done &&
+	    buf_fsm == M2M_STATUS_BUF_NO &&
+	    ctl_fsm == M2M_STATUS_CTL_STALL) {
+		/* Disable interrupts and the channel */
+		control = readl(edmac->regs + M2M_CONTROL);
+		control &= ~(M2M_CONTROL_DONEINT | M2M_CONTROL_NFBINT
+			    | M2M_CONTROL_ENABLE);
+		writel(control, edmac->regs + M2M_CONTROL);
+		return INTERRUPT_DONE;
 	}
 
-	return INTERRUPT_DONE;
+	/*
+	 * Nothing to do this time.
+	 */
+	return INTERRUPT_NEXT_BUFFER;
 }
 
 /*
@@ -703,7 +772,9 @@ static void ep93xx_dma_tasklet(unsigned long data)
 	desc = ep93xx_dma_get_active(edmac);
 	if (desc) {
 		if (desc->complete) {
-			dma_cookie_complete(&desc->txd);
+			/* mark descriptor complete for non cyclic case only */
+			if (!test_bit(EP93XX_DMA_IS_CYCLIC, &edmac->flags))
+				dma_cookie_complete(&desc->txd);
 			list_splice_init(&edmac->active, &list);
 		}
 		callback = desc->txd.callback;
diff --git a/drivers/dma/imx-dma.c b/drivers/dma/imx-dma.c
index bb787d8e152..fcfeb3cd8d3 100644
--- a/drivers/dma/imx-dma.c
+++ b/drivers/dma/imx-dma.c
@@ -227,7 +227,7 @@ static inline int imxdma_sg_next(struct imxdma_desc *d)
 	struct scatterlist *sg = d->sg;
 	unsigned long now;
 
-	now = min(d->len, sg->length);
+	now = min(d->len, sg_dma_len(sg));
 	if (d->len != IMX_DMA_LENGTH_LOOP)
 		d->len -= now;
 
@@ -763,16 +763,16 @@ static struct dma_async_tx_descriptor *imxdma_prep_slave_sg(
 	desc = list_first_entry(&imxdmac->ld_free, struct imxdma_desc, node);
 
 	for_each_sg(sgl, sg, sg_len, i) {
-		dma_length += sg->length;
+		dma_length += sg_dma_len(sg);
 	}
 
 	switch (imxdmac->word_size) {
 	case DMA_SLAVE_BUSWIDTH_4_BYTES:
-		if (sgl->length & 3 || sgl->dma_address & 3)
+		if (sg_dma_len(sgl) & 3 || sgl->dma_address & 3)
 			return NULL;
 		break;
 	case DMA_SLAVE_BUSWIDTH_2_BYTES:
-		if (sgl->length & 1 || sgl->dma_address & 1)
+		if (sg_dma_len(sgl) & 1 || sgl->dma_address & 1)
 			return NULL;
 		break;
 	case DMA_SLAVE_BUSWIDTH_1_BYTE:
@@ -831,13 +831,13 @@ static struct dma_async_tx_descriptor *imxdma_prep_dma_cyclic(
 		imxdmac->sg_list[i].page_link = 0;
 		imxdmac->sg_list[i].offset = 0;
 		imxdmac->sg_list[i].dma_address = dma_addr;
-		imxdmac->sg_list[i].length = period_len;
+		sg_dma_len(&imxdmac->sg_list[i]) = period_len;
 		dma_addr += period_len;
 	}
 
 	/* close the loop */
 	imxdmac->sg_list[periods].offset = 0;
-	imxdmac->sg_list[periods].length = 0;
+	sg_dma_len(&imxdmac->sg_list[periods]) = 0;
 	imxdmac->sg_list[periods].page_link =
 		((unsigned long)imxdmac->sg_list | 0x01) & ~0x02;
 
diff --git a/drivers/dma/imx-sdma.c b/drivers/dma/imx-sdma.c
index d3e38e28bb6..fb4f4990f5e 100644
--- a/drivers/dma/imx-sdma.c
+++ b/drivers/dma/imx-sdma.c
@@ -24,7 +24,7 @@
 #include <linux/mm.h>
 #include <linux/interrupt.h>
 #include <linux/clk.h>
-#include <linux/wait.h>
+#include <linux/delay.h>
 #include <linux/sched.h>
 #include <linux/semaphore.h>
 #include <linux/spinlock.h>
@@ -271,6 +271,7 @@ struct sdma_channel {
 	enum dma_status			status;
 	unsigned int			chn_count;
 	unsigned int			chn_real_count;
+	struct tasklet_struct		tasklet;
 };
 
 #define IMX_DMA_SG_LOOP		BIT(0)
@@ -322,8 +323,9 @@ struct sdma_engine {
 	struct sdma_context_data	*context;
 	dma_addr_t			context_phys;
 	struct dma_device		dma_device;
-	struct clk			*clk;
-	struct mutex			channel_0_lock;
+	struct clk			*clk_ipg;
+	struct clk			*clk_ahb;
+	spinlock_t			channel_0_lock;
 	struct sdma_script_start_addrs	*script_addrs;
 };
 
@@ -401,19 +403,27 @@ static void sdma_enable_channel(struct sdma_engine *sdma, int channel)
 }
 
 /*
- * sdma_run_channel - run a channel and wait till it's done
+ * sdma_run_channel0 - run a channel and wait till it's done
  */
-static int sdma_run_channel(struct sdma_channel *sdmac)
+static int sdma_run_channel0(struct sdma_engine *sdma)
 {
-	struct sdma_engine *sdma = sdmac->sdma;
-	int channel = sdmac->channel;
 	int ret;
+	unsigned long timeout = 500;
 
-	init_completion(&sdmac->done);
+	sdma_enable_channel(sdma, 0);
 
-	sdma_enable_channel(sdma, channel);
+	while (!(ret = readl_relaxed(sdma->regs + SDMA_H_INTR) & 1)) {
+		if (timeout-- <= 0)
+			break;
+		udelay(1);
+	}
 
-	ret = wait_for_completion_timeout(&sdmac->done, HZ);
+	if (ret) {
+		/* Clear the interrupt status */
+		writel_relaxed(ret, sdma->regs + SDMA_H_INTR);
+	} else {
+		dev_err(sdma->dev, "Timeout waiting for CH0 ready\n");
+	}
 
 	return ret ? 0 : -ETIMEDOUT;
 }
@@ -425,17 +435,17 @@ static int sdma_load_script(struct sdma_engine *sdma, void *buf, int size,
 	void *buf_virt;
 	dma_addr_t buf_phys;
 	int ret;
-
-	mutex_lock(&sdma->channel_0_lock);
+	unsigned long flags;
 
 	buf_virt = dma_alloc_coherent(NULL,
 			size,
 			&buf_phys, GFP_KERNEL);
 	if (!buf_virt) {
-		ret = -ENOMEM;
-		goto err_out;
+		return -ENOMEM;
 	}
 
+	spin_lock_irqsave(&sdma->channel_0_lock, flags);
+
 	bd0->mode.command = C0_SETPM;
 	bd0->mode.status = BD_DONE | BD_INTR | BD_WRAP | BD_EXTD;
 	bd0->mode.count = size / 2;
@@ -444,12 +454,11 @@ static int sdma_load_script(struct sdma_engine *sdma, void *buf, int size,
 
 	memcpy(buf_virt, buf, size);
 
-	ret = sdma_run_channel(&sdma->channel[0]);
+	ret = sdma_run_channel0(sdma);
 
-	dma_free_coherent(NULL, size, buf_virt, buf_phys);
+	spin_unlock_irqrestore(&sdma->channel_0_lock, flags);
 
-err_out:
-	mutex_unlock(&sdma->channel_0_lock);
+	dma_free_coherent(NULL, size, buf_virt, buf_phys);
 
 	return ret;
 }
@@ -534,13 +543,11 @@ static void mxc_sdma_handle_channel_normal(struct sdma_channel *sdmac)
 		sdmac->desc.callback(sdmac->desc.callback_param);
 }
 
-static void mxc_sdma_handle_channel(struct sdma_channel *sdmac)
+static void sdma_tasklet(unsigned long data)
 {
-	complete(&sdmac->done);
+	struct sdma_channel *sdmac = (struct sdma_channel *) data;
 
-	/* not interested in channel 0 interrupts */
-	if (sdmac->channel == 0)
-		return;
+	complete(&sdmac->done);
 
 	if (sdmac->flags & IMX_DMA_SG_LOOP)
 		sdma_handle_channel_loop(sdmac);
@@ -554,13 +561,15 @@ static irqreturn_t sdma_int_handler(int irq, void *dev_id)
 	unsigned long stat;
 
 	stat = readl_relaxed(sdma->regs + SDMA_H_INTR);
+	/* not interested in channel 0 interrupts */
+	stat &= ~1;
 	writel_relaxed(stat, sdma->regs + SDMA_H_INTR);
 
 	while (stat) {
 		int channel = fls(stat) - 1;
 		struct sdma_channel *sdmac = &sdma->channel[channel];
 
-		mxc_sdma_handle_channel(sdmac);
+		tasklet_schedule(&sdmac->tasklet);
 
 		__clear_bit(channel, &stat);
 	}
@@ -659,6 +668,7 @@ static int sdma_load_context(struct sdma_channel *sdmac)
 	struct sdma_context_data *context = sdma->context;
 	struct sdma_buffer_descriptor *bd0 = sdma->channel[0].bd;
 	int ret;
+	unsigned long flags;
 
 	if (sdmac->direction == DMA_DEV_TO_MEM) {
 		load_address = sdmac->pc_from_device;
@@ -676,7 +686,7 @@ static int sdma_load_context(struct sdma_channel *sdmac)
 	dev_dbg(sdma->dev, "event_mask0 = 0x%08x\n", (u32)sdmac->event_mask[0]);
 	dev_dbg(sdma->dev, "event_mask1 = 0x%08x\n", (u32)sdmac->event_mask[1]);
 
-	mutex_lock(&sdma->channel_0_lock);
+	spin_lock_irqsave(&sdma->channel_0_lock, flags);
 
 	memset(context, 0, sizeof(*context));
 	context->channel_state.pc = load_address;
@@ -695,10 +705,9 @@ static int sdma_load_context(struct sdma_channel *sdmac)
 	bd0->mode.count = sizeof(*context) / 4;
 	bd0->buffer_addr = sdma->context_phys;
 	bd0->ext_buffer_addr = 2048 + (sizeof(*context) / 4) * channel;
+	ret = sdma_run_channel0(sdma);
 
-	ret = sdma_run_channel(&sdma->channel[0]);
-
-	mutex_unlock(&sdma->channel_0_lock);
+	spin_unlock_irqrestore(&sdma->channel_0_lock, flags);
 
 	return ret;
 }
@@ -859,7 +868,8 @@ static int sdma_alloc_chan_resources(struct dma_chan *chan)
 	sdmac->peripheral_type = data->peripheral_type;
 	sdmac->event_id0 = data->dma_request;
 
-	clk_enable(sdmac->sdma->clk);
+	clk_enable(sdmac->sdma->clk_ipg);
+	clk_enable(sdmac->sdma->clk_ahb);
 
 	ret = sdma_request_channel(sdmac);
 	if (ret)
@@ -896,7 +906,8 @@ static void sdma_free_chan_resources(struct dma_chan *chan)
 
 	dma_free_coherent(NULL, PAGE_SIZE, sdmac->bd, sdmac->bd_phys);
 
-	clk_disable(sdma->clk);
+	clk_disable(sdma->clk_ipg);
+	clk_disable(sdma->clk_ahb);
 }
 
 static struct dma_async_tx_descriptor *sdma_prep_slave_sg(
@@ -938,7 +949,7 @@ static struct dma_async_tx_descriptor *sdma_prep_slave_sg(
 
 		bd->buffer_addr = sg->dma_address;
 
-		count = sg->length;
+		count = sg_dma_len(sg);
 
 		if (count > 0xffff) {
 			dev_err(sdma->dev, "SDMA channel %d: maximum bytes for sg entry exceeded: %d > %d\n",
@@ -1169,12 +1180,14 @@ static void sdma_load_firmware(const struct firmware *fw, void *context)
 	addr = (void *)header + header->script_addrs_start;
 	ram_code = (void *)header + header->ram_code_start;
 
-	clk_enable(sdma->clk);
+	clk_enable(sdma->clk_ipg);
+	clk_enable(sdma->clk_ahb);
 	/* download the RAM image for SDMA */
 	sdma_load_script(sdma, ram_code,
 			header->ram_code_size,
 			addr->ram_code_start_addr);
-	clk_disable(sdma->clk);
+	clk_disable(sdma->clk_ipg);
+	clk_disable(sdma->clk_ahb);
 
 	sdma_add_scripts(sdma, addr);
 
@@ -1216,7 +1229,8 @@ static int __init sdma_init(struct sdma_engine *sdma)
 		return -ENODEV;
 	}
 
-	clk_enable(sdma->clk);
+	clk_enable(sdma->clk_ipg);
+	clk_enable(sdma->clk_ahb);
 
 	/* Be sure SDMA has not started yet */
 	writel_relaxed(0, sdma->regs + SDMA_H_C0PTR);
@@ -1269,12 +1283,14 @@ static int __init sdma_init(struct sdma_engine *sdma)
 	/* Initializes channel's priorities */
 	sdma_set_channel_priority(&sdma->channel[0], 7);
 
-	clk_disable(sdma->clk);
+	clk_disable(sdma->clk_ipg);
+	clk_disable(sdma->clk_ahb);
 
 	return 0;
 
 err_dma_alloc:
-	clk_disable(sdma->clk);
+	clk_disable(sdma->clk_ipg);
+	clk_disable(sdma->clk_ahb);
 	dev_err(sdma->dev, "initialisation failed with %d\n", ret);
 	return ret;
 }
@@ -1297,7 +1313,7 @@ static int __init sdma_probe(struct platform_device *pdev)
 	if (!sdma)
 		return -ENOMEM;
 
-	mutex_init(&sdma->channel_0_lock);
+	spin_lock_init(&sdma->channel_0_lock);
 
 	sdma->dev = &pdev->dev;
 
@@ -1313,12 +1329,21 @@ static int __init sdma_probe(struct platform_device *pdev)
 		goto err_request_region;
 	}
 
-	sdma->clk = clk_get(&pdev->dev, NULL);
-	if (IS_ERR(sdma->clk)) {
-		ret = PTR_ERR(sdma->clk);
+	sdma->clk_ipg = devm_clk_get(&pdev->dev, "ipg");
+	if (IS_ERR(sdma->clk_ipg)) {
+		ret = PTR_ERR(sdma->clk_ipg);
 		goto err_clk;
 	}
 
+	sdma->clk_ahb = devm_clk_get(&pdev->dev, "ahb");
+	if (IS_ERR(sdma->clk_ahb)) {
+		ret = PTR_ERR(sdma->clk_ahb);
+		goto err_clk;
+	}
+
+	clk_prepare(sdma->clk_ipg);
+	clk_prepare(sdma->clk_ahb);
+
 	sdma->regs = ioremap(iores->start, resource_size(iores));
 	if (!sdma->regs) {
 		ret = -ENOMEM;
@@ -1359,6 +1384,8 @@ static int __init sdma_probe(struct platform_device *pdev)
 		dma_cookie_init(&sdmac->chan);
 		sdmac->channel = i;
 
+		tasklet_init(&sdmac->tasklet, sdma_tasklet,
+			     (unsigned long) sdmac);
 		/*
 		 * Add the channel to the DMAC list. Do not add channel 0 though
 		 * because we need it internally in the SDMA driver. This also means
@@ -1426,7 +1453,6 @@ err_alloc:
 err_request_irq:
 	iounmap(sdma->regs);
 err_ioremap:
-	clk_put(sdma->clk);
 err_clk:
 	release_mem_region(iores->start, resource_size(iores));
 err_request_region:
diff --git a/drivers/dma/intel_mid_dma.c b/drivers/dma/intel_mid_dma.c
index c900ca7aaec..222e907bfaa 100644
--- a/drivers/dma/intel_mid_dma.c
+++ b/drivers/dma/intel_mid_dma.c
@@ -394,11 +394,11 @@ static int midc_lli_fill_sg(struct intel_mid_dma_chan *midc,
 			}
 		}
 		/*Populate CTL_HI values*/
-		ctl_hi.ctlx.block_ts = get_block_ts(sg->length,
+		ctl_hi.ctlx.block_ts = get_block_ts(sg_dma_len(sg),
 							desc->width,
 							midc->dma->block_size);
 		/*Populate SAR and DAR values*/
-		sg_phy_addr = sg_phys(sg);
+		sg_phy_addr = sg_dma_address(sg);
 		if (desc->dirn ==  DMA_MEM_TO_DEV) {
 			lli_bloc_desc->sar  = sg_phy_addr;
 			lli_bloc_desc->dar  = mids->dma_slave.dst_addr;
@@ -747,7 +747,7 @@ static struct dma_async_tx_descriptor *intel_mid_dma_prep_slave_sg(
 			txd = intel_mid_dma_prep_memcpy(chan,
 						mids->dma_slave.dst_addr,
 						mids->dma_slave.src_addr,
-						sgl->length,
+						sg_dma_len(sgl),
 						flags);
 			return txd;
 		} else {
@@ -759,7 +759,7 @@ static struct dma_async_tx_descriptor *intel_mid_dma_prep_slave_sg(
 	pr_debug("MDMA: SG Length = %d, direction = %d, Flags = %#lx\n",
 			sg_len, direction, flags);
 
-	txd = intel_mid_dma_prep_memcpy(chan, 0, 0, sgl->length, flags);
+	txd = intel_mid_dma_prep_memcpy(chan, 0, 0, sg_dma_len(sgl), flags);
 	if (NULL == txd) {
 		pr_err("MDMA: Prep memcpy failed\n");
 		return NULL;
diff --git a/drivers/dma/ipu/ipu_idmac.c b/drivers/dma/ipu/ipu_idmac.c
index 62e3f8ec246..5ec72044ea4 100644
--- a/drivers/dma/ipu/ipu_idmac.c
+++ b/drivers/dma/ipu/ipu_idmac.c
@@ -1715,7 +1715,7 @@ static int __init ipu_probe(struct platform_device *pdev)
 	}
 
 	/* Make sure IPU HSP clock is running */
-	clk_enable(ipu_data.ipu_clk);
+	clk_prepare_enable(ipu_data.ipu_clk);
 
 	/* Disable all interrupts */
 	idmac_write_ipureg(&ipu_data, 0, IPU_INT_CTRL_1);
@@ -1747,7 +1747,7 @@ static int __init ipu_probe(struct platform_device *pdev)
 err_idmac_init:
 err_attach_irq:
 	ipu_irq_detach_irq(&ipu_data, pdev);
-	clk_disable(ipu_data.ipu_clk);
+	clk_disable_unprepare(ipu_data.ipu_clk);
 	clk_put(ipu_data.ipu_clk);
 err_clk_get:
 	iounmap(ipu_data.reg_ic);
@@ -1765,7 +1765,7 @@ static int __exit ipu_remove(struct platform_device *pdev)
 
 	ipu_idmac_exit(ipu);
 	ipu_irq_detach_irq(ipu, pdev);
-	clk_disable(ipu->ipu_clk);
+	clk_disable_unprepare(ipu->ipu_clk);
 	clk_put(ipu->ipu_clk);
 	iounmap(ipu->reg_ic);
 	iounmap(ipu->reg_ipu);
diff --git a/drivers/dma/mv_xor.c b/drivers/dma/mv_xor.c
index fa5d55fea46..0b12e68bf79 100644
--- a/drivers/dma/mv_xor.c
+++ b/drivers/dma/mv_xor.c
@@ -25,6 +25,7 @@
 #include <linux/interrupt.h>
 #include <linux/platform_device.h>
 #include <linux/memory.h>
+#include <linux/clk.h>
 #include <plat/mv_xor.h>
 
 #include "dmaengine.h"
@@ -1307,11 +1308,25 @@ static int mv_xor_shared_probe(struct platform_device *pdev)
 	if (dram)
 		mv_xor_conf_mbus_windows(msp, dram);
 
+	/* Not all platforms can gate the clock, so it is not
+	 * an error if the clock does not exists.
+	 */
+	msp->clk = clk_get(&pdev->dev, NULL);
+	if (!IS_ERR(msp->clk))
+		clk_prepare_enable(msp->clk);
+
 	return 0;
 }
 
 static int mv_xor_shared_remove(struct platform_device *pdev)
 {
+	struct mv_xor_shared_private *msp = platform_get_drvdata(pdev);
+
+	if (!IS_ERR(msp->clk)) {
+		clk_disable_unprepare(msp->clk);
+		clk_put(msp->clk);
+	}
+
 	return 0;
 }
 
diff --git a/drivers/dma/mv_xor.h b/drivers/dma/mv_xor.h
index 654876b7ba1..a5b422f5a8a 100644
--- a/drivers/dma/mv_xor.h
+++ b/drivers/dma/mv_xor.h
@@ -55,6 +55,7 @@
 struct mv_xor_shared_private {
 	void __iomem	*xor_base;
 	void __iomem	*xor_high_base;
+	struct clk	*clk;
 };
 
 
diff --git a/drivers/dma/mxs-dma.c b/drivers/dma/mxs-dma.c
index 655d4ce6ed0..c96ab15319f 100644
--- a/drivers/dma/mxs-dma.c
+++ b/drivers/dma/mxs-dma.c
@@ -22,11 +22,14 @@
 #include <linux/platform_device.h>
 #include <linux/dmaengine.h>
 #include <linux/delay.h>
+#include <linux/module.h>
 #include <linux/fsl/mxs-dma.h>
+#include <linux/stmp_device.h>
+#include <linux/of.h>
+#include <linux/of_device.h>
 
 #include <asm/irq.h>
 #include <mach/mxs.h>
-#include <mach/common.h>
 
 #include "dmaengine.h"
 
@@ -36,12 +39,8 @@
  * dma can program the controller registers of peripheral devices.
  */
 
-#define MXS_DMA_APBH		0
-#define MXS_DMA_APBX		1
-#define dma_is_apbh()		(mxs_dma->dev_id == MXS_DMA_APBH)
-
-#define APBH_VERSION_LATEST	3
-#define apbh_is_old()		(mxs_dma->version < APBH_VERSION_LATEST)
+#define dma_is_apbh(mxs_dma)	((mxs_dma)->type == MXS_DMA_APBH)
+#define apbh_is_old(mxs_dma)	((mxs_dma)->dev_id == IMX23_DMA)
 
 #define HW_APBHX_CTRL0				0x000
 #define BM_APBH_CTRL0_APB_BURST8_EN		(1 << 29)
@@ -51,13 +50,14 @@
 #define HW_APBHX_CTRL2				0x020
 #define HW_APBHX_CHANNEL_CTRL			0x030
 #define BP_APBHX_CHANNEL_CTRL_RESET_CHANNEL	16
-#define HW_APBH_VERSION				(cpu_is_mx23() ? 0x3f0 : 0x800)
-#define HW_APBX_VERSION				0x800
-#define BP_APBHX_VERSION_MAJOR			24
-#define HW_APBHX_CHn_NXTCMDAR(n) \
-	(((dma_is_apbh() && apbh_is_old()) ? 0x050 : 0x110) + (n) * 0x70)
-#define HW_APBHX_CHn_SEMA(n) \
-	(((dma_is_apbh() && apbh_is_old()) ? 0x080 : 0x140) + (n) * 0x70)
+/*
+ * The offset of NXTCMDAR register is different per both dma type and version,
+ * while stride for each channel is all the same 0x70.
+ */
+#define HW_APBHX_CHn_NXTCMDAR(d, n) \
+	(((dma_is_apbh(d) && apbh_is_old(d)) ? 0x050 : 0x110) + (n) * 0x70)
+#define HW_APBHX_CHn_SEMA(d, n) \
+	(((dma_is_apbh(d) && apbh_is_old(d)) ? 0x080 : 0x140) + (n) * 0x70)
 
 /*
  * ccw bits definitions
@@ -121,9 +121,19 @@ struct mxs_dma_chan {
 #define MXS_DMA_CHANNELS		16
 #define MXS_DMA_CHANNELS_MASK		0xffff
 
+enum mxs_dma_devtype {
+	MXS_DMA_APBH,
+	MXS_DMA_APBX,
+};
+
+enum mxs_dma_id {
+	IMX23_DMA,
+	IMX28_DMA,
+};
+
 struct mxs_dma_engine {
-	int				dev_id;
-	unsigned int			version;
+	enum mxs_dma_id			dev_id;
+	enum mxs_dma_devtype		type;
 	void __iomem			*base;
 	struct clk			*clk;
 	struct dma_device		dma_device;
@@ -131,17 +141,86 @@ struct mxs_dma_engine {
 	struct mxs_dma_chan		mxs_chans[MXS_DMA_CHANNELS];
 };
 
+struct mxs_dma_type {
+	enum mxs_dma_id id;
+	enum mxs_dma_devtype type;
+};
+
+static struct mxs_dma_type mxs_dma_types[] = {
+	{
+		.id = IMX23_DMA,
+		.type = MXS_DMA_APBH,
+	}, {
+		.id = IMX23_DMA,
+		.type = MXS_DMA_APBX,
+	}, {
+		.id = IMX28_DMA,
+		.type = MXS_DMA_APBH,
+	}, {
+		.id = IMX28_DMA,
+		.type = MXS_DMA_APBX,
+	}
+};
+
+static struct platform_device_id mxs_dma_ids[] = {
+	{
+		.name = "imx23-dma-apbh",
+		.driver_data = (kernel_ulong_t) &mxs_dma_types[0],
+	}, {
+		.name = "imx23-dma-apbx",
+		.driver_data = (kernel_ulong_t) &mxs_dma_types[1],
+	}, {
+		.name = "imx28-dma-apbh",
+		.driver_data = (kernel_ulong_t) &mxs_dma_types[2],
+	}, {
+		.name = "imx28-dma-apbx",
+		.driver_data = (kernel_ulong_t) &mxs_dma_types[3],
+	}, {
+		/* end of list */
+	}
+};
+
+static const struct of_device_id mxs_dma_dt_ids[] = {
+	{ .compatible = "fsl,imx23-dma-apbh", .data = &mxs_dma_ids[0], },
+	{ .compatible = "fsl,imx23-dma-apbx", .data = &mxs_dma_ids[1], },
+	{ .compatible = "fsl,imx28-dma-apbh", .data = &mxs_dma_ids[2], },
+	{ .compatible = "fsl,imx28-dma-apbx", .data = &mxs_dma_ids[3], },
+	{ /* sentinel */ }
+};
+MODULE_DEVICE_TABLE(of, mxs_dma_dt_ids);
+
+static struct mxs_dma_chan *to_mxs_dma_chan(struct dma_chan *chan)
+{
+	return container_of(chan, struct mxs_dma_chan, chan);
+}
+
+int mxs_dma_is_apbh(struct dma_chan *chan)
+{
+	struct mxs_dma_chan *mxs_chan = to_mxs_dma_chan(chan);
+	struct mxs_dma_engine *mxs_dma = mxs_chan->mxs_dma;
+
+	return dma_is_apbh(mxs_dma);
+}
+
+int mxs_dma_is_apbx(struct dma_chan *chan)
+{
+	struct mxs_dma_chan *mxs_chan = to_mxs_dma_chan(chan);
+	struct mxs_dma_engine *mxs_dma = mxs_chan->mxs_dma;
+
+	return !dma_is_apbh(mxs_dma);
+}
+
 static void mxs_dma_reset_chan(struct mxs_dma_chan *mxs_chan)
 {
 	struct mxs_dma_engine *mxs_dma = mxs_chan->mxs_dma;
 	int chan_id = mxs_chan->chan.chan_id;
 
-	if (dma_is_apbh() && apbh_is_old())
+	if (dma_is_apbh(mxs_dma) && apbh_is_old(mxs_dma))
 		writel(1 << (chan_id + BP_APBH_CTRL0_RESET_CHANNEL),
-			mxs_dma->base + HW_APBHX_CTRL0 + MXS_SET_ADDR);
+			mxs_dma->base + HW_APBHX_CTRL0 + STMP_OFFSET_REG_SET);
 	else
 		writel(1 << (chan_id + BP_APBHX_CHANNEL_CTRL_RESET_CHANNEL),
-			mxs_dma->base + HW_APBHX_CHANNEL_CTRL + MXS_SET_ADDR);
+			mxs_dma->base + HW_APBHX_CHANNEL_CTRL + STMP_OFFSET_REG_SET);
 }
 
 static void mxs_dma_enable_chan(struct mxs_dma_chan *mxs_chan)
@@ -151,10 +230,10 @@ static void mxs_dma_enable_chan(struct mxs_dma_chan *mxs_chan)
 
 	/* set cmd_addr up */
 	writel(mxs_chan->ccw_phys,
-		mxs_dma->base + HW_APBHX_CHn_NXTCMDAR(chan_id));
+		mxs_dma->base + HW_APBHX_CHn_NXTCMDAR(mxs_dma, chan_id));
 
 	/* write 1 to SEMA to kick off the channel */
-	writel(1, mxs_dma->base + HW_APBHX_CHn_SEMA(chan_id));
+	writel(1, mxs_dma->base + HW_APBHX_CHn_SEMA(mxs_dma, chan_id));
 }
 
 static void mxs_dma_disable_chan(struct mxs_dma_chan *mxs_chan)
@@ -168,12 +247,12 @@ static void mxs_dma_pause_chan(struct mxs_dma_chan *mxs_chan)
 	int chan_id = mxs_chan->chan.chan_id;
 
 	/* freeze the channel */
-	if (dma_is_apbh() && apbh_is_old())
+	if (dma_is_apbh(mxs_dma) && apbh_is_old(mxs_dma))
 		writel(1 << chan_id,
-			mxs_dma->base + HW_APBHX_CTRL0 + MXS_SET_ADDR);
+			mxs_dma->base + HW_APBHX_CTRL0 + STMP_OFFSET_REG_SET);
 	else
 		writel(1 << chan_id,
-			mxs_dma->base + HW_APBHX_CHANNEL_CTRL + MXS_SET_ADDR);
+			mxs_dma->base + HW_APBHX_CHANNEL_CTRL + STMP_OFFSET_REG_SET);
 
 	mxs_chan->status = DMA_PAUSED;
 }
@@ -184,21 +263,16 @@ static void mxs_dma_resume_chan(struct mxs_dma_chan *mxs_chan)
 	int chan_id = mxs_chan->chan.chan_id;
 
 	/* unfreeze the channel */
-	if (dma_is_apbh() && apbh_is_old())
+	if (dma_is_apbh(mxs_dma) && apbh_is_old(mxs_dma))
 		writel(1 << chan_id,
-			mxs_dma->base + HW_APBHX_CTRL0 + MXS_CLR_ADDR);
+			mxs_dma->base + HW_APBHX_CTRL0 + STMP_OFFSET_REG_CLR);
 	else
 		writel(1 << chan_id,
-			mxs_dma->base + HW_APBHX_CHANNEL_CTRL + MXS_CLR_ADDR);
+			mxs_dma->base + HW_APBHX_CHANNEL_CTRL + STMP_OFFSET_REG_CLR);
 
 	mxs_chan->status = DMA_IN_PROGRESS;
 }
 
-static struct mxs_dma_chan *to_mxs_dma_chan(struct dma_chan *chan)
-{
-	return container_of(chan, struct mxs_dma_chan, chan);
-}
-
 static dma_cookie_t mxs_dma_tx_submit(struct dma_async_tx_descriptor *tx)
 {
 	return dma_cookie_assign(tx);
@@ -220,11 +294,11 @@ static irqreturn_t mxs_dma_int_handler(int irq, void *dev_id)
 	/* completion status */
 	stat1 = readl(mxs_dma->base + HW_APBHX_CTRL1);
 	stat1 &= MXS_DMA_CHANNELS_MASK;
-	writel(stat1, mxs_dma->base + HW_APBHX_CTRL1 + MXS_CLR_ADDR);
+	writel(stat1, mxs_dma->base + HW_APBHX_CTRL1 + STMP_OFFSET_REG_CLR);
 
 	/* error status */
 	stat2 = readl(mxs_dma->base + HW_APBHX_CTRL2);
-	writel(stat2, mxs_dma->base + HW_APBHX_CTRL2 + MXS_CLR_ADDR);
+	writel(stat2, mxs_dma->base + HW_APBHX_CTRL2 + STMP_OFFSET_REG_CLR);
 
 	/*
 	 * When both completion and error of termination bits set at the
@@ -415,9 +489,9 @@ static struct dma_async_tx_descriptor *mxs_dma_prep_slave_sg(
 		ccw->bits |= BF_CCW(MXS_DMA_CMD_NO_XFER, COMMAND);
 	} else {
 		for_each_sg(sgl, sg, sg_len, i) {
-			if (sg->length > MAX_XFER_BYTES) {
+			if (sg_dma_len(sg) > MAX_XFER_BYTES) {
 				dev_err(mxs_dma->dma_device.dev, "maximum bytes for sg entry exceeded: %d > %d\n",
-						sg->length, MAX_XFER_BYTES);
+						sg_dma_len(sg), MAX_XFER_BYTES);
 				goto err_out;
 			}
 
@@ -425,7 +499,7 @@ static struct dma_async_tx_descriptor *mxs_dma_prep_slave_sg(
 
 			ccw->next = mxs_chan->ccw_phys + sizeof(*ccw) * idx;
 			ccw->bufaddr = sg->dma_address;
-			ccw->xfer_bytes = sg->length;
+			ccw->xfer_bytes = sg_dma_len(sg);
 
 			ccw->bits = 0;
 			ccw->bits |= CCW_CHAIN;
@@ -567,27 +641,21 @@ static int __init mxs_dma_init(struct mxs_dma_engine *mxs_dma)
 	if (ret)
 		return ret;
 
-	ret = mxs_reset_block(mxs_dma->base);
+	ret = stmp_reset_block(mxs_dma->base);
 	if (ret)
 		goto err_out;
 
-	/* only major version matters */
-	mxs_dma->version = readl(mxs_dma->base +
-				((mxs_dma->dev_id == MXS_DMA_APBX) ?
-				HW_APBX_VERSION : HW_APBH_VERSION)) >>
-				BP_APBHX_VERSION_MAJOR;
-
 	/* enable apbh burst */
-	if (dma_is_apbh()) {
+	if (dma_is_apbh(mxs_dma)) {
 		writel(BM_APBH_CTRL0_APB_BURST_EN,
-			mxs_dma->base + HW_APBHX_CTRL0 + MXS_SET_ADDR);
+			mxs_dma->base + HW_APBHX_CTRL0 + STMP_OFFSET_REG_SET);
 		writel(BM_APBH_CTRL0_APB_BURST8_EN,
-			mxs_dma->base + HW_APBHX_CTRL0 + MXS_SET_ADDR);
+			mxs_dma->base + HW_APBHX_CTRL0 + STMP_OFFSET_REG_SET);
 	}
 
 	/* enable irq for all the channels */
 	writel(MXS_DMA_CHANNELS_MASK << MXS_DMA_CHANNELS,
-		mxs_dma->base + HW_APBHX_CTRL1 + MXS_SET_ADDR);
+		mxs_dma->base + HW_APBHX_CTRL1 + STMP_OFFSET_REG_SET);
 
 err_out:
 	clk_disable_unprepare(mxs_dma->clk);
@@ -596,8 +664,9 @@ err_out:
 
 static int __init mxs_dma_probe(struct platform_device *pdev)
 {
-	const struct platform_device_id *id_entry =
-				platform_get_device_id(pdev);
+	const struct platform_device_id *id_entry;
+	const struct of_device_id *of_id;
+	const struct mxs_dma_type *dma_type;
 	struct mxs_dma_engine *mxs_dma;
 	struct resource *iores;
 	int ret, i;
@@ -606,7 +675,15 @@ static int __init mxs_dma_probe(struct platform_device *pdev)
 	if (!mxs_dma)
 		return -ENOMEM;
 
-	mxs_dma->dev_id = id_entry->driver_data;
+	of_id = of_match_device(mxs_dma_dt_ids, &pdev->dev);
+	if (of_id)
+		id_entry = of_id->data;
+	else
+		id_entry = platform_get_device_id(pdev);
+
+	dma_type = (struct mxs_dma_type *)id_entry->driver_data;
+	mxs_dma->type = dma_type->type;
+	mxs_dma->dev_id = dma_type->id;
 
 	iores = platform_get_resource(pdev, IORESOURCE_MEM, 0);
 
@@ -689,23 +766,12 @@ err_request_region:
 	return ret;
 }
 
-static struct platform_device_id mxs_dma_type[] = {
-	{
-		.name = "mxs-dma-apbh",
-		.driver_data = MXS_DMA_APBH,
-	}, {
-		.name = "mxs-dma-apbx",
-		.driver_data = MXS_DMA_APBX,
-	}, {
-		/* end of list */
-	}
-};
-
 static struct platform_driver mxs_dma_driver = {
 	.driver		= {
 		.name	= "mxs-dma",
+		.of_match_table = mxs_dma_dt_ids,
 	},
-	.id_table	= mxs_dma_type,
+	.id_table	= mxs_dma_ids,
 };
 
 static int __init mxs_dma_module_init(void)
diff --git a/drivers/dma/pch_dma.c b/drivers/dma/pch_dma.c
index 65c0495a6d4..987ab5cd261 100644
--- a/drivers/dma/pch_dma.c
+++ b/drivers/dma/pch_dma.c
@@ -621,7 +621,7 @@ static struct dma_async_tx_descriptor *pd_prep_slave_sg(struct dma_chan *chan,
 			goto err_desc_get;
 
 		desc->regs.dev_addr = reg;
-		desc->regs.mem_addr = sg_phys(sg);
+		desc->regs.mem_addr = sg_dma_address(sg);
 		desc->regs.size = sg_dma_len(sg);
 		desc->regs.next = DMA_DESC_FOLLOW_WITHOUT_IRQ;
 
diff --git a/drivers/dma/pl330.c b/drivers/dma/pl330.c
index 2ee6e23930a..cbcc28e79be 100644
--- a/drivers/dma/pl330.c
+++ b/drivers/dma/pl330.c
@@ -21,7 +21,6 @@
 #include <linux/interrupt.h>
 #include <linux/dma-mapping.h>
 #include <linux/dmaengine.h>
-#include <linux/interrupt.h>
 #include <linux/amba/bus.h>
 #include <linux/amba/pl330.h>
 #include <linux/pm_runtime.h>
@@ -2322,7 +2321,8 @@ static void pl330_tasklet(unsigned long data)
 	/* Pick up ripe tomatoes */
 	list_for_each_entry_safe(desc, _dt, &pch->work_list, node)
 		if (desc->status == DONE) {
-			dma_cookie_complete(&desc->txd);
+			if (pch->cyclic)
+				dma_cookie_complete(&desc->txd);
 			list_move_tail(&desc->node, &list);
 		}
 
diff --git a/drivers/dma/ste_dma40.c b/drivers/dma/ste_dma40.c
index 2ed1ac3513f..000d309602b 100644
--- a/drivers/dma/ste_dma40.c
+++ b/drivers/dma/ste_dma40.c
@@ -2362,7 +2362,7 @@ dma40_prep_dma_cyclic(struct dma_chan *chan, dma_addr_t dma_addr,
 	}
 
 	sg[periods].offset = 0;
-	sg[periods].length = 0;
+	sg_dma_len(&sg[periods]) = 0;
 	sg[periods].page_link =
 		((unsigned long)sg | 0x01) & ~0x02;
 
diff --git a/drivers/edac/amd64_edac.c b/drivers/edac/amd64_edac.c
index 7ef73c919c5..7be9b7288e9 100644
--- a/drivers/edac/amd64_edac.c
+++ b/drivers/edac/amd64_edac.c
@@ -715,25 +715,6 @@ static inline u64 input_addr_to_sys_addr(struct mem_ctl_info *mci,
 				     input_addr_to_dram_addr(mci, input_addr));
 }
 
-/*
- * Find the minimum and maximum InputAddr values that map to the given @csrow.
- * Pass back these values in *input_addr_min and *input_addr_max.
- */
-static void find_csrow_limits(struct mem_ctl_info *mci, int csrow,
-			      u64 *input_addr_min, u64 *input_addr_max)
-{
-	struct amd64_pvt *pvt;
-	u64 base, mask;
-
-	pvt = mci->pvt_info;
-	BUG_ON((csrow < 0) || (csrow >= pvt->csels[0].b_cnt));
-
-	get_cs_base_and_mask(pvt, csrow, 0, &base, &mask);
-
-	*input_addr_min = base & ~mask;
-	*input_addr_max = base | mask;
-}
-
 /* Map the Error address to a PAGE and PAGE OFFSET. */
 static inline void error_address_to_page_and_offset(u64 error_address,
 						    u32 *page, u32 *offset)
@@ -1058,6 +1039,37 @@ static void k8_map_sysaddr_to_csrow(struct mem_ctl_info *mci, u64 sys_addr,
 	int channel, csrow;
 	u32 page, offset;
 
+	error_address_to_page_and_offset(sys_addr, &page, &offset);
+
+	/*
+	 * Find out which node the error address belongs to. This may be
+	 * different from the node that detected the error.
+	 */
+	src_mci = find_mc_by_sys_addr(mci, sys_addr);
+	if (!src_mci) {
+		amd64_mc_err(mci, "failed to map error addr 0x%lx to a node\n",
+			     (unsigned long)sys_addr);
+		edac_mc_handle_error(HW_EVENT_ERR_CORRECTED, mci,
+				     page, offset, syndrome,
+				     -1, -1, -1,
+				     EDAC_MOD_STR,
+				     "failed to map error addr to a node",
+				     NULL);
+		return;
+	}
+
+	/* Now map the sys_addr to a CSROW */
+	csrow = sys_addr_to_csrow(src_mci, sys_addr);
+	if (csrow < 0) {
+		edac_mc_handle_error(HW_EVENT_ERR_CORRECTED, mci,
+				     page, offset, syndrome,
+				     -1, -1, -1,
+				     EDAC_MOD_STR,
+				     "failed to map error addr to a csrow",
+				     NULL);
+		return;
+	}
+
 	/* CHIPKILL enabled */
 	if (pvt->nbcfg & NBCFG_CHIPKILL) {
 		channel = get_channel_from_ecc_syndrome(mci, syndrome);
@@ -1067,9 +1079,15 @@ static void k8_map_sysaddr_to_csrow(struct mem_ctl_info *mci, u64 sys_addr,
 			 * 2 DIMMs is in error. So we need to ID 'both' of them
 			 * as suspect.
 			 */
-			amd64_mc_warn(mci, "unknown syndrome 0x%04x - possible "
-					   "error reporting race\n", syndrome);
-			edac_mc_handle_ce_no_info(mci, EDAC_MOD_STR);
+			amd64_mc_warn(src_mci, "unknown syndrome 0x%04x - "
+				      "possible error reporting race\n",
+				      syndrome);
+			edac_mc_handle_error(HW_EVENT_ERR_CORRECTED, mci,
+					     page, offset, syndrome,
+					     csrow, -1, -1,
+					     EDAC_MOD_STR,
+					     "unknown syndrome - possible error reporting race",
+					     NULL);
 			return;
 		}
 	} else {
@@ -1084,28 +1102,10 @@ static void k8_map_sysaddr_to_csrow(struct mem_ctl_info *mci, u64 sys_addr,
 		channel = ((sys_addr & BIT(3)) != 0);
 	}
 
-	/*
-	 * Find out which node the error address belongs to. This may be
-	 * different from the node that detected the error.
-	 */
-	src_mci = find_mc_by_sys_addr(mci, sys_addr);
-	if (!src_mci) {
-		amd64_mc_err(mci, "failed to map error addr 0x%lx to a node\n",
-			     (unsigned long)sys_addr);
-		edac_mc_handle_ce_no_info(mci, EDAC_MOD_STR);
-		return;
-	}
-
-	/* Now map the sys_addr to a CSROW */
-	csrow = sys_addr_to_csrow(src_mci, sys_addr);
-	if (csrow < 0) {
-		edac_mc_handle_ce_no_info(src_mci, EDAC_MOD_STR);
-	} else {
-		error_address_to_page_and_offset(sys_addr, &page, &offset);
-
-		edac_mc_handle_ce(src_mci, page, offset, syndrome, csrow,
-				  channel, EDAC_MOD_STR);
-	}
+	edac_mc_handle_error(HW_EVENT_ERR_CORRECTED, src_mci,
+			     page, offset, syndrome,
+			     csrow, channel, -1,
+			     EDAC_MOD_STR, "", NULL);
 }
 
 static int ddr2_cs_size(unsigned i, bool dct_width)
@@ -1611,15 +1611,20 @@ static void f1x_map_sysaddr_to_csrow(struct mem_ctl_info *mci, u64 sys_addr,
 	u32 page, offset;
 	int nid, csrow, chan = 0;
 
+	error_address_to_page_and_offset(sys_addr, &page, &offset);
+
 	csrow = f1x_translate_sysaddr_to_cs(pvt, sys_addr, &nid, &chan);
 
 	if (csrow < 0) {
-		edac_mc_handle_ce_no_info(mci, EDAC_MOD_STR);
+		edac_mc_handle_error(HW_EVENT_ERR_CORRECTED, mci,
+				     page, offset, syndrome,
+				     -1, -1, -1,
+				     EDAC_MOD_STR,
+				     "failed to map error addr to a csrow",
+				     NULL);
 		return;
 	}
 
-	error_address_to_page_and_offset(sys_addr, &page, &offset);
-
 	/*
 	 * We need the syndromes for channel detection only when we're
 	 * ganged. Otherwise @chan should already contain the channel at
@@ -1628,16 +1633,10 @@ static void f1x_map_sysaddr_to_csrow(struct mem_ctl_info *mci, u64 sys_addr,
 	if (dct_ganging_enabled(pvt))
 		chan = get_channel_from_ecc_syndrome(mci, syndrome);
 
-	if (chan >= 0)
-		edac_mc_handle_ce(mci, page, offset, syndrome, csrow, chan,
-				  EDAC_MOD_STR);
-	else
-		/*
-		 * Channel unknown, report all channels on this CSROW as failed.
-		 */
-		for (chan = 0; chan < mci->csrows[csrow].nr_channels; chan++)
-			edac_mc_handle_ce(mci, page, offset, syndrome,
-					  csrow, chan, EDAC_MOD_STR);
+	edac_mc_handle_error(HW_EVENT_ERR_CORRECTED, mci,
+			     page, offset, syndrome,
+			     csrow, chan, -1,
+			     EDAC_MOD_STR, "", NULL);
 }
 
 /*
@@ -1918,7 +1917,12 @@ static void amd64_handle_ce(struct mem_ctl_info *mci, struct mce *m)
 	/* Ensure that the Error Address is VALID */
 	if (!(m->status & MCI_STATUS_ADDRV)) {
 		amd64_mc_err(mci, "HW has no ERROR_ADDRESS available\n");
-		edac_mc_handle_ce_no_info(mci, EDAC_MOD_STR);
+		edac_mc_handle_error(HW_EVENT_ERR_CORRECTED, mci,
+				     0, 0, 0,
+				     -1, -1, -1,
+				     EDAC_MOD_STR,
+				     "HW has no ERROR_ADDRESS available",
+				     NULL);
 		return;
 	}
 
@@ -1942,11 +1946,17 @@ static void amd64_handle_ue(struct mem_ctl_info *mci, struct mce *m)
 
 	if (!(m->status & MCI_STATUS_ADDRV)) {
 		amd64_mc_err(mci, "HW has no ERROR_ADDRESS available\n");
-		edac_mc_handle_ue_no_info(log_mci, EDAC_MOD_STR);
+		edac_mc_handle_error(HW_EVENT_ERR_UNCORRECTED, mci,
+				     0, 0, 0,
+				     -1, -1, -1,
+				     EDAC_MOD_STR,
+				     "HW has no ERROR_ADDRESS available",
+				     NULL);
 		return;
 	}
 
 	sys_addr = get_error_address(m);
+	error_address_to_page_and_offset(sys_addr, &page, &offset);
 
 	/*
 	 * Find out which node the error address belongs to. This may be
@@ -1956,7 +1966,11 @@ static void amd64_handle_ue(struct mem_ctl_info *mci, struct mce *m)
 	if (!src_mci) {
 		amd64_mc_err(mci, "ERROR ADDRESS (0x%lx) NOT mapped to a MC\n",
 				  (unsigned long)sys_addr);
-		edac_mc_handle_ue_no_info(log_mci, EDAC_MOD_STR);
+		edac_mc_handle_error(HW_EVENT_ERR_UNCORRECTED, mci,
+				     page, offset, 0,
+				     -1, -1, -1,
+				     EDAC_MOD_STR,
+				     "ERROR ADDRESS NOT mapped to a MC", NULL);
 		return;
 	}
 
@@ -1966,10 +1980,17 @@ static void amd64_handle_ue(struct mem_ctl_info *mci, struct mce *m)
 	if (csrow < 0) {
 		amd64_mc_err(mci, "ERROR_ADDRESS (0x%lx) NOT mapped to CS\n",
 				  (unsigned long)sys_addr);
-		edac_mc_handle_ue_no_info(log_mci, EDAC_MOD_STR);
+		edac_mc_handle_error(HW_EVENT_ERR_UNCORRECTED, mci,
+				     page, offset, 0,
+				     -1, -1, -1,
+				     EDAC_MOD_STR,
+				     "ERROR ADDRESS NOT mapped to CS",
+				     NULL);
 	} else {
-		error_address_to_page_and_offset(sys_addr, &page, &offset);
-		edac_mc_handle_ue(log_mci, page, offset, csrow, EDAC_MOD_STR);
+		edac_mc_handle_error(HW_EVENT_ERR_UNCORRECTED, mci,
+				     page, offset, 0,
+				     csrow, -1, -1,
+				     EDAC_MOD_STR, "", NULL);
 	}
 }
 
@@ -2171,7 +2192,7 @@ static u32 amd64_csrow_nr_pages(struct amd64_pvt *pvt, u8 dct, int csrow_nr)
 	nr_pages = pvt->ops->dbam_to_cs(pvt, dct, cs_mode) << (20 - PAGE_SHIFT);
 
 	debugf0("  (csrow=%d) DBAM map index= %d\n", csrow_nr, cs_mode);
-	debugf0("    nr_pages= %u  channel-count = %d\n",
+	debugf0("    nr_pages/channel= %u  channel-count = %d\n",
 		nr_pages, pvt->channel_count);
 
 	return nr_pages;
@@ -2185,9 +2206,12 @@ static int init_csrows(struct mem_ctl_info *mci)
 {
 	struct csrow_info *csrow;
 	struct amd64_pvt *pvt = mci->pvt_info;
-	u64 input_addr_min, input_addr_max, sys_addr, base, mask;
+	u64 base, mask;
 	u32 val;
-	int i, empty = 1;
+	int i, j, empty = 1;
+	enum mem_type mtype;
+	enum edac_type edac_mode;
+	int nr_pages = 0;
 
 	amd64_read_pci_cfg(pvt->F3, NBCFG, &val);
 
@@ -2211,41 +2235,32 @@ static int init_csrows(struct mem_ctl_info *mci)
 
 		empty = 0;
 		if (csrow_enabled(i, 0, pvt))
-			csrow->nr_pages = amd64_csrow_nr_pages(pvt, 0, i);
+			nr_pages = amd64_csrow_nr_pages(pvt, 0, i);
 		if (csrow_enabled(i, 1, pvt))
-			csrow->nr_pages += amd64_csrow_nr_pages(pvt, 1, i);
-		find_csrow_limits(mci, i, &input_addr_min, &input_addr_max);
-		sys_addr = input_addr_to_sys_addr(mci, input_addr_min);
-		csrow->first_page = (u32) (sys_addr >> PAGE_SHIFT);
-		sys_addr = input_addr_to_sys_addr(mci, input_addr_max);
-		csrow->last_page = (u32) (sys_addr >> PAGE_SHIFT);
+			nr_pages += amd64_csrow_nr_pages(pvt, 1, i);
 
 		get_cs_base_and_mask(pvt, i, 0, &base, &mask);
-		csrow->page_mask = ~mask;
 		/* 8 bytes of resolution */
 
-		csrow->mtype = amd64_determine_memory_type(pvt, i);
+		mtype = amd64_determine_memory_type(pvt, i);
 
 		debugf1("  for MC node %d csrow %d:\n", pvt->mc_node_id, i);
-		debugf1("    input_addr_min: 0x%lx input_addr_max: 0x%lx\n",
-			(unsigned long)input_addr_min,
-			(unsigned long)input_addr_max);
-		debugf1("    sys_addr: 0x%lx  page_mask: 0x%lx\n",
-			(unsigned long)sys_addr, csrow->page_mask);
-		debugf1("    nr_pages: %u  first_page: 0x%lx "
-			"last_page: 0x%lx\n",
-			(unsigned)csrow->nr_pages,
-			csrow->first_page, csrow->last_page);
+		debugf1("    nr_pages: %u\n", nr_pages * pvt->channel_count);
 
 		/*
 		 * determine whether CHIPKILL or JUST ECC or NO ECC is operating
 		 */
 		if (pvt->nbcfg & NBCFG_ECC_ENABLE)
-			csrow->edac_mode =
-			    (pvt->nbcfg & NBCFG_CHIPKILL) ?
-			    EDAC_S4ECD4ED : EDAC_SECDED;
+			edac_mode = (pvt->nbcfg & NBCFG_CHIPKILL) ?
+				    EDAC_S4ECD4ED : EDAC_SECDED;
 		else
-			csrow->edac_mode = EDAC_NONE;
+			edac_mode = EDAC_NONE;
+
+		for (j = 0; j < pvt->channel_count; j++) {
+			csrow->channels[j].dimm->mtype = mtype;
+			csrow->channels[j].dimm->edac_mode = edac_mode;
+			csrow->channels[j].dimm->nr_pages = nr_pages;
+		}
 	}
 
 	return empty;
@@ -2540,6 +2555,7 @@ static int amd64_init_one_instance(struct pci_dev *F2)
 	struct amd64_pvt *pvt = NULL;
 	struct amd64_family_type *fam_type = NULL;
 	struct mem_ctl_info *mci = NULL;
+	struct edac_mc_layer layers[2];
 	int err = 0, ret;
 	u8 nid = get_node_id(F2);
 
@@ -2574,7 +2590,13 @@ static int amd64_init_one_instance(struct pci_dev *F2)
 		goto err_siblings;
 
 	ret = -ENOMEM;
-	mci = edac_mc_alloc(0, pvt->csels[0].b_cnt, pvt->channel_count, nid);
+	layers[0].type = EDAC_MC_LAYER_CHIP_SELECT;
+	layers[0].size = pvt->csels[0].b_cnt;
+	layers[0].is_virt_csrow = true;
+	layers[1].type = EDAC_MC_LAYER_CHANNEL;
+	layers[1].size = pvt->channel_count;
+	layers[1].is_virt_csrow = false;
+	mci = edac_mc_alloc(nid, ARRAY_SIZE(layers), layers, 0);
 	if (!mci)
 		goto err_siblings;
 
diff --git a/drivers/edac/amd76x_edac.c b/drivers/edac/amd76x_edac.c
index f8fd3c807bd..9774d443fa5 100644
--- a/drivers/edac/amd76x_edac.c
+++ b/drivers/edac/amd76x_edac.c
@@ -29,7 +29,6 @@
 	edac_mc_chipset_printk(mci, level, "amd76x", fmt, ##arg)
 
 #define AMD76X_NR_CSROWS 8
-#define AMD76X_NR_CHANS  1
 #define AMD76X_NR_DIMMS  4
 
 /* AMD 76x register addresses - device 0 function 0 - PCI bridge */
@@ -146,8 +145,10 @@ static int amd76x_process_error_info(struct mem_ctl_info *mci,
 
 		if (handle_errors) {
 			row = (info->ecc_mode_status >> 4) & 0xf;
-			edac_mc_handle_ue(mci, mci->csrows[row].first_page, 0,
-					row, mci->ctl_name);
+			edac_mc_handle_error(HW_EVENT_ERR_UNCORRECTED, mci,
+					     mci->csrows[row].first_page, 0, 0,
+					     row, 0, -1,
+					     mci->ctl_name, "", NULL);
 		}
 	}
 
@@ -159,8 +160,10 @@ static int amd76x_process_error_info(struct mem_ctl_info *mci,
 
 		if (handle_errors) {
 			row = info->ecc_mode_status & 0xf;
-			edac_mc_handle_ce(mci, mci->csrows[row].first_page, 0,
-					0, row, 0, mci->ctl_name);
+			edac_mc_handle_error(HW_EVENT_ERR_CORRECTED, mci,
+					     mci->csrows[row].first_page, 0, 0,
+					     row, 0, -1,
+					     mci->ctl_name, "", NULL);
 		}
 	}
 
@@ -186,11 +189,13 @@ static void amd76x_init_csrows(struct mem_ctl_info *mci, struct pci_dev *pdev,
 			enum edac_type edac_mode)
 {
 	struct csrow_info *csrow;
+	struct dimm_info *dimm;
 	u32 mba, mba_base, mba_mask, dms;
 	int index;
 
 	for (index = 0; index < mci->nr_csrows; index++) {
 		csrow = &mci->csrows[index];
+		dimm = csrow->channels[0].dimm;
 
 		/* find the DRAM Chip Select Base address and mask */
 		pci_read_config_dword(pdev,
@@ -203,13 +208,13 @@ static void amd76x_init_csrows(struct mem_ctl_info *mci, struct pci_dev *pdev,
 		mba_mask = ((mba & 0xff80) << 16) | 0x7fffffUL;
 		pci_read_config_dword(pdev, AMD76X_DRAM_MODE_STATUS, &dms);
 		csrow->first_page = mba_base >> PAGE_SHIFT;
-		csrow->nr_pages = (mba_mask + 1) >> PAGE_SHIFT;
-		csrow->last_page = csrow->first_page + csrow->nr_pages - 1;
+		dimm->nr_pages = (mba_mask + 1) >> PAGE_SHIFT;
+		csrow->last_page = csrow->first_page + dimm->nr_pages - 1;
 		csrow->page_mask = mba_mask >> PAGE_SHIFT;
-		csrow->grain = csrow->nr_pages << PAGE_SHIFT;
-		csrow->mtype = MEM_RDDR;
-		csrow->dtype = ((dms >> index) & 0x1) ? DEV_X4 : DEV_UNKNOWN;
-		csrow->edac_mode = edac_mode;
+		dimm->grain = dimm->nr_pages << PAGE_SHIFT;
+		dimm->mtype = MEM_RDDR;
+		dimm->dtype = ((dms >> index) & 0x1) ? DEV_X4 : DEV_UNKNOWN;
+		dimm->edac_mode = edac_mode;
 	}
 }
 
@@ -230,7 +235,8 @@ static int amd76x_probe1(struct pci_dev *pdev, int dev_idx)
 		EDAC_SECDED,
 		EDAC_SECDED
 	};
-	struct mem_ctl_info *mci = NULL;
+	struct mem_ctl_info *mci;
+	struct edac_mc_layer layers[2];
 	u32 ems;
 	u32 ems_mode;
 	struct amd76x_error_info discard;
@@ -238,11 +244,17 @@ static int amd76x_probe1(struct pci_dev *pdev, int dev_idx)
 	debugf0("%s()\n", __func__);
 	pci_read_config_dword(pdev, AMD76X_ECC_MODE_STATUS, &ems);
 	ems_mode = (ems >> 10) & 0x3;
-	mci = edac_mc_alloc(0, AMD76X_NR_CSROWS, AMD76X_NR_CHANS, 0);
 
-	if (mci == NULL) {
+	layers[0].type = EDAC_MC_LAYER_CHIP_SELECT;
+	layers[0].size = AMD76X_NR_CSROWS;
+	layers[0].is_virt_csrow = true;
+	layers[1].type = EDAC_MC_LAYER_CHANNEL;
+	layers[1].size = 1;
+	layers[1].is_virt_csrow = false;
+	mci = edac_mc_alloc(0, ARRAY_SIZE(layers), layers, 0);
+
+	if (mci == NULL)
 		return -ENOMEM;
-	}
 
 	debugf0("%s(): mci = %p\n", __func__, mci);
 	mci->dev = &pdev->dev;
diff --git a/drivers/edac/cell_edac.c b/drivers/edac/cell_edac.c
index 9a6a274e692..69ee6aab5c7 100644
--- a/drivers/edac/cell_edac.c
+++ b/drivers/edac/cell_edac.c
@@ -48,8 +48,9 @@ static void cell_edac_count_ce(struct mem_ctl_info *mci, int chan, u64 ar)
 	syndrome = (ar & 0x000000001fe00000ul) >> 21;
 
 	/* TODO: Decoding of the error address */
-	edac_mc_handle_ce(mci, csrow->first_page + pfn, offset,
-			  syndrome, 0, chan, "");
+	edac_mc_handle_error(HW_EVENT_ERR_CORRECTED, mci,
+			     csrow->first_page + pfn, offset, syndrome,
+			     0, chan, -1, "", "", NULL);
 }
 
 static void cell_edac_count_ue(struct mem_ctl_info *mci, int chan, u64 ar)
@@ -69,7 +70,9 @@ static void cell_edac_count_ue(struct mem_ctl_info *mci, int chan, u64 ar)
 	offset = address & ~PAGE_MASK;
 
 	/* TODO: Decoding of the error address */
-	edac_mc_handle_ue(mci, csrow->first_page + pfn, offset, 0, "");
+	edac_mc_handle_error(HW_EVENT_ERR_UNCORRECTED, mci,
+			     csrow->first_page + pfn, offset, 0,
+			     0, chan, -1, "", "", NULL);
 }
 
 static void cell_edac_check(struct mem_ctl_info *mci)
@@ -124,8 +127,11 @@ static void cell_edac_check(struct mem_ctl_info *mci)
 static void __devinit cell_edac_init_csrows(struct mem_ctl_info *mci)
 {
 	struct csrow_info		*csrow = &mci->csrows[0];
+	struct dimm_info		*dimm;
 	struct cell_edac_priv		*priv = mci->pvt_info;
 	struct device_node		*np;
+	int				j;
+	u32				nr_pages;
 
 	for (np = NULL;
 	     (np = of_find_node_by_name(np, "memory")) != NULL;) {
@@ -140,15 +146,20 @@ static void __devinit cell_edac_init_csrows(struct mem_ctl_info *mci)
 		if (of_node_to_nid(np) != priv->node)
 			continue;
 		csrow->first_page = r.start >> PAGE_SHIFT;
-		csrow->nr_pages = resource_size(&r) >> PAGE_SHIFT;
-		csrow->last_page = csrow->first_page + csrow->nr_pages - 1;
-		csrow->mtype = MEM_XDR;
-		csrow->edac_mode = EDAC_SECDED;
+		nr_pages = resource_size(&r) >> PAGE_SHIFT;
+		csrow->last_page = csrow->first_page + nr_pages - 1;
+
+		for (j = 0; j < csrow->nr_channels; j++) {
+			dimm = csrow->channels[j].dimm;
+			dimm->mtype = MEM_XDR;
+			dimm->edac_mode = EDAC_SECDED;
+			dimm->nr_pages = nr_pages / csrow->nr_channels;
+		}
 		dev_dbg(mci->dev,
 			"Initialized on node %d, chanmask=0x%x,"
 			" first_page=0x%lx, nr_pages=0x%x\n",
 			priv->node, priv->chanmask,
-			csrow->first_page, csrow->nr_pages);
+			csrow->first_page, nr_pages);
 		break;
 	}
 }
@@ -157,9 +168,10 @@ static int __devinit cell_edac_probe(struct platform_device *pdev)
 {
 	struct cbe_mic_tm_regs __iomem	*regs;
 	struct mem_ctl_info		*mci;
+	struct edac_mc_layer		layers[2];
 	struct cell_edac_priv		*priv;
 	u64				reg;
-	int				rc, chanmask;
+	int				rc, chanmask, num_chans;
 
 	regs = cbe_get_cpu_mic_tm_regs(cbe_node_to_cpu(pdev->id));
 	if (regs == NULL)
@@ -184,8 +196,16 @@ static int __devinit cell_edac_probe(struct platform_device *pdev)
 		in_be64(&regs->mic_fir));
 
 	/* Allocate & init EDAC MC data structure */
-	mci = edac_mc_alloc(sizeof(struct cell_edac_priv), 1,
-			    chanmask == 3 ? 2 : 1, pdev->id);
+	num_chans = chanmask == 3 ? 2 : 1;
+
+	layers[0].type = EDAC_MC_LAYER_CHIP_SELECT;
+	layers[0].size = 1;
+	layers[0].is_virt_csrow = true;
+	layers[1].type = EDAC_MC_LAYER_CHANNEL;
+	layers[1].size = num_chans;
+	layers[1].is_virt_csrow = false;
+	mci = edac_mc_alloc(pdev->id, ARRAY_SIZE(layers), layers,
+			    sizeof(struct cell_edac_priv));
 	if (mci == NULL)
 		return -ENOMEM;
 	priv = mci->pvt_info;
diff --git a/drivers/edac/cpc925_edac.c b/drivers/edac/cpc925_edac.c
index a774c0ddaf5..e22030a9de6 100644
--- a/drivers/edac/cpc925_edac.c
+++ b/drivers/edac/cpc925_edac.c
@@ -329,9 +329,10 @@ static void cpc925_init_csrows(struct mem_ctl_info *mci)
 {
 	struct cpc925_mc_pdata *pdata = mci->pvt_info;
 	struct csrow_info *csrow;
-	int index;
+	struct dimm_info *dimm;
+	int index, j;
 	u32 mbmr, mbbar, bba;
-	unsigned long row_size, last_nr_pages = 0;
+	unsigned long row_size, nr_pages, last_nr_pages = 0;
 
 	get_total_mem(pdata);
 
@@ -350,36 +351,41 @@ static void cpc925_init_csrows(struct mem_ctl_info *mci)
 
 		row_size = bba * (1UL << 28);	/* 256M */
 		csrow->first_page = last_nr_pages;
-		csrow->nr_pages = row_size >> PAGE_SHIFT;
-		csrow->last_page = csrow->first_page + csrow->nr_pages - 1;
+		nr_pages = row_size >> PAGE_SHIFT;
+		csrow->last_page = csrow->first_page + nr_pages - 1;
 		last_nr_pages = csrow->last_page + 1;
 
-		csrow->mtype = MEM_RDDR;
-		csrow->edac_mode = EDAC_SECDED;
-
-		switch (csrow->nr_channels) {
-		case 1: /* Single channel */
-			csrow->grain = 32; /* four-beat burst of 32 bytes */
-			break;
-		case 2: /* Dual channel */
-		default:
-			csrow->grain = 64; /* four-beat burst of 64 bytes */
-			break;
-		}
-
-		switch ((mbmr & MBMR_MODE_MASK) >> MBMR_MODE_SHIFT) {
-		case 6: /* 0110, no way to differentiate X8 VS X16 */
-		case 5:	/* 0101 */
-		case 8: /* 1000 */
-			csrow->dtype = DEV_X16;
-			break;
-		case 7: /* 0111 */
-		case 9: /* 1001 */
-			csrow->dtype = DEV_X8;
-			break;
-		default:
-			csrow->dtype = DEV_UNKNOWN;
-			break;
+		for (j = 0; j < csrow->nr_channels; j++) {
+			dimm = csrow->channels[j].dimm;
+
+			dimm->nr_pages = nr_pages / csrow->nr_channels;
+			dimm->mtype = MEM_RDDR;
+			dimm->edac_mode = EDAC_SECDED;
+
+			switch (csrow->nr_channels) {
+			case 1: /* Single channel */
+				dimm->grain = 32; /* four-beat burst of 32 bytes */
+				break;
+			case 2: /* Dual channel */
+			default:
+				dimm->grain = 64; /* four-beat burst of 64 bytes */
+				break;
+			}
+
+			switch ((mbmr & MBMR_MODE_MASK) >> MBMR_MODE_SHIFT) {
+			case 6: /* 0110, no way to differentiate X8 VS X16 */
+			case 5:	/* 0101 */
+			case 8: /* 1000 */
+				dimm->dtype = DEV_X16;
+				break;
+			case 7: /* 0111 */
+			case 9: /* 1001 */
+				dimm->dtype = DEV_X8;
+				break;
+			default:
+				dimm->dtype = DEV_UNKNOWN;
+				break;
+			}
 		}
 	}
 }
@@ -549,13 +555,18 @@ static void cpc925_mc_check(struct mem_ctl_info *mci)
 	if (apiexcp & CECC_EXCP_DETECTED) {
 		cpc925_mc_printk(mci, KERN_INFO, "DRAM CECC Fault\n");
 		channel = cpc925_mc_find_channel(mci, syndrome);
-		edac_mc_handle_ce(mci, pfn, offset, syndrome,
-				  csrow, channel, mci->ctl_name);
+		edac_mc_handle_error(HW_EVENT_ERR_CORRECTED, mci,
+				     pfn, offset, syndrome,
+				     csrow, channel, -1,
+				     mci->ctl_name, "", NULL);
 	}
 
 	if (apiexcp & UECC_EXCP_DETECTED) {
 		cpc925_mc_printk(mci, KERN_INFO, "DRAM UECC Fault\n");
-		edac_mc_handle_ue(mci, pfn, offset, csrow, mci->ctl_name);
+		edac_mc_handle_error(HW_EVENT_ERR_CORRECTED, mci,
+				     pfn, offset, 0,
+				     csrow, -1, -1,
+				     mci->ctl_name, "", NULL);
 	}
 
 	cpc925_mc_printk(mci, KERN_INFO, "Dump registers:\n");
@@ -927,6 +938,7 @@ static int __devinit cpc925_probe(struct platform_device *pdev)
 {
 	static int edac_mc_idx;
 	struct mem_ctl_info *mci;
+	struct edac_mc_layer layers[2];
 	void __iomem *vbase;
 	struct cpc925_mc_pdata *pdata;
 	struct resource *r;
@@ -962,9 +974,16 @@ static int __devinit cpc925_probe(struct platform_device *pdev)
 		goto err2;
 	}
 
-	nr_channels = cpc925_mc_get_channels(vbase);
-	mci = edac_mc_alloc(sizeof(struct cpc925_mc_pdata),
-			CPC925_NR_CSROWS, nr_channels + 1, edac_mc_idx);
+	nr_channels = cpc925_mc_get_channels(vbase) + 1;
+
+	layers[0].type = EDAC_MC_LAYER_CHIP_SELECT;
+	layers[0].size = CPC925_NR_CSROWS;
+	layers[0].is_virt_csrow = true;
+	layers[1].type = EDAC_MC_LAYER_CHANNEL;
+	layers[1].size = nr_channels;
+	layers[1].is_virt_csrow = false;
+	mci = edac_mc_alloc(edac_mc_idx, ARRAY_SIZE(layers), layers,
+			    sizeof(struct cpc925_mc_pdata));
 	if (!mci) {
 		cpc925_printk(KERN_ERR, "No memory for mem_ctl_info\n");
 		res = -ENOMEM;
diff --git a/drivers/edac/e752x_edac.c b/drivers/edac/e752x_edac.c
index 41223261ede..3186512c973 100644
--- a/drivers/edac/e752x_edac.c
+++ b/drivers/edac/e752x_edac.c
@@ -4,7 +4,11 @@
  * This file may be distributed under the terms of the
  * GNU General Public License.
  *
- * See "enum e752x_chips" below for supported chipsets
+ * Implement support for the e7520, E7525, e7320 and i3100 memory controllers.
+ *
+ * Datasheets:
+ *	http://www.intel.in/content/www/in/en/chipsets/e7525-memory-controller-hub-datasheet.html
+ *	ftp://download.intel.com/design/intarch/datashts/31345803.pdf
  *
  * Written by Tom Zimmerman
  *
@@ -13,8 +17,6 @@
  * 	Wang Zhenyu at intel.com
  * 	Dave Jiang at mvista.com
  *
- * $Id: edac_e752x.c,v 1.5.2.11 2005/10/05 00:43:44 dsp_llnl Exp $
- *
  */
 
 #include <linux/module.h>
@@ -187,6 +189,25 @@ enum e752x_chips {
 	I3100 = 3
 };
 
+/*
+ * Those chips Support single-rank and dual-rank memories only.
+ *
+ * On e752x chips, the odd rows are present only on dual-rank memories.
+ * Dividing the rank by two will provide the dimm#
+ *
+ * i3100 MC has a different mapping: it supports only 4 ranks.
+ *
+ * The mapping is (from 1 to n):
+ *	slot	   single-ranked	double-ranked
+ *	dimm #1 -> rank #4		NA
+ *	dimm #2 -> rank #3		NA
+ *	dimm #3 -> rank #2		Ranks 2 and 3
+ *	dimm #4 -> rank $1		Ranks 1 and 4
+ *
+ * FIXME: The current mapping for i3100 considers that it supports up to 8
+ *	  ranks/chanel, but datasheet says that the MC supports only 4 ranks.
+ */
+
 struct e752x_pvt {
 	struct pci_dev *bridge_ck;
 	struct pci_dev *dev_d0f0;
@@ -350,8 +371,10 @@ static void do_process_ce(struct mem_ctl_info *mci, u16 error_one,
 	channel = !(error_one & 1);
 
 	/* e752x mc reads 34:6 of the DRAM linear address */
-	edac_mc_handle_ce(mci, page, offset_in_page(sec1_add << 4),
-			sec1_syndrome, row, channel, "e752x CE");
+	edac_mc_handle_error(HW_EVENT_ERR_CORRECTED, mci,
+			     page, offset_in_page(sec1_add << 4), sec1_syndrome,
+			     row, channel, -1,
+			     "e752x CE", "", NULL);
 }
 
 static inline void process_ce(struct mem_ctl_info *mci, u16 error_one,
@@ -385,9 +408,12 @@ static void do_process_ue(struct mem_ctl_info *mci, u16 error_one,
 			edac_mc_find_csrow_by_page(mci, block_page);
 
 		/* e752x mc reads 34:6 of the DRAM linear address */
-		edac_mc_handle_ue(mci, block_page,
-				offset_in_page(error_2b << 4),
-				row, "e752x UE from Read");
+		edac_mc_handle_error(HW_EVENT_ERR_UNCORRECTED, mci,
+					block_page,
+					offset_in_page(error_2b << 4), 0,
+					 row, -1, -1,
+					"e752x UE from Read", "", NULL);
+
 	}
 	if (error_one & 0x0404) {
 		error_2b = scrb_add;
@@ -401,9 +427,11 @@ static void do_process_ue(struct mem_ctl_info *mci, u16 error_one,
 			edac_mc_find_csrow_by_page(mci, block_page);
 
 		/* e752x mc reads 34:6 of the DRAM linear address */
-		edac_mc_handle_ue(mci, block_page,
-				offset_in_page(error_2b << 4),
-				row, "e752x UE from Scruber");
+		edac_mc_handle_error(HW_EVENT_ERR_UNCORRECTED, mci,
+					block_page,
+					offset_in_page(error_2b << 4), 0,
+					row, -1, -1,
+					"e752x UE from Scruber", "", NULL);
 	}
 }
 
@@ -426,7 +454,9 @@ static inline void process_ue_no_info_wr(struct mem_ctl_info *mci,
 		return;
 
 	debugf3("%s()\n", __func__);
-	edac_mc_handle_ue_no_info(mci, "e752x UE log memory write");
+	edac_mc_handle_error(HW_EVENT_ERR_UNCORRECTED, mci, 0, 0, 0,
+			     -1, -1, -1,
+			     "e752x UE log memory write", "", NULL);
 }
 
 static void do_process_ded_retry(struct mem_ctl_info *mci, u16 error,
@@ -1044,7 +1074,7 @@ static void e752x_init_csrows(struct mem_ctl_info *mci, struct pci_dev *pdev,
 	int drc_drbg;		/* DRB granularity 0=64mb, 1=128mb */
 	int drc_ddim;		/* DRAM Data Integrity Mode 0=none, 2=edac */
 	u8 value;
-	u32 dra, drc, cumul_size;
+	u32 dra, drc, cumul_size, i, nr_pages;
 
 	dra = 0;
 	for (index = 0; index < 4; index++) {
@@ -1053,7 +1083,7 @@ static void e752x_init_csrows(struct mem_ctl_info *mci, struct pci_dev *pdev,
 		dra |= dra_reg << (index * 8);
 	}
 	pci_read_config_dword(pdev, E752X_DRC, &drc);
-	drc_chan = dual_channel_active(ddrcsr);
+	drc_chan = dual_channel_active(ddrcsr) ? 1 : 0;
 	drc_drbg = drc_chan + 1;	/* 128 in dual mode, 64 in single */
 	drc_ddim = (drc >> 20) & 0x3;
 
@@ -1078,26 +1108,33 @@ static void e752x_init_csrows(struct mem_ctl_info *mci, struct pci_dev *pdev,
 
 		csrow->first_page = last_cumul_size;
 		csrow->last_page = cumul_size - 1;
-		csrow->nr_pages = cumul_size - last_cumul_size;
+		nr_pages = cumul_size - last_cumul_size;
 		last_cumul_size = cumul_size;
-		csrow->grain = 1 << 12;	/* 4KiB - resolution of CELOG */
-		csrow->mtype = MEM_RDDR;	/* only one type supported */
-		csrow->dtype = mem_dev ? DEV_X4 : DEV_X8;
-
-		/*
-		 * if single channel or x8 devices then SECDED
-		 * if dual channel and x4 then S4ECD4ED
-		 */
-		if (drc_ddim) {
-			if (drc_chan && mem_dev) {
-				csrow->edac_mode = EDAC_S4ECD4ED;
-				mci->edac_cap |= EDAC_FLAG_S4ECD4ED;
-			} else {
-				csrow->edac_mode = EDAC_SECDED;
-				mci->edac_cap |= EDAC_FLAG_SECDED;
-			}
-		} else
-			csrow->edac_mode = EDAC_NONE;
+
+		for (i = 0; i < csrow->nr_channels; i++) {
+			struct dimm_info *dimm = csrow->channels[i].dimm;
+
+			debugf3("Initializing rank at (%i,%i)\n", index, i);
+			dimm->nr_pages = nr_pages / csrow->nr_channels;
+			dimm->grain = 1 << 12;	/* 4KiB - resolution of CELOG */
+			dimm->mtype = MEM_RDDR;	/* only one type supported */
+			dimm->dtype = mem_dev ? DEV_X4 : DEV_X8;
+
+			/*
+			* if single channel or x8 devices then SECDED
+			* if dual channel and x4 then S4ECD4ED
+			*/
+			if (drc_ddim) {
+				if (drc_chan && mem_dev) {
+					dimm->edac_mode = EDAC_S4ECD4ED;
+					mci->edac_cap |= EDAC_FLAG_S4ECD4ED;
+				} else {
+					dimm->edac_mode = EDAC_SECDED;
+					mci->edac_cap |= EDAC_FLAG_SECDED;
+				}
+			} else
+				dimm->edac_mode = EDAC_NONE;
+		}
 	}
 }
 
@@ -1226,6 +1263,7 @@ static int e752x_probe1(struct pci_dev *pdev, int dev_idx)
 	u16 pci_data;
 	u8 stat8;
 	struct mem_ctl_info *mci;
+	struct edac_mc_layer layers[2];
 	struct e752x_pvt *pvt;
 	u16 ddrcsr;
 	int drc_chan;		/* Number of channels 0=1chan,1=2chan */
@@ -1252,11 +1290,15 @@ static int e752x_probe1(struct pci_dev *pdev, int dev_idx)
 	/* Dual channel = 1, Single channel = 0 */
 	drc_chan = dual_channel_active(ddrcsr);
 
-	mci = edac_mc_alloc(sizeof(*pvt), E752X_NR_CSROWS, drc_chan + 1, 0);
-
-	if (mci == NULL) {
+	layers[0].type = EDAC_MC_LAYER_CHIP_SELECT;
+	layers[0].size = E752X_NR_CSROWS;
+	layers[0].is_virt_csrow = true;
+	layers[1].type = EDAC_MC_LAYER_CHANNEL;
+	layers[1].size = drc_chan + 1;
+	layers[1].is_virt_csrow = false;
+	mci = edac_mc_alloc(0, ARRAY_SIZE(layers), layers, sizeof(*pvt));
+	if (mci == NULL)
 		return -ENOMEM;
-	}
 
 	debugf3("%s(): init mci\n", __func__);
 	mci->mtype_cap = MEM_FLAG_RDDR;
diff --git a/drivers/edac/e7xxx_edac.c b/drivers/edac/e7xxx_edac.c
index 68dea87b72e..9a9c1a54679 100644
--- a/drivers/edac/e7xxx_edac.c
+++ b/drivers/edac/e7xxx_edac.c
@@ -10,6 +10,9 @@
  * Based on work by Dan Hollis <goemon at anime dot net> and others.
  *	http://www.anime.net/~goemon/linux-ecc/
  *
+ * Datasheet:
+ *	http://www.intel.com/content/www/us/en/chipsets/e7501-chipset-memory-controller-hub-datasheet.html
+ *
  * Contributors:
  *	Eric Biederman (Linux Networx)
  *	Tom Zimmerman (Linux Networx)
@@ -71,7 +74,7 @@
 #endif				/* PCI_DEVICE_ID_INTEL_7505_1_ERR */
 
 #define E7XXX_NR_CSROWS		8	/* number of csrows */
-#define E7XXX_NR_DIMMS		8	/* FIXME - is this correct? */
+#define E7XXX_NR_DIMMS		8	/* 2 channels, 4 dimms/channel */
 
 /* E7XXX register addresses - device 0 function 0 */
 #define E7XXX_DRB		0x60	/* DRAM row boundary register (8b) */
@@ -216,13 +219,15 @@ static void process_ce(struct mem_ctl_info *mci, struct e7xxx_error_info *info)
 	row = edac_mc_find_csrow_by_page(mci, page);
 	/* convert syndrome to channel */
 	channel = e7xxx_find_channel(syndrome);
-	edac_mc_handle_ce(mci, page, 0, syndrome, row, channel, "e7xxx CE");
+	edac_mc_handle_error(HW_EVENT_ERR_CORRECTED, mci, page, 0, syndrome,
+			     row, channel, -1, "e7xxx CE", "", NULL);
 }
 
 static void process_ce_no_info(struct mem_ctl_info *mci)
 {
 	debugf3("%s()\n", __func__);
-	edac_mc_handle_ce_no_info(mci, "e7xxx CE log register overflow");
+	edac_mc_handle_error(HW_EVENT_ERR_UNCORRECTED, mci, 0, 0, 0, -1, -1, -1,
+			     "e7xxx CE log register overflow", "", NULL);
 }
 
 static void process_ue(struct mem_ctl_info *mci, struct e7xxx_error_info *info)
@@ -236,13 +241,17 @@ static void process_ue(struct mem_ctl_info *mci, struct e7xxx_error_info *info)
 	/* FIXME - should use PAGE_SHIFT */
 	block_page = error_2b >> 6;	/* convert to 4k address */
 	row = edac_mc_find_csrow_by_page(mci, block_page);
-	edac_mc_handle_ue(mci, block_page, 0, row, "e7xxx UE");
+
+	edac_mc_handle_error(HW_EVENT_ERR_UNCORRECTED, mci, block_page, 0, 0,
+			     row, -1, -1, "e7xxx UE", "", NULL);
 }
 
 static void process_ue_no_info(struct mem_ctl_info *mci)
 {
 	debugf3("%s()\n", __func__);
-	edac_mc_handle_ue_no_info(mci, "e7xxx UE log register overflow");
+
+	edac_mc_handle_error(HW_EVENT_ERR_UNCORRECTED, mci, 0, 0, 0, -1, -1, -1,
+			     "e7xxx UE log register overflow", "", NULL);
 }
 
 static void e7xxx_get_error_info(struct mem_ctl_info *mci,
@@ -347,11 +356,12 @@ static void e7xxx_init_csrows(struct mem_ctl_info *mci, struct pci_dev *pdev,
 			int dev_idx, u32 drc)
 {
 	unsigned long last_cumul_size;
-	int index;
+	int index, j;
 	u8 value;
-	u32 dra, cumul_size;
+	u32 dra, cumul_size, nr_pages;
 	int drc_chan, drc_drbg, drc_ddim, mem_dev;
 	struct csrow_info *csrow;
+	struct dimm_info *dimm;
 
 	pci_read_config_dword(pdev, E7XXX_DRA, &dra);
 	drc_chan = dual_channel_active(drc, dev_idx);
@@ -379,26 +389,32 @@ static void e7xxx_init_csrows(struct mem_ctl_info *mci, struct pci_dev *pdev,
 
 		csrow->first_page = last_cumul_size;
 		csrow->last_page = cumul_size - 1;
-		csrow->nr_pages = cumul_size - last_cumul_size;
+		nr_pages = cumul_size - last_cumul_size;
 		last_cumul_size = cumul_size;
-		csrow->grain = 1 << 12;	/* 4KiB - resolution of CELOG */
-		csrow->mtype = MEM_RDDR;	/* only one type supported */
-		csrow->dtype = mem_dev ? DEV_X4 : DEV_X8;
-
-		/*
-		 * if single channel or x8 devices then SECDED
-		 * if dual channel and x4 then S4ECD4ED
-		 */
-		if (drc_ddim) {
-			if (drc_chan && mem_dev) {
-				csrow->edac_mode = EDAC_S4ECD4ED;
-				mci->edac_cap |= EDAC_FLAG_S4ECD4ED;
-			} else {
-				csrow->edac_mode = EDAC_SECDED;
-				mci->edac_cap |= EDAC_FLAG_SECDED;
-			}
-		} else
-			csrow->edac_mode = EDAC_NONE;
+
+		for (j = 0; j < drc_chan + 1; j++) {
+			dimm = csrow->channels[j].dimm;
+
+			dimm->nr_pages = nr_pages / (drc_chan + 1);
+			dimm->grain = 1 << 12;	/* 4KiB - resolution of CELOG */
+			dimm->mtype = MEM_RDDR;	/* only one type supported */
+			dimm->dtype = mem_dev ? DEV_X4 : DEV_X8;
+
+			/*
+			* if single channel or x8 devices then SECDED
+			* if dual channel and x4 then S4ECD4ED
+			*/
+			if (drc_ddim) {
+				if (drc_chan && mem_dev) {
+					dimm->edac_mode = EDAC_S4ECD4ED;
+					mci->edac_cap |= EDAC_FLAG_S4ECD4ED;
+				} else {
+					dimm->edac_mode = EDAC_SECDED;
+					mci->edac_cap |= EDAC_FLAG_SECDED;
+				}
+			} else
+				dimm->edac_mode = EDAC_NONE;
+		}
 	}
 }
 
@@ -406,6 +422,7 @@ static int e7xxx_probe1(struct pci_dev *pdev, int dev_idx)
 {
 	u16 pci_data;
 	struct mem_ctl_info *mci = NULL;
+	struct edac_mc_layer layers[2];
 	struct e7xxx_pvt *pvt = NULL;
 	u32 drc;
 	int drc_chan;
@@ -416,8 +433,21 @@ static int e7xxx_probe1(struct pci_dev *pdev, int dev_idx)
 	pci_read_config_dword(pdev, E7XXX_DRC, &drc);
 
 	drc_chan = dual_channel_active(drc, dev_idx);
-	mci = edac_mc_alloc(sizeof(*pvt), E7XXX_NR_CSROWS, drc_chan + 1, 0);
-
+	/*
+	 * According with the datasheet, this device has a maximum of
+	 * 4 DIMMS per channel, either single-rank or dual-rank. So, the
+	 * total amount of dimms is 8 (E7XXX_NR_DIMMS).
+	 * That means that the DIMM is mapped as CSROWs, and the channel
+	 * will map the rank. So, an error to either channel should be
+	 * attributed to the same dimm.
+	 */
+	layers[0].type = EDAC_MC_LAYER_CHIP_SELECT;
+	layers[0].size = E7XXX_NR_CSROWS;
+	layers[0].is_virt_csrow = true;
+	layers[1].type = EDAC_MC_LAYER_CHANNEL;
+	layers[1].size = drc_chan + 1;
+	layers[1].is_virt_csrow = false;
+	mci = edac_mc_alloc(0, ARRAY_SIZE(layers), layers, sizeof(*pvt));
 	if (mci == NULL)
 		return -ENOMEM;
 
diff --git a/drivers/edac/edac_core.h b/drivers/edac/edac_core.h
index e48ab3108ad..117490d4f83 100644
--- a/drivers/edac/edac_core.h
+++ b/drivers/edac/edac_core.h
@@ -107,13 +107,13 @@ extern int edac_debug_level;
  *
  * CPU caches (L1 and L2)
  * DMA engines
- * Core CPU swithces
+ * Core CPU switches
  * Fabric switch units
  * PCIe interface controllers
  * other EDAC/ECC type devices that can be monitored for
  * errors, etc.
  *
- * It allows for a 2 level set of hiearchry. For example:
+ * It allows for a 2 level set of hierarchy. For example:
  *
  * cache could be composed of L1, L2 and L3 levels of cache.
  * Each CPU core would have its own L1 cache, while sharing
@@ -447,8 +447,10 @@ static inline void pci_write_bits32(struct pci_dev *pdev, int offset,
 
 #endif				/* CONFIG_PCI */
 
-extern struct mem_ctl_info *edac_mc_alloc(unsigned sz_pvt, unsigned nr_csrows,
-					  unsigned nr_chans, int edac_index);
+struct mem_ctl_info *edac_mc_alloc(unsigned mc_num,
+				   unsigned n_layers,
+				   struct edac_mc_layer *layers,
+				   unsigned sz_pvt);
 extern int edac_mc_add_mc(struct mem_ctl_info *mci);
 extern void edac_mc_free(struct mem_ctl_info *mci);
 extern struct mem_ctl_info *edac_mc_find(int idx);
@@ -456,35 +458,17 @@ extern struct mem_ctl_info *find_mci_by_dev(struct device *dev);
 extern struct mem_ctl_info *edac_mc_del_mc(struct device *dev);
 extern int edac_mc_find_csrow_by_page(struct mem_ctl_info *mci,
 				      unsigned long page);
-
-/*
- * The no info errors are used when error overflows are reported.
- * There are a limited number of error logging registers that can
- * be exausted.  When all registers are exhausted and an additional
- * error occurs then an error overflow register records that an
- * error occurred and the type of error, but doesn't have any
- * further information.  The ce/ue versions make for cleaner
- * reporting logic and function interface - reduces conditional
- * statement clutter and extra function arguments.
- */
-extern void edac_mc_handle_ce(struct mem_ctl_info *mci,
-			      unsigned long page_frame_number,
-			      unsigned long offset_in_page,
-			      unsigned long syndrome, int row, int channel,
-			      const char *msg);
-extern void edac_mc_handle_ce_no_info(struct mem_ctl_info *mci,
-				      const char *msg);
-extern void edac_mc_handle_ue(struct mem_ctl_info *mci,
-			      unsigned long page_frame_number,
-			      unsigned long offset_in_page, int row,
-			      const char *msg);
-extern void edac_mc_handle_ue_no_info(struct mem_ctl_info *mci,
-				      const char *msg);
-extern void edac_mc_handle_fbd_ue(struct mem_ctl_info *mci, unsigned int csrow,
-				  unsigned int channel0, unsigned int channel1,
-				  char *msg);
-extern void edac_mc_handle_fbd_ce(struct mem_ctl_info *mci, unsigned int csrow,
-				  unsigned int channel, char *msg);
+void edac_mc_handle_error(const enum hw_event_mc_err_type type,
+			  struct mem_ctl_info *mci,
+			  const unsigned long page_frame_number,
+			  const unsigned long offset_in_page,
+			  const unsigned long syndrome,
+			  const int layer0,
+			  const int layer1,
+			  const int layer2,
+			  const char *msg,
+			  const char *other_detail,
+			  const void *mcelog);
 
 /*
  * edac_device APIs
@@ -496,6 +480,7 @@ extern void edac_device_handle_ue(struct edac_device_ctl_info *edac_dev,
 extern void edac_device_handle_ce(struct edac_device_ctl_info *edac_dev,
 				int inst_nr, int block_nr, const char *msg);
 extern int edac_device_alloc_index(void);
+extern const char *edac_layer_name[];
 
 /*
  * edac_pci APIs
diff --git a/drivers/edac/edac_device.c b/drivers/edac/edac_device.c
index 4b154593343..ee3f1f810c1 100644
--- a/drivers/edac/edac_device.c
+++ b/drivers/edac/edac_device.c
@@ -56,7 +56,7 @@ static void edac_device_dump_device(struct edac_device_ctl_info *edac_dev)
  *
  *	The control structure is allocated in complete chunk
  *	from the OS. It is in turn sub allocated to the
- *	various objects that compose the struture
+ *	various objects that compose the structure
  *
  *	The structure has a 'nr_instance' array within itself.
  *	Each instance represents a major component
@@ -79,7 +79,7 @@ struct edac_device_ctl_info *edac_device_alloc_ctl_info(
 	unsigned total_size;
 	unsigned count;
 	unsigned instance, block, attr;
-	void *pvt;
+	void *pvt, *p;
 	int err;
 
 	debugf4("%s() instances=%d blocks=%d\n",
@@ -92,35 +92,30 @@ struct edac_device_ctl_info *edac_device_alloc_ctl_info(
 	 * to be at least as stringent as what the compiler would
 	 * provide if we could simply hardcode everything into a single struct.
 	 */
-	dev_ctl = (struct edac_device_ctl_info *)NULL;
+	p = NULL;
+	dev_ctl = edac_align_ptr(&p, sizeof(*dev_ctl), 1);
 
 	/* Calc the 'end' offset past end of ONE ctl_info structure
 	 * which will become the start of the 'instance' array
 	 */
-	dev_inst = edac_align_ptr(&dev_ctl[1], sizeof(*dev_inst));
+	dev_inst = edac_align_ptr(&p, sizeof(*dev_inst), nr_instances);
 
 	/* Calc the 'end' offset past the instance array within the ctl_info
 	 * which will become the start of the block array
 	 */
-	dev_blk = edac_align_ptr(&dev_inst[nr_instances], sizeof(*dev_blk));
+	count = nr_instances * nr_blocks;
+	dev_blk = edac_align_ptr(&p, sizeof(*dev_blk), count);
 
 	/* Calc the 'end' offset past the dev_blk array
 	 * which will become the start of the attrib array, if any.
 	 */
-	count = nr_instances * nr_blocks;
-	dev_attrib = edac_align_ptr(&dev_blk[count], sizeof(*dev_attrib));
-
-	/* Check for case of when an attribute array is specified */
-	if (nr_attrib > 0) {
-		/* calc how many nr_attrib we need */
+	/* calc how many nr_attrib we need */
+	if (nr_attrib > 0)
 		count *= nr_attrib;
+	dev_attrib = edac_align_ptr(&p, sizeof(*dev_attrib), count);
 
-		/* Calc the 'end' offset past the attributes array */
-		pvt = edac_align_ptr(&dev_attrib[count], sz_private);
-	} else {
-		/* no attribute array specificed */
-		pvt = edac_align_ptr(dev_attrib, sz_private);
-	}
+	/* Calc the 'end' offset past the attributes array */
+	pvt = edac_align_ptr(&p, sz_private, 1);
 
 	/* 'pvt' now points to where the private data area is.
 	 * At this point 'pvt' (like dev_inst,dev_blk and dev_attrib)
@@ -394,7 +389,7 @@ static void edac_device_workq_function(struct work_struct *work_req)
 
 	/* Reschedule the workq for the next time period to start again
 	 * if the number of msec is for 1 sec, then adjust to the next
-	 * whole one second to save timers fireing all over the period
+	 * whole one second to save timers firing all over the period
 	 * between integral seconds
 	 */
 	if (edac_dev->poll_msec == 1000)
@@ -563,7 +558,7 @@ EXPORT_SYMBOL_GPL(edac_device_add_device);
  *	Remove sysfs entries for specified edac_device structure and
  *	then remove edac_device structure from global list
  *
- * @pdev:
+ * @dev:
  *	Pointer to 'struct device' representing edac_device
  *	structure to remove.
  *
diff --git a/drivers/edac/edac_mc.c b/drivers/edac/edac_mc.c
index feef7733fae..10f375032e9 100644
--- a/drivers/edac/edac_mc.c
+++ b/drivers/edac/edac_mc.c
@@ -43,9 +43,26 @@ static void edac_mc_dump_channel(struct rank_info *chan)
 {
 	debugf4("\tchannel = %p\n", chan);
 	debugf4("\tchannel->chan_idx = %d\n", chan->chan_idx);
-	debugf4("\tchannel->ce_count = %d\n", chan->ce_count);
-	debugf4("\tchannel->label = '%s'\n", chan->label);
 	debugf4("\tchannel->csrow = %p\n\n", chan->csrow);
+	debugf4("\tchannel->dimm = %p\n", chan->dimm);
+}
+
+static void edac_mc_dump_dimm(struct dimm_info *dimm)
+{
+	int i;
+
+	debugf4("\tdimm = %p\n", dimm);
+	debugf4("\tdimm->label = '%s'\n", dimm->label);
+	debugf4("\tdimm->nr_pages = 0x%x\n", dimm->nr_pages);
+	debugf4("\tdimm location ");
+	for (i = 0; i < dimm->mci->n_layers; i++) {
+		printk(KERN_CONT "%d", dimm->location[i]);
+		if (i < dimm->mci->n_layers - 1)
+			printk(KERN_CONT ".");
+	}
+	printk(KERN_CONT "\n");
+	debugf4("\tdimm->grain = %d\n", dimm->grain);
+	debugf4("\tdimm->nr_pages = 0x%x\n", dimm->nr_pages);
 }
 
 static void edac_mc_dump_csrow(struct csrow_info *csrow)
@@ -55,7 +72,6 @@ static void edac_mc_dump_csrow(struct csrow_info *csrow)
 	debugf4("\tcsrow->first_page = 0x%lx\n", csrow->first_page);
 	debugf4("\tcsrow->last_page = 0x%lx\n", csrow->last_page);
 	debugf4("\tcsrow->page_mask = 0x%lx\n", csrow->page_mask);
-	debugf4("\tcsrow->nr_pages = 0x%x\n", csrow->nr_pages);
 	debugf4("\tcsrow->nr_channels = %d\n", csrow->nr_channels);
 	debugf4("\tcsrow->channels = %p\n", csrow->channels);
 	debugf4("\tcsrow->mci = %p\n\n", csrow->mci);
@@ -70,6 +86,8 @@ static void edac_mc_dump_mci(struct mem_ctl_info *mci)
 	debugf4("\tmci->edac_check = %p\n", mci->edac_check);
 	debugf3("\tmci->nr_csrows = %d, csrows = %p\n",
 		mci->nr_csrows, mci->csrows);
+	debugf3("\tmci->nr_dimms = %d, dimms = %p\n",
+		mci->tot_dimms, mci->dimms);
 	debugf3("\tdev = %p\n", mci->dev);
 	debugf3("\tmod_name:ctl_name = %s:%s\n", mci->mod_name, mci->ctl_name);
 	debugf3("\tpvt_info = %p\n\n", mci->pvt_info);
@@ -101,18 +119,37 @@ const char *edac_mem_types[] = {
 };
 EXPORT_SYMBOL_GPL(edac_mem_types);
 
-/* 'ptr' points to a possibly unaligned item X such that sizeof(X) is 'size'.
- * Adjust 'ptr' so that its alignment is at least as stringent as what the
- * compiler would provide for X and return the aligned result.
+/**
+ * edac_align_ptr - Prepares the pointer offsets for a single-shot allocation
+ * @p:		pointer to a pointer with the memory offset to be used. At
+ *		return, this will be incremented to point to the next offset
+ * @size:	Size of the data structure to be reserved
+ * @n_elems:	Number of elements that should be reserved
  *
  * If 'size' is a constant, the compiler will optimize this whole function
- * down to either a no-op or the addition of a constant to the value of 'ptr'.
+ * down to either a no-op or the addition of a constant to the value of '*p'.
+ *
+ * The 'p' pointer is absolutely needed to keep the proper advancing
+ * further in memory to the proper offsets when allocating the struct along
+ * with its embedded structs, as edac_device_alloc_ctl_info() does it
+ * above, for example.
+ *
+ * At return, the pointer 'p' will be incremented to be used on a next call
+ * to this function.
  */
-void *edac_align_ptr(void *ptr, unsigned size)
+void *edac_align_ptr(void **p, unsigned size, int n_elems)
 {
 	unsigned align, r;
+	void *ptr = *p;
+
+	*p += size * n_elems;
 
-	/* Here we assume that the alignment of a "long long" is the most
+	/*
+	 * 'p' can possibly be an unaligned item X such that sizeof(X) is
+	 * 'size'.  Adjust 'p' so that its alignment is at least as
+	 * stringent as what the compiler would provide for X and return
+	 * the aligned result.
+	 * Here we assume that the alignment of a "long long" is the most
 	 * stringent alignment that the compiler will ever provide by default.
 	 * As far as I know, this is a reasonable assumption.
 	 */
@@ -132,14 +169,18 @@ void *edac_align_ptr(void *ptr, unsigned size)
 	if (r == 0)
 		return (char *)ptr;
 
+	*p += align - r;
+
 	return (void *)(((unsigned long)ptr) + align - r);
 }
 
 /**
- * edac_mc_alloc: Allocate a struct mem_ctl_info structure
- * @size_pvt:	size of private storage needed
- * @nr_csrows:	Number of CWROWS needed for this MC
- * @nr_chans:	Number of channels for the MC
+ * edac_mc_alloc: Allocate and partially fill a struct mem_ctl_info structure
+ * @mc_num:		Memory controller number
+ * @n_layers:		Number of MC hierarchy layers
+ * layers:		Describes each layer as seen by the Memory Controller
+ * @size_pvt:		size of private storage needed
+ *
  *
  * Everything is kmalloc'ed as one big chunk - more efficient.
  * Only can be used if all structures have the same lifetime - otherwise
@@ -147,32 +188,77 @@ void *edac_align_ptr(void *ptr, unsigned size)
  *
  * Use edac_mc_free() to free mc structures allocated by this function.
  *
+ * NOTE: drivers handle multi-rank memories in different ways: in some
+ * drivers, one multi-rank memory stick is mapped as one entry, while, in
+ * others, a single multi-rank memory stick would be mapped into several
+ * entries. Currently, this function will allocate multiple struct dimm_info
+ * on such scenarios, as grouping the multiple ranks require drivers change.
+ *
  * Returns:
- *	NULL allocation failed
- *	struct mem_ctl_info pointer
+ *	On failure: NULL
+ *	On success: struct mem_ctl_info pointer
  */
-struct mem_ctl_info *edac_mc_alloc(unsigned sz_pvt, unsigned nr_csrows,
-				unsigned nr_chans, int edac_index)
+struct mem_ctl_info *edac_mc_alloc(unsigned mc_num,
+				   unsigned n_layers,
+				   struct edac_mc_layer *layers,
+				   unsigned sz_pvt)
 {
 	struct mem_ctl_info *mci;
-	struct csrow_info *csi, *csrow;
+	struct edac_mc_layer *layer;
+	struct csrow_info *csi, *csr;
 	struct rank_info *chi, *chp, *chan;
-	void *pvt;
-	unsigned size;
-	int row, chn;
-	int err;
+	struct dimm_info *dimm;
+	u32 *ce_per_layer[EDAC_MAX_LAYERS], *ue_per_layer[EDAC_MAX_LAYERS];
+	unsigned pos[EDAC_MAX_LAYERS];
+	unsigned size, tot_dimms = 1, count = 1;
+	unsigned tot_csrows = 1, tot_channels = 1, tot_errcount = 0;
+	void *pvt, *p, *ptr = NULL;
+	int i, j, err, row, chn, n, len;
+	bool per_rank = false;
+
+	BUG_ON(n_layers > EDAC_MAX_LAYERS || n_layers == 0);
+	/*
+	 * Calculate the total amount of dimms and csrows/cschannels while
+	 * in the old API emulation mode
+	 */
+	for (i = 0; i < n_layers; i++) {
+		tot_dimms *= layers[i].size;
+		if (layers[i].is_virt_csrow)
+			tot_csrows *= layers[i].size;
+		else
+			tot_channels *= layers[i].size;
+
+		if (layers[i].type == EDAC_MC_LAYER_CHIP_SELECT)
+			per_rank = true;
+	}
 
 	/* Figure out the offsets of the various items from the start of an mc
 	 * structure.  We want the alignment of each item to be at least as
 	 * stringent as what the compiler would provide if we could simply
 	 * hardcode everything into a single struct.
 	 */
-	mci = (struct mem_ctl_info *)0;
-	csi = edac_align_ptr(&mci[1], sizeof(*csi));
-	chi = edac_align_ptr(&csi[nr_csrows], sizeof(*chi));
-	pvt = edac_align_ptr(&chi[nr_chans * nr_csrows], sz_pvt);
+	mci = edac_align_ptr(&ptr, sizeof(*mci), 1);
+	layer = edac_align_ptr(&ptr, sizeof(*layer), n_layers);
+	csi = edac_align_ptr(&ptr, sizeof(*csi), tot_csrows);
+	chi = edac_align_ptr(&ptr, sizeof(*chi), tot_csrows * tot_channels);
+	dimm = edac_align_ptr(&ptr, sizeof(*dimm), tot_dimms);
+	for (i = 0; i < n_layers; i++) {
+		count *= layers[i].size;
+		debugf4("%s: errcount layer %d size %d\n", __func__, i, count);
+		ce_per_layer[i] = edac_align_ptr(&ptr, sizeof(u32), count);
+		ue_per_layer[i] = edac_align_ptr(&ptr, sizeof(u32), count);
+		tot_errcount += 2 * count;
+	}
+
+	debugf4("%s: allocating %d error counters\n", __func__, tot_errcount);
+	pvt = edac_align_ptr(&ptr, sz_pvt, 1);
 	size = ((unsigned long)pvt) + sz_pvt;
 
+	debugf1("%s(): allocating %u bytes for mci data (%d %s, %d csrows/channels)\n",
+		__func__, size,
+		tot_dimms,
+		per_rank ? "ranks" : "dimms",
+		tot_csrows * tot_channels);
 	mci = kzalloc(size, GFP_KERNEL);
 	if (mci == NULL)
 		return NULL;
@@ -180,28 +266,103 @@ struct mem_ctl_info *edac_mc_alloc(unsigned sz_pvt, unsigned nr_csrows,
 	/* Adjust pointers so they point within the memory we just allocated
 	 * rather than an imaginary chunk of memory located at address 0.
 	 */
+	layer = (struct edac_mc_layer *)(((char *)mci) + ((unsigned long)layer));
 	csi = (struct csrow_info *)(((char *)mci) + ((unsigned long)csi));
 	chi = (struct rank_info *)(((char *)mci) + ((unsigned long)chi));
+	dimm = (struct dimm_info *)(((char *)mci) + ((unsigned long)dimm));
+	for (i = 0; i < n_layers; i++) {
+		mci->ce_per_layer[i] = (u32 *)((char *)mci + ((unsigned long)ce_per_layer[i]));
+		mci->ue_per_layer[i] = (u32 *)((char *)mci + ((unsigned long)ue_per_layer[i]));
+	}
 	pvt = sz_pvt ? (((char *)mci) + ((unsigned long)pvt)) : NULL;
 
 	/* setup index and various internal pointers */
-	mci->mc_idx = edac_index;
+	mci->mc_idx = mc_num;
 	mci->csrows = csi;
+	mci->dimms  = dimm;
+	mci->tot_dimms = tot_dimms;
 	mci->pvt_info = pvt;
-	mci->nr_csrows = nr_csrows;
-
-	for (row = 0; row < nr_csrows; row++) {
-		csrow = &csi[row];
-		csrow->csrow_idx = row;
-		csrow->mci = mci;
-		csrow->nr_channels = nr_chans;
-		chp = &chi[row * nr_chans];
-		csrow->channels = chp;
+	mci->n_layers = n_layers;
+	mci->layers = layer;
+	memcpy(mci->layers, layers, sizeof(*layer) * n_layers);
+	mci->nr_csrows = tot_csrows;
+	mci->num_cschannel = tot_channels;
+	mci->mem_is_per_rank = per_rank;
 
-		for (chn = 0; chn < nr_chans; chn++) {
+	/*
+	 * Fill the csrow struct
+	 */
+	for (row = 0; row < tot_csrows; row++) {
+		csr = &csi[row];
+		csr->csrow_idx = row;
+		csr->mci = mci;
+		csr->nr_channels = tot_channels;
+		chp = &chi[row * tot_channels];
+		csr->channels = chp;
+
+		for (chn = 0; chn < tot_channels; chn++) {
 			chan = &chp[chn];
 			chan->chan_idx = chn;
-			chan->csrow = csrow;
+			chan->csrow = csr;
+		}
+	}
+
+	/*
+	 * Fill the dimm struct
+	 */
+	memset(&pos, 0, sizeof(pos));
+	row = 0;
+	chn = 0;
+	debugf4("%s: initializing %d %s\n", __func__, tot_dimms,
+		per_rank ? "ranks" : "dimms");
+	for (i = 0; i < tot_dimms; i++) {
+		chan = &csi[row].channels[chn];
+		dimm = EDAC_DIMM_PTR(layer, mci->dimms, n_layers,
+			       pos[0], pos[1], pos[2]);
+		dimm->mci = mci;
+
+		debugf2("%s: %d: %s%zd (%d:%d:%d): row %d, chan %d\n", __func__,
+			i, per_rank ? "rank" : "dimm", (dimm - mci->dimms),
+			pos[0], pos[1], pos[2], row, chn);
+
+		/*
+		 * Copy DIMM location and initialize it.
+		 */
+		len = sizeof(dimm->label);
+		p = dimm->label;
+		n = snprintf(p, len, "mc#%u", mc_num);
+		p += n;
+		len -= n;
+		for (j = 0; j < n_layers; j++) {
+			n = snprintf(p, len, "%s#%u",
+				     edac_layer_name[layers[j].type],
+				     pos[j]);
+			p += n;
+			len -= n;
+			dimm->location[j] = pos[j];
+
+			if (len <= 0)
+				break;
+		}
+
+		/* Link it to the csrows old API data */
+		chan->dimm = dimm;
+		dimm->csrow = row;
+		dimm->cschannel = chn;
+
+		/* Increment csrow location */
+		row++;
+		if (row == tot_csrows) {
+			row = 0;
+			chn++;
+		}
+
+		/* Increment dimm location */
+		for (j = n_layers - 1; j >= 0; j--) {
+			pos[j]++;
+			if (pos[j] < layers[j].size)
+				break;
+			pos[j] = 0;
 		}
 	}
 
@@ -490,7 +651,6 @@ EXPORT_SYMBOL(edac_mc_find);
  * edac_mc_add_mc: Insert the 'mci' structure into the mci global list and
  *                 create sysfs entries associated with mci structure
  * @mci: pointer to the mci structure to be added to the list
- * @mc_idx: A unique numeric identifier to be assigned to the 'mci' structure.
  *
  * Return:
  *	0	Success
@@ -517,6 +677,8 @@ int edac_mc_add_mc(struct mem_ctl_info *mci)
 				edac_mc_dump_channel(&mci->csrows[i].
 						channels[j]);
 		}
+		for (i = 0; i < mci->tot_dimms; i++)
+			edac_mc_dump_dimm(&mci->dimms[i]);
 	}
 #endif
 	mutex_lock(&mem_ctls_mutex);
@@ -636,15 +798,19 @@ static void edac_mc_scrub_block(unsigned long page, unsigned long offset,
 int edac_mc_find_csrow_by_page(struct mem_ctl_info *mci, unsigned long page)
 {
 	struct csrow_info *csrows = mci->csrows;
-	int row, i;
+	int row, i, j, n;
 
 	debugf1("MC%d: %s(): 0x%lx\n", mci->mc_idx, __func__, page);
 	row = -1;
 
 	for (i = 0; i < mci->nr_csrows; i++) {
 		struct csrow_info *csrow = &csrows[i];
-
-		if (csrow->nr_pages == 0)
+		n = 0;
+		for (j = 0; j < csrow->nr_channels; j++) {
+			struct dimm_info *dimm = csrow->channels[j].dimm;
+			n += dimm->nr_pages;
+		}
+		if (n == 0)
 			continue;
 
 		debugf3("MC%d: %s(): first(0x%lx) page(0x%lx) last(0x%lx) "
@@ -670,249 +836,307 @@ int edac_mc_find_csrow_by_page(struct mem_ctl_info *mci, unsigned long page)
 }
 EXPORT_SYMBOL_GPL(edac_mc_find_csrow_by_page);
 
-/* FIXME - setable log (warning/emerg) levels */
-/* FIXME - integrate with evlog: http://evlog.sourceforge.net/ */
-void edac_mc_handle_ce(struct mem_ctl_info *mci,
-		unsigned long page_frame_number,
-		unsigned long offset_in_page, unsigned long syndrome,
-		int row, int channel, const char *msg)
-{
-	unsigned long remapped_page;
+const char *edac_layer_name[] = {
+	[EDAC_MC_LAYER_BRANCH] = "branch",
+	[EDAC_MC_LAYER_CHANNEL] = "channel",
+	[EDAC_MC_LAYER_SLOT] = "slot",
+	[EDAC_MC_LAYER_CHIP_SELECT] = "csrow",
+};
+EXPORT_SYMBOL_GPL(edac_layer_name);
 
-	debugf3("MC%d: %s()\n", mci->mc_idx, __func__);
+static void edac_inc_ce_error(struct mem_ctl_info *mci,
+				    bool enable_per_layer_report,
+				    const int pos[EDAC_MAX_LAYERS])
+{
+	int i, index = 0;
 
-	/* FIXME - maybe make panic on INTERNAL ERROR an option */
-	if (row >= mci->nr_csrows || row < 0) {
-		/* something is wrong */
-		edac_mc_printk(mci, KERN_ERR,
-			"INTERNAL ERROR: row out of range "
-			"(%d >= %d)\n", row, mci->nr_csrows);
-		edac_mc_handle_ce_no_info(mci, "INTERNAL ERROR");
-		return;
-	}
+	mci->ce_mc++;
 
-	if (channel >= mci->csrows[row].nr_channels || channel < 0) {
-		/* something is wrong */
-		edac_mc_printk(mci, KERN_ERR,
-			"INTERNAL ERROR: channel out of range "
-			"(%d >= %d)\n", channel,
-			mci->csrows[row].nr_channels);
-		edac_mc_handle_ce_no_info(mci, "INTERNAL ERROR");
+	if (!enable_per_layer_report) {
+		mci->ce_noinfo_count++;
 		return;
 	}
 
-	if (edac_mc_get_log_ce())
-		/* FIXME - put in DIMM location */
-		edac_mc_printk(mci, KERN_WARNING,
-			"CE page 0x%lx, offset 0x%lx, grain %d, syndrome "
-			"0x%lx, row %d, channel %d, label \"%s\": %s\n",
-			page_frame_number, offset_in_page,
-			mci->csrows[row].grain, syndrome, row, channel,
-			mci->csrows[row].channels[channel].label, msg);
-
-	mci->ce_count++;
-	mci->csrows[row].ce_count++;
-	mci->csrows[row].channels[channel].ce_count++;
-
-	if (mci->scrub_mode & SCRUB_SW_SRC) {
-		/*
-		 * Some MC's can remap memory so that it is still available
-		 * at a different address when PCI devices map into memory.
-		 * MC's that can't do this lose the memory where PCI devices
-		 * are mapped.  This mapping is MC dependent and so we call
-		 * back into the MC driver for it to map the MC page to
-		 * a physical (CPU) page which can then be mapped to a virtual
-		 * page - which can then be scrubbed.
-		 */
-		remapped_page = mci->ctl_page_to_phys ?
-			mci->ctl_page_to_phys(mci, page_frame_number) :
-			page_frame_number;
+	for (i = 0; i < mci->n_layers; i++) {
+		if (pos[i] < 0)
+			break;
+		index += pos[i];
+		mci->ce_per_layer[i][index]++;
 
-		edac_mc_scrub_block(remapped_page, offset_in_page,
-				mci->csrows[row].grain);
+		if (i < mci->n_layers - 1)
+			index *= mci->layers[i + 1].size;
 	}
 }
-EXPORT_SYMBOL_GPL(edac_mc_handle_ce);
 
-void edac_mc_handle_ce_no_info(struct mem_ctl_info *mci, const char *msg)
+static void edac_inc_ue_error(struct mem_ctl_info *mci,
+				    bool enable_per_layer_report,
+				    const int pos[EDAC_MAX_LAYERS])
 {
-	if (edac_mc_get_log_ce())
-		edac_mc_printk(mci, KERN_WARNING,
-			"CE - no information available: %s\n", msg);
+	int i, index = 0;
 
-	mci->ce_noinfo_count++;
-	mci->ce_count++;
-}
-EXPORT_SYMBOL_GPL(edac_mc_handle_ce_no_info);
+	mci->ue_mc++;
 
-void edac_mc_handle_ue(struct mem_ctl_info *mci,
-		unsigned long page_frame_number,
-		unsigned long offset_in_page, int row, const char *msg)
-{
-	int len = EDAC_MC_LABEL_LEN * 4;
-	char labels[len + 1];
-	char *pos = labels;
-	int chan;
-	int chars;
-
-	debugf3("MC%d: %s()\n", mci->mc_idx, __func__);
-
-	/* FIXME - maybe make panic on INTERNAL ERROR an option */
-	if (row >= mci->nr_csrows || row < 0) {
-		/* something is wrong */
-		edac_mc_printk(mci, KERN_ERR,
-			"INTERNAL ERROR: row out of range "
-			"(%d >= %d)\n", row, mci->nr_csrows);
-		edac_mc_handle_ue_no_info(mci, "INTERNAL ERROR");
+	if (!enable_per_layer_report) {
+		mci->ce_noinfo_count++;
 		return;
 	}
 
-	chars = snprintf(pos, len + 1, "%s",
-			 mci->csrows[row].channels[0].label);
-	len -= chars;
-	pos += chars;
+	for (i = 0; i < mci->n_layers; i++) {
+		if (pos[i] < 0)
+			break;
+		index += pos[i];
+		mci->ue_per_layer[i][index]++;
 
-	for (chan = 1; (chan < mci->csrows[row].nr_channels) && (len > 0);
-		chan++) {
-		chars = snprintf(pos, len + 1, ":%s",
-				 mci->csrows[row].channels[chan].label);
-		len -= chars;
-		pos += chars;
+		if (i < mci->n_layers - 1)
+			index *= mci->layers[i + 1].size;
 	}
+}
 
-	if (edac_mc_get_log_ue())
-		edac_mc_printk(mci, KERN_EMERG,
-			"UE page 0x%lx, offset 0x%lx, grain %d, row %d, "
-			"labels \"%s\": %s\n", page_frame_number,
-			offset_in_page, mci->csrows[row].grain, row,
-			labels, msg);
+static void edac_ce_error(struct mem_ctl_info *mci,
+			  const int pos[EDAC_MAX_LAYERS],
+			  const char *msg,
+			  const char *location,
+			  const char *label,
+			  const char *detail,
+			  const char *other_detail,
+			  const bool enable_per_layer_report,
+			  const unsigned long page_frame_number,
+			  const unsigned long offset_in_page,
+			  u32 grain)
+{
+	unsigned long remapped_page;
 
-	if (edac_mc_get_panic_on_ue())
-		panic("EDAC MC%d: UE page 0x%lx, offset 0x%lx, grain %d, "
-			"row %d, labels \"%s\": %s\n", mci->mc_idx,
-			page_frame_number, offset_in_page,
-			mci->csrows[row].grain, row, labels, msg);
+	if (edac_mc_get_log_ce()) {
+		if (other_detail && *other_detail)
+			edac_mc_printk(mci, KERN_WARNING,
+				       "CE %s on %s (%s%s - %s)\n",
+				       msg, label, location,
+				       detail, other_detail);
+		else
+			edac_mc_printk(mci, KERN_WARNING,
+				       "CE %s on %s (%s%s)\n",
+				       msg, label, location,
+				       detail);
+	}
+	edac_inc_ce_error(mci, enable_per_layer_report, pos);
 
-	mci->ue_count++;
-	mci->csrows[row].ue_count++;
+	if (mci->scrub_mode & SCRUB_SW_SRC) {
+		/*
+			* Some memory controllers (called MCs below) can remap
+			* memory so that it is still available at a different
+			* address when PCI devices map into memory.
+			* MC's that can't do this, lose the memory where PCI
+			* devices are mapped. This mapping is MC-dependent
+			* and so we call back into the MC driver for it to
+			* map the MC page to a physical (CPU) page which can
+			* then be mapped to a virtual page - which can then
+			* be scrubbed.
+			*/
+		remapped_page = mci->ctl_page_to_phys ?
+			mci->ctl_page_to_phys(mci, page_frame_number) :
+			page_frame_number;
+
+		edac_mc_scrub_block(remapped_page,
+					offset_in_page, grain);
+	}
 }
-EXPORT_SYMBOL_GPL(edac_mc_handle_ue);
 
-void edac_mc_handle_ue_no_info(struct mem_ctl_info *mci, const char *msg)
+static void edac_ue_error(struct mem_ctl_info *mci,
+			  const int pos[EDAC_MAX_LAYERS],
+			  const char *msg,
+			  const char *location,
+			  const char *label,
+			  const char *detail,
+			  const char *other_detail,
+			  const bool enable_per_layer_report)
 {
-	if (edac_mc_get_panic_on_ue())
-		panic("EDAC MC%d: Uncorrected Error", mci->mc_idx);
+	if (edac_mc_get_log_ue()) {
+		if (other_detail && *other_detail)
+			edac_mc_printk(mci, KERN_WARNING,
+				       "UE %s on %s (%s%s - %s)\n",
+			               msg, label, location, detail,
+				       other_detail);
+		else
+			edac_mc_printk(mci, KERN_WARNING,
+				       "UE %s on %s (%s%s)\n",
+			               msg, label, location, detail);
+	}
 
-	if (edac_mc_get_log_ue())
-		edac_mc_printk(mci, KERN_WARNING,
-			"UE - no information available: %s\n", msg);
-	mci->ue_noinfo_count++;
-	mci->ue_count++;
+	if (edac_mc_get_panic_on_ue()) {
+		if (other_detail && *other_detail)
+			panic("UE %s on %s (%s%s - %s)\n",
+			      msg, label, location, detail, other_detail);
+		else
+			panic("UE %s on %s (%s%s)\n",
+			      msg, label, location, detail);
+	}
+
+	edac_inc_ue_error(mci, enable_per_layer_report, pos);
 }
-EXPORT_SYMBOL_GPL(edac_mc_handle_ue_no_info);
 
-/*************************************************************
- * On Fully Buffered DIMM modules, this help function is
- * called to process UE events
- */
-void edac_mc_handle_fbd_ue(struct mem_ctl_info *mci,
-			unsigned int csrow,
-			unsigned int channela,
-			unsigned int channelb, char *msg)
+#define OTHER_LABEL " or "
+void edac_mc_handle_error(const enum hw_event_mc_err_type type,
+			  struct mem_ctl_info *mci,
+			  const unsigned long page_frame_number,
+			  const unsigned long offset_in_page,
+			  const unsigned long syndrome,
+			  const int layer0,
+			  const int layer1,
+			  const int layer2,
+			  const char *msg,
+			  const char *other_detail,
+			  const void *mcelog)
 {
-	int len = EDAC_MC_LABEL_LEN * 4;
-	char labels[len + 1];
-	char *pos = labels;
-	int chars;
+	/* FIXME: too much for stack: move it to some pre-alocated area */
+	char detail[80], location[80];
+	char label[(EDAC_MC_LABEL_LEN + 1 + sizeof(OTHER_LABEL)) * mci->tot_dimms];
+	char *p;
+	int row = -1, chan = -1;
+	int pos[EDAC_MAX_LAYERS] = { layer0, layer1, layer2 };
+	int i;
+	u32 grain;
+	bool enable_per_layer_report = false;
 
-	if (csrow >= mci->nr_csrows) {
-		/* something is wrong */
-		edac_mc_printk(mci, KERN_ERR,
-			"INTERNAL ERROR: row out of range (%d >= %d)\n",
-			csrow, mci->nr_csrows);
-		edac_mc_handle_ue_no_info(mci, "INTERNAL ERROR");
-		return;
-	}
+	debugf3("MC%d: %s()\n", mci->mc_idx, __func__);
 
-	if (channela >= mci->csrows[csrow].nr_channels) {
-		/* something is wrong */
-		edac_mc_printk(mci, KERN_ERR,
-			"INTERNAL ERROR: channel-a out of range "
-			"(%d >= %d)\n",
-			channela, mci->csrows[csrow].nr_channels);
-		edac_mc_handle_ue_no_info(mci, "INTERNAL ERROR");
-		return;
+	/*
+	 * Check if the event report is consistent and if the memory
+	 * location is known. If it is known, enable_per_layer_report will be
+	 * true, the DIMM(s) label info will be filled and the per-layer
+	 * error counters will be incremented.
+	 */
+	for (i = 0; i < mci->n_layers; i++) {
+		if (pos[i] >= (int)mci->layers[i].size) {
+			if (type == HW_EVENT_ERR_CORRECTED)
+				p = "CE";
+			else
+				p = "UE";
+
+			edac_mc_printk(mci, KERN_ERR,
+				       "INTERNAL ERROR: %s value is out of range (%d >= %d)\n",
+				       edac_layer_name[mci->layers[i].type],
+				       pos[i], mci->layers[i].size);
+			/*
+			 * Instead of just returning it, let's use what's
+			 * known about the error. The increment routines and
+			 * the DIMM filter logic will do the right thing by
+			 * pointing the likely damaged DIMMs.
+			 */
+			pos[i] = -1;
+		}
+		if (pos[i] >= 0)
+			enable_per_layer_report = true;
 	}
 
-	if (channelb >= mci->csrows[csrow].nr_channels) {
-		/* something is wrong */
-		edac_mc_printk(mci, KERN_ERR,
-			"INTERNAL ERROR: channel-b out of range "
-			"(%d >= %d)\n",
-			channelb, mci->csrows[csrow].nr_channels);
-		edac_mc_handle_ue_no_info(mci, "INTERNAL ERROR");
-		return;
-	}
+	/*
+	 * Get the dimm label/grain that applies to the match criteria.
+	 * As the error algorithm may not be able to point to just one memory
+	 * stick, the logic here will get all possible labels that could
+	 * pottentially be affected by the error.
+	 * On FB-DIMM memory controllers, for uncorrected errors, it is common
+	 * to have only the MC channel and the MC dimm (also called "branch")
+	 * but the channel is not known, as the memory is arranged in pairs,
+	 * where each memory belongs to a separate channel within the same
+	 * branch.
+	 */
+	grain = 0;
+	p = label;
+	*p = '\0';
+	for (i = 0; i < mci->tot_dimms; i++) {
+		struct dimm_info *dimm = &mci->dimms[i];
 
-	mci->ue_count++;
-	mci->csrows[csrow].ue_count++;
+		if (layer0 >= 0 && layer0 != dimm->location[0])
+			continue;
+		if (layer1 >= 0 && layer1 != dimm->location[1])
+			continue;
+		if (layer2 >= 0 && layer2 != dimm->location[2])
+			continue;
 
-	/* Generate the DIMM labels from the specified channels */
-	chars = snprintf(pos, len + 1, "%s",
-			 mci->csrows[csrow].channels[channela].label);
-	len -= chars;
-	pos += chars;
-	chars = snprintf(pos, len + 1, "-%s",
-			 mci->csrows[csrow].channels[channelb].label);
+		/* get the max grain, over the error match range */
+		if (dimm->grain > grain)
+			grain = dimm->grain;
 
-	if (edac_mc_get_log_ue())
-		edac_mc_printk(mci, KERN_EMERG,
-			"UE row %d, channel-a= %d channel-b= %d "
-			"labels \"%s\": %s\n", csrow, channela, channelb,
-			labels, msg);
+		/*
+		 * If the error is memory-controller wide, there's no need to
+		 * seek for the affected DIMMs because the whole
+		 * channel/memory controller/...  may be affected.
+		 * Also, don't show errors for empty DIMM slots.
+		 */
+		if (enable_per_layer_report && dimm->nr_pages) {
+			if (p != label) {
+				strcpy(p, OTHER_LABEL);
+				p += strlen(OTHER_LABEL);
+			}
+			strcpy(p, dimm->label);
+			p += strlen(p);
+			*p = '\0';
+
+			/*
+			 * get csrow/channel of the DIMM, in order to allow
+			 * incrementing the compat API counters
+			 */
+			debugf4("%s: %s csrows map: (%d,%d)\n",
+				__func__,
+				mci->mem_is_per_rank ? "rank" : "dimm",
+				dimm->csrow, dimm->cschannel);
+
+			if (row == -1)
+				row = dimm->csrow;
+			else if (row >= 0 && row != dimm->csrow)
+				row = -2;
+
+			if (chan == -1)
+				chan = dimm->cschannel;
+			else if (chan >= 0 && chan != dimm->cschannel)
+				chan = -2;
+		}
+	}
 
-	if (edac_mc_get_panic_on_ue())
-		panic("UE row %d, channel-a= %d channel-b= %d "
-			"labels \"%s\": %s\n", csrow, channela,
-			channelb, labels, msg);
-}
-EXPORT_SYMBOL(edac_mc_handle_fbd_ue);
+	if (!enable_per_layer_report) {
+		strcpy(label, "any memory");
+	} else {
+		debugf4("%s: csrow/channel to increment: (%d,%d)\n",
+			__func__, row, chan);
+		if (p == label)
+			strcpy(label, "unknown memory");
+		if (type == HW_EVENT_ERR_CORRECTED) {
+			if (row >= 0) {
+				mci->csrows[row].ce_count++;
+				if (chan >= 0)
+					mci->csrows[row].channels[chan].ce_count++;
+			}
+		} else
+			if (row >= 0)
+				mci->csrows[row].ue_count++;
+	}
 
-/*************************************************************
- * On Fully Buffered DIMM modules, this help function is
- * called to process CE events
- */
-void edac_mc_handle_fbd_ce(struct mem_ctl_info *mci,
-			unsigned int csrow, unsigned int channel, char *msg)
-{
+	/* Fill the RAM location data */
+	p = location;
+	for (i = 0; i < mci->n_layers; i++) {
+		if (pos[i] < 0)
+			continue;
 
-	/* Ensure boundary values */
-	if (csrow >= mci->nr_csrows) {
-		/* something is wrong */
-		edac_mc_printk(mci, KERN_ERR,
-			"INTERNAL ERROR: row out of range (%d >= %d)\n",
-			csrow, mci->nr_csrows);
-		edac_mc_handle_ce_no_info(mci, "INTERNAL ERROR");
-		return;
-	}
-	if (channel >= mci->csrows[csrow].nr_channels) {
-		/* something is wrong */
-		edac_mc_printk(mci, KERN_ERR,
-			"INTERNAL ERROR: channel out of range (%d >= %d)\n",
-			channel, mci->csrows[csrow].nr_channels);
-		edac_mc_handle_ce_no_info(mci, "INTERNAL ERROR");
-		return;
+		p += sprintf(p, "%s:%d ",
+			     edac_layer_name[mci->layers[i].type],
+			     pos[i]);
 	}
 
-	if (edac_mc_get_log_ce())
-		/* FIXME - put in DIMM location */
-		edac_mc_printk(mci, KERN_WARNING,
-			"CE row %d, channel %d, label \"%s\": %s\n",
-			csrow, channel,
-			mci->csrows[csrow].channels[channel].label, msg);
+	/* Memory type dependent details about the error */
+	if (type == HW_EVENT_ERR_CORRECTED) {
+		snprintf(detail, sizeof(detail),
+			"page:0x%lx offset:0x%lx grain:%d syndrome:0x%lx",
+			page_frame_number, offset_in_page,
+			grain, syndrome);
+		edac_ce_error(mci, pos, msg, location, label, detail,
+			      other_detail, enable_per_layer_report,
+			      page_frame_number, offset_in_page, grain);
+	} else {
+		snprintf(detail, sizeof(detail),
+			"page:0x%lx offset:0x%lx grain:%d",
+			page_frame_number, offset_in_page, grain);
 
-	mci->ce_count++;
-	mci->csrows[csrow].ce_count++;
-	mci->csrows[csrow].channels[channel].ce_count++;
+		edac_ue_error(mci, pos, msg, location, label, detail,
+			      other_detail, enable_per_layer_report);
+	}
 }
-EXPORT_SYMBOL(edac_mc_handle_fbd_ce);
+EXPORT_SYMBOL_GPL(edac_mc_handle_error);
diff --git a/drivers/edac/edac_mc_sysfs.c b/drivers/edac/edac_mc_sysfs.c
index e9a28f576d1..f6a29b0eedc 100644
--- a/drivers/edac/edac_mc_sysfs.c
+++ b/drivers/edac/edac_mc_sysfs.c
@@ -144,25 +144,31 @@ static ssize_t csrow_ce_count_show(struct csrow_info *csrow, char *data,
 static ssize_t csrow_size_show(struct csrow_info *csrow, char *data,
 				int private)
 {
-	return sprintf(data, "%u\n", PAGES_TO_MiB(csrow->nr_pages));
+	int i;
+	u32 nr_pages = 0;
+
+	for (i = 0; i < csrow->nr_channels; i++)
+		nr_pages += csrow->channels[i].dimm->nr_pages;
+
+	return sprintf(data, "%u\n", PAGES_TO_MiB(nr_pages));
 }
 
 static ssize_t csrow_mem_type_show(struct csrow_info *csrow, char *data,
 				int private)
 {
-	return sprintf(data, "%s\n", mem_types[csrow->mtype]);
+	return sprintf(data, "%s\n", mem_types[csrow->channels[0].dimm->mtype]);
 }
 
 static ssize_t csrow_dev_type_show(struct csrow_info *csrow, char *data,
 				int private)
 {
-	return sprintf(data, "%s\n", dev_types[csrow->dtype]);
+	return sprintf(data, "%s\n", dev_types[csrow->channels[0].dimm->dtype]);
 }
 
 static ssize_t csrow_edac_mode_show(struct csrow_info *csrow, char *data,
 				int private)
 {
-	return sprintf(data, "%s\n", edac_caps[csrow->edac_mode]);
+	return sprintf(data, "%s\n", edac_caps[csrow->channels[0].dimm->edac_mode]);
 }
 
 /* show/store functions for DIMM Label attributes */
@@ -170,11 +176,11 @@ static ssize_t channel_dimm_label_show(struct csrow_info *csrow,
 				char *data, int channel)
 {
 	/* if field has not been initialized, there is nothing to send */
-	if (!csrow->channels[channel].label[0])
+	if (!csrow->channels[channel].dimm->label[0])
 		return 0;
 
 	return snprintf(data, EDAC_MC_LABEL_LEN, "%s\n",
-			csrow->channels[channel].label);
+			csrow->channels[channel].dimm->label);
 }
 
 static ssize_t channel_dimm_label_store(struct csrow_info *csrow,
@@ -184,8 +190,8 @@ static ssize_t channel_dimm_label_store(struct csrow_info *csrow,
 	ssize_t max_size = 0;
 
 	max_size = min((ssize_t) count, (ssize_t) EDAC_MC_LABEL_LEN - 1);
-	strncpy(csrow->channels[channel].label, data, max_size);
-	csrow->channels[channel].label[max_size] = '\0';
+	strncpy(csrow->channels[channel].dimm->label, data, max_size);
+	csrow->channels[channel].dimm->label[max_size] = '\0';
 
 	return max_size;
 }
@@ -419,8 +425,8 @@ static ssize_t mci_reset_counters_store(struct mem_ctl_info *mci,
 
 	mci->ue_noinfo_count = 0;
 	mci->ce_noinfo_count = 0;
-	mci->ue_count = 0;
-	mci->ce_count = 0;
+	mci->ue_mc = 0;
+	mci->ce_mc = 0;
 
 	for (row = 0; row < mci->nr_csrows; row++) {
 		struct csrow_info *ri = &mci->csrows[row];
@@ -489,12 +495,12 @@ static ssize_t mci_sdram_scrub_rate_show(struct mem_ctl_info *mci, char *data)
 /* default attribute files for the MCI object */
 static ssize_t mci_ue_count_show(struct mem_ctl_info *mci, char *data)
 {
-	return sprintf(data, "%d\n", mci->ue_count);
+	return sprintf(data, "%d\n", mci->ue_mc);
 }
 
 static ssize_t mci_ce_count_show(struct mem_ctl_info *mci, char *data)
 {
-	return sprintf(data, "%d\n", mci->ce_count);
+	return sprintf(data, "%d\n", mci->ce_mc);
 }
 
 static ssize_t mci_ce_noinfo_show(struct mem_ctl_info *mci, char *data)
@@ -519,16 +525,16 @@ static ssize_t mci_ctl_name_show(struct mem_ctl_info *mci, char *data)
 
 static ssize_t mci_size_mb_show(struct mem_ctl_info *mci, char *data)
 {
-	int total_pages, csrow_idx;
+	int total_pages = 0, csrow_idx, j;
 
-	for (total_pages = csrow_idx = 0; csrow_idx < mci->nr_csrows;
-		csrow_idx++) {
+	for (csrow_idx = 0; csrow_idx < mci->nr_csrows; csrow_idx++) {
 		struct csrow_info *csrow = &mci->csrows[csrow_idx];
 
-		if (!csrow->nr_pages)
-			continue;
+		for (j = 0; j < csrow->nr_channels; j++) {
+			struct dimm_info *dimm = csrow->channels[j].dimm;
 
-		total_pages += csrow->nr_pages;
+			total_pages += dimm->nr_pages;
+		}
 	}
 
 	return sprintf(data, "%u\n", PAGES_TO_MiB(total_pages));
@@ -900,7 +906,7 @@ static void edac_remove_mci_instance_attributes(struct mem_ctl_info *mci,
  */
 int edac_create_sysfs_mci_device(struct mem_ctl_info *mci)
 {
-	int i;
+	int i, j;
 	int err;
 	struct csrow_info *csrow;
 	struct kobject *kobj_mci = &mci->edac_mci_kobj;
@@ -934,10 +940,13 @@ int edac_create_sysfs_mci_device(struct mem_ctl_info *mci)
 	/* Make directories for each CSROW object under the mc<id> kobject
 	 */
 	for (i = 0; i < mci->nr_csrows; i++) {
+		int nr_pages = 0;
+
 		csrow = &mci->csrows[i];
+		for (j = 0; j < csrow->nr_channels; j++)
+			nr_pages += csrow->channels[j].dimm->nr_pages;
 
-		/* Only expose populated CSROWs */
-		if (csrow->nr_pages > 0) {
+		if (nr_pages > 0) {
 			err = edac_create_csrow_object(mci, csrow, i);
 			if (err) {
 				debugf1("%s() failure: create csrow %d obj\n",
@@ -949,12 +958,15 @@ int edac_create_sysfs_mci_device(struct mem_ctl_info *mci)
 
 	return 0;
 
-	/* CSROW error: backout what has already been registered,  */
 fail1:
 	for (i--; i >= 0; i--) {
-		if (csrow->nr_pages > 0) {
+		int nr_pages = 0;
+
+		csrow = &mci->csrows[i];
+		for (j = 0; j < csrow->nr_channels; j++)
+			nr_pages += csrow->channels[j].dimm->nr_pages;
+		if (nr_pages > 0)
 			kobject_put(&mci->csrows[i].kobj);
-		}
 	}
 
 	/* remove the mci instance's attributes, if any */
@@ -973,14 +985,20 @@ fail0:
  */
 void edac_remove_sysfs_mci_device(struct mem_ctl_info *mci)
 {
-	int i;
+	struct csrow_info *csrow;
+	int i, j;
 
 	debugf0("%s()\n", __func__);
 
 	/* remove all csrow kobjects */
 	debugf4("%s()  unregister this mci kobj\n", __func__);
 	for (i = 0; i < mci->nr_csrows; i++) {
-		if (mci->csrows[i].nr_pages > 0) {
+		int nr_pages = 0;
+
+		csrow = &mci->csrows[i];
+		for (j = 0; j < csrow->nr_channels; j++)
+			nr_pages += csrow->channels[j].dimm->nr_pages;
+		if (nr_pages > 0) {
 			debugf0("%s()  unreg csrow-%d\n", __func__, i);
 			kobject_put(&mci->csrows[i].kobj);
 		}
diff --git a/drivers/edac/edac_module.h b/drivers/edac/edac_module.h
index 00f81b47a51..0ea7d14cb93 100644
--- a/drivers/edac/edac_module.h
+++ b/drivers/edac/edac_module.h
@@ -50,7 +50,7 @@ extern void edac_device_reset_delay_period(struct edac_device_ctl_info
 					   *edac_dev, unsigned long value);
 extern void edac_mc_reset_delay_period(int value);
 
-extern void *edac_align_ptr(void *ptr, unsigned size);
+extern void *edac_align_ptr(void **p, unsigned size, int n_elems);
 
 /*
  * EDAC PCI functions
diff --git a/drivers/edac/edac_pci.c b/drivers/edac/edac_pci.c
index 63af1c5673d..f1ac8664988 100644
--- a/drivers/edac/edac_pci.c
+++ b/drivers/edac/edac_pci.c
@@ -42,13 +42,13 @@ struct edac_pci_ctl_info *edac_pci_alloc_ctl_info(unsigned int sz_pvt,
 						const char *edac_pci_name)
 {
 	struct edac_pci_ctl_info *pci;
-	void *pvt;
+	void *p = NULL, *pvt;
 	unsigned int size;
 
 	debugf1("%s()\n", __func__);
 
-	pci = (struct edac_pci_ctl_info *)0;
-	pvt = edac_align_ptr(&pci[1], sz_pvt);
+	pci = edac_align_ptr(&p, sizeof(*pci), 1);
+	pvt = edac_align_ptr(&p, 1, sz_pvt);
 	size = ((unsigned long)pvt) + sz_pvt;
 
 	/* Alloc the needed control struct memory */
diff --git a/drivers/edac/i3000_edac.c b/drivers/edac/i3000_edac.c
index 277689a6884..8ad1744faac 100644
--- a/drivers/edac/i3000_edac.c
+++ b/drivers/edac/i3000_edac.c
@@ -245,7 +245,9 @@ static int i3000_process_error_info(struct mem_ctl_info *mci,
 		return 1;
 
 	if ((info->errsts ^ info->errsts2) & I3000_ERRSTS_BITS) {
-		edac_mc_handle_ce_no_info(mci, "UE overwrote CE");
+		edac_mc_handle_error(HW_EVENT_ERR_UNCORRECTED, mci, 0, 0, 0,
+				     -1, -1, -1,
+				     "UE overwrote CE", "", NULL);
 		info->errsts = info->errsts2;
 	}
 
@@ -256,10 +258,15 @@ static int i3000_process_error_info(struct mem_ctl_info *mci,
 	row = edac_mc_find_csrow_by_page(mci, pfn);
 
 	if (info->errsts & I3000_ERRSTS_UE)
-		edac_mc_handle_ue(mci, pfn, offset, row, "i3000 UE");
+		edac_mc_handle_error(HW_EVENT_ERR_UNCORRECTED, mci,
+				     pfn, offset, 0,
+				     row, -1, -1,
+				     "i3000 UE", "", NULL);
 	else
-		edac_mc_handle_ce(mci, pfn, offset, info->derrsyn, row,
-				multi_chan ? channel : 0, "i3000 CE");
+		edac_mc_handle_error(HW_EVENT_ERR_CORRECTED, mci,
+				     pfn, offset, info->derrsyn,
+				     row, multi_chan ? channel : 0, -1,
+				     "i3000 CE", "", NULL);
 
 	return 1;
 }
@@ -304,9 +311,10 @@ static int i3000_is_interleaved(const unsigned char *c0dra,
 static int i3000_probe1(struct pci_dev *pdev, int dev_idx)
 {
 	int rc;
-	int i;
+	int i, j;
 	struct mem_ctl_info *mci = NULL;
-	unsigned long last_cumul_size;
+	struct edac_mc_layer layers[2];
+	unsigned long last_cumul_size, nr_pages;
 	int interleaved, nr_channels;
 	unsigned char dra[I3000_RANKS / 2], drb[I3000_RANKS];
 	unsigned char *c0dra = dra, *c1dra = &dra[I3000_RANKS_PER_CHANNEL / 2];
@@ -347,7 +355,14 @@ static int i3000_probe1(struct pci_dev *pdev, int dev_idx)
 	 */
 	interleaved = i3000_is_interleaved(c0dra, c1dra, c0drb, c1drb);
 	nr_channels = interleaved ? 2 : 1;
-	mci = edac_mc_alloc(0, I3000_RANKS / nr_channels, nr_channels, 0);
+
+	layers[0].type = EDAC_MC_LAYER_CHIP_SELECT;
+	layers[0].size = I3000_RANKS / nr_channels;
+	layers[0].is_virt_csrow = true;
+	layers[1].type = EDAC_MC_LAYER_CHANNEL;
+	layers[1].size = nr_channels;
+	layers[1].is_virt_csrow = false;
+	mci = edac_mc_alloc(0, ARRAY_SIZE(layers), layers, 0);
 	if (!mci)
 		return -ENOMEM;
 
@@ -386,19 +401,23 @@ static int i3000_probe1(struct pci_dev *pdev, int dev_idx)
 			cumul_size <<= 1;
 		debugf3("MC: %s(): (%d) cumul_size 0x%x\n",
 			__func__, i, cumul_size);
-		if (cumul_size == last_cumul_size) {
-			csrow->mtype = MEM_EMPTY;
+		if (cumul_size == last_cumul_size)
 			continue;
-		}
 
 		csrow->first_page = last_cumul_size;
 		csrow->last_page = cumul_size - 1;
-		csrow->nr_pages = cumul_size - last_cumul_size;
+		nr_pages = cumul_size - last_cumul_size;
 		last_cumul_size = cumul_size;
-		csrow->grain = I3000_DEAP_GRAIN;
-		csrow->mtype = MEM_DDR2;
-		csrow->dtype = DEV_UNKNOWN;
-		csrow->edac_mode = EDAC_UNKNOWN;
+
+		for (j = 0; j < nr_channels; j++) {
+			struct dimm_info *dimm = csrow->channels[j].dimm;
+
+			dimm->nr_pages = nr_pages / nr_channels;
+			dimm->grain = I3000_DEAP_GRAIN;
+			dimm->mtype = MEM_DDR2;
+			dimm->dtype = DEV_UNKNOWN;
+			dimm->edac_mode = EDAC_UNKNOWN;
+		}
 	}
 
 	/*
diff --git a/drivers/edac/i3200_edac.c b/drivers/edac/i3200_edac.c
index 046808c6357..bbe43ef7182 100644
--- a/drivers/edac/i3200_edac.c
+++ b/drivers/edac/i3200_edac.c
@@ -23,6 +23,7 @@
 
 #define PCI_DEVICE_ID_INTEL_3200_HB    0x29f0
 
+#define I3200_DIMMS		4
 #define I3200_RANKS		8
 #define I3200_RANKS_PER_CHANNEL	4
 #define I3200_CHANNELS		2
@@ -217,21 +218,25 @@ static void i3200_process_error_info(struct mem_ctl_info *mci,
 		return;
 
 	if ((info->errsts ^ info->errsts2) & I3200_ERRSTS_BITS) {
-		edac_mc_handle_ce_no_info(mci, "UE overwrote CE");
+		edac_mc_handle_error(HW_EVENT_ERR_UNCORRECTED, mci, 0, 0, 0,
+				     -1, -1, -1, "UE overwrote CE", "", NULL);
 		info->errsts = info->errsts2;
 	}
 
 	for (channel = 0; channel < nr_channels; channel++) {
 		log = info->eccerrlog[channel];
 		if (log & I3200_ECCERRLOG_UE) {
-			edac_mc_handle_ue(mci, 0, 0,
-				eccerrlog_row(channel, log),
-				"i3200 UE");
+			edac_mc_handle_error(HW_EVENT_ERR_UNCORRECTED, mci,
+					     0, 0, 0,
+					     eccerrlog_row(channel, log),
+					     -1, -1,
+					     "i3000 UE", "", NULL);
 		} else if (log & I3200_ECCERRLOG_CE) {
-			edac_mc_handle_ce(mci, 0, 0,
-				eccerrlog_syndrome(log),
-				eccerrlog_row(channel, log), 0,
-				"i3200 CE");
+			edac_mc_handle_error(HW_EVENT_ERR_UNCORRECTED, mci,
+					     0, 0, eccerrlog_syndrome(log),
+					     eccerrlog_row(channel, log),
+					     -1, -1,
+					     "i3000 UE", "", NULL);
 		}
 	}
 }
@@ -319,9 +324,9 @@ static unsigned long drb_to_nr_pages(
 static int i3200_probe1(struct pci_dev *pdev, int dev_idx)
 {
 	int rc;
-	int i;
+	int i, j;
 	struct mem_ctl_info *mci = NULL;
-	unsigned long last_page;
+	struct edac_mc_layer layers[2];
 	u16 drbs[I3200_CHANNELS][I3200_RANKS_PER_CHANNEL];
 	bool stacked;
 	void __iomem *window;
@@ -336,8 +341,14 @@ static int i3200_probe1(struct pci_dev *pdev, int dev_idx)
 	i3200_get_drbs(window, drbs);
 	nr_channels = how_many_channels(pdev);
 
-	mci = edac_mc_alloc(sizeof(struct i3200_priv), I3200_RANKS,
-		nr_channels, 0);
+	layers[0].type = EDAC_MC_LAYER_CHIP_SELECT;
+	layers[0].size = I3200_DIMMS;
+	layers[0].is_virt_csrow = true;
+	layers[1].type = EDAC_MC_LAYER_CHANNEL;
+	layers[1].size = nr_channels;
+	layers[1].is_virt_csrow = false;
+	mci = edac_mc_alloc(0, ARRAY_SIZE(layers), layers,
+			    sizeof(struct i3200_priv));
 	if (!mci)
 		return -ENOMEM;
 
@@ -366,7 +377,6 @@ static int i3200_probe1(struct pci_dev *pdev, int dev_idx)
 	 * cumulative; the last one will contain the total memory
 	 * contained in all ranks.
 	 */
-	last_page = -1UL;
 	for (i = 0; i < mci->nr_csrows; i++) {
 		unsigned long nr_pages;
 		struct csrow_info *csrow = &mci->csrows[i];
@@ -375,20 +385,18 @@ static int i3200_probe1(struct pci_dev *pdev, int dev_idx)
 			i / I3200_RANKS_PER_CHANNEL,
 			i % I3200_RANKS_PER_CHANNEL);
 
-		if (nr_pages == 0) {
-			csrow->mtype = MEM_EMPTY;
+		if (nr_pages == 0)
 			continue;
-		}
 
-		csrow->first_page = last_page + 1;
-		last_page += nr_pages;
-		csrow->last_page = last_page;
-		csrow->nr_pages = nr_pages;
+		for (j = 0; j < nr_channels; j++) {
+			struct dimm_info *dimm = csrow->channels[j].dimm;
 
-		csrow->grain = nr_pages << PAGE_SHIFT;
-		csrow->mtype = MEM_DDR2;
-		csrow->dtype = DEV_UNKNOWN;
-		csrow->edac_mode = EDAC_UNKNOWN;
+			dimm->nr_pages = nr_pages / nr_channels;
+			dimm->grain = nr_pages << PAGE_SHIFT;
+			dimm->mtype = MEM_DDR2;
+			dimm->dtype = DEV_UNKNOWN;
+			dimm->edac_mode = EDAC_UNKNOWN;
+		}
 	}
 
 	i3200_clear_error_info(mci);
diff --git a/drivers/edac/i5000_edac.c b/drivers/edac/i5000_edac.c
index a2680d8e744..11ea835f155 100644
--- a/drivers/edac/i5000_edac.c
+++ b/drivers/edac/i5000_edac.c
@@ -270,7 +270,8 @@
 #define MTR3		0x8C
 
 #define NUM_MTRS		4
-#define CHANNELS_PER_BRANCH	(2)
+#define CHANNELS_PER_BRANCH	2
+#define MAX_BRANCHES		2
 
 /* Defines to extract the vaious fields from the
  *	MTRx - Memory Technology Registers
@@ -473,7 +474,6 @@ static void i5000_process_fatal_error_info(struct mem_ctl_info *mci,
 	char msg[EDAC_MC_LABEL_LEN + 1 + 160];
 	char *specific = NULL;
 	u32 allErrors;
-	int branch;
 	int channel;
 	int bank;
 	int rank;
@@ -485,8 +485,7 @@ static void i5000_process_fatal_error_info(struct mem_ctl_info *mci,
 	if (!allErrors)
 		return;		/* if no error, return now */
 
-	branch = EXTRACT_FBDCHAN_INDX(info->ferr_fat_fbd);
-	channel = branch;
+	channel = EXTRACT_FBDCHAN_INDX(info->ferr_fat_fbd);
 
 	/* Use the NON-Recoverable macros to extract data */
 	bank = NREC_BANK(info->nrecmema);
@@ -495,9 +494,9 @@ static void i5000_process_fatal_error_info(struct mem_ctl_info *mci,
 	ras = NREC_RAS(info->nrecmemb);
 	cas = NREC_CAS(info->nrecmemb);
 
-	debugf0("\t\tCSROW= %d  Channels= %d,%d  (Branch= %d "
-		"DRAM Bank= %d rdwr= %s ras= %d cas= %d)\n",
-		rank, channel, channel + 1, branch >> 1, bank,
+	debugf0("\t\tCSROW= %d  Channel= %d "
+		"(DRAM Bank= %d rdwr= %s ras= %d cas= %d)\n",
+		rank, channel, bank,
 		rdwr ? "Write" : "Read", ras, cas);
 
 	/* Only 1 bit will be on */
@@ -533,13 +532,14 @@ static void i5000_process_fatal_error_info(struct mem_ctl_info *mci,
 
 	/* Form out message */
 	snprintf(msg, sizeof(msg),
-		 "(Branch=%d DRAM-Bank=%d RDWR=%s RAS=%d CAS=%d "
-		 "FATAL Err=0x%x (%s))",
-		 branch >> 1, bank, rdwr ? "Write" : "Read", ras, cas,
-		 allErrors, specific);
+		 "Bank=%d RAS=%d CAS=%d FATAL Err=0x%x (%s)",
+		 bank, ras, cas, allErrors, specific);
 
 	/* Call the helper to output message */
-	edac_mc_handle_fbd_ue(mci, rank, channel, channel + 1, msg);
+	edac_mc_handle_error(HW_EVENT_ERR_FATAL, mci, 0, 0, 0,
+			     channel >> 1, channel & 1, rank,
+			     rdwr ? "Write error" : "Read error",
+			     msg, NULL);
 }
 
 /*
@@ -633,13 +633,14 @@ static void i5000_process_nonfatal_error_info(struct mem_ctl_info *mci,
 
 		/* Form out message */
 		snprintf(msg, sizeof(msg),
-			 "(Branch=%d DRAM-Bank=%d RDWR=%s RAS=%d "
-			 "CAS=%d, UE Err=0x%x (%s))",
-			 branch >> 1, bank, rdwr ? "Write" : "Read", ras, cas,
-			 ue_errors, specific);
+			 "Rank=%d Bank=%d RAS=%d CAS=%d, UE Err=0x%x (%s)",
+			 rank, bank, ras, cas, ue_errors, specific);
 
 		/* Call the helper to output message */
-		edac_mc_handle_fbd_ue(mci, rank, channel, channel + 1, msg);
+		edac_mc_handle_error(HW_EVENT_ERR_UNCORRECTED, mci, 0, 0, 0,
+				channel >> 1, -1, rank,
+				rdwr ? "Write error" : "Read error",
+				msg, NULL);
 	}
 
 	/* Check correctable errors */
@@ -685,13 +686,16 @@ static void i5000_process_nonfatal_error_info(struct mem_ctl_info *mci,
 
 		/* Form out message */
 		snprintf(msg, sizeof(msg),
-			 "(Branch=%d DRAM-Bank=%d RDWR=%s RAS=%d "
+			 "Rank=%d Bank=%d RDWR=%s RAS=%d "
 			 "CAS=%d, CE Err=0x%x (%s))", branch >> 1, bank,
 			 rdwr ? "Write" : "Read", ras, cas, ce_errors,
 			 specific);
 
 		/* Call the helper to output message */
-		edac_mc_handle_fbd_ce(mci, rank, channel, msg);
+		edac_mc_handle_error(HW_EVENT_ERR_CORRECTED, mci, 0, 0, 0,
+				channel >> 1, channel % 2, rank,
+				rdwr ? "Write error" : "Read error",
+				msg, NULL);
 	}
 
 	if (!misc_messages)
@@ -731,11 +735,12 @@ static void i5000_process_nonfatal_error_info(struct mem_ctl_info *mci,
 
 		/* Form out message */
 		snprintf(msg, sizeof(msg),
-			 "(Branch=%d Err=%#x (%s))", branch >> 1,
-			 misc_errors, specific);
+			 "Err=%#x (%s)", misc_errors, specific);
 
 		/* Call the helper to output message */
-		edac_mc_handle_fbd_ce(mci, 0, 0, msg);
+		edac_mc_handle_error(HW_EVENT_ERR_CORRECTED, mci, 0, 0, 0,
+				branch >> 1, -1, -1,
+				"Misc error", msg, NULL);
 	}
 }
 
@@ -956,14 +961,14 @@ static int determine_amb_present_reg(struct i5000_pvt *pvt, int channel)
  *
  *	return the proper MTR register as determine by the csrow and channel desired
  */
-static int determine_mtr(struct i5000_pvt *pvt, int csrow, int channel)
+static int determine_mtr(struct i5000_pvt *pvt, int slot, int channel)
 {
 	int mtr;
 
 	if (channel < CHANNELS_PER_BRANCH)
-		mtr = pvt->b0_mtr[csrow >> 1];
+		mtr = pvt->b0_mtr[slot];
 	else
-		mtr = pvt->b1_mtr[csrow >> 1];
+		mtr = pvt->b1_mtr[slot];
 
 	return mtr;
 }
@@ -988,37 +993,34 @@ static void decode_mtr(int slot_row, u16 mtr)
 	debugf2("\t\tNUMCOL: %s\n", numcol_toString[MTR_DIMM_COLS(mtr)]);
 }
 
-static void handle_channel(struct i5000_pvt *pvt, int csrow, int channel,
+static void handle_channel(struct i5000_pvt *pvt, int slot, int channel,
 			struct i5000_dimm_info *dinfo)
 {
 	int mtr;
 	int amb_present_reg;
 	int addrBits;
 
-	mtr = determine_mtr(pvt, csrow, channel);
+	mtr = determine_mtr(pvt, slot, channel);
 	if (MTR_DIMMS_PRESENT(mtr)) {
 		amb_present_reg = determine_amb_present_reg(pvt, channel);
 
-		/* Determine if there is  a  DIMM present in this DIMM slot */
-		if (amb_present_reg & (1 << (csrow >> 1))) {
+		/* Determine if there is a DIMM present in this DIMM slot */
+		if (amb_present_reg) {
 			dinfo->dual_rank = MTR_DIMM_RANK(mtr);
 
-			if (!((dinfo->dual_rank == 0) &&
-				((csrow & 0x1) == 0x1))) {
-				/* Start with the number of bits for a Bank
-				 * on the DRAM */
-				addrBits = MTR_DRAM_BANKS_ADDR_BITS(mtr);
-				/* Add thenumber of ROW bits */
-				addrBits += MTR_DIMM_ROWS_ADDR_BITS(mtr);
-				/* add the number of COLUMN bits */
-				addrBits += MTR_DIMM_COLS_ADDR_BITS(mtr);
-
-				addrBits += 6;	/* add 64 bits per DIMM */
-				addrBits -= 20;	/* divide by 2^^20 */
-				addrBits -= 3;	/* 8 bits per bytes */
-
-				dinfo->megabytes = 1 << addrBits;
-			}
+			/* Start with the number of bits for a Bank
+				* on the DRAM */
+			addrBits = MTR_DRAM_BANKS_ADDR_BITS(mtr);
+			/* Add the number of ROW bits */
+			addrBits += MTR_DIMM_ROWS_ADDR_BITS(mtr);
+			/* add the number of COLUMN bits */
+			addrBits += MTR_DIMM_COLS_ADDR_BITS(mtr);
+
+			addrBits += 6;	/* add 64 bits per DIMM */
+			addrBits -= 20;	/* divide by 2^^20 */
+			addrBits -= 3;	/* 8 bits per bytes */
+
+			dinfo->megabytes = 1 << addrBits;
 		}
 	}
 }
@@ -1032,10 +1034,9 @@ static void handle_channel(struct i5000_pvt *pvt, int csrow, int channel,
 static void calculate_dimm_size(struct i5000_pvt *pvt)
 {
 	struct i5000_dimm_info *dinfo;
-	int csrow, max_csrows;
+	int slot, channel, branch;
 	char *p, *mem_buffer;
 	int space, n;
-	int channel;
 
 	/* ================= Generate some debug output ================= */
 	space = PAGE_SIZE;
@@ -1046,22 +1047,17 @@ static void calculate_dimm_size(struct i5000_pvt *pvt)
 		return;
 	}
 
-	n = snprintf(p, space, "\n");
-	p += n;
-	space -= n;
-
-	/* Scan all the actual CSROWS (which is # of DIMMS * 2)
+	/* Scan all the actual slots
 	 * and calculate the information for each DIMM
-	 * Start with the highest csrow first, to display it first
-	 * and work toward the 0th csrow
+	 * Start with the highest slot first, to display it first
+	 * and work toward the 0th slot
 	 */
-	max_csrows = pvt->maxdimmperch * 2;
-	for (csrow = max_csrows - 1; csrow >= 0; csrow--) {
+	for (slot = pvt->maxdimmperch - 1; slot >= 0; slot--) {
 
-		/* on an odd csrow, first output a 'boundary' marker,
+		/* on an odd slot, first output a 'boundary' marker,
 		 * then reset the message buffer  */
-		if (csrow & 0x1) {
-			n = snprintf(p, space, "---------------------------"
+		if (slot & 0x1) {
+			n = snprintf(p, space, "--------------------------"
 				"--------------------------------");
 			p += n;
 			space -= n;
@@ -1069,30 +1065,39 @@ static void calculate_dimm_size(struct i5000_pvt *pvt)
 			p = mem_buffer;
 			space = PAGE_SIZE;
 		}
-		n = snprintf(p, space, "csrow %2d    ", csrow);
+		n = snprintf(p, space, "slot %2d    ", slot);
 		p += n;
 		space -= n;
 
 		for (channel = 0; channel < pvt->maxch; channel++) {
-			dinfo = &pvt->dimm_info[csrow][channel];
-			handle_channel(pvt, csrow, channel, dinfo);
-			n = snprintf(p, space, "%4d MB   | ", dinfo->megabytes);
+			dinfo = &pvt->dimm_info[slot][channel];
+			handle_channel(pvt, slot, channel, dinfo);
+			if (dinfo->megabytes)
+				n = snprintf(p, space, "%4d MB %dR| ",
+					     dinfo->megabytes, dinfo->dual_rank + 1);
+			else
+				n = snprintf(p, space, "%4d MB   | ", 0);
 			p += n;
 			space -= n;
 		}
-		n = snprintf(p, space, "\n");
 		p += n;
 		space -= n;
+		debugf2("%s\n", mem_buffer);
+		p = mem_buffer;
+		space = PAGE_SIZE;
 	}
 
 	/* Output the last bottom 'boundary' marker */
-	n = snprintf(p, space, "---------------------------"
-		"--------------------------------\n");
+	n = snprintf(p, space, "--------------------------"
+		"--------------------------------");
 	p += n;
 	space -= n;
+	debugf2("%s\n", mem_buffer);
+	p = mem_buffer;
+	space = PAGE_SIZE;
 
 	/* now output the 'channel' labels */
-	n = snprintf(p, space, "            ");
+	n = snprintf(p, space, "           ");
 	p += n;
 	space -= n;
 	for (channel = 0; channel < pvt->maxch; channel++) {
@@ -1100,9 +1105,17 @@ static void calculate_dimm_size(struct i5000_pvt *pvt)
 		p += n;
 		space -= n;
 	}
-	n = snprintf(p, space, "\n");
+	debugf2("%s\n", mem_buffer);
+	p = mem_buffer;
+	space = PAGE_SIZE;
+
+	n = snprintf(p, space, "           ");
 	p += n;
-	space -= n;
+	for (branch = 0; branch < MAX_BRANCHES; branch++) {
+		n = snprintf(p, space, "       branch %d       | ", branch);
+		p += n;
+		space -= n;
+	}
 
 	/* output the last message and free buffer */
 	debugf2("%s\n", mem_buffer);
@@ -1235,13 +1248,13 @@ static void i5000_get_mc_regs(struct mem_ctl_info *mci)
 static int i5000_init_csrows(struct mem_ctl_info *mci)
 {
 	struct i5000_pvt *pvt;
-	struct csrow_info *p_csrow;
+	struct dimm_info *dimm;
 	int empty, channel_count;
 	int max_csrows;
-	int mtr, mtr1;
+	int mtr;
 	int csrow_megs;
 	int channel;
-	int csrow;
+	int slot;
 
 	pvt = mci->pvt_info;
 
@@ -1250,43 +1263,40 @@ static int i5000_init_csrows(struct mem_ctl_info *mci)
 
 	empty = 1;		/* Assume NO memory */
 
-	for (csrow = 0; csrow < max_csrows; csrow++) {
-		p_csrow = &mci->csrows[csrow];
-
-		p_csrow->csrow_idx = csrow;
-
-		/* use branch 0 for the basis */
-		mtr = pvt->b0_mtr[csrow >> 1];
-		mtr1 = pvt->b1_mtr[csrow >> 1];
-
-		/* if no DIMMS on this row, continue */
-		if (!MTR_DIMMS_PRESENT(mtr) && !MTR_DIMMS_PRESENT(mtr1))
-			continue;
+	/*
+	 * FIXME: The memory layout used to map slot/channel into the
+	 * real memory architecture is weird: branch+slot are "csrows"
+	 * and channel is channel. That required an extra array (dimm_info)
+	 * to map the dimms. A good cleanup would be to remove this array,
+	 * and do a loop here with branch, channel, slot
+	 */
+	for (slot = 0; slot < max_csrows; slot++) {
+		for (channel = 0; channel < pvt->maxch; channel++) {
 
-		/* FAKE OUT VALUES, FIXME */
-		p_csrow->first_page = 0 + csrow * 20;
-		p_csrow->last_page = 9 + csrow * 20;
-		p_csrow->page_mask = 0xFFF;
+			mtr = determine_mtr(pvt, slot, channel);
 
-		p_csrow->grain = 8;
+			if (!MTR_DIMMS_PRESENT(mtr))
+				continue;
 
-		csrow_megs = 0;
-		for (channel = 0; channel < pvt->maxch; channel++) {
-			csrow_megs += pvt->dimm_info[csrow][channel].megabytes;
-		}
+			dimm = EDAC_DIMM_PTR(mci->layers, mci->dimms, mci->n_layers,
+				       channel / MAX_BRANCHES,
+				       channel % MAX_BRANCHES, slot);
 
-		p_csrow->nr_pages = csrow_megs << 8;
+			csrow_megs = pvt->dimm_info[slot][channel].megabytes;
+			dimm->grain = 8;
 
-		/* Assume DDR2 for now */
-		p_csrow->mtype = MEM_FB_DDR2;
+			/* Assume DDR2 for now */
+			dimm->mtype = MEM_FB_DDR2;
 
-		/* ask what device type on this row */
-		if (MTR_DRAM_WIDTH(mtr))
-			p_csrow->dtype = DEV_X8;
-		else
-			p_csrow->dtype = DEV_X4;
+			/* ask what device type on this row */
+			if (MTR_DRAM_WIDTH(mtr))
+				dimm->dtype = DEV_X8;
+			else
+				dimm->dtype = DEV_X4;
 
-		p_csrow->edac_mode = EDAC_S8ECD8ED;
+			dimm->edac_mode = EDAC_S8ECD8ED;
+			dimm->nr_pages = csrow_megs << 8;
+		}
 
 		empty = 0;
 	}
@@ -1317,7 +1327,7 @@ static void i5000_enable_error_reporting(struct mem_ctl_info *mci)
 }
 
 /*
- * i5000_get_dimm_and_channel_counts(pdev, &num_csrows, &num_channels)
+ * i5000_get_dimm_and_channel_counts(pdev, &nr_csrows, &num_channels)
  *
  *	ask the device how many channels are present and how many CSROWS
  *	 as well
@@ -1332,7 +1342,7 @@ static void i5000_get_dimm_and_channel_counts(struct pci_dev *pdev,
 	 * supported on this memory controller
 	 */
 	pci_read_config_byte(pdev, MAXDIMMPERCH, &value);
-	*num_dimms_per_channel = (int)value *2;
+	*num_dimms_per_channel = (int)value;
 
 	pci_read_config_byte(pdev, MAXCH, &value);
 	*num_channels = (int)value;
@@ -1348,10 +1358,10 @@ static void i5000_get_dimm_and_channel_counts(struct pci_dev *pdev,
 static int i5000_probe1(struct pci_dev *pdev, int dev_idx)
 {
 	struct mem_ctl_info *mci;
+	struct edac_mc_layer layers[3];
 	struct i5000_pvt *pvt;
 	int num_channels;
 	int num_dimms_per_channel;
-	int num_csrows;
 
 	debugf0("MC: %s: %s(), pdev bus %u dev=0x%x fn=0x%x\n",
 		__FILE__, __func__,
@@ -1377,14 +1387,22 @@ static int i5000_probe1(struct pci_dev *pdev, int dev_idx)
 	 */
 	i5000_get_dimm_and_channel_counts(pdev, &num_dimms_per_channel,
 					&num_channels);
-	num_csrows = num_dimms_per_channel * 2;
 
-	debugf0("MC: %s(): Number of - Channels= %d  DIMMS= %d  CSROWS= %d\n",
-		__func__, num_channels, num_dimms_per_channel, num_csrows);
+	debugf0("MC: %s(): Number of Branches=2 Channels= %d  DIMMS= %d\n",
+		__func__, num_channels, num_dimms_per_channel);
 
 	/* allocate a new MC control structure */
-	mci = edac_mc_alloc(sizeof(*pvt), num_csrows, num_channels, 0);
 
+	layers[0].type = EDAC_MC_LAYER_BRANCH;
+	layers[0].size = MAX_BRANCHES;
+	layers[0].is_virt_csrow = false;
+	layers[1].type = EDAC_MC_LAYER_CHANNEL;
+	layers[1].size = num_channels / MAX_BRANCHES;
+	layers[1].is_virt_csrow = false;
+	layers[2].type = EDAC_MC_LAYER_SLOT;
+	layers[2].size = num_dimms_per_channel;
+	layers[2].is_virt_csrow = true;
+	mci = edac_mc_alloc(0, ARRAY_SIZE(layers), layers, sizeof(*pvt));
 	if (mci == NULL)
 		return -ENOMEM;
 
diff --git a/drivers/edac/i5100_edac.c b/drivers/edac/i5100_edac.c
index d500749464e..e9e7c2a29dc 100644
--- a/drivers/edac/i5100_edac.c
+++ b/drivers/edac/i5100_edac.c
@@ -14,6 +14,11 @@
  * rows for each respective channel are laid out one after another,
  * the first half belonging to channel 0, the second half belonging
  * to channel 1.
+ *
+ * This driver is for DDR2 DIMMs, and it uses chip select to select among the
+ * several ranks. However, instead of showing memories as ranks, it outputs
+ * them as DIMM's. An internal table creates the association between ranks
+ * and DIMM's.
  */
 #include <linux/module.h>
 #include <linux/init.h>
@@ -410,14 +415,6 @@ static int i5100_csrow_to_chan(const struct mem_ctl_info *mci, int csrow)
 	return csrow / priv->ranksperchan;
 }
 
-static unsigned i5100_rank_to_csrow(const struct mem_ctl_info *mci,
-				    int chan, int rank)
-{
-	const struct i5100_priv *priv = mci->pvt_info;
-
-	return chan * priv->ranksperchan + rank;
-}
-
 static void i5100_handle_ce(struct mem_ctl_info *mci,
 			    int chan,
 			    unsigned bank,
@@ -427,17 +424,17 @@ static void i5100_handle_ce(struct mem_ctl_info *mci,
 			    unsigned ras,
 			    const char *msg)
 {
-	const int csrow = i5100_rank_to_csrow(mci, chan, rank);
+	char detail[80];
 
-	printk(KERN_ERR
-		"CE chan %d, bank %u, rank %u, syndrome 0x%lx, "
-		"cas %u, ras %u, csrow %u, label \"%s\": %s\n",
-		chan, bank, rank, syndrome, cas, ras,
-		csrow, mci->csrows[csrow].channels[0].label, msg);
+	/* Form out message */
+	snprintf(detail, sizeof(detail),
+		 "bank %u, cas %u, ras %u\n",
+		 bank, cas, ras);
 
-	mci->ce_count++;
-	mci->csrows[csrow].ce_count++;
-	mci->csrows[csrow].channels[0].ce_count++;
+	edac_mc_handle_error(HW_EVENT_ERR_CORRECTED, mci,
+			     0, 0, syndrome,
+			     chan, rank, -1,
+			     msg, detail, NULL);
 }
 
 static void i5100_handle_ue(struct mem_ctl_info *mci,
@@ -449,16 +446,17 @@ static void i5100_handle_ue(struct mem_ctl_info *mci,
 			    unsigned ras,
 			    const char *msg)
 {
-	const int csrow = i5100_rank_to_csrow(mci, chan, rank);
+	char detail[80];
 
-	printk(KERN_ERR
-		"UE chan %d, bank %u, rank %u, syndrome 0x%lx, "
-		"cas %u, ras %u, csrow %u, label \"%s\": %s\n",
-		chan, bank, rank, syndrome, cas, ras,
-		csrow, mci->csrows[csrow].channels[0].label, msg);
+	/* Form out message */
+	snprintf(detail, sizeof(detail),
+		 "bank %u, cas %u, ras %u\n",
+		 bank, cas, ras);
 
-	mci->ue_count++;
-	mci->csrows[csrow].ue_count++;
+	edac_mc_handle_error(HW_EVENT_ERR_UNCORRECTED, mci,
+			     0, 0, syndrome,
+			     chan, rank, -1,
+			     msg, detail, NULL);
 }
 
 static void i5100_read_log(struct mem_ctl_info *mci, int chan,
@@ -835,10 +833,10 @@ static void __devinit i5100_init_interleaving(struct pci_dev *pdev,
 static void __devinit i5100_init_csrows(struct mem_ctl_info *mci)
 {
 	int i;
-	unsigned long total_pages = 0UL;
 	struct i5100_priv *priv = mci->pvt_info;
 
-	for (i = 0; i < mci->nr_csrows; i++) {
+	for (i = 0; i < mci->tot_dimms; i++) {
+		struct dimm_info *dimm;
 		const unsigned long npages = i5100_npages(mci, i);
 		const unsigned chan = i5100_csrow_to_chan(mci, i);
 		const unsigned rank = i5100_csrow_to_rank(mci, i);
@@ -846,33 +844,23 @@ static void __devinit i5100_init_csrows(struct mem_ctl_info *mci)
 		if (!npages)
 			continue;
 
-		/*
-		 * FIXME: these two are totally bogus -- I don't see how to
-		 * map them correctly to this structure...
-		 */
-		mci->csrows[i].first_page = total_pages;
-		mci->csrows[i].last_page = total_pages + npages - 1;
-		mci->csrows[i].page_mask = 0UL;
-
-		mci->csrows[i].nr_pages = npages;
-		mci->csrows[i].grain = 32;
-		mci->csrows[i].csrow_idx = i;
-		mci->csrows[i].dtype =
-			(priv->mtr[chan][rank].width == 4) ? DEV_X4 : DEV_X8;
-		mci->csrows[i].ue_count = 0;
-		mci->csrows[i].ce_count = 0;
-		mci->csrows[i].mtype = MEM_RDDR2;
-		mci->csrows[i].edac_mode = EDAC_SECDED;
-		mci->csrows[i].mci = mci;
-		mci->csrows[i].nr_channels = 1;
-		mci->csrows[i].channels[0].chan_idx = 0;
-		mci->csrows[i].channels[0].ce_count = 0;
-		mci->csrows[i].channels[0].csrow = mci->csrows + i;
-		snprintf(mci->csrows[i].channels[0].label,
-			 sizeof(mci->csrows[i].channels[0].label),
-			 "DIMM%u", i5100_rank_to_slot(mci, chan, rank));
-
-		total_pages += npages;
+		dimm = EDAC_DIMM_PTR(mci->layers, mci->dimms, mci->n_layers,
+			       chan, rank, 0);
+
+		dimm->nr_pages = npages;
+		if (npages) {
+			dimm->grain = 32;
+			dimm->dtype = (priv->mtr[chan][rank].width == 4) ?
+					DEV_X4 : DEV_X8;
+			dimm->mtype = MEM_RDDR2;
+			dimm->edac_mode = EDAC_SECDED;
+			snprintf(dimm->label, sizeof(dimm->label),
+				"DIMM%u",
+				i5100_rank_to_slot(mci, chan, rank));
+		}
+
+		debugf2("dimm channel %d, rank %d, size %ld\n",
+			chan, rank, (long)PAGES_TO_MiB(npages));
 	}
 }
 
@@ -881,6 +869,7 @@ static int __devinit i5100_init_one(struct pci_dev *pdev,
 {
 	int rc;
 	struct mem_ctl_info *mci;
+	struct edac_mc_layer layers[2];
 	struct i5100_priv *priv;
 	struct pci_dev *ch0mm, *ch1mm;
 	int ret = 0;
@@ -941,7 +930,14 @@ static int __devinit i5100_init_one(struct pci_dev *pdev,
 		goto bail_ch1;
 	}
 
-	mci = edac_mc_alloc(sizeof(*priv), ranksperch * 2, 1, 0);
+	layers[0].type = EDAC_MC_LAYER_CHANNEL;
+	layers[0].size = 2;
+	layers[0].is_virt_csrow = false;
+	layers[1].type = EDAC_MC_LAYER_SLOT;
+	layers[1].size = ranksperch;
+	layers[1].is_virt_csrow = true;
+	mci = edac_mc_alloc(0, ARRAY_SIZE(layers), layers,
+			    sizeof(*priv));
 	if (!mci) {
 		ret = -ENOMEM;
 		goto bail_disable_ch1;
diff --git a/drivers/edac/i5400_edac.c b/drivers/edac/i5400_edac.c
index 1869a1018fb..6640c29e188 100644
--- a/drivers/edac/i5400_edac.c
+++ b/drivers/edac/i5400_edac.c
@@ -18,6 +18,10 @@
  * Intel 5400 Chipset Memory Controller Hub (MCH) - Datasheet
  * 	http://developer.intel.com/design/chipsets/datashts/313070.htm
  *
+ * This Memory Controller manages DDR2 FB-DIMMs. It has 2 branches, each with
+ * 2 channels operating in lockstep no-mirror mode. Each channel can have up to
+ * 4 dimm's, each with up to 8GB.
+ *
  */
 
 #include <linux/module.h>
@@ -44,12 +48,10 @@
 	edac_mc_chipset_printk(mci, level, "i5400", fmt, ##arg)
 
 /* Limits for i5400 */
-#define NUM_MTRS_PER_BRANCH	4
+#define MAX_BRANCHES		2
 #define CHANNELS_PER_BRANCH	2
-#define MAX_DIMMS_PER_CHANNEL	NUM_MTRS_PER_BRANCH
-#define	MAX_CHANNELS		4
-/* max possible csrows per channel */
-#define MAX_CSROWS		(MAX_DIMMS_PER_CHANNEL)
+#define DIMMS_PER_CHANNEL	4
+#define	MAX_CHANNELS		(MAX_BRANCHES * CHANNELS_PER_BRANCH)
 
 /* Device 16,
  * Function 0: System Address
@@ -347,16 +349,16 @@ struct i5400_pvt {
 
 	u16 mir0, mir1;
 
-	u16 b0_mtr[NUM_MTRS_PER_BRANCH];	/* Memory Technlogy Reg */
+	u16 b0_mtr[DIMMS_PER_CHANNEL];	/* Memory Technlogy Reg */
 	u16 b0_ambpresent0;			/* Branch 0, Channel 0 */
 	u16 b0_ambpresent1;			/* Brnach 0, Channel 1 */
 
-	u16 b1_mtr[NUM_MTRS_PER_BRANCH];	/* Memory Technlogy Reg */
+	u16 b1_mtr[DIMMS_PER_CHANNEL];	/* Memory Technlogy Reg */
 	u16 b1_ambpresent0;			/* Branch 1, Channel 8 */
 	u16 b1_ambpresent1;			/* Branch 1, Channel 1 */
 
 	/* DIMM information matrix, allocating architecture maximums */
-	struct i5400_dimm_info dimm_info[MAX_CSROWS][MAX_CHANNELS];
+	struct i5400_dimm_info dimm_info[DIMMS_PER_CHANNEL][MAX_CHANNELS];
 
 	/* Actual values for this controller */
 	int maxch;				/* Max channels */
@@ -532,13 +534,15 @@ static void i5400_proccess_non_recoverable_info(struct mem_ctl_info *mci,
 	int ras, cas;
 	int errnum;
 	char *type = NULL;
+	enum hw_event_mc_err_type tp_event = HW_EVENT_ERR_UNCORRECTED;
 
 	if (!allErrors)
 		return;		/* if no error, return now */
 
-	if (allErrors &  ERROR_FAT_MASK)
+	if (allErrors &  ERROR_FAT_MASK) {
 		type = "FATAL";
-	else if (allErrors & FERR_NF_UNCORRECTABLE)
+		tp_event = HW_EVENT_ERR_FATAL;
+	} else if (allErrors & FERR_NF_UNCORRECTABLE)
 		type = "NON-FATAL uncorrected";
 	else
 		type = "NON-FATAL recoverable";
@@ -556,7 +560,7 @@ static void i5400_proccess_non_recoverable_info(struct mem_ctl_info *mci,
 	ras = nrec_ras(info);
 	cas = nrec_cas(info);
 
-	debugf0("\t\tCSROW= %d  Channels= %d,%d  (Branch= %d "
+	debugf0("\t\tDIMM= %d  Channels= %d,%d  (Branch= %d "
 		"DRAM Bank= %d Buffer ID = %d rdwr= %s ras= %d cas= %d)\n",
 		rank, channel, channel + 1, branch >> 1, bank,
 		buf_id, rdwr_str(rdwr), ras, cas);
@@ -566,13 +570,13 @@ static void i5400_proccess_non_recoverable_info(struct mem_ctl_info *mci,
 
 	/* Form out message */
 	snprintf(msg, sizeof(msg),
-		 "%s (Branch=%d DRAM-Bank=%d Buffer ID = %d RDWR=%s "
-		 "RAS=%d CAS=%d %s Err=0x%lx (%s))",
-		 type, branch >> 1, bank, buf_id, rdwr_str(rdwr), ras, cas,
-		 type, allErrors, error_name[errnum]);
+		 "Bank=%d Buffer ID = %d RAS=%d CAS=%d Err=0x%lx (%s)",
+		 bank, buf_id, ras, cas, allErrors, error_name[errnum]);
 
-	/* Call the helper to output message */
-	edac_mc_handle_fbd_ue(mci, rank, channel, channel + 1, msg);
+	edac_mc_handle_error(tp_event, mci, 0, 0, 0,
+			     branch >> 1, -1, rank,
+			     rdwr ? "Write error" : "Read error",
+			     msg, NULL);
 }
 
 /*
@@ -630,7 +634,7 @@ static void i5400_process_nonfatal_error_info(struct mem_ctl_info *mci,
 		/* Only 1 bit will be on */
 		errnum = find_first_bit(&allErrors, ARRAY_SIZE(error_name));
 
-		debugf0("\t\tCSROW= %d Channel= %d  (Branch %d "
+		debugf0("\t\tDIMM= %d Channel= %d  (Branch %d "
 			"DRAM Bank= %d rdwr= %s ras= %d cas= %d)\n",
 			rank, channel, branch >> 1, bank,
 			rdwr_str(rdwr), ras, cas);
@@ -642,8 +646,10 @@ static void i5400_process_nonfatal_error_info(struct mem_ctl_info *mci,
 			 branch >> 1, bank, rdwr_str(rdwr), ras, cas,
 			 allErrors, error_name[errnum]);
 
-		/* Call the helper to output message */
-		edac_mc_handle_fbd_ce(mci, rank, channel, msg);
+		edac_mc_handle_error(HW_EVENT_ERR_CORRECTED, mci, 0, 0, 0,
+				     branch >> 1, channel % 2, rank,
+				     rdwr ? "Write error" : "Read error",
+				     msg, NULL);
 
 		return;
 	}
@@ -831,8 +837,8 @@ static int i5400_get_devices(struct mem_ctl_info *mci, int dev_idx)
 /*
  *	determine_amb_present
  *
- *		the information is contained in NUM_MTRS_PER_BRANCH different
- *		registers determining which of the NUM_MTRS_PER_BRANCH requires
+ *		the information is contained in DIMMS_PER_CHANNEL different
+ *		registers determining which of the DIMMS_PER_CHANNEL requires
  *              knowing which channel is in question
  *
  *	2 branches, each with 2 channels
@@ -861,11 +867,11 @@ static int determine_amb_present_reg(struct i5400_pvt *pvt, int channel)
 }
 
 /*
- * determine_mtr(pvt, csrow, channel)
+ * determine_mtr(pvt, dimm, channel)
  *
- * return the proper MTR register as determine by the csrow and desired channel
+ * return the proper MTR register as determine by the dimm and desired channel
  */
-static int determine_mtr(struct i5400_pvt *pvt, int csrow, int channel)
+static int determine_mtr(struct i5400_pvt *pvt, int dimm, int channel)
 {
 	int mtr;
 	int n;
@@ -873,11 +879,11 @@ static int determine_mtr(struct i5400_pvt *pvt, int csrow, int channel)
 	/* There is one MTR for each slot pair of FB-DIMMs,
 	   Each slot pair may be at branch 0 or branch 1.
 	 */
-	n = csrow;
+	n = dimm;
 
-	if (n >= NUM_MTRS_PER_BRANCH) {
-		debugf0("ERROR: trying to access an invalid csrow: %d\n",
-			csrow);
+	if (n >= DIMMS_PER_CHANNEL) {
+		debugf0("ERROR: trying to access an invalid dimm: %d\n",
+			dimm);
 		return 0;
 	}
 
@@ -913,19 +919,19 @@ static void decode_mtr(int slot_row, u16 mtr)
 	debugf2("\t\tNUMCOL: %s\n", numcol_toString[MTR_DIMM_COLS(mtr)]);
 }
 
-static void handle_channel(struct i5400_pvt *pvt, int csrow, int channel,
+static void handle_channel(struct i5400_pvt *pvt, int dimm, int channel,
 			struct i5400_dimm_info *dinfo)
 {
 	int mtr;
 	int amb_present_reg;
 	int addrBits;
 
-	mtr = determine_mtr(pvt, csrow, channel);
+	mtr = determine_mtr(pvt, dimm, channel);
 	if (MTR_DIMMS_PRESENT(mtr)) {
 		amb_present_reg = determine_amb_present_reg(pvt, channel);
 
 		/* Determine if there is a DIMM present in this DIMM slot */
-		if (amb_present_reg & (1 << csrow)) {
+		if (amb_present_reg & (1 << dimm)) {
 			/* Start with the number of bits for a Bank
 			 * on the DRAM */
 			addrBits = MTR_DRAM_BANKS_ADDR_BITS(mtr);
@@ -954,10 +960,10 @@ static void handle_channel(struct i5400_pvt *pvt, int csrow, int channel,
 static void calculate_dimm_size(struct i5400_pvt *pvt)
 {
 	struct i5400_dimm_info *dinfo;
-	int csrow, max_csrows;
+	int dimm, max_dimms;
 	char *p, *mem_buffer;
 	int space, n;
-	int channel;
+	int channel, branch;
 
 	/* ================= Generate some debug output ================= */
 	space = PAGE_SIZE;
@@ -968,32 +974,32 @@ static void calculate_dimm_size(struct i5400_pvt *pvt)
 		return;
 	}
 
-	/* Scan all the actual CSROWS
+	/* Scan all the actual DIMMS
 	 * and calculate the information for each DIMM
-	 * Start with the highest csrow first, to display it first
-	 * and work toward the 0th csrow
+	 * Start with the highest dimm first, to display it first
+	 * and work toward the 0th dimm
 	 */
-	max_csrows = pvt->maxdimmperch;
-	for (csrow = max_csrows - 1; csrow >= 0; csrow--) {
+	max_dimms = pvt->maxdimmperch;
+	for (dimm = max_dimms - 1; dimm >= 0; dimm--) {
 
-		/* on an odd csrow, first output a 'boundary' marker,
+		/* on an odd dimm, first output a 'boundary' marker,
 		 * then reset the message buffer  */
-		if (csrow & 0x1) {
+		if (dimm & 0x1) {
 			n = snprintf(p, space, "---------------------------"
-					"--------------------------------");
+					"-------------------------------");
 			p += n;
 			space -= n;
 			debugf2("%s\n", mem_buffer);
 			p = mem_buffer;
 			space = PAGE_SIZE;
 		}
-		n = snprintf(p, space, "csrow %2d    ", csrow);
+		n = snprintf(p, space, "dimm %2d    ", dimm);
 		p += n;
 		space -= n;
 
 		for (channel = 0; channel < pvt->maxch; channel++) {
-			dinfo = &pvt->dimm_info[csrow][channel];
-			handle_channel(pvt, csrow, channel, dinfo);
+			dinfo = &pvt->dimm_info[dimm][channel];
+			handle_channel(pvt, dimm, channel, dinfo);
 			n = snprintf(p, space, "%4d MB   | ", dinfo->megabytes);
 			p += n;
 			space -= n;
@@ -1005,7 +1011,7 @@ static void calculate_dimm_size(struct i5400_pvt *pvt)
 
 	/* Output the last bottom 'boundary' marker */
 	n = snprintf(p, space, "---------------------------"
-			"--------------------------------");
+			"-------------------------------");
 	p += n;
 	space -= n;
 	debugf2("%s\n", mem_buffer);
@@ -1013,7 +1019,7 @@ static void calculate_dimm_size(struct i5400_pvt *pvt)
 	space = PAGE_SIZE;
 
 	/* now output the 'channel' labels */
-	n = snprintf(p, space, "            ");
+	n = snprintf(p, space, "           ");
 	p += n;
 	space -= n;
 	for (channel = 0; channel < pvt->maxch; channel++) {
@@ -1022,6 +1028,19 @@ static void calculate_dimm_size(struct i5400_pvt *pvt)
 		space -= n;
 	}
 
+	space -= n;
+	debugf2("%s\n", mem_buffer);
+	p = mem_buffer;
+	space = PAGE_SIZE;
+
+	n = snprintf(p, space, "           ");
+	p += n;
+	for (branch = 0; branch < MAX_BRANCHES; branch++) {
+		n = snprintf(p, space, "       branch %d       | ", branch);
+		p += n;
+		space -= n;
+	}
+
 	/* output the last message and free buffer */
 	debugf2("%s\n", mem_buffer);
 	kfree(mem_buffer);
@@ -1080,7 +1099,7 @@ static void i5400_get_mc_regs(struct mem_ctl_info *mci)
 	debugf2("MIR1: limit= 0x%x  WAY1= %u  WAY0= %x\n", limit, way1, way0);
 
 	/* Get the set of MTR[0-3] regs by each branch */
-	for (slot_row = 0; slot_row < NUM_MTRS_PER_BRANCH; slot_row++) {
+	for (slot_row = 0; slot_row < DIMMS_PER_CHANNEL; slot_row++) {
 		int where = MTR0 + (slot_row * sizeof(u16));
 
 		/* Branch 0 set of MTR registers */
@@ -1105,7 +1124,7 @@ static void i5400_get_mc_regs(struct mem_ctl_info *mci)
 	/* Read and dump branch 0's MTRs */
 	debugf2("\nMemory Technology Registers:\n");
 	debugf2("   Branch 0:\n");
-	for (slot_row = 0; slot_row < NUM_MTRS_PER_BRANCH; slot_row++)
+	for (slot_row = 0; slot_row < DIMMS_PER_CHANNEL; slot_row++)
 		decode_mtr(slot_row, pvt->b0_mtr[slot_row]);
 
 	pci_read_config_word(pvt->branch_0, AMBPRESENT_0,
@@ -1122,7 +1141,7 @@ static void i5400_get_mc_regs(struct mem_ctl_info *mci)
 	} else {
 		/* Read and dump  branch 1's MTRs */
 		debugf2("   Branch 1:\n");
-		for (slot_row = 0; slot_row < NUM_MTRS_PER_BRANCH; slot_row++)
+		for (slot_row = 0; slot_row < DIMMS_PER_CHANNEL; slot_row++)
 			decode_mtr(slot_row, pvt->b1_mtr[slot_row]);
 
 		pci_read_config_word(pvt->branch_1, AMBPRESENT_0,
@@ -1141,7 +1160,7 @@ static void i5400_get_mc_regs(struct mem_ctl_info *mci)
 }
 
 /*
- *	i5400_init_csrows	Initialize the 'csrows' table within
+ *	i5400_init_dimms	Initialize the 'dimms' table within
  *				the mci control	structure with the
  *				addressing of memory.
  *
@@ -1149,64 +1168,68 @@ static void i5400_get_mc_regs(struct mem_ctl_info *mci)
  *		0	success
  *		1	no actual memory found on this MC
  */
-static int i5400_init_csrows(struct mem_ctl_info *mci)
+static int i5400_init_dimms(struct mem_ctl_info *mci)
 {
 	struct i5400_pvt *pvt;
-	struct csrow_info *p_csrow;
-	int empty, channel_count;
-	int max_csrows;
+	struct dimm_info *dimm;
+	int ndimms, channel_count;
+	int max_dimms;
 	int mtr;
-	int csrow_megs;
-	int channel;
-	int csrow;
+	int size_mb;
+	int  channel, slot;
 
 	pvt = mci->pvt_info;
 
 	channel_count = pvt->maxch;
-	max_csrows = pvt->maxdimmperch;
+	max_dimms = pvt->maxdimmperch;
 
-	empty = 1;		/* Assume NO memory */
+	ndimms = 0;
 
-	for (csrow = 0; csrow < max_csrows; csrow++) {
-		p_csrow = &mci->csrows[csrow];
-
-		p_csrow->csrow_idx = csrow;
-
-		/* use branch 0 for the basis */
-		mtr = determine_mtr(pvt, csrow, 0);
-
-		/* if no DIMMS on this row, continue */
-		if (!MTR_DIMMS_PRESENT(mtr))
-			continue;
-
-		/* FAKE OUT VALUES, FIXME */
-		p_csrow->first_page = 0 + csrow * 20;
-		p_csrow->last_page = 9 + csrow * 20;
-		p_csrow->page_mask = 0xFFF;
-
-		p_csrow->grain = 8;
-
-		csrow_megs = 0;
-		for (channel = 0; channel < pvt->maxch; channel++)
-			csrow_megs += pvt->dimm_info[csrow][channel].megabytes;
-
-		p_csrow->nr_pages = csrow_megs << 8;
-
-		/* Assume DDR2 for now */
-		p_csrow->mtype = MEM_FB_DDR2;
-
-		/* ask what device type on this row */
-		if (MTR_DRAM_WIDTH(mtr))
-			p_csrow->dtype = DEV_X8;
-		else
-			p_csrow->dtype = DEV_X4;
-
-		p_csrow->edac_mode = EDAC_S8ECD8ED;
-
-		empty = 0;
+	/*
+	 * FIXME: remove  pvt->dimm_info[slot][channel] and use the 3
+	 * layers here.
+	 */
+	for (channel = 0; channel < mci->layers[0].size * mci->layers[1].size;
+	     channel++) {
+		for (slot = 0; slot < mci->layers[2].size; slot++) {
+			mtr = determine_mtr(pvt, slot, channel);
+
+			/* if no DIMMS on this slot, continue */
+			if (!MTR_DIMMS_PRESENT(mtr))
+				continue;
+
+			dimm = EDAC_DIMM_PTR(mci->layers, mci->dimms, mci->n_layers,
+				       channel / 2, channel % 2, slot);
+
+			size_mb =  pvt->dimm_info[slot][channel].megabytes;
+
+			debugf2("%s: dimm%zd (branch %d channel %d slot %d): %d.%03d GB\n",
+				__func__, dimm - mci->dimms,
+				channel / 2, channel % 2, slot,
+				size_mb / 1000, size_mb % 1000);
+
+			dimm->nr_pages = size_mb << 8;
+			dimm->grain = 8;
+			dimm->dtype = MTR_DRAM_WIDTH(mtr) ? DEV_X8 : DEV_X4;
+			dimm->mtype = MEM_FB_DDR2;
+			/*
+			 * The eccc mechanism is SDDC (aka SECC), with
+			 * is similar to Chipkill.
+			 */
+			dimm->edac_mode = MTR_DRAM_WIDTH(mtr) ?
+					  EDAC_S8ECD8ED : EDAC_S4ECD4ED;
+			ndimms++;
+		}
 	}
 
-	return empty;
+	/*
+	 * When just one memory is provided, it should be at location (0,0,0).
+	 * With such single-DIMM mode, the SDCC algorithm degrades to SECDEC+.
+	 */
+	if (ndimms == 1)
+		mci->dimms[0].edac_mode = EDAC_SECDED;
+
+	return (ndimms == 0);
 }
 
 /*
@@ -1242,9 +1265,7 @@ static int i5400_probe1(struct pci_dev *pdev, int dev_idx)
 {
 	struct mem_ctl_info *mci;
 	struct i5400_pvt *pvt;
-	int num_channels;
-	int num_dimms_per_channel;
-	int num_csrows;
+	struct edac_mc_layer layers[3];
 
 	if (dev_idx >= ARRAY_SIZE(i5400_devs))
 		return -EINVAL;
@@ -1258,23 +1279,21 @@ static int i5400_probe1(struct pci_dev *pdev, int dev_idx)
 	if (PCI_FUNC(pdev->devfn) != 0)
 		return -ENODEV;
 
-	/* As we don't have a motherboard identification routine to determine
-	 * actual number of slots/dimms per channel, we thus utilize the
-	 * resource as specified by the chipset. Thus, we might have
-	 * have more DIMMs per channel than actually on the mobo, but this
-	 * allows the driver to support up to the chipset max, without
-	 * some fancy mobo determination.
+	/*
+	 * allocate a new MC control structure
+	 *
+	 * This drivers uses the DIMM slot as "csrow" and the rest as "channel".
 	 */
-	num_dimms_per_channel = MAX_DIMMS_PER_CHANNEL;
-	num_channels = MAX_CHANNELS;
-	num_csrows = num_dimms_per_channel;
-
-	debugf0("MC: %s(): Number of - Channels= %d  DIMMS= %d  CSROWS= %d\n",
-		__func__, num_channels, num_dimms_per_channel, num_csrows);
-
-	/* allocate a new MC control structure */
-	mci = edac_mc_alloc(sizeof(*pvt), num_csrows, num_channels, 0);
-
+	layers[0].type = EDAC_MC_LAYER_BRANCH;
+	layers[0].size = MAX_BRANCHES;
+	layers[0].is_virt_csrow = false;
+	layers[1].type = EDAC_MC_LAYER_CHANNEL;
+	layers[1].size = CHANNELS_PER_BRANCH;
+	layers[1].is_virt_csrow = false;
+	layers[2].type = EDAC_MC_LAYER_SLOT;
+	layers[2].size = DIMMS_PER_CHANNEL;
+	layers[2].is_virt_csrow = true;
+	mci = edac_mc_alloc(0, ARRAY_SIZE(layers), layers, sizeof(*pvt));
 	if (mci == NULL)
 		return -ENOMEM;
 
@@ -1284,8 +1303,8 @@ static int i5400_probe1(struct pci_dev *pdev, int dev_idx)
 
 	pvt = mci->pvt_info;
 	pvt->system_address = pdev;	/* Record this device in our private */
-	pvt->maxch = num_channels;
-	pvt->maxdimmperch = num_dimms_per_channel;
+	pvt->maxch = MAX_CHANNELS;
+	pvt->maxdimmperch = DIMMS_PER_CHANNEL;
 
 	/* 'get' the pci devices we want to reserve for our use */
 	if (i5400_get_devices(mci, dev_idx))
@@ -1307,13 +1326,13 @@ static int i5400_probe1(struct pci_dev *pdev, int dev_idx)
 	/* Set the function pointer to an actual operation function */
 	mci->edac_check = i5400_check_error;
 
-	/* initialize the MC control structure 'csrows' table
+	/* initialize the MC control structure 'dimms' table
 	 * with the mapping and control information */
-	if (i5400_init_csrows(mci)) {
+	if (i5400_init_dimms(mci)) {
 		debugf0("MC: Setting mci->edac_cap to EDAC_FLAG_NONE\n"
-			"    because i5400_init_csrows() returned nonzero "
+			"    because i5400_init_dimms() returned nonzero "
 			"value\n");
-		mci->edac_cap = EDAC_FLAG_NONE;	/* no csrows found */
+		mci->edac_cap = EDAC_FLAG_NONE;	/* no dimms found */
 	} else {
 		debugf1("MC: Enable error reporting now\n");
 		i5400_enable_error_reporting(mci);
diff --git a/drivers/edac/i7300_edac.c b/drivers/edac/i7300_edac.c
index 3bafa3bca14..97c22fd650e 100644
--- a/drivers/edac/i7300_edac.c
+++ b/drivers/edac/i7300_edac.c
@@ -464,17 +464,14 @@ static void i7300_process_fbd_error(struct mem_ctl_info *mci)
 				FERR_FAT_FBD, error_reg);
 
 		snprintf(pvt->tmp_prt_buffer, PAGE_SIZE,
-			"FATAL (Branch=%d DRAM-Bank=%d %s "
-			"RAS=%d CAS=%d Err=0x%lx (%s))",
-			branch, bank,
-			is_wr ? "RDWR" : "RD",
-			ras, cas,
-			errors, specific);
-
-		/* Call the helper to output message */
-		edac_mc_handle_fbd_ue(mci, rank, branch << 1,
-				      (branch << 1) + 1,
-				      pvt->tmp_prt_buffer);
+			 "Bank=%d RAS=%d CAS=%d Err=0x%lx (%s))",
+			 bank, ras, cas, errors, specific);
+
+		edac_mc_handle_error(HW_EVENT_ERR_FATAL, mci, 0, 0, 0,
+				     branch, -1, rank,
+				     is_wr ? "Write error" : "Read error",
+				     pvt->tmp_prt_buffer, NULL);
+
 	}
 
 	/* read in the 1st NON-FATAL error register */
@@ -513,23 +510,14 @@ static void i7300_process_fbd_error(struct mem_ctl_info *mci)
 
 		/* Form out message */
 		snprintf(pvt->tmp_prt_buffer, PAGE_SIZE,
-			"Corrected error (Branch=%d, Channel %d), "
-			" DRAM-Bank=%d %s "
-			"RAS=%d CAS=%d, CE Err=0x%lx, Syndrome=0x%08x(%s))",
-			branch, channel,
-			bank,
-			is_wr ? "RDWR" : "RD",
-			ras, cas,
-			errors, syndrome, specific);
-
-		/*
-		 * Call the helper to output message
-		 * NOTE: Errors are reported per-branch, and not per-channel
-		 *	 Currently, we don't know how to identify the right
-		 *	 channel.
-		 */
-		edac_mc_handle_fbd_ce(mci, rank, channel,
-				      pvt->tmp_prt_buffer);
+			 "DRAM-Bank=%d RAS=%d CAS=%d, Err=0x%lx (%s))",
+			 bank, ras, cas, errors, specific);
+
+		edac_mc_handle_error(HW_EVENT_ERR_CORRECTED, mci, 0, 0,
+				     syndrome,
+				     branch >> 1, channel % 2, rank,
+				     is_wr ? "Write error" : "Read error",
+				     pvt->tmp_prt_buffer, NULL);
 	}
 	return;
 }
@@ -617,8 +605,7 @@ static void i7300_enable_error_reporting(struct mem_ctl_info *mci)
 static int decode_mtr(struct i7300_pvt *pvt,
 		      int slot, int ch, int branch,
 		      struct i7300_dimm_info *dinfo,
-		      struct csrow_info *p_csrow,
-		      u32 *nr_pages)
+		      struct dimm_info *dimm)
 {
 	int mtr, ans, addrBits, channel;
 
@@ -650,7 +637,6 @@ static int decode_mtr(struct i7300_pvt *pvt,
 	addrBits -= 3;	/* 8 bits per bytes */
 
 	dinfo->megabytes = 1 << addrBits;
-	*nr_pages = dinfo->megabytes << 8;
 
 	debugf2("\t\tWIDTH: x%d\n", MTR_DRAM_WIDTH(mtr));
 
@@ -663,11 +649,6 @@ static int decode_mtr(struct i7300_pvt *pvt,
 	debugf2("\t\tNUMCOL: %s\n", numcol_toString[MTR_DIMM_COLS(mtr)]);
 	debugf2("\t\tSIZE: %d MB\n", dinfo->megabytes);
 
-	p_csrow->grain = 8;
-	p_csrow->mtype = MEM_FB_DDR2;
-	p_csrow->csrow_idx = slot;
-	p_csrow->page_mask = 0;
-
 	/*
 	 * The type of error detection actually depends of the
 	 * mode of operation. When it is just one single memory chip, at
@@ -677,15 +658,18 @@ static int decode_mtr(struct i7300_pvt *pvt,
 	 * See datasheet Sections 7.3.6 to 7.3.8
 	 */
 
+	dimm->nr_pages = MiB_TO_PAGES(dinfo->megabytes);
+	dimm->grain = 8;
+	dimm->mtype = MEM_FB_DDR2;
 	if (IS_SINGLE_MODE(pvt->mc_settings_a)) {
-		p_csrow->edac_mode = EDAC_SECDED;
+		dimm->edac_mode = EDAC_SECDED;
 		debugf2("\t\tECC code is 8-byte-over-32-byte SECDED+ code\n");
 	} else {
 		debugf2("\t\tECC code is on Lockstep mode\n");
 		if (MTR_DRAM_WIDTH(mtr) == 8)
-			p_csrow->edac_mode = EDAC_S8ECD8ED;
+			dimm->edac_mode = EDAC_S8ECD8ED;
 		else
-			p_csrow->edac_mode = EDAC_S4ECD4ED;
+			dimm->edac_mode = EDAC_S4ECD4ED;
 	}
 
 	/* ask what device type on this row */
@@ -694,9 +678,9 @@ static int decode_mtr(struct i7300_pvt *pvt,
 			IS_SCRBALGO_ENHANCED(pvt->mc_settings) ?
 					    "enhanced" : "normal");
 
-		p_csrow->dtype = DEV_X8;
+		dimm->dtype = DEV_X8;
 	} else
-		p_csrow->dtype = DEV_X4;
+		dimm->dtype = DEV_X4;
 
 	return mtr;
 }
@@ -774,11 +758,10 @@ static int i7300_init_csrows(struct mem_ctl_info *mci)
 {
 	struct i7300_pvt *pvt;
 	struct i7300_dimm_info *dinfo;
-	struct csrow_info *p_csrow;
 	int rc = -ENODEV;
 	int mtr;
 	int ch, branch, slot, channel;
-	u32 last_page = 0, nr_pages;
+	struct dimm_info *dimm;
 
 	pvt = mci->pvt_info;
 
@@ -809,25 +792,23 @@ static int i7300_init_csrows(struct mem_ctl_info *mci)
 			pci_read_config_word(pvt->pci_dev_2x_0_fbd_branch[branch],
 					where,
 					&pvt->mtr[slot][branch]);
-			for (ch = 0; ch < MAX_BRANCHES; ch++) {
+			for (ch = 0; ch < MAX_CH_PER_BRANCH; ch++) {
 				int channel = to_channel(ch, branch);
 
+				dimm = EDAC_DIMM_PTR(mci->layers, mci->dimms,
+					       mci->n_layers, branch, ch, slot);
+
 				dinfo = &pvt->dimm_info[slot][channel];
-				p_csrow = &mci->csrows[slot];
 
 				mtr = decode_mtr(pvt, slot, ch, branch,
-						 dinfo, p_csrow, &nr_pages);
+						 dinfo, dimm);
+
 				/* if no DIMMS on this row, continue */
 				if (!MTR_DIMMS_PRESENT(mtr))
 					continue;
 
-				/* Update per_csrow memory count */
-				p_csrow->nr_pages += nr_pages;
-				p_csrow->first_page = last_page;
-				last_page += nr_pages;
-				p_csrow->last_page = last_page;
-
 				rc = 0;
+
 			}
 		}
 	}
@@ -1042,10 +1023,8 @@ static int __devinit i7300_init_one(struct pci_dev *pdev,
 				    const struct pci_device_id *id)
 {
 	struct mem_ctl_info *mci;
+	struct edac_mc_layer layers[3];
 	struct i7300_pvt *pvt;
-	int num_channels;
-	int num_dimms_per_channel;
-	int num_csrows;
 	int rc;
 
 	/* wake up device */
@@ -1062,23 +1041,17 @@ static int __devinit i7300_init_one(struct pci_dev *pdev,
 	if (PCI_FUNC(pdev->devfn) != 0)
 		return -ENODEV;
 
-	/* As we don't have a motherboard identification routine to determine
-	 * actual number of slots/dimms per channel, we thus utilize the
-	 * resource as specified by the chipset. Thus, we might have
-	 * have more DIMMs per channel than actually on the mobo, but this
-	 * allows the driver to support up to the chipset max, without
-	 * some fancy mobo determination.
-	 */
-	num_dimms_per_channel = MAX_SLOTS;
-	num_channels = MAX_CHANNELS;
-	num_csrows = MAX_SLOTS * MAX_CHANNELS;
-
-	debugf0("MC: %s(): Number of - Channels= %d  DIMMS= %d  CSROWS= %d\n",
-		__func__, num_channels, num_dimms_per_channel, num_csrows);
-
 	/* allocate a new MC control structure */
-	mci = edac_mc_alloc(sizeof(*pvt), num_csrows, num_channels, 0);
-
+	layers[0].type = EDAC_MC_LAYER_BRANCH;
+	layers[0].size = MAX_BRANCHES;
+	layers[0].is_virt_csrow = false;
+	layers[1].type = EDAC_MC_LAYER_CHANNEL;
+	layers[1].size = MAX_CH_PER_BRANCH;
+	layers[1].is_virt_csrow = true;
+	layers[2].type = EDAC_MC_LAYER_SLOT;
+	layers[2].size = MAX_SLOTS;
+	layers[2].is_virt_csrow = true;
+	mci = edac_mc_alloc(0, ARRAY_SIZE(layers), layers, sizeof(*pvt));
 	if (mci == NULL)
 		return -ENOMEM;
 
diff --git a/drivers/edac/i7core_edac.c b/drivers/edac/i7core_edac.c
index 85226ccf529..d27778f65a5 100644
--- a/drivers/edac/i7core_edac.c
+++ b/drivers/edac/i7core_edac.c
@@ -90,7 +90,7 @@ MODULE_PARM_DESC(use_pci_fixup, "Enable PCI fixup to seek for hidden devices");
 #define MC_MAX_DOD	0x64
 
 /*
- * OFFSETS for Device 3 Function 4, as inicated on Xeon 5500 datasheet:
+ * OFFSETS for Device 3 Function 4, as indicated on Xeon 5500 datasheet:
  * http://www.arrownac.com/manufacturers/intel/s/nehalem/5500-datasheet-v2.pdf
  */
 
@@ -101,7 +101,7 @@ MODULE_PARM_DESC(use_pci_fixup, "Enable PCI fixup to seek for hidden devices");
   #define DIMM1_COR_ERR(r)			(((r) >> 16) & 0x7fff)
   #define DIMM0_COR_ERR(r)			((r) & 0x7fff)
 
-/* OFFSETS for Device 3 Function 2, as inicated on Xeon 5500 datasheet */
+/* OFFSETS for Device 3 Function 2, as indicated on Xeon 5500 datasheet */
 #define MC_SSRCONTROL		0x48
   #define SSR_MODE_DISABLE	0x00
   #define SSR_MODE_ENABLE	0x01
@@ -221,7 +221,9 @@ struct i7core_inject {
 };
 
 struct i7core_channel {
-	u32		ranks;
+	bool		is_3dimms_present;
+	bool		is_single_4rank;
+	bool		has_4rank;
 	u32		dimms;
 };
 
@@ -257,7 +259,6 @@ struct i7core_pvt {
 	struct i7core_channel	channel[NUM_CHANS];
 
 	int		ce_count_available;
-	int 		csrow_map[NUM_CHANS][MAX_DIMMS];
 
 			/* ECC corrected errors counts per udimm */
 	unsigned long	udimm_ce_count[MAX_DIMMS];
@@ -398,7 +399,7 @@ static DEFINE_PCI_DEVICE_TABLE(i7core_pci_tbl) = {
 };
 
 /****************************************************************************
-			Anciliary status routines
+			Ancillary status routines
  ****************************************************************************/
 
 	/* MC_CONTROL bits */
@@ -492,116 +493,15 @@ static void free_i7core_dev(struct i7core_dev *i7core_dev)
 /****************************************************************************
 			Memory check routines
  ****************************************************************************/
-static struct pci_dev *get_pdev_slot_func(u8 socket, unsigned slot,
-					  unsigned func)
-{
-	struct i7core_dev *i7core_dev = get_i7core_dev(socket);
-	int i;
-
-	if (!i7core_dev)
-		return NULL;
-
-	for (i = 0; i < i7core_dev->n_devs; i++) {
-		if (!i7core_dev->pdev[i])
-			continue;
-
-		if (PCI_SLOT(i7core_dev->pdev[i]->devfn) == slot &&
-		    PCI_FUNC(i7core_dev->pdev[i]->devfn) == func) {
-			return i7core_dev->pdev[i];
-		}
-	}
-
-	return NULL;
-}
-
-/**
- * i7core_get_active_channels() - gets the number of channels and csrows
- * @socket:	Quick Path Interconnect socket
- * @channels:	Number of channels that will be returned
- * @csrows:	Number of csrows found
- *
- * Since EDAC core needs to know in advance the number of available channels
- * and csrows, in order to allocate memory for csrows/channels, it is needed
- * to run two similar steps. At the first step, implemented on this function,
- * it checks the number of csrows/channels present at one socket.
- * this is used in order to properly allocate the size of mci components.
- *
- * It should be noticed that none of the current available datasheets explain
- * or even mention how csrows are seen by the memory controller. So, we need
- * to add a fake description for csrows.
- * So, this driver is attributing one DIMM memory for one csrow.
- */
-static int i7core_get_active_channels(const u8 socket, unsigned *channels,
-				      unsigned *csrows)
-{
-	struct pci_dev *pdev = NULL;
-	int i, j;
-	u32 status, control;
-
-	*channels = 0;
-	*csrows = 0;
-
-	pdev = get_pdev_slot_func(socket, 3, 0);
-	if (!pdev) {
-		i7core_printk(KERN_ERR, "Couldn't find socket %d fn 3.0!!!\n",
-			      socket);
-		return -ENODEV;
-	}
-
-	/* Device 3 function 0 reads */
-	pci_read_config_dword(pdev, MC_STATUS, &status);
-	pci_read_config_dword(pdev, MC_CONTROL, &control);
-
-	for (i = 0; i < NUM_CHANS; i++) {
-		u32 dimm_dod[3];
-		/* Check if the channel is active */
-		if (!(control & (1 << (8 + i))))
-			continue;
-
-		/* Check if the channel is disabled */
-		if (status & (1 << i))
-			continue;
-
-		pdev = get_pdev_slot_func(socket, i + 4, 1);
-		if (!pdev) {
-			i7core_printk(KERN_ERR, "Couldn't find socket %d "
-						"fn %d.%d!!!\n",
-						socket, i + 4, 1);
-			return -ENODEV;
-		}
-		/* Devices 4-6 function 1 */
-		pci_read_config_dword(pdev,
-				MC_DOD_CH_DIMM0, &dimm_dod[0]);
-		pci_read_config_dword(pdev,
-				MC_DOD_CH_DIMM1, &dimm_dod[1]);
-		pci_read_config_dword(pdev,
-				MC_DOD_CH_DIMM2, &dimm_dod[2]);
 
-		(*channels)++;
-
-		for (j = 0; j < 3; j++) {
-			if (!DIMM_PRESENT(dimm_dod[j]))
-				continue;
-			(*csrows)++;
-		}
-	}
-
-	debugf0("Number of active channels on socket %d: %d\n",
-		socket, *channels);
-
-	return 0;
-}
-
-static int get_dimm_config(const struct mem_ctl_info *mci)
+static int get_dimm_config(struct mem_ctl_info *mci)
 {
 	struct i7core_pvt *pvt = mci->pvt_info;
-	struct csrow_info *csr;
 	struct pci_dev *pdev;
 	int i, j;
-	int csrow = 0;
-	unsigned long last_page = 0;
 	enum edac_type mode;
 	enum mem_type mtype;
+	struct dimm_info *dimm;
 
 	/* Get data from the MC register, function 0 */
 	pdev = pvt->pci_mcr[0];
@@ -657,21 +557,20 @@ static int get_dimm_config(const struct mem_ctl_info *mci)
 		pci_read_config_dword(pvt->pci_ch[i][0],
 				MC_CHANNEL_DIMM_INIT_PARAMS, &data);
 
-		pvt->channel[i].ranks = (data & QUAD_RANK_PRESENT) ?
-						4 : 2;
+
+		if (data & THREE_DIMMS_PRESENT)
+			pvt->channel[i].is_3dimms_present = true;
+
+		if (data & SINGLE_QUAD_RANK_PRESENT)
+			pvt->channel[i].is_single_4rank = true;
+
+		if (data & QUAD_RANK_PRESENT)
+			pvt->channel[i].has_4rank = true;
 
 		if (data & REGISTERED_DIMM)
 			mtype = MEM_RDDR3;
 		else
 			mtype = MEM_DDR3;
-#if 0
-		if (data & THREE_DIMMS_PRESENT)
-			pvt->channel[i].dimms = 3;
-		else if (data & SINGLE_QUAD_RANK_PRESENT)
-			pvt->channel[i].dimms = 1;
-		else
-			pvt->channel[i].dimms = 2;
-#endif
 
 		/* Devices 4-6 function 1 */
 		pci_read_config_dword(pvt->pci_ch[i][1],
@@ -682,11 +581,13 @@ static int get_dimm_config(const struct mem_ctl_info *mci)
 				MC_DOD_CH_DIMM2, &dimm_dod[2]);
 
 		debugf0("Ch%d phy rd%d, wr%d (0x%08x): "
-			"%d ranks, %cDIMMs\n",
+			"%s%s%s%cDIMMs\n",
 			i,
 			RDLCH(pvt->info.ch_map, i), WRLCH(pvt->info.ch_map, i),
 			data,
-			pvt->channel[i].ranks,
+			pvt->channel[i].is_3dimms_present ? "3DIMMS " : "",
+			pvt->channel[i].is_3dimms_present ? "SINGLE_4R " : "",
+			pvt->channel[i].has_4rank ? "HAS_4R " : "",
 			(data & REGISTERED_DIMM) ? 'R' : 'U');
 
 		for (j = 0; j < 3; j++) {
@@ -696,6 +597,8 @@ static int get_dimm_config(const struct mem_ctl_info *mci)
 			if (!DIMM_PRESENT(dimm_dod[j]))
 				continue;
 
+			dimm = EDAC_DIMM_PTR(mci->layers, mci->dimms, mci->n_layers,
+				       i, j, 0);
 			banks = numbank(MC_DOD_NUMBANK(dimm_dod[j]));
 			ranks = numrank(MC_DOD_NUMRANK(dimm_dod[j]));
 			rows = numrow(MC_DOD_NUMROW(dimm_dod[j]));
@@ -704,8 +607,6 @@ static int get_dimm_config(const struct mem_ctl_info *mci)
 			/* DDR3 has 8 I/O banks */
 			size = (rows * cols * banks * ranks) >> (20 - 3);
 
-			pvt->channel[i].dimms++;
-
 			debugf0("\tdimm %d %d Mb offset: %x, "
 				"bank: %d, rank: %d, row: %#x, col: %#x\n",
 				j, size,
@@ -714,44 +615,28 @@ static int get_dimm_config(const struct mem_ctl_info *mci)
 
 			npages = MiB_TO_PAGES(size);
 
-			csr = &mci->csrows[csrow];
-			csr->first_page = last_page + 1;
-			last_page += npages;
-			csr->last_page = last_page;
-			csr->nr_pages = npages;
-
-			csr->page_mask = 0;
-			csr->grain = 8;
-			csr->csrow_idx = csrow;
-			csr->nr_channels = 1;
-
-			csr->channels[0].chan_idx = i;
-			csr->channels[0].ce_count = 0;
-
-			pvt->csrow_map[i][j] = csrow;
+			dimm->nr_pages = npages;
 
 			switch (banks) {
 			case 4:
-				csr->dtype = DEV_X4;
+				dimm->dtype = DEV_X4;
 				break;
 			case 8:
-				csr->dtype = DEV_X8;
+				dimm->dtype = DEV_X8;
 				break;
 			case 16:
-				csr->dtype = DEV_X16;
+				dimm->dtype = DEV_X16;
 				break;
 			default:
-				csr->dtype = DEV_UNKNOWN;
+				dimm->dtype = DEV_UNKNOWN;
 			}
 
-			csr->edac_mode = mode;
-			csr->mtype = mtype;
-			snprintf(csr->channels[0].label,
-					sizeof(csr->channels[0].label),
-					"CPU#%uChannel#%u_DIMM#%u",
-					pvt->i7core_dev->socket, i, j);
-
-			csrow++;
+			snprintf(dimm->label, sizeof(dimm->label),
+				 "CPU#%uChannel#%u_DIMM#%u",
+				 pvt->i7core_dev->socket, i, j);
+			dimm->grain = 8;
+			dimm->edac_mode = mode;
+			dimm->mtype = mtype;
 		}
 
 		pci_read_config_dword(pdev, MC_SAG_CH_0, &value[0]);
@@ -1361,7 +1246,7 @@ static int i7core_get_onedevice(struct pci_dev **prev,
 			      dev_descr->dev_id, *prev);
 
 	/*
-	 * On Xeon 55xx, the Intel Quckpath Arch Generic Non-core regs
+	 * On Xeon 55xx, the Intel QuickPath Arch Generic Non-core regs
 	 * is at addr 8086:2c40, instead of 8086:2c41. So, we need
 	 * to probe for the alternate address in case of failure
 	 */
@@ -1567,22 +1452,16 @@ error:
 /****************************************************************************
 			Error check routines
  ****************************************************************************/
-static void i7core_rdimm_update_csrow(struct mem_ctl_info *mci,
+static void i7core_rdimm_update_errcount(struct mem_ctl_info *mci,
 				      const int chan,
 				      const int dimm,
 				      const int add)
 {
-	char *msg;
-	struct i7core_pvt *pvt = mci->pvt_info;
-	int row = pvt->csrow_map[chan][dimm], i;
+	int i;
 
 	for (i = 0; i < add; i++) {
-		msg = kasprintf(GFP_KERNEL, "Corrected error "
-				"(Socket=%d channel=%d dimm=%d)",
-				pvt->i7core_dev->socket, chan, dimm);
-
-		edac_mc_handle_fbd_ce(mci, row, 0, msg);
-		kfree (msg);
+		edac_mc_handle_error(HW_EVENT_ERR_CORRECTED, mci, 0, 0, 0,
+				     chan, dimm, -1, "error", "", NULL);
 	}
 }
 
@@ -1623,11 +1502,11 @@ static void i7core_rdimm_update_ce_count(struct mem_ctl_info *mci,
 
 	/*updated the edac core */
 	if (add0 != 0)
-		i7core_rdimm_update_csrow(mci, chan, 0, add0);
+		i7core_rdimm_update_errcount(mci, chan, 0, add0);
 	if (add1 != 0)
-		i7core_rdimm_update_csrow(mci, chan, 1, add1);
+		i7core_rdimm_update_errcount(mci, chan, 1, add1);
 	if (add2 != 0)
-		i7core_rdimm_update_csrow(mci, chan, 2, add2);
+		i7core_rdimm_update_errcount(mci, chan, 2, add2);
 
 }
 
@@ -1747,20 +1626,30 @@ static void i7core_mce_output_error(struct mem_ctl_info *mci,
 				    const struct mce *m)
 {
 	struct i7core_pvt *pvt = mci->pvt_info;
-	char *type, *optype, *err, *msg;
+	char *type, *optype, *err, msg[80];
+	enum hw_event_mc_err_type tp_event;
 	unsigned long error = m->status & 0x1ff0000l;
+	bool uncorrected_error = m->mcgstatus & 1ll << 61;
+	bool ripv = m->mcgstatus & 1;
 	u32 optypenum = (m->status >> 4) & 0x07;
 	u32 core_err_cnt = (m->status >> 38) & 0x7fff;
 	u32 dimm = (m->misc >> 16) & 0x3;
 	u32 channel = (m->misc >> 18) & 0x3;
 	u32 syndrome = m->misc >> 32;
 	u32 errnum = find_first_bit(&error, 32);
-	int csrow;
 
-	if (m->mcgstatus & 1)
-		type = "FATAL";
-	else
-		type = "NON_FATAL";
+	if (uncorrected_error) {
+		if (ripv) {
+			type = "FATAL";
+			tp_event = HW_EVENT_ERR_FATAL;
+		} else {
+			type = "NON_FATAL";
+			tp_event = HW_EVENT_ERR_UNCORRECTED;
+		}
+	} else {
+		type = "CORRECTED";
+		tp_event = HW_EVENT_ERR_CORRECTED;
+	}
 
 	switch (optypenum) {
 	case 0:
@@ -1815,27 +1704,20 @@ static void i7core_mce_output_error(struct mem_ctl_info *mci,
 		err = "unknown";
 	}
 
-	/* FIXME: should convert addr into bank and rank information */
-	msg = kasprintf(GFP_ATOMIC,
-		"%s (addr = 0x%08llx, cpu=%d, Dimm=%d, Channel=%d, "
-		"syndrome=0x%08x, count=%d, Err=%08llx:%08llx (%s: %s))\n",
-		type, (long long) m->addr, m->cpu, dimm, channel,
-		syndrome, core_err_cnt, (long long)m->status,
-		(long long)m->misc, optype, err);
-
-	debugf0("%s", msg);
-
-	csrow = pvt->csrow_map[channel][dimm];
+	snprintf(msg, sizeof(msg), "count=%d %s", core_err_cnt, optype);
 
-	/* Call the helper to output message */
-	if (m->mcgstatus & 1)
-		edac_mc_handle_fbd_ue(mci, csrow, 0,
-				0 /* FIXME: should be channel here */, msg);
-	else if (!pvt->is_registered)
-		edac_mc_handle_fbd_ce(mci, csrow,
-				0 /* FIXME: should be channel here */, msg);
-
-	kfree(msg);
+	/*
+	 * Call the helper to output message
+	 * FIXME: what to do if core_err_cnt > 1? Currently, it generates
+	 * only one event
+	 */
+	if (uncorrected_error || !pvt->is_registered)
+		edac_mc_handle_error(tp_event, mci,
+				     m->addr >> PAGE_SHIFT,
+				     m->addr & ~PAGE_MASK,
+				     syndrome,
+				     channel, dimm, -1,
+				     err, msg, m);
 }
 
 /*
@@ -2132,7 +2014,7 @@ static int set_sdram_scrub_rate(struct mem_ctl_info *mci, u32 new_bw)
 
 /*
  * get_sdram_scrub_rate		This routine convert current scrub rate value
- *				into byte/sec bandwidth accourding to
+ *				into byte/sec bandwidth according to
  *				SCRUBINTERVAL formula found in datasheet.
  */
 static int get_sdram_scrub_rate(struct mem_ctl_info *mci)
@@ -2252,15 +2134,19 @@ static int i7core_register_mci(struct i7core_dev *i7core_dev)
 {
 	struct mem_ctl_info *mci;
 	struct i7core_pvt *pvt;
-	int rc, channels, csrows;
-
-	/* Check the number of active and not disabled channels */
-	rc = i7core_get_active_channels(i7core_dev->socket, &channels, &csrows);
-	if (unlikely(rc < 0))
-		return rc;
+	int rc;
+	struct edac_mc_layer layers[2];
 
 	/* allocate a new MC control structure */
-	mci = edac_mc_alloc(sizeof(*pvt), csrows, channels, i7core_dev->socket);
+
+	layers[0].type = EDAC_MC_LAYER_CHANNEL;
+	layers[0].size = NUM_CHANS;
+	layers[0].is_virt_csrow = false;
+	layers[1].type = EDAC_MC_LAYER_SLOT;
+	layers[1].size = MAX_DIMMS;
+	layers[1].is_virt_csrow = true;
+	mci = edac_mc_alloc(i7core_dev->socket, ARRAY_SIZE(layers), layers,
+			    sizeof(*pvt));
 	if (unlikely(!mci))
 		return -ENOMEM;
 
diff --git a/drivers/edac/i82443bxgx_edac.c b/drivers/edac/i82443bxgx_edac.c
index 3bf2b2f490e..52072c28a8a 100644
--- a/drivers/edac/i82443bxgx_edac.c
+++ b/drivers/edac/i82443bxgx_edac.c
@@ -12,7 +12,7 @@
  * 440GX fix by Jason Uhlenkott <juhlenko@akamai.com>.
  *
  * Written with reference to 82443BX Host Bridge Datasheet:
- * http://download.intel.com/design/chipsets/datashts/29063301.pdf 
+ * http://download.intel.com/design/chipsets/datashts/29063301.pdf
  * references to this document given in [].
  *
  * This module doesn't support the 440LX, but it may be possible to
@@ -156,19 +156,19 @@ static int i82443bxgx_edacmc_process_error_info(struct mem_ctl_info *mci,
 	if (info->eap & I82443BXGX_EAP_OFFSET_SBE) {
 		error_found = 1;
 		if (handle_errors)
-			edac_mc_handle_ce(mci, page, pageoffset,
-				/* 440BX/GX don't make syndrome information
-				 * available */
-				0, edac_mc_find_csrow_by_page(mci, page), 0,
-				mci->ctl_name);
+			edac_mc_handle_error(HW_EVENT_ERR_CORRECTED, mci,
+					     page, pageoffset, 0,
+					     edac_mc_find_csrow_by_page(mci, page),
+					     0, -1, mci->ctl_name, "", NULL);
 	}
 
 	if (info->eap & I82443BXGX_EAP_OFFSET_MBE) {
 		error_found = 1;
 		if (handle_errors)
-			edac_mc_handle_ue(mci, page, pageoffset,
-					edac_mc_find_csrow_by_page(mci, page),
-					mci->ctl_name);
+			edac_mc_handle_error(HW_EVENT_ERR_UNCORRECTED, mci,
+					     page, pageoffset, 0,
+					     edac_mc_find_csrow_by_page(mci, page),
+					     0, -1, mci->ctl_name, "", NULL);
 	}
 
 	return error_found;
@@ -189,6 +189,7 @@ static void i82443bxgx_init_csrows(struct mem_ctl_info *mci,
 				enum mem_type mtype)
 {
 	struct csrow_info *csrow;
+	struct dimm_info *dimm;
 	int index;
 	u8 drbar, dramc;
 	u32 row_base, row_high_limit, row_high_limit_last;
@@ -197,6 +198,8 @@ static void i82443bxgx_init_csrows(struct mem_ctl_info *mci,
 	row_high_limit_last = 0;
 	for (index = 0; index < mci->nr_csrows; index++) {
 		csrow = &mci->csrows[index];
+		dimm = csrow->channels[0].dimm;
+
 		pci_read_config_byte(pdev, I82443BXGX_DRB + index, &drbar);
 		debugf1("MC%d: %s: %s() Row=%d DRB = %#0x\n",
 			mci->mc_idx, __FILE__, __func__, index, drbar);
@@ -217,14 +220,14 @@ static void i82443bxgx_init_csrows(struct mem_ctl_info *mci,
 		row_base = row_high_limit_last;
 		csrow->first_page = row_base >> PAGE_SHIFT;
 		csrow->last_page = (row_high_limit >> PAGE_SHIFT) - 1;
-		csrow->nr_pages = csrow->last_page - csrow->first_page + 1;
+		dimm->nr_pages = csrow->last_page - csrow->first_page + 1;
 		/* EAP reports in 4kilobyte granularity [61] */
-		csrow->grain = 1 << 12;
-		csrow->mtype = mtype;
+		dimm->grain = 1 << 12;
+		dimm->mtype = mtype;
 		/* I don't think 440BX can tell you device type? FIXME? */
-		csrow->dtype = DEV_UNKNOWN;
+		dimm->dtype = DEV_UNKNOWN;
 		/* Mode is global to all rows on 440BX */
-		csrow->edac_mode = edac_mode;
+		dimm->edac_mode = edac_mode;
 		row_high_limit_last = row_high_limit;
 	}
 }
@@ -232,6 +235,7 @@ static void i82443bxgx_init_csrows(struct mem_ctl_info *mci,
 static int i82443bxgx_edacmc_probe1(struct pci_dev *pdev, int dev_idx)
 {
 	struct mem_ctl_info *mci;
+	struct edac_mc_layer layers[2];
 	u8 dramc;
 	u32 nbxcfg, ecc_mode;
 	enum mem_type mtype;
@@ -245,8 +249,13 @@ static int i82443bxgx_edacmc_probe1(struct pci_dev *pdev, int dev_idx)
 	if (pci_read_config_dword(pdev, I82443BXGX_NBXCFG, &nbxcfg))
 		return -EIO;
 
-	mci = edac_mc_alloc(0, I82443BXGX_NR_CSROWS, I82443BXGX_NR_CHANS, 0);
-
+	layers[0].type = EDAC_MC_LAYER_CHIP_SELECT;
+	layers[0].size = I82443BXGX_NR_CSROWS;
+	layers[0].is_virt_csrow = true;
+	layers[1].type = EDAC_MC_LAYER_CHANNEL;
+	layers[1].size = I82443BXGX_NR_CHANS;
+	layers[1].is_virt_csrow = false;
+	mci = edac_mc_alloc(0, ARRAY_SIZE(layers), layers, 0);
 	if (mci == NULL)
 		return -ENOMEM;
 
diff --git a/drivers/edac/i82860_edac.c b/drivers/edac/i82860_edac.c
index c779092d18d..08045059d10 100644
--- a/drivers/edac/i82860_edac.c
+++ b/drivers/edac/i82860_edac.c
@@ -99,6 +99,7 @@ static int i82860_process_error_info(struct mem_ctl_info *mci,
 				struct i82860_error_info *info,
 				int handle_errors)
 {
+	struct dimm_info *dimm;
 	int row;
 
 	if (!(info->errsts2 & 0x0003))
@@ -108,18 +109,25 @@ static int i82860_process_error_info(struct mem_ctl_info *mci,
 		return 1;
 
 	if ((info->errsts ^ info->errsts2) & 0x0003) {
-		edac_mc_handle_ce_no_info(mci, "UE overwrote CE");
+		edac_mc_handle_error(HW_EVENT_ERR_UNCORRECTED, mci, 0, 0, 0,
+				     -1, -1, -1, "UE overwrote CE", "", NULL);
 		info->errsts = info->errsts2;
 	}
 
 	info->eap >>= PAGE_SHIFT;
 	row = edac_mc_find_csrow_by_page(mci, info->eap);
+	dimm = mci->csrows[row].channels[0].dimm;
 
 	if (info->errsts & 0x0002)
-		edac_mc_handle_ue(mci, info->eap, 0, row, "i82860 UE");
+		edac_mc_handle_error(HW_EVENT_ERR_UNCORRECTED, mci,
+				     info->eap, 0, 0,
+				     dimm->location[0], dimm->location[1], -1,
+				     "i82860 UE", "", NULL);
 	else
-		edac_mc_handle_ce(mci, info->eap, 0, info->derrsyn, row, 0,
-				"i82860 UE");
+		edac_mc_handle_error(HW_EVENT_ERR_UNCORRECTED, mci,
+				     info->eap, 0, info->derrsyn,
+				     dimm->location[0], dimm->location[1], -1,
+				     "i82860 CE", "", NULL);
 
 	return 1;
 }
@@ -140,6 +148,7 @@ static void i82860_init_csrows(struct mem_ctl_info *mci, struct pci_dev *pdev)
 	u16 value;
 	u32 cumul_size;
 	struct csrow_info *csrow;
+	struct dimm_info *dimm;
 	int index;
 
 	pci_read_config_word(pdev, I82860_MCHCFG, &mchcfg_ddim);
@@ -153,6 +162,8 @@ static void i82860_init_csrows(struct mem_ctl_info *mci, struct pci_dev *pdev)
 	 */
 	for (index = 0; index < mci->nr_csrows; index++) {
 		csrow = &mci->csrows[index];
+		dimm = csrow->channels[0].dimm;
+
 		pci_read_config_word(pdev, I82860_GBA + index * 2, &value);
 		cumul_size = (value & I82860_GBA_MASK) <<
 			(I82860_GBA_SHIFT - PAGE_SHIFT);
@@ -164,30 +175,38 @@ static void i82860_init_csrows(struct mem_ctl_info *mci, struct pci_dev *pdev)
 
 		csrow->first_page = last_cumul_size;
 		csrow->last_page = cumul_size - 1;
-		csrow->nr_pages = cumul_size - last_cumul_size;
+		dimm->nr_pages = cumul_size - last_cumul_size;
 		last_cumul_size = cumul_size;
-		csrow->grain = 1 << 12;	/* I82860_EAP has 4KiB reolution */
-		csrow->mtype = MEM_RMBS;
-		csrow->dtype = DEV_UNKNOWN;
-		csrow->edac_mode = mchcfg_ddim ? EDAC_SECDED : EDAC_NONE;
+		dimm->grain = 1 << 12;	/* I82860_EAP has 4KiB reolution */
+		dimm->mtype = MEM_RMBS;
+		dimm->dtype = DEV_UNKNOWN;
+		dimm->edac_mode = mchcfg_ddim ? EDAC_SECDED : EDAC_NONE;
 	}
 }
 
 static int i82860_probe1(struct pci_dev *pdev, int dev_idx)
 {
 	struct mem_ctl_info *mci;
+	struct edac_mc_layer layers[2];
 	struct i82860_error_info discard;
 
-	/* RDRAM has channels but these don't map onto the abstractions that
-	   edac uses.
-	   The device groups from the GRA registers seem to map reasonably
-	   well onto the notion of a chip select row.
-	   There are 16 GRA registers and since the name is associated with
-	   the channel and the GRA registers map to physical devices so we are
-	   going to make 1 channel for group.
+	/*
+	 * RDRAM has channels but these don't map onto the csrow abstraction.
+	 * According with the datasheet, there are 2 Rambus channels, supporting
+	 * up to 16 direct RDRAM devices.
+	 * The device groups from the GRA registers seem to map reasonably
+	 * well onto the notion of a chip select row.
+	 * There are 16 GRA registers and since the name is associated with
+	 * the channel and the GRA registers map to physical devices so we are
+	 * going to make 1 channel for group.
 	 */
-	mci = edac_mc_alloc(0, 16, 1, 0);
-
+	layers[0].type = EDAC_MC_LAYER_CHANNEL;
+	layers[0].size = 2;
+	layers[0].is_virt_csrow = true;
+	layers[1].type = EDAC_MC_LAYER_SLOT;
+	layers[1].size = 8;
+	layers[1].is_virt_csrow = true;
+	mci = edac_mc_alloc(0, ARRAY_SIZE(layers), layers, 0);
 	if (!mci)
 		return -ENOMEM;
 
diff --git a/drivers/edac/i82875p_edac.c b/drivers/edac/i82875p_edac.c
index 10f15d85fb5..b613e31c16e 100644
--- a/drivers/edac/i82875p_edac.c
+++ b/drivers/edac/i82875p_edac.c
@@ -38,7 +38,8 @@
 #endif				/* PCI_DEVICE_ID_INTEL_82875_6 */
 
 /* four csrows in dual channel, eight in single channel */
-#define I82875P_NR_CSROWS(nr_chans) (8/(nr_chans))
+#define I82875P_NR_DIMMS		8
+#define I82875P_NR_CSROWS(nr_chans)	(I82875P_NR_DIMMS / (nr_chans))
 
 /* Intel 82875p register addresses - device 0 function 0 - DRAM Controller */
 #define I82875P_EAP		0x58	/* Error Address Pointer (32b)
@@ -235,7 +236,9 @@ static int i82875p_process_error_info(struct mem_ctl_info *mci,
 		return 1;
 
 	if ((info->errsts ^ info->errsts2) & 0x0081) {
-		edac_mc_handle_ce_no_info(mci, "UE overwrote CE");
+		edac_mc_handle_error(HW_EVENT_ERR_UNCORRECTED, mci, 0, 0, 0,
+				     -1, -1, -1,
+				     "UE overwrote CE", "", NULL);
 		info->errsts = info->errsts2;
 	}
 
@@ -243,11 +246,15 @@ static int i82875p_process_error_info(struct mem_ctl_info *mci,
 	row = edac_mc_find_csrow_by_page(mci, info->eap);
 
 	if (info->errsts & 0x0080)
-		edac_mc_handle_ue(mci, info->eap, 0, row, "i82875p UE");
+		edac_mc_handle_error(HW_EVENT_ERR_UNCORRECTED, mci,
+				     info->eap, 0, 0,
+				     row, -1, -1,
+				     "i82875p UE", "", NULL);
 	else
-		edac_mc_handle_ce(mci, info->eap, 0, info->derrsyn, row,
-				multi_chan ? (info->des & 0x1) : 0,
-				"i82875p CE");
+		edac_mc_handle_error(HW_EVENT_ERR_CORRECTED, mci,
+				     info->eap, 0, info->derrsyn,
+				     row, multi_chan ? (info->des & 0x1) : 0,
+				     -1, "i82875p CE", "", NULL);
 
 	return 1;
 }
@@ -342,11 +349,13 @@ static void i82875p_init_csrows(struct mem_ctl_info *mci,
 				void __iomem * ovrfl_window, u32 drc)
 {
 	struct csrow_info *csrow;
+	struct dimm_info *dimm;
+	unsigned nr_chans = dual_channel_active(drc) + 1;
 	unsigned long last_cumul_size;
 	u8 value;
 	u32 drc_ddim;		/* DRAM Data Integrity Mode 0=none,2=edac */
-	u32 cumul_size;
-	int index;
+	u32 cumul_size, nr_pages;
+	int index, j;
 
 	drc_ddim = (drc >> 18) & 0x1;
 	last_cumul_size = 0;
@@ -369,12 +378,18 @@ static void i82875p_init_csrows(struct mem_ctl_info *mci,
 
 		csrow->first_page = last_cumul_size;
 		csrow->last_page = cumul_size - 1;
-		csrow->nr_pages = cumul_size - last_cumul_size;
+		nr_pages = cumul_size - last_cumul_size;
 		last_cumul_size = cumul_size;
-		csrow->grain = 1 << 12;	/* I82875P_EAP has 4KiB reolution */
-		csrow->mtype = MEM_DDR;
-		csrow->dtype = DEV_UNKNOWN;
-		csrow->edac_mode = drc_ddim ? EDAC_SECDED : EDAC_NONE;
+
+		for (j = 0; j < nr_chans; j++) {
+			dimm = csrow->channels[j].dimm;
+
+			dimm->nr_pages = nr_pages / nr_chans;
+			dimm->grain = 1 << 12;	/* I82875P_EAP has 4KiB reolution */
+			dimm->mtype = MEM_DDR;
+			dimm->dtype = DEV_UNKNOWN;
+			dimm->edac_mode = drc_ddim ? EDAC_SECDED : EDAC_NONE;
+		}
 	}
 }
 
@@ -382,6 +397,7 @@ static int i82875p_probe1(struct pci_dev *pdev, int dev_idx)
 {
 	int rc = -ENODEV;
 	struct mem_ctl_info *mci;
+	struct edac_mc_layer layers[2];
 	struct i82875p_pvt *pvt;
 	struct pci_dev *ovrfl_pdev;
 	void __iomem *ovrfl_window;
@@ -397,9 +413,14 @@ static int i82875p_probe1(struct pci_dev *pdev, int dev_idx)
 		return -ENODEV;
 	drc = readl(ovrfl_window + I82875P_DRC);
 	nr_chans = dual_channel_active(drc) + 1;
-	mci = edac_mc_alloc(sizeof(*pvt), I82875P_NR_CSROWS(nr_chans),
-			nr_chans, 0);
 
+	layers[0].type = EDAC_MC_LAYER_CHIP_SELECT;
+	layers[0].size = I82875P_NR_CSROWS(nr_chans);
+	layers[0].is_virt_csrow = true;
+	layers[1].type = EDAC_MC_LAYER_CHANNEL;
+	layers[1].size = nr_chans;
+	layers[1].is_virt_csrow = false;
+	mci = edac_mc_alloc(0, ARRAY_SIZE(layers), layers, sizeof(*pvt));
 	if (!mci) {
 		rc = -ENOMEM;
 		goto fail0;
diff --git a/drivers/edac/i82975x_edac.c b/drivers/edac/i82975x_edac.c
index 0cd8368f88f..433332c7cdb 100644
--- a/drivers/edac/i82975x_edac.c
+++ b/drivers/edac/i82975x_edac.c
@@ -29,7 +29,8 @@
 #define PCI_DEVICE_ID_INTEL_82975_0	0x277c
 #endif				/* PCI_DEVICE_ID_INTEL_82975_0 */
 
-#define I82975X_NR_CSROWS(nr_chans)		(8/(nr_chans))
+#define I82975X_NR_DIMMS		8
+#define I82975X_NR_CSROWS(nr_chans)	(I82975X_NR_DIMMS / (nr_chans))
 
 /* Intel 82975X register addresses - device 0 function 0 - DRAM Controller */
 #define I82975X_EAP		0x58	/* Dram Error Address Pointer (32b)
@@ -287,7 +288,8 @@ static int i82975x_process_error_info(struct mem_ctl_info *mci,
 		return 1;
 
 	if ((info->errsts ^ info->errsts2) & 0x0003) {
-		edac_mc_handle_ce_no_info(mci, "UE overwrote CE");
+		edac_mc_handle_error(HW_EVENT_ERR_UNCORRECTED, mci, 0, 0, 0,
+				     -1, -1, -1, "UE overwrote CE", "", NULL);
 		info->errsts = info->errsts2;
 	}
 
@@ -309,13 +311,18 @@ static int i82975x_process_error_info(struct mem_ctl_info *mci,
 	chan = (mci->csrows[row].nr_channels == 1) ? 0 : info->eap & 1;
 	offst = info->eap
 			& ((1 << PAGE_SHIFT) -
-				(1 << mci->csrows[row].grain));
+			   (1 << mci->csrows[row].channels[chan].dimm->grain));
 
 	if (info->errsts & 0x0002)
-		edac_mc_handle_ue(mci, page, offst , row, "i82975x UE");
+		edac_mc_handle_error(HW_EVENT_ERR_UNCORRECTED, mci,
+				     page, offst, 0,
+				     row, -1, -1,
+				     "i82975x UE", "", NULL);
 	else
-		edac_mc_handle_ce(mci, page, offst, info->derrsyn, row,
-				chan, "i82975x CE");
+		edac_mc_handle_error(HW_EVENT_ERR_CORRECTED, mci,
+				     page, offst, info->derrsyn,
+				     row, chan ? chan : 0, -1,
+				     "i82975x CE", "", NULL);
 
 	return 1;
 }
@@ -370,8 +377,10 @@ static void i82975x_init_csrows(struct mem_ctl_info *mci,
 	struct csrow_info *csrow;
 	unsigned long last_cumul_size;
 	u8 value;
-	u32 cumul_size;
+	u32 cumul_size, nr_pages;
 	int index, chan;
+	struct dimm_info *dimm;
+	enum dev_type dtype;
 
 	last_cumul_size = 0;
 
@@ -400,28 +409,33 @@ static void i82975x_init_csrows(struct mem_ctl_info *mci,
 		debugf3("%s(): (%d) cumul_size 0x%x\n", __func__, index,
 			cumul_size);
 
+		nr_pages = cumul_size - last_cumul_size;
+		if (!nr_pages)
+			continue;
+
 		/*
 		 * Initialise dram labels
 		 * index values:
 		 *   [0-7] for single-channel; i.e. csrow->nr_channels = 1
 		 *   [0-3] for dual-channel; i.e. csrow->nr_channels = 2
 		 */
-		for (chan = 0; chan < csrow->nr_channels; chan++)
-			strncpy(csrow->channels[chan].label,
+		dtype = i82975x_dram_type(mch_window, index);
+		for (chan = 0; chan < csrow->nr_channels; chan++) {
+			dimm = mci->csrows[index].channels[chan].dimm;
+
+			dimm->nr_pages = nr_pages / csrow->nr_channels;
+			strncpy(csrow->channels[chan].dimm->label,
 					labels[(index >> 1) + (chan * 2)],
 					EDAC_MC_LABEL_LEN);
-
-		if (cumul_size == last_cumul_size)
-			continue;	/* not populated */
+			dimm->grain = 1 << 7;	/* 128Byte cache-line resolution */
+			dimm->dtype = i82975x_dram_type(mch_window, index);
+			dimm->mtype = MEM_DDR2; /* I82975x supports only DDR2 */
+			dimm->edac_mode = EDAC_SECDED; /* only supported */
+		}
 
 		csrow->first_page = last_cumul_size;
 		csrow->last_page = cumul_size - 1;
-		csrow->nr_pages = cumul_size - last_cumul_size;
 		last_cumul_size = cumul_size;
-		csrow->grain = 1 << 7;	/* 128Byte cache-line resolution */
-		csrow->mtype = MEM_DDR2; /* I82975x supports only DDR2 */
-		csrow->dtype = i82975x_dram_type(mch_window, index);
-		csrow->edac_mode = EDAC_SECDED; /* only supported */
 	}
 }
 
@@ -463,6 +477,7 @@ static int i82975x_probe1(struct pci_dev *pdev, int dev_idx)
 {
 	int rc = -ENODEV;
 	struct mem_ctl_info *mci;
+	struct edac_mc_layer layers[2];
 	struct i82975x_pvt *pvt;
 	void __iomem *mch_window;
 	u32 mchbar;
@@ -531,8 +546,13 @@ static int i82975x_probe1(struct pci_dev *pdev, int dev_idx)
 	chans = dual_channel_active(mch_window) + 1;
 
 	/* assuming only one controller, index thus is 0 */
-	mci = edac_mc_alloc(sizeof(*pvt), I82975X_NR_CSROWS(chans),
-					chans, 0);
+	layers[0].type = EDAC_MC_LAYER_CHIP_SELECT;
+	layers[0].size = I82975X_NR_DIMMS;
+	layers[0].is_virt_csrow = true;
+	layers[1].type = EDAC_MC_LAYER_CHANNEL;
+	layers[1].size = I82975X_NR_CSROWS(chans);
+	layers[1].is_virt_csrow = false;
+	mci = edac_mc_alloc(0, ARRAY_SIZE(layers), layers, sizeof(*pvt));
 	if (!mci) {
 		rc = -ENOMEM;
 		goto fail1;
diff --git a/drivers/edac/mpc85xx_edac.c b/drivers/edac/mpc85xx_edac.c
index 73464a62adf..4c402353ba9 100644
--- a/drivers/edac/mpc85xx_edac.c
+++ b/drivers/edac/mpc85xx_edac.c
@@ -854,12 +854,16 @@ static void mpc85xx_mc_check(struct mem_ctl_info *mci)
 		mpc85xx_mc_printk(mci, KERN_ERR, "PFN out of range!\n");
 
 	if (err_detect & DDR_EDE_SBE)
-		edac_mc_handle_ce(mci, pfn, err_addr & ~PAGE_MASK,
-				  syndrome, row_index, 0, mci->ctl_name);
+		edac_mc_handle_error(HW_EVENT_ERR_CORRECTED, mci,
+				     pfn, err_addr & ~PAGE_MASK, syndrome,
+				     row_index, 0, -1,
+				     mci->ctl_name, "", NULL);
 
 	if (err_detect & DDR_EDE_MBE)
-		edac_mc_handle_ue(mci, pfn, err_addr & ~PAGE_MASK,
-				  row_index, mci->ctl_name);
+		edac_mc_handle_error(HW_EVENT_ERR_UNCORRECTED, mci,
+				     pfn, err_addr & ~PAGE_MASK, syndrome,
+				     row_index, 0, -1,
+				     mci->ctl_name, "", NULL);
 
 	out_be32(pdata->mc_vbase + MPC85XX_MC_ERR_DETECT, err_detect);
 }
@@ -883,6 +887,7 @@ static void __devinit mpc85xx_init_csrows(struct mem_ctl_info *mci)
 {
 	struct mpc85xx_mc_pdata *pdata = mci->pvt_info;
 	struct csrow_info *csrow;
+	struct dimm_info *dimm;
 	u32 sdram_ctl;
 	u32 sdtype;
 	enum mem_type mtype;
@@ -929,6 +934,8 @@ static void __devinit mpc85xx_init_csrows(struct mem_ctl_info *mci)
 		u32 end;
 
 		csrow = &mci->csrows[index];
+		dimm = csrow->channels[0].dimm;
+
 		cs_bnds = in_be32(pdata->mc_vbase + MPC85XX_MC_CS_BNDS_0 +
 				  (index * MPC85XX_MC_CS_BNDS_OFS));
 
@@ -944,19 +951,21 @@ static void __devinit mpc85xx_init_csrows(struct mem_ctl_info *mci)
 
 		csrow->first_page = start;
 		csrow->last_page = end;
-		csrow->nr_pages = end + 1 - start;
-		csrow->grain = 8;
-		csrow->mtype = mtype;
-		csrow->dtype = DEV_UNKNOWN;
+
+		dimm->nr_pages = end + 1 - start;
+		dimm->grain = 8;
+		dimm->mtype = mtype;
+		dimm->dtype = DEV_UNKNOWN;
 		if (sdram_ctl & DSC_X32_EN)
-			csrow->dtype = DEV_X32;
-		csrow->edac_mode = EDAC_SECDED;
+			dimm->dtype = DEV_X32;
+		dimm->edac_mode = EDAC_SECDED;
 	}
 }
 
 static int __devinit mpc85xx_mc_err_probe(struct platform_device *op)
 {
 	struct mem_ctl_info *mci;
+	struct edac_mc_layer layers[2];
 	struct mpc85xx_mc_pdata *pdata;
 	struct resource r;
 	u32 sdram_ctl;
@@ -965,7 +974,13 @@ static int __devinit mpc85xx_mc_err_probe(struct platform_device *op)
 	if (!devres_open_group(&op->dev, mpc85xx_mc_err_probe, GFP_KERNEL))
 		return -ENOMEM;
 
-	mci = edac_mc_alloc(sizeof(*pdata), 4, 1, edac_mc_idx);
+	layers[0].type = EDAC_MC_LAYER_CHIP_SELECT;
+	layers[0].size = 4;
+	layers[0].is_virt_csrow = true;
+	layers[1].type = EDAC_MC_LAYER_CHANNEL;
+	layers[1].size = 1;
+	layers[1].is_virt_csrow = false;
+	mci = edac_mc_alloc(edac_mc_idx, ARRAY_SIZE(layers), sizeof(*pdata));
 	if (!mci) {
 		devres_release_group(&op->dev, mpc85xx_mc_err_probe);
 		return -ENOMEM;
diff --git a/drivers/edac/mv64x60_edac.c b/drivers/edac/mv64x60_edac.c
index 7e5ff367705..b0bb5a3d252 100644
--- a/drivers/edac/mv64x60_edac.c
+++ b/drivers/edac/mv64x60_edac.c
@@ -611,12 +611,17 @@ static void mv64x60_mc_check(struct mem_ctl_info *mci)
 
 	/* first bit clear in ECC Err Reg, 1 bit error, correctable by HW */
 	if (!(reg & 0x1))
-		edac_mc_handle_ce(mci, err_addr >> PAGE_SHIFT,
-				  err_addr & PAGE_MASK, syndrome, 0, 0,
-				  mci->ctl_name);
+		edac_mc_handle_error(HW_EVENT_ERR_CORRECTED, mci,
+				     err_addr >> PAGE_SHIFT,
+				     err_addr & PAGE_MASK, syndrome,
+				     0, 0, -1,
+				     mci->ctl_name, "", NULL);
 	else	/* 2 bit error, UE */
-		edac_mc_handle_ue(mci, err_addr >> PAGE_SHIFT,
-				  err_addr & PAGE_MASK, 0, mci->ctl_name);
+		edac_mc_handle_error(HW_EVENT_ERR_UNCORRECTED, mci,
+				     err_addr >> PAGE_SHIFT,
+				     err_addr & PAGE_MASK, 0,
+				     0, 0, -1,
+				     mci->ctl_name, "", NULL);
 
 	/* clear the error */
 	out_le32(pdata->mc_vbase + MV64X60_SDRAM_ERR_ADDR, 0);
@@ -656,6 +661,8 @@ static void mv64x60_init_csrows(struct mem_ctl_info *mci,
 				struct mv64x60_mc_pdata *pdata)
 {
 	struct csrow_info *csrow;
+	struct dimm_info *dimm;
+
 	u32 devtype;
 	u32 ctl;
 
@@ -664,35 +671,36 @@ static void mv64x60_init_csrows(struct mem_ctl_info *mci,
 	ctl = in_le32(pdata->mc_vbase + MV64X60_SDRAM_CONFIG);
 
 	csrow = &mci->csrows[0];
-	csrow->first_page = 0;
-	csrow->nr_pages = pdata->total_mem >> PAGE_SHIFT;
-	csrow->last_page = csrow->first_page + csrow->nr_pages - 1;
-	csrow->grain = 8;
+	dimm = csrow->channels[0].dimm;
+
+	dimm->nr_pages = pdata->total_mem >> PAGE_SHIFT;
+	dimm->grain = 8;
 
-	csrow->mtype = (ctl & MV64X60_SDRAM_REGISTERED) ? MEM_RDDR : MEM_DDR;
+	dimm->mtype = (ctl & MV64X60_SDRAM_REGISTERED) ? MEM_RDDR : MEM_DDR;
 
 	devtype = (ctl >> 20) & 0x3;
 	switch (devtype) {
 	case 0x0:
-		csrow->dtype = DEV_X32;
+		dimm->dtype = DEV_X32;
 		break;
 	case 0x2:		/* could be X8 too, but no way to tell */
-		csrow->dtype = DEV_X16;
+		dimm->dtype = DEV_X16;
 		break;
 	case 0x3:
-		csrow->dtype = DEV_X4;
+		dimm->dtype = DEV_X4;
 		break;
 	default:
-		csrow->dtype = DEV_UNKNOWN;
+		dimm->dtype = DEV_UNKNOWN;
 		break;
 	}
 
-	csrow->edac_mode = EDAC_SECDED;
+	dimm->edac_mode = EDAC_SECDED;
 }
 
 static int __devinit mv64x60_mc_err_probe(struct platform_device *pdev)
 {
 	struct mem_ctl_info *mci;
+	struct edac_mc_layer layers[2];
 	struct mv64x60_mc_pdata *pdata;
 	struct resource *r;
 	u32 ctl;
@@ -701,7 +709,14 @@ static int __devinit mv64x60_mc_err_probe(struct platform_device *pdev)
 	if (!devres_open_group(&pdev->dev, mv64x60_mc_err_probe, GFP_KERNEL))
 		return -ENOMEM;
 
-	mci = edac_mc_alloc(sizeof(struct mv64x60_mc_pdata), 1, 1, edac_mc_idx);
+	layers[0].type = EDAC_MC_LAYER_CHIP_SELECT;
+	layers[0].size = 1;
+	layers[0].is_virt_csrow = true;
+	layers[1].type = EDAC_MC_LAYER_CHANNEL;
+	layers[1].size = 1;
+	layers[1].is_virt_csrow = false;
+	mci = edac_mc_alloc(edac_mc_idx, ARRAY_SIZE(layers), layers,
+			    sizeof(struct mv64x60_mc_pdata));
 	if (!mci) {
 		printk(KERN_ERR "%s: No memory for CPU err\n", __func__);
 		devres_release_group(&pdev->dev, mv64x60_mc_err_probe);
diff --git a/drivers/edac/pasemi_edac.c b/drivers/edac/pasemi_edac.c
index 7f71ee43674..b095a906a99 100644
--- a/drivers/edac/pasemi_edac.c
+++ b/drivers/edac/pasemi_edac.c
@@ -110,15 +110,16 @@ static void pasemi_edac_process_error_info(struct mem_ctl_info *mci, u32 errsta)
 	/* uncorrectable/multi-bit errors */
 	if (errsta & (MCDEBUG_ERRSTA_MBE_STATUS |
 		      MCDEBUG_ERRSTA_RFL_STATUS)) {
-		edac_mc_handle_ue(mci, mci->csrows[cs].first_page, 0,
-				  cs, mci->ctl_name);
+		edac_mc_handle_error(HW_EVENT_ERR_UNCORRECTED, mci,
+				     mci->csrows[cs].first_page, 0, 0,
+				     cs, 0, -1, mci->ctl_name, "", NULL);
 	}
 
 	/* correctable/single-bit errors */
-	if (errsta & MCDEBUG_ERRSTA_SBE_STATUS) {
-		edac_mc_handle_ce(mci, mci->csrows[cs].first_page, 0,
-				  0, cs, 0, mci->ctl_name);
-	}
+	if (errsta & MCDEBUG_ERRSTA_SBE_STATUS)
+		edac_mc_handle_error(HW_EVENT_ERR_CORRECTED, mci,
+				     mci->csrows[cs].first_page, 0, 0,
+				     cs, 0, -1, mci->ctl_name, "", NULL);
 }
 
 static void pasemi_edac_check(struct mem_ctl_info *mci)
@@ -135,11 +136,13 @@ static int pasemi_edac_init_csrows(struct mem_ctl_info *mci,
 				   enum edac_type edac_mode)
 {
 	struct csrow_info *csrow;
+	struct dimm_info *dimm;
 	u32 rankcfg;
 	int index;
 
 	for (index = 0; index < mci->nr_csrows; index++) {
 		csrow = &mci->csrows[index];
+		dimm = csrow->channels[0].dimm;
 
 		pci_read_config_dword(pdev,
 				      MCDRAM_RANKCFG + (index * 12),
@@ -151,20 +154,20 @@ static int pasemi_edac_init_csrows(struct mem_ctl_info *mci,
 		switch ((rankcfg & MCDRAM_RANKCFG_TYPE_SIZE_M) >>
 			MCDRAM_RANKCFG_TYPE_SIZE_S) {
 		case 0:
-			csrow->nr_pages = 128 << (20 - PAGE_SHIFT);
+			dimm->nr_pages = 128 << (20 - PAGE_SHIFT);
 			break;
 		case 1:
-			csrow->nr_pages = 256 << (20 - PAGE_SHIFT);
+			dimm->nr_pages = 256 << (20 - PAGE_SHIFT);
 			break;
 		case 2:
 		case 3:
-			csrow->nr_pages = 512 << (20 - PAGE_SHIFT);
+			dimm->nr_pages = 512 << (20 - PAGE_SHIFT);
 			break;
 		case 4:
-			csrow->nr_pages = 1024 << (20 - PAGE_SHIFT);
+			dimm->nr_pages = 1024 << (20 - PAGE_SHIFT);
 			break;
 		case 5:
-			csrow->nr_pages = 2048 << (20 - PAGE_SHIFT);
+			dimm->nr_pages = 2048 << (20 - PAGE_SHIFT);
 			break;
 		default:
 			edac_mc_printk(mci, KERN_ERR,
@@ -174,13 +177,13 @@ static int pasemi_edac_init_csrows(struct mem_ctl_info *mci,
 		}
 
 		csrow->first_page = last_page_in_mmc;
-		csrow->last_page = csrow->first_page + csrow->nr_pages - 1;
-		last_page_in_mmc += csrow->nr_pages;
+		csrow->last_page = csrow->first_page + dimm->nr_pages - 1;
+		last_page_in_mmc += dimm->nr_pages;
 		csrow->page_mask = 0;
-		csrow->grain = PASEMI_EDAC_ERROR_GRAIN;
-		csrow->mtype = MEM_DDR;
-		csrow->dtype = DEV_UNKNOWN;
-		csrow->edac_mode = edac_mode;
+		dimm->grain = PASEMI_EDAC_ERROR_GRAIN;
+		dimm->mtype = MEM_DDR;
+		dimm->dtype = DEV_UNKNOWN;
+		dimm->edac_mode = edac_mode;
 	}
 	return 0;
 }
@@ -189,6 +192,7 @@ static int __devinit pasemi_edac_probe(struct pci_dev *pdev,
 		const struct pci_device_id *ent)
 {
 	struct mem_ctl_info *mci = NULL;
+	struct edac_mc_layer layers[2];
 	u32 errctl1, errcor, scrub, mcen;
 
 	pci_read_config_dword(pdev, MCCFG_MCEN, &mcen);
@@ -205,9 +209,14 @@ static int __devinit pasemi_edac_probe(struct pci_dev *pdev,
 		MCDEBUG_ERRCTL1_RFL_LOG_EN;
 	pci_write_config_dword(pdev, MCDEBUG_ERRCTL1, errctl1);
 
-	mci = edac_mc_alloc(0, PASEMI_EDAC_NR_CSROWS, PASEMI_EDAC_NR_CHANS,
-				system_mmc_id++);
-
+	layers[0].type = EDAC_MC_LAYER_CHIP_SELECT;
+	layers[0].size = PASEMI_EDAC_NR_CSROWS;
+	layers[0].is_virt_csrow = true;
+	layers[1].type = EDAC_MC_LAYER_CHANNEL;
+	layers[1].size = PASEMI_EDAC_NR_CHANS;
+	layers[1].is_virt_csrow = false;
+	mci = edac_mc_alloc(system_mmc_id++, ARRAY_SIZE(layers), layers,
+			    0);
 	if (mci == NULL)
 		return -ENOMEM;
 
diff --git a/drivers/edac/ppc4xx_edac.c b/drivers/edac/ppc4xx_edac.c
index d427c69bb8b..f3f9fed06ad 100644
--- a/drivers/edac/ppc4xx_edac.c
+++ b/drivers/edac/ppc4xx_edac.c
@@ -727,7 +727,10 @@ ppc4xx_edac_handle_ce(struct mem_ctl_info *mci,
 
 	for (row = 0; row < mci->nr_csrows; row++)
 		if (ppc4xx_edac_check_bank_error(status, row))
-			edac_mc_handle_ce_no_info(mci, message);
+			edac_mc_handle_error(HW_EVENT_ERR_CORRECTED, mci,
+					     0, 0, 0,
+					     row, 0, -1,
+					     message, "", NULL);
 }
 
 /**
@@ -755,7 +758,10 @@ ppc4xx_edac_handle_ue(struct mem_ctl_info *mci,
 
 	for (row = 0; row < mci->nr_csrows; row++)
 		if (ppc4xx_edac_check_bank_error(status, row))
-			edac_mc_handle_ue(mci, page, offset, row, message);
+			edac_mc_handle_error(HW_EVENT_ERR_UNCORRECTED, mci,
+					     page, offset, 0,
+					     row, 0, -1,
+					     message, "", NULL);
 }
 
 /**
@@ -895,9 +901,8 @@ ppc4xx_edac_init_csrows(struct mem_ctl_info *mci, u32 mcopt1)
 	enum mem_type mtype;
 	enum dev_type dtype;
 	enum edac_type edac_mode;
-	int row;
-	u32 mbxcf, size;
-	static u32 ppc4xx_last_page;
+	int row, j;
+	u32 mbxcf, size, nr_pages;
 
 	/* Establish the memory type and width */
 
@@ -948,7 +953,7 @@ ppc4xx_edac_init_csrows(struct mem_ctl_info *mci, u32 mcopt1)
 		case SDRAM_MBCF_SZ_2GB:
 		case SDRAM_MBCF_SZ_4GB:
 		case SDRAM_MBCF_SZ_8GB:
-			csi->nr_pages = SDRAM_MBCF_SZ_TO_PAGES(size);
+			nr_pages = SDRAM_MBCF_SZ_TO_PAGES(size);
 			break;
 		default:
 			ppc4xx_edac_mc_printk(KERN_ERR, mci,
@@ -959,10 +964,6 @@ ppc4xx_edac_init_csrows(struct mem_ctl_info *mci, u32 mcopt1)
 			goto done;
 		}
 
-		csi->first_page = ppc4xx_last_page;
-		csi->last_page	= csi->first_page + csi->nr_pages - 1;
-		csi->page_mask	= 0;
-
 		/*
 		 * It's unclear exactly what grain should be set to
 		 * here. The SDRAM_ECCES register allows resolution of
@@ -975,15 +976,17 @@ ppc4xx_edac_init_csrows(struct mem_ctl_info *mci, u32 mcopt1)
 		 * possible values would be the PLB width (16), the
 		 * page size (PAGE_SIZE) or the memory width (2 or 4).
 		 */
+		for (j = 0; j < csi->nr_channels; j++) {
+			struct dimm_info *dimm = csi->channels[j].dimm;
 
-		csi->grain	= 1;
-
-		csi->mtype	= mtype;
-		csi->dtype	= dtype;
+			dimm->nr_pages  = nr_pages / csi->nr_channels;
+			dimm->grain	= 1;
 
-		csi->edac_mode	= edac_mode;
+			dimm->mtype	= mtype;
+			dimm->dtype	= dtype;
 
-		ppc4xx_last_page += csi->nr_pages;
+			dimm->edac_mode	= edac_mode;
+		}
 	}
 
  done:
@@ -1236,6 +1239,7 @@ static int __devinit ppc4xx_edac_probe(struct platform_device *op)
 	dcr_host_t dcr_host;
 	const struct device_node *np = op->dev.of_node;
 	struct mem_ctl_info *mci = NULL;
+	struct edac_mc_layer layers[2];
 	static int ppc4xx_edac_instance;
 
 	/*
@@ -1281,12 +1285,14 @@ static int __devinit ppc4xx_edac_probe(struct platform_device *op)
 	 * controller instance and perform the appropriate
 	 * initialization.
 	 */
-
-	mci = edac_mc_alloc(sizeof(struct ppc4xx_edac_pdata),
-			    ppc4xx_edac_nr_csrows,
-			    ppc4xx_edac_nr_chans,
-			    ppc4xx_edac_instance);
-
+	layers[0].type = EDAC_MC_LAYER_CHIP_SELECT;
+	layers[0].size = ppc4xx_edac_nr_csrows;
+	layers[0].is_virt_csrow = true;
+	layers[1].type = EDAC_MC_LAYER_CHANNEL;
+	layers[1].size = ppc4xx_edac_nr_chans;
+	layers[1].is_virt_csrow = false;
+	mci = edac_mc_alloc(ppc4xx_edac_instance, ARRAY_SIZE(layers), layers,
+			    sizeof(struct ppc4xx_edac_pdata));
 	if (mci == NULL) {
 		ppc4xx_edac_printk(KERN_ERR, "%s: "
 				   "Failed to allocate EDAC MC instance!\n",
diff --git a/drivers/edac/r82600_edac.c b/drivers/edac/r82600_edac.c
index 6d908ad72d6..e1cacd164f3 100644
--- a/drivers/edac/r82600_edac.c
+++ b/drivers/edac/r82600_edac.c
@@ -179,10 +179,11 @@ static int r82600_process_error_info(struct mem_ctl_info *mci,
 		error_found = 1;
 
 		if (handle_errors)
-			edac_mc_handle_ce(mci, page, 0,	/* not avail */
-					syndrome,
-					edac_mc_find_csrow_by_page(mci, page),
-					0, mci->ctl_name);
+			edac_mc_handle_error(HW_EVENT_ERR_CORRECTED, mci,
+					     page, 0, syndrome,
+					     edac_mc_find_csrow_by_page(mci, page),
+					     0, -1,
+					     mci->ctl_name, "", NULL);
 	}
 
 	if (info->eapr & BIT(1)) {	/* UE? */
@@ -190,9 +191,11 @@ static int r82600_process_error_info(struct mem_ctl_info *mci,
 
 		if (handle_errors)
 			/* 82600 doesn't give enough info */
-			edac_mc_handle_ue(mci, page, 0,
-					edac_mc_find_csrow_by_page(mci, page),
-					mci->ctl_name);
+			edac_mc_handle_error(HW_EVENT_ERR_UNCORRECTED, mci,
+					     page, 0, 0,
+					     edac_mc_find_csrow_by_page(mci, page),
+					     0, -1,
+					     mci->ctl_name, "", NULL);
 	}
 
 	return error_found;
@@ -216,6 +219,7 @@ static void r82600_init_csrows(struct mem_ctl_info *mci, struct pci_dev *pdev,
 			u8 dramcr)
 {
 	struct csrow_info *csrow;
+	struct dimm_info *dimm;
 	int index;
 	u8 drbar;		/* SDRAM Row Boundary Address Register */
 	u32 row_high_limit, row_high_limit_last;
@@ -227,6 +231,7 @@ static void r82600_init_csrows(struct mem_ctl_info *mci, struct pci_dev *pdev,
 
 	for (index = 0; index < mci->nr_csrows; index++) {
 		csrow = &mci->csrows[index];
+		dimm = csrow->channels[0].dimm;
 
 		/* find the DRAM Chip Select Base address and mask */
 		pci_read_config_byte(pdev, R82600_DRBA + index, &drbar);
@@ -247,16 +252,17 @@ static void r82600_init_csrows(struct mem_ctl_info *mci, struct pci_dev *pdev,
 
 		csrow->first_page = row_base >> PAGE_SHIFT;
 		csrow->last_page = (row_high_limit >> PAGE_SHIFT) - 1;
-		csrow->nr_pages = csrow->last_page - csrow->first_page + 1;
+
+		dimm->nr_pages = csrow->last_page - csrow->first_page + 1;
 		/* Error address is top 19 bits - so granularity is      *
 		 * 14 bits                                               */
-		csrow->grain = 1 << 14;
-		csrow->mtype = reg_sdram ? MEM_RDDR : MEM_DDR;
+		dimm->grain = 1 << 14;
+		dimm->mtype = reg_sdram ? MEM_RDDR : MEM_DDR;
 		/* FIXME - check that this is unknowable with this chipset */
-		csrow->dtype = DEV_UNKNOWN;
+		dimm->dtype = DEV_UNKNOWN;
 
 		/* Mode is global on 82600 */
-		csrow->edac_mode = ecc_on ? EDAC_SECDED : EDAC_NONE;
+		dimm->edac_mode = ecc_on ? EDAC_SECDED : EDAC_NONE;
 		row_high_limit_last = row_high_limit;
 	}
 }
@@ -264,6 +270,7 @@ static void r82600_init_csrows(struct mem_ctl_info *mci, struct pci_dev *pdev,
 static int r82600_probe1(struct pci_dev *pdev, int dev_idx)
 {
 	struct mem_ctl_info *mci;
+	struct edac_mc_layer layers[2];
 	u8 dramcr;
 	u32 eapr;
 	u32 scrub_disabled;
@@ -278,8 +285,13 @@ static int r82600_probe1(struct pci_dev *pdev, int dev_idx)
 	debugf2("%s(): sdram refresh rate = %#0x\n", __func__,
 		sdram_refresh_rate);
 	debugf2("%s(): DRAMC register = %#0x\n", __func__, dramcr);
-	mci = edac_mc_alloc(0, R82600_NR_CSROWS, R82600_NR_CHANS, 0);
-
+	layers[0].type = EDAC_MC_LAYER_CHIP_SELECT;
+	layers[0].size = R82600_NR_CSROWS;
+	layers[0].is_virt_csrow = true;
+	layers[1].type = EDAC_MC_LAYER_CHANNEL;
+	layers[1].size = R82600_NR_CHANS;
+	layers[1].is_virt_csrow = false;
+	mci = edac_mc_alloc(0, ARRAY_SIZE(layers), layers, 0);
 	if (mci == NULL)
 		return -ENOMEM;
 
diff --git a/drivers/edac/sb_edac.c b/drivers/edac/sb_edac.c
index a203536d90d..4adaf4b7da9 100644
--- a/drivers/edac/sb_edac.c
+++ b/drivers/edac/sb_edac.c
@@ -58,7 +58,7 @@ static int probed;
 
 /*
  * FIXME: For now, let's order by device function, as it makes
- * easier for driver's development proccess. This table should be
+ * easier for driver's development process. This table should be
  * moved to pci_id.h when submitted upstream
  */
 #define PCI_DEVICE_ID_INTEL_SBRIDGE_SAD0	0x3cf4	/* 12.6 */
@@ -314,8 +314,6 @@ struct sbridge_pvt {
 	struct sbridge_info	info;
 	struct sbridge_channel	channel[NUM_CHANNELS];
 
-	int 			csrow_map[NUM_CHANNELS][MAX_DIMMS];
-
 	/* Memory type detection */
 	bool			is_mirrored, is_lockstep, is_close_pg;
 
@@ -375,7 +373,7 @@ static DEFINE_PCI_DEVICE_TABLE(sbridge_pci_tbl) = {
 
 
 /****************************************************************************
-			Anciliary status routines
+			Ancillary status routines
  ****************************************************************************/
 
 static inline int numrank(u32 mtr)
@@ -487,29 +485,14 @@ static struct pci_dev *get_pdev_slot_func(u8 bus, unsigned slot,
 }
 
 /**
- * sbridge_get_active_channels() - gets the number of channels and csrows
+ * check_if_ecc_is_active() - Checks if ECC is active
  * bus:		Device bus
- * @channels:	Number of channels that will be returned
- * @csrows:	Number of csrows found
- *
- * Since EDAC core needs to know in advance the number of available channels
- * and csrows, in order to allocate memory for csrows/channels, it is needed
- * to run two similar steps. At the first step, implemented on this function,
- * it checks the number of csrows/channels present at one socket, identified
- * by the associated PCI bus.
- * this is used in order to properly allocate the size of mci components.
- * Note: one csrow is one dimm.
  */
-static int sbridge_get_active_channels(const u8 bus, unsigned *channels,
-				      unsigned *csrows)
+static int check_if_ecc_is_active(const u8 bus)
 {
 	struct pci_dev *pdev = NULL;
-	int i, j;
 	u32 mcmtr;
 
-	*channels = 0;
-	*csrows = 0;
-
 	pdev = get_pdev_slot_func(bus, 15, 0);
 	if (!pdev) {
 		sbridge_printk(KERN_ERR, "Couldn't find PCI device "
@@ -523,41 +506,14 @@ static int sbridge_get_active_channels(const u8 bus, unsigned *channels,
 		sbridge_printk(KERN_ERR, "ECC is disabled. Aborting\n");
 		return -ENODEV;
 	}
-
-	for (i = 0; i < NUM_CHANNELS; i++) {
-		u32 mtr;
-
-		/* Device 15 functions 2 - 5  */
-		pdev = get_pdev_slot_func(bus, 15, 2 + i);
-		if (!pdev) {
-			sbridge_printk(KERN_ERR, "Couldn't find PCI device "
-						 "%2x.%02d.%d!!!\n",
-						 bus, 15, 2 + i);
-			return -ENODEV;
-		}
-		(*channels)++;
-
-		for (j = 0; j < ARRAY_SIZE(mtr_regs); j++) {
-			pci_read_config_dword(pdev, mtr_regs[j], &mtr);
-			debugf1("Bus#%02x channel #%d  MTR%d = %x\n", bus, i, j, mtr);
-			if (IS_DIMM_PRESENT(mtr))
-				(*csrows)++;
-		}
-	}
-
-	debugf0("Number of active channels: %d, number of active dimms: %d\n",
-		*channels, *csrows);
-
 	return 0;
 }
 
-static int get_dimm_config(const struct mem_ctl_info *mci)
+static int get_dimm_config(struct mem_ctl_info *mci)
 {
 	struct sbridge_pvt *pvt = mci->pvt_info;
-	struct csrow_info *csr;
+	struct dimm_info *dimm;
 	int i, j, banks, ranks, rows, cols, size, npages;
-	int csrow = 0;
-	unsigned long last_page = 0;
 	u32 reg;
 	enum edac_type mode;
 	enum mem_type mtype;
@@ -616,6 +572,8 @@ static int get_dimm_config(const struct mem_ctl_info *mci)
 		u32 mtr;
 
 		for (j = 0; j < ARRAY_SIZE(mtr_regs); j++) {
+			dimm = EDAC_DIMM_PTR(mci->layers, mci->dimms, mci->n_layers,
+				       i, j, 0);
 			pci_read_config_dword(pvt->pci_tad[i],
 					      mtr_regs[j], &mtr);
 			debugf4("Channel #%d  MTR%d = %x\n", i, j, mtr);
@@ -634,29 +592,15 @@ static int get_dimm_config(const struct mem_ctl_info *mci)
 					pvt->sbridge_dev->mc, i, j,
 					size, npages,
 					banks, ranks, rows, cols);
-				csr = &mci->csrows[csrow];
-
-				csr->first_page = last_page;
-				csr->last_page = last_page + npages - 1;
-				csr->page_mask = 0UL;	/* Unused */
-				csr->nr_pages = npages;
-				csr->grain = 32;
-				csr->csrow_idx = csrow;
-				csr->dtype = (banks == 8) ? DEV_X8 : DEV_X4;
-				csr->ce_count = 0;
-				csr->ue_count = 0;
-				csr->mtype = mtype;
-				csr->edac_mode = mode;
-				csr->nr_channels = 1;
-				csr->channels[0].chan_idx = i;
-				csr->channels[0].ce_count = 0;
-				pvt->csrow_map[i][j] = csrow;
-				snprintf(csr->channels[0].label,
-					 sizeof(csr->channels[0].label),
+
+				dimm->nr_pages = npages;
+				dimm->grain = 32;
+				dimm->dtype = (banks == 8) ? DEV_X8 : DEV_X4;
+				dimm->mtype = mtype;
+				dimm->edac_mode = mode;
+				snprintf(dimm->label, sizeof(dimm->label),
 					 "CPU_SrcID#%u_Channel#%u_DIMM#%u",
 					 pvt->sbridge_dev->source_id, i, j);
-				last_page += npages;
-				csrow++;
 			}
 		}
 	}
@@ -844,11 +788,10 @@ static int get_memory_error_data(struct mem_ctl_info *mci,
 				 u8 *socket,
 				 long *channel_mask,
 				 u8 *rank,
-				 char *area_type)
+				 char **area_type, char *msg)
 {
 	struct mem_ctl_info	*new_mci;
 	struct sbridge_pvt *pvt = mci->pvt_info;
-	char			msg[256];
 	int 			n_rir, n_sads, n_tads, sad_way, sck_xch;
 	int			sad_interl, idx, base_ch;
 	int			interleave_mode;
@@ -870,12 +813,10 @@ static int get_memory_error_data(struct mem_ctl_info *mci,
 	 */
 	if ((addr > (u64) pvt->tolm) && (addr < (1LL << 32))) {
 		sprintf(msg, "Error at TOLM area, on addr 0x%08Lx", addr);
-		edac_mc_handle_ce_no_info(mci, msg);
 		return -EINVAL;
 	}
 	if (addr >= (u64)pvt->tohm) {
 		sprintf(msg, "Error at MMIOH area, on addr 0x%016Lx", addr);
-		edac_mc_handle_ce_no_info(mci, msg);
 		return -EINVAL;
 	}
 
@@ -892,7 +833,6 @@ static int get_memory_error_data(struct mem_ctl_info *mci,
 		limit = SAD_LIMIT(reg);
 		if (limit <= prv) {
 			sprintf(msg, "Can't discover the memory socket");
-			edac_mc_handle_ce_no_info(mci, msg);
 			return -EINVAL;
 		}
 		if  (addr <= limit)
@@ -901,10 +841,9 @@ static int get_memory_error_data(struct mem_ctl_info *mci,
 	}
 	if (n_sads == MAX_SAD) {
 		sprintf(msg, "Can't discover the memory socket");
-		edac_mc_handle_ce_no_info(mci, msg);
 		return -EINVAL;
 	}
-	area_type = get_dram_attr(reg);
+	*area_type = get_dram_attr(reg);
 	interleave_mode = INTERLEAVE_MODE(reg);
 
 	pci_read_config_dword(pvt->pci_sad0, interleave_list[n_sads],
@@ -942,7 +881,6 @@ static int get_memory_error_data(struct mem_ctl_info *mci,
 		break;
 	default:
 		sprintf(msg, "Can't discover socket interleave");
-		edac_mc_handle_ce_no_info(mci, msg);
 		return -EINVAL;
 	}
 	*socket = sad_interleave[idx];
@@ -957,7 +895,6 @@ static int get_memory_error_data(struct mem_ctl_info *mci,
 	if (!new_mci) {
 		sprintf(msg, "Struct for socket #%u wasn't initialized",
 			*socket);
-		edac_mc_handle_ce_no_info(mci, msg);
 		return -EINVAL;
 	}
 	mci = new_mci;
@@ -973,7 +910,6 @@ static int get_memory_error_data(struct mem_ctl_info *mci,
 		limit = TAD_LIMIT(reg);
 		if (limit <= prv) {
 			sprintf(msg, "Can't discover the memory channel");
-			edac_mc_handle_ce_no_info(mci, msg);
 			return -EINVAL;
 		}
 		if  (addr <= limit)
@@ -1013,7 +949,6 @@ static int get_memory_error_data(struct mem_ctl_info *mci,
 		break;
 	default:
 		sprintf(msg, "Can't discover the TAD target");
-		edac_mc_handle_ce_no_info(mci, msg);
 		return -EINVAL;
 	}
 	*channel_mask = 1 << base_ch;
@@ -1027,7 +962,6 @@ static int get_memory_error_data(struct mem_ctl_info *mci,
 			break;
 		default:
 			sprintf(msg, "Invalid mirror set. Can't decode addr");
-			edac_mc_handle_ce_no_info(mci, msg);
 			return -EINVAL;
 		}
 	} else
@@ -1055,7 +989,6 @@ static int get_memory_error_data(struct mem_ctl_info *mci,
 	if (offset > addr) {
 		sprintf(msg, "Can't calculate ch addr: TAD offset 0x%08Lx is too high for addr 0x%08Lx!",
 			offset, addr);
-		edac_mc_handle_ce_no_info(mci, msg);
 		return -EINVAL;
 	}
 	addr -= offset;
@@ -1095,7 +1028,6 @@ static int get_memory_error_data(struct mem_ctl_info *mci,
 	if (n_rir == MAX_RIR_RANGES) {
 		sprintf(msg, "Can't discover the memory rank for ch addr 0x%08Lx",
 			ch_addr);
-		edac_mc_handle_ce_no_info(mci, msg);
 		return -EINVAL;
 	}
 	rir_way = RIR_WAY(reg);
@@ -1409,7 +1341,8 @@ static void sbridge_mce_output_error(struct mem_ctl_info *mci,
 {
 	struct mem_ctl_info *new_mci;
 	struct sbridge_pvt *pvt = mci->pvt_info;
-	char *type, *optype, *msg, *recoverable_msg;
+	enum hw_event_mc_err_type tp_event;
+	char *type, *optype, msg[256];
 	bool ripv = GET_BITFIELD(m->mcgstatus, 0, 0);
 	bool overflow = GET_BITFIELD(m->status, 62, 62);
 	bool uncorrected_error = GET_BITFIELD(m->status, 61, 61);
@@ -1421,16 +1354,24 @@ static void sbridge_mce_output_error(struct mem_ctl_info *mci,
 	u32 optypenum = GET_BITFIELD(m->status, 4, 6);
 	long channel_mask, first_channel;
 	u8  rank, socket;
-	int csrow, rc, dimm;
-	char *area_type = "Unknown";
-
-	if (ripv)
-		type = "NON_FATAL";
-	else
-		type = "FATAL";
+	int rc, dimm;
+	char *area_type = NULL;
+
+	if (uncorrected_error) {
+		if (ripv) {
+			type = "FATAL";
+			tp_event = HW_EVENT_ERR_FATAL;
+		} else {
+			type = "NON_FATAL";
+			tp_event = HW_EVENT_ERR_UNCORRECTED;
+		}
+	} else {
+		type = "CORRECTED";
+		tp_event = HW_EVENT_ERR_CORRECTED;
+	}
 
 	/*
-	 * According with Table 15-9 of the Intel Archictecture spec vol 3A,
+	 * According with Table 15-9 of the Intel Architecture spec vol 3A,
 	 * memory errors should fit in this mask:
 	 *	000f 0000 1mmm cccc (binary)
 	 * where:
@@ -1445,19 +1386,19 @@ static void sbridge_mce_output_error(struct mem_ctl_info *mci,
 	} else {
 		switch (optypenum) {
 		case 0:
-			optype = "generic undef request";
+			optype = "generic undef request error";
 			break;
 		case 1:
-			optype = "memory read";
+			optype = "memory read error";
 			break;
 		case 2:
-			optype = "memory write";
+			optype = "memory write error";
 			break;
 		case 3:
-			optype = "addr/cmd";
+			optype = "addr/cmd error";
 			break;
 		case 4:
-			optype = "memory scrubbing";
+			optype = "memory scrubbing error";
 			break;
 		default:
 			optype = "reserved";
@@ -1466,13 +1407,13 @@ static void sbridge_mce_output_error(struct mem_ctl_info *mci,
 	}
 
 	rc = get_memory_error_data(mci, m->addr, &socket,
-				   &channel_mask, &rank, area_type);
+				   &channel_mask, &rank, &area_type, msg);
 	if (rc < 0)
-		return;
+		goto err_parsing;
 	new_mci = get_mci_for_node_id(socket);
 	if (!new_mci) {
-		edac_mc_handle_ce_no_info(mci, "Error: socket got corrupted!");
-		return;
+		strcpy(msg, "Error: socket got corrupted!");
+		goto err_parsing;
 	}
 	mci = new_mci;
 	pvt = mci->pvt_info;
@@ -1486,45 +1427,39 @@ static void sbridge_mce_output_error(struct mem_ctl_info *mci,
 	else
 		dimm = 2;
 
-	csrow = pvt->csrow_map[first_channel][dimm];
-
-	if (uncorrected_error && recoverable)
-		recoverable_msg = " recoverable";
-	else
-		recoverable_msg = "";
 
 	/*
-	 * FIXME: What should we do with "channel" information on mcelog?
-	 * Probably, we can just discard it, as the channel information
-	 * comes from the get_memory_error_data() address decoding
+	 * FIXME: On some memory configurations (mirror, lockstep), the
+	 * Memory Controller can't point the error to a single DIMM. The
+	 * EDAC core should be handling the channel mask, in order to point
+	 * to the group of dimm's where the error may be happening.
 	 */
-	msg = kasprintf(GFP_ATOMIC,
-			"%d %s error(s): %s on %s area %s%s: cpu=%d Err=%04x:%04x (ch=%d), "
-			"addr = 0x%08llx => socket=%d, Channel=%ld(mask=%ld), rank=%d\n",
-			core_err_cnt,
-			area_type,
-			optype,
-			type,
-			recoverable_msg,
-			overflow ? "OVERFLOW" : "",
-			m->cpu,
-			mscod, errcode,
-			channel,		/* 1111b means not specified */
-			(long long) m->addr,
-			socket,
-			first_channel,		/* This is the real channel on SB */
-			channel_mask,
-			rank);
+	snprintf(msg, sizeof(msg),
+		 "count:%d%s%s area:%s err_code:%04x:%04x socket:%d channel_mask:%ld rank:%d",
+		 core_err_cnt,
+		 overflow ? " OVERFLOW" : "",
+		 (uncorrected_error && recoverable) ? " recoverable" : "",
+		 area_type,
+		 mscod, errcode,
+		 socket,
+		 channel_mask,
+		 rank);
 
 	debugf0("%s", msg);
 
+	/* FIXME: need support for channel mask */
+
 	/* Call the helper to output message */
-	if (uncorrected_error)
-		edac_mc_handle_fbd_ue(mci, csrow, 0, 0, msg);
-	else
-		edac_mc_handle_fbd_ce(mci, csrow, 0, msg);
+	edac_mc_handle_error(tp_event, mci,
+			     m->addr >> PAGE_SHIFT, m->addr & ~PAGE_MASK, 0,
+			     channel, dimm, -1,
+			     optype, msg, m);
+	return;
+err_parsing:
+	edac_mc_handle_error(tp_event, mci, 0, 0, 0,
+			     -1, -1, -1,
+			     msg, "", m);
 
-	kfree(msg);
 }
 
 /*
@@ -1683,16 +1618,25 @@ static void sbridge_unregister_mci(struct sbridge_dev *sbridge_dev)
 static int sbridge_register_mci(struct sbridge_dev *sbridge_dev)
 {
 	struct mem_ctl_info *mci;
+	struct edac_mc_layer layers[2];
 	struct sbridge_pvt *pvt;
-	int rc, channels, csrows;
+	int rc;
 
 	/* Check the number of active and not disabled channels */
-	rc = sbridge_get_active_channels(sbridge_dev->bus, &channels, &csrows);
+	rc = check_if_ecc_is_active(sbridge_dev->bus);
 	if (unlikely(rc < 0))
 		return rc;
 
 	/* allocate a new MC control structure */
-	mci = edac_mc_alloc(sizeof(*pvt), csrows, channels, sbridge_dev->mc);
+	layers[0].type = EDAC_MC_LAYER_CHANNEL;
+	layers[0].size = NUM_CHANNELS;
+	layers[0].is_virt_csrow = false;
+	layers[1].type = EDAC_MC_LAYER_SLOT;
+	layers[1].size = MAX_DIMMS;
+	layers[1].is_virt_csrow = true;
+	mci = edac_mc_alloc(sbridge_dev->mc, ARRAY_SIZE(layers), layers,
+			    sizeof(*pvt));
+
 	if (unlikely(!mci))
 		return -ENOMEM;
 
diff --git a/drivers/edac/tile_edac.c b/drivers/edac/tile_edac.c
index e99d0097618..7bb4614730d 100644
--- a/drivers/edac/tile_edac.c
+++ b/drivers/edac/tile_edac.c
@@ -71,7 +71,10 @@ static void tile_edac_check(struct mem_ctl_info *mci)
 	if (mem_error.sbe_count != priv->ce_count) {
 		dev_dbg(mci->dev, "ECC CE err on node %d\n", priv->node);
 		priv->ce_count = mem_error.sbe_count;
-		edac_mc_handle_ce(mci, 0, 0, 0, 0, 0, mci->ctl_name);
+		edac_mc_handle_error(HW_EVENT_ERR_CORRECTED, mci,
+				     0, 0, 0,
+				     0, 0, -1,
+				     mci->ctl_name, "", NULL);
 	}
 }
 
@@ -84,6 +87,7 @@ static int __devinit tile_edac_init_csrows(struct mem_ctl_info *mci)
 	struct csrow_info	*csrow = &mci->csrows[0];
 	struct tile_edac_priv	*priv = mci->pvt_info;
 	struct mshim_mem_info	mem_info;
+	struct dimm_info *dimm = csrow->channels[0].dimm;
 
 	if (hv_dev_pread(priv->hv_devhdl, 0, (HV_VirtAddr)&mem_info,
 		sizeof(struct mshim_mem_info), MSHIM_MEM_INFO_OFF) !=
@@ -93,27 +97,25 @@ static int __devinit tile_edac_init_csrows(struct mem_ctl_info *mci)
 	}
 
 	if (mem_info.mem_ecc)
-		csrow->edac_mode = EDAC_SECDED;
+		dimm->edac_mode = EDAC_SECDED;
 	else
-		csrow->edac_mode = EDAC_NONE;
+		dimm->edac_mode = EDAC_NONE;
 	switch (mem_info.mem_type) {
 	case DDR2:
-		csrow->mtype = MEM_DDR2;
+		dimm->mtype = MEM_DDR2;
 		break;
 
 	case DDR3:
-		csrow->mtype = MEM_DDR3;
+		dimm->mtype = MEM_DDR3;
 		break;
 
 	default:
 		return -1;
 	}
 
-	csrow->first_page = 0;
-	csrow->nr_pages = mem_info.mem_size >> PAGE_SHIFT;
-	csrow->last_page = csrow->first_page + csrow->nr_pages - 1;
-	csrow->grain = TILE_EDAC_ERROR_GRAIN;
-	csrow->dtype = DEV_UNKNOWN;
+	dimm->nr_pages = mem_info.mem_size >> PAGE_SHIFT;
+	dimm->grain = TILE_EDAC_ERROR_GRAIN;
+	dimm->dtype = DEV_UNKNOWN;
 
 	return 0;
 }
@@ -123,6 +125,7 @@ static int __devinit tile_edac_mc_probe(struct platform_device *pdev)
 	char			hv_file[32];
 	int			hv_devhdl;
 	struct mem_ctl_info	*mci;
+	struct edac_mc_layer	layers[2];
 	struct tile_edac_priv	*priv;
 	int			rc;
 
@@ -132,8 +135,14 @@ static int __devinit tile_edac_mc_probe(struct platform_device *pdev)
 		return -EINVAL;
 
 	/* A TILE MC has a single channel and one chip-select row. */
-	mci = edac_mc_alloc(sizeof(struct tile_edac_priv),
-		TILE_EDAC_NR_CSROWS, TILE_EDAC_NR_CHANS, pdev->id);
+	layers[0].type = EDAC_MC_LAYER_CHIP_SELECT;
+	layers[0].size = TILE_EDAC_NR_CSROWS;
+	layers[0].is_virt_csrow = true;
+	layers[1].type = EDAC_MC_LAYER_CHANNEL;
+	layers[1].size = TILE_EDAC_NR_CHANS;
+	layers[1].is_virt_csrow = false;
+	mci = edac_mc_alloc(pdev->id, ARRAY_SIZE(layers), layers,
+			    sizeof(struct tile_edac_priv));
 	if (mci == NULL)
 		return -ENOMEM;
 	priv = mci->pvt_info;
diff --git a/drivers/edac/x38_edac.c b/drivers/edac/x38_edac.c
index a438297389e..1ac7962d63e 100644
--- a/drivers/edac/x38_edac.c
+++ b/drivers/edac/x38_edac.c
@@ -215,19 +215,26 @@ static void x38_process_error_info(struct mem_ctl_info *mci,
 		return;
 
 	if ((info->errsts ^ info->errsts2) & X38_ERRSTS_BITS) {
-		edac_mc_handle_ce_no_info(mci, "UE overwrote CE");
+		edac_mc_handle_error(HW_EVENT_ERR_UNCORRECTED, mci, 0, 0, 0,
+				     -1, -1, -1,
+				     "UE overwrote CE", "", NULL);
 		info->errsts = info->errsts2;
 	}
 
 	for (channel = 0; channel < x38_channel_num; channel++) {
 		log = info->eccerrlog[channel];
 		if (log & X38_ECCERRLOG_UE) {
-			edac_mc_handle_ue(mci, 0, 0,
-				eccerrlog_row(channel, log), "x38 UE");
+			edac_mc_handle_error(HW_EVENT_ERR_UNCORRECTED, mci,
+					     0, 0, 0,
+					     eccerrlog_row(channel, log),
+					     -1, -1,
+					     "x38 UE", "", NULL);
 		} else if (log & X38_ECCERRLOG_CE) {
-			edac_mc_handle_ce(mci, 0, 0,
-				eccerrlog_syndrome(log),
-				eccerrlog_row(channel, log), 0, "x38 CE");
+			edac_mc_handle_error(HW_EVENT_ERR_CORRECTED, mci,
+					     0, 0, eccerrlog_syndrome(log),
+					     eccerrlog_row(channel, log),
+					     -1, -1,
+					     "x38 CE", "", NULL);
 		}
 	}
 }
@@ -317,9 +324,9 @@ static unsigned long drb_to_nr_pages(
 static int x38_probe1(struct pci_dev *pdev, int dev_idx)
 {
 	int rc;
-	int i;
+	int i, j;
 	struct mem_ctl_info *mci = NULL;
-	unsigned long last_page;
+	struct edac_mc_layer layers[2];
 	u16 drbs[X38_CHANNELS][X38_RANKS_PER_CHANNEL];
 	bool stacked;
 	void __iomem *window;
@@ -335,7 +342,13 @@ static int x38_probe1(struct pci_dev *pdev, int dev_idx)
 	how_many_channel(pdev);
 
 	/* FIXME: unconventional pvt_info usage */
-	mci = edac_mc_alloc(0, X38_RANKS, x38_channel_num, 0);
+	layers[0].type = EDAC_MC_LAYER_CHIP_SELECT;
+	layers[0].size = X38_RANKS;
+	layers[0].is_virt_csrow = true;
+	layers[1].type = EDAC_MC_LAYER_CHANNEL;
+	layers[1].size = x38_channel_num;
+	layers[1].is_virt_csrow = false;
+	mci = edac_mc_alloc(0, ARRAY_SIZE(layers), layers, 0);
 	if (!mci)
 		return -ENOMEM;
 
@@ -363,7 +376,6 @@ static int x38_probe1(struct pci_dev *pdev, int dev_idx)
 	 * cumulative; the last one will contain the total memory
 	 * contained in all ranks.
 	 */
-	last_page = -1UL;
 	for (i = 0; i < mci->nr_csrows; i++) {
 		unsigned long nr_pages;
 		struct csrow_info *csrow = &mci->csrows[i];
@@ -372,20 +384,18 @@ static int x38_probe1(struct pci_dev *pdev, int dev_idx)
 			i / X38_RANKS_PER_CHANNEL,
 			i % X38_RANKS_PER_CHANNEL);
 
-		if (nr_pages == 0) {
-			csrow->mtype = MEM_EMPTY;
+		if (nr_pages == 0)
 			continue;
-		}
 
-		csrow->first_page = last_page + 1;
-		last_page += nr_pages;
-		csrow->last_page = last_page;
-		csrow->nr_pages = nr_pages;
+		for (j = 0; j < x38_channel_num; j++) {
+			struct dimm_info *dimm = csrow->channels[j].dimm;
 
-		csrow->grain = nr_pages << PAGE_SHIFT;
-		csrow->mtype = MEM_DDR2;
-		csrow->dtype = DEV_UNKNOWN;
-		csrow->edac_mode = EDAC_UNKNOWN;
+			dimm->nr_pages = nr_pages / x38_channel_num;
+			dimm->grain = nr_pages << PAGE_SHIFT;
+			dimm->mtype = MEM_DDR2;
+			dimm->dtype = DEV_UNKNOWN;
+			dimm->edac_mode = EDAC_UNKNOWN;
+		}
 	}
 
 	x38_clear_error_info(mci);
diff --git a/drivers/extcon/Kconfig b/drivers/extcon/Kconfig
new file mode 100644
index 00000000000..29c5cf852ef
--- /dev/null
+++ b/drivers/extcon/Kconfig
@@ -0,0 +1,32 @@
+menuconfig EXTCON
+	tristate "External Connector Class (extcon) support"
+	help
+	  Say Y here to enable external connector class (extcon) support.
+	  This allows monitoring external connectors by userspace
+	  via sysfs and uevent and supports external connectors with
+	  multiple states; i.e., an extcon that may have multiple
+	  cables attached. For example, an external connector of a device
+	  may be used to connect an HDMI cable and a AC adaptor, and to
+	  host USB ports. Many of 30-pin connectors including PDMI are
+	  also good examples.
+
+if EXTCON
+
+comment "Extcon Device Drivers"
+
+config EXTCON_GPIO
+	tristate "GPIO extcon support"
+	depends on GENERIC_GPIO
+	help
+	  Say Y here to enable GPIO based extcon support. Note that GPIO
+	  extcon supports single state per extcon instance.
+
+config EXTCON_MAX8997
+	tristate "MAX8997 EXTCON Support"
+	depends on MFD_MAX8997
+	help
+	  If you say yes here you get support for the MUIC device of
+	  Maxim MAX8997 PMIC. The MAX8997 MUIC is a USB port accessory
+	  detector and switch.
+
+endif # MULTISTATE_SWITCH
diff --git a/drivers/extcon/Makefile b/drivers/extcon/Makefile
new file mode 100644
index 00000000000..86020bdb6da
--- /dev/null
+++ b/drivers/extcon/Makefile
@@ -0,0 +1,7 @@
+#
+# Makefile for external connector class (extcon) devices
+#
+
+obj-$(CONFIG_EXTCON)		+= extcon_class.o
+obj-$(CONFIG_EXTCON_GPIO)	+= extcon_gpio.o
+obj-$(CONFIG_EXTCON_MAX8997)	+= extcon-max8997.o
diff --git a/drivers/misc/max8997-muic.c b/drivers/extcon/extcon-max8997.c
index 19591eaa492..23416e44376 100644
--- a/drivers/misc/max8997-muic.c
+++ b/drivers/extcon/extcon-max8997.c
@@ -1,7 +1,7 @@
 /*
- * max8997-muic.c - MAX8997 muic driver for the Maxim 8997
+ * extcon-max8997.c - MAX8997 extcon driver to support MAX8997 MUIC
  *
- *  Copyright (C) 2011 Samsung Electrnoics
+ *  Copyright (C) 2012 Samsung Electrnoics
  *  Donggeun Kim <dg77.kim@samsung.com>
  *
  * This program is free software; you can redistribute it and/or modify
@@ -13,11 +13,6 @@
  * 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>
@@ -30,6 +25,9 @@
 #include <linux/kobject.h>
 #include <linux/mfd/max8997.h>
 #include <linux/mfd/max8997-private.h>
+#include <linux/extcon.h>
+
+#define	DEV_NAME			"max8997-muic"
 
 /* MAX8997-MUIC STATUS1 register */
 #define STATUS1_ADC_SHIFT		0
@@ -95,7 +93,6 @@ static struct max8997_muic_irq muic_irqs[] = {
 
 struct max8997_muic_info {
 	struct device *dev;
-	struct max8997_dev *iodev;
 	struct i2c_client *muic;
 	struct max8997_muic_platform_data *muic_pdata;
 
@@ -106,12 +103,28 @@ struct max8997_muic_info {
 	int pre_adc;
 
 	struct mutex mutex;
+
+	struct extcon_dev	*edev;
+};
+
+const char *max8997_extcon_cable[] = {
+	[0] = "USB",
+	[1] = "USB-Host",
+	[2] = "TA",
+	[3] = "Fast-charger",
+	[4] = "Slow-charger",
+	[5] = "Charge-downstream",
+	[6] = "MHL",
+	[7] = "Dock-desk",
+	[7] = "Dock-card",
+	[8] = "JIG",
+
+	NULL,
 };
 
 static int max8997_muic_handle_usb(struct max8997_muic_info *info,
 			enum max8997_muic_usb_type usb_type, bool attached)
 {
-	struct max8997_muic_platform_data *mdata = info->muic_pdata;
 	int ret = 0;
 
 	if (usb_type == MAX8997_USB_HOST) {
@@ -125,25 +138,25 @@ static int max8997_muic_handle_usb(struct max8997_muic_info *info,
 		}
 	}
 
-	if (mdata->usb_callback)
-		mdata->usb_callback(usb_type, attached);
+	switch (usb_type) {
+	case MAX8997_USB_HOST:
+		extcon_set_cable_state(info->edev, "USB-Host", attached);
+		break;
+	case MAX8997_USB_DEVICE:
+		extcon_set_cable_state(info->edev, "USB", attached);
+		break;
+	default:
+		ret = -EINVAL;
+		break;
+	}
+
 out:
 	return ret;
 }
 
-static void max8997_muic_handle_mhl(struct max8997_muic_info *info,
-			bool attached)
-{
-	struct max8997_muic_platform_data *mdata = info->muic_pdata;
-
-	if (mdata->mhl_callback)
-		mdata->mhl_callback(attached);
-}
-
 static int max8997_muic_handle_dock(struct max8997_muic_info *info,
 			int adc, bool attached)
 {
-	struct max8997_muic_platform_data *mdata = info->muic_pdata;
 	int ret = 0;
 
 	/* switch to AUDIO */
@@ -157,14 +170,13 @@ static int max8997_muic_handle_dock(struct max8997_muic_info *info,
 
 	switch (adc) {
 	case MAX8997_ADC_DESKDOCK:
-		if (mdata->deskdock_callback)
-			mdata->deskdock_callback(attached);
+		extcon_set_cable_state(info->edev, "Dock-desk", attached);
 		break;
 	case MAX8997_ADC_CARDOCK:
-		if (mdata->cardock_callback)
-			mdata->cardock_callback(attached);
+		extcon_set_cable_state(info->edev, "Dock-card", attached);
 		break;
 	default:
+		ret = -EINVAL;
 		break;
 	}
 out:
@@ -174,7 +186,6 @@ out:
 static int max8997_muic_handle_jig_uart(struct max8997_muic_info *info,
 			bool attached)
 {
-	struct max8997_muic_platform_data *mdata = info->muic_pdata;
 	int ret = 0;
 
 	/* switch to UART */
@@ -186,8 +197,7 @@ static int max8997_muic_handle_jig_uart(struct max8997_muic_info *info,
 		goto out;
 	}
 
-	if (mdata->uart_callback)
-		mdata->uart_callback(attached);
+	extcon_set_cable_state(info->edev, "JIG", attached);
 out:
 	return ret;
 }
@@ -201,7 +211,7 @@ static int max8997_muic_handle_adc_detach(struct max8997_muic_info *info)
 		ret = max8997_muic_handle_usb(info, MAX8997_USB_HOST, false);
 		break;
 	case MAX8997_ADC_MHL:
-		max8997_muic_handle_mhl(info, false);
+		extcon_set_cable_state(info->edev, "MHL", false);
 		break;
 	case MAX8997_ADC_JIG_USB_1:
 	case MAX8997_ADC_JIG_USB_2:
@@ -230,7 +240,7 @@ static int max8997_muic_handle_adc(struct max8997_muic_info *info, int adc)
 		ret = max8997_muic_handle_usb(info, MAX8997_USB_HOST, true);
 		break;
 	case MAX8997_ADC_MHL:
-		max8997_muic_handle_mhl(info, true);
+		extcon_set_cable_state(info->edev, "MHL", true);
 		break;
 	case MAX8997_ADC_JIG_USB_1:
 	case MAX8997_ADC_JIG_USB_2:
@@ -247,10 +257,40 @@ static int max8997_muic_handle_adc(struct max8997_muic_info *info, int adc)
 		ret = max8997_muic_handle_adc_detach(info);
 		break;
 	default:
-		break;
+		ret = -EINVAL;
+		goto out;
 	}
 
 	info->pre_adc = adc;
+out:
+	return ret;
+}
+
+static int max8997_muic_handle_charger_type_detach(
+				struct max8997_muic_info *info)
+{
+	int ret = 0;
+
+	switch (info->pre_charger_type) {
+	case MAX8997_CHARGER_TYPE_USB:
+		extcon_set_cable_state(info->edev, "USB", false);
+		break;
+	case MAX8997_CHARGER_TYPE_DOWNSTREAM_PORT:
+		extcon_set_cable_state(info->edev, "Charge-downstream", false);
+		break;
+	case MAX8997_CHARGER_TYPE_DEDICATED_CHG:
+		extcon_set_cable_state(info->edev, "TA", false);
+		break;
+	case MAX8997_CHARGER_TYPE_500MA:
+		extcon_set_cable_state(info->edev, "Slow-charger", false);
+		break;
+	case MAX8997_CHARGER_TYPE_1A:
+		extcon_set_cable_state(info->edev, "Fast-charger", false);
+		break;
+	default:
+		ret = -EINVAL;
+		break;
+	}
 
 	return ret;
 }
@@ -258,7 +298,6 @@ static int max8997_muic_handle_adc(struct max8997_muic_info *info, int adc)
 static int max8997_muic_handle_charger_type(struct max8997_muic_info *info,
 				enum max8997_muic_charger_type charger_type)
 {
-	struct max8997_muic_platform_data *mdata = info->muic_pdata;
 	u8 adc;
 	int ret;
 
@@ -270,30 +309,29 @@ static int max8997_muic_handle_charger_type(struct max8997_muic_info *info,
 
 	switch (charger_type) {
 	case MAX8997_CHARGER_TYPE_NONE:
-		if (mdata->charger_callback)
-			mdata->charger_callback(false, charger_type);
-		if (info->pre_charger_type == MAX8997_CHARGER_TYPE_USB) {
-			max8997_muic_handle_usb(info,
-					MAX8997_USB_DEVICE, false);
-		}
+		ret = max8997_muic_handle_charger_type_detach(info);
 		break;
 	case MAX8997_CHARGER_TYPE_USB:
 		if ((adc & STATUS1_ADC_MASK) == MAX8997_ADC_OPEN) {
 			max8997_muic_handle_usb(info,
 					MAX8997_USB_DEVICE, true);
 		}
-		if (mdata->charger_callback)
-			mdata->charger_callback(true, charger_type);
 		break;
 	case MAX8997_CHARGER_TYPE_DOWNSTREAM_PORT:
+		extcon_set_cable_state(info->edev, "Charge-downstream", true);
+		break;
 	case MAX8997_CHARGER_TYPE_DEDICATED_CHG:
+		extcon_set_cable_state(info->edev, "TA", true);
+		break;
 	case MAX8997_CHARGER_TYPE_500MA:
+		extcon_set_cable_state(info->edev, "Slow-charger", true);
+		break;
 	case MAX8997_CHARGER_TYPE_1A:
-		if (mdata->charger_callback)
-			mdata->charger_callback(true, charger_type);
+		extcon_set_cable_state(info->edev, "Fast-charger", true);
 		break;
 	default:
-		break;
+		ret = -EINVAL;
+		goto out;
 	}
 
 	info->pre_charger_type = charger_type;
@@ -305,18 +343,17 @@ static void max8997_muic_irq_work(struct work_struct *work)
 {
 	struct max8997_muic_info *info = container_of(work,
 			struct max8997_muic_info, irq_work);
-	struct max8997_platform_data *pdata =
-				dev_get_platdata(info->iodev->dev);
-	u8 status[3];
+	struct max8997_dev *max8997 = i2c_get_clientdata(info->muic);
+	u8 status[2];
 	u8 adc, chg_type;
 
-	int irq_type = info->irq - pdata->irq_base;
+	int irq_type = info->irq - max8997->irq_base;
 	int ret;
 
 	mutex_lock(&info->mutex);
 
 	ret = max8997_bulk_read(info->muic, MAX8997_MUIC_REG_STATUS1,
-				3, status);
+				2, status);
 	if (ret) {
 		dev_err(info->dev, "failed to read muic register\n");
 		mutex_unlock(&info->mutex);
@@ -340,8 +377,8 @@ static void max8997_muic_irq_work(struct work_struct *work)
 		max8997_muic_handle_charger_type(info, chg_type);
 		break;
 	default:
-		dev_info(info->dev, "misc interrupt: %s occurred\n",
-			 muic_irqs[irq_type].name);
+		dev_info(info->dev, "misc interrupt: irq %d occurred\n",
+				irq_type);
 		break;
 	}
 
@@ -387,21 +424,10 @@ static void max8997_muic_detect_dev(struct max8997_muic_info *info)
 	max8997_muic_handle_charger_type(info, chg_type);
 }
 
-static void max8997_initialize_device(struct max8997_muic_info *info)
-{
-	struct max8997_muic_platform_data *mdata = info->muic_pdata;
-	int i;
-
-	for (i = 0; i < mdata->num_init_data; i++) {
-		max8997_write_reg(info->muic, mdata->init_data[i].addr,
-				mdata->init_data[i].data);
-	}
-}
-
 static int __devinit max8997_muic_probe(struct platform_device *pdev)
 {
-	struct max8997_dev *iodev = dev_get_drvdata(pdev->dev.parent);
-	struct max8997_platform_data *pdata = dev_get_platdata(iodev->dev);
+	struct max8997_dev *max8997 = dev_get_drvdata(pdev->dev.parent);
+	struct max8997_platform_data *pdata = dev_get_platdata(max8997->dev);
 	struct max8997_muic_info *info;
 	int ret, i;
 
@@ -412,16 +438,8 @@ static int __devinit max8997_muic_probe(struct platform_device *pdev)
 		goto err_kfree;
 	}
 
-	if (!pdata->muic_pdata) {
-		dev_err(&pdev->dev, "failed to get platform_data\n");
-		ret = -EINVAL;
-		goto err_pdata;
-	}
-	info->muic_pdata = pdata->muic_pdata;
-
 	info->dev = &pdev->dev;
-	info->iodev = iodev;
-	info->muic = iodev->muic;
+	info->muic = max8997->muic;
 
 	platform_set_drvdata(pdev, info);
 	mutex_init(&info->mutex);
@@ -440,24 +458,45 @@ static int __devinit max8997_muic_probe(struct platform_device *pdev)
 				"failed: irq request (IRQ: %d,"
 				" error :%d)\n",
 				muic_irq->irq, ret);
-
-			for (i = i - 1; i >= 0; i--)
-				free_irq(muic_irq->irq, info);
-
 			goto err_irq;
 		}
 	}
 
+	/* External connector */
+	info->edev = kzalloc(sizeof(struct extcon_dev), GFP_KERNEL);
+	if (!info->edev) {
+		dev_err(&pdev->dev, "failed to allocate memory for extcon\n");
+		ret = -ENOMEM;
+		goto err_irq;
+	}
+	info->edev->name = DEV_NAME;
+	info->edev->supported_cable = max8997_extcon_cable;
+	ret = extcon_dev_register(info->edev, NULL);
+	if (ret) {
+		dev_err(&pdev->dev, "failed to register extcon device\n");
+		goto err_extcon;
+	}
+
 	/* Initialize registers according to platform data */
-	max8997_initialize_device(info);
+	if (pdata->muic_pdata) {
+		struct max8997_muic_platform_data *mdata = info->muic_pdata;
+
+		for (i = 0; i < mdata->num_init_data; i++) {
+			max8997_write_reg(info->muic, mdata->init_data[i].addr,
+					mdata->init_data[i].data);
+		}
+	}
 
 	/* Initial device detection */
 	max8997_muic_detect_dev(info);
 
 	return ret;
 
+err_extcon:
+	kfree(info->edev);
 err_irq:
-err_pdata:
+	while (--i >= 0)
+		free_irq(pdata->irq_base + muic_irqs[i].irq, info);
 	kfree(info);
 err_kfree:
 	return ret;
@@ -466,14 +505,15 @@ err_kfree:
 static int __devexit max8997_muic_remove(struct platform_device *pdev)
 {
 	struct max8997_muic_info *info = platform_get_drvdata(pdev);
-	struct max8997_platform_data *pdata =
-				dev_get_platdata(info->iodev->dev);
+	struct max8997_dev *max8997 = i2c_get_clientdata(info->muic);
 	int i;
 
 	for (i = 0; i < ARRAY_SIZE(muic_irqs); i++)
-		free_irq(pdata->irq_base + muic_irqs[i].irq, info);
+		free_irq(max8997->irq_base + muic_irqs[i].irq, info);
 	cancel_work_sync(&info->irq_work);
 
+	extcon_dev_unregister(info->edev);
+
 	kfree(info);
 
 	return 0;
@@ -481,7 +521,7 @@ static int __devexit max8997_muic_remove(struct platform_device *pdev)
 
 static struct platform_driver max8997_muic_driver = {
 	.driver		= {
-		.name	= "max8997-muic",
+		.name	= DEV_NAME,
 		.owner	= THIS_MODULE,
 	},
 	.probe		= max8997_muic_probe,
@@ -490,6 +530,6 @@ static struct platform_driver max8997_muic_driver = {
 
 module_platform_driver(max8997_muic_driver);
 
-MODULE_DESCRIPTION("Maxim MAX8997 MUIC driver");
+MODULE_DESCRIPTION("Maxim MAX8997 Extcon driver");
 MODULE_AUTHOR("Donggeun Kim <dg77.kim@samsung.com>");
 MODULE_LICENSE("GPL");
diff --git a/drivers/extcon/extcon_class.c b/drivers/extcon/extcon_class.c
new file mode 100644
index 00000000000..f598a700ec1
--- /dev/null
+++ b/drivers/extcon/extcon_class.c
@@ -0,0 +1,832 @@
+/*
+ *  drivers/extcon/extcon_class.c
+ *
+ *  External connector (extcon) class driver
+ *
+ * Copyright (C) 2012 Samsung Electronics
+ * Author: Donggeun Kim <dg77.kim@samsung.com>
+ * Author: MyungJoo Ham <myungjoo.ham@samsung.com>
+ *
+ * based on android/drivers/switch/switch_class.c
+ * Copyright (C) 2008 Google, Inc.
+ * Author: Mike Lockwood <lockwood@android.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/types.h>
+#include <linux/init.h>
+#include <linux/device.h>
+#include <linux/fs.h>
+#include <linux/err.h>
+#include <linux/extcon.h>
+#include <linux/slab.h>
+
+/*
+ * extcon_cable_name suggests the standard cable names for commonly used
+ * cable types.
+ *
+ * However, please do not use extcon_cable_name directly for extcon_dev
+ * struct's supported_cable pointer unless your device really supports
+ * every single port-type of the following cable names. Please choose cable
+ * names that are actually used in your extcon device.
+ */
+const char *extcon_cable_name[] = {
+	[EXTCON_USB]		= "USB",
+	[EXTCON_USB_HOST]	= "USB-Host",
+	[EXTCON_TA]		= "TA",
+	[EXTCON_FAST_CHARGER]	= "Fast-charger",
+	[EXTCON_SLOW_CHARGER]	= "Slow-charger",
+	[EXTCON_CHARGE_DOWNSTREAM]	= "Charge-downstream",
+	[EXTCON_HDMI]		= "HDMI",
+	[EXTCON_MHL]		= "MHL",
+	[EXTCON_DVI]		= "DVI",
+	[EXTCON_VGA]		= "VGA",
+	[EXTCON_DOCK]		= "Dock",
+	[EXTCON_LINE_IN]	= "Line-in",
+	[EXTCON_LINE_OUT]	= "Line-out",
+	[EXTCON_MIC_IN]		= "Microphone",
+	[EXTCON_HEADPHONE_OUT]	= "Headphone",
+	[EXTCON_SPDIF_IN]	= "SPDIF-in",
+	[EXTCON_SPDIF_OUT]	= "SPDIF-out",
+	[EXTCON_VIDEO_IN]	= "Video-in",
+	[EXTCON_VIDEO_OUT]	= "Video-out",
+	[EXTCON_MECHANICAL]	= "Mechanical",
+
+	NULL,
+};
+
+struct class *extcon_class;
+#if defined(CONFIG_ANDROID)
+static struct class_compat *switch_class;
+#endif /* CONFIG_ANDROID */
+
+static LIST_HEAD(extcon_dev_list);
+static DEFINE_MUTEX(extcon_dev_list_lock);
+
+/**
+ * check_mutually_exclusive - Check if new_state violates mutually_exclusive
+ *			    condition.
+ * @edev:	the extcon device
+ * @new_state:	new cable attach status for @edev
+ *
+ * Returns 0 if nothing violates. Returns the index + 1 for the first
+ * violated condition.
+ */
+static int check_mutually_exclusive(struct extcon_dev *edev, u32 new_state)
+{
+	int i = 0;
+
+	if (!edev->mutually_exclusive)
+		return 0;
+
+	for (i = 0; edev->mutually_exclusive[i]; i++) {
+		int count = 0, j;
+		u32 correspondants = new_state & edev->mutually_exclusive[i];
+		u32 exp = 1;
+
+		for (j = 0; j < 32; j++) {
+			if (exp & correspondants)
+				count++;
+			if (count > 1)
+				return i + 1;
+			exp <<= 1;
+		}
+	}
+
+	return 0;
+}
+
+static ssize_t state_show(struct device *dev, struct device_attribute *attr,
+			  char *buf)
+{
+	int i, count = 0;
+	struct extcon_dev *edev = (struct extcon_dev *) dev_get_drvdata(dev);
+
+	if (edev->print_state) {
+		int ret = edev->print_state(edev, buf);
+
+		if (ret >= 0)
+			return ret;
+		/* Use default if failed */
+	}
+
+	if (edev->max_supported == 0)
+		return sprintf(buf, "%u\n", edev->state);
+
+	for (i = 0; i < SUPPORTED_CABLE_MAX; i++) {
+		if (!edev->supported_cable[i])
+			break;
+		count += sprintf(buf + count, "%s=%d\n",
+				 edev->supported_cable[i],
+				 !!(edev->state & (1 << i)));
+	}
+
+	return count;
+}
+
+int extcon_set_state(struct extcon_dev *edev, u32 state);
+static ssize_t state_store(struct device *dev, struct device_attribute *attr,
+			   const char *buf, size_t count)
+{
+	u32 state;
+	ssize_t ret = 0;
+	struct extcon_dev *edev = (struct extcon_dev *) dev_get_drvdata(dev);
+
+	ret = sscanf(buf, "0x%x", &state);
+	if (ret == 0)
+		ret = -EINVAL;
+	else
+		ret = extcon_set_state(edev, state);
+
+	if (ret < 0)
+		return ret;
+
+	return count;
+}
+
+static ssize_t name_show(struct device *dev, struct device_attribute *attr,
+		char *buf)
+{
+	struct extcon_dev *edev = (struct extcon_dev *) dev_get_drvdata(dev);
+
+	/* Optional callback given by the user */
+	if (edev->print_name) {
+		int ret = edev->print_name(edev, buf);
+		if (ret >= 0)
+			return ret;
+	}
+
+	return sprintf(buf, "%s\n", dev_name(edev->dev));
+}
+
+static ssize_t cable_name_show(struct device *dev,
+			       struct device_attribute *attr, char *buf)
+{
+	struct extcon_cable *cable = container_of(attr, struct extcon_cable,
+						  attr_name);
+
+	return sprintf(buf, "%s\n",
+		       cable->edev->supported_cable[cable->cable_index]);
+}
+
+static ssize_t cable_state_show(struct device *dev,
+				struct device_attribute *attr, char *buf)
+{
+	struct extcon_cable *cable = container_of(attr, struct extcon_cable,
+						  attr_state);
+
+	return sprintf(buf, "%d\n",
+		       extcon_get_cable_state_(cable->edev,
+					       cable->cable_index));
+}
+
+static ssize_t cable_state_store(struct device *dev,
+				 struct device_attribute *attr, const char *buf,
+				 size_t count)
+{
+	struct extcon_cable *cable = container_of(attr, struct extcon_cable,
+						  attr_state);
+	int ret, state;
+
+	ret = sscanf(buf, "%d", &state);
+	if (ret == 0)
+		ret = -EINVAL;
+	else
+		ret = extcon_set_cable_state_(cable->edev, cable->cable_index,
+					      state);
+
+	if (ret < 0)
+		return ret;
+	return count;
+}
+
+/**
+ * extcon_update_state() - Update the cable attach states of the extcon device
+ *			only for the masked bits.
+ * @edev:	the extcon device
+ * @mask:	the bit mask to designate updated bits.
+ * @state:	new cable attach status for @edev
+ *
+ * Changing the state sends uevent with environment variable containing
+ * the name of extcon device (envp[0]) and the state output (envp[1]).
+ * Tizen uses this format for extcon device to get events from ports.
+ * Android uses this format as well.
+ *
+ * Note that the notifier provides which bits are changed in the state
+ * variable with the val parameter (second) to the callback.
+ */
+int extcon_update_state(struct extcon_dev *edev, u32 mask, u32 state)
+{
+	char name_buf[120];
+	char state_buf[120];
+	char *prop_buf;
+	char *envp[3];
+	int env_offset = 0;
+	int length;
+	unsigned long flags;
+
+	spin_lock_irqsave(&edev->lock, flags);
+
+	if (edev->state != ((edev->state & ~mask) | (state & mask))) {
+		u32 old_state = edev->state;
+
+		if (check_mutually_exclusive(edev, (edev->state & ~mask) |
+						   (state & mask))) {
+			spin_unlock_irqrestore(&edev->lock, flags);
+			return -EPERM;
+		}
+
+		edev->state &= ~mask;
+		edev->state |= state & mask;
+
+		raw_notifier_call_chain(&edev->nh, old_state, edev);
+
+		/* This could be in interrupt handler */
+		prop_buf = (char *)get_zeroed_page(GFP_ATOMIC);
+		if (prop_buf) {
+			length = name_show(edev->dev, NULL, prop_buf);
+			if (length > 0) {
+				if (prop_buf[length - 1] == '\n')
+					prop_buf[length - 1] = 0;
+				snprintf(name_buf, sizeof(name_buf),
+					"NAME=%s", prop_buf);
+				envp[env_offset++] = name_buf;
+			}
+			length = state_show(edev->dev, NULL, prop_buf);
+			if (length > 0) {
+				if (prop_buf[length - 1] == '\n')
+					prop_buf[length - 1] = 0;
+				snprintf(state_buf, sizeof(state_buf),
+					"STATE=%s", prop_buf);
+				envp[env_offset++] = state_buf;
+			}
+			envp[env_offset] = NULL;
+			/* Unlock early before uevent */
+			spin_unlock_irqrestore(&edev->lock, flags);
+
+			kobject_uevent_env(&edev->dev->kobj, KOBJ_CHANGE, envp);
+			free_page((unsigned long)prop_buf);
+		} else {
+			/* Unlock early before uevent */
+			spin_unlock_irqrestore(&edev->lock, flags);
+
+			dev_err(edev->dev, "out of memory in extcon_set_state\n");
+			kobject_uevent(&edev->dev->kobj, KOBJ_CHANGE);
+		}
+	} else {
+		/* No changes */
+		spin_unlock_irqrestore(&edev->lock, flags);
+	}
+
+	return 0;
+}
+EXPORT_SYMBOL_GPL(extcon_update_state);
+
+/**
+ * extcon_set_state() - Set the cable attach states of the extcon device.
+ * @edev:	the extcon device
+ * @state:	new cable attach status for @edev
+ *
+ * Note that notifier provides which bits are changed in the state
+ * variable with the val parameter (second) to the callback.
+ */
+int extcon_set_state(struct extcon_dev *edev, u32 state)
+{
+	return extcon_update_state(edev, 0xffffffff, state);
+}
+EXPORT_SYMBOL_GPL(extcon_set_state);
+
+/**
+ * extcon_find_cable_index() - Get the cable index based on the cable name.
+ * @edev:	the extcon device that has the cable.
+ * @cable_name:	cable name to be searched.
+ *
+ * Note that accessing a cable state based on cable_index is faster than
+ * cable_name because using cable_name induces a loop with strncmp().
+ * Thus, when get/set_cable_state is repeatedly used, using cable_index
+ * is recommended.
+ */
+int extcon_find_cable_index(struct extcon_dev *edev, const char *cable_name)
+{
+	int i;
+
+	if (edev->supported_cable) {
+		for (i = 0; edev->supported_cable[i]; i++) {
+			if (!strncmp(edev->supported_cable[i],
+				cable_name, CABLE_NAME_MAX))
+				return i;
+		}
+	}
+
+	return -EINVAL;
+}
+EXPORT_SYMBOL_GPL(extcon_find_cable_index);
+
+/**
+ * extcon_get_cable_state_() - Get the status of a specific cable.
+ * @edev:	the extcon device that has the cable.
+ * @index:	cable index that can be retrieved by extcon_find_cable_index().
+ */
+int extcon_get_cable_state_(struct extcon_dev *edev, int index)
+{
+	if (index < 0 || (edev->max_supported && edev->max_supported <= index))
+		return -EINVAL;
+
+	return !!(edev->state & (1 << index));
+}
+EXPORT_SYMBOL_GPL(extcon_get_cable_state_);
+
+/**
+ * extcon_get_cable_state() - Get the status of a specific cable.
+ * @edev:	the extcon device that has the cable.
+ * @cable_name:	cable name.
+ *
+ * Note that this is slower than extcon_get_cable_state_.
+ */
+int extcon_get_cable_state(struct extcon_dev *edev, const char *cable_name)
+{
+	return extcon_get_cable_state_(edev, extcon_find_cable_index
+						(edev, cable_name));
+}
+EXPORT_SYMBOL_GPL(extcon_get_cable_state);
+
+/**
+ * extcon_get_cable_state_() - Set the status of a specific cable.
+ * @edev:	the extcon device that has the cable.
+ * @index:	cable index that can be retrieved by extcon_find_cable_index().
+ * @cable_state:	the new cable status. The default semantics is
+ *			true: attached / false: detached.
+ */
+int extcon_set_cable_state_(struct extcon_dev *edev,
+			int index, bool cable_state)
+{
+	u32 state;
+
+	if (index < 0 || (edev->max_supported && edev->max_supported <= index))
+		return -EINVAL;
+
+	state = cable_state ? (1 << index) : 0;
+	return extcon_update_state(edev, 1 << index, state);
+}
+EXPORT_SYMBOL_GPL(extcon_set_cable_state_);
+
+/**
+ * extcon_get_cable_state() - Set the status of a specific cable.
+ * @edev:	the extcon device that has the cable.
+ * @cable_name:	cable name.
+ * @cable_state:	the new cable status. The default semantics is
+ *			true: attached / false: detached.
+ *
+ * Note that this is slower than extcon_set_cable_state_.
+ */
+int extcon_set_cable_state(struct extcon_dev *edev,
+			const char *cable_name, bool cable_state)
+{
+	return extcon_set_cable_state_(edev, extcon_find_cable_index
+					(edev, cable_name), cable_state);
+}
+EXPORT_SYMBOL_GPL(extcon_set_cable_state);
+
+/**
+ * extcon_get_extcon_dev() - Get the extcon device instance from the name
+ * @extcon_name:	The extcon name provided with extcon_dev_register()
+ */
+struct extcon_dev *extcon_get_extcon_dev(const char *extcon_name)
+{
+	struct extcon_dev *sd;
+
+	mutex_lock(&extcon_dev_list_lock);
+	list_for_each_entry(sd, &extcon_dev_list, entry) {
+		if (!strcmp(sd->name, extcon_name))
+			goto out;
+	}
+	sd = NULL;
+out:
+	mutex_unlock(&extcon_dev_list_lock);
+	return sd;
+}
+EXPORT_SYMBOL_GPL(extcon_get_extcon_dev);
+
+static int _call_per_cable(struct notifier_block *nb, unsigned long val,
+			   void *ptr)
+{
+	struct extcon_specific_cable_nb *obj = container_of(nb,
+			struct extcon_specific_cable_nb, internal_nb);
+	struct extcon_dev *edev = ptr;
+
+	if ((val & (1 << obj->cable_index)) !=
+	    (edev->state & (1 << obj->cable_index))) {
+		bool cable_state = true;
+
+		obj->previous_value = val;
+
+		if (val & (1 << obj->cable_index))
+			cable_state = false;
+
+		return obj->user_nb->notifier_call(obj->user_nb,
+				cable_state, ptr);
+	}
+
+	return NOTIFY_OK;
+}
+
+/**
+ * extcon_register_interest() - Register a notifier for a state change of a
+ *			      specific cable, not a entier set of cables of a
+ *			      extcon device.
+ * @obj:	an empty extcon_specific_cable_nb object to be returned.
+ * @extcon_name:	the name of extcon device.
+ * @cable_name:		the target cable name.
+ * @nb:		the notifier block to get notified.
+ *
+ * Provide an empty extcon_specific_cable_nb. extcon_register_interest() sets
+ * the struct for you.
+ *
+ * extcon_register_interest is a helper function for those who want to get
+ * notification for a single specific cable's status change. If a user wants
+ * to get notification for any changes of all cables of a extcon device,
+ * he/she should use the general extcon_register_notifier().
+ *
+ * Note that the second parameter given to the callback of nb (val) is
+ * "old_state", not the current state. The current state can be retrieved
+ * by looking at the third pameter (edev pointer)'s state value.
+ */
+int extcon_register_interest(struct extcon_specific_cable_nb *obj,
+			     const char *extcon_name, const char *cable_name,
+			     struct notifier_block *nb)
+{
+	if (!obj || !extcon_name || !cable_name || !nb)
+		return -EINVAL;
+
+	obj->edev = extcon_get_extcon_dev(extcon_name);
+	if (!obj->edev)
+		return -ENODEV;
+
+	obj->cable_index = extcon_find_cable_index(obj->edev, cable_name);
+	if (obj->cable_index < 0)
+		return -ENODEV;
+
+	obj->user_nb = nb;
+
+	obj->internal_nb.notifier_call = _call_per_cable;
+
+	return raw_notifier_chain_register(&obj->edev->nh, &obj->internal_nb);
+}
+
+/**
+ * extcon_unregister_interest() - Unregister the notifier registered by
+ *				extcon_register_interest().
+ * @obj:	the extcon_specific_cable_nb object returned by
+ *		extcon_register_interest().
+ */
+int extcon_unregister_interest(struct extcon_specific_cable_nb *obj)
+{
+	if (!obj)
+		return -EINVAL;
+
+	return raw_notifier_chain_unregister(&obj->edev->nh, &obj->internal_nb);
+}
+
+/**
+ * extcon_register_notifier() - Register a notifee to get notified by
+ *			      any attach status changes from the extcon.
+ * @edev:	the extcon device.
+ * @nb:		a notifier block to be registered.
+ *
+ * Note that the second parameter given to the callback of nb (val) is
+ * "old_state", not the current state. The current state can be retrieved
+ * by looking at the third pameter (edev pointer)'s state value.
+ */
+int extcon_register_notifier(struct extcon_dev *edev,
+			struct notifier_block *nb)
+{
+	return raw_notifier_chain_register(&edev->nh, nb);
+}
+EXPORT_SYMBOL_GPL(extcon_register_notifier);
+
+/**
+ * extcon_unregister_notifier() - Unregister a notifee from the extcon device.
+ * @edev:	the extcon device.
+ * @nb:		a registered notifier block to be unregistered.
+ */
+int extcon_unregister_notifier(struct extcon_dev *edev,
+			struct notifier_block *nb)
+{
+	return raw_notifier_chain_unregister(&edev->nh, nb);
+}
+EXPORT_SYMBOL_GPL(extcon_unregister_notifier);
+
+static struct device_attribute extcon_attrs[] = {
+	__ATTR(state, S_IRUGO | S_IWUSR, state_show, state_store),
+	__ATTR_RO(name),
+	__ATTR_NULL,
+};
+
+static int create_extcon_class(void)
+{
+	if (!extcon_class) {
+		extcon_class = class_create(THIS_MODULE, "extcon");
+		if (IS_ERR(extcon_class))
+			return PTR_ERR(extcon_class);
+		extcon_class->dev_attrs = extcon_attrs;
+
+#if defined(CONFIG_ANDROID)
+		switch_class = class_compat_register("switch");
+		if (WARN(!switch_class, "cannot allocate"))
+			return -ENOMEM;
+#endif /* CONFIG_ANDROID */
+	}
+
+	return 0;
+}
+
+static void extcon_cleanup(struct extcon_dev *edev, bool skip)
+{
+	mutex_lock(&extcon_dev_list_lock);
+	list_del(&edev->entry);
+	mutex_unlock(&extcon_dev_list_lock);
+
+	if (!skip && get_device(edev->dev)) {
+		int index;
+
+		if (edev->mutually_exclusive && edev->max_supported) {
+			for (index = 0; edev->mutually_exclusive[index];
+			     index++)
+				kfree(edev->d_attrs_muex[index].attr.name);
+			kfree(edev->d_attrs_muex);
+			kfree(edev->attrs_muex);
+		}
+
+		for (index = 0; index < edev->max_supported; index++)
+			kfree(edev->cables[index].attr_g.name);
+
+		if (edev->max_supported) {
+			kfree(edev->extcon_dev_type.groups);
+			kfree(edev->cables);
+		}
+
+		device_unregister(edev->dev);
+		put_device(edev->dev);
+	}
+
+	kfree(edev->dev);
+}
+
+static void extcon_dev_release(struct device *dev)
+{
+	struct extcon_dev *edev = (struct extcon_dev *) dev_get_drvdata(dev);
+
+	extcon_cleanup(edev, true);
+}
+
+static const char *muex_name = "mutually_exclusive";
+static void dummy_sysfs_dev_release(struct device *dev)
+{
+}
+
+/**
+ * extcon_dev_register() - Register a new extcon device
+ * @edev	: the new extcon device (should be allocated before calling)
+ * @dev		: the parent device for this extcon device.
+ *
+ * Among the members of edev struct, please set the "user initializing data"
+ * in any case and set the "optional callbacks" if required. However, please
+ * do not set the values of "internal data", which are initialized by
+ * this function.
+ */
+int extcon_dev_register(struct extcon_dev *edev, struct device *dev)
+{
+	int ret, index = 0;
+
+	if (!extcon_class) {
+		ret = create_extcon_class();
+		if (ret < 0)
+			return ret;
+	}
+
+	if (edev->supported_cable) {
+		/* Get size of array */
+		for (index = 0; edev->supported_cable[index]; index++)
+			;
+		edev->max_supported = index;
+	} else {
+		edev->max_supported = 0;
+	}
+
+	if (index > SUPPORTED_CABLE_MAX) {
+		dev_err(edev->dev, "extcon: maximum number of supported cables exceeded.\n");
+		return -EINVAL;
+	}
+
+	edev->dev = kzalloc(sizeof(struct device), GFP_KERNEL);
+	if (!edev->dev)
+		return -ENOMEM;
+	edev->dev->parent = dev;
+	edev->dev->class = extcon_class;
+	edev->dev->release = extcon_dev_release;
+
+	dev_set_name(edev->dev, edev->name ? edev->name : dev_name(dev));
+
+	if (edev->max_supported) {
+		char buf[10];
+		char *str;
+		struct extcon_cable *cable;
+
+		edev->cables = kzalloc(sizeof(struct extcon_cable) *
+				       edev->max_supported, GFP_KERNEL);
+		if (!edev->cables) {
+			ret = -ENOMEM;
+			goto err_sysfs_alloc;
+		}
+		for (index = 0; index < edev->max_supported; index++) {
+			cable = &edev->cables[index];
+
+			snprintf(buf, 10, "cable.%d", index);
+			str = kzalloc(sizeof(char) * (strlen(buf) + 1),
+				      GFP_KERNEL);
+			if (!str) {
+				for (index--; index >= 0; index--) {
+					cable = &edev->cables[index];
+					kfree(cable->attr_g.name);
+				}
+				ret = -ENOMEM;
+
+				goto err_alloc_cables;
+			}
+			strcpy(str, buf);
+
+			cable->edev = edev;
+			cable->cable_index = index;
+			cable->attrs[0] = &cable->attr_name.attr;
+			cable->attrs[1] = &cable->attr_state.attr;
+			cable->attrs[2] = NULL;
+			cable->attr_g.name = str;
+			cable->attr_g.attrs = cable->attrs;
+
+			cable->attr_name.attr.name = "name";
+			cable->attr_name.attr.mode = 0444;
+			cable->attr_name.show = cable_name_show;
+
+			cable->attr_state.attr.name = "state";
+			cable->attr_state.attr.mode = 0644;
+			cable->attr_state.show = cable_state_show;
+			cable->attr_state.store = cable_state_store;
+		}
+	}
+
+	if (edev->max_supported && edev->mutually_exclusive) {
+		char buf[80];
+		char *name;
+
+		/* Count the size of mutually_exclusive array */
+		for (index = 0; edev->mutually_exclusive[index]; index++)
+			;
+
+		edev->attrs_muex = kzalloc(sizeof(struct attribute *) *
+					   (index + 1), GFP_KERNEL);
+		if (!edev->attrs_muex) {
+			ret = -ENOMEM;
+			goto err_muex;
+		}
+
+		edev->d_attrs_muex = kzalloc(sizeof(struct device_attribute) *
+					     index, GFP_KERNEL);
+		if (!edev->d_attrs_muex) {
+			ret = -ENOMEM;
+			kfree(edev->attrs_muex);
+			goto err_muex;
+		}
+
+		for (index = 0; edev->mutually_exclusive[index]; index++) {
+			sprintf(buf, "0x%x", edev->mutually_exclusive[index]);
+			name = kzalloc(sizeof(char) * (strlen(buf) + 1),
+				       GFP_KERNEL);
+			if (!name) {
+				for (index--; index >= 0; index--) {
+					kfree(edev->d_attrs_muex[index].attr.
+					      name);
+				}
+				kfree(edev->d_attrs_muex);
+				kfree(edev->attrs_muex);
+				ret = -ENOMEM;
+				goto err_muex;
+			}
+			strcpy(name, buf);
+			edev->d_attrs_muex[index].attr.name = name;
+			edev->d_attrs_muex[index].attr.mode = 0000;
+			edev->attrs_muex[index] = &edev->d_attrs_muex[index]
+							.attr;
+		}
+		edev->attr_g_muex.name = muex_name;
+		edev->attr_g_muex.attrs = edev->attrs_muex;
+
+	}
+
+	if (edev->max_supported) {
+		edev->extcon_dev_type.groups =
+			kzalloc(sizeof(struct attribute_group *) *
+				(edev->max_supported + 2), GFP_KERNEL);
+		if (!edev->extcon_dev_type.groups) {
+			ret = -ENOMEM;
+			goto err_alloc_groups;
+		}
+
+		edev->extcon_dev_type.name = dev_name(edev->dev);
+		edev->extcon_dev_type.release = dummy_sysfs_dev_release;
+
+		for (index = 0; index < edev->max_supported; index++)
+			edev->extcon_dev_type.groups[index] =
+				&edev->cables[index].attr_g;
+		if (edev->mutually_exclusive)
+			edev->extcon_dev_type.groups[index] =
+				&edev->attr_g_muex;
+
+		edev->dev->type = &edev->extcon_dev_type;
+	}
+
+	ret = device_register(edev->dev);
+	if (ret) {
+		put_device(edev->dev);
+		goto err_dev;
+	}
+#if defined(CONFIG_ANDROID)
+	if (switch_class)
+		ret = class_compat_create_link(switch_class, edev->dev,
+					       dev);
+#endif /* CONFIG_ANDROID */
+
+	spin_lock_init(&edev->lock);
+
+	RAW_INIT_NOTIFIER_HEAD(&edev->nh);
+
+	dev_set_drvdata(edev->dev, edev);
+	edev->state = 0;
+
+	mutex_lock(&extcon_dev_list_lock);
+	list_add(&edev->entry, &extcon_dev_list);
+	mutex_unlock(&extcon_dev_list_lock);
+
+	return 0;
+
+err_dev:
+	if (edev->max_supported)
+		kfree(edev->extcon_dev_type.groups);
+err_alloc_groups:
+	if (edev->max_supported && edev->mutually_exclusive) {
+		for (index = 0; edev->mutually_exclusive[index]; index++)
+			kfree(edev->d_attrs_muex[index].attr.name);
+		kfree(edev->d_attrs_muex);
+		kfree(edev->attrs_muex);
+	}
+err_muex:
+	for (index = 0; index < edev->max_supported; index++)
+		kfree(edev->cables[index].attr_g.name);
+err_alloc_cables:
+	if (edev->max_supported)
+		kfree(edev->cables);
+err_sysfs_alloc:
+	kfree(edev->dev);
+	return ret;
+}
+EXPORT_SYMBOL_GPL(extcon_dev_register);
+
+/**
+ * extcon_dev_unregister() - Unregister the extcon device.
+ * @edev:	the extcon device instance to be unregitered.
+ *
+ * Note that this does not call kfree(edev) because edev was not allocated
+ * by this class.
+ */
+void extcon_dev_unregister(struct extcon_dev *edev)
+{
+	extcon_cleanup(edev, false);
+}
+EXPORT_SYMBOL_GPL(extcon_dev_unregister);
+
+static int __init extcon_class_init(void)
+{
+	return create_extcon_class();
+}
+module_init(extcon_class_init);
+
+static void __exit extcon_class_exit(void)
+{
+	class_destroy(extcon_class);
+}
+module_exit(extcon_class_exit);
+
+MODULE_AUTHOR("Mike Lockwood <lockwood@android.com>");
+MODULE_AUTHOR("Donggeun Kim <dg77.kim@samsung.com>");
+MODULE_AUTHOR("MyungJoo Ham <myungjoo.ham@samsung.com>");
+MODULE_DESCRIPTION("External connector (extcon) class driver");
+MODULE_LICENSE("GPL");
diff --git a/drivers/extcon/extcon_gpio.c b/drivers/extcon/extcon_gpio.c
new file mode 100644
index 00000000000..fe7a07b4733
--- /dev/null
+++ b/drivers/extcon/extcon_gpio.c
@@ -0,0 +1,169 @@
+/*
+ *  drivers/extcon/extcon_gpio.c
+ *
+ *  Single-state GPIO extcon driver based on extcon class
+ *
+ * Copyright (C) 2008 Google, Inc.
+ * Author: Mike Lockwood <lockwood@android.com>
+ *
+ * Modified by MyungJoo Ham <myungjoo.ham@samsung.com> to support extcon
+ * (originally switch class is supported)
+ *
+ * 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/kernel.h>
+#include <linux/init.h>
+#include <linux/interrupt.h>
+#include <linux/platform_device.h>
+#include <linux/slab.h>
+#include <linux/extcon.h>
+#include <linux/workqueue.h>
+#include <linux/gpio.h>
+#include <linux/extcon.h>
+#include <linux/extcon/extcon_gpio.h>
+
+struct gpio_extcon_data {
+	struct extcon_dev edev;
+	unsigned gpio;
+	const char *state_on;
+	const char *state_off;
+	int irq;
+	struct delayed_work work;
+	unsigned long debounce_jiffies;
+};
+
+static void gpio_extcon_work(struct work_struct *work)
+{
+	int state;
+	struct gpio_extcon_data	*data =
+		container_of(to_delayed_work(work), struct gpio_extcon_data,
+			     work);
+
+	state = gpio_get_value(data->gpio);
+	extcon_set_state(&data->edev, state);
+}
+
+static irqreturn_t gpio_irq_handler(int irq, void *dev_id)
+{
+	struct gpio_extcon_data *extcon_data = dev_id;
+
+	schedule_delayed_work(&extcon_data->work,
+			      extcon_data->debounce_jiffies);
+	return IRQ_HANDLED;
+}
+
+static ssize_t extcon_gpio_print_state(struct extcon_dev *edev, char *buf)
+{
+	struct gpio_extcon_data	*extcon_data =
+		container_of(edev, struct gpio_extcon_data, edev);
+	const char *state;
+	if (extcon_get_state(edev))
+		state = extcon_data->state_on;
+	else
+		state = extcon_data->state_off;
+
+	if (state)
+		return sprintf(buf, "%s\n", state);
+	return -EINVAL;
+}
+
+static int __devinit gpio_extcon_probe(struct platform_device *pdev)
+{
+	struct gpio_extcon_platform_data *pdata = pdev->dev.platform_data;
+	struct gpio_extcon_data *extcon_data;
+	int ret = 0;
+
+	if (!pdata)
+		return -EBUSY;
+	if (!pdata->irq_flags) {
+		dev_err(&pdev->dev, "IRQ flag is not specified.\n");
+		return -EINVAL;
+	}
+
+	extcon_data = devm_kzalloc(&pdev->dev, sizeof(struct gpio_extcon_data),
+				   GFP_KERNEL);
+	if (!extcon_data)
+		return -ENOMEM;
+
+	extcon_data->edev.name = pdata->name;
+	extcon_data->gpio = pdata->gpio;
+	extcon_data->state_on = pdata->state_on;
+	extcon_data->state_off = pdata->state_off;
+	if (pdata->state_on && pdata->state_off)
+		extcon_data->edev.print_state = extcon_gpio_print_state;
+	extcon_data->debounce_jiffies = msecs_to_jiffies(pdata->debounce);
+
+	ret = extcon_dev_register(&extcon_data->edev, &pdev->dev);
+	if (ret < 0)
+		goto err_extcon_dev_register;
+
+	ret = gpio_request_one(extcon_data->gpio, GPIOF_DIR_IN, pdev->name);
+	if (ret < 0)
+		goto err_request_gpio;
+
+	INIT_DELAYED_WORK(&extcon_data->work, gpio_extcon_work);
+
+	extcon_data->irq = gpio_to_irq(extcon_data->gpio);
+	if (extcon_data->irq < 0) {
+		ret = extcon_data->irq;
+		goto err_detect_irq_num_failed;
+	}
+
+	ret = request_any_context_irq(extcon_data->irq, gpio_irq_handler,
+				      pdata->irq_flags, pdev->name,
+				      extcon_data);
+	if (ret < 0)
+		goto err_request_irq;
+
+	/* Perform initial detection */
+	gpio_extcon_work(&extcon_data->work.work);
+
+	return 0;
+
+err_request_irq:
+err_detect_irq_num_failed:
+	gpio_free(extcon_data->gpio);
+err_request_gpio:
+	extcon_dev_unregister(&extcon_data->edev);
+err_extcon_dev_register:
+	devm_kfree(&pdev->dev, extcon_data);
+
+	return ret;
+}
+
+static int __devexit gpio_extcon_remove(struct platform_device *pdev)
+{
+	struct gpio_extcon_data *extcon_data = platform_get_drvdata(pdev);
+
+	cancel_delayed_work_sync(&extcon_data->work);
+	gpio_free(extcon_data->gpio);
+	extcon_dev_unregister(&extcon_data->edev);
+	devm_kfree(&pdev->dev, extcon_data);
+
+	return 0;
+}
+
+static struct platform_driver gpio_extcon_driver = {
+	.probe		= gpio_extcon_probe,
+	.remove		= __devexit_p(gpio_extcon_remove),
+	.driver		= {
+		.name	= "extcon-gpio",
+		.owner	= THIS_MODULE,
+	},
+};
+
+module_platform_driver(gpio_extcon_driver);
+
+MODULE_AUTHOR("Mike Lockwood <lockwood@android.com>");
+MODULE_DESCRIPTION("GPIO extcon driver");
+MODULE_LICENSE("GPL");
diff --git a/drivers/firewire/core-card.c b/drivers/firewire/core-card.c
index cc595eba7ba..57ea7f46417 100644
--- a/drivers/firewire/core-card.c
+++ b/drivers/firewire/core-card.c
@@ -421,8 +421,8 @@ static void bm_work(struct work_struct *work)
 			 * root, and thus, IRM.
 			 */
 			new_root_id = local_id;
-			fw_notice(card, "%s, making local node (%02x) root\n",
-				  "BM lock failed", new_root_id);
+			fw_notice(card, "BM lock failed (%s), making local node (%02x) root\n",
+				  fw_rcode_string(rcode), new_root_id);
 			goto pick_me;
 		}
 	} else if (card->bm_generation != generation) {
@@ -676,6 +676,7 @@ void fw_card_release(struct kref *kref)
 
 	complete(&card->done);
 }
+EXPORT_SYMBOL_GPL(fw_card_release);
 
 void fw_core_remove_card(struct fw_card *card)
 {
diff --git a/drivers/firewire/core-cdev.c b/drivers/firewire/core-cdev.c
index 2e6b24547e2..2783f69dada 100644
--- a/drivers/firewire/core-cdev.c
+++ b/drivers/firewire/core-cdev.c
@@ -22,6 +22,7 @@
 #include <linux/compat.h>
 #include <linux/delay.h>
 #include <linux/device.h>
+#include <linux/dma-mapping.h>
 #include <linux/errno.h>
 #include <linux/firewire.h>
 #include <linux/firewire-cdev.h>
@@ -70,6 +71,7 @@ struct client {
 	u64 iso_closure;
 	struct fw_iso_buffer buffer;
 	unsigned long vm_start;
+	bool buffer_is_mapped;
 
 	struct list_head phy_receiver_link;
 	u64 phy_receiver_closure;
@@ -959,11 +961,20 @@ static void iso_mc_callback(struct fw_iso_context *context,
 		    sizeof(e->interrupt), NULL, 0);
 }
 
+static enum dma_data_direction iso_dma_direction(struct fw_iso_context *context)
+{
+		if (context->type == FW_ISO_CONTEXT_TRANSMIT)
+			return DMA_TO_DEVICE;
+		else
+			return DMA_FROM_DEVICE;
+}
+
 static int ioctl_create_iso_context(struct client *client, union ioctl_arg *arg)
 {
 	struct fw_cdev_create_iso_context *a = &arg->create_iso_context;
 	struct fw_iso_context *context;
 	fw_iso_callback_t cb;
+	int ret;
 
 	BUILD_BUG_ON(FW_CDEV_ISO_CONTEXT_TRANSMIT != FW_ISO_CONTEXT_TRANSMIT ||
 		     FW_CDEV_ISO_CONTEXT_RECEIVE  != FW_ISO_CONTEXT_RECEIVE  ||
@@ -1004,8 +1015,21 @@ static int ioctl_create_iso_context(struct client *client, union ioctl_arg *arg)
 	if (client->iso_context != NULL) {
 		spin_unlock_irq(&client->lock);
 		fw_iso_context_destroy(context);
+
 		return -EBUSY;
 	}
+	if (!client->buffer_is_mapped) {
+		ret = fw_iso_buffer_map_dma(&client->buffer,
+					    client->device->card,
+					    iso_dma_direction(context));
+		if (ret < 0) {
+			spin_unlock_irq(&client->lock);
+			fw_iso_context_destroy(context);
+
+			return ret;
+		}
+		client->buffer_is_mapped = true;
+	}
 	client->iso_closure = a->closure;
 	client->iso_context = context;
 	spin_unlock_irq(&client->lock);
@@ -1651,7 +1675,6 @@ static long fw_device_op_compat_ioctl(struct file *file,
 static int fw_device_op_mmap(struct file *file, struct vm_area_struct *vma)
 {
 	struct client *client = file->private_data;
-	enum dma_data_direction direction;
 	unsigned long size;
 	int page_count, ret;
 
@@ -1674,20 +1697,28 @@ static int fw_device_op_mmap(struct file *file, struct vm_area_struct *vma)
 	if (size & ~PAGE_MASK)
 		return -EINVAL;
 
-	if (vma->vm_flags & VM_WRITE)
-		direction = DMA_TO_DEVICE;
-	else
-		direction = DMA_FROM_DEVICE;
-
-	ret = fw_iso_buffer_init(&client->buffer, client->device->card,
-				 page_count, direction);
+	ret = fw_iso_buffer_alloc(&client->buffer, page_count);
 	if (ret < 0)
 		return ret;
 
-	ret = fw_iso_buffer_map(&client->buffer, vma);
+	spin_lock_irq(&client->lock);
+	if (client->iso_context) {
+		ret = fw_iso_buffer_map_dma(&client->buffer,
+				client->device->card,
+				iso_dma_direction(client->iso_context));
+		client->buffer_is_mapped = (ret == 0);
+	}
+	spin_unlock_irq(&client->lock);
 	if (ret < 0)
-		fw_iso_buffer_destroy(&client->buffer, client->device->card);
+		goto fail;
 
+	ret = fw_iso_buffer_map_vma(&client->buffer, vma);
+	if (ret < 0)
+		goto fail;
+
+	return 0;
+ fail:
+	fw_iso_buffer_destroy(&client->buffer, client->device->card);
 	return ret;
 }
 
diff --git a/drivers/firewire/core-device.c b/drivers/firewire/core-device.c
index 68109e9bb04..4d460ef8716 100644
--- a/drivers/firewire/core-device.c
+++ b/drivers/firewire/core-device.c
@@ -481,6 +481,7 @@ static int read_rom(struct fw_device *device,
  * generation changes under us, read_config_rom will fail and get retried.
  * It's better to start all over in this case because the node from which we
  * are reading the ROM may have changed the ROM during the reset.
+ * Returns either a result code or a negative error code.
  */
 static int read_config_rom(struct fw_device *device, int generation)
 {
@@ -488,7 +489,7 @@ static int read_config_rom(struct fw_device *device, int generation)
 	const u32 *old_rom, *new_rom;
 	u32 *rom, *stack;
 	u32 sp, key;
-	int i, end, length, ret = -1;
+	int i, end, length, ret;
 
 	rom = kmalloc(sizeof(*rom) * MAX_CONFIG_ROM_SIZE +
 		      sizeof(*stack) * MAX_CONFIG_ROM_SIZE, GFP_KERNEL);
@@ -502,18 +503,21 @@ static int read_config_rom(struct fw_device *device, int generation)
 
 	/* First read the bus info block. */
 	for (i = 0; i < 5; i++) {
-		if (read_rom(device, generation, i, &rom[i]) != RCODE_COMPLETE)
+		ret = read_rom(device, generation, i, &rom[i]);
+		if (ret != RCODE_COMPLETE)
 			goto out;
 		/*
-		 * As per IEEE1212 7.2, during power-up, devices can
+		 * As per IEEE1212 7.2, during initialization, devices can
 		 * reply with a 0 for the first quadlet of the config
 		 * rom to indicate that they are booting (for example,
 		 * if the firmware is on the disk of a external
 		 * harddisk).  In that case we just fail, and the
 		 * retry mechanism will try again later.
 		 */
-		if (i == 0 && rom[i] == 0)
+		if (i == 0 && rom[i] == 0) {
+			ret = RCODE_BUSY;
 			goto out;
+		}
 	}
 
 	device->max_speed = device->node->max_speed;
@@ -563,11 +567,14 @@ static int read_config_rom(struct fw_device *device, int generation)
 		 */
 		key = stack[--sp];
 		i = key & 0xffffff;
-		if (WARN_ON(i >= MAX_CONFIG_ROM_SIZE))
+		if (WARN_ON(i >= MAX_CONFIG_ROM_SIZE)) {
+			ret = -ENXIO;
 			goto out;
+		}
 
 		/* Read header quadlet for the block to get the length. */
-		if (read_rom(device, generation, i, &rom[i]) != RCODE_COMPLETE)
+		ret = read_rom(device, generation, i, &rom[i]);
+		if (ret != RCODE_COMPLETE)
 			goto out;
 		end = i + (rom[i] >> 16) + 1;
 		if (end > MAX_CONFIG_ROM_SIZE) {
@@ -590,8 +597,8 @@ static int read_config_rom(struct fw_device *device, int generation)
 		 * it references another block, and push it in that case.
 		 */
 		for (; i < end; i++) {
-			if (read_rom(device, generation, i, &rom[i]) !=
-			    RCODE_COMPLETE)
+			ret = read_rom(device, generation, i, &rom[i]);
+			if (ret != RCODE_COMPLETE)
 				goto out;
 
 			if ((key >> 30) != 3 || (rom[i] >> 30) < 2)
@@ -619,8 +626,10 @@ static int read_config_rom(struct fw_device *device, int generation)
 
 	old_rom = device->config_rom;
 	new_rom = kmemdup(rom, length * 4, GFP_KERNEL);
-	if (new_rom == NULL)
+	if (new_rom == NULL) {
+		ret = -ENOMEM;
 		goto out;
+	}
 
 	down_write(&fw_device_rwsem);
 	device->config_rom = new_rom;
@@ -628,7 +637,7 @@ static int read_config_rom(struct fw_device *device, int generation)
 	up_write(&fw_device_rwsem);
 
 	kfree(old_rom);
-	ret = 0;
+	ret = RCODE_COMPLETE;
 	device->max_rec	= rom[2] >> 12 & 0xf;
 	device->cmc	= rom[2] >> 30 & 1;
 	device->irmc	= rom[2] >> 31 & 1;
@@ -967,15 +976,17 @@ static void fw_device_init(struct work_struct *work)
 	 * device.
 	 */
 
-	if (read_config_rom(device, device->generation) < 0) {
+	ret = read_config_rom(device, device->generation);
+	if (ret != RCODE_COMPLETE) {
 		if (device->config_rom_retries < MAX_RETRIES &&
 		    atomic_read(&device->state) == FW_DEVICE_INITIALIZING) {
 			device->config_rom_retries++;
 			fw_schedule_device_work(device, RETRY_DELAY);
 		} else {
 			if (device->node->link_on)
-				fw_notice(card, "giving up on Config ROM for node id %x\n",
-					  device->node_id);
+				fw_notice(card, "giving up on node %x: reading config rom failed: %s\n",
+					  device->node_id,
+					  fw_rcode_string(ret));
 			if (device->node == card->root_node)
 				fw_schedule_bm_work(card, 0);
 			fw_device_release(&device->device);
@@ -1069,31 +1080,30 @@ static void fw_device_init(struct work_struct *work)
 	put_device(&device->device);	/* our reference */
 }
 
-enum {
-	REREAD_BIB_ERROR,
-	REREAD_BIB_GONE,
-	REREAD_BIB_UNCHANGED,
-	REREAD_BIB_CHANGED,
-};
-
 /* Reread and compare bus info block and header of root directory */
-static int reread_config_rom(struct fw_device *device, int generation)
+static int reread_config_rom(struct fw_device *device, int generation,
+			     bool *changed)
 {
 	u32 q;
-	int i;
+	int i, rcode;
 
 	for (i = 0; i < 6; i++) {
-		if (read_rom(device, generation, i, &q) != RCODE_COMPLETE)
-			return REREAD_BIB_ERROR;
+		rcode = read_rom(device, generation, i, &q);
+		if (rcode != RCODE_COMPLETE)
+			return rcode;
 
 		if (i == 0 && q == 0)
-			return REREAD_BIB_GONE;
+			/* inaccessible (see read_config_rom); retry later */
+			return RCODE_BUSY;
 
-		if (q != device->config_rom[i])
-			return REREAD_BIB_CHANGED;
+		if (q != device->config_rom[i]) {
+			*changed = true;
+			return RCODE_COMPLETE;
+		}
 	}
 
-	return REREAD_BIB_UNCHANGED;
+	*changed = false;
+	return RCODE_COMPLETE;
 }
 
 static void fw_device_refresh(struct work_struct *work)
@@ -1101,23 +1111,14 @@ static void fw_device_refresh(struct work_struct *work)
 	struct fw_device *device =
 		container_of(work, struct fw_device, work.work);
 	struct fw_card *card = device->card;
-	int node_id = device->node_id;
-
-	switch (reread_config_rom(device, device->generation)) {
-	case REREAD_BIB_ERROR:
-		if (device->config_rom_retries < MAX_RETRIES / 2 &&
-		    atomic_read(&device->state) == FW_DEVICE_INITIALIZING) {
-			device->config_rom_retries++;
-			fw_schedule_device_work(device, RETRY_DELAY / 2);
-
-			return;
-		}
-		goto give_up;
+	int ret, node_id = device->node_id;
+	bool changed;
 
-	case REREAD_BIB_GONE:
-		goto gone;
+	ret = reread_config_rom(device, device->generation, &changed);
+	if (ret != RCODE_COMPLETE)
+		goto failed_config_rom;
 
-	case REREAD_BIB_UNCHANGED:
+	if (!changed) {
 		if (atomic_cmpxchg(&device->state,
 				   FW_DEVICE_INITIALIZING,
 				   FW_DEVICE_RUNNING) == FW_DEVICE_GONE)
@@ -1126,9 +1127,6 @@ static void fw_device_refresh(struct work_struct *work)
 		fw_device_update(work);
 		device->config_rom_retries = 0;
 		goto out;
-
-	case REREAD_BIB_CHANGED:
-		break;
 	}
 
 	/*
@@ -1137,16 +1135,9 @@ static void fw_device_refresh(struct work_struct *work)
 	 */
 	device_for_each_child(&device->device, NULL, shutdown_unit);
 
-	if (read_config_rom(device, device->generation) < 0) {
-		if (device->config_rom_retries < MAX_RETRIES &&
-		    atomic_read(&device->state) == FW_DEVICE_INITIALIZING) {
-			device->config_rom_retries++;
-			fw_schedule_device_work(device, RETRY_DELAY);
-
-			return;
-		}
-		goto give_up;
-	}
+	ret = read_config_rom(device, device->generation);
+	if (ret != RCODE_COMPLETE)
+		goto failed_config_rom;
 
 	fw_device_cdev_update(device);
 	create_units(device);
@@ -1163,9 +1154,16 @@ static void fw_device_refresh(struct work_struct *work)
 	device->config_rom_retries = 0;
 	goto out;
 
- give_up:
-	fw_notice(card, "giving up on refresh of device %s\n",
-		  dev_name(&device->device));
+ failed_config_rom:
+	if (device->config_rom_retries < MAX_RETRIES &&
+	    atomic_read(&device->state) == FW_DEVICE_INITIALIZING) {
+		device->config_rom_retries++;
+		fw_schedule_device_work(device, RETRY_DELAY);
+		return;
+	}
+
+	fw_notice(card, "giving up on refresh of device %s: %s\n",
+		  dev_name(&device->device), fw_rcode_string(ret));
  gone:
 	atomic_set(&device->state, FW_DEVICE_GONE);
 	PREPARE_DELAYED_WORK(&device->work, fw_device_shutdown);
diff --git a/drivers/firewire/core-iso.c b/drivers/firewire/core-iso.c
index d1565828ae2..8382e27e9a2 100644
--- a/drivers/firewire/core-iso.c
+++ b/drivers/firewire/core-iso.c
@@ -39,52 +39,73 @@
  * Isochronous DMA context management
  */
 
-int fw_iso_buffer_init(struct fw_iso_buffer *buffer, struct fw_card *card,
-		       int page_count, enum dma_data_direction direction)
+int fw_iso_buffer_alloc(struct fw_iso_buffer *buffer, int page_count)
 {
-	int i, j;
-	dma_addr_t address;
-
-	buffer->page_count = page_count;
-	buffer->direction = direction;
+	int i;
 
+	buffer->page_count = 0;
+	buffer->page_count_mapped = 0;
 	buffer->pages = kmalloc(page_count * sizeof(buffer->pages[0]),
 				GFP_KERNEL);
 	if (buffer->pages == NULL)
-		goto out;
+		return -ENOMEM;
 
-	for (i = 0; i < buffer->page_count; i++) {
+	for (i = 0; i < page_count; i++) {
 		buffer->pages[i] = alloc_page(GFP_KERNEL | GFP_DMA32 | __GFP_ZERO);
 		if (buffer->pages[i] == NULL)
-			goto out_pages;
+			break;
+	}
+	buffer->page_count = i;
+	if (i < page_count) {
+		fw_iso_buffer_destroy(buffer, NULL);
+		return -ENOMEM;
+	}
 
+	return 0;
+}
+
+int fw_iso_buffer_map_dma(struct fw_iso_buffer *buffer, struct fw_card *card,
+			  enum dma_data_direction direction)
+{
+	dma_addr_t address;
+	int i;
+
+	buffer->direction = direction;
+
+	for (i = 0; i < buffer->page_count; i++) {
 		address = dma_map_page(card->device, buffer->pages[i],
 				       0, PAGE_SIZE, direction);
-		if (dma_mapping_error(card->device, address)) {
-			__free_page(buffer->pages[i]);
-			goto out_pages;
-		}
+		if (dma_mapping_error(card->device, address))
+			break;
+
 		set_page_private(buffer->pages[i], address);
 	}
+	buffer->page_count_mapped = i;
+	if (i < buffer->page_count)
+		return -ENOMEM;
 
 	return 0;
+}
 
- out_pages:
-	for (j = 0; j < i; j++) {
-		address = page_private(buffer->pages[j]);
-		dma_unmap_page(card->device, address,
-			       PAGE_SIZE, direction);
-		__free_page(buffer->pages[j]);
-	}
-	kfree(buffer->pages);
- out:
-	buffer->pages = NULL;
+int fw_iso_buffer_init(struct fw_iso_buffer *buffer, struct fw_card *card,
+		       int page_count, enum dma_data_direction direction)
+{
+	int ret;
+
+	ret = fw_iso_buffer_alloc(buffer, page_count);
+	if (ret < 0)
+		return ret;
+
+	ret = fw_iso_buffer_map_dma(buffer, card, direction);
+	if (ret < 0)
+		fw_iso_buffer_destroy(buffer, card);
 
-	return -ENOMEM;
+	return ret;
 }
 EXPORT_SYMBOL(fw_iso_buffer_init);
 
-int fw_iso_buffer_map(struct fw_iso_buffer *buffer, struct vm_area_struct *vma)
+int fw_iso_buffer_map_vma(struct fw_iso_buffer *buffer,
+			  struct vm_area_struct *vma)
 {
 	unsigned long uaddr;
 	int i, err;
@@ -107,15 +128,18 @@ void fw_iso_buffer_destroy(struct fw_iso_buffer *buffer,
 	int i;
 	dma_addr_t address;
 
-	for (i = 0; i < buffer->page_count; i++) {
+	for (i = 0; i < buffer->page_count_mapped; i++) {
 		address = page_private(buffer->pages[i]);
 		dma_unmap_page(card->device, address,
 			       PAGE_SIZE, buffer->direction);
-		__free_page(buffer->pages[i]);
 	}
+	for (i = 0; i < buffer->page_count; i++)
+		__free_page(buffer->pages[i]);
 
 	kfree(buffer->pages);
 	buffer->pages = NULL;
+	buffer->page_count = 0;
+	buffer->page_count_mapped = 0;
 }
 EXPORT_SYMBOL(fw_iso_buffer_destroy);
 
diff --git a/drivers/firewire/core-transaction.c b/drivers/firewire/core-transaction.c
index dea2dcc9310..780708dc6e2 100644
--- a/drivers/firewire/core-transaction.c
+++ b/drivers/firewire/core-transaction.c
@@ -820,6 +820,15 @@ void fw_send_response(struct fw_card *card,
 }
 EXPORT_SYMBOL(fw_send_response);
 
+/**
+ * fw_get_request_speed() - returns speed at which the @request was received
+ */
+int fw_get_request_speed(struct fw_request *request)
+{
+	return request->response.speed;
+}
+EXPORT_SYMBOL(fw_get_request_speed);
+
 static void handle_exclusive_region_request(struct fw_card *card,
 					    struct fw_packet *p,
 					    struct fw_request *request,
@@ -994,6 +1003,32 @@ void fw_core_handle_response(struct fw_card *card, struct fw_packet *p)
 }
 EXPORT_SYMBOL(fw_core_handle_response);
 
+/**
+ * fw_rcode_string - convert a firewire result code to an error description
+ * @rcode: the result code
+ */
+const char *fw_rcode_string(int rcode)
+{
+	static const char *const names[] = {
+		[RCODE_COMPLETE]       = "no error",
+		[RCODE_CONFLICT_ERROR] = "conflict error",
+		[RCODE_DATA_ERROR]     = "data error",
+		[RCODE_TYPE_ERROR]     = "type error",
+		[RCODE_ADDRESS_ERROR]  = "address error",
+		[RCODE_SEND_ERROR]     = "send error",
+		[RCODE_CANCELLED]      = "timeout",
+		[RCODE_BUSY]           = "busy",
+		[RCODE_GENERATION]     = "bus reset",
+		[RCODE_NO_ACK]         = "no ack",
+	};
+
+	if ((unsigned int)rcode < ARRAY_SIZE(names) && names[rcode])
+		return names[rcode];
+	else
+		return "unknown";
+}
+EXPORT_SYMBOL(fw_rcode_string);
+
 static const struct fw_address_region topology_map_region =
 	{ .start = CSR_REGISTER_BASE | CSR_TOPOLOGY_MAP,
 	  .end   = CSR_REGISTER_BASE | CSR_TOPOLOGY_MAP_END, };
diff --git a/drivers/firewire/core.h b/drivers/firewire/core.h
index 9047f5547d9..515a42c786d 100644
--- a/drivers/firewire/core.h
+++ b/drivers/firewire/core.h
@@ -3,6 +3,7 @@
 
 #include <linux/compiler.h>
 #include <linux/device.h>
+#include <linux/dma-mapping.h>
 #include <linux/fs.h>
 #include <linux/list.h>
 #include <linux/idr.h>
@@ -120,21 +121,6 @@ int fw_compute_block_crc(__be32 *block);
 void fw_schedule_bus_reset(struct fw_card *card, bool delayed, bool short_reset);
 void fw_schedule_bm_work(struct fw_card *card, unsigned long delay);
 
-static inline struct fw_card *fw_card_get(struct fw_card *card)
-{
-	kref_get(&card->kref);
-
-	return card;
-}
-
-void fw_card_release(struct kref *kref);
-
-static inline void fw_card_put(struct fw_card *card)
-{
-	kref_put(&card->kref, fw_card_release);
-}
-
-
 /* -cdev */
 
 extern const struct file_operations fw_device_ops;
@@ -169,7 +155,11 @@ void fw_node_event(struct fw_card *card, struct fw_node *node, int event);
 
 /* -iso */
 
-int fw_iso_buffer_map(struct fw_iso_buffer *buffer, struct vm_area_struct *vma);
+int fw_iso_buffer_alloc(struct fw_iso_buffer *buffer, int page_count);
+int fw_iso_buffer_map_dma(struct fw_iso_buffer *buffer, struct fw_card *card,
+			  enum dma_data_direction direction);
+int fw_iso_buffer_map_vma(struct fw_iso_buffer *buffer,
+			  struct vm_area_struct *vma);
 
 
 /* -topology */
diff --git a/drivers/firewire/nosy.c b/drivers/firewire/nosy.c
index a7c4422a688..4ebfb227367 100644
--- a/drivers/firewire/nosy.c
+++ b/drivers/firewire/nosy.c
@@ -693,6 +693,8 @@ static struct pci_device_id pci_table[] __devinitdata = {
 	{ }	/* Terminating entry */
 };
 
+MODULE_DEVICE_TABLE(pci, pci_table);
+
 static struct pci_driver lynx_pci_driver = {
 	.name =		driver_name,
 	.id_table =	pci_table,
@@ -700,22 +702,8 @@ static struct pci_driver lynx_pci_driver = {
 	.remove =	remove_card,
 };
 
+module_pci_driver(lynx_pci_driver);
+
 MODULE_AUTHOR("Kristian Hoegsberg");
 MODULE_DESCRIPTION("Snoop mode driver for TI pcilynx 1394 controllers");
 MODULE_LICENSE("GPL");
-MODULE_DEVICE_TABLE(pci, pci_table);
-
-static int __init nosy_init(void)
-{
-	return pci_register_driver(&lynx_pci_driver);
-}
-
-static void __exit nosy_cleanup(void)
-{
-	pci_unregister_driver(&lynx_pci_driver);
-
-	pr_info("Unloaded %s\n", driver_name);
-}
-
-module_init(nosy_init);
-module_exit(nosy_cleanup);
diff --git a/drivers/firewire/ohci.c b/drivers/firewire/ohci.c
index 2b5460075a9..c1af05e834b 100644
--- a/drivers/firewire/ohci.c
+++ b/drivers/firewire/ohci.c
@@ -1821,9 +1821,8 @@ static void bus_reset_work(struct work_struct *work)
 {
 	struct fw_ohci *ohci =
 		container_of(work, struct fw_ohci, bus_reset_work);
-	int self_id_count, i, j, reg;
-	int generation, new_generation;
-	unsigned long flags;
+	int self_id_count, generation, new_generation, i, j;
+	u32 reg;
 	void *free_rom = NULL;
 	dma_addr_t free_rom_bus = 0;
 	bool is_new_root;
@@ -1930,13 +1929,13 @@ static void bus_reset_work(struct work_struct *work)
 	}
 
 	/* FIXME: Document how the locking works. */
-	spin_lock_irqsave(&ohci->lock, flags);
+	spin_lock_irq(&ohci->lock);
 
 	ohci->generation = -1; /* prevent AT packet queueing */
 	context_stop(&ohci->at_request_ctx);
 	context_stop(&ohci->at_response_ctx);
 
-	spin_unlock_irqrestore(&ohci->lock, flags);
+	spin_unlock_irq(&ohci->lock);
 
 	/*
 	 * Per OHCI 1.2 draft, clause 7.2.3.3, hardware may leave unsent
@@ -1946,7 +1945,7 @@ static void bus_reset_work(struct work_struct *work)
 	at_context_flush(&ohci->at_request_ctx);
 	at_context_flush(&ohci->at_response_ctx);
 
-	spin_lock_irqsave(&ohci->lock, flags);
+	spin_lock_irq(&ohci->lock);
 
 	ohci->generation = generation;
 	reg_write(ohci, OHCI1394_IntEventClear, OHCI1394_busReset);
@@ -1990,7 +1989,7 @@ static void bus_reset_work(struct work_struct *work)
 	reg_write(ohci, OHCI1394_PhyReqFilterLoSet, ~0);
 #endif
 
-	spin_unlock_irqrestore(&ohci->lock, flags);
+	spin_unlock_irq(&ohci->lock);
 
 	if (free_rom)
 		dma_free_coherent(ohci->card.device, CONFIG_ROM_SIZE,
@@ -2402,7 +2401,6 @@ static int ohci_set_config_rom(struct fw_card *card,
 			       const __be32 *config_rom, size_t length)
 {
 	struct fw_ohci *ohci;
-	unsigned long flags;
 	__be32 *next_config_rom;
 	dma_addr_t uninitialized_var(next_config_rom_bus);
 
@@ -2441,7 +2439,7 @@ static int ohci_set_config_rom(struct fw_card *card,
 	if (next_config_rom == NULL)
 		return -ENOMEM;
 
-	spin_lock_irqsave(&ohci->lock, flags);
+	spin_lock_irq(&ohci->lock);
 
 	/*
 	 * If there is not an already pending config_rom update,
@@ -2467,7 +2465,7 @@ static int ohci_set_config_rom(struct fw_card *card,
 
 	reg_write(ohci, OHCI1394_ConfigROMmap, ohci->next_config_rom_bus);
 
-	spin_unlock_irqrestore(&ohci->lock, flags);
+	spin_unlock_irq(&ohci->lock);
 
 	/* If we didn't use the DMA allocation, delete it. */
 	if (next_config_rom != NULL)
@@ -2891,10 +2889,9 @@ static struct fw_iso_context *ohci_allocate_iso_context(struct fw_card *card,
 	descriptor_callback_t uninitialized_var(callback);
 	u64 *uninitialized_var(channels);
 	u32 *uninitialized_var(mask), uninitialized_var(regs);
-	unsigned long flags;
 	int index, ret = -EBUSY;
 
-	spin_lock_irqsave(&ohci->lock, flags);
+	spin_lock_irq(&ohci->lock);
 
 	switch (type) {
 	case FW_ISO_CONTEXT_TRANSMIT:
@@ -2938,7 +2935,7 @@ static struct fw_iso_context *ohci_allocate_iso_context(struct fw_card *card,
 		ret = -ENOSYS;
 	}
 
-	spin_unlock_irqrestore(&ohci->lock, flags);
+	spin_unlock_irq(&ohci->lock);
 
 	if (index < 0)
 		return ERR_PTR(ret);
@@ -2964,7 +2961,7 @@ static struct fw_iso_context *ohci_allocate_iso_context(struct fw_card *card,
  out_with_header:
 	free_page((unsigned long)ctx->header);
  out:
-	spin_lock_irqsave(&ohci->lock, flags);
+	spin_lock_irq(&ohci->lock);
 
 	switch (type) {
 	case FW_ISO_CONTEXT_RECEIVE:
@@ -2977,7 +2974,7 @@ static struct fw_iso_context *ohci_allocate_iso_context(struct fw_card *card,
 	}
 	*mask |= 1 << index;
 
-	spin_unlock_irqrestore(&ohci->lock, flags);
+	spin_unlock_irq(&ohci->lock);
 
 	return ERR_PTR(ret);
 }
@@ -3789,6 +3786,8 @@ static struct pci_driver fw_ohci_pci_driver = {
 #endif
 };
 
+module_pci_driver(fw_ohci_pci_driver);
+
 MODULE_AUTHOR("Kristian Hoegsberg <krh@bitplanet.net>");
 MODULE_DESCRIPTION("Driver for PCI OHCI IEEE1394 controllers");
 MODULE_LICENSE("GPL");
@@ -3797,16 +3796,3 @@ MODULE_LICENSE("GPL");
 #ifndef CONFIG_IEEE1394_OHCI1394_MODULE
 MODULE_ALIAS("ohci1394");
 #endif
-
-static int __init fw_ohci_init(void)
-{
-	return pci_register_driver(&fw_ohci_pci_driver);
-}
-
-static void __exit fw_ohci_cleanup(void)
-{
-	pci_unregister_driver(&fw_ohci_pci_driver);
-}
-
-module_init(fw_ohci_init);
-module_exit(fw_ohci_cleanup);
diff --git a/drivers/firewire/sbp2.c b/drivers/firewire/sbp2.c
index b7e65d7eab6..1162d6b3bf8 100644
--- a/drivers/firewire/sbp2.c
+++ b/drivers/firewire/sbp2.c
@@ -207,9 +207,8 @@ static const struct device *lu_dev(const struct sbp2_logical_unit *lu)
 #define SBP2_MAX_CDB_SIZE		16
 
 /*
- * The default maximum s/g segment size of a FireWire controller is
- * usually 0x10000, but SBP-2 only allows 0xffff. Since buffers have to
- * be quadlet-aligned, we set the length limit to 0xffff & ~3.
+ * The maximum SBP-2 data buffer size is 0xffff.  We quadlet-align this
+ * for compatibility with earlier versions of this driver.
  */
 #define SBP2_MAX_SEG_SIZE		0xfffc
 
@@ -1163,7 +1162,8 @@ static int sbp2_probe(struct device *dev)
 
 	shost->max_cmd_len = SBP2_MAX_CDB_SIZE;
 
-	if (scsi_add_host(shost, &unit->device) < 0)
+	if (scsi_add_host_with_dma(shost, &unit->device,
+				   device->card->device) < 0)
 		goto fail_shost_put;
 
 	/* implicit directory ID */
@@ -1295,10 +1295,7 @@ static struct fw_driver sbp2_driver = {
 static void sbp2_unmap_scatterlist(struct device *card_device,
 				   struct sbp2_command_orb *orb)
 {
-	if (scsi_sg_count(orb->cmd))
-		dma_unmap_sg(card_device, scsi_sglist(orb->cmd),
-			     scsi_sg_count(orb->cmd),
-			     orb->cmd->sc_data_direction);
+	scsi_dma_unmap(orb->cmd);
 
 	if (orb->request.misc & cpu_to_be32(COMMAND_ORB_PAGE_TABLE_PRESENT))
 		dma_unmap_single(card_device, orb->page_table_bus,
@@ -1404,9 +1401,8 @@ static int sbp2_map_scatterlist(struct sbp2_command_orb *orb,
 	struct scatterlist *sg = scsi_sglist(orb->cmd);
 	int i, n;
 
-	n = dma_map_sg(device->card->device, sg, scsi_sg_count(orb->cmd),
-		       orb->cmd->sc_data_direction);
-	if (n == 0)
+	n = scsi_dma_map(orb->cmd);
+	if (n <= 0)
 		goto fail;
 
 	/*
@@ -1452,8 +1448,7 @@ static int sbp2_map_scatterlist(struct sbp2_command_orb *orb,
 	return 0;
 
  fail_page_table:
-	dma_unmap_sg(device->card->device, scsi_sglist(orb->cmd),
-		     scsi_sg_count(orb->cmd), orb->cmd->sc_data_direction);
+	scsi_dma_unmap(orb->cmd);
  fail:
 	return -ENOMEM;
 }
@@ -1534,7 +1529,10 @@ static int sbp2_scsi_slave_alloc(struct scsi_device *sdev)
 
 	sdev->allow_restart = 1;
 
-	/* SBP-2 requires quadlet alignment of the data buffers. */
+	/*
+	 * SBP-2 does not require any alignment, but we set it anyway
+	 * for compatibility with earlier versions of this driver.
+	 */
 	blk_queue_update_dma_alignment(sdev->request_queue, 4 - 1);
 
 	if (lu->tgt->workarounds & SBP2_WORKAROUND_INQUIRY_36)
@@ -1568,8 +1566,6 @@ static int sbp2_scsi_slave_configure(struct scsi_device *sdev)
 	if (lu->tgt->workarounds & SBP2_WORKAROUND_128K_MAX_TRANS)
 		blk_queue_max_hw_sectors(sdev->request_queue, 128 * 1024 / 512);
 
-	blk_queue_max_segment_size(sdev->request_queue, SBP2_MAX_SEG_SIZE);
-
 	return 0;
 }
 
diff --git a/drivers/gpio/Kconfig b/drivers/gpio/Kconfig
index e03653d6935..c4067d0141f 100644
--- a/drivers/gpio/Kconfig
+++ b/drivers/gpio/Kconfig
@@ -2,6 +2,14 @@
 # GPIO infrastructure and drivers
 #
 
+config ARCH_HAVE_CUSTOM_GPIO_H
+	bool
+	help
+	  Selecting this config option from the architecture Kconfig allows
+	  the architecture to provide a custom asm/gpio.h implementation
+	  overriding the default implementations.  New uses of this are
+	  strongly discouraged.
+
 config ARCH_WANT_OPTIONAL_GPIOLIB
 	bool
 	help
@@ -37,6 +45,10 @@ menuconfig GPIOLIB
 
 if GPIOLIB
 
+config OF_GPIO
+	def_bool y
+	depends on OF && !SPARC
+
 config DEBUG_GPIO
 	bool "Debug GPIO calls"
 	depends on DEBUG_KERNEL
@@ -91,11 +103,25 @@ config GPIO_IT8761E
 	help
 	  Say yes here to support GPIO functionality of IT8761E super I/O chip.
 
+config GPIO_EM
+	tristate "Emma Mobile GPIO"
+	depends on ARM
+	help
+	  Say yes here to support GPIO on Renesas Emma Mobile SoCs.
+
 config GPIO_EP93XX
 	def_bool y
 	depends on ARCH_EP93XX
 	select GPIO_GENERIC
 
+config GPIO_MM_LANTIQ
+	bool "Lantiq Memory mapped GPIOs"
+	depends on LANTIQ && SOC_XWAY
+	help
+	  This enables support for memory mapped GPIOs on the External Bus Unit
+	  (EBU) found on Lantiq SoCs. The gpios are output only as they are
+	  created by attaching a 16bit latch to the bus.
+
 config GPIO_MPC5200
 	def_bool y
 	depends on PPC_MPC52xx
@@ -149,6 +175,14 @@ config GPIO_PXA
 	help
 	  Say yes here to support the PXA GPIO device
 
+config GPIO_STA2X11
+	bool "STA2x11/ConneXt GPIO support"
+	depends on MFD_STA2X11
+	select GENERIC_IRQ_CHIP
+	help
+	  Say yes here to support the STA2x11/ConneXt GPIO device.
+	  The GPIO module has 128 GPIO pins with alternate functions.
+
 config GPIO_XILINX
 	bool "Xilinx GPIO support"
 	depends on PPC_OF || MICROBLAZE
@@ -162,13 +196,13 @@ config GPIO_VR41XX
 	  Say yes here to support the NEC VR4100 series General-purpose I/O Uint
 
 config GPIO_SCH
-	tristate "Intel SCH/TunnelCreek GPIO"
+	tristate "Intel SCH/TunnelCreek/Centerton GPIO"
 	depends on PCI && X86
 	select MFD_CORE
 	select LPC_SCH
 	help
-	  Say yes here to support GPIO interface on Intel Poulsbo SCH
-	  or Intel Tunnel Creek processor.
+	  Say yes here to support GPIO interface on Intel Poulsbo SCH,
+	  Intel Tunnel Creek processor or Intel Centerton processor.
 	  The Intel SCH contains a total of 14 GPIO pins. Ten GPIOs are
 	  powered by the core power rail and are turned off during sleep
 	  modes (S3 and higher). The remaining four GPIOs are powered by
@@ -177,6 +211,22 @@ config GPIO_SCH
 	  system from the Suspend-to-RAM state.
 	  The Intel Tunnel Creek processor has 5 GPIOs powered by the
 	  core power rail and 9 from suspend power supply.
+	  The Intel Centerton processor has a total of 30 GPIO pins.
+	  Twenty-one are powered by the core power rail and 9 from the
+	  suspend power supply.
+
+config GPIO_ICH
+	tristate "Intel ICH GPIO"
+	depends on PCI && X86
+	select MFD_CORE
+	select LPC_ICH
+	help
+	  Say yes here to support the GPIO functionality of a number of Intel
+	  ICH-based chipsets.  Currently supported devices: ICH6, ICH7, ICH8
+	  ICH9, ICH10, Series 5/3400 (eg Ibex Peak), Series 6/C200 (eg
+	  Cougar Point), NM10 (Tiger Point), and 3100 (Whitmore Lake).
+
+	  If unsure, say N.
 
 config GPIO_VX855
 	tristate "VIA VX855/VX875 GPIO"
@@ -243,7 +293,7 @@ config GPIO_MC9S08DZ60
 	  Select this to enable the MC9S08DZ60 GPIO driver
 
 config GPIO_PCA953X
-	tristate "PCA953x, PCA955x, TCA64xx, and MAX7310 I/O ports"
+	tristate "PCA953x, PCA955x, PCA957x, TCA64xx, and MAX7310 I/O ports"
 	depends on I2C
 	help
 	  Say yes here to provide access to several register-oriented
@@ -252,10 +302,11 @@ config GPIO_PCA953X
 
 	  4 bits:	pca9536, pca9537
 
-	  8 bits:	max7310, pca9534, pca9538, pca9554, pca9557,
-	  		tca6408
+	  8 bits:	max7310, max7315, pca6107, pca9534, pca9538, pca9554,
+			pca9556, pca9557, pca9574, tca6408
 
-	  16 bits:	pca9535, pca9539, pca9555, tca6416
+	  16 bits:	max7312, max7313, pca9535, pca9539, pca9555, pca9575,
+			tca6416
 
 config GPIO_PCA953X_IRQ
 	bool "Interrupt controller support for PCA953x"
@@ -288,6 +339,15 @@ config GPIO_PCF857X
 	  This driver provides an in-kernel interface to those GPIOs using
 	  platform-neutral GPIO calls.
 
+config GPIO_RC5T583
+	bool "RICOH RC5T583 GPIO"
+	depends on MFD_RC5T583
+	help
+	  Select this option to enable GPIO driver for the Ricoh RC5T583
+	  chip family.
+	  This driver provides the support for driving/reading the gpio pins
+	  of RC5T583 device through standard gpio library.
+
 config GPIO_SX150X
 	bool "Semtech SX150x I2C GPIO expander"
 	depends on I2C=y
@@ -306,6 +366,16 @@ config GPIO_STMPE
 	  This enables support for the GPIOs found on the STMPE I/O
 	  Expanders.
 
+config GPIO_STP_XWAY
+	bool "XWAY STP GPIOs"
+	depends on SOC_XWAY
+	help
+	  This enables support for the Serial To Parallel (STP) unit found on
+	  XWAY SoC. The STP allows the SoC to drive a shift registers cascade,
+	  that can be up to 24 bit. This peripheral is aimed at driving leds.
+	  Some of the gpios/leds can be auto updated by the soc with dsl and
+	  phy status.
+
 config GPIO_TC3589X
 	bool "TC3589X GPIOs"
 	depends on MFD_TC3589X
@@ -399,6 +469,7 @@ config GPIO_BT8XX
 config GPIO_LANGWELL
 	bool "Intel Langwell/Penwell GPIO support"
 	depends on PCI && X86
+	select IRQ_DOMAIN
 	help
 	  Say Y here to support Intel Langwell/Penwell GPIO.
 
@@ -514,4 +585,12 @@ config GPIO_TPS65910
 	help
 	  Select this option to enable GPIO driver for the TPS65910
 	  chip family.
+
+config GPIO_MSIC
+	bool "Intel MSIC mixed signal gpio support"
+	depends on MFD_INTEL_MSIC
+	help
+	  Enable support for GPIO on intel MSIC controllers found in
+	  intel MID devices
+
 endif
diff --git a/drivers/gpio/Makefile b/drivers/gpio/Makefile
index 007f54bd008..0f55662002c 100644
--- a/drivers/gpio/Makefile
+++ b/drivers/gpio/Makefile
@@ -3,6 +3,7 @@
 ccflags-$(CONFIG_DEBUG_GPIO)	+= -DDEBUG
 
 obj-$(CONFIG_GPIOLIB)		+= gpiolib.o devres.o
+obj-$(CONFIG_OF_GPIO)		+= gpiolib-of.o
 
 # Device drivers. Generally keep list sorted alphabetically
 obj-$(CONFIG_GPIO_GENERIC)	+= gpio-generic.o
@@ -15,8 +16,10 @@ obj-$(CONFIG_GPIO_BT8XX)	+= gpio-bt8xx.o
 obj-$(CONFIG_GPIO_CS5535)	+= gpio-cs5535.o
 obj-$(CONFIG_GPIO_DA9052)	+= gpio-da9052.o
 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_ICH)		+= gpio-ich.o
 obj-$(CONFIG_GPIO_IT8761E)	+= gpio-it8761e.o
 obj-$(CONFIG_GPIO_JANZ_TTL)	+= gpio-janz-ttl.o
 obj-$(CONFIG_ARCH_KS8695)	+= gpio-ks8695.o
@@ -30,25 +33,29 @@ obj-$(CONFIG_GPIO_MC33880)	+= gpio-mc33880.o
 obj-$(CONFIG_GPIO_MC9S08DZ60)	+= gpio-mc9s08dz60.o
 obj-$(CONFIG_GPIO_MCP23S08)	+= gpio-mcp23s08.o
 obj-$(CONFIG_GPIO_ML_IOH)	+= gpio-ml-ioh.o
+obj-$(CONFIG_GPIO_MM_LANTIQ)	+= gpio-mm-lantiq.o
 obj-$(CONFIG_GPIO_MPC5200)	+= gpio-mpc5200.o
 obj-$(CONFIG_GPIO_MPC8XXX)	+= gpio-mpc8xxx.o
+obj-$(CONFIG_GPIO_MSIC)		+= gpio-msic.o
 obj-$(CONFIG_GPIO_MSM_V1)	+= gpio-msm-v1.o
 obj-$(CONFIG_GPIO_MSM_V2)	+= gpio-msm-v2.o
 obj-$(CONFIG_GPIO_MXC)		+= gpio-mxc.o
 obj-$(CONFIG_GPIO_MXS)		+= gpio-mxs.o
-obj-$(CONFIG_PLAT_NOMADIK)	+= gpio-nomadik.o
 obj-$(CONFIG_ARCH_OMAP)		+= gpio-omap.o
 obj-$(CONFIG_GPIO_PCA953X)	+= gpio-pca953x.o
 obj-$(CONFIG_GPIO_PCF857X)	+= gpio-pcf857x.o
 obj-$(CONFIG_GPIO_PCH)		+= gpio-pch.o
 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_PLAT_SAMSUNG)	+= gpio-samsung.o
 obj-$(CONFIG_ARCH_SA1100)	+= gpio-sa1100.o
 obj-$(CONFIG_GPIO_SCH)		+= gpio-sch.o
 obj-$(CONFIG_GPIO_SODAVILLE)	+= gpio-sodaville.o
+obj-$(CONFIG_GPIO_STA2X11)	+= gpio-sta2x11.o
 obj-$(CONFIG_GPIO_STMPE)	+= gpio-stmpe.o
+obj-$(CONFIG_GPIO_STP_XWAY)	+= gpio-stp-xway.o
 obj-$(CONFIG_GPIO_SX150X)	+= gpio-sx150x.o
 obj-$(CONFIG_GPIO_TC3589X)	+= gpio-tc3589x.o
 obj-$(CONFIG_ARCH_TEGRA)	+= gpio-tegra.o
diff --git a/drivers/gpio/devres.c b/drivers/gpio/devres.c
index 3dd29399cef..9e9947cb86a 100644
--- a/drivers/gpio/devres.c
+++ b/drivers/gpio/devres.c
@@ -71,6 +71,35 @@ int devm_gpio_request(struct device *dev, unsigned gpio, const char *label)
 EXPORT_SYMBOL(devm_gpio_request);
 
 /**
+ *	devm_gpio_request_one - request a single GPIO with initial setup
+ *	@dev:   device to request for
+ *	@gpio:	the GPIO number
+ *	@flags:	GPIO configuration as specified by GPIOF_*
+ *	@label:	a literal description string of this GPIO
+ */
+int devm_gpio_request_one(struct device *dev, unsigned gpio,
+			  unsigned long flags, const char *label)
+{
+	unsigned *dr;
+	int rc;
+
+	dr = devres_alloc(devm_gpio_release, sizeof(unsigned), GFP_KERNEL);
+	if (!dr)
+		return -ENOMEM;
+
+	rc = gpio_request_one(gpio, flags, label);
+	if (rc) {
+		devres_free(dr);
+		return rc;
+	}
+
+	*dr = gpio;
+	devres_add(dev, dr);
+
+	return 0;
+}
+
+/**
  *      devm_gpio_free - free an interrupt
  *      @dev: device to free gpio for
  *      @gpio: gpio to free
@@ -83,8 +112,7 @@ EXPORT_SYMBOL(devm_gpio_request);
 void devm_gpio_free(struct device *dev, unsigned int gpio)
 {
 
-	WARN_ON(devres_destroy(dev, devm_gpio_release, devm_gpio_match,
+	WARN_ON(devres_release(dev, devm_gpio_release, devm_gpio_match,
 		&gpio));
-	gpio_free(gpio);
 }
 EXPORT_SYMBOL(devm_gpio_free);
diff --git a/drivers/gpio/gpio-bt8xx.c b/drivers/gpio/gpio-bt8xx.c
index 5ca4098ba09..e4cc7eb69bb 100644
--- a/drivers/gpio/gpio-bt8xx.c
+++ b/drivers/gpio/gpio-bt8xx.c
@@ -328,17 +328,7 @@ static struct pci_driver bt8xxgpio_pci_driver = {
 	.resume		= bt8xxgpio_resume,
 };
 
-static int __init bt8xxgpio_init(void)
-{
-	return pci_register_driver(&bt8xxgpio_pci_driver);
-}
-module_init(bt8xxgpio_init)
-
-static void __exit bt8xxgpio_exit(void)
-{
-	pci_unregister_driver(&bt8xxgpio_pci_driver);
-}
-module_exit(bt8xxgpio_exit)
+module_pci_driver(bt8xxgpio_pci_driver);
 
 MODULE_LICENSE("GPL");
 MODULE_AUTHOR("Michael Buesch");
diff --git a/drivers/gpio/gpio-em.c b/drivers/gpio/gpio-em.c
new file mode 100644
index 00000000000..150d9768811
--- /dev/null
+++ b/drivers/gpio/gpio-em.c
@@ -0,0 +1,418 @@
+/*
+ * Emma Mobile GPIO Support - GIO
+ *
+ *  Copyright (C) 2012 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/bitops.h>
+#include <linux/err.h>
+#include <linux/gpio.h>
+#include <linux/slab.h>
+#include <linux/module.h>
+#include <linux/platform_data/gpio-em.h>
+
+struct em_gio_priv {
+	void __iomem *base0;
+	void __iomem *base1;
+	unsigned int irq_base;
+	spinlock_t sense_lock;
+	struct platform_device *pdev;
+	struct gpio_chip gpio_chip;
+	struct irq_chip irq_chip;
+	struct irq_domain *irq_domain;
+};
+
+#define GIO_E1 0x00
+#define GIO_E0 0x04
+#define GIO_EM 0x04
+#define GIO_OL 0x08
+#define GIO_OH 0x0c
+#define GIO_I 0x10
+#define GIO_IIA 0x14
+#define GIO_IEN 0x18
+#define GIO_IDS 0x1c
+#define GIO_IIM 0x1c
+#define GIO_RAW 0x20
+#define GIO_MST 0x24
+#define GIO_IIR 0x28
+
+#define GIO_IDT0 0x40
+#define GIO_IDT1 0x44
+#define GIO_IDT2 0x48
+#define GIO_IDT3 0x4c
+#define GIO_RAWBL 0x50
+#define GIO_RAWBH 0x54
+#define GIO_IRBL 0x58
+#define GIO_IRBH 0x5c
+
+#define GIO_IDT(n) (GIO_IDT0 + ((n) * 4))
+
+static inline unsigned long em_gio_read(struct em_gio_priv *p, int offs)
+{
+	if (offs < GIO_IDT0)
+		return ioread32(p->base0 + offs);
+	else
+		return ioread32(p->base1 + (offs - GIO_IDT0));
+}
+
+static inline void em_gio_write(struct em_gio_priv *p, int offs,
+				unsigned long value)
+{
+	if (offs < GIO_IDT0)
+		iowrite32(value, p->base0 + offs);
+	else
+		iowrite32(value, p->base1 + (offs - GIO_IDT0));
+}
+
+static inline struct em_gio_priv *irq_to_priv(struct irq_data *d)
+{
+	struct irq_chip *chip = irq_data_get_irq_chip(d);
+	return container_of(chip, struct em_gio_priv, irq_chip);
+}
+
+static void em_gio_irq_disable(struct irq_data *d)
+{
+	struct em_gio_priv *p = irq_to_priv(d);
+
+	em_gio_write(p, GIO_IDS, BIT(irqd_to_hwirq(d)));
+}
+
+static void em_gio_irq_enable(struct irq_data *d)
+{
+	struct em_gio_priv *p = irq_to_priv(d);
+
+	em_gio_write(p, GIO_IEN, BIT(irqd_to_hwirq(d)));
+}
+
+#define GIO_ASYNC(x) (x + 8)
+
+static unsigned char em_gio_sense_table[IRQ_TYPE_SENSE_MASK + 1] = {
+	[IRQ_TYPE_EDGE_RISING] = GIO_ASYNC(0x00),
+	[IRQ_TYPE_EDGE_FALLING] = GIO_ASYNC(0x01),
+	[IRQ_TYPE_LEVEL_HIGH] = GIO_ASYNC(0x02),
+	[IRQ_TYPE_LEVEL_LOW] = GIO_ASYNC(0x03),
+	[IRQ_TYPE_EDGE_BOTH] = GIO_ASYNC(0x04),
+};
+
+static int em_gio_irq_set_type(struct irq_data *d, unsigned int type)
+{
+	unsigned char value = em_gio_sense_table[type & IRQ_TYPE_SENSE_MASK];
+	struct em_gio_priv *p = irq_to_priv(d);
+	unsigned int reg, offset, shift;
+	unsigned long flags;
+	unsigned long tmp;
+
+	if (!value)
+		return -EINVAL;
+
+	offset = irqd_to_hwirq(d);
+
+	pr_debug("gio: sense irq = %d, mode = %d\n", offset, value);
+
+	/* 8 x 4 bit fields in 4 IDT registers */
+	reg = GIO_IDT(offset >> 3);
+	shift = (offset & 0x07) << 4;
+
+	spin_lock_irqsave(&p->sense_lock, flags);
+
+	/* disable the interrupt in IIA */
+	tmp = em_gio_read(p, GIO_IIA);
+	tmp &= ~BIT(offset);
+	em_gio_write(p, GIO_IIA, tmp);
+
+	/* change the sense setting in IDT */
+	tmp = em_gio_read(p, reg);
+	tmp &= ~(0xf << shift);
+	tmp |= value << shift;
+	em_gio_write(p, reg, tmp);
+
+	/* clear pending interrupts */
+	em_gio_write(p, GIO_IIR, BIT(offset));
+
+	/* enable the interrupt in IIA */
+	tmp = em_gio_read(p, GIO_IIA);
+	tmp |= BIT(offset);
+	em_gio_write(p, GIO_IIA, tmp);
+
+	spin_unlock_irqrestore(&p->sense_lock, flags);
+
+	return 0;
+}
+
+static irqreturn_t em_gio_irq_handler(int irq, void *dev_id)
+{
+	struct em_gio_priv *p = dev_id;
+	unsigned long pending;
+	unsigned int offset, irqs_handled = 0;
+
+	while ((pending = em_gio_read(p, GIO_MST))) {
+		offset = __ffs(pending);
+		em_gio_write(p, GIO_IIR, BIT(offset));
+		generic_handle_irq(irq_find_mapping(p->irq_domain, offset));
+		irqs_handled++;
+	}
+
+	return irqs_handled ? IRQ_HANDLED : IRQ_NONE;
+}
+
+static inline struct em_gio_priv *gpio_to_priv(struct gpio_chip *chip)
+{
+	return container_of(chip, struct em_gio_priv, gpio_chip);
+}
+
+static int em_gio_direction_input(struct gpio_chip *chip, unsigned offset)
+{
+	em_gio_write(gpio_to_priv(chip), GIO_E0, BIT(offset));
+	return 0;
+}
+
+static int em_gio_get(struct gpio_chip *chip, unsigned offset)
+{
+	return (int)(em_gio_read(gpio_to_priv(chip), GIO_I) & BIT(offset));
+}
+
+static void __em_gio_set(struct gpio_chip *chip, unsigned int reg,
+			 unsigned shift, int value)
+{
+	/* upper 16 bits contains mask and lower 16 actual value */
+	em_gio_write(gpio_to_priv(chip), reg,
+		     (1 << (shift + 16)) | (value << shift));
+}
+
+static void em_gio_set(struct gpio_chip *chip, unsigned offset, int value)
+{
+	/* output is split into two registers */
+	if (offset < 16)
+		__em_gio_set(chip, GIO_OL, offset, value);
+	else
+		__em_gio_set(chip, GIO_OH, offset - 16, value);
+}
+
+static int em_gio_direction_output(struct gpio_chip *chip, unsigned offset,
+				   int value)
+{
+	/* write GPIO value to output before selecting output mode of pin */
+	em_gio_set(chip, offset, value);
+	em_gio_write(gpio_to_priv(chip), GIO_E1, BIT(offset));
+	return 0;
+}
+
+static int em_gio_to_irq(struct gpio_chip *chip, unsigned offset)
+{
+	return irq_find_mapping(gpio_to_priv(chip)->irq_domain, offset);
+}
+
+static int em_gio_irq_domain_map(struct irq_domain *h, unsigned int virq,
+				 irq_hw_number_t hw)
+{
+	struct em_gio_priv *p = h->host_data;
+
+	pr_debug("gio: 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 em_gio_irq_domain_ops = {
+	.map	= em_gio_irq_domain_map,
+};
+
+static int __devinit em_gio_irq_domain_init(struct em_gio_priv *p)
+{
+	struct platform_device *pdev = p->pdev;
+	struct gpio_em_config *pdata = pdev->dev.platform_data;
+
+	p->irq_base = irq_alloc_descs(pdata->irq_base, 0,
+				      pdata->number_of_pins, numa_node_id());
+	if (IS_ERR_VALUE(p->irq_base)) {
+		dev_err(&pdev->dev, "cannot get irq_desc\n");
+		return -ENXIO;
+	}
+	pr_debug("gio: hw base = %d, nr = %d, sw base = %d\n",
+		 pdata->gpio_base, pdata->number_of_pins, p->irq_base);
+
+	p->irq_domain = irq_domain_add_legacy(pdev->dev.of_node,
+					      pdata->number_of_pins,
+					      p->irq_base, 0,
+					      &em_gio_irq_domain_ops, p);
+	if (!p->irq_domain) {
+		irq_free_descs(p->irq_base, pdata->number_of_pins);
+		return -ENXIO;
+	}
+
+	return 0;
+}
+
+static void __devexit em_gio_irq_domain_cleanup(struct em_gio_priv *p)
+{
+	struct gpio_em_config *pdata = p->pdev->dev.platform_data;
+
+	irq_free_descs(p->irq_base, pdata->number_of_pins);
+	/* FIXME: irq domain wants to be freed! */
+}
+
+static int __devinit em_gio_probe(struct platform_device *pdev)
+{
+	struct gpio_em_config *pdata = pdev->dev.platform_data;
+	struct em_gio_priv *p;
+	struct resource *io[2], *irq[2];
+	struct gpio_chip *gpio_chip;
+	struct irq_chip *irq_chip;
+	const char *name = dev_name(&pdev->dev);
+	int ret;
+
+	p = kzalloc(sizeof(*p), GFP_KERNEL);
+	if (!p) {
+		dev_err(&pdev->dev, "failed to allocate driver data\n");
+		ret = -ENOMEM;
+		goto err0;
+	}
+
+	p->pdev = pdev;
+	platform_set_drvdata(pdev, p);
+	spin_lock_init(&p->sense_lock);
+
+	io[0] = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	io[1] = platform_get_resource(pdev, IORESOURCE_MEM, 1);
+	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");
+		ret = -EINVAL;
+		goto err1;
+	}
+
+	p->base0 = ioremap_nocache(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;
+	}
+
+	p->base1 = ioremap_nocache(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;
+	}
+
+	gpio_chip = &p->gpio_chip;
+	gpio_chip->direction_input = em_gio_direction_input;
+	gpio_chip->get = em_gio_get;
+	gpio_chip->direction_output = em_gio_direction_output;
+	gpio_chip->set = em_gio_set;
+	gpio_chip->to_irq = em_gio_to_irq;
+	gpio_chip->label = name;
+	gpio_chip->owner = THIS_MODULE;
+	gpio_chip->base = pdata->gpio_base;
+	gpio_chip->ngpio = pdata->number_of_pins;
+
+	irq_chip = &p->irq_chip;
+	irq_chip->name = name;
+	irq_chip->irq_mask = em_gio_irq_disable;
+	irq_chip->irq_unmask = em_gio_irq_enable;
+	irq_chip->irq_enable = em_gio_irq_enable;
+	irq_chip->irq_disable = em_gio_irq_disable;
+	irq_chip->irq_set_type = em_gio_irq_set_type;
+	irq_chip->flags	= IRQCHIP_SKIP_SET_WAKE;
+
+	ret = em_gio_irq_domain_init(p);
+	if (ret) {
+		dev_err(&pdev->dev, "cannot initialize irq domain\n");
+		goto err3;
+	}
+
+	if (request_irq(irq[0]->start, em_gio_irq_handler, 0, name, p)) {
+		dev_err(&pdev->dev, "failed to request low IRQ\n");
+		ret = -ENOENT;
+		goto err4;
+	}
+
+	if (request_irq(irq[1]->start, em_gio_irq_handler, 0, name, p)) {
+		dev_err(&pdev->dev, "failed to request high IRQ\n");
+		ret = -ENOENT;
+		goto err5;
+	}
+
+	ret = gpiochip_add(gpio_chip);
+	if (ret) {
+		dev_err(&pdev->dev, "failed to add GPIO controller\n");
+		goto err6;
+	}
+	return 0;
+
+err6:
+	free_irq(irq[1]->start, pdev);
+err5:
+	free_irq(irq[0]->start, pdev);
+err4:
+	em_gio_irq_domain_cleanup(p);
+err3:
+	iounmap(p->base1);
+err2:
+	iounmap(p->base0);
+err1:
+	kfree(p);
+err0:
+	return ret;
+}
+
+static int __devexit 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);
+	em_gio_irq_domain_cleanup(p);
+	iounmap(p->base1);
+	iounmap(p->base0);
+	kfree(p);
+	return 0;
+}
+
+static struct platform_driver em_gio_device_driver = {
+	.probe		= em_gio_probe,
+	.remove		= __devexit_p(em_gio_remove),
+	.driver		= {
+		.name	= "em_gio",
+	}
+};
+
+module_platform_driver(em_gio_device_driver);
+
+MODULE_AUTHOR("Magnus Damm");
+MODULE_DESCRIPTION("Renesas Emma Mobile GIO Driver");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/gpio/gpio-ep93xx.c b/drivers/gpio/gpio-ep93xx.c
index 776b772523e..9fe5b8fe9be 100644
--- a/drivers/gpio/gpio-ep93xx.c
+++ b/drivers/gpio/gpio-ep93xx.c
@@ -325,7 +325,7 @@ static int ep93xx_gpio_add_bank(struct bgpio_chip *bgc, struct device *dev,
 	void __iomem *dir =  mmio_base + bank->dir;
 	int err;
 
-	err = bgpio_init(bgc, dev, 1, data, NULL, NULL, dir, NULL, false);
+	err = bgpio_init(bgc, dev, 1, data, NULL, NULL, dir, NULL, 0);
 	if (err)
 		return err;
 
diff --git a/drivers/gpio/gpio-generic.c b/drivers/gpio/gpio-generic.c
index e38dd0c3197..82e2e4fe599 100644
--- a/drivers/gpio/gpio-generic.c
+++ b/drivers/gpio/gpio-generic.c
@@ -364,7 +364,7 @@ EXPORT_SYMBOL_GPL(bgpio_remove);
 int bgpio_init(struct bgpio_chip *bgc, struct device *dev,
 	       unsigned long sz, void __iomem *dat, void __iomem *set,
 	       void __iomem *clr, void __iomem *dirout, void __iomem *dirin,
-	       bool big_endian)
+	       unsigned long flags)
 {
 	int ret;
 
@@ -385,7 +385,7 @@ int bgpio_init(struct bgpio_chip *bgc, struct device *dev,
 	if (ret)
 		return ret;
 
-	ret = bgpio_setup_accessors(dev, bgc, big_endian);
+	ret = bgpio_setup_accessors(dev, bgc, flags & BGPIOF_BIG_ENDIAN);
 	if (ret)
 		return ret;
 
@@ -394,6 +394,11 @@ int bgpio_init(struct bgpio_chip *bgc, struct device *dev,
 		return ret;
 
 	bgc->data = bgc->read_reg(bgc->reg_dat);
+	if (bgc->gc.set == bgpio_set_set &&
+			!(flags & BGPIOF_UNREADABLE_REG_SET))
+		bgc->data = bgc->read_reg(bgc->reg_set);
+	if (bgc->reg_dir && !(flags & BGPIOF_UNREADABLE_REG_DIR))
+		bgc->dir = bgc->read_reg(bgc->reg_dir);
 
 	return ret;
 }
@@ -449,7 +454,7 @@ static int __devinit bgpio_pdev_probe(struct platform_device *pdev)
 	void __iomem *dirout;
 	void __iomem *dirin;
 	unsigned long sz;
-	bool be;
+	unsigned long flags = 0;
 	int err;
 	struct bgpio_chip *bgc;
 	struct bgpio_pdata *pdata = dev_get_platdata(dev);
@@ -480,13 +485,14 @@ static int __devinit bgpio_pdev_probe(struct platform_device *pdev)
 	if (err)
 		return err;
 
-	be = !strcmp(platform_get_device_id(pdev)->name, "basic-mmio-gpio-be");
+	if (!strcmp(platform_get_device_id(pdev)->name, "basic-mmio-gpio-be"))
+		flags |= BGPIOF_BIG_ENDIAN;
 
 	bgc = devm_kzalloc(&pdev->dev, sizeof(*bgc), GFP_KERNEL);
 	if (!bgc)
 		return -ENOMEM;
 
-	err = bgpio_init(bgc, dev, sz, dat, set, clr, dirout, dirin, be);
+	err = bgpio_init(bgc, dev, sz, dat, set, clr, dirout, dirin, flags);
 	if (err)
 		return err;
 
diff --git a/drivers/gpio/gpio-ich.c b/drivers/gpio/gpio-ich.c
new file mode 100644
index 00000000000..b7c06517403
--- /dev/null
+++ b/drivers/gpio/gpio-ich.c
@@ -0,0 +1,419 @@
+/*
+ * Intel ICH6-10, Series 5 and 6 GPIO driver
+ *
+ * Copyright (C) 2010 Extreme Engineering Solutions.
+ *
+ * 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 <linux/module.h>
+#include <linux/pci.h>
+#include <linux/gpio.h>
+#include <linux/platform_device.h>
+#include <linux/mfd/lpc_ich.h>
+
+#define DRV_NAME "gpio_ich"
+
+/*
+ * GPIO register offsets in GPIO I/O space.
+ * Each chunk of 32 GPIOs is manipulated via its own USE_SELx, IO_SELx, and
+ * LVLx registers.  Logic in the read/write functions takes a register and
+ * an absolute bit number and determines the proper register offset and bit
+ * number in that register.  For example, to read the value of GPIO bit 50
+ * the code would access offset ichx_regs[2(=GPIO_LVL)][1(=50/32)],
+ * bit 18 (50%32).
+ */
+enum GPIO_REG {
+	GPIO_USE_SEL = 0,
+	GPIO_IO_SEL,
+	GPIO_LVL,
+};
+
+static const u8 ichx_regs[3][3] = {
+	{0x00, 0x30, 0x40},	/* USE_SEL[1-3] offsets */
+	{0x04, 0x34, 0x44},	/* IO_SEL[1-3] offsets */
+	{0x0c, 0x38, 0x48},	/* LVL[1-3] offsets */
+};
+
+#define ICHX_WRITE(val, reg, base_res)	outl(val, (reg) + (base_res)->start)
+#define ICHX_READ(reg, base_res)	inl((reg) + (base_res)->start)
+
+struct ichx_desc {
+	/* Max GPIO pins the chipset can have */
+	uint ngpio;
+
+	/* Whether the chipset has GPIO in GPE0_STS in the PM IO region */
+	bool uses_gpe0;
+
+	/* USE_SEL is bogus on some chipsets, eg 3100 */
+	u32 use_sel_ignore[3];
+
+	/* Some chipsets have quirks, let these use their own request/get */
+	int (*request)(struct gpio_chip *chip, unsigned offset);
+	int (*get)(struct gpio_chip *chip, unsigned offset);
+};
+
+static struct {
+	spinlock_t lock;
+	struct platform_device *dev;
+	struct gpio_chip chip;
+	struct resource *gpio_base;	/* GPIO IO base */
+	struct resource *pm_base;	/* Power Mangagment IO base */
+	struct ichx_desc *desc;	/* Pointer to chipset-specific description */
+	u32 orig_gpio_ctrl;	/* Orig CTRL value, used to restore on exit */
+} ichx_priv;
+
+static int modparam_gpiobase = -1;	/* dynamic */
+module_param_named(gpiobase, modparam_gpiobase, int, 0444);
+MODULE_PARM_DESC(gpiobase, "The GPIO number base. -1 means dynamic, "
+			   "which is the default.");
+
+static int ichx_write_bit(int reg, unsigned nr, int val, int verify)
+{
+	unsigned long flags;
+	u32 data, tmp;
+	int reg_nr = nr / 32;
+	int bit = nr & 0x1f;
+	int ret = 0;
+
+	spin_lock_irqsave(&ichx_priv.lock, flags);
+
+	data = ICHX_READ(ichx_regs[reg][reg_nr], ichx_priv.gpio_base);
+	if (val)
+		data |= 1 << bit;
+	else
+		data &= ~(1 << bit);
+	ICHX_WRITE(data, ichx_regs[reg][reg_nr], ichx_priv.gpio_base);
+	tmp = ICHX_READ(ichx_regs[reg][reg_nr], ichx_priv.gpio_base);
+	if (verify && data != tmp)
+		ret = -EPERM;
+
+	spin_unlock_irqrestore(&ichx_priv.lock, flags);
+
+	return ret;
+}
+
+static int ichx_read_bit(int reg, unsigned nr)
+{
+	unsigned long flags;
+	u32 data;
+	int reg_nr = nr / 32;
+	int bit = nr & 0x1f;
+
+	spin_lock_irqsave(&ichx_priv.lock, flags);
+
+	data = ICHX_READ(ichx_regs[reg][reg_nr], ichx_priv.gpio_base);
+
+	spin_unlock_irqrestore(&ichx_priv.lock, flags);
+
+	return data & (1 << bit) ? 1 : 0;
+}
+
+static int ichx_gpio_direction_input(struct gpio_chip *gpio, unsigned nr)
+{
+	/*
+	 * Try setting pin as an input and verify it worked since many pins
+	 * are output-only.
+	 */
+	if (ichx_write_bit(GPIO_IO_SEL, nr, 1, 1))
+		return -EINVAL;
+
+	return 0;
+}
+
+static int ichx_gpio_direction_output(struct gpio_chip *gpio, unsigned nr,
+					int val)
+{
+	/* Set GPIO output value. */
+	ichx_write_bit(GPIO_LVL, nr, val, 0);
+
+	/*
+	 * Try setting pin as an output and verify it worked since many pins
+	 * are input-only.
+	 */
+	if (ichx_write_bit(GPIO_IO_SEL, nr, 0, 1))
+		return -EINVAL;
+
+	return 0;
+}
+
+static int ichx_gpio_get(struct gpio_chip *chip, unsigned nr)
+{
+	return ichx_read_bit(GPIO_LVL, nr);
+}
+
+static int ich6_gpio_get(struct gpio_chip *chip, unsigned nr)
+{
+	unsigned long flags;
+	u32 data;
+
+	/*
+	 * GPI 0 - 15 need to be read from the power management registers on
+	 * a ICH6/3100 bridge.
+	 */
+	if (nr < 16) {
+		if (!ichx_priv.pm_base)
+			return -ENXIO;
+
+		spin_lock_irqsave(&ichx_priv.lock, flags);
+
+		/* GPI 0 - 15 are latched, write 1 to clear*/
+		ICHX_WRITE(1 << (16 + nr), 0, ichx_priv.pm_base);
+		data = ICHX_READ(0, ichx_priv.pm_base);
+
+		spin_unlock_irqrestore(&ichx_priv.lock, flags);
+
+		return (data >> 16) & (1 << nr) ? 1 : 0;
+	} else {
+		return ichx_gpio_get(chip, nr);
+	}
+}
+
+static int ichx_gpio_request(struct gpio_chip *chip, unsigned nr)
+{
+	/*
+	 * 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
+	 * the chipset's USE value can be trusted for this specific bit.
+	 * If it can't be trusted, assume that the pin can be used as a GPIO.
+	 */
+	if (ichx_priv.desc->use_sel_ignore[nr / 32] & (1 << (nr & 0x1f)))
+		return 1;
+
+	return ichx_read_bit(GPIO_USE_SEL, nr) ? 0 : -ENODEV;
+}
+
+static int ich6_gpio_request(struct gpio_chip *chip, unsigned nr)
+{
+	/*
+	 * Fixups for bits 16 and 17 are necessary on the Intel ICH6/3100
+	 * bridge as they are controlled by USE register bits 0 and 1.  See
+	 * "Table 704 GPIO_USE_SEL1 register" in the i3100 datasheet for
+	 * additional info.
+	 */
+	if (nr == 16 || nr == 17)
+		nr -= 16;
+
+	return ichx_gpio_request(chip, nr);
+}
+
+static void ichx_gpio_set(struct gpio_chip *chip, unsigned nr, int val)
+{
+	ichx_write_bit(GPIO_LVL, nr, val, 0);
+}
+
+static void __devinit ichx_gpiolib_setup(struct gpio_chip *chip)
+{
+	chip->owner = THIS_MODULE;
+	chip->label = DRV_NAME;
+	chip->dev = &ichx_priv.dev->dev;
+
+	/* Allow chip-specific overrides of request()/get() */
+	chip->request = ichx_priv.desc->request ?
+		ichx_priv.desc->request : ichx_gpio_request;
+	chip->get = ichx_priv.desc->get ?
+		ichx_priv.desc->get : ichx_gpio_get;
+
+	chip->set = ichx_gpio_set;
+	chip->direction_input = ichx_gpio_direction_input;
+	chip->direction_output = ichx_gpio_direction_output;
+	chip->base = modparam_gpiobase;
+	chip->ngpio = ichx_priv.desc->ngpio;
+	chip->can_sleep = 0;
+	chip->dbg_show = NULL;
+}
+
+/* ICH6-based, 631xesb-based */
+static struct ichx_desc ich6_desc = {
+	/* Bridges using the ICH6 controller need fixups for GPIO 0 - 17 */
+	.request = ich6_gpio_request,
+	.get = ich6_gpio_get,
+
+	/* GPIO 0-15 are read in the GPE0_STS PM register */
+	.uses_gpe0 = true,
+
+	.ngpio = 50,
+};
+
+/* Intel 3100 */
+static struct ichx_desc i3100_desc = {
+	/*
+	 * Bits 16,17, 20 of USE_SEL and bit 16 of USE_SEL2 always read 0 on
+	 * the Intel 3100.  See "Table 712. GPIO Summary Table" of 3100
+	 * Datasheet for more info.
+	 */
+	.use_sel_ignore = {0x00130000, 0x00010000, 0x0},
+
+	/* The 3100 needs fixups for GPIO 0 - 17 */
+	.request = ich6_gpio_request,
+	.get = ich6_gpio_get,
+
+	/* GPIO 0-15 are read in the GPE0_STS PM register */
+	.uses_gpe0 = true,
+
+	.ngpio = 50,
+};
+
+/* ICH7 and ICH8-based */
+static struct ichx_desc ich7_desc = {
+	.ngpio = 50,
+};
+
+/* ICH9-based */
+static struct ichx_desc ich9_desc = {
+	.ngpio = 61,
+};
+
+/* ICH10-based - Consumer/corporate versions have different amount of GPIO */
+static struct ichx_desc ich10_cons_desc = {
+	.ngpio = 61,
+};
+static struct ichx_desc ich10_corp_desc = {
+	.ngpio = 72,
+};
+
+/* Intel 5 series, 6 series, 3400 series, and C200 series */
+static struct ichx_desc intel5_desc = {
+	.ngpio = 76,
+};
+
+static int __devinit ichx_gpio_probe(struct platform_device *pdev)
+{
+	struct resource *res_base, *res_pm;
+	int err;
+	struct lpc_ich_info *ich_info = pdev->dev.platform_data;
+
+	if (!ich_info)
+		return -ENODEV;
+
+	ichx_priv.dev = pdev;
+
+	switch (ich_info->gpio_version) {
+	case ICH_I3100_GPIO:
+		ichx_priv.desc = &i3100_desc;
+		break;
+	case ICH_V5_GPIO:
+		ichx_priv.desc = &intel5_desc;
+		break;
+	case ICH_V6_GPIO:
+		ichx_priv.desc = &ich6_desc;
+		break;
+	case ICH_V7_GPIO:
+		ichx_priv.desc = &ich7_desc;
+		break;
+	case ICH_V9_GPIO:
+		ichx_priv.desc = &ich9_desc;
+		break;
+	case ICH_V10CORP_GPIO:
+		ichx_priv.desc = &ich10_corp_desc;
+		break;
+	case ICH_V10CONS_GPIO:
+		ichx_priv.desc = &ich10_cons_desc;
+		break;
+	default:
+		return -ENODEV;
+	}
+
+	res_base = platform_get_resource(pdev, IORESOURCE_IO, ICH_RES_GPIO);
+	if (!res_base || !res_base->start || !res_base->end)
+		return -ENODEV;
+
+	if (!request_region(res_base->start, resource_size(res_base),
+				pdev->name))
+		return -EBUSY;
+
+	ichx_priv.gpio_base = res_base;
+
+	/*
+	 * If necessary, determine the I/O address of ACPI/power management
+	 * registers which are needed to read the the GPE0 register for GPI pins
+	 * 0 - 15 on some chipsets.
+	 */
+	if (!ichx_priv.desc->uses_gpe0)
+		goto init;
+
+	res_pm = platform_get_resource(pdev, IORESOURCE_IO, ICH_RES_GPE0);
+	if (!res_pm) {
+		pr_warn("ACPI BAR is unavailable, GPI 0 - 15 unavailable\n");
+		goto init;
+	}
+
+	if (!request_region(res_pm->start, resource_size(res_pm),
+			pdev->name)) {
+		pr_warn("ACPI BAR is busy, GPI 0 - 15 unavailable\n");
+		goto init;
+	}
+
+	ichx_priv.pm_base = res_pm;
+
+init:
+	ichx_gpiolib_setup(&ichx_priv.chip);
+	err = gpiochip_add(&ichx_priv.chip);
+	if (err) {
+		pr_err("Failed to register GPIOs\n");
+		goto add_err;
+	}
+
+	pr_info("GPIO from %d to %d on %s\n", ichx_priv.chip.base,
+	       ichx_priv.chip.base + ichx_priv.chip.ngpio - 1, DRV_NAME);
+
+	return 0;
+
+add_err:
+	release_region(ichx_priv.gpio_base->start,
+			resource_size(ichx_priv.gpio_base));
+	if (ichx_priv.pm_base)
+		release_region(ichx_priv.pm_base->start,
+				resource_size(ichx_priv.pm_base));
+	return err;
+}
+
+static int __devexit ichx_gpio_remove(struct platform_device *pdev)
+{
+	int err;
+
+	err = gpiochip_remove(&ichx_priv.chip);
+	if (err) {
+		dev_err(&pdev->dev, "%s failed, %d\n",
+				"gpiochip_remove()", err);
+		return err;
+	}
+
+	release_region(ichx_priv.gpio_base->start,
+				resource_size(ichx_priv.gpio_base));
+	if (ichx_priv.pm_base)
+		release_region(ichx_priv.pm_base->start,
+				resource_size(ichx_priv.pm_base));
+
+	return 0;
+}
+
+static struct platform_driver ichx_gpio_driver = {
+	.driver		= {
+		.owner	= THIS_MODULE,
+		.name	= DRV_NAME,
+	},
+	.probe		= ichx_gpio_probe,
+	.remove		= __devexit_p(ichx_gpio_remove),
+};
+
+module_platform_driver(ichx_gpio_driver);
+
+MODULE_AUTHOR("Peter Tyser <ptyser@xes-inc.com>");
+MODULE_DESCRIPTION("GPIO interface for Intel ICH series");
+MODULE_LICENSE("GPL");
+MODULE_ALIAS("platform:"DRV_NAME);
diff --git a/drivers/gpio/gpio-langwell.c b/drivers/gpio/gpio-langwell.c
index 00692e89ef8..a1c8754f52c 100644
--- a/drivers/gpio/gpio-langwell.c
+++ b/drivers/gpio/gpio-langwell.c
@@ -36,6 +36,7 @@
 #include <linux/gpio.h>
 #include <linux/slab.h>
 #include <linux/pm_runtime.h>
+#include <linux/irqdomain.h>
 
 /*
  * Langwell chip has 64 pins and thus there are 2 32bit registers to control
@@ -66,8 +67,8 @@ struct lnw_gpio {
 	struct gpio_chip		chip;
 	void				*reg_base;
 	spinlock_t			lock;
-	unsigned			irq_base;
 	struct pci_dev			*pdev;
+	struct irq_domain		*domain;
 };
 
 static void __iomem *gpio_reg(struct gpio_chip *chip, unsigned offset,
@@ -176,13 +177,13 @@ static int lnw_gpio_direction_output(struct gpio_chip *chip,
 static int lnw_gpio_to_irq(struct gpio_chip *chip, unsigned offset)
 {
 	struct lnw_gpio *lnw = container_of(chip, struct lnw_gpio, chip);
-	return lnw->irq_base + offset;
+	return irq_create_mapping(lnw->domain, offset);
 }
 
 static int lnw_irq_type(struct irq_data *d, unsigned type)
 {
 	struct lnw_gpio *lnw = irq_data_get_irq_chip_data(d);
-	u32 gpio = d->irq - lnw->irq_base;
+	u32 gpio = irqd_to_hwirq(d);
 	unsigned long flags;
 	u32 value;
 	void __iomem *grer = gpio_reg(&lnw->chip, gpio, GRER);
@@ -249,20 +250,55 @@ static void lnw_irq_handler(unsigned irq, struct irq_desc *desc)
 	/* check GPIO controller to check which pin triggered the interrupt */
 	for (base = 0; base < lnw->chip.ngpio; base += 32) {
 		gedr = gpio_reg(&lnw->chip, base, GEDR);
-		pending = readl(gedr);
-		while (pending) {
+		while ((pending = readl(gedr))) {
 			gpio = __ffs(pending);
 			mask = BIT(gpio);
-			pending &= ~mask;
 			/* Clear before handling so we can't lose an edge */
 			writel(mask, gedr);
-			generic_handle_irq(lnw->irq_base + base + gpio);
+			generic_handle_irq(irq_find_mapping(lnw->domain,
+							    base + gpio));
 		}
 	}
 
 	chip->irq_eoi(data);
 }
 
+static void lnw_irq_init_hw(struct lnw_gpio *lnw)
+{
+	void __iomem *reg;
+	unsigned base;
+
+	for (base = 0; base < lnw->chip.ngpio; base += 32) {
+		/* Clear the rising-edge detect register */
+		reg = gpio_reg(&lnw->chip, base, GRER);
+		writel(0, reg);
+		/* Clear the falling-edge detect register */
+		reg = gpio_reg(&lnw->chip, base, GFER);
+		writel(0, reg);
+		/* Clear the edge detect status register */
+		reg = gpio_reg(&lnw->chip, base, GEDR);
+		writel(~0, reg);
+	}
+}
+
+static int lnw_gpio_irq_map(struct irq_domain *d, unsigned int virq,
+			    irq_hw_number_t hw)
+{
+	struct lnw_gpio *lnw = d->host_data;
+
+	irq_set_chip_and_handler_name(virq, &lnw_irqchip, handle_simple_irq,
+				      "demux");
+	irq_set_chip_data(virq, lnw);
+	irq_set_irq_type(virq, IRQ_TYPE_NONE);
+
+	return 0;
+}
+
+static const struct irq_domain_ops lnw_gpio_irq_ops = {
+	.map = lnw_gpio_irq_map,
+	.xlate = irq_domain_xlate_twocell,
+};
+
 #ifdef CONFIG_PM
 static int lnw_gpio_runtime_resume(struct device *dev)
 {
@@ -300,23 +336,22 @@ static int __devinit lnw_gpio_probe(struct pci_dev *pdev,
 			const struct pci_device_id *id)
 {
 	void *base;
-	int i;
 	resource_size_t start, len;
 	struct lnw_gpio *lnw;
-	u32 irq_base;
 	u32 gpio_base;
 	int retval = 0;
+	int ngpio = id->driver_data;
 
 	retval = pci_enable_device(pdev);
 	if (retval)
-		goto done;
+		return retval;
 
 	retval = pci_request_regions(pdev, "langwell_gpio");
 	if (retval) {
 		dev_err(&pdev->dev, "error requesting resources\n");
 		goto err2;
 	}
-	/* get the irq_base from bar1 */
+	/* get the gpio_base from bar1 */
 	start = pci_resource_start(pdev, 1);
 	len = pci_resource_len(pdev, 1);
 	base = ioremap_nocache(start, len);
@@ -324,28 +359,32 @@ static int __devinit lnw_gpio_probe(struct pci_dev *pdev,
 		dev_err(&pdev->dev, "error mapping bar1\n");
 		goto err3;
 	}
-	irq_base = *(u32 *)base;
 	gpio_base = *((u32 *)base + 1);
 	/* release the IO mapping, since we already get the info from bar1 */
 	iounmap(base);
 	/* get the register base from bar0 */
 	start = pci_resource_start(pdev, 0);
 	len = pci_resource_len(pdev, 0);
-	base = ioremap_nocache(start, len);
+	base = devm_ioremap_nocache(&pdev->dev, start, len);
 	if (!base) {
 		dev_err(&pdev->dev, "error mapping bar0\n");
 		retval = -EFAULT;
 		goto err3;
 	}
 
-	lnw = kzalloc(sizeof(struct lnw_gpio), GFP_KERNEL);
+	lnw = devm_kzalloc(&pdev->dev, sizeof(struct lnw_gpio), GFP_KERNEL);
 	if (!lnw) {
 		dev_err(&pdev->dev, "can't allocate langwell_gpio chip data\n");
 		retval = -ENOMEM;
-		goto err4;
+		goto err3;
 	}
+
+	lnw->domain = irq_domain_add_linear(pdev->dev.of_node, ngpio,
+					    &lnw_gpio_irq_ops, lnw);
+	if (!lnw->domain)
+		goto err3;
+
 	lnw->reg_base = base;
-	lnw->irq_base = irq_base;
 	lnw->chip.label = dev_name(&pdev->dev);
 	lnw->chip.request = lnw_gpio_request;
 	lnw->chip.direction_input = lnw_gpio_direction_input;
@@ -354,38 +393,32 @@ static int __devinit lnw_gpio_probe(struct pci_dev *pdev,
 	lnw->chip.set = lnw_gpio_set;
 	lnw->chip.to_irq = lnw_gpio_to_irq;
 	lnw->chip.base = gpio_base;
-	lnw->chip.ngpio = id->driver_data;
+	lnw->chip.ngpio = ngpio;
 	lnw->chip.can_sleep = 0;
 	lnw->pdev = pdev;
 	pci_set_drvdata(pdev, lnw);
 	retval = gpiochip_add(&lnw->chip);
 	if (retval) {
 		dev_err(&pdev->dev, "langwell gpiochip_add error %d\n", retval);
-		goto err5;
+		goto err3;
 	}
+
+	lnw_irq_init_hw(lnw);
+
 	irq_set_handler_data(pdev->irq, lnw);
 	irq_set_chained_handler(pdev->irq, lnw_irq_handler);
-	for (i = 0; i < lnw->chip.ngpio; i++) {
-		irq_set_chip_and_handler_name(i + lnw->irq_base, &lnw_irqchip,
-					      handle_simple_irq, "demux");
-		irq_set_chip_data(i + lnw->irq_base, lnw);
-	}
 
 	spin_lock_init(&lnw->lock);
 
 	pm_runtime_put_noidle(&pdev->dev);
 	pm_runtime_allow(&pdev->dev);
 
-	goto done;
-err5:
-	kfree(lnw);
-err4:
-	iounmap(base);
+	return 0;
+
 err3:
 	pci_release_regions(pdev);
 err2:
 	pci_disable_device(pdev);
-done:
 	return retval;
 }
 
diff --git a/drivers/gpio/gpio-lpc32xx.c b/drivers/gpio/gpio-lpc32xx.c
index 61c2d08d37b..c2199beca98 100644
--- a/drivers/gpio/gpio-lpc32xx.c
+++ b/drivers/gpio/gpio-lpc32xx.c
@@ -21,6 +21,9 @@
 #include <linux/io.h>
 #include <linux/errno.h>
 #include <linux/gpio.h>
+#include <linux/of_gpio.h>
+#include <linux/platform_device.h>
+#include <linux/module.h>
 
 #include <mach/hardware.h>
 #include <mach/platform.h>
@@ -454,10 +457,57 @@ static struct lpc32xx_gpio_chip lpc32xx_gpiochip[] = {
 	},
 };
 
+/* Empty now, can be removed later when mach-lpc32xx is finally switched over
+ * to DT support
+ */
 void __init lpc32xx_gpio_init(void)
 {
+}
+
+static int lpc32xx_of_xlate(struct gpio_chip *gc,
+			    const struct of_phandle_args *gpiospec, u32 *flags)
+{
+	/* Is this the correct bank? */
+	u32 bank = gpiospec->args[0];
+	if ((bank > ARRAY_SIZE(lpc32xx_gpiochip) ||
+	    (gc != &lpc32xx_gpiochip[bank].chip)))
+		return -EINVAL;
+
+	if (flags)
+		*flags = gpiospec->args[2];
+	return gpiospec->args[1];
+}
+
+static int __devinit lpc32xx_gpio_probe(struct platform_device *pdev)
+{
 	int i;
 
-	for (i = 0; i < ARRAY_SIZE(lpc32xx_gpiochip); i++)
+	for (i = 0; i < ARRAY_SIZE(lpc32xx_gpiochip); i++) {
+		if (pdev->dev.of_node) {
+			lpc32xx_gpiochip[i].chip.of_xlate = lpc32xx_of_xlate;
+			lpc32xx_gpiochip[i].chip.of_gpio_n_cells = 3;
+			lpc32xx_gpiochip[i].chip.of_node = pdev->dev.of_node;
+		}
 		gpiochip_add(&lpc32xx_gpiochip[i].chip);
+	}
+
+	return 0;
 }
+
+#ifdef CONFIG_OF
+static struct of_device_id lpc32xx_gpio_of_match[] __devinitdata = {
+	{ .compatible = "nxp,lpc3220-gpio", },
+	{ },
+};
+#endif
+
+static struct platform_driver lpc32xx_gpio_driver = {
+	.driver		= {
+		.name	= "lpc32xx-gpio",
+		.owner	= THIS_MODULE,
+		.of_match_table = of_match_ptr(lpc32xx_gpio_of_match),
+	},
+	.probe		= lpc32xx_gpio_probe,
+};
+
+module_platform_driver(lpc32xx_gpio_driver);
diff --git a/drivers/gpio/gpio-mcp23s08.c b/drivers/gpio/gpio-mcp23s08.c
index c5d83a8a91c..0f425189de1 100644
--- a/drivers/gpio/gpio-mcp23s08.c
+++ b/drivers/gpio/gpio-mcp23s08.c
@@ -353,7 +353,7 @@ static void mcp23s08_dbg_show(struct seq_file *s, struct gpio_chip *chip)
 			chip->base + t, bank, t, label,
 			(mcp->cache[MCP_IODIR] & mask) ? "in " : "out",
 			(mcp->cache[MCP_GPIO] & mask) ? "hi" : "lo",
-			(mcp->cache[MCP_GPPU] & mask) ? "  " : "up");
+			(mcp->cache[MCP_GPPU] & mask) ? "up" : "  ");
 		/* NOTE:  ignoring the irq-related registers */
 		seq_printf(s, "\n");
 	}
diff --git a/drivers/gpio/gpio-ml-ioh.c b/drivers/gpio/gpio-ml-ioh.c
index f0febe5b822..db01f151d41 100644
--- a/drivers/gpio/gpio-ml-ioh.c
+++ b/drivers/gpio/gpio-ml-ioh.c
@@ -611,17 +611,7 @@ static struct pci_driver ioh_gpio_driver = {
 	.resume = ioh_gpio_resume
 };
 
-static int __init ioh_gpio_pci_init(void)
-{
-	return pci_register_driver(&ioh_gpio_driver);
-}
-module_init(ioh_gpio_pci_init);
-
-static void __exit ioh_gpio_pci_exit(void)
-{
-	pci_unregister_driver(&ioh_gpio_driver);
-}
-module_exit(ioh_gpio_pci_exit);
+module_pci_driver(ioh_gpio_driver);
 
 MODULE_DESCRIPTION("OKI SEMICONDUCTOR ML-IOH series GPIO Driver");
 MODULE_LICENSE("GPL");
diff --git a/drivers/gpio/gpio-mm-lantiq.c b/drivers/gpio/gpio-mm-lantiq.c
new file mode 100644
index 00000000000..2983dfbd066
--- /dev/null
+++ b/drivers/gpio/gpio-mm-lantiq.c
@@ -0,0 +1,158 @@
+/*
+ *  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.
+ *
+ *  Copyright (C) 2012 John Crispin <blogic@openwrt.org>
+ */
+
+#include <linux/init.h>
+#include <linux/module.h>
+#include <linux/types.h>
+#include <linux/platform_device.h>
+#include <linux/mutex.h>
+#include <linux/gpio.h>
+#include <linux/of.h>
+#include <linux/of_gpio.h>
+#include <linux/io.h>
+#include <linux/slab.h>
+
+#include <lantiq_soc.h>
+
+/*
+ * By attaching hardware latches to the EBU it is possible to create output
+ * only gpios. This driver configures a special memory address, which when
+ * written to outputs 16 bit to the latches.
+ */
+
+#define LTQ_EBU_BUSCON	0x1e7ff		/* 16 bit access, slowest timing */
+#define LTQ_EBU_WP	0x80000000	/* write protect bit */
+
+struct ltq_mm {
+	struct of_mm_gpio_chip mmchip;
+	u16 shadow;	/* shadow the latches state */
+};
+
+/**
+ * ltq_mm_apply() - write the shadow value to the ebu address.
+ * @chip:     Pointer to our private data structure.
+ *
+ * Write the shadow value to the EBU to set the gpios. We need to set the
+ * global EBU lock to make sure that PCI/MTD dont break.
+ */
+static void ltq_mm_apply(struct ltq_mm *chip)
+{
+	unsigned long flags;
+
+	spin_lock_irqsave(&ebu_lock, flags);
+	ltq_ebu_w32(LTQ_EBU_BUSCON, LTQ_EBU_BUSCON1);
+	__raw_writew(chip->shadow, chip->mmchip.regs);
+	ltq_ebu_w32(LTQ_EBU_BUSCON | LTQ_EBU_WP, LTQ_EBU_BUSCON1);
+	spin_unlock_irqrestore(&ebu_lock, flags);
+}
+
+/**
+ * ltq_mm_set() - gpio_chip->set - set gpios.
+ * @gc:     Pointer to gpio_chip device structure.
+ * @gpio:   GPIO signal number.
+ * @val:    Value to be written to specified signal.
+ *
+ * Set the shadow value and call ltq_mm_apply.
+ */
+static void ltq_mm_set(struct gpio_chip *gc, unsigned offset, int value)
+{
+	struct of_mm_gpio_chip *mm_gc = to_of_mm_gpio_chip(gc);
+	struct ltq_mm *chip =
+		container_of(mm_gc, struct ltq_mm, mmchip);
+
+	if (value)
+		chip->shadow |= (1 << offset);
+	else
+		chip->shadow &= ~(1 << offset);
+	ltq_mm_apply(chip);
+}
+
+/**
+ * ltq_mm_dir_out() - gpio_chip->dir_out - set gpio direction.
+ * @gc:     Pointer to gpio_chip device structure.
+ * @gpio:   GPIO signal number.
+ * @val:    Value to be written to specified signal.
+ *
+ * Same as ltq_mm_set, always returns 0.
+ */
+static int ltq_mm_dir_out(struct gpio_chip *gc, unsigned offset, int value)
+{
+	ltq_mm_set(gc, offset, value);
+
+	return 0;
+}
+
+/**
+ * ltq_mm_save_regs() - Set initial values of GPIO pins
+ * @mm_gc: pointer to memory mapped GPIO chip structure
+ */
+static void ltq_mm_save_regs(struct of_mm_gpio_chip *mm_gc)
+{
+	struct ltq_mm *chip =
+		container_of(mm_gc, struct ltq_mm, mmchip);
+
+	/* tell the ebu controller which memory address we will be using */
+	ltq_ebu_w32(CPHYSADDR(chip->mmchip.regs) | 0x1, LTQ_EBU_ADDRSEL1);
+
+	ltq_mm_apply(chip);
+}
+
+static int ltq_mm_probe(struct platform_device *pdev)
+{
+	struct resource *res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	struct ltq_mm *chip;
+	const __be32 *shadow;
+	int ret = 0;
+
+	if (!res) {
+		dev_err(&pdev->dev, "failed to get memory resource\n");
+		return -ENOENT;
+	}
+
+	chip = kzalloc(sizeof(*chip), GFP_KERNEL);
+	if (!chip)
+		return -ENOMEM;
+
+	chip->mmchip.gc.ngpio = 16;
+	chip->mmchip.gc.label = "gpio-mm-ltq";
+	chip->mmchip.gc.direction_output = ltq_mm_dir_out;
+	chip->mmchip.gc.set = ltq_mm_set;
+	chip->mmchip.save_regs = ltq_mm_save_regs;
+
+	/* store the shadow value if one was passed by the devicetree */
+	shadow = of_get_property(pdev->dev.of_node, "lantiq,shadow", NULL);
+	if (shadow)
+		chip->shadow = be32_to_cpu(*shadow);
+
+	ret = of_mm_gpiochip_add(pdev->dev.of_node, &chip->mmchip);
+	if (ret)
+		kfree(chip);
+	return ret;
+}
+
+static const struct of_device_id ltq_mm_match[] = {
+	{ .compatible = "lantiq,gpio-mm" },
+	{},
+};
+MODULE_DEVICE_TABLE(of, ltq_mm_match);
+
+static struct platform_driver ltq_mm_driver = {
+	.probe = ltq_mm_probe,
+	.driver = {
+		.name = "gpio-mm-ltq",
+		.owner = THIS_MODULE,
+		.of_match_table = ltq_mm_match,
+	},
+};
+
+static int __init ltq_mm_init(void)
+{
+	return platform_driver_register(&ltq_mm_driver);
+}
+
+subsys_initcall(ltq_mm_init);
diff --git a/drivers/gpio/gpio-mpc8xxx.c b/drivers/gpio/gpio-mpc8xxx.c
index e6568c19c93..5a1817eedd1 100644
--- a/drivers/gpio/gpio-mpc8xxx.c
+++ b/drivers/gpio/gpio-mpc8xxx.c
@@ -163,7 +163,8 @@ static void mpc8xxx_gpio_irq_cascade(unsigned int irq, struct irq_desc *desc)
 	if (mask)
 		generic_handle_irq(irq_linear_revmap(mpc8xxx_gc->irq,
 						     32 - ffs(mask)));
-	chip->irq_eoi(&desc->irq_data);
+	if (chip->irq_eoi)
+		chip->irq_eoi(&desc->irq_data);
 }
 
 static void mpc8xxx_irq_unmask(struct irq_data *d)
diff --git a/drivers/gpio/gpio-msic.c b/drivers/gpio/gpio-msic.c
new file mode 100644
index 00000000000..71a838f4450
--- /dev/null
+++ b/drivers/gpio/gpio-msic.c
@@ -0,0 +1,339 @@
+/*
+ * Intel Medfield MSIC GPIO driver>
+ * Copyright (c) 2011, Intel Corporation.
+ *
+ * Author: Mathias Nyman <mathias.nyman@linux.intel.com>
+ * Based on intel_pmic_gpio.c
+ *
+ * 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.
+ *
+ */
+
+#include <linux/module.h>
+#include <linux/kernel.h>
+#include <linux/slab.h>
+#include <linux/interrupt.h>
+#include <linux/init.h>
+#include <linux/gpio.h>
+#include <linux/platform_device.h>
+#include <linux/mfd/intel_msic.h>
+
+/* the offset for the mapping of global gpio pin to irq */
+#define MSIC_GPIO_IRQ_OFFSET	0x100
+
+#define MSIC_GPIO_DIR_IN	0
+#define MSIC_GPIO_DIR_OUT	BIT(5)
+#define MSIC_GPIO_TRIG_FALL	BIT(1)
+#define MSIC_GPIO_TRIG_RISE	BIT(2)
+
+/* masks for msic gpio output GPIOxxxxCTLO registers */
+#define MSIC_GPIO_DIR_MASK	BIT(5)
+#define MSIC_GPIO_DRV_MASK	BIT(4)
+#define MSIC_GPIO_REN_MASK	BIT(3)
+#define MSIC_GPIO_RVAL_MASK	(BIT(2) | BIT(1))
+#define MSIC_GPIO_DOUT_MASK	BIT(0)
+
+/* masks for msic gpio input GPIOxxxxCTLI registers */
+#define MSIC_GPIO_GLBYP_MASK	BIT(5)
+#define MSIC_GPIO_DBNC_MASK	(BIT(4) | BIT(3))
+#define MSIC_GPIO_INTCNT_MASK	(BIT(2) | BIT(1))
+#define MSIC_GPIO_DIN_MASK	BIT(0)
+
+#define MSIC_NUM_GPIO		24
+
+struct msic_gpio {
+	struct platform_device	*pdev;
+	struct mutex		buslock;
+	struct gpio_chip	chip;
+	int			irq;
+	unsigned		irq_base;
+	unsigned long		trig_change_mask;
+	unsigned		trig_type;
+};
+
+/*
+ * MSIC has 24 gpios, 16 low voltage (1.2-1.8v) and 8 high voltage (3v).
+ * Both the high and low voltage gpios are divided in two banks.
+ * GPIOs are numbered with GPIO0LV0 as gpio_base in the following order:
+ * GPIO0LV0..GPIO0LV7: low voltage, bank 0, gpio_base
+ * GPIO1LV0..GPIO1LV7: low voltage, bank 1,  gpio_base + 8
+ * GPIO0HV0..GPIO0HV3: high voltage, bank 0, gpio_base + 16
+ * GPIO1HV0..GPIO1HV3: high voltage, bank 1, gpio_base + 20
+ */
+
+static int msic_gpio_to_ireg(unsigned offset)
+{
+	if (offset >= MSIC_NUM_GPIO)
+		return -EINVAL;
+
+	if (offset < 8)
+		return INTEL_MSIC_GPIO0LV0CTLI - offset;
+	if (offset < 16)
+		return INTEL_MSIC_GPIO1LV0CTLI - offset + 8;
+	if (offset < 20)
+		return INTEL_MSIC_GPIO0HV0CTLI - offset + 16;
+
+	return INTEL_MSIC_GPIO1HV0CTLI - offset + 20;
+}
+
+static int msic_gpio_to_oreg(unsigned offset)
+{
+	if (offset >= MSIC_NUM_GPIO)
+		return -EINVAL;
+
+	if (offset < 8)
+		return INTEL_MSIC_GPIO0LV0CTLO - offset;
+	if (offset < 16)
+		return INTEL_MSIC_GPIO1LV0CTLO - offset + 8;
+	if (offset < 20)
+		return INTEL_MSIC_GPIO0HV0CTLO - offset + 16;
+
+	return INTEL_MSIC_GPIO1HV0CTLO + offset + 20;
+}
+
+static int msic_gpio_direction_input(struct gpio_chip *chip, unsigned offset)
+{
+	int reg;
+
+	reg = msic_gpio_to_oreg(offset);
+	if (reg < 0)
+		return reg;
+
+	return intel_msic_reg_update(reg, MSIC_GPIO_DIR_IN, MSIC_GPIO_DIR_MASK);
+}
+
+static int msic_gpio_direction_output(struct gpio_chip *chip,
+			unsigned offset, int value)
+{
+	int reg;
+	unsigned mask;
+
+	value = (!!value) | MSIC_GPIO_DIR_OUT;
+	mask = MSIC_GPIO_DIR_MASK | MSIC_GPIO_DOUT_MASK;
+
+	reg = msic_gpio_to_oreg(offset);
+	if (reg < 0)
+		return reg;
+
+	return intel_msic_reg_update(reg, value, mask);
+}
+
+static int msic_gpio_get(struct gpio_chip *chip, unsigned offset)
+{
+	u8 r;
+	int ret;
+	int reg;
+
+	reg = msic_gpio_to_ireg(offset);
+	if (reg < 0)
+		return reg;
+
+	ret = intel_msic_reg_read(reg, &r);
+	if (ret < 0)
+		return ret;
+
+	return r & MSIC_GPIO_DIN_MASK;
+}
+
+static void msic_gpio_set(struct gpio_chip *chip, unsigned offset, int value)
+{
+	int reg;
+
+	reg = msic_gpio_to_oreg(offset);
+	if (reg < 0)
+		return;
+
+	intel_msic_reg_update(reg, !!value , MSIC_GPIO_DOUT_MASK);
+}
+
+/*
+ * This is called from genirq with mg->buslock locked and
+ * irq_desc->lock held. We can not access the scu bus here, so we
+ * store the change and update in the bus_sync_unlock() function below
+ */
+static int msic_irq_type(struct irq_data *data, unsigned type)
+{
+	struct msic_gpio *mg = irq_data_get_irq_chip_data(data);
+	u32 gpio = data->irq - mg->irq_base;
+
+	if (gpio >= mg->chip.ngpio)
+		return -EINVAL;
+
+	/* mark for which gpio the trigger changed, protected by buslock */
+	mg->trig_change_mask |= (1 << gpio);
+	mg->trig_type = type;
+
+	return 0;
+}
+
+static int msic_gpio_to_irq(struct gpio_chip *chip, unsigned offset)
+{
+	struct msic_gpio *mg = container_of(chip, struct msic_gpio, chip);
+	return mg->irq_base + offset;
+}
+
+static void msic_bus_lock(struct irq_data *data)
+{
+	struct msic_gpio *mg = irq_data_get_irq_chip_data(data);
+	mutex_lock(&mg->buslock);
+}
+
+static void msic_bus_sync_unlock(struct irq_data *data)
+{
+	struct msic_gpio *mg = irq_data_get_irq_chip_data(data);
+	int offset;
+	int reg;
+	u8 trig = 0;
+
+	/* We can only get one change at a time as the buslock covers the
+	   entire transaction. The irq_desc->lock is dropped before we are
+	   called but that is fine */
+	if (mg->trig_change_mask) {
+		offset = __ffs(mg->trig_change_mask);
+
+		reg = msic_gpio_to_ireg(offset);
+		if (reg < 0)
+			goto out;
+
+		if (mg->trig_type & IRQ_TYPE_EDGE_RISING)
+			trig |= MSIC_GPIO_TRIG_RISE;
+		if (mg->trig_type & IRQ_TYPE_EDGE_FALLING)
+			trig |= MSIC_GPIO_TRIG_FALL;
+
+		intel_msic_reg_update(reg, trig, MSIC_GPIO_INTCNT_MASK);
+		mg->trig_change_mask = 0;
+	}
+out:
+	mutex_unlock(&mg->buslock);
+}
+
+/* Firmware does all the masking and unmasking for us, no masking here. */
+static void msic_irq_unmask(struct irq_data *data) { }
+
+static void msic_irq_mask(struct irq_data *data) { }
+
+static struct irq_chip msic_irqchip = {
+	.name			= "MSIC-GPIO",
+	.irq_mask		= msic_irq_mask,
+	.irq_unmask		= msic_irq_unmask,
+	.irq_set_type		= msic_irq_type,
+	.irq_bus_lock		= msic_bus_lock,
+	.irq_bus_sync_unlock	= msic_bus_sync_unlock,
+};
+
+static void msic_gpio_irq_handler(unsigned irq, struct irq_desc *desc)
+{
+	struct irq_data *data = irq_desc_get_irq_data(desc);
+	struct msic_gpio *mg = irq_data_get_irq_handler_data(data);
+	struct irq_chip *chip = irq_data_get_irq_chip(data);
+	struct intel_msic *msic = pdev_to_intel_msic(mg->pdev);
+	int i;
+	int bitnr;
+	u8 pin;
+	unsigned long pending = 0;
+
+	for (i = 0; i < (mg->chip.ngpio / BITS_PER_BYTE); i++) {
+		intel_msic_irq_read(msic, INTEL_MSIC_GPIO0LVIRQ + i, &pin);
+		pending = pin;
+
+		if (pending) {
+			for_each_set_bit(bitnr, &pending, BITS_PER_BYTE)
+				generic_handle_irq(mg->irq_base +
+						   (i * BITS_PER_BYTE) + bitnr);
+		}
+	}
+	chip->irq_eoi(data);
+}
+
+static int __devinit platform_msic_gpio_probe(struct platform_device *pdev)
+{
+	struct device *dev = &pdev->dev;
+	struct intel_msic_gpio_pdata *pdata = dev->platform_data;
+	struct msic_gpio *mg;
+	int irq = platform_get_irq(pdev, 0);
+	int retval;
+	int i;
+
+	if (irq < 0) {
+		dev_err(dev, "no IRQ line\n");
+		return -EINVAL;
+	}
+
+	if (!pdata || !pdata->gpio_base) {
+		dev_err(dev, "incorrect or missing platform data\n");
+		return -EINVAL;
+	}
+
+	mg = kzalloc(sizeof(*mg), GFP_KERNEL);
+	if (!mg)
+		return -ENOMEM;
+
+	dev_set_drvdata(dev, mg);
+
+	mg->pdev = pdev;
+	mg->irq = irq;
+	mg->irq_base = pdata->gpio_base + MSIC_GPIO_IRQ_OFFSET;
+	mg->chip.label = "msic_gpio";
+	mg->chip.direction_input = msic_gpio_direction_input;
+	mg->chip.direction_output = msic_gpio_direction_output;
+	mg->chip.get = msic_gpio_get;
+	mg->chip.set = msic_gpio_set;
+	mg->chip.to_irq = msic_gpio_to_irq;
+	mg->chip.base = pdata->gpio_base;
+	mg->chip.ngpio = MSIC_NUM_GPIO;
+	mg->chip.can_sleep = 1;
+	mg->chip.dev = dev;
+
+	mutex_init(&mg->buslock);
+
+	retval = gpiochip_add(&mg->chip);
+	if (retval) {
+		dev_err(dev, "Adding MSIC gpio chip failed\n");
+		goto err;
+	}
+
+	for (i = 0; i < mg->chip.ngpio; i++) {
+		irq_set_chip_data(i + mg->irq_base, mg);
+		irq_set_chip_and_handler_name(i + mg->irq_base,
+					      &msic_irqchip,
+					      handle_simple_irq,
+					      "demux");
+	}
+	irq_set_chained_handler(mg->irq, msic_gpio_irq_handler);
+	irq_set_handler_data(mg->irq, mg);
+
+	return 0;
+err:
+	kfree(mg);
+	return retval;
+}
+
+static struct platform_driver platform_msic_gpio_driver = {
+	.driver = {
+		.name		= "msic_gpio",
+		.owner		= THIS_MODULE,
+	},
+	.probe		= platform_msic_gpio_probe,
+};
+
+static int __init platform_msic_gpio_init(void)
+{
+	return platform_driver_register(&platform_msic_gpio_driver);
+}
+
+subsys_initcall(platform_msic_gpio_init);
+
+MODULE_AUTHOR("Mathias Nyman <mathias.nyman@linux.intel.com>");
+MODULE_DESCRIPTION("Intel Medfield MSIC GPIO driver");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/gpio/gpio-mxc.c b/drivers/gpio/gpio-mxc.c
index e7914763457..c337143b18f 100644
--- a/drivers/gpio/gpio-mxc.c
+++ b/drivers/gpio/gpio-mxc.c
@@ -417,7 +417,7 @@ static int __devinit mxc_gpio_probe(struct platform_device *pdev)
 	err = bgpio_init(&port->bgc, &pdev->dev, 4,
 			 port->base + GPIO_PSR,
 			 port->base + GPIO_DR, NULL,
-			 port->base + GPIO_GDIR, NULL, false);
+			 port->base + GPIO_GDIR, NULL, 0);
 	if (err)
 		goto out_iounmap;
 
diff --git a/drivers/gpio/gpio-mxs.c b/drivers/gpio/gpio-mxs.c
index 385c58e8405..39e49566996 100644
--- a/drivers/gpio/gpio-mxs.c
+++ b/drivers/gpio/gpio-mxs.c
@@ -25,23 +25,25 @@
 #include <linux/io.h>
 #include <linux/irq.h>
 #include <linux/gpio.h>
+#include <linux/of.h>
+#include <linux/of_address.h>
+#include <linux/of_device.h>
 #include <linux/platform_device.h>
 #include <linux/slab.h>
 #include <linux/basic_mmio_gpio.h>
 #include <linux/module.h>
-#include <mach/mxs.h>
 
 #define MXS_SET		0x4
 #define MXS_CLR		0x8
 
-#define PINCTRL_DOUT(n)		((cpu_is_mx23() ? 0x0500 : 0x0700) + (n) * 0x10)
-#define PINCTRL_DIN(n)		((cpu_is_mx23() ? 0x0600 : 0x0900) + (n) * 0x10)
-#define PINCTRL_DOE(n)		((cpu_is_mx23() ? 0x0700 : 0x0b00) + (n) * 0x10)
-#define PINCTRL_PIN2IRQ(n)	((cpu_is_mx23() ? 0x0800 : 0x1000) + (n) * 0x10)
-#define PINCTRL_IRQEN(n)	((cpu_is_mx23() ? 0x0900 : 0x1100) + (n) * 0x10)
-#define PINCTRL_IRQLEV(n)	((cpu_is_mx23() ? 0x0a00 : 0x1200) + (n) * 0x10)
-#define PINCTRL_IRQPOL(n)	((cpu_is_mx23() ? 0x0b00 : 0x1300) + (n) * 0x10)
-#define PINCTRL_IRQSTAT(n)	((cpu_is_mx23() ? 0x0c00 : 0x1400) + (n) * 0x10)
+#define PINCTRL_DOUT(p)		((is_imx23_gpio(p) ? 0x0500 : 0x0700) + (p->id) * 0x10)
+#define PINCTRL_DIN(p)		((is_imx23_gpio(p) ? 0x0600 : 0x0900) + (p->id) * 0x10)
+#define PINCTRL_DOE(p)		((is_imx23_gpio(p) ? 0x0700 : 0x0b00) + (p->id) * 0x10)
+#define PINCTRL_PIN2IRQ(p)	((is_imx23_gpio(p) ? 0x0800 : 0x1000) + (p->id) * 0x10)
+#define PINCTRL_IRQEN(p)	((is_imx23_gpio(p) ? 0x0900 : 0x1100) + (p->id) * 0x10)
+#define PINCTRL_IRQLEV(p)	((is_imx23_gpio(p) ? 0x0a00 : 0x1200) + (p->id) * 0x10)
+#define PINCTRL_IRQPOL(p)	((is_imx23_gpio(p) ? 0x0b00 : 0x1300) + (p->id) * 0x10)
+#define PINCTRL_IRQSTAT(p)	((is_imx23_gpio(p) ? 0x0c00 : 0x1400) + (p->id) * 0x10)
 
 #define GPIO_INT_FALL_EDGE	0x0
 #define GPIO_INT_LOW_LEV	0x1
@@ -52,14 +54,30 @@
 
 #define irq_to_gpio(irq)	((irq) - MXS_GPIO_IRQ_START)
 
+enum mxs_gpio_id {
+	IMX23_GPIO,
+	IMX28_GPIO,
+};
+
 struct mxs_gpio_port {
 	void __iomem *base;
 	int id;
 	int irq;
 	int virtual_irq_start;
 	struct bgpio_chip bgc;
+	enum mxs_gpio_id devid;
 };
 
+static inline int is_imx23_gpio(struct mxs_gpio_port *port)
+{
+	return port->devid == IMX23_GPIO;
+}
+
+static inline int is_imx28_gpio(struct mxs_gpio_port *port)
+{
+	return port->devid == IMX28_GPIO;
+}
+
 /* Note: This driver assumes 32 GPIOs are handled in one register */
 
 static int mxs_gpio_set_irq_type(struct irq_data *d, unsigned int type)
@@ -89,21 +107,21 @@ static int mxs_gpio_set_irq_type(struct irq_data *d, unsigned int type)
 	}
 
 	/* set level or edge */
-	pin_addr = port->base + PINCTRL_IRQLEV(port->id);
+	pin_addr = port->base + PINCTRL_IRQLEV(port);
 	if (edge & GPIO_INT_LEV_MASK)
 		writel(pin_mask, pin_addr + MXS_SET);
 	else
 		writel(pin_mask, pin_addr + MXS_CLR);
 
 	/* set polarity */
-	pin_addr = port->base + PINCTRL_IRQPOL(port->id);
+	pin_addr = port->base + PINCTRL_IRQPOL(port);
 	if (edge & GPIO_INT_POL_MASK)
 		writel(pin_mask, pin_addr + MXS_SET);
 	else
 		writel(pin_mask, pin_addr + MXS_CLR);
 
 	writel(1 << (gpio & 0x1f),
-	       port->base + PINCTRL_IRQSTAT(port->id) + MXS_CLR);
+	       port->base + PINCTRL_IRQSTAT(port) + MXS_CLR);
 
 	return 0;
 }
@@ -117,8 +135,8 @@ static void mxs_gpio_irq_handler(u32 irq, struct irq_desc *desc)
 
 	desc->irq_data.chip->irq_ack(&desc->irq_data);
 
-	irq_stat = readl(port->base + PINCTRL_IRQSTAT(port->id)) &
-			readl(port->base + PINCTRL_IRQEN(port->id));
+	irq_stat = readl(port->base + PINCTRL_IRQSTAT(port)) &
+			readl(port->base + PINCTRL_IRQEN(port));
 
 	while (irq_stat != 0) {
 		int irqoffset = fls(irq_stat) - 1;
@@ -164,8 +182,8 @@ static void __init mxs_gpio_init_gc(struct mxs_gpio_port *port)
 	ct->chip.irq_unmask = irq_gc_mask_set_bit;
 	ct->chip.irq_set_type = mxs_gpio_set_irq_type;
 	ct->chip.irq_set_wake = mxs_gpio_set_wake_irq;
-	ct->regs.ack = PINCTRL_IRQSTAT(port->id) + MXS_CLR;
-	ct->regs.mask = PINCTRL_IRQEN(port->id);
+	ct->regs.ack = PINCTRL_IRQSTAT(port) + MXS_CLR;
+	ct->regs.mask = PINCTRL_IRQEN(port);
 
 	irq_setup_generic_chip(gc, IRQ_MSK(32), 0, IRQ_NOREQUEST, 0);
 }
@@ -179,60 +197,83 @@ static int mxs_gpio_to_irq(struct gpio_chip *gc, unsigned offset)
 	return port->virtual_irq_start + offset;
 }
 
+static struct platform_device_id mxs_gpio_ids[] = {
+	{
+		.name = "imx23-gpio",
+		.driver_data = IMX23_GPIO,
+	}, {
+		.name = "imx28-gpio",
+		.driver_data = IMX28_GPIO,
+	}, {
+		/* sentinel */
+	}
+};
+MODULE_DEVICE_TABLE(platform, mxs_gpio_ids);
+
+static const struct of_device_id mxs_gpio_dt_ids[] = {
+	{ .compatible = "fsl,imx23-gpio", .data = (void *) IMX23_GPIO, },
+	{ .compatible = "fsl,imx28-gpio", .data = (void *) IMX28_GPIO, },
+	{ /* sentinel */ }
+};
+MODULE_DEVICE_TABLE(of, mxs_gpio_dt_ids);
+
 static int __devinit mxs_gpio_probe(struct platform_device *pdev)
 {
+	const struct of_device_id *of_id =
+			of_match_device(mxs_gpio_dt_ids, &pdev->dev);
+	struct device_node *np = pdev->dev.of_node;
+	struct device_node *parent;
 	static void __iomem *base;
 	struct mxs_gpio_port *port;
 	struct resource *iores = NULL;
 	int err;
 
-	port = kzalloc(sizeof(struct mxs_gpio_port), GFP_KERNEL);
+	port = devm_kzalloc(&pdev->dev, sizeof(*port), GFP_KERNEL);
 	if (!port)
 		return -ENOMEM;
 
-	port->id = pdev->id;
+	if (np) {
+		port->id = of_alias_get_id(np, "gpio");
+		if (port->id < 0)
+			return port->id;
+		port->devid = (enum mxs_gpio_id) of_id->data;
+	} else {
+		port->id = pdev->id;
+		port->devid = pdev->id_entry->driver_data;
+	}
 	port->virtual_irq_start = MXS_GPIO_IRQ_START + port->id * 32;
 
+	port->irq = platform_get_irq(pdev, 0);
+	if (port->irq < 0)
+		return port->irq;
+
 	/*
 	 * map memory region only once, as all the gpio ports
 	 * share the same one
 	 */
 	if (!base) {
-		iores = platform_get_resource(pdev, IORESOURCE_MEM, 0);
-		if (!iores) {
-			err = -ENODEV;
-			goto out_kfree;
-		}
-
-		if (!request_mem_region(iores->start, resource_size(iores),
-					pdev->name)) {
-			err = -EBUSY;
-			goto out_kfree;
-		}
-
-		base = ioremap(iores->start, resource_size(iores));
-		if (!base) {
-			err = -ENOMEM;
-			goto out_release_mem;
+		if (np) {
+			parent = of_get_parent(np);
+			base = of_iomap(parent, 0);
+			of_node_put(parent);
+		} else {
+			iores = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+			base = devm_request_and_ioremap(&pdev->dev, iores);
 		}
+		if (!base)
+			return -EADDRNOTAVAIL;
 	}
 	port->base = base;
 
-	port->irq = platform_get_irq(pdev, 0);
-	if (port->irq < 0) {
-		err = -EINVAL;
-		goto out_iounmap;
-	}
-
 	/*
 	 * select the pin interrupt functionality but initially
 	 * disable the interrupts
 	 */
-	writel(~0U, port->base + PINCTRL_PIN2IRQ(port->id));
-	writel(0, port->base + PINCTRL_IRQEN(port->id));
+	writel(~0U, port->base + PINCTRL_PIN2IRQ(port));
+	writel(0, port->base + PINCTRL_IRQEN(port));
 
 	/* clear address has to be used to clear IRQSTAT bits */
-	writel(~0U, port->base + PINCTRL_IRQSTAT(port->id) + MXS_CLR);
+	writel(~0U, port->base + PINCTRL_IRQSTAT(port) + MXS_CLR);
 
 	/* gpio-mxs can be a generic irq chip */
 	mxs_gpio_init_gc(port);
@@ -242,41 +283,32 @@ static int __devinit mxs_gpio_probe(struct platform_device *pdev)
 	irq_set_handler_data(port->irq, port);
 
 	err = bgpio_init(&port->bgc, &pdev->dev, 4,
-			 port->base + PINCTRL_DIN(port->id),
-			 port->base + PINCTRL_DOUT(port->id), NULL,
-			 port->base + PINCTRL_DOE(port->id), NULL, false);
+			 port->base + PINCTRL_DIN(port),
+			 port->base + PINCTRL_DOUT(port), NULL,
+			 port->base + PINCTRL_DOE(port), NULL, 0);
 	if (err)
-		goto out_iounmap;
+		return err;
 
 	port->bgc.gc.to_irq = mxs_gpio_to_irq;
 	port->bgc.gc.base = port->id * 32;
 
 	err = gpiochip_add(&port->bgc.gc);
-	if (err)
-		goto out_bgpio_remove;
+	if (err) {
+		bgpio_remove(&port->bgc);
+		return err;
+	}
 
 	return 0;
-
-out_bgpio_remove:
-	bgpio_remove(&port->bgc);
-out_iounmap:
-	if (iores)
-		iounmap(port->base);
-out_release_mem:
-	if (iores)
-		release_mem_region(iores->start, resource_size(iores));
-out_kfree:
-	kfree(port);
-	dev_info(&pdev->dev, "%s failed with errno %d\n", __func__, err);
-	return err;
 }
 
 static struct platform_driver mxs_gpio_driver = {
 	.driver		= {
 		.name	= "gpio-mxs",
 		.owner	= THIS_MODULE,
+		.of_match_table = mxs_gpio_dt_ids,
 	},
 	.probe		= mxs_gpio_probe,
+	.id_table	= mxs_gpio_ids,
 };
 
 static int __init mxs_gpio_init(void)
diff --git a/drivers/gpio/gpio-omap.c b/drivers/gpio/gpio-omap.c
index 1adc2ec1e38..c4ed1722734 100644
--- a/drivers/gpio/gpio-omap.c
+++ b/drivers/gpio/gpio-omap.c
@@ -57,14 +57,10 @@ struct gpio_bank {
 	u16 irq;
 	int irq_base;
 	struct irq_domain *domain;
-	u32 suspend_wakeup;
-	u32 saved_wakeup;
 	u32 non_wakeup_gpios;
 	u32 enabled_non_wakeup_gpios;
 	struct gpio_regs context;
 	u32 saved_datain;
-	u32 saved_fallingdetect;
-	u32 saved_risingdetect;
 	u32 level_mask;
 	u32 toggle_mask;
 	spinlock_t lock;
@@ -516,11 +512,11 @@ static int _set_gpio_wakeup(struct gpio_bank *bank, int gpio, int enable)
 
 	spin_lock_irqsave(&bank->lock, flags);
 	if (enable)
-		bank->suspend_wakeup |= gpio_bit;
+		bank->context.wake_en |= gpio_bit;
 	else
-		bank->suspend_wakeup &= ~gpio_bit;
+		bank->context.wake_en &= ~gpio_bit;
 
-	__raw_writel(bank->suspend_wakeup, bank->base + bank->regs->wkup_en);
+	__raw_writel(bank->context.wake_en, bank->base + bank->regs->wkup_en);
 	spin_unlock_irqrestore(&bank->lock, flags);
 
 	return 0;
@@ -640,7 +636,6 @@ static void gpio_irq_handler(unsigned int irq, struct irq_desc *desc)
 	u32 isr;
 	unsigned int gpio_irq, gpio_index;
 	struct gpio_bank *bank;
-	u32 retrigger = 0;
 	int unmasked = 0;
 	struct irq_chip *chip = irq_desc_get_chip(desc);
 
@@ -677,8 +672,6 @@ static void gpio_irq_handler(unsigned int irq, struct irq_desc *desc)
 			chained_irq_exit(chip, desc);
 		}
 
-		isr |= retrigger;
-		retrigger = 0;
 		if (!isr)
 			break;
 
@@ -789,8 +782,7 @@ static int omap_mpuio_suspend_noirq(struct device *dev)
 	unsigned long		flags;
 
 	spin_lock_irqsave(&bank->lock, flags);
-	bank->saved_wakeup = __raw_readl(mask_reg);
-	__raw_writel(0xffff & ~bank->suspend_wakeup, mask_reg);
+	__raw_writel(0xffff & ~bank->context.wake_en, mask_reg);
 	spin_unlock_irqrestore(&bank->lock, flags);
 
 	return 0;
@@ -805,7 +797,7 @@ static int omap_mpuio_resume_noirq(struct device *dev)
 	unsigned long		flags;
 
 	spin_lock_irqsave(&bank->lock, flags);
-	__raw_writel(bank->saved_wakeup, mask_reg);
+	__raw_writel(bank->context.wake_en, mask_reg);
 	spin_unlock_irqrestore(&bank->lock, flags);
 
 	return 0;
@@ -965,18 +957,15 @@ static void omap_gpio_mod_init(struct gpio_bank *bank)
 	}
 
 	_gpio_rmw(base, bank->regs->irqenable, l, bank->regs->irqenable_inv);
-	_gpio_rmw(base, bank->regs->irqstatus, l,
-					bank->regs->irqenable_inv == false);
-	_gpio_rmw(base, bank->regs->irqenable, l, bank->regs->debounce_en != 0);
-	_gpio_rmw(base, bank->regs->irqenable, l, bank->regs->ctrl != 0);
+	_gpio_rmw(base, bank->regs->irqstatus, l, !bank->regs->irqenable_inv);
 	if (bank->regs->debounce_en)
-		_gpio_rmw(base, bank->regs->debounce_en, 0, 1);
+		__raw_writel(0, base + bank->regs->debounce_en);
 
 	/* Save OE default value (0xffffffff) in the context */
 	bank->context.oe = __raw_readl(bank->base + bank->regs->direction);
 	 /* Initialize interface clk ungated, module enabled */
 	if (bank->regs->ctrl)
-		_gpio_rmw(base, bank->regs->ctrl, 0, 1);
+		__raw_writel(0, base + bank->regs->ctrl);
 }
 
 static __devinit void
@@ -1155,54 +1144,6 @@ static int __devinit omap_gpio_probe(struct platform_device *pdev)
 
 #ifdef CONFIG_ARCH_OMAP2PLUS
 
-#if defined(CONFIG_PM_SLEEP)
-static int omap_gpio_suspend(struct device *dev)
-{
-	struct platform_device *pdev = to_platform_device(dev);
-	struct gpio_bank *bank = platform_get_drvdata(pdev);
-	void __iomem *base = bank->base;
-	void __iomem *wakeup_enable;
-	unsigned long flags;
-
-	if (!bank->mod_usage || !bank->loses_context)
-		return 0;
-
-	if (!bank->regs->wkup_en || !bank->suspend_wakeup)
-		return 0;
-
-	wakeup_enable = bank->base + bank->regs->wkup_en;
-
-	spin_lock_irqsave(&bank->lock, flags);
-	bank->saved_wakeup = __raw_readl(wakeup_enable);
-	_gpio_rmw(base, bank->regs->wkup_en, 0xffffffff, 0);
-	_gpio_rmw(base, bank->regs->wkup_en, bank->suspend_wakeup, 1);
-	spin_unlock_irqrestore(&bank->lock, flags);
-
-	return 0;
-}
-
-static int omap_gpio_resume(struct device *dev)
-{
-	struct platform_device *pdev = to_platform_device(dev);
-	struct gpio_bank *bank = platform_get_drvdata(pdev);
-	void __iomem *base = bank->base;
-	unsigned long flags;
-
-	if (!bank->mod_usage || !bank->loses_context)
-		return 0;
-
-	if (!bank->regs->wkup_en || !bank->saved_wakeup)
-		return 0;
-
-	spin_lock_irqsave(&bank->lock, flags);
-	_gpio_rmw(base, bank->regs->wkup_en, 0xffffffff, 0);
-	_gpio_rmw(base, bank->regs->wkup_en, bank->saved_wakeup, 1);
-	spin_unlock_irqrestore(&bank->lock, flags);
-
-	return 0;
-}
-#endif /* CONFIG_PM_SLEEP */
-
 #if defined(CONFIG_PM_RUNTIME)
 static void omap_gpio_restore_context(struct gpio_bank *bank);
 
@@ -1236,6 +1177,9 @@ static int omap_gpio_runtime_suspend(struct device *dev)
 		__raw_writel(wake_hi | bank->context.risingdetect,
 			     bank->base + bank->regs->risingdetect);
 
+	if (!bank->enabled_non_wakeup_gpios)
+		goto update_gpio_context_count;
+
 	if (bank->power_mode != OFF_MODE) {
 		bank->power_mode = 0;
 		goto update_gpio_context_count;
@@ -1247,11 +1191,9 @@ static int omap_gpio_runtime_suspend(struct device *dev)
 	 */
 	bank->saved_datain = __raw_readl(bank->base +
 						bank->regs->datain);
-	l1 = __raw_readl(bank->base + bank->regs->fallingdetect);
-	l2 = __raw_readl(bank->base + bank->regs->risingdetect);
+	l1 = bank->context.fallingdetect;
+	l2 = bank->context.risingdetect;
 
-	bank->saved_fallingdetect = l1;
-	bank->saved_risingdetect = l2;
 	l1 &= ~bank->enabled_non_wakeup_gpios;
 	l2 &= ~bank->enabled_non_wakeup_gpios;
 
@@ -1293,16 +1235,10 @@ static int omap_gpio_runtime_resume(struct device *dev)
 	__raw_writel(bank->context.risingdetect,
 		     bank->base + bank->regs->risingdetect);
 
-	if (!bank->workaround_enabled) {
-		spin_unlock_irqrestore(&bank->lock, flags);
-		return 0;
-	}
-
 	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 ||
-						!context_lost_cnt_after) {
+		if (context_lost_cnt_after != bank->context_loss_count) {
 			omap_gpio_restore_context(bank);
 		} else {
 			spin_unlock_irqrestore(&bank->lock, flags);
@@ -1310,9 +1246,14 @@ static int omap_gpio_runtime_resume(struct device *dev)
 		}
 	}
 
-	__raw_writel(bank->saved_fallingdetect,
+	if (!bank->workaround_enabled) {
+		spin_unlock_irqrestore(&bank->lock, flags);
+		return 0;
+	}
+
+	__raw_writel(bank->context.fallingdetect,
 			bank->base + bank->regs->fallingdetect);
-	__raw_writel(bank->saved_risingdetect,
+	__raw_writel(bank->context.risingdetect,
 			bank->base + bank->regs->risingdetect);
 	l = __raw_readl(bank->base + bank->regs->datain);
 
@@ -1329,14 +1270,15 @@ static int omap_gpio_runtime_resume(struct device *dev)
 	 * No need to generate IRQs for the rising edge for gpio IRQs
 	 * configured with falling edge only; and vice versa.
 	 */
-	gen0 = l & bank->saved_fallingdetect;
+	gen0 = l & bank->context.fallingdetect;
 	gen0 &= bank->saved_datain;
 
-	gen1 = l & bank->saved_risingdetect;
+	gen1 = l & bank->context.risingdetect;
 	gen1 &= ~(bank->saved_datain);
 
 	/* FIXME: Consider GPIO IRQs with level detections properly! */
-	gen = l & (~(bank->saved_fallingdetect) & ~(bank->saved_risingdetect));
+	gen = l & (~(bank->context.fallingdetect) &
+					 ~(bank->context.risingdetect));
 	/* Consider all GPIO IRQs needed to be updated */
 	gen |= gen0 | gen1;
 
@@ -1346,14 +1288,14 @@ static int omap_gpio_runtime_resume(struct device *dev)
 		old0 = __raw_readl(bank->base + bank->regs->leveldetect0);
 		old1 = __raw_readl(bank->base + bank->regs->leveldetect1);
 
-		if (cpu_is_omap24xx() || cpu_is_omap34xx()) {
+		if (!bank->regs->irqstatus_raw0) {
 			__raw_writel(old0 | gen, bank->base +
 						bank->regs->leveldetect0);
 			__raw_writel(old1 | gen, bank->base +
 						bank->regs->leveldetect1);
 		}
 
-		if (cpu_is_omap44xx()) {
+		if (bank->regs->irqstatus_raw0) {
 			__raw_writel(old0 | l, bank->base +
 						bank->regs->leveldetect0);
 			__raw_writel(old1 | l, bank->base +
@@ -1432,14 +1374,11 @@ static void omap_gpio_restore_context(struct gpio_bank *bank)
 }
 #endif /* CONFIG_PM_RUNTIME */
 #else
-#define omap_gpio_suspend NULL
-#define omap_gpio_resume NULL
 #define omap_gpio_runtime_suspend NULL
 #define omap_gpio_runtime_resume NULL
 #endif
 
 static const struct dev_pm_ops gpio_pm_ops = {
-	SET_SYSTEM_SLEEP_PM_OPS(omap_gpio_suspend, omap_gpio_resume)
 	SET_RUNTIME_PM_OPS(omap_gpio_runtime_suspend, omap_gpio_runtime_resume,
 									NULL)
 };
diff --git a/drivers/gpio/gpio-pca953x.c b/drivers/gpio/gpio-pca953x.c
index d3f3e8f5456..1c313c710be 100644
--- a/drivers/gpio/gpio-pca953x.c
+++ b/drivers/gpio/gpio-pca953x.c
@@ -28,6 +28,8 @@
 #define PCA953X_INVERT		2
 #define PCA953X_DIRECTION	3
 
+#define REG_ADDR_AI		0x80
+
 #define PCA957X_IN		0
 #define PCA957X_INVRT		1
 #define PCA957X_BKEN		2
@@ -63,15 +65,15 @@ static const struct i2c_device_id pca953x_id[] = {
 	{ "pca6107", 8  | PCA953X_TYPE | PCA_INT, },
 	{ "tca6408", 8  | PCA953X_TYPE | PCA_INT, },
 	{ "tca6416", 16 | PCA953X_TYPE | PCA_INT, },
-	/* NYET:  { "tca6424", 24, }, */
+	{ "tca6424", 24 | PCA953X_TYPE | PCA_INT, },
 	{ }
 };
 MODULE_DEVICE_TABLE(i2c, pca953x_id);
 
 struct pca953x_chip {
 	unsigned gpio_start;
-	uint16_t reg_output;
-	uint16_t reg_direction;
+	u32 reg_output;
+	u32 reg_direction;
 	struct mutex i2c_lock;
 
 #ifdef CONFIG_GPIO_PCA953X_IRQ
@@ -89,12 +91,20 @@ struct pca953x_chip {
 	int	chip_type;
 };
 
-static int pca953x_write_reg(struct pca953x_chip *chip, int reg, uint16_t val)
+static int pca953x_write_reg(struct pca953x_chip *chip, int reg, u32 val)
 {
 	int ret = 0;
 
 	if (chip->gpio_chip.ngpio <= 8)
 		ret = i2c_smbus_write_byte_data(chip->client, reg, val);
+	else if (chip->gpio_chip.ngpio == 24) {
+		ret = i2c_smbus_write_word_data(chip->client,
+						(reg << 2) | REG_ADDR_AI,
+						val & 0xffff);
+		ret = i2c_smbus_write_byte_data(chip->client,
+						(reg << 2) + 2,
+						(val & 0xff0000) >> 16);
+	}
 	else {
 		switch (chip->chip_type) {
 		case PCA953X_TYPE:
@@ -121,12 +131,17 @@ static int pca953x_write_reg(struct pca953x_chip *chip, int reg, uint16_t val)
 	return 0;
 }
 
-static int pca953x_read_reg(struct pca953x_chip *chip, int reg, uint16_t *val)
+static int pca953x_read_reg(struct pca953x_chip *chip, int reg, u32 *val)
 {
 	int ret;
 
 	if (chip->gpio_chip.ngpio <= 8)
 		ret = i2c_smbus_read_byte_data(chip->client, reg);
+	else if (chip->gpio_chip.ngpio == 24) {
+		ret =  i2c_smbus_read_word_data(chip->client, reg << 2);
+		ret |= (i2c_smbus_read_byte_data(chip->client,
+						 (reg << 2) + 2)<<16);
+	}
 	else
 		ret = i2c_smbus_read_word_data(chip->client, reg << 1);
 
@@ -135,14 +150,14 @@ static int pca953x_read_reg(struct pca953x_chip *chip, int reg, uint16_t *val)
 		return ret;
 	}
 
-	*val = (uint16_t)ret;
+	*val = (u32)ret;
 	return 0;
 }
 
 static int pca953x_gpio_direction_input(struct gpio_chip *gc, unsigned off)
 {
 	struct pca953x_chip *chip;
-	uint16_t reg_val;
+	uint reg_val;
 	int ret, offset = 0;
 
 	chip = container_of(gc, struct pca953x_chip, gpio_chip);
@@ -173,7 +188,7 @@ static int pca953x_gpio_direction_output(struct gpio_chip *gc,
 		unsigned off, int val)
 {
 	struct pca953x_chip *chip;
-	uint16_t reg_val;
+	uint reg_val;
 	int ret, offset = 0;
 
 	chip = container_of(gc, struct pca953x_chip, gpio_chip);
@@ -223,7 +238,7 @@ exit:
 static int pca953x_gpio_get_value(struct gpio_chip *gc, unsigned off)
 {
 	struct pca953x_chip *chip;
-	uint16_t reg_val;
+	u32 reg_val;
 	int ret, offset = 0;
 
 	chip = container_of(gc, struct pca953x_chip, gpio_chip);
@@ -253,7 +268,7 @@ static int pca953x_gpio_get_value(struct gpio_chip *gc, unsigned off)
 static void pca953x_gpio_set_value(struct gpio_chip *gc, unsigned off, int val)
 {
 	struct pca953x_chip *chip;
-	uint16_t reg_val;
+	u32 reg_val;
 	int ret, offset = 0;
 
 	chip = container_of(gc, struct pca953x_chip, gpio_chip);
@@ -386,7 +401,7 @@ static struct irq_chip pca953x_irq_chip = {
 
 static uint16_t pca953x_irq_pending(struct pca953x_chip *chip)
 {
-	uint16_t cur_stat;
+	u32 cur_stat;
 	uint16_t old_stat;
 	uint16_t pending;
 	uint16_t trigger;
@@ -449,6 +464,7 @@ static int pca953x_irq_setup(struct pca953x_chip *chip,
 {
 	struct i2c_client *client = chip->client;
 	int ret, offset = 0;
+	u32 temporary;
 
 	if (irq_base != -1
 			&& (id->driver_data & PCA_INT)) {
@@ -462,7 +478,8 @@ static int pca953x_irq_setup(struct pca953x_chip *chip,
 			offset = PCA957X_IN;
 			break;
 		}
-		ret = pca953x_read_reg(chip, offset, &chip->irq_stat);
+		ret = pca953x_read_reg(chip, offset, &temporary);
+		chip->irq_stat = temporary;
 		if (ret)
 			goto out_failed;
 
@@ -603,7 +620,7 @@ out:
 static int __devinit device_pca957x_init(struct pca953x_chip *chip, int invert)
 {
 	int ret;
-	uint16_t val = 0;
+	u32 val = 0;
 
 	/* Let every port in proper state, that could save power */
 	pca953x_write_reg(chip, PCA957X_PUPD, 0x0);
diff --git a/drivers/gpio/gpio-pch.c b/drivers/gpio/gpio-pch.c
index e8729cc2ba2..139ad3e2001 100644
--- a/drivers/gpio/gpio-pch.c
+++ b/drivers/gpio/gpio-pch.c
@@ -230,16 +230,12 @@ static void pch_gpio_setup(struct pch_gpio *chip)
 
 static int pch_irq_type(struct irq_data *d, unsigned int type)
 {
-	u32 im;
-	u32 __iomem *im_reg;
-	u32 ien;
-	u32 im_pos;
-	int ch;
-	unsigned long flags;
-	u32 val;
-	int irq = d->irq;
 	struct irq_chip_generic *gc = irq_data_get_irq_chip_data(d);
 	struct pch_gpio *chip = gc->private;
+	u32 im, im_pos, val;
+	u32 __iomem *im_reg;
+	unsigned long flags;
+	int ch, irq = d->irq;
 
 	ch = irq - chip->irq_base;
 	if (irq <= chip->irq_base + 7) {
@@ -270,30 +266,22 @@ static int pch_irq_type(struct irq_data *d, unsigned int type)
 	case IRQ_TYPE_LEVEL_LOW:
 		val = PCH_LEVEL_L;
 		break;
-	case IRQ_TYPE_PROBE:
-		goto end;
 	default:
-		dev_warn(chip->dev, "%s: unknown type(%dd)",
-			__func__, type);
-		goto end;
+		goto unlock;
 	}
 
 	/* Set interrupt mode */
 	im = ioread32(im_reg) & ~(PCH_IM_MASK << (im_pos * 4));
 	iowrite32(im | (val << (im_pos * 4)), im_reg);
 
-	/* iclr */
-	iowrite32(BIT(ch), &chip->reg->iclr);
+	/* And the handler */
+	if (type & (IRQ_TYPE_LEVEL_LOW | IRQ_TYPE_LEVEL_HIGH))
+		__irq_set_handler_locked(d->irq, handle_level_irq);
+	else if (type & (IRQ_TYPE_EDGE_FALLING | IRQ_TYPE_EDGE_RISING))
+		__irq_set_handler_locked(d->irq, handle_edge_irq);
 
-	/* IMASKCLR */
-	iowrite32(BIT(ch), &chip->reg->imaskclr);
-
-	/* Enable interrupt */
-	ien = ioread32(&chip->reg->ien);
-	iowrite32(ien | BIT(ch), &chip->reg->ien);
-end:
+unlock:
 	spin_unlock_irqrestore(&chip->spinlock, flags);
-
 	return 0;
 }
 
@@ -313,18 +301,24 @@ static void pch_irq_mask(struct irq_data *d)
 	iowrite32(1 << (d->irq - chip->irq_base), &chip->reg->imask);
 }
 
+static void pch_irq_ack(struct irq_data *d)
+{
+	struct irq_chip_generic *gc = irq_data_get_irq_chip_data(d);
+	struct pch_gpio *chip = gc->private;
+
+	iowrite32(1 << (d->irq - chip->irq_base), &chip->reg->iclr);
+}
+
 static irqreturn_t pch_gpio_handler(int irq, void *dev_id)
 {
 	struct pch_gpio *chip = dev_id;
 	u32 reg_val = ioread32(&chip->reg->istatus);
-	int i;
-	int ret = IRQ_NONE;
+	int i, ret = IRQ_NONE;
 
 	for (i = 0; i < gpio_pins[chip->ioh]; i++) {
 		if (reg_val & BIT(i)) {
 			dev_dbg(chip->dev, "%s:[%d]:irq=%d  status=0x%x\n",
 				__func__, i, irq, reg_val);
-			iowrite32(BIT(i), &chip->reg->iclr);
 			generic_handle_irq(chip->irq_base + i);
 			ret = IRQ_HANDLED;
 		}
@@ -343,6 +337,7 @@ static __devinit void pch_gpio_alloc_generic_chip(struct pch_gpio *chip,
 	gc->private = chip;
 	ct = gc->chip_types;
 
+	ct->chip.irq_ack = pch_irq_ack;
 	ct->chip.irq_mask = pch_irq_mask;
 	ct->chip.irq_unmask = pch_irq_unmask;
 	ct->chip.irq_set_type = pch_irq_type;
@@ -357,6 +352,7 @@ static int __devinit pch_gpio_probe(struct pci_dev *pdev,
 	s32 ret;
 	struct pch_gpio *chip;
 	int irq_base;
+	u32 msk;
 
 	chip = kzalloc(sizeof(*chip), GFP_KERNEL);
 	if (chip == NULL)
@@ -408,8 +404,13 @@ static int __devinit pch_gpio_probe(struct pci_dev *pdev,
 	}
 	chip->irq_base = irq_base;
 
+	/* Mask all interrupts, but enable them */
+	msk = (1 << gpio_pins[chip->ioh]) - 1;
+	iowrite32(msk, &chip->reg->imask);
+	iowrite32(msk, &chip->reg->ien);
+
 	ret = request_irq(pdev->irq, pch_gpio_handler,
-			     IRQF_SHARED, KBUILD_MODNAME, chip);
+			  IRQF_SHARED, KBUILD_MODNAME, chip);
 	if (ret != 0) {
 		dev_err(&pdev->dev,
 			"%s request_irq failed\n", __func__);
@@ -418,8 +419,6 @@ static int __devinit pch_gpio_probe(struct pci_dev *pdev,
 
 	pch_gpio_alloc_generic_chip(chip, irq_base, gpio_pins[chip->ioh]);
 
-	/* Initialize interrupt ien register */
-	iowrite32(0, &chip->reg->ien);
 end:
 	return 0;
 
@@ -539,17 +538,7 @@ static struct pci_driver pch_gpio_driver = {
 	.resume = pch_gpio_resume
 };
 
-static int __init pch_gpio_pci_init(void)
-{
-	return pci_register_driver(&pch_gpio_driver);
-}
-module_init(pch_gpio_pci_init);
-
-static void __exit pch_gpio_pci_exit(void)
-{
-	pci_unregister_driver(&pch_gpio_driver);
-}
-module_exit(pch_gpio_pci_exit);
+module_pci_driver(pch_gpio_driver);
 
 MODULE_DESCRIPTION("PCH GPIO PCI Driver");
 MODULE_LICENSE("GPL");
diff --git a/drivers/gpio/gpio-pxa.c b/drivers/gpio/gpio-pxa.c
index fc3ace3fd4c..58a6a63a6ec 100644
--- a/drivers/gpio/gpio-pxa.c
+++ b/drivers/gpio/gpio-pxa.c
@@ -11,13 +11,17 @@
  *  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/clk.h>
 #include <linux/err.h>
 #include <linux/gpio.h>
 #include <linux/gpio-pxa.h>
 #include <linux/init.h>
 #include <linux/irq.h>
+#include <linux/irqdomain.h>
 #include <linux/io.h>
+#include <linux/of.h>
+#include <linux/of_device.h>
 #include <linux/platform_device.h>
 #include <linux/syscore_ops.h>
 #include <linux/slab.h>
@@ -56,6 +60,10 @@
 
 int pxa_last_gpio;
 
+#ifdef CONFIG_OF
+static struct irq_domain *domain;
+#endif
+
 struct pxa_gpio_chip {
 	struct gpio_chip chip;
 	void __iomem	*regbase;
@@ -81,7 +89,6 @@ enum {
 	PXA3XX_GPIO,
 	PXA93X_GPIO,
 	MMP_GPIO = 0x10,
-	MMP2_GPIO,
 };
 
 static DEFINE_SPINLOCK(gpio_lock);
@@ -475,22 +482,92 @@ static int pxa_gpio_nums(void)
 		gpio_type = MMP_GPIO;
 	} else if (cpu_is_mmp2()) {
 		count = 191;
-		gpio_type = MMP2_GPIO;
+		gpio_type = MMP_GPIO;
 	}
 #endif /* CONFIG_ARCH_MMP */
 	return count;
 }
 
+static struct of_device_id pxa_gpio_dt_ids[] = {
+	{ .compatible = "mrvl,pxa-gpio" },
+	{ .compatible = "mrvl,mmp-gpio", .data = (void *)MMP_GPIO },
+	{}
+};
+
+static int pxa_irq_domain_map(struct irq_domain *d, unsigned int irq,
+			      irq_hw_number_t hw)
+{
+	irq_set_chip_and_handler(irq, &pxa_muxed_gpio_chip,
+				 handle_edge_irq);
+	set_irq_flags(irq, IRQF_VALID | IRQF_PROBE);
+	return 0;
+}
+
+const struct irq_domain_ops pxa_irq_domain_ops = {
+	.map	= pxa_irq_domain_map,
+};
+
+#ifdef CONFIG_OF
+static int __devinit pxa_gpio_probe_dt(struct platform_device *pdev)
+{
+	int ret, nr_banks, nr_gpios, irq_base;
+	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);
+
+	if (!of_id) {
+		dev_err(&pdev->dev, "Failed to find gpio controller\n");
+		return -EFAULT;
+	}
+	gpio_type = (int)of_id->data;
+
+	next = of_get_next_child(np, NULL);
+	prev = next;
+	if (!next) {
+		dev_err(&pdev->dev, "Failed to find child gpio node\n");
+		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;
+	pxa_last_gpio = nr_gpios - 1;
+
+	irq_base = irq_alloc_descs(-1, 0, nr_gpios, 0);
+	if (irq_base < 0) {
+		dev_err(&pdev->dev, "Failed to allocate IRQ numbers\n");
+		goto err;
+	}
+	domain = irq_domain_add_legacy(np, nr_gpios, irq_base, 0,
+				       &pxa_irq_domain_ops, NULL);
+	return 0;
+err:
+	iounmap(gpio_reg_base);
+	return ret;
+}
+#else
+#define pxa_gpio_probe_dt(pdev)		(-1)
+#endif
+
 static int __devinit pxa_gpio_probe(struct platform_device *pdev)
 {
 	struct pxa_gpio_chip *c;
 	struct resource *res;
 	struct clk *clk;
 	struct pxa_gpio_platform_data *info;
-	int gpio, irq, ret;
+	int gpio, irq, ret, use_of = 0;
 	int irq0 = 0, irq1 = 0, irq_mux, gpio_offset = 0;
 
-	pxa_last_gpio = pxa_gpio_nums();
+	ret = pxa_gpio_probe_dt(pdev);
+	if (ret < 0)
+		pxa_last_gpio = pxa_gpio_nums();
+	else
+		use_of = 1;
 	if (!pxa_last_gpio)
 		return -EINVAL;
 
@@ -545,25 +622,27 @@ static int __devinit pxa_gpio_probe(struct platform_device *pdev)
 			writel_relaxed(~0, c->regbase + ED_MASK_OFFSET);
 	}
 
+	if (!use_of) {
 #ifdef CONFIG_ARCH_PXA
-	irq = gpio_to_irq(0);
-	irq_set_chip_and_handler(irq, &pxa_muxed_gpio_chip,
-				 handle_edge_irq);
-	set_irq_flags(irq, IRQF_VALID | IRQF_PROBE);
-	irq_set_chained_handler(IRQ_GPIO0, pxa_gpio_demux_handler);
-
-	irq = gpio_to_irq(1);
-	irq_set_chip_and_handler(irq, &pxa_muxed_gpio_chip,
-				 handle_edge_irq);
-	set_irq_flags(irq, IRQF_VALID | IRQF_PROBE);
-	irq_set_chained_handler(IRQ_GPIO1, pxa_gpio_demux_handler);
-#endif
+		irq = gpio_to_irq(0);
+		irq_set_chip_and_handler(irq, &pxa_muxed_gpio_chip,
+					 handle_edge_irq);
+		set_irq_flags(irq, IRQF_VALID | IRQF_PROBE);
+		irq_set_chained_handler(IRQ_GPIO0, pxa_gpio_demux_handler);
 
-	for (irq  = gpio_to_irq(gpio_offset);
-		irq <= gpio_to_irq(pxa_last_gpio); irq++) {
+		irq = gpio_to_irq(1);
 		irq_set_chip_and_handler(irq, &pxa_muxed_gpio_chip,
 					 handle_edge_irq);
 		set_irq_flags(irq, IRQF_VALID | IRQF_PROBE);
+		irq_set_chained_handler(IRQ_GPIO1, pxa_gpio_demux_handler);
+#endif
+
+		for (irq  = gpio_to_irq(gpio_offset);
+			irq <= gpio_to_irq(pxa_last_gpio); irq++) {
+			irq_set_chip_and_handler(irq, &pxa_muxed_gpio_chip,
+						 handle_edge_irq);
+			set_irq_flags(irq, IRQF_VALID | IRQF_PROBE);
+		}
 	}
 
 	irq_set_chained_handler(irq_mux, pxa_gpio_demux_handler);
@@ -574,6 +653,7 @@ static struct platform_driver pxa_gpio_driver = {
 	.probe		= pxa_gpio_probe,
 	.driver		= {
 		.name	= "pxa-gpio",
+		.of_match_table = pxa_gpio_dt_ids,
 	},
 };
 
diff --git a/drivers/gpio/gpio-rc5t583.c b/drivers/gpio/gpio-rc5t583.c
new file mode 100644
index 00000000000..08428bf1771
--- /dev/null
+++ b/drivers/gpio/gpio-rc5t583.c
@@ -0,0 +1,180 @@
+/*
+ * GPIO driver for RICOH583 power management chip.
+ *
+ * Copyright (c) 2012, NVIDIA CORPORATION.  All rights reserved.
+ * Author: Laxman dewangan <ldewangan@nvidia.com>
+ *
+ * Based on code
+ *	Copyright (C) 2011 RICOH COMPANY,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, see <http://www.gnu.org/licenses/>.
+ *
+ */
+#include <linux/init.h>
+#include <linux/kernel.h>
+#include <linux/slab.h>
+#include <linux/module.h>
+#include <linux/platform_device.h>
+#include <linux/device.h>
+#include <linux/gpio.h>
+#include <linux/mfd/rc5t583.h>
+
+struct rc5t583_gpio {
+	struct gpio_chip gpio_chip;
+	struct rc5t583 *rc5t583;
+};
+
+static inline struct rc5t583_gpio *to_rc5t583_gpio(struct gpio_chip *chip)
+{
+	return container_of(chip, struct rc5t583_gpio, gpio_chip);
+}
+
+static int rc5t583_gpio_get(struct gpio_chip *gc, unsigned int offset)
+{
+	struct rc5t583_gpio *rc5t583_gpio = to_rc5t583_gpio(gc);
+	struct device *parent = rc5t583_gpio->rc5t583->dev;
+	uint8_t val = 0;
+	int ret;
+
+	ret = rc5t583_read(parent, RC5T583_GPIO_MON_IOIN, &val);
+	if (ret < 0)
+		return ret;
+
+	return !!(val & BIT(offset));
+}
+
+static void rc5t583_gpio_set(struct gpio_chip *gc, unsigned int offset, int val)
+{
+	struct rc5t583_gpio *rc5t583_gpio = to_rc5t583_gpio(gc);
+	struct device *parent = rc5t583_gpio->rc5t583->dev;
+	if (val)
+		rc5t583_set_bits(parent, RC5T583_GPIO_IOOUT, BIT(offset));
+	else
+		rc5t583_clear_bits(parent, RC5T583_GPIO_IOOUT, BIT(offset));
+}
+
+static int rc5t583_gpio_dir_input(struct gpio_chip *gc, unsigned int offset)
+{
+	struct rc5t583_gpio *rc5t583_gpio = to_rc5t583_gpio(gc);
+	struct device *parent = rc5t583_gpio->rc5t583->dev;
+	int ret;
+
+	ret = rc5t583_clear_bits(parent, RC5T583_GPIO_IOSEL, BIT(offset));
+	if (ret < 0)
+		return ret;
+
+	/* Set pin to gpio mode */
+	return rc5t583_clear_bits(parent, RC5T583_GPIO_PGSEL, BIT(offset));
+}
+
+static int rc5t583_gpio_dir_output(struct gpio_chip *gc, unsigned offset,
+			int value)
+{
+	struct rc5t583_gpio *rc5t583_gpio = to_rc5t583_gpio(gc);
+	struct device *parent = rc5t583_gpio->rc5t583->dev;
+	int ret;
+
+	rc5t583_gpio_set(gc, offset, value);
+	ret = rc5t583_set_bits(parent, RC5T583_GPIO_IOSEL, BIT(offset));
+	if (ret < 0)
+		return ret;
+
+	/* Set pin to gpio mode */
+	return rc5t583_clear_bits(parent, RC5T583_GPIO_PGSEL, BIT(offset));
+}
+
+static int rc5t583_gpio_to_irq(struct gpio_chip *gc, unsigned offset)
+{
+	struct rc5t583_gpio *rc5t583_gpio = to_rc5t583_gpio(gc);
+
+	if ((offset >= 0) && (offset < 8))
+		return rc5t583_gpio->rc5t583->irq_base +
+				RC5T583_IRQ_GPIO0 + offset;
+	return -EINVAL;
+}
+
+static void rc5t583_gpio_free(struct gpio_chip *gc, unsigned offset)
+{
+	struct rc5t583_gpio *rc5t583_gpio = to_rc5t583_gpio(gc);
+	struct device *parent = rc5t583_gpio->rc5t583->dev;
+
+	rc5t583_set_bits(parent, RC5T583_GPIO_PGSEL, BIT(offset));
+}
+
+static int __devinit rc5t583_gpio_probe(struct platform_device *pdev)
+{
+	struct rc5t583 *rc5t583 = dev_get_drvdata(pdev->dev.parent);
+	struct rc5t583_platform_data *pdata = dev_get_platdata(rc5t583->dev);
+	struct rc5t583_gpio *rc5t583_gpio;
+
+	rc5t583_gpio = devm_kzalloc(&pdev->dev, sizeof(*rc5t583_gpio),
+					GFP_KERNEL);
+	if (!rc5t583_gpio) {
+		dev_warn(&pdev->dev, "Mem allocation for rc5t583_gpio failed");
+		return -ENOMEM;
+	}
+
+	rc5t583_gpio->gpio_chip.label = "gpio-rc5t583",
+	rc5t583_gpio->gpio_chip.owner = THIS_MODULE,
+	rc5t583_gpio->gpio_chip.free = rc5t583_gpio_free,
+	rc5t583_gpio->gpio_chip.direction_input = rc5t583_gpio_dir_input,
+	rc5t583_gpio->gpio_chip.direction_output = rc5t583_gpio_dir_output,
+	rc5t583_gpio->gpio_chip.set = rc5t583_gpio_set,
+	rc5t583_gpio->gpio_chip.get = rc5t583_gpio_get,
+	rc5t583_gpio->gpio_chip.to_irq = rc5t583_gpio_to_irq,
+	rc5t583_gpio->gpio_chip.ngpio = RC5T583_MAX_GPIO,
+	rc5t583_gpio->gpio_chip.can_sleep = 1,
+	rc5t583_gpio->gpio_chip.dev = &pdev->dev;
+	rc5t583_gpio->gpio_chip.base = -1;
+	rc5t583_gpio->rc5t583 = rc5t583;
+
+	if (pdata && pdata->gpio_base)
+		rc5t583_gpio->gpio_chip.base = pdata->gpio_base;
+
+	platform_set_drvdata(pdev, rc5t583_gpio);
+
+	return gpiochip_add(&rc5t583_gpio->gpio_chip);
+}
+
+static int __devexit rc5t583_gpio_remove(struct platform_device *pdev)
+{
+	struct rc5t583_gpio *rc5t583_gpio = platform_get_drvdata(pdev);
+
+	return gpiochip_remove(&rc5t583_gpio->gpio_chip);
+}
+
+static struct platform_driver rc5t583_gpio_driver = {
+	.driver = {
+		.name    = "rc5t583-gpio",
+		.owner   = THIS_MODULE,
+	},
+	.probe		= rc5t583_gpio_probe,
+	.remove		= __devexit_p(rc5t583_gpio_remove),
+};
+
+static int __init rc5t583_gpio_init(void)
+{
+	return platform_driver_register(&rc5t583_gpio_driver);
+}
+subsys_initcall(rc5t583_gpio_init);
+
+static void __exit rc5t583_gpio_exit(void)
+{
+	platform_driver_unregister(&rc5t583_gpio_driver);
+}
+module_exit(rc5t583_gpio_exit);
+
+MODULE_AUTHOR("Laxman Dewangan <ldewangan@nvidia.com>");
+MODULE_DESCRIPTION("GPIO interface for RC5T583");
+MODULE_LICENSE("GPL v2");
+MODULE_ALIAS("platform:rc5t583-gpio");
diff --git a/drivers/gpio/gpio-samsung.c b/drivers/gpio/gpio-samsung.c
index 19d6fc0229c..7bb00448e13 100644
--- a/drivers/gpio/gpio-samsung.c
+++ b/drivers/gpio/gpio-samsung.c
@@ -452,12 +452,14 @@ static struct samsung_gpio_cfg s3c24xx_gpiocfg_banka = {
 };
 #endif
 
+#if defined(CONFIG_ARCH_EXYNOS4) || defined(CONFIG_ARCH_EXYNOS5)
 static struct samsung_gpio_cfg exynos_gpio_cfg = {
 	.set_pull	= exynos_gpio_setpull,
 	.get_pull	= exynos_gpio_getpull,
 	.set_config	= samsung_gpio_setcfg_4bit,
 	.get_config	= samsung_gpio_getcfg_4bit,
 };
+#endif
 
 #if defined(CONFIG_CPU_S5P6440) || defined(CONFIG_CPU_S5P6450)
 static struct samsung_gpio_cfg s5p64x0_gpio_cfg_rbank = {
@@ -2123,8 +2125,8 @@ static struct samsung_gpio_chip s5pv210_gpios_4bit[] = {
  * uses the above macro and depends on the banks being listed in order here.
  */
 
-static struct samsung_gpio_chip exynos4_gpios_1[] = {
 #ifdef CONFIG_ARCH_EXYNOS4
+static struct samsung_gpio_chip exynos4_gpios_1[] = {
 	{
 		.chip	= {
 			.base	= EXYNOS4_GPA0(0),
@@ -2222,11 +2224,11 @@ static struct samsung_gpio_chip exynos4_gpios_1[] = {
 			.label	= "GPF3",
 		},
 	},
-#endif
 };
+#endif
 
-static struct samsung_gpio_chip exynos4_gpios_2[] = {
 #ifdef CONFIG_ARCH_EXYNOS4
+static struct samsung_gpio_chip exynos4_gpios_2[] = {
 	{
 		.chip	= {
 			.base	= EXYNOS4_GPJ0(0),
@@ -2367,11 +2369,11 @@ static struct samsung_gpio_chip exynos4_gpios_2[] = {
 			.to_irq	= samsung_gpiolib_to_irq,
 		},
 	},
-#endif
 };
+#endif
 
-static struct samsung_gpio_chip exynos4_gpios_3[] = {
 #ifdef CONFIG_ARCH_EXYNOS4
+static struct samsung_gpio_chip exynos4_gpios_3[] = {
 	{
 		.chip	= {
 			.base	= EXYNOS4_GPZ(0),
@@ -2379,8 +2381,8 @@ static struct samsung_gpio_chip exynos4_gpios_3[] = {
 			.label	= "GPZ",
 		},
 	},
-#endif
 };
+#endif
 
 #ifdef CONFIG_ARCH_EXYNOS5
 static struct samsung_gpio_chip exynos5_gpios_1[] = {
@@ -2452,6 +2454,12 @@ static struct samsung_gpio_chip exynos5_gpios_1[] = {
 		},
 	}, {
 		.chip	= {
+			.base	= EXYNOS5_GPC4(0),
+			.ngpio	= EXYNOS5_GPIO_C4_NR,
+			.label	= "GPC4",
+		},
+	}, {
+		.chip	= {
 			.base	= EXYNOS5_GPD0(0),
 			.ngpio	= EXYNOS5_GPIO_D0_NR,
 			.label	= "GPD0",
@@ -2714,12 +2722,227 @@ static __init void exynos_gpiolib_attach_ofnode(struct samsung_gpio_chip *chip,
 }
 #endif /* defined(CONFIG_ARCH_EXYNOS) && defined(CONFIG_OF) */
 
+static __init void exynos4_gpiolib_init(void)
+{
+#ifdef CONFIG_CPU_EXYNOS4210
+	struct samsung_gpio_chip *chip;
+	int i, nr_chips;
+	void __iomem *gpio_base1, *gpio_base2, *gpio_base3;
+	int group = 0;
+	void __iomem *gpx_base;
+
+	/* gpio part1 */
+	gpio_base1 = ioremap(EXYNOS4_PA_GPIO1, SZ_4K);
+	if (gpio_base1 == NULL) {
+		pr_err("unable to ioremap for gpio_base1\n");
+		goto err_ioremap1;
+	}
+
+	chip = exynos4_gpios_1;
+	nr_chips = ARRAY_SIZE(exynos4_gpios_1);
+
+	for (i = 0; i < nr_chips; i++, chip++) {
+		if (!chip->config) {
+			chip->config = &exynos_gpio_cfg;
+			chip->group = group++;
+		}
+		exynos_gpiolib_attach_ofnode(chip,
+				EXYNOS4_PA_GPIO1, i * 0x20);
+	}
+	samsung_gpiolib_add_4bit_chips(exynos4_gpios_1,
+				       nr_chips, gpio_base1);
+
+	/* gpio part2 */
+	gpio_base2 = ioremap(EXYNOS4_PA_GPIO2, SZ_4K);
+	if (gpio_base2 == NULL) {
+		pr_err("unable to ioremap for gpio_base2\n");
+		goto err_ioremap2;
+	}
+
+	/* need to set base address for gpx */
+	chip = &exynos4_gpios_2[16];
+	gpx_base = gpio_base2 + 0xC00;
+	for (i = 0; i < 4; i++, chip++, gpx_base += 0x20)
+		chip->base = gpx_base;
+
+	chip = exynos4_gpios_2;
+	nr_chips = ARRAY_SIZE(exynos4_gpios_2);
+
+	for (i = 0; i < nr_chips; i++, chip++) {
+		if (!chip->config) {
+			chip->config = &exynos_gpio_cfg;
+			chip->group = group++;
+		}
+		exynos_gpiolib_attach_ofnode(chip,
+				EXYNOS4_PA_GPIO2, i * 0x20);
+	}
+	samsung_gpiolib_add_4bit_chips(exynos4_gpios_2,
+				       nr_chips, gpio_base2);
+
+	/* gpio part3 */
+	gpio_base3 = ioremap(EXYNOS4_PA_GPIO3, SZ_256);
+	if (gpio_base3 == NULL) {
+		pr_err("unable to ioremap for gpio_base3\n");
+		goto err_ioremap3;
+	}
+
+	chip = exynos4_gpios_3;
+	nr_chips = ARRAY_SIZE(exynos4_gpios_3);
+
+	for (i = 0; i < nr_chips; i++, chip++) {
+		if (!chip->config) {
+			chip->config = &exynos_gpio_cfg;
+			chip->group = group++;
+		}
+		exynos_gpiolib_attach_ofnode(chip,
+				EXYNOS4_PA_GPIO3, i * 0x20);
+	}
+	samsung_gpiolib_add_4bit_chips(exynos4_gpios_3,
+				       nr_chips, gpio_base3);
+
+#if defined(CONFIG_CPU_EXYNOS4210) && defined(CONFIG_S5P_GPIO_INT)
+	s5p_register_gpioint_bank(IRQ_GPIO_XA, 0, IRQ_GPIO1_NR_GROUPS);
+	s5p_register_gpioint_bank(IRQ_GPIO_XB, IRQ_GPIO1_NR_GROUPS, IRQ_GPIO2_NR_GROUPS);
+#endif
+
+	return;
+
+err_ioremap3:
+	iounmap(gpio_base2);
+err_ioremap2:
+	iounmap(gpio_base1);
+err_ioremap1:
+	return;
+#endif	/* CONFIG_CPU_EXYNOS4210 */
+}
+
+static __init void exynos5_gpiolib_init(void)
+{
+#ifdef CONFIG_SOC_EXYNOS5250
+	struct samsung_gpio_chip *chip;
+	int i, nr_chips;
+	void __iomem *gpio_base1, *gpio_base2, *gpio_base3, *gpio_base4;
+	int group = 0;
+	void __iomem *gpx_base;
+
+	/* gpio part1 */
+	gpio_base1 = ioremap(EXYNOS5_PA_GPIO1, SZ_4K);
+	if (gpio_base1 == NULL) {
+		pr_err("unable to ioremap for gpio_base1\n");
+		goto err_ioremap1;
+	}
+
+	/* need to set base address for gpc4 */
+	exonys5_gpios_1[11].base = gpio_base1 + 0x2E0;
+
+	/* need to set base address for gpx */
+	chip = &exynos5_gpios_1[21];
+	gpx_base = gpio_base1 + 0xC00;
+	for (i = 0; i < 4; i++, chip++, gpx_base += 0x20)
+		chip->base = gpx_base;
+
+	chip = exynos5_gpios_1;
+	nr_chips = ARRAY_SIZE(exynos5_gpios_1);
+
+	for (i = 0; i < nr_chips; i++, chip++) {
+		if (!chip->config) {
+			chip->config = &exynos_gpio_cfg;
+			chip->group = group++;
+		}
+		exynos_gpiolib_attach_ofnode(chip,
+				EXYNOS5_PA_GPIO1, i * 0x20);
+	}
+	samsung_gpiolib_add_4bit_chips(exynos5_gpios_1,
+				       nr_chips, gpio_base1);
+
+	/* gpio part2 */
+	gpio_base2 = ioremap(EXYNOS5_PA_GPIO2, SZ_4K);
+	if (gpio_base2 == NULL) {
+		pr_err("unable to ioremap for gpio_base2\n");
+		goto err_ioremap2;
+	}
+
+	chip = exynos5_gpios_2;
+	nr_chips = ARRAY_SIZE(exynos5_gpios_2);
+
+	for (i = 0; i < nr_chips; i++, chip++) {
+		if (!chip->config) {
+			chip->config = &exynos_gpio_cfg;
+			chip->group = group++;
+		}
+		exynos_gpiolib_attach_ofnode(chip,
+				EXYNOS5_PA_GPIO2, i * 0x20);
+	}
+	samsung_gpiolib_add_4bit_chips(exynos5_gpios_2,
+				       nr_chips, gpio_base2);
+
+	/* gpio part3 */
+	gpio_base3 = ioremap(EXYNOS5_PA_GPIO3, SZ_4K);
+	if (gpio_base3 == NULL) {
+		pr_err("unable to ioremap for gpio_base3\n");
+		goto err_ioremap3;
+	}
+
+	/* need to set base address for gpv */
+	exynos5_gpios_3[0].base = gpio_base3;
+	exynos5_gpios_3[1].base = gpio_base3 + 0x20;
+	exynos5_gpios_3[2].base = gpio_base3 + 0x60;
+	exynos5_gpios_3[3].base = gpio_base3 + 0x80;
+	exynos5_gpios_3[4].base = gpio_base3 + 0xC0;
+
+	chip = exynos5_gpios_3;
+	nr_chips = ARRAY_SIZE(exynos5_gpios_3);
+
+	for (i = 0; i < nr_chips; i++, chip++) {
+		if (!chip->config) {
+			chip->config = &exynos_gpio_cfg;
+			chip->group = group++;
+		}
+		exynos_gpiolib_attach_ofnode(chip,
+				EXYNOS5_PA_GPIO3, i * 0x20);
+	}
+	samsung_gpiolib_add_4bit_chips(exynos5_gpios_3,
+				       nr_chips, gpio_base3);
+
+	/* gpio part4 */
+	gpio_base4 = ioremap(EXYNOS5_PA_GPIO4, SZ_4K);
+	if (gpio_base4 == NULL) {
+		pr_err("unable to ioremap for gpio_base4\n");
+		goto err_ioremap4;
+	}
+
+	chip = exynos5_gpios_4;
+	nr_chips = ARRAY_SIZE(exynos5_gpios_4);
+
+	for (i = 0; i < nr_chips; i++, chip++) {
+		if (!chip->config) {
+			chip->config = &exynos_gpio_cfg;
+			chip->group = group++;
+		}
+		exynos_gpiolib_attach_ofnode(chip,
+				EXYNOS5_PA_GPIO4, i * 0x20);
+	}
+	samsung_gpiolib_add_4bit_chips(exynos5_gpios_4,
+				       nr_chips, gpio_base4);
+	return;
+
+err_ioremap4:
+	iounmap(gpio_base3);
+err_ioremap3:
+	iounmap(gpio_base2);
+err_ioremap2:
+	iounmap(gpio_base1);
+err_ioremap1:
+	return;
+
+#endif	/* CONFIG_SOC_EXYNOS5250 */
+}
+
 /* TODO: cleanup soc_is_* */
 static __init int samsung_gpiolib_init(void)
 {
 	struct samsung_gpio_chip *chip;
 	int i, nr_chips;
-	void __iomem *gpio_base1, *gpio_base2, *gpio_base3, *gpio_base4;
 	int group = 0;
 
 	samsung_gpiolib_set_cfg(samsung_gpio_cfgs, ARRAY_SIZE(samsung_gpio_cfgs));
@@ -2785,200 +3008,15 @@ static __init int samsung_gpiolib_init(void)
 		s5p_register_gpioint_bank(IRQ_GPIOINT, 0, S5P_GPIOINT_GROUP_MAXNR);
 #endif
 	} else if (soc_is_exynos4210()) {
-#ifdef CONFIG_CPU_EXYNOS4210
-		void __iomem *gpx_base;
-
-		/* gpio part1 */
-		gpio_base1 = ioremap(EXYNOS4_PA_GPIO1, SZ_4K);
-		if (gpio_base1 == NULL) {
-			pr_err("unable to ioremap for gpio_base1\n");
-			goto err_ioremap1;
-		}
-
-		chip = exynos4_gpios_1;
-		nr_chips = ARRAY_SIZE(exynos4_gpios_1);
-
-		for (i = 0; i < nr_chips; i++, chip++) {
-			if (!chip->config) {
-				chip->config = &exynos_gpio_cfg;
-				chip->group = group++;
-			}
-			exynos_gpiolib_attach_ofnode(chip,
-					EXYNOS4_PA_GPIO1, i * 0x20);
-		}
-		samsung_gpiolib_add_4bit_chips(exynos4_gpios_1,
-					       nr_chips, gpio_base1);
-
-		/* gpio part2 */
-		gpio_base2 = ioremap(EXYNOS4_PA_GPIO2, SZ_4K);
-		if (gpio_base2 == NULL) {
-			pr_err("unable to ioremap for gpio_base2\n");
-			goto err_ioremap2;
-		}
-
-		/* need to set base address for gpx */
-		chip = &exynos4_gpios_2[16];
-		gpx_base = gpio_base2 + 0xC00;
-		for (i = 0; i < 4; i++, chip++, gpx_base += 0x20)
-			chip->base = gpx_base;
-
-		chip = exynos4_gpios_2;
-		nr_chips = ARRAY_SIZE(exynos4_gpios_2);
-
-		for (i = 0; i < nr_chips; i++, chip++) {
-			if (!chip->config) {
-				chip->config = &exynos_gpio_cfg;
-				chip->group = group++;
-			}
-			exynos_gpiolib_attach_ofnode(chip,
-					EXYNOS4_PA_GPIO2, i * 0x20);
-		}
-		samsung_gpiolib_add_4bit_chips(exynos4_gpios_2,
-					       nr_chips, gpio_base2);
-
-		/* gpio part3 */
-		gpio_base3 = ioremap(EXYNOS4_PA_GPIO3, SZ_256);
-		if (gpio_base3 == NULL) {
-			pr_err("unable to ioremap for gpio_base3\n");
-			goto err_ioremap3;
-		}
-
-		chip = exynos4_gpios_3;
-		nr_chips = ARRAY_SIZE(exynos4_gpios_3);
-
-		for (i = 0; i < nr_chips; i++, chip++) {
-			if (!chip->config) {
-				chip->config = &exynos_gpio_cfg;
-				chip->group = group++;
-			}
-			exynos_gpiolib_attach_ofnode(chip,
-					EXYNOS4_PA_GPIO3, i * 0x20);
-		}
-		samsung_gpiolib_add_4bit_chips(exynos4_gpios_3,
-					       nr_chips, gpio_base3);
-
-#if defined(CONFIG_CPU_EXYNOS4210) && defined(CONFIG_S5P_GPIO_INT)
-		s5p_register_gpioint_bank(IRQ_GPIO_XA, 0, IRQ_GPIO1_NR_GROUPS);
-		s5p_register_gpioint_bank(IRQ_GPIO_XB, IRQ_GPIO1_NR_GROUPS, IRQ_GPIO2_NR_GROUPS);
-#endif
-
-#endif	/* CONFIG_CPU_EXYNOS4210 */
+		exynos4_gpiolib_init();
 	} else if (soc_is_exynos5250()) {
-#ifdef CONFIG_SOC_EXYNOS5250
-		void __iomem *gpx_base;
-
-		/* gpio part1 */
-		gpio_base1 = ioremap(EXYNOS5_PA_GPIO1, SZ_4K);
-		if (gpio_base1 == NULL) {
-			pr_err("unable to ioremap for gpio_base1\n");
-			goto err_ioremap1;
-		}
-
-		/* need to set base address for gpx */
-		chip = &exynos5_gpios_1[20];
-		gpx_base = gpio_base1 + 0xC00;
-		for (i = 0; i < 4; i++, chip++, gpx_base += 0x20)
-			chip->base = gpx_base;
-
-		chip = exynos5_gpios_1;
-		nr_chips = ARRAY_SIZE(exynos5_gpios_1);
-
-		for (i = 0; i < nr_chips; i++, chip++) {
-			if (!chip->config) {
-				chip->config = &exynos_gpio_cfg;
-				chip->group = group++;
-			}
-			exynos_gpiolib_attach_ofnode(chip,
-					EXYNOS5_PA_GPIO1, i * 0x20);
-		}
-		samsung_gpiolib_add_4bit_chips(exynos5_gpios_1,
-					       nr_chips, gpio_base1);
-
-		/* gpio part2 */
-		gpio_base2 = ioremap(EXYNOS5_PA_GPIO2, SZ_4K);
-		if (gpio_base2 == NULL) {
-			pr_err("unable to ioremap for gpio_base2\n");
-			goto err_ioremap2;
-		}
-
-		chip = exynos5_gpios_2;
-		nr_chips = ARRAY_SIZE(exynos5_gpios_2);
-
-		for (i = 0; i < nr_chips; i++, chip++) {
-			if (!chip->config) {
-				chip->config = &exynos_gpio_cfg;
-				chip->group = group++;
-			}
-			exynos_gpiolib_attach_ofnode(chip,
-					EXYNOS5_PA_GPIO2, i * 0x20);
-		}
-		samsung_gpiolib_add_4bit_chips(exynos5_gpios_2,
-					       nr_chips, gpio_base2);
-
-		/* gpio part3 */
-		gpio_base3 = ioremap(EXYNOS5_PA_GPIO3, SZ_4K);
-		if (gpio_base3 == NULL) {
-			pr_err("unable to ioremap for gpio_base3\n");
-			goto err_ioremap3;
-		}
-
-		/* need to set base address for gpv */
-		exynos5_gpios_3[0].base = gpio_base3;
-		exynos5_gpios_3[1].base = gpio_base3 + 0x20;
-		exynos5_gpios_3[2].base = gpio_base3 + 0x60;
-		exynos5_gpios_3[3].base = gpio_base3 + 0x80;
-		exynos5_gpios_3[4].base = gpio_base3 + 0xC0;
-
-		chip = exynos5_gpios_3;
-		nr_chips = ARRAY_SIZE(exynos5_gpios_3);
-
-		for (i = 0; i < nr_chips; i++, chip++) {
-			if (!chip->config) {
-				chip->config = &exynos_gpio_cfg;
-				chip->group = group++;
-			}
-			exynos_gpiolib_attach_ofnode(chip,
-					EXYNOS5_PA_GPIO3, i * 0x20);
-		}
-		samsung_gpiolib_add_4bit_chips(exynos5_gpios_3,
-					       nr_chips, gpio_base3);
-
-		/* gpio part4 */
-		gpio_base4 = ioremap(EXYNOS5_PA_GPIO4, SZ_4K);
-		if (gpio_base4 == NULL) {
-			pr_err("unable to ioremap for gpio_base4\n");
-			goto err_ioremap4;
-		}
-
-		chip = exynos5_gpios_4;
-		nr_chips = ARRAY_SIZE(exynos5_gpios_4);
-
-		for (i = 0; i < nr_chips; i++, chip++) {
-			if (!chip->config) {
-				chip->config = &exynos_gpio_cfg;
-				chip->group = group++;
-			}
-			exynos_gpiolib_attach_ofnode(chip,
-					EXYNOS5_PA_GPIO4, i * 0x20);
-		}
-		samsung_gpiolib_add_4bit_chips(exynos5_gpios_4,
-					       nr_chips, gpio_base4);
-#endif	/* CONFIG_SOC_EXYNOS5250 */
+		exynos5_gpiolib_init();
 	} else {
 		WARN(1, "Unknown SoC in gpio-samsung, no GPIOs added\n");
 		return -ENODEV;
 	}
 
 	return 0;
-
-err_ioremap4:
-	iounmap(gpio_base3);
-err_ioremap3:
-	iounmap(gpio_base2);
-err_ioremap2:
-	iounmap(gpio_base1);
-err_ioremap1:
-	return -ENOMEM;
 }
 core_initcall(samsung_gpiolib_init);
 
diff --git a/drivers/gpio/gpio-sch.c b/drivers/gpio/gpio-sch.c
index 8cadf4d683a..424dce8e3f3 100644
--- a/drivers/gpio/gpio-sch.c
+++ b/drivers/gpio/gpio-sch.c
@@ -232,6 +232,14 @@ static int __devinit sch_gpio_probe(struct platform_device *pdev)
 			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:
 			return -ENODEV;
 	}
diff --git a/drivers/gpio/gpio-sodaville.c b/drivers/gpio/gpio-sodaville.c
index 031e5d24837..9d9891f7a60 100644
--- a/drivers/gpio/gpio-sodaville.c
+++ b/drivers/gpio/gpio-sodaville.c
@@ -224,7 +224,7 @@ static int __devinit sdv_gpio_probe(struct pci_dev *pdev,
 
 	ret = bgpio_init(&sd->bgpio, &pdev->dev, 4,
 			sd->gpio_pub_base + GPINR, sd->gpio_pub_base + GPOUTR,
-			NULL, sd->gpio_pub_base + GPOER, NULL, false);
+			NULL, sd->gpio_pub_base + GPOER, NULL, 0);
 	if (ret)
 		goto unmap;
 	sd->bgpio.gc.ngpio = SDV_NUM_PUB_GPIOS;
@@ -282,17 +282,7 @@ static struct pci_driver sdv_gpio_driver = {
 	.remove = sdv_gpio_remove,
 };
 
-static int __init sdv_gpio_init(void)
-{
-	return pci_register_driver(&sdv_gpio_driver);
-}
-module_init(sdv_gpio_init);
-
-static void __exit sdv_gpio_exit(void)
-{
-	pci_unregister_driver(&sdv_gpio_driver);
-}
-module_exit(sdv_gpio_exit);
+module_pci_driver(sdv_gpio_driver);
 
 MODULE_AUTHOR("Hans J. Koch <hjk@linutronix.de>");
 MODULE_DESCRIPTION("GPIO interface for Intel Sodaville SoCs");
diff --git a/drivers/gpio/gpio-sta2x11.c b/drivers/gpio/gpio-sta2x11.c
new file mode 100644
index 00000000000..38416be8ba1
--- /dev/null
+++ b/drivers/gpio/gpio-sta2x11.c
@@ -0,0 +1,435 @@
+/*
+ * STMicroelectronics ConneXt (STA2X11) GPIO driver
+ *
+ * Copyright 2012 ST Microelectronics (Alessandro Rubini)
+ * Based on gpio-ml-ioh.c, Copyright 2010 OKI Semiconductors Ltd.
+ * Also based on previous sta2x11 work, Copyright 2011 Wind River Systems, 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/kernel.h>
+#include <linux/slab.h>
+#include <linux/gpio.h>
+#include <linux/interrupt.h>
+#include <linux/irq.h>
+#include <linux/pci.h>
+#include <linux/platform_device.h>
+#include <linux/mfd/sta2x11-mfd.h>
+
+struct gsta_regs {
+	u32 dat;		/* 0x00 */
+	u32 dats;
+	u32 datc;
+	u32 pdis;
+	u32 dir;		/* 0x10 */
+	u32 dirs;
+	u32 dirc;
+	u32 unused_1c;
+	u32 afsela;		/* 0x20 */
+	u32 unused_24[7];
+	u32 rimsc;		/* 0x40 */
+	u32 fimsc;
+	u32 is;
+	u32 ic;
+};
+
+struct gsta_gpio {
+	spinlock_t			lock;
+	struct device			*dev;
+	void __iomem			*reg_base;
+	struct gsta_regs __iomem	*regs[GSTA_NR_BLOCKS];
+	struct gpio_chip		gpio;
+	int				irq_base;
+	/* FIXME: save the whole config here (AF, ...) */
+	unsigned			irq_type[GSTA_NR_GPIO];
+};
+
+static inline struct gsta_regs __iomem *__regs(struct gsta_gpio *chip, int nr)
+{
+	return chip->regs[nr / GSTA_GPIO_PER_BLOCK];
+}
+
+static inline u32 __bit(int nr)
+{
+	return 1U << (nr % GSTA_GPIO_PER_BLOCK);
+}
+
+/*
+ * gpio methods
+ */
+
+static void gsta_gpio_set(struct gpio_chip *gpio, unsigned nr, int val)
+{
+	struct gsta_gpio *chip = container_of(gpio, struct gsta_gpio, gpio);
+	struct gsta_regs __iomem *regs = __regs(chip, nr);
+	u32 bit = __bit(nr);
+
+	if (val)
+		writel(bit, &regs->dats);
+	else
+		writel(bit, &regs->datc);
+}
+
+static int gsta_gpio_get(struct gpio_chip *gpio, unsigned nr)
+{
+	struct gsta_gpio *chip = container_of(gpio, struct gsta_gpio, gpio);
+	struct gsta_regs __iomem *regs = __regs(chip, nr);
+	u32 bit = __bit(nr);
+
+	return readl(&regs->dat) & bit;
+}
+
+static int gsta_gpio_direction_output(struct gpio_chip *gpio, unsigned nr,
+				      int val)
+{
+	struct gsta_gpio *chip = container_of(gpio, struct gsta_gpio, gpio);
+	struct gsta_regs __iomem *regs = __regs(chip, nr);
+	u32 bit = __bit(nr);
+
+	writel(bit, &regs->dirs);
+	/* Data register after direction, otherwise pullup/down is selected */
+	if (val)
+		writel(bit, &regs->dats);
+	else
+		writel(bit, &regs->datc);
+	return 0;
+}
+
+static int gsta_gpio_direction_input(struct gpio_chip *gpio, unsigned nr)
+{
+	struct gsta_gpio *chip = container_of(gpio, struct gsta_gpio, gpio);
+	struct gsta_regs __iomem *regs = __regs(chip, nr);
+	u32 bit = __bit(nr);
+
+	writel(bit, &regs->dirc);
+	return 0;
+}
+
+static int gsta_gpio_to_irq(struct gpio_chip *gpio, unsigned offset)
+{
+	struct gsta_gpio *chip = container_of(gpio, struct gsta_gpio, gpio);
+	return chip->irq_base + offset;
+}
+
+static void gsta_gpio_setup(struct gsta_gpio *chip) /* called from probe */
+{
+	struct gpio_chip *gpio = &chip->gpio;
+
+	/*
+	 * ARCH_NR_GPIOS is currently 256 and dynamic allocation starts
+	 * from the end. However, for compatibility, we need the first
+	 * ConneXt device to start from gpio 0: it's the main chipset
+	 * on most boards so documents and drivers assume gpio0..gpio127
+	 */
+	static int gpio_base;
+
+	gpio->label = dev_name(chip->dev);
+	gpio->owner = THIS_MODULE;
+	gpio->direction_input = gsta_gpio_direction_input;
+	gpio->get = gsta_gpio_get;
+	gpio->direction_output = gsta_gpio_direction_output;
+	gpio->set = gsta_gpio_set;
+	gpio->dbg_show = NULL;
+	gpio->base = gpio_base;
+	gpio->ngpio = GSTA_NR_GPIO;
+	gpio->can_sleep = 0;
+	gpio->to_irq = gsta_gpio_to_irq;
+
+	/*
+	 * After the first device, turn to dynamic gpio numbers.
+	 * For example, with ARCH_NR_GPIOS = 256 we can fit two cards
+	 */
+	if (!gpio_base)
+		gpio_base = -1;
+}
+
+/*
+ * Special method: alternate functions and pullup/pulldown. This is only
+ * invoked on startup to configure gpio's according to platform data.
+ * FIXME : this functionality shall be managed (and exported to other drivers)
+ * via the pin control subsystem.
+ */
+static void gsta_set_config(struct gsta_gpio *chip, int nr, unsigned cfg)
+{
+	struct gsta_regs __iomem *regs = __regs(chip, nr);
+	unsigned long flags;
+	u32 bit = __bit(nr);
+	u32 val;
+	int err = 0;
+
+	pr_info("%s: %p %i %i\n", __func__, chip, nr, cfg);
+
+	if (cfg == PINMUX_TYPE_NONE)
+		return;
+
+	/* Alternate function or not? */
+	spin_lock_irqsave(&chip->lock, flags);
+	val = readl(&regs->afsela);
+	if (cfg == PINMUX_TYPE_FUNCTION)
+		val |= bit;
+	else
+		val &= ~bit;
+	writel(val | bit, &regs->afsela);
+	if (cfg == PINMUX_TYPE_FUNCTION) {
+		spin_unlock_irqrestore(&chip->lock, flags);
+		return;
+	}
+
+	/* not alternate function: set details */
+	switch (cfg) {
+	case PINMUX_TYPE_OUTPUT_LOW:
+		writel(bit, &regs->dirs);
+		writel(bit, &regs->datc);
+		break;
+	case PINMUX_TYPE_OUTPUT_HIGH:
+		writel(bit, &regs->dirs);
+		writel(bit, &regs->dats);
+		break;
+	case PINMUX_TYPE_INPUT:
+		writel(bit, &regs->dirc);
+		val = readl(&regs->pdis) | bit;
+		writel(val, &regs->pdis);
+		break;
+	case PINMUX_TYPE_INPUT_PULLUP:
+		writel(bit, &regs->dirc);
+		val = readl(&regs->pdis) & ~bit;
+		writel(val, &regs->pdis);
+		writel(bit, &regs->dats);
+		break;
+	case PINMUX_TYPE_INPUT_PULLDOWN:
+		writel(bit, &regs->dirc);
+		val = readl(&regs->pdis) & ~bit;
+		writel(val, &regs->pdis);
+		writel(bit, &regs->datc);
+		break;
+	default:
+		err = 1;
+	}
+	spin_unlock_irqrestore(&chip->lock, flags);
+	if (err)
+		pr_err("%s: chip %p, pin %i, cfg %i is invalid\n",
+		       __func__, chip, nr, cfg);
+}
+
+/*
+ * Irq methods
+ */
+
+static void gsta_irq_disable(struct irq_data *data)
+{
+	struct irq_chip_generic *gc = irq_data_get_irq_chip_data(data);
+	struct gsta_gpio *chip = gc->private;
+	int nr = data->irq - chip->irq_base;
+	struct gsta_regs __iomem *regs = __regs(chip, nr);
+	u32 bit = __bit(nr);
+	u32 val;
+	unsigned long flags;
+
+	spin_lock_irqsave(&chip->lock, flags);
+	if (chip->irq_type[nr] & IRQ_TYPE_EDGE_RISING) {
+		val = readl(&regs->rimsc) & ~bit;
+		writel(val, &regs->rimsc);
+	}
+	if (chip->irq_type[nr] & IRQ_TYPE_EDGE_FALLING) {
+		val = readl(&regs->fimsc) & ~bit;
+		writel(val, &regs->fimsc);
+	}
+	spin_unlock_irqrestore(&chip->lock, flags);
+	return;
+}
+
+static void gsta_irq_enable(struct irq_data *data)
+{
+	struct irq_chip_generic *gc = irq_data_get_irq_chip_data(data);
+	struct gsta_gpio *chip = gc->private;
+	int nr = data->irq - chip->irq_base;
+	struct gsta_regs __iomem *regs = __regs(chip, nr);
+	u32 bit = __bit(nr);
+	u32 val;
+	int type;
+	unsigned long flags;
+
+	type = chip->irq_type[nr];
+
+	spin_lock_irqsave(&chip->lock, flags);
+	val = readl(&regs->rimsc);
+	if (type & IRQ_TYPE_EDGE_RISING)
+		writel(val | bit, &regs->rimsc);
+	else
+		writel(val & ~bit, &regs->rimsc);
+	val = readl(&regs->rimsc);
+	if (type & IRQ_TYPE_EDGE_FALLING)
+		writel(val | bit, &regs->fimsc);
+	else
+		writel(val & ~bit, &regs->fimsc);
+	spin_unlock_irqrestore(&chip->lock, flags);
+	return;
+}
+
+static int gsta_irq_type(struct irq_data *d, unsigned int type)
+{
+	struct irq_chip_generic *gc = irq_data_get_irq_chip_data(d);
+	struct gsta_gpio *chip = gc->private;
+	int nr = d->irq - chip->irq_base;
+
+	/* We only support edge interrupts */
+	if (!(type & (IRQ_TYPE_EDGE_RISING | IRQ_TYPE_EDGE_FALLING))) {
+		pr_debug("%s: unsupported type 0x%x\n", __func__, type);
+		return -EINVAL;
+	}
+
+	chip->irq_type[nr] = type; /* used for enable/disable */
+
+	gsta_irq_enable(d);
+	return 0;
+}
+
+static irqreturn_t gsta_gpio_handler(int irq, void *dev_id)
+{
+	struct gsta_gpio *chip = dev_id;
+	struct gsta_regs __iomem *regs;
+	u32 is;
+	int i, nr, base;
+	irqreturn_t ret = IRQ_NONE;
+
+	for (i = 0; i < GSTA_NR_BLOCKS; i++) {
+		regs = chip->regs[i];
+		base = chip->irq_base + i * GSTA_GPIO_PER_BLOCK;
+		while ((is = readl(&regs->is))) {
+			nr = __ffs(is);
+			irq = base + nr;
+			generic_handle_irq(irq);
+			writel(1 << nr, &regs->ic);
+			ret = IRQ_HANDLED;
+		}
+	}
+	return ret;
+}
+
+static __devinit void gsta_alloc_irq_chip(struct gsta_gpio *chip)
+{
+	struct irq_chip_generic *gc;
+	struct irq_chip_type *ct;
+
+	gc = irq_alloc_generic_chip(KBUILD_MODNAME, 1, chip->irq_base,
+				     chip->reg_base, handle_simple_irq);
+	gc->private = chip;
+	ct = gc->chip_types;
+
+	ct->chip.irq_set_type = gsta_irq_type;
+	ct->chip.irq_disable = gsta_irq_disable;
+	ct->chip.irq_enable = gsta_irq_enable;
+
+	/* FIXME: this makes at most 32 interrupts. Request 0 by now */
+	irq_setup_generic_chip(gc, 0 /* IRQ_MSK(GSTA_GPIO_PER_BLOCK) */, 0,
+			       IRQ_NOREQUEST | IRQ_NOPROBE, 0);
+
+	/* Set up all all 128 interrupts: code from setup_generic_chip */
+	{
+		struct irq_chip_type *ct = gc->chip_types;
+		int i, j;
+		for (j = 0; j < GSTA_NR_GPIO; j++) {
+			i = chip->irq_base + j;
+			irq_set_chip_and_handler(i, &ct->chip, ct->handler);
+			irq_set_chip_data(i, gc);
+			irq_modify_status(i, IRQ_NOREQUEST | IRQ_NOPROBE, 0);
+		}
+		gc->irq_cnt = i - gc->irq_base;
+	}
+}
+
+/* The platform device used here is instantiated by the MFD device */
+static int __devinit gsta_probe(struct platform_device *dev)
+{
+	int i, err;
+	struct pci_dev *pdev;
+	struct sta2x11_gpio_pdata *gpio_pdata;
+	struct gsta_gpio *chip;
+	struct resource *res;
+
+	pdev = *(struct pci_dev **)(dev->dev.platform_data);
+	gpio_pdata = dev_get_platdata(&pdev->dev);
+
+	if (gpio_pdata == NULL)
+		dev_err(&dev->dev, "no gpio config\n");
+	pr_debug("gpio config: %p\n", gpio_pdata);
+
+	res = platform_get_resource(dev, IORESOURCE_MEM, 0);
+
+	chip = devm_kzalloc(&dev->dev, sizeof(*chip), GFP_KERNEL);
+	chip->dev = &dev->dev;
+	chip->reg_base = devm_request_and_ioremap(&dev->dev, res);
+
+	for (i = 0; i < GSTA_NR_BLOCKS; i++) {
+		chip->regs[i] = chip->reg_base + i * 4096;
+		/* disable all irqs */
+		writel(0, &chip->regs[i]->rimsc);
+		writel(0, &chip->regs[i]->fimsc);
+		writel(~0, &chip->regs[i]->ic);
+	}
+	spin_lock_init(&chip->lock);
+	gsta_gpio_setup(chip);
+	for (i = 0; i < GSTA_NR_GPIO; i++)
+		gsta_set_config(chip, i, gpio_pdata->pinconfig[i]);
+
+	/* 384 was used in previous code: be compatible for other drivers */
+	err = irq_alloc_descs(-1, 384, GSTA_NR_GPIO, NUMA_NO_NODE);
+	if (err < 0) {
+		dev_warn(&dev->dev, "sta2x11 gpio: Can't get irq base (%i)\n",
+			 -err);
+		return err;
+	}
+	chip->irq_base = err;
+	gsta_alloc_irq_chip(chip);
+
+	err = request_irq(pdev->irq, gsta_gpio_handler,
+			     IRQF_SHARED, KBUILD_MODNAME, chip);
+	if (err < 0) {
+		dev_err(&dev->dev, "sta2x11 gpio: Can't request irq (%i)\n",
+			-err);
+		goto err_free_descs;
+	}
+
+	err = gpiochip_add(&chip->gpio);
+	if (err < 0) {
+		dev_err(&dev->dev, "sta2x11 gpio: Can't register (%i)\n",
+			-err);
+		goto err_free_irq;
+	}
+
+	platform_set_drvdata(dev, chip);
+	return 0;
+
+err_free_irq:
+	free_irq(pdev->irq, chip);
+err_free_descs:
+	irq_free_descs(chip->irq_base, GSTA_NR_GPIO);
+	return err;
+}
+
+static struct platform_driver sta2x11_gpio_platform_driver = {
+	.driver = {
+		.name	= "sta2x11-gpio",
+		.owner	= THIS_MODULE,
+	},
+	.probe = gsta_probe,
+};
+
+module_platform_driver(sta2x11_gpio_platform_driver);
+
+MODULE_LICENSE("GPL v2");
+MODULE_DESCRIPTION("sta2x11_gpio GPIO driver");
diff --git a/drivers/gpio/gpio-stp-xway.c b/drivers/gpio/gpio-stp-xway.c
new file mode 100644
index 00000000000..e35096bf3cf
--- /dev/null
+++ b/drivers/gpio/gpio-stp-xway.c
@@ -0,0 +1,301 @@
+/*
+ *  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.
+ *
+ *  Copyright (C) 2012 John Crispin <blogic@openwrt.org>
+ *
+ */
+
+#include <linux/slab.h>
+#include <linux/init.h>
+#include <linux/module.h>
+#include <linux/types.h>
+#include <linux/of_platform.h>
+#include <linux/mutex.h>
+#include <linux/gpio.h>
+#include <linux/io.h>
+#include <linux/of_gpio.h>
+#include <linux/clk.h>
+#include <linux/err.h>
+
+#include <lantiq_soc.h>
+
+/*
+ * The Serial To Parallel (STP) is found on MIPS based Lantiq socs. It is a
+ * peripheral controller used to drive external shift register cascades. At most
+ * 3 groups of 8 bits can be driven. The hardware is able to allow the DSL modem
+ * to drive the 2 LSBs of the cascade automatically.
+ */
+
+/* control register 0 */
+#define XWAY_STP_CON0		0x00
+/* control register 1 */
+#define XWAY_STP_CON1		0x04
+/* data register 0 */
+#define XWAY_STP_CPU0		0x08
+/* data register 1 */
+#define XWAY_STP_CPU1		0x0C
+/* access register */
+#define XWAY_STP_AR		0x10
+
+/* software or hardware update select bit */
+#define XWAY_STP_CON_SWU	BIT(31)
+
+/* automatic update rates */
+#define XWAY_STP_2HZ		0
+#define XWAY_STP_4HZ		BIT(23)
+#define XWAY_STP_8HZ		BIT(24)
+#define XWAY_STP_10HZ		(BIT(24) | BIT(23))
+#define XWAY_STP_SPEED_MASK	(0xf << 23)
+
+/* clock source for automatic update */
+#define XWAY_STP_UPD_FPI	BIT(31)
+#define XWAY_STP_UPD_MASK	(BIT(31) | BIT(30))
+
+/* let the adsl core drive the 2 LSBs */
+#define XWAY_STP_ADSL_SHIFT	24
+#define XWAY_STP_ADSL_MASK	0x3
+
+/* 2 groups of 3 bits can be driven by the phys */
+#define XWAY_STP_PHY_MASK	0x3
+#define XWAY_STP_PHY1_SHIFT	27
+#define XWAY_STP_PHY2_SHIFT	15
+
+/* STP has 3 groups of 8 bits */
+#define XWAY_STP_GROUP0		BIT(0)
+#define XWAY_STP_GROUP1		BIT(1)
+#define XWAY_STP_GROUP2		BIT(2)
+#define XWAY_STP_GROUP_MASK	(0x7)
+
+/* Edge configuration bits */
+#define XWAY_STP_FALLING	BIT(26)
+#define XWAY_STP_EDGE_MASK	BIT(26)
+
+#define xway_stp_r32(m, reg)		__raw_readl(m + reg)
+#define xway_stp_w32(m, val, reg)	__raw_writel(val, m + reg)
+#define xway_stp_w32_mask(m, clear, set, reg) \
+		ltq_w32((ltq_r32(m + reg) & ~(clear)) | (set), \
+		m + reg)
+
+struct xway_stp {
+	struct gpio_chip gc;
+	void __iomem *virt;
+	u32 edge;	/* rising or falling edge triggered shift register */
+	u16 shadow;	/* shadow the shift registers state */
+	u8 groups;	/* we can drive 1-3 groups of 8bit each */
+	u8 dsl;		/* the 2 LSBs can be driven by the dsl core */
+	u8 phy1;	/* 3 bits can be driven by phy1 */
+	u8 phy2;	/* 3 bits can be driven by phy2 */
+	u8 reserved;	/* mask out the hw driven bits in gpio_request */
+};
+
+/**
+ * xway_stp_set() - gpio_chip->set - set gpios.
+ * @gc:     Pointer to gpio_chip device structure.
+ * @gpio:   GPIO signal number.
+ * @val:    Value to be written to specified signal.
+ *
+ * Set the shadow value and call ltq_ebu_apply.
+ */
+static void xway_stp_set(struct gpio_chip *gc, unsigned gpio, int val)
+{
+	struct xway_stp *chip =
+		container_of(gc, struct xway_stp, gc);
+
+	if (val)
+		chip->shadow |= BIT(gpio);
+	else
+		chip->shadow &= ~BIT(gpio);
+	xway_stp_w32(chip->virt, chip->shadow, XWAY_STP_CPU0);
+	xway_stp_w32_mask(chip->virt, 0, XWAY_STP_CON_SWU, XWAY_STP_CON0);
+}
+
+/**
+ * xway_stp_dir_out() - gpio_chip->dir_out - set gpio direction.
+ * @gc:     Pointer to gpio_chip device structure.
+ * @gpio:   GPIO signal number.
+ * @val:    Value to be written to specified signal.
+ *
+ * Same as xway_stp_set, always returns 0.
+ */
+static int xway_stp_dir_out(struct gpio_chip *gc, unsigned gpio, int val)
+{
+	xway_stp_set(gc, gpio, val);
+
+	return 0;
+}
+
+/**
+ * xway_stp_request() - gpio_chip->request
+ * @gc:     Pointer to gpio_chip device structure.
+ * @gpio:   GPIO signal number.
+ *
+ * We mask out the HW driven pins
+ */
+static int xway_stp_request(struct gpio_chip *gc, unsigned gpio)
+{
+	struct xway_stp *chip =
+		container_of(gc, struct xway_stp, gc);
+
+	if ((gpio < 8) && (chip->reserved & BIT(gpio))) {
+		dev_err(gc->dev, "GPIO %d is driven by hardware\n", gpio);
+		return -ENODEV;
+	}
+
+	return 0;
+}
+
+/**
+ * xway_stp_hw_init() - Configure the STP unit and enable the clock gate
+ * @virt: pointer to the remapped register range
+ */
+static int xway_stp_hw_init(struct xway_stp *chip)
+{
+	/* sane defaults */
+	xway_stp_w32(chip->virt, 0, XWAY_STP_AR);
+	xway_stp_w32(chip->virt, 0, XWAY_STP_CPU0);
+	xway_stp_w32(chip->virt, 0, XWAY_STP_CPU1);
+	xway_stp_w32(chip->virt, XWAY_STP_CON_SWU, XWAY_STP_CON0);
+	xway_stp_w32(chip->virt, 0, XWAY_STP_CON1);
+
+	/* apply edge trigger settings for the shift register */
+	xway_stp_w32_mask(chip->virt, XWAY_STP_EDGE_MASK,
+				chip->edge, XWAY_STP_CON0);
+
+	/* apply led group settings */
+	xway_stp_w32_mask(chip->virt, XWAY_STP_GROUP_MASK,
+				chip->groups, XWAY_STP_CON1);
+
+	/* tell the hardware which pins are controlled by the dsl modem */
+	xway_stp_w32_mask(chip->virt,
+			XWAY_STP_ADSL_MASK << XWAY_STP_ADSL_SHIFT,
+			chip->dsl << XWAY_STP_ADSL_SHIFT,
+			XWAY_STP_CON0);
+
+	/* tell the hardware which pins are controlled by the phys */
+	xway_stp_w32_mask(chip->virt,
+			XWAY_STP_PHY_MASK << XWAY_STP_PHY1_SHIFT,
+			chip->phy1 << XWAY_STP_PHY1_SHIFT,
+			XWAY_STP_CON0);
+	xway_stp_w32_mask(chip->virt,
+			XWAY_STP_PHY_MASK << XWAY_STP_PHY2_SHIFT,
+			chip->phy2 << XWAY_STP_PHY2_SHIFT,
+			XWAY_STP_CON1);
+
+	/* mask out the hw driven bits in gpio_request */
+	chip->reserved = (chip->phy2 << 5) | (chip->phy1 << 2) | chip->dsl;
+
+	/*
+	 * if we have pins that are driven by hw, we need to tell the stp what
+	 * clock to use as a timer.
+	 */
+	if (chip->reserved)
+		xway_stp_w32_mask(chip->virt, XWAY_STP_UPD_MASK,
+			XWAY_STP_UPD_FPI, XWAY_STP_CON1);
+
+	return 0;
+}
+
+static int __devinit xway_stp_probe(struct platform_device *pdev)
+{
+	struct resource *res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	const __be32 *shadow, *groups, *dsl, *phy;
+	struct xway_stp *chip;
+	struct clk *clk;
+	int ret = 0;
+
+	if (!res) {
+		dev_err(&pdev->dev, "failed to request STP resource\n");
+		return -ENOENT;
+	}
+
+	chip = devm_kzalloc(&pdev->dev, sizeof(*chip), GFP_KERNEL);
+	if (!chip)
+		return -ENOMEM;
+
+	chip->virt = devm_request_and_ioremap(&pdev->dev, res);
+	if (!chip->virt) {
+		dev_err(&pdev->dev, "failed to remap STP memory\n");
+		return -ENOMEM;
+	}
+	chip->gc.dev = &pdev->dev;
+	chip->gc.label = "stp-xway";
+	chip->gc.direction_output = xway_stp_dir_out;
+	chip->gc.set = xway_stp_set;
+	chip->gc.request = xway_stp_request;
+	chip->gc.base = -1;
+	chip->gc.owner = THIS_MODULE;
+
+	/* store the shadow value if one was passed by the devicetree */
+	shadow = of_get_property(pdev->dev.of_node, "lantiq,shadow", NULL);
+	if (shadow)
+		chip->shadow = be32_to_cpu(*shadow);
+
+	/* find out which gpio groups should be enabled */
+	groups = of_get_property(pdev->dev.of_node, "lantiq,groups", NULL);
+	if (groups)
+		chip->groups = be32_to_cpu(*groups) & XWAY_STP_GROUP_MASK;
+	else
+		chip->groups = XWAY_STP_GROUP0;
+	chip->gc.ngpio = fls(chip->groups) * 8;
+
+	/* find out which gpios are controlled by the dsl core */
+	dsl = of_get_property(pdev->dev.of_node, "lantiq,dsl", NULL);
+	if (dsl)
+		chip->dsl = be32_to_cpu(*dsl) & XWAY_STP_ADSL_MASK;
+
+	/* find out which gpios are controlled by the phys */
+	if (of_machine_is_compatible("lantiq,ar9") ||
+			of_machine_is_compatible("lantiq,gr9") ||
+			of_machine_is_compatible("lantiq,vr9")) {
+		phy = of_get_property(pdev->dev.of_node, "lantiq,phy1", NULL);
+		if (phy)
+			chip->phy1 = be32_to_cpu(*phy) & XWAY_STP_PHY_MASK;
+		phy = of_get_property(pdev->dev.of_node, "lantiq,phy2", NULL);
+		if (phy)
+			chip->phy2 = be32_to_cpu(*phy) & XWAY_STP_PHY_MASK;
+	}
+
+	/* check which edge trigger we should use, default to a falling edge */
+	if (!of_find_property(pdev->dev.of_node, "lantiq,rising", NULL))
+		chip->edge = XWAY_STP_FALLING;
+
+	clk = clk_get(&pdev->dev, NULL);
+	if (IS_ERR(clk)) {
+		dev_err(&pdev->dev, "Failed to get clock\n");
+		return PTR_ERR(clk);
+	}
+	clk_enable(clk);
+
+	ret = xway_stp_hw_init(chip);
+	if (!ret)
+		ret = gpiochip_add(&chip->gc);
+
+	if (!ret)
+		dev_info(&pdev->dev, "Init done\n");
+
+	return ret;
+}
+
+static const struct of_device_id xway_stp_match[] = {
+	{ .compatible = "lantiq,gpio-stp-xway" },
+	{},
+};
+MODULE_DEVICE_TABLE(of, xway_stp_match);
+
+static struct platform_driver xway_stp_driver = {
+	.probe = xway_stp_probe,
+	.driver = {
+		.name = "gpio-stp-xway",
+		.owner = THIS_MODULE,
+		.of_match_table = xway_stp_match,
+	},
+};
+
+int __init xway_stp_init(void)
+{
+	return platform_driver_register(&xway_stp_driver);
+}
+
+subsys_initcall(xway_stp_init);
diff --git a/drivers/gpio/gpio-tegra.c b/drivers/gpio/gpio-tegra.c
index 12f349b3830..dc5184d5789 100644
--- a/drivers/gpio/gpio-tegra.c
+++ b/drivers/gpio/gpio-tegra.c
@@ -26,10 +26,10 @@
 #include <linux/platform_device.h>
 #include <linux/module.h>
 #include <linux/irqdomain.h>
+#include <linux/pinctrl/consumer.h>
 
 #include <asm/mach/irq.h>
 
-#include <mach/gpio-tegra.h>
 #include <mach/iomap.h>
 #include <mach/suspend.h>
 
@@ -108,18 +108,29 @@ static void tegra_gpio_mask_write(u32 reg, int gpio, int value)
 	tegra_gpio_writel(val, reg);
 }
 
-void tegra_gpio_enable(int gpio)
+static void tegra_gpio_enable(int gpio)
 {
 	tegra_gpio_mask_write(GPIO_MSK_CNF(gpio), gpio, 1);
 }
 EXPORT_SYMBOL_GPL(tegra_gpio_enable);
 
-void tegra_gpio_disable(int gpio)
+static void tegra_gpio_disable(int gpio)
 {
 	tegra_gpio_mask_write(GPIO_MSK_CNF(gpio), gpio, 0);
 }
 EXPORT_SYMBOL_GPL(tegra_gpio_disable);
 
+int tegra_gpio_request(struct gpio_chip *chip, unsigned offset)
+{
+	return pinctrl_request_gpio(offset);
+}
+
+void tegra_gpio_free(struct gpio_chip *chip, unsigned offset)
+{
+	pinctrl_free_gpio(offset);
+	tegra_gpio_disable(offset);
+}
+
 static void tegra_gpio_set(struct gpio_chip *chip, unsigned offset, int value)
 {
 	tegra_gpio_mask_write(GPIO_MSK_OUT(offset), offset, value);
@@ -133,6 +144,7 @@ static int tegra_gpio_get(struct gpio_chip *chip, unsigned offset)
 static int tegra_gpio_direction_input(struct gpio_chip *chip, unsigned offset)
 {
 	tegra_gpio_mask_write(GPIO_MSK_OE(offset), offset, 0);
+	tegra_gpio_enable(offset);
 	return 0;
 }
 
@@ -141,6 +153,7 @@ static int tegra_gpio_direction_output(struct gpio_chip *chip, unsigned offset,
 {
 	tegra_gpio_set(chip, offset, value);
 	tegra_gpio_mask_write(GPIO_MSK_OE(offset), offset, 1);
+	tegra_gpio_enable(offset);
 	return 0;
 }
 
@@ -151,13 +164,14 @@ static int tegra_gpio_to_irq(struct gpio_chip *chip, unsigned offset)
 
 static struct gpio_chip tegra_gpio_chip = {
 	.label			= "tegra-gpio",
+	.request		= tegra_gpio_request,
+	.free			= tegra_gpio_free,
 	.direction_input	= tegra_gpio_direction_input,
 	.get			= tegra_gpio_get,
 	.direction_output	= tegra_gpio_direction_output,
 	.set			= tegra_gpio_set,
 	.to_irq			= tegra_gpio_to_irq,
 	.base			= 0,
-	.ngpio			= TEGRA_NR_GPIOS,
 };
 
 static void tegra_gpio_irq_ack(struct irq_data *d)
@@ -224,6 +238,9 @@ static int tegra_gpio_irq_set_type(struct irq_data *d, unsigned int type)
 
 	spin_unlock_irqrestore(&bank->lvl_lock[port], flags);
 
+	tegra_gpio_mask_write(GPIO_MSK_OE(gpio), gpio, 0);
+	tegra_gpio_enable(gpio);
+
 	if (type & (IRQ_TYPE_LEVEL_LOW | IRQ_TYPE_LEVEL_HIGH))
 		__irq_set_handler_locked(d->irq, handle_level_irq);
 	else if (type & (IRQ_TYPE_EDGE_FALLING | IRQ_TYPE_EDGE_RISING))
@@ -490,20 +507,6 @@ static int __init tegra_gpio_init(void)
 }
 postcore_initcall(tegra_gpio_init);
 
-void tegra_gpio_config(struct tegra_gpio_table *table, int num)
-{
-	int i;
-
-	for (i = 0; i < num; i++) {
-		int gpio = table[i].gpio;
-
-		if (table[i].enable)
-			tegra_gpio_enable(gpio);
-		else
-			tegra_gpio_disable(gpio);
-	}
-}
-
 #ifdef	CONFIG_DEBUG_FS
 
 #include <linux/debugfs.h>
diff --git a/drivers/gpio/gpio-tps65910.c b/drivers/gpio/gpio-tps65910.c
index 7eef648a335..c1ad2884f2e 100644
--- a/drivers/gpio/gpio-tps65910.c
+++ b/drivers/gpio/gpio-tps65910.c
@@ -18,14 +18,27 @@
 #include <linux/errno.h>
 #include <linux/gpio.h>
 #include <linux/i2c.h>
+#include <linux/platform_device.h>
 #include <linux/mfd/tps65910.h>
+#include <linux/of_device.h>
+
+struct tps65910_gpio {
+	struct gpio_chip gpio_chip;
+	struct tps65910 *tps65910;
+};
+
+static inline struct tps65910_gpio *to_tps65910_gpio(struct gpio_chip *chip)
+{
+	return container_of(chip, struct tps65910_gpio, gpio_chip);
+}
 
 static int tps65910_gpio_get(struct gpio_chip *gc, unsigned offset)
 {
-	struct tps65910 *tps65910 = container_of(gc, struct tps65910, gpio);
-	uint8_t val;
+	struct tps65910_gpio *tps65910_gpio = to_tps65910_gpio(gc);
+	struct tps65910 *tps65910 = tps65910_gpio->tps65910;
+	unsigned int val;
 
-	tps65910->read(tps65910, TPS65910_GPIO0 + offset, 1, &val);
+	tps65910_reg_read(tps65910, TPS65910_GPIO0 + offset, &val);
 
 	if (val & GPIO_STS_MASK)
 		return 1;
@@ -36,83 +49,170 @@ static int tps65910_gpio_get(struct gpio_chip *gc, unsigned offset)
 static void tps65910_gpio_set(struct gpio_chip *gc, unsigned offset,
 			      int value)
 {
-	struct tps65910 *tps65910 = container_of(gc, struct tps65910, gpio);
+	struct tps65910_gpio *tps65910_gpio = to_tps65910_gpio(gc);
+	struct tps65910 *tps65910 = tps65910_gpio->tps65910;
 
 	if (value)
-		tps65910_set_bits(tps65910, TPS65910_GPIO0 + offset,
+		tps65910_reg_set_bits(tps65910, TPS65910_GPIO0 + offset,
 						GPIO_SET_MASK);
 	else
-		tps65910_clear_bits(tps65910, TPS65910_GPIO0 + offset,
+		tps65910_reg_clear_bits(tps65910, TPS65910_GPIO0 + offset,
 						GPIO_SET_MASK);
 }
 
 static int tps65910_gpio_output(struct gpio_chip *gc, unsigned offset,
 				int value)
 {
-	struct tps65910 *tps65910 = container_of(gc, struct tps65910, gpio);
+	struct tps65910_gpio *tps65910_gpio = to_tps65910_gpio(gc);
+	struct tps65910 *tps65910 = tps65910_gpio->tps65910;
 
 	/* Set the initial value */
 	tps65910_gpio_set(gc, offset, value);
 
-	return tps65910_set_bits(tps65910, TPS65910_GPIO0 + offset,
+	return tps65910_reg_set_bits(tps65910, TPS65910_GPIO0 + offset,
 						GPIO_CFG_MASK);
 }
 
 static int tps65910_gpio_input(struct gpio_chip *gc, unsigned offset)
 {
-	struct tps65910 *tps65910 = container_of(gc, struct tps65910, gpio);
+	struct tps65910_gpio *tps65910_gpio = to_tps65910_gpio(gc);
+	struct tps65910 *tps65910 = tps65910_gpio->tps65910;
 
-	return tps65910_clear_bits(tps65910, TPS65910_GPIO0 + offset,
+	return tps65910_reg_clear_bits(tps65910, TPS65910_GPIO0 + offset,
 						GPIO_CFG_MASK);
 }
 
-void tps65910_gpio_init(struct tps65910 *tps65910, int gpio_base)
+#ifdef CONFIG_OF
+static struct tps65910_board *tps65910_parse_dt_for_gpio(struct device *dev,
+		struct tps65910 *tps65910, int chip_ngpio)
 {
+	struct tps65910_board *tps65910_board = tps65910->of_plat_data;
+	unsigned int prop_array[TPS6591X_MAX_NUM_GPIO];
+	int ngpio = min(chip_ngpio, TPS6591X_MAX_NUM_GPIO);
 	int ret;
-	struct tps65910_board *board_data;
+	int idx;
+
+	tps65910_board->gpio_base = -1;
+	ret = of_property_read_u32_array(tps65910->dev->of_node,
+			"ti,en-gpio-sleep", prop_array, ngpio);
+	if (ret < 0) {
+		dev_dbg(dev, "ti,en-gpio-sleep not specified\n");
+		return tps65910_board;
+	}
 
-	if (!gpio_base)
-		return;
+	for (idx = 0; idx < ngpio; idx++)
+		tps65910_board->en_gpio_sleep[idx] = (prop_array[idx] != 0);
 
-	tps65910->gpio.owner		= THIS_MODULE;
-	tps65910->gpio.label		= tps65910->i2c_client->name;
-	tps65910->gpio.dev		= tps65910->dev;
-	tps65910->gpio.base		= gpio_base;
+	return tps65910_board;
+}
+#else
+static struct tps65910_board *tps65910_parse_dt_for_gpio(struct device *dev,
+		struct tps65910 *tps65910, int chip_ngpio)
+{
+	return NULL;
+}
+#endif
+
+static int __devinit tps65910_gpio_probe(struct platform_device *pdev)
+{
+	struct tps65910 *tps65910 = dev_get_drvdata(pdev->dev.parent);
+	struct tps65910_board *pdata = dev_get_platdata(tps65910->dev);
+	struct tps65910_gpio *tps65910_gpio;
+	int ret;
+	int i;
+
+	tps65910_gpio = devm_kzalloc(&pdev->dev,
+				sizeof(*tps65910_gpio), GFP_KERNEL);
+	if (!tps65910_gpio) {
+		dev_err(&pdev->dev, "Could not allocate tps65910_gpio\n");
+		return -ENOMEM;
+	}
+
+	tps65910_gpio->tps65910 = tps65910;
+
+	tps65910_gpio->gpio_chip.owner = THIS_MODULE;
+	tps65910_gpio->gpio_chip.label = tps65910->i2c_client->name;
 
 	switch(tps65910_chip_id(tps65910)) {
 	case TPS65910:
-		tps65910->gpio.ngpio	= TPS65910_NUM_GPIO;
+		tps65910_gpio->gpio_chip.ngpio = TPS65910_NUM_GPIO;
 		break;
 	case TPS65911:
-		tps65910->gpio.ngpio	= TPS65911_NUM_GPIO;
+		tps65910_gpio->gpio_chip.ngpio = TPS65911_NUM_GPIO;
 		break;
 	default:
-		return;
+		return -EINVAL;
+	}
+	tps65910_gpio->gpio_chip.can_sleep = 1;
+	tps65910_gpio->gpio_chip.direction_input = tps65910_gpio_input;
+	tps65910_gpio->gpio_chip.direction_output = tps65910_gpio_output;
+	tps65910_gpio->gpio_chip.set	= tps65910_gpio_set;
+	tps65910_gpio->gpio_chip.get	= tps65910_gpio_get;
+	tps65910_gpio->gpio_chip.dev = &pdev->dev;
+	if (pdata && pdata->gpio_base)
+		tps65910_gpio->gpio_chip.base = pdata->gpio_base;
+	else
+		tps65910_gpio->gpio_chip.base = -1;
+
+	if (!pdata && tps65910->dev->of_node)
+		pdata = tps65910_parse_dt_for_gpio(&pdev->dev, tps65910,
+			tps65910_gpio->gpio_chip.ngpio);
+
+	if (!pdata)
+		goto skip_init;
+
+	/* Configure sleep control for gpios if provided */
+	for (i = 0; i < tps65910_gpio->gpio_chip.ngpio; ++i) {
+		if (!pdata->en_gpio_sleep[i])
+			continue;
+
+		ret = tps65910_reg_set_bits(tps65910,
+			TPS65910_GPIO0 + i, GPIO_SLEEP_MASK);
+		if (ret < 0)
+			dev_warn(tps65910->dev,
+				"GPIO Sleep setting failed with err %d\n", ret);
 	}
-	tps65910->gpio.can_sleep	= 1;
-
-	tps65910->gpio.direction_input	= tps65910_gpio_input;
-	tps65910->gpio.direction_output	= tps65910_gpio_output;
-	tps65910->gpio.set		= tps65910_gpio_set;
-	tps65910->gpio.get		= tps65910_gpio_get;
-
-	/* Configure sleep control for gpios */
-	board_data = dev_get_platdata(tps65910->dev);
-	if (board_data) {
-		int i;
-		for (i = 0; i < tps65910->gpio.ngpio; ++i) {
-			if (board_data->en_gpio_sleep[i]) {
-				ret = tps65910_set_bits(tps65910,
-					TPS65910_GPIO0 + i, GPIO_SLEEP_MASK);
-				if (ret < 0)
-					dev_warn(tps65910->dev,
-						"GPIO Sleep setting failed\n");
-			}
-		}
+
+skip_init:
+	ret = gpiochip_add(&tps65910_gpio->gpio_chip);
+	if (ret < 0) {
+		dev_err(&pdev->dev, "Could not register gpiochip, %d\n", ret);
+		return ret;
 	}
 
-	ret = gpiochip_add(&tps65910->gpio);
+	platform_set_drvdata(pdev, tps65910_gpio);
+
+	return ret;
+}
+
+static int __devexit tps65910_gpio_remove(struct platform_device *pdev)
+{
+	struct tps65910_gpio *tps65910_gpio = platform_get_drvdata(pdev);
 
-	if (ret)
-		dev_warn(tps65910->dev, "GPIO registration failed: %d\n", ret);
+	return gpiochip_remove(&tps65910_gpio->gpio_chip);
 }
+
+static struct platform_driver tps65910_gpio_driver = {
+	.driver.name    = "tps65910-gpio",
+	.driver.owner   = THIS_MODULE,
+	.probe		= tps65910_gpio_probe,
+	.remove		= __devexit_p(tps65910_gpio_remove),
+};
+
+static int __init tps65910_gpio_init(void)
+{
+	return platform_driver_register(&tps65910_gpio_driver);
+}
+subsys_initcall(tps65910_gpio_init);
+
+static void __exit tps65910_gpio_exit(void)
+{
+	platform_driver_unregister(&tps65910_gpio_driver);
+}
+module_exit(tps65910_gpio_exit);
+
+MODULE_AUTHOR("Graeme Gregory <gg@slimlogic.co.uk>");
+MODULE_AUTHOR("Jorge Eduardo Candelaria jedu@slimlogic.co.uk>");
+MODULE_DESCRIPTION("GPIO interface for TPS65910/TPS6511 PMICs");
+MODULE_LICENSE("GPL v2");
+MODULE_ALIAS("platform:tps65910-gpio");
diff --git a/drivers/gpio/gpio-wm831x.c b/drivers/gpio/gpio-wm831x.c
index deb949e75ec..e56a2165641 100644
--- a/drivers/gpio/gpio-wm831x.c
+++ b/drivers/gpio/gpio-wm831x.c
@@ -102,10 +102,8 @@ static int wm831x_gpio_to_irq(struct gpio_chip *chip, unsigned offset)
 	struct wm831x_gpio *wm831x_gpio = to_wm831x_gpio(chip);
 	struct wm831x *wm831x = wm831x_gpio->wm831x;
 
-	if (!wm831x->irq_base)
-		return -EINVAL;
-
-	return wm831x->irq_base + WM831X_IRQ_GPIO_1 + offset;
+	return irq_create_mapping(wm831x->irq_domain,
+				  WM831X_IRQ_GPIO_1 + offset);
 }
 
 static int wm831x_gpio_set_debounce(struct gpio_chip *chip, unsigned offset,
diff --git a/drivers/of/gpio.c b/drivers/gpio/gpiolib-of.c
index bf984b6dc47..d18068a9f3e 100644
--- a/drivers/of/gpio.c
+++ b/drivers/gpio/gpiolib-of.c
@@ -15,11 +15,39 @@
 #include <linux/errno.h>
 #include <linux/module.h>
 #include <linux/io.h>
+#include <linux/gpio.h>
 #include <linux/of.h>
 #include <linux/of_address.h>
 #include <linux/of_gpio.h>
 #include <linux/slab.h>
 
+/* Private data structure for of_gpiochip_is_match */
+struct gg_data {
+	enum of_gpio_flags *flags;
+	struct of_phandle_args gpiospec;
+
+	int out_gpio;
+};
+
+/* Private function for resolving node pointer to gpio_chip */
+static int of_gpiochip_find_and_xlate(struct gpio_chip *gc, void *data)
+{
+	struct gg_data *gg_data = data;
+	int ret;
+
+	if ((gc->of_node != gg_data->gpiospec.np) ||
+	    (gc->of_gpio_n_cells != gg_data->gpiospec.args_count) ||
+	    (!gc->of_xlate))
+		return false;
+
+	ret = gc->of_xlate(gc, &gg_data->gpiospec, gg_data->flags);
+	if (ret < 0)
+		return false;
+
+	gg_data->out_gpio = ret + gc->base;
+	return true;
+}
+
 /**
  * of_get_named_gpio_flags() - Get a GPIO number and flags to use with GPIO API
  * @np:		device node to get GPIO from
@@ -34,46 +62,25 @@
 int of_get_named_gpio_flags(struct device_node *np, const char *propname,
                            int index, enum of_gpio_flags *flags)
 {
+	struct gg_data gg_data = { .flags = flags, .out_gpio = -ENODEV };
 	int ret;
-	struct gpio_chip *gc;
-	struct of_phandle_args gpiospec;
+
+	/* .of_xlate might decide to not fill in the flags, so clear it. */
+	if (flags)
+		*flags = 0;
 
 	ret = of_parse_phandle_with_args(np, propname, "#gpio-cells", index,
-					 &gpiospec);
+					 &gg_data.gpiospec);
 	if (ret) {
 		pr_debug("%s: can't parse gpios property\n", __func__);
-		goto err0;
-	}
-
-	gc = of_node_to_gpiochip(gpiospec.np);
-	if (!gc) {
-		pr_debug("%s: gpio controller %s isn't registered\n",
-			 np->full_name, gpiospec.np->full_name);
-		ret = -ENODEV;
-		goto err1;
-	}
-
-	if (gpiospec.args_count != gc->of_gpio_n_cells) {
-		pr_debug("%s: wrong #gpio-cells for %s\n",
-			 np->full_name, gpiospec.np->full_name);
-		ret = -EINVAL;
-		goto err1;
+		return -EINVAL;
 	}
 
-	/* .xlate might decide to not fill in the flags, so clear it. */
-	if (flags)
-		*flags = 0;
-
-	ret = gc->of_xlate(gc, &gpiospec, flags);
-	if (ret < 0)
-		goto err1;
+	gpiochip_find(&gg_data, of_gpiochip_find_and_xlate);
 
-	ret += gc->base;
-err1:
-	of_node_put(gpiospec.np);
-err0:
+	of_node_put(gg_data.gpiospec.np);
 	pr_debug("%s exited with status %d\n", __func__, ret);
-	return ret;
+	return gg_data.out_gpio;
 }
 EXPORT_SYMBOL(of_get_named_gpio_flags);
 
@@ -227,14 +234,3 @@ void of_gpiochip_remove(struct gpio_chip *chip)
 	if (chip->of_node)
 		of_node_put(chip->of_node);
 }
-
-/* Private function for resolving node pointer to gpio_chip */
-static int of_gpiochip_is_match(struct gpio_chip *chip, const void *data)
-{
-	return chip->of_node == data;
-}
-
-struct gpio_chip *of_node_to_gpiochip(struct device_node *np)
-{
-	return gpiochip_find(np, of_gpiochip_is_match);
-}
diff --git a/drivers/gpio/gpiolib.c b/drivers/gpio/gpiolib.c
index 5a75510d66b..120b2a0e316 100644
--- a/drivers/gpio/gpiolib.c
+++ b/drivers/gpio/gpiolib.c
@@ -1093,7 +1093,7 @@ unlock:
 	if (status)
 		goto fail;
 
-	pr_info("gpiochip_add: registered GPIOs %d to %d on device: %s\n",
+	pr_debug("gpiochip_add: registered GPIOs %d to %d on device: %s\n",
 		chip->base, chip->base + chip->ngpio - 1,
 		chip->label ? : "generic");
 
@@ -1154,9 +1154,9 @@ EXPORT_SYMBOL_GPL(gpiochip_remove);
  * non-zero, this function will return to the caller and not iterate over any
  * more gpio_chips.
  */
-struct gpio_chip *gpiochip_find(const void *data,
+struct gpio_chip *gpiochip_find(void *data,
 				int (*match)(struct gpio_chip *chip,
-					     const void *data))
+					     void *data))
 {
 	struct gpio_chip *chip = NULL;
 	unsigned long flags;
@@ -1302,8 +1302,18 @@ int gpio_request_one(unsigned gpio, unsigned long flags, const char *label)
 				(flags & GPIOF_INIT_HIGH) ? 1 : 0);
 
 	if (err)
-		gpio_free(gpio);
+		goto free_gpio;
+
+	if (flags & GPIOF_EXPORT) {
+		err = gpio_export(gpio, flags & GPIOF_EXPORT_CHANGEABLE);
+		if (err)
+			goto free_gpio;
+	}
+
+	return 0;
 
+ free_gpio:
+	gpio_free(gpio);
 	return err;
 }
 EXPORT_SYMBOL_GPL(gpio_request_one);
diff --git a/drivers/gpu/drm/Kconfig b/drivers/gpu/drm/Kconfig
index e354bc0b052..23120c00a88 100644
--- a/drivers/gpu/drm/Kconfig
+++ b/drivers/gpu/drm/Kconfig
@@ -186,3 +186,9 @@ source "drivers/gpu/drm/vmwgfx/Kconfig"
 source "drivers/gpu/drm/gma500/Kconfig"
 
 source "drivers/gpu/drm/udl/Kconfig"
+
+source "drivers/gpu/drm/ast/Kconfig"
+
+source "drivers/gpu/drm/mgag200/Kconfig"
+
+source "drivers/gpu/drm/cirrus/Kconfig"
diff --git a/drivers/gpu/drm/Makefile b/drivers/gpu/drm/Makefile
index c20da5bda35..f65f65ed0dd 100644
--- a/drivers/gpu/drm/Makefile
+++ b/drivers/gpu/drm/Makefile
@@ -34,6 +34,8 @@ obj-$(CONFIG_DRM_RADEON)+= radeon/
 obj-$(CONFIG_DRM_MGA)	+= mga/
 obj-$(CONFIG_DRM_I810)	+= i810/
 obj-$(CONFIG_DRM_I915)  += i915/
+obj-$(CONFIG_DRM_MGAG200) += mgag200/
+obj-$(CONFIG_DRM_CIRRUS_QEMU) += cirrus/
 obj-$(CONFIG_DRM_SIS)   += sis/
 obj-$(CONFIG_DRM_SAVAGE)+= savage/
 obj-$(CONFIG_DRM_VMWGFX)+= vmwgfx/
@@ -42,4 +44,5 @@ obj-$(CONFIG_DRM_NOUVEAU) +=nouveau/
 obj-$(CONFIG_DRM_EXYNOS) +=exynos/
 obj-$(CONFIG_DRM_GMA500) += gma500/
 obj-$(CONFIG_DRM_UDL) += udl/
+obj-$(CONFIG_DRM_AST) += ast/
 obj-y			+= i2c/
diff --git a/drivers/gpu/drm/ast/Kconfig b/drivers/gpu/drm/ast/Kconfig
new file mode 100644
index 00000000000..a277b125788
--- /dev/null
+++ b/drivers/gpu/drm/ast/Kconfig
@@ -0,0 +1,16 @@
+config DRM_AST
+	tristate "AST server chips"
+	depends on DRM && PCI && EXPERIMENTAL
+	select DRM_TTM
+	select FB_SYS_COPYAREA
+	select FB_SYS_FILLRECT
+	select FB_SYS_IMAGEBLIT
+	select DRM_KMS_HELPER
+	select DRM_TTM
+	help
+	 Say yes for experimental AST GPU driver. Do not enable
+	 this driver without having a working -modesetting,
+	 and a version of AST that knows to fail if KMS
+	 is bound to the driver. These GPUs are commonly found
+	 in server chipsets.
+
diff --git a/drivers/gpu/drm/ast/Makefile b/drivers/gpu/drm/ast/Makefile
new file mode 100644
index 00000000000..8df4f284ee2
--- /dev/null
+++ b/drivers/gpu/drm/ast/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
+
+ast-y := ast_drv.o ast_main.o ast_mode.o ast_fb.o ast_ttm.o ast_post.o
+
+obj-$(CONFIG_DRM_AST) := ast.o
\ No newline at end of file
diff --git a/drivers/gpu/drm/ast/ast_dram_tables.h b/drivers/gpu/drm/ast/ast_dram_tables.h
new file mode 100644
index 00000000000..cc04539c0ff
--- /dev/null
+++ b/drivers/gpu/drm/ast/ast_dram_tables.h
@@ -0,0 +1,144 @@
+#ifndef AST_DRAM_TABLES_H
+#define AST_DRAM_TABLES_H
+
+/* DRAM timing tables */
+struct ast_dramstruct {
+	u16 index;
+	u32 data;
+};
+
+static const struct ast_dramstruct ast2000_dram_table_data[] = {
+	{ 0x0108, 0x00000000 },
+	{ 0x0120, 0x00004a21 },
+	{ 0xFF00, 0x00000043 },
+	{ 0x0000, 0xFFFFFFFF },
+	{ 0x0004, 0x00000089 },
+	{ 0x0008, 0x22331353 },
+	{ 0x000C, 0x0d07000b },
+	{ 0x0010, 0x11113333 },
+	{ 0x0020, 0x00110350 },
+	{ 0x0028, 0x1e0828f0 },
+	{ 0x0024, 0x00000001 },
+	{ 0x001C, 0x00000000 },
+	{ 0x0014, 0x00000003 },
+	{ 0xFF00, 0x00000043 },
+	{ 0x0018, 0x00000131 },
+	{ 0x0014, 0x00000001 },
+	{ 0xFF00, 0x00000043 },
+	{ 0x0018, 0x00000031 },
+	{ 0x0014, 0x00000001 },
+	{ 0xFF00, 0x00000043 },
+	{ 0x0028, 0x1e0828f1 },
+	{ 0x0024, 0x00000003 },
+	{ 0x002C, 0x1f0f28fb },
+	{ 0x0030, 0xFFFFFE01 },
+	{ 0xFFFF, 0xFFFFFFFF }
+};
+
+static const struct ast_dramstruct ast1100_dram_table_data[] = {
+	{ 0x2000, 0x1688a8a8 },
+	{ 0x2020, 0x000041f0 },
+	{ 0xFF00, 0x00000043 },
+	{ 0x0000, 0xfc600309 },
+	{ 0x006C, 0x00909090 },
+	{ 0x0064, 0x00050000 },
+	{ 0x0004, 0x00000585 },
+	{ 0x0008, 0x0011030f },
+	{ 0x0010, 0x22201724 },
+	{ 0x0018, 0x1e29011a },
+	{ 0x0020, 0x00c82222 },
+	{ 0x0014, 0x01001523 },
+	{ 0x001C, 0x1024010d },
+	{ 0x0024, 0x00cb2522 },
+	{ 0x0038, 0xffffff82 },
+	{ 0x003C, 0x00000000 },
+	{ 0x0040, 0x00000000 },
+	{ 0x0044, 0x00000000 },
+	{ 0x0048, 0x00000000 },
+	{ 0x004C, 0x00000000 },
+	{ 0x0050, 0x00000000 },
+	{ 0x0054, 0x00000000 },
+	{ 0x0058, 0x00000000 },
+	{ 0x005C, 0x00000000 },
+	{ 0x0060, 0x032aa02a },
+	{ 0x0064, 0x002d3000 },
+	{ 0x0068, 0x00000000 },
+	{ 0x0070, 0x00000000 },
+	{ 0x0074, 0x00000000 },
+	{ 0x0078, 0x00000000 },
+	{ 0x007C, 0x00000000 },
+	{ 0x0034, 0x00000001 },
+	{ 0xFF00, 0x00000043 },
+	{ 0x002C, 0x00000732 },
+	{ 0x0030, 0x00000040 },
+	{ 0x0028, 0x00000005 },
+	{ 0x0028, 0x00000007 },
+	{ 0x0028, 0x00000003 },
+	{ 0x0028, 0x00000001 },
+	{ 0x000C, 0x00005a08 },
+	{ 0x002C, 0x00000632 },
+	{ 0x0028, 0x00000001 },
+	{ 0x0030, 0x000003c0 },
+	{ 0x0028, 0x00000003 },
+	{ 0x0030, 0x00000040 },
+	{ 0x0028, 0x00000003 },
+	{ 0x000C, 0x00005a21 },
+	{ 0x0034, 0x00007c03 },
+	{ 0x0120, 0x00004c41 },
+	{ 0xffff, 0xffffffff },
+};
+
+static const struct ast_dramstruct ast2100_dram_table_data[] = {
+	{ 0x2000, 0x1688a8a8 },
+	{ 0x2020, 0x00004120 },
+	{ 0xFF00, 0x00000043 },
+	{ 0x0000, 0xfc600309 },
+	{ 0x006C, 0x00909090 },
+	{ 0x0064, 0x00070000 },
+	{ 0x0004, 0x00000489 },
+	{ 0x0008, 0x0011030f },
+	{ 0x0010, 0x32302926 },
+	{ 0x0018, 0x274c0122 },
+	{ 0x0020, 0x00ce2222 },
+	{ 0x0014, 0x01001523 },
+	{ 0x001C, 0x1024010d },
+	{ 0x0024, 0x00cb2522 },
+	{ 0x0038, 0xffffff82 },
+	{ 0x003C, 0x00000000 },
+	{ 0x0040, 0x00000000 },
+	{ 0x0044, 0x00000000 },
+	{ 0x0048, 0x00000000 },
+	{ 0x004C, 0x00000000 },
+	{ 0x0050, 0x00000000 },
+	{ 0x0054, 0x00000000 },
+	{ 0x0058, 0x00000000 },
+	{ 0x005C, 0x00000000 },
+	{ 0x0060, 0x0f2aa02a },
+	{ 0x0064, 0x003f3005 },
+	{ 0x0068, 0x02020202 },
+	{ 0x0070, 0x00000000 },
+	{ 0x0074, 0x00000000 },
+	{ 0x0078, 0x00000000 },
+	{ 0x007C, 0x00000000 },
+	{ 0x0034, 0x00000001 },
+	{ 0xFF00, 0x00000043 },
+	{ 0x002C, 0x00000942 },
+	{ 0x0030, 0x00000040 },
+	{ 0x0028, 0x00000005 },
+	{ 0x0028, 0x00000007 },
+	{ 0x0028, 0x00000003 },
+	{ 0x0028, 0x00000001 },
+	{ 0x000C, 0x00005a08 },
+	{ 0x002C, 0x00000842 },
+	{ 0x0028, 0x00000001 },
+	{ 0x0030, 0x000003c0 },
+	{ 0x0028, 0x00000003 },
+	{ 0x0030, 0x00000040 },
+	{ 0x0028, 0x00000003 },
+	{ 0x000C, 0x00005a21 },
+	{ 0x0034, 0x00007c03 },
+	{ 0x0120, 0x00005061 },
+	{ 0xffff, 0xffffffff },
+};
+
+#endif
diff --git a/drivers/gpu/drm/ast/ast_drv.c b/drivers/gpu/drm/ast/ast_drv.c
new file mode 100644
index 00000000000..d0c4574ef49
--- /dev/null
+++ b/drivers/gpu/drm/ast/ast_drv.c
@@ -0,0 +1,244 @@
+/*
+ * 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, 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: Dave Airlie <airlied@redhat.com>
+ */
+#include <linux/module.h>
+#include <linux/console.h>
+
+#include "drmP.h"
+#include "drm.h"
+#include "drm_crtc_helper.h"
+
+#include "ast_drv.h"
+
+int ast_modeset = -1;
+
+MODULE_PARM_DESC(modeset, "Disable/Enable modesetting");
+module_param_named(modeset, ast_modeset, int, 0400);
+
+#define PCI_VENDOR_ASPEED 0x1a03
+
+static struct drm_driver driver;
+
+#define AST_VGA_DEVICE(id, info) {		\
+	.class = PCI_BASE_CLASS_DISPLAY << 16,	\
+	.class_mask = 0xff0000,			\
+	.vendor = PCI_VENDOR_ASPEED,			\
+	.device = id,				\
+	.subvendor = PCI_ANY_ID,		\
+	.subdevice = PCI_ANY_ID,		\
+	.driver_data = (unsigned long) info }
+
+static DEFINE_PCI_DEVICE_TABLE(pciidlist) = {
+	AST_VGA_DEVICE(PCI_CHIP_AST2000, NULL),
+	AST_VGA_DEVICE(PCI_CHIP_AST2100, NULL),
+	/*	AST_VGA_DEVICE(PCI_CHIP_AST1180, NULL), - don't bind to 1180 for now */
+	{0, 0, 0},
+};
+
+MODULE_DEVICE_TABLE(pci, pciidlist);
+
+static int __devinit
+ast_pci_probe(struct pci_dev *pdev, const struct pci_device_id *ent)
+{
+	return drm_get_pci_dev(pdev, ent, &driver);
+}
+
+static void
+ast_pci_remove(struct pci_dev *pdev)
+{
+	struct drm_device *dev = pci_get_drvdata(pdev);
+
+	drm_put_dev(dev);
+}
+
+
+
+static int ast_drm_freeze(struct drm_device *dev)
+{
+	drm_kms_helper_poll_disable(dev);
+
+	pci_save_state(dev->pdev);
+
+	console_lock();
+	ast_fbdev_set_suspend(dev, 1);
+	console_unlock();
+	return 0;
+}
+
+static int ast_drm_thaw(struct drm_device *dev)
+{
+	int error = 0;
+
+	ast_post_gpu(dev);
+
+	drm_mode_config_reset(dev);
+	mutex_lock(&dev->mode_config.mutex);
+	drm_helper_resume_force_mode(dev);
+	mutex_unlock(&dev->mode_config.mutex);
+
+	console_lock();
+	ast_fbdev_set_suspend(dev, 0);
+	console_unlock();
+	return error;
+}
+
+static int ast_drm_resume(struct drm_device *dev)
+{
+	int ret;
+
+	if (pci_enable_device(dev->pdev))
+		return -EIO;
+
+	ret = ast_drm_thaw(dev);
+	if (ret)
+		return ret;
+
+	drm_kms_helper_poll_enable(dev);
+	return 0;
+}
+
+static int ast_pm_suspend(struct device *dev)
+{
+	struct pci_dev *pdev = to_pci_dev(dev);
+	struct drm_device *ddev = pci_get_drvdata(pdev);
+	int error;
+
+	error = ast_drm_freeze(ddev);
+	if (error)
+		return error;
+
+	pci_disable_device(pdev);
+	pci_set_power_state(pdev, PCI_D3hot);
+	return 0;
+}
+static int ast_pm_resume(struct device *dev)
+{
+	struct pci_dev *pdev = to_pci_dev(dev);
+	struct drm_device *ddev = pci_get_drvdata(pdev);
+	return ast_drm_resume(ddev);
+}
+
+static int ast_pm_freeze(struct device *dev)
+{
+	struct pci_dev *pdev = to_pci_dev(dev);
+	struct drm_device *ddev = pci_get_drvdata(pdev);
+
+	if (!ddev || !ddev->dev_private)
+		return -ENODEV;
+	return ast_drm_freeze(ddev);
+
+}
+
+static int ast_pm_thaw(struct device *dev)
+{
+	struct pci_dev *pdev = to_pci_dev(dev);
+	struct drm_device *ddev = pci_get_drvdata(pdev);
+	return ast_drm_thaw(ddev);
+}
+
+static int ast_pm_poweroff(struct device *dev)
+{
+	struct pci_dev *pdev = to_pci_dev(dev);
+	struct drm_device *ddev = pci_get_drvdata(pdev);
+
+	return ast_drm_freeze(ddev);
+}
+
+static const struct dev_pm_ops ast_pm_ops = {
+	.suspend = ast_pm_suspend,
+	.resume = ast_pm_resume,
+	.freeze = ast_pm_freeze,
+	.thaw = ast_pm_thaw,
+	.poweroff = ast_pm_poweroff,
+	.restore = ast_pm_resume,
+};
+
+static struct pci_driver ast_pci_driver = {
+	.name = DRIVER_NAME,
+	.id_table = pciidlist,
+	.probe = ast_pci_probe,
+	.remove = ast_pci_remove,
+	.driver.pm = &ast_pm_ops,
+};
+
+static const struct file_operations ast_fops = {
+	.owner = THIS_MODULE,
+	.open = drm_open,
+	.release = drm_release,
+	.unlocked_ioctl = drm_ioctl,
+	.mmap = ast_mmap,
+	.poll = drm_poll,
+	.fasync = drm_fasync,
+	.read = drm_read,
+};
+
+static struct drm_driver driver = {
+	.driver_features = DRIVER_USE_MTRR | DRIVER_MODESET | DRIVER_GEM,
+	.dev_priv_size = 0,
+
+	.load = ast_driver_load,
+	.unload = ast_driver_unload,
+
+	.fops = &ast_fops,
+	.name = DRIVER_NAME,
+	.desc = DRIVER_DESC,
+	.date = DRIVER_DATE,
+	.major = DRIVER_MAJOR,
+	.minor = DRIVER_MINOR,
+	.patchlevel = DRIVER_PATCHLEVEL,
+
+	.gem_init_object = ast_gem_init_object,
+	.gem_free_object = ast_gem_free_object,
+	.dumb_create = ast_dumb_create,
+	.dumb_map_offset = ast_dumb_mmap_offset,
+	.dumb_destroy = ast_dumb_destroy,
+
+};
+
+static int __init ast_init(void)
+{
+#ifdef CONFIG_VGA_CONSOLE
+	if (vgacon_text_force() && ast_modeset == -1)
+		return -EINVAL;
+#endif
+
+	if (ast_modeset == 0)
+		return -EINVAL;
+	return drm_pci_init(&driver, &ast_pci_driver);
+}
+static void __exit ast_exit(void)
+{
+	drm_pci_exit(&driver, &ast_pci_driver);
+}
+
+module_init(ast_init);
+module_exit(ast_exit);
+
+MODULE_AUTHOR(DRIVER_AUTHOR);
+MODULE_DESCRIPTION(DRIVER_DESC);
+MODULE_LICENSE("GPL and additional rights");
+
diff --git a/drivers/gpu/drm/ast/ast_drv.h b/drivers/gpu/drm/ast/ast_drv.h
new file mode 100644
index 00000000000..d4af9edcbb9
--- /dev/null
+++ b/drivers/gpu/drm/ast/ast_drv.h
@@ -0,0 +1,356 @@
+/*
+ * 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, 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: Dave Airlie <airlied@redhat.com>
+ */
+#ifndef __AST_DRV_H__
+#define __AST_DRV_H__
+
+#include "drm_fb_helper.h"
+
+#include "ttm/ttm_bo_api.h"
+#include "ttm/ttm_bo_driver.h"
+#include "ttm/ttm_placement.h"
+#include "ttm/ttm_memory.h"
+#include "ttm/ttm_module.h"
+
+#include <linux/i2c.h>
+#include <linux/i2c-algo-bit.h>
+
+#define DRIVER_AUTHOR		"Dave Airlie"
+
+#define DRIVER_NAME		"ast"
+#define DRIVER_DESC		"AST"
+#define DRIVER_DATE		"20120228"
+
+#define DRIVER_MAJOR		0
+#define DRIVER_MINOR		1
+#define DRIVER_PATCHLEVEL	0
+
+#define PCI_CHIP_AST2000 0x2000
+#define PCI_CHIP_AST2100 0x2010
+#define PCI_CHIP_AST1180 0x1180
+
+
+enum ast_chip {
+	AST2000,
+	AST2100,
+	AST1100,
+	AST2200,
+	AST2150,
+	AST2300,
+	AST1180,
+};
+
+#define AST_DRAM_512Mx16 0
+#define AST_DRAM_1Gx16   1
+#define AST_DRAM_512Mx32 2
+#define AST_DRAM_1Gx32   3
+#define AST_DRAM_2Gx16   6
+#define AST_DRAM_4Gx16   7
+
+struct ast_fbdev;
+
+struct ast_private {
+	struct drm_device *dev;
+
+	void __iomem *regs;
+	void __iomem *ioregs;
+
+	enum ast_chip chip;
+	bool vga2_clone;
+	uint32_t dram_bus_width;
+	uint32_t dram_type;
+	uint32_t mclk;
+	uint32_t vram_size;
+
+	struct ast_fbdev *fbdev;
+
+	int fb_mtrr;
+
+	struct {
+		struct drm_global_reference mem_global_ref;
+		struct ttm_bo_global_ref bo_global_ref;
+		struct ttm_bo_device bdev;
+		atomic_t validate_sequence;
+	} ttm;
+
+	struct drm_gem_object *cursor_cache;
+	uint64_t cursor_cache_gpu_addr;
+	struct ttm_bo_kmap_obj cache_kmap;
+	int next_cursor;
+};
+
+int ast_driver_load(struct drm_device *dev, unsigned long flags);
+int ast_driver_unload(struct drm_device *dev);
+
+struct ast_gem_object;
+
+#define AST_IO_AR_PORT_WRITE		(0x40)
+#define AST_IO_MISC_PORT_WRITE		(0x42)
+#define AST_IO_SEQ_PORT			(0x44)
+#define AST_DAC_INDEX_READ		(0x3c7)
+#define AST_IO_DAC_INDEX_WRITE		(0x48)
+#define AST_IO_DAC_DATA		        (0x49)
+#define AST_IO_GR_PORT			(0x4E)
+#define AST_IO_CRTC_PORT		(0x54)
+#define AST_IO_INPUT_STATUS1_READ	(0x5A)
+#define AST_IO_MISC_PORT_READ		(0x4C)
+
+#define __ast_read(x) \
+static inline u##x ast_read##x(struct ast_private *ast, u32 reg) { \
+u##x val = 0;\
+val = ioread##x(ast->regs + reg); \
+return val;\
+}
+
+__ast_read(8);
+__ast_read(16);
+__ast_read(32)
+
+#define __ast_io_read(x) \
+static inline u##x ast_io_read##x(struct ast_private *ast, u32 reg) { \
+u##x val = 0;\
+val = ioread##x(ast->ioregs + reg); \
+return val;\
+}
+
+__ast_io_read(8);
+__ast_io_read(16);
+__ast_io_read(32);
+
+#define __ast_write(x) \
+static inline void ast_write##x(struct ast_private *ast, u32 reg, u##x val) {\
+	iowrite##x(val, ast->regs + reg);\
+	}
+
+__ast_write(8);
+__ast_write(16);
+__ast_write(32);
+
+#define __ast_io_write(x) \
+static inline void ast_io_write##x(struct ast_private *ast, u32 reg, u##x val) {\
+	iowrite##x(val, ast->ioregs + reg);\
+	}
+
+__ast_io_write(8);
+__ast_io_write(16);
+#undef __ast_io_write
+
+static inline void ast_set_index_reg(struct ast_private *ast,
+				     uint32_t base, uint8_t index,
+				     uint8_t val)
+{
+	ast_io_write16(ast, base, ((u16)val << 8) | index);
+}
+
+void ast_set_index_reg_mask(struct ast_private *ast,
+			    uint32_t base, uint8_t index,
+			    uint8_t mask, uint8_t val);
+uint8_t ast_get_index_reg(struct ast_private *ast,
+			  uint32_t base, uint8_t index);
+uint8_t ast_get_index_reg_mask(struct ast_private *ast,
+			       uint32_t base, uint8_t index, uint8_t mask);
+
+static inline void ast_open_key(struct ast_private *ast)
+{
+	ast_set_index_reg_mask(ast, AST_IO_CRTC_PORT, 0xA1, 0xFF, 0x04);
+}
+
+#define AST_VIDMEM_SIZE_8M    0x00800000
+#define AST_VIDMEM_SIZE_16M   0x01000000
+#define AST_VIDMEM_SIZE_32M   0x02000000
+#define AST_VIDMEM_SIZE_64M   0x04000000
+#define AST_VIDMEM_SIZE_128M  0x08000000
+
+#define AST_VIDMEM_DEFAULT_SIZE AST_VIDMEM_SIZE_8M
+
+#define AST_MAX_HWC_WIDTH 64
+#define AST_MAX_HWC_HEIGHT 64
+
+#define AST_HWC_SIZE                (AST_MAX_HWC_WIDTH*AST_MAX_HWC_HEIGHT*2)
+#define AST_HWC_SIGNATURE_SIZE      32
+
+#define AST_DEFAULT_HWC_NUM 2
+/* define for signature structure */
+#define AST_HWC_SIGNATURE_CHECKSUM  0x00
+#define AST_HWC_SIGNATURE_SizeX     0x04
+#define AST_HWC_SIGNATURE_SizeY     0x08
+#define AST_HWC_SIGNATURE_X         0x0C
+#define AST_HWC_SIGNATURE_Y         0x10
+#define AST_HWC_SIGNATURE_HOTSPOTX  0x14
+#define AST_HWC_SIGNATURE_HOTSPOTY  0x18
+
+
+struct ast_i2c_chan {
+	struct i2c_adapter adapter;
+	struct drm_device *dev;
+	struct i2c_algo_bit_data bit;
+};
+
+struct ast_connector {
+	struct drm_connector base;
+	struct ast_i2c_chan *i2c;
+};
+
+struct ast_crtc {
+	struct drm_crtc base;
+	u8 lut_r[256], lut_g[256], lut_b[256];
+	struct drm_gem_object *cursor_bo;
+	uint64_t cursor_addr;
+	int cursor_width, cursor_height;
+	u8 offset_x, offset_y;
+};
+
+struct ast_encoder {
+	struct drm_encoder base;
+};
+
+struct ast_framebuffer {
+	struct drm_framebuffer base;
+	struct drm_gem_object *obj;
+};
+
+struct ast_fbdev {
+	struct drm_fb_helper helper;
+	struct ast_framebuffer afb;
+	struct list_head fbdev_list;
+	void *sysram;
+	int size;
+	struct ttm_bo_kmap_obj mapping;
+};
+
+#define to_ast_crtc(x) container_of(x, struct ast_crtc, base)
+#define to_ast_connector(x) container_of(x, struct ast_connector, base)
+#define to_ast_encoder(x) container_of(x, struct ast_encoder, base)
+#define to_ast_framebuffer(x) container_of(x, struct ast_framebuffer, base)
+
+struct ast_vbios_stdtable {
+	u8 misc;
+	u8 seq[4];
+	u8 crtc[25];
+	u8 ar[20];
+	u8 gr[9];
+};
+
+struct ast_vbios_enhtable {
+	u32 ht;
+	u32 hde;
+	u32 hfp;
+	u32 hsync;
+	u32 vt;
+	u32 vde;
+	u32 vfp;
+	u32 vsync;
+	u32 dclk_index;
+	u32 flags;
+	u32 refresh_rate;
+	u32 refresh_rate_index;
+	u32 mode_id;
+};
+
+struct ast_vbios_dclk_info {
+	u8 param1;
+	u8 param2;
+	u8 param3;
+};
+
+struct ast_vbios_mode_info {
+	struct ast_vbios_stdtable *std_table;
+	struct ast_vbios_enhtable *enh_table;
+};
+
+extern int ast_mode_init(struct drm_device *dev);
+extern void ast_mode_fini(struct drm_device *dev);
+
+int ast_framebuffer_init(struct drm_device *dev,
+			 struct ast_framebuffer *ast_fb,
+			 struct drm_mode_fb_cmd2 *mode_cmd,
+			 struct drm_gem_object *obj);
+
+int ast_fbdev_init(struct drm_device *dev);
+void ast_fbdev_fini(struct drm_device *dev);
+void ast_fbdev_set_suspend(struct drm_device *dev, int state);
+
+struct ast_bo {
+	struct ttm_buffer_object bo;
+	struct ttm_placement placement;
+	struct ttm_bo_kmap_obj kmap;
+	struct drm_gem_object gem;
+	u32 placements[3];
+	int pin_count;
+};
+#define gem_to_ast_bo(gobj) container_of((gobj), struct ast_bo, gem)
+
+static inline struct ast_bo *
+ast_bo(struct ttm_buffer_object *bo)
+{
+	return container_of(bo, struct ast_bo, bo);
+}
+
+
+#define to_ast_obj(x) container_of(x, struct ast_gem_object, base)
+
+#define AST_MM_ALIGN_SHIFT 4
+#define AST_MM_ALIGN_MASK ((1 << AST_MM_ALIGN_SHIFT) - 1)
+
+extern int ast_dumb_create(struct drm_file *file,
+			   struct drm_device *dev,
+			   struct drm_mode_create_dumb *args);
+extern int ast_dumb_destroy(struct drm_file *file,
+			    struct drm_device *dev,
+			    uint32_t handle);
+
+extern int ast_gem_init_object(struct drm_gem_object *obj);
+extern void ast_gem_free_object(struct drm_gem_object *obj);
+extern int ast_dumb_mmap_offset(struct drm_file *file,
+				struct drm_device *dev,
+				uint32_t handle,
+				uint64_t *offset);
+
+#define DRM_FILE_PAGE_OFFSET (0x100000000ULL >> PAGE_SHIFT)
+
+int ast_mm_init(struct ast_private *ast);
+void ast_mm_fini(struct ast_private *ast);
+
+int ast_bo_create(struct drm_device *dev, int size, int align,
+		  uint32_t flags, struct ast_bo **pastbo);
+
+int ast_gem_create(struct drm_device *dev,
+		   u32 size, bool iskernel,
+		   struct drm_gem_object **obj);
+
+int ast_bo_pin(struct ast_bo *bo, u32 pl_flag, u64 *gpu_addr);
+int ast_bo_unpin(struct ast_bo *bo);
+
+int ast_bo_reserve(struct ast_bo *bo, bool no_wait);
+void ast_bo_unreserve(struct ast_bo *bo);
+void ast_ttm_placement(struct ast_bo *bo, int domain);
+int ast_bo_push_sysram(struct ast_bo *bo);
+int ast_mmap(struct file *filp, struct vm_area_struct *vma);
+
+/* ast post */
+void ast_post_gpu(struct drm_device *dev);
+#endif
diff --git a/drivers/gpu/drm/ast/ast_fb.c b/drivers/gpu/drm/ast/ast_fb.c
new file mode 100644
index 00000000000..2fc8e9e860b
--- /dev/null
+++ b/drivers/gpu/drm/ast/ast_fb.c
@@ -0,0 +1,341 @@
+/*
+ * 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, 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: Dave Airlie <airlied@redhat.com>
+ */
+#include <linux/module.h>
+#include <linux/kernel.h>
+#include <linux/errno.h>
+#include <linux/string.h>
+#include <linux/mm.h>
+#include <linux/tty.h>
+#include <linux/sysrq.h>
+#include <linux/delay.h>
+#include <linux/fb.h>
+#include <linux/init.h>
+
+
+#include "drmP.h"
+#include "drm.h"
+#include "drm_crtc.h"
+#include "drm_fb_helper.h"
+#include "ast_drv.h"
+
+static void ast_dirty_update(struct ast_fbdev *afbdev,
+			     int x, int y, int width, int height)
+{
+	int i;
+	struct drm_gem_object *obj;
+	struct ast_bo *bo;
+	int src_offset, dst_offset;
+	int bpp = (afbdev->afb.base.bits_per_pixel + 7)/8;
+	int ret;
+	bool unmap = false;
+
+	obj = afbdev->afb.obj;
+	bo = gem_to_ast_bo(obj);
+
+	ret = ast_bo_reserve(bo, true);
+	if (ret) {
+		DRM_ERROR("failed to reserve fb bo\n");
+		return;
+	}
+
+	if (!bo->kmap.virtual) {
+		ret = ttm_bo_kmap(&bo->bo, 0, bo->bo.num_pages, &bo->kmap);
+		if (ret) {
+			DRM_ERROR("failed to kmap fb updates\n");
+			ast_bo_unreserve(bo);
+			return;
+		}
+		unmap = true;
+	}
+	for (i = y; i < y + height; 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);
+
+	}
+	if (unmap)
+		ttm_bo_kunmap(&bo->kmap);
+
+	ast_bo_unreserve(bo);
+}
+
+static void ast_fillrect(struct fb_info *info,
+			 const struct fb_fillrect *rect)
+{
+	struct ast_fbdev *afbdev = info->par;
+	sys_fillrect(info, rect);
+	ast_dirty_update(afbdev, rect->dx, rect->dy, rect->width,
+			 rect->height);
+}
+
+static void ast_copyarea(struct fb_info *info,
+			 const struct fb_copyarea *area)
+{
+	struct ast_fbdev *afbdev = info->par;
+	sys_copyarea(info, area);
+	ast_dirty_update(afbdev, area->dx, area->dy, area->width,
+			 area->height);
+}
+
+static void ast_imageblit(struct fb_info *info,
+			  const struct fb_image *image)
+{
+	struct ast_fbdev *afbdev = info->par;
+	sys_imageblit(info, image);
+	ast_dirty_update(afbdev, image->dx, image->dy, image->width,
+			 image->height);
+}
+
+static struct fb_ops astfb_ops = {
+	.owner = THIS_MODULE,
+	.fb_check_var = drm_fb_helper_check_var,
+	.fb_set_par = drm_fb_helper_set_par,
+	.fb_fillrect = ast_fillrect,
+	.fb_copyarea = ast_copyarea,
+	.fb_imageblit = ast_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 int astfb_create_object(struct ast_fbdev *afbdev,
+			       struct drm_mode_fb_cmd2 *mode_cmd,
+			       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 = ast_gem_create(dev, size, true, &gobj);
+	if (ret)
+		return ret;
+
+	*gobj_p = gobj;
+	return ret;
+}
+
+static int astfb_create(struct ast_fbdev *afbdev,
+			struct drm_fb_helper_surface_size *sizes)
+{
+	struct drm_device *dev = afbdev->helper.dev;
+	struct drm_mode_fb_cmd2 mode_cmd;
+	struct drm_framebuffer *fb;
+	struct fb_info *info;
+	int size, ret;
+	struct device *device = &dev->pdev->dev;
+	void *sysram;
+	struct drm_gem_object *gobj = NULL;
+	struct ast_bo *bo = NULL;
+	mode_cmd.width = sizes->surface_width;
+	mode_cmd.height = sizes->surface_height;
+	mode_cmd.pitches[0] = mode_cmd.width * ((sizes->surface_bpp + 7)/8);
+
+	mode_cmd.pixel_format = drm_mode_legacy_fb_format(sizes->surface_bpp,
+							  sizes->surface_depth);
+
+	size = mode_cmd.pitches[0] * mode_cmd.height;
+
+	ret = astfb_create_object(afbdev, &mode_cmd, &gobj);
+	if (ret) {
+		DRM_ERROR("failed to create fbcon backing object %d\n", ret);
+		return ret;
+	}
+	bo = gem_to_ast_bo(gobj);
+
+	sysram = vmalloc(size);
+	if (!sysram)
+		return -ENOMEM;
+
+	info = framebuffer_alloc(0, device);
+	if (!info) {
+		ret = -ENOMEM;
+		goto out;
+	}
+	info->par = afbdev;
+
+	ret = ast_framebuffer_init(dev, &afbdev->afb, &mode_cmd, gobj);
+	if (ret)
+		goto out;
+
+	afbdev->sysram = sysram;
+	afbdev->size = size;
+
+	fb = &afbdev->afb.base;
+	afbdev->helper.fb = fb;
+	afbdev->helper.fbdev = info;
+
+	strcpy(info->fix.id, "astdrmfb");
+
+	info->flags = FBINFO_DEFAULT | FBINFO_CAN_FORCE_OUTPUT;
+	info->fbops = &astfb_ops;
+
+	ret = fb_alloc_cmap(&info->cmap, 256, 0);
+	if (ret) {
+		ret = -ENOMEM;
+		goto out;
+	}
+
+	info->apertures = alloc_apertures(1);
+	if (!info->apertures) {
+		ret = -ENOMEM;
+		goto out;
+	}
+	info->apertures->ranges[0].base = pci_resource_start(dev->pdev, 0);
+	info->apertures->ranges[0].size = pci_resource_len(dev->pdev, 0);
+
+	drm_fb_helper_fill_fix(info, fb->pitches[0], fb->depth);
+	drm_fb_helper_fill_var(info, &afbdev->helper, sizes->fb_width, sizes->fb_height);
+
+	info->screen_base = sysram;
+	info->screen_size = size;
+
+	info->pixmap.flags = FB_PIXMAP_SYSTEM;
+
+	DRM_DEBUG_KMS("allocated %dx%d\n",
+		      fb->width, fb->height);
+
+	return 0;
+out:
+	return ret;
+}
+
+static void ast_fb_gamma_set(struct drm_crtc *crtc, u16 red, u16 green,
+			       u16 blue, int regno)
+{
+	struct ast_crtc *ast_crtc = to_ast_crtc(crtc);
+	ast_crtc->lut_r[regno] = red >> 8;
+	ast_crtc->lut_g[regno] = green >> 8;
+	ast_crtc->lut_b[regno] = blue >> 8;
+}
+
+static void ast_fb_gamma_get(struct drm_crtc *crtc, u16 *red, u16 *green,
+			       u16 *blue, int regno)
+{
+	struct ast_crtc *ast_crtc = to_ast_crtc(crtc);
+	*red = ast_crtc->lut_r[regno] << 8;
+	*green = ast_crtc->lut_g[regno] << 8;
+	*blue = ast_crtc->lut_b[regno] << 8;
+}
+
+static int ast_find_or_create_single(struct drm_fb_helper *helper,
+					  struct drm_fb_helper_surface_size *sizes)
+{
+	struct ast_fbdev *afbdev = (struct ast_fbdev *)helper;
+	int new_fb = 0;
+	int ret;
+
+	if (!helper->fb) {
+		ret = astfb_create(afbdev, sizes);
+		if (ret)
+			return ret;
+		new_fb = 1;
+	}
+	return new_fb;
+}
+
+static struct drm_fb_helper_funcs ast_fb_helper_funcs = {
+	.gamma_set = ast_fb_gamma_set,
+	.gamma_get = ast_fb_gamma_get,
+	.fb_probe = ast_find_or_create_single,
+};
+
+static void ast_fbdev_destroy(struct drm_device *dev,
+			      struct ast_fbdev *afbdev)
+{
+	struct fb_info *info;
+	struct ast_framebuffer *afb = &afbdev->afb;
+	if (afbdev->helper.fbdev) {
+		info = afbdev->helper.fbdev;
+		unregister_framebuffer(info);
+		if (info->cmap.len)
+			fb_dealloc_cmap(&info->cmap);
+		framebuffer_release(info);
+	}
+
+	if (afb->obj) {
+		drm_gem_object_unreference_unlocked(afb->obj);
+		afb->obj = NULL;
+	}
+	drm_fb_helper_fini(&afbdev->helper);
+
+	vfree(afbdev->sysram);
+	drm_framebuffer_cleanup(&afb->base);
+}
+
+int ast_fbdev_init(struct drm_device *dev)
+{
+	struct ast_private *ast = dev->dev_private;
+	struct ast_fbdev *afbdev;
+	int ret;
+
+	afbdev = kzalloc(sizeof(struct ast_fbdev), GFP_KERNEL);
+	if (!afbdev)
+		return -ENOMEM;
+
+	ast->fbdev = afbdev;
+	afbdev->helper.funcs = &ast_fb_helper_funcs;
+	ret = drm_fb_helper_init(dev, &afbdev->helper,
+				 1, 1);
+	if (ret) {
+		kfree(afbdev);
+		return ret;
+	}
+
+	drm_fb_helper_single_add_all_connectors(&afbdev->helper);
+	drm_fb_helper_initial_config(&afbdev->helper, 32);
+	return 0;
+}
+
+void ast_fbdev_fini(struct drm_device *dev)
+{
+	struct ast_private *ast = dev->dev_private;
+
+	if (!ast->fbdev)
+		return;
+
+	ast_fbdev_destroy(dev, ast->fbdev);
+	kfree(ast->fbdev);
+	ast->fbdev = NULL;
+}
+
+void ast_fbdev_set_suspend(struct drm_device *dev, int state)
+{
+	struct ast_private *ast = dev->dev_private;
+
+	if (!ast->fbdev)
+		return;
+
+	fb_set_suspend(ast->fbdev->helper.fbdev, state);
+}
diff --git a/drivers/gpu/drm/ast/ast_main.c b/drivers/gpu/drm/ast/ast_main.c
new file mode 100644
index 00000000000..95ae55b8214
--- /dev/null
+++ b/drivers/gpu/drm/ast/ast_main.c
@@ -0,0 +1,527 @@
+/*
+ * 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, 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: Dave Airlie <airlied@redhat.com>
+ */
+#include "drmP.h"
+#include "ast_drv.h"
+
+
+#include "drm_fb_helper.h"
+#include "drm_crtc_helper.h"
+
+#include "ast_dram_tables.h"
+
+void ast_set_index_reg_mask(struct ast_private *ast,
+			    uint32_t base, uint8_t index,
+			    uint8_t mask, uint8_t val)
+{
+	u8 tmp;
+	ast_io_write8(ast, base, index);
+	tmp = (ast_io_read8(ast, base + 1) & mask) | val;
+	ast_set_index_reg(ast, base, index, tmp);
+}
+
+uint8_t ast_get_index_reg(struct ast_private *ast,
+			  uint32_t base, uint8_t index)
+{
+	uint8_t ret;
+	ast_io_write8(ast, base, index);
+	ret = ast_io_read8(ast, base + 1);
+	return ret;
+}
+
+uint8_t ast_get_index_reg_mask(struct ast_private *ast,
+			       uint32_t base, uint8_t index, uint8_t mask)
+{
+	uint8_t ret;
+	ast_io_write8(ast, base, index);
+	ret = ast_io_read8(ast, base + 1) & mask;
+	return ret;
+}
+
+
+static int ast_detect_chip(struct drm_device *dev)
+{
+	struct ast_private *ast = dev->dev_private;
+
+	if (dev->pdev->device == PCI_CHIP_AST1180) {
+		ast->chip = AST1100;
+		DRM_INFO("AST 1180 detected\n");
+	} else {
+		if (dev->pdev->revision >= 0x20) {
+			ast->chip = AST2300;
+			DRM_INFO("AST 2300 detected\n");
+		} else if (dev->pdev->revision >= 0x10) {
+			uint32_t data;
+			ast_write32(ast, 0xf004, 0x1e6e0000);
+			ast_write32(ast, 0xf000, 0x1);
+
+			data = ast_read32(ast, 0x1207c);
+			switch (data & 0x0300) {
+			case 0x0200:
+				ast->chip = AST1100;
+				DRM_INFO("AST 1100 detected\n");
+				break;
+			case 0x0100:
+				ast->chip = AST2200;
+				DRM_INFO("AST 2200 detected\n");
+				break;
+			case 0x0000:
+				ast->chip = AST2150;
+				DRM_INFO("AST 2150 detected\n");
+				break;
+			default:
+				ast->chip = AST2100;
+				DRM_INFO("AST 2100 detected\n");
+				break;
+			}
+			ast->vga2_clone = false;
+		} else {
+			ast->chip = 2000;
+			DRM_INFO("AST 2000 detected\n");
+		}
+	}
+	return 0;
+}
+
+static int ast_get_dram_info(struct drm_device *dev)
+{
+	struct ast_private *ast = dev->dev_private;
+	uint32_t data, data2;
+	uint32_t denum, num, div, ref_pll;
+
+	ast_write32(ast, 0xf004, 0x1e6e0000);
+	ast_write32(ast, 0xf000, 0x1);
+
+
+	ast_write32(ast, 0x10000, 0xfc600309);
+
+	do {
+		;
+	} while (ast_read32(ast, 0x10000) != 0x01);
+	data = ast_read32(ast, 0x10004);
+
+	if (data & 0x400)
+		ast->dram_bus_width = 16;
+	else
+		ast->dram_bus_width = 32;
+
+	if (ast->chip == AST2300) {
+		switch (data & 0x03) {
+		case 0:
+			ast->dram_type = AST_DRAM_512Mx16;
+			break;
+		default:
+		case 1:
+			ast->dram_type = AST_DRAM_1Gx16;
+			break;
+		case 2:
+			ast->dram_type = AST_DRAM_2Gx16;
+			break;
+		case 3:
+			ast->dram_type = AST_DRAM_4Gx16;
+			break;
+		}
+	} else {
+		switch (data & 0x0c) {
+		case 0:
+		case 4:
+			ast->dram_type = AST_DRAM_512Mx16;
+			break;
+		case 8:
+			if (data & 0x40)
+				ast->dram_type = AST_DRAM_1Gx16;
+			else
+				ast->dram_type = AST_DRAM_512Mx32;
+			break;
+		case 0xc:
+			ast->dram_type = AST_DRAM_1Gx32;
+			break;
+		}
+	}
+
+	data = ast_read32(ast, 0x10120);
+	data2 = ast_read32(ast, 0x10170);
+	if (data2 & 0x2000)
+		ref_pll = 14318;
+	else
+		ref_pll = 12000;
+
+	denum = data & 0x1f;
+	num = (data & 0x3fe0) >> 5;
+	data = (data & 0xc000) >> 14;
+	switch (data) {
+	case 3:
+		div = 0x4;
+		break;
+	case 2:
+	case 1:
+		div = 0x2;
+		break;
+	default:
+		div = 0x1;
+		break;
+	}
+	ast->mclk = ref_pll * (num + 2) / (denum + 2) * (div * 1000);
+	return 0;
+}
+
+uint32_t ast_get_max_dclk(struct drm_device *dev, int bpp)
+{
+	struct ast_private *ast = dev->dev_private;
+	uint32_t dclk, jreg;
+	uint32_t dram_bus_width, mclk, dram_bandwidth, actual_dram_bandwidth, dram_efficency = 500;
+
+	dram_bus_width = ast->dram_bus_width;
+	mclk = ast->mclk;
+
+	if (ast->chip == AST2100 ||
+	    ast->chip == AST1100 ||
+	    ast->chip == AST2200 ||
+	    ast->chip == AST2150 ||
+	    ast->dram_bus_width == 16)
+		dram_efficency = 600;
+	else if (ast->chip == AST2300)
+		dram_efficency = 400;
+
+	dram_bandwidth = mclk * dram_bus_width * 2 / 8;
+	actual_dram_bandwidth = dram_bandwidth * dram_efficency / 1000;
+
+	if (ast->chip == AST1180)
+		dclk = actual_dram_bandwidth / ((bpp + 1) / 8);
+	else {
+		jreg = ast_get_index_reg_mask(ast, AST_IO_CRTC_PORT, 0xd0, 0xff);
+		if ((jreg & 0x08) && (ast->chip == AST2000))
+			dclk = actual_dram_bandwidth / ((bpp + 1 + 16) / 8);
+		else if ((jreg & 0x08) && (bpp == 8))
+			dclk = actual_dram_bandwidth / ((bpp + 1 + 24) / 8);
+		else
+			dclk = actual_dram_bandwidth / ((bpp + 1) / 8);
+	}
+
+	if (ast->chip == AST2100 ||
+	    ast->chip == AST2200 ||
+	    ast->chip == AST2300 ||
+	    ast->chip == AST1180) {
+		if (dclk > 200)
+			dclk = 200;
+	} else {
+		if (dclk > 165)
+			dclk = 165;
+	}
+
+	return dclk;
+}
+
+static void ast_user_framebuffer_destroy(struct drm_framebuffer *fb)
+{
+	struct ast_framebuffer *ast_fb = to_ast_framebuffer(fb);
+	if (ast_fb->obj)
+		drm_gem_object_unreference_unlocked(ast_fb->obj);
+
+	drm_framebuffer_cleanup(fb);
+	kfree(fb);
+}
+
+static int ast_user_framebuffer_create_handle(struct drm_framebuffer *fb,
+					      struct drm_file *file,
+					      unsigned int *handle)
+{
+	return -EINVAL;
+}
+
+static const struct drm_framebuffer_funcs ast_fb_funcs = {
+	.destroy = ast_user_framebuffer_destroy,
+	.create_handle = ast_user_framebuffer_create_handle,
+};
+
+
+int ast_framebuffer_init(struct drm_device *dev,
+			 struct ast_framebuffer *ast_fb,
+			 struct drm_mode_fb_cmd2 *mode_cmd,
+			 struct drm_gem_object *obj)
+{
+	int ret;
+
+	ret = drm_framebuffer_init(dev, &ast_fb->base, &ast_fb_funcs);
+	if (ret) {
+		DRM_ERROR("framebuffer init failed %d\n", ret);
+		return ret;
+	}
+	drm_helper_mode_fill_fb_struct(&ast_fb->base, mode_cmd);
+	ast_fb->obj = obj;
+	return 0;
+}
+
+static struct drm_framebuffer *
+ast_user_framebuffer_create(struct drm_device *dev,
+	       struct drm_file *filp,
+	       struct drm_mode_fb_cmd2 *mode_cmd)
+{
+	struct drm_gem_object *obj;
+	struct ast_framebuffer *ast_fb;
+	int ret;
+
+	obj = drm_gem_object_lookup(dev, filp, mode_cmd->handles[0]);
+	if (obj == NULL)
+		return ERR_PTR(-ENOENT);
+
+	ast_fb = kzalloc(sizeof(*ast_fb), GFP_KERNEL);
+	if (!ast_fb) {
+		drm_gem_object_unreference_unlocked(obj);
+		return ERR_PTR(-ENOMEM);
+	}
+
+	ret = ast_framebuffer_init(dev, ast_fb, mode_cmd, obj);
+	if (ret) {
+		drm_gem_object_unreference_unlocked(obj);
+		kfree(ast_fb);
+		return ERR_PTR(ret);
+	}
+	return &ast_fb->base;
+}
+
+static const struct drm_mode_config_funcs ast_mode_funcs = {
+	.fb_create = ast_user_framebuffer_create,
+};
+
+static u32 ast_get_vram_info(struct drm_device *dev)
+{
+	struct ast_private *ast = dev->dev_private;
+	u8 jreg;
+
+	ast_open_key(ast);
+
+	jreg = ast_get_index_reg_mask(ast, AST_IO_CRTC_PORT, 0xaa, 0xff);
+	switch (jreg & 3) {
+	case 0: return AST_VIDMEM_SIZE_8M;
+	case 1: return AST_VIDMEM_SIZE_16M;
+	case 2: return AST_VIDMEM_SIZE_32M;
+	case 3: return AST_VIDMEM_SIZE_64M;
+	}
+	return AST_VIDMEM_DEFAULT_SIZE;
+}
+
+int ast_driver_load(struct drm_device *dev, unsigned long flags)
+{
+	struct ast_private *ast;
+	int ret = 0;
+
+	ast = kzalloc(sizeof(struct ast_private), GFP_KERNEL);
+	if (!ast)
+		return -ENOMEM;
+
+	dev->dev_private = ast;
+	ast->dev = dev;
+
+	ast->regs = pci_iomap(dev->pdev, 1, 0);
+	if (!ast->regs) {
+		ret = -EIO;
+		goto out_free;
+	}
+	ast->ioregs = pci_iomap(dev->pdev, 2, 0);
+	if (!ast->ioregs) {
+		ret = -EIO;
+		goto out_free;
+	}
+
+	ast_detect_chip(dev);
+
+	if (ast->chip != AST1180) {
+		ast_get_dram_info(dev);
+		ast->vram_size = ast_get_vram_info(dev);
+		DRM_INFO("dram %d %d %d %08x\n", ast->mclk, ast->dram_type, ast->dram_bus_width, ast->vram_size);
+	}
+
+	ret = ast_mm_init(ast);
+	if (ret)
+		goto out_free;
+
+	drm_mode_config_init(dev);
+
+	dev->mode_config.funcs = (void *)&ast_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;
+
+	if (ast->chip == AST2100 ||
+	    ast->chip == AST2200 ||
+	    ast->chip == AST2300 ||
+	    ast->chip == AST1180) {
+		dev->mode_config.max_width = 1920;
+		dev->mode_config.max_height = 2048;
+	} else {
+		dev->mode_config.max_width = 1600;
+		dev->mode_config.max_height = 1200;
+	}
+
+	ret = ast_mode_init(dev);
+	if (ret)
+		goto out_free;
+
+	ret = ast_fbdev_init(dev);
+	if (ret)
+		goto out_free;
+
+	return 0;
+out_free:
+	kfree(ast);
+	dev->dev_private = NULL;
+	return ret;
+}
+
+int ast_driver_unload(struct drm_device *dev)
+{
+	struct ast_private *ast = dev->dev_private;
+
+	ast_mode_fini(dev);
+	ast_fbdev_fini(dev);
+	drm_mode_config_cleanup(dev);
+
+	ast_mm_fini(ast);
+	pci_iounmap(dev->pdev, ast->ioregs);
+	pci_iounmap(dev->pdev, ast->regs);
+	kfree(ast);
+	return 0;
+}
+
+int ast_gem_create(struct drm_device *dev,
+		   u32 size, bool iskernel,
+		   struct drm_gem_object **obj)
+{
+	struct ast_bo *astbo;
+	int ret;
+
+	*obj = NULL;
+
+	size = roundup(size, PAGE_SIZE);
+	if (size == 0)
+		return -EINVAL;
+
+	ret = ast_bo_create(dev, size, 0, 0, &astbo);
+	if (ret) {
+		if (ret != -ERESTARTSYS)
+			DRM_ERROR("failed to allocate GEM object\n");
+		return ret;
+	}
+	*obj = &astbo->gem;
+	return 0;
+}
+
+int ast_dumb_create(struct drm_file *file,
+		    struct drm_device *dev,
+		    struct drm_mode_create_dumb *args)
+{
+	int ret;
+	struct drm_gem_object *gobj;
+	u32 handle;
+
+	args->pitch = args->width * ((args->bpp + 7) / 8);
+	args->size = args->pitch * args->height;
+
+	ret = ast_gem_create(dev, args->size, false,
+			     &gobj);
+	if (ret)
+		return ret;
+
+	ret = drm_gem_handle_create(file, gobj, &handle);
+	drm_gem_object_unreference_unlocked(gobj);
+	if (ret)
+		return ret;
+
+	args->handle = handle;
+	return 0;
+}
+
+int ast_dumb_destroy(struct drm_file *file,
+		     struct drm_device *dev,
+		     uint32_t handle)
+{
+	return drm_gem_handle_delete(file, handle);
+}
+
+int ast_gem_init_object(struct drm_gem_object *obj)
+{
+	BUG();
+	return 0;
+}
+
+void ast_bo_unref(struct ast_bo **bo)
+{
+	struct ttm_buffer_object *tbo;
+
+	if ((*bo) == NULL)
+		return;
+
+	tbo = &((*bo)->bo);
+	ttm_bo_unref(&tbo);
+	if (tbo == NULL)
+		*bo = NULL;
+
+}
+void ast_gem_free_object(struct drm_gem_object *obj)
+{
+	struct ast_bo *ast_bo = gem_to_ast_bo(obj);
+
+	if (!ast_bo)
+		return;
+	ast_bo_unref(&ast_bo);
+}
+
+
+static inline u64 ast_bo_mmap_offset(struct ast_bo *bo)
+{
+	return bo->bo.addr_space_offset;
+}
+int
+ast_dumb_mmap_offset(struct drm_file *file,
+		     struct drm_device *dev,
+		     uint32_t handle,
+		     uint64_t *offset)
+{
+	struct drm_gem_object *obj;
+	int ret;
+	struct ast_bo *bo;
+
+	mutex_lock(&dev->struct_mutex);
+	obj = drm_gem_object_lookup(dev, file, handle);
+	if (obj == NULL) {
+		ret = -ENOENT;
+		goto out_unlock;
+	}
+
+	bo = gem_to_ast_bo(obj);
+	*offset = ast_bo_mmap_offset(bo);
+
+	drm_gem_object_unreference(obj);
+	ret = 0;
+out_unlock:
+	mutex_unlock(&dev->struct_mutex);
+	return ret;
+
+}
+
diff --git a/drivers/gpu/drm/ast/ast_mode.c b/drivers/gpu/drm/ast/ast_mode.c
new file mode 100644
index 00000000000..65f9d231af1
--- /dev/null
+++ b/drivers/gpu/drm/ast/ast_mode.c
@@ -0,0 +1,1160 @@
+/*
+ * Copyright 2012 Red Hat Inc.
+ * Parts based on xf86-video-ast
+ * Copyright (c) 2005 ASPEED Technology 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, 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: Dave Airlie <airlied@redhat.com>
+ */
+#include <linux/export.h>
+#include "drmP.h"
+#include "drm_crtc.h"
+#include "drm_crtc_helper.h"
+#include "ast_drv.h"
+
+#include "ast_tables.h"
+
+static struct ast_i2c_chan *ast_i2c_create(struct drm_device *dev);
+static void ast_i2c_destroy(struct ast_i2c_chan *i2c);
+static int ast_cursor_set(struct drm_crtc *crtc,
+			  struct drm_file *file_priv,
+			  uint32_t handle,
+			  uint32_t width,
+			  uint32_t height);
+static int ast_cursor_move(struct drm_crtc *crtc,
+			   int x, int y);
+
+static inline void ast_load_palette_index(struct ast_private *ast,
+				     u8 index, u8 red, u8 green,
+				     u8 blue)
+{
+	ast_io_write8(ast, AST_IO_DAC_INDEX_WRITE, index);
+	ast_io_read8(ast, AST_IO_SEQ_PORT);
+	ast_io_write8(ast, AST_IO_DAC_DATA, red);
+	ast_io_read8(ast, AST_IO_SEQ_PORT);
+	ast_io_write8(ast, AST_IO_DAC_DATA, green);
+	ast_io_read8(ast, AST_IO_SEQ_PORT);
+	ast_io_write8(ast, AST_IO_DAC_DATA, blue);
+	ast_io_read8(ast, AST_IO_SEQ_PORT);
+}
+
+static void ast_crtc_load_lut(struct drm_crtc *crtc)
+{
+	struct ast_private *ast = crtc->dev->dev_private;
+	struct ast_crtc *ast_crtc = to_ast_crtc(crtc);
+	int i;
+
+	if (!crtc->enabled)
+		return;
+
+	for (i = 0; i < 256; i++)
+		ast_load_palette_index(ast, i, ast_crtc->lut_r[i],
+				       ast_crtc->lut_g[i], ast_crtc->lut_b[i]);
+}
+
+static bool ast_get_vbios_mode_info(struct drm_crtc *crtc, struct drm_display_mode *mode,
+				    struct drm_display_mode *adjusted_mode,
+				    struct ast_vbios_mode_info *vbios_mode)
+{
+	struct ast_private *ast = crtc->dev->dev_private;
+	u32 refresh_rate_index = 0, mode_id, color_index, refresh_rate;
+	u32 hborder, vborder;
+
+	switch (crtc->fb->bits_per_pixel) {
+	case 8:
+		vbios_mode->std_table = &vbios_stdtable[VGAModeIndex];
+		color_index = VGAModeIndex - 1;
+		break;
+	case 16:
+		vbios_mode->std_table = &vbios_stdtable[HiCModeIndex];
+		color_index = HiCModeIndex;
+		break;
+	case 24:
+	case 32:
+		vbios_mode->std_table = &vbios_stdtable[TrueCModeIndex];
+		color_index = TrueCModeIndex;
+		break;
+	default:
+		return false;
+	}
+
+	switch (crtc->mode.crtc_hdisplay) {
+	case 640:
+		vbios_mode->enh_table = &res_640x480[refresh_rate_index];
+		break;
+	case 800:
+		vbios_mode->enh_table = &res_800x600[refresh_rate_index];
+		break;
+	case 1024:
+		vbios_mode->enh_table = &res_1024x768[refresh_rate_index];
+		break;
+	case 1280:
+		if (crtc->mode.crtc_vdisplay == 800)
+			vbios_mode->enh_table = &res_1280x800[refresh_rate_index];
+		else
+			vbios_mode->enh_table = &res_1280x1024[refresh_rate_index];
+		break;
+	case 1440:
+		vbios_mode->enh_table = &res_1440x900[refresh_rate_index];
+		break;
+	case 1600:
+		vbios_mode->enh_table = &res_1600x1200[refresh_rate_index];
+		break;
+	case 1680:
+		vbios_mode->enh_table = &res_1680x1050[refresh_rate_index];
+		break;
+	case 1920:
+		if (crtc->mode.crtc_vdisplay == 1080)
+			vbios_mode->enh_table = &res_1920x1080[refresh_rate_index];
+		else
+			vbios_mode->enh_table = &res_1920x1200[refresh_rate_index];
+		break;
+	default:
+		return false;
+	}
+
+	refresh_rate = drm_mode_vrefresh(mode);
+	while (vbios_mode->enh_table->refresh_rate < refresh_rate) {
+		vbios_mode->enh_table++;
+		if ((vbios_mode->enh_table->refresh_rate > refresh_rate) ||
+		    (vbios_mode->enh_table->refresh_rate == 0xff)) {
+			vbios_mode->enh_table--;
+			break;
+		}
+	}
+
+	hborder = (vbios_mode->enh_table->flags & HBorder) ? 8 : 0;
+	vborder = (vbios_mode->enh_table->flags & VBorder) ? 8 : 0;
+
+	adjusted_mode->crtc_htotal = vbios_mode->enh_table->ht;
+	adjusted_mode->crtc_hblank_start = vbios_mode->enh_table->hde + hborder;
+	adjusted_mode->crtc_hblank_end = vbios_mode->enh_table->ht - hborder;
+	adjusted_mode->crtc_hsync_start = vbios_mode->enh_table->hde + hborder +
+		vbios_mode->enh_table->hfp;
+	adjusted_mode->crtc_hsync_end = (vbios_mode->enh_table->hde + hborder +
+					 vbios_mode->enh_table->hfp +
+					 vbios_mode->enh_table->hsync);
+
+	adjusted_mode->crtc_vtotal = vbios_mode->enh_table->vt;
+	adjusted_mode->crtc_vblank_start = vbios_mode->enh_table->vde + vborder;
+	adjusted_mode->crtc_vblank_end = vbios_mode->enh_table->vt - vborder;
+	adjusted_mode->crtc_vsync_start = vbios_mode->enh_table->vde + vborder +
+		vbios_mode->enh_table->vfp;
+	adjusted_mode->crtc_vsync_end = (vbios_mode->enh_table->vde + vborder +
+					 vbios_mode->enh_table->vfp +
+					 vbios_mode->enh_table->vsync);
+
+	refresh_rate_index = vbios_mode->enh_table->refresh_rate_index;
+	mode_id = vbios_mode->enh_table->mode_id;
+
+	if (ast->chip == AST1180) {
+		/* TODO 1180 */
+	} else {
+		ast_set_index_reg(ast, AST_IO_CRTC_PORT, 0x8c, (u8)((color_index & 0xf) << 4));
+		ast_set_index_reg(ast, AST_IO_CRTC_PORT, 0x8d, refresh_rate_index & 0xff);
+		ast_set_index_reg(ast, AST_IO_CRTC_PORT, 0x8e, mode_id & 0xff);
+
+		ast_set_index_reg(ast, AST_IO_CRTC_PORT, 0x91, 0xa8);
+		ast_set_index_reg(ast, AST_IO_CRTC_PORT, 0x92, crtc->fb->bits_per_pixel);
+		ast_set_index_reg(ast, AST_IO_CRTC_PORT, 0x93, adjusted_mode->clock / 1000);
+		ast_set_index_reg(ast, AST_IO_CRTC_PORT, 0x94, adjusted_mode->crtc_hdisplay);
+		ast_set_index_reg(ast, AST_IO_CRTC_PORT, 0x95, adjusted_mode->crtc_hdisplay >> 8);
+
+		ast_set_index_reg(ast, AST_IO_CRTC_PORT, 0x96, adjusted_mode->crtc_vdisplay);
+		ast_set_index_reg(ast, AST_IO_CRTC_PORT, 0x97, adjusted_mode->crtc_vdisplay >> 8);
+	}
+
+	return true;
+
+
+}
+static void ast_set_std_reg(struct drm_crtc *crtc, struct drm_display_mode *mode,
+			    struct ast_vbios_mode_info *vbios_mode)
+{
+	struct ast_private *ast = crtc->dev->dev_private;
+	struct ast_vbios_stdtable *stdtable;
+	u32 i;
+	u8 jreg;
+
+	stdtable = vbios_mode->std_table;
+
+	jreg = stdtable->misc;
+	ast_io_write8(ast, AST_IO_MISC_PORT_WRITE, jreg);
+
+	/* Set SEQ */
+	ast_set_index_reg(ast, AST_IO_SEQ_PORT, 0x00, 0x03);
+	for (i = 0; i < 4; i++) {
+		jreg = stdtable->seq[i];
+		if (!i)
+			jreg |= 0x20;
+		ast_set_index_reg(ast, AST_IO_SEQ_PORT, (i + 1) , jreg);
+	}
+
+	/* Set CRTC */
+	ast_set_index_reg_mask(ast, AST_IO_CRTC_PORT, 0x11, 0x7f, 0x00);
+	for (i = 0; i < 25; i++)
+		ast_set_index_reg(ast, AST_IO_CRTC_PORT, i, stdtable->crtc[i]);
+
+	/* set AR */
+	jreg = ast_io_read8(ast, AST_IO_INPUT_STATUS1_READ);
+	for (i = 0; i < 20; i++) {
+		jreg = stdtable->ar[i];
+		ast_io_write8(ast, AST_IO_AR_PORT_WRITE, (u8)i);
+		ast_io_write8(ast, AST_IO_AR_PORT_WRITE, jreg);
+	}
+	ast_io_write8(ast, AST_IO_AR_PORT_WRITE, 0x14);
+	ast_io_write8(ast, AST_IO_AR_PORT_WRITE, 0x00);
+
+	jreg = ast_io_read8(ast, AST_IO_INPUT_STATUS1_READ);
+	ast_io_write8(ast, AST_IO_AR_PORT_WRITE, 0x20);
+
+	/* Set GR */
+	for (i = 0; i < 9; i++)
+		ast_set_index_reg(ast, AST_IO_GR_PORT, i, stdtable->gr[i]);
+}
+
+static void ast_set_crtc_reg(struct drm_crtc *crtc, struct drm_display_mode *mode,
+			     struct ast_vbios_mode_info *vbios_mode)
+{
+	struct ast_private *ast = crtc->dev->dev_private;
+	u8 jreg05 = 0, jreg07 = 0, jreg09 = 0, jregAC = 0, jregAD = 0, jregAE = 0;
+	u16 temp;
+
+	ast_set_index_reg_mask(ast, AST_IO_CRTC_PORT, 0x11, 0x7f, 0x00);
+
+	temp = (mode->crtc_htotal >> 3) - 5;
+	if (temp & 0x100)
+		jregAC |= 0x01; /* HT D[8] */
+	ast_set_index_reg_mask(ast, AST_IO_CRTC_PORT, 0x00, 0x00, temp);
+
+	temp = (mode->crtc_hdisplay >> 3) - 1;
+	if (temp & 0x100)
+		jregAC |= 0x04; /* HDE D[8] */
+	ast_set_index_reg_mask(ast, AST_IO_CRTC_PORT, 0x01, 0x00, temp);
+
+	temp = (mode->crtc_hblank_start >> 3) - 1;
+	if (temp & 0x100)
+		jregAC |= 0x10; /* HBS D[8] */
+	ast_set_index_reg_mask(ast, AST_IO_CRTC_PORT, 0x02, 0x00, temp);
+
+	temp = ((mode->crtc_hblank_end >> 3) - 1) & 0x7f;
+	if (temp & 0x20)
+		jreg05 |= 0x80;  /* HBE D[5] */
+	if (temp & 0x40)
+		jregAD |= 0x01;  /* HBE D[5] */
+	ast_set_index_reg_mask(ast, AST_IO_CRTC_PORT, 0x03, 0xE0, (temp & 0x1f));
+
+	temp = (mode->crtc_hsync_start >> 3) - 1;
+	if (temp & 0x100)
+		jregAC |= 0x40; /* HRS D[5] */
+	ast_set_index_reg_mask(ast, AST_IO_CRTC_PORT, 0x04, 0x00, temp);
+
+	temp = ((mode->crtc_hsync_end >> 3) - 1) & 0x3f;
+	if (temp & 0x20)
+		jregAD |= 0x04; /* HRE D[5] */
+	ast_set_index_reg_mask(ast, AST_IO_CRTC_PORT, 0x05, 0x60, (u8)((temp & 0x1f) | jreg05));
+
+	ast_set_index_reg_mask(ast, AST_IO_CRTC_PORT, 0xAC, 0x00, jregAC);
+	ast_set_index_reg_mask(ast, AST_IO_CRTC_PORT, 0xAD, 0x00, jregAD);
+
+	/* vert timings */
+	temp = (mode->crtc_vtotal) - 2;
+	if (temp & 0x100)
+		jreg07 |= 0x01;
+	if (temp & 0x200)
+		jreg07 |= 0x20;
+	if (temp & 0x400)
+		jregAE |= 0x01;
+	ast_set_index_reg_mask(ast, AST_IO_CRTC_PORT, 0x06, 0x00, temp);
+
+	temp = (mode->crtc_vsync_start) - 1;
+	if (temp & 0x100)
+		jreg07 |= 0x04;
+	if (temp & 0x200)
+		jreg07 |= 0x80;
+	if (temp & 0x400)
+		jregAE |= 0x08;
+	ast_set_index_reg_mask(ast, AST_IO_CRTC_PORT, 0x10, 0x00, temp);
+
+	temp = (mode->crtc_vsync_end - 1) & 0x3f;
+	if (temp & 0x10)
+		jregAE |= 0x20;
+	if (temp & 0x20)
+		jregAE |= 0x40;
+	ast_set_index_reg_mask(ast, AST_IO_CRTC_PORT, 0x11, 0x70, temp & 0xf);
+
+	temp = mode->crtc_vdisplay - 1;
+	if (temp & 0x100)
+		jreg07 |= 0x02;
+	if (temp & 0x200)
+		jreg07 |= 0x40;
+	if (temp & 0x400)
+		jregAE |= 0x02;
+	ast_set_index_reg_mask(ast, AST_IO_CRTC_PORT, 0x12, 0x00, temp);
+
+	temp = mode->crtc_vblank_start - 1;
+	if (temp & 0x100)
+		jreg07 |= 0x08;
+	if (temp & 0x200)
+		jreg09 |= 0x20;
+	if (temp & 0x400)
+		jregAE |= 0x04;
+	ast_set_index_reg_mask(ast, AST_IO_CRTC_PORT, 0x15, 0x00, temp);
+
+	temp = mode->crtc_vblank_end - 1;
+	if (temp & 0x100)
+		jregAE |= 0x10;
+	ast_set_index_reg_mask(ast, AST_IO_CRTC_PORT, 0x16, 0x00, temp);
+
+	ast_set_index_reg_mask(ast, AST_IO_CRTC_PORT, 0x07, 0x00, jreg07);
+	ast_set_index_reg_mask(ast, AST_IO_CRTC_PORT, 0x09, 0xdf, jreg09);
+	ast_set_index_reg_mask(ast, AST_IO_CRTC_PORT, 0xAE, 0x00, (jregAE | 0x80));
+
+	ast_set_index_reg_mask(ast, AST_IO_CRTC_PORT, 0x11, 0x7f, 0x80);
+}
+
+static void ast_set_offset_reg(struct drm_crtc *crtc)
+{
+	struct ast_private *ast = crtc->dev->dev_private;
+
+	u16 offset;
+
+	offset = crtc->fb->pitches[0] >> 3;
+	ast_set_index_reg(ast, AST_IO_CRTC_PORT, 0x13, (offset & 0xff));
+	ast_set_index_reg(ast, AST_IO_CRTC_PORT, 0xb0, (offset >> 8) & 0x3f);
+}
+
+static void ast_set_dclk_reg(struct drm_device *dev, struct drm_display_mode *mode,
+			     struct ast_vbios_mode_info *vbios_mode)
+{
+	struct ast_private *ast = dev->dev_private;
+	struct ast_vbios_dclk_info *clk_info;
+
+	clk_info = &dclk_table[vbios_mode->enh_table->dclk_index];
+
+	ast_set_index_reg_mask(ast, AST_IO_CRTC_PORT, 0xc0, 0x00, clk_info->param1);
+	ast_set_index_reg_mask(ast, AST_IO_CRTC_PORT, 0xc1, 0x00, clk_info->param2);
+	ast_set_index_reg_mask(ast, AST_IO_CRTC_PORT, 0xbb, 0x0f,
+			       (clk_info->param3 & 0x80) | ((clk_info->param3 & 0x3) << 4));
+}
+
+static void ast_set_ext_reg(struct drm_crtc *crtc, struct drm_display_mode *mode,
+			     struct ast_vbios_mode_info *vbios_mode)
+{
+	struct ast_private *ast = crtc->dev->dev_private;
+	u8 jregA0 = 0, jregA3 = 0, jregA8 = 0;
+
+	switch (crtc->fb->bits_per_pixel) {
+	case 8:
+		jregA0 = 0x70;
+		jregA3 = 0x01;
+		jregA8 = 0x00;
+		break;
+	case 15:
+	case 16:
+		jregA0 = 0x70;
+		jregA3 = 0x04;
+		jregA8 = 0x02;
+		break;
+	case 32:
+		jregA0 = 0x70;
+		jregA3 = 0x08;
+		jregA8 = 0x02;
+		break;
+	}
+
+	ast_set_index_reg_mask(ast, AST_IO_CRTC_PORT, 0xa0, 0x8f, jregA0);
+	ast_set_index_reg_mask(ast, AST_IO_CRTC_PORT, 0xa3, 0xf0, jregA3);
+	ast_set_index_reg_mask(ast, AST_IO_CRTC_PORT, 0xa8, 0xfd, jregA8);
+
+	/* Set Threshold */
+	if (ast->chip == AST2300) {
+		ast_set_index_reg(ast, AST_IO_CRTC_PORT, 0xa7, 0x78);
+		ast_set_index_reg(ast, AST_IO_CRTC_PORT, 0xa6, 0x60);
+	} else if (ast->chip == AST2100 ||
+		   ast->chip == AST1100 ||
+		   ast->chip == AST2200 ||
+		   ast->chip == AST2150) {
+		ast_set_index_reg(ast, AST_IO_CRTC_PORT, 0xa7, 0x3f);
+		ast_set_index_reg(ast, AST_IO_CRTC_PORT, 0xa6, 0x2f);
+	} else {
+		ast_set_index_reg(ast, AST_IO_CRTC_PORT, 0xa7, 0x2f);
+		ast_set_index_reg(ast, AST_IO_CRTC_PORT, 0xa6, 0x1f);
+	}
+}
+
+void ast_set_sync_reg(struct drm_device *dev, struct drm_display_mode *mode,
+		      struct ast_vbios_mode_info *vbios_mode)
+{
+	struct ast_private *ast = dev->dev_private;
+	u8 jreg;
+
+	jreg = ast_io_read8(ast, AST_IO_MISC_PORT_READ);
+	jreg |= (vbios_mode->enh_table->flags & SyncNN);
+	ast_io_write8(ast, AST_IO_MISC_PORT_WRITE, jreg);
+}
+
+bool ast_set_dac_reg(struct drm_crtc *crtc, struct drm_display_mode *mode,
+		     struct ast_vbios_mode_info *vbios_mode)
+{
+	switch (crtc->fb->bits_per_pixel) {
+	case 8:
+		break;
+	default:
+		return false;
+	}
+	return true;
+}
+
+void ast_set_start_address_crt1(struct drm_crtc *crtc, unsigned offset)
+{
+	struct ast_private *ast = crtc->dev->dev_private;
+	u32 addr;
+
+	addr = offset >> 2;
+	ast_set_index_reg(ast, AST_IO_CRTC_PORT, 0x0d, (u8)(addr & 0xff));
+	ast_set_index_reg(ast, AST_IO_CRTC_PORT, 0x0c, (u8)((addr >> 8) & 0xff));
+	ast_set_index_reg(ast, AST_IO_CRTC_PORT, 0xaf, (u8)((addr >> 16) & 0xff));
+
+}
+
+static void ast_crtc_dpms(struct drm_crtc *crtc, int mode)
+{
+	struct ast_private *ast = crtc->dev->dev_private;
+
+	if (ast->chip == AST1180)
+		return;
+
+	switch (mode) {
+	case DRM_MODE_DPMS_ON:
+	case DRM_MODE_DPMS_STANDBY:
+	case DRM_MODE_DPMS_SUSPEND:
+		ast_set_index_reg_mask(ast, AST_IO_SEQ_PORT, 0x1, 0xdf, 0);
+		ast_crtc_load_lut(crtc);
+		break;
+	case DRM_MODE_DPMS_OFF:
+		ast_set_index_reg_mask(ast, AST_IO_SEQ_PORT, 0x1, 0xdf, 0x20);
+		break;
+	}
+}
+
+static bool ast_crtc_mode_fixup(struct drm_crtc *crtc,
+				  struct drm_display_mode *mode,
+				  struct drm_display_mode *adjusted_mode)
+{
+	return true;
+}
+
+/* ast is different - we will force move buffers out of VRAM */
+static int ast_crtc_do_set_base(struct drm_crtc *crtc,
+				struct drm_framebuffer *fb,
+				int x, int y, int atomic)
+{
+	struct ast_private *ast = crtc->dev->dev_private;
+	struct drm_gem_object *obj;
+	struct ast_framebuffer *ast_fb;
+	struct ast_bo *bo;
+	int ret;
+	u64 gpu_addr;
+
+	/* push the previous fb to system ram */
+	if (!atomic && fb) {
+		ast_fb = to_ast_framebuffer(fb);
+		obj = ast_fb->obj;
+		bo = gem_to_ast_bo(obj);
+		ret = ast_bo_reserve(bo, false);
+		if (ret)
+			return ret;
+		ast_bo_push_sysram(bo);
+		ast_bo_unreserve(bo);
+	}
+
+	ast_fb = to_ast_framebuffer(crtc->fb);
+	obj = ast_fb->obj;
+	bo = gem_to_ast_bo(obj);
+
+	ret = ast_bo_reserve(bo, false);
+	if (ret)
+		return ret;
+
+	ret = ast_bo_pin(bo, TTM_PL_FLAG_VRAM, &gpu_addr);
+	if (ret) {
+		ast_bo_unreserve(bo);
+		return ret;
+	}
+
+	if (&ast->fbdev->afb == ast_fb) {
+		/* if pushing console in kmap it */
+		ret = ttm_bo_kmap(&bo->bo, 0, bo->bo.num_pages, &bo->kmap);
+		if (ret)
+			DRM_ERROR("failed to kmap fbcon\n");
+	}
+	ast_bo_unreserve(bo);
+
+	ast_set_start_address_crt1(crtc, (u32)gpu_addr);
+
+	return 0;
+}
+
+static int ast_crtc_mode_set_base(struct drm_crtc *crtc, int x, int y,
+			     struct drm_framebuffer *old_fb)
+{
+	return ast_crtc_do_set_base(crtc, old_fb, x, y, 0);
+}
+
+static int ast_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 ast_private *ast = crtc->dev->dev_private;
+	struct ast_vbios_mode_info vbios_mode;
+	bool ret;
+	if (ast->chip == AST1180) {
+		DRM_ERROR("AST 1180 modesetting not supported\n");
+		return -EINVAL;
+	}
+
+	ret = ast_get_vbios_mode_info(crtc, mode, adjusted_mode, &vbios_mode);
+	if (ret == false)
+		return -EINVAL;
+	ast_open_key(ast);
+
+	ast_set_index_reg_mask(ast, AST_IO_CRTC_PORT, 0xa1, 0xff, 0x04);
+
+	ast_set_std_reg(crtc, adjusted_mode, &vbios_mode);
+	ast_set_crtc_reg(crtc, adjusted_mode, &vbios_mode);
+	ast_set_offset_reg(crtc);
+	ast_set_dclk_reg(dev, adjusted_mode, &vbios_mode);
+	ast_set_ext_reg(crtc, adjusted_mode, &vbios_mode);
+	ast_set_sync_reg(dev, adjusted_mode, &vbios_mode);
+	ast_set_dac_reg(crtc, adjusted_mode, &vbios_mode);
+
+	ast_crtc_mode_set_base(crtc, x, y, old_fb);
+
+	return 0;
+}
+
+static void ast_crtc_disable(struct drm_crtc *crtc)
+{
+
+}
+
+static void ast_crtc_prepare(struct drm_crtc *crtc)
+{
+
+}
+
+static void ast_crtc_commit(struct drm_crtc *crtc)
+{
+	struct ast_private *ast = crtc->dev->dev_private;
+	ast_set_index_reg_mask(ast, AST_IO_SEQ_PORT, 0x1, 0xdf, 0);
+}
+
+
+static const struct drm_crtc_helper_funcs ast_crtc_helper_funcs = {
+	.dpms = ast_crtc_dpms,
+	.mode_fixup = ast_crtc_mode_fixup,
+	.mode_set = ast_crtc_mode_set,
+	.mode_set_base = ast_crtc_mode_set_base,
+	.disable = ast_crtc_disable,
+	.load_lut = ast_crtc_load_lut,
+	.disable = ast_crtc_disable,
+	.prepare = ast_crtc_prepare,
+	.commit = ast_crtc_commit,
+
+};
+
+static void ast_crtc_reset(struct drm_crtc *crtc)
+{
+
+}
+
+static void ast_crtc_gamma_set(struct drm_crtc *crtc, u16 *red, u16 *green,
+				 u16 *blue, uint32_t start, uint32_t size)
+{
+	struct ast_crtc *ast_crtc = to_ast_crtc(crtc);
+	int end = (start + size > 256) ? 256 : start + size, i;
+
+	/* userspace palettes are always correct as is */
+	for (i = start; i < end; i++) {
+		ast_crtc->lut_r[i] = red[i] >> 8;
+		ast_crtc->lut_g[i] = green[i] >> 8;
+		ast_crtc->lut_b[i] = blue[i] >> 8;
+	}
+	ast_crtc_load_lut(crtc);
+}
+
+
+static void ast_crtc_destroy(struct drm_crtc *crtc)
+{
+	drm_crtc_cleanup(crtc);
+	kfree(crtc);
+}
+
+static const struct drm_crtc_funcs ast_crtc_funcs = {
+	.cursor_set = ast_cursor_set,
+	.cursor_move = ast_cursor_move,
+	.reset = ast_crtc_reset,
+	.set_config = drm_crtc_helper_set_config,
+	.gamma_set = ast_crtc_gamma_set,
+	.destroy = ast_crtc_destroy,
+};
+
+int ast_crtc_init(struct drm_device *dev)
+{
+	struct ast_crtc *crtc;
+	int i;
+
+	crtc = kzalloc(sizeof(struct ast_crtc), GFP_KERNEL);
+	if (!crtc)
+		return -ENOMEM;
+
+	drm_crtc_init(dev, &crtc->base, &ast_crtc_funcs);
+	drm_mode_crtc_set_gamma_size(&crtc->base, 256);
+	drm_crtc_helper_add(&crtc->base, &ast_crtc_helper_funcs);
+
+	for (i = 0; i < 256; i++) {
+		crtc->lut_r[i] = i;
+		crtc->lut_g[i] = i;
+		crtc->lut_b[i] = i;
+	}
+	return 0;
+}
+
+static void ast_encoder_destroy(struct drm_encoder *encoder)
+{
+	drm_encoder_cleanup(encoder);
+	kfree(encoder);
+}
+
+
+static struct drm_encoder *ast_best_single_encoder(struct drm_connector *connector)
+{
+	int enc_id = connector->encoder_ids[0];
+	struct drm_mode_object *obj;
+	struct drm_encoder *encoder;
+
+	/* pick the encoder ids */
+	if (enc_id) {
+		obj = drm_mode_object_find(connector->dev, enc_id, DRM_MODE_OBJECT_ENCODER);
+		if (!obj)
+			return NULL;
+		encoder = obj_to_encoder(obj);
+		return encoder;
+	}
+	return NULL;
+}
+
+
+static const struct drm_encoder_funcs ast_enc_funcs = {
+	.destroy = ast_encoder_destroy,
+};
+
+static void ast_encoder_dpms(struct drm_encoder *encoder, int mode)
+{
+
+}
+
+static bool ast_mode_fixup(struct drm_encoder *encoder,
+			   struct drm_display_mode *mode,
+			   struct drm_display_mode *adjusted_mode)
+{
+	return true;
+}
+
+static void ast_encoder_mode_set(struct drm_encoder *encoder,
+			       struct drm_display_mode *mode,
+			       struct drm_display_mode *adjusted_mode)
+{
+}
+
+static void ast_encoder_prepare(struct drm_encoder *encoder)
+{
+
+}
+
+static void ast_encoder_commit(struct drm_encoder *encoder)
+{
+
+}
+
+
+static const struct drm_encoder_helper_funcs ast_enc_helper_funcs = {
+	.dpms = ast_encoder_dpms,
+	.mode_fixup = ast_mode_fixup,
+	.prepare = ast_encoder_prepare,
+	.commit = ast_encoder_commit,
+	.mode_set = ast_encoder_mode_set,
+};
+
+int ast_encoder_init(struct drm_device *dev)
+{
+	struct ast_encoder *ast_encoder;
+
+	ast_encoder = kzalloc(sizeof(struct ast_encoder), GFP_KERNEL);
+	if (!ast_encoder)
+		return -ENOMEM;
+
+	drm_encoder_init(dev, &ast_encoder->base, &ast_enc_funcs,
+			 DRM_MODE_ENCODER_DAC);
+	drm_encoder_helper_add(&ast_encoder->base, &ast_enc_helper_funcs);
+
+	ast_encoder->base.possible_crtcs = 1;
+	return 0;
+}
+
+static int ast_get_modes(struct drm_connector *connector)
+{
+	struct ast_connector *ast_connector = to_ast_connector(connector);
+	struct edid *edid;
+	int ret;
+
+	edid = drm_get_edid(connector, &ast_connector->i2c->adapter);
+	if (edid) {
+		drm_mode_connector_update_edid_property(&ast_connector->base, edid);
+		ret = drm_add_edid_modes(connector, edid);
+		return ret;
+	} else
+		drm_mode_connector_update_edid_property(&ast_connector->base, NULL);
+	return 0;
+}
+
+static int ast_mode_valid(struct drm_connector *connector,
+			  struct drm_display_mode *mode)
+{
+	return MODE_OK;
+}
+
+static void ast_connector_destroy(struct drm_connector *connector)
+{
+	struct ast_connector *ast_connector = to_ast_connector(connector);
+	ast_i2c_destroy(ast_connector->i2c);
+	drm_sysfs_connector_remove(connector);
+	drm_connector_cleanup(connector);
+	kfree(connector);
+}
+
+static enum drm_connector_status
+ast_connector_detect(struct drm_connector *connector, bool force)
+{
+	return connector_status_connected;
+}
+
+static const struct drm_connector_helper_funcs ast_connector_helper_funcs = {
+	.mode_valid = ast_mode_valid,
+	.get_modes = ast_get_modes,
+	.best_encoder = ast_best_single_encoder,
+};
+
+static const struct drm_connector_funcs ast_connector_funcs = {
+	.dpms = drm_helper_connector_dpms,
+	.detect = ast_connector_detect,
+	.fill_modes = drm_helper_probe_single_connector_modes,
+	.destroy = ast_connector_destroy,
+};
+
+int ast_connector_init(struct drm_device *dev)
+{
+	struct ast_connector *ast_connector;
+	struct drm_connector *connector;
+	struct drm_encoder *encoder;
+
+	ast_connector = kzalloc(sizeof(struct ast_connector), GFP_KERNEL);
+	if (!ast_connector)
+		return -ENOMEM;
+
+	connector = &ast_connector->base;
+	drm_connector_init(dev, connector, &ast_connector_funcs, DRM_MODE_CONNECTOR_VGA);
+
+	drm_connector_helper_add(connector, &ast_connector_helper_funcs);
+
+	connector->interlace_allowed = 0;
+	connector->doublescan_allowed = 0;
+
+	drm_sysfs_connector_add(connector);
+
+	connector->polled = DRM_CONNECTOR_POLL_CONNECT;
+
+	encoder = list_first_entry(&dev->mode_config.encoder_list, struct drm_encoder, head);
+	drm_mode_connector_attach_encoder(connector, encoder);
+
+	ast_connector->i2c = ast_i2c_create(dev);
+	if (!ast_connector->i2c)
+		DRM_ERROR("failed to add ddc bus for connector\n");
+
+	return 0;
+}
+
+/* allocate cursor cache and pin at start of VRAM */
+int ast_cursor_init(struct drm_device *dev)
+{
+	struct ast_private *ast = dev->dev_private;
+	int size;
+	int ret;
+	struct drm_gem_object *obj;
+	struct ast_bo *bo;
+	uint64_t gpu_addr;
+
+	size = (AST_HWC_SIZE + AST_HWC_SIGNATURE_SIZE) * AST_DEFAULT_HWC_NUM;
+
+	ret = ast_gem_create(dev, size, true, &obj);
+	if (ret)
+		return ret;
+	bo = gem_to_ast_bo(obj);
+	ret = ast_bo_reserve(bo, false);
+	if (unlikely(ret != 0))
+		goto fail;
+
+	ret = ast_bo_pin(bo, TTM_PL_FLAG_VRAM, &gpu_addr);
+	ast_bo_unreserve(bo);
+	if (ret)
+		goto fail;
+
+	/* kmap the object */
+	ret = ttm_bo_kmap(&bo->bo, 0, bo->bo.num_pages, &ast->cache_kmap);
+	if (ret)
+		goto fail;
+
+	ast->cursor_cache = obj;
+	ast->cursor_cache_gpu_addr = gpu_addr;
+	DRM_ERROR("pinned cursor cache at %llx\n", ast->cursor_cache_gpu_addr);
+	return 0;
+fail:
+	return ret;
+}
+
+void ast_cursor_fini(struct drm_device *dev)
+{
+	struct ast_private *ast = dev->dev_private;
+	ttm_bo_kunmap(&ast->cache_kmap);
+	drm_gem_object_unreference_unlocked(ast->cursor_cache);
+}
+
+int ast_mode_init(struct drm_device *dev)
+{
+	ast_cursor_init(dev);
+	ast_crtc_init(dev);
+	ast_encoder_init(dev);
+	ast_connector_init(dev);
+	return 0;
+}
+
+void ast_mode_fini(struct drm_device *dev)
+{
+	ast_cursor_fini(dev);
+}
+
+static int get_clock(void *i2c_priv)
+{
+	struct ast_i2c_chan *i2c = i2c_priv;
+	struct ast_private *ast = i2c->dev->dev_private;
+	uint32_t val;
+
+	val = ast_get_index_reg_mask(ast, AST_IO_CRTC_PORT, 0xb7, 0x10) >> 4;
+	return val & 1 ? 1 : 0;
+}
+
+static int get_data(void *i2c_priv)
+{
+	struct ast_i2c_chan *i2c = i2c_priv;
+	struct ast_private *ast = i2c->dev->dev_private;
+	uint32_t val;
+
+	val = ast_get_index_reg_mask(ast, AST_IO_CRTC_PORT, 0xb7, 0x20) >> 5;
+	return val & 1 ? 1 : 0;
+}
+
+static void set_clock(void *i2c_priv, int clock)
+{
+	struct ast_i2c_chan *i2c = i2c_priv;
+	struct ast_private *ast = i2c->dev->dev_private;
+	int i;
+	u8 ujcrb7, jtemp;
+
+	for (i = 0; i < 0x10000; i++) {
+		ujcrb7 = ((clock & 0x01) ? 0 : 1);
+		ast_set_index_reg_mask(ast, AST_IO_CRTC_PORT, 0xb7, 0xfe, ujcrb7);
+		jtemp = ast_get_index_reg_mask(ast, AST_IO_CRTC_PORT, 0xb7, 0x01);
+		if (ujcrb7 == jtemp)
+			break;
+	}
+}
+
+static void set_data(void *i2c_priv, int data)
+{
+	struct ast_i2c_chan *i2c = i2c_priv;
+	struct ast_private *ast = i2c->dev->dev_private;
+	int i;
+	u8 ujcrb7, jtemp;
+
+	for (i = 0; i < 0x10000; i++) {
+		ujcrb7 = ((data & 0x01) ? 0 : 1) << 2;
+		ast_set_index_reg_mask(ast, AST_IO_CRTC_PORT, 0xb7, 0xfb, ujcrb7);
+		jtemp = ast_get_index_reg_mask(ast, AST_IO_CRTC_PORT, 0xb7, 0x04);
+		if (ujcrb7 == jtemp)
+			break;
+	}
+}
+
+static struct ast_i2c_chan *ast_i2c_create(struct drm_device *dev)
+{
+	struct ast_i2c_chan *i2c;
+	int ret;
+
+	i2c = kzalloc(sizeof(struct ast_i2c_chan), GFP_KERNEL);
+	if (!i2c)
+		return NULL;
+
+	i2c->adapter.owner = THIS_MODULE;
+	i2c->adapter.class = I2C_CLASS_DDC;
+	i2c->adapter.dev.parent = &dev->pdev->dev;
+	i2c->dev = dev;
+	i2c_set_adapdata(&i2c->adapter, i2c);
+	snprintf(i2c->adapter.name, sizeof(i2c->adapter.name),
+		 "AST i2c bit bus");
+	i2c->adapter.algo_data = &i2c->bit;
+
+	i2c->bit.udelay = 20;
+	i2c->bit.timeout = 2;
+	i2c->bit.data = i2c;
+	i2c->bit.setsda = set_data;
+	i2c->bit.setscl = set_clock;
+	i2c->bit.getsda = get_data;
+	i2c->bit.getscl = get_clock;
+	ret = i2c_bit_add_bus(&i2c->adapter);
+	if (ret) {
+		DRM_ERROR("Failed to register bit i2c\n");
+		goto out_free;
+	}
+
+	return i2c;
+out_free:
+	kfree(i2c);
+	return NULL;
+}
+
+static void ast_i2c_destroy(struct ast_i2c_chan *i2c)
+{
+	if (!i2c)
+		return;
+	i2c_del_adapter(&i2c->adapter);
+	kfree(i2c);
+}
+
+void ast_show_cursor(struct drm_crtc *crtc)
+{
+	struct ast_private *ast = crtc->dev->dev_private;
+	u8 jreg;
+
+	jreg = 0x2;
+	/* enable ARGB cursor */
+	jreg |= 1;
+	ast_set_index_reg_mask(ast, AST_IO_CRTC_PORT, 0xcb, 0xfc, jreg);
+}
+
+void ast_hide_cursor(struct drm_crtc *crtc)
+{
+	struct ast_private *ast = crtc->dev->dev_private;
+	ast_set_index_reg_mask(ast, AST_IO_CRTC_PORT, 0xcb, 0xfc, 0x00);
+}
+
+static u32 copy_cursor_image(u8 *src, u8 *dst, int width, int height)
+{
+	union {
+		u32 ul;
+		u8 b[4];
+	} srcdata32[2], data32;
+	union {
+		u16 us;
+		u8 b[2];
+	} data16;
+	u32 csum = 0;
+	s32 alpha_dst_delta, last_alpha_dst_delta;
+	u8 *srcxor, *dstxor;
+	int i, j;
+	u32 per_pixel_copy, two_pixel_copy;
+
+	alpha_dst_delta = AST_MAX_HWC_WIDTH << 1;
+	last_alpha_dst_delta = alpha_dst_delta - (width << 1);
+
+	srcxor = src;
+	dstxor = (u8 *)dst + last_alpha_dst_delta + (AST_MAX_HWC_HEIGHT - height) * alpha_dst_delta;
+	per_pixel_copy = width & 1;
+	two_pixel_copy = width >> 1;
+
+	for (j = 0; j < height; j++) {
+		for (i = 0; i < two_pixel_copy; i++) {
+			srcdata32[0].ul = *((u32 *)srcxor) & 0xf0f0f0f0;
+			srcdata32[1].ul = *((u32 *)(srcxor + 4)) & 0xf0f0f0f0;
+			data32.b[0] = srcdata32[0].b[1] | (srcdata32[0].b[0] >> 4);
+			data32.b[1] = srcdata32[0].b[3] | (srcdata32[0].b[2] >> 4);
+			data32.b[2] = srcdata32[0].b[1] | (srcdata32[1].b[0] >> 4);
+			data32.b[3] = srcdata32[0].b[3] | (srcdata32[1].b[2] >> 4);
+
+			writel(data32.ul, dstxor);
+			csum += data32.ul;
+
+			dstxor += 4;
+			srcxor += 8;
+
+		}
+
+		for (i = 0; i < per_pixel_copy; i++) {
+			srcdata32[0].ul = *((u32 *)srcxor) & 0xf0f0f0f0;
+			data16.b[0] = srcdata32[0].b[1] | (srcdata32[0].b[0] >> 4);
+			data16.b[1] = srcdata32[0].b[3] | (srcdata32[0].b[2] >> 4);
+			writew(data16.us, dstxor);
+			csum += (u32)data16.us;
+
+			dstxor += 2;
+			srcxor += 4;
+		}
+		dstxor += last_alpha_dst_delta;
+	}
+	return csum;
+}
+
+static int ast_cursor_set(struct drm_crtc *crtc,
+			  struct drm_file *file_priv,
+			  uint32_t handle,
+			  uint32_t width,
+			  uint32_t height)
+{
+	struct ast_private *ast = crtc->dev->dev_private;
+	struct ast_crtc *ast_crtc = to_ast_crtc(crtc);
+	struct drm_gem_object *obj;
+	struct ast_bo *bo;
+	uint64_t gpu_addr;
+	u32 csum;
+	int ret;
+	struct ttm_bo_kmap_obj uobj_map;
+	u8 *src, *dst;
+	bool src_isiomem, dst_isiomem;
+	if (!handle) {
+		ast_hide_cursor(crtc);
+		return 0;
+	}
+
+	if (width > AST_MAX_HWC_WIDTH || height > AST_MAX_HWC_HEIGHT)
+		return -EINVAL;
+
+	obj = drm_gem_object_lookup(crtc->dev, file_priv, handle);
+	if (!obj) {
+		DRM_ERROR("Cannot find cursor object %x for crtc\n", handle);
+		return -ENOENT;
+	}
+	bo = gem_to_ast_bo(obj);
+
+	ret = ast_bo_reserve(bo, false);
+	if (ret)
+		goto fail;
+
+	ret = ttm_bo_kmap(&bo->bo, 0, bo->bo.num_pages, &uobj_map);
+
+	src = ttm_kmap_obj_virtual(&uobj_map, &src_isiomem);
+	dst = ttm_kmap_obj_virtual(&ast->cache_kmap, &dst_isiomem);
+
+	if (src_isiomem == true)
+		DRM_ERROR("src cursor bo should be in main memory\n");
+	if (dst_isiomem == false)
+		DRM_ERROR("dst bo should be in VRAM\n");
+
+	dst += (AST_HWC_SIZE + AST_HWC_SIGNATURE_SIZE)*ast->next_cursor;
+
+	/* do data transfer to cursor cache */
+	csum = copy_cursor_image(src, dst, width, height);
+
+	/* write checksum + signature */
+	ttm_bo_kunmap(&uobj_map);
+	ast_bo_unreserve(bo);
+	{
+		u8 *dst = (u8 *)ast->cache_kmap.virtual + (AST_HWC_SIZE + AST_HWC_SIGNATURE_SIZE)*ast->next_cursor + AST_HWC_SIZE;
+		writel(csum, dst);
+		writel(width, dst + AST_HWC_SIGNATURE_SizeX);
+		writel(height, dst + AST_HWC_SIGNATURE_SizeY);
+		writel(0, dst + AST_HWC_SIGNATURE_HOTSPOTX);
+		writel(0, dst + AST_HWC_SIGNATURE_HOTSPOTY);
+
+		/* set pattern offset */
+		gpu_addr = ast->cursor_cache_gpu_addr;
+		gpu_addr += (AST_HWC_SIZE + AST_HWC_SIGNATURE_SIZE)*ast->next_cursor;
+		gpu_addr >>= 3;
+		ast_set_index_reg(ast, AST_IO_CRTC_PORT, 0xc8, gpu_addr & 0xff);
+		ast_set_index_reg(ast, AST_IO_CRTC_PORT, 0xc9, (gpu_addr >> 8) & 0xff);
+		ast_set_index_reg(ast, AST_IO_CRTC_PORT, 0xca, (gpu_addr >> 16) & 0xff);
+	}
+	ast_crtc->cursor_width = width;
+	ast_crtc->cursor_height = height;
+	ast_crtc->offset_x = AST_MAX_HWC_WIDTH - width;
+	ast_crtc->offset_y = AST_MAX_HWC_WIDTH - height;
+
+	ast->next_cursor = (ast->next_cursor + 1) % AST_DEFAULT_HWC_NUM;
+
+	ast_show_cursor(crtc);
+
+	drm_gem_object_unreference_unlocked(obj);
+	return 0;
+fail:
+	drm_gem_object_unreference_unlocked(obj);
+	return ret;
+}
+
+static int ast_cursor_move(struct drm_crtc *crtc,
+			   int x, int y)
+{
+	struct ast_crtc *ast_crtc = to_ast_crtc(crtc);
+	struct ast_private *ast = crtc->dev->dev_private;
+	int x_offset, y_offset;
+	u8 *sig;
+
+	sig = (u8 *)ast->cache_kmap.virtual + (AST_HWC_SIZE + AST_HWC_SIGNATURE_SIZE)*ast->next_cursor + AST_HWC_SIZE;
+	writel(x, sig + AST_HWC_SIGNATURE_X);
+	writel(y, sig + AST_HWC_SIGNATURE_Y);
+
+	x_offset = ast_crtc->offset_x;
+	y_offset = ast_crtc->offset_y;
+	if (x < 0) {
+		x_offset = (-x) + ast_crtc->offset_x;
+		x = 0;
+	}
+
+	if (y < 0) {
+		y_offset = (-y) + ast_crtc->offset_y;
+		y = 0;
+	}
+	ast_set_index_reg(ast, AST_IO_CRTC_PORT, 0xc2, x_offset);
+	ast_set_index_reg(ast, AST_IO_CRTC_PORT, 0xc3, y_offset);
+	ast_set_index_reg(ast, AST_IO_CRTC_PORT, 0xc4, (x & 0xff));
+	ast_set_index_reg(ast, AST_IO_CRTC_PORT, 0xc5, ((x >> 8) & 0x0f));
+	ast_set_index_reg(ast, AST_IO_CRTC_PORT, 0xc6, (y & 0xff));
+	ast_set_index_reg(ast, AST_IO_CRTC_PORT, 0xc7, ((y >> 8) & 0x07));
+
+	/* dummy write to fire HWC */
+	ast_set_index_reg_mask(ast, AST_IO_CRTC_PORT, 0xCB, 0xFF, 0x00);
+
+	return 0;
+}
diff --git a/drivers/gpu/drm/ast/ast_post.c b/drivers/gpu/drm/ast/ast_post.c
new file mode 100644
index 00000000000..6edbee63b0c
--- /dev/null
+++ b/drivers/gpu/drm/ast/ast_post.c
@@ -0,0 +1,1780 @@
+/*
+ * 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, 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: Dave Airlie <airlied@redhat.com>
+ */
+
+#include "drmP.h"
+#include "ast_drv.h"
+
+#include "ast_dram_tables.h"
+
+static void ast_init_dram_2300(struct drm_device *dev);
+
+static void
+ast_enable_vga(struct drm_device *dev)
+{
+	struct ast_private *ast = dev->dev_private;
+
+	ast_io_write8(ast, 0x43, 0x01);
+	ast_io_write8(ast, 0x42, 0x01);
+}
+
+#if 0 /* will use later */
+static bool
+ast_is_vga_enabled(struct drm_device *dev)
+{
+	struct ast_private *ast = dev->dev_private;
+	u8 ch;
+
+	if (ast->chip == AST1180) {
+		/* TODO 1180 */
+	} else {
+		ch = ast_io_read8(ast, 0x43);
+		if (ch) {
+			ast_open_key(ast);
+			ch = ast_get_index_reg_mask(ast, AST_IO_CRTC_PORT, 0xb6, 0xff);
+			return ch & 0x04;
+		}
+	}
+	return 0;
+}
+#endif
+
+static const u8 extreginfo[] = { 0x0f, 0x04, 0x1c, 0xff };
+static const u8 extreginfo_ast2300a0[] = { 0x0f, 0x04, 0x1c, 0xff };
+static const u8 extreginfo_ast2300[] = { 0x0f, 0x04, 0x1f, 0xff };
+
+static void
+ast_set_def_ext_reg(struct drm_device *dev)
+{
+	struct ast_private *ast = dev->dev_private;
+	u8 i, index, reg;
+	const u8 *ext_reg_info;
+
+	/* reset scratch */
+	for (i = 0x81; i <= 0x8f; i++)
+		ast_set_index_reg(ast, AST_IO_CRTC_PORT, i, 0x00);
+
+	if (ast->chip == AST2300) {
+		if (dev->pdev->revision >= 0x20)
+			ext_reg_info = extreginfo_ast2300;
+		else
+			ext_reg_info = extreginfo_ast2300a0;
+	} else
+		ext_reg_info = extreginfo;
+
+	index = 0xa0;
+	while (*ext_reg_info != 0xff) {
+		ast_set_index_reg_mask(ast, AST_IO_CRTC_PORT, index, 0x00, *ext_reg_info);
+		index++;
+		ext_reg_info++;
+	}
+
+	/* disable standard IO/MEM decode if secondary */
+	/* ast_set_index_reg-mask(ast, AST_IO_CRTC_PORT, 0xa1, 0xff, 0x3); */
+
+	/* Set Ext. Default */
+	ast_set_index_reg_mask(ast, AST_IO_CRTC_PORT, 0x8c, 0x00, 0x01);
+	ast_set_index_reg_mask(ast, AST_IO_CRTC_PORT, 0xb7, 0x00, 0x00);
+
+	/* Enable RAMDAC for A1 */
+	reg = 0x04;
+	if (ast->chip == AST2300)
+		reg |= 0x20;
+	ast_set_index_reg_mask(ast, AST_IO_CRTC_PORT, 0xb6, 0xff, reg);
+}
+
+static inline u32 mindwm(struct ast_private *ast, u32 r)
+{
+	ast_write32(ast, 0xf004, r & 0xffff0000);
+	ast_write32(ast, 0xf000, 0x1);
+
+	return ast_read32(ast, 0x10000 + (r & 0x0000ffff));
+}
+
+static inline void moutdwm(struct ast_private *ast, u32 r, u32 v)
+{
+	ast_write32(ast, 0xf004, r & 0xffff0000);
+	ast_write32(ast, 0xf000, 0x1);
+	ast_write32(ast, 0x10000 + (r & 0x0000ffff), v);
+}
+
+/*
+ * AST2100/2150 DLL CBR Setting
+ */
+#define CBR_SIZE_AST2150	     ((16 << 10) - 1)
+#define CBR_PASSNUM_AST2150          5
+#define CBR_THRESHOLD_AST2150        10
+#define CBR_THRESHOLD2_AST2150       10
+#define TIMEOUT_AST2150              5000000
+
+#define CBR_PATNUM_AST2150           8
+
+static const u32 pattern_AST2150[14] = {
+	0xFF00FF00,
+	0xCC33CC33,
+	0xAA55AA55,
+	0xFFFE0001,
+	0x683501FE,
+	0x0F1929B0,
+	0x2D0B4346,
+	0x60767F02,
+	0x6FBE36A6,
+	0x3A253035,
+	0x3019686D,
+	0x41C6167E,
+	0x620152BF,
+	0x20F050E0
+};
+
+static u32 mmctestburst2_ast2150(struct ast_private *ast, u32 datagen)
+{
+	u32 data, timeout;
+
+	moutdwm(ast, 0x1e6e0070, 0x00000000);
+	moutdwm(ast, 0x1e6e0070, 0x00000001 | (datagen << 3));
+	timeout = 0;
+	do {
+		data = mindwm(ast, 0x1e6e0070) & 0x40;
+		if (++timeout > TIMEOUT_AST2150) {
+			moutdwm(ast, 0x1e6e0070, 0x00000000);
+			return 0xffffffff;
+		}
+	} while (!data);
+	moutdwm(ast, 0x1e6e0070, 0x00000000);
+	moutdwm(ast, 0x1e6e0070, 0x00000003 | (datagen << 3));
+	timeout = 0;
+	do {
+		data = mindwm(ast, 0x1e6e0070) & 0x40;
+		if (++timeout > TIMEOUT_AST2150) {
+			moutdwm(ast, 0x1e6e0070, 0x00000000);
+			return 0xffffffff;
+		}
+	} while (!data);
+	data = (mindwm(ast, 0x1e6e0070) & 0x80) >> 7;
+	moutdwm(ast, 0x1e6e0070, 0x00000000);
+	return data;
+}
+
+#if 0 /* unused in DDX driver - here for completeness */
+static u32 mmctestsingle2_ast2150(struct ast_private *ast, u32 datagen)
+{
+	u32 data, timeout;
+
+	moutdwm(ast, 0x1e6e0070, 0x00000000);
+	moutdwm(ast, 0x1e6e0070, 0x00000005 | (datagen << 3));
+	timeout = 0;
+	do {
+		data = mindwm(ast, 0x1e6e0070) & 0x40;
+		if (++timeout > TIMEOUT_AST2150) {
+			moutdwm(ast, 0x1e6e0070, 0x00000000);
+			return 0xffffffff;
+		}
+	} while (!data);
+	data = (mindwm(ast, 0x1e6e0070) & 0x80) >> 7;
+	moutdwm(ast, 0x1e6e0070, 0x00000000);
+	return data;
+}
+#endif
+
+static int cbrtest_ast2150(struct ast_private *ast)
+{
+	int i;
+
+	for (i = 0; i < 8; i++)
+		if (mmctestburst2_ast2150(ast, i))
+			return 0;
+	return 1;
+}
+
+static int cbrscan_ast2150(struct ast_private *ast, int busw)
+{
+	u32 patcnt, loop;
+
+	for (patcnt = 0; patcnt < CBR_PATNUM_AST2150; patcnt++) {
+		moutdwm(ast, 0x1e6e007c, pattern_AST2150[patcnt]);
+		for (loop = 0; loop < CBR_PASSNUM_AST2150; loop++) {
+			if (cbrtest_ast2150(ast))
+				break;
+		}
+		if (loop == CBR_PASSNUM_AST2150)
+			return 0;
+	}
+	return 1;
+}
+
+
+static void cbrdlli_ast2150(struct ast_private *ast, int busw)
+{
+	u32 dll_min[4], dll_max[4], dlli, data, passcnt;
+
+cbr_start:
+	dll_min[0] = dll_min[1] = dll_min[2] = dll_min[3] = 0xff;
+	dll_max[0] = dll_max[1] = dll_max[2] = dll_max[3] = 0x0;
+	passcnt = 0;
+
+	for (dlli = 0; dlli < 100; dlli++) {
+		moutdwm(ast, 0x1e6e0068, dlli | (dlli << 8) | (dlli << 16) | (dlli << 24));
+		data = cbrscan_ast2150(ast, busw);
+		if (data != 0) {
+			if (data & 0x1) {
+				if (dll_min[0] > dlli)
+					dll_min[0] = dlli;
+				if (dll_max[0] < dlli)
+					dll_max[0] = dlli;
+			}
+			passcnt++;
+		} else if (passcnt >= CBR_THRESHOLD_AST2150)
+			goto cbr_start;
+	}
+	if (dll_max[0] == 0 || (dll_max[0]-dll_min[0]) < CBR_THRESHOLD_AST2150)
+		goto cbr_start;
+
+	dlli = dll_min[0] + (((dll_max[0] - dll_min[0]) * 7) >> 4);
+	moutdwm(ast, 0x1e6e0068, dlli | (dlli << 8) | (dlli << 16) | (dlli << 24));
+}
+
+
+
+static void ast_init_dram_reg(struct drm_device *dev)
+{
+	struct ast_private *ast = dev->dev_private;
+	u8 j;
+	u32 data, temp, i;
+	const struct ast_dramstruct *dram_reg_info;
+
+	j = ast_get_index_reg_mask(ast, AST_IO_CRTC_PORT, 0xd0, 0xff);
+
+	if ((j & 0x80) == 0) { /* VGA only */
+		if (ast->chip == AST2000) {
+			dram_reg_info = ast2000_dram_table_data;
+			ast_write32(ast, 0xf004, 0x1e6e0000);
+			ast_write32(ast, 0xf000, 0x1);
+			ast_write32(ast, 0x10100, 0xa8);
+
+			do {
+				;
+			} while (ast_read32(ast, 0x10100) != 0xa8);
+		} else {/* AST2100/1100 */
+			if (ast->chip == AST2100 || ast->chip == 2200)
+				dram_reg_info = ast2100_dram_table_data;
+			else
+				dram_reg_info = ast1100_dram_table_data;
+
+			ast_write32(ast, 0xf004, 0x1e6e0000);
+			ast_write32(ast, 0xf000, 0x1);
+			ast_write32(ast, 0x12000, 0x1688A8A8);
+			do {
+				;
+			} while (ast_read32(ast, 0x12000) != 0x01);
+
+			ast_write32(ast, 0x10000, 0xfc600309);
+			do {
+				;
+			} while (ast_read32(ast, 0x10000) != 0x01);
+		}
+
+		while (dram_reg_info->index != 0xffff) {
+			if (dram_reg_info->index == 0xff00) {/* delay fn */
+				for (i = 0; i < 15; i++)
+					udelay(dram_reg_info->data);
+			} else if (dram_reg_info->index == 0x4 && ast->chip != AST2000) {
+				data = dram_reg_info->data;
+				if (ast->dram_type == AST_DRAM_1Gx16)
+					data = 0x00000d89;
+				else if (ast->dram_type == AST_DRAM_1Gx32)
+					data = 0x00000c8d;
+
+				temp = ast_read32(ast, 0x12070);
+				temp &= 0xc;
+				temp <<= 2;
+				ast_write32(ast, 0x10000 + dram_reg_info->index, data | temp);
+			} else
+				ast_write32(ast, 0x10000 + dram_reg_info->index, dram_reg_info->data);
+			dram_reg_info++;
+		}
+
+		/* AST 2100/2150 DRAM calibration */
+		data = ast_read32(ast, 0x10120);
+		if (data == 0x5061) { /* 266Mhz */
+			data = ast_read32(ast, 0x10004);
+			if (data & 0x40)
+				cbrdlli_ast2150(ast, 16); /* 16 bits */
+			else
+				cbrdlli_ast2150(ast, 32); /* 32 bits */
+		}
+
+		switch (ast->chip) {
+		case AST2000:
+			temp = ast_read32(ast, 0x10140);
+			ast_write32(ast, 0x10140, temp | 0x40);
+			break;
+		case AST1100:
+		case AST2100:
+		case AST2200:
+		case AST2150:
+			temp = ast_read32(ast, 0x1200c);
+			ast_write32(ast, 0x1200c, temp & 0xfffffffd);
+			temp = ast_read32(ast, 0x12040);
+			ast_write32(ast, 0x12040, temp | 0x40);
+			break;
+		default:
+			break;
+		}
+	}
+
+	/* wait ready */
+	do {
+		j = ast_get_index_reg_mask(ast, AST_IO_CRTC_PORT, 0xd0, 0xff);
+	} while ((j & 0x40) == 0);
+}
+
+void ast_post_gpu(struct drm_device *dev)
+{
+	u32 reg;
+	struct ast_private *ast = dev->dev_private;
+
+	pci_read_config_dword(ast->dev->pdev, 0x04, &reg);
+	reg |= 0x3;
+	pci_write_config_dword(ast->dev->pdev, 0x04, reg);
+
+	ast_enable_vga(dev);
+	ast_open_key(ast);
+	ast_set_def_ext_reg(dev);
+
+	if (ast->chip == AST2300)
+		ast_init_dram_2300(dev);
+	else
+		ast_init_dram_reg(dev);
+}
+
+/* AST 2300 DRAM settings */
+#define AST_DDR3 0
+#define AST_DDR2 1
+
+struct ast2300_dram_param {
+	u32 dram_type;
+	u32 dram_chipid;
+	u32 dram_freq;
+	u32 vram_size;
+	u32 odt;
+	u32 wodt;
+	u32 rodt;
+	u32 dram_config;
+	u32 reg_PERIOD;
+	u32 reg_MADJ;
+	u32 reg_SADJ;
+	u32 reg_MRS;
+	u32 reg_EMRS;
+	u32 reg_AC1;
+	u32 reg_AC2;
+	u32 reg_DQSIC;
+	u32 reg_DRV;
+	u32 reg_IOZ;
+	u32 reg_DQIDLY;
+	u32 reg_FREQ;
+	u32 madj_max;
+	u32 dll2_finetune_step;
+};
+
+/*
+ * DQSI DLL CBR Setting
+ */
+#define CBR_SIZE1            ((4  << 10) - 1)
+#define CBR_SIZE2            ((64 << 10) - 1)
+#define CBR_PASSNUM          5
+#define CBR_PASSNUM2         5
+#define CBR_THRESHOLD        10
+#define CBR_THRESHOLD2       10
+#define TIMEOUT              5000000
+#define CBR_PATNUM           8
+
+static const u32 pattern[8] = {
+	0xFF00FF00,
+	0xCC33CC33,
+	0xAA55AA55,
+	0x88778877,
+	0x92CC4D6E,
+	0x543D3CDE,
+	0xF1E843C7,
+	0x7C61D253
+};
+
+#if 0 /* unused in DDX, included for completeness */
+static int mmc_test_burst(struct ast_private *ast, u32 datagen)
+{
+	u32 data, timeout;
+
+	moutdwm(ast, 0x1e6e0070, 0x00000000);
+	moutdwm(ast, 0x1e6e0070, 0x000000c1 | (datagen << 3));
+	timeout = 0;
+	do {
+		data = mindwm(ast, 0x1e6e0070) & 0x3000;
+		if (data & 0x2000) {
+			return 0;
+		}
+		if (++timeout > TIMEOUT) {
+			moutdwm(ast, 0x1e6e0070, 0x00000000);
+			return 0;
+		}
+	} while (!data);
+	moutdwm(ast, 0x1e6e0070, 0x00000000);
+	return 1;
+}
+#endif
+
+static int mmc_test_burst2(struct ast_private *ast, u32 datagen)
+{
+	u32 data, timeout;
+
+	moutdwm(ast, 0x1e6e0070, 0x00000000);
+	moutdwm(ast, 0x1e6e0070, 0x00000041 | (datagen << 3));
+	timeout = 0;
+	do {
+		data = mindwm(ast, 0x1e6e0070) & 0x1000;
+		if (++timeout > TIMEOUT) {
+			moutdwm(ast, 0x1e6e0070, 0x0);
+			return -1;
+		}
+	} while (!data);
+	data = mindwm(ast, 0x1e6e0078);
+	data = (data | (data >> 16)) & 0xffff;
+	moutdwm(ast, 0x1e6e0070, 0x0);
+	return data;
+}
+
+#if 0 /* Unused in DDX here for completeness */
+static int mmc_test_single(struct ast_private *ast, u32 datagen)
+{
+	u32 data, timeout;
+
+	moutdwm(ast, 0x1e6e0070, 0x00000000);
+	moutdwm(ast, 0x1e6e0070, 0x000000c5 | (datagen << 3));
+	timeout = 0;
+	do {
+		data = mindwm(ast, 0x1e6e0070) & 0x3000;
+		if (data & 0x2000)
+			return 0;
+		if (++timeout > TIMEOUT) {
+			moutdwm(ast, 0x1e6e0070, 0x0);
+			return 0;
+		}
+	} while (!data);
+	moutdwm(ast, 0x1e6e0070, 0x0);
+	return 1;
+}
+#endif
+
+static int mmc_test_single2(struct ast_private *ast, u32 datagen)
+{
+	u32 data, timeout;
+
+	moutdwm(ast, 0x1e6e0070, 0x00000000);
+	moutdwm(ast, 0x1e6e0070, 0x00000005 | (datagen << 3));
+	timeout = 0;
+	do {
+		data = mindwm(ast, 0x1e6e0070) & 0x1000;
+		if (++timeout > TIMEOUT) {
+			moutdwm(ast, 0x1e6e0070, 0x0);
+			return -1;
+		}
+	} while (!data);
+	data = mindwm(ast, 0x1e6e0078);
+	data = (data | (data >> 16)) & 0xffff;
+	moutdwm(ast, 0x1e6e0070, 0x0);
+	return data;
+}
+
+static int cbr_test(struct ast_private *ast)
+{
+	u32 data;
+	int i;
+	data = mmc_test_single2(ast, 0);
+	if ((data & 0xff) && (data & 0xff00))
+		return 0;
+	for (i = 0; i < 8; i++) {
+		data = mmc_test_burst2(ast, i);
+		if ((data & 0xff) && (data & 0xff00))
+			return 0;
+	}
+	if (!data)
+		return 3;
+	else if (data & 0xff)
+		return 2;
+	return 1;
+}
+
+static int cbr_scan(struct ast_private *ast)
+{
+	u32 data, data2, patcnt, loop;
+
+	data2 = 3;
+	for (patcnt = 0; patcnt < CBR_PATNUM; patcnt++) {
+		moutdwm(ast, 0x1e6e007c, pattern[patcnt]);
+		for (loop = 0; loop < CBR_PASSNUM2; loop++) {
+			if ((data = cbr_test(ast)) != 0) {
+				data2 &= data;
+				if (!data2)
+					return 0;
+				break;
+			}
+		}
+		if (loop == CBR_PASSNUM2)
+			return 0;
+	}
+	return data2;
+}
+
+static u32 cbr_test2(struct ast_private *ast)
+{
+	u32 data;
+
+	data = mmc_test_burst2(ast, 0);
+	if (data == 0xffff)
+		return 0;
+	data |= mmc_test_single2(ast, 0);
+	if (data == 0xffff)
+		return 0;
+
+	return ~data & 0xffff;
+}
+
+static u32 cbr_scan2(struct ast_private *ast)
+{
+	u32 data, data2, patcnt, loop;
+
+	data2 = 0xffff;
+	for (patcnt = 0; patcnt < CBR_PATNUM; patcnt++) {
+		moutdwm(ast, 0x1e6e007c, pattern[patcnt]);
+		for (loop = 0; loop < CBR_PASSNUM2; loop++) {
+			if ((data = cbr_test2(ast)) != 0) {
+				data2 &= data;
+				if (!data)
+					return 0;
+				break;
+			}
+		}
+		if (loop == CBR_PASSNUM2)
+			return 0;
+	}
+	return data2;
+}
+
+#if 0 /* unused in DDX - added for completeness */
+static void finetuneDQI(struct ast_private *ast, struct ast2300_dram_param *param)
+{
+	u32 gold_sadj[2], dllmin[16], dllmax[16], dlli, data, cnt, mask, passcnt;
+
+	gold_sadj[0] = (mindwm(ast, 0x1E6E0024) >> 16) & 0xffff;
+	gold_sadj[1] = gold_sadj[0] >> 8;
+	gold_sadj[0] = gold_sadj[0] & 0xff;
+	gold_sadj[0] = (gold_sadj[0] + gold_sadj[1]) >> 1;
+	gold_sadj[1] = gold_sadj[0];
+
+	for (cnt = 0; cnt < 16; cnt++) {
+		dllmin[cnt] = 0xff;
+		dllmax[cnt] = 0x0;
+	}
+	passcnt = 0;
+	for (dlli = 0; dlli < 76; dlli++) {
+		moutdwm(ast, 0x1E6E0068, 0x00001400 | (dlli << 16) | (dlli << 24));
+		/* Wait DQSI latch phase calibration */
+		moutdwm(ast, 0x1E6E0074, 0x00000010);
+		moutdwm(ast, 0x1E6E0070, 0x00000003);
+		do {
+			data = mindwm(ast, 0x1E6E0070);
+		} while (!(data & 0x00001000));
+		moutdwm(ast, 0x1E6E0070, 0x00000000);
+
+		moutdwm(ast, 0x1E6E0074, CBR_SIZE1);
+		data = cbr_scan2(ast);
+		if (data != 0) {
+			mask = 0x00010001;
+			for (cnt = 0; cnt < 16; cnt++) {
+				if (data & mask) {
+					if (dllmin[cnt] > dlli) {
+						dllmin[cnt] = dlli;
+					}
+					if (dllmax[cnt] < dlli) {
+						dllmax[cnt] = dlli;
+					}
+				}
+				mask <<= 1;
+			}
+			passcnt++;
+		} else if (passcnt >= CBR_THRESHOLD) {
+			break;
+		}
+	}
+	data = 0;
+	for (cnt = 0; cnt < 8; cnt++) {
+		data >>= 3;
+		if ((dllmax[cnt] > dllmin[cnt]) && ((dllmax[cnt] - dllmin[cnt]) >= CBR_THRESHOLD)) {
+			dlli = (dllmin[cnt] + dllmax[cnt]) >> 1;
+			if (gold_sadj[0] >= dlli) {
+				dlli = (gold_sadj[0] - dlli) >> 1;
+				if (dlli > 3) {
+					dlli = 3;
+				}
+			} else {
+				dlli = (dlli - gold_sadj[0]) >> 1;
+				if (dlli > 4) {
+					dlli = 4;
+				}
+				dlli = (8 - dlli) & 0x7;
+			}
+			data |= dlli << 21;
+		}
+	}
+	moutdwm(ast, 0x1E6E0080, data);
+
+	data = 0;
+	for (cnt = 8; cnt < 16; cnt++) {
+		data >>= 3;
+		if ((dllmax[cnt] > dllmin[cnt]) && ((dllmax[cnt] - dllmin[cnt]) >= CBR_THRESHOLD)) {
+			dlli = (dllmin[cnt] + dllmax[cnt]) >> 1;
+			if (gold_sadj[1] >= dlli) {
+				dlli = (gold_sadj[1] - dlli) >> 1;
+				if (dlli > 3) {
+					dlli = 3;
+				} else {
+					dlli = (dlli - 1) & 0x7;
+				}
+			} else {
+				dlli = (dlli - gold_sadj[1]) >> 1;
+				dlli += 1;
+				if (dlli > 4) {
+					dlli = 4;
+				}
+				dlli = (8 - dlli) & 0x7;
+			}
+			data |= dlli << 21;
+		}
+	}
+	moutdwm(ast, 0x1E6E0084, data);
+
+} /* finetuneDQI */
+#endif
+
+static void finetuneDQI_L(struct ast_private *ast, struct ast2300_dram_param *param)
+{
+	u32 gold_sadj[2], dllmin[16], dllmax[16], dlli, data, cnt, mask, passcnt;
+
+FINETUNE_START:
+	for (cnt = 0; cnt < 16; cnt++) {
+		dllmin[cnt] = 0xff;
+		dllmax[cnt] = 0x0;
+	}
+	passcnt = 0;
+	for (dlli = 0; dlli < 76; dlli++) {
+		moutdwm(ast, 0x1E6E0068, 0x00001400 | (dlli << 16) | (dlli << 24));
+		/* Wait DQSI latch phase calibration */
+		moutdwm(ast, 0x1E6E0074, 0x00000010);
+		moutdwm(ast, 0x1E6E0070, 0x00000003);
+		do {
+			data = mindwm(ast, 0x1E6E0070);
+		} while (!(data & 0x00001000));
+		moutdwm(ast, 0x1E6E0070, 0x00000000);
+
+		moutdwm(ast, 0x1E6E0074, CBR_SIZE1);
+		data = cbr_scan2(ast);
+		if (data != 0) {
+			mask = 0x00010001;
+			for (cnt = 0; cnt < 16; cnt++) {
+				if (data & mask) {
+					if (dllmin[cnt] > dlli) {
+						dllmin[cnt] = dlli;
+					}
+					if (dllmax[cnt] < dlli) {
+						dllmax[cnt] = dlli;
+					}
+				}
+				mask <<= 1;
+			}
+			passcnt++;
+		} else if (passcnt >= CBR_THRESHOLD2) {
+			break;
+		}
+	}
+	gold_sadj[0] = 0x0;
+	passcnt = 0;
+	for (cnt = 0; cnt < 16; cnt++) {
+		if ((dllmax[cnt] > dllmin[cnt]) && ((dllmax[cnt] - dllmin[cnt]) >= CBR_THRESHOLD2)) {
+			gold_sadj[0] += dllmin[cnt];
+			passcnt++;
+		}
+	}
+	if (passcnt != 16) {
+		goto FINETUNE_START;
+	}
+	gold_sadj[0] = gold_sadj[0] >> 4;
+	gold_sadj[1] = gold_sadj[0];
+
+	data = 0;
+	for (cnt = 0; cnt < 8; cnt++) {
+		data >>= 3;
+		if ((dllmax[cnt] > dllmin[cnt]) && ((dllmax[cnt] - dllmin[cnt]) >= CBR_THRESHOLD2)) {
+			dlli = dllmin[cnt];
+			if (gold_sadj[0] >= dlli) {
+				dlli = ((gold_sadj[0] - dlli) * 19) >> 5;
+				if (dlli > 3) {
+					dlli = 3;
+				}
+			} else {
+				dlli = ((dlli - gold_sadj[0]) * 19) >> 5;
+				if (dlli > 4) {
+					dlli = 4;
+				}
+				dlli = (8 - dlli) & 0x7;
+			}
+			data |= dlli << 21;
+		}
+	}
+	moutdwm(ast, 0x1E6E0080, data);
+
+	data = 0;
+	for (cnt = 8; cnt < 16; cnt++) {
+		data >>= 3;
+		if ((dllmax[cnt] > dllmin[cnt]) && ((dllmax[cnt] - dllmin[cnt]) >= CBR_THRESHOLD2)) {
+			dlli = dllmin[cnt];
+			if (gold_sadj[1] >= dlli) {
+				dlli = ((gold_sadj[1] - dlli) * 19) >> 5;
+				if (dlli > 3) {
+					dlli = 3;
+				} else {
+					dlli = (dlli - 1) & 0x7;
+				}
+			} else {
+				dlli = ((dlli - gold_sadj[1]) * 19) >> 5;
+				dlli += 1;
+				if (dlli > 4) {
+					dlli = 4;
+				}
+				dlli = (8 - dlli) & 0x7;
+			}
+			data |= dlli << 21;
+		}
+	}
+	moutdwm(ast, 0x1E6E0084, data);
+
+} /* finetuneDQI_L */
+
+static void finetuneDQI_L2(struct ast_private *ast, struct ast2300_dram_param *param)
+{
+	u32 gold_sadj[2], dllmin[16], dllmax[16], dlli, data, cnt, mask, passcnt, data2;
+
+	for (cnt = 0; cnt < 16; cnt++) {
+		dllmin[cnt] = 0xff;
+		dllmax[cnt] = 0x0;
+	}
+	passcnt = 0;
+	for (dlli = 0; dlli < 76; dlli++) {
+		moutdwm(ast, 0x1E6E0068, 0x00001400 | (dlli << 16) | (dlli << 24));
+		/* Wait DQSI latch phase calibration */
+		moutdwm(ast, 0x1E6E0074, 0x00000010);
+		moutdwm(ast, 0x1E6E0070, 0x00000003);
+		do {
+			data = mindwm(ast, 0x1E6E0070);
+		} while (!(data & 0x00001000));
+		moutdwm(ast, 0x1E6E0070, 0x00000000);
+
+		moutdwm(ast, 0x1E6E0074, CBR_SIZE2);
+		data = cbr_scan2(ast);
+		if (data != 0) {
+			mask = 0x00010001;
+			for (cnt = 0; cnt < 16; cnt++) {
+				if (data & mask) {
+					if (dllmin[cnt] > dlli) {
+						dllmin[cnt] = dlli;
+					}
+					if (dllmax[cnt] < dlli) {
+						dllmax[cnt] = dlli;
+					}
+				}
+				mask <<= 1;
+			}
+			passcnt++;
+		} else if (passcnt >= CBR_THRESHOLD2) {
+			break;
+		}
+	}
+	gold_sadj[0] = 0x0;
+	gold_sadj[1] = 0xFF;
+	for (cnt = 0; cnt < 8; cnt++) {
+		if ((dllmax[cnt] > dllmin[cnt]) && ((dllmax[cnt] - dllmin[cnt]) >= CBR_THRESHOLD2)) {
+			if (gold_sadj[0] < dllmin[cnt]) {
+				gold_sadj[0] = dllmin[cnt];
+			}
+			if (gold_sadj[1] > dllmax[cnt]) {
+				gold_sadj[1] = dllmax[cnt];
+			}
+		}
+	}
+	gold_sadj[0] = (gold_sadj[1] + gold_sadj[0]) >> 1;
+	gold_sadj[1] = mindwm(ast, 0x1E6E0080);
+
+	data = 0;
+	for (cnt = 0; cnt < 8; cnt++) {
+		data >>= 3;
+		data2 = gold_sadj[1] & 0x7;
+		gold_sadj[1] >>= 3;
+		if ((dllmax[cnt] > dllmin[cnt]) && ((dllmax[cnt] - dllmin[cnt]) >= CBR_THRESHOLD2)) {
+			dlli = (dllmin[cnt] + dllmax[cnt]) >> 1;
+			if (gold_sadj[0] >= dlli) {
+				dlli = (gold_sadj[0] - dlli) >> 1;
+				if (dlli > 0) {
+					dlli = 1;
+				}
+				if (data2 != 3) {
+					data2 = (data2 + dlli) & 0x7;
+				}
+			} else {
+				dlli = (dlli - gold_sadj[0]) >> 1;
+				if (dlli > 0) {
+					dlli = 1;
+				}
+				if (data2 != 4) {
+					data2 = (data2 - dlli) & 0x7;
+				}
+			}
+		}
+		data |= data2 << 21;
+	}
+	moutdwm(ast, 0x1E6E0080, data);
+
+	gold_sadj[0] = 0x0;
+	gold_sadj[1] = 0xFF;
+	for (cnt = 8; cnt < 16; cnt++) {
+		if ((dllmax[cnt] > dllmin[cnt]) && ((dllmax[cnt] - dllmin[cnt]) >= CBR_THRESHOLD2)) {
+			if (gold_sadj[0] < dllmin[cnt]) {
+				gold_sadj[0] = dllmin[cnt];
+			}
+			if (gold_sadj[1] > dllmax[cnt]) {
+				gold_sadj[1] = dllmax[cnt];
+			}
+		}
+	}
+	gold_sadj[0] = (gold_sadj[1] + gold_sadj[0]) >> 1;
+	gold_sadj[1] = mindwm(ast, 0x1E6E0084);
+
+	data = 0;
+	for (cnt = 8; cnt < 16; cnt++) {
+		data >>= 3;
+		data2 = gold_sadj[1] & 0x7;
+		gold_sadj[1] >>= 3;
+		if ((dllmax[cnt] > dllmin[cnt]) && ((dllmax[cnt] - dllmin[cnt]) >= CBR_THRESHOLD2)) {
+			dlli = (dllmin[cnt] + dllmax[cnt]) >> 1;
+			if (gold_sadj[0] >= dlli) {
+				dlli = (gold_sadj[0] - dlli) >> 1;
+				if (dlli > 0) {
+					dlli = 1;
+				}
+				if (data2 != 3) {
+					data2 = (data2 + dlli) & 0x7;
+				}
+			} else {
+				dlli = (dlli - gold_sadj[0]) >> 1;
+				if (dlli > 0) {
+					dlli = 1;
+				}
+				if (data2 != 4) {
+					data2 = (data2 - dlli) & 0x7;
+				}
+			}
+		}
+		data |= data2 << 21;
+	}
+	moutdwm(ast, 0x1E6E0084, data);
+
+} /* finetuneDQI_L2 */
+
+static void cbr_dll2(struct ast_private *ast, struct ast2300_dram_param *param)
+{
+	u32 dllmin[2], dllmax[2], dlli, data, data2, passcnt;
+
+
+	finetuneDQI_L(ast, param);
+	finetuneDQI_L2(ast, param);
+
+CBR_START2:
+	dllmin[0] = dllmin[1] = 0xff;
+	dllmax[0] = dllmax[1] = 0x0;
+	passcnt = 0;
+	for (dlli = 0; dlli < 76; dlli++) {
+		moutdwm(ast, 0x1E6E0068, 0x00001300 | (dlli << 16) | (dlli << 24));
+		/* Wait DQSI latch phase calibration */
+		moutdwm(ast, 0x1E6E0074, 0x00000010);
+		moutdwm(ast, 0x1E6E0070, 0x00000003);
+		do {
+			data = mindwm(ast, 0x1E6E0070);
+		} while (!(data & 0x00001000));
+		moutdwm(ast, 0x1E6E0070, 0x00000000);
+
+		moutdwm(ast, 0x1E6E0074, CBR_SIZE2);
+		data = cbr_scan(ast);
+		if (data != 0) {
+			if (data & 0x1) {
+				if (dllmin[0] > dlli) {
+					dllmin[0] = dlli;
+				}
+				if (dllmax[0] < dlli) {
+					dllmax[0] = dlli;
+				}
+			}
+			if (data & 0x2) {
+				if (dllmin[1] > dlli) {
+					dllmin[1] = dlli;
+				}
+				if (dllmax[1] < dlli) {
+					dllmax[1] = dlli;
+				}
+			}
+			passcnt++;
+		} else if (passcnt >= CBR_THRESHOLD) {
+			break;
+		}
+	}
+	if (dllmax[0] == 0 || (dllmax[0]-dllmin[0]) < CBR_THRESHOLD) {
+		goto CBR_START2;
+	}
+	if (dllmax[1] == 0 || (dllmax[1]-dllmin[1]) < CBR_THRESHOLD) {
+		goto CBR_START2;
+	}
+	dlli  = (dllmin[1] + dllmax[1]) >> 1;
+	dlli <<= 8;
+	dlli += (dllmin[0] + dllmax[0]) >> 1;
+	moutdwm(ast, 0x1E6E0068, (mindwm(ast, 0x1E6E0068) & 0xFFFF) | (dlli << 16));
+
+	data  = (mindwm(ast, 0x1E6E0080) >> 24) & 0x1F;
+	data2 = (mindwm(ast, 0x1E6E0018) & 0xff80ffff) | (data << 16);
+	moutdwm(ast, 0x1E6E0018, data2);
+	moutdwm(ast, 0x1E6E0024, 0x8001 | (data << 1) | (param->dll2_finetune_step << 8));
+
+	/* Wait DQSI latch phase calibration */
+	moutdwm(ast, 0x1E6E0074, 0x00000010);
+	moutdwm(ast, 0x1E6E0070, 0x00000003);
+	do {
+		data = mindwm(ast, 0x1E6E0070);
+	} while (!(data & 0x00001000));
+	moutdwm(ast, 0x1E6E0070, 0x00000000);
+	moutdwm(ast, 0x1E6E0070, 0x00000003);
+	do {
+		data = mindwm(ast, 0x1E6E0070);
+	} while (!(data & 0x00001000));
+	moutdwm(ast, 0x1E6E0070, 0x00000000);
+} /* CBRDLL2 */
+
+static void get_ddr3_info(struct ast_private *ast, struct ast2300_dram_param *param)
+{
+	u32 trap, trap_AC2, trap_MRS;
+
+	moutdwm(ast, 0x1E6E2000, 0x1688A8A8);
+
+	/* Ger trap info */
+	trap = (mindwm(ast, 0x1E6E2070) >> 25) & 0x3;
+	trap_AC2  = 0x00020000 + (trap << 16);
+	trap_AC2 |= 0x00300000 + ((trap & 0x2) << 19);
+	trap_MRS  = 0x00000010 + (trap << 4);
+	trap_MRS |= ((trap & 0x2) << 18);
+
+	param->reg_MADJ       = 0x00034C4C;
+	param->reg_SADJ       = 0x00001800;
+	param->reg_DRV        = 0x000000F0;
+	param->reg_PERIOD     = param->dram_freq;
+	param->rodt           = 0;
+
+	switch (param->dram_freq) {
+	case 336:
+		moutdwm(ast, 0x1E6E2020, 0x0190);
+		param->wodt          = 0;
+		param->reg_AC1       = 0x22202725;
+		param->reg_AC2       = 0xAA007613 | trap_AC2;
+		param->reg_DQSIC     = 0x000000BA;
+		param->reg_MRS       = 0x04001400 | trap_MRS;
+		param->reg_EMRS      = 0x00000000;
+		param->reg_IOZ       = 0x00000034;
+		param->reg_DQIDLY    = 0x00000074;
+		param->reg_FREQ      = 0x00004DC0;
+		param->madj_max      = 96;
+		param->dll2_finetune_step = 3;
+		break;
+	default:
+	case 396:
+		moutdwm(ast, 0x1E6E2020, 0x03F1);
+		param->wodt          = 1;
+		param->reg_AC1       = 0x33302825;
+		param->reg_AC2       = 0xCC009617 | trap_AC2;
+		param->reg_DQSIC     = 0x000000E2;
+		param->reg_MRS       = 0x04001600 | trap_MRS;
+		param->reg_EMRS      = 0x00000000;
+		param->reg_IOZ       = 0x00000034;
+		param->reg_DRV       = 0x000000FA;
+		param->reg_DQIDLY    = 0x00000089;
+		param->reg_FREQ      = 0x000050C0;
+		param->madj_max      = 96;
+		param->dll2_finetune_step = 4;
+
+		switch (param->dram_chipid) {
+		default:
+		case AST_DRAM_512Mx16:
+		case AST_DRAM_1Gx16:
+			param->reg_AC2   = 0xCC009617 | trap_AC2;
+			break;
+		case AST_DRAM_2Gx16:
+			param->reg_AC2   = 0xCC009622 | trap_AC2;
+			break;
+		case AST_DRAM_4Gx16:
+			param->reg_AC2   = 0xCC00963F | trap_AC2;
+			break;
+		}
+		break;
+
+	case 408:
+		moutdwm(ast, 0x1E6E2020, 0x01F0);
+		param->wodt          = 1;
+		param->reg_AC1       = 0x33302825;
+		param->reg_AC2       = 0xCC009617 | trap_AC2;
+		param->reg_DQSIC     = 0x000000E2;
+		param->reg_MRS       = 0x04001600 | trap_MRS;
+		param->reg_EMRS      = 0x00000000;
+		param->reg_IOZ       = 0x00000034;
+		param->reg_DRV       = 0x000000FA;
+		param->reg_DQIDLY    = 0x00000089;
+		param->reg_FREQ      = 0x000050C0;
+		param->madj_max      = 96;
+		param->dll2_finetune_step = 4;
+
+		switch (param->dram_chipid) {
+		default:
+		case AST_DRAM_512Mx16:
+		case AST_DRAM_1Gx16:
+			param->reg_AC2   = 0xCC009617 | trap_AC2;
+			break;
+		case AST_DRAM_2Gx16:
+			param->reg_AC2   = 0xCC009622 | trap_AC2;
+			break;
+		case AST_DRAM_4Gx16:
+			param->reg_AC2   = 0xCC00963F | trap_AC2;
+			break;
+		}
+
+		break;
+	case 456:
+		moutdwm(ast, 0x1E6E2020, 0x0230);
+		param->wodt          = 0;
+		param->reg_AC1       = 0x33302926;
+		param->reg_AC2       = 0xCD44961A;
+		param->reg_DQSIC     = 0x000000FC;
+		param->reg_MRS       = 0x00081830;
+		param->reg_EMRS      = 0x00000000;
+		param->reg_IOZ       = 0x00000045;
+		param->reg_DQIDLY    = 0x00000097;
+		param->reg_FREQ      = 0x000052C0;
+		param->madj_max      = 88;
+		param->dll2_finetune_step = 4;
+		break;
+	case 504:
+		moutdwm(ast, 0x1E6E2020, 0x0270);
+		param->wodt          = 1;
+		param->reg_AC1       = 0x33302926;
+		param->reg_AC2       = 0xDE44A61D;
+		param->reg_DQSIC     = 0x00000117;
+		param->reg_MRS       = 0x00081A30;
+		param->reg_EMRS      = 0x00000000;
+		param->reg_IOZ       = 0x070000BB;
+		param->reg_DQIDLY    = 0x000000A0;
+		param->reg_FREQ      = 0x000054C0;
+		param->madj_max      = 79;
+		param->dll2_finetune_step = 4;
+		break;
+	case 528:
+		moutdwm(ast, 0x1E6E2020, 0x0290);
+		param->wodt          = 1;
+		param->rodt          = 1;
+		param->reg_AC1       = 0x33302926;
+		param->reg_AC2       = 0xEF44B61E;
+		param->reg_DQSIC     = 0x00000125;
+		param->reg_MRS       = 0x00081A30;
+		param->reg_EMRS      = 0x00000040;
+		param->reg_DRV       = 0x000000F5;
+		param->reg_IOZ       = 0x00000023;
+		param->reg_DQIDLY    = 0x00000088;
+		param->reg_FREQ      = 0x000055C0;
+		param->madj_max      = 76;
+		param->dll2_finetune_step = 3;
+		break;
+	case 576:
+		moutdwm(ast, 0x1E6E2020, 0x0140);
+		param->reg_MADJ      = 0x00136868;
+		param->reg_SADJ      = 0x00004534;
+		param->wodt          = 1;
+		param->rodt          = 1;
+		param->reg_AC1       = 0x33302A37;
+		param->reg_AC2       = 0xEF56B61E;
+		param->reg_DQSIC     = 0x0000013F;
+		param->reg_MRS       = 0x00101A50;
+		param->reg_EMRS      = 0x00000040;
+		param->reg_DRV       = 0x000000FA;
+		param->reg_IOZ       = 0x00000023;
+		param->reg_DQIDLY    = 0x00000078;
+		param->reg_FREQ      = 0x000057C0;
+		param->madj_max      = 136;
+		param->dll2_finetune_step = 3;
+		break;
+	case 600:
+		moutdwm(ast, 0x1E6E2020, 0x02E1);
+		param->reg_MADJ      = 0x00136868;
+		param->reg_SADJ      = 0x00004534;
+		param->wodt          = 1;
+		param->rodt          = 1;
+		param->reg_AC1       = 0x32302A37;
+		param->reg_AC2       = 0xDF56B61F;
+		param->reg_DQSIC     = 0x0000014D;
+		param->reg_MRS       = 0x00101A50;
+		param->reg_EMRS      = 0x00000004;
+		param->reg_DRV       = 0x000000F5;
+		param->reg_IOZ       = 0x00000023;
+		param->reg_DQIDLY    = 0x00000078;
+		param->reg_FREQ      = 0x000058C0;
+		param->madj_max      = 132;
+		param->dll2_finetune_step = 3;
+		break;
+	case 624:
+		moutdwm(ast, 0x1E6E2020, 0x0160);
+		param->reg_MADJ      = 0x00136868;
+		param->reg_SADJ      = 0x00004534;
+		param->wodt          = 1;
+		param->rodt          = 1;
+		param->reg_AC1       = 0x32302A37;
+		param->reg_AC2       = 0xEF56B621;
+		param->reg_DQSIC     = 0x0000015A;
+		param->reg_MRS       = 0x02101A50;
+		param->reg_EMRS      = 0x00000004;
+		param->reg_DRV       = 0x000000F5;
+		param->reg_IOZ       = 0x00000034;
+		param->reg_DQIDLY    = 0x00000078;
+		param->reg_FREQ      = 0x000059C0;
+		param->madj_max      = 128;
+		param->dll2_finetune_step = 3;
+		break;
+	} /* switch freq */
+
+	switch (param->dram_chipid) {
+	case AST_DRAM_512Mx16:
+		param->dram_config = 0x130;
+		break;
+	default:
+	case AST_DRAM_1Gx16:
+		param->dram_config = 0x131;
+		break;
+	case AST_DRAM_2Gx16:
+		param->dram_config = 0x132;
+		break;
+	case AST_DRAM_4Gx16:
+		param->dram_config = 0x133;
+		break;
+	}; /* switch size */
+
+	switch (param->vram_size) {
+	default:
+	case AST_VIDMEM_SIZE_8M:
+		param->dram_config |= 0x00;
+		break;
+	case AST_VIDMEM_SIZE_16M:
+		param->dram_config |= 0x04;
+		break;
+	case AST_VIDMEM_SIZE_32M:
+		param->dram_config |= 0x08;
+		break;
+	case AST_VIDMEM_SIZE_64M:
+		param->dram_config |= 0x0c;
+		break;
+	}
+
+}
+
+static void ddr3_init(struct ast_private *ast, struct ast2300_dram_param *param)
+{
+	u32 data, data2;
+
+	moutdwm(ast, 0x1E6E0000, 0xFC600309);
+	moutdwm(ast, 0x1E6E0018, 0x00000100);
+	moutdwm(ast, 0x1E6E0024, 0x00000000);
+	moutdwm(ast, 0x1E6E0034, 0x00000000);
+	udelay(10);
+	moutdwm(ast, 0x1E6E0064, param->reg_MADJ);
+	moutdwm(ast, 0x1E6E0068, param->reg_SADJ);
+	udelay(10);
+	moutdwm(ast, 0x1E6E0064, param->reg_MADJ | 0xC0000);
+	udelay(10);
+
+	moutdwm(ast, 0x1E6E0004, param->dram_config);
+	moutdwm(ast, 0x1E6E0008, 0x90040f);
+	moutdwm(ast, 0x1E6E0010, param->reg_AC1);
+	moutdwm(ast, 0x1E6E0014, param->reg_AC2);
+	moutdwm(ast, 0x1E6E0020, param->reg_DQSIC);
+	moutdwm(ast, 0x1E6E0080, 0x00000000);
+	moutdwm(ast, 0x1E6E0084, 0x00000000);
+	moutdwm(ast, 0x1E6E0088, param->reg_DQIDLY);
+	moutdwm(ast, 0x1E6E0018, 0x4040A170);
+	moutdwm(ast, 0x1E6E0018, 0x20402370);
+	moutdwm(ast, 0x1E6E0038, 0x00000000);
+	moutdwm(ast, 0x1E6E0040, 0xFF444444);
+	moutdwm(ast, 0x1E6E0044, 0x22222222);
+	moutdwm(ast, 0x1E6E0048, 0x22222222);
+	moutdwm(ast, 0x1E6E004C, 0x00000002);
+	moutdwm(ast, 0x1E6E0050, 0x80000000);
+	moutdwm(ast, 0x1E6E0050, 0x00000000);
+	moutdwm(ast, 0x1E6E0054, 0);
+	moutdwm(ast, 0x1E6E0060, param->reg_DRV);
+	moutdwm(ast, 0x1E6E006C, param->reg_IOZ);
+	moutdwm(ast, 0x1E6E0070, 0x00000000);
+	moutdwm(ast, 0x1E6E0074, 0x00000000);
+	moutdwm(ast, 0x1E6E0078, 0x00000000);
+	moutdwm(ast, 0x1E6E007C, 0x00000000);
+	/* Wait MCLK2X lock to MCLK */
+	do {
+		data = mindwm(ast, 0x1E6E001C);
+	} while (!(data & 0x08000000));
+	moutdwm(ast, 0x1E6E0034, 0x00000001);
+	moutdwm(ast, 0x1E6E000C, 0x00005C04);
+	udelay(10);
+	moutdwm(ast, 0x1E6E000C, 0x00000000);
+	moutdwm(ast, 0x1E6E0034, 0x00000000);
+	data = mindwm(ast, 0x1E6E001C);
+	data = (data >> 8) & 0xff;
+	while ((data & 0x08) || ((data & 0x7) < 2) || (data < 4)) {
+		data2 = (mindwm(ast, 0x1E6E0064) & 0xfff3ffff) + 4;
+		if ((data2 & 0xff) > param->madj_max) {
+			break;
+		}
+		moutdwm(ast, 0x1E6E0064, data2);
+		if (data2 & 0x00100000) {
+			data2 = ((data2 & 0xff) >> 3) + 3;
+		} else {
+			data2 = ((data2 & 0xff) >> 2) + 5;
+		}
+		data = mindwm(ast, 0x1E6E0068) & 0xffff00ff;
+		data2 += data & 0xff;
+		data = data | (data2 << 8);
+		moutdwm(ast, 0x1E6E0068, data);
+		udelay(10);
+		moutdwm(ast, 0x1E6E0064, mindwm(ast, 0x1E6E0064) | 0xC0000);
+		udelay(10);
+		data = mindwm(ast, 0x1E6E0018) & 0xfffff1ff;
+		moutdwm(ast, 0x1E6E0018, data);
+		data = data | 0x200;
+		moutdwm(ast, 0x1E6E0018, data);
+		do {
+			data = mindwm(ast, 0x1E6E001C);
+		} while (!(data & 0x08000000));
+
+		moutdwm(ast, 0x1E6E0034, 0x00000001);
+		moutdwm(ast, 0x1E6E000C, 0x00005C04);
+		udelay(10);
+		moutdwm(ast, 0x1E6E000C, 0x00000000);
+		moutdwm(ast, 0x1E6E0034, 0x00000000);
+		data = mindwm(ast, 0x1E6E001C);
+		data = (data >> 8) & 0xff;
+	}
+	data = mindwm(ast, 0x1E6E0018) | 0xC00;
+	moutdwm(ast, 0x1E6E0018, data);
+
+	moutdwm(ast, 0x1E6E0034, 0x00000001);
+	moutdwm(ast, 0x1E6E000C, 0x00000040);
+	udelay(50);
+	/* Mode Register Setting */
+	moutdwm(ast, 0x1E6E002C, param->reg_MRS | 0x100);
+	moutdwm(ast, 0x1E6E0030, param->reg_EMRS);
+	moutdwm(ast, 0x1E6E0028, 0x00000005);
+	moutdwm(ast, 0x1E6E0028, 0x00000007);
+	moutdwm(ast, 0x1E6E0028, 0x00000003);
+	moutdwm(ast, 0x1E6E0028, 0x00000001);
+	moutdwm(ast, 0x1E6E002C, param->reg_MRS);
+	moutdwm(ast, 0x1E6E000C, 0x00005C08);
+	moutdwm(ast, 0x1E6E0028, 0x00000001);
+
+	moutdwm(ast, 0x1E6E000C, 0x7FFF5C01);
+	data = 0;
+	if (param->wodt) {
+		data = 0x300;
+	}
+	if (param->rodt) {
+		data = data | 0x3000 | ((param->reg_AC2 & 0x60000) >> 3);
+	}
+	moutdwm(ast, 0x1E6E0034, data | 0x3);
+
+	/* Wait DQI delay lock */
+	do {
+		data = mindwm(ast, 0x1E6E0080);
+	} while (!(data & 0x40000000));
+	/* Wait DQSI delay lock */
+	do {
+		data = mindwm(ast, 0x1E6E0020);
+	} while (!(data & 0x00000800));
+	/* Calibrate the DQSI delay */
+	cbr_dll2(ast, param);
+
+	moutdwm(ast, 0x1E6E0120, param->reg_FREQ);
+	/* ECC Memory Initialization */
+#ifdef ECC
+	moutdwm(ast, 0x1E6E007C, 0x00000000);
+	moutdwm(ast, 0x1E6E0070, 0x221);
+	do {
+		data = mindwm(ast, 0x1E6E0070);
+	} while (!(data & 0x00001000));
+	moutdwm(ast, 0x1E6E0070, 0x00000000);
+	moutdwm(ast, 0x1E6E0050, 0x80000000);
+	moutdwm(ast, 0x1E6E0050, 0x00000000);
+#endif
+
+
+}
+
+static void get_ddr2_info(struct ast_private *ast, struct ast2300_dram_param *param)
+{
+	u32 trap, trap_AC2, trap_MRS;
+
+	moutdwm(ast, 0x1E6E2000, 0x1688A8A8);
+
+	/* Ger trap info */
+	trap = (mindwm(ast, 0x1E6E2070) >> 25) & 0x3;
+	trap_AC2  = (trap << 20) | (trap << 16);
+	trap_AC2 += 0x00110000;
+	trap_MRS  = 0x00000040 | (trap << 4);
+
+
+	param->reg_MADJ       = 0x00034C4C;
+	param->reg_SADJ       = 0x00001800;
+	param->reg_DRV        = 0x000000F0;
+	param->reg_PERIOD     = param->dram_freq;
+	param->rodt           = 0;
+
+	switch (param->dram_freq) {
+	case 264:
+		moutdwm(ast, 0x1E6E2020, 0x0130);
+		param->wodt          = 0;
+		param->reg_AC1       = 0x11101513;
+		param->reg_AC2       = 0x78117011;
+		param->reg_DQSIC     = 0x00000092;
+		param->reg_MRS       = 0x00000842;
+		param->reg_EMRS      = 0x00000000;
+		param->reg_DRV       = 0x000000F0;
+		param->reg_IOZ       = 0x00000034;
+		param->reg_DQIDLY    = 0x0000005A;
+		param->reg_FREQ      = 0x00004AC0;
+		param->madj_max      = 138;
+		param->dll2_finetune_step = 3;
+		break;
+	case 336:
+		moutdwm(ast, 0x1E6E2020, 0x0190);
+		param->wodt          = 1;
+		param->reg_AC1       = 0x22202613;
+		param->reg_AC2       = 0xAA009016 | trap_AC2;
+		param->reg_DQSIC     = 0x000000BA;
+		param->reg_MRS       = 0x00000A02 | trap_MRS;
+		param->reg_EMRS      = 0x00000040;
+		param->reg_DRV       = 0x000000FA;
+		param->reg_IOZ       = 0x00000034;
+		param->reg_DQIDLY    = 0x00000074;
+		param->reg_FREQ      = 0x00004DC0;
+		param->madj_max      = 96;
+		param->dll2_finetune_step = 3;
+		break;
+	default:
+	case 396:
+		moutdwm(ast, 0x1E6E2020, 0x03F1);
+		param->wodt          = 1;
+		param->rodt          = 0;
+		param->reg_AC1       = 0x33302714;
+		param->reg_AC2       = 0xCC00B01B | trap_AC2;
+		param->reg_DQSIC     = 0x000000E2;
+		param->reg_MRS       = 0x00000C02 | trap_MRS;
+		param->reg_EMRS      = 0x00000040;
+		param->reg_DRV       = 0x000000FA;
+		param->reg_IOZ       = 0x00000034;
+		param->reg_DQIDLY    = 0x00000089;
+		param->reg_FREQ      = 0x000050C0;
+		param->madj_max      = 96;
+		param->dll2_finetune_step = 4;
+
+		switch (param->dram_chipid) {
+		case AST_DRAM_512Mx16:
+			param->reg_AC2   = 0xCC00B016 | trap_AC2;
+			break;
+		default:
+		case AST_DRAM_1Gx16:
+			param->reg_AC2   = 0xCC00B01B | trap_AC2;
+			break;
+		case AST_DRAM_2Gx16:
+			param->reg_AC2   = 0xCC00B02B | trap_AC2;
+			break;
+		case AST_DRAM_4Gx16:
+			param->reg_AC2   = 0xCC00B03F | trap_AC2;
+			break;
+		}
+
+		break;
+
+	case 408:
+		moutdwm(ast, 0x1E6E2020, 0x01F0);
+		param->wodt          = 1;
+		param->rodt          = 0;
+		param->reg_AC1       = 0x33302714;
+		param->reg_AC2       = 0xCC00B01B | trap_AC2;
+		param->reg_DQSIC     = 0x000000E2;
+		param->reg_MRS       = 0x00000C02 | trap_MRS;
+		param->reg_EMRS      = 0x00000040;
+		param->reg_DRV       = 0x000000FA;
+		param->reg_IOZ       = 0x00000034;
+		param->reg_DQIDLY    = 0x00000089;
+		param->reg_FREQ      = 0x000050C0;
+		param->madj_max      = 96;
+		param->dll2_finetune_step = 4;
+
+		switch (param->dram_chipid) {
+		case AST_DRAM_512Mx16:
+			param->reg_AC2   = 0xCC00B016 | trap_AC2;
+			break;
+		default:
+		case AST_DRAM_1Gx16:
+			param->reg_AC2   = 0xCC00B01B | trap_AC2;
+			break;
+		case AST_DRAM_2Gx16:
+			param->reg_AC2   = 0xCC00B02B | trap_AC2;
+			break;
+		case AST_DRAM_4Gx16:
+			param->reg_AC2   = 0xCC00B03F | trap_AC2;
+			break;
+		}
+
+		break;
+	case 456:
+		moutdwm(ast, 0x1E6E2020, 0x0230);
+		param->wodt          = 0;
+		param->reg_AC1       = 0x33302815;
+		param->reg_AC2       = 0xCD44B01E;
+		param->reg_DQSIC     = 0x000000FC;
+		param->reg_MRS       = 0x00000E72;
+		param->reg_EMRS      = 0x00000000;
+		param->reg_DRV       = 0x00000000;
+		param->reg_IOZ       = 0x00000034;
+		param->reg_DQIDLY    = 0x00000097;
+		param->reg_FREQ      = 0x000052C0;
+		param->madj_max      = 88;
+		param->dll2_finetune_step = 3;
+		break;
+	case 504:
+		moutdwm(ast, 0x1E6E2020, 0x0261);
+		param->wodt          = 1;
+		param->rodt          = 1;
+		param->reg_AC1       = 0x33302815;
+		param->reg_AC2       = 0xDE44C022;
+		param->reg_DQSIC     = 0x00000117;
+		param->reg_MRS       = 0x00000E72;
+		param->reg_EMRS      = 0x00000040;
+		param->reg_DRV       = 0x0000000A;
+		param->reg_IOZ       = 0x00000045;
+		param->reg_DQIDLY    = 0x000000A0;
+		param->reg_FREQ      = 0x000054C0;
+		param->madj_max      = 79;
+		param->dll2_finetune_step = 3;
+		break;
+	case 528:
+		moutdwm(ast, 0x1E6E2020, 0x0120);
+		param->wodt          = 1;
+		param->rodt          = 1;
+		param->reg_AC1       = 0x33302815;
+		param->reg_AC2       = 0xEF44D024;
+		param->reg_DQSIC     = 0x00000125;
+		param->reg_MRS       = 0x00000E72;
+		param->reg_EMRS      = 0x00000004;
+		param->reg_DRV       = 0x000000F9;
+		param->reg_IOZ       = 0x00000045;
+		param->reg_DQIDLY    = 0x000000A7;
+		param->reg_FREQ      = 0x000055C0;
+		param->madj_max      = 76;
+		param->dll2_finetune_step = 3;
+		break;
+	case 552:
+		moutdwm(ast, 0x1E6E2020, 0x02A1);
+		param->wodt          = 1;
+		param->rodt          = 1;
+		param->reg_AC1       = 0x43402915;
+		param->reg_AC2       = 0xFF44E025;
+		param->reg_DQSIC     = 0x00000132;
+		param->reg_MRS       = 0x00000E72;
+		param->reg_EMRS      = 0x00000040;
+		param->reg_DRV       = 0x0000000A;
+		param->reg_IOZ       = 0x00000045;
+		param->reg_DQIDLY    = 0x000000AD;
+		param->reg_FREQ      = 0x000056C0;
+		param->madj_max      = 76;
+		param->dll2_finetune_step = 3;
+		break;
+	case 576:
+		moutdwm(ast, 0x1E6E2020, 0x0140);
+		param->wodt          = 1;
+		param->rodt          = 1;
+		param->reg_AC1       = 0x43402915;
+		param->reg_AC2       = 0xFF44E027;
+		param->reg_DQSIC     = 0x0000013F;
+		param->reg_MRS       = 0x00000E72;
+		param->reg_EMRS      = 0x00000004;
+		param->reg_DRV       = 0x000000F5;
+		param->reg_IOZ       = 0x00000045;
+		param->reg_DQIDLY    = 0x000000B3;
+		param->reg_FREQ      = 0x000057C0;
+		param->madj_max      = 76;
+		param->dll2_finetune_step = 3;
+		break;
+	}
+
+	switch (param->dram_chipid) {
+	case AST_DRAM_512Mx16:
+		param->dram_config = 0x100;
+		break;
+	default:
+	case AST_DRAM_1Gx16:
+		param->dram_config = 0x121;
+		break;
+	case AST_DRAM_2Gx16:
+		param->dram_config = 0x122;
+		break;
+	case AST_DRAM_4Gx16:
+		param->dram_config = 0x123;
+		break;
+	}; /* switch size */
+
+	switch (param->vram_size) {
+	default:
+	case AST_VIDMEM_SIZE_8M:
+		param->dram_config |= 0x00;
+		break;
+	case AST_VIDMEM_SIZE_16M:
+		param->dram_config |= 0x04;
+		break;
+	case AST_VIDMEM_SIZE_32M:
+		param->dram_config |= 0x08;
+		break;
+	case AST_VIDMEM_SIZE_64M:
+		param->dram_config |= 0x0c;
+		break;
+	}
+}
+
+static void ddr2_init(struct ast_private *ast, struct ast2300_dram_param *param)
+{
+	u32 data, data2;
+
+	moutdwm(ast, 0x1E6E0000, 0xFC600309);
+	moutdwm(ast, 0x1E6E0018, 0x00000100);
+	moutdwm(ast, 0x1E6E0024, 0x00000000);
+	moutdwm(ast, 0x1E6E0064, param->reg_MADJ);
+	moutdwm(ast, 0x1E6E0068, param->reg_SADJ);
+	udelay(10);
+	moutdwm(ast, 0x1E6E0064, param->reg_MADJ | 0xC0000);
+	udelay(10);
+
+	moutdwm(ast, 0x1E6E0004, param->dram_config);
+	moutdwm(ast, 0x1E6E0008, 0x90040f);
+	moutdwm(ast, 0x1E6E0010, param->reg_AC1);
+	moutdwm(ast, 0x1E6E0014, param->reg_AC2);
+	moutdwm(ast, 0x1E6E0020, param->reg_DQSIC);
+	moutdwm(ast, 0x1E6E0080, 0x00000000);
+	moutdwm(ast, 0x1E6E0084, 0x00000000);
+	moutdwm(ast, 0x1E6E0088, param->reg_DQIDLY);
+	moutdwm(ast, 0x1E6E0018, 0x4040A130);
+	moutdwm(ast, 0x1E6E0018, 0x20402330);
+	moutdwm(ast, 0x1E6E0038, 0x00000000);
+	moutdwm(ast, 0x1E6E0040, 0xFF808000);
+	moutdwm(ast, 0x1E6E0044, 0x88848466);
+	moutdwm(ast, 0x1E6E0048, 0x44440008);
+	moutdwm(ast, 0x1E6E004C, 0x00000000);
+	moutdwm(ast, 0x1E6E0050, 0x80000000);
+	moutdwm(ast, 0x1E6E0050, 0x00000000);
+	moutdwm(ast, 0x1E6E0054, 0);
+	moutdwm(ast, 0x1E6E0060, param->reg_DRV);
+	moutdwm(ast, 0x1E6E006C, param->reg_IOZ);
+	moutdwm(ast, 0x1E6E0070, 0x00000000);
+	moutdwm(ast, 0x1E6E0074, 0x00000000);
+	moutdwm(ast, 0x1E6E0078, 0x00000000);
+	moutdwm(ast, 0x1E6E007C, 0x00000000);
+
+	/* Wait MCLK2X lock to MCLK */
+	do {
+		data = mindwm(ast, 0x1E6E001C);
+	} while (!(data & 0x08000000));
+	moutdwm(ast, 0x1E6E0034, 0x00000001);
+	moutdwm(ast, 0x1E6E000C, 0x00005C04);
+	udelay(10);
+	moutdwm(ast, 0x1E6E000C, 0x00000000);
+	moutdwm(ast, 0x1E6E0034, 0x00000000);
+	data = mindwm(ast, 0x1E6E001C);
+	data = (data >> 8) & 0xff;
+	while ((data & 0x08) || ((data & 0x7) < 2) || (data < 4)) {
+		data2 = (mindwm(ast, 0x1E6E0064) & 0xfff3ffff) + 4;
+		if ((data2 & 0xff) > param->madj_max) {
+			break;
+		}
+		moutdwm(ast, 0x1E6E0064, data2);
+		if (data2 & 0x00100000) {
+			data2 = ((data2 & 0xff) >> 3) + 3;
+		} else {
+			data2 = ((data2 & 0xff) >> 2) + 5;
+		}
+		data = mindwm(ast, 0x1E6E0068) & 0xffff00ff;
+		data2 += data & 0xff;
+		data = data | (data2 << 8);
+		moutdwm(ast, 0x1E6E0068, data);
+		udelay(10);
+		moutdwm(ast, 0x1E6E0064, mindwm(ast, 0x1E6E0064) | 0xC0000);
+		udelay(10);
+		data = mindwm(ast, 0x1E6E0018) & 0xfffff1ff;
+		moutdwm(ast, 0x1E6E0018, data);
+		data = data | 0x200;
+		moutdwm(ast, 0x1E6E0018, data);
+		do {
+			data = mindwm(ast, 0x1E6E001C);
+		} while (!(data & 0x08000000));
+
+		moutdwm(ast, 0x1E6E0034, 0x00000001);
+		moutdwm(ast, 0x1E6E000C, 0x00005C04);
+		udelay(10);
+		moutdwm(ast, 0x1E6E000C, 0x00000000);
+		moutdwm(ast, 0x1E6E0034, 0x00000000);
+		data = mindwm(ast, 0x1E6E001C);
+		data = (data >> 8) & 0xff;
+	}
+	data = mindwm(ast, 0x1E6E0018) | 0xC00;
+	moutdwm(ast, 0x1E6E0018, data);
+
+	moutdwm(ast, 0x1E6E0034, 0x00000001);
+	moutdwm(ast, 0x1E6E000C, 0x00000000);
+	udelay(50);
+	/* Mode Register Setting */
+	moutdwm(ast, 0x1E6E002C, param->reg_MRS | 0x100);
+	moutdwm(ast, 0x1E6E0030, param->reg_EMRS);
+	moutdwm(ast, 0x1E6E0028, 0x00000005);
+	moutdwm(ast, 0x1E6E0028, 0x00000007);
+	moutdwm(ast, 0x1E6E0028, 0x00000003);
+	moutdwm(ast, 0x1E6E0028, 0x00000001);
+
+	moutdwm(ast, 0x1E6E000C, 0x00005C08);
+	moutdwm(ast, 0x1E6E002C, param->reg_MRS);
+	moutdwm(ast, 0x1E6E0028, 0x00000001);
+	moutdwm(ast, 0x1E6E0030, param->reg_EMRS | 0x380);
+	moutdwm(ast, 0x1E6E0028, 0x00000003);
+	moutdwm(ast, 0x1E6E0030, param->reg_EMRS);
+	moutdwm(ast, 0x1E6E0028, 0x00000003);
+
+	moutdwm(ast, 0x1E6E000C, 0x7FFF5C01);
+	data = 0;
+	if (param->wodt) {
+		data = 0x500;
+	}
+	if (param->rodt) {
+		data = data | 0x3000 | ((param->reg_AC2 & 0x60000) >> 3);
+	}
+	moutdwm(ast, 0x1E6E0034, data | 0x3);
+	moutdwm(ast, 0x1E6E0120, param->reg_FREQ);
+
+	/* Wait DQI delay lock */
+	do {
+		data = mindwm(ast, 0x1E6E0080);
+	} while (!(data & 0x40000000));
+	/* Wait DQSI delay lock */
+	do {
+		data = mindwm(ast, 0x1E6E0020);
+	} while (!(data & 0x00000800));
+	/* Calibrate the DQSI delay */
+	cbr_dll2(ast, param);
+
+	/* ECC Memory Initialization */
+#ifdef ECC
+	moutdwm(ast, 0x1E6E007C, 0x00000000);
+	moutdwm(ast, 0x1E6E0070, 0x221);
+	do {
+		data = mindwm(ast, 0x1E6E0070);
+	} while (!(data & 0x00001000));
+	moutdwm(ast, 0x1E6E0070, 0x00000000);
+	moutdwm(ast, 0x1E6E0050, 0x80000000);
+	moutdwm(ast, 0x1E6E0050, 0x00000000);
+#endif
+
+}
+
+static void ast_init_dram_2300(struct drm_device *dev)
+{
+	struct ast_private *ast = dev->dev_private;
+	struct ast2300_dram_param param;
+	u32 temp;
+	u8 reg;
+
+	reg = ast_get_index_reg_mask(ast, AST_IO_CRTC_PORT, 0xd0, 0xff);
+	if ((reg & 0x80) == 0) {/* vga only */
+		ast_write32(ast, 0xf004, 0x1e6e0000);
+		ast_write32(ast, 0xf000, 0x1);
+		ast_write32(ast, 0x12000, 0x1688a8a8);
+		do {
+			;
+		} while (ast_read32(ast, 0x12000) != 0x1);
+
+		ast_write32(ast, 0x10000, 0xfc600309);
+		do {
+			;
+		} while (ast_read32(ast, 0x10000) != 0x1);
+
+		/* Slow down CPU/AHB CLK in VGA only mode */
+		temp = ast_read32(ast, 0x12008);
+		temp |= 0x73;
+		ast_write32(ast, 0x12008, temp);
+
+		param.dram_type = AST_DDR3;
+		if (temp & 0x01000000)
+			param.dram_type = AST_DDR2;
+		param.dram_chipid = ast->dram_type;
+		param.dram_freq = ast->mclk;
+		param.vram_size = ast->vram_size;
+
+		if (param.dram_type == AST_DDR3) {
+			get_ddr3_info(ast, &param);
+			ddr3_init(ast, &param);
+		} else {
+			get_ddr2_info(ast, &param);
+			ddr2_init(ast, &param);
+		}
+
+		temp = mindwm(ast, 0x1e6e2040);
+		moutdwm(ast, 0x1e6e2040, temp | 0x40);
+	}
+
+	/* wait ready */
+	do {
+		reg = ast_get_index_reg_mask(ast, AST_IO_CRTC_PORT, 0xd0, 0xff);
+	} while ((reg & 0x40) == 0);
+}
+
diff --git a/drivers/gpu/drm/ast/ast_tables.h b/drivers/gpu/drm/ast/ast_tables.h
new file mode 100644
index 00000000000..95fa6aba26b
--- /dev/null
+++ b/drivers/gpu/drm/ast/ast_tables.h
@@ -0,0 +1,265 @@
+/*
+ * Copyright (c) 2005 ASPEED Technology Inc.
+ *
+ * Permission to use, copy, modify, distribute, and sell this software and its
+ * documentation for any purpose is hereby granted without fee, provided that
+ * the above copyright notice appear in all copies and that both that
+ * copyright notice and this permission notice appear in supporting
+ * documentation, and that the name of the authors not be used in
+ * advertising or publicity pertaining to distribution of the software without
+ * specific, written prior permission.  The authors makes no representations
+ * about the suitability of this software for any purpose.  It is provided
+ * "as is" without express or implied warranty.
+ *
+ * THE AUTHORS DISCLAIMS ALL WARRANTIES WITH REGARD TO THIS SOFTWARE,
+ * INCLUDING ALL IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS, IN NO
+ * EVENT SHALL THE AUTHORS BE LIABLE FOR ANY SPECIAL, 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.
+ */
+/* Ported from xf86-video-ast driver */
+
+#ifndef AST_TABLES_H
+#define AST_TABLES_H
+
+/* Std. Table Index Definition */
+#define TextModeIndex		0
+#define EGAModeIndex		1
+#define VGAModeIndex		2
+#define HiCModeIndex		3
+#define TrueCModeIndex		4
+
+#define Charx8Dot               0x00000001
+#define HalfDCLK                0x00000002
+#define DoubleScanMode          0x00000004
+#define LineCompareOff          0x00000008
+#define SyncPP                  0x00000000
+#define SyncPN                  0x00000040
+#define SyncNP                  0x00000080
+#define SyncNN                  0x000000C0
+#define HBorder                 0x00000020
+#define VBorder                 0x00000010
+#define WideScreenMode		0x00000100
+
+
+/* DCLK Index */
+#define VCLK25_175     		0x00
+#define VCLK28_322     		0x01
+#define VCLK31_5       		0x02
+#define VCLK36         		0x03
+#define VCLK40         		0x04
+#define VCLK49_5       		0x05
+#define VCLK50         		0x06
+#define VCLK56_25      		0x07
+#define VCLK65		 	0x08
+#define VCLK75	        	0x09
+#define VCLK78_75      		0x0A
+#define VCLK94_5       		0x0B
+#define VCLK108        		0x0C
+#define VCLK135        		0x0D
+#define VCLK157_5      		0x0E
+#define VCLK162        		0x0F
+/* #define VCLK193_25     		0x10 */
+#define VCLK154     		0x10
+#define VCLK83_5    		0x11
+#define VCLK106_5   		0x12
+#define VCLK146_25  		0x13
+#define VCLK148_5   		0x14
+
+static struct ast_vbios_dclk_info dclk_table[] = {
+	{0x2C, 0xE7, 0x03},					/* 00: VCLK25_175	*/
+	{0x95, 0x62, 0x03},				        /* 01: VCLK28_322	*/
+	{0x67, 0x63, 0x01},				        /* 02: VCLK31_5         */
+	{0x76, 0x63, 0x01},				        /* 03: VCLK36         	*/
+	{0xEE, 0x67, 0x01},				        /* 04: VCLK40          	*/
+	{0x82, 0x62, 0x01}, 			        /* 05: VCLK49_5        	*/
+	{0xC6, 0x64, 0x01},                        	        /* 06: VCLK50          	*/
+	{0x94, 0x62, 0x01},                        	        /* 07: VCLK56_25       	*/
+	{0x80, 0x64, 0x00},                        	        /* 08: VCLK65		*/
+	{0x7B, 0x63, 0x00},                        	        /* 09: VCLK75	        */
+	{0x67, 0x62, 0x00},				        /* 0A: VCLK78_75       	*/
+	{0x7C, 0x62, 0x00},                        	        /* 0B: VCLK94_5        	*/
+	{0x8E, 0x62, 0x00},                        	        /* 0C: VCLK108         	*/
+	{0x85, 0x24, 0x00},                        	        /* 0D: VCLK135         	*/
+	{0x67, 0x22, 0x00},                        	        /* 0E: VCLK157_5       	*/
+	{0x6A, 0x22, 0x00},				        /* 0F: VCLK162         	*/
+	{0x4d, 0x4c, 0x80},				        /* 10: VCLK154      	*/
+	{0xa7, 0x78, 0x80},					/* 11: VCLK83.5         */
+	{0x28, 0x49, 0x80},					/* 12: VCLK106.5        */
+	{0x37, 0x49, 0x80},					/* 13: VCLK146.25       */
+	{0x1f, 0x45, 0x80},					/* 14: VCLK148.5        */
+};
+
+static struct ast_vbios_stdtable vbios_stdtable[] = {
+	/* MD_2_3_400 */
+	{
+		0x67,
+		{0x00,0x03,0x00,0x02},
+		{0x5f,0x4f,0x50,0x82,0x55,0x81,0xbf,0x1f,
+		 0x00,0x4f,0x0d,0x0e,0x00,0x00,0x00,0x00,
+		 0x9c,0x8e,0x8f,0x28,0x1f,0x96,0xb9,0xa3,
+		 0xff},
+		{0x00,0x01,0x02,0x03,0x04,0x05,0x14,0x07,
+		 0x38,0x39,0x3a,0x3b,0x3c,0x3d,0x3e,0x3f,
+		 0x0c,0x00,0x0f,0x08},
+		{0x00,0x00,0x00,0x00,0x00,0x10,0x0e,0x00,
+		 0xff}
+	},
+	/* Mode12/ExtEGATable */
+	{
+		0xe3,
+		{0x01,0x0f,0x00,0x06},
+		{0x5f,0x4f,0x50,0x82,0x55,0x81,0x0b,0x3e,
+		 0x00,0x40,0x00,0x00,0x00,0x00,0x00,0x00,
+		 0xe9,0x8b,0xdf,0x28,0x00,0xe7,0x04,0xe3,
+		 0xff},
+		{0x00,0x01,0x02,0x03,0x04,0x05,0x14,0x07,
+		 0x38,0x39,0x3a,0x3b,0x3c,0x3d,0x3e,0x3f,
+		 0x01,0x00,0x0f,0x00},
+		{0x00,0x00,0x00,0x00,0x00,0x00,0x05,0x0f,
+		 0xff}
+	},
+	/* ExtVGATable */
+	{
+		0x2f,
+		{0x01,0x0f,0x00,0x0e},
+		{0x5f,0x4f,0x50,0x82,0x54,0x80,0x0b,0x3e,
+		 0x00,0x40,0x00,0x00,0x00,0x00,0x00,0x00,
+		 0xea,0x8c,0xdf,0x28,0x40,0xe7,0x04,0xa3,
+		 0xff},
+		{0x00,0x01,0x02,0x03,0x04,0x05,0x06,0x07,
+		 0x08,0x09,0x0a,0x0b,0x0c,0x0d,0x0e,0x0f,
+		 0x01,0x00,0x00,0x00},
+		{0x00,0x00,0x00,0x00,0x00,0x40,0x05,0x0f,
+		 0xff}
+	},
+	/* ExtHiCTable */
+	{
+		0x2f,
+		{0x01,0x0f,0x00,0x0e},
+		{0x5f,0x4f,0x50,0x82,0x54,0x80,0x0b,0x3e,
+		 0x00,0x40,0x00,0x00,0x00,0x00,0x00,0x00,
+		 0xea,0x8c,0xdf,0x28,0x40,0xe7,0x04,0xa3,
+		 0xff},
+		{0x00,0x01,0x02,0x03,0x04,0x05,0x06,0x07,
+		 0x08,0x09,0x0a,0x0b,0x0c,0x0d,0x0e,0x0f,
+		 0x01,0x00,0x00,0x00},
+		{0x00,0x00,0x00,0x00,0x00,0x00,0x05,0x0f,
+		 0xff}
+	},
+	/* ExtTrueCTable */
+	{
+		0x2f,
+		{0x01,0x0f,0x00,0x0e},
+		{0x5f,0x4f,0x50,0x82,0x54,0x80,0x0b,0x3e,
+		 0x00,0x40,0x00,0x00,0x00,0x00,0x00,0x00,
+		 0xea,0x8c,0xdf,0x28,0x40,0xe7,0x04,0xa3,
+		 0xff},
+		{0x00,0x01,0x02,0x03,0x04,0x05,0x06,0x07,
+		 0x08,0x09,0x0a,0x0b,0x0c,0x0d,0x0e,0x0f,
+		 0x01,0x00,0x00,0x00},
+		{0x00,0x00,0x00,0x00,0x00,0x00,0x05,0x0f,
+		 0xff}
+	},
+};
+
+static struct ast_vbios_enhtable res_640x480[] = {
+	{ 800, 640, 8, 96, 525, 480, 2, 2, VCLK25_175,	/* 60Hz */
+	  (SyncNN | HBorder | VBorder | Charx8Dot), 60, 1, 0x2E },
+	{ 832, 640, 16, 40, 520, 480, 1, 3, VCLK31_5,	/* 72Hz */
+	  (SyncNN | HBorder | VBorder | Charx8Dot), 72, 2, 0x2E  },
+	{ 840, 640, 16, 64, 500, 480, 1, 3, VCLK31_5,	/* 75Hz */
+	  (SyncNN | Charx8Dot) , 75, 3, 0x2E },
+	{ 832, 640, 56, 56, 509, 480, 1, 3, VCLK36,		/* 85Hz */
+	  (SyncNN | Charx8Dot) , 85, 4, 0x2E },
+	{ 832, 640, 56, 56, 509, 480, 1, 3, VCLK36,		/* end */
+	  (SyncNN | Charx8Dot) , 0xFF, 4, 0x2E },
+};
+
+static struct ast_vbios_enhtable res_800x600[] = {
+	{1024, 800, 24, 72, 625, 600, 1, 2, VCLK36,		/* 56Hz */
+	 (SyncPP | Charx8Dot), 56, 1, 0x30 },
+	{1056, 800, 40, 128, 628, 600, 1, 4, VCLK40,	/* 60Hz */
+	 (SyncPP | Charx8Dot), 60, 2, 0x30 },
+	{1040, 800, 56, 120, 666, 600, 37, 6, VCLK50,	/* 72Hz */
+	 (SyncPP | Charx8Dot), 72, 3, 0x30 },
+	{1056, 800, 16, 80, 625, 600, 1, 3, VCLK49_5,	/* 75Hz */
+	 (SyncPP | Charx8Dot), 75, 4, 0x30 },
+	{1048, 800, 32, 64, 631, 600, 1, 3, VCLK56_25,	/* 85Hz */
+	 (SyncPP | Charx8Dot), 84, 5, 0x30 },
+	{1048, 800, 32, 64, 631, 600, 1, 3, VCLK56_25,	/* end */
+	 (SyncPP | Charx8Dot), 0xFF, 5, 0x30 },
+};
+
+
+static struct ast_vbios_enhtable res_1024x768[] = {
+	{1344, 1024, 24, 136, 806, 768, 3, 6, VCLK65,	/* 60Hz */
+	 (SyncNN | Charx8Dot), 60, 1, 0x31 },
+	{1328, 1024, 24, 136, 806, 768, 3, 6, VCLK75,	/* 70Hz */
+	 (SyncNN | Charx8Dot), 70, 2, 0x31 },
+	{1312, 1024, 16, 96, 800, 768, 1, 3, VCLK78_75,	/* 75Hz */
+	 (SyncPP | Charx8Dot), 75, 3, 0x31 },
+	{1376, 1024, 48, 96, 808, 768, 1, 3, VCLK94_5,	/* 85Hz */
+	 (SyncPP | Charx8Dot), 84, 4, 0x31 },
+	{1376, 1024, 48, 96, 808, 768, 1, 3, VCLK94_5,	/* end */
+	 (SyncPP | Charx8Dot), 0xFF, 4, 0x31 },
+};
+
+static struct ast_vbios_enhtable res_1280x1024[] = {
+	{1688, 1280, 48, 112, 1066, 1024, 1, 3, VCLK108,	/* 60Hz */
+	 (SyncPP | Charx8Dot), 60, 1, 0x32 },
+	{1688, 1280, 16, 144, 1066, 1024, 1, 3, VCLK135,	/* 75Hz */
+	 (SyncPP | Charx8Dot), 75, 2, 0x32 },
+	{1728, 1280, 64, 160, 1072, 1024, 1, 3, VCLK157_5,	/* 85Hz */
+	 (SyncPP | Charx8Dot), 85, 3, 0x32 },
+	{1728, 1280, 64, 160, 1072, 1024, 1, 3, VCLK157_5,	/* end */
+	 (SyncPP | Charx8Dot), 0xFF, 3, 0x32 },
+};
+
+static struct ast_vbios_enhtable res_1600x1200[] = {
+	{2160, 1600, 64, 192, 1250, 1200, 1, 3, VCLK162,	/* 60Hz */
+	 (SyncPP | Charx8Dot), 60, 1, 0x33 },
+	{2160, 1600, 64, 192, 1250, 1200, 1, 3, VCLK162,	/* end */
+	 (SyncPP | Charx8Dot), 0xFF, 1, 0x33 },
+};
+
+static struct ast_vbios_enhtable res_1920x1200[] = {
+	{2080, 1920, 48, 32, 1235, 1200, 3, 6, VCLK154,	/* 60Hz */
+	 (SyncNP | Charx8Dot), 60, 1, 0x34 },
+	{2080, 1920, 48, 32, 1235, 1200, 3, 6, VCLK154,	/* 60Hz */
+	 (SyncNP | Charx8Dot), 0xFF, 1, 0x34 },
+};
+
+/* 16:10 */
+static struct ast_vbios_enhtable res_1280x800[] = {
+	{1680, 1280, 72,128,  831,  800, 3, 6, VCLK83_5,	/* 60Hz */
+	 (SyncPN | Charx8Dot | LineCompareOff | WideScreenMode), 60, 1, 0x35 },
+	{1680, 1280, 72,128,  831,  800, 3, 6, VCLK83_5,	/* 60Hz */
+	 (SyncPN | Charx8Dot | LineCompareOff | WideScreenMode), 0xFF, 1, 0x35 },
+
+};
+
+static struct ast_vbios_enhtable res_1440x900[] = {
+	{1904, 1440, 80,152,  934,  900, 3, 6, VCLK106_5,	/* 60Hz */
+	 (SyncPN | Charx8Dot | LineCompareOff | WideScreenMode), 60, 1, 0x36 },
+	{1904, 1440, 80,152,  934,  900, 3, 6, VCLK106_5,	/* 60Hz */
+	 (SyncPN | Charx8Dot | LineCompareOff | WideScreenMode), 0xFF, 1, 0x36 },
+};
+
+static struct ast_vbios_enhtable res_1680x1050[] = {
+	{2240, 1680,104,176, 1089, 1050, 3, 6, VCLK146_25,	/* 60Hz */
+	 (SyncPN | Charx8Dot | LineCompareOff | WideScreenMode), 60, 1, 0x37 },
+	{2240, 1680,104,176, 1089, 1050, 3, 6, VCLK146_25,	/* 60Hz */
+	 (SyncPN | Charx8Dot | LineCompareOff | WideScreenMode), 0xFF, 1, 0x37 },
+};
+
+/* HDTV */
+static struct ast_vbios_enhtable res_1920x1080[] = {
+	{2200, 1920, 88, 44, 1125, 1080, 4, 5, VCLK148_5,	/* 60Hz */
+	 (SyncNP | Charx8Dot | LineCompareOff | WideScreenMode), 60, 1, 0x38 },
+	{2200, 1920, 88, 44, 1125, 1080, 4, 5, VCLK148_5,	/* 60Hz */
+	 (SyncNP | Charx8Dot | LineCompareOff | WideScreenMode), 0xFF, 1, 0x38 },
+};
+#endif
diff --git a/drivers/gpu/drm/ast/ast_ttm.c b/drivers/gpu/drm/ast/ast_ttm.c
new file mode 100644
index 00000000000..6cf2adea66b
--- /dev/null
+++ b/drivers/gpu/drm/ast/ast_ttm.c
@@ -0,0 +1,453 @@
+/*
+ * 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, 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: Dave Airlie <airlied@redhat.com>
+ */
+#include "drmP.h"
+#include "ast_drv.h"
+#include <ttm/ttm_page_alloc.h>
+
+static inline struct ast_private *
+ast_bdev(struct ttm_bo_device *bd)
+{
+	return container_of(bd, struct ast_private, ttm.bdev);
+}
+
+static int
+ast_ttm_mem_global_init(struct drm_global_reference *ref)
+{
+	return ttm_mem_global_init(ref->object);
+}
+
+static void
+ast_ttm_mem_global_release(struct drm_global_reference *ref)
+{
+	ttm_mem_global_release(ref->object);
+}
+
+static int ast_ttm_global_init(struct ast_private *ast)
+{
+	struct drm_global_reference *global_ref;
+	int r;
+
+	global_ref = &ast->ttm.mem_global_ref;
+	global_ref->global_type = DRM_GLOBAL_TTM_MEM;
+	global_ref->size = sizeof(struct ttm_mem_global);
+	global_ref->init = &ast_ttm_mem_global_init;
+	global_ref->release = &ast_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;
+	}
+
+	ast->ttm.bo_global_ref.mem_glob =
+		ast->ttm.mem_global_ref.object;
+	global_ref = &ast->ttm.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(&ast->ttm.mem_global_ref);
+		return r;
+	}
+	return 0;
+}
+
+void
+ast_ttm_global_release(struct ast_private *ast)
+{
+	if (ast->ttm.mem_global_ref.release == NULL)
+		return;
+
+	drm_global_item_unref(&ast->ttm.bo_global_ref.ref);
+	drm_global_item_unref(&ast->ttm.mem_global_ref);
+	ast->ttm.mem_global_ref.release = NULL;
+}
+
+
+static void ast_bo_ttm_destroy(struct ttm_buffer_object *tbo)
+{
+	struct ast_bo *bo;
+
+	bo = container_of(tbo, struct ast_bo, bo);
+
+	drm_gem_object_release(&bo->gem);
+	kfree(bo);
+}
+
+bool ast_ttm_bo_is_ast_bo(struct ttm_buffer_object *bo)
+{
+	if (bo->destroy == &ast_bo_ttm_destroy)
+		return true;
+	return false;
+}
+
+static int
+ast_bo_init_mem_type(struct ttm_bo_device *bdev, uint32_t type,
+		     struct ttm_mem_type_manager *man)
+{
+	switch (type) {
+	case TTM_PL_SYSTEM:
+		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:
+		man->func = &ttm_bo_manager_func;
+		man->flags = TTM_MEMTYPE_FLAG_FIXED |
+			TTM_MEMTYPE_FLAG_MAPPABLE;
+		man->available_caching = TTM_PL_FLAG_UNCACHED |
+			TTM_PL_FLAG_WC;
+		man->default_caching = TTM_PL_FLAG_WC;
+		break;
+	default:
+		DRM_ERROR("Unsupported memory type %u\n", (unsigned)type);
+		return -EINVAL;
+	}
+	return 0;
+}
+
+static void
+ast_bo_evict_flags(struct ttm_buffer_object *bo, struct ttm_placement *pl)
+{
+	struct ast_bo *astbo = ast_bo(bo);
+
+	if (!ast_ttm_bo_is_ast_bo(bo))
+		return;
+
+	ast_ttm_placement(astbo, TTM_PL_FLAG_SYSTEM);
+	*pl = astbo->placement;
+}
+
+static int ast_bo_verify_access(struct ttm_buffer_object *bo, struct file *filp)
+{
+	return 0;
+}
+
+static int ast_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 ast_private *ast = ast_bdev(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.offset = mem->start << PAGE_SHIFT;
+		mem->bus.base = pci_resource_start(ast->dev->pdev, 0);
+		mem->bus.is_iomem = true;
+		break;
+	default:
+		return -EINVAL;
+		break;
+	}
+	return 0;
+}
+
+static void ast_ttm_io_mem_free(struct ttm_bo_device *bdev, struct ttm_mem_reg *mem)
+{
+}
+
+static int ast_bo_move(struct ttm_buffer_object *bo,
+		       bool evict, bool interruptible,
+		       bool no_wait_reserve, bool no_wait_gpu,
+		       struct ttm_mem_reg *new_mem)
+{
+	int r;
+	r = ttm_bo_move_memcpy(bo, evict, no_wait_reserve, no_wait_gpu, new_mem);
+	return r;
+}
+
+
+static void ast_ttm_backend_destroy(struct ttm_tt *tt)
+{
+	ttm_tt_fini(tt);
+	kfree(tt);
+}
+
+static struct ttm_backend_func ast_tt_backend_func = {
+	.destroy = &ast_ttm_backend_destroy,
+};
+
+
+struct ttm_tt *ast_ttm_tt_create(struct ttm_bo_device *bdev,
+				 unsigned long size, uint32_t page_flags,
+				 struct page *dummy_read_page)
+{
+	struct ttm_tt *tt;
+
+	tt = kzalloc(sizeof(struct ttm_tt), GFP_KERNEL);
+	if (tt == NULL)
+		return NULL;
+	tt->func = &ast_tt_backend_func;
+	if (ttm_tt_init(tt, bdev, size, page_flags, dummy_read_page)) {
+		kfree(tt);
+		return NULL;
+	}
+	return tt;
+}
+
+static int ast_ttm_tt_populate(struct ttm_tt *ttm)
+{
+	return ttm_pool_populate(ttm);
+}
+
+static void ast_ttm_tt_unpopulate(struct ttm_tt *ttm)
+{
+	ttm_pool_unpopulate(ttm);
+}
+
+struct ttm_bo_driver ast_bo_driver = {
+	.ttm_tt_create = ast_ttm_tt_create,
+	.ttm_tt_populate = ast_ttm_tt_populate,
+	.ttm_tt_unpopulate = ast_ttm_tt_unpopulate,
+	.init_mem_type = ast_bo_init_mem_type,
+	.evict_flags = ast_bo_evict_flags,
+	.move = ast_bo_move,
+	.verify_access = ast_bo_verify_access,
+	.io_mem_reserve = &ast_ttm_io_mem_reserve,
+	.io_mem_free = &ast_ttm_io_mem_free,
+};
+
+int ast_mm_init(struct ast_private *ast)
+{
+	int ret;
+	struct drm_device *dev = ast->dev;
+	struct ttm_bo_device *bdev = &ast->ttm.bdev;
+
+	ret = ast_ttm_global_init(ast);
+	if (ret)
+		return ret;
+
+	ret = ttm_bo_device_init(&ast->ttm.bdev,
+				 ast->ttm.bo_global_ref.ref.object,
+				 &ast_bo_driver, DRM_FILE_PAGE_OFFSET,
+				 true);
+	if (ret) {
+		DRM_ERROR("Error initialising bo driver; %d\n", ret);
+		return ret;
+	}
+
+	ret = ttm_bo_init_mm(bdev, TTM_PL_VRAM,
+			     ast->vram_size >> PAGE_SHIFT);
+	if (ret) {
+		DRM_ERROR("Failed ttm VRAM init: %d\n", ret);
+		return ret;
+	}
+
+	ast->fb_mtrr = drm_mtrr_add(pci_resource_start(dev->pdev, 0),
+				    pci_resource_len(dev->pdev, 0),
+				    DRM_MTRR_WC);
+
+	return 0;
+}
+
+void ast_mm_fini(struct ast_private *ast)
+{
+	struct drm_device *dev = ast->dev;
+	ttm_bo_device_release(&ast->ttm.bdev);
+
+	ast_ttm_global_release(ast);
+
+	if (ast->fb_mtrr >= 0) {
+		drm_mtrr_del(ast->fb_mtrr,
+			     pci_resource_start(dev->pdev, 0),
+			     pci_resource_len(dev->pdev, 0), DRM_MTRR_WC);
+		ast->fb_mtrr = -1;
+	}
+}
+
+void ast_ttm_placement(struct ast_bo *bo, int domain)
+{
+	u32 c = 0;
+	bo->placement.fpfn = 0;
+	bo->placement.lpfn = 0;
+	bo->placement.placement = bo->placements;
+	bo->placement.busy_placement = bo->placements;
+	if (domain & TTM_PL_FLAG_VRAM)
+		bo->placements[c++] = TTM_PL_FLAG_WC | TTM_PL_FLAG_UNCACHED | TTM_PL_FLAG_VRAM;
+	if (domain & TTM_PL_FLAG_SYSTEM)
+		bo->placements[c++] = TTM_PL_MASK_CACHING | TTM_PL_FLAG_SYSTEM;
+	if (!c)
+		bo->placements[c++] = TTM_PL_MASK_CACHING | TTM_PL_FLAG_SYSTEM;
+	bo->placement.num_placement = c;
+	bo->placement.num_busy_placement = c;
+}
+
+int ast_bo_reserve(struct ast_bo *bo, bool no_wait)
+{
+	int ret;
+
+	ret = ttm_bo_reserve(&bo->bo, true, no_wait, false, 0);
+	if (ret) {
+		if (ret != -ERESTARTSYS)
+			DRM_ERROR("reserve failed %p\n", bo);
+		return ret;
+	}
+	return 0;
+}
+
+void ast_bo_unreserve(struct ast_bo *bo)
+{
+	ttm_bo_unreserve(&bo->bo);
+}
+
+int ast_bo_create(struct drm_device *dev, int size, int align,
+		  uint32_t flags, struct ast_bo **pastbo)
+{
+	struct ast_private *ast = dev->dev_private;
+	struct ast_bo *astbo;
+	size_t acc_size;
+	int ret;
+
+	astbo = kzalloc(sizeof(struct ast_bo), GFP_KERNEL);
+	if (!astbo)
+		return -ENOMEM;
+
+	ret = drm_gem_object_init(dev, &astbo->gem, size);
+	if (ret) {
+		kfree(astbo);
+		return ret;
+	}
+
+	astbo->gem.driver_private = NULL;
+	astbo->bo.bdev = &ast->ttm.bdev;
+
+	ast_ttm_placement(astbo, TTM_PL_FLAG_VRAM | TTM_PL_FLAG_SYSTEM);
+
+	acc_size = ttm_bo_dma_acc_size(&ast->ttm.bdev, size,
+				       sizeof(struct ast_bo));
+
+	ret = ttm_bo_init(&ast->ttm.bdev, &astbo->bo, size,
+			  ttm_bo_type_device, &astbo->placement,
+			  align >> PAGE_SHIFT, 0, false, NULL, acc_size,
+			  NULL, ast_bo_ttm_destroy);
+	if (ret)
+		return ret;
+
+	*pastbo = astbo;
+	return 0;
+}
+
+static inline u64 ast_bo_gpu_offset(struct ast_bo *bo)
+{
+	return bo->bo.offset;
+}
+
+int ast_bo_pin(struct ast_bo *bo, u32 pl_flag, u64 *gpu_addr)
+{
+	int i, ret;
+
+	if (bo->pin_count) {
+		bo->pin_count++;
+		if (gpu_addr)
+			*gpu_addr = ast_bo_gpu_offset(bo);
+	}
+
+	ast_ttm_placement(bo, pl_flag);
+	for (i = 0; i < bo->placement.num_placement; i++)
+		bo->placements[i] |= TTM_PL_FLAG_NO_EVICT;
+	ret = ttm_bo_validate(&bo->bo, &bo->placement, false, false, false);
+	if (ret)
+		return ret;
+
+	bo->pin_count = 1;
+	if (gpu_addr)
+		*gpu_addr = ast_bo_gpu_offset(bo);
+	return 0;
+}
+
+int ast_bo_unpin(struct ast_bo *bo)
+{
+	int i, ret;
+	if (!bo->pin_count) {
+		DRM_ERROR("unpin bad %p\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;
+	ret = ttm_bo_validate(&bo->bo, &bo->placement, false, false, false);
+	if (ret)
+		return ret;
+
+	return 0;
+}
+
+int ast_bo_push_sysram(struct ast_bo *bo)
+{
+	int i, ret;
+	if (!bo->pin_count) {
+		DRM_ERROR("unpin bad %p\n", bo);
+		return 0;
+	}
+	bo->pin_count--;
+	if (bo->pin_count)
+		return 0;
+
+	if (bo->kmap.virtual)
+		ttm_bo_kunmap(&bo->kmap);
+
+	ast_ttm_placement(bo, TTM_PL_FLAG_SYSTEM);
+	for (i = 0; i < bo->placement.num_placement ; i++)
+		bo->placements[i] |= TTM_PL_FLAG_NO_EVICT;
+
+	ret = ttm_bo_validate(&bo->bo, &bo->placement, false, false, false);
+	if (ret) {
+		DRM_ERROR("pushing to VRAM failed\n");
+		return ret;
+	}
+	return 0;
+}
+
+int ast_mmap(struct file *filp, struct vm_area_struct *vma)
+{
+	struct drm_file *file_priv;
+	struct ast_private *ast;
+
+	if (unlikely(vma->vm_pgoff < DRM_FILE_PAGE_OFFSET))
+		return drm_mmap(filp, vma);
+
+	file_priv = filp->private_data;
+	ast = file_priv->minor->dev->dev_private;
+	return ttm_bo_mmap(filp, vma, &ast->ttm.bdev);
+}
diff --git a/drivers/gpu/drm/cirrus/Kconfig b/drivers/gpu/drm/cirrus/Kconfig
new file mode 100644
index 00000000000..fc154dd7529
--- /dev/null
+++ b/drivers/gpu/drm/cirrus/Kconfig
@@ -0,0 +1,12 @@
+config DRM_CIRRUS_QEMU
+	tristate "Cirrus driver for QEMU emulated device"
+	depends on DRM && PCI && EXPERIMENTAL
+	select FB_SYS_FILLRECT
+	select FB_SYS_COPYAREA
+	select FB_SYS_IMAGEBLIT
+	select DRM_KMS_HELPER
+	select DRM_TTM
+	help
+	 This is a KMS driver for emulated cirrus device in qemu.
+	 It is *NOT* intended for real cirrus devices. This requires
+	 the modesetting userspace X.org driver.
diff --git a/drivers/gpu/drm/cirrus/Makefile b/drivers/gpu/drm/cirrus/Makefile
new file mode 100644
index 00000000000..69ffe7006d5
--- /dev/null
+++ b/drivers/gpu/drm/cirrus/Makefile
@@ -0,0 +1,5 @@
+ccflags-y := -Iinclude/drm
+cirrus-y  := cirrus_main.o cirrus_mode.o \
+	cirrus_drv.o cirrus_fbdev.o cirrus_ttm.o
+
+obj-$(CONFIG_DRM_CIRRUS_QEMU) += cirrus.o
diff --git a/drivers/gpu/drm/cirrus/cirrus_drv.c b/drivers/gpu/drm/cirrus/cirrus_drv.c
new file mode 100644
index 00000000000..d7038230b71
--- /dev/null
+++ b/drivers/gpu/drm/cirrus/cirrus_drv.c
@@ -0,0 +1,108 @@
+/*
+ * Copyright 2012 Red Hat <mjg@redhat.com>
+ *
+ * This file is subject to the terms and conditions of the GNU General
+ * Public License version 2. See the file COPYING in the main
+ * directory of this archive for more details.
+ *
+ * Authors: Matthew Garrett
+ *          Dave Airlie
+ */
+#include <linux/module.h>
+#include <linux/console.h>
+#include "drmP.h"
+#include "drm.h"
+
+#include "cirrus_drv.h"
+
+int cirrus_modeset = -1;
+
+MODULE_PARM_DESC(modeset, "Disable/Enable modesetting");
+module_param_named(modeset, cirrus_modeset, int, 0400);
+
+/*
+ * This is the generic driver code. This binds the driver to the drm core,
+ * which then performs further device association and calls our graphics init
+ * functions
+ */
+
+static struct drm_driver driver;
+
+/* only bind to the cirrus chip in qemu */
+static DEFINE_PCI_DEVICE_TABLE(pciidlist) = {
+	{ PCI_VENDOR_ID_CIRRUS, PCI_DEVICE_ID_CIRRUS_5446, 0x1af4, 0x1100, 0,
+	  0, 0 },
+	{0,}
+};
+
+static int __devinit
+cirrus_pci_probe(struct pci_dev *pdev, const struct pci_device_id *ent)
+{
+	return drm_get_pci_dev(pdev, ent, &driver);
+}
+
+static void cirrus_pci_remove(struct pci_dev *pdev)
+{
+	struct drm_device *dev = pci_get_drvdata(pdev);
+
+	drm_put_dev(dev);
+}
+
+static const struct file_operations cirrus_driver_fops = {
+	.owner = THIS_MODULE,
+	.open = drm_open,
+	.release = drm_release,
+	.unlocked_ioctl = drm_ioctl,
+	.mmap = cirrus_mmap,
+	.poll = drm_poll,
+	.fasync = drm_fasync,
+};
+static struct drm_driver driver = {
+	.driver_features = DRIVER_MODESET | DRIVER_GEM | DRIVER_USE_MTRR,
+	.load = cirrus_driver_load,
+	.unload = cirrus_driver_unload,
+	.fops = &cirrus_driver_fops,
+	.name = DRIVER_NAME,
+	.desc = DRIVER_DESC,
+	.date = DRIVER_DATE,
+	.major = DRIVER_MAJOR,
+	.minor = DRIVER_MINOR,
+	.patchlevel = DRIVER_PATCHLEVEL,
+	.gem_init_object = cirrus_gem_init_object,
+	.gem_free_object = cirrus_gem_free_object,
+	.dumb_create = cirrus_dumb_create,
+	.dumb_map_offset = cirrus_dumb_mmap_offset,
+	.dumb_destroy = cirrus_dumb_destroy,
+};
+
+static struct pci_driver cirrus_pci_driver = {
+	.name = DRIVER_NAME,
+	.id_table = pciidlist,
+	.probe = cirrus_pci_probe,
+	.remove = cirrus_pci_remove,
+};
+
+static int __init cirrus_init(void)
+{
+#ifdef CONFIG_VGA_CONSOLE
+	if (vgacon_text_force() && cirrus_modeset == -1)
+		return -EINVAL;
+#endif
+
+	if (cirrus_modeset == 0)
+		return -EINVAL;
+	return drm_pci_init(&driver, &cirrus_pci_driver);
+}
+
+static void __exit cirrus_exit(void)
+{
+	drm_pci_exit(&driver, &cirrus_pci_driver);
+}
+
+module_init(cirrus_init);
+module_exit(cirrus_exit);
+
+MODULE_DEVICE_TABLE(pci, pciidlist);
+MODULE_AUTHOR(DRIVER_AUTHOR);
+MODULE_DESCRIPTION(DRIVER_DESC);
+MODULE_LICENSE("GPL");
diff --git a/drivers/gpu/drm/cirrus/cirrus_drv.h b/drivers/gpu/drm/cirrus/cirrus_drv.h
new file mode 100644
index 00000000000..21bdfa8836f
--- /dev/null
+++ b/drivers/gpu/drm/cirrus/cirrus_drv.h
@@ -0,0 +1,246 @@
+/*
+ * Copyright 2012 Red Hat
+ *
+ * This file is subject to the terms and conditions of the GNU General
+ * Public License version 2. See the file COPYING in the main
+ * directory of this archive for more details.
+ *
+ * Authors: Matthew Garrett
+ *          Dave Airlie
+ */
+#ifndef __CIRRUS_DRV_H__
+#define __CIRRUS_DRV_H__
+
+#include <video/vga.h>
+
+#include <drm/drm_fb_helper.h>
+
+#include "ttm/ttm_bo_api.h"
+#include "ttm/ttm_bo_driver.h"
+#include "ttm/ttm_placement.h"
+#include "ttm/ttm_memory.h"
+#include "ttm/ttm_module.h"
+
+#define DRIVER_AUTHOR		"Matthew Garrett"
+
+#define DRIVER_NAME		"cirrus"
+#define DRIVER_DESC		"qemu Cirrus emulation"
+#define DRIVER_DATE		"20110418"
+
+#define DRIVER_MAJOR		1
+#define DRIVER_MINOR		0
+#define DRIVER_PATCHLEVEL	0
+
+#define CIRRUSFB_CONN_LIMIT 1
+
+#define RREG8(reg) ioread8(((void __iomem *)cdev->rmmio) + (reg))
+#define WREG8(reg, v) iowrite8(v, ((void __iomem *)cdev->rmmio) + (reg))
+#define RREG32(reg) ioread32(((void __iomem *)cdev->rmmio) + (reg))
+#define WREG32(reg, v) iowrite32(v, ((void __iomem *)cdev->rmmio) + (reg))
+
+#define SEQ_INDEX 4
+#define SEQ_DATA 5
+
+#define WREG_SEQ(reg, v)					\
+	do {							\
+		WREG8(SEQ_INDEX, reg);				\
+		WREG8(SEQ_DATA, v);				\
+	} while (0)						\
+
+#define CRT_INDEX 0x14
+#define CRT_DATA 0x15
+
+#define WREG_CRT(reg, v)					\
+	do {							\
+		WREG8(CRT_INDEX, reg);				\
+		WREG8(CRT_DATA, v);				\
+	} while (0)						\
+
+#define GFX_INDEX 0xe
+#define GFX_DATA 0xf
+
+#define WREG_GFX(reg, v)					\
+	do {							\
+		WREG8(GFX_INDEX, reg);				\
+		WREG8(GFX_DATA, v);				\
+	} while (0)						\
+
+/*
+ * Cirrus has a "hidden" DAC register that can be accessed by writing to
+ * the pixel mask register to reset the state, then reading from the register
+ * four times. The next write will then pass to the DAC
+ */
+#define VGA_DAC_MASK 0x6
+
+#define WREG_HDR(v)						\
+	do {							\
+		RREG8(VGA_DAC_MASK);					\
+		RREG8(VGA_DAC_MASK);					\
+		RREG8(VGA_DAC_MASK);					\
+		RREG8(VGA_DAC_MASK);					\
+		WREG8(VGA_DAC_MASK, v);					\
+	} while (0)						\
+
+
+#define CIRRUS_MAX_FB_HEIGHT 4096
+#define CIRRUS_MAX_FB_WIDTH 4096
+
+#define CIRRUS_DPMS_CLEARED (-1)
+
+#define to_cirrus_crtc(x) container_of(x, struct cirrus_crtc, base)
+#define to_cirrus_encoder(x) container_of(x, struct cirrus_encoder, base)
+#define to_cirrus_framebuffer(x) container_of(x, struct cirrus_framebuffer, base)
+
+struct cirrus_crtc {
+	struct drm_crtc			base;
+	u8				lut_r[256], lut_g[256], lut_b[256];
+	int				last_dpms;
+	bool				enabled;
+};
+
+struct cirrus_fbdev;
+struct cirrus_mode_info {
+	bool				mode_config_initialized;
+	struct cirrus_crtc		*crtc;
+	/* pointer to fbdev info structure */
+	struct cirrus_fbdev		*gfbdev;
+};
+
+struct cirrus_encoder {
+	struct drm_encoder		base;
+	int				last_dpms;
+};
+
+struct cirrus_connector {
+	struct drm_connector		base;
+};
+
+struct cirrus_framebuffer {
+	struct drm_framebuffer		base;
+	struct drm_gem_object *obj;
+};
+
+struct cirrus_mc {
+	resource_size_t			vram_size;
+	resource_size_t			vram_base;
+};
+
+struct cirrus_device {
+	struct drm_device		*dev;
+	unsigned long			flags;
+
+	resource_size_t			rmmio_base;
+	resource_size_t			rmmio_size;
+	void __iomem			*rmmio;
+
+	struct cirrus_mc			mc;
+	struct cirrus_mode_info		mode_info;
+
+	int				num_crtc;
+	int fb_mtrr;
+
+	struct {
+		struct drm_global_reference mem_global_ref;
+		struct ttm_bo_global_ref bo_global_ref;
+		struct ttm_bo_device bdev;
+		atomic_t validate_sequence;
+	} ttm;
+
+};
+
+
+struct cirrus_fbdev {
+	struct drm_fb_helper helper;
+	struct cirrus_framebuffer gfb;
+	struct list_head fbdev_list;
+	void *sysram;
+	int size;
+};
+
+struct cirrus_bo {
+	struct ttm_buffer_object bo;
+	struct ttm_placement placement;
+	struct ttm_bo_kmap_obj kmap;
+	struct drm_gem_object gem;
+	u32 placements[3];
+	int pin_count;
+};
+#define gem_to_cirrus_bo(gobj) container_of((gobj), struct cirrus_bo, gem)
+
+static inline struct cirrus_bo *
+cirrus_bo(struct ttm_buffer_object *bo)
+{
+	return container_of(bo, struct cirrus_bo, bo);
+}
+
+
+#define to_cirrus_obj(x) container_of(x, struct cirrus_gem_object, base)
+#define DRM_FILE_PAGE_OFFSET (0x100000000ULL >> PAGE_SHIFT)
+
+				/* cirrus_mode.c */
+void cirrus_crtc_fb_gamma_set(struct drm_crtc *crtc, u16 red, u16 green,
+			     u16 blue, int regno);
+void cirrus_crtc_fb_gamma_get(struct drm_crtc *crtc, u16 *red, u16 *green,
+			     u16 *blue, int regno);
+
+
+				/* cirrus_main.c */
+int cirrus_device_init(struct cirrus_device *cdev,
+		      struct drm_device *ddev,
+		      struct pci_dev *pdev,
+		      uint32_t flags);
+void cirrus_device_fini(struct cirrus_device *cdev);
+int cirrus_gem_init_object(struct drm_gem_object *obj);
+void cirrus_gem_free_object(struct drm_gem_object *obj);
+int cirrus_dumb_mmap_offset(struct drm_file *file,
+			    struct drm_device *dev,
+			    uint32_t handle,
+			    uint64_t *offset);
+int cirrus_gem_create(struct drm_device *dev,
+		   u32 size, bool iskernel,
+		      struct drm_gem_object **obj);
+int cirrus_dumb_create(struct drm_file *file,
+		    struct drm_device *dev,
+		       struct drm_mode_create_dumb *args);
+int cirrus_dumb_destroy(struct drm_file *file,
+		     struct drm_device *dev,
+			uint32_t handle);
+
+int cirrus_framebuffer_init(struct drm_device *dev,
+			   struct cirrus_framebuffer *gfb,
+			    struct drm_mode_fb_cmd2 *mode_cmd,
+			    struct drm_gem_object *obj);
+
+				/* cirrus_display.c */
+int cirrus_modeset_init(struct cirrus_device *cdev);
+void cirrus_modeset_fini(struct cirrus_device *cdev);
+
+				/* cirrus_fbdev.c */
+int cirrus_fbdev_init(struct cirrus_device *cdev);
+void cirrus_fbdev_fini(struct cirrus_device *cdev);
+
+
+
+				/* cirrus_irq.c */
+void cirrus_driver_irq_preinstall(struct drm_device *dev);
+int cirrus_driver_irq_postinstall(struct drm_device *dev);
+void cirrus_driver_irq_uninstall(struct drm_device *dev);
+irqreturn_t cirrus_driver_irq_handler(DRM_IRQ_ARGS);
+
+				/* cirrus_kms.c */
+int cirrus_driver_load(struct drm_device *dev, unsigned long flags);
+int cirrus_driver_unload(struct drm_device *dev);
+extern struct drm_ioctl_desc cirrus_ioctls[];
+extern int cirrus_max_ioctl;
+
+int cirrus_mm_init(struct cirrus_device *cirrus);
+void cirrus_mm_fini(struct cirrus_device *cirrus);
+void cirrus_ttm_placement(struct cirrus_bo *bo, int domain);
+int cirrus_bo_create(struct drm_device *dev, int size, int align,
+		     uint32_t flags, struct cirrus_bo **pcirrusbo);
+int cirrus_mmap(struct file *filp, struct vm_area_struct *vma);
+int cirrus_bo_reserve(struct cirrus_bo *bo, bool no_wait);
+void cirrus_bo_unreserve(struct cirrus_bo *bo);
+int cirrus_bo_push_sysram(struct cirrus_bo *bo);
+int cirrus_bo_pin(struct cirrus_bo *bo, u32 pl_flag, u64 *gpu_addr);
+#endif				/* __CIRRUS_DRV_H__ */
diff --git a/drivers/gpu/drm/cirrus/cirrus_fbdev.c b/drivers/gpu/drm/cirrus/cirrus_fbdev.c
new file mode 100644
index 00000000000..9a276a53699
--- /dev/null
+++ b/drivers/gpu/drm/cirrus/cirrus_fbdev.c
@@ -0,0 +1,307 @@
+/*
+ * Copyright 2012 Red Hat
+ *
+ * This file is subject to the terms and conditions of the GNU General
+ * Public License version 2. See the file COPYING in the main
+ * directory of this archive for more details.
+ *
+ * Authors: Matthew Garrett
+ *          Dave Airlie
+ */
+#include <linux/module.h>
+#include "drmP.h"
+#include "drm.h"
+#include "drm_fb_helper.h"
+
+#include <linux/fb.h>
+
+#include "cirrus_drv.h"
+
+static void cirrus_dirty_update(struct cirrus_fbdev *afbdev,
+			     int x, int y, int width, int height)
+{
+	int i;
+	struct drm_gem_object *obj;
+	struct cirrus_bo *bo;
+	int src_offset, dst_offset;
+	int bpp = (afbdev->gfb.base.bits_per_pixel + 7)/8;
+	int ret;
+	bool unmap = false;
+
+	obj = afbdev->gfb.obj;
+	bo = gem_to_cirrus_bo(obj);
+
+	ret = cirrus_bo_reserve(bo, true);
+	if (ret) {
+		DRM_ERROR("failed to reserve fb bo\n");
+		return;
+	}
+
+	if (!bo->kmap.virtual) {
+		ret = ttm_bo_kmap(&bo->bo, 0, bo->bo.num_pages, &bo->kmap);
+		if (ret) {
+			DRM_ERROR("failed to kmap fb updates\n");
+			cirrus_bo_unreserve(bo);
+			return;
+		}
+		unmap = true;
+	}
+	for (i = y; i < y + height; i++) {
+		/* assume equal stride for now */
+		src_offset = dst_offset = i * afbdev->gfb.base.pitches[0] + (x * bpp);
+		memcpy_toio(bo->kmap.virtual + src_offset, afbdev->sysram + src_offset, width * bpp);
+
+	}
+	if (unmap)
+		ttm_bo_kunmap(&bo->kmap);
+
+	cirrus_bo_unreserve(bo);
+}
+
+static void cirrus_fillrect(struct fb_info *info,
+			 const struct fb_fillrect *rect)
+{
+	struct cirrus_fbdev *afbdev = info->par;
+	sys_fillrect(info, rect);
+	cirrus_dirty_update(afbdev, rect->dx, rect->dy, rect->width,
+			 rect->height);
+}
+
+static void cirrus_copyarea(struct fb_info *info,
+			 const struct fb_copyarea *area)
+{
+	struct cirrus_fbdev *afbdev = info->par;
+	sys_copyarea(info, area);
+	cirrus_dirty_update(afbdev, area->dx, area->dy, area->width,
+			 area->height);
+}
+
+static void cirrus_imageblit(struct fb_info *info,
+			  const struct fb_image *image)
+{
+	struct cirrus_fbdev *afbdev = info->par;
+	sys_imageblit(info, image);
+	cirrus_dirty_update(afbdev, image->dx, image->dy, image->width,
+			 image->height);
+}
+
+
+static struct fb_ops cirrusfb_ops = {
+	.owner = THIS_MODULE,
+	.fb_check_var = drm_fb_helper_check_var,
+	.fb_set_par = drm_fb_helper_set_par,
+	.fb_fillrect = cirrus_fillrect,
+	.fb_copyarea = cirrus_copyarea,
+	.fb_imageblit = cirrus_imageblit,
+	.fb_pan_display = drm_fb_helper_pan_display,
+	.fb_blank = drm_fb_helper_blank,
+	.fb_setcmap = drm_fb_helper_setcmap,
+};
+
+static int cirrusfb_create_object(struct cirrus_fbdev *afbdev,
+			       struct drm_mode_fb_cmd2 *mode_cmd,
+			       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);
+
+	if (bpp > 24)
+		return -EINVAL;
+	size = mode_cmd->pitches[0] * mode_cmd->height;
+	ret = cirrus_gem_create(dev, size, true, &gobj);
+	if (ret)
+		return ret;
+
+	*gobj_p = gobj;
+	return ret;
+}
+
+static int cirrusfb_create(struct cirrus_fbdev *gfbdev,
+			   struct drm_fb_helper_surface_size *sizes)
+{
+	struct drm_device *dev = gfbdev->helper.dev;
+	struct cirrus_device *cdev = gfbdev->helper.dev->dev_private;
+	struct fb_info *info;
+	struct drm_framebuffer *fb;
+	struct drm_mode_fb_cmd2 mode_cmd;
+	struct device *device = &dev->pdev->dev;
+	void *sysram;
+	struct drm_gem_object *gobj = NULL;
+	struct cirrus_bo *bo = NULL;
+	int size, ret;
+
+	mode_cmd.width = sizes->surface_width;
+	mode_cmd.height = sizes->surface_height;
+	mode_cmd.pitches[0] = mode_cmd.width * ((sizes->surface_bpp + 7) / 8);
+	mode_cmd.pixel_format = drm_mode_legacy_fb_format(sizes->surface_bpp,
+							  sizes->surface_depth);
+	size = mode_cmd.pitches[0] * mode_cmd.height;
+
+	ret = cirrusfb_create_object(gfbdev, &mode_cmd, &gobj);
+	if (ret) {
+		DRM_ERROR("failed to create fbcon backing object %d\n", ret);
+		return ret;
+	}
+
+	bo = gem_to_cirrus_bo(gobj);
+
+	sysram = vmalloc(size);
+	if (!sysram)
+		return -ENOMEM;
+
+	info = framebuffer_alloc(0, device);
+	if (info == NULL)
+		return -ENOMEM;
+
+	info->par = gfbdev;
+
+	ret = cirrus_framebuffer_init(cdev->dev, &gfbdev->gfb, &mode_cmd, gobj);
+	if (ret)
+		return ret;
+
+	gfbdev->sysram = sysram;
+	gfbdev->size = size;
+
+	fb = &gfbdev->gfb.base;
+	if (!fb) {
+		DRM_INFO("fb is NULL\n");
+		return -EINVAL;
+	}
+
+	/* setup helper */
+	gfbdev->helper.fb = fb;
+	gfbdev->helper.fbdev = info;
+
+	strcpy(info->fix.id, "cirrusdrmfb");
+
+
+	info->flags = FBINFO_DEFAULT;
+	info->fbops = &cirrusfb_ops;
+
+	drm_fb_helper_fill_fix(info, fb->pitches[0], fb->depth);
+	drm_fb_helper_fill_var(info, &gfbdev->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_iounmap;
+	}
+	info->apertures->ranges[0].base = cdev->dev->mode_config.fb_base;
+	info->apertures->ranges[0].size = cdev->mc.vram_size;
+
+	info->screen_base = sysram;
+	info->screen_size = size;
+
+	info->fix.mmio_start = 0;
+	info->fix.mmio_len = 0;
+
+	ret = fb_alloc_cmap(&info->cmap, 256, 0);
+	if (ret) {
+		DRM_ERROR("%s: can't allocate color map\n", info->fix.id);
+		ret = -ENOMEM;
+		goto out_iounmap;
+	}
+
+	DRM_INFO("fb mappable at 0x%lX\n", info->fix.smem_start);
+	DRM_INFO("vram aper at 0x%lX\n", (unsigned long)info->fix.smem_start);
+	DRM_INFO("size %lu\n", (unsigned long)info->fix.smem_len);
+	DRM_INFO("fb depth is %d\n", fb->depth);
+	DRM_INFO("   pitch is %d\n", fb->pitches[0]);
+
+	return 0;
+out_iounmap:
+	return ret;
+}
+
+static int cirrus_fb_find_or_create_single(struct drm_fb_helper *helper,
+					   struct drm_fb_helper_surface_size
+					   *sizes)
+{
+	struct cirrus_fbdev *gfbdev = (struct cirrus_fbdev *)helper;
+	int new_fb = 0;
+	int ret;
+
+	if (!helper->fb) {
+		ret = cirrusfb_create(gfbdev, sizes);
+		if (ret)
+			return ret;
+		new_fb = 1;
+	}
+	return new_fb;
+}
+
+static int cirrus_fbdev_destroy(struct drm_device *dev,
+				struct cirrus_fbdev *gfbdev)
+{
+	struct fb_info *info;
+	struct cirrus_framebuffer *gfb = &gfbdev->gfb;
+
+	if (gfbdev->helper.fbdev) {
+		info = gfbdev->helper.fbdev;
+
+		unregister_framebuffer(info);
+		if (info->cmap.len)
+			fb_dealloc_cmap(&info->cmap);
+		framebuffer_release(info);
+	}
+
+	if (gfb->obj) {
+		drm_gem_object_unreference_unlocked(gfb->obj);
+		gfb->obj = NULL;
+	}
+
+	vfree(gfbdev->sysram);
+	drm_fb_helper_fini(&gfbdev->helper);
+	drm_framebuffer_cleanup(&gfb->base);
+
+	return 0;
+}
+
+static struct drm_fb_helper_funcs cirrus_fb_helper_funcs = {
+	.gamma_set = cirrus_crtc_fb_gamma_set,
+	.gamma_get = cirrus_crtc_fb_gamma_get,
+	.fb_probe = cirrus_fb_find_or_create_single,
+};
+
+int cirrus_fbdev_init(struct cirrus_device *cdev)
+{
+	struct cirrus_fbdev *gfbdev;
+	int ret;
+	int bpp_sel = 24;
+
+	/*bpp_sel = 8;*/
+	gfbdev = kzalloc(sizeof(struct cirrus_fbdev), GFP_KERNEL);
+	if (!gfbdev)
+		return -ENOMEM;
+
+	cdev->mode_info.gfbdev = gfbdev;
+	gfbdev->helper.funcs = &cirrus_fb_helper_funcs;
+
+	ret = drm_fb_helper_init(cdev->dev, &gfbdev->helper,
+				 cdev->num_crtc, CIRRUSFB_CONN_LIMIT);
+	if (ret) {
+		kfree(gfbdev);
+		return ret;
+	}
+	drm_fb_helper_single_add_all_connectors(&gfbdev->helper);
+	drm_fb_helper_initial_config(&gfbdev->helper, bpp_sel);
+
+	return 0;
+}
+
+void cirrus_fbdev_fini(struct cirrus_device *cdev)
+{
+	if (!cdev->mode_info.gfbdev)
+		return;
+
+	cirrus_fbdev_destroy(cdev->dev, cdev->mode_info.gfbdev);
+	kfree(cdev->mode_info.gfbdev);
+	cdev->mode_info.gfbdev = NULL;
+}
diff --git a/drivers/gpu/drm/cirrus/cirrus_main.c b/drivers/gpu/drm/cirrus/cirrus_main.c
new file mode 100644
index 00000000000..e3c12257841
--- /dev/null
+++ b/drivers/gpu/drm/cirrus/cirrus_main.c
@@ -0,0 +1,335 @@
+/*
+ * Copyright 2012 Red Hat
+ *
+ * This file is subject to the terms and conditions of the GNU General
+ * Public License version 2. See the file COPYING in the main
+ * directory of this archive for more details.
+ *
+ * Authors: Matthew Garrett
+ *          Dave Airlie
+ */
+#include "drmP.h"
+#include "drm.h"
+#include "drm_crtc_helper.h"
+
+#include "cirrus_drv.h"
+
+
+static void cirrus_user_framebuffer_destroy(struct drm_framebuffer *fb)
+{
+	struct cirrus_framebuffer *cirrus_fb = to_cirrus_framebuffer(fb);
+	if (cirrus_fb->obj)
+		drm_gem_object_unreference_unlocked(cirrus_fb->obj);
+	drm_framebuffer_cleanup(fb);
+	kfree(fb);
+}
+
+static int cirrus_user_framebuffer_create_handle(struct drm_framebuffer *fb,
+						 struct drm_file *file_priv,
+						 unsigned int *handle)
+{
+	return 0;
+}
+
+static const struct drm_framebuffer_funcs cirrus_fb_funcs = {
+	.destroy = cirrus_user_framebuffer_destroy,
+	.create_handle = cirrus_user_framebuffer_create_handle,
+};
+
+int cirrus_framebuffer_init(struct drm_device *dev,
+			    struct cirrus_framebuffer *gfb,
+			    struct drm_mode_fb_cmd2 *mode_cmd,
+			    struct drm_gem_object *obj)
+{
+	int ret;
+
+	ret = drm_framebuffer_init(dev, &gfb->base, &cirrus_fb_funcs);
+	if (ret) {
+		DRM_ERROR("drm_framebuffer_init failed: %d\n", ret);
+		return ret;
+	}
+	drm_helper_mode_fill_fb_struct(&gfb->base, mode_cmd);
+	gfb->obj = obj;
+	return 0;
+}
+
+static struct drm_framebuffer *
+cirrus_user_framebuffer_create(struct drm_device *dev,
+			       struct drm_file *filp,
+			       struct drm_mode_fb_cmd2 *mode_cmd)
+{
+	struct drm_gem_object *obj;
+	struct cirrus_framebuffer *cirrus_fb;
+	int ret;
+	u32 bpp, depth;
+
+	drm_fb_get_bpp_depth(mode_cmd->pixel_format, &depth, &bpp);
+	/* cirrus can't handle > 24bpp framebuffers at all */
+	if (bpp > 24)
+		return ERR_PTR(-EINVAL);
+
+	obj = drm_gem_object_lookup(dev, filp, mode_cmd->handles[0]);
+	if (obj == NULL)
+		return ERR_PTR(-ENOENT);
+
+	cirrus_fb = kzalloc(sizeof(*cirrus_fb), GFP_KERNEL);
+	if (!cirrus_fb) {
+		drm_gem_object_unreference_unlocked(obj);
+		return ERR_PTR(-ENOMEM);
+	}
+
+	ret = cirrus_framebuffer_init(dev, cirrus_fb, mode_cmd, obj);
+	if (ret) {
+		drm_gem_object_unreference_unlocked(obj);
+		kfree(cirrus_fb);
+		return ERR_PTR(ret);
+	}
+	return &cirrus_fb->base;
+}
+
+static const struct drm_mode_config_funcs cirrus_mode_funcs = {
+	.fb_create = cirrus_user_framebuffer_create,
+};
+
+/* Unmap the framebuffer from the core and release the memory */
+static void cirrus_vram_fini(struct cirrus_device *cdev)
+{
+	iounmap(cdev->rmmio);
+	cdev->rmmio = NULL;
+	if (cdev->mc.vram_base)
+		release_mem_region(cdev->mc.vram_base, cdev->mc.vram_size);
+}
+
+/* Map the framebuffer from the card and configure the core */
+static int cirrus_vram_init(struct cirrus_device *cdev)
+{
+	/* BAR 0 is VRAM */
+	cdev->mc.vram_base = pci_resource_start(cdev->dev->pdev, 0);
+	/* We have 4MB of VRAM */
+	cdev->mc.vram_size = 4 * 1024 * 1024;
+
+	if (!request_mem_region(cdev->mc.vram_base, cdev->mc.vram_size,
+				"cirrusdrmfb_vram")) {
+		DRM_ERROR("can't reserve VRAM\n");
+		return -ENXIO;
+	}
+
+	return 0;
+}
+
+/*
+ * Our emulated hardware has two sets of memory. One is video RAM and can
+ * simply be used as a linear framebuffer - the other provides mmio access
+ * to the display registers. The latter can also be accessed via IO port
+ * access, but we map the range and use mmio to program them instead
+ */
+
+int cirrus_device_init(struct cirrus_device *cdev,
+		       struct drm_device *ddev,
+		       struct pci_dev *pdev, uint32_t flags)
+{
+	int ret;
+
+	cdev->dev = ddev;
+	cdev->flags = flags;
+
+	/* Hardcode the number of CRTCs to 1 */
+	cdev->num_crtc = 1;
+
+	/* BAR 0 is the framebuffer, BAR 1 contains registers */
+	cdev->rmmio_base = pci_resource_start(cdev->dev->pdev, 1);
+	cdev->rmmio_size = pci_resource_len(cdev->dev->pdev, 1);
+
+	if (!request_mem_region(cdev->rmmio_base, cdev->rmmio_size,
+				"cirrusdrmfb_mmio")) {
+		DRM_ERROR("can't reserve mmio registers\n");
+		return -ENOMEM;
+	}
+
+	cdev->rmmio = ioremap(cdev->rmmio_base, cdev->rmmio_size);
+
+	if (cdev->rmmio == NULL)
+		return -ENOMEM;
+
+	ret = cirrus_vram_init(cdev);
+	if (ret) {
+		release_mem_region(cdev->rmmio_base, cdev->rmmio_size);
+		return ret;
+	}
+
+	return 0;
+}
+
+void cirrus_device_fini(struct cirrus_device *cdev)
+{
+	release_mem_region(cdev->rmmio_base, cdev->rmmio_size);
+	cirrus_vram_fini(cdev);
+}
+
+/*
+ * Functions here will be called by the core once it's bound the driver to
+ * a PCI device
+ */
+
+int cirrus_driver_load(struct drm_device *dev, unsigned long flags)
+{
+	struct cirrus_device *cdev;
+	int r;
+
+	cdev = kzalloc(sizeof(struct cirrus_device), GFP_KERNEL);
+	if (cdev == NULL)
+		return -ENOMEM;
+	dev->dev_private = (void *)cdev;
+
+	r = cirrus_device_init(cdev, dev, dev->pdev, flags);
+	if (r) {
+		dev_err(&dev->pdev->dev, "Fatal error during GPU init: %d\n", r);
+		goto out;
+	}
+
+	r = cirrus_mm_init(cdev);
+	if (r)
+		dev_err(&dev->pdev->dev, "fatal err on mm init\n");
+
+	r = cirrus_modeset_init(cdev);
+	if (r)
+		dev_err(&dev->pdev->dev, "Fatal error during modeset init: %d\n", r);
+
+	dev->mode_config.funcs = (void *)&cirrus_mode_funcs;
+out:
+	if (r)
+		cirrus_driver_unload(dev);
+	return r;
+}
+
+int cirrus_driver_unload(struct drm_device *dev)
+{
+	struct cirrus_device *cdev = dev->dev_private;
+
+	if (cdev == NULL)
+		return 0;
+	cirrus_modeset_fini(cdev);
+	cirrus_mm_fini(cdev);
+	cirrus_device_fini(cdev);
+	kfree(cdev);
+	dev->dev_private = NULL;
+	return 0;
+}
+
+int cirrus_gem_create(struct drm_device *dev,
+		   u32 size, bool iskernel,
+		   struct drm_gem_object **obj)
+{
+	struct cirrus_bo *cirrusbo;
+	int ret;
+
+	*obj = NULL;
+
+	size = roundup(size, PAGE_SIZE);
+	if (size == 0)
+		return -EINVAL;
+
+	ret = cirrus_bo_create(dev, size, 0, 0, &cirrusbo);
+	if (ret) {
+		if (ret != -ERESTARTSYS)
+			DRM_ERROR("failed to allocate GEM object\n");
+		return ret;
+	}
+	*obj = &cirrusbo->gem;
+	return 0;
+}
+
+int cirrus_dumb_create(struct drm_file *file,
+		    struct drm_device *dev,
+		    struct drm_mode_create_dumb *args)
+{
+	int ret;
+	struct drm_gem_object *gobj;
+	u32 handle;
+
+	args->pitch = args->width * ((args->bpp + 7) / 8);
+	args->size = args->pitch * args->height;
+
+	ret = cirrus_gem_create(dev, args->size, false,
+			     &gobj);
+	if (ret)
+		return ret;
+
+	ret = drm_gem_handle_create(file, gobj, &handle);
+	drm_gem_object_unreference_unlocked(gobj);
+	if (ret)
+		return ret;
+
+	args->handle = handle;
+	return 0;
+}
+
+int cirrus_dumb_destroy(struct drm_file *file,
+		     struct drm_device *dev,
+		     uint32_t handle)
+{
+	return drm_gem_handle_delete(file, handle);
+}
+
+int cirrus_gem_init_object(struct drm_gem_object *obj)
+{
+	BUG();
+	return 0;
+}
+
+void cirrus_bo_unref(struct cirrus_bo **bo)
+{
+	struct ttm_buffer_object *tbo;
+
+	if ((*bo) == NULL)
+		return;
+
+	tbo = &((*bo)->bo);
+	ttm_bo_unref(&tbo);
+	if (tbo == NULL)
+		*bo = NULL;
+
+}
+
+void cirrus_gem_free_object(struct drm_gem_object *obj)
+{
+	struct cirrus_bo *cirrus_bo = gem_to_cirrus_bo(obj);
+
+	if (!cirrus_bo)
+		return;
+	cirrus_bo_unref(&cirrus_bo);
+}
+
+
+static inline u64 cirrus_bo_mmap_offset(struct cirrus_bo *bo)
+{
+	return bo->bo.addr_space_offset;
+}
+
+int
+cirrus_dumb_mmap_offset(struct drm_file *file,
+		     struct drm_device *dev,
+		     uint32_t handle,
+		     uint64_t *offset)
+{
+	struct drm_gem_object *obj;
+	int ret;
+	struct cirrus_bo *bo;
+
+	mutex_lock(&dev->struct_mutex);
+	obj = drm_gem_object_lookup(dev, file, handle);
+	if (obj == NULL) {
+		ret = -ENOENT;
+		goto out_unlock;
+	}
+
+	bo = gem_to_cirrus_bo(obj);
+	*offset = cirrus_bo_mmap_offset(bo);
+
+	drm_gem_object_unreference(obj);
+	ret = 0;
+out_unlock:
+	mutex_unlock(&dev->struct_mutex);
+	return ret;
+
+}
diff --git a/drivers/gpu/drm/cirrus/cirrus_mode.c b/drivers/gpu/drm/cirrus/cirrus_mode.c
new file mode 100644
index 00000000000..100f6308c50
--- /dev/null
+++ b/drivers/gpu/drm/cirrus/cirrus_mode.c
@@ -0,0 +1,629 @@
+
+/*
+ * Copyright 2012 Red Hat
+ *
+ * This file is subject to the terms and conditions of the GNU General
+ * Public License version 2. See the file COPYING in the main
+ * directory of this archive for more details.
+ *
+ * Authors: Matthew Garrett
+ *          Dave Airlie
+ *
+ * Portions of this code derived from cirrusfb.c:
+ * drivers/video/cirrusfb.c - driver for Cirrus Logic chipsets
+ *
+ * Copyright 1999-2001 Jeff Garzik <jgarzik@pobox.com>
+ */
+#include "drmP.h"
+#include "drm.h"
+#include "drm_crtc_helper.h"
+
+#include <video/cirrus.h>
+
+#include "cirrus_drv.h"
+
+#define CIRRUS_LUT_SIZE 256
+
+#define PALETTE_INDEX 0x8
+#define PALETTE_DATA 0x9
+
+/*
+ * This file contains setup code for the CRTC.
+ */
+
+static void cirrus_crtc_load_lut(struct drm_crtc *crtc)
+{
+	struct cirrus_crtc *cirrus_crtc = to_cirrus_crtc(crtc);
+	struct drm_device *dev = crtc->dev;
+	struct cirrus_device *cdev = dev->dev_private;
+	int i;
+
+	if (!crtc->enabled)
+		return;
+
+	for (i = 0; i < CIRRUS_LUT_SIZE; i++) {
+		/* VGA registers */
+		WREG8(PALETTE_INDEX, i);
+		WREG8(PALETTE_DATA, cirrus_crtc->lut_r[i]);
+		WREG8(PALETTE_DATA, cirrus_crtc->lut_g[i]);
+		WREG8(PALETTE_DATA, cirrus_crtc->lut_b[i]);
+	}
+}
+
+/*
+ * The DRM core requires DPMS functions, but they make little sense in our
+ * case and so are just stubs
+ */
+
+static void cirrus_crtc_dpms(struct drm_crtc *crtc, int mode)
+{
+	struct drm_device *dev = crtc->dev;
+	struct cirrus_device *cdev = dev->dev_private;
+	u8 sr01, gr0e;
+
+	switch (mode) {
+	case DRM_MODE_DPMS_ON:
+		sr01 = 0x00;
+		gr0e = 0x00;
+		break;
+	case DRM_MODE_DPMS_STANDBY:
+		sr01 = 0x20;
+		gr0e = 0x02;
+		break;
+	case DRM_MODE_DPMS_SUSPEND:
+		sr01 = 0x20;
+		gr0e = 0x04;
+		break;
+	case DRM_MODE_DPMS_OFF:
+		sr01 = 0x20;
+		gr0e = 0x06;
+		break;
+	default:
+		return;
+	}
+
+	WREG8(SEQ_INDEX, 0x1);
+	sr01 |= RREG8(SEQ_DATA) & ~0x20;
+	WREG_SEQ(0x1, sr01);
+
+	WREG8(GFX_INDEX, 0xe);
+	gr0e |= RREG8(GFX_DATA) & ~0x06;
+	WREG_GFX(0xe, gr0e);
+}
+
+/*
+ * The core passes the desired mode to the CRTC code to see whether any
+ * CRTC-specific modifications need to be made to it. We're in a position
+ * to just pass that straight through, so this does nothing
+ */
+static bool cirrus_crtc_mode_fixup(struct drm_crtc *crtc,
+				   struct drm_display_mode *mode,
+				   struct drm_display_mode *adjusted_mode)
+{
+	return true;
+}
+
+void cirrus_set_start_address(struct drm_crtc *crtc, unsigned offset)
+{
+	struct cirrus_device *cdev = crtc->dev->dev_private;
+	u32 addr;
+	u8 tmp;
+
+	addr = offset >> 2;
+	WREG_CRT(0x0c, (u8)((addr >> 8) & 0xff));
+	WREG_CRT(0x0d, (u8)(addr & 0xff));
+
+	WREG8(CRT_INDEX, 0x1b);
+	tmp = RREG8(CRT_DATA);
+	tmp &= 0xf2;
+	tmp |= (addr >> 16) & 0x01;
+	tmp |= (addr >> 15) & 0x0c;
+	WREG_CRT(0x1b, tmp);
+	WREG8(CRT_INDEX, 0x1d);
+	tmp = RREG8(CRT_DATA);
+	tmp &= 0x7f;
+	tmp |= (addr >> 12) & 0x80;
+	WREG_CRT(0x1d, tmp);
+}
+
+/* cirrus is different - we will force move buffers out of VRAM */
+static int cirrus_crtc_do_set_base(struct drm_crtc *crtc,
+				struct drm_framebuffer *fb,
+				int x, int y, int atomic)
+{
+	struct cirrus_device *cdev = crtc->dev->dev_private;
+	struct drm_gem_object *obj;
+	struct cirrus_framebuffer *cirrus_fb;
+	struct cirrus_bo *bo;
+	int ret;
+	u64 gpu_addr;
+
+	/* push the previous fb to system ram */
+	if (!atomic && fb) {
+		cirrus_fb = to_cirrus_framebuffer(fb);
+		obj = cirrus_fb->obj;
+		bo = gem_to_cirrus_bo(obj);
+		ret = cirrus_bo_reserve(bo, false);
+		if (ret)
+			return ret;
+		cirrus_bo_push_sysram(bo);
+		cirrus_bo_unreserve(bo);
+	}
+
+	cirrus_fb = to_cirrus_framebuffer(crtc->fb);
+	obj = cirrus_fb->obj;
+	bo = gem_to_cirrus_bo(obj);
+
+	ret = cirrus_bo_reserve(bo, false);
+	if (ret)
+		return ret;
+
+	ret = cirrus_bo_pin(bo, TTM_PL_FLAG_VRAM, &gpu_addr);
+	if (ret) {
+		cirrus_bo_unreserve(bo);
+		return ret;
+	}
+
+	if (&cdev->mode_info.gfbdev->gfb == cirrus_fb) {
+		/* if pushing console in kmap it */
+		ret = ttm_bo_kmap(&bo->bo, 0, bo->bo.num_pages, &bo->kmap);
+		if (ret)
+			DRM_ERROR("failed to kmap fbcon\n");
+	}
+	cirrus_bo_unreserve(bo);
+
+	cirrus_set_start_address(crtc, (u32)gpu_addr);
+	return 0;
+}
+
+static int cirrus_crtc_mode_set_base(struct drm_crtc *crtc, int x, int y,
+			     struct drm_framebuffer *old_fb)
+{
+	return cirrus_crtc_do_set_base(crtc, old_fb, x, y, 0);
+}
+
+/*
+ * The meat of this driver. The core passes us a mode and we have to program
+ * it. The modesetting here is the bare minimum required to satisfy the qemu
+ * emulation of this hardware, and running this against a real device is
+ * likely to result in an inadequately programmed mode. We've already had
+ * the opportunity to modify the mode, so whatever we receive here should
+ * be something that can be correctly programmed and displayed
+ */
+static int cirrus_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 cirrus_device *cdev = dev->dev_private;
+	int hsyncstart, hsyncend, htotal, hdispend;
+	int vtotal, vdispend;
+	int tmp;
+	int sr07 = 0, hdr = 0;
+
+	htotal = mode->htotal / 8;
+	hsyncend = mode->hsync_end / 8;
+	hsyncstart = mode->hsync_start / 8;
+	hdispend = mode->hdisplay / 8;
+
+	vtotal = mode->vtotal;
+	vdispend = mode->vdisplay;
+
+	vdispend -= 1;
+	vtotal -= 2;
+
+	htotal -= 5;
+	hdispend -= 1;
+	hsyncstart += 1;
+	hsyncend += 1;
+
+	WREG_CRT(VGA_CRTC_V_SYNC_END, 0x20);
+	WREG_CRT(VGA_CRTC_H_TOTAL, htotal);
+	WREG_CRT(VGA_CRTC_H_DISP, hdispend);
+	WREG_CRT(VGA_CRTC_H_SYNC_START, hsyncstart);
+	WREG_CRT(VGA_CRTC_H_SYNC_END, hsyncend);
+	WREG_CRT(VGA_CRTC_V_TOTAL, vtotal & 0xff);
+	WREG_CRT(VGA_CRTC_V_DISP_END, vdispend & 0xff);
+
+	tmp = 0x40;
+	if ((vdispend + 1) & 512)
+		tmp |= 0x20;
+	WREG_CRT(VGA_CRTC_MAX_SCAN, tmp);
+
+	/*
+	 * Overflow bits for values that don't fit in the standard registers
+	 */
+	tmp = 16;
+	if (vtotal & 256)
+		tmp |= 1;
+	if (vdispend & 256)
+		tmp |= 2;
+	if ((vdispend + 1) & 256)
+		tmp |= 8;
+	if (vtotal & 512)
+		tmp |= 32;
+	if (vdispend & 512)
+		tmp |= 64;
+	WREG_CRT(VGA_CRTC_OVERFLOW, tmp);
+
+	tmp = 0;
+
+	/* More overflow bits */
+
+	if ((htotal + 5) & 64)
+		tmp |= 16;
+	if ((htotal + 5) & 128)
+		tmp |= 32;
+	if (vtotal & 256)
+		tmp |= 64;
+	if (vtotal & 512)
+		tmp |= 128;
+
+	WREG_CRT(CL_CRT1A, tmp);
+
+	/* Disable Hercules/CGA compatibility */
+	WREG_CRT(VGA_CRTC_MODE, 0x03);
+
+	WREG8(SEQ_INDEX, 0x7);
+	sr07 = RREG8(SEQ_DATA);
+	sr07 &= 0xe0;
+	hdr = 0;
+	switch (crtc->fb->bits_per_pixel) {
+	case 8:
+		sr07 |= 0x11;
+		break;
+	case 16:
+		sr07 |= 0xc1;
+		hdr = 0xc0;
+		break;
+	case 24:
+		sr07 |= 0x15;
+		hdr = 0xc5;
+		break;
+	case 32:
+		sr07 |= 0x19;
+		hdr = 0xc5;
+		break;
+	default:
+		return -1;
+	}
+
+	WREG_SEQ(0x7, sr07);
+
+	/* Program the pitch */
+	tmp = crtc->fb->pitches[0] / 8;
+	WREG_CRT(VGA_CRTC_OFFSET, tmp);
+
+	/* Enable extended blanking and pitch bits, and enable full memory */
+	tmp = 0x22;
+	tmp |= (crtc->fb->pitches[0] >> 7) & 0x10;
+	tmp |= (crtc->fb->pitches[0] >> 6) & 0x40;
+	WREG_CRT(0x1b, tmp);
+
+	/* Enable high-colour modes */
+	WREG_GFX(VGA_GFX_MODE, 0x40);
+
+	/* And set graphics mode */
+	WREG_GFX(VGA_GFX_MISC, 0x01);
+
+	WREG_HDR(hdr);
+	cirrus_crtc_do_set_base(crtc, old_fb, x, y, 0);
+	return 0;
+}
+
+/*
+ * This is called before a mode is programmed. A typical use might be to
+ * enable DPMS during the programming to avoid seeing intermediate stages,
+ * but that's not relevant to us
+ */
+static void cirrus_crtc_prepare(struct drm_crtc *crtc)
+{
+}
+
+/*
+ * This is called after a mode is programmed. It should reverse anything done
+ * by the prepare function
+ */
+static void cirrus_crtc_commit(struct drm_crtc *crtc)
+{
+}
+
+/*
+ * The core can pass us a set of gamma values to program. We actually only
+ * use this for 8-bit mode so can't perform smooth fades on deeper modes,
+ * but it's a requirement that we provide the function
+ */
+static void cirrus_crtc_gamma_set(struct drm_crtc *crtc, u16 *red, u16 *green,
+				  u16 *blue, uint32_t start, uint32_t size)
+{
+	struct cirrus_crtc *cirrus_crtc = to_cirrus_crtc(crtc);
+	int i;
+
+	if (size != CIRRUS_LUT_SIZE)
+		return;
+
+	for (i = 0; i < CIRRUS_LUT_SIZE; i++) {
+		cirrus_crtc->lut_r[i] = red[i];
+		cirrus_crtc->lut_g[i] = green[i];
+		cirrus_crtc->lut_b[i] = blue[i];
+	}
+	cirrus_crtc_load_lut(crtc);
+}
+
+/* Simple cleanup function */
+static void cirrus_crtc_destroy(struct drm_crtc *crtc)
+{
+	struct cirrus_crtc *cirrus_crtc = to_cirrus_crtc(crtc);
+
+	drm_crtc_cleanup(crtc);
+	kfree(cirrus_crtc);
+}
+
+/* These provide the minimum set of functions required to handle a CRTC */
+static const struct drm_crtc_funcs cirrus_crtc_funcs = {
+	.gamma_set = cirrus_crtc_gamma_set,
+	.set_config = drm_crtc_helper_set_config,
+	.destroy = cirrus_crtc_destroy,
+};
+
+static const struct drm_crtc_helper_funcs cirrus_helper_funcs = {
+	.dpms = cirrus_crtc_dpms,
+	.mode_fixup = cirrus_crtc_mode_fixup,
+	.mode_set = cirrus_crtc_mode_set,
+	.mode_set_base = cirrus_crtc_mode_set_base,
+	.prepare = cirrus_crtc_prepare,
+	.commit = cirrus_crtc_commit,
+	.load_lut = cirrus_crtc_load_lut,
+};
+
+/* CRTC setup */
+static void cirrus_crtc_init(struct drm_device *dev)
+{
+	struct cirrus_device *cdev = dev->dev_private;
+	struct cirrus_crtc *cirrus_crtc;
+	int i;
+
+	cirrus_crtc = kzalloc(sizeof(struct cirrus_crtc) +
+			      (CIRRUSFB_CONN_LIMIT * sizeof(struct drm_connector *)),
+			      GFP_KERNEL);
+
+	if (cirrus_crtc == NULL)
+		return;
+
+	drm_crtc_init(dev, &cirrus_crtc->base, &cirrus_crtc_funcs);
+
+	drm_mode_crtc_set_gamma_size(&cirrus_crtc->base, CIRRUS_LUT_SIZE);
+	cdev->mode_info.crtc = cirrus_crtc;
+
+	for (i = 0; i < CIRRUS_LUT_SIZE; i++) {
+		cirrus_crtc->lut_r[i] = i;
+		cirrus_crtc->lut_g[i] = i;
+		cirrus_crtc->lut_b[i] = i;
+	}
+
+	drm_crtc_helper_add(&cirrus_crtc->base, &cirrus_helper_funcs);
+}
+
+/** Sets the color ramps on behalf of fbcon */
+void cirrus_crtc_fb_gamma_set(struct drm_crtc *crtc, u16 red, u16 green,
+			      u16 blue, int regno)
+{
+	struct cirrus_crtc *cirrus_crtc = to_cirrus_crtc(crtc);
+
+	cirrus_crtc->lut_r[regno] = red;
+	cirrus_crtc->lut_g[regno] = green;
+	cirrus_crtc->lut_b[regno] = blue;
+}
+
+/** Gets the color ramps on behalf of fbcon */
+void cirrus_crtc_fb_gamma_get(struct drm_crtc *crtc, u16 *red, u16 *green,
+			      u16 *blue, int regno)
+{
+	struct cirrus_crtc *cirrus_crtc = to_cirrus_crtc(crtc);
+
+	*red = cirrus_crtc->lut_r[regno];
+	*green = cirrus_crtc->lut_g[regno];
+	*blue = cirrus_crtc->lut_b[regno];
+}
+
+
+static bool cirrus_encoder_mode_fixup(struct drm_encoder *encoder,
+				  struct drm_display_mode *mode,
+				  struct drm_display_mode *adjusted_mode)
+{
+	return true;
+}
+
+static void cirrus_encoder_mode_set(struct drm_encoder *encoder,
+				struct drm_display_mode *mode,
+				struct drm_display_mode *adjusted_mode)
+{
+}
+
+static void cirrus_encoder_dpms(struct drm_encoder *encoder, int state)
+{
+	return;
+}
+
+static void cirrus_encoder_prepare(struct drm_encoder *encoder)
+{
+}
+
+static void cirrus_encoder_commit(struct drm_encoder *encoder)
+{
+}
+
+void cirrus_encoder_destroy(struct drm_encoder *encoder)
+{
+	struct cirrus_encoder *cirrus_encoder = to_cirrus_encoder(encoder);
+	drm_encoder_cleanup(encoder);
+	kfree(cirrus_encoder);
+}
+
+static const struct drm_encoder_helper_funcs cirrus_encoder_helper_funcs = {
+	.dpms = cirrus_encoder_dpms,
+	.mode_fixup = cirrus_encoder_mode_fixup,
+	.mode_set = cirrus_encoder_mode_set,
+	.prepare = cirrus_encoder_prepare,
+	.commit = cirrus_encoder_commit,
+};
+
+static const struct drm_encoder_funcs cirrus_encoder_encoder_funcs = {
+	.destroy = cirrus_encoder_destroy,
+};
+
+static struct drm_encoder *cirrus_encoder_init(struct drm_device *dev)
+{
+	struct drm_encoder *encoder;
+	struct cirrus_encoder *cirrus_encoder;
+
+	cirrus_encoder = kzalloc(sizeof(struct cirrus_encoder), GFP_KERNEL);
+	if (!cirrus_encoder)
+		return NULL;
+
+	encoder = &cirrus_encoder->base;
+	encoder->possible_crtcs = 0x1;
+
+	drm_encoder_init(dev, encoder, &cirrus_encoder_encoder_funcs,
+			 DRM_MODE_ENCODER_DAC);
+	drm_encoder_helper_add(encoder, &cirrus_encoder_helper_funcs);
+
+	return encoder;
+}
+
+
+int cirrus_vga_get_modes(struct drm_connector *connector)
+{
+	/* Just add a static list of modes */
+	drm_add_modes_noedid(connector, 640, 480);
+	drm_add_modes_noedid(connector, 800, 600);
+	drm_add_modes_noedid(connector, 1024, 768);
+	drm_add_modes_noedid(connector, 1280, 1024);
+
+	return 4;
+}
+
+static int cirrus_vga_mode_valid(struct drm_connector *connector,
+				 struct drm_display_mode *mode)
+{
+	/* Any mode we've added is valid */
+	return MODE_OK;
+}
+
+struct drm_encoder *cirrus_connector_best_encoder(struct drm_connector
+						  *connector)
+{
+	int enc_id = connector->encoder_ids[0];
+	struct drm_mode_object *obj;
+	struct drm_encoder *encoder;
+
+	/* pick the encoder ids */
+	if (enc_id) {
+		obj =
+		    drm_mode_object_find(connector->dev, enc_id,
+					 DRM_MODE_OBJECT_ENCODER);
+		if (!obj)
+			return NULL;
+		encoder = obj_to_encoder(obj);
+		return encoder;
+	}
+	return NULL;
+}
+
+static enum drm_connector_status cirrus_vga_detect(struct drm_connector
+						   *connector, bool force)
+{
+	return connector_status_connected;
+}
+
+static void cirrus_connector_destroy(struct drm_connector *connector)
+{
+	drm_connector_cleanup(connector);
+	kfree(connector);
+}
+
+struct drm_connector_helper_funcs cirrus_vga_connector_helper_funcs = {
+	.get_modes = cirrus_vga_get_modes,
+	.mode_valid = cirrus_vga_mode_valid,
+	.best_encoder = cirrus_connector_best_encoder,
+};
+
+struct drm_connector_funcs cirrus_vga_connector_funcs = {
+	.dpms = drm_helper_connector_dpms,
+	.detect = cirrus_vga_detect,
+	.fill_modes = drm_helper_probe_single_connector_modes,
+	.destroy = cirrus_connector_destroy,
+};
+
+static struct drm_connector *cirrus_vga_init(struct drm_device *dev)
+{
+	struct drm_connector *connector;
+	struct cirrus_connector *cirrus_connector;
+
+	cirrus_connector = kzalloc(sizeof(struct cirrus_connector), GFP_KERNEL);
+	if (!cirrus_connector)
+		return NULL;
+
+	connector = &cirrus_connector->base;
+
+	drm_connector_init(dev, connector,
+			   &cirrus_vga_connector_funcs, DRM_MODE_CONNECTOR_VGA);
+
+	drm_connector_helper_add(connector, &cirrus_vga_connector_helper_funcs);
+
+	return connector;
+}
+
+
+int cirrus_modeset_init(struct cirrus_device *cdev)
+{
+	struct drm_encoder *encoder;
+	struct drm_connector *connector;
+	int ret;
+
+	drm_mode_config_init(cdev->dev);
+	cdev->mode_info.mode_config_initialized = true;
+
+	cdev->dev->mode_config.max_width = CIRRUS_MAX_FB_WIDTH;
+	cdev->dev->mode_config.max_height = CIRRUS_MAX_FB_HEIGHT;
+
+	cdev->dev->mode_config.fb_base = cdev->mc.vram_base;
+	cdev->dev->mode_config.preferred_depth = 24;
+	/* don't prefer a shadow on virt GPU */
+	cdev->dev->mode_config.prefer_shadow = 0;
+
+	cirrus_crtc_init(cdev->dev);
+
+	encoder = cirrus_encoder_init(cdev->dev);
+	if (!encoder) {
+		DRM_ERROR("cirrus_encoder_init failed\n");
+		return -1;
+	}
+
+	connector = cirrus_vga_init(cdev->dev);
+	if (!connector) {
+		DRM_ERROR("cirrus_vga_init failed\n");
+		return -1;
+	}
+
+	drm_mode_connector_attach_encoder(connector, encoder);
+
+	ret = cirrus_fbdev_init(cdev);
+	if (ret) {
+		DRM_ERROR("cirrus_fbdev_init failed\n");
+		return ret;
+	}
+
+	return 0;
+}
+
+void cirrus_modeset_fini(struct cirrus_device *cdev)
+{
+	cirrus_fbdev_fini(cdev);
+
+	if (cdev->mode_info.mode_config_initialized) {
+		drm_mode_config_cleanup(cdev->dev);
+		cdev->mode_info.mode_config_initialized = false;
+	}
+}
diff --git a/drivers/gpu/drm/cirrus/cirrus_ttm.c b/drivers/gpu/drm/cirrus/cirrus_ttm.c
new file mode 100644
index 00000000000..2ebcd11a502
--- /dev/null
+++ b/drivers/gpu/drm/cirrus/cirrus_ttm.c
@@ -0,0 +1,453 @@
+/*
+ * 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, 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: Dave Airlie <airlied@redhat.com>
+ */
+#include "drmP.h"
+#include "cirrus_drv.h"
+#include <ttm/ttm_page_alloc.h>
+
+static inline struct cirrus_device *
+cirrus_bdev(struct ttm_bo_device *bd)
+{
+	return container_of(bd, struct cirrus_device, ttm.bdev);
+}
+
+static int
+cirrus_ttm_mem_global_init(struct drm_global_reference *ref)
+{
+	return ttm_mem_global_init(ref->object);
+}
+
+static void
+cirrus_ttm_mem_global_release(struct drm_global_reference *ref)
+{
+	ttm_mem_global_release(ref->object);
+}
+
+static int cirrus_ttm_global_init(struct cirrus_device *cirrus)
+{
+	struct drm_global_reference *global_ref;
+	int r;
+
+	global_ref = &cirrus->ttm.mem_global_ref;
+	global_ref->global_type = DRM_GLOBAL_TTM_MEM;
+	global_ref->size = sizeof(struct ttm_mem_global);
+	global_ref->init = &cirrus_ttm_mem_global_init;
+	global_ref->release = &cirrus_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;
+	}
+
+	cirrus->ttm.bo_global_ref.mem_glob =
+		cirrus->ttm.mem_global_ref.object;
+	global_ref = &cirrus->ttm.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(&cirrus->ttm.mem_global_ref);
+		return r;
+	}
+	return 0;
+}
+
+void
+cirrus_ttm_global_release(struct cirrus_device *cirrus)
+{
+	if (cirrus->ttm.mem_global_ref.release == NULL)
+		return;
+
+	drm_global_item_unref(&cirrus->ttm.bo_global_ref.ref);
+	drm_global_item_unref(&cirrus->ttm.mem_global_ref);
+	cirrus->ttm.mem_global_ref.release = NULL;
+}
+
+
+static void cirrus_bo_ttm_destroy(struct ttm_buffer_object *tbo)
+{
+	struct cirrus_bo *bo;
+
+	bo = container_of(tbo, struct cirrus_bo, bo);
+
+	drm_gem_object_release(&bo->gem);
+	kfree(bo);
+}
+
+bool cirrus_ttm_bo_is_cirrus_bo(struct ttm_buffer_object *bo)
+{
+	if (bo->destroy == &cirrus_bo_ttm_destroy)
+		return true;
+	return false;
+}
+
+static int
+cirrus_bo_init_mem_type(struct ttm_bo_device *bdev, uint32_t type,
+		     struct ttm_mem_type_manager *man)
+{
+	switch (type) {
+	case TTM_PL_SYSTEM:
+		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:
+		man->func = &ttm_bo_manager_func;
+		man->flags = TTM_MEMTYPE_FLAG_FIXED |
+			TTM_MEMTYPE_FLAG_MAPPABLE;
+		man->available_caching = TTM_PL_FLAG_UNCACHED |
+			TTM_PL_FLAG_WC;
+		man->default_caching = TTM_PL_FLAG_WC;
+		break;
+	default:
+		DRM_ERROR("Unsupported memory type %u\n", (unsigned)type);
+		return -EINVAL;
+	}
+	return 0;
+}
+
+static void
+cirrus_bo_evict_flags(struct ttm_buffer_object *bo, struct ttm_placement *pl)
+{
+	struct cirrus_bo *cirrusbo = cirrus_bo(bo);
+
+	if (!cirrus_ttm_bo_is_cirrus_bo(bo))
+		return;
+
+	cirrus_ttm_placement(cirrusbo, TTM_PL_FLAG_SYSTEM);
+	*pl = cirrusbo->placement;
+}
+
+static int cirrus_bo_verify_access(struct ttm_buffer_object *bo, struct file *filp)
+{
+	return 0;
+}
+
+static int cirrus_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 cirrus_device *cirrus = cirrus_bdev(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.offset = mem->start << PAGE_SHIFT;
+		mem->bus.base = pci_resource_start(cirrus->dev->pdev, 0);
+		mem->bus.is_iomem = true;
+		break;
+	default:
+		return -EINVAL;
+		break;
+	}
+	return 0;
+}
+
+static void cirrus_ttm_io_mem_free(struct ttm_bo_device *bdev, struct ttm_mem_reg *mem)
+{
+}
+
+static int cirrus_bo_move(struct ttm_buffer_object *bo,
+		       bool evict, bool interruptible,
+		       bool no_wait_reserve, bool no_wait_gpu,
+		       struct ttm_mem_reg *new_mem)
+{
+	int r;
+	r = ttm_bo_move_memcpy(bo, evict, no_wait_reserve, no_wait_gpu, new_mem);
+	return r;
+}
+
+
+static void cirrus_ttm_backend_destroy(struct ttm_tt *tt)
+{
+	ttm_tt_fini(tt);
+	kfree(tt);
+}
+
+static struct ttm_backend_func cirrus_tt_backend_func = {
+	.destroy = &cirrus_ttm_backend_destroy,
+};
+
+
+struct ttm_tt *cirrus_ttm_tt_create(struct ttm_bo_device *bdev,
+				 unsigned long size, uint32_t page_flags,
+				 struct page *dummy_read_page)
+{
+	struct ttm_tt *tt;
+
+	tt = kzalloc(sizeof(struct ttm_tt), GFP_KERNEL);
+	if (tt == NULL)
+		return NULL;
+	tt->func = &cirrus_tt_backend_func;
+	if (ttm_tt_init(tt, bdev, size, page_flags, dummy_read_page)) {
+		kfree(tt);
+		return NULL;
+	}
+	return tt;
+}
+
+static int cirrus_ttm_tt_populate(struct ttm_tt *ttm)
+{
+	return ttm_pool_populate(ttm);
+}
+
+static void cirrus_ttm_tt_unpopulate(struct ttm_tt *ttm)
+{
+	ttm_pool_unpopulate(ttm);
+}
+
+struct ttm_bo_driver cirrus_bo_driver = {
+	.ttm_tt_create = cirrus_ttm_tt_create,
+	.ttm_tt_populate = cirrus_ttm_tt_populate,
+	.ttm_tt_unpopulate = cirrus_ttm_tt_unpopulate,
+	.init_mem_type = cirrus_bo_init_mem_type,
+	.evict_flags = cirrus_bo_evict_flags,
+	.move = cirrus_bo_move,
+	.verify_access = cirrus_bo_verify_access,
+	.io_mem_reserve = &cirrus_ttm_io_mem_reserve,
+	.io_mem_free = &cirrus_ttm_io_mem_free,
+};
+
+int cirrus_mm_init(struct cirrus_device *cirrus)
+{
+	int ret;
+	struct drm_device *dev = cirrus->dev;
+	struct ttm_bo_device *bdev = &cirrus->ttm.bdev;
+
+	ret = cirrus_ttm_global_init(cirrus);
+	if (ret)
+		return ret;
+
+	ret = ttm_bo_device_init(&cirrus->ttm.bdev,
+				 cirrus->ttm.bo_global_ref.ref.object,
+				 &cirrus_bo_driver, DRM_FILE_PAGE_OFFSET,
+				 true);
+	if (ret) {
+		DRM_ERROR("Error initialising bo driver; %d\n", ret);
+		return ret;
+	}
+
+	ret = ttm_bo_init_mm(bdev, TTM_PL_VRAM,
+			     cirrus->mc.vram_size >> PAGE_SHIFT);
+	if (ret) {
+		DRM_ERROR("Failed ttm VRAM init: %d\n", ret);
+		return ret;
+	}
+
+	cirrus->fb_mtrr = drm_mtrr_add(pci_resource_start(dev->pdev, 0),
+				    pci_resource_len(dev->pdev, 0),
+				    DRM_MTRR_WC);
+
+	return 0;
+}
+
+void cirrus_mm_fini(struct cirrus_device *cirrus)
+{
+	struct drm_device *dev = cirrus->dev;
+	ttm_bo_device_release(&cirrus->ttm.bdev);
+
+	cirrus_ttm_global_release(cirrus);
+
+	if (cirrus->fb_mtrr >= 0) {
+		drm_mtrr_del(cirrus->fb_mtrr,
+			     pci_resource_start(dev->pdev, 0),
+			     pci_resource_len(dev->pdev, 0), DRM_MTRR_WC);
+		cirrus->fb_mtrr = -1;
+	}
+}
+
+void cirrus_ttm_placement(struct cirrus_bo *bo, int domain)
+{
+	u32 c = 0;
+	bo->placement.fpfn = 0;
+	bo->placement.lpfn = 0;
+	bo->placement.placement = bo->placements;
+	bo->placement.busy_placement = bo->placements;
+	if (domain & TTM_PL_FLAG_VRAM)
+		bo->placements[c++] = TTM_PL_FLAG_WC | TTM_PL_FLAG_UNCACHED | TTM_PL_FLAG_VRAM;
+	if (domain & TTM_PL_FLAG_SYSTEM)
+		bo->placements[c++] = TTM_PL_MASK_CACHING | TTM_PL_FLAG_SYSTEM;
+	if (!c)
+		bo->placements[c++] = TTM_PL_MASK_CACHING | TTM_PL_FLAG_SYSTEM;
+	bo->placement.num_placement = c;
+	bo->placement.num_busy_placement = c;
+}
+
+int cirrus_bo_reserve(struct cirrus_bo *bo, bool no_wait)
+{
+	int ret;
+
+	ret = ttm_bo_reserve(&bo->bo, true, no_wait, false, 0);
+	if (ret) {
+		if (ret != -ERESTARTSYS)
+			DRM_ERROR("reserve failed %p\n", bo);
+		return ret;
+	}
+	return 0;
+}
+
+void cirrus_bo_unreserve(struct cirrus_bo *bo)
+{
+	ttm_bo_unreserve(&bo->bo);
+}
+
+int cirrus_bo_create(struct drm_device *dev, int size, int align,
+		  uint32_t flags, struct cirrus_bo **pcirrusbo)
+{
+	struct cirrus_device *cirrus = dev->dev_private;
+	struct cirrus_bo *cirrusbo;
+	size_t acc_size;
+	int ret;
+
+	cirrusbo = kzalloc(sizeof(struct cirrus_bo), GFP_KERNEL);
+	if (!cirrusbo)
+		return -ENOMEM;
+
+	ret = drm_gem_object_init(dev, &cirrusbo->gem, size);
+	if (ret) {
+		kfree(cirrusbo);
+		return ret;
+	}
+
+	cirrusbo->gem.driver_private = NULL;
+	cirrusbo->bo.bdev = &cirrus->ttm.bdev;
+
+	cirrus_ttm_placement(cirrusbo, TTM_PL_FLAG_VRAM | TTM_PL_FLAG_SYSTEM);
+
+	acc_size = ttm_bo_dma_acc_size(&cirrus->ttm.bdev, size,
+				       sizeof(struct cirrus_bo));
+
+	ret = ttm_bo_init(&cirrus->ttm.bdev, &cirrusbo->bo, size,
+			  ttm_bo_type_device, &cirrusbo->placement,
+			  align >> PAGE_SHIFT, 0, false, NULL, acc_size,
+			  NULL, cirrus_bo_ttm_destroy);
+	if (ret)
+		return ret;
+
+	*pcirrusbo = cirrusbo;
+	return 0;
+}
+
+static inline u64 cirrus_bo_gpu_offset(struct cirrus_bo *bo)
+{
+	return bo->bo.offset;
+}
+
+int cirrus_bo_pin(struct cirrus_bo *bo, u32 pl_flag, u64 *gpu_addr)
+{
+	int i, ret;
+
+	if (bo->pin_count) {
+		bo->pin_count++;
+		if (gpu_addr)
+			*gpu_addr = cirrus_bo_gpu_offset(bo);
+	}
+
+	cirrus_ttm_placement(bo, pl_flag);
+	for (i = 0; i < bo->placement.num_placement; i++)
+		bo->placements[i] |= TTM_PL_FLAG_NO_EVICT;
+	ret = ttm_bo_validate(&bo->bo, &bo->placement, false, false, false);
+	if (ret)
+		return ret;
+
+	bo->pin_count = 1;
+	if (gpu_addr)
+		*gpu_addr = cirrus_bo_gpu_offset(bo);
+	return 0;
+}
+
+int cirrus_bo_unpin(struct cirrus_bo *bo)
+{
+	int i, ret;
+	if (!bo->pin_count) {
+		DRM_ERROR("unpin bad %p\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;
+	ret = ttm_bo_validate(&bo->bo, &bo->placement, false, false, false);
+	if (ret)
+		return ret;
+
+	return 0;
+}
+
+int cirrus_bo_push_sysram(struct cirrus_bo *bo)
+{
+	int i, ret;
+	if (!bo->pin_count) {
+		DRM_ERROR("unpin bad %p\n", bo);
+		return 0;
+	}
+	bo->pin_count--;
+	if (bo->pin_count)
+		return 0;
+
+	if (bo->kmap.virtual)
+		ttm_bo_kunmap(&bo->kmap);
+
+	cirrus_ttm_placement(bo, TTM_PL_FLAG_SYSTEM);
+	for (i = 0; i < bo->placement.num_placement ; i++)
+		bo->placements[i] |= TTM_PL_FLAG_NO_EVICT;
+
+	ret = ttm_bo_validate(&bo->bo, &bo->placement, false, false, false);
+	if (ret) {
+		DRM_ERROR("pushing to VRAM failed\n");
+		return ret;
+	}
+	return 0;
+}
+
+int cirrus_mmap(struct file *filp, struct vm_area_struct *vma)
+{
+	struct drm_file *file_priv;
+	struct cirrus_device *cirrus;
+
+	if (unlikely(vma->vm_pgoff < DRM_FILE_PAGE_OFFSET))
+		return drm_mmap(filp, vma);
+
+	file_priv = filp->private_data;
+	cirrus = file_priv->minor->dev->dev_private;
+	return ttm_bo_mmap(filp, vma, &cirrus->ttm.bdev);
+}
diff --git a/drivers/gpu/drm/drm_cache.c b/drivers/gpu/drm/drm_cache.c
index 4b8653b932f..08758e06147 100644
--- a/drivers/gpu/drm/drm_cache.c
+++ b/drivers/gpu/drm/drm_cache.c
@@ -98,3 +98,26 @@ drm_clflush_pages(struct page *pages[], unsigned long num_pages)
 #endif
 }
 EXPORT_SYMBOL(drm_clflush_pages);
+
+void
+drm_clflush_virt_range(char *addr, unsigned long length)
+{
+#if defined(CONFIG_X86)
+	if (cpu_has_clflush) {
+		char *end = addr + length;
+		mb();
+		for (; addr < end; addr += boot_cpu_data.x86_clflush_size)
+			clflush(addr);
+		clflush(end - 1);
+		mb();
+		return;
+	}
+
+	if (on_each_cpu(drm_clflush_ipi_handler, NULL, 1) != 0)
+		printk(KERN_ERR "Timed out waiting for cache flush.\n");
+#else
+	printk(KERN_ERR "Architecture has no drm_cache.c support\n");
+	WARN_ON_ONCE(1);
+#endif
+}
+EXPORT_SYMBOL(drm_clflush_virt_range);
diff --git a/drivers/gpu/drm/drm_context.c b/drivers/gpu/drm/drm_context.c
index 325365f6d35..affa629589a 100644
--- a/drivers/gpu/drm/drm_context.c
+++ b/drivers/gpu/drm/drm_context.c
@@ -85,11 +85,12 @@ again:
 	mutex_lock(&dev->struct_mutex);
 	ret = idr_get_new_above(&dev->ctx_idr, NULL,
 				DRM_RESERVED_CONTEXTS, &new_id);
-	if (ret == -EAGAIN) {
-		mutex_unlock(&dev->struct_mutex);
-		goto again;
-	}
 	mutex_unlock(&dev->struct_mutex);
+	if (ret == -EAGAIN)
+		goto again;
+	else if (ret)
+		return ret;
+
 	return new_id;
 }
 
diff --git a/drivers/gpu/drm/drm_crtc.c b/drivers/gpu/drm/drm_crtc.c
index c79870a75c2..92cea9d77ec 100644
--- a/drivers/gpu/drm/drm_crtc.c
+++ b/drivers/gpu/drm/drm_crtc.c
@@ -227,7 +227,7 @@ static int drm_mode_object_get(struct drm_device *dev,
 again:
 	if (idr_pre_get(&dev->mode_config.crtc_idr, GFP_KERNEL) == 0) {
 		DRM_ERROR("Ran out memory getting a mode number\n");
-		return -EINVAL;
+		return -ENOMEM;
 	}
 
 	mutex_lock(&dev->mode_config.idr_mutex);
@@ -235,6 +235,8 @@ again:
 	mutex_unlock(&dev->mode_config.idr_mutex);
 	if (ret == -EAGAIN)
 		goto again;
+	else if (ret)
+		return ret;
 
 	obj->id = new_id;
 	obj->type = obj_type;
@@ -361,7 +363,7 @@ EXPORT_SYMBOL(drm_framebuffer_cleanup);
  * @funcs: callbacks for the new CRTC
  *
  * LOCKING:
- * Caller must hold mode config lock.
+ * Takes mode_config lock.
  *
  * Inits a new object created as base part of an driver crtc object.
  *
@@ -382,6 +384,8 @@ int drm_crtc_init(struct drm_device *dev, struct drm_crtc *crtc,
 	if (ret)
 		goto out;
 
+	crtc->base.properties = &crtc->properties;
+
 	list_add_tail(&crtc->head, &dev->mode_config.crtc_list);
 	dev->mode_config.num_crtc++;
 
@@ -481,6 +485,7 @@ int drm_connector_init(struct drm_device *dev,
 	if (ret)
 		goto out;
 
+	connector->base.properties = &connector->properties;
 	connector->dev = dev;
 	connector->funcs = funcs;
 	connector->connector_type = connector_type;
@@ -603,6 +608,7 @@ int drm_plane_init(struct drm_device *dev, struct drm_plane *plane,
 	if (ret)
 		goto out;
 
+	plane->base.properties = &plane->properties;
 	plane->dev = dev;
 	plane->funcs = funcs;
 	plane->format_types = kmalloc(sizeof(uint32_t) * format_count,
@@ -1422,11 +1428,7 @@ int drm_mode_getconnector(struct drm_device *dev, void *data,
 	}
 	connector = obj_to_connector(obj);
 
-	for (i = 0; i < DRM_CONNECTOR_MAX_PROPERTY; i++) {
-		if (connector->property_ids[i] != 0) {
-			props_count++;
-		}
-	}
+	props_count = connector->properties.count;
 
 	for (i = 0; i < DRM_CONNECTOR_MAX_ENCODER; i++) {
 		if (connector->encoder_ids[i] != 0) {
@@ -1479,21 +1481,19 @@ int drm_mode_getconnector(struct drm_device *dev, void *data,
 		copied = 0;
 		prop_ptr = (uint32_t __user *)(unsigned long)(out_resp->props_ptr);
 		prop_values = (uint64_t __user *)(unsigned long)(out_resp->prop_values_ptr);
-		for (i = 0; i < DRM_CONNECTOR_MAX_PROPERTY; i++) {
-			if (connector->property_ids[i] != 0) {
-				if (put_user(connector->property_ids[i],
-					     prop_ptr + copied)) {
-					ret = -EFAULT;
-					goto out;
-				}
+		for (i = 0; i < connector->properties.count; i++) {
+			if (put_user(connector->properties.ids[i],
+				     prop_ptr + copied)) {
+				ret = -EFAULT;
+				goto out;
+			}
 
-				if (put_user(connector->property_values[i],
-					     prop_values + copied)) {
-					ret = -EFAULT;
-					goto out;
-				}
-				copied++;
+			if (put_user(connector->properties.values[i],
+				     prop_values + copied)) {
+				ret = -EFAULT;
+				goto out;
 			}
+			copied++;
 		}
 	}
 	out_resp->count_props = props_count;
@@ -1830,7 +1830,7 @@ int drm_mode_setcrtc(struct drm_device *dev, void *data,
 	struct drm_display_mode *mode = NULL;
 	struct drm_mode_set set;
 	uint32_t __user *set_connectors_ptr;
-	int ret = 0;
+	int ret;
 	int i;
 
 	if (!drm_core_check_feature(dev, DRIVER_MODESET))
@@ -2102,7 +2102,7 @@ int drm_mode_addfb(struct drm_device *dev,
 
 	fb = dev->mode_config.funcs->fb_create(dev, file_priv, &r);
 	if (IS_ERR(fb)) {
-		DRM_ERROR("could not create framebuffer\n");
+		DRM_DEBUG_KMS("could not create framebuffer\n");
 		ret = PTR_ERR(fb);
 		goto out;
 	}
@@ -2185,6 +2185,47 @@ static int format_check(struct drm_mode_fb_cmd2 *r)
 	}
 }
 
+static int framebuffer_check(struct drm_mode_fb_cmd2 *r)
+{
+	int ret, hsub, vsub, num_planes, i;
+
+	ret = format_check(r);
+	if (ret) {
+		DRM_DEBUG_KMS("bad framebuffer format 0x%08x\n", r->pixel_format);
+		return ret;
+	}
+
+	hsub = drm_format_horz_chroma_subsampling(r->pixel_format);
+	vsub = drm_format_vert_chroma_subsampling(r->pixel_format);
+	num_planes = drm_format_num_planes(r->pixel_format);
+
+	if (r->width == 0 || r->width % hsub) {
+		DRM_DEBUG_KMS("bad framebuffer width %u\n", r->height);
+		return -EINVAL;
+	}
+
+	if (r->height == 0 || r->height % vsub) {
+		DRM_DEBUG_KMS("bad framebuffer height %u\n", r->height);
+		return -EINVAL;
+	}
+
+	for (i = 0; i < num_planes; i++) {
+		unsigned int width = r->width / (i != 0 ? hsub : 1);
+
+		if (!r->handles[i]) {
+			DRM_DEBUG_KMS("no buffer object handle for plane %d\n", i);
+			return -EINVAL;
+		}
+
+		if (r->pitches[i] < drm_format_plane_cpp(r->pixel_format, i) * width) {
+			DRM_DEBUG_KMS("bad pitch %u for plane %d\n", r->pitches[i], i);
+			return -EINVAL;
+		}
+	}
+
+	return 0;
+}
+
 /**
  * drm_mode_addfb2 - add an FB to the graphics configuration
  * @inode: inode from the ioctl
@@ -2208,33 +2249,31 @@ int drm_mode_addfb2(struct drm_device *dev,
 	struct drm_mode_fb_cmd2 *r = data;
 	struct drm_mode_config *config = &dev->mode_config;
 	struct drm_framebuffer *fb;
-	int ret = 0;
+	int ret;
 
 	if (!drm_core_check_feature(dev, DRIVER_MODESET))
 		return -EINVAL;
 
 	if ((config->min_width > r->width) || (r->width > config->max_width)) {
-		DRM_ERROR("bad framebuffer width %d, should be >= %d && <= %d\n",
+		DRM_DEBUG_KMS("bad framebuffer width %d, should be >= %d && <= %d\n",
 			  r->width, config->min_width, config->max_width);
 		return -EINVAL;
 	}
 	if ((config->min_height > r->height) || (r->height > config->max_height)) {
-		DRM_ERROR("bad framebuffer height %d, should be >= %d && <= %d\n",
+		DRM_DEBUG_KMS("bad framebuffer height %d, should be >= %d && <= %d\n",
 			  r->height, config->min_height, config->max_height);
 		return -EINVAL;
 	}
 
-	ret = format_check(r);
-	if (ret) {
-		DRM_ERROR("bad framebuffer format 0x%08x\n", r->pixel_format);
+	ret = framebuffer_check(r);
+	if (ret)
 		return ret;
-	}
 
 	mutex_lock(&dev->mode_config.mutex);
 
 	fb = dev->mode_config.funcs->fb_create(dev, file_priv, r);
 	if (IS_ERR(fb)) {
-		DRM_ERROR("could not create framebuffer\n");
+		DRM_DEBUG_KMS("could not create framebuffer\n");
 		ret = PTR_ERR(fb);
 		goto out;
 	}
@@ -2365,7 +2404,7 @@ int drm_mode_dirtyfb_ioctl(struct drm_device *dev,
 	struct drm_framebuffer *fb;
 	unsigned flags;
 	int num_clips;
-	int ret = 0;
+	int ret;
 
 	if (!drm_core_check_feature(dev, DRIVER_MODESET))
 		return -EINVAL;
@@ -2564,7 +2603,7 @@ int drm_mode_attachmode_ioctl(struct drm_device *dev,
 	struct drm_display_mode *mode;
 	struct drm_mode_object *obj;
 	struct drm_mode_modeinfo *umode = &mode_cmd->mode;
-	int ret = 0;
+	int ret;
 
 	if (!drm_core_check_feature(dev, DRIVER_MODESET))
 		return -EINVAL;
@@ -2618,7 +2657,7 @@ int drm_mode_detachmode_ioctl(struct drm_device *dev,
 	struct drm_connector *connector;
 	struct drm_display_mode mode;
 	struct drm_mode_modeinfo *umode = &mode_cmd->mode;
-	int ret = 0;
+	int ret;
 
 	if (!drm_core_check_feature(dev, DRIVER_MODESET))
 		return -EINVAL;
@@ -2710,6 +2749,34 @@ struct drm_property *drm_property_create_enum(struct drm_device *dev, int flags,
 }
 EXPORT_SYMBOL(drm_property_create_enum);
 
+struct drm_property *drm_property_create_bitmask(struct drm_device *dev,
+					 int flags, const char *name,
+					 const struct drm_prop_enum_list *props,
+					 int num_values)
+{
+	struct drm_property *property;
+	int i, ret;
+
+	flags |= DRM_MODE_PROP_BITMASK;
+
+	property = drm_property_create(dev, flags, name, num_values);
+	if (!property)
+		return NULL;
+
+	for (i = 0; i < num_values; i++) {
+		ret = drm_property_add_enum(property, i,
+				      props[i].type,
+				      props[i].name);
+		if (ret) {
+			drm_property_destroy(dev, property);
+			return NULL;
+		}
+	}
+
+	return property;
+}
+EXPORT_SYMBOL(drm_property_create_bitmask);
+
 struct drm_property *drm_property_create_range(struct drm_device *dev, int flags,
 					 const char *name,
 					 uint64_t min, uint64_t max)
@@ -2734,7 +2801,14 @@ int drm_property_add_enum(struct drm_property *property, int index,
 {
 	struct drm_property_enum *prop_enum;
 
-	if (!(property->flags & DRM_MODE_PROP_ENUM))
+	if (!(property->flags & (DRM_MODE_PROP_ENUM | DRM_MODE_PROP_BITMASK)))
+		return -EINVAL;
+
+	/*
+	 * Bitmask enum properties have the additional constraint of values
+	 * from 0 to 63
+	 */
+	if ((property->flags & DRM_MODE_PROP_BITMASK) && (value > 63))
 		return -EINVAL;
 
 	if (!list_empty(&property->enum_blob_list)) {
@@ -2778,60 +2852,78 @@ void drm_property_destroy(struct drm_device *dev, struct drm_property *property)
 }
 EXPORT_SYMBOL(drm_property_destroy);
 
-int drm_connector_attach_property(struct drm_connector *connector,
+void drm_connector_attach_property(struct drm_connector *connector,
 			       struct drm_property *property, uint64_t init_val)
 {
-	int i;
-
-	for (i = 0; i < DRM_CONNECTOR_MAX_PROPERTY; i++) {
-		if (connector->property_ids[i] == 0) {
-			connector->property_ids[i] = property->base.id;
-			connector->property_values[i] = init_val;
-			break;
-		}
-	}
-
-	if (i == DRM_CONNECTOR_MAX_PROPERTY)
-		return -EINVAL;
-	return 0;
+	drm_object_attach_property(&connector->base, property, init_val);
 }
 EXPORT_SYMBOL(drm_connector_attach_property);
 
 int drm_connector_property_set_value(struct drm_connector *connector,
 				  struct drm_property *property, uint64_t value)
 {
+	return drm_object_property_set_value(&connector->base, property, value);
+}
+EXPORT_SYMBOL(drm_connector_property_set_value);
+
+int drm_connector_property_get_value(struct drm_connector *connector,
+				  struct drm_property *property, uint64_t *val)
+{
+	return drm_object_property_get_value(&connector->base, property, val);
+}
+EXPORT_SYMBOL(drm_connector_property_get_value);
+
+void drm_object_attach_property(struct drm_mode_object *obj,
+				struct drm_property *property,
+				uint64_t init_val)
+{
+	int count = obj->properties->count;
+
+	if (count == DRM_OBJECT_MAX_PROPERTY) {
+		WARN(1, "Failed to attach object property (type: 0x%x). Please "
+			"increase DRM_OBJECT_MAX_PROPERTY by 1 for each time "
+			"you see this message on the same object type.\n",
+			obj->type);
+		return;
+	}
+
+	obj->properties->ids[count] = property->base.id;
+	obj->properties->values[count] = init_val;
+	obj->properties->count++;
+}
+EXPORT_SYMBOL(drm_object_attach_property);
+
+int drm_object_property_set_value(struct drm_mode_object *obj,
+				  struct drm_property *property, uint64_t val)
+{
 	int i;
 
-	for (i = 0; i < DRM_CONNECTOR_MAX_PROPERTY; i++) {
-		if (connector->property_ids[i] == property->base.id) {
-			connector->property_values[i] = value;
-			break;
+	for (i = 0; i < obj->properties->count; i++) {
+		if (obj->properties->ids[i] == property->base.id) {
+			obj->properties->values[i] = val;
+			return 0;
 		}
 	}
 
-	if (i == DRM_CONNECTOR_MAX_PROPERTY)
-		return -EINVAL;
-	return 0;
+	return -EINVAL;
 }
-EXPORT_SYMBOL(drm_connector_property_set_value);
+EXPORT_SYMBOL(drm_object_property_set_value);
 
-int drm_connector_property_get_value(struct drm_connector *connector,
+int drm_object_property_get_value(struct drm_mode_object *obj,
 				  struct drm_property *property, uint64_t *val)
 {
 	int i;
 
-	for (i = 0; i < DRM_CONNECTOR_MAX_PROPERTY; i++) {
-		if (connector->property_ids[i] == property->base.id) {
-			*val = connector->property_values[i];
-			break;
+	for (i = 0; i < obj->properties->count; i++) {
+		if (obj->properties->ids[i] == property->base.id) {
+			*val = obj->properties->values[i];
+			return 0;
 		}
 	}
 
-	if (i == DRM_CONNECTOR_MAX_PROPERTY)
-		return -EINVAL;
-	return 0;
+	return -EINVAL;
 }
-EXPORT_SYMBOL(drm_connector_property_get_value);
+EXPORT_SYMBOL(drm_object_property_get_value);
 
 int drm_mode_getproperty_ioctl(struct drm_device *dev,
 			       void *data, struct drm_file *file_priv)
@@ -2862,7 +2954,7 @@ int drm_mode_getproperty_ioctl(struct drm_device *dev,
 	}
 	property = obj_to_property(obj);
 
-	if (property->flags & DRM_MODE_PROP_ENUM) {
+	if (property->flags & (DRM_MODE_PROP_ENUM | DRM_MODE_PROP_BITMASK)) {
 		list_for_each_entry(prop_enum, &property->enum_blob_list, head)
 			enum_count++;
 	} else if (property->flags & DRM_MODE_PROP_BLOB) {
@@ -2887,7 +2979,7 @@ int drm_mode_getproperty_ioctl(struct drm_device *dev,
 	}
 	out_resp->count_values = value_count;
 
-	if (property->flags & DRM_MODE_PROP_ENUM) {
+	if (property->flags & (DRM_MODE_PROP_ENUM | DRM_MODE_PROP_BITMASK)) {
 		if ((out_resp->count_enum_blobs >= enum_count) && enum_count) {
 			copied = 0;
 			enum_ptr = (struct drm_mode_property_enum __user *)(unsigned long)out_resp->enum_blob_ptr;
@@ -3009,7 +3101,7 @@ int drm_mode_connector_update_edid_property(struct drm_connector *connector,
 					    struct edid *edid)
 {
 	struct drm_device *dev = connector->dev;
-	int ret = 0, size;
+	int ret, size;
 
 	if (connector->edid_blob_ptr)
 		drm_property_destroy_blob(dev, connector->edid_blob_ptr);
@@ -3033,75 +3125,202 @@ int drm_mode_connector_update_edid_property(struct drm_connector *connector,
 }
 EXPORT_SYMBOL(drm_mode_connector_update_edid_property);
 
+static bool drm_property_change_is_valid(struct drm_property *property,
+					 __u64 value)
+{
+	if (property->flags & DRM_MODE_PROP_IMMUTABLE)
+		return false;
+	if (property->flags & DRM_MODE_PROP_RANGE) {
+		if (value < property->values[0] || value > property->values[1])
+			return false;
+		return true;
+	} else if (property->flags & DRM_MODE_PROP_BITMASK) {
+		int i;
+		__u64 valid_mask = 0;
+		for (i = 0; i < property->num_values; i++)
+			valid_mask |= (1ULL << property->values[i]);
+		return !(value & ~valid_mask);
+	} else {
+		int i;
+		for (i = 0; i < property->num_values; i++)
+			if (property->values[i] == value)
+				return true;
+		return false;
+	}
+}
+
 int drm_mode_connector_property_set_ioctl(struct drm_device *dev,
 				       void *data, struct drm_file *file_priv)
 {
-	struct drm_mode_connector_set_property *out_resp = data;
-	struct drm_mode_object *obj;
-	struct drm_property *property;
-	struct drm_connector *connector;
+	struct drm_mode_connector_set_property *conn_set_prop = data;
+	struct drm_mode_obj_set_property obj_set_prop = {
+		.value = conn_set_prop->value,
+		.prop_id = conn_set_prop->prop_id,
+		.obj_id = conn_set_prop->connector_id,
+		.obj_type = DRM_MODE_OBJECT_CONNECTOR
+	};
+
+	/* It does all the locking and checking we need */
+	return drm_mode_obj_set_property_ioctl(dev, &obj_set_prop, file_priv);
+}
+
+static int drm_mode_connector_set_obj_prop(struct drm_mode_object *obj,
+					   struct drm_property *property,
+					   uint64_t value)
+{
 	int ret = -EINVAL;
+	struct drm_connector *connector = obj_to_connector(obj);
+
+	/* Do DPMS ourselves */
+	if (property == connector->dev->mode_config.dpms_property) {
+		if (connector->funcs->dpms)
+			(*connector->funcs->dpms)(connector, (int)value);
+		ret = 0;
+	} else if (connector->funcs->set_property)
+		ret = connector->funcs->set_property(connector, property, value);
+
+	/* store the property value if successful */
+	if (!ret)
+		drm_connector_property_set_value(connector, property, value);
+	return ret;
+}
+
+static int drm_mode_crtc_set_obj_prop(struct drm_mode_object *obj,
+				      struct drm_property *property,
+				      uint64_t value)
+{
+	int ret = -EINVAL;
+	struct drm_crtc *crtc = obj_to_crtc(obj);
+
+	if (crtc->funcs->set_property)
+		ret = crtc->funcs->set_property(crtc, property, value);
+	if (!ret)
+		drm_object_property_set_value(obj, property, value);
+
+	return ret;
+}
+
+static int drm_mode_plane_set_obj_prop(struct drm_mode_object *obj,
+				      struct drm_property *property,
+				      uint64_t value)
+{
+	int ret = -EINVAL;
+	struct drm_plane *plane = obj_to_plane(obj);
+
+	if (plane->funcs->set_property)
+		ret = plane->funcs->set_property(plane, property, value);
+	if (!ret)
+		drm_object_property_set_value(obj, property, value);
+
+	return ret;
+}
+
+int drm_mode_obj_get_properties_ioctl(struct drm_device *dev, void *data,
+				      struct drm_file *file_priv)
+{
+	struct drm_mode_obj_get_properties *arg = data;
+	struct drm_mode_object *obj;
+	int ret = 0;
 	int i;
+	int copied = 0;
+	int props_count = 0;
+	uint32_t __user *props_ptr;
+	uint64_t __user *prop_values_ptr;
 
 	if (!drm_core_check_feature(dev, DRIVER_MODESET))
 		return -EINVAL;
 
 	mutex_lock(&dev->mode_config.mutex);
 
-	obj = drm_mode_object_find(dev, out_resp->connector_id, DRM_MODE_OBJECT_CONNECTOR);
+	obj = drm_mode_object_find(dev, arg->obj_id, arg->obj_type);
 	if (!obj) {
+		ret = -EINVAL;
+		goto out;
+	}
+	if (!obj->properties) {
+		ret = -EINVAL;
 		goto out;
 	}
-	connector = obj_to_connector(obj);
 
-	for (i = 0; i < DRM_CONNECTOR_MAX_PROPERTY; i++) {
-		if (connector->property_ids[i] == out_resp->prop_id)
-			break;
+	props_count = obj->properties->count;
+
+	/* This ioctl is called twice, once to determine how much space is
+	 * needed, and the 2nd time to fill it. */
+	if ((arg->count_props >= props_count) && props_count) {
+		copied = 0;
+		props_ptr = (uint32_t __user *)(unsigned long)(arg->props_ptr);
+		prop_values_ptr = (uint64_t __user *)(unsigned long)
+				  (arg->prop_values_ptr);
+		for (i = 0; i < props_count; i++) {
+			if (put_user(obj->properties->ids[i],
+				     props_ptr + copied)) {
+				ret = -EFAULT;
+				goto out;
+			}
+			if (put_user(obj->properties->values[i],
+				     prop_values_ptr + copied)) {
+				ret = -EFAULT;
+				goto out;
+			}
+			copied++;
+		}
 	}
+	arg->count_props = props_count;
+out:
+	mutex_unlock(&dev->mode_config.mutex);
+	return ret;
+}
+
+int drm_mode_obj_set_property_ioctl(struct drm_device *dev, void *data,
+				    struct drm_file *file_priv)
+{
+	struct drm_mode_obj_set_property *arg = data;
+	struct drm_mode_object *arg_obj;
+	struct drm_mode_object *prop_obj;
+	struct drm_property *property;
+	int ret = -EINVAL;
+	int i;
+
+	if (!drm_core_check_feature(dev, DRIVER_MODESET))
+		return -EINVAL;
+
+	mutex_lock(&dev->mode_config.mutex);
 
-	if (i == DRM_CONNECTOR_MAX_PROPERTY) {
+	arg_obj = drm_mode_object_find(dev, arg->obj_id, arg->obj_type);
+	if (!arg_obj)
+		goto out;
+	if (!arg_obj->properties)
 		goto out;
-	}
 
-	obj = drm_mode_object_find(dev, out_resp->prop_id, DRM_MODE_OBJECT_PROPERTY);
-	if (!obj) {
+	for (i = 0; i < arg_obj->properties->count; i++)
+		if (arg_obj->properties->ids[i] == arg->prop_id)
+			break;
+
+	if (i == arg_obj->properties->count)
 		goto out;
-	}
-	property = obj_to_property(obj);
 
-	if (property->flags & DRM_MODE_PROP_IMMUTABLE)
+	prop_obj = drm_mode_object_find(dev, arg->prop_id,
+					DRM_MODE_OBJECT_PROPERTY);
+	if (!prop_obj)
 		goto out;
+	property = obj_to_property(prop_obj);
 
-	if (property->flags & DRM_MODE_PROP_RANGE) {
-		if (out_resp->value < property->values[0])
-			goto out;
+	if (!drm_property_change_is_valid(property, arg->value))
+		goto out;
 
-		if (out_resp->value > property->values[1])
-			goto out;
-	} else {
-		int found = 0;
-		for (i = 0; i < property->num_values; i++) {
-			if (property->values[i] == out_resp->value) {
-				found = 1;
-				break;
-			}
-		}
-		if (!found) {
-			goto out;
-		}
+	switch (arg_obj->type) {
+	case DRM_MODE_OBJECT_CONNECTOR:
+		ret = drm_mode_connector_set_obj_prop(arg_obj, property,
+						      arg->value);
+		break;
+	case DRM_MODE_OBJECT_CRTC:
+		ret = drm_mode_crtc_set_obj_prop(arg_obj, property, arg->value);
+		break;
+	case DRM_MODE_OBJECT_PLANE:
+		ret = drm_mode_plane_set_obj_prop(arg_obj, property, arg->value);
+		break;
 	}
 
-	/* Do DPMS ourselves */
-	if (property == connector->dev->mode_config.dpms_property) {
-		if (connector->funcs->dpms)
-			(*connector->funcs->dpms)(connector, (int) out_resp->value);
-		ret = 0;
-	} else if (connector->funcs->set_property)
-		ret = connector->funcs->set_property(connector, property, out_resp->value);
-
-	/* store the property value if successful */
-	if (!ret)
-		drm_connector_property_set_value(connector, property, out_resp->value);
 out:
 	mutex_unlock(&dev->mode_config.mutex);
 	return ret;
@@ -3173,6 +3392,11 @@ int drm_mode_gamma_set_ioctl(struct drm_device *dev,
 	}
 	crtc = obj_to_crtc(obj);
 
+	if (crtc->funcs->gamma_set == NULL) {
+		ret = -ENOSYS;
+		goto out;
+	}
+
 	/* memcpy into gamma store */
 	if (crtc_lut->gamma_size != crtc->gamma_size) {
 		ret = -EINVAL;
@@ -3468,3 +3692,140 @@ void drm_fb_get_bpp_depth(uint32_t format, unsigned int *depth,
 	}
 }
 EXPORT_SYMBOL(drm_fb_get_bpp_depth);
+
+/**
+ * drm_format_num_planes - get the number of planes for format
+ * @format: pixel format (DRM_FORMAT_*)
+ *
+ * RETURNS:
+ * The number of planes used by the specified pixel format.
+ */
+int drm_format_num_planes(uint32_t format)
+{
+	switch (format) {
+	case DRM_FORMAT_YUV410:
+	case DRM_FORMAT_YVU410:
+	case DRM_FORMAT_YUV411:
+	case DRM_FORMAT_YVU411:
+	case DRM_FORMAT_YUV420:
+	case DRM_FORMAT_YVU420:
+	case DRM_FORMAT_YUV422:
+	case DRM_FORMAT_YVU422:
+	case DRM_FORMAT_YUV444:
+	case DRM_FORMAT_YVU444:
+		return 3;
+	case DRM_FORMAT_NV12:
+	case DRM_FORMAT_NV21:
+	case DRM_FORMAT_NV16:
+	case DRM_FORMAT_NV61:
+		return 2;
+	default:
+		return 1;
+	}
+}
+EXPORT_SYMBOL(drm_format_num_planes);
+
+/**
+ * drm_format_plane_cpp - determine the bytes per pixel value
+ * @format: pixel format (DRM_FORMAT_*)
+ * @plane: plane index
+ *
+ * RETURNS:
+ * The bytes per pixel value for the specified plane.
+ */
+int drm_format_plane_cpp(uint32_t format, int plane)
+{
+	unsigned int depth;
+	int bpp;
+
+	if (plane >= drm_format_num_planes(format))
+		return 0;
+
+	switch (format) {
+	case DRM_FORMAT_YUYV:
+	case DRM_FORMAT_YVYU:
+	case DRM_FORMAT_UYVY:
+	case DRM_FORMAT_VYUY:
+		return 2;
+	case DRM_FORMAT_NV12:
+	case DRM_FORMAT_NV21:
+	case DRM_FORMAT_NV16:
+	case DRM_FORMAT_NV61:
+		return plane ? 2 : 1;
+	case DRM_FORMAT_YUV410:
+	case DRM_FORMAT_YVU410:
+	case DRM_FORMAT_YUV411:
+	case DRM_FORMAT_YVU411:
+	case DRM_FORMAT_YUV420:
+	case DRM_FORMAT_YVU420:
+	case DRM_FORMAT_YUV422:
+	case DRM_FORMAT_YVU422:
+	case DRM_FORMAT_YUV444:
+	case DRM_FORMAT_YVU444:
+		return 1;
+	default:
+		drm_fb_get_bpp_depth(format, &depth, &bpp);
+		return bpp >> 3;
+	}
+}
+EXPORT_SYMBOL(drm_format_plane_cpp);
+
+/**
+ * drm_format_horz_chroma_subsampling - get the horizontal chroma subsampling factor
+ * @format: pixel format (DRM_FORMAT_*)
+ *
+ * RETURNS:
+ * The horizontal chroma subsampling factor for the
+ * specified pixel format.
+ */
+int drm_format_horz_chroma_subsampling(uint32_t format)
+{
+	switch (format) {
+	case DRM_FORMAT_YUV411:
+	case DRM_FORMAT_YVU411:
+	case DRM_FORMAT_YUV410:
+	case DRM_FORMAT_YVU410:
+		return 4;
+	case DRM_FORMAT_YUYV:
+	case DRM_FORMAT_YVYU:
+	case DRM_FORMAT_UYVY:
+	case DRM_FORMAT_VYUY:
+	case DRM_FORMAT_NV12:
+	case DRM_FORMAT_NV21:
+	case DRM_FORMAT_NV16:
+	case DRM_FORMAT_NV61:
+	case DRM_FORMAT_YUV422:
+	case DRM_FORMAT_YVU422:
+	case DRM_FORMAT_YUV420:
+	case DRM_FORMAT_YVU420:
+		return 2;
+	default:
+		return 1;
+	}
+}
+EXPORT_SYMBOL(drm_format_horz_chroma_subsampling);
+
+/**
+ * drm_format_vert_chroma_subsampling - get the vertical chroma subsampling factor
+ * @format: pixel format (DRM_FORMAT_*)
+ *
+ * RETURNS:
+ * The vertical chroma subsampling factor for the
+ * specified pixel format.
+ */
+int drm_format_vert_chroma_subsampling(uint32_t format)
+{
+	switch (format) {
+	case DRM_FORMAT_YUV410:
+	case DRM_FORMAT_YVU410:
+		return 4;
+	case DRM_FORMAT_YUV420:
+	case DRM_FORMAT_YVU420:
+	case DRM_FORMAT_NV12:
+	case DRM_FORMAT_NV21:
+		return 2;
+	default:
+		return 1;
+	}
+}
+EXPORT_SYMBOL(drm_format_vert_chroma_subsampling);
diff --git a/drivers/gpu/drm/drm_crtc_helper.c b/drivers/gpu/drm/drm_crtc_helper.c
index 81118893264..3252e7067d8 100644
--- a/drivers/gpu/drm/drm_crtc_helper.c
+++ b/drivers/gpu/drm/drm_crtc_helper.c
@@ -518,7 +518,7 @@ int drm_crtc_helper_set_config(struct drm_mode_set *set)
 	int count = 0, ro, fail = 0;
 	struct drm_crtc_helper_funcs *crtc_funcs;
 	struct drm_mode_set save_set;
-	int ret = 0;
+	int ret;
 	int i;
 
 	DRM_DEBUG_KMS("\n");
@@ -1023,36 +1023,3 @@ void drm_helper_hpd_irq_event(struct drm_device *dev)
 		queue_delayed_work(system_nrt_wq, &dev->mode_config.output_poll_work, 0);
 }
 EXPORT_SYMBOL(drm_helper_hpd_irq_event);
-
-
-/**
- * drm_format_num_planes - get the number of planes for format
- * @format: pixel format (DRM_FORMAT_*)
- *
- * RETURNS:
- * The number of planes used by the specified pixel format.
- */
-int drm_format_num_planes(uint32_t format)
-{
-	switch (format) {
-	case DRM_FORMAT_YUV410:
-	case DRM_FORMAT_YVU410:
-	case DRM_FORMAT_YUV411:
-	case DRM_FORMAT_YVU411:
-	case DRM_FORMAT_YUV420:
-	case DRM_FORMAT_YVU420:
-	case DRM_FORMAT_YUV422:
-	case DRM_FORMAT_YVU422:
-	case DRM_FORMAT_YUV444:
-	case DRM_FORMAT_YVU444:
-		return 3;
-	case DRM_FORMAT_NV12:
-	case DRM_FORMAT_NV21:
-	case DRM_FORMAT_NV16:
-	case DRM_FORMAT_NV61:
-		return 2;
-	default:
-		return 1;
-	}
-}
-EXPORT_SYMBOL(drm_format_num_planes);
diff --git a/drivers/gpu/drm/drm_drv.c b/drivers/gpu/drm/drm_drv.c
index 6116e3b7539..8a9d0792e4e 100644
--- a/drivers/gpu/drm/drm_drv.c
+++ b/drivers/gpu/drm/drm_drv.c
@@ -163,7 +163,9 @@ static struct drm_ioctl_desc drm_ioctls[] = {
 	DRM_IOCTL_DEF(DRM_IOCTL_MODE_DIRTYFB, drm_mode_dirtyfb_ioctl, DRM_MASTER|DRM_CONTROL_ALLOW|DRM_UNLOCKED),
 	DRM_IOCTL_DEF(DRM_IOCTL_MODE_CREATE_DUMB, drm_mode_create_dumb_ioctl, DRM_MASTER|DRM_CONTROL_ALLOW|DRM_UNLOCKED),
 	DRM_IOCTL_DEF(DRM_IOCTL_MODE_MAP_DUMB, drm_mode_mmap_dumb_ioctl, DRM_MASTER|DRM_CONTROL_ALLOW|DRM_UNLOCKED),
-	DRM_IOCTL_DEF(DRM_IOCTL_MODE_DESTROY_DUMB, drm_mode_destroy_dumb_ioctl, DRM_MASTER|DRM_CONTROL_ALLOW|DRM_UNLOCKED)
+	DRM_IOCTL_DEF(DRM_IOCTL_MODE_DESTROY_DUMB, drm_mode_destroy_dumb_ioctl, DRM_MASTER|DRM_CONTROL_ALLOW|DRM_UNLOCKED),
+	DRM_IOCTL_DEF(DRM_IOCTL_MODE_OBJ_GETPROPERTIES, drm_mode_obj_get_properties_ioctl, DRM_MASTER|DRM_CONTROL_ALLOW|DRM_UNLOCKED),
+	DRM_IOCTL_DEF(DRM_IOCTL_MODE_OBJ_SETPROPERTY, drm_mode_obj_set_property_ioctl, DRM_MASTER|DRM_CONTROL_ALLOW|DRM_UNLOCKED),
 };
 
 #define DRM_CORE_IOCTL_COUNT	ARRAY_SIZE( drm_ioctls )
diff --git a/drivers/gpu/drm/drm_edid.c b/drivers/gpu/drm/drm_edid.c
index 5a18b0df828..608bddfc7e3 100644
--- a/drivers/gpu/drm/drm_edid.c
+++ b/drivers/gpu/drm/drm_edid.c
@@ -81,7 +81,7 @@ struct detailed_mode_closure {
 #define LEVEL_CVT	3
 
 static struct edid_quirk {
-	char *vendor;
+	char vendor[4];
 	int product_id;
 	u32 quirks;
 } edid_quirk_list[] = {
@@ -149,13 +149,13 @@ EXPORT_SYMBOL(drm_edid_header_is_valid);
  * Sanity check the EDID block (base or extension).  Return 0 if the block
  * doesn't check out, or 1 if it's valid.
  */
-bool drm_edid_block_valid(u8 *raw_edid)
+bool drm_edid_block_valid(u8 *raw_edid, int block)
 {
 	int i;
 	u8 csum = 0;
 	struct edid *edid = (struct edid *)raw_edid;
 
-	if (raw_edid[0] == 0x00) {
+	if (block == 0) {
 		int score = drm_edid_header_is_valid(raw_edid);
 		if (score == 8) ;
 		else if (score >= 6) {
@@ -219,7 +219,7 @@ bool drm_edid_is_valid(struct edid *edid)
 		return false;
 
 	for (i = 0; i <= edid->extensions; i++)
-		if (!drm_edid_block_valid(raw + i * EDID_LENGTH))
+		if (!drm_edid_block_valid(raw + i * EDID_LENGTH, i))
 			return false;
 
 	return true;
@@ -299,7 +299,7 @@ drm_do_get_edid(struct drm_connector *connector, struct i2c_adapter *adapter)
 	for (i = 0; i < 4; i++) {
 		if (drm_do_probe_ddc_edid(adapter, block, 0, EDID_LENGTH))
 			goto out;
-		if (drm_edid_block_valid(block))
+		if (drm_edid_block_valid(block, 0))
 			break;
 		if (i == 0 && drm_edid_is_zero(block, EDID_LENGTH)) {
 			connector->null_edid_counter++;
@@ -324,7 +324,7 @@ drm_do_get_edid(struct drm_connector *connector, struct i2c_adapter *adapter)
 				  block + (valid_extensions + 1) * EDID_LENGTH,
 				  j, EDID_LENGTH))
 				goto out;
-			if (drm_edid_block_valid(block + (valid_extensions + 1) * EDID_LENGTH)) {
+			if (drm_edid_block_valid(block + (valid_extensions + 1) * EDID_LENGTH, j)) {
 				valid_extensions++;
 				break;
 			}
@@ -486,23 +486,47 @@ static void edid_fixup_preferred(struct drm_connector *connector,
 	preferred_mode->type |= DRM_MODE_TYPE_PREFERRED;
 }
 
+static bool
+mode_is_rb(const struct drm_display_mode *mode)
+{
+	return (mode->htotal - mode->hdisplay == 160) &&
+	       (mode->hsync_end - mode->hdisplay == 80) &&
+	       (mode->hsync_end - mode->hsync_start == 32) &&
+	       (mode->vsync_start - mode->vdisplay == 3);
+}
+
+/*
+ * drm_mode_find_dmt - Create a copy of a mode if present in DMT
+ * @dev: Device to duplicate against
+ * @hsize: Mode width
+ * @vsize: Mode height
+ * @fresh: Mode refresh rate
+ * @rb: Mode reduced-blanking-ness
+ *
+ * Walk the DMT mode list looking for a match for the given parameters.
+ * Return a newly allocated copy of the mode, or NULL if not found.
+ */
 struct drm_display_mode *drm_mode_find_dmt(struct drm_device *dev,
-					   int hsize, int vsize, int fresh)
+					   int hsize, int vsize, int fresh,
+					   bool rb)
 {
-	struct drm_display_mode *mode = NULL;
 	int i;
 
 	for (i = 0; i < drm_num_dmt_modes; i++) {
 		const struct drm_display_mode *ptr = &drm_dmt_modes[i];
-		if (hsize == ptr->hdisplay &&
-			vsize == ptr->vdisplay &&
-			fresh == drm_mode_vrefresh(ptr)) {
-			/* get the expected default mode */
-			mode = drm_mode_duplicate(dev, ptr);
-			break;
-		}
+		if (hsize != ptr->hdisplay)
+			continue;
+		if (vsize != ptr->vdisplay)
+			continue;
+		if (fresh != drm_mode_vrefresh(ptr))
+			continue;
+		if (rb != mode_is_rb(ptr))
+			continue;
+
+		return drm_mode_duplicate(dev, ptr);
 	}
-	return mode;
+
+	return NULL;
 }
 EXPORT_SYMBOL(drm_mode_find_dmt);
 
@@ -731,10 +755,17 @@ drm_mode_std(struct drm_connector *connector, struct edid *edid,
 	}
 
 	/* check whether it can be found in default mode table */
-	mode = drm_mode_find_dmt(dev, hsize, vsize, vrefresh_rate);
+	if (drm_monitor_supports_rb(edid)) {
+		mode = drm_mode_find_dmt(dev, hsize, vsize, vrefresh_rate,
+					 true);
+		if (mode)
+			return mode;
+	}
+	mode = drm_mode_find_dmt(dev, hsize, vsize, vrefresh_rate, false);
 	if (mode)
 		return mode;
 
+	/* okay, generate it */
 	switch (timing_level) {
 	case LEVEL_DMT:
 		break;
@@ -748,6 +779,8 @@ drm_mode_std(struct drm_connector *connector, struct edid *edid,
 		 * secondary GTF curve.  Please don't do that.
 		 */
 		mode = drm_gtf_mode(dev, hsize, vsize, vrefresh_rate, 0, 0);
+		if (!mode)
+			return NULL;
 		if (drm_mode_hsync(mode) > drm_gtf2_hbreak(edid)) {
 			drm_mode_destroy(dev, mode);
 			mode = drm_gtf_mode_complex(dev, hsize, vsize,
@@ -909,15 +942,6 @@ static struct drm_display_mode *drm_mode_detailed(struct drm_device *dev,
 }
 
 static bool
-mode_is_rb(const struct drm_display_mode *mode)
-{
-	return (mode->htotal - mode->hdisplay == 160) &&
-	       (mode->hsync_end - mode->hdisplay == 80) &&
-	       (mode->hsync_end - mode->hsync_start == 32) &&
-	       (mode->vsync_start - mode->vdisplay == 3);
-}
-
-static bool
 mode_in_hsync_range(const struct drm_display_mode *mode,
 		    struct edid *edid, u8 *t)
 {
@@ -994,12 +1018,8 @@ mode_in_range(const struct drm_display_mode *mode, struct edid *edid,
 	return true;
 }
 
-/*
- * XXX If drm_dmt_modes ever regrows the CVT-R modes (and it will) this will
- * need to account for them.
- */
 static int
-drm_gtf_modes_for_range(struct drm_connector *connector, struct edid *edid,
+drm_dmt_modes_for_range(struct drm_connector *connector, struct edid *edid,
 			struct detailed_timing *timing)
 {
 	int i, modes = 0;
@@ -1019,17 +1039,110 @@ drm_gtf_modes_for_range(struct drm_connector *connector, struct edid *edid,
 	return modes;
 }
 
+/* fix up 1366x768 mode from 1368x768;
+ * GFT/CVT can't express 1366 width which isn't dividable by 8
+ */
+static void fixup_mode_1366x768(struct drm_display_mode *mode)
+{
+	if (mode->hdisplay == 1368 && mode->vdisplay == 768) {
+		mode->hdisplay = 1366;
+		mode->hsync_start--;
+		mode->hsync_end--;
+		drm_mode_set_name(mode);
+	}
+}
+
+static int
+drm_gtf_modes_for_range(struct drm_connector *connector, struct edid *edid,
+			struct detailed_timing *timing)
+{
+	int i, modes = 0;
+	struct drm_display_mode *newmode;
+	struct drm_device *dev = connector->dev;
+
+	for (i = 0; i < num_extra_modes; i++) {
+		const struct minimode *m = &extra_modes[i];
+		newmode = drm_gtf_mode(dev, m->w, m->h, m->r, 0, 0);
+		if (!newmode)
+			return modes;
+
+		fixup_mode_1366x768(newmode);
+		if (!mode_in_range(newmode, edid, timing)) {
+			drm_mode_destroy(dev, newmode);
+			continue;
+		}
+
+		drm_mode_probed_add(connector, newmode);
+		modes++;
+	}
+
+	return modes;
+}
+
+static int
+drm_cvt_modes_for_range(struct drm_connector *connector, struct edid *edid,
+			struct detailed_timing *timing)
+{
+	int i, modes = 0;
+	struct drm_display_mode *newmode;
+	struct drm_device *dev = connector->dev;
+	bool rb = drm_monitor_supports_rb(edid);
+
+	for (i = 0; i < num_extra_modes; i++) {
+		const struct minimode *m = &extra_modes[i];
+		newmode = drm_cvt_mode(dev, m->w, m->h, m->r, rb, 0, 0);
+		if (!newmode)
+			return modes;
+
+		fixup_mode_1366x768(newmode);
+		if (!mode_in_range(newmode, edid, timing)) {
+			drm_mode_destroy(dev, newmode);
+			continue;
+		}
+
+		drm_mode_probed_add(connector, newmode);
+		modes++;
+	}
+
+	return modes;
+}
+
 static void
 do_inferred_modes(struct detailed_timing *timing, void *c)
 {
 	struct detailed_mode_closure *closure = c;
 	struct detailed_non_pixel *data = &timing->data.other_data;
-	int gtf = (closure->edid->features & DRM_EDID_FEATURE_DEFAULT_GTF);
+	struct detailed_data_monitor_range *range = &data->data.range;
+
+	if (data->type != EDID_DETAIL_MONITOR_RANGE)
+		return;
 
-	if (gtf && data->type == EDID_DETAIL_MONITOR_RANGE)
+	closure->modes += drm_dmt_modes_for_range(closure->connector,
+						  closure->edid,
+						  timing);
+	
+	if (!version_greater(closure->edid, 1, 1))
+		return; /* GTF not defined yet */
+
+	switch (range->flags) {
+	case 0x02: /* secondary gtf, XXX could do more */
+	case 0x00: /* default gtf */
 		closure->modes += drm_gtf_modes_for_range(closure->connector,
 							  closure->edid,
 							  timing);
+		break;
+	case 0x04: /* cvt, only in 1.4+ */
+		if (!version_greater(closure->edid, 1, 3))
+			break;
+
+		closure->modes += drm_cvt_modes_for_range(closure->connector,
+							  closure->edid,
+							  timing);
+		break;
+	case 0x01: /* just the ranges, no formula */
+	default:
+		break;
+	}
 }
 
 static int
@@ -1062,8 +1175,8 @@ drm_est3_modes(struct drm_connector *connector, struct detailed_timing *timing)
 				mode = drm_mode_find_dmt(connector->dev,
 							 est3_modes[m].w,
 							 est3_modes[m].h,
-							 est3_modes[m].r
-							 /*, est3_modes[m].rb */);
+							 est3_modes[m].r,
+							 est3_modes[m].rb);
 				if (mode) {
 					drm_mode_probed_add(connector, mode);
 					modes++;
@@ -1312,6 +1425,8 @@ add_detailed_modes(struct drm_connector *connector, struct edid *edid,
 #define VENDOR_BLOCK    0x03
 #define SPEAKER_BLOCK	0x04
 #define EDID_BASIC_AUDIO	(1 << 6)
+#define EDID_CEA_YCRCB444	(1 << 5)
+#define EDID_CEA_YCRCB422	(1 << 4)
 
 /**
  * Search EDID for CEA extension block.
@@ -1666,13 +1781,29 @@ static void drm_add_display_info(struct edid *edid,
 	info->bpc = 0;
 	info->color_formats = 0;
 
-	/* Only defined for 1.4 with digital displays */
-	if (edid->revision < 4)
+	if (edid->revision < 3)
 		return;
 
 	if (!(edid->input & DRM_EDID_INPUT_DIGITAL))
 		return;
 
+	/* Get data from CEA blocks if present */
+	edid_ext = drm_find_cea_extension(edid);
+	if (edid_ext) {
+		info->cea_rev = edid_ext[1];
+
+		/* The existence of a CEA block should imply RGB support */
+		info->color_formats = DRM_COLOR_FORMAT_RGB444;
+		if (edid_ext[3] & EDID_CEA_YCRCB444)
+			info->color_formats |= DRM_COLOR_FORMAT_YCRCB444;
+		if (edid_ext[3] & EDID_CEA_YCRCB422)
+			info->color_formats |= DRM_COLOR_FORMAT_YCRCB422;
+	}
+
+	/* Only defined for 1.4 with digital displays */
+	if (edid->revision < 4)
+		return;
+
 	switch (edid->input & DRM_EDID_DIGITAL_DEPTH_MASK) {
 	case DRM_EDID_DIGITAL_DEPTH_6:
 		info->bpc = 6;
@@ -1698,18 +1829,11 @@ static void drm_add_display_info(struct edid *edid,
 		break;
 	}
 
-	info->color_formats = DRM_COLOR_FORMAT_RGB444;
-	if (info->color_formats & DRM_EDID_FEATURE_RGB_YCRCB444)
-		info->color_formats = DRM_COLOR_FORMAT_YCRCB444;
-	if (info->color_formats & DRM_EDID_FEATURE_RGB_YCRCB422)
-		info->color_formats = DRM_COLOR_FORMAT_YCRCB422;
-
-	/* Get data from CEA blocks if present */
-	edid_ext = drm_find_cea_extension(edid);
-	if (!edid_ext)
-		return;
-
-	info->cea_rev = edid_ext[1];
+	info->color_formats |= DRM_COLOR_FORMAT_RGB444;
+	if (edid->features & DRM_EDID_FEATURE_RGB_YCRCB444)
+		info->color_formats |= DRM_COLOR_FORMAT_YCRCB444;
+	if (edid->features & DRM_EDID_FEATURE_RGB_YCRCB422)
+		info->color_formats |= DRM_COLOR_FORMAT_YCRCB422;
 }
 
 /**
diff --git a/drivers/gpu/drm/drm_edid_load.c b/drivers/gpu/drm/drm_edid_load.c
index da9acba2dd6..66d4a28ad5a 100644
--- a/drivers/gpu/drm/drm_edid_load.c
+++ b/drivers/gpu/drm/drm_edid_load.c
@@ -173,7 +173,7 @@ static int edid_load(struct drm_connector *connector, char *name,
 	}
 	memcpy(edid, fwdata, fwsize);
 
-	if (!drm_edid_block_valid(edid)) {
+	if (!drm_edid_block_valid(edid, 0)) {
 		DRM_ERROR("Base block of EDID firmware \"%s\" is invalid ",
 		    name);
 		kfree(edid);
@@ -185,7 +185,7 @@ static int edid_load(struct drm_connector *connector, char *name,
 		if (i != valid_extensions + 1)
 			memcpy(edid + (valid_extensions + 1) * EDID_LENGTH,
 			    edid + i * EDID_LENGTH, EDID_LENGTH);
-		if (drm_edid_block_valid(edid + i * EDID_LENGTH))
+		if (drm_edid_block_valid(edid + i * EDID_LENGTH, i))
 			valid_extensions++;
 	}
 
@@ -220,18 +220,18 @@ int drm_load_edid_firmware(struct drm_connector *connector)
 {
 	char *connector_name = drm_get_connector_name(connector);
 	char *edidname = edid_firmware, *last, *colon;
-	int ret = 0;
+	int ret;
 
 	if (*edidname == '\0')
-		return ret;
+		return 0;
 
 	colon = strchr(edidname, ':');
 	if (colon != NULL) {
 		if (strncmp(connector_name, edidname, colon - edidname))
-			return ret;
+			return 0;
 		edidname = colon + 1;
 		if (*edidname == '\0')
-			return ret;
+			return 0;
 	}
 
 	last = edidname + strlen(edidname) - 1;
diff --git a/drivers/gpu/drm/drm_edid_modes.h b/drivers/gpu/drm/drm_edid_modes.h
index a91ffb11722..ff98a7eb38d 100644
--- a/drivers/gpu/drm/drm_edid_modes.h
+++ b/drivers/gpu/drm/drm_edid_modes.h
@@ -30,7 +30,6 @@
 /*
  * Autogenerated from the DMT spec.
  * This table is copied from xfree86/modes/xf86EdidModes.c.
- * But the mode with Reduced blank feature is deleted.
  */
 static const struct drm_display_mode drm_dmt_modes[] = {
 	/* 640x350@85Hz */
@@ -81,6 +80,10 @@ static const struct drm_display_mode drm_dmt_modes[] = {
 	{ DRM_MODE("800x600", DRM_MODE_TYPE_DRIVER, 56250, 800, 832,
 		   896, 1048, 0, 600, 601, 604, 631, 0,
 		   DRM_MODE_FLAG_PHSYNC | DRM_MODE_FLAG_PVSYNC) },
+	/* 800x600@120Hz RB */
+	{ DRM_MODE("800x600", DRM_MODE_TYPE_DRIVER, 73250, 800, 848,
+		   880, 960, 0, 600, 603, 607, 636, 0,
+		   DRM_MODE_FLAG_PHSYNC | DRM_MODE_FLAG_NVSYNC) },
 	/* 848x480@60Hz */
 	{ DRM_MODE("848x480", DRM_MODE_TYPE_DRIVER, 33750, 848, 864,
 		   976, 1088, 0, 480, 486, 494, 517, 0,
@@ -106,10 +109,18 @@ static const struct drm_display_mode drm_dmt_modes[] = {
 	{ DRM_MODE("1024x768", DRM_MODE_TYPE_DRIVER, 94500, 1024, 1072,
 		   1168, 1376, 0, 768, 769, 772, 808, 0,
 		   DRM_MODE_FLAG_PHSYNC | DRM_MODE_FLAG_PVSYNC) },
+	/* 1024x768@120Hz RB */
+	{ DRM_MODE("1024x768", DRM_MODE_TYPE_DRIVER, 115500, 1024, 1072,
+		   1104, 1184, 0, 768, 771, 775, 813, 0,
+		   DRM_MODE_FLAG_PHSYNC | DRM_MODE_FLAG_NVSYNC) },
 	/* 1152x864@75Hz */
 	{ DRM_MODE("1152x864", DRM_MODE_TYPE_DRIVER, 108000, 1152, 1216,
 		   1344, 1600, 0, 864, 865, 868, 900, 0,
 		   DRM_MODE_FLAG_PHSYNC | DRM_MODE_FLAG_PVSYNC) },
+	/* 1280x768@60Hz RB */
+	{ DRM_MODE("1280x768", DRM_MODE_TYPE_DRIVER, 68250, 1280, 1328,
+		   1360, 1440, 0, 768, 771, 778, 790, 0,
+		   DRM_MODE_FLAG_PHSYNC | DRM_MODE_FLAG_NVSYNC) },
 	/* 1280x768@60Hz */
 	{ DRM_MODE("1280x768", DRM_MODE_TYPE_DRIVER, 79500, 1280, 1344,
 		   1472, 1664, 0, 768, 771, 778, 798, 0,
@@ -122,6 +133,14 @@ static const struct drm_display_mode drm_dmt_modes[] = {
 	{ DRM_MODE("1280x768", DRM_MODE_TYPE_DRIVER, 117500, 1280, 1360,
 		   1496, 1712, 0, 768, 771, 778, 809, 0,
 		   DRM_MODE_FLAG_NHSYNC | DRM_MODE_FLAG_PVSYNC) },
+	/* 1280x768@120Hz RB */
+	{ DRM_MODE("1280x768", DRM_MODE_TYPE_DRIVER, 140250, 1280, 1328,
+		   1360, 1440, 0, 768, 771, 778, 813, 0,
+		   DRM_MODE_FLAG_PHSYNC | DRM_MODE_FLAG_NVSYNC) },
+	/* 1280x800@60Hz RB */
+	{ DRM_MODE("1280x800", DRM_MODE_TYPE_DRIVER, 71000, 1280, 1328,
+		   1360, 1440, 0, 800, 803, 809, 823, 0,
+		   DRM_MODE_FLAG_PHSYNC | DRM_MODE_FLAG_NVSYNC) },
 	/* 1280x800@60Hz */
 	{ DRM_MODE("1280x800", DRM_MODE_TYPE_DRIVER, 83500, 1280, 1352,
 		   1480, 1680, 0, 800, 803, 809, 831, 0,
@@ -134,6 +153,10 @@ static const struct drm_display_mode drm_dmt_modes[] = {
 	{ DRM_MODE("1280x800", DRM_MODE_TYPE_DRIVER, 122500, 1280, 1360,
 		   1496, 1712, 0, 800, 803, 809, 843, 0,
 		   DRM_MODE_FLAG_NHSYNC | DRM_MODE_FLAG_PVSYNC) },
+	/* 1280x800@120Hz RB */
+	{ DRM_MODE("1280x800", DRM_MODE_TYPE_DRIVER, 146250, 1280, 1328,
+		   1360, 1440, 0, 800, 803, 809, 847, 0,
+		   DRM_MODE_FLAG_PHSYNC | DRM_MODE_FLAG_NVSYNC) },
 	/* 1280x960@60Hz */
 	{ DRM_MODE("1280x960", DRM_MODE_TYPE_DRIVER, 108000, 1280, 1376,
 		   1488, 1800, 0, 960, 961, 964, 1000, 0,
@@ -142,6 +165,10 @@ static const struct drm_display_mode drm_dmt_modes[] = {
 	{ DRM_MODE("1280x960", DRM_MODE_TYPE_DRIVER, 148500, 1280, 1344,
 		   1504, 1728, 0, 960, 961, 964, 1011, 0,
 		   DRM_MODE_FLAG_PHSYNC | DRM_MODE_FLAG_PVSYNC) },
+	/* 1280x960@120Hz RB */
+	{ DRM_MODE("1280x960", DRM_MODE_TYPE_DRIVER, 175500, 1280, 1328,
+		   1360, 1440, 0, 960, 963, 967, 1017, 0,
+		   DRM_MODE_FLAG_PHSYNC | DRM_MODE_FLAG_NVSYNC) },
 	/* 1280x1024@60Hz */
 	{ DRM_MODE("1280x1024", DRM_MODE_TYPE_DRIVER, 108000, 1280, 1328,
 		   1440, 1688, 0, 1024, 1025, 1028, 1066, 0,
@@ -154,22 +181,42 @@ static const struct drm_display_mode drm_dmt_modes[] = {
 	{ DRM_MODE("1280x1024", DRM_MODE_TYPE_DRIVER, 157500, 1280, 1344,
 		   1504, 1728, 0, 1024, 1025, 1028, 1072, 0,
 		   DRM_MODE_FLAG_PHSYNC | DRM_MODE_FLAG_PVSYNC) },
+	/* 1280x1024@120Hz RB */
+	{ DRM_MODE("1280x1024", DRM_MODE_TYPE_DRIVER, 187250, 1280, 1328,
+		   1360, 1440, 0, 1024, 1027, 1034, 1084, 0,
+		   DRM_MODE_FLAG_PHSYNC | DRM_MODE_FLAG_NVSYNC) },
 	/* 1360x768@60Hz */
 	{ DRM_MODE("1360x768", DRM_MODE_TYPE_DRIVER, 85500, 1360, 1424,
 		   1536, 1792, 0, 768, 771, 777, 795, 0,
 		   DRM_MODE_FLAG_PHSYNC | DRM_MODE_FLAG_PVSYNC) },
-	/* 1440x1050@60Hz */
+	/* 1360x768@120Hz RB */
+	{ DRM_MODE("1360x768", DRM_MODE_TYPE_DRIVER, 148250, 1360, 1408,
+		   1440, 1520, 0, 768, 771, 776, 813, 0,
+		   DRM_MODE_FLAG_PHSYNC | DRM_MODE_FLAG_NVSYNC) },
+	/* 1400x1050@60Hz RB */
+	{ DRM_MODE("1400x1050", DRM_MODE_TYPE_DRIVER, 101000, 1400, 1448,
+		   1480, 1560, 0, 1050, 1053, 1057, 1080, 0,
+		   DRM_MODE_FLAG_PHSYNC | DRM_MODE_FLAG_NVSYNC) },
+	/* 1400x1050@60Hz */
 	{ DRM_MODE("1400x1050", DRM_MODE_TYPE_DRIVER, 121750, 1400, 1488,
 		   1632, 1864, 0, 1050, 1053, 1057, 1089, 0,
 		   DRM_MODE_FLAG_NHSYNC | DRM_MODE_FLAG_PVSYNC) },
-	/* 1440x1050@75Hz */
+	/* 1400x1050@75Hz */
 	{ DRM_MODE("1400x1050", DRM_MODE_TYPE_DRIVER, 156000, 1400, 1504,
 		   1648, 1896, 0, 1050, 1053, 1057, 1099, 0,
 		   DRM_MODE_FLAG_NHSYNC | DRM_MODE_FLAG_PVSYNC) },
-	/* 1440x1050@85Hz */
+	/* 1400x1050@85Hz */
 	{ DRM_MODE("1400x1050", DRM_MODE_TYPE_DRIVER, 179500, 1400, 1504,
 		   1656, 1912, 0, 1050, 1053, 1057, 1105, 0,
 		   DRM_MODE_FLAG_NHSYNC | DRM_MODE_FLAG_PVSYNC) },
+	/* 1400x1050@120Hz RB */
+	{ DRM_MODE("1400x1050", DRM_MODE_TYPE_DRIVER, 208000, 1400, 1448,
+		   1480, 1560, 0, 1050, 1053, 1057, 1112, 0,
+		   DRM_MODE_FLAG_PHSYNC | DRM_MODE_FLAG_NVSYNC) },
+	/* 1440x900@60Hz RB */
+	{ DRM_MODE("1440x900", DRM_MODE_TYPE_DRIVER, 88750, 1440, 1488,
+		   1520, 1600, 0, 900, 903, 909, 926, 0,
+		   DRM_MODE_FLAG_PHSYNC | DRM_MODE_FLAG_NVSYNC) },
 	/* 1440x900@60Hz */
 	{ DRM_MODE("1440x900", DRM_MODE_TYPE_DRIVER, 106500, 1440, 1520,
 		   1672, 1904, 0, 900, 903, 909, 934, 0,
@@ -182,6 +229,10 @@ static const struct drm_display_mode drm_dmt_modes[] = {
 	{ DRM_MODE("1440x900", DRM_MODE_TYPE_DRIVER, 157000, 1440, 1544,
 		   1696, 1952, 0, 900, 903, 909, 948, 0,
 		   DRM_MODE_FLAG_NHSYNC | DRM_MODE_FLAG_PVSYNC) },
+	/* 1440x900@120Hz RB */
+	{ DRM_MODE("1440x900", DRM_MODE_TYPE_DRIVER, 182750, 1440, 1488,
+		   1520, 1600, 0, 900, 903, 909, 953, 0,
+		   DRM_MODE_FLAG_PHSYNC | DRM_MODE_FLAG_NVSYNC) },
 	/* 1600x1200@60Hz */
 	{ DRM_MODE("1600x1200", DRM_MODE_TYPE_DRIVER, 162000, 1600, 1664,
 		   1856, 2160, 0, 1200, 1201, 1204, 1250, 0,
@@ -202,6 +253,14 @@ static const struct drm_display_mode drm_dmt_modes[] = {
 	{ DRM_MODE("1600x1200", DRM_MODE_TYPE_DRIVER, 229500, 1600, 1664,
 		   1856, 2160, 0, 1200, 1201, 1204, 1250, 0,
 		   DRM_MODE_FLAG_PHSYNC | DRM_MODE_FLAG_PVSYNC) },
+	/* 1600x1200@120Hz RB */
+	{ DRM_MODE("1600x1200", DRM_MODE_TYPE_DRIVER, 268250, 1600, 1648,
+		   1680, 1760, 0, 1200, 1203, 1207, 1271, 0,
+		   DRM_MODE_FLAG_PHSYNC | DRM_MODE_FLAG_NVSYNC) },
+	/* 1680x1050@60Hz RB */
+	{ DRM_MODE("1680x1050", DRM_MODE_TYPE_DRIVER, 119000, 1680, 1728,
+		   1760, 1840, 0, 1050, 1053, 1059, 1080, 0,
+		   DRM_MODE_FLAG_PHSYNC | DRM_MODE_FLAG_NVSYNC) },
 	/* 1680x1050@60Hz */
 	{ DRM_MODE("1680x1050", DRM_MODE_TYPE_DRIVER, 146250, 1680, 1784,
 		   1960, 2240, 0, 1050, 1053, 1059, 1089, 0,
@@ -214,15 +273,23 @@ static const struct drm_display_mode drm_dmt_modes[] = {
 	{ DRM_MODE("1680x1050", DRM_MODE_TYPE_DRIVER, 214750, 1680, 1808,
 		   1984, 2288, 0, 1050, 1053, 1059, 1105, 0,
 		   DRM_MODE_FLAG_NHSYNC | DRM_MODE_FLAG_PVSYNC) },
+	/* 1680x1050@120Hz RB */
+	{ DRM_MODE("1680x1050", DRM_MODE_TYPE_DRIVER, 245500, 1680, 1728,
+		   1760, 1840, 0, 1050, 1053, 1059, 1112, 0,
+		   DRM_MODE_FLAG_PHSYNC | DRM_MODE_FLAG_NVSYNC) },
 	/* 1792x1344@60Hz */
 	{ DRM_MODE("1792x1344", DRM_MODE_TYPE_DRIVER, 204750, 1792, 1920,
 		   2120, 2448, 0, 1344, 1345, 1348, 1394, 0,
 		   DRM_MODE_FLAG_NHSYNC | DRM_MODE_FLAG_PVSYNC) },
-	/* 1729x1344@75Hz */
+	/* 1792x1344@75Hz */
 	{ DRM_MODE("1792x1344", DRM_MODE_TYPE_DRIVER, 261000, 1792, 1888,
 		   2104, 2456, 0, 1344, 1345, 1348, 1417, 0,
 		   DRM_MODE_FLAG_NHSYNC | DRM_MODE_FLAG_PVSYNC) },
-	/* 1853x1392@60Hz */
+	/* 1792x1344@120Hz RB */
+	{ DRM_MODE("1792x1344", DRM_MODE_TYPE_DRIVER, 333250, 1792, 1840,
+		   1872, 1952, 0, 1344, 1347, 1351, 1423, 0,
+		   DRM_MODE_FLAG_PHSYNC | DRM_MODE_FLAG_NVSYNC) },
+	/* 1856x1392@60Hz */
 	{ DRM_MODE("1856x1392", DRM_MODE_TYPE_DRIVER, 218250, 1856, 1952,
 		   2176, 2528, 0, 1392, 1393, 1396, 1439, 0,
 		   DRM_MODE_FLAG_NHSYNC | DRM_MODE_FLAG_PVSYNC) },
@@ -230,6 +297,14 @@ static const struct drm_display_mode drm_dmt_modes[] = {
 	{ DRM_MODE("1856x1392", DRM_MODE_TYPE_DRIVER, 288000, 1856, 1984,
 		   2208, 2560, 0, 1392, 1395, 1399, 1500, 0,
 		   DRM_MODE_FLAG_NHSYNC | DRM_MODE_FLAG_PVSYNC) },
+	/* 1856x1392@120Hz RB */
+	{ DRM_MODE("1856x1392", DRM_MODE_TYPE_DRIVER, 356500, 1856, 1904,
+		   1936, 2016, 0, 1392, 1395, 1399, 1474, 0,
+		   DRM_MODE_FLAG_PHSYNC | DRM_MODE_FLAG_NVSYNC) },
+	/* 1920x1200@60Hz RB */
+	{ DRM_MODE("1920x1200", DRM_MODE_TYPE_DRIVER, 154000, 1920, 1968,
+		   2000, 2080, 0, 1200, 1203, 1209, 1235, 0,
+		   DRM_MODE_FLAG_PHSYNC | DRM_MODE_FLAG_NVSYNC) },
 	/* 1920x1200@60Hz */
 	{ DRM_MODE("1920x1200", DRM_MODE_TYPE_DRIVER, 193250, 1920, 2056,
 		   2256, 2592, 0, 1200, 1203, 1209, 1245, 0,
@@ -242,6 +317,10 @@ static const struct drm_display_mode drm_dmt_modes[] = {
 	{ DRM_MODE("1920x1200", DRM_MODE_TYPE_DRIVER, 281250, 1920, 2064,
 		   2272, 2624, 0, 1200, 1203, 1209, 1262, 0,
 		   DRM_MODE_FLAG_NHSYNC | DRM_MODE_FLAG_PVSYNC) },
+	/* 1920x1200@120Hz RB */
+	{ DRM_MODE("1920x1200", DRM_MODE_TYPE_DRIVER, 317000, 1920, 1968,
+		   2000, 2080, 0, 1200, 1203, 1209, 1271, 0,
+		   DRM_MODE_FLAG_PHSYNC | DRM_MODE_FLAG_NVSYNC) },
 	/* 1920x1440@60Hz */
 	{ DRM_MODE("1920x1440", DRM_MODE_TYPE_DRIVER, 234000, 1920, 2048,
 		   2256, 2600, 0, 1440, 1441, 1444, 1500, 0,
@@ -250,6 +329,14 @@ static const struct drm_display_mode drm_dmt_modes[] = {
 	{ DRM_MODE("1920x1440", DRM_MODE_TYPE_DRIVER, 297000, 1920, 2064,
 		   2288, 2640, 0, 1440, 1441, 1444, 1500, 0,
 		   DRM_MODE_FLAG_NHSYNC | DRM_MODE_FLAG_PVSYNC) },
+	/* 1920x1440@120Hz RB */
+	{ DRM_MODE("1920x1440", DRM_MODE_TYPE_DRIVER, 380500, 1920, 1968,
+		   2000, 2080, 0, 1440, 1443, 1447, 1525, 0,
+		   DRM_MODE_FLAG_PHSYNC | DRM_MODE_FLAG_NVSYNC) },
+	/* 2560x1600@60Hz RB */
+	{ DRM_MODE("2560x1600", DRM_MODE_TYPE_DRIVER, 268500, 2560, 2608,
+		   2640, 2720, 0, 1600, 1603, 1609, 1646, 0,
+		   DRM_MODE_FLAG_PHSYNC | DRM_MODE_FLAG_NVSYNC) },
 	/* 2560x1600@60Hz */
 	{ DRM_MODE("2560x1600", DRM_MODE_TYPE_DRIVER, 348500, 2560, 2752,
 		   3032, 3504, 0, 1600, 1603, 1609, 1658, 0,
@@ -262,6 +349,11 @@ static const struct drm_display_mode drm_dmt_modes[] = {
 	{ DRM_MODE("2560x1600", DRM_MODE_TYPE_DRIVER, 505250, 2560, 2768,
 		   3048, 3536, 0, 1600, 1603, 1609, 1682, 0,
 		   DRM_MODE_FLAG_NHSYNC | DRM_MODE_FLAG_PVSYNC) },
+	/* 2560x1600@120Hz RB */
+	{ DRM_MODE("2560x1600", DRM_MODE_TYPE_DRIVER, 552750, 2560, 2608,
+		   2640, 2720, 0, 1600, 1603, 1609, 1694, 0,
+		   DRM_MODE_FLAG_PHSYNC | DRM_MODE_FLAG_NVSYNC) },
+
 };
 static const int drm_num_dmt_modes =
 	sizeof(drm_dmt_modes) / sizeof(struct drm_display_mode);
@@ -320,12 +412,14 @@ static const struct drm_display_mode edid_est_modes[] = {
 		   DRM_MODE_FLAG_PHSYNC | DRM_MODE_FLAG_PVSYNC) }, /* 1152x864@75Hz */
 };
 
-static const struct {
+struct minimode {
 	short w;
 	short h;
 	short r;
 	short rb;
-} est3_modes[] = {
+};
+
+static const struct minimode est3_modes[] = {
 	/* byte 6 */
 	{ 640, 350, 85, 0 },
 	{ 640, 400, 85, 0 },
@@ -377,288 +471,304 @@ static const struct {
 	{ 1920, 1440, 60, 0 },
 	{ 1920, 1440, 75, 0 },
 };
-static const int num_est3_modes = sizeof(est3_modes) / sizeof(est3_modes[0]);
+static const int num_est3_modes = ARRAY_SIZE(est3_modes);
+
+static const struct minimode extra_modes[] = {
+	{ 1024, 576,  60, 0 },
+	{ 1366, 768,  60, 0 },
+	{ 1600, 900,  60, 0 },
+	{ 1680, 945,  60, 0 },
+	{ 1920, 1080, 60, 0 },
+	{ 2048, 1152, 60, 0 },
+	{ 2048, 1536, 60, 0 },
+};
+static const int num_extra_modes = ARRAY_SIZE(extra_modes);
 
 /*
  * Probably taken from CEA-861 spec.
  * This table is converted from xorg's hw/xfree86/modes/xf86EdidModes.c.
  */
 static const struct drm_display_mode edid_cea_modes[] = {
-	/* 640x480@60Hz */
+	/* 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) },
-	/* 720x480@60Hz */
+	/* 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) },
-	/* 720x480@60Hz */
+	/* 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) },
-	/* 1280x720@60Hz */
+	/* 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) },
-	/* 1920x1080i@60Hz */
+	/* 5 - 1920x1080i@60Hz */
 	{ DRM_MODE("1920x1080", 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) },
-	/* 1440x480i@60Hz */
+	/* 6 - 1440x480i@60Hz */
 	{ DRM_MODE("1440x480", 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) },
-	/* 1440x480i@60Hz */
+			DRM_MODE_FLAG_INTERLACE | DRM_MODE_FLAG_DBLCLK) },
+	/* 7 - 1440x480i@60Hz */
 	{ DRM_MODE("1440x480", 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) },
-	/* 1440x240@60Hz */
+			DRM_MODE_FLAG_INTERLACE | DRM_MODE_FLAG_DBLCLK) },
+	/* 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) },
-	/* 1440x240@60Hz */
+		   DRM_MODE_FLAG_NHSYNC | DRM_MODE_FLAG_NVSYNC |
+			DRM_MODE_FLAG_DBLCLK) },
+	/* 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) },
-	/* 2880x480i@60Hz */
+		   DRM_MODE_FLAG_NHSYNC | DRM_MODE_FLAG_NVSYNC |
+			DRM_MODE_FLAG_DBLCLK) },
+	/* 10 - 2880x480i@60Hz */
 	{ DRM_MODE("2880x480", 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) },
-	/* 2880x480i@60Hz */
+	/* 11 - 2880x480i@60Hz */
 	{ DRM_MODE("2880x480", 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) },
-	/* 2880x240@60Hz */
+	/* 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) },
-	/* 2880x240@60Hz */
+	/* 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) },
-	/* 1440x480@60Hz */
+	/* 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) },
-	/* 1440x480@60Hz */
+	/* 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) },
-	/* 1920x1080@60Hz */
+	/* 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) },
-	/* 720x576@50Hz */
+	/* 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) },
-	/* 720x576@50Hz */
+	/* 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) },
-	/* 1280x720@50Hz */
+	/* 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) },
-	/* 1920x1080i@50Hz */
+	/* 20 - 1920x1080i@50Hz */
 	{ DRM_MODE("1920x1080", 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) },
-	/* 1440x576i@50Hz */
+	/* 21 - 1440x576i@50Hz */
 	{ DRM_MODE("1440x576", 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) },
-	/* 1440x576i@50Hz */
+			DRM_MODE_FLAG_INTERLACE | DRM_MODE_FLAG_DBLCLK) },
+	/* 22 - 1440x576i@50Hz */
 	{ DRM_MODE("1440x576", 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) },
-	/* 1440x288@50Hz */
+			DRM_MODE_FLAG_INTERLACE | DRM_MODE_FLAG_DBLCLK) },
+	/* 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) },
-	/* 1440x288@50Hz */
+		   DRM_MODE_FLAG_NHSYNC | DRM_MODE_FLAG_NVSYNC |
+			DRM_MODE_FLAG_DBLCLK) },
+	/* 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) },
-	/* 2880x576i@50Hz */
+		   DRM_MODE_FLAG_NHSYNC | DRM_MODE_FLAG_NVSYNC |
+			DRM_MODE_FLAG_DBLCLK) },
+	/* 25 - 2880x576i@50Hz */
 	{ DRM_MODE("2880x576", 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) },
-	/* 2880x576i@50Hz */
+	/* 26 - 2880x576i@50Hz */
 	{ DRM_MODE("2880x576", 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) },
-	/* 2880x288@50Hz */
+	/* 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) },
-	/* 2880x288@50Hz */
+	/* 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) },
-	/* 1440x576@50Hz */
+	/* 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) },
-	/* 1440x576@50Hz */
+	/* 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) },
-	/* 1920x1080@50Hz */
+	/* 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) },
-	/* 1920x1080@24Hz */
+	/* 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) },
-	/* 1920x1080@25Hz */
+	/* 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) },
-	/* 1920x1080@30Hz */
+	/* 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) },
-	/* 2880x480@60Hz */
+	/* 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) },
-	/* 2880x480@60Hz */
+	/* 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) },
-	/* 2880x576@50Hz */
+	/* 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) },
-	/* 2880x576@50Hz */
+	/* 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) },
-	/* 1920x1080i@50Hz */
+	/* 39 - 1920x1080i@50Hz */
 	{ DRM_MODE("1920x1080", 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) },
-	/* 1920x1080i@100Hz */
+	/* 40 - 1920x1080i@100Hz */
 	{ DRM_MODE("1920x1080", 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) },
-	/* 1280x720@100Hz */
+	/* 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) },
-	/* 720x576@100Hz */
+	/* 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) },
-	/* 720x576@100Hz */
+	/* 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) },
-	/* 1440x576i@100Hz */
+	/* 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) },
-	/* 1440x576i@100Hz */
+		   DRM_MODE_FLAG_NHSYNC | DRM_MODE_FLAG_NVSYNC |
+			DRM_MODE_FLAG_DBLCLK) },
+	/* 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) },
-	/* 1920x1080i@120Hz */
+		   DRM_MODE_FLAG_NHSYNC | DRM_MODE_FLAG_NVSYNC |
+			DRM_MODE_FLAG_DBLCLK) },
+	/* 46 - 1920x1080i@120Hz */
 	{ DRM_MODE("1920x1080", 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) },
-	/* 1280x720@120Hz */
+	/* 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) },
-	/* 720x480@120Hz */
+	/* 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) },
-	/* 720x480@120Hz */
+	/* 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) },
-	/* 1440x480i@120Hz */
+	/* 50 - 1440x480i@120Hz */
 	{ DRM_MODE("1440x480", 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) },
-	/* 1440x480i@120Hz */
+			DRM_MODE_FLAG_INTERLACE | DRM_MODE_FLAG_DBLCLK) },
+	/* 51 - 1440x480i@120Hz */
 	{ DRM_MODE("1440x480", 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) },
-	/* 720x576@200Hz */
+			DRM_MODE_FLAG_INTERLACE | DRM_MODE_FLAG_DBLCLK) },
+	/* 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) },
-	/* 720x576@200Hz */
+	/* 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) },
-	/* 1440x576i@200Hz */
+	/* 54 - 1440x576i@200Hz */
 	{ DRM_MODE("1440x576", 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) },
-	/* 1440x576i@200Hz */
+			DRM_MODE_FLAG_INTERLACE | DRM_MODE_FLAG_DBLCLK) },
+	/* 55 - 1440x576i@200Hz */
 	{ DRM_MODE("1440x576", 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) },
-	/* 720x480@240Hz */
+			DRM_MODE_FLAG_INTERLACE | DRM_MODE_FLAG_DBLCLK) },
+	/* 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) },
-	/* 720x480@240Hz */
+	/* 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) },
-	/* 1440x480i@240 */
+	/* 58 - 1440x480i@240 */
 	{ DRM_MODE("1440x480", 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) },
-	/* 1440x480i@240 */
+			DRM_MODE_FLAG_INTERLACE | DRM_MODE_FLAG_DBLCLK) },
+	/* 59 - 1440x480i@240 */
 	{ DRM_MODE("1440x480", 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) },
-	/* 1280x720@24Hz */
+			DRM_MODE_FLAG_INTERLACE | DRM_MODE_FLAG_DBLCLK) },
+	/* 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) },
-	/* 1280x720@25Hz */
+	/* 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) },
-	/* 1280x720@30Hz */
+	/* 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) },
-	/* 1920x1080@120Hz */
+	/* 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) },
-	/* 1920x1080@100Hz */
+	/* 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) },
 };
-static const int drm_num_cea_modes =
-	sizeof (edid_cea_modes) / sizeof (edid_cea_modes[0]);
+static const int drm_num_cea_modes = ARRAY_SIZE(edid_cea_modes);
diff --git a/drivers/gpu/drm/drm_fb_helper.c b/drivers/gpu/drm/drm_fb_helper.c
index a0d6e894d97..5683b7fdd74 100644
--- a/drivers/gpu/drm/drm_fb_helper.c
+++ b/drivers/gpu/drm/drm_fb_helper.c
@@ -136,6 +136,9 @@ static void drm_fb_helper_restore_lut_atomic(struct drm_crtc *crtc)
 {
 	uint16_t *r_base, *g_base, *b_base;
 
+	if (crtc->funcs->gamma_set == NULL)
+		return;
+
 	r_base = crtc->gamma_store;
 	g_base = r_base + crtc->gamma_size;
 	b_base = g_base + crtc->gamma_size;
@@ -383,7 +386,6 @@ int drm_fb_helper_init(struct drm_device *dev,
 		       int crtc_count, int max_conn_count)
 {
 	struct drm_crtc *crtc;
-	int ret = 0;
 	int i;
 
 	fb_helper->dev = dev;
@@ -408,10 +410,8 @@ int drm_fb_helper_init(struct drm_device *dev,
 				sizeof(struct drm_connector *),
 				GFP_KERNEL);
 
-		if (!fb_helper->crtc_info[i].mode_set.connectors) {
-			ret = -ENOMEM;
+		if (!fb_helper->crtc_info[i].mode_set.connectors)
 			goto out_free;
-		}
 		fb_helper->crtc_info[i].mode_set.num_connectors = 0;
 	}
 
@@ -1083,7 +1083,7 @@ static bool drm_target_cloned(struct drm_fb_helper *fb_helper,
 
 	/* try and find a 1024x768 mode on each connector */
 	can_clone = true;
-	dmt_mode = drm_mode_find_dmt(fb_helper->dev, 1024, 768, 60);
+	dmt_mode = drm_mode_find_dmt(fb_helper->dev, 1024, 768, 60, false);
 
 	for (i = 0; i < fb_helper->connector_count; i++) {
 
diff --git a/drivers/gpu/drm/drm_gem.c b/drivers/gpu/drm/drm_gem.c
index 83114b5e3ce..d58e69da1fb 100644
--- a/drivers/gpu/drm/drm_gem.c
+++ b/drivers/gpu/drm/drm_gem.c
@@ -201,6 +201,19 @@ free:
 }
 EXPORT_SYMBOL(drm_gem_object_alloc);
 
+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,
+				obj->import_attach->dmabuf);
+	}
+	if (obj->export_dma_buf) {
+		drm_prime_remove_imported_buf_handle(&filp->prime,
+				obj->export_dma_buf);
+	}
+}
+
 /**
  * Removes the mapping from handle to filp for this object.
  */
@@ -233,9 +246,7 @@ drm_gem_handle_delete(struct drm_file *filp, u32 handle)
 	idr_remove(&filp->object_idr, handle);
 	spin_unlock(&filp->table_lock);
 
-	if (obj->import_attach)
-		drm_prime_remove_imported_buf_handle(&filp->prime,
-				obj->import_attach->dmabuf);
+	drm_gem_remove_prime_handles(obj, filp);
 
 	if (dev->driver->gem_close_object)
 		dev->driver->gem_close_object(obj, filp);
@@ -272,8 +283,7 @@ again:
 	spin_unlock(&file_priv->table_lock);
 	if (ret == -EAGAIN)
 		goto again;
-
-	if (ret != 0)
+	else if (ret)
 		return ret;
 
 	drm_gem_object_handle_reference(obj);
@@ -329,7 +339,7 @@ drm_gem_create_mmap_offset(struct drm_gem_object *obj)
 	struct drm_gem_mm *mm = dev->mm_private;
 	struct drm_map_list *list;
 	struct drm_local_map *map;
-	int ret = 0;
+	int ret;
 
 	/* Set the object up for mmap'ing */
 	list = &obj->map_list;
@@ -456,8 +466,7 @@ again:
 
 		if (ret == -EAGAIN)
 			goto again;
-
-		if (ret != 0)
+		else if (ret)
 			goto err;
 
 		/* Allocate a reference for the name table.  */
@@ -532,9 +541,7 @@ drm_gem_object_release_handle(int id, void *ptr, void *data)
 	struct drm_gem_object *obj = ptr;
 	struct drm_device *dev = obj->dev;
 
-	if (obj->import_attach)
-		drm_prime_remove_imported_buf_handle(&file_priv->prime,
-				obj->import_attach->dmabuf);
+	drm_gem_remove_prime_handles(obj, file_priv);
 
 	if (dev->driver->gem_close_object)
 		dev->driver->gem_close_object(obj, file_priv);
@@ -628,7 +635,7 @@ void drm_gem_vm_open(struct vm_area_struct *vma)
 	drm_gem_object_reference(obj);
 
 	mutex_lock(&obj->dev->struct_mutex);
-	drm_vm_open_locked(vma);
+	drm_vm_open_locked(obj->dev, vma);
 	mutex_unlock(&obj->dev->struct_mutex);
 }
 EXPORT_SYMBOL(drm_gem_vm_open);
@@ -639,7 +646,7 @@ void drm_gem_vm_close(struct vm_area_struct *vma)
 	struct drm_device *dev = obj->dev;
 
 	mutex_lock(&dev->struct_mutex);
-	drm_vm_close_locked(vma);
+	drm_vm_close_locked(obj->dev, vma);
 	drm_gem_object_unreference(obj);
 	mutex_unlock(&dev->struct_mutex);
 }
@@ -712,7 +719,7 @@ int drm_gem_mmap(struct file *filp, struct vm_area_struct *vma)
 	 */
 	drm_gem_object_reference(obj);
 
-	drm_vm_open_locked(vma);
+	drm_vm_open_locked(dev, vma);
 
 out_unlock:
 	mutex_unlock(&dev->struct_mutex);
diff --git a/drivers/gpu/drm/drm_ioctl.c b/drivers/gpu/drm/drm_ioctl.c
index cf85155da2a..64a62c69731 100644
--- a/drivers/gpu/drm/drm_ioctl.c
+++ b/drivers/gpu/drm/drm_ioctl.c
@@ -283,6 +283,10 @@ int drm_getcap(struct drm_device *dev, void *data, struct drm_file *file_priv)
 	case DRM_CAP_DUMB_PREFER_SHADOW:
 		req->value = dev->mode_config.prefer_shadow;
 		break;
+	case DRM_CAP_PRIME:
+		req->value |= dev->driver->prime_fd_to_handle ? DRM_PRIME_CAP_IMPORT : 0;
+		req->value |= dev->driver->prime_handle_to_fd ? DRM_PRIME_CAP_EXPORT : 0;
+		break;
 	default:
 		return -EINVAL;
 	}
diff --git a/drivers/gpu/drm/drm_irq.c b/drivers/gpu/drm/drm_irq.c
index c869436e238..c798eeae0a0 100644
--- a/drivers/gpu/drm/drm_irq.c
+++ b/drivers/gpu/drm/drm_irq.c
@@ -189,7 +189,7 @@ void drm_vblank_cleanup(struct drm_device *dev)
 	if (dev->num_crtcs == 0)
 		return;
 
-	del_timer(&dev->vblank_disable_timer);
+	del_timer_sync(&dev->vblank_disable_timer);
 
 	vblank_disable_fn((unsigned long)dev);
 
@@ -310,7 +310,7 @@ static void drm_irq_vgaarb_nokms(void *cookie, bool state)
  */
 int drm_irq_install(struct drm_device *dev)
 {
-	int ret = 0;
+	int ret;
 	unsigned long sh_flags = 0;
 	char *irqname;
 
@@ -731,7 +731,7 @@ EXPORT_SYMBOL(drm_calc_vbltimestamp_from_scanoutpos);
 u32 drm_get_last_vbltimestamp(struct drm_device *dev, int crtc,
 			      struct timeval *tvblank, unsigned flags)
 {
-	int ret = 0;
+	int ret;
 
 	/* Define requested maximum error on timestamps (nanoseconds). */
 	int max_error = (int) drm_timestamp_precision * 1000;
@@ -1031,18 +1031,15 @@ int drm_modeset_ctl(struct drm_device *dev, void *data,
 		    struct drm_file *file_priv)
 {
 	struct drm_modeset_ctl *modeset = data;
-	int ret = 0;
 	unsigned int crtc;
 
 	/* If drm_vblank_init() hasn't been called yet, just no-op */
 	if (!dev->num_crtcs)
-		goto out;
+		return 0;
 
 	crtc = modeset->crtc;
-	if (crtc >= dev->num_crtcs) {
-		ret = -EINVAL;
-		goto out;
-	}
+	if (crtc >= dev->num_crtcs)
+		return -EINVAL;
 
 	switch (modeset->cmd) {
 	case _DRM_PRE_MODESET:
@@ -1052,12 +1049,10 @@ int drm_modeset_ctl(struct drm_device *dev, void *data,
 		drm_vblank_post_modeset(dev, crtc);
 		break;
 	default:
-		ret = -EINVAL;
-		break;
+		return -EINVAL;
 	}
 
-out:
-	return ret;
+	return 0;
 }
 
 static int drm_queue_vblank_event(struct drm_device *dev, int pipe,
@@ -1154,7 +1149,7 @@ int drm_wait_vblank(struct drm_device *dev, void *data,
 		    struct drm_file *file_priv)
 {
 	union drm_wait_vblank *vblwait = data;
-	int ret = 0;
+	int ret;
 	unsigned int flags, seq, crtc, high_crtc;
 
 	if ((!drm_dev_to_irq(dev)) || (!dev->irq_enabled))
diff --git a/drivers/gpu/drm/drm_lock.c b/drivers/gpu/drm/drm_lock.c
index c79c713eeba..52115204169 100644
--- a/drivers/gpu/drm/drm_lock.c
+++ b/drivers/gpu/drm/drm_lock.c
@@ -331,7 +331,7 @@ static int drm_notifier(void *priv)
 
 void drm_idlelock_take(struct drm_lock_data *lock_data)
 {
-	int ret = 0;
+	int ret;
 
 	spin_lock_bh(&lock_data->spinlock);
 	lock_data->kernel_waiters++;
diff --git a/drivers/gpu/drm/drm_prime.c b/drivers/gpu/drm/drm_prime.c
index 1bdf2b54eaf..f546ff98a11 100644
--- a/drivers/gpu/drm/drm_prime.c
+++ b/drivers/gpu/drm/drm_prime.c
@@ -68,6 +68,7 @@ int drm_gem_prime_handle_to_fd(struct drm_device *dev,
 {
 	struct drm_gem_object *obj;
 	void *buf;
+	int ret;
 
 	obj = drm_gem_object_lookup(dev, file_priv, handle);
 	if (!obj)
@@ -100,6 +101,17 @@ int drm_gem_prime_handle_to_fd(struct drm_device *dev,
 		obj->export_dma_buf = buf;
 		*prime_fd = dma_buf_fd(buf, flags);
 	}
+	/* 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;
+	}
+
 	mutex_unlock(&file_priv->prime.lock);
 	return 0;
 }
@@ -227,6 +239,42 @@ out:
 }
 EXPORT_SYMBOL(drm_prime_pages_to_sg);
 
+/* export an sg table into an array of pages and addresses
+   this is currently required by the TTM driver in order to do correct fault
+   handling */
+int drm_prime_sg_to_page_addr_arrays(struct sg_table *sgt, struct page **pages,
+				     dma_addr_t *addrs, int max_pages)
+{
+	unsigned count;
+	struct scatterlist *sg;
+	struct page *page;
+	u32 len, offset;
+	int pg_index;
+	dma_addr_t addr;
+
+	pg_index = 0;
+	for_each_sg(sgt->sgl, sg, sgt->nents, count) {
+		len = sg->length;
+		offset = sg->offset;
+		page = sg_page(sg);
+		addr = sg_dma_address(sg);
+
+		while (len > 0) {
+			if (WARN_ON(pg_index >= max_pages))
+				return -1;
+			pages[pg_index] = page;
+			if (addrs)
+				addrs[pg_index] = addr;
+
+			page++;
+			addr += PAGE_SIZE;
+			len -= PAGE_SIZE;
+			pg_index++;
+		}
+	}
+	return 0;
+}
+EXPORT_SYMBOL(drm_prime_sg_to_page_addr_arrays);
 /* helper function to cleanup a GEM/prime object */
 void drm_prime_gem_destroy(struct drm_gem_object *obj, struct sg_table *sg)
 {
diff --git a/drivers/gpu/drm/drm_stub.c b/drivers/gpu/drm/drm_stub.c
index aa454f80e10..21bcd4a555d 100644
--- a/drivers/gpu/drm/drm_stub.c
+++ b/drivers/gpu/drm/drm_stub.c
@@ -122,11 +122,10 @@ again:
 	ret = idr_get_new_above(&drm_minors_idr, NULL,
 				base, &new_id);
 	mutex_unlock(&dev->struct_mutex);
-	if (ret == -EAGAIN) {
+	if (ret == -EAGAIN)
 		goto again;
-	} else if (ret) {
+	else if (ret)
 		return ret;
-	}
 
 	if (new_id >= limit) {
 		idr_remove(&drm_minors_idr, new_id);
@@ -211,7 +210,7 @@ EXPORT_SYMBOL(drm_master_put);
 int drm_setmaster_ioctl(struct drm_device *dev, void *data,
 			struct drm_file *file_priv)
 {
-	int ret = 0;
+	int ret;
 
 	if (file_priv->is_master)
 		return 0;
diff --git a/drivers/gpu/drm/drm_sysfs.c b/drivers/gpu/drm/drm_sysfs.c
index 5a7bd51fc3d..45cf1dd3eb9 100644
--- a/drivers/gpu/drm/drm_sysfs.c
+++ b/drivers/gpu/drm/drm_sysfs.c
@@ -347,17 +347,17 @@ static struct bin_attribute edid_attr = {
 };
 
 /**
- * drm_sysfs_connector_add - add an connector to sysfs
+ * drm_sysfs_connector_add - add a connector to sysfs
  * @connector: connector to add
  *
- * Create an connector device in sysfs, along with its associated connector
+ * Create a connector device in sysfs, along with its associated connector
  * properties (so far, connection status, dpms, mode list & edid) and
  * generate a hotplug event so userspace knows there's a new connector
  * available.
  *
  * Note:
- * This routine should only be called *once* for each DRM minor registered.
- * A second call for an already registered device will trigger the BUG_ON
+ * This routine should only be called *once* for each registered connector.
+ * A second call for an already registered connector will trigger the BUG_ON
  * below.
  */
 int drm_sysfs_connector_add(struct drm_connector *connector)
@@ -366,7 +366,7 @@ int drm_sysfs_connector_add(struct drm_connector *connector)
 	int attr_cnt = 0;
 	int opt_cnt = 0;
 	int i;
-	int ret = 0;
+	int ret;
 
 	/* We shouldn't get called more than once for the same connector */
 	BUG_ON(device_is_registered(&connector->kdev));
diff --git a/drivers/gpu/drm/drm_vm.c b/drivers/gpu/drm/drm_vm.c
index 14956181834..961ee08927f 100644
--- a/drivers/gpu/drm/drm_vm.c
+++ b/drivers/gpu/drm/drm_vm.c
@@ -406,10 +406,9 @@ static const struct vm_operations_struct drm_vm_sg_ops = {
  * Create a new drm_vma_entry structure as the \p vma private data entry and
  * add it to drm_device::vmalist.
  */
-void drm_vm_open_locked(struct vm_area_struct *vma)
+void drm_vm_open_locked(struct drm_device *dev,
+		struct vm_area_struct *vma)
 {
-	struct drm_file *priv = vma->vm_file->private_data;
-	struct drm_device *dev = priv->minor->dev;
 	struct drm_vma_entry *vma_entry;
 
 	DRM_DEBUG("0x%08lx,0x%08lx\n",
@@ -430,14 +429,13 @@ static void drm_vm_open(struct vm_area_struct *vma)
 	struct drm_device *dev = priv->minor->dev;
 
 	mutex_lock(&dev->struct_mutex);
-	drm_vm_open_locked(vma);
+	drm_vm_open_locked(dev, vma);
 	mutex_unlock(&dev->struct_mutex);
 }
 
-void drm_vm_close_locked(struct vm_area_struct *vma)
+void drm_vm_close_locked(struct drm_device *dev,
+		struct vm_area_struct *vma)
 {
-	struct drm_file *priv = vma->vm_file->private_data;
-	struct drm_device *dev = priv->minor->dev;
 	struct drm_vma_entry *pt, *temp;
 
 	DRM_DEBUG("0x%08lx,0x%08lx\n",
@@ -467,7 +465,7 @@ static void drm_vm_close(struct vm_area_struct *vma)
 	struct drm_device *dev = priv->minor->dev;
 
 	mutex_lock(&dev->struct_mutex);
-	drm_vm_close_locked(vma);
+	drm_vm_close_locked(dev, vma);
 	mutex_unlock(&dev->struct_mutex);
 }
 
@@ -519,7 +517,7 @@ static int drm_mmap_dma(struct file *filp, struct vm_area_struct *vma)
 	vma->vm_flags |= VM_RESERVED;	/* Don't swap */
 	vma->vm_flags |= VM_DONTEXPAND;
 
-	drm_vm_open_locked(vma);
+	drm_vm_open_locked(dev, vma);
 	return 0;
 }
 
@@ -670,7 +668,7 @@ int drm_mmap_locked(struct file *filp, struct vm_area_struct *vma)
 	vma->vm_flags |= VM_RESERVED;	/* Don't swap */
 	vma->vm_flags |= VM_DONTEXPAND;
 
-	drm_vm_open_locked(vma);
+	drm_vm_open_locked(dev, vma);
 	return 0;
 }
 
diff --git a/drivers/gpu/drm/exynos/Kconfig b/drivers/gpu/drm/exynos/Kconfig
index 3343ac437fe..7f5096763b7 100644
--- a/drivers/gpu/drm/exynos/Kconfig
+++ b/drivers/gpu/drm/exynos/Kconfig
@@ -10,6 +10,12 @@ config DRM_EXYNOS
 	  Choose this option if you have a Samsung SoC EXYNOS chipset.
 	  If M is selected the module will be called exynosdrm.
 
+config DRM_EXYNOS_DMABUF
+	bool "EXYNOS DRM DMABUF"
+	depends on DRM_EXYNOS
+	help
+	  Choose this option if you want to use DMABUF feature for DRM.
+
 config DRM_EXYNOS_FIMD
 	bool "Exynos DRM FIMD"
 	depends on DRM_EXYNOS && !FB_S3C
@@ -27,3 +33,9 @@ config DRM_EXYNOS_VIDI
 	depends on DRM_EXYNOS
 	help
 	  Choose this option if you want to use Exynos VIDI for DRM.
+
+config DRM_EXYNOS_G2D
+	bool "Exynos DRM G2D"
+	depends on DRM_EXYNOS
+	help
+	  Choose this option if you want to use Exynos G2D for DRM.
diff --git a/drivers/gpu/drm/exynos/Makefile b/drivers/gpu/drm/exynos/Makefile
index 9e0bff8badf..eb651ca8e2a 100644
--- a/drivers/gpu/drm/exynos/Makefile
+++ b/drivers/gpu/drm/exynos/Makefile
@@ -8,10 +8,12 @@ exynosdrm-y := exynos_drm_drv.o exynos_drm_encoder.o exynos_drm_connector.o \
 		exynos_drm_buf.o exynos_drm_gem.o exynos_drm_core.o \
 		exynos_drm_plane.o
 
+exynosdrm-$(CONFIG_DRM_EXYNOS_DMABUF) += exynos_drm_dmabuf.o
 exynosdrm-$(CONFIG_DRM_EXYNOS_FIMD)	+= exynos_drm_fimd.o
 exynosdrm-$(CONFIG_DRM_EXYNOS_HDMI)	+= exynos_hdmi.o exynos_mixer.o \
 					   exynos_ddc.o exynos_hdmiphy.o \
 					   exynos_drm_hdmi.o
 exynosdrm-$(CONFIG_DRM_EXYNOS_VIDI)	+= exynos_drm_vidi.o
+exynosdrm-$(CONFIG_DRM_EXYNOS_G2D)	+= exynos_drm_g2d.o
 
 obj-$(CONFIG_DRM_EXYNOS)		+= exynosdrm.o
diff --git a/drivers/gpu/drm/exynos/exynos_drm_buf.c b/drivers/gpu/drm/exynos/exynos_drm_buf.c
index de8d2090bce..b3cb0a69fbf 100644
--- a/drivers/gpu/drm/exynos/exynos_drm_buf.c
+++ b/drivers/gpu/drm/exynos/exynos_drm_buf.c
@@ -35,7 +35,7 @@ static int lowlevel_buffer_allocate(struct drm_device *dev,
 		unsigned int flags, struct exynos_drm_gem_buf *buf)
 {
 	dma_addr_t start_addr;
-	unsigned int npages, page_size, i = 0;
+	unsigned int npages, i = 0;
 	struct scatterlist *sgl;
 	int ret = 0;
 
@@ -53,13 +53,13 @@ static int lowlevel_buffer_allocate(struct drm_device *dev,
 
 	if (buf->size >= SZ_1M) {
 		npages = buf->size >> SECTION_SHIFT;
-		page_size = SECTION_SIZE;
+		buf->page_size = SECTION_SIZE;
 	} else if (buf->size >= SZ_64K) {
 		npages = buf->size >> 16;
-		page_size = SZ_64K;
+		buf->page_size = SZ_64K;
 	} else {
 		npages = buf->size >> PAGE_SHIFT;
-		page_size = PAGE_SIZE;
+		buf->page_size = PAGE_SIZE;
 	}
 
 	buf->sgt = kzalloc(sizeof(struct sg_table), GFP_KERNEL);
@@ -96,9 +96,9 @@ static int lowlevel_buffer_allocate(struct drm_device *dev,
 
 	while (i < npages) {
 		buf->pages[i] = phys_to_page(start_addr);
-		sg_set_page(sgl, buf->pages[i], page_size, 0);
+		sg_set_page(sgl, buf->pages[i], buf->page_size, 0);
 		sg_dma_address(sgl) = start_addr;
-		start_addr += page_size;
+		start_addr += buf->page_size;
 		sgl = sg_next(sgl);
 		i++;
 	}
diff --git a/drivers/gpu/drm/exynos/exynos_drm_crtc.c b/drivers/gpu/drm/exynos/exynos_drm_crtc.c
index 3486ffed0bf..4afb625128d 100644
--- a/drivers/gpu/drm/exynos/exynos_drm_crtc.c
+++ b/drivers/gpu/drm/exynos/exynos_drm_crtc.c
@@ -105,6 +105,8 @@ int exynos_drm_overlay_update(struct exynos_drm_overlay *overlay,
 	overlay->fb_y = pos->fb_y;
 	overlay->fb_width = fb->width;
 	overlay->fb_height = fb->height;
+	overlay->src_width = pos->src_w;
+	overlay->src_height = pos->src_h;
 	overlay->bpp = fb->bits_per_pixel;
 	overlay->pitch = fb->pitches[0];
 	overlay->pixel_format = fb->pixel_format;
@@ -153,6 +155,8 @@ static int exynos_drm_crtc_update(struct drm_crtc *crtc)
 	pos.crtc_y = 0;
 	pos.crtc_w = fb->width - crtc->x;
 	pos.crtc_h = fb->height - crtc->y;
+	pos.src_w = pos.crtc_w;
+	pos.src_h = pos.crtc_h;
 
 	return exynos_drm_overlay_update(overlay, crtc->fb, mode, &pos);
 }
diff --git a/drivers/gpu/drm/exynos/exynos_drm_crtc.h b/drivers/gpu/drm/exynos/exynos_drm_crtc.h
index 25f72a62cb8..16b8e2195a0 100644
--- a/drivers/gpu/drm/exynos/exynos_drm_crtc.h
+++ b/drivers/gpu/drm/exynos/exynos_drm_crtc.h
@@ -42,6 +42,8 @@ void exynos_drm_crtc_disable_vblank(struct drm_device *dev, int crtc);
  *	- the unit is screen coordinates.
  * @fb_y: offset y on a framebuffer to be displayed
  *	- the unit is screen coordinates.
+ * @src_w: width of source area to be displayed from a framebuffer.
+ * @src_h: height of source area to be displayed from a framebuffer.
  * @crtc_x: offset x on hardware screen.
  * @crtc_y: offset y on hardware screen.
  * @crtc_w: width of hardware screen.
@@ -50,6 +52,8 @@ void exynos_drm_crtc_disable_vblank(struct drm_device *dev, int crtc);
 struct exynos_drm_crtc_pos {
 	unsigned int fb_x;
 	unsigned int fb_y;
+	unsigned int src_w;
+	unsigned int src_h;
 	unsigned int crtc_x;
 	unsigned int crtc_y;
 	unsigned int crtc_w;
diff --git a/drivers/gpu/drm/exynos/exynos_drm_dmabuf.c b/drivers/gpu/drm/exynos/exynos_drm_dmabuf.c
new file mode 100644
index 00000000000..274909271c3
--- /dev/null
+++ b/drivers/gpu/drm/exynos/exynos_drm_dmabuf.c
@@ -0,0 +1,272 @@
+/* exynos_drm_dmabuf.c
+ *
+ * Copyright (c) 2012 Samsung Electronics Co., Ltd.
+ * Author: Inki Dae <inki.dae@samsung.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
+ * 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.
+ */
+
+#include "drmP.h"
+#include "drm.h"
+#include "exynos_drm_drv.h"
+#include "exynos_drm_gem.h"
+
+#include <linux/dma-buf.h>
+
+static struct sg_table *exynos_pages_to_sg(struct page **pages, int nr_pages,
+		unsigned int page_size)
+{
+	struct sg_table *sgt = NULL;
+	struct scatterlist *sgl;
+	int i, ret;
+
+	sgt = kzalloc(sizeof(*sgt), GFP_KERNEL);
+	if (!sgt)
+		goto out;
+
+	ret = sg_alloc_table(sgt, nr_pages, GFP_KERNEL);
+	if (ret)
+		goto err_free_sgt;
+
+	if (page_size < PAGE_SIZE)
+		page_size = PAGE_SIZE;
+
+	for_each_sg(sgt->sgl, sgl, nr_pages, i)
+		sg_set_page(sgl, pages[i], page_size, 0);
+
+	return sgt;
+
+err_free_sgt:
+	kfree(sgt);
+	sgt = NULL;
+out:
+	return NULL;
+}
+
+static struct sg_table *
+		exynos_gem_map_dma_buf(struct dma_buf_attachment *attach,
+					enum dma_data_direction dir)
+{
+	struct exynos_drm_gem_obj *gem_obj = attach->dmabuf->priv;
+	struct drm_device *dev = gem_obj->base.dev;
+	struct exynos_drm_gem_buf *buf;
+	struct sg_table *sgt = NULL;
+	unsigned int npages;
+	int nents;
+
+	DRM_DEBUG_PRIME("%s\n", __FILE__);
+
+	mutex_lock(&dev->struct_mutex);
+
+	buf = gem_obj->buffer;
+
+	/* there should always be pages allocated. */
+	if (!buf->pages) {
+		DRM_ERROR("pages is null.\n");
+		goto err_unlock;
+	}
+
+	npages = buf->size / buf->page_size;
+
+	sgt = exynos_pages_to_sg(buf->pages, npages, buf->page_size);
+	nents = dma_map_sg(attach->dev, sgt->sgl, sgt->nents, dir);
+
+	DRM_DEBUG_PRIME("npages = %d buffer size = 0x%lx page_size = 0x%lx\n",
+			npages, buf->size, buf->page_size);
+
+err_unlock:
+	mutex_unlock(&dev->struct_mutex);
+	return sgt;
+}
+
+static void exynos_gem_unmap_dma_buf(struct dma_buf_attachment *attach,
+						struct sg_table *sgt,
+						enum dma_data_direction dir)
+{
+	dma_unmap_sg(attach->dev, sgt->sgl, sgt->nents, dir);
+	sg_free_table(sgt);
+	kfree(sgt);
+	sgt = NULL;
+}
+
+static void exynos_dmabuf_release(struct dma_buf *dmabuf)
+{
+	struct exynos_drm_gem_obj *exynos_gem_obj = dmabuf->priv;
+
+	DRM_DEBUG_PRIME("%s\n", __FILE__);
+
+	/*
+	 * exynos_dmabuf_release() call means that file object's
+	 * f_count is 0 and it calls drm_gem_object_handle_unreference()
+	 * to drop the references that these values had been increased
+	 * at drm_prime_handle_to_fd()
+	 */
+	if (exynos_gem_obj->base.export_dma_buf == dmabuf) {
+		exynos_gem_obj->base.export_dma_buf = NULL;
+
+		/*
+		 * drop this gem object refcount to release allocated buffer
+		 * and resources.
+		 */
+		drm_gem_object_unreference_unlocked(&exynos_gem_obj->base);
+	}
+}
+
+static void *exynos_gem_dmabuf_kmap_atomic(struct dma_buf *dma_buf,
+						unsigned long page_num)
+{
+	/* TODO */
+
+	return NULL;
+}
+
+static void exynos_gem_dmabuf_kunmap_atomic(struct dma_buf *dma_buf,
+						unsigned long page_num,
+						void *addr)
+{
+	/* TODO */
+}
+
+static void *exynos_gem_dmabuf_kmap(struct dma_buf *dma_buf,
+					unsigned long page_num)
+{
+	/* TODO */
+
+	return NULL;
+}
+
+static void exynos_gem_dmabuf_kunmap(struct dma_buf *dma_buf,
+					unsigned long page_num, void *addr)
+{
+	/* TODO */
+}
+
+static struct dma_buf_ops exynos_dmabuf_ops = {
+	.map_dma_buf		= exynos_gem_map_dma_buf,
+	.unmap_dma_buf		= exynos_gem_unmap_dma_buf,
+	.kmap			= exynos_gem_dmabuf_kmap,
+	.kmap_atomic		= exynos_gem_dmabuf_kmap_atomic,
+	.kunmap			= exynos_gem_dmabuf_kunmap,
+	.kunmap_atomic		= exynos_gem_dmabuf_kunmap_atomic,
+	.release		= exynos_dmabuf_release,
+};
+
+struct dma_buf *exynos_dmabuf_prime_export(struct drm_device *drm_dev,
+				struct drm_gem_object *obj, int flags)
+{
+	struct exynos_drm_gem_obj *exynos_gem_obj = to_exynos_gem_obj(obj);
+
+	return dma_buf_export(exynos_gem_obj, &exynos_dmabuf_ops,
+				exynos_gem_obj->base.size, 0600);
+}
+
+struct drm_gem_object *exynos_dmabuf_prime_import(struct drm_device *drm_dev,
+				struct dma_buf *dma_buf)
+{
+	struct dma_buf_attachment *attach;
+	struct sg_table *sgt;
+	struct scatterlist *sgl;
+	struct exynos_drm_gem_obj *exynos_gem_obj;
+	struct exynos_drm_gem_buf *buffer;
+	struct page *page;
+	int ret, i = 0;
+
+	DRM_DEBUG_PRIME("%s\n", __FILE__);
+
+	/* is this one of own objects? */
+	if (dma_buf->ops == &exynos_dmabuf_ops) {
+		struct drm_gem_object *obj;
+
+		exynos_gem_obj = dma_buf->priv;
+		obj = &exynos_gem_obj->base;
+
+		/* is it from our device? */
+		if (obj->dev == drm_dev) {
+			drm_gem_object_reference(obj);
+			return obj;
+		}
+	}
+
+	attach = dma_buf_attach(dma_buf, drm_dev->dev);
+	if (IS_ERR(attach))
+		return ERR_PTR(-EINVAL);
+
+
+	sgt = dma_buf_map_attachment(attach, DMA_BIDIRECTIONAL);
+	if (IS_ERR(sgt)) {
+		ret = PTR_ERR(sgt);
+		goto err_buf_detach;
+	}
+
+	buffer = kzalloc(sizeof(*buffer), GFP_KERNEL);
+	if (!buffer) {
+		DRM_ERROR("failed to allocate exynos_drm_gem_buf.\n");
+		ret = -ENOMEM;
+		goto err_unmap_attach;
+	}
+
+	buffer->pages = kzalloc(sizeof(*page) * sgt->nents, GFP_KERNEL);
+	if (!buffer->pages) {
+		DRM_ERROR("failed to allocate pages.\n");
+		ret = -ENOMEM;
+		goto err_free_buffer;
+	}
+
+	exynos_gem_obj = exynos_drm_gem_init(drm_dev, dma_buf->size);
+	if (!exynos_gem_obj) {
+		ret = -ENOMEM;
+		goto err_free_pages;
+	}
+
+	sgl = sgt->sgl;
+	buffer->dma_addr = sg_dma_address(sgl);
+
+	while (i < sgt->nents) {
+		buffer->pages[i] = sg_page(sgl);
+		buffer->size += sg_dma_len(sgl);
+		sgl = sg_next(sgl);
+		i++;
+	}
+
+	exynos_gem_obj->buffer = buffer;
+	buffer->sgt = sgt;
+	exynos_gem_obj->base.import_attach = attach;
+
+	DRM_DEBUG_PRIME("dma_addr = 0x%x, size = 0x%lx\n", buffer->dma_addr,
+								buffer->size);
+
+	return &exynos_gem_obj->base;
+
+err_free_pages:
+	kfree(buffer->pages);
+	buffer->pages = NULL;
+err_free_buffer:
+	kfree(buffer);
+	buffer = NULL;
+err_unmap_attach:
+	dma_buf_unmap_attachment(attach, sgt, DMA_BIDIRECTIONAL);
+err_buf_detach:
+	dma_buf_detach(dma_buf, attach);
+	return ERR_PTR(ret);
+}
+
+MODULE_AUTHOR("Inki Dae <inki.dae@samsung.com>");
+MODULE_DESCRIPTION("Samsung SoC DRM DMABUF Module");
+MODULE_LICENSE("GPL");
diff --git a/drivers/gpu/drm/exynos/exynos_drm_dmabuf.h b/drivers/gpu/drm/exynos/exynos_drm_dmabuf.h
new file mode 100644
index 00000000000..662a8f98ccd
--- /dev/null
+++ b/drivers/gpu/drm/exynos/exynos_drm_dmabuf.h
@@ -0,0 +1,39 @@
+/* exynos_drm_dmabuf.h
+ *
+ * Copyright (c) 2012 Samsung Electronics Co., Ltd.
+ * Author: Inki Dae <inki.dae@samsung.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
+ * 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.
+ */
+
+#ifndef _EXYNOS_DRM_DMABUF_H_
+#define _EXYNOS_DRM_DMABUF_H_
+
+#ifdef CONFIG_DRM_EXYNOS_DMABUF
+struct dma_buf *exynos_dmabuf_prime_export(struct drm_device *drm_dev,
+				struct drm_gem_object *obj, int flags);
+
+struct drm_gem_object *exynos_dmabuf_prime_import(struct drm_device *drm_dev,
+						struct dma_buf *dma_buf);
+#else
+#define exynos_dmabuf_prime_export		NULL
+#define exynos_dmabuf_prime_import		NULL
+#endif
+#endif
diff --git a/drivers/gpu/drm/exynos/exynos_drm_drv.c b/drivers/gpu/drm/exynos/exynos_drm_drv.c
index a6819b5f842..420953197d0 100644
--- a/drivers/gpu/drm/exynos/exynos_drm_drv.c
+++ b/drivers/gpu/drm/exynos/exynos_drm_drv.c
@@ -39,6 +39,8 @@
 #include "exynos_drm_gem.h"
 #include "exynos_drm_plane.h"
 #include "exynos_drm_vidi.h"
+#include "exynos_drm_dmabuf.h"
+#include "exynos_drm_g2d.h"
 
 #define DRIVER_NAME	"exynos"
 #define DRIVER_DESC	"Samsung SoC DRM"
@@ -147,8 +149,17 @@ static int exynos_drm_unload(struct drm_device *dev)
 
 static int exynos_drm_open(struct drm_device *dev, struct drm_file *file)
 {
+	struct drm_exynos_file_private *file_priv;
+
 	DRM_DEBUG_DRIVER("%s\n", __FILE__);
 
+	file_priv = kzalloc(sizeof(*file_priv), GFP_KERNEL);
+	if (!file_priv)
+		return -ENOMEM;
+
+	drm_prime_init_file_private(&file->prime);
+	file->driver_priv = file_priv;
+
 	return exynos_drm_subdrv_open(dev, file);
 }
 
@@ -170,6 +181,7 @@ static void exynos_drm_preclose(struct drm_device *dev,
 			e->base.destroy(&e->base);
 		}
 	}
+	drm_prime_destroy_file_private(&file->prime);
 	spin_unlock_irqrestore(&dev->event_lock, flags);
 
 	exynos_drm_subdrv_close(dev, file);
@@ -193,7 +205,7 @@ static void exynos_drm_lastclose(struct drm_device *dev)
 	exynos_drm_fbdev_restore_mode(dev);
 }
 
-static struct vm_operations_struct exynos_drm_gem_vm_ops = {
+static const struct vm_operations_struct exynos_drm_gem_vm_ops = {
 	.fault = exynos_drm_gem_fault,
 	.open = drm_gem_vm_open,
 	.close = drm_gem_vm_close,
@@ -207,10 +219,18 @@ static struct drm_ioctl_desc exynos_ioctls[] = {
 			DRM_AUTH),
 	DRM_IOCTL_DEF_DRV(EXYNOS_GEM_MMAP,
 			exynos_drm_gem_mmap_ioctl, DRM_UNLOCKED | DRM_AUTH),
+	DRM_IOCTL_DEF_DRV(EXYNOS_GEM_GET,
+			exynos_drm_gem_get_ioctl, DRM_UNLOCKED),
 	DRM_IOCTL_DEF_DRV(EXYNOS_PLANE_SET_ZPOS, exynos_plane_set_zpos_ioctl,
 			DRM_UNLOCKED | DRM_AUTH),
 	DRM_IOCTL_DEF_DRV(EXYNOS_VIDI_CONNECTION,
 			vidi_connection_ioctl, DRM_UNLOCKED | DRM_AUTH),
+	DRM_IOCTL_DEF_DRV(EXYNOS_G2D_GET_VER,
+			exynos_g2d_get_ver_ioctl, DRM_UNLOCKED | DRM_AUTH),
+	DRM_IOCTL_DEF_DRV(EXYNOS_G2D_SET_CMDLIST,
+			exynos_g2d_set_cmdlist_ioctl, DRM_UNLOCKED | DRM_AUTH),
+	DRM_IOCTL_DEF_DRV(EXYNOS_G2D_EXEC,
+			exynos_g2d_exec_ioctl, DRM_UNLOCKED | DRM_AUTH),
 };
 
 static const struct file_operations exynos_drm_driver_fops = {
@@ -225,7 +245,7 @@ static const struct file_operations exynos_drm_driver_fops = {
 
 static struct drm_driver exynos_drm_driver = {
 	.driver_features	= DRIVER_HAVE_IRQ | DRIVER_BUS_PLATFORM |
-				  DRIVER_MODESET | DRIVER_GEM,
+				  DRIVER_MODESET | DRIVER_GEM | DRIVER_PRIME,
 	.load			= exynos_drm_load,
 	.unload			= exynos_drm_unload,
 	.open			= exynos_drm_open,
@@ -241,6 +261,10 @@ static struct drm_driver exynos_drm_driver = {
 	.dumb_create		= exynos_drm_gem_dumb_create,
 	.dumb_map_offset	= exynos_drm_gem_dumb_map_offset,
 	.dumb_destroy		= exynos_drm_gem_dumb_destroy,
+	.prime_handle_to_fd	= drm_gem_prime_handle_to_fd,
+	.prime_fd_to_handle	= drm_gem_prime_fd_to_handle,
+	.gem_prime_export	= exynos_dmabuf_prime_export,
+	.gem_prime_import	= exynos_dmabuf_prime_import,
 	.ioctls			= exynos_ioctls,
 	.fops			= &exynos_drm_driver_fops,
 	.name	= DRIVER_NAME,
@@ -307,6 +331,12 @@ static int __init exynos_drm_init(void)
 		goto out_vidi;
 #endif
 
+#ifdef CONFIG_DRM_EXYNOS_G2D
+	ret = platform_driver_register(&g2d_driver);
+	if (ret < 0)
+		goto out_g2d;
+#endif
+
 	ret = platform_driver_register(&exynos_drm_platform_driver);
 	if (ret < 0)
 		goto out;
@@ -314,6 +344,11 @@ static int __init exynos_drm_init(void)
 	return 0;
 
 out:
+#ifdef CONFIG_DRM_EXYNOS_G2D
+	platform_driver_unregister(&g2d_driver);
+out_g2d:
+#endif
+
 #ifdef CONFIG_DRM_EXYNOS_VIDI
 out_vidi:
 	platform_driver_unregister(&vidi_driver);
@@ -341,6 +376,10 @@ static void __exit exynos_drm_exit(void)
 
 	platform_driver_unregister(&exynos_drm_platform_driver);
 
+#ifdef CONFIG_DRM_EXYNOS_G2D
+	platform_driver_unregister(&g2d_driver);
+#endif
+
 #ifdef CONFIG_DRM_EXYNOS_HDMI
 	platform_driver_unregister(&exynos_drm_common_hdmi_driver);
 	platform_driver_unregister(&mixer_driver);
diff --git a/drivers/gpu/drm/exynos/exynos_drm_drv.h b/drivers/gpu/drm/exynos/exynos_drm_drv.h
index 1d814175cd4..c82c90c443e 100644
--- a/drivers/gpu/drm/exynos/exynos_drm_drv.h
+++ b/drivers/gpu/drm/exynos/exynos_drm_drv.h
@@ -77,6 +77,8 @@ struct exynos_drm_overlay_ops {
  *	- the unit is screen coordinates.
  * @fb_width: width of a framebuffer.
  * @fb_height: height of a framebuffer.
+ * @src_width: width of a partial image to be displayed from framebuffer.
+ * @src_height: height of a partial image to be displayed from framebuffer.
  * @crtc_x: offset x on hardware screen.
  * @crtc_y: offset y on hardware screen.
  * @crtc_width: window width to be displayed (hardware screen).
@@ -108,6 +110,8 @@ struct exynos_drm_overlay {
 	unsigned int fb_y;
 	unsigned int fb_width;
 	unsigned int fb_height;
+	unsigned int src_width;
+	unsigned int src_height;
 	unsigned int crtc_x;
 	unsigned int crtc_y;
 	unsigned int crtc_width;
@@ -205,6 +209,18 @@ struct exynos_drm_manager {
 	struct exynos_drm_display_ops *display_ops;
 };
 
+struct exynos_drm_g2d_private {
+	struct device		*dev;
+	struct list_head	inuse_cmdlist;
+	struct list_head	event_list;
+	struct list_head	gem_list;
+	unsigned int		gem_nr;
+};
+
+struct drm_exynos_file_private {
+	struct exynos_drm_g2d_private	*g2d_priv;
+};
+
 /*
  * Exynos drm private structure.
  */
@@ -287,4 +303,5 @@ extern struct platform_driver hdmi_driver;
 extern struct platform_driver mixer_driver;
 extern struct platform_driver exynos_drm_common_hdmi_driver;
 extern struct platform_driver vidi_driver;
+extern struct platform_driver g2d_driver;
 #endif
diff --git a/drivers/gpu/drm/exynos/exynos_drm_fb.c b/drivers/gpu/drm/exynos/exynos_drm_fb.c
index c38c8f468fa..f82a299553f 100644
--- a/drivers/gpu/drm/exynos/exynos_drm_fb.c
+++ b/drivers/gpu/drm/exynos/exynos_drm_fb.c
@@ -191,7 +191,7 @@ static void exynos_drm_output_poll_changed(struct drm_device *dev)
 		drm_fb_helper_hotplug_event(fb_helper);
 }
 
-static struct drm_mode_config_funcs exynos_drm_mode_config_funcs = {
+static const struct drm_mode_config_funcs exynos_drm_mode_config_funcs = {
 	.fb_create = exynos_user_fb_create,
 	.output_poll_changed = exynos_drm_output_poll_changed,
 };
diff --git a/drivers/gpu/drm/exynos/exynos_drm_g2d.c b/drivers/gpu/drm/exynos/exynos_drm_g2d.c
new file mode 100644
index 00000000000..d2d88f22a03
--- /dev/null
+++ b/drivers/gpu/drm/exynos/exynos_drm_g2d.c
@@ -0,0 +1,937 @@
+/*
+ * Copyright (C) 2012 Samsung Electronics Co.Ltd
+ * Authors: Joonyoung Shim <jy0922.shim@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 Foundationr
+ */
+
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/clk.h>
+#include <linux/err.h>
+#include <linux/interrupt.h>
+#include <linux/io.h>
+#include <linux/platform_device.h>
+#include <linux/pm_runtime.h>
+#include <linux/slab.h>
+#include <linux/workqueue.h>
+
+#include "drmP.h"
+#include "exynos_drm.h"
+#include "exynos_drm_drv.h"
+#include "exynos_drm_gem.h"
+
+#define G2D_HW_MAJOR_VER		4
+#define G2D_HW_MINOR_VER		1
+
+/* vaild register range set from user: 0x0104 ~ 0x0880 */
+#define G2D_VALID_START			0x0104
+#define G2D_VALID_END			0x0880
+
+/* general registers */
+#define G2D_SOFT_RESET			0x0000
+#define G2D_INTEN			0x0004
+#define G2D_INTC_PEND			0x000C
+#define G2D_DMA_SFR_BASE_ADDR		0x0080
+#define G2D_DMA_COMMAND			0x0084
+#define G2D_DMA_STATUS			0x008C
+#define G2D_DMA_HOLD_CMD		0x0090
+
+/* command registers */
+#define G2D_BITBLT_START		0x0100
+
+/* registers for base address */
+#define G2D_SRC_BASE_ADDR		0x0304
+#define G2D_SRC_PLANE2_BASE_ADDR	0x0318
+#define G2D_DST_BASE_ADDR		0x0404
+#define G2D_DST_PLANE2_BASE_ADDR	0x0418
+#define G2D_PAT_BASE_ADDR		0x0500
+#define G2D_MSK_BASE_ADDR		0x0520
+
+/* G2D_SOFT_RESET */
+#define G2D_SFRCLEAR			(1 << 1)
+#define G2D_R				(1 << 0)
+
+/* G2D_INTEN */
+#define G2D_INTEN_ACF			(1 << 3)
+#define G2D_INTEN_UCF			(1 << 2)
+#define G2D_INTEN_GCF			(1 << 1)
+#define G2D_INTEN_SCF			(1 << 0)
+
+/* G2D_INTC_PEND */
+#define G2D_INTP_ACMD_FIN		(1 << 3)
+#define G2D_INTP_UCMD_FIN		(1 << 2)
+#define G2D_INTP_GCMD_FIN		(1 << 1)
+#define G2D_INTP_SCMD_FIN		(1 << 0)
+
+/* G2D_DMA_COMMAND */
+#define G2D_DMA_HALT			(1 << 2)
+#define G2D_DMA_CONTINUE		(1 << 1)
+#define G2D_DMA_START			(1 << 0)
+
+/* G2D_DMA_STATUS */
+#define G2D_DMA_LIST_DONE_COUNT		(0xFF << 17)
+#define G2D_DMA_BITBLT_DONE_COUNT	(0xFFFF << 1)
+#define G2D_DMA_DONE			(1 << 0)
+#define G2D_DMA_LIST_DONE_COUNT_OFFSET	17
+
+/* G2D_DMA_HOLD_CMD */
+#define G2D_USET_HOLD			(1 << 2)
+#define G2D_LIST_HOLD			(1 << 1)
+#define G2D_BITBLT_HOLD			(1 << 0)
+
+/* G2D_BITBLT_START */
+#define G2D_START_CASESEL		(1 << 2)
+#define G2D_START_NHOLT			(1 << 1)
+#define G2D_START_BITBLT		(1 << 0)
+
+#define G2D_CMDLIST_SIZE		(PAGE_SIZE / 4)
+#define G2D_CMDLIST_NUM			64
+#define G2D_CMDLIST_POOL_SIZE		(G2D_CMDLIST_SIZE * G2D_CMDLIST_NUM)
+#define G2D_CMDLIST_DATA_NUM		(G2D_CMDLIST_SIZE / sizeof(u32) - 2)
+
+/* cmdlist data structure */
+struct g2d_cmdlist {
+	u32	head;
+	u32	data[G2D_CMDLIST_DATA_NUM];
+	u32	last;	/* last data offset */
+};
+
+struct drm_exynos_pending_g2d_event {
+	struct drm_pending_event	base;
+	struct drm_exynos_g2d_event	event;
+};
+
+struct g2d_gem_node {
+	struct list_head	list;
+	unsigned int		handle;
+};
+
+struct g2d_cmdlist_node {
+	struct list_head	list;
+	struct g2d_cmdlist	*cmdlist;
+	unsigned int		gem_nr;
+	dma_addr_t		dma_addr;
+
+	struct drm_exynos_pending_g2d_event	*event;
+};
+
+struct g2d_runqueue_node {
+	struct list_head	list;
+	struct list_head	run_cmdlist;
+	struct list_head	event_list;
+	struct completion	complete;
+	int			async;
+};
+
+struct g2d_data {
+	struct device			*dev;
+	struct clk			*gate_clk;
+	struct resource			*regs_res;
+	void __iomem			*regs;
+	int				irq;
+	struct workqueue_struct		*g2d_workq;
+	struct work_struct		runqueue_work;
+	struct exynos_drm_subdrv	subdrv;
+	bool				suspended;
+
+	/* cmdlist */
+	struct g2d_cmdlist_node		*cmdlist_node;
+	struct list_head		free_cmdlist;
+	struct mutex			cmdlist_mutex;
+	dma_addr_t			cmdlist_pool;
+	void				*cmdlist_pool_virt;
+
+	/* runqueue*/
+	struct g2d_runqueue_node	*runqueue_node;
+	struct list_head		runqueue;
+	struct mutex			runqueue_mutex;
+	struct kmem_cache		*runqueue_slab;
+};
+
+static int g2d_init_cmdlist(struct g2d_data *g2d)
+{
+	struct device *dev = g2d->dev;
+	struct g2d_cmdlist_node *node = g2d->cmdlist_node;
+	int nr;
+	int ret;
+
+	g2d->cmdlist_pool_virt = dma_alloc_coherent(dev, G2D_CMDLIST_POOL_SIZE,
+						&g2d->cmdlist_pool, GFP_KERNEL);
+	if (!g2d->cmdlist_pool_virt) {
+		dev_err(dev, "failed to allocate dma memory\n");
+		return -ENOMEM;
+	}
+
+	node = kcalloc(G2D_CMDLIST_NUM, G2D_CMDLIST_NUM * sizeof(*node),
+			GFP_KERNEL);
+	if (!node) {
+		dev_err(dev, "failed to allocate memory\n");
+		ret = -ENOMEM;
+		goto err;
+	}
+
+	for (nr = 0; nr < G2D_CMDLIST_NUM; nr++) {
+		node[nr].cmdlist =
+			g2d->cmdlist_pool_virt + nr * G2D_CMDLIST_SIZE;
+		node[nr].dma_addr =
+			g2d->cmdlist_pool + nr * G2D_CMDLIST_SIZE;
+
+		list_add_tail(&node[nr].list, &g2d->free_cmdlist);
+	}
+
+	return 0;
+
+err:
+	dma_free_coherent(dev, G2D_CMDLIST_POOL_SIZE, g2d->cmdlist_pool_virt,
+			g2d->cmdlist_pool);
+	return ret;
+}
+
+static void g2d_fini_cmdlist(struct g2d_data *g2d)
+{
+	struct device *dev = g2d->dev;
+
+	kfree(g2d->cmdlist_node);
+	dma_free_coherent(dev, G2D_CMDLIST_POOL_SIZE, g2d->cmdlist_pool_virt,
+			g2d->cmdlist_pool);
+}
+
+static struct g2d_cmdlist_node *g2d_get_cmdlist(struct g2d_data *g2d)
+{
+	struct device *dev = g2d->dev;
+	struct g2d_cmdlist_node *node;
+
+	mutex_lock(&g2d->cmdlist_mutex);
+	if (list_empty(&g2d->free_cmdlist)) {
+		dev_err(dev, "there is no free cmdlist\n");
+		mutex_unlock(&g2d->cmdlist_mutex);
+		return NULL;
+	}
+
+	node = list_first_entry(&g2d->free_cmdlist, struct g2d_cmdlist_node,
+				list);
+	list_del_init(&node->list);
+	mutex_unlock(&g2d->cmdlist_mutex);
+
+	return node;
+}
+
+static void g2d_put_cmdlist(struct g2d_data *g2d, struct g2d_cmdlist_node *node)
+{
+	mutex_lock(&g2d->cmdlist_mutex);
+	list_move_tail(&node->list, &g2d->free_cmdlist);
+	mutex_unlock(&g2d->cmdlist_mutex);
+}
+
+static void g2d_add_cmdlist_to_inuse(struct exynos_drm_g2d_private *g2d_priv,
+				     struct g2d_cmdlist_node *node)
+{
+	struct g2d_cmdlist_node *lnode;
+
+	if (list_empty(&g2d_priv->inuse_cmdlist))
+		goto add_to_list;
+
+	/* this links to base address of new cmdlist */
+	lnode = list_entry(g2d_priv->inuse_cmdlist.prev,
+				struct g2d_cmdlist_node, list);
+	lnode->cmdlist->data[lnode->cmdlist->last] = node->dma_addr;
+
+add_to_list:
+	list_add_tail(&node->list, &g2d_priv->inuse_cmdlist);
+
+	if (node->event)
+		list_add_tail(&node->event->base.link, &g2d_priv->event_list);
+}
+
+static int g2d_get_cmdlist_gem(struct drm_device *drm_dev,
+			       struct drm_file *file,
+			       struct g2d_cmdlist_node *node)
+{
+	struct drm_exynos_file_private *file_priv = file->driver_priv;
+	struct exynos_drm_g2d_private *g2d_priv = file_priv->g2d_priv;
+	struct g2d_cmdlist *cmdlist = node->cmdlist;
+	dma_addr_t *addr;
+	int offset;
+	int i;
+
+	for (i = 0; i < node->gem_nr; i++) {
+		struct g2d_gem_node *gem_node;
+
+		gem_node = kzalloc(sizeof(*gem_node), GFP_KERNEL);
+		if (!gem_node) {
+			dev_err(g2d_priv->dev, "failed to allocate gem node\n");
+			return -ENOMEM;
+		}
+
+		offset = cmdlist->last - (i * 2 + 1);
+		gem_node->handle = cmdlist->data[offset];
+
+		addr = exynos_drm_gem_get_dma_addr(drm_dev, gem_node->handle,
+						   file);
+		if (IS_ERR(addr)) {
+			node->gem_nr = i;
+			kfree(gem_node);
+			return PTR_ERR(addr);
+		}
+
+		cmdlist->data[offset] = *addr;
+		list_add_tail(&gem_node->list, &g2d_priv->gem_list);
+		g2d_priv->gem_nr++;
+	}
+
+	return 0;
+}
+
+static void g2d_put_cmdlist_gem(struct drm_device *drm_dev,
+				struct drm_file *file,
+				unsigned int nr)
+{
+	struct drm_exynos_file_private *file_priv = file->driver_priv;
+	struct exynos_drm_g2d_private *g2d_priv = file_priv->g2d_priv;
+	struct g2d_gem_node *node, *n;
+
+	list_for_each_entry_safe_reverse(node, n, &g2d_priv->gem_list, list) {
+		if (!nr)
+			break;
+
+		exynos_drm_gem_put_dma_addr(drm_dev, node->handle, file);
+		list_del_init(&node->list);
+		kfree(node);
+		nr--;
+	}
+}
+
+static void g2d_dma_start(struct g2d_data *g2d,
+			  struct g2d_runqueue_node *runqueue_node)
+{
+	struct g2d_cmdlist_node *node =
+				list_first_entry(&runqueue_node->run_cmdlist,
+						struct g2d_cmdlist_node, list);
+
+	pm_runtime_get_sync(g2d->dev);
+	clk_enable(g2d->gate_clk);
+
+	/* interrupt enable */
+	writel_relaxed(G2D_INTEN_ACF | G2D_INTEN_UCF | G2D_INTEN_GCF,
+			g2d->regs + G2D_INTEN);
+
+	writel_relaxed(node->dma_addr, g2d->regs + G2D_DMA_SFR_BASE_ADDR);
+	writel_relaxed(G2D_DMA_START, g2d->regs + G2D_DMA_COMMAND);
+}
+
+static struct g2d_runqueue_node *g2d_get_runqueue_node(struct g2d_data *g2d)
+{
+	struct g2d_runqueue_node *runqueue_node;
+
+	if (list_empty(&g2d->runqueue))
+		return NULL;
+
+	runqueue_node = list_first_entry(&g2d->runqueue,
+					 struct g2d_runqueue_node, list);
+	list_del_init(&runqueue_node->list);
+	return runqueue_node;
+}
+
+static void g2d_free_runqueue_node(struct g2d_data *g2d,
+				   struct g2d_runqueue_node *runqueue_node)
+{
+	if (!runqueue_node)
+		return;
+
+	mutex_lock(&g2d->cmdlist_mutex);
+	list_splice_tail_init(&runqueue_node->run_cmdlist, &g2d->free_cmdlist);
+	mutex_unlock(&g2d->cmdlist_mutex);
+
+	kmem_cache_free(g2d->runqueue_slab, runqueue_node);
+}
+
+static void g2d_exec_runqueue(struct g2d_data *g2d)
+{
+	g2d->runqueue_node = g2d_get_runqueue_node(g2d);
+	if (g2d->runqueue_node)
+		g2d_dma_start(g2d, g2d->runqueue_node);
+}
+
+static void g2d_runqueue_worker(struct work_struct *work)
+{
+	struct g2d_data *g2d = container_of(work, struct g2d_data,
+					    runqueue_work);
+
+
+	mutex_lock(&g2d->runqueue_mutex);
+	clk_disable(g2d->gate_clk);
+	pm_runtime_put_sync(g2d->dev);
+
+	complete(&g2d->runqueue_node->complete);
+	if (g2d->runqueue_node->async)
+		g2d_free_runqueue_node(g2d, g2d->runqueue_node);
+
+	if (g2d->suspended)
+		g2d->runqueue_node = NULL;
+	else
+		g2d_exec_runqueue(g2d);
+	mutex_unlock(&g2d->runqueue_mutex);
+}
+
+static void g2d_finish_event(struct g2d_data *g2d, u32 cmdlist_no)
+{
+	struct drm_device *drm_dev = g2d->subdrv.drm_dev;
+	struct g2d_runqueue_node *runqueue_node = g2d->runqueue_node;
+	struct drm_exynos_pending_g2d_event *e;
+	struct timeval now;
+	unsigned long flags;
+
+	if (list_empty(&runqueue_node->event_list))
+		return;
+
+	e = list_first_entry(&runqueue_node->event_list,
+			     struct drm_exynos_pending_g2d_event, base.link);
+
+	do_gettimeofday(&now);
+	e->event.tv_sec = now.tv_sec;
+	e->event.tv_usec = now.tv_usec;
+	e->event.cmdlist_no = cmdlist_no;
+
+	spin_lock_irqsave(&drm_dev->event_lock, flags);
+	list_move_tail(&e->base.link, &e->base.file_priv->event_list);
+	wake_up_interruptible(&e->base.file_priv->event_wait);
+	spin_unlock_irqrestore(&drm_dev->event_lock, flags);
+}
+
+static irqreturn_t g2d_irq_handler(int irq, void *dev_id)
+{
+	struct g2d_data *g2d = dev_id;
+	u32 pending;
+
+	pending = readl_relaxed(g2d->regs + G2D_INTC_PEND);
+	if (pending)
+		writel_relaxed(pending, g2d->regs + G2D_INTC_PEND);
+
+	if (pending & G2D_INTP_GCMD_FIN) {
+		u32 cmdlist_no = readl_relaxed(g2d->regs + G2D_DMA_STATUS);
+
+		cmdlist_no = (cmdlist_no & G2D_DMA_LIST_DONE_COUNT) >>
+						G2D_DMA_LIST_DONE_COUNT_OFFSET;
+
+		g2d_finish_event(g2d, cmdlist_no);
+
+		writel_relaxed(0, g2d->regs + G2D_DMA_HOLD_CMD);
+		if (!(pending & G2D_INTP_ACMD_FIN)) {
+			writel_relaxed(G2D_DMA_CONTINUE,
+					g2d->regs + G2D_DMA_COMMAND);
+		}
+	}
+
+	if (pending & G2D_INTP_ACMD_FIN)
+		queue_work(g2d->g2d_workq, &g2d->runqueue_work);
+
+	return IRQ_HANDLED;
+}
+
+static int g2d_check_reg_offset(struct device *dev, struct g2d_cmdlist *cmdlist,
+				int nr, bool for_addr)
+{
+	int reg_offset;
+	int index;
+	int i;
+
+	for (i = 0; i < nr; i++) {
+		index = cmdlist->last - 2 * (i + 1);
+		reg_offset = cmdlist->data[index] & ~0xfffff000;
+
+		if (reg_offset < G2D_VALID_START || reg_offset > G2D_VALID_END)
+			goto err;
+		if (reg_offset % 4)
+			goto err;
+
+		switch (reg_offset) {
+		case G2D_SRC_BASE_ADDR:
+		case G2D_SRC_PLANE2_BASE_ADDR:
+		case G2D_DST_BASE_ADDR:
+		case G2D_DST_PLANE2_BASE_ADDR:
+		case G2D_PAT_BASE_ADDR:
+		case G2D_MSK_BASE_ADDR:
+			if (!for_addr)
+				goto err;
+			break;
+		default:
+			if (for_addr)
+				goto err;
+			break;
+		}
+	}
+
+	return 0;
+
+err:
+	dev_err(dev, "Bad register offset: 0x%x\n", cmdlist->data[index]);
+	return -EINVAL;
+}
+
+/* ioctl functions */
+int exynos_g2d_get_ver_ioctl(struct drm_device *drm_dev, void *data,
+			     struct drm_file *file)
+{
+	struct drm_exynos_g2d_get_ver *ver = data;
+
+	ver->major = G2D_HW_MAJOR_VER;
+	ver->minor = G2D_HW_MINOR_VER;
+
+	return 0;
+}
+EXPORT_SYMBOL_GPL(exynos_g2d_get_ver_ioctl);
+
+int exynos_g2d_set_cmdlist_ioctl(struct drm_device *drm_dev, void *data,
+				 struct drm_file *file)
+{
+	struct drm_exynos_file_private *file_priv = file->driver_priv;
+	struct exynos_drm_g2d_private *g2d_priv = file_priv->g2d_priv;
+	struct device *dev = g2d_priv->dev;
+	struct g2d_data *g2d;
+	struct drm_exynos_g2d_set_cmdlist *req = data;
+	struct drm_exynos_g2d_cmd *cmd;
+	struct drm_exynos_pending_g2d_event *e;
+	struct g2d_cmdlist_node *node;
+	struct g2d_cmdlist *cmdlist;
+	unsigned long flags;
+	int size;
+	int ret;
+
+	if (!dev)
+		return -ENODEV;
+
+	g2d = dev_get_drvdata(dev);
+	if (!g2d)
+		return -EFAULT;
+
+	node = g2d_get_cmdlist(g2d);
+	if (!node)
+		return -ENOMEM;
+
+	node->event = NULL;
+
+	if (req->event_type != G2D_EVENT_NOT) {
+		spin_lock_irqsave(&drm_dev->event_lock, flags);
+		if (file->event_space < sizeof(e->event)) {
+			spin_unlock_irqrestore(&drm_dev->event_lock, flags);
+			ret = -ENOMEM;
+			goto err;
+		}
+		file->event_space -= sizeof(e->event);
+		spin_unlock_irqrestore(&drm_dev->event_lock, flags);
+
+		e = kzalloc(sizeof(*node->event), GFP_KERNEL);
+		if (!e) {
+			dev_err(dev, "failed to allocate event\n");
+
+			spin_lock_irqsave(&drm_dev->event_lock, flags);
+			file->event_space += sizeof(e->event);
+			spin_unlock_irqrestore(&drm_dev->event_lock, flags);
+
+			ret = -ENOMEM;
+			goto err;
+		}
+
+		e->event.base.type = DRM_EXYNOS_G2D_EVENT;
+		e->event.base.length = sizeof(e->event);
+		e->event.user_data = req->user_data;
+		e->base.event = &e->event.base;
+		e->base.file_priv = file;
+		e->base.destroy = (void (*) (struct drm_pending_event *)) kfree;
+
+		node->event = e;
+	}
+
+	cmdlist = node->cmdlist;
+
+	cmdlist->last = 0;
+
+	/*
+	 * If don't clear SFR registers, the cmdlist is affected by register
+	 * values of previous cmdlist. G2D hw executes SFR clear command and
+	 * a next command at the same time then the next command is ignored and
+	 * is executed rightly from next next command, so needs a dummy command
+	 * to next command of SFR clear command.
+	 */
+	cmdlist->data[cmdlist->last++] = G2D_SOFT_RESET;
+	cmdlist->data[cmdlist->last++] = G2D_SFRCLEAR;
+	cmdlist->data[cmdlist->last++] = G2D_SRC_BASE_ADDR;
+	cmdlist->data[cmdlist->last++] = 0;
+
+	if (node->event) {
+		cmdlist->data[cmdlist->last++] = G2D_DMA_HOLD_CMD;
+		cmdlist->data[cmdlist->last++] = G2D_LIST_HOLD;
+	}
+
+	/* Check size of cmdlist: last 2 is about G2D_BITBLT_START */
+	size = cmdlist->last + req->cmd_nr * 2 + req->cmd_gem_nr * 2 + 2;
+	if (size > G2D_CMDLIST_DATA_NUM) {
+		dev_err(dev, "cmdlist size is too big\n");
+		ret = -EINVAL;
+		goto err_free_event;
+	}
+
+	cmd = (struct drm_exynos_g2d_cmd *)(uint32_t)req->cmd;
+
+	if (copy_from_user(cmdlist->data + cmdlist->last,
+				(void __user *)cmd,
+				sizeof(*cmd) * req->cmd_nr)) {
+		ret = -EFAULT;
+		goto err_free_event;
+	}
+	cmdlist->last += req->cmd_nr * 2;
+
+	ret = g2d_check_reg_offset(dev, cmdlist, req->cmd_nr, false);
+	if (ret < 0)
+		goto err_free_event;
+
+	node->gem_nr = req->cmd_gem_nr;
+	if (req->cmd_gem_nr) {
+		struct drm_exynos_g2d_cmd *cmd_gem;
+
+		cmd_gem = (struct drm_exynos_g2d_cmd *)(uint32_t)req->cmd_gem;
+
+		if (copy_from_user(cmdlist->data + cmdlist->last,
+					(void __user *)cmd_gem,
+					sizeof(*cmd_gem) * req->cmd_gem_nr)) {
+			ret = -EFAULT;
+			goto err_free_event;
+		}
+		cmdlist->last += req->cmd_gem_nr * 2;
+
+		ret = g2d_check_reg_offset(dev, cmdlist, req->cmd_gem_nr, true);
+		if (ret < 0)
+			goto err_free_event;
+
+		ret = g2d_get_cmdlist_gem(drm_dev, file, node);
+		if (ret < 0)
+			goto err_unmap;
+	}
+
+	cmdlist->data[cmdlist->last++] = G2D_BITBLT_START;
+	cmdlist->data[cmdlist->last++] = G2D_START_BITBLT;
+
+	/* head */
+	cmdlist->head = cmdlist->last / 2;
+
+	/* tail */
+	cmdlist->data[cmdlist->last] = 0;
+
+	g2d_add_cmdlist_to_inuse(g2d_priv, node);
+
+	return 0;
+
+err_unmap:
+	g2d_put_cmdlist_gem(drm_dev, file, node->gem_nr);
+err_free_event:
+	if (node->event) {
+		spin_lock_irqsave(&drm_dev->event_lock, flags);
+		file->event_space += sizeof(e->event);
+		spin_unlock_irqrestore(&drm_dev->event_lock, flags);
+		kfree(node->event);
+	}
+err:
+	g2d_put_cmdlist(g2d, node);
+	return ret;
+}
+EXPORT_SYMBOL_GPL(exynos_g2d_set_cmdlist_ioctl);
+
+int exynos_g2d_exec_ioctl(struct drm_device *drm_dev, void *data,
+			  struct drm_file *file)
+{
+	struct drm_exynos_file_private *file_priv = file->driver_priv;
+	struct exynos_drm_g2d_private *g2d_priv = file_priv->g2d_priv;
+	struct device *dev = g2d_priv->dev;
+	struct g2d_data *g2d;
+	struct drm_exynos_g2d_exec *req = data;
+	struct g2d_runqueue_node *runqueue_node;
+	struct list_head *run_cmdlist;
+	struct list_head *event_list;
+
+	if (!dev)
+		return -ENODEV;
+
+	g2d = dev_get_drvdata(dev);
+	if (!g2d)
+		return -EFAULT;
+
+	runqueue_node = kmem_cache_alloc(g2d->runqueue_slab, GFP_KERNEL);
+	if (!runqueue_node) {
+		dev_err(dev, "failed to allocate memory\n");
+		return -ENOMEM;
+	}
+	run_cmdlist = &runqueue_node->run_cmdlist;
+	event_list = &runqueue_node->event_list;
+	INIT_LIST_HEAD(run_cmdlist);
+	INIT_LIST_HEAD(event_list);
+	init_completion(&runqueue_node->complete);
+	runqueue_node->async = req->async;
+
+	list_splice_init(&g2d_priv->inuse_cmdlist, run_cmdlist);
+	list_splice_init(&g2d_priv->event_list, event_list);
+
+	if (list_empty(run_cmdlist)) {
+		dev_err(dev, "there is no inuse cmdlist\n");
+		kmem_cache_free(g2d->runqueue_slab, runqueue_node);
+		return -EPERM;
+	}
+
+	mutex_lock(&g2d->runqueue_mutex);
+	list_add_tail(&runqueue_node->list, &g2d->runqueue);
+	if (!g2d->runqueue_node)
+		g2d_exec_runqueue(g2d);
+	mutex_unlock(&g2d->runqueue_mutex);
+
+	if (runqueue_node->async)
+		goto out;
+
+	wait_for_completion(&runqueue_node->complete);
+	g2d_free_runqueue_node(g2d, runqueue_node);
+
+out:
+	return 0;
+}
+EXPORT_SYMBOL_GPL(exynos_g2d_exec_ioctl);
+
+static int g2d_open(struct drm_device *drm_dev, struct device *dev,
+			struct drm_file *file)
+{
+	struct drm_exynos_file_private *file_priv = file->driver_priv;
+	struct exynos_drm_g2d_private *g2d_priv;
+
+	g2d_priv = kzalloc(sizeof(*g2d_priv), GFP_KERNEL);
+	if (!g2d_priv) {
+		dev_err(dev, "failed to allocate g2d private data\n");
+		return -ENOMEM;
+	}
+
+	g2d_priv->dev = dev;
+	file_priv->g2d_priv = g2d_priv;
+
+	INIT_LIST_HEAD(&g2d_priv->inuse_cmdlist);
+	INIT_LIST_HEAD(&g2d_priv->event_list);
+	INIT_LIST_HEAD(&g2d_priv->gem_list);
+
+	return 0;
+}
+
+static void g2d_close(struct drm_device *drm_dev, struct device *dev,
+			struct drm_file *file)
+{
+	struct drm_exynos_file_private *file_priv = file->driver_priv;
+	struct exynos_drm_g2d_private *g2d_priv = file_priv->g2d_priv;
+	struct g2d_data *g2d;
+	struct g2d_cmdlist_node *node, *n;
+
+	if (!dev)
+		return;
+
+	g2d = dev_get_drvdata(dev);
+	if (!g2d)
+		return;
+
+	mutex_lock(&g2d->cmdlist_mutex);
+	list_for_each_entry_safe(node, n, &g2d_priv->inuse_cmdlist, list)
+		list_move_tail(&node->list, &g2d->free_cmdlist);
+	mutex_unlock(&g2d->cmdlist_mutex);
+
+	g2d_put_cmdlist_gem(drm_dev, file, g2d_priv->gem_nr);
+
+	kfree(file_priv->g2d_priv);
+}
+
+static int __devinit g2d_probe(struct platform_device *pdev)
+{
+	struct device *dev = &pdev->dev;
+	struct resource *res;
+	struct g2d_data *g2d;
+	struct exynos_drm_subdrv *subdrv;
+	int ret;
+
+	g2d = kzalloc(sizeof(*g2d), GFP_KERNEL);
+	if (!g2d) {
+		dev_err(dev, "failed to allocate driver data\n");
+		return -ENOMEM;
+	}
+
+	g2d->runqueue_slab = kmem_cache_create("g2d_runqueue_slab",
+			sizeof(struct g2d_runqueue_node), 0, 0, NULL);
+	if (!g2d->runqueue_slab) {
+		ret = -ENOMEM;
+		goto err_free_mem;
+	}
+
+	g2d->dev = dev;
+
+	g2d->g2d_workq = create_singlethread_workqueue("g2d");
+	if (!g2d->g2d_workq) {
+		dev_err(dev, "failed to create workqueue\n");
+		ret = -EINVAL;
+		goto err_destroy_slab;
+	}
+
+	INIT_WORK(&g2d->runqueue_work, g2d_runqueue_worker);
+	INIT_LIST_HEAD(&g2d->free_cmdlist);
+	INIT_LIST_HEAD(&g2d->runqueue);
+
+	mutex_init(&g2d->cmdlist_mutex);
+	mutex_init(&g2d->runqueue_mutex);
+
+	ret = g2d_init_cmdlist(g2d);
+	if (ret < 0)
+		goto err_destroy_workqueue;
+
+	g2d->gate_clk = clk_get(dev, "fimg2d");
+	if (IS_ERR(g2d->gate_clk)) {
+		dev_err(dev, "failed to get gate clock\n");
+		ret = PTR_ERR(g2d->gate_clk);
+		goto err_fini_cmdlist;
+	}
+
+	pm_runtime_enable(dev);
+
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	if (!res) {
+		dev_err(dev, "failed to get I/O memory\n");
+		ret = -ENOENT;
+		goto err_put_clk;
+	}
+
+	g2d->regs_res = request_mem_region(res->start, resource_size(res),
+					   dev_name(dev));
+	if (!g2d->regs_res) {
+		dev_err(dev, "failed to request I/O memory\n");
+		ret = -ENOENT;
+		goto err_put_clk;
+	}
+
+	g2d->regs = ioremap(res->start, resource_size(res));
+	if (!g2d->regs) {
+		dev_err(dev, "failed to remap I/O memory\n");
+		ret = -ENXIO;
+		goto err_release_res;
+	}
+
+	g2d->irq = platform_get_irq(pdev, 0);
+	if (g2d->irq < 0) {
+		dev_err(dev, "failed to get irq\n");
+		ret = g2d->irq;
+		goto err_unmap_base;
+	}
+
+	ret = request_irq(g2d->irq, g2d_irq_handler, 0, "drm_g2d", g2d);
+	if (ret < 0) {
+		dev_err(dev, "irq request failed\n");
+		goto err_unmap_base;
+	}
+
+	platform_set_drvdata(pdev, g2d);
+
+	subdrv = &g2d->subdrv;
+	subdrv->dev = dev;
+	subdrv->open = g2d_open;
+	subdrv->close = g2d_close;
+
+	ret = exynos_drm_subdrv_register(subdrv);
+	if (ret < 0) {
+		dev_err(dev, "failed to register drm g2d device\n");
+		goto err_free_irq;
+	}
+
+	dev_info(dev, "The exynos g2d(ver %d.%d) successfully probed\n",
+			G2D_HW_MAJOR_VER, G2D_HW_MINOR_VER);
+
+	return 0;
+
+err_free_irq:
+	free_irq(g2d->irq, g2d);
+err_unmap_base:
+	iounmap(g2d->regs);
+err_release_res:
+	release_resource(g2d->regs_res);
+	kfree(g2d->regs_res);
+err_put_clk:
+	pm_runtime_disable(dev);
+	clk_put(g2d->gate_clk);
+err_fini_cmdlist:
+	g2d_fini_cmdlist(g2d);
+err_destroy_workqueue:
+	destroy_workqueue(g2d->g2d_workq);
+err_destroy_slab:
+	kmem_cache_destroy(g2d->runqueue_slab);
+err_free_mem:
+	kfree(g2d);
+	return ret;
+}
+
+static int __devexit g2d_remove(struct platform_device *pdev)
+{
+	struct g2d_data *g2d = platform_get_drvdata(pdev);
+
+	cancel_work_sync(&g2d->runqueue_work);
+	exynos_drm_subdrv_unregister(&g2d->subdrv);
+	free_irq(g2d->irq, g2d);
+
+	while (g2d->runqueue_node) {
+		g2d_free_runqueue_node(g2d, g2d->runqueue_node);
+		g2d->runqueue_node = g2d_get_runqueue_node(g2d);
+	}
+
+	iounmap(g2d->regs);
+	release_resource(g2d->regs_res);
+	kfree(g2d->regs_res);
+
+	pm_runtime_disable(&pdev->dev);
+	clk_put(g2d->gate_clk);
+
+	g2d_fini_cmdlist(g2d);
+	destroy_workqueue(g2d->g2d_workq);
+	kmem_cache_destroy(g2d->runqueue_slab);
+	kfree(g2d);
+
+	return 0;
+}
+
+#ifdef CONFIG_PM_SLEEP
+static int g2d_suspend(struct device *dev)
+{
+	struct g2d_data *g2d = dev_get_drvdata(dev);
+
+	mutex_lock(&g2d->runqueue_mutex);
+	g2d->suspended = true;
+	mutex_unlock(&g2d->runqueue_mutex);
+
+	while (g2d->runqueue_node)
+		/* FIXME: good range? */
+		usleep_range(500, 1000);
+
+	flush_work_sync(&g2d->runqueue_work);
+
+	return 0;
+}
+
+static int g2d_resume(struct device *dev)
+{
+	struct g2d_data *g2d = dev_get_drvdata(dev);
+
+	g2d->suspended = false;
+	g2d_exec_runqueue(g2d);
+
+	return 0;
+}
+#endif
+
+SIMPLE_DEV_PM_OPS(g2d_pm_ops, g2d_suspend, g2d_resume);
+
+struct platform_driver g2d_driver = {
+	.probe		= g2d_probe,
+	.remove		= __devexit_p(g2d_remove),
+	.driver		= {
+		.name	= "s5p-g2d",
+		.owner	= THIS_MODULE,
+		.pm	= &g2d_pm_ops,
+	},
+};
diff --git a/drivers/gpu/drm/exynos/exynos_drm_g2d.h b/drivers/gpu/drm/exynos/exynos_drm_g2d.h
new file mode 100644
index 00000000000..1a9c7ca8c15
--- /dev/null
+++ b/drivers/gpu/drm/exynos/exynos_drm_g2d.h
@@ -0,0 +1,36 @@
+/*
+ * Copyright (C) 2012 Samsung Electronics Co.Ltd
+ * Authors: Joonyoung Shim <jy0922.shim@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 Foundationr
+ */
+
+#ifdef CONFIG_DRM_EXYNOS_G2D
+extern int exynos_g2d_get_ver_ioctl(struct drm_device *dev, void *data,
+				    struct drm_file *file_priv);
+extern int exynos_g2d_set_cmdlist_ioctl(struct drm_device *dev, void *data,
+					struct drm_file *file_priv);
+extern int exynos_g2d_exec_ioctl(struct drm_device *dev, void *data,
+				 struct drm_file *file_priv);
+#else
+static inline int exynos_g2d_get_ver_ioctl(struct drm_device *dev, void *data,
+					   struct drm_file *file_priv)
+{
+	return -ENODEV;
+}
+
+static inline int exynos_g2d_set_cmdlist_ioctl(struct drm_device *dev,
+					       void *data,
+					       struct drm_file *file_priv)
+{
+	return -ENODEV;
+}
+
+static inline int exynos_g2d_exec_ioctl(struct drm_device *dev, void *data,
+					struct drm_file *file_priv)
+{
+	return -ENODEV;
+}
+#endif
diff --git a/drivers/gpu/drm/exynos/exynos_drm_gem.c b/drivers/gpu/drm/exynos/exynos_drm_gem.c
index 1dffa8359f8..fc91293c456 100644
--- a/drivers/gpu/drm/exynos/exynos_drm_gem.c
+++ b/drivers/gpu/drm/exynos/exynos_drm_gem.c
@@ -66,6 +66,22 @@ static int check_gem_flags(unsigned int flags)
 	return 0;
 }
 
+static void update_vm_cache_attr(struct exynos_drm_gem_obj *obj,
+					struct vm_area_struct *vma)
+{
+	DRM_DEBUG_KMS("flags = 0x%x\n", obj->flags);
+
+	/* non-cachable as default. */
+	if (obj->flags & EXYNOS_BO_CACHABLE)
+		vma->vm_page_prot = vm_get_page_prot(vma->vm_flags);
+	else if (obj->flags & EXYNOS_BO_WC)
+		vma->vm_page_prot =
+			pgprot_writecombine(vm_get_page_prot(vma->vm_flags));
+	else
+		vma->vm_page_prot =
+			pgprot_noncached(vm_get_page_prot(vma->vm_flags));
+}
+
 static unsigned long roundup_gem_size(unsigned long size, unsigned int flags)
 {
 	if (!IS_NONCONTIG_BUFFER(flags)) {
@@ -80,7 +96,7 @@ out:
 	return roundup(size, PAGE_SIZE);
 }
 
-static struct page **exynos_gem_get_pages(struct drm_gem_object *obj,
+struct page **exynos_gem_get_pages(struct drm_gem_object *obj,
 						gfp_t gfpmask)
 {
 	struct inode *inode;
@@ -180,6 +196,7 @@ static int exynos_drm_gem_get_pages(struct drm_gem_object *obj)
 	}
 
 	npages = obj->size >> PAGE_SHIFT;
+	buf->page_size = PAGE_SIZE;
 
 	buf->sgt = kzalloc(sizeof(struct sg_table), GFP_KERNEL);
 	if (!buf->sgt) {
@@ -262,24 +279,24 @@ static int exynos_drm_gem_handle_create(struct drm_gem_object *obj,
 void exynos_drm_gem_destroy(struct exynos_drm_gem_obj *exynos_gem_obj)
 {
 	struct drm_gem_object *obj;
+	struct exynos_drm_gem_buf *buf;
 
 	DRM_DEBUG_KMS("%s\n", __FILE__);
 
-	if (!exynos_gem_obj)
-		return;
-
 	obj = &exynos_gem_obj->base;
+	buf = exynos_gem_obj->buffer;
 
 	DRM_DEBUG_KMS("handle count = %d\n", atomic_read(&obj->handle_count));
 
-	if ((exynos_gem_obj->flags & EXYNOS_BO_NONCONTIG) &&
-			exynos_gem_obj->buffer->pages)
+	if (!buf->pages)
+		return;
+
+	if (exynos_gem_obj->flags & EXYNOS_BO_NONCONTIG)
 		exynos_drm_gem_put_pages(obj);
 	else
-		exynos_drm_free_buf(obj->dev, exynos_gem_obj->flags,
-					exynos_gem_obj->buffer);
+		exynos_drm_free_buf(obj->dev, exynos_gem_obj->flags, buf);
 
-	exynos_drm_fini_buf(obj->dev, exynos_gem_obj->buffer);
+	exynos_drm_fini_buf(obj->dev, buf);
 	exynos_gem_obj->buffer = NULL;
 
 	if (obj->map_list.map)
@@ -292,7 +309,7 @@ void exynos_drm_gem_destroy(struct exynos_drm_gem_obj *exynos_gem_obj)
 	exynos_gem_obj = NULL;
 }
 
-static struct exynos_drm_gem_obj *exynos_drm_gem_init(struct drm_device *dev,
+struct exynos_drm_gem_obj *exynos_drm_gem_init(struct drm_device *dev,
 						      unsigned long size)
 {
 	struct exynos_drm_gem_obj *exynos_gem_obj;
@@ -493,8 +510,7 @@ static int exynos_drm_gem_mmap_buffer(struct file *filp,
 
 	vma->vm_flags |= (VM_IO | VM_RESERVED);
 
-	/* in case of direct mapping, always having non-cachable attribute */
-	vma->vm_page_prot = pgprot_noncached(vma->vm_page_prot);
+	update_vm_cache_attr(exynos_gem_obj, vma);
 
 	vm_size = usize = vma->vm_end - vma->vm_start;
 
@@ -588,6 +604,32 @@ int exynos_drm_gem_mmap_ioctl(struct drm_device *dev, void *data,
 	return 0;
 }
 
+int exynos_drm_gem_get_ioctl(struct drm_device *dev, void *data,
+				      struct drm_file *file_priv)
+{	struct exynos_drm_gem_obj *exynos_gem_obj;
+	struct drm_exynos_gem_info *args = data;
+	struct drm_gem_object *obj;
+
+	mutex_lock(&dev->struct_mutex);
+
+	obj = drm_gem_object_lookup(dev, file_priv, args->handle);
+	if (!obj) {
+		DRM_ERROR("failed to lookup gem object.\n");
+		mutex_unlock(&dev->struct_mutex);
+		return -EINVAL;
+	}
+
+	exynos_gem_obj = to_exynos_gem_obj(obj);
+
+	args->flags = exynos_gem_obj->flags;
+	args->size = exynos_gem_obj->size;
+
+	drm_gem_object_unreference(obj);
+	mutex_unlock(&dev->struct_mutex);
+
+	return 0;
+}
+
 int exynos_drm_gem_init_object(struct drm_gem_object *obj)
 {
 	DRM_DEBUG_KMS("%s\n", __FILE__);
@@ -597,8 +639,17 @@ int exynos_drm_gem_init_object(struct drm_gem_object *obj)
 
 void exynos_drm_gem_free_object(struct drm_gem_object *obj)
 {
+	struct exynos_drm_gem_obj *exynos_gem_obj;
+	struct exynos_drm_gem_buf *buf;
+
 	DRM_DEBUG_KMS("%s\n", __FILE__);
 
+	exynos_gem_obj = to_exynos_gem_obj(obj);
+	buf = exynos_gem_obj->buffer;
+
+	if (obj->import_attach)
+		drm_prime_gem_destroy(obj, buf->sgt);
+
 	exynos_drm_gem_destroy(to_exynos_gem_obj(obj));
 }
 
@@ -724,6 +775,8 @@ int exynos_drm_gem_fault(struct vm_area_struct *vma, struct vm_fault *vmf)
 
 int exynos_drm_gem_mmap(struct file *filp, struct vm_area_struct *vma)
 {
+	struct exynos_drm_gem_obj *exynos_gem_obj;
+	struct drm_gem_object *obj;
 	int ret;
 
 	DRM_DEBUG_KMS("%s\n", __FILE__);
@@ -735,8 +788,20 @@ int exynos_drm_gem_mmap(struct file *filp, struct vm_area_struct *vma)
 		return ret;
 	}
 
+	obj = vma->vm_private_data;
+	exynos_gem_obj = to_exynos_gem_obj(obj);
+
+	ret = check_gem_flags(exynos_gem_obj->flags);
+	if (ret) {
+		drm_gem_vm_close(vma);
+		drm_gem_free_mmap_offset(obj);
+		return ret;
+	}
+
 	vma->vm_flags &= ~VM_PFNMAP;
 	vma->vm_flags |= VM_MIXEDMAP;
 
+	update_vm_cache_attr(exynos_gem_obj, vma);
+
 	return ret;
 }
diff --git a/drivers/gpu/drm/exynos/exynos_drm_gem.h b/drivers/gpu/drm/exynos/exynos_drm_gem.h
index 4ed84203950..14d038b6cb0 100644
--- a/drivers/gpu/drm/exynos/exynos_drm_gem.h
+++ b/drivers/gpu/drm/exynos/exynos_drm_gem.h
@@ -40,6 +40,7 @@
  *	device address with IOMMU.
  * @sgt: sg table to transfer page data.
  * @pages: contain all pages to allocated memory region.
+ * @page_size: could be 4K, 64K or 1MB.
  * @size: size of allocated memory region.
  */
 struct exynos_drm_gem_buf {
@@ -47,6 +48,7 @@ struct exynos_drm_gem_buf {
 	dma_addr_t		dma_addr;
 	struct sg_table		*sgt;
 	struct page		**pages;
+	unsigned long		page_size;
 	unsigned long		size;
 };
 
@@ -74,9 +76,15 @@ struct exynos_drm_gem_obj {
 	unsigned int			flags;
 };
 
+struct page **exynos_gem_get_pages(struct drm_gem_object *obj, gfp_t gfpmask);
+
 /* destroy a buffer with gem object */
 void exynos_drm_gem_destroy(struct exynos_drm_gem_obj *exynos_gem_obj);
 
+/* create a private gem object and initialize it. */
+struct exynos_drm_gem_obj *exynos_drm_gem_init(struct drm_device *dev,
+						      unsigned long size);
+
 /* create a new buffer with gem object */
 struct exynos_drm_gem_obj *exynos_drm_gem_create(struct drm_device *dev,
 						unsigned int flags,
@@ -119,6 +127,10 @@ int exynos_drm_gem_map_offset_ioctl(struct drm_device *dev, void *data,
 int exynos_drm_gem_mmap_ioctl(struct drm_device *dev, void *data,
 			      struct drm_file *file_priv);
 
+/* get buffer information to memory region allocated by gem. */
+int exynos_drm_gem_get_ioctl(struct drm_device *dev, void *data,
+				      struct drm_file *file_priv);
+
 /* initialize gem object. */
 int exynos_drm_gem_init_object(struct drm_gem_object *obj);
 
diff --git a/drivers/gpu/drm/exynos/exynos_drm_hdmi.c b/drivers/gpu/drm/exynos/exynos_drm_hdmi.c
index 3424463676e..5d9d2c2f8f3 100644
--- a/drivers/gpu/drm/exynos/exynos_drm_hdmi.c
+++ b/drivers/gpu/drm/exynos/exynos_drm_hdmi.c
@@ -37,6 +37,8 @@ struct drm_hdmi_context {
 	struct exynos_drm_subdrv	subdrv;
 	struct exynos_drm_hdmi_context	*hdmi_ctx;
 	struct exynos_drm_hdmi_context	*mixer_ctx;
+
+	bool	enabled[MIXER_WIN_NR];
 };
 
 void exynos_hdmi_ops_register(struct exynos_hdmi_ops *ops)
@@ -189,23 +191,34 @@ static void drm_hdmi_dpms(struct device *subdrv_dev, int mode)
 
 	DRM_DEBUG_KMS("%s\n", __FILE__);
 
-	switch (mode) {
-	case DRM_MODE_DPMS_ON:
-		break;
-	case DRM_MODE_DPMS_STANDBY:
-	case DRM_MODE_DPMS_SUSPEND:
-	case DRM_MODE_DPMS_OFF:
-		if (hdmi_ops && hdmi_ops->disable)
-			hdmi_ops->disable(ctx->hdmi_ctx->ctx);
-		break;
-	default:
-		DRM_DEBUG_KMS("unkown dps mode: %d\n", mode);
-		break;
+	if (mixer_ops && mixer_ops->dpms)
+		mixer_ops->dpms(ctx->mixer_ctx->ctx, mode);
+
+	if (hdmi_ops && hdmi_ops->dpms)
+		hdmi_ops->dpms(ctx->hdmi_ctx->ctx, mode);
+}
+
+static void drm_hdmi_apply(struct device *subdrv_dev)
+{
+	struct drm_hdmi_context *ctx = to_context(subdrv_dev);
+	int i;
+
+	DRM_DEBUG_KMS("%s\n", __FILE__);
+
+	for (i = 0; i < MIXER_WIN_NR; i++) {
+		if (!ctx->enabled[i])
+			continue;
+		if (mixer_ops && mixer_ops->win_commit)
+			mixer_ops->win_commit(ctx->mixer_ctx->ctx, i);
 	}
+
+	if (hdmi_ops && hdmi_ops->commit)
+		hdmi_ops->commit(ctx->hdmi_ctx->ctx);
 }
 
 static struct exynos_drm_manager_ops drm_hdmi_manager_ops = {
 	.dpms = drm_hdmi_dpms,
+	.apply = drm_hdmi_apply,
 	.enable_vblank = drm_hdmi_enable_vblank,
 	.disable_vblank = drm_hdmi_disable_vblank,
 	.mode_fixup = drm_hdmi_mode_fixup,
@@ -228,21 +241,37 @@ static void drm_mixer_mode_set(struct device *subdrv_dev,
 static void drm_mixer_commit(struct device *subdrv_dev, int zpos)
 {
 	struct drm_hdmi_context *ctx = to_context(subdrv_dev);
+	int win = (zpos == DEFAULT_ZPOS) ? MIXER_DEFAULT_WIN : zpos;
 
 	DRM_DEBUG_KMS("%s\n", __FILE__);
 
+	if (win < 0 || win > MIXER_WIN_NR) {
+		DRM_ERROR("mixer window[%d] is wrong\n", win);
+		return;
+	}
+
 	if (mixer_ops && mixer_ops->win_commit)
-		mixer_ops->win_commit(ctx->mixer_ctx->ctx, zpos);
+		mixer_ops->win_commit(ctx->mixer_ctx->ctx, win);
+
+	ctx->enabled[win] = true;
 }
 
 static void drm_mixer_disable(struct device *subdrv_dev, int zpos)
 {
 	struct drm_hdmi_context *ctx = to_context(subdrv_dev);
+	int win = (zpos == DEFAULT_ZPOS) ? MIXER_DEFAULT_WIN : zpos;
 
 	DRM_DEBUG_KMS("%s\n", __FILE__);
 
+	if (win < 0 || win > MIXER_WIN_NR) {
+		DRM_ERROR("mixer window[%d] is wrong\n", win);
+		return;
+	}
+
 	if (mixer_ops && mixer_ops->win_disable)
-		mixer_ops->win_disable(ctx->mixer_ctx->ctx, zpos);
+		mixer_ops->win_disable(ctx->mixer_ctx->ctx, win);
+
+	ctx->enabled[win] = false;
 }
 
 static struct exynos_drm_overlay_ops drm_hdmi_overlay_ops = {
@@ -335,25 +364,6 @@ static int __devinit exynos_drm_hdmi_probe(struct platform_device *pdev)
 	return 0;
 }
 
-static int hdmi_runtime_suspend(struct device *dev)
-{
-	DRM_DEBUG_KMS("%s\n", __FILE__);
-
-	return 0;
-}
-
-static int hdmi_runtime_resume(struct device *dev)
-{
-	DRM_DEBUG_KMS("%s\n", __FILE__);
-
-	return 0;
-}
-
-static const struct dev_pm_ops hdmi_pm_ops = {
-	.runtime_suspend = hdmi_runtime_suspend,
-	.runtime_resume	 = hdmi_runtime_resume,
-};
-
 static int __devexit exynos_drm_hdmi_remove(struct platform_device *pdev)
 {
 	struct drm_hdmi_context *ctx = platform_get_drvdata(pdev);
@@ -372,6 +382,5 @@ struct platform_driver exynos_drm_common_hdmi_driver = {
 	.driver		= {
 		.name	= "exynos-drm-hdmi",
 		.owner	= THIS_MODULE,
-		.pm = &hdmi_pm_ops,
 	},
 };
diff --git a/drivers/gpu/drm/exynos/exynos_drm_hdmi.h b/drivers/gpu/drm/exynos/exynos_drm_hdmi.h
index f3ae192c8dc..bd8126996e5 100644
--- a/drivers/gpu/drm/exynos/exynos_drm_hdmi.h
+++ b/drivers/gpu/drm/exynos/exynos_drm_hdmi.h
@@ -26,6 +26,9 @@
 #ifndef _EXYNOS_DRM_HDMI_H_
 #define _EXYNOS_DRM_HDMI_H_
 
+#define MIXER_WIN_NR		3
+#define MIXER_DEFAULT_WIN	0
+
 /*
  * exynos hdmi common context structure.
  *
@@ -54,13 +57,14 @@ struct exynos_hdmi_ops {
 	void (*get_max_resol)(void *ctx, unsigned int *width,
 				unsigned int *height);
 	void (*commit)(void *ctx);
-	void (*disable)(void *ctx);
+	void (*dpms)(void *ctx, int mode);
 };
 
 struct exynos_mixer_ops {
 	/* manager */
 	int (*enable_vblank)(void *ctx, int pipe);
 	void (*disable_vblank)(void *ctx);
+	void (*dpms)(void *ctx, int mode);
 
 	/* overlay */
 	void (*win_mode_set)(void *ctx, struct exynos_drm_overlay *overlay);
diff --git a/drivers/gpu/drm/exynos/exynos_drm_plane.c b/drivers/gpu/drm/exynos/exynos_drm_plane.c
index f92fe4c6174..c4c6525d465 100644
--- a/drivers/gpu/drm/exynos/exynos_drm_plane.c
+++ b/drivers/gpu/drm/exynos/exynos_drm_plane.c
@@ -41,8 +41,6 @@ exynos_update_plane(struct drm_plane *plane, struct drm_crtc *crtc,
 		container_of(plane, struct exynos_plane, base);
 	struct exynos_drm_overlay *overlay = &exynos_plane->overlay;
 	struct exynos_drm_crtc_pos pos;
-	unsigned int x = src_x >> 16;
-	unsigned int y = src_y >> 16;
 	int ret;
 
 	DRM_DEBUG_KMS("[%d] %s\n", __LINE__, __func__);
@@ -53,10 +51,12 @@ exynos_update_plane(struct drm_plane *plane, struct drm_crtc *crtc,
 	pos.crtc_w = crtc_w;
 	pos.crtc_h = crtc_h;
 
-	pos.fb_x = x;
-	pos.fb_y = y;
+	/* considering 16.16 fixed point of source values */
+	pos.fb_x = src_x >> 16;
+	pos.fb_y = src_y >> 16;
+	pos.src_w = src_w >> 16;
+	pos.src_h = src_h >> 16;
 
-	/* TODO: scale feature */
 	ret = exynos_drm_overlay_update(overlay, fb, &crtc->mode, &pos);
 	if (ret < 0)
 		return ret;
diff --git a/drivers/gpu/drm/exynos/exynos_hdmi.c b/drivers/gpu/drm/exynos/exynos_hdmi.c
index b0035387645..a137e9e39a3 100644
--- a/drivers/gpu/drm/exynos/exynos_hdmi.c
+++ b/drivers/gpu/drm/exynos/exynos_hdmi.c
@@ -57,18 +57,16 @@ struct hdmi_resources {
 struct hdmi_context {
 	struct device			*dev;
 	struct drm_device		*drm_dev;
-	struct fb_videomode		*default_timing;
-	unsigned int			is_v13:1;
-	unsigned int			default_win;
-	unsigned int			default_bpp;
-	bool				hpd_handle;
-	bool				enabled;
+	bool				hpd;
+	bool				powered;
+	bool				is_v13;
+	bool				dvi_mode;
+	struct mutex			hdmi_mutex;
 
 	struct resource			*regs_res;
 	void __iomem			*regs;
-	unsigned int			irq;
-	struct workqueue_struct		*wq;
-	struct work_struct		hotplug_work;
+	unsigned int			external_irq;
+	unsigned int			internal_irq;
 
 	struct i2c_client		*ddc_port;
 	struct i2c_client		*hdmiphy_port;
@@ -78,6 +76,9 @@ struct hdmi_context {
 
 	struct hdmi_resources		res;
 	void				*parent_ctx;
+
+	void				(*cfg_hpd)(bool external);
+	int				(*get_hpd)(void);
 };
 
 /* HDMI Version 1.3 */
@@ -361,6 +362,13 @@ static const u8 hdmiphy_conf27_027[32] = {
 	0x54, 0xe3, 0x24, 0x00, 0x00, 0x00, 0x01, 0x00,
 };
 
+static const u8 hdmiphy_conf74_176[32] = {
+	0x01, 0xd1, 0x1f, 0x10, 0x40, 0x5b, 0xef, 0x08,
+	0x81, 0xa0, 0xb9, 0xd8, 0x45, 0xa0, 0xac, 0x80,
+	0x5a, 0x80, 0x11, 0x04, 0x02, 0x22, 0x44, 0x86,
+	0x54, 0xa6, 0x24, 0x01, 0x00, 0x00, 0x01, 0x00,
+};
+
 static const u8 hdmiphy_conf74_25[32] = {
 	0x01, 0xd1, 0x1f, 0x10, 0x40, 0x40, 0xf8, 0x08,
 	0x81, 0xa0, 0xba, 0xd8, 0x45, 0xa0, 0xac, 0x80,
@@ -750,6 +758,63 @@ static const struct hdmi_preset_conf hdmi_conf_1080i60 = {
 	},
 };
 
+static const struct hdmi_preset_conf hdmi_conf_1080p30 = {
+	.core = {
+		.h_blank = {0x18, 0x01},
+		.v2_blank = {0x65, 0x04},
+		.v1_blank = {0x2d, 0x00},
+		.v_line = {0x65, 0x04},
+		.h_line = {0x98, 0x08},
+		.hsync_pol = {0x00},
+		.vsync_pol = {0x00},
+		.int_pro_mode = {0x00},
+		.v_blank_f0 = {0xff, 0xff},
+		.v_blank_f1 = {0xff, 0xff},
+		.h_sync_start = {0x56, 0x00},
+		.h_sync_end = {0x82, 0x00},
+		.v_sync_line_bef_2 = {0x09, 0x00},
+		.v_sync_line_bef_1 = {0x04, 0x00},
+		.v_sync_line_aft_2 = {0xff, 0xff},
+		.v_sync_line_aft_1 = {0xff, 0xff},
+		.v_sync_line_aft_pxl_2 = {0xff, 0xff},
+		.v_sync_line_aft_pxl_1 = {0xff, 0xff},
+		.v_blank_f2 = {0xff, 0xff},
+		.v_blank_f3 = {0xff, 0xff},
+		.v_blank_f4 = {0xff, 0xff},
+		.v_blank_f5 = {0xff, 0xff},
+		.v_sync_line_aft_3 = {0xff, 0xff},
+		.v_sync_line_aft_4 = {0xff, 0xff},
+		.v_sync_line_aft_5 = {0xff, 0xff},
+		.v_sync_line_aft_6 = {0xff, 0xff},
+		.v_sync_line_aft_pxl_3 = {0xff, 0xff},
+		.v_sync_line_aft_pxl_4 = {0xff, 0xff},
+		.v_sync_line_aft_pxl_5 = {0xff, 0xff},
+		.v_sync_line_aft_pxl_6 = {0xff, 0xff},
+		.vact_space_1 = {0xff, 0xff},
+		.vact_space_2 = {0xff, 0xff},
+		.vact_space_3 = {0xff, 0xff},
+		.vact_space_4 = {0xff, 0xff},
+		.vact_space_5 = {0xff, 0xff},
+		.vact_space_6 = {0xff, 0xff},
+		/* other don't care */
+	},
+	.tg = {
+		0x00, /* cmd */
+		0x98, 0x08, /* h_fsz */
+		0x18, 0x01, 0x80, 0x07, /* hact */
+		0x65, 0x04, /* v_fsz */
+		0x01, 0x00, 0x33, 0x02, /* vsync */
+		0x2d, 0x00, 0x38, 0x04, /* vact */
+		0x33, 0x02, /* field_chg */
+		0x48, 0x02, /* vact_st2 */
+		0x00, 0x00, /* vact_st3 */
+		0x00, 0x00, /* vact_st4 */
+		0x01, 0x00, 0x01, 0x00, /* vsync top/bot */
+		0x01, 0x00, 0x33, 0x02, /* field top/bot */
+		0x00, /* 3d FP */
+	},
+};
+
 static const struct hdmi_preset_conf hdmi_conf_1080p50 = {
 	.core = {
 		.h_blank = {0xd0, 0x02},
@@ -864,6 +929,7 @@ static const struct hdmi_conf hdmi_confs[] = {
 	{ 1280, 720, 60, false, hdmiphy_conf74_25, &hdmi_conf_720p60 },
 	{ 1920, 1080, 50, true, hdmiphy_conf74_25, &hdmi_conf_1080i50 },
 	{ 1920, 1080, 60, true, hdmiphy_conf74_25, &hdmi_conf_1080i60 },
+	{ 1920, 1080, 30, false, hdmiphy_conf74_176, &hdmi_conf_1080p30 },
 	{ 1920, 1080, 50, false, hdmiphy_conf148_5, &hdmi_conf_1080p50 },
 	{ 1920, 1080, 60, false, hdmiphy_conf148_5, &hdmi_conf_1080p60 },
 };
@@ -1194,12 +1260,8 @@ static int hdmi_conf_index(struct hdmi_context *hdata,
 static bool hdmi_is_connected(void *ctx)
 {
 	struct hdmi_context *hdata = ctx;
-	u32 val = hdmi_reg_read(hdata, HDMI_HPD_STATUS);
-
-	if (val)
-		return true;
 
-	return false;
+	return hdata->hpd;
 }
 
 static int hdmi_get_edid(void *ctx, struct drm_connector *connector,
@@ -1215,10 +1277,12 @@ static int hdmi_get_edid(void *ctx, struct drm_connector *connector,
 
 	raw_edid = drm_get_edid(connector, hdata->ddc_port->adapter);
 	if (raw_edid) {
+		hdata->dvi_mode = !drm_detect_hdmi_monitor(raw_edid);
 		memcpy(edid, raw_edid, min((1 + raw_edid->extensions)
 					* EDID_LENGTH, len));
-		DRM_DEBUG_KMS("width[%d] x height[%d]\n",
-				raw_edid->width_cm, raw_edid->height_cm);
+		DRM_DEBUG_KMS("%s : width[%d] x height[%d]\n",
+			(hdata->dvi_mode ? "dvi monitor" : "hdmi monitor"),
+			raw_edid->width_cm, raw_edid->height_cm);
 	} else {
 		return -ENODEV;
 	}
@@ -1289,28 +1353,6 @@ static int hdmi_check_timing(void *ctx, void *timing)
 		return hdmi_v14_check_timing(check_timing);
 }
 
-static int hdmi_display_power_on(void *ctx, int mode)
-{
-	DRM_DEBUG_KMS("[%d] %s\n", __LINE__, __func__);
-
-	switch (mode) {
-	case DRM_MODE_DPMS_ON:
-		DRM_DEBUG_KMS("hdmi [on]\n");
-		break;
-	case DRM_MODE_DPMS_STANDBY:
-		break;
-	case DRM_MODE_DPMS_SUSPEND:
-		break;
-	case DRM_MODE_DPMS_OFF:
-		DRM_DEBUG_KMS("hdmi [off]\n");
-		break;
-	default:
-		break;
-	}
-
-	return 0;
-}
-
 static void hdmi_set_acr(u32 freq, u8 *acr)
 {
 	u32 n, cts;
@@ -1463,10 +1505,7 @@ static void hdmi_audio_init(struct hdmi_context *hdata)
 
 static void hdmi_audio_control(struct hdmi_context *hdata, bool onoff)
 {
-	u32 mod;
-
-	mod = hdmi_reg_read(hdata, HDMI_MODE_SEL);
-	if (mod & HDMI_DVI_MODE_EN)
+	if (hdata->dvi_mode)
 		return;
 
 	hdmi_reg_writeb(hdata, HDMI_AUI_CON, onoff ? 2 : 0);
@@ -1478,9 +1517,6 @@ static void hdmi_conf_reset(struct hdmi_context *hdata)
 {
 	u32 reg;
 
-	/* disable hpd handle for drm */
-	hdata->hpd_handle = false;
-
 	if (hdata->is_v13)
 		reg = HDMI_V13_CORE_RSTOUT;
 	else
@@ -1491,16 +1527,10 @@ static void hdmi_conf_reset(struct hdmi_context *hdata)
 	mdelay(10);
 	hdmi_reg_writemask(hdata, reg, ~0, HDMI_CORE_SW_RSTOUT);
 	mdelay(10);
-
-	/* enable hpd handle for drm */
-	hdata->hpd_handle = true;
 }
 
 static void hdmi_conf_init(struct hdmi_context *hdata)
 {
-	/* disable hpd handle for drm */
-	hdata->hpd_handle = false;
-
 	/* enable HPD interrupts */
 	hdmi_reg_writemask(hdata, HDMI_INTC_CON, 0, HDMI_INTC_EN_GLOBAL |
 		HDMI_INTC_EN_HPD_PLUG | HDMI_INTC_EN_HPD_UNPLUG);
@@ -1514,6 +1544,14 @@ static void hdmi_conf_init(struct hdmi_context *hdata)
 	/* disable bluescreen */
 	hdmi_reg_writemask(hdata, HDMI_CON_0, 0, HDMI_BLUE_SCR_EN);
 
+	if (hdata->dvi_mode) {
+		/* choose DVI mode */
+		hdmi_reg_writemask(hdata, HDMI_MODE_SEL,
+				HDMI_MODE_DVI_EN, HDMI_MODE_MASK);
+		hdmi_reg_writeb(hdata, HDMI_CON_2,
+				HDMI_VID_PREAMBLE_DIS | HDMI_GUARD_BAND_DIS);
+	}
+
 	if (hdata->is_v13) {
 		/* choose bluescreen (fecal) color */
 		hdmi_reg_writeb(hdata, HDMI_V13_BLUE_SCREEN_0, 0x12);
@@ -1535,9 +1573,6 @@ static void hdmi_conf_init(struct hdmi_context *hdata)
 		hdmi_reg_writeb(hdata, HDMI_AVI_BYTE(1), 2 << 5);
 		hdmi_reg_writemask(hdata, HDMI_CON_1, 2, 3 << 5);
 	}
-
-	/* enable hpd handle for drm */
-	hdata->hpd_handle = true;
 }
 
 static void hdmi_v13_timing_apply(struct hdmi_context *hdata)
@@ -1890,8 +1925,11 @@ static void hdmi_conf_apply(struct hdmi_context *hdata)
 	hdmiphy_conf_reset(hdata);
 	hdmiphy_conf_apply(hdata);
 
+	mutex_lock(&hdata->hdmi_mutex);
 	hdmi_conf_reset(hdata);
 	hdmi_conf_init(hdata);
+	mutex_unlock(&hdata->hdmi_mutex);
+
 	hdmi_audio_init(hdata);
 
 	/* setting core registers */
@@ -1971,20 +2009,86 @@ static void hdmi_commit(void *ctx)
 	DRM_DEBUG_KMS("[%d] %s\n", __LINE__, __func__);
 
 	hdmi_conf_apply(hdata);
+}
+
+static void hdmi_poweron(struct hdmi_context *hdata)
+{
+	struct hdmi_resources *res = &hdata->res;
+
+	DRM_DEBUG_KMS("[%d] %s\n", __LINE__, __func__);
+
+	mutex_lock(&hdata->hdmi_mutex);
+	if (hdata->powered) {
+		mutex_unlock(&hdata->hdmi_mutex);
+		return;
+	}
+
+	hdata->powered = true;
+
+	if (hdata->cfg_hpd)
+		hdata->cfg_hpd(true);
+	mutex_unlock(&hdata->hdmi_mutex);
+
+	pm_runtime_get_sync(hdata->dev);
+
+	regulator_bulk_enable(res->regul_count, res->regul_bulk);
+	clk_enable(res->hdmiphy);
+	clk_enable(res->hdmi);
+	clk_enable(res->sclk_hdmi);
+}
+
+static void hdmi_poweroff(struct hdmi_context *hdata)
+{
+	struct hdmi_resources *res = &hdata->res;
+
+	DRM_DEBUG_KMS("[%d] %s\n", __LINE__, __func__);
+
+	mutex_lock(&hdata->hdmi_mutex);
+	if (!hdata->powered)
+		goto out;
+	mutex_unlock(&hdata->hdmi_mutex);
+
+	/*
+	 * The TV power domain needs any condition of hdmiphy to turn off and
+	 * its reset state seems to meet the condition.
+	 */
+	hdmiphy_conf_reset(hdata);
+
+	clk_disable(res->sclk_hdmi);
+	clk_disable(res->hdmi);
+	clk_disable(res->hdmiphy);
+	regulator_bulk_disable(res->regul_count, res->regul_bulk);
+
+	pm_runtime_put_sync(hdata->dev);
 
-	hdata->enabled = true;
+	mutex_lock(&hdata->hdmi_mutex);
+	if (hdata->cfg_hpd)
+		hdata->cfg_hpd(false);
+
+	hdata->powered = false;
+
+out:
+	mutex_unlock(&hdata->hdmi_mutex);
 }
 
-static void hdmi_disable(void *ctx)
+static void hdmi_dpms(void *ctx, int mode)
 {
 	struct hdmi_context *hdata = ctx;
 
 	DRM_DEBUG_KMS("[%d] %s\n", __LINE__, __func__);
 
-	if (hdata->enabled) {
-		hdmi_audio_control(hdata, false);
-		hdmiphy_conf_reset(hdata);
-		hdmi_conf_reset(hdata);
+	switch (mode) {
+	case DRM_MODE_DPMS_ON:
+		hdmi_poweron(hdata);
+		break;
+	case DRM_MODE_DPMS_STANDBY:
+	case DRM_MODE_DPMS_SUSPEND:
+	case DRM_MODE_DPMS_OFF:
+		hdmi_poweroff(hdata);
+		break;
+	default:
+		DRM_DEBUG_KMS("unknown dpms mode: %d\n", mode);
+		break;
 	}
 }
 
@@ -1993,30 +2097,35 @@ static struct exynos_hdmi_ops hdmi_ops = {
 	.is_connected	= hdmi_is_connected,
 	.get_edid	= hdmi_get_edid,
 	.check_timing	= hdmi_check_timing,
-	.power_on	= hdmi_display_power_on,
 
 	/* manager */
 	.mode_fixup	= hdmi_mode_fixup,
 	.mode_set	= hdmi_mode_set,
 	.get_max_resol	= hdmi_get_max_resol,
 	.commit		= hdmi_commit,
-	.disable	= hdmi_disable,
+	.dpms		= hdmi_dpms,
 };
 
-/*
- * Handle hotplug events outside the interrupt handler proper.
- */
-static void hdmi_hotplug_func(struct work_struct *work)
+static irqreturn_t hdmi_external_irq_thread(int irq, void *arg)
 {
-	struct hdmi_context *hdata =
-		container_of(work, struct hdmi_context, hotplug_work);
-	struct exynos_drm_hdmi_context *ctx =
-		(struct exynos_drm_hdmi_context *)hdata->parent_ctx;
+	struct exynos_drm_hdmi_context *ctx = arg;
+	struct hdmi_context *hdata = ctx->ctx;
+
+	if (!hdata->get_hpd)
+		goto out;
+
+	mutex_lock(&hdata->hdmi_mutex);
+	hdata->hpd = hdata->get_hpd();
+	mutex_unlock(&hdata->hdmi_mutex);
 
-	drm_helper_hpd_irq_event(ctx->drm_dev);
+	if (ctx->drm_dev)
+		drm_helper_hpd_irq_event(ctx->drm_dev);
+
+out:
+	return IRQ_HANDLED;
 }
 
-static irqreturn_t hdmi_irq_handler(int irq, void *arg)
+static irqreturn_t hdmi_internal_irq_thread(int irq, void *arg)
 {
 	struct exynos_drm_hdmi_context *ctx = arg;
 	struct hdmi_context *hdata = ctx->ctx;
@@ -2025,19 +2134,28 @@ static irqreturn_t hdmi_irq_handler(int irq, void *arg)
 	intc_flag = hdmi_reg_read(hdata, HDMI_INTC_FLAG);
 	/* clearing flags for HPD plug/unplug */
 	if (intc_flag & HDMI_INTC_FLAG_HPD_UNPLUG) {
-		DRM_DEBUG_KMS("unplugged, handling:%d\n", hdata->hpd_handle);
+		DRM_DEBUG_KMS("unplugged\n");
 		hdmi_reg_writemask(hdata, HDMI_INTC_FLAG, ~0,
 			HDMI_INTC_FLAG_HPD_UNPLUG);
 	}
 	if (intc_flag & HDMI_INTC_FLAG_HPD_PLUG) {
-		DRM_DEBUG_KMS("plugged, handling:%d\n", hdata->hpd_handle);
+		DRM_DEBUG_KMS("plugged\n");
 		hdmi_reg_writemask(hdata, HDMI_INTC_FLAG, ~0,
 			HDMI_INTC_FLAG_HPD_PLUG);
 	}
 
-	if (ctx->drm_dev && hdata->hpd_handle)
-		queue_work(hdata->wq, &hdata->hotplug_work);
+	mutex_lock(&hdata->hdmi_mutex);
+	hdata->hpd = hdmi_reg_read(hdata, HDMI_HPD_STATUS);
+	if (hdata->powered && hdata->hpd) {
+		mutex_unlock(&hdata->hdmi_mutex);
+		goto out;
+	}
+	mutex_unlock(&hdata->hdmi_mutex);
+
+	if (ctx->drm_dev)
+		drm_helper_hpd_irq_event(ctx->drm_dev);
 
+out:
 	return IRQ_HANDLED;
 }
 
@@ -2131,68 +2249,6 @@ static int hdmi_resources_cleanup(struct hdmi_context *hdata)
 	return 0;
 }
 
-static void hdmi_resource_poweron(struct hdmi_context *hdata)
-{
-	struct hdmi_resources *res = &hdata->res;
-
-	DRM_DEBUG_KMS("[%d] %s\n", __LINE__, __func__);
-
-	/* turn HDMI power on */
-	regulator_bulk_enable(res->regul_count, res->regul_bulk);
-	/* power-on hdmi physical interface */
-	clk_enable(res->hdmiphy);
-	/* turn clocks on */
-	clk_enable(res->hdmi);
-	clk_enable(res->sclk_hdmi);
-
-	hdmiphy_conf_reset(hdata);
-	hdmi_conf_reset(hdata);
-	hdmi_conf_init(hdata);
-	hdmi_audio_init(hdata);
-}
-
-static void hdmi_resource_poweroff(struct hdmi_context *hdata)
-{
-	struct hdmi_resources *res = &hdata->res;
-
-	DRM_DEBUG_KMS("[%d] %s\n", __LINE__, __func__);
-
-	/* turn clocks off */
-	clk_disable(res->sclk_hdmi);
-	clk_disable(res->hdmi);
-	/* power-off hdmiphy */
-	clk_disable(res->hdmiphy);
-	/* turn HDMI power off */
-	regulator_bulk_disable(res->regul_count, res->regul_bulk);
-}
-
-static int hdmi_runtime_suspend(struct device *dev)
-{
-	struct exynos_drm_hdmi_context *ctx = get_hdmi_context(dev);
-
-	DRM_DEBUG_KMS("%s\n", __func__);
-
-	hdmi_resource_poweroff(ctx->ctx);
-
-	return 0;
-}
-
-static int hdmi_runtime_resume(struct device *dev)
-{
-	struct exynos_drm_hdmi_context *ctx = get_hdmi_context(dev);
-
-	DRM_DEBUG_KMS("%s\n", __func__);
-
-	hdmi_resource_poweron(ctx->ctx);
-
-	return 0;
-}
-
-static const struct dev_pm_ops hdmi_pm_ops = {
-	.runtime_suspend = hdmi_runtime_suspend,
-	.runtime_resume	 = hdmi_runtime_resume,
-};
-
 static struct i2c_client *hdmi_ddc, *hdmi_hdmiphy;
 
 void hdmi_attach_ddc_client(struct i2c_client *ddc)
@@ -2237,15 +2293,16 @@ static int __devinit hdmi_probe(struct platform_device *pdev)
 		return -ENOMEM;
 	}
 
+	mutex_init(&hdata->hdmi_mutex);
+
 	drm_hdmi_ctx->ctx = (void *)hdata;
 	hdata->parent_ctx = (void *)drm_hdmi_ctx;
 
 	platform_set_drvdata(pdev, drm_hdmi_ctx);
 
 	hdata->is_v13 = pdata->is_v13;
-	hdata->default_win = pdata->default_win;
-	hdata->default_timing = &pdata->timing;
-	hdata->default_bpp = pdata->bpp;
+	hdata->cfg_hpd = pdata->cfg_hpd;
+	hdata->get_hpd = pdata->get_hpd;
 	hdata->dev = dev;
 
 	ret = hdmi_resources_init(hdata);
@@ -2294,41 +2351,49 @@ static int __devinit hdmi_probe(struct platform_device *pdev)
 
 	hdata->hdmiphy_port = hdmi_hdmiphy;
 
-	res = platform_get_resource(pdev, IORESOURCE_IRQ, 0);
-	if (res == NULL) {
-		DRM_ERROR("get interrupt resource failed.\n");
-		ret = -ENXIO;
+	hdata->external_irq = platform_get_irq_byname(pdev, "external_irq");
+	if (hdata->external_irq < 0) {
+		DRM_ERROR("failed to get platform irq\n");
+		ret = hdata->external_irq;
 		goto err_hdmiphy;
 	}
 
-	/* create workqueue and hotplug work */
-	hdata->wq = alloc_workqueue("exynos-drm-hdmi",
-			WQ_UNBOUND | WQ_NON_REENTRANT, 1);
-	if (hdata->wq == NULL) {
-		DRM_ERROR("Failed to create workqueue.\n");
-		ret = -ENOMEM;
+	hdata->internal_irq = platform_get_irq_byname(pdev, "internal_irq");
+	if (hdata->internal_irq < 0) {
+		DRM_ERROR("failed to get platform internal irq\n");
+		ret = hdata->internal_irq;
 		goto err_hdmiphy;
 	}
-	INIT_WORK(&hdata->hotplug_work, hdmi_hotplug_func);
 
-	/* register hpd interrupt */
-	ret = request_irq(res->start, hdmi_irq_handler, 0, "drm_hdmi",
-				drm_hdmi_ctx);
+	ret = request_threaded_irq(hdata->external_irq, NULL,
+			hdmi_external_irq_thread, IRQF_TRIGGER_RISING |
+			IRQF_TRIGGER_FALLING | IRQF_ONESHOT,
+			"hdmi_external", drm_hdmi_ctx);
 	if (ret) {
-		DRM_ERROR("request interrupt failed.\n");
-		goto err_workqueue;
+		DRM_ERROR("failed to register hdmi internal interrupt\n");
+		goto err_hdmiphy;
+	}
+
+	if (hdata->cfg_hpd)
+		hdata->cfg_hpd(false);
+
+	ret = request_threaded_irq(hdata->internal_irq, NULL,
+			hdmi_internal_irq_thread, IRQF_ONESHOT,
+			"hdmi_internal", drm_hdmi_ctx);
+	if (ret) {
+		DRM_ERROR("failed to register hdmi internal interrupt\n");
+		goto err_free_irq;
 	}
-	hdata->irq = res->start;
 
 	/* register specific callbacks to common hdmi. */
 	exynos_hdmi_ops_register(&hdmi_ops);
 
-	hdmi_resource_poweron(hdata);
+	pm_runtime_enable(dev);
 
 	return 0;
 
-err_workqueue:
-	destroy_workqueue(hdata->wq);
+err_free_irq:
+	free_irq(hdata->external_irq, drm_hdmi_ctx);
 err_hdmiphy:
 	i2c_del_driver(&hdmiphy_driver);
 err_ddc:
@@ -2348,18 +2413,15 @@ err_data:
 
 static int __devexit hdmi_remove(struct platform_device *pdev)
 {
+	struct device *dev = &pdev->dev;
 	struct exynos_drm_hdmi_context *ctx = platform_get_drvdata(pdev);
 	struct hdmi_context *hdata = ctx->ctx;
 
 	DRM_DEBUG_KMS("[%d] %s\n", __LINE__, __func__);
 
-	hdmi_resource_poweroff(hdata);
+	pm_runtime_disable(dev);
 
-	disable_irq(hdata->irq);
-	free_irq(hdata->irq, hdata);
-
-	cancel_work_sync(&hdata->hotplug_work);
-	destroy_workqueue(hdata->wq);
+	free_irq(hdata->internal_irq, hdata);
 
 	hdmi_resources_cleanup(hdata);
 
@@ -2378,12 +2440,43 @@ static int __devexit hdmi_remove(struct platform_device *pdev)
 	return 0;
 }
 
+#ifdef CONFIG_PM_SLEEP
+static int hdmi_suspend(struct device *dev)
+{
+	struct exynos_drm_hdmi_context *ctx = get_hdmi_context(dev);
+	struct hdmi_context *hdata = ctx->ctx;
+
+	disable_irq(hdata->internal_irq);
+	disable_irq(hdata->external_irq);
+
+	hdata->hpd = false;
+	if (ctx->drm_dev)
+		drm_helper_hpd_irq_event(ctx->drm_dev);
+
+	hdmi_poweroff(hdata);
+
+	return 0;
+}
+
+static int hdmi_resume(struct device *dev)
+{
+	struct exynos_drm_hdmi_context *ctx = get_hdmi_context(dev);
+	struct hdmi_context *hdata = ctx->ctx;
+
+	enable_irq(hdata->external_irq);
+	enable_irq(hdata->internal_irq);
+	return 0;
+}
+#endif
+
+static SIMPLE_DEV_PM_OPS(hdmi_pm_ops, hdmi_suspend, hdmi_resume);
+
 struct platform_driver hdmi_driver = {
 	.probe		= hdmi_probe,
 	.remove		= __devexit_p(hdmi_remove),
 	.driver		= {
 		.name	= "exynos4-hdmi",
 		.owner	= THIS_MODULE,
-		.pm = &hdmi_pm_ops,
+		.pm	= &hdmi_pm_ops,
 	},
 };
diff --git a/drivers/gpu/drm/exynos/exynos_mixer.c b/drivers/gpu/drm/exynos/exynos_mixer.c
index e15438c0112..68ef0102837 100644
--- a/drivers/gpu/drm/exynos/exynos_mixer.c
+++ b/drivers/gpu/drm/exynos/exynos_mixer.c
@@ -37,9 +37,6 @@
 #include "exynos_drm_drv.h"
 #include "exynos_drm_hdmi.h"
 
-#define MIXER_WIN_NR		3
-#define MIXER_DEFAULT_WIN	0
-
 #define get_mixer_context(dev)	platform_get_drvdata(to_platform_device(dev))
 
 struct hdmi_win_data {
@@ -57,13 +54,14 @@ struct hdmi_win_data {
 	unsigned int		fb_y;
 	unsigned int		fb_width;
 	unsigned int		fb_height;
+	unsigned int		src_width;
+	unsigned int		src_height;
 	unsigned int		mode_width;
 	unsigned int		mode_height;
 	unsigned int		scan_flags;
 };
 
 struct mixer_resources {
-	struct device		*dev;
 	int			irq;
 	void __iomem		*mixer_regs;
 	void __iomem		*vp_regs;
@@ -76,10 +74,13 @@ struct mixer_resources {
 };
 
 struct mixer_context {
-	unsigned int		irq;
+	struct device		*dev;
 	int			pipe;
 	bool			interlace;
+	bool			powered;
+	u32			int_en;
 
+	struct mutex		mixer_mutex;
 	struct mixer_resources	mixer_res;
 	struct hdmi_win_data	win_data[MIXER_WIN_NR];
 };
@@ -352,10 +353,7 @@ static void vp_video_buffer(struct mixer_context *ctx, int win)
 	struct mixer_resources *res = &ctx->mixer_res;
 	unsigned long flags;
 	struct hdmi_win_data *win_data;
-	unsigned int full_width, full_height, width, height;
 	unsigned int x_ratio, y_ratio;
-	unsigned int src_x_offset, src_y_offset, dst_x_offset, dst_y_offset;
-	unsigned int mode_width, mode_height;
 	unsigned int buf_num;
 	dma_addr_t luma_addr[2], chroma_addr[2];
 	bool tiled_mode = false;
@@ -382,21 +380,9 @@ static void vp_video_buffer(struct mixer_context *ctx, int win)
 		return;
 	}
 
-	full_width = win_data->fb_width;
-	full_height = win_data->fb_height;
-	width = win_data->crtc_width;
-	height = win_data->crtc_height;
-	mode_width = win_data->mode_width;
-	mode_height = win_data->mode_height;
-
 	/* scaling feature: (src << 16) / dst */
-	x_ratio = (width << 16) / width;
-	y_ratio = (height << 16) / height;
-
-	src_x_offset = win_data->fb_x;
-	src_y_offset = win_data->fb_y;
-	dst_x_offset = win_data->crtc_x;
-	dst_y_offset = win_data->crtc_y;
+	x_ratio = (win_data->src_width << 16) / win_data->crtc_width;
+	y_ratio = (win_data->src_height << 16) / win_data->crtc_height;
 
 	if (buf_num == 2) {
 		luma_addr[0] = win_data->dma_addr;
@@ -404,7 +390,7 @@ static void vp_video_buffer(struct mixer_context *ctx, int win)
 	} else {
 		luma_addr[0] = win_data->dma_addr;
 		chroma_addr[0] = win_data->dma_addr
-			+ (full_width * full_height);
+			+ (win_data->fb_width * win_data->fb_height);
 	}
 
 	if (win_data->scan_flags & DRM_MODE_FLAG_INTERLACE) {
@@ -413,8 +399,8 @@ static void vp_video_buffer(struct mixer_context *ctx, int win)
 			luma_addr[1] = luma_addr[0] + 0x40;
 			chroma_addr[1] = chroma_addr[0] + 0x40;
 		} else {
-			luma_addr[1] = luma_addr[0] + full_width;
-			chroma_addr[1] = chroma_addr[0] + full_width;
+			luma_addr[1] = luma_addr[0] + win_data->fb_width;
+			chroma_addr[1] = chroma_addr[0] + win_data->fb_width;
 		}
 	} else {
 		ctx->interlace = false;
@@ -435,26 +421,26 @@ static void vp_video_buffer(struct mixer_context *ctx, int win)
 	vp_reg_writemask(res, VP_MODE, val, VP_MODE_FMT_MASK);
 
 	/* setting size of input image */
-	vp_reg_write(res, VP_IMG_SIZE_Y, VP_IMG_HSIZE(full_width) |
-		VP_IMG_VSIZE(full_height));
+	vp_reg_write(res, VP_IMG_SIZE_Y, VP_IMG_HSIZE(win_data->fb_width) |
+		VP_IMG_VSIZE(win_data->fb_height));
 	/* chroma height has to reduced by 2 to avoid chroma distorions */
-	vp_reg_write(res, VP_IMG_SIZE_C, VP_IMG_HSIZE(full_width) |
-		VP_IMG_VSIZE(full_height / 2));
+	vp_reg_write(res, VP_IMG_SIZE_C, VP_IMG_HSIZE(win_data->fb_width) |
+		VP_IMG_VSIZE(win_data->fb_height / 2));
 
-	vp_reg_write(res, VP_SRC_WIDTH, width);
-	vp_reg_write(res, VP_SRC_HEIGHT, height);
+	vp_reg_write(res, VP_SRC_WIDTH, win_data->src_width);
+	vp_reg_write(res, VP_SRC_HEIGHT, win_data->src_height);
 	vp_reg_write(res, VP_SRC_H_POSITION,
-			VP_SRC_H_POSITION_VAL(src_x_offset));
-	vp_reg_write(res, VP_SRC_V_POSITION, src_y_offset);
+			VP_SRC_H_POSITION_VAL(win_data->fb_x));
+	vp_reg_write(res, VP_SRC_V_POSITION, win_data->fb_y);
 
-	vp_reg_write(res, VP_DST_WIDTH, width);
-	vp_reg_write(res, VP_DST_H_POSITION, dst_x_offset);
+	vp_reg_write(res, VP_DST_WIDTH, win_data->crtc_width);
+	vp_reg_write(res, VP_DST_H_POSITION, win_data->crtc_x);
 	if (ctx->interlace) {
-		vp_reg_write(res, VP_DST_HEIGHT, height / 2);
-		vp_reg_write(res, VP_DST_V_POSITION, dst_y_offset / 2);
+		vp_reg_write(res, VP_DST_HEIGHT, win_data->crtc_height / 2);
+		vp_reg_write(res, VP_DST_V_POSITION, win_data->crtc_y / 2);
 	} else {
-		vp_reg_write(res, VP_DST_HEIGHT, height);
-		vp_reg_write(res, VP_DST_V_POSITION, dst_y_offset);
+		vp_reg_write(res, VP_DST_HEIGHT, win_data->crtc_height);
+		vp_reg_write(res, VP_DST_V_POSITION, win_data->crtc_y);
 	}
 
 	vp_reg_write(res, VP_H_RATIO, x_ratio);
@@ -468,8 +454,8 @@ static void vp_video_buffer(struct mixer_context *ctx, int win)
 	vp_reg_write(res, VP_TOP_C_PTR, chroma_addr[0]);
 	vp_reg_write(res, VP_BOT_C_PTR, chroma_addr[1]);
 
-	mixer_cfg_scan(ctx, mode_height);
-	mixer_cfg_rgb_fmt(ctx, mode_height);
+	mixer_cfg_scan(ctx, win_data->mode_height);
+	mixer_cfg_rgb_fmt(ctx, win_data->mode_height);
 	mixer_cfg_layer(ctx, win, true);
 	mixer_run(ctx);
 
@@ -484,10 +470,8 @@ static void mixer_graph_buffer(struct mixer_context *ctx, int win)
 	struct mixer_resources *res = &ctx->mixer_res;
 	unsigned long flags;
 	struct hdmi_win_data *win_data;
-	unsigned int full_width, width, height;
 	unsigned int x_ratio, y_ratio;
 	unsigned int src_x_offset, src_y_offset, dst_x_offset, dst_y_offset;
-	unsigned int mode_width, mode_height;
 	dma_addr_t dma_addr;
 	unsigned int fmt;
 	u32 val;
@@ -510,26 +494,17 @@ static void mixer_graph_buffer(struct mixer_context *ctx, int win)
 		fmt = ARGB8888;
 	}
 
-	dma_addr = win_data->dma_addr;
-	full_width = win_data->fb_width;
-	width = win_data->crtc_width;
-	height = win_data->crtc_height;
-	mode_width = win_data->mode_width;
-	mode_height = win_data->mode_height;
-
 	/* 2x scaling feature */
 	x_ratio = 0;
 	y_ratio = 0;
 
-	src_x_offset = win_data->fb_x;
-	src_y_offset = win_data->fb_y;
 	dst_x_offset = win_data->crtc_x;
 	dst_y_offset = win_data->crtc_y;
 
 	/* converting dma address base and source offset */
-	dma_addr = dma_addr
-		+ (src_x_offset * win_data->bpp >> 3)
-		+ (src_y_offset * full_width * win_data->bpp >> 3);
+	dma_addr = win_data->dma_addr
+		+ (win_data->fb_x * win_data->bpp >> 3)
+		+ (win_data->fb_y * win_data->fb_width * win_data->bpp >> 3);
 	src_x_offset = 0;
 	src_y_offset = 0;
 
@@ -546,10 +521,10 @@ static void mixer_graph_buffer(struct mixer_context *ctx, int win)
 		MXR_GRP_CFG_FORMAT_VAL(fmt), MXR_GRP_CFG_FORMAT_MASK);
 
 	/* setup geometry */
-	mixer_reg_write(res, MXR_GRAPHIC_SPAN(win), full_width);
+	mixer_reg_write(res, MXR_GRAPHIC_SPAN(win), win_data->fb_width);
 
-	val  = MXR_GRP_WH_WIDTH(width);
-	val |= MXR_GRP_WH_HEIGHT(height);
+	val  = MXR_GRP_WH_WIDTH(win_data->crtc_width);
+	val |= MXR_GRP_WH_HEIGHT(win_data->crtc_height);
 	val |= MXR_GRP_WH_H_SCALE(x_ratio);
 	val |= MXR_GRP_WH_V_SCALE(y_ratio);
 	mixer_reg_write(res, MXR_GRAPHIC_WH(win), val);
@@ -567,8 +542,8 @@ static void mixer_graph_buffer(struct mixer_context *ctx, int win)
 	/* set buffer address to mixer */
 	mixer_reg_write(res, MXR_GRAPHIC_BASE(win), dma_addr);
 
-	mixer_cfg_scan(ctx, mode_height);
-	mixer_cfg_rgb_fmt(ctx, mode_height);
+	mixer_cfg_scan(ctx, win_data->mode_height);
+	mixer_cfg_rgb_fmt(ctx, win_data->mode_height);
 	mixer_cfg_layer(ctx, win, true);
 	mixer_run(ctx);
 
@@ -591,6 +566,116 @@ static void vp_win_reset(struct mixer_context *ctx)
 	WARN(tries == 0, "failed to reset Video Processor\n");
 }
 
+static void mixer_win_reset(struct mixer_context *ctx)
+{
+	struct mixer_resources *res = &ctx->mixer_res;
+	unsigned long flags;
+	u32 val; /* value stored to register */
+
+	spin_lock_irqsave(&res->reg_slock, flags);
+	mixer_vsync_set_update(ctx, false);
+
+	mixer_reg_writemask(res, MXR_CFG, MXR_CFG_DST_HDMI, MXR_CFG_DST_MASK);
+
+	/* set output in RGB888 mode */
+	mixer_reg_writemask(res, MXR_CFG, MXR_CFG_OUT_RGB888, MXR_CFG_OUT_MASK);
+
+	/* 16 beat burst in DMA */
+	mixer_reg_writemask(res, MXR_STATUS, MXR_STATUS_16_BURST,
+		MXR_STATUS_BURST_MASK);
+
+	/* setting default layer priority: layer1 > layer0 > video
+	 * because typical usage scenario would be
+	 * layer1 - OSD
+	 * layer0 - framebuffer
+	 * video - video overlay
+	 */
+	val = MXR_LAYER_CFG_GRP1_VAL(3);
+	val |= MXR_LAYER_CFG_GRP0_VAL(2);
+	val |= MXR_LAYER_CFG_VP_VAL(1);
+	mixer_reg_write(res, MXR_LAYER_CFG, val);
+
+	/* setting background color */
+	mixer_reg_write(res, MXR_BG_COLOR0, 0x008080);
+	mixer_reg_write(res, MXR_BG_COLOR1, 0x008080);
+	mixer_reg_write(res, MXR_BG_COLOR2, 0x008080);
+
+	/* setting graphical layers */
+
+	val  = MXR_GRP_CFG_COLOR_KEY_DISABLE; /* no blank key */
+	val |= MXR_GRP_CFG_WIN_BLEND_EN;
+	val |= MXR_GRP_CFG_ALPHA_VAL(0xff); /* non-transparent alpha */
+
+	/* the same configuration for both layers */
+	mixer_reg_write(res, MXR_GRAPHIC_CFG(0), val);
+
+	val |= MXR_GRP_CFG_BLEND_PRE_MUL;
+	val |= MXR_GRP_CFG_PIXEL_BLEND_EN;
+	mixer_reg_write(res, MXR_GRAPHIC_CFG(1), val);
+
+	/* configuration of Video Processor Registers */
+	vp_win_reset(ctx);
+	vp_default_filter(res);
+
+	/* disable all layers */
+	mixer_reg_writemask(res, MXR_CFG, 0, MXR_CFG_GRP0_ENABLE);
+	mixer_reg_writemask(res, MXR_CFG, 0, MXR_CFG_GRP1_ENABLE);
+	mixer_reg_writemask(res, MXR_CFG, 0, MXR_CFG_VP_ENABLE);
+
+	mixer_vsync_set_update(ctx, true);
+	spin_unlock_irqrestore(&res->reg_slock, flags);
+}
+
+static void mixer_poweron(struct mixer_context *ctx)
+{
+	struct mixer_resources *res = &ctx->mixer_res;
+
+	DRM_DEBUG_KMS("[%d] %s\n", __LINE__, __func__);
+
+	mutex_lock(&ctx->mixer_mutex);
+	if (ctx->powered) {
+		mutex_unlock(&ctx->mixer_mutex);
+		return;
+	}
+	ctx->powered = true;
+	mutex_unlock(&ctx->mixer_mutex);
+
+	pm_runtime_get_sync(ctx->dev);
+
+	clk_enable(res->mixer);
+	clk_enable(res->vp);
+	clk_enable(res->sclk_mixer);
+
+	mixer_reg_write(res, MXR_INT_EN, ctx->int_en);
+	mixer_win_reset(ctx);
+}
+
+static void mixer_poweroff(struct mixer_context *ctx)
+{
+	struct mixer_resources *res = &ctx->mixer_res;
+
+	DRM_DEBUG_KMS("[%d] %s\n", __LINE__, __func__);
+
+	mutex_lock(&ctx->mixer_mutex);
+	if (!ctx->powered)
+		goto out;
+	mutex_unlock(&ctx->mixer_mutex);
+
+	ctx->int_en = mixer_reg_read(res, MXR_INT_EN);
+
+	clk_disable(res->mixer);
+	clk_disable(res->vp);
+	clk_disable(res->sclk_mixer);
+
+	pm_runtime_put_sync(ctx->dev);
+
+	mutex_lock(&ctx->mixer_mutex);
+	ctx->powered = false;
+
+out:
+	mutex_unlock(&ctx->mixer_mutex);
+}
+
 static int mixer_enable_vblank(void *ctx, int pipe)
 {
 	struct mixer_context *mixer_ctx = ctx;
@@ -618,6 +703,27 @@ static void mixer_disable_vblank(void *ctx)
 	mixer_reg_writemask(res, MXR_INT_EN, 0, MXR_INT_EN_VSYNC);
 }
 
+static void mixer_dpms(void *ctx, int mode)
+{
+	struct mixer_context *mixer_ctx = ctx;
+
+	DRM_DEBUG_KMS("[%d] %s\n", __LINE__, __func__);
+
+	switch (mode) {
+	case DRM_MODE_DPMS_ON:
+		mixer_poweron(mixer_ctx);
+		break;
+	case DRM_MODE_DPMS_STANDBY:
+	case DRM_MODE_DPMS_SUSPEND:
+	case DRM_MODE_DPMS_OFF:
+		mixer_poweroff(mixer_ctx);
+		break;
+	default:
+		DRM_DEBUG_KMS("unknown dpms mode: %d\n", mode);
+		break;
+	}
+}
+
 static void mixer_win_mode_set(void *ctx,
 			      struct exynos_drm_overlay *overlay)
 {
@@ -643,7 +749,7 @@ static void mixer_win_mode_set(void *ctx,
 		win = MIXER_DEFAULT_WIN;
 
 	if (win < 0 || win > MIXER_WIN_NR) {
-		DRM_ERROR("overlay plane[%d] is wrong\n", win);
+		DRM_ERROR("mixer window[%d] is wrong\n", win);
 		return;
 	}
 
@@ -665,6 +771,8 @@ static void mixer_win_mode_set(void *ctx,
 	win_data->fb_y = overlay->fb_y;
 	win_data->fb_width = overlay->fb_width;
 	win_data->fb_height = overlay->fb_height;
+	win_data->src_width = overlay->src_width;
+	win_data->src_height = overlay->src_height;
 
 	win_data->mode_width = overlay->mode_width;
 	win_data->mode_height = overlay->mode_height;
@@ -672,44 +780,26 @@ static void mixer_win_mode_set(void *ctx,
 	win_data->scan_flags = overlay->scan_flag;
 }
 
-static void mixer_win_commit(void *ctx, int zpos)
+static void mixer_win_commit(void *ctx, int win)
 {
 	struct mixer_context *mixer_ctx = ctx;
-	int win = zpos;
 
 	DRM_DEBUG_KMS("[%d] %s, win: %d\n", __LINE__, __func__, win);
 
-	if (win == DEFAULT_ZPOS)
-		win = MIXER_DEFAULT_WIN;
-
-	if (win < 0 || win > MIXER_WIN_NR) {
-		DRM_ERROR("overlay plane[%d] is wrong\n", win);
-		return;
-	}
-
 	if (win > 1)
 		vp_video_buffer(mixer_ctx, win);
 	else
 		mixer_graph_buffer(mixer_ctx, win);
 }
 
-static void mixer_win_disable(void *ctx, int zpos)
+static void mixer_win_disable(void *ctx, int win)
 {
 	struct mixer_context *mixer_ctx = ctx;
 	struct mixer_resources *res = &mixer_ctx->mixer_res;
 	unsigned long flags;
-	int win = zpos;
 
 	DRM_DEBUG_KMS("[%d] %s, win: %d\n", __LINE__, __func__, win);
 
-	if (win == DEFAULT_ZPOS)
-		win = MIXER_DEFAULT_WIN;
-
-	if (win < 0 || win > MIXER_WIN_NR) {
-		DRM_ERROR("overlay plane[%d] is wrong\n", win);
-		return;
-	}
-
 	spin_lock_irqsave(&res->reg_slock, flags);
 	mixer_vsync_set_update(mixer_ctx, false);
 
@@ -723,6 +813,7 @@ static struct exynos_mixer_ops mixer_ops = {
 	/* manager */
 	.enable_vblank		= mixer_enable_vblank,
 	.disable_vblank		= mixer_disable_vblank,
+	.dpms			= mixer_dpms,
 
 	/* overlay */
 	.win_mode_set		= mixer_win_mode_set,
@@ -773,7 +864,7 @@ static irqreturn_t mixer_irq_handler(int irq, void *arg)
 	struct exynos_drm_hdmi_context *drm_hdmi_ctx = arg;
 	struct mixer_context *ctx = drm_hdmi_ctx->ctx;
 	struct mixer_resources *res = &ctx->mixer_res;
-	u32 val, val_base;
+	u32 val, base, shadow;
 
 	spin_lock(&res->reg_slock);
 
@@ -784,12 +875,14 @@ static irqreturn_t mixer_irq_handler(int irq, void *arg)
 	if (val & MXR_INT_STATUS_VSYNC) {
 		/* interlace scan need to check shadow register */
 		if (ctx->interlace) {
-			val_base = mixer_reg_read(res, MXR_GRAPHIC_BASE_S(0));
-			if (ctx->win_data[0].dma_addr != val_base)
+			base = mixer_reg_read(res, MXR_GRAPHIC_BASE(0));
+			shadow = mixer_reg_read(res, MXR_GRAPHIC_BASE_S(0));
+			if (base != shadow)
 				goto out;
 
-			val_base = mixer_reg_read(res, MXR_GRAPHIC_BASE_S(1));
-			if (ctx->win_data[1].dma_addr != val_base)
+			base = mixer_reg_read(res, MXR_GRAPHIC_BASE(1));
+			shadow = mixer_reg_read(res, MXR_GRAPHIC_BASE_S(1));
+			if (base != shadow)
 				goto out;
 		}
 
@@ -811,117 +904,6 @@ out:
 	return IRQ_HANDLED;
 }
 
-static void mixer_win_reset(struct mixer_context *ctx)
-{
-	struct mixer_resources *res = &ctx->mixer_res;
-	unsigned long flags;
-	u32 val; /* value stored to register */
-
-	spin_lock_irqsave(&res->reg_slock, flags);
-	mixer_vsync_set_update(ctx, false);
-
-	mixer_reg_writemask(res, MXR_CFG, MXR_CFG_DST_HDMI, MXR_CFG_DST_MASK);
-
-	/* set output in RGB888 mode */
-	mixer_reg_writemask(res, MXR_CFG, MXR_CFG_OUT_RGB888, MXR_CFG_OUT_MASK);
-
-	/* 16 beat burst in DMA */
-	mixer_reg_writemask(res, MXR_STATUS, MXR_STATUS_16_BURST,
-		MXR_STATUS_BURST_MASK);
-
-	/* setting default layer priority: layer1 > layer0 > video
-	 * because typical usage scenario would be
-	 * layer1 - OSD
-	 * layer0 - framebuffer
-	 * video - video overlay
-	 */
-	val = MXR_LAYER_CFG_GRP1_VAL(3);
-	val |= MXR_LAYER_CFG_GRP0_VAL(2);
-	val |= MXR_LAYER_CFG_VP_VAL(1);
-	mixer_reg_write(res, MXR_LAYER_CFG, val);
-
-	/* setting background color */
-	mixer_reg_write(res, MXR_BG_COLOR0, 0x008080);
-	mixer_reg_write(res, MXR_BG_COLOR1, 0x008080);
-	mixer_reg_write(res, MXR_BG_COLOR2, 0x008080);
-
-	/* setting graphical layers */
-
-	val  = MXR_GRP_CFG_COLOR_KEY_DISABLE; /* no blank key */
-	val |= MXR_GRP_CFG_WIN_BLEND_EN;
-	val |= MXR_GRP_CFG_ALPHA_VAL(0xff); /* non-transparent alpha */
-
-	/* the same configuration for both layers */
-	mixer_reg_write(res, MXR_GRAPHIC_CFG(0), val);
-
-	val |= MXR_GRP_CFG_BLEND_PRE_MUL;
-	val |= MXR_GRP_CFG_PIXEL_BLEND_EN;
-	mixer_reg_write(res, MXR_GRAPHIC_CFG(1), val);
-
-	/* configuration of Video Processor Registers */
-	vp_win_reset(ctx);
-	vp_default_filter(res);
-
-	/* disable all layers */
-	mixer_reg_writemask(res, MXR_CFG, 0, MXR_CFG_GRP0_ENABLE);
-	mixer_reg_writemask(res, MXR_CFG, 0, MXR_CFG_GRP1_ENABLE);
-	mixer_reg_writemask(res, MXR_CFG, 0, MXR_CFG_VP_ENABLE);
-
-	mixer_vsync_set_update(ctx, true);
-	spin_unlock_irqrestore(&res->reg_slock, flags);
-}
-
-static void mixer_resource_poweron(struct mixer_context *ctx)
-{
-	struct mixer_resources *res = &ctx->mixer_res;
-
-	DRM_DEBUG_KMS("[%d] %s\n", __LINE__, __func__);
-
-	clk_enable(res->mixer);
-	clk_enable(res->vp);
-	clk_enable(res->sclk_mixer);
-
-	mixer_win_reset(ctx);
-}
-
-static void mixer_resource_poweroff(struct mixer_context *ctx)
-{
-	struct mixer_resources *res = &ctx->mixer_res;
-
-	DRM_DEBUG_KMS("[%d] %s\n", __LINE__, __func__);
-
-	clk_disable(res->mixer);
-	clk_disable(res->vp);
-	clk_disable(res->sclk_mixer);
-}
-
-static int mixer_runtime_resume(struct device *dev)
-{
-	struct exynos_drm_hdmi_context *ctx = get_mixer_context(dev);
-
-	DRM_DEBUG_KMS("resume - start\n");
-
-	mixer_resource_poweron(ctx->ctx);
-
-	return 0;
-}
-
-static int mixer_runtime_suspend(struct device *dev)
-{
-	struct exynos_drm_hdmi_context *ctx = get_mixer_context(dev);
-
-	DRM_DEBUG_KMS("suspend - start\n");
-
-	mixer_resource_poweroff(ctx->ctx);
-
-	return 0;
-}
-
-static const struct dev_pm_ops mixer_pm_ops = {
-	.runtime_suspend = mixer_runtime_suspend,
-	.runtime_resume	 = mixer_runtime_resume,
-};
-
 static int __devinit mixer_resources_init(struct exynos_drm_hdmi_context *ctx,
 				 struct platform_device *pdev)
 {
@@ -931,7 +913,6 @@ static int __devinit mixer_resources_init(struct exynos_drm_hdmi_context *ctx,
 	struct resource *res;
 	int ret;
 
-	mixer_res->dev = dev;
 	spin_lock_init(&mixer_res->reg_slock);
 
 	mixer_res->mixer = clk_get(dev, "mixer");
@@ -1027,7 +1008,6 @@ fail:
 		clk_put(mixer_res->vp);
 	if (!IS_ERR_OR_NULL(mixer_res->mixer))
 		clk_put(mixer_res->mixer);
-	mixer_res->dev = NULL;
 	return ret;
 }
 
@@ -1035,7 +1015,6 @@ static void mixer_resources_cleanup(struct mixer_context *ctx)
 {
 	struct mixer_resources *res = &ctx->mixer_res;
 
-	disable_irq(res->irq);
 	free_irq(res->irq, ctx);
 
 	iounmap(res->vp_regs);
@@ -1064,6 +1043,9 @@ static int __devinit mixer_probe(struct platform_device *pdev)
 		return -ENOMEM;
 	}
 
+	mutex_init(&ctx->mixer_mutex);
+
+	ctx->dev = &pdev->dev;
 	drm_hdmi_ctx->ctx = (void *)ctx;
 
 	platform_set_drvdata(pdev, drm_hdmi_ctx);
@@ -1076,7 +1058,7 @@ static int __devinit mixer_probe(struct platform_device *pdev)
 	/* register specific callback point to common hdmi. */
 	exynos_mixer_ops_register(&mixer_ops);
 
-	mixer_resource_poweron(ctx);
+	pm_runtime_enable(dev);
 
 	return 0;
 
@@ -1095,12 +1077,27 @@ static int mixer_remove(struct platform_device *pdev)
 
 	dev_info(dev, "remove successful\n");
 
-	mixer_resource_poweroff(ctx);
+	pm_runtime_disable(&pdev->dev);
+
 	mixer_resources_cleanup(ctx);
 
 	return 0;
 }
 
+#ifdef CONFIG_PM_SLEEP
+static int mixer_suspend(struct device *dev)
+{
+	struct exynos_drm_hdmi_context *drm_hdmi_ctx = get_mixer_context(dev);
+	struct mixer_context *ctx = drm_hdmi_ctx->ctx;
+
+	mixer_poweroff(ctx);
+
+	return 0;
+}
+#endif
+
+static SIMPLE_DEV_PM_OPS(mixer_pm_ops, mixer_suspend, NULL);
+
 struct platform_driver mixer_driver = {
 	.driver = {
 		.name = "s5p-mixer",
diff --git a/drivers/gpu/drm/exynos/regs-hdmi.h b/drivers/gpu/drm/exynos/regs-hdmi.h
index 3c04bea842c..9cc7c5e9718 100644
--- a/drivers/gpu/drm/exynos/regs-hdmi.h
+++ b/drivers/gpu/drm/exynos/regs-hdmi.h
@@ -138,14 +138,16 @@
 #define HDMI_ASP_MASK			(1 << 2)
 #define HDMI_EN				(1 << 0)
 
+/* HDMI_CON_2 */
+#define HDMI_VID_PREAMBLE_DIS		(1 << 5)
+#define HDMI_GUARD_BAND_DIS		(1 << 1)
+
 /* HDMI_PHY_STATUS */
 #define HDMI_PHY_STATUS_READY		(1 << 0)
 
 /* HDMI_MODE_SEL */
 #define HDMI_MODE_HDMI_EN		(1 << 1)
 #define HDMI_MODE_DVI_EN		(1 << 0)
-#define HDMI_DVI_MODE_EN		(1)
-#define HDMI_DVI_MODE_DIS		(0)
 #define HDMI_MODE_MASK			(3 << 0)
 
 /* HDMI_TG_CMD */
diff --git a/drivers/gpu/drm/gma500/Makefile b/drivers/gpu/drm/gma500/Makefile
index 1583982917c..abfa2a93f0d 100644
--- a/drivers/gpu/drm/gma500/Makefile
+++ b/drivers/gpu/drm/gma500/Makefile
@@ -1,7 +1,7 @@
 #
 #	KMS driver for the GMA500
 #
-ccflags-y += -Iinclude/drm
+ccflags-y += -I$(srctree)/include/drm
 
 gma500_gfx-y += gem_glue.o \
 	  accel_2d.o \
@@ -12,7 +12,6 @@ gma500_gfx-y += gem_glue.o \
 	  intel_bios.o \
 	  intel_i2c.o \
 	  intel_gmbus.o \
-	  intel_opregion.o \
 	  mmu.o \
 	  power.o \
 	  psb_drv.o \
@@ -25,6 +24,8 @@ gma500_gfx-y += gem_glue.o \
 	  psb_device.o \
 	  mid_bios.o
 
+gma500_gfx-$(CONFIG_ACPI) +=  opregion.o \
+
 gma500_gfx-$(CONFIG_DRM_GMA3600) +=  cdv_device.o \
 	  cdv_intel_crt.o \
 	  cdv_intel_display.o \
diff --git a/drivers/gpu/drm/gma500/cdv_device.c b/drivers/gpu/drm/gma500/cdv_device.c
index a54cc738926..9764045428c 100644
--- a/drivers/gpu/drm/gma500/cdv_device.c
+++ b/drivers/gpu/drm/gma500/cdv_device.c
@@ -49,13 +49,15 @@ static void cdv_disable_vga(struct drm_device *dev)
 static int cdv_output_init(struct drm_device *dev)
 {
 	struct drm_psb_private *dev_priv = dev->dev_private;
+
+	drm_mode_create_scaling_mode_property(dev);
+
 	cdv_disable_vga(dev);
 
 	cdv_intel_crt_init(dev, &dev_priv->mode_dev);
 	cdv_intel_lvds_init(dev, &dev_priv->mode_dev);
 
-	/* These bits indicate HDMI not SDVO on CDV, but we don't yet support
-	   the HDMI interface */
+	/* These bits indicate HDMI not SDVO on CDV */
 	if (REG_READ(SDVOB) & SDVO_DETECTED)
 		cdv_hdmi_init(dev, &dev_priv->mode_dev, SDVOB);
 	if (REG_READ(SDVOC) & SDVO_DETECTED)
@@ -66,76 +68,71 @@ static int cdv_output_init(struct drm_device *dev)
 #ifdef CONFIG_BACKLIGHT_CLASS_DEVICE
 
 /*
- *	Poulsbo Backlight Interfaces
+ *	Cedartrail Backlght Interfaces
  */
 
-#define BLC_PWM_PRECISION_FACTOR 100	/* 10000000 */
-#define BLC_PWM_FREQ_CALC_CONSTANT 32
-#define MHz 1000000
-
-#define PSB_BLC_PWM_PRECISION_FACTOR    10
-#define PSB_BLC_MAX_PWM_REG_FREQ        0xFFFE
-#define PSB_BLC_MIN_PWM_REG_FREQ        0x2
-
-#define PSB_BACKLIGHT_PWM_POLARITY_BIT_CLEAR (0xFFFE)
-#define PSB_BACKLIGHT_PWM_CTL_SHIFT	(16)
-
-static int cdv_brightness;
 static struct backlight_device *cdv_backlight_device;
 
-static int cdv_get_brightness(struct backlight_device *bd)
+static int cdv_backlight_combination_mode(struct drm_device *dev)
 {
-	/* return locally cached var instead of HW read (due to DPST etc.) */
-	/* FIXME: ideally return actual value in case firmware fiddled with
-	   it */
-	return cdv_brightness;
+	return REG_READ(BLC_PWM_CTL2) & PWM_LEGACY_MODE;
 }
 
-
-static int cdv_backlight_setup(struct drm_device *dev)
+static int cdv_get_brightness(struct backlight_device *bd)
 {
-	struct drm_psb_private *dev_priv = dev->dev_private;
-	unsigned long core_clock;
-	/* u32 bl_max_freq; */
-	/* unsigned long value; */
-	u16 bl_max_freq;
-	uint32_t value;
-	uint32_t blc_pwm_precision_factor;
-
-	/* get bl_max_freq and pol from dev_priv*/
-	if (!dev_priv->lvds_bl) {
-		dev_err(dev->dev, "Has no valid LVDS backlight info\n");
-		return -ENOENT;
-	}
-	bl_max_freq = dev_priv->lvds_bl->freq;
-	blc_pwm_precision_factor = PSB_BLC_PWM_PRECISION_FACTOR;
+	struct drm_device *dev = bl_get_data(bd);
+	u32 val = REG_READ(BLC_PWM_CTL) & BACKLIGHT_DUTY_CYCLE_MASK;
 
-	core_clock = dev_priv->core_freq;
+	if (cdv_backlight_combination_mode(dev)) {
+		u8 lbpc;
 
-	value = (core_clock * MHz) / BLC_PWM_FREQ_CALC_CONSTANT;
-	value *= blc_pwm_precision_factor;
-	value /= bl_max_freq;
-	value /= blc_pwm_precision_factor;
+		val &= ~1;
+		pci_read_config_byte(dev->pdev, 0xF4, &lbpc);
+		val *= lbpc;
+	}
+	return val;
+}
 
-	if (value > (unsigned long long)PSB_BLC_MAX_PWM_REG_FREQ ||
-		 value < (unsigned long long)PSB_BLC_MIN_PWM_REG_FREQ)
-				return -ERANGE;
-	else {
-		/* FIXME */
+static u32 cdv_get_max_backlight(struct drm_device *dev)
+{
+	u32 max = REG_READ(BLC_PWM_CTL);
+
+	if (max == 0) {
+		DRM_DEBUG_KMS("LVDS Panel PWM value is 0!\n");
+		/* i915 does this, I believe which means that we should not
+		 * smash PWM control as firmware will take control of it. */
+		return 1;
 	}
-	return 0;
+
+	max >>= 16;
+	if (cdv_backlight_combination_mode(dev))
+		max *= 0xff;
+	return max;
 }
 
 static int cdv_set_brightness(struct backlight_device *bd)
 {
+	struct drm_device *dev = bl_get_data(bd);
 	int level = bd->props.brightness;
+	u32 blc_pwm_ctl;
 
 	/* Percentage 1-100% being valid */
 	if (level < 1)
 		level = 1;
 
-	/*cdv_intel_lvds_set_brightness(dev, level); FIXME */
-	cdv_brightness = level;
+	if (cdv_backlight_combination_mode(dev)) {
+		u32 max = cdv_get_max_backlight(dev);
+		u8 lbpc;
+
+		lbpc = level * 0xfe / max + 1;
+		level /= lbpc;
+
+		pci_write_config_byte(dev->pdev, 0xF4, lbpc);
+	}
+
+	blc_pwm_ctl = REG_READ(BLC_PWM_CTL) & ~BACKLIGHT_DUTY_CYCLE_MASK;
+	REG_WRITE(BLC_PWM_CTL, (blc_pwm_ctl |
+				(level << BACKLIGHT_DUTY_CYCLE_SHIFT)));
 	return 0;
 }
 
@@ -147,7 +144,6 @@ static const struct backlight_ops cdv_ops = {
 static int cdv_backlight_init(struct drm_device *dev)
 {
 	struct drm_psb_private *dev_priv = dev->dev_private;
-	int ret;
 	struct backlight_properties props;
 
 	memset(&props, 0, sizeof(struct backlight_properties));
@@ -159,14 +155,9 @@ static int cdv_backlight_init(struct drm_device *dev)
 	if (IS_ERR(cdv_backlight_device))
 		return PTR_ERR(cdv_backlight_device);
 
-	ret = cdv_backlight_setup(dev);
-	if (ret < 0) {
-		backlight_device_unregister(cdv_backlight_device);
-		cdv_backlight_device = NULL;
-		return ret;
-	}
-	cdv_backlight_device->props.brightness = 100;
-	cdv_backlight_device->props.max_brightness = 100;
+	cdv_backlight_device->props.brightness =
+			cdv_get_brightness(cdv_backlight_device);
+	cdv_backlight_device->props.max_brightness = cdv_get_max_backlight(dev);
 	backlight_update_status(cdv_backlight_device);
 	dev_priv->backlight_device = cdv_backlight_device;
 	return 0;
@@ -238,6 +229,19 @@ static void cdv_init_pm(struct drm_device *dev)
 	dev_err(dev->dev, "GPU: power management timed out.\n");
 }
 
+static void cdv_errata(struct drm_device *dev)
+{
+	/* Disable bonus launch.
+	 *	CPU and GPU competes for memory and display misses updates and
+	 *	flickers. Worst with dual core, dual displays.
+	 *
+	 *	Fixes were done to Win 7 gfx driver to disable a feature called
+	 *	Bonus Launch to work around the issue, by degrading
+	 *	performance.
+	 */
+	 CDV_MSG_WRITE32(3, 0x30, 0x08027108);
+}
+
 /**
  *	cdv_save_display_registers	-	save registers lost on suspend
  *	@dev: our DRM device
@@ -251,7 +255,7 @@ static int cdv_save_display_registers(struct drm_device *dev)
 	struct psb_save_area *regs = &dev_priv->regs;
 	struct drm_connector *connector;
 
-	dev_info(dev->dev, "Saving GPU registers.\n");
+	dev_dbg(dev->dev, "Saving GPU registers.\n");
 
 	pci_read_config_byte(dev->pdev, 0xF4, &regs->cdv.saveLBB);
 
@@ -355,7 +359,7 @@ static int cdv_restore_display_registers(struct drm_device *dev)
 	REG_WRITE(PSB_INT_MASK_R, regs->cdv.saveIMR);
 
 	/* Fix arbitration bug */
-	CDV_MSG_WRITE32(3, 0x30, 0x08027108);
+	cdv_errata(dev);
 
 	drm_mode_config_reset(dev);
 
@@ -447,13 +451,106 @@ static void cdv_get_core_freq(struct drm_device *dev)
 	}
 }
 
+static void cdv_hotplug_work_func(struct work_struct *work)
+{
+        struct drm_psb_private *dev_priv = container_of(work, struct drm_psb_private,
+							hotplug_work);                 
+        struct drm_device *dev = dev_priv->dev;
+
+        /* Just fire off a uevent and let userspace tell us what to do */
+        drm_helper_hpd_irq_event(dev);
+}                       
+
+/* The core driver has received a hotplug IRQ. We are in IRQ context
+   so extract the needed information and kick off queued processing */
+   
+static int cdv_hotplug_event(struct drm_device *dev)
+{
+	struct drm_psb_private *dev_priv = dev->dev_private;
+	schedule_work(&dev_priv->hotplug_work);
+	REG_WRITE(PORT_HOTPLUG_STAT, REG_READ(PORT_HOTPLUG_STAT));
+	return 1;
+}
+
+static void cdv_hotplug_enable(struct drm_device *dev, bool on)
+{
+	if (on) {
+		u32 hotplug = REG_READ(PORT_HOTPLUG_EN);
+		hotplug |= HDMIB_HOTPLUG_INT_EN | HDMIC_HOTPLUG_INT_EN |
+			   HDMID_HOTPLUG_INT_EN | CRT_HOTPLUG_INT_EN;
+		REG_WRITE(PORT_HOTPLUG_EN, hotplug);
+	}  else {
+		REG_WRITE(PORT_HOTPLUG_EN, 0);
+		REG_WRITE(PORT_HOTPLUG_STAT, REG_READ(PORT_HOTPLUG_STAT));
+	}	
+}
+
+/* Cedarview */
+static const struct psb_offset cdv_regmap[2] = {
+	{
+		.fp0 = FPA0,
+		.fp1 = FPA1,
+		.cntr = DSPACNTR,
+		.conf = PIPEACONF,
+		.src = PIPEASRC,
+		.dpll = DPLL_A,
+		.dpll_md = DPLL_A_MD,
+		.htotal = HTOTAL_A,
+		.hblank = HBLANK_A,
+		.hsync = HSYNC_A,
+		.vtotal = VTOTAL_A,
+		.vblank = VBLANK_A,
+		.vsync = VSYNC_A,
+		.stride = DSPASTRIDE,
+		.size = DSPASIZE,
+		.pos = DSPAPOS,
+		.base = DSPABASE,
+		.surf = DSPASURF,
+		.addr = DSPABASE,
+		.status = PIPEASTAT,
+		.linoff = DSPALINOFF,
+		.tileoff = DSPATILEOFF,
+		.palette = PALETTE_A,
+	},
+	{
+		.fp0 = FPB0,
+		.fp1 = FPB1,
+		.cntr = DSPBCNTR,
+		.conf = PIPEBCONF,
+		.src = PIPEBSRC,
+		.dpll = DPLL_B,
+		.dpll_md = DPLL_B_MD,
+		.htotal = HTOTAL_B,
+		.hblank = HBLANK_B,
+		.hsync = HSYNC_B,
+		.vtotal = VTOTAL_B,
+		.vblank = VBLANK_B,
+		.vsync = VSYNC_B,
+		.stride = DSPBSTRIDE,
+		.size = DSPBSIZE,
+		.pos = DSPBPOS,
+		.base = DSPBBASE,
+		.surf = DSPBSURF,
+		.addr = DSPBBASE,
+		.status = PIPEBSTAT,
+		.linoff = DSPBLINOFF,
+		.tileoff = DSPBTILEOFF,
+		.palette = PALETTE_B,
+	}
+};
+
 static int cdv_chip_setup(struct drm_device *dev)
 {
+	struct drm_psb_private *dev_priv = dev->dev_private;
+	INIT_WORK(&dev_priv->hotplug_work, cdv_hotplug_work_func);
+
+	if (pci_enable_msi(dev->pdev))
+		dev_warn(dev->dev, "Enabling MSI failed!\n");
+	dev_priv->regmap = cdv_regmap;
 	cdv_get_core_freq(dev);
-	gma_intel_opregion_init(dev);
+	psb_intel_opregion_init(dev);
 	psb_intel_init_bios(dev);
-	REG_WRITE(PORT_HOTPLUG_EN, 0);
-	REG_WRITE(PORT_HOTPLUG_STAT, REG_READ(PORT_HOTPLUG_STAT));
+	cdv_hotplug_enable(dev, false);
 	return 0;
 }
 
@@ -464,13 +561,19 @@ const struct psb_ops cdv_chip_ops = {
 	.accel_2d = 0,
 	.pipes = 2,
 	.crtcs = 2,
+	.hdmi_mask = (1 << 0) | (1 << 1),
+	.lvds_mask = (1 << 1),
+	.cursor_needs_phys = 0,
 	.sgx_offset = MRST_SGX_OFFSET,
 	.chip_setup = cdv_chip_setup,
+	.errata = cdv_errata,
 
 	.crtc_helper = &cdv_intel_helper_funcs,
 	.crtc_funcs = &cdv_intel_crtc_funcs,
 
 	.output_init = cdv_output_init,
+	.hotplug = cdv_hotplug_event,
+	.hotplug_enable = cdv_hotplug_enable,
 
 #ifdef CONFIG_BACKLIGHT_CLASS_DEVICE
 	.backlight_init = cdv_backlight_init,
diff --git a/drivers/gpu/drm/gma500/cdv_intel_crt.c b/drivers/gpu/drm/gma500/cdv_intel_crt.c
index a71a6cd95bd..18742201860 100644
--- a/drivers/gpu/drm/gma500/cdv_intel_crt.c
+++ b/drivers/gpu/drm/gma500/cdv_intel_crt.c
@@ -67,8 +67,6 @@ static void cdv_intel_crt_dpms(struct drm_encoder *encoder, int mode)
 static int cdv_intel_crt_mode_valid(struct drm_connector *connector,
 				struct drm_display_mode *mode)
 {
-	struct drm_psb_private *dev_priv = connector->dev->dev_private;
-	int max_clock = 0;
 	if (mode->flags & DRM_MODE_FLAG_DBLSCAN)
 		return MODE_NO_DBLESCAN;
 
@@ -77,18 +75,9 @@ static int cdv_intel_crt_mode_valid(struct drm_connector *connector,
 		return MODE_CLOCK_LOW;
 
 	/* The max clock for CDV is 355 instead of 400 */
-	max_clock = 355000;
-	if (mode->clock > max_clock)
+	if (mode->clock > 355000)
 		return MODE_CLOCK_HIGH;
 
-	if (mode->hdisplay > 1680 || mode->vdisplay > 1050)
-		return MODE_PANEL;
-
-	/* We assume worst case scenario of 32 bpp here, since we don't know */
-	if ((ALIGN(mode->hdisplay * 4, 64) * mode->vdisplay) >
-	    dev_priv->vram_stolen_size)
-		return MODE_MEM;
-
 	return MODE_OK;
 }
 
@@ -156,13 +145,7 @@ static bool cdv_intel_crt_detect_hotplug(struct drm_connector *connector,
 	struct drm_device *dev = connector->dev;
 	u32 hotplug_en;
 	int i, tries = 0, ret = false;
-	u32 adpa_orig;
-
-	/* disable the DAC when doing the hotplug detection */
-
-	adpa_orig = REG_READ(ADPA);
-
-	REG_WRITE(ADPA, adpa_orig & ~(ADPA_DAC_ENABLE));
+	u32 orig;
 
 	/*
 	 * On a CDV thep, CRT detect sequence need to be done twice
@@ -170,7 +153,7 @@ static bool cdv_intel_crt_detect_hotplug(struct drm_connector *connector,
 	 */
 	tries = 2;
 
-	hotplug_en = REG_READ(PORT_HOTPLUG_EN);
+	orig = hotplug_en = REG_READ(PORT_HOTPLUG_EN);
 	hotplug_en &= ~(CRT_HOTPLUG_DETECT_MASK);
 	hotplug_en |= CRT_HOTPLUG_FORCE_DETECT;
 
@@ -195,8 +178,11 @@ static bool cdv_intel_crt_detect_hotplug(struct drm_connector *connector,
 	    CRT_HOTPLUG_MONITOR_NONE)
 		ret = true;
 
-	/* Restore the saved ADPA */
-	REG_WRITE(ADPA, adpa_orig);
+	 /* clear the interrupt we just generated, if any */
+	REG_WRITE(PORT_HOTPLUG_STAT, CRT_HOTPLUG_INT_STATUS);
+
+	/* and put the bits back */
+	REG_WRITE(PORT_HOTPLUG_EN, orig);
 	return ret;
 }
 
diff --git a/drivers/gpu/drm/gma500/cdv_intel_display.c b/drivers/gpu/drm/gma500/cdv_intel_display.c
index be8455919b3..c3e9a0f701d 100644
--- a/drivers/gpu/drm/gma500/cdv_intel_display.c
+++ b/drivers/gpu/drm/gma500/cdv_intel_display.c
@@ -216,22 +216,22 @@ static void cdv_sb_reset(struct drm_device *dev)
  */
 static int
 cdv_dpll_set_clock_cdv(struct drm_device *dev, struct drm_crtc *crtc,
-			       struct cdv_intel_clock_t *clock)
+			       struct cdv_intel_clock_t *clock, bool is_lvds)
 {
-	struct psb_intel_crtc *psb_crtc =
-				to_psb_intel_crtc(crtc);
+	struct psb_intel_crtc *psb_crtc = to_psb_intel_crtc(crtc);
 	int pipe = psb_crtc->pipe;
 	u32 m, n_vco, p;
 	int ret = 0;
 	int dpll_reg = (pipe == 0) ? DPLL_A : DPLL_B;
+	int ref_sfr = (pipe == 0) ? SB_REF_DPLLA : SB_REF_DPLLB;
 	u32 ref_value;
+	u32 lane_reg, lane_value;
 
 	cdv_sb_reset(dev);
 
-	if ((REG_READ(dpll_reg) & DPLL_SYNCLOCK_ENABLE) == 0) {
-		DRM_ERROR("Attempting to set DPLL with refclk disabled\n");
-		return -EBUSY;
-	}
+	REG_WRITE(dpll_reg, DPLL_SYNCLOCK_ENABLE | DPLL_VGA_MODE_DIS);
+
+	udelay(100);
 
 	/* Follow the BIOS and write the REF/SFR Register. Hardcoded value */
 	ref_value = 0x68A701;
@@ -241,6 +241,35 @@ cdv_dpll_set_clock_cdv(struct drm_device *dev, struct drm_crtc *crtc,
 	/* We don't know what the other fields of these regs are, so
 	 * leave them in place.
 	 */
+	/* 
+	 * The BIT 14:13 of 0x8010/0x8030 is used to select the ref clk
+	 * for the pipe A/B. Display spec 1.06 has wrong definition.
+	 * Correct definition is like below:
+	 *
+	 * refclka mean use clock from same PLL
+	 *
+	 * if DPLLA sets 01 and DPLLB sets 01, they use clock from their pll
+	 *
+	 * if DPLLA sets 01 and DPLLB sets 02, both use clk from DPLLA
+	 *
+	 */  
+	ret = cdv_sb_read(dev, ref_sfr, &ref_value);
+	if (ret)
+		return ret;
+	ref_value &= ~(REF_CLK_MASK);
+
+	/* use DPLL_A for pipeB on CRT/HDMI */
+	if (pipe == 1 && !is_lvds) {
+		DRM_DEBUG_KMS("use DPLLA for pipe B\n");
+		ref_value |= REF_CLK_DPLLA;
+	} else {
+		DRM_DEBUG_KMS("use their DPLL for pipe A/B\n");
+		ref_value |= REF_CLK_DPLL;
+	}
+	ret = cdv_sb_write(dev, ref_sfr, ref_value);
+	if (ret)
+		return ret;
+
 	ret = cdv_sb_read(dev, SB_M(pipe), &m);
 	if (ret)
 		return ret;
@@ -307,36 +336,29 @@ cdv_dpll_set_clock_cdv(struct drm_device *dev, struct drm_crtc *crtc,
 	if (ret)
 		return ret;
 
-	/* always Program the Lane Register for the Pipe A*/
-	if (pipe == 0) {
-		/* Program the Lane0/1 for HDMI B */
-		u32 lane_reg, lane_value;
-
-		lane_reg = PSB_LANE0;
-		cdv_sb_read(dev, lane_reg, &lane_value);
-		lane_value &= ~(LANE_PLL_MASK);
-		lane_value |= LANE_PLL_ENABLE;
-		cdv_sb_write(dev, lane_reg, lane_value);
-
-		lane_reg = PSB_LANE1;
-		cdv_sb_read(dev, lane_reg, &lane_value);
-		lane_value &= ~(LANE_PLL_MASK);
-		lane_value |= LANE_PLL_ENABLE;
-		cdv_sb_write(dev, lane_reg, lane_value);
-
-		/* Program the Lane2/3 for HDMI C */
-		lane_reg = PSB_LANE2;
-		cdv_sb_read(dev, lane_reg, &lane_value);
-		lane_value &= ~(LANE_PLL_MASK);
-		lane_value |= LANE_PLL_ENABLE;
-		cdv_sb_write(dev, lane_reg, lane_value);
-
-		lane_reg = PSB_LANE3;
-		cdv_sb_read(dev, lane_reg, &lane_value);
-		lane_value &= ~(LANE_PLL_MASK);
-		lane_value |= LANE_PLL_ENABLE;
-		cdv_sb_write(dev, lane_reg, lane_value);
-	}
+	lane_reg = PSB_LANE0;
+	cdv_sb_read(dev, lane_reg, &lane_value);
+	lane_value &= ~(LANE_PLL_MASK);
+	lane_value |= LANE_PLL_ENABLE | LANE_PLL_PIPE(pipe);
+	cdv_sb_write(dev, lane_reg, lane_value);
+
+	lane_reg = PSB_LANE1;
+	cdv_sb_read(dev, lane_reg, &lane_value);
+	lane_value &= ~(LANE_PLL_MASK);
+	lane_value |= LANE_PLL_ENABLE | LANE_PLL_PIPE(pipe);
+	cdv_sb_write(dev, lane_reg, lane_value);
+
+	lane_reg = PSB_LANE2;
+	cdv_sb_read(dev, lane_reg, &lane_value);
+	lane_value &= ~(LANE_PLL_MASK);
+	lane_value |= LANE_PLL_ENABLE | LANE_PLL_PIPE(pipe);
+	cdv_sb_write(dev, lane_reg, lane_value);
+
+	lane_reg = PSB_LANE3;
+	cdv_sb_read(dev, lane_reg, &lane_value);
+	lane_value &= ~(LANE_PLL_MASK);
+	lane_value |= LANE_PLL_ENABLE | LANE_PLL_PIPE(pipe);
+	cdv_sb_write(dev, lane_reg, lane_value);
 
 	return 0;
 }
@@ -480,14 +502,12 @@ static int cdv_intel_pipe_set_base(struct drm_crtc *crtc,
 			    int x, int y, struct drm_framebuffer *old_fb)
 {
 	struct drm_device *dev = crtc->dev;
+	struct drm_psb_private *dev_priv = dev->dev_private;
 	struct psb_intel_crtc *psb_intel_crtc = to_psb_intel_crtc(crtc);
 	struct psb_framebuffer *psbfb = to_psb_fb(crtc->fb);
 	int pipe = psb_intel_crtc->pipe;
+	const struct psb_offset *map = &dev_priv->regmap[pipe];
 	unsigned long start, offset;
-	int dspbase = (pipe == 0 ? DSPABASE : DSPBBASE);
-	int dspsurf = (pipe == 0 ? DSPASURF : DSPBSURF);
-	int dspstride = (pipe == 0) ? DSPASTRIDE : DSPBSTRIDE;
-	int dspcntr_reg = (pipe == 0) ? DSPACNTR : DSPBCNTR;
 	u32 dspcntr;
 	int ret = 0;
 
@@ -509,9 +529,9 @@ static int cdv_intel_pipe_set_base(struct drm_crtc *crtc,
 	start = psbfb->gtt->offset;
 	offset = y * crtc->fb->pitches[0] + x * (crtc->fb->bits_per_pixel / 8);
 
-	REG_WRITE(dspstride, crtc->fb->pitches[0]);
+	REG_WRITE(map->stride, crtc->fb->pitches[0]);
 
-	dspcntr = REG_READ(dspcntr_reg);
+	dspcntr = REG_READ(map->cntr);
 	dspcntr &= ~DISPPLANE_PIXFORMAT_MASK;
 
 	switch (crtc->fb->bits_per_pixel) {
@@ -533,15 +553,15 @@ static int cdv_intel_pipe_set_base(struct drm_crtc *crtc,
 		ret = -EINVAL;
 		goto psb_intel_pipe_set_base_exit;
 	}
-	REG_WRITE(dspcntr_reg, dspcntr);
+	REG_WRITE(map->cntr, dspcntr);
 
 	dev_dbg(dev->dev,
 		"Writing base %08lX %08lX %d %d\n", start, offset, x, y);
 
-	REG_WRITE(dspbase, offset);
-	REG_READ(dspbase);
-	REG_WRITE(dspsurf, start);
-	REG_READ(dspsurf);
+	REG_WRITE(map->base, offset);
+	REG_READ(map->base);
+	REG_WRITE(map->surf, start);
+	REG_READ(map->surf);
 
 psb_intel_pipe_cleaner:
 	/* If there was a previous display we can now unpin it */
@@ -553,6 +573,199 @@ psb_intel_pipe_set_base_exit:
 	return ret;
 }
 
+#define		FIFO_PIPEA		(1 << 0)
+#define		FIFO_PIPEB		(1 << 1)
+
+static bool cdv_intel_pipe_enabled(struct drm_device *dev, int pipe)
+{
+	struct drm_crtc *crtc;
+	struct drm_psb_private *dev_priv = dev->dev_private;
+	struct psb_intel_crtc *psb_intel_crtc = NULL;
+
+	crtc = dev_priv->pipe_to_crtc_mapping[pipe];
+	psb_intel_crtc = to_psb_intel_crtc(crtc);
+
+	if (crtc->fb == NULL || !psb_intel_crtc->active)
+		return false;
+	return true;
+}
+
+static bool cdv_intel_single_pipe_active (struct drm_device *dev)
+{
+	uint32_t pipe_enabled = 0;
+
+	if (cdv_intel_pipe_enabled(dev, 0))
+		pipe_enabled |= FIFO_PIPEA;
+
+	if (cdv_intel_pipe_enabled(dev, 1))
+		pipe_enabled |= FIFO_PIPEB;
+
+
+	DRM_DEBUG_KMS("pipe enabled %x\n", pipe_enabled);
+
+	if (pipe_enabled == FIFO_PIPEA || pipe_enabled == FIFO_PIPEB)
+		return true;
+	else
+		return false;
+}
+
+static bool is_pipeb_lvds(struct drm_device *dev, struct drm_crtc *crtc)
+{
+	struct psb_intel_crtc *psb_intel_crtc = to_psb_intel_crtc(crtc);
+	struct drm_mode_config *mode_config = &dev->mode_config;
+	struct drm_connector *connector;
+
+	if (psb_intel_crtc->pipe != 1)
+		return false;
+
+	list_for_each_entry(connector, &mode_config->connector_list, head) {
+		struct psb_intel_encoder *psb_intel_encoder =
+					psb_intel_attached_encoder(connector);
+
+		if (!connector->encoder
+		    || connector->encoder->crtc != crtc)
+			continue;
+
+		if (psb_intel_encoder->type == INTEL_OUTPUT_LVDS)
+			return true;
+	}
+
+	return false;
+}
+
+static void cdv_intel_disable_self_refresh (struct drm_device *dev)
+{
+	if (REG_READ(FW_BLC_SELF) & FW_BLC_SELF_EN) {
+
+		/* Disable self-refresh before adjust WM */
+		REG_WRITE(FW_BLC_SELF, (REG_READ(FW_BLC_SELF) & ~FW_BLC_SELF_EN));
+		REG_READ(FW_BLC_SELF);
+
+		cdv_intel_wait_for_vblank(dev);
+
+		/* Cedarview workaround to write ovelay plane, which force to leave
+		 * MAX_FIFO state.
+		 */
+		REG_WRITE(OV_OVADD, 0/*dev_priv->ovl_offset*/);
+		REG_READ(OV_OVADD);
+
+		cdv_intel_wait_for_vblank(dev);
+	}
+
+}
+
+static void cdv_intel_update_watermark (struct drm_device *dev, struct drm_crtc *crtc)
+{
+
+	if (cdv_intel_single_pipe_active(dev)) {
+		u32 fw;
+
+		fw = REG_READ(DSPFW1);
+		fw &= ~DSP_FIFO_SR_WM_MASK;
+		fw |= (0x7e << DSP_FIFO_SR_WM_SHIFT);
+		fw &= ~CURSOR_B_FIFO_WM_MASK;
+		fw |= (0x4 << CURSOR_B_FIFO_WM_SHIFT);
+		REG_WRITE(DSPFW1, fw);
+
+		fw = REG_READ(DSPFW2);
+		fw &= ~CURSOR_A_FIFO_WM_MASK;
+		fw |= (0x6 << CURSOR_A_FIFO_WM_SHIFT);
+		fw &= ~DSP_PLANE_C_FIFO_WM_MASK;
+		fw |= (0x8 << DSP_PLANE_C_FIFO_WM_SHIFT);
+		REG_WRITE(DSPFW2, fw);
+
+		REG_WRITE(DSPFW3, 0x36000000);
+
+		/* ignore FW4 */
+
+		if (is_pipeb_lvds(dev, crtc)) {
+			REG_WRITE(DSPFW5, 0x00040330);
+		} else {
+			fw = (3 << DSP_PLANE_B_FIFO_WM1_SHIFT) |
+			     (4 << DSP_PLANE_A_FIFO_WM1_SHIFT) |
+			     (3 << CURSOR_B_FIFO_WM1_SHIFT) |
+			     (4 << CURSOR_FIFO_SR_WM1_SHIFT);
+			REG_WRITE(DSPFW5, fw);
+		}
+
+		REG_WRITE(DSPFW6, 0x10);
+
+		cdv_intel_wait_for_vblank(dev);
+
+		/* enable self-refresh for single pipe active */
+		REG_WRITE(FW_BLC_SELF, FW_BLC_SELF_EN);
+		REG_READ(FW_BLC_SELF);
+		cdv_intel_wait_for_vblank(dev);
+
+	} else {
+
+		/* HW team suggested values... */
+		REG_WRITE(DSPFW1, 0x3f880808);
+		REG_WRITE(DSPFW2, 0x0b020202);
+		REG_WRITE(DSPFW3, 0x24000000);
+		REG_WRITE(DSPFW4, 0x08030202);
+		REG_WRITE(DSPFW5, 0x01010101);
+		REG_WRITE(DSPFW6, 0x1d0);
+
+		cdv_intel_wait_for_vblank(dev);
+
+		cdv_intel_disable_self_refresh(dev);
+	
+	}
+}
+
+/** Loads the palette/gamma unit for the CRTC with the prepared values */
+static void cdv_intel_crtc_load_lut(struct drm_crtc *crtc)
+{
+	struct drm_device *dev = crtc->dev;
+	struct drm_psb_private *dev_priv = dev->dev_private;
+	struct psb_intel_crtc *psb_intel_crtc = to_psb_intel_crtc(crtc);
+	int palreg = PALETTE_A;
+	int i;
+
+	/* The clocks have to be on to load the palette. */
+	if (!crtc->enabled)
+		return;
+
+	switch (psb_intel_crtc->pipe) {
+	case 0:
+		break;
+	case 1:
+		palreg = PALETTE_B;
+		break;
+	case 2:
+		palreg = PALETTE_C;
+		break;
+	default:
+		dev_err(dev->dev, "Illegal Pipe Number.\n");
+		return;
+	}
+
+	if (gma_power_begin(dev, false)) {
+		for (i = 0; i < 256; i++) {
+			REG_WRITE(palreg + 4 * i,
+				  ((psb_intel_crtc->lut_r[i] +
+				  psb_intel_crtc->lut_adj[i]) << 16) |
+				  ((psb_intel_crtc->lut_g[i] +
+				  psb_intel_crtc->lut_adj[i]) << 8) |
+				  (psb_intel_crtc->lut_b[i] +
+				  psb_intel_crtc->lut_adj[i]));
+		}
+		gma_power_end(dev);
+	} else {
+		for (i = 0; i < 256; i++) {
+			dev_priv->regs.pipe[0].palette[i] =
+				  ((psb_intel_crtc->lut_r[i] +
+				  psb_intel_crtc->lut_adj[i]) << 16) |
+				  ((psb_intel_crtc->lut_g[i] +
+				  psb_intel_crtc->lut_adj[i]) << 8) |
+				  (psb_intel_crtc->lut_b[i] +
+				  psb_intel_crtc->lut_adj[i]);
+		}
+
+	}
+}
+
 /**
  * Sets the power management mode of the pipe and plane.
  *
@@ -562,62 +775,80 @@ psb_intel_pipe_set_base_exit:
 static void cdv_intel_crtc_dpms(struct drm_crtc *crtc, int mode)
 {
 	struct drm_device *dev = crtc->dev;
+	struct drm_psb_private *dev_priv = dev->dev_private;
 	struct psb_intel_crtc *psb_intel_crtc = to_psb_intel_crtc(crtc);
 	int pipe = psb_intel_crtc->pipe;
-	int dpll_reg = (pipe == 0) ? DPLL_A : DPLL_B;
-	int dspcntr_reg = (pipe == 0) ? DSPACNTR : DSPBCNTR;
-	int dspbase_reg = (pipe == 0) ? DSPABASE : DSPBBASE;
-	int pipeconf_reg = (pipe == 0) ? PIPEACONF : PIPEBCONF;
+	const struct psb_offset *map = &dev_priv->regmap[pipe];
 	u32 temp;
 
 	/* XXX: When our outputs are all unaware of DPMS modes other than off
 	 * and on, we should map those modes to DRM_MODE_DPMS_OFF in the CRTC.
 	 */
+	cdv_intel_disable_self_refresh(dev);
+
 	switch (mode) {
 	case DRM_MODE_DPMS_ON:
 	case DRM_MODE_DPMS_STANDBY:
 	case DRM_MODE_DPMS_SUSPEND:
+		if (psb_intel_crtc->active)
+			return;
+
+		psb_intel_crtc->active = true;
+
 		/* Enable the DPLL */
-		temp = REG_READ(dpll_reg);
+		temp = REG_READ(map->dpll);
 		if ((temp & DPLL_VCO_ENABLE) == 0) {
-			REG_WRITE(dpll_reg, temp);
-			REG_READ(dpll_reg);
+			REG_WRITE(map->dpll, temp);
+			REG_READ(map->dpll);
 			/* Wait for the clocks to stabilize. */
 			udelay(150);
-			REG_WRITE(dpll_reg, temp | DPLL_VCO_ENABLE);
-			REG_READ(dpll_reg);
+			REG_WRITE(map->dpll, temp | DPLL_VCO_ENABLE);
+			REG_READ(map->dpll);
 			/* Wait for the clocks to stabilize. */
 			udelay(150);
-			REG_WRITE(dpll_reg, temp | DPLL_VCO_ENABLE);
-			REG_READ(dpll_reg);
+			REG_WRITE(map->dpll, temp | DPLL_VCO_ENABLE);
+			REG_READ(map->dpll);
 			/* Wait for the clocks to stabilize. */
 			udelay(150);
 		}
 
 		/* Jim Bish - switch plan and pipe per scott */
 		/* Enable the plane */
-		temp = REG_READ(dspcntr_reg);
+		temp = REG_READ(map->cntr);
 		if ((temp & DISPLAY_PLANE_ENABLE) == 0) {
-			REG_WRITE(dspcntr_reg,
+			REG_WRITE(map->cntr,
 				  temp | DISPLAY_PLANE_ENABLE);
 			/* Flush the plane changes */
-			REG_WRITE(dspbase_reg, REG_READ(dspbase_reg));
+			REG_WRITE(map->base, REG_READ(map->base));
 		}
 
 		udelay(150);
 
 		/* Enable the pipe */
-		temp = REG_READ(pipeconf_reg);
+		temp = REG_READ(map->conf);
 		if ((temp & PIPEACONF_ENABLE) == 0)
-			REG_WRITE(pipeconf_reg, temp | PIPEACONF_ENABLE);
+			REG_WRITE(map->conf, temp | PIPEACONF_ENABLE);
+
+		temp = REG_READ(map->status);
+		temp &= ~(0xFFFF);
+		temp |= PIPE_FIFO_UNDERRUN;
+		REG_WRITE(map->status, temp);
+		REG_READ(map->status);
 
-		psb_intel_crtc_load_lut(crtc);
+		cdv_intel_update_watermark(dev, crtc);
+		cdv_intel_crtc_load_lut(crtc);
 
 		/* Give the overlay scaler a chance to enable
 		 * if it's on this pipe */
 		/* psb_intel_crtc_dpms_video(crtc, true); TODO */
+		psb_intel_crtc->crtc_enable = true;
 		break;
 	case DRM_MODE_DPMS_OFF:
+		if (!psb_intel_crtc->active)
+			return;
+
+		psb_intel_crtc->active = false;
+
 		/* Give the overlay scaler a chance to disable
 		 * if it's on this pipe */
 		/* psb_intel_crtc_dpms_video(crtc, FALSE); TODO */
@@ -627,14 +858,15 @@ static void cdv_intel_crtc_dpms(struct drm_crtc *crtc, int mode)
 
 		/* Jim Bish - changed pipe/plane here as well. */
 
+		drm_vblank_off(dev, pipe);
 		/* Wait for vblank for the disable to take effect */
 		cdv_intel_wait_for_vblank(dev);
 
 		/* Next, disable display pipes */
-		temp = REG_READ(pipeconf_reg);
+		temp = REG_READ(map->conf);
 		if ((temp & PIPEACONF_ENABLE) != 0) {
-			REG_WRITE(pipeconf_reg, temp & ~PIPEACONF_ENABLE);
-			REG_READ(pipeconf_reg);
+			REG_WRITE(map->conf, temp & ~PIPEACONF_ENABLE);
+			REG_READ(map->conf);
 		}
 
 		/* Wait for vblank for the disable to take effect. */
@@ -643,23 +875,25 @@ static void cdv_intel_crtc_dpms(struct drm_crtc *crtc, int mode)
 		udelay(150);
 
 		/* Disable display plane */
-		temp = REG_READ(dspcntr_reg);
+		temp = REG_READ(map->cntr);
 		if ((temp & DISPLAY_PLANE_ENABLE) != 0) {
-			REG_WRITE(dspcntr_reg,
+			REG_WRITE(map->cntr,
 				  temp & ~DISPLAY_PLANE_ENABLE);
 			/* Flush the plane changes */
-			REG_WRITE(dspbase_reg, REG_READ(dspbase_reg));
-			REG_READ(dspbase_reg);
+			REG_WRITE(map->base, REG_READ(map->base));
+			REG_READ(map->base);
 		}
 
-		temp = REG_READ(dpll_reg);
+		temp = REG_READ(map->dpll);
 		if ((temp & DPLL_VCO_ENABLE) != 0) {
-			REG_WRITE(dpll_reg, temp & ~DPLL_VCO_ENABLE);
-			REG_READ(dpll_reg);
+			REG_WRITE(map->dpll, temp & ~DPLL_VCO_ENABLE);
+			REG_READ(map->dpll);
 		}
 
 		/* Wait for the clocks to turn off. */
 		udelay(150);
+		cdv_intel_update_watermark(dev, crtc);
+		psb_intel_crtc->crtc_enable = false;
 		break;
 	}
 	/*Set FIFO Watermarks*/
@@ -709,21 +943,10 @@ static int cdv_intel_crtc_mode_set(struct drm_crtc *crtc,
 			       struct drm_framebuffer *old_fb)
 {
 	struct drm_device *dev = crtc->dev;
+	struct drm_psb_private *dev_priv = dev->dev_private;
 	struct psb_intel_crtc *psb_intel_crtc = to_psb_intel_crtc(crtc);
 	int pipe = psb_intel_crtc->pipe;
-	int dpll_reg = (pipe == 0) ? DPLL_A : DPLL_B;
-	int dpll_md_reg = (psb_intel_crtc->pipe == 0) ? DPLL_A_MD : DPLL_B_MD;
-	int dspcntr_reg = (pipe == 0) ? DSPACNTR : DSPBCNTR;
-	int pipeconf_reg = (pipe == 0) ? PIPEACONF : PIPEBCONF;
-	int htot_reg = (pipe == 0) ? HTOTAL_A : HTOTAL_B;
-	int hblank_reg = (pipe == 0) ? HBLANK_A : HBLANK_B;
-	int hsync_reg = (pipe == 0) ? HSYNC_A : HSYNC_B;
-	int vtot_reg = (pipe == 0) ? VTOTAL_A : VTOTAL_B;
-	int vblank_reg = (pipe == 0) ? VBLANK_A : VBLANK_B;
-	int vsync_reg = (pipe == 0) ? VSYNC_A : VSYNC_B;
-	int dspsize_reg = (pipe == 0) ? DSPASIZE : DSPBSIZE;
-	int dsppos_reg = (pipe == 0) ? DSPAPOS : DSPBPOS;
-	int pipesrc_reg = (pipe == 0) ? PIPEASRC : PIPEBSRC;
+	const struct psb_offset *map = &dev_priv->regmap[pipe];
 	int refclk;
 	struct cdv_intel_clock_t clock;
 	u32 dpll = 0, dspcntr, pipeconf;
@@ -757,13 +980,18 @@ static int cdv_intel_crtc_mode_set(struct drm_crtc *crtc,
 		}
 	}
 
-	refclk = 96000;
-
-	/* Hack selection about ref clk for CRT */
-	/* Select 27MHz as the reference clk for HDMI */
-	if (is_crt || is_hdmi)
+	if (dev_priv->dplla_96mhz)
+		/* low-end sku, 96/100 mhz */
+		refclk = 96000;
+	else
+		/* high-end sku, 27/100 mhz */
 		refclk = 27000;
 
+	if (is_lvds && dev_priv->lvds_use_ssc) {
+		refclk = dev_priv->lvds_ssc_freq * 1000;
+		DRM_DEBUG_KMS("Use SSC reference clock %d Mhz\n", dev_priv->lvds_ssc_freq);
+	}
+
 	drm_mode_debug_printmodeline(adjusted_mode);
 
 	ok = cdv_intel_find_best_PLL(crtc, adjusted_mode->clock, refclk,
@@ -779,18 +1007,17 @@ static int cdv_intel_crtc_mode_set(struct drm_crtc *crtc,
 /*	dpll |= PLL_REF_INPUT_TVCLKINBC; */
 		dpll |= 3;
 	}
-		dpll |= PLL_REF_INPUT_DREFCLK;
+/*		dpll |= PLL_REF_INPUT_DREFCLK; */
 
 	dpll |= DPLL_SYNCLOCK_ENABLE;
-	dpll |= DPLL_VGA_MODE_DIS;
-	if (is_lvds)
+/*	if (is_lvds)
 		dpll |= DPLLB_MODE_LVDS;
 	else
-		dpll |= DPLLB_MODE_DAC_SERIAL;
+		dpll |= DPLLB_MODE_DAC_SERIAL; */
 	/* dpll |= (2 << 11); */
 
 	/* setup pipeconf */
-	pipeconf = REG_READ(pipeconf_reg);
+	pipeconf = REG_READ(map->conf);
 
 	/* Set up the display plane register */
 	dspcntr = DISPPLANE_GAMMA_ENABLE;
@@ -803,10 +1030,10 @@ static int cdv_intel_crtc_mode_set(struct drm_crtc *crtc,
 	dspcntr |= DISPLAY_PLANE_ENABLE;
 	pipeconf |= PIPEACONF_ENABLE;
 
-	REG_WRITE(dpll_reg, dpll | DPLL_VGA_MODE_DIS | DPLL_SYNCLOCK_ENABLE);
-	REG_READ(dpll_reg);
+	REG_WRITE(map->dpll, dpll | DPLL_VGA_MODE_DIS | DPLL_SYNCLOCK_ENABLE);
+	REG_READ(map->dpll);
 
-	cdv_dpll_set_clock_cdv(dev, crtc, &clock);
+	cdv_dpll_set_clock_cdv(dev, crtc, &clock, is_lvds);
 
 	udelay(150);
 
@@ -848,48 +1075,48 @@ static int cdv_intel_crtc_mode_set(struct drm_crtc *crtc,
 	DRM_DEBUG_KMS("Mode for pipe %c:\n", pipe == 0 ? 'A' : 'B');
 	drm_mode_debug_printmodeline(mode);
 
-	REG_WRITE(dpll_reg,
-		(REG_READ(dpll_reg) & ~DPLL_LOCK) | DPLL_VCO_ENABLE);
-	REG_READ(dpll_reg);
+	REG_WRITE(map->dpll,
+		(REG_READ(map->dpll) & ~DPLL_LOCK) | DPLL_VCO_ENABLE);
+	REG_READ(map->dpll);
 	/* Wait for the clocks to stabilize. */
 	udelay(150); /* 42 usec w/o calibration, 110 with.  rounded up. */
 
-	if (!(REG_READ(dpll_reg) & DPLL_LOCK)) {
+	if (!(REG_READ(map->dpll) & DPLL_LOCK)) {
 		dev_err(dev->dev, "Failed to get DPLL lock\n");
 		return -EBUSY;
 	}
 
 	{
 		int sdvo_pixel_multiply = adjusted_mode->clock / mode->clock;
-		REG_WRITE(dpll_md_reg, (0 << DPLL_MD_UDI_DIVIDER_SHIFT) | ((sdvo_pixel_multiply - 1) << DPLL_MD_UDI_MULTIPLIER_SHIFT));
+		REG_WRITE(map->dpll_md, (0 << DPLL_MD_UDI_DIVIDER_SHIFT) | ((sdvo_pixel_multiply - 1) << DPLL_MD_UDI_MULTIPLIER_SHIFT));
 	}
 
-	REG_WRITE(htot_reg, (adjusted_mode->crtc_hdisplay - 1) |
+	REG_WRITE(map->htotal, (adjusted_mode->crtc_hdisplay - 1) |
 		  ((adjusted_mode->crtc_htotal - 1) << 16));
-	REG_WRITE(hblank_reg, (adjusted_mode->crtc_hblank_start - 1) |
+	REG_WRITE(map->hblank, (adjusted_mode->crtc_hblank_start - 1) |
 		  ((adjusted_mode->crtc_hblank_end - 1) << 16));
-	REG_WRITE(hsync_reg, (adjusted_mode->crtc_hsync_start - 1) |
+	REG_WRITE(map->hsync, (adjusted_mode->crtc_hsync_start - 1) |
 		  ((adjusted_mode->crtc_hsync_end - 1) << 16));
-	REG_WRITE(vtot_reg, (adjusted_mode->crtc_vdisplay - 1) |
+	REG_WRITE(map->vtotal, (adjusted_mode->crtc_vdisplay - 1) |
 		  ((adjusted_mode->crtc_vtotal - 1) << 16));
-	REG_WRITE(vblank_reg, (adjusted_mode->crtc_vblank_start - 1) |
+	REG_WRITE(map->vblank, (adjusted_mode->crtc_vblank_start - 1) |
 		  ((adjusted_mode->crtc_vblank_end - 1) << 16));
-	REG_WRITE(vsync_reg, (adjusted_mode->crtc_vsync_start - 1) |
+	REG_WRITE(map->vsync, (adjusted_mode->crtc_vsync_start - 1) |
 		  ((adjusted_mode->crtc_vsync_end - 1) << 16));
 	/* pipesrc and dspsize control the size that is scaled from,
 	 * which should always be the user's requested size.
 	 */
-	REG_WRITE(dspsize_reg,
+	REG_WRITE(map->size,
 		  ((mode->vdisplay - 1) << 16) | (mode->hdisplay - 1));
-	REG_WRITE(dsppos_reg, 0);
-	REG_WRITE(pipesrc_reg,
+	REG_WRITE(map->pos, 0);
+	REG_WRITE(map->src,
 		  ((mode->hdisplay - 1) << 16) | (mode->vdisplay - 1));
-	REG_WRITE(pipeconf_reg, pipeconf);
-	REG_READ(pipeconf_reg);
+	REG_WRITE(map->conf, pipeconf);
+	REG_READ(map->conf);
 
 	cdv_intel_wait_for_vblank(dev);
 
-	REG_WRITE(dspcntr_reg, dspcntr);
+	REG_WRITE(map->cntr, dspcntr);
 
 	/* Flush the plane changes */
 	{
@@ -903,58 +1130,6 @@ static int cdv_intel_crtc_mode_set(struct drm_crtc *crtc,
 	return 0;
 }
 
-/** Loads the palette/gamma unit for the CRTC with the prepared values */
-static void cdv_intel_crtc_load_lut(struct drm_crtc *crtc)
-{
-	struct drm_device *dev = crtc->dev;
-	struct drm_psb_private *dev_priv =
-				(struct drm_psb_private *)dev->dev_private;
-	struct psb_intel_crtc *psb_intel_crtc = to_psb_intel_crtc(crtc);
-	int palreg = PALETTE_A;
-	int i;
-
-	/* The clocks have to be on to load the palette. */
-	if (!crtc->enabled)
-		return;
-
-	switch (psb_intel_crtc->pipe) {
-	case 0:
-		break;
-	case 1:
-		palreg = PALETTE_B;
-		break;
-	case 2:
-		palreg = PALETTE_C;
-		break;
-	default:
-		dev_err(dev->dev, "Illegal Pipe Number.\n");
-		return;
-	}
-
-	if (gma_power_begin(dev, false)) {
-		for (i = 0; i < 256; i++) {
-			REG_WRITE(palreg + 4 * i,
-				  ((psb_intel_crtc->lut_r[i] +
-				  psb_intel_crtc->lut_adj[i]) << 16) |
-				  ((psb_intel_crtc->lut_g[i] +
-				  psb_intel_crtc->lut_adj[i]) << 8) |
-				  (psb_intel_crtc->lut_b[i] +
-				  psb_intel_crtc->lut_adj[i]));
-		}
-		gma_power_end(dev);
-	} else {
-		for (i = 0; i < 256; i++) {
-			dev_priv->regs.psb.save_palette_a[i] =
-				  ((psb_intel_crtc->lut_r[i] +
-				  psb_intel_crtc->lut_adj[i]) << 16) |
-				  ((psb_intel_crtc->lut_g[i] +
-				  psb_intel_crtc->lut_adj[i]) << 8) |
-				  (psb_intel_crtc->lut_b[i] +
-				  psb_intel_crtc->lut_adj[i]);
-		}
-
-	}
-}
 
 /**
  * Save HW states of giving crtc
@@ -962,11 +1137,10 @@ static void cdv_intel_crtc_load_lut(struct drm_crtc *crtc)
 static void cdv_intel_crtc_save(struct drm_crtc *crtc)
 {
 	struct drm_device *dev = crtc->dev;
-	/* struct drm_psb_private *dev_priv =
-			(struct drm_psb_private *)dev->dev_private; */
+	struct drm_psb_private *dev_priv = dev->dev_private;
 	struct psb_intel_crtc *psb_intel_crtc = to_psb_intel_crtc(crtc);
 	struct psb_intel_crtc_state *crtc_state = psb_intel_crtc->crtc_state;
-	int pipeA = (psb_intel_crtc->pipe == 0);
+	const struct psb_offset *map = &dev_priv->regmap[psb_intel_crtc->pipe];
 	uint32_t paletteReg;
 	int i;
 
@@ -975,25 +1149,25 @@ static void cdv_intel_crtc_save(struct drm_crtc *crtc)
 		return;
 	}
 
-	crtc_state->saveDSPCNTR = REG_READ(pipeA ? DSPACNTR : DSPBCNTR);
-	crtc_state->savePIPECONF = REG_READ(pipeA ? PIPEACONF : PIPEBCONF);
-	crtc_state->savePIPESRC = REG_READ(pipeA ? PIPEASRC : PIPEBSRC);
-	crtc_state->saveFP0 = REG_READ(pipeA ? FPA0 : FPB0);
-	crtc_state->saveFP1 = REG_READ(pipeA ? FPA1 : FPB1);
-	crtc_state->saveDPLL = REG_READ(pipeA ? DPLL_A : DPLL_B);
-	crtc_state->saveHTOTAL = REG_READ(pipeA ? HTOTAL_A : HTOTAL_B);
-	crtc_state->saveHBLANK = REG_READ(pipeA ? HBLANK_A : HBLANK_B);
-	crtc_state->saveHSYNC = REG_READ(pipeA ? HSYNC_A : HSYNC_B);
-	crtc_state->saveVTOTAL = REG_READ(pipeA ? VTOTAL_A : VTOTAL_B);
-	crtc_state->saveVBLANK = REG_READ(pipeA ? VBLANK_A : VBLANK_B);
-	crtc_state->saveVSYNC = REG_READ(pipeA ? VSYNC_A : VSYNC_B);
-	crtc_state->saveDSPSTRIDE = REG_READ(pipeA ? DSPASTRIDE : DSPBSTRIDE);
+	crtc_state->saveDSPCNTR = REG_READ(map->cntr);
+	crtc_state->savePIPECONF = REG_READ(map->conf);
+	crtc_state->savePIPESRC = REG_READ(map->src);
+	crtc_state->saveFP0 = REG_READ(map->fp0);
+	crtc_state->saveFP1 = REG_READ(map->fp1);
+	crtc_state->saveDPLL = REG_READ(map->dpll);
+	crtc_state->saveHTOTAL = REG_READ(map->htotal);
+	crtc_state->saveHBLANK = REG_READ(map->hblank);
+	crtc_state->saveHSYNC = REG_READ(map->hsync);
+	crtc_state->saveVTOTAL = REG_READ(map->vtotal);
+	crtc_state->saveVBLANK = REG_READ(map->vblank);
+	crtc_state->saveVSYNC = REG_READ(map->vsync);
+	crtc_state->saveDSPSTRIDE = REG_READ(map->stride);
 
 	/*NOTE: DSPSIZE DSPPOS only for psb*/
-	crtc_state->saveDSPSIZE = REG_READ(pipeA ? DSPASIZE : DSPBSIZE);
-	crtc_state->saveDSPPOS = REG_READ(pipeA ? DSPAPOS : DSPBPOS);
+	crtc_state->saveDSPSIZE = REG_READ(map->size);
+	crtc_state->saveDSPPOS = REG_READ(map->pos);
 
-	crtc_state->saveDSPBASE = REG_READ(pipeA ? DSPABASE : DSPBBASE);
+	crtc_state->saveDSPBASE = REG_READ(map->base);
 
 	DRM_DEBUG("(%x %x %x %x %x %x %x %x %x %x %x %x %x %x %x %x)\n",
 			crtc_state->saveDSPCNTR,
@@ -1014,7 +1188,7 @@ static void cdv_intel_crtc_save(struct drm_crtc *crtc)
 			crtc_state->saveDSPBASE
 		);
 
-	paletteReg = pipeA ? PALETTE_A : PALETTE_B;
+	paletteReg = map->palette;
 	for (i = 0; i < 256; ++i)
 		crtc_state->savePalette[i] = REG_READ(paletteReg + (i << 2));
 }
@@ -1025,12 +1199,10 @@ static void cdv_intel_crtc_save(struct drm_crtc *crtc)
 static void cdv_intel_crtc_restore(struct drm_crtc *crtc)
 {
 	struct drm_device *dev = crtc->dev;
-	/* struct drm_psb_private * dev_priv =
-				(struct drm_psb_private *)dev->dev_private; */
+	struct drm_psb_private *dev_priv = dev->dev_private;
 	struct psb_intel_crtc *psb_intel_crtc =  to_psb_intel_crtc(crtc);
 	struct psb_intel_crtc_state *crtc_state = psb_intel_crtc->crtc_state;
-	/* struct drm_crtc_helper_funcs * crtc_funcs = crtc->helper_private; */
-	int pipeA = (psb_intel_crtc->pipe == 0);
+	const struct psb_offset *map = &dev_priv->regmap[psb_intel_crtc->pipe];
 	uint32_t paletteReg;
 	int i;
 
@@ -1041,23 +1213,23 @@ static void cdv_intel_crtc_restore(struct drm_crtc *crtc)
 
 	DRM_DEBUG(
 		"current:(%x %x %x %x %x %x %x %x %x %x %x %x %x %x %x %x)\n",
-		REG_READ(pipeA ? DSPACNTR : DSPBCNTR),
-		REG_READ(pipeA ? PIPEACONF : PIPEBCONF),
-		REG_READ(pipeA ? PIPEASRC : PIPEBSRC),
-		REG_READ(pipeA ? FPA0 : FPB0),
-		REG_READ(pipeA ? FPA1 : FPB1),
-		REG_READ(pipeA ? DPLL_A : DPLL_B),
-		REG_READ(pipeA ? HTOTAL_A : HTOTAL_B),
-		REG_READ(pipeA ? HBLANK_A : HBLANK_B),
-		REG_READ(pipeA ? HSYNC_A : HSYNC_B),
-		REG_READ(pipeA ? VTOTAL_A : VTOTAL_B),
-		REG_READ(pipeA ? VBLANK_A : VBLANK_B),
-		REG_READ(pipeA ? VSYNC_A : VSYNC_B),
-		REG_READ(pipeA ? DSPASTRIDE : DSPBSTRIDE),
-		REG_READ(pipeA ? DSPASIZE : DSPBSIZE),
-		REG_READ(pipeA ? DSPAPOS : DSPBPOS),
-		REG_READ(pipeA ? DSPABASE : DSPBBASE)
-		);
+		REG_READ(map->cntr),
+		REG_READ(map->conf),
+		REG_READ(map->src),
+		REG_READ(map->fp0),
+		REG_READ(map->fp1),
+		REG_READ(map->dpll),
+		REG_READ(map->htotal),
+		REG_READ(map->hblank),
+		REG_READ(map->hsync),
+		REG_READ(map->vtotal),
+		REG_READ(map->vblank),
+		REG_READ(map->vsync),
+		REG_READ(map->stride),
+		REG_READ(map->size),
+		REG_READ(map->pos),
+		REG_READ(map->base)
+	);
 
 	DRM_DEBUG(
 		"saved: (%x %x %x %x %x %x %x %x %x %x %x %x %x %x %x %x)\n",
@@ -1077,51 +1249,51 @@ static void cdv_intel_crtc_restore(struct drm_crtc *crtc)
 		crtc_state->saveDSPSIZE,
 		crtc_state->saveDSPPOS,
 		crtc_state->saveDSPBASE
-		);
+	);
 
 
 	if (crtc_state->saveDPLL & DPLL_VCO_ENABLE) {
-		REG_WRITE(pipeA ? DPLL_A : DPLL_B,
-			crtc_state->saveDPLL & ~DPLL_VCO_ENABLE);
-		REG_READ(pipeA ? DPLL_A : DPLL_B);
+		REG_WRITE(map->dpll,
+				crtc_state->saveDPLL & ~DPLL_VCO_ENABLE);
+		REG_READ(map->dpll);
 		DRM_DEBUG("write dpll: %x\n",
-				REG_READ(pipeA ? DPLL_A : DPLL_B));
+				REG_READ(map->dpll));
 		udelay(150);
 	}
 
-	REG_WRITE(pipeA ? FPA0 : FPB0, crtc_state->saveFP0);
-	REG_READ(pipeA ? FPA0 : FPB0);
+	REG_WRITE(map->fp0, crtc_state->saveFP0);
+	REG_READ(map->fp0);
 
-	REG_WRITE(pipeA ? FPA1 : FPB1, crtc_state->saveFP1);
-	REG_READ(pipeA ? FPA1 : FPB1);
+	REG_WRITE(map->fp1, crtc_state->saveFP1);
+	REG_READ(map->fp1);
 
-	REG_WRITE(pipeA ? DPLL_A : DPLL_B, crtc_state->saveDPLL);
-	REG_READ(pipeA ? DPLL_A : DPLL_B);
+	REG_WRITE(map->dpll, crtc_state->saveDPLL);
+	REG_READ(map->dpll);
 	udelay(150);
 
-	REG_WRITE(pipeA ? HTOTAL_A : HTOTAL_B, crtc_state->saveHTOTAL);
-	REG_WRITE(pipeA ? HBLANK_A : HBLANK_B, crtc_state->saveHBLANK);
-	REG_WRITE(pipeA ? HSYNC_A : HSYNC_B, crtc_state->saveHSYNC);
-	REG_WRITE(pipeA ? VTOTAL_A : VTOTAL_B, crtc_state->saveVTOTAL);
-	REG_WRITE(pipeA ? VBLANK_A : VBLANK_B, crtc_state->saveVBLANK);
-	REG_WRITE(pipeA ? VSYNC_A : VSYNC_B, crtc_state->saveVSYNC);
-	REG_WRITE(pipeA ? DSPASTRIDE : DSPBSTRIDE, crtc_state->saveDSPSTRIDE);
+	REG_WRITE(map->htotal, crtc_state->saveHTOTAL);
+	REG_WRITE(map->hblank, crtc_state->saveHBLANK);
+	REG_WRITE(map->hsync, crtc_state->saveHSYNC);
+	REG_WRITE(map->vtotal, crtc_state->saveVTOTAL);
+	REG_WRITE(map->vblank, crtc_state->saveVBLANK);
+	REG_WRITE(map->vsync, crtc_state->saveVSYNC);
+	REG_WRITE(map->stride, crtc_state->saveDSPSTRIDE);
 
-	REG_WRITE(pipeA ? DSPASIZE : DSPBSIZE, crtc_state->saveDSPSIZE);
-	REG_WRITE(pipeA ? DSPAPOS : DSPBPOS, crtc_state->saveDSPPOS);
+	REG_WRITE(map->size, crtc_state->saveDSPSIZE);
+	REG_WRITE(map->pos, crtc_state->saveDSPPOS);
 
-	REG_WRITE(pipeA ? PIPEASRC : PIPEBSRC, crtc_state->savePIPESRC);
-	REG_WRITE(pipeA ? DSPABASE : DSPBBASE, crtc_state->saveDSPBASE);
-	REG_WRITE(pipeA ? PIPEACONF : PIPEBCONF, crtc_state->savePIPECONF);
+	REG_WRITE(map->src, crtc_state->savePIPESRC);
+	REG_WRITE(map->base, crtc_state->saveDSPBASE);
+	REG_WRITE(map->conf, crtc_state->savePIPECONF);
 
 	cdv_intel_wait_for_vblank(dev);
 
-	REG_WRITE(pipeA ? DSPACNTR : DSPBCNTR, crtc_state->saveDSPCNTR);
-	REG_WRITE(pipeA ? DSPABASE : DSPBBASE, crtc_state->saveDSPBASE);
+	REG_WRITE(map->cntr, crtc_state->saveDSPCNTR);
+	REG_WRITE(map->base, crtc_state->saveDSPBASE);
 
 	cdv_intel_wait_for_vblank(dev);
 
-	paletteReg = pipeA ? PALETTE_A : PALETTE_B;
+	paletteReg = map->palette;
 	for (i = 0; i < 256; ++i)
 		REG_WRITE(paletteReg + (i << 2), crtc_state->savePalette[i]);
 }
@@ -1296,35 +1468,30 @@ static void i8xx_clock(int refclk, struct cdv_intel_clock_t *clock)
 static int cdv_intel_crtc_clock_get(struct drm_device *dev,
 				struct drm_crtc *crtc)
 {
+	struct drm_psb_private *dev_priv = dev->dev_private;
 	struct psb_intel_crtc *psb_intel_crtc = to_psb_intel_crtc(crtc);
 	int pipe = psb_intel_crtc->pipe;
+	const struct psb_offset *map = &dev_priv->regmap[pipe];
 	u32 dpll;
 	u32 fp;
 	struct cdv_intel_clock_t clock;
 	bool is_lvds;
-	struct drm_psb_private *dev_priv = dev->dev_private;
+	struct psb_pipe *p = &dev_priv->regs.pipe[pipe];
 
 	if (gma_power_begin(dev, false)) {
-		dpll = REG_READ((pipe == 0) ? DPLL_A : DPLL_B);
+		dpll = REG_READ(map->dpll);
 		if ((dpll & DISPLAY_RATE_SELECT_FPA1) == 0)
-			fp = REG_READ((pipe == 0) ? FPA0 : FPB0);
+			fp = REG_READ(map->fp0);
 		else
-			fp = REG_READ((pipe == 0) ? FPA1 : FPB1);
+			fp = REG_READ(map->fp1);
 		is_lvds = (pipe == 1) && (REG_READ(LVDS) & LVDS_PORT_EN);
 		gma_power_end(dev);
 	} else {
-		dpll = (pipe == 0) ?
-			dev_priv->regs.psb.saveDPLL_A :
-			dev_priv->regs.psb.saveDPLL_B;
-
+		dpll = p->dpll;
 		if ((dpll & DISPLAY_RATE_SELECT_FPA1) == 0)
-			fp = (pipe == 0) ?
-				dev_priv->regs.psb.saveFPA0 :
-				dev_priv->regs.psb.saveFPB0;
+			fp = p->fp0;
 		else
-			fp = (pipe == 0) ?
-				dev_priv->regs.psb.saveFPA1 :
-				dev_priv->regs.psb.saveFPB1;
+			fp = p->fp1;
 
 		is_lvds = (pipe == 1) &&
 				(dev_priv->regs.psb.saveLVDS & LVDS_PORT_EN);
@@ -1382,32 +1549,26 @@ struct drm_display_mode *cdv_intel_crtc_mode_get(struct drm_device *dev,
 {
 	struct psb_intel_crtc *psb_intel_crtc = to_psb_intel_crtc(crtc);
 	int pipe = psb_intel_crtc->pipe;
+	struct drm_psb_private *dev_priv = dev->dev_private;
+	struct psb_pipe *p = &dev_priv->regs.pipe[pipe];
+	const struct psb_offset *map = &dev_priv->regmap[pipe];
 	struct drm_display_mode *mode;
 	int htot;
 	int hsync;
 	int vtot;
 	int vsync;
-	struct drm_psb_private *dev_priv = dev->dev_private;
 
 	if (gma_power_begin(dev, false)) {
-		htot = REG_READ((pipe == 0) ? HTOTAL_A : HTOTAL_B);
-		hsync = REG_READ((pipe == 0) ? HSYNC_A : HSYNC_B);
-		vtot = REG_READ((pipe == 0) ? VTOTAL_A : VTOTAL_B);
-		vsync = REG_READ((pipe == 0) ? VSYNC_A : VSYNC_B);
+		htot = REG_READ(map->htotal);
+		hsync = REG_READ(map->hsync);
+		vtot = REG_READ(map->vtotal);
+		vsync = REG_READ(map->vsync);
 		gma_power_end(dev);
 	} else {
-		htot = (pipe == 0) ?
-			dev_priv->regs.psb.saveHTOTAL_A :
-			dev_priv->regs.psb.saveHTOTAL_B;
-		hsync = (pipe == 0) ?
-			dev_priv->regs.psb.saveHSYNC_A :
-			dev_priv->regs.psb.saveHSYNC_B;
-		vtot = (pipe == 0) ?
-			dev_priv->regs.psb.saveVTOTAL_A :
-			dev_priv->regs.psb.saveVTOTAL_B;
-		vsync = (pipe == 0) ?
-			dev_priv->regs.psb.saveVSYNC_A :
-			dev_priv->regs.psb.saveVSYNC_B;
+		htot = p->htotal;
+		hsync = p->hsync;
+		vtot = p->vtotal;
+		vsync = p->vsync;
 	}
 
 	mode = kzalloc(sizeof(*mode), GFP_KERNEL);
diff --git a/drivers/gpu/drm/gma500/cdv_intel_hdmi.c b/drivers/gpu/drm/gma500/cdv_intel_hdmi.c
index 8d526955500..88b59d4a7b7 100644
--- a/drivers/gpu/drm/gma500/cdv_intel_hdmi.c
+++ b/drivers/gpu/drm/gma500/cdv_intel_hdmi.c
@@ -242,8 +242,6 @@ static int cdv_hdmi_get_modes(struct drm_connector *connector)
 static int cdv_hdmi_mode_valid(struct drm_connector *connector,
 				 struct drm_display_mode *mode)
 {
-	struct drm_psb_private *dev_priv = connector->dev->dev_private;
-
 	if (mode->clock > 165000)
 		return MODE_CLOCK_HIGH;
 	if (mode->clock < 20000)
@@ -257,11 +255,6 @@ static int cdv_hdmi_mode_valid(struct drm_connector *connector,
 	if (mode->flags & DRM_MODE_FLAG_INTERLACE)
 		return MODE_NO_INTERLACE;
 
-	/* We assume worst case scenario of 32 bpp here, since we don't know */
-	if ((ALIGN(mode->hdisplay * 4, 64) * mode->vdisplay) >
-	    dev_priv->vram_stolen_size)
-		return MODE_MEM;
-
 	return MODE_OK;
 }
 
diff --git a/drivers/gpu/drm/gma500/cdv_intel_lvds.c b/drivers/gpu/drm/gma500/cdv_intel_lvds.c
index 8359c1a3f45..ff5b58eb878 100644
--- a/drivers/gpu/drm/gma500/cdv_intel_lvds.c
+++ b/drivers/gpu/drm/gma500/cdv_intel_lvds.c
@@ -356,6 +356,8 @@ static void cdv_intel_lvds_mode_set(struct drm_encoder *encoder,
 {
 	struct drm_device *dev = encoder->dev;
 	struct drm_psb_private *dev_priv = dev->dev_private;
+	struct psb_intel_crtc *psb_intel_crtc = to_psb_intel_crtc(
+							encoder->crtc);
 	u32 pfit_control;
 
 	/*
@@ -377,6 +379,8 @@ static void cdv_intel_lvds_mode_set(struct drm_encoder *encoder,
 	else
 		pfit_control = 0;
 
+	pfit_control |= psb_intel_crtc->pipe << PFIT_PIPE_SHIFT;
+
 	if (dev_priv->lvds_dither)
 		pfit_control |= PANEL_8TO6_DITHER_ENABLE;
 
@@ -552,10 +556,60 @@ static void cdv_intel_lvds_enc_destroy(struct drm_encoder *encoder)
 	drm_encoder_cleanup(encoder);
 }
 
-const struct drm_encoder_funcs cdv_intel_lvds_enc_funcs = {
+static const struct drm_encoder_funcs cdv_intel_lvds_enc_funcs = {
 	.destroy = cdv_intel_lvds_enc_destroy,
 };
 
+/*
+ * Enumerate the child dev array parsed from VBT to check whether
+ * the LVDS is present.
+ * If it is present, return 1.
+ * If it is not present, return false.
+ * If no child dev is parsed from VBT, it assumes that the LVDS is present.
+ */
+static bool lvds_is_present_in_vbt(struct drm_device *dev,
+				   u8 *i2c_pin)
+{
+	struct drm_psb_private *dev_priv = dev->dev_private;
+	int i;
+
+	if (!dev_priv->child_dev_num)
+		return true;
+
+	for (i = 0; i < dev_priv->child_dev_num; i++) {
+		struct child_device_config *child = dev_priv->child_dev + i;
+
+		/* If the device type is not LFP, continue.
+		 * We have to check both the new identifiers as well as the
+		 * old for compatibility with some BIOSes.
+		 */
+		if (child->device_type != DEVICE_TYPE_INT_LFP &&
+		    child->device_type != DEVICE_TYPE_LFP)
+			continue;
+
+		if (child->i2c_pin)
+		    *i2c_pin = child->i2c_pin;
+
+		/* However, we cannot trust the BIOS writers to populate
+		 * the VBT correctly.  Since LVDS requires additional
+		 * information from AIM blocks, a non-zero addin offset is
+		 * a good indicator that the LVDS is actually present.
+		 */
+		if (child->addin_offset)
+			return true;
+
+		/* But even then some BIOS writers perform some black magic
+		 * and instantiate the device without reference to any
+		 * additional data.  Trust that if the VBT was written into
+		 * the OpRegion then they have validated the LVDS's existence.
+		 */
+		if (dev_priv->opregion.vbt)
+			return true;
+	}
+
+	return false;
+}
+
 /**
  * cdv_intel_lvds_init - setup LVDS connectors on this device
  * @dev: drm device
@@ -576,6 +630,13 @@ void cdv_intel_lvds_init(struct drm_device *dev,
 	struct drm_psb_private *dev_priv = dev->dev_private;
 	u32 lvds;
 	int pipe;
+	u8 pin;
+
+	pin = GMBUS_PORT_PANEL;
+	if (!lvds_is_present_in_vbt(dev, &pin)) {
+		DRM_DEBUG_KMS("LVDS is not present in VBT\n");
+		return;
+	}
 
 	psb_intel_encoder = kzalloc(sizeof(struct psb_intel_encoder),
 				    GFP_KERNEL);
@@ -710,6 +771,19 @@ void cdv_intel_lvds_init(struct drm_device *dev,
 		goto failed_find;
 	}
 
+	/* setup PWM */
+	{
+		u32 pwm;
+
+		pwm = REG_READ(BLC_PWM_CTL2);
+		if (pipe == 1)
+			pwm |= PWM_PIPE_B;
+		else
+			pwm &= ~PWM_PIPE_B;
+		pwm |= PWM_ENABLE;
+		REG_WRITE(BLC_PWM_CTL2, pwm);
+	}
+
 out:
 	drm_sysfs_connector_add(connector);
 	return;
diff --git a/drivers/gpu/drm/gma500/framebuffer.c b/drivers/gpu/drm/gma500/framebuffer.c
index 8ea202f1ba5..5732b5702e1 100644
--- a/drivers/gpu/drm/gma500/framebuffer.c
+++ b/drivers/gpu/drm/gma500/framebuffer.c
@@ -153,7 +153,7 @@ static void psbfb_vm_close(struct vm_area_struct *vma)
 {
 }
 
-static struct vm_operations_struct psbfb_vm_ops = {
+static const struct vm_operations_struct psbfb_vm_ops = {
 	.fault	= psbfb_vm_fault,
 	.open	= psbfb_vm_open,
 	.close	= psbfb_vm_close
@@ -408,6 +408,8 @@ static int psbfb_create(struct psb_fbdev *fbdev,
 			return -ENOMEM;
 	}
 
+	memset(dev_priv->vram_addr + backing->offset, 0, size);
+
 	mutex_lock(&dev->struct_mutex);
 
 	info = framebuffer_alloc(0, device);
@@ -453,8 +455,7 @@ static int psbfb_create(struct psb_fbdev *fbdev,
 	info->fix.ypanstep = 0;
 
 	/* Accessed stolen memory directly */
-	info->screen_base = (char *)dev_priv->vram_addr +
-							backing->offset;
+	info->screen_base = dev_priv->vram_addr + backing->offset;
 	info->screen_size = size;
 
 	if (dev_priv->gtt.stolen_size) {
@@ -475,7 +476,7 @@ static int psbfb_create(struct psb_fbdev *fbdev,
 
 	/* Use default scratch pixmap (info->pixmap.flags = FB_PIXMAP_SYSTEM) */
 
-	dev_info(dev->dev, "allocated %dx%d fb\n",
+	dev_dbg(dev->dev, "allocated %dx%d fb\n",
 					psbfb->base.width, psbfb->base.height);
 
 	mutex_unlock(&dev->struct_mutex);
@@ -543,9 +544,25 @@ static int psbfb_probe(struct drm_fb_helper *helper,
 				struct drm_fb_helper_surface_size *sizes)
 {
 	struct psb_fbdev *psb_fbdev = (struct psb_fbdev *)helper;
+	struct drm_device *dev = psb_fbdev->psb_fb_helper.dev;
+	struct drm_psb_private *dev_priv = dev->dev_private;
 	int new_fb = 0;
+	int bytespp;
 	int ret;
 
+	bytespp = sizes->surface_bpp / 8;
+	if (bytespp == 3)	/* no 24bit packed */
+		bytespp = 4;
+
+	/* If the mode will not fit in 32bit then switch to 16bit to get
+	   a console on full resolution. The X mode setting server will
+	   allocate its own 32bit GEM framebuffer */
+	if (ALIGN(sizes->fb_width * bytespp, 64) * sizes->fb_height >
+	                dev_priv->vram_stolen_size) {
+                sizes->surface_bpp = 16;
+                sizes->surface_depth = 16;
+        }
+
 	if (!helper->fb) {
 		ret = psbfb_create(psb_fbdev, sizes);
 		if (ret)
@@ -555,7 +572,7 @@ static int psbfb_probe(struct drm_fb_helper *helper,
 	return new_fb;
 }
 
-struct drm_fb_helper_funcs psb_fb_helper_funcs = {
+static struct drm_fb_helper_funcs psb_fb_helper_funcs = {
 	.gamma_set = psbfb_gamma_set,
 	.gamma_get = psbfb_gamma_get,
 	.fb_probe = psbfb_probe,
@@ -732,10 +749,7 @@ static void psb_setup_outputs(struct drm_device *dev)
 			clone_mask = (1 << INTEL_OUTPUT_SDVO);
 			break;
 		case INTEL_OUTPUT_LVDS:
-			if (IS_MRST(dev))
-				crtc_mask = (1 << 0);
-			else
-				crtc_mask = (1 << 1);
+		        crtc_mask = dev_priv->ops->lvds_mask;
 			clone_mask = (1 << INTEL_OUTPUT_LVDS);
 			break;
 		case INTEL_OUTPUT_MIPI:
@@ -747,10 +761,7 @@ static void psb_setup_outputs(struct drm_device *dev)
 			clone_mask = (1 << INTEL_OUTPUT_MIPI2);
 			break;
 		case INTEL_OUTPUT_HDMI:
-			if (IS_MFLD(dev))
-				crtc_mask = (1 << 1);
-			else	
-				crtc_mask = (1 << 0);
+		        crtc_mask = dev_priv->ops->hdmi_mask;
 			clone_mask = (1 << INTEL_OUTPUT_HDMI);
 			break;
 		}
@@ -771,7 +782,7 @@ void psb_modeset_init(struct drm_device *dev)
 	dev->mode_config.min_width = 0;
 	dev->mode_config.min_height = 0;
 
-	dev->mode_config.funcs = (void *) &psb_mode_funcs;
+	dev->mode_config.funcs = &psb_mode_funcs;
 
 	/* set memory base */
 	/* Oaktrail and Poulsbo should use BAR 2*/
@@ -786,15 +797,23 @@ void psb_modeset_init(struct drm_device *dev)
 	dev->mode_config.max_height = 2048;
 
 	psb_setup_outputs(dev);
+
+	if (dev_priv->ops->errata)
+	        dev_priv->ops->errata(dev);
+
+        dev_priv->modeset = true;
 }
 
 void psb_modeset_cleanup(struct drm_device *dev)
 {
-	mutex_lock(&dev->struct_mutex);
+	struct drm_psb_private *dev_priv = dev->dev_private;
+	if (dev_priv->modeset) {
+		mutex_lock(&dev->struct_mutex);
 
-	drm_kms_helper_poll_fini(dev);
-	psb_fbdev_fini(dev);
-	drm_mode_config_cleanup(dev);
+		drm_kms_helper_poll_fini(dev);
+		psb_fbdev_fini(dev);
+		drm_mode_config_cleanup(dev);
 
-	mutex_unlock(&dev->struct_mutex);
+		mutex_unlock(&dev->struct_mutex);
+	}
 }
diff --git a/drivers/gpu/drm/gma500/gem.c b/drivers/gpu/drm/gma500/gem.c
index 9fbb86868e2..fc7d144bc2d 100644
--- a/drivers/gpu/drm/gma500/gem.c
+++ b/drivers/gpu/drm/gma500/gem.c
@@ -124,6 +124,8 @@ static int psb_gem_create(struct drm_file *file,
 		dev_err(dev->dev, "GEM init failed for %lld\n", size);
 		return -ENOMEM;
 	}
+	/* Limit the object to 32bit mappings */
+	mapping_set_gfp_mask(r->gem.filp->f_mapping, GFP_KERNEL | __GFP_DMA32);
 	/* Give the object a handle so we can carry it more easily */
 	ret = drm_gem_handle_create(file, &r->gem, &handle);
 	if (ret) {
diff --git a/drivers/gpu/drm/gma500/gtt.c b/drivers/gpu/drm/gma500/gtt.c
index c6465b40090..04a371aceb3 100644
--- a/drivers/gpu/drm/gma500/gtt.c
+++ b/drivers/gpu/drm/gma500/gtt.c
@@ -39,6 +39,10 @@ static inline uint32_t psb_gtt_mask_pte(uint32_t pfn, int type)
 {
 	uint32_t mask = PSB_PTE_VALID;
 
+	/* Ensure we explode rather than put an invalid low mapping of
+	   a high mapping page into the gtt */
+	BUG_ON(pfn & ~(0xFFFFFFFF >> PAGE_SHIFT));
+
 	if (type & PSB_MMU_CACHED_MEMORY)
 		mask |= PSB_PTE_CACHED;
 	if (type & PSB_MMU_RO_MEMORY)
@@ -57,7 +61,7 @@ static inline uint32_t psb_gtt_mask_pte(uint32_t pfn, int type)
  *	Given a gtt_range object return the GTT offset of the page table
  *	entries for this gtt_range
  */
-static u32 *psb_gtt_entry(struct drm_device *dev, struct gtt_range *r)
+static u32 __iomem *psb_gtt_entry(struct drm_device *dev, struct gtt_range *r)
 {
 	struct drm_psb_private *dev_priv = dev->dev_private;
 	unsigned long offset;
@@ -78,7 +82,8 @@ static u32 *psb_gtt_entry(struct drm_device *dev, struct gtt_range *r)
  */
 static int psb_gtt_insert(struct drm_device *dev, struct gtt_range *r)
 {
-	u32 *gtt_slot, pte;
+	u32 __iomem *gtt_slot;
+	u32 pte;
 	struct page **pages;
 	int i;
 
@@ -93,7 +98,7 @@ static int psb_gtt_insert(struct drm_device *dev, struct gtt_range *r)
 	pages = r->pages;
 
 	/* Make sure changes are visible to the GPU */
-	set_pages_array_uc(pages, r->npage);
+	set_pages_array_wc(pages, r->npage);
 
 	/* Write our page entries into the GTT itself */
 	for (i = r->roll; i < r->npage; i++) {
@@ -122,7 +127,8 @@ static int psb_gtt_insert(struct drm_device *dev, struct gtt_range *r)
 static void psb_gtt_remove(struct drm_device *dev, struct gtt_range *r)
 {
 	struct drm_psb_private *dev_priv = dev->dev_private;
-	u32 *gtt_slot, pte;
+	u32 __iomem *gtt_slot;
+	u32 pte;
 	int i;
 
 	WARN_ON(r->stolen);
@@ -148,7 +154,8 @@ static void psb_gtt_remove(struct drm_device *dev, struct gtt_range *r)
  */
 void psb_gtt_roll(struct drm_device *dev, struct gtt_range *r, int roll)
 {
-	u32 *gtt_slot, pte;
+	u32 __iomem *gtt_slot;
+	u32 pte;
 	int i;
 
 	if (roll >= r->npage) {
@@ -409,8 +416,6 @@ int psb_gtt_init(struct drm_device *dev, int resume)
 	unsigned long stolen_size, vram_stolen_size;
 	unsigned i, num_pages;
 	unsigned pfn_base;
-	uint32_t vram_pages;
-	uint32_t dvmt_mode = 0;
 	struct psb_gtt *pg;
 
 	int ret = 0;
@@ -483,13 +488,8 @@ int psb_gtt_init(struct drm_device *dev, int resume)
 
 	stolen_size = vram_stolen_size;
 
-	printk(KERN_INFO "Stolen memory information\n");
-	printk(KERN_INFO "       base in RAM: 0x%x\n", dev_priv->stolen_base);
-	printk(KERN_INFO "       size: %luK, calculated by (GTT RAM base) - (Stolen base), seems wrong\n",
-		vram_stolen_size/1024);
-	dvmt_mode = (dev_priv->gmch_ctrl >> 4) & 0x7;
-	printk(KERN_INFO "      the correct size should be: %dM(dvmt mode=%d)\n",
-		(dvmt_mode == 1) ? 1 : (2 << (dvmt_mode - 1)), dvmt_mode);
+	dev_dbg(dev->dev, "Stolen memory base 0x%x, size %luK\n",
+			dev_priv->stolen_base, vram_stolen_size / 1024);
 
 	if (resume && (gtt_pages != pg->gtt_pages) &&
 	    (stolen_size != pg->stolen_size)) {
@@ -525,8 +525,8 @@ int psb_gtt_init(struct drm_device *dev, int resume)
 	 */
 
 	pfn_base = dev_priv->stolen_base >> PAGE_SHIFT;
-	vram_pages = num_pages = vram_stolen_size >> PAGE_SHIFT;
-	printk(KERN_INFO"Set up %d stolen pages starting at 0x%08x, GTT offset %dK\n",
+	num_pages = vram_stolen_size >> PAGE_SHIFT;
+	dev_dbg(dev->dev, "Set up %d stolen pages starting at 0x%08x, GTT offset %dK\n",
 		num_pages, pfn_base << PAGE_SHIFT, 0);
 	for (i = 0; i < num_pages; ++i) {
 		pte = psb_gtt_mask_pte(pfn_base + i, 0);
diff --git a/drivers/gpu/drm/gma500/intel_bios.c b/drivers/gpu/drm/gma500/intel_bios.c
index d4d0c5b8bf9..973d7f6d66b 100644
--- a/drivers/gpu/drm/gma500/intel_bios.c
+++ b/drivers/gpu/drm/gma500/intel_bios.c
@@ -26,6 +26,8 @@
 #include "psb_intel_reg.h"
 #include "intel_bios.h"
 
+#define	SLAVE_ADDR1	0x70
+#define	SLAVE_ADDR2	0x72
 
 static void *find_section(struct bdb_header *bdb, int section_id)
 {
@@ -52,6 +54,16 @@ static void *find_section(struct bdb_header *bdb, int section_id)
 	return NULL;
 }
 
+static u16
+get_blocksize(void *p)
+{
+	u16 *block_ptr, block_size;
+
+	block_ptr = (u16 *)((char *)p - 2);
+	block_size = *block_ptr;
+	return block_size;
+}
+
 static void fill_detail_timing_data(struct drm_display_mode *panel_fixed_mode,
 			struct lvds_dvo_timing *dvo_timing)
 {
@@ -75,6 +87,16 @@ static void fill_detail_timing_data(struct drm_display_mode *panel_fixed_mode,
 	panel_fixed_mode->clock = dvo_timing->clock * 10;
 	panel_fixed_mode->type = DRM_MODE_TYPE_PREFERRED;
 
+	if (dvo_timing->hsync_positive)
+		panel_fixed_mode->flags |= DRM_MODE_FLAG_PHSYNC;
+	else
+		panel_fixed_mode->flags |= DRM_MODE_FLAG_NHSYNC;
+
+	if (dvo_timing->vsync_positive)
+		panel_fixed_mode->flags |= DRM_MODE_FLAG_PVSYNC;
+	else
+		panel_fixed_mode->flags |= DRM_MODE_FLAG_NVSYNC;
+
 	/* Some VBTs have bogus h/vtotal values */
 	if (panel_fixed_mode->hsync_end > panel_fixed_mode->htotal)
 		panel_fixed_mode->htotal = panel_fixed_mode->hsync_end + 1;
@@ -217,6 +239,180 @@ static void parse_general_features(struct drm_psb_private *dev_priv,
 	}
 }
 
+static void
+parse_sdvo_device_mapping(struct drm_psb_private *dev_priv,
+			  struct bdb_header *bdb)
+{
+	struct sdvo_device_mapping *p_mapping;
+	struct bdb_general_definitions *p_defs;
+	struct child_device_config *p_child;
+	int i, child_device_num, count;
+	u16	block_size;
+
+	p_defs = find_section(bdb, BDB_GENERAL_DEFINITIONS);
+	if (!p_defs) {
+		DRM_DEBUG_KMS("No general definition block is found, unable to construct sdvo mapping.\n");
+		return;
+	}
+	/* judge whether the size of child device meets the requirements.
+	 * If the child device size obtained from general definition block
+	 * is different with sizeof(struct child_device_config), skip the
+	 * parsing of sdvo device info
+	 */
+	if (p_defs->child_dev_size != sizeof(*p_child)) {
+		/* different child dev size . Ignore it */
+		DRM_DEBUG_KMS("different child size is found. Invalid.\n");
+		return;
+	}
+	/* get the block size of general definitions */
+	block_size = get_blocksize(p_defs);
+	/* get the number of child device */
+	child_device_num = (block_size - sizeof(*p_defs)) /
+				sizeof(*p_child);
+	count = 0;
+	for (i = 0; i < child_device_num; i++) {
+		p_child = &(p_defs->devices[i]);
+		if (!p_child->device_type) {
+			/* skip the device block if device type is invalid */
+			continue;
+		}
+		if (p_child->slave_addr != SLAVE_ADDR1 &&
+			p_child->slave_addr != SLAVE_ADDR2) {
+			/*
+			 * If the slave address is neither 0x70 nor 0x72,
+			 * it is not a SDVO device. Skip it.
+			 */
+			continue;
+		}
+		if (p_child->dvo_port != DEVICE_PORT_DVOB &&
+			p_child->dvo_port != DEVICE_PORT_DVOC) {
+			/* skip the incorrect SDVO port */
+			DRM_DEBUG_KMS("Incorrect SDVO port. Skip it\n");
+			continue;
+		}
+		DRM_DEBUG_KMS("the SDVO device with slave addr %2x is found on"
+				" %s port\n",
+				p_child->slave_addr,
+				(p_child->dvo_port == DEVICE_PORT_DVOB) ?
+					"SDVOB" : "SDVOC");
+		p_mapping = &(dev_priv->sdvo_mappings[p_child->dvo_port - 1]);
+		if (!p_mapping->initialized) {
+			p_mapping->dvo_port = p_child->dvo_port;
+			p_mapping->slave_addr = p_child->slave_addr;
+			p_mapping->dvo_wiring = p_child->dvo_wiring;
+			p_mapping->ddc_pin = p_child->ddc_pin;
+			p_mapping->i2c_pin = p_child->i2c_pin;
+			p_mapping->initialized = 1;
+			DRM_DEBUG_KMS("SDVO device: dvo=%x, addr=%x, wiring=%d, ddc_pin=%d, i2c_pin=%d\n",
+				      p_mapping->dvo_port,
+				      p_mapping->slave_addr,
+				      p_mapping->dvo_wiring,
+				      p_mapping->ddc_pin,
+				      p_mapping->i2c_pin);
+		} else {
+			DRM_DEBUG_KMS("Maybe one SDVO port is shared by "
+					 "two SDVO device.\n");
+		}
+		if (p_child->slave2_addr) {
+			/* Maybe this is a SDVO device with multiple inputs */
+			/* And the mapping info is not added */
+			DRM_DEBUG_KMS("there exists the slave2_addr. Maybe this"
+				" is a SDVO device with multiple inputs.\n");
+		}
+		count++;
+	}
+
+	if (!count) {
+		/* No SDVO device info is found */
+		DRM_DEBUG_KMS("No SDVO device info is found in VBT\n");
+	}
+	return;
+}
+
+
+static void
+parse_driver_features(struct drm_psb_private *dev_priv,
+		      struct bdb_header *bdb)
+{
+	struct bdb_driver_features *driver;
+
+	driver = find_section(bdb, BDB_DRIVER_FEATURES);
+	if (!driver)
+		return;
+
+	/* This bit means to use 96Mhz for DPLL_A or not */
+	if (driver->primary_lfp_id)
+		dev_priv->dplla_96mhz = true;
+	else
+		dev_priv->dplla_96mhz = false;
+}
+
+static void
+parse_device_mapping(struct drm_psb_private *dev_priv,
+		       struct bdb_header *bdb)
+{
+	struct bdb_general_definitions *p_defs;
+	struct child_device_config *p_child, *child_dev_ptr;
+	int i, child_device_num, count;
+	u16	block_size;
+
+	p_defs = find_section(bdb, BDB_GENERAL_DEFINITIONS);
+	if (!p_defs) {
+		DRM_DEBUG_KMS("No general definition block is found, no devices defined.\n");
+		return;
+	}
+	/* judge whether the size of child device meets the requirements.
+	 * If the child device size obtained from general definition block
+	 * is different with sizeof(struct child_device_config), skip the
+	 * parsing of sdvo device info
+	 */
+	if (p_defs->child_dev_size != sizeof(*p_child)) {
+		/* different child dev size . Ignore it */
+		DRM_DEBUG_KMS("different child size is found. Invalid.\n");
+		return;
+	}
+	/* get the block size of general definitions */
+	block_size = get_blocksize(p_defs);
+	/* get the number of child device */
+	child_device_num = (block_size - sizeof(*p_defs)) /
+				sizeof(*p_child);
+	count = 0;
+	/* get the number of child devices that are present */
+	for (i = 0; i < child_device_num; i++) {
+		p_child = &(p_defs->devices[i]);
+		if (!p_child->device_type) {
+			/* skip the device block if device type is invalid */
+			continue;
+		}
+		count++;
+	}
+	if (!count) {
+		DRM_DEBUG_KMS("no child dev is parsed from VBT\n");
+		return;
+	}
+	dev_priv->child_dev = kcalloc(count, sizeof(*p_child), GFP_KERNEL);
+	if (!dev_priv->child_dev) {
+		DRM_DEBUG_KMS("No memory space for child devices\n");
+		return;
+	}
+
+	dev_priv->child_dev_num = count;
+	count = 0;
+	for (i = 0; i < child_device_num; i++) {
+		p_child = &(p_defs->devices[i]);
+		if (!p_child->device_type) {
+			/* skip the device block if device type is invalid */
+			continue;
+		}
+		child_dev_ptr = dev_priv->child_dev + count;
+		count++;
+		memcpy((void *)child_dev_ptr, (void *)p_child,
+					sizeof(*p_child));
+	}
+	return;
+}
+
+
 /**
  * psb_intel_init_bios - initialize VBIOS settings & find VBT
  * @dev: DRM device
@@ -236,38 +432,54 @@ bool psb_intel_init_bios(struct drm_device *dev)
 	struct drm_psb_private *dev_priv = dev->dev_private;
 	struct pci_dev *pdev = dev->pdev;
 	struct vbt_header *vbt = NULL;
-	struct bdb_header *bdb;
-	u8 __iomem *bios;
+	struct bdb_header *bdb = NULL;
+	u8 __iomem *bios = NULL;
 	size_t size;
 	int i;
 
-	bios = pci_map_rom(pdev, &size);
-	if (!bios)
-		return -1;
+	/* XXX Should this validation be moved to intel_opregion.c? */
+	if (dev_priv->opregion.vbt) {
+		struct vbt_header *vbt = dev_priv->opregion.vbt;
+		if (memcmp(vbt->signature, "$VBT", 4) == 0) {
+			DRM_DEBUG_KMS("Using VBT from OpRegion: %20s\n",
+					 vbt->signature);
+			bdb = (struct bdb_header *)((char *)vbt + vbt->bdb_offset);
+		} else
+			dev_priv->opregion.vbt = NULL;
+	}
 
-	/* Scour memory looking for the VBT signature */
-	for (i = 0; i + 4 < size; i++) {
-		if (!memcmp(bios + i, "$VBT", 4)) {
-			vbt = (struct vbt_header *)(bios + i);
-			break;
+	if (bdb == NULL) {
+		bios = pci_map_rom(pdev, &size);
+		if (!bios)
+			return -1;
+
+		/* Scour memory looking for the VBT signature */
+		for (i = 0; i + 4 < size; i++) {
+			if (!memcmp(bios + i, "$VBT", 4)) {
+				vbt = (struct vbt_header *)(bios + i);
+				break;
+			}
 		}
-	}
 
-	if (!vbt) {
-		dev_err(dev->dev, "VBT signature missing\n");
-		pci_unmap_rom(pdev, bios);
-		return -1;
+		if (!vbt) {
+			dev_err(dev->dev, "VBT signature missing\n");
+			pci_unmap_rom(pdev, bios);
+			return -1;
+		}
+		bdb = (struct bdb_header *)(bios + i + vbt->bdb_offset);
 	}
 
-	bdb = (struct bdb_header *)(bios + i + vbt->bdb_offset);
-
-	/* Grab useful general definitions */
+	/* Grab useful general dxefinitions */
 	parse_general_features(dev_priv, bdb);
+	parse_driver_features(dev_priv, bdb);
 	parse_lfp_panel_data(dev_priv, bdb);
 	parse_sdvo_panel_data(dev_priv, bdb);
+	parse_sdvo_device_mapping(dev_priv, bdb);
+	parse_device_mapping(dev_priv, bdb);
 	parse_backlight_data(dev_priv, bdb);
 
-	pci_unmap_rom(pdev, bios);
+	if (bios)
+		pci_unmap_rom(pdev, bios);
 
 	return 0;
 }
@@ -278,26 +490,8 @@ bool psb_intel_init_bios(struct drm_device *dev)
 void psb_intel_destroy_bios(struct drm_device *dev)
 {
 	struct drm_psb_private *dev_priv = dev->dev_private;
-	struct drm_display_mode *sdvo_lvds_vbt_mode =
-				dev_priv->sdvo_lvds_vbt_mode;
-	struct drm_display_mode *lfp_lvds_vbt_mode =
-				dev_priv->lfp_lvds_vbt_mode;
-	struct bdb_lvds_backlight *lvds_bl =
-				dev_priv->lvds_bl;
-
-	/*free sdvo panel mode*/
-	if (sdvo_lvds_vbt_mode) {
-		dev_priv->sdvo_lvds_vbt_mode = NULL;
-		kfree(sdvo_lvds_vbt_mode);
-	}
 
-	if (lfp_lvds_vbt_mode) {
-		dev_priv->lfp_lvds_vbt_mode = NULL;
-		kfree(lfp_lvds_vbt_mode);
-	}
-
-	if (lvds_bl) {
-		dev_priv->lvds_bl = NULL;
-		kfree(lvds_bl);
-	}
+	kfree(dev_priv->sdvo_lvds_vbt_mode);
+	kfree(dev_priv->lfp_lvds_vbt_mode);
+	kfree(dev_priv->lvds_bl);
 }
diff --git a/drivers/gpu/drm/gma500/intel_bios.h b/drivers/gpu/drm/gma500/intel_bios.h
index 70f1bf01818..0a738663eb5 100644
--- a/drivers/gpu/drm/gma500/intel_bios.h
+++ b/drivers/gpu/drm/gma500/intel_bios.h
@@ -127,9 +127,93 @@ struct bdb_general_features {
 	/* bits 5 */
 	u8 int_crt_support:1;
 	u8 int_tv_support:1;
-	u8 rsvd11:6; /* finish byte */
+	u8 int_efp_support:1;
+	u8 dp_ssc_enb:1;	/* PCH attached eDP supports SSC */
+	u8 dp_ssc_freq:1;	/* SSC freq for PCH attached eDP */
+	u8 rsvd11:3; /* finish byte */
 } __attribute__((packed));
 
+/* pre-915 */
+#define GPIO_PIN_DVI_LVDS	0x03 /* "DVI/LVDS DDC GPIO pins" */
+#define GPIO_PIN_ADD_I2C	0x05 /* "ADDCARD I2C GPIO pins" */
+#define GPIO_PIN_ADD_DDC	0x04 /* "ADDCARD DDC GPIO pins" */
+#define GPIO_PIN_ADD_DDC_I2C	0x06 /* "ADDCARD DDC/I2C GPIO pins" */
+
+/* Pre 915 */
+#define DEVICE_TYPE_NONE	0x00
+#define DEVICE_TYPE_CRT		0x01
+#define DEVICE_TYPE_TV		0x09
+#define DEVICE_TYPE_EFP		0x12
+#define DEVICE_TYPE_LFP		0x22
+/* On 915+ */
+#define DEVICE_TYPE_CRT_DPMS		0x6001
+#define DEVICE_TYPE_CRT_DPMS_HOTPLUG	0x4001
+#define DEVICE_TYPE_TV_COMPOSITE	0x0209
+#define DEVICE_TYPE_TV_MACROVISION	0x0289
+#define DEVICE_TYPE_TV_RF_COMPOSITE	0x020c
+#define DEVICE_TYPE_TV_SVIDEO_COMPOSITE	0x0609
+#define DEVICE_TYPE_TV_SCART		0x0209
+#define DEVICE_TYPE_TV_CODEC_HOTPLUG_PWR 0x6009
+#define DEVICE_TYPE_EFP_HOTPLUG_PWR	0x6012
+#define DEVICE_TYPE_EFP_DVI_HOTPLUG_PWR	0x6052
+#define DEVICE_TYPE_EFP_DVI_I		0x6053
+#define DEVICE_TYPE_EFP_DVI_D_DUAL	0x6152
+#define DEVICE_TYPE_EFP_DVI_D_HDCP	0x60d2
+#define DEVICE_TYPE_OPENLDI_HOTPLUG_PWR	0x6062
+#define DEVICE_TYPE_OPENLDI_DUALPIX	0x6162
+#define DEVICE_TYPE_LFP_PANELLINK	0x5012
+#define DEVICE_TYPE_LFP_CMOS_PWR	0x5042
+#define DEVICE_TYPE_LFP_LVDS_PWR	0x5062
+#define DEVICE_TYPE_LFP_LVDS_DUAL	0x5162
+#define DEVICE_TYPE_LFP_LVDS_DUAL_HDCP	0x51e2
+
+#define DEVICE_CFG_NONE		0x00
+#define DEVICE_CFG_12BIT_DVOB	0x01
+#define DEVICE_CFG_12BIT_DVOC	0x02
+#define DEVICE_CFG_24BIT_DVOBC	0x09
+#define DEVICE_CFG_24BIT_DVOCB	0x0a
+#define DEVICE_CFG_DUAL_DVOB	0x11
+#define DEVICE_CFG_DUAL_DVOC	0x12
+#define DEVICE_CFG_DUAL_DVOBC	0x13
+#define DEVICE_CFG_DUAL_LINK_DVOBC	0x19
+#define DEVICE_CFG_DUAL_LINK_DVOCB	0x1a
+
+#define DEVICE_WIRE_NONE	0x00
+#define DEVICE_WIRE_DVOB	0x01
+#define DEVICE_WIRE_DVOC	0x02
+#define DEVICE_WIRE_DVOBC	0x03
+#define DEVICE_WIRE_DVOBB	0x05
+#define DEVICE_WIRE_DVOCC	0x06
+#define DEVICE_WIRE_DVOB_MASTER 0x0d
+#define DEVICE_WIRE_DVOC_MASTER 0x0e
+
+#define DEVICE_PORT_DVOA	0x00 /* none on 845+ */
+#define DEVICE_PORT_DVOB	0x01
+#define DEVICE_PORT_DVOC	0x02
+
+struct child_device_config {
+	u16 handle;
+	u16 device_type;
+	u8  device_id[10]; /* ascii string */
+	u16 addin_offset;
+	u8  dvo_port; /* See Device_PORT_* above */
+	u8  i2c_pin;
+	u8  slave_addr;
+	u8  ddc_pin;
+	u16 edid_ptr;
+	u8  dvo_cfg; /* See DEVICE_CFG_* above */
+	u8  dvo2_port;
+	u8  i2c2_pin;
+	u8  slave2_addr;
+	u8  ddc2_pin;
+	u8  capabilities;
+	u8  dvo_wiring;/* See DEVICE_WIRE_* above */
+	u8  dvo2_wiring;
+	u16 extended_type;
+	u8  dvo_function;
+} __attribute__((packed));
+
+
 struct bdb_general_definitions {
 	/* DDC GPIO */
 	u8 crt_ddc_gmbus_pin;
@@ -144,13 +228,18 @@ struct bdb_general_definitions {
 	u8 boot_display[2];
 	u8 child_dev_size;
 
-	/* device info */
-	u8 tv_or_lvds_info[33];
-	u8 dev1[33];
-	u8 dev2[33];
-	u8 dev3[33];
-	u8 dev4[33];
-	/* may be another device block here on some platforms */
+	/*
+	 * Device info:
+	 * If TV is present, it'll be at devices[0].
+	 * LVDS will be next, either devices[0] or [1], if present.
+	 * On some platforms the number of device is 6. But could be as few as
+	 * 4 if both TV and LVDS are missing.
+	 * And the device num is related with the size of general definition
+	 * block. It is obtained by using the following formula:
+	 * number = (block_size - sizeof(bdb_general_definitions))/
+	 *	     sizeof(child_device_config);
+	 */
+	struct child_device_config devices[0];
 };
 
 struct bdb_lvds_options {
@@ -302,6 +391,45 @@ struct bdb_sdvo_lvds_options {
 	u8 panel_misc_bits_4;
 } __attribute__((packed));
 
+struct bdb_driver_features {
+	u8 boot_dev_algorithm:1;
+	u8 block_display_switch:1;
+	u8 allow_display_switch:1;
+	u8 hotplug_dvo:1;
+	u8 dual_view_zoom:1;
+	u8 int15h_hook:1;
+	u8 sprite_in_clone:1;
+	u8 primary_lfp_id:1;
+
+	u16 boot_mode_x;
+	u16 boot_mode_y;
+	u8 boot_mode_bpp;
+	u8 boot_mode_refresh;
+
+	u16 enable_lfp_primary:1;
+	u16 selective_mode_pruning:1;
+	u16 dual_frequency:1;
+	u16 render_clock_freq:1; /* 0: high freq; 1: low freq */
+	u16 nt_clone_support:1;
+	u16 power_scheme_ui:1; /* 0: CUI; 1: 3rd party */
+	u16 sprite_display_assign:1; /* 0: secondary; 1: primary */
+	u16 cui_aspect_scaling:1;
+	u16 preserve_aspect_ratio:1;
+	u16 sdvo_device_power_down:1;
+	u16 crt_hotplug:1;
+	u16 lvds_config:2;
+	u16 tv_hotplug:1;
+	u16 hdmi_config:2;
+
+	u8 static_display:1;
+	u8 reserved2:7;
+	u16 legacy_crt_max_x;
+	u16 legacy_crt_max_y;
+	u8 legacy_crt_max_refresh;
+
+	u8 hdmi_termination;
+	u8 custom_vbt_version;
+} __attribute__((packed));
 
 extern bool psb_intel_init_bios(struct drm_device *dev);
 extern void psb_intel_destroy_bios(struct drm_device *dev);
@@ -427,4 +555,21 @@ extern void psb_intel_destroy_bios(struct drm_device *dev);
 #define   SWF14_APM_STANDBY	0x1
 #define   SWF14_APM_RESTORE	0x0
 
+/* Add the device class for LFP, TV, HDMI */
+#define	 DEVICE_TYPE_INT_LFP	0x1022
+#define	 DEVICE_TYPE_INT_TV	0x1009
+#define	 DEVICE_TYPE_HDMI	0x60D2
+#define	 DEVICE_TYPE_DP		0x68C6
+#define	 DEVICE_TYPE_eDP	0x78C6
+
+/* define the DVO port for HDMI output type */
+#define		DVO_B		1
+#define		DVO_C		2
+#define		DVO_D		3
+
+/* define the PORT for DP output type */
+#define		PORT_IDPB	7
+#define		PORT_IDPC	8
+#define		PORT_IDPD	9
+
 #endif /* _I830_BIOS_H_ */
diff --git a/drivers/gpu/drm/gma500/mdfld_device.c b/drivers/gpu/drm/gma500/mdfld_device.c
index af656787db0..265ad0de44a 100644
--- a/drivers/gpu/drm/gma500/mdfld_device.c
+++ b/drivers/gpu/drm/gma500/mdfld_device.c
@@ -163,142 +163,30 @@ struct backlight_device *mdfld_get_backlight_device(void)
  *
  * Notes: FIXME_JLIU7 need to add the support for DPI MIPI & HDMI audio
  */
-static int mdfld_save_display_registers(struct drm_device *dev, int pipe)
+static int mdfld_save_display_registers(struct drm_device *dev, int pipenum)
 {
 	struct drm_psb_private *dev_priv = dev->dev_private;
 	struct medfield_state *regs = &dev_priv->regs.mdfld;
+	struct psb_pipe *pipe = &dev_priv->regs.pipe[pipenum];
+	const struct psb_offset *map = &dev_priv->regmap[pipenum];
 	int i;
+	u32 *mipi_val;
 
 	/* register */
-	u32 dpll_reg = MRST_DPLL_A;
-	u32 fp_reg = MRST_FPA0;
-	u32 pipeconf_reg = PIPEACONF;
-	u32 htot_reg = HTOTAL_A;
-	u32 hblank_reg = HBLANK_A;
-	u32 hsync_reg = HSYNC_A;
-	u32 vtot_reg = VTOTAL_A;
-	u32 vblank_reg = VBLANK_A;
-	u32 vsync_reg = VSYNC_A;
-	u32 pipesrc_reg = PIPEASRC;
-	u32 dspstride_reg = DSPASTRIDE;
-	u32 dsplinoff_reg = DSPALINOFF;
-	u32 dsptileoff_reg = DSPATILEOFF;
-	u32 dspsize_reg = DSPASIZE;
-	u32 dsppos_reg = DSPAPOS;
-	u32 dspsurf_reg = DSPASURF;
 	u32 mipi_reg = MIPI;
-	u32 dspcntr_reg = DSPACNTR;
-	u32 dspstatus_reg = PIPEASTAT;
-	u32 palette_reg = PALETTE_A;
-
-	/* pointer to values */
-	u32 *dpll_val = &regs->saveDPLL_A;
-	u32 *fp_val = &regs->saveFPA0;
-	u32 *pipeconf_val = &regs->savePIPEACONF;
-	u32 *htot_val = &regs->saveHTOTAL_A;
-	u32 *hblank_val = &regs->saveHBLANK_A;
-	u32 *hsync_val = &regs->saveHSYNC_A;
-	u32 *vtot_val = &regs->saveVTOTAL_A;
-	u32 *vblank_val = &regs->saveVBLANK_A;
-	u32 *vsync_val = &regs->saveVSYNC_A;
-	u32 *pipesrc_val = &regs->savePIPEASRC;
-	u32 *dspstride_val = &regs->saveDSPASTRIDE;
-	u32 *dsplinoff_val = &regs->saveDSPALINOFF;
-	u32 *dsptileoff_val = &regs->saveDSPATILEOFF;
-	u32 *dspsize_val = &regs->saveDSPASIZE;
-	u32 *dsppos_val = &regs->saveDSPAPOS;
-	u32 *dspsurf_val = &regs->saveDSPASURF;
-	u32 *mipi_val = &regs->saveMIPI;
-	u32 *dspcntr_val = &regs->saveDSPACNTR;
-	u32 *dspstatus_val = &regs->saveDSPASTATUS;
-	u32 *palette_val = regs->save_palette_a;
-
-	switch (pipe) {
+
+	switch (pipenum) {
 	case 0:
+		mipi_val = &regs->saveMIPI;
 		break;
 	case 1:
-		/* regester */
-		dpll_reg = MDFLD_DPLL_B;
-		fp_reg = MDFLD_DPLL_DIV0;
-		pipeconf_reg = PIPEBCONF;
-		htot_reg = HTOTAL_B;
-		hblank_reg = HBLANK_B;
-		hsync_reg = HSYNC_B;
-		vtot_reg = VTOTAL_B;
-		vblank_reg = VBLANK_B;
-		vsync_reg = VSYNC_B;
-		pipesrc_reg = PIPEBSRC;
-		dspstride_reg = DSPBSTRIDE;
-		dsplinoff_reg = DSPBLINOFF;
-		dsptileoff_reg = DSPBTILEOFF;
-		dspsize_reg = DSPBSIZE;
-		dsppos_reg = DSPBPOS;
-		dspsurf_reg = DSPBSURF;
-		dspcntr_reg = DSPBCNTR;
-		dspstatus_reg = PIPEBSTAT;
-		palette_reg = PALETTE_B;
-
-		/* values */
-		dpll_val = &regs->saveDPLL_B;
-		fp_val = &regs->saveFPB0;
-		pipeconf_val = &regs->savePIPEBCONF;
-		htot_val = &regs->saveHTOTAL_B;
-		hblank_val = &regs->saveHBLANK_B;
-		hsync_val = &regs->saveHSYNC_B;
-		vtot_val = &regs->saveVTOTAL_B;
-		vblank_val = &regs->saveVBLANK_B;
-		vsync_val = &regs->saveVSYNC_B;
-		pipesrc_val = &regs->savePIPEBSRC;
-		dspstride_val = &regs->saveDSPBSTRIDE;
-		dsplinoff_val = &regs->saveDSPBLINOFF;
-		dsptileoff_val = &regs->saveDSPBTILEOFF;
-		dspsize_val = &regs->saveDSPBSIZE;
-		dsppos_val = &regs->saveDSPBPOS;
-		dspsurf_val = &regs->saveDSPBSURF;
-		dspcntr_val = &regs->saveDSPBCNTR;
-		dspstatus_val = &regs->saveDSPBSTATUS;
-		palette_val = regs->save_palette_b;
+		mipi_val = &regs->saveMIPI;
 		break;
 	case 2:
 		/* register */
-		pipeconf_reg = PIPECCONF;
-		htot_reg = HTOTAL_C;
-		hblank_reg = HBLANK_C;
-		hsync_reg = HSYNC_C;
-		vtot_reg = VTOTAL_C;
-		vblank_reg = VBLANK_C;
-		vsync_reg = VSYNC_C;
-		pipesrc_reg = PIPECSRC;
-		dspstride_reg = DSPCSTRIDE;
-		dsplinoff_reg = DSPCLINOFF;
-		dsptileoff_reg = DSPCTILEOFF;
-		dspsize_reg = DSPCSIZE;
-		dsppos_reg = DSPCPOS;
-		dspsurf_reg = DSPCSURF;
 		mipi_reg = MIPI_C;
-		dspcntr_reg = DSPCCNTR;
-		dspstatus_reg = PIPECSTAT;
-		palette_reg = PALETTE_C;
-
 		/* pointer to values */
-		pipeconf_val = &regs->savePIPECCONF;
-		htot_val = &regs->saveHTOTAL_C;
-		hblank_val = &regs->saveHBLANK_C;
-		hsync_val = &regs->saveHSYNC_C;
-		vtot_val = &regs->saveVTOTAL_C;
-		vblank_val = &regs->saveVBLANK_C;
-		vsync_val = &regs->saveVSYNC_C;
-		pipesrc_val = &regs->savePIPECSRC;
-		dspstride_val = &regs->saveDSPCSTRIDE;
-		dsplinoff_val = &regs->saveDSPCLINOFF;
-		dsptileoff_val = &regs->saveDSPCTILEOFF;
-		dspsize_val = &regs->saveDSPCSIZE;
-		dsppos_val = &regs->saveDSPCPOS;
-		dspsurf_val = &regs->saveDSPCSURF;
 		mipi_val = &regs->saveMIPI_C;
-		dspcntr_val = &regs->saveDSPCCNTR;
-		dspstatus_val = &regs->saveDSPCSTATUS;
-		palette_val = regs->save_palette_c;
 		break;
 	default:
 		DRM_ERROR("%s, invalid pipe number.\n", __func__);
@@ -306,30 +194,30 @@ static int mdfld_save_display_registers(struct drm_device *dev, int pipe)
 	}
 
 	/* Pipe & plane A info */
-	*dpll_val = PSB_RVDC32(dpll_reg);
-	*fp_val = PSB_RVDC32(fp_reg);
-	*pipeconf_val = PSB_RVDC32(pipeconf_reg);
-	*htot_val = PSB_RVDC32(htot_reg);
-	*hblank_val = PSB_RVDC32(hblank_reg);
-	*hsync_val = PSB_RVDC32(hsync_reg);
-	*vtot_val = PSB_RVDC32(vtot_reg);
-	*vblank_val = PSB_RVDC32(vblank_reg);
-	*vsync_val = PSB_RVDC32(vsync_reg);
-	*pipesrc_val = PSB_RVDC32(pipesrc_reg);
-	*dspstride_val = PSB_RVDC32(dspstride_reg);
-	*dsplinoff_val = PSB_RVDC32(dsplinoff_reg);
-	*dsptileoff_val = PSB_RVDC32(dsptileoff_reg);
-	*dspsize_val = PSB_RVDC32(dspsize_reg);
-	*dsppos_val = PSB_RVDC32(dsppos_reg);
-	*dspsurf_val = PSB_RVDC32(dspsurf_reg);
-	*dspcntr_val = PSB_RVDC32(dspcntr_reg);
-	*dspstatus_val = PSB_RVDC32(dspstatus_reg);
+	pipe->dpll = PSB_RVDC32(map->dpll);
+	pipe->fp0 = PSB_RVDC32(map->fp0);
+	pipe->conf = PSB_RVDC32(map->conf);
+	pipe->htotal = PSB_RVDC32(map->htotal);
+	pipe->hblank = PSB_RVDC32(map->hblank);
+	pipe->hsync = PSB_RVDC32(map->hsync);
+	pipe->vtotal = PSB_RVDC32(map->vtotal);
+	pipe->vblank = PSB_RVDC32(map->vblank);
+	pipe->vsync = PSB_RVDC32(map->vsync);
+	pipe->src = PSB_RVDC32(map->src);
+	pipe->stride = PSB_RVDC32(map->stride);
+	pipe->linoff = PSB_RVDC32(map->linoff);
+	pipe->tileoff = PSB_RVDC32(map->tileoff);
+	pipe->size = PSB_RVDC32(map->size);
+	pipe->pos = PSB_RVDC32(map->pos);
+	pipe->surf = PSB_RVDC32(map->surf);
+	pipe->cntr = PSB_RVDC32(map->cntr);
+	pipe->status = PSB_RVDC32(map->status);
 
 	/*save palette (gamma) */
 	for (i = 0; i < 256; i++)
-		palette_val[i] = PSB_RVDC32(palette_reg + (i << 2));
+		pipe->palette[i] = PSB_RVDC32(map->palette + (i << 2));
 
-	if (pipe == 1) {
+	if (pipenum == 1) {
 		regs->savePFIT_CONTROL = PSB_RVDC32(PFIT_CONTROL);
 		regs->savePFIT_PGM_RATIOS = PSB_RVDC32(PFIT_PGM_RATIOS);
 
@@ -349,7 +237,7 @@ static int mdfld_save_display_registers(struct drm_device *dev, int pipe)
  *
  * Notes: FIXME_JLIU7 need to add the support for DPI MIPI & HDMI audio
  */
-static int mdfld_restore_display_registers(struct drm_device *dev, int pipe)
+static int mdfld_restore_display_registers(struct drm_device *dev, int pipenum)
 {
 	/* To get  panel out of ULPS mode. */
 	u32 temp = 0;
@@ -357,142 +245,30 @@ static int mdfld_restore_display_registers(struct drm_device *dev, int pipe)
 	struct drm_psb_private *dev_priv = dev->dev_private;
 	struct mdfld_dsi_config *dsi_config = NULL;
 	struct medfield_state *regs = &dev_priv->regs.mdfld;
-	u32 i = 0;
-	u32 dpll = 0;
+	struct psb_pipe *pipe = &dev_priv->regs.pipe[pipenum];
+	const struct psb_offset *map = &dev_priv->regmap[pipenum];
+	u32 i;
+	u32 dpll;
 	u32 timeout = 0;
 
-	/* regester */
-	u32 dpll_reg = MRST_DPLL_A;
-	u32 fp_reg = MRST_FPA0;
-	u32 pipeconf_reg = PIPEACONF;
-	u32 htot_reg = HTOTAL_A;
-	u32 hblank_reg = HBLANK_A;
-	u32 hsync_reg = HSYNC_A;
-	u32 vtot_reg = VTOTAL_A;
-	u32 vblank_reg = VBLANK_A;
-	u32 vsync_reg = VSYNC_A;
-	u32 pipesrc_reg = PIPEASRC;
-	u32 dspstride_reg = DSPASTRIDE;
-	u32 dsplinoff_reg = DSPALINOFF;
-	u32 dsptileoff_reg = DSPATILEOFF;
-	u32 dspsize_reg = DSPASIZE;
-	u32 dsppos_reg = DSPAPOS;
-	u32 dspsurf_reg = DSPASURF;
-	u32 dspstatus_reg = PIPEASTAT;
+	/* register */
 	u32 mipi_reg = MIPI;
-	u32 dspcntr_reg = DSPACNTR;
-	u32 palette_reg = PALETTE_A;
 
 	/* values */
-	u32 dpll_val = regs->saveDPLL_A & ~DPLL_VCO_ENABLE;
-	u32 fp_val = regs->saveFPA0;
-	u32 pipeconf_val = regs->savePIPEACONF;
-	u32 htot_val = regs->saveHTOTAL_A;
-	u32 hblank_val = regs->saveHBLANK_A;
-	u32 hsync_val = regs->saveHSYNC_A;
-	u32 vtot_val = regs->saveVTOTAL_A;
-	u32 vblank_val = regs->saveVBLANK_A;
-	u32 vsync_val = regs->saveVSYNC_A;
-	u32 pipesrc_val = regs->savePIPEASRC;
-	u32 dspstride_val = regs->saveDSPASTRIDE;
-	u32 dsplinoff_val = regs->saveDSPALINOFF;
-	u32 dsptileoff_val = regs->saveDSPATILEOFF;
-	u32 dspsize_val = regs->saveDSPASIZE;
-	u32 dsppos_val = regs->saveDSPAPOS;
-	u32 dspsurf_val = regs->saveDSPASURF;
-	u32 dspstatus_val = regs->saveDSPASTATUS;
+	u32 dpll_val = pipe->dpll;
 	u32 mipi_val = regs->saveMIPI;
-	u32 dspcntr_val = regs->saveDSPACNTR;
-	u32 *palette_val = regs->save_palette_a;
 
-	switch (pipe) {
+	switch (pipenum) {
 	case 0:
+		dpll_val &= ~DPLL_VCO_ENABLE;
 		dsi_config = dev_priv->dsi_configs[0];
 		break;
 	case 1:
-		/* regester */
-		dpll_reg = MDFLD_DPLL_B;
-		fp_reg = MDFLD_DPLL_DIV0;
-		pipeconf_reg = PIPEBCONF;
-		htot_reg = HTOTAL_B;
-		hblank_reg = HBLANK_B;
-		hsync_reg = HSYNC_B;
-		vtot_reg = VTOTAL_B;
-		vblank_reg = VBLANK_B;
-		vsync_reg = VSYNC_B;
-		pipesrc_reg = PIPEBSRC;
-		dspstride_reg = DSPBSTRIDE;
-		dsplinoff_reg = DSPBLINOFF;
-		dsptileoff_reg = DSPBTILEOFF;
-		dspsize_reg = DSPBSIZE;
-		dsppos_reg = DSPBPOS;
-		dspsurf_reg = DSPBSURF;
-		dspcntr_reg = DSPBCNTR;
-		dspstatus_reg = PIPEBSTAT;
-		palette_reg = PALETTE_B;
-
-		/* values */
-		dpll_val = regs->saveDPLL_B & ~DPLL_VCO_ENABLE;
-		fp_val = regs->saveFPB0;
-		pipeconf_val = regs->savePIPEBCONF;
-		htot_val = regs->saveHTOTAL_B;
-		hblank_val = regs->saveHBLANK_B;
-		hsync_val = regs->saveHSYNC_B;
-		vtot_val = regs->saveVTOTAL_B;
-		vblank_val = regs->saveVBLANK_B;
-		vsync_val = regs->saveVSYNC_B;
-		pipesrc_val = regs->savePIPEBSRC;
-		dspstride_val = regs->saveDSPBSTRIDE;
-		dsplinoff_val = regs->saveDSPBLINOFF;
-		dsptileoff_val = regs->saveDSPBTILEOFF;
-		dspsize_val = regs->saveDSPBSIZE;
-		dsppos_val = regs->saveDSPBPOS;
-		dspsurf_val = regs->saveDSPBSURF;
-		dspcntr_val = regs->saveDSPBCNTR;
-		dspstatus_val = regs->saveDSPBSTATUS;
-		palette_val = regs->save_palette_b;
+		dpll_val &= ~DPLL_VCO_ENABLE;
 		break;
 	case 2:
-		/* regester */
-		pipeconf_reg = PIPECCONF;
-		htot_reg = HTOTAL_C;
-		hblank_reg = HBLANK_C;
-		hsync_reg = HSYNC_C;
-		vtot_reg = VTOTAL_C;
-		vblank_reg = VBLANK_C;
-		vsync_reg = VSYNC_C;
-		pipesrc_reg = PIPECSRC;
-		dspstride_reg = DSPCSTRIDE;
-		dsplinoff_reg = DSPCLINOFF;
-		dsptileoff_reg = DSPCTILEOFF;
-		dspsize_reg = DSPCSIZE;
-		dsppos_reg = DSPCPOS;
-		dspsurf_reg = DSPCSURF;
 		mipi_reg = MIPI_C;
-		dspcntr_reg = DSPCCNTR;
-		dspstatus_reg = PIPECSTAT;
-		palette_reg = PALETTE_C;
-
-		/* values */
-		pipeconf_val = regs->savePIPECCONF;
-		htot_val = regs->saveHTOTAL_C;
-		hblank_val = regs->saveHBLANK_C;
-		hsync_val = regs->saveHSYNC_C;
-		vtot_val = regs->saveVTOTAL_C;
-		vblank_val = regs->saveVBLANK_C;
-		vsync_val = regs->saveVSYNC_C;
-		pipesrc_val = regs->savePIPECSRC;
-		dspstride_val = regs->saveDSPCSTRIDE;
-		dsplinoff_val = regs->saveDSPCLINOFF;
-		dsptileoff_val = regs->saveDSPCTILEOFF;
-		dspsize_val = regs->saveDSPCSIZE;
-		dsppos_val = regs->saveDSPCPOS;
-		dspsurf_val = regs->saveDSPCSURF;
 		mipi_val = regs->saveMIPI_C;
-		dspcntr_val = regs->saveDSPCCNTR;
-		dspstatus_val = regs->saveDSPCSTATUS;
-		palette_val = regs->save_palette_c;
-
 		dsi_config = dev_priv->dsi_configs[1];
 		break;
 	default:
@@ -503,14 +279,14 @@ static int mdfld_restore_display_registers(struct drm_device *dev, int pipe)
 	/*make sure VGA plane is off. it initializes to on after reset!*/
 	PSB_WVDC32(0x80000000, VGACNTRL);
 
-	if (pipe == 1) {
-		PSB_WVDC32(dpll_val & ~DPLL_VCO_ENABLE, dpll_reg);
-		PSB_RVDC32(dpll_reg);
+	if (pipenum == 1) {
+		PSB_WVDC32(dpll_val & ~DPLL_VCO_ENABLE, map->dpll);
+		PSB_RVDC32(map->dpll);
 
-		PSB_WVDC32(fp_val, fp_reg);
+		PSB_WVDC32(pipe->fp0, map->fp0);
 	} else {
 
-		dpll = PSB_RVDC32(dpll_reg);
+		dpll = PSB_RVDC32(map->dpll);
 
 		if (!(dpll & DPLL_VCO_ENABLE)) {
 
@@ -518,23 +294,23 @@ static int mdfld_restore_display_registers(struct drm_device *dev, int pipe)
 			   before enable the VCO */
 			if (dpll & MDFLD_PWR_GATE_EN) {
 				dpll &= ~MDFLD_PWR_GATE_EN;
-				PSB_WVDC32(dpll, dpll_reg);
+				PSB_WVDC32(dpll, map->dpll);
 				/* FIXME_MDFLD PO - change 500 to 1 after PO */
 				udelay(500);
 			}
 
-			PSB_WVDC32(fp_val, fp_reg);
-			PSB_WVDC32(dpll_val, dpll_reg);
+			PSB_WVDC32(pipe->fp0, map->fp0);
+			PSB_WVDC32(dpll_val, map->dpll);
 			/* FIXME_MDFLD PO - change 500 to 1 after PO */
 			udelay(500);
 
 			dpll_val |= DPLL_VCO_ENABLE;
-			PSB_WVDC32(dpll_val, dpll_reg);
-			PSB_RVDC32(dpll_reg);
+			PSB_WVDC32(dpll_val, map->dpll);
+			PSB_RVDC32(map->dpll);
 
 			/* wait for DSI PLL to lock */
 			while (timeout < 20000 &&
-			  !(PSB_RVDC32(pipeconf_reg) & PIPECONF_DSIPLL_LOCK)) {
+			  !(PSB_RVDC32(map->conf) & PIPECONF_DSIPLL_LOCK)) {
 				udelay(150);
 				timeout++;
 			}
@@ -547,28 +323,28 @@ static int mdfld_restore_display_registers(struct drm_device *dev, int pipe)
 		}
 	}
 	/* Restore mode */
-	PSB_WVDC32(htot_val, htot_reg);
-	PSB_WVDC32(hblank_val, hblank_reg);
-	PSB_WVDC32(hsync_val, hsync_reg);
-	PSB_WVDC32(vtot_val, vtot_reg);
-	PSB_WVDC32(vblank_val, vblank_reg);
-	PSB_WVDC32(vsync_val, vsync_reg);
-	PSB_WVDC32(pipesrc_val, pipesrc_reg);
-	PSB_WVDC32(dspstatus_val, dspstatus_reg);
+	PSB_WVDC32(pipe->htotal, map->htotal);
+	PSB_WVDC32(pipe->hblank, map->hblank);
+	PSB_WVDC32(pipe->hsync, map->hsync);
+	PSB_WVDC32(pipe->vtotal, map->vtotal);
+	PSB_WVDC32(pipe->vblank, map->vblank);
+	PSB_WVDC32(pipe->vsync, map->vsync);
+	PSB_WVDC32(pipe->src, map->src);
+	PSB_WVDC32(pipe->status, map->status);
 
 	/*set up the plane*/
-	PSB_WVDC32(dspstride_val, dspstride_reg);
-	PSB_WVDC32(dsplinoff_val, dsplinoff_reg);
-	PSB_WVDC32(dsptileoff_val, dsptileoff_reg);
-	PSB_WVDC32(dspsize_val, dspsize_reg);
-	PSB_WVDC32(dsppos_val, dsppos_reg);
-	PSB_WVDC32(dspsurf_val, dspsurf_reg);
-
-	if (pipe == 1) {
+	PSB_WVDC32(pipe->stride, map->stride);
+	PSB_WVDC32(pipe->linoff, map->linoff);
+	PSB_WVDC32(pipe->tileoff, map->tileoff);
+	PSB_WVDC32(pipe->size, map->size);
+	PSB_WVDC32(pipe->pos, map->pos);
+	PSB_WVDC32(pipe->surf, map->surf);
+
+	if (pipenum == 1) {
 		/* restore palette (gamma) */
 		/*DRM_UDELAY(50000); */
 		for (i = 0; i < 256; i++)
-			PSB_WVDC32(palette_val[i], palette_reg + (i << 2));
+			PSB_WVDC32(pipe->palette[i], map->palette + (i << 2));
 
 		PSB_WVDC32(regs->savePFIT_CONTROL, PFIT_CONTROL);
 		PSB_WVDC32(regs->savePFIT_PGM_RATIOS, PFIT_PGM_RATIOS);
@@ -578,7 +354,7 @@ static int mdfld_restore_display_registers(struct drm_device *dev, int pipe)
 		/*TODO: resume pipe*/
 
 		/*enable the plane*/
-		PSB_WVDC32(dspcntr_val & ~DISPLAY_PLANE_ENABLE, dspcntr_reg);
+		PSB_WVDC32(pipe->cntr & ~DISPLAY_PLANE_ENABLE, map->cntr);
 
 		return 0;
 	}
@@ -588,7 +364,7 @@ static int mdfld_restore_display_registers(struct drm_device *dev, int pipe)
 
 	/*setup MIPI adapter + MIPI IP registers*/
 	if (dsi_config)
-		mdfld_dsi_controller_init(dsi_config, pipe);
+		mdfld_dsi_controller_init(dsi_config, pipenum);
 
 	if (in_atomic() || in_interrupt())
 		mdelay(20);
@@ -596,7 +372,7 @@ static int mdfld_restore_display_registers(struct drm_device *dev, int pipe)
 		msleep(20);
 
 	/*enable the plane*/
-	PSB_WVDC32(dspcntr_val, dspcntr_reg);
+	PSB_WVDC32(pipe->cntr, map->cntr);
 
 	if (in_atomic() || in_interrupt())
 		mdelay(20);
@@ -625,12 +401,12 @@ static int mdfld_restore_display_registers(struct drm_device *dev, int pipe)
 	mdelay(1);
 
 	/*enable the pipe*/
-	PSB_WVDC32(pipeconf_val, pipeconf_reg);
+	PSB_WVDC32(pipe->conf, map->conf);
 
 	/* restore palette (gamma) */
 	/*DRM_UDELAY(50000); */
 	for (i = 0; i < 256; i++)
-		PSB_WVDC32(palette_val[i], palette_reg + (i << 2));
+		PSB_WVDC32(pipe->palette[i], map->palette + (i << 2));
 
 	return 0;
 }
@@ -667,14 +443,98 @@ static int mdfld_power_up(struct drm_device *dev)
 	return 0;
 }
 
+/* Medfield  */
+static const struct psb_offset mdfld_regmap[3] = {
+	{
+		.fp0 = MRST_FPA0,
+		.fp1 = MRST_FPA1,
+		.cntr = DSPACNTR,
+		.conf = PIPEACONF,
+		.src = PIPEASRC,
+		.dpll = MRST_DPLL_A,
+		.htotal = HTOTAL_A,
+		.hblank = HBLANK_A,
+		.hsync = HSYNC_A,
+		.vtotal = VTOTAL_A,
+		.vblank = VBLANK_A,
+		.vsync = VSYNC_A,
+		.stride = DSPASTRIDE,
+		.size = DSPASIZE,
+		.pos = DSPAPOS,
+		.surf = DSPASURF,
+		.addr = MRST_DSPABASE,
+		.status = PIPEASTAT,
+		.linoff = DSPALINOFF,
+		.tileoff = DSPATILEOFF,
+		.palette = PALETTE_A,
+	},
+	{
+		.fp0 = MDFLD_DPLL_DIV0,
+		.cntr = DSPBCNTR,
+		.conf = PIPEBCONF,
+		.src = PIPEBSRC,
+		.dpll = MDFLD_DPLL_B,
+		.htotal = HTOTAL_B,
+		.hblank = HBLANK_B,
+		.hsync = HSYNC_B,
+		.vtotal = VTOTAL_B,
+		.vblank = VBLANK_B,
+		.vsync = VSYNC_B,
+		.stride = DSPBSTRIDE,
+		.size = DSPBSIZE,
+		.pos = DSPBPOS,
+		.surf = DSPBSURF,
+		.addr = MRST_DSPBBASE,
+		.status = PIPEBSTAT,
+		.linoff = DSPBLINOFF,
+		.tileoff = DSPBTILEOFF,
+		.palette = PALETTE_B,
+	},
+	{
+		.fp0 = MRST_FPA0,	/* This is what the old code did ?? */
+		.cntr = DSPCCNTR,
+		.conf = PIPECCONF,
+		.src = PIPECSRC,
+		/* No DPLL_C */
+		.dpll = MRST_DPLL_A,
+		.htotal = HTOTAL_C,
+		.hblank = HBLANK_C,
+		.hsync = HSYNC_C,
+		.vtotal = VTOTAL_C,
+		.vblank = VBLANK_C,
+		.vsync = VSYNC_C,
+		.stride = DSPCSTRIDE,
+		.size = DSPBSIZE,
+		.pos = DSPCPOS,
+		.surf = DSPCSURF,
+		.addr = MDFLD_DSPCBASE,
+		.status = PIPECSTAT,
+		.linoff = DSPCLINOFF,
+		.tileoff = DSPCTILEOFF,
+		.palette = PALETTE_C,
+	},
+};
+
+static int mdfld_chip_setup(struct drm_device *dev)
+{
+	struct drm_psb_private *dev_priv = dev->dev_private;
+	if (pci_enable_msi(dev->pdev))
+		dev_warn(dev->dev, "Enabling MSI failed!\n");
+	dev_priv->regmap = mdfld_regmap;
+	return mid_chip_setup(dev);
+}
+
 const struct psb_ops mdfld_chip_ops = {
 	.name = "mdfld",
 	.accel_2d = 0,
 	.pipes = 3,
 	.crtcs = 3,
+	.lvds_mask = (1 << 1),
+	.hdmi_mask = (1 << 1),
+	.cursor_needs_phys = 0,
 	.sgx_offset = MRST_SGX_OFFSET,
 
-	.chip_setup = mid_chip_setup,
+	.chip_setup = mdfld_chip_setup,
 	.crtc_helper = &mdfld_helper_funcs,
 	.crtc_funcs = &psb_intel_crtc_funcs,
 
diff --git a/drivers/gpu/drm/gma500/mdfld_dsi_dpi.c b/drivers/gpu/drm/gma500/mdfld_dsi_dpi.c
index d52358b744a..b34ff097b97 100644
--- a/drivers/gpu/drm/gma500/mdfld_dsi_dpi.c
+++ b/drivers/gpu/drm/gma500/mdfld_dsi_dpi.c
@@ -869,7 +869,6 @@ void mdfld_dsi_dpi_mode_set(struct drm_encoder *encoder,
 		mdfld_set_pipe_timing(dsi_config, pipe);
 
 		REG_WRITE(DSPABASE, 0x00);
-		REG_WRITE(DSPASTRIDE, (mode->hdisplay * 4));
 		REG_WRITE(DSPASIZE,
 			((mode->vdisplay - 1) << 16) | (mode->hdisplay - 1));
 
diff --git a/drivers/gpu/drm/gma500/mdfld_dsi_pkg_sender.c b/drivers/gpu/drm/gma500/mdfld_dsi_pkg_sender.c
index baa0e14165e..489ffd2c66e 100644
--- a/drivers/gpu/drm/gma500/mdfld_dsi_pkg_sender.c
+++ b/drivers/gpu/drm/gma500/mdfld_dsi_pkg_sender.c
@@ -605,6 +605,8 @@ int mdfld_dsi_pkg_sender_init(struct mdfld_dsi_connector *dsi_connector,
 	struct mdfld_dsi_config *dsi_config =
 				mdfld_dsi_get_config(dsi_connector);
 	struct drm_device *dev = dsi_config->dev;
+	struct drm_psb_private *dev_priv = dev->dev_private;
+	const struct psb_offset *map = &dev_priv->regmap[pipe];
 	u32 mipi_val = 0;
 
 	if (!dsi_connector) {
@@ -632,21 +634,13 @@ int mdfld_dsi_pkg_sender_init(struct mdfld_dsi_connector *dsi_connector,
 	pkg_sender->status = MDFLD_DSI_PKG_SENDER_FREE;
 
 	/*init regs*/
-	if (pipe == 0) {
-		pkg_sender->dpll_reg = MRST_DPLL_A;
-		pkg_sender->dspcntr_reg = DSPACNTR;
-		pkg_sender->pipeconf_reg = PIPEACONF;
-		pkg_sender->dsplinoff_reg = DSPALINOFF;
-		pkg_sender->dspsurf_reg = DSPASURF;
-		pkg_sender->pipestat_reg = PIPEASTAT;
-	} else if (pipe == 2) {
-		pkg_sender->dpll_reg = MRST_DPLL_A;
-		pkg_sender->dspcntr_reg = DSPCCNTR;
-		pkg_sender->pipeconf_reg = PIPECCONF;
-		pkg_sender->dsplinoff_reg = DSPCLINOFF;
-		pkg_sender->dspsurf_reg = DSPCSURF;
-		pkg_sender->pipestat_reg = PIPECSTAT;
-	}
+	/* FIXME: should just copy the regmap ptr ? */
+	pkg_sender->dpll_reg = map->dpll;
+	pkg_sender->dspcntr_reg = map->cntr;
+	pkg_sender->pipeconf_reg = map->conf;
+	pkg_sender->dsplinoff_reg = map->linoff;
+	pkg_sender->dspsurf_reg = map->surf;
+	pkg_sender->pipestat_reg = map->status;
 
 	pkg_sender->mipi_intr_stat_reg = MIPI_INTR_STAT_REG(pipe);
 	pkg_sender->mipi_lp_gen_data_reg = MIPI_LP_GEN_DATA_REG(pipe);
diff --git a/drivers/gpu/drm/gma500/mdfld_intel_display.c b/drivers/gpu/drm/gma500/mdfld_intel_display.c
index a35a2921bdf..3f3cd619c79 100644
--- a/drivers/gpu/drm/gma500/mdfld_intel_display.c
+++ b/drivers/gpu/drm/gma500/mdfld_intel_display.c
@@ -50,17 +50,14 @@ struct mrst_clock_t {
 
 void mdfldWaitForPipeDisable(struct drm_device *dev, int pipe)
 {
+	struct drm_psb_private *dev_priv = dev->dev_private;
+	const struct psb_offset *map = &dev_priv->regmap[pipe];
 	int count, temp;
-	u32 pipeconf_reg = PIPEACONF;
 
 	switch (pipe) {
 	case 0:
-		break;
 	case 1:
-		pipeconf_reg = PIPEBCONF;
-		break;
 	case 2:
-		pipeconf_reg = PIPECCONF;
 		break;
 	default:
 		DRM_ERROR("Illegal Pipe Number.\n");
@@ -73,7 +70,7 @@ void mdfldWaitForPipeDisable(struct drm_device *dev, int pipe)
 
 	/* Wait for for the pipe disable to take effect. */
 	for (count = 0; count < COUNT_MAX; count++) {
-		temp = REG_READ(pipeconf_reg);
+		temp = REG_READ(map->conf);
 		if ((temp & PIPEACONF_PIPE_STATE) == 0)
 			break;
 	}
@@ -81,17 +78,14 @@ void mdfldWaitForPipeDisable(struct drm_device *dev, int pipe)
 
 void mdfldWaitForPipeEnable(struct drm_device *dev, int pipe)
 {
+	struct drm_psb_private *dev_priv = dev->dev_private;
+	const struct psb_offset *map = &dev_priv->regmap[pipe];
 	int count, temp;
-	u32 pipeconf_reg = PIPEACONF;
 
 	switch (pipe) {
 	case 0:
-		break;
 	case 1:
-		pipeconf_reg = PIPEBCONF;
-		break;
 	case 2:
-		pipeconf_reg = PIPECCONF;
 		break;
 	default:
 		DRM_ERROR("Illegal Pipe Number.\n");
@@ -104,7 +98,7 @@ void mdfldWaitForPipeEnable(struct drm_device *dev, int pipe)
 
 	/* Wait for for the pipe enable to take effect. */
 	for (count = 0; count < COUNT_MAX; count++) {
-		temp = REG_READ(pipeconf_reg);
+		temp = REG_READ(map->conf);
 		if ((temp & PIPEACONF_PIPE_STATE) == 1)
 			break;
 	}
@@ -189,15 +183,12 @@ static int mdfld__intel_pipe_set_base(struct drm_crtc *crtc, int x, int y,
 				struct drm_framebuffer *old_fb)
 {
 	struct drm_device *dev = crtc->dev;
-	/* struct drm_i915_master_private *master_priv; */
+	struct drm_psb_private *dev_priv = dev->dev_private;
 	struct psb_intel_crtc *psb_intel_crtc = to_psb_intel_crtc(crtc);
 	struct psb_framebuffer *psbfb = to_psb_fb(crtc->fb);
 	int pipe = psb_intel_crtc->pipe;
+	const struct psb_offset *map = &dev_priv->regmap[pipe];
 	unsigned long start, offset;
-	int dsplinoff = DSPALINOFF;
-	int dspsurf = DSPASURF;
-	int dspstride = DSPASTRIDE;
-	int dspcntr_reg = DSPACNTR;
 	u32 dspcntr;
 	int ret;
 
@@ -215,23 +206,7 @@ static int mdfld__intel_pipe_set_base(struct drm_crtc *crtc, int x, int y,
 	if (ret)
 		return ret;
 
-	switch (pipe) {
-	case 0:
-		dsplinoff = DSPALINOFF;
-		break;
-	case 1:
-		dsplinoff = DSPBLINOFF;
-		dspsurf = DSPBSURF;
-		dspstride = DSPBSTRIDE;
-		dspcntr_reg = DSPBCNTR;
-		break;
-	case 2:
-		dsplinoff = DSPCLINOFF;
-		dspsurf = DSPCSURF;
-		dspstride = DSPCSTRIDE;
-		dspcntr_reg = DSPCCNTR;
-		break;
-	default:
+	if (pipe > 2) {
 		DRM_ERROR("Illegal Pipe Number.\n");
 		return -EINVAL;
 	}
@@ -242,8 +217,8 @@ static int mdfld__intel_pipe_set_base(struct drm_crtc *crtc, int x, int y,
 	start = psbfb->gtt->offset;
 	offset = y * crtc->fb->pitches[0] + x * (crtc->fb->bits_per_pixel / 8);
 
-	REG_WRITE(dspstride, crtc->fb->pitches[0]);
-	dspcntr = REG_READ(dspcntr_reg);
+	REG_WRITE(map->stride, crtc->fb->pitches[0]);
+	dspcntr = REG_READ(map->cntr);
 	dspcntr &= ~DISPPLANE_PIXFORMAT_MASK;
 
 	switch (crtc->fb->bits_per_pixel) {
@@ -261,14 +236,14 @@ static int mdfld__intel_pipe_set_base(struct drm_crtc *crtc, int x, int y,
 		dspcntr |= DISPPLANE_32BPP_NO_ALPHA;
 		break;
 	}
-	REG_WRITE(dspcntr_reg, dspcntr);
+	REG_WRITE(map->cntr, dspcntr);
 
 	dev_dbg(dev->dev, "Writing base %08lX %08lX %d %d\n",
 						start, offset, x, y);
-	REG_WRITE(dsplinoff, offset);
-	REG_READ(dsplinoff);
-	REG_WRITE(dspsurf, start);
-	REG_READ(dspsurf);
+	REG_WRITE(map->linoff, offset);
+	REG_READ(map->linoff);
+	REG_WRITE(map->surf, start);
+	REG_READ(map->surf);
 
 	gma_power_end(dev);
 
@@ -281,78 +256,56 @@ static int mdfld__intel_pipe_set_base(struct drm_crtc *crtc, int x, int y,
  */
 void mdfld_disable_crtc(struct drm_device *dev, int pipe)
 {
-	int dpll_reg = MRST_DPLL_A;
-	int dspcntr_reg = DSPACNTR;
-	int dspbase_reg = MRST_DSPABASE;
-	int pipeconf_reg = PIPEACONF;
+	struct drm_psb_private *dev_priv = dev->dev_private;
+	const struct psb_offset *map = &dev_priv->regmap[pipe];
 	u32 temp;
 
 	dev_dbg(dev->dev, "pipe = %d\n", pipe);
 
 
-	switch (pipe) {
-	case 0:
-		break;
-	case 1:
-		dpll_reg = MDFLD_DPLL_B;
-		dspcntr_reg = DSPBCNTR;
-		dspbase_reg = DSPBSURF;
-		pipeconf_reg = PIPEBCONF;
-		break;
-	case 2:
-		dpll_reg = MRST_DPLL_A;
-		dspcntr_reg = DSPCCNTR;
-		dspbase_reg = MDFLD_DSPCBASE;
-		pipeconf_reg = PIPECCONF;
-		break;
-	default:
-		DRM_ERROR("Illegal Pipe Number.\n");
-		return;
-	}
-
 	if (pipe != 1)
 		mdfld_dsi_gen_fifo_ready(dev, MIPI_GEN_FIFO_STAT_REG(pipe),
 				HS_CTRL_FIFO_EMPTY | HS_DATA_FIFO_EMPTY);
 
 	/* Disable display plane */
-	temp = REG_READ(dspcntr_reg);
+	temp = REG_READ(map->cntr);
 	if ((temp & DISPLAY_PLANE_ENABLE) != 0) {
-		REG_WRITE(dspcntr_reg,
+		REG_WRITE(map->cntr,
 			  temp & ~DISPLAY_PLANE_ENABLE);
 		/* Flush the plane changes */
-		REG_WRITE(dspbase_reg, REG_READ(dspbase_reg));
-		REG_READ(dspbase_reg);
+		REG_WRITE(map->base, REG_READ(map->base));
+		REG_READ(map->base);
 	}
 
 	/* FIXME_JLIU7 MDFLD_PO revisit */
 
 	/* Next, disable display pipes */
-	temp = REG_READ(pipeconf_reg);
+	temp = REG_READ(map->conf);
 	if ((temp & PIPEACONF_ENABLE) != 0) {
 		temp &= ~PIPEACONF_ENABLE;
 		temp |= PIPECONF_PLANE_OFF | PIPECONF_CURSOR_OFF;
-		REG_WRITE(pipeconf_reg, temp);
-		REG_READ(pipeconf_reg);
+		REG_WRITE(map->conf, temp);
+		REG_READ(map->conf);
 
 		/* Wait for for the pipe disable to take effect. */
 		mdfldWaitForPipeDisable(dev, pipe);
 	}
 
-	temp = REG_READ(dpll_reg);
+	temp = REG_READ(map->dpll);
 	if (temp & DPLL_VCO_ENABLE) {
 		if ((pipe != 1 &&
 			!((REG_READ(PIPEACONF) | REG_READ(PIPECCONF))
 				& PIPEACONF_ENABLE)) || pipe == 1) {
 			temp &= ~(DPLL_VCO_ENABLE);
-			REG_WRITE(dpll_reg, temp);
-			REG_READ(dpll_reg);
+			REG_WRITE(map->dpll, temp);
+			REG_READ(map->dpll);
 			/* Wait for the clocks to turn off. */
 			/* FIXME_MDFLD PO may need more delay */
 			udelay(500);
 
 			if (!(temp & MDFLD_PWR_GATE_EN)) {
 				/* gating power of DPLL */
-				REG_WRITE(dpll_reg, temp | MDFLD_PWR_GATE_EN);
+				REG_WRITE(map->dpll, temp | MDFLD_PWR_GATE_EN);
 				/* FIXME_MDFLD PO - change 500 to 1 after PO */
 				udelay(5000);
 			}
@@ -373,41 +326,15 @@ static void mdfld_crtc_dpms(struct drm_crtc *crtc, int mode)
 	struct drm_psb_private *dev_priv = dev->dev_private;
 	struct psb_intel_crtc *psb_intel_crtc = to_psb_intel_crtc(crtc);
 	int pipe = psb_intel_crtc->pipe;
-	int dpll_reg = MRST_DPLL_A;
-	int dspcntr_reg = DSPACNTR;
-	int dspbase_reg = MRST_DSPABASE;
-	int pipeconf_reg = PIPEACONF;
-	u32 pipestat_reg = PIPEASTAT;
+	const struct psb_offset *map = &dev_priv->regmap[pipe];
 	u32 pipeconf = dev_priv->pipeconf[pipe];
 	u32 temp;
 	int timeout = 0;
 
 	dev_dbg(dev->dev, "mode = %d, pipe = %d\n", mode, pipe);
 
-/* FIXME_JLIU7 MDFLD_PO replaced w/ the following function */
-/* mdfld_dbi_dpms (struct drm_device *dev, int pipe, bool enabled) */
-
-	switch (pipe) {
-	case 0:
-		break;
-	case 1:
-		dpll_reg = DPLL_B;
-		dspcntr_reg = DSPBCNTR;
-		dspbase_reg = MRST_DSPBBASE;
-		pipeconf_reg = PIPEBCONF;
-		dpll_reg = MDFLD_DPLL_B;
-		break;
-	case 2:
-		dpll_reg = MRST_DPLL_A;
-		dspcntr_reg = DSPCCNTR;
-		dspbase_reg = MDFLD_DSPCBASE;
-		pipeconf_reg = PIPECCONF;
-		pipestat_reg = PIPECSTAT;
-		break;
-	default:
-		DRM_ERROR("Illegal Pipe Number.\n");
-		return;
-	}
+	/* Note: Old code uses pipe a stat for pipe b but that appears
+	   to be a bug */
 
 	if (!gma_power_begin(dev, true))
 		return;
@@ -420,25 +347,25 @@ static void mdfld_crtc_dpms(struct drm_crtc *crtc, int mode)
 	case DRM_MODE_DPMS_STANDBY:
 	case DRM_MODE_DPMS_SUSPEND:
 		/* Enable the DPLL */
-		temp = REG_READ(dpll_reg);
+		temp = REG_READ(map->dpll);
 
 		if ((temp & DPLL_VCO_ENABLE) == 0) {
 			/* When ungating power of DPLL, needs to wait 0.5us
 			   before enable the VCO */
 			if (temp & MDFLD_PWR_GATE_EN) {
 				temp &= ~MDFLD_PWR_GATE_EN;
-				REG_WRITE(dpll_reg, temp);
+				REG_WRITE(map->dpll, temp);
 				/* FIXME_MDFLD PO - change 500 to 1 after PO */
 				udelay(500);
 			}
 
-			REG_WRITE(dpll_reg, temp);
-			REG_READ(dpll_reg);
+			REG_WRITE(map->dpll, temp);
+			REG_READ(map->dpll);
 			/* FIXME_MDFLD PO - change 500 to 1 after PO */
 			udelay(500);
 
-			REG_WRITE(dpll_reg, temp | DPLL_VCO_ENABLE);
-			REG_READ(dpll_reg);
+			REG_WRITE(map->dpll, temp | DPLL_VCO_ENABLE);
+			REG_READ(map->dpll);
 
 			/**
 			 * wait for DSI PLL to lock
@@ -446,25 +373,25 @@ static void mdfld_crtc_dpms(struct drm_crtc *crtc, int mode)
 			 * since both MIPI pipes share the same PLL.
 			 */
 			while ((pipe != 2) && (timeout < 20000) &&
-			  !(REG_READ(pipeconf_reg) & PIPECONF_DSIPLL_LOCK)) {
+			  !(REG_READ(map->conf) & PIPECONF_DSIPLL_LOCK)) {
 				udelay(150);
 				timeout++;
 			}
 		}
 
 		/* Enable the plane */
-		temp = REG_READ(dspcntr_reg);
+		temp = REG_READ(map->cntr);
 		if ((temp & DISPLAY_PLANE_ENABLE) == 0) {
-			REG_WRITE(dspcntr_reg,
+			REG_WRITE(map->cntr,
 				temp | DISPLAY_PLANE_ENABLE);
 			/* Flush the plane changes */
-			REG_WRITE(dspbase_reg, REG_READ(dspbase_reg));
+			REG_WRITE(map->base, REG_READ(map->base));
 		}
 
 		/* Enable the pipe */
-		temp = REG_READ(pipeconf_reg);
+		temp = REG_READ(map->conf);
 		if ((temp & PIPEACONF_ENABLE) == 0) {
-			REG_WRITE(pipeconf_reg, pipeconf);
+			REG_WRITE(map->conf, pipeconf);
 
 			/* Wait for for the pipe enable to take effect. */
 			mdfldWaitForPipeEnable(dev, pipe);
@@ -473,39 +400,39 @@ static void mdfld_crtc_dpms(struct drm_crtc *crtc, int mode)
 		/*workaround for sighting 3741701 Random X blank display*/
 		/*perform w/a in video mode only on pipe A or C*/
 		if (pipe == 0 || pipe == 2) {
-			REG_WRITE(pipestat_reg, REG_READ(pipestat_reg));
+			REG_WRITE(map->status, REG_READ(map->status));
 			msleep(100);
-			if (PIPE_VBLANK_STATUS & REG_READ(pipestat_reg))
+			if (PIPE_VBLANK_STATUS & REG_READ(map->status))
 				dev_dbg(dev->dev, "OK");
 			else {
 				dev_dbg(dev->dev, "STUCK!!!!");
 				/*shutdown controller*/
-				temp = REG_READ(dspcntr_reg);
-				REG_WRITE(dspcntr_reg,
+				temp = REG_READ(map->cntr);
+				REG_WRITE(map->cntr,
 						temp & ~DISPLAY_PLANE_ENABLE);
-				REG_WRITE(dspbase_reg, REG_READ(dspbase_reg));
+				REG_WRITE(map->base, REG_READ(map->base));
 				/*mdfld_dsi_dpi_shut_down(dev, pipe);*/
 				REG_WRITE(0xb048, 1);
 				msleep(100);
-				temp = REG_READ(pipeconf_reg);
+				temp = REG_READ(map->conf);
 				temp &= ~PIPEACONF_ENABLE;
-				REG_WRITE(pipeconf_reg, temp);
+				REG_WRITE(map->conf, temp);
 				msleep(100); /*wait for pipe disable*/
 				REG_WRITE(MIPI_DEVICE_READY_REG(pipe), 0);
 				msleep(100);
 				REG_WRITE(0xb004, REG_READ(0xb004));
 				/* try to bring the controller back up again*/
 				REG_WRITE(MIPI_DEVICE_READY_REG(pipe), 1);
-				temp = REG_READ(dspcntr_reg);
-				REG_WRITE(dspcntr_reg,
+				temp = REG_READ(map->cntr);
+				REG_WRITE(map->cntr,
 						temp | DISPLAY_PLANE_ENABLE);
-				REG_WRITE(dspbase_reg, REG_READ(dspbase_reg));
+				REG_WRITE(map->base, REG_READ(map->base));
 				/*mdfld_dsi_dpi_turn_on(dev, pipe);*/
 				REG_WRITE(0xb048, 2);
 				msleep(100);
-				temp = REG_READ(pipeconf_reg);
+				temp = REG_READ(map->conf);
 				temp |= PIPEACONF_ENABLE;
-				REG_WRITE(pipeconf_reg, temp);
+				REG_WRITE(map->conf, temp);
 			}
 		}
 
@@ -529,35 +456,35 @@ static void mdfld_crtc_dpms(struct drm_crtc *crtc, int mode)
 		REG_WRITE(VGACNTRL, VGA_DISP_DISABLE);
 
 		/* Disable display plane */
-		temp = REG_READ(dspcntr_reg);
+		temp = REG_READ(map->cntr);
 		if ((temp & DISPLAY_PLANE_ENABLE) != 0) {
-			REG_WRITE(dspcntr_reg,
+			REG_WRITE(map->cntr,
 				  temp & ~DISPLAY_PLANE_ENABLE);
 			/* Flush the plane changes */
-			REG_WRITE(dspbase_reg, REG_READ(dspbase_reg));
-			REG_READ(dspbase_reg);
+			REG_WRITE(map->base, REG_READ(map->base));
+			REG_READ(map->base);
 		}
 
 		/* Next, disable display pipes */
-		temp = REG_READ(pipeconf_reg);
+		temp = REG_READ(map->conf);
 		if ((temp & PIPEACONF_ENABLE) != 0) {
 			temp &= ~PIPEACONF_ENABLE;
 			temp |= PIPECONF_PLANE_OFF | PIPECONF_CURSOR_OFF;
-			REG_WRITE(pipeconf_reg, temp);
-			REG_READ(pipeconf_reg);
+			REG_WRITE(map->conf, temp);
+			REG_READ(map->conf);
 
 			/* Wait for for the pipe disable to take effect. */
 			mdfldWaitForPipeDisable(dev, pipe);
 		}
 
-		temp = REG_READ(dpll_reg);
+		temp = REG_READ(map->dpll);
 		if (temp & DPLL_VCO_ENABLE) {
 			if ((pipe != 1 && !((REG_READ(PIPEACONF)
 				| REG_READ(PIPECCONF)) & PIPEACONF_ENABLE))
 					|| pipe == 1) {
 				temp &= ~(DPLL_VCO_ENABLE);
-				REG_WRITE(dpll_reg, temp);
-				REG_READ(dpll_reg);
+				REG_WRITE(map->dpll, temp);
+				REG_READ(map->dpll);
 				/* Wait for the clocks to turn off. */
 				/* FIXME_MDFLD PO may need more delay */
 				udelay(500);
@@ -764,21 +691,7 @@ static int mdfld_crtc_mode_set(struct drm_crtc *crtc,
 	struct psb_intel_crtc *psb_intel_crtc = to_psb_intel_crtc(crtc);
 	struct drm_psb_private *dev_priv = dev->dev_private;
 	int pipe = psb_intel_crtc->pipe;
-	int fp_reg = MRST_FPA0;
-	int dpll_reg = MRST_DPLL_A;
-	int dspcntr_reg = DSPACNTR;
-	int pipeconf_reg = PIPEACONF;
-	int htot_reg = HTOTAL_A;
-	int hblank_reg = HBLANK_A;
-	int hsync_reg = HSYNC_A;
-	int vtot_reg = VTOTAL_A;
-	int vblank_reg = VBLANK_A;
-	int vsync_reg = VSYNC_A;
-	int dspsize_reg = DSPASIZE;
-	int dsppos_reg = DSPAPOS;
-	int pipesrc_reg = PIPEASRC;
-	u32 *pipeconf = &dev_priv->pipeconf[pipe];
-	u32 *dspcntr = &dev_priv->dspcntr[pipe];
+	const struct psb_offset *map = &dev_priv->regmap[pipe];
 	int refclk = 0;
 	int clk_n = 0, clk_p2 = 0, clk_byte = 1, clk = 0, m_conv = 0,
 								clk_tmp = 0;
@@ -806,45 +719,6 @@ static int mdfld_crtc_mode_set(struct drm_crtc *crtc,
 	}
 #endif
 
-	switch (pipe) {
-	case 0:
-		break;
-	case 1:
-		fp_reg = FPB0;
-		dpll_reg = DPLL_B;
-		dspcntr_reg = DSPBCNTR;
-		pipeconf_reg = PIPEBCONF;
-		htot_reg = HTOTAL_B;
-		hblank_reg = HBLANK_B;
-		hsync_reg = HSYNC_B;
-		vtot_reg = VTOTAL_B;
-		vblank_reg = VBLANK_B;
-		vsync_reg = VSYNC_B;
-		dspsize_reg = DSPBSIZE;
-		dsppos_reg = DSPBPOS;
-		pipesrc_reg = PIPEBSRC;
-		fp_reg = MDFLD_DPLL_DIV0;
-		dpll_reg = MDFLD_DPLL_B;
-		break;
-	case 2:
-		dpll_reg = MRST_DPLL_A;
-		dspcntr_reg = DSPCCNTR;
-		pipeconf_reg = PIPECCONF;
-		htot_reg = HTOTAL_C;
-		hblank_reg = HBLANK_C;
-		hsync_reg = HSYNC_C;
-		vtot_reg = VTOTAL_C;
-		vblank_reg = VBLANK_C;
-		vsync_reg = VSYNC_C;
-		dspsize_reg = DSPCSIZE;
-		dsppos_reg = DSPCPOS;
-		pipesrc_reg = PIPECSRC;
-		break;
-	default:
-		DRM_ERROR("Illegal Pipe Number.\n");
-		return 0;
-	}
-
 	ret = check_fb(crtc->fb);
 	if (ret)
 		return ret;
@@ -929,21 +803,21 @@ static int mdfld_crtc_mode_set(struct drm_crtc *crtc,
 		 * contained within the displayable area of the screen image
 		 * (frame buffer).
 		 */
-		REG_WRITE(dspsize_reg, ((min(mode->crtc_vdisplay, adjusted_mode->crtc_vdisplay) - 1) << 16)
+		REG_WRITE(map->size, ((min(mode->crtc_vdisplay, adjusted_mode->crtc_vdisplay) - 1) << 16)
 				| (min(mode->crtc_hdisplay, adjusted_mode->crtc_hdisplay) - 1));
 		/* Set the CRTC with encoder mode. */
-		REG_WRITE(pipesrc_reg, ((mode->crtc_hdisplay - 1) << 16)
+		REG_WRITE(map->src, ((mode->crtc_hdisplay - 1) << 16)
 				 | (mode->crtc_vdisplay - 1));
 	} else {
-		REG_WRITE(dspsize_reg,
+		REG_WRITE(map->size,
 				((mode->crtc_vdisplay - 1) << 16) |
 						(mode->crtc_hdisplay - 1));
-		REG_WRITE(pipesrc_reg,
+		REG_WRITE(map->src,
 				((mode->crtc_hdisplay - 1) << 16) |
 						(mode->crtc_vdisplay - 1));
 	}
 
-	REG_WRITE(dsppos_reg, 0);
+	REG_WRITE(map->pos, 0);
 
 	if (psb_intel_encoder)
 		drm_connector_property_get_value(connector,
@@ -961,34 +835,34 @@ static int mdfld_crtc_mode_set(struct drm_crtc *crtc,
 		offsetY = (adjusted_mode->crtc_vdisplay -
 					mode->crtc_vdisplay) / 2;
 
-		REG_WRITE(htot_reg, (mode->crtc_hdisplay - 1) |
+		REG_WRITE(map->htotal, (mode->crtc_hdisplay - 1) |
 			((adjusted_mode->crtc_htotal - 1) << 16));
-		REG_WRITE(vtot_reg, (mode->crtc_vdisplay - 1) |
+		REG_WRITE(map->vtotal, (mode->crtc_vdisplay - 1) |
 			((adjusted_mode->crtc_vtotal - 1) << 16));
-		REG_WRITE(hblank_reg, (adjusted_mode->crtc_hblank_start -
+		REG_WRITE(map->hblank, (adjusted_mode->crtc_hblank_start -
 								offsetX - 1) |
 			((adjusted_mode->crtc_hblank_end - offsetX - 1) << 16));
-		REG_WRITE(hsync_reg, (adjusted_mode->crtc_hsync_start -
+		REG_WRITE(map->hsync, (adjusted_mode->crtc_hsync_start -
 								offsetX - 1) |
 			((adjusted_mode->crtc_hsync_end - offsetX - 1) << 16));
-		REG_WRITE(vblank_reg, (adjusted_mode->crtc_vblank_start -
+		REG_WRITE(map->vblank, (adjusted_mode->crtc_vblank_start -
 								offsetY - 1) |
 			((adjusted_mode->crtc_vblank_end - offsetY - 1) << 16));
-		REG_WRITE(vsync_reg, (adjusted_mode->crtc_vsync_start -
+		REG_WRITE(map->vsync, (adjusted_mode->crtc_vsync_start -
 								offsetY - 1) |
 			((adjusted_mode->crtc_vsync_end - offsetY - 1) << 16));
 	} else {
-		REG_WRITE(htot_reg, (adjusted_mode->crtc_hdisplay - 1) |
+		REG_WRITE(map->htotal, (adjusted_mode->crtc_hdisplay - 1) |
 			((adjusted_mode->crtc_htotal - 1) << 16));
-		REG_WRITE(vtot_reg, (adjusted_mode->crtc_vdisplay - 1) |
+		REG_WRITE(map->vtotal, (adjusted_mode->crtc_vdisplay - 1) |
 			((adjusted_mode->crtc_vtotal - 1) << 16));
-		REG_WRITE(hblank_reg, (adjusted_mode->crtc_hblank_start - 1) |
+		REG_WRITE(map->hblank, (adjusted_mode->crtc_hblank_start - 1) |
 			((adjusted_mode->crtc_hblank_end - 1) << 16));
-		REG_WRITE(hsync_reg, (adjusted_mode->crtc_hsync_start - 1) |
+		REG_WRITE(map->hsync, (adjusted_mode->crtc_hsync_start - 1) |
 			((adjusted_mode->crtc_hsync_end - 1) << 16));
-		REG_WRITE(vblank_reg, (adjusted_mode->crtc_vblank_start - 1) |
+		REG_WRITE(map->vblank, (adjusted_mode->crtc_vblank_start - 1) |
 			((adjusted_mode->crtc_vblank_end - 1) << 16));
-		REG_WRITE(vsync_reg, (adjusted_mode->crtc_vsync_start - 1) |
+		REG_WRITE(map->vsync, (adjusted_mode->crtc_vsync_start - 1) |
 			((adjusted_mode->crtc_vsync_end - 1) << 16));
 	}
 
@@ -1000,12 +874,12 @@ static int mdfld_crtc_mode_set(struct drm_crtc *crtc,
 	}
 
 	/* setup pipeconf */
-	*pipeconf = PIPEACONF_ENABLE; /* FIXME_JLIU7 REG_READ(pipeconf_reg); */
+	dev_priv->pipeconf[pipe] = PIPEACONF_ENABLE; /* FIXME_JLIU7 REG_READ(pipeconf_reg); */
 
 	/* Set up the display plane register */
-	*dspcntr = REG_READ(dspcntr_reg);
-	*dspcntr |= pipe << DISPPLANE_SEL_PIPE_POS;
-	*dspcntr |= DISPLAY_PLANE_ENABLE;
+	dev_priv->dspcntr[pipe] = REG_READ(map->cntr);
+	dev_priv->dspcntr[pipe] |= pipe << DISPPLANE_SEL_PIPE_POS;
+	dev_priv->dspcntr[pipe] |= DISPLAY_PLANE_ENABLE;
 
 	if (is_mipi2)
 		goto mrst_crtc_mode_set_exit;
@@ -1070,21 +944,21 @@ static int mdfld_crtc_mode_set(struct drm_crtc *crtc,
 					clock.p1, m_conv);
 		}
 
-		dpll = REG_READ(dpll_reg);
+		dpll = REG_READ(map->dpll);
 
 		if (dpll & DPLL_VCO_ENABLE) {
 			dpll &= ~DPLL_VCO_ENABLE;
-			REG_WRITE(dpll_reg, dpll);
-			REG_READ(dpll_reg);
+			REG_WRITE(map->dpll, dpll);
+			REG_READ(map->dpll);
 
 			/* FIXME jliu7 check the DPLL lock bit PIPEACONF[29] */
 			/* FIXME_MDFLD PO - change 500 to 1 after PO */
 			udelay(500);
 
 			/* reset M1, N1 & P1 */
-			REG_WRITE(fp_reg, 0);
+			REG_WRITE(map->fp0, 0);
 			dpll &= ~MDFLD_P1_MASK;
-			REG_WRITE(dpll_reg, dpll);
+			REG_WRITE(map->dpll, dpll);
 			/* FIXME_MDFLD PO - change 500 to 1 after PO */
 			udelay(500);
 		}
@@ -1093,7 +967,7 @@ static int mdfld_crtc_mode_set(struct drm_crtc *crtc,
 		 * enable the VCO */
 		if (dpll & MDFLD_PWR_GATE_EN) {
 			dpll &= ~MDFLD_PWR_GATE_EN;
-			REG_WRITE(dpll_reg, dpll);
+			REG_WRITE(map->dpll, dpll);
 			/* FIXME_MDFLD PO - change 500 to 1 after PO */
 			udelay(500);
 		}
@@ -1134,18 +1008,18 @@ static int mdfld_crtc_mode_set(struct drm_crtc *crtc,
 		fp = 0x000000c1;
 	}
 
-	REG_WRITE(fp_reg, fp);
-	REG_WRITE(dpll_reg, dpll);
+	REG_WRITE(map->fp0, fp);
+	REG_WRITE(map->dpll, dpll);
 	/* FIXME_MDFLD PO - change 500 to 1 after PO */
 	udelay(500);
 
 	dpll |= DPLL_VCO_ENABLE;
-	REG_WRITE(dpll_reg, dpll);
-	REG_READ(dpll_reg);
+	REG_WRITE(map->dpll, dpll);
+	REG_READ(map->dpll);
 
 	/* wait for DSI PLL to lock */
 	while (timeout < 20000 &&
-			!(REG_READ(pipeconf_reg) & PIPECONF_DSIPLL_LOCK)) {
+			!(REG_READ(map->conf) & PIPECONF_DSIPLL_LOCK)) {
 		udelay(150);
 		timeout++;
 	}
@@ -1155,11 +1029,11 @@ static int mdfld_crtc_mode_set(struct drm_crtc *crtc,
 
 	dev_dbg(dev->dev, "is_mipi = 0x%x\n", is_mipi);
 
-	REG_WRITE(pipeconf_reg, *pipeconf);
-	REG_READ(pipeconf_reg);
+	REG_WRITE(map->conf, dev_priv->pipeconf[pipe]);
+	REG_READ(map->conf);
 
 	/* Wait for for the pipe enable to take effect. */
-	REG_WRITE(dspcntr_reg, *dspcntr);
+	REG_WRITE(map->cntr, dev_priv->dspcntr[pipe]);
 	psb_intel_wait_for_vblank(dev);
 
 mrst_crtc_mode_set_exit:
diff --git a/drivers/gpu/drm/gma500/mid_bios.c b/drivers/gpu/drm/gma500/mid_bios.c
index 5eee9ad80da..b2a790bd989 100644
--- a/drivers/gpu/drm/gma500/mid_bios.c
+++ b/drivers/gpu/drm/gma500/mid_bios.c
@@ -118,139 +118,214 @@ static void mid_get_pci_revID(struct drm_psb_private *dev_priv)
 					dev_priv->platform_rev_id);
 }
 
+struct vbt_header {
+	u32 signature;
+	u8 revision;
+} __packed;
+
+/* The same for r0 and r1 */
+struct vbt_r0 {
+	struct vbt_header vbt_header;
+	u8 size;
+	u8 checksum;
+} __packed;
+
+struct vbt_r10 {
+	struct vbt_header vbt_header;
+	u8 checksum;
+	u16 size;
+	u8 panel_count;
+	u8 primary_panel_idx;
+	u8 secondary_panel_idx;
+	u8 __reserved[5];
+} __packed;
+
+static int read_vbt_r0(u32 addr, struct vbt_r0 *vbt)
+{
+	void __iomem *vbt_virtual;
+
+	vbt_virtual = ioremap(addr, sizeof(*vbt));
+	if (vbt_virtual == NULL)
+		return -1;
+
+	memcpy_fromio(vbt, vbt_virtual, sizeof(*vbt));
+	iounmap(vbt_virtual);
+
+	return 0;
+}
+
+static int read_vbt_r10(u32 addr, struct vbt_r10 *vbt)
+{
+	void __iomem *vbt_virtual;
+
+	vbt_virtual = ioremap(addr, sizeof(*vbt));
+	if (!vbt_virtual)
+		return -1;
+
+	memcpy_fromio(vbt, vbt_virtual, sizeof(*vbt));
+	iounmap(vbt_virtual);
+
+	return 0;
+}
+
+static int mid_get_vbt_data_r0(struct drm_psb_private *dev_priv, u32 addr)
+{
+	struct vbt_r0 vbt;
+	void __iomem *gct_virtual;
+	struct gct_r0 gct;
+	u8 bpi;
+
+	if (read_vbt_r0(addr, &vbt))
+		return -1;
+
+	gct_virtual = ioremap(addr + sizeof(vbt), vbt.size - sizeof(vbt));
+	if (!gct_virtual)
+		return -1;
+	memcpy_fromio(&gct, gct_virtual, sizeof(gct));
+	iounmap(gct_virtual);
+
+	bpi = gct.PD.BootPanelIndex;
+	dev_priv->gct_data.bpi = bpi;
+	dev_priv->gct_data.pt = gct.PD.PanelType;
+	dev_priv->gct_data.DTD = gct.panel[bpi].DTD;
+	dev_priv->gct_data.Panel_Port_Control =
+		gct.panel[bpi].Panel_Port_Control;
+	dev_priv->gct_data.Panel_MIPI_Display_Descriptor =
+		gct.panel[bpi].Panel_MIPI_Display_Descriptor;
+
+	return 0;
+}
+
+static int mid_get_vbt_data_r1(struct drm_psb_private *dev_priv, u32 addr)
+{
+	struct vbt_r0 vbt;
+	void __iomem *gct_virtual;
+	struct gct_r1 gct;
+	u8 bpi;
+
+	if (read_vbt_r0(addr, &vbt))
+		return -1;
+
+	gct_virtual = ioremap(addr + sizeof(vbt), vbt.size - sizeof(vbt));
+	if (!gct_virtual)
+		return -1;
+	memcpy_fromio(&gct, gct_virtual, sizeof(gct));
+	iounmap(gct_virtual);
+
+	bpi = gct.PD.BootPanelIndex;
+	dev_priv->gct_data.bpi = bpi;
+	dev_priv->gct_data.pt = gct.PD.PanelType;
+	dev_priv->gct_data.DTD = gct.panel[bpi].DTD;
+	dev_priv->gct_data.Panel_Port_Control =
+		gct.panel[bpi].Panel_Port_Control;
+	dev_priv->gct_data.Panel_MIPI_Display_Descriptor =
+		gct.panel[bpi].Panel_MIPI_Display_Descriptor;
+
+	return 0;
+}
+
+static int mid_get_vbt_data_r10(struct drm_psb_private *dev_priv, u32 addr)
+{
+	struct vbt_r10 vbt;
+	void __iomem *gct_virtual;
+	struct gct_r10 *gct;
+	struct oaktrail_timing_info *dp_ti = &dev_priv->gct_data.DTD;
+	struct gct_r10_timing_info *ti;
+	int ret = -1;
+
+	if (read_vbt_r10(addr, &vbt))
+		return -1;
+
+	gct = kmalloc(sizeof(*gct) * vbt.panel_count, GFP_KERNEL);
+	if (!gct)
+		return -1;
+
+	gct_virtual = ioremap(addr + sizeof(vbt),
+			sizeof(*gct) * vbt.panel_count);
+	if (!gct_virtual)
+		goto out;
+	memcpy_fromio(gct, gct_virtual, sizeof(*gct));
+	iounmap(gct_virtual);
+
+	dev_priv->gct_data.bpi = vbt.primary_panel_idx;
+	dev_priv->gct_data.Panel_MIPI_Display_Descriptor =
+		gct[vbt.primary_panel_idx].Panel_MIPI_Display_Descriptor;
+
+	ti = &gct[vbt.primary_panel_idx].DTD;
+	dp_ti->pixel_clock = ti->pixel_clock;
+	dp_ti->hactive_hi = ti->hactive_hi;
+	dp_ti->hactive_lo = ti->hactive_lo;
+	dp_ti->hblank_hi = ti->hblank_hi;
+	dp_ti->hblank_lo = ti->hblank_lo;
+	dp_ti->hsync_offset_hi = ti->hsync_offset_hi;
+	dp_ti->hsync_offset_lo = ti->hsync_offset_lo;
+	dp_ti->hsync_pulse_width_hi = ti->hsync_pulse_width_hi;
+	dp_ti->hsync_pulse_width_lo = ti->hsync_pulse_width_lo;
+	dp_ti->vactive_hi = ti->vactive_hi;
+	dp_ti->vactive_lo = ti->vactive_lo;
+	dp_ti->vblank_hi = ti->vblank_hi;
+	dp_ti->vblank_lo = ti->vblank_lo;
+	dp_ti->vsync_offset_hi = ti->vsync_offset_hi;
+	dp_ti->vsync_offset_lo = ti->vsync_offset_lo;
+	dp_ti->vsync_pulse_width_hi = ti->vsync_pulse_width_hi;
+	dp_ti->vsync_pulse_width_lo = ti->vsync_pulse_width_lo;
+
+	ret = 0;
+out:
+	kfree(gct);
+	return ret;
+}
+
 static void mid_get_vbt_data(struct drm_psb_private *dev_priv)
 {
 	struct drm_device *dev = dev_priv->dev;
-	struct oaktrail_vbt *vbt = &dev_priv->vbt_data;
 	u32 addr;
-	u16 new_size;
-	u8 *vbt_virtual;
-	u8 bpi;
-	u8 number_desc = 0;
-	struct oaktrail_timing_info *dp_ti = &dev_priv->gct_data.DTD;
-	struct gct_r10_timing_info ti;
-	void *pGCT;
+	u8 __iomem *vbt_virtual;
+	struct vbt_header vbt_header;
 	struct pci_dev *pci_gfx_root = pci_get_bus_and_slot(0, PCI_DEVFN(2, 0));
+	int ret = -1;
 
-	/* Get the address of the platform config vbt, B0:D2:F0;0xFC */
+	/* Get the address of the platform config vbt */
 	pci_read_config_dword(pci_gfx_root, 0xFC, &addr);
 	pci_dev_put(pci_gfx_root);
 
 	dev_dbg(dev->dev, "drm platform config address is %x\n", addr);
 
-	/* check for platform config address == 0. */
-	/* this means fw doesn't support vbt */
-
-	if (addr == 0) {
-		vbt->size = 0;
-		return;
-	}
+	if (!addr)
+		goto out;
 
 	/* get the virtual address of the vbt */
-	vbt_virtual = ioremap(addr, sizeof(*vbt));
-	if (vbt_virtual == NULL) {
-		vbt->size = 0;
-		return;
-	}
+	vbt_virtual = ioremap(addr, sizeof(vbt_header));
+	if (!vbt_virtual)
+		goto out;
 
-	memcpy(vbt, vbt_virtual, sizeof(*vbt));
-	iounmap(vbt_virtual); /* Free virtual address space */
+	memcpy_fromio(&vbt_header, vbt_virtual, sizeof(vbt_header));
+	iounmap(vbt_virtual);
 
-	/* No matching signature don't process the data */
-	if (memcmp(vbt->signature, "$GCT", 4)) {
-		vbt->size = 0;
-		return;
-	}
+	if (memcmp(&vbt_header.signature, "$GCT", 4))
+		goto out;
+
+	dev_dbg(dev->dev, "GCT revision is %02x\n", vbt_header.revision);
 
-	dev_dbg(dev->dev, "GCT revision is %x\n", vbt->revision);
-
-	switch (vbt->revision) {
-	case 0:
-		vbt->oaktrail_gct = ioremap(addr + sizeof(*vbt) - 4,
-					vbt->size - sizeof(*vbt) + 4);
-		pGCT = vbt->oaktrail_gct;
-		bpi = ((struct oaktrail_gct_v1 *)pGCT)->PD.BootPanelIndex;
-		dev_priv->gct_data.bpi = bpi;
-		dev_priv->gct_data.pt =
-			((struct oaktrail_gct_v1 *)pGCT)->PD.PanelType;
-		memcpy(&dev_priv->gct_data.DTD,
-			&((struct oaktrail_gct_v1 *)pGCT)->panel[bpi].DTD,
-				sizeof(struct oaktrail_timing_info));
-		dev_priv->gct_data.Panel_Port_Control =
-		  ((struct oaktrail_gct_v1 *)pGCT)->panel[bpi].Panel_Port_Control;
-		dev_priv->gct_data.Panel_MIPI_Display_Descriptor =
-			((struct oaktrail_gct_v1 *)pGCT)->panel[bpi].Panel_MIPI_Display_Descriptor;
+	switch (vbt_header.revision) {
+	case 0x00:
+		ret = mid_get_vbt_data_r0(dev_priv, addr);
 		break;
-	case 1:
-		vbt->oaktrail_gct = ioremap(addr + sizeof(*vbt) - 4,
-					vbt->size - sizeof(*vbt) + 4);
-		pGCT = vbt->oaktrail_gct;
-		bpi = ((struct oaktrail_gct_v2 *)pGCT)->PD.BootPanelIndex;
-		dev_priv->gct_data.bpi = bpi;
-		dev_priv->gct_data.pt =
-			((struct oaktrail_gct_v2 *)pGCT)->PD.PanelType;
-		memcpy(&dev_priv->gct_data.DTD,
-			&((struct oaktrail_gct_v2 *)pGCT)->panel[bpi].DTD,
-				sizeof(struct oaktrail_timing_info));
-		dev_priv->gct_data.Panel_Port_Control =
-		  ((struct oaktrail_gct_v2 *)pGCT)->panel[bpi].Panel_Port_Control;
-		dev_priv->gct_data.Panel_MIPI_Display_Descriptor =
-			((struct oaktrail_gct_v2 *)pGCT)->panel[bpi].Panel_MIPI_Display_Descriptor;
+	case 0x01:
+		ret = mid_get_vbt_data_r1(dev_priv, addr);
 		break;
 	case 0x10:
-		/*header definition changed from rev 01 (v2) to rev 10h. */
-		/*so, some values have changed location*/
-		new_size = vbt->checksum; /*checksum contains lo size byte*/
-		/*LSB of oaktrail_gct contains hi size byte*/
-		new_size |= ((0xff & (unsigned int)(long)vbt->oaktrail_gct)) << 8;
-
-		vbt->checksum = vbt->size; /*size contains the checksum*/
-		if (new_size > 0xff)
-			vbt->size = 0xff; /*restrict size to 255*/
-		else
-			vbt->size = new_size;
-
-		/* number of descriptors defined in the GCT */
-		number_desc = ((0xff00 & (unsigned int)(long)vbt->oaktrail_gct)) >> 8;
-		bpi = ((0xff0000 & (unsigned int)(long)vbt->oaktrail_gct)) >> 16;
-		vbt->oaktrail_gct = ioremap(addr + GCT_R10_HEADER_SIZE,
-				GCT_R10_DISPLAY_DESC_SIZE * number_desc);
-		pGCT = vbt->oaktrail_gct;
-		pGCT = (u8 *)pGCT + (bpi*GCT_R10_DISPLAY_DESC_SIZE);
-		dev_priv->gct_data.bpi = bpi; /*save boot panel id*/
-
-		/*copy the GCT display timings into a temp structure*/
-		memcpy(&ti, pGCT, sizeof(struct gct_r10_timing_info));
-
-		/*now copy the temp struct into the dev_priv->gct_data*/
-		dp_ti->pixel_clock = ti.pixel_clock;
-		dp_ti->hactive_hi = ti.hactive_hi;
-		dp_ti->hactive_lo = ti.hactive_lo;
-		dp_ti->hblank_hi = ti.hblank_hi;
-		dp_ti->hblank_lo = ti.hblank_lo;
-		dp_ti->hsync_offset_hi = ti.hsync_offset_hi;
-		dp_ti->hsync_offset_lo = ti.hsync_offset_lo;
-		dp_ti->hsync_pulse_width_hi = ti.hsync_pulse_width_hi;
-		dp_ti->hsync_pulse_width_lo = ti.hsync_pulse_width_lo;
-		dp_ti->vactive_hi = ti.vactive_hi;
-		dp_ti->vactive_lo = ti.vactive_lo;
-		dp_ti->vblank_hi = ti.vblank_hi;
-		dp_ti->vblank_lo = ti.vblank_lo;
-		dp_ti->vsync_offset_hi = ti.vsync_offset_hi;
-		dp_ti->vsync_offset_lo = ti.vsync_offset_lo;
-		dp_ti->vsync_pulse_width_hi = ti.vsync_pulse_width_hi;
-		dp_ti->vsync_pulse_width_lo = ti.vsync_pulse_width_lo;
-
-		/* Move the MIPI_Display_Descriptor data from GCT to dev priv */
-		dev_priv->gct_data.Panel_MIPI_Display_Descriptor =
-							*((u8 *)pGCT + 0x0d);
-		dev_priv->gct_data.Panel_MIPI_Display_Descriptor |=
-						(*((u8 *)pGCT + 0x0e)) << 8;
+		ret = mid_get_vbt_data_r10(dev_priv, addr);
 		break;
 	default:
 		dev_err(dev->dev, "Unknown revision of GCT!\n");
-		vbt->size = 0;
 	}
+
+out:
+	if (ret)
+		dev_err(dev->dev, "Unable to read GCT!");
+	else
+		dev_priv->has_gct = true;
 }
 
 int mid_chip_setup(struct drm_device *dev)
diff --git a/drivers/gpu/drm/gma500/oaktrail.h b/drivers/gpu/drm/gma500/oaktrail.h
index 2da1f368f14..f2f9f38a536 100644
--- a/drivers/gpu/drm/gma500/oaktrail.h
+++ b/drivers/gpu/drm/gma500/oaktrail.h
@@ -19,14 +19,6 @@
 
 /* MID device specific descriptors */
 
-struct oaktrail_vbt {
-	s8 signature[4];	/*4 bytes,"$GCT" */
-	u8 revision;
-	u8 size;
-	u8 checksum;
-	void *oaktrail_gct;
-} __packed;
-
 struct oaktrail_timing_info {
 	u16 pixel_clock;
 	u8 hactive_lo;
@@ -161,7 +153,7 @@ union oaktrail_panel_rx {
 	u16 panel_receiver;
 } __packed;
 
-struct oaktrail_gct_v1 {
+struct gct_r0 {
 	union { /*8 bits,Defined as follows: */
 		struct {
 			u8 PanelType:4; /*4 bits, Bit field for panels*/
@@ -178,7 +170,7 @@ struct oaktrail_gct_v1 {
 	union oaktrail_panel_rx panelrx[4]; /* panel receivers*/
 } __packed;
 
-struct oaktrail_gct_v2 {
+struct gct_r1 {
 	union { /*8 bits,Defined as follows: */
 		struct {
 			u8 PanelType:4; /*4 bits, Bit field for panels*/
@@ -195,6 +187,16 @@ struct oaktrail_gct_v2 {
 	union oaktrail_panel_rx panelrx[4]; /* panel receivers*/
 } __packed;
 
+struct gct_r10 {
+	struct gct_r10_timing_info DTD;
+	u16 Panel_MIPI_Display_Descriptor;
+	u16 Panel_MIPI_Receiver_Descriptor;
+	u16 Panel_Backlight_Inverter_Descriptor;
+	u8 Panel_Initial_Brightness;
+	u32 MIPI_Ctlr_Init_ptr;
+	u32 MIPI_Panel_Init_ptr;
+} __packed;
+
 struct oaktrail_gct_data {
 	u8 bpi; /* boot panel index, number of panel used during boot */
 	u8 pt; /* panel type, 4 bit field, 0=lvds, 1=mipi */
@@ -213,9 +215,6 @@ struct oaktrail_gct_data {
 #define MODE_SETTING_IN_DSR		0x4
 #define MODE_SETTING_ENCODER_DONE	0x8
 
-#define GCT_R10_HEADER_SIZE		16
-#define GCT_R10_DISPLAY_DESC_SIZE	28
-
 /*
  *	Moorestown HDMI interfaces
  */
diff --git a/drivers/gpu/drm/gma500/oaktrail_crtc.c b/drivers/gpu/drm/gma500/oaktrail_crtc.c
index a39b0d0d680..f821c835ca9 100644
--- a/drivers/gpu/drm/gma500/oaktrail_crtc.c
+++ b/drivers/gpu/drm/gma500/oaktrail_crtc.c
@@ -162,12 +162,10 @@ mrstFindBestPLL(struct drm_crtc *crtc, int target, int refclk,
 static void oaktrail_crtc_dpms(struct drm_crtc *crtc, int mode)
 {
 	struct drm_device *dev = crtc->dev;
+	struct drm_psb_private *dev_priv = dev->dev_private;
 	struct psb_intel_crtc *psb_intel_crtc = to_psb_intel_crtc(crtc);
 	int pipe = psb_intel_crtc->pipe;
-	int dpll_reg = (pipe == 0) ? MRST_DPLL_A : DPLL_B;
-	int dspcntr_reg = (pipe == 0) ? DSPACNTR : DSPBCNTR;
-	int dspbase_reg = (pipe == 0) ? MRST_DSPABASE : DSPBBASE;
-	int pipeconf_reg = (pipe == 0) ? PIPEACONF : PIPEBCONF;
+	const struct psb_offset *map = &dev_priv->regmap[pipe];
 	u32 temp;
 
 	if (!gma_power_begin(dev, true))
@@ -181,32 +179,32 @@ static void oaktrail_crtc_dpms(struct drm_crtc *crtc, int mode)
 	case DRM_MODE_DPMS_STANDBY:
 	case DRM_MODE_DPMS_SUSPEND:
 		/* Enable the DPLL */
-		temp = REG_READ(dpll_reg);
+		temp = REG_READ(map->dpll);
 		if ((temp & DPLL_VCO_ENABLE) == 0) {
-			REG_WRITE(dpll_reg, temp);
-			REG_READ(dpll_reg);
+			REG_WRITE(map->dpll, temp);
+			REG_READ(map->dpll);
 			/* Wait for the clocks to stabilize. */
 			udelay(150);
-			REG_WRITE(dpll_reg, temp | DPLL_VCO_ENABLE);
-			REG_READ(dpll_reg);
+			REG_WRITE(map->dpll, temp | DPLL_VCO_ENABLE);
+			REG_READ(map->dpll);
 			/* Wait for the clocks to stabilize. */
 			udelay(150);
-			REG_WRITE(dpll_reg, temp | DPLL_VCO_ENABLE);
-			REG_READ(dpll_reg);
+			REG_WRITE(map->dpll, temp | DPLL_VCO_ENABLE);
+			REG_READ(map->dpll);
 			/* Wait for the clocks to stabilize. */
 			udelay(150);
 		}
 		/* Enable the pipe */
-		temp = REG_READ(pipeconf_reg);
+		temp = REG_READ(map->conf);
 		if ((temp & PIPEACONF_ENABLE) == 0)
-			REG_WRITE(pipeconf_reg, temp | PIPEACONF_ENABLE);
+			REG_WRITE(map->conf, temp | PIPEACONF_ENABLE);
 		/* Enable the plane */
-		temp = REG_READ(dspcntr_reg);
+		temp = REG_READ(map->cntr);
 		if ((temp & DISPLAY_PLANE_ENABLE) == 0) {
-			REG_WRITE(dspcntr_reg,
+			REG_WRITE(map->cntr,
 				  temp | DISPLAY_PLANE_ENABLE);
 			/* Flush the plane changes */
-			REG_WRITE(dspbase_reg, REG_READ(dspbase_reg));
+			REG_WRITE(map->base, REG_READ(map->base));
 		}
 
 		psb_intel_crtc_load_lut(crtc);
@@ -223,28 +221,28 @@ static void oaktrail_crtc_dpms(struct drm_crtc *crtc, int mode)
 		/* Disable the VGA plane that we never use */
 		REG_WRITE(VGACNTRL, VGA_DISP_DISABLE);
 		/* Disable display plane */
-		temp = REG_READ(dspcntr_reg);
+		temp = REG_READ(map->cntr);
 		if ((temp & DISPLAY_PLANE_ENABLE) != 0) {
-			REG_WRITE(dspcntr_reg,
+			REG_WRITE(map->cntr,
 				  temp & ~DISPLAY_PLANE_ENABLE);
 			/* Flush the plane changes */
-			REG_WRITE(dspbase_reg, REG_READ(dspbase_reg));
-			REG_READ(dspbase_reg);
+			REG_WRITE(map->base, REG_READ(map->base));
+			REG_READ(map->base);
 		}
 
 		/* Next, disable display pipes */
-		temp = REG_READ(pipeconf_reg);
+		temp = REG_READ(map->conf);
 		if ((temp & PIPEACONF_ENABLE) != 0) {
-			REG_WRITE(pipeconf_reg, temp & ~PIPEACONF_ENABLE);
-			REG_READ(pipeconf_reg);
+			REG_WRITE(map->conf, temp & ~PIPEACONF_ENABLE);
+			REG_READ(map->conf);
 		}
 		/* Wait for for the pipe disable to take effect. */
 		psb_intel_wait_for_vblank(dev);
 
-		temp = REG_READ(dpll_reg);
+		temp = REG_READ(map->dpll);
 		if ((temp & DPLL_VCO_ENABLE) != 0) {
-			REG_WRITE(dpll_reg, temp & ~DPLL_VCO_ENABLE);
-			REG_READ(dpll_reg);
+			REG_WRITE(map->dpll, temp & ~DPLL_VCO_ENABLE);
+			REG_READ(map->dpll);
 		}
 
 		/* Wait for the clocks to turn off. */
@@ -292,17 +290,7 @@ static int oaktrail_crtc_mode_set(struct drm_crtc *crtc,
 	struct psb_intel_crtc *psb_intel_crtc = to_psb_intel_crtc(crtc);
 	struct drm_psb_private *dev_priv = dev->dev_private;
 	int pipe = psb_intel_crtc->pipe;
-	int fp_reg = (pipe == 0) ? MRST_FPA0 : FPB0;
-	int dpll_reg = (pipe == 0) ? MRST_DPLL_A : DPLL_B;
-	int dspcntr_reg = (pipe == 0) ? DSPACNTR : DSPBCNTR;
-	int pipeconf_reg = (pipe == 0) ? PIPEACONF : PIPEBCONF;
-	int htot_reg = (pipe == 0) ? HTOTAL_A : HTOTAL_B;
-	int hblank_reg = (pipe == 0) ? HBLANK_A : HBLANK_B;
-	int hsync_reg = (pipe == 0) ? HSYNC_A : HSYNC_B;
-	int vtot_reg = (pipe == 0) ? VTOTAL_A : VTOTAL_B;
-	int vblank_reg = (pipe == 0) ? VBLANK_A : VBLANK_B;
-	int vsync_reg = (pipe == 0) ? VSYNC_A : VSYNC_B;
-	int pipesrc_reg = (pipe == 0) ? PIPEASRC : PIPEBSRC;
+	const struct psb_offset *map = &dev_priv->regmap[pipe];
 	int refclk = 0;
 	struct oaktrail_clock_t clock;
 	u32 dpll = 0, fp = 0, dspcntr, pipeconf;
@@ -350,7 +338,7 @@ static int oaktrail_crtc_mode_set(struct drm_crtc *crtc,
 	if (oaktrail_panel_fitter_pipe(dev) == pipe)
 		REG_WRITE(PFIT_CONTROL, 0);
 
-	REG_WRITE(pipesrc_reg,
+	REG_WRITE(map->src,
 		  ((mode->crtc_hdisplay - 1) << 16) |
 		  (mode->crtc_vdisplay - 1));
 
@@ -369,34 +357,34 @@ static int oaktrail_crtc_mode_set(struct drm_crtc *crtc,
 		offsetY = (adjusted_mode->crtc_vdisplay -
 			   mode->crtc_vdisplay) / 2;
 
-		REG_WRITE(htot_reg, (mode->crtc_hdisplay - 1) |
+		REG_WRITE(map->htotal, (mode->crtc_hdisplay - 1) |
 			((adjusted_mode->crtc_htotal - 1) << 16));
-		REG_WRITE(vtot_reg, (mode->crtc_vdisplay - 1) |
+		REG_WRITE(map->vtotal, (mode->crtc_vdisplay - 1) |
 			((adjusted_mode->crtc_vtotal - 1) << 16));
-		REG_WRITE(hblank_reg,
+		REG_WRITE(map->hblank,
 			(adjusted_mode->crtc_hblank_start - offsetX - 1) |
 			((adjusted_mode->crtc_hblank_end - offsetX - 1) << 16));
-		REG_WRITE(hsync_reg,
+		REG_WRITE(map->hsync,
 			(adjusted_mode->crtc_hsync_start - offsetX - 1) |
 			((adjusted_mode->crtc_hsync_end - offsetX - 1) << 16));
-		REG_WRITE(vblank_reg,
+		REG_WRITE(map->vblank,
 			(adjusted_mode->crtc_vblank_start - offsetY - 1) |
 			((adjusted_mode->crtc_vblank_end - offsetY - 1) << 16));
-		REG_WRITE(vsync_reg,
+		REG_WRITE(map->vsync,
 			(adjusted_mode->crtc_vsync_start - offsetY - 1) |
 			((adjusted_mode->crtc_vsync_end - offsetY - 1) << 16));
 	} else {
-		REG_WRITE(htot_reg, (adjusted_mode->crtc_hdisplay - 1) |
+		REG_WRITE(map->htotal, (adjusted_mode->crtc_hdisplay - 1) |
 			((adjusted_mode->crtc_htotal - 1) << 16));
-		REG_WRITE(vtot_reg, (adjusted_mode->crtc_vdisplay - 1) |
+		REG_WRITE(map->vtotal, (adjusted_mode->crtc_vdisplay - 1) |
 			((adjusted_mode->crtc_vtotal - 1) << 16));
-		REG_WRITE(hblank_reg, (adjusted_mode->crtc_hblank_start - 1) |
+		REG_WRITE(map->hblank, (adjusted_mode->crtc_hblank_start - 1) |
 			((adjusted_mode->crtc_hblank_end - 1) << 16));
-		REG_WRITE(hsync_reg, (adjusted_mode->crtc_hsync_start - 1) |
+		REG_WRITE(map->hsync, (adjusted_mode->crtc_hsync_start - 1) |
 			((adjusted_mode->crtc_hsync_end - 1) << 16));
-		REG_WRITE(vblank_reg, (adjusted_mode->crtc_vblank_start - 1) |
+		REG_WRITE(map->vblank, (adjusted_mode->crtc_vblank_start - 1) |
 			((adjusted_mode->crtc_vblank_end - 1) << 16));
-		REG_WRITE(vsync_reg, (adjusted_mode->crtc_vsync_start - 1) |
+		REG_WRITE(map->vsync, (adjusted_mode->crtc_vsync_start - 1) |
 			((adjusted_mode->crtc_vsync_end - 1) << 16));
 	}
 
@@ -408,10 +396,10 @@ static int oaktrail_crtc_mode_set(struct drm_crtc *crtc,
 	}
 
 	/* setup pipeconf */
-	pipeconf = REG_READ(pipeconf_reg);
+	pipeconf = REG_READ(map->conf);
 
 	/* Set up the display plane register */
-	dspcntr = REG_READ(dspcntr_reg);
+	dspcntr = REG_READ(map->cntr);
 	dspcntr |= DISPPLANE_GAMMA_ENABLE;
 
 	if (pipe == 0)
@@ -467,30 +455,30 @@ static int oaktrail_crtc_mode_set(struct drm_crtc *crtc,
 	mrstPrintPll("chosen", &clock);
 
 	if (dpll & DPLL_VCO_ENABLE) {
-		REG_WRITE(fp_reg, fp);
-		REG_WRITE(dpll_reg, dpll & ~DPLL_VCO_ENABLE);
-		REG_READ(dpll_reg);
+		REG_WRITE(map->fp0, fp);
+		REG_WRITE(map->dpll, dpll & ~DPLL_VCO_ENABLE);
+		REG_READ(map->dpll);
 		/* Check the DPLLA lock bit PIPEACONF[29] */
 		udelay(150);
 	}
 
-	REG_WRITE(fp_reg, fp);
-	REG_WRITE(dpll_reg, dpll);
-	REG_READ(dpll_reg);
+	REG_WRITE(map->fp0, fp);
+	REG_WRITE(map->dpll, dpll);
+	REG_READ(map->dpll);
 	/* Wait for the clocks to stabilize. */
 	udelay(150);
 
 	/* write it again -- the BIOS does, after all */
-	REG_WRITE(dpll_reg, dpll);
-	REG_READ(dpll_reg);
+	REG_WRITE(map->dpll, dpll);
+	REG_READ(map->dpll);
 	/* Wait for the clocks to stabilize. */
 	udelay(150);
 
-	REG_WRITE(pipeconf_reg, pipeconf);
-	REG_READ(pipeconf_reg);
+	REG_WRITE(map->conf, pipeconf);
+	REG_READ(map->conf);
 	psb_intel_wait_for_vblank(dev);
 
-	REG_WRITE(dspcntr_reg, dspcntr);
+	REG_WRITE(map->cntr, dspcntr);
 	psb_intel_wait_for_vblank(dev);
 
 oaktrail_crtc_mode_set_exit:
@@ -509,15 +497,13 @@ static int oaktrail_pipe_set_base(struct drm_crtc *crtc,
 			    int x, int y, struct drm_framebuffer *old_fb)
 {
 	struct drm_device *dev = crtc->dev;
+	struct drm_psb_private *dev_priv = dev->dev_private;
 	struct psb_intel_crtc *psb_intel_crtc = to_psb_intel_crtc(crtc);
 	struct psb_framebuffer *psbfb = to_psb_fb(crtc->fb);
 	int pipe = psb_intel_crtc->pipe;
+	const struct psb_offset *map = &dev_priv->regmap[pipe];
 	unsigned long start, offset;
 
-	int dspbase = (pipe == 0 ? DSPALINOFF : DSPBBASE);
-	int dspsurf = (pipe == 0 ? DSPASURF : DSPBSURF);
-	int dspstride = (pipe == 0) ? DSPASTRIDE : DSPBSTRIDE;
-	int dspcntr_reg = (pipe == 0) ? DSPACNTR : DSPBCNTR;
 	u32 dspcntr;
 	int ret = 0;
 
@@ -533,9 +519,9 @@ static int oaktrail_pipe_set_base(struct drm_crtc *crtc,
 	start = psbfb->gtt->offset;
 	offset = y * crtc->fb->pitches[0] + x * (crtc->fb->bits_per_pixel / 8);
 
-	REG_WRITE(dspstride, crtc->fb->pitches[0]);
+	REG_WRITE(map->stride, crtc->fb->pitches[0]);
 
-	dspcntr = REG_READ(dspcntr_reg);
+	dspcntr = REG_READ(map->cntr);
 	dspcntr &= ~DISPPLANE_PIXFORMAT_MASK;
 
 	switch (crtc->fb->bits_per_pixel) {
@@ -557,12 +543,12 @@ static int oaktrail_pipe_set_base(struct drm_crtc *crtc,
 		ret = -EINVAL;
 		goto pipe_set_base_exit;
 	}
-	REG_WRITE(dspcntr_reg, dspcntr);
+	REG_WRITE(map->cntr, dspcntr);
 
-	REG_WRITE(dspbase, offset);
-	REG_READ(dspbase);
-	REG_WRITE(dspsurf, start);
-	REG_READ(dspsurf);
+	REG_WRITE(map->base, offset);
+	REG_READ(map->base);
+	REG_WRITE(map->surf, start);
+	REG_READ(map->surf);
 
 pipe_set_base_exit:
 	gma_power_end(dev);
diff --git a/drivers/gpu/drm/gma500/oaktrail_device.c b/drivers/gpu/drm/gma500/oaktrail_device.c
index 41d1924ea31..0f9b7db80f6 100644
--- a/drivers/gpu/drm/gma500/oaktrail_device.c
+++ b/drivers/gpu/drm/gma500/oaktrail_device.c
@@ -187,6 +187,7 @@ static int oaktrail_save_display_registers(struct drm_device *dev)
 {
 	struct drm_psb_private *dev_priv = dev->dev_private;
 	struct psb_save_area *regs = &dev_priv->regs;
+	struct psb_pipe *p = &regs->pipe[0];
 	int i;
 	u32 pp_stat;
 
@@ -201,24 +202,24 @@ static int oaktrail_save_display_registers(struct drm_device *dev)
 	regs->psb.saveCHICKENBIT = PSB_RVDC32(DSPCHICKENBIT);
 
 	/* Pipe & plane A info */
-	regs->psb.savePIPEACONF = PSB_RVDC32(PIPEACONF);
-	regs->psb.savePIPEASRC = PSB_RVDC32(PIPEASRC);
-	regs->psb.saveFPA0 = PSB_RVDC32(MRST_FPA0);
-	regs->psb.saveFPA1 = PSB_RVDC32(MRST_FPA1);
-	regs->psb.saveDPLL_A = PSB_RVDC32(MRST_DPLL_A);
-	regs->psb.saveHTOTAL_A = PSB_RVDC32(HTOTAL_A);
-	regs->psb.saveHBLANK_A = PSB_RVDC32(HBLANK_A);
-	regs->psb.saveHSYNC_A = PSB_RVDC32(HSYNC_A);
-	regs->psb.saveVTOTAL_A = PSB_RVDC32(VTOTAL_A);
-	regs->psb.saveVBLANK_A = PSB_RVDC32(VBLANK_A);
-	regs->psb.saveVSYNC_A = PSB_RVDC32(VSYNC_A);
+	p->conf = PSB_RVDC32(PIPEACONF);
+	p->src = PSB_RVDC32(PIPEASRC);
+	p->fp0 = PSB_RVDC32(MRST_FPA0);
+	p->fp1 = PSB_RVDC32(MRST_FPA1);
+	p->dpll = PSB_RVDC32(MRST_DPLL_A);
+	p->htotal = PSB_RVDC32(HTOTAL_A);
+	p->hblank = PSB_RVDC32(HBLANK_A);
+	p->hsync = PSB_RVDC32(HSYNC_A);
+	p->vtotal = PSB_RVDC32(VTOTAL_A);
+	p->vblank = PSB_RVDC32(VBLANK_A);
+	p->vsync = PSB_RVDC32(VSYNC_A);
 	regs->psb.saveBCLRPAT_A = PSB_RVDC32(BCLRPAT_A);
-	regs->psb.saveDSPACNTR = PSB_RVDC32(DSPACNTR);
-	regs->psb.saveDSPASTRIDE = PSB_RVDC32(DSPASTRIDE);
-	regs->psb.saveDSPAADDR = PSB_RVDC32(DSPABASE);
-	regs->psb.saveDSPASURF = PSB_RVDC32(DSPASURF);
-	regs->psb.saveDSPALINOFF = PSB_RVDC32(DSPALINOFF);
-	regs->psb.saveDSPATILEOFF = PSB_RVDC32(DSPATILEOFF);
+	p->cntr = PSB_RVDC32(DSPACNTR);
+	p->stride = PSB_RVDC32(DSPASTRIDE);
+	p->addr = PSB_RVDC32(DSPABASE);
+	p->surf = PSB_RVDC32(DSPASURF);
+	p->linoff = PSB_RVDC32(DSPALINOFF);
+	p->tileoff = PSB_RVDC32(DSPATILEOFF);
 
 	/* Save cursor regs */
 	regs->psb.saveDSPACURSOR_CTRL = PSB_RVDC32(CURACNTR);
@@ -227,7 +228,7 @@ static int oaktrail_save_display_registers(struct drm_device *dev)
 
 	/* Save palette (gamma) */
 	for (i = 0; i < 256; i++)
-		regs->psb.save_palette_a[i] = PSB_RVDC32(PALETTE_A + (i << 2));
+		p->palette[i] = PSB_RVDC32(PALETTE_A + (i << 2));
 
 	if (dev_priv->hdmi_priv)
 		oaktrail_hdmi_save(dev);
@@ -300,6 +301,7 @@ static int oaktrail_restore_display_registers(struct drm_device *dev)
 {
 	struct drm_psb_private *dev_priv = dev->dev_private;
 	struct psb_save_area *regs = &dev_priv->regs;
+	struct psb_pipe *p = &regs->pipe[0];
 	u32 pp_stat;
 	int i;
 
@@ -317,21 +319,21 @@ static int oaktrail_restore_display_registers(struct drm_device *dev)
 	PSB_WVDC32(0x80000000, VGACNTRL);
 
 	/* set the plls */
-	PSB_WVDC32(regs->psb.saveFPA0, MRST_FPA0);
-	PSB_WVDC32(regs->psb.saveFPA1, MRST_FPA1);
+	PSB_WVDC32(p->fp0, MRST_FPA0);
+	PSB_WVDC32(p->fp1, MRST_FPA1);
 
 	/* Actually enable it */
-	PSB_WVDC32(regs->psb.saveDPLL_A, MRST_DPLL_A);
+	PSB_WVDC32(p->dpll, MRST_DPLL_A);
 	DRM_UDELAY(150);
 
 	/* Restore mode */
-	PSB_WVDC32(regs->psb.saveHTOTAL_A, HTOTAL_A);
-	PSB_WVDC32(regs->psb.saveHBLANK_A, HBLANK_A);
-	PSB_WVDC32(regs->psb.saveHSYNC_A, HSYNC_A);
-	PSB_WVDC32(regs->psb.saveVTOTAL_A, VTOTAL_A);
-	PSB_WVDC32(regs->psb.saveVBLANK_A, VBLANK_A);
-	PSB_WVDC32(regs->psb.saveVSYNC_A, VSYNC_A);
-	PSB_WVDC32(regs->psb.savePIPEASRC, PIPEASRC);
+	PSB_WVDC32(p->htotal, HTOTAL_A);
+	PSB_WVDC32(p->hblank, HBLANK_A);
+	PSB_WVDC32(p->hsync, HSYNC_A);
+	PSB_WVDC32(p->vtotal, VTOTAL_A);
+	PSB_WVDC32(p->vblank, VBLANK_A);
+	PSB_WVDC32(p->vsync, VSYNC_A);
+	PSB_WVDC32(p->src, PIPEASRC);
 	PSB_WVDC32(regs->psb.saveBCLRPAT_A, BCLRPAT_A);
 
 	/* Restore performance mode*/
@@ -339,16 +341,16 @@ static int oaktrail_restore_display_registers(struct drm_device *dev)
 
 	/* Enable the pipe*/
 	if (dev_priv->iLVDS_enable)
-		PSB_WVDC32(regs->psb.savePIPEACONF, PIPEACONF);
+		PSB_WVDC32(p->conf, PIPEACONF);
 
 	/* Set up the plane*/
-	PSB_WVDC32(regs->psb.saveDSPALINOFF, DSPALINOFF);
-	PSB_WVDC32(regs->psb.saveDSPASTRIDE, DSPASTRIDE);
-	PSB_WVDC32(regs->psb.saveDSPATILEOFF, DSPATILEOFF);
+	PSB_WVDC32(p->linoff, DSPALINOFF);
+	PSB_WVDC32(p->stride, DSPASTRIDE);
+	PSB_WVDC32(p->tileoff, DSPATILEOFF);
 
 	/* Enable the plane */
-	PSB_WVDC32(regs->psb.saveDSPACNTR, DSPACNTR);
-	PSB_WVDC32(regs->psb.saveDSPASURF, DSPASURF);
+	PSB_WVDC32(p->cntr, DSPACNTR);
+	PSB_WVDC32(p->surf, DSPASURF);
 
 	/* Enable Cursor A */
 	PSB_WVDC32(regs->psb.saveDSPACURSOR_CTRL, CURACNTR);
@@ -357,7 +359,7 @@ static int oaktrail_restore_display_registers(struct drm_device *dev)
 
 	/* Restore palette (gamma) */
 	for (i = 0; i < 256; i++)
-		PSB_WVDC32(regs->psb.save_palette_a[i], PALETTE_A + (i << 2));
+		PSB_WVDC32(p->palette[i], PALETTE_A + (i << 2));
 
 	if (dev_priv->hdmi_priv)
 		oaktrail_hdmi_restore(dev);
@@ -454,31 +456,84 @@ static int oaktrail_power_up(struct drm_device *dev)
 	return 0;
 }
 
+/* Oaktrail */
+static const struct psb_offset oaktrail_regmap[2] = {
+	{
+		.fp0 = MRST_FPA0,
+		.fp1 = MRST_FPA1,
+		.cntr = DSPACNTR,
+		.conf = PIPEACONF,
+		.src = PIPEASRC,
+		.dpll = MRST_DPLL_A,
+		.htotal = HTOTAL_A,
+		.hblank = HBLANK_A,
+		.hsync = HSYNC_A,
+		.vtotal = VTOTAL_A,
+		.vblank = VBLANK_A,
+		.vsync = VSYNC_A,
+		.stride = DSPASTRIDE,
+		.size = DSPASIZE,
+		.pos = DSPAPOS,
+		.surf = DSPASURF,
+		.addr = MRST_DSPABASE,
+		.status = PIPEASTAT,
+		.linoff = DSPALINOFF,
+		.tileoff = DSPATILEOFF,
+		.palette = PALETTE_A,
+	},
+	{
+		.fp0 = FPB0,
+		.fp1 = FPB1,
+		.cntr = DSPBCNTR,
+		.conf = PIPEBCONF,
+		.src = PIPEBSRC,
+		.dpll = DPLL_B,
+		.htotal = HTOTAL_B,
+		.hblank = HBLANK_B,
+		.hsync = HSYNC_B,
+		.vtotal = VTOTAL_B,
+		.vblank = VBLANK_B,
+		.vsync = VSYNC_B,
+		.stride = DSPBSTRIDE,
+		.size = DSPBSIZE,
+		.pos = DSPBPOS,
+		.surf = DSPBSURF,
+		.addr = DSPBBASE,
+		.status = PIPEBSTAT,
+		.linoff = DSPBLINOFF,
+		.tileoff = DSPBTILEOFF,
+		.palette = PALETTE_B,
+	},
+};
 
 static int oaktrail_chip_setup(struct drm_device *dev)
 {
 	struct drm_psb_private *dev_priv = dev->dev_private;
-	struct oaktrail_vbt *vbt = &dev_priv->vbt_data;
 	int ret;
 	
+	if (pci_enable_msi(dev->pdev))
+		dev_warn(dev->dev, "Enabling MSI failed!\n");
+
+	dev_priv->regmap = oaktrail_regmap;
+
 	ret = mid_chip_setup(dev);
 	if (ret < 0)
 		return ret;
-	if (vbt->size == 0) {
+	if (!dev_priv->has_gct) {
 		/* Now pull the BIOS data */
-		gma_intel_opregion_init(dev);
+		psb_intel_opregion_init(dev);
 		psb_intel_init_bios(dev);
 	}
+	oaktrail_hdmi_setup(dev);
 	return 0;
 }
 
 static void oaktrail_teardown(struct drm_device *dev)
 {
 	struct drm_psb_private *dev_priv = dev->dev_private;
-	struct oaktrail_vbt *vbt = &dev_priv->vbt_data;
 
 	oaktrail_hdmi_teardown(dev);
-	if (vbt->size == 0)
+	if (!dev_priv->has_gct)
 		psb_intel_destroy_bios(dev);
 }
 
@@ -487,6 +542,9 @@ const struct psb_ops oaktrail_chip_ops = {
 	.accel_2d = 1,
 	.pipes = 2,
 	.crtcs = 2,
+	.hdmi_mask = (1 << 0),
+	.lvds_mask = (1 << 0),
+	.cursor_needs_phys = 0,
 	.sgx_offset = MRST_SGX_OFFSET,
 
 	.chip_setup = oaktrail_chip_setup,
diff --git a/drivers/gpu/drm/gma500/oaktrail_hdmi.c b/drivers/gpu/drm/gma500/oaktrail_hdmi.c
index f8b367b45f6..c10899c953b 100644
--- a/drivers/gpu/drm/gma500/oaktrail_hdmi.c
+++ b/drivers/gpu/drm/gma500/oaktrail_hdmi.c
@@ -179,7 +179,6 @@ static void oaktrail_hdmi_dpms(struct drm_encoder *encoder, int mode)
 static int oaktrail_hdmi_mode_valid(struct drm_connector *connector,
 				struct drm_display_mode *mode)
 {
-	struct drm_psb_private *dev_priv = connector->dev->dev_private;
 	if (mode->clock > 165000)
 		return MODE_CLOCK_HIGH;
 	if (mode->clock < 20000)
@@ -188,11 +187,6 @@ static int oaktrail_hdmi_mode_valid(struct drm_connector *connector,
 	if (mode->flags & DRM_MODE_FLAG_DBLSCAN)
 		return MODE_NO_DBLESCAN;
 
-	/* We assume worst case scenario of 32 bpp here, since we don't know */
-	if ((ALIGN(mode->hdisplay * 4, 64) * mode->vdisplay) >
-	    dev_priv->vram_stolen_size)
-		return MODE_MEM;
-
 	return MODE_OK;
 }
 
@@ -440,6 +434,7 @@ void oaktrail_hdmi_save(struct drm_device *dev)
 	struct drm_psb_private *dev_priv = dev->dev_private;
 	struct oaktrail_hdmi_dev *hdmi_dev = dev_priv->hdmi_priv;
 	struct psb_state *regs = &dev_priv->regs.psb;
+	struct psb_pipe *pipeb = &dev_priv->regs.pipe[1];
 	int i;
 
 	/* dpll */
@@ -450,14 +445,14 @@ void oaktrail_hdmi_save(struct drm_device *dev)
 	hdmi_dev->saveDPLL_CLK_ENABLE = PSB_RVDC32(DPLL_CLK_ENABLE);
 
 	/* pipe B */
-	regs->savePIPEBCONF = PSB_RVDC32(PIPEBCONF);
-	regs->savePIPEBSRC  = PSB_RVDC32(PIPEBSRC);
-	regs->saveHTOTAL_B  = PSB_RVDC32(HTOTAL_B);
-	regs->saveHBLANK_B  = PSB_RVDC32(HBLANK_B);
-	regs->saveHSYNC_B   = PSB_RVDC32(HSYNC_B);
-	regs->saveVTOTAL_B  = PSB_RVDC32(VTOTAL_B);
-	regs->saveVBLANK_B  = PSB_RVDC32(VBLANK_B);
-	regs->saveVSYNC_B   = PSB_RVDC32(VSYNC_B);
+	pipeb->conf = PSB_RVDC32(PIPEBCONF);
+	pipeb->src = PSB_RVDC32(PIPEBSRC);
+	pipeb->htotal = PSB_RVDC32(HTOTAL_B);
+	pipeb->hblank = PSB_RVDC32(HBLANK_B);
+	pipeb->hsync = PSB_RVDC32(HSYNC_B);
+	pipeb->vtotal = PSB_RVDC32(VTOTAL_B);
+	pipeb->vblank = PSB_RVDC32(VBLANK_B);
+	pipeb->vsync = PSB_RVDC32(VSYNC_B);
 
 	hdmi_dev->savePCH_PIPEBCONF = PSB_RVDC32(PCH_PIPEBCONF);
 	hdmi_dev->savePCH_PIPEBSRC = PSB_RVDC32(PCH_PIPEBSRC);
@@ -469,12 +464,12 @@ void oaktrail_hdmi_save(struct drm_device *dev)
 	hdmi_dev->savePCH_VSYNC_B  = PSB_RVDC32(PCH_VSYNC_B);
 
 	/* plane */
-	regs->saveDSPBCNTR = PSB_RVDC32(DSPBCNTR);
-	regs->saveDSPBSTRIDE = PSB_RVDC32(DSPBSTRIDE);
-	regs->saveDSPBADDR = PSB_RVDC32(DSPBBASE);
-	regs->saveDSPBSURF = PSB_RVDC32(DSPBSURF);
-	regs->saveDSPBLINOFF = PSB_RVDC32(DSPBLINOFF);
-	regs->saveDSPBTILEOFF = PSB_RVDC32(DSPBTILEOFF);
+	pipeb->cntr = PSB_RVDC32(DSPBCNTR);
+	pipeb->stride = PSB_RVDC32(DSPBSTRIDE);
+	pipeb->addr = PSB_RVDC32(DSPBBASE);
+	pipeb->surf = PSB_RVDC32(DSPBSURF);
+	pipeb->linoff = PSB_RVDC32(DSPBLINOFF);
+	pipeb->tileoff = PSB_RVDC32(DSPBTILEOFF);
 
 	/* cursor B */
 	regs->saveDSPBCURSOR_CTRL = PSB_RVDC32(CURBCNTR);
@@ -483,7 +478,7 @@ void oaktrail_hdmi_save(struct drm_device *dev)
 
 	/* save palette */
 	for (i = 0; i < 256; i++)
-		regs->save_palette_b[i] = PSB_RVDC32(PALETTE_B + (i << 2));
+		pipeb->palette[i] = PSB_RVDC32(PALETTE_B + (i << 2));
 }
 
 /* restore HDMI register state */
@@ -492,6 +487,7 @@ void oaktrail_hdmi_restore(struct drm_device *dev)
 	struct drm_psb_private *dev_priv = dev->dev_private;
 	struct oaktrail_hdmi_dev *hdmi_dev = dev_priv->hdmi_priv;
 	struct psb_state *regs = &dev_priv->regs.psb;
+	struct psb_pipe *pipeb = &dev_priv->regs.pipe[1];
 	int i;
 
 	/* dpll */
@@ -503,13 +499,13 @@ void oaktrail_hdmi_restore(struct drm_device *dev)
 	DRM_UDELAY(150);
 
 	/* pipe */
-	PSB_WVDC32(regs->savePIPEBSRC, PIPEBSRC);
-	PSB_WVDC32(regs->saveHTOTAL_B, HTOTAL_B);
-	PSB_WVDC32(regs->saveHBLANK_B, HBLANK_B);
-	PSB_WVDC32(regs->saveHSYNC_B,  HSYNC_B);
-	PSB_WVDC32(regs->saveVTOTAL_B, VTOTAL_B);
-	PSB_WVDC32(regs->saveVBLANK_B, VBLANK_B);
-	PSB_WVDC32(regs->saveVSYNC_B,  VSYNC_B);
+	PSB_WVDC32(pipeb->src, PIPEBSRC);
+	PSB_WVDC32(pipeb->htotal, HTOTAL_B);
+	PSB_WVDC32(pipeb->hblank, HBLANK_B);
+	PSB_WVDC32(pipeb->hsync,  HSYNC_B);
+	PSB_WVDC32(pipeb->vtotal, VTOTAL_B);
+	PSB_WVDC32(pipeb->vblank, VBLANK_B);
+	PSB_WVDC32(pipeb->vsync,  VSYNC_B);
 
 	PSB_WVDC32(hdmi_dev->savePCH_PIPEBSRC, PCH_PIPEBSRC);
 	PSB_WVDC32(hdmi_dev->savePCH_HTOTAL_B, PCH_HTOTAL_B);
@@ -519,15 +515,15 @@ void oaktrail_hdmi_restore(struct drm_device *dev)
 	PSB_WVDC32(hdmi_dev->savePCH_VBLANK_B, PCH_VBLANK_B);
 	PSB_WVDC32(hdmi_dev->savePCH_VSYNC_B,  PCH_VSYNC_B);
 
-	PSB_WVDC32(regs->savePIPEBCONF, PIPEBCONF);
+	PSB_WVDC32(pipeb->conf, PIPEBCONF);
 	PSB_WVDC32(hdmi_dev->savePCH_PIPEBCONF, PCH_PIPEBCONF);
 
 	/* plane */
-	PSB_WVDC32(regs->saveDSPBLINOFF, DSPBLINOFF);
-	PSB_WVDC32(regs->saveDSPBSTRIDE, DSPBSTRIDE);
-	PSB_WVDC32(regs->saveDSPBTILEOFF, DSPBTILEOFF);
-	PSB_WVDC32(regs->saveDSPBCNTR, DSPBCNTR);
-	PSB_WVDC32(regs->saveDSPBSURF, DSPBSURF);
+	PSB_WVDC32(pipeb->linoff, DSPBLINOFF);
+	PSB_WVDC32(pipeb->stride, DSPBSTRIDE);
+	PSB_WVDC32(pipeb->tileoff, DSPBTILEOFF);
+	PSB_WVDC32(pipeb->cntr, DSPBCNTR);
+	PSB_WVDC32(pipeb->surf, DSPBSURF);
 
 	/* cursor B */
 	PSB_WVDC32(regs->saveDSPBCURSOR_CTRL, CURBCNTR);
@@ -536,5 +532,5 @@ void oaktrail_hdmi_restore(struct drm_device *dev)
 
 	/* restore palette */
 	for (i = 0; i < 256; i++)
-		PSB_WVDC32(regs->save_palette_b[i], PALETTE_B + (i << 2));
+		PSB_WVDC32(pipeb->palette[i], PALETTE_B + (i << 2));
 }
diff --git a/drivers/gpu/drm/gma500/oaktrail_hdmi_i2c.c b/drivers/gpu/drm/gma500/oaktrail_hdmi_i2c.c
index 5e84fbde749..88627e3ba1e 100644
--- a/drivers/gpu/drm/gma500/oaktrail_hdmi_i2c.c
+++ b/drivers/gpu/drm/gma500/oaktrail_hdmi_i2c.c
@@ -250,7 +250,7 @@ static irqreturn_t oaktrail_hdmi_i2c_handler(int this_irq, void *dev)
  */
 static void oaktrail_hdmi_i2c_gpio_fix(void)
 {
-	void *base;
+	void __iomem *base;
 	unsigned int gpio_base = 0xff12c000;
 	int gpio_len = 0x1000;
 	u32 temp;
diff --git a/drivers/gpu/drm/gma500/oaktrail_lvds.c b/drivers/gpu/drm/gma500/oaktrail_lvds.c
index 654f32b22b2..558c77fb55e 100644
--- a/drivers/gpu/drm/gma500/oaktrail_lvds.c
+++ b/drivers/gpu/drm/gma500/oaktrail_lvds.c
@@ -257,7 +257,7 @@ static void oaktrail_lvds_get_configuration_mode(struct drm_device *dev,
 	mode_dev->panel_fixed_mode = NULL;
 
 	/* Use the firmware provided data on Moorestown */
-	if (dev_priv->vbt_data.size != 0x00) { /*if non-zero, then use vbt*/
+	if (dev_priv->has_gct) {
 		mode = kzalloc(sizeof(*mode), GFP_KERNEL);
 		if (!mode)
 			return;
@@ -371,7 +371,7 @@ void oaktrail_lvds_init(struct drm_device *dev,
 					BRIGHTNESS_MAX_LEVEL);
 
 	mode_dev->panel_wants_dither = false;
-	if (dev_priv->vbt_data.size != 0x00)
+	if (dev_priv->has_gct)
 		mode_dev->panel_wants_dither = (dev_priv->gct_data.
 			Panel_Port_Control & MRST_PANEL_8TO6_DITHER_ENABLE);
         if (dev_priv->lvds_dither)
diff --git a/drivers/gpu/drm/gma500/opregion.c b/drivers/gpu/drm/gma500/opregion.c
new file mode 100644
index 00000000000..4f186eca3a3
--- /dev/null
+++ b/drivers/gpu/drm/gma500/opregion.c
@@ -0,0 +1,344 @@
+/*
+ * Copyright 2011 Intel Corporation
+ *
+ * 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.
+ *
+ */
+#include <linux/acpi.h>
+#include <linux/acpi_io.h>
+#include "psb_drv.h"
+#include "psb_intel_reg.h"
+
+#define PCI_ASLE 0xe4
+#define PCI_ASLS 0xfc
+
+#define OPREGION_HEADER_OFFSET 0
+#define OPREGION_ACPI_OFFSET   0x100
+#define   ACPI_CLID 0x01ac /* current lid state indicator */
+#define   ACPI_CDCK 0x01b0 /* current docking state indicator */
+#define OPREGION_SWSCI_OFFSET  0x200
+#define OPREGION_ASLE_OFFSET   0x300
+#define OPREGION_VBT_OFFSET    0x400
+
+#define OPREGION_SIGNATURE "IntelGraphicsMem"
+#define MBOX_ACPI      (1<<0)
+#define MBOX_SWSCI     (1<<1)
+#define MBOX_ASLE      (1<<2)
+
+struct opregion_header {
+	u8 signature[16];
+	u32 size;
+	u32 opregion_ver;
+	u8 bios_ver[32];
+	u8 vbios_ver[16];
+	u8 driver_ver[16];
+	u32 mboxes;
+	u8 reserved[164];
+} __packed;
+
+/* OpRegion mailbox #1: public ACPI methods */
+struct opregion_acpi {
+	u32 drdy;	/* driver readiness */
+	u32 csts;	/* notification status */
+	u32 cevt;	/* current event */
+	u8 rsvd1[20];
+	u32 didl[8];	/* supported display devices ID list */
+	u32 cpdl[8];	/* currently presented display list */
+	u32 cadl[8];	/* currently active display list */
+	u32 nadl[8];	/* next active devices list */
+	u32 aslp;	/* ASL sleep time-out */
+	u32 tidx;	/* toggle table index */
+	u32 chpd;	/* current hotplug enable indicator */
+	u32 clid;	/* current lid state*/
+	u32 cdck;	/* current docking state */
+	u32 sxsw;	/* Sx state resume */
+	u32 evts;	/* ASL supported events */
+	u32 cnot;	/* current OS notification */
+	u32 nrdy;	/* driver status */
+	u8 rsvd2[60];
+} __packed;
+
+/* OpRegion mailbox #2: SWSCI */
+struct opregion_swsci {
+	/*FIXME: add it later*/
+} __packed;
+
+/* OpRegion mailbox #3: ASLE */
+struct opregion_asle {
+	u32 ardy;	/* driver readiness */
+	u32 aslc;	/* ASLE interrupt command */
+	u32 tche;	/* technology enabled indicator */
+	u32 alsi;	/* current ALS illuminance reading */
+	u32 bclp;	/* backlight brightness to set */
+	u32 pfit;	/* panel fitting state */
+	u32 cblv;	/* current brightness level */
+	u16 bclm[20];	/* backlight level duty cycle mapping table */
+	u32 cpfm;	/* current panel fitting mode */
+	u32 epfm;	/* enabled panel fitting modes */
+	u8 plut[74];	/* panel LUT and identifier */
+	u32 pfmb;	/* PWM freq and min brightness */
+	u8 rsvd[102];
+} __packed;
+
+/* ASLE irq request bits */
+#define ASLE_SET_ALS_ILLUM     (1 << 0)
+#define ASLE_SET_BACKLIGHT     (1 << 1)
+#define ASLE_SET_PFIT          (1 << 2)
+#define ASLE_SET_PWM_FREQ      (1 << 3)
+#define ASLE_REQ_MSK           0xf
+
+/* response bits of ASLE irq request */
+#define ASLE_ALS_ILLUM_FAILED   (1<<10)
+#define ASLE_BACKLIGHT_FAILED   (1<<12)
+#define ASLE_PFIT_FAILED        (1<<14)
+#define ASLE_PWM_FREQ_FAILED    (1<<16)
+
+/* ASLE backlight brightness to set */
+#define ASLE_BCLP_VALID                (1<<31)
+#define ASLE_BCLP_MSK          (~(1<<31))
+
+/* ASLE panel fitting request */
+#define ASLE_PFIT_VALID         (1<<31)
+#define ASLE_PFIT_CENTER (1<<0)
+#define ASLE_PFIT_STRETCH_TEXT (1<<1)
+#define ASLE_PFIT_STRETCH_GFX (1<<2)
+
+/* response bits of ASLE irq request */
+#define ASLE_ALS_ILLUM_FAILED	(1<<10)
+#define ASLE_BACKLIGHT_FAILED	(1<<12)
+#define ASLE_PFIT_FAILED	(1<<14)
+#define ASLE_PWM_FREQ_FAILED	(1<<16)
+
+/* ASLE backlight brightness to set */
+#define ASLE_BCLP_VALID                (1<<31)
+#define ASLE_BCLP_MSK          (~(1<<31))
+
+/* ASLE panel fitting request */
+#define ASLE_PFIT_VALID         (1<<31)
+#define ASLE_PFIT_CENTER (1<<0)
+#define ASLE_PFIT_STRETCH_TEXT (1<<1)
+#define ASLE_PFIT_STRETCH_GFX (1<<2)
+
+/* PWM frequency and minimum brightness */
+#define ASLE_PFMB_BRIGHTNESS_MASK (0xff)
+#define ASLE_PFMB_BRIGHTNESS_VALID (1<<8)
+#define ASLE_PFMB_PWM_MASK (0x7ffffe00)
+#define ASLE_PFMB_PWM_VALID (1<<31)
+
+#define ASLE_CBLV_VALID         (1<<31)
+
+static u32 asle_set_backlight(struct drm_device *dev, u32 bclp)
+{
+	struct drm_psb_private *dev_priv = dev->dev_private;
+	struct opregion_asle *asle = dev_priv->opregion.asle;
+	struct backlight_device *bd = dev_priv->backlight_device;
+
+	DRM_DEBUG_DRIVER("asle set backlight %x\n", bclp);
+
+	if (!(bclp & ASLE_BCLP_VALID))
+		return ASLE_BACKLIGHT_FAILED;
+
+	if (bd == NULL)
+		return ASLE_BACKLIGHT_FAILED;
+
+	bclp &= ASLE_BCLP_MSK;
+	if (bclp > 255)
+		return ASLE_BACKLIGHT_FAILED;
+
+	if (config_enabled(CONFIG_BACKLIGHT_CLASS_DEVICE)) {
+		int max = bd->props.max_brightness;
+		bd->props.brightness = bclp * max / 255;
+		backlight_update_status(bd);
+	}
+
+	asle->cblv = (bclp * 0x64) / 0xff | ASLE_CBLV_VALID;
+
+	return 0;
+}
+
+void psb_intel_opregion_asle_intr(struct drm_device *dev)
+{
+	struct drm_psb_private *dev_priv = dev->dev_private;
+	struct opregion_asle *asle = dev_priv->opregion.asle;
+	u32 asle_stat = 0;
+	u32 asle_req;
+
+	if (!asle)
+		return;
+
+	asle_req = asle->aslc & ASLE_REQ_MSK;
+	if (!asle_req) {
+		DRM_DEBUG_DRIVER("non asle set request??\n");
+		return;
+	}
+
+	if (asle_req & ASLE_SET_BACKLIGHT)
+		asle_stat |= asle_set_backlight(dev, asle->bclp);
+
+	asle->aslc = asle_stat;
+}
+
+#define ASLE_ALS_EN    (1<<0)
+#define ASLE_BLC_EN    (1<<1)
+#define ASLE_PFIT_EN   (1<<2)
+#define ASLE_PFMB_EN   (1<<3)
+
+void psb_intel_opregion_enable_asle(struct drm_device *dev)
+{
+	struct drm_psb_private *dev_priv = dev->dev_private;
+	struct opregion_asle *asle = dev_priv->opregion.asle;
+
+	if (asle) {
+		/* Don't do this on Medfield or other non PC like devices, they
+		   use the bit for something different altogether */
+		psb_enable_pipestat(dev_priv, 0, PIPE_LEGACY_BLC_EVENT_ENABLE);
+		psb_enable_pipestat(dev_priv, 1, PIPE_LEGACY_BLC_EVENT_ENABLE);
+
+		asle->tche = ASLE_ALS_EN | ASLE_BLC_EN | ASLE_PFIT_EN
+								| ASLE_PFMB_EN;
+		asle->ardy = 1;
+	}
+}
+
+#define ACPI_EV_DISPLAY_SWITCH (1<<0)
+#define ACPI_EV_LID            (1<<1)
+#define ACPI_EV_DOCK           (1<<2)
+
+static struct psb_intel_opregion *system_opregion;
+
+static int psb_intel_opregion_video_event(struct notifier_block *nb,
+					  unsigned long val, void *data)
+{
+	/* The only video events relevant to opregion are 0x80. These indicate
+	   either a docking event, lid switch or display switch request. In
+	   Linux, these are handled by the dock, button and video drivers.
+	   We might want to fix the video driver to be opregion-aware in
+	   future, but right now we just indicate to the firmware that the
+	   request has been handled */
+
+	struct opregion_acpi *acpi;
+
+	if (!system_opregion)
+		return NOTIFY_DONE;
+
+	acpi = system_opregion->acpi;
+	acpi->csts = 0;
+
+	return NOTIFY_OK;
+}
+
+static struct notifier_block psb_intel_opregion_notifier = {
+	.notifier_call = psb_intel_opregion_video_event,
+};
+
+void psb_intel_opregion_init(struct drm_device *dev)
+{
+	struct drm_psb_private *dev_priv = dev->dev_private;
+	struct psb_intel_opregion *opregion = &dev_priv->opregion;
+
+	if (!opregion->header)
+		return;
+
+	if (opregion->acpi) {
+		/* Notify BIOS we are ready to handle ACPI video ext notifs.
+		 * Right now, all the events are handled by the ACPI video
+		 * module. We don't actually need to do anything with them. */
+		opregion->acpi->csts = 0;
+		opregion->acpi->drdy = 1;
+
+		system_opregion = opregion;
+		register_acpi_notifier(&psb_intel_opregion_notifier);
+	}
+
+	if (opregion->asle)
+		psb_intel_opregion_enable_asle(dev);
+}
+
+void psb_intel_opregion_fini(struct drm_device *dev)
+{
+	struct drm_psb_private *dev_priv = dev->dev_private;
+	struct psb_intel_opregion *opregion = &dev_priv->opregion;
+
+	if (!opregion->header)
+		return;
+
+	if (opregion->acpi) {
+		opregion->acpi->drdy = 0;
+
+		system_opregion = NULL;
+		unregister_acpi_notifier(&psb_intel_opregion_notifier);
+	}
+
+	/* just clear all opregion memory pointers now */
+	iounmap(opregion->header);
+	opregion->header = NULL;
+	opregion->acpi = NULL;
+	opregion->swsci = NULL;
+	opregion->asle = NULL;
+	opregion->vbt = NULL;
+}
+
+int psb_intel_opregion_setup(struct drm_device *dev)
+{
+	struct drm_psb_private *dev_priv = dev->dev_private;
+	struct psb_intel_opregion *opregion = &dev_priv->opregion;
+	u32 opregion_phy, mboxes;
+	void __iomem *base;
+	int err = 0;
+
+	pci_read_config_dword(dev->pdev, PCI_ASLS, &opregion_phy);
+	if (opregion_phy == 0) {
+		DRM_DEBUG_DRIVER("ACPI Opregion not supported\n");
+		return -ENOTSUPP;
+	}
+	DRM_DEBUG("OpRegion detected at 0x%8x\n", opregion_phy);
+	base = acpi_os_ioremap(opregion_phy, 8*1024);
+	if (!base)
+		return -ENOMEM;
+
+	if (memcmp(base, OPREGION_SIGNATURE, 16)) {
+		DRM_DEBUG_DRIVER("opregion signature mismatch\n");
+		err = -EINVAL;
+		goto err_out;
+	}
+
+	opregion->header = base;
+	opregion->vbt = base + OPREGION_VBT_OFFSET;
+
+	opregion->lid_state = base + ACPI_CLID;
+
+	mboxes = opregion->header->mboxes;
+	if (mboxes & MBOX_ACPI) {
+		DRM_DEBUG_DRIVER("Public ACPI methods supported\n");
+		opregion->acpi = base + OPREGION_ACPI_OFFSET;
+	}
+
+	if (mboxes & MBOX_ASLE) {
+		DRM_DEBUG_DRIVER("ASLE supported\n");
+		opregion->asle = base + OPREGION_ASLE_OFFSET;
+	}
+
+	return 0;
+
+err_out:
+	iounmap(base);
+	return err;
+}
+
diff --git a/drivers/gpu/drm/gma500/intel_opregion.c b/drivers/gpu/drm/gma500/opregion.h
index d946bc1b17b..72dc6b92126 100644
--- a/drivers/gpu/drm/gma500/intel_opregion.c
+++ b/drivers/gpu/drm/gma500/opregion.h
@@ -1,5 +1,5 @@
 /*
- * Copyright 2010 Intel Corporation
+ * Copyright 2012 Intel Corporation
  *
  * Permission is hereby granted, free of charge, to any person obtaining a
  * copy of this software and associated documentation files (the "Software"),
@@ -20,62 +20,30 @@
  * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER
  * DEALINGS IN THE SOFTWARE.
  *
- * FIXME: resolve with the i915 version
  */
 
-#include "psb_drv.h"
+#if defined(CONFIG_ACPI)
+extern void psb_intel_opregion_asle_intr(struct drm_device *dev);
+extern void psb_intel_opregion_init(struct drm_device *dev);
+extern void psb_intel_opregion_fini(struct drm_device *dev);
+extern int psb_intel_opregion_setup(struct drm_device *dev);
 
-struct opregion_header {
-	u8 signature[16];
-	u32 size;
-	u32 opregion_ver;
-	u8 bios_ver[32];
-	u8 vbios_ver[16];
-	u8 driver_ver[16];
-	u32 mboxes;
-	u8 reserved[164];
-} __packed;
+#else
 
-struct opregion_apci {
-	/*FIXME: add it later*/
-} __packed;
-
-struct opregion_swsci {
-	/*FIXME: add it later*/
-} __packed;
-
-struct opregion_acpi {
-	/*FIXME: add it later*/
-} __packed;
-
-int gma_intel_opregion_init(struct drm_device *dev)
+extern inline void psb_intel_opregion_asle_intr(struct drm_device *dev)
 {
-	struct drm_psb_private *dev_priv = dev->dev_private;
-	u32 opregion_phy;
-	void *base;
-	u32 *lid_state;
-
-	dev_priv->lid_state = NULL;
-
-	pci_read_config_dword(dev->pdev, 0xfc, &opregion_phy);
-	if (opregion_phy == 0)
-		return -ENOTSUPP;
-
-	base = ioremap(opregion_phy, 8*1024);
-	if (!base)
-		return -ENOMEM;
+}
 
-	lid_state = base + 0x01ac;
+extern inline void psb_intel_opregion_init(struct drm_device *dev)
+{
+}
 
-	dev_priv->lid_state = lid_state;
-	dev_priv->lid_last_state = readl(lid_state);
-	return 0;
+extern inline void psb_intel_opregion_fini(struct drm_device *dev)
+{
 }
 
-int gma_intel_opregion_exit(struct drm_device *dev)
+extern inline int psb_intel_opregion_setup(struct drm_device *dev)
 {
-	struct drm_psb_private *dev_priv = dev->dev_private;
-	if (dev_priv->lid_state)
-		iounmap(dev_priv->lid_state);
 	return 0;
 }
+#endif
diff --git a/drivers/gpu/drm/gma500/psb_device.c b/drivers/gpu/drm/gma500/psb_device.c
index 95d163e4f1f..eff039bf92d 100644
--- a/drivers/gpu/drm/gma500/psb_device.c
+++ b/drivers/gpu/drm/gma500/psb_device.c
@@ -197,7 +197,8 @@ static int psb_save_display_registers(struct drm_device *dev)
 	}
 
 	list_for_each_entry(connector, &dev->mode_config.connector_list, head)
-		connector->funcs->save(connector);
+		if (connector->funcs->save)
+			connector->funcs->save(connector);
 
 	mutex_unlock(&dev->mode_config.mutex);
 	return 0;
@@ -235,7 +236,8 @@ static int psb_restore_display_registers(struct drm_device *dev)
 			crtc->funcs->restore(crtc);
 
 	list_for_each_entry(connector, &dev->mode_config.connector_list, head)
-		connector->funcs->restore(connector);
+		if (connector->funcs->restore)
+			connector->funcs->restore(connector);
 
 	mutex_unlock(&dev->mode_config.mutex);
 	return 0;
@@ -289,17 +291,80 @@ static void psb_get_core_freq(struct drm_device *dev)
 	}
 }
 
+/* Poulsbo */
+static const struct psb_offset psb_regmap[2] = {
+	{
+		.fp0 = FPA0,
+		.fp1 = FPA1,
+		.cntr = DSPACNTR,
+		.conf = PIPEACONF,
+		.src = PIPEASRC,
+		.dpll = DPLL_A,
+		.htotal = HTOTAL_A,
+		.hblank = HBLANK_A,
+		.hsync = HSYNC_A,
+		.vtotal = VTOTAL_A,
+		.vblank = VBLANK_A,
+		.vsync = VSYNC_A,
+		.stride = DSPASTRIDE,
+		.size = DSPASIZE,
+		.pos = DSPAPOS,
+		.base = DSPABASE,
+		.surf = DSPASURF,
+		.addr = DSPABASE,
+		.status = PIPEASTAT,
+		.linoff = DSPALINOFF,
+		.tileoff = DSPATILEOFF,
+		.palette = PALETTE_A,
+	},
+	{
+		.fp0 = FPB0,
+		.fp1 = FPB1,
+		.cntr = DSPBCNTR,
+		.conf = PIPEBCONF,
+		.src = PIPEBSRC,
+		.dpll = DPLL_B,
+		.htotal = HTOTAL_B,
+		.hblank = HBLANK_B,
+		.hsync = HSYNC_B,
+		.vtotal = VTOTAL_B,
+		.vblank = VBLANK_B,
+		.vsync = VSYNC_B,
+		.stride = DSPBSTRIDE,
+		.size = DSPBSIZE,
+		.pos = DSPBPOS,
+		.base = DSPBBASE,
+		.surf = DSPBSURF,
+		.addr = DSPBBASE,
+		.status = PIPEBSTAT,
+		.linoff = DSPBLINOFF,
+		.tileoff = DSPBTILEOFF,
+		.palette = PALETTE_B,
+	}
+};
+
 static int psb_chip_setup(struct drm_device *dev)
 {
+	struct drm_psb_private *dev_priv = dev->dev_private;
+	dev_priv->regmap = psb_regmap;
 	psb_get_core_freq(dev);
 	gma_intel_setup_gmbus(dev);
-	gma_intel_opregion_init(dev);
+	psb_intel_opregion_init(dev);
 	psb_intel_init_bios(dev);
 	return 0;
 }
 
+/* Not exactly an erratum more an irritation */
+static void psb_chip_errata(struct drm_device *dev)
+{
+	struct drm_psb_private *dev_priv = dev->dev_private;
+	psb_lid_timer_init(dev_priv);
+}
+
 static void psb_chip_teardown(struct drm_device *dev)
 {
+	struct drm_psb_private *dev_priv = dev->dev_private;
+	psb_lid_timer_takedown(dev_priv);
 	gma_intel_teardown_gmbus(dev);
 }
 
@@ -308,9 +373,13 @@ const struct psb_ops psb_chip_ops = {
 	.accel_2d = 1,
 	.pipes = 2,
 	.crtcs = 2,
+	.hdmi_mask = (1 << 0),
+	.lvds_mask = (1 << 1),
+	.cursor_needs_phys = 1,
 	.sgx_offset = PSB_SGX_OFFSET,
 	.chip_setup = psb_chip_setup,
 	.chip_teardown = psb_chip_teardown,
+	.errata = psb_chip_errata,
 
 	.crtc_helper = &psb_intel_helper_funcs,
 	.crtc_funcs = &psb_intel_crtc_funcs,
diff --git a/drivers/gpu/drm/gma500/psb_drv.c b/drivers/gpu/drm/gma500/psb_drv.c
index c34adf9d910..caba6e08693 100644
--- a/drivers/gpu/drm/gma500/psb_drv.c
+++ b/drivers/gpu/drm/gma500/psb_drv.c
@@ -79,6 +79,14 @@ static DEFINE_PCI_DEVICE_TABLE(pciidlist) = {
 	{ 0x8086, 0x0be5, PCI_ANY_ID, PCI_ANY_ID, 0, 0, (long) &cdv_chip_ops},
 	{ 0x8086, 0x0be6, PCI_ANY_ID, PCI_ANY_ID, 0, 0, (long) &cdv_chip_ops},
 	{ 0x8086, 0x0be7, PCI_ANY_ID, PCI_ANY_ID, 0, 0, (long) &cdv_chip_ops},
+	{ 0x8086, 0x0be8, PCI_ANY_ID, PCI_ANY_ID, 0, 0, (long) &cdv_chip_ops},
+	{ 0x8086, 0x0be9, PCI_ANY_ID, PCI_ANY_ID, 0, 0, (long) &cdv_chip_ops},
+	{ 0x8086, 0x0bea, PCI_ANY_ID, PCI_ANY_ID, 0, 0, (long) &cdv_chip_ops},
+	{ 0x8086, 0x0beb, PCI_ANY_ID, PCI_ANY_ID, 0, 0, (long) &cdv_chip_ops},
+	{ 0x8086, 0x0bec, PCI_ANY_ID, PCI_ANY_ID, 0, 0, (long) &cdv_chip_ops},
+	{ 0x8086, 0x0bed, PCI_ANY_ID, PCI_ANY_ID, 0, 0, (long) &cdv_chip_ops},
+	{ 0x8086, 0x0bee, PCI_ANY_ID, PCI_ANY_ID, 0, 0, (long) &cdv_chip_ops},
+	{ 0x8086, 0x0bef, PCI_ANY_ID, PCI_ANY_ID, 0, 0, (long) &cdv_chip_ops},
 #endif
 	{ 0, }
 };
@@ -144,10 +152,6 @@ static void psb_lastclose(struct drm_device *dev)
 	return;
 }
 
-static void psb_do_takedown(struct drm_device *dev)
-{
-}
-
 static int psb_do_init(struct drm_device *dev)
 {
 	struct drm_psb_private *dev_priv = dev->dev_private;
@@ -172,24 +176,6 @@ static int psb_do_init(struct drm_device *dev)
 	dev_priv->gatt_free_offset = pg->mmu_gatt_start +
 	    (stolen_gtt << PAGE_SHIFT) * 1024;
 
-	if (1 || drm_debug) {
-		uint32_t core_id = PSB_RSGX32(PSB_CR_CORE_ID);
-		uint32_t core_rev = PSB_RSGX32(PSB_CR_CORE_REVISION);
-		DRM_INFO("SGX core id = 0x%08x\n", core_id);
-		DRM_INFO("SGX core rev major = 0x%02x, minor = 0x%02x\n",
-			 (core_rev & _PSB_CC_REVISION_MAJOR_MASK) >>
-			 _PSB_CC_REVISION_MAJOR_SHIFT,
-			 (core_rev & _PSB_CC_REVISION_MINOR_MASK) >>
-			 _PSB_CC_REVISION_MINOR_SHIFT);
-		DRM_INFO
-		    ("SGX core rev maintenance = 0x%02x, designer = 0x%02x\n",
-		     (core_rev & _PSB_CC_REVISION_MAINTENANCE_MASK) >>
-		     _PSB_CC_REVISION_MAINTENANCE_SHIFT,
-		     (core_rev & _PSB_CC_REVISION_DESIGNER_MASK) >>
-		     _PSB_CC_REVISION_DESIGNER_SHIFT);
-	}
-
-
 	spin_lock_init(&dev_priv->irqmask_lock);
 	spin_lock_init(&dev_priv->lock_2d);
 
@@ -204,7 +190,6 @@ static int psb_do_init(struct drm_device *dev)
 	PSB_WSGX32(pg->gatt_start, PSB_CR_BIF_TWOD_REQ_BASE);
 	return 0;
 out_err:
-	psb_do_takedown(dev);
 	return ret;
 }
 
@@ -214,18 +199,16 @@ static int psb_driver_unload(struct drm_device *dev)
 
 	/* Kill vblank etc here */
 
-	gma_backlight_exit(dev);
-
-	psb_modeset_cleanup(dev);
 
 	if (dev_priv) {
-		psb_lid_timer_takedown(dev_priv);
-		gma_intel_opregion_exit(dev);
+		if (dev_priv->backlight_device)
+			gma_backlight_exit(dev);
+		psb_modeset_cleanup(dev);
 
 		if (dev_priv->ops->chip_teardown)
 			dev_priv->ops->chip_teardown(dev);
-		psb_do_takedown(dev);
 
+		psb_intel_opregion_fini(dev);
 
 		if (dev_priv->pf_pd) {
 			psb_mmu_free_pagedir(dev_priv->pf_pd);
@@ -246,6 +229,7 @@ static int psb_driver_unload(struct drm_device *dev)
 		}
 		psb_gtt_takedown(dev);
 		if (dev_priv->scratch_page) {
+			set_pages_wb(dev_priv->scratch_page, 1);
 			__free_page(dev_priv->scratch_page);
 			dev_priv->scratch_page = NULL;
 		}
@@ -258,15 +242,13 @@ static int psb_driver_unload(struct drm_device *dev)
 			dev_priv->sgx_reg = NULL;
 		}
 
+		/* Destroy VBT data */
+		psb_intel_destroy_bios(dev);
+
 		kfree(dev_priv);
 		dev->dev_private = NULL;
-
-		/*destroy VBT data*/
-		psb_intel_destroy_bios(dev);
 	}
-
 	gma_power_uninit(dev);
-
 	return 0;
 }
 
@@ -290,11 +272,6 @@ static int psb_driver_load(struct drm_device *dev, unsigned long chipset)
 
 	pci_set_master(dev->pdev);
 
-	if (!IS_PSB(dev)) {
-		if (pci_enable_msi(dev->pdev))
-			dev_warn(dev->dev, "Enabling MSI failed!\n");
-	}
-
 	dev_priv->num_pipe = dev_priv->ops->pipes;
 
 	resource_start = pci_resource_start(dev->pdev, PSB_MMIO_RESOURCE);
@@ -309,6 +286,8 @@ static int psb_driver_load(struct drm_device *dev, unsigned long chipset)
 	if (!dev_priv->sgx_reg)
 		goto out_err;
 
+	psb_intel_opregion_setup(dev);
+
 	ret = dev_priv->ops->chip_setup(dev);
 	if (ret)
 		goto out_err;
@@ -348,10 +327,7 @@ static int psb_driver_load(struct drm_device *dev, unsigned long chipset)
 	PSB_WSGX32(0x20000000, PSB_CR_PDS_EXEC_BASE);
 	PSB_WSGX32(0x30000000, PSB_CR_BIF_3D_REQ_BASE);
 
-/*	igd_opregion_init(&dev_priv->opregion_dev); */
 	acpi_video_register();
-	if (dev_priv->lid_state)
-		psb_lid_timer_init(dev_priv);
 
 	ret = drm_vblank_init(dev, dev_priv->num_pipe);
 	if (ret)
@@ -370,8 +346,8 @@ static int psb_driver_load(struct drm_device *dev, unsigned long chipset)
 	PSB_WVDC32(0x00000000, PSB_INT_ENABLE_R);
 	PSB_WVDC32(0xFFFFFFFF, PSB_INT_MASK_R);
 	spin_unlock_irqrestore(&dev_priv->irqmask_lock, irqflags);
-	if (IS_PSB(dev) && drm_core_check_feature(dev, DRIVER_MODESET))
-		drm_irq_install(dev);
+
+	drm_irq_install(dev);
 
 	dev->vblank_disable_allowed = 1;
 
@@ -619,7 +595,7 @@ static const struct dev_pm_ops psb_pm_ops = {
 	.runtime_idle = psb_runtime_idle,
 };
 
-static struct vm_operations_struct psb_gem_vm_ops = {
+static const struct vm_operations_struct psb_gem_vm_ops = {
 	.fault = psb_gem_fault,
 	.open = drm_gem_vm_open,
 	.close = drm_gem_vm_close,
diff --git a/drivers/gpu/drm/gma500/psb_drv.h b/drivers/gpu/drm/gma500/psb_drv.h
index 40ce2c9bc2e..1bd115ecefe 100644
--- a/drivers/gpu/drm/gma500/psb_drv.h
+++ b/drivers/gpu/drm/gma500/psb_drv.h
@@ -30,6 +30,7 @@
 #include "psb_intel_drv.h"
 #include "gtt.h"
 #include "power.h"
+#include "opregion.h"
 #include "oaktrail.h"
 
 /* Append new drm mode definition here, align with libdrm definition */
@@ -120,6 +121,7 @@ enum {
 #define PSB_HWSTAM		  0x2098
 #define PSB_INSTPM		  0x20C0
 #define PSB_INT_IDENTITY_R        0x20A4
+#define _PSB_IRQ_ASLE		  (1<<0)
 #define _MDFLD_PIPEC_EVENT_FLAG   (1<<2)
 #define _MDFLD_PIPEC_VBLANK_FLAG  (1<<3)
 #define _PSB_DPST_PIPEB_FLAG      (1<<4)
@@ -130,6 +132,7 @@ enum {
 #define _PSB_VSYNC_PIPEA_FLAG	  (1<<7)
 #define _MDFLD_MIPIA_FLAG	  (1<<16)
 #define _MDFLD_MIPIC_FLAG	  (1<<17)
+#define _PSB_IRQ_DISP_HOTSYNC	  (1<<17)
 #define _PSB_IRQ_SGX_FLAG	  (1<<18)
 #define _PSB_IRQ_MSVDX_FLAG	  (1<<19)
 #define _LNC_IRQ_TOPAZ_FLAG	  (1<<20)
@@ -257,7 +260,8 @@ struct psb_intel_opregion {
 	struct opregion_acpi *acpi;
 	struct opregion_swsci *swsci;
 	struct opregion_asle *asle;
-	int enabled;
+	void *vbt;
+	u32 __iomem *lid_state;
 };
 
 struct sdvo_device_mapping {
@@ -277,50 +281,72 @@ struct intel_gmbus {
 };
 
 /*
+ *	Register offset maps
+ */
+
+struct psb_offset {
+	u32	fp0;
+	u32	fp1;
+	u32	cntr;
+	u32	conf;
+	u32	src;
+	u32	dpll;
+	u32	dpll_md;
+	u32	htotal;
+	u32	hblank;
+	u32	hsync;
+	u32	vtotal;
+	u32	vblank;
+	u32	vsync;
+	u32	stride;
+	u32	size;
+	u32	pos;
+	u32	surf;
+	u32	addr;
+	u32	base;
+	u32	status;
+	u32	linoff;
+	u32	tileoff;
+	u32	palette;
+};
+
+/*
  *	Register save state. This is used to hold the context when the
  *	device is powered off. In the case of Oaktrail this can (but does not
  *	yet) include screen blank. Operations occuring during the save
  *	update the register cache instead.
  */
+
+/*
+ *	Common status for pipes.
+ */
+struct psb_pipe {
+	u32	fp0;
+	u32	fp1;
+	u32	cntr;
+	u32	conf;
+	u32	src;
+	u32	dpll;
+	u32	dpll_md;
+	u32	htotal;
+	u32	hblank;
+	u32	hsync;
+	u32	vtotal;
+	u32	vblank;
+	u32	vsync;
+	u32	stride;
+	u32	size;
+	u32	pos;
+	u32	base;
+	u32	surf;
+	u32	addr;
+	u32	status;
+	u32	linoff;
+	u32	tileoff;
+	u32	palette[256];
+};
+
 struct psb_state {
-	uint32_t saveDSPACNTR;
-	uint32_t saveDSPBCNTR;
-	uint32_t savePIPEACONF;
-	uint32_t savePIPEBCONF;
-	uint32_t savePIPEASRC;
-	uint32_t savePIPEBSRC;
-	uint32_t saveFPA0;
-	uint32_t saveFPA1;
-	uint32_t saveDPLL_A;
-	uint32_t saveDPLL_A_MD;
-	uint32_t saveHTOTAL_A;
-	uint32_t saveHBLANK_A;
-	uint32_t saveHSYNC_A;
-	uint32_t saveVTOTAL_A;
-	uint32_t saveVBLANK_A;
-	uint32_t saveVSYNC_A;
-	uint32_t saveDSPASTRIDE;
-	uint32_t saveDSPASIZE;
-	uint32_t saveDSPAPOS;
-	uint32_t saveDSPABASE;
-	uint32_t saveDSPASURF;
-	uint32_t saveDSPASTATUS;
-	uint32_t saveFPB0;
-	uint32_t saveFPB1;
-	uint32_t saveDPLL_B;
-	uint32_t saveDPLL_B_MD;
-	uint32_t saveHTOTAL_B;
-	uint32_t saveHBLANK_B;
-	uint32_t saveHSYNC_B;
-	uint32_t saveVTOTAL_B;
-	uint32_t saveVBLANK_B;
-	uint32_t saveVSYNC_B;
-	uint32_t saveDSPBSTRIDE;
-	uint32_t saveDSPBSIZE;
-	uint32_t saveDSPBPOS;
-	uint32_t saveDSPBBASE;
-	uint32_t saveDSPBSURF;
-	uint32_t saveDSPBSTATUS;
 	uint32_t saveVCLK_DIVISOR_VGA0;
 	uint32_t saveVCLK_DIVISOR_VGA1;
 	uint32_t saveVCLK_POST_DIV;
@@ -335,14 +361,8 @@ struct psb_state {
 	uint32_t savePP_CONTROL;
 	uint32_t savePP_CYCLE;
 	uint32_t savePFIT_CONTROL;
-	uint32_t savePaletteA[256];
-	uint32_t savePaletteB[256];
 	uint32_t saveCLOCKGATING;
 	uint32_t saveDSPARB;
-	uint32_t saveDSPATILEOFF;
-	uint32_t saveDSPBTILEOFF;
-	uint32_t saveDSPAADDR;
-	uint32_t saveDSPBADDR;
 	uint32_t savePFIT_AUTO_RATIOS;
 	uint32_t savePFIT_PGM_RATIOS;
 	uint32_t savePP_ON_DELAYS;
@@ -350,8 +370,6 @@ struct psb_state {
 	uint32_t savePP_DIVISOR;
 	uint32_t saveBCLRPAT_A;
 	uint32_t saveBCLRPAT_B;
-	uint32_t saveDSPALINOFF;
-	uint32_t saveDSPBLINOFF;
 	uint32_t savePERF_MODE;
 	uint32_t saveDSPFW1;
 	uint32_t saveDSPFW2;
@@ -366,8 +384,6 @@ struct psb_state {
 	uint32_t saveDSPBCURSOR_BASE;
 	uint32_t saveDSPACURSOR_POS;
 	uint32_t saveDSPBCURSOR_POS;
-	uint32_t save_palette_a[256];
-	uint32_t save_palette_b[256];
 	uint32_t saveOV_OVADD;
 	uint32_t saveOV_OGAMC0;
 	uint32_t saveOV_OGAMC1;
@@ -390,64 +406,7 @@ struct psb_state {
 };
 
 struct medfield_state {
-	uint32_t saveDPLL_A;
-	uint32_t saveFPA0;
-	uint32_t savePIPEACONF;
-	uint32_t saveHTOTAL_A;
-	uint32_t saveHBLANK_A;
-	uint32_t saveHSYNC_A;
-	uint32_t saveVTOTAL_A;
-	uint32_t saveVBLANK_A;
-	uint32_t saveVSYNC_A;
-	uint32_t savePIPEASRC;
-	uint32_t saveDSPASTRIDE;
-	uint32_t saveDSPALINOFF;
-	uint32_t saveDSPATILEOFF;
-	uint32_t saveDSPASIZE;
-	uint32_t saveDSPAPOS;
-	uint32_t saveDSPASURF;
-	uint32_t saveDSPACNTR;
-	uint32_t saveDSPASTATUS;
-	uint32_t save_palette_a[256];
 	uint32_t saveMIPI;
-
-	uint32_t saveDPLL_B;
-	uint32_t saveFPB0;
-	uint32_t savePIPEBCONF;
-	uint32_t saveHTOTAL_B;
-	uint32_t saveHBLANK_B;
-	uint32_t saveHSYNC_B;
-	uint32_t saveVTOTAL_B;
-	uint32_t saveVBLANK_B;
-	uint32_t saveVSYNC_B;
-	uint32_t savePIPEBSRC;
-	uint32_t saveDSPBSTRIDE;
-	uint32_t saveDSPBLINOFF;
-	uint32_t saveDSPBTILEOFF;
-	uint32_t saveDSPBSIZE;
-	uint32_t saveDSPBPOS;
-	uint32_t saveDSPBSURF;
-	uint32_t saveDSPBCNTR;
-	uint32_t saveDSPBSTATUS;
-	uint32_t save_palette_b[256];
-
-	uint32_t savePIPECCONF;
-	uint32_t saveHTOTAL_C;
-	uint32_t saveHBLANK_C;
-	uint32_t saveHSYNC_C;
-	uint32_t saveVTOTAL_C;
-	uint32_t saveVBLANK_C;
-	uint32_t saveVSYNC_C;
-	uint32_t savePIPECSRC;
-	uint32_t saveDSPCSTRIDE;
-	uint32_t saveDSPCLINOFF;
-	uint32_t saveDSPCTILEOFF;
-	uint32_t saveDSPCSIZE;
-	uint32_t saveDSPCPOS;
-	uint32_t saveDSPCSURF;
-	uint32_t saveDSPCCNTR;
-	uint32_t saveDSPCSTATUS;
-	uint32_t save_palette_c[256];
 	uint32_t saveMIPI_C;
 
 	uint32_t savePFIT_CONTROL;
@@ -476,6 +435,7 @@ struct cdv_state {
 };
 
 struct psb_save_area {
+	struct psb_pipe pipe[3];
 	uint32_t saveBSM;
 	uint32_t saveVBT;
 	union {
@@ -494,15 +454,19 @@ struct psb_ops;
 struct drm_psb_private {
 	struct drm_device *dev;
 	const struct psb_ops *ops;
+	const struct psb_offset *regmap;
+	
+	struct child_device_config *child_dev;
+	int child_dev_num;
 
 	struct psb_gtt gtt;
 
 	/* GTT Memory manager */
 	struct psb_gtt_mm *gtt_mm;
 	struct page *scratch_page;
-	u32 *gtt_map;
+	u32 __iomem *gtt_map;
 	uint32_t stolen_base;
-	void *vram_addr;
+	u8 __iomem *vram_addr;
 	unsigned long vram_stolen_size;
 	int gtt_initialized;
 	u16 gmch_ctrl;		/* Saved GTT setup */
@@ -518,8 +482,8 @@ struct drm_psb_private {
 	 * Register base
 	 */
 
-	uint8_t *sgx_reg;
-	uint8_t *vdc_reg;
+	uint8_t __iomem *sgx_reg;
+	uint8_t __iomem *vdc_reg;
 	uint32_t gatt_free_offset;
 
 	/*
@@ -543,6 +507,7 @@ struct drm_psb_private {
 	 * Modesetting
 	 */
 	struct psb_intel_mode_device mode_dev;
+	bool modeset;	/* true if we have done the mode_device setup */
 
 	struct drm_crtc *plane_to_crtc_mapping[PSB_NUM_PIPE];
 	struct drm_crtc *pipe_to_crtc_mapping[PSB_NUM_PIPE];
@@ -605,7 +570,7 @@ struct drm_psb_private {
 	int rpm_enabled;
 
 	/* MID specific */
-	struct oaktrail_vbt vbt_data;
+	bool has_gct;
 	struct oaktrail_gct_data gct_data;
 
 	/* Oaktrail HDMI state */
@@ -621,6 +586,11 @@ struct drm_psb_private {
 	uint32_t msi_addr;
 	uint32_t msi_data;
 
+	/*
+	 * Hotplug handling
+	 */
+
+	struct work_struct hotplug_work;
 
 	/*
 	 * LID-Switch
@@ -628,7 +598,6 @@ struct drm_psb_private {
 	spinlock_t lid_lock;
 	struct timer_list lid_timer;
 	struct psb_intel_opregion opregion;
-	u32 *lid_state;
 	u32 lid_last_state;
 
 	/*
@@ -669,6 +638,8 @@ struct drm_psb_private {
 	u32 dspcntr[3];
 
 	int mdfld_panel_id;
+
+	bool dplla_96mhz;	/* DPLL data from the VBT */
 };
 
 
@@ -682,6 +653,9 @@ struct psb_ops {
 	int pipes;		/* Number of output pipes */
 	int crtcs;		/* Number of CRTCs */
 	int sgx_offset;		/* Base offset of SGX device */
+	int hdmi_mask;		/* Mask of HDMI CRTCs */
+	int lvds_mask;		/* Mask of LVDS CRTCs */
+	int cursor_needs_phys;  /* If cursor base reg need physical address */
 
 	/* Sub functions */
 	struct drm_crtc_helper_funcs const *crtc_helper;
@@ -690,9 +664,13 @@ struct psb_ops {
 	/* Setup hooks */
 	int (*chip_setup)(struct drm_device *dev);
 	void (*chip_teardown)(struct drm_device *dev);
+	/* Optional helper caller after modeset */
+	void (*errata)(struct drm_device *dev);
 
 	/* Display management hooks */
 	int (*output_init)(struct drm_device *dev);
+	int (*hotplug)(struct drm_device *dev);
+	void (*hotplug_enable)(struct drm_device *dev, bool on);
 	/* Power management hooks */
 	void (*init_pm)(struct drm_device *dev);
 	int (*save_regs)(struct drm_device *dev);
@@ -789,12 +767,6 @@ psb_disable_pipestat(struct drm_psb_private *dev_priv, int pipe, u32 mask);
 extern u32 psb_get_vblank_counter(struct drm_device *dev, int crtc);
 
 /*
- * intel_opregion.c
- */
-extern int gma_intel_opregion_init(struct drm_device *dev);
-extern int gma_intel_opregion_exit(struct drm_device *dev);
-
-/*
  * framebuffer.c
  */
 extern int psbfb_probed(struct drm_device *dev);
diff --git a/drivers/gpu/drm/gma500/psb_intel_display.c b/drivers/gpu/drm/gma500/psb_intel_display.c
index 2616558457c..36c3c99612f 100644
--- a/drivers/gpu/drm/gma500/psb_intel_display.c
+++ b/drivers/gpu/drm/gma500/psb_intel_display.c
@@ -337,15 +337,12 @@ static int psb_intel_pipe_set_base(struct drm_crtc *crtc,
 			    int x, int y, struct drm_framebuffer *old_fb)
 {
 	struct drm_device *dev = crtc->dev;
-	/* struct drm_i915_master_private *master_priv; */
+	struct drm_psb_private *dev_priv = dev->dev_private;
 	struct psb_intel_crtc *psb_intel_crtc = to_psb_intel_crtc(crtc);
 	struct psb_framebuffer *psbfb = to_psb_fb(crtc->fb);
 	int pipe = psb_intel_crtc->pipe;
+	const struct psb_offset *map = &dev_priv->regmap[pipe];
 	unsigned long start, offset;
-	int dspbase = (pipe == 0 ? DSPABASE : DSPBBASE);
-	int dspsurf = (pipe == 0 ? DSPASURF : DSPBSURF);
-	int dspstride = (pipe == 0) ? DSPASTRIDE : DSPBSTRIDE;
-	int dspcntr_reg = (pipe == 0) ? DSPACNTR : DSPBCNTR;
 	u32 dspcntr;
 	int ret = 0;
 
@@ -367,9 +364,9 @@ static int psb_intel_pipe_set_base(struct drm_crtc *crtc,
 
 	offset = y * crtc->fb->pitches[0] + x * (crtc->fb->bits_per_pixel / 8);
 
-	REG_WRITE(dspstride, crtc->fb->pitches[0]);
+	REG_WRITE(map->stride, crtc->fb->pitches[0]);
 
-	dspcntr = REG_READ(dspcntr_reg);
+	dspcntr = REG_READ(map->cntr);
 	dspcntr &= ~DISPPLANE_PIXFORMAT_MASK;
 
 	switch (crtc->fb->bits_per_pixel) {
@@ -392,18 +389,10 @@ static int psb_intel_pipe_set_base(struct drm_crtc *crtc,
 		psb_gtt_unpin(psbfb->gtt);
 		goto psb_intel_pipe_set_base_exit;
 	}
-	REG_WRITE(dspcntr_reg, dspcntr);
-
+	REG_WRITE(map->cntr, dspcntr);
 
-	if (0 /* FIXMEAC - check what PSB needs */) {
-		REG_WRITE(dspbase, offset);
-		REG_READ(dspbase);
-		REG_WRITE(dspsurf, start);
-		REG_READ(dspsurf);
-	} else {
-		REG_WRITE(dspbase, start + offset);
-		REG_READ(dspbase);
-	}
+	REG_WRITE(map->base, start + offset);
+	REG_READ(map->base);
 
 psb_intel_pipe_cleaner:
 	/* If there was a previous display we can now unpin it */
@@ -424,14 +413,10 @@ psb_intel_pipe_set_base_exit:
 static void psb_intel_crtc_dpms(struct drm_crtc *crtc, int mode)
 {
 	struct drm_device *dev = crtc->dev;
-	/* struct drm_i915_master_private *master_priv; */
-	/* struct drm_i915_private *dev_priv = dev->dev_private; */
+	struct drm_psb_private *dev_priv = dev->dev_private;
 	struct psb_intel_crtc *psb_intel_crtc = to_psb_intel_crtc(crtc);
 	int pipe = psb_intel_crtc->pipe;
-	int dpll_reg = (pipe == 0) ? DPLL_A : DPLL_B;
-	int dspcntr_reg = (pipe == 0) ? DSPACNTR : DSPBCNTR;
-	int dspbase_reg = (pipe == 0) ? DSPABASE : DSPBBASE;
-	int pipeconf_reg = (pipe == 0) ? PIPEACONF : PIPEBCONF;
+	const struct psb_offset *map = &dev_priv->regmap[pipe];
 	u32 temp;
 
 	/* XXX: When our outputs are all unaware of DPMS modes other than off
@@ -442,34 +427,34 @@ static void psb_intel_crtc_dpms(struct drm_crtc *crtc, int mode)
 	case DRM_MODE_DPMS_STANDBY:
 	case DRM_MODE_DPMS_SUSPEND:
 		/* Enable the DPLL */
-		temp = REG_READ(dpll_reg);
+		temp = REG_READ(map->dpll);
 		if ((temp & DPLL_VCO_ENABLE) == 0) {
-			REG_WRITE(dpll_reg, temp);
-			REG_READ(dpll_reg);
+			REG_WRITE(map->dpll, temp);
+			REG_READ(map->dpll);
 			/* Wait for the clocks to stabilize. */
 			udelay(150);
-			REG_WRITE(dpll_reg, temp | DPLL_VCO_ENABLE);
-			REG_READ(dpll_reg);
+			REG_WRITE(map->dpll, temp | DPLL_VCO_ENABLE);
+			REG_READ(map->dpll);
 			/* Wait for the clocks to stabilize. */
 			udelay(150);
-			REG_WRITE(dpll_reg, temp | DPLL_VCO_ENABLE);
-			REG_READ(dpll_reg);
+			REG_WRITE(map->dpll, temp | DPLL_VCO_ENABLE);
+			REG_READ(map->dpll);
 			/* Wait for the clocks to stabilize. */
 			udelay(150);
 		}
 
 		/* Enable the pipe */
-		temp = REG_READ(pipeconf_reg);
+		temp = REG_READ(map->conf);
 		if ((temp & PIPEACONF_ENABLE) == 0)
-			REG_WRITE(pipeconf_reg, temp | PIPEACONF_ENABLE);
+			REG_WRITE(map->conf, temp | PIPEACONF_ENABLE);
 
 		/* Enable the plane */
-		temp = REG_READ(dspcntr_reg);
+		temp = REG_READ(map->cntr);
 		if ((temp & DISPLAY_PLANE_ENABLE) == 0) {
-			REG_WRITE(dspcntr_reg,
+			REG_WRITE(map->cntr,
 				  temp | DISPLAY_PLANE_ENABLE);
 			/* Flush the plane changes */
-			REG_WRITE(dspbase_reg, REG_READ(dspbase_reg));
+			REG_WRITE(map->base, REG_READ(map->base));
 		}
 
 		psb_intel_crtc_load_lut(crtc);
@@ -487,29 +472,29 @@ static void psb_intel_crtc_dpms(struct drm_crtc *crtc, int mode)
 		REG_WRITE(VGACNTRL, VGA_DISP_DISABLE);
 
 		/* Disable display plane */
-		temp = REG_READ(dspcntr_reg);
+		temp = REG_READ(map->cntr);
 		if ((temp & DISPLAY_PLANE_ENABLE) != 0) {
-			REG_WRITE(dspcntr_reg,
+			REG_WRITE(map->cntr,
 				  temp & ~DISPLAY_PLANE_ENABLE);
 			/* Flush the plane changes */
-			REG_WRITE(dspbase_reg, REG_READ(dspbase_reg));
-			REG_READ(dspbase_reg);
+			REG_WRITE(map->base, REG_READ(map->base));
+			REG_READ(map->base);
 		}
 
 		/* Next, disable display pipes */
-		temp = REG_READ(pipeconf_reg);
+		temp = REG_READ(map->conf);
 		if ((temp & PIPEACONF_ENABLE) != 0) {
-			REG_WRITE(pipeconf_reg, temp & ~PIPEACONF_ENABLE);
-			REG_READ(pipeconf_reg);
+			REG_WRITE(map->conf, temp & ~PIPEACONF_ENABLE);
+			REG_READ(map->conf);
 		}
 
 		/* Wait for vblank for the disable to take effect. */
 		psb_intel_wait_for_vblank(dev);
 
-		temp = REG_READ(dpll_reg);
+		temp = REG_READ(map->dpll);
 		if ((temp & DPLL_VCO_ENABLE) != 0) {
-			REG_WRITE(dpll_reg, temp & ~DPLL_VCO_ENABLE);
-			REG_READ(dpll_reg);
+			REG_WRITE(map->dpll, temp & ~DPLL_VCO_ENABLE);
+			REG_READ(map->dpll);
 		}
 
 		/* Wait for the clocks to turn off. */
@@ -589,22 +574,11 @@ static int psb_intel_crtc_mode_set(struct drm_crtc *crtc,
 			       struct drm_framebuffer *old_fb)
 {
 	struct drm_device *dev = crtc->dev;
+	struct drm_psb_private *dev_priv = dev->dev_private;
 	struct psb_intel_crtc *psb_intel_crtc = to_psb_intel_crtc(crtc);
 	struct drm_crtc_helper_funcs *crtc_funcs = crtc->helper_private;
 	int pipe = psb_intel_crtc->pipe;
-	int fp_reg = (pipe == 0) ? FPA0 : FPB0;
-	int dpll_reg = (pipe == 0) ? DPLL_A : DPLL_B;
-	int dspcntr_reg = (pipe == 0) ? DSPACNTR : DSPBCNTR;
-	int pipeconf_reg = (pipe == 0) ? PIPEACONF : PIPEBCONF;
-	int htot_reg = (pipe == 0) ? HTOTAL_A : HTOTAL_B;
-	int hblank_reg = (pipe == 0) ? HBLANK_A : HBLANK_B;
-	int hsync_reg = (pipe == 0) ? HSYNC_A : HSYNC_B;
-	int vtot_reg = (pipe == 0) ? VTOTAL_A : VTOTAL_B;
-	int vblank_reg = (pipe == 0) ? VBLANK_A : VBLANK_B;
-	int vsync_reg = (pipe == 0) ? VSYNC_A : VSYNC_B;
-	int dspsize_reg = (pipe == 0) ? DSPASIZE : DSPBSIZE;
-	int dsppos_reg = (pipe == 0) ? DSPAPOS : DSPBPOS;
-	int pipesrc_reg = (pipe == 0) ? PIPEASRC : PIPEBSRC;
+	const struct psb_offset *map = &dev_priv->regmap[pipe];
 	int refclk;
 	struct psb_intel_clock_t clock;
 	u32 dpll = 0, fp = 0, dspcntr, pipeconf;
@@ -690,7 +664,7 @@ static int psb_intel_crtc_mode_set(struct drm_crtc *crtc,
 	dpll |= PLL_REF_INPUT_DREFCLK;
 
 	/* setup pipeconf */
-	pipeconf = REG_READ(pipeconf_reg);
+	pipeconf = REG_READ(map->conf);
 
 	/* Set up the display plane register */
 	dspcntr = DISPPLANE_GAMMA_ENABLE;
@@ -712,9 +686,9 @@ static int psb_intel_crtc_mode_set(struct drm_crtc *crtc,
 	drm_mode_debug_printmodeline(mode);
 
 	if (dpll & DPLL_VCO_ENABLE) {
-		REG_WRITE(fp_reg, fp);
-		REG_WRITE(dpll_reg, dpll & ~DPLL_VCO_ENABLE);
-		REG_READ(dpll_reg);
+		REG_WRITE(map->fp0, fp);
+		REG_WRITE(map->dpll, dpll & ~DPLL_VCO_ENABLE);
+		REG_READ(map->dpll);
 		udelay(150);
 	}
 
@@ -747,45 +721,45 @@ static int psb_intel_crtc_mode_set(struct drm_crtc *crtc,
 		REG_READ(LVDS);
 	}
 
-	REG_WRITE(fp_reg, fp);
-	REG_WRITE(dpll_reg, dpll);
-	REG_READ(dpll_reg);
+	REG_WRITE(map->fp0, fp);
+	REG_WRITE(map->dpll, dpll);
+	REG_READ(map->dpll);
 	/* Wait for the clocks to stabilize. */
 	udelay(150);
 
 	/* write it again -- the BIOS does, after all */
-	REG_WRITE(dpll_reg, dpll);
+	REG_WRITE(map->dpll, dpll);
 
-	REG_READ(dpll_reg);
+	REG_READ(map->dpll);
 	/* Wait for the clocks to stabilize. */
 	udelay(150);
 
-	REG_WRITE(htot_reg, (adjusted_mode->crtc_hdisplay - 1) |
+	REG_WRITE(map->htotal, (adjusted_mode->crtc_hdisplay - 1) |
 		  ((adjusted_mode->crtc_htotal - 1) << 16));
-	REG_WRITE(hblank_reg, (adjusted_mode->crtc_hblank_start - 1) |
+	REG_WRITE(map->hblank, (adjusted_mode->crtc_hblank_start - 1) |
 		  ((adjusted_mode->crtc_hblank_end - 1) << 16));
-	REG_WRITE(hsync_reg, (adjusted_mode->crtc_hsync_start - 1) |
+	REG_WRITE(map->hsync, (adjusted_mode->crtc_hsync_start - 1) |
 		  ((adjusted_mode->crtc_hsync_end - 1) << 16));
-	REG_WRITE(vtot_reg, (adjusted_mode->crtc_vdisplay - 1) |
+	REG_WRITE(map->vtotal, (adjusted_mode->crtc_vdisplay - 1) |
 		  ((adjusted_mode->crtc_vtotal - 1) << 16));
-	REG_WRITE(vblank_reg, (adjusted_mode->crtc_vblank_start - 1) |
+	REG_WRITE(map->vblank, (adjusted_mode->crtc_vblank_start - 1) |
 		  ((adjusted_mode->crtc_vblank_end - 1) << 16));
-	REG_WRITE(vsync_reg, (adjusted_mode->crtc_vsync_start - 1) |
+	REG_WRITE(map->vsync, (adjusted_mode->crtc_vsync_start - 1) |
 		  ((adjusted_mode->crtc_vsync_end - 1) << 16));
 	/* pipesrc and dspsize control the size that is scaled from,
 	 * which should always be the user's requested size.
 	 */
-	REG_WRITE(dspsize_reg,
+	REG_WRITE(map->size,
 		  ((mode->vdisplay - 1) << 16) | (mode->hdisplay - 1));
-	REG_WRITE(dsppos_reg, 0);
-	REG_WRITE(pipesrc_reg,
+	REG_WRITE(map->pos, 0);
+	REG_WRITE(map->src,
 		  ((mode->hdisplay - 1) << 16) | (mode->vdisplay - 1));
-	REG_WRITE(pipeconf_reg, pipeconf);
-	REG_READ(pipeconf_reg);
+	REG_WRITE(map->conf, pipeconf);
+	REG_READ(map->conf);
 
 	psb_intel_wait_for_vblank(dev);
 
-	REG_WRITE(dspcntr_reg, dspcntr);
+	REG_WRITE(map->cntr, dspcntr);
 
 	/* Flush the plane changes */
 	crtc_funcs->mode_set_base(crtc, x, y, old_fb);
@@ -799,10 +773,10 @@ static int psb_intel_crtc_mode_set(struct drm_crtc *crtc,
 void psb_intel_crtc_load_lut(struct drm_crtc *crtc)
 {
 	struct drm_device *dev = crtc->dev;
-	struct drm_psb_private *dev_priv =
-				(struct drm_psb_private *)dev->dev_private;
+	struct drm_psb_private *dev_priv = dev->dev_private;
 	struct psb_intel_crtc *psb_intel_crtc = to_psb_intel_crtc(crtc);
-	int palreg = PALETTE_A;
+	const struct psb_offset *map = &dev_priv->regmap[psb_intel_crtc->pipe];
+	int palreg = map->palette;
 	int i;
 
 	/* The clocks have to be on to load the palette. */
@@ -811,12 +785,7 @@ void psb_intel_crtc_load_lut(struct drm_crtc *crtc)
 
 	switch (psb_intel_crtc->pipe) {
 	case 0:
-		break;
 	case 1:
-		palreg = PALETTE_B;
-		break;
-	case 2:
-		palreg = PALETTE_C;
 		break;
 	default:
 		dev_err(dev->dev, "Illegal Pipe Number.\n");
@@ -836,7 +805,7 @@ void psb_intel_crtc_load_lut(struct drm_crtc *crtc)
 		gma_power_end(dev);
 	} else {
 		for (i = 0; i < 256; i++) {
-			dev_priv->regs.psb.save_palette_a[i] =
+			dev_priv->regs.pipe[0].palette[i] =
 				  ((psb_intel_crtc->lut_r[i] +
 				  psb_intel_crtc->lut_adj[i]) << 16) |
 				  ((psb_intel_crtc->lut_g[i] +
@@ -854,11 +823,10 @@ void psb_intel_crtc_load_lut(struct drm_crtc *crtc)
 static void psb_intel_crtc_save(struct drm_crtc *crtc)
 {
 	struct drm_device *dev = crtc->dev;
-	/* struct drm_psb_private *dev_priv =
-			(struct drm_psb_private *)dev->dev_private; */
+	struct drm_psb_private *dev_priv = dev->dev_private;
 	struct psb_intel_crtc *psb_intel_crtc = to_psb_intel_crtc(crtc);
 	struct psb_intel_crtc_state *crtc_state = psb_intel_crtc->crtc_state;
-	int pipeA = (psb_intel_crtc->pipe == 0);
+	const struct psb_offset *map = &dev_priv->regmap[psb_intel_crtc->pipe];
 	uint32_t paletteReg;
 	int i;
 
@@ -867,27 +835,27 @@ static void psb_intel_crtc_save(struct drm_crtc *crtc)
 		return;
 	}
 
-	crtc_state->saveDSPCNTR = REG_READ(pipeA ? DSPACNTR : DSPBCNTR);
-	crtc_state->savePIPECONF = REG_READ(pipeA ? PIPEACONF : PIPEBCONF);
-	crtc_state->savePIPESRC = REG_READ(pipeA ? PIPEASRC : PIPEBSRC);
-	crtc_state->saveFP0 = REG_READ(pipeA ? FPA0 : FPB0);
-	crtc_state->saveFP1 = REG_READ(pipeA ? FPA1 : FPB1);
-	crtc_state->saveDPLL = REG_READ(pipeA ? DPLL_A : DPLL_B);
-	crtc_state->saveHTOTAL = REG_READ(pipeA ? HTOTAL_A : HTOTAL_B);
-	crtc_state->saveHBLANK = REG_READ(pipeA ? HBLANK_A : HBLANK_B);
-	crtc_state->saveHSYNC = REG_READ(pipeA ? HSYNC_A : HSYNC_B);
-	crtc_state->saveVTOTAL = REG_READ(pipeA ? VTOTAL_A : VTOTAL_B);
-	crtc_state->saveVBLANK = REG_READ(pipeA ? VBLANK_A : VBLANK_B);
-	crtc_state->saveVSYNC = REG_READ(pipeA ? VSYNC_A : VSYNC_B);
-	crtc_state->saveDSPSTRIDE = REG_READ(pipeA ? DSPASTRIDE : DSPBSTRIDE);
+	crtc_state->saveDSPCNTR = REG_READ(map->cntr);
+	crtc_state->savePIPECONF = REG_READ(map->conf);
+	crtc_state->savePIPESRC = REG_READ(map->src);
+	crtc_state->saveFP0 = REG_READ(map->fp0);
+	crtc_state->saveFP1 = REG_READ(map->fp1);
+	crtc_state->saveDPLL = REG_READ(map->dpll);
+	crtc_state->saveHTOTAL = REG_READ(map->htotal);
+	crtc_state->saveHBLANK = REG_READ(map->hblank);
+	crtc_state->saveHSYNC = REG_READ(map->hsync);
+	crtc_state->saveVTOTAL = REG_READ(map->vtotal);
+	crtc_state->saveVBLANK = REG_READ(map->vblank);
+	crtc_state->saveVSYNC = REG_READ(map->vsync);
+	crtc_state->saveDSPSTRIDE = REG_READ(map->stride);
 
 	/*NOTE: DSPSIZE DSPPOS only for psb*/
-	crtc_state->saveDSPSIZE = REG_READ(pipeA ? DSPASIZE : DSPBSIZE);
-	crtc_state->saveDSPPOS = REG_READ(pipeA ? DSPAPOS : DSPBPOS);
+	crtc_state->saveDSPSIZE = REG_READ(map->size);
+	crtc_state->saveDSPPOS = REG_READ(map->pos);
 
-	crtc_state->saveDSPBASE = REG_READ(pipeA ? DSPABASE : DSPBBASE);
+	crtc_state->saveDSPBASE = REG_READ(map->base);
 
-	paletteReg = pipeA ? PALETTE_A : PALETTE_B;
+	paletteReg = map->palette;
 	for (i = 0; i < 256; ++i)
 		crtc_state->savePalette[i] = REG_READ(paletteReg + (i << 2));
 }
@@ -898,12 +866,10 @@ static void psb_intel_crtc_save(struct drm_crtc *crtc)
 static void psb_intel_crtc_restore(struct drm_crtc *crtc)
 {
 	struct drm_device *dev = crtc->dev;
-	/* struct drm_psb_private * dev_priv =
-				(struct drm_psb_private *)dev->dev_private; */
+	struct drm_psb_private *dev_priv = dev->dev_private;
 	struct psb_intel_crtc *psb_intel_crtc =  to_psb_intel_crtc(crtc);
 	struct psb_intel_crtc_state *crtc_state = psb_intel_crtc->crtc_state;
-	/* struct drm_crtc_helper_funcs * crtc_funcs = crtc->helper_private; */
-	int pipeA = (psb_intel_crtc->pipe == 0);
+	const struct psb_offset *map = &dev_priv->regmap[psb_intel_crtc->pipe];
 	uint32_t paletteReg;
 	int i;
 
@@ -913,45 +879,45 @@ static void psb_intel_crtc_restore(struct drm_crtc *crtc)
 	}
 
 	if (crtc_state->saveDPLL & DPLL_VCO_ENABLE) {
-		REG_WRITE(pipeA ? DPLL_A : DPLL_B,
+		REG_WRITE(map->dpll,
 			crtc_state->saveDPLL & ~DPLL_VCO_ENABLE);
-		REG_READ(pipeA ? DPLL_A : DPLL_B);
+		REG_READ(map->dpll);
 		udelay(150);
 	}
 
-	REG_WRITE(pipeA ? FPA0 : FPB0, crtc_state->saveFP0);
-	REG_READ(pipeA ? FPA0 : FPB0);
+	REG_WRITE(map->fp0, crtc_state->saveFP0);
+	REG_READ(map->fp0);
 
-	REG_WRITE(pipeA ? FPA1 : FPB1, crtc_state->saveFP1);
-	REG_READ(pipeA ? FPA1 : FPB1);
+	REG_WRITE(map->fp1, crtc_state->saveFP1);
+	REG_READ(map->fp1);
 
-	REG_WRITE(pipeA ? DPLL_A : DPLL_B, crtc_state->saveDPLL);
-	REG_READ(pipeA ? DPLL_A : DPLL_B);
+	REG_WRITE(map->dpll, crtc_state->saveDPLL);
+	REG_READ(map->dpll);
 	udelay(150);
 
-	REG_WRITE(pipeA ? HTOTAL_A : HTOTAL_B, crtc_state->saveHTOTAL);
-	REG_WRITE(pipeA ? HBLANK_A : HBLANK_B, crtc_state->saveHBLANK);
-	REG_WRITE(pipeA ? HSYNC_A : HSYNC_B, crtc_state->saveHSYNC);
-	REG_WRITE(pipeA ? VTOTAL_A : VTOTAL_B, crtc_state->saveVTOTAL);
-	REG_WRITE(pipeA ? VBLANK_A : VBLANK_B, crtc_state->saveVBLANK);
-	REG_WRITE(pipeA ? VSYNC_A : VSYNC_B, crtc_state->saveVSYNC);
-	REG_WRITE(pipeA ? DSPASTRIDE : DSPBSTRIDE, crtc_state->saveDSPSTRIDE);
+	REG_WRITE(map->htotal, crtc_state->saveHTOTAL);
+	REG_WRITE(map->hblank, crtc_state->saveHBLANK);
+	REG_WRITE(map->hsync, crtc_state->saveHSYNC);
+	REG_WRITE(map->vtotal, crtc_state->saveVTOTAL);
+	REG_WRITE(map->vblank, crtc_state->saveVBLANK);
+	REG_WRITE(map->vsync, crtc_state->saveVSYNC);
+	REG_WRITE(map->stride, crtc_state->saveDSPSTRIDE);
 
-	REG_WRITE(pipeA ? DSPASIZE : DSPBSIZE, crtc_state->saveDSPSIZE);
-	REG_WRITE(pipeA ? DSPAPOS : DSPBPOS, crtc_state->saveDSPPOS);
+	REG_WRITE(map->size, crtc_state->saveDSPSIZE);
+	REG_WRITE(map->pos, crtc_state->saveDSPPOS);
 
-	REG_WRITE(pipeA ? PIPEASRC : PIPEBSRC, crtc_state->savePIPESRC);
-	REG_WRITE(pipeA ? DSPABASE : DSPBBASE, crtc_state->saveDSPBASE);
-	REG_WRITE(pipeA ? PIPEACONF : PIPEBCONF, crtc_state->savePIPECONF);
+	REG_WRITE(map->src, crtc_state->savePIPESRC);
+	REG_WRITE(map->base, crtc_state->saveDSPBASE);
+	REG_WRITE(map->conf, crtc_state->savePIPECONF);
 
 	psb_intel_wait_for_vblank(dev);
 
-	REG_WRITE(pipeA ? DSPACNTR : DSPBCNTR, crtc_state->saveDSPCNTR);
-	REG_WRITE(pipeA ? DSPABASE : DSPBBASE, crtc_state->saveDSPBASE);
+	REG_WRITE(map->cntr, crtc_state->saveDSPCNTR);
+	REG_WRITE(map->base, crtc_state->saveDSPBASE);
 
 	psb_intel_wait_for_vblank(dev);
 
-	paletteReg = pipeA ? PALETTE_A : PALETTE_B;
+	paletteReg = map->palette;
 	for (i = 0; i < 256; ++i)
 		REG_WRITE(paletteReg + (i << 2), crtc_state->savePalette[i]);
 }
@@ -962,6 +928,7 @@ static int psb_intel_crtc_cursor_set(struct drm_crtc *crtc,
 				 uint32_t width, uint32_t height)
 {
 	struct drm_device *dev = crtc->dev;
+	struct drm_psb_private *dev_priv = dev->dev_private;
 	struct psb_intel_crtc *psb_intel_crtc = to_psb_intel_crtc(crtc);
 	int pipe = psb_intel_crtc->pipe;
 	uint32_t control = (pipe == 0) ? CURACNTR : CURBCNTR;
@@ -969,8 +936,10 @@ static int psb_intel_crtc_cursor_set(struct drm_crtc *crtc,
 	uint32_t temp;
 	size_t addr = 0;
 	struct gtt_range *gt;
+	struct gtt_range *cursor_gt = psb_intel_crtc->cursor_gt;
 	struct drm_gem_object *obj;
-	int ret;
+	void *tmp_dst, *tmp_src;
+	int ret, i, cursor_pages;
 
 	/* if we want to turn of the cursor ignore width and height */
 	if (!handle) {
@@ -1019,10 +988,32 @@ static int psb_intel_crtc_cursor_set(struct drm_crtc *crtc,
 		return ret;
 	}
 
+	if (dev_priv->ops->cursor_needs_phys) {
+		if (cursor_gt == NULL) {
+			dev_err(dev->dev, "No hardware cursor mem available");
+			return -ENOMEM;
+		}
 
-	addr = gt->offset;	/* Or resource.start ??? */
+		/* Prevent overflow */
+		if (gt->npage > 4)
+			cursor_pages = 4;
+		else
+			cursor_pages = gt->npage;
+
+		/* Copy the cursor to cursor mem */
+		tmp_dst = dev_priv->vram_addr + cursor_gt->offset;
+		for (i = 0; i < cursor_pages; i++) {
+			tmp_src = kmap(gt->pages[i]);
+			memcpy(tmp_dst, tmp_src, PAGE_SIZE);
+			kunmap(gt->pages[i]);
+			tmp_dst += PAGE_SIZE;
+		}
 
-	psb_intel_crtc->cursor_addr = addr;
+		addr = psb_intel_crtc->cursor_addr;
+	} else {
+		addr = gt->offset;      /* Or resource.start ??? */
+		psb_intel_crtc->cursor_addr = addr;
+	}
 
 	temp = 0;
 	/* set the pipe for the cursor */
@@ -1115,34 +1106,30 @@ static int psb_intel_crtc_clock_get(struct drm_device *dev,
 				struct drm_crtc *crtc)
 {
 	struct psb_intel_crtc *psb_intel_crtc = to_psb_intel_crtc(crtc);
+	struct drm_psb_private *dev_priv = dev->dev_private;
 	int pipe = psb_intel_crtc->pipe;
+	const struct psb_offset *map = &dev_priv->regmap[pipe];
 	u32 dpll;
 	u32 fp;
 	struct psb_intel_clock_t clock;
 	bool is_lvds;
-	struct drm_psb_private *dev_priv = dev->dev_private;
+	struct psb_pipe *p = &dev_priv->regs.pipe[pipe];
 
 	if (gma_power_begin(dev, false)) {
-		dpll = REG_READ((pipe == 0) ? DPLL_A : DPLL_B);
+		dpll = REG_READ(map->dpll);
 		if ((dpll & DISPLAY_RATE_SELECT_FPA1) == 0)
-			fp = REG_READ((pipe == 0) ? FPA0 : FPB0);
+			fp = REG_READ(map->fp0);
 		else
-			fp = REG_READ((pipe == 0) ? FPA1 : FPB1);
+			fp = REG_READ(map->fp1);
 		is_lvds = (pipe == 1) && (REG_READ(LVDS) & LVDS_PORT_EN);
 		gma_power_end(dev);
 	} else {
-		dpll = (pipe == 0) ?
-			dev_priv->regs.psb.saveDPLL_A :
-			dev_priv->regs.psb.saveDPLL_B;
+		dpll = p->dpll;
 
 		if ((dpll & DISPLAY_RATE_SELECT_FPA1) == 0)
-			fp = (pipe == 0) ?
-				dev_priv->regs.psb.saveFPA0 :
-				dev_priv->regs.psb.saveFPB0;
+			fp = p->fp0;
 		else
-			fp = (pipe == 0) ?
-				dev_priv->regs.psb.saveFPA1 :
-				dev_priv->regs.psb.saveFPB1;
+		        fp = p->fp1;
 
 		is_lvds = (pipe == 1) && (dev_priv->regs.psb.saveLVDS &
 								LVDS_PORT_EN);
@@ -1202,26 +1189,20 @@ struct drm_display_mode *psb_intel_crtc_mode_get(struct drm_device *dev,
 	int vtot;
 	int vsync;
 	struct drm_psb_private *dev_priv = dev->dev_private;
+	struct psb_pipe *p = &dev_priv->regs.pipe[pipe];
+	const struct psb_offset *map = &dev_priv->regmap[pipe];
 
 	if (gma_power_begin(dev, false)) {
-		htot = REG_READ((pipe == 0) ? HTOTAL_A : HTOTAL_B);
-		hsync = REG_READ((pipe == 0) ? HSYNC_A : HSYNC_B);
-		vtot = REG_READ((pipe == 0) ? VTOTAL_A : VTOTAL_B);
-		vsync = REG_READ((pipe == 0) ? VSYNC_A : VSYNC_B);
+		htot = REG_READ(map->htotal);
+		hsync = REG_READ(map->hsync);
+		vtot = REG_READ(map->vtotal);
+		vsync = REG_READ(map->vsync);
 		gma_power_end(dev);
 	} else {
-		htot = (pipe == 0) ?
-			dev_priv->regs.psb.saveHTOTAL_A :
-			dev_priv->regs.psb.saveHTOTAL_B;
-		hsync = (pipe == 0) ?
-			dev_priv->regs.psb.saveHSYNC_A :
-			dev_priv->regs.psb.saveHSYNC_B;
-		vtot = (pipe == 0) ?
-			dev_priv->regs.psb.saveVTOTAL_A :
-			dev_priv->regs.psb.saveVTOTAL_B;
-		vsync = (pipe == 0) ?
-			dev_priv->regs.psb.saveVSYNC_A :
-			dev_priv->regs.psb.saveVSYNC_B;
+		htot = p->htotal;
+		hsync = p->hsync;
+		vtot = p->vtotal;
+		vsync = p->vsync;
 	}
 
 	mode = kzalloc(sizeof(*mode), GFP_KERNEL);
@@ -1257,6 +1238,9 @@ void psb_intel_crtc_destroy(struct drm_crtc *crtc)
 		drm_gem_object_unreference(psb_intel_crtc->cursor_obj);
 		psb_intel_crtc->cursor_obj = NULL;
 	}
+
+	if (psb_intel_crtc->cursor_gt != NULL)
+		psb_gtt_free_range(crtc->dev, psb_intel_crtc->cursor_gt);
 	kfree(psb_intel_crtc->crtc_state);
 	drm_crtc_cleanup(crtc);
 	kfree(psb_intel_crtc);
@@ -1285,13 +1269,33 @@ const struct drm_crtc_funcs psb_intel_crtc_funcs = {
  * Set the default value of cursor control and base register
  * to zero. This is a workaround for h/w defect on Oaktrail
  */
-static void psb_intel_cursor_init(struct drm_device *dev, int pipe)
+static void psb_intel_cursor_init(struct drm_device *dev,
+				  struct psb_intel_crtc *psb_intel_crtc)
 {
+	struct drm_psb_private *dev_priv = dev->dev_private;
 	u32 control[3] = { CURACNTR, CURBCNTR, CURCCNTR };
 	u32 base[3] = { CURABASE, CURBBASE, CURCBASE };
+	struct gtt_range *cursor_gt;
+
+	if (dev_priv->ops->cursor_needs_phys) {
+		/* Allocate 4 pages of stolen mem for a hardware cursor. That
+		 * is enough for the 64 x 64 ARGB cursors we support.
+		 */
+		cursor_gt = psb_gtt_alloc_range(dev, 4 * PAGE_SIZE, "cursor", 1);
+		if (!cursor_gt) {
+			psb_intel_crtc->cursor_gt = NULL;
+			goto out;
+		}
+		psb_intel_crtc->cursor_gt = cursor_gt;
+		psb_intel_crtc->cursor_addr = dev_priv->stolen_base +
+							cursor_gt->offset;
+	} else {
+		psb_intel_crtc->cursor_gt = NULL;
+	}
 
-	REG_WRITE(control[pipe], 0);
-	REG_WRITE(base[pipe], 0);
+out:
+	REG_WRITE(control[psb_intel_crtc->pipe], 0);
+	REG_WRITE(base[psb_intel_crtc->pipe], 0);
 }
 
 void psb_intel_crtc_init(struct drm_device *dev, int pipe,
@@ -1357,7 +1361,7 @@ void psb_intel_crtc_init(struct drm_device *dev, int pipe,
 	psb_intel_crtc->mode_set.connectors =
 	    (struct drm_connector **) (psb_intel_crtc + 1);
 	psb_intel_crtc->mode_set.num_connectors = 0;
-	psb_intel_cursor_init(dev, pipe);
+	psb_intel_cursor_init(dev, psb_intel_crtc);
 }
 
 int psb_intel_get_pipe_from_crtc_id(struct drm_device *dev, void *data,
diff --git a/drivers/gpu/drm/gma500/psb_intel_drv.h b/drivers/gpu/drm/gma500/psb_intel_drv.h
index f40535e5668..2515f83248c 100644
--- a/drivers/gpu/drm/gma500/psb_intel_drv.h
+++ b/drivers/gpu/drm/gma500/psb_intel_drv.h
@@ -106,11 +106,6 @@ struct psb_intel_mode_device {
 	 size_t(*bo_offset) (struct drm_device *dev, void *bo);
 
 	/*
-	 * Cursor (Can go ?)
-	 */
-	int cursor_needs_physical;
-
-	/*
 	 * LVDS info
 	 */
 	int backlight_duty_cycle;	/* restore backlight to this value */
@@ -176,6 +171,7 @@ struct psb_intel_crtc {
 	int pipe;
 	int plane;
 	uint32_t cursor_addr;
+	struct gtt_range *cursor_gt;
 	u8 lut_r[256], lut_g[256], lut_b[256];
 	u8 lut_adj[256];
 	struct psb_intel_framebuffer *fbdev_fb;
@@ -193,6 +189,9 @@ struct psb_intel_crtc {
 	/*crtc mode setting flags*/
 	u32 mode_flags;
 
+	bool active;
+	bool crtc_enable;
+
 	/* Saved Crtc HW states */
 	struct psb_intel_crtc_state *crtc_state;
 };
diff --git a/drivers/gpu/drm/gma500/psb_intel_reg.h b/drivers/gpu/drm/gma500/psb_intel_reg.h
index e89d3a2e8fd..8e8c8efb0a8 100644
--- a/drivers/gpu/drm/gma500/psb_intel_reg.h
+++ b/drivers/gpu/drm/gma500/psb_intel_reg.h
@@ -91,6 +91,9 @@
 
 #define BLC_PWM_CTL		0x61254
 #define BLC_PWM_CTL2		0x61250
+#define  PWM_ENABLE		(1 << 31)
+#define  PWM_LEGACY_MODE	(1 << 30)
+#define  PWM_PIPE_B		(1 << 29)
 #define BLC_PWM_CTL_C		0x62254
 #define BLC_PWM_CTL2_C		0x62250
 #define BACKLIGHT_MODULATION_FREQ_SHIFT		(17)
@@ -216,7 +219,7 @@
 #define DPLLB_LVDS_P2_CLOCK_DIV_14	(0 << 24)	/* i915 */
 #define DPLLB_LVDS_P2_CLOCK_DIV_7	(1 << 24)	/* i915 */
 #define DPLL_P2_CLOCK_DIV_MASK		0x03000000	/* i915 */
-#define DPLL_FPA01_P1_POST_DIV_MASK	0x00ff0000	/* i915 */
+#define DPLL_FPA0h1_P1_POST_DIV_MASK	0x00ff0000	/* i915 */
 #define DPLL_LOCK			(1 << 15)	/* CDV */
 
 /*
@@ -343,6 +346,9 @@
 #define FP_M2_DIV_SHIFT			0
 
 #define PORT_HOTPLUG_EN		0x61110
+#define HDMIB_HOTPLUG_INT_EN		(1 << 29)
+#define HDMIC_HOTPLUG_INT_EN		(1 << 28)
+#define HDMID_HOTPLUG_INT_EN		(1 << 27)
 #define SDVOB_HOTPLUG_INT_EN		(1 << 26)
 #define SDVOC_HOTPLUG_INT_EN		(1 << 25)
 #define TV_HOTPLUG_INT_EN		(1 << 18)
@@ -501,10 +507,12 @@
 #define PIPE_VBLANK_INTERRUPT_ENABLE		(1UL << 17)
 #define PIPE_START_VBLANK_INTERRUPT_ENABLE	(1UL << 18)
 #define PIPE_TE_ENABLE				(1UL << 22)
+#define PIPE_LEGACY_BLC_EVENT_ENABLE		(1UL << 22)
 #define PIPE_DPST_EVENT_ENABLE			(1UL << 23)
 #define PIPE_VSYNC_ENABL			(1UL << 25)
 #define PIPE_HDMI_AUDIO_UNDERRUN		(1UL << 26)
 #define PIPE_HDMI_AUDIO_BUFFER_DONE		(1UL << 27)
+#define PIPE_FIFO_UNDERRUN			(1UL << 31)
 #define PIPE_HDMI_AUDIO_INT_MASK		(PIPE_HDMI_AUDIO_UNDERRUN | \
 						PIPE_HDMI_AUDIO_BUFFER_DONE)
 #define PIPE_EVENT_MASK ((1 << 29)|(1 << 28)|(1 << 27)|(1 << 26)|(1 << 24)|(1 << 23)|(1 << 22)|(1 << 21)|(1 << 20)|(1 << 16))
@@ -569,12 +577,27 @@ struct dpst_guardband {
 #define PIPE_PIXEL_MASK		0x00ffffff
 #define PIPE_PIXEL_SHIFT	0
 
+#define FW_BLC_SELF		0x20e0 
+#define FW_BLC_SELF_EN          (1<<15)
+
 #define DSPARB			0x70030
 #define DSPFW1			0x70034
+#define DSP_FIFO_SR_WM_MASK		0xFF800000
+#define DSP_FIFO_SR_WM_SHIFT		23
+#define CURSOR_B_FIFO_WM_MASK		0x003F0000
+#define CURSOR_B_FIFO_WM_SHIFT		16
 #define DSPFW2			0x70038
+#define CURSOR_A_FIFO_WM_MASK		0x3F00
+#define CURSOR_A_FIFO_WM_SHIFT		8
+#define DSP_PLANE_C_FIFO_WM_MASK	0x7F
+#define DSP_PLANE_C_FIFO_WM_SHIFT	0
 #define DSPFW3			0x7003c
 #define DSPFW4			0x70050
 #define DSPFW5			0x70054
+#define DSP_PLANE_B_FIFO_WM1_SHIFT	24
+#define DSP_PLANE_A_FIFO_WM1_SHIFT	16
+#define CURSOR_B_FIFO_WM1_SHIFT		8
+#define CURSOR_FIFO_SR_WM1_SHIFT	0
 #define DSPFW6			0x70058
 #define DSPCHICKENBIT		0x70400
 #define DSPACNTR		0x70180
@@ -1290,6 +1313,15 @@ No status bits are changed.
 #define SB_N_CB_TUNE_MASK			PSB_MASK(25, 24)
 #define SB_N_CB_TUNE_SHIFT			24
 
+/* the bit 14:13 is used to select between the different reference clock for Pipe A/B */
+#define SB_REF_DPLLA		0x8010
+#define SB_REF_DPLLB		0x8030
+#define	REF_CLK_MASK		(0x3 << 13)
+#define REF_CLK_CORE		(0 << 13)
+#define REF_CLK_DPLL		(1 << 13)
+#define REF_CLK_DPLLA		(2 << 13)
+/* For the DPLL B, it will use the reference clk from DPLL A when using (2 << 13) */
+
 #define _SB_REF_A		0x8018
 #define _SB_REF_B		0x8038
 #define SB_REF_SFR(pipe)	_PIPE(pipe, _SB_REF_A, _SB_REF_B)
@@ -1313,6 +1345,7 @@ No status bits are changed.
 
 #define LANE_PLL_MASK		(0x7 << 20)
 #define LANE_PLL_ENABLE		(0x3 << 20)
+#define LANE_PLL_PIPE(p)	(((p) == 0) ? (1 << 21) : (0 << 21))
 
 
 #endif
diff --git a/drivers/gpu/drm/gma500/psb_intel_sdvo.c b/drivers/gpu/drm/gma500/psb_intel_sdvo.c
index 36330cabcea..d39b15be764 100644
--- a/drivers/gpu/drm/gma500/psb_intel_sdvo.c
+++ b/drivers/gpu/drm/gma500/psb_intel_sdvo.c
@@ -1141,7 +1141,6 @@ static void psb_intel_sdvo_dpms(struct drm_encoder *encoder, int mode)
 static int psb_intel_sdvo_mode_valid(struct drm_connector *connector,
 				 struct drm_display_mode *mode)
 {
-	struct drm_psb_private *dev_priv = connector->dev->dev_private;
 	struct psb_intel_sdvo *psb_intel_sdvo = intel_attached_sdvo(connector);
 
 	if (mode->flags & DRM_MODE_FLAG_DBLSCAN)
@@ -1161,11 +1160,6 @@ static int psb_intel_sdvo_mode_valid(struct drm_connector *connector,
 			return MODE_PANEL;
 	}
 
-	/* We assume worst case scenario of 32 bpp here, since we don't know */
-	if ((ALIGN(mode->hdisplay * 4, 64) * mode->vdisplay) >
-	    dev_priv->vram_stolen_size)
-		return MODE_MEM;
-
 	return MODE_OK;
 }
 
@@ -2044,8 +2038,7 @@ psb_intel_sdvo_add_hdmi_properties(struct psb_intel_sdvo_connector *connector)
 	struct drm_device *dev = connector->base.base.dev;
 
 	intel_attach_force_audio_property(&connector->base.base);
-	if (INTEL_INFO(dev)->gen >= 4 && IS_MOBILE(dev))
-		intel_attach_broadcast_rgb_property(&connector->base.base);
+	intel_attach_broadcast_rgb_property(&connector->base.base);
 	*/
 }
 
diff --git a/drivers/gpu/drm/gma500/psb_irq.c b/drivers/gpu/drm/gma500/psb_irq.c
index 1869586457b..8652cdf3f03 100644
--- a/drivers/gpu/drm/gma500/psb_irq.c
+++ b/drivers/gpu/drm/gma500/psb_irq.c
@@ -190,6 +190,9 @@ static void mid_pipe_event_handler(struct drm_device *dev, int pipe)
  */
 static void psb_vdc_interrupt(struct drm_device *dev, uint32_t vdc_stat)
 {
+	if (vdc_stat & _PSB_IRQ_ASLE)
+		psb_intel_opregion_asle_intr(dev);
+
 	if (vdc_stat & _PSB_VSYNC_PIPEA_FLAG)
 		mid_pipe_event_handler(dev, 0);
 
@@ -199,11 +202,9 @@ static void psb_vdc_interrupt(struct drm_device *dev, uint32_t vdc_stat)
 
 irqreturn_t psb_irq_handler(DRM_IRQ_ARGS)
 {
-	struct drm_device *dev = (struct drm_device *) arg;
-	struct drm_psb_private *dev_priv =
-	    (struct drm_psb_private *) dev->dev_private;
-
-	uint32_t vdc_stat, dsp_int = 0, sgx_int = 0;
+	struct drm_device *dev = arg;
+	struct drm_psb_private *dev_priv = dev->dev_private;
+	uint32_t vdc_stat, dsp_int = 0, sgx_int = 0, hotplug_int = 0;
 	int handled = 0;
 
 	spin_lock(&dev_priv->irqmask_lock);
@@ -220,6 +221,8 @@ irqreturn_t psb_irq_handler(DRM_IRQ_ARGS)
 
 	if (vdc_stat & _PSB_IRQ_SGX_FLAG)
 		sgx_int = 1;
+	if (vdc_stat & _PSB_IRQ_DISP_HOTSYNC)
+		hotplug_int = 1;
 
 	vdc_stat &= dev_priv->vdc_irq_mask;
 	spin_unlock(&dev_priv->irqmask_lock);
@@ -241,6 +244,13 @@ irqreturn_t psb_irq_handler(DRM_IRQ_ARGS)
 		handled = 1;
 	}
 
+	/* Note: this bit has other meanings on some devices, so we will
+	   need to address that later if it ever matters */
+	if (hotplug_int && dev_priv->ops->hotplug) {
+		handled = dev_priv->ops->hotplug(dev);
+		REG_WRITE(PORT_HOTPLUG_STAT, REG_READ(PORT_HOTPLUG_STAT));
+	}
+
 	PSB_WVDC32(vdc_stat, PSB_INT_IDENTITY_R);
 	(void) PSB_RVDC32(PSB_INT_IDENTITY_R);
 	DRM_READMEMORYBARRIER();
@@ -273,6 +283,11 @@ void psb_irq_preinstall(struct drm_device *dev)
 		dev_priv->vdc_irq_mask |= _MDFLD_PIPEC_EVENT_FLAG;
 	*/
 
+	/* Revisit this area - want per device masks ? */
+	if (dev_priv->ops->hotplug)
+		dev_priv->vdc_irq_mask |= _PSB_IRQ_DISP_HOTSYNC;
+	dev_priv->vdc_irq_mask |= _PSB_IRQ_ASLE;
+
 	/* This register is safe even if display island is off */
 	PSB_WVDC32(~dev_priv->vdc_irq_mask, PSB_INT_MASK_R);
 	spin_unlock_irqrestore(&dev_priv->irqmask_lock, irqflags);
@@ -305,18 +320,23 @@ int psb_irq_postinstall(struct drm_device *dev)
 	else
 		psb_disable_pipestat(dev_priv, 2, PIPE_VBLANK_INTERRUPT_ENABLE);
 
+	if (dev_priv->ops->hotplug_enable)
+		dev_priv->ops->hotplug_enable(dev, true);
+
 	spin_unlock_irqrestore(&dev_priv->irqmask_lock, irqflags);
 	return 0;
 }
 
 void psb_irq_uninstall(struct drm_device *dev)
 {
-	struct drm_psb_private *dev_priv =
-	    (struct drm_psb_private *) dev->dev_private;
+	struct drm_psb_private *dev_priv = dev->dev_private;
 	unsigned long irqflags;
 
 	spin_lock_irqsave(&dev_priv->irqmask_lock, irqflags);
 
+	if (dev_priv->ops->hotplug_enable)
+		dev_priv->ops->hotplug_enable(dev, false);
+
 	PSB_WVDC32(0xFFFFFFFF, PSB_HWSTAM);
 
 	if (dev->vblank_enabled[0])
@@ -406,7 +426,7 @@ void psb_irq_turn_off_dpst(struct drm_device *dev)
 		psb_disable_pipestat(dev_priv, 0, PIPE_DPST_EVENT_ENABLE);
 
 		pwm_reg = PSB_RVDC32(PWM_CONTROL_LOGIC);
-		PSB_WVDC32(pwm_reg & !(PWM_PHASEIN_INT_ENABLE),
+		PSB_WVDC32(pwm_reg & ~PWM_PHASEIN_INT_ENABLE,
 							PWM_CONTROL_LOGIC);
 		pwm_reg = PSB_RVDC32(PWM_CONTROL_LOGIC);
 
diff --git a/drivers/gpu/drm/gma500/psb_lid.c b/drivers/gpu/drm/gma500/psb_lid.c
index b867aabe6bf..1d2ebb5e530 100644
--- a/drivers/gpu/drm/gma500/psb_lid.c
+++ b/drivers/gpu/drm/gma500/psb_lid.c
@@ -29,7 +29,7 @@ static void psb_lid_timer_func(unsigned long data)
 	struct drm_device *dev = (struct drm_device *)dev_priv->dev;
 	struct timer_list *lid_timer = &dev_priv->lid_timer;
 	unsigned long irq_flags;
-	u32 *lid_state = dev_priv->lid_state;
+	u32 __iomem *lid_state = dev_priv->opregion.lid_state;
 	u32 pp_status;
 
 	if (readl(lid_state) == dev_priv->lid_last_state)
@@ -40,10 +40,16 @@ static void psb_lid_timer_func(unsigned long data)
 		REG_WRITE(PP_CONTROL, REG_READ(PP_CONTROL) | POWER_TARGET_ON);
 		do {
 			pp_status = REG_READ(PP_STATUS);
-		} while ((pp_status & PP_ON) == 0);
+		} while ((pp_status & PP_ON) == 0 &&
+			 (pp_status & PP_SEQUENCE_MASK) != 0);
 
-		/*FIXME: should be backlight level before*/
-		psb_intel_lvds_set_brightness(dev, 100);
+		if (REG_READ(PP_STATUS) & PP_ON) {
+			/*FIXME: should be backlight level before*/
+			psb_intel_lvds_set_brightness(dev, 100);
+		} else {
+			DRM_DEBUG("LVDS panel never powered up");
+			return;
+		}
 	} else {
 		psb_intel_lvds_set_brightness(dev, 0);
 
diff --git a/drivers/gpu/drm/i915/Makefile b/drivers/gpu/drm/i915/Makefile
index ce7fc77678b..2e9268da58d 100644
--- a/drivers/gpu/drm/i915/Makefile
+++ b/drivers/gpu/drm/i915/Makefile
@@ -11,17 +11,21 @@ i915-y := i915_drv.o i915_dma.o i915_irq.o \
 	  i915_gem_evict.o \
 	  i915_gem_execbuffer.o \
 	  i915_gem_gtt.o \
+	  i915_gem_stolen.o \
 	  i915_gem_tiling.o \
+	  i915_sysfs.o \
 	  i915_trace_points.o \
 	  intel_display.o \
 	  intel_crt.o \
 	  intel_lvds.o \
 	  intel_bios.o \
+	  intel_ddi.o \
 	  intel_dp.o \
 	  intel_hdmi.o \
 	  intel_sdvo.o \
 	  intel_modes.o \
 	  intel_panel.o \
+	  intel_pm.o \
 	  intel_i2c.o \
 	  intel_fb.o \
 	  intel_tv.o \
@@ -34,7 +38,8 @@ i915-y := i915_drv.o i915_dma.o i915_irq.o \
 	  dvo_ch7017.o \
 	  dvo_ivch.o \
 	  dvo_tfp410.o \
-	  dvo_sil164.o
+	  dvo_sil164.o \
+	  i915_gem_dmabuf.o
 
 i915-$(CONFIG_COMPAT)   += i915_ioc32.o
 
diff --git a/drivers/gpu/drm/i915/i915_debugfs.c b/drivers/gpu/drm/i915/i915_debugfs.c
index e6162a1681f..eb2b3c25b9e 100644
--- a/drivers/gpu/drm/i915/i915_debugfs.c
+++ b/drivers/gpu/drm/i915/i915_debugfs.c
@@ -47,7 +47,6 @@ enum {
 	FLUSHING_LIST,
 	INACTIVE_LIST,
 	PINNED_LIST,
-	DEFERRED_FREE_LIST,
 };
 
 static const char *yesno(int v)
@@ -178,18 +177,10 @@ static int i915_gem_object_list_info(struct seq_file *m, void *data)
 		seq_printf(m, "Inactive:\n");
 		head = &dev_priv->mm.inactive_list;
 		break;
-	case PINNED_LIST:
-		seq_printf(m, "Pinned:\n");
-		head = &dev_priv->mm.pinned_list;
-		break;
 	case FLUSHING_LIST:
 		seq_printf(m, "Flushing:\n");
 		head = &dev_priv->mm.flushing_list;
 		break;
-	case DEFERRED_FREE_LIST:
-		seq_printf(m, "Deferred free:\n");
-		head = &dev_priv->mm.deferred_free_list;
-		break;
 	default:
 		mutex_unlock(&dev->struct_mutex);
 		return -EINVAL;
@@ -252,21 +243,11 @@ static int i915_gem_object_info(struct seq_file *m, void* data)
 		   count, mappable_count, size, mappable_size);
 
 	size = count = mappable_size = mappable_count = 0;
-	count_objects(&dev_priv->mm.pinned_list, mm_list);
-	seq_printf(m, "  %u [%u] pinned objects, %zu [%zu] bytes\n",
-		   count, mappable_count, size, mappable_size);
-
-	size = count = mappable_size = mappable_count = 0;
 	count_objects(&dev_priv->mm.inactive_list, mm_list);
 	seq_printf(m, "  %u [%u] inactive objects, %zu [%zu] bytes\n",
 		   count, mappable_count, size, mappable_size);
 
 	size = count = mappable_size = mappable_count = 0;
-	count_objects(&dev_priv->mm.deferred_free_list, mm_list);
-	seq_printf(m, "  %u [%u] freed objects, %zu [%zu] bytes\n",
-		   count, mappable_count, size, mappable_size);
-
-	size = count = mappable_size = mappable_count = 0;
 	list_for_each_entry(obj, &dev_priv->mm.gtt_list, gtt_list) {
 		if (obj->fault_mappable) {
 			size += obj->gtt_space->size;
@@ -294,6 +275,7 @@ static int i915_gem_gtt_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;
+	uintptr_t list = (uintptr_t) node->info_ent->data;
 	struct drm_i915_private *dev_priv = dev->dev_private;
 	struct drm_i915_gem_object *obj;
 	size_t total_obj_size, total_gtt_size;
@@ -305,6 +287,9 @@ static int i915_gem_gtt_info(struct seq_file *m, void* data)
 
 	total_obj_size = total_gtt_size = count = 0;
 	list_for_each_entry(obj, &dev_priv->mm.gtt_list, gtt_list) {
+		if (list == PINNED_LIST && obj->pin_count == 0)
+			continue;
+
 		seq_printf(m, "   ");
 		describe_obj(m, obj);
 		seq_printf(m, "\n");
@@ -321,7 +306,6 @@ static int i915_gem_gtt_info(struct seq_file *m, void* data)
 	return 0;
 }
 
-
 static int i915_gem_pageflip_info(struct seq_file *m, void *data)
 {
 	struct drm_info_node *node = (struct drm_info_node *) m->private;
@@ -430,10 +414,6 @@ static void i915_ring_seqno_info(struct seq_file *m,
 	if (ring->get_seqno) {
 		seq_printf(m, "Current sequence (%s): %d\n",
 			   ring->name, ring->get_seqno(ring));
-		seq_printf(m, "Waiter sequence (%s):  %d\n",
-			   ring->name, ring->waiting_seqno);
-		seq_printf(m, "IRQ sequence (%s):     %d\n",
-			   ring->name, ring->irq_seqno);
 	}
 }
 
@@ -468,7 +448,45 @@ static int i915_interrupt_info(struct seq_file *m, void *data)
 	if (ret)
 		return ret;
 
-	if (!HAS_PCH_SPLIT(dev)) {
+	if (IS_VALLEYVIEW(dev)) {
+		seq_printf(m, "Display IER:\t%08x\n",
+			   I915_READ(VLV_IER));
+		seq_printf(m, "Display IIR:\t%08x\n",
+			   I915_READ(VLV_IIR));
+		seq_printf(m, "Display IIR_RW:\t%08x\n",
+			   I915_READ(VLV_IIR_RW));
+		seq_printf(m, "Display IMR:\t%08x\n",
+			   I915_READ(VLV_IMR));
+		for_each_pipe(pipe)
+			seq_printf(m, "Pipe %c stat:\t%08x\n",
+				   pipe_name(pipe),
+				   I915_READ(PIPESTAT(pipe)));
+
+		seq_printf(m, "Master IER:\t%08x\n",
+			   I915_READ(VLV_MASTER_IER));
+
+		seq_printf(m, "Render IER:\t%08x\n",
+			   I915_READ(GTIER));
+		seq_printf(m, "Render IIR:\t%08x\n",
+			   I915_READ(GTIIR));
+		seq_printf(m, "Render IMR:\t%08x\n",
+			   I915_READ(GTIMR));
+
+		seq_printf(m, "PM IER:\t\t%08x\n",
+			   I915_READ(GEN6_PMIER));
+		seq_printf(m, "PM IIR:\t\t%08x\n",
+			   I915_READ(GEN6_PMIIR));
+		seq_printf(m, "PM IMR:\t\t%08x\n",
+			   I915_READ(GEN6_PMIMR));
+
+		seq_printf(m, "Port hotplug:\t%08x\n",
+			   I915_READ(PORT_HOTPLUG_EN));
+		seq_printf(m, "DPFLIPSTAT:\t%08x\n",
+			   I915_READ(VLV_DPFLIPSTAT));
+		seq_printf(m, "DPINVGTT:\t%08x\n",
+			   I915_READ(DPINVGTT));
+
+	} else if (!HAS_PCH_SPLIT(dev)) {
 		seq_printf(m, "Interrupt enable:    %08x\n",
 			   I915_READ(IER));
 		seq_printf(m, "Interrupt identity:  %08x\n",
@@ -564,69 +582,6 @@ static int i915_hws_info(struct seq_file *m, void *data)
 	return 0;
 }
 
-static int i915_ringbuffer_data(struct seq_file *m, void *data)
-{
-	struct drm_info_node *node = (struct drm_info_node *) m->private;
-	struct drm_device *dev = node->minor->dev;
-	drm_i915_private_t *dev_priv = dev->dev_private;
-	struct intel_ring_buffer *ring;
-	int ret;
-
-	ret = mutex_lock_interruptible(&dev->struct_mutex);
-	if (ret)
-		return ret;
-
-	ring = &dev_priv->ring[(uintptr_t)node->info_ent->data];
-	if (!ring->obj) {
-		seq_printf(m, "No ringbuffer setup\n");
-	} else {
-		const u8 __iomem *virt = ring->virtual_start;
-		uint32_t off;
-
-		for (off = 0; off < ring->size; off += 4) {
-			uint32_t *ptr = (uint32_t *)(virt + off);
-			seq_printf(m, "%08x :  %08x\n", off, *ptr);
-		}
-	}
-	mutex_unlock(&dev->struct_mutex);
-
-	return 0;
-}
-
-static int i915_ringbuffer_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;
-	drm_i915_private_t *dev_priv = dev->dev_private;
-	struct intel_ring_buffer *ring;
-	int ret;
-
-	ring = &dev_priv->ring[(uintptr_t)node->info_ent->data];
-	if (ring->size == 0)
-		return 0;
-
-	ret = mutex_lock_interruptible(&dev->struct_mutex);
-	if (ret)
-		return ret;
-
-	seq_printf(m, "Ring %s:\n", ring->name);
-	seq_printf(m, "  Head :    %08x\n", I915_READ_HEAD(ring) & HEAD_ADDR);
-	seq_printf(m, "  Tail :    %08x\n", I915_READ_TAIL(ring) & TAIL_ADDR);
-	seq_printf(m, "  Size :    %08x\n", ring->size);
-	seq_printf(m, "  Active :  %08x\n", intel_ring_get_active_head(ring));
-	seq_printf(m, "  NOPID :   %08x\n", I915_READ_NOPID(ring));
-	if (IS_GEN6(dev) || IS_GEN7(dev)) {
-		seq_printf(m, "  Sync 0 :   %08x\n", I915_READ_SYNC_0(ring));
-		seq_printf(m, "  Sync 1 :   %08x\n", I915_READ_SYNC_1(ring));
-	}
-	seq_printf(m, "  Control : %08x\n", I915_READ_CTL(ring));
-	seq_printf(m, "  Start :   %08x\n", I915_READ_START(ring));
-
-	mutex_unlock(&dev->struct_mutex);
-
-	return 0;
-}
-
 static const char *ring_str(int ring)
 {
 	switch (ring) {
@@ -704,6 +659,7 @@ static void i915_ring_error_state(struct seq_file *m,
 				  struct drm_i915_error_state *error,
 				  unsigned ring)
 {
+	BUG_ON(ring >= I915_NUM_RINGS); /* shut up confused gcc */
 	seq_printf(m, "%s command stream:\n", ring_str(ring));
 	seq_printf(m, "  HEAD: 0x%08x\n", error->head[ring]);
 	seq_printf(m, "  TAIL: 0x%08x\n", error->tail[ring]);
@@ -718,8 +674,8 @@ static void i915_ring_error_state(struct seq_file *m,
 	if (INTEL_INFO(dev)->gen >= 4)
 		seq_printf(m, "  INSTPS: 0x%08x\n", error->instps[ring]);
 	seq_printf(m, "  INSTPM: 0x%08x\n", error->instpm[ring]);
+	seq_printf(m, "  FADDR: 0x%08x\n", error->faddr[ring]);
 	if (INTEL_INFO(dev)->gen >= 6) {
-		seq_printf(m, "  FADDR: 0x%08x\n", error->faddr[ring]);
 		seq_printf(m, "  FAULT_REG: 0x%08x\n", error->fault_reg[ring]);
 		seq_printf(m, "  SYNC_0: 0x%08x\n",
 			   error->semaphore_mboxes[ring][0]);
@@ -727,31 +683,35 @@ static void i915_ring_error_state(struct seq_file *m,
 			   error->semaphore_mboxes[ring][1]);
 	}
 	seq_printf(m, "  seqno: 0x%08x\n", error->seqno[ring]);
+	seq_printf(m, "  waiting: %s\n", yesno(error->waiting[ring]));
 	seq_printf(m, "  ring->head: 0x%08x\n", error->cpu_ring_head[ring]);
 	seq_printf(m, "  ring->tail: 0x%08x\n", error->cpu_ring_tail[ring]);
 }
 
+struct i915_error_state_file_priv {
+	struct drm_device *dev;
+	struct drm_i915_error_state *error;
+};
+
 static int i915_error_state(struct seq_file *m, void *unused)
 {
-	struct drm_info_node *node = (struct drm_info_node *) m->private;
-	struct drm_device *dev = node->minor->dev;
+	struct i915_error_state_file_priv *error_priv = m->private;
+	struct drm_device *dev = error_priv->dev;
 	drm_i915_private_t *dev_priv = dev->dev_private;
-	struct drm_i915_error_state *error;
-	unsigned long flags;
+	struct drm_i915_error_state *error = error_priv->error;
+	struct intel_ring_buffer *ring;
 	int i, j, page, offset, elt;
 
-	spin_lock_irqsave(&dev_priv->error_lock, flags);
-	if (!dev_priv->first_error) {
+	if (!error) {
 		seq_printf(m, "no error state collected\n");
-		goto out;
+		return 0;
 	}
 
-	error = dev_priv->first_error;
-
 	seq_printf(m, "Time: %ld s %ld us\n", error->time.tv_sec,
 		   error->time.tv_usec);
 	seq_printf(m, "PCI ID: 0x%04x\n", dev->pci_device);
 	seq_printf(m, "EIR: 0x%08x\n", error->eir);
+	seq_printf(m, "IER: 0x%08x\n", error->ier);
 	seq_printf(m, "PGTBL_ER: 0x%08x\n", error->pgtbl_er);
 
 	for (i = 0; i < dev_priv->num_fence_regs; i++)
@@ -762,11 +722,8 @@ static int i915_error_state(struct seq_file *m, void *unused)
 		seq_printf(m, "DONE_REG: 0x%08x\n", error->done_reg);
 	}
 
-	i915_ring_error_state(m, dev, error, RCS);
-	if (HAS_BLT(dev))
-		i915_ring_error_state(m, dev, error, BCS);
-	if (HAS_BSD(dev))
-		i915_ring_error_state(m, dev, error, VCS);
+	for_each_ring(ring, dev_priv, i)
+		i915_ring_error_state(m, dev, error, i);
 
 	if (error->active_bo)
 		print_error_buffers(m, "Active",
@@ -828,12 +785,71 @@ static int i915_error_state(struct seq_file *m, void *unused)
 	if (error->display)
 		intel_display_print_error_state(m, dev, error->display);
 
-out:
+	return 0;
+}
+
+static ssize_t
+i915_error_state_write(struct file *filp,
+		       const char __user *ubuf,
+		       size_t cnt,
+		       loff_t *ppos)
+{
+	struct seq_file *m = filp->private_data;
+	struct i915_error_state_file_priv *error_priv = m->private;
+	struct drm_device *dev = error_priv->dev;
+
+	DRM_DEBUG_DRIVER("Resetting error state\n");
+
+	mutex_lock(&dev->struct_mutex);
+	i915_destroy_error_state(dev);
+	mutex_unlock(&dev->struct_mutex);
+
+	return cnt;
+}
+
+static int i915_error_state_open(struct inode *inode, struct file *file)
+{
+	struct drm_device *dev = inode->i_private;
+	drm_i915_private_t *dev_priv = dev->dev_private;
+	struct i915_error_state_file_priv *error_priv;
+	unsigned long flags;
+
+	error_priv = kzalloc(sizeof(*error_priv), GFP_KERNEL);
+	if (!error_priv)
+		return -ENOMEM;
+
+	error_priv->dev = dev;
+
+	spin_lock_irqsave(&dev_priv->error_lock, flags);
+	error_priv->error = dev_priv->first_error;
+	if (error_priv->error)
+		kref_get(&error_priv->error->ref);
 	spin_unlock_irqrestore(&dev_priv->error_lock, flags);
 
-	return 0;
+	return single_open(file, i915_error_state, error_priv);
+}
+
+static int i915_error_state_release(struct inode *inode, struct file *file)
+{
+	struct seq_file *m = file->private_data;
+	struct i915_error_state_file_priv *error_priv = m->private;
+
+	if (error_priv->error)
+		kref_put(&error_priv->error->ref, i915_error_state_free);
+	kfree(error_priv);
+
+	return single_release(inode, file);
 }
 
+static const struct file_operations i915_error_state_fops = {
+	.owner = THIS_MODULE,
+	.open = i915_error_state_open,
+	.read = seq_read,
+	.write = i915_error_state_write,
+	.llseek = default_llseek,
+	.release = i915_error_state_release,
+};
+
 static int i915_rstdby_delays(struct seq_file *m, void *unused)
 {
 	struct drm_info_node *node = (struct drm_info_node *) m->private;
@@ -1132,6 +1148,17 @@ static int gen6_drpc_info(struct seq_file *m)
 
 	seq_printf(m, "Core Power Down: %s\n",
 		   yesno(gt_core_status & GEN6_CORE_CPD_STATE_MASK));
+
+	/* Not exactly sure what this is */
+	seq_printf(m, "RC6 \"Locked to RPn\" residency since boot: %u\n",
+		   I915_READ(GEN6_GT_GFX_RC6_LOCKED));
+	seq_printf(m, "RC6 residency since boot: %u\n",
+		   I915_READ(GEN6_GT_GFX_RC6));
+	seq_printf(m, "RC6+ residency since boot: %u\n",
+		   I915_READ(GEN6_GT_GFX_RC6p));
+	seq_printf(m, "RC6++ residency since boot: %u\n",
+		   I915_READ(GEN6_GT_GFX_RC6pp));
+
 	return 0;
 }
 
@@ -1306,17 +1333,25 @@ static int i915_opregion(struct seq_file *m, void *unused)
 	struct drm_device *dev = node->minor->dev;
 	drm_i915_private_t *dev_priv = dev->dev_private;
 	struct intel_opregion *opregion = &dev_priv->opregion;
+	void *data = kmalloc(OPREGION_SIZE, GFP_KERNEL);
 	int ret;
 
+	if (data == NULL)
+		return -ENOMEM;
+
 	ret = mutex_lock_interruptible(&dev->struct_mutex);
 	if (ret)
-		return ret;
+		goto out;
 
-	if (opregion->header)
-		seq_write(m, opregion->header, OPREGION_SIZE);
+	if (opregion->header) {
+		memcpy_fromio(data, opregion->header, OPREGION_SIZE);
+		seq_write(m, data, OPREGION_SIZE);
+	}
 
 	mutex_unlock(&dev->struct_mutex);
 
+out:
+	kfree(data);
 	return 0;
 }
 
@@ -1505,6 +1540,53 @@ static int i915_ppgtt_info(struct seq_file *m, void *data)
 	return 0;
 }
 
+static int i915_dpio_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 drm_i915_private *dev_priv = dev->dev_private;
+	int ret;
+
+
+	if (!IS_VALLEYVIEW(dev)) {
+		seq_printf(m, "unsupported\n");
+		return 0;
+	}
+
+	ret = mutex_lock_interruptible(&dev->mode_config.mutex);
+	if (ret)
+		return ret;
+
+	seq_printf(m, "DPIO_CTL: 0x%08x\n", I915_READ(DPIO_CTL));
+
+	seq_printf(m, "DPIO_DIV_A: 0x%08x\n",
+		   intel_dpio_read(dev_priv, _DPIO_DIV_A));
+	seq_printf(m, "DPIO_DIV_B: 0x%08x\n",
+		   intel_dpio_read(dev_priv, _DPIO_DIV_B));
+
+	seq_printf(m, "DPIO_REFSFR_A: 0x%08x\n",
+		   intel_dpio_read(dev_priv, _DPIO_REFSFR_A));
+	seq_printf(m, "DPIO_REFSFR_B: 0x%08x\n",
+		   intel_dpio_read(dev_priv, _DPIO_REFSFR_B));
+
+	seq_printf(m, "DPIO_CORE_CLK_A: 0x%08x\n",
+		   intel_dpio_read(dev_priv, _DPIO_CORE_CLK_A));
+	seq_printf(m, "DPIO_CORE_CLK_B: 0x%08x\n",
+		   intel_dpio_read(dev_priv, _DPIO_CORE_CLK_B));
+
+	seq_printf(m, "DPIO_LFP_COEFF_A: 0x%08x\n",
+		   intel_dpio_read(dev_priv, _DPIO_LFP_COEFF_A));
+	seq_printf(m, "DPIO_LFP_COEFF_B: 0x%08x\n",
+		   intel_dpio_read(dev_priv, _DPIO_LFP_COEFF_B));
+
+	seq_printf(m, "DPIO_FASTCLK_DISABLE: 0x%08x\n",
+		   intel_dpio_read(dev_priv, DPIO_FASTCLK_DISABLE));
+
+	mutex_unlock(&dev->mode_config.mutex);
+
+	return 0;
+}
+
 static ssize_t
 i915_wedged_read(struct file *filp,
 		 char __user *ubuf,
@@ -1562,6 +1644,65 @@ static const struct file_operations i915_wedged_fops = {
 };
 
 static ssize_t
+i915_ring_stop_read(struct file *filp,
+		    char __user *ubuf,
+		    size_t max,
+		    loff_t *ppos)
+{
+	struct drm_device *dev = filp->private_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->stop_rings);
+
+	if (len > sizeof(buf))
+		len = sizeof(buf);
+
+	return simple_read_from_buffer(ubuf, max, ppos, buf, len);
+}
+
+static ssize_t
+i915_ring_stop_write(struct file *filp,
+		     const char __user *ubuf,
+		     size_t cnt,
+		     loff_t *ppos)
+{
+	struct drm_device *dev = filp->private_data;
+	struct drm_i915_private *dev_priv = dev->dev_private;
+	char buf[20];
+	int val = 0;
+
+	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("Stopping rings 0x%08x\n", val);
+
+	mutex_lock(&dev->struct_mutex);
+	dev_priv->stop_rings = val;
+	mutex_unlock(&dev->struct_mutex);
+
+	return cnt;
+}
+
+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,
+};
+
+static ssize_t
 i915_max_freq_read(struct file *filp,
 		   char __user *ubuf,
 		   size_t max,
@@ -1738,7 +1879,7 @@ static int i915_forcewake_open(struct inode *inode, struct file *file)
 	return 0;
 }
 
-int i915_forcewake_release(struct inode *inode, struct file *file)
+static int i915_forcewake_release(struct inode *inode, struct file *file)
 {
 	struct drm_device *dev = inode->i_private;
 	struct drm_i915_private *dev_priv = dev->dev_private;
@@ -1803,11 +1944,10 @@ static struct drm_info_list i915_debugfs_list[] = {
 	{"i915_capabilities", i915_capabilities, 0},
 	{"i915_gem_objects", i915_gem_object_info, 0},
 	{"i915_gem_gtt", i915_gem_gtt_info, 0},
+	{"i915_gem_pinned", i915_gem_gtt_info, 0, (void *) PINNED_LIST},
 	{"i915_gem_active", i915_gem_object_list_info, 0, (void *) ACTIVE_LIST},
 	{"i915_gem_flushing", i915_gem_object_list_info, 0, (void *) FLUSHING_LIST},
 	{"i915_gem_inactive", i915_gem_object_list_info, 0, (void *) INACTIVE_LIST},
-	{"i915_gem_pinned", i915_gem_object_list_info, 0, (void *) PINNED_LIST},
-	{"i915_gem_deferred_free", i915_gem_object_list_info, 0, (void *) DEFERRED_FREE_LIST},
 	{"i915_gem_pageflip", i915_gem_pageflip_info, 0},
 	{"i915_gem_request", i915_gem_request_info, 0},
 	{"i915_gem_seqno", i915_gem_seqno_info, 0},
@@ -1816,13 +1956,6 @@ static struct drm_info_list i915_debugfs_list[] = {
 	{"i915_gem_hws", i915_hws_info, 0, (void *)RCS},
 	{"i915_gem_hws_blt", i915_hws_info, 0, (void *)BCS},
 	{"i915_gem_hws_bsd", i915_hws_info, 0, (void *)VCS},
-	{"i915_ringbuffer_data", i915_ringbuffer_data, 0, (void *)RCS},
-	{"i915_ringbuffer_info", i915_ringbuffer_info, 0, (void *)RCS},
-	{"i915_bsd_ringbuffer_data", i915_ringbuffer_data, 0, (void *)VCS},
-	{"i915_bsd_ringbuffer_info", i915_ringbuffer_info, 0, (void *)VCS},
-	{"i915_blt_ringbuffer_data", i915_ringbuffer_data, 0, (void *)BCS},
-	{"i915_blt_ringbuffer_info", i915_ringbuffer_info, 0, (void *)BCS},
-	{"i915_error_state", i915_error_state, 0},
 	{"i915_rstdby_delays", i915_rstdby_delays, 0},
 	{"i915_cur_delayinfo", i915_cur_delayinfo, 0},
 	{"i915_delayfreq_table", i915_delayfreq_table, 0},
@@ -1839,6 +1972,7 @@ static struct drm_info_list i915_debugfs_list[] = {
 	{"i915_gen6_forcewake_count", i915_gen6_forcewake_count_info, 0},
 	{"i915_swizzle_info", i915_swizzle_info, 0},
 	{"i915_ppgtt_info", i915_ppgtt_info, 0},
+	{"i915_dpio", i915_dpio_info, 0},
 };
 #define I915_DEBUGFS_ENTRIES ARRAY_SIZE(i915_debugfs_list)
 
@@ -1867,6 +2001,17 @@ int i915_debugfs_init(struct drm_minor *minor)
 				  &i915_cache_sharing_fops);
 	if (ret)
 		return ret;
+	ret = i915_debugfs_create(minor->debugfs_root, minor,
+				  "i915_ring_stop",
+				  &i915_ring_stop_fops);
+	if (ret)
+		return ret;
+
+	ret = i915_debugfs_create(minor->debugfs_root, minor,
+				  "i915_error_state",
+				  &i915_error_state_fops);
+	if (ret)
+		return ret;
 
 	return drm_debugfs_create_files(i915_debugfs_list,
 					I915_DEBUGFS_ENTRIES,
@@ -1885,6 +2030,8 @@ void i915_debugfs_cleanup(struct drm_minor *minor)
 				 1, minor);
 	drm_debugfs_remove_files((struct drm_info_list *) &i915_cache_sharing_fops,
 				 1, minor);
+	drm_debugfs_remove_files((struct drm_info_list *) &i915_ring_stop_fops,
+				 1, minor);
 }
 
 #endif /* CONFIG_DEBUG_FS */
diff --git a/drivers/gpu/drm/i915/i915_dma.c b/drivers/gpu/drm/i915/i915_dma.c
index ba60f3c8f91..f94792626b9 100644
--- a/drivers/gpu/drm/i915/i915_dma.c
+++ b/drivers/gpu/drm/i915/i915_dma.c
@@ -26,6 +26,8 @@
  *
  */
 
+#define pr_fmt(fmt) KBUILD_MODNAME ": " fmt
+
 #include "drmP.h"
 #include "drm.h"
 #include "drm_crtc_helper.h"
@@ -34,15 +36,62 @@
 #include "i915_drm.h"
 #include "i915_drv.h"
 #include "i915_trace.h"
-#include "../../../platform/x86/intel_ips.h"
 #include <linux/pci.h>
 #include <linux/vgaarb.h>
 #include <linux/acpi.h>
 #include <linux/pnp.h>
 #include <linux/vga_switcheroo.h>
 #include <linux/slab.h>
-#include <linux/module.h>
 #include <acpi/video.h>
+#include <asm/pat.h>
+
+#define LP_RING(d) (&((struct drm_i915_private *)(d))->ring[RCS])
+
+#define BEGIN_LP_RING(n) \
+	intel_ring_begin(LP_RING(dev_priv), (n))
+
+#define OUT_RING(x) \
+	intel_ring_emit(LP_RING(dev_priv), x)
+
+#define ADVANCE_LP_RING() \
+	intel_ring_advance(LP_RING(dev_priv))
+
+/**
+ * Lock test for when it's just for synchronization of ring access.
+ *
+ * In that case, we don't need to do it when GEM is initialized as nobody else
+ * has access to the ring.
+ */
+#define RING_LOCK_TEST_WITH_RETURN(dev, file) do {			\
+	if (LP_RING(dev->dev_private)->obj == NULL)			\
+		LOCK_TEST_WITH_RETURN(dev, file);			\
+} while (0)
+
+static inline u32
+intel_read_legacy_status_page(struct drm_i915_private *dev_priv, int reg)
+{
+	if (I915_NEED_GFX_HWS(dev_priv->dev))
+		return ioread32(dev_priv->dri1.gfx_hws_cpu_addr + reg);
+	else
+		return intel_read_status_page(LP_RING(dev_priv), reg);
+}
+
+#define READ_HWSP(dev_priv, reg) intel_read_legacy_status_page(dev_priv, reg)
+#define READ_BREADCRUMB(dev_priv) READ_HWSP(dev_priv, I915_BREADCRUMB_INDEX)
+#define I915_BREADCRUMB_INDEX		0x21
+
+void i915_update_dri1_breadcrumb(struct drm_device *dev)
+{
+	drm_i915_private_t *dev_priv = dev->dev_private;
+	struct drm_i915_master_private *master_priv;
+
+	if (dev->primary->master) {
+		master_priv = dev->primary->master->driver_priv;
+		if (master_priv->sarea_priv)
+			master_priv->sarea_priv->last_dispatch =
+				READ_BREADCRUMB(dev_priv);
+	}
+}
 
 static void i915_write_hws_pga(struct drm_device *dev)
 {
@@ -97,7 +146,7 @@ static void i915_free_hws(struct drm_device *dev)
 
 	if (ring->status_page.gfx_addr) {
 		ring->status_page.gfx_addr = 0;
-		drm_core_ioremapfree(&dev_priv->hws_map, dev);
+		iounmap(dev_priv->dri1.gfx_hws_cpu_addr);
 	}
 
 	/* Need to rewrite hardware status page */
@@ -195,7 +244,7 @@ static int i915_initialize(struct drm_device * dev, drm_i915_init_t * init)
 
 	/* Allow hardware batchbuffers unless told otherwise.
 	 */
-	dev_priv->allow_batchbuffer = 1;
+	dev_priv->dri1.allow_batchbuffer = 1;
 
 	return 0;
 }
@@ -207,7 +256,7 @@ static int i915_dma_resume(struct drm_device * dev)
 
 	DRM_DEBUG_DRIVER("%s\n", __func__);
 
-	if (ring->map.handle == NULL) {
+	if (ring->virtual_start == NULL) {
 		DRM_ERROR("can not ioremap virtual address for"
 			  " ring buffer\n");
 		return -ENOMEM;
@@ -236,6 +285,9 @@ static int i915_dma_init(struct drm_device *dev, void *data,
 	drm_i915_init_t *init = data;
 	int retcode = 0;
 
+	if (drm_core_check_feature(dev, DRIVER_MODESET))
+		return -ENODEV;
+
 	switch (init->func) {
 	case I915_INIT_DMA:
 		retcode = i915_initialize(dev, init);
@@ -578,6 +630,9 @@ static int i915_flush_ioctl(struct drm_device *dev, void *data,
 {
 	int ret;
 
+	if (drm_core_check_feature(dev, DRIVER_MODESET))
+		return -ENODEV;
+
 	RING_LOCK_TEST_WITH_RETURN(dev, file_priv);
 
 	mutex_lock(&dev->struct_mutex);
@@ -598,7 +653,10 @@ static int i915_batchbuffer(struct drm_device *dev, void *data,
 	int ret;
 	struct drm_clip_rect *cliprects = NULL;
 
-	if (!dev_priv->allow_batchbuffer) {
+	if (drm_core_check_feature(dev, DRIVER_MODESET))
+		return -ENODEV;
+
+	if (!dev_priv->dri1.allow_batchbuffer) {
 		DRM_ERROR("Batchbuffer ioctl disabled\n");
 		return -EINVAL;
 	}
@@ -655,6 +713,9 @@ static int i915_cmdbuffer(struct drm_device *dev, void *data,
 	DRM_DEBUG_DRIVER("i915 cmdbuffer, buf %p sz %d cliprects %d\n",
 			cmdbuf->buf, cmdbuf->sz, cmdbuf->num_cliprects);
 
+	if (drm_core_check_feature(dev, DRIVER_MODESET))
+		return -ENODEV;
+
 	RING_LOCK_TEST_WITH_RETURN(dev, file_priv);
 
 	if (cmdbuf->num_cliprects < 0)
@@ -706,11 +767,166 @@ fail_batch_free:
 	return ret;
 }
 
+static int i915_emit_irq(struct drm_device * dev)
+{
+	drm_i915_private_t *dev_priv = dev->dev_private;
+	struct drm_i915_master_private *master_priv = dev->primary->master->driver_priv;
+
+	i915_kernel_lost_context(dev);
+
+	DRM_DEBUG_DRIVER("\n");
+
+	dev_priv->counter++;
+	if (dev_priv->counter > 0x7FFFFFFFUL)
+		dev_priv->counter = 1;
+	if (master_priv->sarea_priv)
+		master_priv->sarea_priv->last_enqueue = dev_priv->counter;
+
+	if (BEGIN_LP_RING(4) == 0) {
+		OUT_RING(MI_STORE_DWORD_INDEX);
+		OUT_RING(I915_BREADCRUMB_INDEX << MI_STORE_DWORD_INDEX_SHIFT);
+		OUT_RING(dev_priv->counter);
+		OUT_RING(MI_USER_INTERRUPT);
+		ADVANCE_LP_RING();
+	}
+
+	return dev_priv->counter;
+}
+
+static int i915_wait_irq(struct drm_device * dev, int irq_nr)
+{
+	drm_i915_private_t *dev_priv = (drm_i915_private_t *) dev->dev_private;
+	struct drm_i915_master_private *master_priv = dev->primary->master->driver_priv;
+	int ret = 0;
+	struct intel_ring_buffer *ring = LP_RING(dev_priv);
+
+	DRM_DEBUG_DRIVER("irq_nr=%d breadcrumb=%d\n", irq_nr,
+		  READ_BREADCRUMB(dev_priv));
+
+	if (READ_BREADCRUMB(dev_priv) >= irq_nr) {
+		if (master_priv->sarea_priv)
+			master_priv->sarea_priv->last_dispatch = READ_BREADCRUMB(dev_priv);
+		return 0;
+	}
+
+	if (master_priv->sarea_priv)
+		master_priv->sarea_priv->perf_boxes |= I915_BOX_WAIT;
+
+	if (ring->irq_get(ring)) {
+		DRM_WAIT_ON(ret, ring->irq_queue, 3 * DRM_HZ,
+			    READ_BREADCRUMB(dev_priv) >= irq_nr);
+		ring->irq_put(ring);
+	} else if (wait_for(READ_BREADCRUMB(dev_priv) >= irq_nr, 3000))
+		ret = -EBUSY;
+
+	if (ret == -EBUSY) {
+		DRM_ERROR("EBUSY -- rec: %d emitted: %d\n",
+			  READ_BREADCRUMB(dev_priv), (int)dev_priv->counter);
+	}
+
+	return ret;
+}
+
+/* Needs the lock as it touches the ring.
+ */
+static int i915_irq_emit(struct drm_device *dev, void *data,
+			 struct drm_file *file_priv)
+{
+	drm_i915_private_t *dev_priv = dev->dev_private;
+	drm_i915_irq_emit_t *emit = data;
+	int result;
+
+	if (drm_core_check_feature(dev, DRIVER_MODESET))
+		return -ENODEV;
+
+	if (!dev_priv || !LP_RING(dev_priv)->virtual_start) {
+		DRM_ERROR("called with no initialization\n");
+		return -EINVAL;
+	}
+
+	RING_LOCK_TEST_WITH_RETURN(dev, file_priv);
+
+	mutex_lock(&dev->struct_mutex);
+	result = i915_emit_irq(dev);
+	mutex_unlock(&dev->struct_mutex);
+
+	if (DRM_COPY_TO_USER(emit->irq_seq, &result, sizeof(int))) {
+		DRM_ERROR("copy_to_user\n");
+		return -EFAULT;
+	}
+
+	return 0;
+}
+
+/* Doesn't need the hardware lock.
+ */
+static int i915_irq_wait(struct drm_device *dev, void *data,
+			 struct drm_file *file_priv)
+{
+	drm_i915_private_t *dev_priv = dev->dev_private;
+	drm_i915_irq_wait_t *irqwait = data;
+
+	if (drm_core_check_feature(dev, DRIVER_MODESET))
+		return -ENODEV;
+
+	if (!dev_priv) {
+		DRM_ERROR("called with no initialization\n");
+		return -EINVAL;
+	}
+
+	return i915_wait_irq(dev, irqwait->irq_seq);
+}
+
+static int i915_vblank_pipe_get(struct drm_device *dev, void *data,
+			 struct drm_file *file_priv)
+{
+	drm_i915_private_t *dev_priv = dev->dev_private;
+	drm_i915_vblank_pipe_t *pipe = data;
+
+	if (drm_core_check_feature(dev, DRIVER_MODESET))
+		return -ENODEV;
+
+	if (!dev_priv) {
+		DRM_ERROR("called with no initialization\n");
+		return -EINVAL;
+	}
+
+	pipe->pipe = DRM_I915_VBLANK_PIPE_A | DRM_I915_VBLANK_PIPE_B;
+
+	return 0;
+}
+
+/**
+ * Schedule buffer swap at given vertical blank.
+ */
+static int i915_vblank_swap(struct drm_device *dev, void *data,
+		     struct drm_file *file_priv)
+{
+	/* The delayed swap mechanism was fundamentally racy, and has been
+	 * removed.  The model was that the client requested a delayed flip/swap
+	 * from the kernel, then waited for vblank before continuing to perform
+	 * rendering.  The problem was that the kernel might wake the client
+	 * up before it dispatched the vblank swap (since the lock has to be
+	 * held while touching the ringbuffer), in which case the client would
+	 * clear and start the next frame before the swap occurred, and
+	 * flicker would occur in addition to likely missing the vblank.
+	 *
+	 * In the absence of this ioctl, userland falls back to a correct path
+	 * of waiting for a vblank, then dispatching the swap on its own.
+	 * Context switching to userland and back is plenty fast enough for
+	 * meeting the requirements of vblank swapping.
+	 */
+	return -EINVAL;
+}
+
 static int i915_flip_bufs(struct drm_device *dev, void *data,
 			  struct drm_file *file_priv)
 {
 	int ret;
 
+	if (drm_core_check_feature(dev, DRIVER_MODESET))
+		return -ENODEV;
+
 	DRM_DEBUG_DRIVER("%s\n", __func__);
 
 	RING_LOCK_TEST_WITH_RETURN(dev, file_priv);
@@ -739,7 +955,7 @@ static int i915_getparam(struct drm_device *dev, void *data,
 		value = dev->pdev->irq ? 1 : 0;
 		break;
 	case I915_PARAM_ALLOW_BATCHBUFFER:
-		value = dev_priv->allow_batchbuffer ? 1 : 0;
+		value = dev_priv->dri1.allow_batchbuffer ? 1 : 0;
 		break;
 	case I915_PARAM_LAST_DISPATCH:
 		value = READ_BREADCRUMB(dev_priv);
@@ -748,7 +964,7 @@ static int i915_getparam(struct drm_device *dev, void *data,
 		value = dev->pci_device;
 		break;
 	case I915_PARAM_HAS_GEM:
-		value = dev_priv->has_gem;
+		value = 1;
 		break;
 	case I915_PARAM_NUM_FENCES_AVAIL:
 		value = dev_priv->num_fence_regs - dev_priv->fence_reg_start;
@@ -761,13 +977,13 @@ static int i915_getparam(struct drm_device *dev, void *data,
 		break;
 	case I915_PARAM_HAS_EXECBUF2:
 		/* depends on GEM */
-		value = dev_priv->has_gem;
+		value = 1;
 		break;
 	case I915_PARAM_HAS_BSD:
-		value = HAS_BSD(dev);
+		value = intel_ring_initialized(&dev_priv->ring[VCS]);
 		break;
 	case I915_PARAM_HAS_BLT:
-		value = HAS_BLT(dev);
+		value = intel_ring_initialized(&dev_priv->ring[BCS]);
 		break;
 	case I915_PARAM_HAS_RELAXED_FENCING:
 		value = 1;
@@ -787,6 +1003,9 @@ static int i915_getparam(struct drm_device *dev, void *data,
 	case I915_PARAM_HAS_LLC:
 		value = HAS_LLC(dev);
 		break;
+	case I915_PARAM_HAS_ALIASING_PPGTT:
+		value = dev_priv->mm.aliasing_ppgtt ? 1 : 0;
+		break;
 	default:
 		DRM_DEBUG_DRIVER("Unknown parameter %d\n",
 				 param->param);
@@ -816,10 +1035,9 @@ static int i915_setparam(struct drm_device *dev, void *data,
 	case I915_SETPARAM_USE_MI_BATCHBUFFER_START:
 		break;
 	case I915_SETPARAM_TEX_LRU_LOG_GRANULARITY:
-		dev_priv->tex_lru_log_granularity = param->value;
 		break;
 	case I915_SETPARAM_ALLOW_BATCHBUFFER:
-		dev_priv->allow_batchbuffer = param->value;
+		dev_priv->dri1.allow_batchbuffer = param->value ? 1 : 0;
 		break;
 	case I915_SETPARAM_NUM_USED_FENCES:
 		if (param->value > dev_priv->num_fence_regs ||
@@ -844,6 +1062,9 @@ static int i915_set_status_page(struct drm_device *dev, void *data,
 	drm_i915_hws_addr_t *hws = data;
 	struct intel_ring_buffer *ring = LP_RING(dev_priv);
 
+	if (drm_core_check_feature(dev, DRIVER_MODESET))
+		return -ENODEV;
+
 	if (!I915_NEED_GFX_HWS(dev))
 		return -EINVAL;
 
@@ -861,23 +1082,17 @@ static int i915_set_status_page(struct drm_device *dev, void *data,
 
 	ring->status_page.gfx_addr = hws->addr & (0x1ffff<<12);
 
-	dev_priv->hws_map.offset = dev->agp->base + hws->addr;
-	dev_priv->hws_map.size = 4*1024;
-	dev_priv->hws_map.type = 0;
-	dev_priv->hws_map.flags = 0;
-	dev_priv->hws_map.mtrr = 0;
-
-	drm_core_ioremap_wc(&dev_priv->hws_map, dev);
-	if (dev_priv->hws_map.handle == NULL) {
+	dev_priv->dri1.gfx_hws_cpu_addr = ioremap_wc(dev->agp->base + hws->addr,
+						     4096);
+	if (dev_priv->dri1.gfx_hws_cpu_addr == NULL) {
 		i915_dma_cleanup(dev);
 		ring->status_page.gfx_addr = 0;
 		DRM_ERROR("can not ioremap virtual address for"
 				" G33 hw status page\n");
 		return -ENOMEM;
 	}
-	ring->status_page.page_addr =
-		(void __force __iomem *)dev_priv->hws_map.handle;
-	memset_io(ring->status_page.page_addr, 0, PAGE_SIZE);
+
+	memset_io(dev_priv->dri1.gfx_hws_cpu_addr, 0, PAGE_SIZE);
 	I915_WRITE(HWS_PGA, ring->status_page.gfx_addr);
 
 	DRM_DEBUG_DRIVER("load hws HWS_PGA with gfx mem 0x%x\n",
@@ -1013,133 +1228,6 @@ intel_teardown_mchbar(struct drm_device *dev)
 		release_resource(&dev_priv->mch_res);
 }
 
-#define PTE_ADDRESS_MASK		0xfffff000
-#define PTE_ADDRESS_MASK_HIGH		0x000000f0 /* i915+ */
-#define PTE_MAPPING_TYPE_UNCACHED	(0 << 1)
-#define PTE_MAPPING_TYPE_DCACHE		(1 << 1) /* i830 only */
-#define PTE_MAPPING_TYPE_CACHED		(3 << 1)
-#define PTE_MAPPING_TYPE_MASK		(3 << 1)
-#define PTE_VALID			(1 << 0)
-
-/**
- * i915_stolen_to_phys - take an offset into stolen memory and turn it into
- *                       a physical one
- * @dev: drm device
- * @offset: address to translate
- *
- * Some chip functions require allocations from stolen space and need the
- * physical address of the memory in question.
- */
-static unsigned long i915_stolen_to_phys(struct drm_device *dev, u32 offset)
-{
-	struct drm_i915_private *dev_priv = dev->dev_private;
-	struct pci_dev *pdev = dev_priv->bridge_dev;
-	u32 base;
-
-#if 0
-	/* On the machines I have tested the Graphics Base of Stolen Memory
-	 * is unreliable, so compute the base by subtracting the stolen memory
-	 * from the Top of Low Usable DRAM which is where the BIOS places
-	 * the graphics stolen memory.
-	 */
-	if (INTEL_INFO(dev)->gen > 3 || IS_G33(dev)) {
-		/* top 32bits are reserved = 0 */
-		pci_read_config_dword(pdev, 0xA4, &base);
-	} else {
-		/* XXX presume 8xx is the same as i915 */
-		pci_bus_read_config_dword(pdev->bus, 2, 0x5C, &base);
-	}
-#else
-	if (INTEL_INFO(dev)->gen > 3 || IS_G33(dev)) {
-		u16 val;
-		pci_read_config_word(pdev, 0xb0, &val);
-		base = val >> 4 << 20;
-	} else {
-		u8 val;
-		pci_read_config_byte(pdev, 0x9c, &val);
-		base = val >> 3 << 27;
-	}
-	base -= dev_priv->mm.gtt->stolen_size;
-#endif
-
-	return base + offset;
-}
-
-static void i915_warn_stolen(struct drm_device *dev)
-{
-	DRM_ERROR("not enough stolen space for compressed buffer, disabling\n");
-	DRM_ERROR("hint: you may be able to increase stolen memory size in the BIOS to avoid this\n");
-}
-
-static void i915_setup_compression(struct drm_device *dev, int size)
-{
-	struct drm_i915_private *dev_priv = dev->dev_private;
-	struct drm_mm_node *compressed_fb, *uninitialized_var(compressed_llb);
-	unsigned long cfb_base;
-	unsigned long ll_base = 0;
-
-	/* Just in case the BIOS is doing something questionable. */
-	intel_disable_fbc(dev);
-
-	compressed_fb = drm_mm_search_free(&dev_priv->mm.stolen, size, 4096, 0);
-	if (compressed_fb)
-		compressed_fb = drm_mm_get_block(compressed_fb, size, 4096);
-	if (!compressed_fb)
-		goto err;
-
-	cfb_base = i915_stolen_to_phys(dev, compressed_fb->start);
-	if (!cfb_base)
-		goto err_fb;
-
-	if (!(IS_GM45(dev) || HAS_PCH_SPLIT(dev))) {
-		compressed_llb = drm_mm_search_free(&dev_priv->mm.stolen,
-						    4096, 4096, 0);
-		if (compressed_llb)
-			compressed_llb = drm_mm_get_block(compressed_llb,
-							  4096, 4096);
-		if (!compressed_llb)
-			goto err_fb;
-
-		ll_base = i915_stolen_to_phys(dev, compressed_llb->start);
-		if (!ll_base)
-			goto err_llb;
-	}
-
-	dev_priv->cfb_size = size;
-
-	dev_priv->compressed_fb = compressed_fb;
-	if (HAS_PCH_SPLIT(dev))
-		I915_WRITE(ILK_DPFC_CB_BASE, compressed_fb->start);
-	else if (IS_GM45(dev)) {
-		I915_WRITE(DPFC_CB_BASE, compressed_fb->start);
-	} else {
-		I915_WRITE(FBC_CFB_BASE, cfb_base);
-		I915_WRITE(FBC_LL_BASE, ll_base);
-		dev_priv->compressed_llb = compressed_llb;
-	}
-
-	DRM_DEBUG_KMS("FBC base 0x%08lx, ll base 0x%08lx, size %dM\n",
-		      cfb_base, ll_base, size >> 20);
-	return;
-
-err_llb:
-	drm_mm_put_block(compressed_llb);
-err_fb:
-	drm_mm_put_block(compressed_fb);
-err:
-	dev_priv->no_fbc_reason = FBC_STOLEN_TOO_SMALL;
-	i915_warn_stolen(dev);
-}
-
-static void i915_cleanup_compression(struct drm_device *dev)
-{
-	struct drm_i915_private *dev_priv = dev->dev_private;
-
-	drm_mm_put_block(dev_priv->compressed_fb);
-	if (dev_priv->compressed_llb)
-		drm_mm_put_block(dev_priv->compressed_llb);
-}
-
 /* true = enable decode, false = disable decoder */
 static unsigned int i915_vga_set_decode(void *cookie, bool state)
 {
@@ -1158,14 +1246,14 @@ static void i915_switcheroo_set_state(struct pci_dev *pdev, enum vga_switcheroo_
 	struct drm_device *dev = pci_get_drvdata(pdev);
 	pm_message_t pmm = { .event = PM_EVENT_SUSPEND };
 	if (state == VGA_SWITCHEROO_ON) {
-		printk(KERN_INFO "i915: switched on\n");
+		pr_info("switched on\n");
 		dev->switch_power_state = DRM_SWITCH_POWER_CHANGING;
 		/* i915 resume handler doesn't set to D0 */
 		pci_set_power_state(dev->pdev, PCI_D0);
 		i915_resume(dev);
 		dev->switch_power_state = DRM_SWITCH_POWER_ON;
 	} else {
-		printk(KERN_ERR "i915: switched off\n");
+		pr_err("switched off\n");
 		dev->switch_power_state = DRM_SWITCH_POWER_CHANGING;
 		i915_suspend(dev, pmm);
 		dev->switch_power_state = DRM_SWITCH_POWER_OFF;
@@ -1183,88 +1271,11 @@ static bool i915_switcheroo_can_switch(struct pci_dev *pdev)
 	return can_switch;
 }
 
-static bool
-intel_enable_ppgtt(struct drm_device *dev)
-{
-	if (i915_enable_ppgtt >= 0)
-		return i915_enable_ppgtt;
-
-#ifdef CONFIG_INTEL_IOMMU
-	/* Disable ppgtt on SNB if VT-d is on. */
-	if (INTEL_INFO(dev)->gen == 6 && intel_iommu_gfx_mapped)
-		return false;
-#endif
-
-	return true;
-}
-
-static int i915_load_gem_init(struct drm_device *dev)
-{
-	struct drm_i915_private *dev_priv = dev->dev_private;
-	unsigned long prealloc_size, gtt_size, mappable_size;
-	int ret;
-
-	prealloc_size = dev_priv->mm.gtt->stolen_size;
-	gtt_size = dev_priv->mm.gtt->gtt_total_entries << PAGE_SHIFT;
-	mappable_size = dev_priv->mm.gtt->gtt_mappable_entries << PAGE_SHIFT;
-
-	/* Basic memrange allocator for stolen space */
-	drm_mm_init(&dev_priv->mm.stolen, 0, prealloc_size);
-
-	mutex_lock(&dev->struct_mutex);
-	if (intel_enable_ppgtt(dev) && HAS_ALIASING_PPGTT(dev)) {
-		/* 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;
-		/* For paranoia keep the guard page in between. */
-		gtt_size -= PAGE_SIZE;
-
-		i915_gem_do_init(dev, 0, mappable_size, gtt_size);
-
-		ret = i915_gem_init_aliasing_ppgtt(dev);
-		if (ret) {
-			mutex_unlock(&dev->struct_mutex);
-			return ret;
-		}
-	} else {
-		/* Let GEM Manage all of the aperture.
-		 *
-		 * However, leave one page at the end still bound to the scratch
-		 * page.  There are a number of places where the hardware
-		 * apparently prefetches past the end of the object, and we've
-		 * seen multiple hangs with the GPU head pointer stuck in a
-		 * batchbuffer bound at the last page of the aperture.  One page
-		 * should be enough to keep any prefetching inside of the
-		 * aperture.
-		 */
-		i915_gem_do_init(dev, 0, mappable_size, gtt_size - PAGE_SIZE);
-	}
-
-	ret = i915_gem_init_hw(dev);
-	mutex_unlock(&dev->struct_mutex);
-	if (ret) {
-		i915_gem_cleanup_aliasing_ppgtt(dev);
-		return ret;
-	}
-
-	/* Try to set up FBC with a reasonable compressed buffer size */
-	if (I915_HAS_FBC(dev) && i915_powersave) {
-		int cfb_size;
-
-		/* Leave 1M for line length buffer & misc. */
-
-		/* Try to get a 32M buffer... */
-		if (prealloc_size > (36*1024*1024))
-			cfb_size = 32*1024*1024;
-		else /* fall back to 7/8 of the stolen space */
-			cfb_size = prealloc_size * 7 / 8;
-		i915_setup_compression(dev, cfb_size);
-	}
-
-	/* Allow hardware batchbuffers unless told otherwise. */
-	dev_priv->allow_batchbuffer = 1;
-	return 0;
-}
+static const struct vga_switcheroo_client_ops i915_switcheroo_ops = {
+	.set_gpu_state = i915_switcheroo_set_state,
+	.reprobe = NULL,
+	.can_switch = i915_switcheroo_can_switch,
+};
 
 static int i915_load_modeset_init(struct drm_device *dev)
 {
@@ -1288,22 +1299,22 @@ static int i915_load_modeset_init(struct drm_device *dev)
 
 	intel_register_dsm_handler();
 
-	ret = vga_switcheroo_register_client(dev->pdev,
-					     i915_switcheroo_set_state,
-					     NULL,
-					     i915_switcheroo_can_switch);
+	ret = vga_switcheroo_register_client(dev->pdev, &i915_switcheroo_ops);
 	if (ret)
 		goto cleanup_vga_client;
 
-	/* IIR "flip pending" bit means done if this bit is set */
-	if (IS_GEN3(dev) && (I915_READ(ECOSKPD) & ECO_FLIP_DONE))
-		dev_priv->flip_pending_is_done = true;
+	/* Initialise stolen first so that we may reserve preallocated
+	 * objects for the BIOS to KMS transition.
+	 */
+	ret = i915_gem_init_stolen(dev);
+	if (ret)
+		goto cleanup_vga_switcheroo;
 
 	intel_modeset_init(dev);
 
-	ret = i915_load_gem_init(dev);
+	ret = i915_gem_init(dev);
 	if (ret)
-		goto cleanup_vga_switcheroo;
+		goto cleanup_gem_stolen;
 
 	intel_modeset_gem_init(dev);
 
@@ -1333,6 +1344,8 @@ cleanup_gem:
 	i915_gem_cleanup_ringbuffer(dev);
 	mutex_unlock(&dev->struct_mutex);
 	i915_gem_cleanup_aliasing_ppgtt(dev);
+cleanup_gem_stolen:
+	i915_gem_cleanup_stolen(dev);
 cleanup_vga_switcheroo:
 	vga_switcheroo_unregister_client(dev->pdev);
 cleanup_vga_client:
@@ -1365,572 +1378,26 @@ void i915_master_destroy(struct drm_device *dev, struct drm_master *master)
 	master->driver_priv = NULL;
 }
 
-static void i915_pineview_get_mem_freq(struct drm_device *dev)
-{
-	drm_i915_private_t *dev_priv = dev->dev_private;
-	u32 tmp;
-
-	tmp = I915_READ(CLKCFG);
-
-	switch (tmp & CLKCFG_FSB_MASK) {
-	case CLKCFG_FSB_533:
-		dev_priv->fsb_freq = 533; /* 133*4 */
-		break;
-	case CLKCFG_FSB_800:
-		dev_priv->fsb_freq = 800; /* 200*4 */
-		break;
-	case CLKCFG_FSB_667:
-		dev_priv->fsb_freq =  667; /* 167*4 */
-		break;
-	case CLKCFG_FSB_400:
-		dev_priv->fsb_freq = 400; /* 100*4 */
-		break;
-	}
-
-	switch (tmp & CLKCFG_MEM_MASK) {
-	case CLKCFG_MEM_533:
-		dev_priv->mem_freq = 533;
-		break;
-	case CLKCFG_MEM_667:
-		dev_priv->mem_freq = 667;
-		break;
-	case CLKCFG_MEM_800:
-		dev_priv->mem_freq = 800;
-		break;
-	}
-
-	/* detect pineview DDR3 setting */
-	tmp = I915_READ(CSHRDDR3CTL);
-	dev_priv->is_ddr3 = (tmp & CSHRDDR3CTL_DDR3) ? 1 : 0;
-}
-
-static void i915_ironlake_get_mem_freq(struct drm_device *dev)
-{
-	drm_i915_private_t *dev_priv = dev->dev_private;
-	u16 ddrpll, csipll;
-
-	ddrpll = I915_READ16(DDRMPLL1);
-	csipll = I915_READ16(CSIPLL0);
-
-	switch (ddrpll & 0xff) {
-	case 0xc:
-		dev_priv->mem_freq = 800;
-		break;
-	case 0x10:
-		dev_priv->mem_freq = 1066;
-		break;
-	case 0x14:
-		dev_priv->mem_freq = 1333;
-		break;
-	case 0x18:
-		dev_priv->mem_freq = 1600;
-		break;
-	default:
-		DRM_DEBUG_DRIVER("unknown memory frequency 0x%02x\n",
-				 ddrpll & 0xff);
-		dev_priv->mem_freq = 0;
-		break;
-	}
-
-	dev_priv->r_t = dev_priv->mem_freq;
-
-	switch (csipll & 0x3ff) {
-	case 0x00c:
-		dev_priv->fsb_freq = 3200;
-		break;
-	case 0x00e:
-		dev_priv->fsb_freq = 3733;
-		break;
-	case 0x010:
-		dev_priv->fsb_freq = 4266;
-		break;
-	case 0x012:
-		dev_priv->fsb_freq = 4800;
-		break;
-	case 0x014:
-		dev_priv->fsb_freq = 5333;
-		break;
-	case 0x016:
-		dev_priv->fsb_freq = 5866;
-		break;
-	case 0x018:
-		dev_priv->fsb_freq = 6400;
-		break;
-	default:
-		DRM_DEBUG_DRIVER("unknown fsb frequency 0x%04x\n",
-				 csipll & 0x3ff);
-		dev_priv->fsb_freq = 0;
-		break;
-	}
-
-	if (dev_priv->fsb_freq == 3200) {
-		dev_priv->c_m = 0;
-	} else if (dev_priv->fsb_freq > 3200 && dev_priv->fsb_freq <= 4800) {
-		dev_priv->c_m = 1;
-	} else {
-		dev_priv->c_m = 2;
-	}
-}
-
-static const struct cparams {
-	u16 i;
-	u16 t;
-	u16 m;
-	u16 c;
-} cparams[] = {
-	{ 1, 1333, 301, 28664 },
-	{ 1, 1066, 294, 24460 },
-	{ 1, 800, 294, 25192 },
-	{ 0, 1333, 276, 27605 },
-	{ 0, 1066, 276, 27605 },
-	{ 0, 800, 231, 23784 },
-};
-
-unsigned long i915_chipset_val(struct drm_i915_private *dev_priv)
-{
-	u64 total_count, diff, ret;
-	u32 count1, count2, count3, m = 0, c = 0;
-	unsigned long now = jiffies_to_msecs(jiffies), diff1;
-	int i;
-
-	diff1 = now - dev_priv->last_time1;
-
-	/* Prevent division-by-zero if we are asking too fast.
-	 * Also, we don't get interesting results if we are polling
-	 * faster than once in 10ms, so just return the saved value
-	 * in such cases.
-	 */
-	if (diff1 <= 10)
-		return dev_priv->chipset_power;
-
-	count1 = I915_READ(DMIEC);
-	count2 = I915_READ(DDREC);
-	count3 = I915_READ(CSIEC);
-
-	total_count = count1 + count2 + count3;
-
-	/* FIXME: handle per-counter overflow */
-	if (total_count < dev_priv->last_count1) {
-		diff = ~0UL - dev_priv->last_count1;
-		diff += total_count;
-	} else {
-		diff = total_count - dev_priv->last_count1;
-	}
-
-	for (i = 0; i < ARRAY_SIZE(cparams); i++) {
-		if (cparams[i].i == dev_priv->c_m &&
-		    cparams[i].t == dev_priv->r_t) {
-			m = cparams[i].m;
-			c = cparams[i].c;
-			break;
-		}
-	}
-
-	diff = div_u64(diff, diff1);
-	ret = ((m * diff) + c);
-	ret = div_u64(ret, 10);
-
-	dev_priv->last_count1 = total_count;
-	dev_priv->last_time1 = now;
-
-	dev_priv->chipset_power = ret;
-
-	return ret;
-}
-
-unsigned long i915_mch_val(struct drm_i915_private *dev_priv)
-{
-	unsigned long m, x, b;
-	u32 tsfs;
-
-	tsfs = I915_READ(TSFS);
-
-	m = ((tsfs & TSFS_SLOPE_MASK) >> TSFS_SLOPE_SHIFT);
-	x = I915_READ8(TR1);
-
-	b = tsfs & TSFS_INTR_MASK;
-
-	return ((m * x) / 127) - b;
-}
-
-static u16 pvid_to_extvid(struct drm_i915_private *dev_priv, u8 pxvid)
-{
-	static const struct v_table {
-		u16 vd; /* in .1 mil */
-		u16 vm; /* in .1 mil */
-	} v_table[] = {
-		{ 0, 0, },
-		{ 375, 0, },
-		{ 500, 0, },
-		{ 625, 0, },
-		{ 750, 0, },
-		{ 875, 0, },
-		{ 1000, 0, },
-		{ 1125, 0, },
-		{ 4125, 3000, },
-		{ 4125, 3000, },
-		{ 4125, 3000, },
-		{ 4125, 3000, },
-		{ 4125, 3000, },
-		{ 4125, 3000, },
-		{ 4125, 3000, },
-		{ 4125, 3000, },
-		{ 4125, 3000, },
-		{ 4125, 3000, },
-		{ 4125, 3000, },
-		{ 4125, 3000, },
-		{ 4125, 3000, },
-		{ 4125, 3000, },
-		{ 4125, 3000, },
-		{ 4125, 3000, },
-		{ 4125, 3000, },
-		{ 4125, 3000, },
-		{ 4125, 3000, },
-		{ 4125, 3000, },
-		{ 4125, 3000, },
-		{ 4125, 3000, },
-		{ 4125, 3000, },
-		{ 4125, 3000, },
-		{ 4250, 3125, },
-		{ 4375, 3250, },
-		{ 4500, 3375, },
-		{ 4625, 3500, },
-		{ 4750, 3625, },
-		{ 4875, 3750, },
-		{ 5000, 3875, },
-		{ 5125, 4000, },
-		{ 5250, 4125, },
-		{ 5375, 4250, },
-		{ 5500, 4375, },
-		{ 5625, 4500, },
-		{ 5750, 4625, },
-		{ 5875, 4750, },
-		{ 6000, 4875, },
-		{ 6125, 5000, },
-		{ 6250, 5125, },
-		{ 6375, 5250, },
-		{ 6500, 5375, },
-		{ 6625, 5500, },
-		{ 6750, 5625, },
-		{ 6875, 5750, },
-		{ 7000, 5875, },
-		{ 7125, 6000, },
-		{ 7250, 6125, },
-		{ 7375, 6250, },
-		{ 7500, 6375, },
-		{ 7625, 6500, },
-		{ 7750, 6625, },
-		{ 7875, 6750, },
-		{ 8000, 6875, },
-		{ 8125, 7000, },
-		{ 8250, 7125, },
-		{ 8375, 7250, },
-		{ 8500, 7375, },
-		{ 8625, 7500, },
-		{ 8750, 7625, },
-		{ 8875, 7750, },
-		{ 9000, 7875, },
-		{ 9125, 8000, },
-		{ 9250, 8125, },
-		{ 9375, 8250, },
-		{ 9500, 8375, },
-		{ 9625, 8500, },
-		{ 9750, 8625, },
-		{ 9875, 8750, },
-		{ 10000, 8875, },
-		{ 10125, 9000, },
-		{ 10250, 9125, },
-		{ 10375, 9250, },
-		{ 10500, 9375, },
-		{ 10625, 9500, },
-		{ 10750, 9625, },
-		{ 10875, 9750, },
-		{ 11000, 9875, },
-		{ 11125, 10000, },
-		{ 11250, 10125, },
-		{ 11375, 10250, },
-		{ 11500, 10375, },
-		{ 11625, 10500, },
-		{ 11750, 10625, },
-		{ 11875, 10750, },
-		{ 12000, 10875, },
-		{ 12125, 11000, },
-		{ 12250, 11125, },
-		{ 12375, 11250, },
-		{ 12500, 11375, },
-		{ 12625, 11500, },
-		{ 12750, 11625, },
-		{ 12875, 11750, },
-		{ 13000, 11875, },
-		{ 13125, 12000, },
-		{ 13250, 12125, },
-		{ 13375, 12250, },
-		{ 13500, 12375, },
-		{ 13625, 12500, },
-		{ 13750, 12625, },
-		{ 13875, 12750, },
-		{ 14000, 12875, },
-		{ 14125, 13000, },
-		{ 14250, 13125, },
-		{ 14375, 13250, },
-		{ 14500, 13375, },
-		{ 14625, 13500, },
-		{ 14750, 13625, },
-		{ 14875, 13750, },
-		{ 15000, 13875, },
-		{ 15125, 14000, },
-		{ 15250, 14125, },
-		{ 15375, 14250, },
-		{ 15500, 14375, },
-		{ 15625, 14500, },
-		{ 15750, 14625, },
-		{ 15875, 14750, },
-		{ 16000, 14875, },
-		{ 16125, 15000, },
-	};
-	if (dev_priv->info->is_mobile)
-		return v_table[pxvid].vm;
-	else
-		return v_table[pxvid].vd;
-}
-
-void i915_update_gfx_val(struct drm_i915_private *dev_priv)
+static void
+i915_mtrr_setup(struct drm_i915_private *dev_priv, unsigned long base,
+		unsigned long size)
 {
-	struct timespec now, diff1;
-	u64 diff;
-	unsigned long diffms;
-	u32 count;
-
-	if (dev_priv->info->gen != 5)
-		return;
-
-	getrawmonotonic(&now);
-	diff1 = timespec_sub(now, dev_priv->last_time2);
+	dev_priv->mm.gtt_mtrr = -1;
 
-	/* Don't divide by 0 */
-	diffms = diff1.tv_sec * 1000 + diff1.tv_nsec / 1000000;
-	if (!diffms)
+#if defined(CONFIG_X86_PAT)
+	if (cpu_has_pat)
 		return;
+#endif
 
-	count = I915_READ(GFXEC);
-
-	if (count < dev_priv->last_count2) {
-		diff = ~0UL - dev_priv->last_count2;
-		diff += count;
-	} else {
-		diff = count - dev_priv->last_count2;
-	}
-
-	dev_priv->last_count2 = count;
-	dev_priv->last_time2 = now;
-
-	/* More magic constants... */
-	diff = diff * 1181;
-	diff = div_u64(diff, diffms * 10);
-	dev_priv->gfx_power = diff;
-}
-
-unsigned long i915_gfx_val(struct drm_i915_private *dev_priv)
-{
-	unsigned long t, corr, state1, corr2, state2;
-	u32 pxvid, ext_v;
-
-	pxvid = I915_READ(PXVFREQ_BASE + (dev_priv->cur_delay * 4));
-	pxvid = (pxvid >> 24) & 0x7f;
-	ext_v = pvid_to_extvid(dev_priv, pxvid);
-
-	state1 = ext_v;
-
-	t = i915_mch_val(dev_priv);
-
-	/* Revel in the empirically derived constants */
-
-	/* Correction factor in 1/100000 units */
-	if (t > 80)
-		corr = ((t * 2349) + 135940);
-	else if (t >= 50)
-		corr = ((t * 964) + 29317);
-	else /* < 50 */
-		corr = ((t * 301) + 1004);
-
-	corr = corr * ((150142 * state1) / 10000 - 78642);
-	corr /= 100000;
-	corr2 = (corr * dev_priv->corr);
-
-	state2 = (corr2 * state1) / 10000;
-	state2 /= 100; /* convert to mW */
-
-	i915_update_gfx_val(dev_priv);
-
-	return dev_priv->gfx_power + state2;
-}
-
-/* Global for IPS driver to get at the current i915 device */
-static struct drm_i915_private *i915_mch_dev;
-/*
- * Lock protecting IPS related data structures
- *   - i915_mch_dev
- *   - dev_priv->max_delay
- *   - dev_priv->min_delay
- *   - dev_priv->fmax
- *   - dev_priv->gpu_busy
- */
-static DEFINE_SPINLOCK(mchdev_lock);
-
-/**
- * i915_read_mch_val - return value for IPS use
- *
- * Calculate and return a value for the IPS driver to use when deciding whether
- * we have thermal and power headroom to increase CPU or GPU power budget.
- */
-unsigned long i915_read_mch_val(void)
-{
-	struct drm_i915_private *dev_priv;
-	unsigned long chipset_val, graphics_val, ret = 0;
-
-	spin_lock(&mchdev_lock);
-	if (!i915_mch_dev)
-		goto out_unlock;
-	dev_priv = i915_mch_dev;
-
-	chipset_val = i915_chipset_val(dev_priv);
-	graphics_val = i915_gfx_val(dev_priv);
-
-	ret = chipset_val + graphics_val;
-
-out_unlock:
-	spin_unlock(&mchdev_lock);
-
-	return ret;
-}
-EXPORT_SYMBOL_GPL(i915_read_mch_val);
-
-/**
- * i915_gpu_raise - raise GPU frequency limit
- *
- * Raise the limit; IPS indicates we have thermal headroom.
- */
-bool i915_gpu_raise(void)
-{
-	struct drm_i915_private *dev_priv;
-	bool ret = true;
-
-	spin_lock(&mchdev_lock);
-	if (!i915_mch_dev) {
-		ret = false;
-		goto out_unlock;
-	}
-	dev_priv = i915_mch_dev;
-
-	if (dev_priv->max_delay > dev_priv->fmax)
-		dev_priv->max_delay--;
-
-out_unlock:
-	spin_unlock(&mchdev_lock);
-
-	return ret;
-}
-EXPORT_SYMBOL_GPL(i915_gpu_raise);
-
-/**
- * i915_gpu_lower - lower GPU frequency limit
- *
- * IPS indicates we're close to a thermal limit, so throttle back the GPU
- * frequency maximum.
- */
-bool i915_gpu_lower(void)
-{
-	struct drm_i915_private *dev_priv;
-	bool ret = true;
-
-	spin_lock(&mchdev_lock);
-	if (!i915_mch_dev) {
-		ret = false;
-		goto out_unlock;
-	}
-	dev_priv = i915_mch_dev;
-
-	if (dev_priv->max_delay < dev_priv->min_delay)
-		dev_priv->max_delay++;
-
-out_unlock:
-	spin_unlock(&mchdev_lock);
-
-	return ret;
-}
-EXPORT_SYMBOL_GPL(i915_gpu_lower);
-
-/**
- * i915_gpu_busy - indicate GPU business to IPS
- *
- * Tell the IPS driver whether or not the GPU is busy.
- */
-bool i915_gpu_busy(void)
-{
-	struct drm_i915_private *dev_priv;
-	bool ret = false;
-
-	spin_lock(&mchdev_lock);
-	if (!i915_mch_dev)
-		goto out_unlock;
-	dev_priv = i915_mch_dev;
-
-	ret = dev_priv->busy;
-
-out_unlock:
-	spin_unlock(&mchdev_lock);
-
-	return ret;
-}
-EXPORT_SYMBOL_GPL(i915_gpu_busy);
-
-/**
- * i915_gpu_turbo_disable - disable graphics turbo
- *
- * Disable graphics turbo by resetting the max frequency and setting the
- * current frequency to the default.
- */
-bool i915_gpu_turbo_disable(void)
-{
-	struct drm_i915_private *dev_priv;
-	bool ret = true;
-
-	spin_lock(&mchdev_lock);
-	if (!i915_mch_dev) {
-		ret = false;
-		goto out_unlock;
-	}
-	dev_priv = i915_mch_dev;
-
-	dev_priv->max_delay = dev_priv->fstart;
-
-	if (!ironlake_set_drps(dev_priv->dev, dev_priv->fstart))
-		ret = false;
-
-out_unlock:
-	spin_unlock(&mchdev_lock);
-
-	return ret;
-}
-EXPORT_SYMBOL_GPL(i915_gpu_turbo_disable);
-
-/**
- * Tells the intel_ips driver that the i915 driver is now loaded, if
- * IPS got loaded first.
- *
- * This awkward dance is so that neither module has to depend on the
- * other in order for IPS to do the appropriate communication of
- * GPU turbo limits to i915.
- */
-static void
-ips_ping_for_i915_load(void)
-{
-	void (*link)(void);
-
-	link = symbol_get(ips_link_to_i915_driver);
-	if (link) {
-		link();
-		symbol_put(ips_link_to_i915_driver);
+	/* Set up a WC MTRR for non-PAT systems.  This is more common than
+	 * one would think, because the kernel disables PAT on first
+	 * generation Core chips because WC PAT gets overridden by a UC
+	 * MTRR if present.  Even if a UC MTRR isn't present.
+	 */
+	dev_priv->mm.gtt_mtrr = mtrr_add(base, size, MTRR_TYPE_WRCOMB, 1);
+	if (dev_priv->mm.gtt_mtrr < 0) {
+		DRM_INFO("MTRR allocation failed.  Graphics "
+			 "performance may suffer.\n");
 	}
 }
 
@@ -1948,8 +1415,16 @@ ips_ping_for_i915_load(void)
 int i915_driver_load(struct drm_device *dev, unsigned long flags)
 {
 	struct drm_i915_private *dev_priv;
+	struct intel_device_info *info;
 	int ret = 0, mmio_bar;
-	uint32_t agp_size;
+	uint32_t aperture_size;
+
+	info = (struct intel_device_info *) flags;
+
+	/* Refuse to load on gen6+ without kms enabled. */
+	if (info->gen >= 6 && !drm_core_check_feature(dev, DRIVER_MODESET))
+		return -ENODEV;
+
 
 	/* i915 has 4 more counters */
 	dev->counters += 4;
@@ -1964,7 +1439,7 @@ int i915_driver_load(struct drm_device *dev, unsigned long flags)
 
 	dev->dev_private = (void *)dev_priv;
 	dev_priv->dev = dev;
-	dev_priv->info = (struct intel_device_info *) flags;
+	dev_priv->info = info;
 
 	if (i915_get_bridge_dev(dev)) {
 		ret = -EIO;
@@ -2003,27 +1478,16 @@ int i915_driver_load(struct drm_device *dev, unsigned long flags)
 		goto out_rmmap;
 	}
 
-	agp_size = dev_priv->mm.gtt->gtt_mappable_entries << PAGE_SHIFT;
+	aperture_size = dev_priv->mm.gtt->gtt_mappable_entries << PAGE_SHIFT;
 
 	dev_priv->mm.gtt_mapping =
-		io_mapping_create_wc(dev->agp->base, agp_size);
+		io_mapping_create_wc(dev->agp->base, aperture_size);
 	if (dev_priv->mm.gtt_mapping == NULL) {
 		ret = -EIO;
 		goto out_rmmap;
 	}
 
-	/* Set up a WC MTRR for non-PAT systems.  This is more common than
-	 * one would think, because the kernel disables PAT on first
-	 * generation Core chips because WC PAT gets overridden by a UC
-	 * MTRR if present.  Even if a UC MTRR isn't present.
-	 */
-	dev_priv->mm.gtt_mtrr = mtrr_add(dev->agp->base,
-					 agp_size,
-					 MTRR_TYPE_WRCOMB, 1);
-	if (dev_priv->mm.gtt_mtrr < 0) {
-		DRM_INFO("MTRR allocation failed.  Graphics "
-			 "performance may suffer.\n");
-	}
+	i915_mtrr_setup(dev_priv, dev->agp->base, aperture_size);
 
 	/* The i915 workqueue is primarily used for batched retirement of
 	 * requests (and thus managing bo) once the task has been completed
@@ -2047,9 +1511,6 @@ int i915_driver_load(struct drm_device *dev, unsigned long flags)
 		goto out_mtrrfree;
 	}
 
-	/* enable GEM by default */
-	dev_priv->has_gem = 1;
-
 	intel_irq_init(dev);
 
 	/* Try to make sure MCHBAR is enabled before poking at it */
@@ -2069,11 +1530,6 @@ int i915_driver_load(struct drm_device *dev, unsigned long flags)
 			goto out_gem_unload;
 	}
 
-	if (IS_PINEVIEW(dev))
-		i915_pineview_get_mem_freq(dev);
-	else if (IS_GEN5(dev))
-		i915_ironlake_get_mem_freq(dev);
-
 	/* On the 945G/GM, the chipset reports the MSI capability on the
 	 * integrated graphics even though the support isn't actually there
 	 * according to the published specs.  It doesn't appear to function
@@ -2093,7 +1549,7 @@ int i915_driver_load(struct drm_device *dev, unsigned long flags)
 	spin_lock_init(&dev_priv->error_lock);
 	spin_lock_init(&dev_priv->rps_lock);
 
-	if (IS_IVYBRIDGE(dev))
+	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;
@@ -2117,6 +1573,8 @@ 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();
@@ -2124,14 +1582,8 @@ int i915_driver_load(struct drm_device *dev, unsigned long flags)
 	setup_timer(&dev_priv->hangcheck_timer, i915_hangcheck_elapsed,
 		    (unsigned long) dev);
 
-	if (IS_GEN5(dev)) {
-		spin_lock(&mchdev_lock);
-		i915_mch_dev = dev_priv;
-		dev_priv->mchdev_lock = &mchdev_lock;
-		spin_unlock(&mchdev_lock);
-
-		ips_ping_for_i915_load();
-	}
+	if (IS_GEN5(dev))
+		intel_gpu_ips_init(dev_priv);
 
 	return 0;
 
@@ -2166,17 +1618,18 @@ int i915_driver_unload(struct drm_device *dev)
 	struct drm_i915_private *dev_priv = dev->dev_private;
 	int ret;
 
-	spin_lock(&mchdev_lock);
-	i915_mch_dev = NULL;
-	spin_unlock(&mchdev_lock);
+	intel_gpu_ips_teardown();
+
+	i915_teardown_sysfs(dev);
 
 	if (dev_priv->mm.inactive_shrinker.shrink)
 		unregister_shrinker(&dev_priv->mm.inactive_shrinker);
 
 	mutex_lock(&dev->struct_mutex);
-	ret = i915_gpu_idle(dev, true);
+	ret = i915_gpu_idle(dev);
 	if (ret)
 		DRM_ERROR("failed to idle hardware: %d\n", ret);
+	i915_gem_retire_requests(dev);
 	mutex_unlock(&dev->struct_mutex);
 
 	/* Cancel the retire work handler, which should be idle now. */
@@ -2228,8 +1681,7 @@ int i915_driver_unload(struct drm_device *dev)
 		i915_gem_cleanup_ringbuffer(dev);
 		mutex_unlock(&dev->struct_mutex);
 		i915_gem_cleanup_aliasing_ppgtt(dev);
-		if (I915_HAS_FBC(dev) && i915_powersave)
-			i915_cleanup_compression(dev);
+		i915_gem_cleanup_stolen(dev);
 		drm_mm_takedown(&dev_priv->mm.stolen);
 
 		intel_cleanup_overlay(dev);
@@ -2277,7 +1729,7 @@ int i915_driver_open(struct drm_device *dev, struct drm_file *file)
  * mode setting case, we want to restore the kernel's initial mode (just
  * in case the last client left us in a bad state).
  *
- * Additionally, in the non-mode setting case, we'll tear down the AGP
+ * Additionally, in the non-mode setting case, we'll tear down the GTT
  * and DMA structures, since the kernel won't be using them, and clea
  * up any GEM state.
  */
@@ -2322,7 +1774,7 @@ struct drm_ioctl_desc i915_ioctls[] = {
 	DRM_IOCTL_DEF_DRV(I915_INIT_HEAP, drm_noop, DRM_AUTH|DRM_MASTER|DRM_ROOT_ONLY),
 	DRM_IOCTL_DEF_DRV(I915_CMDBUFFER, i915_cmdbuffer, DRM_AUTH),
 	DRM_IOCTL_DEF_DRV(I915_DESTROY_HEAP,  drm_noop, DRM_AUTH|DRM_MASTER|DRM_ROOT_ONLY),
-	DRM_IOCTL_DEF_DRV(I915_SET_VBLANK_PIPE,  i915_vblank_pipe_set, DRM_AUTH|DRM_MASTER|DRM_ROOT_ONLY),
+	DRM_IOCTL_DEF_DRV(I915_SET_VBLANK_PIPE,  drm_noop, DRM_AUTH|DRM_MASTER|DRM_ROOT_ONLY),
 	DRM_IOCTL_DEF_DRV(I915_GET_VBLANK_PIPE,  i915_vblank_pipe_get, DRM_AUTH),
 	DRM_IOCTL_DEF_DRV(I915_VBLANK_SWAP, i915_vblank_swap, DRM_AUTH),
 	DRM_IOCTL_DEF_DRV(I915_HWS_ADDR, i915_set_status_page, DRM_AUTH|DRM_MASTER|DRM_ROOT_ONLY),
@@ -2355,16 +1807,10 @@ struct drm_ioctl_desc i915_ioctls[] = {
 
 int i915_max_ioctl = DRM_ARRAY_SIZE(i915_ioctls);
 
-/**
- * Determine if the device really is AGP or not.
- *
- * All Intel graphics chipsets are treated as AGP, even if they are really
- * PCI-e.
- *
- * \param dev   The device to be tested.
- *
- * \returns
- * A value of 1 is always retured to indictate every i9x5 is AGP.
+/*
+ * This is really ugly: Because old userspace abused the linux agp interface to
+ * manage the gtt, we need to claim that all intel devices are agp.  For
+ * otherwise the drm core refuses to initialize the agp support code.
  */
 int i915_driver_device_is_agp(struct drm_device * dev)
 {
diff --git a/drivers/gpu/drm/i915/i915_drv.c b/drivers/gpu/drm/i915/i915_drv.c
index ae8a64f9f84..238a5216583 100644
--- a/drivers/gpu/drm/i915/i915_drv.c
+++ b/drivers/gpu/drm/i915/i915_drv.c
@@ -84,6 +84,12 @@ MODULE_PARM_DESC(lvds_downclock,
 		"Use panel (LVDS/eDP) downclocking for power savings "
 		"(default: false)");
 
+int i915_lvds_channel_mode __read_mostly;
+module_param_named(lvds_channel_mode, i915_lvds_channel_mode, int, 0600);
+MODULE_PARM_DESC(lvds_channel_mode,
+		 "Specify LVDS channel mode "
+		 "(0=probe BIOS [default], 1=single-channel, 2=dual-channel)");
+
 int i915_panel_use_ssc __read_mostly = -1;
 module_param_named(lvds_use_ssc, i915_panel_use_ssc, int, 0600);
 MODULE_PARM_DESC(lvds_use_ssc,
@@ -93,8 +99,8 @@ MODULE_PARM_DESC(lvds_use_ssc,
 int i915_vbt_sdvo_panel_type __read_mostly = -1;
 module_param_named(vbt_sdvo_panel_type, i915_vbt_sdvo_panel_type, int, 0600);
 MODULE_PARM_DESC(vbt_sdvo_panel_type,
-		"Override selection of SDVO panel mode in the VBT "
-		"(default: auto)");
+		"Override/Ignore selection of SDVO panel mode in the VBT "
+		"(-2=ignore, -1=auto [default], index in VBT BIOS table)");
 
 static bool i915_try_reset __read_mostly = true;
 module_param_named(reset, i915_try_reset, bool, 0600);
@@ -209,6 +215,7 @@ static const struct intel_device_info intel_ironlake_d_info = {
 	.gen = 5,
 	.need_gfx_hws = 1, .has_hotplug = 1,
 	.has_bsd_ring = 1,
+	.has_pch_split = 1,
 };
 
 static const struct intel_device_info intel_ironlake_m_info = {
@@ -216,6 +223,7 @@ static const struct intel_device_info intel_ironlake_m_info = {
 	.need_gfx_hws = 1, .has_hotplug = 1,
 	.has_fbc = 1,
 	.has_bsd_ring = 1,
+	.has_pch_split = 1,
 };
 
 static const struct intel_device_info intel_sandybridge_d_info = {
@@ -224,6 +232,7 @@ static const struct intel_device_info intel_sandybridge_d_info = {
 	.has_bsd_ring = 1,
 	.has_blt_ring = 1,
 	.has_llc = 1,
+	.has_pch_split = 1,
 };
 
 static const struct intel_device_info intel_sandybridge_m_info = {
@@ -233,6 +242,7 @@ static const struct intel_device_info intel_sandybridge_m_info = {
 	.has_bsd_ring = 1,
 	.has_blt_ring = 1,
 	.has_llc = 1,
+	.has_pch_split = 1,
 };
 
 static const struct intel_device_info intel_ivybridge_d_info = {
@@ -241,6 +251,7 @@ static const struct intel_device_info intel_ivybridge_d_info = {
 	.has_bsd_ring = 1,
 	.has_blt_ring = 1,
 	.has_llc = 1,
+	.has_pch_split = 1,
 };
 
 static const struct intel_device_info intel_ivybridge_m_info = {
@@ -250,6 +261,43 @@ static const struct intel_device_info intel_ivybridge_m_info = {
 	.has_bsd_ring = 1,
 	.has_blt_ring = 1,
 	.has_llc = 1,
+	.has_pch_split = 1,
+};
+
+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,
+	.is_valleyview = 1,
+};
+
+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,
+	.is_valleyview = 1,
+};
+
+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_pch_split = 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_pch_split = 1,
 };
 
 static const struct pci_device_id pciidlist[] = {		/* aka */
@@ -297,6 +345,13 @@ static const struct pci_device_id pciidlist[] = {		/* aka */
 	INTEL_VGA_DEVICE(0x0162, &intel_ivybridge_d_info), /* GT2 desktop */
 	INTEL_VGA_DEVICE(0x015a, &intel_ivybridge_d_info), /* GT1 server */
 	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 */
+	INTEL_VGA_DEVICE(0x040a, &intel_haswell_d_info), /* GT1 server */
+	INTEL_VGA_DEVICE(0x041a, &intel_haswell_d_info), /* GT2 server */
+	INTEL_VGA_DEVICE(0x0406, &intel_haswell_m_info), /* GT1 mobile */
+	INTEL_VGA_DEVICE(0x0416, &intel_haswell_m_info), /* GT2 mobile */
+	INTEL_VGA_DEVICE(0x0c16, &intel_haswell_d_info), /* SDV */
 	{0, 0, 0}
 };
 
@@ -308,6 +363,7 @@ MODULE_DEVICE_TABLE(pci, pciidlist);
 #define INTEL_PCH_IBX_DEVICE_ID_TYPE	0x3b00
 #define INTEL_PCH_CPT_DEVICE_ID_TYPE	0x1c00
 #define INTEL_PCH_PPT_DEVICE_ID_TYPE	0x1e00
+#define INTEL_PCH_LPT_DEVICE_ID_TYPE	0x8c00
 
 void intel_detect_pch(struct drm_device *dev)
 {
@@ -328,20 +384,45 @@ void intel_detect_pch(struct drm_device *dev)
 
 			if (id == INTEL_PCH_IBX_DEVICE_ID_TYPE) {
 				dev_priv->pch_type = PCH_IBX;
+				dev_priv->num_pch_pll = 2;
 				DRM_DEBUG_KMS("Found Ibex Peak PCH\n");
 			} else if (id == INTEL_PCH_CPT_DEVICE_ID_TYPE) {
 				dev_priv->pch_type = PCH_CPT;
+				dev_priv->num_pch_pll = 2;
 				DRM_DEBUG_KMS("Found CougarPoint PCH\n");
 			} else if (id == INTEL_PCH_PPT_DEVICE_ID_TYPE) {
 				/* PantherPoint is CPT compatible */
 				dev_priv->pch_type = PCH_CPT;
+				dev_priv->num_pch_pll = 2;
 				DRM_DEBUG_KMS("Found PatherPoint PCH\n");
+			} else if (id == INTEL_PCH_LPT_DEVICE_ID_TYPE) {
+				dev_priv->pch_type = PCH_LPT;
+				dev_priv->num_pch_pll = 0;
+				DRM_DEBUG_KMS("Found LynxPoint PCH\n");
 			}
+			BUG_ON(dev_priv->num_pch_pll > I915_NUM_PLLS);
 		}
 		pci_dev_put(pch);
 	}
 }
 
+bool i915_semaphore_is_enabled(struct drm_device *dev)
+{
+	if (INTEL_INFO(dev)->gen < 6)
+		return 0;
+
+	if (i915_semaphores >= 0)
+		return i915_semaphores;
+
+#ifdef CONFIG_INTEL_IOMMU
+	/* Enable semaphores on SNB when IO remapping is off */
+	if (INTEL_INFO(dev)->gen == 6 && intel_iommu_gfx_mapped)
+		return false;
+#endif
+
+	return 1;
+}
+
 void __gen6_gt_force_wake_get(struct drm_i915_private *dev_priv)
 {
 	int count;
@@ -366,7 +447,7 @@ void __gen6_gt_force_wake_mt_get(struct drm_i915_private *dev_priv)
 	while (count++ < 50 && (I915_READ_NOTRACE(FORCEWAKE_MT_ACK) & 1))
 		udelay(10);
 
-	I915_WRITE_NOTRACE(FORCEWAKE_MT, (1<<16) | 1);
+	I915_WRITE_NOTRACE(FORCEWAKE_MT, _MASKED_BIT_ENABLE(1));
 	POSTING_READ(FORCEWAKE_MT);
 
 	count = 0;
@@ -408,7 +489,7 @@ void __gen6_gt_force_wake_put(struct drm_i915_private *dev_priv)
 
 void __gen6_gt_force_wake_mt_put(struct drm_i915_private *dev_priv)
 {
-	I915_WRITE_NOTRACE(FORCEWAKE_MT, (1<<16) | 0);
+	I915_WRITE_NOTRACE(FORCEWAKE_MT, _MASKED_BIT_DISABLE(1));
 	/* The below doubles as a POSTING_READ */
 	gen6_gt_check_fifodbg(dev_priv);
 }
@@ -446,6 +527,31 @@ int __gen6_gt_wait_for_fifo(struct drm_i915_private *dev_priv)
 	return ret;
 }
 
+void vlv_force_wake_get(struct drm_i915_private *dev_priv)
+{
+	int count;
+
+	count = 0;
+
+	/* Already awake? */
+	if ((I915_READ(0x130094) & 0xa1) == 0xa1)
+		return;
+
+	I915_WRITE_NOTRACE(FORCEWAKE_VLV, 0xffffffff);
+	POSTING_READ(FORCEWAKE_VLV);
+
+	count = 0;
+	while (count++ < 50 && (I915_READ_NOTRACE(FORCEWAKE_ACK_VLV) & 1) == 0)
+		udelay(10);
+}
+
+void vlv_force_wake_put(struct drm_i915_private *dev_priv)
+{
+	I915_WRITE_NOTRACE(FORCEWAKE_VLV, 0xffff0000);
+	/* FIXME: confirm VLV behavior with Punit folks */
+	POSTING_READ(FORCEWAKE_VLV);
+}
+
 static int i915_drm_freeze(struct drm_device *dev)
 {
 	struct drm_i915_private *dev_priv = dev->dev_private;
@@ -525,15 +631,16 @@ static int i915_drm_thaw(struct drm_device *dev)
 
 	/* KMS EnterVT equivalent */
 	if (drm_core_check_feature(dev, DRIVER_MODESET)) {
+		if (HAS_PCH_SPLIT(dev))
+			ironlake_init_pch_refclk(dev);
+
 		mutex_lock(&dev->struct_mutex);
 		dev_priv->mm.suspended = 0;
 
 		error = i915_gem_init_hw(dev);
 		mutex_unlock(&dev->struct_mutex);
 
-		if (HAS_PCH_SPLIT(dev))
-			ironlake_init_pch_refclk(dev);
-
+		intel_modeset_init_hw(dev);
 		drm_mode_config_reset(dev);
 		drm_irq_install(dev);
 
@@ -541,9 +648,6 @@ static int i915_drm_thaw(struct drm_device *dev)
 		mutex_lock(&dev->mode_config.mutex);
 		drm_helper_resume_force_mode(dev);
 		mutex_unlock(&dev->mode_config.mutex);
-
-		if (IS_IRONLAKE_M(dev))
-			ironlake_enable_rc6(dev);
 	}
 
 	intel_opregion_init(dev);
@@ -576,7 +680,7 @@ int i915_resume(struct drm_device *dev)
 	return 0;
 }
 
-static int i8xx_do_reset(struct drm_device *dev, u8 flags)
+static int i8xx_do_reset(struct drm_device *dev)
 {
 	struct drm_i915_private *dev_priv = dev->dev_private;
 
@@ -610,11 +714,12 @@ static int i965_reset_complete(struct drm_device *dev)
 {
 	u8 gdrst;
 	pci_read_config_byte(dev->pdev, I965_GDRST, &gdrst);
-	return gdrst & 0x1;
+	return (gdrst & GRDOM_RESET_ENABLE) == 0;
 }
 
-static int i965_do_reset(struct drm_device *dev, u8 flags)
+static int i965_do_reset(struct drm_device *dev)
 {
+	int ret;
 	u8 gdrst;
 
 	/*
@@ -623,20 +728,43 @@ static int i965_do_reset(struct drm_device *dev, u8 flags)
 	 * triggers the reset; when done, the hardware will clear it.
 	 */
 	pci_read_config_byte(dev->pdev, I965_GDRST, &gdrst);
-	pci_write_config_byte(dev->pdev, I965_GDRST, gdrst | flags | 0x1);
+	pci_write_config_byte(dev->pdev, I965_GDRST,
+			      gdrst | GRDOM_RENDER |
+			      GRDOM_RESET_ENABLE);
+	ret =  wait_for(i965_reset_complete(dev), 500);
+	if (ret)
+		return ret;
+
+	/* We can't reset render&media without also resetting display ... */
+	pci_read_config_byte(dev->pdev, I965_GDRST, &gdrst);
+	pci_write_config_byte(dev->pdev, I965_GDRST,
+			      gdrst | GRDOM_MEDIA |
+			      GRDOM_RESET_ENABLE);
 
 	return wait_for(i965_reset_complete(dev), 500);
 }
 
-static int ironlake_do_reset(struct drm_device *dev, u8 flags)
+static int ironlake_do_reset(struct drm_device *dev)
 {
 	struct drm_i915_private *dev_priv = dev->dev_private;
-	u32 gdrst = I915_READ(MCHBAR_MIRROR_BASE + ILK_GDSR);
-	I915_WRITE(MCHBAR_MIRROR_BASE + ILK_GDSR, gdrst | flags | 0x1);
+	u32 gdrst;
+	int ret;
+
+	gdrst = I915_READ(MCHBAR_MIRROR_BASE + ILK_GDSR);
+	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);
+	if (ret)
+		return ret;
+
+	/* We can't reset render&media without also resetting display ... */
+	gdrst = I915_READ(MCHBAR_MIRROR_BASE + ILK_GDSR);
+	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);
 }
 
-static int gen6_do_reset(struct drm_device *dev, u8 flags)
+static int gen6_do_reset(struct drm_device *dev)
 {
 	struct drm_i915_private *dev_priv = dev->dev_private;
 	int	ret;
@@ -671,10 +799,44 @@ static int gen6_do_reset(struct drm_device *dev, u8 flags)
 	return ret;
 }
 
+static int intel_gpu_reset(struct drm_device *dev)
+{
+	struct drm_i915_private *dev_priv = dev->dev_private;
+	int ret = -ENODEV;
+
+	switch (INTEL_INFO(dev)->gen) {
+	case 7:
+	case 6:
+		ret = gen6_do_reset(dev);
+		break;
+	case 5:
+		ret = ironlake_do_reset(dev);
+		break;
+	case 4:
+		ret = i965_do_reset(dev);
+		break;
+	case 2:
+		ret = i8xx_do_reset(dev);
+		break;
+	}
+
+	/* Also reset the gpu hangman. */
+	if (dev_priv->stop_rings) {
+		DRM_DEBUG("Simulated gpu hang, resetting stop_rings\n");
+		dev_priv->stop_rings = 0;
+		if (ret == -ENODEV) {
+			DRM_ERROR("Reset not implemented, but ignoring "
+				  "error for simulated gpu hangs\n");
+			ret = 0;
+		}
+	}
+
+	return ret;
+}
+
 /**
  * i915_reset - reset chip after a hang
  * @dev: drm device to reset
- * @flags: reset domains
  *
  * Reset the chip.  Useful if a hang is detected. Returns zero on successful
  * reset or otherwise an error code.
@@ -687,14 +849,9 @@ static int gen6_do_reset(struct drm_device *dev, u8 flags)
  *   - re-init interrupt state
  *   - re-init display
  */
-int i915_reset(struct drm_device *dev, u8 flags)
+int i915_reset(struct drm_device *dev)
 {
 	drm_i915_private_t *dev_priv = dev->dev_private;
-	/*
-	 * We really should only reset the display subsystem if we actually
-	 * need to
-	 */
-	bool need_display = true;
 	int ret;
 
 	if (!i915_try_reset)
@@ -703,26 +860,16 @@ int i915_reset(struct drm_device *dev, u8 flags)
 	if (!mutex_trylock(&dev->struct_mutex))
 		return -EBUSY;
 
+	dev_priv->stop_rings = 0;
+
 	i915_gem_reset(dev);
 
 	ret = -ENODEV;
-	if (get_seconds() - dev_priv->last_gpu_reset < 5) {
+	if (get_seconds() - dev_priv->last_gpu_reset < 5)
 		DRM_ERROR("GPU hanging too fast, declaring wedged!\n");
-	} else switch (INTEL_INFO(dev)->gen) {
-	case 7:
-	case 6:
-		ret = gen6_do_reset(dev, flags);
-		break;
-	case 5:
-		ret = ironlake_do_reset(dev, flags);
-		break;
-	case 4:
-		ret = i965_do_reset(dev, flags);
-		break;
-	case 2:
-		ret = i8xx_do_reset(dev, flags);
-		break;
-	}
+	else
+		ret = intel_gpu_reset(dev);
+
 	dev_priv->last_gpu_reset = get_seconds();
 	if (ret) {
 		DRM_ERROR("Failed to reset chip.\n");
@@ -746,36 +893,27 @@ int i915_reset(struct drm_device *dev, u8 flags)
 	 */
 	if (drm_core_check_feature(dev, DRIVER_MODESET) ||
 			!dev_priv->mm.suspended) {
+		struct intel_ring_buffer *ring;
+		int i;
+
 		dev_priv->mm.suspended = 0;
 
 		i915_gem_init_swizzling(dev);
 
-		dev_priv->ring[RCS].init(&dev_priv->ring[RCS]);
-		if (HAS_BSD(dev))
-		    dev_priv->ring[VCS].init(&dev_priv->ring[VCS]);
-		if (HAS_BLT(dev))
-		    dev_priv->ring[BCS].init(&dev_priv->ring[BCS]);
+		for_each_ring(ring, dev_priv, i)
+			ring->init(ring);
 
 		i915_gem_init_ppgtt(dev);
 
 		mutex_unlock(&dev->struct_mutex);
-		drm_irq_uninstall(dev);
-		drm_mode_config_reset(dev);
-		drm_irq_install(dev);
-		mutex_lock(&dev->struct_mutex);
-	}
 
-	mutex_unlock(&dev->struct_mutex);
+		if (drm_core_check_feature(dev, DRIVER_MODESET))
+			intel_modeset_init_hw(dev);
 
-	/*
-	 * Perform a full modeset as on later generations, e.g. Ironlake, we may
-	 * need to retrain the display link and cannot just restore the register
-	 * values.
-	 */
-	if (need_display) {
-		mutex_lock(&dev->mode_config.mutex);
-		drm_helper_resume_force_mode(dev);
-		mutex_unlock(&dev->mode_config.mutex);
+		drm_irq_uninstall(dev);
+		drm_irq_install(dev);
+	} else {
+		mutex_unlock(&dev->struct_mutex);
 	}
 
 	return 0;
@@ -874,7 +1012,7 @@ static const struct dev_pm_ops i915_pm_ops = {
 	.restore = i915_pm_resume,
 };
 
-static struct vm_operations_struct i915_gem_vm_ops = {
+static const struct vm_operations_struct i915_gem_vm_ops = {
 	.fault = i915_gem_fault,
 	.open = drm_gem_vm_open,
 	.close = drm_gem_vm_close,
@@ -901,7 +1039,7 @@ static struct drm_driver driver = {
 	 */
 	.driver_features =
 	    DRIVER_USE_AGP | DRIVER_REQUIRE_AGP | /* DRIVER_USE_MTRR |*/
-	    DRIVER_HAVE_IRQ | DRIVER_IRQ_SHARED | DRIVER_GEM,
+	    DRIVER_HAVE_IRQ | DRIVER_IRQ_SHARED | DRIVER_GEM | DRIVER_PRIME,
 	.load = i915_driver_load,
 	.unload = i915_driver_unload,
 	.open = i915_driver_open,
@@ -924,6 +1062,12 @@ static struct drm_driver driver = {
 	.gem_init_object = i915_gem_init_object,
 	.gem_free_object = i915_gem_free_object,
 	.gem_vm_ops = &i915_gem_vm_ops,
+
+	.prime_handle_to_fd = drm_gem_prime_handle_to_fd,
+	.prime_fd_to_handle = drm_gem_prime_fd_to_handle,
+	.gem_prime_export = i915_gem_prime_export,
+	.gem_prime_import = i915_gem_prime_import,
+
 	.dumb_create = i915_gem_dumb_create,
 	.dumb_map_offset = i915_gem_mmap_gtt,
 	.dumb_destroy = i915_gem_dumb_destroy,
@@ -993,6 +1137,13 @@ MODULE_AUTHOR(DRIVER_AUTHOR);
 MODULE_DESCRIPTION(DRIVER_DESC);
 MODULE_LICENSE("GPL and additional rights");
 
+/* We give fast paths for the really cool registers */
+#define NEEDS_FORCE_WAKE(dev_priv, reg) \
+       (((dev_priv)->info->gen >= 6) && \
+        ((reg) < 0x40000) &&            \
+        ((reg) != FORCEWAKE)) && \
+       (!IS_VALLEYVIEW((dev_priv)->dev))
+
 #define __i915_read(x, y) \
 u##x i915_read##x(struct drm_i915_private *dev_priv, u32 reg) { \
 	u##x val = 0; \
diff --git a/drivers/gpu/drm/i915/i915_drv.h b/drivers/gpu/drm/i915/i915_drv.h
index 5fabc6c31fe..377c21f531e 100644
--- a/drivers/gpu/drm/i915/i915_drv.h
+++ b/drivers/gpu/drm/i915/i915_drv.h
@@ -38,6 +38,8 @@
 #include <linux/i2c-algo-bit.h>
 #include <drm/intel-gtt.h>
 #include <linux/backlight.h>
+#include <linux/intel-iommu.h>
+#include <linux/kref.h>
 
 /* General customization:
  */
@@ -63,10 +65,30 @@ enum plane {
 };
 #define plane_name(p) ((p) + 'A')
 
+enum port {
+	PORT_A = 0,
+	PORT_B,
+	PORT_C,
+	PORT_D,
+	PORT_E,
+	I915_MAX_PORTS
+};
+#define port_name(p) ((p) + 'A')
+
 #define I915_GEM_GPU_DOMAINS	(~(I915_GEM_DOMAIN_CPU | I915_GEM_DOMAIN_GTT))
 
 #define for_each_pipe(p) for ((p) = 0; (p) < dev_priv->num_pipe; (p)++)
 
+struct intel_pch_pll {
+	int refcount; /* count of number of CRTCs sharing this PLL */
+	int active; /* count of number of active CRTCs (i.e. DPMS on) */
+	bool on; /* is the PLL actually active? Disabled during modeset */
+	int pll_reg;
+	int fp0_reg;
+	int fp1_reg;
+};
+#define I915_NUM_PLLS 2
+
 /* Interface history:
  *
  * 1.1: Original.
@@ -111,11 +133,11 @@ struct opregion_asle;
 struct drm_i915_private;
 
 struct intel_opregion {
-	struct opregion_header *header;
-	struct opregion_acpi *acpi;
-	struct opregion_swsci *swsci;
-	struct opregion_asle *asle;
-	void *vbt;
+	struct opregion_header __iomem *header;
+	struct opregion_acpi __iomem *acpi;
+	struct opregion_swsci __iomem *swsci;
+	struct opregion_asle __iomem *asle;
+	void __iomem *vbt;
 	u32 __iomem *lid_state;
 };
 #define OPREGION_SIZE            (8*1024)
@@ -135,7 +157,6 @@ struct drm_i915_master_private {
 struct drm_i915_fence_reg {
 	struct list_head lru_list;
 	struct drm_i915_gem_object *obj;
-	uint32_t setup_seqno;
 	int pin_count;
 };
 
@@ -151,8 +172,11 @@ struct sdvo_device_mapping {
 struct intel_display_error_state;
 
 struct drm_i915_error_state {
+	struct kref ref;
 	u32 eir;
 	u32 pgtbl_er;
+	u32 ier;
+	bool waiting[I915_NUM_RINGS];
 	u32 pipestat[I915_MAX_PIPES];
 	u32 tail[I915_NUM_RINGS];
 	u32 head[I915_NUM_RINGS];
@@ -218,11 +242,15 @@ struct drm_i915_display_funcs {
 	void (*update_wm)(struct drm_device *dev);
 	void (*update_sprite_wm)(struct drm_device *dev, int pipe,
 				 uint32_t sprite_width, int pixel_size);
+	void (*sanitize_pm)(struct drm_device *dev);
+	void (*update_linetime_wm)(struct drm_device *dev, int pipe,
+				 struct drm_display_mode *mode);
 	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 (*off)(struct drm_crtc *crtc);
 	void (*write_eld)(struct drm_connector *connector,
 			  struct drm_crtc *crtc);
 	void (*fdi_link_train)(struct drm_crtc *crtc);
@@ -255,6 +283,9 @@ struct intel_device_info {
 	u8 is_broadwater:1;
 	u8 is_crestline:1;
 	u8 is_ivybridge:1;
+	u8 is_valleyview:1;
+	u8 has_pch_split:1;
+	u8 is_haswell:1;
 	u8 has_fbc:1;
 	u8 has_pipe_cxsr:1;
 	u8 has_hotplug:1;
@@ -291,10 +322,12 @@ enum no_fbc_reason {
 enum intel_pch {
 	PCH_IBX,	/* Ibexpeak PCH */
 	PCH_CPT,	/* Cougarpoint PCH */
+	PCH_LPT,	/* Lynxpoint PCH */
 };
 
 #define QUIRK_PIPEA_FORCE (1<<0)
 #define QUIRK_LVDS_SSC_DISABLE (1<<1)
+#define QUIRK_INVERT_BRIGHTNESS (1<<2)
 
 struct intel_fbdev;
 struct intel_fbc_work;
@@ -302,7 +335,6 @@ struct intel_fbc_work;
 struct intel_gmbus {
 	struct i2c_adapter adapter;
 	bool force_bit;
-	bool has_gpio;
 	u32 reg0;
 	u32 gpio_reg;
 	struct i2c_algo_bit_data bit_algo;
@@ -314,7 +346,6 @@ typedef struct drm_i915_private {
 
 	const struct intel_device_info *info;
 
-	int has_gem;
 	int relative_constants_mode;
 
 	void __iomem *regs;
@@ -326,19 +357,23 @@ typedef struct drm_i915_private {
 	/** gt_lock is also taken in irq contexts. */
 	struct spinlock gt_lock;
 
-	struct intel_gmbus *gmbus;
+	struct intel_gmbus gmbus[GMBUS_NUM_PORTS];
 
 	/** gmbus_mutex protects against concurrent usage of the single hw gmbus
 	 * controller on different i2c buses. */
 	struct mutex gmbus_mutex;
 
+	/**
+	 * Base address of the gmbus and gpio block.
+	 */
+	uint32_t gpio_mmio_base;
+
 	struct pci_dev *bridge_dev;
 	struct intel_ring_buffer ring[I915_NUM_RINGS];
 	uint32_t next_seqno;
 
 	drm_dma_handle_t *status_page_dmah;
 	uint32_t counter;
-	drm_local_map_t hws_map;
 	struct drm_i915_gem_object *pwrctx;
 	struct drm_i915_gem_object *renderctx;
 
@@ -354,6 +389,10 @@ typedef struct drm_i915_private {
 
 	/* protects the irq masks */
 	spinlock_t irq_lock;
+
+	/* DPIO indirect register protection */
+	spinlock_t dpio_lock;
+
 	/** Cached value of IMR to avoid reads in updating the bitfield */
 	u32 pipestat[2];
 	u32 irq_mask;
@@ -363,22 +402,20 @@ typedef struct drm_i915_private {
 	u32 hotplug_supported_mask;
 	struct work_struct hotplug_work;
 
-	int tex_lru_log_granularity;
-	int allow_batchbuffer;
 	unsigned int sr01, adpa, ppcr, dvob, dvoc, lvds;
-	int vblank_pipe;
 	int num_pipe;
+	int num_pch_pll;
 
 	/* For hangcheck timer */
 #define DRM_I915_HANGCHECK_PERIOD 1500 /* in ms */
 	struct timer_list hangcheck_timer;
 	int hangcheck_count;
-	uint32_t last_acthd;
-	uint32_t last_acthd_bsd;
-	uint32_t last_acthd_blt;
+	uint32_t last_acthd[I915_NUM_RINGS];
 	uint32_t last_instdone;
 	uint32_t last_instdone1;
 
+	unsigned int stop_rings;
+
 	unsigned long cfb_size;
 	unsigned int cfb_fb;
 	enum plane cfb_plane;
@@ -405,6 +442,8 @@ typedef struct drm_i915_private {
 	unsigned int lvds_use_ssc:1;
 	unsigned int display_clock_mode:1;
 	int lvds_ssc_freq;
+	unsigned int bios_lvds_val; /* initial [PCH_]LVDS reg val in VBIOS */
+	unsigned int lvds_val; /* used for checking LVDS channel mode */
 	struct {
 		int rate;
 		int lanes;
@@ -428,6 +467,7 @@ typedef struct drm_i915_private {
 	unsigned int fsb_freq, mem_freq, is_ddr3;
 
 	spinlock_t error_lock;
+	/* Protected by dev->error_lock. */
 	struct drm_i915_error_state *first_error;
 	struct work_struct error_work;
 	struct completion error_completion;
@@ -652,24 +692,10 @@ typedef struct drm_i915_private {
 		 */
 		struct list_head inactive_list;
 
-		/**
-		 * LRU list of objects which are not in the ringbuffer but
-		 * are still pinned in the GTT.
-		 */
-		struct list_head pinned_list;
-
 		/** LRU list of objects with fence regs on them. */
 		struct list_head fence_list;
 
 		/**
-		 * List of objects currently pending being freed.
-		 *
-		 * These objects are no longer in use, but due to a signal
-		 * we were prevented from freeing them at the appointed time.
-		 */
-		struct list_head deferred_free_list;
-
-		/**
 		 * We leave the user IRQ off as much as possible,
 		 * but this means that requests will finish and never
 		 * be retired once the system goes idle. Set a timer to
@@ -717,6 +743,16 @@ typedef struct drm_i915_private {
 		size_t object_memory;
 		u32 object_count;
 	} mm;
+
+	/* Old dri1 support infrastructure, beware the dragons ya fools entering
+	 * here! */
+	struct {
+		unsigned allow_batchbuffer : 1;
+		u32 __iomem *gfx_hws_cpu_addr;
+	} dri1;
+
+	/* Kernel Modesetting */
+
 	struct sdvo_device_mapping sdvo_mappings[2];
 	/* indicate whether the LVDS_BORDER should be enabled or not */
 	unsigned int lvds_border_bits;
@@ -726,7 +762,8 @@ typedef struct drm_i915_private {
 	struct drm_crtc *plane_to_crtc_mapping[3];
 	struct drm_crtc *pipe_to_crtc_mapping[3];
 	wait_queue_head_t pending_flip_queue;
-	bool flip_pending_is_done;
+
+	struct intel_pch_pll pch_plls[I915_NUM_PLLS];
 
 	/* Reclocking support */
 	bool render_reclock_avail;
@@ -781,6 +818,11 @@ typedef struct drm_i915_private {
 	struct drm_property *force_audio_property;
 } drm_i915_private_t;
 
+/* Iterate over initialised rings */
+#define for_each_ring(ring__, dev_priv__, i__) \
+	for ((i__) = 0; (i__) < I915_NUM_RINGS; (i__)++) \
+		if (((ring__) = &(dev_priv__)->ring[(i__)]), intel_ring_initialized((ring__)))
+
 enum hdmi_force_audio {
 	HDMI_AUDIO_OFF_DVI = -2,	/* no aux data for HDMI-DVI converter */
 	HDMI_AUDIO_OFF,			/* force turn off HDMI audio */
@@ -844,7 +886,14 @@ struct drm_i915_gem_object {
 	 * Current tiling mode for the object.
 	 */
 	unsigned int tiling_mode:2;
-	unsigned int tiling_changed:1;
+	/**
+	 * Whether the tiling parameters for the currently associated fence
+	 * register have changed. Note that for the purposes of tracking
+	 * tiling changes we also treat the unfenced register, the register
+	 * slot that the object occupies whilst it executes a fenced
+	 * command (such as BLT on gen2/3), as a "fence".
+	 */
+	unsigned int fence_dirty:1;
 
 	/** How many users have pinned this object in GTT space. The following
 	 * users can each hold at most one reference: pwrite/pread, pin_ioctl
@@ -881,6 +930,7 @@ struct drm_i915_gem_object {
 	unsigned int cache_level:2;
 
 	unsigned int has_aliasing_ppgtt_mapping:1;
+	unsigned int has_global_gtt_mapping:1;
 
 	struct page **pages;
 
@@ -890,6 +940,8 @@ struct drm_i915_gem_object {
 	struct scatterlist *sg_list;
 	int num_sg;
 
+	/* prime dma-buf support */
+	struct sg_table *sg_table;
 	/**
 	 * Used for performing relocations during execbuffer insertion.
 	 */
@@ -904,13 +956,12 @@ struct drm_i915_gem_object {
 	 */
 	uint32_t gtt_offset;
 
-	/** Breadcrumb of last rendering to the buffer. */
-	uint32_t last_rendering_seqno;
 	struct intel_ring_buffer *ring;
 
+	/** Breadcrumb of last rendering to the buffer. */
+	uint32_t last_rendering_seqno;
 	/** Breadcrumb of last fenced GPU access to the buffer. */
 	uint32_t last_fenced_seqno;
-	struct intel_ring_buffer *last_fenced_ring;
 
 	/** Current tiling stride for the object, if it's tiled. */
 	uint32_t stride;
@@ -918,13 +969,6 @@ struct drm_i915_gem_object {
 	/** Record of address bit 17 of each page at last unbind. */
 	unsigned long *bit_17;
 
-
-	/**
-	 * If present, while GEM_DOMAIN_CPU is in the read domain this array
-	 * flags which individual pages are valid.
-	 */
-	uint8_t *page_cpu_valid;
-
 	/** User space pin count and filp owning the pin */
 	uint32_t user_pin_count;
 	struct drm_file *pin_filp;
@@ -1001,6 +1045,8 @@ struct drm_i915_file_private {
 #define IS_IRONLAKE_D(dev)	((dev)->pci_device == 0x0042)
 #define IS_IRONLAKE_M(dev)	((dev)->pci_device == 0x0046)
 #define IS_IVYBRIDGE(dev)	(INTEL_INFO(dev)->is_ivybridge)
+#define IS_VALLEYVIEW(dev)	(INTEL_INFO(dev)->is_valleyview)
+#define IS_HASWELL(dev)	(INTEL_INFO(dev)->is_haswell)
 #define IS_MOBILE(dev)		(INTEL_INFO(dev)->is_mobile)
 
 /*
@@ -1044,10 +1090,11 @@ struct drm_i915_file_private {
 #define HAS_PIPE_CXSR(dev) (INTEL_INFO(dev)->has_pipe_cxsr)
 #define I915_HAS_FBC(dev) (INTEL_INFO(dev)->has_fbc)
 
-#define HAS_PCH_SPLIT(dev) (IS_GEN5(dev) || IS_GEN6(dev) || IS_IVYBRIDGE(dev))
+#define HAS_PCH_SPLIT(dev) (INTEL_INFO(dev)->has_pch_split)
 #define HAS_PIPE_CONTROL(dev) (INTEL_INFO(dev)->gen >= 5)
 
 #define INTEL_PCH_TYPE(dev) (((struct drm_i915_private *)(dev)->dev_private)->pch_type)
+#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)
 
@@ -1081,6 +1128,7 @@ extern int i915_panel_ignore_lid __read_mostly;
 extern unsigned int i915_powersave __read_mostly;
 extern int i915_semaphores __read_mostly;
 extern unsigned int i915_lvds_downclock __read_mostly;
+extern int i915_lvds_channel_mode __read_mostly;
 extern int i915_panel_use_ssc __read_mostly;
 extern int i915_vbt_sdvo_panel_type __read_mostly;
 extern int i915_enable_rc6 __read_mostly;
@@ -1094,6 +1142,7 @@ extern int i915_master_create(struct drm_device *dev, struct drm_master *master)
 extern void i915_master_destroy(struct drm_device *dev, struct drm_master *master);
 
 				/* i915_dma.c */
+void i915_update_dri1_breadcrumb(struct drm_device *dev);
 extern void i915_kernel_lost_context(struct drm_device * dev);
 extern int i915_driver_load(struct drm_device *, unsigned long flags);
 extern int i915_driver_unload(struct drm_device *);
@@ -1104,12 +1153,14 @@ extern void i915_driver_preclose(struct drm_device *dev,
 extern void i915_driver_postclose(struct drm_device *dev,
 				  struct drm_file *file_priv);
 extern int i915_driver_device_is_agp(struct drm_device * dev);
+#ifdef CONFIG_COMPAT
 extern long i915_compat_ioctl(struct file *filp, unsigned int cmd,
 			      unsigned long arg);
+#endif
 extern int i915_emit_box(struct drm_device *dev,
 			 struct drm_clip_rect *box,
 			 int DR1, int DR4);
-extern int i915_reset(struct drm_device *dev, u8 flags);
+extern int i915_reset(struct drm_device *dev);
 extern unsigned long i915_chipset_val(struct drm_i915_private *dev_priv);
 extern unsigned long i915_mch_val(struct drm_i915_private *dev_priv);
 extern unsigned long i915_gfx_val(struct drm_i915_private *dev_priv);
@@ -1119,19 +1170,10 @@ extern void i915_update_gfx_val(struct drm_i915_private *dev_priv);
 /* i915_irq.c */
 void i915_hangcheck_elapsed(unsigned long data);
 void i915_handle_error(struct drm_device *dev, bool wedged);
-extern int i915_irq_emit(struct drm_device *dev, void *data,
-			 struct drm_file *file_priv);
-extern int i915_irq_wait(struct drm_device *dev, void *data,
-			 struct drm_file *file_priv);
 
 extern void intel_irq_init(struct drm_device *dev);
 
-extern int i915_vblank_pipe_set(struct drm_device *dev, void *data,
-				struct drm_file *file_priv);
-extern int i915_vblank_pipe_get(struct drm_device *dev, void *data,
-				struct drm_file *file_priv);
-extern int i915_vblank_swap(struct drm_device *dev, void *data,
-			    struct drm_file *file_priv);
+void i915_error_state_free(struct kref *error_ref);
 
 void
 i915_enable_pipestat(drm_i915_private_t *dev_priv, int pipe, u32 mask);
@@ -1205,8 +1247,12 @@ int __must_check i915_gem_object_unbind(struct drm_i915_gem_object *obj);
 void i915_gem_release_mmap(struct drm_i915_gem_object *obj);
 void i915_gem_lastclose(struct drm_device *dev);
 
+int i915_gem_object_get_pages_gtt(struct drm_i915_gem_object *obj,
+				  gfp_t gfpmask);
 int __must_check i915_mutex_lock_interruptible(struct drm_device *dev);
 int __must_check i915_gem_object_wait_rendering(struct drm_i915_gem_object *obj);
+int i915_gem_object_sync(struct drm_i915_gem_object *obj,
+			 struct intel_ring_buffer *to);
 void i915_gem_object_move_to_active(struct drm_i915_gem_object *obj,
 				    struct intel_ring_buffer *ring,
 				    u32 seqno);
@@ -1229,17 +1275,18 @@ i915_seqno_passed(uint32_t seq1, uint32_t seq2)
 
 u32 i915_gem_next_request_seqno(struct intel_ring_buffer *ring);
 
-int __must_check i915_gem_object_get_fence(struct drm_i915_gem_object *obj,
-					   struct intel_ring_buffer *pipelined);
+int __must_check i915_gem_object_get_fence(struct drm_i915_gem_object *obj);
 int __must_check i915_gem_object_put_fence(struct drm_i915_gem_object *obj);
 
-static inline void
+static inline bool
 i915_gem_object_pin_fence(struct drm_i915_gem_object *obj)
 {
 	if (obj->fence_reg != I915_FENCE_REG_NONE) {
 		struct drm_i915_private *dev_priv = obj->base.dev->dev_private;
 		dev_priv->fence_regs[obj->fence_reg].pin_count++;
-	}
+		return true;
+	} else
+		return false;
 }
 
 static inline void
@@ -1260,27 +1307,25 @@ int __must_check i915_gem_object_set_domain(struct drm_i915_gem_object *obj,
 					    uint32_t read_domains,
 					    uint32_t write_domain);
 int __must_check i915_gem_object_finish_gpu(struct drm_i915_gem_object *obj);
+int __must_check i915_gem_init(struct drm_device *dev);
 int __must_check i915_gem_init_hw(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);
-void i915_gem_do_init(struct drm_device *dev,
-		      unsigned long start,
-		      unsigned long mappable_end,
-		      unsigned long end);
-int __must_check i915_gpu_idle(struct drm_device *dev, bool do_retire);
+int __must_check i915_gpu_idle(struct drm_device *dev);
 int __must_check i915_gem_idle(struct drm_device *dev);
 int __must_check i915_add_request(struct intel_ring_buffer *ring,
 				  struct drm_file *file,
 				  struct drm_i915_gem_request *request);
 int __must_check i915_wait_request(struct intel_ring_buffer *ring,
-				   uint32_t seqno,
-				   bool do_retire);
+				   uint32_t seqno);
 int i915_gem_fault(struct vm_area_struct *vma, struct vm_fault *vmf);
 int __must_check
 i915_gem_object_set_to_gtt_domain(struct drm_i915_gem_object *obj,
 				  bool write);
 int __must_check
+i915_gem_object_set_to_cpu_domain(struct drm_i915_gem_object *obj, bool write);
+int __must_check
 i915_gem_object_pin_to_display_plane(struct drm_i915_gem_object *obj,
 				     u32 alignment,
 				     struct intel_ring_buffer *pipelined);
@@ -1301,6 +1346,13 @@ i915_gem_get_unfenced_gtt_alignment(struct drm_device *dev,
 int i915_gem_object_set_cache_level(struct drm_i915_gem_object *obj,
 				    enum i915_cache_level cache_level);
 
+struct drm_gem_object *i915_gem_prime_import(struct drm_device *dev,
+				struct dma_buf *dma_buf);
+
+struct dma_buf *i915_gem_prime_export(struct drm_device *dev,
+				struct drm_gem_object *gem_obj, int flags);
+
+
 /* i915_gem_gtt.c */
 int __must_check i915_gem_init_aliasing_ppgtt(struct drm_device *dev);
 void i915_gem_cleanup_aliasing_ppgtt(struct drm_device *dev);
@@ -1311,18 +1363,24 @@ void i915_ppgtt_unbind_object(struct i915_hw_ppgtt *ppgtt,
 			      struct drm_i915_gem_object *obj);
 
 void i915_gem_restore_gtt_mappings(struct drm_device *dev);
-int __must_check i915_gem_gtt_bind_object(struct drm_i915_gem_object *obj);
-void i915_gem_gtt_rebind_object(struct drm_i915_gem_object *obj,
+int __must_check i915_gem_gtt_prepare_object(struct drm_i915_gem_object *obj);
+void i915_gem_gtt_bind_object(struct drm_i915_gem_object *obj,
 				enum i915_cache_level cache_level);
 void i915_gem_gtt_unbind_object(struct drm_i915_gem_object *obj);
+void i915_gem_gtt_finish_object(struct drm_i915_gem_object *obj);
+void i915_gem_init_global_gtt(struct drm_device *dev,
+			      unsigned long start,
+			      unsigned long mappable_end,
+			      unsigned long end);
 
 /* i915_gem_evict.c */
 int __must_check i915_gem_evict_something(struct drm_device *dev, int min_size,
 					  unsigned alignment, bool mappable);
-int __must_check i915_gem_evict_everything(struct drm_device *dev,
-					   bool purgeable_only);
-int __must_check i915_gem_evict_inactive(struct drm_device *dev,
-					 bool purgeable_only);
+int i915_gem_evict_everything(struct drm_device *dev, bool purgeable_only);
+
+/* i915_gem_stolen.c */
+int i915_gem_init_stolen(struct drm_device *dev);
+void i915_gem_cleanup_stolen(struct drm_device *dev);
 
 /* i915_gem_tiling.c */
 void i915_gem_detect_bit_6_swizzle(struct drm_device *dev);
@@ -1354,9 +1412,20 @@ extern int i915_restore_state(struct drm_device *dev);
 extern int i915_save_state(struct drm_device *dev);
 extern int i915_restore_state(struct drm_device *dev);
 
+/* i915_sysfs.c */
+void i915_setup_sysfs(struct drm_device *dev_priv);
+void i915_teardown_sysfs(struct drm_device *dev_priv);
+
 /* intel_i2c.c */
 extern int intel_setup_gmbus(struct drm_device *dev);
 extern void intel_teardown_gmbus(struct drm_device *dev);
+extern inline bool intel_gmbus_is_port_valid(unsigned port)
+{
+	return (port >= GMBUS_PORT_SSC && port <= GMBUS_PORT_DPD);
+}
+
+extern struct i2c_adapter *intel_gmbus_get_adapter(
+		struct drm_i915_private *dev_priv, unsigned port);
 extern void intel_gmbus_set_speed(struct i2c_adapter *adapter, int speed);
 extern void intel_gmbus_force_bit(struct i2c_adapter *adapter, bool force_bit);
 extern inline bool intel_gmbus_is_forced_bit(struct i2c_adapter *adapter)
@@ -1391,6 +1460,7 @@ static inline void intel_unregister_dsm_handler(void) { return; }
 #endif /* CONFIG_ACPI */
 
 /* modesetting */
+extern void intel_modeset_init_hw(struct drm_device *dev);
 extern void intel_modeset_init(struct drm_device *dev);
 extern void intel_modeset_gem_init(struct drm_device *dev);
 extern void intel_modeset_cleanup(struct drm_device *dev);
@@ -1403,12 +1473,17 @@ extern void ironlake_enable_rc6(struct drm_device *dev);
 extern void gen6_set_rps(struct drm_device *dev, u8 val);
 extern void intel_detect_pch(struct drm_device *dev);
 extern int intel_trans_dp_port_sel(struct drm_crtc *crtc);
+extern int intel_enable_rc6(const struct drm_device *dev);
 
+extern bool i915_semaphore_is_enabled(struct drm_device *dev);
 extern void __gen6_gt_force_wake_get(struct drm_i915_private *dev_priv);
 extern void __gen6_gt_force_wake_mt_get(struct drm_i915_private *dev_priv);
 extern void __gen6_gt_force_wake_put(struct drm_i915_private *dev_priv);
 extern void __gen6_gt_force_wake_mt_put(struct drm_i915_private *dev_priv);
 
+extern void vlv_force_wake_get(struct drm_i915_private *dev_priv);
+extern void vlv_force_wake_put(struct drm_i915_private *dev_priv);
+
 /* overlay */
 #ifdef CONFIG_DEBUG_FS
 extern struct intel_overlay_error_state *intel_overlay_capture_error_state(struct drm_device *dev);
@@ -1420,28 +1495,6 @@ extern void intel_display_print_error_state(struct seq_file *m,
 					    struct intel_display_error_state *error);
 #endif
 
-#define LP_RING(d) (&((struct drm_i915_private *)(d))->ring[RCS])
-
-#define BEGIN_LP_RING(n) \
-	intel_ring_begin(LP_RING(dev_priv), (n))
-
-#define OUT_RING(x) \
-	intel_ring_emit(LP_RING(dev_priv), x)
-
-#define ADVANCE_LP_RING() \
-	intel_ring_advance(LP_RING(dev_priv))
-
-/**
- * Lock test for when it's just for synchronization of ring access.
- *
- * In that case, we don't need to do it when GEM is initialized as nobody else
- * has access to the ring.
- */
-#define RING_LOCK_TEST_WITH_RETURN(dev, file) do {			\
-	if (LP_RING(dev->dev_private)->obj == NULL)			\
-		LOCK_TEST_WITH_RETURN(dev, file);			\
-} while (0)
-
 /* On SNB platform, before reading ring registers forcewake bit
  * must be set to prevent GT core from power down and stale values being
  * returned.
@@ -1450,12 +1503,6 @@ void gen6_gt_force_wake_get(struct drm_i915_private *dev_priv);
 void gen6_gt_force_wake_put(struct drm_i915_private *dev_priv);
 int __gen6_gt_wait_for_fifo(struct drm_i915_private *dev_priv);
 
-/* We give fast paths for the really cool registers */
-#define NEEDS_FORCE_WAKE(dev_priv, reg) \
-	(((dev_priv)->info->gen >= 6) && \
-	 ((reg) < 0x40000) &&		 \
-	 ((reg) != FORCEWAKE))
-
 #define __i915_read(x, y) \
 	u##x i915_read##x(struct drm_i915_private *dev_priv, u32 reg);
 
diff --git a/drivers/gpu/drm/i915/i915_gem.c b/drivers/gpu/drm/i915/i915_gem.c
index 0d1e4b7b4b9..c1e5c66553d 100644
--- a/drivers/gpu/drm/i915/i915_gem.c
+++ b/drivers/gpu/drm/i915/i915_gem.c
@@ -35,31 +35,41 @@
 #include <linux/slab.h>
 #include <linux/swap.h>
 #include <linux/pci.h>
+#include <linux/dma-buf.h>
 
 static __must_check int i915_gem_object_flush_gpu_write_domain(struct drm_i915_gem_object *obj);
 static void i915_gem_object_flush_gtt_write_domain(struct drm_i915_gem_object *obj);
 static void i915_gem_object_flush_cpu_write_domain(struct drm_i915_gem_object *obj);
-static __must_check int i915_gem_object_set_to_cpu_domain(struct drm_i915_gem_object *obj,
-							  bool write);
-static __must_check int i915_gem_object_set_cpu_read_domain_range(struct drm_i915_gem_object *obj,
-								  uint64_t offset,
-								  uint64_t size);
-static void i915_gem_object_set_to_full_cpu_read_domain(struct drm_i915_gem_object *obj);
 static __must_check int i915_gem_object_bind_to_gtt(struct drm_i915_gem_object *obj,
 						    unsigned alignment,
 						    bool map_and_fenceable);
-static void i915_gem_clear_fence_reg(struct drm_device *dev,
-				     struct drm_i915_fence_reg *reg);
 static int i915_gem_phys_pwrite(struct drm_device *dev,
 				struct drm_i915_gem_object *obj,
 				struct drm_i915_gem_pwrite *args,
 				struct drm_file *file);
-static void i915_gem_free_object_tail(struct drm_i915_gem_object *obj);
+
+static void i915_gem_write_fence(struct drm_device *dev, int reg,
+				 struct drm_i915_gem_object *obj);
+static void i915_gem_object_update_fence(struct drm_i915_gem_object *obj,
+					 struct drm_i915_fence_reg *fence,
+					 bool enable);
 
 static int i915_gem_inactive_shrink(struct shrinker *shrinker,
 				    struct shrink_control *sc);
 static void i915_gem_object_truncate(struct drm_i915_gem_object *obj);
 
+static inline void i915_gem_object_fence_lost(struct drm_i915_gem_object *obj)
+{
+	if (obj->tiling_mode)
+		i915_gem_release_mmap(obj);
+
+	/* As we do not have an associated fence register, we will force
+	 * a tiling change if we ever need to acquire one.
+	 */
+	obj->fence_dirty = false;
+	obj->fence_reg = I915_FENCE_REG_NONE;
+}
+
 /* some bookkeeping */
 static void i915_gem_info_add_obj(struct drm_i915_private *dev_priv,
 				  size_t size)
@@ -122,26 +132,7 @@ int i915_mutex_lock_interruptible(struct drm_device *dev)
 static inline bool
 i915_gem_object_is_inactive(struct drm_i915_gem_object *obj)
 {
-	return obj->gtt_space && !obj->active && obj->pin_count == 0;
-}
-
-void i915_gem_do_init(struct drm_device *dev,
-		      unsigned long start,
-		      unsigned long mappable_end,
-		      unsigned long end)
-{
-	drm_i915_private_t *dev_priv = dev->dev_private;
-
-	drm_mm_init(&dev_priv->mm.gtt_space, start, end - start);
-
-	dev_priv->mm.gtt_start = start;
-	dev_priv->mm.gtt_mappable_end = mappable_end;
-	dev_priv->mm.gtt_end = end;
-	dev_priv->mm.gtt_total = end - start;
-	dev_priv->mm.mappable_gtt_total = min(end, mappable_end) - start;
-
-	/* Take over this portion of the GTT */
-	intel_gtt_clear_range(start / PAGE_SIZE, (end-start) / PAGE_SIZE);
+	return !obj->active;
 }
 
 int
@@ -150,12 +141,20 @@ i915_gem_init_ioctl(struct drm_device *dev, void *data,
 {
 	struct drm_i915_gem_init *args = data;
 
+	if (drm_core_check_feature(dev, DRIVER_MODESET))
+		return -ENODEV;
+
 	if (args->gtt_start >= args->gtt_end ||
 	    (args->gtt_end | args->gtt_start) & (PAGE_SIZE - 1))
 		return -EINVAL;
 
+	/* GEM with user mode setting was never supported on ilk and later. */
+	if (INTEL_INFO(dev)->gen >= 5)
+		return -ENODEV;
+
 	mutex_lock(&dev->struct_mutex);
-	i915_gem_do_init(dev, args->gtt_start, args->gtt_end, args->gtt_end);
+	i915_gem_init_global_gtt(dev, args->gtt_start,
+				 args->gtt_end, args->gtt_end);
 	mutex_unlock(&dev->struct_mutex);
 
 	return 0;
@@ -170,13 +169,11 @@ i915_gem_get_aperture_ioctl(struct drm_device *dev, void *data,
 	struct drm_i915_gem_object *obj;
 	size_t pinned;
 
-	if (!(dev->driver->driver_features & DRIVER_GEM))
-		return -ENODEV;
-
 	pinned = 0;
 	mutex_lock(&dev->struct_mutex);
-	list_for_each_entry(obj, &dev_priv->mm.pinned_list, mm_list)
-		pinned += obj->gtt_space->size;
+	list_for_each_entry(obj, &dev_priv->mm.gtt_list, gtt_list)
+		if (obj->pin_count)
+			pinned += obj->gtt_space->size;
 	mutex_unlock(&dev->struct_mutex);
 
 	args->aper_size = dev_priv->mm.gtt_total;
@@ -247,6 +244,7 @@ i915_gem_create_ioctl(struct drm_device *dev, void *data,
 		      struct drm_file *file)
 {
 	struct drm_i915_gem_create *args = data;
+
 	return i915_gem_create(file, dev,
 			       args->size, &args->handle);
 }
@@ -259,66 +257,6 @@ static int i915_gem_object_needs_bit17_swizzle(struct drm_i915_gem_object *obj)
 		obj->tiling_mode != I915_TILING_NONE;
 }
 
-/**
- * This is the fast shmem pread path, which attempts to copy_from_user directly
- * from the backing pages of the object to the user's address space.  On a
- * fault, it fails so we can fall back to i915_gem_shmem_pwrite_slow().
- */
-static int
-i915_gem_shmem_pread_fast(struct drm_device *dev,
-			  struct drm_i915_gem_object *obj,
-			  struct drm_i915_gem_pread *args,
-			  struct drm_file *file)
-{
-	struct address_space *mapping = obj->base.filp->f_path.dentry->d_inode->i_mapping;
-	ssize_t remain;
-	loff_t offset;
-	char __user *user_data;
-	int page_offset, page_length;
-
-	user_data = (char __user *) (uintptr_t) args->data_ptr;
-	remain = args->size;
-
-	offset = args->offset;
-
-	while (remain > 0) {
-		struct page *page;
-		char *vaddr;
-		int ret;
-
-		/* Operation in this page
-		 *
-		 * page_offset = offset within page
-		 * page_length = bytes to copy for this page
-		 */
-		page_offset = offset_in_page(offset);
-		page_length = remain;
-		if ((page_offset + remain) > PAGE_SIZE)
-			page_length = PAGE_SIZE - page_offset;
-
-		page = shmem_read_mapping_page(mapping, offset >> PAGE_SHIFT);
-		if (IS_ERR(page))
-			return PTR_ERR(page);
-
-		vaddr = kmap_atomic(page);
-		ret = __copy_to_user_inatomic(user_data,
-					      vaddr + page_offset,
-					      page_length);
-		kunmap_atomic(vaddr);
-
-		mark_page_accessed(page);
-		page_cache_release(page);
-		if (ret)
-			return -EFAULT;
-
-		remain -= page_length;
-		user_data += page_length;
-		offset += page_length;
-	}
-
-	return 0;
-}
-
 static inline int
 __copy_to_user_swizzled(char __user *cpu_vaddr,
 			const char *gpu_vaddr, int gpu_offset,
@@ -346,8 +284,8 @@ __copy_to_user_swizzled(char __user *cpu_vaddr,
 }
 
 static inline int
-__copy_from_user_swizzled(char __user *gpu_vaddr, int gpu_offset,
-			  const char *cpu_vaddr,
+__copy_from_user_swizzled(char *gpu_vaddr, int gpu_offset,
+			  const char __user *cpu_vaddr,
 			  int length)
 {
 	int ret, cpu_offset = 0;
@@ -371,37 +309,121 @@ __copy_from_user_swizzled(char __user *gpu_vaddr, int gpu_offset,
 	return 0;
 }
 
-/**
- * This is the fallback shmem pread path, which allocates temporary storage
- * in kernel space to copy_to_user into outside of the struct_mutex, so we
- * can copy out of the object's backing pages while holding the struct mutex
- * and not take page faults.
- */
+/* Per-page copy function for the shmem pread fastpath.
+ * Flushes invalid cachelines before reading the target if
+ * needs_clflush is set. */
 static int
-i915_gem_shmem_pread_slow(struct drm_device *dev,
-			  struct drm_i915_gem_object *obj,
-			  struct drm_i915_gem_pread *args,
-			  struct drm_file *file)
+shmem_pread_fast(struct page *page, int shmem_page_offset, int page_length,
+		 char __user *user_data,
+		 bool page_do_bit17_swizzling, bool needs_clflush)
+{
+	char *vaddr;
+	int ret;
+
+	if (unlikely(page_do_bit17_swizzling))
+		return -EINVAL;
+
+	vaddr = kmap_atomic(page);
+	if (needs_clflush)
+		drm_clflush_virt_range(vaddr + shmem_page_offset,
+				       page_length);
+	ret = __copy_to_user_inatomic(user_data,
+				      vaddr + shmem_page_offset,
+				      page_length);
+	kunmap_atomic(vaddr);
+
+	return ret;
+}
+
+static void
+shmem_clflush_swizzled_range(char *addr, unsigned long length,
+			     bool swizzled)
+{
+	if (unlikely(swizzled)) {
+		unsigned long start = (unsigned long) addr;
+		unsigned long end = (unsigned long) addr + length;
+
+		/* For swizzling simply ensure that we always flush both
+		 * channels. Lame, but simple and it works. Swizzled
+		 * pwrite/pread is far from a hotpath - current userspace
+		 * doesn't use it at all. */
+		start = round_down(start, 128);
+		end = round_up(end, 128);
+
+		drm_clflush_virt_range((void *)start, end - start);
+	} else {
+		drm_clflush_virt_range(addr, length);
+	}
+
+}
+
+/* Only difference to the fast-path function is that this can handle bit17
+ * and uses non-atomic copy and kmap functions. */
+static int
+shmem_pread_slow(struct page *page, int shmem_page_offset, int page_length,
+		 char __user *user_data,
+		 bool page_do_bit17_swizzling, bool needs_clflush)
+{
+	char *vaddr;
+	int ret;
+
+	vaddr = kmap(page);
+	if (needs_clflush)
+		shmem_clflush_swizzled_range(vaddr + shmem_page_offset,
+					     page_length,
+					     page_do_bit17_swizzling);
+
+	if (page_do_bit17_swizzling)
+		ret = __copy_to_user_swizzled(user_data,
+					      vaddr, shmem_page_offset,
+					      page_length);
+	else
+		ret = __copy_to_user(user_data,
+				     vaddr + shmem_page_offset,
+				     page_length);
+	kunmap(page);
+
+	return ret;
+}
+
+static int
+i915_gem_shmem_pread(struct drm_device *dev,
+		     struct drm_i915_gem_object *obj,
+		     struct drm_i915_gem_pread *args,
+		     struct drm_file *file)
 {
 	struct address_space *mapping = obj->base.filp->f_path.dentry->d_inode->i_mapping;
 	char __user *user_data;
 	ssize_t remain;
 	loff_t offset;
-	int shmem_page_offset, page_length, ret;
+	int shmem_page_offset, page_length, ret = 0;
 	int obj_do_bit17_swizzling, page_do_bit17_swizzling;
+	int hit_slowpath = 0;
+	int prefaulted = 0;
+	int needs_clflush = 0;
+	int release_page;
 
 	user_data = (char __user *) (uintptr_t) args->data_ptr;
 	remain = args->size;
 
 	obj_do_bit17_swizzling = i915_gem_object_needs_bit17_swizzle(obj);
 
-	offset = args->offset;
+	if (!(obj->base.read_domains & I915_GEM_DOMAIN_CPU)) {
+		/* If we're not in the cpu read domain, set ourself into the gtt
+		 * read domain and manually flush cachelines (if required). This
+		 * optimizes for the case when the gpu will dirty the data
+		 * anyway again before the next pread happens. */
+		if (obj->cache_level == I915_CACHE_NONE)
+			needs_clflush = 1;
+		ret = i915_gem_object_set_to_gtt_domain(obj, false);
+		if (ret)
+			return ret;
+	}
 
-	mutex_unlock(&dev->struct_mutex);
+	offset = args->offset;
 
 	while (remain > 0) {
 		struct page *page;
-		char *vaddr;
 
 		/* Operation in this page
 		 *
@@ -413,28 +435,51 @@ i915_gem_shmem_pread_slow(struct drm_device *dev,
 		if ((shmem_page_offset + page_length) > PAGE_SIZE)
 			page_length = PAGE_SIZE - shmem_page_offset;
 
-		page = shmem_read_mapping_page(mapping, offset >> PAGE_SHIFT);
-		if (IS_ERR(page)) {
-			ret = PTR_ERR(page);
-			goto out;
+		if (obj->pages) {
+			page = obj->pages[offset >> PAGE_SHIFT];
+			release_page = 0;
+		} else {
+			page = shmem_read_mapping_page(mapping, offset >> PAGE_SHIFT);
+			if (IS_ERR(page)) {
+				ret = PTR_ERR(page);
+				goto out;
+			}
+			release_page = 1;
 		}
 
 		page_do_bit17_swizzling = obj_do_bit17_swizzling &&
 			(page_to_phys(page) & (1 << 17)) != 0;
 
-		vaddr = kmap(page);
-		if (page_do_bit17_swizzling)
-			ret = __copy_to_user_swizzled(user_data,
-						      vaddr, shmem_page_offset,
-						      page_length);
-		else
-			ret = __copy_to_user(user_data,
-					     vaddr + shmem_page_offset,
-					     page_length);
-		kunmap(page);
+		ret = shmem_pread_fast(page, shmem_page_offset, page_length,
+				       user_data, page_do_bit17_swizzling,
+				       needs_clflush);
+		if (ret == 0)
+			goto next_page;
 
-		mark_page_accessed(page);
+		hit_slowpath = 1;
+		page_cache_get(page);
+		mutex_unlock(&dev->struct_mutex);
+
+		if (!prefaulted) {
+			ret = fault_in_multipages_writeable(user_data, remain);
+			/* Userspace is tricking us, but we've already clobbered
+			 * its pages with the prefault and promised to write the
+			 * data up to the first fault. Hence ignore any errors
+			 * and just continue. */
+			(void)ret;
+			prefaulted = 1;
+		}
+
+		ret = shmem_pread_slow(page, shmem_page_offset, page_length,
+				       user_data, page_do_bit17_swizzling,
+				       needs_clflush);
+
+		mutex_lock(&dev->struct_mutex);
 		page_cache_release(page);
+next_page:
+		mark_page_accessed(page);
+		if (release_page)
+			page_cache_release(page);
 
 		if (ret) {
 			ret = -EFAULT;
@@ -447,10 +492,11 @@ i915_gem_shmem_pread_slow(struct drm_device *dev,
 	}
 
 out:
-	mutex_lock(&dev->struct_mutex);
-	/* Fixup: Kill any reinstated backing storage pages */
-	if (obj->madv == __I915_MADV_PURGED)
-		i915_gem_object_truncate(obj);
+	if (hit_slowpath) {
+		/* Fixup: Kill any reinstated backing storage pages */
+		if (obj->madv == __I915_MADV_PURGED)
+			i915_gem_object_truncate(obj);
+	}
 
 	return ret;
 }
@@ -476,11 +522,6 @@ i915_gem_pread_ioctl(struct drm_device *dev, void *data,
 		       args->size))
 		return -EFAULT;
 
-	ret = fault_in_pages_writeable((char __user *)(uintptr_t)args->data_ptr,
-				       args->size);
-	if (ret)
-		return -EFAULT;
-
 	ret = i915_mutex_lock_interruptible(dev);
 	if (ret)
 		return ret;
@@ -498,19 +539,17 @@ i915_gem_pread_ioctl(struct drm_device *dev, void *data,
 		goto out;
 	}
 
-	trace_i915_gem_object_pread(obj, args->offset, args->size);
-
-	ret = i915_gem_object_set_cpu_read_domain_range(obj,
-							args->offset,
-							args->size);
-	if (ret)
+	/* prime objects have no backing filp to GEM pread/pwrite
+	 * pages from.
+	 */
+	if (!obj->base.filp) {
+		ret = -EINVAL;
 		goto out;
+	}
 
-	ret = -EFAULT;
-	if (!i915_gem_object_needs_bit17_swizzle(obj))
-		ret = i915_gem_shmem_pread_fast(dev, obj, args, file);
-	if (ret == -EFAULT)
-		ret = i915_gem_shmem_pread_slow(dev, obj, args, file);
+	trace_i915_gem_object_pread(obj, args->offset, args->size);
+
+	ret = i915_gem_shmem_pread(dev, obj, args, file);
 
 out:
 	drm_gem_object_unreference(&obj->base);
@@ -529,40 +568,19 @@ fast_user_write(struct io_mapping *mapping,
 		char __user *user_data,
 		int length)
 {
-	char *vaddr_atomic;
+	void __iomem *vaddr_atomic;
+	void *vaddr;
 	unsigned long unwritten;
 
 	vaddr_atomic = io_mapping_map_atomic_wc(mapping, page_base);
-	unwritten = __copy_from_user_inatomic_nocache(vaddr_atomic + page_offset,
+	/* We can use the cpu mem copy function because this is X86. */
+	vaddr = (void __force*)vaddr_atomic + page_offset;
+	unwritten = __copy_from_user_inatomic_nocache(vaddr,
 						      user_data, length);
 	io_mapping_unmap_atomic(vaddr_atomic);
 	return unwritten;
 }
 
-/* Here's the write path which can sleep for
- * page faults
- */
-
-static inline void
-slow_kernel_write(struct io_mapping *mapping,
-		  loff_t gtt_base, int gtt_offset,
-		  struct page *user_page, int user_offset,
-		  int length)
-{
-	char __iomem *dst_vaddr;
-	char *src_vaddr;
-
-	dst_vaddr = io_mapping_map_wc(mapping, gtt_base);
-	src_vaddr = kmap(user_page);
-
-	memcpy_toio(dst_vaddr + gtt_offset,
-		    src_vaddr + user_offset,
-		    length);
-
-	kunmap(user_page);
-	io_mapping_unmap(dst_vaddr);
-}
-
 /**
  * This is the fast pwrite path, where we copy the data directly from the
  * user into the GTT, uncached.
@@ -577,7 +595,19 @@ i915_gem_gtt_pwrite_fast(struct drm_device *dev,
 	ssize_t remain;
 	loff_t offset, page_base;
 	char __user *user_data;
-	int page_offset, page_length;
+	int page_offset, page_length, ret;
+
+	ret = i915_gem_object_pin(obj, 0, true);
+	if (ret)
+		goto out;
+
+	ret = i915_gem_object_set_to_gtt_domain(obj, true);
+	if (ret)
+		goto out_unpin;
+
+	ret = i915_gem_object_put_fence(obj);
+	if (ret)
+		goto out_unpin;
 
 	user_data = (char __user *) (uintptr_t) args->data_ptr;
 	remain = args->size;
@@ -602,214 +632,133 @@ i915_gem_gtt_pwrite_fast(struct drm_device *dev,
 		 * retry in the slow path.
 		 */
 		if (fast_user_write(dev_priv->mm.gtt_mapping, page_base,
-				    page_offset, user_data, page_length))
-			return -EFAULT;
+				    page_offset, user_data, page_length)) {
+			ret = -EFAULT;
+			goto out_unpin;
+		}
 
 		remain -= page_length;
 		user_data += page_length;
 		offset += page_length;
 	}
 
-	return 0;
+out_unpin:
+	i915_gem_object_unpin(obj);
+out:
+	return ret;
 }
 
-/**
- * This is the fallback GTT pwrite path, which uses get_user_pages to pin
- * the memory and maps it using kmap_atomic for copying.
- *
- * This code resulted in x11perf -rgb10text consuming about 10% more CPU
- * than using i915_gem_gtt_pwrite_fast on a G45 (32-bit).
- */
+/* Per-page copy function for the shmem pwrite fastpath.
+ * Flushes invalid cachelines before writing to the target if
+ * needs_clflush_before is set and flushes out any written cachelines after
+ * writing if needs_clflush is set. */
 static int
-i915_gem_gtt_pwrite_slow(struct drm_device *dev,
-			 struct drm_i915_gem_object *obj,
-			 struct drm_i915_gem_pwrite *args,
-			 struct drm_file *file)
+shmem_pwrite_fast(struct page *page, int shmem_page_offset, int page_length,
+		  char __user *user_data,
+		  bool page_do_bit17_swizzling,
+		  bool needs_clflush_before,
+		  bool needs_clflush_after)
 {
-	drm_i915_private_t *dev_priv = dev->dev_private;
-	ssize_t remain;
-	loff_t gtt_page_base, offset;
-	loff_t first_data_page, last_data_page, num_pages;
-	loff_t pinned_pages, i;
-	struct page **user_pages;
-	struct mm_struct *mm = current->mm;
-	int gtt_page_offset, data_page_offset, data_page_index, page_length;
+	char *vaddr;
 	int ret;
-	uint64_t data_ptr = args->data_ptr;
-
-	remain = args->size;
-
-	/* Pin the user pages containing the data.  We can't fault while
-	 * holding the struct mutex, and all of the pwrite implementations
-	 * want to hold it while dereferencing the user data.
-	 */
-	first_data_page = data_ptr / PAGE_SIZE;
-	last_data_page = (data_ptr + args->size - 1) / PAGE_SIZE;
-	num_pages = last_data_page - first_data_page + 1;
-
-	user_pages = drm_malloc_ab(num_pages, sizeof(struct page *));
-	if (user_pages == NULL)
-		return -ENOMEM;
-
-	mutex_unlock(&dev->struct_mutex);
-	down_read(&mm->mmap_sem);
-	pinned_pages = get_user_pages(current, mm, (uintptr_t)args->data_ptr,
-				      num_pages, 0, 0, user_pages, NULL);
-	up_read(&mm->mmap_sem);
-	mutex_lock(&dev->struct_mutex);
-	if (pinned_pages < num_pages) {
-		ret = -EFAULT;
-		goto out_unpin_pages;
-	}
-
-	ret = i915_gem_object_set_to_gtt_domain(obj, true);
-	if (ret)
-		goto out_unpin_pages;
-
-	ret = i915_gem_object_put_fence(obj);
-	if (ret)
-		goto out_unpin_pages;
-
-	offset = obj->gtt_offset + args->offset;
-
-	while (remain > 0) {
-		/* Operation in this page
-		 *
-		 * gtt_page_base = page offset within aperture
-		 * gtt_page_offset = offset within page in aperture
-		 * data_page_index = page number in get_user_pages return
-		 * data_page_offset = offset with data_page_index page.
-		 * page_length = bytes to copy for this page
-		 */
-		gtt_page_base = offset & PAGE_MASK;
-		gtt_page_offset = offset_in_page(offset);
-		data_page_index = data_ptr / PAGE_SIZE - first_data_page;
-		data_page_offset = offset_in_page(data_ptr);
-
-		page_length = remain;
-		if ((gtt_page_offset + page_length) > PAGE_SIZE)
-			page_length = PAGE_SIZE - gtt_page_offset;
-		if ((data_page_offset + page_length) > PAGE_SIZE)
-			page_length = PAGE_SIZE - data_page_offset;
 
-		slow_kernel_write(dev_priv->mm.gtt_mapping,
-				  gtt_page_base, gtt_page_offset,
-				  user_pages[data_page_index],
-				  data_page_offset,
-				  page_length);
-
-		remain -= page_length;
-		offset += page_length;
-		data_ptr += page_length;
-	}
+	if (unlikely(page_do_bit17_swizzling))
+		return -EINVAL;
 
-out_unpin_pages:
-	for (i = 0; i < pinned_pages; i++)
-		page_cache_release(user_pages[i]);
-	drm_free_large(user_pages);
+	vaddr = kmap_atomic(page);
+	if (needs_clflush_before)
+		drm_clflush_virt_range(vaddr + shmem_page_offset,
+				       page_length);
+	ret = __copy_from_user_inatomic_nocache(vaddr + shmem_page_offset,
+						user_data,
+						page_length);
+	if (needs_clflush_after)
+		drm_clflush_virt_range(vaddr + shmem_page_offset,
+				       page_length);
+	kunmap_atomic(vaddr);
 
 	return ret;
 }
 
-/**
- * This is the fast shmem pwrite path, which attempts to directly
- * copy_from_user into the kmapped pages backing the object.
- */
+/* Only difference to the fast-path function is that this can handle bit17
+ * and uses non-atomic copy and kmap functions. */
 static int
-i915_gem_shmem_pwrite_fast(struct drm_device *dev,
-			   struct drm_i915_gem_object *obj,
-			   struct drm_i915_gem_pwrite *args,
-			   struct drm_file *file)
+shmem_pwrite_slow(struct page *page, int shmem_page_offset, int page_length,
+		  char __user *user_data,
+		  bool page_do_bit17_swizzling,
+		  bool needs_clflush_before,
+		  bool needs_clflush_after)
 {
-	struct address_space *mapping = obj->base.filp->f_path.dentry->d_inode->i_mapping;
-	ssize_t remain;
-	loff_t offset;
-	char __user *user_data;
-	int page_offset, page_length;
-
-	user_data = (char __user *) (uintptr_t) args->data_ptr;
-	remain = args->size;
-
-	offset = args->offset;
-	obj->dirty = 1;
-
-	while (remain > 0) {
-		struct page *page;
-		char *vaddr;
-		int ret;
-
-		/* Operation in this page
-		 *
-		 * page_offset = offset within page
-		 * page_length = bytes to copy for this page
-		 */
-		page_offset = offset_in_page(offset);
-		page_length = remain;
-		if ((page_offset + remain) > PAGE_SIZE)
-			page_length = PAGE_SIZE - page_offset;
-
-		page = shmem_read_mapping_page(mapping, offset >> PAGE_SHIFT);
-		if (IS_ERR(page))
-			return PTR_ERR(page);
+	char *vaddr;
+	int ret;
 
-		vaddr = kmap_atomic(page);
-		ret = __copy_from_user_inatomic(vaddr + page_offset,
+	vaddr = kmap(page);
+	if (unlikely(needs_clflush_before || page_do_bit17_swizzling))
+		shmem_clflush_swizzled_range(vaddr + shmem_page_offset,
+					     page_length,
+					     page_do_bit17_swizzling);
+	if (page_do_bit17_swizzling)
+		ret = __copy_from_user_swizzled(vaddr, shmem_page_offset,
 						user_data,
 						page_length);
-		kunmap_atomic(vaddr);
-
-		set_page_dirty(page);
-		mark_page_accessed(page);
-		page_cache_release(page);
-
-		/* If we get a fault while copying data, then (presumably) our
-		 * source page isn't available.  Return the error and we'll
-		 * retry in the slow path.
-		 */
-		if (ret)
-			return -EFAULT;
-
-		remain -= page_length;
-		user_data += page_length;
-		offset += page_length;
-	}
+	else
+		ret = __copy_from_user(vaddr + shmem_page_offset,
+				       user_data,
+				       page_length);
+	if (needs_clflush_after)
+		shmem_clflush_swizzled_range(vaddr + shmem_page_offset,
+					     page_length,
+					     page_do_bit17_swizzling);
+	kunmap(page);
 
-	return 0;
+	return ret;
 }
 
-/**
- * This is the fallback shmem pwrite path, which uses get_user_pages to pin
- * the memory and maps it using kmap_atomic for copying.
- *
- * This avoids taking mmap_sem for faulting on the user's address while the
- * struct_mutex is held.
- */
 static int
-i915_gem_shmem_pwrite_slow(struct drm_device *dev,
-			   struct drm_i915_gem_object *obj,
-			   struct drm_i915_gem_pwrite *args,
-			   struct drm_file *file)
+i915_gem_shmem_pwrite(struct drm_device *dev,
+		      struct drm_i915_gem_object *obj,
+		      struct drm_i915_gem_pwrite *args,
+		      struct drm_file *file)
 {
 	struct address_space *mapping = obj->base.filp->f_path.dentry->d_inode->i_mapping;
 	ssize_t remain;
 	loff_t offset;
 	char __user *user_data;
-	int shmem_page_offset, page_length, ret;
+	int shmem_page_offset, page_length, ret = 0;
 	int obj_do_bit17_swizzling, page_do_bit17_swizzling;
+	int hit_slowpath = 0;
+	int needs_clflush_after = 0;
+	int needs_clflush_before = 0;
+	int release_page;
 
 	user_data = (char __user *) (uintptr_t) args->data_ptr;
 	remain = args->size;
 
 	obj_do_bit17_swizzling = i915_gem_object_needs_bit17_swizzle(obj);
 
+	if (obj->base.write_domain != I915_GEM_DOMAIN_CPU) {
+		/* If we're not in the cpu write domain, set ourself into the gtt
+		 * write domain and manually flush cachelines (if required). This
+		 * optimizes for the case when the gpu will use the data
+		 * right away and we therefore have to clflush anyway. */
+		if (obj->cache_level == I915_CACHE_NONE)
+			needs_clflush_after = 1;
+		ret = i915_gem_object_set_to_gtt_domain(obj, true);
+		if (ret)
+			return ret;
+	}
+	/* Same trick applies for invalidate partially written cachelines before
+	 * writing.  */
+	if (!(obj->base.read_domains & I915_GEM_DOMAIN_CPU)
+	    && obj->cache_level == I915_CACHE_NONE)
+		needs_clflush_before = 1;
+
 	offset = args->offset;
 	obj->dirty = 1;
 
-	mutex_unlock(&dev->struct_mutex);
-
 	while (remain > 0) {
 		struct page *page;
-		char *vaddr;
+		int partial_cacheline_write;
 
 		/* Operation in this page
 		 *
@@ -822,29 +771,51 @@ i915_gem_shmem_pwrite_slow(struct drm_device *dev,
 		if ((shmem_page_offset + page_length) > PAGE_SIZE)
 			page_length = PAGE_SIZE - shmem_page_offset;
 
-		page = shmem_read_mapping_page(mapping, offset >> PAGE_SHIFT);
-		if (IS_ERR(page)) {
-			ret = PTR_ERR(page);
-			goto out;
+		/* If we don't overwrite a cacheline completely we need to be
+		 * careful to have up-to-date data by first clflushing. Don't
+		 * overcomplicate things and flush the entire patch. */
+		partial_cacheline_write = needs_clflush_before &&
+			((shmem_page_offset | page_length)
+				& (boot_cpu_data.x86_clflush_size - 1));
+
+		if (obj->pages) {
+			page = obj->pages[offset >> PAGE_SHIFT];
+			release_page = 0;
+		} else {
+			page = shmem_read_mapping_page(mapping, offset >> PAGE_SHIFT);
+			if (IS_ERR(page)) {
+				ret = PTR_ERR(page);
+				goto out;
+			}
+			release_page = 1;
 		}
 
 		page_do_bit17_swizzling = obj_do_bit17_swizzling &&
 			(page_to_phys(page) & (1 << 17)) != 0;
 
-		vaddr = kmap(page);
-		if (page_do_bit17_swizzling)
-			ret = __copy_from_user_swizzled(vaddr, shmem_page_offset,
-							user_data,
-							page_length);
-		else
-			ret = __copy_from_user(vaddr + shmem_page_offset,
-					       user_data,
-					       page_length);
-		kunmap(page);
+		ret = shmem_pwrite_fast(page, shmem_page_offset, page_length,
+					user_data, page_do_bit17_swizzling,
+					partial_cacheline_write,
+					needs_clflush_after);
+		if (ret == 0)
+			goto next_page;
+
+		hit_slowpath = 1;
+		page_cache_get(page);
+		mutex_unlock(&dev->struct_mutex);
+
+		ret = shmem_pwrite_slow(page, shmem_page_offset, page_length,
+					user_data, page_do_bit17_swizzling,
+					partial_cacheline_write,
+					needs_clflush_after);
 
+		mutex_lock(&dev->struct_mutex);
+		page_cache_release(page);
+next_page:
 		set_page_dirty(page);
 		mark_page_accessed(page);
-		page_cache_release(page);
+		if (release_page)
+			page_cache_release(page);
 
 		if (ret) {
 			ret = -EFAULT;
@@ -857,17 +828,21 @@ i915_gem_shmem_pwrite_slow(struct drm_device *dev,
 	}
 
 out:
-	mutex_lock(&dev->struct_mutex);
-	/* Fixup: Kill any reinstated backing storage pages */
-	if (obj->madv == __I915_MADV_PURGED)
-		i915_gem_object_truncate(obj);
-	/* and flush dirty cachelines in case the object isn't in the cpu write
-	 * domain anymore. */
-	if (obj->base.write_domain != I915_GEM_DOMAIN_CPU) {
-		i915_gem_clflush_object(obj);
-		intel_gtt_chipset_flush();
+	if (hit_slowpath) {
+		/* Fixup: Kill any reinstated backing storage pages */
+		if (obj->madv == __I915_MADV_PURGED)
+			i915_gem_object_truncate(obj);
+		/* and flush dirty cachelines in case the object isn't in the cpu write
+		 * domain anymore. */
+		if (obj->base.write_domain != I915_GEM_DOMAIN_CPU) {
+			i915_gem_clflush_object(obj);
+			intel_gtt_chipset_flush();
+		}
 	}
 
+	if (needs_clflush_after)
+		intel_gtt_chipset_flush();
+
 	return ret;
 }
 
@@ -892,8 +867,8 @@ i915_gem_pwrite_ioctl(struct drm_device *dev, void *data,
 		       args->size))
 		return -EFAULT;
 
-	ret = fault_in_pages_readable((char __user *)(uintptr_t)args->data_ptr,
-				      args->size);
+	ret = fault_in_multipages_readable((char __user *)(uintptr_t)args->data_ptr,
+					   args->size);
 	if (ret)
 		return -EFAULT;
 
@@ -914,8 +889,17 @@ i915_gem_pwrite_ioctl(struct drm_device *dev, void *data,
 		goto out;
 	}
 
+	/* prime objects have no backing filp to GEM pread/pwrite
+	 * pages from.
+	 */
+	if (!obj->base.filp) {
+		ret = -EINVAL;
+		goto out;
+	}
+
 	trace_i915_gem_object_pwrite(obj, args->offset, args->size);
 
+	ret = -EFAULT;
 	/* We can only do the GTT pwrite on untiled buffers, as otherwise
 	 * it would end up going through the fenced access, and we'll get
 	 * different detiling behavior between reading and writing.
@@ -928,42 +912,18 @@ i915_gem_pwrite_ioctl(struct drm_device *dev, void *data,
 	}
 
 	if (obj->gtt_space &&
+	    obj->cache_level == I915_CACHE_NONE &&
+	    obj->tiling_mode == I915_TILING_NONE &&
+	    obj->map_and_fenceable &&
 	    obj->base.write_domain != I915_GEM_DOMAIN_CPU) {
-		ret = i915_gem_object_pin(obj, 0, true);
-		if (ret)
-			goto out;
-
-		ret = i915_gem_object_set_to_gtt_domain(obj, true);
-		if (ret)
-			goto out_unpin;
-
-		ret = i915_gem_object_put_fence(obj);
-		if (ret)
-			goto out_unpin;
-
 		ret = i915_gem_gtt_pwrite_fast(dev, obj, args, file);
-		if (ret == -EFAULT)
-			ret = i915_gem_gtt_pwrite_slow(dev, obj, args, file);
-
-out_unpin:
-		i915_gem_object_unpin(obj);
-
-		if (ret != -EFAULT)
-			goto out;
-		/* Fall through to the shmfs paths because the gtt paths might
-		 * fail with non-page-backed user pointers (e.g. gtt mappings
-		 * when moving data between textures). */
+		/* Note that the gtt paths might fail with non-page-backed user
+		 * pointers (e.g. gtt mappings when moving data between
+		 * textures). Fallback to the shmem path in that case. */
 	}
 
-	ret = i915_gem_object_set_to_cpu_domain(obj, 1);
-	if (ret)
-		goto out;
-
-	ret = -EFAULT;
-	if (!i915_gem_object_needs_bit17_swizzle(obj))
-		ret = i915_gem_shmem_pwrite_fast(dev, obj, args, file);
 	if (ret == -EFAULT)
-		ret = i915_gem_shmem_pwrite_slow(dev, obj, args, file);
+		ret = i915_gem_shmem_pwrite(dev, obj, args, file);
 
 out:
 	drm_gem_object_unreference(&obj->base);
@@ -986,9 +946,6 @@ i915_gem_set_domain_ioctl(struct drm_device *dev, void *data,
 	uint32_t write_domain = args->write_domain;
 	int ret;
 
-	if (!(dev->driver->driver_features & DRIVER_GEM))
-		return -ENODEV;
-
 	/* Only handle setting domains to types used by the CPU. */
 	if (write_domain & I915_GEM_GPU_DOMAINS)
 		return -EINVAL;
@@ -1042,9 +999,6 @@ i915_gem_sw_finish_ioctl(struct drm_device *dev, void *data,
 	struct drm_i915_gem_object *obj;
 	int ret = 0;
 
-	if (!(dev->driver->driver_features & DRIVER_GEM))
-		return -ENODEV;
-
 	ret = i915_mutex_lock_interruptible(dev);
 	if (ret)
 		return ret;
@@ -1080,13 +1034,18 @@ i915_gem_mmap_ioctl(struct drm_device *dev, void *data,
 	struct drm_gem_object *obj;
 	unsigned long addr;
 
-	if (!(dev->driver->driver_features & DRIVER_GEM))
-		return -ENODEV;
-
 	obj = drm_gem_object_lookup(dev, file, args->handle);
 	if (obj == NULL)
 		return -ENOENT;
 
+	/* prime objects have no backing filp to GEM mmap
+	 * pages from.
+	 */
+	if (!obj->filp) {
+		drm_gem_object_unreference_unlocked(obj);
+		return -EINVAL;
+	}
+
 	addr = vm_mmap(obj->filp, 0, args->size,
 		       PROT_READ | PROT_WRITE, MAP_SHARED,
 		       args->offset);
@@ -1151,10 +1110,10 @@ int i915_gem_fault(struct vm_area_struct *vma, struct vm_fault *vmf)
 			goto unlock;
 	}
 
-	if (obj->tiling_mode == I915_TILING_NONE)
-		ret = i915_gem_object_put_fence(obj);
-	else
-		ret = i915_gem_object_get_fence(obj, NULL);
+	if (!obj->has_global_gtt_mapping)
+		i915_gem_gtt_bind_object(obj, obj->cache_level);
+
+	ret = i915_gem_object_get_fence(obj);
 	if (ret)
 		goto unlock;
 
@@ -1308,9 +1267,6 @@ i915_gem_mmap_gtt(struct drm_file *file,
 	struct drm_i915_gem_object *obj;
 	int ret;
 
-	if (!(dev->driver->driver_features & DRIVER_GEM))
-		return -ENODEV;
-
 	ret = i915_mutex_lock_interruptible(dev);
 	if (ret)
 		return ret;
@@ -1368,14 +1324,10 @@ i915_gem_mmap_gtt_ioctl(struct drm_device *dev, void *data,
 {
 	struct drm_i915_gem_mmap_gtt *args = data;
 
-	if (!(dev->driver->driver_features & DRIVER_GEM))
-		return -ENODEV;
-
 	return i915_gem_mmap_gtt(file, dev, args->handle, &args->offset);
 }
 
-
-static int
+int
 i915_gem_object_get_pages_gtt(struct drm_i915_gem_object *obj,
 			      gfp_t gfpmask)
 {
@@ -1384,6 +1336,9 @@ i915_gem_object_get_pages_gtt(struct drm_i915_gem_object *obj,
 	struct inode *inode;
 	struct page *page;
 
+	if (obj->pages || obj->sg_table)
+		return 0;
+
 	/* Get the list of pages out of our struct file.  They'll be pinned
 	 * at this point until we release them.
 	 */
@@ -1425,6 +1380,9 @@ i915_gem_object_put_pages_gtt(struct drm_i915_gem_object *obj)
 	int page_count = obj->base.size / PAGE_SIZE;
 	int i;
 
+	if (!obj->pages)
+		return;
+
 	BUG_ON(obj->madv == __I915_MADV_PURGED);
 
 	if (i915_gem_object_needs_bit17_swizzle(obj))
@@ -1473,7 +1431,6 @@ i915_gem_object_move_to_active(struct drm_i915_gem_object *obj,
 
 	if (obj->fenced_gpu_access) {
 		obj->last_fenced_seqno = seqno;
-		obj->last_fenced_ring = ring;
 
 		/* Bump MRU to take account of the delayed flush */
 		if (obj->fence_reg != I915_FENCE_REG_NONE) {
@@ -1512,15 +1469,11 @@ i915_gem_object_move_to_inactive(struct drm_i915_gem_object *obj)
 	struct drm_device *dev = obj->base.dev;
 	struct drm_i915_private *dev_priv = dev->dev_private;
 
-	if (obj->pin_count != 0)
-		list_move_tail(&obj->mm_list, &dev_priv->mm.pinned_list);
-	else
-		list_move_tail(&obj->mm_list, &dev_priv->mm.inactive_list);
+	list_move_tail(&obj->mm_list, &dev_priv->mm.inactive_list);
 
 	BUG_ON(!list_empty(&obj->gpu_write_list));
 	BUG_ON(!obj->active);
 	obj->ring = NULL;
-	obj->last_fenced_ring = NULL;
 
 	i915_gem_object_move_off_active(obj);
 	obj->fenced_gpu_access = false;
@@ -1546,6 +1499,9 @@ i915_gem_object_truncate(struct drm_i915_gem_object *obj)
 	inode = obj->base.filp->f_path.dentry->d_inode;
 	shmem_truncate_range(inode, 0, (loff_t)-1);
 
+	if (obj->base.map_list.map)
+		drm_gem_free_mmap_offset(&obj->base);
+
 	obj->madv = __I915_MADV_PURGED;
 }
 
@@ -1711,30 +1667,29 @@ 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];
-		struct drm_i915_gem_object *obj = reg->obj;
 
-		if (!obj)
-			continue;
+		i915_gem_write_fence(dev, i, NULL);
 
-		if (obj->tiling_mode)
-			i915_gem_release_mmap(obj);
+		if (reg->obj)
+			i915_gem_object_fence_lost(reg->obj);
 
-		reg->obj->fence_reg = I915_FENCE_REG_NONE;
-		reg->obj->fenced_gpu_access = false;
-		reg->obj->last_fenced_seqno = 0;
-		reg->obj->last_fenced_ring = NULL;
-		i915_gem_clear_fence_reg(dev, reg);
+		reg->pin_count = 0;
+		reg->obj = NULL;
+		INIT_LIST_HEAD(&reg->lru_list);
 	}
+
+	INIT_LIST_HEAD(&dev_priv->mm.fence_list);
 }
 
 void i915_gem_reset(struct drm_device *dev)
 {
 	struct drm_i915_private *dev_priv = dev->dev_private;
 	struct drm_i915_gem_object *obj;
+	struct intel_ring_buffer *ring;
 	int i;
 
-	for (i = 0; i < I915_NUM_RINGS; i++)
-		i915_gem_reset_ring_lists(dev_priv, &dev_priv->ring[i]);
+	for_each_ring(ring, dev_priv, i)
+		i915_gem_reset_ring_lists(dev_priv, ring);
 
 	/* Remove anything from the flushing lists. The GPU cache is likely
 	 * to be lost on reset along with the data, so simply move the
@@ -1839,24 +1794,11 @@ void
 i915_gem_retire_requests(struct drm_device *dev)
 {
 	drm_i915_private_t *dev_priv = dev->dev_private;
+	struct intel_ring_buffer *ring;
 	int i;
 
-	if (!list_empty(&dev_priv->mm.deferred_free_list)) {
-	    struct drm_i915_gem_object *obj, *next;
-
-	    /* We must be careful that during unbind() we do not
-	     * accidentally infinitely recurse into retire requests.
-	     * Currently:
-	     *   retire -> free -> unbind -> wait -> retire_ring
-	     */
-	    list_for_each_entry_safe(obj, next,
-				     &dev_priv->mm.deferred_free_list,
-				     mm_list)
-		    i915_gem_free_object_tail(obj);
-	}
-
-	for (i = 0; i < I915_NUM_RINGS; i++)
-		i915_gem_retire_requests_ring(&dev_priv->ring[i]);
+	for_each_ring(ring, dev_priv, i)
+		i915_gem_retire_requests_ring(ring);
 }
 
 static void
@@ -1864,6 +1806,7 @@ i915_gem_retire_work_handler(struct work_struct *work)
 {
 	drm_i915_private_t *dev_priv;
 	struct drm_device *dev;
+	struct intel_ring_buffer *ring;
 	bool idle;
 	int i;
 
@@ -1883,9 +1826,7 @@ i915_gem_retire_work_handler(struct work_struct *work)
 	 * objects indefinitely.
 	 */
 	idle = true;
-	for (i = 0; i < I915_NUM_RINGS; i++) {
-		struct intel_ring_buffer *ring = &dev_priv->ring[i];
-
+	for_each_ring(ring, dev_priv, i) {
 		if (!list_empty(&ring->gpu_write_list)) {
 			struct drm_i915_gem_request *request;
 			int ret;
@@ -1907,20 +1848,10 @@ i915_gem_retire_work_handler(struct work_struct *work)
 	mutex_unlock(&dev->struct_mutex);
 }
 
-/**
- * Waits for a sequence number to be signaled, and cleans up the
- * request and object lists appropriately for that event.
- */
-int
-i915_wait_request(struct intel_ring_buffer *ring,
-		  uint32_t seqno,
-		  bool do_retire)
+static int
+i915_gem_check_wedge(struct drm_i915_private *dev_priv)
 {
-	drm_i915_private_t *dev_priv = ring->dev->dev_private;
-	u32 ier;
-	int ret = 0;
-
-	BUG_ON(seqno == 0);
+	BUG_ON(!mutex_is_locked(&dev_priv->dev->struct_mutex));
 
 	if (atomic_read(&dev_priv->mm.wedged)) {
 		struct completion *x = &dev_priv->error_completion;
@@ -1935,6 +1866,20 @@ i915_wait_request(struct intel_ring_buffer *ring,
 		return recovery_complete ? -EIO : -EAGAIN;
 	}
 
+	return 0;
+}
+
+/*
+ * Compare seqno against outstanding lazy request. Emit a request if they are
+ * equal.
+ */
+static int
+i915_gem_check_olr(struct intel_ring_buffer *ring, u32 seqno)
+{
+	int ret = 0;
+
+	BUG_ON(!mutex_is_locked(&ring->dev->struct_mutex));
+
 	if (seqno == ring->outstanding_lazy_request) {
 		struct drm_i915_gem_request *request;
 
@@ -1948,54 +1893,67 @@ i915_wait_request(struct intel_ring_buffer *ring,
 			return ret;
 		}
 
-		seqno = request->seqno;
+		BUG_ON(seqno != request->seqno);
 	}
 
-	if (!i915_seqno_passed(ring->get_seqno(ring), seqno)) {
-		if (HAS_PCH_SPLIT(ring->dev))
-			ier = I915_READ(DEIER) | I915_READ(GTIER);
-		else
-			ier = I915_READ(IER);
-		if (!ier) {
-			DRM_ERROR("something (likely vbetool) disabled "
-				  "interrupts, re-enabling\n");
-			ring->dev->driver->irq_preinstall(ring->dev);
-			ring->dev->driver->irq_postinstall(ring->dev);
-		}
+	return ret;
+}
 
-		trace_i915_gem_request_wait_begin(ring, seqno);
-
-		ring->waiting_seqno = seqno;
-		if (ring->irq_get(ring)) {
-			if (dev_priv->mm.interruptible)
-				ret = wait_event_interruptible(ring->irq_queue,
-							       i915_seqno_passed(ring->get_seqno(ring), seqno)
-							       || atomic_read(&dev_priv->mm.wedged));
-			else
-				wait_event(ring->irq_queue,
-					   i915_seqno_passed(ring->get_seqno(ring), seqno)
-					   || atomic_read(&dev_priv->mm.wedged));
-
-			ring->irq_put(ring);
-		} else if (wait_for_atomic(i915_seqno_passed(ring->get_seqno(ring),
-							     seqno) ||
-					   atomic_read(&dev_priv->mm.wedged), 3000))
-			ret = -EBUSY;
-		ring->waiting_seqno = 0;
-
-		trace_i915_gem_request_wait_end(ring, seqno);
-	}
+static int __wait_seqno(struct intel_ring_buffer *ring, u32 seqno,
+			bool interruptible)
+{
+	drm_i915_private_t *dev_priv = ring->dev->dev_private;
+	int ret = 0;
+
+	if (i915_seqno_passed(ring->get_seqno(ring), seqno))
+		return 0;
+
+	trace_i915_gem_request_wait_begin(ring, seqno);
+	if (WARN_ON(!ring->irq_get(ring)))
+		return -ENODEV;
+
+#define EXIT_COND \
+	(i915_seqno_passed(ring->get_seqno(ring), seqno) || \
+	atomic_read(&dev_priv->mm.wedged))
+
+	if (interruptible)
+		ret = wait_event_interruptible(ring->irq_queue,
+					       EXIT_COND);
+	else
+		wait_event(ring->irq_queue, EXIT_COND);
+
+	ring->irq_put(ring);
+	trace_i915_gem_request_wait_end(ring, seqno);
+#undef EXIT_COND
+
+	return ret;
+}
+
+/**
+ * Waits for a sequence number to be signaled, and cleans up the
+ * request and object lists appropriately for that event.
+ */
+int
+i915_wait_request(struct intel_ring_buffer *ring,
+		  uint32_t seqno)
+{
+	drm_i915_private_t *dev_priv = ring->dev->dev_private;
+	int ret = 0;
+
+	BUG_ON(seqno == 0);
+
+	ret = i915_gem_check_wedge(dev_priv);
+	if (ret)
+		return ret;
+
+	ret = i915_gem_check_olr(ring, seqno);
+	if (ret)
+		return ret;
+
+	ret = __wait_seqno(ring, seqno, dev_priv->mm.interruptible);
 	if (atomic_read(&dev_priv->mm.wedged))
 		ret = -EAGAIN;
 
-	/* Directly dispatch request retiring.  While we have the work queue
-	 * to handle this, the waiter on a request often wants an associated
-	 * buffer to have made it to the inactive list, and we would need
-	 * a separate wait queue to handle that.
-	 */
-	if (ret == 0 && do_retire)
-		i915_gem_retire_requests_ring(ring);
-
 	return ret;
 }
 
@@ -2017,15 +1975,58 @@ i915_gem_object_wait_rendering(struct drm_i915_gem_object *obj)
 	 * it.
 	 */
 	if (obj->active) {
-		ret = i915_wait_request(obj->ring, obj->last_rendering_seqno,
-					true);
+		ret = i915_wait_request(obj->ring, obj->last_rendering_seqno);
 		if (ret)
 			return ret;
+		i915_gem_retire_requests_ring(obj->ring);
 	}
 
 	return 0;
 }
 
+/**
+ * i915_gem_object_sync - sync an object to a ring.
+ *
+ * @obj: object which may be in use on another ring.
+ * @to: ring we wish to use the object on. May be NULL.
+ *
+ * This code is meant to abstract object synchronization with the GPU.
+ * Calling with NULL implies synchronizing the object with the CPU
+ * rather than a particular GPU ring.
+ *
+ * Returns 0 if successful, else propagates up the lower layer error.
+ */
+int
+i915_gem_object_sync(struct drm_i915_gem_object *obj,
+		     struct intel_ring_buffer *to)
+{
+	struct intel_ring_buffer *from = obj->ring;
+	u32 seqno;
+	int ret, idx;
+
+	if (from == NULL || to == from)
+		return 0;
+
+	if (to == NULL || !i915_semaphore_is_enabled(obj->base.dev))
+		return i915_gem_object_wait_rendering(obj);
+
+	idx = intel_ring_sync_index(from, to);
+
+	seqno = obj->last_rendering_seqno;
+	if (seqno <= from->sync_seqno[idx])
+		return 0;
+
+	ret = i915_gem_check_olr(obj->ring, seqno);
+	if (ret)
+		return ret;
+
+	ret = to->sync_to(to, from, seqno);
+	if (!ret)
+		from->sync_seqno[idx] = seqno;
+
+	return ret;
+}
+
 static void i915_gem_object_finish_gtt(struct drm_i915_gem_object *obj)
 {
 	u32 old_write_domain, old_read_domains;
@@ -2068,7 +2069,7 @@ i915_gem_object_unbind(struct drm_i915_gem_object *obj)
 	}
 
 	ret = i915_gem_object_finish_gpu(obj);
-	if (ret == -ERESTARTSYS)
+	if (ret)
 		return ret;
 	/* Continue on if we fail due to EIO, the GPU is hung so we
 	 * should be safe and we need to cleanup or else we might
@@ -2095,16 +2096,18 @@ i915_gem_object_unbind(struct drm_i915_gem_object *obj)
 
 	/* release the fence reg _after_ flushing */
 	ret = i915_gem_object_put_fence(obj);
-	if (ret == -ERESTARTSYS)
+	if (ret)
 		return ret;
 
 	trace_i915_gem_object_unbind(obj);
 
-	i915_gem_gtt_unbind_object(obj);
+	if (obj->has_global_gtt_mapping)
+		i915_gem_gtt_unbind_object(obj);
 	if (obj->has_aliasing_ppgtt_mapping) {
 		i915_ppgtt_unbind_object(dev_priv->mm.aliasing_ppgtt, obj);
 		obj->has_aliasing_ppgtt_mapping = 0;
 	}
+	i915_gem_gtt_finish_object(obj);
 
 	i915_gem_object_put_pages_gtt(obj);
 
@@ -2145,7 +2148,7 @@ i915_gem_flush_ring(struct intel_ring_buffer *ring,
 	return 0;
 }
 
-static int i915_ring_idle(struct intel_ring_buffer *ring, bool do_retire)
+static int i915_ring_idle(struct intel_ring_buffer *ring)
 {
 	int ret;
 
@@ -2159,208 +2162,201 @@ static int i915_ring_idle(struct intel_ring_buffer *ring, bool do_retire)
 			return ret;
 	}
 
-	return i915_wait_request(ring, i915_gem_next_request_seqno(ring),
-				 do_retire);
+	return i915_wait_request(ring, i915_gem_next_request_seqno(ring));
 }
 
-int i915_gpu_idle(struct drm_device *dev, bool do_retire)
+int i915_gpu_idle(struct drm_device *dev)
 {
 	drm_i915_private_t *dev_priv = dev->dev_private;
+	struct intel_ring_buffer *ring;
 	int ret, i;
 
 	/* Flush everything onto the inactive list. */
-	for (i = 0; i < I915_NUM_RINGS; i++) {
-		ret = i915_ring_idle(&dev_priv->ring[i], do_retire);
+	for_each_ring(ring, dev_priv, i) {
+		ret = i915_ring_idle(ring);
 		if (ret)
 			return ret;
+
+		/* Is the device fubar? */
+		if (WARN_ON(!list_empty(&ring->gpu_write_list)))
+			return -EBUSY;
 	}
 
 	return 0;
 }
 
-static int sandybridge_write_fence_reg(struct drm_i915_gem_object *obj,
-				       struct intel_ring_buffer *pipelined)
+static void sandybridge_write_fence_reg(struct drm_device *dev, int reg,
+					struct drm_i915_gem_object *obj)
 {
-	struct drm_device *dev = obj->base.dev;
 	drm_i915_private_t *dev_priv = dev->dev_private;
-	u32 size = obj->gtt_space->size;
-	int regnum = obj->fence_reg;
 	uint64_t val;
 
-	val = (uint64_t)((obj->gtt_offset + size - 4096) &
-			 0xfffff000) << 32;
-	val |= obj->gtt_offset & 0xfffff000;
-	val |= (uint64_t)((obj->stride / 128) - 1) <<
-		SANDYBRIDGE_FENCE_PITCH_SHIFT;
+	if (obj) {
+		u32 size = obj->gtt_space->size;
 
-	if (obj->tiling_mode == I915_TILING_Y)
-		val |= 1 << I965_FENCE_TILING_Y_SHIFT;
-	val |= I965_FENCE_REG_VALID;
+		val = (uint64_t)((obj->gtt_offset + size - 4096) &
+				 0xfffff000) << 32;
+		val |= obj->gtt_offset & 0xfffff000;
+		val |= (uint64_t)((obj->stride / 128) - 1) <<
+			SANDYBRIDGE_FENCE_PITCH_SHIFT;
 
-	if (pipelined) {
-		int ret = intel_ring_begin(pipelined, 6);
-		if (ret)
-			return ret;
-
-		intel_ring_emit(pipelined, MI_NOOP);
-		intel_ring_emit(pipelined, MI_LOAD_REGISTER_IMM(2));
-		intel_ring_emit(pipelined, FENCE_REG_SANDYBRIDGE_0 + regnum*8);
-		intel_ring_emit(pipelined, (u32)val);
-		intel_ring_emit(pipelined, FENCE_REG_SANDYBRIDGE_0 + regnum*8 + 4);
-		intel_ring_emit(pipelined, (u32)(val >> 32));
-		intel_ring_advance(pipelined);
+		if (obj->tiling_mode == I915_TILING_Y)
+			val |= 1 << I965_FENCE_TILING_Y_SHIFT;
+		val |= I965_FENCE_REG_VALID;
 	} else
-		I915_WRITE64(FENCE_REG_SANDYBRIDGE_0 + regnum * 8, val);
+		val = 0;
 
-	return 0;
+	I915_WRITE64(FENCE_REG_SANDYBRIDGE_0 + reg * 8, val);
+	POSTING_READ(FENCE_REG_SANDYBRIDGE_0 + reg * 8);
 }
 
-static int i965_write_fence_reg(struct drm_i915_gem_object *obj,
-				struct intel_ring_buffer *pipelined)
+static void i965_write_fence_reg(struct drm_device *dev, int reg,
+				 struct drm_i915_gem_object *obj)
 {
-	struct drm_device *dev = obj->base.dev;
 	drm_i915_private_t *dev_priv = dev->dev_private;
-	u32 size = obj->gtt_space->size;
-	int regnum = obj->fence_reg;
 	uint64_t val;
 
-	val = (uint64_t)((obj->gtt_offset + size - 4096) &
-		    0xfffff000) << 32;
-	val |= obj->gtt_offset & 0xfffff000;
-	val |= ((obj->stride / 128) - 1) << I965_FENCE_PITCH_SHIFT;
-	if (obj->tiling_mode == I915_TILING_Y)
-		val |= 1 << I965_FENCE_TILING_Y_SHIFT;
-	val |= I965_FENCE_REG_VALID;
+	if (obj) {
+		u32 size = obj->gtt_space->size;
 
-	if (pipelined) {
-		int ret = intel_ring_begin(pipelined, 6);
-		if (ret)
-			return ret;
-
-		intel_ring_emit(pipelined, MI_NOOP);
-		intel_ring_emit(pipelined, MI_LOAD_REGISTER_IMM(2));
-		intel_ring_emit(pipelined, FENCE_REG_965_0 + regnum*8);
-		intel_ring_emit(pipelined, (u32)val);
-		intel_ring_emit(pipelined, FENCE_REG_965_0 + regnum*8 + 4);
-		intel_ring_emit(pipelined, (u32)(val >> 32));
-		intel_ring_advance(pipelined);
+		val = (uint64_t)((obj->gtt_offset + size - 4096) &
+				 0xfffff000) << 32;
+		val |= obj->gtt_offset & 0xfffff000;
+		val |= ((obj->stride / 128) - 1) << I965_FENCE_PITCH_SHIFT;
+		if (obj->tiling_mode == I915_TILING_Y)
+			val |= 1 << I965_FENCE_TILING_Y_SHIFT;
+		val |= I965_FENCE_REG_VALID;
 	} else
-		I915_WRITE64(FENCE_REG_965_0 + regnum * 8, val);
+		val = 0;
 
-	return 0;
+	I915_WRITE64(FENCE_REG_965_0 + reg * 8, val);
+	POSTING_READ(FENCE_REG_965_0 + reg * 8);
 }
 
-static int i915_write_fence_reg(struct drm_i915_gem_object *obj,
-				struct intel_ring_buffer *pipelined)
+static void i915_write_fence_reg(struct drm_device *dev, int reg,
+				 struct drm_i915_gem_object *obj)
 {
-	struct drm_device *dev = obj->base.dev;
 	drm_i915_private_t *dev_priv = dev->dev_private;
-	u32 size = obj->gtt_space->size;
-	u32 fence_reg, val, pitch_val;
-	int tile_width;
-
-	if (WARN((obj->gtt_offset & ~I915_FENCE_START_MASK) ||
-		 (size & -size) != size ||
-		 (obj->gtt_offset & (size - 1)),
-		 "object 0x%08x [fenceable? %d] not 1M or pot-size (0x%08x) aligned\n",
-		 obj->gtt_offset, obj->map_and_fenceable, size))
-		return -EINVAL;
+	u32 val;
 
-	if (obj->tiling_mode == I915_TILING_Y && HAS_128_BYTE_Y_TILING(dev))
-		tile_width = 128;
-	else
-		tile_width = 512;
-
-	/* Note: pitch better be a power of two tile widths */
-	pitch_val = obj->stride / tile_width;
-	pitch_val = ffs(pitch_val) - 1;
-
-	val = obj->gtt_offset;
-	if (obj->tiling_mode == I915_TILING_Y)
-		val |= 1 << I830_FENCE_TILING_Y_SHIFT;
-	val |= I915_FENCE_SIZE_BITS(size);
-	val |= pitch_val << I830_FENCE_PITCH_SHIFT;
-	val |= I830_FENCE_REG_VALID;
-
-	fence_reg = obj->fence_reg;
-	if (fence_reg < 8)
-		fence_reg = FENCE_REG_830_0 + fence_reg * 4;
-	else
-		fence_reg = FENCE_REG_945_8 + (fence_reg - 8) * 4;
+	if (obj) {
+		u32 size = obj->gtt_space->size;
+		int pitch_val;
+		int tile_width;
 
-	if (pipelined) {
-		int ret = intel_ring_begin(pipelined, 4);
-		if (ret)
-			return ret;
+		WARN((obj->gtt_offset & ~I915_FENCE_START_MASK) ||
+		     (size & -size) != size ||
+		     (obj->gtt_offset & (size - 1)),
+		     "object 0x%08x [fenceable? %d] not 1M or pot-size (0x%08x) aligned\n",
+		     obj->gtt_offset, obj->map_and_fenceable, size);
 
-		intel_ring_emit(pipelined, MI_NOOP);
-		intel_ring_emit(pipelined, MI_LOAD_REGISTER_IMM(1));
-		intel_ring_emit(pipelined, fence_reg);
-		intel_ring_emit(pipelined, val);
-		intel_ring_advance(pipelined);
+		if (obj->tiling_mode == I915_TILING_Y && HAS_128_BYTE_Y_TILING(dev))
+			tile_width = 128;
+		else
+			tile_width = 512;
+
+		/* Note: pitch better be a power of two tile widths */
+		pitch_val = obj->stride / tile_width;
+		pitch_val = ffs(pitch_val) - 1;
+
+		val = obj->gtt_offset;
+		if (obj->tiling_mode == I915_TILING_Y)
+			val |= 1 << I830_FENCE_TILING_Y_SHIFT;
+		val |= I915_FENCE_SIZE_BITS(size);
+		val |= pitch_val << I830_FENCE_PITCH_SHIFT;
+		val |= I830_FENCE_REG_VALID;
 	} else
-		I915_WRITE(fence_reg, val);
+		val = 0;
 
-	return 0;
+	if (reg < 8)
+		reg = FENCE_REG_830_0 + reg * 4;
+	else
+		reg = FENCE_REG_945_8 + (reg - 8) * 4;
+
+	I915_WRITE(reg, val);
+	POSTING_READ(reg);
 }
 
-static int i830_write_fence_reg(struct drm_i915_gem_object *obj,
-				struct intel_ring_buffer *pipelined)
+static void i830_write_fence_reg(struct drm_device *dev, int reg,
+				struct drm_i915_gem_object *obj)
 {
-	struct drm_device *dev = obj->base.dev;
 	drm_i915_private_t *dev_priv = dev->dev_private;
-	u32 size = obj->gtt_space->size;
-	int regnum = obj->fence_reg;
 	uint32_t val;
-	uint32_t pitch_val;
 
-	if (WARN((obj->gtt_offset & ~I830_FENCE_START_MASK) ||
-		 (size & -size) != size ||
-		 (obj->gtt_offset & (size - 1)),
-		 "object 0x%08x not 512K or pot-size 0x%08x aligned\n",
-		 obj->gtt_offset, size))
-		return -EINVAL;
-
-	pitch_val = obj->stride / 128;
-	pitch_val = ffs(pitch_val) - 1;
-
-	val = obj->gtt_offset;
-	if (obj->tiling_mode == I915_TILING_Y)
-		val |= 1 << I830_FENCE_TILING_Y_SHIFT;
-	val |= I830_FENCE_SIZE_BITS(size);
-	val |= pitch_val << I830_FENCE_PITCH_SHIFT;
-	val |= I830_FENCE_REG_VALID;
+	if (obj) {
+		u32 size = obj->gtt_space->size;
+		uint32_t pitch_val;
+
+		WARN((obj->gtt_offset & ~I830_FENCE_START_MASK) ||
+		     (size & -size) != size ||
+		     (obj->gtt_offset & (size - 1)),
+		     "object 0x%08x not 512K or pot-size 0x%08x aligned\n",
+		     obj->gtt_offset, size);
+
+		pitch_val = obj->stride / 128;
+		pitch_val = ffs(pitch_val) - 1;
+
+		val = obj->gtt_offset;
+		if (obj->tiling_mode == I915_TILING_Y)
+			val |= 1 << I830_FENCE_TILING_Y_SHIFT;
+		val |= I830_FENCE_SIZE_BITS(size);
+		val |= pitch_val << I830_FENCE_PITCH_SHIFT;
+		val |= I830_FENCE_REG_VALID;
+	} else
+		val = 0;
 
-	if (pipelined) {
-		int ret = intel_ring_begin(pipelined, 4);
-		if (ret)
-			return ret;
+	I915_WRITE(FENCE_REG_830_0 + reg * 4, val);
+	POSTING_READ(FENCE_REG_830_0 + reg * 4);
+}
 
-		intel_ring_emit(pipelined, MI_NOOP);
-		intel_ring_emit(pipelined, MI_LOAD_REGISTER_IMM(1));
-		intel_ring_emit(pipelined, FENCE_REG_830_0 + regnum*4);
-		intel_ring_emit(pipelined, val);
-		intel_ring_advance(pipelined);
-	} else
-		I915_WRITE(FENCE_REG_830_0 + regnum * 4, val);
+static void i915_gem_write_fence(struct drm_device *dev, int reg,
+				 struct drm_i915_gem_object *obj)
+{
+	switch (INTEL_INFO(dev)->gen) {
+	case 7:
+	case 6: sandybridge_write_fence_reg(dev, reg, obj); break;
+	case 5:
+	case 4: i965_write_fence_reg(dev, reg, obj); break;
+	case 3: i915_write_fence_reg(dev, reg, obj); break;
+	case 2: i830_write_fence_reg(dev, reg, obj); break;
+	default: break;
+	}
+}
 
-	return 0;
+static inline int fence_number(struct drm_i915_private *dev_priv,
+			       struct drm_i915_fence_reg *fence)
+{
+	return fence - dev_priv->fence_regs;
 }
 
-static bool ring_passed_seqno(struct intel_ring_buffer *ring, u32 seqno)
+static void i915_gem_object_update_fence(struct drm_i915_gem_object *obj,
+					 struct drm_i915_fence_reg *fence,
+					 bool enable)
 {
-	return i915_seqno_passed(ring->get_seqno(ring), seqno);
+	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);
+
+	if (enable) {
+		obj->fence_reg = reg;
+		fence->obj = obj;
+		list_move_tail(&fence->lru_list, &dev_priv->mm.fence_list);
+	} else {
+		obj->fence_reg = I915_FENCE_REG_NONE;
+		fence->obj = NULL;
+		list_del_init(&fence->lru_list);
+	}
 }
 
 static int
-i915_gem_object_flush_fence(struct drm_i915_gem_object *obj,
-			    struct intel_ring_buffer *pipelined)
+i915_gem_object_flush_fence(struct drm_i915_gem_object *obj)
 {
 	int ret;
 
 	if (obj->fenced_gpu_access) {
 		if (obj->base.write_domain & I915_GEM_GPU_DOMAINS) {
-			ret = i915_gem_flush_ring(obj->last_fenced_ring,
+			ret = i915_gem_flush_ring(obj->ring,
 						  0, obj->base.write_domain);
 			if (ret)
 				return ret;
@@ -2369,18 +2365,12 @@ i915_gem_object_flush_fence(struct drm_i915_gem_object *obj,
 		obj->fenced_gpu_access = false;
 	}
 
-	if (obj->last_fenced_seqno && pipelined != obj->last_fenced_ring) {
-		if (!ring_passed_seqno(obj->last_fenced_ring,
-				       obj->last_fenced_seqno)) {
-			ret = i915_wait_request(obj->last_fenced_ring,
-						obj->last_fenced_seqno,
-						true);
-			if (ret)
-				return ret;
-		}
+	if (obj->last_fenced_seqno) {
+		ret = i915_wait_request(obj->ring, obj->last_fenced_seqno);
+		if (ret)
+			return ret;
 
 		obj->last_fenced_seqno = 0;
-		obj->last_fenced_ring = NULL;
 	}
 
 	/* Ensure that all CPU reads are completed before installing a fence
@@ -2395,34 +2385,29 @@ i915_gem_object_flush_fence(struct drm_i915_gem_object *obj,
 int
 i915_gem_object_put_fence(struct drm_i915_gem_object *obj)
 {
+	struct drm_i915_private *dev_priv = obj->base.dev->dev_private;
 	int ret;
 
-	if (obj->tiling_mode)
-		i915_gem_release_mmap(obj);
-
-	ret = i915_gem_object_flush_fence(obj, NULL);
+	ret = i915_gem_object_flush_fence(obj);
 	if (ret)
 		return ret;
 
-	if (obj->fence_reg != I915_FENCE_REG_NONE) {
-		struct drm_i915_private *dev_priv = obj->base.dev->dev_private;
-
-		WARN_ON(dev_priv->fence_regs[obj->fence_reg].pin_count);
-		i915_gem_clear_fence_reg(obj->base.dev,
-					 &dev_priv->fence_regs[obj->fence_reg]);
+	if (obj->fence_reg == I915_FENCE_REG_NONE)
+		return 0;
 
-		obj->fence_reg = I915_FENCE_REG_NONE;
-	}
+	i915_gem_object_update_fence(obj,
+				     &dev_priv->fence_regs[obj->fence_reg],
+				     false);
+	i915_gem_object_fence_lost(obj);
 
 	return 0;
 }
 
 static struct drm_i915_fence_reg *
-i915_find_fence_reg(struct drm_device *dev,
-		    struct intel_ring_buffer *pipelined)
+i915_find_fence_reg(struct drm_device *dev)
 {
 	struct drm_i915_private *dev_priv = dev->dev_private;
-	struct drm_i915_fence_reg *reg, *first, *avail;
+	struct drm_i915_fence_reg *reg, *avail;
 	int i;
 
 	/* First try to find a free reg */
@@ -2440,204 +2425,77 @@ i915_find_fence_reg(struct drm_device *dev,
 		return NULL;
 
 	/* None available, try to steal one or wait for a user to finish */
-	avail = first = NULL;
 	list_for_each_entry(reg, &dev_priv->mm.fence_list, lru_list) {
 		if (reg->pin_count)
 			continue;
 
-		if (first == NULL)
-			first = reg;
-
-		if (!pipelined ||
-		    !reg->obj->last_fenced_ring ||
-		    reg->obj->last_fenced_ring == pipelined) {
-			avail = reg;
-			break;
-		}
+		return reg;
 	}
 
-	if (avail == NULL)
-		avail = first;
-
-	return avail;
+	return NULL;
 }
 
 /**
- * i915_gem_object_get_fence - set up a fence reg for an object
+ * i915_gem_object_get_fence - set up fencing for an object
  * @obj: object to map through a fence reg
- * @pipelined: ring on which to queue the change, or NULL for CPU access
- * @interruptible: must we wait uninterruptibly for the register to retire?
  *
  * When mapping objects through the GTT, userspace wants to be able to write
  * to them without having to worry about swizzling if the object is tiled.
- *
  * This function walks the fence regs looking for a free one for @obj,
  * stealing one if it can't find any.
  *
  * It then sets up the reg based on the object's properties: address, pitch
  * and tiling format.
+ *
+ * For an untiled surface, this removes any existing fence.
  */
 int
-i915_gem_object_get_fence(struct drm_i915_gem_object *obj,
-			  struct intel_ring_buffer *pipelined)
+i915_gem_object_get_fence(struct drm_i915_gem_object *obj)
 {
 	struct drm_device *dev = obj->base.dev;
 	struct drm_i915_private *dev_priv = dev->dev_private;
+	bool enable = obj->tiling_mode != I915_TILING_NONE;
 	struct drm_i915_fence_reg *reg;
 	int ret;
 
-	/* XXX disable pipelining. There are bugs. Shocking. */
-	pipelined = NULL;
+	/* Have we updated the tiling parameters upon the object and so
+	 * will need to serialise the write to the associated fence register?
+	 */
+	if (obj->fence_dirty) {
+		ret = i915_gem_object_flush_fence(obj);
+		if (ret)
+			return ret;
+	}
 
 	/* Just update our place in the LRU if our fence is getting reused. */
 	if (obj->fence_reg != I915_FENCE_REG_NONE) {
 		reg = &dev_priv->fence_regs[obj->fence_reg];
-		list_move_tail(&reg->lru_list, &dev_priv->mm.fence_list);
-
-		if (obj->tiling_changed) {
-			ret = i915_gem_object_flush_fence(obj, pipelined);
-			if (ret)
-				return ret;
-
-			if (!obj->fenced_gpu_access && !obj->last_fenced_seqno)
-				pipelined = NULL;
-
-			if (pipelined) {
-				reg->setup_seqno =
-					i915_gem_next_request_seqno(pipelined);
-				obj->last_fenced_seqno = reg->setup_seqno;
-				obj->last_fenced_ring = pipelined;
-			}
-
-			goto update;
+		if (!obj->fence_dirty) {
+			list_move_tail(&reg->lru_list,
+				       &dev_priv->mm.fence_list);
+			return 0;
 		}
+	} else if (enable) {
+		reg = i915_find_fence_reg(dev);
+		if (reg == NULL)
+			return -EDEADLK;
 
-		if (!pipelined) {
-			if (reg->setup_seqno) {
-				if (!ring_passed_seqno(obj->last_fenced_ring,
-						       reg->setup_seqno)) {
-					ret = i915_wait_request(obj->last_fenced_ring,
-								reg->setup_seqno,
-								true);
-					if (ret)
-						return ret;
-				}
+		if (reg->obj) {
+			struct drm_i915_gem_object *old = reg->obj;
 
-				reg->setup_seqno = 0;
-			}
-		} else if (obj->last_fenced_ring &&
-			   obj->last_fenced_ring != pipelined) {
-			ret = i915_gem_object_flush_fence(obj, pipelined);
+			ret = i915_gem_object_flush_fence(old);
 			if (ret)
 				return ret;
-		}
-
-		return 0;
-	}
-
-	reg = i915_find_fence_reg(dev, pipelined);
-	if (reg == NULL)
-		return -EDEADLK;
-
-	ret = i915_gem_object_flush_fence(obj, pipelined);
-	if (ret)
-		return ret;
-
-	if (reg->obj) {
-		struct drm_i915_gem_object *old = reg->obj;
-
-		drm_gem_object_reference(&old->base);
-
-		if (old->tiling_mode)
-			i915_gem_release_mmap(old);
 
-		ret = i915_gem_object_flush_fence(old, pipelined);
-		if (ret) {
-			drm_gem_object_unreference(&old->base);
-			return ret;
+			i915_gem_object_fence_lost(old);
 		}
+	} else
+		return 0;
 
-		if (old->last_fenced_seqno == 0 && obj->last_fenced_seqno == 0)
-			pipelined = NULL;
-
-		old->fence_reg = I915_FENCE_REG_NONE;
-		old->last_fenced_ring = pipelined;
-		old->last_fenced_seqno =
-			pipelined ? i915_gem_next_request_seqno(pipelined) : 0;
-
-		drm_gem_object_unreference(&old->base);
-	} else if (obj->last_fenced_seqno == 0)
-		pipelined = NULL;
-
-	reg->obj = obj;
-	list_move_tail(&reg->lru_list, &dev_priv->mm.fence_list);
-	obj->fence_reg = reg - dev_priv->fence_regs;
-	obj->last_fenced_ring = pipelined;
-
-	reg->setup_seqno =
-		pipelined ? i915_gem_next_request_seqno(pipelined) : 0;
-	obj->last_fenced_seqno = reg->setup_seqno;
-
-update:
-	obj->tiling_changed = false;
-	switch (INTEL_INFO(dev)->gen) {
-	case 7:
-	case 6:
-		ret = sandybridge_write_fence_reg(obj, pipelined);
-		break;
-	case 5:
-	case 4:
-		ret = i965_write_fence_reg(obj, pipelined);
-		break;
-	case 3:
-		ret = i915_write_fence_reg(obj, pipelined);
-		break;
-	case 2:
-		ret = i830_write_fence_reg(obj, pipelined);
-		break;
-	}
-
-	return ret;
-}
-
-/**
- * i915_gem_clear_fence_reg - clear out fence register info
- * @obj: object to clear
- *
- * Zeroes out the fence register itself and clears out the associated
- * data structures in dev_priv and obj.
- */
-static void
-i915_gem_clear_fence_reg(struct drm_device *dev,
-			 struct drm_i915_fence_reg *reg)
-{
-	drm_i915_private_t *dev_priv = dev->dev_private;
-	uint32_t fence_reg = reg - dev_priv->fence_regs;
-
-	switch (INTEL_INFO(dev)->gen) {
-	case 7:
-	case 6:
-		I915_WRITE64(FENCE_REG_SANDYBRIDGE_0 + fence_reg*8, 0);
-		break;
-	case 5:
-	case 4:
-		I915_WRITE64(FENCE_REG_965_0 + fence_reg*8, 0);
-		break;
-	case 3:
-		if (fence_reg >= 8)
-			fence_reg = FENCE_REG_945_8 + (fence_reg - 8) * 4;
-		else
-	case 2:
-			fence_reg = FENCE_REG_830_0 + fence_reg * 4;
-
-		I915_WRITE(fence_reg, 0);
-		break;
-	}
+	i915_gem_object_update_fence(obj, reg, enable);
+	obj->fence_dirty = false;
 
-	list_del_init(&reg->lru_list);
-	reg->obj = NULL;
-	reg->setup_seqno = 0;
-	reg->pin_count = 0;
+	return 0;
 }
 
 /**
@@ -2749,7 +2607,7 @@ i915_gem_object_bind_to_gtt(struct drm_i915_gem_object *obj,
 		return ret;
 	}
 
-	ret = i915_gem_gtt_bind_object(obj);
+	ret = i915_gem_gtt_prepare_object(obj);
 	if (ret) {
 		i915_gem_object_put_pages_gtt(obj);
 		drm_mm_put_block(obj->gtt_space);
@@ -2761,6 +2619,9 @@ i915_gem_object_bind_to_gtt(struct drm_i915_gem_object *obj,
 		goto search_free;
 	}
 
+	if (!dev_priv->mm.aliasing_ppgtt)
+		i915_gem_gtt_bind_object(obj, obj->cache_level);
+
 	list_add_tail(&obj->gtt_list, &dev_priv->mm.gtt_list);
 	list_add_tail(&obj->mm_list, &dev_priv->mm.inactive_list);
 
@@ -2878,6 +2739,7 @@ i915_gem_object_flush_cpu_write_domain(struct drm_i915_gem_object *obj)
 int
 i915_gem_object_set_to_gtt_domain(struct drm_i915_gem_object *obj, bool write)
 {
+	drm_i915_private_t *dev_priv = obj->base.dev->dev_private;
 	uint32_t old_write_domain, old_read_domains;
 	int ret;
 
@@ -2918,6 +2780,10 @@ i915_gem_object_set_to_gtt_domain(struct drm_i915_gem_object *obj, bool write)
 					    old_read_domains,
 					    old_write_domain);
 
+	/* And bump the LRU for this access */
+	if (i915_gem_object_is_inactive(obj))
+		list_move_tail(&obj->mm_list, &dev_priv->mm.inactive_list);
+
 	return 0;
 }
 
@@ -2953,7 +2819,8 @@ int i915_gem_object_set_cache_level(struct drm_i915_gem_object *obj,
 				return ret;
 		}
 
-		i915_gem_gtt_rebind_object(obj, cache_level);
+		if (obj->has_global_gtt_mapping)
+			i915_gem_gtt_bind_object(obj, cache_level);
 		if (obj->has_aliasing_ppgtt_mapping)
 			i915_ppgtt_bind_object(dev_priv->mm.aliasing_ppgtt,
 					       obj, cache_level);
@@ -2990,11 +2857,6 @@ int i915_gem_object_set_cache_level(struct drm_i915_gem_object *obj,
  * Prepare buffer for display plane (scanout, cursors, etc).
  * Can be called from an uninterruptible phase (modesetting) and allows
  * any flushes to be pipelined (for pageflips).
- *
- * For the display plane, we want to be in the GTT but out of any write
- * domains. So in many ways this looks like set_to_gtt_domain() apart from the
- * ability to pipeline the waits, pinning and any additional subtleties
- * that may differentiate the display plane from ordinary buffers.
  */
 int
 i915_gem_object_pin_to_display_plane(struct drm_i915_gem_object *obj,
@@ -3009,8 +2871,8 @@ i915_gem_object_pin_to_display_plane(struct drm_i915_gem_object *obj,
 		return ret;
 
 	if (pipelined != obj->ring) {
-		ret = i915_gem_object_wait_rendering(obj);
-		if (ret == -ERESTARTSYS)
+		ret = i915_gem_object_sync(obj, pipelined);
+		if (ret)
 			return ret;
 	}
 
@@ -3082,7 +2944,7 @@ i915_gem_object_finish_gpu(struct drm_i915_gem_object *obj)
  * This function returns when the move is complete, including waiting on
  * flushes to occur.
  */
-static int
+int
 i915_gem_object_set_to_cpu_domain(struct drm_i915_gem_object *obj, bool write)
 {
 	uint32_t old_write_domain, old_read_domains;
@@ -3095,17 +2957,14 @@ i915_gem_object_set_to_cpu_domain(struct drm_i915_gem_object *obj, bool write)
 	if (ret)
 		return ret;
 
-	ret = i915_gem_object_wait_rendering(obj);
-	if (ret)
-		return ret;
+	if (write || obj->pending_gpu_write) {
+		ret = i915_gem_object_wait_rendering(obj);
+		if (ret)
+			return ret;
+	}
 
 	i915_gem_object_flush_gtt_write_domain(obj);
 
-	/* If we have a partially-valid cache of the object in the CPU,
-	 * finish invalidating it and free the per-page flags.
-	 */
-	i915_gem_object_set_to_full_cpu_read_domain(obj);
-
 	old_write_domain = obj->base.write_domain;
 	old_read_domains = obj->base.read_domains;
 
@@ -3136,113 +2995,6 @@ i915_gem_object_set_to_cpu_domain(struct drm_i915_gem_object *obj, bool write)
 	return 0;
 }
 
-/**
- * Moves the object from a partially CPU read to a full one.
- *
- * Note that this only resolves i915_gem_object_set_cpu_read_domain_range(),
- * and doesn't handle transitioning from !(read_domains & I915_GEM_DOMAIN_CPU).
- */
-static void
-i915_gem_object_set_to_full_cpu_read_domain(struct drm_i915_gem_object *obj)
-{
-	if (!obj->page_cpu_valid)
-		return;
-
-	/* If we're partially in the CPU read domain, finish moving it in.
-	 */
-	if (obj->base.read_domains & I915_GEM_DOMAIN_CPU) {
-		int i;
-
-		for (i = 0; i <= (obj->base.size - 1) / PAGE_SIZE; i++) {
-			if (obj->page_cpu_valid[i])
-				continue;
-			drm_clflush_pages(obj->pages + i, 1);
-		}
-	}
-
-	/* Free the page_cpu_valid mappings which are now stale, whether
-	 * or not we've got I915_GEM_DOMAIN_CPU.
-	 */
-	kfree(obj->page_cpu_valid);
-	obj->page_cpu_valid = NULL;
-}
-
-/**
- * Set the CPU read domain on a range of the object.
- *
- * The object ends up with I915_GEM_DOMAIN_CPU in its read flags although it's
- * not entirely valid.  The page_cpu_valid member of the object flags which
- * pages have been flushed, and will be respected by
- * i915_gem_object_set_to_cpu_domain() if it's called on to get a valid mapping
- * of the whole object.
- *
- * This function returns when the move is complete, including waiting on
- * flushes to occur.
- */
-static int
-i915_gem_object_set_cpu_read_domain_range(struct drm_i915_gem_object *obj,
-					  uint64_t offset, uint64_t size)
-{
-	uint32_t old_read_domains;
-	int i, ret;
-
-	if (offset == 0 && size == obj->base.size)
-		return i915_gem_object_set_to_cpu_domain(obj, 0);
-
-	ret = i915_gem_object_flush_gpu_write_domain(obj);
-	if (ret)
-		return ret;
-
-	ret = i915_gem_object_wait_rendering(obj);
-	if (ret)
-		return ret;
-
-	i915_gem_object_flush_gtt_write_domain(obj);
-
-	/* If we're already fully in the CPU read domain, we're done. */
-	if (obj->page_cpu_valid == NULL &&
-	    (obj->base.read_domains & I915_GEM_DOMAIN_CPU) != 0)
-		return 0;
-
-	/* Otherwise, create/clear the per-page CPU read domain flag if we're
-	 * newly adding I915_GEM_DOMAIN_CPU
-	 */
-	if (obj->page_cpu_valid == NULL) {
-		obj->page_cpu_valid = kzalloc(obj->base.size / PAGE_SIZE,
-					      GFP_KERNEL);
-		if (obj->page_cpu_valid == NULL)
-			return -ENOMEM;
-	} else if ((obj->base.read_domains & I915_GEM_DOMAIN_CPU) == 0)
-		memset(obj->page_cpu_valid, 0, obj->base.size / PAGE_SIZE);
-
-	/* Flush the cache on any pages that are still invalid from the CPU's
-	 * perspective.
-	 */
-	for (i = offset / PAGE_SIZE; i <= (offset + size - 1) / PAGE_SIZE;
-	     i++) {
-		if (obj->page_cpu_valid[i])
-			continue;
-
-		drm_clflush_pages(obj->pages + i, 1);
-
-		obj->page_cpu_valid[i] = 1;
-	}
-
-	/* It should now be out of any other write domains, and we can update
-	 * the domain values for our changes.
-	 */
-	BUG_ON((obj->base.write_domain & ~I915_GEM_DOMAIN_CPU) != 0);
-
-	old_read_domains = obj->base.read_domains;
-	obj->base.read_domains |= I915_GEM_DOMAIN_CPU;
-
-	trace_i915_gem_object_change_domain(obj,
-					    old_read_domains,
-					    obj->base.write_domain);
-
-	return 0;
-}
-
 /* Throttle our rendering by waiting until the ring has completed our requests
  * emitted over 20 msec ago.
  *
@@ -3280,28 +3032,7 @@ i915_gem_ring_throttle(struct drm_device *dev, struct drm_file *file)
 	if (seqno == 0)
 		return 0;
 
-	ret = 0;
-	if (!i915_seqno_passed(ring->get_seqno(ring), seqno)) {
-		/* And wait for the seqno passing without holding any locks and
-		 * causing extra latency for others. This is safe as the irq
-		 * generation is designed to be run atomically and so is
-		 * lockless.
-		 */
-		if (ring->irq_get(ring)) {
-			ret = wait_event_interruptible(ring->irq_queue,
-						       i915_seqno_passed(ring->get_seqno(ring), seqno)
-						       || atomic_read(&dev_priv->mm.wedged));
-			ring->irq_put(ring);
-
-			if (ret == 0 && atomic_read(&dev_priv->mm.wedged))
-				ret = -EIO;
-		} else if (wait_for_atomic(i915_seqno_passed(ring->get_seqno(ring),
-							     seqno) ||
-				    atomic_read(&dev_priv->mm.wedged), 3000)) {
-			ret = -EBUSY;
-		}
-	}
-
+	ret = __wait_seqno(ring, seqno, true);
 	if (ret == 0)
 		queue_delayed_work(dev_priv->wq, &dev_priv->mm.retire_work, 0);
 
@@ -3313,12 +3044,9 @@ i915_gem_object_pin(struct drm_i915_gem_object *obj,
 		    uint32_t alignment,
 		    bool map_and_fenceable)
 {
-	struct drm_device *dev = obj->base.dev;
-	struct drm_i915_private *dev_priv = dev->dev_private;
 	int ret;
 
 	BUG_ON(obj->pin_count == DRM_I915_GEM_OBJECT_MAX_PIN_COUNT);
-	WARN_ON(i915_verify_lists(dev));
 
 	if (obj->gtt_space != NULL) {
 		if ((alignment && obj->gtt_offset & (alignment - 1)) ||
@@ -3343,34 +3071,23 @@ i915_gem_object_pin(struct drm_i915_gem_object *obj,
 			return ret;
 	}
 
-	if (obj->pin_count++ == 0) {
-		if (!obj->active)
-			list_move_tail(&obj->mm_list,
-				       &dev_priv->mm.pinned_list);
-	}
+	if (!obj->has_global_gtt_mapping && map_and_fenceable)
+		i915_gem_gtt_bind_object(obj, obj->cache_level);
+
+	obj->pin_count++;
 	obj->pin_mappable |= map_and_fenceable;
 
-	WARN_ON(i915_verify_lists(dev));
 	return 0;
 }
 
 void
 i915_gem_object_unpin(struct drm_i915_gem_object *obj)
 {
-	struct drm_device *dev = obj->base.dev;
-	drm_i915_private_t *dev_priv = dev->dev_private;
-
-	WARN_ON(i915_verify_lists(dev));
 	BUG_ON(obj->pin_count == 0);
 	BUG_ON(obj->gtt_space == NULL);
 
-	if (--obj->pin_count == 0) {
-		if (!obj->active)
-			list_move_tail(&obj->mm_list,
-				       &dev_priv->mm.inactive_list);
+	if (--obj->pin_count == 0)
 		obj->pin_mappable = false;
-	}
-	WARN_ON(i915_verify_lists(dev));
 }
 
 int
@@ -3494,20 +3211,9 @@ i915_gem_busy_ioctl(struct drm_device *dev, void *data,
 		if (obj->base.write_domain & I915_GEM_GPU_DOMAINS) {
 			ret = i915_gem_flush_ring(obj->ring,
 						  0, obj->base.write_domain);
-		} else if (obj->ring->outstanding_lazy_request ==
-			   obj->last_rendering_seqno) {
-			struct drm_i915_gem_request *request;
-
-			/* This ring is not being cleared by active usage,
-			 * so emit a request to do so.
-			 */
-			request = kzalloc(sizeof(*request), GFP_KERNEL);
-			if (request) {
-				ret = i915_add_request(obj->ring, NULL, request);
-				if (ret)
-					kfree(request);
-			} else
-				ret = -ENOMEM;
+		} else {
+			ret = i915_gem_check_olr(obj->ring,
+						 obj->last_rendering_seqno);
 		}
 
 		/* Update the active list for the hardware's current position.
@@ -3643,46 +3349,42 @@ int i915_gem_init_object(struct drm_gem_object *obj)
 	return 0;
 }
 
-static void i915_gem_free_object_tail(struct drm_i915_gem_object *obj)
+void i915_gem_free_object(struct drm_gem_object *gem_obj)
 {
+	struct drm_i915_gem_object *obj = to_intel_bo(gem_obj);
 	struct drm_device *dev = obj->base.dev;
 	drm_i915_private_t *dev_priv = dev->dev_private;
-	int ret;
-
-	ret = i915_gem_object_unbind(obj);
-	if (ret == -ERESTARTSYS) {
-		list_move(&obj->mm_list,
-			  &dev_priv->mm.deferred_free_list);
-		return;
-	}
 
 	trace_i915_gem_object_destroy(obj);
 
+	if (gem_obj->import_attach)
+		drm_prime_gem_destroy(gem_obj, obj->sg_table);
+
+	if (obj->phys_obj)
+		i915_gem_detach_phys_object(dev, obj);
+
+	obj->pin_count = 0;
+	if (WARN_ON(i915_gem_object_unbind(obj) == -ERESTARTSYS)) {
+		bool was_interruptible;
+
+		was_interruptible = dev_priv->mm.interruptible;
+		dev_priv->mm.interruptible = false;
+
+		WARN_ON(i915_gem_object_unbind(obj));
+
+		dev_priv->mm.interruptible = was_interruptible;
+	}
+
 	if (obj->base.map_list.map)
 		drm_gem_free_mmap_offset(&obj->base);
 
 	drm_gem_object_release(&obj->base);
 	i915_gem_info_remove_obj(dev_priv, obj->base.size);
 
-	kfree(obj->page_cpu_valid);
 	kfree(obj->bit_17);
 	kfree(obj);
 }
 
-void i915_gem_free_object(struct drm_gem_object *gem_obj)
-{
-	struct drm_i915_gem_object *obj = to_intel_bo(gem_obj);
-	struct drm_device *dev = obj->base.dev;
-
-	while (obj->pin_count > 0)
-		i915_gem_object_unpin(obj);
-
-	if (obj->phys_obj)
-		i915_gem_detach_phys_object(dev, obj);
-
-	i915_gem_free_object_tail(obj);
-}
-
 int
 i915_gem_idle(struct drm_device *dev)
 {
@@ -3696,20 +3398,16 @@ i915_gem_idle(struct drm_device *dev)
 		return 0;
 	}
 
-	ret = i915_gpu_idle(dev, true);
+	ret = i915_gpu_idle(dev);
 	if (ret) {
 		mutex_unlock(&dev->struct_mutex);
 		return ret;
 	}
+	i915_gem_retire_requests(dev);
 
 	/* Under UMS, be paranoid and evict. */
-	if (!drm_core_check_feature(dev, DRIVER_MODESET)) {
-		ret = i915_gem_evict_inactive(dev, false);
-		if (ret) {
-			mutex_unlock(&dev->struct_mutex);
-			return ret;
-		}
-	}
+	if (!drm_core_check_feature(dev, DRIVER_MODESET))
+		i915_gem_evict_everything(dev, false);
 
 	i915_gem_reset_fences(dev);
 
@@ -3747,9 +3445,9 @@ void i915_gem_init_swizzling(struct drm_device *dev)
 
 	I915_WRITE(TILECTL, I915_READ(TILECTL) | TILECTL_SWZCTL);
 	if (IS_GEN6(dev))
-		I915_WRITE(ARB_MODE, ARB_MODE_ENABLE(ARB_MODE_SWIZZLE_SNB));
+		I915_WRITE(ARB_MODE, _MASKED_BIT_ENABLE(ARB_MODE_SWIZZLE_SNB));
 	else
-		I915_WRITE(ARB_MODE, ARB_MODE_ENABLE(ARB_MODE_SWIZZLE_IVB));
+		I915_WRITE(ARB_MODE, _MASKED_BIT_ENABLE(ARB_MODE_SWIZZLE_IVB));
 }
 
 void i915_gem_init_ppgtt(struct drm_device *dev)
@@ -3787,21 +3485,27 @@ void i915_gem_init_ppgtt(struct drm_device *dev)
 	pd_offset <<= 16;
 
 	if (INTEL_INFO(dev)->gen == 6) {
-		uint32_t ecochk = I915_READ(GAM_ECOCHK);
+		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, GFX_MODE_ENABLE(GFX_PPGTT_ENABLE));
+		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 (i = 0; i < I915_NUM_RINGS; i++) {
-		ring = &dev_priv->ring[i];
-
+	for_each_ring(ring, dev_priv, i) {
 		if (INTEL_INFO(dev)->gen >= 7)
 			I915_WRITE(RING_MODE_GEN7(ring),
-				   GFX_MODE_ENABLE(GFX_PPGTT_ENABLE));
+				   _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);
@@ -3845,14 +3549,80 @@ cleanup_render_ring:
 	return ret;
 }
 
+static bool
+intel_enable_ppgtt(struct drm_device *dev)
+{
+	if (i915_enable_ppgtt >= 0)
+		return i915_enable_ppgtt;
+
+#ifdef CONFIG_INTEL_IOMMU
+	/* Disable ppgtt on SNB if VT-d is on. */
+	if (INTEL_INFO(dev)->gen == 6 && intel_iommu_gfx_mapped)
+		return false;
+#endif
+
+	return true;
+}
+
+int i915_gem_init(struct drm_device *dev)
+{
+	struct drm_i915_private *dev_priv = dev->dev_private;
+	unsigned long gtt_size, mappable_size;
+	int ret;
+
+	gtt_size = dev_priv->mm.gtt->gtt_total_entries << PAGE_SHIFT;
+	mappable_size = dev_priv->mm.gtt->gtt_mappable_entries << PAGE_SHIFT;
+
+	mutex_lock(&dev->struct_mutex);
+	if (intel_enable_ppgtt(dev) && HAS_ALIASING_PPGTT(dev)) {
+		/* 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_init_global_gtt(dev, 0, mappable_size, gtt_size);
+
+		ret = i915_gem_init_aliasing_ppgtt(dev);
+		if (ret) {
+			mutex_unlock(&dev->struct_mutex);
+			return ret;
+		}
+	} else {
+		/* Let GEM Manage all of the aperture.
+		 *
+		 * However, leave one page at the end still bound to the scratch
+		 * page.  There are a number of places where the hardware
+		 * apparently prefetches past the end of the object, and we've
+		 * seen multiple hangs with the GPU head pointer stuck in a
+		 * batchbuffer bound at the last page of the aperture.  One page
+		 * should be enough to keep any prefetching inside of the
+		 * aperture.
+		 */
+		i915_gem_init_global_gtt(dev, 0, mappable_size,
+					 gtt_size);
+	}
+
+	ret = i915_gem_init_hw(dev);
+	mutex_unlock(&dev->struct_mutex);
+	if (ret) {
+		i915_gem_cleanup_aliasing_ppgtt(dev);
+		return ret;
+	}
+
+	/* Allow hardware batchbuffers unless told otherwise, but not for KMS. */
+	if (!drm_core_check_feature(dev, DRIVER_MODESET))
+		dev_priv->dri1.allow_batchbuffer = 1;
+	return 0;
+}
+
 void
 i915_gem_cleanup_ringbuffer(struct drm_device *dev)
 {
 	drm_i915_private_t *dev_priv = dev->dev_private;
+	struct intel_ring_buffer *ring;
 	int i;
 
-	for (i = 0; i < I915_NUM_RINGS; i++)
-		intel_cleanup_ring_buffer(&dev_priv->ring[i]);
+	for_each_ring(ring, dev_priv, i)
+		intel_cleanup_ring_buffer(ring);
 }
 
 int
@@ -3860,7 +3630,7 @@ i915_gem_entervt_ioctl(struct drm_device *dev, void *data,
 		       struct drm_file *file_priv)
 {
 	drm_i915_private_t *dev_priv = dev->dev_private;
-	int ret, i;
+	int ret;
 
 	if (drm_core_check_feature(dev, DRIVER_MODESET))
 		return 0;
@@ -3882,10 +3652,6 @@ i915_gem_entervt_ioctl(struct drm_device *dev, void *data,
 	BUG_ON(!list_empty(&dev_priv->mm.active_list));
 	BUG_ON(!list_empty(&dev_priv->mm.flushing_list));
 	BUG_ON(!list_empty(&dev_priv->mm.inactive_list));
-	for (i = 0; i < I915_NUM_RINGS; i++) {
-		BUG_ON(!list_empty(&dev_priv->ring[i].active_list));
-		BUG_ON(!list_empty(&dev_priv->ring[i].request_list));
-	}
 	mutex_unlock(&dev->struct_mutex);
 
 	ret = drm_irq_install(dev);
@@ -3944,9 +3710,7 @@ i915_gem_load(struct drm_device *dev)
 	INIT_LIST_HEAD(&dev_priv->mm.active_list);
 	INIT_LIST_HEAD(&dev_priv->mm.flushing_list);
 	INIT_LIST_HEAD(&dev_priv->mm.inactive_list);
-	INIT_LIST_HEAD(&dev_priv->mm.pinned_list);
 	INIT_LIST_HEAD(&dev_priv->mm.fence_list);
-	INIT_LIST_HEAD(&dev_priv->mm.deferred_free_list);
 	INIT_LIST_HEAD(&dev_priv->mm.gtt_list);
 	for (i = 0; i < I915_NUM_RINGS; i++)
 		init_ring_lists(&dev_priv->ring[i]);
@@ -3958,12 +3722,8 @@ i915_gem_load(struct drm_device *dev)
 
 	/* On GEN3 we really need to make sure the ARB C3 LP bit is set */
 	if (IS_GEN3(dev)) {
-		u32 tmp = I915_READ(MI_ARB_STATE);
-		if (!(tmp & MI_ARB_C3_LP_WRITE_ENABLE)) {
-			/* arb state is a masked write, so set bit + bit in mask */
-			tmp = MI_ARB_C3_LP_WRITE_ENABLE | (MI_ARB_C3_LP_WRITE_ENABLE << MI_ARB_MASK_SHIFT);
-			I915_WRITE(MI_ARB_STATE, tmp);
-		}
+		I915_WRITE(MI_ARB_STATE,
+			   _MASKED_BIT_ENABLE(MI_ARB_C3_LP_WRITE_ENABLE));
 	}
 
 	dev_priv->relative_constants_mode = I915_EXEC_CONSTANTS_REL_GENERAL;
@@ -3978,9 +3738,7 @@ i915_gem_load(struct drm_device *dev)
 		dev_priv->num_fence_regs = 8;
 
 	/* Initialize fence registers to zero */
-	for (i = 0; i < dev_priv->num_fence_regs; i++) {
-		i915_gem_clear_fence_reg(dev, &dev_priv->fence_regs[i]);
-	}
+	i915_gem_reset_fences(dev);
 
 	i915_gem_detect_bit_6_swizzle(dev);
 	init_waitqueue_head(&dev_priv->pending_flip_queue);
@@ -4268,7 +4026,7 @@ rescan:
 		 * This has a dramatic impact to reduce the number of
 		 * OOM-killer events whilst running the GPU aggressively.
 		 */
-		if (i915_gpu_idle(dev, true) == 0)
+		if (i915_gpu_idle(dev) == 0)
 			goto rescan;
 	}
 	mutex_unlock(&dev->struct_mutex);
diff --git a/drivers/gpu/drm/i915/i915_gem_debug.c b/drivers/gpu/drm/i915/i915_gem_debug.c
index cc93cac242d..a4f6aaabca9 100644
--- a/drivers/gpu/drm/i915/i915_gem_debug.c
+++ b/drivers/gpu/drm/i915/i915_gem_debug.c
@@ -114,22 +114,6 @@ i915_verify_lists(struct drm_device *dev)
 		}
 	}
 
-	list_for_each_entry(obj, &dev_priv->mm.pinned_list, list) {
-		if (obj->base.dev != dev ||
-		    !atomic_read(&obj->base.refcount.refcount)) {
-			DRM_ERROR("freed pinned %p\n", obj);
-			err++;
-			break;
-		} else if (!obj->pin_count || obj->active ||
-			   (obj->base.write_domain & I915_GEM_GPU_DOMAINS)) {
-			DRM_ERROR("invalid pinned %p (p %d a %d w %x)\n",
-				  obj,
-				  obj->pin_count, obj->active,
-				  obj->base.write_domain);
-			err++;
-		}
-	}
-
 	return warned = err;
 }
 #endif /* WATCH_INACTIVE */
diff --git a/drivers/gpu/drm/i915/i915_gem_dmabuf.c b/drivers/gpu/drm/i915/i915_gem_dmabuf.c
new file mode 100644
index 00000000000..8e269178d6a
--- /dev/null
+++ b/drivers/gpu/drm/i915/i915_gem_dmabuf.c
@@ -0,0 +1,171 @@
+/*
+ * 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 (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:
+ *	Dave Airlie <airlied@redhat.com>
+ */
+#include "drmP.h"
+#include "i915_drv.h"
+#include <linux/dma-buf.h>
+
+static struct sg_table *i915_gem_map_dma_buf(struct dma_buf_attachment *attachment,
+				      enum dma_data_direction dir)
+{
+	struct drm_i915_gem_object *obj = attachment->dmabuf->priv;
+	struct drm_device *dev = obj->base.dev;
+	int npages = obj->base.size / PAGE_SIZE;
+	struct sg_table *sg = NULL;
+	int ret;
+	int nents;
+
+	ret = i915_mutex_lock_interruptible(dev);
+	if (ret)
+		return ERR_PTR(ret);
+
+	if (!obj->pages) {
+		ret = i915_gem_object_get_pages_gtt(obj, __GFP_NORETRY | __GFP_NOWARN);
+		if (ret)
+			goto out;
+	}
+
+	/* link the pages into an SG then map the sg */
+	sg = drm_prime_pages_to_sg(obj->pages, npages);
+	nents = dma_map_sg(attachment->dev, sg->sgl, sg->nents, dir);
+out:
+	mutex_unlock(&dev->struct_mutex);
+	return sg;
+}
+
+static void i915_gem_unmap_dma_buf(struct dma_buf_attachment *attachment,
+			    struct sg_table *sg, enum dma_data_direction dir)
+{
+	dma_unmap_sg(attachment->dev, sg->sgl, sg->nents, dir);
+	sg_free_table(sg);
+	kfree(sg);
+}
+
+static void i915_gem_dmabuf_release(struct dma_buf *dma_buf)
+{
+	struct drm_i915_gem_object *obj = dma_buf->priv;
+
+	if (obj->base.export_dma_buf == dma_buf) {
+		/* drop the reference on the export fd holds */
+		obj->base.export_dma_buf = NULL;
+		drm_gem_object_unreference_unlocked(&obj->base);
+	}
+}
+
+static void *i915_gem_dmabuf_kmap_atomic(struct dma_buf *dma_buf, unsigned long page_num)
+{
+	return NULL;
+}
+
+static void i915_gem_dmabuf_kunmap_atomic(struct dma_buf *dma_buf, unsigned long page_num, void *addr)
+{
+
+}
+static void *i915_gem_dmabuf_kmap(struct dma_buf *dma_buf, unsigned long page_num)
+{
+	return NULL;
+}
+
+static void i915_gem_dmabuf_kunmap(struct dma_buf *dma_buf, unsigned long page_num, void *addr)
+{
+
+}
+
+static const struct dma_buf_ops i915_dmabuf_ops =  {
+	.map_dma_buf = i915_gem_map_dma_buf,
+	.unmap_dma_buf = i915_gem_unmap_dma_buf,
+	.release = i915_gem_dmabuf_release,
+	.kmap = i915_gem_dmabuf_kmap,
+	.kmap_atomic = i915_gem_dmabuf_kmap_atomic,
+	.kunmap = i915_gem_dmabuf_kunmap,
+	.kunmap_atomic = i915_gem_dmabuf_kunmap_atomic,
+};
+
+struct dma_buf *i915_gem_prime_export(struct drm_device *dev,
+				struct drm_gem_object *gem_obj, int flags)
+{
+	struct drm_i915_gem_object *obj = to_intel_bo(gem_obj);
+
+	return dma_buf_export(obj, &i915_dmabuf_ops,
+						  obj->base.size, 0600);
+}
+
+struct drm_gem_object *i915_gem_prime_import(struct drm_device *dev,
+				struct dma_buf *dma_buf)
+{
+	struct dma_buf_attachment *attach;
+	struct sg_table *sg;
+	struct drm_i915_gem_object *obj;
+	int npages;
+	int size;
+	int ret;
+
+	/* is this one of own objects? */
+	if (dma_buf->ops == &i915_dmabuf_ops) {
+		obj = dma_buf->priv;
+		/* is it from our device? */
+		if (obj->base.dev == dev) {
+			drm_gem_object_reference(&obj->base);
+			return &obj->base;
+		}
+	}
+
+	/* need to attach */
+	attach = dma_buf_attach(dma_buf, dev->dev);
+	if (IS_ERR(attach))
+		return ERR_CAST(attach);
+
+	sg = dma_buf_map_attachment(attach, DMA_BIDIRECTIONAL);
+	if (IS_ERR(sg)) {
+		ret = PTR_ERR(sg);
+		goto fail_detach;
+	}
+
+	size = dma_buf->size;
+	npages = size / PAGE_SIZE;
+
+	obj = kzalloc(sizeof(*obj), GFP_KERNEL);
+	if (obj == NULL) {
+		ret = -ENOMEM;
+		goto fail_unmap;
+	}
+
+	ret = drm_gem_private_object_init(dev, &obj->base, size);
+	if (ret) {
+		kfree(obj);
+		goto fail_unmap;
+	}
+
+	obj->sg_table = sg;
+	obj->base.import_attach = attach;
+
+	return &obj->base;
+
+fail_unmap:
+	dma_buf_unmap_attachment(attach, sg, DMA_BIDIRECTIONAL);
+fail_detach:
+	dma_buf_detach(dma_buf, attach);
+	return ERR_PTR(ret);
+}
diff --git a/drivers/gpu/drm/i915/i915_gem_evict.c b/drivers/gpu/drm/i915/i915_gem_evict.c
index 21a82710f4b..ae7c24e12e5 100644
--- a/drivers/gpu/drm/i915/i915_gem_evict.c
+++ b/drivers/gpu/drm/i915/i915_gem_evict.c
@@ -35,6 +35,9 @@
 static bool
 mark_free(struct drm_i915_gem_object *obj, struct list_head *unwind)
 {
+	if (obj->pin_count)
+		return false;
+
 	list_add(&obj->exec_list, unwind);
 	return drm_mm_scan_add_block(obj->gtt_space);
 }
@@ -90,7 +93,7 @@ i915_gem_evict_something(struct drm_device *dev, int min_size,
 	/* Now merge in the soon-to-be-expired objects... */
 	list_for_each_entry(obj, &dev_priv->mm.active_list, mm_list) {
 		/* Does the object require an outstanding flush? */
-		if (obj->base.write_domain || obj->pin_count)
+		if (obj->base.write_domain)
 			continue;
 
 		if (mark_free(obj, &unwind_list))
@@ -99,14 +102,11 @@ i915_gem_evict_something(struct drm_device *dev, int min_size,
 
 	/* Finally add anything with a pending flush (in order of retirement) */
 	list_for_each_entry(obj, &dev_priv->mm.flushing_list, mm_list) {
-		if (obj->pin_count)
-			continue;
-
 		if (mark_free(obj, &unwind_list))
 			goto found;
 	}
 	list_for_each_entry(obj, &dev_priv->mm.active_list, mm_list) {
-		if (!obj->base.write_domain || obj->pin_count)
+		if (!obj->base.write_domain)
 			continue;
 
 		if (mark_free(obj, &unwind_list))
@@ -166,8 +166,9 @@ int
 i915_gem_evict_everything(struct drm_device *dev, bool purgeable_only)
 {
 	drm_i915_private_t *dev_priv = dev->dev_private;
-	int ret;
+	struct drm_i915_gem_object *obj, *next;
 	bool lists_empty;
+	int ret;
 
 	lists_empty = (list_empty(&dev_priv->mm.inactive_list) &&
 		       list_empty(&dev_priv->mm.flushing_list) &&
@@ -177,29 +178,24 @@ i915_gem_evict_everything(struct drm_device *dev, bool purgeable_only)
 
 	trace_i915_gem_evict_everything(dev, purgeable_only);
 
-	/* Flush everything (on to the inactive lists) and evict */
-	ret = i915_gpu_idle(dev, true);
+	/* The gpu_idle will flush everything in the write domain to the
+	 * active list. Then we must move everything off the active list
+	 * with retire requests.
+	 */
+	ret = i915_gpu_idle(dev);
 	if (ret)
 		return ret;
 
-	BUG_ON(!list_empty(&dev_priv->mm.flushing_list));
+	i915_gem_retire_requests(dev);
 
-	return i915_gem_evict_inactive(dev, purgeable_only);
-}
-
-/** Unbinds all inactive objects. */
-int
-i915_gem_evict_inactive(struct drm_device *dev, bool purgeable_only)
-{
-	drm_i915_private_t *dev_priv = dev->dev_private;
-	struct drm_i915_gem_object *obj, *next;
+	BUG_ON(!list_empty(&dev_priv->mm.flushing_list));
 
+	/* Having flushed everything, unbind() should never raise an error */
 	list_for_each_entry_safe(obj, next,
 				 &dev_priv->mm.inactive_list, mm_list) {
 		if (!purgeable_only || obj->madv != I915_MADV_WILLNEED) {
-			int ret = i915_gem_object_unbind(obj);
-			if (ret)
-				return ret;
+			if (obj->pin_count == 0)
+				WARN_ON(i915_gem_object_unbind(obj));
 		}
 	}
 
diff --git a/drivers/gpu/drm/i915/i915_gem_execbuffer.c b/drivers/gpu/drm/i915/i915_gem_execbuffer.c
index de431942ded..974a9f1068a 100644
--- a/drivers/gpu/drm/i915/i915_gem_execbuffer.c
+++ b/drivers/gpu/drm/i915/i915_gem_execbuffer.c
@@ -266,6 +266,12 @@ eb_destroy(struct eb_objects *eb)
 	kfree(eb);
 }
 
+static inline int use_cpu_reloc(struct drm_i915_gem_object *obj)
+{
+	return (obj->base.write_domain == I915_GEM_DOMAIN_CPU ||
+		obj->cache_level != I915_CACHE_NONE);
+}
+
 static int
 i915_gem_execbuffer_relocate_entry(struct drm_i915_gem_object *obj,
 				   struct eb_objects *eb,
@@ -273,6 +279,7 @@ i915_gem_execbuffer_relocate_entry(struct drm_i915_gem_object *obj,
 {
 	struct drm_device *dev = obj->base.dev;
 	struct drm_gem_object *target_obj;
+	struct drm_i915_gem_object *target_i915_obj;
 	uint32_t target_offset;
 	int ret = -EINVAL;
 
@@ -281,7 +288,8 @@ i915_gem_execbuffer_relocate_entry(struct drm_i915_gem_object *obj,
 	if (unlikely(target_obj == NULL))
 		return -ENOENT;
 
-	target_offset = to_intel_bo(target_obj)->gtt_offset;
+	target_i915_obj = to_intel_bo(target_obj);
+	target_offset = target_i915_obj->gtt_offset;
 
 	/* The target buffer should have appeared before us in the
 	 * exec_object list, so it should have a GTT space bound by now.
@@ -352,11 +360,19 @@ i915_gem_execbuffer_relocate_entry(struct drm_i915_gem_object *obj,
 		return ret;
 	}
 
+	/* We can't wait for rendering with pagefaults disabled */
+	if (obj->active && in_atomic())
+		return -EFAULT;
+
 	reloc->delta += target_offset;
-	if (obj->base.write_domain == I915_GEM_DOMAIN_CPU) {
+	if (use_cpu_reloc(obj)) {
 		uint32_t page_offset = reloc->offset & ~PAGE_MASK;
 		char *vaddr;
 
+		ret = i915_gem_object_set_to_cpu_domain(obj, 1);
+		if (ret)
+			return ret;
+
 		vaddr = kmap_atomic(obj->pages[reloc->offset >> PAGE_SHIFT]);
 		*(uint32_t *)(vaddr + page_offset) = reloc->delta;
 		kunmap_atomic(vaddr);
@@ -365,11 +381,11 @@ i915_gem_execbuffer_relocate_entry(struct drm_i915_gem_object *obj,
 		uint32_t __iomem *reloc_entry;
 		void __iomem *reloc_page;
 
-		/* We can't wait for rendering with pagefaults disabled */
-		if (obj->active && in_atomic())
-			return -EFAULT;
+		ret = i915_gem_object_set_to_gtt_domain(obj, true);
+		if (ret)
+			return ret;
 
-		ret = i915_gem_object_set_to_gtt_domain(obj, 1);
+		ret = i915_gem_object_put_fence(obj);
 		if (ret)
 			return ret;
 
@@ -383,6 +399,16 @@ i915_gem_execbuffer_relocate_entry(struct drm_i915_gem_object *obj,
 		io_mapping_unmap_atomic(reloc_page);
 	}
 
+	/* Sandybridge PPGTT errata: We need a global gtt mapping for MI and
+	 * pipe_control writes because the gpu doesn't properly redirect them
+	 * through the ppgtt for non_secure batchbuffers. */
+	if (unlikely(IS_GEN6(dev) &&
+	    reloc->write_domain == I915_GEM_DOMAIN_INSTRUCTION &&
+	    !target_i915_obj->has_global_gtt_mapping)) {
+		i915_gem_gtt_bind_object(target_i915_obj,
+					 target_i915_obj->cache_level);
+	}
+
 	/* and update the user's relocation entry */
 	reloc->presumed_offset = target_offset;
 
@@ -393,30 +419,46 @@ static int
 i915_gem_execbuffer_relocate_object(struct drm_i915_gem_object *obj,
 				    struct eb_objects *eb)
 {
+#define N_RELOC(x) ((x) / sizeof(struct drm_i915_gem_relocation_entry))
+	struct drm_i915_gem_relocation_entry stack_reloc[N_RELOC(512)];
 	struct drm_i915_gem_relocation_entry __user *user_relocs;
 	struct drm_i915_gem_exec_object2 *entry = obj->exec_entry;
-	int i, ret;
+	int remain, ret;
 
 	user_relocs = (void __user *)(uintptr_t)entry->relocs_ptr;
-	for (i = 0; i < entry->relocation_count; i++) {
-		struct drm_i915_gem_relocation_entry reloc;
 
-		if (__copy_from_user_inatomic(&reloc,
-					      user_relocs+i,
-					      sizeof(reloc)))
+	remain = entry->relocation_count;
+	while (remain) {
+		struct drm_i915_gem_relocation_entry *r = stack_reloc;
+		int count = remain;
+		if (count > ARRAY_SIZE(stack_reloc))
+			count = ARRAY_SIZE(stack_reloc);
+		remain -= count;
+
+		if (__copy_from_user_inatomic(r, user_relocs, count*sizeof(r[0])))
 			return -EFAULT;
 
-		ret = i915_gem_execbuffer_relocate_entry(obj, eb, &reloc);
-		if (ret)
-			return ret;
+		do {
+			u64 offset = r->presumed_offset;
 
-		if (__copy_to_user_inatomic(&user_relocs[i].presumed_offset,
-					    &reloc.presumed_offset,
-					    sizeof(reloc.presumed_offset)))
-			return -EFAULT;
+			ret = i915_gem_execbuffer_relocate_entry(obj, eb, r);
+			if (ret)
+				return ret;
+
+			if (r->presumed_offset != offset &&
+			    __copy_to_user_inatomic(&user_relocs->presumed_offset,
+						    &r->presumed_offset,
+						    sizeof(r->presumed_offset))) {
+				return -EFAULT;
+			}
+
+			user_relocs++;
+			r++;
+		} while (--count);
 	}
 
 	return 0;
+#undef N_RELOC
 }
 
 static int
@@ -465,6 +507,13 @@ i915_gem_execbuffer_relocate(struct drm_device *dev,
 #define  __EXEC_OBJECT_HAS_FENCE (1<<31)
 
 static int
+need_reloc_mappable(struct drm_i915_gem_object *obj)
+{
+	struct drm_i915_gem_exec_object2 *entry = obj->exec_entry;
+	return entry->relocation_count && !use_cpu_reloc(obj);
+}
+
+static int
 pin_and_fence_object(struct drm_i915_gem_object *obj,
 		     struct intel_ring_buffer *ring)
 {
@@ -477,8 +526,7 @@ pin_and_fence_object(struct drm_i915_gem_object *obj,
 		has_fenced_gpu_access &&
 		entry->flags & EXEC_OBJECT_NEEDS_FENCE &&
 		obj->tiling_mode != I915_TILING_NONE;
-	need_mappable =
-		entry->relocation_count ? true : need_fence;
+	need_mappable = need_fence || need_reloc_mappable(obj);
 
 	ret = i915_gem_object_pin(obj, entry->alignment, need_mappable);
 	if (ret)
@@ -486,18 +534,13 @@ pin_and_fence_object(struct drm_i915_gem_object *obj,
 
 	if (has_fenced_gpu_access) {
 		if (entry->flags & EXEC_OBJECT_NEEDS_FENCE) {
-			if (obj->tiling_mode) {
-				ret = i915_gem_object_get_fence(obj, ring);
-				if (ret)
-					goto err_unpin;
+			ret = i915_gem_object_get_fence(obj);
+			if (ret)
+				goto err_unpin;
 
+			if (i915_gem_object_pin_fence(obj))
 				entry->flags |= __EXEC_OBJECT_HAS_FENCE;
-				i915_gem_object_pin_fence(obj);
-			} else {
-				ret = i915_gem_object_put_fence(obj);
-				if (ret)
-					goto err_unpin;
-			}
+
 			obj->pending_fenced_gpu_access = true;
 		}
 	}
@@ -535,8 +578,7 @@ i915_gem_execbuffer_reserve(struct intel_ring_buffer *ring,
 			has_fenced_gpu_access &&
 			entry->flags & EXEC_OBJECT_NEEDS_FENCE &&
 			obj->tiling_mode != I915_TILING_NONE;
-		need_mappable =
-			entry->relocation_count ? true : need_fence;
+		need_mappable = need_fence || need_reloc_mappable(obj);
 
 		if (need_mappable)
 			list_move(&obj->exec_list, &ordered_objects);
@@ -576,8 +618,7 @@ i915_gem_execbuffer_reserve(struct intel_ring_buffer *ring,
 				has_fenced_gpu_access &&
 				entry->flags & EXEC_OBJECT_NEEDS_FENCE &&
 				obj->tiling_mode != I915_TILING_NONE;
-			need_mappable =
-				entry->relocation_count ? true : need_fence;
+			need_mappable = need_fence || need_reloc_mappable(obj);
 
 			if ((entry->alignment && obj->gtt_offset & (entry->alignment - 1)) ||
 			    (need_mappable && !obj->map_and_fenceable))
@@ -798,64 +839,6 @@ i915_gem_execbuffer_flush(struct drm_device *dev,
 	return 0;
 }
 
-static bool
-intel_enable_semaphores(struct drm_device *dev)
-{
-	if (INTEL_INFO(dev)->gen < 6)
-		return 0;
-
-	if (i915_semaphores >= 0)
-		return i915_semaphores;
-
-	/* Disable semaphores on SNB */
-	if (INTEL_INFO(dev)->gen == 6)
-		return 0;
-
-	return 1;
-}
-
-static int
-i915_gem_execbuffer_sync_rings(struct drm_i915_gem_object *obj,
-			       struct intel_ring_buffer *to)
-{
-	struct intel_ring_buffer *from = obj->ring;
-	u32 seqno;
-	int ret, idx;
-
-	if (from == NULL || to == from)
-		return 0;
-
-	/* XXX gpu semaphores are implicated in various hard hangs on SNB */
-	if (!intel_enable_semaphores(obj->base.dev))
-		return i915_gem_object_wait_rendering(obj);
-
-	idx = intel_ring_sync_index(from, to);
-
-	seqno = obj->last_rendering_seqno;
-	if (seqno <= from->sync_seqno[idx])
-		return 0;
-
-	if (seqno == from->outstanding_lazy_request) {
-		struct drm_i915_gem_request *request;
-
-		request = kzalloc(sizeof(*request), GFP_KERNEL);
-		if (request == NULL)
-			return -ENOMEM;
-
-		ret = i915_add_request(from, NULL, request);
-		if (ret) {
-			kfree(request);
-			return ret;
-		}
-
-		seqno = request->seqno;
-	}
-
-	from->sync_seqno[idx] = seqno;
-
-	return to->sync_to(to, from, seqno - 1);
-}
-
 static int
 i915_gem_execbuffer_wait_for_flips(struct intel_ring_buffer *ring, u32 flips)
 {
@@ -917,7 +900,7 @@ i915_gem_execbuffer_move_to_gpu(struct intel_ring_buffer *ring,
 	}
 
 	list_for_each_entry(obj, objects, exec_list) {
-		ret = i915_gem_execbuffer_sync_rings(obj, ring);
+		ret = i915_gem_object_sync(obj, ring);
 		if (ret)
 			return ret;
 	}
@@ -955,7 +938,7 @@ validate_exec_list(struct drm_i915_gem_exec_object2 *exec,
 		if (!access_ok(VERIFY_WRITE, ptr, length))
 			return -EFAULT;
 
-		if (fault_in_pages_readable(ptr, length))
+		if (fault_in_multipages_readable(ptr, length))
 			return -EFAULT;
 	}
 
@@ -984,11 +967,14 @@ i915_gem_execbuffer_move_to_active(struct list_head *objects,
 			obj->pending_gpu_write = true;
 			list_move_tail(&obj->gpu_write_list,
 				       &ring->gpu_write_list);
-			intel_mark_busy(ring->dev, obj);
+			if (obj->pin_count) /* check for potential scanout */
+				intel_mark_busy(ring->dev, obj);
 		}
 
 		trace_i915_gem_object_change_domain(obj, old_read, old_write);
 	}
+
+	intel_mark_busy(ring->dev, NULL);
 }
 
 static void
@@ -1078,17 +1064,9 @@ i915_gem_do_execbuffer(struct drm_device *dev, void *data,
 		ring = &dev_priv->ring[RCS];
 		break;
 	case I915_EXEC_BSD:
-		if (!HAS_BSD(dev)) {
-			DRM_DEBUG("execbuf with invalid ring (BSD)\n");
-			return -EINVAL;
-		}
 		ring = &dev_priv->ring[VCS];
 		break;
 	case I915_EXEC_BLT:
-		if (!HAS_BLT(dev)) {
-			DRM_DEBUG("execbuf with invalid ring (BLT)\n");
-			return -EINVAL;
-		}
 		ring = &dev_priv->ring[BCS];
 		break;
 	default:
@@ -1096,6 +1074,11 @@ i915_gem_do_execbuffer(struct drm_device *dev, void *data,
 			  (int)(args->flags & I915_EXEC_RING_MASK));
 		return -EINVAL;
 	}
+	if (!intel_ring_initialized(ring)) {
+		DRM_DEBUG("execbuf with invalid ring: %d\n",
+			  (int)(args->flags & I915_EXEC_RING_MASK));
+		return -EINVAL;
+	}
 
 	mode = args->flags & I915_EXEC_CONSTANTS_MASK;
 	mask = I915_EXEC_CONSTANTS_MASK;
@@ -1133,11 +1116,17 @@ i915_gem_do_execbuffer(struct drm_device *dev, void *data,
 			return -EINVAL;
 		}
 
+		if (INTEL_INFO(dev)->gen >= 5) {
+			DRM_DEBUG("clip rectangles are only valid on pre-gen5\n");
+			return -EINVAL;
+		}
+
 		if (args->num_cliprects > UINT_MAX / sizeof(*cliprects)) {
 			DRM_DEBUG("execbuf with %u cliprects\n",
 				  args->num_cliprects);
 			return -EINVAL;
 		}
+
 		cliprects = kmalloc(args->num_cliprects * sizeof(*cliprects),
 				    GFP_KERNEL);
 		if (cliprects == NULL) {
@@ -1242,9 +1231,10 @@ i915_gem_do_execbuffer(struct drm_device *dev, void *data,
 			 * so every billion or so execbuffers, we need to stall
 			 * the GPU in order to reset the counters.
 			 */
-			ret = i915_gpu_idle(dev, true);
+			ret = i915_gpu_idle(dev);
 			if (ret)
 				goto err;
+			i915_gem_retire_requests(dev);
 
 			BUG_ON(ring->sync_seqno[i]);
 		}
diff --git a/drivers/gpu/drm/i915/i915_gem_gtt.c b/drivers/gpu/drm/i915/i915_gem_gtt.c
index a135c61f411..9fd25a43553 100644
--- a/drivers/gpu/drm/i915/i915_gem_gtt.c
+++ b/drivers/gpu/drm/i915/i915_gem_gtt.c
@@ -96,11 +96,10 @@ int i915_gem_init_aliasing_ppgtt(struct drm_device *dev)
 					     GFP_KERNEL);
 		if (!ppgtt->pt_dma_addr)
 			goto err_pt_alloc;
-	}
 
-	for (i = 0; i < ppgtt->num_pd_entries; i++) {
-		dma_addr_t pt_addr;
-		if (dev_priv->mm.gtt->needs_dmar) {
+		for (i = 0; i < ppgtt->num_pd_entries; i++) {
+			dma_addr_t pt_addr;
+
 			pt_addr = pci_map_page(dev->pdev, ppgtt->pt_pages[i],
 					       0, 4096,
 					       PCI_DMA_BIDIRECTIONAL);
@@ -112,8 +111,7 @@ int i915_gem_init_aliasing_ppgtt(struct drm_device *dev)
 
 			}
 			ppgtt->pt_dma_addr[i] = pt_addr;
-		} else
-			pt_addr = page_to_phys(ppgtt->pt_pages[i]);
+		}
 	}
 
 	ppgtt->scratch_page_dma_addr = dev_priv->mm.gtt->scratch_page_dma;
@@ -269,7 +267,13 @@ void i915_ppgtt_bind_object(struct i915_hw_ppgtt *ppgtt,
 		BUG();
 	}
 
-	if (dev_priv->mm.gtt->needs_dmar) {
+	if (obj->sg_table) {
+		i915_ppgtt_insert_sg_entries(ppgtt,
+					     obj->sg_table->sgl,
+					     obj->sg_table->nents,
+					     obj->gtt_space->start >> PAGE_SHIFT,
+					     pte_flags);
+	} else if (dev_priv->mm.gtt->needs_dmar) {
 		BUG_ON(!obj->sg_list);
 
 		i915_ppgtt_insert_sg_entries(ppgtt,
@@ -319,7 +323,7 @@ static bool do_idling(struct drm_i915_private *dev_priv)
 
 	if (unlikely(dev_priv->mm.gtt->do_idle_maps)) {
 		dev_priv->mm.interruptible = false;
-		if (i915_gpu_idle(dev_priv->dev, false)) {
+		if (i915_gpu_idle(dev_priv->dev)) {
 			DRM_ERROR("Couldn't idle GPU\n");
 			/* Wait a bit, in hopes it avoids the hang */
 			udelay(10);
@@ -346,48 +350,39 @@ void i915_gem_restore_gtt_mappings(struct drm_device *dev)
 
 	list_for_each_entry(obj, &dev_priv->mm.gtt_list, gtt_list) {
 		i915_gem_clflush_object(obj);
-		i915_gem_gtt_rebind_object(obj, obj->cache_level);
+		i915_gem_gtt_bind_object(obj, obj->cache_level);
 	}
 
 	intel_gtt_chipset_flush();
 }
 
-int i915_gem_gtt_bind_object(struct drm_i915_gem_object *obj)
+int i915_gem_gtt_prepare_object(struct drm_i915_gem_object *obj)
 {
 	struct drm_device *dev = obj->base.dev;
 	struct drm_i915_private *dev_priv = dev->dev_private;
-	unsigned int agp_type = cache_level_to_agp_type(dev, obj->cache_level);
-	int ret;
-
-	if (dev_priv->mm.gtt->needs_dmar) {
-		ret = intel_gtt_map_memory(obj->pages,
-					   obj->base.size >> PAGE_SHIFT,
-					   &obj->sg_list,
-					   &obj->num_sg);
-		if (ret != 0)
-			return ret;
-
-		intel_gtt_insert_sg_entries(obj->sg_list,
-					    obj->num_sg,
-					    obj->gtt_space->start >> PAGE_SHIFT,
-					    agp_type);
-	} else
-		intel_gtt_insert_pages(obj->gtt_space->start >> PAGE_SHIFT,
-				       obj->base.size >> PAGE_SHIFT,
-				       obj->pages,
-				       agp_type);
 
-	return 0;
+	if (dev_priv->mm.gtt->needs_dmar)
+		return intel_gtt_map_memory(obj->pages,
+					    obj->base.size >> PAGE_SHIFT,
+					    &obj->sg_list,
+					    &obj->num_sg);
+	else
+		return 0;
 }
 
-void i915_gem_gtt_rebind_object(struct drm_i915_gem_object *obj,
-				enum i915_cache_level cache_level)
+void i915_gem_gtt_bind_object(struct drm_i915_gem_object *obj,
+			      enum i915_cache_level cache_level)
 {
 	struct drm_device *dev = obj->base.dev;
 	struct drm_i915_private *dev_priv = dev->dev_private;
 	unsigned int agp_type = cache_level_to_agp_type(dev, cache_level);
 
-	if (dev_priv->mm.gtt->needs_dmar) {
+	if (obj->sg_table) {
+		intel_gtt_insert_sg_entries(obj->sg_table->sgl,
+					    obj->sg_table->nents,
+					    obj->gtt_space->start >> PAGE_SHIFT,
+					    agp_type);
+	} else if (dev_priv->mm.gtt->needs_dmar) {
 		BUG_ON(!obj->sg_list);
 
 		intel_gtt_insert_sg_entries(obj->sg_list,
@@ -399,19 +394,26 @@ void i915_gem_gtt_rebind_object(struct drm_i915_gem_object *obj,
 				       obj->base.size >> PAGE_SHIFT,
 				       obj->pages,
 				       agp_type);
+
+	obj->has_global_gtt_mapping = 1;
 }
 
 void i915_gem_gtt_unbind_object(struct drm_i915_gem_object *obj)
 {
+	intel_gtt_clear_range(obj->gtt_space->start >> PAGE_SHIFT,
+			      obj->base.size >> PAGE_SHIFT);
+
+	obj->has_global_gtt_mapping = 0;
+}
+
+void i915_gem_gtt_finish_object(struct drm_i915_gem_object *obj)
+{
 	struct drm_device *dev = obj->base.dev;
 	struct drm_i915_private *dev_priv = dev->dev_private;
 	bool interruptible;
 
 	interruptible = do_idling(dev_priv);
 
-	intel_gtt_clear_range(obj->gtt_space->start >> PAGE_SHIFT,
-			      obj->base.size >> PAGE_SHIFT);
-
 	if (obj->sg_list) {
 		intel_gtt_unmap_memory(obj->sg_list, obj->num_sg);
 		obj->sg_list = NULL;
@@ -419,3 +421,23 @@ void i915_gem_gtt_unbind_object(struct drm_i915_gem_object *obj)
 
 	undo_idling(dev_priv, interruptible);
 }
+
+void i915_gem_init_global_gtt(struct drm_device *dev,
+			      unsigned long start,
+			      unsigned long mappable_end,
+			      unsigned long end)
+{
+	drm_i915_private_t *dev_priv = dev->dev_private;
+
+	/* Substract the guard page ... */
+	drm_mm_init(&dev_priv->mm.gtt_space, start, end - start - PAGE_SIZE);
+
+	dev_priv->mm.gtt_start = start;
+	dev_priv->mm.gtt_mappable_end = mappable_end;
+	dev_priv->mm.gtt_end = end;
+	dev_priv->mm.gtt_total = end - start;
+	dev_priv->mm.mappable_gtt_total = min(end, mappable_end) - start;
+
+	/* ... but ensure that we clear the entire range. */
+	intel_gtt_clear_range(start / PAGE_SIZE, (end-start) / PAGE_SIZE);
+}
diff --git a/drivers/gpu/drm/i915/i915_gem_stolen.c b/drivers/gpu/drm/i915/i915_gem_stolen.c
new file mode 100644
index 00000000000..ada2e90a2a6
--- /dev/null
+++ b/drivers/gpu/drm/i915/i915_gem_stolen.c
@@ -0,0 +1,202 @@
+/*
+ * Copyright © 2008-2012 Intel Corporation
+ *
+ * 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:
+ *    Eric Anholt <eric@anholt.net>
+ *    Chris Wilson <chris@chris-wilson.co.uk>
+ *
+ */
+
+#include "drmP.h"
+#include "drm.h"
+#include "i915_drm.h"
+#include "i915_drv.h"
+
+/*
+ * The BIOS typically reserves some of the system's memory for the exclusive
+ * use of the integrated graphics. This memory is no longer available for
+ * use by the OS and so the user finds that his system has less memory
+ * available than he put in. We refer to this memory as stolen.
+ *
+ * The BIOS will allocate its framebuffer from the stolen memory. Our
+ * goal is try to reuse that object for our own fbcon which must always
+ * be available for panics. Anything else we can reuse the stolen memory
+ * for is a boon.
+ */
+
+#define PTE_ADDRESS_MASK		0xfffff000
+#define PTE_ADDRESS_MASK_HIGH		0x000000f0 /* i915+ */
+#define PTE_MAPPING_TYPE_UNCACHED	(0 << 1)
+#define PTE_MAPPING_TYPE_DCACHE		(1 << 1) /* i830 only */
+#define PTE_MAPPING_TYPE_CACHED		(3 << 1)
+#define PTE_MAPPING_TYPE_MASK		(3 << 1)
+#define PTE_VALID			(1 << 0)
+
+/**
+ * i915_stolen_to_phys - take an offset into stolen memory and turn it into
+ *                       a physical one
+ * @dev: drm device
+ * @offset: address to translate
+ *
+ * Some chip functions require allocations from stolen space and need the
+ * physical address of the memory in question.
+ */
+static unsigned long i915_stolen_to_phys(struct drm_device *dev, u32 offset)
+{
+	struct drm_i915_private *dev_priv = dev->dev_private;
+	struct pci_dev *pdev = dev_priv->bridge_dev;
+	u32 base;
+
+#if 0
+	/* On the machines I have tested the Graphics Base of Stolen Memory
+	 * is unreliable, so compute the base by subtracting the stolen memory
+	 * from the Top of Low Usable DRAM which is where the BIOS places
+	 * the graphics stolen memory.
+	 */
+	if (INTEL_INFO(dev)->gen > 3 || IS_G33(dev)) {
+		/* top 32bits are reserved = 0 */
+		pci_read_config_dword(pdev, 0xA4, &base);
+	} else {
+		/* XXX presume 8xx is the same as i915 */
+		pci_bus_read_config_dword(pdev->bus, 2, 0x5C, &base);
+	}
+#else
+	if (INTEL_INFO(dev)->gen > 3 || IS_G33(dev)) {
+		u16 val;
+		pci_read_config_word(pdev, 0xb0, &val);
+		base = val >> 4 << 20;
+	} else {
+		u8 val;
+		pci_read_config_byte(pdev, 0x9c, &val);
+		base = val >> 3 << 27;
+	}
+	base -= dev_priv->mm.gtt->stolen_size;
+#endif
+
+	return base + offset;
+}
+
+static void i915_warn_stolen(struct drm_device *dev)
+{
+	DRM_INFO("not enough stolen space for compressed buffer, disabling\n");
+	DRM_INFO("hint: you may be able to increase stolen memory size in the BIOS to avoid this\n");
+}
+
+static void i915_setup_compression(struct drm_device *dev, int size)
+{
+	struct drm_i915_private *dev_priv = dev->dev_private;
+	struct drm_mm_node *compressed_fb, *uninitialized_var(compressed_llb);
+	unsigned long cfb_base;
+	unsigned long ll_base = 0;
+
+	/* Just in case the BIOS is doing something questionable. */
+	intel_disable_fbc(dev);
+
+	compressed_fb = drm_mm_search_free(&dev_priv->mm.stolen, size, 4096, 0);
+	if (compressed_fb)
+		compressed_fb = drm_mm_get_block(compressed_fb, size, 4096);
+	if (!compressed_fb)
+		goto err;
+
+	cfb_base = i915_stolen_to_phys(dev, compressed_fb->start);
+	if (!cfb_base)
+		goto err_fb;
+
+	if (!(IS_GM45(dev) || HAS_PCH_SPLIT(dev))) {
+		compressed_llb = drm_mm_search_free(&dev_priv->mm.stolen,
+						    4096, 4096, 0);
+		if (compressed_llb)
+			compressed_llb = drm_mm_get_block(compressed_llb,
+							  4096, 4096);
+		if (!compressed_llb)
+			goto err_fb;
+
+		ll_base = i915_stolen_to_phys(dev, compressed_llb->start);
+		if (!ll_base)
+			goto err_llb;
+	}
+
+	dev_priv->cfb_size = size;
+
+	dev_priv->compressed_fb = compressed_fb;
+	if (HAS_PCH_SPLIT(dev))
+		I915_WRITE(ILK_DPFC_CB_BASE, compressed_fb->start);
+	else if (IS_GM45(dev)) {
+		I915_WRITE(DPFC_CB_BASE, compressed_fb->start);
+	} else {
+		I915_WRITE(FBC_CFB_BASE, cfb_base);
+		I915_WRITE(FBC_LL_BASE, ll_base);
+		dev_priv->compressed_llb = compressed_llb;
+	}
+
+	DRM_DEBUG_KMS("FBC base 0x%08lx, ll base 0x%08lx, size %dM\n",
+		      cfb_base, ll_base, size >> 20);
+	return;
+
+err_llb:
+	drm_mm_put_block(compressed_llb);
+err_fb:
+	drm_mm_put_block(compressed_fb);
+err:
+	dev_priv->no_fbc_reason = FBC_STOLEN_TOO_SMALL;
+	i915_warn_stolen(dev);
+}
+
+static void i915_cleanup_compression(struct drm_device *dev)
+{
+	struct drm_i915_private *dev_priv = dev->dev_private;
+
+	drm_mm_put_block(dev_priv->compressed_fb);
+	if (dev_priv->compressed_llb)
+		drm_mm_put_block(dev_priv->compressed_llb);
+}
+
+void i915_gem_cleanup_stolen(struct drm_device *dev)
+{
+	if (I915_HAS_FBC(dev) && i915_powersave)
+		i915_cleanup_compression(dev);
+}
+
+int i915_gem_init_stolen(struct drm_device *dev)
+{
+	struct drm_i915_private *dev_priv = dev->dev_private;
+	unsigned long prealloc_size = dev_priv->mm.gtt->stolen_size;
+
+	/* Basic memrange allocator for stolen space */
+	drm_mm_init(&dev_priv->mm.stolen, 0, prealloc_size);
+
+	/* Try to set up FBC with a reasonable compressed buffer size */
+	if (I915_HAS_FBC(dev) && i915_powersave) {
+		int cfb_size;
+
+		/* Leave 1M for line length buffer & misc. */
+
+		/* Try to get a 32M buffer... */
+		if (prealloc_size > (36*1024*1024))
+			cfb_size = 32*1024*1024;
+		else /* fall back to 7/8 of the stolen space */
+			cfb_size = prealloc_size * 7 / 8;
+		i915_setup_compression(dev, cfb_size);
+	}
+
+	return 0;
+}
diff --git a/drivers/gpu/drm/i915/i915_gem_tiling.c b/drivers/gpu/drm/i915/i915_gem_tiling.c
index 1a930666598..b964df51cec 100644
--- a/drivers/gpu/drm/i915/i915_gem_tiling.c
+++ b/drivers/gpu/drm/i915/i915_gem_tiling.c
@@ -354,9 +354,15 @@ i915_gem_set_tiling(struct drm_device *dev, void *data,
 		/* We need to rebind the object if its current allocation
 		 * no longer meets the alignment restrictions for its new
 		 * tiling mode. Otherwise we can just leave it alone, but
-		 * need to ensure that any fence register is cleared.
+		 * need to ensure that any fence register is updated before
+		 * the next fenced (either through the GTT or by the BLT unit
+		 * on older GPUs) access.
+		 *
+		 * After updating the tiling parameters, we then flag whether
+		 * we need to update an associated fence register. Note this
+		 * has to also include the unfenced register the GPU uses
+		 * whilst executing a fenced command for an untiled object.
 		 */
-		i915_gem_release_mmap(obj);
 
 		obj->map_and_fenceable =
 			obj->gtt_space == NULL ||
@@ -374,9 +380,15 @@ i915_gem_set_tiling(struct drm_device *dev, void *data,
 		}
 
 		if (ret == 0) {
-			obj->tiling_changed = true;
+			obj->fence_dirty =
+				obj->fenced_gpu_access ||
+				obj->fence_reg != I915_FENCE_REG_NONE;
+
 			obj->tiling_mode = args->tiling_mode;
 			obj->stride = args->stride;
+
+			/* Force the fence to be reacquired for GTT access */
+			i915_gem_release_mmap(obj);
 		}
 	}
 	/* we have to maintain this existing ABI... */
diff --git a/drivers/gpu/drm/i915/i915_ioc32.c b/drivers/gpu/drm/i915/i915_ioc32.c
index 13b028994b2..0e72abb9f70 100644
--- a/drivers/gpu/drm/i915/i915_ioc32.c
+++ b/drivers/gpu/drm/i915/i915_ioc32.c
@@ -34,6 +34,7 @@
 #include "drmP.h"
 #include "drm.h"
 #include "i915_drm.h"
+#include "i915_drv.h"
 
 typedef struct _drm_i915_batchbuffer32 {
 	int start;		/* agp offset */
@@ -181,7 +182,7 @@ static int compat_i915_alloc(struct file *file, unsigned int cmd,
 			 (unsigned long)request);
 }
 
-drm_ioctl_compat_t *i915_compat_ioctls[] = {
+static drm_ioctl_compat_t *i915_compat_ioctls[] = {
 	[DRM_I915_BATCHBUFFER] = compat_i915_batchbuffer,
 	[DRM_I915_CMDBUFFER] = compat_i915_cmdbuffer,
 	[DRM_I915_GETPARAM] = compat_i915_getparam,
@@ -189,6 +190,7 @@ drm_ioctl_compat_t *i915_compat_ioctls[] = {
 	[DRM_I915_ALLOC] = compat_i915_alloc
 };
 
+#ifdef CONFIG_COMPAT
 /**
  * Called whenever a 32-bit process running under a 64-bit kernel
  * performs an ioctl on /dev/dri/card<n>.
@@ -217,3 +219,4 @@ long i915_compat_ioctl(struct file *filp, unsigned int cmd, unsigned long arg)
 
 	return ret;
 }
+#endif
diff --git a/drivers/gpu/drm/i915/i915_irq.c b/drivers/gpu/drm/i915/i915_irq.c
index afd4e03e337..cc4a6330761 100644
--- a/drivers/gpu/drm/i915/i915_irq.c
+++ b/drivers/gpu/drm/i915/i915_irq.c
@@ -26,6 +26,8 @@
  *
  */
 
+#define pr_fmt(fmt) KBUILD_MODNAME ": " fmt
+
 #include <linux/sysrq.h>
 #include <linux/slab.h>
 #include "drmP.h"
@@ -35,35 +37,6 @@
 #include "i915_trace.h"
 #include "intel_drv.h"
 
-#define MAX_NOPID ((u32)~0)
-
-/**
- * Interrupts that are always left unmasked.
- *
- * Since pipe events are edge-triggered from the PIPESTAT register to IIR,
- * we leave them always unmasked in IMR and then control enabling them through
- * PIPESTAT alone.
- */
-#define I915_INTERRUPT_ENABLE_FIX			\
-	(I915_ASLE_INTERRUPT |				\
-	 I915_DISPLAY_PIPE_A_EVENT_INTERRUPT |		\
-	 I915_DISPLAY_PIPE_B_EVENT_INTERRUPT |		\
-	 I915_DISPLAY_PLANE_A_FLIP_PENDING_INTERRUPT |	\
-	 I915_DISPLAY_PLANE_B_FLIP_PENDING_INTERRUPT |	\
-	 I915_RENDER_COMMAND_PARSER_ERROR_INTERRUPT)
-
-/** Interrupts that we mask and unmask at runtime. */
-#define I915_INTERRUPT_ENABLE_VAR (I915_USER_INTERRUPT | I915_BSD_USER_INTERRUPT)
-
-#define I915_PIPE_VBLANK_STATUS	(PIPE_START_VBLANK_INTERRUPT_STATUS |\
-				 PIPE_VBLANK_INTERRUPT_STATUS)
-
-#define I915_PIPE_VBLANK_ENABLE	(PIPE_START_VBLANK_INTERRUPT_ENABLE |\
-				 PIPE_VBLANK_INTERRUPT_ENABLE)
-
-#define DRM_I915_VBLANK_PIPE_ALL	(DRM_I915_VBLANK_PIPE_A | \
-					 DRM_I915_VBLANK_PIPE_B)
-
 /* For display hotplug interrupt */
 static void
 ironlake_enable_display_irq(drm_i915_private_t *dev_priv, u32 mask)
@@ -118,6 +91,10 @@ void intel_enable_asle(struct drm_device *dev)
 	drm_i915_private_t *dev_priv = dev->dev_private;
 	unsigned long irqflags;
 
+	/* FIXME: opregion/asle for VLV */
+	if (IS_VALLEYVIEW(dev))
+		return;
+
 	spin_lock_irqsave(&dev_priv->irq_lock, irqflags);
 
 	if (HAS_PCH_SPLIT(dev))
@@ -354,15 +331,12 @@ static void notify_ring(struct drm_device *dev,
 			struct intel_ring_buffer *ring)
 {
 	struct drm_i915_private *dev_priv = dev->dev_private;
-	u32 seqno;
 
 	if (ring->obj == NULL)
 		return;
 
-	seqno = ring->get_seqno(ring);
-	trace_i915_gem_request_complete(ring, seqno);
+	trace_i915_gem_request_complete(ring, ring->get_seqno(ring));
 
-	ring->irq_seqno = seqno;
 	wake_up_all(&ring->irq_queue);
 	if (i915_enable_hangcheck) {
 		dev_priv->hangcheck_count = 0;
@@ -424,13 +398,145 @@ static void gen6_pm_rps_work(struct work_struct *work)
 	mutex_unlock(&dev_priv->dev->struct_mutex);
 }
 
-static void pch_irq_handler(struct drm_device *dev)
+static void snb_gt_irq_handler(struct drm_device *dev,
+			       struct drm_i915_private *dev_priv,
+			       u32 gt_iir)
+{
+
+	if (gt_iir & (GEN6_RENDER_USER_INTERRUPT |
+		      GEN6_RENDER_PIPE_CONTROL_NOTIFY_INTERRUPT))
+		notify_ring(dev, &dev_priv->ring[RCS]);
+	if (gt_iir & GEN6_BSD_USER_INTERRUPT)
+		notify_ring(dev, &dev_priv->ring[VCS]);
+	if (gt_iir & GEN6_BLITTER_USER_INTERRUPT)
+		notify_ring(dev, &dev_priv->ring[BCS]);
+
+	if (gt_iir & (GT_GEN6_BLT_CS_ERROR_INTERRUPT |
+		      GT_GEN6_BSD_CS_ERROR_INTERRUPT |
+		      GT_RENDER_CS_ERROR_INTERRUPT)) {
+		DRM_ERROR("GT error interrupt 0x%08x\n", gt_iir);
+		i915_handle_error(dev, false);
+	}
+}
+
+static void gen6_queue_rps_work(struct drm_i915_private *dev_priv,
+				u32 pm_iir)
+{
+	unsigned long flags;
+
+	/*
+	 * IIR bits should never already be set because IMR should
+	 * prevent an interrupt from being shown in IIR. The warning
+	 * displays a case where we've unsafely cleared
+	 * dev_priv->pm_iir. Although missing an interrupt of the same
+	 * type is not a problem, it displays a problem in the logic.
+	 *
+	 * The mask bit in IMR is cleared by rps_work.
+	 */
+
+	spin_lock_irqsave(&dev_priv->rps_lock, flags);
+	WARN(dev_priv->pm_iir & pm_iir, "Missed a PM interrupt\n");
+	dev_priv->pm_iir |= pm_iir;
+	I915_WRITE(GEN6_PMIMR, dev_priv->pm_iir);
+	POSTING_READ(GEN6_PMIMR);
+	spin_unlock_irqrestore(&dev_priv->rps_lock, flags);
+
+	queue_work(dev_priv->wq, &dev_priv->rps_work);
+}
+
+static irqreturn_t valleyview_irq_handler(DRM_IRQ_ARGS)
 {
+	struct drm_device *dev = (struct drm_device *) arg;
 	drm_i915_private_t *dev_priv = (drm_i915_private_t *) dev->dev_private;
-	u32 pch_iir;
+	u32 iir, gt_iir, pm_iir;
+	irqreturn_t ret = IRQ_NONE;
+	unsigned long irqflags;
 	int pipe;
+	u32 pipe_stats[I915_MAX_PIPES];
+	u32 vblank_status;
+	int vblank = 0;
+	bool blc_event;
 
-	pch_iir = I915_READ(SDEIIR);
+	atomic_inc(&dev_priv->irq_received);
+
+	vblank_status = PIPE_START_VBLANK_INTERRUPT_STATUS |
+		PIPE_VBLANK_INTERRUPT_STATUS;
+
+	while (true) {
+		iir = I915_READ(VLV_IIR);
+		gt_iir = I915_READ(GTIIR);
+		pm_iir = I915_READ(GEN6_PMIIR);
+
+		if (gt_iir == 0 && pm_iir == 0 && iir == 0)
+			goto out;
+
+		ret = IRQ_HANDLED;
+
+		snb_gt_irq_handler(dev, dev_priv, gt_iir);
+
+		spin_lock_irqsave(&dev_priv->irq_lock, irqflags);
+		for_each_pipe(pipe) {
+			int reg = PIPESTAT(pipe);
+			pipe_stats[pipe] = I915_READ(reg);
+
+			/*
+			 * Clear the PIPE*STAT regs before the IIR
+			 */
+			if (pipe_stats[pipe] & 0x8000ffff) {
+				if (pipe_stats[pipe] & PIPE_FIFO_UNDERRUN_STATUS)
+					DRM_DEBUG_DRIVER("pipe %c underrun\n",
+							 pipe_name(pipe));
+				I915_WRITE(reg, pipe_stats[pipe]);
+			}
+		}
+		spin_unlock_irqrestore(&dev_priv->irq_lock, irqflags);
+
+		/* Consume port.  Then clear IIR or we'll miss events */
+		if (iir & I915_DISPLAY_PORT_INTERRUPT) {
+			u32 hotplug_status = I915_READ(PORT_HOTPLUG_STAT);
+
+			DRM_DEBUG_DRIVER("hotplug event received, stat 0x%08x\n",
+					 hotplug_status);
+			if (hotplug_status & dev_priv->hotplug_supported_mask)
+				queue_work(dev_priv->wq,
+					   &dev_priv->hotplug_work);
+
+			I915_WRITE(PORT_HOTPLUG_STAT, hotplug_status);
+			I915_READ(PORT_HOTPLUG_STAT);
+		}
+
+
+		if (iir & I915_DISPLAY_PIPE_A_VBLANK_INTERRUPT) {
+			drm_handle_vblank(dev, 0);
+			vblank++;
+			intel_finish_page_flip(dev, 0);
+		}
+
+		if (iir & I915_DISPLAY_PIPE_B_VBLANK_INTERRUPT) {
+			drm_handle_vblank(dev, 1);
+			vblank++;
+			intel_finish_page_flip(dev, 0);
+		}
+
+		if (pipe_stats[pipe] & PIPE_LEGACY_BLC_EVENT_STATUS)
+			blc_event = true;
+
+		if (pm_iir & GEN6_PM_DEFERRED_EVENTS)
+			gen6_queue_rps_work(dev_priv, pm_iir);
+
+		I915_WRITE(GTIIR, gt_iir);
+		I915_WRITE(GEN6_PMIIR, pm_iir);
+		I915_WRITE(VLV_IIR, iir);
+	}
+
+out:
+	return ret;
+}
+
+static void pch_irq_handler(struct drm_device *dev, u32 pch_iir)
+{
+	drm_i915_private_t *dev_priv = (drm_i915_private_t *) dev->dev_private;
+	int pipe;
 
 	if (pch_iir & SDE_AUDIO_POWER_MASK)
 		DRM_DEBUG_DRIVER("PCH audio power change on port %d\n",
@@ -471,91 +577,77 @@ static irqreturn_t ivybridge_irq_handler(DRM_IRQ_ARGS)
 {
 	struct drm_device *dev = (struct drm_device *) arg;
 	drm_i915_private_t *dev_priv = (drm_i915_private_t *) dev->dev_private;
-	int ret = IRQ_NONE;
-	u32 de_iir, gt_iir, de_ier, pch_iir, pm_iir;
-	struct drm_i915_master_private *master_priv;
+	u32 de_iir, gt_iir, de_ier, pm_iir;
+	irqreturn_t ret = IRQ_NONE;
+	int i;
 
 	atomic_inc(&dev_priv->irq_received);
 
 	/* disable master interrupt before clearing iir  */
 	de_ier = I915_READ(DEIER);
 	I915_WRITE(DEIER, de_ier & ~DE_MASTER_IRQ_CONTROL);
-	POSTING_READ(DEIER);
 
-	de_iir = I915_READ(DEIIR);
 	gt_iir = I915_READ(GTIIR);
-	pch_iir = I915_READ(SDEIIR);
-	pm_iir = I915_READ(GEN6_PMIIR);
-
-	if (de_iir == 0 && gt_iir == 0 && pch_iir == 0 && pm_iir == 0)
-		goto done;
-
-	ret = IRQ_HANDLED;
-
-	if (dev->primary->master) {
-		master_priv = dev->primary->master->driver_priv;
-		if (master_priv->sarea_priv)
-			master_priv->sarea_priv->last_dispatch =
-				READ_BREADCRUMB(dev_priv);
+	if (gt_iir) {
+		snb_gt_irq_handler(dev, dev_priv, gt_iir);
+		I915_WRITE(GTIIR, gt_iir);
+		ret = IRQ_HANDLED;
 	}
 
-	if (gt_iir & (GT_USER_INTERRUPT | GT_PIPE_NOTIFY))
-		notify_ring(dev, &dev_priv->ring[RCS]);
-	if (gt_iir & GT_GEN6_BSD_USER_INTERRUPT)
-		notify_ring(dev, &dev_priv->ring[VCS]);
-	if (gt_iir & GT_BLT_USER_INTERRUPT)
-		notify_ring(dev, &dev_priv->ring[BCS]);
-
-	if (de_iir & DE_GSE_IVB)
-		intel_opregion_gse_intr(dev);
-
-	if (de_iir & DE_PLANEA_FLIP_DONE_IVB) {
-		intel_prepare_page_flip(dev, 0);
-		intel_finish_page_flip_plane(dev, 0);
-	}
+	de_iir = I915_READ(DEIIR);
+	if (de_iir) {
+		if (de_iir & DE_GSE_IVB)
+			intel_opregion_gse_intr(dev);
+
+		for (i = 0; i < 3; i++) {
+			if (de_iir & (DE_PLANEA_FLIP_DONE_IVB << (5 * i))) {
+				intel_prepare_page_flip(dev, i);
+				intel_finish_page_flip_plane(dev, i);
+			}
+			if (de_iir & (DE_PIPEA_VBLANK_IVB << (5 * i)))
+				drm_handle_vblank(dev, i);
+		}
 
-	if (de_iir & DE_PLANEB_FLIP_DONE_IVB) {
-		intel_prepare_page_flip(dev, 1);
-		intel_finish_page_flip_plane(dev, 1);
-	}
+		/* check event from PCH */
+		if (de_iir & DE_PCH_EVENT_IVB) {
+			u32 pch_iir = I915_READ(SDEIIR);
 
-	if (de_iir & DE_PIPEA_VBLANK_IVB)
-		drm_handle_vblank(dev, 0);
+			if (pch_iir & SDE_HOTPLUG_MASK_CPT)
+				queue_work(dev_priv->wq, &dev_priv->hotplug_work);
+			pch_irq_handler(dev, pch_iir);
 
-	if (de_iir & DE_PIPEB_VBLANK_IVB)
-		drm_handle_vblank(dev, 1);
+			/* clear PCH hotplug event before clear CPU irq */
+			I915_WRITE(SDEIIR, pch_iir);
+		}
 
-	/* check event from PCH */
-	if (de_iir & DE_PCH_EVENT_IVB) {
-		if (pch_iir & SDE_HOTPLUG_MASK_CPT)
-			queue_work(dev_priv->wq, &dev_priv->hotplug_work);
-		pch_irq_handler(dev);
+		I915_WRITE(DEIIR, de_iir);
+		ret = IRQ_HANDLED;
 	}
 
-	if (pm_iir & GEN6_PM_DEFERRED_EVENTS) {
-		unsigned long flags;
-		spin_lock_irqsave(&dev_priv->rps_lock, flags);
-		WARN(dev_priv->pm_iir & pm_iir, "Missed a PM interrupt\n");
-		dev_priv->pm_iir |= pm_iir;
-		I915_WRITE(GEN6_PMIMR, dev_priv->pm_iir);
-		POSTING_READ(GEN6_PMIMR);
-		spin_unlock_irqrestore(&dev_priv->rps_lock, flags);
-		queue_work(dev_priv->wq, &dev_priv->rps_work);
+	pm_iir = I915_READ(GEN6_PMIIR);
+	if (pm_iir) {
+		if (pm_iir & GEN6_PM_DEFERRED_EVENTS)
+			gen6_queue_rps_work(dev_priv, pm_iir);
+		I915_WRITE(GEN6_PMIIR, pm_iir);
+		ret = IRQ_HANDLED;
 	}
 
-	/* should clear PCH hotplug event before clear CPU irq */
-	I915_WRITE(SDEIIR, pch_iir);
-	I915_WRITE(GTIIR, gt_iir);
-	I915_WRITE(DEIIR, de_iir);
-	I915_WRITE(GEN6_PMIIR, pm_iir);
-
-done:
 	I915_WRITE(DEIER, de_ier);
 	POSTING_READ(DEIER);
 
 	return ret;
 }
 
+static void ilk_gt_irq_handler(struct drm_device *dev,
+			       struct drm_i915_private *dev_priv,
+			       u32 gt_iir)
+{
+	if (gt_iir & (GT_USER_INTERRUPT | GT_PIPE_NOTIFY))
+		notify_ring(dev, &dev_priv->ring[RCS]);
+	if (gt_iir & GT_BSD_USER_INTERRUPT)
+		notify_ring(dev, &dev_priv->ring[VCS]);
+}
+
 static irqreturn_t ironlake_irq_handler(DRM_IRQ_ARGS)
 {
 	struct drm_device *dev = (struct drm_device *) arg;
@@ -563,14 +655,9 @@ static irqreturn_t ironlake_irq_handler(DRM_IRQ_ARGS)
 	int ret = IRQ_NONE;
 	u32 de_iir, gt_iir, de_ier, pch_iir, pm_iir;
 	u32 hotplug_mask;
-	struct drm_i915_master_private *master_priv;
-	u32 bsd_usr_interrupt = GT_BSD_USER_INTERRUPT;
 
 	atomic_inc(&dev_priv->irq_received);
 
-	if (IS_GEN6(dev))
-		bsd_usr_interrupt = GT_GEN6_BSD_USER_INTERRUPT;
-
 	/* disable master interrupt before clearing iir  */
 	de_ier = I915_READ(DEIER);
 	I915_WRITE(DEIER, de_ier & ~DE_MASTER_IRQ_CONTROL);
@@ -592,19 +679,10 @@ static irqreturn_t ironlake_irq_handler(DRM_IRQ_ARGS)
 
 	ret = IRQ_HANDLED;
 
-	if (dev->primary->master) {
-		master_priv = dev->primary->master->driver_priv;
-		if (master_priv->sarea_priv)
-			master_priv->sarea_priv->last_dispatch =
-				READ_BREADCRUMB(dev_priv);
-	}
-
-	if (gt_iir & (GT_USER_INTERRUPT | GT_PIPE_NOTIFY))
-		notify_ring(dev, &dev_priv->ring[RCS]);
-	if (gt_iir & bsd_usr_interrupt)
-		notify_ring(dev, &dev_priv->ring[VCS]);
-	if (gt_iir & GT_BLT_USER_INTERRUPT)
-		notify_ring(dev, &dev_priv->ring[BCS]);
+	if (IS_GEN5(dev))
+		ilk_gt_irq_handler(dev, dev_priv, gt_iir);
+	else
+		snb_gt_irq_handler(dev, dev_priv, gt_iir);
 
 	if (de_iir & DE_GSE)
 		intel_opregion_gse_intr(dev);
@@ -629,7 +707,7 @@ static irqreturn_t ironlake_irq_handler(DRM_IRQ_ARGS)
 	if (de_iir & DE_PCH_EVENT) {
 		if (pch_iir & hotplug_mask)
 			queue_work(dev_priv->wq, &dev_priv->hotplug_work);
-		pch_irq_handler(dev);
+		pch_irq_handler(dev, pch_iir);
 	}
 
 	if (de_iir & DE_PCU_EVENT) {
@@ -637,25 +715,8 @@ static irqreturn_t ironlake_irq_handler(DRM_IRQ_ARGS)
 		i915_handle_rps_change(dev);
 	}
 
-	if (IS_GEN6(dev) && pm_iir & GEN6_PM_DEFERRED_EVENTS) {
-		/*
-		 * IIR bits should never already be set because IMR should
-		 * prevent an interrupt from being shown in IIR. The warning
-		 * displays a case where we've unsafely cleared
-		 * dev_priv->pm_iir. Although missing an interrupt of the same
-		 * type is not a problem, it displays a problem in the logic.
-		 *
-		 * The mask bit in IMR is cleared by rps_work.
-		 */
-		unsigned long flags;
-		spin_lock_irqsave(&dev_priv->rps_lock, flags);
-		WARN(dev_priv->pm_iir & pm_iir, "Missed a PM interrupt\n");
-		dev_priv->pm_iir |= pm_iir;
-		I915_WRITE(GEN6_PMIMR, dev_priv->pm_iir);
-		POSTING_READ(GEN6_PMIMR);
-		spin_unlock_irqrestore(&dev_priv->rps_lock, flags);
-		queue_work(dev_priv->wq, &dev_priv->rps_work);
-	}
+	if (IS_GEN6(dev) && pm_iir & GEN6_PM_DEFERRED_EVENTS)
+		gen6_queue_rps_work(dev_priv, pm_iir);
 
 	/* should clear PCH hotplug event before clear CPU irq */
 	I915_WRITE(SDEIIR, pch_iir);
@@ -691,7 +752,7 @@ static void i915_error_work_func(struct work_struct *work)
 	if (atomic_read(&dev_priv->mm.wedged)) {
 		DRM_DEBUG_DRIVER("resetting chip\n");
 		kobject_uevent_env(&dev->primary->kdev.kobj, KOBJ_CHANGE, reset_event);
-		if (!i915_reset(dev, GRDOM_RENDER)) {
+		if (!i915_reset(dev)) {
 			atomic_set(&dev_priv->mm.wedged, 0);
 			kobject_uevent_env(&dev->primary->kdev.kobj, KOBJ_CHANGE, reset_done_event);
 		}
@@ -727,7 +788,8 @@ i915_error_object_create(struct drm_i915_private *dev_priv,
 			goto unwind;
 
 		local_irq_save(flags);
-		if (reloc_offset < dev_priv->mm.gtt_mappable_end) {
+		if (reloc_offset < dev_priv->mm.gtt_mappable_end &&
+		    src->has_global_gtt_mapping) {
 			void __iomem *s;
 
 			/* Simply ignore tiling or any overlapping fence.
@@ -782,10 +844,11 @@ i915_error_object_free(struct drm_i915_error_object *obj)
 	kfree(obj);
 }
 
-static void
-i915_error_state_free(struct drm_device *dev,
-		      struct drm_i915_error_state *error)
+void
+i915_error_state_free(struct kref *error_ref)
 {
+	struct drm_i915_error_state *error = container_of(error_ref,
+							  typeof(*error), ref);
 	int i;
 
 	for (i = 0; i < ARRAY_SIZE(error->ring); i++) {
@@ -798,37 +861,56 @@ i915_error_state_free(struct drm_device *dev,
 	kfree(error->overlay);
 	kfree(error);
 }
+static void capture_bo(struct drm_i915_error_buffer *err,
+		       struct drm_i915_gem_object *obj)
+{
+	err->size = obj->base.size;
+	err->name = obj->base.name;
+	err->seqno = obj->last_rendering_seqno;
+	err->gtt_offset = obj->gtt_offset;
+	err->read_domains = obj->base.read_domains;
+	err->write_domain = obj->base.write_domain;
+	err->fence_reg = obj->fence_reg;
+	err->pinned = 0;
+	if (obj->pin_count > 0)
+		err->pinned = 1;
+	if (obj->user_pin_count > 0)
+		err->pinned = -1;
+	err->tiling = obj->tiling_mode;
+	err->dirty = obj->dirty;
+	err->purgeable = obj->madv != I915_MADV_WILLNEED;
+	err->ring = obj->ring ? obj->ring->id : -1;
+	err->cache_level = obj->cache_level;
+}
 
-static u32 capture_bo_list(struct drm_i915_error_buffer *err,
-			   int count,
-			   struct list_head *head)
+static u32 capture_active_bo(struct drm_i915_error_buffer *err,
+			     int count, struct list_head *head)
 {
 	struct drm_i915_gem_object *obj;
 	int i = 0;
 
 	list_for_each_entry(obj, head, mm_list) {
-		err->size = obj->base.size;
-		err->name = obj->base.name;
-		err->seqno = obj->last_rendering_seqno;
-		err->gtt_offset = obj->gtt_offset;
-		err->read_domains = obj->base.read_domains;
-		err->write_domain = obj->base.write_domain;
-		err->fence_reg = obj->fence_reg;
-		err->pinned = 0;
-		if (obj->pin_count > 0)
-			err->pinned = 1;
-		if (obj->user_pin_count > 0)
-			err->pinned = -1;
-		err->tiling = obj->tiling_mode;
-		err->dirty = obj->dirty;
-		err->purgeable = obj->madv != I915_MADV_WILLNEED;
-		err->ring = obj->ring ? obj->ring->id : -1;
-		err->cache_level = obj->cache_level;
-
+		capture_bo(err++, obj);
 		if (++i == count)
 			break;
+	}
 
-		err++;
+	return i;
+}
+
+static u32 capture_pinned_bo(struct drm_i915_error_buffer *err,
+			     int count, struct list_head *head)
+{
+	struct drm_i915_gem_object *obj;
+	int i = 0;
+
+	list_for_each_entry(obj, head, gtt_list) {
+		if (obj->pin_count == 0)
+			continue;
+
+		capture_bo(err++, obj);
+		if (++i == count)
+			break;
 	}
 
 	return i;
@@ -901,7 +983,6 @@ static void i915_record_ring_state(struct drm_device *dev,
 	struct drm_i915_private *dev_priv = dev->dev_private;
 
 	if (INTEL_INFO(dev)->gen >= 6) {
-		error->faddr[ring->id] = I915_READ(RING_DMA_FADD(ring->mmio_base));
 		error->fault_reg[ring->id] = I915_READ(RING_FAULT_REG(ring));
 		error->semaphore_mboxes[ring->id][0]
 			= I915_READ(RING_SYNC_0(ring->mmio_base));
@@ -910,6 +991,7 @@ static void i915_record_ring_state(struct drm_device *dev,
 	}
 
 	if (INTEL_INFO(dev)->gen >= 4) {
+		error->faddr[ring->id] = I915_READ(RING_DMA_FADD(ring->mmio_base));
 		error->ipeir[ring->id] = I915_READ(RING_IPEIR(ring->mmio_base));
 		error->ipehr[ring->id] = I915_READ(RING_IPEHR(ring->mmio_base));
 		error->instdone[ring->id] = I915_READ(RING_INSTDONE(ring->mmio_base));
@@ -919,11 +1001,13 @@ static void i915_record_ring_state(struct drm_device *dev,
 			error->bbaddr = I915_READ64(BB_ADDR);
 		}
 	} else {
+		error->faddr[ring->id] = I915_READ(DMA_FADD_I8XX);
 		error->ipeir[ring->id] = I915_READ(IPEIR);
 		error->ipehr[ring->id] = I915_READ(IPEHR);
 		error->instdone[ring->id] = I915_READ(INSTDONE);
 	}
 
+	error->waiting[ring->id] = waitqueue_active(&ring->irq_queue);
 	error->instpm[ring->id] = I915_READ(RING_INSTPM(ring->mmio_base));
 	error->seqno[ring->id] = ring->get_seqno(ring);
 	error->acthd[ring->id] = intel_ring_get_active_head(ring);
@@ -938,15 +1022,11 @@ static void i915_gem_record_rings(struct drm_device *dev,
 				  struct drm_i915_error_state *error)
 {
 	struct drm_i915_private *dev_priv = dev->dev_private;
+	struct intel_ring_buffer *ring;
 	struct drm_i915_gem_request *request;
 	int i, count;
 
-	for (i = 0; i < I915_NUM_RINGS; i++) {
-		struct intel_ring_buffer *ring = &dev_priv->ring[i];
-
-		if (ring->obj == NULL)
-			continue;
-
+	for_each_ring(ring, dev_priv, i) {
 		i915_record_ring_state(dev, error, ring);
 
 		error->ring[i].batchbuffer =
@@ -1013,8 +1093,19 @@ static void i915_capture_error_state(struct drm_device *dev)
 	DRM_INFO("capturing error event; look for more information in /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);
+
+	if (HAS_PCH_SPLIT(dev))
+		error->ier = I915_READ(DEIER) | I915_READ(GTIER);
+	else if (IS_VALLEYVIEW(dev))
+		error->ier = I915_READ(GTIER) | I915_READ(VLV_IER);
+	else if (IS_GEN2(dev))
+		error->ier = I915_READ16(IER);
+	else
+		error->ier = I915_READ(IER);
+
 	for_each_pipe(pipe)
 		error->pipestat[pipe] = I915_READ(PIPESTAT(pipe));
 
@@ -1034,8 +1125,9 @@ static void i915_capture_error_state(struct drm_device *dev)
 	list_for_each_entry(obj, &dev_priv->mm.active_list, mm_list)
 		i++;
 	error->active_bo_count = i;
-	list_for_each_entry(obj, &dev_priv->mm.pinned_list, mm_list)
-		i++;
+	list_for_each_entry(obj, &dev_priv->mm.gtt_list, gtt_list)
+		if (obj->pin_count)
+			i++;
 	error->pinned_bo_count = i - error->active_bo_count;
 
 	error->active_bo = NULL;
@@ -1050,15 +1142,15 @@ static void i915_capture_error_state(struct drm_device *dev)
 
 	if (error->active_bo)
 		error->active_bo_count =
-			capture_bo_list(error->active_bo,
-					error->active_bo_count,
-					&dev_priv->mm.active_list);
+			capture_active_bo(error->active_bo,
+					  error->active_bo_count,
+					  &dev_priv->mm.active_list);
 
 	if (error->pinned_bo)
 		error->pinned_bo_count =
-			capture_bo_list(error->pinned_bo,
-					error->pinned_bo_count,
-					&dev_priv->mm.pinned_list);
+			capture_pinned_bo(error->pinned_bo,
+					  error->pinned_bo_count,
+					  &dev_priv->mm.gtt_list);
 
 	do_gettimeofday(&error->time);
 
@@ -1073,7 +1165,7 @@ static void i915_capture_error_state(struct drm_device *dev)
 	spin_unlock_irqrestore(&dev_priv->error_lock, flags);
 
 	if (error)
-		i915_error_state_free(dev, error);
+		i915_error_state_free(&error->ref);
 }
 
 void i915_destroy_error_state(struct drm_device *dev)
@@ -1088,7 +1180,7 @@ void i915_destroy_error_state(struct drm_device *dev)
 	spin_unlock_irqrestore(&dev_priv->error_lock, flags);
 
 	if (error)
-		i915_error_state_free(dev, error);
+		kref_put(&error->ref, i915_error_state_free);
 }
 #else
 #define i915_capture_error_state(x)
@@ -1103,33 +1195,26 @@ static void i915_report_and_clear_eir(struct drm_device *dev)
 	if (!eir)
 		return;
 
-	printk(KERN_ERR "render error detected, EIR: 0x%08x\n",
-	       eir);
+	pr_err("render error detected, EIR: 0x%08x\n", eir);
 
 	if (IS_G4X(dev)) {
 		if (eir & (GM45_ERROR_MEM_PRIV | GM45_ERROR_CP_PRIV)) {
 			u32 ipeir = I915_READ(IPEIR_I965);
 
-			printk(KERN_ERR "  IPEIR: 0x%08x\n",
-			       I915_READ(IPEIR_I965));
-			printk(KERN_ERR "  IPEHR: 0x%08x\n",
-			       I915_READ(IPEHR_I965));
-			printk(KERN_ERR "  INSTDONE: 0x%08x\n",
+			pr_err("  IPEIR: 0x%08x\n", I915_READ(IPEIR_I965));
+			pr_err("  IPEHR: 0x%08x\n", I915_READ(IPEHR_I965));
+			pr_err("  INSTDONE: 0x%08x\n",
 			       I915_READ(INSTDONE_I965));
-			printk(KERN_ERR "  INSTPS: 0x%08x\n",
-			       I915_READ(INSTPS));
-			printk(KERN_ERR "  INSTDONE1: 0x%08x\n",
-			       I915_READ(INSTDONE1));
-			printk(KERN_ERR "  ACTHD: 0x%08x\n",
-			       I915_READ(ACTHD_I965));
+			pr_err("  INSTPS: 0x%08x\n", I915_READ(INSTPS));
+			pr_err("  INSTDONE1: 0x%08x\n", I915_READ(INSTDONE1));
+			pr_err("  ACTHD: 0x%08x\n", I915_READ(ACTHD_I965));
 			I915_WRITE(IPEIR_I965, ipeir);
 			POSTING_READ(IPEIR_I965);
 		}
 		if (eir & GM45_ERROR_PAGE_TABLE) {
 			u32 pgtbl_err = I915_READ(PGTBL_ER);
-			printk(KERN_ERR "page table error\n");
-			printk(KERN_ERR "  PGTBL_ER: 0x%08x\n",
-			       pgtbl_err);
+			pr_err("page table error\n");
+			pr_err("  PGTBL_ER: 0x%08x\n", pgtbl_err);
 			I915_WRITE(PGTBL_ER, pgtbl_err);
 			POSTING_READ(PGTBL_ER);
 		}
@@ -1138,53 +1223,42 @@ static void i915_report_and_clear_eir(struct drm_device *dev)
 	if (!IS_GEN2(dev)) {
 		if (eir & I915_ERROR_PAGE_TABLE) {
 			u32 pgtbl_err = I915_READ(PGTBL_ER);
-			printk(KERN_ERR "page table error\n");
-			printk(KERN_ERR "  PGTBL_ER: 0x%08x\n",
-			       pgtbl_err);
+			pr_err("page table error\n");
+			pr_err("  PGTBL_ER: 0x%08x\n", pgtbl_err);
 			I915_WRITE(PGTBL_ER, pgtbl_err);
 			POSTING_READ(PGTBL_ER);
 		}
 	}
 
 	if (eir & I915_ERROR_MEMORY_REFRESH) {
-		printk(KERN_ERR "memory refresh error:\n");
+		pr_err("memory refresh error:\n");
 		for_each_pipe(pipe)
-			printk(KERN_ERR "pipe %c stat: 0x%08x\n",
+			pr_err("pipe %c stat: 0x%08x\n",
 			       pipe_name(pipe), I915_READ(PIPESTAT(pipe)));
 		/* pipestat has already been acked */
 	}
 	if (eir & I915_ERROR_INSTRUCTION) {
-		printk(KERN_ERR "instruction error\n");
-		printk(KERN_ERR "  INSTPM: 0x%08x\n",
-		       I915_READ(INSTPM));
+		pr_err("instruction error\n");
+		pr_err("  INSTPM: 0x%08x\n", I915_READ(INSTPM));
 		if (INTEL_INFO(dev)->gen < 4) {
 			u32 ipeir = I915_READ(IPEIR);
 
-			printk(KERN_ERR "  IPEIR: 0x%08x\n",
-			       I915_READ(IPEIR));
-			printk(KERN_ERR "  IPEHR: 0x%08x\n",
-			       I915_READ(IPEHR));
-			printk(KERN_ERR "  INSTDONE: 0x%08x\n",
-			       I915_READ(INSTDONE));
-			printk(KERN_ERR "  ACTHD: 0x%08x\n",
-			       I915_READ(ACTHD));
+			pr_err("  IPEIR: 0x%08x\n", I915_READ(IPEIR));
+			pr_err("  IPEHR: 0x%08x\n", I915_READ(IPEHR));
+			pr_err("  INSTDONE: 0x%08x\n", I915_READ(INSTDONE));
+			pr_err("  ACTHD: 0x%08x\n", I915_READ(ACTHD));
 			I915_WRITE(IPEIR, ipeir);
 			POSTING_READ(IPEIR);
 		} else {
 			u32 ipeir = I915_READ(IPEIR_I965);
 
-			printk(KERN_ERR "  IPEIR: 0x%08x\n",
-			       I915_READ(IPEIR_I965));
-			printk(KERN_ERR "  IPEHR: 0x%08x\n",
-			       I915_READ(IPEHR_I965));
-			printk(KERN_ERR "  INSTDONE: 0x%08x\n",
+			pr_err("  IPEIR: 0x%08x\n", I915_READ(IPEIR_I965));
+			pr_err("  IPEHR: 0x%08x\n", I915_READ(IPEHR_I965));
+			pr_err("  INSTDONE: 0x%08x\n",
 			       I915_READ(INSTDONE_I965));
-			printk(KERN_ERR "  INSTPS: 0x%08x\n",
-			       I915_READ(INSTPS));
-			printk(KERN_ERR "  INSTDONE1: 0x%08x\n",
-			       I915_READ(INSTDONE1));
-			printk(KERN_ERR "  ACTHD: 0x%08x\n",
-			       I915_READ(ACTHD_I965));
+			pr_err("  INSTPS: 0x%08x\n", I915_READ(INSTPS));
+			pr_err("  INSTDONE1: 0x%08x\n", I915_READ(INSTDONE1));
+			pr_err("  ACTHD: 0x%08x\n", I915_READ(ACTHD_I965));
 			I915_WRITE(IPEIR_I965, ipeir);
 			POSTING_READ(IPEIR_I965);
 		}
@@ -1217,6 +1291,8 @@ static void i915_report_and_clear_eir(struct drm_device *dev)
 void i915_handle_error(struct drm_device *dev, bool wedged)
 {
 	struct drm_i915_private *dev_priv = dev->dev_private;
+	struct intel_ring_buffer *ring;
+	int i;
 
 	i915_capture_error_state(dev);
 	i915_report_and_clear_eir(dev);
@@ -1228,11 +1304,8 @@ void i915_handle_error(struct drm_device *dev, bool wedged)
 		/*
 		 * Wakeup waiting processes so they don't hang
 		 */
-		wake_up_all(&dev_priv->ring[RCS].irq_queue);
-		if (HAS_BSD(dev))
-			wake_up_all(&dev_priv->ring[VCS].irq_queue);
-		if (HAS_BLT(dev))
-			wake_up_all(&dev_priv->ring[BCS].irq_queue);
+		for_each_ring(ring, dev_priv, i)
+			wake_up_all(&ring->irq_queue);
 	}
 
 	queue_work(dev_priv->wq, &dev_priv->error_work);
@@ -1265,7 +1338,8 @@ static void i915_pageflip_stall_check(struct drm_device *dev, int pipe)
 	obj = work->pending_flip_obj;
 	if (INTEL_INFO(dev)->gen >= 4) {
 		int dspsurf = DSPSURF(intel_crtc->plane);
-		stall_detected = I915_READ(dspsurf) == obj->gtt_offset;
+		stall_detected = I915_HI_DISPBASE(I915_READ(dspsurf)) ==
+					obj->gtt_offset;
 	} else {
 		int dspaddr = DSPADDR(intel_crtc->plane);
 		stall_detected = I915_READ(dspaddr) == (obj->gtt_offset +
@@ -1281,248 +1355,6 @@ static void i915_pageflip_stall_check(struct drm_device *dev, int pipe)
 	}
 }
 
-static irqreturn_t i915_driver_irq_handler(DRM_IRQ_ARGS)
-{
-	struct drm_device *dev = (struct drm_device *) arg;
-	drm_i915_private_t *dev_priv = (drm_i915_private_t *) dev->dev_private;
-	struct drm_i915_master_private *master_priv;
-	u32 iir, new_iir;
-	u32 pipe_stats[I915_MAX_PIPES];
-	u32 vblank_status;
-	int vblank = 0;
-	unsigned long irqflags;
-	int irq_received;
-	int ret = IRQ_NONE, pipe;
-	bool blc_event = false;
-
-	atomic_inc(&dev_priv->irq_received);
-
-	iir = I915_READ(IIR);
-
-	if (INTEL_INFO(dev)->gen >= 4)
-		vblank_status = PIPE_START_VBLANK_INTERRUPT_STATUS;
-	else
-		vblank_status = PIPE_VBLANK_INTERRUPT_STATUS;
-
-	for (;;) {
-		irq_received = iir != 0;
-
-		/* Can't rely on pipestat interrupt bit in iir as it might
-		 * have been cleared after the pipestat interrupt was received.
-		 * It doesn't set the bit in iir again, but it still produces
-		 * interrupts (for non-MSI).
-		 */
-		spin_lock_irqsave(&dev_priv->irq_lock, irqflags);
-		if (iir & I915_RENDER_COMMAND_PARSER_ERROR_INTERRUPT)
-			i915_handle_error(dev, false);
-
-		for_each_pipe(pipe) {
-			int reg = PIPESTAT(pipe);
-			pipe_stats[pipe] = I915_READ(reg);
-
-			/*
-			 * Clear the PIPE*STAT regs before the IIR
-			 */
-			if (pipe_stats[pipe] & 0x8000ffff) {
-				if (pipe_stats[pipe] & PIPE_FIFO_UNDERRUN_STATUS)
-					DRM_DEBUG_DRIVER("pipe %c underrun\n",
-							 pipe_name(pipe));
-				I915_WRITE(reg, pipe_stats[pipe]);
-				irq_received = 1;
-			}
-		}
-		spin_unlock_irqrestore(&dev_priv->irq_lock, irqflags);
-
-		if (!irq_received)
-			break;
-
-		ret = IRQ_HANDLED;
-
-		/* Consume port.  Then clear IIR or we'll miss events */
-		if ((I915_HAS_HOTPLUG(dev)) &&
-		    (iir & I915_DISPLAY_PORT_INTERRUPT)) {
-			u32 hotplug_status = I915_READ(PORT_HOTPLUG_STAT);
-
-			DRM_DEBUG_DRIVER("hotplug event received, stat 0x%08x\n",
-				  hotplug_status);
-			if (hotplug_status & dev_priv->hotplug_supported_mask)
-				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);
-		new_iir = I915_READ(IIR); /* Flush posted writes */
-
-		if (dev->primary->master) {
-			master_priv = dev->primary->master->driver_priv;
-			if (master_priv->sarea_priv)
-				master_priv->sarea_priv->last_dispatch =
-					READ_BREADCRUMB(dev_priv);
-		}
-
-		if (iir & I915_USER_INTERRUPT)
-			notify_ring(dev, &dev_priv->ring[RCS]);
-		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 (dev_priv->flip_pending_is_done)
-				intel_finish_page_flip_plane(dev, 0);
-		}
-
-		if (iir & I915_DISPLAY_PLANE_B_FLIP_PENDING_INTERRUPT) {
-			intel_prepare_page_flip(dev, 1);
-			if (dev_priv->flip_pending_is_done)
-				intel_finish_page_flip_plane(dev, 1);
-		}
-
-		for_each_pipe(pipe) {
-			if (pipe_stats[pipe] & vblank_status &&
-			    drm_handle_vblank(dev, pipe)) {
-				vblank++;
-				if (!dev_priv->flip_pending_is_done) {
-					i915_pageflip_stall_check(dev, pipe);
-					intel_finish_page_flip(dev, pipe);
-				}
-			}
-
-			if (pipe_stats[pipe] & PIPE_LEGACY_BLC_EVENT_STATUS)
-				blc_event = true;
-		}
-
-
-		if (blc_event || (iir & I915_ASLE_INTERRUPT))
-			intel_opregion_asle_intr(dev);
-
-		/* With MSI, interrupts are only generated when iir
-		 * transitions from zero to nonzero.  If another bit got
-		 * set while we were handling the existing iir bits, then
-		 * we would never get another interrupt.
-		 *
-		 * This is fine on non-MSI as well, as if we hit this path
-		 * we avoid exiting the interrupt handler only to generate
-		 * another one.
-		 *
-		 * Note that for MSI this could cause a stray interrupt report
-		 * if an interrupt landed in the time between writing IIR and
-		 * the posting read.  This should be rare enough to never
-		 * trigger the 99% of 100,000 interrupts test for disabling
-		 * stray interrupts.
-		 */
-		iir = new_iir;
-	}
-
-	return ret;
-}
-
-static int i915_emit_irq(struct drm_device * dev)
-{
-	drm_i915_private_t *dev_priv = dev->dev_private;
-	struct drm_i915_master_private *master_priv = dev->primary->master->driver_priv;
-
-	i915_kernel_lost_context(dev);
-
-	DRM_DEBUG_DRIVER("\n");
-
-	dev_priv->counter++;
-	if (dev_priv->counter > 0x7FFFFFFFUL)
-		dev_priv->counter = 1;
-	if (master_priv->sarea_priv)
-		master_priv->sarea_priv->last_enqueue = dev_priv->counter;
-
-	if (BEGIN_LP_RING(4) == 0) {
-		OUT_RING(MI_STORE_DWORD_INDEX);
-		OUT_RING(I915_BREADCRUMB_INDEX << MI_STORE_DWORD_INDEX_SHIFT);
-		OUT_RING(dev_priv->counter);
-		OUT_RING(MI_USER_INTERRUPT);
-		ADVANCE_LP_RING();
-	}
-
-	return dev_priv->counter;
-}
-
-static int i915_wait_irq(struct drm_device * dev, int irq_nr)
-{
-	drm_i915_private_t *dev_priv = (drm_i915_private_t *) dev->dev_private;
-	struct drm_i915_master_private *master_priv = dev->primary->master->driver_priv;
-	int ret = 0;
-	struct intel_ring_buffer *ring = LP_RING(dev_priv);
-
-	DRM_DEBUG_DRIVER("irq_nr=%d breadcrumb=%d\n", irq_nr,
-		  READ_BREADCRUMB(dev_priv));
-
-	if (READ_BREADCRUMB(dev_priv) >= irq_nr) {
-		if (master_priv->sarea_priv)
-			master_priv->sarea_priv->last_dispatch = READ_BREADCRUMB(dev_priv);
-		return 0;
-	}
-
-	if (master_priv->sarea_priv)
-		master_priv->sarea_priv->perf_boxes |= I915_BOX_WAIT;
-
-	if (ring->irq_get(ring)) {
-		DRM_WAIT_ON(ret, ring->irq_queue, 3 * DRM_HZ,
-			    READ_BREADCRUMB(dev_priv) >= irq_nr);
-		ring->irq_put(ring);
-	} else if (wait_for(READ_BREADCRUMB(dev_priv) >= irq_nr, 3000))
-		ret = -EBUSY;
-
-	if (ret == -EBUSY) {
-		DRM_ERROR("EBUSY -- rec: %d emitted: %d\n",
-			  READ_BREADCRUMB(dev_priv), (int)dev_priv->counter);
-	}
-
-	return ret;
-}
-
-/* Needs the lock as it touches the ring.
- */
-int i915_irq_emit(struct drm_device *dev, void *data,
-			 struct drm_file *file_priv)
-{
-	drm_i915_private_t *dev_priv = dev->dev_private;
-	drm_i915_irq_emit_t *emit = data;
-	int result;
-
-	if (!dev_priv || !LP_RING(dev_priv)->virtual_start) {
-		DRM_ERROR("called with no initialization\n");
-		return -EINVAL;
-	}
-
-	RING_LOCK_TEST_WITH_RETURN(dev, file_priv);
-
-	mutex_lock(&dev->struct_mutex);
-	result = i915_emit_irq(dev);
-	mutex_unlock(&dev->struct_mutex);
-
-	if (DRM_COPY_TO_USER(emit->irq_seq, &result, sizeof(int))) {
-		DRM_ERROR("copy_to_user\n");
-		return -EFAULT;
-	}
-
-	return 0;
-}
-
-/* Doesn't need the hardware lock.
- */
-int i915_irq_wait(struct drm_device *dev, void *data,
-			 struct drm_file *file_priv)
-{
-	drm_i915_private_t *dev_priv = dev->dev_private;
-	drm_i915_irq_wait_t *irqwait = data;
-
-	if (!dev_priv) {
-		DRM_ERROR("called with no initialization\n");
-		return -EINVAL;
-	}
-
-	return i915_wait_irq(dev, irqwait->irq_seq);
-}
-
 /* Called from drm generic code, passed 'crtc' which
  * we use as a pipe index
  */
@@ -1544,7 +1376,7 @@ static int i915_enable_vblank(struct drm_device *dev, int pipe)
 
 	/* maintain vblank delivery even in deep C-states */
 	if (dev_priv->info->gen == 3)
-		I915_WRITE(INSTPM, INSTPM_AGPBUSY_DIS << 16);
+		I915_WRITE(INSTPM, _MASKED_BIT_DISABLE(INSTPM_AGPBUSY_DIS));
 	spin_unlock_irqrestore(&dev_priv->irq_lock, irqflags);
 
 	return 0;
@@ -1575,8 +1407,34 @@ static int ivybridge_enable_vblank(struct drm_device *dev, int pipe)
 		return -EINVAL;
 
 	spin_lock_irqsave(&dev_priv->irq_lock, irqflags);
-	ironlake_enable_display_irq(dev_priv, (pipe == 0) ?
-				    DE_PIPEA_VBLANK_IVB : DE_PIPEB_VBLANK_IVB);
+	ironlake_enable_display_irq(dev_priv,
+				    DE_PIPEA_VBLANK_IVB << (5 * pipe));
+	spin_unlock_irqrestore(&dev_priv->irq_lock, irqflags);
+
+	return 0;
+}
+
+static int valleyview_enable_vblank(struct drm_device *dev, int pipe)
+{
+	drm_i915_private_t *dev_priv = (drm_i915_private_t *) dev->dev_private;
+	unsigned long irqflags;
+	u32 dpfl, imr;
+
+	if (!i915_pipe_enabled(dev, pipe))
+		return -EINVAL;
+
+	spin_lock_irqsave(&dev_priv->irq_lock, irqflags);
+	dpfl = I915_READ(VLV_DPFLIPSTAT);
+	imr = I915_READ(VLV_IMR);
+	if (pipe == 0) {
+		dpfl |= PIPEA_VBLANK_INT_EN;
+		imr &= ~I915_DISPLAY_PIPE_A_VBLANK_INTERRUPT;
+	} else {
+		dpfl |= PIPEA_VBLANK_INT_EN;
+		imr &= ~I915_DISPLAY_PIPE_B_VBLANK_INTERRUPT;
+	}
+	I915_WRITE(VLV_DPFLIPSTAT, dpfl);
+	I915_WRITE(VLV_IMR, imr);
 	spin_unlock_irqrestore(&dev_priv->irq_lock, irqflags);
 
 	return 0;
@@ -1592,8 +1450,7 @@ static void i915_disable_vblank(struct drm_device *dev, int pipe)
 
 	spin_lock_irqsave(&dev_priv->irq_lock, irqflags);
 	if (dev_priv->info->gen == 3)
-		I915_WRITE(INSTPM,
-			   INSTPM_AGPBUSY_DIS << 16 | INSTPM_AGPBUSY_DIS);
+		I915_WRITE(INSTPM, _MASKED_BIT_ENABLE(INSTPM_AGPBUSY_DIS));
 
 	i915_disable_pipestat(dev_priv, pipe,
 			      PIPE_VBLANK_INTERRUPT_ENABLE |
@@ -1618,63 +1475,30 @@ static void ivybridge_disable_vblank(struct drm_device *dev, int pipe)
 	unsigned long irqflags;
 
 	spin_lock_irqsave(&dev_priv->irq_lock, irqflags);
-	ironlake_disable_display_irq(dev_priv, (pipe == 0) ?
-				     DE_PIPEA_VBLANK_IVB : DE_PIPEB_VBLANK_IVB);
+	ironlake_disable_display_irq(dev_priv,
+				     DE_PIPEA_VBLANK_IVB << (pipe * 5));
 	spin_unlock_irqrestore(&dev_priv->irq_lock, irqflags);
 }
 
-/* Set the vblank monitor pipe
- */
-int i915_vblank_pipe_set(struct drm_device *dev, void *data,
-			 struct drm_file *file_priv)
+static void valleyview_disable_vblank(struct drm_device *dev, int pipe)
 {
-	drm_i915_private_t *dev_priv = dev->dev_private;
-
-	if (!dev_priv) {
-		DRM_ERROR("called with no initialization\n");
-		return -EINVAL;
-	}
-
-	return 0;
-}
-
-int i915_vblank_pipe_get(struct drm_device *dev, void *data,
-			 struct drm_file *file_priv)
-{
-	drm_i915_private_t *dev_priv = dev->dev_private;
-	drm_i915_vblank_pipe_t *pipe = data;
+	drm_i915_private_t *dev_priv = (drm_i915_private_t *) dev->dev_private;
+	unsigned long irqflags;
+	u32 dpfl, imr;
 
-	if (!dev_priv) {
-		DRM_ERROR("called with no initialization\n");
-		return -EINVAL;
+	spin_lock_irqsave(&dev_priv->irq_lock, irqflags);
+	dpfl = I915_READ(VLV_DPFLIPSTAT);
+	imr = I915_READ(VLV_IMR);
+	if (pipe == 0) {
+		dpfl &= ~PIPEA_VBLANK_INT_EN;
+		imr |= I915_DISPLAY_PIPE_A_VBLANK_INTERRUPT;
+	} else {
+		dpfl &= ~PIPEB_VBLANK_INT_EN;
+		imr |= I915_DISPLAY_PIPE_B_VBLANK_INTERRUPT;
 	}
-
-	pipe->pipe = DRM_I915_VBLANK_PIPE_A | DRM_I915_VBLANK_PIPE_B;
-
-	return 0;
-}
-
-/**
- * Schedule buffer swap at given vertical blank.
- */
-int i915_vblank_swap(struct drm_device *dev, void *data,
-		     struct drm_file *file_priv)
-{
-	/* The delayed swap mechanism was fundamentally racy, and has been
-	 * removed.  The model was that the client requested a delayed flip/swap
-	 * from the kernel, then waited for vblank before continuing to perform
-	 * rendering.  The problem was that the kernel might wake the client
-	 * up before it dispatched the vblank swap (since the lock has to be
-	 * held while touching the ringbuffer), in which case the client would
-	 * clear and start the next frame before the swap occurred, and
-	 * flicker would occur in addition to likely missing the vblank.
-	 *
-	 * In the absence of this ioctl, userland falls back to a correct path
-	 * of waiting for a vblank, then dispatching the swap on its own.
-	 * Context switching to userland and back is plenty fast enough for
-	 * meeting the requirements of vblank swapping.
-	 */
-	return -EINVAL;
+	I915_WRITE(VLV_IMR, imr);
+	I915_WRITE(VLV_DPFLIPSTAT, dpfl);
+	spin_unlock_irqrestore(&dev_priv->irq_lock, irqflags);
 }
 
 static u32
@@ -1689,11 +1513,9 @@ static bool i915_hangcheck_ring_idle(struct intel_ring_buffer *ring, bool *err)
 	if (list_empty(&ring->request_list) ||
 	    i915_seqno_passed(ring->get_seqno(ring), ring_last_seqno(ring))) {
 		/* Issue a wake-up to catch stuck h/w. */
-		if (ring->waiting_seqno && waitqueue_active(&ring->irq_queue)) {
-			DRM_ERROR("Hangcheck timer elapsed... %s idle [waiting on %d, at %d], missed IRQ?\n",
-				  ring->name,
-				  ring->waiting_seqno,
-				  ring->get_seqno(ring));
+		if (waitqueue_active(&ring->irq_queue)) {
+			DRM_ERROR("Hangcheck timer elapsed... %s idle\n",
+				  ring->name);
 			wake_up_all(&ring->irq_queue);
 			*err = true;
 		}
@@ -1716,6 +1538,35 @@ static bool kick_ring(struct intel_ring_buffer *ring)
 	return false;
 }
 
+static bool i915_hangcheck_hung(struct drm_device *dev)
+{
+	drm_i915_private_t *dev_priv = dev->dev_private;
+
+	if (dev_priv->hangcheck_count++ > 1) {
+		bool hung = true;
+
+		DRM_ERROR("Hangcheck timer elapsed... GPU hung\n");
+		i915_handle_error(dev, true);
+
+		if (!IS_GEN2(dev)) {
+			struct intel_ring_buffer *ring;
+			int i;
+
+			/* Is the chip hanging on a WAIT_FOR_EVENT?
+			 * If so we can simply poke the RB_WAIT bit
+			 * and break the hang. This should work on
+			 * all but the second generation chipsets.
+			 */
+			for_each_ring(ring, dev_priv, i)
+				hung &= !kick_ring(ring);
+		}
+
+		return hung;
+	}
+
+	return false;
+}
+
 /**
  * This is called when the chip hasn't reported back with completed
  * batchbuffers in a long time. The first time this is called we simply record
@@ -1726,19 +1577,31 @@ void i915_hangcheck_elapsed(unsigned long data)
 {
 	struct drm_device *dev = (struct drm_device *)data;
 	drm_i915_private_t *dev_priv = dev->dev_private;
-	uint32_t acthd, instdone, instdone1, acthd_bsd, acthd_blt;
-	bool err = false;
+	uint32_t acthd[I915_NUM_RINGS], instdone, instdone1;
+	struct intel_ring_buffer *ring;
+	bool err = false, idle;
+	int i;
 
 	if (!i915_enable_hangcheck)
 		return;
 
+	memset(acthd, 0, sizeof(acthd));
+	idle = true;
+	for_each_ring(ring, dev_priv, i) {
+	    idle &= i915_hangcheck_ring_idle(ring, &err);
+	    acthd[i] = intel_ring_get_active_head(ring);
+	}
+
 	/* If all work is done then ACTHD clearly hasn't advanced. */
-	if (i915_hangcheck_ring_idle(&dev_priv->ring[RCS], &err) &&
-	    i915_hangcheck_ring_idle(&dev_priv->ring[VCS], &err) &&
-	    i915_hangcheck_ring_idle(&dev_priv->ring[BCS], &err)) {
-		dev_priv->hangcheck_count = 0;
-		if (err)
+	if (idle) {
+		if (err) {
+			if (i915_hangcheck_hung(dev))
+				return;
+
 			goto repeat;
+		}
+
+		dev_priv->hangcheck_count = 0;
 		return;
 	}
 
@@ -1749,47 +1612,16 @@ void i915_hangcheck_elapsed(unsigned long data)
 		instdone = I915_READ(INSTDONE_I965);
 		instdone1 = I915_READ(INSTDONE1);
 	}
-	acthd = intel_ring_get_active_head(&dev_priv->ring[RCS]);
-	acthd_bsd = HAS_BSD(dev) ?
-		intel_ring_get_active_head(&dev_priv->ring[VCS]) : 0;
-	acthd_blt = HAS_BLT(dev) ?
-		intel_ring_get_active_head(&dev_priv->ring[BCS]) : 0;
 
-	if (dev_priv->last_acthd == acthd &&
-	    dev_priv->last_acthd_bsd == acthd_bsd &&
-	    dev_priv->last_acthd_blt == acthd_blt &&
+	if (memcmp(dev_priv->last_acthd, acthd, sizeof(acthd)) == 0 &&
 	    dev_priv->last_instdone == instdone &&
 	    dev_priv->last_instdone1 == instdone1) {
-		if (dev_priv->hangcheck_count++ > 1) {
-			DRM_ERROR("Hangcheck timer elapsed... GPU hung\n");
-			i915_handle_error(dev, true);
-
-			if (!IS_GEN2(dev)) {
-				/* Is the chip hanging on a WAIT_FOR_EVENT?
-				 * If so we can simply poke the RB_WAIT bit
-				 * and break the hang. This should work on
-				 * all but the second generation chipsets.
-				 */
-				if (kick_ring(&dev_priv->ring[RCS]))
-					goto repeat;
-
-				if (HAS_BSD(dev) &&
-				    kick_ring(&dev_priv->ring[VCS]))
-					goto repeat;
-
-				if (HAS_BLT(dev) &&
-				    kick_ring(&dev_priv->ring[BCS]))
-					goto repeat;
-			}
-
+		if (i915_hangcheck_hung(dev))
 			return;
-		}
 	} else {
 		dev_priv->hangcheck_count = 0;
 
-		dev_priv->last_acthd = acthd;
-		dev_priv->last_acthd_bsd = acthd_bsd;
-		dev_priv->last_acthd_blt = acthd_blt;
+		memcpy(dev_priv->last_acthd, acthd, sizeof(acthd));
 		dev_priv->last_instdone = instdone;
 		dev_priv->last_instdone1 = instdone1;
 	}
@@ -1808,10 +1640,6 @@ static void ironlake_irq_preinstall(struct drm_device *dev)
 
 	atomic_set(&dev_priv->irq_received, 0);
 
-	INIT_WORK(&dev_priv->hotplug_work, i915_hotplug_work_func);
-	INIT_WORK(&dev_priv->error_work, i915_error_work_func);
-	if (IS_GEN6(dev) || IS_IVYBRIDGE(dev))
-		INIT_WORK(&dev_priv->rps_work, gen6_pm_rps_work);
 
 	I915_WRITE(HWSTAM, 0xeffe);
 
@@ -1832,6 +1660,38 @@ static void ironlake_irq_preinstall(struct drm_device *dev)
 	POSTING_READ(SDEIER);
 }
 
+static void valleyview_irq_preinstall(struct drm_device *dev)
+{
+	drm_i915_private_t *dev_priv = (drm_i915_private_t *) dev->dev_private;
+	int pipe;
+
+	atomic_set(&dev_priv->irq_received, 0);
+
+	/* VLV magic */
+	I915_WRITE(VLV_IMR, 0);
+	I915_WRITE(RING_IMR(RENDER_RING_BASE), 0);
+	I915_WRITE(RING_IMR(GEN6_BSD_RING_BASE), 0);
+	I915_WRITE(RING_IMR(BLT_RING_BASE), 0);
+
+	/* and GT */
+	I915_WRITE(GTIIR, I915_READ(GTIIR));
+	I915_WRITE(GTIIR, I915_READ(GTIIR));
+	I915_WRITE(GTIMR, 0xffffffff);
+	I915_WRITE(GTIER, 0x0);
+	POSTING_READ(GTIER);
+
+	I915_WRITE(DPINVGTT, 0xff);
+
+	I915_WRITE(PORT_HOTPLUG_EN, 0);
+	I915_WRITE(PORT_HOTPLUG_STAT, I915_READ(PORT_HOTPLUG_STAT));
+	for_each_pipe(pipe)
+		I915_WRITE(PIPESTAT(pipe), 0xffff);
+	I915_WRITE(VLV_IIR, 0xffffffff);
+	I915_WRITE(VLV_IMR, 0xffffffff);
+	I915_WRITE(VLV_IER, 0x0);
+	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)
@@ -1861,13 +1721,6 @@ static int ironlake_irq_postinstall(struct drm_device *dev)
 	u32 render_irqs;
 	u32 hotplug_mask;
 
-	DRM_INIT_WAITQUEUE(&dev_priv->ring[RCS].irq_queue);
-	if (HAS_BSD(dev))
-		DRM_INIT_WAITQUEUE(&dev_priv->ring[VCS].irq_queue);
-	if (HAS_BLT(dev))
-		DRM_INIT_WAITQUEUE(&dev_priv->ring[BCS].irq_queue);
-
-	dev_priv->vblank_pipe = DRM_I915_VBLANK_PIPE_A | DRM_I915_VBLANK_PIPE_B;
 	dev_priv->irq_mask = ~display_mask;
 
 	/* should always can generate irq */
@@ -1884,8 +1737,8 @@ static int ironlake_irq_postinstall(struct drm_device *dev)
 	if (IS_GEN6(dev))
 		render_irqs =
 			GT_USER_INTERRUPT |
-			GT_GEN6_BSD_USER_INTERRUPT |
-			GT_BLT_USER_INTERRUPT;
+			GEN6_BSD_USER_INTERRUPT |
+			GEN6_BLITTER_USER_INTERRUPT;
 	else
 		render_irqs =
 			GT_USER_INTERRUPT |
@@ -1930,26 +1783,24 @@ static int ivybridge_irq_postinstall(struct drm_device *dev)
 {
 	drm_i915_private_t *dev_priv = (drm_i915_private_t *) dev->dev_private;
 	/* enable kind of interrupts always enabled */
-	u32 display_mask = DE_MASTER_IRQ_CONTROL | DE_GSE_IVB |
-		DE_PCH_EVENT_IVB | DE_PLANEA_FLIP_DONE_IVB |
-		DE_PLANEB_FLIP_DONE_IVB;
+	u32 display_mask =
+		DE_MASTER_IRQ_CONTROL | DE_GSE_IVB | DE_PCH_EVENT_IVB |
+		DE_PLANEC_FLIP_DONE_IVB |
+		DE_PLANEB_FLIP_DONE_IVB |
+		DE_PLANEA_FLIP_DONE_IVB;
 	u32 render_irqs;
 	u32 hotplug_mask;
 
-	DRM_INIT_WAITQUEUE(&dev_priv->ring[RCS].irq_queue);
-	if (HAS_BSD(dev))
-		DRM_INIT_WAITQUEUE(&dev_priv->ring[VCS].irq_queue);
-	if (HAS_BLT(dev))
-		DRM_INIT_WAITQUEUE(&dev_priv->ring[BCS].irq_queue);
-
-	dev_priv->vblank_pipe = DRM_I915_VBLANK_PIPE_A | DRM_I915_VBLANK_PIPE_B;
 	dev_priv->irq_mask = ~display_mask;
 
 	/* should always can generate irq */
 	I915_WRITE(DEIIR, I915_READ(DEIIR));
 	I915_WRITE(DEIMR, dev_priv->irq_mask);
-	I915_WRITE(DEIER, display_mask | DE_PIPEA_VBLANK_IVB |
-		   DE_PIPEB_VBLANK_IVB);
+	I915_WRITE(DEIER,
+		   display_mask |
+		   DE_PIPEC_VBLANK_IVB |
+		   DE_PIPEB_VBLANK_IVB |
+		   DE_PIPEA_VBLANK_IVB);
 	POSTING_READ(DEIER);
 
 	dev_priv->gt_irq_mask = ~0;
@@ -1957,8 +1808,8 @@ static int ivybridge_irq_postinstall(struct drm_device *dev)
 	I915_WRITE(GTIIR, I915_READ(GTIIR));
 	I915_WRITE(GTIMR, dev_priv->gt_irq_mask);
 
-	render_irqs = GT_USER_INTERRUPT | GT_GEN6_BSD_USER_INTERRUPT |
-		GT_BLT_USER_INTERRUPT;
+	render_irqs = GT_USER_INTERRUPT | GEN6_BSD_USER_INTERRUPT |
+		GEN6_BLITTER_USER_INTERRUPT;
 	I915_WRITE(GTIER, render_irqs);
 	POSTING_READ(GTIER);
 
@@ -1978,15 +1829,496 @@ static int ivybridge_irq_postinstall(struct drm_device *dev)
 	return 0;
 }
 
-static void i915_driver_irq_preinstall(struct drm_device * dev)
+static int valleyview_irq_postinstall(struct drm_device *dev)
+{
+	drm_i915_private_t *dev_priv = (drm_i915_private_t *) dev->dev_private;
+	u32 render_irqs;
+	u32 enable_mask;
+	u32 hotplug_en = I915_READ(PORT_HOTPLUG_EN);
+	u16 msid;
+
+	enable_mask = I915_DISPLAY_PORT_INTERRUPT;
+	enable_mask |= I915_DISPLAY_PIPE_A_VBLANK_INTERRUPT |
+		I915_DISPLAY_PIPE_B_VBLANK_INTERRUPT;
+
+	dev_priv->irq_mask = ~enable_mask;
+
+	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);
+	msid &= 0xff; /* mask out delivery bits */
+	msid |= (1<<14);
+	pci_write_config_word(dev_priv->dev->pdev, 0x98, msid);
+
+	I915_WRITE(VLV_IMR, dev_priv->irq_mask);
+	I915_WRITE(VLV_IER, enable_mask);
+	I915_WRITE(VLV_IIR, 0xffffffff);
+	I915_WRITE(PIPESTAT(0), 0xffff);
+	I915_WRITE(PIPESTAT(1), 0xffff);
+	POSTING_READ(VLV_IER);
+
+	I915_WRITE(VLV_IIR, 0xffffffff);
+	I915_WRITE(VLV_IIR, 0xffffffff);
+
+	render_irqs = GT_GEN6_BLT_FLUSHDW_NOTIFY_INTERRUPT |
+		GT_GEN6_BLT_CS_ERROR_INTERRUPT |
+		GT_GEN6_BLT_USER_INTERRUPT |
+		GT_GEN6_BSD_USER_INTERRUPT |
+		GT_GEN6_BSD_CS_ERROR_INTERRUPT |
+		GT_GEN7_L3_PARITY_ERROR_INTERRUPT |
+		GT_PIPE_NOTIFY |
+		GT_RENDER_CS_ERROR_INTERRUPT |
+		GT_SYNC_STATUS |
+		GT_USER_INTERRUPT;
+
+	dev_priv->gt_irq_mask = ~render_irqs;
+
+	I915_WRITE(GTIIR, I915_READ(GTIIR));
+	I915_WRITE(GTIIR, I915_READ(GTIIR));
+	I915_WRITE(GTIMR, 0);
+	I915_WRITE(GTIER, render_irqs);
+	POSTING_READ(GTIER);
+
+	/* ack & enable invalid PTE error interrupts */
+#if 0 /* FIXME: add support to irq handler for checking these bits */
+	I915_WRITE(DPINVGTT, DPINVGTT_STATUS_MASK);
+	I915_WRITE(DPINVGTT, DPINVGTT_EN_MASK);
+#endif
+
+	I915_WRITE(VLV_MASTER_IER, MASTER_INTERRUPT_ENABLE);
+#if 0 /* FIXME: check register definitions; some have moved */
+	/* Note HDMI and DP share bits */
+	if (dev_priv->hotplug_supported_mask & HDMIB_HOTPLUG_INT_STATUS)
+		hotplug_en |= HDMIB_HOTPLUG_INT_EN;
+	if (dev_priv->hotplug_supported_mask & HDMIC_HOTPLUG_INT_STATUS)
+		hotplug_en |= HDMIC_HOTPLUG_INT_EN;
+	if (dev_priv->hotplug_supported_mask & HDMID_HOTPLUG_INT_STATUS)
+		hotplug_en |= HDMID_HOTPLUG_INT_EN;
+	if (dev_priv->hotplug_supported_mask & SDVOC_HOTPLUG_INT_STATUS)
+		hotplug_en |= SDVOC_HOTPLUG_INT_EN;
+	if (dev_priv->hotplug_supported_mask & SDVOB_HOTPLUG_INT_STATUS)
+		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;
+	}
+#endif
+
+	I915_WRITE(PORT_HOTPLUG_EN, hotplug_en);
+
+	return 0;
+}
+
+static void valleyview_irq_uninstall(struct drm_device *dev)
+{
+	drm_i915_private_t *dev_priv = (drm_i915_private_t *) dev->dev_private;
+	int pipe;
+
+	if (!dev_priv)
+		return;
+
+	for_each_pipe(pipe)
+		I915_WRITE(PIPESTAT(pipe), 0xffff);
+
+	I915_WRITE(HWSTAM, 0xffffffff);
+	I915_WRITE(PORT_HOTPLUG_EN, 0);
+	I915_WRITE(PORT_HOTPLUG_STAT, I915_READ(PORT_HOTPLUG_STAT));
+	for_each_pipe(pipe)
+		I915_WRITE(PIPESTAT(pipe), 0xffff);
+	I915_WRITE(VLV_IIR, 0xffffffff);
+	I915_WRITE(VLV_IMR, 0xffffffff);
+	I915_WRITE(VLV_IER, 0x0);
+	POSTING_READ(VLV_IER);
+}
+
+static void ironlake_irq_uninstall(struct drm_device *dev)
+{
+	drm_i915_private_t *dev_priv = (drm_i915_private_t *) dev->dev_private;
+
+	if (!dev_priv)
+		return;
+
+	I915_WRITE(HWSTAM, 0xffffffff);
+
+	I915_WRITE(DEIMR, 0xffffffff);
+	I915_WRITE(DEIER, 0x0);
+	I915_WRITE(DEIIR, I915_READ(DEIIR));
+
+	I915_WRITE(GTIMR, 0xffffffff);
+	I915_WRITE(GTIER, 0x0);
+	I915_WRITE(GTIIR, I915_READ(GTIIR));
+
+	I915_WRITE(SDEIMR, 0xffffffff);
+	I915_WRITE(SDEIER, 0x0);
+	I915_WRITE(SDEIIR, I915_READ(SDEIIR));
+}
+
+static void i8xx_irq_preinstall(struct drm_device * dev)
 {
 	drm_i915_private_t *dev_priv = (drm_i915_private_t *) dev->dev_private;
 	int pipe;
 
 	atomic_set(&dev_priv->irq_received, 0);
 
-	INIT_WORK(&dev_priv->hotplug_work, i915_hotplug_work_func);
-	INIT_WORK(&dev_priv->error_work, i915_error_work_func);
+	for_each_pipe(pipe)
+		I915_WRITE(PIPESTAT(pipe), 0);
+	I915_WRITE16(IMR, 0xffff);
+	I915_WRITE16(IER, 0x0);
+	POSTING_READ16(IER);
+}
+
+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));
+
+	/* Unmask the interrupts that we always want on. */
+	dev_priv->irq_mask =
+		~(I915_DISPLAY_PIPE_A_EVENT_INTERRUPT |
+		  I915_DISPLAY_PIPE_B_EVENT_INTERRUPT |
+		  I915_DISPLAY_PLANE_A_FLIP_PENDING_INTERRUPT |
+		  I915_DISPLAY_PLANE_B_FLIP_PENDING_INTERRUPT |
+		  I915_RENDER_COMMAND_PARSER_ERROR_INTERRUPT);
+	I915_WRITE16(IMR, dev_priv->irq_mask);
+
+	I915_WRITE16(IER,
+		     I915_DISPLAY_PIPE_A_EVENT_INTERRUPT |
+		     I915_DISPLAY_PIPE_B_EVENT_INTERRUPT |
+		     I915_RENDER_COMMAND_PARSER_ERROR_INTERRUPT |
+		     I915_USER_INTERRUPT);
+	POSTING_READ16(IER);
+
+	return 0;
+}
+
+static irqreturn_t i8xx_irq_handler(DRM_IRQ_ARGS)
+{
+	struct drm_device *dev = (struct drm_device *) arg;
+	drm_i915_private_t *dev_priv = (drm_i915_private_t *) dev->dev_private;
+	u16 iir, new_iir;
+	u32 pipe_stats[2];
+	unsigned long irqflags;
+	int irq_received;
+	int pipe;
+	u16 flip_mask =
+		I915_DISPLAY_PLANE_A_FLIP_PENDING_INTERRUPT |
+		I915_DISPLAY_PLANE_B_FLIP_PENDING_INTERRUPT;
+
+	atomic_inc(&dev_priv->irq_received);
+
+	iir = I915_READ16(IIR);
+	if (iir == 0)
+		return IRQ_NONE;
+
+	while (iir & ~flip_mask) {
+		/* Can't rely on pipestat interrupt bit in iir as it might
+		 * have been cleared after the pipestat interrupt was received.
+		 * It doesn't set the bit in iir again, but it still produces
+		 * interrupts (for non-MSI).
+		 */
+		spin_lock_irqsave(&dev_priv->irq_lock, irqflags);
+		if (iir & I915_RENDER_COMMAND_PARSER_ERROR_INTERRUPT)
+			i915_handle_error(dev, false);
+
+		for_each_pipe(pipe) {
+			int reg = PIPESTAT(pipe);
+			pipe_stats[pipe] = I915_READ(reg);
+
+			/*
+			 * Clear the PIPE*STAT regs before the IIR
+			 */
+			if (pipe_stats[pipe] & 0x8000ffff) {
+				if (pipe_stats[pipe] & PIPE_FIFO_UNDERRUN_STATUS)
+					DRM_DEBUG_DRIVER("pipe %c underrun\n",
+							 pipe_name(pipe));
+				I915_WRITE(reg, pipe_stats[pipe]);
+				irq_received = 1;
+			}
+		}
+		spin_unlock_irqrestore(&dev_priv->irq_lock, irqflags);
+
+		I915_WRITE16(IIR, iir & ~flip_mask);
+		new_iir = I915_READ16(IIR); /* Flush posted writes */
+
+		i915_update_dri1_breadcrumb(dev);
+
+		if (iir & I915_USER_INTERRUPT)
+			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;
+			}
+		}
+
+		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;
+			}
+		}
+
+		iir = new_iir;
+	}
+
+	return IRQ_HANDLED;
+}
+
+static void i8xx_irq_uninstall(struct drm_device * dev)
+{
+	drm_i915_private_t *dev_priv = (drm_i915_private_t *) dev->dev_private;
+	int pipe;
+
+	for_each_pipe(pipe) {
+		/* Clear enable bits; then clear status bits */
+		I915_WRITE(PIPESTAT(pipe), 0);
+		I915_WRITE(PIPESTAT(pipe), I915_READ(PIPESTAT(pipe)));
+	}
+	I915_WRITE16(IMR, 0xffff);
+	I915_WRITE16(IER, 0x0);
+	I915_WRITE16(IIR, I915_READ16(IIR));
+}
+
+static void i915_irq_preinstall(struct drm_device * dev)
+{
+	drm_i915_private_t *dev_priv = (drm_i915_private_t *) dev->dev_private;
+	int pipe;
+
+	atomic_set(&dev_priv->irq_received, 0);
+
+	if (I915_HAS_HOTPLUG(dev)) {
+		I915_WRITE(PORT_HOTPLUG_EN, 0);
+		I915_WRITE(PORT_HOTPLUG_STAT, I915_READ(PORT_HOTPLUG_STAT));
+	}
+
+	I915_WRITE16(HWSTAM, 0xeffe);
+	for_each_pipe(pipe)
+		I915_WRITE(PIPESTAT(pipe), 0);
+	I915_WRITE(IMR, 0xffffffff);
+	I915_WRITE(IER, 0x0);
+	POSTING_READ(IER);
+}
+
+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. */
+	dev_priv->irq_mask =
+		~(I915_ASLE_INTERRUPT |
+		  I915_DISPLAY_PIPE_A_EVENT_INTERRUPT |
+		  I915_DISPLAY_PIPE_B_EVENT_INTERRUPT |
+		  I915_DISPLAY_PLANE_A_FLIP_PENDING_INTERRUPT |
+		  I915_DISPLAY_PLANE_B_FLIP_PENDING_INTERRUPT |
+		  I915_RENDER_COMMAND_PARSER_ERROR_INTERRUPT);
+
+	enable_mask =
+		I915_ASLE_INTERRUPT |
+		I915_DISPLAY_PIPE_A_EVENT_INTERRUPT |
+		I915_DISPLAY_PIPE_B_EVENT_INTERRUPT |
+		I915_RENDER_COMMAND_PARSER_ERROR_INTERRUPT |
+		I915_USER_INTERRUPT;
+
+	if (I915_HAS_HOTPLUG(dev)) {
+		/* Enable in IER... */
+		enable_mask |= I915_DISPLAY_PORT_INTERRUPT;
+		/* and unmask in IMR */
+		dev_priv->irq_mask &= ~I915_DISPLAY_PORT_INTERRUPT;
+	}
+
+	I915_WRITE(IMR, dev_priv->irq_mask);
+	I915_WRITE(IER, enable_mask);
+	POSTING_READ(IER);
+
+	if (I915_HAS_HOTPLUG(dev)) {
+		u32 hotplug_en = I915_READ(PORT_HOTPLUG_EN);
+
+		if (dev_priv->hotplug_supported_mask & HDMIB_HOTPLUG_INT_STATUS)
+			hotplug_en |= HDMIB_HOTPLUG_INT_EN;
+		if (dev_priv->hotplug_supported_mask & HDMIC_HOTPLUG_INT_STATUS)
+			hotplug_en |= HDMIC_HOTPLUG_INT_EN;
+		if (dev_priv->hotplug_supported_mask & HDMID_HOTPLUG_INT_STATUS)
+			hotplug_en |= HDMID_HOTPLUG_INT_EN;
+		if (dev_priv->hotplug_supported_mask & SDVOC_HOTPLUG_INT_STATUS)
+			hotplug_en |= SDVOC_HOTPLUG_INT_EN;
+		if (dev_priv->hotplug_supported_mask & SDVOB_HOTPLUG_INT_STATUS)
+			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;
+		}
+
+		/* Ignore TV since it's buggy */
+
+		I915_WRITE(PORT_HOTPLUG_EN, hotplug_en);
+	}
+
+	intel_opregion_enable_asle(dev);
+
+	return 0;
+}
+
+static irqreturn_t i915_irq_handler(DRM_IRQ_ARGS)
+{
+	struct drm_device *dev = (struct drm_device *) arg;
+	drm_i915_private_t *dev_priv = (drm_i915_private_t *) dev->dev_private;
+	u32 iir, new_iir, pipe_stats[I915_MAX_PIPES];
+	unsigned long irqflags;
+	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);
+
+	iir = I915_READ(IIR);
+	do {
+		bool irq_received = (iir & ~flip_mask) != 0;
+		bool blc_event = false;
+
+		/* Can't rely on pipestat interrupt bit in iir as it might
+		 * have been cleared after the pipestat interrupt was received.
+		 * It doesn't set the bit in iir again, but it still produces
+		 * interrupts (for non-MSI).
+		 */
+		spin_lock_irqsave(&dev_priv->irq_lock, irqflags);
+		if (iir & I915_RENDER_COMMAND_PARSER_ERROR_INTERRUPT)
+			i915_handle_error(dev, false);
+
+		for_each_pipe(pipe) {
+			int reg = PIPESTAT(pipe);
+			pipe_stats[pipe] = I915_READ(reg);
+
+			/* Clear the PIPE*STAT regs before the IIR */
+			if (pipe_stats[pipe] & 0x8000ffff) {
+				if (pipe_stats[pipe] & PIPE_FIFO_UNDERRUN_STATUS)
+					DRM_DEBUG_DRIVER("pipe %c underrun\n",
+							 pipe_name(pipe));
+				I915_WRITE(reg, pipe_stats[pipe]);
+				irq_received = true;
+			}
+		}
+		spin_unlock_irqrestore(&dev_priv->irq_lock, irqflags);
+
+		if (!irq_received)
+			break;
+
+		/* Consume port.  Then clear IIR or we'll miss events */
+		if ((I915_HAS_HOTPLUG(dev)) &&
+		    (iir & I915_DISPLAY_PORT_INTERRUPT)) {
+			u32 hotplug_status = I915_READ(PORT_HOTPLUG_STAT);
+
+			DRM_DEBUG_DRIVER("hotplug event received, stat 0x%08x\n",
+				  hotplug_status);
+			if (hotplug_status & dev_priv->hotplug_supported_mask)
+				queue_work(dev_priv->wq,
+					   &dev_priv->hotplug_work);
+
+			I915_WRITE(PORT_HOTPLUG_STAT, hotplug_status);
+			POSTING_READ(PORT_HOTPLUG_STAT);
+		}
+
+		I915_WRITE(IIR, iir & ~flip_mask);
+		new_iir = I915_READ(IIR); /* Flush posted writes */
+
+		if (iir & I915_USER_INTERRUPT)
+			notify_ring(dev, &dev_priv->ring[RCS]);
+
+		for_each_pipe(pipe) {
+			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];
+				}
+			}
+
+			if (pipe_stats[pipe] & PIPE_LEGACY_BLC_EVENT_STATUS)
+				blc_event = true;
+		}
+
+		if (blc_event || (iir & I915_ASLE_INTERRUPT))
+			intel_opregion_asle_intr(dev);
+
+		/* With MSI, interrupts are only generated when iir
+		 * transitions from zero to nonzero.  If another bit got
+		 * set while we were handling the existing iir bits, then
+		 * we would never get another interrupt.
+		 *
+		 * This is fine on non-MSI as well, as if we hit this path
+		 * we avoid exiting the interrupt handler only to generate
+		 * another one.
+		 *
+		 * Note that for MSI this could cause a stray interrupt report
+		 * if an interrupt landed in the time between writing IIR and
+		 * the posting read.  This should be rare enough to never
+		 * trigger the 99% of 100,000 interrupts test for disabling
+		 * stray interrupts.
+		 */
+		ret = IRQ_HANDLED;
+		iir = new_iir;
+	} while (iir & ~flip_mask);
+
+	i915_update_dri1_breadcrumb(dev);
+
+	return ret;
+}
+
+static void i915_irq_uninstall(struct drm_device * dev)
+{
+	drm_i915_private_t *dev_priv = (drm_i915_private_t *) dev->dev_private;
+	int pipe;
+
+	if (I915_HAS_HOTPLUG(dev)) {
+		I915_WRITE(PORT_HOTPLUG_EN, 0);
+		I915_WRITE(PORT_HOTPLUG_STAT, I915_READ(PORT_HOTPLUG_STAT));
+	}
+
+	I915_WRITE16(HWSTAM, 0xffff);
+	for_each_pipe(pipe) {
+		/* Clear enable bits; then clear status bits */
+		I915_WRITE(PIPESTAT(pipe), 0);
+		I915_WRITE(PIPESTAT(pipe), I915_READ(PIPESTAT(pipe)));
+	}
+	I915_WRITE(IMR, 0xffffffff);
+	I915_WRITE(IER, 0x0);
+
+	I915_WRITE(IIR, I915_READ(IIR));
+}
+
+static void i965_irq_preinstall(struct drm_device * dev)
+{
+	drm_i915_private_t *dev_priv = (drm_i915_private_t *) dev->dev_private;
+	int pipe;
+
+	atomic_set(&dev_priv->irq_received, 0);
 
 	if (I915_HAS_HOTPLUG(dev)) {
 		I915_WRITE(PORT_HOTPLUG_EN, 0);
@@ -2001,20 +2333,25 @@ static void i915_driver_irq_preinstall(struct drm_device * dev)
 	POSTING_READ(IER);
 }
 
-/*
- * Must be called after intel_modeset_init or hotplug interrupts won't be
- * enabled correctly.
- */
-static int i915_driver_irq_postinstall(struct drm_device *dev)
+static int i965_irq_postinstall(struct drm_device *dev)
 {
 	drm_i915_private_t *dev_priv = (drm_i915_private_t *) dev->dev_private;
-	u32 enable_mask = I915_INTERRUPT_ENABLE_FIX | I915_INTERRUPT_ENABLE_VAR;
+	u32 enable_mask;
 	u32 error_mask;
 
-	dev_priv->vblank_pipe = DRM_I915_VBLANK_PIPE_A | DRM_I915_VBLANK_PIPE_B;
-
 	/* Unmask the interrupts that we always want on. */
-	dev_priv->irq_mask = ~I915_INTERRUPT_ENABLE_FIX;
+	dev_priv->irq_mask = ~(I915_ASLE_INTERRUPT |
+			       I915_DISPLAY_PIPE_A_EVENT_INTERRUPT |
+			       I915_DISPLAY_PIPE_B_EVENT_INTERRUPT |
+			       I915_DISPLAY_PLANE_A_FLIP_PENDING_INTERRUPT |
+			       I915_DISPLAY_PLANE_B_FLIP_PENDING_INTERRUPT |
+			       I915_RENDER_COMMAND_PARSER_ERROR_INTERRUPT);
+
+	enable_mask = ~dev_priv->irq_mask;
+	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;
@@ -2081,31 +2418,124 @@ static int i915_driver_irq_postinstall(struct drm_device *dev)
 	return 0;
 }
 
-static void ironlake_irq_uninstall(struct drm_device *dev)
+static irqreturn_t i965_irq_handler(DRM_IRQ_ARGS)
 {
+	struct drm_device *dev = (struct drm_device *) arg;
 	drm_i915_private_t *dev_priv = (drm_i915_private_t *) dev->dev_private;
+	u32 iir, new_iir;
+	u32 pipe_stats[I915_MAX_PIPES];
+	unsigned long irqflags;
+	int irq_received;
+	int ret = IRQ_NONE, pipe;
 
-	if (!dev_priv)
-		return;
+	atomic_inc(&dev_priv->irq_received);
 
-	dev_priv->vblank_pipe = 0;
+	iir = I915_READ(IIR);
 
-	I915_WRITE(HWSTAM, 0xffffffff);
+	for (;;) {
+		bool blc_event = false;
 
-	I915_WRITE(DEIMR, 0xffffffff);
-	I915_WRITE(DEIER, 0x0);
-	I915_WRITE(DEIIR, I915_READ(DEIIR));
+		irq_received = iir != 0;
 
-	I915_WRITE(GTIMR, 0xffffffff);
-	I915_WRITE(GTIER, 0x0);
-	I915_WRITE(GTIIR, I915_READ(GTIIR));
+		/* Can't rely on pipestat interrupt bit in iir as it might
+		 * have been cleared after the pipestat interrupt was received.
+		 * It doesn't set the bit in iir again, but it still produces
+		 * interrupts (for non-MSI).
+		 */
+		spin_lock_irqsave(&dev_priv->irq_lock, irqflags);
+		if (iir & I915_RENDER_COMMAND_PARSER_ERROR_INTERRUPT)
+			i915_handle_error(dev, false);
 
-	I915_WRITE(SDEIMR, 0xffffffff);
-	I915_WRITE(SDEIER, 0x0);
-	I915_WRITE(SDEIIR, I915_READ(SDEIIR));
+		for_each_pipe(pipe) {
+			int reg = PIPESTAT(pipe);
+			pipe_stats[pipe] = I915_READ(reg);
+
+			/*
+			 * Clear the PIPE*STAT regs before the IIR
+			 */
+			if (pipe_stats[pipe] & 0x8000ffff) {
+				if (pipe_stats[pipe] & PIPE_FIFO_UNDERRUN_STATUS)
+					DRM_DEBUG_DRIVER("pipe %c underrun\n",
+							 pipe_name(pipe));
+				I915_WRITE(reg, pipe_stats[pipe]);
+				irq_received = 1;
+			}
+		}
+		spin_unlock_irqrestore(&dev_priv->irq_lock, irqflags);
+
+		if (!irq_received)
+			break;
+
+		ret = IRQ_HANDLED;
+
+		/* Consume port.  Then clear IIR or we'll miss events */
+		if ((I915_HAS_HOTPLUG(dev)) &&
+		    (iir & I915_DISPLAY_PORT_INTERRUPT)) {
+			u32 hotplug_status = I915_READ(PORT_HOTPLUG_STAT);
+
+			DRM_DEBUG_DRIVER("hotplug event received, stat 0x%08x\n",
+				  hotplug_status);
+			if (hotplug_status & dev_priv->hotplug_supported_mask)
+				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);
+		new_iir = I915_READ(IIR); /* Flush posted writes */
+
+		if (iir & I915_USER_INTERRUPT)
+			notify_ring(dev, &dev_priv->ring[RCS]);
+		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);
+			}
+
+			if (pipe_stats[pipe] & PIPE_LEGACY_BLC_EVENT_STATUS)
+				blc_event = true;
+		}
+
+
+		if (blc_event || (iir & I915_ASLE_INTERRUPT))
+			intel_opregion_asle_intr(dev);
+
+		/* With MSI, interrupts are only generated when iir
+		 * transitions from zero to nonzero.  If another bit got
+		 * set while we were handling the existing iir bits, then
+		 * we would never get another interrupt.
+		 *
+		 * This is fine on non-MSI as well, as if we hit this path
+		 * we avoid exiting the interrupt handler only to generate
+		 * another one.
+		 *
+		 * Note that for MSI this could cause a stray interrupt report
+		 * if an interrupt landed in the time between writing IIR and
+		 * the posting read.  This should be rare enough to never
+		 * trigger the 99% of 100,000 interrupts test for disabling
+		 * stray interrupts.
+		 */
+		iir = new_iir;
+	}
+
+	i915_update_dri1_breadcrumb(dev);
+
+	return ret;
 }
 
-static void i915_driver_irq_uninstall(struct drm_device * dev)
+static void i965_irq_uninstall(struct drm_device * dev)
 {
 	drm_i915_private_t *dev_priv = (drm_i915_private_t *) dev->dev_private;
 	int pipe;
@@ -2113,8 +2543,6 @@ static void i915_driver_irq_uninstall(struct drm_device * dev)
 	if (!dev_priv)
 		return;
 
-	dev_priv->vblank_pipe = 0;
-
 	if (I915_HAS_HOTPLUG(dev)) {
 		I915_WRITE(PORT_HOTPLUG_EN, 0);
 		I915_WRITE(PORT_HOTPLUG_STAT, I915_READ(PORT_HOTPLUG_STAT));
@@ -2134,9 +2562,15 @@ static void i915_driver_irq_uninstall(struct drm_device * dev)
 
 void intel_irq_init(struct drm_device *dev)
 {
+	struct drm_i915_private *dev_priv = dev->dev_private;
+
+	INIT_WORK(&dev_priv->hotplug_work, i915_hotplug_work_func);
+	INIT_WORK(&dev_priv->error_work, i915_error_work_func);
+	INIT_WORK(&dev_priv->rps_work, gen6_pm_rps_work);
+
 	dev->driver->get_vblank_counter = i915_get_vblank_counter;
 	dev->max_vblank_count = 0xffffff; /* only 24 bits of frame count */
-	if (IS_G4X(dev) || IS_GEN5(dev) || IS_GEN6(dev) || IS_IVYBRIDGE(dev)) {
+	if (IS_G4X(dev) || INTEL_INFO(dev)->gen >= 5) {
 		dev->max_vblank_count = 0xffffffff; /* full 32 bit counter */
 		dev->driver->get_vblank_counter = gm45_get_vblank_counter;
 	}
@@ -2147,7 +2581,14 @@ void intel_irq_init(struct drm_device *dev)
 		dev->driver->get_vblank_timestamp = NULL;
 	dev->driver->get_scanout_position = i915_get_crtc_scanoutpos;
 
-	if (IS_IVYBRIDGE(dev)) {
+	if (IS_VALLEYVIEW(dev)) {
+		dev->driver->irq_handler = valleyview_irq_handler;
+		dev->driver->irq_preinstall = valleyview_irq_preinstall;
+		dev->driver->irq_postinstall = valleyview_irq_postinstall;
+		dev->driver->irq_uninstall = valleyview_irq_uninstall;
+		dev->driver->enable_vblank = valleyview_enable_vblank;
+		dev->driver->disable_vblank = valleyview_disable_vblank;
+	} else if (IS_IVYBRIDGE(dev)) {
 		/* Share pre & uninstall handlers with ILK/SNB */
 		dev->driver->irq_handler = ivybridge_irq_handler;
 		dev->driver->irq_preinstall = ironlake_irq_preinstall;
@@ -2155,6 +2596,14 @@ 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;
+	} else if (IS_HASWELL(dev)) {
+		/* Share interrupts handling with IVB */
+		dev->driver->irq_handler = ivybridge_irq_handler;
+		dev->driver->irq_preinstall = ironlake_irq_preinstall;
+		dev->driver->irq_postinstall = ivybridge_irq_postinstall;
+		dev->driver->irq_uninstall = ironlake_irq_uninstall;
+		dev->driver->enable_vblank = ivybridge_enable_vblank;
+		dev->driver->disable_vblank = ivybridge_disable_vblank;
 	} else if (HAS_PCH_SPLIT(dev)) {
 		dev->driver->irq_handler = ironlake_irq_handler;
 		dev->driver->irq_preinstall = ironlake_irq_preinstall;
@@ -2163,10 +2612,25 @@ void intel_irq_init(struct drm_device *dev)
 		dev->driver->enable_vblank = ironlake_enable_vblank;
 		dev->driver->disable_vblank = ironlake_disable_vblank;
 	} else {
-		dev->driver->irq_preinstall = i915_driver_irq_preinstall;
-		dev->driver->irq_postinstall = i915_driver_irq_postinstall;
-		dev->driver->irq_uninstall = i915_driver_irq_uninstall;
-		dev->driver->irq_handler = i915_driver_irq_handler;
+		if (INTEL_INFO(dev)->gen == 2) {
+			dev->driver->irq_preinstall = i8xx_irq_preinstall;
+			dev->driver->irq_postinstall = i8xx_irq_postinstall;
+			dev->driver->irq_handler = i8xx_irq_handler;
+			dev->driver->irq_uninstall = i8xx_irq_uninstall;
+		} else if (INTEL_INFO(dev)->gen == 3) {
+			/* IIR "flip pending" means done if this bit is set */
+			I915_WRITE(ECOSKPD, _MASKED_BIT_DISABLE(ECO_FLIP_DONE));
+
+			dev->driver->irq_preinstall = i915_irq_preinstall;
+			dev->driver->irq_postinstall = i915_irq_postinstall;
+			dev->driver->irq_uninstall = i915_irq_uninstall;
+			dev->driver->irq_handler = i915_irq_handler;
+		} else {
+			dev->driver->irq_preinstall = i965_irq_preinstall;
+			dev->driver->irq_postinstall = i965_irq_postinstall;
+			dev->driver->irq_uninstall = i965_irq_uninstall;
+			dev->driver->irq_handler = i965_irq_handler;
+		}
 		dev->driver->enable_vblank = i915_enable_vblank;
 		dev->driver->disable_vblank = i915_disable_vblank;
 	}
diff --git a/drivers/gpu/drm/i915/i915_reg.h b/drivers/gpu/drm/i915/i915_reg.h
index 9d24d65f0c3..2d49b9507ed 100644
--- a/drivers/gpu/drm/i915/i915_reg.h
+++ b/drivers/gpu/drm/i915/i915_reg.h
@@ -27,6 +27,11 @@
 
 #define _PIPE(pipe, a, b) ((a) + (pipe)*((b)-(a)))
 
+#define _PORT(port, a, b) ((a) + (port)*((b)-(a)))
+
+#define _MASKED_BIT_ENABLE(a) (((a) << 16) | (a))
+#define _MASKED_BIT_DISABLE(a) ((a) << 16)
+
 /*
  * The Bridge device's PCI config space has information about the
  * fb aperture size and the amount of pre-reserved memory.
@@ -77,6 +82,7 @@
 #define  GRDOM_FULL	(0<<2)
 #define  GRDOM_RENDER	(1<<2)
 #define  GRDOM_MEDIA	(3<<2)
+#define  GRDOM_RESET_ENABLE (1<<0)
 
 #define GEN6_MBCUNIT_SNPCR	0x900c /* for LLC config */
 #define   GEN6_MBC_SNPCR_SHIFT	21
@@ -125,6 +131,13 @@
 #define   ECOCHK_PPGTT_CACHE64B		(0x3<<3)
 #define   ECOCHK_PPGTT_CACHE4B		(0x0<<3)
 
+#define GAC_ECO_BITS			0x14090
+#define   ECOBITS_PPGTT_CACHE64B	(3<<8)
+#define   ECOBITS_PPGTT_CACHE4B		(0<<8)
+
+#define GAB_CTL				0x24000
+#define   GAB_CTL_CONT_AFTER_PAGEFAULT	(1<<8)
+
 /* VGA stuff */
 
 #define VGA_ST01_MDA 0x3ba
@@ -222,6 +235,7 @@
 #define   MI_BATCH_NON_SECURE	(1)
 #define   MI_BATCH_NON_SECURE_I965 (1<<8)
 #define MI_BATCH_BUFFER_START	MI_INSTR(0x31, 0)
+#define   MI_BATCH_GTT		    (2<<6) /* aliased with (1<<7) on gen4 */
 #define MI_SEMAPHORE_MBOX	MI_INSTR(0x16, 1) /* gen6+ */
 #define  MI_SEMAPHORE_GLOBAL_GTT    (1<<22)
 #define  MI_SEMAPHORE_UPDATE	    (1<<21)
@@ -301,6 +315,61 @@
 #define  DEBUG_RESET_RENDER		(1<<8)
 #define  DEBUG_RESET_DISPLAY		(1<<9)
 
+/*
+ * DPIO - a special bus for various display related registers to hide behind:
+ *  0x800c: m1, m2, n, p1, p2, k dividers
+ *  0x8014: REF and SFR select
+ *  0x8014: N divider, VCO select
+ *  0x801c/3c: core clock bits
+ *  0x8048/68: low pass filter coefficients
+ *  0x8100: fast clock controls
+ */
+#define DPIO_PKT			0x2100
+#define  DPIO_RID			(0<<24)
+#define  DPIO_OP_WRITE			(1<<16)
+#define  DPIO_OP_READ			(0<<16)
+#define  DPIO_PORTID			(0x12<<8)
+#define  DPIO_BYTE			(0xf<<4)
+#define  DPIO_BUSY			(1<<0) /* status only */
+#define DPIO_DATA			0x2104
+#define DPIO_REG			0x2108
+#define DPIO_CTL			0x2110
+#define  DPIO_MODSEL1			(1<<3) /* if ref clk b == 27 */
+#define  DPIO_MODSEL0			(1<<2) /* if ref clk a == 27 */
+#define  DPIO_SFR_BYPASS		(1<<1)
+#define  DPIO_RESET			(1<<0)
+
+#define _DPIO_DIV_A			0x800c
+#define   DPIO_POST_DIV_SHIFT		(28) /* 3 bits */
+#define   DPIO_K_SHIFT			(24) /* 4 bits */
+#define   DPIO_P1_SHIFT			(21) /* 3 bits */
+#define   DPIO_P2_SHIFT			(16) /* 5 bits */
+#define   DPIO_N_SHIFT			(12) /* 4 bits */
+#define   DPIO_ENABLE_CALIBRATION	(1<<11)
+#define   DPIO_M1DIV_SHIFT		(8) /* 3 bits */
+#define   DPIO_M2DIV_MASK		0xff
+#define _DPIO_DIV_B			0x802c
+#define DPIO_DIV(pipe) _PIPE(pipe, _DPIO_DIV_A, _DPIO_DIV_B)
+
+#define _DPIO_REFSFR_A			0x8014
+#define   DPIO_REFSEL_OVERRIDE		27
+#define   DPIO_PLL_MODESEL_SHIFT	24 /* 3 bits */
+#define   DPIO_BIAS_CURRENT_CTL_SHIFT	21 /* 3 bits, always 0x7 */
+#define   DPIO_PLL_REFCLK_SEL_SHIFT	16 /* 2 bits */
+#define   DPIO_DRIVER_CTL_SHIFT		12 /* always set to 0x8 */
+#define   DPIO_CLK_BIAS_CTL_SHIFT	8 /* always set to 0x5 */
+#define _DPIO_REFSFR_B			0x8034
+#define DPIO_REFSFR(pipe) _PIPE(pipe, _DPIO_REFSFR_A, _DPIO_REFSFR_B)
+
+#define _DPIO_CORE_CLK_A		0x801c
+#define _DPIO_CORE_CLK_B		0x803c
+#define DPIO_CORE_CLK(pipe) _PIPE(pipe, _DPIO_CORE_CLK_A, _DPIO_CORE_CLK_B)
+
+#define _DPIO_LFP_COEFF_A		0x8048
+#define _DPIO_LFP_COEFF_B		0x8068
+#define DPIO_LFP_COEFF(pipe) _PIPE(pipe, _DPIO_LFP_COEFF_A, _DPIO_LFP_COEFF_B)
+
+#define DPIO_FASTCLK_DISABLE		0x8100
 
 /*
  * Fence registers
@@ -360,8 +429,6 @@
 #define ARB_MODE		0x04030
 #define   ARB_MODE_SWIZZLE_SNB	(1<<4)
 #define   ARB_MODE_SWIZZLE_IVB	(1<<5)
-#define   ARB_MODE_ENABLE(x)	GFX_MODE_ENABLE(x)
-#define   ARB_MODE_DISABLE(x)	GFX_MODE_DISABLE(x)
 #define RENDER_HWS_PGA_GEN7	(0x04080)
 #define RING_FAULT_REG(ring)	(0x4094 + 0x100*(ring)->id)
 #define DONE_REG		0x40b0
@@ -417,6 +484,7 @@
 #define INSTDONE	0x02090
 #define NOPID		0x02094
 #define HWSTAM		0x02098
+#define DMA_FADD_I8XX	0x020d0
 
 #define ERROR_GEN6	0x040a0
 
@@ -432,6 +500,7 @@
  */
 # define _3D_CHICKEN2_WM_READ_PIPELINED			(1 << 14)
 #define _3D_CHICKEN3	0x02090
+#define  _3D_CHICKEN_SF_DISABLE_FASTCLIP_CULL		(1 << 5)
 
 #define MI_MODE		0x0209c
 # define VS_TIMER_DISPATCH				(1 << 6)
@@ -447,14 +516,16 @@
 #define   GFX_PSMI_GRANULARITY		(1<<10)
 #define   GFX_PPGTT_ENABLE		(1<<9)
 
-#define GFX_MODE_ENABLE(bit) (((bit) << 16) | (bit))
-#define GFX_MODE_DISABLE(bit) (((bit) << 16) | (0))
-
 #define SCPD0		0x0209c /* 915+ only */
 #define IER		0x020a0
 #define IIR		0x020a4
 #define IMR		0x020a8
 #define ISR		0x020ac
+#define VLV_IIR_RW	0x182084
+#define VLV_IER		0x1820a0
+#define VLV_IIR		0x1820a4
+#define VLV_IMR		0x1820a8
+#define VLV_ISR		0x1820ac
 #define   I915_PIPE_CONTROL_NOTIFY_INTERRUPT		(1<<18)
 #define   I915_DISPLAY_PORT_INTERRUPT			(1<<17)
 #define   I915_RENDER_COMMAND_PARSER_ERROR_INTERRUPT	(1<<15)
@@ -500,7 +571,6 @@
 #define LM_BURST_LENGTH     0x00000700
 #define LM_FIFO_WATERMARK   0x0000001F
 #define MI_ARB_STATE	0x020e4 /* 915+ only */
-#define   MI_ARB_MASK_SHIFT	  16	/* shift for enable bits */
 
 /* Make render/texture TLB fetches lower priorty than associated data
  *   fetches. This is not turned on by default
@@ -565,7 +635,6 @@
 #define   MI_ARB_DISPLAY_PRIORITY_B_A		(1 << 0)	/* display B > display A */
 
 #define CACHE_MODE_0	0x02120 /* 915+ only */
-#define   CM0_MASK_SHIFT          16
 #define   CM0_IZ_OPT_DISABLE      (1<<6)
 #define   CM0_ZR_OPT_DISABLE      (1<<5)
 #define	  CM0_STC_EVICT_DISABLE_LRA_SNB	(1<<5)
@@ -579,7 +648,12 @@
 #define   ECO_GATING_CX_ONLY	(1<<3)
 #define   ECO_FLIP_DONE		(1<<0)
 
-/* GEN6 interrupt control */
+#define CACHE_MODE_1		0x7004 /* IVB+ */
+#define   PIXEL_SUBSPAN_COLLECT_OPT_DISABLE (1<<6)
+
+/* GEN6 interrupt control
+ * Note that the per-ring interrupt bits do alias with the global interrupt bits
+ * in GTIMR. */
 #define GEN6_RENDER_HWSTAM	0x2098
 #define GEN6_RENDER_IMR		0x20a8
 #define   GEN6_RENDER_CONTEXT_SWITCH_INTERRUPT		(1 << 8)
@@ -615,6 +689,21 @@
 
 #define GEN6_BSD_RNCID			0x12198
 
+#define GEN7_FF_THREAD_MODE		0x20a0
+#define   GEN7_FF_SCHED_MASK		0x0077070
+#define   GEN7_FF_TS_SCHED_HS1		(0x5<<16)
+#define   GEN7_FF_TS_SCHED_HS0		(0x3<<16)
+#define   GEN7_FF_TS_SCHED_LOAD_BALANCE	(0x1<<16)
+#define   GEN7_FF_TS_SCHED_HW		(0x0<<16) /* Default */
+#define   GEN7_FF_VS_SCHED_HS1		(0x5<<12)
+#define   GEN7_FF_VS_SCHED_HS0		(0x3<<12)
+#define   GEN7_FF_VS_SCHED_LOAD_BALANCE	(0x1<<12) /* Default */
+#define   GEN7_FF_VS_SCHED_HW		(0x0<<12)
+#define   GEN7_FF_DS_SCHED_HS1		(0x5<<4)
+#define   GEN7_FF_DS_SCHED_HS0		(0x3<<4)
+#define   GEN7_FF_DS_SCHED_LOAD_BALANCE	(0x1<<4)  /* Default */
+#define   GEN7_FF_DS_SCHED_HW		(0x0<<4)
+
 /*
  * Framebuffer compression (915+ only)
  */
@@ -743,9 +832,9 @@
 #define   GMBUS_PORT_PANEL	3
 #define   GMBUS_PORT_DPC	4 /* HDMIC */
 #define   GMBUS_PORT_DPB	5 /* SDVO, HDMIB */
-				  /* 6 reserved */
-#define   GMBUS_PORT_DPD	7 /* HDMID */
-#define   GMBUS_NUM_PORTS       8
+#define   GMBUS_PORT_DPD	6 /* HDMID */
+#define   GMBUS_PORT_RESERVED	7 /* 7 reserved */
+#define   GMBUS_NUM_PORTS	(GMBUS_PORT_DPD - GMBUS_PORT_SSC + 1)
 #define GMBUS1			0x5104 /* command/status */
 #define   GMBUS_SW_CLR_INT	(1<<31)
 #define   GMBUS_SW_RDY		(1<<30)
@@ -797,7 +886,9 @@
 #define DPLL(pipe) _PIPE(pipe, _DPLL_A, _DPLL_B)
 #define   DPLL_VCO_ENABLE		(1 << 31)
 #define   DPLL_DVO_HIGH_SPEED		(1 << 30)
+#define   DPLL_EXT_BUFFER_ENABLE_VLV	(1 << 30)
 #define   DPLL_SYNCLOCK_ENABLE		(1 << 29)
+#define   DPLL_REFA_CLK_ENABLE_VLV	(1 << 29)
 #define   DPLL_VGA_MODE_DIS		(1 << 28)
 #define   DPLLB_MODE_DAC_SERIAL		(1 << 26) /* i915 */
 #define   DPLLB_MODE_LVDS		(2 << 26) /* i915 */
@@ -809,6 +900,7 @@
 #define   DPLL_P2_CLOCK_DIV_MASK	0x03000000 /* i915 */
 #define   DPLL_FPA01_P1_POST_DIV_MASK	0x00ff0000 /* i915 */
 #define   DPLL_FPA01_P1_POST_DIV_MASK_PINEVIEW	0x00ff8000 /* Pineview */
+#define   DPLL_INTEGRATED_CLOCK_VLV	(1<<13)
 
 #define SRX_INDEX		0x3c4
 #define SRX_DATA		0x3c5
@@ -904,6 +996,7 @@
 #define   DPLL_MD_VGA_UDI_MULTIPLIER_SHIFT	0
 #define _DPLL_B_MD 0x06020 /* 965+ only */
 #define DPLL_MD(pipe) _PIPE(pipe, _DPLL_A_MD, _DPLL_B_MD)
+
 #define _FPA0	0x06040
 #define _FPA1	0x06044
 #define _FPB0	0x06048
@@ -1044,6 +1137,9 @@
 #define RAMCLK_GATE_D		0x6210		/* CRL only */
 #define DEUC			0x6214          /* CRL only */
 
+#define FW_BLC_SELF_VLV		0x6500
+#define  FW_CSPWRDWNEN		(1<<15)
+
 /*
  * Palette regs
  */
@@ -1601,9 +1697,12 @@
 /* Video Data Island Packet control */
 #define VIDEO_DIP_DATA		0x61178
 #define VIDEO_DIP_CTL		0x61170
+/* Pre HSW: */
 #define   VIDEO_DIP_ENABLE		(1 << 31)
 #define   VIDEO_DIP_PORT_B		(1 << 29)
 #define   VIDEO_DIP_PORT_C		(2 << 29)
+#define   VIDEO_DIP_PORT_D		(3 << 29)
+#define   VIDEO_DIP_PORT_MASK		(3 << 29)
 #define   VIDEO_DIP_ENABLE_AVI		(1 << 21)
 #define   VIDEO_DIP_ENABLE_VENDOR	(2 << 21)
 #define   VIDEO_DIP_ENABLE_SPD		(8 << 21)
@@ -1614,6 +1713,10 @@
 #define   VIDEO_DIP_FREQ_ONCE		(0 << 16)
 #define   VIDEO_DIP_FREQ_VSYNC		(1 << 16)
 #define   VIDEO_DIP_FREQ_2VSYNC		(2 << 16)
+#define   VIDEO_DIP_FREQ_MASK		(3 << 16)
+/* HSW and later: */
+#define   VIDEO_DIP_ENABLE_AVI_HSW	(1 << 12)
+#define   VIDEO_DIP_ENABLE_SPD_HSW	(1 << 0)
 
 /* Panel power sequencing */
 #define PP_STATUS	0x61200
@@ -2380,7 +2483,8 @@
 
 /* Pipe A */
 #define _PIPEADSL		0x70000
-#define   DSL_LINEMASK		0x00000fff
+#define   DSL_LINEMASK_GEN2	0x00000fff
+#define   DSL_LINEMASK_GEN3	0x00001fff
 #define _PIPEACONF		0x70008
 #define   PIPECONF_ENABLE	(1<<31)
 #define   PIPECONF_DISABLE	0
@@ -2422,23 +2526,30 @@
 #define   PIPECONF_DITHER_TYPE_TEMP (3<<2)
 #define _PIPEASTAT		0x70024
 #define   PIPE_FIFO_UNDERRUN_STATUS		(1UL<<31)
+#define   SPRITE1_FLIPDONE_INT_EN_VLV		(1UL<<30)
 #define   PIPE_CRC_ERROR_ENABLE			(1UL<<29)
 #define   PIPE_CRC_DONE_ENABLE			(1UL<<28)
 #define   PIPE_GMBUS_EVENT_ENABLE		(1UL<<27)
+#define   PLANE_FLIP_DONE_INT_EN_VLV		(1UL<<26)
 #define   PIPE_HOTPLUG_INTERRUPT_ENABLE		(1UL<<26)
 #define   PIPE_VSYNC_INTERRUPT_ENABLE		(1UL<<25)
 #define   PIPE_DISPLAY_LINE_COMPARE_ENABLE	(1UL<<24)
 #define   PIPE_DPST_EVENT_ENABLE		(1UL<<23)
+#define   SPRITE0_FLIP_DONE_INT_EN_VLV		(1UL<<26)
 #define   PIPE_LEGACY_BLC_EVENT_ENABLE		(1UL<<22)
 #define   PIPE_ODD_FIELD_INTERRUPT_ENABLE	(1UL<<21)
 #define   PIPE_EVEN_FIELD_INTERRUPT_ENABLE	(1UL<<20)
 #define   PIPE_HOTPLUG_TV_INTERRUPT_ENABLE	(1UL<<18) /* pre-965 */
 #define   PIPE_START_VBLANK_INTERRUPT_ENABLE	(1UL<<18) /* 965 or later */
 #define   PIPE_VBLANK_INTERRUPT_ENABLE		(1UL<<17)
+#define   PIPEA_HBLANK_INT_EN_VLV		(1UL<<16)
 #define   PIPE_OVERLAY_UPDATED_ENABLE		(1UL<<16)
+#define   SPRITE1_FLIPDONE_INT_STATUS_VLV	(1UL<<15)
+#define   SPRITE0_FLIPDONE_INT_STATUS_VLV	(1UL<<15)
 #define   PIPE_CRC_ERROR_INTERRUPT_STATUS	(1UL<<13)
 #define   PIPE_CRC_DONE_INTERRUPT_STATUS	(1UL<<12)
 #define   PIPE_GMBUS_INTERRUPT_STATUS		(1UL<<11)
+#define   PLANE_FLIPDONE_INT_STATUS_VLV		(1UL<<10)
 #define   PIPE_HOTPLUG_INTERRUPT_STATUS		(1UL<<10)
 #define   PIPE_VSYNC_INTERRUPT_STATUS		(1UL<<9)
 #define   PIPE_DISPLAY_LINE_COMPARE_STATUS	(1UL<<8)
@@ -2463,6 +2574,40 @@
 #define PIPEFRAMEPIXEL(pipe)  _PIPE(pipe, _PIPEAFRAMEPIXEL, _PIPEBFRAMEPIXEL)
 #define PIPESTAT(pipe) _PIPE(pipe, _PIPEASTAT, _PIPEBSTAT)
 
+#define VLV_DPFLIPSTAT				0x70028
+#define   PIPEB_LINE_COMPARE_STATUS		(1<<29)
+#define   PIPEB_HLINE_INT_EN			(1<<28)
+#define   PIPEB_VBLANK_INT_EN			(1<<27)
+#define   SPRITED_FLIPDONE_INT_EN		(1<<26)
+#define   SPRITEC_FLIPDONE_INT_EN		(1<<25)
+#define   PLANEB_FLIPDONE_INT_EN		(1<<24)
+#define   PIPEA_LINE_COMPARE_STATUS		(1<<21)
+#define   PIPEA_HLINE_INT_EN			(1<<20)
+#define   PIPEA_VBLANK_INT_EN			(1<<19)
+#define   SPRITEB_FLIPDONE_INT_EN		(1<<18)
+#define   SPRITEA_FLIPDONE_INT_EN		(1<<17)
+#define   PLANEA_FLIPDONE_INT_EN		(1<<16)
+
+#define DPINVGTT				0x7002c /* VLV only */
+#define   CURSORB_INVALID_GTT_INT_EN		(1<<23)
+#define   CURSORA_INVALID_GTT_INT_EN		(1<<22)
+#define   SPRITED_INVALID_GTT_INT_EN		(1<<21)
+#define   SPRITEC_INVALID_GTT_INT_EN		(1<<20)
+#define   PLANEB_INVALID_GTT_INT_EN		(1<<19)
+#define   SPRITEB_INVALID_GTT_INT_EN		(1<<18)
+#define   SPRITEA_INVALID_GTT_INT_EN		(1<<17)
+#define   PLANEA_INVALID_GTT_INT_EN		(1<<16)
+#define   DPINVGTT_EN_MASK			0xff0000
+#define   CURSORB_INVALID_GTT_STATUS		(1<<7)
+#define   CURSORA_INVALID_GTT_STATUS		(1<<6)
+#define   SPRITED_INVALID_GTT_STATUS		(1<<5)
+#define   SPRITEC_INVALID_GTT_STATUS		(1<<4)
+#define   PLANEB_INVALID_GTT_STATUS		(1<<3)
+#define   SPRITEB_INVALID_GTT_STATUS		(1<<2)
+#define   SPRITEA_INVALID_GTT_STATUS		(1<<1)
+#define   PLANEA_INVALID_GTT_STATUS		(1<<0)
+#define   DPINVGTT_STATUS_MASK			0xff
+
 #define DSPARB			0x70030
 #define   DSPARB_CSTART_MASK	(0x7f << 7)
 #define   DSPARB_CSTART_SHIFT	7
@@ -2492,11 +2637,28 @@
 #define   DSPFW_HPLL_CURSOR_MASK	(0x3f<<16)
 #define   DSPFW_HPLL_SR_MASK		(0x1ff)
 
+/* drain latency register values*/
+#define DRAIN_LATENCY_PRECISION_32	32
+#define DRAIN_LATENCY_PRECISION_16	16
+#define VLV_DDL1			0x70050
+#define DDL_CURSORA_PRECISION_32	(1<<31)
+#define DDL_CURSORA_PRECISION_16	(0<<31)
+#define DDL_CURSORA_SHIFT		24
+#define DDL_PLANEA_PRECISION_32		(1<<7)
+#define DDL_PLANEA_PRECISION_16		(0<<7)
+#define VLV_DDL2			0x70054
+#define DDL_CURSORB_PRECISION_32	(1<<31)
+#define DDL_CURSORB_PRECISION_16	(0<<31)
+#define DDL_CURSORB_SHIFT		24
+#define DDL_PLANEB_PRECISION_32		(1<<7)
+#define DDL_PLANEB_PRECISION_16		(0<<7)
+
 /* FIFO watermark sizes etc */
 #define G4X_FIFO_LINE_SIZE	64
 #define I915_FIFO_LINE_SIZE	64
 #define I830_FIFO_LINE_SIZE	32
 
+#define VALLEYVIEW_FIFO_SIZE	255
 #define G4X_FIFO_SIZE		127
 #define I965_FIFO_SIZE		512
 #define I945_FIFO_SIZE		127
@@ -2504,6 +2666,7 @@
 #define I855GM_FIFO_SIZE	127 /* In cachelines */
 #define I830_FIFO_SIZE		95
 
+#define VALLEYVIEW_MAX_WM	0xff
 #define G4X_MAX_WM		0x3f
 #define I915_MAX_WM		0x3f
 
@@ -2518,6 +2681,7 @@
 #define PINEVIEW_CURSOR_DFT_WM	0
 #define PINEVIEW_CURSOR_GUARD_WM	5
 
+#define VALLEYVIEW_CURSOR_MAX_WM 64
 #define I965_CURSOR_FIFO	64
 #define I965_CURSOR_MAX_WM	32
 #define I965_CURSOR_DFT_WM	8
@@ -2726,6 +2890,13 @@
 #define DSPSURF(plane) _PIPE(plane, _DSPASURF, _DSPBSURF)
 #define DSPTILEOFF(plane) _PIPE(plane, _DSPATILEOFF, _DSPBTILEOFF)
 
+/* Display/Sprite base address macros */
+#define DISP_BASEADDR_MASK	(0xfffff000)
+#define I915_LO_DISPBASE(val)	(val & ~DISP_BASEADDR_MASK)
+#define I915_HI_DISPBASE(val)	(val & DISP_BASEADDR_MASK)
+#define I915_MODIFY_DISPBASE(reg, gfx_addr) \
+		(I915_WRITE(reg, gfx_addr | I915_LO_DISPBASE(I915_READ(reg))))
+
 /* VBIOS flags */
 #define SWF00			0x71410
 #define SWF01			0x71414
@@ -3058,25 +3229,38 @@
 #define DE_PCH_EVENT_IVB		(1<<28)
 #define DE_DP_A_HOTPLUG_IVB		(1<<27)
 #define DE_AUX_CHANNEL_A_IVB		(1<<26)
+#define DE_SPRITEC_FLIP_DONE_IVB	(1<<14)
+#define DE_PLANEC_FLIP_DONE_IVB		(1<<13)
+#define DE_PIPEC_VBLANK_IVB		(1<<10)
 #define DE_SPRITEB_FLIP_DONE_IVB	(1<<9)
-#define DE_SPRITEA_FLIP_DONE_IVB	(1<<4)
 #define DE_PLANEB_FLIP_DONE_IVB		(1<<8)
-#define DE_PLANEA_FLIP_DONE_IVB		(1<<3)
 #define DE_PIPEB_VBLANK_IVB		(1<<5)
+#define DE_SPRITEA_FLIP_DONE_IVB	(1<<4)
+#define DE_PLANEA_FLIP_DONE_IVB		(1<<3)
 #define DE_PIPEA_VBLANK_IVB		(1<<0)
 
+#define VLV_MASTER_IER			0x4400c /* Gunit master IER */
+#define   MASTER_INTERRUPT_ENABLE	(1<<31)
+
 #define DEISR   0x44000
 #define DEIMR   0x44004
 #define DEIIR   0x44008
 #define DEIER   0x4400c
 
-/* GT interrupt */
-#define GT_PIPE_NOTIFY		(1 << 4)
-#define GT_SYNC_STATUS          (1 << 2)
-#define GT_USER_INTERRUPT       (1 << 0)
-#define GT_BSD_USER_INTERRUPT   (1 << 5)
-#define GT_GEN6_BSD_USER_INTERRUPT	(1 << 12)
-#define GT_BLT_USER_INTERRUPT	(1 << 22)
+/* GT interrupt.
+ * Note that for gen6+ the ring-specific interrupt bits do alias with the
+ * corresponding bits in the per-ring interrupt control registers. */
+#define GT_GEN6_BLT_FLUSHDW_NOTIFY_INTERRUPT	(1 << 26)
+#define GT_GEN6_BLT_CS_ERROR_INTERRUPT		(1 << 25)
+#define GT_GEN6_BLT_USER_INTERRUPT		(1 << 22)
+#define GT_GEN6_BSD_CS_ERROR_INTERRUPT		(1 << 15)
+#define GT_GEN6_BSD_USER_INTERRUPT		(1 << 12)
+#define GT_BSD_USER_INTERRUPT			(1 << 5) /* ilk only */
+#define GT_GEN7_L3_PARITY_ERROR_INTERRUPT	(1 << 5)
+#define GT_PIPE_NOTIFY				(1 << 4)
+#define GT_RENDER_CS_ERROR_INTERRUPT		(1 << 3)
+#define GT_SYNC_STATUS				(1 << 2)
+#define GT_USER_INTERRUPT			(1 << 0)
 
 #define GTISR   0x44010
 #define GTIMR   0x44014
@@ -3226,15 +3410,15 @@
 
 #define _PCH_DPLL_A              0xc6014
 #define _PCH_DPLL_B              0xc6018
-#define PCH_DPLL(pipe) (pipe == 0 ?  _PCH_DPLL_A : _PCH_DPLL_B)
+#define _PCH_DPLL(pll) (pll == 0 ? _PCH_DPLL_A : _PCH_DPLL_B)
 
 #define _PCH_FPA0                0xc6040
 #define  FP_CB_TUNE		(0x3<<22)
 #define _PCH_FPA1                0xc6044
 #define _PCH_FPB0                0xc6048
 #define _PCH_FPB1                0xc604c
-#define PCH_FP0(pipe) (pipe == 0 ? _PCH_FPA0 : _PCH_FPB0)
-#define PCH_FP1(pipe) (pipe == 0 ? _PCH_FPA1 : _PCH_FPB1)
+#define _PCH_FP0(pll) (pll == 0 ? _PCH_FPA0 : _PCH_FPB0)
+#define _PCH_FP1(pll) (pll == 0 ? _PCH_FPA1 : _PCH_FPB1)
 
 #define PCH_DPLL_TEST           0xc606c
 
@@ -3329,6 +3513,57 @@
 #define TVIDEO_DIP_DATA(pipe) _PIPE(pipe, _VIDEO_DIP_DATA_A, _VIDEO_DIP_DATA_B)
 #define TVIDEO_DIP_GCP(pipe) _PIPE(pipe, _VIDEO_DIP_GCP_A, _VIDEO_DIP_GCP_B)
 
+#define VLV_VIDEO_DIP_CTL_A		0x60220
+#define VLV_VIDEO_DIP_DATA_A		0x60208
+#define VLV_VIDEO_DIP_GDCP_PAYLOAD_A	0x60210
+
+#define VLV_VIDEO_DIP_CTL_B		0x61170
+#define VLV_VIDEO_DIP_DATA_B		0x61174
+#define VLV_VIDEO_DIP_GDCP_PAYLOAD_B	0x61178
+
+#define VLV_TVIDEO_DIP_CTL(pipe) \
+	 _PIPE(pipe, VLV_VIDEO_DIP_CTL_A, VLV_VIDEO_DIP_CTL_B)
+#define VLV_TVIDEO_DIP_DATA(pipe) \
+	 _PIPE(pipe, VLV_VIDEO_DIP_DATA_A, VLV_VIDEO_DIP_DATA_B)
+#define VLV_TVIDEO_DIP_GCP(pipe) \
+	_PIPE(pipe, VLV_VIDEO_DIP_GDCP_PAYLOAD_A, VLV_VIDEO_DIP_GDCP_PAYLOAD_B)
+
+/* Haswell DIP controls */
+#define HSW_VIDEO_DIP_CTL_A		0x60200
+#define HSW_VIDEO_DIP_AVI_DATA_A	0x60220
+#define HSW_VIDEO_DIP_VS_DATA_A		0x60260
+#define HSW_VIDEO_DIP_SPD_DATA_A	0x602A0
+#define HSW_VIDEO_DIP_GMP_DATA_A	0x602E0
+#define HSW_VIDEO_DIP_VSC_DATA_A	0x60320
+#define HSW_VIDEO_DIP_AVI_ECC_A		0x60240
+#define HSW_VIDEO_DIP_VS_ECC_A		0x60280
+#define HSW_VIDEO_DIP_SPD_ECC_A		0x602C0
+#define HSW_VIDEO_DIP_GMP_ECC_A		0x60300
+#define HSW_VIDEO_DIP_VSC_ECC_A		0x60344
+#define HSW_VIDEO_DIP_GCP_A		0x60210
+
+#define HSW_VIDEO_DIP_CTL_B		0x61200
+#define HSW_VIDEO_DIP_AVI_DATA_B	0x61220
+#define HSW_VIDEO_DIP_VS_DATA_B		0x61260
+#define HSW_VIDEO_DIP_SPD_DATA_B	0x612A0
+#define HSW_VIDEO_DIP_GMP_DATA_B	0x612E0
+#define HSW_VIDEO_DIP_VSC_DATA_B	0x61320
+#define HSW_VIDEO_DIP_BVI_ECC_B		0x61240
+#define HSW_VIDEO_DIP_VS_ECC_B		0x61280
+#define HSW_VIDEO_DIP_SPD_ECC_B		0x612C0
+#define HSW_VIDEO_DIP_GMP_ECC_B		0x61300
+#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 _TRANS_HTOTAL_B          0xe1000
 #define _TRANS_HBLANK_B          0xe1004
 #define _TRANS_HSYNC_B           0xe1008
@@ -3489,6 +3724,9 @@
 #define  FDI_LINK_TRAIN_PATTERN_IDLE_CPT	(2<<8)
 #define  FDI_LINK_TRAIN_NORMAL_CPT		(3<<8)
 #define  FDI_LINK_TRAIN_PATTERN_MASK_CPT	(3<<8)
+/* LPT */
+#define  FDI_PORT_WIDTH_2X_LPT			(1<<19)
+#define  FDI_PORT_WIDTH_1X_LPT			(0<<19)
 
 #define _FDI_RXA_MISC            0xf0010
 #define _FDI_RXB_MISC            0xf1010
@@ -3549,6 +3787,7 @@
 #define  ADPA_CRT_HOTPLUG_FORCE_TRIGGER (1<<16)
 
 /* or SDVOB */
+#define VLV_HDMIB 0x61140
 #define HDMIB   0xe1140
 #define  PORT_ENABLE    (1 << 31)
 #define  TRANSCODER(pipe)       ((pipe) << 30)
@@ -3714,6 +3953,8 @@
 #define  EDP_LINK_TRAIN_VOL_EMP_MASK_IVB	(0x3f<<22)
 
 #define  FORCEWAKE				0xA18C
+#define  FORCEWAKE_VLV				0x1300b0
+#define  FORCEWAKE_ACK_VLV			0x1300b4
 #define  FORCEWAKE_ACK				0x130090
 #define  FORCEWAKE_MT				0xa188 /* multi-threaded */
 #define  FORCEWAKE_MT_ACK			0x130040
@@ -3731,6 +3972,7 @@
 
 #define GEN6_UCGCTL1				0x9400
 # define GEN6_BLBUNIT_CLOCK_GATE_DISABLE		(1 << 5)
+# define GEN6_CSUNIT_CLOCK_GATE_DISABLE			(1 << 7)
 
 #define GEN6_UCGCTL2				0x9404
 # define GEN6_RCZUNIT_CLOCK_GATE_DISABLE		(1 << 13)
@@ -3811,6 +4053,11 @@
 						 GEN6_PM_RP_DOWN_THRESHOLD | \
 						 GEN6_PM_RP_DOWN_TIMEOUT)
 
+#define GEN6_GT_GFX_RC6_LOCKED			0x138104
+#define GEN6_GT_GFX_RC6				0x138108
+#define GEN6_GT_GFX_RC6p			0x13810C
+#define GEN6_GT_GFX_RC6pp			0x138110
+
 #define GEN6_PCODE_MAILBOX			0x138124
 #define   GEN6_PCODE_READY			(1<<31)
 #define   GEN6_READ_OC_PARAMS			0xc
@@ -3870,4 +4117,197 @@
 #define   AUD_CONFIG_PIXEL_CLOCK_HDMI		(0xf << 16)
 #define   AUD_CONFIG_DISABLE_NCTS		(1 << 3)
 
+/* HSW Power Wells */
+#define HSW_PWR_WELL_CTL1		0x45400		/* BIOS */
+#define HSW_PWR_WELL_CTL2		0x45404		/* Driver */
+#define HSW_PWR_WELL_CTL3		0x45408		/* KVMR */
+#define HSW_PWR_WELL_CTL4		0x4540C		/* Debug */
+#define   HSW_PWR_WELL_ENABLE				(1<<31)
+#define   HSW_PWR_WELL_STATE				(1<<30)
+#define HSW_PWR_WELL_CTL5		0x45410
+#define   HSW_PWR_WELL_ENABLE_SINGLE_STEP	(1<<31)
+#define   HSW_PWR_WELL_PWR_GATE_OVERRIDE	(1<<20)
+#define   HSW_PWR_WELL_FORCE_ON				(1<<19)
+#define HSW_PWR_WELL_CTL6		0x45414
+
+/* Per-pipe DDI Function Control */
+#define PIPE_DDI_FUNC_CTL_A			0x60400
+#define PIPE_DDI_FUNC_CTL_B			0x61400
+#define PIPE_DDI_FUNC_CTL_C			0x62400
+#define PIPE_DDI_FUNC_CTL_EDP		0x6F400
+#define DDI_FUNC_CTL(pipe) _PIPE(pipe, \
+					PIPE_DDI_FUNC_CTL_A, \
+					PIPE_DDI_FUNC_CTL_B)
+#define  PIPE_DDI_FUNC_ENABLE		(1<<31)
+/* Those bits are ignored by pipe EDP since it can only connect to DDI A */
+#define  PIPE_DDI_PORT_MASK				(0xf<<28)
+#define  PIPE_DDI_SELECT_PORT(x)		((x)<<28)
+#define  PIPE_DDI_MODE_SELECT_HDMI		(0<<24)
+#define  PIPE_DDI_MODE_SELECT_DVI		(1<<24)
+#define  PIPE_DDI_MODE_SELECT_DP_SST	(2<<24)
+#define  PIPE_DDI_MODE_SELECT_DP_MST	(3<<24)
+#define  PIPE_DDI_MODE_SELECT_FDI		(4<<24)
+#define  PIPE_DDI_BPC_8					(0<<20)
+#define  PIPE_DDI_BPC_10				(1<<20)
+#define  PIPE_DDI_BPC_6					(2<<20)
+#define  PIPE_DDI_BPC_12				(3<<20)
+#define  PIPE_DDI_BFI_ENABLE			(1<<4)
+#define  PIPE_DDI_PORT_WIDTH_X1			(0<<1)
+#define  PIPE_DDI_PORT_WIDTH_X2			(1<<1)
+#define  PIPE_DDI_PORT_WIDTH_X4			(3<<1)
+
+/* DisplayPort Transport Control */
+#define DP_TP_CTL_A			0x64040
+#define DP_TP_CTL_B			0x64140
+#define DP_TP_CTL(port) _PORT(port, \
+					DP_TP_CTL_A, \
+					DP_TP_CTL_B)
+#define  DP_TP_CTL_ENABLE		(1<<31)
+#define  DP_TP_CTL_MODE_SST	(0<<27)
+#define  DP_TP_CTL_MODE_MST	(1<<27)
+#define  DP_TP_CTL_ENHANCED_FRAME_ENABLE	(1<<18)
+#define  DP_TP_CTL_FDI_AUTOTRAIN	(1<<15)
+#define  DP_TP_CTL_LINK_TRAIN_MASK		(7<<8)
+#define  DP_TP_CTL_LINK_TRAIN_PAT1		(0<<8)
+#define  DP_TP_CTL_LINK_TRAIN_PAT2		(1<<8)
+#define  DP_TP_CTL_LINK_TRAIN_NORMAL	(3<<8)
+
+/* DisplayPort Transport Status */
+#define DP_TP_STATUS_A			0x64044
+#define DP_TP_STATUS_B			0x64144
+#define DP_TP_STATUS(port) _PORT(port, \
+					DP_TP_STATUS_A, \
+					DP_TP_STATUS_B)
+#define  DP_TP_STATUS_AUTOTRAIN_DONE	(1<<12)
+
+/* DDI Buffer Control */
+#define DDI_BUF_CTL_A				0x64000
+#define DDI_BUF_CTL_B				0x64100
+#define DDI_BUF_CTL(port) _PORT(port, \
+					DDI_BUF_CTL_A, \
+					DDI_BUF_CTL_B)
+#define  DDI_BUF_CTL_ENABLE				(1<<31)
+#define  DDI_BUF_EMP_400MV_0DB_HSW		(0<<24)   /* Sel0 */
+#define  DDI_BUF_EMP_400MV_3_5DB_HSW	(1<<24)   /* Sel1 */
+#define  DDI_BUF_EMP_400MV_6DB_HSW		(2<<24)   /* Sel2 */
+#define  DDI_BUF_EMP_400MV_9_5DB_HSW	(3<<24)   /* Sel3 */
+#define  DDI_BUF_EMP_600MV_0DB_HSW		(4<<24)   /* Sel4 */
+#define  DDI_BUF_EMP_600MV_3_5DB_HSW	(5<<24)   /* Sel5 */
+#define  DDI_BUF_EMP_600MV_6DB_HSW		(6<<24)   /* Sel6 */
+#define  DDI_BUF_EMP_800MV_0DB_HSW		(7<<24)   /* Sel7 */
+#define  DDI_BUF_EMP_800MV_3_5DB_HSW	(8<<24)   /* Sel8 */
+#define  DDI_BUF_EMP_MASK				(0xf<<24)
+#define  DDI_BUF_IS_IDLE				(1<<7)
+#define  DDI_PORT_WIDTH_X1				(0<<1)
+#define  DDI_PORT_WIDTH_X2				(1<<1)
+#define  DDI_PORT_WIDTH_X4				(3<<1)
+#define  DDI_INIT_DISPLAY_DETECTED		(1<<0)
+
+/* DDI Buffer Translations */
+#define DDI_BUF_TRANS_A				0x64E00
+#define DDI_BUF_TRANS_B				0x64E60
+#define DDI_BUF_TRANS(port) _PORT(port, \
+					DDI_BUF_TRANS_A, \
+					DDI_BUF_TRANS_B)
+
+/* Sideband Interface (SBI) is programmed indirectly, via
+ * SBI_ADDR, which contains the register offset; and SBI_DATA,
+ * which contains the payload */
+#define SBI_ADDR				0xC6000
+#define SBI_DATA				0xC6004
+#define SBI_CTL_STAT			0xC6008
+#define  SBI_CTL_OP_CRRD		(0x6<<8)
+#define  SBI_CTL_OP_CRWR		(0x7<<8)
+#define  SBI_RESPONSE_FAIL		(0x1<<1)
+#define  SBI_RESPONSE_SUCCESS	(0x0<<1)
+#define  SBI_BUSY				(0x1<<0)
+#define  SBI_READY				(0x0<<0)
+
+/* SBI offsets */
+#define  SBI_SSCDIVINTPHASE6		0x0600
+#define   SBI_SSCDIVINTPHASE_DIVSEL_MASK	((0x7f)<<1)
+#define   SBI_SSCDIVINTPHASE_DIVSEL(x)		((x)<<1)
+#define   SBI_SSCDIVINTPHASE_INCVAL_MASK	((0x7f)<<8)
+#define   SBI_SSCDIVINTPHASE_INCVAL(x)		((x)<<8)
+#define   SBI_SSCDIVINTPHASE_DIR(x)			((x)<<15)
+#define   SBI_SSCDIVINTPHASE_PROPAGATE		(1<<0)
+#define  SBI_SSCCTL					0x020c
+#define  SBI_SSCCTL6				0x060C
+#define   SBI_SSCCTL_DISABLE		(1<<0)
+#define  SBI_SSCAUXDIV6				0x0610
+#define   SBI_SSCAUXDIV_FINALDIV2SEL(x)		((x)<<4)
+#define  SBI_DBUFF0					0x2a00
+
+/* LPT PIXCLK_GATE */
+#define PIXCLK_GATE				0xC6020
+#define  PIXCLK_GATE_UNGATE		1<<0
+#define  PIXCLK_GATE_GATE		0<<0
+
+/* SPLL */
+#define SPLL_CTL				0x46020
+#define  SPLL_PLL_ENABLE		(1<<31)
+#define  SPLL_PLL_SCC			(1<<28)
+#define  SPLL_PLL_NON_SCC		(2<<28)
+#define  SPLL_PLL_FREQ_810MHz	(0<<26)
+#define  SPLL_PLL_FREQ_1350MHz	(1<<26)
+
+/* WRPLL */
+#define WRPLL_CTL1				0x46040
+#define WRPLL_CTL2				0x46060
+#define  WRPLL_PLL_ENABLE				(1<<31)
+#define  WRPLL_PLL_SELECT_SSC			(0x01<<28)
+#define  WRPLL_PLL_SELECT_NON_SCC		(0x02<<28)
+#define  WRPLL_PLL_SELECT_LCPLL_2700	(0x03<<28)
+/* WRPLL divider programming */
+#define  WRPLL_DIVIDER_REFERENCE(x)		((x)<<0)
+#define  WRPLL_DIVIDER_POST(x)			((x)<<8)
+#define  WRPLL_DIVIDER_FEEDBACK(x)		((x)<<16)
+
+/* Port clock selection */
+#define PORT_CLK_SEL_A			0x46100
+#define PORT_CLK_SEL_B			0x46104
+#define PORT_CLK_SEL(port) _PORT(port, \
+					PORT_CLK_SEL_A, \
+					PORT_CLK_SEL_B)
+#define  PORT_CLK_SEL_LCPLL_2700	(0<<29)
+#define  PORT_CLK_SEL_LCPLL_1350	(1<<29)
+#define  PORT_CLK_SEL_LCPLL_810		(2<<29)
+#define  PORT_CLK_SEL_SPLL			(3<<29)
+#define  PORT_CLK_SEL_WRPLL1		(4<<29)
+#define  PORT_CLK_SEL_WRPLL2		(5<<29)
+
+/* Pipe clock selection */
+#define PIPE_CLK_SEL_A			0x46140
+#define PIPE_CLK_SEL_B			0x46144
+#define PIPE_CLK_SEL(pipe) _PIPE(pipe, \
+					PIPE_CLK_SEL_A, \
+					PIPE_CLK_SEL_B)
+/* For each pipe, we need to select the corresponding port clock */
+#define  PIPE_CLK_SEL_DISABLED	(0x0<<29)
+#define  PIPE_CLK_SEL_PORT(x)	((x+1)<<29)
+
+/* LCPLL Control */
+#define LCPLL_CTL				0x130040
+#define  LCPLL_PLL_DISABLE		(1<<31)
+#define  LCPLL_PLL_LOCK			(1<<30)
+#define  LCPLL_CD_CLOCK_DISABLE	(1<<25)
+#define  LCPLL_CD2X_CLOCK_DISABLE	(1<<23)
+
+/* Pipe WM_LINETIME - watermark line time */
+#define PIPE_WM_LINETIME_A		0x45270
+#define PIPE_WM_LINETIME_B		0x45274
+#define PIPE_WM_LINETIME(pipe) _PIPE(pipe, \
+					PIPE_WM_LINETIME_A, \
+					PIPE_WM_LINETIME_A)
+#define   PIPE_WM_LINETIME_MASK		(0x1ff)
+#define   PIPE_WM_LINETIME_TIME(x)			((x))
+#define   PIPE_WM_LINETIME_IPS_LINETIME_MASK	(0x1ff<<16)
+#define   PIPE_WM_LINETIME_IPS_LINETIME(x)		((x)<<16)
+
+/* SFUSE_STRAP */
+#define SFUSE_STRAP				0xc2014
+#define  SFUSE_STRAP_DDIB_DETECTED	(1<<2)
+#define  SFUSE_STRAP_DDIC_DETECTED	(1<<1)
+#define  SFUSE_STRAP_DDID_DETECTED	(1<<0)
+
 #endif /* _I915_REG_H_ */
diff --git a/drivers/gpu/drm/i915/i915_suspend.c b/drivers/gpu/drm/i915/i915_suspend.c
index 2b5eb229ff2..0ede02a99d9 100644
--- a/drivers/gpu/drm/i915/i915_suspend.c
+++ b/drivers/gpu/drm/i915/i915_suspend.c
@@ -40,7 +40,7 @@ static bool i915_pipe_enabled(struct drm_device *dev, enum pipe pipe)
 		return false;
 
 	if (HAS_PCH_SPLIT(dev))
-		dpll_reg = PCH_DPLL(pipe);
+		dpll_reg = _PCH_DPLL(pipe);
 	else
 		dpll_reg = (pipe == PIPE_A) ? _DPLL_A : _DPLL_B;
 
@@ -876,22 +876,6 @@ int i915_restore_state(struct drm_device *dev)
 		I915_WRITE(IER, dev_priv->saveIER);
 		I915_WRITE(IMR, dev_priv->saveIMR);
 	}
-	mutex_unlock(&dev->struct_mutex);
-
-	if (drm_core_check_feature(dev, DRIVER_MODESET))
-		intel_init_clock_gating(dev);
-
-	if (IS_IRONLAKE_M(dev)) {
-		ironlake_enable_drps(dev);
-		intel_init_emon(dev);
-	}
-
-	if (INTEL_INFO(dev)->gen >= 6) {
-		gen6_enable_rps(dev_priv);
-		gen6_update_ring_freq(dev_priv);
-	}
-
-	mutex_lock(&dev->struct_mutex);
 
 	/* Cache mode state */
 	I915_WRITE(CACHE_MODE_0, dev_priv->saveCACHE_MODE_0 | 0xffff0000);
diff --git a/drivers/gpu/drm/i915/i915_sysfs.c b/drivers/gpu/drm/i915/i915_sysfs.c
new file mode 100644
index 00000000000..79f83445afa
--- /dev/null
+++ b/drivers/gpu/drm/i915/i915_sysfs.c
@@ -0,0 +1,111 @@
+/*
+ * Copyright © 2012 Intel Corporation
+ *
+ * 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:
+ *    Ben Widawsky <ben@bwidawsk.net>
+ *
+ */
+
+#include <linux/device.h>
+#include <linux/module.h>
+#include <linux/stat.h>
+#include <linux/sysfs.h>
+#include "i915_drv.h"
+
+static u32 calc_residency(struct drm_device *dev, const u32 reg)
+{
+	struct drm_i915_private *dev_priv = dev->dev_private;
+	u64 raw_time; /* 32b value may overflow during fixed point math */
+
+	if (!intel_enable_rc6(dev))
+		return 0;
+
+	raw_time = I915_READ(reg) * 128ULL;
+	return DIV_ROUND_UP_ULL(raw_time, 100000);
+}
+
+static ssize_t
+show_rc6_mask(struct device *dev, struct device_attribute *attr, char *buf)
+{
+	struct drm_minor *dminor = container_of(dev, struct drm_minor, kdev);
+	return snprintf(buf, PAGE_SIZE, "%x", intel_enable_rc6(dminor->dev));
+}
+
+static ssize_t
+show_rc6_ms(struct device *dev, struct device_attribute *attr, char *buf)
+{
+	struct drm_minor *dminor = container_of(dev, struct drm_minor, kdev);
+	u32 rc6_residency = calc_residency(dminor->dev, GEN6_GT_GFX_RC6);
+	return snprintf(buf, PAGE_SIZE, "%u", rc6_residency);
+}
+
+static ssize_t
+show_rc6p_ms(struct device *dev, struct device_attribute *attr, char *buf)
+{
+	struct drm_minor *dminor = container_of(dev, struct drm_minor, kdev);
+	u32 rc6p_residency = calc_residency(dminor->dev, GEN6_GT_GFX_RC6p);
+	return snprintf(buf, PAGE_SIZE, "%u", rc6p_residency);
+}
+
+static ssize_t
+show_rc6pp_ms(struct device *dev, struct device_attribute *attr, char *buf)
+{
+	struct drm_minor *dminor = container_of(dev, struct drm_minor, kdev);
+	u32 rc6pp_residency = calc_residency(dminor->dev, GEN6_GT_GFX_RC6pp);
+	return snprintf(buf, PAGE_SIZE, "%u", rc6pp_residency);
+}
+
+static DEVICE_ATTR(rc6_enable, S_IRUGO, show_rc6_mask, NULL);
+static DEVICE_ATTR(rc6_residency_ms, S_IRUGO, show_rc6_ms, NULL);
+static DEVICE_ATTR(rc6p_residency_ms, S_IRUGO, show_rc6p_ms, NULL);
+static DEVICE_ATTR(rc6pp_residency_ms, S_IRUGO, show_rc6pp_ms, NULL);
+
+static struct attribute *rc6_attrs[] = {
+	&dev_attr_rc6_enable.attr,
+	&dev_attr_rc6_residency_ms.attr,
+	&dev_attr_rc6p_residency_ms.attr,
+	&dev_attr_rc6pp_residency_ms.attr,
+	NULL
+};
+
+static struct attribute_group rc6_attr_group = {
+	.name = power_group_name,
+	.attrs =  rc6_attrs
+};
+
+void i915_setup_sysfs(struct drm_device *dev)
+{
+	int ret;
+
+	/* ILK doesn't have any residency information */
+	if (INTEL_INFO(dev)->gen < 6)
+		return;
+
+	ret = sysfs_merge_group(&dev->primary->kdev.kobj, &rc6_attr_group);
+	if (ret)
+		DRM_ERROR("sysfs setup failed\n");
+}
+
+void i915_teardown_sysfs(struct drm_device *dev)
+{
+	sysfs_unmerge_group(&dev->primary->kdev.kobj, &rc6_attr_group);
+}
diff --git a/drivers/gpu/drm/i915/i915_trace_points.c b/drivers/gpu/drm/i915/i915_trace_points.c
index ead876eb6ea..f1df2bd4ecf 100644
--- a/drivers/gpu/drm/i915/i915_trace_points.c
+++ b/drivers/gpu/drm/i915/i915_trace_points.c
@@ -7,5 +7,7 @@
 
 #include "i915_drv.h"
 
+#ifndef __CHECKER__
 #define CREATE_TRACE_POINTS
 #include "i915_trace.h"
+#endif
diff --git a/drivers/gpu/drm/i915/intel_acpi.c b/drivers/gpu/drm/i915/intel_acpi.c
index bae3edf956a..f413899475e 100644
--- a/drivers/gpu/drm/i915/intel_acpi.c
+++ b/drivers/gpu/drm/i915/intel_acpi.c
@@ -9,6 +9,7 @@
 #include <acpi/acpi_drivers.h>
 
 #include "drmP.h"
+#include "i915_drv.h"
 
 #define INTEL_DSM_REVISION_ID 1 /* For Calpella anyway... */
 
@@ -182,8 +183,6 @@ static void intel_dsm_platform_mux_info(void)
 			DRM_DEBUG_DRIVER("  hpd mux info: %s\n",
 			       intel_dsm_mux_type(info->buffer.pointer[3]));
 		}
-	} else {
-		DRM_ERROR("MUX INFO call failed\n");
 	}
 
 out:
diff --git a/drivers/gpu/drm/i915/intel_bios.c b/drivers/gpu/drm/i915/intel_bios.c
index b48fc2a8410..353459362f6 100644
--- a/drivers/gpu/drm/i915/intel_bios.c
+++ b/drivers/gpu/drm/i915/intel_bios.c
@@ -174,6 +174,28 @@ get_lvds_dvo_timing(const struct bdb_lvds_lfp_data *lvds_lfp_data,
 	return (struct lvds_dvo_timing *)(entry + dvo_timing_offset);
 }
 
+/* get lvds_fp_timing entry
+ * this function may return NULL if the corresponding entry is invalid
+ */
+static const struct lvds_fp_timing *
+get_lvds_fp_timing(const struct bdb_header *bdb,
+		   const struct bdb_lvds_lfp_data *data,
+		   const struct bdb_lvds_lfp_data_ptrs *ptrs,
+		   int index)
+{
+	size_t data_ofs = (const u8 *)data - (const u8 *)bdb;
+	u16 data_size = ((const u16 *)data)[-1]; /* stored in header */
+	size_t ofs;
+
+	if (index >= ARRAY_SIZE(ptrs->ptr))
+		return NULL;
+	ofs = ptrs->ptr[index].fp_timing_offset;
+	if (ofs < data_ofs ||
+	    ofs + sizeof(struct lvds_fp_timing) > data_ofs + data_size)
+		return NULL;
+	return (const struct lvds_fp_timing *)((const u8 *)bdb + ofs);
+}
+
 /* Try to find integrated panel data */
 static void
 parse_lfp_panel_data(struct drm_i915_private *dev_priv,
@@ -183,6 +205,7 @@ parse_lfp_panel_data(struct drm_i915_private *dev_priv,
 	const struct bdb_lvds_lfp_data *lvds_lfp_data;
 	const struct bdb_lvds_lfp_data_ptrs *lvds_lfp_data_ptrs;
 	const struct lvds_dvo_timing *panel_dvo_timing;
+	const struct lvds_fp_timing *fp_timing;
 	struct drm_display_mode *panel_fixed_mode;
 	int i, downclock;
 
@@ -244,6 +267,19 @@ parse_lfp_panel_data(struct drm_i915_private *dev_priv,
 			      "Normal Clock %dKHz, downclock %dKHz\n",
 			      panel_fixed_mode->clock, 10*downclock);
 	}
+
+	fp_timing = get_lvds_fp_timing(bdb, lvds_lfp_data,
+				       lvds_lfp_data_ptrs,
+				       lvds_options->panel_type);
+	if (fp_timing) {
+		/* check the resolution, just to be sure */
+		if (fp_timing->x_res == panel_fixed_mode->hdisplay &&
+		    fp_timing->y_res == panel_fixed_mode->vdisplay) {
+			dev_priv->bios_lvds_val = fp_timing->lvds_reg_val;
+			DRM_DEBUG_KMS("VBT initial LVDS value %x\n",
+				      dev_priv->bios_lvds_val);
+		}
+	}
 }
 
 /* Try to find sdvo panel data */
@@ -256,6 +292,11 @@ parse_sdvo_panel_data(struct drm_i915_private *dev_priv,
 	int index;
 
 	index = i915_vbt_sdvo_panel_type;
+	if (index == -2) {
+		DRM_DEBUG_KMS("Ignore SDVO panel mode from BIOS VBT tables.\n");
+		return;
+	}
+
 	if (index == -1) {
 		struct bdb_sdvo_lvds_options *sdvo_lvds_options;
 
@@ -332,11 +373,11 @@ parse_general_definitions(struct drm_i915_private *dev_priv,
 		if (block_size >= sizeof(*general)) {
 			int bus_pin = general->crt_ddc_gmbus_pin;
 			DRM_DEBUG_KMS("crt_ddc_bus_pin: %d\n", bus_pin);
-			if (bus_pin >= 1 && bus_pin <= 6)
+			if (intel_gmbus_is_port_valid(bus_pin))
 				dev_priv->crt_ddc_pin = bus_pin;
 		} else {
 			DRM_DEBUG_KMS("BDB_GD too small (%d). Invalid.\n",
-				  block_size);
+				      block_size);
 		}
 	}
 }
diff --git a/drivers/gpu/drm/i915/intel_crt.c b/drivers/gpu/drm/i915/intel_crt.c
index 90b9793fd5d..75a70c46ef1 100644
--- a/drivers/gpu/drm/i915/intel_crt.c
+++ b/drivers/gpu/drm/i915/intel_crt.c
@@ -55,18 +55,36 @@ static struct intel_crt *intel_attached_crt(struct drm_connector *connector)
 			    struct intel_crt, base);
 }
 
-static void intel_crt_dpms(struct drm_encoder *encoder, int mode)
+static void pch_crt_dpms(struct drm_encoder *encoder, int mode)
 {
 	struct drm_device *dev = encoder->dev;
 	struct drm_i915_private *dev_priv = dev->dev_private;
-	u32 temp, reg;
+	u32 temp;
 
-	if (HAS_PCH_SPLIT(dev))
-		reg = PCH_ADPA;
-	else
-		reg = ADPA;
+	temp = I915_READ(PCH_ADPA);
+	temp &= ~ADPA_DAC_ENABLE;
+
+	switch (mode) {
+	case DRM_MODE_DPMS_ON:
+		temp |= ADPA_DAC_ENABLE;
+		break;
+	case DRM_MODE_DPMS_STANDBY:
+	case DRM_MODE_DPMS_SUSPEND:
+	case DRM_MODE_DPMS_OFF:
+		/* Just leave port enable cleared */
+		break;
+	}
+
+	I915_WRITE(PCH_ADPA, temp);
+}
 
-	temp = I915_READ(reg);
+static void gmch_crt_dpms(struct drm_encoder *encoder, int mode)
+{
+	struct drm_device *dev = encoder->dev;
+	struct drm_i915_private *dev_priv = dev->dev_private;
+	u32 temp;
+
+	temp = I915_READ(ADPA);
 	temp &= ~(ADPA_HSYNC_CNTL_DISABLE | ADPA_VSYNC_CNTL_DISABLE);
 	temp &= ~ADPA_DAC_ENABLE;
 
@@ -85,7 +103,7 @@ static void intel_crt_dpms(struct drm_encoder *encoder, int mode)
 		break;
 	}
 
-	I915_WRITE(reg, temp);
+	I915_WRITE(ADPA, temp);
 }
 
 static int intel_crt_mode_valid(struct drm_connector *connector,
@@ -278,9 +296,10 @@ static bool intel_crt_detect_ddc(struct drm_connector *connector)
 	if (intel_ddc_probe(&crt->base, dev_priv->crt_ddc_pin)) {
 		struct edid *edid;
 		bool is_digital = false;
+		struct i2c_adapter *i2c;
 
-		edid = drm_get_edid(connector,
-			&dev_priv->gmbus[dev_priv->crt_ddc_pin].adapter);
+		i2c = intel_gmbus_get_adapter(dev_priv, dev_priv->crt_ddc_pin);
+		edid = drm_get_edid(connector, i2c);
 		/*
 		 * This may be a DVI-I connector with a shared DDC
 		 * link between analog and digital outputs, so we
@@ -476,15 +495,16 @@ static int intel_crt_get_modes(struct drm_connector *connector)
 	struct drm_device *dev = connector->dev;
 	struct drm_i915_private *dev_priv = dev->dev_private;
 	int ret;
+	struct i2c_adapter *i2c;
 
-	ret = intel_ddc_get_modes(connector,
-				 &dev_priv->gmbus[dev_priv->crt_ddc_pin].adapter);
+	i2c = intel_gmbus_get_adapter(dev_priv, dev_priv->crt_ddc_pin);
+	ret = intel_ddc_get_modes(connector, i2c);
 	if (ret || !IS_G4X(dev))
 		return ret;
 
 	/* Try to probe digital port for output in DVI-I -> VGA mode. */
-	return intel_ddc_get_modes(connector,
-				   &dev_priv->gmbus[GMBUS_PORT_DPB].adapter);
+	i2c = intel_gmbus_get_adapter(dev_priv, GMBUS_PORT_DPB);
+	return intel_ddc_get_modes(connector, i2c);
 }
 
 static int intel_crt_set_property(struct drm_connector *connector,
@@ -507,12 +527,20 @@ static void intel_crt_reset(struct drm_connector *connector)
  * Routines for controlling stuff on the analog port
  */
 
-static const struct drm_encoder_helper_funcs intel_crt_helper_funcs = {
-	.dpms = intel_crt_dpms,
+static const struct drm_encoder_helper_funcs pch_encoder_funcs = {
 	.mode_fixup = intel_crt_mode_fixup,
 	.prepare = intel_encoder_prepare,
 	.commit = intel_encoder_commit,
 	.mode_set = intel_crt_mode_set,
+	.dpms = pch_crt_dpms,
+};
+
+static const struct drm_encoder_helper_funcs gmch_encoder_funcs = {
+	.mode_fixup = intel_crt_mode_fixup,
+	.prepare = intel_encoder_prepare,
+	.commit = intel_encoder_commit,
+	.mode_set = intel_crt_mode_set,
+	.dpms = gmch_crt_dpms,
 };
 
 static const struct drm_connector_funcs intel_crt_connector_funcs = {
@@ -536,7 +564,7 @@ static const struct drm_encoder_funcs intel_crt_enc_funcs = {
 
 static int __init intel_no_crt_dmi_callback(const struct dmi_system_id *id)
 {
-	DRM_DEBUG_KMS("Skipping CRT initialization for %s\n", id->ident);
+	DRM_INFO("Skipping CRT initialization for %s\n", id->ident);
 	return 1;
 }
 
@@ -558,6 +586,7 @@ void intel_crt_init(struct drm_device *dev)
 	struct intel_crt *crt;
 	struct intel_connector *intel_connector;
 	struct drm_i915_private *dev_priv = dev->dev_private;
+	const struct drm_encoder_helper_funcs *encoder_helper_funcs;
 
 	/* Skip machines without VGA that falsely report hotplug events */
 	if (dmi_check_system(intel_no_crt))
@@ -586,14 +615,23 @@ void intel_crt_init(struct drm_device *dev)
 	crt->base.clone_mask = (1 << INTEL_SDVO_NON_TV_CLONE_BIT |
 				1 << INTEL_ANALOG_CLONE_BIT |
 				1 << INTEL_SDVO_LVDS_CLONE_BIT);
-	crt->base.crtc_mask = (1 << 0) | (1 << 1);
+	if (IS_HASWELL(dev))
+		crt->base.crtc_mask = (1 << 0);
+	else
+		crt->base.crtc_mask = (1 << 0) | (1 << 1);
+
 	if (IS_GEN2(dev))
 		connector->interlace_allowed = 0;
 	else
 		connector->interlace_allowed = 1;
 	connector->doublescan_allowed = 0;
 
-	drm_encoder_helper_add(&crt->base.base, &intel_crt_helper_funcs);
+	if (HAS_PCH_SPLIT(dev))
+		encoder_helper_funcs = &pch_encoder_funcs;
+	else
+		encoder_helper_funcs = &gmch_encoder_funcs;
+
+	drm_encoder_helper_add(&crt->base.base, encoder_helper_funcs);
 	drm_connector_helper_add(connector, &intel_crt_connector_helper_funcs);
 
 	drm_sysfs_connector_add(connector);
diff --git a/drivers/gpu/drm/i915/intel_ddi.c b/drivers/gpu/drm/i915/intel_ddi.c
new file mode 100644
index 00000000000..46d1e886c69
--- /dev/null
+++ b/drivers/gpu/drm/i915/intel_ddi.c
@@ -0,0 +1,755 @@
+/*
+ * Copyright © 2012 Intel Corporation
+ *
+ * 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:
+ *    Eugeni Dodonov <eugeni.dodonov@intel.com>
+ *
+ */
+
+#include "i915_drv.h"
+#include "intel_drv.h"
+
+/* HDMI/DVI modes ignore everything but the last 2 items. So we share
+ * them for both DP and FDI transports, allowing those ports to
+ * automatically adapt to HDMI connections as well
+ */
+static const u32 hsw_ddi_translations_dp[] = {
+	0x00FFFFFF, 0x0006000E,		/* DP parameters */
+	0x00D75FFF, 0x0005000A,
+	0x00C30FFF, 0x00040006,
+	0x80AAAFFF, 0x000B0000,
+	0x00FFFFFF, 0x0005000A,
+	0x00D75FFF, 0x000C0004,
+	0x80C30FFF, 0x000B0000,
+	0x00FFFFFF, 0x00040006,
+	0x80D75FFF, 0x000B0000,
+	0x00FFFFFF, 0x00040006		/* HDMI parameters */
+};
+
+static const u32 hsw_ddi_translations_fdi[] = {
+	0x00FFFFFF, 0x0007000E,		/* FDI parameters */
+	0x00D75FFF, 0x000F000A,
+	0x00C30FFF, 0x00060006,
+	0x00AAAFFF, 0x001E0000,
+	0x00FFFFFF, 0x000F000A,
+	0x00D75FFF, 0x00160004,
+	0x00C30FFF, 0x001E0000,
+	0x00FFFFFF, 0x00060006,
+	0x00D75FFF, 0x001E0000,
+	0x00FFFFFF, 0x00040006		/* HDMI parameters */
+};
+
+/* On Haswell, DDI port buffers must be programmed with correct values
+ * in advance. The buffer values are different for FDI and DP modes,
+ * but the HDMI/DVI fields are shared among those. So we program the DDI
+ * in either FDI or DP modes only, as HDMI connections will work with both
+ * of those
+ */
+void intel_prepare_ddi_buffers(struct drm_device *dev, enum port port, bool use_fdi_mode)
+{
+	struct drm_i915_private *dev_priv = dev->dev_private;
+	u32 reg;
+	int i;
+	const u32 *ddi_translations = ((use_fdi_mode) ?
+		hsw_ddi_translations_fdi :
+		hsw_ddi_translations_dp);
+
+	DRM_DEBUG_DRIVER("Initializing DDI buffers for port %c in %s mode\n",
+			port_name(port),
+			use_fdi_mode ? "FDI" : "DP");
+
+	WARN((use_fdi_mode && (port != PORT_E)),
+		"Programming port %c in FDI mode, this probably will not work.\n",
+		port_name(port));
+
+	for (i=0, reg=DDI_BUF_TRANS(port); i < ARRAY_SIZE(hsw_ddi_translations_fdi); i++) {
+		I915_WRITE(reg, ddi_translations[i]);
+		reg += 4;
+	}
+}
+
+/* Program DDI buffers translations for DP. By default, program ports A-D in DP
+ * mode and port E for FDI.
+ */
+void intel_prepare_ddi(struct drm_device *dev)
+{
+	int port;
+
+	if (IS_HASWELL(dev)) {
+		for (port = PORT_A; port < PORT_E; port++)
+			intel_prepare_ddi_buffers(dev, port, false);
+
+		/* DDI E is the suggested one to work in FDI mode, so program is as such by
+		 * default. It will have to be re-programmed in case a digital DP output
+		 * will be detected on it
+		 */
+		intel_prepare_ddi_buffers(dev, PORT_E, true);
+	}
+}
+
+static const long hsw_ddi_buf_ctl_values[] = {
+	DDI_BUF_EMP_400MV_0DB_HSW,
+	DDI_BUF_EMP_400MV_3_5DB_HSW,
+	DDI_BUF_EMP_400MV_6DB_HSW,
+	DDI_BUF_EMP_400MV_9_5DB_HSW,
+	DDI_BUF_EMP_600MV_0DB_HSW,
+	DDI_BUF_EMP_600MV_3_5DB_HSW,
+	DDI_BUF_EMP_600MV_6DB_HSW,
+	DDI_BUF_EMP_800MV_0DB_HSW,
+	DDI_BUF_EMP_800MV_3_5DB_HSW
+};
+
+
+/* Starting with Haswell, different DDI ports can work in FDI mode for
+ * connection to the PCH-located connectors. For this, it is necessary to train
+ * both the DDI port and PCH receiver for the desired DDI buffer settings.
+ *
+ * The recommended port to work in FDI mode is DDI E, which we use here. Also,
+ * please note that when FDI mode is active on DDI E, it shares 2 lines with
+ * DDI A (which is used for eDP)
+ */
+
+void hsw_fdi_link_train(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);
+	int pipe = intel_crtc->pipe;
+	u32 reg, temp, i;
+
+	/* Configure CPU PLL, wait for warmup */
+	I915_WRITE(SPLL_CTL,
+			SPLL_PLL_ENABLE |
+			SPLL_PLL_FREQ_1350MHz |
+			SPLL_PLL_SCC);
+
+	/* Use SPLL to drive the output when in FDI mode */
+	I915_WRITE(PORT_CLK_SEL(PORT_E),
+			PORT_CLK_SEL_SPLL);
+	I915_WRITE(PIPE_CLK_SEL(pipe),
+			PIPE_CLK_SEL_PORT(PORT_E));
+
+	udelay(20);
+
+	/* Start the training iterating through available voltages and emphasis */
+	for (i=0; i < ARRAY_SIZE(hsw_ddi_buf_ctl_values); i++) {
+		/* Configure DP_TP_CTL with auto-training */
+		I915_WRITE(DP_TP_CTL(PORT_E),
+					DP_TP_CTL_FDI_AUTOTRAIN |
+					DP_TP_CTL_ENHANCED_FRAME_ENABLE |
+					DP_TP_CTL_LINK_TRAIN_PAT1 |
+					DP_TP_CTL_ENABLE);
+
+		/* Configure and enable DDI_BUF_CTL for DDI E with next voltage */
+		temp = I915_READ(DDI_BUF_CTL(PORT_E));
+		temp = (temp & ~DDI_BUF_EMP_MASK);
+		I915_WRITE(DDI_BUF_CTL(PORT_E),
+				temp |
+				DDI_BUF_CTL_ENABLE |
+				DDI_PORT_WIDTH_X2 |
+				hsw_ddi_buf_ctl_values[i]);
+
+		udelay(600);
+
+		/* Enable CPU FDI Receiver with auto-training */
+		reg = FDI_RX_CTL(pipe);
+		I915_WRITE(reg,
+				I915_READ(reg) |
+					FDI_LINK_TRAIN_AUTO |
+					FDI_RX_ENABLE |
+					FDI_LINK_TRAIN_PATTERN_1_CPT |
+					FDI_RX_ENHANCE_FRAME_ENABLE |
+					FDI_PORT_WIDTH_2X_LPT |
+					FDI_RX_PLL_ENABLE);
+		POSTING_READ(reg);
+		udelay(100);
+
+		temp = I915_READ(DP_TP_STATUS(PORT_E));
+		if (temp & DP_TP_STATUS_AUTOTRAIN_DONE) {
+			DRM_DEBUG_DRIVER("BUF_CTL training done on %d step\n", i);
+
+			/* Enable normal pixel sending for FDI */
+			I915_WRITE(DP_TP_CTL(PORT_E),
+						DP_TP_CTL_FDI_AUTOTRAIN |
+						DP_TP_CTL_LINK_TRAIN_NORMAL |
+						DP_TP_CTL_ENHANCED_FRAME_ENABLE |
+						DP_TP_CTL_ENABLE);
+
+			/* Enable PIPE_DDI_FUNC_CTL for the pipe to work in FDI mode */
+			temp = I915_READ(DDI_FUNC_CTL(pipe));
+			temp &= ~PIPE_DDI_PORT_MASK;
+			temp |= PIPE_DDI_SELECT_PORT(PORT_E) |
+					PIPE_DDI_MODE_SELECT_FDI |
+					PIPE_DDI_FUNC_ENABLE |
+					PIPE_DDI_PORT_WIDTH_X2;
+			I915_WRITE(DDI_FUNC_CTL(pipe),
+					temp);
+			break;
+		} else {
+			DRM_ERROR("Error training BUF_CTL %d\n", i);
+
+			/* Disable DP_TP_CTL and FDI_RX_CTL) and retry */
+			I915_WRITE(DP_TP_CTL(PORT_E),
+					I915_READ(DP_TP_CTL(PORT_E)) &
+						~DP_TP_CTL_ENABLE);
+			I915_WRITE(FDI_RX_CTL(pipe),
+					I915_READ(FDI_RX_CTL(pipe)) &
+						~FDI_RX_PLL_ENABLE);
+			continue;
+		}
+	}
+
+	DRM_DEBUG_KMS("FDI train done.\n");
+}
+
+/* For DDI connections, it is possible to support different outputs over the
+ * same DDI port, such as HDMI or DP or even VGA via FDI. So we don't know by
+ * the time the output is detected what exactly is on the other end of it. This
+ * function aims at providing support for this detection and proper output
+ * configuration.
+ */
+void intel_ddi_init(struct drm_device *dev, enum port port)
+{
+	/* For now, we don't do any proper output detection and assume that we
+	 * handle HDMI only */
+
+	switch(port){
+	case PORT_A:
+		/* We don't handle eDP and DP yet */
+		DRM_DEBUG_DRIVER("Found digital output on DDI port A\n");
+		break;
+	/* Assume that the  ports B, C and D are working in HDMI mode for now */
+	case PORT_B:
+	case PORT_C:
+	case PORT_D:
+		intel_hdmi_init(dev, DDI_BUF_CTL(port));
+		break;
+	default:
+		DRM_DEBUG_DRIVER("No handlers defined for port %d, skipping DDI initialization\n",
+				port);
+		break;
+	}
+}
+
+/* WRPLL clock dividers */
+struct wrpll_tmds_clock {
+	u32 clock;
+	u16 p;		/* Post divider */
+	u16 n2;		/* Feedback divider */
+	u16 r2;		/* Reference divider */
+};
+
+/* Table of matching values for WRPLL clocks programming for each frequency */
+static const struct wrpll_tmds_clock wrpll_tmds_clock_table[] = {
+	{19750,	38,	25,	18},
+	{20000,	48,	32,	18},
+	{21000,	36,	21,	15},
+	{21912,	42,	29,	17},
+	{22000,	36,	22,	15},
+	{23000,	36,	23,	15},
+	{23500,	40,	40,	23},
+	{23750,	26,	16,	14},
+	{23750,	26,	16,	14},
+	{24000,	36,	24,	15},
+	{25000,	36,	25,	15},
+	{25175,	26,	40,	33},
+	{25200,	30,	21,	15},
+	{26000,	36,	26,	15},
+	{27000,	30,	21,	14},
+	{27027,	18,	100,	111},
+	{27500,	30,	29,	19},
+	{28000,	34,	30,	17},
+	{28320,	26,	30,	22},
+	{28322,	32,	42,	25},
+	{28750,	24,	23,	18},
+	{29000,	30,	29,	18},
+	{29750,	32,	30,	17},
+	{30000,	30,	25,	15},
+	{30750,	30,	41,	24},
+	{31000,	30,	31,	18},
+	{31500,	30,	28,	16},
+	{32000,	30,	32,	18},
+	{32500,	28,	32,	19},
+	{33000,	24,	22,	15},
+	{34000,	28,	30,	17},
+	{35000,	26,	32,	19},
+	{35500,	24,	30,	19},
+	{36000,	26,	26,	15},
+	{36750,	26,	46,	26},
+	{37000,	24,	23,	14},
+	{37762,	22,	40,	26},
+	{37800,	20,	21,	15},
+	{38000,	24,	27,	16},
+	{38250,	24,	34,	20},
+	{39000,	24,	26,	15},
+	{40000,	24,	32,	18},
+	{40500,	20,	21,	14},
+	{40541,	22,	147,	89},
+	{40750,	18,	19,	14},
+	{41000,	16,	17,	14},
+	{41500,	22,	44,	26},
+	{41540,	22,	44,	26},
+	{42000,	18,	21,	15},
+	{42500,	22,	45,	26},
+	{43000,	20,	43,	27},
+	{43163,	20,	24,	15},
+	{44000,	18,	22,	15},
+	{44900,	20,	108,	65},
+	{45000,	20,	25,	15},
+	{45250,	20,	52,	31},
+	{46000,	18,	23,	15},
+	{46750,	20,	45,	26},
+	{47000,	20,	40,	23},
+	{48000,	18,	24,	15},
+	{49000,	18,	49,	30},
+	{49500,	16,	22,	15},
+	{50000,	18,	25,	15},
+	{50500,	18,	32,	19},
+	{51000,	18,	34,	20},
+	{52000,	18,	26,	15},
+	{52406,	14,	34,	25},
+	{53000,	16,	22,	14},
+	{54000,	16,	24,	15},
+	{54054,	16,	173,	108},
+	{54500,	14,	24,	17},
+	{55000,	12,	22,	18},
+	{56000,	14,	45,	31},
+	{56250,	16,	25,	15},
+	{56750,	14,	25,	17},
+	{57000,	16,	27,	16},
+	{58000,	16,	43,	25},
+	{58250,	16,	38,	22},
+	{58750,	16,	40,	23},
+	{59000,	14,	26,	17},
+	{59341,	14,	40,	26},
+	{59400,	16,	44,	25},
+	{60000,	16,	32,	18},
+	{60500,	12,	39,	29},
+	{61000,	14,	49,	31},
+	{62000,	14,	37,	23},
+	{62250,	14,	42,	26},
+	{63000,	12,	21,	15},
+	{63500,	14,	28,	17},
+	{64000,	12,	27,	19},
+	{65000,	14,	32,	19},
+	{65250,	12,	29,	20},
+	{65500,	12,	32,	22},
+	{66000,	12,	22,	15},
+	{66667,	14,	38,	22},
+	{66750,	10,	21,	17},
+	{67000,	14,	33,	19},
+	{67750,	14,	58,	33},
+	{68000,	14,	30,	17},
+	{68179,	14,	46,	26},
+	{68250,	14,	46,	26},
+	{69000,	12,	23,	15},
+	{70000,	12,	28,	18},
+	{71000,	12,	30,	19},
+	{72000,	12,	24,	15},
+	{73000,	10,	23,	17},
+	{74000,	12,	23,	14},
+	{74176,	8,	100,	91},
+	{74250,	10,	22,	16},
+	{74481,	12,	43,	26},
+	{74500,	10,	29,	21},
+	{75000,	12,	25,	15},
+	{75250,	10,	39,	28},
+	{76000,	12,	27,	16},
+	{77000,	12,	53,	31},
+	{78000,	12,	26,	15},
+	{78750,	12,	28,	16},
+	{79000,	10,	38,	26},
+	{79500,	10,	28,	19},
+	{80000,	12,	32,	18},
+	{81000,	10,	21,	14},
+	{81081,	6,	100,	111},
+	{81624,	8,	29,	24},
+	{82000,	8,	17,	14},
+	{83000,	10,	40,	26},
+	{83950,	10,	28,	18},
+	{84000,	10,	28,	18},
+	{84750,	6,	16,	17},
+	{85000,	6,	17,	18},
+	{85250,	10,	30,	19},
+	{85750,	10,	27,	17},
+	{86000,	10,	43,	27},
+	{87000,	10,	29,	18},
+	{88000,	10,	44,	27},
+	{88500,	10,	41,	25},
+	{89000,	10,	28,	17},
+	{89012,	6,	90,	91},
+	{89100,	10,	33,	20},
+	{90000,	10,	25,	15},
+	{91000,	10,	32,	19},
+	{92000,	10,	46,	27},
+	{93000,	10,	31,	18},
+	{94000,	10,	40,	23},
+	{94500,	10,	28,	16},
+	{95000,	10,	44,	25},
+	{95654,	10,	39,	22},
+	{95750,	10,	39,	22},
+	{96000,	10,	32,	18},
+	{97000,	8,	23,	16},
+	{97750,	8,	42,	29},
+	{98000,	8,	45,	31},
+	{99000,	8,	22,	15},
+	{99750,	8,	34,	23},
+	{100000,	6,	20,	18},
+	{100500,	6,	19,	17},
+	{101000,	6,	37,	33},
+	{101250,	8,	21,	14},
+	{102000,	6,	17,	15},
+	{102250,	6,	25,	22},
+	{103000,	8,	29,	19},
+	{104000,	8,	37,	24},
+	{105000,	8,	28,	18},
+	{106000,	8,	22,	14},
+	{107000,	8,	46,	29},
+	{107214,	8,	27,	17},
+	{108000,	8,	24,	15},
+	{108108,	8,	173,	108},
+	{109000,	6,	23,	19},
+	{109000,	6,	23,	19},
+	{110000,	6,	22,	18},
+	{110013,	6,	22,	18},
+	{110250,	8,	49,	30},
+	{110500,	8,	36,	22},
+	{111000,	8,	23,	14},
+	{111264,	8,	150,	91},
+	{111375,	8,	33,	20},
+	{112000,	8,	63,	38},
+	{112500,	8,	25,	15},
+	{113100,	8,	57,	34},
+	{113309,	8,	42,	25},
+	{114000,	8,	27,	16},
+	{115000,	6,	23,	18},
+	{116000,	8,	43,	25},
+	{117000,	8,	26,	15},
+	{117500,	8,	40,	23},
+	{118000,	6,	38,	29},
+	{119000,	8,	30,	17},
+	{119500,	8,	46,	26},
+	{119651,	8,	39,	22},
+	{120000,	8,	32,	18},
+	{121000,	6,	39,	29},
+	{121250,	6,	31,	23},
+	{121750,	6,	23,	17},
+	{122000,	6,	42,	31},
+	{122614,	6,	30,	22},
+	{123000,	6,	41,	30},
+	{123379,	6,	37,	27},
+	{124000,	6,	51,	37},
+	{125000,	6,	25,	18},
+	{125250,	4,	13,	14},
+	{125750,	4,	27,	29},
+	{126000,	6,	21,	15},
+	{127000,	6,	24,	17},
+	{127250,	6,	41,	29},
+	{128000,	6,	27,	19},
+	{129000,	6,	43,	30},
+	{129859,	4,	25,	26},
+	{130000,	6,	26,	18},
+	{130250,	6,	42,	29},
+	{131000,	6,	32,	22},
+	{131500,	6,	38,	26},
+	{131850,	6,	41,	28},
+	{132000,	6,	22,	15},
+	{132750,	6,	28,	19},
+	{133000,	6,	34,	23},
+	{133330,	6,	37,	25},
+	{134000,	6,	61,	41},
+	{135000,	6,	21,	14},
+	{135250,	6,	167,	111},
+	{136000,	6,	62,	41},
+	{137000,	6,	35,	23},
+	{138000,	6,	23,	15},
+	{138500,	6,	40,	26},
+	{138750,	6,	37,	24},
+	{139000,	6,	34,	22},
+	{139050,	6,	34,	22},
+	{139054,	6,	34,	22},
+	{140000,	6,	28,	18},
+	{141000,	6,	36,	23},
+	{141500,	6,	22,	14},
+	{142000,	6,	30,	19},
+	{143000,	6,	27,	17},
+	{143472,	4,	17,	16},
+	{144000,	6,	24,	15},
+	{145000,	6,	29,	18},
+	{146000,	6,	47,	29},
+	{146250,	6,	26,	16},
+	{147000,	6,	49,	30},
+	{147891,	6,	23,	14},
+	{148000,	6,	23,	14},
+	{148250,	6,	28,	17},
+	{148352,	4,	100,	91},
+	{148500,	6,	33,	20},
+	{149000,	6,	48,	29},
+	{150000,	6,	25,	15},
+	{151000,	4,	19,	17},
+	{152000,	6,	27,	16},
+	{152280,	6,	44,	26},
+	{153000,	6,	34,	20},
+	{154000,	6,	53,	31},
+	{155000,	6,	31,	18},
+	{155250,	6,	50,	29},
+	{155750,	6,	45,	26},
+	{156000,	6,	26,	15},
+	{157000,	6,	61,	35},
+	{157500,	6,	28,	16},
+	{158000,	6,	65,	37},
+	{158250,	6,	44,	25},
+	{159000,	6,	53,	30},
+	{159500,	6,	39,	22},
+	{160000,	6,	32,	18},
+	{161000,	4,	31,	26},
+	{162000,	4,	18,	15},
+	{162162,	4,	131,	109},
+	{162500,	4,	53,	44},
+	{163000,	4,	29,	24},
+	{164000,	4,	17,	14},
+	{165000,	4,	22,	18},
+	{166000,	4,	32,	26},
+	{167000,	4,	26,	21},
+	{168000,	4,	46,	37},
+	{169000,	4,	104,	83},
+	{169128,	4,	64,	51},
+	{169500,	4,	39,	31},
+	{170000,	4,	34,	27},
+	{171000,	4,	19,	15},
+	{172000,	4,	51,	40},
+	{172750,	4,	32,	25},
+	{172800,	4,	32,	25},
+	{173000,	4,	41,	32},
+	{174000,	4,	49,	38},
+	{174787,	4,	22,	17},
+	{175000,	4,	35,	27},
+	{176000,	4,	30,	23},
+	{177000,	4,	38,	29},
+	{178000,	4,	29,	22},
+	{178500,	4,	37,	28},
+	{179000,	4,	53,	40},
+	{179500,	4,	73,	55},
+	{180000,	4,	20,	15},
+	{181000,	4,	55,	41},
+	{182000,	4,	31,	23},
+	{183000,	4,	42,	31},
+	{184000,	4,	30,	22},
+	{184750,	4,	26,	19},
+	{185000,	4,	37,	27},
+	{186000,	4,	51,	37},
+	{187000,	4,	36,	26},
+	{188000,	4,	32,	23},
+	{189000,	4,	21,	15},
+	{190000,	4,	38,	27},
+	{190960,	4,	41,	29},
+	{191000,	4,	41,	29},
+	{192000,	4,	27,	19},
+	{192250,	4,	37,	26},
+	{193000,	4,	20,	14},
+	{193250,	4,	53,	37},
+	{194000,	4,	23,	16},
+	{194208,	4,	23,	16},
+	{195000,	4,	26,	18},
+	{196000,	4,	45,	31},
+	{197000,	4,	35,	24},
+	{197750,	4,	41,	28},
+	{198000,	4,	22,	15},
+	{198500,	4,	25,	17},
+	{199000,	4,	28,	19},
+	{200000,	4,	37,	25},
+	{201000,	4,	61,	41},
+	{202000,	4,	112,	75},
+	{202500,	4,	21,	14},
+	{203000,	4,	146,	97},
+	{204000,	4,	62,	41},
+	{204750,	4,	44,	29},
+	{205000,	4,	38,	25},
+	{206000,	4,	29,	19},
+	{207000,	4,	23,	15},
+	{207500,	4,	40,	26},
+	{208000,	4,	37,	24},
+	{208900,	4,	48,	31},
+	{209000,	4,	48,	31},
+	{209250,	4,	31,	20},
+	{210000,	4,	28,	18},
+	{211000,	4,	25,	16},
+	{212000,	4,	22,	14},
+	{213000,	4,	30,	19},
+	{213750,	4,	38,	24},
+	{214000,	4,	46,	29},
+	{214750,	4,	35,	22},
+	{215000,	4,	43,	27},
+	{216000,	4,	24,	15},
+	{217000,	4,	37,	23},
+	{218000,	4,	42,	26},
+	{218250,	4,	42,	26},
+	{218750,	4,	34,	21},
+	{219000,	4,	47,	29},
+	{219000,	4,	47,	29},
+	{220000,	4,	44,	27},
+	{220640,	4,	49,	30},
+	{220750,	4,	36,	22},
+	{221000,	4,	36,	22},
+	{222000,	4,	23,	14},
+	{222525,	4,	28,	17},
+	{222750,	4,	33,	20},
+	{227000,	4,	37,	22},
+	{230250,	4,	29,	17},
+	{233500,	4,	38,	22},
+	{235000,	4,	40,	23},
+	{238000,	4,	30,	17},
+	{241500,	2,	17,	19},
+	{245250,	2,	20,	22},
+	{247750,	2,	22,	24},
+	{253250,	2,	15,	16},
+	{256250,	2,	18,	19},
+	{262500,	2,	31,	32},
+	{267250,	2,	66,	67},
+	{268500,	2,	94,	95},
+	{270000,	2,	14,	14},
+	{272500,	2,	77,	76},
+	{273750,	2,	57,	56},
+	{280750,	2,	24,	23},
+	{281250,	2,	23,	22},
+	{286000,	2,	17,	16},
+	{291750,	2,	26,	24},
+	{296703,	2,	56,	51},
+	{297000,	2,	22,	20},
+	{298000,	2,	21,	19},
+};
+
+void intel_ddi_mode_set(struct drm_encoder *encoder,
+				struct drm_display_mode *mode,
+				struct drm_display_mode *adjusted_mode)
+{
+	struct drm_device *dev = encoder->dev;
+	struct drm_i915_private *dev_priv = dev->dev_private;
+	struct drm_crtc *crtc = encoder->crtc;
+	struct intel_crtc *intel_crtc = to_intel_crtc(crtc);
+	struct intel_hdmi *intel_hdmi = enc_to_intel_hdmi(encoder);
+	int port = intel_hdmi->ddi_port;
+	int pipe = intel_crtc->pipe;
+	int p, n2, r2, valid=0;
+	u32 temp, i;
+
+	/* On Haswell, we need to enable the clocks and prepare DDI function to
+	 * work in HDMI mode for this pipe.
+	 */
+	DRM_DEBUG_KMS("Preparing HDMI DDI mode for Haswell on port %c, pipe %c\n", port_name(port), pipe_name(pipe));
+
+	for (i=0; i < ARRAY_SIZE(wrpll_tmds_clock_table); i++) {
+		if (crtc->mode.clock == wrpll_tmds_clock_table[i].clock) {
+			p = wrpll_tmds_clock_table[i].p;
+			n2 = wrpll_tmds_clock_table[i].n2;
+			r2 = wrpll_tmds_clock_table[i].r2;
+
+			DRM_DEBUG_KMS("WR PLL clock: found settings for %dKHz refresh rate: p=%d, n2=%d, r2=%d\n",
+					crtc->mode.clock,
+					p, n2, r2);
+
+			valid = 1;
+			break;
+		}
+	}
+
+	if (!valid) {
+		DRM_ERROR("Unable to find WR PLL clock settings for %dKHz refresh rate\n",
+				crtc->mode.clock);
+		return;
+	}
+
+	/* Enable LCPLL if disabled */
+	temp = I915_READ(LCPLL_CTL);
+	if (temp & LCPLL_PLL_DISABLE)
+		I915_WRITE(LCPLL_CTL,
+				temp & ~LCPLL_PLL_DISABLE);
+
+	/* Configure WR PLL 1, program the correct divider values for
+	 * the desired frequency and wait for warmup */
+	I915_WRITE(WRPLL_CTL1,
+			WRPLL_PLL_ENABLE |
+			WRPLL_PLL_SELECT_LCPLL_2700 |
+			WRPLL_DIVIDER_REFERENCE(r2) |
+			WRPLL_DIVIDER_FEEDBACK(n2) |
+			WRPLL_DIVIDER_POST(p));
+
+	udelay(20);
+
+	/* Use WRPLL1 clock to drive the output to the port, and tell the pipe to use
+	 * this port for connection.
+	 */
+	I915_WRITE(PORT_CLK_SEL(port),
+			PORT_CLK_SEL_WRPLL1);
+	I915_WRITE(PIPE_CLK_SEL(pipe),
+			PIPE_CLK_SEL_PORT(port));
+
+	udelay(20);
+
+	if (intel_hdmi->has_audio) {
+		/* Proper support for digital audio needs a new logic and a new set
+		 * of registers, so we leave it for future patch bombing.
+		 */
+		DRM_DEBUG_DRIVER("HDMI audio on pipe %c not yet supported on DDI\n",
+				 pipe_name(intel_crtc->pipe));
+	}
+
+	/* Enable PIPE_DDI_FUNC_CTL for the pipe to work in HDMI mode */
+	temp = I915_READ(DDI_FUNC_CTL(pipe));
+	temp &= ~PIPE_DDI_PORT_MASK;
+	temp &= ~PIPE_DDI_BPC_12;
+	temp |= PIPE_DDI_SELECT_PORT(port) |
+			PIPE_DDI_MODE_SELECT_HDMI |
+			((intel_crtc->bpp > 24) ?
+				PIPE_DDI_BPC_12 :
+				PIPE_DDI_BPC_8) |
+			PIPE_DDI_FUNC_ENABLE;
+
+	I915_WRITE(DDI_FUNC_CTL(pipe), temp);
+
+	intel_hdmi_set_avi_infoframe(encoder, adjusted_mode);
+	intel_hdmi_set_spd_infoframe(encoder);
+}
+
+void intel_ddi_dpms(struct drm_encoder *encoder, int mode)
+{
+	struct drm_device *dev = encoder->dev;
+	struct drm_i915_private *dev_priv = dev->dev_private;
+	struct intel_hdmi *intel_hdmi = enc_to_intel_hdmi(encoder);
+	int port = intel_hdmi->ddi_port;
+	u32 temp;
+
+	temp = I915_READ(DDI_BUF_CTL(port));
+
+	if (mode != DRM_MODE_DPMS_ON) {
+		temp &= ~DDI_BUF_CTL_ENABLE;
+	} else {
+		temp |= DDI_BUF_CTL_ENABLE;
+	}
+
+	/* Enable DDI_BUF_CTL. In HDMI/DVI mode, the port width,
+	 * and swing/emphasis values are ignored so nothing special needs
+	 * to be done besides enabling the port.
+	 */
+	I915_WRITE(DDI_BUF_CTL(port),
+			temp);
+}
diff --git a/drivers/gpu/drm/i915/intel_display.c b/drivers/gpu/drm/i915/intel_display.c
index 1b1cf3b3ff5..ee61ad1e642 100644
--- a/drivers/gpu/drm/i915/intel_display.c
+++ b/drivers/gpu/drm/i915/intel_display.c
@@ -24,7 +24,7 @@
  *	Eric Anholt <eric@anholt.net>
  */
 
-#include <linux/cpufreq.h>
+#include <linux/dmi.h>
 #include <linux/module.h>
 #include <linux/input.h>
 #include <linux/i2c.h>
@@ -44,7 +44,6 @@
 #define HAS_eDP (intel_pipe_has_type(crtc, INTEL_OUTPUT_EDP))
 
 bool intel_pipe_has_type(struct drm_crtc *crtc, int type);
-static void intel_update_watermarks(struct drm_device *dev);
 static void intel_increase_pllclock(struct drm_crtc *crtc);
 static void intel_crtc_update_cursor(struct drm_crtc *crtc, bool on);
 
@@ -360,6 +359,88 @@ static const intel_limit_t intel_limits_ironlake_display_port = {
 	.find_pll = intel_find_pll_ironlake_dp,
 };
 
+u32 intel_dpio_read(struct drm_i915_private *dev_priv, int reg)
+{
+	unsigned long flags;
+	u32 val = 0;
+
+	spin_lock_irqsave(&dev_priv->dpio_lock, flags);
+	if (wait_for_atomic_us((I915_READ(DPIO_PKT) & DPIO_BUSY) == 0, 100)) {
+		DRM_ERROR("DPIO idle wait timed out\n");
+		goto out_unlock;
+	}
+
+	I915_WRITE(DPIO_REG, reg);
+	I915_WRITE(DPIO_PKT, DPIO_RID | DPIO_OP_READ | DPIO_PORTID |
+		   DPIO_BYTE);
+	if (wait_for_atomic_us((I915_READ(DPIO_PKT) & DPIO_BUSY) == 0, 100)) {
+		DRM_ERROR("DPIO read wait timed out\n");
+		goto out_unlock;
+	}
+	val = I915_READ(DPIO_DATA);
+
+out_unlock:
+	spin_unlock_irqrestore(&dev_priv->dpio_lock, flags);
+	return val;
+}
+
+static void vlv_init_dpio(struct drm_device *dev)
+{
+	struct drm_i915_private *dev_priv = dev->dev_private;
+
+	/* Reset the DPIO config */
+	I915_WRITE(DPIO_CTL, 0);
+	POSTING_READ(DPIO_CTL);
+	I915_WRITE(DPIO_CTL, 1);
+	POSTING_READ(DPIO_CTL);
+}
+
+static int intel_dual_link_lvds_callback(const struct dmi_system_id *id)
+{
+	DRM_INFO("Forcing lvds to dual link mode on %s\n", id->ident);
+	return 1;
+}
+
+static const struct dmi_system_id intel_dual_link_lvds[] = {
+	{
+		.callback = intel_dual_link_lvds_callback,
+		.ident = "Apple MacBook Pro (Core i5/i7 Series)",
+		.matches = {
+			DMI_MATCH(DMI_SYS_VENDOR, "Apple Inc."),
+			DMI_MATCH(DMI_PRODUCT_NAME, "MacBookPro8,2"),
+		},
+	},
+	{ }	/* terminating entry */
+};
+
+static bool is_dual_link_lvds(struct drm_i915_private *dev_priv,
+			      unsigned int reg)
+{
+	unsigned int val;
+
+	/* use the module option value if specified */
+	if (i915_lvds_channel_mode > 0)
+		return i915_lvds_channel_mode == 2;
+
+	if (dmi_check_system(intel_dual_link_lvds))
+		return true;
+
+	if (dev_priv->lvds_val)
+		val = dev_priv->lvds_val;
+	else {
+		/* BIOS should set the proper LVDS register value at boot, but
+		 * in reality, it doesn't set the value when the lid is closed;
+		 * we need to check "the value to be set" in VBT when LVDS
+		 * register is uninitialized.
+		 */
+		val = I915_READ(reg);
+		if (!(val & ~LVDS_DETECTED))
+			val = dev_priv->bios_lvds_val;
+		dev_priv->lvds_val = val;
+	}
+	return (val & LVDS_CLKB_POWER_MASK) == LVDS_CLKB_POWER_UP;
+}
+
 static const intel_limit_t *intel_ironlake_limit(struct drm_crtc *crtc,
 						int refclk)
 {
@@ -368,8 +449,7 @@ static const intel_limit_t *intel_ironlake_limit(struct drm_crtc *crtc,
 	const intel_limit_t *limit;
 
 	if (intel_pipe_has_type(crtc, INTEL_OUTPUT_LVDS)) {
-		if ((I915_READ(PCH_LVDS) & LVDS_CLKB_POWER_MASK) ==
-		    LVDS_CLKB_POWER_UP) {
+		if (is_dual_link_lvds(dev_priv, PCH_LVDS)) {
 			/* LVDS dual channel */
 			if (refclk == 100000)
 				limit = &intel_limits_ironlake_dual_lvds_100m;
@@ -397,8 +477,7 @@ static const intel_limit_t *intel_g4x_limit(struct drm_crtc *crtc)
 	const intel_limit_t *limit;
 
 	if (intel_pipe_has_type(crtc, INTEL_OUTPUT_LVDS)) {
-		if ((I915_READ(LVDS) & LVDS_CLKB_POWER_MASK) ==
-		    LVDS_CLKB_POWER_UP)
+		if (is_dual_link_lvds(dev_priv, LVDS))
 			/* LVDS with dual channel */
 			limit = &intel_limits_g4x_dual_channel_lvds;
 		else
@@ -536,8 +615,7 @@ intel_find_best_PLL(const intel_limit_t *limit, struct drm_crtc *crtc,
 		 * reliably set up different single/dual channel state, if we
 		 * even can.
 		 */
-		if ((I915_READ(LVDS) & LVDS_CLKB_POWER_MASK) ==
-		    LVDS_CLKB_POWER_UP)
+		if (is_dual_link_lvds(dev_priv, LVDS))
 			clock.p2 = limit->p2.p2_fast;
 		else
 			clock.p2 = limit->p2.p2_slow;
@@ -706,6 +784,17 @@ intel_find_pll_g4x_dp(const intel_limit_t *limit, struct drm_crtc *crtc,
 	return true;
 }
 
+static void ironlake_wait_for_vblank(struct drm_device *dev, int pipe)
+{
+	struct drm_i915_private *dev_priv = dev->dev_private;
+	u32 frame, frame_reg = PIPEFRAME(pipe);
+
+	frame = I915_READ(frame_reg);
+
+	if (wait_for(I915_READ_NOTRACE(frame_reg) != frame, 50))
+		DRM_DEBUG_KMS("vblank wait timed out\n");
+}
+
 /**
  * intel_wait_for_vblank - wait for vblank on a given pipe
  * @dev: drm device
@@ -719,6 +808,11 @@ void intel_wait_for_vblank(struct drm_device *dev, int pipe)
 	struct drm_i915_private *dev_priv = dev->dev_private;
 	int pipestat_reg = PIPESTAT(pipe);
 
+	if (INTEL_INFO(dev)->gen >= 5) {
+		ironlake_wait_for_vblank(dev, pipe);
+		return;
+	}
+
 	/* Clear existing vblank status. Note this will clear any other
 	 * sticky status fields as well.
 	 *
@@ -771,15 +865,20 @@ void intel_wait_for_pipe_off(struct drm_device *dev, int pipe)
 			     100))
 			DRM_DEBUG_KMS("pipe_off wait timed out\n");
 	} else {
-		u32 last_line;
+		u32 last_line, line_mask;
 		int reg = PIPEDSL(pipe);
 		unsigned long timeout = jiffies + msecs_to_jiffies(100);
 
+		if (IS_GEN2(dev))
+			line_mask = DSL_LINEMASK_GEN2;
+		else
+			line_mask = DSL_LINEMASK_GEN3;
+
 		/* Wait for the display line to settle */
 		do {
-			last_line = I915_READ(reg) & DSL_LINEMASK;
+			last_line = I915_READ(reg) & line_mask;
 			mdelay(5);
-		} while (((I915_READ(reg) & DSL_LINEMASK) != last_line) &&
+		} while (((I915_READ(reg) & line_mask) != last_line) &&
 			 time_after(timeout, jiffies));
 		if (time_after(jiffies, timeout))
 			DRM_DEBUG_KMS("pipe_off wait timed out\n");
@@ -811,26 +910,33 @@ static void assert_pll(struct drm_i915_private *dev_priv,
 
 /* For ILK+ */
 static void assert_pch_pll(struct drm_i915_private *dev_priv,
-			   enum pipe pipe, bool state)
+			   struct intel_crtc *intel_crtc, bool state)
 {
 	int reg;
 	u32 val;
 	bool cur_state;
 
+	if (HAS_PCH_LPT(dev_priv->dev)) {
+		DRM_DEBUG_DRIVER("LPT detected: skipping PCH PLL test\n");
+		return;
+	}
+
+	if (!intel_crtc->pch_pll) {
+		WARN(1, "asserting PCH PLL enabled with no PLL\n");
+		return;
+	}
+
 	if (HAS_PCH_CPT(dev_priv->dev)) {
 		u32 pch_dpll;
 
 		pch_dpll = I915_READ(PCH_DPLL_SEL);
 
 		/* Make sure the selected PLL is enabled to the transcoder */
-		WARN(!((pch_dpll >> (4 * pipe)) & 8),
-		     "transcoder %d PLL not enabled\n", pipe);
-
-		/* Convert the transcoder pipe number to a pll pipe number */
-		pipe = (pch_dpll >> (4 * pipe)) & 1;
+		WARN(!((pch_dpll >> (4 * intel_crtc->pipe)) & 8),
+		     "transcoder %d PLL not enabled\n", intel_crtc->pipe);
 	}
 
-	reg = PCH_DPLL(pipe);
+	reg = intel_crtc->pch_pll->pll_reg;
 	val = I915_READ(reg);
 	cur_state = !!(val & DPLL_VCO_ENABLE);
 	WARN(cur_state != state,
@@ -847,9 +953,16 @@ static void assert_fdi_tx(struct drm_i915_private *dev_priv,
 	u32 val;
 	bool cur_state;
 
-	reg = FDI_TX_CTL(pipe);
-	val = I915_READ(reg);
-	cur_state = !!(val & FDI_TX_ENABLE);
+	if (IS_HASWELL(dev_priv->dev)) {
+		/* On Haswell, DDI is used instead of FDI_TX_CTL */
+		reg = DDI_FUNC_CTL(pipe);
+		val = I915_READ(reg);
+		cur_state = !!(val & PIPE_DDI_FUNC_ENABLE);
+	} else {
+		reg = FDI_TX_CTL(pipe);
+		val = I915_READ(reg);
+		cur_state = !!(val & FDI_TX_ENABLE);
+	}
 	WARN(cur_state != state,
 	     "FDI TX state assertion failure (expected %s, current %s)\n",
 	     state_string(state), state_string(cur_state));
@@ -864,9 +977,14 @@ static void assert_fdi_rx(struct drm_i915_private *dev_priv,
 	u32 val;
 	bool cur_state;
 
-	reg = FDI_RX_CTL(pipe);
-	val = I915_READ(reg);
-	cur_state = !!(val & FDI_RX_ENABLE);
+	if (IS_HASWELL(dev_priv->dev) && pipe > 0) {
+			DRM_ERROR("Attempting to enable FDI_RX on Haswell pipe > 0\n");
+			return;
+	} else {
+		reg = FDI_RX_CTL(pipe);
+		val = I915_READ(reg);
+		cur_state = !!(val & FDI_RX_ENABLE);
+	}
 	WARN(cur_state != state,
 	     "FDI RX state assertion failure (expected %s, current %s)\n",
 	     state_string(state), state_string(cur_state));
@@ -884,6 +1002,10 @@ static void assert_fdi_tx_pll_enabled(struct drm_i915_private *dev_priv,
 	if (dev_priv->info->gen == 5)
 		return;
 
+	/* On Haswell, DDI ports are responsible for the FDI PLL setup */
+	if (IS_HASWELL(dev_priv->dev))
+		return;
+
 	reg = FDI_TX_CTL(pipe);
 	val = I915_READ(reg);
 	WARN(!(val & FDI_TX_PLL_ENABLE), "FDI TX PLL assertion failure, should be active but is disabled\n");
@@ -895,6 +1017,10 @@ static void assert_fdi_rx_pll_enabled(struct drm_i915_private *dev_priv,
 	int reg;
 	u32 val;
 
+	if (IS_HASWELL(dev_priv->dev) && pipe > 0) {
+		DRM_ERROR("Attempting to enable FDI on Haswell with pipe > 0\n");
+		return;
+	}
 	reg = FDI_RX_CTL(pipe);
 	val = I915_READ(reg);
 	WARN(!(val & FDI_RX_PLL_ENABLE), "FDI RX PLL assertion failure, should be active but is disabled\n");
@@ -1000,6 +1126,11 @@ static void assert_pch_refclk_enabled(struct drm_i915_private *dev_priv)
 	u32 val;
 	bool enabled;
 
+	if (HAS_PCH_LPT(dev_priv->dev)) {
+		DRM_DEBUG_DRIVER("LPT does not has PCH refclk, skipping check\n");
+		return;
+	}
+
 	val = I915_READ(PCH_DREF_CONTROL);
 	enabled = !!(val & (DREF_SSC_SOURCE_MASK | DREF_NONSPREAD_SOURCE_MASK |
 			    DREF_SUPERSPREAD_SOURCE_MASK));
@@ -1198,6 +1329,69 @@ static void intel_disable_pll(struct drm_i915_private *dev_priv, enum pipe pipe)
 	POSTING_READ(reg);
 }
 
+/* SBI access */
+static void
+intel_sbi_write(struct drm_i915_private *dev_priv, u16 reg, u32 value)
+{
+	unsigned long flags;
+
+	spin_lock_irqsave(&dev_priv->dpio_lock, flags);
+	if (wait_for((I915_READ(SBI_CTL_STAT) & SBI_READY) == 0,
+				100)) {
+		DRM_ERROR("timeout waiting for SBI to become ready\n");
+		goto out_unlock;
+	}
+
+	I915_WRITE(SBI_ADDR,
+			(reg << 16));
+	I915_WRITE(SBI_DATA,
+			value);
+	I915_WRITE(SBI_CTL_STAT,
+			SBI_BUSY |
+			SBI_CTL_OP_CRWR);
+
+	if (wait_for((I915_READ(SBI_CTL_STAT) & (SBI_READY | SBI_RESPONSE_SUCCESS)) == 0,
+				100)) {
+		DRM_ERROR("timeout waiting for SBI to complete write transaction\n");
+		goto out_unlock;
+	}
+
+out_unlock:
+	spin_unlock_irqrestore(&dev_priv->dpio_lock, flags);
+}
+
+static u32
+intel_sbi_read(struct drm_i915_private *dev_priv, u16 reg)
+{
+	unsigned long flags;
+	u32 value;
+
+	spin_lock_irqsave(&dev_priv->dpio_lock, flags);
+	if (wait_for((I915_READ(SBI_CTL_STAT) & SBI_READY) == 0,
+				100)) {
+		DRM_ERROR("timeout waiting for SBI to become ready\n");
+		goto out_unlock;
+	}
+
+	I915_WRITE(SBI_ADDR,
+			(reg << 16));
+	I915_WRITE(SBI_CTL_STAT,
+			SBI_BUSY |
+			SBI_CTL_OP_CRRD);
+
+	if (wait_for((I915_READ(SBI_CTL_STAT) & (SBI_READY | SBI_RESPONSE_SUCCESS)) == 0,
+				100)) {
+		DRM_ERROR("timeout waiting for SBI to complete read transaction\n");
+		goto out_unlock;
+	}
+
+	value = I915_READ(SBI_DATA);
+
+out_unlock:
+	spin_unlock_irqrestore(&dev_priv->dpio_lock, flags);
+	return value;
+}
+
 /**
  * intel_enable_pch_pll - enable PCH PLL
  * @dev_priv: i915 private structure
@@ -1206,60 +1400,88 @@ static void intel_disable_pll(struct drm_i915_private *dev_priv, enum pipe pipe)
  * The PCH PLL needs to be enabled before the PCH transcoder, since it
  * drives the transcoder clock.
  */
-static void intel_enable_pch_pll(struct drm_i915_private *dev_priv,
-				 enum pipe pipe)
+static void intel_enable_pch_pll(struct intel_crtc *intel_crtc)
 {
+	struct drm_i915_private *dev_priv = intel_crtc->base.dev->dev_private;
+	struct intel_pch_pll *pll;
 	int reg;
 	u32 val;
 
-	if (pipe > 1)
+	/* PCH PLLs only available on ILK, SNB and IVB */
+	BUG_ON(dev_priv->info->gen < 5);
+	pll = intel_crtc->pch_pll;
+	if (pll == NULL)
 		return;
 
-	/* PCH only available on ILK+ */
-	BUG_ON(dev_priv->info->gen < 5);
+	if (WARN_ON(pll->refcount == 0))
+		return;
+
+	DRM_DEBUG_KMS("enable PCH PLL %x (active %d, on? %d)for crtc %d\n",
+		      pll->pll_reg, pll->active, pll->on,
+		      intel_crtc->base.base.id);
 
 	/* PCH refclock must be enabled first */
 	assert_pch_refclk_enabled(dev_priv);
 
-	reg = PCH_DPLL(pipe);
+	if (pll->active++ && pll->on) {
+		assert_pch_pll_enabled(dev_priv, intel_crtc);
+		return;
+	}
+
+	DRM_DEBUG_KMS("enabling PCH PLL %x\n", pll->pll_reg);
+
+	reg = pll->pll_reg;
 	val = I915_READ(reg);
 	val |= DPLL_VCO_ENABLE;
 	I915_WRITE(reg, val);
 	POSTING_READ(reg);
 	udelay(200);
+
+	pll->on = true;
 }
 
-static void intel_disable_pch_pll(struct drm_i915_private *dev_priv,
-				  enum pipe pipe)
+static void intel_disable_pch_pll(struct intel_crtc *intel_crtc)
 {
+	struct drm_i915_private *dev_priv = intel_crtc->base.dev->dev_private;
+	struct intel_pch_pll *pll = intel_crtc->pch_pll;
 	int reg;
-	u32 val, pll_mask = TRANSC_DPLL_ENABLE | TRANSC_DPLLB_SEL,
-		pll_sel = TRANSC_DPLL_ENABLE;
-
-	if (pipe > 1)
-		return;
+	u32 val;
 
 	/* PCH only available on ILK+ */
 	BUG_ON(dev_priv->info->gen < 5);
+	if (pll == NULL)
+	       return;
 
-	/* Make sure transcoder isn't still depending on us */
-	assert_transcoder_disabled(dev_priv, pipe);
+	if (WARN_ON(pll->refcount == 0))
+		return;
 
-	if (pipe == 0)
-		pll_sel |= TRANSC_DPLLA_SEL;
-	else if (pipe == 1)
-		pll_sel |= TRANSC_DPLLB_SEL;
+	DRM_DEBUG_KMS("disable PCH PLL %x (active %d, on? %d) for crtc %d\n",
+		      pll->pll_reg, pll->active, pll->on,
+		      intel_crtc->base.base.id);
 
+	if (WARN_ON(pll->active == 0)) {
+		assert_pch_pll_disabled(dev_priv, intel_crtc);
+		return;
+	}
 
-	if ((I915_READ(PCH_DPLL_SEL) & pll_mask) == pll_sel)
+	if (--pll->active) {
+		assert_pch_pll_enabled(dev_priv, intel_crtc);
 		return;
+	}
+
+	DRM_DEBUG_KMS("disabling PCH PLL %x\n", pll->pll_reg);
 
-	reg = PCH_DPLL(pipe);
+	/* Make sure transcoder isn't still depending on us */
+	assert_transcoder_disabled(dev_priv, intel_crtc->pipe);
+
+	reg = pll->pll_reg;
 	val = I915_READ(reg);
 	val &= ~DPLL_VCO_ENABLE;
 	I915_WRITE(reg, val);
 	POSTING_READ(reg);
 	udelay(200);
+
+	pll->on = false;
 }
 
 static void intel_enable_transcoder(struct drm_i915_private *dev_priv,
@@ -1273,12 +1495,16 @@ static void intel_enable_transcoder(struct drm_i915_private *dev_priv,
 	BUG_ON(dev_priv->info->gen < 5);
 
 	/* Make sure PCH DPLL is enabled */
-	assert_pch_pll_enabled(dev_priv, pipe);
+	assert_pch_pll_enabled(dev_priv, to_intel_crtc(crtc));
 
 	/* FDI must be feeding us bits for PCH ports */
 	assert_fdi_tx_enabled(dev_priv, pipe);
 	assert_fdi_rx_enabled(dev_priv, pipe);
 
+	if (IS_HASWELL(dev_priv->dev) && pipe > 0) {
+		DRM_ERROR("Attempting to enable transcoder on Haswell with pipe > 0\n");
+		return;
+	}
 	reg = TRANSCONF(pipe);
 	val = I915_READ(reg);
 	pipeconf_val = I915_READ(PIPECONF(pipe));
@@ -1415,7 +1641,7 @@ static void intel_disable_pipe(struct drm_i915_private *dev_priv,
  * Plane regs are double buffered, going from enabled->disabled needs a
  * trigger in order to latch.  The display address reg provides this.
  */
-static void intel_flush_display_plane(struct drm_i915_private *dev_priv,
+void intel_flush_display_plane(struct drm_i915_private *dev_priv,
 				      enum plane plane)
 {
 	I915_WRITE(DSPADDR(plane), I915_READ(DSPADDR(plane)));
@@ -1526,490 +1752,6 @@ static void intel_disable_pch_ports(struct drm_i915_private *dev_priv,
 	disable_pch_hdmi(dev_priv, pipe, HDMID);
 }
 
-static void i8xx_disable_fbc(struct drm_device *dev)
-{
-	struct drm_i915_private *dev_priv = dev->dev_private;
-	u32 fbc_ctl;
-
-	/* Disable compression */
-	fbc_ctl = I915_READ(FBC_CONTROL);
-	if ((fbc_ctl & FBC_CTL_EN) == 0)
-		return;
-
-	fbc_ctl &= ~FBC_CTL_EN;
-	I915_WRITE(FBC_CONTROL, fbc_ctl);
-
-	/* Wait for compressing bit to clear */
-	if (wait_for((I915_READ(FBC_STATUS) & FBC_STAT_COMPRESSING) == 0, 10)) {
-		DRM_DEBUG_KMS("FBC idle timed out\n");
-		return;
-	}
-
-	DRM_DEBUG_KMS("disabled FBC\n");
-}
-
-static void i8xx_enable_fbc(struct drm_crtc *crtc, unsigned long interval)
-{
-	struct drm_device *dev = crtc->dev;
-	struct drm_i915_private *dev_priv = dev->dev_private;
-	struct drm_framebuffer *fb = crtc->fb;
-	struct intel_framebuffer *intel_fb = to_intel_framebuffer(fb);
-	struct drm_i915_gem_object *obj = intel_fb->obj;
-	struct intel_crtc *intel_crtc = to_intel_crtc(crtc);
-	int cfb_pitch;
-	int plane, i;
-	u32 fbc_ctl, fbc_ctl2;
-
-	cfb_pitch = dev_priv->cfb_size / FBC_LL_SIZE;
-	if (fb->pitches[0] < cfb_pitch)
-		cfb_pitch = fb->pitches[0];
-
-	/* FBC_CTL wants 64B units */
-	cfb_pitch = (cfb_pitch / 64) - 1;
-	plane = intel_crtc->plane == 0 ? FBC_CTL_PLANEA : FBC_CTL_PLANEB;
-
-	/* Clear old tags */
-	for (i = 0; i < (FBC_LL_SIZE / 32) + 1; i++)
-		I915_WRITE(FBC_TAG + (i * 4), 0);
-
-	/* Set it up... */
-	fbc_ctl2 = FBC_CTL_FENCE_DBL | FBC_CTL_IDLE_IMM | FBC_CTL_CPU_FENCE;
-	fbc_ctl2 |= plane;
-	I915_WRITE(FBC_CONTROL2, fbc_ctl2);
-	I915_WRITE(FBC_FENCE_OFF, crtc->y);
-
-	/* enable it... */
-	fbc_ctl = FBC_CTL_EN | FBC_CTL_PERIODIC;
-	if (IS_I945GM(dev))
-		fbc_ctl |= FBC_CTL_C3_IDLE; /* 945 needs special SR handling */
-	fbc_ctl |= (cfb_pitch & 0xff) << FBC_CTL_STRIDE_SHIFT;
-	fbc_ctl |= (interval & 0x2fff) << FBC_CTL_INTERVAL_SHIFT;
-	fbc_ctl |= obj->fence_reg;
-	I915_WRITE(FBC_CONTROL, fbc_ctl);
-
-	DRM_DEBUG_KMS("enabled FBC, pitch %d, yoff %d, plane %d, ",
-		      cfb_pitch, crtc->y, intel_crtc->plane);
-}
-
-static bool i8xx_fbc_enabled(struct drm_device *dev)
-{
-	struct drm_i915_private *dev_priv = dev->dev_private;
-
-	return I915_READ(FBC_CONTROL) & FBC_CTL_EN;
-}
-
-static void g4x_enable_fbc(struct drm_crtc *crtc, unsigned long interval)
-{
-	struct drm_device *dev = crtc->dev;
-	struct drm_i915_private *dev_priv = dev->dev_private;
-	struct drm_framebuffer *fb = crtc->fb;
-	struct intel_framebuffer *intel_fb = to_intel_framebuffer(fb);
-	struct drm_i915_gem_object *obj = intel_fb->obj;
-	struct intel_crtc *intel_crtc = to_intel_crtc(crtc);
-	int plane = intel_crtc->plane == 0 ? DPFC_CTL_PLANEA : DPFC_CTL_PLANEB;
-	unsigned long stall_watermark = 200;
-	u32 dpfc_ctl;
-
-	dpfc_ctl = plane | DPFC_SR_EN | DPFC_CTL_LIMIT_1X;
-	dpfc_ctl |= DPFC_CTL_FENCE_EN | obj->fence_reg;
-	I915_WRITE(DPFC_CHICKEN, DPFC_HT_MODIFY);
-
-	I915_WRITE(DPFC_RECOMP_CTL, DPFC_RECOMP_STALL_EN |
-		   (stall_watermark << DPFC_RECOMP_STALL_WM_SHIFT) |
-		   (interval << DPFC_RECOMP_TIMER_COUNT_SHIFT));
-	I915_WRITE(DPFC_FENCE_YOFF, crtc->y);
-
-	/* enable it... */
-	I915_WRITE(DPFC_CONTROL, I915_READ(DPFC_CONTROL) | DPFC_CTL_EN);
-
-	DRM_DEBUG_KMS("enabled fbc on plane %d\n", intel_crtc->plane);
-}
-
-static void g4x_disable_fbc(struct drm_device *dev)
-{
-	struct drm_i915_private *dev_priv = dev->dev_private;
-	u32 dpfc_ctl;
-
-	/* Disable compression */
-	dpfc_ctl = I915_READ(DPFC_CONTROL);
-	if (dpfc_ctl & DPFC_CTL_EN) {
-		dpfc_ctl &= ~DPFC_CTL_EN;
-		I915_WRITE(DPFC_CONTROL, dpfc_ctl);
-
-		DRM_DEBUG_KMS("disabled FBC\n");
-	}
-}
-
-static bool g4x_fbc_enabled(struct drm_device *dev)
-{
-	struct drm_i915_private *dev_priv = dev->dev_private;
-
-	return I915_READ(DPFC_CONTROL) & DPFC_CTL_EN;
-}
-
-static void sandybridge_blit_fbc_update(struct drm_device *dev)
-{
-	struct drm_i915_private *dev_priv = dev->dev_private;
-	u32 blt_ecoskpd;
-
-	/* Make sure blitter notifies FBC of writes */
-	gen6_gt_force_wake_get(dev_priv);
-	blt_ecoskpd = I915_READ(GEN6_BLITTER_ECOSKPD);
-	blt_ecoskpd |= GEN6_BLITTER_FBC_NOTIFY <<
-		GEN6_BLITTER_LOCK_SHIFT;
-	I915_WRITE(GEN6_BLITTER_ECOSKPD, blt_ecoskpd);
-	blt_ecoskpd |= GEN6_BLITTER_FBC_NOTIFY;
-	I915_WRITE(GEN6_BLITTER_ECOSKPD, blt_ecoskpd);
-	blt_ecoskpd &= ~(GEN6_BLITTER_FBC_NOTIFY <<
-			 GEN6_BLITTER_LOCK_SHIFT);
-	I915_WRITE(GEN6_BLITTER_ECOSKPD, blt_ecoskpd);
-	POSTING_READ(GEN6_BLITTER_ECOSKPD);
-	gen6_gt_force_wake_put(dev_priv);
-}
-
-static void ironlake_enable_fbc(struct drm_crtc *crtc, unsigned long interval)
-{
-	struct drm_device *dev = crtc->dev;
-	struct drm_i915_private *dev_priv = dev->dev_private;
-	struct drm_framebuffer *fb = crtc->fb;
-	struct intel_framebuffer *intel_fb = to_intel_framebuffer(fb);
-	struct drm_i915_gem_object *obj = intel_fb->obj;
-	struct intel_crtc *intel_crtc = to_intel_crtc(crtc);
-	int plane = intel_crtc->plane == 0 ? DPFC_CTL_PLANEA : DPFC_CTL_PLANEB;
-	unsigned long stall_watermark = 200;
-	u32 dpfc_ctl;
-
-	dpfc_ctl = I915_READ(ILK_DPFC_CONTROL);
-	dpfc_ctl &= DPFC_RESERVED;
-	dpfc_ctl |= (plane | DPFC_CTL_LIMIT_1X);
-	/* Set persistent mode for front-buffer rendering, ala X. */
-	dpfc_ctl |= DPFC_CTL_PERSISTENT_MODE;
-	dpfc_ctl |= (DPFC_CTL_FENCE_EN | obj->fence_reg);
-	I915_WRITE(ILK_DPFC_CHICKEN, DPFC_HT_MODIFY);
-
-	I915_WRITE(ILK_DPFC_RECOMP_CTL, DPFC_RECOMP_STALL_EN |
-		   (stall_watermark << DPFC_RECOMP_STALL_WM_SHIFT) |
-		   (interval << DPFC_RECOMP_TIMER_COUNT_SHIFT));
-	I915_WRITE(ILK_DPFC_FENCE_YOFF, crtc->y);
-	I915_WRITE(ILK_FBC_RT_BASE, obj->gtt_offset | ILK_FBC_RT_VALID);
-	/* enable it... */
-	I915_WRITE(ILK_DPFC_CONTROL, dpfc_ctl | DPFC_CTL_EN);
-
-	if (IS_GEN6(dev)) {
-		I915_WRITE(SNB_DPFC_CTL_SA,
-			   SNB_CPU_FENCE_ENABLE | obj->fence_reg);
-		I915_WRITE(DPFC_CPU_FENCE_OFFSET, crtc->y);
-		sandybridge_blit_fbc_update(dev);
-	}
-
-	DRM_DEBUG_KMS("enabled fbc on plane %d\n", intel_crtc->plane);
-}
-
-static void ironlake_disable_fbc(struct drm_device *dev)
-{
-	struct drm_i915_private *dev_priv = dev->dev_private;
-	u32 dpfc_ctl;
-
-	/* Disable compression */
-	dpfc_ctl = I915_READ(ILK_DPFC_CONTROL);
-	if (dpfc_ctl & DPFC_CTL_EN) {
-		dpfc_ctl &= ~DPFC_CTL_EN;
-		I915_WRITE(ILK_DPFC_CONTROL, dpfc_ctl);
-
-		DRM_DEBUG_KMS("disabled FBC\n");
-	}
-}
-
-static bool ironlake_fbc_enabled(struct drm_device *dev)
-{
-	struct drm_i915_private *dev_priv = dev->dev_private;
-
-	return I915_READ(ILK_DPFC_CONTROL) & DPFC_CTL_EN;
-}
-
-bool intel_fbc_enabled(struct drm_device *dev)
-{
-	struct drm_i915_private *dev_priv = dev->dev_private;
-
-	if (!dev_priv->display.fbc_enabled)
-		return false;
-
-	return dev_priv->display.fbc_enabled(dev);
-}
-
-static void intel_fbc_work_fn(struct work_struct *__work)
-{
-	struct intel_fbc_work *work =
-		container_of(to_delayed_work(__work),
-			     struct intel_fbc_work, work);
-	struct drm_device *dev = work->crtc->dev;
-	struct drm_i915_private *dev_priv = dev->dev_private;
-
-	mutex_lock(&dev->struct_mutex);
-	if (work == dev_priv->fbc_work) {
-		/* Double check that we haven't switched fb without cancelling
-		 * the prior work.
-		 */
-		if (work->crtc->fb == work->fb) {
-			dev_priv->display.enable_fbc(work->crtc,
-						     work->interval);
-
-			dev_priv->cfb_plane = to_intel_crtc(work->crtc)->plane;
-			dev_priv->cfb_fb = work->crtc->fb->base.id;
-			dev_priv->cfb_y = work->crtc->y;
-		}
-
-		dev_priv->fbc_work = NULL;
-	}
-	mutex_unlock(&dev->struct_mutex);
-
-	kfree(work);
-}
-
-static void intel_cancel_fbc_work(struct drm_i915_private *dev_priv)
-{
-	if (dev_priv->fbc_work == NULL)
-		return;
-
-	DRM_DEBUG_KMS("cancelling pending FBC enable\n");
-
-	/* Synchronisation is provided by struct_mutex and checking of
-	 * dev_priv->fbc_work, so we can perform the cancellation
-	 * entirely asynchronously.
-	 */
-	if (cancel_delayed_work(&dev_priv->fbc_work->work))
-		/* tasklet was killed before being run, clean up */
-		kfree(dev_priv->fbc_work);
-
-	/* Mark the work as no longer wanted so that if it does
-	 * wake-up (because the work was already running and waiting
-	 * for our mutex), it will discover that is no longer
-	 * necessary to run.
-	 */
-	dev_priv->fbc_work = NULL;
-}
-
-static void intel_enable_fbc(struct drm_crtc *crtc, unsigned long interval)
-{
-	struct intel_fbc_work *work;
-	struct drm_device *dev = crtc->dev;
-	struct drm_i915_private *dev_priv = dev->dev_private;
-
-	if (!dev_priv->display.enable_fbc)
-		return;
-
-	intel_cancel_fbc_work(dev_priv);
-
-	work = kzalloc(sizeof *work, GFP_KERNEL);
-	if (work == NULL) {
-		dev_priv->display.enable_fbc(crtc, interval);
-		return;
-	}
-
-	work->crtc = crtc;
-	work->fb = crtc->fb;
-	work->interval = interval;
-	INIT_DELAYED_WORK(&work->work, intel_fbc_work_fn);
-
-	dev_priv->fbc_work = work;
-
-	DRM_DEBUG_KMS("scheduling delayed FBC enable\n");
-
-	/* Delay the actual enabling to let pageflipping cease and the
-	 * display to settle before starting the compression. Note that
-	 * this delay also serves a second purpose: it allows for a
-	 * vblank to pass after disabling the FBC before we attempt
-	 * to modify the control registers.
-	 *
-	 * A more complicated solution would involve tracking vblanks
-	 * following the termination of the page-flipping sequence
-	 * and indeed performing the enable as a co-routine and not
-	 * waiting synchronously upon the vblank.
-	 */
-	schedule_delayed_work(&work->work, msecs_to_jiffies(50));
-}
-
-void intel_disable_fbc(struct drm_device *dev)
-{
-	struct drm_i915_private *dev_priv = dev->dev_private;
-
-	intel_cancel_fbc_work(dev_priv);
-
-	if (!dev_priv->display.disable_fbc)
-		return;
-
-	dev_priv->display.disable_fbc(dev);
-	dev_priv->cfb_plane = -1;
-}
-
-/**
- * intel_update_fbc - enable/disable FBC as needed
- * @dev: the drm_device
- *
- * Set up the framebuffer compression hardware at mode set time.  We
- * enable it if possible:
- *   - plane A only (on pre-965)
- *   - no pixel mulitply/line duplication
- *   - no alpha buffer discard
- *   - no dual wide
- *   - framebuffer <= 2048 in width, 1536 in height
- *
- * We can't assume that any compression will take place (worst case),
- * so the compressed buffer has to be the same size as the uncompressed
- * one.  It also must reside (along with the line length buffer) in
- * stolen memory.
- *
- * We need to enable/disable FBC on a global basis.
- */
-static void intel_update_fbc(struct drm_device *dev)
-{
-	struct drm_i915_private *dev_priv = dev->dev_private;
-	struct drm_crtc *crtc = NULL, *tmp_crtc;
-	struct intel_crtc *intel_crtc;
-	struct drm_framebuffer *fb;
-	struct intel_framebuffer *intel_fb;
-	struct drm_i915_gem_object *obj;
-	int enable_fbc;
-
-	DRM_DEBUG_KMS("\n");
-
-	if (!i915_powersave)
-		return;
-
-	if (!I915_HAS_FBC(dev))
-		return;
-
-	/*
-	 * If FBC is already on, we just have to verify that we can
-	 * keep it that way...
-	 * Need to disable if:
-	 *   - more than one pipe is active
-	 *   - changing FBC params (stride, fence, mode)
-	 *   - new fb is too large to fit in compressed buffer
-	 *   - going to an unsupported config (interlace, pixel multiply, etc.)
-	 */
-	list_for_each_entry(tmp_crtc, &dev->mode_config.crtc_list, head) {
-		if (tmp_crtc->enabled && tmp_crtc->fb) {
-			if (crtc) {
-				DRM_DEBUG_KMS("more than one pipe active, disabling compression\n");
-				dev_priv->no_fbc_reason = FBC_MULTIPLE_PIPES;
-				goto out_disable;
-			}
-			crtc = tmp_crtc;
-		}
-	}
-
-	if (!crtc || crtc->fb == NULL) {
-		DRM_DEBUG_KMS("no output, disabling\n");
-		dev_priv->no_fbc_reason = FBC_NO_OUTPUT;
-		goto out_disable;
-	}
-
-	intel_crtc = to_intel_crtc(crtc);
-	fb = crtc->fb;
-	intel_fb = to_intel_framebuffer(fb);
-	obj = intel_fb->obj;
-
-	enable_fbc = i915_enable_fbc;
-	if (enable_fbc < 0) {
-		DRM_DEBUG_KMS("fbc set to per-chip default\n");
-		enable_fbc = 1;
-		if (INTEL_INFO(dev)->gen <= 6)
-			enable_fbc = 0;
-	}
-	if (!enable_fbc) {
-		DRM_DEBUG_KMS("fbc disabled per module param\n");
-		dev_priv->no_fbc_reason = FBC_MODULE_PARAM;
-		goto out_disable;
-	}
-	if (intel_fb->obj->base.size > dev_priv->cfb_size) {
-		DRM_DEBUG_KMS("framebuffer too large, disabling "
-			      "compression\n");
-		dev_priv->no_fbc_reason = FBC_STOLEN_TOO_SMALL;
-		goto out_disable;
-	}
-	if ((crtc->mode.flags & DRM_MODE_FLAG_INTERLACE) ||
-	    (crtc->mode.flags & DRM_MODE_FLAG_DBLSCAN)) {
-		DRM_DEBUG_KMS("mode incompatible with compression, "
-			      "disabling\n");
-		dev_priv->no_fbc_reason = FBC_UNSUPPORTED_MODE;
-		goto out_disable;
-	}
-	if ((crtc->mode.hdisplay > 2048) ||
-	    (crtc->mode.vdisplay > 1536)) {
-		DRM_DEBUG_KMS("mode too large for compression, disabling\n");
-		dev_priv->no_fbc_reason = FBC_MODE_TOO_LARGE;
-		goto out_disable;
-	}
-	if ((IS_I915GM(dev) || IS_I945GM(dev)) && intel_crtc->plane != 0) {
-		DRM_DEBUG_KMS("plane not 0, disabling compression\n");
-		dev_priv->no_fbc_reason = FBC_BAD_PLANE;
-		goto out_disable;
-	}
-
-	/* The use of a CPU fence is mandatory in order to detect writes
-	 * by the CPU to the scanout and trigger updates to the FBC.
-	 */
-	if (obj->tiling_mode != I915_TILING_X ||
-	    obj->fence_reg == I915_FENCE_REG_NONE) {
-		DRM_DEBUG_KMS("framebuffer not tiled or fenced, disabling compression\n");
-		dev_priv->no_fbc_reason = FBC_NOT_TILED;
-		goto out_disable;
-	}
-
-	/* If the kernel debugger is active, always disable compression */
-	if (in_dbg_master())
-		goto out_disable;
-
-	/* If the scanout has not changed, don't modify the FBC settings.
-	 * Note that we make the fundamental assumption that the fb->obj
-	 * cannot be unpinned (and have its GTT offset and fence revoked)
-	 * without first being decoupled from the scanout and FBC disabled.
-	 */
-	if (dev_priv->cfb_plane == intel_crtc->plane &&
-	    dev_priv->cfb_fb == fb->base.id &&
-	    dev_priv->cfb_y == crtc->y)
-		return;
-
-	if (intel_fbc_enabled(dev)) {
-		/* We update FBC along two paths, after changing fb/crtc
-		 * configuration (modeswitching) and after page-flipping
-		 * finishes. For the latter, we know that not only did
-		 * we disable the FBC at the start of the page-flip
-		 * sequence, but also more than one vblank has passed.
-		 *
-		 * For the former case of modeswitching, it is possible
-		 * to switch between two FBC valid configurations
-		 * instantaneously so we do need to disable the FBC
-		 * before we can modify its control registers. We also
-		 * have to wait for the next vblank for that to take
-		 * effect. However, since we delay enabling FBC we can
-		 * assume that a vblank has passed since disabling and
-		 * that we can safely alter the registers in the deferred
-		 * callback.
-		 *
-		 * In the scenario that we go from a valid to invalid
-		 * and then back to valid FBC configuration we have
-		 * no strict enforcement that a vblank occurred since
-		 * disabling the FBC. However, along all current pipe
-		 * disabling paths we do need to wait for a vblank at
-		 * some point. And we wait before enabling FBC anyway.
-		 */
-		DRM_DEBUG_KMS("disabling active FBC for update\n");
-		intel_disable_fbc(dev);
-	}
-
-	intel_enable_fbc(crtc, 500);
-	return;
-
-out_disable:
-	/* Multiple disables should be harmless */
-	if (intel_fbc_enabled(dev)) {
-		DRM_DEBUG_KMS("unsupported config, disabling FBC\n");
-		intel_disable_fbc(dev);
-	}
-}
-
 int
 intel_pin_and_fence_fb_obj(struct drm_device *dev,
 			   struct drm_i915_gem_object *obj,
@@ -2050,13 +1792,11 @@ intel_pin_and_fence_fb_obj(struct drm_device *dev,
 	 * framebuffer compression.  For simplicity, we always install
 	 * a fence as the cost is not that onerous.
 	 */
-	if (obj->tiling_mode != I915_TILING_NONE) {
-		ret = i915_gem_object_get_fence(obj, pipelined);
-		if (ret)
-			goto err_unpin;
+	ret = i915_gem_object_get_fence(obj);
+	if (ret)
+		goto err_unpin;
 
-		i915_gem_object_pin_fence(obj);
-	}
+	i915_gem_object_pin_fence(obj);
 
 	dev_priv->mm.interruptible = true;
 	return 0;
@@ -2137,7 +1877,7 @@ static int i9xx_update_plane(struct drm_crtc *crtc, struct drm_framebuffer *fb,
 		      Start, Offset, x, y, fb->pitches[0]);
 	I915_WRITE(DSPSTRIDE(plane), fb->pitches[0]);
 	if (INTEL_INFO(dev)->gen >= 4) {
-		I915_WRITE(DSPSURF(plane), Start);
+		I915_MODIFY_DISPBASE(DSPSURF(plane), Start);
 		I915_WRITE(DSPTILEOFF(plane), (y << 16) | x);
 		I915_WRITE(DSPADDR(plane), Offset);
 	} else
@@ -2217,7 +1957,7 @@ static int ironlake_update_plane(struct drm_crtc *crtc,
 	DRM_DEBUG_KMS("Writing base %08lX %08lX %d %d %d\n",
 		      Start, Offset, x, y, fb->pitches[0]);
 	I915_WRITE(DSPSTRIDE(plane), fb->pitches[0]);
-	I915_WRITE(DSPSURF(plane), Start);
+	I915_MODIFY_DISPBASE(DSPSURF(plane), Start);
 	I915_WRITE(DSPTILEOFF(plane), (y << 16) | x);
 	I915_WRITE(DSPADDR(plane), Offset);
 	POSTING_READ(reg);
@@ -2232,16 +1972,12 @@ intel_pipe_set_base_atomic(struct drm_crtc *crtc, struct drm_framebuffer *fb,
 {
 	struct drm_device *dev = crtc->dev;
 	struct drm_i915_private *dev_priv = dev->dev_private;
-	int ret;
-
-	ret = dev_priv->display.update_plane(crtc, fb, x, y);
-	if (ret)
-		return ret;
 
-	intel_update_fbc(dev);
+	if (dev_priv->display.disable_fbc)
+		dev_priv->display.disable_fbc(dev);
 	intel_increase_pllclock(crtc);
 
-	return 0;
+	return dev_priv->display.update_plane(crtc, fb, x, y);
 }
 
 static int
@@ -2276,6 +2012,7 @@ intel_pipe_set_base(struct drm_crtc *crtc, int x, int y,
 		    struct drm_framebuffer *old_fb)
 {
 	struct drm_device *dev = crtc->dev;
+	struct drm_i915_private *dev_priv = dev->dev_private;
 	struct drm_i915_master_private *master_priv;
 	struct intel_crtc *intel_crtc = to_intel_crtc(crtc);
 	int ret;
@@ -2286,16 +2023,10 @@ intel_pipe_set_base(struct drm_crtc *crtc, int x, int y,
 		return 0;
 	}
 
-	switch (intel_crtc->plane) {
-	case 0:
-	case 1:
-		break;
-	case 2:
-		if (IS_IVYBRIDGE(dev))
-			break;
-		/* fall through otherwise */
-	default:
-		DRM_ERROR("no plane for crtc\n");
+	if(intel_crtc->plane > dev_priv->num_pipe) {
+		DRM_ERROR("no plane for crtc: plane %d, num_pipes %d\n",
+				intel_crtc->plane,
+				dev_priv->num_pipe);
 		return -EINVAL;
 	}
 
@@ -2312,8 +2043,7 @@ intel_pipe_set_base(struct drm_crtc *crtc, int x, int y,
 	if (old_fb)
 		intel_finish_fb(old_fb);
 
-	ret = intel_pipe_set_base_atomic(crtc, crtc->fb, x, y,
-					 LEAVE_ATOMIC_MODE_SET);
+	ret = dev_priv->display.update_plane(crtc, crtc->fb, x, y);
 	if (ret) {
 		intel_unpin_fb_obj(to_intel_framebuffer(crtc->fb)->obj);
 		mutex_unlock(&dev->struct_mutex);
@@ -2326,6 +2056,7 @@ intel_pipe_set_base(struct drm_crtc *crtc, int x, int y,
 		intel_unpin_fb_obj(to_intel_framebuffer(old_fb)->obj);
 	}
 
+	intel_update_fbc(dev);
 	mutex_unlock(&dev->struct_mutex);
 
 	if (!dev->primary->master)
@@ -2547,7 +2278,7 @@ static void gen6_fdi_link_train(struct drm_crtc *crtc)
 	struct drm_i915_private *dev_priv = dev->dev_private;
 	struct intel_crtc *intel_crtc = to_intel_crtc(crtc);
 	int pipe = intel_crtc->pipe;
-	u32 reg, temp, i;
+	u32 reg, temp, i, retry;
 
 	/* Train 1: umask FDI RX Interrupt symbol_lock and bit_lock bit
 	   for train result */
@@ -2599,15 +2330,19 @@ static void gen6_fdi_link_train(struct drm_crtc *crtc)
 		POSTING_READ(reg);
 		udelay(500);
 
-		reg = FDI_RX_IIR(pipe);
-		temp = I915_READ(reg);
-		DRM_DEBUG_KMS("FDI_RX_IIR 0x%x\n", temp);
-
-		if (temp & FDI_RX_BIT_LOCK) {
-			I915_WRITE(reg, temp | FDI_RX_BIT_LOCK);
-			DRM_DEBUG_KMS("FDI train 1 done.\n");
-			break;
+		for (retry = 0; retry < 5; retry++) {
+			reg = FDI_RX_IIR(pipe);
+			temp = I915_READ(reg);
+			DRM_DEBUG_KMS("FDI_RX_IIR 0x%x\n", temp);
+			if (temp & FDI_RX_BIT_LOCK) {
+				I915_WRITE(reg, temp | FDI_RX_BIT_LOCK);
+				DRM_DEBUG_KMS("FDI train 1 done.\n");
+				break;
+			}
+			udelay(50);
 		}
+		if (retry < 5)
+			break;
 	}
 	if (i == 4)
 		DRM_ERROR("FDI train 1 fail!\n");
@@ -2648,15 +2383,19 @@ static void gen6_fdi_link_train(struct drm_crtc *crtc)
 		POSTING_READ(reg);
 		udelay(500);
 
-		reg = FDI_RX_IIR(pipe);
-		temp = I915_READ(reg);
-		DRM_DEBUG_KMS("FDI_RX_IIR 0x%x\n", temp);
-
-		if (temp & FDI_RX_SYMBOL_LOCK) {
-			I915_WRITE(reg, temp | FDI_RX_SYMBOL_LOCK);
-			DRM_DEBUG_KMS("FDI train 2 done.\n");
-			break;
+		for (retry = 0; retry < 5; retry++) {
+			reg = FDI_RX_IIR(pipe);
+			temp = I915_READ(reg);
+			DRM_DEBUG_KMS("FDI_RX_IIR 0x%x\n", temp);
+			if (temp & FDI_RX_SYMBOL_LOCK) {
+				I915_WRITE(reg, temp | FDI_RX_SYMBOL_LOCK);
+				DRM_DEBUG_KMS("FDI train 2 done.\n");
+				break;
+			}
+			udelay(50);
 		}
+		if (retry < 5)
+			break;
 	}
 	if (i == 4)
 		DRM_ERROR("FDI train 2 fail!\n");
@@ -2808,14 +2547,18 @@ static void ironlake_fdi_pll_enable(struct drm_crtc *crtc)
 	POSTING_READ(reg);
 	udelay(200);
 
-	/* Enable CPU FDI TX PLL, always on for Ironlake */
-	reg = FDI_TX_CTL(pipe);
-	temp = I915_READ(reg);
-	if ((temp & FDI_TX_PLL_ENABLE) == 0) {
-		I915_WRITE(reg, temp | FDI_TX_PLL_ENABLE);
+	/* On Haswell, the PLL configuration for ports and pipes is handled
+	 * separately, as part of DDI setup */
+	if (!IS_HASWELL(dev)) {
+		/* Enable CPU FDI TX PLL, always on for Ironlake */
+		reg = FDI_TX_CTL(pipe);
+		temp = I915_READ(reg);
+		if ((temp & FDI_TX_PLL_ENABLE) == 0) {
+			I915_WRITE(reg, temp | FDI_TX_PLL_ENABLE);
 
-		POSTING_READ(reg);
-		udelay(100);
+			POSTING_READ(reg);
+			udelay(100);
+		}
 	}
 }
 
@@ -2888,38 +2631,16 @@ static void ironlake_fdi_disable(struct drm_crtc *crtc)
 	udelay(100);
 }
 
-/*
- * When we disable a pipe, we need to clear any pending scanline wait events
- * to avoid hanging the ring, which we assume we are waiting on.
- */
-static void intel_clear_scanline_wait(struct drm_device *dev)
-{
-	struct drm_i915_private *dev_priv = dev->dev_private;
-	struct intel_ring_buffer *ring;
-	u32 tmp;
-
-	if (IS_GEN2(dev))
-		/* Can't break the hang on i8xx */
-		return;
-
-	ring = LP_RING(dev_priv);
-	tmp = I915_READ_CTL(ring);
-	if (tmp & RING_WAIT)
-		I915_WRITE_CTL(ring, tmp);
-}
-
 static void intel_crtc_wait_for_pending_flips(struct drm_crtc *crtc)
 {
-	struct drm_i915_gem_object *obj;
-	struct drm_i915_private *dev_priv;
+	struct drm_device *dev = crtc->dev;
 
 	if (crtc->fb == NULL)
 		return;
 
-	obj = to_intel_framebuffer(crtc->fb)->obj;
-	dev_priv = crtc->dev->dev_private;
-	wait_event(dev_priv->pending_flip_queue,
-		   atomic_read(&obj->pending_flip) == 0);
+	mutex_lock(&dev->struct_mutex);
+	intel_finish_fb(crtc->fb);
+	mutex_unlock(&dev->struct_mutex);
 }
 
 static bool intel_crtc_driving_pch(struct drm_crtc *crtc)
@@ -2936,6 +2657,22 @@ static bool intel_crtc_driving_pch(struct drm_crtc *crtc)
 		if (encoder->base.crtc != crtc)
 			continue;
 
+		/* On Haswell, LPT PCH handles the VGA connection via FDI, and Haswell
+		 * CPU handles all others */
+		if (IS_HASWELL(dev)) {
+			/* It is still unclear how this will work on PPT, so throw up a warning */
+			WARN_ON(!HAS_PCH_LPT(dev));
+
+			if (encoder->type == DRM_MODE_ENCODER_DAC) {
+				DRM_DEBUG_KMS("Haswell detected DAC encoder, assuming is PCH\n");
+				return true;
+			} else {
+				DRM_DEBUG_KMS("Haswell detected encoder %d, assuming is CPU\n",
+						encoder->type);
+				return false;
+			}
+		}
+
 		switch (encoder->type) {
 		case INTEL_OUTPUT_EDP:
 			if (!intel_encoder_is_pch_edp(&encoder->base))
@@ -2947,6 +2684,97 @@ static bool intel_crtc_driving_pch(struct drm_crtc *crtc)
 	return true;
 }
 
+/* Program iCLKIP clock to the desired frequency */
+static void lpt_program_iclkip(struct drm_crtc *crtc)
+{
+	struct drm_device *dev = crtc->dev;
+	struct drm_i915_private *dev_priv = dev->dev_private;
+	u32 divsel, phaseinc, auxdiv, phasedir = 0;
+	u32 temp;
+
+	/* It is necessary to ungate the pixclk gate prior to programming
+	 * the divisors, and gate it back when it is done.
+	 */
+	I915_WRITE(PIXCLK_GATE, PIXCLK_GATE_GATE);
+
+	/* Disable SSCCTL */
+	intel_sbi_write(dev_priv, SBI_SSCCTL6,
+				intel_sbi_read(dev_priv, SBI_SSCCTL6) |
+					SBI_SSCCTL_DISABLE);
+
+	/* 20MHz is a corner case which is out of range for the 7-bit divisor */
+	if (crtc->mode.clock == 20000) {
+		auxdiv = 1;
+		divsel = 0x41;
+		phaseinc = 0x20;
+	} else {
+		/* The iCLK virtual clock root frequency is in MHz,
+		 * but the crtc->mode.clock in in KHz. To get the divisors,
+		 * it is necessary to divide one by another, so we
+		 * convert the virtual clock precision to KHz here for higher
+		 * precision.
+		 */
+		u32 iclk_virtual_root_freq = 172800 * 1000;
+		u32 iclk_pi_range = 64;
+		u32 desired_divisor, msb_divisor_value, pi_value;
+
+		desired_divisor = (iclk_virtual_root_freq / crtc->mode.clock);
+		msb_divisor_value = desired_divisor / iclk_pi_range;
+		pi_value = desired_divisor % iclk_pi_range;
+
+		auxdiv = 0;
+		divsel = msb_divisor_value - 2;
+		phaseinc = pi_value;
+	}
+
+	/* This should not happen with any sane values */
+	WARN_ON(SBI_SSCDIVINTPHASE_DIVSEL(divsel) &
+		~SBI_SSCDIVINTPHASE_DIVSEL_MASK);
+	WARN_ON(SBI_SSCDIVINTPHASE_DIR(phasedir) &
+		~SBI_SSCDIVINTPHASE_INCVAL_MASK);
+
+	DRM_DEBUG_KMS("iCLKIP clock: found settings for %dKHz refresh rate: auxdiv=%x, divsel=%x, phasedir=%x, phaseinc=%x\n",
+			crtc->mode.clock,
+			auxdiv,
+			divsel,
+			phasedir,
+			phaseinc);
+
+	/* Program SSCDIVINTPHASE6 */
+	temp = intel_sbi_read(dev_priv, SBI_SSCDIVINTPHASE6);
+	temp &= ~SBI_SSCDIVINTPHASE_DIVSEL_MASK;
+	temp |= SBI_SSCDIVINTPHASE_DIVSEL(divsel);
+	temp &= ~SBI_SSCDIVINTPHASE_INCVAL_MASK;
+	temp |= SBI_SSCDIVINTPHASE_INCVAL(phaseinc);
+	temp |= SBI_SSCDIVINTPHASE_DIR(phasedir);
+	temp |= SBI_SSCDIVINTPHASE_PROPAGATE;
+
+	intel_sbi_write(dev_priv,
+			SBI_SSCDIVINTPHASE6,
+			temp);
+
+	/* Program SSCAUXDIV */
+	temp = intel_sbi_read(dev_priv, SBI_SSCAUXDIV6);
+	temp &= ~SBI_SSCAUXDIV_FINALDIV2SEL(1);
+	temp |= SBI_SSCAUXDIV_FINALDIV2SEL(auxdiv);
+	intel_sbi_write(dev_priv,
+			SBI_SSCAUXDIV6,
+			temp);
+
+
+	/* Enable modulator and associated divider */
+	temp = intel_sbi_read(dev_priv, SBI_SSCCTL6);
+	temp &= ~SBI_SSCCTL_DISABLE;
+	intel_sbi_write(dev_priv,
+			SBI_SSCCTL6,
+			temp);
+
+	/* Wait for initialization time */
+	udelay(24);
+
+	I915_WRITE(PIXCLK_GATE, PIXCLK_GATE_UNGATE);
+}
+
 /*
  * Enable PCH resources required for PCH ports:
  *   - PCH PLLs
@@ -2961,29 +2789,41 @@ static void ironlake_pch_enable(struct drm_crtc *crtc)
 	struct drm_i915_private *dev_priv = dev->dev_private;
 	struct intel_crtc *intel_crtc = to_intel_crtc(crtc);
 	int pipe = intel_crtc->pipe;
-	u32 reg, temp, transc_sel;
+	u32 reg, temp;
+
+	assert_transcoder_disabled(dev_priv, pipe);
 
 	/* For PCH output, training FDI link */
 	dev_priv->display.fdi_link_train(crtc);
 
-	intel_enable_pch_pll(dev_priv, pipe);
+	intel_enable_pch_pll(intel_crtc);
 
-	if (HAS_PCH_CPT(dev)) {
-		transc_sel = intel_crtc->use_pll_a ? TRANSC_DPLLA_SEL :
-			TRANSC_DPLLB_SEL;
+	if (HAS_PCH_LPT(dev)) {
+		DRM_DEBUG_KMS("LPT detected: programming iCLKIP\n");
+		lpt_program_iclkip(crtc);
+	} else if (HAS_PCH_CPT(dev)) {
+		u32 sel;
 
-		/* Be sure PCH DPLL SEL is set */
 		temp = I915_READ(PCH_DPLL_SEL);
-		if (pipe == 0) {
-			temp &= ~(TRANSA_DPLLB_SEL);
-			temp |= (TRANSA_DPLL_ENABLE | TRANSA_DPLLA_SEL);
-		} else if (pipe == 1) {
-			temp &= ~(TRANSB_DPLLB_SEL);
-			temp |= (TRANSB_DPLL_ENABLE | TRANSB_DPLLB_SEL);
-		} else if (pipe == 2) {
-			temp &= ~(TRANSC_DPLLB_SEL);
-			temp |= (TRANSC_DPLL_ENABLE | transc_sel);
+		switch (pipe) {
+		default:
+		case 0:
+			temp |= TRANSA_DPLL_ENABLE;
+			sel = TRANSA_DPLLB_SEL;
+			break;
+		case 1:
+			temp |= TRANSB_DPLL_ENABLE;
+			sel = TRANSB_DPLLB_SEL;
+			break;
+		case 2:
+			temp |= TRANSC_DPLL_ENABLE;
+			sel = TRANSC_DPLLB_SEL;
+			break;
 		}
+		if (intel_crtc->pch_pll->pll_reg == _PCH_DPLL_B)
+			temp |= sel;
+		else
+			temp &= ~sel;
 		I915_WRITE(PCH_DPLL_SEL, temp);
 	}
 
@@ -2998,7 +2838,8 @@ static void ironlake_pch_enable(struct drm_crtc *crtc)
 	I915_WRITE(TRANS_VSYNC(pipe),  I915_READ(VSYNC(pipe)));
 	I915_WRITE(TRANS_VSYNCSHIFT(pipe),  I915_READ(VSYNCSHIFT(pipe)));
 
-	intel_fdi_normal_train(crtc);
+	if (!IS_HASWELL(dev))
+		intel_fdi_normal_train(crtc);
 
 	/* For PCH DP, enable TRANS_DP_CTL */
 	if (HAS_PCH_CPT(dev) &&
@@ -3041,6 +2882,93 @@ static void ironlake_pch_enable(struct drm_crtc *crtc)
 	intel_enable_transcoder(dev_priv, pipe);
 }
 
+static void intel_put_pch_pll(struct intel_crtc *intel_crtc)
+{
+	struct intel_pch_pll *pll = intel_crtc->pch_pll;
+
+	if (pll == NULL)
+		return;
+
+	if (pll->refcount == 0) {
+		WARN(1, "bad PCH PLL refcount\n");
+		return;
+	}
+
+	--pll->refcount;
+	intel_crtc->pch_pll = NULL;
+}
+
+static struct intel_pch_pll *intel_get_pch_pll(struct intel_crtc *intel_crtc, u32 dpll, u32 fp)
+{
+	struct drm_i915_private *dev_priv = intel_crtc->base.dev->dev_private;
+	struct intel_pch_pll *pll;
+	int i;
+
+	pll = intel_crtc->pch_pll;
+	if (pll) {
+		DRM_DEBUG_KMS("CRTC:%d reusing existing PCH PLL %x\n",
+			      intel_crtc->base.base.id, pll->pll_reg);
+		goto prepare;
+	}
+
+	if (HAS_PCH_IBX(dev_priv->dev)) {
+		/* Ironlake PCH has a fixed PLL->PCH pipe mapping. */
+		i = intel_crtc->pipe;
+		pll = &dev_priv->pch_plls[i];
+
+		DRM_DEBUG_KMS("CRTC:%d using pre-allocated PCH PLL %x\n",
+			      intel_crtc->base.base.id, pll->pll_reg);
+
+		goto found;
+	}
+
+	for (i = 0; i < dev_priv->num_pch_pll; i++) {
+		pll = &dev_priv->pch_plls[i];
+
+		/* Only want to check enabled timings first */
+		if (pll->refcount == 0)
+			continue;
+
+		if (dpll == (I915_READ(pll->pll_reg) & 0x7fffffff) &&
+		    fp == I915_READ(pll->fp0_reg)) {
+			DRM_DEBUG_KMS("CRTC:%d sharing existing PCH PLL %x (refcount %d, ative %d)\n",
+				      intel_crtc->base.base.id,
+				      pll->pll_reg, pll->refcount, pll->active);
+
+			goto found;
+		}
+	}
+
+	/* Ok no matching timings, maybe there's a free one? */
+	for (i = 0; i < dev_priv->num_pch_pll; i++) {
+		pll = &dev_priv->pch_plls[i];
+		if (pll->refcount == 0) {
+			DRM_DEBUG_KMS("CRTC:%d allocated PCH PLL %x\n",
+				      intel_crtc->base.base.id, pll->pll_reg);
+			goto found;
+		}
+	}
+
+	return NULL;
+
+found:
+	intel_crtc->pch_pll = pll;
+	pll->refcount++;
+	DRM_DEBUG_DRIVER("using pll %d for pipe %d\n", i, intel_crtc->pipe);
+prepare: /* separate function? */
+	DRM_DEBUG_DRIVER("switching PLL %x off\n", pll->pll_reg);
+
+	/* Wait for the clocks to stabilize before rewriting the regs */
+	I915_WRITE(pll->pll_reg, dpll & ~DPLL_VCO_ENABLE);
+	POSTING_READ(pll->pll_reg);
+	udelay(150);
+
+	I915_WRITE(pll->fp0_reg, fp);
+	I915_WRITE(pll->pll_reg, dpll & ~DPLL_VCO_ENABLE);
+	pll->on = false;
+	return pll;
+}
+
 void intel_cpt_verify_modeset(struct drm_device *dev, int pipe)
 {
 	struct drm_i915_private *dev_priv = dev->dev_private;
@@ -3185,8 +3113,7 @@ static void ironlake_crtc_disable(struct drm_crtc *crtc)
 	}
 
 	/* disable PCH DPLL */
-	if (!intel_crtc->no_pll)
-		intel_disable_pch_pll(dev_priv, pipe);
+	intel_disable_pch_pll(intel_crtc);
 
 	/* Switch from PCDclk to Rawclk */
 	reg = FDI_RX_CTL(pipe);
@@ -3214,7 +3141,6 @@ static void ironlake_crtc_disable(struct drm_crtc *crtc)
 
 	mutex_lock(&dev->struct_mutex);
 	intel_update_fbc(dev);
-	intel_clear_scanline_wait(dev);
 	mutex_unlock(&dev->struct_mutex);
 }
 
@@ -3242,6 +3168,12 @@ static void ironlake_crtc_dpms(struct drm_crtc *crtc, int mode)
 	}
 }
 
+static void ironlake_crtc_off(struct drm_crtc *crtc)
+{
+	struct intel_crtc *intel_crtc = to_intel_crtc(crtc);
+	intel_put_pch_pll(intel_crtc);
+}
+
 static void intel_crtc_dpms_overlay(struct intel_crtc *intel_crtc, bool enable)
 {
 	if (!enable && intel_crtc->overlay) {
@@ -3313,7 +3245,6 @@ static void i9xx_crtc_disable(struct drm_crtc *crtc)
 	intel_crtc->active = false;
 	intel_update_fbc(dev);
 	intel_update_watermarks(dev);
-	intel_clear_scanline_wait(dev);
 }
 
 static void i9xx_crtc_dpms(struct drm_crtc *crtc, int mode)
@@ -3333,6 +3264,10 @@ static void i9xx_crtc_dpms(struct drm_crtc *crtc, int mode)
 	}
 }
 
+static void i9xx_crtc_off(struct drm_crtc *crtc)
+{
+}
+
 /**
  * Sets the power management mode of the pipe and plane.
  */
@@ -3380,25 +3315,11 @@ static void intel_crtc_disable(struct drm_crtc *crtc)
 {
 	struct drm_crtc_helper_funcs *crtc_funcs = crtc->helper_private;
 	struct drm_device *dev = crtc->dev;
-
-	/* Flush any pending WAITs before we disable the pipe. Note that
-	 * we need to drop the struct_mutex in order to acquire it again
-	 * during the lowlevel dpms routines around a couple of the
-	 * operations. It does not look trivial nor desirable to move
-	 * that locking higher. So instead we leave a window for the
-	 * submission of further commands on the fb before we can actually
-	 * disable it. This race with userspace exists anyway, and we can
-	 * only rely on the pipe being disabled by userspace after it
-	 * receives the hotplug notification and has flushed any pending
-	 * batches.
-	 */
-	if (crtc->fb) {
-		mutex_lock(&dev->struct_mutex);
-		intel_finish_fb(crtc->fb);
-		mutex_unlock(&dev->struct_mutex);
-	}
+	struct drm_i915_private *dev_priv = dev->dev_private;
 
 	crtc_funcs->dpms(crtc, DRM_MODE_DPMS_OFF);
+	dev_priv->display.off(crtc);
+
 	assert_plane_disabled(dev->dev_private, to_intel_crtc(crtc)->plane);
 	assert_pipe_disabled(dev->dev_private, to_intel_crtc(crtc)->pipe);
 
@@ -3448,8 +3369,7 @@ void intel_encoder_commit(struct drm_encoder *encoder)
 {
 	struct drm_encoder_helper_funcs *encoder_funcs = encoder->helper_private;
 	struct drm_device *dev = encoder->dev;
-	struct intel_encoder *intel_encoder = to_intel_encoder(encoder);
-	struct intel_crtc *intel_crtc = to_intel_crtc(intel_encoder->base.crtc);
+	struct intel_crtc *intel_crtc = to_intel_crtc(encoder->crtc);
 
 	/* lvds has its own version of commit see intel_lvds_commit */
 	encoder_funcs->dpms(encoder, DRM_MODE_DPMS_ON);
@@ -3487,6 +3407,11 @@ static bool intel_crtc_mode_fixup(struct drm_crtc *crtc,
 	return true;
 }
 
+static int valleyview_get_display_clock_speed(struct drm_device *dev)
+{
+	return 400000; /* FIXME */
+}
+
 static int i945_get_display_clock_speed(struct drm_device *dev)
 {
 	return 400000;
@@ -3584,1342 +3509,6 @@ ironlake_compute_m_n(int bits_per_pixel, int nlanes, int pixel_clock,
 	fdi_reduce_ratio(&m_n->link_m, &m_n->link_n);
 }
 
-
-struct intel_watermark_params {
-	unsigned long fifo_size;
-	unsigned long max_wm;
-	unsigned long default_wm;
-	unsigned long guard_size;
-	unsigned long cacheline_size;
-};
-
-/* Pineview has different values for various configs */
-static const struct intel_watermark_params pineview_display_wm = {
-	PINEVIEW_DISPLAY_FIFO,
-	PINEVIEW_MAX_WM,
-	PINEVIEW_DFT_WM,
-	PINEVIEW_GUARD_WM,
-	PINEVIEW_FIFO_LINE_SIZE
-};
-static const struct intel_watermark_params pineview_display_hplloff_wm = {
-	PINEVIEW_DISPLAY_FIFO,
-	PINEVIEW_MAX_WM,
-	PINEVIEW_DFT_HPLLOFF_WM,
-	PINEVIEW_GUARD_WM,
-	PINEVIEW_FIFO_LINE_SIZE
-};
-static const struct intel_watermark_params pineview_cursor_wm = {
-	PINEVIEW_CURSOR_FIFO,
-	PINEVIEW_CURSOR_MAX_WM,
-	PINEVIEW_CURSOR_DFT_WM,
-	PINEVIEW_CURSOR_GUARD_WM,
-	PINEVIEW_FIFO_LINE_SIZE,
-};
-static const struct intel_watermark_params pineview_cursor_hplloff_wm = {
-	PINEVIEW_CURSOR_FIFO,
-	PINEVIEW_CURSOR_MAX_WM,
-	PINEVIEW_CURSOR_DFT_WM,
-	PINEVIEW_CURSOR_GUARD_WM,
-	PINEVIEW_FIFO_LINE_SIZE
-};
-static const struct intel_watermark_params g4x_wm_info = {
-	G4X_FIFO_SIZE,
-	G4X_MAX_WM,
-	G4X_MAX_WM,
-	2,
-	G4X_FIFO_LINE_SIZE,
-};
-static const struct intel_watermark_params g4x_cursor_wm_info = {
-	I965_CURSOR_FIFO,
-	I965_CURSOR_MAX_WM,
-	I965_CURSOR_DFT_WM,
-	2,
-	G4X_FIFO_LINE_SIZE,
-};
-static const struct intel_watermark_params i965_cursor_wm_info = {
-	I965_CURSOR_FIFO,
-	I965_CURSOR_MAX_WM,
-	I965_CURSOR_DFT_WM,
-	2,
-	I915_FIFO_LINE_SIZE,
-};
-static const struct intel_watermark_params i945_wm_info = {
-	I945_FIFO_SIZE,
-	I915_MAX_WM,
-	1,
-	2,
-	I915_FIFO_LINE_SIZE
-};
-static const struct intel_watermark_params i915_wm_info = {
-	I915_FIFO_SIZE,
-	I915_MAX_WM,
-	1,
-	2,
-	I915_FIFO_LINE_SIZE
-};
-static const struct intel_watermark_params i855_wm_info = {
-	I855GM_FIFO_SIZE,
-	I915_MAX_WM,
-	1,
-	2,
-	I830_FIFO_LINE_SIZE
-};
-static const struct intel_watermark_params i830_wm_info = {
-	I830_FIFO_SIZE,
-	I915_MAX_WM,
-	1,
-	2,
-	I830_FIFO_LINE_SIZE
-};
-
-static const struct intel_watermark_params ironlake_display_wm_info = {
-	ILK_DISPLAY_FIFO,
-	ILK_DISPLAY_MAXWM,
-	ILK_DISPLAY_DFTWM,
-	2,
-	ILK_FIFO_LINE_SIZE
-};
-static const struct intel_watermark_params ironlake_cursor_wm_info = {
-	ILK_CURSOR_FIFO,
-	ILK_CURSOR_MAXWM,
-	ILK_CURSOR_DFTWM,
-	2,
-	ILK_FIFO_LINE_SIZE
-};
-static const struct intel_watermark_params ironlake_display_srwm_info = {
-	ILK_DISPLAY_SR_FIFO,
-	ILK_DISPLAY_MAX_SRWM,
-	ILK_DISPLAY_DFT_SRWM,
-	2,
-	ILK_FIFO_LINE_SIZE
-};
-static const struct intel_watermark_params ironlake_cursor_srwm_info = {
-	ILK_CURSOR_SR_FIFO,
-	ILK_CURSOR_MAX_SRWM,
-	ILK_CURSOR_DFT_SRWM,
-	2,
-	ILK_FIFO_LINE_SIZE
-};
-
-static const struct intel_watermark_params sandybridge_display_wm_info = {
-	SNB_DISPLAY_FIFO,
-	SNB_DISPLAY_MAXWM,
-	SNB_DISPLAY_DFTWM,
-	2,
-	SNB_FIFO_LINE_SIZE
-};
-static const struct intel_watermark_params sandybridge_cursor_wm_info = {
-	SNB_CURSOR_FIFO,
-	SNB_CURSOR_MAXWM,
-	SNB_CURSOR_DFTWM,
-	2,
-	SNB_FIFO_LINE_SIZE
-};
-static const struct intel_watermark_params sandybridge_display_srwm_info = {
-	SNB_DISPLAY_SR_FIFO,
-	SNB_DISPLAY_MAX_SRWM,
-	SNB_DISPLAY_DFT_SRWM,
-	2,
-	SNB_FIFO_LINE_SIZE
-};
-static const struct intel_watermark_params sandybridge_cursor_srwm_info = {
-	SNB_CURSOR_SR_FIFO,
-	SNB_CURSOR_MAX_SRWM,
-	SNB_CURSOR_DFT_SRWM,
-	2,
-	SNB_FIFO_LINE_SIZE
-};
-
-
-/**
- * intel_calculate_wm - calculate watermark level
- * @clock_in_khz: pixel clock
- * @wm: chip FIFO params
- * @pixel_size: display pixel size
- * @latency_ns: memory latency for the platform
- *
- * Calculate the watermark level (the level at which the display plane will
- * start fetching from memory again).  Each chip has a different display
- * FIFO size and allocation, so the caller needs to figure that out and pass
- * in the correct intel_watermark_params structure.
- *
- * As the pixel clock runs, the FIFO will be drained at a rate that depends
- * on the pixel size.  When it reaches the watermark level, it'll start
- * fetching FIFO line sized based chunks from memory until the FIFO fills
- * past the watermark point.  If the FIFO drains completely, a FIFO underrun
- * will occur, and a display engine hang could result.
- */
-static unsigned long intel_calculate_wm(unsigned long clock_in_khz,
-					const struct intel_watermark_params *wm,
-					int fifo_size,
-					int pixel_size,
-					unsigned long latency_ns)
-{
-	long entries_required, wm_size;
-
-	/*
-	 * Note: we need to make sure we don't overflow for various clock &
-	 * latency values.
-	 * clocks go from a few thousand to several hundred thousand.
-	 * latency is usually a few thousand
-	 */
-	entries_required = ((clock_in_khz / 1000) * pixel_size * latency_ns) /
-		1000;
-	entries_required = DIV_ROUND_UP(entries_required, wm->cacheline_size);
-
-	DRM_DEBUG_KMS("FIFO entries required for mode: %ld\n", entries_required);
-
-	wm_size = fifo_size - (entries_required + wm->guard_size);
-
-	DRM_DEBUG_KMS("FIFO watermark level: %ld\n", wm_size);
-
-	/* Don't promote wm_size to unsigned... */
-	if (wm_size > (long)wm->max_wm)
-		wm_size = wm->max_wm;
-	if (wm_size <= 0)
-		wm_size = wm->default_wm;
-	return wm_size;
-}
-
-struct cxsr_latency {
-	int is_desktop;
-	int is_ddr3;
-	unsigned long fsb_freq;
-	unsigned long mem_freq;
-	unsigned long display_sr;
-	unsigned long display_hpll_disable;
-	unsigned long cursor_sr;
-	unsigned long cursor_hpll_disable;
-};
-
-static const struct cxsr_latency cxsr_latency_table[] = {
-	{1, 0, 800, 400, 3382, 33382, 3983, 33983},    /* DDR2-400 SC */
-	{1, 0, 800, 667, 3354, 33354, 3807, 33807},    /* DDR2-667 SC */
-	{1, 0, 800, 800, 3347, 33347, 3763, 33763},    /* DDR2-800 SC */
-	{1, 1, 800, 667, 6420, 36420, 6873, 36873},    /* DDR3-667 SC */
-	{1, 1, 800, 800, 5902, 35902, 6318, 36318},    /* DDR3-800 SC */
-
-	{1, 0, 667, 400, 3400, 33400, 4021, 34021},    /* DDR2-400 SC */
-	{1, 0, 667, 667, 3372, 33372, 3845, 33845},    /* DDR2-667 SC */
-	{1, 0, 667, 800, 3386, 33386, 3822, 33822},    /* DDR2-800 SC */
-	{1, 1, 667, 667, 6438, 36438, 6911, 36911},    /* DDR3-667 SC */
-	{1, 1, 667, 800, 5941, 35941, 6377, 36377},    /* DDR3-800 SC */
-
-	{1, 0, 400, 400, 3472, 33472, 4173, 34173},    /* DDR2-400 SC */
-	{1, 0, 400, 667, 3443, 33443, 3996, 33996},    /* DDR2-667 SC */
-	{1, 0, 400, 800, 3430, 33430, 3946, 33946},    /* DDR2-800 SC */
-	{1, 1, 400, 667, 6509, 36509, 7062, 37062},    /* DDR3-667 SC */
-	{1, 1, 400, 800, 5985, 35985, 6501, 36501},    /* DDR3-800 SC */
-
-	{0, 0, 800, 400, 3438, 33438, 4065, 34065},    /* DDR2-400 SC */
-	{0, 0, 800, 667, 3410, 33410, 3889, 33889},    /* DDR2-667 SC */
-	{0, 0, 800, 800, 3403, 33403, 3845, 33845},    /* DDR2-800 SC */
-	{0, 1, 800, 667, 6476, 36476, 6955, 36955},    /* DDR3-667 SC */
-	{0, 1, 800, 800, 5958, 35958, 6400, 36400},    /* DDR3-800 SC */
-
-	{0, 0, 667, 400, 3456, 33456, 4103, 34106},    /* DDR2-400 SC */
-	{0, 0, 667, 667, 3428, 33428, 3927, 33927},    /* DDR2-667 SC */
-	{0, 0, 667, 800, 3443, 33443, 3905, 33905},    /* DDR2-800 SC */
-	{0, 1, 667, 667, 6494, 36494, 6993, 36993},    /* DDR3-667 SC */
-	{0, 1, 667, 800, 5998, 35998, 6460, 36460},    /* DDR3-800 SC */
-
-	{0, 0, 400, 400, 3528, 33528, 4255, 34255},    /* DDR2-400 SC */
-	{0, 0, 400, 667, 3500, 33500, 4079, 34079},    /* DDR2-667 SC */
-	{0, 0, 400, 800, 3487, 33487, 4029, 34029},    /* DDR2-800 SC */
-	{0, 1, 400, 667, 6566, 36566, 7145, 37145},    /* DDR3-667 SC */
-	{0, 1, 400, 800, 6042, 36042, 6584, 36584},    /* DDR3-800 SC */
-};
-
-static const struct cxsr_latency *intel_get_cxsr_latency(int is_desktop,
-							 int is_ddr3,
-							 int fsb,
-							 int mem)
-{
-	const struct cxsr_latency *latency;
-	int i;
-
-	if (fsb == 0 || mem == 0)
-		return NULL;
-
-	for (i = 0; i < ARRAY_SIZE(cxsr_latency_table); i++) {
-		latency = &cxsr_latency_table[i];
-		if (is_desktop == latency->is_desktop &&
-		    is_ddr3 == latency->is_ddr3 &&
-		    fsb == latency->fsb_freq && mem == latency->mem_freq)
-			return latency;
-	}
-
-	DRM_DEBUG_KMS("Unknown FSB/MEM found, disable CxSR\n");
-
-	return NULL;
-}
-
-static void pineview_disable_cxsr(struct drm_device *dev)
-{
-	struct drm_i915_private *dev_priv = dev->dev_private;
-
-	/* deactivate cxsr */
-	I915_WRITE(DSPFW3, I915_READ(DSPFW3) & ~PINEVIEW_SELF_REFRESH_EN);
-}
-
-/*
- * Latency for FIFO fetches is dependent on several factors:
- *   - memory configuration (speed, channels)
- *   - chipset
- *   - current MCH state
- * It can be fairly high in some situations, so here we assume a fairly
- * pessimal value.  It's a tradeoff between extra memory fetches (if we
- * set this value too high, the FIFO will fetch frequently to stay full)
- * and power consumption (set it too low to save power and we might see
- * FIFO underruns and display "flicker").
- *
- * A value of 5us seems to be a good balance; safe for very low end
- * platforms but not overly aggressive on lower latency configs.
- */
-static const int latency_ns = 5000;
-
-static int i9xx_get_fifo_size(struct drm_device *dev, int plane)
-{
-	struct drm_i915_private *dev_priv = dev->dev_private;
-	uint32_t dsparb = I915_READ(DSPARB);
-	int size;
-
-	size = dsparb & 0x7f;
-	if (plane)
-		size = ((dsparb >> DSPARB_CSTART_SHIFT) & 0x7f) - size;
-
-	DRM_DEBUG_KMS("FIFO size - (0x%08x) %s: %d\n", dsparb,
-		      plane ? "B" : "A", size);
-
-	return size;
-}
-
-static int i85x_get_fifo_size(struct drm_device *dev, int plane)
-{
-	struct drm_i915_private *dev_priv = dev->dev_private;
-	uint32_t dsparb = I915_READ(DSPARB);
-	int size;
-
-	size = dsparb & 0x1ff;
-	if (plane)
-		size = ((dsparb >> DSPARB_BEND_SHIFT) & 0x1ff) - size;
-	size >>= 1; /* Convert to cachelines */
-
-	DRM_DEBUG_KMS("FIFO size - (0x%08x) %s: %d\n", dsparb,
-		      plane ? "B" : "A", size);
-
-	return size;
-}
-
-static int i845_get_fifo_size(struct drm_device *dev, int plane)
-{
-	struct drm_i915_private *dev_priv = dev->dev_private;
-	uint32_t dsparb = I915_READ(DSPARB);
-	int size;
-
-	size = dsparb & 0x7f;
-	size >>= 2; /* Convert to cachelines */
-
-	DRM_DEBUG_KMS("FIFO size - (0x%08x) %s: %d\n", dsparb,
-		      plane ? "B" : "A",
-		      size);
-
-	return size;
-}
-
-static int i830_get_fifo_size(struct drm_device *dev, int plane)
-{
-	struct drm_i915_private *dev_priv = dev->dev_private;
-	uint32_t dsparb = I915_READ(DSPARB);
-	int size;
-
-	size = dsparb & 0x7f;
-	size >>= 1; /* Convert to cachelines */
-
-	DRM_DEBUG_KMS("FIFO size - (0x%08x) %s: %d\n", dsparb,
-		      plane ? "B" : "A", size);
-
-	return size;
-}
-
-static struct drm_crtc *single_enabled_crtc(struct drm_device *dev)
-{
-	struct drm_crtc *crtc, *enabled = NULL;
-
-	list_for_each_entry(crtc, &dev->mode_config.crtc_list, head) {
-		if (crtc->enabled && crtc->fb) {
-			if (enabled)
-				return NULL;
-			enabled = crtc;
-		}
-	}
-
-	return enabled;
-}
-
-static void pineview_update_wm(struct drm_device *dev)
-{
-	struct drm_i915_private *dev_priv = dev->dev_private;
-	struct drm_crtc *crtc;
-	const struct cxsr_latency *latency;
-	u32 reg;
-	unsigned long wm;
-
-	latency = intel_get_cxsr_latency(IS_PINEVIEW_G(dev), dev_priv->is_ddr3,
-					 dev_priv->fsb_freq, dev_priv->mem_freq);
-	if (!latency) {
-		DRM_DEBUG_KMS("Unknown FSB/MEM found, disable CxSR\n");
-		pineview_disable_cxsr(dev);
-		return;
-	}
-
-	crtc = single_enabled_crtc(dev);
-	if (crtc) {
-		int clock = crtc->mode.clock;
-		int pixel_size = crtc->fb->bits_per_pixel / 8;
-
-		/* Display SR */
-		wm = intel_calculate_wm(clock, &pineview_display_wm,
-					pineview_display_wm.fifo_size,
-					pixel_size, latency->display_sr);
-		reg = I915_READ(DSPFW1);
-		reg &= ~DSPFW_SR_MASK;
-		reg |= wm << DSPFW_SR_SHIFT;
-		I915_WRITE(DSPFW1, reg);
-		DRM_DEBUG_KMS("DSPFW1 register is %x\n", reg);
-
-		/* cursor SR */
-		wm = intel_calculate_wm(clock, &pineview_cursor_wm,
-					pineview_display_wm.fifo_size,
-					pixel_size, latency->cursor_sr);
-		reg = I915_READ(DSPFW3);
-		reg &= ~DSPFW_CURSOR_SR_MASK;
-		reg |= (wm & 0x3f) << DSPFW_CURSOR_SR_SHIFT;
-		I915_WRITE(DSPFW3, reg);
-
-		/* Display HPLL off SR */
-		wm = intel_calculate_wm(clock, &pineview_display_hplloff_wm,
-					pineview_display_hplloff_wm.fifo_size,
-					pixel_size, latency->display_hpll_disable);
-		reg = I915_READ(DSPFW3);
-		reg &= ~DSPFW_HPLL_SR_MASK;
-		reg |= wm & DSPFW_HPLL_SR_MASK;
-		I915_WRITE(DSPFW3, reg);
-
-		/* cursor HPLL off SR */
-		wm = intel_calculate_wm(clock, &pineview_cursor_hplloff_wm,
-					pineview_display_hplloff_wm.fifo_size,
-					pixel_size, latency->cursor_hpll_disable);
-		reg = I915_READ(DSPFW3);
-		reg &= ~DSPFW_HPLL_CURSOR_MASK;
-		reg |= (wm & 0x3f) << DSPFW_HPLL_CURSOR_SHIFT;
-		I915_WRITE(DSPFW3, reg);
-		DRM_DEBUG_KMS("DSPFW3 register is %x\n", reg);
-
-		/* activate cxsr */
-		I915_WRITE(DSPFW3,
-			   I915_READ(DSPFW3) | PINEVIEW_SELF_REFRESH_EN);
-		DRM_DEBUG_KMS("Self-refresh is enabled\n");
-	} else {
-		pineview_disable_cxsr(dev);
-		DRM_DEBUG_KMS("Self-refresh is disabled\n");
-	}
-}
-
-static bool g4x_compute_wm0(struct drm_device *dev,
-			    int plane,
-			    const struct intel_watermark_params *display,
-			    int display_latency_ns,
-			    const struct intel_watermark_params *cursor,
-			    int cursor_latency_ns,
-			    int *plane_wm,
-			    int *cursor_wm)
-{
-	struct drm_crtc *crtc;
-	int htotal, hdisplay, clock, pixel_size;
-	int line_time_us, line_count;
-	int entries, tlb_miss;
-
-	crtc = intel_get_crtc_for_plane(dev, plane);
-	if (crtc->fb == NULL || !crtc->enabled) {
-		*cursor_wm = cursor->guard_size;
-		*plane_wm = display->guard_size;
-		return false;
-	}
-
-	htotal = crtc->mode.htotal;
-	hdisplay = crtc->mode.hdisplay;
-	clock = crtc->mode.clock;
-	pixel_size = crtc->fb->bits_per_pixel / 8;
-
-	/* Use the small buffer method to calculate plane watermark */
-	entries = ((clock * pixel_size / 1000) * display_latency_ns) / 1000;
-	tlb_miss = display->fifo_size*display->cacheline_size - hdisplay * 8;
-	if (tlb_miss > 0)
-		entries += tlb_miss;
-	entries = DIV_ROUND_UP(entries, display->cacheline_size);
-	*plane_wm = entries + display->guard_size;
-	if (*plane_wm > (int)display->max_wm)
-		*plane_wm = display->max_wm;
-
-	/* Use the large buffer method to calculate cursor watermark */
-	line_time_us = ((htotal * 1000) / clock);
-	line_count = (cursor_latency_ns / line_time_us + 1000) / 1000;
-	entries = line_count * 64 * pixel_size;
-	tlb_miss = cursor->fifo_size*cursor->cacheline_size - hdisplay * 8;
-	if (tlb_miss > 0)
-		entries += tlb_miss;
-	entries = DIV_ROUND_UP(entries, cursor->cacheline_size);
-	*cursor_wm = entries + cursor->guard_size;
-	if (*cursor_wm > (int)cursor->max_wm)
-		*cursor_wm = (int)cursor->max_wm;
-
-	return true;
-}
-
-/*
- * Check the wm result.
- *
- * If any calculated watermark values is larger than the maximum value that
- * can be programmed into the associated watermark register, that watermark
- * must be disabled.
- */
-static bool g4x_check_srwm(struct drm_device *dev,
-			   int display_wm, int cursor_wm,
-			   const struct intel_watermark_params *display,
-			   const struct intel_watermark_params *cursor)
-{
-	DRM_DEBUG_KMS("SR watermark: display plane %d, cursor %d\n",
-		      display_wm, cursor_wm);
-
-	if (display_wm > display->max_wm) {
-		DRM_DEBUG_KMS("display watermark is too large(%d/%ld), disabling\n",
-			      display_wm, display->max_wm);
-		return false;
-	}
-
-	if (cursor_wm > cursor->max_wm) {
-		DRM_DEBUG_KMS("cursor watermark is too large(%d/%ld), disabling\n",
-			      cursor_wm, cursor->max_wm);
-		return false;
-	}
-
-	if (!(display_wm || cursor_wm)) {
-		DRM_DEBUG_KMS("SR latency is 0, disabling\n");
-		return false;
-	}
-
-	return true;
-}
-
-static bool g4x_compute_srwm(struct drm_device *dev,
-			     int plane,
-			     int latency_ns,
-			     const struct intel_watermark_params *display,
-			     const struct intel_watermark_params *cursor,
-			     int *display_wm, int *cursor_wm)
-{
-	struct drm_crtc *crtc;
-	int hdisplay, htotal, pixel_size, clock;
-	unsigned long line_time_us;
-	int line_count, line_size;
-	int small, large;
-	int entries;
-
-	if (!latency_ns) {
-		*display_wm = *cursor_wm = 0;
-		return false;
-	}
-
-	crtc = intel_get_crtc_for_plane(dev, plane);
-	hdisplay = crtc->mode.hdisplay;
-	htotal = crtc->mode.htotal;
-	clock = crtc->mode.clock;
-	pixel_size = crtc->fb->bits_per_pixel / 8;
-
-	line_time_us = (htotal * 1000) / clock;
-	line_count = (latency_ns / line_time_us + 1000) / 1000;
-	line_size = hdisplay * pixel_size;
-
-	/* Use the minimum of the small and large buffer method for primary */
-	small = ((clock * pixel_size / 1000) * latency_ns) / 1000;
-	large = line_count * line_size;
-
-	entries = DIV_ROUND_UP(min(small, large), display->cacheline_size);
-	*display_wm = entries + display->guard_size;
-
-	/* calculate the self-refresh watermark for display cursor */
-	entries = line_count * pixel_size * 64;
-	entries = DIV_ROUND_UP(entries, cursor->cacheline_size);
-	*cursor_wm = entries + cursor->guard_size;
-
-	return g4x_check_srwm(dev,
-			      *display_wm, *cursor_wm,
-			      display, cursor);
-}
-
-#define single_plane_enabled(mask) is_power_of_2(mask)
-
-static void g4x_update_wm(struct drm_device *dev)
-{
-	static const int sr_latency_ns = 12000;
-	struct drm_i915_private *dev_priv = dev->dev_private;
-	int planea_wm, planeb_wm, cursora_wm, cursorb_wm;
-	int plane_sr, cursor_sr;
-	unsigned int enabled = 0;
-
-	if (g4x_compute_wm0(dev, 0,
-			    &g4x_wm_info, latency_ns,
-			    &g4x_cursor_wm_info, latency_ns,
-			    &planea_wm, &cursora_wm))
-		enabled |= 1;
-
-	if (g4x_compute_wm0(dev, 1,
-			    &g4x_wm_info, latency_ns,
-			    &g4x_cursor_wm_info, latency_ns,
-			    &planeb_wm, &cursorb_wm))
-		enabled |= 2;
-
-	plane_sr = cursor_sr = 0;
-	if (single_plane_enabled(enabled) &&
-	    g4x_compute_srwm(dev, ffs(enabled) - 1,
-			     sr_latency_ns,
-			     &g4x_wm_info,
-			     &g4x_cursor_wm_info,
-			     &plane_sr, &cursor_sr))
-		I915_WRITE(FW_BLC_SELF, FW_BLC_SELF_EN);
-	else
-		I915_WRITE(FW_BLC_SELF,
-			   I915_READ(FW_BLC_SELF) & ~FW_BLC_SELF_EN);
-
-	DRM_DEBUG_KMS("Setting FIFO watermarks - A: plane=%d, cursor=%d, B: plane=%d, cursor=%d, SR: plane=%d, cursor=%d\n",
-		      planea_wm, cursora_wm,
-		      planeb_wm, cursorb_wm,
-		      plane_sr, cursor_sr);
-
-	I915_WRITE(DSPFW1,
-		   (plane_sr << DSPFW_SR_SHIFT) |
-		   (cursorb_wm << DSPFW_CURSORB_SHIFT) |
-		   (planeb_wm << DSPFW_PLANEB_SHIFT) |
-		   planea_wm);
-	I915_WRITE(DSPFW2,
-		   (I915_READ(DSPFW2) & DSPFW_CURSORA_MASK) |
-		   (cursora_wm << DSPFW_CURSORA_SHIFT));
-	/* HPLL off in SR has some issues on G4x... disable it */
-	I915_WRITE(DSPFW3,
-		   (I915_READ(DSPFW3) & ~DSPFW_HPLL_SR_EN) |
-		   (cursor_sr << DSPFW_CURSOR_SR_SHIFT));
-}
-
-static void i965_update_wm(struct drm_device *dev)
-{
-	struct drm_i915_private *dev_priv = dev->dev_private;
-	struct drm_crtc *crtc;
-	int srwm = 1;
-	int cursor_sr = 16;
-
-	/* Calc sr entries for one plane configs */
-	crtc = single_enabled_crtc(dev);
-	if (crtc) {
-		/* self-refresh has much higher latency */
-		static const int sr_latency_ns = 12000;
-		int clock = crtc->mode.clock;
-		int htotal = crtc->mode.htotal;
-		int hdisplay = crtc->mode.hdisplay;
-		int pixel_size = crtc->fb->bits_per_pixel / 8;
-		unsigned long line_time_us;
-		int entries;
-
-		line_time_us = ((htotal * 1000) / clock);
-
-		/* Use ns/us then divide to preserve precision */
-		entries = (((sr_latency_ns / line_time_us) + 1000) / 1000) *
-			pixel_size * hdisplay;
-		entries = DIV_ROUND_UP(entries, I915_FIFO_LINE_SIZE);
-		srwm = I965_FIFO_SIZE - entries;
-		if (srwm < 0)
-			srwm = 1;
-		srwm &= 0x1ff;
-		DRM_DEBUG_KMS("self-refresh entries: %d, wm: %d\n",
-			      entries, srwm);
-
-		entries = (((sr_latency_ns / line_time_us) + 1000) / 1000) *
-			pixel_size * 64;
-		entries = DIV_ROUND_UP(entries,
-					  i965_cursor_wm_info.cacheline_size);
-		cursor_sr = i965_cursor_wm_info.fifo_size -
-			(entries + i965_cursor_wm_info.guard_size);
-
-		if (cursor_sr > i965_cursor_wm_info.max_wm)
-			cursor_sr = i965_cursor_wm_info.max_wm;
-
-		DRM_DEBUG_KMS("self-refresh watermark: display plane %d "
-			      "cursor %d\n", srwm, cursor_sr);
-
-		if (IS_CRESTLINE(dev))
-			I915_WRITE(FW_BLC_SELF, FW_BLC_SELF_EN);
-	} else {
-		/* Turn off self refresh if both pipes are enabled */
-		if (IS_CRESTLINE(dev))
-			I915_WRITE(FW_BLC_SELF, I915_READ(FW_BLC_SELF)
-				   & ~FW_BLC_SELF_EN);
-	}
-
-	DRM_DEBUG_KMS("Setting FIFO watermarks - A: 8, B: 8, C: 8, SR %d\n",
-		      srwm);
-
-	/* 965 has limitations... */
-	I915_WRITE(DSPFW1, (srwm << DSPFW_SR_SHIFT) |
-		   (8 << 16) | (8 << 8) | (8 << 0));
-	I915_WRITE(DSPFW2, (8 << 8) | (8 << 0));
-	/* update cursor SR watermark */
-	I915_WRITE(DSPFW3, (cursor_sr << DSPFW_CURSOR_SR_SHIFT));
-}
-
-static void i9xx_update_wm(struct drm_device *dev)
-{
-	struct drm_i915_private *dev_priv = dev->dev_private;
-	const struct intel_watermark_params *wm_info;
-	uint32_t fwater_lo;
-	uint32_t fwater_hi;
-	int cwm, srwm = 1;
-	int fifo_size;
-	int planea_wm, planeb_wm;
-	struct drm_crtc *crtc, *enabled = NULL;
-
-	if (IS_I945GM(dev))
-		wm_info = &i945_wm_info;
-	else if (!IS_GEN2(dev))
-		wm_info = &i915_wm_info;
-	else
-		wm_info = &i855_wm_info;
-
-	fifo_size = dev_priv->display.get_fifo_size(dev, 0);
-	crtc = intel_get_crtc_for_plane(dev, 0);
-	if (crtc->enabled && crtc->fb) {
-		planea_wm = intel_calculate_wm(crtc->mode.clock,
-					       wm_info, fifo_size,
-					       crtc->fb->bits_per_pixel / 8,
-					       latency_ns);
-		enabled = crtc;
-	} else
-		planea_wm = fifo_size - wm_info->guard_size;
-
-	fifo_size = dev_priv->display.get_fifo_size(dev, 1);
-	crtc = intel_get_crtc_for_plane(dev, 1);
-	if (crtc->enabled && crtc->fb) {
-		planeb_wm = intel_calculate_wm(crtc->mode.clock,
-					       wm_info, fifo_size,
-					       crtc->fb->bits_per_pixel / 8,
-					       latency_ns);
-		if (enabled == NULL)
-			enabled = crtc;
-		else
-			enabled = NULL;
-	} else
-		planeb_wm = fifo_size - wm_info->guard_size;
-
-	DRM_DEBUG_KMS("FIFO watermarks - A: %d, B: %d\n", planea_wm, planeb_wm);
-
-	/*
-	 * Overlay gets an aggressive default since video jitter is bad.
-	 */
-	cwm = 2;
-
-	/* Play safe and disable self-refresh before adjusting watermarks. */
-	if (IS_I945G(dev) || IS_I945GM(dev))
-		I915_WRITE(FW_BLC_SELF, FW_BLC_SELF_EN_MASK | 0);
-	else if (IS_I915GM(dev))
-		I915_WRITE(INSTPM, I915_READ(INSTPM) & ~INSTPM_SELF_EN);
-
-	/* Calc sr entries for one plane configs */
-	if (HAS_FW_BLC(dev) && enabled) {
-		/* self-refresh has much higher latency */
-		static const int sr_latency_ns = 6000;
-		int clock = enabled->mode.clock;
-		int htotal = enabled->mode.htotal;
-		int hdisplay = enabled->mode.hdisplay;
-		int pixel_size = enabled->fb->bits_per_pixel / 8;
-		unsigned long line_time_us;
-		int entries;
-
-		line_time_us = (htotal * 1000) / clock;
-
-		/* Use ns/us then divide to preserve precision */
-		entries = (((sr_latency_ns / line_time_us) + 1000) / 1000) *
-			pixel_size * hdisplay;
-		entries = DIV_ROUND_UP(entries, wm_info->cacheline_size);
-		DRM_DEBUG_KMS("self-refresh entries: %d\n", entries);
-		srwm = wm_info->fifo_size - entries;
-		if (srwm < 0)
-			srwm = 1;
-
-		if (IS_I945G(dev) || IS_I945GM(dev))
-			I915_WRITE(FW_BLC_SELF,
-				   FW_BLC_SELF_FIFO_MASK | (srwm & 0xff));
-		else if (IS_I915GM(dev))
-			I915_WRITE(FW_BLC_SELF, srwm & 0x3f);
-	}
-
-	DRM_DEBUG_KMS("Setting FIFO watermarks - A: %d, B: %d, C: %d, SR %d\n",
-		      planea_wm, planeb_wm, cwm, srwm);
-
-	fwater_lo = ((planeb_wm & 0x3f) << 16) | (planea_wm & 0x3f);
-	fwater_hi = (cwm & 0x1f);
-
-	/* Set request length to 8 cachelines per fetch */
-	fwater_lo = fwater_lo | (1 << 24) | (1 << 8);
-	fwater_hi = fwater_hi | (1 << 8);
-
-	I915_WRITE(FW_BLC, fwater_lo);
-	I915_WRITE(FW_BLC2, fwater_hi);
-
-	if (HAS_FW_BLC(dev)) {
-		if (enabled) {
-			if (IS_I945G(dev) || IS_I945GM(dev))
-				I915_WRITE(FW_BLC_SELF,
-					   FW_BLC_SELF_EN_MASK | FW_BLC_SELF_EN);
-			else if (IS_I915GM(dev))
-				I915_WRITE(INSTPM, I915_READ(INSTPM) | INSTPM_SELF_EN);
-			DRM_DEBUG_KMS("memory self refresh enabled\n");
-		} else
-			DRM_DEBUG_KMS("memory self refresh disabled\n");
-	}
-}
-
-static void i830_update_wm(struct drm_device *dev)
-{
-	struct drm_i915_private *dev_priv = dev->dev_private;
-	struct drm_crtc *crtc;
-	uint32_t fwater_lo;
-	int planea_wm;
-
-	crtc = single_enabled_crtc(dev);
-	if (crtc == NULL)
-		return;
-
-	planea_wm = intel_calculate_wm(crtc->mode.clock, &i830_wm_info,
-				       dev_priv->display.get_fifo_size(dev, 0),
-				       crtc->fb->bits_per_pixel / 8,
-				       latency_ns);
-	fwater_lo = I915_READ(FW_BLC) & ~0xfff;
-	fwater_lo |= (3<<8) | planea_wm;
-
-	DRM_DEBUG_KMS("Setting FIFO watermarks - A: %d\n", planea_wm);
-
-	I915_WRITE(FW_BLC, fwater_lo);
-}
-
-#define ILK_LP0_PLANE_LATENCY		700
-#define ILK_LP0_CURSOR_LATENCY		1300
-
-/*
- * Check the wm result.
- *
- * If any calculated watermark values is larger than the maximum value that
- * can be programmed into the associated watermark register, that watermark
- * must be disabled.
- */
-static bool ironlake_check_srwm(struct drm_device *dev, int level,
-				int fbc_wm, int display_wm, int cursor_wm,
-				const struct intel_watermark_params *display,
-				const struct intel_watermark_params *cursor)
-{
-	struct drm_i915_private *dev_priv = dev->dev_private;
-
-	DRM_DEBUG_KMS("watermark %d: display plane %d, fbc lines %d,"
-		      " cursor %d\n", level, display_wm, fbc_wm, cursor_wm);
-
-	if (fbc_wm > SNB_FBC_MAX_SRWM) {
-		DRM_DEBUG_KMS("fbc watermark(%d) is too large(%d), disabling wm%d+\n",
-			      fbc_wm, SNB_FBC_MAX_SRWM, level);
-
-		/* fbc has it's own way to disable FBC WM */
-		I915_WRITE(DISP_ARB_CTL,
-			   I915_READ(DISP_ARB_CTL) | DISP_FBC_WM_DIS);
-		return false;
-	}
-
-	if (display_wm > display->max_wm) {
-		DRM_DEBUG_KMS("display watermark(%d) is too large(%d), disabling wm%d+\n",
-			      display_wm, SNB_DISPLAY_MAX_SRWM, level);
-		return false;
-	}
-
-	if (cursor_wm > cursor->max_wm) {
-		DRM_DEBUG_KMS("cursor watermark(%d) is too large(%d), disabling wm%d+\n",
-			      cursor_wm, SNB_CURSOR_MAX_SRWM, level);
-		return false;
-	}
-
-	if (!(fbc_wm || display_wm || cursor_wm)) {
-		DRM_DEBUG_KMS("latency %d is 0, disabling wm%d+\n", level, level);
-		return false;
-	}
-
-	return true;
-}
-
-/*
- * Compute watermark values of WM[1-3],
- */
-static bool ironlake_compute_srwm(struct drm_device *dev, int level, int plane,
-				  int latency_ns,
-				  const struct intel_watermark_params *display,
-				  const struct intel_watermark_params *cursor,
-				  int *fbc_wm, int *display_wm, int *cursor_wm)
-{
-	struct drm_crtc *crtc;
-	unsigned long line_time_us;
-	int hdisplay, htotal, pixel_size, clock;
-	int line_count, line_size;
-	int small, large;
-	int entries;
-
-	if (!latency_ns) {
-		*fbc_wm = *display_wm = *cursor_wm = 0;
-		return false;
-	}
-
-	crtc = intel_get_crtc_for_plane(dev, plane);
-	hdisplay = crtc->mode.hdisplay;
-	htotal = crtc->mode.htotal;
-	clock = crtc->mode.clock;
-	pixel_size = crtc->fb->bits_per_pixel / 8;
-
-	line_time_us = (htotal * 1000) / clock;
-	line_count = (latency_ns / line_time_us + 1000) / 1000;
-	line_size = hdisplay * pixel_size;
-
-	/* Use the minimum of the small and large buffer method for primary */
-	small = ((clock * pixel_size / 1000) * latency_ns) / 1000;
-	large = line_count * line_size;
-
-	entries = DIV_ROUND_UP(min(small, large), display->cacheline_size);
-	*display_wm = entries + display->guard_size;
-
-	/*
-	 * Spec says:
-	 * FBC WM = ((Final Primary WM * 64) / number of bytes per line) + 2
-	 */
-	*fbc_wm = DIV_ROUND_UP(*display_wm * 64, line_size) + 2;
-
-	/* calculate the self-refresh watermark for display cursor */
-	entries = line_count * pixel_size * 64;
-	entries = DIV_ROUND_UP(entries, cursor->cacheline_size);
-	*cursor_wm = entries + cursor->guard_size;
-
-	return ironlake_check_srwm(dev, level,
-				   *fbc_wm, *display_wm, *cursor_wm,
-				   display, cursor);
-}
-
-static void ironlake_update_wm(struct drm_device *dev)
-{
-	struct drm_i915_private *dev_priv = dev->dev_private;
-	int fbc_wm, plane_wm, cursor_wm;
-	unsigned int enabled;
-
-	enabled = 0;
-	if (g4x_compute_wm0(dev, 0,
-			    &ironlake_display_wm_info,
-			    ILK_LP0_PLANE_LATENCY,
-			    &ironlake_cursor_wm_info,
-			    ILK_LP0_CURSOR_LATENCY,
-			    &plane_wm, &cursor_wm)) {
-		I915_WRITE(WM0_PIPEA_ILK,
-			   (plane_wm << WM0_PIPE_PLANE_SHIFT) | cursor_wm);
-		DRM_DEBUG_KMS("FIFO watermarks For pipe A -"
-			      " plane %d, " "cursor: %d\n",
-			      plane_wm, cursor_wm);
-		enabled |= 1;
-	}
-
-	if (g4x_compute_wm0(dev, 1,
-			    &ironlake_display_wm_info,
-			    ILK_LP0_PLANE_LATENCY,
-			    &ironlake_cursor_wm_info,
-			    ILK_LP0_CURSOR_LATENCY,
-			    &plane_wm, &cursor_wm)) {
-		I915_WRITE(WM0_PIPEB_ILK,
-			   (plane_wm << WM0_PIPE_PLANE_SHIFT) | cursor_wm);
-		DRM_DEBUG_KMS("FIFO watermarks For pipe B -"
-			      " plane %d, cursor: %d\n",
-			      plane_wm, cursor_wm);
-		enabled |= 2;
-	}
-
-	/*
-	 * Calculate and update the self-refresh watermark only when one
-	 * display plane is used.
-	 */
-	I915_WRITE(WM3_LP_ILK, 0);
-	I915_WRITE(WM2_LP_ILK, 0);
-	I915_WRITE(WM1_LP_ILK, 0);
-
-	if (!single_plane_enabled(enabled))
-		return;
-	enabled = ffs(enabled) - 1;
-
-	/* WM1 */
-	if (!ironlake_compute_srwm(dev, 1, enabled,
-				   ILK_READ_WM1_LATENCY() * 500,
-				   &ironlake_display_srwm_info,
-				   &ironlake_cursor_srwm_info,
-				   &fbc_wm, &plane_wm, &cursor_wm))
-		return;
-
-	I915_WRITE(WM1_LP_ILK,
-		   WM1_LP_SR_EN |
-		   (ILK_READ_WM1_LATENCY() << WM1_LP_LATENCY_SHIFT) |
-		   (fbc_wm << WM1_LP_FBC_SHIFT) |
-		   (plane_wm << WM1_LP_SR_SHIFT) |
-		   cursor_wm);
-
-	/* WM2 */
-	if (!ironlake_compute_srwm(dev, 2, enabled,
-				   ILK_READ_WM2_LATENCY() * 500,
-				   &ironlake_display_srwm_info,
-				   &ironlake_cursor_srwm_info,
-				   &fbc_wm, &plane_wm, &cursor_wm))
-		return;
-
-	I915_WRITE(WM2_LP_ILK,
-		   WM2_LP_EN |
-		   (ILK_READ_WM2_LATENCY() << WM1_LP_LATENCY_SHIFT) |
-		   (fbc_wm << WM1_LP_FBC_SHIFT) |
-		   (plane_wm << WM1_LP_SR_SHIFT) |
-		   cursor_wm);
-
-	/*
-	 * WM3 is unsupported on ILK, probably because we don't have latency
-	 * data for that power state
-	 */
-}
-
-void sandybridge_update_wm(struct drm_device *dev)
-{
-	struct drm_i915_private *dev_priv = dev->dev_private;
-	int latency = SNB_READ_WM0_LATENCY() * 100;	/* In unit 0.1us */
-	u32 val;
-	int fbc_wm, plane_wm, cursor_wm;
-	unsigned int enabled;
-
-	enabled = 0;
-	if (g4x_compute_wm0(dev, 0,
-			    &sandybridge_display_wm_info, latency,
-			    &sandybridge_cursor_wm_info, latency,
-			    &plane_wm, &cursor_wm)) {
-		val = I915_READ(WM0_PIPEA_ILK);
-		val &= ~(WM0_PIPE_PLANE_MASK | WM0_PIPE_CURSOR_MASK);
-		I915_WRITE(WM0_PIPEA_ILK, val |
-			   ((plane_wm << WM0_PIPE_PLANE_SHIFT) | cursor_wm));
-		DRM_DEBUG_KMS("FIFO watermarks For pipe A -"
-			      " plane %d, " "cursor: %d\n",
-			      plane_wm, cursor_wm);
-		enabled |= 1;
-	}
-
-	if (g4x_compute_wm0(dev, 1,
-			    &sandybridge_display_wm_info, latency,
-			    &sandybridge_cursor_wm_info, latency,
-			    &plane_wm, &cursor_wm)) {
-		val = I915_READ(WM0_PIPEB_ILK);
-		val &= ~(WM0_PIPE_PLANE_MASK | WM0_PIPE_CURSOR_MASK);
-		I915_WRITE(WM0_PIPEB_ILK, val |
-			   ((plane_wm << WM0_PIPE_PLANE_SHIFT) | cursor_wm));
-		DRM_DEBUG_KMS("FIFO watermarks For pipe B -"
-			      " plane %d, cursor: %d\n",
-			      plane_wm, cursor_wm);
-		enabled |= 2;
-	}
-
-	/* IVB has 3 pipes */
-	if (IS_IVYBRIDGE(dev) &&
-	    g4x_compute_wm0(dev, 2,
-			    &sandybridge_display_wm_info, latency,
-			    &sandybridge_cursor_wm_info, latency,
-			    &plane_wm, &cursor_wm)) {
-		val = I915_READ(WM0_PIPEC_IVB);
-		val &= ~(WM0_PIPE_PLANE_MASK | WM0_PIPE_CURSOR_MASK);
-		I915_WRITE(WM0_PIPEC_IVB, val |
-			   ((plane_wm << WM0_PIPE_PLANE_SHIFT) | cursor_wm));
-		DRM_DEBUG_KMS("FIFO watermarks For pipe C -"
-			      " plane %d, cursor: %d\n",
-			      plane_wm, cursor_wm);
-		enabled |= 3;
-	}
-
-	/*
-	 * Calculate and update the self-refresh watermark only when one
-	 * display plane is used.
-	 *
-	 * SNB support 3 levels of watermark.
-	 *
-	 * WM1/WM2/WM2 watermarks have to be enabled in the ascending order,
-	 * and disabled in the descending order
-	 *
-	 */
-	I915_WRITE(WM3_LP_ILK, 0);
-	I915_WRITE(WM2_LP_ILK, 0);
-	I915_WRITE(WM1_LP_ILK, 0);
-
-	if (!single_plane_enabled(enabled) ||
-	    dev_priv->sprite_scaling_enabled)
-		return;
-	enabled = ffs(enabled) - 1;
-
-	/* WM1 */
-	if (!ironlake_compute_srwm(dev, 1, enabled,
-				   SNB_READ_WM1_LATENCY() * 500,
-				   &sandybridge_display_srwm_info,
-				   &sandybridge_cursor_srwm_info,
-				   &fbc_wm, &plane_wm, &cursor_wm))
-		return;
-
-	I915_WRITE(WM1_LP_ILK,
-		   WM1_LP_SR_EN |
-		   (SNB_READ_WM1_LATENCY() << WM1_LP_LATENCY_SHIFT) |
-		   (fbc_wm << WM1_LP_FBC_SHIFT) |
-		   (plane_wm << WM1_LP_SR_SHIFT) |
-		   cursor_wm);
-
-	/* WM2 */
-	if (!ironlake_compute_srwm(dev, 2, enabled,
-				   SNB_READ_WM2_LATENCY() * 500,
-				   &sandybridge_display_srwm_info,
-				   &sandybridge_cursor_srwm_info,
-				   &fbc_wm, &plane_wm, &cursor_wm))
-		return;
-
-	I915_WRITE(WM2_LP_ILK,
-		   WM2_LP_EN |
-		   (SNB_READ_WM2_LATENCY() << WM1_LP_LATENCY_SHIFT) |
-		   (fbc_wm << WM1_LP_FBC_SHIFT) |
-		   (plane_wm << WM1_LP_SR_SHIFT) |
-		   cursor_wm);
-
-	/* WM3 */
-	if (!ironlake_compute_srwm(dev, 3, enabled,
-				   SNB_READ_WM3_LATENCY() * 500,
-				   &sandybridge_display_srwm_info,
-				   &sandybridge_cursor_srwm_info,
-				   &fbc_wm, &plane_wm, &cursor_wm))
-		return;
-
-	I915_WRITE(WM3_LP_ILK,
-		   WM3_LP_EN |
-		   (SNB_READ_WM3_LATENCY() << WM1_LP_LATENCY_SHIFT) |
-		   (fbc_wm << WM1_LP_FBC_SHIFT) |
-		   (plane_wm << WM1_LP_SR_SHIFT) |
-		   cursor_wm);
-}
-
-static bool
-sandybridge_compute_sprite_wm(struct drm_device *dev, int plane,
-			      uint32_t sprite_width, int pixel_size,
-			      const struct intel_watermark_params *display,
-			      int display_latency_ns, int *sprite_wm)
-{
-	struct drm_crtc *crtc;
-	int clock;
-	int entries, tlb_miss;
-
-	crtc = intel_get_crtc_for_plane(dev, plane);
-	if (crtc->fb == NULL || !crtc->enabled) {
-		*sprite_wm = display->guard_size;
-		return false;
-	}
-
-	clock = crtc->mode.clock;
-
-	/* Use the small buffer method to calculate the sprite watermark */
-	entries = ((clock * pixel_size / 1000) * display_latency_ns) / 1000;
-	tlb_miss = display->fifo_size*display->cacheline_size -
-		sprite_width * 8;
-	if (tlb_miss > 0)
-		entries += tlb_miss;
-	entries = DIV_ROUND_UP(entries, display->cacheline_size);
-	*sprite_wm = entries + display->guard_size;
-	if (*sprite_wm > (int)display->max_wm)
-		*sprite_wm = display->max_wm;
-
-	return true;
-}
-
-static bool
-sandybridge_compute_sprite_srwm(struct drm_device *dev, int plane,
-				uint32_t sprite_width, int pixel_size,
-				const struct intel_watermark_params *display,
-				int latency_ns, int *sprite_wm)
-{
-	struct drm_crtc *crtc;
-	unsigned long line_time_us;
-	int clock;
-	int line_count, line_size;
-	int small, large;
-	int entries;
-
-	if (!latency_ns) {
-		*sprite_wm = 0;
-		return false;
-	}
-
-	crtc = intel_get_crtc_for_plane(dev, plane);
-	clock = crtc->mode.clock;
-	if (!clock) {
-		*sprite_wm = 0;
-		return false;
-	}
-
-	line_time_us = (sprite_width * 1000) / clock;
-	if (!line_time_us) {
-		*sprite_wm = 0;
-		return false;
-	}
-
-	line_count = (latency_ns / line_time_us + 1000) / 1000;
-	line_size = sprite_width * pixel_size;
-
-	/* Use the minimum of the small and large buffer method for primary */
-	small = ((clock * pixel_size / 1000) * latency_ns) / 1000;
-	large = line_count * line_size;
-
-	entries = DIV_ROUND_UP(min(small, large), display->cacheline_size);
-	*sprite_wm = entries + display->guard_size;
-
-	return *sprite_wm > 0x3ff ? false : true;
-}
-
-static void sandybridge_update_sprite_wm(struct drm_device *dev, int pipe,
-					 uint32_t sprite_width, int pixel_size)
-{
-	struct drm_i915_private *dev_priv = dev->dev_private;
-	int latency = SNB_READ_WM0_LATENCY() * 100;	/* In unit 0.1us */
-	u32 val;
-	int sprite_wm, reg;
-	int ret;
-
-	switch (pipe) {
-	case 0:
-		reg = WM0_PIPEA_ILK;
-		break;
-	case 1:
-		reg = WM0_PIPEB_ILK;
-		break;
-	case 2:
-		reg = WM0_PIPEC_IVB;
-		break;
-	default:
-		return; /* bad pipe */
-	}
-
-	ret = sandybridge_compute_sprite_wm(dev, pipe, sprite_width, pixel_size,
-					    &sandybridge_display_wm_info,
-					    latency, &sprite_wm);
-	if (!ret) {
-		DRM_DEBUG_KMS("failed to compute sprite wm for pipe %d\n",
-			      pipe);
-		return;
-	}
-
-	val = I915_READ(reg);
-	val &= ~WM0_PIPE_SPRITE_MASK;
-	I915_WRITE(reg, val | (sprite_wm << WM0_PIPE_SPRITE_SHIFT));
-	DRM_DEBUG_KMS("sprite watermarks For pipe %d - %d\n", pipe, sprite_wm);
-
-
-	ret = sandybridge_compute_sprite_srwm(dev, pipe, sprite_width,
-					      pixel_size,
-					      &sandybridge_display_srwm_info,
-					      SNB_READ_WM1_LATENCY() * 500,
-					      &sprite_wm);
-	if (!ret) {
-		DRM_DEBUG_KMS("failed to compute sprite lp1 wm on pipe %d\n",
-			      pipe);
-		return;
-	}
-	I915_WRITE(WM1S_LP_ILK, sprite_wm);
-
-	/* Only IVB has two more LP watermarks for sprite */
-	if (!IS_IVYBRIDGE(dev))
-		return;
-
-	ret = sandybridge_compute_sprite_srwm(dev, pipe, sprite_width,
-					      pixel_size,
-					      &sandybridge_display_srwm_info,
-					      SNB_READ_WM2_LATENCY() * 500,
-					      &sprite_wm);
-	if (!ret) {
-		DRM_DEBUG_KMS("failed to compute sprite lp2 wm on pipe %d\n",
-			      pipe);
-		return;
-	}
-	I915_WRITE(WM2S_LP_IVB, sprite_wm);
-
-	ret = sandybridge_compute_sprite_srwm(dev, pipe, sprite_width,
-					      pixel_size,
-					      &sandybridge_display_srwm_info,
-					      SNB_READ_WM3_LATENCY() * 500,
-					      &sprite_wm);
-	if (!ret) {
-		DRM_DEBUG_KMS("failed to compute sprite lp3 wm on pipe %d\n",
-			      pipe);
-		return;
-	}
-	I915_WRITE(WM3S_LP_IVB, sprite_wm);
-}
-
-/**
- * intel_update_watermarks - update FIFO watermark values based on current modes
- *
- * Calculate watermark values for the various WM regs based on current mode
- * and plane configuration.
- *
- * There are several cases to deal with here:
- *   - normal (i.e. non-self-refresh)
- *   - self-refresh (SR) mode
- *   - lines are large relative to FIFO size (buffer can hold up to 2)
- *   - lines are small relative to FIFO size (buffer can hold more than 2
- *     lines), so need to account for TLB latency
- *
- *   The normal calculation is:
- *     watermark = dotclock * bytes per pixel * latency
- *   where latency is platform & configuration dependent (we assume pessimal
- *   values here).
- *
- *   The SR calculation is:
- *     watermark = (trunc(latency/line time)+1) * surface width *
- *       bytes per pixel
- *   where
- *     line time = htotal / dotclock
- *     surface width = hdisplay for normal plane and 64 for cursor
- *   and latency is assumed to be high, as above.
- *
- * The final value programmed to the register should always be rounded up,
- * and include an extra 2 entries to account for clock crossings.
- *
- * We don't use the sprite, so we can ignore that.  And on Crestline we have
- * to set the non-SR watermarks to 8.
- */
-static void intel_update_watermarks(struct drm_device *dev)
-{
-	struct drm_i915_private *dev_priv = dev->dev_private;
-
-	if (dev_priv->display.update_wm)
-		dev_priv->display.update_wm(dev);
-}
-
-void intel_update_sprite_watermarks(struct drm_device *dev, int pipe,
-				    uint32_t sprite_width, int pixel_size)
-{
-	struct drm_i915_private *dev_priv = dev->dev_private;
-
-	if (dev_priv->display.update_sprite_wm)
-		dev_priv->display.update_sprite_wm(dev, pipe, sprite_width,
-						   pixel_size);
-}
-
 static inline bool intel_panel_use_ssc(struct drm_i915_private *dev_priv)
 {
 	if (i915_panel_use_ssc >= 0)
@@ -5143,6 +3732,222 @@ static void i9xx_update_pll_dividers(struct drm_crtc *crtc,
 	}
 }
 
+static void intel_update_lvds(struct drm_crtc *crtc, intel_clock_t *clock,
+			      struct drm_display_mode *adjusted_mode)
+{
+	struct drm_device *dev = crtc->dev;
+	struct drm_i915_private *dev_priv = dev->dev_private;
+	struct intel_crtc *intel_crtc = to_intel_crtc(crtc);
+	int pipe = intel_crtc->pipe;
+	u32 temp;
+
+	temp = I915_READ(LVDS);
+	temp |= LVDS_PORT_EN | LVDS_A0A2_CLKA_POWER_UP;
+	if (pipe == 1) {
+		temp |= LVDS_PIPEB_SELECT;
+	} else {
+		temp &= ~LVDS_PIPEB_SELECT;
+	}
+	/* set the corresponsding LVDS_BORDER bit */
+	temp |= dev_priv->lvds_border_bits;
+	/* Set the B0-B3 data pairs corresponding to whether we're going to
+	 * set the DPLLs for dual-channel mode or not.
+	 */
+	if (clock->p2 == 7)
+		temp |= LVDS_B0B3_POWER_UP | LVDS_CLKB_POWER_UP;
+	else
+		temp &= ~(LVDS_B0B3_POWER_UP | LVDS_CLKB_POWER_UP);
+
+	/* It would be nice to set 24 vs 18-bit mode (LVDS_A3_POWER_UP)
+	 * appropriately here, but we need to look more thoroughly into how
+	 * panels behave in the two modes.
+	 */
+	/* set the dithering flag on LVDS as needed */
+	if (INTEL_INFO(dev)->gen >= 4) {
+		if (dev_priv->lvds_dither)
+			temp |= LVDS_ENABLE_DITHER;
+		else
+			temp &= ~LVDS_ENABLE_DITHER;
+	}
+	temp &= ~(LVDS_HSYNC_POLARITY | LVDS_VSYNC_POLARITY);
+	if (adjusted_mode->flags & DRM_MODE_FLAG_NHSYNC)
+		temp |= LVDS_HSYNC_POLARITY;
+	if (adjusted_mode->flags & DRM_MODE_FLAG_NVSYNC)
+		temp |= LVDS_VSYNC_POLARITY;
+	I915_WRITE(LVDS, temp);
+}
+
+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,
+			    int num_connectors)
+{
+	struct drm_device *dev = crtc->dev;
+	struct drm_i915_private *dev_priv = dev->dev_private;
+	struct intel_crtc *intel_crtc = to_intel_crtc(crtc);
+	int pipe = intel_crtc->pipe;
+	u32 dpll;
+	bool is_sdvo;
+
+	is_sdvo = intel_pipe_has_type(crtc, INTEL_OUTPUT_SDVO) ||
+		intel_pipe_has_type(crtc, INTEL_OUTPUT_HDMI);
+
+	dpll = DPLL_VGA_MODE_DIS;
+
+	if (intel_pipe_has_type(crtc, 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;
+		}
+		dpll |= DPLL_DVO_HIGH_SPEED;
+	}
+	if (intel_pipe_has_type(crtc, INTEL_OUTPUT_DISPLAYPORT))
+		dpll |= DPLL_DVO_HIGH_SPEED;
+
+	/* compute bitmask from p1 value */
+	if (IS_PINEVIEW(dev))
+		dpll |= (1 << (clock->p1 - 1)) << DPLL_FPA01_P1_POST_DIV_SHIFT_PINEVIEW;
+	else {
+		dpll |= (1 << (clock->p1 - 1)) << DPLL_FPA01_P1_POST_DIV_SHIFT;
+		if (IS_G4X(dev) && reduced_clock)
+			dpll |= (1 << (reduced_clock->p1 - 1)) << DPLL_FPA1_P1_POST_DIV_SHIFT;
+	}
+	switch (clock->p2) {
+	case 5:
+		dpll |= DPLL_DAC_SERIAL_P2_CLOCK_DIV_5;
+		break;
+	case 7:
+		dpll |= DPLLB_LVDS_P2_CLOCK_DIV_7;
+		break;
+	case 10:
+		dpll |= DPLL_DAC_SERIAL_P2_CLOCK_DIV_10;
+		break;
+	case 14:
+		dpll |= DPLLB_LVDS_P2_CLOCK_DIV_14;
+		break;
+	}
+	if (INTEL_INFO(dev)->gen >= 4)
+		dpll |= (6 << PLL_LOAD_PULSE_PHASE_SHIFT);
+
+	if (is_sdvo && intel_pipe_has_type(crtc, INTEL_OUTPUT_TVOUT))
+		dpll |= PLL_REF_INPUT_TVCLKINBC;
+	else 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) &&
+		 intel_panel_use_ssc(dev_priv) && num_connectors < 2)
+		dpll |= PLLB_REF_INPUT_SPREADSPECTRUMIN;
+	else
+		dpll |= PLL_REF_INPUT_DREFCLK;
+
+	dpll |= DPLL_VCO_ENABLE;
+	I915_WRITE(DPLL(pipe), dpll & ~DPLL_VCO_ENABLE);
+	POSTING_READ(DPLL(pipe));
+	udelay(150);
+
+	/* The LVDS pin pair needs to be on before the DPLLs are enabled.
+	 * This is an exception to the general rule that mode_set doesn't turn
+	 * things on.
+	 */
+	if (intel_pipe_has_type(crtc, INTEL_OUTPUT_LVDS))
+		intel_update_lvds(crtc, clock, adjusted_mode);
+
+	if (intel_pipe_has_type(crtc, INTEL_OUTPUT_DISPLAYPORT))
+		intel_dp_set_m_n(crtc, mode, adjusted_mode);
+
+	I915_WRITE(DPLL(pipe), dpll);
+
+	/* Wait for the clocks to stabilize. */
+	POSTING_READ(DPLL(pipe));
+	udelay(150);
+
+	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;
+		}
+		I915_WRITE(DPLL_MD(pipe), temp);
+	} else {
+		/* The pixel multiplier can only be updated once the
+		 * DPLL is enabled and the clocks are stable.
+		 *
+		 * So write it again.
+		 */
+		I915_WRITE(DPLL(pipe), dpll);
+	}
+}
+
+static void i8xx_update_pll(struct drm_crtc *crtc,
+			    struct drm_display_mode *adjusted_mode,
+			    intel_clock_t *clock,
+			    int num_connectors)
+{
+	struct drm_device *dev = crtc->dev;
+	struct drm_i915_private *dev_priv = dev->dev_private;
+	struct intel_crtc *intel_crtc = to_intel_crtc(crtc);
+	int pipe = intel_crtc->pipe;
+	u32 dpll;
+
+	dpll = DPLL_VGA_MODE_DIS;
+
+	if (intel_pipe_has_type(crtc, INTEL_OUTPUT_LVDS)) {
+		dpll |= (1 << (clock->p1 - 1)) << DPLL_FPA01_P1_POST_DIV_SHIFT;
+	} else {
+		if (clock->p1 == 2)
+			dpll |= PLL_P1_DIVIDE_BY_TWO;
+		else
+			dpll |= (clock->p1 - 2) << DPLL_FPA01_P1_POST_DIV_SHIFT;
+		if (clock->p2 == 4)
+			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) &&
+		 intel_panel_use_ssc(dev_priv) && num_connectors < 2)
+		dpll |= PLLB_REF_INPUT_SPREADSPECTRUMIN;
+	else
+		dpll |= PLL_REF_INPUT_DREFCLK;
+
+	dpll |= DPLL_VCO_ENABLE;
+	I915_WRITE(DPLL(pipe), dpll & ~DPLL_VCO_ENABLE);
+	POSTING_READ(DPLL(pipe));
+	udelay(150);
+
+	I915_WRITE(DPLL(pipe), dpll);
+
+	/* Wait for the clocks to stabilize. */
+	POSTING_READ(DPLL(pipe));
+	udelay(150);
+
+	/* The LVDS pin pair needs to be on before the DPLLs are enabled.
+	 * This is an exception to the general rule that mode_set doesn't turn
+	 * things on.
+	 */
+	if (intel_pipe_has_type(crtc, INTEL_OUTPUT_LVDS))
+		intel_update_lvds(crtc, clock, adjusted_mode);
+
+	/* The pixel multiplier can only be updated once the
+	 * DPLL is enabled and the clocks are stable.
+	 *
+	 * So write it again.
+	 */
+	I915_WRITE(DPLL(pipe), dpll);
+}
+
 static int i9xx_crtc_mode_set(struct drm_crtc *crtc,
 			      struct drm_display_mode *mode,
 			      struct drm_display_mode *adjusted_mode,
@@ -5156,15 +3961,13 @@ static int i9xx_crtc_mode_set(struct drm_crtc *crtc,
 	int plane = intel_crtc->plane;
 	int refclk, num_connectors = 0;
 	intel_clock_t clock, reduced_clock;
-	u32 dpll, dspcntr, pipeconf, vsyncshift;
-	bool ok, has_reduced_clock = false, is_sdvo = false, is_dvo = false;
-	bool is_crt = false, is_lvds = false, is_tv = false, is_dp = false;
+	u32 dspcntr, pipeconf, vsyncshift;
+	bool ok, has_reduced_clock = false, is_sdvo = false;
+	bool is_lvds = false, is_tv = false, is_dp = false;
 	struct drm_mode_config *mode_config = &dev->mode_config;
 	struct intel_encoder *encoder;
 	const intel_limit_t *limit;
 	int ret;
-	u32 temp;
-	u32 lvds_sync = 0;
 
 	list_for_each_entry(encoder, &mode_config->encoder_list, base.head) {
 		if (encoder->base.crtc != crtc)
@@ -5180,15 +3983,9 @@ static int i9xx_crtc_mode_set(struct drm_crtc *crtc,
 			if (encoder->needs_tv_clock)
 				is_tv = true;
 			break;
-		case INTEL_OUTPUT_DVO:
-			is_dvo = true;
-			break;
 		case INTEL_OUTPUT_TVOUT:
 			is_tv = true;
 			break;
-		case INTEL_OUTPUT_ANALOG:
-			is_crt = true;
-			break;
 		case INTEL_OUTPUT_DISPLAYPORT:
 			is_dp = true;
 			break;
@@ -5235,71 +4032,12 @@ static int i9xx_crtc_mode_set(struct drm_crtc *crtc,
 	i9xx_update_pll_dividers(crtc, &clock, has_reduced_clock ?
 				 &reduced_clock : NULL);
 
-	dpll = DPLL_VGA_MODE_DIS;
-
-	if (!IS_GEN2(dev)) {
-		if (is_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;
-			}
-			dpll |= DPLL_DVO_HIGH_SPEED;
-		}
-		if (is_dp)
-			dpll |= DPLL_DVO_HIGH_SPEED;
-
-		/* compute bitmask from p1 value */
-		if (IS_PINEVIEW(dev))
-			dpll |= (1 << (clock.p1 - 1)) << DPLL_FPA01_P1_POST_DIV_SHIFT_PINEVIEW;
-		else {
-			dpll |= (1 << (clock.p1 - 1)) << DPLL_FPA01_P1_POST_DIV_SHIFT;
-			if (IS_G4X(dev) && has_reduced_clock)
-				dpll |= (1 << (reduced_clock.p1 - 1)) << DPLL_FPA1_P1_POST_DIV_SHIFT;
-		}
-		switch (clock.p2) {
-		case 5:
-			dpll |= DPLL_DAC_SERIAL_P2_CLOCK_DIV_5;
-			break;
-		case 7:
-			dpll |= DPLLB_LVDS_P2_CLOCK_DIV_7;
-			break;
-		case 10:
-			dpll |= DPLL_DAC_SERIAL_P2_CLOCK_DIV_10;
-			break;
-		case 14:
-			dpll |= DPLLB_LVDS_P2_CLOCK_DIV_14;
-			break;
-		}
-		if (INTEL_INFO(dev)->gen >= 4)
-			dpll |= (6 << PLL_LOAD_PULSE_PHASE_SHIFT);
-	} else {
-		if (is_lvds) {
-			dpll |= (1 << (clock.p1 - 1)) << DPLL_FPA01_P1_POST_DIV_SHIFT;
-		} else {
-			if (clock.p1 == 2)
-				dpll |= PLL_P1_DIVIDE_BY_TWO;
-			else
-				dpll |= (clock.p1 - 2) << DPLL_FPA01_P1_POST_DIV_SHIFT;
-			if (clock.p2 == 4)
-				dpll |= PLL_P2_DIVIDE_BY_4;
-		}
-	}
-
-	if (is_sdvo && is_tv)
-		dpll |= PLL_REF_INPUT_TVCLKINBC;
-	else if (is_tv)
-		/* XXX: just matching BIOS for now */
-		/*	dpll |= PLL_REF_INPUT_TVCLKINBC; */
-		dpll |= 3;
-	else if (is_lvds && intel_panel_use_ssc(dev_priv) && num_connectors < 2)
-		dpll |= PLLB_REF_INPUT_SPREADSPECTRUMIN;
+	if (IS_GEN2(dev))
+		i8xx_update_pll(crtc, adjusted_mode, &clock, num_connectors);
 	else
-		dpll |= PLL_REF_INPUT_DREFCLK;
+		i9xx_update_pll(crtc, mode, adjusted_mode, &clock,
+				has_reduced_clock ? &reduced_clock : NULL,
+				num_connectors);
 
 	/* setup pipeconf */
 	pipeconf = I915_READ(PIPECONF(pipe));
@@ -5336,97 +4074,9 @@ static int i9xx_crtc_mode_set(struct drm_crtc *crtc,
 		}
 	}
 
-	dpll |= DPLL_VCO_ENABLE;
-
 	DRM_DEBUG_KMS("Mode for pipe %c:\n", pipe == 0 ? 'A' : 'B');
 	drm_mode_debug_printmodeline(mode);
 
-	I915_WRITE(DPLL(pipe), dpll & ~DPLL_VCO_ENABLE);
-
-	POSTING_READ(DPLL(pipe));
-	udelay(150);
-
-	/* The LVDS pin pair needs to be on before the DPLLs are enabled.
-	 * This is an exception to the general rule that mode_set doesn't turn
-	 * things on.
-	 */
-	if (is_lvds) {
-		temp = I915_READ(LVDS);
-		temp |= LVDS_PORT_EN | LVDS_A0A2_CLKA_POWER_UP;
-		if (pipe == 1) {
-			temp |= LVDS_PIPEB_SELECT;
-		} else {
-			temp &= ~LVDS_PIPEB_SELECT;
-		}
-		/* set the corresponsding LVDS_BORDER bit */
-		temp |= dev_priv->lvds_border_bits;
-		/* Set the B0-B3 data pairs corresponding to whether we're going to
-		 * set the DPLLs for dual-channel mode or not.
-		 */
-		if (clock.p2 == 7)
-			temp |= LVDS_B0B3_POWER_UP | LVDS_CLKB_POWER_UP;
-		else
-			temp &= ~(LVDS_B0B3_POWER_UP | LVDS_CLKB_POWER_UP);
-
-		/* It would be nice to set 24 vs 18-bit mode (LVDS_A3_POWER_UP)
-		 * appropriately here, but we need to look more thoroughly into how
-		 * panels behave in the two modes.
-		 */
-		/* set the dithering flag on LVDS as needed */
-		if (INTEL_INFO(dev)->gen >= 4) {
-			if (dev_priv->lvds_dither)
-				temp |= LVDS_ENABLE_DITHER;
-			else
-				temp &= ~LVDS_ENABLE_DITHER;
-		}
-		if (adjusted_mode->flags & DRM_MODE_FLAG_NHSYNC)
-			lvds_sync |= LVDS_HSYNC_POLARITY;
-		if (adjusted_mode->flags & DRM_MODE_FLAG_NVSYNC)
-			lvds_sync |= LVDS_VSYNC_POLARITY;
-		if ((temp & (LVDS_HSYNC_POLARITY | LVDS_VSYNC_POLARITY))
-		    != lvds_sync) {
-			char flags[2] = "-+";
-			DRM_INFO("Changing LVDS panel from "
-				 "(%chsync, %cvsync) to (%chsync, %cvsync)\n",
-				 flags[!(temp & LVDS_HSYNC_POLARITY)],
-				 flags[!(temp & LVDS_VSYNC_POLARITY)],
-				 flags[!(lvds_sync & LVDS_HSYNC_POLARITY)],
-				 flags[!(lvds_sync & LVDS_VSYNC_POLARITY)]);
-			temp &= ~(LVDS_HSYNC_POLARITY | LVDS_VSYNC_POLARITY);
-			temp |= lvds_sync;
-		}
-		I915_WRITE(LVDS, temp);
-	}
-
-	if (is_dp) {
-		intel_dp_set_m_n(crtc, mode, adjusted_mode);
-	}
-
-	I915_WRITE(DPLL(pipe), dpll);
-
-	/* Wait for the clocks to stabilize. */
-	POSTING_READ(DPLL(pipe));
-	udelay(150);
-
-	if (INTEL_INFO(dev)->gen >= 4) {
-		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;
-		}
-		I915_WRITE(DPLL_MD(pipe), temp);
-	} else {
-		/* The pixel multiplier can only be updated once the
-		 * DPLL is enabled and the clocks are stable.
-		 *
-		 * So write it again.
-		 */
-		I915_WRITE(DPLL(pipe), dpll);
-	}
-
 	if (HAS_PIPE_CXSR(dev)) {
 		if (intel_crtc->lowfreq_avail) {
 			DRM_DEBUG_KMS("enabling CxSR downclocking\n");
@@ -5492,7 +4142,6 @@ static int i9xx_crtc_mode_set(struct drm_crtc *crtc,
 
 	I915_WRITE(DSPCNTR(plane), dspcntr);
 	POSTING_READ(DSPCNTR(plane));
-	intel_enable_plane(dev_priv, plane, pipe);
 
 	ret = intel_pipe_set_base(crtc, x, y, old_fb);
 
@@ -5668,17 +4317,16 @@ static int ironlake_crtc_mode_set(struct drm_crtc *crtc,
 	u32 dpll, fp = 0, fp2 = 0, dspcntr, pipeconf;
 	bool ok, has_reduced_clock = false, is_sdvo = false;
 	bool is_crt = false, is_lvds = false, is_tv = false, is_dp = false;
-	struct intel_encoder *has_edp_encoder = NULL;
 	struct drm_mode_config *mode_config = &dev->mode_config;
-	struct intel_encoder *encoder;
+	struct intel_encoder *encoder, *edp_encoder = NULL;
 	const intel_limit_t *limit;
 	int ret;
 	struct fdi_m_n m_n = {0};
 	u32 temp;
-	u32 lvds_sync = 0;
 	int target_clock, pixel_multiplier, lane, link_bw, factor;
 	unsigned int pipe_bpp;
 	bool dither;
+	bool is_cpu_edp = false, is_pch_edp = false;
 
 	list_for_each_entry(encoder, &mode_config->encoder_list, base.head) {
 		if (encoder->base.crtc != crtc)
@@ -5704,7 +4352,12 @@ static int ironlake_crtc_mode_set(struct drm_crtc *crtc,
 			is_dp = true;
 			break;
 		case INTEL_OUTPUT_EDP:
-			has_edp_encoder = encoder;
+			is_dp = true;
+			if (intel_encoder_is_pch_edp(&encoder->base))
+				is_pch_edp = true;
+			else
+				is_cpu_edp = true;
+			edp_encoder = encoder;
 			break;
 		}
 
@@ -5767,15 +4420,13 @@ static int ironlake_crtc_mode_set(struct drm_crtc *crtc,
 	lane = 0;
 	/* CPU eDP doesn't require FDI link, so just set DP M/N
 	   according to current link config */
-	if (has_edp_encoder &&
-	    !intel_encoder_is_pch_edp(&has_edp_encoder->base)) {
+	if (is_cpu_edp) {
 		target_clock = mode->clock;
-		intel_edp_link_config(has_edp_encoder,
-				      &lane, &link_bw);
+		intel_edp_link_config(edp_encoder, &lane, &link_bw);
 	} else {
 		/* [e]DP over FDI requires target mode clock
 		   instead of link clock */
-		if (is_dp || intel_encoder_is_pch_edp(&has_edp_encoder->base))
+		if (is_dp)
 			target_clock = mode->clock;
 		else
 			target_clock = adjusted_mode->clock;
@@ -5866,7 +4517,7 @@ static int ironlake_crtc_mode_set(struct drm_crtc *crtc,
 		}
 		dpll |= DPLL_DVO_HIGH_SPEED;
 	}
-	if (is_dp || intel_encoder_is_pch_edp(&has_edp_encoder->base))
+	if (is_dp && !is_cpu_edp)
 		dpll |= DPLL_DVO_HIGH_SPEED;
 
 	/* compute bitmask from p1 value */
@@ -5909,30 +4560,22 @@ static int ironlake_crtc_mode_set(struct drm_crtc *crtc,
 	DRM_DEBUG_KMS("Mode for pipe %d:\n", pipe);
 	drm_mode_debug_printmodeline(mode);
 
-	/* PCH eDP needs FDI, but CPU eDP does not */
-	if (!intel_crtc->no_pll) {
-		if (!has_edp_encoder ||
-		    intel_encoder_is_pch_edp(&has_edp_encoder->base)) {
-			I915_WRITE(PCH_FP0(pipe), fp);
-			I915_WRITE(PCH_DPLL(pipe), dpll & ~DPLL_VCO_ENABLE);
-
-			POSTING_READ(PCH_DPLL(pipe));
-			udelay(150);
-		}
-	} else {
-		if (dpll == (I915_READ(PCH_DPLL(0)) & 0x7fffffff) &&
-		    fp == I915_READ(PCH_FP0(0))) {
-			intel_crtc->use_pll_a = true;
-			DRM_DEBUG_KMS("using pipe a dpll\n");
-		} else if (dpll == (I915_READ(PCH_DPLL(1)) & 0x7fffffff) &&
-			   fp == I915_READ(PCH_FP0(1))) {
-			intel_crtc->use_pll_a = false;
-			DRM_DEBUG_KMS("using pipe b dpll\n");
-		} else {
-			DRM_DEBUG_KMS("no matching PLL configuration for pipe 2\n");
+	/* CPU eDP is the only output that doesn't need a PCH PLL of its own on
+	 * pre-Haswell/LPT generation */
+	if (HAS_PCH_LPT(dev)) {
+		DRM_DEBUG_KMS("LPT detected: no PLL for pipe %d necessary\n",
+				pipe);
+	} else if (!is_cpu_edp) {
+		struct intel_pch_pll *pll;
+
+		pll = intel_get_pch_pll(intel_crtc, dpll, fp);
+		if (pll == NULL) {
+			DRM_DEBUG_DRIVER("failed to find PLL for pipe %d\n",
+					 pipe);
 			return -EINVAL;
 		}
-	}
+	} else
+		intel_put_pch_pll(intel_crtc);
 
 	/* The LVDS pin pair needs to be on before the DPLLs are enabled.
 	 * This is an exception to the general rule that mode_set doesn't turn
@@ -5965,22 +4608,11 @@ static int ironlake_crtc_mode_set(struct drm_crtc *crtc,
 		 * appropriately here, but we need to look more thoroughly into how
 		 * panels behave in the two modes.
 		 */
+		temp &= ~(LVDS_HSYNC_POLARITY | LVDS_VSYNC_POLARITY);
 		if (adjusted_mode->flags & DRM_MODE_FLAG_NHSYNC)
-			lvds_sync |= LVDS_HSYNC_POLARITY;
+			temp |= LVDS_HSYNC_POLARITY;
 		if (adjusted_mode->flags & DRM_MODE_FLAG_NVSYNC)
-			lvds_sync |= LVDS_VSYNC_POLARITY;
-		if ((temp & (LVDS_HSYNC_POLARITY | LVDS_VSYNC_POLARITY))
-		    != lvds_sync) {
-			char flags[2] = "-+";
-			DRM_INFO("Changing LVDS panel from "
-				 "(%chsync, %cvsync) to (%chsync, %cvsync)\n",
-				 flags[!(temp & LVDS_HSYNC_POLARITY)],
-				 flags[!(temp & LVDS_VSYNC_POLARITY)],
-				 flags[!(lvds_sync & LVDS_HSYNC_POLARITY)],
-				 flags[!(lvds_sync & LVDS_VSYNC_POLARITY)]);
-			temp &= ~(LVDS_HSYNC_POLARITY | LVDS_VSYNC_POLARITY);
-			temp |= lvds_sync;
-		}
+			temp |= LVDS_VSYNC_POLARITY;
 		I915_WRITE(PCH_LVDS, temp);
 	}
 
@@ -5990,7 +4622,7 @@ static int ironlake_crtc_mode_set(struct drm_crtc *crtc,
 		pipeconf |= PIPECONF_DITHER_EN;
 		pipeconf |= PIPECONF_DITHER_TYPE_SP;
 	}
-	if (is_dp || intel_encoder_is_pch_edp(&has_edp_encoder->base)) {
+	if (is_dp && !is_cpu_edp) {
 		intel_dp_set_m_n(crtc, mode, adjusted_mode);
 	} else {
 		/* For non-DP output, clear any trans DP clock recovery setting.*/
@@ -6000,13 +4632,11 @@ static int ironlake_crtc_mode_set(struct drm_crtc *crtc,
 		I915_WRITE(TRANSDPLINK_N1(pipe), 0);
 	}
 
-	if (!intel_crtc->no_pll &&
-	    (!has_edp_encoder ||
-	     intel_encoder_is_pch_edp(&has_edp_encoder->base))) {
-		I915_WRITE(PCH_DPLL(pipe), dpll);
+	if (intel_crtc->pch_pll) {
+		I915_WRITE(intel_crtc->pch_pll->pll_reg, dpll);
 
 		/* Wait for the clocks to stabilize. */
-		POSTING_READ(PCH_DPLL(pipe));
+		POSTING_READ(intel_crtc->pch_pll->pll_reg);
 		udelay(150);
 
 		/* The pixel multiplier can only be updated once the
@@ -6014,20 +4644,20 @@ static int ironlake_crtc_mode_set(struct drm_crtc *crtc,
 		 *
 		 * So write it again.
 		 */
-		I915_WRITE(PCH_DPLL(pipe), dpll);
+		I915_WRITE(intel_crtc->pch_pll->pll_reg, dpll);
 	}
 
 	intel_crtc->lowfreq_avail = false;
-	if (!intel_crtc->no_pll) {
+	if (intel_crtc->pch_pll) {
 		if (is_lvds && has_reduced_clock && i915_powersave) {
-			I915_WRITE(PCH_FP1(pipe), fp2);
+			I915_WRITE(intel_crtc->pch_pll->fp1_reg, fp2);
 			intel_crtc->lowfreq_avail = true;
 			if (HAS_PIPE_CXSR(dev)) {
 				DRM_DEBUG_KMS("enabling CxSR downclocking\n");
 				pipeconf |= PIPECONF_CXSR_DOWNCLOCK;
 			}
 		} else {
-			I915_WRITE(PCH_FP1(pipe), fp);
+			I915_WRITE(intel_crtc->pch_pll->fp1_reg, fp);
 			if (HAS_PIPE_CXSR(dev)) {
 				DRM_DEBUG_KMS("disabling CxSR downclocking\n");
 				pipeconf &= ~PIPECONF_CXSR_DOWNCLOCK;
@@ -6080,10 +4710,8 @@ static int ironlake_crtc_mode_set(struct drm_crtc *crtc,
 	I915_WRITE(PIPE_LINK_M1(pipe), m_n.link_m);
 	I915_WRITE(PIPE_LINK_N1(pipe), m_n.link_n);
 
-	if (has_edp_encoder &&
-	    !intel_encoder_is_pch_edp(&has_edp_encoder->base)) {
+	if (is_cpu_edp)
 		ironlake_set_pll_edp(crtc, adjusted_mode->clock);
-	}
 
 	I915_WRITE(PIPECONF(pipe), pipeconf);
 	POSTING_READ(PIPECONF(pipe));
@@ -6097,6 +4725,8 @@ static int ironlake_crtc_mode_set(struct drm_crtc *crtc,
 
 	intel_update_watermarks(dev);
 
+	intel_update_linetime_watermarks(dev, pipe, adjusted_mode);
+
 	return ret;
 }
 
@@ -6451,7 +5081,7 @@ static void intel_crtc_update_cursor(struct drm_crtc *crtc,
 	if (!visible && !intel_crtc->cursor_visible)
 		return;
 
-	if (IS_IVYBRIDGE(dev)) {
+	if (IS_IVYBRIDGE(dev) || IS_HASWELL(dev)) {
 		I915_WRITE(CURPOS_IVB(pipe), pos);
 		ivb_update_cursor(crtc, base);
 	} else {
@@ -6461,9 +5091,6 @@ static void intel_crtc_update_cursor(struct drm_crtc *crtc,
 		else
 			i9xx_update_cursor(crtc, base);
 	}
-
-	if (visible)
-		intel_mark_busy(dev, to_intel_framebuffer(crtc->fb)->obj);
 }
 
 static int intel_crtc_cursor_set(struct drm_crtc *crtc,
@@ -6987,7 +5614,6 @@ struct drm_display_mode *intel_crtc_mode_get(struct drm_device *dev,
 	mode->vsync_end = ((vsync & 0xffff0000) >> 16) + 1;
 
 	drm_mode_set_name(mode);
-	drm_mode_set_crtcinfo(mode, 0);
 
 	return mode;
 }
@@ -7086,7 +5712,7 @@ static void intel_decrease_pllclock(struct drm_crtc *crtc)
 	if (!HAS_PIPE_CXSR(dev) && intel_crtc->lowfreq_avail) {
 		int pipe = intel_crtc->pipe;
 		int dpll_reg = DPLL(pipe);
-		u32 dpll;
+		int dpll;
 
 		DRM_DEBUG_DRIVER("downclocking LVDS\n");
 
@@ -7100,6 +5726,7 @@ static void intel_decrease_pllclock(struct drm_crtc *crtc)
 		if (!(dpll & DISPLAY_RATE_SELECT_FPA1))
 			DRM_DEBUG_DRIVER("failed to downclock LVDS!\n");
 	}
+
 }
 
 /**
@@ -7158,12 +5785,16 @@ void intel_mark_busy(struct drm_device *dev, struct drm_i915_gem_object *obj)
 	if (!drm_core_check_feature(dev, DRIVER_MODESET))
 		return;
 
-	if (!dev_priv->busy)
+	if (!dev_priv->busy) {
+		intel_sanitize_pm(dev);
 		dev_priv->busy = true;
-	else
+	} else
 		mod_timer(&dev_priv->idle_timer, jiffies +
 			  msecs_to_jiffies(GPU_IDLE_TIMEOUT));
 
+	if (obj == NULL)
+		return;
+
 	list_for_each_entry(crtc, &dev->mode_config.crtc_list, head) {
 		if (!crtc->fb)
 			continue;
@@ -7336,18 +5967,19 @@ static int intel_gen2_queue_flip(struct drm_device *dev,
 	struct intel_crtc *intel_crtc = to_intel_crtc(crtc);
 	unsigned long offset;
 	u32 flip_mask;
+	struct intel_ring_buffer *ring = &dev_priv->ring[RCS];
 	int ret;
 
-	ret = intel_pin_and_fence_fb_obj(dev, obj, LP_RING(dev_priv));
+	ret = intel_pin_and_fence_fb_obj(dev, obj, ring);
 	if (ret)
-		goto out;
+		goto err;
 
 	/* Offset into the new buffer for cases of shared fbs between CRTCs */
 	offset = crtc->y * fb->pitches[0] + crtc->x * fb->bits_per_pixel/8;
 
-	ret = BEGIN_LP_RING(6);
+	ret = intel_ring_begin(ring, 6);
 	if (ret)
-		goto out;
+		goto err_unpin;
 
 	/* Can't queue multiple flips, so wait for the previous
 	 * one to finish before executing the next.
@@ -7356,15 +5988,19 @@ static int intel_gen2_queue_flip(struct drm_device *dev,
 		flip_mask = MI_WAIT_FOR_PLANE_B_FLIP;
 	else
 		flip_mask = MI_WAIT_FOR_PLANE_A_FLIP;
-	OUT_RING(MI_WAIT_FOR_EVENT | flip_mask);
-	OUT_RING(MI_NOOP);
-	OUT_RING(MI_DISPLAY_FLIP |
-		 MI_DISPLAY_FLIP_PLANE(intel_crtc->plane));
-	OUT_RING(fb->pitches[0]);
-	OUT_RING(obj->gtt_offset + offset);
-	OUT_RING(0); /* aux display base address, unused */
-	ADVANCE_LP_RING();
-out:
+	intel_ring_emit(ring, MI_WAIT_FOR_EVENT | flip_mask);
+	intel_ring_emit(ring, MI_NOOP);
+	intel_ring_emit(ring, MI_DISPLAY_FLIP |
+			MI_DISPLAY_FLIP_PLANE(intel_crtc->plane));
+	intel_ring_emit(ring, fb->pitches[0]);
+	intel_ring_emit(ring, obj->gtt_offset + offset);
+	intel_ring_emit(ring, 0); /* aux display base address, unused */
+	intel_ring_advance(ring);
+	return 0;
+
+err_unpin:
+	intel_unpin_fb_obj(obj);
+err:
 	return ret;
 }
 
@@ -7377,33 +6013,38 @@ static int intel_gen3_queue_flip(struct drm_device *dev,
 	struct intel_crtc *intel_crtc = to_intel_crtc(crtc);
 	unsigned long offset;
 	u32 flip_mask;
+	struct intel_ring_buffer *ring = &dev_priv->ring[RCS];
 	int ret;
 
-	ret = intel_pin_and_fence_fb_obj(dev, obj, LP_RING(dev_priv));
+	ret = intel_pin_and_fence_fb_obj(dev, obj, ring);
 	if (ret)
-		goto out;
+		goto err;
 
 	/* Offset into the new buffer for cases of shared fbs between CRTCs */
 	offset = crtc->y * fb->pitches[0] + crtc->x * fb->bits_per_pixel/8;
 
-	ret = BEGIN_LP_RING(6);
+	ret = intel_ring_begin(ring, 6);
 	if (ret)
-		goto out;
+		goto err_unpin;
 
 	if (intel_crtc->plane)
 		flip_mask = MI_WAIT_FOR_PLANE_B_FLIP;
 	else
 		flip_mask = MI_WAIT_FOR_PLANE_A_FLIP;
-	OUT_RING(MI_WAIT_FOR_EVENT | flip_mask);
-	OUT_RING(MI_NOOP);
-	OUT_RING(MI_DISPLAY_FLIP_I915 |
-		 MI_DISPLAY_FLIP_PLANE(intel_crtc->plane));
-	OUT_RING(fb->pitches[0]);
-	OUT_RING(obj->gtt_offset + offset);
-	OUT_RING(MI_NOOP);
-
-	ADVANCE_LP_RING();
-out:
+	intel_ring_emit(ring, MI_WAIT_FOR_EVENT | flip_mask);
+	intel_ring_emit(ring, MI_NOOP);
+	intel_ring_emit(ring, MI_DISPLAY_FLIP_I915 |
+			MI_DISPLAY_FLIP_PLANE(intel_crtc->plane));
+	intel_ring_emit(ring, fb->pitches[0]);
+	intel_ring_emit(ring, obj->gtt_offset + offset);
+	intel_ring_emit(ring, MI_NOOP);
+
+	intel_ring_advance(ring);
+	return 0;
+
+err_unpin:
+	intel_unpin_fb_obj(obj);
+err:
 	return ret;
 }
 
@@ -7415,24 +6056,25 @@ static int intel_gen4_queue_flip(struct drm_device *dev,
 	struct drm_i915_private *dev_priv = dev->dev_private;
 	struct intel_crtc *intel_crtc = to_intel_crtc(crtc);
 	uint32_t pf, pipesrc;
+	struct intel_ring_buffer *ring = &dev_priv->ring[RCS];
 	int ret;
 
-	ret = intel_pin_and_fence_fb_obj(dev, obj, LP_RING(dev_priv));
+	ret = intel_pin_and_fence_fb_obj(dev, obj, ring);
 	if (ret)
-		goto out;
+		goto err;
 
-	ret = BEGIN_LP_RING(4);
+	ret = intel_ring_begin(ring, 4);
 	if (ret)
-		goto out;
+		goto err_unpin;
 
 	/* i965+ uses the linear or tiled offsets from the
 	 * Display Registers (which do not change across a page-flip)
 	 * so we need only reprogram the base address.
 	 */
-	OUT_RING(MI_DISPLAY_FLIP |
-		 MI_DISPLAY_FLIP_PLANE(intel_crtc->plane));
-	OUT_RING(fb->pitches[0]);
-	OUT_RING(obj->gtt_offset | obj->tiling_mode);
+	intel_ring_emit(ring, MI_DISPLAY_FLIP |
+			MI_DISPLAY_FLIP_PLANE(intel_crtc->plane));
+	intel_ring_emit(ring, fb->pitches[0]);
+	intel_ring_emit(ring, obj->gtt_offset | obj->tiling_mode);
 
 	/* XXX Enabling the panel-fitter across page-flip is so far
 	 * untested on non-native modes, so ignore it for now.
@@ -7440,9 +6082,13 @@ static int intel_gen4_queue_flip(struct drm_device *dev,
 	 */
 	pf = 0;
 	pipesrc = I915_READ(PIPESRC(intel_crtc->pipe)) & 0x0fff0fff;
-	OUT_RING(pf | pipesrc);
-	ADVANCE_LP_RING();
-out:
+	intel_ring_emit(ring, pf | pipesrc);
+	intel_ring_advance(ring);
+	return 0;
+
+err_unpin:
+	intel_unpin_fb_obj(obj);
+err:
 	return ret;
 }
 
@@ -7453,21 +6099,22 @@ static int intel_gen6_queue_flip(struct drm_device *dev,
 {
 	struct drm_i915_private *dev_priv = dev->dev_private;
 	struct intel_crtc *intel_crtc = to_intel_crtc(crtc);
+	struct intel_ring_buffer *ring = &dev_priv->ring[RCS];
 	uint32_t pf, pipesrc;
 	int ret;
 
-	ret = intel_pin_and_fence_fb_obj(dev, obj, LP_RING(dev_priv));
+	ret = intel_pin_and_fence_fb_obj(dev, obj, ring);
 	if (ret)
-		goto out;
+		goto err;
 
-	ret = BEGIN_LP_RING(4);
+	ret = intel_ring_begin(ring, 4);
 	if (ret)
-		goto out;
+		goto err_unpin;
 
-	OUT_RING(MI_DISPLAY_FLIP |
-		 MI_DISPLAY_FLIP_PLANE(intel_crtc->plane));
-	OUT_RING(fb->pitches[0] | obj->tiling_mode);
-	OUT_RING(obj->gtt_offset);
+	intel_ring_emit(ring, MI_DISPLAY_FLIP |
+			MI_DISPLAY_FLIP_PLANE(intel_crtc->plane));
+	intel_ring_emit(ring, fb->pitches[0] | obj->tiling_mode);
+	intel_ring_emit(ring, obj->gtt_offset);
 
 	/* Contrary to the suggestions in the documentation,
 	 * "Enable Panel Fitter" does not seem to be required when page
@@ -7477,9 +6124,13 @@ static int intel_gen6_queue_flip(struct drm_device *dev,
 	 */
 	pf = 0;
 	pipesrc = I915_READ(PIPESRC(intel_crtc->pipe)) & 0x0fff0fff;
-	OUT_RING(pf | pipesrc);
-	ADVANCE_LP_RING();
-out:
+	intel_ring_emit(ring, pf | pipesrc);
+	intel_ring_advance(ring);
+	return 0;
+
+err_unpin:
+	intel_unpin_fb_obj(obj);
+err:
 	return ret;
 }
 
@@ -7501,18 +6152,22 @@ static int intel_gen7_queue_flip(struct drm_device *dev,
 
 	ret = intel_pin_and_fence_fb_obj(dev, obj, ring);
 	if (ret)
-		goto out;
+		goto err;
 
 	ret = intel_ring_begin(ring, 4);
 	if (ret)
-		goto out;
+		goto err_unpin;
 
 	intel_ring_emit(ring, MI_DISPLAY_FLIP_I915 | (intel_crtc->plane << 19));
 	intel_ring_emit(ring, (fb->pitches[0] | obj->tiling_mode));
 	intel_ring_emit(ring, (obj->gtt_offset));
 	intel_ring_emit(ring, (MI_NOOP));
 	intel_ring_advance(ring);
-out:
+	return 0;
+
+err_unpin:
+	intel_unpin_fb_obj(obj);
+err:
 	return ret;
 }
 
@@ -7589,6 +6244,7 @@ static int intel_crtc_page_flip(struct drm_crtc *crtc,
 		goto cleanup_pending;
 
 	intel_disable_fbc(dev);
+	intel_mark_busy(dev, obj);
 	mutex_unlock(&dev->struct_mutex);
 
 	trace_i915_flip_request(intel_crtc->plane, obj);
@@ -7617,10 +6273,11 @@ static void intel_sanitize_modesetting(struct drm_device *dev,
 {
 	struct drm_i915_private *dev_priv = dev->dev_private;
 	u32 reg, val;
+	int i;
 
 	/* Clear any frame start delays used for debugging left by the BIOS */
-	for_each_pipe(pipe) {
-		reg = PIPECONF(pipe);
+	for_each_pipe(i) {
+		reg = PIPECONF(i);
 		I915_WRITE(reg, I915_READ(reg) & ~PIPECONF_FRAME_START_DELAY_MASK);
 	}
 
@@ -7690,6 +6347,23 @@ static const struct drm_crtc_funcs intel_crtc_funcs = {
 	.page_flip = intel_crtc_page_flip,
 };
 
+static void intel_pch_pll_init(struct drm_device *dev)
+{
+	drm_i915_private_t *dev_priv = dev->dev_private;
+	int i;
+
+	if (dev_priv->num_pch_pll == 0) {
+		DRM_DEBUG_KMS("No PCH PLLs on this hardware, skipping initialisation\n");
+		return;
+	}
+
+	for (i = 0; i < dev_priv->num_pch_pll; i++) {
+		dev_priv->pch_plls[i].pll_reg = _PCH_DPLL(i);
+		dev_priv->pch_plls[i].fp0_reg = _PCH_FP0(i);
+		dev_priv->pch_plls[i].fp1_reg = _PCH_FP1(i);
+	}
+}
+
 static void intel_crtc_init(struct drm_device *dev, int pipe)
 {
 	drm_i915_private_t *dev_priv = dev->dev_private;
@@ -7727,8 +6401,6 @@ static void intel_crtc_init(struct drm_device *dev, int pipe)
 	intel_crtc->bpp = 24; /* default for pre-Ironlake */
 
 	if (HAS_PCH_SPLIT(dev)) {
-		if (pipe == 2 && IS_IVYBRIDGE(dev))
-			intel_crtc->no_pll = true;
 		intel_helper_funcs.prepare = ironlake_crtc_prepare;
 		intel_helper_funcs.commit = ironlake_crtc_commit;
 	} else {
@@ -7747,15 +6419,12 @@ static void intel_crtc_init(struct drm_device *dev, int pipe)
 int intel_get_pipe_from_crtc_id(struct drm_device *dev, void *data,
 				struct drm_file *file)
 {
-	drm_i915_private_t *dev_priv = dev->dev_private;
 	struct drm_i915_get_pipe_from_crtc_id *pipe_from_crtc_id = data;
 	struct drm_mode_object *drmmode_obj;
 	struct intel_crtc *crtc;
 
-	if (!dev_priv) {
-		DRM_ERROR("called with no initialization\n");
-		return -EINVAL;
-	}
+	if (!drm_core_check_feature(dev, DRIVER_MODESET))
+		return -ENODEV;
 
 	drmmode_obj = drm_mode_object_find(dev, pipe_from_crtc_id->crtc_id,
 			DRM_MODE_OBJECT_CRTC);
@@ -7828,12 +6497,31 @@ static void intel_setup_outputs(struct drm_device *dev)
 
 	intel_crt_init(dev);
 
-	if (HAS_PCH_SPLIT(dev)) {
+	if (IS_HASWELL(dev)) {
+		int found;
+
+		/* Haswell uses DDI functions to detect digital outputs */
+		found = I915_READ(DDI_BUF_CTL_A) & DDI_INIT_DISPLAY_DETECTED;
+		/* DDI A only supports eDP */
+		if (found)
+			intel_ddi_init(dev, PORT_A);
+
+		/* DDI B, C and D detection is indicated by the SFUSE_STRAP
+		 * register */
+		found = I915_READ(SFUSE_STRAP);
+
+		if (found & SFUSE_STRAP_DDIB_DETECTED)
+			intel_ddi_init(dev, PORT_B);
+		if (found & SFUSE_STRAP_DDIC_DETECTED)
+			intel_ddi_init(dev, PORT_C);
+		if (found & SFUSE_STRAP_DDID_DETECTED)
+			intel_ddi_init(dev, PORT_D);
+	} else if (HAS_PCH_SPLIT(dev)) {
 		int found;
 
 		if (I915_READ(HDMIB) & PORT_DETECTED) {
 			/* PCH SDVOB multiplex with HDMIB */
-			found = intel_sdvo_init(dev, PCH_SDVOB);
+			found = intel_sdvo_init(dev, PCH_SDVOB, true);
 			if (!found)
 				intel_hdmi_init(dev, HDMIB);
 			if (!found && (I915_READ(PCH_DP_B) & DP_DETECTED))
@@ -7857,7 +6545,7 @@ static void intel_setup_outputs(struct drm_device *dev)
 
 		if (I915_READ(SDVOB) & SDVO_DETECTED) {
 			DRM_DEBUG_KMS("probing SDVOB\n");
-			found = intel_sdvo_init(dev, SDVOB);
+			found = intel_sdvo_init(dev, SDVOB, true);
 			if (!found && SUPPORTS_INTEGRATED_HDMI(dev)) {
 				DRM_DEBUG_KMS("probing HDMI on SDVOB\n");
 				intel_hdmi_init(dev, SDVOB);
@@ -7873,7 +6561,7 @@ static void intel_setup_outputs(struct drm_device *dev)
 
 		if (I915_READ(SDVOB) & SDVO_DETECTED) {
 			DRM_DEBUG_KMS("probing SDVOC\n");
-			found = intel_sdvo_init(dev, SDVOC);
+			found = intel_sdvo_init(dev, SDVOC, false);
 		}
 
 		if (!found && (I915_READ(SDVOC) & SDVO_DETECTED)) {
@@ -8002,882 +6690,6 @@ static const struct drm_mode_config_funcs intel_mode_funcs = {
 	.output_poll_changed = intel_fb_output_poll_changed,
 };
 
-static struct drm_i915_gem_object *
-intel_alloc_context_page(struct drm_device *dev)
-{
-	struct drm_i915_gem_object *ctx;
-	int ret;
-
-	WARN_ON(!mutex_is_locked(&dev->struct_mutex));
-
-	ctx = i915_gem_alloc_object(dev, 4096);
-	if (!ctx) {
-		DRM_DEBUG("failed to alloc power context, RC6 disabled\n");
-		return NULL;
-	}
-
-	ret = i915_gem_object_pin(ctx, 4096, true);
-	if (ret) {
-		DRM_ERROR("failed to pin power context: %d\n", ret);
-		goto err_unref;
-	}
-
-	ret = i915_gem_object_set_to_gtt_domain(ctx, 1);
-	if (ret) {
-		DRM_ERROR("failed to set-domain on power context: %d\n", ret);
-		goto err_unpin;
-	}
-
-	return ctx;
-
-err_unpin:
-	i915_gem_object_unpin(ctx);
-err_unref:
-	drm_gem_object_unreference(&ctx->base);
-	mutex_unlock(&dev->struct_mutex);
-	return NULL;
-}
-
-bool ironlake_set_drps(struct drm_device *dev, u8 val)
-{
-	struct drm_i915_private *dev_priv = dev->dev_private;
-	u16 rgvswctl;
-
-	rgvswctl = I915_READ16(MEMSWCTL);
-	if (rgvswctl & MEMCTL_CMD_STS) {
-		DRM_DEBUG("gpu busy, RCS change rejected\n");
-		return false; /* still busy with another command */
-	}
-
-	rgvswctl = (MEMCTL_CMD_CHFREQ << MEMCTL_CMD_SHIFT) |
-		(val << MEMCTL_FREQ_SHIFT) | MEMCTL_SFCAVM;
-	I915_WRITE16(MEMSWCTL, rgvswctl);
-	POSTING_READ16(MEMSWCTL);
-
-	rgvswctl |= MEMCTL_CMD_STS;
-	I915_WRITE16(MEMSWCTL, rgvswctl);
-
-	return true;
-}
-
-void ironlake_enable_drps(struct drm_device *dev)
-{
-	struct drm_i915_private *dev_priv = dev->dev_private;
-	u32 rgvmodectl = I915_READ(MEMMODECTL);
-	u8 fmax, fmin, fstart, vstart;
-
-	/* Enable temp reporting */
-	I915_WRITE16(PMMISC, I915_READ(PMMISC) | MCPPCE_EN);
-	I915_WRITE16(TSC1, I915_READ(TSC1) | TSE);
-
-	/* 100ms RC evaluation intervals */
-	I915_WRITE(RCUPEI, 100000);
-	I915_WRITE(RCDNEI, 100000);
-
-	/* Set max/min thresholds to 90ms and 80ms respectively */
-	I915_WRITE(RCBMAXAVG, 90000);
-	I915_WRITE(RCBMINAVG, 80000);
-
-	I915_WRITE(MEMIHYST, 1);
-
-	/* Set up min, max, and cur for interrupt handling */
-	fmax = (rgvmodectl & MEMMODE_FMAX_MASK) >> MEMMODE_FMAX_SHIFT;
-	fmin = (rgvmodectl & MEMMODE_FMIN_MASK);
-	fstart = (rgvmodectl & MEMMODE_FSTART_MASK) >>
-		MEMMODE_FSTART_SHIFT;
-
-	vstart = (I915_READ(PXVFREQ_BASE + (fstart * 4)) & PXVFREQ_PX_MASK) >>
-		PXVFREQ_PX_SHIFT;
-
-	dev_priv->fmax = fmax; /* IPS callback will increase this */
-	dev_priv->fstart = fstart;
-
-	dev_priv->max_delay = fstart;
-	dev_priv->min_delay = fmin;
-	dev_priv->cur_delay = fstart;
-
-	DRM_DEBUG_DRIVER("fmax: %d, fmin: %d, fstart: %d\n",
-			 fmax, fmin, fstart);
-
-	I915_WRITE(MEMINTREN, MEMINT_CX_SUPR_EN | MEMINT_EVAL_CHG_EN);
-
-	/*
-	 * Interrupts will be enabled in ironlake_irq_postinstall
-	 */
-
-	I915_WRITE(VIDSTART, vstart);
-	POSTING_READ(VIDSTART);
-
-	rgvmodectl |= MEMMODE_SWMODE_EN;
-	I915_WRITE(MEMMODECTL, rgvmodectl);
-
-	if (wait_for((I915_READ(MEMSWCTL) & MEMCTL_CMD_STS) == 0, 10))
-		DRM_ERROR("stuck trying to change perf mode\n");
-	msleep(1);
-
-	ironlake_set_drps(dev, fstart);
-
-	dev_priv->last_count1 = I915_READ(0x112e4) + I915_READ(0x112e8) +
-		I915_READ(0x112e0);
-	dev_priv->last_time1 = jiffies_to_msecs(jiffies);
-	dev_priv->last_count2 = I915_READ(0x112f4);
-	getrawmonotonic(&dev_priv->last_time2);
-}
-
-void ironlake_disable_drps(struct drm_device *dev)
-{
-	struct drm_i915_private *dev_priv = dev->dev_private;
-	u16 rgvswctl = I915_READ16(MEMSWCTL);
-
-	/* Ack interrupts, disable EFC interrupt */
-	I915_WRITE(MEMINTREN, I915_READ(MEMINTREN) & ~MEMINT_EVAL_CHG_EN);
-	I915_WRITE(MEMINTRSTS, MEMINT_EVAL_CHG);
-	I915_WRITE(DEIER, I915_READ(DEIER) & ~DE_PCU_EVENT);
-	I915_WRITE(DEIIR, DE_PCU_EVENT);
-	I915_WRITE(DEIMR, I915_READ(DEIMR) | DE_PCU_EVENT);
-
-	/* Go back to the starting frequency */
-	ironlake_set_drps(dev, dev_priv->fstart);
-	msleep(1);
-	rgvswctl |= MEMCTL_CMD_STS;
-	I915_WRITE(MEMSWCTL, rgvswctl);
-	msleep(1);
-
-}
-
-void gen6_set_rps(struct drm_device *dev, u8 val)
-{
-	struct drm_i915_private *dev_priv = dev->dev_private;
-	u32 swreq;
-
-	swreq = (val & 0x3ff) << 25;
-	I915_WRITE(GEN6_RPNSWREQ, swreq);
-}
-
-void gen6_disable_rps(struct drm_device *dev)
-{
-	struct drm_i915_private *dev_priv = dev->dev_private;
-
-	I915_WRITE(GEN6_RPNSWREQ, 1 << 31);
-	I915_WRITE(GEN6_PMINTRMSK, 0xffffffff);
-	I915_WRITE(GEN6_PMIER, 0);
-	/* Complete PM interrupt masking here doesn't race with the rps work
-	 * item again unmasking PM interrupts because that is using a different
-	 * register (PMIMR) to mask PM interrupts. The only risk is in leaving
-	 * stale bits in PMIIR and PMIMR which gen6_enable_rps will clean up. */
-
-	spin_lock_irq(&dev_priv->rps_lock);
-	dev_priv->pm_iir = 0;
-	spin_unlock_irq(&dev_priv->rps_lock);
-
-	I915_WRITE(GEN6_PMIIR, I915_READ(GEN6_PMIIR));
-}
-
-static unsigned long intel_pxfreq(u32 vidfreq)
-{
-	unsigned long freq;
-	int div = (vidfreq & 0x3f0000) >> 16;
-	int post = (vidfreq & 0x3000) >> 12;
-	int pre = (vidfreq & 0x7);
-
-	if (!pre)
-		return 0;
-
-	freq = ((div * 133333) / ((1<<post) * pre));
-
-	return freq;
-}
-
-void intel_init_emon(struct drm_device *dev)
-{
-	struct drm_i915_private *dev_priv = dev->dev_private;
-	u32 lcfuse;
-	u8 pxw[16];
-	int i;
-
-	/* Disable to program */
-	I915_WRITE(ECR, 0);
-	POSTING_READ(ECR);
-
-	/* Program energy weights for various events */
-	I915_WRITE(SDEW, 0x15040d00);
-	I915_WRITE(CSIEW0, 0x007f0000);
-	I915_WRITE(CSIEW1, 0x1e220004);
-	I915_WRITE(CSIEW2, 0x04000004);
-
-	for (i = 0; i < 5; i++)
-		I915_WRITE(PEW + (i * 4), 0);
-	for (i = 0; i < 3; i++)
-		I915_WRITE(DEW + (i * 4), 0);
-
-	/* Program P-state weights to account for frequency power adjustment */
-	for (i = 0; i < 16; i++) {
-		u32 pxvidfreq = I915_READ(PXVFREQ_BASE + (i * 4));
-		unsigned long freq = intel_pxfreq(pxvidfreq);
-		unsigned long vid = (pxvidfreq & PXVFREQ_PX_MASK) >>
-			PXVFREQ_PX_SHIFT;
-		unsigned long val;
-
-		val = vid * vid;
-		val *= (freq / 1000);
-		val *= 255;
-		val /= (127*127*900);
-		if (val > 0xff)
-			DRM_ERROR("bad pxval: %ld\n", val);
-		pxw[i] = val;
-	}
-	/* Render standby states get 0 weight */
-	pxw[14] = 0;
-	pxw[15] = 0;
-
-	for (i = 0; i < 4; i++) {
-		u32 val = (pxw[i*4] << 24) | (pxw[(i*4)+1] << 16) |
-			(pxw[(i*4)+2] << 8) | (pxw[(i*4)+3]);
-		I915_WRITE(PXW + (i * 4), val);
-	}
-
-	/* Adjust magic regs to magic values (more experimental results) */
-	I915_WRITE(OGW0, 0);
-	I915_WRITE(OGW1, 0);
-	I915_WRITE(EG0, 0x00007f00);
-	I915_WRITE(EG1, 0x0000000e);
-	I915_WRITE(EG2, 0x000e0000);
-	I915_WRITE(EG3, 0x68000300);
-	I915_WRITE(EG4, 0x42000000);
-	I915_WRITE(EG5, 0x00140031);
-	I915_WRITE(EG6, 0);
-	I915_WRITE(EG7, 0);
-
-	for (i = 0; i < 8; i++)
-		I915_WRITE(PXWL + (i * 4), 0);
-
-	/* Enable PMON + select events */
-	I915_WRITE(ECR, 0x80000019);
-
-	lcfuse = I915_READ(LCFUSE02);
-
-	dev_priv->corr = (lcfuse & LCFUSE_HIV_MASK);
-}
-
-static int intel_enable_rc6(struct drm_device *dev)
-{
-	/*
-	 * Respect the kernel parameter if it is set
-	 */
-	if (i915_enable_rc6 >= 0)
-		return i915_enable_rc6;
-
-	/*
-	 * Disable RC6 on Ironlake
-	 */
-	if (INTEL_INFO(dev)->gen == 5)
-		return 0;
-
-	/*
-	 * Disable rc6 on Sandybridge
-	 */
-	if (INTEL_INFO(dev)->gen == 6) {
-		DRM_DEBUG_DRIVER("Sandybridge: deep RC6 disabled\n");
-		return INTEL_RC6_ENABLE;
-	}
-	DRM_DEBUG_DRIVER("RC6 and deep RC6 enabled\n");
-	return (INTEL_RC6_ENABLE | INTEL_RC6p_ENABLE);
-}
-
-void gen6_enable_rps(struct drm_i915_private *dev_priv)
-{
-	u32 rp_state_cap = I915_READ(GEN6_RP_STATE_CAP);
-	u32 gt_perf_status = I915_READ(GEN6_GT_PERF_STATUS);
-	u32 pcu_mbox, rc6_mask = 0;
-	u32 gtfifodbg;
-	int cur_freq, min_freq, max_freq;
-	int rc6_mode;
-	int i;
-
-	/* Here begins a magic sequence of register writes to enable
-	 * auto-downclocking.
-	 *
-	 * Perhaps there might be some value in exposing these to
-	 * userspace...
-	 */
-	I915_WRITE(GEN6_RC_STATE, 0);
-	mutex_lock(&dev_priv->dev->struct_mutex);
-
-	/* Clear the DBG now so we don't confuse earlier errors */
-	if ((gtfifodbg = I915_READ(GTFIFODBG))) {
-		DRM_ERROR("GT fifo had a previous error %x\n", gtfifodbg);
-		I915_WRITE(GTFIFODBG, gtfifodbg);
-	}
-
-	gen6_gt_force_wake_get(dev_priv);
-
-	/* disable the counters and set deterministic thresholds */
-	I915_WRITE(GEN6_RC_CONTROL, 0);
-
-	I915_WRITE(GEN6_RC1_WAKE_RATE_LIMIT, 1000 << 16);
-	I915_WRITE(GEN6_RC6_WAKE_RATE_LIMIT, 40 << 16 | 30);
-	I915_WRITE(GEN6_RC6pp_WAKE_RATE_LIMIT, 30);
-	I915_WRITE(GEN6_RC_EVALUATION_INTERVAL, 125000);
-	I915_WRITE(GEN6_RC_IDLE_HYSTERSIS, 25);
-
-	for (i = 0; i < I915_NUM_RINGS; i++)
-		I915_WRITE(RING_MAX_IDLE(dev_priv->ring[i].mmio_base), 10);
-
-	I915_WRITE(GEN6_RC_SLEEP, 0);
-	I915_WRITE(GEN6_RC1e_THRESHOLD, 1000);
-	I915_WRITE(GEN6_RC6_THRESHOLD, 50000);
-	I915_WRITE(GEN6_RC6p_THRESHOLD, 100000);
-	I915_WRITE(GEN6_RC6pp_THRESHOLD, 64000); /* unused */
-
-	rc6_mode = intel_enable_rc6(dev_priv->dev);
-	if (rc6_mode & INTEL_RC6_ENABLE)
-		rc6_mask |= GEN6_RC_CTL_RC6_ENABLE;
-
-	if (rc6_mode & INTEL_RC6p_ENABLE)
-		rc6_mask |= GEN6_RC_CTL_RC6p_ENABLE;
-
-	if (rc6_mode & INTEL_RC6pp_ENABLE)
-		rc6_mask |= GEN6_RC_CTL_RC6pp_ENABLE;
-
-	DRM_INFO("Enabling RC6 states: RC6 %s, RC6p %s, RC6pp %s\n",
-			(rc6_mode & INTEL_RC6_ENABLE) ? "on" : "off",
-			(rc6_mode & INTEL_RC6p_ENABLE) ? "on" : "off",
-			(rc6_mode & INTEL_RC6pp_ENABLE) ? "on" : "off");
-
-	I915_WRITE(GEN6_RC_CONTROL,
-		   rc6_mask |
-		   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));
-
-	I915_WRITE(GEN6_RP_DOWN_TIMEOUT, 1000000);
-	I915_WRITE(GEN6_RP_INTERRUPT_LIMITS,
-		   18 << 24 |
-		   6 << 16);
-	I915_WRITE(GEN6_RP_UP_THRESHOLD, 10000);
-	I915_WRITE(GEN6_RP_DOWN_THRESHOLD, 1000000);
-	I915_WRITE(GEN6_RP_UP_EI, 100000);
-	I915_WRITE(GEN6_RP_DOWN_EI, 5000000);
-	I915_WRITE(GEN6_RP_IDLE_HYSTERSIS, 10);
-	I915_WRITE(GEN6_RP_CONTROL,
-		   GEN6_RP_MEDIA_TURBO |
-		   GEN6_RP_MEDIA_HW_MODE |
-		   GEN6_RP_MEDIA_IS_GFX |
-		   GEN6_RP_ENABLE |
-		   GEN6_RP_UP_BUSY_AVG |
-		   GEN6_RP_DOWN_IDLE_CONT);
-
-	if (wait_for((I915_READ(GEN6_PCODE_MAILBOX) & GEN6_PCODE_READY) == 0,
-		     500))
-		DRM_ERROR("timeout waiting for pcode mailbox to become idle\n");
-
-	I915_WRITE(GEN6_PCODE_DATA, 0);
-	I915_WRITE(GEN6_PCODE_MAILBOX,
-		   GEN6_PCODE_READY |
-		   GEN6_PCODE_WRITE_MIN_FREQ_TABLE);
-	if (wait_for((I915_READ(GEN6_PCODE_MAILBOX) & GEN6_PCODE_READY) == 0,
-		     500))
-		DRM_ERROR("timeout waiting for pcode mailbox to finish\n");
-
-	min_freq = (rp_state_cap & 0xff0000) >> 16;
-	max_freq = rp_state_cap & 0xff;
-	cur_freq = (gt_perf_status & 0xff00) >> 8;
-
-	/* Check for overclock support */
-	if (wait_for((I915_READ(GEN6_PCODE_MAILBOX) & GEN6_PCODE_READY) == 0,
-		     500))
-		DRM_ERROR("timeout waiting for pcode mailbox to become idle\n");
-	I915_WRITE(GEN6_PCODE_MAILBOX, GEN6_READ_OC_PARAMS);
-	pcu_mbox = I915_READ(GEN6_PCODE_DATA);
-	if (wait_for((I915_READ(GEN6_PCODE_MAILBOX) & GEN6_PCODE_READY) == 0,
-		     500))
-		DRM_ERROR("timeout waiting for pcode mailbox to finish\n");
-	if (pcu_mbox & (1<<31)) { /* OC supported */
-		max_freq = pcu_mbox & 0xff;
-		DRM_DEBUG_DRIVER("overclocking supported, adjusting frequency max to %dMHz\n", pcu_mbox * 50);
-	}
-
-	/* In units of 100MHz */
-	dev_priv->max_delay = max_freq;
-	dev_priv->min_delay = min_freq;
-	dev_priv->cur_delay = cur_freq;
-
-	/* requires MSI enabled */
-	I915_WRITE(GEN6_PMIER,
-		   GEN6_PM_MBOX_EVENT |
-		   GEN6_PM_THERMAL_EVENT |
-		   GEN6_PM_RP_DOWN_TIMEOUT |
-		   GEN6_PM_RP_UP_THRESHOLD |
-		   GEN6_PM_RP_DOWN_THRESHOLD |
-		   GEN6_PM_RP_UP_EI_EXPIRED |
-		   GEN6_PM_RP_DOWN_EI_EXPIRED);
-	spin_lock_irq(&dev_priv->rps_lock);
-	WARN_ON(dev_priv->pm_iir != 0);
-	I915_WRITE(GEN6_PMIMR, 0);
-	spin_unlock_irq(&dev_priv->rps_lock);
-	/* enable all PM interrupts */
-	I915_WRITE(GEN6_PMINTRMSK, 0);
-
-	gen6_gt_force_wake_put(dev_priv);
-	mutex_unlock(&dev_priv->dev->struct_mutex);
-}
-
-void gen6_update_ring_freq(struct drm_i915_private *dev_priv)
-{
-	int min_freq = 15;
-	int gpu_freq, ia_freq, max_ia_freq;
-	int scaling_factor = 180;
-
-	max_ia_freq = cpufreq_quick_get_max(0);
-	/*
-	 * Default to measured freq if none found, PCU will ensure we don't go
-	 * over
-	 */
-	if (!max_ia_freq)
-		max_ia_freq = tsc_khz;
-
-	/* Convert from kHz to MHz */
-	max_ia_freq /= 1000;
-
-	mutex_lock(&dev_priv->dev->struct_mutex);
-
-	/*
-	 * 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
-	 * the PCU should use as a reference to determine the ring frequency.
-	 */
-	for (gpu_freq = dev_priv->max_delay; gpu_freq >= dev_priv->min_delay;
-	     gpu_freq--) {
-		int diff = dev_priv->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);
-
-		I915_WRITE(GEN6_PCODE_DATA,
-			   (ia_freq << GEN6_PCODE_FREQ_IA_RATIO_SHIFT) |
-			   gpu_freq);
-		I915_WRITE(GEN6_PCODE_MAILBOX, GEN6_PCODE_READY |
-			   GEN6_PCODE_WRITE_MIN_FREQ_TABLE);
-		if (wait_for((I915_READ(GEN6_PCODE_MAILBOX) &
-			      GEN6_PCODE_READY) == 0, 10)) {
-			DRM_ERROR("pcode write of freq table timed out\n");
-			continue;
-		}
-	}
-
-	mutex_unlock(&dev_priv->dev->struct_mutex);
-}
-
-static void ironlake_init_clock_gating(struct drm_device *dev)
-{
-	struct drm_i915_private *dev_priv = dev->dev_private;
-	uint32_t dspclk_gate = VRHUNIT_CLOCK_GATE_DISABLE;
-
-	/* Required for FBC */
-	dspclk_gate |= DPFCUNIT_CLOCK_GATE_DISABLE |
-		DPFCRUNIT_CLOCK_GATE_DISABLE |
-		DPFDUNIT_CLOCK_GATE_DISABLE;
-	/* Required for CxSR */
-	dspclk_gate |= DPARBUNIT_CLOCK_GATE_DISABLE;
-
-	I915_WRITE(PCH_3DCGDIS0,
-		   MARIUNIT_CLOCK_GATE_DISABLE |
-		   SVSMUNIT_CLOCK_GATE_DISABLE);
-	I915_WRITE(PCH_3DCGDIS1,
-		   VFMUNIT_CLOCK_GATE_DISABLE);
-
-	I915_WRITE(PCH_DSPCLK_GATE_D, dspclk_gate);
-
-	/*
-	 * According to the spec the following bits should be set in
-	 * order to enable memory self-refresh
-	 * The bit 22/21 of 0x42004
-	 * The bit 5 of 0x42020
-	 * The bit 15 of 0x45000
-	 */
-	I915_WRITE(ILK_DISPLAY_CHICKEN2,
-		   (I915_READ(ILK_DISPLAY_CHICKEN2) |
-		    ILK_DPARB_GATE | ILK_VSDPFD_FULL));
-	I915_WRITE(ILK_DSPCLK_GATE,
-		   (I915_READ(ILK_DSPCLK_GATE) |
-		    ILK_DPARB_CLK_GATE));
-	I915_WRITE(DISP_ARB_CTL,
-		   (I915_READ(DISP_ARB_CTL) |
-		    DISP_FBC_WM_DIS));
-	I915_WRITE(WM3_LP_ILK, 0);
-	I915_WRITE(WM2_LP_ILK, 0);
-	I915_WRITE(WM1_LP_ILK, 0);
-
-	/*
-	 * Based on the document from hardware guys the following bits
-	 * should be set unconditionally in order to enable FBC.
-	 * The bit 22 of 0x42000
-	 * The bit 22 of 0x42004
-	 * The bit 7,8,9 of 0x42020.
-	 */
-	if (IS_IRONLAKE_M(dev)) {
-		I915_WRITE(ILK_DISPLAY_CHICKEN1,
-			   I915_READ(ILK_DISPLAY_CHICKEN1) |
-			   ILK_FBCQ_DIS);
-		I915_WRITE(ILK_DISPLAY_CHICKEN2,
-			   I915_READ(ILK_DISPLAY_CHICKEN2) |
-			   ILK_DPARB_GATE);
-		I915_WRITE(ILK_DSPCLK_GATE,
-			   I915_READ(ILK_DSPCLK_GATE) |
-			   ILK_DPFC_DIS1 |
-			   ILK_DPFC_DIS2 |
-			   ILK_CLK_FBC);
-	}
-
-	I915_WRITE(ILK_DISPLAY_CHICKEN2,
-		   I915_READ(ILK_DISPLAY_CHICKEN2) |
-		   ILK_ELPIN_409_SELECT);
-	I915_WRITE(_3D_CHICKEN2,
-		   _3D_CHICKEN2_WM_READ_PIPELINED << 16 |
-		   _3D_CHICKEN2_WM_READ_PIPELINED);
-}
-
-static void gen6_init_clock_gating(struct drm_device *dev)
-{
-	struct drm_i915_private *dev_priv = dev->dev_private;
-	int pipe;
-	uint32_t dspclk_gate = VRHUNIT_CLOCK_GATE_DISABLE;
-
-	I915_WRITE(PCH_DSPCLK_GATE_D, dspclk_gate);
-
-	I915_WRITE(ILK_DISPLAY_CHICKEN2,
-		   I915_READ(ILK_DISPLAY_CHICKEN2) |
-		   ILK_ELPIN_409_SELECT);
-
-	I915_WRITE(WM3_LP_ILK, 0);
-	I915_WRITE(WM2_LP_ILK, 0);
-	I915_WRITE(WM1_LP_ILK, 0);
-
-	I915_WRITE(GEN6_UCGCTL1,
-		   I915_READ(GEN6_UCGCTL1) |
-		   GEN6_BLBUNIT_CLOCK_GATE_DISABLE);
-
-	/* According to the BSpec vol1g, bit 12 (RCPBUNIT) clock
-	 * gating disable must be set.  Failure to set it results in
-	 * flickering pixels due to Z write ordering failures after
-	 * some amount of runtime in the Mesa "fire" demo, and Unigine
-	 * Sanctuary and Tropics, and apparently anything else with
-	 * alpha test or pixel discard.
-	 *
-	 * According to the spec, bit 11 (RCCUNIT) must also be set,
-	 * but we didn't debug actual testcases to find it out.
-	 */
-	I915_WRITE(GEN6_UCGCTL2,
-		   GEN6_RCPBUNIT_CLOCK_GATE_DISABLE |
-		   GEN6_RCCUNIT_CLOCK_GATE_DISABLE);
-
-	/*
-	 * According to the spec the following bits should be
-	 * set in order to enable memory self-refresh and fbc:
-	 * The bit21 and bit22 of 0x42000
-	 * The bit21 and bit22 of 0x42004
-	 * The bit5 and bit7 of 0x42020
-	 * The bit14 of 0x70180
-	 * The bit14 of 0x71180
-	 */
-	I915_WRITE(ILK_DISPLAY_CHICKEN1,
-		   I915_READ(ILK_DISPLAY_CHICKEN1) |
-		   ILK_FBCQ_DIS | ILK_PABSTRETCH_DIS);
-	I915_WRITE(ILK_DISPLAY_CHICKEN2,
-		   I915_READ(ILK_DISPLAY_CHICKEN2) |
-		   ILK_DPARB_GATE | ILK_VSDPFD_FULL);
-	I915_WRITE(ILK_DSPCLK_GATE,
-		   I915_READ(ILK_DSPCLK_GATE) |
-		   ILK_DPARB_CLK_GATE  |
-		   ILK_DPFD_CLK_GATE);
-
-	for_each_pipe(pipe) {
-		I915_WRITE(DSPCNTR(pipe),
-			   I915_READ(DSPCNTR(pipe)) |
-			   DISPPLANE_TRICKLE_FEED_DISABLE);
-		intel_flush_display_plane(dev_priv, pipe);
-	}
-}
-
-static void ivybridge_init_clock_gating(struct drm_device *dev)
-{
-	struct drm_i915_private *dev_priv = dev->dev_private;
-	int pipe;
-	uint32_t dspclk_gate = VRHUNIT_CLOCK_GATE_DISABLE;
-
-	I915_WRITE(PCH_DSPCLK_GATE_D, dspclk_gate);
-
-	I915_WRITE(WM3_LP_ILK, 0);
-	I915_WRITE(WM2_LP_ILK, 0);
-	I915_WRITE(WM1_LP_ILK, 0);
-
-	/* According to the spec, bit 13 (RCZUNIT) must be set on IVB.
-	 * This implements the WaDisableRCZUnitClockGating workaround.
-	 */
-	I915_WRITE(GEN6_UCGCTL2, GEN6_RCZUNIT_CLOCK_GATE_DISABLE);
-
-	I915_WRITE(ILK_DSPCLK_GATE, IVB_VRHUNIT_CLK_GATE);
-
-	I915_WRITE(IVB_CHICKEN3,
-		   CHICKEN3_DGMG_REQ_OUT_FIX_DISABLE |
-		   CHICKEN3_DGMG_DONE_FIX_DISABLE);
-
-	/* Apply the WaDisableRHWOOptimizationForRenderHang workaround. */
-	I915_WRITE(GEN7_COMMON_SLICE_CHICKEN1,
-		   GEN7_CSC1_RHWO_OPT_DISABLE_IN_RCC);
-
-	/* WaApplyL3ControlAndL3ChickenMode requires those two on Ivy Bridge */
-	I915_WRITE(GEN7_L3CNTLREG1,
-			GEN7_WA_FOR_GEN7_L3_CONTROL);
-	I915_WRITE(GEN7_L3_CHICKEN_MODE_REGISTER,
-			GEN7_WA_L3_CHICKEN_MODE);
-
-	/* This is required by WaCatErrorRejectionIssue */
-	I915_WRITE(GEN7_SQ_CHICKEN_MBCUNIT_CONFIG,
-			I915_READ(GEN7_SQ_CHICKEN_MBCUNIT_CONFIG) |
-			GEN7_SQ_CHICKEN_MBCUNIT_SQINTMOB);
-
-	for_each_pipe(pipe) {
-		I915_WRITE(DSPCNTR(pipe),
-			   I915_READ(DSPCNTR(pipe)) |
-			   DISPPLANE_TRICKLE_FEED_DISABLE);
-		intel_flush_display_plane(dev_priv, pipe);
-	}
-}
-
-static void g4x_init_clock_gating(struct drm_device *dev)
-{
-	struct drm_i915_private *dev_priv = dev->dev_private;
-	uint32_t dspclk_gate;
-
-	I915_WRITE(RENCLK_GATE_D1, 0);
-	I915_WRITE(RENCLK_GATE_D2, VF_UNIT_CLOCK_GATE_DISABLE |
-		   GS_UNIT_CLOCK_GATE_DISABLE |
-		   CL_UNIT_CLOCK_GATE_DISABLE);
-	I915_WRITE(RAMCLK_GATE_D, 0);
-	dspclk_gate = VRHUNIT_CLOCK_GATE_DISABLE |
-		OVRUNIT_CLOCK_GATE_DISABLE |
-		OVCUNIT_CLOCK_GATE_DISABLE;
-	if (IS_GM45(dev))
-		dspclk_gate |= DSSUNIT_CLOCK_GATE_DISABLE;
-	I915_WRITE(DSPCLK_GATE_D, dspclk_gate);
-}
-
-static void crestline_init_clock_gating(struct drm_device *dev)
-{
-	struct drm_i915_private *dev_priv = dev->dev_private;
-
-	I915_WRITE(RENCLK_GATE_D1, I965_RCC_CLOCK_GATE_DISABLE);
-	I915_WRITE(RENCLK_GATE_D2, 0);
-	I915_WRITE(DSPCLK_GATE_D, 0);
-	I915_WRITE(RAMCLK_GATE_D, 0);
-	I915_WRITE16(DEUC, 0);
-}
-
-static void broadwater_init_clock_gating(struct drm_device *dev)
-{
-	struct drm_i915_private *dev_priv = dev->dev_private;
-
-	I915_WRITE(RENCLK_GATE_D1, I965_RCZ_CLOCK_GATE_DISABLE |
-		   I965_RCC_CLOCK_GATE_DISABLE |
-		   I965_RCPB_CLOCK_GATE_DISABLE |
-		   I965_ISC_CLOCK_GATE_DISABLE |
-		   I965_FBC_CLOCK_GATE_DISABLE);
-	I915_WRITE(RENCLK_GATE_D2, 0);
-}
-
-static void gen3_init_clock_gating(struct drm_device *dev)
-{
-	struct drm_i915_private *dev_priv = dev->dev_private;
-	u32 dstate = I915_READ(D_STATE);
-
-	dstate |= DSTATE_PLL_D3_OFF | DSTATE_GFX_CLOCK_GATING |
-		DSTATE_DOT_CLOCK_GATING;
-	I915_WRITE(D_STATE, dstate);
-}
-
-static void i85x_init_clock_gating(struct drm_device *dev)
-{
-	struct drm_i915_private *dev_priv = dev->dev_private;
-
-	I915_WRITE(RENCLK_GATE_D1, SV_CLOCK_GATE_DISABLE);
-}
-
-static void i830_init_clock_gating(struct drm_device *dev)
-{
-	struct drm_i915_private *dev_priv = dev->dev_private;
-
-	I915_WRITE(DSPCLK_GATE_D, OVRUNIT_CLOCK_GATE_DISABLE);
-}
-
-static void ibx_init_clock_gating(struct drm_device *dev)
-{
-	struct drm_i915_private *dev_priv = dev->dev_private;
-
-	/*
-	 * On Ibex Peak and Cougar Point, we need to disable clock
-	 * gating for the panel power sequencer or it will fail to
-	 * start up when no ports are active.
-	 */
-	I915_WRITE(SOUTH_DSPCLK_GATE_D, PCH_DPLSUNIT_CLOCK_GATE_DISABLE);
-}
-
-static void cpt_init_clock_gating(struct drm_device *dev)
-{
-	struct drm_i915_private *dev_priv = dev->dev_private;
-	int pipe;
-
-	/*
-	 * On Ibex Peak and Cougar Point, we need to disable clock
-	 * gating for the panel power sequencer or it will fail to
-	 * start up when no ports are active.
-	 */
-	I915_WRITE(SOUTH_DSPCLK_GATE_D, PCH_DPLSUNIT_CLOCK_GATE_DISABLE);
-	I915_WRITE(SOUTH_CHICKEN2, I915_READ(SOUTH_CHICKEN2) |
-		   DPLS_EDP_PPS_FIX_DIS);
-	/* Without this, mode sets may fail silently on FDI */
-	for_each_pipe(pipe)
-		I915_WRITE(TRANS_CHICKEN2(pipe), TRANS_AUTOTRAIN_GEN_STALL_DIS);
-}
-
-static void ironlake_teardown_rc6(struct drm_device *dev)
-{
-	struct drm_i915_private *dev_priv = dev->dev_private;
-
-	if (dev_priv->renderctx) {
-		i915_gem_object_unpin(dev_priv->renderctx);
-		drm_gem_object_unreference(&dev_priv->renderctx->base);
-		dev_priv->renderctx = NULL;
-	}
-
-	if (dev_priv->pwrctx) {
-		i915_gem_object_unpin(dev_priv->pwrctx);
-		drm_gem_object_unreference(&dev_priv->pwrctx->base);
-		dev_priv->pwrctx = NULL;
-	}
-}
-
-static void ironlake_disable_rc6(struct drm_device *dev)
-{
-	struct drm_i915_private *dev_priv = dev->dev_private;
-
-	if (I915_READ(PWRCTXA)) {
-		/* Wake the GPU, prevent RC6, then restore RSTDBYCTL */
-		I915_WRITE(RSTDBYCTL, I915_READ(RSTDBYCTL) | RCX_SW_EXIT);
-		wait_for(((I915_READ(RSTDBYCTL) & RSX_STATUS_MASK) == RSX_STATUS_ON),
-			 50);
-
-		I915_WRITE(PWRCTXA, 0);
-		POSTING_READ(PWRCTXA);
-
-		I915_WRITE(RSTDBYCTL, I915_READ(RSTDBYCTL) & ~RCX_SW_EXIT);
-		POSTING_READ(RSTDBYCTL);
-	}
-
-	ironlake_teardown_rc6(dev);
-}
-
-static int ironlake_setup_rc6(struct drm_device *dev)
-{
-	struct drm_i915_private *dev_priv = dev->dev_private;
-
-	if (dev_priv->renderctx == NULL)
-		dev_priv->renderctx = intel_alloc_context_page(dev);
-	if (!dev_priv->renderctx)
-		return -ENOMEM;
-
-	if (dev_priv->pwrctx == NULL)
-		dev_priv->pwrctx = intel_alloc_context_page(dev);
-	if (!dev_priv->pwrctx) {
-		ironlake_teardown_rc6(dev);
-		return -ENOMEM;
-	}
-
-	return 0;
-}
-
-void ironlake_enable_rc6(struct drm_device *dev)
-{
-	struct drm_i915_private *dev_priv = dev->dev_private;
-	int ret;
-
-	/* rc6 disabled by default due to repeated reports of hanging during
-	 * boot and resume.
-	 */
-	if (!intel_enable_rc6(dev))
-		return;
-
-	mutex_lock(&dev->struct_mutex);
-	ret = ironlake_setup_rc6(dev);
-	if (ret) {
-		mutex_unlock(&dev->struct_mutex);
-		return;
-	}
-
-	/*
-	 * GPU can automatically power down the render unit if given a page
-	 * to save state.
-	 */
-	ret = BEGIN_LP_RING(6);
-	if (ret) {
-		ironlake_teardown_rc6(dev);
-		mutex_unlock(&dev->struct_mutex);
-		return;
-	}
-
-	OUT_RING(MI_SUSPEND_FLUSH | MI_SUSPEND_FLUSH_EN);
-	OUT_RING(MI_SET_CONTEXT);
-	OUT_RING(dev_priv->renderctx->gtt_offset |
-		 MI_MM_SPACE_GTT |
-		 MI_SAVE_EXT_STATE_EN |
-		 MI_RESTORE_EXT_STATE_EN |
-		 MI_RESTORE_INHIBIT);
-	OUT_RING(MI_SUSPEND_FLUSH);
-	OUT_RING(MI_NOOP);
-	OUT_RING(MI_FLUSH);
-	ADVANCE_LP_RING();
-
-	/*
-	 * Wait for the command parser to advance past MI_SET_CONTEXT. The HW
-	 * does an implicit flush, combined with MI_FLUSH above, it should be
-	 * safe to assume that renderctx is valid
-	 */
-	ret = intel_wait_ring_idle(LP_RING(dev_priv));
-	if (ret) {
-		DRM_ERROR("failed to enable ironlake power power savings\n");
-		ironlake_teardown_rc6(dev);
-		mutex_unlock(&dev->struct_mutex);
-		return;
-	}
-
-	I915_WRITE(PWRCTXA, dev_priv->pwrctx->gtt_offset | PWRCTX_EN);
-	I915_WRITE(RSTDBYCTL, I915_READ(RSTDBYCTL) & ~RCX_SW_EXIT);
-	mutex_unlock(&dev->struct_mutex);
-}
-
-void intel_init_clock_gating(struct drm_device *dev)
-{
-	struct drm_i915_private *dev_priv = dev->dev_private;
-
-	dev_priv->display.init_clock_gating(dev);
-
-	if (dev_priv->display.init_pch_clock_gating)
-		dev_priv->display.init_pch_clock_gating(dev);
-}
-
 /* Set up chip specific display functions */
 static void intel_init_display(struct drm_device *dev)
 {
@@ -8887,32 +6699,20 @@ static void intel_init_display(struct drm_device *dev)
 	if (HAS_PCH_SPLIT(dev)) {
 		dev_priv->display.dpms = ironlake_crtc_dpms;
 		dev_priv->display.crtc_mode_set = ironlake_crtc_mode_set;
+		dev_priv->display.off = ironlake_crtc_off;
 		dev_priv->display.update_plane = ironlake_update_plane;
 	} else {
 		dev_priv->display.dpms = i9xx_crtc_dpms;
 		dev_priv->display.crtc_mode_set = i9xx_crtc_mode_set;
+		dev_priv->display.off = i9xx_crtc_off;
 		dev_priv->display.update_plane = i9xx_update_plane;
 	}
 
-	if (I915_HAS_FBC(dev)) {
-		if (HAS_PCH_SPLIT(dev)) {
-			dev_priv->display.fbc_enabled = ironlake_fbc_enabled;
-			dev_priv->display.enable_fbc = ironlake_enable_fbc;
-			dev_priv->display.disable_fbc = ironlake_disable_fbc;
-		} else if (IS_GM45(dev)) {
-			dev_priv->display.fbc_enabled = g4x_fbc_enabled;
-			dev_priv->display.enable_fbc = g4x_enable_fbc;
-			dev_priv->display.disable_fbc = g4x_disable_fbc;
-		} else if (IS_CRESTLINE(dev)) {
-			dev_priv->display.fbc_enabled = i8xx_fbc_enabled;
-			dev_priv->display.enable_fbc = i8xx_enable_fbc;
-			dev_priv->display.disable_fbc = i8xx_disable_fbc;
-		}
-		/* 855GM needs testing */
-	}
-
 	/* Returns the core display clock speed */
-	if (IS_I945G(dev) || (IS_G33(dev) && !IS_PINEVIEW_M(dev)))
+	if (IS_VALLEYVIEW(dev))
+		dev_priv->display.get_display_clock_speed =
+			valleyview_get_display_clock_speed;
+	else if (IS_I945G(dev) || (IS_G33(dev) && !IS_PINEVIEW_M(dev)))
 		dev_priv->display.get_display_clock_speed =
 			i945_get_display_clock_speed;
 	else if (IS_I915G(dev))
@@ -8934,124 +6734,27 @@ static void intel_init_display(struct drm_device *dev)
 		dev_priv->display.get_display_clock_speed =
 			i830_get_display_clock_speed;
 
-	/* For FIFO watermark updates */
 	if (HAS_PCH_SPLIT(dev)) {
-		dev_priv->display.force_wake_get = __gen6_gt_force_wake_get;
-		dev_priv->display.force_wake_put = __gen6_gt_force_wake_put;
-
-		/* IVB configs may use multi-threaded forcewake */
-		if (IS_IVYBRIDGE(dev)) {
-			u32	ecobus;
-
-			/* A small trick here - if the bios hasn't configured MT forcewake,
-			 * and if the device is in RC6, then force_wake_mt_get will not wake
-			 * the device and the ECOBUS read will return zero. Which will be
-			 * (correctly) interpreted by the test below as MT forcewake being
-			 * disabled.
-			 */
-			mutex_lock(&dev->struct_mutex);
-			__gen6_gt_force_wake_mt_get(dev_priv);
-			ecobus = I915_READ_NOTRACE(ECOBUS);
-			__gen6_gt_force_wake_mt_put(dev_priv);
-			mutex_unlock(&dev->struct_mutex);
-
-			if (ecobus & FORCEWAKE_MT_ENABLE) {
-				DRM_DEBUG_KMS("Using MT version of forcewake\n");
-				dev_priv->display.force_wake_get =
-					__gen6_gt_force_wake_mt_get;
-				dev_priv->display.force_wake_put =
-					__gen6_gt_force_wake_mt_put;
-			}
-		}
-
-		if (HAS_PCH_IBX(dev))
-			dev_priv->display.init_pch_clock_gating = ibx_init_clock_gating;
-		else if (HAS_PCH_CPT(dev))
-			dev_priv->display.init_pch_clock_gating = cpt_init_clock_gating;
-
 		if (IS_GEN5(dev)) {
-			if (I915_READ(MLTR_ILK) & ILK_SRLT_MASK)
-				dev_priv->display.update_wm = ironlake_update_wm;
-			else {
-				DRM_DEBUG_KMS("Failed to get proper latency. "
-					      "Disable CxSR\n");
-				dev_priv->display.update_wm = NULL;
-			}
 			dev_priv->display.fdi_link_train = ironlake_fdi_link_train;
-			dev_priv->display.init_clock_gating = ironlake_init_clock_gating;
 			dev_priv->display.write_eld = ironlake_write_eld;
 		} else if (IS_GEN6(dev)) {
-			if (SNB_READ_WM0_LATENCY()) {
-				dev_priv->display.update_wm = sandybridge_update_wm;
-				dev_priv->display.update_sprite_wm = sandybridge_update_sprite_wm;
-			} else {
-				DRM_DEBUG_KMS("Failed to read display plane latency. "
-					      "Disable CxSR\n");
-				dev_priv->display.update_wm = NULL;
-			}
 			dev_priv->display.fdi_link_train = gen6_fdi_link_train;
-			dev_priv->display.init_clock_gating = gen6_init_clock_gating;
 			dev_priv->display.write_eld = ironlake_write_eld;
 		} else if (IS_IVYBRIDGE(dev)) {
 			/* FIXME: detect B0+ stepping and use auto training */
 			dev_priv->display.fdi_link_train = ivb_manual_fdi_link_train;
-			if (SNB_READ_WM0_LATENCY()) {
-				dev_priv->display.update_wm = sandybridge_update_wm;
-				dev_priv->display.update_sprite_wm = sandybridge_update_sprite_wm;
-			} else {
-				DRM_DEBUG_KMS("Failed to read display plane latency. "
-					      "Disable CxSR\n");
-				dev_priv->display.update_wm = NULL;
-			}
-			dev_priv->display.init_clock_gating = ivybridge_init_clock_gating;
+			dev_priv->display.write_eld = ironlake_write_eld;
+		} else if (IS_HASWELL(dev)) {
+			dev_priv->display.fdi_link_train = hsw_fdi_link_train;
 			dev_priv->display.write_eld = ironlake_write_eld;
 		} else
 			dev_priv->display.update_wm = NULL;
-	} else if (IS_PINEVIEW(dev)) {
-		if (!intel_get_cxsr_latency(IS_PINEVIEW_G(dev),
-					    dev_priv->is_ddr3,
-					    dev_priv->fsb_freq,
-					    dev_priv->mem_freq)) {
-			DRM_INFO("failed to find known CxSR latency "
-				 "(found ddr%s fsb freq %d, mem freq %d), "
-				 "disabling CxSR\n",
-				 (dev_priv->is_ddr3 == 1) ? "3" : "2",
-				 dev_priv->fsb_freq, dev_priv->mem_freq);
-			/* Disable CxSR and never update its watermark again */
-			pineview_disable_cxsr(dev);
-			dev_priv->display.update_wm = NULL;
-		} else
-			dev_priv->display.update_wm = pineview_update_wm;
-		dev_priv->display.init_clock_gating = gen3_init_clock_gating;
+	} else if (IS_VALLEYVIEW(dev)) {
+		dev_priv->display.force_wake_get = vlv_force_wake_get;
+		dev_priv->display.force_wake_put = vlv_force_wake_put;
 	} else if (IS_G4X(dev)) {
 		dev_priv->display.write_eld = g4x_write_eld;
-		dev_priv->display.update_wm = g4x_update_wm;
-		dev_priv->display.init_clock_gating = g4x_init_clock_gating;
-	} else if (IS_GEN4(dev)) {
-		dev_priv->display.update_wm = i965_update_wm;
-		if (IS_CRESTLINE(dev))
-			dev_priv->display.init_clock_gating = crestline_init_clock_gating;
-		else if (IS_BROADWATER(dev))
-			dev_priv->display.init_clock_gating = broadwater_init_clock_gating;
-	} else if (IS_GEN3(dev)) {
-		dev_priv->display.update_wm = i9xx_update_wm;
-		dev_priv->display.get_fifo_size = i9xx_get_fifo_size;
-		dev_priv->display.init_clock_gating = gen3_init_clock_gating;
-	} else if (IS_I865G(dev)) {
-		dev_priv->display.update_wm = i830_update_wm;
-		dev_priv->display.init_clock_gating = i85x_init_clock_gating;
-		dev_priv->display.get_fifo_size = i830_get_fifo_size;
-	} else if (IS_I85X(dev)) {
-		dev_priv->display.update_wm = i9xx_update_wm;
-		dev_priv->display.get_fifo_size = i85x_get_fifo_size;
-		dev_priv->display.init_clock_gating = i85x_init_clock_gating;
-	} else {
-		dev_priv->display.update_wm = i830_update_wm;
-		dev_priv->display.init_clock_gating = i830_init_clock_gating;
-		if (IS_845G(dev))
-			dev_priv->display.get_fifo_size = i845_get_fifo_size;
-		else
-			dev_priv->display.get_fifo_size = i830_get_fifo_size;
 	}
 
 	/* Default just returns -ENODEV to indicate unsupported */
@@ -9090,7 +6793,7 @@ static void quirk_pipea_force(struct drm_device *dev)
 	struct drm_i915_private *dev_priv = dev->dev_private;
 
 	dev_priv->quirks |= QUIRK_PIPEA_FORCE;
-	DRM_DEBUG_DRIVER("applying pipe a force quirk\n");
+	DRM_INFO("applying pipe a force quirk\n");
 }
 
 /*
@@ -9100,6 +6803,18 @@ static void quirk_ssc_force_disable(struct drm_device *dev)
 {
 	struct drm_i915_private *dev_priv = dev->dev_private;
 	dev_priv->quirks |= QUIRK_LVDS_SSC_DISABLE;
+	DRM_INFO("applying lvds SSC disable quirk\n");
+}
+
+/*
+ * A machine (e.g. Acer Aspire 5734Z) may need to invert the panel backlight
+ * brightness value
+ */
+static void quirk_invert_brightness(struct drm_device *dev)
+{
+	struct drm_i915_private *dev_priv = dev->dev_private;
+	dev_priv->quirks |= QUIRK_INVERT_BRIGHTNESS;
+	DRM_INFO("applying inverted panel brightness quirk\n");
 }
 
 struct intel_quirk {
@@ -9109,7 +6824,7 @@ struct intel_quirk {
 	void (*hook)(struct drm_device *dev);
 };
 
-struct intel_quirk intel_quirks[] = {
+static struct intel_quirk intel_quirks[] = {
 	/* HP Mini needs pipe A force quirk (LP: #322104) */
 	{ 0x27ae, 0x103c, 0x361a, quirk_pipea_force },
 
@@ -9134,6 +6849,9 @@ struct intel_quirk intel_quirks[] = {
 
 	/* Sony Vaio Y cannot use SSC on LVDS */
 	{ 0x0046, 0x104d, 0x9076, quirk_ssc_force_disable },
+
+	/* Acer Aspire 5734Z must invert backlight brightness */
+	{ 0x2a42, 0x1025, 0x0459, quirk_invert_brightness },
 };
 
 static void intel_init_quirks(struct drm_device *dev)
@@ -9166,7 +6884,7 @@ static void i915_disable_vga(struct drm_device *dev)
 		vga_reg = VGACNTRL;
 
 	vga_get_uninterruptible(dev->pdev, VGA_RSRC_LEGACY_IO);
-	outb(1, VGA_SR_INDEX);
+	outb(SR01, VGA_SR_INDEX);
 	sr1 = inb(VGA_SR_DATA);
 	outb(sr1 | 1<<5, VGA_SR_DATA);
 	vga_put(dev->pdev, VGA_RSRC_LEGACY_IO);
@@ -9176,6 +6894,40 @@ static void i915_disable_vga(struct drm_device *dev)
 	POSTING_READ(vga_reg);
 }
 
+static void ivb_pch_pwm_override(struct drm_device *dev)
+{
+	struct drm_i915_private *dev_priv = dev->dev_private;
+
+	/*
+	 * IVB has CPU eDP backlight regs too, set things up to let the
+	 * PCH regs control the backlight
+	 */
+	I915_WRITE(BLC_PWM_CPU_CTL2, PWM_ENABLE);
+	I915_WRITE(BLC_PWM_CPU_CTL, 0);
+	I915_WRITE(BLC_PWM_PCH_CTL1, PWM_ENABLE | (1<<30));
+}
+
+void intel_modeset_init_hw(struct drm_device *dev)
+{
+	struct drm_i915_private *dev_priv = dev->dev_private;
+
+	intel_init_clock_gating(dev);
+
+	if (IS_IRONLAKE_M(dev)) {
+		ironlake_enable_drps(dev);
+		ironlake_enable_rc6(dev);
+		intel_init_emon(dev);
+	}
+
+	if ((IS_GEN6(dev) || IS_GEN7(dev)) && !IS_VALLEYVIEW(dev)) {
+		gen6_enable_rps(dev_priv);
+		gen6_update_ring_freq(dev_priv);
+	}
+
+	if (IS_IVYBRIDGE(dev))
+		ivb_pch_pwm_override(dev);
+}
+
 void intel_modeset_init(struct drm_device *dev)
 {
 	struct drm_i915_private *dev_priv = dev->dev_private;
@@ -9189,10 +6941,14 @@ void intel_modeset_init(struct drm_device *dev)
 	dev->mode_config.preferred_depth = 24;
 	dev->mode_config.prefer_shadow = 1;
 
-	dev->mode_config.funcs = (void *)&intel_mode_funcs;
+	dev->mode_config.funcs = &intel_mode_funcs;
 
 	intel_init_quirks(dev);
 
+	intel_init_pm(dev);
+
+	intel_prepare_ddi(dev);
+
 	intel_init_display(dev);
 
 	if (IS_GEN2(dev)) {
@@ -9217,22 +6973,12 @@ void intel_modeset_init(struct drm_device *dev)
 			DRM_DEBUG_KMS("plane %d init failed: %d\n", i, ret);
 	}
 
+	intel_pch_pll_init(dev);
+
 	/* Just disable it once at startup */
 	i915_disable_vga(dev);
 	intel_setup_outputs(dev);
 
-	intel_init_clock_gating(dev);
-
-	if (IS_IRONLAKE_M(dev)) {
-		ironlake_enable_drps(dev);
-		intel_init_emon(dev);
-	}
-
-	if (IS_GEN6(dev) || IS_GEN7(dev)) {
-		gen6_enable_rps(dev_priv);
-		gen6_update_ring_freq(dev_priv);
-	}
-
 	INIT_WORK(&dev_priv->idle_work, intel_idle_update);
 	setup_timer(&dev_priv->idle_timer, intel_gpu_idle_timer,
 		    (unsigned long)dev);
@@ -9240,8 +6986,7 @@ void intel_modeset_init(struct drm_device *dev)
 
 void intel_modeset_gem_init(struct drm_device *dev)
 {
-	if (IS_IRONLAKE_M(dev))
-		ironlake_enable_rc6(dev);
+	intel_modeset_init_hw(dev);
 
 	intel_setup_overlay(dev);
 }
@@ -9271,12 +7016,15 @@ void intel_modeset_cleanup(struct drm_device *dev)
 
 	if (IS_IRONLAKE_M(dev))
 		ironlake_disable_drps(dev);
-	if (IS_GEN6(dev) || IS_GEN7(dev))
+	if ((IS_GEN6(dev) || IS_GEN7(dev)) && !IS_VALLEYVIEW(dev))
 		gen6_disable_rps(dev);
 
 	if (IS_IRONLAKE_M(dev))
 		ironlake_disable_rc6(dev);
 
+	if (IS_VALLEYVIEW(dev))
+		vlv_init_dpio(dev);
+
 	mutex_unlock(&dev->struct_mutex);
 
 	/* Disable the irq before mode object teardown, for the irq might
diff --git a/drivers/gpu/drm/i915/intel_dp.c b/drivers/gpu/drm/i915/intel_dp.c
index 4b637919f74..71c7096e386 100644
--- a/drivers/gpu/drm/i915/intel_dp.c
+++ b/drivers/gpu/drm/i915/intel_dp.c
@@ -688,7 +688,7 @@ intel_dp_mode_fixup(struct drm_encoder *encoder, struct drm_display_mode *mode,
 	int lane_count, clock;
 	int max_lane_count = intel_dp_max_lane_count(intel_dp);
 	int max_clock = intel_dp_max_link_bw(intel_dp) == DP_LINK_BW_2_7 ? 1 : 0;
-	int bpp;
+	int bpp, mode_rate;
 	static int bws[2] = { DP_LINK_BW_1_62, DP_LINK_BW_2_7 };
 
 	if (is_edp(intel_dp) && intel_dp->panel_fixed_mode) {
@@ -702,24 +702,30 @@ intel_dp_mode_fixup(struct drm_encoder *encoder, struct drm_display_mode *mode,
 		mode->clock = intel_dp->panel_fixed_mode->clock;
 	}
 
+	DRM_DEBUG_KMS("DP link computation with max lane count %i "
+		      "max bw %02x pixel clock %iKHz\n",
+		      max_lane_count, bws[max_clock], mode->clock);
+
 	if (!intel_dp_adjust_dithering(intel_dp, mode, adjusted_mode))
 		return false;
 
 	bpp = adjusted_mode->private_flags & INTEL_MODE_DP_FORCE_6BPC ? 18 : 24;
+	mode_rate = intel_dp_link_required(mode->clock, bpp);
 
 	for (lane_count = 1; lane_count <= max_lane_count; lane_count <<= 1) {
 		for (clock = 0; clock <= max_clock; clock++) {
 			int link_avail = intel_dp_max_data_rate(intel_dp_link_clock(bws[clock]), lane_count);
 
-			if (intel_dp_link_required(mode->clock, bpp)
-					<= link_avail) {
+			if (mode_rate <= link_avail) {
 				intel_dp->link_bw = bws[clock];
 				intel_dp->lane_count = lane_count;
 				adjusted_mode->clock = intel_dp_link_clock(intel_dp->link_bw);
-				DRM_DEBUG_KMS("Display port link bw %02x lane "
-						"count %d clock %d\n",
+				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);
+				       adjusted_mode->clock, bpp);
+				DRM_DEBUG_KMS("DP link bw required %i available %i\n",
+					      mode_rate, link_avail);
 				return true;
 			}
 		}
@@ -1149,6 +1155,7 @@ static void ironlake_edp_panel_off(struct intel_dp *intel_dp)
 	DRM_DEBUG_KMS("Turn eDP power off\n");
 
 	WARN(intel_dp->want_panel_vdd, "Cannot turn power off while VDD is on\n");
+	ironlake_panel_vdd_off_sync(intel_dp); /* finish any pending work */
 
 	pp = ironlake_get_pp_control(dev_priv);
 	pp &= ~(POWER_TARGET_ON | EDP_FORCE_VDD | PANEL_POWER_RESET | EDP_BLC_ENABLE);
@@ -1954,6 +1961,23 @@ intel_dp_get_dpcd(struct intel_dp *intel_dp)
 	return false;
 }
 
+static void
+intel_dp_probe_oui(struct intel_dp *intel_dp)
+{
+	u8 buf[3];
+
+	if (!(intel_dp->dpcd[DP_DOWN_STREAM_PORT_COUNT] & DP_OUI_SUPPORT))
+		return;
+
+	if (intel_dp_aux_native_read_retry(intel_dp, DP_SINK_OUI, buf, 3))
+		DRM_DEBUG_KMS("Sink OUI: %02hx%02hx%02hx\n",
+			      buf[0], buf[1], buf[2]);
+
+	if (intel_dp_aux_native_read_retry(intel_dp, DP_BRANCH_OUI, buf, 3))
+		DRM_DEBUG_KMS("Branch OUI: %02hx%02hx%02hx\n",
+			      buf[0], buf[1], buf[2]);
+}
+
 static bool
 intel_dp_get_sink_irq(struct intel_dp *intel_dp, u8 *sink_irq_vector)
 {
@@ -2137,6 +2161,8 @@ intel_dp_detect(struct drm_connector *connector, bool force)
 	if (status != connector_status_connected)
 		return status;
 
+	intel_dp_probe_oui(intel_dp);
+
 	if (intel_dp->force_audio != HDMI_AUDIO_AUTO) {
 		intel_dp->has_audio = (intel_dp->force_audio == HDMI_AUDIO_ON);
 	} else {
@@ -2438,6 +2464,7 @@ intel_dp_init(struct drm_device *dev, int output_reg)
 	}
 
 	intel_encoder->crtc_mask = (1 << 0) | (1 << 1) | (1 << 2);
+
 	connector->interlace_allowed = true;
 	connector->doublescan_allowed = 0;
 
@@ -2483,6 +2510,13 @@ intel_dp_init(struct drm_device *dev, int output_reg)
 		pp_off = I915_READ(PCH_PP_OFF_DELAYS);
 		pp_div = I915_READ(PCH_PP_DIVISOR);
 
+		if (!pp_on || !pp_off || !pp_div) {
+			DRM_INFO("bad panel power sequencing delays, disabling panel\n");
+			intel_dp_encoder_destroy(&intel_dp->base.base);
+			intel_dp_destroy(&intel_connector->base);
+			return;
+		}
+
 		/* Pull timing values out of registers */
 		cur.t1_t3 = (pp_on & PANEL_POWER_UP_DELAY_MASK) >>
 			PANEL_POWER_UP_DELAY_SHIFT;
diff --git a/drivers/gpu/drm/i915/intel_drv.h b/drivers/gpu/drm/i915/intel_drv.h
index 715afa15302..3e0918834e7 100644
--- a/drivers/gpu/drm/i915/intel_drv.h
+++ b/drivers/gpu/drm/i915/intel_drv.h
@@ -45,6 +45,18 @@
 	ret__;								\
 })
 
+#define wait_for_atomic_us(COND, US) ({ \
+	int i, ret__ = -ETIMEDOUT;	\
+	for (i = 0; i < (US); i++) {	\
+		if ((COND)) {		\
+			ret__ = 0;	\
+			break;		\
+		}			\
+		udelay(1);		\
+	}				\
+	ret__;				\
+})
+
 #define wait_for(COND, MS) _wait_for(COND, MS, 1)
 #define wait_for_atomic(COND, MS) _wait_for(COND, MS, 0)
 
@@ -171,8 +183,8 @@ struct intel_crtc {
 	bool cursor_visible;
 	unsigned int bpp;
 
-	bool no_pll; /* tertiary pipe for IVB */
-	bool use_pll_a;
+	/* We can share PLLs across outputs if the timings match */
+	struct intel_pch_pll *pch_pll;
 };
 
 struct intel_plane {
@@ -196,6 +208,25 @@ struct intel_plane {
 			     struct drm_intel_sprite_colorkey *key);
 };
 
+struct intel_watermark_params {
+	unsigned long fifo_size;
+	unsigned long max_wm;
+	unsigned long default_wm;
+	unsigned long guard_size;
+	unsigned long cacheline_size;
+};
+
+struct cxsr_latency {
+	int is_desktop;
+	int is_ddr3;
+	unsigned long fsb_freq;
+	unsigned long mem_freq;
+	unsigned long display_sr;
+	unsigned long display_hpll_disable;
+	unsigned long cursor_sr;
+	unsigned long cursor_hpll_disable;
+};
+
 #define to_intel_crtc(x) container_of(x, struct intel_crtc, base)
 #define to_intel_connector(x) container_of(x, struct intel_connector, base)
 #define to_intel_encoder(x) container_of(x, struct intel_encoder, base)
@@ -207,6 +238,8 @@ struct intel_plane {
 #define DIP_TYPE_AVI    0x82
 #define DIP_VERSION_AVI 0x2
 #define DIP_LEN_AVI     13
+#define DIP_AVI_PR_1    0
+#define DIP_AVI_PR_2    1
 
 #define DIP_TYPE_SPD	0x83
 #define DIP_VERSION_SPD	0x1
@@ -240,23 +273,36 @@ struct dip_infoframe {
 			uint8_t ITC_EC_Q_SC;
 			/* PB4 - VIC 6:0 */
 			uint8_t VIC;
-			/* PB5 - PR 3:0 */
-			uint8_t PR;
+			/* PB5 - YQ 7:6, CN 5:4, PR 3:0 */
+			uint8_t YQ_CN_PR;
 			/* PB6 to PB13 */
 			uint16_t top_bar_end;
 			uint16_t bottom_bar_start;
 			uint16_t left_bar_end;
 			uint16_t right_bar_start;
-		} avi;
+		} __attribute__ ((packed)) avi;
 		struct {
 			uint8_t vn[8];
 			uint8_t pd[16];
 			uint8_t sdi;
-		} spd;
+		} __attribute__ ((packed)) spd;
 		uint8_t payload[27];
 	} __attribute__ ((packed)) body;
 } __attribute__((packed));
 
+struct intel_hdmi {
+	struct intel_encoder base;
+	u32 sdvox_reg;
+	int ddc_bus;
+	int ddi_port;
+	uint32_t color_range;
+	bool has_hdmi_sink;
+	bool has_audio;
+	enum hdmi_force_audio force_audio;
+	void (*write_infoframe)(struct drm_encoder *encoder,
+				struct dip_infoframe *frame);
+};
+
 static inline struct drm_crtc *
 intel_get_crtc_for_pipe(struct drm_device *dev, int pipe)
 {
@@ -296,8 +342,13 @@ 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);
-void intel_dip_infoframe_csum(struct dip_infoframe *avi_if);
-extern bool intel_sdvo_init(struct drm_device *dev, int output_device);
+extern struct intel_hdmi *enc_to_intel_hdmi(struct drm_encoder *encoder);
+extern void intel_hdmi_set_avi_infoframe(struct drm_encoder *encoder,
+			    struct drm_display_mode *adjusted_mode);
+extern void intel_hdmi_set_spd_infoframe(struct drm_encoder *encoder);
+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);
 extern void intel_dvo_init(struct drm_device *dev);
 extern void intel_tv_init(struct drm_device *dev);
 extern void intel_mark_busy(struct drm_device *dev,
@@ -311,6 +362,10 @@ extern bool intel_dpd_is_edp(struct drm_device *dev);
 extern void intel_edp_link_config(struct intel_encoder *, int *, int *);
 extern bool intel_encoder_is_pch_edp(struct drm_encoder *encoder);
 extern int intel_plane_init(struct drm_device *dev, enum pipe pipe);
+extern void intel_flush_display_plane(struct drm_i915_private *dev_priv,
+				      enum plane plane);
+
+void intel_sanitize_pm(struct drm_device *dev);
 
 /* intel_panel.c */
 extern void intel_fixed_panel_mode(struct drm_display_mode *fixed_mode,
@@ -368,12 +423,9 @@ extern void intel_crtc_fb_gamma_set(struct drm_crtc *crtc, u16 red, u16 green,
 extern void intel_crtc_fb_gamma_get(struct drm_crtc *crtc, u16 *red, u16 *green,
 				    u16 *blue, int regno);
 extern void intel_enable_clock_gating(struct drm_device *dev);
+extern void ironlake_disable_rc6(struct drm_device *dev);
 extern void ironlake_enable_drps(struct drm_device *dev);
 extern void ironlake_disable_drps(struct drm_device *dev);
-extern void gen6_enable_rps(struct drm_i915_private *dev_priv);
-extern void gen6_update_ring_freq(struct drm_i915_private *dev_priv);
-extern void gen6_disable_rps(struct drm_device *dev);
-extern void intel_init_emon(struct drm_device *dev);
 
 extern int intel_pin_and_fence_fb_obj(struct drm_device *dev,
 				      struct drm_i915_gem_object *obj,
@@ -411,16 +463,43 @@ 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_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);
 
 /* For use by IVB LP watermark workaround in intel_sprite.c */
-extern void sandybridge_update_wm(struct drm_device *dev);
+extern void intel_update_watermarks(struct drm_device *dev);
 extern void intel_update_sprite_watermarks(struct drm_device *dev, int pipe,
 					   uint32_t sprite_width,
 					   int pixel_size);
+extern void intel_update_linetime_watermarks(struct drm_device *dev, int pipe,
+			 struct drm_display_mode *mode);
 
 extern int intel_sprite_set_colorkey(struct drm_device *dev, void *data,
 				     struct drm_file *file_priv);
 extern int intel_sprite_get_colorkey(struct drm_device *dev, void *data,
 				     struct drm_file *file_priv);
 
+extern u32 intel_dpio_read(struct drm_i915_private *dev_priv, int reg);
+
+/* Power-related functions, located in intel_pm.c */
+extern void intel_init_pm(struct drm_device *dev);
+/* FBC */
+extern bool intel_fbc_enabled(struct drm_device *dev);
+extern void intel_enable_fbc(struct drm_crtc *crtc, unsigned long interval);
+extern void intel_update_fbc(struct drm_device *dev);
+/* IPS */
+extern void intel_gpu_ips_init(struct drm_i915_private *dev_priv);
+extern void intel_gpu_ips_teardown(void);
+
+extern void gen6_enable_rps(struct drm_i915_private *dev_priv);
+extern void gen6_update_ring_freq(struct drm_i915_private *dev_priv);
+extern void gen6_disable_rps(struct drm_device *dev);
+extern void intel_init_emon(struct drm_device *dev);
+
+extern void intel_ddi_dpms(struct drm_encoder *encoder, int mode);
+extern void intel_ddi_mode_set(struct drm_encoder *encoder,
+				struct drm_display_mode *mode,
+				struct drm_display_mode *adjusted_mode);
+
 #endif /* __INTEL_DRV_H__ */
diff --git a/drivers/gpu/drm/i915/intel_dvo.c b/drivers/gpu/drm/i915/intel_dvo.c
index 020a7d7f744..60ba50b956f 100644
--- a/drivers/gpu/drm/i915/intel_dvo.c
+++ b/drivers/gpu/drm/i915/intel_dvo.c
@@ -243,7 +243,7 @@ static int intel_dvo_get_modes(struct drm_connector *connector)
 	 * that's not the case.
 	 */
 	intel_ddc_get_modes(connector,
-			    &dev_priv->gmbus[GMBUS_PORT_DPC].adapter);
+			    intel_gmbus_get_adapter(dev_priv, GMBUS_PORT_DPC));
 	if (!list_empty(&connector->probed_modes))
 		return 1;
 
@@ -375,7 +375,7 @@ void intel_dvo_init(struct drm_device *dev)
 		 * special cases, but otherwise default to what's defined
 		 * in the spec.
 		 */
-		if (dvo->gpio != 0)
+		if (intel_gmbus_is_port_valid(dvo->gpio))
 			gpio = dvo->gpio;
 		else if (dvo->type == INTEL_DVO_CHIP_LVDS)
 			gpio = GMBUS_PORT_SSC;
@@ -386,7 +386,7 @@ void intel_dvo_init(struct drm_device *dev)
 		 * It appears that everything is on GPIOE except for panels
 		 * on i830 laptops, which are on GPIOB (DVOA).
 		 */
-		i2c = &dev_priv->gmbus[gpio].adapter;
+		i2c = intel_gmbus_get_adapter(dev_priv, gpio);
 
 		intel_dvo->dev = *dvo;
 		if (!dvo->dev_ops->init(&intel_dvo->dev, i2c))
diff --git a/drivers/gpu/drm/i915/intel_fb.c b/drivers/gpu/drm/i915/intel_fb.c
index 6e9ee33fd41..bf8690720a0 100644
--- a/drivers/gpu/drm/i915/intel_fb.c
+++ b/drivers/gpu/drm/i915/intel_fb.c
@@ -94,7 +94,7 @@ static int intelfb_create(struct intel_fbdev *ifbdev,
 	mutex_lock(&dev->struct_mutex);
 
 	/* Flush everything out, we'll be doing GTT only from now on */
-	ret = intel_pin_and_fence_fb_obj(dev, obj, false);
+	ret = intel_pin_and_fence_fb_obj(dev, obj, NULL);
 	if (ret) {
 		DRM_ERROR("failed to pin fb: %d\n", ret);
 		goto out_unref;
diff --git a/drivers/gpu/drm/i915/intel_hdmi.c b/drivers/gpu/drm/i915/intel_hdmi.c
index 2d7f47b56b6..2ead3bf7c21 100644
--- a/drivers/gpu/drm/i915/intel_hdmi.c
+++ b/drivers/gpu/drm/i915/intel_hdmi.c
@@ -37,19 +37,7 @@
 #include "i915_drm.h"
 #include "i915_drv.h"
 
-struct intel_hdmi {
-	struct intel_encoder base;
-	u32 sdvox_reg;
-	int ddc_bus;
-	uint32_t color_range;
-	bool has_hdmi_sink;
-	bool has_audio;
-	enum hdmi_force_audio force_audio;
-	void (*write_infoframe)(struct drm_encoder *encoder,
-				struct dip_infoframe *frame);
-};
-
-static struct intel_hdmi *enc_to_intel_hdmi(struct drm_encoder *encoder)
+struct intel_hdmi *enc_to_intel_hdmi(struct drm_encoder *encoder)
 {
 	return container_of(encoder, struct intel_hdmi, base.base);
 }
@@ -75,108 +63,246 @@ void intel_dip_infoframe_csum(struct dip_infoframe *frame)
 	frame->checksum = 0x100 - sum;
 }
 
-static u32 intel_infoframe_index(struct dip_infoframe *frame)
+static u32 g4x_infoframe_index(struct dip_infoframe *frame)
 {
-	u32 flags = 0;
-
 	switch (frame->type) {
 	case DIP_TYPE_AVI:
-		flags |= VIDEO_DIP_SELECT_AVI;
-		break;
+		return VIDEO_DIP_SELECT_AVI;
 	case DIP_TYPE_SPD:
-		flags |= VIDEO_DIP_SELECT_SPD;
-		break;
+		return VIDEO_DIP_SELECT_SPD;
 	default:
 		DRM_DEBUG_DRIVER("unknown info frame type %d\n", frame->type);
-		break;
+		return 0;
 	}
-
-	return flags;
 }
 
-static u32 intel_infoframe_flags(struct dip_infoframe *frame)
+static u32 g4x_infoframe_enable(struct dip_infoframe *frame)
 {
-	u32 flags = 0;
+	switch (frame->type) {
+	case DIP_TYPE_AVI:
+		return VIDEO_DIP_ENABLE_AVI;
+	case DIP_TYPE_SPD:
+		return VIDEO_DIP_ENABLE_SPD;
+	default:
+		DRM_DEBUG_DRIVER("unknown info frame type %d\n", frame->type);
+		return 0;
+	}
+}
 
+static u32 hsw_infoframe_enable(struct dip_infoframe *frame)
+{
 	switch (frame->type) {
 	case DIP_TYPE_AVI:
-		flags |= VIDEO_DIP_ENABLE_AVI | VIDEO_DIP_FREQ_VSYNC;
-		break;
+		return VIDEO_DIP_ENABLE_AVI_HSW;
 	case DIP_TYPE_SPD:
-		flags |= VIDEO_DIP_ENABLE_SPD | VIDEO_DIP_FREQ_VSYNC;
-		break;
+		return VIDEO_DIP_ENABLE_SPD_HSW;
 	default:
 		DRM_DEBUG_DRIVER("unknown info frame type %d\n", frame->type);
-		break;
+		return 0;
 	}
+}
 
-	return flags;
+static u32 hsw_infoframe_data_reg(struct dip_infoframe *frame, enum pipe pipe)
+{
+	switch (frame->type) {
+	case DIP_TYPE_AVI:
+		return HSW_TVIDEO_DIP_AVI_DATA(pipe);
+	case DIP_TYPE_SPD:
+		return HSW_TVIDEO_DIP_SPD_DATA(pipe);
+	default:
+		DRM_DEBUG_DRIVER("unknown info frame type %d\n", frame->type);
+		return 0;
+	}
 }
 
-static void i9xx_write_infoframe(struct drm_encoder *encoder,
-				 struct dip_infoframe *frame)
+static void g4x_write_infoframe(struct drm_encoder *encoder,
+				struct dip_infoframe *frame)
 {
 	uint32_t *data = (uint32_t *)frame;
 	struct drm_device *dev = encoder->dev;
 	struct drm_i915_private *dev_priv = dev->dev_private;
 	struct intel_hdmi *intel_hdmi = enc_to_intel_hdmi(encoder);
-	u32 port, flags, val = I915_READ(VIDEO_DIP_CTL);
+	u32 val = I915_READ(VIDEO_DIP_CTL);
 	unsigned i, len = DIP_HEADER_SIZE + frame->len;
 
-
-	/* XXX first guess at handling video port, is this corrent? */
+	val &= ~VIDEO_DIP_PORT_MASK;
 	if (intel_hdmi->sdvox_reg == SDVOB)
-		port = VIDEO_DIP_PORT_B;
+		val |= VIDEO_DIP_PORT_B;
 	else if (intel_hdmi->sdvox_reg == SDVOC)
-		port = VIDEO_DIP_PORT_C;
+		val |= VIDEO_DIP_PORT_C;
 	else
 		return;
 
-	flags = intel_infoframe_index(frame);
+	val &= ~(VIDEO_DIP_SELECT_MASK | 0xf); /* clear DIP data offset */
+	val |= g4x_infoframe_index(frame);
 
-	val &= ~VIDEO_DIP_SELECT_MASK;
+	val &= ~g4x_infoframe_enable(frame);
+	val |= VIDEO_DIP_ENABLE;
 
-	I915_WRITE(VIDEO_DIP_CTL, VIDEO_DIP_ENABLE | val | port | flags);
+	I915_WRITE(VIDEO_DIP_CTL, val);
 
 	for (i = 0; i < len; i += 4) {
 		I915_WRITE(VIDEO_DIP_DATA, *data);
 		data++;
 	}
 
-	flags |= intel_infoframe_flags(frame);
+	val |= g4x_infoframe_enable(frame);
+	val &= ~VIDEO_DIP_FREQ_MASK;
+	val |= VIDEO_DIP_FREQ_VSYNC;
 
-	I915_WRITE(VIDEO_DIP_CTL, VIDEO_DIP_ENABLE | val | port | flags);
+	I915_WRITE(VIDEO_DIP_CTL, val);
 }
 
-static void ironlake_write_infoframe(struct drm_encoder *encoder,
-				     struct dip_infoframe *frame)
+static void ibx_write_infoframe(struct drm_encoder *encoder,
+				struct dip_infoframe *frame)
 {
 	uint32_t *data = (uint32_t *)frame;
 	struct drm_device *dev = encoder->dev;
 	struct drm_i915_private *dev_priv = dev->dev_private;
-	struct drm_crtc *crtc = encoder->crtc;
-	struct intel_crtc *intel_crtc = to_intel_crtc(crtc);
+	struct intel_crtc *intel_crtc = to_intel_crtc(encoder->crtc);
+	struct intel_hdmi *intel_hdmi = enc_to_intel_hdmi(encoder);
 	int reg = TVIDEO_DIP_CTL(intel_crtc->pipe);
 	unsigned i, len = DIP_HEADER_SIZE + frame->len;
-	u32 flags, val = I915_READ(reg);
+	u32 val = I915_READ(reg);
+
+	val &= ~VIDEO_DIP_PORT_MASK;
+	switch (intel_hdmi->sdvox_reg) {
+	case HDMIB:
+		val |= VIDEO_DIP_PORT_B;
+		break;
+	case HDMIC:
+		val |= VIDEO_DIP_PORT_C;
+		break;
+	case HDMID:
+		val |= VIDEO_DIP_PORT_D;
+		break;
+	default:
+		return;
+	}
 
 	intel_wait_for_vblank(dev, intel_crtc->pipe);
 
-	flags = intel_infoframe_index(frame);
+	val &= ~(VIDEO_DIP_SELECT_MASK | 0xf); /* clear DIP data offset */
+	val |= g4x_infoframe_index(frame);
+
+	val &= ~g4x_infoframe_enable(frame);
+	val |= VIDEO_DIP_ENABLE;
+
+	I915_WRITE(reg, val);
+
+	for (i = 0; i < len; i += 4) {
+		I915_WRITE(TVIDEO_DIP_DATA(intel_crtc->pipe), *data);
+		data++;
+	}
+
+	val |= g4x_infoframe_enable(frame);
+	val &= ~VIDEO_DIP_FREQ_MASK;
+	val |= VIDEO_DIP_FREQ_VSYNC;
+
+	I915_WRITE(reg, val);
+}
+
+static void cpt_write_infoframe(struct drm_encoder *encoder,
+				struct dip_infoframe *frame)
+{
+	uint32_t *data = (uint32_t *)frame;
+	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);
+	int reg = TVIDEO_DIP_CTL(intel_crtc->pipe);
+	unsigned i, len = DIP_HEADER_SIZE + frame->len;
+	u32 val = I915_READ(reg);
+
+	intel_wait_for_vblank(dev, intel_crtc->pipe);
 
 	val &= ~(VIDEO_DIP_SELECT_MASK | 0xf); /* clear DIP data offset */
+	val |= g4x_infoframe_index(frame);
+
+	/* The DIP control register spec says that we need to update the AVI
+	 * infoframe without clearing its enable bit */
+	if (frame->type == DIP_TYPE_AVI)
+		val |= VIDEO_DIP_ENABLE_AVI;
+	else
+		val &= ~g4x_infoframe_enable(frame);
+
+	val |= VIDEO_DIP_ENABLE;
 
-	I915_WRITE(reg, VIDEO_DIP_ENABLE | val | flags);
+	I915_WRITE(reg, val);
 
 	for (i = 0; i < len; i += 4) {
 		I915_WRITE(TVIDEO_DIP_DATA(intel_crtc->pipe), *data);
 		data++;
 	}
 
-	flags |= intel_infoframe_flags(frame);
+	val |= g4x_infoframe_enable(frame);
+	val &= ~VIDEO_DIP_FREQ_MASK;
+	val |= VIDEO_DIP_FREQ_VSYNC;
 
-	I915_WRITE(reg, VIDEO_DIP_ENABLE | val | flags);
+	I915_WRITE(reg, val);
 }
+
+static void vlv_write_infoframe(struct drm_encoder *encoder,
+				     struct dip_infoframe *frame)
+{
+	uint32_t *data = (uint32_t *)frame;
+	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);
+	int reg = VLV_TVIDEO_DIP_CTL(intel_crtc->pipe);
+	unsigned i, len = DIP_HEADER_SIZE + frame->len;
+	u32 val = I915_READ(reg);
+
+	intel_wait_for_vblank(dev, intel_crtc->pipe);
+
+	val &= ~(VIDEO_DIP_SELECT_MASK | 0xf); /* clear DIP data offset */
+	val |= g4x_infoframe_index(frame);
+
+	val &= ~g4x_infoframe_enable(frame);
+	val |= VIDEO_DIP_ENABLE;
+
+	I915_WRITE(reg, val);
+
+	for (i = 0; i < len; i += 4) {
+		I915_WRITE(VLV_TVIDEO_DIP_DATA(intel_crtc->pipe), *data);
+		data++;
+	}
+
+	val |= g4x_infoframe_enable(frame);
+	val &= ~VIDEO_DIP_FREQ_MASK;
+	val |= VIDEO_DIP_FREQ_VSYNC;
+
+	I915_WRITE(reg, val);
+}
+
+static void hsw_write_infoframe(struct drm_encoder *encoder,
+				struct dip_infoframe *frame)
+{
+	uint32_t *data = (uint32_t *)frame;
+	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);
+	unsigned int i, len = DIP_HEADER_SIZE + frame->len;
+	u32 val = I915_READ(ctl_reg);
+
+	if (data_reg == 0)
+		return;
+
+	intel_wait_for_vblank(dev, intel_crtc->pipe);
+
+	val &= ~hsw_infoframe_enable(frame);
+	I915_WRITE(ctl_reg, val);
+
+	for (i = 0; i < len; i += 4) {
+		I915_WRITE(data_reg + i, *data);
+		data++;
+	}
+
+	val |= hsw_infoframe_enable(frame);
+	I915_WRITE(ctl_reg, val);
+}
+
 static void intel_set_infoframe(struct drm_encoder *encoder,
 				struct dip_infoframe *frame)
 {
@@ -189,7 +315,8 @@ static void intel_set_infoframe(struct drm_encoder *encoder,
 	intel_hdmi->write_infoframe(encoder, frame);
 }
 
-static void intel_hdmi_set_avi_infoframe(struct drm_encoder *encoder)
+void intel_hdmi_set_avi_infoframe(struct drm_encoder *encoder,
+					 struct drm_display_mode *adjusted_mode)
 {
 	struct dip_infoframe avi_if = {
 		.type = DIP_TYPE_AVI,
@@ -197,10 +324,13 @@ static void intel_hdmi_set_avi_infoframe(struct drm_encoder *encoder)
 		.len = DIP_LEN_AVI,
 	};
 
+	if (adjusted_mode->flags & DRM_MODE_FLAG_DBLCLK)
+		avi_if.body.avi.YQ_CN_PR |= DIP_AVI_PR_2;
+
 	intel_set_infoframe(encoder, &avi_if);
 }
 
-static void intel_hdmi_set_spd_infoframe(struct drm_encoder *encoder)
+void intel_hdmi_set_spd_infoframe(struct drm_encoder *encoder)
 {
 	struct dip_infoframe spd_if;
 
@@ -221,8 +351,7 @@ static void intel_hdmi_mode_set(struct drm_encoder *encoder,
 {
 	struct drm_device *dev = encoder->dev;
 	struct drm_i915_private *dev_priv = dev->dev_private;
-	struct drm_crtc *crtc = encoder->crtc;
-	struct intel_crtc *intel_crtc = to_intel_crtc(crtc);
+	struct intel_crtc *intel_crtc = to_intel_crtc(encoder->crtc);
 	struct intel_hdmi *intel_hdmi = enc_to_intel_hdmi(encoder);
 	u32 sdvox;
 
@@ -259,7 +388,7 @@ static void intel_hdmi_mode_set(struct drm_encoder *encoder,
 	I915_WRITE(intel_hdmi->sdvox_reg, sdvox);
 	POSTING_READ(intel_hdmi->sdvox_reg);
 
-	intel_hdmi_set_avi_infoframe(encoder);
+	intel_hdmi_set_avi_infoframe(encoder, adjusted_mode);
 	intel_hdmi_set_spd_infoframe(encoder);
 }
 
@@ -334,7 +463,8 @@ intel_hdmi_detect(struct drm_connector *connector, bool force)
 	intel_hdmi->has_hdmi_sink = false;
 	intel_hdmi->has_audio = false;
 	edid = drm_get_edid(connector,
-			    &dev_priv->gmbus[intel_hdmi->ddc_bus].adapter);
+			    intel_gmbus_get_adapter(dev_priv,
+						    intel_hdmi->ddc_bus));
 
 	if (edid) {
 		if (edid->input & DRM_EDID_INPUT_DIGITAL) {
@@ -367,7 +497,8 @@ static int intel_hdmi_get_modes(struct drm_connector *connector)
 	 */
 
 	return intel_ddc_get_modes(connector,
-				   &dev_priv->gmbus[intel_hdmi->ddc_bus].adapter);
+				   intel_gmbus_get_adapter(dev_priv,
+							   intel_hdmi->ddc_bus));
 }
 
 static bool
@@ -379,7 +510,8 @@ intel_hdmi_detect_audio(struct drm_connector *connector)
 	bool has_audio = false;
 
 	edid = drm_get_edid(connector,
-			    &dev_priv->gmbus[intel_hdmi->ddc_bus].adapter);
+			    intel_gmbus_get_adapter(dev_priv,
+						    intel_hdmi->ddc_bus));
 	if (edid) {
 		if (edid->input & DRM_EDID_INPUT_DIGITAL)
 			has_audio = drm_detect_monitor_audio(edid);
@@ -393,8 +525,8 @@ intel_hdmi_detect_audio(struct drm_connector *connector)
 
 static int
 intel_hdmi_set_property(struct drm_connector *connector,
-		      struct drm_property *property,
-		      uint64_t val)
+			struct drm_property *property,
+			uint64_t val)
 {
 	struct intel_hdmi *intel_hdmi = intel_attached_hdmi(connector);
 	struct drm_i915_private *dev_priv = connector->dev->dev_private;
@@ -453,6 +585,14 @@ static void intel_hdmi_destroy(struct drm_connector *connector)
 	kfree(connector);
 }
 
+static const struct drm_encoder_helper_funcs intel_hdmi_helper_funcs_hsw = {
+	.dpms = intel_ddi_dpms,
+	.mode_fixup = intel_hdmi_mode_fixup,
+	.prepare = intel_encoder_prepare,
+	.mode_set = intel_ddi_mode_set,
+	.commit = intel_encoder_commit,
+};
+
 static const struct drm_encoder_helper_funcs intel_hdmi_helper_funcs = {
 	.dpms = intel_hdmi_dpms,
 	.mode_fixup = intel_hdmi_mode_fixup,
@@ -542,20 +682,60 @@ void intel_hdmi_init(struct drm_device *dev, int sdvox_reg)
 		intel_encoder->clone_mask = (1 << INTEL_HDMIF_CLONE_BIT);
 		intel_hdmi->ddc_bus = GMBUS_PORT_DPD;
 		dev_priv->hotplug_supported_mask |= HDMID_HOTPLUG_INT_STATUS;
+	} else if (sdvox_reg == DDI_BUF_CTL(PORT_B)) {
+		DRM_DEBUG_DRIVER("LPT: detected output on DDI B\n");
+		intel_encoder->clone_mask = (1 << INTEL_HDMIB_CLONE_BIT);
+		intel_hdmi->ddc_bus = GMBUS_PORT_DPB;
+		intel_hdmi->ddi_port = PORT_B;
+		dev_priv->hotplug_supported_mask |= HDMIB_HOTPLUG_INT_STATUS;
+	} else if (sdvox_reg == DDI_BUF_CTL(PORT_C)) {
+		DRM_DEBUG_DRIVER("LPT: detected output on DDI C\n");
+		intel_encoder->clone_mask = (1 << INTEL_HDMIC_CLONE_BIT);
+		intel_hdmi->ddc_bus = GMBUS_PORT_DPC;
+		intel_hdmi->ddi_port = PORT_C;
+		dev_priv->hotplug_supported_mask |= HDMIC_HOTPLUG_INT_STATUS;
+	} else if (sdvox_reg == DDI_BUF_CTL(PORT_D)) {
+		DRM_DEBUG_DRIVER("LPT: detected output on DDI D\n");
+		intel_encoder->clone_mask = (1 << INTEL_HDMID_CLONE_BIT);
+		intel_hdmi->ddc_bus = GMBUS_PORT_DPD;
+		intel_hdmi->ddi_port = PORT_D;
+		dev_priv->hotplug_supported_mask |= HDMID_HOTPLUG_INT_STATUS;
+	} else {
+		/* If we got an unknown sdvox_reg, things are pretty much broken
+		 * in a way that we should let the kernel know about it */
+		BUG();
 	}
 
 	intel_hdmi->sdvox_reg = sdvox_reg;
 
 	if (!HAS_PCH_SPLIT(dev)) {
-		intel_hdmi->write_infoframe = i9xx_write_infoframe;
+		intel_hdmi->write_infoframe = g4x_write_infoframe;
 		I915_WRITE(VIDEO_DIP_CTL, 0);
+	} else if (IS_VALLEYVIEW(dev)) {
+		intel_hdmi->write_infoframe = vlv_write_infoframe;
+		for_each_pipe(i)
+			I915_WRITE(VLV_TVIDEO_DIP_CTL(i), 0);
+	} else if (IS_HASWELL(dev)) {
+		/* FIXME: Haswell has a new set of DIP frame registers, but we are
+		 * just doing the minimal required for HDMI to work at this stage.
+		 */
+		intel_hdmi->write_infoframe = hsw_write_infoframe;
+		for_each_pipe(i)
+			I915_WRITE(HSW_TVIDEO_DIP_CTL(i), 0);
+	} else if (HAS_PCH_IBX(dev)) {
+		intel_hdmi->write_infoframe = ibx_write_infoframe;
+		for_each_pipe(i)
+			I915_WRITE(TVIDEO_DIP_CTL(i), 0);
 	} else {
-		intel_hdmi->write_infoframe = ironlake_write_infoframe;
+		intel_hdmi->write_infoframe = cpt_write_infoframe;
 		for_each_pipe(i)
 			I915_WRITE(TVIDEO_DIP_CTL(i), 0);
 	}
 
-	drm_encoder_helper_add(&intel_encoder->base, &intel_hdmi_helper_funcs);
+	if (IS_HASWELL(dev))
+		drm_encoder_helper_add(&intel_encoder->base, &intel_hdmi_helper_funcs_hsw);
+	else
+		drm_encoder_helper_add(&intel_encoder->base, &intel_hdmi_helper_funcs);
 
 	intel_hdmi_add_properties(intel_hdmi, connector);
 
diff --git a/drivers/gpu/drm/i915/intel_i2c.c b/drivers/gpu/drm/i915/intel_i2c.c
index 8fdc9570021..4a9707dd0f9 100644
--- a/drivers/gpu/drm/i915/intel_i2c.c
+++ b/drivers/gpu/drm/i915/intel_i2c.c
@@ -35,6 +35,20 @@
 #include "i915_drm.h"
 #include "i915_drv.h"
 
+struct gmbus_port {
+	const char *name;
+	int reg;
+};
+
+static const struct gmbus_port gmbus_ports[] = {
+	{ "ssc", GPIOB },
+	{ "vga", GPIOA },
+	{ "panel", GPIOC },
+	{ "dpc", GPIOD },
+	{ "dpb", GPIOE },
+	{ "dpd", GPIOF },
+};
+
 /* Intel GPIO access functions */
 
 #define I2C_RISEFALL_TIME 10
@@ -49,10 +63,7 @@ void
 intel_i2c_reset(struct drm_device *dev)
 {
 	struct drm_i915_private *dev_priv = dev->dev_private;
-	if (HAS_PCH_SPLIT(dev))
-		I915_WRITE(PCH_GMBUS0, 0);
-	else
-		I915_WRITE(GMBUS0, 0);
+	I915_WRITE(dev_priv->gpio_mmio_base + GMBUS0, 0);
 }
 
 static void intel_i2c_quirk_set(struct drm_i915_private *dev_priv, bool enable)
@@ -140,63 +151,173 @@ static void set_data(void *data, int state_high)
 	POSTING_READ(bus->gpio_reg);
 }
 
-static bool
+static int
+intel_gpio_pre_xfer(struct i2c_adapter *adapter)
+{
+	struct intel_gmbus *bus = container_of(adapter,
+					       struct intel_gmbus,
+					       adapter);
+	struct drm_i915_private *dev_priv = bus->dev_priv;
+
+	intel_i2c_reset(dev_priv->dev);
+	intel_i2c_quirk_set(dev_priv, true);
+	set_data(bus, 1);
+	set_clock(bus, 1);
+	udelay(I2C_RISEFALL_TIME);
+	return 0;
+}
+
+static void
+intel_gpio_post_xfer(struct i2c_adapter *adapter)
+{
+	struct intel_gmbus *bus = container_of(adapter,
+					       struct intel_gmbus,
+					       adapter);
+	struct drm_i915_private *dev_priv = bus->dev_priv;
+
+	set_data(bus, 1);
+	set_clock(bus, 1);
+	intel_i2c_quirk_set(dev_priv, false);
+}
+
+static void
 intel_gpio_setup(struct intel_gmbus *bus, u32 pin)
 {
 	struct drm_i915_private *dev_priv = bus->dev_priv;
-	static const int map_pin_to_reg[] = {
-		0,
-		GPIOB,
-		GPIOA,
-		GPIOC,
-		GPIOD,
-		GPIOE,
-		0,
-		GPIOF,
-	};
 	struct i2c_algo_bit_data *algo;
 
-	if (pin >= ARRAY_SIZE(map_pin_to_reg) || !map_pin_to_reg[pin])
-		return false;
-
 	algo = &bus->bit_algo;
 
-	bus->gpio_reg = map_pin_to_reg[pin];
-	if (HAS_PCH_SPLIT(dev_priv->dev))
-		bus->gpio_reg += PCH_GPIOA - GPIOA;
+	/* -1 to map pin pair to gmbus index */
+	bus->gpio_reg = dev_priv->gpio_mmio_base + gmbus_ports[pin - 1].reg;
 
 	bus->adapter.algo_data = algo;
 	algo->setsda = set_data;
 	algo->setscl = set_clock;
 	algo->getsda = get_data;
 	algo->getscl = get_clock;
+	algo->pre_xfer = intel_gpio_pre_xfer;
+	algo->post_xfer = intel_gpio_post_xfer;
 	algo->udelay = I2C_RISEFALL_TIME;
 	algo->timeout = usecs_to_jiffies(2200);
 	algo->data = bus;
+}
+
+static int
+gmbus_xfer_read(struct drm_i915_private *dev_priv, struct i2c_msg *msg,
+		u32 gmbus1_index)
+{
+	int reg_offset = dev_priv->gpio_mmio_base;
+	u16 len = msg->len;
+	u8 *buf = msg->buf;
+
+	I915_WRITE(GMBUS1 + reg_offset,
+		   gmbus1_index |
+		   GMBUS_CYCLE_WAIT |
+		   (len << GMBUS_BYTE_COUNT_SHIFT) |
+		   (msg->addr << GMBUS_SLAVE_ADDR_SHIFT) |
+		   GMBUS_SLAVE_READ | GMBUS_SW_RDY);
+	while (len) {
+		int ret;
+		u32 val, loop = 0;
+		u32 gmbus2;
+
+		ret = wait_for((gmbus2 = I915_READ(GMBUS2 + reg_offset)) &
+			       (GMBUS_SATOER | GMBUS_HW_RDY),
+			       50);
+		if (ret)
+			return -ETIMEDOUT;
+		if (gmbus2 & GMBUS_SATOER)
+			return -ENXIO;
+
+		val = I915_READ(GMBUS3 + reg_offset);
+		do {
+			*buf++ = val & 0xff;
+			val >>= 8;
+		} while (--len && ++loop < 4);
+	}
 
-	return true;
+	return 0;
 }
 
 static int
-intel_i2c_quirk_xfer(struct intel_gmbus *bus,
-		     struct i2c_msg *msgs,
-		     int num)
+gmbus_xfer_write(struct drm_i915_private *dev_priv, struct i2c_msg *msg)
 {
-	struct drm_i915_private *dev_priv = bus->dev_priv;
+	int reg_offset = dev_priv->gpio_mmio_base;
+	u16 len = msg->len;
+	u8 *buf = msg->buf;
+	u32 val, loop;
+
+	val = loop = 0;
+	while (len && loop < 4) {
+		val |= *buf++ << (8 * loop++);
+		len -= 1;
+	}
+
+	I915_WRITE(GMBUS3 + reg_offset, val);
+	I915_WRITE(GMBUS1 + reg_offset,
+		   GMBUS_CYCLE_WAIT |
+		   (msg->len << GMBUS_BYTE_COUNT_SHIFT) |
+		   (msg->addr << GMBUS_SLAVE_ADDR_SHIFT) |
+		   GMBUS_SLAVE_WRITE | GMBUS_SW_RDY);
+	while (len) {
+		int ret;
+		u32 gmbus2;
+
+		val = loop = 0;
+		do {
+			val |= *buf++ << (8 * loop);
+		} while (--len && ++loop < 4);
+
+		I915_WRITE(GMBUS3 + reg_offset, val);
+
+		ret = wait_for((gmbus2 = I915_READ(GMBUS2 + reg_offset)) &
+			       (GMBUS_SATOER | GMBUS_HW_RDY),
+			       50);
+		if (ret)
+			return -ETIMEDOUT;
+		if (gmbus2 & GMBUS_SATOER)
+			return -ENXIO;
+	}
+	return 0;
+}
+
+/*
+ * The gmbus controller can combine a 1 or 2 byte write with a read that
+ * immediately follows it by using an "INDEX" cycle.
+ */
+static bool
+gmbus_is_index_read(struct i2c_msg *msgs, int i, int num)
+{
+	return (i + 1 < num &&
+		!(msgs[i].flags & I2C_M_RD) && msgs[i].len <= 2 &&
+		(msgs[i + 1].flags & I2C_M_RD));
+}
+
+static int
+gmbus_xfer_index_read(struct drm_i915_private *dev_priv, struct i2c_msg *msgs)
+{
+	int reg_offset = dev_priv->gpio_mmio_base;
+	u32 gmbus1_index = 0;
+	u32 gmbus5 = 0;
 	int ret;
 
-	intel_i2c_reset(dev_priv->dev);
+	if (msgs[0].len == 2)
+		gmbus5 = GMBUS_2BYTE_INDEX_EN |
+			 msgs[0].buf[1] | (msgs[0].buf[0] << 8);
+	if (msgs[0].len == 1)
+		gmbus1_index = GMBUS_CYCLE_INDEX |
+			       (msgs[0].buf[0] << GMBUS_SLAVE_INDEX_SHIFT);
 
-	intel_i2c_quirk_set(dev_priv, true);
-	set_data(bus, 1);
-	set_clock(bus, 1);
-	udelay(I2C_RISEFALL_TIME);
+	/* GMBUS5 holds 16-bit index */
+	if (gmbus5)
+		I915_WRITE(GMBUS5 + reg_offset, gmbus5);
 
-	ret = i2c_bit_algo.master_xfer(&bus->adapter, msgs, num);
+	ret = gmbus_xfer_read(dev_priv, &msgs[1], gmbus1_index);
 
-	set_data(bus, 1);
-	set_clock(bus, 1);
-	intel_i2c_quirk_set(dev_priv, false);
+	/* Clear GMBUS5 after each index transfer */
+	if (gmbus5)
+		I915_WRITE(GMBUS5 + reg_offset, 0);
 
 	return ret;
 }
@@ -210,117 +331,108 @@ gmbus_xfer(struct i2c_adapter *adapter,
 					       struct intel_gmbus,
 					       adapter);
 	struct drm_i915_private *dev_priv = bus->dev_priv;
-	int i, reg_offset, ret;
+	int i, reg_offset;
+	int ret = 0;
 
 	mutex_lock(&dev_priv->gmbus_mutex);
 
 	if (bus->force_bit) {
-		ret = intel_i2c_quirk_xfer(bus, msgs, num);
+		ret = i2c_bit_algo.master_xfer(adapter, msgs, num);
 		goto out;
 	}
 
-	reg_offset = HAS_PCH_SPLIT(dev_priv->dev) ? PCH_GMBUS0 - GMBUS0 : 0;
+	reg_offset = dev_priv->gpio_mmio_base;
 
 	I915_WRITE(GMBUS0 + reg_offset, bus->reg0);
 
 	for (i = 0; i < num; i++) {
-		u16 len = msgs[i].len;
-		u8 *buf = msgs[i].buf;
-
-		if (msgs[i].flags & I2C_M_RD) {
-			I915_WRITE(GMBUS1 + reg_offset,
-				   GMBUS_CYCLE_WAIT |
-				   (i + 1 == num ? GMBUS_CYCLE_STOP : 0) |
-				   (len << GMBUS_BYTE_COUNT_SHIFT) |
-				   (msgs[i].addr << GMBUS_SLAVE_ADDR_SHIFT) |
-				   GMBUS_SLAVE_READ | GMBUS_SW_RDY);
-			POSTING_READ(GMBUS2+reg_offset);
-			do {
-				u32 val, loop = 0;
-
-				if (wait_for(I915_READ(GMBUS2 + reg_offset) & (GMBUS_SATOER | GMBUS_HW_RDY), 50))
-					goto timeout;
-				if (I915_READ(GMBUS2 + reg_offset) & GMBUS_SATOER)
-					goto clear_err;
-
-				val = I915_READ(GMBUS3 + reg_offset);
-				do {
-					*buf++ = val & 0xff;
-					val >>= 8;
-				} while (--len && ++loop < 4);
-			} while (len);
+		u32 gmbus2;
+
+		if (gmbus_is_index_read(msgs, i, num)) {
+			ret = gmbus_xfer_index_read(dev_priv, &msgs[i]);
+			i += 1;  /* set i to the index of the read xfer */
+		} else if (msgs[i].flags & I2C_M_RD) {
+			ret = gmbus_xfer_read(dev_priv, &msgs[i], 0);
 		} else {
-			u32 val, loop;
-
-			val = loop = 0;
-			do {
-				val |= *buf++ << (8 * loop);
-			} while (--len && ++loop < 4);
-
-			I915_WRITE(GMBUS3 + reg_offset, val);
-			I915_WRITE(GMBUS1 + reg_offset,
-				   GMBUS_CYCLE_WAIT |
-				   (i + 1 == num ? GMBUS_CYCLE_STOP : 0) |
-				   (msgs[i].len << GMBUS_BYTE_COUNT_SHIFT) |
-				   (msgs[i].addr << GMBUS_SLAVE_ADDR_SHIFT) |
-				   GMBUS_SLAVE_WRITE | GMBUS_SW_RDY);
-			POSTING_READ(GMBUS2+reg_offset);
-
-			while (len) {
-				if (wait_for(I915_READ(GMBUS2 + reg_offset) & (GMBUS_SATOER | GMBUS_HW_RDY), 50))
-					goto timeout;
-				if (I915_READ(GMBUS2 + reg_offset) & GMBUS_SATOER)
-					goto clear_err;
-
-				val = loop = 0;
-				do {
-					val |= *buf++ << (8 * loop);
-				} while (--len && ++loop < 4);
-
-				I915_WRITE(GMBUS3 + reg_offset, val);
-				POSTING_READ(GMBUS2+reg_offset);
-			}
+			ret = gmbus_xfer_write(dev_priv, &msgs[i]);
 		}
 
-		if (i + 1 < num && wait_for(I915_READ(GMBUS2 + reg_offset) & (GMBUS_SATOER | GMBUS_HW_WAIT_PHASE), 50))
+		if (ret == -ETIMEDOUT)
+			goto timeout;
+		if (ret == -ENXIO)
+			goto clear_err;
+
+		ret = wait_for((gmbus2 = I915_READ(GMBUS2 + reg_offset)) &
+			       (GMBUS_SATOER | GMBUS_HW_WAIT_PHASE),
+			       50);
+		if (ret)
 			goto timeout;
-		if (I915_READ(GMBUS2 + reg_offset) & GMBUS_SATOER)
+		if (gmbus2 & GMBUS_SATOER)
 			goto clear_err;
 	}
 
-	goto done;
+	/* Generate a STOP condition on the bus. Note that gmbus can't generata
+	 * a STOP on the very first cycle. To simplify the code we
+	 * unconditionally generate the STOP condition with an additional gmbus
+	 * cycle. */
+	I915_WRITE(GMBUS1 + reg_offset, GMBUS_CYCLE_STOP | GMBUS_SW_RDY);
+
+	/* Mark the GMBUS interface as disabled after waiting for idle.
+	 * We will re-enable it at the start of the next xfer,
+	 * till then let it sleep.
+	 */
+	if (wait_for((I915_READ(GMBUS2 + reg_offset) & GMBUS_ACTIVE) == 0,
+		     10)) {
+		DRM_DEBUG_KMS("GMBUS [%s] timed out waiting for idle\n",
+			 adapter->name);
+		ret = -ETIMEDOUT;
+	}
+	I915_WRITE(GMBUS0 + reg_offset, 0);
+	ret = ret ?: i;
+	goto out;
 
 clear_err:
+	/*
+	 * Wait for bus to IDLE before clearing NAK.
+	 * If we clear the NAK while bus is still active, then it will stay
+	 * active and the next transaction may fail.
+	 */
+	if (wait_for((I915_READ(GMBUS2 + reg_offset) & GMBUS_ACTIVE) == 0,
+		     10))
+		DRM_DEBUG_KMS("GMBUS [%s] timed out after NAK\n",
+			      adapter->name);
+
 	/* Toggle the Software Clear Interrupt bit. This has the effect
 	 * of resetting the GMBUS controller and so clearing the
 	 * BUS_ERROR raised by the slave's NAK.
 	 */
 	I915_WRITE(GMBUS1 + reg_offset, GMBUS_SW_CLR_INT);
 	I915_WRITE(GMBUS1 + reg_offset, 0);
+	I915_WRITE(GMBUS0 + reg_offset, 0);
 
-done:
-	/* Mark the GMBUS interface as disabled after waiting for idle.
-	 * We will re-enable it at the start of the next xfer,
-	 * till then let it sleep.
+	DRM_DEBUG_KMS("GMBUS [%s] NAK for addr: %04x %c(%d)\n",
+			 adapter->name, msgs[i].addr,
+			 (msgs[i].flags & I2C_M_RD) ? 'r' : 'w', msgs[i].len);
+
+	/*
+	 * If no ACK is received during the address phase of a transaction,
+	 * the adapter must report -ENXIO.
+	 * It is not clear what to return if no ACK is received at other times.
+	 * So, we always return -ENXIO in all NAK cases, to ensure we send
+	 * it at least during the one case that is specified.
 	 */
-	if (wait_for((I915_READ(GMBUS2 + reg_offset) & GMBUS_ACTIVE) == 0, 10))
-		DRM_INFO("GMBUS timed out waiting for idle\n");
-	I915_WRITE(GMBUS0 + reg_offset, 0);
-	ret = i;
+	ret = -ENXIO;
 	goto out;
 
 timeout:
-	DRM_INFO("GMBUS timed out, falling back to bit banging on pin %d [%s]\n",
-		 bus->reg0 & 0xff, bus->adapter.name);
+	DRM_INFO("GMBUS [%s] timed out, falling back to bit banging on pin %d\n",
+		 bus->adapter.name, bus->reg0 & 0xff);
 	I915_WRITE(GMBUS0 + reg_offset, 0);
 
 	/* Hardware may not support GMBUS over these pins? Try GPIO bitbanging instead. */
-	if (!bus->has_gpio) {
-		ret = -EIO;
-	} else {
-		bus->force_bit = true;
-		ret = intel_i2c_quirk_xfer(bus, msgs, num);
-	}
+	bus->force_bit = true;
+	ret = i2c_bit_algo.master_xfer(adapter, msgs, num);
+
 out:
 	mutex_unlock(&dev_priv->gmbus_mutex);
 	return ret;
@@ -346,35 +458,26 @@ static const struct i2c_algorithm gmbus_algorithm = {
  */
 int intel_setup_gmbus(struct drm_device *dev)
 {
-	static const char *names[GMBUS_NUM_PORTS] = {
-		"disabled",
-		"ssc",
-		"vga",
-		"panel",
-		"dpc",
-		"dpb",
-		"reserved",
-		"dpd",
-	};
 	struct drm_i915_private *dev_priv = dev->dev_private;
 	int ret, i;
 
-	dev_priv->gmbus = kcalloc(GMBUS_NUM_PORTS, sizeof(struct intel_gmbus),
-				  GFP_KERNEL);
-	if (dev_priv->gmbus == NULL)
-		return -ENOMEM;
+	if (HAS_PCH_SPLIT(dev))
+		dev_priv->gpio_mmio_base = PCH_GPIOA - GPIOA;
+	else
+		dev_priv->gpio_mmio_base = 0;
 
 	mutex_init(&dev_priv->gmbus_mutex);
 
 	for (i = 0; i < GMBUS_NUM_PORTS; i++) {
 		struct intel_gmbus *bus = &dev_priv->gmbus[i];
+		u32 port = i + 1; /* +1 to map gmbus index to pin pair */
 
 		bus->adapter.owner = THIS_MODULE;
 		bus->adapter.class = I2C_CLASS_DDC;
 		snprintf(bus->adapter.name,
 			 sizeof(bus->adapter.name),
 			 "i915 gmbus %s",
-			 names[i]);
+			 gmbus_ports[i].name);
 
 		bus->adapter.dev.parent = &dev->pdev->dev;
 		bus->dev_priv = dev_priv;
@@ -385,13 +488,13 @@ int intel_setup_gmbus(struct drm_device *dev)
 			goto err;
 
 		/* By default use a conservative clock rate */
-		bus->reg0 = i | GMBUS_RATE_100KHZ;
+		bus->reg0 = port | GMBUS_RATE_100KHZ;
 
-		bus->has_gpio = intel_gpio_setup(bus, i);
-
-		/* XXX force bit banging until GMBUS is fully debugged */
-		if (bus->has_gpio)
+		/* gmbus seems to be broken on i830 */
+		if (IS_I830(dev))
 			bus->force_bit = true;
+
+		intel_gpio_setup(bus, port);
 	}
 
 	intel_i2c_reset(dev_priv->dev);
@@ -403,11 +506,18 @@ err:
 		struct intel_gmbus *bus = &dev_priv->gmbus[i];
 		i2c_del_adapter(&bus->adapter);
 	}
-	kfree(dev_priv->gmbus);
-	dev_priv->gmbus = NULL;
 	return ret;
 }
 
+struct i2c_adapter *intel_gmbus_get_adapter(struct drm_i915_private *dev_priv,
+					    unsigned port)
+{
+	WARN_ON(!intel_gmbus_is_port_valid(port));
+	/* -1 to map pin pair to gmbus index */
+	return (intel_gmbus_is_port_valid(port)) ?
+		&dev_priv->gmbus[port - 1].adapter : NULL;
+}
+
 void intel_gmbus_set_speed(struct i2c_adapter *adapter, int speed)
 {
 	struct intel_gmbus *bus = to_intel_gmbus(adapter);
@@ -419,8 +529,7 @@ void intel_gmbus_force_bit(struct i2c_adapter *adapter, bool force_bit)
 {
 	struct intel_gmbus *bus = to_intel_gmbus(adapter);
 
-	if (bus->has_gpio)
-		bus->force_bit = force_bit;
+	bus->force_bit = force_bit;
 }
 
 void intel_teardown_gmbus(struct drm_device *dev)
@@ -435,7 +544,4 @@ void intel_teardown_gmbus(struct drm_device *dev)
 		struct intel_gmbus *bus = &dev_priv->gmbus[i];
 		i2c_del_adapter(&bus->adapter);
 	}
-
-	kfree(dev_priv->gmbus);
-	dev_priv->gmbus = NULL;
 }
diff --git a/drivers/gpu/drm/i915/intel_lvds.c b/drivers/gpu/drm/i915/intel_lvds.c
index 9c71183629c..9dee82350de 100644
--- a/drivers/gpu/drm/i915/intel_lvds.c
+++ b/drivers/gpu/drm/i915/intel_lvds.c
@@ -480,7 +480,7 @@ static int intel_lvds_get_modes(struct drm_connector *connector)
 
 static int intel_no_modeset_on_lid_dmi_callback(const struct dmi_system_id *id)
 {
-	DRM_DEBUG_KMS("Skipping forced modeset for %s\n", id->ident);
+	DRM_INFO("Skipping forced modeset for %s\n", id->ident);
 	return 1;
 }
 
@@ -628,7 +628,7 @@ static const struct drm_encoder_funcs intel_lvds_enc_funcs = {
 
 static int __init intel_no_lvds_dmi_callback(const struct dmi_system_id *id)
 {
-	DRM_DEBUG_KMS("Skipping LVDS initialization for %s\n", id->ident);
+	DRM_INFO("Skipping LVDS initialization for %s\n", id->ident);
 	return 1;
 }
 
@@ -851,8 +851,8 @@ static bool lvds_is_present_in_vbt(struct drm_device *dev,
 		    child->device_type != DEVICE_TYPE_LFP)
 			continue;
 
-		if (child->i2c_pin)
-		    *i2c_pin = child->i2c_pin;
+		if (intel_gmbus_is_port_valid(child->i2c_pin))
+			*i2c_pin = child->i2c_pin;
 
 		/* However, we cannot trust the BIOS writers to populate
 		 * the VBT correctly.  Since LVDS requires additional
@@ -993,7 +993,8 @@ bool intel_lvds_init(struct drm_device *dev)
 	 * preferred mode is the right one.
 	 */
 	intel_lvds->edid = drm_get_edid(connector,
-					&dev_priv->gmbus[pin].adapter);
+					intel_gmbus_get_adapter(dev_priv,
+								pin));
 	if (intel_lvds->edid) {
 		if (drm_add_edid_modes(connector,
 				       intel_lvds->edid)) {
diff --git a/drivers/gpu/drm/i915/intel_modes.c b/drivers/gpu/drm/i915/intel_modes.c
index d1928e79d9b..d67ec3a51e4 100644
--- a/drivers/gpu/drm/i915/intel_modes.c
+++ b/drivers/gpu/drm/i915/intel_modes.c
@@ -56,7 +56,8 @@ bool intel_ddc_probe(struct intel_encoder *intel_encoder, int ddc_bus)
 		}
 	};
 
-	return i2c_transfer(&dev_priv->gmbus[ddc_bus].adapter, msgs, 2) == 2;
+	return i2c_transfer(intel_gmbus_get_adapter(dev_priv, ddc_bus),
+			    msgs, 2) == 2;
 }
 
 /**
diff --git a/drivers/gpu/drm/i915/intel_opregion.c b/drivers/gpu/drm/i915/intel_opregion.c
index 289140bc83c..18bd0af855d 100644
--- a/drivers/gpu/drm/i915/intel_opregion.c
+++ b/drivers/gpu/drm/i915/intel_opregion.c
@@ -25,6 +25,8 @@
  *
  */
 
+#define pr_fmt(fmt) KBUILD_MODNAME ": " fmt
+
 #include <linux/acpi.h>
 #include <linux/acpi_io.h>
 #include <acpi/video.h>
@@ -149,7 +151,7 @@ struct opregion_asle {
 static u32 asle_set_backlight(struct drm_device *dev, u32 bclp)
 {
 	struct drm_i915_private *dev_priv = dev->dev_private;
-	struct opregion_asle *asle = dev_priv->opregion.asle;
+	struct opregion_asle __iomem *asle = dev_priv->opregion.asle;
 	u32 max;
 
 	if (!(bclp & ASLE_BCLP_VALID))
@@ -161,7 +163,7 @@ static u32 asle_set_backlight(struct drm_device *dev, u32 bclp)
 
 	max = intel_panel_get_max_backlight(dev);
 	intel_panel_set_backlight(dev, bclp * max / 255);
-	asle->cblv = (bclp*0x64)/0xff | ASLE_CBLV_VALID;
+	iowrite32((bclp*0x64)/0xff | ASLE_CBLV_VALID, &asle->cblv);
 
 	return 0;
 }
@@ -198,14 +200,14 @@ static u32 asle_set_pfit(struct drm_device *dev, u32 pfit)
 void intel_opregion_asle_intr(struct drm_device *dev)
 {
 	struct drm_i915_private *dev_priv = dev->dev_private;
-	struct opregion_asle *asle = dev_priv->opregion.asle;
+	struct opregion_asle __iomem *asle = dev_priv->opregion.asle;
 	u32 asle_stat = 0;
 	u32 asle_req;
 
 	if (!asle)
 		return;
 
-	asle_req = asle->aslc & ASLE_REQ_MSK;
+	asle_req = ioread32(&asle->aslc) & ASLE_REQ_MSK;
 
 	if (!asle_req) {
 		DRM_DEBUG_DRIVER("non asle set request??\n");
@@ -213,31 +215,31 @@ void intel_opregion_asle_intr(struct drm_device *dev)
 	}
 
 	if (asle_req & ASLE_SET_ALS_ILLUM)
-		asle_stat |= asle_set_als_illum(dev, asle->alsi);
+		asle_stat |= asle_set_als_illum(dev, ioread32(&asle->alsi));
 
 	if (asle_req & ASLE_SET_BACKLIGHT)
-		asle_stat |= asle_set_backlight(dev, asle->bclp);
+		asle_stat |= asle_set_backlight(dev, ioread32(&asle->bclp));
 
 	if (asle_req & ASLE_SET_PFIT)
-		asle_stat |= asle_set_pfit(dev, asle->pfit);
+		asle_stat |= asle_set_pfit(dev, ioread32(&asle->pfit));
 
 	if (asle_req & ASLE_SET_PWM_FREQ)
-		asle_stat |= asle_set_pwm_freq(dev, asle->pfmb);
+		asle_stat |= asle_set_pwm_freq(dev, ioread32(&asle->pfmb));
 
-	asle->aslc = asle_stat;
+	iowrite32(asle_stat, &asle->aslc);
 }
 
 void intel_opregion_gse_intr(struct drm_device *dev)
 {
 	struct drm_i915_private *dev_priv = dev->dev_private;
-	struct opregion_asle *asle = dev_priv->opregion.asle;
+	struct opregion_asle __iomem *asle = dev_priv->opregion.asle;
 	u32 asle_stat = 0;
 	u32 asle_req;
 
 	if (!asle)
 		return;
 
-	asle_req = asle->aslc & ASLE_REQ_MSK;
+	asle_req = ioread32(&asle->aslc) & ASLE_REQ_MSK;
 
 	if (!asle_req) {
 		DRM_DEBUG_DRIVER("non asle set request??\n");
@@ -250,7 +252,7 @@ void intel_opregion_gse_intr(struct drm_device *dev)
 	}
 
 	if (asle_req & ASLE_SET_BACKLIGHT)
-		asle_stat |= asle_set_backlight(dev, asle->bclp);
+		asle_stat |= asle_set_backlight(dev, ioread32(&asle->bclp));
 
 	if (asle_req & ASLE_SET_PFIT) {
 		DRM_DEBUG_DRIVER("Pfit is not supported\n");
@@ -262,7 +264,7 @@ void intel_opregion_gse_intr(struct drm_device *dev)
 		asle_stat |= ASLE_PWM_FREQ_FAILED;
 	}
 
-	asle->aslc = asle_stat;
+	iowrite32(asle_stat, &asle->aslc);
 }
 #define ASLE_ALS_EN    (1<<0)
 #define ASLE_BLC_EN    (1<<1)
@@ -272,15 +274,16 @@ void intel_opregion_gse_intr(struct drm_device *dev)
 void intel_opregion_enable_asle(struct drm_device *dev)
 {
 	struct drm_i915_private *dev_priv = dev->dev_private;
-	struct opregion_asle *asle = dev_priv->opregion.asle;
+	struct opregion_asle __iomem *asle = dev_priv->opregion.asle;
 
 	if (asle) {
 		if (IS_MOBILE(dev))
 			intel_enable_asle(dev);
 
-		asle->tche = ASLE_ALS_EN | ASLE_BLC_EN | ASLE_PFIT_EN |
-			ASLE_PFMB_EN;
-		asle->ardy = 1;
+		iowrite32(ASLE_ALS_EN | ASLE_BLC_EN | ASLE_PFIT_EN |
+			  ASLE_PFMB_EN,
+			  &asle->tche);
+		iowrite32(1, &asle->ardy);
 	}
 }
 
@@ -298,7 +301,7 @@ static int intel_opregion_video_event(struct notifier_block *nb,
 	   Linux, these are handled by the dock, button and video drivers.
 	*/
 
-	struct opregion_acpi *acpi;
+	struct opregion_acpi __iomem *acpi;
 	struct acpi_bus_event *event = data;
 	int ret = NOTIFY_OK;
 
@@ -310,10 +313,11 @@ static int intel_opregion_video_event(struct notifier_block *nb,
 
 	acpi = system_opregion->acpi;
 
-	if (event->type == 0x80 && !(acpi->cevt & 0x1))
+	if (event->type == 0x80 &&
+	    (ioread32(&acpi->cevt) & 1) == 0)
 		ret = NOTIFY_BAD;
 
-	acpi->csts = 0;
+	iowrite32(0, &acpi->csts);
 
 	return ret;
 }
@@ -337,6 +341,7 @@ static void intel_didl_outputs(struct drm_device *dev)
 	struct acpi_device *acpi_dev, *acpi_cdev, *acpi_video_bus = NULL;
 	unsigned long long device_id;
 	acpi_status status;
+	u32 temp;
 	int i = 0;
 
 	handle = DEVICE_ACPI_HANDLE(&dev->pdev->dev);
@@ -355,7 +360,7 @@ static void intel_didl_outputs(struct drm_device *dev)
 	}
 
 	if (!acpi_video_bus) {
-		printk(KERN_WARNING "No ACPI video bus found\n");
+		pr_warn("No ACPI video bus found\n");
 		return;
 	}
 
@@ -371,7 +376,8 @@ static void intel_didl_outputs(struct drm_device *dev)
 		if (ACPI_SUCCESS(status)) {
 			if (!device_id)
 				goto blind_set;
-			opregion->acpi->didl[i] = (u32)(device_id & 0x0f0f);
+			iowrite32((u32)(device_id & 0x0f0f),
+				  &opregion->acpi->didl[i]);
 			i++;
 		}
 	}
@@ -379,7 +385,7 @@ static void intel_didl_outputs(struct drm_device *dev)
 end:
 	/* If fewer than 8 outputs, the list must be null terminated */
 	if (i < 8)
-		opregion->acpi->didl[i] = 0;
+		iowrite32(0, &opregion->acpi->didl[i]);
 	return;
 
 blind_set:
@@ -413,7 +419,9 @@ blind_set:
 			output_type = ACPI_LVDS_OUTPUT;
 			break;
 		}
-		opregion->acpi->didl[i] |= (1<<31) | output_type | i;
+		temp = ioread32(&opregion->acpi->didl[i]);
+		iowrite32(temp | (1<<31) | output_type | i,
+			  &opregion->acpi->didl[i]);
 		i++;
 	}
 	goto end;
@@ -434,8 +442,8 @@ void intel_opregion_init(struct drm_device *dev)
 		/* Notify BIOS we are ready to handle ACPI video ext notifs.
 		 * Right now, all the events are handled by the ACPI video module.
 		 * We don't actually need to do anything with them. */
-		opregion->acpi->csts = 0;
-		opregion->acpi->drdy = 1;
+		iowrite32(0, &opregion->acpi->csts);
+		iowrite32(1, &opregion->acpi->drdy);
 
 		system_opregion = opregion;
 		register_acpi_notifier(&intel_opregion_notifier);
@@ -454,7 +462,7 @@ void intel_opregion_fini(struct drm_device *dev)
 		return;
 
 	if (opregion->acpi) {
-		opregion->acpi->drdy = 0;
+		iowrite32(0, &opregion->acpi->drdy);
 
 		system_opregion = NULL;
 		unregister_acpi_notifier(&intel_opregion_notifier);
@@ -474,8 +482,9 @@ int intel_opregion_setup(struct drm_device *dev)
 {
 	struct drm_i915_private *dev_priv = dev->dev_private;
 	struct intel_opregion *opregion = &dev_priv->opregion;
-	void *base;
+	void __iomem *base;
 	u32 asls, mboxes;
+	char buf[sizeof(OPREGION_SIGNATURE)];
 	int err = 0;
 
 	pci_read_config_dword(dev->pdev, PCI_ASLS, &asls);
@@ -489,7 +498,9 @@ int intel_opregion_setup(struct drm_device *dev)
 	if (!base)
 		return -ENOMEM;
 
-	if (memcmp(base, OPREGION_SIGNATURE, 16)) {
+	memcpy_fromio(buf, base, sizeof(buf));
+
+	if (memcmp(buf, OPREGION_SIGNATURE, 16)) {
 		DRM_DEBUG_DRIVER("opregion signature mismatch\n");
 		err = -EINVAL;
 		goto err_out;
@@ -499,7 +510,7 @@ int intel_opregion_setup(struct drm_device *dev)
 
 	opregion->lid_state = base + ACPI_CLID;
 
-	mboxes = opregion->header->mboxes;
+	mboxes = ioread32(&opregion->header->mboxes);
 	if (mboxes & MBOX_ACPI) {
 		DRM_DEBUG_DRIVER("Public ACPI methods supported\n");
 		opregion->acpi = base + OPREGION_ACPI_OFFSET;
diff --git a/drivers/gpu/drm/i915/intel_overlay.c b/drivers/gpu/drm/i915/intel_overlay.c
index 80b331c322f..458743da377 100644
--- a/drivers/gpu/drm/i915/intel_overlay.c
+++ b/drivers/gpu/drm/i915/intel_overlay.c
@@ -187,14 +187,14 @@ struct intel_overlay {
 	void (*flip_tail)(struct intel_overlay *);
 };
 
-static struct overlay_registers *
+static struct overlay_registers __iomem *
 intel_overlay_map_regs(struct intel_overlay *overlay)
 {
 	drm_i915_private_t *dev_priv = overlay->dev->dev_private;
-	struct overlay_registers *regs;
+	struct overlay_registers __iomem *regs;
 
 	if (OVERLAY_NEEDS_PHYSICAL(overlay->dev))
-		regs = overlay->reg_bo->phys_obj->handle->vaddr;
+		regs = (struct overlay_registers __iomem *)overlay->reg_bo->phys_obj->handle->vaddr;
 	else
 		regs = io_mapping_map_wc(dev_priv->mm.gtt_mapping,
 					 overlay->reg_bo->gtt_offset);
@@ -203,7 +203,7 @@ intel_overlay_map_regs(struct intel_overlay *overlay)
 }
 
 static void intel_overlay_unmap_regs(struct intel_overlay *overlay,
-				     struct overlay_registers *regs)
+				     struct overlay_registers __iomem *regs)
 {
 	if (!OVERLAY_NEEDS_PHYSICAL(overlay->dev))
 		io_mapping_unmap(regs);
@@ -215,20 +215,21 @@ static int intel_overlay_do_wait_request(struct intel_overlay *overlay,
 {
 	struct drm_device *dev = overlay->dev;
 	drm_i915_private_t *dev_priv = dev->dev_private;
+	struct intel_ring_buffer *ring = &dev_priv->ring[RCS];
 	int ret;
 
 	BUG_ON(overlay->last_flip_req);
-	ret = i915_add_request(LP_RING(dev_priv), NULL, request);
+	ret = i915_add_request(ring, NULL, request);
 	if (ret) {
 	    kfree(request);
 	    return ret;
 	}
 	overlay->last_flip_req = request->seqno;
 	overlay->flip_tail = tail;
-	ret = i915_wait_request(LP_RING(dev_priv), overlay->last_flip_req,
-				true);
+	ret = i915_wait_request(ring, overlay->last_flip_req);
 	if (ret)
 		return ret;
+	i915_gem_retire_requests(dev);
 
 	overlay->last_flip_req = 0;
 	return 0;
@@ -262,7 +263,7 @@ i830_activate_pipe_a(struct drm_device *dev)
 	DRM_DEBUG_DRIVER("Enabling pipe A in order to enable overlay\n");
 
 	mode = drm_mode_duplicate(dev, &vesa_640x480);
-	drm_mode_set_crtcinfo(mode, 0);
+
 	if (!drm_crtc_helper_set_mode(&crtc->base, mode,
 				       crtc->base.x, crtc->base.y,
 				       crtc->base.fb))
@@ -287,6 +288,7 @@ static int intel_overlay_on(struct intel_overlay *overlay)
 {
 	struct drm_device *dev = overlay->dev;
 	struct drm_i915_private *dev_priv = dev->dev_private;
+	struct intel_ring_buffer *ring = &dev_priv->ring[RCS];
 	struct drm_i915_gem_request *request;
 	int pipe_a_quirk = 0;
 	int ret;
@@ -306,17 +308,17 @@ static int intel_overlay_on(struct intel_overlay *overlay)
 		goto out;
 	}
 
-	ret = BEGIN_LP_RING(4);
+	ret = intel_ring_begin(ring, 4);
 	if (ret) {
 		kfree(request);
 		goto out;
 	}
 
-	OUT_RING(MI_OVERLAY_FLIP | MI_OVERLAY_ON);
-	OUT_RING(overlay->flip_addr | OFC_UPDATE);
-	OUT_RING(MI_WAIT_FOR_EVENT | MI_WAIT_FOR_OVERLAY_FLIP);
-	OUT_RING(MI_NOOP);
-	ADVANCE_LP_RING();
+	intel_ring_emit(ring, MI_OVERLAY_FLIP | MI_OVERLAY_ON);
+	intel_ring_emit(ring, overlay->flip_addr | OFC_UPDATE);
+	intel_ring_emit(ring, MI_WAIT_FOR_EVENT | MI_WAIT_FOR_OVERLAY_FLIP);
+	intel_ring_emit(ring, MI_NOOP);
+	intel_ring_advance(ring);
 
 	ret = intel_overlay_do_wait_request(overlay, request, NULL);
 out:
@@ -332,6 +334,7 @@ static int intel_overlay_continue(struct intel_overlay *overlay,
 {
 	struct drm_device *dev = overlay->dev;
 	drm_i915_private_t *dev_priv = dev->dev_private;
+	struct intel_ring_buffer *ring = &dev_priv->ring[RCS];
 	struct drm_i915_gem_request *request;
 	u32 flip_addr = overlay->flip_addr;
 	u32 tmp;
@@ -351,16 +354,16 @@ static int intel_overlay_continue(struct intel_overlay *overlay,
 	if (tmp & (1 << 17))
 		DRM_DEBUG("overlay underrun, DOVSTA: %x\n", tmp);
 
-	ret = BEGIN_LP_RING(2);
+	ret = intel_ring_begin(ring, 2);
 	if (ret) {
 		kfree(request);
 		return ret;
 	}
-	OUT_RING(MI_OVERLAY_FLIP | MI_OVERLAY_CONTINUE);
-	OUT_RING(flip_addr);
-	ADVANCE_LP_RING();
+	intel_ring_emit(ring, MI_OVERLAY_FLIP | MI_OVERLAY_CONTINUE);
+	intel_ring_emit(ring, flip_addr);
+	intel_ring_advance(ring);
 
-	ret = i915_add_request(LP_RING(dev_priv), NULL, request);
+	ret = i915_add_request(ring, NULL, request);
 	if (ret) {
 		kfree(request);
 		return ret;
@@ -401,6 +404,7 @@ static int intel_overlay_off(struct intel_overlay *overlay)
 {
 	struct drm_device *dev = overlay->dev;
 	struct drm_i915_private *dev_priv = dev->dev_private;
+	struct intel_ring_buffer *ring = &dev_priv->ring[RCS];
 	u32 flip_addr = overlay->flip_addr;
 	struct drm_i915_gem_request *request;
 	int ret;
@@ -417,20 +421,20 @@ static int intel_overlay_off(struct intel_overlay *overlay)
 	 * of the hw. Do it in both cases */
 	flip_addr |= OFC_UPDATE;
 
-	ret = BEGIN_LP_RING(6);
+	ret = intel_ring_begin(ring, 6);
 	if (ret) {
 		kfree(request);
 		return ret;
 	}
 	/* wait for overlay to go idle */
-	OUT_RING(MI_OVERLAY_FLIP | MI_OVERLAY_CONTINUE);
-	OUT_RING(flip_addr);
-	OUT_RING(MI_WAIT_FOR_EVENT | MI_WAIT_FOR_OVERLAY_FLIP);
+	intel_ring_emit(ring, MI_OVERLAY_FLIP | MI_OVERLAY_CONTINUE);
+	intel_ring_emit(ring, flip_addr);
+	intel_ring_emit(ring, MI_WAIT_FOR_EVENT | MI_WAIT_FOR_OVERLAY_FLIP);
 	/* turn overlay off */
-	OUT_RING(MI_OVERLAY_FLIP | MI_OVERLAY_OFF);
-	OUT_RING(flip_addr);
-	OUT_RING(MI_WAIT_FOR_EVENT | MI_WAIT_FOR_OVERLAY_FLIP);
-	ADVANCE_LP_RING();
+	intel_ring_emit(ring, MI_OVERLAY_FLIP | MI_OVERLAY_OFF);
+	intel_ring_emit(ring, flip_addr);
+	intel_ring_emit(ring, MI_WAIT_FOR_EVENT | MI_WAIT_FOR_OVERLAY_FLIP);
+	intel_ring_advance(ring);
 
 	return intel_overlay_do_wait_request(overlay, request,
 					     intel_overlay_off_tail);
@@ -442,15 +446,16 @@ static int intel_overlay_recover_from_interrupt(struct intel_overlay *overlay)
 {
 	struct drm_device *dev = overlay->dev;
 	drm_i915_private_t *dev_priv = dev->dev_private;
+	struct intel_ring_buffer *ring = &dev_priv->ring[RCS];
 	int ret;
 
 	if (overlay->last_flip_req == 0)
 		return 0;
 
-	ret = i915_wait_request(LP_RING(dev_priv), overlay->last_flip_req,
-				true);
+	ret = i915_wait_request(ring, overlay->last_flip_req);
 	if (ret)
 		return ret;
+	i915_gem_retire_requests(dev);
 
 	if (overlay->flip_tail)
 		overlay->flip_tail(overlay);
@@ -467,6 +472,7 @@ static int intel_overlay_release_old_vid(struct intel_overlay *overlay)
 {
 	struct drm_device *dev = overlay->dev;
 	drm_i915_private_t *dev_priv = dev->dev_private;
+	struct intel_ring_buffer *ring = &dev_priv->ring[RCS];
 	int ret;
 
 	/* Only wait if there is actually an old frame to release to
@@ -483,15 +489,15 @@ static int intel_overlay_release_old_vid(struct intel_overlay *overlay)
 		if (request == NULL)
 			return -ENOMEM;
 
-		ret = BEGIN_LP_RING(2);
+		ret = intel_ring_begin(ring, 2);
 		if (ret) {
 			kfree(request);
 			return ret;
 		}
 
-		OUT_RING(MI_WAIT_FOR_EVENT | MI_WAIT_FOR_OVERLAY_FLIP);
-		OUT_RING(MI_NOOP);
-		ADVANCE_LP_RING();
+		intel_ring_emit(ring, MI_WAIT_FOR_EVENT | MI_WAIT_FOR_OVERLAY_FLIP);
+		intel_ring_emit(ring, MI_NOOP);
+		intel_ring_advance(ring);
 
 		ret = intel_overlay_do_wait_request(overlay, request,
 						    intel_overlay_release_old_vid_tail);
@@ -619,14 +625,15 @@ static const u16 uv_static_hcoeffs[N_HORIZ_UV_TAPS * N_PHASES] = {
 	0x3000, 0x0800, 0x3000
 };
 
-static void update_polyphase_filter(struct overlay_registers *regs)
+static void update_polyphase_filter(struct overlay_registers __iomem *regs)
 {
-	memcpy(regs->Y_HCOEFS, y_static_hcoeffs, sizeof(y_static_hcoeffs));
-	memcpy(regs->UV_HCOEFS, uv_static_hcoeffs, sizeof(uv_static_hcoeffs));
+	memcpy_toio(regs->Y_HCOEFS, y_static_hcoeffs, sizeof(y_static_hcoeffs));
+	memcpy_toio(regs->UV_HCOEFS, uv_static_hcoeffs,
+		    sizeof(uv_static_hcoeffs));
 }
 
 static bool update_scaling_factors(struct intel_overlay *overlay,
-				   struct overlay_registers *regs,
+				   struct overlay_registers __iomem *regs,
 				   struct put_image_params *params)
 {
 	/* fixed point with a 12 bit shift */
@@ -665,16 +672,19 @@ static bool update_scaling_factors(struct intel_overlay *overlay,
 	overlay->old_xscale = xscale;
 	overlay->old_yscale = yscale;
 
-	regs->YRGBSCALE = (((yscale & FRACT_MASK) << 20) |
-			   ((xscale >> FP_SHIFT)  << 16) |
-			   ((xscale & FRACT_MASK) << 3));
+	iowrite32(((yscale & FRACT_MASK) << 20) |
+		  ((xscale >> FP_SHIFT)  << 16) |
+		  ((xscale & FRACT_MASK) << 3),
+		 &regs->YRGBSCALE);
 
-	regs->UVSCALE = (((yscale_UV & FRACT_MASK) << 20) |
-			 ((xscale_UV >> FP_SHIFT)  << 16) |
-			 ((xscale_UV & FRACT_MASK) << 3));
+	iowrite32(((yscale_UV & FRACT_MASK) << 20) |
+		  ((xscale_UV >> FP_SHIFT)  << 16) |
+		  ((xscale_UV & FRACT_MASK) << 3),
+		 &regs->UVSCALE);
 
-	regs->UVSCALEV = ((((yscale    >> FP_SHIFT) << 16) |
-			   ((yscale_UV >> FP_SHIFT) << 0)));
+	iowrite32((((yscale    >> FP_SHIFT) << 16) |
+		   ((yscale_UV >> FP_SHIFT) << 0)),
+		 &regs->UVSCALEV);
 
 	if (scale_changed)
 		update_polyphase_filter(regs);
@@ -683,30 +693,32 @@ static bool update_scaling_factors(struct intel_overlay *overlay,
 }
 
 static void update_colorkey(struct intel_overlay *overlay,
-			    struct overlay_registers *regs)
+			    struct overlay_registers __iomem *regs)
 {
 	u32 key = overlay->color_key;
 
 	switch (overlay->crtc->base.fb->bits_per_pixel) {
 	case 8:
-		regs->DCLRKV = 0;
-		regs->DCLRKM = CLK_RGB8I_MASK | DST_KEY_ENABLE;
+		iowrite32(0, &regs->DCLRKV);
+		iowrite32(CLK_RGB8I_MASK | DST_KEY_ENABLE, &regs->DCLRKM);
 		break;
 
 	case 16:
 		if (overlay->crtc->base.fb->depth == 15) {
-			regs->DCLRKV = RGB15_TO_COLORKEY(key);
-			regs->DCLRKM = CLK_RGB15_MASK | DST_KEY_ENABLE;
+			iowrite32(RGB15_TO_COLORKEY(key), &regs->DCLRKV);
+			iowrite32(CLK_RGB15_MASK | DST_KEY_ENABLE,
+				  &regs->DCLRKM);
 		} else {
-			regs->DCLRKV = RGB16_TO_COLORKEY(key);
-			regs->DCLRKM = CLK_RGB16_MASK | DST_KEY_ENABLE;
+			iowrite32(RGB16_TO_COLORKEY(key), &regs->DCLRKV);
+			iowrite32(CLK_RGB16_MASK | DST_KEY_ENABLE,
+				  &regs->DCLRKM);
 		}
 		break;
 
 	case 24:
 	case 32:
-		regs->DCLRKV = key;
-		regs->DCLRKM = CLK_RGB24_MASK | DST_KEY_ENABLE;
+		iowrite32(key, &regs->DCLRKV);
+		iowrite32(CLK_RGB24_MASK | DST_KEY_ENABLE, &regs->DCLRKM);
 		break;
 	}
 }
@@ -761,9 +773,10 @@ static int intel_overlay_do_put_image(struct intel_overlay *overlay,
 				      struct put_image_params *params)
 {
 	int ret, tmp_width;
-	struct overlay_registers *regs;
+	struct overlay_registers __iomem *regs;
 	bool scale_changed = false;
 	struct drm_device *dev = overlay->dev;
+	u32 swidth, swidthsw, sheight, ostride;
 
 	BUG_ON(!mutex_is_locked(&dev->struct_mutex));
 	BUG_ON(!mutex_is_locked(&dev->mode_config.mutex));
@@ -782,16 +795,18 @@ static int intel_overlay_do_put_image(struct intel_overlay *overlay,
 		goto out_unpin;
 
 	if (!overlay->active) {
+		u32 oconfig;
 		regs = intel_overlay_map_regs(overlay);
 		if (!regs) {
 			ret = -ENOMEM;
 			goto out_unpin;
 		}
-		regs->OCONFIG = OCONF_CC_OUT_8BIT;
+		oconfig = OCONF_CC_OUT_8BIT;
 		if (IS_GEN4(overlay->dev))
-			regs->OCONFIG |= OCONF_CSC_MODE_BT709;
-		regs->OCONFIG |= overlay->crtc->pipe == 0 ?
+			oconfig |= OCONF_CSC_MODE_BT709;
+		oconfig |= overlay->crtc->pipe == 0 ?
 			OCONF_PIPE_A : OCONF_PIPE_B;
+		iowrite32(oconfig, &regs->OCONFIG);
 		intel_overlay_unmap_regs(overlay, regs);
 
 		ret = intel_overlay_on(overlay);
@@ -805,42 +820,46 @@ static int intel_overlay_do_put_image(struct intel_overlay *overlay,
 		goto out_unpin;
 	}
 
-	regs->DWINPOS = (params->dst_y << 16) | params->dst_x;
-	regs->DWINSZ = (params->dst_h << 16) | params->dst_w;
+	iowrite32((params->dst_y << 16) | params->dst_x, &regs->DWINPOS);
+	iowrite32((params->dst_h << 16) | params->dst_w, &regs->DWINSZ);
 
 	if (params->format & I915_OVERLAY_YUV_PACKED)
 		tmp_width = packed_width_bytes(params->format, params->src_w);
 	else
 		tmp_width = params->src_w;
 
-	regs->SWIDTH = params->src_w;
-	regs->SWIDTHSW = calc_swidthsw(overlay->dev,
-				       params->offset_Y, tmp_width);
-	regs->SHEIGHT = params->src_h;
-	regs->OBUF_0Y = new_bo->gtt_offset + params->offset_Y;
-	regs->OSTRIDE = params->stride_Y;
+	swidth = params->src_w;
+	swidthsw = calc_swidthsw(overlay->dev, params->offset_Y, tmp_width);
+	sheight = params->src_h;
+	iowrite32(new_bo->gtt_offset + params->offset_Y, &regs->OBUF_0Y);
+	ostride = params->stride_Y;
 
 	if (params->format & I915_OVERLAY_YUV_PLANAR) {
 		int uv_hscale = uv_hsubsampling(params->format);
 		int uv_vscale = uv_vsubsampling(params->format);
 		u32 tmp_U, tmp_V;
-		regs->SWIDTH |= (params->src_w/uv_hscale) << 16;
+		swidth |= (params->src_w/uv_hscale) << 16;
 		tmp_U = calc_swidthsw(overlay->dev, params->offset_U,
 				      params->src_w/uv_hscale);
 		tmp_V = calc_swidthsw(overlay->dev, params->offset_V,
 				      params->src_w/uv_hscale);
-		regs->SWIDTHSW |= max_t(u32, tmp_U, tmp_V) << 16;
-		regs->SHEIGHT |= (params->src_h/uv_vscale) << 16;
-		regs->OBUF_0U = new_bo->gtt_offset + params->offset_U;
-		regs->OBUF_0V = new_bo->gtt_offset + params->offset_V;
-		regs->OSTRIDE |= params->stride_UV << 16;
+		swidthsw |= max_t(u32, tmp_U, tmp_V) << 16;
+		sheight |= (params->src_h/uv_vscale) << 16;
+		iowrite32(new_bo->gtt_offset + params->offset_U, &regs->OBUF_0U);
+		iowrite32(new_bo->gtt_offset + params->offset_V, &regs->OBUF_0V);
+		ostride |= params->stride_UV << 16;
 	}
 
+	iowrite32(swidth, &regs->SWIDTH);
+	iowrite32(swidthsw, &regs->SWIDTHSW);
+	iowrite32(sheight, &regs->SHEIGHT);
+	iowrite32(ostride, &regs->OSTRIDE);
+
 	scale_changed = update_scaling_factors(overlay, regs, params);
 
 	update_colorkey(overlay, regs);
 
-	regs->OCMD = overlay_cmd_reg(params);
+	iowrite32(overlay_cmd_reg(params), &regs->OCMD);
 
 	intel_overlay_unmap_regs(overlay, regs);
 
@@ -860,7 +879,7 @@ out_unpin:
 
 int intel_overlay_switch_off(struct intel_overlay *overlay)
 {
-	struct overlay_registers *regs;
+	struct overlay_registers __iomem *regs;
 	struct drm_device *dev = overlay->dev;
 	int ret;
 
@@ -879,7 +898,7 @@ int intel_overlay_switch_off(struct intel_overlay *overlay)
 		return ret;
 
 	regs = intel_overlay_map_regs(overlay);
-	regs->OCMD = 0;
+	iowrite32(0, &regs->OCMD);
 	intel_overlay_unmap_regs(overlay, regs);
 
 	ret = intel_overlay_off(overlay);
@@ -1109,11 +1128,7 @@ int intel_overlay_put_image(struct drm_device *dev, void *data,
 	struct put_image_params *params;
 	int ret;
 
-	if (!dev_priv) {
-		DRM_ERROR("called with no initialization\n");
-		return -EINVAL;
-	}
-
+	/* No need to check for DRIVER_MODESET - we don't set it up then. */
 	overlay = dev_priv->overlay;
 	if (!overlay) {
 		DRM_DEBUG("userspace bug: no overlay\n");
@@ -1250,10 +1265,11 @@ out_free:
 }
 
 static void update_reg_attrs(struct intel_overlay *overlay,
-			     struct overlay_registers *regs)
+			     struct overlay_registers __iomem *regs)
 {
-	regs->OCLRC0 = (overlay->contrast << 18) | (overlay->brightness & 0xff);
-	regs->OCLRC1 = overlay->saturation;
+	iowrite32((overlay->contrast << 18) | (overlay->brightness & 0xff),
+		  &regs->OCLRC0);
+	iowrite32(overlay->saturation, &regs->OCLRC1);
 }
 
 static bool check_gamma_bounds(u32 gamma1, u32 gamma2)
@@ -1306,14 +1322,10 @@ int intel_overlay_attrs(struct drm_device *dev, void *data,
 	struct drm_intel_overlay_attrs *attrs = data;
 	drm_i915_private_t *dev_priv = dev->dev_private;
 	struct intel_overlay *overlay;
-	struct overlay_registers *regs;
+	struct overlay_registers __iomem *regs;
 	int ret;
 
-	if (!dev_priv) {
-		DRM_ERROR("called with no initialization\n");
-		return -EINVAL;
-	}
-
+	/* No need to check for DRIVER_MODESET - we don't set it up then. */
 	overlay = dev_priv->overlay;
 	if (!overlay) {
 		DRM_DEBUG("userspace bug: no overlay\n");
@@ -1396,7 +1408,7 @@ void intel_setup_overlay(struct drm_device *dev)
 	drm_i915_private_t *dev_priv = dev->dev_private;
 	struct intel_overlay *overlay;
 	struct drm_i915_gem_object *reg_bo;
-	struct overlay_registers *regs;
+	struct overlay_registers __iomem *regs;
 	int ret;
 
 	if (!HAS_OVERLAY(dev))
@@ -1451,7 +1463,7 @@ void intel_setup_overlay(struct drm_device *dev)
 	if (!regs)
 		goto out_unpin_bo;
 
-	memset(regs, 0, sizeof(struct overlay_registers));
+	memset_io(regs, 0, sizeof(struct overlay_registers));
 	update_polyphase_filter(regs);
 	update_reg_attrs(overlay, regs);
 
@@ -1499,14 +1511,17 @@ struct intel_overlay_error_state {
 	u32 isr;
 };
 
-static struct overlay_registers *
+static struct overlay_registers __iomem *
 intel_overlay_map_regs_atomic(struct intel_overlay *overlay)
 {
 	drm_i915_private_t *dev_priv = overlay->dev->dev_private;
-	struct overlay_registers *regs;
+	struct overlay_registers __iomem *regs;
 
 	if (OVERLAY_NEEDS_PHYSICAL(overlay->dev))
-		regs = overlay->reg_bo->phys_obj->handle->vaddr;
+		/* Cast to make sparse happy, but it's wc memory anyway, so
+		 * equivalent to the wc io mapping on X86. */
+		regs = (struct overlay_registers __iomem *)
+			overlay->reg_bo->phys_obj->handle->vaddr;
 	else
 		regs = io_mapping_map_atomic_wc(dev_priv->mm.gtt_mapping,
 						overlay->reg_bo->gtt_offset);
@@ -1515,7 +1530,7 @@ intel_overlay_map_regs_atomic(struct intel_overlay *overlay)
 }
 
 static void intel_overlay_unmap_regs_atomic(struct intel_overlay *overlay,
-					    struct overlay_registers *regs)
+					struct overlay_registers __iomem *regs)
 {
 	if (!OVERLAY_NEEDS_PHYSICAL(overlay->dev))
 		io_mapping_unmap_atomic(regs);
@@ -1540,9 +1555,9 @@ intel_overlay_capture_error_state(struct drm_device *dev)
 	error->dovsta = I915_READ(DOVSTA);
 	error->isr = I915_READ(ISR);
 	if (OVERLAY_NEEDS_PHYSICAL(overlay->dev))
-		error->base = (long) overlay->reg_bo->phys_obj->handle->vaddr;
+		error->base = (__force long)overlay->reg_bo->phys_obj->handle->vaddr;
 	else
-		error->base = (long) overlay->reg_bo->gtt_offset;
+		error->base = overlay->reg_bo->gtt_offset;
 
 	regs = intel_overlay_map_regs_atomic(overlay);
 	if (!regs)
diff --git a/drivers/gpu/drm/i915/intel_panel.c b/drivers/gpu/drm/i915/intel_panel.c
index 48177ec4720..2a1625d84a6 100644
--- a/drivers/gpu/drm/i915/intel_panel.c
+++ b/drivers/gpu/drm/i915/intel_panel.c
@@ -28,6 +28,9 @@
  *      Chris Wilson <chris@chris-wilson.co.uk>
  */
 
+#define pr_fmt(fmt) KBUILD_MODNAME ": " fmt
+
+#include <linux/moduleparam.h>
 #include "intel_drv.h"
 
 #define PCI_LBPC 0xf4 /* legacy/combination backlight modes */
@@ -169,7 +172,7 @@ u32 intel_panel_get_max_backlight(struct drm_device *dev)
 		/* XXX add code here to query mode clock or hardware clock
 		 * and program max PWM appropriately.
 		 */
-		printk_once(KERN_WARNING "fixme: max PWM is zero.\n");
+		pr_warn_once("fixme: max PWM is zero\n");
 		return 1;
 	}
 
@@ -189,6 +192,27 @@ u32 intel_panel_get_max_backlight(struct drm_device *dev)
 	return max;
 }
 
+static int i915_panel_invert_brightness;
+MODULE_PARM_DESC(invert_brightness, "Invert backlight brightness "
+	"(-1 force normal, 0 machine defaults, 1 force inversion), please "
+	"report PCI device ID, subsystem vendor and subsystem device ID "
+	"to dri-devel@lists.freedesktop.org, if your machine needs it. "
+	"It will then be included in an upcoming module version.");
+module_param_named(invert_brightness, i915_panel_invert_brightness, int, 0600);
+static u32 intel_panel_compute_brightness(struct drm_device *dev, u32 val)
+{
+	struct drm_i915_private *dev_priv = dev->dev_private;
+
+	if (i915_panel_invert_brightness < 0)
+		return val;
+
+	if (i915_panel_invert_brightness > 0 ||
+	    dev_priv->quirks & QUIRK_INVERT_BRIGHTNESS)
+		return intel_panel_get_max_backlight(dev) - val;
+
+	return val;
+}
+
 u32 intel_panel_get_backlight(struct drm_device *dev)
 {
 	struct drm_i915_private *dev_priv = dev->dev_private;
@@ -209,6 +233,7 @@ u32 intel_panel_get_backlight(struct drm_device *dev)
 		}
 	}
 
+	val = intel_panel_compute_brightness(dev, val);
 	DRM_DEBUG_DRIVER("get backlight PWM = %d\n", val);
 	return val;
 }
@@ -226,6 +251,7 @@ static void intel_panel_actually_set_backlight(struct drm_device *dev, u32 level
 	u32 tmp;
 
 	DRM_DEBUG_DRIVER("set backlight PWM = %d\n", level);
+	level = intel_panel_compute_brightness(dev, level);
 
 	if (HAS_PCH_SPLIT(dev))
 		return intel_pch_panel_set_backlight(dev, level);
@@ -342,6 +368,7 @@ int intel_panel_setup_backlight(struct drm_device *dev)
 	else
 		return -ENODEV;
 
+	memset(&props, 0, sizeof(props));
 	props.type = BACKLIGHT_RAW;
 	props.max_brightness = intel_panel_get_max_backlight(dev);
 	dev_priv->backlight =
diff --git a/drivers/gpu/drm/i915/intel_pm.c b/drivers/gpu/drm/i915/intel_pm.c
new file mode 100644
index 00000000000..8e79ff67ec9
--- /dev/null
+++ b/drivers/gpu/drm/i915/intel_pm.c
@@ -0,0 +1,3796 @@
+/*
+ * Copyright © 2012 Intel Corporation
+ *
+ * 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:
+ *    Eugeni Dodonov <eugeni.dodonov@intel.com>
+ *
+ */
+
+#include <linux/cpufreq.h>
+#include "i915_drv.h"
+#include "intel_drv.h"
+#include "../../../platform/x86/intel_ips.h"
+#include <linux/module.h>
+
+/* FBC, or Frame Buffer Compression, is a technique employed to compress the
+ * framebuffer contents in-memory, aiming at reducing the required bandwidth
+ * during in-memory transfers and, therefore, reduce the power packet.
+ *
+ * The benefits of FBC are mostly visible with solid backgrounds and
+ * variation-less patterns.
+ *
+ * FBC-related functionality can be enabled by the means of the
+ * i915.i915_enable_fbc parameter
+ */
+
+static void i8xx_disable_fbc(struct drm_device *dev)
+{
+	struct drm_i915_private *dev_priv = dev->dev_private;
+	u32 fbc_ctl;
+
+	/* Disable compression */
+	fbc_ctl = I915_READ(FBC_CONTROL);
+	if ((fbc_ctl & FBC_CTL_EN) == 0)
+		return;
+
+	fbc_ctl &= ~FBC_CTL_EN;
+	I915_WRITE(FBC_CONTROL, fbc_ctl);
+
+	/* Wait for compressing bit to clear */
+	if (wait_for((I915_READ(FBC_STATUS) & FBC_STAT_COMPRESSING) == 0, 10)) {
+		DRM_DEBUG_KMS("FBC idle timed out\n");
+		return;
+	}
+
+	DRM_DEBUG_KMS("disabled FBC\n");
+}
+
+static void i8xx_enable_fbc(struct drm_crtc *crtc, unsigned long interval)
+{
+	struct drm_device *dev = crtc->dev;
+	struct drm_i915_private *dev_priv = dev->dev_private;
+	struct drm_framebuffer *fb = crtc->fb;
+	struct intel_framebuffer *intel_fb = to_intel_framebuffer(fb);
+	struct drm_i915_gem_object *obj = intel_fb->obj;
+	struct intel_crtc *intel_crtc = to_intel_crtc(crtc);
+	int cfb_pitch;
+	int plane, i;
+	u32 fbc_ctl, fbc_ctl2;
+
+	cfb_pitch = dev_priv->cfb_size / FBC_LL_SIZE;
+	if (fb->pitches[0] < cfb_pitch)
+		cfb_pitch = fb->pitches[0];
+
+	/* FBC_CTL wants 64B units */
+	cfb_pitch = (cfb_pitch / 64) - 1;
+	plane = intel_crtc->plane == 0 ? FBC_CTL_PLANEA : FBC_CTL_PLANEB;
+
+	/* Clear old tags */
+	for (i = 0; i < (FBC_LL_SIZE / 32) + 1; i++)
+		I915_WRITE(FBC_TAG + (i * 4), 0);
+
+	/* Set it up... */
+	fbc_ctl2 = FBC_CTL_FENCE_DBL | FBC_CTL_IDLE_IMM | FBC_CTL_CPU_FENCE;
+	fbc_ctl2 |= plane;
+	I915_WRITE(FBC_CONTROL2, fbc_ctl2);
+	I915_WRITE(FBC_FENCE_OFF, crtc->y);
+
+	/* enable it... */
+	fbc_ctl = FBC_CTL_EN | FBC_CTL_PERIODIC;
+	if (IS_I945GM(dev))
+		fbc_ctl |= FBC_CTL_C3_IDLE; /* 945 needs special SR handling */
+	fbc_ctl |= (cfb_pitch & 0xff) << FBC_CTL_STRIDE_SHIFT;
+	fbc_ctl |= (interval & 0x2fff) << FBC_CTL_INTERVAL_SHIFT;
+	fbc_ctl |= obj->fence_reg;
+	I915_WRITE(FBC_CONTROL, fbc_ctl);
+
+	DRM_DEBUG_KMS("enabled FBC, pitch %d, yoff %d, plane %d, ",
+		      cfb_pitch, crtc->y, intel_crtc->plane);
+}
+
+static bool i8xx_fbc_enabled(struct drm_device *dev)
+{
+	struct drm_i915_private *dev_priv = dev->dev_private;
+
+	return I915_READ(FBC_CONTROL) & FBC_CTL_EN;
+}
+
+static void g4x_enable_fbc(struct drm_crtc *crtc, unsigned long interval)
+{
+	struct drm_device *dev = crtc->dev;
+	struct drm_i915_private *dev_priv = dev->dev_private;
+	struct drm_framebuffer *fb = crtc->fb;
+	struct intel_framebuffer *intel_fb = to_intel_framebuffer(fb);
+	struct drm_i915_gem_object *obj = intel_fb->obj;
+	struct intel_crtc *intel_crtc = to_intel_crtc(crtc);
+	int plane = intel_crtc->plane == 0 ? DPFC_CTL_PLANEA : DPFC_CTL_PLANEB;
+	unsigned long stall_watermark = 200;
+	u32 dpfc_ctl;
+
+	dpfc_ctl = plane | DPFC_SR_EN | DPFC_CTL_LIMIT_1X;
+	dpfc_ctl |= DPFC_CTL_FENCE_EN | obj->fence_reg;
+	I915_WRITE(DPFC_CHICKEN, DPFC_HT_MODIFY);
+
+	I915_WRITE(DPFC_RECOMP_CTL, DPFC_RECOMP_STALL_EN |
+		   (stall_watermark << DPFC_RECOMP_STALL_WM_SHIFT) |
+		   (interval << DPFC_RECOMP_TIMER_COUNT_SHIFT));
+	I915_WRITE(DPFC_FENCE_YOFF, crtc->y);
+
+	/* enable it... */
+	I915_WRITE(DPFC_CONTROL, I915_READ(DPFC_CONTROL) | DPFC_CTL_EN);
+
+	DRM_DEBUG_KMS("enabled fbc on plane %d\n", intel_crtc->plane);
+}
+
+static void g4x_disable_fbc(struct drm_device *dev)
+{
+	struct drm_i915_private *dev_priv = dev->dev_private;
+	u32 dpfc_ctl;
+
+	/* Disable compression */
+	dpfc_ctl = I915_READ(DPFC_CONTROL);
+	if (dpfc_ctl & DPFC_CTL_EN) {
+		dpfc_ctl &= ~DPFC_CTL_EN;
+		I915_WRITE(DPFC_CONTROL, dpfc_ctl);
+
+		DRM_DEBUG_KMS("disabled FBC\n");
+	}
+}
+
+static bool g4x_fbc_enabled(struct drm_device *dev)
+{
+	struct drm_i915_private *dev_priv = dev->dev_private;
+
+	return I915_READ(DPFC_CONTROL) & DPFC_CTL_EN;
+}
+
+static void sandybridge_blit_fbc_update(struct drm_device *dev)
+{
+	struct drm_i915_private *dev_priv = dev->dev_private;
+	u32 blt_ecoskpd;
+
+	/* Make sure blitter notifies FBC of writes */
+	gen6_gt_force_wake_get(dev_priv);
+	blt_ecoskpd = I915_READ(GEN6_BLITTER_ECOSKPD);
+	blt_ecoskpd |= GEN6_BLITTER_FBC_NOTIFY <<
+		GEN6_BLITTER_LOCK_SHIFT;
+	I915_WRITE(GEN6_BLITTER_ECOSKPD, blt_ecoskpd);
+	blt_ecoskpd |= GEN6_BLITTER_FBC_NOTIFY;
+	I915_WRITE(GEN6_BLITTER_ECOSKPD, blt_ecoskpd);
+	blt_ecoskpd &= ~(GEN6_BLITTER_FBC_NOTIFY <<
+			 GEN6_BLITTER_LOCK_SHIFT);
+	I915_WRITE(GEN6_BLITTER_ECOSKPD, blt_ecoskpd);
+	POSTING_READ(GEN6_BLITTER_ECOSKPD);
+	gen6_gt_force_wake_put(dev_priv);
+}
+
+static void ironlake_enable_fbc(struct drm_crtc *crtc, unsigned long interval)
+{
+	struct drm_device *dev = crtc->dev;
+	struct drm_i915_private *dev_priv = dev->dev_private;
+	struct drm_framebuffer *fb = crtc->fb;
+	struct intel_framebuffer *intel_fb = to_intel_framebuffer(fb);
+	struct drm_i915_gem_object *obj = intel_fb->obj;
+	struct intel_crtc *intel_crtc = to_intel_crtc(crtc);
+	int plane = intel_crtc->plane == 0 ? DPFC_CTL_PLANEA : DPFC_CTL_PLANEB;
+	unsigned long stall_watermark = 200;
+	u32 dpfc_ctl;
+
+	dpfc_ctl = I915_READ(ILK_DPFC_CONTROL);
+	dpfc_ctl &= DPFC_RESERVED;
+	dpfc_ctl |= (plane | DPFC_CTL_LIMIT_1X);
+	/* Set persistent mode for front-buffer rendering, ala X. */
+	dpfc_ctl |= DPFC_CTL_PERSISTENT_MODE;
+	dpfc_ctl |= (DPFC_CTL_FENCE_EN | obj->fence_reg);
+	I915_WRITE(ILK_DPFC_CHICKEN, DPFC_HT_MODIFY);
+
+	I915_WRITE(ILK_DPFC_RECOMP_CTL, DPFC_RECOMP_STALL_EN |
+		   (stall_watermark << DPFC_RECOMP_STALL_WM_SHIFT) |
+		   (interval << DPFC_RECOMP_TIMER_COUNT_SHIFT));
+	I915_WRITE(ILK_DPFC_FENCE_YOFF, crtc->y);
+	I915_WRITE(ILK_FBC_RT_BASE, obj->gtt_offset | ILK_FBC_RT_VALID);
+	/* enable it... */
+	I915_WRITE(ILK_DPFC_CONTROL, dpfc_ctl | DPFC_CTL_EN);
+
+	if (IS_GEN6(dev)) {
+		I915_WRITE(SNB_DPFC_CTL_SA,
+			   SNB_CPU_FENCE_ENABLE | obj->fence_reg);
+		I915_WRITE(DPFC_CPU_FENCE_OFFSET, crtc->y);
+		sandybridge_blit_fbc_update(dev);
+	}
+
+	DRM_DEBUG_KMS("enabled fbc on plane %d\n", intel_crtc->plane);
+}
+
+static void ironlake_disable_fbc(struct drm_device *dev)
+{
+	struct drm_i915_private *dev_priv = dev->dev_private;
+	u32 dpfc_ctl;
+
+	/* Disable compression */
+	dpfc_ctl = I915_READ(ILK_DPFC_CONTROL);
+	if (dpfc_ctl & DPFC_CTL_EN) {
+		dpfc_ctl &= ~DPFC_CTL_EN;
+		I915_WRITE(ILK_DPFC_CONTROL, dpfc_ctl);
+
+		DRM_DEBUG_KMS("disabled FBC\n");
+	}
+}
+
+static bool ironlake_fbc_enabled(struct drm_device *dev)
+{
+	struct drm_i915_private *dev_priv = dev->dev_private;
+
+	return I915_READ(ILK_DPFC_CONTROL) & DPFC_CTL_EN;
+}
+
+bool intel_fbc_enabled(struct drm_device *dev)
+{
+	struct drm_i915_private *dev_priv = dev->dev_private;
+
+	if (!dev_priv->display.fbc_enabled)
+		return false;
+
+	return dev_priv->display.fbc_enabled(dev);
+}
+
+static void intel_fbc_work_fn(struct work_struct *__work)
+{
+	struct intel_fbc_work *work =
+		container_of(to_delayed_work(__work),
+			     struct intel_fbc_work, work);
+	struct drm_device *dev = work->crtc->dev;
+	struct drm_i915_private *dev_priv = dev->dev_private;
+
+	mutex_lock(&dev->struct_mutex);
+	if (work == dev_priv->fbc_work) {
+		/* Double check that we haven't switched fb without cancelling
+		 * the prior work.
+		 */
+		if (work->crtc->fb == work->fb) {
+			dev_priv->display.enable_fbc(work->crtc,
+						     work->interval);
+
+			dev_priv->cfb_plane = to_intel_crtc(work->crtc)->plane;
+			dev_priv->cfb_fb = work->crtc->fb->base.id;
+			dev_priv->cfb_y = work->crtc->y;
+		}
+
+		dev_priv->fbc_work = NULL;
+	}
+	mutex_unlock(&dev->struct_mutex);
+
+	kfree(work);
+}
+
+static void intel_cancel_fbc_work(struct drm_i915_private *dev_priv)
+{
+	if (dev_priv->fbc_work == NULL)
+		return;
+
+	DRM_DEBUG_KMS("cancelling pending FBC enable\n");
+
+	/* Synchronisation is provided by struct_mutex and checking of
+	 * dev_priv->fbc_work, so we can perform the cancellation
+	 * entirely asynchronously.
+	 */
+	if (cancel_delayed_work(&dev_priv->fbc_work->work))
+		/* tasklet was killed before being run, clean up */
+		kfree(dev_priv->fbc_work);
+
+	/* Mark the work as no longer wanted so that if it does
+	 * wake-up (because the work was already running and waiting
+	 * for our mutex), it will discover that is no longer
+	 * necessary to run.
+	 */
+	dev_priv->fbc_work = NULL;
+}
+
+void intel_enable_fbc(struct drm_crtc *crtc, unsigned long interval)
+{
+	struct intel_fbc_work *work;
+	struct drm_device *dev = crtc->dev;
+	struct drm_i915_private *dev_priv = dev->dev_private;
+
+	if (!dev_priv->display.enable_fbc)
+		return;
+
+	intel_cancel_fbc_work(dev_priv);
+
+	work = kzalloc(sizeof *work, GFP_KERNEL);
+	if (work == NULL) {
+		dev_priv->display.enable_fbc(crtc, interval);
+		return;
+	}
+
+	work->crtc = crtc;
+	work->fb = crtc->fb;
+	work->interval = interval;
+	INIT_DELAYED_WORK(&work->work, intel_fbc_work_fn);
+
+	dev_priv->fbc_work = work;
+
+	DRM_DEBUG_KMS("scheduling delayed FBC enable\n");
+
+	/* Delay the actual enabling to let pageflipping cease and the
+	 * display to settle before starting the compression. Note that
+	 * this delay also serves a second purpose: it allows for a
+	 * vblank to pass after disabling the FBC before we attempt
+	 * to modify the control registers.
+	 *
+	 * A more complicated solution would involve tracking vblanks
+	 * following the termination of the page-flipping sequence
+	 * and indeed performing the enable as a co-routine and not
+	 * waiting synchronously upon the vblank.
+	 */
+	schedule_delayed_work(&work->work, msecs_to_jiffies(50));
+}
+
+void intel_disable_fbc(struct drm_device *dev)
+{
+	struct drm_i915_private *dev_priv = dev->dev_private;
+
+	intel_cancel_fbc_work(dev_priv);
+
+	if (!dev_priv->display.disable_fbc)
+		return;
+
+	dev_priv->display.disable_fbc(dev);
+	dev_priv->cfb_plane = -1;
+}
+
+/**
+ * intel_update_fbc - enable/disable FBC as needed
+ * @dev: the drm_device
+ *
+ * Set up the framebuffer compression hardware at mode set time.  We
+ * enable it if possible:
+ *   - plane A only (on pre-965)
+ *   - no pixel mulitply/line duplication
+ *   - no alpha buffer discard
+ *   - no dual wide
+ *   - framebuffer <= 2048 in width, 1536 in height
+ *
+ * We can't assume that any compression will take place (worst case),
+ * so the compressed buffer has to be the same size as the uncompressed
+ * one.  It also must reside (along with the line length buffer) in
+ * stolen memory.
+ *
+ * We need to enable/disable FBC on a global basis.
+ */
+void intel_update_fbc(struct drm_device *dev)
+{
+	struct drm_i915_private *dev_priv = dev->dev_private;
+	struct drm_crtc *crtc = NULL, *tmp_crtc;
+	struct intel_crtc *intel_crtc;
+	struct drm_framebuffer *fb;
+	struct intel_framebuffer *intel_fb;
+	struct drm_i915_gem_object *obj;
+	int enable_fbc;
+
+	DRM_DEBUG_KMS("\n");
+
+	if (!i915_powersave)
+		return;
+
+	if (!I915_HAS_FBC(dev))
+		return;
+
+	/*
+	 * If FBC is already on, we just have to verify that we can
+	 * keep it that way...
+	 * Need to disable if:
+	 *   - more than one pipe is active
+	 *   - changing FBC params (stride, fence, mode)
+	 *   - new fb is too large to fit in compressed buffer
+	 *   - going to an unsupported config (interlace, pixel multiply, etc.)
+	 */
+	list_for_each_entry(tmp_crtc, &dev->mode_config.crtc_list, head) {
+		if (tmp_crtc->enabled && tmp_crtc->fb) {
+			if (crtc) {
+				DRM_DEBUG_KMS("more than one pipe active, disabling compression\n");
+				dev_priv->no_fbc_reason = FBC_MULTIPLE_PIPES;
+				goto out_disable;
+			}
+			crtc = tmp_crtc;
+		}
+	}
+
+	if (!crtc || crtc->fb == NULL) {
+		DRM_DEBUG_KMS("no output, disabling\n");
+		dev_priv->no_fbc_reason = FBC_NO_OUTPUT;
+		goto out_disable;
+	}
+
+	intel_crtc = to_intel_crtc(crtc);
+	fb = crtc->fb;
+	intel_fb = to_intel_framebuffer(fb);
+	obj = intel_fb->obj;
+
+	enable_fbc = i915_enable_fbc;
+	if (enable_fbc < 0) {
+		DRM_DEBUG_KMS("fbc set to per-chip default\n");
+		enable_fbc = 1;
+		if (INTEL_INFO(dev)->gen <= 6)
+			enable_fbc = 0;
+	}
+	if (!enable_fbc) {
+		DRM_DEBUG_KMS("fbc disabled per module param\n");
+		dev_priv->no_fbc_reason = FBC_MODULE_PARAM;
+		goto out_disable;
+	}
+	if (intel_fb->obj->base.size > dev_priv->cfb_size) {
+		DRM_DEBUG_KMS("framebuffer too large, disabling "
+			      "compression\n");
+		dev_priv->no_fbc_reason = FBC_STOLEN_TOO_SMALL;
+		goto out_disable;
+	}
+	if ((crtc->mode.flags & DRM_MODE_FLAG_INTERLACE) ||
+	    (crtc->mode.flags & DRM_MODE_FLAG_DBLSCAN)) {
+		DRM_DEBUG_KMS("mode incompatible with compression, "
+			      "disabling\n");
+		dev_priv->no_fbc_reason = FBC_UNSUPPORTED_MODE;
+		goto out_disable;
+	}
+	if ((crtc->mode.hdisplay > 2048) ||
+	    (crtc->mode.vdisplay > 1536)) {
+		DRM_DEBUG_KMS("mode too large for compression, disabling\n");
+		dev_priv->no_fbc_reason = FBC_MODE_TOO_LARGE;
+		goto out_disable;
+	}
+	if ((IS_I915GM(dev) || IS_I945GM(dev)) && intel_crtc->plane != 0) {
+		DRM_DEBUG_KMS("plane not 0, disabling compression\n");
+		dev_priv->no_fbc_reason = FBC_BAD_PLANE;
+		goto out_disable;
+	}
+
+	/* The use of a CPU fence is mandatory in order to detect writes
+	 * by the CPU to the scanout and trigger updates to the FBC.
+	 */
+	if (obj->tiling_mode != I915_TILING_X ||
+	    obj->fence_reg == I915_FENCE_REG_NONE) {
+		DRM_DEBUG_KMS("framebuffer not tiled or fenced, disabling compression\n");
+		dev_priv->no_fbc_reason = FBC_NOT_TILED;
+		goto out_disable;
+	}
+
+	/* If the kernel debugger is active, always disable compression */
+	if (in_dbg_master())
+		goto out_disable;
+
+	/* If the scanout has not changed, don't modify the FBC settings.
+	 * Note that we make the fundamental assumption that the fb->obj
+	 * cannot be unpinned (and have its GTT offset and fence revoked)
+	 * without first being decoupled from the scanout and FBC disabled.
+	 */
+	if (dev_priv->cfb_plane == intel_crtc->plane &&
+	    dev_priv->cfb_fb == fb->base.id &&
+	    dev_priv->cfb_y == crtc->y)
+		return;
+
+	if (intel_fbc_enabled(dev)) {
+		/* We update FBC along two paths, after changing fb/crtc
+		 * configuration (modeswitching) and after page-flipping
+		 * finishes. For the latter, we know that not only did
+		 * we disable the FBC at the start of the page-flip
+		 * sequence, but also more than one vblank has passed.
+		 *
+		 * For the former case of modeswitching, it is possible
+		 * to switch between two FBC valid configurations
+		 * instantaneously so we do need to disable the FBC
+		 * before we can modify its control registers. We also
+		 * have to wait for the next vblank for that to take
+		 * effect. However, since we delay enabling FBC we can
+		 * assume that a vblank has passed since disabling and
+		 * that we can safely alter the registers in the deferred
+		 * callback.
+		 *
+		 * In the scenario that we go from a valid to invalid
+		 * and then back to valid FBC configuration we have
+		 * no strict enforcement that a vblank occurred since
+		 * disabling the FBC. However, along all current pipe
+		 * disabling paths we do need to wait for a vblank at
+		 * some point. And we wait before enabling FBC anyway.
+		 */
+		DRM_DEBUG_KMS("disabling active FBC for update\n");
+		intel_disable_fbc(dev);
+	}
+
+	intel_enable_fbc(crtc, 500);
+	return;
+
+out_disable:
+	/* Multiple disables should be harmless */
+	if (intel_fbc_enabled(dev)) {
+		DRM_DEBUG_KMS("unsupported config, disabling FBC\n");
+		intel_disable_fbc(dev);
+	}
+}
+
+static void i915_pineview_get_mem_freq(struct drm_device *dev)
+{
+	drm_i915_private_t *dev_priv = dev->dev_private;
+	u32 tmp;
+
+	tmp = I915_READ(CLKCFG);
+
+	switch (tmp & CLKCFG_FSB_MASK) {
+	case CLKCFG_FSB_533:
+		dev_priv->fsb_freq = 533; /* 133*4 */
+		break;
+	case CLKCFG_FSB_800:
+		dev_priv->fsb_freq = 800; /* 200*4 */
+		break;
+	case CLKCFG_FSB_667:
+		dev_priv->fsb_freq =  667; /* 167*4 */
+		break;
+	case CLKCFG_FSB_400:
+		dev_priv->fsb_freq = 400; /* 100*4 */
+		break;
+	}
+
+	switch (tmp & CLKCFG_MEM_MASK) {
+	case CLKCFG_MEM_533:
+		dev_priv->mem_freq = 533;
+		break;
+	case CLKCFG_MEM_667:
+		dev_priv->mem_freq = 667;
+		break;
+	case CLKCFG_MEM_800:
+		dev_priv->mem_freq = 800;
+		break;
+	}
+
+	/* detect pineview DDR3 setting */
+	tmp = I915_READ(CSHRDDR3CTL);
+	dev_priv->is_ddr3 = (tmp & CSHRDDR3CTL_DDR3) ? 1 : 0;
+}
+
+static void i915_ironlake_get_mem_freq(struct drm_device *dev)
+{
+	drm_i915_private_t *dev_priv = dev->dev_private;
+	u16 ddrpll, csipll;
+
+	ddrpll = I915_READ16(DDRMPLL1);
+	csipll = I915_READ16(CSIPLL0);
+
+	switch (ddrpll & 0xff) {
+	case 0xc:
+		dev_priv->mem_freq = 800;
+		break;
+	case 0x10:
+		dev_priv->mem_freq = 1066;
+		break;
+	case 0x14:
+		dev_priv->mem_freq = 1333;
+		break;
+	case 0x18:
+		dev_priv->mem_freq = 1600;
+		break;
+	default:
+		DRM_DEBUG_DRIVER("unknown memory frequency 0x%02x\n",
+				 ddrpll & 0xff);
+		dev_priv->mem_freq = 0;
+		break;
+	}
+
+	dev_priv->r_t = dev_priv->mem_freq;
+
+	switch (csipll & 0x3ff) {
+	case 0x00c:
+		dev_priv->fsb_freq = 3200;
+		break;
+	case 0x00e:
+		dev_priv->fsb_freq = 3733;
+		break;
+	case 0x010:
+		dev_priv->fsb_freq = 4266;
+		break;
+	case 0x012:
+		dev_priv->fsb_freq = 4800;
+		break;
+	case 0x014:
+		dev_priv->fsb_freq = 5333;
+		break;
+	case 0x016:
+		dev_priv->fsb_freq = 5866;
+		break;
+	case 0x018:
+		dev_priv->fsb_freq = 6400;
+		break;
+	default:
+		DRM_DEBUG_DRIVER("unknown fsb frequency 0x%04x\n",
+				 csipll & 0x3ff);
+		dev_priv->fsb_freq = 0;
+		break;
+	}
+
+	if (dev_priv->fsb_freq == 3200) {
+		dev_priv->c_m = 0;
+	} else if (dev_priv->fsb_freq > 3200 && dev_priv->fsb_freq <= 4800) {
+		dev_priv->c_m = 1;
+	} else {
+		dev_priv->c_m = 2;
+	}
+}
+
+static const struct cxsr_latency cxsr_latency_table[] = {
+	{1, 0, 800, 400, 3382, 33382, 3983, 33983},    /* DDR2-400 SC */
+	{1, 0, 800, 667, 3354, 33354, 3807, 33807},    /* DDR2-667 SC */
+	{1, 0, 800, 800, 3347, 33347, 3763, 33763},    /* DDR2-800 SC */
+	{1, 1, 800, 667, 6420, 36420, 6873, 36873},    /* DDR3-667 SC */
+	{1, 1, 800, 800, 5902, 35902, 6318, 36318},    /* DDR3-800 SC */
+
+	{1, 0, 667, 400, 3400, 33400, 4021, 34021},    /* DDR2-400 SC */
+	{1, 0, 667, 667, 3372, 33372, 3845, 33845},    /* DDR2-667 SC */
+	{1, 0, 667, 800, 3386, 33386, 3822, 33822},    /* DDR2-800 SC */
+	{1, 1, 667, 667, 6438, 36438, 6911, 36911},    /* DDR3-667 SC */
+	{1, 1, 667, 800, 5941, 35941, 6377, 36377},    /* DDR3-800 SC */
+
+	{1, 0, 400, 400, 3472, 33472, 4173, 34173},    /* DDR2-400 SC */
+	{1, 0, 400, 667, 3443, 33443, 3996, 33996},    /* DDR2-667 SC */
+	{1, 0, 400, 800, 3430, 33430, 3946, 33946},    /* DDR2-800 SC */
+	{1, 1, 400, 667, 6509, 36509, 7062, 37062},    /* DDR3-667 SC */
+	{1, 1, 400, 800, 5985, 35985, 6501, 36501},    /* DDR3-800 SC */
+
+	{0, 0, 800, 400, 3438, 33438, 4065, 34065},    /* DDR2-400 SC */
+	{0, 0, 800, 667, 3410, 33410, 3889, 33889},    /* DDR2-667 SC */
+	{0, 0, 800, 800, 3403, 33403, 3845, 33845},    /* DDR2-800 SC */
+	{0, 1, 800, 667, 6476, 36476, 6955, 36955},    /* DDR3-667 SC */
+	{0, 1, 800, 800, 5958, 35958, 6400, 36400},    /* DDR3-800 SC */
+
+	{0, 0, 667, 400, 3456, 33456, 4103, 34106},    /* DDR2-400 SC */
+	{0, 0, 667, 667, 3428, 33428, 3927, 33927},    /* DDR2-667 SC */
+	{0, 0, 667, 800, 3443, 33443, 3905, 33905},    /* DDR2-800 SC */
+	{0, 1, 667, 667, 6494, 36494, 6993, 36993},    /* DDR3-667 SC */
+	{0, 1, 667, 800, 5998, 35998, 6460, 36460},    /* DDR3-800 SC */
+
+	{0, 0, 400, 400, 3528, 33528, 4255, 34255},    /* DDR2-400 SC */
+	{0, 0, 400, 667, 3500, 33500, 4079, 34079},    /* DDR2-667 SC */
+	{0, 0, 400, 800, 3487, 33487, 4029, 34029},    /* DDR2-800 SC */
+	{0, 1, 400, 667, 6566, 36566, 7145, 37145},    /* DDR3-667 SC */
+	{0, 1, 400, 800, 6042, 36042, 6584, 36584},    /* DDR3-800 SC */
+};
+
+static const struct cxsr_latency *intel_get_cxsr_latency(int is_desktop,
+							 int is_ddr3,
+							 int fsb,
+							 int mem)
+{
+	const struct cxsr_latency *latency;
+	int i;
+
+	if (fsb == 0 || mem == 0)
+		return NULL;
+
+	for (i = 0; i < ARRAY_SIZE(cxsr_latency_table); i++) {
+		latency = &cxsr_latency_table[i];
+		if (is_desktop == latency->is_desktop &&
+		    is_ddr3 == latency->is_ddr3 &&
+		    fsb == latency->fsb_freq && mem == latency->mem_freq)
+			return latency;
+	}
+
+	DRM_DEBUG_KMS("Unknown FSB/MEM found, disable CxSR\n");
+
+	return NULL;
+}
+
+static void pineview_disable_cxsr(struct drm_device *dev)
+{
+	struct drm_i915_private *dev_priv = dev->dev_private;
+
+	/* deactivate cxsr */
+	I915_WRITE(DSPFW3, I915_READ(DSPFW3) & ~PINEVIEW_SELF_REFRESH_EN);
+}
+
+/*
+ * Latency for FIFO fetches is dependent on several factors:
+ *   - memory configuration (speed, channels)
+ *   - chipset
+ *   - current MCH state
+ * It can be fairly high in some situations, so here we assume a fairly
+ * pessimal value.  It's a tradeoff between extra memory fetches (if we
+ * set this value too high, the FIFO will fetch frequently to stay full)
+ * and power consumption (set it too low to save power and we might see
+ * FIFO underruns and display "flicker").
+ *
+ * A value of 5us seems to be a good balance; safe for very low end
+ * platforms but not overly aggressive on lower latency configs.
+ */
+static const int latency_ns = 5000;
+
+static int i9xx_get_fifo_size(struct drm_device *dev, int plane)
+{
+	struct drm_i915_private *dev_priv = dev->dev_private;
+	uint32_t dsparb = I915_READ(DSPARB);
+	int size;
+
+	size = dsparb & 0x7f;
+	if (plane)
+		size = ((dsparb >> DSPARB_CSTART_SHIFT) & 0x7f) - size;
+
+	DRM_DEBUG_KMS("FIFO size - (0x%08x) %s: %d\n", dsparb,
+		      plane ? "B" : "A", size);
+
+	return size;
+}
+
+static int i85x_get_fifo_size(struct drm_device *dev, int plane)
+{
+	struct drm_i915_private *dev_priv = dev->dev_private;
+	uint32_t dsparb = I915_READ(DSPARB);
+	int size;
+
+	size = dsparb & 0x1ff;
+	if (plane)
+		size = ((dsparb >> DSPARB_BEND_SHIFT) & 0x1ff) - size;
+	size >>= 1; /* Convert to cachelines */
+
+	DRM_DEBUG_KMS("FIFO size - (0x%08x) %s: %d\n", dsparb,
+		      plane ? "B" : "A", size);
+
+	return size;
+}
+
+static int i845_get_fifo_size(struct drm_device *dev, int plane)
+{
+	struct drm_i915_private *dev_priv = dev->dev_private;
+	uint32_t dsparb = I915_READ(DSPARB);
+	int size;
+
+	size = dsparb & 0x7f;
+	size >>= 2; /* Convert to cachelines */
+
+	DRM_DEBUG_KMS("FIFO size - (0x%08x) %s: %d\n", dsparb,
+		      plane ? "B" : "A",
+		      size);
+
+	return size;
+}
+
+static int i830_get_fifo_size(struct drm_device *dev, int plane)
+{
+	struct drm_i915_private *dev_priv = dev->dev_private;
+	uint32_t dsparb = I915_READ(DSPARB);
+	int size;
+
+	size = dsparb & 0x7f;
+	size >>= 1; /* Convert to cachelines */
+
+	DRM_DEBUG_KMS("FIFO size - (0x%08x) %s: %d\n", dsparb,
+		      plane ? "B" : "A", size);
+
+	return size;
+}
+
+/* Pineview has different values for various configs */
+static const struct intel_watermark_params pineview_display_wm = {
+	PINEVIEW_DISPLAY_FIFO,
+	PINEVIEW_MAX_WM,
+	PINEVIEW_DFT_WM,
+	PINEVIEW_GUARD_WM,
+	PINEVIEW_FIFO_LINE_SIZE
+};
+static const struct intel_watermark_params pineview_display_hplloff_wm = {
+	PINEVIEW_DISPLAY_FIFO,
+	PINEVIEW_MAX_WM,
+	PINEVIEW_DFT_HPLLOFF_WM,
+	PINEVIEW_GUARD_WM,
+	PINEVIEW_FIFO_LINE_SIZE
+};
+static const struct intel_watermark_params pineview_cursor_wm = {
+	PINEVIEW_CURSOR_FIFO,
+	PINEVIEW_CURSOR_MAX_WM,
+	PINEVIEW_CURSOR_DFT_WM,
+	PINEVIEW_CURSOR_GUARD_WM,
+	PINEVIEW_FIFO_LINE_SIZE,
+};
+static const struct intel_watermark_params pineview_cursor_hplloff_wm = {
+	PINEVIEW_CURSOR_FIFO,
+	PINEVIEW_CURSOR_MAX_WM,
+	PINEVIEW_CURSOR_DFT_WM,
+	PINEVIEW_CURSOR_GUARD_WM,
+	PINEVIEW_FIFO_LINE_SIZE
+};
+static const struct intel_watermark_params g4x_wm_info = {
+	G4X_FIFO_SIZE,
+	G4X_MAX_WM,
+	G4X_MAX_WM,
+	2,
+	G4X_FIFO_LINE_SIZE,
+};
+static const struct intel_watermark_params g4x_cursor_wm_info = {
+	I965_CURSOR_FIFO,
+	I965_CURSOR_MAX_WM,
+	I965_CURSOR_DFT_WM,
+	2,
+	G4X_FIFO_LINE_SIZE,
+};
+static const struct intel_watermark_params valleyview_wm_info = {
+	VALLEYVIEW_FIFO_SIZE,
+	VALLEYVIEW_MAX_WM,
+	VALLEYVIEW_MAX_WM,
+	2,
+	G4X_FIFO_LINE_SIZE,
+};
+static const struct intel_watermark_params valleyview_cursor_wm_info = {
+	I965_CURSOR_FIFO,
+	VALLEYVIEW_CURSOR_MAX_WM,
+	I965_CURSOR_DFT_WM,
+	2,
+	G4X_FIFO_LINE_SIZE,
+};
+static const struct intel_watermark_params i965_cursor_wm_info = {
+	I965_CURSOR_FIFO,
+	I965_CURSOR_MAX_WM,
+	I965_CURSOR_DFT_WM,
+	2,
+	I915_FIFO_LINE_SIZE,
+};
+static const struct intel_watermark_params i945_wm_info = {
+	I945_FIFO_SIZE,
+	I915_MAX_WM,
+	1,
+	2,
+	I915_FIFO_LINE_SIZE
+};
+static const struct intel_watermark_params i915_wm_info = {
+	I915_FIFO_SIZE,
+	I915_MAX_WM,
+	1,
+	2,
+	I915_FIFO_LINE_SIZE
+};
+static const struct intel_watermark_params i855_wm_info = {
+	I855GM_FIFO_SIZE,
+	I915_MAX_WM,
+	1,
+	2,
+	I830_FIFO_LINE_SIZE
+};
+static const struct intel_watermark_params i830_wm_info = {
+	I830_FIFO_SIZE,
+	I915_MAX_WM,
+	1,
+	2,
+	I830_FIFO_LINE_SIZE
+};
+
+static const struct intel_watermark_params ironlake_display_wm_info = {
+	ILK_DISPLAY_FIFO,
+	ILK_DISPLAY_MAXWM,
+	ILK_DISPLAY_DFTWM,
+	2,
+	ILK_FIFO_LINE_SIZE
+};
+static const struct intel_watermark_params ironlake_cursor_wm_info = {
+	ILK_CURSOR_FIFO,
+	ILK_CURSOR_MAXWM,
+	ILK_CURSOR_DFTWM,
+	2,
+	ILK_FIFO_LINE_SIZE
+};
+static const struct intel_watermark_params ironlake_display_srwm_info = {
+	ILK_DISPLAY_SR_FIFO,
+	ILK_DISPLAY_MAX_SRWM,
+	ILK_DISPLAY_DFT_SRWM,
+	2,
+	ILK_FIFO_LINE_SIZE
+};
+static const struct intel_watermark_params ironlake_cursor_srwm_info = {
+	ILK_CURSOR_SR_FIFO,
+	ILK_CURSOR_MAX_SRWM,
+	ILK_CURSOR_DFT_SRWM,
+	2,
+	ILK_FIFO_LINE_SIZE
+};
+
+static const struct intel_watermark_params sandybridge_display_wm_info = {
+	SNB_DISPLAY_FIFO,
+	SNB_DISPLAY_MAXWM,
+	SNB_DISPLAY_DFTWM,
+	2,
+	SNB_FIFO_LINE_SIZE
+};
+static const struct intel_watermark_params sandybridge_cursor_wm_info = {
+	SNB_CURSOR_FIFO,
+	SNB_CURSOR_MAXWM,
+	SNB_CURSOR_DFTWM,
+	2,
+	SNB_FIFO_LINE_SIZE
+};
+static const struct intel_watermark_params sandybridge_display_srwm_info = {
+	SNB_DISPLAY_SR_FIFO,
+	SNB_DISPLAY_MAX_SRWM,
+	SNB_DISPLAY_DFT_SRWM,
+	2,
+	SNB_FIFO_LINE_SIZE
+};
+static const struct intel_watermark_params sandybridge_cursor_srwm_info = {
+	SNB_CURSOR_SR_FIFO,
+	SNB_CURSOR_MAX_SRWM,
+	SNB_CURSOR_DFT_SRWM,
+	2,
+	SNB_FIFO_LINE_SIZE
+};
+
+
+/**
+ * intel_calculate_wm - calculate watermark level
+ * @clock_in_khz: pixel clock
+ * @wm: chip FIFO params
+ * @pixel_size: display pixel size
+ * @latency_ns: memory latency for the platform
+ *
+ * Calculate the watermark level (the level at which the display plane will
+ * start fetching from memory again).  Each chip has a different display
+ * FIFO size and allocation, so the caller needs to figure that out and pass
+ * in the correct intel_watermark_params structure.
+ *
+ * As the pixel clock runs, the FIFO will be drained at a rate that depends
+ * on the pixel size.  When it reaches the watermark level, it'll start
+ * fetching FIFO line sized based chunks from memory until the FIFO fills
+ * past the watermark point.  If the FIFO drains completely, a FIFO underrun
+ * will occur, and a display engine hang could result.
+ */
+static unsigned long intel_calculate_wm(unsigned long clock_in_khz,
+					const struct intel_watermark_params *wm,
+					int fifo_size,
+					int pixel_size,
+					unsigned long latency_ns)
+{
+	long entries_required, wm_size;
+
+	/*
+	 * Note: we need to make sure we don't overflow for various clock &
+	 * latency values.
+	 * clocks go from a few thousand to several hundred thousand.
+	 * latency is usually a few thousand
+	 */
+	entries_required = ((clock_in_khz / 1000) * pixel_size * latency_ns) /
+		1000;
+	entries_required = DIV_ROUND_UP(entries_required, wm->cacheline_size);
+
+	DRM_DEBUG_KMS("FIFO entries required for mode: %ld\n", entries_required);
+
+	wm_size = fifo_size - (entries_required + wm->guard_size);
+
+	DRM_DEBUG_KMS("FIFO watermark level: %ld\n", wm_size);
+
+	/* Don't promote wm_size to unsigned... */
+	if (wm_size > (long)wm->max_wm)
+		wm_size = wm->max_wm;
+	if (wm_size <= 0)
+		wm_size = wm->default_wm;
+	return wm_size;
+}
+
+static struct drm_crtc *single_enabled_crtc(struct drm_device *dev)
+{
+	struct drm_crtc *crtc, *enabled = NULL;
+
+	list_for_each_entry(crtc, &dev->mode_config.crtc_list, head) {
+		if (crtc->enabled && crtc->fb) {
+			if (enabled)
+				return NULL;
+			enabled = crtc;
+		}
+	}
+
+	return enabled;
+}
+
+static void pineview_update_wm(struct drm_device *dev)
+{
+	struct drm_i915_private *dev_priv = dev->dev_private;
+	struct drm_crtc *crtc;
+	const struct cxsr_latency *latency;
+	u32 reg;
+	unsigned long wm;
+
+	latency = intel_get_cxsr_latency(IS_PINEVIEW_G(dev), dev_priv->is_ddr3,
+					 dev_priv->fsb_freq, dev_priv->mem_freq);
+	if (!latency) {
+		DRM_DEBUG_KMS("Unknown FSB/MEM found, disable CxSR\n");
+		pineview_disable_cxsr(dev);
+		return;
+	}
+
+	crtc = single_enabled_crtc(dev);
+	if (crtc) {
+		int clock = crtc->mode.clock;
+		int pixel_size = crtc->fb->bits_per_pixel / 8;
+
+		/* Display SR */
+		wm = intel_calculate_wm(clock, &pineview_display_wm,
+					pineview_display_wm.fifo_size,
+					pixel_size, latency->display_sr);
+		reg = I915_READ(DSPFW1);
+		reg &= ~DSPFW_SR_MASK;
+		reg |= wm << DSPFW_SR_SHIFT;
+		I915_WRITE(DSPFW1, reg);
+		DRM_DEBUG_KMS("DSPFW1 register is %x\n", reg);
+
+		/* cursor SR */
+		wm = intel_calculate_wm(clock, &pineview_cursor_wm,
+					pineview_display_wm.fifo_size,
+					pixel_size, latency->cursor_sr);
+		reg = I915_READ(DSPFW3);
+		reg &= ~DSPFW_CURSOR_SR_MASK;
+		reg |= (wm & 0x3f) << DSPFW_CURSOR_SR_SHIFT;
+		I915_WRITE(DSPFW3, reg);
+
+		/* Display HPLL off SR */
+		wm = intel_calculate_wm(clock, &pineview_display_hplloff_wm,
+					pineview_display_hplloff_wm.fifo_size,
+					pixel_size, latency->display_hpll_disable);
+		reg = I915_READ(DSPFW3);
+		reg &= ~DSPFW_HPLL_SR_MASK;
+		reg |= wm & DSPFW_HPLL_SR_MASK;
+		I915_WRITE(DSPFW3, reg);
+
+		/* cursor HPLL off SR */
+		wm = intel_calculate_wm(clock, &pineview_cursor_hplloff_wm,
+					pineview_display_hplloff_wm.fifo_size,
+					pixel_size, latency->cursor_hpll_disable);
+		reg = I915_READ(DSPFW3);
+		reg &= ~DSPFW_HPLL_CURSOR_MASK;
+		reg |= (wm & 0x3f) << DSPFW_HPLL_CURSOR_SHIFT;
+		I915_WRITE(DSPFW3, reg);
+		DRM_DEBUG_KMS("DSPFW3 register is %x\n", reg);
+
+		/* activate cxsr */
+		I915_WRITE(DSPFW3,
+			   I915_READ(DSPFW3) | PINEVIEW_SELF_REFRESH_EN);
+		DRM_DEBUG_KMS("Self-refresh is enabled\n");
+	} else {
+		pineview_disable_cxsr(dev);
+		DRM_DEBUG_KMS("Self-refresh is disabled\n");
+	}
+}
+
+static bool g4x_compute_wm0(struct drm_device *dev,
+			    int plane,
+			    const struct intel_watermark_params *display,
+			    int display_latency_ns,
+			    const struct intel_watermark_params *cursor,
+			    int cursor_latency_ns,
+			    int *plane_wm,
+			    int *cursor_wm)
+{
+	struct drm_crtc *crtc;
+	int htotal, hdisplay, clock, pixel_size;
+	int line_time_us, line_count;
+	int entries, tlb_miss;
+
+	crtc = intel_get_crtc_for_plane(dev, plane);
+	if (crtc->fb == NULL || !crtc->enabled) {
+		*cursor_wm = cursor->guard_size;
+		*plane_wm = display->guard_size;
+		return false;
+	}
+
+	htotal = crtc->mode.htotal;
+	hdisplay = crtc->mode.hdisplay;
+	clock = crtc->mode.clock;
+	pixel_size = crtc->fb->bits_per_pixel / 8;
+
+	/* Use the small buffer method to calculate plane watermark */
+	entries = ((clock * pixel_size / 1000) * display_latency_ns) / 1000;
+	tlb_miss = display->fifo_size*display->cacheline_size - hdisplay * 8;
+	if (tlb_miss > 0)
+		entries += tlb_miss;
+	entries = DIV_ROUND_UP(entries, display->cacheline_size);
+	*plane_wm = entries + display->guard_size;
+	if (*plane_wm > (int)display->max_wm)
+		*plane_wm = display->max_wm;
+
+	/* Use the large buffer method to calculate cursor watermark */
+	line_time_us = ((htotal * 1000) / clock);
+	line_count = (cursor_latency_ns / line_time_us + 1000) / 1000;
+	entries = line_count * 64 * pixel_size;
+	tlb_miss = cursor->fifo_size*cursor->cacheline_size - hdisplay * 8;
+	if (tlb_miss > 0)
+		entries += tlb_miss;
+	entries = DIV_ROUND_UP(entries, cursor->cacheline_size);
+	*cursor_wm = entries + cursor->guard_size;
+	if (*cursor_wm > (int)cursor->max_wm)
+		*cursor_wm = (int)cursor->max_wm;
+
+	return true;
+}
+
+/*
+ * Check the wm result.
+ *
+ * If any calculated watermark values is larger than the maximum value that
+ * can be programmed into the associated watermark register, that watermark
+ * must be disabled.
+ */
+static bool g4x_check_srwm(struct drm_device *dev,
+			   int display_wm, int cursor_wm,
+			   const struct intel_watermark_params *display,
+			   const struct intel_watermark_params *cursor)
+{
+	DRM_DEBUG_KMS("SR watermark: display plane %d, cursor %d\n",
+		      display_wm, cursor_wm);
+
+	if (display_wm > display->max_wm) {
+		DRM_DEBUG_KMS("display watermark is too large(%d/%ld), disabling\n",
+			      display_wm, display->max_wm);
+		return false;
+	}
+
+	if (cursor_wm > cursor->max_wm) {
+		DRM_DEBUG_KMS("cursor watermark is too large(%d/%ld), disabling\n",
+			      cursor_wm, cursor->max_wm);
+		return false;
+	}
+
+	if (!(display_wm || cursor_wm)) {
+		DRM_DEBUG_KMS("SR latency is 0, disabling\n");
+		return false;
+	}
+
+	return true;
+}
+
+static bool g4x_compute_srwm(struct drm_device *dev,
+			     int plane,
+			     int latency_ns,
+			     const struct intel_watermark_params *display,
+			     const struct intel_watermark_params *cursor,
+			     int *display_wm, int *cursor_wm)
+{
+	struct drm_crtc *crtc;
+	int hdisplay, htotal, pixel_size, clock;
+	unsigned long line_time_us;
+	int line_count, line_size;
+	int small, large;
+	int entries;
+
+	if (!latency_ns) {
+		*display_wm = *cursor_wm = 0;
+		return false;
+	}
+
+	crtc = intel_get_crtc_for_plane(dev, plane);
+	hdisplay = crtc->mode.hdisplay;
+	htotal = crtc->mode.htotal;
+	clock = crtc->mode.clock;
+	pixel_size = crtc->fb->bits_per_pixel / 8;
+
+	line_time_us = (htotal * 1000) / clock;
+	line_count = (latency_ns / line_time_us + 1000) / 1000;
+	line_size = hdisplay * pixel_size;
+
+	/* Use the minimum of the small and large buffer method for primary */
+	small = ((clock * pixel_size / 1000) * latency_ns) / 1000;
+	large = line_count * line_size;
+
+	entries = DIV_ROUND_UP(min(small, large), display->cacheline_size);
+	*display_wm = entries + display->guard_size;
+
+	/* calculate the self-refresh watermark for display cursor */
+	entries = line_count * pixel_size * 64;
+	entries = DIV_ROUND_UP(entries, cursor->cacheline_size);
+	*cursor_wm = entries + cursor->guard_size;
+
+	return g4x_check_srwm(dev,
+			      *display_wm, *cursor_wm,
+			      display, cursor);
+}
+
+static bool vlv_compute_drain_latency(struct drm_device *dev,
+				     int plane,
+				     int *plane_prec_mult,
+				     int *plane_dl,
+				     int *cursor_prec_mult,
+				     int *cursor_dl)
+{
+	struct drm_crtc *crtc;
+	int clock, pixel_size;
+	int entries;
+
+	crtc = intel_get_crtc_for_plane(dev, plane);
+	if (crtc->fb == NULL || !crtc->enabled)
+		return false;
+
+	clock = crtc->mode.clock;	/* VESA DOT Clock */
+	pixel_size = crtc->fb->bits_per_pixel / 8;	/* BPP */
+
+	entries = (clock / 1000) * pixel_size;
+	*plane_prec_mult = (entries > 256) ?
+		DRAIN_LATENCY_PRECISION_32 : DRAIN_LATENCY_PRECISION_16;
+	*plane_dl = (64 * (*plane_prec_mult) * 4) / ((clock / 1000) *
+						     pixel_size);
+
+	entries = (clock / 1000) * 4;	/* BPP is always 4 for cursor */
+	*cursor_prec_mult = (entries > 256) ?
+		DRAIN_LATENCY_PRECISION_32 : DRAIN_LATENCY_PRECISION_16;
+	*cursor_dl = (64 * (*cursor_prec_mult) * 4) / ((clock / 1000) * 4);
+
+	return true;
+}
+
+/*
+ * Update drain latency registers of memory arbiter
+ *
+ * Valleyview SoC has a new memory arbiter and needs drain latency registers
+ * to be programmed. Each plane has a drain latency multiplier and a drain
+ * latency value.
+ */
+
+static void vlv_update_drain_latency(struct drm_device *dev)
+{
+	struct drm_i915_private *dev_priv = dev->dev_private;
+	int planea_prec, planea_dl, planeb_prec, planeb_dl;
+	int cursora_prec, cursora_dl, cursorb_prec, cursorb_dl;
+	int plane_prec_mult, cursor_prec_mult; /* Precision multiplier is
+							either 16 or 32 */
+
+	/* For plane A, Cursor A */
+	if (vlv_compute_drain_latency(dev, 0, &plane_prec_mult, &planea_dl,
+				      &cursor_prec_mult, &cursora_dl)) {
+		cursora_prec = (cursor_prec_mult == DRAIN_LATENCY_PRECISION_32) ?
+			DDL_CURSORA_PRECISION_32 : DDL_CURSORA_PRECISION_16;
+		planea_prec = (plane_prec_mult == DRAIN_LATENCY_PRECISION_32) ?
+			DDL_PLANEA_PRECISION_32 : DDL_PLANEA_PRECISION_16;
+
+		I915_WRITE(VLV_DDL1, cursora_prec |
+				(cursora_dl << DDL_CURSORA_SHIFT) |
+				planea_prec | planea_dl);
+	}
+
+	/* For plane B, Cursor B */
+	if (vlv_compute_drain_latency(dev, 1, &plane_prec_mult, &planeb_dl,
+				      &cursor_prec_mult, &cursorb_dl)) {
+		cursorb_prec = (cursor_prec_mult == DRAIN_LATENCY_PRECISION_32) ?
+			DDL_CURSORB_PRECISION_32 : DDL_CURSORB_PRECISION_16;
+		planeb_prec = (plane_prec_mult == DRAIN_LATENCY_PRECISION_32) ?
+			DDL_PLANEB_PRECISION_32 : DDL_PLANEB_PRECISION_16;
+
+		I915_WRITE(VLV_DDL2, cursorb_prec |
+				(cursorb_dl << DDL_CURSORB_SHIFT) |
+				planeb_prec | planeb_dl);
+	}
+}
+
+#define single_plane_enabled(mask) is_power_of_2(mask)
+
+static void valleyview_update_wm(struct drm_device *dev)
+{
+	static const int sr_latency_ns = 12000;
+	struct drm_i915_private *dev_priv = dev->dev_private;
+	int planea_wm, planeb_wm, cursora_wm, cursorb_wm;
+	int plane_sr, cursor_sr;
+	unsigned int enabled = 0;
+
+	vlv_update_drain_latency(dev);
+
+	if (g4x_compute_wm0(dev, 0,
+			    &valleyview_wm_info, latency_ns,
+			    &valleyview_cursor_wm_info, latency_ns,
+			    &planea_wm, &cursora_wm))
+		enabled |= 1;
+
+	if (g4x_compute_wm0(dev, 1,
+			    &valleyview_wm_info, latency_ns,
+			    &valleyview_cursor_wm_info, latency_ns,
+			    &planeb_wm, &cursorb_wm))
+		enabled |= 2;
+
+	plane_sr = cursor_sr = 0;
+	if (single_plane_enabled(enabled) &&
+	    g4x_compute_srwm(dev, ffs(enabled) - 1,
+			     sr_latency_ns,
+			     &valleyview_wm_info,
+			     &valleyview_cursor_wm_info,
+			     &plane_sr, &cursor_sr))
+		I915_WRITE(FW_BLC_SELF_VLV, FW_CSPWRDWNEN);
+	else
+		I915_WRITE(FW_BLC_SELF_VLV,
+			   I915_READ(FW_BLC_SELF_VLV) & ~FW_CSPWRDWNEN);
+
+	DRM_DEBUG_KMS("Setting FIFO watermarks - A: plane=%d, cursor=%d, B: plane=%d, cursor=%d, SR: plane=%d, cursor=%d\n",
+		      planea_wm, cursora_wm,
+		      planeb_wm, cursorb_wm,
+		      plane_sr, cursor_sr);
+
+	I915_WRITE(DSPFW1,
+		   (plane_sr << DSPFW_SR_SHIFT) |
+		   (cursorb_wm << DSPFW_CURSORB_SHIFT) |
+		   (planeb_wm << DSPFW_PLANEB_SHIFT) |
+		   planea_wm);
+	I915_WRITE(DSPFW2,
+		   (I915_READ(DSPFW2) & DSPFW_CURSORA_MASK) |
+		   (cursora_wm << DSPFW_CURSORA_SHIFT));
+	I915_WRITE(DSPFW3,
+		   (I915_READ(DSPFW3) | (cursor_sr << DSPFW_CURSOR_SR_SHIFT)));
+}
+
+static void g4x_update_wm(struct drm_device *dev)
+{
+	static const int sr_latency_ns = 12000;
+	struct drm_i915_private *dev_priv = dev->dev_private;
+	int planea_wm, planeb_wm, cursora_wm, cursorb_wm;
+	int plane_sr, cursor_sr;
+	unsigned int enabled = 0;
+
+	if (g4x_compute_wm0(dev, 0,
+			    &g4x_wm_info, latency_ns,
+			    &g4x_cursor_wm_info, latency_ns,
+			    &planea_wm, &cursora_wm))
+		enabled |= 1;
+
+	if (g4x_compute_wm0(dev, 1,
+			    &g4x_wm_info, latency_ns,
+			    &g4x_cursor_wm_info, latency_ns,
+			    &planeb_wm, &cursorb_wm))
+		enabled |= 2;
+
+	plane_sr = cursor_sr = 0;
+	if (single_plane_enabled(enabled) &&
+	    g4x_compute_srwm(dev, ffs(enabled) - 1,
+			     sr_latency_ns,
+			     &g4x_wm_info,
+			     &g4x_cursor_wm_info,
+			     &plane_sr, &cursor_sr))
+		I915_WRITE(FW_BLC_SELF, FW_BLC_SELF_EN);
+	else
+		I915_WRITE(FW_BLC_SELF,
+			   I915_READ(FW_BLC_SELF) & ~FW_BLC_SELF_EN);
+
+	DRM_DEBUG_KMS("Setting FIFO watermarks - A: plane=%d, cursor=%d, B: plane=%d, cursor=%d, SR: plane=%d, cursor=%d\n",
+		      planea_wm, cursora_wm,
+		      planeb_wm, cursorb_wm,
+		      plane_sr, cursor_sr);
+
+	I915_WRITE(DSPFW1,
+		   (plane_sr << DSPFW_SR_SHIFT) |
+		   (cursorb_wm << DSPFW_CURSORB_SHIFT) |
+		   (planeb_wm << DSPFW_PLANEB_SHIFT) |
+		   planea_wm);
+	I915_WRITE(DSPFW2,
+		   (I915_READ(DSPFW2) & DSPFW_CURSORA_MASK) |
+		   (cursora_wm << DSPFW_CURSORA_SHIFT));
+	/* HPLL off in SR has some issues on G4x... disable it */
+	I915_WRITE(DSPFW3,
+		   (I915_READ(DSPFW3) & ~DSPFW_HPLL_SR_EN) |
+		   (cursor_sr << DSPFW_CURSOR_SR_SHIFT));
+}
+
+static void i965_update_wm(struct drm_device *dev)
+{
+	struct drm_i915_private *dev_priv = dev->dev_private;
+	struct drm_crtc *crtc;
+	int srwm = 1;
+	int cursor_sr = 16;
+
+	/* Calc sr entries for one plane configs */
+	crtc = single_enabled_crtc(dev);
+	if (crtc) {
+		/* self-refresh has much higher latency */
+		static const int sr_latency_ns = 12000;
+		int clock = crtc->mode.clock;
+		int htotal = crtc->mode.htotal;
+		int hdisplay = crtc->mode.hdisplay;
+		int pixel_size = crtc->fb->bits_per_pixel / 8;
+		unsigned long line_time_us;
+		int entries;
+
+		line_time_us = ((htotal * 1000) / clock);
+
+		/* Use ns/us then divide to preserve precision */
+		entries = (((sr_latency_ns / line_time_us) + 1000) / 1000) *
+			pixel_size * hdisplay;
+		entries = DIV_ROUND_UP(entries, I915_FIFO_LINE_SIZE);
+		srwm = I965_FIFO_SIZE - entries;
+		if (srwm < 0)
+			srwm = 1;
+		srwm &= 0x1ff;
+		DRM_DEBUG_KMS("self-refresh entries: %d, wm: %d\n",
+			      entries, srwm);
+
+		entries = (((sr_latency_ns / line_time_us) + 1000) / 1000) *
+			pixel_size * 64;
+		entries = DIV_ROUND_UP(entries,
+					  i965_cursor_wm_info.cacheline_size);
+		cursor_sr = i965_cursor_wm_info.fifo_size -
+			(entries + i965_cursor_wm_info.guard_size);
+
+		if (cursor_sr > i965_cursor_wm_info.max_wm)
+			cursor_sr = i965_cursor_wm_info.max_wm;
+
+		DRM_DEBUG_KMS("self-refresh watermark: display plane %d "
+			      "cursor %d\n", srwm, cursor_sr);
+
+		if (IS_CRESTLINE(dev))
+			I915_WRITE(FW_BLC_SELF, FW_BLC_SELF_EN);
+	} else {
+		/* Turn off self refresh if both pipes are enabled */
+		if (IS_CRESTLINE(dev))
+			I915_WRITE(FW_BLC_SELF, I915_READ(FW_BLC_SELF)
+				   & ~FW_BLC_SELF_EN);
+	}
+
+	DRM_DEBUG_KMS("Setting FIFO watermarks - A: 8, B: 8, C: 8, SR %d\n",
+		      srwm);
+
+	/* 965 has limitations... */
+	I915_WRITE(DSPFW1, (srwm << DSPFW_SR_SHIFT) |
+		   (8 << 16) | (8 << 8) | (8 << 0));
+	I915_WRITE(DSPFW2, (8 << 8) | (8 << 0));
+	/* update cursor SR watermark */
+	I915_WRITE(DSPFW3, (cursor_sr << DSPFW_CURSOR_SR_SHIFT));
+}
+
+static void i9xx_update_wm(struct drm_device *dev)
+{
+	struct drm_i915_private *dev_priv = dev->dev_private;
+	const struct intel_watermark_params *wm_info;
+	uint32_t fwater_lo;
+	uint32_t fwater_hi;
+	int cwm, srwm = 1;
+	int fifo_size;
+	int planea_wm, planeb_wm;
+	struct drm_crtc *crtc, *enabled = NULL;
+
+	if (IS_I945GM(dev))
+		wm_info = &i945_wm_info;
+	else if (!IS_GEN2(dev))
+		wm_info = &i915_wm_info;
+	else
+		wm_info = &i855_wm_info;
+
+	fifo_size = dev_priv->display.get_fifo_size(dev, 0);
+	crtc = intel_get_crtc_for_plane(dev, 0);
+	if (crtc->enabled && crtc->fb) {
+		planea_wm = intel_calculate_wm(crtc->mode.clock,
+					       wm_info, fifo_size,
+					       crtc->fb->bits_per_pixel / 8,
+					       latency_ns);
+		enabled = crtc;
+	} else
+		planea_wm = fifo_size - wm_info->guard_size;
+
+	fifo_size = dev_priv->display.get_fifo_size(dev, 1);
+	crtc = intel_get_crtc_for_plane(dev, 1);
+	if (crtc->enabled && crtc->fb) {
+		planeb_wm = intel_calculate_wm(crtc->mode.clock,
+					       wm_info, fifo_size,
+					       crtc->fb->bits_per_pixel / 8,
+					       latency_ns);
+		if (enabled == NULL)
+			enabled = crtc;
+		else
+			enabled = NULL;
+	} else
+		planeb_wm = fifo_size - wm_info->guard_size;
+
+	DRM_DEBUG_KMS("FIFO watermarks - A: %d, B: %d\n", planea_wm, planeb_wm);
+
+	/*
+	 * Overlay gets an aggressive default since video jitter is bad.
+	 */
+	cwm = 2;
+
+	/* Play safe and disable self-refresh before adjusting watermarks. */
+	if (IS_I945G(dev) || IS_I945GM(dev))
+		I915_WRITE(FW_BLC_SELF, FW_BLC_SELF_EN_MASK | 0);
+	else if (IS_I915GM(dev))
+		I915_WRITE(INSTPM, I915_READ(INSTPM) & ~INSTPM_SELF_EN);
+
+	/* Calc sr entries for one plane configs */
+	if (HAS_FW_BLC(dev) && enabled) {
+		/* self-refresh has much higher latency */
+		static const int sr_latency_ns = 6000;
+		int clock = enabled->mode.clock;
+		int htotal = enabled->mode.htotal;
+		int hdisplay = enabled->mode.hdisplay;
+		int pixel_size = enabled->fb->bits_per_pixel / 8;
+		unsigned long line_time_us;
+		int entries;
+
+		line_time_us = (htotal * 1000) / clock;
+
+		/* Use ns/us then divide to preserve precision */
+		entries = (((sr_latency_ns / line_time_us) + 1000) / 1000) *
+			pixel_size * hdisplay;
+		entries = DIV_ROUND_UP(entries, wm_info->cacheline_size);
+		DRM_DEBUG_KMS("self-refresh entries: %d\n", entries);
+		srwm = wm_info->fifo_size - entries;
+		if (srwm < 0)
+			srwm = 1;
+
+		if (IS_I945G(dev) || IS_I945GM(dev))
+			I915_WRITE(FW_BLC_SELF,
+				   FW_BLC_SELF_FIFO_MASK | (srwm & 0xff));
+		else if (IS_I915GM(dev))
+			I915_WRITE(FW_BLC_SELF, srwm & 0x3f);
+	}
+
+	DRM_DEBUG_KMS("Setting FIFO watermarks - A: %d, B: %d, C: %d, SR %d\n",
+		      planea_wm, planeb_wm, cwm, srwm);
+
+	fwater_lo = ((planeb_wm & 0x3f) << 16) | (planea_wm & 0x3f);
+	fwater_hi = (cwm & 0x1f);
+
+	/* Set request length to 8 cachelines per fetch */
+	fwater_lo = fwater_lo | (1 << 24) | (1 << 8);
+	fwater_hi = fwater_hi | (1 << 8);
+
+	I915_WRITE(FW_BLC, fwater_lo);
+	I915_WRITE(FW_BLC2, fwater_hi);
+
+	if (HAS_FW_BLC(dev)) {
+		if (enabled) {
+			if (IS_I945G(dev) || IS_I945GM(dev))
+				I915_WRITE(FW_BLC_SELF,
+					   FW_BLC_SELF_EN_MASK | FW_BLC_SELF_EN);
+			else if (IS_I915GM(dev))
+				I915_WRITE(INSTPM, I915_READ(INSTPM) | INSTPM_SELF_EN);
+			DRM_DEBUG_KMS("memory self refresh enabled\n");
+		} else
+			DRM_DEBUG_KMS("memory self refresh disabled\n");
+	}
+}
+
+static void i830_update_wm(struct drm_device *dev)
+{
+	struct drm_i915_private *dev_priv = dev->dev_private;
+	struct drm_crtc *crtc;
+	uint32_t fwater_lo;
+	int planea_wm;
+
+	crtc = single_enabled_crtc(dev);
+	if (crtc == NULL)
+		return;
+
+	planea_wm = intel_calculate_wm(crtc->mode.clock, &i830_wm_info,
+				       dev_priv->display.get_fifo_size(dev, 0),
+				       crtc->fb->bits_per_pixel / 8,
+				       latency_ns);
+	fwater_lo = I915_READ(FW_BLC) & ~0xfff;
+	fwater_lo |= (3<<8) | planea_wm;
+
+	DRM_DEBUG_KMS("Setting FIFO watermarks - A: %d\n", planea_wm);
+
+	I915_WRITE(FW_BLC, fwater_lo);
+}
+
+#define ILK_LP0_PLANE_LATENCY		700
+#define ILK_LP0_CURSOR_LATENCY		1300
+
+/*
+ * Check the wm result.
+ *
+ * If any calculated watermark values is larger than the maximum value that
+ * can be programmed into the associated watermark register, that watermark
+ * must be disabled.
+ */
+static bool ironlake_check_srwm(struct drm_device *dev, int level,
+				int fbc_wm, int display_wm, int cursor_wm,
+				const struct intel_watermark_params *display,
+				const struct intel_watermark_params *cursor)
+{
+	struct drm_i915_private *dev_priv = dev->dev_private;
+
+	DRM_DEBUG_KMS("watermark %d: display plane %d, fbc lines %d,"
+		      " cursor %d\n", level, display_wm, fbc_wm, cursor_wm);
+
+	if (fbc_wm > SNB_FBC_MAX_SRWM) {
+		DRM_DEBUG_KMS("fbc watermark(%d) is too large(%d), disabling wm%d+\n",
+			      fbc_wm, SNB_FBC_MAX_SRWM, level);
+
+		/* fbc has it's own way to disable FBC WM */
+		I915_WRITE(DISP_ARB_CTL,
+			   I915_READ(DISP_ARB_CTL) | DISP_FBC_WM_DIS);
+		return false;
+	}
+
+	if (display_wm > display->max_wm) {
+		DRM_DEBUG_KMS("display watermark(%d) is too large(%d), disabling wm%d+\n",
+			      display_wm, SNB_DISPLAY_MAX_SRWM, level);
+		return false;
+	}
+
+	if (cursor_wm > cursor->max_wm) {
+		DRM_DEBUG_KMS("cursor watermark(%d) is too large(%d), disabling wm%d+\n",
+			      cursor_wm, SNB_CURSOR_MAX_SRWM, level);
+		return false;
+	}
+
+	if (!(fbc_wm || display_wm || cursor_wm)) {
+		DRM_DEBUG_KMS("latency %d is 0, disabling wm%d+\n", level, level);
+		return false;
+	}
+
+	return true;
+}
+
+/*
+ * Compute watermark values of WM[1-3],
+ */
+static bool ironlake_compute_srwm(struct drm_device *dev, int level, int plane,
+				  int latency_ns,
+				  const struct intel_watermark_params *display,
+				  const struct intel_watermark_params *cursor,
+				  int *fbc_wm, int *display_wm, int *cursor_wm)
+{
+	struct drm_crtc *crtc;
+	unsigned long line_time_us;
+	int hdisplay, htotal, pixel_size, clock;
+	int line_count, line_size;
+	int small, large;
+	int entries;
+
+	if (!latency_ns) {
+		*fbc_wm = *display_wm = *cursor_wm = 0;
+		return false;
+	}
+
+	crtc = intel_get_crtc_for_plane(dev, plane);
+	hdisplay = crtc->mode.hdisplay;
+	htotal = crtc->mode.htotal;
+	clock = crtc->mode.clock;
+	pixel_size = crtc->fb->bits_per_pixel / 8;
+
+	line_time_us = (htotal * 1000) / clock;
+	line_count = (latency_ns / line_time_us + 1000) / 1000;
+	line_size = hdisplay * pixel_size;
+
+	/* Use the minimum of the small and large buffer method for primary */
+	small = ((clock * pixel_size / 1000) * latency_ns) / 1000;
+	large = line_count * line_size;
+
+	entries = DIV_ROUND_UP(min(small, large), display->cacheline_size);
+	*display_wm = entries + display->guard_size;
+
+	/*
+	 * Spec says:
+	 * FBC WM = ((Final Primary WM * 64) / number of bytes per line) + 2
+	 */
+	*fbc_wm = DIV_ROUND_UP(*display_wm * 64, line_size) + 2;
+
+	/* calculate the self-refresh watermark for display cursor */
+	entries = line_count * pixel_size * 64;
+	entries = DIV_ROUND_UP(entries, cursor->cacheline_size);
+	*cursor_wm = entries + cursor->guard_size;
+
+	return ironlake_check_srwm(dev, level,
+				   *fbc_wm, *display_wm, *cursor_wm,
+				   display, cursor);
+}
+
+static void ironlake_update_wm(struct drm_device *dev)
+{
+	struct drm_i915_private *dev_priv = dev->dev_private;
+	int fbc_wm, plane_wm, cursor_wm;
+	unsigned int enabled;
+
+	enabled = 0;
+	if (g4x_compute_wm0(dev, 0,
+			    &ironlake_display_wm_info,
+			    ILK_LP0_PLANE_LATENCY,
+			    &ironlake_cursor_wm_info,
+			    ILK_LP0_CURSOR_LATENCY,
+			    &plane_wm, &cursor_wm)) {
+		I915_WRITE(WM0_PIPEA_ILK,
+			   (plane_wm << WM0_PIPE_PLANE_SHIFT) | cursor_wm);
+		DRM_DEBUG_KMS("FIFO watermarks For pipe A -"
+			      " plane %d, " "cursor: %d\n",
+			      plane_wm, cursor_wm);
+		enabled |= 1;
+	}
+
+	if (g4x_compute_wm0(dev, 1,
+			    &ironlake_display_wm_info,
+			    ILK_LP0_PLANE_LATENCY,
+			    &ironlake_cursor_wm_info,
+			    ILK_LP0_CURSOR_LATENCY,
+			    &plane_wm, &cursor_wm)) {
+		I915_WRITE(WM0_PIPEB_ILK,
+			   (plane_wm << WM0_PIPE_PLANE_SHIFT) | cursor_wm);
+		DRM_DEBUG_KMS("FIFO watermarks For pipe B -"
+			      " plane %d, cursor: %d\n",
+			      plane_wm, cursor_wm);
+		enabled |= 2;
+	}
+
+	/*
+	 * Calculate and update the self-refresh watermark only when one
+	 * display plane is used.
+	 */
+	I915_WRITE(WM3_LP_ILK, 0);
+	I915_WRITE(WM2_LP_ILK, 0);
+	I915_WRITE(WM1_LP_ILK, 0);
+
+	if (!single_plane_enabled(enabled))
+		return;
+	enabled = ffs(enabled) - 1;
+
+	/* WM1 */
+	if (!ironlake_compute_srwm(dev, 1, enabled,
+				   ILK_READ_WM1_LATENCY() * 500,
+				   &ironlake_display_srwm_info,
+				   &ironlake_cursor_srwm_info,
+				   &fbc_wm, &plane_wm, &cursor_wm))
+		return;
+
+	I915_WRITE(WM1_LP_ILK,
+		   WM1_LP_SR_EN |
+		   (ILK_READ_WM1_LATENCY() << WM1_LP_LATENCY_SHIFT) |
+		   (fbc_wm << WM1_LP_FBC_SHIFT) |
+		   (plane_wm << WM1_LP_SR_SHIFT) |
+		   cursor_wm);
+
+	/* WM2 */
+	if (!ironlake_compute_srwm(dev, 2, enabled,
+				   ILK_READ_WM2_LATENCY() * 500,
+				   &ironlake_display_srwm_info,
+				   &ironlake_cursor_srwm_info,
+				   &fbc_wm, &plane_wm, &cursor_wm))
+		return;
+
+	I915_WRITE(WM2_LP_ILK,
+		   WM2_LP_EN |
+		   (ILK_READ_WM2_LATENCY() << WM1_LP_LATENCY_SHIFT) |
+		   (fbc_wm << WM1_LP_FBC_SHIFT) |
+		   (plane_wm << WM1_LP_SR_SHIFT) |
+		   cursor_wm);
+
+	/*
+	 * WM3 is unsupported on ILK, probably because we don't have latency
+	 * data for that power state
+	 */
+}
+
+static void sandybridge_update_wm(struct drm_device *dev)
+{
+	struct drm_i915_private *dev_priv = dev->dev_private;
+	int latency = SNB_READ_WM0_LATENCY() * 100;	/* In unit 0.1us */
+	u32 val;
+	int fbc_wm, plane_wm, cursor_wm;
+	unsigned int enabled;
+
+	enabled = 0;
+	if (g4x_compute_wm0(dev, 0,
+			    &sandybridge_display_wm_info, latency,
+			    &sandybridge_cursor_wm_info, latency,
+			    &plane_wm, &cursor_wm)) {
+		val = I915_READ(WM0_PIPEA_ILK);
+		val &= ~(WM0_PIPE_PLANE_MASK | WM0_PIPE_CURSOR_MASK);
+		I915_WRITE(WM0_PIPEA_ILK, val |
+			   ((plane_wm << WM0_PIPE_PLANE_SHIFT) | cursor_wm));
+		DRM_DEBUG_KMS("FIFO watermarks For pipe A -"
+			      " plane %d, " "cursor: %d\n",
+			      plane_wm, cursor_wm);
+		enabled |= 1;
+	}
+
+	if (g4x_compute_wm0(dev, 1,
+			    &sandybridge_display_wm_info, latency,
+			    &sandybridge_cursor_wm_info, latency,
+			    &plane_wm, &cursor_wm)) {
+		val = I915_READ(WM0_PIPEB_ILK);
+		val &= ~(WM0_PIPE_PLANE_MASK | WM0_PIPE_CURSOR_MASK);
+		I915_WRITE(WM0_PIPEB_ILK, val |
+			   ((plane_wm << WM0_PIPE_PLANE_SHIFT) | cursor_wm));
+		DRM_DEBUG_KMS("FIFO watermarks For pipe B -"
+			      " plane %d, cursor: %d\n",
+			      plane_wm, cursor_wm);
+		enabled |= 2;
+	}
+
+	if ((dev_priv->num_pipe == 3) &&
+	    g4x_compute_wm0(dev, 2,
+			    &sandybridge_display_wm_info, latency,
+			    &sandybridge_cursor_wm_info, latency,
+			    &plane_wm, &cursor_wm)) {
+		val = I915_READ(WM0_PIPEC_IVB);
+		val &= ~(WM0_PIPE_PLANE_MASK | WM0_PIPE_CURSOR_MASK);
+		I915_WRITE(WM0_PIPEC_IVB, val |
+			   ((plane_wm << WM0_PIPE_PLANE_SHIFT) | cursor_wm));
+		DRM_DEBUG_KMS("FIFO watermarks For pipe C -"
+			      " plane %d, cursor: %d\n",
+			      plane_wm, cursor_wm);
+		enabled |= 3;
+	}
+
+	/*
+	 * Calculate and update the self-refresh watermark only when one
+	 * display plane is used.
+	 *
+	 * SNB support 3 levels of watermark.
+	 *
+	 * WM1/WM2/WM2 watermarks have to be enabled in the ascending order,
+	 * and disabled in the descending order
+	 *
+	 */
+	I915_WRITE(WM3_LP_ILK, 0);
+	I915_WRITE(WM2_LP_ILK, 0);
+	I915_WRITE(WM1_LP_ILK, 0);
+
+	if (!single_plane_enabled(enabled) ||
+	    dev_priv->sprite_scaling_enabled)
+		return;
+	enabled = ffs(enabled) - 1;
+
+	/* WM1 */
+	if (!ironlake_compute_srwm(dev, 1, enabled,
+				   SNB_READ_WM1_LATENCY() * 500,
+				   &sandybridge_display_srwm_info,
+				   &sandybridge_cursor_srwm_info,
+				   &fbc_wm, &plane_wm, &cursor_wm))
+		return;
+
+	I915_WRITE(WM1_LP_ILK,
+		   WM1_LP_SR_EN |
+		   (SNB_READ_WM1_LATENCY() << WM1_LP_LATENCY_SHIFT) |
+		   (fbc_wm << WM1_LP_FBC_SHIFT) |
+		   (plane_wm << WM1_LP_SR_SHIFT) |
+		   cursor_wm);
+
+	/* WM2 */
+	if (!ironlake_compute_srwm(dev, 2, enabled,
+				   SNB_READ_WM2_LATENCY() * 500,
+				   &sandybridge_display_srwm_info,
+				   &sandybridge_cursor_srwm_info,
+				   &fbc_wm, &plane_wm, &cursor_wm))
+		return;
+
+	I915_WRITE(WM2_LP_ILK,
+		   WM2_LP_EN |
+		   (SNB_READ_WM2_LATENCY() << WM1_LP_LATENCY_SHIFT) |
+		   (fbc_wm << WM1_LP_FBC_SHIFT) |
+		   (plane_wm << WM1_LP_SR_SHIFT) |
+		   cursor_wm);
+
+	/* WM3 */
+	if (!ironlake_compute_srwm(dev, 3, enabled,
+				   SNB_READ_WM3_LATENCY() * 500,
+				   &sandybridge_display_srwm_info,
+				   &sandybridge_cursor_srwm_info,
+				   &fbc_wm, &plane_wm, &cursor_wm))
+		return;
+
+	I915_WRITE(WM3_LP_ILK,
+		   WM3_LP_EN |
+		   (SNB_READ_WM3_LATENCY() << WM1_LP_LATENCY_SHIFT) |
+		   (fbc_wm << WM1_LP_FBC_SHIFT) |
+		   (plane_wm << WM1_LP_SR_SHIFT) |
+		   cursor_wm);
+}
+
+static void
+haswell_update_linetime_wm(struct drm_device *dev, int pipe,
+				 struct drm_display_mode *mode)
+{
+	struct drm_i915_private *dev_priv = dev->dev_private;
+	u32 temp;
+
+	temp = I915_READ(PIPE_WM_LINETIME(pipe));
+	temp &= ~PIPE_WM_LINETIME_MASK;
+
+	/* The WM are computed with base on how long it takes to fill a single
+	 * row at the given clock rate, multiplied by 8.
+	 * */
+	temp |= PIPE_WM_LINETIME_TIME(
+		((mode->crtc_hdisplay * 1000) / mode->clock) * 8);
+
+	/* IPS watermarks are only used by pipe A, and are ignored by
+	 * pipes B and C.  They are calculated similarly to the common
+	 * linetime values, except that we are using CD clock frequency
+	 * in MHz instead of pixel rate for the division.
+	 *
+	 * This is a placeholder for the IPS watermark calculation code.
+	 */
+
+	I915_WRITE(PIPE_WM_LINETIME(pipe), temp);
+}
+
+static bool
+sandybridge_compute_sprite_wm(struct drm_device *dev, int plane,
+			      uint32_t sprite_width, int pixel_size,
+			      const struct intel_watermark_params *display,
+			      int display_latency_ns, int *sprite_wm)
+{
+	struct drm_crtc *crtc;
+	int clock;
+	int entries, tlb_miss;
+
+	crtc = intel_get_crtc_for_plane(dev, plane);
+	if (crtc->fb == NULL || !crtc->enabled) {
+		*sprite_wm = display->guard_size;
+		return false;
+	}
+
+	clock = crtc->mode.clock;
+
+	/* Use the small buffer method to calculate the sprite watermark */
+	entries = ((clock * pixel_size / 1000) * display_latency_ns) / 1000;
+	tlb_miss = display->fifo_size*display->cacheline_size -
+		sprite_width * 8;
+	if (tlb_miss > 0)
+		entries += tlb_miss;
+	entries = DIV_ROUND_UP(entries, display->cacheline_size);
+	*sprite_wm = entries + display->guard_size;
+	if (*sprite_wm > (int)display->max_wm)
+		*sprite_wm = display->max_wm;
+
+	return true;
+}
+
+static bool
+sandybridge_compute_sprite_srwm(struct drm_device *dev, int plane,
+				uint32_t sprite_width, int pixel_size,
+				const struct intel_watermark_params *display,
+				int latency_ns, int *sprite_wm)
+{
+	struct drm_crtc *crtc;
+	unsigned long line_time_us;
+	int clock;
+	int line_count, line_size;
+	int small, large;
+	int entries;
+
+	if (!latency_ns) {
+		*sprite_wm = 0;
+		return false;
+	}
+
+	crtc = intel_get_crtc_for_plane(dev, plane);
+	clock = crtc->mode.clock;
+	if (!clock) {
+		*sprite_wm = 0;
+		return false;
+	}
+
+	line_time_us = (sprite_width * 1000) / clock;
+	if (!line_time_us) {
+		*sprite_wm = 0;
+		return false;
+	}
+
+	line_count = (latency_ns / line_time_us + 1000) / 1000;
+	line_size = sprite_width * pixel_size;
+
+	/* Use the minimum of the small and large buffer method for primary */
+	small = ((clock * pixel_size / 1000) * latency_ns) / 1000;
+	large = line_count * line_size;
+
+	entries = DIV_ROUND_UP(min(small, large), display->cacheline_size);
+	*sprite_wm = entries + display->guard_size;
+
+	return *sprite_wm > 0x3ff ? false : true;
+}
+
+static void sandybridge_update_sprite_wm(struct drm_device *dev, int pipe,
+					 uint32_t sprite_width, int pixel_size)
+{
+	struct drm_i915_private *dev_priv = dev->dev_private;
+	int latency = SNB_READ_WM0_LATENCY() * 100;	/* In unit 0.1us */
+	u32 val;
+	int sprite_wm, reg;
+	int ret;
+
+	switch (pipe) {
+	case 0:
+		reg = WM0_PIPEA_ILK;
+		break;
+	case 1:
+		reg = WM0_PIPEB_ILK;
+		break;
+	case 2:
+		reg = WM0_PIPEC_IVB;
+		break;
+	default:
+		return; /* bad pipe */
+	}
+
+	ret = sandybridge_compute_sprite_wm(dev, pipe, sprite_width, pixel_size,
+					    &sandybridge_display_wm_info,
+					    latency, &sprite_wm);
+	if (!ret) {
+		DRM_DEBUG_KMS("failed to compute sprite wm for pipe %d\n",
+			      pipe);
+		return;
+	}
+
+	val = I915_READ(reg);
+	val &= ~WM0_PIPE_SPRITE_MASK;
+	I915_WRITE(reg, val | (sprite_wm << WM0_PIPE_SPRITE_SHIFT));
+	DRM_DEBUG_KMS("sprite watermarks For pipe %d - %d\n", pipe, sprite_wm);
+
+
+	ret = sandybridge_compute_sprite_srwm(dev, pipe, sprite_width,
+					      pixel_size,
+					      &sandybridge_display_srwm_info,
+					      SNB_READ_WM1_LATENCY() * 500,
+					      &sprite_wm);
+	if (!ret) {
+		DRM_DEBUG_KMS("failed to compute sprite lp1 wm on pipe %d\n",
+			      pipe);
+		return;
+	}
+	I915_WRITE(WM1S_LP_ILK, sprite_wm);
+
+	/* Only IVB has two more LP watermarks for sprite */
+	if (!IS_IVYBRIDGE(dev))
+		return;
+
+	ret = sandybridge_compute_sprite_srwm(dev, pipe, sprite_width,
+					      pixel_size,
+					      &sandybridge_display_srwm_info,
+					      SNB_READ_WM2_LATENCY() * 500,
+					      &sprite_wm);
+	if (!ret) {
+		DRM_DEBUG_KMS("failed to compute sprite lp2 wm on pipe %d\n",
+			      pipe);
+		return;
+	}
+	I915_WRITE(WM2S_LP_IVB, sprite_wm);
+
+	ret = sandybridge_compute_sprite_srwm(dev, pipe, sprite_width,
+					      pixel_size,
+					      &sandybridge_display_srwm_info,
+					      SNB_READ_WM3_LATENCY() * 500,
+					      &sprite_wm);
+	if (!ret) {
+		DRM_DEBUG_KMS("failed to compute sprite lp3 wm on pipe %d\n",
+			      pipe);
+		return;
+	}
+	I915_WRITE(WM3S_LP_IVB, sprite_wm);
+}
+
+/**
+ * intel_update_watermarks - update FIFO watermark values based on current modes
+ *
+ * Calculate watermark values for the various WM regs based on current mode
+ * and plane configuration.
+ *
+ * There are several cases to deal with here:
+ *   - normal (i.e. non-self-refresh)
+ *   - self-refresh (SR) mode
+ *   - lines are large relative to FIFO size (buffer can hold up to 2)
+ *   - lines are small relative to FIFO size (buffer can hold more than 2
+ *     lines), so need to account for TLB latency
+ *
+ *   The normal calculation is:
+ *     watermark = dotclock * bytes per pixel * latency
+ *   where latency is platform & configuration dependent (we assume pessimal
+ *   values here).
+ *
+ *   The SR calculation is:
+ *     watermark = (trunc(latency/line time)+1) * surface width *
+ *       bytes per pixel
+ *   where
+ *     line time = htotal / dotclock
+ *     surface width = hdisplay for normal plane and 64 for cursor
+ *   and latency is assumed to be high, as above.
+ *
+ * The final value programmed to the register should always be rounded up,
+ * and include an extra 2 entries to account for clock crossings.
+ *
+ * We don't use the sprite, so we can ignore that.  And on Crestline we have
+ * to set the non-SR watermarks to 8.
+ */
+void intel_update_watermarks(struct drm_device *dev)
+{
+	struct drm_i915_private *dev_priv = dev->dev_private;
+
+	if (dev_priv->display.update_wm)
+		dev_priv->display.update_wm(dev);
+}
+
+void intel_update_linetime_watermarks(struct drm_device *dev,
+		int pipe, struct drm_display_mode *mode)
+{
+	struct drm_i915_private *dev_priv = dev->dev_private;
+
+	if (dev_priv->display.update_linetime_wm)
+		dev_priv->display.update_linetime_wm(dev, pipe, mode);
+}
+
+void intel_update_sprite_watermarks(struct drm_device *dev, int pipe,
+				    uint32_t sprite_width, int pixel_size)
+{
+	struct drm_i915_private *dev_priv = dev->dev_private;
+
+	if (dev_priv->display.update_sprite_wm)
+		dev_priv->display.update_sprite_wm(dev, pipe, sprite_width,
+						   pixel_size);
+}
+
+static struct drm_i915_gem_object *
+intel_alloc_context_page(struct drm_device *dev)
+{
+	struct drm_i915_gem_object *ctx;
+	int ret;
+
+	WARN_ON(!mutex_is_locked(&dev->struct_mutex));
+
+	ctx = i915_gem_alloc_object(dev, 4096);
+	if (!ctx) {
+		DRM_DEBUG("failed to alloc power context, RC6 disabled\n");
+		return NULL;
+	}
+
+	ret = i915_gem_object_pin(ctx, 4096, true);
+	if (ret) {
+		DRM_ERROR("failed to pin power context: %d\n", ret);
+		goto err_unref;
+	}
+
+	ret = i915_gem_object_set_to_gtt_domain(ctx, 1);
+	if (ret) {
+		DRM_ERROR("failed to set-domain on power context: %d\n", ret);
+		goto err_unpin;
+	}
+
+	return ctx;
+
+err_unpin:
+	i915_gem_object_unpin(ctx);
+err_unref:
+	drm_gem_object_unreference(&ctx->base);
+	mutex_unlock(&dev->struct_mutex);
+	return NULL;
+}
+
+bool ironlake_set_drps(struct drm_device *dev, u8 val)
+{
+	struct drm_i915_private *dev_priv = dev->dev_private;
+	u16 rgvswctl;
+
+	rgvswctl = I915_READ16(MEMSWCTL);
+	if (rgvswctl & MEMCTL_CMD_STS) {
+		DRM_DEBUG("gpu busy, RCS change rejected\n");
+		return false; /* still busy with another command */
+	}
+
+	rgvswctl = (MEMCTL_CMD_CHFREQ << MEMCTL_CMD_SHIFT) |
+		(val << MEMCTL_FREQ_SHIFT) | MEMCTL_SFCAVM;
+	I915_WRITE16(MEMSWCTL, rgvswctl);
+	POSTING_READ16(MEMSWCTL);
+
+	rgvswctl |= MEMCTL_CMD_STS;
+	I915_WRITE16(MEMSWCTL, rgvswctl);
+
+	return true;
+}
+
+void ironlake_enable_drps(struct drm_device *dev)
+{
+	struct drm_i915_private *dev_priv = dev->dev_private;
+	u32 rgvmodectl = I915_READ(MEMMODECTL);
+	u8 fmax, fmin, fstart, vstart;
+
+	/* Enable temp reporting */
+	I915_WRITE16(PMMISC, I915_READ(PMMISC) | MCPPCE_EN);
+	I915_WRITE16(TSC1, I915_READ(TSC1) | TSE);
+
+	/* 100ms RC evaluation intervals */
+	I915_WRITE(RCUPEI, 100000);
+	I915_WRITE(RCDNEI, 100000);
+
+	/* Set max/min thresholds to 90ms and 80ms respectively */
+	I915_WRITE(RCBMAXAVG, 90000);
+	I915_WRITE(RCBMINAVG, 80000);
+
+	I915_WRITE(MEMIHYST, 1);
+
+	/* Set up min, max, and cur for interrupt handling */
+	fmax = (rgvmodectl & MEMMODE_FMAX_MASK) >> MEMMODE_FMAX_SHIFT;
+	fmin = (rgvmodectl & MEMMODE_FMIN_MASK);
+	fstart = (rgvmodectl & MEMMODE_FSTART_MASK) >>
+		MEMMODE_FSTART_SHIFT;
+
+	vstart = (I915_READ(PXVFREQ_BASE + (fstart * 4)) & PXVFREQ_PX_MASK) >>
+		PXVFREQ_PX_SHIFT;
+
+	dev_priv->fmax = fmax; /* IPS callback will increase this */
+	dev_priv->fstart = fstart;
+
+	dev_priv->max_delay = fstart;
+	dev_priv->min_delay = fmin;
+	dev_priv->cur_delay = fstart;
+
+	DRM_DEBUG_DRIVER("fmax: %d, fmin: %d, fstart: %d\n",
+			 fmax, fmin, fstart);
+
+	I915_WRITE(MEMINTREN, MEMINT_CX_SUPR_EN | MEMINT_EVAL_CHG_EN);
+
+	/*
+	 * Interrupts will be enabled in ironlake_irq_postinstall
+	 */
+
+	I915_WRITE(VIDSTART, vstart);
+	POSTING_READ(VIDSTART);
+
+	rgvmodectl |= MEMMODE_SWMODE_EN;
+	I915_WRITE(MEMMODECTL, rgvmodectl);
+
+	if (wait_for((I915_READ(MEMSWCTL) & MEMCTL_CMD_STS) == 0, 10))
+		DRM_ERROR("stuck trying to change perf mode\n");
+	msleep(1);
+
+	ironlake_set_drps(dev, fstart);
+
+	dev_priv->last_count1 = I915_READ(0x112e4) + I915_READ(0x112e8) +
+		I915_READ(0x112e0);
+	dev_priv->last_time1 = jiffies_to_msecs(jiffies);
+	dev_priv->last_count2 = I915_READ(0x112f4);
+	getrawmonotonic(&dev_priv->last_time2);
+}
+
+void ironlake_disable_drps(struct drm_device *dev)
+{
+	struct drm_i915_private *dev_priv = dev->dev_private;
+	u16 rgvswctl = I915_READ16(MEMSWCTL);
+
+	/* Ack interrupts, disable EFC interrupt */
+	I915_WRITE(MEMINTREN, I915_READ(MEMINTREN) & ~MEMINT_EVAL_CHG_EN);
+	I915_WRITE(MEMINTRSTS, MEMINT_EVAL_CHG);
+	I915_WRITE(DEIER, I915_READ(DEIER) & ~DE_PCU_EVENT);
+	I915_WRITE(DEIIR, DE_PCU_EVENT);
+	I915_WRITE(DEIMR, I915_READ(DEIMR) | DE_PCU_EVENT);
+
+	/* Go back to the starting frequency */
+	ironlake_set_drps(dev, dev_priv->fstart);
+	msleep(1);
+	rgvswctl |= MEMCTL_CMD_STS;
+	I915_WRITE(MEMSWCTL, rgvswctl);
+	msleep(1);
+
+}
+
+void gen6_set_rps(struct drm_device *dev, u8 val)
+{
+	struct drm_i915_private *dev_priv = dev->dev_private;
+	u32 swreq;
+
+	swreq = (val & 0x3ff) << 25;
+	I915_WRITE(GEN6_RPNSWREQ, swreq);
+}
+
+void gen6_disable_rps(struct drm_device *dev)
+{
+	struct drm_i915_private *dev_priv = dev->dev_private;
+
+	I915_WRITE(GEN6_RPNSWREQ, 1 << 31);
+	I915_WRITE(GEN6_PMINTRMSK, 0xffffffff);
+	I915_WRITE(GEN6_PMIER, 0);
+	/* Complete PM interrupt masking here doesn't race with the rps work
+	 * item again unmasking PM interrupts because that is using a different
+	 * register (PMIMR) to mask PM interrupts. The only risk is in leaving
+	 * stale bits in PMIIR and PMIMR which gen6_enable_rps will clean up. */
+
+	spin_lock_irq(&dev_priv->rps_lock);
+	dev_priv->pm_iir = 0;
+	spin_unlock_irq(&dev_priv->rps_lock);
+
+	I915_WRITE(GEN6_PMIIR, I915_READ(GEN6_PMIIR));
+}
+
+int intel_enable_rc6(const struct drm_device *dev)
+{
+	/*
+	 * Respect the kernel parameter if it is set
+	 */
+	if (i915_enable_rc6 >= 0)
+		return i915_enable_rc6;
+
+	/*
+	 * Disable RC6 on Ironlake
+	 */
+	if (INTEL_INFO(dev)->gen == 5)
+		return 0;
+
+	/* Sorry Haswell, no RC6 for you for now. */
+	if (IS_HASWELL(dev))
+		return 0;
+
+	/*
+	 * Disable rc6 on Sandybridge
+	 */
+	if (INTEL_INFO(dev)->gen == 6) {
+		DRM_DEBUG_DRIVER("Sandybridge: deep RC6 disabled\n");
+		return INTEL_RC6_ENABLE;
+	}
+	DRM_DEBUG_DRIVER("RC6 and deep RC6 enabled\n");
+	return (INTEL_RC6_ENABLE | INTEL_RC6p_ENABLE);
+}
+
+void gen6_enable_rps(struct drm_i915_private *dev_priv)
+{
+	struct intel_ring_buffer *ring;
+	u32 rp_state_cap = I915_READ(GEN6_RP_STATE_CAP);
+	u32 gt_perf_status = I915_READ(GEN6_GT_PERF_STATUS);
+	u32 pcu_mbox, rc6_mask = 0;
+	u32 gtfifodbg;
+	int cur_freq, min_freq, max_freq;
+	int rc6_mode;
+	int i;
+
+	/* Here begins a magic sequence of register writes to enable
+	 * auto-downclocking.
+	 *
+	 * Perhaps there might be some value in exposing these to
+	 * userspace...
+	 */
+	I915_WRITE(GEN6_RC_STATE, 0);
+	mutex_lock(&dev_priv->dev->struct_mutex);
+
+	/* Clear the DBG now so we don't confuse earlier errors */
+	if ((gtfifodbg = I915_READ(GTFIFODBG))) {
+		DRM_ERROR("GT fifo had a previous error %x\n", gtfifodbg);
+		I915_WRITE(GTFIFODBG, gtfifodbg);
+	}
+
+	gen6_gt_force_wake_get(dev_priv);
+
+	/* disable the counters and set deterministic thresholds */
+	I915_WRITE(GEN6_RC_CONTROL, 0);
+
+	I915_WRITE(GEN6_RC1_WAKE_RATE_LIMIT, 1000 << 16);
+	I915_WRITE(GEN6_RC6_WAKE_RATE_LIMIT, 40 << 16 | 30);
+	I915_WRITE(GEN6_RC6pp_WAKE_RATE_LIMIT, 30);
+	I915_WRITE(GEN6_RC_EVALUATION_INTERVAL, 125000);
+	I915_WRITE(GEN6_RC_IDLE_HYSTERSIS, 25);
+
+	for_each_ring(ring, dev_priv, i)
+		I915_WRITE(RING_MAX_IDLE(ring->mmio_base), 10);
+
+	I915_WRITE(GEN6_RC_SLEEP, 0);
+	I915_WRITE(GEN6_RC1e_THRESHOLD, 1000);
+	I915_WRITE(GEN6_RC6_THRESHOLD, 50000);
+	I915_WRITE(GEN6_RC6p_THRESHOLD, 100000);
+	I915_WRITE(GEN6_RC6pp_THRESHOLD, 64000); /* unused */
+
+	rc6_mode = intel_enable_rc6(dev_priv->dev);
+	if (rc6_mode & INTEL_RC6_ENABLE)
+		rc6_mask |= GEN6_RC_CTL_RC6_ENABLE;
+
+	if (rc6_mode & INTEL_RC6p_ENABLE)
+		rc6_mask |= GEN6_RC_CTL_RC6p_ENABLE;
+
+	if (rc6_mode & INTEL_RC6pp_ENABLE)
+		rc6_mask |= GEN6_RC_CTL_RC6pp_ENABLE;
+
+	DRM_INFO("Enabling RC6 states: RC6 %s, RC6p %s, RC6pp %s\n",
+			(rc6_mode & INTEL_RC6_ENABLE) ? "on" : "off",
+			(rc6_mode & INTEL_RC6p_ENABLE) ? "on" : "off",
+			(rc6_mode & INTEL_RC6pp_ENABLE) ? "on" : "off");
+
+	I915_WRITE(GEN6_RC_CONTROL,
+		   rc6_mask |
+		   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));
+
+	I915_WRITE(GEN6_RP_DOWN_TIMEOUT, 1000000);
+	I915_WRITE(GEN6_RP_INTERRUPT_LIMITS,
+		   18 << 24 |
+		   6 << 16);
+	I915_WRITE(GEN6_RP_UP_THRESHOLD, 10000);
+	I915_WRITE(GEN6_RP_DOWN_THRESHOLD, 1000000);
+	I915_WRITE(GEN6_RP_UP_EI, 100000);
+	I915_WRITE(GEN6_RP_DOWN_EI, 5000000);
+	I915_WRITE(GEN6_RP_IDLE_HYSTERSIS, 10);
+	I915_WRITE(GEN6_RP_CONTROL,
+		   GEN6_RP_MEDIA_TURBO |
+		   GEN6_RP_MEDIA_HW_MODE |
+		   GEN6_RP_MEDIA_IS_GFX |
+		   GEN6_RP_ENABLE |
+		   GEN6_RP_UP_BUSY_AVG |
+		   GEN6_RP_DOWN_IDLE_CONT);
+
+	if (wait_for((I915_READ(GEN6_PCODE_MAILBOX) & GEN6_PCODE_READY) == 0,
+		     500))
+		DRM_ERROR("timeout waiting for pcode mailbox to become idle\n");
+
+	I915_WRITE(GEN6_PCODE_DATA, 0);
+	I915_WRITE(GEN6_PCODE_MAILBOX,
+		   GEN6_PCODE_READY |
+		   GEN6_PCODE_WRITE_MIN_FREQ_TABLE);
+	if (wait_for((I915_READ(GEN6_PCODE_MAILBOX) & GEN6_PCODE_READY) == 0,
+		     500))
+		DRM_ERROR("timeout waiting for pcode mailbox to finish\n");
+
+	min_freq = (rp_state_cap & 0xff0000) >> 16;
+	max_freq = rp_state_cap & 0xff;
+	cur_freq = (gt_perf_status & 0xff00) >> 8;
+
+	/* Check for overclock support */
+	if (wait_for((I915_READ(GEN6_PCODE_MAILBOX) & GEN6_PCODE_READY) == 0,
+		     500))
+		DRM_ERROR("timeout waiting for pcode mailbox to become idle\n");
+	I915_WRITE(GEN6_PCODE_MAILBOX, GEN6_READ_OC_PARAMS);
+	pcu_mbox = I915_READ(GEN6_PCODE_DATA);
+	if (wait_for((I915_READ(GEN6_PCODE_MAILBOX) & GEN6_PCODE_READY) == 0,
+		     500))
+		DRM_ERROR("timeout waiting for pcode mailbox to finish\n");
+	if (pcu_mbox & (1<<31)) { /* OC supported */
+		max_freq = pcu_mbox & 0xff;
+		DRM_DEBUG_DRIVER("overclocking supported, adjusting frequency max to %dMHz\n", pcu_mbox * 50);
+	}
+
+	/* In units of 100MHz */
+	dev_priv->max_delay = max_freq;
+	dev_priv->min_delay = min_freq;
+	dev_priv->cur_delay = cur_freq;
+
+	/* requires MSI enabled */
+	I915_WRITE(GEN6_PMIER,
+		   GEN6_PM_MBOX_EVENT |
+		   GEN6_PM_THERMAL_EVENT |
+		   GEN6_PM_RP_DOWN_TIMEOUT |
+		   GEN6_PM_RP_UP_THRESHOLD |
+		   GEN6_PM_RP_DOWN_THRESHOLD |
+		   GEN6_PM_RP_UP_EI_EXPIRED |
+		   GEN6_PM_RP_DOWN_EI_EXPIRED);
+	spin_lock_irq(&dev_priv->rps_lock);
+	WARN_ON(dev_priv->pm_iir != 0);
+	I915_WRITE(GEN6_PMIMR, 0);
+	spin_unlock_irq(&dev_priv->rps_lock);
+	/* enable all PM interrupts */
+	I915_WRITE(GEN6_PMINTRMSK, 0);
+
+	gen6_gt_force_wake_put(dev_priv);
+	mutex_unlock(&dev_priv->dev->struct_mutex);
+}
+
+void gen6_update_ring_freq(struct drm_i915_private *dev_priv)
+{
+	int min_freq = 15;
+	int gpu_freq, ia_freq, max_ia_freq;
+	int scaling_factor = 180;
+
+	max_ia_freq = cpufreq_quick_get_max(0);
+	/*
+	 * Default to measured freq if none found, PCU will ensure we don't go
+	 * over
+	 */
+	if (!max_ia_freq)
+		max_ia_freq = tsc_khz;
+
+	/* Convert from kHz to MHz */
+	max_ia_freq /= 1000;
+
+	mutex_lock(&dev_priv->dev->struct_mutex);
+
+	/*
+	 * 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
+	 * the PCU should use as a reference to determine the ring frequency.
+	 */
+	for (gpu_freq = dev_priv->max_delay; gpu_freq >= dev_priv->min_delay;
+	     gpu_freq--) {
+		int diff = dev_priv->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);
+
+		I915_WRITE(GEN6_PCODE_DATA,
+			   (ia_freq << GEN6_PCODE_FREQ_IA_RATIO_SHIFT) |
+			   gpu_freq);
+		I915_WRITE(GEN6_PCODE_MAILBOX, GEN6_PCODE_READY |
+			   GEN6_PCODE_WRITE_MIN_FREQ_TABLE);
+		if (wait_for((I915_READ(GEN6_PCODE_MAILBOX) &
+			      GEN6_PCODE_READY) == 0, 10)) {
+			DRM_ERROR("pcode write of freq table timed out\n");
+			continue;
+		}
+	}
+
+	mutex_unlock(&dev_priv->dev->struct_mutex);
+}
+
+static void ironlake_teardown_rc6(struct drm_device *dev)
+{
+	struct drm_i915_private *dev_priv = dev->dev_private;
+
+	if (dev_priv->renderctx) {
+		i915_gem_object_unpin(dev_priv->renderctx);
+		drm_gem_object_unreference(&dev_priv->renderctx->base);
+		dev_priv->renderctx = NULL;
+	}
+
+	if (dev_priv->pwrctx) {
+		i915_gem_object_unpin(dev_priv->pwrctx);
+		drm_gem_object_unreference(&dev_priv->pwrctx->base);
+		dev_priv->pwrctx = NULL;
+	}
+}
+
+void ironlake_disable_rc6(struct drm_device *dev)
+{
+	struct drm_i915_private *dev_priv = dev->dev_private;
+
+	if (I915_READ(PWRCTXA)) {
+		/* Wake the GPU, prevent RC6, then restore RSTDBYCTL */
+		I915_WRITE(RSTDBYCTL, I915_READ(RSTDBYCTL) | RCX_SW_EXIT);
+		wait_for(((I915_READ(RSTDBYCTL) & RSX_STATUS_MASK) == RSX_STATUS_ON),
+			 50);
+
+		I915_WRITE(PWRCTXA, 0);
+		POSTING_READ(PWRCTXA);
+
+		I915_WRITE(RSTDBYCTL, I915_READ(RSTDBYCTL) & ~RCX_SW_EXIT);
+		POSTING_READ(RSTDBYCTL);
+	}
+
+	ironlake_teardown_rc6(dev);
+}
+
+static int ironlake_setup_rc6(struct drm_device *dev)
+{
+	struct drm_i915_private *dev_priv = dev->dev_private;
+
+	if (dev_priv->renderctx == NULL)
+		dev_priv->renderctx = intel_alloc_context_page(dev);
+	if (!dev_priv->renderctx)
+		return -ENOMEM;
+
+	if (dev_priv->pwrctx == NULL)
+		dev_priv->pwrctx = intel_alloc_context_page(dev);
+	if (!dev_priv->pwrctx) {
+		ironlake_teardown_rc6(dev);
+		return -ENOMEM;
+	}
+
+	return 0;
+}
+
+void ironlake_enable_rc6(struct drm_device *dev)
+{
+	struct drm_i915_private *dev_priv = dev->dev_private;
+	struct intel_ring_buffer *ring = &dev_priv->ring[RCS];
+	int ret;
+
+	/* rc6 disabled by default due to repeated reports of hanging during
+	 * boot and resume.
+	 */
+	if (!intel_enable_rc6(dev))
+		return;
+
+	mutex_lock(&dev->struct_mutex);
+	ret = ironlake_setup_rc6(dev);
+	if (ret) {
+		mutex_unlock(&dev->struct_mutex);
+		return;
+	}
+
+	/*
+	 * GPU can automatically power down the render unit if given a page
+	 * to save state.
+	 */
+	ret = intel_ring_begin(ring, 6);
+	if (ret) {
+		ironlake_teardown_rc6(dev);
+		mutex_unlock(&dev->struct_mutex);
+		return;
+	}
+
+	intel_ring_emit(ring, MI_SUSPEND_FLUSH | MI_SUSPEND_FLUSH_EN);
+	intel_ring_emit(ring, MI_SET_CONTEXT);
+	intel_ring_emit(ring, dev_priv->renderctx->gtt_offset |
+			MI_MM_SPACE_GTT |
+			MI_SAVE_EXT_STATE_EN |
+			MI_RESTORE_EXT_STATE_EN |
+			MI_RESTORE_INHIBIT);
+	intel_ring_emit(ring, MI_SUSPEND_FLUSH);
+	intel_ring_emit(ring, MI_NOOP);
+	intel_ring_emit(ring, MI_FLUSH);
+	intel_ring_advance(ring);
+
+	/*
+	 * Wait for the command parser to advance past MI_SET_CONTEXT. The HW
+	 * does an implicit flush, combined with MI_FLUSH above, it should be
+	 * safe to assume that renderctx is valid
+	 */
+	ret = intel_wait_ring_idle(ring);
+	if (ret) {
+		DRM_ERROR("failed to enable ironlake power power savings\n");
+		ironlake_teardown_rc6(dev);
+		mutex_unlock(&dev->struct_mutex);
+		return;
+	}
+
+	I915_WRITE(PWRCTXA, dev_priv->pwrctx->gtt_offset | PWRCTX_EN);
+	I915_WRITE(RSTDBYCTL, I915_READ(RSTDBYCTL) & ~RCX_SW_EXIT);
+	mutex_unlock(&dev->struct_mutex);
+}
+
+static unsigned long intel_pxfreq(u32 vidfreq)
+{
+	unsigned long freq;
+	int div = (vidfreq & 0x3f0000) >> 16;
+	int post = (vidfreq & 0x3000) >> 12;
+	int pre = (vidfreq & 0x7);
+
+	if (!pre)
+		return 0;
+
+	freq = ((div * 133333) / ((1<<post) * pre));
+
+	return freq;
+}
+
+static const struct cparams {
+	u16 i;
+	u16 t;
+	u16 m;
+	u16 c;
+} cparams[] = {
+	{ 1, 1333, 301, 28664 },
+	{ 1, 1066, 294, 24460 },
+	{ 1, 800, 294, 25192 },
+	{ 0, 1333, 276, 27605 },
+	{ 0, 1066, 276, 27605 },
+	{ 0, 800, 231, 23784 },
+};
+
+unsigned long i915_chipset_val(struct drm_i915_private *dev_priv)
+{
+	u64 total_count, diff, ret;
+	u32 count1, count2, count3, m = 0, c = 0;
+	unsigned long now = jiffies_to_msecs(jiffies), diff1;
+	int i;
+
+	diff1 = now - dev_priv->last_time1;
+
+	/* Prevent division-by-zero if we are asking too fast.
+	 * Also, we don't get interesting results if we are polling
+	 * faster than once in 10ms, so just return the saved value
+	 * in such cases.
+	 */
+	if (diff1 <= 10)
+		return dev_priv->chipset_power;
+
+	count1 = I915_READ(DMIEC);
+	count2 = I915_READ(DDREC);
+	count3 = I915_READ(CSIEC);
+
+	total_count = count1 + count2 + count3;
+
+	/* FIXME: handle per-counter overflow */
+	if (total_count < dev_priv->last_count1) {
+		diff = ~0UL - dev_priv->last_count1;
+		diff += total_count;
+	} else {
+		diff = total_count - dev_priv->last_count1;
+	}
+
+	for (i = 0; i < ARRAY_SIZE(cparams); i++) {
+		if (cparams[i].i == dev_priv->c_m &&
+		    cparams[i].t == dev_priv->r_t) {
+			m = cparams[i].m;
+			c = cparams[i].c;
+			break;
+		}
+	}
+
+	diff = div_u64(diff, diff1);
+	ret = ((m * diff) + c);
+	ret = div_u64(ret, 10);
+
+	dev_priv->last_count1 = total_count;
+	dev_priv->last_time1 = now;
+
+	dev_priv->chipset_power = ret;
+
+	return ret;
+}
+
+unsigned long i915_mch_val(struct drm_i915_private *dev_priv)
+{
+	unsigned long m, x, b;
+	u32 tsfs;
+
+	tsfs = I915_READ(TSFS);
+
+	m = ((tsfs & TSFS_SLOPE_MASK) >> TSFS_SLOPE_SHIFT);
+	x = I915_READ8(TR1);
+
+	b = tsfs & TSFS_INTR_MASK;
+
+	return ((m * x) / 127) - b;
+}
+
+static u16 pvid_to_extvid(struct drm_i915_private *dev_priv, u8 pxvid)
+{
+	static const struct v_table {
+		u16 vd; /* in .1 mil */
+		u16 vm; /* in .1 mil */
+	} v_table[] = {
+		{ 0, 0, },
+		{ 375, 0, },
+		{ 500, 0, },
+		{ 625, 0, },
+		{ 750, 0, },
+		{ 875, 0, },
+		{ 1000, 0, },
+		{ 1125, 0, },
+		{ 4125, 3000, },
+		{ 4125, 3000, },
+		{ 4125, 3000, },
+		{ 4125, 3000, },
+		{ 4125, 3000, },
+		{ 4125, 3000, },
+		{ 4125, 3000, },
+		{ 4125, 3000, },
+		{ 4125, 3000, },
+		{ 4125, 3000, },
+		{ 4125, 3000, },
+		{ 4125, 3000, },
+		{ 4125, 3000, },
+		{ 4125, 3000, },
+		{ 4125, 3000, },
+		{ 4125, 3000, },
+		{ 4125, 3000, },
+		{ 4125, 3000, },
+		{ 4125, 3000, },
+		{ 4125, 3000, },
+		{ 4125, 3000, },
+		{ 4125, 3000, },
+		{ 4125, 3000, },
+		{ 4125, 3000, },
+		{ 4250, 3125, },
+		{ 4375, 3250, },
+		{ 4500, 3375, },
+		{ 4625, 3500, },
+		{ 4750, 3625, },
+		{ 4875, 3750, },
+		{ 5000, 3875, },
+		{ 5125, 4000, },
+		{ 5250, 4125, },
+		{ 5375, 4250, },
+		{ 5500, 4375, },
+		{ 5625, 4500, },
+		{ 5750, 4625, },
+		{ 5875, 4750, },
+		{ 6000, 4875, },
+		{ 6125, 5000, },
+		{ 6250, 5125, },
+		{ 6375, 5250, },
+		{ 6500, 5375, },
+		{ 6625, 5500, },
+		{ 6750, 5625, },
+		{ 6875, 5750, },
+		{ 7000, 5875, },
+		{ 7125, 6000, },
+		{ 7250, 6125, },
+		{ 7375, 6250, },
+		{ 7500, 6375, },
+		{ 7625, 6500, },
+		{ 7750, 6625, },
+		{ 7875, 6750, },
+		{ 8000, 6875, },
+		{ 8125, 7000, },
+		{ 8250, 7125, },
+		{ 8375, 7250, },
+		{ 8500, 7375, },
+		{ 8625, 7500, },
+		{ 8750, 7625, },
+		{ 8875, 7750, },
+		{ 9000, 7875, },
+		{ 9125, 8000, },
+		{ 9250, 8125, },
+		{ 9375, 8250, },
+		{ 9500, 8375, },
+		{ 9625, 8500, },
+		{ 9750, 8625, },
+		{ 9875, 8750, },
+		{ 10000, 8875, },
+		{ 10125, 9000, },
+		{ 10250, 9125, },
+		{ 10375, 9250, },
+		{ 10500, 9375, },
+		{ 10625, 9500, },
+		{ 10750, 9625, },
+		{ 10875, 9750, },
+		{ 11000, 9875, },
+		{ 11125, 10000, },
+		{ 11250, 10125, },
+		{ 11375, 10250, },
+		{ 11500, 10375, },
+		{ 11625, 10500, },
+		{ 11750, 10625, },
+		{ 11875, 10750, },
+		{ 12000, 10875, },
+		{ 12125, 11000, },
+		{ 12250, 11125, },
+		{ 12375, 11250, },
+		{ 12500, 11375, },
+		{ 12625, 11500, },
+		{ 12750, 11625, },
+		{ 12875, 11750, },
+		{ 13000, 11875, },
+		{ 13125, 12000, },
+		{ 13250, 12125, },
+		{ 13375, 12250, },
+		{ 13500, 12375, },
+		{ 13625, 12500, },
+		{ 13750, 12625, },
+		{ 13875, 12750, },
+		{ 14000, 12875, },
+		{ 14125, 13000, },
+		{ 14250, 13125, },
+		{ 14375, 13250, },
+		{ 14500, 13375, },
+		{ 14625, 13500, },
+		{ 14750, 13625, },
+		{ 14875, 13750, },
+		{ 15000, 13875, },
+		{ 15125, 14000, },
+		{ 15250, 14125, },
+		{ 15375, 14250, },
+		{ 15500, 14375, },
+		{ 15625, 14500, },
+		{ 15750, 14625, },
+		{ 15875, 14750, },
+		{ 16000, 14875, },
+		{ 16125, 15000, },
+	};
+	if (dev_priv->info->is_mobile)
+		return v_table[pxvid].vm;
+	else
+		return v_table[pxvid].vd;
+}
+
+void i915_update_gfx_val(struct drm_i915_private *dev_priv)
+{
+	struct timespec now, diff1;
+	u64 diff;
+	unsigned long diffms;
+	u32 count;
+
+	if (dev_priv->info->gen != 5)
+		return;
+
+	getrawmonotonic(&now);
+	diff1 = timespec_sub(now, dev_priv->last_time2);
+
+	/* Don't divide by 0 */
+	diffms = diff1.tv_sec * 1000 + diff1.tv_nsec / 1000000;
+	if (!diffms)
+		return;
+
+	count = I915_READ(GFXEC);
+
+	if (count < dev_priv->last_count2) {
+		diff = ~0UL - dev_priv->last_count2;
+		diff += count;
+	} else {
+		diff = count - dev_priv->last_count2;
+	}
+
+	dev_priv->last_count2 = count;
+	dev_priv->last_time2 = now;
+
+	/* More magic constants... */
+	diff = diff * 1181;
+	diff = div_u64(diff, diffms * 10);
+	dev_priv->gfx_power = diff;
+}
+
+unsigned long i915_gfx_val(struct drm_i915_private *dev_priv)
+{
+	unsigned long t, corr, state1, corr2, state2;
+	u32 pxvid, ext_v;
+
+	pxvid = I915_READ(PXVFREQ_BASE + (dev_priv->cur_delay * 4));
+	pxvid = (pxvid >> 24) & 0x7f;
+	ext_v = pvid_to_extvid(dev_priv, pxvid);
+
+	state1 = ext_v;
+
+	t = i915_mch_val(dev_priv);
+
+	/* Revel in the empirically derived constants */
+
+	/* Correction factor in 1/100000 units */
+	if (t > 80)
+		corr = ((t * 2349) + 135940);
+	else if (t >= 50)
+		corr = ((t * 964) + 29317);
+	else /* < 50 */
+		corr = ((t * 301) + 1004);
+
+	corr = corr * ((150142 * state1) / 10000 - 78642);
+	corr /= 100000;
+	corr2 = (corr * dev_priv->corr);
+
+	state2 = (corr2 * state1) / 10000;
+	state2 /= 100; /* convert to mW */
+
+	i915_update_gfx_val(dev_priv);
+
+	return dev_priv->gfx_power + state2;
+}
+
+/* Global for IPS driver to get at the current i915 device */
+static struct drm_i915_private *i915_mch_dev;
+/*
+ * Lock protecting IPS related data structures
+ *   - i915_mch_dev
+ *   - dev_priv->max_delay
+ *   - dev_priv->min_delay
+ *   - dev_priv->fmax
+ *   - dev_priv->gpu_busy
+ */
+static DEFINE_SPINLOCK(mchdev_lock);
+
+/**
+ * i915_read_mch_val - return value for IPS use
+ *
+ * Calculate and return a value for the IPS driver to use when deciding whether
+ * we have thermal and power headroom to increase CPU or GPU power budget.
+ */
+unsigned long i915_read_mch_val(void)
+{
+	struct drm_i915_private *dev_priv;
+	unsigned long chipset_val, graphics_val, ret = 0;
+
+	spin_lock(&mchdev_lock);
+	if (!i915_mch_dev)
+		goto out_unlock;
+	dev_priv = i915_mch_dev;
+
+	chipset_val = i915_chipset_val(dev_priv);
+	graphics_val = i915_gfx_val(dev_priv);
+
+	ret = chipset_val + graphics_val;
+
+out_unlock:
+	spin_unlock(&mchdev_lock);
+
+	return ret;
+}
+EXPORT_SYMBOL_GPL(i915_read_mch_val);
+
+/**
+ * i915_gpu_raise - raise GPU frequency limit
+ *
+ * Raise the limit; IPS indicates we have thermal headroom.
+ */
+bool i915_gpu_raise(void)
+{
+	struct drm_i915_private *dev_priv;
+	bool ret = true;
+
+	spin_lock(&mchdev_lock);
+	if (!i915_mch_dev) {
+		ret = false;
+		goto out_unlock;
+	}
+	dev_priv = i915_mch_dev;
+
+	if (dev_priv->max_delay > dev_priv->fmax)
+		dev_priv->max_delay--;
+
+out_unlock:
+	spin_unlock(&mchdev_lock);
+
+	return ret;
+}
+EXPORT_SYMBOL_GPL(i915_gpu_raise);
+
+/**
+ * i915_gpu_lower - lower GPU frequency limit
+ *
+ * IPS indicates we're close to a thermal limit, so throttle back the GPU
+ * frequency maximum.
+ */
+bool i915_gpu_lower(void)
+{
+	struct drm_i915_private *dev_priv;
+	bool ret = true;
+
+	spin_lock(&mchdev_lock);
+	if (!i915_mch_dev) {
+		ret = false;
+		goto out_unlock;
+	}
+	dev_priv = i915_mch_dev;
+
+	if (dev_priv->max_delay < dev_priv->min_delay)
+		dev_priv->max_delay++;
+
+out_unlock:
+	spin_unlock(&mchdev_lock);
+
+	return ret;
+}
+EXPORT_SYMBOL_GPL(i915_gpu_lower);
+
+/**
+ * i915_gpu_busy - indicate GPU business to IPS
+ *
+ * Tell the IPS driver whether or not the GPU is busy.
+ */
+bool i915_gpu_busy(void)
+{
+	struct drm_i915_private *dev_priv;
+	bool ret = false;
+
+	spin_lock(&mchdev_lock);
+	if (!i915_mch_dev)
+		goto out_unlock;
+	dev_priv = i915_mch_dev;
+
+	ret = dev_priv->busy;
+
+out_unlock:
+	spin_unlock(&mchdev_lock);
+
+	return ret;
+}
+EXPORT_SYMBOL_GPL(i915_gpu_busy);
+
+/**
+ * i915_gpu_turbo_disable - disable graphics turbo
+ *
+ * Disable graphics turbo by resetting the max frequency and setting the
+ * current frequency to the default.
+ */
+bool i915_gpu_turbo_disable(void)
+{
+	struct drm_i915_private *dev_priv;
+	bool ret = true;
+
+	spin_lock(&mchdev_lock);
+	if (!i915_mch_dev) {
+		ret = false;
+		goto out_unlock;
+	}
+	dev_priv = i915_mch_dev;
+
+	dev_priv->max_delay = dev_priv->fstart;
+
+	if (!ironlake_set_drps(dev_priv->dev, dev_priv->fstart))
+		ret = false;
+
+out_unlock:
+	spin_unlock(&mchdev_lock);
+
+	return ret;
+}
+EXPORT_SYMBOL_GPL(i915_gpu_turbo_disable);
+
+/**
+ * Tells the intel_ips driver that the i915 driver is now loaded, if
+ * IPS got loaded first.
+ *
+ * This awkward dance is so that neither module has to depend on the
+ * other in order for IPS to do the appropriate communication of
+ * GPU turbo limits to i915.
+ */
+static void
+ips_ping_for_i915_load(void)
+{
+	void (*link)(void);
+
+	link = symbol_get(ips_link_to_i915_driver);
+	if (link) {
+		link();
+		symbol_put(ips_link_to_i915_driver);
+	}
+}
+
+void intel_gpu_ips_init(struct drm_i915_private *dev_priv)
+{
+	spin_lock(&mchdev_lock);
+	i915_mch_dev = dev_priv;
+	dev_priv->mchdev_lock = &mchdev_lock;
+	spin_unlock(&mchdev_lock);
+
+	ips_ping_for_i915_load();
+}
+
+void intel_gpu_ips_teardown(void)
+{
+	spin_lock(&mchdev_lock);
+	i915_mch_dev = NULL;
+	spin_unlock(&mchdev_lock);
+}
+
+void intel_init_emon(struct drm_device *dev)
+{
+	struct drm_i915_private *dev_priv = dev->dev_private;
+	u32 lcfuse;
+	u8 pxw[16];
+	int i;
+
+	/* Disable to program */
+	I915_WRITE(ECR, 0);
+	POSTING_READ(ECR);
+
+	/* Program energy weights for various events */
+	I915_WRITE(SDEW, 0x15040d00);
+	I915_WRITE(CSIEW0, 0x007f0000);
+	I915_WRITE(CSIEW1, 0x1e220004);
+	I915_WRITE(CSIEW2, 0x04000004);
+
+	for (i = 0; i < 5; i++)
+		I915_WRITE(PEW + (i * 4), 0);
+	for (i = 0; i < 3; i++)
+		I915_WRITE(DEW + (i * 4), 0);
+
+	/* Program P-state weights to account for frequency power adjustment */
+	for (i = 0; i < 16; i++) {
+		u32 pxvidfreq = I915_READ(PXVFREQ_BASE + (i * 4));
+		unsigned long freq = intel_pxfreq(pxvidfreq);
+		unsigned long vid = (pxvidfreq & PXVFREQ_PX_MASK) >>
+			PXVFREQ_PX_SHIFT;
+		unsigned long val;
+
+		val = vid * vid;
+		val *= (freq / 1000);
+		val *= 255;
+		val /= (127*127*900);
+		if (val > 0xff)
+			DRM_ERROR("bad pxval: %ld\n", val);
+		pxw[i] = val;
+	}
+	/* Render standby states get 0 weight */
+	pxw[14] = 0;
+	pxw[15] = 0;
+
+	for (i = 0; i < 4; i++) {
+		u32 val = (pxw[i*4] << 24) | (pxw[(i*4)+1] << 16) |
+			(pxw[(i*4)+2] << 8) | (pxw[(i*4)+3]);
+		I915_WRITE(PXW + (i * 4), val);
+	}
+
+	/* Adjust magic regs to magic values (more experimental results) */
+	I915_WRITE(OGW0, 0);
+	I915_WRITE(OGW1, 0);
+	I915_WRITE(EG0, 0x00007f00);
+	I915_WRITE(EG1, 0x0000000e);
+	I915_WRITE(EG2, 0x000e0000);
+	I915_WRITE(EG3, 0x68000300);
+	I915_WRITE(EG4, 0x42000000);
+	I915_WRITE(EG5, 0x00140031);
+	I915_WRITE(EG6, 0);
+	I915_WRITE(EG7, 0);
+
+	for (i = 0; i < 8; i++)
+		I915_WRITE(PXWL + (i * 4), 0);
+
+	/* Enable PMON + select events */
+	I915_WRITE(ECR, 0x80000019);
+
+	lcfuse = I915_READ(LCFUSE02);
+
+	dev_priv->corr = (lcfuse & LCFUSE_HIV_MASK);
+}
+
+static void ironlake_init_clock_gating(struct drm_device *dev)
+{
+	struct drm_i915_private *dev_priv = dev->dev_private;
+	uint32_t dspclk_gate = VRHUNIT_CLOCK_GATE_DISABLE;
+
+	/* Required for FBC */
+	dspclk_gate |= DPFCUNIT_CLOCK_GATE_DISABLE |
+		DPFCRUNIT_CLOCK_GATE_DISABLE |
+		DPFDUNIT_CLOCK_GATE_DISABLE;
+	/* Required for CxSR */
+	dspclk_gate |= DPARBUNIT_CLOCK_GATE_DISABLE;
+
+	I915_WRITE(PCH_3DCGDIS0,
+		   MARIUNIT_CLOCK_GATE_DISABLE |
+		   SVSMUNIT_CLOCK_GATE_DISABLE);
+	I915_WRITE(PCH_3DCGDIS1,
+		   VFMUNIT_CLOCK_GATE_DISABLE);
+
+	I915_WRITE(PCH_DSPCLK_GATE_D, dspclk_gate);
+
+	/*
+	 * According to the spec the following bits should be set in
+	 * order to enable memory self-refresh
+	 * The bit 22/21 of 0x42004
+	 * The bit 5 of 0x42020
+	 * The bit 15 of 0x45000
+	 */
+	I915_WRITE(ILK_DISPLAY_CHICKEN2,
+		   (I915_READ(ILK_DISPLAY_CHICKEN2) |
+		    ILK_DPARB_GATE | ILK_VSDPFD_FULL));
+	I915_WRITE(ILK_DSPCLK_GATE,
+		   (I915_READ(ILK_DSPCLK_GATE) |
+		    ILK_DPARB_CLK_GATE));
+	I915_WRITE(DISP_ARB_CTL,
+		   (I915_READ(DISP_ARB_CTL) |
+		    DISP_FBC_WM_DIS));
+	I915_WRITE(WM3_LP_ILK, 0);
+	I915_WRITE(WM2_LP_ILK, 0);
+	I915_WRITE(WM1_LP_ILK, 0);
+
+	/*
+	 * Based on the document from hardware guys the following bits
+	 * should be set unconditionally in order to enable FBC.
+	 * The bit 22 of 0x42000
+	 * The bit 22 of 0x42004
+	 * The bit 7,8,9 of 0x42020.
+	 */
+	if (IS_IRONLAKE_M(dev)) {
+		I915_WRITE(ILK_DISPLAY_CHICKEN1,
+			   I915_READ(ILK_DISPLAY_CHICKEN1) |
+			   ILK_FBCQ_DIS);
+		I915_WRITE(ILK_DISPLAY_CHICKEN2,
+			   I915_READ(ILK_DISPLAY_CHICKEN2) |
+			   ILK_DPARB_GATE);
+		I915_WRITE(ILK_DSPCLK_GATE,
+			   I915_READ(ILK_DSPCLK_GATE) |
+			   ILK_DPFC_DIS1 |
+			   ILK_DPFC_DIS2 |
+			   ILK_CLK_FBC);
+	}
+
+	I915_WRITE(ILK_DISPLAY_CHICKEN2,
+		   I915_READ(ILK_DISPLAY_CHICKEN2) |
+		   ILK_ELPIN_409_SELECT);
+	I915_WRITE(_3D_CHICKEN2,
+		   _3D_CHICKEN2_WM_READ_PIPELINED << 16 |
+		   _3D_CHICKEN2_WM_READ_PIPELINED);
+}
+
+static void gen6_init_clock_gating(struct drm_device *dev)
+{
+	struct drm_i915_private *dev_priv = dev->dev_private;
+	int pipe;
+	uint32_t dspclk_gate = VRHUNIT_CLOCK_GATE_DISABLE;
+
+	I915_WRITE(PCH_DSPCLK_GATE_D, dspclk_gate);
+
+	I915_WRITE(ILK_DISPLAY_CHICKEN2,
+		   I915_READ(ILK_DISPLAY_CHICKEN2) |
+		   ILK_ELPIN_409_SELECT);
+
+	I915_WRITE(WM3_LP_ILK, 0);
+	I915_WRITE(WM2_LP_ILK, 0);
+	I915_WRITE(WM1_LP_ILK, 0);
+
+	I915_WRITE(CACHE_MODE_0,
+		   _MASKED_BIT_DISABLE(CM0_STC_EVICT_DISABLE_LRA_SNB));
+
+	I915_WRITE(GEN6_UCGCTL1,
+		   I915_READ(GEN6_UCGCTL1) |
+		   GEN6_BLBUNIT_CLOCK_GATE_DISABLE |
+		   GEN6_CSUNIT_CLOCK_GATE_DISABLE);
+
+	/* According to the BSpec vol1g, bit 12 (RCPBUNIT) clock
+	 * gating disable must be set.  Failure to set it results in
+	 * flickering pixels due to Z write ordering failures after
+	 * some amount of runtime in the Mesa "fire" demo, and Unigine
+	 * Sanctuary and Tropics, and apparently anything else with
+	 * alpha test or pixel discard.
+	 *
+	 * According to the spec, bit 11 (RCCUNIT) must also be set,
+	 * but we didn't debug actual testcases to find it out.
+	 */
+	I915_WRITE(GEN6_UCGCTL2,
+		   GEN6_RCPBUNIT_CLOCK_GATE_DISABLE |
+		   GEN6_RCCUNIT_CLOCK_GATE_DISABLE);
+
+	/* Bspec says we need to always set all mask bits. */
+	I915_WRITE(_3D_CHICKEN, (0xFFFF << 16) |
+		   _3D_CHICKEN_SF_DISABLE_FASTCLIP_CULL);
+
+	/*
+	 * According to the spec the following bits should be
+	 * set in order to enable memory self-refresh and fbc:
+	 * The bit21 and bit22 of 0x42000
+	 * The bit21 and bit22 of 0x42004
+	 * The bit5 and bit7 of 0x42020
+	 * The bit14 of 0x70180
+	 * The bit14 of 0x71180
+	 */
+	I915_WRITE(ILK_DISPLAY_CHICKEN1,
+		   I915_READ(ILK_DISPLAY_CHICKEN1) |
+		   ILK_FBCQ_DIS | ILK_PABSTRETCH_DIS);
+	I915_WRITE(ILK_DISPLAY_CHICKEN2,
+		   I915_READ(ILK_DISPLAY_CHICKEN2) |
+		   ILK_DPARB_GATE | ILK_VSDPFD_FULL);
+	I915_WRITE(ILK_DSPCLK_GATE,
+		   I915_READ(ILK_DSPCLK_GATE) |
+		   ILK_DPARB_CLK_GATE  |
+		   ILK_DPFD_CLK_GATE);
+
+	for_each_pipe(pipe) {
+		I915_WRITE(DSPCNTR(pipe),
+			   I915_READ(DSPCNTR(pipe)) |
+			   DISPPLANE_TRICKLE_FEED_DISABLE);
+		intel_flush_display_plane(dev_priv, pipe);
+	}
+}
+
+static void gen7_setup_fixed_func_scheduler(struct drm_i915_private *dev_priv)
+{
+	uint32_t reg = I915_READ(GEN7_FF_THREAD_MODE);
+
+	reg &= ~GEN7_FF_SCHED_MASK;
+	reg |= GEN7_FF_TS_SCHED_HW;
+	reg |= GEN7_FF_VS_SCHED_HW;
+	reg |= GEN7_FF_DS_SCHED_HW;
+
+	I915_WRITE(GEN7_FF_THREAD_MODE, reg);
+}
+
+static void ivybridge_init_clock_gating(struct drm_device *dev)
+{
+	struct drm_i915_private *dev_priv = dev->dev_private;
+	int pipe;
+	uint32_t dspclk_gate = VRHUNIT_CLOCK_GATE_DISABLE;
+
+	I915_WRITE(PCH_DSPCLK_GATE_D, dspclk_gate);
+
+	I915_WRITE(WM3_LP_ILK, 0);
+	I915_WRITE(WM2_LP_ILK, 0);
+	I915_WRITE(WM1_LP_ILK, 0);
+
+	/* According to the spec, bit 13 (RCZUNIT) must be set on IVB.
+	 * This implements the WaDisableRCZUnitClockGating workaround.
+	 */
+	I915_WRITE(GEN6_UCGCTL2, GEN6_RCZUNIT_CLOCK_GATE_DISABLE);
+
+	I915_WRITE(ILK_DSPCLK_GATE, IVB_VRHUNIT_CLK_GATE);
+
+	I915_WRITE(IVB_CHICKEN3,
+		   CHICKEN3_DGMG_REQ_OUT_FIX_DISABLE |
+		   CHICKEN3_DGMG_DONE_FIX_DISABLE);
+
+	/* Apply the WaDisableRHWOOptimizationForRenderHang workaround. */
+	I915_WRITE(GEN7_COMMON_SLICE_CHICKEN1,
+		   GEN7_CSC1_RHWO_OPT_DISABLE_IN_RCC);
+
+	/* WaApplyL3ControlAndL3ChickenMode requires those two on Ivy Bridge */
+	I915_WRITE(GEN7_L3CNTLREG1,
+			GEN7_WA_FOR_GEN7_L3_CONTROL);
+	I915_WRITE(GEN7_L3_CHICKEN_MODE_REGISTER,
+			GEN7_WA_L3_CHICKEN_MODE);
+
+	/* This is required by WaCatErrorRejectionIssue */
+	I915_WRITE(GEN7_SQ_CHICKEN_MBCUNIT_CONFIG,
+			I915_READ(GEN7_SQ_CHICKEN_MBCUNIT_CONFIG) |
+			GEN7_SQ_CHICKEN_MBCUNIT_SQINTMOB);
+
+	for_each_pipe(pipe) {
+		I915_WRITE(DSPCNTR(pipe),
+			   I915_READ(DSPCNTR(pipe)) |
+			   DISPPLANE_TRICKLE_FEED_DISABLE);
+		intel_flush_display_plane(dev_priv, pipe);
+	}
+
+	gen7_setup_fixed_func_scheduler(dev_priv);
+
+	/* WaDisable4x2SubspanOptimization */
+	I915_WRITE(CACHE_MODE_1,
+		   _MASKED_BIT_ENABLE(PIXEL_SUBSPAN_COLLECT_OPT_DISABLE));
+}
+
+static void valleyview_init_clock_gating(struct drm_device *dev)
+{
+	struct drm_i915_private *dev_priv = dev->dev_private;
+	int pipe;
+	uint32_t dspclk_gate = VRHUNIT_CLOCK_GATE_DISABLE;
+
+	I915_WRITE(PCH_DSPCLK_GATE_D, dspclk_gate);
+
+	I915_WRITE(WM3_LP_ILK, 0);
+	I915_WRITE(WM2_LP_ILK, 0);
+	I915_WRITE(WM1_LP_ILK, 0);
+
+	/* According to the spec, bit 13 (RCZUNIT) must be set on IVB.
+	 * This implements the WaDisableRCZUnitClockGating workaround.
+	 */
+	I915_WRITE(GEN6_UCGCTL2, GEN6_RCZUNIT_CLOCK_GATE_DISABLE);
+
+	I915_WRITE(ILK_DSPCLK_GATE, IVB_VRHUNIT_CLK_GATE);
+
+	I915_WRITE(IVB_CHICKEN3,
+		   CHICKEN3_DGMG_REQ_OUT_FIX_DISABLE |
+		   CHICKEN3_DGMG_DONE_FIX_DISABLE);
+
+	/* Apply the WaDisableRHWOOptimizationForRenderHang workaround. */
+	I915_WRITE(GEN7_COMMON_SLICE_CHICKEN1,
+		   GEN7_CSC1_RHWO_OPT_DISABLE_IN_RCC);
+
+	/* WaApplyL3ControlAndL3ChickenMode requires those two on Ivy Bridge */
+	I915_WRITE(GEN7_L3CNTLREG1, GEN7_WA_FOR_GEN7_L3_CONTROL);
+	I915_WRITE(GEN7_L3_CHICKEN_MODE_REGISTER, GEN7_WA_L3_CHICKEN_MODE);
+
+	/* This is required by WaCatErrorRejectionIssue */
+	I915_WRITE(GEN7_SQ_CHICKEN_MBCUNIT_CONFIG,
+		   I915_READ(GEN7_SQ_CHICKEN_MBCUNIT_CONFIG) |
+		   GEN7_SQ_CHICKEN_MBCUNIT_SQINTMOB);
+
+	for_each_pipe(pipe) {
+		I915_WRITE(DSPCNTR(pipe),
+			   I915_READ(DSPCNTR(pipe)) |
+			   DISPPLANE_TRICKLE_FEED_DISABLE);
+		intel_flush_display_plane(dev_priv, pipe);
+	}
+
+	I915_WRITE(CACHE_MODE_1,
+		   _MASKED_BIT_ENABLE(PIXEL_SUBSPAN_COLLECT_OPT_DISABLE));
+}
+
+static void g4x_init_clock_gating(struct drm_device *dev)
+{
+	struct drm_i915_private *dev_priv = dev->dev_private;
+	uint32_t dspclk_gate;
+
+	I915_WRITE(RENCLK_GATE_D1, 0);
+	I915_WRITE(RENCLK_GATE_D2, VF_UNIT_CLOCK_GATE_DISABLE |
+		   GS_UNIT_CLOCK_GATE_DISABLE |
+		   CL_UNIT_CLOCK_GATE_DISABLE);
+	I915_WRITE(RAMCLK_GATE_D, 0);
+	dspclk_gate = VRHUNIT_CLOCK_GATE_DISABLE |
+		OVRUNIT_CLOCK_GATE_DISABLE |
+		OVCUNIT_CLOCK_GATE_DISABLE;
+	if (IS_GM45(dev))
+		dspclk_gate |= DSSUNIT_CLOCK_GATE_DISABLE;
+	I915_WRITE(DSPCLK_GATE_D, dspclk_gate);
+}
+
+static void crestline_init_clock_gating(struct drm_device *dev)
+{
+	struct drm_i915_private *dev_priv = dev->dev_private;
+
+	I915_WRITE(RENCLK_GATE_D1, I965_RCC_CLOCK_GATE_DISABLE);
+	I915_WRITE(RENCLK_GATE_D2, 0);
+	I915_WRITE(DSPCLK_GATE_D, 0);
+	I915_WRITE(RAMCLK_GATE_D, 0);
+	I915_WRITE16(DEUC, 0);
+}
+
+static void broadwater_init_clock_gating(struct drm_device *dev)
+{
+	struct drm_i915_private *dev_priv = dev->dev_private;
+
+	I915_WRITE(RENCLK_GATE_D1, I965_RCZ_CLOCK_GATE_DISABLE |
+		   I965_RCC_CLOCK_GATE_DISABLE |
+		   I965_RCPB_CLOCK_GATE_DISABLE |
+		   I965_ISC_CLOCK_GATE_DISABLE |
+		   I965_FBC_CLOCK_GATE_DISABLE);
+	I915_WRITE(RENCLK_GATE_D2, 0);
+}
+
+static void gen3_init_clock_gating(struct drm_device *dev)
+{
+	struct drm_i915_private *dev_priv = dev->dev_private;
+	u32 dstate = I915_READ(D_STATE);
+
+	dstate |= DSTATE_PLL_D3_OFF | DSTATE_GFX_CLOCK_GATING |
+		DSTATE_DOT_CLOCK_GATING;
+	I915_WRITE(D_STATE, dstate);
+
+	if (IS_PINEVIEW(dev))
+		I915_WRITE(ECOSKPD, _MASKED_BIT_ENABLE(ECO_GATING_CX_ONLY));
+}
+
+static void i85x_init_clock_gating(struct drm_device *dev)
+{
+	struct drm_i915_private *dev_priv = dev->dev_private;
+
+	I915_WRITE(RENCLK_GATE_D1, SV_CLOCK_GATE_DISABLE);
+}
+
+static void i830_init_clock_gating(struct drm_device *dev)
+{
+	struct drm_i915_private *dev_priv = dev->dev_private;
+
+	I915_WRITE(DSPCLK_GATE_D, OVRUNIT_CLOCK_GATE_DISABLE);
+}
+
+static void ibx_init_clock_gating(struct drm_device *dev)
+{
+	struct drm_i915_private *dev_priv = dev->dev_private;
+
+	/*
+	 * On Ibex Peak and Cougar Point, we need to disable clock
+	 * gating for the panel power sequencer or it will fail to
+	 * start up when no ports are active.
+	 */
+	I915_WRITE(SOUTH_DSPCLK_GATE_D, PCH_DPLSUNIT_CLOCK_GATE_DISABLE);
+}
+
+static void cpt_init_clock_gating(struct drm_device *dev)
+{
+	struct drm_i915_private *dev_priv = dev->dev_private;
+	int pipe;
+
+	/*
+	 * On Ibex Peak and Cougar Point, we need to disable clock
+	 * gating for the panel power sequencer or it will fail to
+	 * start up when no ports are active.
+	 */
+	I915_WRITE(SOUTH_DSPCLK_GATE_D, PCH_DPLSUNIT_CLOCK_GATE_DISABLE);
+	I915_WRITE(SOUTH_CHICKEN2, I915_READ(SOUTH_CHICKEN2) |
+		   DPLS_EDP_PPS_FIX_DIS);
+	/* Without this, mode sets may fail silently on FDI */
+	for_each_pipe(pipe)
+		I915_WRITE(TRANS_CHICKEN2(pipe), TRANS_AUTOTRAIN_GEN_STALL_DIS);
+}
+
+void intel_init_clock_gating(struct drm_device *dev)
+{
+	struct drm_i915_private *dev_priv = dev->dev_private;
+
+	dev_priv->display.init_clock_gating(dev);
+
+	if (dev_priv->display.init_pch_clock_gating)
+		dev_priv->display.init_pch_clock_gating(dev);
+}
+
+static void gen6_sanitize_pm(struct drm_device *dev)
+{
+	struct drm_i915_private *dev_priv = dev->dev_private;
+	u32 limits, delay, old;
+
+	gen6_gt_force_wake_get(dev_priv);
+
+	old = limits = I915_READ(GEN6_RP_INTERRUPT_LIMITS);
+	/* Make sure we continue to get interrupts
+	 * until we hit the minimum or maximum frequencies.
+	 */
+	limits &= ~(0x3f << 16 | 0x3f << 24);
+	delay = dev_priv->cur_delay;
+	if (delay < dev_priv->max_delay)
+		limits |= (dev_priv->max_delay & 0x3f) << 24;
+	if (delay > dev_priv->min_delay)
+		limits |= (dev_priv->min_delay & 0x3f) << 16;
+
+	if (old != limits) {
+		DRM_ERROR("Power management discrepancy: GEN6_RP_INTERRUPT_LIMITS expected %08x, was %08x\n",
+			  limits, old);
+		I915_WRITE(GEN6_RP_INTERRUPT_LIMITS, limits);
+	}
+
+	gen6_gt_force_wake_put(dev_priv);
+}
+
+void intel_sanitize_pm(struct drm_device *dev)
+{
+	struct drm_i915_private *dev_priv = dev->dev_private;
+
+	if (dev_priv->display.sanitize_pm)
+		dev_priv->display.sanitize_pm(dev);
+}
+
+/* Starting with Haswell, we have different power wells for
+ * different parts of the GPU. This attempts to enable them all.
+ */
+void intel_init_power_wells(struct drm_device *dev)
+{
+	struct drm_i915_private *dev_priv = dev->dev_private;
+	unsigned long power_wells[] = {
+		HSW_PWR_WELL_CTL1,
+		HSW_PWR_WELL_CTL2,
+		HSW_PWR_WELL_CTL4
+	};
+	int i;
+
+	if (!IS_HASWELL(dev))
+		return;
+
+	mutex_lock(&dev->struct_mutex);
+
+	for (i = 0; i < ARRAY_SIZE(power_wells); i++) {
+		int well = I915_READ(power_wells[i]);
+
+		if ((well & HSW_PWR_WELL_STATE) == 0) {
+			I915_WRITE(power_wells[i], well & HSW_PWR_WELL_ENABLE);
+			if (wait_for(I915_READ(power_wells[i] & HSW_PWR_WELL_STATE), 20))
+				DRM_ERROR("Error enabling power well %lx\n", power_wells[i]);
+		}
+	}
+
+	mutex_unlock(&dev->struct_mutex);
+}
+
+/* Set up chip specific power management-related functions */
+void intel_init_pm(struct drm_device *dev)
+{
+	struct drm_i915_private *dev_priv = dev->dev_private;
+
+	if (I915_HAS_FBC(dev)) {
+		if (HAS_PCH_SPLIT(dev)) {
+			dev_priv->display.fbc_enabled = ironlake_fbc_enabled;
+			dev_priv->display.enable_fbc = ironlake_enable_fbc;
+			dev_priv->display.disable_fbc = ironlake_disable_fbc;
+		} else if (IS_GM45(dev)) {
+			dev_priv->display.fbc_enabled = g4x_fbc_enabled;
+			dev_priv->display.enable_fbc = g4x_enable_fbc;
+			dev_priv->display.disable_fbc = g4x_disable_fbc;
+		} else if (IS_CRESTLINE(dev)) {
+			dev_priv->display.fbc_enabled = i8xx_fbc_enabled;
+			dev_priv->display.enable_fbc = i8xx_enable_fbc;
+			dev_priv->display.disable_fbc = i8xx_disable_fbc;
+		}
+		/* 855GM needs testing */
+	}
+
+	/* For cxsr */
+	if (IS_PINEVIEW(dev))
+		i915_pineview_get_mem_freq(dev);
+	else if (IS_GEN5(dev))
+		i915_ironlake_get_mem_freq(dev);
+
+	/* For FIFO watermark updates */
+	if (HAS_PCH_SPLIT(dev)) {
+		dev_priv->display.force_wake_get = __gen6_gt_force_wake_get;
+		dev_priv->display.force_wake_put = __gen6_gt_force_wake_put;
+
+		/* IVB configs may use multi-threaded forcewake */
+		if (IS_IVYBRIDGE(dev) || IS_HASWELL(dev)) {
+			u32	ecobus;
+
+			/* A small trick here - if the bios hasn't configured MT forcewake,
+			 * and if the device is in RC6, then force_wake_mt_get will not wake
+			 * the device and the ECOBUS read will return zero. Which will be
+			 * (correctly) interpreted by the test below as MT forcewake being
+			 * disabled.
+			 */
+			mutex_lock(&dev->struct_mutex);
+			__gen6_gt_force_wake_mt_get(dev_priv);
+			ecobus = I915_READ_NOTRACE(ECOBUS);
+			__gen6_gt_force_wake_mt_put(dev_priv);
+			mutex_unlock(&dev->struct_mutex);
+
+			if (ecobus & FORCEWAKE_MT_ENABLE) {
+				DRM_DEBUG_KMS("Using MT version of forcewake\n");
+				dev_priv->display.force_wake_get =
+					__gen6_gt_force_wake_mt_get;
+				dev_priv->display.force_wake_put =
+					__gen6_gt_force_wake_mt_put;
+			}
+		}
+
+		if (HAS_PCH_IBX(dev))
+			dev_priv->display.init_pch_clock_gating = ibx_init_clock_gating;
+		else if (HAS_PCH_CPT(dev))
+			dev_priv->display.init_pch_clock_gating = cpt_init_clock_gating;
+
+		if (IS_GEN5(dev)) {
+			if (I915_READ(MLTR_ILK) & ILK_SRLT_MASK)
+				dev_priv->display.update_wm = ironlake_update_wm;
+			else {
+				DRM_DEBUG_KMS("Failed to get proper latency. "
+					      "Disable CxSR\n");
+				dev_priv->display.update_wm = NULL;
+			}
+			dev_priv->display.init_clock_gating = ironlake_init_clock_gating;
+		} else if (IS_GEN6(dev)) {
+			if (SNB_READ_WM0_LATENCY()) {
+				dev_priv->display.update_wm = sandybridge_update_wm;
+				dev_priv->display.update_sprite_wm = sandybridge_update_sprite_wm;
+			} else {
+				DRM_DEBUG_KMS("Failed to read display plane latency. "
+					      "Disable CxSR\n");
+				dev_priv->display.update_wm = NULL;
+			}
+			dev_priv->display.init_clock_gating = gen6_init_clock_gating;
+			dev_priv->display.sanitize_pm = gen6_sanitize_pm;
+		} else if (IS_IVYBRIDGE(dev)) {
+			/* FIXME: detect B0+ stepping and use auto training */
+			if (SNB_READ_WM0_LATENCY()) {
+				dev_priv->display.update_wm = sandybridge_update_wm;
+				dev_priv->display.update_sprite_wm = sandybridge_update_sprite_wm;
+			} else {
+				DRM_DEBUG_KMS("Failed to read display plane latency. "
+					      "Disable CxSR\n");
+				dev_priv->display.update_wm = NULL;
+			}
+			dev_priv->display.init_clock_gating = ivybridge_init_clock_gating;
+			dev_priv->display.sanitize_pm = gen6_sanitize_pm;
+		} else if (IS_HASWELL(dev)) {
+			if (SNB_READ_WM0_LATENCY()) {
+				dev_priv->display.update_wm = sandybridge_update_wm;
+				dev_priv->display.update_sprite_wm = sandybridge_update_sprite_wm;
+				dev_priv->display.update_linetime_wm = haswell_update_linetime_wm;
+			} else {
+				DRM_DEBUG_KMS("Failed to read display plane latency. "
+					      "Disable CxSR\n");
+				dev_priv->display.update_wm = NULL;
+			}
+			dev_priv->display.init_clock_gating = ivybridge_init_clock_gating;
+			dev_priv->display.sanitize_pm = gen6_sanitize_pm;
+		} else
+			dev_priv->display.update_wm = NULL;
+	} else if (IS_VALLEYVIEW(dev)) {
+		dev_priv->display.update_wm = valleyview_update_wm;
+		dev_priv->display.init_clock_gating =
+			valleyview_init_clock_gating;
+		dev_priv->display.force_wake_get = vlv_force_wake_get;
+		dev_priv->display.force_wake_put = vlv_force_wake_put;
+	} else if (IS_PINEVIEW(dev)) {
+		if (!intel_get_cxsr_latency(IS_PINEVIEW_G(dev),
+					    dev_priv->is_ddr3,
+					    dev_priv->fsb_freq,
+					    dev_priv->mem_freq)) {
+			DRM_INFO("failed to find known CxSR latency "
+				 "(found ddr%s fsb freq %d, mem freq %d), "
+				 "disabling CxSR\n",
+				 (dev_priv->is_ddr3 == 1) ? "3" : "2",
+				 dev_priv->fsb_freq, dev_priv->mem_freq);
+			/* Disable CxSR and never update its watermark again */
+			pineview_disable_cxsr(dev);
+			dev_priv->display.update_wm = NULL;
+		} else
+			dev_priv->display.update_wm = pineview_update_wm;
+		dev_priv->display.init_clock_gating = gen3_init_clock_gating;
+	} else if (IS_G4X(dev)) {
+		dev_priv->display.update_wm = g4x_update_wm;
+		dev_priv->display.init_clock_gating = g4x_init_clock_gating;
+	} else if (IS_GEN4(dev)) {
+		dev_priv->display.update_wm = i965_update_wm;
+		if (IS_CRESTLINE(dev))
+			dev_priv->display.init_clock_gating = crestline_init_clock_gating;
+		else if (IS_BROADWATER(dev))
+			dev_priv->display.init_clock_gating = broadwater_init_clock_gating;
+	} else if (IS_GEN3(dev)) {
+		dev_priv->display.update_wm = i9xx_update_wm;
+		dev_priv->display.get_fifo_size = i9xx_get_fifo_size;
+		dev_priv->display.init_clock_gating = gen3_init_clock_gating;
+	} else if (IS_I865G(dev)) {
+		dev_priv->display.update_wm = i830_update_wm;
+		dev_priv->display.init_clock_gating = i85x_init_clock_gating;
+		dev_priv->display.get_fifo_size = i830_get_fifo_size;
+	} else if (IS_I85X(dev)) {
+		dev_priv->display.update_wm = i9xx_update_wm;
+		dev_priv->display.get_fifo_size = i85x_get_fifo_size;
+		dev_priv->display.init_clock_gating = i85x_init_clock_gating;
+	} else {
+		dev_priv->display.update_wm = i830_update_wm;
+		dev_priv->display.init_clock_gating = i830_init_clock_gating;
+		if (IS_845G(dev))
+			dev_priv->display.get_fifo_size = i845_get_fifo_size;
+		else
+			dev_priv->display.get_fifo_size = i830_get_fifo_size;
+	}
+
+	/* We attempt to init the necessary power wells early in the initialization
+	 * time, so the subsystems that expect power to be enabled can work.
+	 */
+	intel_init_power_wells(dev);
+}
+
diff --git a/drivers/gpu/drm/i915/intel_ringbuffer.c b/drivers/gpu/drm/i915/intel_ringbuffer.c
index 80fce51e2f4..b59b6d5b758 100644
--- a/drivers/gpu/drm/i915/intel_ringbuffer.c
+++ b/drivers/gpu/drm/i915/intel_ringbuffer.c
@@ -53,9 +53,35 @@ static inline int ring_space(struct intel_ring_buffer *ring)
 }
 
 static int
-render_ring_flush(struct intel_ring_buffer *ring,
-		  u32	invalidate_domains,
-		  u32	flush_domains)
+gen2_render_ring_flush(struct intel_ring_buffer *ring,
+		       u32	invalidate_domains,
+		       u32	flush_domains)
+{
+	u32 cmd;
+	int ret;
+
+	cmd = MI_FLUSH;
+	if (((invalidate_domains|flush_domains) & I915_GEM_DOMAIN_RENDER) == 0)
+		cmd |= MI_NO_WRITE_FLUSH;
+
+	if (invalidate_domains & I915_GEM_DOMAIN_SAMPLER)
+		cmd |= MI_READ_FLUSH;
+
+	ret = intel_ring_begin(ring, 2);
+	if (ret)
+		return ret;
+
+	intel_ring_emit(ring, cmd);
+	intel_ring_emit(ring, MI_NOOP);
+	intel_ring_advance(ring);
+
+	return 0;
+}
+
+static int
+gen4_render_ring_flush(struct intel_ring_buffer *ring,
+		       u32	invalidate_domains,
+		       u32	flush_domains)
 {
 	struct drm_device *dev = ring->dev;
 	u32 cmd;
@@ -90,17 +116,8 @@ render_ring_flush(struct intel_ring_buffer *ring,
 	 */
 
 	cmd = MI_FLUSH | MI_NO_WRITE_FLUSH;
-	if ((invalidate_domains|flush_domains) &
-	    I915_GEM_DOMAIN_RENDER)
+	if ((invalidate_domains|flush_domains) & I915_GEM_DOMAIN_RENDER)
 		cmd &= ~MI_NO_WRITE_FLUSH;
-	if (INTEL_INFO(dev)->gen < 4) {
-		/*
-		 * On the 965, the sampler cache always gets flushed
-		 * and this bit is reserved.
-		 */
-		if (invalidate_domains & I915_GEM_DOMAIN_SAMPLER)
-			cmd |= MI_READ_FLUSH;
-	}
 	if (invalidate_domains & I915_GEM_DOMAIN_INSTRUCTION)
 		cmd |= MI_EXE_FLUSH;
 
@@ -290,9 +307,9 @@ static int init_ring_common(struct intel_ring_buffer *ring)
 			| RING_VALID);
 
 	/* If the head is still not zero, the ring is dead */
-	if ((I915_READ_CTL(ring) & RING_VALID) == 0 ||
-	    I915_READ_START(ring) != obj->gtt_offset ||
-	    (I915_READ_HEAD(ring) & HEAD_ADDR) != 0) {
+	if (wait_for((I915_READ_CTL(ring) & RING_VALID) != 0 &&
+		     I915_READ_START(ring) == obj->gtt_offset &&
+		     (I915_READ_HEAD(ring) & HEAD_ADDR) == 0, 50)) {
 		DRM_ERROR("%s initialization failed "
 				"ctl %08x head %08x tail %08x start %08x\n",
 				ring->name,
@@ -384,12 +401,11 @@ static int init_render_ring(struct intel_ring_buffer *ring)
 	int ret = init_ring_common(ring);
 
 	if (INTEL_INFO(dev)->gen > 3) {
-		int mode = VS_TIMER_DISPATCH << 16 | VS_TIMER_DISPATCH;
-		I915_WRITE(MI_MODE, mode);
+		I915_WRITE(MI_MODE, _MASKED_BIT_ENABLE(VS_TIMER_DISPATCH));
 		if (IS_GEN7(dev))
 			I915_WRITE(GFX_MODE_GEN7,
-				   GFX_MODE_DISABLE(GFX_TLB_INVALIDATE_ALWAYS) |
-				   GFX_MODE_ENABLE(GFX_REPLAY_MODE));
+				   _MASKED_BIT_DISABLE(GFX_TLB_INVALIDATE_ALWAYS) |
+				   _MASKED_BIT_ENABLE(GFX_REPLAY_MODE));
 	}
 
 	if (INTEL_INFO(dev)->gen >= 5) {
@@ -398,19 +414,19 @@ static int init_render_ring(struct intel_ring_buffer *ring)
 			return ret;
 	}
 
-	if (INTEL_INFO(dev)->gen >= 6) {
-		I915_WRITE(INSTPM,
-			   INSTPM_FORCE_ORDERING << 16 | INSTPM_FORCE_ORDERING);
-
+	if (IS_GEN6(dev)) {
 		/* From the Sandybridge PRM, volume 1 part 3, page 24:
 		 * "If this bit is set, STCunit will have LRA as replacement
 		 *  policy. [...] This bit must be reset.  LRA replacement
 		 *  policy is not supported."
 		 */
 		I915_WRITE(CACHE_MODE_0,
-			   CM0_STC_EVICT_DISABLE_LRA_SNB << CM0_MASK_SHIFT);
+			   _MASKED_BIT_DISABLE(CM0_STC_EVICT_DISABLE_LRA_SNB));
 	}
 
+	if (INTEL_INFO(dev)->gen >= 6)
+		I915_WRITE(INSTPM, _MASKED_BIT_ENABLE(INSTPM_FORCE_ORDERING));
+
 	return ret;
 }
 
@@ -480,21 +496,30 @@ gen6_add_request(struct intel_ring_buffer *ring,
  * @seqno - seqno which the waiter will block on
  */
 static int
-intel_ring_sync(struct intel_ring_buffer *waiter,
-		struct intel_ring_buffer *signaller,
-		int ring,
-		u32 seqno)
+gen6_ring_sync(struct intel_ring_buffer *waiter,
+	       struct intel_ring_buffer *signaller,
+	       u32 seqno)
 {
 	int ret;
 	u32 dw1 = MI_SEMAPHORE_MBOX |
 		  MI_SEMAPHORE_COMPARE |
 		  MI_SEMAPHORE_REGISTER;
 
+	/* Throughout all of the GEM code, seqno passed implies our current
+	 * seqno is >= the last seqno executed. However for hardware the
+	 * comparison is strictly greater than.
+	 */
+	seqno -= 1;
+
+	WARN_ON(signaller->semaphore_register[waiter->id] ==
+		MI_SEMAPHORE_SYNC_INVALID);
+
 	ret = intel_ring_begin(waiter, 4);
 	if (ret)
 		return ret;
 
-	intel_ring_emit(waiter, dw1 | signaller->semaphore_register[ring]);
+	intel_ring_emit(waiter,
+			dw1 | signaller->semaphore_register[waiter->id]);
 	intel_ring_emit(waiter, seqno);
 	intel_ring_emit(waiter, 0);
 	intel_ring_emit(waiter, MI_NOOP);
@@ -503,47 +528,6 @@ intel_ring_sync(struct intel_ring_buffer *waiter,
 	return 0;
 }
 
-/* VCS->RCS (RVSYNC) or BCS->RCS (RBSYNC) */
-int
-render_ring_sync_to(struct intel_ring_buffer *waiter,
-		    struct intel_ring_buffer *signaller,
-		    u32 seqno)
-{
-	WARN_ON(signaller->semaphore_register[RCS] == MI_SEMAPHORE_SYNC_INVALID);
-	return intel_ring_sync(waiter,
-			       signaller,
-			       RCS,
-			       seqno);
-}
-
-/* RCS->VCS (VRSYNC) or BCS->VCS (VBSYNC) */
-int
-gen6_bsd_ring_sync_to(struct intel_ring_buffer *waiter,
-		      struct intel_ring_buffer *signaller,
-		      u32 seqno)
-{
-	WARN_ON(signaller->semaphore_register[VCS] == MI_SEMAPHORE_SYNC_INVALID);
-	return intel_ring_sync(waiter,
-			       signaller,
-			       VCS,
-			       seqno);
-}
-
-/* RCS->BCS (BRSYNC) or VCS->BCS (BVSYNC) */
-int
-gen6_blt_ring_sync_to(struct intel_ring_buffer *waiter,
-		      struct intel_ring_buffer *signaller,
-		      u32 seqno)
-{
-	WARN_ON(signaller->semaphore_register[BCS] == MI_SEMAPHORE_SYNC_INVALID);
-	return intel_ring_sync(waiter,
-			       signaller,
-			       BCS,
-			       seqno);
-}
-
-
-
 #define PIPE_CONTROL_FLUSH(ring__, addr__)					\
 do {									\
 	intel_ring_emit(ring__, GFX_OP_PIPE_CONTROL(4) | PIPE_CONTROL_QW_WRITE |		\
@@ -605,27 +589,6 @@ pc_render_add_request(struct intel_ring_buffer *ring,
 	return 0;
 }
 
-static int
-render_ring_add_request(struct intel_ring_buffer *ring,
-			u32 *result)
-{
-	u32 seqno = i915_gem_next_request_seqno(ring);
-	int ret;
-
-	ret = intel_ring_begin(ring, 4);
-	if (ret)
-		return ret;
-
-	intel_ring_emit(ring, MI_STORE_DWORD_INDEX);
-	intel_ring_emit(ring, I915_GEM_HWS_INDEX << MI_STORE_DWORD_INDEX_SHIFT);
-	intel_ring_emit(ring, seqno);
-	intel_ring_emit(ring, MI_USER_INTERRUPT);
-	intel_ring_advance(ring);
-
-	*result = seqno;
-	return 0;
-}
-
 static u32
 gen6_ring_get_seqno(struct intel_ring_buffer *ring)
 {
@@ -652,76 +615,115 @@ pc_render_get_seqno(struct intel_ring_buffer *ring)
 	return pc->cpu_page[0];
 }
 
-static void
-ironlake_enable_irq(drm_i915_private_t *dev_priv, u32 mask)
+static bool
+gen5_ring_get_irq(struct intel_ring_buffer *ring)
 {
-	dev_priv->gt_irq_mask &= ~mask;
-	I915_WRITE(GTIMR, dev_priv->gt_irq_mask);
-	POSTING_READ(GTIMR);
+	struct drm_device *dev = ring->dev;
+	drm_i915_private_t *dev_priv = dev->dev_private;
+	unsigned long flags;
+
+	if (!dev->irq_enabled)
+		return false;
+
+	spin_lock_irqsave(&dev_priv->irq_lock, flags);
+	if (ring->irq_refcount++ == 0) {
+		dev_priv->gt_irq_mask &= ~ring->irq_enable_mask;
+		I915_WRITE(GTIMR, dev_priv->gt_irq_mask);
+		POSTING_READ(GTIMR);
+	}
+	spin_unlock_irqrestore(&dev_priv->irq_lock, flags);
+
+	return true;
 }
 
 static void
-ironlake_disable_irq(drm_i915_private_t *dev_priv, u32 mask)
+gen5_ring_put_irq(struct intel_ring_buffer *ring)
 {
-	dev_priv->gt_irq_mask |= mask;
-	I915_WRITE(GTIMR, dev_priv->gt_irq_mask);
-	POSTING_READ(GTIMR);
+	struct drm_device *dev = ring->dev;
+	drm_i915_private_t *dev_priv = dev->dev_private;
+	unsigned long flags;
+
+	spin_lock_irqsave(&dev_priv->irq_lock, flags);
+	if (--ring->irq_refcount == 0) {
+		dev_priv->gt_irq_mask |= ring->irq_enable_mask;
+		I915_WRITE(GTIMR, dev_priv->gt_irq_mask);
+		POSTING_READ(GTIMR);
+	}
+	spin_unlock_irqrestore(&dev_priv->irq_lock, flags);
 }
 
-static void
-i915_enable_irq(drm_i915_private_t *dev_priv, u32 mask)
+static bool
+i9xx_ring_get_irq(struct intel_ring_buffer *ring)
 {
-	dev_priv->irq_mask &= ~mask;
-	I915_WRITE(IMR, dev_priv->irq_mask);
-	POSTING_READ(IMR);
+	struct drm_device *dev = ring->dev;
+	drm_i915_private_t *dev_priv = dev->dev_private;
+	unsigned long flags;
+
+	if (!dev->irq_enabled)
+		return false;
+
+	spin_lock_irqsave(&dev_priv->irq_lock, flags);
+	if (ring->irq_refcount++ == 0) {
+		dev_priv->irq_mask &= ~ring->irq_enable_mask;
+		I915_WRITE(IMR, dev_priv->irq_mask);
+		POSTING_READ(IMR);
+	}
+	spin_unlock_irqrestore(&dev_priv->irq_lock, flags);
+
+	return true;
 }
 
 static void
-i915_disable_irq(drm_i915_private_t *dev_priv, u32 mask)
+i9xx_ring_put_irq(struct intel_ring_buffer *ring)
 {
-	dev_priv->irq_mask |= mask;
-	I915_WRITE(IMR, dev_priv->irq_mask);
-	POSTING_READ(IMR);
+	struct drm_device *dev = ring->dev;
+	drm_i915_private_t *dev_priv = dev->dev_private;
+	unsigned long flags;
+
+	spin_lock_irqsave(&dev_priv->irq_lock, flags);
+	if (--ring->irq_refcount == 0) {
+		dev_priv->irq_mask |= ring->irq_enable_mask;
+		I915_WRITE(IMR, dev_priv->irq_mask);
+		POSTING_READ(IMR);
+	}
+	spin_unlock_irqrestore(&dev_priv->irq_lock, flags);
 }
 
 static bool
-render_ring_get_irq(struct intel_ring_buffer *ring)
+i8xx_ring_get_irq(struct intel_ring_buffer *ring)
 {
 	struct drm_device *dev = ring->dev;
 	drm_i915_private_t *dev_priv = dev->dev_private;
+	unsigned long flags;
 
 	if (!dev->irq_enabled)
 		return false;
 
-	spin_lock(&ring->irq_lock);
+	spin_lock_irqsave(&dev_priv->irq_lock, flags);
 	if (ring->irq_refcount++ == 0) {
-		if (HAS_PCH_SPLIT(dev))
-			ironlake_enable_irq(dev_priv,
-					    GT_PIPE_NOTIFY | GT_USER_INTERRUPT);
-		else
-			i915_enable_irq(dev_priv, I915_USER_INTERRUPT);
+		dev_priv->irq_mask &= ~ring->irq_enable_mask;
+		I915_WRITE16(IMR, dev_priv->irq_mask);
+		POSTING_READ16(IMR);
 	}
-	spin_unlock(&ring->irq_lock);
+	spin_unlock_irqrestore(&dev_priv->irq_lock, flags);
 
 	return true;
 }
 
 static void
-render_ring_put_irq(struct intel_ring_buffer *ring)
+i8xx_ring_put_irq(struct intel_ring_buffer *ring)
 {
 	struct drm_device *dev = ring->dev;
 	drm_i915_private_t *dev_priv = dev->dev_private;
+	unsigned long flags;
 
-	spin_lock(&ring->irq_lock);
+	spin_lock_irqsave(&dev_priv->irq_lock, flags);
 	if (--ring->irq_refcount == 0) {
-		if (HAS_PCH_SPLIT(dev))
-			ironlake_disable_irq(dev_priv,
-					     GT_USER_INTERRUPT |
-					     GT_PIPE_NOTIFY);
-		else
-			i915_disable_irq(dev_priv, I915_USER_INTERRUPT);
+		dev_priv->irq_mask |= ring->irq_enable_mask;
+		I915_WRITE16(IMR, dev_priv->irq_mask);
+		POSTING_READ16(IMR);
 	}
-	spin_unlock(&ring->irq_lock);
+	spin_unlock_irqrestore(&dev_priv->irq_lock, flags);
 }
 
 void intel_ring_setup_status_page(struct intel_ring_buffer *ring)
@@ -773,7 +775,7 @@ bsd_ring_flush(struct intel_ring_buffer *ring,
 }
 
 static int
-ring_add_request(struct intel_ring_buffer *ring,
+i9xx_add_request(struct intel_ring_buffer *ring,
 		 u32 *result)
 {
 	u32 seqno;
@@ -796,10 +798,11 @@ ring_add_request(struct intel_ring_buffer *ring,
 }
 
 static bool
-gen6_ring_get_irq(struct intel_ring_buffer *ring, u32 gflag, u32 rflag)
+gen6_ring_get_irq(struct intel_ring_buffer *ring)
 {
 	struct drm_device *dev = ring->dev;
 	drm_i915_private_t *dev_priv = dev->dev_private;
+	unsigned long flags;
 
 	if (!dev->irq_enabled)
 	       return false;
@@ -809,120 +812,87 @@ gen6_ring_get_irq(struct intel_ring_buffer *ring, u32 gflag, u32 rflag)
 	 * blt/bsd rings on ivb. */
 	gen6_gt_force_wake_get(dev_priv);
 
-	spin_lock(&ring->irq_lock);
+	spin_lock_irqsave(&dev_priv->irq_lock, flags);
 	if (ring->irq_refcount++ == 0) {
-		ring->irq_mask &= ~rflag;
-		I915_WRITE_IMR(ring, ring->irq_mask);
-		ironlake_enable_irq(dev_priv, gflag);
+		I915_WRITE_IMR(ring, ~ring->irq_enable_mask);
+		dev_priv->gt_irq_mask &= ~ring->irq_enable_mask;
+		I915_WRITE(GTIMR, dev_priv->gt_irq_mask);
+		POSTING_READ(GTIMR);
 	}
-	spin_unlock(&ring->irq_lock);
+	spin_unlock_irqrestore(&dev_priv->irq_lock, flags);
 
 	return true;
 }
 
 static void
-gen6_ring_put_irq(struct intel_ring_buffer *ring, u32 gflag, u32 rflag)
+gen6_ring_put_irq(struct intel_ring_buffer *ring)
 {
 	struct drm_device *dev = ring->dev;
 	drm_i915_private_t *dev_priv = dev->dev_private;
+	unsigned long flags;
 
-	spin_lock(&ring->irq_lock);
+	spin_lock_irqsave(&dev_priv->irq_lock, flags);
 	if (--ring->irq_refcount == 0) {
-		ring->irq_mask |= rflag;
-		I915_WRITE_IMR(ring, ring->irq_mask);
-		ironlake_disable_irq(dev_priv, gflag);
+		I915_WRITE_IMR(ring, ~0);
+		dev_priv->gt_irq_mask |= ring->irq_enable_mask;
+		I915_WRITE(GTIMR, dev_priv->gt_irq_mask);
+		POSTING_READ(GTIMR);
 	}
-	spin_unlock(&ring->irq_lock);
+	spin_unlock_irqrestore(&dev_priv->irq_lock, flags);
 
 	gen6_gt_force_wake_put(dev_priv);
 }
 
-static bool
-bsd_ring_get_irq(struct intel_ring_buffer *ring)
+static int
+i965_dispatch_execbuffer(struct intel_ring_buffer *ring, u32 offset, u32 length)
 {
-	struct drm_device *dev = ring->dev;
-	drm_i915_private_t *dev_priv = dev->dev_private;
-
-	if (!dev->irq_enabled)
-		return false;
+	int ret;
 
-	spin_lock(&ring->irq_lock);
-	if (ring->irq_refcount++ == 0) {
-		if (IS_G4X(dev))
-			i915_enable_irq(dev_priv, I915_BSD_USER_INTERRUPT);
-		else
-			ironlake_enable_irq(dev_priv, GT_BSD_USER_INTERRUPT);
-	}
-	spin_unlock(&ring->irq_lock);
+	ret = intel_ring_begin(ring, 2);
+	if (ret)
+		return ret;
 
-	return true;
-}
-static void
-bsd_ring_put_irq(struct intel_ring_buffer *ring)
-{
-	struct drm_device *dev = ring->dev;
-	drm_i915_private_t *dev_priv = dev->dev_private;
+	intel_ring_emit(ring,
+			MI_BATCH_BUFFER_START |
+			MI_BATCH_GTT |
+			MI_BATCH_NON_SECURE_I965);
+	intel_ring_emit(ring, offset);
+	intel_ring_advance(ring);
 
-	spin_lock(&ring->irq_lock);
-	if (--ring->irq_refcount == 0) {
-		if (IS_G4X(dev))
-			i915_disable_irq(dev_priv, I915_BSD_USER_INTERRUPT);
-		else
-			ironlake_disable_irq(dev_priv, GT_BSD_USER_INTERRUPT);
-	}
-	spin_unlock(&ring->irq_lock);
+	return 0;
 }
 
 static int
-ring_dispatch_execbuffer(struct intel_ring_buffer *ring, u32 offset, u32 length)
+i830_dispatch_execbuffer(struct intel_ring_buffer *ring,
+				u32 offset, u32 len)
 {
 	int ret;
 
-	ret = intel_ring_begin(ring, 2);
+	ret = intel_ring_begin(ring, 4);
 	if (ret)
 		return ret;
 
-	intel_ring_emit(ring,
-			MI_BATCH_BUFFER_START | (2 << 6) |
-			MI_BATCH_NON_SECURE_I965);
-	intel_ring_emit(ring, offset);
+	intel_ring_emit(ring, MI_BATCH_BUFFER);
+	intel_ring_emit(ring, offset | MI_BATCH_NON_SECURE);
+	intel_ring_emit(ring, offset + len - 8);
+	intel_ring_emit(ring, 0);
 	intel_ring_advance(ring);
 
 	return 0;
 }
 
 static int
-render_ring_dispatch_execbuffer(struct intel_ring_buffer *ring,
+i915_dispatch_execbuffer(struct intel_ring_buffer *ring,
 				u32 offset, u32 len)
 {
-	struct drm_device *dev = ring->dev;
 	int ret;
 
-	if (IS_I830(dev) || IS_845G(dev)) {
-		ret = intel_ring_begin(ring, 4);
-		if (ret)
-			return ret;
-
-		intel_ring_emit(ring, MI_BATCH_BUFFER);
-		intel_ring_emit(ring, offset | MI_BATCH_NON_SECURE);
-		intel_ring_emit(ring, offset + len - 8);
-		intel_ring_emit(ring, 0);
-	} else {
-		ret = intel_ring_begin(ring, 2);
-		if (ret)
-			return ret;
+	ret = intel_ring_begin(ring, 2);
+	if (ret)
+		return ret;
 
-		if (INTEL_INFO(dev)->gen >= 4) {
-			intel_ring_emit(ring,
-					MI_BATCH_BUFFER_START | (2 << 6) |
-					MI_BATCH_NON_SECURE_I965);
-			intel_ring_emit(ring, offset);
-		} else {
-			intel_ring_emit(ring,
-					MI_BATCH_BUFFER_START | (2 << 6));
-			intel_ring_emit(ring, offset | MI_BATCH_NON_SECURE);
-		}
-	}
+	intel_ring_emit(ring, MI_BATCH_BUFFER_START | MI_BATCH_GTT);
+	intel_ring_emit(ring, offset | MI_BATCH_NON_SECURE);
 	intel_ring_advance(ring);
 
 	return 0;
@@ -930,7 +900,6 @@ render_ring_dispatch_execbuffer(struct intel_ring_buffer *ring,
 
 static void cleanup_status_page(struct intel_ring_buffer *ring)
 {
-	drm_i915_private_t *dev_priv = ring->dev->dev_private;
 	struct drm_i915_gem_object *obj;
 
 	obj = ring->status_page.obj;
@@ -941,14 +910,11 @@ static void cleanup_status_page(struct intel_ring_buffer *ring)
 	i915_gem_object_unpin(obj);
 	drm_gem_object_unreference(&obj->base);
 	ring->status_page.obj = NULL;
-
-	memset(&dev_priv->hws_map, 0, sizeof(dev_priv->hws_map));
 }
 
 static int init_status_page(struct intel_ring_buffer *ring)
 {
 	struct drm_device *dev = ring->dev;
-	drm_i915_private_t *dev_priv = dev->dev_private;
 	struct drm_i915_gem_object *obj;
 	int ret;
 
@@ -969,7 +935,6 @@ static int init_status_page(struct intel_ring_buffer *ring)
 	ring->status_page.gfx_addr = obj->gtt_offset;
 	ring->status_page.page_addr = kmap(obj->pages[0]);
 	if (ring->status_page.page_addr == NULL) {
-		memset(&dev_priv->hws_map, 0, sizeof(dev_priv->hws_map));
 		goto err_unpin;
 	}
 	ring->status_page.obj = obj;
@@ -989,8 +954,8 @@ err:
 	return ret;
 }
 
-int intel_init_ring_buffer(struct drm_device *dev,
-			   struct intel_ring_buffer *ring)
+static int intel_init_ring_buffer(struct drm_device *dev,
+				  struct intel_ring_buffer *ring)
 {
 	struct drm_i915_gem_object *obj;
 	int ret;
@@ -999,10 +964,9 @@ int intel_init_ring_buffer(struct drm_device *dev,
 	INIT_LIST_HEAD(&ring->active_list);
 	INIT_LIST_HEAD(&ring->request_list);
 	INIT_LIST_HEAD(&ring->gpu_write_list);
+	ring->size = 32 * PAGE_SIZE;
 
 	init_waitqueue_head(&ring->irq_queue);
-	spin_lock_init(&ring->irq_lock);
-	ring->irq_mask = ~0;
 
 	if (I915_NEED_GFX_HWS(dev)) {
 		ret = init_status_page(ring);
@@ -1023,20 +987,14 @@ int intel_init_ring_buffer(struct drm_device *dev,
 	if (ret)
 		goto err_unref;
 
-	ring->map.size = ring->size;
-	ring->map.offset = dev->agp->base + obj->gtt_offset;
-	ring->map.type = 0;
-	ring->map.flags = 0;
-	ring->map.mtrr = 0;
-
-	drm_core_ioremap_wc(&ring->map, dev);
-	if (ring->map.handle == NULL) {
+	ring->virtual_start = ioremap_wc(dev->agp->base + obj->gtt_offset,
+					 ring->size);
+	if (ring->virtual_start == NULL) {
 		DRM_ERROR("Failed to map ringbuffer.\n");
 		ret = -EINVAL;
 		goto err_unpin;
 	}
 
-	ring->virtual_start = ring->map.handle;
 	ret = ring->init(ring);
 	if (ret)
 		goto err_unmap;
@@ -1052,7 +1010,7 @@ int intel_init_ring_buffer(struct drm_device *dev,
 	return 0;
 
 err_unmap:
-	drm_core_ioremapfree(&ring->map, dev);
+	iounmap(ring->virtual_start);
 err_unpin:
 	i915_gem_object_unpin(obj);
 err_unref:
@@ -1080,7 +1038,7 @@ void intel_cleanup_ring_buffer(struct intel_ring_buffer *ring)
 
 	I915_WRITE_CTL(ring, 0);
 
-	drm_core_ioremapfree(&ring->map, ring->dev);
+	iounmap(ring->virtual_start);
 
 	i915_gem_object_unpin(ring->obj);
 	drm_gem_object_unreference(&ring->obj->base);
@@ -1094,7 +1052,7 @@ void intel_cleanup_ring_buffer(struct intel_ring_buffer *ring)
 
 static int intel_wrap_ring_buffer(struct intel_ring_buffer *ring)
 {
-	unsigned int *virt;
+	uint32_t __iomem *virt;
 	int rem = ring->size - ring->tail;
 
 	if (ring->space < rem) {
@@ -1103,12 +1061,10 @@ static int intel_wrap_ring_buffer(struct intel_ring_buffer *ring)
 			return ret;
 	}
 
-	virt = (unsigned int *)(ring->virtual_start + ring->tail);
-	rem /= 8;
-	while (rem--) {
-		*virt++ = MI_NOOP;
-		*virt++ = MI_NOOP;
-	}
+	virt = ring->virtual_start + ring->tail;
+	rem /= 4;
+	while (rem--)
+		iowrite32(MI_NOOP, virt++);
 
 	ring->tail = 0;
 	ring->space = ring_space(ring);
@@ -1129,9 +1085,11 @@ static int intel_ring_wait_seqno(struct intel_ring_buffer *ring, u32 seqno)
 	was_interruptible = dev_priv->mm.interruptible;
 	dev_priv->mm.interruptible = false;
 
-	ret = i915_wait_request(ring, seqno, true);
+	ret = i915_wait_request(ring, seqno);
 
 	dev_priv->mm.interruptible = was_interruptible;
+	if (!ret)
+		i915_gem_retire_requests_ring(ring);
 
 	return ret;
 }
@@ -1205,15 +1163,12 @@ int intel_wait_ring_buffer(struct intel_ring_buffer *ring, int n)
 		return ret;
 
 	trace_i915_ring_wait_begin(ring);
-	if (drm_core_check_feature(dev, DRIVER_GEM))
-		/* With GEM the hangcheck timer should kick us out of the loop,
-		 * leaving it early runs the risk of corrupting GEM state (due
-		 * to running on almost untested codepaths). But on resume
-		 * timers don't work yet, so prevent a complete hang in that
-		 * case by choosing an insanely large timeout. */
-		end = jiffies + 60 * HZ;
-	else
-		end = jiffies + 3 * HZ;
+	/* With GEM the hangcheck timer should kick us out of the loop,
+	 * leaving it early runs the risk of corrupting GEM state (due
+	 * to running on almost untested codepaths). But on resume
+	 * timers don't work yet, so prevent a complete hang in that
+	 * case by choosing an insanely large timeout. */
+	end = jiffies + 60 * HZ;
 
 	do {
 		ring->head = I915_READ_HEAD(ring);
@@ -1265,48 +1220,14 @@ int intel_ring_begin(struct intel_ring_buffer *ring,
 
 void intel_ring_advance(struct intel_ring_buffer *ring)
 {
+	struct drm_i915_private *dev_priv = ring->dev->dev_private;
+
 	ring->tail &= ring->size - 1;
+	if (dev_priv->stop_rings & intel_ring_flag(ring))
+		return;
 	ring->write_tail(ring, ring->tail);
 }
 
-static const struct intel_ring_buffer render_ring = {
-	.name			= "render ring",
-	.id			= RCS,
-	.mmio_base		= RENDER_RING_BASE,
-	.size			= 32 * PAGE_SIZE,
-	.init			= init_render_ring,
-	.write_tail		= ring_write_tail,
-	.flush			= render_ring_flush,
-	.add_request		= render_ring_add_request,
-	.get_seqno		= ring_get_seqno,
-	.irq_get		= render_ring_get_irq,
-	.irq_put		= render_ring_put_irq,
-	.dispatch_execbuffer	= render_ring_dispatch_execbuffer,
-	.cleanup		= render_ring_cleanup,
-	.sync_to		= render_ring_sync_to,
-	.semaphore_register	= {MI_SEMAPHORE_SYNC_INVALID,
-				   MI_SEMAPHORE_SYNC_RV,
-				   MI_SEMAPHORE_SYNC_RB},
-	.signal_mbox		= {GEN6_VRSYNC, GEN6_BRSYNC},
-};
-
-/* ring buffer for bit-stream decoder */
-
-static const struct intel_ring_buffer bsd_ring = {
-	.name                   = "bsd ring",
-	.id			= VCS,
-	.mmio_base		= BSD_RING_BASE,
-	.size			= 32 * PAGE_SIZE,
-	.init			= init_ring_common,
-	.write_tail		= ring_write_tail,
-	.flush			= bsd_ring_flush,
-	.add_request		= ring_add_request,
-	.get_seqno		= ring_get_seqno,
-	.irq_get		= bsd_ring_get_irq,
-	.irq_put		= bsd_ring_put_irq,
-	.dispatch_execbuffer	= ring_dispatch_execbuffer,
-};
-
 
 static void gen6_bsd_ring_write_tail(struct intel_ring_buffer *ring,
 				     u32 value)
@@ -1369,77 +1290,8 @@ gen6_ring_dispatch_execbuffer(struct intel_ring_buffer *ring,
 	return 0;
 }
 
-static bool
-gen6_render_ring_get_irq(struct intel_ring_buffer *ring)
-{
-	return gen6_ring_get_irq(ring,
-				 GT_USER_INTERRUPT,
-				 GEN6_RENDER_USER_INTERRUPT);
-}
-
-static void
-gen6_render_ring_put_irq(struct intel_ring_buffer *ring)
-{
-	return gen6_ring_put_irq(ring,
-				 GT_USER_INTERRUPT,
-				 GEN6_RENDER_USER_INTERRUPT);
-}
-
-static bool
-gen6_bsd_ring_get_irq(struct intel_ring_buffer *ring)
-{
-	return gen6_ring_get_irq(ring,
-				 GT_GEN6_BSD_USER_INTERRUPT,
-				 GEN6_BSD_USER_INTERRUPT);
-}
-
-static void
-gen6_bsd_ring_put_irq(struct intel_ring_buffer *ring)
-{
-	return gen6_ring_put_irq(ring,
-				 GT_GEN6_BSD_USER_INTERRUPT,
-				 GEN6_BSD_USER_INTERRUPT);
-}
-
-/* ring buffer for Video Codec for Gen6+ */
-static const struct intel_ring_buffer gen6_bsd_ring = {
-	.name			= "gen6 bsd ring",
-	.id			= VCS,
-	.mmio_base		= GEN6_BSD_RING_BASE,
-	.size			= 32 * PAGE_SIZE,
-	.init			= init_ring_common,
-	.write_tail		= gen6_bsd_ring_write_tail,
-	.flush			= gen6_ring_flush,
-	.add_request		= gen6_add_request,
-	.get_seqno		= gen6_ring_get_seqno,
-	.irq_get		= gen6_bsd_ring_get_irq,
-	.irq_put		= gen6_bsd_ring_put_irq,
-	.dispatch_execbuffer	= gen6_ring_dispatch_execbuffer,
-	.sync_to		= gen6_bsd_ring_sync_to,
-	.semaphore_register	= {MI_SEMAPHORE_SYNC_VR,
-				   MI_SEMAPHORE_SYNC_INVALID,
-				   MI_SEMAPHORE_SYNC_VB},
-	.signal_mbox		= {GEN6_RVSYNC, GEN6_BVSYNC},
-};
-
 /* Blitter support (SandyBridge+) */
 
-static bool
-blt_ring_get_irq(struct intel_ring_buffer *ring)
-{
-	return gen6_ring_get_irq(ring,
-				 GT_BLT_USER_INTERRUPT,
-				 GEN6_BLITTER_USER_INTERRUPT);
-}
-
-static void
-blt_ring_put_irq(struct intel_ring_buffer *ring)
-{
-	gen6_ring_put_irq(ring,
-			  GT_BLT_USER_INTERRUPT,
-			  GEN6_BLITTER_USER_INTERRUPT);
-}
-
 static int blt_ring_flush(struct intel_ring_buffer *ring,
 			  u32 invalidate, u32 flush)
 {
@@ -1461,42 +1313,63 @@ static int blt_ring_flush(struct intel_ring_buffer *ring,
 	return 0;
 }
 
-static const struct intel_ring_buffer gen6_blt_ring = {
-	.name			= "blt ring",
-	.id			= BCS,
-	.mmio_base		= BLT_RING_BASE,
-	.size			= 32 * PAGE_SIZE,
-	.init			= init_ring_common,
-	.write_tail		= ring_write_tail,
-	.flush			= blt_ring_flush,
-	.add_request		= gen6_add_request,
-	.get_seqno		= gen6_ring_get_seqno,
-	.irq_get		= blt_ring_get_irq,
-	.irq_put		= blt_ring_put_irq,
-	.dispatch_execbuffer	= gen6_ring_dispatch_execbuffer,
-	.sync_to		= gen6_blt_ring_sync_to,
-	.semaphore_register	= {MI_SEMAPHORE_SYNC_BR,
-				   MI_SEMAPHORE_SYNC_BV,
-				   MI_SEMAPHORE_SYNC_INVALID},
-	.signal_mbox		= {GEN6_RBSYNC, GEN6_VBSYNC},
-};
-
 int intel_init_render_ring_buffer(struct drm_device *dev)
 {
 	drm_i915_private_t *dev_priv = dev->dev_private;
 	struct intel_ring_buffer *ring = &dev_priv->ring[RCS];
 
-	*ring = render_ring;
+	ring->name = "render ring";
+	ring->id = RCS;
+	ring->mmio_base = RENDER_RING_BASE;
+
 	if (INTEL_INFO(dev)->gen >= 6) {
 		ring->add_request = gen6_add_request;
 		ring->flush = gen6_render_ring_flush;
-		ring->irq_get = gen6_render_ring_get_irq;
-		ring->irq_put = gen6_render_ring_put_irq;
+		ring->irq_get = gen6_ring_get_irq;
+		ring->irq_put = gen6_ring_put_irq;
+		ring->irq_enable_mask = GT_USER_INTERRUPT;
 		ring->get_seqno = gen6_ring_get_seqno;
+		ring->sync_to = gen6_ring_sync;
+		ring->semaphore_register[0] = MI_SEMAPHORE_SYNC_INVALID;
+		ring->semaphore_register[1] = MI_SEMAPHORE_SYNC_RV;
+		ring->semaphore_register[2] = MI_SEMAPHORE_SYNC_RB;
+		ring->signal_mbox[0] = GEN6_VRSYNC;
+		ring->signal_mbox[1] = GEN6_BRSYNC;
 	} else if (IS_GEN5(dev)) {
 		ring->add_request = pc_render_add_request;
+		ring->flush = gen4_render_ring_flush;
 		ring->get_seqno = pc_render_get_seqno;
+		ring->irq_get = gen5_ring_get_irq;
+		ring->irq_put = gen5_ring_put_irq;
+		ring->irq_enable_mask = GT_USER_INTERRUPT | GT_PIPE_NOTIFY;
+	} else {
+		ring->add_request = i9xx_add_request;
+		if (INTEL_INFO(dev)->gen < 4)
+			ring->flush = gen2_render_ring_flush;
+		else
+			ring->flush = gen4_render_ring_flush;
+		ring->get_seqno = ring_get_seqno;
+		if (IS_GEN2(dev)) {
+			ring->irq_get = i8xx_ring_get_irq;
+			ring->irq_put = i8xx_ring_put_irq;
+		} else {
+			ring->irq_get = i9xx_ring_get_irq;
+			ring->irq_put = i9xx_ring_put_irq;
+		}
+		ring->irq_enable_mask = I915_USER_INTERRUPT;
 	}
+	ring->write_tail = ring_write_tail;
+	if (INTEL_INFO(dev)->gen >= 6)
+		ring->dispatch_execbuffer = gen6_ring_dispatch_execbuffer;
+	else if (INTEL_INFO(dev)->gen >= 4)
+		ring->dispatch_execbuffer = i965_dispatch_execbuffer;
+	else if (IS_I830(dev) || IS_845G(dev))
+		ring->dispatch_execbuffer = i830_dispatch_execbuffer;
+	else
+		ring->dispatch_execbuffer = i915_dispatch_execbuffer;
+	ring->init = init_render_ring;
+	ring->cleanup = render_ring_cleanup;
+
 
 	if (!I915_NEED_GFX_HWS(dev)) {
 		ring->status_page.page_addr = dev_priv->status_page_dmah->vaddr;
@@ -1511,15 +1384,41 @@ int intel_render_ring_init_dri(struct drm_device *dev, u64 start, u32 size)
 	drm_i915_private_t *dev_priv = dev->dev_private;
 	struct intel_ring_buffer *ring = &dev_priv->ring[RCS];
 
-	*ring = render_ring;
+	ring->name = "render ring";
+	ring->id = RCS;
+	ring->mmio_base = RENDER_RING_BASE;
+
 	if (INTEL_INFO(dev)->gen >= 6) {
-		ring->add_request = gen6_add_request;
-		ring->irq_get = gen6_render_ring_get_irq;
-		ring->irq_put = gen6_render_ring_put_irq;
-	} else if (IS_GEN5(dev)) {
-		ring->add_request = pc_render_add_request;
-		ring->get_seqno = pc_render_get_seqno;
+		/* non-kms not supported on gen6+ */
+		return -ENODEV;
+	}
+
+	/* Note: gem is not supported on gen5/ilk without kms (the corresponding
+	 * gem_init ioctl returns with -ENODEV). Hence we do not need to set up
+	 * the special gen5 functions. */
+	ring->add_request = i9xx_add_request;
+	if (INTEL_INFO(dev)->gen < 4)
+		ring->flush = gen2_render_ring_flush;
+	else
+		ring->flush = gen4_render_ring_flush;
+	ring->get_seqno = ring_get_seqno;
+	if (IS_GEN2(dev)) {
+		ring->irq_get = i8xx_ring_get_irq;
+		ring->irq_put = i8xx_ring_put_irq;
+	} else {
+		ring->irq_get = i9xx_ring_get_irq;
+		ring->irq_put = i9xx_ring_put_irq;
 	}
+	ring->irq_enable_mask = I915_USER_INTERRUPT;
+	ring->write_tail = ring_write_tail;
+	if (INTEL_INFO(dev)->gen >= 4)
+		ring->dispatch_execbuffer = i965_dispatch_execbuffer;
+	else if (IS_I830(dev) || IS_845G(dev))
+		ring->dispatch_execbuffer = i830_dispatch_execbuffer;
+	else
+		ring->dispatch_execbuffer = i915_dispatch_execbuffer;
+	ring->init = init_render_ring;
+	ring->cleanup = render_ring_cleanup;
 
 	if (!I915_NEED_GFX_HWS(dev))
 		ring->status_page.page_addr = dev_priv->status_page_dmah->vaddr;
@@ -1534,20 +1433,13 @@ int intel_render_ring_init_dri(struct drm_device *dev, u64 start, u32 size)
 	if (IS_I830(ring->dev))
 		ring->effective_size -= 128;
 
-	ring->map.offset = start;
-	ring->map.size = size;
-	ring->map.type = 0;
-	ring->map.flags = 0;
-	ring->map.mtrr = 0;
-
-	drm_core_ioremap_wc(&ring->map, dev);
-	if (ring->map.handle == NULL) {
+	ring->virtual_start = ioremap_wc(start, size);
+	if (ring->virtual_start == NULL) {
 		DRM_ERROR("can not ioremap virtual address for"
 			  " ring buffer\n");
 		return -ENOMEM;
 	}
 
-	ring->virtual_start = (void __force __iomem *)ring->map.handle;
 	return 0;
 }
 
@@ -1556,10 +1448,46 @@ int intel_init_bsd_ring_buffer(struct drm_device *dev)
 	drm_i915_private_t *dev_priv = dev->dev_private;
 	struct intel_ring_buffer *ring = &dev_priv->ring[VCS];
 
-	if (IS_GEN6(dev) || IS_GEN7(dev))
-		*ring = gen6_bsd_ring;
-	else
-		*ring = bsd_ring;
+	ring->name = "bsd ring";
+	ring->id = VCS;
+
+	ring->write_tail = ring_write_tail;
+	if (IS_GEN6(dev) || IS_GEN7(dev)) {
+		ring->mmio_base = GEN6_BSD_RING_BASE;
+		/* gen6 bsd needs a special wa for tail updates */
+		if (IS_GEN6(dev))
+			ring->write_tail = gen6_bsd_ring_write_tail;
+		ring->flush = gen6_ring_flush;
+		ring->add_request = gen6_add_request;
+		ring->get_seqno = gen6_ring_get_seqno;
+		ring->irq_enable_mask = GEN6_BSD_USER_INTERRUPT;
+		ring->irq_get = gen6_ring_get_irq;
+		ring->irq_put = gen6_ring_put_irq;
+		ring->dispatch_execbuffer = gen6_ring_dispatch_execbuffer;
+		ring->sync_to = gen6_ring_sync;
+		ring->semaphore_register[0] = MI_SEMAPHORE_SYNC_VR;
+		ring->semaphore_register[1] = MI_SEMAPHORE_SYNC_INVALID;
+		ring->semaphore_register[2] = MI_SEMAPHORE_SYNC_VB;
+		ring->signal_mbox[0] = GEN6_RVSYNC;
+		ring->signal_mbox[1] = GEN6_BVSYNC;
+	} else {
+		ring->mmio_base = BSD_RING_BASE;
+		ring->flush = bsd_ring_flush;
+		ring->add_request = i9xx_add_request;
+		ring->get_seqno = ring_get_seqno;
+		if (IS_GEN5(dev)) {
+			ring->irq_enable_mask = GT_BSD_USER_INTERRUPT;
+			ring->irq_get = gen5_ring_get_irq;
+			ring->irq_put = gen5_ring_put_irq;
+		} else {
+			ring->irq_enable_mask = I915_BSD_USER_INTERRUPT;
+			ring->irq_get = i9xx_ring_get_irq;
+			ring->irq_put = i9xx_ring_put_irq;
+		}
+		ring->dispatch_execbuffer = i965_dispatch_execbuffer;
+	}
+	ring->init = init_ring_common;
+
 
 	return intel_init_ring_buffer(dev, ring);
 }
@@ -1569,7 +1497,25 @@ int intel_init_blt_ring_buffer(struct drm_device *dev)
 	drm_i915_private_t *dev_priv = dev->dev_private;
 	struct intel_ring_buffer *ring = &dev_priv->ring[BCS];
 
-	*ring = gen6_blt_ring;
+	ring->name = "blitter ring";
+	ring->id = BCS;
+
+	ring->mmio_base = BLT_RING_BASE;
+	ring->write_tail = ring_write_tail;
+	ring->flush = blt_ring_flush;
+	ring->add_request = gen6_add_request;
+	ring->get_seqno = gen6_ring_get_seqno;
+	ring->irq_enable_mask = GEN6_BLITTER_USER_INTERRUPT;
+	ring->irq_get = gen6_ring_get_irq;
+	ring->irq_put = gen6_ring_put_irq;
+	ring->dispatch_execbuffer = gen6_ring_dispatch_execbuffer;
+	ring->sync_to = gen6_ring_sync;
+	ring->semaphore_register[0] = MI_SEMAPHORE_SYNC_BR;
+	ring->semaphore_register[1] = MI_SEMAPHORE_SYNC_BV;
+	ring->semaphore_register[2] = MI_SEMAPHORE_SYNC_INVALID;
+	ring->signal_mbox[0] = GEN6_RBSYNC;
+	ring->signal_mbox[1] = GEN6_VBSYNC;
+	ring->init = init_ring_common;
 
 	return intel_init_ring_buffer(dev, ring);
 }
diff --git a/drivers/gpu/drm/i915/intel_ringbuffer.h b/drivers/gpu/drm/i915/intel_ringbuffer.h
index bc0365b8fa4..55d3da26bae 100644
--- a/drivers/gpu/drm/i915/intel_ringbuffer.h
+++ b/drivers/gpu/drm/i915/intel_ringbuffer.h
@@ -2,7 +2,7 @@
 #define _INTEL_RINGBUFFER_H_
 
 struct  intel_hw_status_page {
-	u32	__iomem	*page_addr;
+	u32		*page_addr;
 	unsigned int	gfx_addr;
 	struct		drm_i915_gem_object *obj;
 };
@@ -56,12 +56,9 @@ struct  intel_ring_buffer {
 	 */
 	u32		last_retired_head;
 
-	spinlock_t	irq_lock;
-	u32		irq_refcount;
-	u32		irq_mask;
-	u32		irq_seqno;		/* last seq seem at irq time */
+	u32		irq_refcount;		/* protected by dev_priv->irq_lock */
+	u32		irq_enable_mask;	/* bitmask to enable ring interrupt */
 	u32		trace_irq_seqno;
-	u32		waiting_seqno;
 	u32		sync_seqno[I915_NUM_RINGS-1];
 	bool __must_check (*irq_get)(struct intel_ring_buffer *ring);
 	void		(*irq_put)(struct intel_ring_buffer *ring);
@@ -118,11 +115,16 @@ struct  intel_ring_buffer {
 	u32 outstanding_lazy_request;
 
 	wait_queue_head_t irq_queue;
-	drm_local_map_t map;
 
 	void *private;
 };
 
+static inline bool
+intel_ring_initialized(struct intel_ring_buffer *ring)
+{
+	return ring->obj != NULL;
+}
+
 static inline unsigned
 intel_ring_flag(struct intel_ring_buffer *ring)
 {
@@ -152,7 +154,9 @@ static inline u32
 intel_read_status_page(struct intel_ring_buffer *ring,
 		       int reg)
 {
-	return ioread32(ring->status_page.page_addr + reg);
+	/* Ensure that the compiler doesn't optimize away the load. */
+	barrier();
+	return ring->status_page.page_addr[reg];
 }
 
 /**
@@ -170,10 +174,7 @@ intel_read_status_page(struct intel_ring_buffer *ring,
  *
  * The area from dword 0x20 to 0x3ff is available for driver usage.
  */
-#define READ_HWSP(dev_priv, reg) intel_read_status_page(LP_RING(dev_priv), reg)
-#define READ_BREADCRUMB(dev_priv) READ_HWSP(dev_priv, I915_BREADCRUMB_INDEX)
 #define I915_GEM_HWS_INDEX		0x20
-#define I915_BREADCRUMB_INDEX		0x21
 
 void intel_cleanup_ring_buffer(struct intel_ring_buffer *ring);
 
diff --git a/drivers/gpu/drm/i915/intel_sdvo.c b/drivers/gpu/drm/i915/intel_sdvo.c
index 232d77d07d8..a949b73880c 100644
--- a/drivers/gpu/drm/i915/intel_sdvo.c
+++ b/drivers/gpu/drm/i915/intel_sdvo.c
@@ -41,7 +41,7 @@
 #define SDVO_TMDS_MASK (SDVO_OUTPUT_TMDS0 | SDVO_OUTPUT_TMDS1)
 #define SDVO_RGB_MASK  (SDVO_OUTPUT_RGB0 | SDVO_OUTPUT_RGB1)
 #define SDVO_LVDS_MASK (SDVO_OUTPUT_LVDS0 | SDVO_OUTPUT_LVDS1)
-#define SDVO_TV_MASK   (SDVO_OUTPUT_CVBS0 | SDVO_OUTPUT_SVID0)
+#define SDVO_TV_MASK   (SDVO_OUTPUT_CVBS0 | SDVO_OUTPUT_SVID0 | SDVO_OUTPUT_YPRPB0)
 
 #define SDVO_OUTPUT_MASK (SDVO_TMDS_MASK | SDVO_RGB_MASK | SDVO_LVDS_MASK |\
 			SDVO_TV_MASK)
@@ -74,7 +74,7 @@ struct intel_sdvo {
 	struct i2c_adapter ddc;
 
 	/* Register for the SDVO device: SDVOB or SDVOC */
-	int sdvo_reg;
+	uint32_t sdvo_reg;
 
 	/* Active outputs controlled by this SDVO output */
 	uint16_t controlled_output;
@@ -114,6 +114,9 @@ struct intel_sdvo {
 	 */
 	bool is_tv;
 
+	/* On different gens SDVOB is at different places. */
+	bool is_sdvob;
+
 	/* This is for current tv format name */
 	int tv_format_index;
 
@@ -403,8 +406,7 @@ static const struct _sdvo_cmd_name {
 	SDVO_CMD_NAME_ENTRY(SDVO_CMD_GET_HBUF_DATA),
 };
 
-#define IS_SDVOB(reg)	(reg == SDVOB || reg == PCH_SDVOB)
-#define SDVO_NAME(svdo) (IS_SDVOB((svdo)->sdvo_reg) ? "SDVOB" : "SDVOC")
+#define SDVO_NAME(svdo) ((svdo)->is_sdvob ? "SDVOB" : "SDVOC")
 
 static void intel_sdvo_debug_write(struct intel_sdvo *intel_sdvo, u8 cmd,
 				   const void *args, int args_len)
@@ -441,9 +443,17 @@ static const char *cmd_status_names[] = {
 static bool intel_sdvo_write_cmd(struct intel_sdvo *intel_sdvo, u8 cmd,
 				 const void *args, int args_len)
 {
-	u8 buf[args_len*2 + 2], status;
-	struct i2c_msg msgs[args_len + 3];
-	int i, ret;
+	u8 *buf, status;
+	struct i2c_msg *msgs;
+	int i, ret = true;
+
+	buf = (u8 *)kzalloc(args_len * 2 + 2, GFP_KERNEL);
+	if (!buf)
+		return false;
+
+	msgs = kcalloc(args_len + 3, sizeof(*msgs), GFP_KERNEL);
+	if (!msgs)
+		return false;
 
 	intel_sdvo_debug_write(intel_sdvo, cmd, args, args_len);
 
@@ -477,15 +487,19 @@ static bool intel_sdvo_write_cmd(struct intel_sdvo *intel_sdvo, u8 cmd,
 	ret = i2c_transfer(intel_sdvo->i2c, msgs, i+3);
 	if (ret < 0) {
 		DRM_DEBUG_KMS("I2c transfer returned %d\n", ret);
-		return false;
+		ret = false;
+		goto out;
 	}
 	if (ret != i+3) {
 		/* failure in I2C transfer */
 		DRM_DEBUG_KMS("I2c transfer returned %d/%d\n", ret, i+3);
-		return false;
+		ret = false;
 	}
 
-	return true;
+out:
+	kfree(msgs);
+	kfree(buf);
+	return ret;
 }
 
 static bool intel_sdvo_read_response(struct intel_sdvo *intel_sdvo,
@@ -733,18 +747,18 @@ static void intel_sdvo_get_dtd_from_mode(struct intel_sdvo_dtd *dtd,
 	uint16_t h_sync_offset, v_sync_offset;
 	int mode_clock;
 
-	width = mode->crtc_hdisplay;
-	height = mode->crtc_vdisplay;
+	width = mode->hdisplay;
+	height = mode->vdisplay;
 
 	/* do some mode translations */
-	h_blank_len = mode->crtc_hblank_end - mode->crtc_hblank_start;
-	h_sync_len = mode->crtc_hsync_end - mode->crtc_hsync_start;
+	h_blank_len = mode->htotal - mode->hdisplay;
+	h_sync_len = mode->hsync_end - mode->hsync_start;
 
-	v_blank_len = mode->crtc_vblank_end - mode->crtc_vblank_start;
-	v_sync_len = mode->crtc_vsync_end - mode->crtc_vsync_start;
+	v_blank_len = mode->vtotal - mode->vdisplay;
+	v_sync_len = mode->vsync_end - mode->vsync_start;
 
-	h_sync_offset = mode->crtc_hsync_start - mode->crtc_hblank_start;
-	v_sync_offset = mode->crtc_vsync_start - mode->crtc_vblank_start;
+	h_sync_offset = mode->hsync_start - mode->hdisplay;
+	v_sync_offset = mode->vsync_start - mode->vdisplay;
 
 	mode_clock = mode->clock;
 	mode_clock /= intel_mode_get_pixel_multiplier(mode) ?: 1;
@@ -873,17 +887,24 @@ static bool intel_sdvo_set_avi_infoframe(struct intel_sdvo *intel_sdvo)
 	};
 	uint8_t tx_rate = SDVO_HBUF_TX_VSYNC;
 	uint8_t set_buf_index[2] = { 1, 0 };
-	uint64_t *data = (uint64_t *)&avi_if;
+	uint8_t sdvo_data[4 + sizeof(avi_if.body.avi)];
+	uint64_t *data = (uint64_t *)sdvo_data;
 	unsigned i;
 
 	intel_dip_infoframe_csum(&avi_if);
 
+	/* sdvo spec says that the ecc is handled by the hw, and it looks like
+	 * we must not send the ecc field, either. */
+	memcpy(sdvo_data, &avi_if, 3);
+	sdvo_data[3] = avi_if.checksum;
+	memcpy(&sdvo_data[4], &avi_if.body, sizeof(avi_if.body.avi));
+
 	if (!intel_sdvo_set_value(intel_sdvo,
 				  SDVO_CMD_SET_HBUF_INDEX,
 				  set_buf_index, 2))
 		return false;
 
-	for (i = 0; i < sizeof(avi_if); i += 8) {
+	for (i = 0; i < sizeof(sdvo_data); i += 8) {
 		if (!intel_sdvo_set_value(intel_sdvo,
 					  SDVO_CMD_SET_HBUF_DATA,
 					  data, 8))
@@ -1220,8 +1241,14 @@ static bool intel_sdvo_get_capabilities(struct intel_sdvo *intel_sdvo, struct in
 
 static int intel_sdvo_supports_hotplug(struct intel_sdvo *intel_sdvo)
 {
+	struct drm_device *dev = intel_sdvo->base.base.dev;
 	u8 response[2];
 
+	/* HW Erratum: SDVO Hotplug is broken on all i945G chips, there's noise
+	 * on the line. */
+	if (IS_I945G(dev) || IS_I945GM(dev))
+		return false;
+
 	return intel_sdvo_get_value(intel_sdvo, SDVO_CMD_GET_HOT_PLUG_SUPPORT,
 				    &response, 2) && response[0];
 }
@@ -1254,10 +1281,11 @@ intel_sdvo_get_analog_edid(struct drm_connector *connector)
 	struct drm_i915_private *dev_priv = connector->dev->dev_private;
 
 	return drm_get_edid(connector,
-			    &dev_priv->gmbus[dev_priv->crt_ddc_pin].adapter);
+			    intel_gmbus_get_adapter(dev_priv,
+						    dev_priv->crt_ddc_pin));
 }
 
-enum drm_connector_status
+static enum drm_connector_status
 intel_sdvo_tmds_sink_detect(struct drm_connector *connector)
 {
 	struct intel_sdvo *intel_sdvo = intel_attached_sdvo(connector);
@@ -1343,8 +1371,7 @@ intel_sdvo_detect(struct drm_connector *connector, bool force)
 		return connector_status_unknown;
 
 	/* add 30ms delay when the output type might be TV */
-	if (intel_sdvo->caps.output_flags &
-	    (SDVO_OUTPUT_SVID0 | SDVO_OUTPUT_CVBS0))
+	if (intel_sdvo->caps.output_flags & SDVO_TV_MASK)
 		mdelay(30);
 
 	if (!intel_sdvo_read_response(intel_sdvo, &response, 2))
@@ -1564,9 +1591,6 @@ end:
 			intel_sdvo->sdvo_lvds_fixed_mode =
 				drm_mode_duplicate(connector->dev, newmode);
 
-			drm_mode_set_crtcinfo(intel_sdvo->sdvo_lvds_fixed_mode,
-					      0);
-
 			intel_sdvo->is_lvds = true;
 			break;
 		}
@@ -1895,7 +1919,7 @@ intel_sdvo_select_ddc_bus(struct drm_i915_private *dev_priv,
 {
 	struct sdvo_device_mapping *mapping;
 
-	if (IS_SDVOB(reg))
+	if (sdvo->is_sdvob)
 		mapping = &(dev_priv->sdvo_mappings[0]);
 	else
 		mapping = &(dev_priv->sdvo_mappings[1]);
@@ -1913,7 +1937,7 @@ intel_sdvo_select_i2c_bus(struct drm_i915_private *dev_priv,
 	struct sdvo_device_mapping *mapping;
 	u8 pin;
 
-	if (IS_SDVOB(reg))
+	if (sdvo->is_sdvob)
 		mapping = &dev_priv->sdvo_mappings[0];
 	else
 		mapping = &dev_priv->sdvo_mappings[1];
@@ -1922,12 +1946,12 @@ intel_sdvo_select_i2c_bus(struct drm_i915_private *dev_priv,
 	if (mapping->initialized)
 		pin = mapping->i2c_pin;
 
-	if (pin < GMBUS_NUM_PORTS) {
-		sdvo->i2c = &dev_priv->gmbus[pin].adapter;
+	if (intel_gmbus_is_port_valid(pin)) {
+		sdvo->i2c = intel_gmbus_get_adapter(dev_priv, pin);
 		intel_gmbus_set_speed(sdvo->i2c, GMBUS_RATE_1MHZ);
 		intel_gmbus_force_bit(sdvo->i2c, true);
 	} else {
-		sdvo->i2c = &dev_priv->gmbus[GMBUS_PORT_DPB].adapter;
+		sdvo->i2c = intel_gmbus_get_adapter(dev_priv, GMBUS_PORT_DPB);
 	}
 }
 
@@ -1938,12 +1962,12 @@ intel_sdvo_is_hdmi_connector(struct intel_sdvo *intel_sdvo, int device)
 }
 
 static u8
-intel_sdvo_get_slave_addr(struct drm_device *dev, int sdvo_reg)
+intel_sdvo_get_slave_addr(struct drm_device *dev, struct intel_sdvo *sdvo)
 {
 	struct drm_i915_private *dev_priv = dev->dev_private;
 	struct sdvo_device_mapping *my_mapping, *other_mapping;
 
-	if (IS_SDVOB(sdvo_reg)) {
+	if (sdvo->is_sdvob) {
 		my_mapping = &dev_priv->sdvo_mappings[0];
 		other_mapping = &dev_priv->sdvo_mappings[1];
 	} else {
@@ -1968,7 +1992,7 @@ intel_sdvo_get_slave_addr(struct drm_device *dev, int sdvo_reg)
 	/* No SDVO device info is found for another DVO port,
 	 * so use mapping assumption we had before BIOS parsing.
 	 */
-	if (IS_SDVOB(sdvo_reg))
+	if (sdvo->is_sdvob)
 		return 0x70;
 	else
 		return 0x72;
@@ -2193,6 +2217,10 @@ intel_sdvo_output_setup(struct intel_sdvo *intel_sdvo, uint16_t flags)
 		if (!intel_sdvo_tv_init(intel_sdvo, SDVO_OUTPUT_CVBS0))
 			return false;
 
+	if (flags & SDVO_OUTPUT_YPRPB0)
+		if (!intel_sdvo_tv_init(intel_sdvo, SDVO_OUTPUT_YPRPB0))
+			return false;
+
 	if (flags & SDVO_OUTPUT_RGB0)
 		if (!intel_sdvo_analog_init(intel_sdvo, 0))
 			return false;
@@ -2484,7 +2512,7 @@ intel_sdvo_init_ddc_proxy(struct intel_sdvo *sdvo,
 	return i2c_add_adapter(&sdvo->ddc) == 0;
 }
 
-bool intel_sdvo_init(struct drm_device *dev, int sdvo_reg)
+bool intel_sdvo_init(struct drm_device *dev, uint32_t sdvo_reg, bool is_sdvob)
 {
 	struct drm_i915_private *dev_priv = dev->dev_private;
 	struct intel_encoder *intel_encoder;
@@ -2496,7 +2524,8 @@ bool intel_sdvo_init(struct drm_device *dev, int sdvo_reg)
 		return false;
 
 	intel_sdvo->sdvo_reg = sdvo_reg;
-	intel_sdvo->slave_addr = intel_sdvo_get_slave_addr(dev, sdvo_reg) >> 1;
+	intel_sdvo->is_sdvob = is_sdvob;
+	intel_sdvo->slave_addr = intel_sdvo_get_slave_addr(dev, intel_sdvo) >> 1;
 	intel_sdvo_select_i2c_bus(dev_priv, intel_sdvo, sdvo_reg);
 	if (!intel_sdvo_init_ddc_proxy(intel_sdvo, dev)) {
 		kfree(intel_sdvo);
@@ -2513,13 +2542,13 @@ bool intel_sdvo_init(struct drm_device *dev, int sdvo_reg)
 		u8 byte;
 
 		if (!intel_sdvo_read_byte(intel_sdvo, i, &byte)) {
-			DRM_DEBUG_KMS("No SDVO device found on SDVO%c\n",
-				      IS_SDVOB(sdvo_reg) ? 'B' : 'C');
+			DRM_DEBUG_KMS("No SDVO device found on %s\n",
+				      SDVO_NAME(intel_sdvo));
 			goto err;
 		}
 	}
 
-	if (IS_SDVOB(sdvo_reg))
+	if (intel_sdvo->is_sdvob)
 		dev_priv->hotplug_supported_mask |= SDVOB_HOTPLUG_INT_STATUS;
 	else
 		dev_priv->hotplug_supported_mask |= SDVOC_HOTPLUG_INT_STATUS;
@@ -2540,8 +2569,8 @@ bool intel_sdvo_init(struct drm_device *dev, int sdvo_reg)
 
 	if (intel_sdvo_output_setup(intel_sdvo,
 				    intel_sdvo->caps.output_flags) != true) {
-		DRM_DEBUG_KMS("SDVO output failed to setup on SDVO%c\n",
-			      IS_SDVOB(sdvo_reg) ? 'B' : 'C');
+		DRM_DEBUG_KMS("SDVO output failed to setup on %s\n",
+			      SDVO_NAME(intel_sdvo));
 		goto err;
 	}
 
diff --git a/drivers/gpu/drm/i915/intel_sprite.c b/drivers/gpu/drm/i915/intel_sprite.c
index e90dfb625c4..2a20fb0781d 100644
--- a/drivers/gpu/drm/i915/intel_sprite.c
+++ b/drivers/gpu/drm/i915/intel_sprite.c
@@ -110,14 +110,18 @@ ivb_update_plane(struct drm_plane *plane, struct drm_framebuffer *fb,
 	 * when scaling is disabled.
 	 */
 	if (crtc_w != src_w || crtc_h != src_h) {
-		dev_priv->sprite_scaling_enabled = true;
-		sandybridge_update_wm(dev);
-		intel_wait_for_vblank(dev, pipe);
+		if (!dev_priv->sprite_scaling_enabled) {
+			dev_priv->sprite_scaling_enabled = true;
+			intel_update_watermarks(dev);
+			intel_wait_for_vblank(dev, pipe);
+		}
 		sprscale = SPRITE_SCALE_ENABLE | (src_w << 16) | src_h;
 	} else {
-		dev_priv->sprite_scaling_enabled = false;
-		/* potentially re-enable LP watermarks */
-		sandybridge_update_wm(dev);
+		if (dev_priv->sprite_scaling_enabled) {
+			dev_priv->sprite_scaling_enabled = false;
+			/* potentially re-enable LP watermarks */
+			intel_update_watermarks(dev);
+		}
 	}
 
 	I915_WRITE(SPRSTRIDE(pipe), fb->pitches[0]);
@@ -133,7 +137,7 @@ ivb_update_plane(struct drm_plane *plane, struct drm_framebuffer *fb,
 	I915_WRITE(SPRSIZE(pipe), (crtc_h << 16) | crtc_w);
 	I915_WRITE(SPRSCALE(pipe), sprscale);
 	I915_WRITE(SPRCTL(pipe), sprctl);
-	I915_WRITE(SPRSURF(pipe), obj->gtt_offset);
+	I915_MODIFY_DISPBASE(SPRSURF(pipe), obj->gtt_offset);
 	POSTING_READ(SPRSURF(pipe));
 }
 
@@ -149,8 +153,11 @@ ivb_disable_plane(struct drm_plane *plane)
 	/* Can't leave the scaler enabled... */
 	I915_WRITE(SPRSCALE(pipe), 0);
 	/* Activate double buffered register update */
-	I915_WRITE(SPRSURF(pipe), 0);
+	I915_MODIFY_DISPBASE(SPRSURF(pipe), 0);
 	POSTING_READ(SPRSURF(pipe));
+
+	dev_priv->sprite_scaling_enabled = false;
+	intel_update_watermarks(dev);
 }
 
 static int
@@ -208,7 +215,7 @@ ivb_get_colorkey(struct drm_plane *plane, struct drm_intel_sprite_colorkey *key)
 }
 
 static void
-snb_update_plane(struct drm_plane *plane, struct drm_framebuffer *fb,
+ilk_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,
 		 uint32_t x, uint32_t y,
@@ -218,7 +225,7 @@ snb_update_plane(struct drm_plane *plane, struct drm_framebuffer *fb,
 	struct drm_i915_private *dev_priv = dev->dev_private;
 	struct intel_plane *intel_plane = to_intel_plane(plane);
 	int pipe = intel_plane->pipe, pixel_size;
-	u32 dvscntr, dvsscale = 0;
+	u32 dvscntr, dvsscale;
 
 	dvscntr = I915_READ(DVSCNTR(pipe));
 
@@ -262,8 +269,8 @@ snb_update_plane(struct drm_plane *plane, struct drm_framebuffer *fb,
 	if (obj->tiling_mode != I915_TILING_NONE)
 		dvscntr |= DVS_TILED;
 
-	/* must disable */
-	dvscntr |= DVS_TRICKLE_FEED_DISABLE;
+	if (IS_GEN6(dev))
+		dvscntr |= DVS_TRICKLE_FEED_DISABLE; /* must disable */
 	dvscntr |= DVS_ENABLE;
 
 	/* Sizes are 0 based */
@@ -274,7 +281,8 @@ snb_update_plane(struct drm_plane *plane, struct drm_framebuffer *fb,
 
 	intel_update_sprite_watermarks(dev, pipe, crtc_w, pixel_size);
 
-	if (crtc_w != src_w || crtc_h != src_h)
+	dvsscale = 0;
+	if (IS_GEN5(dev) || crtc_w != src_w || crtc_h != src_h)
 		dvsscale = DVS_SCALE_ENABLE | (src_w << 16) | src_h;
 
 	I915_WRITE(DVSSTRIDE(pipe), fb->pitches[0]);
@@ -290,12 +298,12 @@ snb_update_plane(struct drm_plane *plane, struct drm_framebuffer *fb,
 	I915_WRITE(DVSSIZE(pipe), (crtc_h << 16) | crtc_w);
 	I915_WRITE(DVSSCALE(pipe), dvsscale);
 	I915_WRITE(DVSCNTR(pipe), dvscntr);
-	I915_WRITE(DVSSURF(pipe), obj->gtt_offset);
+	I915_MODIFY_DISPBASE(DVSSURF(pipe), obj->gtt_offset);
 	POSTING_READ(DVSSURF(pipe));
 }
 
 static void
-snb_disable_plane(struct drm_plane *plane)
+ilk_disable_plane(struct drm_plane *plane)
 {
 	struct drm_device *dev = plane->dev;
 	struct drm_i915_private *dev_priv = dev->dev_private;
@@ -306,7 +314,7 @@ snb_disable_plane(struct drm_plane *plane)
 	/* Disable the scaler */
 	I915_WRITE(DVSSCALE(pipe), 0);
 	/* Flush double buffered register updates */
-	I915_WRITE(DVSSURF(pipe), 0);
+	I915_MODIFY_DISPBASE(DVSSURF(pipe), 0);
 	POSTING_READ(DVSSURF(pipe));
 }
 
@@ -333,7 +341,7 @@ intel_disable_primary(struct drm_crtc *crtc)
 }
 
 static int
-snb_update_colorkey(struct drm_plane *plane,
+ilk_update_colorkey(struct drm_plane *plane,
 		    struct drm_intel_sprite_colorkey *key)
 {
 	struct drm_device *dev = plane->dev;
@@ -362,7 +370,7 @@ snb_update_colorkey(struct drm_plane *plane,
 }
 
 static void
-snb_get_colorkey(struct drm_plane *plane, struct drm_intel_sprite_colorkey *key)
+ilk_get_colorkey(struct drm_plane *plane, struct drm_intel_sprite_colorkey *key)
 {
 	struct drm_device *dev = plane->dev;
 	struct drm_i915_private *dev_priv = dev->dev_private;
@@ -550,14 +558,13 @@ int intel_sprite_set_colorkey(struct drm_device *dev, void *data,
 			      struct drm_file *file_priv)
 {
 	struct drm_intel_sprite_colorkey *set = data;
-	struct drm_i915_private *dev_priv = dev->dev_private;
 	struct drm_mode_object *obj;
 	struct drm_plane *plane;
 	struct intel_plane *intel_plane;
 	int ret = 0;
 
-	if (!dev_priv)
-		return -EINVAL;
+	if (!drm_core_check_feature(dev, DRIVER_MODESET))
+		return -ENODEV;
 
 	/* Make sure we don't try to enable both src & dest simultaneously */
 	if ((set->flags & (I915_SET_COLORKEY_DESTINATION | I915_SET_COLORKEY_SOURCE)) == (I915_SET_COLORKEY_DESTINATION | I915_SET_COLORKEY_SOURCE))
@@ -584,14 +591,13 @@ int intel_sprite_get_colorkey(struct drm_device *dev, void *data,
 			      struct drm_file *file_priv)
 {
 	struct drm_intel_sprite_colorkey *get = data;
-	struct drm_i915_private *dev_priv = dev->dev_private;
 	struct drm_mode_object *obj;
 	struct drm_plane *plane;
 	struct intel_plane *intel_plane;
 	int ret = 0;
 
-	if (!dev_priv)
-		return -EINVAL;
+	if (!drm_core_check_feature(dev, DRIVER_MODESET))
+		return -ENODEV;
 
 	mutex_lock(&dev->mode_config.mutex);
 
@@ -616,6 +622,14 @@ static const struct drm_plane_funcs intel_plane_funcs = {
 	.destroy = intel_destroy_plane,
 };
 
+static uint32_t ilk_plane_formats[] = {
+	DRM_FORMAT_XRGB8888,
+	DRM_FORMAT_YUYV,
+	DRM_FORMAT_YVYU,
+	DRM_FORMAT_UYVY,
+	DRM_FORMAT_VYUY,
+};
+
 static uint32_t snb_plane_formats[] = {
 	DRM_FORMAT_XBGR8888,
 	DRM_FORMAT_XRGB8888,
@@ -630,34 +644,56 @@ intel_plane_init(struct drm_device *dev, enum pipe pipe)
 {
 	struct intel_plane *intel_plane;
 	unsigned long possible_crtcs;
+	const uint32_t *plane_formats;
+	int num_plane_formats;
 	int ret;
 
-	if (!(IS_GEN6(dev) || IS_GEN7(dev)))
+	if (INTEL_INFO(dev)->gen < 5)
 		return -ENODEV;
 
 	intel_plane = kzalloc(sizeof(struct intel_plane), GFP_KERNEL);
 	if (!intel_plane)
 		return -ENOMEM;
 
-	if (IS_GEN6(dev)) {
+	switch (INTEL_INFO(dev)->gen) {
+	case 5:
+	case 6:
 		intel_plane->max_downscale = 16;
-		intel_plane->update_plane = snb_update_plane;
-		intel_plane->disable_plane = snb_disable_plane;
-		intel_plane->update_colorkey = snb_update_colorkey;
-		intel_plane->get_colorkey = snb_get_colorkey;
-	} else if (IS_GEN7(dev)) {
+		intel_plane->update_plane = ilk_update_plane;
+		intel_plane->disable_plane = ilk_disable_plane;
+		intel_plane->update_colorkey = ilk_update_colorkey;
+		intel_plane->get_colorkey = ilk_get_colorkey;
+
+		if (IS_GEN6(dev)) {
+			plane_formats = snb_plane_formats;
+			num_plane_formats = ARRAY_SIZE(snb_plane_formats);
+		} else {
+			plane_formats = ilk_plane_formats;
+			num_plane_formats = ARRAY_SIZE(ilk_plane_formats);
+		}
+		break;
+
+	case 7:
 		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:
+		return -ENODEV;
 	}
 
 	intel_plane->pipe = pipe;
 	possible_crtcs = (1 << pipe);
 	ret = drm_plane_init(dev, &intel_plane->base, possible_crtcs,
-			     &intel_plane_funcs, snb_plane_formats,
-			     ARRAY_SIZE(snb_plane_formats), false);
+			     &intel_plane_funcs,
+			     plane_formats, num_plane_formats,
+			     false);
 	if (ret)
 		kfree(intel_plane);
 
diff --git a/drivers/gpu/drm/i915/intel_tv.c b/drivers/gpu/drm/i915/intel_tv.c
index 05f765ef546..3346612d295 100644
--- a/drivers/gpu/drm/i915/intel_tv.c
+++ b/drivers/gpu/drm/i915/intel_tv.c
@@ -811,7 +811,7 @@ intel_tv_mode_lookup(const char *tv_format)
 {
 	int i;
 
-	for (i = 0; i < sizeof(tv_modes) / sizeof(tv_modes[0]); i++) {
+	for (i = 0; i < ARRAY_SIZE(tv_modes); i++) {
 		const struct tv_mode *tv_mode = &tv_modes[i];
 
 		if (!strcmp(tv_format, tv_mode->name))
@@ -1153,6 +1153,15 @@ intel_tv_detect_type(struct intel_tv *intel_tv,
 		   DAC_B_0_7_V |
 		   DAC_C_0_7_V);
 
+
+	/*
+	 * The TV sense state should be cleared to zero on cantiga platform. Otherwise
+	 * the TV is misdetected. This is hardware requirement.
+	 */
+	if (IS_GM45(dev))
+		tv_dac &= ~(TVDAC_STATE_CHG_EN | TVDAC_A_SENSE_CTL |
+			    TVDAC_B_SENSE_CTL | TVDAC_C_SENSE_CTL);
+
 	I915_WRITE(TV_CTL, tv_ctl);
 	I915_WRITE(TV_DAC, tv_dac);
 	POSTING_READ(TV_DAC);
@@ -1240,11 +1249,8 @@ intel_tv_detect(struct drm_connector *connector, bool force)
 	int type;
 
 	mode = reported_modes[0];
-	drm_mode_set_crtcinfo(&mode, 0);
 
-	if (intel_tv->base.base.crtc && intel_tv->base.base.crtc->enabled) {
-		type = intel_tv_detect_type(intel_tv, connector);
-	} else if (force) {
+	if (force) {
 		struct intel_load_detect_pipe tmp;
 
 		if (intel_get_load_detect_pipe(&intel_tv->base, connector,
diff --git a/drivers/gpu/drm/mgag200/Kconfig b/drivers/gpu/drm/mgag200/Kconfig
new file mode 100644
index 00000000000..d63013497f6
--- /dev/null
+++ b/drivers/gpu/drm/mgag200/Kconfig
@@ -0,0 +1,15 @@
+config DRM_MGAG200
+	tristate "Kernel modesetting driver for MGA G200 server engines"
+	depends on DRM && PCI && EXPERIMENTAL
+	select FB_SYS_FILLRECT
+	select FB_SYS_COPYAREA
+	select FB_SYS_IMAGEBLIT
+	select DRM_KMS_HELPER
+	select DRM_TTM
+	help
+	 This is a KMS driver for the MGA G200 server chips, it
+         does not support the original MGA G200 or any of the desktop
+         chips. It requires 0.3.0 of the modesetting userspace driver,
+         and a version of mga driver that will fail on KMS enabled
+         devices.
+
diff --git a/drivers/gpu/drm/mgag200/Makefile b/drivers/gpu/drm/mgag200/Makefile
new file mode 100644
index 00000000000..7db592eedbf
--- /dev/null
+++ b/drivers/gpu/drm/mgag200/Makefile
@@ -0,0 +1,5 @@
+ccflags-y := -Iinclude/drm
+mgag200-y   := mgag200_main.o mgag200_mode.o \
+	mgag200_drv.o mgag200_fb.o mgag200_i2c.o mgag200_ttm.o
+
+obj-$(CONFIG_DRM_MGAG200) += mgag200.o
diff --git a/drivers/gpu/drm/mgag200/mgag200_drv.c b/drivers/gpu/drm/mgag200/mgag200_drv.c
new file mode 100644
index 00000000000..3c8e04f5471
--- /dev/null
+++ b/drivers/gpu/drm/mgag200/mgag200_drv.c
@@ -0,0 +1,116 @@
+/*
+ * Copyright 2012 Red Hat
+ *
+ * This file is subject to the terms and conditions of the GNU General
+ * Public License version 2. See the file COPYING in the main
+ * directory of this archive for more details.
+ *
+ * Authors: Matthew Garrett
+ *          Dave Airlie
+ */
+#include <linux/module.h>
+#include <linux/console.h>
+#include "drmP.h"
+#include "drm.h"
+
+#include "mgag200_drv.h"
+
+#include "drm_pciids.h"
+
+/*
+ * This is the generic driver code. This binds the driver to the drm core,
+ * which then performs further device association and calls our graphics init
+ * functions
+ */
+int mgag200_modeset = -1;
+
+MODULE_PARM_DESC(modeset, "Disable/Enable modesetting");
+module_param_named(modeset, mgag200_modeset, int, 0400);
+
+static struct drm_driver driver;
+
+static DEFINE_PCI_DEVICE_TABLE(pciidlist) = {
+	{ PCI_VENDOR_ID_MATROX, 0x522, PCI_ANY_ID, PCI_ANY_ID, 0, 0, G200_SE_A },
+	{ PCI_VENDOR_ID_MATROX, 0x524, PCI_ANY_ID, PCI_ANY_ID, 0, 0, G200_SE_B },
+	{ PCI_VENDOR_ID_MATROX, 0x530, PCI_ANY_ID, PCI_ANY_ID, 0, 0, G200_EV },
+	{ PCI_VENDOR_ID_MATROX, 0x532, PCI_ANY_ID, PCI_ANY_ID, 0, 0, G200_WB },
+	{ PCI_VENDOR_ID_MATROX, 0x533, PCI_ANY_ID, PCI_ANY_ID, 0, 0, G200_EH },
+	{ PCI_VENDOR_ID_MATROX, 0x534, PCI_ANY_ID, PCI_ANY_ID, 0, 0, G200_ER },
+	{0,}
+};
+
+MODULE_DEVICE_TABLE(pci, pciidlist);
+
+static int __devinit
+mga_pci_probe(struct pci_dev *pdev, const struct pci_device_id *ent)
+{
+	return drm_get_pci_dev(pdev, ent, &driver);
+}
+
+static void mga_pci_remove(struct pci_dev *pdev)
+{
+	struct drm_device *dev = pci_get_drvdata(pdev);
+
+	drm_put_dev(dev);
+}
+
+static const struct file_operations mgag200_driver_fops = {
+	.owner = THIS_MODULE,
+	.open = drm_open,
+	.release = drm_release,
+	.unlocked_ioctl = drm_ioctl,
+	.mmap = mgag200_mmap,
+	.poll = drm_poll,
+	.fasync = drm_fasync,
+	.read = drm_read,
+};
+
+static struct drm_driver driver = {
+	.driver_features = DRIVER_GEM | DRIVER_MODESET | DRIVER_USE_MTRR,
+	.load = mgag200_driver_load,
+	.unload = mgag200_driver_unload,
+	.fops = &mgag200_driver_fops,
+	.name = DRIVER_NAME,
+	.desc = DRIVER_DESC,
+	.date = DRIVER_DATE,
+	.major = DRIVER_MAJOR,
+	.minor = DRIVER_MINOR,
+	.patchlevel = DRIVER_PATCHLEVEL,
+
+	.gem_init_object = mgag200_gem_init_object,
+	.gem_free_object = mgag200_gem_free_object,
+	.dumb_create = mgag200_dumb_create,
+	.dumb_map_offset = mgag200_dumb_mmap_offset,
+	.dumb_destroy = mgag200_dumb_destroy,
+};
+
+static struct pci_driver mgag200_pci_driver = {
+	.name = DRIVER_NAME,
+	.id_table = pciidlist,
+	.probe = mga_pci_probe,
+	.remove = mga_pci_remove,
+};
+
+static int __init mgag200_init(void)
+{
+#ifdef CONFIG_VGA_CONSOLE
+	if (vgacon_text_force() && mgag200_modeset == -1)
+		return -EINVAL;
+#endif
+
+	if (mgag200_modeset == 0)
+		return -EINVAL;
+	return drm_pci_init(&driver, &mgag200_pci_driver);
+}
+
+static void __exit mgag200_exit(void)
+{
+	drm_pci_exit(&driver, &mgag200_pci_driver);
+}
+
+module_init(mgag200_init);
+module_exit(mgag200_exit);
+
+MODULE_AUTHOR(DRIVER_AUTHOR);
+MODULE_DESCRIPTION(DRIVER_DESC);
+MODULE_LICENSE("GPL");
diff --git a/drivers/gpu/drm/mgag200/mgag200_drv.h b/drivers/gpu/drm/mgag200/mgag200_drv.h
new file mode 100644
index 00000000000..6f13b356323
--- /dev/null
+++ b/drivers/gpu/drm/mgag200/mgag200_drv.h
@@ -0,0 +1,276 @@
+/*
+ * Copyright 2010 Matt Turner.
+ * Copyright 2012 Red Hat 
+ *
+ * This file is subject to the terms and conditions of the GNU General
+ * Public License version 2. See the file COPYING in the main
+ * directory of this archive for more details.
+ *
+ * Authors: Matthew Garrett
+ * 	    Matt Turner
+ *	    Dave Airlie
+ */
+#ifndef __MGAG200_DRV_H__
+#define __MGAG200_DRV_H__
+
+#include <video/vga.h>
+
+#include "drm/drm_fb_helper.h"
+#include "ttm/ttm_bo_api.h"
+#include "ttm/ttm_bo_driver.h"
+#include "ttm/ttm_placement.h"
+#include "ttm/ttm_memory.h"
+#include "ttm/ttm_module.h"
+
+#include <linux/i2c.h>
+#include <linux/i2c-algo-bit.h>
+
+#include "mgag200_reg.h"
+
+#define DRIVER_AUTHOR		"Matthew Garrett"
+
+#define DRIVER_NAME		"mgag200"
+#define DRIVER_DESC		"MGA G200 SE"
+#define DRIVER_DATE		"20110418"
+
+#define DRIVER_MAJOR		1
+#define DRIVER_MINOR		0
+#define DRIVER_PATCHLEVEL	0
+
+#define MGAG200FB_CONN_LIMIT 1
+
+#define RREG8(reg) ioread8(((void __iomem *)mdev->rmmio) + (reg))
+#define WREG8(reg, v) iowrite8(v, ((void __iomem *)mdev->rmmio) + (reg))
+#define RREG32(reg) ioread32(((void __iomem *)mdev->rmmio) + (reg))
+#define WREG32(reg, v) iowrite32(v, ((void __iomem *)mdev->rmmio) + (reg))
+
+#define ATTR_INDEX 0x1fc0
+#define ATTR_DATA 0x1fc1
+
+#define WREG_ATTR(reg, v)					\
+	do {							\
+		RREG8(0x1fda);					\
+		WREG8(ATTR_INDEX, reg);				\
+		WREG8(ATTR_DATA, v);				\
+	} while (0)						\
+
+#define WREG_SEQ(reg, v)					\
+	do {							\
+		WREG8(MGAREG_SEQ_INDEX, reg);			\
+		WREG8(MGAREG_SEQ_DATA, v);			\
+	} while (0)						\
+
+#define WREG_CRT(reg, v)					\
+	do {							\
+		WREG8(MGAREG_CRTC_INDEX, reg);			\
+		WREG8(MGAREG_CRTC_DATA, v);			\
+	} while (0)						\
+
+
+#define WREG_ECRT(reg, v)					\
+	do {							\
+		WREG8(MGAREG_CRTCEXT_INDEX, reg);				\
+		WREG8(MGAREG_CRTCEXT_DATA, v);				\
+	} while (0)						\
+
+#define GFX_INDEX 0x1fce
+#define GFX_DATA 0x1fcf
+
+#define WREG_GFX(reg, v)					\
+	do {							\
+		WREG8(GFX_INDEX, reg);				\
+		WREG8(GFX_DATA, v);				\
+	} while (0)						\
+
+#define DAC_INDEX 0x3c00
+#define DAC_DATA 0x3c0a
+
+#define WREG_DAC(reg, v)					\
+	do {							\
+		WREG8(DAC_INDEX, reg);				\
+		WREG8(DAC_DATA, v);				\
+	} while (0)						\
+
+#define MGA_MISC_OUT 0x1fc2
+#define MGA_MISC_IN 0x1fcc
+
+#define MGAG200_MAX_FB_HEIGHT 4096
+#define MGAG200_MAX_FB_WIDTH 4096
+
+#define MATROX_DPMS_CLEARED (-1)
+
+#define to_mga_crtc(x) container_of(x, struct mga_crtc, base)
+#define to_mga_encoder(x) container_of(x, struct mga_encoder, base)
+#define to_mga_connector(x) container_of(x, struct mga_connector, base)
+#define to_mga_framebuffer(x) container_of(x, struct mga_framebuffer, base)
+
+struct mga_framebuffer {
+	struct drm_framebuffer base;
+	struct drm_gem_object *obj;
+};
+
+struct mga_fbdev {
+	struct drm_fb_helper helper;
+	struct mga_framebuffer mfb;
+	struct list_head fbdev_list;
+	void *sysram;
+	int size;
+	struct ttm_bo_kmap_obj mapping;
+};
+
+struct mga_crtc {
+	struct drm_crtc base;
+	u8 lut_r[256], lut_g[256], lut_b[256];
+	int last_dpms;
+	bool enabled;
+};
+
+struct mga_mode_info {
+	bool mode_config_initialized;
+	struct mga_crtc *crtc;
+};
+
+struct mga_encoder {
+	struct drm_encoder base;
+	int last_dpms;
+};
+
+
+struct mga_i2c_chan {
+	struct i2c_adapter adapter;
+	struct drm_device *dev;
+	struct i2c_algo_bit_data bit;
+	int data, clock;
+};
+
+struct mga_connector {
+	struct drm_connector base;
+	struct mga_i2c_chan *i2c;
+};
+
+
+struct mga_mc {
+	resource_size_t			vram_size;
+	resource_size_t			vram_base;
+	resource_size_t			vram_window;
+};
+
+enum mga_type {
+	G200_SE_A,
+	G200_SE_B,
+	G200_WB,
+	G200_EV,
+	G200_EH,
+	G200_ER,
+};
+
+#define IS_G200_SE(mdev) (mdev->type == G200_SE_A || mdev->type == G200_SE_B)
+
+struct mga_device {
+	struct drm_device		*dev;
+	unsigned long			flags;
+
+	resource_size_t			rmmio_base;
+	resource_size_t			rmmio_size;
+	void __iomem			*rmmio;
+
+	drm_local_map_t			*framebuffer;
+
+	struct mga_mc			mc;
+	struct mga_mode_info		mode_info;
+
+	struct mga_fbdev *mfbdev;
+
+	bool				suspended;
+	int				num_crtc;
+	enum mga_type			type;
+	int				has_sdram;
+	struct drm_display_mode		mode;
+
+	int bpp_shifts[4];
+
+	int fb_mtrr;
+
+	struct {
+		struct drm_global_reference mem_global_ref;
+		struct ttm_bo_global_ref bo_global_ref;
+		struct ttm_bo_device bdev;
+		atomic_t validate_sequence;
+	} ttm;
+
+	u32 reg_1e24; /* SE model number */
+};
+
+
+struct mgag200_bo {
+	struct ttm_buffer_object bo;
+	struct ttm_placement placement;
+	struct ttm_bo_kmap_obj kmap;
+	struct drm_gem_object gem;
+	u32 placements[3];
+	int pin_count;
+};
+#define gem_to_mga_bo(gobj) container_of((gobj), struct mgag200_bo, gem)
+
+static inline struct mgag200_bo *
+mgag200_bo(struct ttm_buffer_object *bo)
+{
+	return container_of(bo, struct mgag200_bo, bo);
+}
+				/* mga_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,
+			     u16 *blue, int regno);
+
+				/* mgag200_mode.c */
+int mgag200_modeset_init(struct mga_device *mdev);
+void mgag200_modeset_fini(struct mga_device *mdev);
+
+				/* mga_fbdev.c */
+int mgag200_fbdev_init(struct mga_device *mdev);
+void mgag200_fbdev_fini(struct mga_device *mdev);
+
+				/* mgag200_main.c */
+int mgag200_framebuffer_init(struct drm_device *dev,
+			     struct mga_framebuffer *mfb,
+			     struct drm_mode_fb_cmd2 *mode_cmd,
+			     struct drm_gem_object *obj);
+
+
+int mgag200_driver_load(struct drm_device *dev, unsigned long flags);
+int mgag200_driver_unload(struct drm_device *dev);
+int mgag200_gem_create(struct drm_device *dev,
+		   u32 size, bool iskernel,
+		       struct drm_gem_object **obj);
+int mgag200_gem_init_object(struct drm_gem_object *obj);
+int mgag200_dumb_create(struct drm_file *file,
+			struct drm_device *dev,
+			struct drm_mode_create_dumb *args);
+int mgag200_dumb_destroy(struct drm_file *file,
+			 struct drm_device *dev,
+			 uint32_t handle);
+void mgag200_gem_free_object(struct drm_gem_object *obj);
+int
+mgag200_dumb_mmap_offset(struct drm_file *file,
+			 struct drm_device *dev,
+			 uint32_t handle,
+			 uint64_t *offset);
+				/* mga_i2c.c */
+struct mga_i2c_chan *mgag200_i2c_create(struct drm_device *dev);
+void mgag200_i2c_destroy(struct mga_i2c_chan *i2c);
+
+#define DRM_FILE_PAGE_OFFSET (0x100000000ULL >> PAGE_SHIFT)
+void mgag200_ttm_placement(struct mgag200_bo *bo, int domain);
+
+int mgag200_bo_reserve(struct mgag200_bo *bo, bool no_wait);
+void mgag200_bo_unreserve(struct mgag200_bo *bo);
+int mgag200_bo_create(struct drm_device *dev, int size, int align,
+		      uint32_t flags, struct mgag200_bo **pastbo);
+int mgag200_mm_init(struct mga_device *mdev);
+void mgag200_mm_fini(struct mga_device *mdev);
+int mgag200_mmap(struct file *filp, struct vm_area_struct *vma);
+int mgag200_bo_pin(struct mgag200_bo *bo, u32 pl_flag, u64 *gpu_addr);
+int mgag200_bo_unpin(struct mgag200_bo *bo);
+int mgag200_bo_push_sysram(struct mgag200_bo *bo);
+#endif				/* __MGAG200_DRV_H__ */
diff --git a/drivers/gpu/drm/mgag200/mgag200_fb.c b/drivers/gpu/drm/mgag200/mgag200_fb.c
new file mode 100644
index 00000000000..880d3369760
--- /dev/null
+++ b/drivers/gpu/drm/mgag200/mgag200_fb.c
@@ -0,0 +1,294 @@
+/*
+ * Copyright 2010 Matt Turner.
+ * Copyright 2012 Red Hat
+ *
+ * This file is subject to the terms and conditions of the GNU General
+ * Public License version 2. See the file COPYING in the main
+ * directory of this archive for more details.
+ *
+ * Authors: Matthew Garrett
+ *          Matt Turner
+ *          Dave Airlie
+ */
+#include <linux/module.h>
+#include "drmP.h"
+#include "drm.h"
+#include "drm_fb_helper.h"
+
+#include <linux/fb.h>
+
+#include "mgag200_drv.h"
+
+static void mga_dirty_update(struct mga_fbdev *mfbdev,
+			     int x, int y, int width, int height)
+{
+	int i;
+	struct drm_gem_object *obj;
+	struct mgag200_bo *bo;
+	int src_offset, dst_offset;
+	int bpp = (mfbdev->mfb.base.bits_per_pixel + 7)/8;
+	int ret;
+	bool unmap = false;
+
+	obj = mfbdev->mfb.obj;
+	bo = gem_to_mga_bo(obj);
+
+	ret = mgag200_bo_reserve(bo, true);
+	if (ret) {
+		DRM_ERROR("failed to reserve fb bo\n");
+		return;
+	}
+
+	if (!bo->kmap.virtual) {
+		ret = ttm_bo_kmap(&bo->bo, 0, bo->bo.num_pages, &bo->kmap);
+		if (ret) {
+			DRM_ERROR("failed to kmap fb updates\n");
+			mgag200_bo_unreserve(bo);
+			return;
+		}
+		unmap = true;
+	}
+	for (i = y; i < y + height; 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);
+
+	}
+	if (unmap)
+		ttm_bo_kunmap(&bo->kmap);
+
+	mgag200_bo_unreserve(bo);
+}
+
+static void mga_fillrect(struct fb_info *info,
+			 const struct fb_fillrect *rect)
+{
+	struct mga_fbdev *mfbdev = info->par;
+	sys_fillrect(info, rect);
+	mga_dirty_update(mfbdev, rect->dx, rect->dy, rect->width,
+			 rect->height);
+}
+
+static void mga_copyarea(struct fb_info *info,
+			 const struct fb_copyarea *area)
+{
+	struct mga_fbdev *mfbdev = info->par;
+	sys_copyarea(info, area);
+	mga_dirty_update(mfbdev, area->dx, area->dy, area->width,
+			 area->height);
+}
+
+static void mga_imageblit(struct fb_info *info,
+			  const struct fb_image *image)
+{
+	struct mga_fbdev *mfbdev = info->par;
+	sys_imageblit(info, image);
+	mga_dirty_update(mfbdev, image->dx, image->dy, image->width,
+			 image->height);
+}
+
+
+static struct fb_ops mgag200fb_ops = {
+	.owner = THIS_MODULE,
+	.fb_check_var = drm_fb_helper_check_var,
+	.fb_set_par = drm_fb_helper_set_par,
+	.fb_fillrect = mga_fillrect,
+	.fb_copyarea = mga_copyarea,
+	.fb_imageblit = mga_imageblit,
+	.fb_pan_display = drm_fb_helper_pan_display,
+	.fb_blank = drm_fb_helper_blank,
+	.fb_setcmap = drm_fb_helper_setcmap,
+};
+
+static int mgag200fb_create_object(struct mga_fbdev *afbdev,
+				   struct drm_mode_fb_cmd2 *mode_cmd,
+				   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);
+	if (ret)
+		return ret;
+
+	*gobj_p = gobj;
+	return ret;
+}
+
+static int mgag200fb_create(struct mga_fbdev *mfbdev,
+			   struct drm_fb_helper_surface_size *sizes)
+{
+	struct drm_device *dev = mfbdev->helper.dev;
+	struct drm_mode_fb_cmd2 mode_cmd;
+	struct mga_device *mdev = dev->dev_private;
+	struct fb_info *info;
+	struct drm_framebuffer *fb;
+	struct drm_gem_object *gobj = NULL;
+	struct device *device = &dev->pdev->dev;
+	struct mgag200_bo *bo;
+	int ret;
+	void *sysram;
+	int size;
+
+	mode_cmd.width = sizes->surface_width;
+	mode_cmd.height = sizes->surface_height;
+	mode_cmd.pitches[0] = mode_cmd.width * ((sizes->surface_bpp + 7) / 8);
+
+	mode_cmd.pixel_format = drm_mode_legacy_fb_format(sizes->surface_bpp,
+							  sizes->surface_depth);
+	size = mode_cmd.pitches[0] * mode_cmd.height;
+
+	ret = mgag200fb_create_object(mfbdev, &mode_cmd, &gobj);
+	if (ret) {
+		DRM_ERROR("failed to create fbcon backing object %d\n", ret);
+		return ret;
+	}
+	bo = gem_to_mga_bo(gobj);
+
+	sysram = vmalloc(size);
+	if (!sysram)
+		return -ENOMEM;
+
+	info = framebuffer_alloc(0, device);
+	if (info == NULL)
+		return -ENOMEM;
+
+	info->par = mfbdev;
+
+	ret = mgag200_framebuffer_init(dev, &mfbdev->mfb, &mode_cmd, gobj);
+	if (ret)
+		return ret;
+
+	mfbdev->sysram = sysram;
+	mfbdev->size = size;
+
+	fb = &mfbdev->mfb.base;
+
+	/* setup helper */
+	mfbdev->helper.fb = fb;
+	mfbdev->helper.fbdev = info;
+
+	ret = fb_alloc_cmap(&info->cmap, 256, 0);
+	if (ret) {
+		DRM_ERROR("%s: can't allocate color map\n", info->fix.id);
+		ret = -ENOMEM;
+		goto out;
+	}
+
+	strcpy(info->fix.id, "mgadrmfb");
+
+	info->flags = FBINFO_DEFAULT | FBINFO_CAN_FORCE_OUTPUT;
+	info->fbops = &mgag200fb_ops;
+
+	/* setup aperture base/size for vesafb takeover */
+	info->apertures = alloc_apertures(1);
+	if (!info->apertures) {
+		ret = -ENOMEM;
+		goto out;
+	}
+	info->apertures->ranges[0].base = mdev->dev->mode_config.fb_base;
+	info->apertures->ranges[0].size = mdev->mc.vram_size;
+
+	drm_fb_helper_fill_fix(info, fb->pitches[0], fb->depth);
+	drm_fb_helper_fill_var(info, &mfbdev->helper, sizes->fb_width,
+			       sizes->fb_height);
+
+	info->screen_base = sysram;
+	info->screen_size = size;
+	info->pixmap.flags = FB_PIXMAP_SYSTEM;
+
+	DRM_DEBUG_KMS("allocated %dx%d\n",
+		      fb->width, fb->height);
+	return 0;
+out:
+	return ret;
+}
+
+static int mga_fb_find_or_create_single(struct drm_fb_helper *helper,
+					   struct drm_fb_helper_surface_size
+					   *sizes)
+{
+	struct mga_fbdev *mfbdev = (struct mga_fbdev *)helper;
+	int new_fb = 0;
+	int ret;
+
+	if (!helper->fb) {
+		ret = mgag200fb_create(mfbdev, sizes);
+		if (ret)
+			return ret;
+		new_fb = 1;
+	}
+	return new_fb;
+}
+
+static int mga_fbdev_destroy(struct drm_device *dev,
+				struct mga_fbdev *mfbdev)
+{
+	struct fb_info *info;
+	struct mga_framebuffer *mfb = &mfbdev->mfb;
+
+	if (mfbdev->helper.fbdev) {
+		info = mfbdev->helper.fbdev;
+
+		unregister_framebuffer(info);
+		if (info->cmap.len)
+			fb_dealloc_cmap(&info->cmap);
+		framebuffer_release(info);
+	}
+
+	if (mfb->obj) {
+		drm_gem_object_unreference_unlocked(mfb->obj);
+		mfb->obj = NULL;
+	}
+	drm_fb_helper_fini(&mfbdev->helper);
+	vfree(mfbdev->sysram);
+	drm_framebuffer_cleanup(&mfb->base);
+
+	return 0;
+}
+
+static struct drm_fb_helper_funcs mga_fb_helper_funcs = {
+	.gamma_set = mga_crtc_fb_gamma_set,
+	.gamma_get = mga_crtc_fb_gamma_get,
+	.fb_probe = mga_fb_find_or_create_single,
+};
+
+int mgag200_fbdev_init(struct mga_device *mdev)
+{
+	struct mga_fbdev *mfbdev;
+	int ret;
+
+	mfbdev = kzalloc(sizeof(struct mga_fbdev), GFP_KERNEL);
+	if (!mfbdev)
+		return -ENOMEM;
+
+	mdev->mfbdev = mfbdev;
+	mfbdev->helper.funcs = &mga_fb_helper_funcs;
+
+	ret = drm_fb_helper_init(mdev->dev, &mfbdev->helper,
+				 mdev->num_crtc, MGAG200FB_CONN_LIMIT);
+	if (ret) {
+		kfree(mfbdev);
+		return ret;
+	}
+	drm_fb_helper_single_add_all_connectors(&mfbdev->helper);
+	drm_fb_helper_initial_config(&mfbdev->helper, 32);
+
+	return 0;
+}
+
+void mgag200_fbdev_fini(struct mga_device *mdev)
+{
+	if (!mdev->mfbdev)
+		return;
+
+	mga_fbdev_destroy(mdev->dev, mdev->mfbdev);
+	kfree(mdev->mfbdev);
+	mdev->mfbdev = NULL;
+}
diff --git a/drivers/gpu/drm/mgag200/mgag200_i2c.c b/drivers/gpu/drm/mgag200/mgag200_i2c.c
new file mode 100644
index 00000000000..dd3568a1b6b
--- /dev/null
+++ b/drivers/gpu/drm/mgag200/mgag200_i2c.c
@@ -0,0 +1,156 @@
+/*
+ * 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, 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: Dave Airlie <airlied@redhat.com>
+ */
+#include <linux/export.h>
+#include <linux/i2c.h>
+#include <linux/i2c-algo-bit.h>
+#include "drmP.h"
+#include "drm.h"
+
+#include "mgag200_drv.h"
+
+static int mga_i2c_read_gpio(struct mga_device *mdev)
+{
+	WREG8(DAC_INDEX, MGA1064_GEN_IO_DATA);
+	return RREG8(DAC_DATA);
+}
+
+static void mga_i2c_set_gpio(struct mga_device *mdev, int mask, int val)
+{
+	int tmp;
+
+	WREG8(DAC_INDEX, MGA1064_GEN_IO_CTL);
+	tmp = (RREG8(DAC_DATA) & mask) | val;
+	WREG_DAC(MGA1064_GEN_IO_CTL, tmp);
+	WREG_DAC(MGA1064_GEN_IO_DATA, 0);
+}
+
+static inline void mga_i2c_set(struct mga_device *mdev, int mask, int state)
+{
+	if (state)
+		state = 0;
+	else
+		state = mask;
+	mga_i2c_set_gpio(mdev, ~mask, state);
+}
+
+static void mga_gpio_setsda(void *data, int state)
+{
+	struct mga_i2c_chan *i2c = data;
+	struct mga_device *mdev = i2c->dev->dev_private;
+	mga_i2c_set(mdev, i2c->data, state);
+}
+
+static void mga_gpio_setscl(void *data, int state)
+{
+	struct mga_i2c_chan *i2c = data;
+	struct mga_device *mdev = i2c->dev->dev_private;
+	mga_i2c_set(mdev, i2c->clock, state);
+}
+
+static int mga_gpio_getsda(void *data)
+{
+	struct mga_i2c_chan *i2c = data;
+	struct mga_device *mdev = i2c->dev->dev_private;
+	return (mga_i2c_read_gpio(mdev) & i2c->data) ? 1 : 0;
+}
+
+static int mga_gpio_getscl(void *data)
+{
+	struct mga_i2c_chan *i2c = data;
+	struct mga_device *mdev = i2c->dev->dev_private;
+	return (mga_i2c_read_gpio(mdev) & i2c->clock) ? 1 : 0;
+}
+
+struct mga_i2c_chan *mgag200_i2c_create(struct drm_device *dev)
+{
+	struct mga_device *mdev = dev->dev_private;
+	struct mga_i2c_chan *i2c;
+	int ret;
+	int data, clock;
+
+	WREG_DAC(MGA1064_GEN_IO_DATA, 0xff);
+	WREG_DAC(MGA1064_GEN_IO_CTL, 0);
+
+	switch (mdev->type) {
+	case G200_SE_A:
+	case G200_SE_B:
+	case G200_EV:
+	case G200_WB:
+		data = 1;
+		clock = 2;
+		break;
+	case G200_EH:
+	case G200_ER:
+		data = 2;
+		clock = 1;
+		break;
+	default:
+		data = 2;
+		clock = 8;
+		break;
+	}
+
+	i2c = kzalloc(sizeof(struct mga_i2c_chan), GFP_KERNEL);
+	if (!i2c)
+		return NULL;
+
+	i2c->data = data;
+	i2c->clock = clock;
+	i2c->adapter.owner = THIS_MODULE;
+	i2c->adapter.class = I2C_CLASS_DDC;
+	i2c->adapter.dev.parent = &dev->pdev->dev;
+	i2c->dev = dev;
+	i2c_set_adapdata(&i2c->adapter, i2c);
+	snprintf(i2c->adapter.name, sizeof(i2c->adapter.name), "mga i2c");
+
+	i2c->adapter.algo_data = &i2c->bit;
+
+	i2c->bit.udelay = 10;
+	i2c->bit.timeout = 2;
+	i2c->bit.data = i2c;
+	i2c->bit.setsda		= mga_gpio_setsda;
+	i2c->bit.setscl		= mga_gpio_setscl;
+	i2c->bit.getsda		= mga_gpio_getsda;
+	i2c->bit.getscl		= mga_gpio_getscl;
+
+	ret = i2c_bit_add_bus(&i2c->adapter);
+	if (ret) {
+		kfree(i2c);
+		i2c = NULL;
+	}
+	return i2c;
+}
+
+void mgag200_i2c_destroy(struct mga_i2c_chan *i2c)
+{
+	if (!i2c)
+		return;
+	i2c_del_adapter(&i2c->adapter);
+	kfree(i2c);
+}
+
diff --git a/drivers/gpu/drm/mgag200/mgag200_main.c b/drivers/gpu/drm/mgag200/mgag200_main.c
new file mode 100644
index 00000000000..636a81cd2f3
--- /dev/null
+++ b/drivers/gpu/drm/mgag200/mgag200_main.c
@@ -0,0 +1,388 @@
+/*
+ * Copyright 2010 Matt Turner.
+ * Copyright 2012 Red Hat
+ *
+ * This file is subject to the terms and conditions of the GNU General
+ * Public License version 2. See the file COPYING in the main
+ * directory of this archive for more details.
+ *
+ * Authors: Matthew Garrett
+ *          Matt Turner
+ *          Dave Airlie
+ */
+#include "drmP.h"
+#include "drm.h"
+#include "drm_crtc_helper.h"
+#include "mgag200_drv.h"
+
+static void mga_user_framebuffer_destroy(struct drm_framebuffer *fb)
+{
+	struct mga_framebuffer *mga_fb = to_mga_framebuffer(fb);
+	if (mga_fb->obj)
+		drm_gem_object_unreference_unlocked(mga_fb->obj);
+	drm_framebuffer_cleanup(fb);
+	kfree(fb);
+}
+
+static int mga_user_framebuffer_create_handle(struct drm_framebuffer *fb,
+						 struct drm_file *file_priv,
+						 unsigned int *handle)
+{
+	return 0;
+}
+
+static const struct drm_framebuffer_funcs mga_fb_funcs = {
+	.destroy = mga_user_framebuffer_destroy,
+	.create_handle = mga_user_framebuffer_create_handle,
+};
+
+int mgag200_framebuffer_init(struct drm_device *dev,
+			     struct mga_framebuffer *gfb,
+			     struct drm_mode_fb_cmd2 *mode_cmd,
+			     struct drm_gem_object *obj)
+{
+	int ret = drm_framebuffer_init(dev, &gfb->base, &mga_fb_funcs);
+	if (ret) {
+		DRM_ERROR("drm_framebuffer_init failed: %d\n", ret);
+		return ret;
+	}
+	drm_helper_mode_fill_fb_struct(&gfb->base, mode_cmd);
+	gfb->obj = obj;
+	return 0;
+}
+
+static struct drm_framebuffer *
+mgag200_user_framebuffer_create(struct drm_device *dev,
+				struct drm_file *filp,
+				struct drm_mode_fb_cmd2 *mode_cmd)
+{
+	struct drm_gem_object *obj;
+	struct mga_framebuffer *mga_fb;
+	int ret;
+
+	obj = drm_gem_object_lookup(dev, filp, mode_cmd->handles[0]);
+	if (obj == NULL)
+		return ERR_PTR(-ENOENT);
+
+	mga_fb = kzalloc(sizeof(*mga_fb), GFP_KERNEL);
+	if (!mga_fb) {
+		drm_gem_object_unreference_unlocked(obj);
+		return ERR_PTR(-ENOMEM);
+	}
+
+	ret = mgag200_framebuffer_init(dev, mga_fb, mode_cmd, obj);
+	if (ret) {
+		drm_gem_object_unreference_unlocked(obj);
+		kfree(mga_fb);
+		return ERR_PTR(ret);
+	}
+	return &mga_fb->base;
+}
+
+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;
+	int orig;
+	int test1, test2;
+	int orig1, orig2;
+
+	/* Probe */
+	orig = ioread16(mem);
+	iowrite16(0, mem);
+
+	for (offset = 0x100000; offset < mdev->mc.vram_window; offset += 0x4000) {
+		orig1 = ioread8(mem + offset);
+		orig2 = ioread8(mem + offset + 0x100);
+
+		iowrite16(0xaa55, mem + offset);
+		iowrite16(0xaa55, mem + offset + 0x100);
+
+		test1 = ioread16(mem + offset);
+		test2 = ioread16(mem);
+
+		iowrite16(orig1, mem + offset);
+		iowrite16(orig2, mem + offset + 0x100);
+
+		if (test1 != 0xaa55) {
+			break;
+		}
+
+		if (test2) {
+			break;
+		}
+	}
+
+	iowrite16(orig, mem);
+	return offset - 65536;
+}
+
+/* Map the framebuffer from the card and configure the core */
+static int mga_vram_init(struct mga_device *mdev)
+{
+	void __iomem *mem;
+	struct apertures_struct *aper = alloc_apertures(1);
+
+	/* BAR 0 is VRAM */
+	mdev->mc.vram_base = pci_resource_start(mdev->dev->pdev, 0);
+	mdev->mc.vram_window = pci_resource_len(mdev->dev->pdev, 0);
+
+	aper->ranges[0].base = mdev->mc.vram_base;
+	aper->ranges[0].size = mdev->mc.vram_window;
+	aper->count = 1;
+
+	remove_conflicting_framebuffers(aper, "mgafb", true);
+
+	if (!request_mem_region(mdev->mc.vram_base, mdev->mc.vram_window,
+				"mgadrmfb_vram")) {
+		DRM_ERROR("can't reserve VRAM\n");
+		return -ENXIO;
+	}
+
+	mem = pci_iomap(mdev->dev->pdev, 0, 0);
+
+	mdev->mc.vram_size = mga_probe_vram(mdev, mem);
+
+	pci_iounmap(mdev->dev->pdev, mem);
+
+	return 0;
+}
+
+static int mgag200_device_init(struct drm_device *dev,
+			       uint32_t flags)
+{
+	struct mga_device *mdev = dev->dev_private;
+	int ret, option;
+
+	mdev->type = flags;
+
+	/* Hardcode the number of CRTCs to 1 */
+	mdev->num_crtc = 1;
+
+	pci_read_config_dword(dev->pdev, PCI_MGA_OPTION, &option);
+	mdev->has_sdram = !(option & (1 << 14));
+
+	/* BAR 0 is the framebuffer, BAR 1 contains registers */
+	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,
+				"mgadrmfb_mmio")) {
+		DRM_ERROR("can't reserve mmio registers\n");
+		return -ENOMEM;
+	}
+
+	mdev->rmmio = pci_iomap(dev->pdev, 1, 0);
+	if (mdev->rmmio == NULL)
+		return -ENOMEM;
+
+	/* stash G200 SE model number for later use */
+	if (IS_G200_SE(mdev))
+		mdev->reg_1e24 = RREG32(0x1e24);
+
+	ret = mga_vram_init(mdev);
+	if (ret) {
+		release_mem_region(mdev->rmmio_base, mdev->rmmio_size);
+		return ret;
+	}
+
+	mdev->bpp_shifts[0] = 0;
+	mdev->bpp_shifts[1] = 1;
+	mdev->bpp_shifts[2] = 0;
+	mdev->bpp_shifts[3] = 2;
+	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
+ */
+
+
+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);
+	if (mdev == NULL)
+		return -ENOMEM;
+	dev->dev_private = (void *)mdev;
+	mdev->dev = dev;
+
+	r = mgag200_device_init(dev, flags);
+	if (r) {
+		dev_err(&dev->pdev->dev, "Fatal error during GPU init: %d\n", r);
+		goto out;
+	}
+	r = mgag200_mm_init(mdev);
+	if (r)
+		goto out;
+
+	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;
+
+	r = mgag200_modeset_init(mdev);
+	if (r)
+		dev_err(&dev->pdev->dev, "Fatal error during modeset init: %d\n", r);
+out:
+	if (r)
+		mgag200_driver_unload(dev);
+	return r;
+}
+
+int mgag200_driver_unload(struct drm_device *dev)
+{
+	struct mga_device *mdev = dev->dev_private;
+
+	if (mdev == NULL)
+		return 0;
+	mgag200_modeset_fini(mdev);
+	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;
+}
+
+int mgag200_gem_create(struct drm_device *dev,
+		   u32 size, bool iskernel,
+		   struct drm_gem_object **obj)
+{
+	struct mgag200_bo *astbo;
+	int ret;
+
+	*obj = NULL;
+
+	size = roundup(size, PAGE_SIZE);
+	if (size == 0)
+		return -EINVAL;
+
+	ret = mgag200_bo_create(dev, size, 0, 0, &astbo);
+	if (ret) {
+		if (ret != -ERESTARTSYS)
+			DRM_ERROR("failed to allocate GEM object\n");
+		return ret;
+	}
+	*obj = &astbo->gem;
+	return 0;
+}
+
+int mgag200_dumb_create(struct drm_file *file,
+		    struct drm_device *dev,
+		    struct drm_mode_create_dumb *args)
+{
+	int ret;
+	struct drm_gem_object *gobj;
+	u32 handle;
+
+	args->pitch = args->width * ((args->bpp + 7) / 8);
+	args->size = args->pitch * args->height;
+
+	ret = mgag200_gem_create(dev, args->size, false,
+			     &gobj);
+	if (ret)
+		return ret;
+
+	ret = drm_gem_handle_create(file, gobj, &handle);
+	drm_gem_object_unreference_unlocked(gobj);
+	if (ret)
+		return ret;
+
+	args->handle = handle;
+	return 0;
+}
+
+int mgag200_dumb_destroy(struct drm_file *file,
+		     struct drm_device *dev,
+		     uint32_t handle)
+{
+	return drm_gem_handle_delete(file, handle);
+}
+
+int mgag200_gem_init_object(struct drm_gem_object *obj)
+{
+	BUG();
+	return 0;
+}
+
+void mgag200_bo_unref(struct mgag200_bo **bo)
+{
+	struct ttm_buffer_object *tbo;
+
+	if ((*bo) == NULL)
+		return;
+
+	tbo = &((*bo)->bo);
+	ttm_bo_unref(&tbo);
+	if (tbo == NULL)
+		*bo = NULL;
+
+}
+
+void mgag200_gem_free_object(struct drm_gem_object *obj)
+{
+	struct mgag200_bo *mgag200_bo = gem_to_mga_bo(obj);
+
+	if (!mgag200_bo)
+		return;
+	mgag200_bo_unref(&mgag200_bo);
+}
+
+
+static inline u64 mgag200_bo_mmap_offset(struct mgag200_bo *bo)
+{
+	return bo->bo.addr_space_offset;
+}
+
+int
+mgag200_dumb_mmap_offset(struct drm_file *file,
+		     struct drm_device *dev,
+		     uint32_t handle,
+		     uint64_t *offset)
+{
+	struct drm_gem_object *obj;
+	int ret;
+	struct mgag200_bo *bo;
+
+	mutex_lock(&dev->struct_mutex);
+	obj = drm_gem_object_lookup(dev, file, handle);
+	if (obj == NULL) {
+		ret = -ENOENT;
+		goto out_unlock;
+	}
+
+	bo = gem_to_mga_bo(obj);
+	*offset = mgag200_bo_mmap_offset(bo);
+
+	drm_gem_object_unreference(obj);
+	ret = 0;
+out_unlock:
+	mutex_unlock(&dev->struct_mutex);
+	return ret;
+
+}
diff --git a/drivers/gpu/drm/mgag200/mgag200_mode.c b/drivers/gpu/drm/mgag200/mgag200_mode.c
new file mode 100644
index 00000000000..d303061b251
--- /dev/null
+++ b/drivers/gpu/drm/mgag200/mgag200_mode.c
@@ -0,0 +1,1533 @@
+/*
+ * Copyright 2010 Matt Turner.
+ * Copyright 2012 Red Hat
+ *
+ * This file is subject to the terms and conditions of the GNU General
+ * Public License version 2. See the file COPYING in the main
+ * directory of this archive for more details.
+ *
+ * Authors: Matthew Garrett
+ *	    Matt Turner
+ *	    Dave Airlie
+ */
+
+#include <linux/delay.h>
+
+#include "drmP.h"
+#include "drm.h"
+#include "drm_crtc_helper.h"
+
+#include "mgag200_drv.h"
+
+#define MGAG200_LUT_SIZE 256
+
+/*
+ * This file contains setup code for the CRTC.
+ */
+
+static void mga_crtc_load_lut(struct drm_crtc *crtc)
+{
+	struct mga_crtc *mga_crtc = to_mga_crtc(crtc);
+	struct drm_device *dev = crtc->dev;
+	struct mga_device *mdev = dev->dev_private;
+	int i;
+
+	if (!crtc->enabled)
+		return;
+
+	WREG8(DAC_INDEX + MGA1064_INDEX, 0);
+
+	for (i = 0; i < MGAG200_LUT_SIZE; i++) {
+		/* VGA registers */
+		WREG8(DAC_INDEX + MGA1064_COL_PAL, mga_crtc->lut_r[i]);
+		WREG8(DAC_INDEX + MGA1064_COL_PAL, mga_crtc->lut_g[i]);
+		WREG8(DAC_INDEX + MGA1064_COL_PAL, mga_crtc->lut_b[i]);
+	}
+}
+
+static inline void mga_wait_vsync(struct mga_device *mdev)
+{
+	unsigned int count = 0;
+	unsigned int status = 0;
+
+	do {
+		status = RREG32(MGAREG_Status);
+		count++;
+	} while ((status & 0x08) && (count < 250000));
+	count = 0;
+	status = 0;
+	do {
+		status = RREG32(MGAREG_Status);
+		count++;
+	} while (!(status & 0x08) && (count < 250000));
+}
+
+static inline void mga_wait_busy(struct mga_device *mdev)
+{
+	unsigned int count = 0;
+	unsigned int status = 0;
+	do {
+		status = RREG8(MGAREG_Status + 2);
+		count++;
+	} while ((status & 0x01) && (count < 500000));
+}
+
+/*
+ * The core passes the desired mode to the CRTC code to see whether any
+ * CRTC-specific modifications need to be made to it. We're in a position
+ * to just pass that straight through, so this does nothing
+ */
+static bool mga_crtc_mode_fixup(struct drm_crtc *crtc,
+				   struct drm_display_mode *mode,
+				   struct drm_display_mode *adjusted_mode)
+{
+	return true;
+}
+
+static int mga_g200se_set_plls(struct mga_device *mdev, long clock)
+{
+	unsigned int vcomax, vcomin, pllreffreq;
+	unsigned int delta, tmpdelta, permitteddelta;
+	unsigned int testp, testm, testn;
+	unsigned int p, m, n;
+	unsigned int computed;
+
+	m = n = p = 0;
+	vcomax = 320000;
+	vcomin = 160000;
+	pllreffreq = 25000;
+
+	delta = 0xffffffff;
+	permitteddelta = clock * 5 / 1000;
+
+	for (testp = 8; testp > 0; testp /= 2) {
+		if (clock * testp > vcomax)
+			continue;
+		if (clock * testp < vcomin)
+			continue;
+
+		for (testn = 17; testn < 256; testn++) {
+			for (testm = 1; testm < 32; testm++) {
+				computed = (pllreffreq * testn) /
+					(testm * testp);
+				if (computed > clock)
+					tmpdelta = computed - clock;
+				else
+					tmpdelta = clock - computed;
+				if (tmpdelta < delta) {
+					delta = tmpdelta;
+					m = testm - 1;
+					n = testn - 1;
+					p = testp - 1;
+				}
+			}
+		}
+	}
+
+	if (delta > permitteddelta) {
+		printk(KERN_WARNING "PLL delta too large\n");
+		return 1;
+	}
+
+	WREG_DAC(MGA1064_PIX_PLLC_M, m);
+	WREG_DAC(MGA1064_PIX_PLLC_N, n);
+	WREG_DAC(MGA1064_PIX_PLLC_P, p);
+	return 0;
+}
+
+static int mga_g200wb_set_plls(struct mga_device *mdev, long clock)
+{
+	unsigned int vcomax, vcomin, pllreffreq;
+	unsigned int delta, tmpdelta, permitteddelta;
+	unsigned int testp, testm, testn;
+	unsigned int p, m, n;
+	unsigned int computed;
+	int i, j, tmpcount, vcount;
+	bool pll_locked = false;
+	u8 tmp;
+
+	m = n = p = 0;
+	vcomax = 550000;
+	vcomin = 150000;
+	pllreffreq = 48000;
+
+	delta = 0xffffffff;
+	permitteddelta = clock * 5 / 1000;
+
+	for (testp = 1; testp < 9; testp++) {
+		if (clock * testp > vcomax)
+			continue;
+		if (clock * testp < vcomin)
+			continue;
+
+		for (testm = 1; testm < 17; testm++) {
+			for (testn = 1; testn < 151; testn++) {
+				computed = (pllreffreq * testn) /
+					(testm * testp);
+				if (computed > clock)
+					tmpdelta = computed - clock;
+				else
+					tmpdelta = clock - computed;
+				if (tmpdelta < delta) {
+					delta = tmpdelta;
+					n = testn - 1;
+					m = (testm - 1) | ((n >> 1) & 0x80);
+					p = testp - 1;
+				}
+			}
+		}
+	}
+
+	for (i = 0; i <= 32 && pll_locked == false; i++) {
+		if (i > 0) {
+			WREG8(MGAREG_CRTC_INDEX, 0x1e);
+			tmp = RREG8(MGAREG_CRTC_DATA);
+			if (tmp < 0xff)
+				WREG8(MGAREG_CRTC_DATA, tmp+1);
+		}
+
+		/* set pixclkdis to 1 */
+		WREG8(DAC_INDEX, MGA1064_PIX_CLK_CTL);
+		tmp = RREG8(DAC_DATA);
+		tmp |= MGA1064_PIX_CLK_CTL_CLK_DIS;
+		WREG_DAC(MGA1064_PIX_CLK_CTL_CLK_DIS, tmp);
+
+		WREG8(DAC_INDEX, MGA1064_REMHEADCTL);
+		tmp = RREG8(DAC_DATA);
+		tmp |= MGA1064_REMHEADCTL_CLKDIS;
+		WREG_DAC(MGA1064_REMHEADCTL, tmp);
+
+		/* select PLL Set C */
+		tmp = RREG8(MGAREG_MEM_MISC_READ);
+		tmp |= 0x3 << 2;
+		WREG8(MGAREG_MEM_MISC_WRITE, tmp);
+
+		WREG8(DAC_INDEX, MGA1064_PIX_CLK_CTL);
+		tmp = RREG8(DAC_DATA);
+		tmp |= MGA1064_PIX_CLK_CTL_CLK_POW_DOWN | 0x80;
+		WREG_DAC(MGA1064_PIX_CLK_CTL, tmp);
+
+		udelay(500);
+
+		/* reset the PLL */
+		WREG8(DAC_INDEX, MGA1064_VREF_CTL);
+		tmp = RREG8(DAC_DATA);
+		tmp &= ~0x04;
+		WREG_DAC(MGA1064_VREF_CTL, tmp);
+
+		udelay(50);
+
+		/* program pixel pll register */
+		WREG_DAC(MGA1064_WB_PIX_PLLC_N, n);
+		WREG_DAC(MGA1064_WB_PIX_PLLC_M, m);
+		WREG_DAC(MGA1064_WB_PIX_PLLC_P, p);
+
+		udelay(50);
+
+		/* turn pll on */
+		WREG8(DAC_INDEX, MGA1064_VREF_CTL);
+		tmp = RREG8(DAC_DATA);
+		tmp |= 0x04;
+		WREG_DAC(MGA1064_VREF_CTL, tmp);
+
+		udelay(500);
+
+		/* select the pixel pll */
+		WREG8(DAC_INDEX, MGA1064_PIX_CLK_CTL);
+		tmp = RREG8(DAC_DATA);
+		tmp &= ~MGA1064_PIX_CLK_CTL_SEL_MSK;
+		tmp |= MGA1064_PIX_CLK_CTL_SEL_PLL;
+		WREG_DAC(MGA1064_PIX_CLK_CTL, tmp);
+
+		WREG8(DAC_INDEX, MGA1064_REMHEADCTL);
+		tmp = RREG8(DAC_DATA);
+		tmp &= ~MGA1064_REMHEADCTL_CLKSL_MSK;
+		tmp |= MGA1064_REMHEADCTL_CLKSL_PLL;
+		WREG_DAC(MGA1064_REMHEADCTL, tmp);
+
+		/* reset dotclock rate bit */
+		WREG8(MGAREG_SEQ_INDEX, 1);
+		tmp = RREG8(MGAREG_SEQ_DATA);
+		tmp &= ~0x8;
+		WREG8(MGAREG_SEQ_DATA, tmp);
+
+		WREG8(DAC_INDEX, MGA1064_PIX_CLK_CTL);
+		tmp = RREG8(DAC_DATA);
+		tmp &= ~MGA1064_PIX_CLK_CTL_CLK_DIS;
+		WREG_DAC(MGA1064_PIX_CLK_CTL, tmp);
+
+		vcount = RREG8(MGAREG_VCOUNT);
+
+		for (j = 0; j < 30 && pll_locked == false; j++) {
+			tmpcount = RREG8(MGAREG_VCOUNT);
+			if (tmpcount < vcount)
+				vcount = 0;
+			if ((tmpcount - vcount) > 2)
+				pll_locked = true;
+			else
+				udelay(5);
+		}
+	}
+	WREG8(DAC_INDEX, MGA1064_REMHEADCTL);
+	tmp = RREG8(DAC_DATA);
+	tmp &= ~MGA1064_REMHEADCTL_CLKDIS;
+	WREG_DAC(MGA1064_REMHEADCTL, tmp);
+	return 0;
+}
+
+static int mga_g200ev_set_plls(struct mga_device *mdev, long clock)
+{
+	unsigned int vcomax, vcomin, pllreffreq;
+	unsigned int delta, tmpdelta, permitteddelta;
+	unsigned int testp, testm, testn;
+	unsigned int p, m, n;
+	unsigned int computed;
+	u8 tmp;
+
+	m = n = p = 0;
+	vcomax = 550000;
+	vcomin = 150000;
+	pllreffreq = 50000;
+
+	delta = 0xffffffff;
+	permitteddelta = clock * 5 / 1000;
+
+	for (testp = 16; testp > 0; testp--) {
+		if (clock * testp > vcomax)
+			continue;
+		if (clock * testp < vcomin)
+			continue;
+
+		for (testn = 1; testn < 257; testn++) {
+			for (testm = 1; testm < 17; testm++) {
+				computed = (pllreffreq * testn) /
+					(testm * testp);
+				if (computed > clock)
+					tmpdelta = computed - clock;
+				else
+					tmpdelta = clock - computed;
+				if (tmpdelta < delta) {
+					delta = tmpdelta;
+					n = testn - 1;
+					m = testm - 1;
+					p = testp - 1;
+				}
+			}
+		}
+	}
+
+	WREG8(DAC_INDEX, MGA1064_PIX_CLK_CTL);
+	tmp = RREG8(DAC_DATA);
+	tmp |= MGA1064_PIX_CLK_CTL_CLK_DIS;
+	WREG_DAC(MGA1064_PIX_CLK_CTL_CLK_DIS, tmp);
+
+	tmp = RREG8(MGAREG_MEM_MISC_READ);
+	tmp |= 0x3 << 2;
+	WREG8(MGAREG_MEM_MISC_WRITE, tmp);
+
+	WREG8(DAC_INDEX, MGA1064_PIX_PLL_STAT);
+	tmp = RREG8(DAC_DATA);
+	WREG_DAC(MGA1064_PIX_PLL_STAT, tmp & ~0x40);
+
+	WREG8(DAC_INDEX, MGA1064_PIX_CLK_CTL);
+	tmp = RREG8(DAC_DATA);
+	tmp |= MGA1064_PIX_CLK_CTL_CLK_POW_DOWN;
+	WREG_DAC(MGA1064_PIX_CLK_CTL, tmp);
+
+	WREG_DAC(MGA1064_EV_PIX_PLLC_M, m);
+	WREG_DAC(MGA1064_EV_PIX_PLLC_N, n);
+	WREG_DAC(MGA1064_EV_PIX_PLLC_P, p);
+
+	udelay(50);
+
+	WREG8(DAC_INDEX, MGA1064_PIX_CLK_CTL);
+	tmp = RREG8(DAC_DATA);
+	tmp &= ~MGA1064_PIX_CLK_CTL_CLK_POW_DOWN;
+	WREG_DAC(MGA1064_PIX_CLK_CTL, tmp);
+
+	udelay(500);
+
+	WREG8(DAC_INDEX, MGA1064_PIX_CLK_CTL);
+	tmp = RREG8(DAC_DATA);
+	tmp &= ~MGA1064_PIX_CLK_CTL_SEL_MSK;
+	tmp |= MGA1064_PIX_CLK_CTL_SEL_PLL;
+	WREG_DAC(MGA1064_PIX_CLK_CTL, tmp);
+
+	WREG8(DAC_INDEX, MGA1064_PIX_PLL_STAT);
+	tmp = RREG8(DAC_DATA);
+	WREG_DAC(MGA1064_PIX_PLL_STAT, tmp | 0x40);
+
+	tmp = RREG8(MGAREG_MEM_MISC_READ);
+	tmp |= (0x3 << 2);
+	WREG8(MGAREG_MEM_MISC_WRITE, tmp);
+
+	WREG8(DAC_INDEX, MGA1064_PIX_CLK_CTL);
+	tmp = RREG8(DAC_DATA);
+	tmp &= ~MGA1064_PIX_CLK_CTL_CLK_DIS;
+	WREG_DAC(MGA1064_PIX_CLK_CTL, tmp);
+
+	return 0;
+}
+
+static int mga_g200eh_set_plls(struct mga_device *mdev, long clock)
+{
+	unsigned int vcomax, vcomin, pllreffreq;
+	unsigned int delta, tmpdelta, permitteddelta;
+	unsigned int testp, testm, testn;
+	unsigned int p, m, n;
+	unsigned int computed;
+	int i, j, tmpcount, vcount;
+	u8 tmp;
+	bool pll_locked = false;
+
+	m = n = p = 0;
+	vcomax = 800000;
+	vcomin = 400000;
+	pllreffreq = 3333;
+
+	delta = 0xffffffff;
+	permitteddelta = clock * 5 / 1000;
+
+	for (testp = 16; testp > 0; testp--) {
+		if (clock * testp > vcomax)
+			continue;
+		if (clock * testp < vcomin)
+			continue;
+
+		for (testm = 1; testm < 33; testm++) {
+			for (testn = 1; testn < 257; testn++) {
+				computed = (pllreffreq * testn) /
+					(testm * testp);
+				if (computed > clock)
+					tmpdelta = computed - clock;
+				else
+					tmpdelta = clock - computed;
+				if (tmpdelta < delta) {
+					delta = tmpdelta;
+					n = testn - 1;
+					m = (testm - 1) | ((n >> 1) & 0x80);
+					p = testp - 1;
+				}
+				if ((clock * testp) >= 600000)
+					p |= 80;
+			}
+		}
+	}
+	for (i = 0; i <= 32 && pll_locked == false; i++) {
+		WREG8(DAC_INDEX, MGA1064_PIX_CLK_CTL);
+		tmp = RREG8(DAC_DATA);
+		tmp |= MGA1064_PIX_CLK_CTL_CLK_DIS;
+		WREG_DAC(MGA1064_PIX_CLK_CTL_CLK_DIS, tmp);
+
+		tmp = RREG8(MGAREG_MEM_MISC_READ);
+		tmp |= 0x3 << 2;
+		WREG8(MGAREG_MEM_MISC_WRITE, tmp);
+
+		WREG8(DAC_INDEX, MGA1064_PIX_CLK_CTL);
+		tmp = RREG8(DAC_DATA);
+		tmp |= MGA1064_PIX_CLK_CTL_CLK_POW_DOWN;
+		WREG_DAC(MGA1064_PIX_CLK_CTL, tmp);
+
+		udelay(500);
+
+		WREG_DAC(MGA1064_EH_PIX_PLLC_M, m);
+		WREG_DAC(MGA1064_EH_PIX_PLLC_N, n);
+		WREG_DAC(MGA1064_EH_PIX_PLLC_P, p);
+
+		udelay(500);
+
+		WREG8(DAC_INDEX, MGA1064_PIX_CLK_CTL);
+		tmp = RREG8(DAC_DATA);
+		tmp &= ~MGA1064_PIX_CLK_CTL_SEL_MSK;
+		tmp |= MGA1064_PIX_CLK_CTL_SEL_PLL;
+		WREG_DAC(MGA1064_PIX_CLK_CTL, tmp);
+
+		WREG8(DAC_INDEX, MGA1064_PIX_CLK_CTL);
+		tmp = RREG8(DAC_DATA);
+		tmp &= ~MGA1064_PIX_CLK_CTL_CLK_DIS;
+		tmp &= ~MGA1064_PIX_CLK_CTL_CLK_POW_DOWN;
+		WREG_DAC(MGA1064_PIX_CLK_CTL, tmp);
+
+		vcount = RREG8(MGAREG_VCOUNT);
+
+		for (j = 0; j < 30 && pll_locked == false; j++) {
+			tmpcount = RREG8(MGAREG_VCOUNT);
+			if (tmpcount < vcount)
+				vcount = 0;
+			if ((tmpcount - vcount) > 2)
+				pll_locked = true;
+			else
+				udelay(5);
+		}
+	}
+
+	return 0;
+}
+
+static int mga_g200er_set_plls(struct mga_device *mdev, long clock)
+{
+	unsigned int vcomax, vcomin, pllreffreq;
+	unsigned int delta, tmpdelta;
+	unsigned int testr, testn, testm, testo;
+	unsigned int p, m, n;
+	unsigned int computed;
+	int tmp;
+
+	m = n = p = 0;
+	vcomax = 1488000;
+	vcomin = 1056000;
+	pllreffreq = 48000;
+
+	delta = 0xffffffff;
+
+	for (testr = 0; testr < 4; testr++) {
+		if (delta == 0)
+			break;
+		for (testn = 5; testn < 129; testn++) {
+			if (delta == 0)
+				break;
+			for (testm = 3; testm >= 0; testm--) {
+				if (delta == 0)
+					break;
+				for (testo = 5; testo < 33; testo++) {
+					computed = pllreffreq * (testn + 1) /
+						(testr + 1);
+					if (computed < vcomin)
+						continue;
+					if (computed > vcomax)
+						continue;
+					if (computed > clock)
+						tmpdelta = computed - clock;
+					else
+						tmpdelta = clock - computed;
+					if (tmpdelta < delta) {
+						delta = tmpdelta;
+						m = testm | (testo << 3);
+						n = testn;
+						p = testr | (testr << 3);
+					}
+				}
+			}
+		}
+	}
+
+	WREG8(DAC_INDEX, MGA1064_PIX_CLK_CTL);
+	tmp = RREG8(DAC_DATA);
+	tmp |= MGA1064_PIX_CLK_CTL_CLK_DIS;
+	WREG_DAC(MGA1064_PIX_CLK_CTL_CLK_DIS, tmp);
+
+	WREG8(DAC_INDEX, MGA1064_REMHEADCTL);
+	tmp = RREG8(DAC_DATA);
+	tmp |= MGA1064_REMHEADCTL_CLKDIS;
+	WREG_DAC(MGA1064_REMHEADCTL, tmp);
+
+	tmp = RREG8(MGAREG_MEM_MISC_READ);
+	tmp |= (0x3<<2) | 0xc0;
+	WREG8(MGAREG_MEM_MISC_WRITE, tmp);
+
+	WREG8(DAC_INDEX, MGA1064_PIX_CLK_CTL);
+	tmp = RREG8(DAC_DATA);
+	tmp &= ~MGA1064_PIX_CLK_CTL_CLK_DIS;
+	tmp |= MGA1064_PIX_CLK_CTL_CLK_POW_DOWN;
+	WREG_DAC(MGA1064_PIX_CLK_CTL, tmp);
+
+	udelay(500);
+
+	WREG_DAC(MGA1064_ER_PIX_PLLC_N, n);
+	WREG_DAC(MGA1064_ER_PIX_PLLC_M, m);
+	WREG_DAC(MGA1064_ER_PIX_PLLC_P, p);
+
+	udelay(50);
+
+	return 0;
+}
+
+static int mga_crtc_set_plls(struct mga_device *mdev, long clock)
+{
+	switch(mdev->type) {
+	case G200_SE_A:
+	case G200_SE_B:
+		return mga_g200se_set_plls(mdev, clock);
+		break;
+	case G200_WB:
+		return mga_g200wb_set_plls(mdev, clock);
+		break;
+	case G200_EV:
+		return mga_g200ev_set_plls(mdev, clock);
+		break;
+	case G200_EH:
+		return mga_g200eh_set_plls(mdev, clock);
+		break;
+	case G200_ER:
+		return mga_g200er_set_plls(mdev, clock);
+		break;
+	}
+	return 0;
+}
+
+static void mga_g200wb_prepare(struct drm_crtc *crtc)
+{
+	struct mga_device *mdev = crtc->dev->dev_private;
+	u8 tmp;
+	int iter_max;
+
+	/* 1- The first step is to warn the BMC of an upcoming mode change.
+	 * We are putting the misc<0> to output.*/
+
+	WREG8(DAC_INDEX, MGA1064_GEN_IO_CTL);
+	tmp = RREG8(DAC_DATA);
+	tmp |= 0x10;
+	WREG_DAC(MGA1064_GEN_IO_CTL, tmp);
+
+	/* we are putting a 1 on the misc<0> line */
+	WREG8(DAC_INDEX, MGA1064_GEN_IO_DATA);
+	tmp = RREG8(DAC_DATA);
+	tmp |= 0x10;
+	WREG_DAC(MGA1064_GEN_IO_DATA, tmp);
+
+	/* 2- Second step to mask and further scan request
+	 * This will be done by asserting the remfreqmsk bit (XSPAREREG<7>)
+	 */
+	WREG8(DAC_INDEX, MGA1064_SPAREREG);
+	tmp = RREG8(DAC_DATA);
+	tmp |= 0x80;
+	WREG_DAC(MGA1064_SPAREREG, tmp);
+
+	/* 3a- the third step is to verifu if there is an active scan
+	 * We are searching for a 0 on remhsyncsts <XSPAREREG<0>)
+	 */
+	iter_max = 300;
+	while (!(tmp & 0x1) && iter_max) {
+		WREG8(DAC_INDEX, MGA1064_SPAREREG);
+		tmp = RREG8(DAC_DATA);
+		udelay(1000);
+		iter_max--;
+	}
+
+	/* 3b- this step occurs only if the remove is actually scanning
+	 * we are waiting for the end of the frame which is a 1 on
+	 * remvsyncsts (XSPAREREG<1>)
+	 */
+	if (iter_max) {
+		iter_max = 300;
+		while ((tmp & 0x2) && iter_max) {
+			WREG8(DAC_INDEX, MGA1064_SPAREREG);
+			tmp = RREG8(DAC_DATA);
+			udelay(1000);
+			iter_max--;
+		}
+	}
+}
+
+static void mga_g200wb_commit(struct drm_crtc *crtc)
+{
+	u8 tmp;
+	struct mga_device *mdev = crtc->dev->dev_private;
+
+	/* 1- The first step is to ensure that the vrsten and hrsten are set */
+	WREG8(MGAREG_CRTCEXT_INDEX, 1);
+	tmp = RREG8(MGAREG_CRTCEXT_DATA);
+	WREG8(MGAREG_CRTCEXT_DATA, tmp | 0x88);
+
+	/* 2- second step is to assert the rstlvl2 */
+	WREG8(DAC_INDEX, MGA1064_REMHEADCTL2);
+	tmp = RREG8(DAC_DATA);
+	tmp |= 0x8;
+	WREG8(DAC_DATA, tmp);
+
+	/* wait 10 us */
+	udelay(10);
+
+	/* 3- deassert rstlvl2 */
+	tmp &= ~0x08;
+	WREG8(DAC_INDEX, MGA1064_REMHEADCTL2);
+	WREG8(DAC_DATA, tmp);
+
+	/* 4- remove mask of scan request */
+	WREG8(DAC_INDEX, MGA1064_SPAREREG);
+	tmp = RREG8(DAC_DATA);
+	tmp &= ~0x80;
+	WREG8(DAC_DATA, tmp);
+
+	/* 5- put back a 0 on the misc<0> line */
+	WREG8(DAC_INDEX, MGA1064_GEN_IO_DATA);
+	tmp = RREG8(DAC_DATA);
+	tmp &= ~0x10;
+	WREG_DAC(MGA1064_GEN_IO_DATA, tmp);
+}
+
+
+void mga_set_start_address(struct drm_crtc *crtc, unsigned offset)
+{
+	struct mga_device *mdev = crtc->dev->dev_private;
+	u32 addr;
+	int count;
+
+	while (RREG8(0x1fda) & 0x08);
+	while (!(RREG8(0x1fda) & 0x08));
+
+	count = RREG8(MGAREG_VCOUNT) + 2;
+	while (RREG8(MGAREG_VCOUNT) < count);
+
+	addr = offset >> 2;
+	WREG_CRT(0x0d, (u8)(addr & 0xff));
+	WREG_CRT(0x0c, (u8)(addr >> 8) & 0xff);
+	WREG_CRT(0xaf, (u8)(addr >> 16) & 0xf);
+}
+
+
+/* ast is different - we will force move buffers out of VRAM */
+static int mga_crtc_do_set_base(struct drm_crtc *crtc,
+				struct drm_framebuffer *fb,
+				int x, int y, int atomic)
+{
+	struct mga_device *mdev = crtc->dev->dev_private;
+	struct drm_gem_object *obj;
+	struct mga_framebuffer *mga_fb;
+	struct mgag200_bo *bo;
+	int ret;
+	u64 gpu_addr;
+
+	/* push the previous fb to system ram */
+	if (!atomic && fb) {
+		mga_fb = to_mga_framebuffer(fb);
+		obj = mga_fb->obj;
+		bo = gem_to_mga_bo(obj);
+		ret = mgag200_bo_reserve(bo, false);
+		if (ret)
+			return ret;
+		mgag200_bo_push_sysram(bo);
+		mgag200_bo_unreserve(bo);
+	}
+
+	mga_fb = to_mga_framebuffer(crtc->fb);
+	obj = mga_fb->obj;
+	bo = gem_to_mga_bo(obj);
+
+	ret = mgag200_bo_reserve(bo, false);
+	if (ret)
+		return ret;
+
+	ret = mgag200_bo_pin(bo, TTM_PL_FLAG_VRAM, &gpu_addr);
+	if (ret) {
+		mgag200_bo_unreserve(bo);
+		return ret;
+	}
+
+	if (&mdev->mfbdev->mfb == mga_fb) {
+		/* if pushing console in kmap it */
+		ret = ttm_bo_kmap(&bo->bo, 0, bo->bo.num_pages, &bo->kmap);
+		if (ret)
+			DRM_ERROR("failed to kmap fbcon\n");
+
+	}
+	mgag200_bo_unreserve(bo);
+
+	DRM_INFO("mga base %llx\n", gpu_addr);
+
+	mga_set_start_address(crtc, (u32)gpu_addr);
+
+	return 0;
+}
+
+static int mga_crtc_mode_set_base(struct drm_crtc *crtc, int x, int y,
+				  struct drm_framebuffer *old_fb)
+{
+	return mga_crtc_do_set_base(crtc, old_fb, x, y, 0);
+}
+
+static int mga_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 mga_device *mdev = dev->dev_private;
+	int hdisplay, hsyncstart, hsyncend, htotal;
+	int vdisplay, vsyncstart, vsyncend, vtotal;
+	int pitch;
+	int option = 0, option2 = 0;
+	int i;
+	unsigned char misc = 0;
+	unsigned char ext_vga[6];
+	unsigned char ext_vga_index24;
+	unsigned char dac_index90 = 0;
+	u8 bppshift;
+
+	static unsigned char dacvalue[] = {
+		/* 0x00: */        0,    0,    0,    0,    0,    0, 0x00,    0,
+		/* 0x08: */        0,    0,    0,    0,    0,    0,    0,    0,
+		/* 0x10: */        0,    0,    0,    0,    0,    0,    0,    0,
+		/* 0x18: */     0x00,    0, 0xC9, 0xFF, 0xBF, 0x20, 0x1F, 0x20,
+		/* 0x20: */     0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+		/* 0x28: */     0x00, 0x00, 0x00, 0x00,    0,    0,    0, 0x40,
+		/* 0x30: */     0x00, 0xB0, 0x00, 0xC2, 0x34, 0x14, 0x02, 0x83,
+		/* 0x38: */     0x00, 0x93, 0x00, 0x77, 0x00, 0x00, 0x00, 0x3A,
+		/* 0x40: */        0,    0,    0,    0,    0,    0,    0,    0,
+		/* 0x48: */        0,    0,    0,    0,    0,    0,    0,    0
+	};
+
+	bppshift = mdev->bpp_shifts[(crtc->fb->bits_per_pixel >> 3) - 1];
+
+	switch (mdev->type) {
+	case G200_SE_A:
+	case G200_SE_B:
+		dacvalue[MGA1064_VREF_CTL] = 0x03;
+		dacvalue[MGA1064_PIX_CLK_CTL] = MGA1064_PIX_CLK_CTL_SEL_PLL;
+		dacvalue[MGA1064_MISC_CTL] = MGA1064_MISC_CTL_DAC_EN |
+					     MGA1064_MISC_CTL_VGA8 |
+					     MGA1064_MISC_CTL_DAC_RAM_CS;
+		if (mdev->has_sdram)
+			option = 0x40049120;
+		else
+			option = 0x4004d120;
+		option2 = 0x00008000;
+		break;
+	case G200_WB:
+		dacvalue[MGA1064_VREF_CTL] = 0x07;
+		option = 0x41049120;
+		option2 = 0x0000b000;
+		break;
+	case G200_EV:
+		dacvalue[MGA1064_PIX_CLK_CTL] = MGA1064_PIX_CLK_CTL_SEL_PLL;
+		dacvalue[MGA1064_MISC_CTL] = MGA1064_MISC_CTL_VGA8 |
+					     MGA1064_MISC_CTL_DAC_RAM_CS;
+		option = 0x00000120;
+		option2 = 0x0000b000;
+		break;
+	case G200_EH:
+		dacvalue[MGA1064_MISC_CTL] = MGA1064_MISC_CTL_VGA8 |
+					     MGA1064_MISC_CTL_DAC_RAM_CS;
+		option = 0x00000120;
+		option2 = 0x0000b000;
+		break;
+	case G200_ER:
+		dac_index90 = 0;
+		break;
+	}
+
+	switch (crtc->fb->bits_per_pixel) {
+	case 8:
+		dacvalue[MGA1064_MUL_CTL] = MGA1064_MUL_CTL_8bits;
+		break;
+	case 16:
+		if (crtc->fb->depth == 15)
+			dacvalue[MGA1064_MUL_CTL] = MGA1064_MUL_CTL_15bits;
+		else
+			dacvalue[MGA1064_MUL_CTL] = MGA1064_MUL_CTL_16bits;
+		break;
+	case 24:
+		dacvalue[MGA1064_MUL_CTL] = MGA1064_MUL_CTL_24bits;
+		break;
+	case 32:
+		dacvalue[MGA1064_MUL_CTL] = MGA1064_MUL_CTL_32_24bits;
+		break;
+	}
+
+	if (mode->flags & DRM_MODE_FLAG_NHSYNC)
+		misc |= 0x40;
+	if (mode->flags & DRM_MODE_FLAG_NVSYNC)
+		misc |= 0x80;
+
+
+	for (i = 0; i < sizeof(dacvalue); i++) {
+		if ((i <= 0x03) ||
+		    (i == 0x07) ||
+		    (i == 0x0b) ||
+		    (i == 0x0f) ||
+		    ((i >= 0x13) && (i <= 0x17)) ||
+		    (i == 0x1b) ||
+		    (i == 0x1c) ||
+		    ((i >= 0x1f) && (i <= 0x29)) ||
+		    ((i >= 0x30) && (i <= 0x37)))
+			continue;
+		if (IS_G200_SE(mdev) &&
+		    ((i == 0x2c) || (i == 0x2d) || (i == 0x2e)))
+			continue;
+		if ((mdev->type == G200_EV || mdev->type == G200_WB || mdev->type == G200_EH) &&
+		    (i >= 0x44) && (i <= 0x4e))
+			continue;
+
+		WREG_DAC(i, dacvalue[i]);
+	}
+
+	if (mdev->type == G200_ER) {
+		WREG_DAC(0x90, dac_index90);
+	}
+
+
+	if (option)
+		pci_write_config_dword(dev->pdev, PCI_MGA_OPTION, option);
+	if (option2)
+		pci_write_config_dword(dev->pdev, PCI_MGA_OPTION2, option2);
+
+	WREG_SEQ(2, 0xf);
+	WREG_SEQ(3, 0);
+	WREG_SEQ(4, 0xe);
+
+	pitch = crtc->fb->pitches[0] / (crtc->fb->bits_per_pixel / 8);
+	if (crtc->fb->bits_per_pixel == 24)
+		pitch = pitch >> (4 - bppshift);
+	else
+		pitch = pitch >> (4 - bppshift);
+
+	hdisplay = mode->hdisplay / 8 - 1;
+	hsyncstart = mode->hsync_start / 8 - 1;
+	hsyncend = mode->hsync_end / 8 - 1;
+	htotal = mode->htotal / 8 - 1;
+
+	/* Work around hardware quirk */
+	if ((htotal & 0x07) == 0x06 || (htotal & 0x07) == 0x04)
+		htotal++;
+
+	vdisplay = mode->vdisplay - 1;
+	vsyncstart = mode->vsync_start - 1;
+	vsyncend = mode->vsync_end - 1;
+	vtotal = mode->vtotal - 2;
+
+	WREG_GFX(0, 0);
+	WREG_GFX(1, 0);
+	WREG_GFX(2, 0);
+	WREG_GFX(3, 0);
+	WREG_GFX(4, 0);
+	WREG_GFX(5, 0x40);
+	WREG_GFX(6, 0x5);
+	WREG_GFX(7, 0xf);
+	WREG_GFX(8, 0xf);
+
+	WREG_CRT(0, htotal - 4);
+	WREG_CRT(1, hdisplay);
+	WREG_CRT(2, hdisplay);
+	WREG_CRT(3, (htotal & 0x1F) | 0x80);
+	WREG_CRT(4, hsyncstart);
+	WREG_CRT(5, ((htotal & 0x20) << 2) | (hsyncend & 0x1F));
+	WREG_CRT(6, vtotal & 0xFF);
+	WREG_CRT(7, ((vtotal & 0x100) >> 8) |
+		 ((vdisplay & 0x100) >> 7) |
+		 ((vsyncstart & 0x100) >> 6) |
+		 ((vdisplay & 0x100) >> 5) |
+		 ((vdisplay & 0x100) >> 4) | /* linecomp */
+		 ((vtotal & 0x200) >> 4)|
+		 ((vdisplay & 0x200) >> 3) |
+		 ((vsyncstart & 0x200) >> 2));
+	WREG_CRT(9, ((vdisplay & 0x200) >> 4) |
+		 ((vdisplay & 0x200) >> 3));
+	WREG_CRT(10, 0);
+	WREG_CRT(11, 0);
+	WREG_CRT(12, 0);
+	WREG_CRT(13, 0);
+	WREG_CRT(14, 0);
+	WREG_CRT(15, 0);
+	WREG_CRT(16, vsyncstart & 0xFF);
+	WREG_CRT(17, (vsyncend & 0x0F) | 0x20);
+	WREG_CRT(18, vdisplay & 0xFF);
+	WREG_CRT(19, pitch & 0xFF);
+	WREG_CRT(20, 0);
+	WREG_CRT(21, vdisplay & 0xFF);
+	WREG_CRT(22, (vtotal + 1) & 0xFF);
+	WREG_CRT(23, 0xc3);
+	WREG_CRT(24, vdisplay & 0xFF);
+
+	ext_vga[0] = 0;
+	ext_vga[5] = 0;
+
+	/* TODO interlace */
+
+	ext_vga[0] |= (pitch & 0x300) >> 4;
+	ext_vga[1] = (((htotal - 4) & 0x100) >> 8) |
+		((hdisplay & 0x100) >> 7) |
+		((hsyncstart & 0x100) >> 6) |
+		(htotal & 0x40);
+	ext_vga[2] = ((vtotal & 0xc00) >> 10) |
+		((vdisplay & 0x400) >> 8) |
+		((vdisplay & 0xc00) >> 7) |
+		((vsyncstart & 0xc00) >> 5) |
+		((vdisplay & 0x400) >> 3);
+	if (crtc->fb->bits_per_pixel == 24)
+		ext_vga[3] = (((1 << bppshift) * 3) - 1) | 0x80;
+	else
+		ext_vga[3] = ((1 << bppshift) - 1) | 0x80;
+	ext_vga[4] = 0;
+	if (mdev->type == G200_WB)
+		ext_vga[1] |= 0x88;
+
+	ext_vga_index24 = 0x05;
+
+	/* Set pixel clocks */
+	misc = 0x2d;
+	WREG8(MGA_MISC_OUT, misc);
+
+	mga_crtc_set_plls(mdev, mode->clock);
+
+	for (i = 0; i < 6; i++) {
+		WREG_ECRT(i, ext_vga[i]);
+	}
+
+	if (mdev->type == G200_ER)
+		WREG_ECRT(24, ext_vga_index24);
+
+	if (mdev->type == G200_EV) {
+		WREG_ECRT(6, 0);
+	}
+
+	WREG_ECRT(0, ext_vga[0]);
+	/* Enable mga pixel clock */
+	misc = 0x2d;
+
+	WREG8(MGA_MISC_OUT, misc);
+
+	if (adjusted_mode)
+		memcpy(&mdev->mode, mode, sizeof(struct drm_display_mode));
+
+	mga_crtc_do_set_base(crtc, old_fb, x, y, 0);
+
+	/* reset tagfifo */
+	if (mdev->type == G200_ER) {
+		u32 mem_ctl = RREG32(MGAREG_MEMCTL);
+		u8 seq1;
+
+		/* screen off */
+		WREG8(MGAREG_SEQ_INDEX, 0x01);
+		seq1 = RREG8(MGAREG_SEQ_DATA) | 0x20;
+		WREG8(MGAREG_SEQ_DATA, seq1);
+
+		WREG32(MGAREG_MEMCTL, mem_ctl | 0x00200000);
+		udelay(1000);
+		WREG32(MGAREG_MEMCTL, mem_ctl & ~0x00200000);
+
+		WREG8(MGAREG_SEQ_DATA, seq1 & ~0x20);
+	}
+
+
+	if (IS_G200_SE(mdev)) {
+		if (mdev->reg_1e24 >= 0x02) {
+			u8 hi_pri_lvl;
+			u32 bpp;
+			u32 mb;
+
+			if (crtc->fb->bits_per_pixel > 16)
+				bpp = 32;
+			else if (crtc->fb->bits_per_pixel > 8)
+				bpp = 16;
+			else
+				bpp = 8;
+
+			mb = (mode->clock * bpp) / 1000;
+			if (mb > 3100)
+				hi_pri_lvl = 0;
+			else if (mb > 2600)
+				hi_pri_lvl = 1;
+			else if (mb > 1900)
+				hi_pri_lvl = 2;
+			else if (mb > 1160)
+				hi_pri_lvl = 3;
+			else if (mb > 440)
+				hi_pri_lvl = 4;
+			else
+				hi_pri_lvl = 5;
+
+			WREG8(0x1fde, 0x06);
+			WREG8(0x1fdf, hi_pri_lvl);
+		} else {
+			if (mdev->reg_1e24 >= 0x01)
+				WREG8(0x1fdf, 0x03);
+			else
+				WREG8(0x1fdf, 0x04);
+		}
+	}
+	return 0;
+}
+
+#if 0 /* code from mjg to attempt D3 on crtc dpms off - revisit later */
+static int mga_suspend(struct drm_crtc *crtc)
+{
+	struct mga_crtc *mga_crtc = to_mga_crtc(crtc);
+	struct drm_device *dev = crtc->dev;
+	struct mga_device *mdev = dev->dev_private;
+	struct pci_dev *pdev = dev->pdev;
+	int option;
+
+	if (mdev->suspended)
+		return 0;
+
+	WREG_SEQ(1, 0x20);
+	WREG_ECRT(1, 0x30);
+	/* Disable the pixel clock */
+	WREG_DAC(0x1a, 0x05);
+	/* Power down the DAC */
+	WREG_DAC(0x1e, 0x18);
+	/* Power down the pixel PLL */
+	WREG_DAC(0x1a, 0x0d);
+
+	/* Disable PLLs and clocks */
+	pci_read_config_dword(pdev, PCI_MGA_OPTION, &option);
+	option &= ~(0x1F8024);
+	pci_write_config_dword(pdev, PCI_MGA_OPTION, option);
+	pci_set_power_state(pdev, PCI_D3hot);
+	pci_disable_device(pdev);
+
+	mdev->suspended = true;
+
+	return 0;
+}
+
+static int mga_resume(struct drm_crtc *crtc)
+{
+	struct mga_crtc *mga_crtc = to_mga_crtc(crtc);
+	struct drm_device *dev = crtc->dev;
+	struct mga_device *mdev = dev->dev_private;
+	struct pci_dev *pdev = dev->pdev;
+	int option;
+
+	if (!mdev->suspended)
+		return 0;
+
+	pci_set_power_state(pdev, PCI_D0);
+	pci_enable_device(pdev);
+
+	/* Disable sysclk */
+	pci_read_config_dword(pdev, PCI_MGA_OPTION, &option);
+	option &= ~(0x4);
+	pci_write_config_dword(pdev, PCI_MGA_OPTION, option);
+
+	mdev->suspended = false;
+
+	return 0;
+}
+
+#endif
+
+static void mga_crtc_dpms(struct drm_crtc *crtc, int mode)
+{
+	struct drm_device *dev = crtc->dev;
+	struct mga_device *mdev = dev->dev_private;
+	u8 seq1 = 0, crtcext1 = 0;
+
+	switch (mode) {
+	case DRM_MODE_DPMS_ON:
+		seq1 = 0;
+		crtcext1 = 0;
+		mga_crtc_load_lut(crtc);
+		break;
+	case DRM_MODE_DPMS_STANDBY:
+		seq1 = 0x20;
+		crtcext1 = 0x10;
+		break;
+	case DRM_MODE_DPMS_SUSPEND:
+		seq1 = 0x20;
+		crtcext1 = 0x20;
+		break;
+	case DRM_MODE_DPMS_OFF:
+		seq1 = 0x20;
+		crtcext1 = 0x30;
+		break;
+	}
+
+#if 0
+	if (mode == DRM_MODE_DPMS_OFF) {
+		mga_suspend(crtc);
+	}
+#endif
+	WREG8(MGAREG_SEQ_INDEX, 0x01);
+	seq1 |= RREG8(MGAREG_SEQ_DATA) & ~0x20;
+	mga_wait_vsync(mdev);
+	mga_wait_busy(mdev);
+	WREG8(MGAREG_SEQ_DATA, seq1);
+	msleep(20);
+	WREG8(MGAREG_CRTCEXT_INDEX, 0x01);
+	crtcext1 |= RREG8(MGAREG_CRTCEXT_DATA) & ~0x30;
+	WREG8(MGAREG_CRTCEXT_DATA, crtcext1);
+
+#if 0
+	if (mode == DRM_MODE_DPMS_ON && mdev->suspended == true) {
+		mga_resume(crtc);
+		drm_helper_resume_force_mode(dev);
+	}
+#endif
+}
+
+/*
+ * This is called before a mode is programmed. A typical use might be to
+ * enable DPMS during the programming to avoid seeing intermediate stages,
+ * but that's not relevant to us
+ */
+static void mga_crtc_prepare(struct drm_crtc *crtc)
+{
+	struct drm_device *dev = crtc->dev;
+	struct mga_device *mdev = dev->dev_private;
+	u8 tmp;
+
+	/*	mga_resume(crtc);*/
+
+	WREG8(MGAREG_CRTC_INDEX, 0x11);
+	tmp = RREG8(MGAREG_CRTC_DATA);
+	WREG_CRT(0x11, tmp | 0x80);
+
+	if (mdev->type == G200_SE_A || mdev->type == G200_SE_B) {
+		WREG_SEQ(0, 1);
+		msleep(50);
+		WREG_SEQ(1, 0x20);
+		msleep(20);
+	} else {
+		WREG8(MGAREG_SEQ_INDEX, 0x1);
+		tmp = RREG8(MGAREG_SEQ_DATA);
+
+		/* start sync reset */
+		WREG_SEQ(0, 1);
+		WREG_SEQ(1, tmp | 0x20);
+	}
+
+	if (mdev->type == G200_WB)
+		mga_g200wb_prepare(crtc);
+
+	WREG_CRT(17, 0);
+}
+
+/*
+ * This is called after a mode is programmed. It should reverse anything done
+ * by the prepare function
+ */
+static void mga_crtc_commit(struct drm_crtc *crtc)
+{
+	struct drm_device *dev = crtc->dev;
+	struct mga_device *mdev = dev->dev_private;
+	struct drm_crtc_helper_funcs *crtc_funcs = crtc->helper_private;
+	u8 tmp;
+
+	if (mdev->type == G200_WB)
+		mga_g200wb_commit(crtc);
+
+	if (mdev->type == G200_SE_A || mdev->type == G200_SE_B) {
+		msleep(50);
+		WREG_SEQ(1, 0x0);
+		msleep(20);
+		WREG_SEQ(0, 0x3);
+	} else {
+		WREG8(MGAREG_SEQ_INDEX, 0x1);
+		tmp = RREG8(MGAREG_SEQ_DATA);
+
+		tmp &= ~0x20;
+		WREG_SEQ(0x1, tmp);
+		WREG_SEQ(0, 3);
+	}
+	crtc_funcs->dpms(crtc, DRM_MODE_DPMS_ON);
+}
+
+/*
+ * The core can pass us a set of gamma values to program. We actually only
+ * use this for 8-bit mode so can't perform smooth fades on deeper modes,
+ * but it's a requirement that we provide the function
+ */
+static void mga_crtc_gamma_set(struct drm_crtc *crtc, u16 *red, u16 *green,
+				  u16 *blue, uint32_t start, uint32_t size)
+{
+	struct mga_crtc *mga_crtc = to_mga_crtc(crtc);
+	int end = (start + size > MGAG200_LUT_SIZE) ? MGAG200_LUT_SIZE : start + size;
+	int i;
+
+	for (i = start; i < end; i++) {
+		mga_crtc->lut_r[i] = red[i] >> 8;
+		mga_crtc->lut_g[i] = green[i] >> 8;
+		mga_crtc->lut_b[i] = blue[i] >> 8;
+	}
+	mga_crtc_load_lut(crtc);
+}
+
+/* Simple cleanup function */
+static void mga_crtc_destroy(struct drm_crtc *crtc)
+{
+	struct mga_crtc *mga_crtc = to_mga_crtc(crtc);
+
+	drm_crtc_cleanup(crtc);
+	kfree(mga_crtc);
+}
+
+/* These provide the minimum set of functions required to handle a CRTC */
+static const struct drm_crtc_funcs mga_crtc_funcs = {
+	.gamma_set = mga_crtc_gamma_set,
+	.set_config = drm_crtc_helper_set_config,
+	.destroy = mga_crtc_destroy,
+};
+
+static const struct drm_crtc_helper_funcs mga_helper_funcs = {
+	.dpms = mga_crtc_dpms,
+	.mode_fixup = mga_crtc_mode_fixup,
+	.mode_set = mga_crtc_mode_set,
+	.mode_set_base = mga_crtc_mode_set_base,
+	.prepare = mga_crtc_prepare,
+	.commit = mga_crtc_commit,
+	.load_lut = mga_crtc_load_lut,
+};
+
+/* CRTC setup */
+static void mga_crtc_init(struct drm_device *dev)
+{
+	struct mga_device *mdev = dev->dev_private;
+	struct mga_crtc *mga_crtc;
+	int i;
+
+	mga_crtc = kzalloc(sizeof(struct mga_crtc) +
+			      (MGAG200FB_CONN_LIMIT * sizeof(struct drm_connector *)),
+			      GFP_KERNEL);
+
+	if (mga_crtc == NULL)
+		return;
+
+	drm_crtc_init(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;
+
+	for (i = 0; i < MGAG200_LUT_SIZE; i++) {
+		mga_crtc->lut_r[i] = i;
+		mga_crtc->lut_g[i] = i;
+		mga_crtc->lut_b[i] = i;
+	}
+
+	drm_crtc_helper_add(&mga_crtc->base, &mga_helper_funcs);
+}
+
+/** Sets the color ramps on behalf of fbcon */
+void mga_crtc_fb_gamma_set(struct drm_crtc *crtc, u16 red, u16 green,
+			      u16 blue, int regno)
+{
+	struct mga_crtc *mga_crtc = to_mga_crtc(crtc);
+
+	mga_crtc->lut_r[regno] = red >> 8;
+	mga_crtc->lut_g[regno] = green >> 8;
+	mga_crtc->lut_b[regno] = blue >> 8;
+}
+
+/** Gets the color ramps on behalf of fbcon */
+void mga_crtc_fb_gamma_get(struct drm_crtc *crtc, u16 *red, u16 *green,
+			      u16 *blue, int regno)
+{
+	struct mga_crtc *mga_crtc = to_mga_crtc(crtc);
+
+	*red = (u16)mga_crtc->lut_r[regno] << 8;
+	*green = (u16)mga_crtc->lut_g[regno] << 8;
+	*blue = (u16)mga_crtc->lut_b[regno] << 8;
+}
+
+/*
+ * The encoder comes after the CRTC in the output pipeline, but before
+ * the connector. It's responsible for ensuring that the digital
+ * stream is appropriately converted into the output format. Setup is
+ * very simple in this case - all we have to do is inform qemu of the
+ * colour depth in order to ensure that it displays appropriately
+ */
+
+/*
+ * These functions are analagous to those in the CRTC code, but are intended
+ * to handle any encoder-specific limitations
+ */
+static bool mga_encoder_mode_fixup(struct drm_encoder *encoder,
+				  struct drm_display_mode *mode,
+				  struct drm_display_mode *adjusted_mode)
+{
+	return true;
+}
+
+static void mga_encoder_mode_set(struct drm_encoder *encoder,
+				struct drm_display_mode *mode,
+				struct drm_display_mode *adjusted_mode)
+{
+
+}
+
+static void mga_encoder_dpms(struct drm_encoder *encoder, int state)
+{
+	return;
+}
+
+static void mga_encoder_prepare(struct drm_encoder *encoder)
+{
+}
+
+static void mga_encoder_commit(struct drm_encoder *encoder)
+{
+}
+
+void mga_encoder_destroy(struct drm_encoder *encoder)
+{
+	struct mga_encoder *mga_encoder = to_mga_encoder(encoder);
+	drm_encoder_cleanup(encoder);
+	kfree(mga_encoder);
+}
+
+static const struct drm_encoder_helper_funcs mga_encoder_helper_funcs = {
+	.dpms = mga_encoder_dpms,
+	.mode_fixup = mga_encoder_mode_fixup,
+	.mode_set = mga_encoder_mode_set,
+	.prepare = mga_encoder_prepare,
+	.commit = mga_encoder_commit,
+};
+
+static const struct drm_encoder_funcs mga_encoder_encoder_funcs = {
+	.destroy = mga_encoder_destroy,
+};
+
+static struct drm_encoder *mga_encoder_init(struct drm_device *dev)
+{
+	struct drm_encoder *encoder;
+	struct mga_encoder *mga_encoder;
+
+	mga_encoder = kzalloc(sizeof(struct mga_encoder), GFP_KERNEL);
+	if (!mga_encoder)
+		return NULL;
+
+	encoder = &mga_encoder->base;
+	encoder->possible_crtcs = 0x1;
+
+	drm_encoder_init(dev, encoder, &mga_encoder_encoder_funcs,
+			 DRM_MODE_ENCODER_DAC);
+	drm_encoder_helper_add(encoder, &mga_encoder_helper_funcs);
+
+	return encoder;
+}
+
+
+static int mga_vga_get_modes(struct drm_connector *connector)
+{
+	struct mga_connector *mga_connector = to_mga_connector(connector);
+	struct edid *edid;
+	int ret = 0;
+
+	edid = drm_get_edid(connector, &mga_connector->i2c->adapter);
+	if (edid) {
+		drm_mode_connector_update_edid_property(connector, edid);
+		ret = drm_add_edid_modes(connector, edid);
+		connector->display_info.raw_edid = NULL;
+		kfree(edid);
+	}
+	return ret;
+}
+
+static int mga_vga_mode_valid(struct drm_connector *connector,
+				 struct drm_display_mode *mode)
+{
+	/* FIXME: Add bandwidth and g200se limitations */
+
+	if (mode->crtc_hdisplay > 2048 || mode->crtc_hsync_start > 4096 ||
+	    mode->crtc_hsync_end > 4096 || mode->crtc_htotal > 4096 ||
+	    mode->crtc_vdisplay > 2048 || mode->crtc_vsync_start > 4096 ||
+	    mode->crtc_vsync_end > 4096 || mode->crtc_vtotal > 4096) {
+		return MODE_BAD;
+	}
+
+	return MODE_OK;
+}
+
+struct drm_encoder *mga_connector_best_encoder(struct drm_connector
+						  *connector)
+{
+	int enc_id = connector->encoder_ids[0];
+	struct drm_mode_object *obj;
+	struct drm_encoder *encoder;
+
+	/* pick the encoder ids */
+	if (enc_id) {
+		obj =
+		    drm_mode_object_find(connector->dev, enc_id,
+					 DRM_MODE_OBJECT_ENCODER);
+		if (!obj)
+			return NULL;
+		encoder = obj_to_encoder(obj);
+		return encoder;
+	}
+	return NULL;
+}
+
+static enum drm_connector_status mga_vga_detect(struct drm_connector
+						   *connector, bool force)
+{
+	return connector_status_connected;
+}
+
+static void mga_connector_destroy(struct drm_connector *connector)
+{
+	struct mga_connector *mga_connector = to_mga_connector(connector);
+	mgag200_i2c_destroy(mga_connector->i2c);
+	drm_connector_cleanup(connector);
+	kfree(connector);
+}
+
+struct drm_connector_helper_funcs mga_vga_connector_helper_funcs = {
+	.get_modes = mga_vga_get_modes,
+	.mode_valid = mga_vga_mode_valid,
+	.best_encoder = mga_connector_best_encoder,
+};
+
+struct drm_connector_funcs mga_vga_connector_funcs = {
+	.dpms = drm_helper_connector_dpms,
+	.detect = mga_vga_detect,
+	.fill_modes = drm_helper_probe_single_connector_modes,
+	.destroy = mga_connector_destroy,
+};
+
+static struct drm_connector *mga_vga_init(struct drm_device *dev)
+{
+	struct drm_connector *connector;
+	struct mga_connector *mga_connector;
+
+	mga_connector = kzalloc(sizeof(struct mga_connector), GFP_KERNEL);
+	if (!mga_connector)
+		return NULL;
+
+	connector = &mga_connector->base;
+
+	drm_connector_init(dev, connector,
+			   &mga_vga_connector_funcs, DRM_MODE_CONNECTOR_VGA);
+
+	drm_connector_helper_add(connector, &mga_vga_connector_helper_funcs);
+
+	mga_connector->i2c = mgag200_i2c_create(dev);
+	if (!mga_connector->i2c)
+		DRM_ERROR("failed to add ddc bus\n");
+
+	return connector;
+}
+
+
+int mgag200_modeset_init(struct mga_device *mdev)
+{
+	struct drm_encoder *encoder;
+	struct drm_connector *connector;
+	int ret;
+
+	mdev->mode_info.mode_config_initialized = true;
+
+	mdev->dev->mode_config.max_width = MGAG200_MAX_FB_WIDTH;
+	mdev->dev->mode_config.max_height = MGAG200_MAX_FB_HEIGHT;
+
+	mdev->dev->mode_config.fb_base = mdev->mc.vram_base;
+
+	mga_crtc_init(mdev->dev);
+
+	encoder = mga_encoder_init(mdev->dev);
+	if (!encoder) {
+		DRM_ERROR("mga_encoder_init failed\n");
+		return -1;
+	}
+
+	connector = mga_vga_init(mdev->dev);
+	if (!connector) {
+		DRM_ERROR("mga_vga_init failed\n");
+		return -1;
+	}
+
+	drm_mode_connector_attach_encoder(connector, encoder);
+
+	ret = mgag200_fbdev_init(mdev);
+	if (ret) {
+		DRM_ERROR("mga_fbdev_init failed\n");
+		return ret;
+	}
+
+	return 0;
+}
+
+void mgag200_modeset_fini(struct mga_device *mdev)
+{
+
+}
diff --git a/drivers/gpu/drm/mgag200/mgag200_reg.h b/drivers/gpu/drm/mgag200/mgag200_reg.h
new file mode 100644
index 00000000000..fb24d8655fe
--- /dev/null
+++ b/drivers/gpu/drm/mgag200/mgag200_reg.h
@@ -0,0 +1,661 @@
+/*
+ * MGA Millennium (MGA2064W) functions
+ * MGA Mystique (MGA1064SG) functions
+ *
+ * Copyright 1996 The XFree86 Project, Inc.
+ *
+ * Authors
+ *		Dirk Hohndel
+ *			hohndel@XFree86.Org
+ *		David Dawes
+ *			dawes@XFree86.Org
+ * Contributors:
+ *		Guy DESBIEF, Aix-en-provence, France
+ *			g.desbief@aix.pacwan.net
+ *		MGA1064SG Mystique register file
+ */
+
+
+#ifndef _MGA_REG_H_
+#define _MGA_REG_H_
+
+#define	MGAREG_DWGCTL		0x1c00
+#define	MGAREG_MACCESS		0x1c04
+/* the following is a mystique only register */
+#define MGAREG_MCTLWTST		0x1c08
+#define	MGAREG_ZORG		0x1c0c
+
+#define	MGAREG_PAT0		0x1c10
+#define	MGAREG_PAT1		0x1c14
+#define	MGAREG_PLNWT		0x1c1c
+
+#define	MGAREG_BCOL		0x1c20
+#define	MGAREG_FCOL		0x1c24
+
+#define	MGAREG_SRC0		0x1c30
+#define	MGAREG_SRC1		0x1c34
+#define	MGAREG_SRC2		0x1c38
+#define	MGAREG_SRC3		0x1c3c
+
+#define	MGAREG_XYSTRT		0x1c40
+#define	MGAREG_XYEND		0x1c44
+
+#define	MGAREG_SHIFT		0x1c50
+/* the following is a mystique only register */
+#define MGAREG_DMAPAD		0x1c54
+#define	MGAREG_SGN		0x1c58
+#define	MGAREG_LEN		0x1c5c
+
+#define	MGAREG_AR0		0x1c60
+#define	MGAREG_AR1		0x1c64
+#define	MGAREG_AR2		0x1c68
+#define	MGAREG_AR3		0x1c6c
+#define	MGAREG_AR4		0x1c70
+#define	MGAREG_AR5		0x1c74
+#define	MGAREG_AR6		0x1c78
+
+#define	MGAREG_CXBNDRY		0x1c80
+#define	MGAREG_FXBNDRY		0x1c84
+#define	MGAREG_YDSTLEN		0x1c88
+#define	MGAREG_PITCH		0x1c8c
+
+#define	MGAREG_YDST		0x1c90
+#define	MGAREG_YDSTORG		0x1c94
+#define	MGAREG_YTOP		0x1c98
+#define	MGAREG_YBOT		0x1c9c
+
+#define	MGAREG_CXLEFT		0x1ca0
+#define	MGAREG_CXRIGHT		0x1ca4
+#define	MGAREG_FXLEFT		0x1ca8
+#define	MGAREG_FXRIGHT		0x1cac
+
+#define	MGAREG_XDST		0x1cb0
+
+#define	MGAREG_DR0		0x1cc0
+#define	MGAREG_DR1		0x1cc4
+#define	MGAREG_DR2		0x1cc8
+#define	MGAREG_DR3		0x1ccc
+
+#define	MGAREG_DR4		0x1cd0
+#define	MGAREG_DR5		0x1cd4
+#define	MGAREG_DR6		0x1cd8
+#define	MGAREG_DR7		0x1cdc
+
+#define	MGAREG_DR8		0x1ce0
+#define	MGAREG_DR9		0x1ce4
+#define	MGAREG_DR10		0x1ce8
+#define	MGAREG_DR11		0x1cec
+
+#define	MGAREG_DR12		0x1cf0
+#define	MGAREG_DR13		0x1cf4
+#define	MGAREG_DR14		0x1cf8
+#define	MGAREG_DR15		0x1cfc
+
+#define MGAREG_SRCORG		0x2cb4
+#define MGAREG_DSTORG		0x2cb8
+
+/* add or or this to one of the previous "power registers" to start
+   the drawing engine */
+
+#define MGAREG_EXEC		0x0100
+
+#define	MGAREG_FIFOSTATUS	0x1e10
+#define	MGAREG_Status		0x1e14
+#define MGAREG_CACHEFLUSH       0x1fff
+#define	MGAREG_ICLEAR		0x1e18
+#define	MGAREG_IEN		0x1e1c
+
+#define	MGAREG_VCOUNT		0x1e20
+
+#define	MGAREG_Reset		0x1e40
+
+#define	MGAREG_OPMODE		0x1e54
+
+/* Warp Registers */
+#define MGAREG_WIADDR           0x1dc0
+#define MGAREG_WIADDR2          0x1dd8
+#define MGAREG_WGETMSB          0x1dc8
+#define MGAREG_WVRTXSZ          0x1dcc
+#define MGAREG_WACCEPTSEQ       0x1dd4
+#define MGAREG_WMISC            0x1e70
+
+#define MGAREG_MEMCTL           0x2e08
+
+/* OPMODE register additives */
+
+#define MGAOPM_DMA_GENERAL	(0x00 << 2)
+#define MGAOPM_DMA_BLIT		(0x01 << 2)
+#define MGAOPM_DMA_VECTOR	(0x10 << 2)
+
+/* MACCESS register additives */
+#define MGAMAC_PW8               0x00
+#define MGAMAC_PW16              0x01
+#define MGAMAC_PW24              0x03 /* not a typo */
+#define MGAMAC_PW32              0x02 /* not a typo */
+#define MGAMAC_BYPASS332         0x10000000
+#define MGAMAC_NODITHER          0x40000000
+#define MGAMAC_DIT555            0x80000000
+
+/* DWGCTL register additives */
+
+/* Lines */
+
+#define MGADWG_LINE_OPEN	0x00
+#define MGADWG_AUTOLINE_OPEN	0x01
+#define MGADWG_LINE_CLOSE	0x02
+#define MGADWG_AUTOLINE_CLOSE	0x03
+
+/* Trapezoids */
+#define MGADWG_TRAP		0x04
+#define MGADWG_TEXTURE_TRAP	0x06
+
+/* BitBlts */
+
+#define MGADWG_BITBLT		0x08
+#define MGADWG_FBITBLT		0x0c
+#define MGADWG_ILOAD		0x09
+#define MGADWG_ILOAD_SCALE	0x0d
+#define MGADWG_ILOAD_FILTER	0x0f
+#define MGADWG_ILOAD_HIQH	0x07
+#define MGADWG_ILOAD_HIQHV	0x0e
+#define MGADWG_IDUMP		0x0a
+
+/* atype access to WRAM */
+
+#define MGADWG_RPL		( 0x00 << 4 )
+#define MGADWG_RSTR		( 0x01 << 4 )
+#define MGADWG_ZI		( 0x03 << 4 )
+#define MGADWG_BLK 		( 0x04 << 4 )
+#define MGADWG_I		( 0x07 << 4 )
+
+/* specifies whether bit blits are linear or xy */
+#define MGADWG_LINEAR		( 0x01 << 7 )
+
+/* z drawing mode. use MGADWG_NOZCMP for always */
+
+#define MGADWG_NOZCMP		( 0x00 << 8 )
+#define MGADWG_ZE		( 0x02 << 8 )
+#define MGADWG_ZNE		( 0x03 << 8 )
+#define MGADWG_ZLT		( 0x04 << 8 )
+#define MGADWG_ZLTE		( 0x05 << 8 )
+#define MGADWG_GT		( 0x06 << 8 )
+#define MGADWG_GTE		( 0x07 << 8 )
+
+/* use this to force colour expansion circuitry to do its stuff */
+
+#define MGADWG_SOLID		( 0x01 << 11 )
+
+/* ar register at zero */
+
+#define MGADWG_ARZERO		( 0x01 << 12 )
+
+#define MGADWG_SGNZERO		( 0x01 << 13 )
+
+#define MGADWG_SHIFTZERO	( 0x01 << 14 )
+
+/* See table on 4-43 for bop ALU operations */
+
+/* See table on 4-44 for translucidity masks */
+
+#define MGADWG_BMONOLEF		( 0x00 << 25 )
+#define MGADWG_BMONOWF		( 0x04 << 25 )
+#define MGADWG_BPLAN		( 0x01 << 25 )
+
+/* note that if bfcol is specified and you're doing a bitblt, it causes
+   a fbitblt to be performed, so check that you obey the fbitblt rules */
+
+#define MGADWG_BFCOL   		( 0x02 << 25 )
+#define MGADWG_BUYUV		( 0x0e << 25 )
+#define MGADWG_BU32BGR		( 0x03 << 25 )
+#define MGADWG_BU32RGB		( 0x07 << 25 )
+#define MGADWG_BU24BGR		( 0x0b << 25 )
+#define MGADWG_BU24RGB		( 0x0f << 25 )
+
+#define MGADWG_PATTERN		( 0x01 << 29 )
+#define MGADWG_TRANSC		( 0x01 << 30 )
+#define MGAREG_MISC_WRITE	0x3c2
+#define MGAREG_MISC_READ	0x3cc
+#define MGAREG_MEM_MISC_WRITE       0x1fc2
+#define MGAREG_MEM_MISC_READ        0x1fcc
+
+#define MGAREG_MISC_IOADSEL	(0x1 << 0)
+#define MGAREG_MISC_RAMMAPEN	(0x1 << 1)
+#define MGAREG_MISC_CLK_SEL_VGA25	(0x0 << 2)
+#define MGAREG_MISC_CLK_SEL_VGA28	(0x1 << 2)
+#define MGAREG_MISC_CLK_SEL_MGA_PIX	(0x2 << 2)
+#define MGAREG_MISC_CLK_SEL_MGA_MSK	(0x3 << 2)
+#define MGAREG_MISC_VIDEO_DIS	(0x1 << 4)
+#define MGAREG_MISC_HIGH_PG_SEL	(0x1 << 5)
+
+/* MMIO VGA registers */
+#define MGAREG_SEQ_INDEX	0x1fc4
+#define MGAREG_SEQ_DATA		0x1fc5
+#define MGAREG_CRTC_INDEX	0x1fd4
+#define MGAREG_CRTC_DATA	0x1fd5
+#define MGAREG_CRTCEXT_INDEX	0x1fde
+#define MGAREG_CRTCEXT_DATA	0x1fdf
+
+
+
+/* MGA bits for registers PCI_OPTION_REG */
+#define MGA1064_OPT_SYS_CLK_PCI   		( 0x00 << 0 )
+#define MGA1064_OPT_SYS_CLK_PLL   		( 0x01 << 0 )
+#define MGA1064_OPT_SYS_CLK_EXT   		( 0x02 << 0 )
+#define MGA1064_OPT_SYS_CLK_MSK   		( 0x03 << 0 )
+
+#define MGA1064_OPT_SYS_CLK_DIS   		( 0x01 << 2 )
+#define MGA1064_OPT_G_CLK_DIV_1   		( 0x01 << 3 )
+#define MGA1064_OPT_M_CLK_DIV_1   		( 0x01 << 4 )
+
+#define MGA1064_OPT_SYS_PLL_PDN   		( 0x01 << 5 )
+#define MGA1064_OPT_VGA_ION   		( 0x01 << 8 )
+
+/* MGA registers in PCI config space */
+#define PCI_MGA_INDEX		0x44
+#define PCI_MGA_DATA		0x48
+#define PCI_MGA_OPTION		0x40
+#define PCI_MGA_OPTION2		0x50
+#define PCI_MGA_OPTION3		0x54
+
+#define RAMDAC_OFFSET		0x3c00
+
+/* TVP3026 direct registers */
+
+#define TVP3026_INDEX		0x00
+#define TVP3026_WADR_PAL	0x00
+#define TVP3026_COL_PAL		0x01
+#define TVP3026_PIX_RD_MSK	0x02
+#define TVP3026_RADR_PAL	0x03
+#define TVP3026_CUR_COL_ADDR	0x04
+#define TVP3026_CUR_COL_DATA	0x05
+#define TVP3026_DATA		0x0a
+#define TVP3026_CUR_RAM		0x0b
+#define TVP3026_CUR_XLOW	0x0c
+#define TVP3026_CUR_XHI		0x0d
+#define TVP3026_CUR_YLOW	0x0e
+#define TVP3026_CUR_YHI		0x0f
+
+/* TVP3026 indirect registers */
+
+#define TVP3026_SILICON_REV	0x01
+#define TVP3026_CURSOR_CTL	0x06
+#define TVP3026_LATCH_CTL	0x0f
+#define TVP3026_TRUE_COLOR_CTL	0x18
+#define TVP3026_MUX_CTL		0x19
+#define TVP3026_CLK_SEL		0x1a
+#define TVP3026_PAL_PAGE	0x1c
+#define TVP3026_GEN_CTL		0x1d
+#define TVP3026_MISC_CTL	0x1e
+#define TVP3026_GEN_IO_CTL	0x2a
+#define TVP3026_GEN_IO_DATA	0x2b
+#define TVP3026_PLL_ADDR	0x2c
+#define TVP3026_PIX_CLK_DATA	0x2d
+#define TVP3026_MEM_CLK_DATA	0x2e
+#define TVP3026_LOAD_CLK_DATA	0x2f
+#define TVP3026_KEY_RED_LOW	0x32
+#define TVP3026_KEY_RED_HI	0x33
+#define TVP3026_KEY_GREEN_LOW	0x34
+#define TVP3026_KEY_GREEN_HI	0x35
+#define TVP3026_KEY_BLUE_LOW	0x36
+#define TVP3026_KEY_BLUE_HI	0x37
+#define TVP3026_KEY_CTL		0x38
+#define TVP3026_MCLK_CTL	0x39
+#define TVP3026_SENSE_TEST	0x3a
+#define TVP3026_TEST_DATA	0x3b
+#define TVP3026_CRC_LSB		0x3c
+#define TVP3026_CRC_MSB		0x3d
+#define TVP3026_CRC_CTL		0x3e
+#define TVP3026_ID		0x3f
+#define TVP3026_RESET		0xff
+
+
+/* MGA1064 DAC Register file */
+/* MGA1064 direct registers */
+
+#define MGA1064_INDEX		0x00
+#define MGA1064_WADR_PAL	0x00
+#define MGA1064_SPAREREG        0x00
+#define MGA1064_COL_PAL		0x01
+#define MGA1064_PIX_RD_MSK	0x02
+#define MGA1064_RADR_PAL	0x03
+#define MGA1064_DATA		0x0a
+
+#define MGA1064_CUR_XLOW	0x0c
+#define MGA1064_CUR_XHI		0x0d
+#define MGA1064_CUR_YLOW	0x0e
+#define MGA1064_CUR_YHI		0x0f
+
+/* MGA1064 indirect registers */
+#define MGA1064_DVI_PIPE_CTL    0x03
+#define MGA1064_CURSOR_BASE_ADR_LOW	0x04
+#define MGA1064_CURSOR_BASE_ADR_HI	0x05
+#define MGA1064_CURSOR_CTL	0x06
+#define MGA1064_CURSOR_COL0_RED	0x08
+#define MGA1064_CURSOR_COL0_GREEN	0x09
+#define MGA1064_CURSOR_COL0_BLUE	0x0a
+
+#define MGA1064_CURSOR_COL1_RED	0x0c
+#define MGA1064_CURSOR_COL1_GREEN	0x0d
+#define MGA1064_CURSOR_COL1_BLUE	0x0e
+
+#define MGA1064_CURSOR_COL2_RED	0x010
+#define MGA1064_CURSOR_COL2_GREEN	0x011
+#define MGA1064_CURSOR_COL2_BLUE	0x012
+
+#define MGA1064_VREF_CTL	0x018
+
+#define MGA1064_MUL_CTL		0x19
+#define MGA1064_MUL_CTL_8bits		0x0
+#define MGA1064_MUL_CTL_15bits		0x01
+#define MGA1064_MUL_CTL_16bits		0x02
+#define MGA1064_MUL_CTL_24bits		0x03
+#define MGA1064_MUL_CTL_32bits		0x04
+#define MGA1064_MUL_CTL_2G8V16bits		0x05
+#define MGA1064_MUL_CTL_G16V16bits		0x06
+#define MGA1064_MUL_CTL_32_24bits		0x07
+
+#define MGA1064_PIX_CLK_CTL		0x1a
+#define MGA1064_PIX_CLK_CTL_CLK_DIS		( 0x01 << 2 )
+#define MGA1064_PIX_CLK_CTL_CLK_POW_DOWN	( 0x01 << 3 )
+#define MGA1064_PIX_CLK_CTL_SEL_PCI		( 0x00 << 0 )
+#define MGA1064_PIX_CLK_CTL_SEL_PLL		( 0x01 << 0 )
+#define MGA1064_PIX_CLK_CTL_SEL_EXT		( 0x02 << 0 )
+#define MGA1064_PIX_CLK_CTL_SEL_MSK		( 0x03 << 0 )
+
+#define MGA1064_GEN_CTL		0x1d
+#define MGA1064_GEN_CTL_SYNC_ON_GREEN_DIS      (0x01 << 5)
+#define MGA1064_MISC_CTL	0x1e
+#define MGA1064_MISC_CTL_DAC_EN                ( 0x01 << 0 )
+#define MGA1064_MISC_CTL_VGA   		( 0x01 << 1 )
+#define MGA1064_MISC_CTL_DIS_CON   		( 0x03 << 1 )
+#define MGA1064_MISC_CTL_MAFC   		( 0x02 << 1 )
+#define MGA1064_MISC_CTL_VGA8   		( 0x01 << 3 )
+#define MGA1064_MISC_CTL_DAC_RAM_CS   		( 0x01 << 4 )
+
+#define MGA1064_GEN_IO_CTL2	0x29
+#define MGA1064_GEN_IO_CTL	0x2a
+#define MGA1064_GEN_IO_DATA	0x2b
+#define MGA1064_SYS_PLL_M	0x2c
+#define MGA1064_SYS_PLL_N	0x2d
+#define MGA1064_SYS_PLL_P	0x2e
+#define MGA1064_SYS_PLL_STAT	0x2f
+
+#define MGA1064_REMHEADCTL     0x30
+#define MGA1064_REMHEADCTL_CLKDIS ( 0x01 << 0 )
+#define MGA1064_REMHEADCTL_CLKSL_OFF ( 0x00 << 1 )
+#define MGA1064_REMHEADCTL_CLKSL_PLL ( 0x01 << 1 )
+#define MGA1064_REMHEADCTL_CLKSL_PCI ( 0x02 << 1 )
+#define MGA1064_REMHEADCTL_CLKSL_MSK ( 0x03 << 1 )
+
+#define MGA1064_REMHEADCTL2     0x31
+
+#define MGA1064_ZOOM_CTL	0x38
+#define MGA1064_SENSE_TST	0x3a
+
+#define MGA1064_CRC_LSB		0x3c
+#define MGA1064_CRC_MSB		0x3d
+#define MGA1064_CRC_CTL		0x3e
+#define MGA1064_COL_KEY_MSK_LSB		0x40
+#define MGA1064_COL_KEY_MSK_MSB		0x41
+#define MGA1064_COL_KEY_LSB		0x42
+#define MGA1064_COL_KEY_MSB		0x43
+#define MGA1064_PIX_PLLA_M	0x44
+#define MGA1064_PIX_PLLA_N	0x45
+#define MGA1064_PIX_PLLA_P	0x46
+#define MGA1064_PIX_PLLB_M	0x48
+#define MGA1064_PIX_PLLB_N	0x49
+#define MGA1064_PIX_PLLB_P	0x4a
+#define MGA1064_PIX_PLLC_M	0x4c
+#define MGA1064_PIX_PLLC_N	0x4d
+#define MGA1064_PIX_PLLC_P	0x4e
+
+#define MGA1064_PIX_PLL_STAT	0x4f
+
+/*Added for G450 dual head*/
+
+#define MGA1064_VID_PLL_STAT    0x8c
+#define MGA1064_VID_PLL_P       0x8D
+#define MGA1064_VID_PLL_M       0x8E
+#define MGA1064_VID_PLL_N       0x8F
+
+/* Modified PLL for G200 Winbond (G200WB) */
+#define MGA1064_WB_PIX_PLLC_M	0xb7
+#define MGA1064_WB_PIX_PLLC_N	0xb6
+#define MGA1064_WB_PIX_PLLC_P	0xb8
+
+/* Modified PLL for G200 Maxim (G200EV) */
+#define MGA1064_EV_PIX_PLLC_M	0xb6
+#define MGA1064_EV_PIX_PLLC_N	0xb7
+#define MGA1064_EV_PIX_PLLC_P	0xb8
+
+/* Modified PLL for G200 EH */
+#define MGA1064_EH_PIX_PLLC_M   0xb6
+#define MGA1064_EH_PIX_PLLC_N   0xb7
+#define MGA1064_EH_PIX_PLLC_P   0xb8
+
+/* Modified PLL for G200 Maxim (G200ER) */
+#define MGA1064_ER_PIX_PLLC_M	0xb7
+#define MGA1064_ER_PIX_PLLC_N	0xb6
+#define MGA1064_ER_PIX_PLLC_P	0xb8
+
+#define MGA1064_DISP_CTL        0x8a
+#define MGA1064_DISP_CTL_DAC1OUTSEL_MASK       0x01
+#define MGA1064_DISP_CTL_DAC1OUTSEL_DIS        0x00
+#define MGA1064_DISP_CTL_DAC1OUTSEL_EN         0x01
+#define MGA1064_DISP_CTL_DAC2OUTSEL_MASK       (0x03 << 2)
+#define MGA1064_DISP_CTL_DAC2OUTSEL_DIS        0x00
+#define MGA1064_DISP_CTL_DAC2OUTSEL_CRTC1      (0x01 << 2)
+#define MGA1064_DISP_CTL_DAC2OUTSEL_CRTC2      (0x02 << 2)
+#define MGA1064_DISP_CTL_DAC2OUTSEL_TVE        (0x03 << 2)
+#define MGA1064_DISP_CTL_PANOUTSEL_MASK        (0x03 << 5)
+#define MGA1064_DISP_CTL_PANOUTSEL_DIS         0x00
+#define MGA1064_DISP_CTL_PANOUTSEL_CRTC1       (0x01 << 5)
+#define MGA1064_DISP_CTL_PANOUTSEL_CRTC2RGB    (0x02 << 5)
+#define MGA1064_DISP_CTL_PANOUTSEL_CRTC2656    (0x03 << 5)
+
+#define MGA1064_SYNC_CTL        0x8b
+
+#define MGA1064_PWR_CTL         0xa0
+#define MGA1064_PWR_CTL_DAC2_EN                (0x01 << 0)
+#define MGA1064_PWR_CTL_VID_PLL_EN             (0x01 << 1)
+#define MGA1064_PWR_CTL_PANEL_EN               (0x01 << 2)
+#define MGA1064_PWR_CTL_RFIFO_EN               (0x01 << 3)
+#define MGA1064_PWR_CTL_CFIFO_EN               (0x01 << 4)
+
+#define MGA1064_PAN_CTL         0xa2
+
+/* Using crtc2 */
+#define MGAREG2_C2CTL            0x10
+#define MGAREG2_C2HPARAM         0x14
+#define MGAREG2_C2HSYNC          0x18
+#define MGAREG2_C2VPARAM         0x1c
+#define MGAREG2_C2VSYNC          0x20
+#define MGAREG2_C2STARTADD0      0x28
+
+#define MGAREG2_C2OFFSET         0x40
+#define MGAREG2_C2DATACTL        0x4c
+
+#define MGAREG_C2CTL            0x3c10
+#define MGAREG_C2CTL_C2_EN                     0x01
+
+#define MGAREG_C2_HIPRILVL_M                   (0x07 << 4)
+#define MGAREG_C2_MAXHIPRI_M                   (0x07 << 8)
+
+#define MGAREG_C2CTL_PIXCLKSEL_MASK            (0x03 << 1)
+#define MGAREG_C2CTL_PIXCLKSELH_MASK           (0x01 << 14)
+#define MGAREG_C2CTL_PIXCLKSEL_PCICLK          0x00
+#define MGAREG_C2CTL_PIXCLKSEL_VDOCLK          (0x01 << 1)
+#define MGAREG_C2CTL_PIXCLKSEL_PIXELPLL        (0x02 << 1)
+#define MGAREG_C2CTL_PIXCLKSEL_VIDEOPLL        (0x03 << 1)
+#define MGAREG_C2CTL_PIXCLKSEL_VDCLK           (0x01 << 14)
+
+#define MGAREG_C2CTL_PIXCLKSEL_CRISTAL         (0x01 << 1) | (0x01 << 14)
+#define MGAREG_C2CTL_PIXCLKSEL_SYSTEMPLL       (0x02 << 1) | (0x01 << 14)
+
+#define MGAREG_C2CTL_PIXCLKDIS_MASK            (0x01 << 3)
+#define MGAREG_C2CTL_PIXCLKDIS_DISABLE         (0x01 << 3)
+
+#define MGAREG_C2CTL_CRTCDACSEL_MASK           (0x01 << 20)
+#define MGAREG_C2CTL_CRTCDACSEL_CRTC1          0x00
+#define MGAREG_C2CTL_CRTCDACSEL_CRTC2          (0x01 << 20)
+
+#define MGAREG_C2HPARAM         0x3c14
+#define MGAREG_C2HSYNC          0x3c18
+#define MGAREG_C2VPARAM         0x3c1c
+#define MGAREG_C2VSYNC          0x3c20
+#define MGAREG_C2STARTADD0      0x3c28
+
+#define MGAREG_C2OFFSET         0x3c40
+#define MGAREG_C2DATACTL        0x3c4c
+
+/* video register */
+
+#define MGAREG_BESA1C3ORG	0x3d60
+#define MGAREG_BESA1CORG	0x3d10
+#define MGAREG_BESA1ORG		0x3d00
+#define MGAREG_BESCTL		0x3d20
+#define MGAREG_BESGLOBCTL	0x3dc0
+#define MGAREG_BESHCOORD	0x3d28
+#define MGAREG_BESHISCAL	0x3d30
+#define MGAREG_BESHSRCEND	0x3d3c
+#define MGAREG_BESHSRCLST	0x3d50
+#define MGAREG_BESHSRCST	0x3d38
+#define MGAREG_BESLUMACTL	0x3d40
+#define MGAREG_BESPITCH		0x3d24
+#define MGAREG_BESV1SRCLST	0x3d54
+#define MGAREG_BESV1WGHT	0x3d48
+#define MGAREG_BESVCOORD	0x3d2c
+#define MGAREG_BESVISCAL	0x3d34
+
+/* texture engine registers */
+
+#define MGAREG_TMR0		0x2c00
+#define MGAREG_TMR1		0x2c04
+#define MGAREG_TMR2		0x2c08
+#define MGAREG_TMR3		0x2c0c
+#define MGAREG_TMR4		0x2c10
+#define MGAREG_TMR5		0x2c14
+#define MGAREG_TMR6		0x2c18
+#define MGAREG_TMR7		0x2c1c
+#define MGAREG_TMR8		0x2c20
+#define MGAREG_TEXORG		0x2c24
+#define MGAREG_TEXWIDTH		0x2c28
+#define MGAREG_TEXHEIGHT	0x2c2c
+#define MGAREG_TEXCTL		0x2c30
+#    define MGA_TW4                             (0x00000000)
+#    define MGA_TW8                             (0x00000001)
+#    define MGA_TW15                            (0x00000002)
+#    define MGA_TW16                            (0x00000003)
+#    define MGA_TW12                            (0x00000004)
+#    define MGA_TW32                            (0x00000006)
+#    define MGA_TW8A                            (0x00000007)
+#    define MGA_TW8AL                           (0x00000008)
+#    define MGA_TW422                           (0x0000000A)
+#    define MGA_TW422UYVY                       (0x0000000B)
+#    define MGA_PITCHLIN                        (0x00000100)
+#    define MGA_NOPERSPECTIVE                   (0x00200000)
+#    define MGA_TAKEY                           (0x02000000)
+#    define MGA_TAMASK                          (0x04000000)
+#    define MGA_CLAMPUV                         (0x18000000)
+#    define MGA_TEXMODULATE                     (0x20000000)
+#define MGAREG_TEXCTL2		0x2c3c
+#    define MGA_G400_TC2_MAGIC                  (0x00008000)
+#    define MGA_TC2_DECALBLEND                  (0x00000001)
+#    define MGA_TC2_IDECAL                      (0x00000002)
+#    define MGA_TC2_DECALDIS                    (0x00000004)
+#    define MGA_TC2_CKSTRANSDIS                 (0x00000010)
+#    define MGA_TC2_BORDEREN                    (0x00000020)
+#    define MGA_TC2_SPECEN                      (0x00000040)
+#    define MGA_TC2_DUALTEX                     (0x00000080)
+#    define MGA_TC2_TABLEFOG                    (0x00000100)
+#    define MGA_TC2_BUMPMAP                     (0x00000200)
+#    define MGA_TC2_SELECT_TMU1                 (0x80000000)
+#define MGAREG_TEXTRANS		0x2c34
+#define MGAREG_TEXTRANSHIGH	0x2c38
+#define MGAREG_TEXFILTER	0x2c58
+#    define MGA_MIN_NRST                        (0x00000000)
+#    define MGA_MIN_BILIN                       (0x00000002)
+#    define MGA_MIN_ANISO                       (0x0000000D)
+#    define MGA_MAG_NRST                        (0x00000000)
+#    define MGA_MAG_BILIN                       (0x00000020)
+#    define MGA_FILTERALPHA                     (0x00100000)
+#define MGAREG_ALPHASTART	0x2c70
+#define MGAREG_ALPHAXINC	0x2c74
+#define MGAREG_ALPHAYINC	0x2c78
+#define MGAREG_ALPHACTRL	0x2c7c
+#    define MGA_SRC_ZERO                        (0x00000000)
+#    define MGA_SRC_ONE                         (0x00000001)
+#    define MGA_SRC_DST_COLOR                   (0x00000002)
+#    define MGA_SRC_ONE_MINUS_DST_COLOR         (0x00000003)
+#    define MGA_SRC_ALPHA                       (0x00000004)
+#    define MGA_SRC_ONE_MINUS_SRC_ALPHA         (0x00000005)
+#    define MGA_SRC_DST_ALPHA                   (0x00000006)
+#    define MGA_SRC_ONE_MINUS_DST_ALPHA         (0x00000007)
+#    define MGA_SRC_SRC_ALPHA_SATURATE          (0x00000008)
+#    define MGA_SRC_BLEND_MASK                  (0x0000000f)
+#    define MGA_DST_ZERO                        (0x00000000)
+#    define MGA_DST_ONE                         (0x00000010)
+#    define MGA_DST_SRC_COLOR                   (0x00000020)
+#    define MGA_DST_ONE_MINUS_SRC_COLOR         (0x00000030)
+#    define MGA_DST_SRC_ALPHA                   (0x00000040)
+#    define MGA_DST_ONE_MINUS_SRC_ALPHA         (0x00000050)
+#    define MGA_DST_DST_ALPHA                   (0x00000060)
+#    define MGA_DST_ONE_MINUS_DST_ALPHA         (0x00000070)
+#    define MGA_DST_BLEND_MASK                  (0x00000070)
+#    define MGA_ALPHACHANNEL                    (0x00000100)
+#    define MGA_VIDEOALPHA                      (0x00000200)
+#    define MGA_DIFFUSEDALPHA                   (0x01000000)
+#    define MGA_MODULATEDALPHA                  (0x02000000)
+#define MGAREG_TDUALSTAGE0                      (0x2CF8)
+#define MGAREG_TDUALSTAGE1                      (0x2CFC)
+#    define MGA_TDS_COLOR_ARG2_DIFFUSE          (0x00000000)
+#    define MGA_TDS_COLOR_ARG2_SPECULAR         (0x00000001)
+#    define MGA_TDS_COLOR_ARG2_FCOL             (0x00000002)
+#    define MGA_TDS_COLOR_ARG2_PREVSTAGE        (0x00000003)
+#    define MGA_TDS_COLOR_ALPHA_DIFFUSE         (0x00000000)
+#    define MGA_TDS_COLOR_ALPHA_FCOL            (0x00000004)
+#    define MGA_TDS_COLOR_ALPHA_CURRTEX         (0x00000008)
+#    define MGA_TDS_COLOR_ALPHA_PREVTEX         (0x0000000c)
+#    define MGA_TDS_COLOR_ALPHA_PREVSTAGE       (0x00000010)
+#    define MGA_TDS_COLOR_ARG1_REPLICATEALPHA   (0x00000020)
+#    define MGA_TDS_COLOR_ARG1_INV              (0x00000040)
+#    define MGA_TDS_COLOR_ARG2_REPLICATEALPHA   (0x00000080)
+#    define MGA_TDS_COLOR_ARG2_INV              (0x00000100)
+#    define MGA_TDS_COLOR_ALPHA1INV             (0x00000200)
+#    define MGA_TDS_COLOR_ALPHA2INV             (0x00000400)
+#    define MGA_TDS_COLOR_ARG1MUL_ALPHA1        (0x00000800)
+#    define MGA_TDS_COLOR_ARG2MUL_ALPHA2        (0x00001000)
+#    define MGA_TDS_COLOR_ARG1ADD_MULOUT        (0x00002000)
+#    define MGA_TDS_COLOR_ARG2ADD_MULOUT        (0x00004000)
+#    define MGA_TDS_COLOR_MODBRIGHT_2X          (0x00008000)
+#    define MGA_TDS_COLOR_MODBRIGHT_4X          (0x00010000)
+#    define MGA_TDS_COLOR_ADD_SUB               (0x00000000)
+#    define MGA_TDS_COLOR_ADD_ADD               (0x00020000)
+#    define MGA_TDS_COLOR_ADD2X                 (0x00040000)
+#    define MGA_TDS_COLOR_ADDBIAS               (0x00080000)
+#    define MGA_TDS_COLOR_BLEND                 (0x00100000)
+#    define MGA_TDS_COLOR_SEL_ARG1              (0x00000000)
+#    define MGA_TDS_COLOR_SEL_ARG2              (0x00200000)
+#    define MGA_TDS_COLOR_SEL_ADD               (0x00400000)
+#    define MGA_TDS_COLOR_SEL_MUL               (0x00600000)
+#    define MGA_TDS_ALPHA_ARG1_INV              (0x00800000)
+#    define MGA_TDS_ALPHA_ARG2_DIFFUSE          (0x00000000)
+#    define MGA_TDS_ALPHA_ARG2_FCOL             (0x01000000)
+#    define MGA_TDS_ALPHA_ARG2_PREVTEX          (0x02000000)
+#    define MGA_TDS_ALPHA_ARG2_PREVSTAGE        (0x03000000)
+#    define MGA_TDS_ALPHA_ARG2_INV              (0x04000000)
+#    define MGA_TDS_ALPHA_ADD                   (0x08000000)
+#    define MGA_TDS_ALPHA_ADDBIAS               (0x10000000)
+#    define MGA_TDS_ALPHA_ADD2X                 (0x20000000)
+#    define MGA_TDS_ALPHA_SEL_ARG1              (0x00000000)
+#    define MGA_TDS_ALPHA_SEL_ARG2              (0x40000000)
+#    define MGA_TDS_ALPHA_SEL_ADD               (0x80000000)
+#    define MGA_TDS_ALPHA_SEL_MUL               (0xc0000000)
+
+#define MGAREG_DWGSYNC		0x2c4c
+
+#define MGAREG_AGP_PLL		0x1e4c
+#define MGA_AGP2XPLL_ENABLE		0x1
+#define MGA_AGP2XPLL_DISABLE		0x0
+
+#endif
diff --git a/drivers/gpu/drm/mgag200/mgag200_ttm.c b/drivers/gpu/drm/mgag200/mgag200_ttm.c
new file mode 100644
index 00000000000..b223dcb7a71
--- /dev/null
+++ b/drivers/gpu/drm/mgag200/mgag200_ttm.c
@@ -0,0 +1,452 @@
+/*
+ * 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, 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: Dave Airlie <airlied@redhat.com>
+ */
+#include "drmP.h"
+#include "mgag200_drv.h"
+#include <ttm/ttm_page_alloc.h>
+
+static inline struct mga_device *
+mgag200_bdev(struct ttm_bo_device *bd)
+{
+	return container_of(bd, struct mga_device, ttm.bdev);
+}
+
+static int
+mgag200_ttm_mem_global_init(struct drm_global_reference *ref)
+{
+	return ttm_mem_global_init(ref->object);
+}
+
+static void
+mgag200_ttm_mem_global_release(struct drm_global_reference *ref)
+{
+	ttm_mem_global_release(ref->object);
+}
+
+static int mgag200_ttm_global_init(struct mga_device *ast)
+{
+	struct drm_global_reference *global_ref;
+	int r;
+
+	global_ref = &ast->ttm.mem_global_ref;
+	global_ref->global_type = DRM_GLOBAL_TTM_MEM;
+	global_ref->size = sizeof(struct ttm_mem_global);
+	global_ref->init = &mgag200_ttm_mem_global_init;
+	global_ref->release = &mgag200_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;
+	}
+
+	ast->ttm.bo_global_ref.mem_glob =
+		ast->ttm.mem_global_ref.object;
+	global_ref = &ast->ttm.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(&ast->ttm.mem_global_ref);
+		return r;
+	}
+	return 0;
+}
+
+void
+mgag200_ttm_global_release(struct mga_device *ast)
+{
+	if (ast->ttm.mem_global_ref.release == NULL)
+		return;
+
+	drm_global_item_unref(&ast->ttm.bo_global_ref.ref);
+	drm_global_item_unref(&ast->ttm.mem_global_ref);
+	ast->ttm.mem_global_ref.release = NULL;
+}
+
+
+static void mgag200_bo_ttm_destroy(struct ttm_buffer_object *tbo)
+{
+	struct mgag200_bo *bo;
+
+	bo = container_of(tbo, struct mgag200_bo, bo);
+
+	drm_gem_object_release(&bo->gem);
+	kfree(bo);
+}
+
+bool mgag200_ttm_bo_is_mgag200_bo(struct ttm_buffer_object *bo)
+{
+	if (bo->destroy == &mgag200_bo_ttm_destroy)
+		return true;
+	return false;
+}
+
+static int
+mgag200_bo_init_mem_type(struct ttm_bo_device *bdev, uint32_t type,
+		     struct ttm_mem_type_manager *man)
+{
+	switch (type) {
+	case TTM_PL_SYSTEM:
+		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:
+		man->func = &ttm_bo_manager_func;
+		man->flags = TTM_MEMTYPE_FLAG_FIXED |
+			TTM_MEMTYPE_FLAG_MAPPABLE;
+		man->available_caching = TTM_PL_FLAG_UNCACHED |
+			TTM_PL_FLAG_WC;
+		man->default_caching = TTM_PL_FLAG_WC;
+		break;
+	default:
+		DRM_ERROR("Unsupported memory type %u\n", (unsigned)type);
+		return -EINVAL;
+	}
+	return 0;
+}
+
+static void
+mgag200_bo_evict_flags(struct ttm_buffer_object *bo, struct ttm_placement *pl)
+{
+	struct mgag200_bo *mgabo = mgag200_bo(bo);
+
+	if (!mgag200_ttm_bo_is_mgag200_bo(bo))
+		return;
+
+	mgag200_ttm_placement(mgabo, TTM_PL_FLAG_SYSTEM);
+	*pl = mgabo->placement;
+}
+
+static int mgag200_bo_verify_access(struct ttm_buffer_object *bo, struct file *filp)
+{
+	return 0;
+}
+
+static int mgag200_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 mga_device *mdev = mgag200_bdev(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.offset = mem->start << PAGE_SHIFT;
+		mem->bus.base = pci_resource_start(mdev->dev->pdev, 0);
+		mem->bus.is_iomem = true;
+		break;
+	default:
+		return -EINVAL;
+		break;
+	}
+	return 0;
+}
+
+static void mgag200_ttm_io_mem_free(struct ttm_bo_device *bdev, struct ttm_mem_reg *mem)
+{
+}
+
+static int mgag200_bo_move(struct ttm_buffer_object *bo,
+		       bool evict, bool interruptible,
+		       bool no_wait_reserve, bool no_wait_gpu,
+		       struct ttm_mem_reg *new_mem)
+{
+	int r;
+	r = ttm_bo_move_memcpy(bo, evict, no_wait_reserve, no_wait_gpu, new_mem);
+	return r;
+}
+
+
+static void mgag200_ttm_backend_destroy(struct ttm_tt *tt)
+{
+	ttm_tt_fini(tt);
+	kfree(tt);
+}
+
+static struct ttm_backend_func mgag200_tt_backend_func = {
+	.destroy = &mgag200_ttm_backend_destroy,
+};
+
+
+struct ttm_tt *mgag200_ttm_tt_create(struct ttm_bo_device *bdev,
+				 unsigned long size, uint32_t page_flags,
+				 struct page *dummy_read_page)
+{
+	struct ttm_tt *tt;
+
+	tt = kzalloc(sizeof(struct ttm_tt), GFP_KERNEL);
+	if (tt == NULL)
+		return NULL;
+	tt->func = &mgag200_tt_backend_func;
+	if (ttm_tt_init(tt, bdev, size, page_flags, dummy_read_page)) {
+		kfree(tt);
+		return NULL;
+	}
+	return tt;
+}
+
+static int mgag200_ttm_tt_populate(struct ttm_tt *ttm)
+{
+	return ttm_pool_populate(ttm);
+}
+
+static void mgag200_ttm_tt_unpopulate(struct ttm_tt *ttm)
+{
+	ttm_pool_unpopulate(ttm);
+}
+
+struct ttm_bo_driver mgag200_bo_driver = {
+	.ttm_tt_create = mgag200_ttm_tt_create,
+	.ttm_tt_populate = mgag200_ttm_tt_populate,
+	.ttm_tt_unpopulate = mgag200_ttm_tt_unpopulate,
+	.init_mem_type = mgag200_bo_init_mem_type,
+	.evict_flags = mgag200_bo_evict_flags,
+	.move = mgag200_bo_move,
+	.verify_access = mgag200_bo_verify_access,
+	.io_mem_reserve = &mgag200_ttm_io_mem_reserve,
+	.io_mem_free = &mgag200_ttm_io_mem_free,
+};
+
+int mgag200_mm_init(struct mga_device *mdev)
+{
+	int ret;
+	struct drm_device *dev = mdev->dev;
+	struct ttm_bo_device *bdev = &mdev->ttm.bdev;
+
+	ret = mgag200_ttm_global_init(mdev);
+	if (ret)
+		return ret;
+
+	ret = ttm_bo_device_init(&mdev->ttm.bdev,
+				 mdev->ttm.bo_global_ref.ref.object,
+				 &mgag200_bo_driver, DRM_FILE_PAGE_OFFSET,
+				 true);
+	if (ret) {
+		DRM_ERROR("Error initialising bo driver; %d\n", ret);
+		return ret;
+	}
+
+	ret = ttm_bo_init_mm(bdev, TTM_PL_VRAM, mdev->mc.vram_size >> PAGE_SHIFT);
+	if (ret) {
+		DRM_ERROR("Failed ttm VRAM init: %d\n", ret);
+		return ret;
+	}
+
+	mdev->fb_mtrr = drm_mtrr_add(pci_resource_start(dev->pdev, 0),
+				    pci_resource_len(dev->pdev, 0),
+				    DRM_MTRR_WC);
+
+	return 0;
+}
+
+void mgag200_mm_fini(struct mga_device *mdev)
+{
+	struct drm_device *dev = mdev->dev;
+	ttm_bo_device_release(&mdev->ttm.bdev);
+
+	mgag200_ttm_global_release(mdev);
+
+	if (mdev->fb_mtrr >= 0) {
+		drm_mtrr_del(mdev->fb_mtrr,
+			     pci_resource_start(dev->pdev, 0),
+			     pci_resource_len(dev->pdev, 0), DRM_MTRR_WC);
+		mdev->fb_mtrr = -1;
+	}
+}
+
+void mgag200_ttm_placement(struct mgag200_bo *bo, int domain)
+{
+	u32 c = 0;
+	bo->placement.fpfn = 0;
+	bo->placement.lpfn = 0;
+	bo->placement.placement = bo->placements;
+	bo->placement.busy_placement = bo->placements;
+	if (domain & TTM_PL_FLAG_VRAM)
+		bo->placements[c++] = TTM_PL_FLAG_WC | TTM_PL_FLAG_UNCACHED | TTM_PL_FLAG_VRAM;
+	if (domain & TTM_PL_FLAG_SYSTEM)
+		bo->placements[c++] = TTM_PL_MASK_CACHING | TTM_PL_FLAG_SYSTEM;
+	if (!c)
+		bo->placements[c++] = TTM_PL_MASK_CACHING | TTM_PL_FLAG_SYSTEM;
+	bo->placement.num_placement = c;
+	bo->placement.num_busy_placement = c;
+}
+
+int mgag200_bo_reserve(struct mgag200_bo *bo, bool no_wait)
+{
+	int ret;
+
+	ret = ttm_bo_reserve(&bo->bo, true, no_wait, false, 0);
+	if (ret) {
+		if (ret != -ERESTARTSYS)
+			DRM_ERROR("reserve failed %p\n", bo);
+		return ret;
+	}
+	return 0;
+}
+
+void mgag200_bo_unreserve(struct mgag200_bo *bo)
+{
+	ttm_bo_unreserve(&bo->bo);
+}
+
+int mgag200_bo_create(struct drm_device *dev, int size, int align,
+		  uint32_t flags, struct mgag200_bo **pmgabo)
+{
+	struct mga_device *mdev = dev->dev_private;
+	struct mgag200_bo *mgabo;
+	size_t acc_size;
+	int ret;
+
+	mgabo = kzalloc(sizeof(struct mgag200_bo), GFP_KERNEL);
+	if (!mgabo)
+		return -ENOMEM;
+
+	ret = drm_gem_object_init(dev, &mgabo->gem, size);
+	if (ret) {
+		kfree(mgabo);
+		return ret;
+	}
+
+	mgabo->gem.driver_private = NULL;
+	mgabo->bo.bdev = &mdev->ttm.bdev;
+
+	mgag200_ttm_placement(mgabo, TTM_PL_FLAG_VRAM | TTM_PL_FLAG_SYSTEM);
+
+	acc_size = ttm_bo_dma_acc_size(&mdev->ttm.bdev, size,
+				       sizeof(struct mgag200_bo));
+
+	ret = ttm_bo_init(&mdev->ttm.bdev, &mgabo->bo, size,
+			  ttm_bo_type_device, &mgabo->placement,
+			  align >> PAGE_SHIFT, 0, false, NULL, acc_size,
+			  NULL, mgag200_bo_ttm_destroy);
+	if (ret)
+		return ret;
+
+	*pmgabo = mgabo;
+	return 0;
+}
+
+static inline u64 mgag200_bo_gpu_offset(struct mgag200_bo *bo)
+{
+	return bo->bo.offset;
+}
+
+int mgag200_bo_pin(struct mgag200_bo *bo, u32 pl_flag, u64 *gpu_addr)
+{
+	int i, ret;
+
+	if (bo->pin_count) {
+		bo->pin_count++;
+		if (gpu_addr)
+			*gpu_addr = mgag200_bo_gpu_offset(bo);
+	}
+
+	mgag200_ttm_placement(bo, pl_flag);
+	for (i = 0; i < bo->placement.num_placement; i++)
+		bo->placements[i] |= TTM_PL_FLAG_NO_EVICT;
+	ret = ttm_bo_validate(&bo->bo, &bo->placement, false, false, false);
+	if (ret)
+		return ret;
+
+	bo->pin_count = 1;
+	if (gpu_addr)
+		*gpu_addr = mgag200_bo_gpu_offset(bo);
+	return 0;
+}
+
+int mgag200_bo_unpin(struct mgag200_bo *bo)
+{
+	int i, ret;
+	if (!bo->pin_count) {
+		DRM_ERROR("unpin bad %p\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;
+	ret = ttm_bo_validate(&bo->bo, &bo->placement, false, false, false);
+	if (ret)
+		return ret;
+
+	return 0;
+}
+
+int mgag200_bo_push_sysram(struct mgag200_bo *bo)
+{
+	int i, ret;
+	if (!bo->pin_count) {
+		DRM_ERROR("unpin bad %p\n", bo);
+		return 0;
+	}
+	bo->pin_count--;
+	if (bo->pin_count)
+		return 0;
+
+	if (bo->kmap.virtual)
+		ttm_bo_kunmap(&bo->kmap);
+
+	mgag200_ttm_placement(bo, TTM_PL_FLAG_SYSTEM);
+	for (i = 0; i < bo->placement.num_placement ; i++)
+		bo->placements[i] |= TTM_PL_FLAG_NO_EVICT;
+
+	ret = ttm_bo_validate(&bo->bo, &bo->placement, false, false, false);
+	if (ret) {
+		DRM_ERROR("pushing to VRAM failed\n");
+		return ret;
+	}
+	return 0;
+}
+
+int mgag200_mmap(struct file *filp, struct vm_area_struct *vma)
+{
+	struct drm_file *file_priv;
+	struct mga_device *mdev;
+
+	if (unlikely(vma->vm_pgoff < DRM_FILE_PAGE_OFFSET))
+		return drm_mmap(filp, vma);
+
+	file_priv = filp->private_data;
+	mdev = file_priv->minor->dev->dev_private;
+	return ttm_bo_mmap(filp, vma, &mdev->ttm.bdev);
+}
diff --git a/drivers/gpu/drm/nouveau/Makefile b/drivers/gpu/drm/nouveau/Makefile
index 1a2ad7eb173..fe5267d06ab 100644
--- a/drivers/gpu/drm/nouveau/Makefile
+++ b/drivers/gpu/drm/nouveau/Makefile
@@ -16,10 +16,13 @@ nouveau-y := nouveau_drv.o nouveau_state.o nouveau_channel.o nouveau_mem.o \
              nv04_mc.o nv40_mc.o nv50_mc.o \
              nv04_fb.o nv10_fb.o nv20_fb.o nv30_fb.o nv40_fb.o \
              nv50_fb.o nvc0_fb.o \
-             nv04_fifo.o nv10_fifo.o nv40_fifo.o nv50_fifo.o nvc0_fifo.o \
+             nv04_fifo.o nv10_fifo.o nv17_fifo.o nv40_fifo.o nv50_fifo.o \
+             nv84_fifo.o nvc0_fifo.o nve0_fifo.o \
+             nv04_fence.o nv10_fence.o nv84_fence.o nvc0_fence.o \
+             nv04_software.o nv50_software.o nvc0_software.o \
              nv04_graph.o nv10_graph.o nv20_graph.o \
-             nv40_graph.o nv50_graph.o nvc0_graph.o \
-             nv40_grctx.o nv50_grctx.o nvc0_grctx.o \
+             nv40_graph.o nv50_graph.o nvc0_graph.o nve0_graph.o \
+             nv40_grctx.o nv50_grctx.o nvc0_grctx.o nve0_grctx.o \
              nv84_crypt.o nv98_crypt.o \
              nva3_copy.o nvc0_copy.o \
              nv31_mpeg.o nv50_mpeg.o \
@@ -37,7 +40,7 @@ nouveau-y := nouveau_drv.o nouveau_state.o nouveau_channel.o nouveau_mem.o \
 	     nv50_calc.o \
 	     nv04_pm.o nv40_pm.o nv50_pm.o nva3_pm.o nvc0_pm.o \
 	     nv50_vram.o nvc0_vram.o \
-	     nv50_vm.o nvc0_vm.o
+	     nv50_vm.o nvc0_vm.o nouveau_prime.o
 
 nouveau-$(CONFIG_DRM_NOUVEAU_DEBUG) += nouveau_debugfs.o
 nouveau-$(CONFIG_COMPAT) += nouveau_ioc32.o
diff --git a/drivers/gpu/drm/nouveau/nouveau_acpi.c b/drivers/gpu/drm/nouveau/nouveau_acpi.c
index 284bd25d5d2..fc841e87b34 100644
--- a/drivers/gpu/drm/nouveau/nouveau_acpi.c
+++ b/drivers/gpu/drm/nouveau/nouveau_acpi.c
@@ -338,7 +338,8 @@ void nouveau_switcheroo_optimus_dsm(void)
 
 void nouveau_unregister_dsm_handler(void)
 {
-	vga_switcheroo_unregister_handler();
+	if (nouveau_dsm_priv.optimus_detected || nouveau_dsm_priv.dsm_detected)
+		vga_switcheroo_unregister_handler();
 }
 
 /* retrieve the ROM in 4k blocks */
diff --git a/drivers/gpu/drm/nouveau/nouveau_bios.c b/drivers/gpu/drm/nouveau/nouveau_bios.c
index 0be4a815e70..2f11e16a81a 100644
--- a/drivers/gpu/drm/nouveau/nouveau_bios.c
+++ b/drivers/gpu/drm/nouveau/nouveau_bios.c
@@ -30,6 +30,7 @@
 #include "nouveau_gpio.h"
 
 #include <linux/io-mapping.h>
+#include <linux/firmware.h>
 
 /* these defines are made up */
 #define NV_CIO_CRE_44_HEADA 0x0
@@ -195,35 +196,24 @@ static void
 bios_shadow_acpi(struct nvbios *bios)
 {
 	struct pci_dev *pdev = bios->dev->pdev;
-	int ptr, len, ret;
-	u8 data[3];
+	int cnt = 65536 / ROM_BIOS_PAGE;
+	int ret;
 
 	if (!nouveau_acpi_rom_supported(pdev))
 		return;
 
-	ret = nouveau_acpi_get_bios_chunk(data, 0, sizeof(data));
-	if (ret != sizeof(data))
-		return;
-
-	bios->length = min(data[2] * 512, 65536);
-	bios->data = kmalloc(bios->length, GFP_KERNEL);
+	bios->data = kmalloc(cnt * ROM_BIOS_PAGE, GFP_KERNEL);
 	if (!bios->data)
 		return;
 
-	len = bios->length;
-	ptr = 0;
-	while (len) {
-		int size = (len > ROM_BIOS_PAGE) ? ROM_BIOS_PAGE : len;
-
-		ret = nouveau_acpi_get_bios_chunk(bios->data, ptr, size);
-		if (ret != size) {
-			kfree(bios->data);
-			bios->data = NULL;
+	bios->length = 0;
+	while (cnt--) {
+		ret = nouveau_acpi_get_bios_chunk(bios->data, bios->length,
+						  ROM_BIOS_PAGE);
+		if (ret != ROM_BIOS_PAGE)
 			return;
-		}
 
-		len -= size;
-		ptr += size;
+		bios->length += ROM_BIOS_PAGE;
 	}
 }
 
@@ -249,8 +239,12 @@ bios_shadow(struct drm_device *dev)
 	struct drm_nouveau_private *dev_priv = dev->dev_private;
 	struct nvbios *bios = &dev_priv->vbios;
 	struct methods *mthd, *best;
+	const struct firmware *fw;
+	char fname[32];
+	int ret;
 
 	if (nouveau_vbios) {
+		/* try to match one of the built-in methods */
 		mthd = shadow_methods;
 		do {
 			if (strcasecmp(nouveau_vbios, mthd->desc))
@@ -263,6 +257,22 @@ bios_shadow(struct drm_device *dev)
 				return true;
 		} while ((++mthd)->shadow);
 
+		/* attempt to load firmware image */
+		snprintf(fname, sizeof(fname), "nouveau/%s", nouveau_vbios);
+		ret = request_firmware(&fw, fname, &dev->pdev->dev);
+		if (ret == 0) {
+			bios->length = fw->size;
+			bios->data   = kmemdup(fw->data, fw->size, GFP_KERNEL);
+			release_firmware(fw);
+
+			NV_INFO(dev, "VBIOS image: %s\n", nouveau_vbios);
+			if (score_vbios(bios, 1))
+				return true;
+
+			kfree(bios->data);
+			bios->data = NULL;
+		}
+
 		NV_ERROR(dev, "VBIOS source \'%s\' invalid\n", nouveau_vbios);
 	}
 
@@ -273,6 +283,7 @@ bios_shadow(struct drm_device *dev)
 		mthd->score = score_vbios(bios, mthd->rw);
 		mthd->size = bios->length;
 		mthd->data = bios->data;
+		bios->data = NULL;
 	} while (mthd->score != 3 && (++mthd)->shadow);
 
 	mthd = shadow_methods;
diff --git a/drivers/gpu/drm/nouveau/nouveau_bo.c b/drivers/gpu/drm/nouveau/nouveau_bo.c
index 7d15a774f9c..7f80ed52356 100644
--- a/drivers/gpu/drm/nouveau/nouveau_bo.c
+++ b/drivers/gpu/drm/nouveau/nouveau_bo.c
@@ -35,6 +35,8 @@
 #include "nouveau_dma.h"
 #include "nouveau_mm.h"
 #include "nouveau_vm.h"
+#include "nouveau_fence.h"
+#include "nouveau_ramht.h"
 
 #include <linux/log2.h>
 #include <linux/slab.h>
@@ -89,12 +91,17 @@ nouveau_bo_fixup_align(struct nouveau_bo *nvbo, u32 flags,
 int
 nouveau_bo_new(struct drm_device *dev, int size, int align,
 	       uint32_t flags, uint32_t tile_mode, uint32_t tile_flags,
+	       struct sg_table *sg,
 	       struct nouveau_bo **pnvbo)
 {
 	struct drm_nouveau_private *dev_priv = dev->dev_private;
 	struct nouveau_bo *nvbo;
 	size_t acc_size;
 	int ret;
+	int type = ttm_bo_type_device;
+
+	if (sg)
+		type = ttm_bo_type_sg;
 
 	nvbo = kzalloc(sizeof(struct nouveau_bo), GFP_KERNEL);
 	if (!nvbo)
@@ -120,8 +127,8 @@ nouveau_bo_new(struct drm_device *dev, int size, int align,
 				       sizeof(struct nouveau_bo));
 
 	ret = ttm_bo_init(&dev_priv->ttm.bdev, &nvbo->bo, size,
-			  ttm_bo_type_device, &nvbo->placement,
-			  align >> PAGE_SHIFT, 0, false, NULL, acc_size,
+			  type, &nvbo->placement,
+			  align >> PAGE_SHIFT, 0, false, NULL, acc_size, sg,
 			  nouveau_bo_del_ttm);
 	if (ret) {
 		/* ttm will call nouveau_bo_del_ttm if it fails.. */
@@ -473,7 +480,7 @@ nouveau_bo_move_accel_cleanup(struct nouveau_channel *chan,
 	struct nouveau_fence *fence = NULL;
 	int ret;
 
-	ret = nouveau_fence_new(chan, &fence, true);
+	ret = nouveau_fence_new(chan, &fence);
 	if (ret)
 		return ret;
 
@@ -484,6 +491,76 @@ nouveau_bo_move_accel_cleanup(struct nouveau_channel *chan,
 }
 
 static int
+nve0_bo_move_copy(struct nouveau_channel *chan, struct ttm_buffer_object *bo,
+		  struct ttm_mem_reg *old_mem, struct ttm_mem_reg *new_mem)
+{
+	struct nouveau_mem *node = old_mem->mm_node;
+	int ret = RING_SPACE(chan, 10);
+	if (ret == 0) {
+		BEGIN_NVC0(chan, NvSubCopy, 0x0400, 8);
+		OUT_RING  (chan, upper_32_bits(node->vma[0].offset));
+		OUT_RING  (chan, lower_32_bits(node->vma[0].offset));
+		OUT_RING  (chan, upper_32_bits(node->vma[1].offset));
+		OUT_RING  (chan, lower_32_bits(node->vma[1].offset));
+		OUT_RING  (chan, PAGE_SIZE);
+		OUT_RING  (chan, PAGE_SIZE);
+		OUT_RING  (chan, PAGE_SIZE);
+		OUT_RING  (chan, new_mem->num_pages);
+		BEGIN_IMC0(chan, NvSubCopy, 0x0300, 0x0386);
+	}
+	return ret;
+}
+
+static int
+nvc0_bo_move_init(struct nouveau_channel *chan, u32 handle)
+{
+	int ret = RING_SPACE(chan, 2);
+	if (ret == 0) {
+		BEGIN_NVC0(chan, NvSubCopy, 0x0000, 1);
+		OUT_RING  (chan, handle);
+	}
+	return ret;
+}
+
+static int
+nvc0_bo_move_copy(struct nouveau_channel *chan, struct ttm_buffer_object *bo,
+		  struct ttm_mem_reg *old_mem, struct ttm_mem_reg *new_mem)
+{
+	struct nouveau_mem *node = old_mem->mm_node;
+	u64 src_offset = node->vma[0].offset;
+	u64 dst_offset = node->vma[1].offset;
+	u32 page_count = new_mem->num_pages;
+	int ret;
+
+	page_count = new_mem->num_pages;
+	while (page_count) {
+		int line_count = (page_count > 8191) ? 8191 : page_count;
+
+		ret = RING_SPACE(chan, 11);
+		if (ret)
+			return ret;
+
+		BEGIN_NVC0(chan, NvSubCopy, 0x030c, 8);
+		OUT_RING  (chan, upper_32_bits(src_offset));
+		OUT_RING  (chan, lower_32_bits(src_offset));
+		OUT_RING  (chan, upper_32_bits(dst_offset));
+		OUT_RING  (chan, lower_32_bits(dst_offset));
+		OUT_RING  (chan, PAGE_SIZE);
+		OUT_RING  (chan, PAGE_SIZE);
+		OUT_RING  (chan, PAGE_SIZE);
+		OUT_RING  (chan, line_count);
+		BEGIN_NVC0(chan, NvSubCopy, 0x0300, 1);
+		OUT_RING  (chan, 0x00000110);
+
+		page_count -= line_count;
+		src_offset += (PAGE_SIZE * line_count);
+		dst_offset += (PAGE_SIZE * line_count);
+	}
+
+	return 0;
+}
+
+static int
 nvc0_bo_move_m2mf(struct nouveau_channel *chan, struct ttm_buffer_object *bo,
 		  struct ttm_mem_reg *old_mem, struct ttm_mem_reg *new_mem)
 {
@@ -501,17 +578,17 @@ nvc0_bo_move_m2mf(struct nouveau_channel *chan, struct ttm_buffer_object *bo,
 		if (ret)
 			return ret;
 
-		BEGIN_NVC0(chan, 2, NvSubM2MF, 0x0238, 2);
+		BEGIN_NVC0(chan, NvSubCopy, 0x0238, 2);
 		OUT_RING  (chan, upper_32_bits(dst_offset));
 		OUT_RING  (chan, lower_32_bits(dst_offset));
-		BEGIN_NVC0(chan, 2, NvSubM2MF, 0x030c, 6);
+		BEGIN_NVC0(chan, NvSubCopy, 0x030c, 6);
 		OUT_RING  (chan, upper_32_bits(src_offset));
 		OUT_RING  (chan, lower_32_bits(src_offset));
 		OUT_RING  (chan, PAGE_SIZE); /* src_pitch */
 		OUT_RING  (chan, PAGE_SIZE); /* dst_pitch */
 		OUT_RING  (chan, PAGE_SIZE); /* line_length */
 		OUT_RING  (chan, line_count);
-		BEGIN_NVC0(chan, 2, NvSubM2MF, 0x0300, 1);
+		BEGIN_NVC0(chan, NvSubCopy, 0x0300, 1);
 		OUT_RING  (chan, 0x00100110);
 
 		page_count -= line_count;
@@ -523,6 +600,102 @@ nvc0_bo_move_m2mf(struct nouveau_channel *chan, struct ttm_buffer_object *bo,
 }
 
 static int
+nva3_bo_move_copy(struct nouveau_channel *chan, struct ttm_buffer_object *bo,
+		  struct ttm_mem_reg *old_mem, struct ttm_mem_reg *new_mem)
+{
+	struct nouveau_mem *node = old_mem->mm_node;
+	u64 src_offset = node->vma[0].offset;
+	u64 dst_offset = node->vma[1].offset;
+	u32 page_count = new_mem->num_pages;
+	int ret;
+
+	page_count = new_mem->num_pages;
+	while (page_count) {
+		int line_count = (page_count > 8191) ? 8191 : page_count;
+
+		ret = RING_SPACE(chan, 11);
+		if (ret)
+			return ret;
+
+		BEGIN_NV04(chan, NvSubCopy, 0x030c, 8);
+		OUT_RING  (chan, upper_32_bits(src_offset));
+		OUT_RING  (chan, lower_32_bits(src_offset));
+		OUT_RING  (chan, upper_32_bits(dst_offset));
+		OUT_RING  (chan, lower_32_bits(dst_offset));
+		OUT_RING  (chan, PAGE_SIZE);
+		OUT_RING  (chan, PAGE_SIZE);
+		OUT_RING  (chan, PAGE_SIZE);
+		OUT_RING  (chan, line_count);
+		BEGIN_NV04(chan, NvSubCopy, 0x0300, 1);
+		OUT_RING  (chan, 0x00000110);
+
+		page_count -= line_count;
+		src_offset += (PAGE_SIZE * line_count);
+		dst_offset += (PAGE_SIZE * line_count);
+	}
+
+	return 0;
+}
+
+static int
+nv98_bo_move_exec(struct nouveau_channel *chan, struct ttm_buffer_object *bo,
+		  struct ttm_mem_reg *old_mem, struct ttm_mem_reg *new_mem)
+{
+	struct nouveau_mem *node = old_mem->mm_node;
+	int ret = RING_SPACE(chan, 7);
+	if (ret == 0) {
+		BEGIN_NV04(chan, NvSubCopy, 0x0320, 6);
+		OUT_RING  (chan, upper_32_bits(node->vma[0].offset));
+		OUT_RING  (chan, lower_32_bits(node->vma[0].offset));
+		OUT_RING  (chan, upper_32_bits(node->vma[1].offset));
+		OUT_RING  (chan, lower_32_bits(node->vma[1].offset));
+		OUT_RING  (chan, 0x00000000 /* COPY */);
+		OUT_RING  (chan, new_mem->num_pages << PAGE_SHIFT);
+	}
+	return ret;
+}
+
+static int
+nv84_bo_move_exec(struct nouveau_channel *chan, struct ttm_buffer_object *bo,
+		  struct ttm_mem_reg *old_mem, struct ttm_mem_reg *new_mem)
+{
+	struct nouveau_mem *node = old_mem->mm_node;
+	int ret = RING_SPACE(chan, 7);
+	if (ret == 0) {
+		BEGIN_NV04(chan, NvSubCopy, 0x0304, 6);
+		OUT_RING  (chan, new_mem->num_pages << PAGE_SHIFT);
+		OUT_RING  (chan, upper_32_bits(node->vma[0].offset));
+		OUT_RING  (chan, lower_32_bits(node->vma[0].offset));
+		OUT_RING  (chan, upper_32_bits(node->vma[1].offset));
+		OUT_RING  (chan, lower_32_bits(node->vma[1].offset));
+		OUT_RING  (chan, 0x00000000 /* MODE_COPY, QUERY_NONE */);
+	}
+	return ret;
+}
+
+static int
+nv50_bo_move_init(struct nouveau_channel *chan, u32 handle)
+{
+	int ret = nouveau_notifier_alloc(chan, NvNotify0, 32, 0xfe0, 0x1000,
+					 &chan->m2mf_ntfy);
+	if (ret == 0) {
+		ret = RING_SPACE(chan, 6);
+		if (ret == 0) {
+			BEGIN_NV04(chan, NvSubCopy, 0x0000, 1);
+			OUT_RING  (chan, handle);
+			BEGIN_NV04(chan, NvSubCopy, 0x0180, 3);
+			OUT_RING  (chan, NvNotify0);
+			OUT_RING  (chan, NvDmaFB);
+			OUT_RING  (chan, NvDmaFB);
+		} else {
+			nouveau_ramht_remove(chan, NvNotify0);
+		}
+	}
+
+	return ret;
+}
+
+static int
 nv50_bo_move_m2mf(struct nouveau_channel *chan, struct ttm_buffer_object *bo,
 		  struct ttm_mem_reg *old_mem, struct ttm_mem_reg *new_mem)
 {
@@ -546,7 +719,7 @@ nv50_bo_move_m2mf(struct nouveau_channel *chan, struct ttm_buffer_object *bo,
 			if (ret)
 				return ret;
 
-			BEGIN_RING(chan, NvSubM2MF, 0x0200, 7);
+			BEGIN_NV04(chan, NvSubCopy, 0x0200, 7);
 			OUT_RING  (chan, 0);
 			OUT_RING  (chan, 0);
 			OUT_RING  (chan, stride);
@@ -559,7 +732,7 @@ nv50_bo_move_m2mf(struct nouveau_channel *chan, struct ttm_buffer_object *bo,
 			if (ret)
 				return ret;
 
-			BEGIN_RING(chan, NvSubM2MF, 0x0200, 1);
+			BEGIN_NV04(chan, NvSubCopy, 0x0200, 1);
 			OUT_RING  (chan, 1);
 		}
 		if (old_mem->mem_type == TTM_PL_VRAM &&
@@ -568,7 +741,7 @@ nv50_bo_move_m2mf(struct nouveau_channel *chan, struct ttm_buffer_object *bo,
 			if (ret)
 				return ret;
 
-			BEGIN_RING(chan, NvSubM2MF, 0x021c, 7);
+			BEGIN_NV04(chan, NvSubCopy, 0x021c, 7);
 			OUT_RING  (chan, 0);
 			OUT_RING  (chan, 0);
 			OUT_RING  (chan, stride);
@@ -581,7 +754,7 @@ nv50_bo_move_m2mf(struct nouveau_channel *chan, struct ttm_buffer_object *bo,
 			if (ret)
 				return ret;
 
-			BEGIN_RING(chan, NvSubM2MF, 0x021c, 1);
+			BEGIN_NV04(chan, NvSubCopy, 0x021c, 1);
 			OUT_RING  (chan, 1);
 		}
 
@@ -589,10 +762,10 @@ nv50_bo_move_m2mf(struct nouveau_channel *chan, struct ttm_buffer_object *bo,
 		if (ret)
 			return ret;
 
-		BEGIN_RING(chan, NvSubM2MF, 0x0238, 2);
+		BEGIN_NV04(chan, NvSubCopy, 0x0238, 2);
 		OUT_RING  (chan, upper_32_bits(src_offset));
 		OUT_RING  (chan, upper_32_bits(dst_offset));
-		BEGIN_RING(chan, NvSubM2MF, 0x030c, 8);
+		BEGIN_NV04(chan, NvSubCopy, 0x030c, 8);
 		OUT_RING  (chan, lower_32_bits(src_offset));
 		OUT_RING  (chan, lower_32_bits(dst_offset));
 		OUT_RING  (chan, stride);
@@ -601,7 +774,7 @@ nv50_bo_move_m2mf(struct nouveau_channel *chan, struct ttm_buffer_object *bo,
 		OUT_RING  (chan, height);
 		OUT_RING  (chan, 0x00000101);
 		OUT_RING  (chan, 0x00000000);
-		BEGIN_RING(chan, NvSubM2MF, NV_MEMORY_TO_MEMORY_FORMAT_NOP, 1);
+		BEGIN_NV04(chan, NvSubCopy, NV_MEMORY_TO_MEMORY_FORMAT_NOP, 1);
 		OUT_RING  (chan, 0);
 
 		length -= amount;
@@ -612,6 +785,24 @@ nv50_bo_move_m2mf(struct nouveau_channel *chan, struct ttm_buffer_object *bo,
 	return 0;
 }
 
+static int
+nv04_bo_move_init(struct nouveau_channel *chan, u32 handle)
+{
+	int ret = nouveau_notifier_alloc(chan, NvNotify0, 32, 0xfe0, 0x1000,
+					 &chan->m2mf_ntfy);
+	if (ret == 0) {
+		ret = RING_SPACE(chan, 4);
+		if (ret == 0) {
+			BEGIN_NV04(chan, NvSubCopy, 0x0000, 1);
+			OUT_RING  (chan, handle);
+			BEGIN_NV04(chan, NvSubCopy, 0x0180, 1);
+			OUT_RING  (chan, NvNotify0);
+		}
+	}
+
+	return ret;
+}
+
 static inline uint32_t
 nouveau_bo_mem_ctxdma(struct ttm_buffer_object *bo,
 		      struct nouveau_channel *chan, struct ttm_mem_reg *mem)
@@ -634,7 +825,7 @@ nv04_bo_move_m2mf(struct nouveau_channel *chan, struct ttm_buffer_object *bo,
 	if (ret)
 		return ret;
 
-	BEGIN_RING(chan, NvSubM2MF, NV_MEMORY_TO_MEMORY_FORMAT_DMA_SOURCE, 2);
+	BEGIN_NV04(chan, NvSubCopy, NV_MEMORY_TO_MEMORY_FORMAT_DMA_SOURCE, 2);
 	OUT_RING  (chan, nouveau_bo_mem_ctxdma(bo, chan, old_mem));
 	OUT_RING  (chan, nouveau_bo_mem_ctxdma(bo, chan, new_mem));
 
@@ -646,7 +837,7 @@ nv04_bo_move_m2mf(struct nouveau_channel *chan, struct ttm_buffer_object *bo,
 		if (ret)
 			return ret;
 
-		BEGIN_RING(chan, NvSubM2MF,
+		BEGIN_NV04(chan, NvSubCopy,
 				 NV_MEMORY_TO_MEMORY_FORMAT_OFFSET_IN, 8);
 		OUT_RING  (chan, src_offset);
 		OUT_RING  (chan, dst_offset);
@@ -656,7 +847,7 @@ nv04_bo_move_m2mf(struct nouveau_channel *chan, struct ttm_buffer_object *bo,
 		OUT_RING  (chan, line_count);
 		OUT_RING  (chan, 0x00000101);
 		OUT_RING  (chan, 0x00000000);
-		BEGIN_RING(chan, NvSubM2MF, NV_MEMORY_TO_MEMORY_FORMAT_NOP, 1);
+		BEGIN_NV04(chan, NvSubCopy, NV_MEMORY_TO_MEMORY_FORMAT_NOP, 1);
 		OUT_RING  (chan, 0);
 
 		page_count -= line_count;
@@ -716,13 +907,7 @@ nouveau_bo_move_m2mf(struct ttm_buffer_object *bo, int evict, bool intr,
 			goto out;
 	}
 
-	if (dev_priv->card_type < NV_50)
-		ret = nv04_bo_move_m2mf(chan, bo, &bo->mem, new_mem);
-	else
-	if (dev_priv->card_type < NV_C0)
-		ret = nv50_bo_move_m2mf(chan, bo, &bo->mem, new_mem);
-	else
-		ret = nvc0_bo_move_m2mf(chan, bo, &bo->mem, new_mem);
+	ret = dev_priv->ttm.move(chan, bo, &bo->mem, new_mem);
 	if (ret == 0) {
 		ret = nouveau_bo_move_accel_cleanup(chan, nvbo, evict,
 						    no_wait_reserve,
@@ -734,6 +919,49 @@ out:
 	return ret;
 }
 
+void
+nouveau_bo_move_init(struct nouveau_channel *chan)
+{
+	struct drm_nouveau_private *dev_priv = chan->dev->dev_private;
+	static const struct {
+		const char *name;
+		int engine;
+		u32 oclass;
+		int (*exec)(struct nouveau_channel *,
+			    struct ttm_buffer_object *,
+			    struct ttm_mem_reg *, struct ttm_mem_reg *);
+		int (*init)(struct nouveau_channel *, u32 handle);
+	} _methods[] = {
+		{  "COPY", 0, 0xa0b5, nve0_bo_move_copy, nvc0_bo_move_init },
+		{ "COPY1", 5, 0x90b8, nvc0_bo_move_copy, nvc0_bo_move_init },
+		{ "COPY0", 4, 0x90b5, nvc0_bo_move_copy, nvc0_bo_move_init },
+		{  "COPY", 0, 0x85b5, nva3_bo_move_copy, nv50_bo_move_init },
+		{ "CRYPT", 0, 0x74c1, nv84_bo_move_exec, nv50_bo_move_init },
+		{  "M2MF", 0, 0x9039, nvc0_bo_move_m2mf, nvc0_bo_move_init },
+		{  "M2MF", 0, 0x5039, nv50_bo_move_m2mf, nv50_bo_move_init },
+		{  "M2MF", 0, 0x0039, nv04_bo_move_m2mf, nv04_bo_move_init },
+		{},
+		{ "CRYPT", 0, 0x88b4, nv98_bo_move_exec, nv50_bo_move_init },
+	}, *mthd = _methods;
+	const char *name = "CPU";
+	int ret;
+
+	do {
+		u32 handle = (mthd->engine << 16) | mthd->oclass;
+		ret = nouveau_gpuobj_gr_new(chan, handle, mthd->oclass);
+		if (ret == 0) {
+			ret = mthd->init(chan, handle);
+			if (ret == 0) {
+				dev_priv->ttm.move = mthd->exec;
+				name = mthd->name;
+				break;
+			}
+		}
+	} while ((++mthd)->exec);
+
+	NV_INFO(chan->dev, "MM: using %s for buffer copies\n", name);
+}
+
 static int
 nouveau_bo_move_flipd(struct ttm_buffer_object *bo, bool evict, bool intr,
 		      bool no_wait_reserve, bool no_wait_gpu,
@@ -817,9 +1045,14 @@ nouveau_bo_move_ntfy(struct ttm_buffer_object *bo, struct ttm_mem_reg *new_mem)
 		} else
 		if (new_mem && new_mem->mem_type == TTM_PL_TT &&
 		    nvbo->page_shift == vma->vm->spg_shift) {
-			nouveau_vm_map_sg(vma, 0, new_mem->
-					  num_pages << PAGE_SHIFT,
-					  new_mem->mm_node);
+			if (((struct nouveau_mem *)new_mem->mm_node)->sg)
+				nouveau_vm_map_sg_table(vma, 0, new_mem->
+						  num_pages << PAGE_SHIFT,
+						  new_mem->mm_node);
+			else
+				nouveau_vm_map_sg(vma, 0, new_mem->
+						  num_pages << PAGE_SHIFT,
+						  new_mem->mm_node);
 		} else {
 			nouveau_vm_unmap(vma);
 		}
@@ -885,8 +1118,8 @@ nouveau_bo_move(struct ttm_buffer_object *bo, bool evict, bool intr,
 		goto out;
 	}
 
-	/* Software copy if the card isn't up and running yet. */
-	if (!dev_priv->channel) {
+	/* CPU copy if we have no accelerated method available */
+	if (!dev_priv->ttm.move) {
 		ret = ttm_bo_move_memcpy(bo, evict, no_wait_reserve, no_wait_gpu, new_mem);
 		goto out;
 	}
@@ -1030,26 +1263,10 @@ nouveau_ttm_fault_reserve_notify(struct ttm_buffer_object *bo)
 
 	nvbo->placement.fpfn = 0;
 	nvbo->placement.lpfn = dev_priv->fb_mappable_pages;
-	nouveau_bo_placement_set(nvbo, TTM_PL_VRAM, 0);
+	nouveau_bo_placement_set(nvbo, TTM_PL_FLAG_VRAM, 0);
 	return nouveau_bo_validate(nvbo, false, true, false);
 }
 
-void
-nouveau_bo_fence(struct nouveau_bo *nvbo, struct nouveau_fence *fence)
-{
-	struct nouveau_fence *old_fence;
-
-	if (likely(fence))
-		nouveau_fence_ref(fence);
-
-	spin_lock(&nvbo->bo.bdev->fence_lock);
-	old_fence = nvbo->bo.sync_obj;
-	nvbo->bo.sync_obj = fence;
-	spin_unlock(&nvbo->bo.bdev->fence_lock);
-
-	nouveau_fence_unref(&old_fence);
-}
-
 static int
 nouveau_ttm_tt_populate(struct ttm_tt *ttm)
 {
@@ -1058,10 +1275,19 @@ nouveau_ttm_tt_populate(struct ttm_tt *ttm)
 	struct drm_device *dev;
 	unsigned i;
 	int r;
+	bool slave = !!(ttm->page_flags & TTM_PAGE_FLAG_SG);
 
 	if (ttm->state != tt_unpopulated)
 		return 0;
 
+	if (slave && ttm->sg) {
+		/* make userspace faulting work */
+		drm_prime_sg_to_page_addr_arrays(ttm->sg, ttm->pages,
+						 ttm_dma->dma_address, ttm->num_pages);
+		ttm->state = tt_unbound;
+		return 0;
+	}
+
 	dev_priv = nouveau_bdev(ttm->bdev);
 	dev = dev_priv->dev;
 
@@ -1106,6 +1332,10 @@ nouveau_ttm_tt_unpopulate(struct ttm_tt *ttm)
 	struct drm_nouveau_private *dev_priv;
 	struct drm_device *dev;
 	unsigned i;
+	bool slave = !!(ttm->page_flags & TTM_PAGE_FLAG_SG);
+
+	if (slave)
+		return;
 
 	dev_priv = nouveau_bdev(ttm->bdev);
 	dev = dev_priv->dev;
@@ -1134,6 +1364,52 @@ nouveau_ttm_tt_unpopulate(struct ttm_tt *ttm)
 	ttm_pool_unpopulate(ttm);
 }
 
+void
+nouveau_bo_fence(struct nouveau_bo *nvbo, struct nouveau_fence *fence)
+{
+	struct nouveau_fence *old_fence = NULL;
+
+	if (likely(fence))
+		nouveau_fence_ref(fence);
+
+	spin_lock(&nvbo->bo.bdev->fence_lock);
+	old_fence = nvbo->bo.sync_obj;
+	nvbo->bo.sync_obj = fence;
+	spin_unlock(&nvbo->bo.bdev->fence_lock);
+
+	nouveau_fence_unref(&old_fence);
+}
+
+static void
+nouveau_bo_fence_unref(void **sync_obj)
+{
+	nouveau_fence_unref((struct nouveau_fence **)sync_obj);
+}
+
+static void *
+nouveau_bo_fence_ref(void *sync_obj)
+{
+	return nouveau_fence_ref(sync_obj);
+}
+
+static bool
+nouveau_bo_fence_signalled(void *sync_obj, void *sync_arg)
+{
+	return nouveau_fence_done(sync_obj);
+}
+
+static int
+nouveau_bo_fence_wait(void *sync_obj, void *sync_arg, bool lazy, bool intr)
+{
+	return nouveau_fence_wait(sync_obj, lazy, intr);
+}
+
+static int
+nouveau_bo_fence_flush(void *sync_obj, void *sync_arg)
+{
+	return 0;
+}
+
 struct ttm_bo_driver nouveau_bo_driver = {
 	.ttm_tt_create = &nouveau_ttm_tt_create,
 	.ttm_tt_populate = &nouveau_ttm_tt_populate,
@@ -1144,11 +1420,11 @@ struct ttm_bo_driver nouveau_bo_driver = {
 	.move_notify = nouveau_bo_move_ntfy,
 	.move = nouveau_bo_move,
 	.verify_access = nouveau_bo_verify_access,
-	.sync_obj_signaled = __nouveau_fence_signalled,
-	.sync_obj_wait = __nouveau_fence_wait,
-	.sync_obj_flush = __nouveau_fence_flush,
-	.sync_obj_unref = __nouveau_fence_unref,
-	.sync_obj_ref = __nouveau_fence_ref,
+	.sync_obj_signaled = nouveau_bo_fence_signalled,
+	.sync_obj_wait = nouveau_bo_fence_wait,
+	.sync_obj_flush = nouveau_bo_fence_flush,
+	.sync_obj_unref = nouveau_bo_fence_unref,
+	.sync_obj_ref = nouveau_bo_fence_ref,
 	.fault_reserve_notify = &nouveau_ttm_fault_reserve_notify,
 	.io_mem_reserve = &nouveau_ttm_io_mem_reserve,
 	.io_mem_free = &nouveau_ttm_io_mem_free,
@@ -1181,9 +1457,12 @@ nouveau_bo_vma_add(struct nouveau_bo *nvbo, struct nouveau_vm *vm,
 
 	if (nvbo->bo.mem.mem_type == TTM_PL_VRAM)
 		nouveau_vm_map(vma, nvbo->bo.mem.mm_node);
-	else
-	if (nvbo->bo.mem.mem_type == TTM_PL_TT)
-		nouveau_vm_map_sg(vma, 0, size, node);
+	else if (nvbo->bo.mem.mem_type == TTM_PL_TT) {
+		if (node->sg)
+			nouveau_vm_map_sg_table(vma, 0, size, node);
+		else
+			nouveau_vm_map_sg(vma, 0, size, node);
+	}
 
 	list_add_tail(&vma->head, &nvbo->vma_list);
 	vma->refcount = 1;
diff --git a/drivers/gpu/drm/nouveau/nouveau_channel.c b/drivers/gpu/drm/nouveau/nouveau_channel.c
index 846afb0bfef..629d8a2df5b 100644
--- a/drivers/gpu/drm/nouveau/nouveau_channel.c
+++ b/drivers/gpu/drm/nouveau/nouveau_channel.c
@@ -27,7 +27,10 @@
 #include "nouveau_drv.h"
 #include "nouveau_drm.h"
 #include "nouveau_dma.h"
+#include "nouveau_fifo.h"
 #include "nouveau_ramht.h"
+#include "nouveau_fence.h"
+#include "nouveau_software.h"
 
 static int
 nouveau_channel_pushbuf_init(struct nouveau_channel *chan)
@@ -38,7 +41,7 @@ nouveau_channel_pushbuf_init(struct nouveau_channel *chan)
 	int ret;
 
 	/* allocate buffer object */
-	ret = nouveau_bo_new(dev, 65536, 0, mem, 0, 0, &chan->pushbuf_bo);
+	ret = nouveau_bo_new(dev, 65536, 0, mem, 0, 0, NULL, &chan->pushbuf_bo);
 	if (ret)
 		goto out;
 
@@ -117,8 +120,9 @@ nouveau_channel_alloc(struct drm_device *dev, struct nouveau_channel **chan_ret,
 		      struct drm_file *file_priv,
 		      uint32_t vram_handle, uint32_t gart_handle)
 {
+	struct nouveau_exec_engine *fence = nv_engine(dev, NVOBJ_ENGINE_FENCE);
+	struct nouveau_fifo_priv *pfifo = nv_engine(dev, NVOBJ_ENGINE_FIFO);
 	struct drm_nouveau_private *dev_priv = dev->dev_private;
-	struct nouveau_fifo_engine *pfifo = &dev_priv->engine.fifo;
 	struct nouveau_fpriv *fpriv = nouveau_fpriv(file_priv);
 	struct nouveau_channel *chan;
 	unsigned long flags;
@@ -155,10 +159,6 @@ nouveau_channel_alloc(struct drm_device *dev, struct nouveau_channel **chan_ret,
 	}
 
 	NV_DEBUG(dev, "initialising channel %d\n", chan->id);
-	INIT_LIST_HEAD(&chan->nvsw.vbl_wait);
-	INIT_LIST_HEAD(&chan->nvsw.flip);
-	INIT_LIST_HEAD(&chan->fence.pending);
-	spin_lock_init(&chan->fence.lock);
 
 	/* setup channel's memory and vm */
 	ret = nouveau_gpuobj_channel_init(chan, vram_handle, gart_handle);
@@ -188,20 +188,15 @@ nouveau_channel_alloc(struct drm_device *dev, struct nouveau_channel **chan_ret,
 	chan->user_put = 0x40;
 	chan->user_get = 0x44;
 	if (dev_priv->card_type >= NV_50)
-                chan->user_get_hi = 0x60;
+		chan->user_get_hi = 0x60;
 
-	/* disable the fifo caches */
-	pfifo->reassign(dev, false);
-
-	/* Construct initial RAMFC for new channel */
-	ret = pfifo->create_context(chan);
+	/* create fifo context */
+	ret = pfifo->base.context_new(chan, NVOBJ_ENGINE_FIFO);
 	if (ret) {
 		nouveau_channel_put(&chan);
 		return ret;
 	}
 
-	pfifo->reassign(dev, true);
-
 	/* Insert NOPs for NOUVEAU_DMA_SKIPS */
 	ret = RING_SPACE(chan, NOUVEAU_DMA_SKIPS);
 	if (ret) {
@@ -211,9 +206,28 @@ nouveau_channel_alloc(struct drm_device *dev, struct nouveau_channel **chan_ret,
 
 	for (i = 0; i < NOUVEAU_DMA_SKIPS; i++)
 		OUT_RING  (chan, 0x00000000);
+
+	ret = nouveau_gpuobj_gr_new(chan, NvSw, nouveau_software_class(dev));
+	if (ret) {
+		nouveau_channel_put(&chan);
+		return ret;
+	}
+
+	if (dev_priv->card_type < NV_C0) {
+		ret = RING_SPACE(chan, 2);
+		if (ret) {
+			nouveau_channel_put(&chan);
+			return ret;
+		}
+
+		BEGIN_NV04(chan, NvSubSw, NV01_SUBCHAN_OBJECT, 1);
+		OUT_RING  (chan, NvSw);
+		FIRE_RING (chan);
+	}
+
 	FIRE_RING(chan);
 
-	ret = nouveau_fence_channel_init(chan);
+	ret = fence->context_new(chan, NVOBJ_ENGINE_FENCE);
 	if (ret) {
 		nouveau_channel_put(&chan);
 		return ret;
@@ -268,7 +282,6 @@ nouveau_channel_put_unlocked(struct nouveau_channel **pchan)
 	struct nouveau_channel *chan = *pchan;
 	struct drm_device *dev = chan->dev;
 	struct drm_nouveau_private *dev_priv = dev->dev_private;
-	struct nouveau_fifo_engine *pfifo = &dev_priv->engine.fifo;
 	unsigned long flags;
 	int i;
 
@@ -285,24 +298,12 @@ nouveau_channel_put_unlocked(struct nouveau_channel **pchan)
 	/* give it chance to idle */
 	nouveau_channel_idle(chan);
 
-	/* ensure all outstanding fences are signaled.  they should be if the
-	 * above attempts at idling were OK, but if we failed this'll tell TTM
-	 * we're done with the buffers.
-	 */
-	nouveau_fence_channel_fini(chan);
-
-	/* boot it off the hardware */
-	pfifo->reassign(dev, false);
-
 	/* destroy the engine specific contexts */
-	pfifo->destroy_context(chan);
-	for (i = 0; i < NVOBJ_ENGINE_NR; i++) {
+	for (i = NVOBJ_ENGINE_NR - 1; i >= 0; i--) {
 		if (chan->engctx[i])
 			dev_priv->eng[i]->context_del(chan, i);
 	}
 
-	pfifo->reassign(dev, true);
-
 	/* aside from its resources, the channel should now be dead,
 	 * remove it from the channel list
 	 */
@@ -354,38 +355,37 @@ nouveau_channel_ref(struct nouveau_channel *chan,
 	*pchan = chan;
 }
 
-void
+int
 nouveau_channel_idle(struct nouveau_channel *chan)
 {
 	struct drm_device *dev = chan->dev;
 	struct nouveau_fence *fence = NULL;
 	int ret;
 
-	nouveau_fence_update(chan);
-
-	if (chan->fence.sequence != chan->fence.sequence_ack) {
-		ret = nouveau_fence_new(chan, &fence, true);
-		if (!ret) {
-			ret = nouveau_fence_wait(fence, false, false);
-			nouveau_fence_unref(&fence);
-		}
-
-		if (ret)
-			NV_ERROR(dev, "Failed to idle channel %d.\n", chan->id);
+	ret = nouveau_fence_new(chan, &fence);
+	if (!ret) {
+		ret = nouveau_fence_wait(fence, false, false);
+		nouveau_fence_unref(&fence);
 	}
+
+	if (ret)
+		NV_ERROR(dev, "Failed to idle channel %d.\n", chan->id);
+	return ret;
 }
 
 /* cleans up all the fifos from file_priv */
 void
 nouveau_channel_cleanup(struct drm_device *dev, struct drm_file *file_priv)
 {
-	struct drm_nouveau_private *dev_priv = dev->dev_private;
-	struct nouveau_engine *engine = &dev_priv->engine;
+	struct nouveau_fifo_priv *pfifo = nv_engine(dev, NVOBJ_ENGINE_FIFO);
 	struct nouveau_channel *chan;
 	int i;
 
+	if (!pfifo)
+		return;
+
 	NV_DEBUG(dev, "clearing FIFO enables from file_priv\n");
-	for (i = 0; i < engine->fifo.channels; i++) {
+	for (i = 0; i < pfifo->channels; i++) {
 		chan = nouveau_channel_get(file_priv, i);
 		if (IS_ERR(chan))
 			continue;
diff --git a/drivers/gpu/drm/nouveau/nouveau_connector.c b/drivers/gpu/drm/nouveau/nouveau_connector.c
index fa860358add..7b11edb077d 100644
--- a/drivers/gpu/drm/nouveau/nouveau_connector.c
+++ b/drivers/gpu/drm/nouveau/nouveau_connector.c
@@ -654,7 +654,13 @@ nouveau_connector_detect_depth(struct drm_connector *connector)
 	if (nv_connector->edid && connector->display_info.bpc)
 		return;
 
-	/* if not, we're out of options unless we're LVDS, default to 8bpc */
+	/* EDID 1.4 is *supposed* to be supported on eDP, but, Apple... */
+	if (nv_connector->type == DCB_CONNECTOR_eDP) {
+		connector->display_info.bpc = 6;
+		return;
+	}
+
+	/* we're out of options unless we're LVDS, default to 8bpc */
 	if (nv_encoder->dcb->type != OUTPUT_LVDS) {
 		connector->display_info.bpc = 8;
 		return;
diff --git a/drivers/gpu/drm/nouveau/nouveau_debugfs.c b/drivers/gpu/drm/nouveau/nouveau_debugfs.c
index fa2ec491f6a..188c92b327e 100644
--- a/drivers/gpu/drm/nouveau/nouveau_debugfs.c
+++ b/drivers/gpu/drm/nouveau/nouveau_debugfs.c
@@ -67,8 +67,6 @@ nouveau_debugfs_channel_info(struct seq_file *m, void *data)
 			   nvchan_rd32(chan, 0x8c));
 	}
 
-	seq_printf(m, "last fence    : %d\n", chan->fence.sequence);
-	seq_printf(m, "last signalled: %d\n", chan->fence.sequence_ack);
 	return 0;
 }
 
diff --git a/drivers/gpu/drm/nouveau/nouveau_display.c b/drivers/gpu/drm/nouveau/nouveau_display.c
index a85e112863d..69688ef5cf4 100644
--- a/drivers/gpu/drm/nouveau/nouveau_display.c
+++ b/drivers/gpu/drm/nouveau/nouveau_display.c
@@ -33,7 +33,9 @@
 #include "nouveau_crtc.h"
 #include "nouveau_dma.h"
 #include "nouveau_connector.h"
+#include "nouveau_software.h"
 #include "nouveau_gpio.h"
+#include "nouveau_fence.h"
 #include "nv50_display.h"
 
 static void
@@ -300,7 +302,7 @@ nouveau_display_create(struct drm_device *dev)
 		disp->color_vibrance_property->values[1] = 200; /* -100..+100 */
 	}
 
-	dev->mode_config.funcs = (void *)&nouveau_mode_config_funcs;
+	dev->mode_config.funcs = &nouveau_mode_config_funcs;
 	dev->mode_config.fb_base = pci_resource_start(dev->pdev, 1);
 
 	dev->mode_config.min_width = 0;
@@ -325,14 +327,21 @@ nouveau_display_create(struct drm_device *dev)
 
 	ret = disp->create(dev);
 	if (ret)
-		return ret;
+		goto disp_create_err;
 
 	if (dev->mode_config.num_crtc) {
 		ret = drm_vblank_init(dev, dev->mode_config.num_crtc);
 		if (ret)
-			return ret;
+			goto vblank_err;
 	}
 
+	return 0;
+
+vblank_err:
+	disp->destroy(dev);
+disp_create_err:
+	drm_kms_helper_poll_fini(dev);
+	drm_mode_config_cleanup(dev);
 	return ret;
 }
 
@@ -425,6 +434,7 @@ nouveau_page_flip_emit(struct nouveau_channel *chan,
 		       struct nouveau_page_flip_state *s,
 		       struct nouveau_fence **pfence)
 {
+	struct nouveau_software_chan *swch = chan->engctx[NVOBJ_ENGINE_SW];
 	struct drm_nouveau_private *dev_priv = chan->dev->dev_private;
 	struct drm_device *dev = chan->dev;
 	unsigned long flags;
@@ -432,7 +442,7 @@ nouveau_page_flip_emit(struct nouveau_channel *chan,
 
 	/* Queue it to the pending list */
 	spin_lock_irqsave(&dev->event_lock, flags);
-	list_add_tail(&s->head, &chan->nvsw.flip);
+	list_add_tail(&s->head, &swch->flip);
 	spin_unlock_irqrestore(&dev->event_lock, flags);
 
 	/* Synchronize with the old framebuffer */
@@ -446,17 +456,17 @@ nouveau_page_flip_emit(struct nouveau_channel *chan,
 		goto fail;
 
 	if (dev_priv->card_type < NV_C0) {
-		BEGIN_RING(chan, NvSubSw, NV_SW_PAGE_FLIP, 1);
+		BEGIN_NV04(chan, NvSubSw, NV_SW_PAGE_FLIP, 1);
 		OUT_RING  (chan, 0x00000000);
 		OUT_RING  (chan, 0x00000000);
 	} else {
-		BEGIN_NVC0(chan, 2, 0, NV10_SUBCHAN_REF_CNT, 1);
-		OUT_RING  (chan, ++chan->fence.sequence);
-		BEGIN_NVC0(chan, 8, 0, NVSW_SUBCHAN_PAGE_FLIP, 0x0000);
+		BEGIN_NVC0(chan, 0, NV10_SUBCHAN_REF_CNT, 1);
+		OUT_RING  (chan, 0);
+		BEGIN_IMC0(chan, 0, NVSW_SUBCHAN_PAGE_FLIP, 0x0000);
 	}
 	FIRE_RING (chan);
 
-	ret = nouveau_fence_new(chan, pfence, true);
+	ret = nouveau_fence_new(chan, pfence);
 	if (ret)
 		goto fail;
 
@@ -477,7 +487,7 @@ nouveau_crtc_page_flip(struct drm_crtc *crtc, struct drm_framebuffer *fb,
 	struct nouveau_bo *old_bo = nouveau_framebuffer(crtc->fb)->nvbo;
 	struct nouveau_bo *new_bo = nouveau_framebuffer(fb)->nvbo;
 	struct nouveau_page_flip_state *s;
-	struct nouveau_channel *chan;
+	struct nouveau_channel *chan = NULL;
 	struct nouveau_fence *fence;
 	int ret;
 
@@ -500,7 +510,9 @@ nouveau_crtc_page_flip(struct drm_crtc *crtc, struct drm_framebuffer *fb,
 		  new_bo->bo.offset };
 
 	/* Choose the channel the flip will be handled in */
-	chan = nouveau_fence_channel(new_bo->bo.sync_obj);
+	fence = new_bo->bo.sync_obj;
+	if (fence)
+		chan = nouveau_channel_get_unlocked(fence->channel);
 	if (!chan)
 		chan = nouveau_channel_get_unlocked(dev_priv->channel);
 	mutex_lock(&chan->mutex);
@@ -540,20 +552,20 @@ int
 nouveau_finish_page_flip(struct nouveau_channel *chan,
 			 struct nouveau_page_flip_state *ps)
 {
+	struct nouveau_software_chan *swch = chan->engctx[NVOBJ_ENGINE_SW];
 	struct drm_device *dev = chan->dev;
 	struct nouveau_page_flip_state *s;
 	unsigned long flags;
 
 	spin_lock_irqsave(&dev->event_lock, flags);
 
-	if (list_empty(&chan->nvsw.flip)) {
+	if (list_empty(&swch->flip)) {
 		NV_ERROR(dev, "Unexpected pageflip in channel %d.\n", chan->id);
 		spin_unlock_irqrestore(&dev->event_lock, flags);
 		return -EINVAL;
 	}
 
-	s = list_first_entry(&chan->nvsw.flip,
-			     struct nouveau_page_flip_state, head);
+	s = list_first_entry(&swch->flip, struct nouveau_page_flip_state, head);
 	if (s->event) {
 		struct drm_pending_vblank_event *e = s->event;
 		struct timeval now;
diff --git a/drivers/gpu/drm/nouveau/nouveau_dma.h b/drivers/gpu/drm/nouveau/nouveau_dma.h
index 23d4edf992b..8db68be9544 100644
--- a/drivers/gpu/drm/nouveau/nouveau_dma.h
+++ b/drivers/gpu/drm/nouveau/nouveau_dma.h
@@ -48,12 +48,12 @@ void nv50_dma_push(struct nouveau_channel *, struct nouveau_bo *,
 
 /* Hardcoded object assignments to subchannels (subchannel id). */
 enum {
-	NvSubM2MF	= 0,
+	NvSubCtxSurf2D  = 0,
 	NvSubSw		= 1,
-	NvSub2D		= 2,
-	NvSubCtxSurf2D  = 2,
+	NvSubImageBlit  = 2,
+	NvSub2D		= 3,
 	NvSubGdiRect    = 3,
-	NvSubImageBlit  = 4
+	NvSubCopy	= 4,
 };
 
 /* Object handles. */
@@ -73,6 +73,7 @@ enum {
 	NvSema		= 0x8000000f,
 	NvEvoSema0	= 0x80000010,
 	NvEvoSema1	= 0x80000011,
+	NvNotify1       = 0x80000012,
 
 	/* G80+ display objects */
 	NvEvoVRAM	= 0x01000000,
@@ -127,15 +128,33 @@ extern void
 OUT_RINGp(struct nouveau_channel *chan, const void *data, unsigned nr_dwords);
 
 static inline void
-BEGIN_NVC0(struct nouveau_channel *chan, int op, int subc, int mthd, int size)
+BEGIN_NV04(struct nouveau_channel *chan, int subc, int mthd, int size)
 {
-	OUT_RING(chan, (op << 28) | (size << 16) | (subc << 13) | (mthd >> 2));
+	OUT_RING(chan, 0x00000000 | (subc << 13) | (size << 18) | mthd);
 }
 
 static inline void
-BEGIN_RING(struct nouveau_channel *chan, int subc, int mthd, int size)
+BEGIN_NI04(struct nouveau_channel *chan, int subc, int mthd, int size)
 {
-	OUT_RING(chan, (subc << 13) | (size << 18) | mthd);
+	OUT_RING(chan, 0x40000000 | (subc << 13) | (size << 18) | mthd);
+}
+
+static inline void
+BEGIN_NVC0(struct nouveau_channel *chan, int subc, int mthd, int size)
+{
+	OUT_RING(chan, 0x20000000 | (size << 16) | (subc << 13) | (mthd >> 2));
+}
+
+static inline void
+BEGIN_NIC0(struct nouveau_channel *chan, int subc, int mthd, int size)
+{
+	OUT_RING(chan, 0x60000000 | (size << 16) | (subc << 13) | (mthd >> 2));
+}
+
+static inline void
+BEGIN_IMC0(struct nouveau_channel *chan, int subc, int mthd, u16 data)
+{
+	OUT_RING(chan, 0x80000000 | (data << 16) | (subc << 13) | (mthd >> 2));
 }
 
 #define WRITE_PUT(val) do {                                                    \
diff --git a/drivers/gpu/drm/nouveau/nouveau_dp.c b/drivers/gpu/drm/nouveau/nouveau_dp.c
index d996134b1b2..7e289d2ad8e 100644
--- a/drivers/gpu/drm/nouveau/nouveau_dp.c
+++ b/drivers/gpu/drm/nouveau/nouveau_dp.c
@@ -510,6 +510,25 @@ nouveau_dp_dpms(struct drm_encoder *encoder, int mode, u32 datarate,
 		nouveau_dp_link_train(encoder, datarate, func);
 }
 
+static void
+nouveau_dp_probe_oui(struct drm_device *dev, struct nouveau_i2c_chan *auxch,
+		     u8 *dpcd)
+{
+	u8 buf[3];
+
+	if (!(dpcd[DP_DOWN_STREAM_PORT_COUNT] & DP_OUI_SUPPORT))
+		return;
+
+	if (!auxch_tx(dev, auxch->drive, 9, DP_SINK_OUI, buf, 3))
+		NV_DEBUG_KMS(dev, "Sink OUI: %02hx%02hx%02hx\n",
+			     buf[0], buf[1], buf[2]);
+
+	if (!auxch_tx(dev, auxch->drive, 9, DP_BRANCH_OUI, buf, 3))
+		NV_DEBUG_KMS(dev, "Branch OUI: %02hx%02hx%02hx\n",
+			     buf[0], buf[1], buf[2]);
+
+}
+
 bool
 nouveau_dp_detect(struct drm_encoder *encoder)
 {
@@ -544,6 +563,8 @@ nouveau_dp_detect(struct drm_encoder *encoder)
 	NV_DEBUG_KMS(dev, "maximum: %dx%d\n",
 		     nv_encoder->dp.link_nr, nv_encoder->dp.link_bw);
 
+	nouveau_dp_probe_oui(dev, auxch, dpcd);
+
 	return true;
 }
 
diff --git a/drivers/gpu/drm/nouveau/nouveau_drv.c b/drivers/gpu/drm/nouveau/nouveau_drv.c
index 4f2030bd567..cad254c8e38 100644
--- a/drivers/gpu/drm/nouveau/nouveau_drv.c
+++ b/drivers/gpu/drm/nouveau/nouveau_drv.c
@@ -33,6 +33,7 @@
 #include "nouveau_fb.h"
 #include "nouveau_fbcon.h"
 #include "nouveau_pm.h"
+#include "nouveau_fifo.h"
 #include "nv50_display.h"
 
 #include "drm_pciids.h"
@@ -175,7 +176,7 @@ nouveau_pci_suspend(struct pci_dev *pdev, pm_message_t pm_state)
 	struct drm_device *dev = pci_get_drvdata(pdev);
 	struct drm_nouveau_private *dev_priv = dev->dev_private;
 	struct nouveau_instmem_engine *pinstmem = &dev_priv->engine.instmem;
-	struct nouveau_fifo_engine *pfifo = &dev_priv->engine.fifo;
+	struct nouveau_fifo_priv *pfifo = nv_engine(dev, NVOBJ_ENGINE_FIFO);
 	struct nouveau_channel *chan;
 	struct drm_crtc *crtc;
 	int ret, i, e;
@@ -214,17 +215,13 @@ nouveau_pci_suspend(struct pci_dev *pdev, pm_message_t pm_state)
 	ttm_bo_evict_mm(&dev_priv->ttm.bdev, TTM_PL_VRAM);
 
 	NV_INFO(dev, "Idling channels...\n");
-	for (i = 0; i < pfifo->channels; i++) {
+	for (i = 0; i < (pfifo ? pfifo->channels : 0); i++) {
 		chan = dev_priv->channels.ptr[i];
 
 		if (chan && chan->pushbuf_bo)
 			nouveau_channel_idle(chan);
 	}
 
-	pfifo->reassign(dev, false);
-	pfifo->disable(dev);
-	pfifo->unload_context(dev);
-
 	for (e = NVOBJ_ENGINE_NR - 1; e >= 0; e--) {
 		if (!dev_priv->eng[e])
 			continue;
@@ -265,8 +262,6 @@ out_abort:
 		if (dev_priv->eng[e])
 			dev_priv->eng[e]->init(dev, e);
 	}
-	pfifo->enable(dev);
-	pfifo->reassign(dev, true);
 	return ret;
 }
 
@@ -274,6 +269,7 @@ int
 nouveau_pci_resume(struct pci_dev *pdev)
 {
 	struct drm_device *dev = pci_get_drvdata(pdev);
+	struct nouveau_fifo_priv *pfifo = nv_engine(dev, NVOBJ_ENGINE_FIFO);
 	struct drm_nouveau_private *dev_priv = dev->dev_private;
 	struct nouveau_engine *engine = &dev_priv->engine;
 	struct drm_crtc *crtc;
@@ -321,7 +317,6 @@ nouveau_pci_resume(struct pci_dev *pdev)
 		if (dev_priv->eng[i])
 			dev_priv->eng[i]->init(dev, i);
 	}
-	engine->fifo.init(dev);
 
 	nouveau_irq_postinstall(dev);
 
@@ -330,7 +325,7 @@ nouveau_pci_resume(struct pci_dev *pdev)
 		struct nouveau_channel *chan;
 		int j;
 
-		for (i = 0; i < dev_priv->engine.fifo.channels; i++) {
+		for (i = 0; i < (pfifo ? pfifo->channels : 0); i++) {
 			chan = dev_priv->channels.ptr[i];
 			if (!chan || !chan->pushbuf_bo)
 				continue;
@@ -408,7 +403,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_MODESET | DRIVER_PRIME,
 	.load = nouveau_load,
 	.firstopen = nouveau_firstopen,
 	.lastclose = nouveau_lastclose,
@@ -430,6 +425,12 @@ static struct drm_driver driver = {
 	.reclaim_buffers = drm_core_reclaim_buffers,
 	.ioctls = nouveau_ioctls,
 	.fops = &nouveau_driver_fops,
+
+	.prime_handle_to_fd = drm_gem_prime_handle_to_fd,
+	.prime_fd_to_handle = drm_gem_prime_fd_to_handle,
+	.gem_prime_export = nouveau_gem_prime_export,
+	.gem_prime_import = nouveau_gem_prime_import,
+
 	.gem_init_object = nouveau_gem_object_new,
 	.gem_free_object = nouveau_gem_object_del,
 	.gem_open_object = nouveau_gem_object_open,
diff --git a/drivers/gpu/drm/nouveau/nouveau_drv.h b/drivers/gpu/drm/nouveau/nouveau_drv.h
index 3aef353a926..634d222c93d 100644
--- a/drivers/gpu/drm/nouveau/nouveau_drv.h
+++ b/drivers/gpu/drm/nouveau/nouveau_drv.h
@@ -70,7 +70,7 @@ struct nouveau_mem;
 
 #define MAX_NUM_DCB_ENTRIES 16
 
-#define NOUVEAU_MAX_CHANNEL_NR 128
+#define NOUVEAU_MAX_CHANNEL_NR 4096
 #define NOUVEAU_MAX_TILE_NR 15
 
 struct nouveau_mem {
@@ -86,6 +86,7 @@ struct nouveau_mem {
 	u32 memtype;
 	u64 offset;
 	u64 size;
+	struct sg_table *sg;
 };
 
 struct nouveau_tile_reg {
@@ -164,8 +165,10 @@ enum nouveau_flags {
 #define NVOBJ_ENGINE_PPP	NVOBJ_ENGINE_MPEG
 #define NVOBJ_ENGINE_BSP	6
 #define NVOBJ_ENGINE_VP		7
-#define NVOBJ_ENGINE_DISPLAY	15
+#define NVOBJ_ENGINE_FIFO	14
+#define NVOBJ_ENGINE_FENCE	15
 #define NVOBJ_ENGINE_NR		16
+#define NVOBJ_ENGINE_DISPLAY	(NVOBJ_ENGINE_NR + 0) /*XXX*/
 
 #define NVOBJ_FLAG_DONT_MAP             (1 << 0)
 #define NVOBJ_FLAG_ZERO_ALLOC		(1 << 1)
@@ -233,17 +236,6 @@ struct nouveau_channel {
 	uint32_t user_get_hi;
 	uint32_t user_put;
 
-	/* Fencing */
-	struct {
-		/* lock protects the pending list only */
-		spinlock_t lock;
-		struct list_head pending;
-		uint32_t sequence;
-		uint32_t sequence_ack;
-		atomic_t last_sequence_irq;
-		struct nouveau_vma vma;
-	} fence;
-
 	/* DMA push buffer */
 	struct nouveau_gpuobj *pushbuf;
 	struct nouveau_bo     *pushbuf_bo;
@@ -257,8 +249,6 @@ struct nouveau_channel {
 
 	/* PFIFO context */
 	struct nouveau_gpuobj *ramfc;
-	struct nouveau_gpuobj *cache;
-	void *fifo_priv;
 
 	/* Execution engine contexts */
 	void *engctx[NVOBJ_ENGINE_NR];
@@ -292,18 +282,6 @@ struct nouveau_channel {
 		int ib_put;
 	} dma;
 
-	uint32_t sw_subchannel[8];
-
-	struct nouveau_vma dispc_vma[4];
-	struct {
-		struct nouveau_gpuobj *vblsem;
-		uint32_t vblsem_head;
-		uint32_t vblsem_offset;
-		uint32_t vblsem_rval;
-		struct list_head vbl_wait;
-		struct list_head flip;
-	} nvsw;
-
 	struct {
 		bool active;
 		char name[32];
@@ -366,30 +344,6 @@ struct nouveau_fb_engine {
 	void (*free_tile_region)(struct drm_device *dev, int i);
 };
 
-struct nouveau_fifo_engine {
-	void *priv;
-	int  channels;
-
-	struct nouveau_gpuobj *playlist[2];
-	int cur_playlist;
-
-	int  (*init)(struct drm_device *);
-	void (*takedown)(struct drm_device *);
-
-	void (*disable)(struct drm_device *);
-	void (*enable)(struct drm_device *);
-	bool (*reassign)(struct drm_device *, bool enable);
-	bool (*cache_pull)(struct drm_device *dev, bool enable);
-
-	int  (*channel_id)(struct drm_device *);
-
-	int  (*create_context)(struct nouveau_channel *);
-	void (*destroy_context)(struct nouveau_channel *);
-	int  (*load_context)(struct nouveau_channel *);
-	int  (*unload_context)(struct drm_device *);
-	void (*tlb_flush)(struct drm_device *dev);
-};
-
 struct nouveau_display_engine {
 	void *priv;
 	int (*early_init)(struct drm_device *);
@@ -597,7 +551,6 @@ struct nouveau_engine {
 	struct nouveau_mc_engine      mc;
 	struct nouveau_timer_engine   timer;
 	struct nouveau_fb_engine      fb;
-	struct nouveau_fifo_engine    fifo;
 	struct nouveau_display_engine display;
 	struct nouveau_gpio_engine    gpio;
 	struct nouveau_pm_engine      pm;
@@ -740,6 +693,9 @@ struct drm_nouveau_private {
 		struct ttm_bo_global_ref bo_global_ref;
 		struct ttm_bo_device bdev;
 		atomic_t validate_sequence;
+		int (*move)(struct nouveau_channel *,
+			    struct ttm_buffer_object *,
+			    struct ttm_mem_reg *, struct ttm_mem_reg *);
 	} ttm;
 
 	struct {
@@ -977,7 +933,7 @@ extern void nouveau_channel_put_unlocked(struct nouveau_channel **);
 extern void nouveau_channel_put(struct nouveau_channel **);
 extern void nouveau_channel_ref(struct nouveau_channel *chan,
 				struct nouveau_channel **pchan);
-extern void nouveau_channel_idle(struct nouveau_channel *chan);
+extern int  nouveau_channel_idle(struct nouveau_channel *chan);
 
 /* nouveau_object.c */
 #define NVOBJ_ENGINE_ADD(d, e, p) do {                                         \
@@ -1209,56 +1165,6 @@ extern void nv50_fb_vm_trap(struct drm_device *, int display);
 extern int  nvc0_fb_init(struct drm_device *);
 extern void nvc0_fb_takedown(struct drm_device *);
 
-/* nv04_fifo.c */
-extern int  nv04_fifo_init(struct drm_device *);
-extern void nv04_fifo_fini(struct drm_device *);
-extern void nv04_fifo_disable(struct drm_device *);
-extern void nv04_fifo_enable(struct drm_device *);
-extern bool nv04_fifo_reassign(struct drm_device *, bool);
-extern bool nv04_fifo_cache_pull(struct drm_device *, bool);
-extern int  nv04_fifo_channel_id(struct drm_device *);
-extern int  nv04_fifo_create_context(struct nouveau_channel *);
-extern void nv04_fifo_destroy_context(struct nouveau_channel *);
-extern int  nv04_fifo_load_context(struct nouveau_channel *);
-extern int  nv04_fifo_unload_context(struct drm_device *);
-extern void nv04_fifo_isr(struct drm_device *);
-
-/* nv10_fifo.c */
-extern int  nv10_fifo_init(struct drm_device *);
-extern int  nv10_fifo_channel_id(struct drm_device *);
-extern int  nv10_fifo_create_context(struct nouveau_channel *);
-extern int  nv10_fifo_load_context(struct nouveau_channel *);
-extern int  nv10_fifo_unload_context(struct drm_device *);
-
-/* nv40_fifo.c */
-extern int  nv40_fifo_init(struct drm_device *);
-extern int  nv40_fifo_create_context(struct nouveau_channel *);
-extern int  nv40_fifo_load_context(struct nouveau_channel *);
-extern int  nv40_fifo_unload_context(struct drm_device *);
-
-/* nv50_fifo.c */
-extern int  nv50_fifo_init(struct drm_device *);
-extern void nv50_fifo_takedown(struct drm_device *);
-extern int  nv50_fifo_channel_id(struct drm_device *);
-extern int  nv50_fifo_create_context(struct nouveau_channel *);
-extern void nv50_fifo_destroy_context(struct nouveau_channel *);
-extern int  nv50_fifo_load_context(struct nouveau_channel *);
-extern int  nv50_fifo_unload_context(struct drm_device *);
-extern void nv50_fifo_tlb_flush(struct drm_device *dev);
-
-/* nvc0_fifo.c */
-extern int  nvc0_fifo_init(struct drm_device *);
-extern void nvc0_fifo_takedown(struct drm_device *);
-extern void nvc0_fifo_disable(struct drm_device *);
-extern void nvc0_fifo_enable(struct drm_device *);
-extern bool nvc0_fifo_reassign(struct drm_device *, bool);
-extern bool nvc0_fifo_cache_pull(struct drm_device *, bool);
-extern int  nvc0_fifo_channel_id(struct drm_device *);
-extern int  nvc0_fifo_create_context(struct nouveau_channel *);
-extern void nvc0_fifo_destroy_context(struct nouveau_channel *);
-extern int  nvc0_fifo_load_context(struct nouveau_channel *);
-extern int  nvc0_fifo_unload_context(struct drm_device *);
-
 /* nv04_graph.c */
 extern int  nv04_graph_create(struct drm_device *);
 extern int  nv04_graph_object_new(struct nouveau_channel *, int, u32, u16);
@@ -1277,18 +1183,23 @@ extern int  nv20_graph_create(struct drm_device *);
 
 /* nv40_graph.c */
 extern int  nv40_graph_create(struct drm_device *);
-extern void nv40_grctx_init(struct nouveau_grctx *);
+extern void nv40_grctx_init(struct drm_device *, u32 *size);
+extern void nv40_grctx_fill(struct drm_device *, struct nouveau_gpuobj *);
 
 /* nv50_graph.c */
 extern int  nv50_graph_create(struct drm_device *);
-extern int  nv50_grctx_init(struct nouveau_grctx *);
 extern struct nouveau_enum nv50_data_error_names[];
 extern int  nv50_graph_isr_chid(struct drm_device *dev, u64 inst);
+extern int  nv50_grctx_init(struct drm_device *, u32 *, u32, u32 *, u32 *);
+extern void nv50_grctx_fill(struct drm_device *, struct nouveau_gpuobj *);
 
 /* nvc0_graph.c */
 extern int  nvc0_graph_create(struct drm_device *);
 extern int  nvc0_graph_isr_chid(struct drm_device *dev, u64 inst);
 
+/* nve0_graph.c */
+extern int  nve0_graph_create(struct drm_device *);
+
 /* nv84_crypt.c */
 extern int  nv84_crypt_create(struct drm_device *);
 
@@ -1414,9 +1325,12 @@ extern int nv04_crtc_create(struct drm_device *, int index);
 
 /* nouveau_bo.c */
 extern struct ttm_bo_driver nouveau_bo_driver;
+extern void nouveau_bo_move_init(struct nouveau_channel *);
 extern int nouveau_bo_new(struct drm_device *, int size, int align,
 			  uint32_t flags, uint32_t tile_mode,
-			  uint32_t tile_flags, struct nouveau_bo **);
+			  uint32_t tile_flags,
+			  struct sg_table *sg,
+			  struct nouveau_bo **);
 extern int nouveau_bo_pin(struct nouveau_bo *, uint32_t flags);
 extern int nouveau_bo_unpin(struct nouveau_bo *);
 extern int nouveau_bo_map(struct nouveau_bo *);
@@ -1437,50 +1351,6 @@ extern int  nouveau_bo_vma_add(struct nouveau_bo *, struct nouveau_vm *,
 			       struct nouveau_vma *);
 extern void nouveau_bo_vma_del(struct nouveau_bo *, struct nouveau_vma *);
 
-/* nouveau_fence.c */
-struct nouveau_fence;
-extern int nouveau_fence_init(struct drm_device *);
-extern void nouveau_fence_fini(struct drm_device *);
-extern int nouveau_fence_channel_init(struct nouveau_channel *);
-extern void nouveau_fence_channel_fini(struct nouveau_channel *);
-extern void nouveau_fence_update(struct nouveau_channel *);
-extern int nouveau_fence_new(struct nouveau_channel *, struct nouveau_fence **,
-			     bool emit);
-extern int nouveau_fence_emit(struct nouveau_fence *);
-extern void nouveau_fence_work(struct nouveau_fence *fence,
-			       void (*work)(void *priv, bool signalled),
-			       void *priv);
-struct nouveau_channel *nouveau_fence_channel(struct nouveau_fence *);
-
-extern bool __nouveau_fence_signalled(void *obj, void *arg);
-extern int __nouveau_fence_wait(void *obj, void *arg, bool lazy, bool intr);
-extern int __nouveau_fence_flush(void *obj, void *arg);
-extern void __nouveau_fence_unref(void **obj);
-extern void *__nouveau_fence_ref(void *obj);
-
-static inline bool nouveau_fence_signalled(struct nouveau_fence *obj)
-{
-	return __nouveau_fence_signalled(obj, NULL);
-}
-static inline int
-nouveau_fence_wait(struct nouveau_fence *obj, bool lazy, bool intr)
-{
-	return __nouveau_fence_wait(obj, NULL, lazy, intr);
-}
-extern int nouveau_fence_sync(struct nouveau_fence *, struct nouveau_channel *);
-static inline int nouveau_fence_flush(struct nouveau_fence *obj)
-{
-	return __nouveau_fence_flush(obj, NULL);
-}
-static inline void nouveau_fence_unref(struct nouveau_fence **obj)
-{
-	__nouveau_fence_unref((void **)obj);
-}
-static inline struct nouveau_fence *nouveau_fence_ref(struct nouveau_fence *obj)
-{
-	return __nouveau_fence_ref(obj);
-}
-
 /* nouveau_gem.c */
 extern int nouveau_gem_new(struct drm_device *, int size, int align,
 			   uint32_t domain, uint32_t tile_mode,
@@ -1501,6 +1371,11 @@ extern int nouveau_gem_ioctl_cpu_fini(struct drm_device *, void *,
 extern int nouveau_gem_ioctl_info(struct drm_device *, void *,
 				  struct drm_file *);
 
+extern struct dma_buf *nouveau_gem_prime_export(struct drm_device *dev,
+				struct drm_gem_object *obj, int flags);
+extern struct drm_gem_object *nouveau_gem_prime_import(struct drm_device *dev,
+				struct dma_buf *dma_buf);
+
 /* nouveau_display.c */
 int nouveau_display_create(struct drm_device *dev);
 void nouveau_display_destroy(struct drm_device *dev);
@@ -1772,6 +1647,7 @@ nv44_graph_class(struct drm_device *dev)
 #define NV84_SUBCHAN_SEMAPHORE_TRIGGER_ACQUIRE_EQUAL                 0x00000001
 #define NV84_SUBCHAN_SEMAPHORE_TRIGGER_WRITE_LONG                    0x00000002
 #define NV84_SUBCHAN_SEMAPHORE_TRIGGER_ACQUIRE_GEQUAL                0x00000004
+#define NVC0_SUBCHAN_SEMAPHORE_TRIGGER_YIELD                         0x00001000
 #define NV84_SUBCHAN_NOTIFY_INTR                                     0x00000020
 #define NV84_SUBCHAN_WRCACHE_FLUSH                                   0x00000024
 #define NV10_SUBCHAN_REF_CNT                                         0x00000050
diff --git a/drivers/gpu/drm/nouveau/nouveau_fbcon.c b/drivers/gpu/drm/nouveau/nouveau_fbcon.c
index 8113e9201ed..153b9a15469 100644
--- a/drivers/gpu/drm/nouveau/nouveau_fbcon.c
+++ b/drivers/gpu/drm/nouveau/nouveau_fbcon.c
@@ -153,7 +153,7 @@ nouveau_fbcon_sync(struct fb_info *info)
 	struct drm_device *dev = nfbdev->dev;
 	struct drm_nouveau_private *dev_priv = dev->dev_private;
 	struct nouveau_channel *chan = dev_priv->channel;
-	int ret, i;
+	int ret;
 
 	if (!chan || !chan->accel_done || in_interrupt() ||
 	    info->state != FBINFO_STATE_RUNNING ||
@@ -163,38 +163,8 @@ nouveau_fbcon_sync(struct fb_info *info)
 	if (!mutex_trylock(&chan->mutex))
 		return 0;
 
-	ret = RING_SPACE(chan, 4);
-	if (ret) {
-		mutex_unlock(&chan->mutex);
-		nouveau_fbcon_gpu_lockup(info);
-		return 0;
-	}
-
-	if (dev_priv->card_type >= NV_C0) {
-		BEGIN_NVC0(chan, 2, NvSub2D, 0x010c, 1);
-		OUT_RING  (chan, 0);
-		BEGIN_NVC0(chan, 2, NvSub2D, 0x0100, 1);
-		OUT_RING  (chan, 0);
-	} else {
-		BEGIN_RING(chan, 0, 0x0104, 1);
-		OUT_RING  (chan, 0);
-		BEGIN_RING(chan, 0, 0x0100, 1);
-		OUT_RING  (chan, 0);
-	}
-
-	nouveau_bo_wr32(chan->notifier_bo, chan->m2mf_ntfy/4 + 3, 0xffffffff);
-	FIRE_RING(chan);
+	ret = nouveau_channel_idle(chan);
 	mutex_unlock(&chan->mutex);
-
-	ret = -EBUSY;
-	for (i = 0; i < 100000; i++) {
-		if (!nouveau_bo_rd32(chan->notifier_bo, chan->m2mf_ntfy/4 + 3)) {
-			ret = 0;
-			break;
-		}
-		DRM_UDELAY(1);
-	}
-
 	if (ret) {
 		nouveau_fbcon_gpu_lockup(info);
 		return 0;
diff --git a/drivers/gpu/drm/nouveau/nouveau_fence.c b/drivers/gpu/drm/nouveau/nouveau_fence.c
index c1dc20f6cb8..3c180493dab 100644
--- a/drivers/gpu/drm/nouveau/nouveau_fence.c
+++ b/drivers/gpu/drm/nouveau/nouveau_fence.c
@@ -32,220 +32,100 @@
 
 #include "nouveau_drv.h"
 #include "nouveau_ramht.h"
+#include "nouveau_fence.h"
+#include "nouveau_software.h"
 #include "nouveau_dma.h"
 
-#define USE_REFCNT(dev) (nouveau_private(dev)->chipset >= 0x10)
-#define USE_SEMA(dev) (nouveau_private(dev)->chipset >= 0x17)
-
-struct nouveau_fence {
-	struct nouveau_channel *channel;
-	struct kref refcount;
-	struct list_head entry;
-
-	uint32_t sequence;
-	bool signalled;
-
-	void (*work)(void *priv, bool signalled);
-	void *priv;
-};
-
-struct nouveau_semaphore {
-	struct kref ref;
-	struct drm_device *dev;
-	struct drm_mm_node *mem;
-};
-
-static inline struct nouveau_fence *
-nouveau_fence(void *sync_obj)
+void
+nouveau_fence_context_del(struct nouveau_fence_chan *fctx)
 {
-	return (struct nouveau_fence *)sync_obj;
+	struct nouveau_fence *fence, *fnext;
+	spin_lock(&fctx->lock);
+	list_for_each_entry_safe(fence, fnext, &fctx->pending, head) {
+		if (fence->work)
+			fence->work(fence->priv, false);
+		fence->channel = NULL;
+		list_del(&fence->head);
+		nouveau_fence_unref(&fence);
+	}
+	spin_unlock(&fctx->lock);
 }
 
-static void
-nouveau_fence_del(struct kref *ref)
+void
+nouveau_fence_context_new(struct nouveau_fence_chan *fctx)
 {
-	struct nouveau_fence *fence =
-		container_of(ref, struct nouveau_fence, refcount);
-
-	nouveau_channel_ref(NULL, &fence->channel);
-	kfree(fence);
+	INIT_LIST_HEAD(&fctx->pending);
+	spin_lock_init(&fctx->lock);
 }
 
 void
 nouveau_fence_update(struct nouveau_channel *chan)
 {
 	struct drm_device *dev = chan->dev;
-	struct nouveau_fence *tmp, *fence;
-	uint32_t sequence;
+	struct nouveau_fence_priv *priv = nv_engine(dev, NVOBJ_ENGINE_FENCE);
+	struct nouveau_fence_chan *fctx = chan->engctx[NVOBJ_ENGINE_FENCE];
+	struct nouveau_fence *fence, *fnext;
 
-	spin_lock(&chan->fence.lock);
-
-	/* Fetch the last sequence if the channel is still up and running */
-	if (likely(!list_empty(&chan->fence.pending))) {
-		if (USE_REFCNT(dev))
-			sequence = nvchan_rd32(chan, 0x48);
-		else
-			sequence = atomic_read(&chan->fence.last_sequence_irq);
-
-		if (chan->fence.sequence_ack == sequence)
-			goto out;
-		chan->fence.sequence_ack = sequence;
-	}
-
-	list_for_each_entry_safe(fence, tmp, &chan->fence.pending, entry) {
-		if (fence->sequence > chan->fence.sequence_ack)
+	spin_lock(&fctx->lock);
+	list_for_each_entry_safe(fence, fnext, &fctx->pending, head) {
+		if (priv->read(chan) < fence->sequence)
 			break;
 
-		fence->signalled = true;
-		list_del(&fence->entry);
 		if (fence->work)
 			fence->work(fence->priv, true);
-
-		kref_put(&fence->refcount, nouveau_fence_del);
-	}
-
-out:
-	spin_unlock(&chan->fence.lock);
-}
-
-int
-nouveau_fence_new(struct nouveau_channel *chan, struct nouveau_fence **pfence,
-		  bool emit)
-{
-	struct nouveau_fence *fence;
-	int ret = 0;
-
-	fence = kzalloc(sizeof(*fence), GFP_KERNEL);
-	if (!fence)
-		return -ENOMEM;
-	kref_init(&fence->refcount);
-	nouveau_channel_ref(chan, &fence->channel);
-
-	if (emit)
-		ret = nouveau_fence_emit(fence);
-
-	if (ret)
+		fence->channel = NULL;
+		list_del(&fence->head);
 		nouveau_fence_unref(&fence);
-	*pfence = fence;
-	return ret;
-}
-
-struct nouveau_channel *
-nouveau_fence_channel(struct nouveau_fence *fence)
-{
-	return fence ? nouveau_channel_get_unlocked(fence->channel) : NULL;
+	}
+	spin_unlock(&fctx->lock);
 }
 
 int
-nouveau_fence_emit(struct nouveau_fence *fence)
+nouveau_fence_emit(struct nouveau_fence *fence, struct nouveau_channel *chan)
 {
-	struct nouveau_channel *chan = fence->channel;
 	struct drm_device *dev = chan->dev;
-	struct drm_nouveau_private *dev_priv = dev->dev_private;
+	struct nouveau_fence_priv *priv = nv_engine(dev, NVOBJ_ENGINE_FENCE);
+	struct nouveau_fence_chan *fctx = chan->engctx[NVOBJ_ENGINE_FENCE];
 	int ret;
 
-	ret = RING_SPACE(chan, 2);
-	if (ret)
-		return ret;
-
-	if (unlikely(chan->fence.sequence == chan->fence.sequence_ack - 1)) {
-		nouveau_fence_update(chan);
+	fence->channel  = chan;
+	fence->timeout  = jiffies + (3 * DRM_HZ);
+	fence->sequence = ++fctx->sequence;
 
-		BUG_ON(chan->fence.sequence ==
-		       chan->fence.sequence_ack - 1);
+	ret = priv->emit(fence);
+	if (!ret) {
+		kref_get(&fence->kref);
+		spin_lock(&fctx->lock);
+		list_add_tail(&fence->head, &fctx->pending);
+		spin_unlock(&fctx->lock);
 	}
 
-	fence->sequence = ++chan->fence.sequence;
-
-	kref_get(&fence->refcount);
-	spin_lock(&chan->fence.lock);
-	list_add_tail(&fence->entry, &chan->fence.pending);
-	spin_unlock(&chan->fence.lock);
-
-	if (USE_REFCNT(dev)) {
-		if (dev_priv->card_type < NV_C0)
-			BEGIN_RING(chan, 0, NV10_SUBCHAN_REF_CNT, 1);
-		else
-			BEGIN_NVC0(chan, 2, 0, NV10_SUBCHAN_REF_CNT, 1);
-	} else {
-		BEGIN_RING(chan, NvSubSw, 0x0150, 1);
-	}
-	OUT_RING (chan, fence->sequence);
-	FIRE_RING(chan);
-
-	return 0;
-}
-
-void
-nouveau_fence_work(struct nouveau_fence *fence,
-		   void (*work)(void *priv, bool signalled),
-		   void *priv)
-{
-	BUG_ON(fence->work);
-
-	spin_lock(&fence->channel->fence.lock);
-
-	if (fence->signalled) {
-		work(priv, true);
-	} else {
-		fence->work = work;
-		fence->priv = priv;
-	}
-
-	spin_unlock(&fence->channel->fence.lock);
-}
-
-void
-__nouveau_fence_unref(void **sync_obj)
-{
-	struct nouveau_fence *fence = nouveau_fence(*sync_obj);
-
-	if (fence)
-		kref_put(&fence->refcount, nouveau_fence_del);
-	*sync_obj = NULL;
-}
-
-void *
-__nouveau_fence_ref(void *sync_obj)
-{
-	struct nouveau_fence *fence = nouveau_fence(sync_obj);
-
-	kref_get(&fence->refcount);
-	return sync_obj;
+	return ret;
 }
 
 bool
-__nouveau_fence_signalled(void *sync_obj, void *sync_arg)
+nouveau_fence_done(struct nouveau_fence *fence)
 {
-	struct nouveau_fence *fence = nouveau_fence(sync_obj);
-	struct nouveau_channel *chan = fence->channel;
-
-	if (fence->signalled)
-		return true;
-
-	nouveau_fence_update(chan);
-	return fence->signalled;
+	if (fence->channel)
+		nouveau_fence_update(fence->channel);
+	return !fence->channel;
 }
 
 int
-__nouveau_fence_wait(void *sync_obj, void *sync_arg, bool lazy, bool intr)
+nouveau_fence_wait(struct nouveau_fence *fence, bool lazy, bool intr)
 {
-	unsigned long timeout = jiffies + (3 * DRM_HZ);
 	unsigned long sleep_time = NSEC_PER_MSEC / 1000;
 	ktime_t t;
 	int ret = 0;
 
-	while (1) {
-		if (__nouveau_fence_signalled(sync_obj, sync_arg))
-			break;
-
-		if (time_after_eq(jiffies, timeout)) {
+	while (!nouveau_fence_done(fence)) {
+		if (fence->timeout && time_after_eq(jiffies, fence->timeout)) {
 			ret = -EBUSY;
 			break;
 		}
 
-		__set_current_state(intr ? TASK_INTERRUPTIBLE
-			: TASK_UNINTERRUPTIBLE);
+		__set_current_state(intr ? TASK_INTERRUPTIBLE :
+					   TASK_UNINTERRUPTIBLE);
 		if (lazy) {
 			t = ktime_set(0, sleep_time);
 			schedule_hrtimeout(&t, HRTIMER_MODE_REL);
@@ -261,354 +141,72 @@ __nouveau_fence_wait(void *sync_obj, void *sync_arg, bool lazy, bool intr)
 	}
 
 	__set_current_state(TASK_RUNNING);
-
 	return ret;
 }
 
-static struct nouveau_semaphore *
-semaphore_alloc(struct drm_device *dev)
-{
-	struct drm_nouveau_private *dev_priv = dev->dev_private;
-	struct nouveau_semaphore *sema;
-	int size = (dev_priv->chipset < 0x84) ? 4 : 16;
-	int ret, i;
-
-	if (!USE_SEMA(dev))
-		return NULL;
-
-	sema = kmalloc(sizeof(*sema), GFP_KERNEL);
-	if (!sema)
-		goto fail;
-
-	ret = drm_mm_pre_get(&dev_priv->fence.heap);
-	if (ret)
-		goto fail;
-
-	spin_lock(&dev_priv->fence.lock);
-	sema->mem = drm_mm_search_free(&dev_priv->fence.heap, size, 0, 0);
-	if (sema->mem)
-		sema->mem = drm_mm_get_block_atomic(sema->mem, size, 0);
-	spin_unlock(&dev_priv->fence.lock);
-
-	if (!sema->mem)
-		goto fail;
-
-	kref_init(&sema->ref);
-	sema->dev = dev;
-	for (i = sema->mem->start; i < sema->mem->start + size; i += 4)
-		nouveau_bo_wr32(dev_priv->fence.bo, i / 4, 0);
-
-	return sema;
-fail:
-	kfree(sema);
-	return NULL;
-}
-
-static void
-semaphore_free(struct kref *ref)
-{
-	struct nouveau_semaphore *sema =
-		container_of(ref, struct nouveau_semaphore, ref);
-	struct drm_nouveau_private *dev_priv = sema->dev->dev_private;
-
-	spin_lock(&dev_priv->fence.lock);
-	drm_mm_put_block(sema->mem);
-	spin_unlock(&dev_priv->fence.lock);
-
-	kfree(sema);
-}
-
-static void
-semaphore_work(void *priv, bool signalled)
-{
-	struct nouveau_semaphore *sema = priv;
-	struct drm_nouveau_private *dev_priv = sema->dev->dev_private;
-
-	if (unlikely(!signalled))
-		nouveau_bo_wr32(dev_priv->fence.bo, sema->mem->start / 4, 1);
-
-	kref_put(&sema->ref, semaphore_free);
-}
-
-static int
-semaphore_acquire(struct nouveau_channel *chan, struct nouveau_semaphore *sema)
-{
-	struct drm_nouveau_private *dev_priv = chan->dev->dev_private;
-	struct nouveau_fence *fence = NULL;
-	u64 offset = chan->fence.vma.offset + sema->mem->start;
-	int ret;
-
-	if (dev_priv->chipset < 0x84) {
-		ret = RING_SPACE(chan, 4);
-		if (ret)
-			return ret;
-
-		BEGIN_RING(chan, 0, NV11_SUBCHAN_DMA_SEMAPHORE, 3);
-		OUT_RING  (chan, NvSema);
-		OUT_RING  (chan, offset);
-		OUT_RING  (chan, 1);
-	} else
-	if (dev_priv->chipset < 0xc0) {
-		ret = RING_SPACE(chan, 7);
-		if (ret)
-			return ret;
-
-		BEGIN_RING(chan, 0, NV11_SUBCHAN_DMA_SEMAPHORE, 1);
-		OUT_RING  (chan, chan->vram_handle);
-		BEGIN_RING(chan, 0, NV84_SUBCHAN_SEMAPHORE_ADDRESS_HIGH, 4);
-		OUT_RING  (chan, upper_32_bits(offset));
-		OUT_RING  (chan, lower_32_bits(offset));
-		OUT_RING  (chan, 1);
-		OUT_RING  (chan, 1); /* ACQUIRE_EQ */
-	} else {
-		ret = RING_SPACE(chan, 5);
-		if (ret)
-			return ret;
-
-		BEGIN_NVC0(chan, 2, 0, NV84_SUBCHAN_SEMAPHORE_ADDRESS_HIGH, 4);
-		OUT_RING  (chan, upper_32_bits(offset));
-		OUT_RING  (chan, lower_32_bits(offset));
-		OUT_RING  (chan, 1);
-		OUT_RING  (chan, 0x1001); /* ACQUIRE_EQ */
-	}
-
-	/* Delay semaphore destruction until its work is done */
-	ret = nouveau_fence_new(chan, &fence, true);
-	if (ret)
-		return ret;
-
-	kref_get(&sema->ref);
-	nouveau_fence_work(fence, semaphore_work, sema);
-	nouveau_fence_unref(&fence);
-	return 0;
-}
-
-static int
-semaphore_release(struct nouveau_channel *chan, struct nouveau_semaphore *sema)
-{
-	struct drm_nouveau_private *dev_priv = chan->dev->dev_private;
-	struct nouveau_fence *fence = NULL;
-	u64 offset = chan->fence.vma.offset + sema->mem->start;
-	int ret;
-
-	if (dev_priv->chipset < 0x84) {
-		ret = RING_SPACE(chan, 5);
-		if (ret)
-			return ret;
-
-		BEGIN_RING(chan, 0, NV11_SUBCHAN_DMA_SEMAPHORE, 2);
-		OUT_RING  (chan, NvSema);
-		OUT_RING  (chan, offset);
-		BEGIN_RING(chan, 0, NV11_SUBCHAN_SEMAPHORE_RELEASE, 1);
-		OUT_RING  (chan, 1);
-	} else
-	if (dev_priv->chipset < 0xc0) {
-		ret = RING_SPACE(chan, 7);
-		if (ret)
-			return ret;
-
-		BEGIN_RING(chan, 0, NV11_SUBCHAN_DMA_SEMAPHORE, 1);
-		OUT_RING  (chan, chan->vram_handle);
-		BEGIN_RING(chan, 0, NV84_SUBCHAN_SEMAPHORE_ADDRESS_HIGH, 4);
-		OUT_RING  (chan, upper_32_bits(offset));
-		OUT_RING  (chan, lower_32_bits(offset));
-		OUT_RING  (chan, 1);
-		OUT_RING  (chan, 2); /* RELEASE */
-	} else {
-		ret = RING_SPACE(chan, 5);
-		if (ret)
-			return ret;
-
-		BEGIN_NVC0(chan, 2, 0, NV84_SUBCHAN_SEMAPHORE_ADDRESS_HIGH, 4);
-		OUT_RING  (chan, upper_32_bits(offset));
-		OUT_RING  (chan, lower_32_bits(offset));
-		OUT_RING  (chan, 1);
-		OUT_RING  (chan, 0x1002); /* RELEASE */
-	}
-
-	/* Delay semaphore destruction until its work is done */
-	ret = nouveau_fence_new(chan, &fence, true);
-	if (ret)
-		return ret;
-
-	kref_get(&sema->ref);
-	nouveau_fence_work(fence, semaphore_work, sema);
-	nouveau_fence_unref(&fence);
-	return 0;
-}
-
 int
-nouveau_fence_sync(struct nouveau_fence *fence,
-		   struct nouveau_channel *wchan)
+nouveau_fence_sync(struct nouveau_fence *fence, struct nouveau_channel *chan)
 {
-	struct nouveau_channel *chan = nouveau_fence_channel(fence);
-	struct drm_device *dev = wchan->dev;
-	struct nouveau_semaphore *sema;
+	struct drm_device *dev = chan->dev;
+	struct nouveau_fence_priv *priv = nv_engine(dev, NVOBJ_ENGINE_FENCE);
+	struct nouveau_channel *prev;
 	int ret = 0;
 
-	if (likely(!chan || chan == wchan ||
-		   nouveau_fence_signalled(fence)))
-		goto out;
-
-	sema = semaphore_alloc(dev);
-	if (!sema) {
-		/* Early card or broken userspace, fall back to
-		 * software sync. */
-		ret = nouveau_fence_wait(fence, true, false);
-		goto out;
-	}
-
-	/* try to take chan's mutex, if we can't take it right away
-	 * we have to fallback to software sync to prevent locking
-	 * order issues
-	 */
-	if (!mutex_trylock(&chan->mutex)) {
-		ret = nouveau_fence_wait(fence, true, false);
-		goto out_unref;
+	prev = fence ? nouveau_channel_get_unlocked(fence->channel) : NULL;
+	if (prev) {
+		if (unlikely(prev != chan && !nouveau_fence_done(fence))) {
+			ret = priv->sync(fence, prev, chan);
+			if (unlikely(ret))
+				ret = nouveau_fence_wait(fence, true, false);
+		}
+		nouveau_channel_put_unlocked(&prev);
 	}
 
-	/* Make wchan wait until it gets signalled */
-	ret = semaphore_acquire(wchan, sema);
-	if (ret)
-		goto out_unlock;
-
-	/* Signal the semaphore from chan */
-	ret = semaphore_release(chan, sema);
-
-out_unlock:
-	mutex_unlock(&chan->mutex);
-out_unref:
-	kref_put(&sema->ref, semaphore_free);
-out:
-	if (chan)
-		nouveau_channel_put_unlocked(&chan);
 	return ret;
 }
 
-int
-__nouveau_fence_flush(void *sync_obj, void *sync_arg)
+static void
+nouveau_fence_del(struct kref *kref)
 {
-	return 0;
+	struct nouveau_fence *fence = container_of(kref, typeof(*fence), kref);
+	kfree(fence);
 }
 
-int
-nouveau_fence_channel_init(struct nouveau_channel *chan)
+void
+nouveau_fence_unref(struct nouveau_fence **pfence)
 {
-	struct drm_device *dev = chan->dev;
-	struct drm_nouveau_private *dev_priv = dev->dev_private;
-	struct nouveau_gpuobj *obj = NULL;
-	int ret;
-
-	if (dev_priv->card_type < NV_C0) {
-		/* Create an NV_SW object for various sync purposes */
-		ret = nouveau_gpuobj_gr_new(chan, NvSw, NV_SW);
-		if (ret)
-			return ret;
-
-		ret = RING_SPACE(chan, 2);
-		if (ret)
-			return ret;
-
-		BEGIN_RING(chan, NvSubSw, NV01_SUBCHAN_OBJECT, 1);
-		OUT_RING  (chan, NvSw);
-		FIRE_RING (chan);
-	}
-
-	/* Setup area of memory shared between all channels for x-chan sync */
-	if (USE_SEMA(dev) && dev_priv->chipset < 0x84) {
-		struct ttm_mem_reg *mem = &dev_priv->fence.bo->bo.mem;
-
-		ret = nouveau_gpuobj_dma_new(chan, NV_CLASS_DMA_FROM_MEMORY,
-					     mem->start << PAGE_SHIFT,
-					     mem->size, NV_MEM_ACCESS_RW,
-					     NV_MEM_TARGET_VRAM, &obj);
-		if (ret)
-			return ret;
-
-		ret = nouveau_ramht_insert(chan, NvSema, obj);
-		nouveau_gpuobj_ref(NULL, &obj);
-		if (ret)
-			return ret;
-	} else
-	if (USE_SEMA(dev)) {
-		/* map fence bo into channel's vm */
-		ret = nouveau_bo_vma_add(dev_priv->fence.bo, chan->vm,
-					 &chan->fence.vma);
-		if (ret)
-			return ret;
-	}
-
-	atomic_set(&chan->fence.last_sequence_irq, 0);
-	return 0;
+	if (*pfence)
+		kref_put(&(*pfence)->kref, nouveau_fence_del);
+	*pfence = NULL;
 }
 
-void
-nouveau_fence_channel_fini(struct nouveau_channel *chan)
+struct nouveau_fence *
+nouveau_fence_ref(struct nouveau_fence *fence)
 {
-	struct drm_nouveau_private *dev_priv = chan->dev->dev_private;
-	struct nouveau_fence *tmp, *fence;
-
-	spin_lock(&chan->fence.lock);
-	list_for_each_entry_safe(fence, tmp, &chan->fence.pending, entry) {
-		fence->signalled = true;
-		list_del(&fence->entry);
-
-		if (unlikely(fence->work))
-			fence->work(fence->priv, false);
-
-		kref_put(&fence->refcount, nouveau_fence_del);
-	}
-	spin_unlock(&chan->fence.lock);
-
-	nouveau_bo_vma_del(dev_priv->fence.bo, &chan->fence.vma);
+	kref_get(&fence->kref);
+	return fence;
 }
 
 int
-nouveau_fence_init(struct drm_device *dev)
+nouveau_fence_new(struct nouveau_channel *chan, struct nouveau_fence **pfence)
 {
-	struct drm_nouveau_private *dev_priv = dev->dev_private;
-	int size = (dev_priv->chipset < 0x84) ? 4096 : 16384;
-	int ret;
-
-	/* Create a shared VRAM heap for cross-channel sync. */
-	if (USE_SEMA(dev)) {
-		ret = nouveau_bo_new(dev, size, 0, TTM_PL_FLAG_VRAM,
-				     0, 0, &dev_priv->fence.bo);
-		if (ret)
-			return ret;
+	struct nouveau_fence *fence;
+	int ret = 0;
 
-		ret = nouveau_bo_pin(dev_priv->fence.bo, TTM_PL_FLAG_VRAM);
-		if (ret)
-			goto fail;
+	if (unlikely(!chan->engctx[NVOBJ_ENGINE_FENCE]))
+		return -ENODEV;
 
-		ret = nouveau_bo_map(dev_priv->fence.bo);
-		if (ret)
-			goto fail;
+	fence = kzalloc(sizeof(*fence), GFP_KERNEL);
+	if (!fence)
+		return -ENOMEM;
+	kref_init(&fence->kref);
 
-		ret = drm_mm_init(&dev_priv->fence.heap, 0,
-				  dev_priv->fence.bo->bo.mem.size);
+	if (chan) {
+		ret = nouveau_fence_emit(fence, chan);
 		if (ret)
-			goto fail;
-
-		spin_lock_init(&dev_priv->fence.lock);
+			nouveau_fence_unref(&fence);
 	}
 
-	return 0;
-fail:
-	nouveau_bo_unmap(dev_priv->fence.bo);
-	nouveau_bo_ref(NULL, &dev_priv->fence.bo);
+	*pfence = fence;
 	return ret;
 }
-
-void
-nouveau_fence_fini(struct drm_device *dev)
-{
-	struct drm_nouveau_private *dev_priv = dev->dev_private;
-
-	if (USE_SEMA(dev)) {
-		drm_mm_takedown(&dev_priv->fence.heap);
-		nouveau_bo_unmap(dev_priv->fence.bo);
-		nouveau_bo_unpin(dev_priv->fence.bo);
-		nouveau_bo_ref(NULL, &dev_priv->fence.bo);
-	}
-}
diff --git a/drivers/gpu/drm/nouveau/nouveau_fence.h b/drivers/gpu/drm/nouveau/nouveau_fence.h
new file mode 100644
index 00000000000..82ba733393a
--- /dev/null
+++ b/drivers/gpu/drm/nouveau/nouveau_fence.h
@@ -0,0 +1,52 @@
+#ifndef __NOUVEAU_FENCE_H__
+#define __NOUVEAU_FENCE_H__
+
+struct nouveau_fence {
+	struct list_head head;
+	struct kref kref;
+
+	struct nouveau_channel *channel;
+	unsigned long timeout;
+	u32 sequence;
+
+	void (*work)(void *priv, bool signalled);
+	void *priv;
+};
+
+int  nouveau_fence_new(struct nouveau_channel *, struct nouveau_fence **);
+struct nouveau_fence *
+nouveau_fence_ref(struct nouveau_fence *);
+void nouveau_fence_unref(struct nouveau_fence **);
+
+int  nouveau_fence_emit(struct nouveau_fence *, struct nouveau_channel *);
+bool nouveau_fence_done(struct nouveau_fence *);
+int  nouveau_fence_wait(struct nouveau_fence *, bool lazy, bool intr);
+int  nouveau_fence_sync(struct nouveau_fence *, struct nouveau_channel *);
+void nouveau_fence_idle(struct nouveau_channel *);
+void nouveau_fence_update(struct nouveau_channel *);
+
+struct nouveau_fence_chan {
+	struct list_head pending;
+	spinlock_t lock;
+	u32 sequence;
+};
+
+struct nouveau_fence_priv {
+	struct nouveau_exec_engine engine;
+	int (*emit)(struct nouveau_fence *);
+	int (*sync)(struct nouveau_fence *, struct nouveau_channel *,
+		    struct nouveau_channel *);
+	u32 (*read)(struct nouveau_channel *);
+};
+
+void nouveau_fence_context_new(struct nouveau_fence_chan *);
+void nouveau_fence_context_del(struct nouveau_fence_chan *);
+
+int nv04_fence_create(struct drm_device *dev);
+int nv04_fence_mthd(struct nouveau_channel *, u32, u32, u32);
+
+int nv10_fence_create(struct drm_device *dev);
+int nv84_fence_create(struct drm_device *dev);
+int nvc0_fence_create(struct drm_device *dev);
+
+#endif
diff --git a/drivers/gpu/drm/nouveau/nouveau_fifo.h b/drivers/gpu/drm/nouveau/nouveau_fifo.h
new file mode 100644
index 00000000000..ce99cab2f25
--- /dev/null
+++ b/drivers/gpu/drm/nouveau/nouveau_fifo.h
@@ -0,0 +1,32 @@
+#ifndef __NOUVEAU_FIFO_H__
+#define __NOUVEAU_FIFO_H__
+
+struct nouveau_fifo_priv {
+	struct nouveau_exec_engine base;
+	u32 channels;
+};
+
+struct nouveau_fifo_chan {
+};
+
+bool nv04_fifo_cache_pull(struct drm_device *, bool);
+void nv04_fifo_context_del(struct nouveau_channel *, int);
+int  nv04_fifo_fini(struct drm_device *, int, bool);
+int  nv04_fifo_init(struct drm_device *, int);
+void nv04_fifo_isr(struct drm_device *);
+void nv04_fifo_destroy(struct drm_device *, int);
+
+void nv50_fifo_playlist_update(struct drm_device *);
+void nv50_fifo_destroy(struct drm_device *, int);
+void nv50_fifo_tlb_flush(struct drm_device *, int);
+
+int  nv04_fifo_create(struct drm_device *);
+int  nv10_fifo_create(struct drm_device *);
+int  nv17_fifo_create(struct drm_device *);
+int  nv40_fifo_create(struct drm_device *);
+int  nv50_fifo_create(struct drm_device *);
+int  nv84_fifo_create(struct drm_device *);
+int  nvc0_fifo_create(struct drm_device *);
+int  nve0_fifo_create(struct drm_device *);
+
+#endif
diff --git a/drivers/gpu/drm/nouveau/nouveau_gem.c b/drivers/gpu/drm/nouveau/nouveau_gem.c
index ed52a6f4161..30f54231694 100644
--- a/drivers/gpu/drm/nouveau/nouveau_gem.c
+++ b/drivers/gpu/drm/nouveau/nouveau_gem.c
@@ -23,12 +23,14 @@
  * WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
  *
  */
+#include <linux/dma-buf.h>
 #include "drmP.h"
 #include "drm.h"
 
 #include "nouveau_drv.h"
 #include "nouveau_drm.h"
 #include "nouveau_dma.h"
+#include "nouveau_fence.h"
 
 #define nouveau_gem_pushbuf_sync(chan) 0
 
@@ -53,6 +55,9 @@ nouveau_gem_object_del(struct drm_gem_object *gem)
 		nouveau_bo_unpin(nvbo);
 	}
 
+	if (gem->import_attach)
+		drm_prime_gem_destroy(gem, nvbo->bo.sg);
+
 	ttm_bo_unref(&bo);
 
 	drm_gem_object_release(gem);
@@ -139,7 +144,7 @@ nouveau_gem_new(struct drm_device *dev, int size, int align, uint32_t domain,
 		flags |= TTM_PL_FLAG_SYSTEM;
 
 	ret = nouveau_bo_new(dev, size, align, flags, tile_mode,
-			     tile_flags, pnvbo);
+			     tile_flags, NULL, pnvbo);
 	if (ret)
 		return ret;
 	nvbo = *pnvbo;
@@ -704,7 +709,7 @@ nouveau_gem_ioctl_pushbuf(struct drm_device *dev, void *data,
 	}
 
 	if (chan->dma.ib_max) {
-		ret = nouveau_dma_wait(chan, req->nr_push + 1, 6);
+		ret = nouveau_dma_wait(chan, req->nr_push + 1, 16);
 		if (ret) {
 			NV_INFO(dev, "nv50cal_space: %d\n", ret);
 			goto out;
@@ -774,7 +779,7 @@ nouveau_gem_ioctl_pushbuf(struct drm_device *dev, void *data,
 		}
 	}
 
-	ret = nouveau_fence_new(chan, &fence, true);
+	ret = nouveau_fence_new(chan, &fence);
 	if (ret) {
 		NV_ERROR(dev, "error fencing pushbuf: %d\n", ret);
 		WIND_RING(chan);
diff --git a/drivers/gpu/drm/nouveau/nouveau_gpio.c b/drivers/gpu/drm/nouveau/nouveau_gpio.c
index a580cc62337..82c19e82ff0 100644
--- a/drivers/gpu/drm/nouveau/nouveau_gpio.c
+++ b/drivers/gpu/drm/nouveau/nouveau_gpio.c
@@ -387,7 +387,7 @@ nouveau_gpio_reset(struct drm_device *dev)
 		if (dev_priv->card_type >= NV_D0) {
 			nv_mask(dev, 0x00d610 + (line * 4), 0xff, unk0);
 			if (unk1--)
-				nv_mask(dev, 0x00d640 + (unk1 * 4), 0xff, line);
+				nv_mask(dev, 0x00d740 + (unk1 * 4), 0xff, line);
 		} else
 		if (dev_priv->card_type >= NV_50) {
 			static const u32 regs[] = { 0xe100, 0xe28c };
diff --git a/drivers/gpu/drm/nouveau/nouveau_grctx.h b/drivers/gpu/drm/nouveau/nouveau_grctx.h
index 86c2e374e93..b0795ececbd 100644
--- a/drivers/gpu/drm/nouveau/nouveau_grctx.h
+++ b/drivers/gpu/drm/nouveau/nouveau_grctx.h
@@ -18,7 +18,6 @@ struct nouveau_grctx {
 	uint32_t ctxvals_base;
 };
 
-#ifdef CP_CTX
 static inline void
 cp_out(struct nouveau_grctx *ctx, uint32_t inst)
 {
@@ -88,10 +87,8 @@ _cp_bra(struct nouveau_grctx *ctx, u32 mod, int flag, int state, int name)
 		    (state ? 0 : CP_BRA_IF_CLEAR));
 }
 #define cp_bra(c, f, s, n) _cp_bra((c), 0, CP_FLAG_##f, CP_FLAG_##f##_##s, n)
-#ifdef CP_BRA_MOD
 #define cp_cal(c, f, s, n) _cp_bra((c), 1, CP_FLAG_##f, CP_FLAG_##f##_##s, n)
 #define cp_ret(c, f, s) _cp_bra((c), 2, CP_FLAG_##f, CP_FLAG_##f##_##s, 0)
-#endif
 
 static inline void
 _cp_wait(struct nouveau_grctx *ctx, int flag, int state)
@@ -128,6 +125,5 @@ gr_def(struct nouveau_grctx *ctx, uint32_t reg, uint32_t val)
 
 	nv_wo32(ctx->data, reg * 4, val);
 }
-#endif
 
 #endif
diff --git a/drivers/gpu/drm/nouveau/nouveau_hw.c b/drivers/gpu/drm/nouveau/nouveau_hw.c
index ba896e54b79..b87ad3bd773 100644
--- a/drivers/gpu/drm/nouveau/nouveau_hw.c
+++ b/drivers/gpu/drm/nouveau/nouveau_hw.c
@@ -1018,11 +1018,6 @@ nv_load_state_ext(struct drm_device *dev, int head,
 	}
 
 	NVWriteCRTC(dev, head, NV_PCRTC_START, regp->fb_start);
-
-	/* Enable vblank interrupts. */
-	NVWriteCRTC(dev, head, NV_PCRTC_INTR_EN_0,
-		    (dev->vblank_enabled[head] ? 1 : 0));
-	NVWriteCRTC(dev, head, NV_PCRTC_INTR_0, NV_PCRTC_INTR_0_VBLANK);
 }
 
 static void
diff --git a/drivers/gpu/drm/nouveau/nouveau_i2c.c b/drivers/gpu/drm/nouveau/nouveau_i2c.c
index e2be95af2e5..77e564667b5 100644
--- a/drivers/gpu/drm/nouveau/nouveau_i2c.c
+++ b/drivers/gpu/drm/nouveau/nouveau_i2c.c
@@ -29,10 +29,6 @@
 #include "nouveau_i2c.h"
 #include "nouveau_hw.h"
 
-#define T_TIMEOUT  2200000
-#define T_RISEFALL 1000
-#define T_HOLD     5000
-
 static void
 i2c_drive_scl(void *data, int state)
 {
@@ -113,175 +109,6 @@ i2c_sense_sda(void *data)
 	return 0;
 }
 
-static void
-i2c_delay(struct nouveau_i2c_chan *port, u32 nsec)
-{
-	udelay((nsec + 500) / 1000);
-}
-
-static bool
-i2c_raise_scl(struct nouveau_i2c_chan *port)
-{
-	u32 timeout = T_TIMEOUT / T_RISEFALL;
-
-	i2c_drive_scl(port, 1);
-	do {
-		i2c_delay(port, T_RISEFALL);
-	} while (!i2c_sense_scl(port) && --timeout);
-
-	return timeout != 0;
-}
-
-static int
-i2c_start(struct nouveau_i2c_chan *port)
-{
-	int ret = 0;
-
-	port->state  = i2c_sense_scl(port);
-	port->state |= i2c_sense_sda(port) << 1;
-	if (port->state != 3) {
-		i2c_drive_scl(port, 0);
-		i2c_drive_sda(port, 1);
-		if (!i2c_raise_scl(port))
-			ret = -EBUSY;
-	}
-
-	i2c_drive_sda(port, 0);
-	i2c_delay(port, T_HOLD);
-	i2c_drive_scl(port, 0);
-	i2c_delay(port, T_HOLD);
-	return ret;
-}
-
-static void
-i2c_stop(struct nouveau_i2c_chan *port)
-{
-	i2c_drive_scl(port, 0);
-	i2c_drive_sda(port, 0);
-	i2c_delay(port, T_RISEFALL);
-
-	i2c_drive_scl(port, 1);
-	i2c_delay(port, T_HOLD);
-	i2c_drive_sda(port, 1);
-	i2c_delay(port, T_HOLD);
-}
-
-static int
-i2c_bitw(struct nouveau_i2c_chan *port, int sda)
-{
-	i2c_drive_sda(port, sda);
-	i2c_delay(port, T_RISEFALL);
-
-	if (!i2c_raise_scl(port))
-		return -ETIMEDOUT;
-	i2c_delay(port, T_HOLD);
-
-	i2c_drive_scl(port, 0);
-	i2c_delay(port, T_HOLD);
-	return 0;
-}
-
-static int
-i2c_bitr(struct nouveau_i2c_chan *port)
-{
-	int sda;
-
-	i2c_drive_sda(port, 1);
-	i2c_delay(port, T_RISEFALL);
-
-	if (!i2c_raise_scl(port))
-		return -ETIMEDOUT;
-	i2c_delay(port, T_HOLD);
-
-	sda = i2c_sense_sda(port);
-
-	i2c_drive_scl(port, 0);
-	i2c_delay(port, T_HOLD);
-	return sda;
-}
-
-static int
-i2c_get_byte(struct nouveau_i2c_chan *port, u8 *byte, bool last)
-{
-	int i, bit;
-
-	*byte = 0;
-	for (i = 7; i >= 0; i--) {
-		bit = i2c_bitr(port);
-		if (bit < 0)
-			return bit;
-		*byte |= bit << i;
-	}
-
-	return i2c_bitw(port, last ? 1 : 0);
-}
-
-static int
-i2c_put_byte(struct nouveau_i2c_chan *port, u8 byte)
-{
-	int i, ret;
-	for (i = 7; i >= 0; i--) {
-		ret = i2c_bitw(port, !!(byte & (1 << i)));
-		if (ret < 0)
-			return ret;
-	}
-
-	ret = i2c_bitr(port);
-	if (ret == 1) /* nack */
-		ret = -EIO;
-	return ret;
-}
-
-static int
-i2c_addr(struct nouveau_i2c_chan *port, struct i2c_msg *msg)
-{
-	u32 addr = msg->addr << 1;
-	if (msg->flags & I2C_M_RD)
-		addr |= 1;
-	return i2c_put_byte(port, addr);
-}
-
-static int
-i2c_bit_xfer(struct i2c_adapter *adap, struct i2c_msg *msgs, int num)
-{
-	struct nouveau_i2c_chan *port = (struct nouveau_i2c_chan *)adap;
-	struct i2c_msg *msg = msgs;
-	int ret = 0, mcnt = num;
-
-	while (!ret && mcnt--) {
-		u8 remaining = msg->len;
-		u8 *ptr = msg->buf;
-
-		ret = i2c_start(port);
-		if (ret == 0)
-			ret = i2c_addr(port, msg);
-
-		if (msg->flags & I2C_M_RD) {
-			while (!ret && remaining--)
-				ret = i2c_get_byte(port, ptr++, !remaining);
-		} else {
-			while (!ret && remaining--)
-				ret = i2c_put_byte(port, *ptr++);
-		}
-
-		msg++;
-	}
-
-	i2c_stop(port);
-	return (ret < 0) ? ret : num;
-}
-
-static u32
-i2c_bit_func(struct i2c_adapter *adap)
-{
-	return I2C_FUNC_I2C | I2C_FUNC_SMBUS_EMUL;
-}
-
-const struct i2c_algorithm nouveau_i2c_bit_algo = {
-	.master_xfer = i2c_bit_xfer,
-	.functionality = i2c_bit_func
-};
-
 static const uint32_t nv50_i2c_port[] = {
 	0x00e138, 0x00e150, 0x00e168, 0x00e180,
 	0x00e254, 0x00e274, 0x00e764, 0x00e780,
@@ -384,12 +211,10 @@ nouveau_i2c_init(struct drm_device *dev)
 		case 0: /* NV04:NV50 */
 			port->drive = entry[0];
 			port->sense = entry[1];
-			port->adapter.algo = &nouveau_i2c_bit_algo;
 			break;
 		case 4: /* NV4E */
 			port->drive = 0x600800 + entry[1];
 			port->sense = port->drive;
-			port->adapter.algo = &nouveau_i2c_bit_algo;
 			break;
 		case 5: /* NV50- */
 			port->drive = entry[0] & 0x0f;
@@ -402,7 +227,6 @@ nouveau_i2c_init(struct drm_device *dev)
 				port->drive = 0x00d014 + (port->drive * 0x20);
 				port->sense = port->drive;
 			}
-			port->adapter.algo = &nouveau_i2c_bit_algo;
 			break;
 		case 6: /* NV50- DP AUX */
 			port->drive = entry[0];
@@ -413,7 +237,7 @@ nouveau_i2c_init(struct drm_device *dev)
 			break;
 		}
 
-		if (!port->adapter.algo) {
+		if (!port->adapter.algo && !port->drive) {
 			NV_ERROR(dev, "I2C%d: type %d index %x/%x unknown\n",
 				 i, port->type, port->drive, port->sense);
 			kfree(port);
@@ -429,7 +253,26 @@ nouveau_i2c_init(struct drm_device *dev)
 		port->dcb = ROM32(entry[0]);
 		i2c_set_adapdata(&port->adapter, i2c);
 
-		ret = i2c_add_adapter(&port->adapter);
+		if (port->adapter.algo != &nouveau_dp_i2c_algo) {
+			port->adapter.algo_data = &port->bit;
+			port->bit.udelay = 10;
+			port->bit.timeout = usecs_to_jiffies(2200);
+			port->bit.data = port;
+			port->bit.setsda = i2c_drive_sda;
+			port->bit.setscl = i2c_drive_scl;
+			port->bit.getsda = i2c_sense_sda;
+			port->bit.getscl = i2c_sense_scl;
+
+			i2c_drive_scl(port, 0);
+			i2c_drive_sda(port, 1);
+			i2c_drive_scl(port, 1);
+
+			ret = i2c_bit_add_bus(&port->adapter);
+		} else {
+			port->adapter.algo = &nouveau_dp_i2c_algo;
+			ret = i2c_add_adapter(&port->adapter);
+		}
+
 		if (ret) {
 			NV_ERROR(dev, "I2C%d: failed register: %d\n", i, ret);
 			kfree(port);
diff --git a/drivers/gpu/drm/nouveau/nouveau_i2c.h b/drivers/gpu/drm/nouveau/nouveau_i2c.h
index 4d2e4e9031b..1d083893a4d 100644
--- a/drivers/gpu/drm/nouveau/nouveau_i2c.h
+++ b/drivers/gpu/drm/nouveau/nouveau_i2c.h
@@ -34,6 +34,7 @@
 struct nouveau_i2c_chan {
 	struct i2c_adapter adapter;
 	struct drm_device *dev;
+	struct i2c_algo_bit_data bit;
 	struct list_head head;
 	u8  index;
 	u8  type;
diff --git a/drivers/gpu/drm/nouveau/nouveau_mem.c b/drivers/gpu/drm/nouveau/nouveau_mem.c
index b08065f981d..5b498ea32e1 100644
--- a/drivers/gpu/drm/nouveau/nouveau_mem.c
+++ b/drivers/gpu/drm/nouveau/nouveau_mem.c
@@ -39,6 +39,8 @@
 #include "nouveau_pm.h"
 #include "nouveau_mm.h"
 #include "nouveau_vm.h"
+#include "nouveau_fifo.h"
+#include "nouveau_fence.h"
 
 /*
  * NV10-NV40 tiling helpers
@@ -50,7 +52,6 @@ nv10_mem_update_tile_region(struct drm_device *dev,
 			    uint32_t size, uint32_t pitch, uint32_t flags)
 {
 	struct drm_nouveau_private *dev_priv = dev->dev_private;
-	struct nouveau_fifo_engine *pfifo = &dev_priv->engine.fifo;
 	struct nouveau_fb_engine *pfb = &dev_priv->engine.fb;
 	int i = tile - dev_priv->tile.reg, j;
 	unsigned long save;
@@ -64,8 +65,8 @@ nv10_mem_update_tile_region(struct drm_device *dev,
 		pfb->init_tile_region(dev, i, addr, size, pitch, flags);
 
 	spin_lock_irqsave(&dev_priv->context_switch_lock, save);
-	pfifo->reassign(dev, false);
-	pfifo->cache_pull(dev, false);
+	nv_wr32(dev, NV03_PFIFO_CACHES, 0);
+	nv04_fifo_cache_pull(dev, false);
 
 	nouveau_wait_for_idle(dev);
 
@@ -75,8 +76,8 @@ nv10_mem_update_tile_region(struct drm_device *dev,
 			dev_priv->eng[j]->set_tile_region(dev, i);
 	}
 
-	pfifo->cache_pull(dev, true);
-	pfifo->reassign(dev, true);
+	nv04_fifo_cache_pull(dev, true);
+	nv_wr32(dev, NV03_PFIFO_CACHES, 1);
 	spin_unlock_irqrestore(&dev_priv->context_switch_lock, save);
 }
 
@@ -89,7 +90,7 @@ nv10_mem_get_tile_region(struct drm_device *dev, int i)
 	spin_lock(&dev_priv->tile.lock);
 
 	if (!tile->used &&
-	    (!tile->fence || nouveau_fence_signalled(tile->fence)))
+	    (!tile->fence || nouveau_fence_done(tile->fence)))
 		tile->used = true;
 	else
 		tile = NULL;
@@ -416,7 +417,7 @@ nouveau_mem_vram_init(struct drm_device *dev)
 
 	if (dev_priv->card_type < NV_50) {
 		ret = nouveau_bo_new(dev, 256*1024, 0, TTM_PL_FLAG_VRAM,
-				     0, 0, &dev_priv->vga_ram);
+				     0, 0, NULL, &dev_priv->vga_ram);
 		if (ret == 0)
 			ret = nouveau_bo_pin(dev_priv->vga_ram,
 					     TTM_PL_FLAG_VRAM);
@@ -843,6 +844,7 @@ nouveau_mem_timing_calc(struct drm_device *dev, u32 freq,
 		ret = nv50_mem_timing_calc(dev, freq, e, len, boot, t);
 		break;
 	case NV_C0:
+	case NV_D0:
 		ret = nvc0_mem_timing_calc(dev, freq, e, len, boot, t);
 		break;
 	default:
@@ -977,6 +979,8 @@ nouveau_mem_exec(struct nouveau_mem_exec_func *exec,
 		break;
 	case NV_MEM_TYPE_DDR3:
 		tDLLK = 12000;
+		tCKSRE = 2000;
+		tXS = 1000;
 		mr1_dlloff = 0x00000001;
 		break;
 	case NV_MEM_TYPE_GDDR3:
@@ -1023,6 +1027,7 @@ nouveau_mem_exec(struct nouveau_mem_exec_func *exec,
 	exec->refresh_self(exec, false);
 	exec->refresh_auto(exec, true);
 	exec->wait(exec, tXS);
+	exec->wait(exec, tXS);
 
 	/* update MRs */
 	if (mr[2] != info->mr[2]) {
diff --git a/drivers/gpu/drm/nouveau/nouveau_object.c b/drivers/gpu/drm/nouveau/nouveau_object.c
index cc419fae794..b190cc01c82 100644
--- a/drivers/gpu/drm/nouveau/nouveau_object.c
+++ b/drivers/gpu/drm/nouveau/nouveau_object.c
@@ -34,9 +34,10 @@
 #include "drm.h"
 #include "nouveau_drv.h"
 #include "nouveau_drm.h"
+#include "nouveau_fifo.h"
 #include "nouveau_ramht.h"
+#include "nouveau_software.h"
 #include "nouveau_vm.h"
-#include "nv50_display.h"
 
 struct nouveau_gpuobj_method {
 	struct list_head head;
@@ -120,12 +121,13 @@ nouveau_gpuobj_mthd_call2(struct drm_device *dev, int chid,
 			  u32 class, u32 mthd, u32 data)
 {
 	struct drm_nouveau_private *dev_priv = dev->dev_private;
+	struct nouveau_fifo_priv *pfifo = nv_engine(dev, NVOBJ_ENGINE_FIFO);
 	struct nouveau_channel *chan = NULL;
 	unsigned long flags;
 	int ret = -EINVAL;
 
 	spin_lock_irqsave(&dev_priv->channels.lock, flags);
-	if (chid >= 0 && chid < dev_priv->engine.fifo.channels)
+	if (chid >= 0 && chid < pfifo->channels)
 		chan = dev_priv->channels.ptr[chid];
 	if (chan)
 		ret = nouveau_gpuobj_mthd_call(chan, class, mthd, data);
@@ -133,37 +135,6 @@ nouveau_gpuobj_mthd_call2(struct drm_device *dev, int chid,
 	return ret;
 }
 
-/* NVidia uses context objects to drive drawing operations.
-
-   Context objects can be selected into 8 subchannels in the FIFO,
-   and then used via DMA command buffers.
-
-   A context object is referenced by a user defined handle (CARD32). The HW
-   looks up graphics objects in a hash table in the instance RAM.
-
-   An entry in the hash table consists of 2 CARD32. The first CARD32 contains
-   the handle, the second one a bitfield, that contains the address of the
-   object in instance RAM.
-
-   The format of the second CARD32 seems to be:
-
-   NV4 to NV30:
-
-   15: 0  instance_addr >> 4
-   17:16  engine (here uses 1 = graphics)
-   28:24  channel id (here uses 0)
-   31	  valid (use 1)
-
-   NV40:
-
-   15: 0  instance_addr >> 4   (maybe 19-0)
-   21:20  engine (here uses 1 = graphics)
-   I'm unsure about the other bits, but using 0 seems to work.
-
-   The key into the hash table depends on the object handle and channel id and
-   is given as:
-*/
-
 int
 nouveau_gpuobj_new(struct drm_device *dev, struct nouveau_channel *chan,
 		   uint32_t size, int align, uint32_t flags,
@@ -267,7 +238,7 @@ nouveau_gpuobj_takedown(struct drm_device *dev)
 		kfree(oc);
 	}
 
-	BUG_ON(!list_empty(&dev_priv->gpuobj_list));
+	WARN_ON(!list_empty(&dev_priv->gpuobj_list));
 }
 
 
@@ -361,34 +332,6 @@ nouveau_gpuobj_new_fake(struct drm_device *dev, u32 pinst, u64 vinst,
 	return 0;
 }
 
-/*
-   DMA objects are used to reference a piece of memory in the
-   framebuffer, PCI or AGP address space. Each object is 16 bytes big
-   and looks as follows:
-
-   entry[0]
-   11:0  class (seems like I can always use 0 here)
-   12    page table present?
-   13    page entry linear?
-   15:14 access: 0 rw, 1 ro, 2 wo
-   17:16 target: 0 NV memory, 1 NV memory tiled, 2 PCI, 3 AGP
-   31:20 dma adjust (bits 0-11 of the address)
-   entry[1]
-   dma limit (size of transfer)
-   entry[X]
-   1     0 readonly, 1 readwrite
-   31:12 dma frame address of the page (bits 12-31 of the address)
-   entry[N]
-   page table terminator, same value as the first pte, as does nvidia
-   rivatv uses 0xffffffff
-
-   Non linear page tables need a list of frame addresses afterwards,
-   the rivatv project has some info on this.
-
-   The method below creates a DMA object in instance RAM and returns a handle
-   to it that can be used to set up context objects.
-*/
-
 void
 nv50_gpuobj_dma_init(struct nouveau_gpuobj *obj, u32 offset, int class,
 		     u64 base, u64 size, int target, int access,
@@ -540,82 +483,6 @@ nouveau_gpuobj_dma_new(struct nouveau_channel *chan, int class, u64 base,
 	return 0;
 }
 
-/* Context objects in the instance RAM have the following structure.
- * On NV40 they are 32 byte long, on NV30 and smaller 16 bytes.
-
-   NV4 - NV30:
-
-   entry[0]
-   11:0 class
-   12   chroma key enable
-   13   user clip enable
-   14   swizzle enable
-   17:15 patch config:
-       scrcopy_and, rop_and, blend_and, scrcopy, srccopy_pre, blend_pre
-   18   synchronize enable
-   19   endian: 1 big, 0 little
-   21:20 dither mode
-   23    single step enable
-   24    patch status: 0 invalid, 1 valid
-   25    context_surface 0: 1 valid
-   26    context surface 1: 1 valid
-   27    context pattern: 1 valid
-   28    context rop: 1 valid
-   29,30 context beta, beta4
-   entry[1]
-   7:0   mono format
-   15:8  color format
-   31:16 notify instance address
-   entry[2]
-   15:0  dma 0 instance address
-   31:16 dma 1 instance address
-   entry[3]
-   dma method traps
-
-   NV40:
-   No idea what the exact format is. Here's what can be deducted:
-
-   entry[0]:
-   11:0  class  (maybe uses more bits here?)
-   17    user clip enable
-   21:19 patch config
-   25    patch status valid ?
-   entry[1]:
-   15:0  DMA notifier  (maybe 20:0)
-   entry[2]:
-   15:0  DMA 0 instance (maybe 20:0)
-   24    big endian
-   entry[3]:
-   15:0  DMA 1 instance (maybe 20:0)
-   entry[4]:
-   entry[5]:
-   set to 0?
-*/
-static int
-nouveau_gpuobj_sw_new(struct nouveau_channel *chan, u32 handle, u16 class)
-{
-	struct drm_nouveau_private *dev_priv = chan->dev->dev_private;
-	struct nouveau_gpuobj *gpuobj;
-	int ret;
-
-	gpuobj = kzalloc(sizeof(*gpuobj), GFP_KERNEL);
-	if (!gpuobj)
-		return -ENOMEM;
-	gpuobj->dev = chan->dev;
-	gpuobj->engine = NVOBJ_ENGINE_SW;
-	gpuobj->class = class;
-	kref_init(&gpuobj->refcount);
-	gpuobj->cinst = 0x40;
-
-	spin_lock(&dev_priv->ramin_lock);
-	list_add_tail(&gpuobj->list, &dev_priv->gpuobj_list);
-	spin_unlock(&dev_priv->ramin_lock);
-
-	ret = nouveau_ramht_insert(chan, handle, gpuobj);
-	nouveau_gpuobj_ref(NULL, &gpuobj);
-	return ret;
-}
-
 int
 nouveau_gpuobj_gr_new(struct nouveau_channel *chan, u32 handle, int class)
 {
@@ -632,9 +499,6 @@ nouveau_gpuobj_gr_new(struct nouveau_channel *chan, u32 handle, int class)
 		if (oc->id != class)
 			continue;
 
-		if (oc->engine == NVOBJ_ENGINE_SW)
-			return nouveau_gpuobj_sw_new(chan, handle, class);
-
 		if (!chan->engctx[oc->engine]) {
 			ret = eng->context_new(chan, oc->engine);
 			if (ret)
@@ -644,7 +508,6 @@ nouveau_gpuobj_gr_new(struct nouveau_channel *chan, u32 handle, int class)
 		return eng->object_new(chan, oc->engine, handle, class);
 	}
 
-	NV_ERROR(dev, "illegal object class: 0x%x\n", class);
 	return -EINVAL;
 }
 
@@ -693,11 +556,10 @@ nouveau_gpuobj_channel_init_pramin(struct nouveau_channel *chan)
 static int
 nvc0_gpuobj_channel_init(struct nouveau_channel *chan, struct nouveau_vm *vm)
 {
-	struct drm_nouveau_private *dev_priv = chan->dev->dev_private;
 	struct drm_device *dev = chan->dev;
 	struct nouveau_gpuobj *pgd = NULL;
 	struct nouveau_vm_pgd *vpgd;
-	int ret, i;
+	int ret;
 
 	ret = nouveau_gpuobj_new(dev, NULL, 4096, 0x1000, 0, &chan->ramin);
 	if (ret)
@@ -722,19 +584,6 @@ nvc0_gpuobj_channel_init(struct nouveau_channel *chan, struct nouveau_vm *vm)
 	nv_wo32(chan->ramin, 0x0208, 0xffffffff);
 	nv_wo32(chan->ramin, 0x020c, 0x000000ff);
 
-	/* map display semaphore buffers into channel's vm */
-	for (i = 0; i < dev->mode_config.num_crtc; i++) {
-		struct nouveau_bo *bo;
-		if (dev_priv->card_type >= NV_D0)
-			bo = nvd0_display_crtc_sema(dev, i);
-		else
-			bo = nv50_display(dev)->crtc[i].sem.bo;
-
-		ret = nouveau_bo_vma_add(bo, chan->vm, &chan->dispc_vma[i]);
-		if (ret)
-			return ret;
-	}
-
 	return 0;
 }
 
@@ -747,7 +596,7 @@ nouveau_gpuobj_channel_init(struct nouveau_channel *chan,
 	struct nouveau_fpriv *fpriv = nouveau_fpriv(chan->file_priv);
 	struct nouveau_vm *vm = fpriv ? fpriv->vm : dev_priv->chan_vm;
 	struct nouveau_gpuobj *vram = NULL, *tt = NULL;
-	int ret, i;
+	int ret;
 
 	NV_DEBUG(dev, "ch%d vram=0x%08x tt=0x%08x\n", chan->id, vram_h, tt_h);
 	if (dev_priv->card_type >= NV_C0)
@@ -795,25 +644,6 @@ nouveau_gpuobj_channel_init(struct nouveau_channel *chan,
 		nouveau_gpuobj_ref(NULL, &ramht);
 		if (ret)
 			return ret;
-
-		/* dma objects for display sync channel semaphore blocks */
-		for (i = 0; i < dev->mode_config.num_crtc; i++) {
-			struct nouveau_gpuobj *sem = NULL;
-			struct nv50_display_crtc *dispc =
-				&nv50_display(dev)->crtc[i];
-			u64 offset = dispc->sem.bo->bo.offset;
-
-			ret = nouveau_gpuobj_dma_new(chan, 0x3d, offset, 0xfff,
-						     NV_MEM_ACCESS_RW,
-						     NV_MEM_TARGET_VRAM, &sem);
-			if (ret)
-				return ret;
-
-			ret = nouveau_ramht_insert(chan, NvEvoSema0 + i, sem);
-			nouveau_gpuobj_ref(NULL, &sem);
-			if (ret)
-				return ret;
-		}
 	}
 
 	/* VRAM ctxdma */
@@ -873,25 +703,7 @@ nouveau_gpuobj_channel_init(struct nouveau_channel *chan,
 void
 nouveau_gpuobj_channel_takedown(struct nouveau_channel *chan)
 {
-	struct drm_device *dev = chan->dev;
-	struct drm_nouveau_private *dev_priv = dev->dev_private;
-	int i;
-
-	NV_DEBUG(dev, "ch%d\n", chan->id);
-
-	if (dev_priv->card_type >= NV_D0) {
-		for (i = 0; i < dev->mode_config.num_crtc; i++) {
-			struct nouveau_bo *bo = nvd0_display_crtc_sema(dev, i);
-			nouveau_bo_vma_del(bo, &chan->dispc_vma[i]);
-		}
-	} else
-	if (dev_priv->card_type >= NV_50) {
-		struct nv50_display *disp = nv50_display(dev);
-		for (i = 0; i < dev->mode_config.num_crtc; i++) {
-			struct nv50_display_crtc *dispc = &disp->crtc[i];
-			nouveau_bo_vma_del(dispc->sem.bo, &chan->dispc_vma[i]);
-		}
-	}
+	NV_DEBUG(chan->dev, "ch%d\n", chan->id);
 
 	nouveau_vm_ref(NULL, &chan->vm, chan->vm_pd);
 	nouveau_gpuobj_ref(NULL, &chan->vm_pd);
@@ -956,6 +768,17 @@ int nouveau_ioctl_grobj_alloc(struct drm_device *dev, void *data,
 	if (init->handle == ~0)
 		return -EINVAL;
 
+	/* compatibility with userspace that assumes 506e for all chipsets */
+	if (init->class == 0x506e) {
+		init->class = nouveau_software_class(dev);
+		if (init->class == 0x906e)
+			return 0;
+	} else
+	if (init->class == 0x906e) {
+		NV_ERROR(dev, "906e not supported yet\n");
+		return -EINVAL;
+	}
+
 	chan = nouveau_channel_get(file_priv, init->channel);
 	if (IS_ERR(chan))
 		return PTR_ERR(chan);
diff --git a/drivers/gpu/drm/nouveau/nouveau_perf.c b/drivers/gpu/drm/nouveau/nouveau_perf.c
index 69a528d106e..ea6acf1c4a7 100644
--- a/drivers/gpu/drm/nouveau/nouveau_perf.c
+++ b/drivers/gpu/drm/nouveau/nouveau_perf.c
@@ -83,7 +83,7 @@ nouveau_perf_entry(struct drm_device *dev, int idx,
 	return NULL;
 }
 
-static u8 *
+u8 *
 nouveau_perf_rammap(struct drm_device *dev, u32 freq,
 		    u8 *ver, u8 *hdr, u8 *cnt, u8 *len)
 {
diff --git a/drivers/gpu/drm/nouveau/nouveau_pm.h b/drivers/gpu/drm/nouveau/nouveau_pm.h
index 3f82dfea61d..07cac72c72b 100644
--- a/drivers/gpu/drm/nouveau/nouveau_pm.h
+++ b/drivers/gpu/drm/nouveau/nouveau_pm.h
@@ -61,8 +61,10 @@ int  nouveau_voltage_gpio_set(struct drm_device *, int voltage);
 /* nouveau_perf.c */
 void nouveau_perf_init(struct drm_device *);
 void nouveau_perf_fini(struct drm_device *);
-u8 *nouveau_perf_timing(struct drm_device *, u32 freq, u8 *ver, u8 *len);
+u8 *nouveau_perf_rammap(struct drm_device *, u32 freq, u8 *ver,
+			u8 *hdr, u8 *cnt, u8 *len);
 u8 *nouveau_perf_ramcfg(struct drm_device *, u32 freq, u8 *ver, u8 *len);
+u8 *nouveau_perf_timing(struct drm_device *, u32 freq, u8 *ver, u8 *len);
 
 /* nouveau_mem.c */
 void nouveau_mem_timing_init(struct drm_device *);
diff --git a/drivers/gpu/drm/nouveau/nouveau_prime.c b/drivers/gpu/drm/nouveau/nouveau_prime.c
new file mode 100644
index 00000000000..c58aab7370c
--- /dev/null
+++ b/drivers/gpu/drm/nouveau/nouveau_prime.c
@@ -0,0 +1,163 @@
+
+#include "drmP.h"
+#include "drm.h"
+
+#include "nouveau_drv.h"
+#include "nouveau_drm.h"
+#include "nouveau_dma.h"
+
+#include <linux/dma-buf.h>
+
+static struct sg_table *nouveau_gem_map_dma_buf(struct dma_buf_attachment *attachment,
+					  enum dma_data_direction dir)
+{
+	struct nouveau_bo *nvbo = attachment->dmabuf->priv;
+	struct drm_device *dev = nvbo->gem->dev;
+	int npages = nvbo->bo.num_pages;
+	struct sg_table *sg;
+	int nents;
+
+	mutex_lock(&dev->struct_mutex);
+	sg = drm_prime_pages_to_sg(nvbo->bo.ttm->pages, npages);
+	nents = dma_map_sg(attachment->dev, sg->sgl, sg->nents, dir);
+	mutex_unlock(&dev->struct_mutex);
+	return sg;
+}
+
+static void nouveau_gem_unmap_dma_buf(struct dma_buf_attachment *attachment,
+				      struct sg_table *sg, enum dma_data_direction dir)
+{
+	dma_unmap_sg(attachment->dev, sg->sgl, sg->nents, dir);
+	sg_free_table(sg);
+	kfree(sg);
+}
+
+static void nouveau_gem_dmabuf_release(struct dma_buf *dma_buf)
+{
+	struct nouveau_bo *nvbo = dma_buf->priv;
+
+	if (nvbo->gem->export_dma_buf == dma_buf) {
+		nvbo->gem->export_dma_buf = NULL;
+		drm_gem_object_unreference_unlocked(nvbo->gem);
+	}
+}
+
+static void *nouveau_gem_kmap_atomic(struct dma_buf *dma_buf, unsigned long page_num)
+{
+	return NULL;
+}
+
+static void nouveau_gem_kunmap_atomic(struct dma_buf *dma_buf, unsigned long page_num, void *addr)
+{
+
+}
+static void *nouveau_gem_kmap(struct dma_buf *dma_buf, unsigned long page_num)
+{
+	return NULL;
+}
+
+static void nouveau_gem_kunmap(struct dma_buf *dma_buf, unsigned long page_num, void *addr)
+{
+
+}
+
+static const struct dma_buf_ops nouveau_dmabuf_ops =  {
+	.map_dma_buf = nouveau_gem_map_dma_buf,
+	.unmap_dma_buf = nouveau_gem_unmap_dma_buf,
+	.release = nouveau_gem_dmabuf_release,
+	.kmap = nouveau_gem_kmap,
+	.kmap_atomic = nouveau_gem_kmap_atomic,
+	.kunmap = nouveau_gem_kunmap,
+	.kunmap_atomic = nouveau_gem_kunmap_atomic,
+};
+
+static int
+nouveau_prime_new(struct drm_device *dev,
+		  size_t size,
+		  struct sg_table *sg,
+		  struct nouveau_bo **pnvbo)
+{
+	struct nouveau_bo *nvbo;
+	u32 flags = 0;
+	int ret;
+
+	flags = TTM_PL_FLAG_TT;
+
+	ret = nouveau_bo_new(dev, size, 0, flags, 0, 0,
+			     sg, pnvbo);
+	if (ret)
+		return ret;
+	nvbo = *pnvbo;
+
+	/* we restrict allowed domains on nv50+ to only the types
+	 * that were requested at creation time.  not possibly on
+	 * earlier chips without busting the ABI.
+	 */
+	nvbo->valid_domains = NOUVEAU_GEM_DOMAIN_GART;
+	nvbo->gem = drm_gem_object_alloc(dev, nvbo->bo.mem.size);
+	if (!nvbo->gem) {
+		nouveau_bo_ref(NULL, pnvbo);
+		return -ENOMEM;
+	}
+
+	nvbo->gem->driver_private = nvbo;
+	return 0;
+}
+
+struct dma_buf *nouveau_gem_prime_export(struct drm_device *dev,
+				struct drm_gem_object *obj, int flags)
+{
+	struct nouveau_bo *nvbo = nouveau_gem_object(obj);
+	int ret = 0;
+
+	/* pin buffer into GTT */
+	ret = nouveau_bo_pin(nvbo, TTM_PL_FLAG_TT);
+	if (ret)
+		return ERR_PTR(-EINVAL);
+
+	return dma_buf_export(nvbo, &nouveau_dmabuf_ops, obj->size, flags);
+}
+
+struct drm_gem_object *nouveau_gem_prime_import(struct drm_device *dev,
+				struct dma_buf *dma_buf)
+{
+	struct dma_buf_attachment *attach;
+	struct sg_table *sg;
+	struct nouveau_bo *nvbo;
+	int ret;
+
+	if (dma_buf->ops == &nouveau_dmabuf_ops) {
+		nvbo = dma_buf->priv;
+		if (nvbo->gem) {
+			if (nvbo->gem->dev == dev) {
+				drm_gem_object_reference(nvbo->gem);
+				return nvbo->gem;
+			}
+		}
+	}
+	/* need to attach */
+	attach = dma_buf_attach(dma_buf, dev->dev);
+	if (IS_ERR(attach))
+		return ERR_PTR(PTR_ERR(attach));
+
+	sg = dma_buf_map_attachment(attach, DMA_BIDIRECTIONAL);
+	if (IS_ERR(sg)) {
+		ret = PTR_ERR(sg);
+		goto fail_detach;
+	}
+
+	ret = nouveau_prime_new(dev, dma_buf->size, sg, &nvbo);
+	if (ret)
+		goto fail_unmap;
+
+	nvbo->gem->import_attach = attach;
+
+	return nvbo->gem;
+
+fail_unmap:
+	dma_buf_unmap_attachment(attach, sg, DMA_BIDIRECTIONAL);
+fail_detach:
+	dma_buf_detach(dma_buf, attach);
+	return ERR_PTR(ret);
+}
+
diff --git a/drivers/gpu/drm/nouveau/nouveau_sgdma.c b/drivers/gpu/drm/nouveau/nouveau_sgdma.c
index 47f245edf53..38483a042bc 100644
--- a/drivers/gpu/drm/nouveau/nouveau_sgdma.c
+++ b/drivers/gpu/drm/nouveau/nouveau_sgdma.c
@@ -290,7 +290,10 @@ nv50_sgdma_bind(struct ttm_tt *ttm, struct ttm_mem_reg *mem)
 	struct nouveau_mem *node = mem->mm_node;
 
 	/* noop: bound in move_notify() */
-	node->pages = nvbe->ttm.dma_address;
+	if (ttm->sg) {
+		node->sg = ttm->sg;
+	} else
+		node->pages = nvbe->ttm.dma_address;
 	return 0;
 }
 
@@ -338,10 +341,10 @@ nouveau_sgdma_init(struct drm_device *dev)
 	u32 aper_size, align;
 	int ret;
 
-	if (dev_priv->card_type >= NV_40 && pci_is_pcie(dev->pdev))
+	if (dev_priv->card_type >= NV_40)
 		aper_size = 512 * 1024 * 1024;
 	else
-		aper_size = 64 * 1024 * 1024;
+		aper_size = 128 * 1024 * 1024;
 
 	/* Dear NVIDIA, NV44+ would like proper present bits in PTEs for
 	 * christmas.  The cards before it have them, the cards after
diff --git a/drivers/gpu/drm/nouveau/nouveau_software.h b/drivers/gpu/drm/nouveau/nouveau_software.h
new file mode 100644
index 00000000000..e60bc6ce900
--- /dev/null
+++ b/drivers/gpu/drm/nouveau/nouveau_software.h
@@ -0,0 +1,69 @@
+#ifndef __NOUVEAU_SOFTWARE_H__
+#define __NOUVEAU_SOFTWARE_H__
+
+struct nouveau_software_priv {
+	struct nouveau_exec_engine base;
+	struct list_head vblank;
+};
+
+struct nouveau_software_chan {
+	struct list_head flip;
+	struct {
+		struct list_head list;
+		struct nouveau_bo *bo;
+		u32 offset;
+		u32 value;
+		u32 head;
+	} vblank;
+};
+
+static inline void
+nouveau_software_vblank(struct drm_device *dev, int crtc)
+{
+	struct nouveau_software_priv *psw = nv_engine(dev, NVOBJ_ENGINE_SW);
+	struct nouveau_software_chan *pch, *tmp;
+
+	list_for_each_entry_safe(pch, tmp, &psw->vblank, vblank.list) {
+		if (pch->vblank.head != crtc)
+			continue;
+
+		nouveau_bo_wr32(pch->vblank.bo, pch->vblank.offset,
+						pch->vblank.value);
+		list_del(&pch->vblank.list);
+		drm_vblank_put(dev, crtc);
+	}
+}
+
+static inline void
+nouveau_software_context_new(struct nouveau_software_chan *pch)
+{
+	INIT_LIST_HEAD(&pch->flip);
+}
+
+static inline void
+nouveau_software_create(struct nouveau_software_priv *psw)
+{
+	INIT_LIST_HEAD(&psw->vblank);
+}
+
+static inline u16
+nouveau_software_class(struct drm_device *dev)
+{
+	struct drm_nouveau_private *dev_priv = dev->dev_private;
+	if (dev_priv->card_type <= NV_04)
+		return 0x006e;
+	if (dev_priv->card_type <= NV_40)
+		return 0x016e;
+	if (dev_priv->card_type <= NV_50)
+		return 0x506e;
+	if (dev_priv->card_type <= NV_E0)
+		return 0x906e;
+	return 0x0000;
+}
+
+int nv04_software_create(struct drm_device *);
+int nv50_software_create(struct drm_device *);
+int nvc0_software_create(struct drm_device *);
+u64 nvc0_software_crtc(struct nouveau_channel *, int crtc);
+
+#endif
diff --git a/drivers/gpu/drm/nouveau/nouveau_state.c b/drivers/gpu/drm/nouveau/nouveau_state.c
index c2a8511e855..19706f0532e 100644
--- a/drivers/gpu/drm/nouveau/nouveau_state.c
+++ b/drivers/gpu/drm/nouveau/nouveau_state.c
@@ -39,6 +39,9 @@
 #include "nouveau_gpio.h"
 #include "nouveau_pm.h"
 #include "nv50_display.h"
+#include "nouveau_fifo.h"
+#include "nouveau_fence.h"
+#include "nouveau_software.h"
 
 static void nouveau_stub_takedown(struct drm_device *dev) {}
 static int nouveau_stub_init(struct drm_device *dev) { return 0; }
@@ -66,18 +69,6 @@ static int nouveau_init_engine_ptrs(struct drm_device *dev)
 		engine->timer.takedown		= nv04_timer_takedown;
 		engine->fb.init			= nv04_fb_init;
 		engine->fb.takedown		= nv04_fb_takedown;
-		engine->fifo.channels		= 16;
-		engine->fifo.init		= nv04_fifo_init;
-		engine->fifo.takedown		= nv04_fifo_fini;
-		engine->fifo.disable		= nv04_fifo_disable;
-		engine->fifo.enable		= nv04_fifo_enable;
-		engine->fifo.reassign		= nv04_fifo_reassign;
-		engine->fifo.cache_pull		= nv04_fifo_cache_pull;
-		engine->fifo.channel_id		= nv04_fifo_channel_id;
-		engine->fifo.create_context	= nv04_fifo_create_context;
-		engine->fifo.destroy_context	= nv04_fifo_destroy_context;
-		engine->fifo.load_context	= nv04_fifo_load_context;
-		engine->fifo.unload_context	= nv04_fifo_unload_context;
 		engine->display.early_init	= nv04_display_early_init;
 		engine->display.late_takedown	= nv04_display_late_takedown;
 		engine->display.create		= nv04_display_create;
@@ -111,18 +102,6 @@ static int nouveau_init_engine_ptrs(struct drm_device *dev)
 		engine->fb.init_tile_region	= nv10_fb_init_tile_region;
 		engine->fb.set_tile_region	= nv10_fb_set_tile_region;
 		engine->fb.free_tile_region	= nv10_fb_free_tile_region;
-		engine->fifo.channels		= 32;
-		engine->fifo.init		= nv10_fifo_init;
-		engine->fifo.takedown		= nv04_fifo_fini;
-		engine->fifo.disable		= nv04_fifo_disable;
-		engine->fifo.enable		= nv04_fifo_enable;
-		engine->fifo.reassign		= nv04_fifo_reassign;
-		engine->fifo.cache_pull		= nv04_fifo_cache_pull;
-		engine->fifo.channel_id		= nv10_fifo_channel_id;
-		engine->fifo.create_context	= nv10_fifo_create_context;
-		engine->fifo.destroy_context	= nv04_fifo_destroy_context;
-		engine->fifo.load_context	= nv10_fifo_load_context;
-		engine->fifo.unload_context	= nv10_fifo_unload_context;
 		engine->display.early_init	= nv04_display_early_init;
 		engine->display.late_takedown	= nv04_display_late_takedown;
 		engine->display.create		= nv04_display_create;
@@ -162,18 +141,6 @@ static int nouveau_init_engine_ptrs(struct drm_device *dev)
 		engine->fb.init_tile_region	= nv20_fb_init_tile_region;
 		engine->fb.set_tile_region	= nv20_fb_set_tile_region;
 		engine->fb.free_tile_region	= nv20_fb_free_tile_region;
-		engine->fifo.channels		= 32;
-		engine->fifo.init		= nv10_fifo_init;
-		engine->fifo.takedown		= nv04_fifo_fini;
-		engine->fifo.disable		= nv04_fifo_disable;
-		engine->fifo.enable		= nv04_fifo_enable;
-		engine->fifo.reassign		= nv04_fifo_reassign;
-		engine->fifo.cache_pull		= nv04_fifo_cache_pull;
-		engine->fifo.channel_id		= nv10_fifo_channel_id;
-		engine->fifo.create_context	= nv10_fifo_create_context;
-		engine->fifo.destroy_context	= nv04_fifo_destroy_context;
-		engine->fifo.load_context	= nv10_fifo_load_context;
-		engine->fifo.unload_context	= nv10_fifo_unload_context;
 		engine->display.early_init	= nv04_display_early_init;
 		engine->display.late_takedown	= nv04_display_late_takedown;
 		engine->display.create		= nv04_display_create;
@@ -209,18 +176,6 @@ static int nouveau_init_engine_ptrs(struct drm_device *dev)
 		engine->fb.init_tile_region	= nv30_fb_init_tile_region;
 		engine->fb.set_tile_region	= nv10_fb_set_tile_region;
 		engine->fb.free_tile_region	= nv30_fb_free_tile_region;
-		engine->fifo.channels		= 32;
-		engine->fifo.init		= nv10_fifo_init;
-		engine->fifo.takedown		= nv04_fifo_fini;
-		engine->fifo.disable		= nv04_fifo_disable;
-		engine->fifo.enable		= nv04_fifo_enable;
-		engine->fifo.reassign		= nv04_fifo_reassign;
-		engine->fifo.cache_pull		= nv04_fifo_cache_pull;
-		engine->fifo.channel_id		= nv10_fifo_channel_id;
-		engine->fifo.create_context	= nv10_fifo_create_context;
-		engine->fifo.destroy_context	= nv04_fifo_destroy_context;
-		engine->fifo.load_context	= nv10_fifo_load_context;
-		engine->fifo.unload_context	= nv10_fifo_unload_context;
 		engine->display.early_init	= nv04_display_early_init;
 		engine->display.late_takedown	= nv04_display_late_takedown;
 		engine->display.create		= nv04_display_create;
@@ -259,18 +214,6 @@ static int nouveau_init_engine_ptrs(struct drm_device *dev)
 		engine->fb.init_tile_region	= nv30_fb_init_tile_region;
 		engine->fb.set_tile_region	= nv40_fb_set_tile_region;
 		engine->fb.free_tile_region	= nv30_fb_free_tile_region;
-		engine->fifo.channels		= 32;
-		engine->fifo.init		= nv40_fifo_init;
-		engine->fifo.takedown		= nv04_fifo_fini;
-		engine->fifo.disable		= nv04_fifo_disable;
-		engine->fifo.enable		= nv04_fifo_enable;
-		engine->fifo.reassign		= nv04_fifo_reassign;
-		engine->fifo.cache_pull		= nv04_fifo_cache_pull;
-		engine->fifo.channel_id		= nv10_fifo_channel_id;
-		engine->fifo.create_context	= nv40_fifo_create_context;
-		engine->fifo.destroy_context	= nv04_fifo_destroy_context;
-		engine->fifo.load_context	= nv40_fifo_load_context;
-		engine->fifo.unload_context	= nv40_fifo_unload_context;
 		engine->display.early_init	= nv04_display_early_init;
 		engine->display.late_takedown	= nv04_display_late_takedown;
 		engine->display.create		= nv04_display_create;
@@ -317,18 +260,6 @@ static int nouveau_init_engine_ptrs(struct drm_device *dev)
 		engine->timer.takedown		= nv04_timer_takedown;
 		engine->fb.init			= nv50_fb_init;
 		engine->fb.takedown		= nv50_fb_takedown;
-		engine->fifo.channels		= 128;
-		engine->fifo.init		= nv50_fifo_init;
-		engine->fifo.takedown		= nv50_fifo_takedown;
-		engine->fifo.disable		= nv04_fifo_disable;
-		engine->fifo.enable		= nv04_fifo_enable;
-		engine->fifo.reassign		= nv04_fifo_reassign;
-		engine->fifo.channel_id		= nv50_fifo_channel_id;
-		engine->fifo.create_context	= nv50_fifo_create_context;
-		engine->fifo.destroy_context	= nv50_fifo_destroy_context;
-		engine->fifo.load_context	= nv50_fifo_load_context;
-		engine->fifo.unload_context	= nv50_fifo_unload_context;
-		engine->fifo.tlb_flush		= nv50_fifo_tlb_flush;
 		engine->display.early_init	= nv50_display_early_init;
 		engine->display.late_takedown	= nv50_display_late_takedown;
 		engine->display.create		= nv50_display_create;
@@ -392,17 +323,6 @@ static int nouveau_init_engine_ptrs(struct drm_device *dev)
 		engine->timer.takedown		= nv04_timer_takedown;
 		engine->fb.init			= nvc0_fb_init;
 		engine->fb.takedown		= nvc0_fb_takedown;
-		engine->fifo.channels		= 128;
-		engine->fifo.init		= nvc0_fifo_init;
-		engine->fifo.takedown		= nvc0_fifo_takedown;
-		engine->fifo.disable		= nvc0_fifo_disable;
-		engine->fifo.enable		= nvc0_fifo_enable;
-		engine->fifo.reassign		= nvc0_fifo_reassign;
-		engine->fifo.channel_id		= nvc0_fifo_channel_id;
-		engine->fifo.create_context	= nvc0_fifo_create_context;
-		engine->fifo.destroy_context	= nvc0_fifo_destroy_context;
-		engine->fifo.load_context	= nvc0_fifo_load_context;
-		engine->fifo.unload_context	= nvc0_fifo_unload_context;
 		engine->display.early_init	= nv50_display_early_init;
 		engine->display.late_takedown	= nv50_display_late_takedown;
 		engine->display.create		= nv50_display_create;
@@ -445,17 +365,6 @@ static int nouveau_init_engine_ptrs(struct drm_device *dev)
 		engine->timer.takedown		= nv04_timer_takedown;
 		engine->fb.init			= nvc0_fb_init;
 		engine->fb.takedown		= nvc0_fb_takedown;
-		engine->fifo.channels		= 128;
-		engine->fifo.init		= nvc0_fifo_init;
-		engine->fifo.takedown		= nvc0_fifo_takedown;
-		engine->fifo.disable		= nvc0_fifo_disable;
-		engine->fifo.enable		= nvc0_fifo_enable;
-		engine->fifo.reassign		= nvc0_fifo_reassign;
-		engine->fifo.channel_id		= nvc0_fifo_channel_id;
-		engine->fifo.create_context	= nvc0_fifo_create_context;
-		engine->fifo.destroy_context	= nvc0_fifo_destroy_context;
-		engine->fifo.load_context	= nvc0_fifo_load_context;
-		engine->fifo.unload_context	= nvc0_fifo_unload_context;
 		engine->display.early_init	= nouveau_stub_init;
 		engine->display.late_takedown	= nouveau_stub_takedown;
 		engine->display.create		= nvd0_display_create;
@@ -496,13 +405,6 @@ static int nouveau_init_engine_ptrs(struct drm_device *dev)
 		engine->timer.takedown		= nv04_timer_takedown;
 		engine->fb.init			= nvc0_fb_init;
 		engine->fb.takedown		= nvc0_fb_takedown;
-		engine->fifo.channels		= 0;
-		engine->fifo.init		= nouveau_stub_init;
-		engine->fifo.takedown		= nouveau_stub_takedown;
-		engine->fifo.disable		= nvc0_fifo_disable;
-		engine->fifo.enable		= nvc0_fifo_enable;
-		engine->fifo.reassign		= nvc0_fifo_reassign;
-		engine->fifo.unload_context	= nouveau_stub_init;
 		engine->display.early_init	= nouveau_stub_init;
 		engine->display.late_takedown	= nouveau_stub_takedown;
 		engine->display.create		= nvd0_display_create;
@@ -607,61 +509,24 @@ nouveau_card_channel_init(struct drm_device *dev)
 {
 	struct drm_nouveau_private *dev_priv = dev->dev_private;
 	struct nouveau_channel *chan;
-	int ret, oclass;
+	int ret;
 
 	ret = nouveau_channel_alloc(dev, &chan, NULL, NvDmaFB, NvDmaTT);
 	dev_priv->channel = chan;
 	if (ret)
 		return ret;
-
 	mutex_unlock(&dev_priv->channel->mutex);
 
-	if (dev_priv->card_type <= NV_50) {
-		if (dev_priv->card_type < NV_50)
-			oclass = 0x0039;
-		else
-			oclass = 0x5039;
-
-		ret = nouveau_gpuobj_gr_new(chan, NvM2MF, oclass);
-		if (ret)
-			goto error;
-
-		ret = nouveau_notifier_alloc(chan, NvNotify0, 32, 0xfe0, 0x1000,
-					     &chan->m2mf_ntfy);
-		if (ret)
-			goto error;
-
-		ret = RING_SPACE(chan, 6);
-		if (ret)
-			goto error;
-
-		BEGIN_RING(chan, NvSubM2MF, NV_MEMORY_TO_MEMORY_FORMAT_NAME, 1);
-		OUT_RING  (chan, NvM2MF);
-		BEGIN_RING(chan, NvSubM2MF, NV_MEMORY_TO_MEMORY_FORMAT_DMA_NOTIFY, 3);
-		OUT_RING  (chan, NvNotify0);
-		OUT_RING  (chan, chan->vram_handle);
-		OUT_RING  (chan, chan->gart_handle);
-	} else
-	if (dev_priv->card_type <= NV_D0) {
-		ret = nouveau_gpuobj_gr_new(chan, 0x9039, 0x9039);
-		if (ret)
-			goto error;
-
-		ret = RING_SPACE(chan, 2);
-		if (ret)
-			goto error;
-
-		BEGIN_NVC0(chan, 2, NvSubM2MF, 0x0000, 1);
-		OUT_RING  (chan, 0x00009039);
-	}
-
-	FIRE_RING (chan);
-error:
-	if (ret)
-		nouveau_card_channel_fini(dev);
-	return ret;
+	nouveau_bo_move_init(chan);
+	return 0;
 }
 
+static const struct vga_switcheroo_client_ops nouveau_switcheroo_ops = {
+	.set_gpu_state = nouveau_switcheroo_set_state,
+	.reprobe = nouveau_switcheroo_reprobe,
+	.can_switch = nouveau_switcheroo_can_switch,
+};
+
 int
 nouveau_card_init(struct drm_device *dev)
 {
@@ -670,9 +535,7 @@ nouveau_card_init(struct drm_device *dev)
 	int ret, e = 0;
 
 	vga_client_register(dev->pdev, dev, NULL, nouveau_vga_set_decode);
-	vga_switcheroo_register_client(dev->pdev, nouveau_switcheroo_set_state,
-				       nouveau_switcheroo_reprobe,
-				       nouveau_switcheroo_can_switch);
+	vga_switcheroo_register_client(dev->pdev, &nouveau_switcheroo_ops);
 
 	/* Initialise internal driver API hooks */
 	ret = nouveau_init_engine_ptrs(dev);
@@ -745,6 +608,81 @@ nouveau_card_init(struct drm_device *dev)
 	if (!dev_priv->noaccel) {
 		switch (dev_priv->card_type) {
 		case NV_04:
+			nv04_fifo_create(dev);
+			break;
+		case NV_10:
+		case NV_20:
+		case NV_30:
+			if (dev_priv->chipset < 0x17)
+				nv10_fifo_create(dev);
+			else
+				nv17_fifo_create(dev);
+			break;
+		case NV_40:
+			nv40_fifo_create(dev);
+			break;
+		case NV_50:
+			if (dev_priv->chipset == 0x50)
+				nv50_fifo_create(dev);
+			else
+				nv84_fifo_create(dev);
+			break;
+		case NV_C0:
+		case NV_D0:
+			nvc0_fifo_create(dev);
+			break;
+		case NV_E0:
+			nve0_fifo_create(dev);
+			break;
+		default:
+			break;
+		}
+
+		switch (dev_priv->card_type) {
+		case NV_04:
+			nv04_fence_create(dev);
+			break;
+		case NV_10:
+		case NV_20:
+		case NV_30:
+		case NV_40:
+		case NV_50:
+			if (dev_priv->chipset < 0x84)
+				nv10_fence_create(dev);
+			else
+				nv84_fence_create(dev);
+			break;
+		case NV_C0:
+		case NV_D0:
+		case NV_E0:
+			nvc0_fence_create(dev);
+			break;
+		default:
+			break;
+		}
+
+		switch (dev_priv->card_type) {
+		case NV_04:
+		case NV_10:
+		case NV_20:
+		case NV_30:
+		case NV_40:
+			nv04_software_create(dev);
+			break;
+		case NV_50:
+			nv50_software_create(dev);
+			break;
+		case NV_C0:
+		case NV_D0:
+		case NV_E0:
+			nvc0_software_create(dev);
+			break;
+		default:
+			break;
+		}
+
+		switch (dev_priv->card_type) {
+		case NV_04:
 			nv04_graph_create(dev);
 			break;
 		case NV_10:
@@ -764,6 +702,9 @@ nouveau_card_init(struct drm_device *dev)
 		case NV_D0:
 			nvc0_graph_create(dev);
 			break;
+		case NV_E0:
+			nve0_graph_create(dev);
+			break;
 		default:
 			break;
 		}
@@ -796,8 +737,9 @@ nouveau_card_init(struct drm_device *dev)
 			}
 			break;
 		case NV_C0:
-			nvc0_copy_create(dev, 0);
 			nvc0_copy_create(dev, 1);
+		case NV_D0:
+			nvc0_copy_create(dev, 0);
 			break;
 		default:
 			break;
@@ -830,16 +772,11 @@ nouveau_card_init(struct drm_device *dev)
 					goto out_engine;
 			}
 		}
-
-		/* PFIFO */
-		ret = engine->fifo.init(dev);
-		if (ret)
-			goto out_engine;
 	}
 
 	ret = nouveau_irq_init(dev);
 	if (ret)
-		goto out_fifo;
+		goto out_engine;
 
 	ret = nouveau_display_create(dev);
 	if (ret)
@@ -848,14 +785,10 @@ nouveau_card_init(struct drm_device *dev)
 	nouveau_backlight_init(dev);
 	nouveau_pm_init(dev);
 
-	ret = nouveau_fence_init(dev);
-	if (ret)
-		goto out_pm;
-
 	if (dev_priv->eng[NVOBJ_ENGINE_GR]) {
 		ret = nouveau_card_channel_init(dev);
 		if (ret)
-			goto out_fence;
+			goto out_pm;
 	}
 
 	if (dev->mode_config.num_crtc) {
@@ -870,17 +803,12 @@ nouveau_card_init(struct drm_device *dev)
 
 out_chan:
 	nouveau_card_channel_fini(dev);
-out_fence:
-	nouveau_fence_fini(dev);
 out_pm:
 	nouveau_pm_fini(dev);
 	nouveau_backlight_exit(dev);
 	nouveau_display_destroy(dev);
 out_irq:
 	nouveau_irq_fini(dev);
-out_fifo:
-	if (!dev_priv->noaccel)
-		engine->fifo.takedown(dev);
 out_engine:
 	if (!dev_priv->noaccel) {
 		for (e = e - 1; e >= 0; e--) {
@@ -912,6 +840,7 @@ out_bios:
 out_display_early:
 	engine->display.late_takedown(dev);
 out:
+	vga_switcheroo_unregister_client(dev->pdev);
 	vga_client_register(dev->pdev, NULL, NULL, NULL);
 	return ret;
 }
@@ -928,13 +857,11 @@ static void nouveau_card_takedown(struct drm_device *dev)
 	}
 
 	nouveau_card_channel_fini(dev);
-	nouveau_fence_fini(dev);
 	nouveau_pm_fini(dev);
 	nouveau_backlight_exit(dev);
 	nouveau_display_destroy(dev);
 
 	if (!dev_priv->noaccel) {
-		engine->fifo.takedown(dev);
 		for (e = NVOBJ_ENGINE_NR - 1; e >= 0; e--) {
 			if (dev_priv->eng[e]) {
 				dev_priv->eng[e]->fini(dev, e, false);
@@ -969,6 +896,7 @@ static void nouveau_card_takedown(struct drm_device *dev)
 
 	nouveau_irq_fini(dev);
 
+	vga_switcheroo_unregister_client(dev->pdev);
 	vga_client_register(dev->pdev, NULL, NULL, NULL);
 }
 
@@ -1176,7 +1104,7 @@ int nouveau_load(struct drm_device *dev, unsigned long flags)
 		goto err_priv;
 	}
 
-	NV_INFO(dev, "Detected an NV%2x generation card (0x%08x)\n",
+	NV_INFO(dev, "Detected an NV%02x generation card (0x%08x)\n",
 		     dev_priv->card_type, reg0);
 
 	/* map the mmio regs, limiting the amount to preserve vmap space */
@@ -1219,6 +1147,8 @@ int nouveau_load(struct drm_device *dev, unsigned long flags)
 	if (nouveau_noaccel == -1) {
 		switch (dev_priv->chipset) {
 		case 0xd9: /* known broken */
+		case 0xe4: /* needs binary driver firmware */
+		case 0xe7: /* needs binary driver firmware */
 			NV_INFO(dev, "acceleration disabled by default, pass "
 				     "noaccel=0 to force enable\n");
 			dev_priv->noaccel = true;
diff --git a/drivers/gpu/drm/nouveau/nouveau_vm.c b/drivers/gpu/drm/nouveau/nouveau_vm.c
index 2bf6c0350b4..11edd5e91a0 100644
--- a/drivers/gpu/drm/nouveau/nouveau_vm.c
+++ b/drivers/gpu/drm/nouveau/nouveau_vm.c
@@ -77,6 +77,63 @@ nouveau_vm_map(struct nouveau_vma *vma, struct nouveau_mem *node)
 }
 
 void
+nouveau_vm_map_sg_table(struct nouveau_vma *vma, u64 delta, u64 length,
+			struct nouveau_mem *mem)
+{
+	struct nouveau_vm *vm = vma->vm;
+	int big = vma->node->type != vm->spg_shift;
+	u32 offset = vma->node->offset + (delta >> 12);
+	u32 bits = vma->node->type - 12;
+	u32 num  = length >> vma->node->type;
+	u32 pde  = (offset >> vm->pgt_bits) - vm->fpde;
+	u32 pte  = (offset & ((1 << vm->pgt_bits) - 1)) >> bits;
+	u32 max  = 1 << (vm->pgt_bits - bits);
+	unsigned m, sglen;
+	u32 end, len;
+	int i;
+	struct scatterlist *sg;
+
+	for_each_sg(mem->sg->sgl, sg, mem->sg->nents, i) {
+		struct nouveau_gpuobj *pgt = vm->pgt[pde].obj[big];
+		sglen = sg_dma_len(sg) >> PAGE_SHIFT;
+
+		end = pte + sglen;
+		if (unlikely(end >= max))
+			end = max;
+		len = end - pte;
+
+		for (m = 0; m < len; m++) {
+			dma_addr_t addr = sg_dma_address(sg) + (m << PAGE_SHIFT);
+
+			vm->map_sg(vma, pgt, mem, pte, 1, &addr);
+			num--;
+			pte++;
+
+			if (num == 0)
+				goto finish;
+		}
+		if (unlikely(end >= max)) {
+			pde++;
+			pte = 0;
+		}
+		if (m < sglen) {
+			for (; m < sglen; m++) {
+				dma_addr_t addr = sg_dma_address(sg) + (m << PAGE_SHIFT);
+
+				vm->map_sg(vma, pgt, mem, pte, 1, &addr);
+				num--;
+				pte++;
+				if (num == 0)
+					goto finish;
+			}
+		}
+
+	}
+finish:
+	vm->flush(vm);
+}
+
+void
 nouveau_vm_map_sg(struct nouveau_vma *vma, u64 delta, u64 length,
 		  struct nouveau_mem *mem)
 {
diff --git a/drivers/gpu/drm/nouveau/nouveau_vm.h b/drivers/gpu/drm/nouveau/nouveau_vm.h
index 4fb6e728734..a8246e7e4a8 100644
--- a/drivers/gpu/drm/nouveau/nouveau_vm.h
+++ b/drivers/gpu/drm/nouveau/nouveau_vm.h
@@ -72,6 +72,9 @@ struct nouveau_vm {
 		    u64 phys, u64 delta);
 	void (*map_sg)(struct nouveau_vma *, struct nouveau_gpuobj *,
 		       struct nouveau_mem *, u32 pte, u32 cnt, dma_addr_t *);
+
+	void (*map_sg_table)(struct nouveau_vma *, struct nouveau_gpuobj *,
+			     struct nouveau_mem *, u32 pte, u32 cnt, dma_addr_t *);
 	void (*unmap)(struct nouveau_gpuobj *pgt, u32 pte, u32 cnt);
 	void (*flush)(struct nouveau_vm *);
 };
@@ -90,7 +93,8 @@ void nouveau_vm_unmap(struct nouveau_vma *);
 void nouveau_vm_unmap_at(struct nouveau_vma *, u64 offset, u64 length);
 void nouveau_vm_map_sg(struct nouveau_vma *, u64 offset, u64 length,
 		       struct nouveau_mem *);
-
+void nouveau_vm_map_sg_table(struct nouveau_vma *vma, u64 delta, u64 length,
+			     struct nouveau_mem *mem);
 /* nv50_vm.c */
 void nv50_vm_map_pgt(struct nouveau_gpuobj *pgd, u32 pde,
 		     struct nouveau_gpuobj *pgt[2]);
diff --git a/drivers/gpu/drm/nouveau/nv04_crtc.c b/drivers/gpu/drm/nouveau/nv04_crtc.c
index 728d07584d3..4c31c63e552 100644
--- a/drivers/gpu/drm/nouveau/nv04_crtc.c
+++ b/drivers/gpu/drm/nouveau/nv04_crtc.c
@@ -1047,7 +1047,7 @@ nv04_crtc_create(struct drm_device *dev, int crtc_num)
 	drm_mode_crtc_set_gamma_size(&nv_crtc->base, 256);
 
 	ret = nouveau_bo_new(dev, 64*64*4, 0x100, TTM_PL_FLAG_VRAM,
-			     0, 0x0000, &nv_crtc->cursor.nvbo);
+			     0, 0x0000, NULL, &nv_crtc->cursor.nvbo);
 	if (!ret) {
 		ret = nouveau_bo_pin(nv_crtc->cursor.nvbo, TTM_PL_FLAG_VRAM);
 		if (!ret)
diff --git a/drivers/gpu/drm/nouveau/nv04_display.c b/drivers/gpu/drm/nouveau/nv04_display.c
index 7047d37e8da..44488e3a257 100644
--- a/drivers/gpu/drm/nouveau/nv04_display.c
+++ b/drivers/gpu/drm/nouveau/nv04_display.c
@@ -98,6 +98,13 @@ nv04_display_early_init(struct drm_device *dev)
 		NVSetOwner(dev, 0);
 	}
 
+	/* ensure vblank interrupts are off, they can't be enabled until
+	 * drm_vblank has been initialised
+	 */
+	NVWriteCRTC(dev, 0, NV_PCRTC_INTR_EN_0, 0);
+	if (nv_two_heads(dev))
+		NVWriteCRTC(dev, 1, NV_PCRTC_INTR_EN_0, 0);
+
 	return 0;
 }
 
@@ -246,6 +253,10 @@ nv04_display_init(struct drm_device *dev)
 void
 nv04_display_fini(struct drm_device *dev)
 {
+	/* disable vblank interrupts */
+	NVWriteCRTC(dev, 0, NV_PCRTC_INTR_EN_0, 0);
+	if (nv_two_heads(dev))
+		NVWriteCRTC(dev, 1, NV_PCRTC_INTR_EN_0, 0);
 }
 
 static void
diff --git a/drivers/gpu/drm/nouveau/nv04_fbcon.c b/drivers/gpu/drm/nouveau/nv04_fbcon.c
index 7a118937109..7cd7857347e 100644
--- a/drivers/gpu/drm/nouveau/nv04_fbcon.c
+++ b/drivers/gpu/drm/nouveau/nv04_fbcon.c
@@ -41,7 +41,7 @@ nv04_fbcon_copyarea(struct fb_info *info, const struct fb_copyarea *region)
 	if (ret)
 		return ret;
 
-	BEGIN_RING(chan, NvSubImageBlit, 0x0300, 3);
+	BEGIN_NV04(chan, NvSubImageBlit, 0x0300, 3);
 	OUT_RING(chan, (region->sy << 16) | region->sx);
 	OUT_RING(chan, (region->dy << 16) | region->dx);
 	OUT_RING(chan, (region->height << 16) | region->width);
@@ -62,15 +62,15 @@ nv04_fbcon_fillrect(struct fb_info *info, const struct fb_fillrect *rect)
 	if (ret)
 		return ret;
 
-	BEGIN_RING(chan, NvSubGdiRect, 0x02fc, 1);
+	BEGIN_NV04(chan, NvSubGdiRect, 0x02fc, 1);
 	OUT_RING(chan, (rect->rop != ROP_COPY) ? 1 : 3);
-	BEGIN_RING(chan, NvSubGdiRect, 0x03fc, 1);
+	BEGIN_NV04(chan, NvSubGdiRect, 0x03fc, 1);
 	if (info->fix.visual == FB_VISUAL_TRUECOLOR ||
 	    info->fix.visual == FB_VISUAL_DIRECTCOLOR)
 		OUT_RING(chan, ((uint32_t *)info->pseudo_palette)[rect->color]);
 	else
 		OUT_RING(chan, rect->color);
-	BEGIN_RING(chan, NvSubGdiRect, 0x0400, 2);
+	BEGIN_NV04(chan, NvSubGdiRect, 0x0400, 2);
 	OUT_RING(chan, (rect->dx << 16) | rect->dy);
 	OUT_RING(chan, (rect->width << 16) | rect->height);
 	FIRE_RING(chan);
@@ -110,7 +110,7 @@ nv04_fbcon_imageblit(struct fb_info *info, const struct fb_image *image)
 		bg = image->bg_color;
 	}
 
-	BEGIN_RING(chan, NvSubGdiRect, 0x0be4, 7);
+	BEGIN_NV04(chan, NvSubGdiRect, 0x0be4, 7);
 	OUT_RING(chan, (image->dy << 16) | (image->dx & 0xffff));
 	OUT_RING(chan, ((image->dy + image->height) << 16) |
 			 ((image->dx + image->width) & 0xffff));
@@ -127,7 +127,7 @@ nv04_fbcon_imageblit(struct fb_info *info, const struct fb_image *image)
 		if (ret)
 			return ret;
 
-		BEGIN_RING(chan, NvSubGdiRect, 0x0c00, iter_len);
+		BEGIN_NV04(chan, NvSubGdiRect, 0x0c00, iter_len);
 		OUT_RINGp(chan, data, iter_len);
 		data += iter_len;
 		dsize -= iter_len;
@@ -209,25 +209,25 @@ nv04_fbcon_accel_init(struct fb_info *info)
 		return 0;
 	}
 
-	BEGIN_RING(chan, sub, 0x0000, 1);
+	BEGIN_NV04(chan, sub, 0x0000, 1);
 	OUT_RING(chan, NvCtxSurf2D);
-	BEGIN_RING(chan, sub, 0x0184, 2);
+	BEGIN_NV04(chan, sub, 0x0184, 2);
 	OUT_RING(chan, NvDmaFB);
 	OUT_RING(chan, NvDmaFB);
-	BEGIN_RING(chan, sub, 0x0300, 4);
+	BEGIN_NV04(chan, sub, 0x0300, 4);
 	OUT_RING(chan, surface_fmt);
 	OUT_RING(chan, info->fix.line_length | (info->fix.line_length << 16));
 	OUT_RING(chan, info->fix.smem_start - dev->mode_config.fb_base);
 	OUT_RING(chan, info->fix.smem_start - dev->mode_config.fb_base);
 
-	BEGIN_RING(chan, sub, 0x0000, 1);
+	BEGIN_NV04(chan, sub, 0x0000, 1);
 	OUT_RING(chan, NvRop);
-	BEGIN_RING(chan, sub, 0x0300, 1);
+	BEGIN_NV04(chan, sub, 0x0300, 1);
 	OUT_RING(chan, 0x55);
 
-	BEGIN_RING(chan, sub, 0x0000, 1);
+	BEGIN_NV04(chan, sub, 0x0000, 1);
 	OUT_RING(chan, NvImagePatt);
-	BEGIN_RING(chan, sub, 0x0300, 8);
+	BEGIN_NV04(chan, sub, 0x0300, 8);
 	OUT_RING(chan, pattern_fmt);
 #ifdef __BIG_ENDIAN
 	OUT_RING(chan, 2);
@@ -241,31 +241,31 @@ nv04_fbcon_accel_init(struct fb_info *info)
 	OUT_RING(chan, ~0);
 	OUT_RING(chan, ~0);
 
-	BEGIN_RING(chan, sub, 0x0000, 1);
+	BEGIN_NV04(chan, sub, 0x0000, 1);
 	OUT_RING(chan, NvClipRect);
-	BEGIN_RING(chan, sub, 0x0300, 2);
+	BEGIN_NV04(chan, sub, 0x0300, 2);
 	OUT_RING(chan, 0);
 	OUT_RING(chan, (info->var.yres_virtual << 16) | info->var.xres_virtual);
 
-	BEGIN_RING(chan, NvSubImageBlit, 0x0000, 1);
+	BEGIN_NV04(chan, NvSubImageBlit, 0x0000, 1);
 	OUT_RING(chan, NvImageBlit);
-	BEGIN_RING(chan, NvSubImageBlit, 0x019c, 1);
+	BEGIN_NV04(chan, NvSubImageBlit, 0x019c, 1);
 	OUT_RING(chan, NvCtxSurf2D);
-	BEGIN_RING(chan, NvSubImageBlit, 0x02fc, 1);
+	BEGIN_NV04(chan, NvSubImageBlit, 0x02fc, 1);
 	OUT_RING(chan, 3);
 
-	BEGIN_RING(chan, NvSubGdiRect, 0x0000, 1);
+	BEGIN_NV04(chan, NvSubGdiRect, 0x0000, 1);
 	OUT_RING(chan, NvGdiRect);
-	BEGIN_RING(chan, NvSubGdiRect, 0x0198, 1);
+	BEGIN_NV04(chan, NvSubGdiRect, 0x0198, 1);
 	OUT_RING(chan, NvCtxSurf2D);
-	BEGIN_RING(chan, NvSubGdiRect, 0x0188, 2);
+	BEGIN_NV04(chan, NvSubGdiRect, 0x0188, 2);
 	OUT_RING(chan, NvImagePatt);
 	OUT_RING(chan, NvRop);
-	BEGIN_RING(chan, NvSubGdiRect, 0x0304, 1);
+	BEGIN_NV04(chan, NvSubGdiRect, 0x0304, 1);
 	OUT_RING(chan, 1);
-	BEGIN_RING(chan, NvSubGdiRect, 0x0300, 1);
+	BEGIN_NV04(chan, NvSubGdiRect, 0x0300, 1);
 	OUT_RING(chan, rect_fmt);
-	BEGIN_RING(chan, NvSubGdiRect, 0x02fc, 1);
+	BEGIN_NV04(chan, NvSubGdiRect, 0x02fc, 1);
 	OUT_RING(chan, 3);
 
 	FIRE_RING(chan);
diff --git a/drivers/gpu/drm/nouveau/nv04_fence.c b/drivers/gpu/drm/nouveau/nv04_fence.c
new file mode 100644
index 00000000000..abe89db6de2
--- /dev/null
+++ b/drivers/gpu/drm/nouveau/nv04_fence.c
@@ -0,0 +1,140 @@
+/*
+ * 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 "drmP.h"
+#include "nouveau_drv.h"
+#include "nouveau_dma.h"
+#include "nouveau_ramht.h"
+#include "nouveau_fence.h"
+
+struct nv04_fence_chan {
+	struct nouveau_fence_chan base;
+	atomic_t sequence;
+};
+
+struct nv04_fence_priv {
+	struct nouveau_fence_priv base;
+};
+
+static int
+nv04_fence_emit(struct nouveau_fence *fence)
+{
+	struct nouveau_channel *chan = fence->channel;
+	int ret = RING_SPACE(chan, 2);
+	if (ret == 0) {
+		BEGIN_NV04(chan, NvSubSw, 0x0150, 1);
+		OUT_RING  (chan, fence->sequence);
+		FIRE_RING (chan);
+	}
+	return ret;
+}
+
+static int
+nv04_fence_sync(struct nouveau_fence *fence,
+		struct nouveau_channel *prev, struct nouveau_channel *chan)
+{
+	return -ENODEV;
+}
+
+int
+nv04_fence_mthd(struct nouveau_channel *chan, u32 class, u32 mthd, u32 data)
+{
+	struct nv04_fence_chan *fctx = chan->engctx[NVOBJ_ENGINE_FENCE];
+	atomic_set(&fctx->sequence, data);
+	return 0;
+}
+
+static u32
+nv04_fence_read(struct nouveau_channel *chan)
+{
+	struct nv04_fence_chan *fctx = chan->engctx[NVOBJ_ENGINE_FENCE];
+	return atomic_read(&fctx->sequence);
+}
+
+static void
+nv04_fence_context_del(struct nouveau_channel *chan, int engine)
+{
+	struct nv04_fence_chan *fctx = chan->engctx[engine];
+	nouveau_fence_context_del(&fctx->base);
+	chan->engctx[engine] = NULL;
+	kfree(fctx);
+}
+
+static int
+nv04_fence_context_new(struct nouveau_channel *chan, int engine)
+{
+	struct nv04_fence_chan *fctx = kzalloc(sizeof(*fctx), GFP_KERNEL);
+	if (fctx) {
+		nouveau_fence_context_new(&fctx->base);
+		atomic_set(&fctx->sequence, 0);
+		chan->engctx[engine] = fctx;
+		return 0;
+	}
+	return -ENOMEM;
+}
+
+static int
+nv04_fence_fini(struct drm_device *dev, int engine, bool suspend)
+{
+	return 0;
+}
+
+static int
+nv04_fence_init(struct drm_device *dev, int engine)
+{
+	return 0;
+}
+
+static void
+nv04_fence_destroy(struct drm_device *dev, int engine)
+{
+	struct drm_nouveau_private *dev_priv = dev->dev_private;
+	struct nv04_fence_priv *priv = nv_engine(dev, engine);
+
+	dev_priv->eng[engine] = NULL;
+	kfree(priv);
+}
+
+int
+nv04_fence_create(struct drm_device *dev)
+{
+	struct drm_nouveau_private *dev_priv = dev->dev_private;
+	struct nv04_fence_priv *priv;
+	int ret;
+
+	priv = kzalloc(sizeof(*priv), GFP_KERNEL);
+	if (!priv)
+		return -ENOMEM;
+
+	priv->base.engine.destroy = nv04_fence_destroy;
+	priv->base.engine.init = nv04_fence_init;
+	priv->base.engine.fini = nv04_fence_fini;
+	priv->base.engine.context_new = nv04_fence_context_new;
+	priv->base.engine.context_del = nv04_fence_context_del;
+	priv->base.emit = nv04_fence_emit;
+	priv->base.sync = nv04_fence_sync;
+	priv->base.read = nv04_fence_read;
+	dev_priv->eng[NVOBJ_ENGINE_FENCE] = &priv->base.engine;
+	return ret;
+}
diff --git a/drivers/gpu/drm/nouveau/nv04_fifo.c b/drivers/gpu/drm/nouveau/nv04_fifo.c
index db465a3ee1b..a6295cd00ec 100644
--- a/drivers/gpu/drm/nouveau/nv04_fifo.c
+++ b/drivers/gpu/drm/nouveau/nv04_fifo.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (C) 2007 Ben Skeggs.
+ * Copyright (C) 2012 Ben Skeggs.
  * All Rights Reserved.
  *
  * Permission is hereby granted, free of charge, to any person obtaining
@@ -27,49 +27,38 @@
 #include "drmP.h"
 #include "drm.h"
 #include "nouveau_drv.h"
-#include "nouveau_ramht.h"
+#include "nouveau_fifo.h"
 #include "nouveau_util.h"
-
-#define NV04_RAMFC(c) (dev_priv->ramfc->pinst + ((c) * NV04_RAMFC__SIZE))
-#define NV04_RAMFC__SIZE 32
-#define NV04_RAMFC_DMA_PUT                                       0x00
-#define NV04_RAMFC_DMA_GET                                       0x04
-#define NV04_RAMFC_DMA_INSTANCE                                  0x08
-#define NV04_RAMFC_DMA_STATE                                     0x0C
-#define NV04_RAMFC_DMA_FETCH                                     0x10
-#define NV04_RAMFC_ENGINE                                        0x14
-#define NV04_RAMFC_PULL1_ENGINE                                  0x18
-
-#define RAMFC_WR(offset, val) nv_wo32(chan->ramfc, NV04_RAMFC_##offset, (val))
-#define RAMFC_RD(offset)      nv_ro32(chan->ramfc, NV04_RAMFC_##offset)
-
-void
-nv04_fifo_disable(struct drm_device *dev)
-{
-	uint32_t tmp;
-
-	tmp = nv_rd32(dev, NV04_PFIFO_CACHE1_DMA_PUSH);
-	nv_wr32(dev, NV04_PFIFO_CACHE1_DMA_PUSH, tmp & ~1);
-	nv_wr32(dev, NV03_PFIFO_CACHE1_PUSH0, 0);
-	tmp = nv_rd32(dev, NV03_PFIFO_CACHE1_PULL1);
-	nv_wr32(dev, NV04_PFIFO_CACHE1_PULL0, tmp & ~1);
-}
-
-void
-nv04_fifo_enable(struct drm_device *dev)
-{
-	nv_wr32(dev, NV03_PFIFO_CACHE1_PUSH0, 1);
-	nv_wr32(dev, NV04_PFIFO_CACHE1_PULL0, 1);
-}
-
-bool
-nv04_fifo_reassign(struct drm_device *dev, bool enable)
-{
-	uint32_t reassign = nv_rd32(dev, NV03_PFIFO_CACHES);
-
-	nv_wr32(dev, NV03_PFIFO_CACHES, enable ? 1 : 0);
-	return (reassign == 1);
-}
+#include "nouveau_ramht.h"
+#include "nouveau_software.h"
+
+static struct ramfc_desc {
+	unsigned bits:6;
+	unsigned ctxs:5;
+	unsigned ctxp:8;
+	unsigned regs:5;
+	unsigned regp;
+} nv04_ramfc[] = {
+	{ 32,  0, 0x00,  0, NV04_PFIFO_CACHE1_DMA_PUT },
+	{ 32,  0, 0x04,  0, NV04_PFIFO_CACHE1_DMA_GET },
+	{ 16,  0, 0x08,  0, NV04_PFIFO_CACHE1_DMA_INSTANCE },
+	{ 16, 16, 0x08,  0, NV04_PFIFO_CACHE1_DMA_DCOUNT },
+	{ 32,  0, 0x0c,  0, NV04_PFIFO_CACHE1_DMA_STATE },
+	{ 32,  0, 0x10,  0, NV04_PFIFO_CACHE1_DMA_FETCH },
+	{ 32,  0, 0x14,  0, NV04_PFIFO_CACHE1_ENGINE },
+	{ 32,  0, 0x18,  0, NV04_PFIFO_CACHE1_PULL1 },
+	{}
+};
+
+struct nv04_fifo_priv {
+	struct nouveau_fifo_priv base;
+	struct ramfc_desc *ramfc_desc;
+};
+
+struct nv04_fifo_chan {
+	struct nouveau_fifo_chan base;
+	struct nouveau_gpuobj *ramfc;
+};
 
 bool
 nv04_fifo_cache_pull(struct drm_device *dev, bool enable)
@@ -86,13 +75,13 @@ nv04_fifo_cache_pull(struct drm_device *dev, bool enable)
 		 * invalidate the most recently calculated instance.
 		 */
 		if (!nv_wait(dev, NV04_PFIFO_CACHE1_PULL0,
-			     NV04_PFIFO_CACHE1_PULL0_HASH_BUSY, 0))
+				  NV04_PFIFO_CACHE1_PULL0_HASH_BUSY, 0))
 			NV_ERROR(dev, "Timeout idling the PFIFO puller.\n");
 
 		if (nv_rd32(dev, NV04_PFIFO_CACHE1_PULL0) &
-		    NV04_PFIFO_CACHE1_PULL0_HASH_FAILED)
+				 NV04_PFIFO_CACHE1_PULL0_HASH_FAILED)
 			nv_wr32(dev, NV03_PFIFO_INTR_0,
-				NV_PFIFO_INTR_CACHE_ERROR);
+				     NV_PFIFO_INTR_CACHE_ERROR);
 
 		nv_wr32(dev, NV04_PFIFO_CACHE1_HASH, 0);
 	}
@@ -100,242 +89,182 @@ nv04_fifo_cache_pull(struct drm_device *dev, bool enable)
 	return pull & 1;
 }
 
-int
-nv04_fifo_channel_id(struct drm_device *dev)
-{
-	return nv_rd32(dev, NV03_PFIFO_CACHE1_PUSH1) &
-			NV03_PFIFO_CACHE1_PUSH1_CHID_MASK;
-}
-
-#ifdef __BIG_ENDIAN
-#define DMA_FETCH_ENDIANNESS NV_PFIFO_CACHE1_BIG_ENDIAN
-#else
-#define DMA_FETCH_ENDIANNESS 0
-#endif
-
-int
-nv04_fifo_create_context(struct nouveau_channel *chan)
+static int
+nv04_fifo_context_new(struct nouveau_channel *chan, int engine)
 {
 	struct drm_device *dev = chan->dev;
 	struct drm_nouveau_private *dev_priv = dev->dev_private;
+	struct nv04_fifo_priv *priv = nv_engine(dev, engine);
+	struct nv04_fifo_chan *fctx;
 	unsigned long flags;
 	int ret;
 
-	ret = nouveau_gpuobj_new_fake(dev, NV04_RAMFC(chan->id), ~0,
-						NV04_RAMFC__SIZE,
-						NVOBJ_FLAG_ZERO_ALLOC |
-						NVOBJ_FLAG_ZERO_FREE,
-						&chan->ramfc);
-	if (ret)
-		return ret;
+	fctx = chan->engctx[engine] = kzalloc(sizeof(*fctx), GFP_KERNEL);
+	if (!fctx)
+		return -ENOMEM;
 
+	/* map channel control registers */
 	chan->user = ioremap(pci_resource_start(dev->pdev, 0) +
 			     NV03_USER(chan->id), PAGE_SIZE);
-	if (!chan->user)
-		return -ENOMEM;
-
-	spin_lock_irqsave(&dev_priv->context_switch_lock, flags);
-
-	/* Setup initial state */
-	RAMFC_WR(DMA_PUT, chan->pushbuf_base);
-	RAMFC_WR(DMA_GET, chan->pushbuf_base);
-	RAMFC_WR(DMA_INSTANCE, chan->pushbuf->pinst >> 4);
-	RAMFC_WR(DMA_FETCH, (NV_PFIFO_CACHE1_DMA_FETCH_TRIG_128_BYTES |
-			     NV_PFIFO_CACHE1_DMA_FETCH_SIZE_128_BYTES |
-			     NV_PFIFO_CACHE1_DMA_FETCH_MAX_REQS_8 |
-			     DMA_FETCH_ENDIANNESS));
+	if (!chan->user) {
+		ret = -ENOMEM;
+		goto error;
+	}
 
-	/* enable the fifo dma operation */
-	nv_wr32(dev, NV04_PFIFO_MODE,
-		nv_rd32(dev, NV04_PFIFO_MODE) | (1 << chan->id));
+	/* initialise default fifo context */
+	ret = nouveau_gpuobj_new_fake(dev, dev_priv->ramfc->pinst +
+				      chan->id * 32, ~0, 32,
+				      NVOBJ_FLAG_ZERO_FREE, &fctx->ramfc);
+	if (ret)
+		goto error;
+
+	nv_wo32(fctx->ramfc, 0x00, chan->pushbuf_base);
+	nv_wo32(fctx->ramfc, 0x04, chan->pushbuf_base);
+	nv_wo32(fctx->ramfc, 0x08, chan->pushbuf->pinst >> 4);
+	nv_wo32(fctx->ramfc, 0x0c, 0x00000000);
+	nv_wo32(fctx->ramfc, 0x10, NV_PFIFO_CACHE1_DMA_FETCH_TRIG_128_BYTES |
+				   NV_PFIFO_CACHE1_DMA_FETCH_SIZE_128_BYTES |
+#ifdef __BIG_ENDIAN
+				   NV_PFIFO_CACHE1_BIG_ENDIAN |
+#endif
+				   NV_PFIFO_CACHE1_DMA_FETCH_MAX_REQS_8);
+	nv_wo32(fctx->ramfc, 0x14, 0x00000000);
+	nv_wo32(fctx->ramfc, 0x18, 0x00000000);
+	nv_wo32(fctx->ramfc, 0x1c, 0x00000000);
 
+	/* enable dma mode on the channel */
+	spin_lock_irqsave(&dev_priv->context_switch_lock, flags);
+	nv_mask(dev, NV04_PFIFO_MODE, (1 << chan->id), (1 << chan->id));
 	spin_unlock_irqrestore(&dev_priv->context_switch_lock, flags);
-	return 0;
+
+error:
+	if (ret)
+		priv->base.base.context_del(chan, engine);
+	return ret;
 }
 
 void
-nv04_fifo_destroy_context(struct nouveau_channel *chan)
+nv04_fifo_context_del(struct nouveau_channel *chan, int engine)
 {
 	struct drm_device *dev = chan->dev;
 	struct drm_nouveau_private *dev_priv = dev->dev_private;
-	struct nouveau_fifo_engine *pfifo = &dev_priv->engine.fifo;
+	struct nv04_fifo_priv *priv = nv_engine(chan->dev, engine);
+	struct nv04_fifo_chan *fctx = chan->engctx[engine];
+	struct ramfc_desc *c = priv->ramfc_desc;
 	unsigned long flags;
+	int chid;
 
+	/* prevent fifo context switches */
 	spin_lock_irqsave(&dev_priv->context_switch_lock, flags);
-	pfifo->reassign(dev, false);
-
-	/* Unload the context if it's the currently active one */
-	if (pfifo->channel_id(dev) == chan->id) {
-		pfifo->disable(dev);
-		pfifo->unload_context(dev);
-		pfifo->enable(dev);
+	nv_wr32(dev, NV03_PFIFO_CACHES, 0);
+
+	/* if this channel is active, replace it with a null context */
+	chid = nv_rd32(dev, NV03_PFIFO_CACHE1_PUSH1) & priv->base.channels;
+	if (chid == chan->id) {
+		nv_mask(dev, NV04_PFIFO_CACHE1_DMA_PUSH, 0x00000001, 0);
+		nv_wr32(dev, NV03_PFIFO_CACHE1_PUSH0, 0);
+		nv_mask(dev, NV04_PFIFO_CACHE1_PULL0, 0x00000001, 0);
+
+		do {
+			u32 mask = ((1ULL << c->bits) - 1) << c->regs;
+			nv_mask(dev, c->regp, mask, 0x00000000);
+		} while ((++c)->bits);
+
+		nv_wr32(dev, NV03_PFIFO_CACHE1_GET, 0);
+		nv_wr32(dev, NV03_PFIFO_CACHE1_PUT, 0);
+		nv_wr32(dev, NV03_PFIFO_CACHE1_PUSH1, priv->base.channels);
+		nv_wr32(dev, NV03_PFIFO_CACHE1_PUSH0, 1);
+		nv_wr32(dev, NV04_PFIFO_CACHE1_PULL0, 1);
 	}
 
-	/* Keep it from being rescheduled */
+	/* restore normal operation, after disabling dma mode */
 	nv_mask(dev, NV04_PFIFO_MODE, 1 << chan->id, 0);
-
-	pfifo->reassign(dev, true);
+	nv_wr32(dev, NV03_PFIFO_CACHES, 1);
 	spin_unlock_irqrestore(&dev_priv->context_switch_lock, flags);
 
-	/* Free the channel resources */
+	/* clean up */
+	nouveau_gpuobj_ref(NULL, &fctx->ramfc);
+	nouveau_gpuobj_ref(NULL, &chan->ramfc); /*XXX: nv40 */
 	if (chan->user) {
 		iounmap(chan->user);
 		chan->user = NULL;
 	}
-	nouveau_gpuobj_ref(NULL, &chan->ramfc);
-}
-
-static void
-nv04_fifo_do_load_context(struct drm_device *dev, int chid)
-{
-	struct drm_nouveau_private *dev_priv = dev->dev_private;
-	uint32_t fc = NV04_RAMFC(chid), tmp;
-
-	nv_wr32(dev, NV04_PFIFO_CACHE1_DMA_PUT, nv_ri32(dev, fc + 0));
-	nv_wr32(dev, NV04_PFIFO_CACHE1_DMA_GET, nv_ri32(dev, fc + 4));
-	tmp = nv_ri32(dev, fc + 8);
-	nv_wr32(dev, NV04_PFIFO_CACHE1_DMA_INSTANCE, tmp & 0xFFFF);
-	nv_wr32(dev, NV04_PFIFO_CACHE1_DMA_DCOUNT, tmp >> 16);
-	nv_wr32(dev, NV04_PFIFO_CACHE1_DMA_STATE, nv_ri32(dev, fc + 12));
-	nv_wr32(dev, NV04_PFIFO_CACHE1_DMA_FETCH, nv_ri32(dev, fc + 16));
-	nv_wr32(dev, NV04_PFIFO_CACHE1_ENGINE, nv_ri32(dev, fc + 20));
-	nv_wr32(dev, NV04_PFIFO_CACHE1_PULL1, nv_ri32(dev, fc + 24));
-
-	nv_wr32(dev, NV03_PFIFO_CACHE1_GET, 0);
-	nv_wr32(dev, NV03_PFIFO_CACHE1_PUT, 0);
-}
-
-int
-nv04_fifo_load_context(struct nouveau_channel *chan)
-{
-	uint32_t tmp;
-
-	nv_wr32(chan->dev, NV03_PFIFO_CACHE1_PUSH1,
-			   NV03_PFIFO_CACHE1_PUSH1_DMA | chan->id);
-	nv04_fifo_do_load_context(chan->dev, chan->id);
-	nv_wr32(chan->dev, NV04_PFIFO_CACHE1_DMA_PUSH, 1);
-
-	/* Reset NV04_PFIFO_CACHE1_DMA_CTL_AT_INFO to INVALID */
-	tmp = nv_rd32(chan->dev, NV04_PFIFO_CACHE1_DMA_CTL) & ~(1 << 31);
-	nv_wr32(chan->dev, NV04_PFIFO_CACHE1_DMA_CTL, tmp);
-
-	return 0;
 }
 
 int
-nv04_fifo_unload_context(struct drm_device *dev)
+nv04_fifo_init(struct drm_device *dev, int engine)
 {
 	struct drm_nouveau_private *dev_priv = dev->dev_private;
-	struct nouveau_fifo_engine *pfifo = &dev_priv->engine.fifo;
-	struct nouveau_channel *chan = NULL;
-	uint32_t tmp;
-	int chid;
-
-	chid = pfifo->channel_id(dev);
-	if (chid < 0 || chid >= dev_priv->engine.fifo.channels)
-		return 0;
-
-	chan = dev_priv->channels.ptr[chid];
-	if (!chan) {
-		NV_ERROR(dev, "Inactive channel on PFIFO: %d\n", chid);
-		return -EINVAL;
-	}
-
-	RAMFC_WR(DMA_PUT, nv_rd32(dev, NV04_PFIFO_CACHE1_DMA_PUT));
-	RAMFC_WR(DMA_GET, nv_rd32(dev, NV04_PFIFO_CACHE1_DMA_GET));
-	tmp  = nv_rd32(dev, NV04_PFIFO_CACHE1_DMA_DCOUNT) << 16;
-	tmp |= nv_rd32(dev, NV04_PFIFO_CACHE1_DMA_INSTANCE);
-	RAMFC_WR(DMA_INSTANCE, tmp);
-	RAMFC_WR(DMA_STATE, nv_rd32(dev, NV04_PFIFO_CACHE1_DMA_STATE));
-	RAMFC_WR(DMA_FETCH, nv_rd32(dev, NV04_PFIFO_CACHE1_DMA_FETCH));
-	RAMFC_WR(ENGINE, nv_rd32(dev, NV04_PFIFO_CACHE1_ENGINE));
-	RAMFC_WR(PULL1_ENGINE, nv_rd32(dev, NV04_PFIFO_CACHE1_PULL1));
-
-	nv04_fifo_do_load_context(dev, pfifo->channels - 1);
-	nv_wr32(dev, NV03_PFIFO_CACHE1_PUSH1, pfifo->channels - 1);
-	return 0;
-}
+	struct nv04_fifo_priv *priv = nv_engine(dev, engine);
+	int i;
 
-static void
-nv04_fifo_init_reset(struct drm_device *dev)
-{
-	nv_wr32(dev, NV03_PMC_ENABLE,
-		nv_rd32(dev, NV03_PMC_ENABLE) & ~NV_PMC_ENABLE_PFIFO);
-	nv_wr32(dev, NV03_PMC_ENABLE,
-		nv_rd32(dev, NV03_PMC_ENABLE) |  NV_PMC_ENABLE_PFIFO);
-
-	nv_wr32(dev, 0x003224, 0x000f0078);
-	nv_wr32(dev, 0x002044, 0x0101ffff);
-	nv_wr32(dev, 0x002040, 0x000000ff);
-	nv_wr32(dev, 0x002500, 0x00000000);
-	nv_wr32(dev, 0x003000, 0x00000000);
-	nv_wr32(dev, 0x003050, 0x00000000);
-	nv_wr32(dev, 0x003200, 0x00000000);
-	nv_wr32(dev, 0x003250, 0x00000000);
-	nv_wr32(dev, 0x003220, 0x00000000);
-
-	nv_wr32(dev, 0x003250, 0x00000000);
-	nv_wr32(dev, 0x003270, 0x00000000);
-	nv_wr32(dev, 0x003210, 0x00000000);
-}
+	nv_mask(dev, NV03_PMC_ENABLE, NV_PMC_ENABLE_PFIFO, 0);
+	nv_mask(dev, NV03_PMC_ENABLE, NV_PMC_ENABLE_PFIFO, NV_PMC_ENABLE_PFIFO);
 
-static void
-nv04_fifo_init_ramxx(struct drm_device *dev)
-{
-	struct drm_nouveau_private *dev_priv = dev->dev_private;
+	nv_wr32(dev, NV04_PFIFO_DELAY_0, 0x000000ff);
+	nv_wr32(dev, NV04_PFIFO_DMA_TIMESLICE, 0x0101ffff);
 
 	nv_wr32(dev, NV03_PFIFO_RAMHT, (0x03 << 24) /* search 128 */ |
 				       ((dev_priv->ramht->bits - 9) << 16) |
 				       (dev_priv->ramht->gpuobj->pinst >> 8));
 	nv_wr32(dev, NV03_PFIFO_RAMRO, dev_priv->ramro->pinst >> 8);
 	nv_wr32(dev, NV03_PFIFO_RAMFC, dev_priv->ramfc->pinst >> 8);
-}
 
-static void
-nv04_fifo_init_intr(struct drm_device *dev)
-{
-	nouveau_irq_register(dev, 8, nv04_fifo_isr);
-	nv_wr32(dev, 0x002100, 0xffffffff);
-	nv_wr32(dev, 0x002140, 0xffffffff);
+	nv_wr32(dev, NV03_PFIFO_CACHE1_PUSH1, priv->base.channels);
+
+	nv_wr32(dev, NV03_PFIFO_INTR_0, 0xffffffff);
+	nv_wr32(dev, NV03_PFIFO_INTR_EN_0, 0xffffffff);
+
+	nv_wr32(dev, NV03_PFIFO_CACHE1_PUSH0, 1);
+	nv_wr32(dev, NV04_PFIFO_CACHE1_PULL0, 1);
+	nv_wr32(dev, NV03_PFIFO_CACHES, 1);
+
+	for (i = 0; i < priv->base.channels; i++) {
+		if (dev_priv->channels.ptr[i])
+			nv_mask(dev, NV04_PFIFO_MODE, (1 << i), (1 << i));
+	}
+
+	return 0;
 }
 
 int
-nv04_fifo_init(struct drm_device *dev)
+nv04_fifo_fini(struct drm_device *dev, int engine, bool suspend)
 {
 	struct drm_nouveau_private *dev_priv = dev->dev_private;
-	struct nouveau_fifo_engine *pfifo = &dev_priv->engine.fifo;
-	int i;
-
-	nv04_fifo_init_reset(dev);
-	nv04_fifo_init_ramxx(dev);
-
-	nv04_fifo_do_load_context(dev, pfifo->channels - 1);
-	nv_wr32(dev, NV03_PFIFO_CACHE1_PUSH1, pfifo->channels - 1);
+	struct nv04_fifo_priv *priv = nv_engine(dev, engine);
+	struct nouveau_channel *chan;
+	int chid;
 
-	nv04_fifo_init_intr(dev);
-	pfifo->enable(dev);
-	pfifo->reassign(dev, true);
+	/* prevent context switches and halt fifo operation */
+	nv_wr32(dev, NV03_PFIFO_CACHES, 0);
+	nv_wr32(dev, NV04_PFIFO_CACHE1_DMA_PUSH, 0);
+	nv_wr32(dev, NV03_PFIFO_CACHE1_PUSH0, 0);
+	nv_wr32(dev, NV04_PFIFO_CACHE1_PULL0, 0);
 
-	for (i = 0; i < dev_priv->engine.fifo.channels; i++) {
-		if (dev_priv->channels.ptr[i]) {
-			uint32_t mode = nv_rd32(dev, NV04_PFIFO_MODE);
-			nv_wr32(dev, NV04_PFIFO_MODE, mode | (1 << i));
-		}
+	/* store current fifo context in ramfc */
+	chid = nv_rd32(dev, NV03_PFIFO_CACHE1_PUSH1) & priv->base.channels;
+	chan = dev_priv->channels.ptr[chid];
+	if (suspend && chid != priv->base.channels && chan) {
+		struct nv04_fifo_chan *fctx = chan->engctx[engine];
+		struct nouveau_gpuobj *ctx = fctx->ramfc;
+		struct ramfc_desc *c = priv->ramfc_desc;
+		do {
+			u32 rm = ((1ULL << c->bits) - 1) << c->regs;
+			u32 cm = ((1ULL << c->bits) - 1) << c->ctxs;
+			u32 rv = (nv_rd32(dev, c->regp) &  rm) >> c->regs;
+			u32 cv = (nv_ro32(ctx, c->ctxp) & ~cm);
+			nv_wo32(ctx, c->ctxp, cv | (rv << c->ctxs));
+		} while ((++c)->bits);
 	}
 
+	nv_wr32(dev, NV03_PFIFO_INTR_EN_0, 0x00000000);
 	return 0;
 }
 
-void
-nv04_fifo_fini(struct drm_device *dev)
-{
-	nv_wr32(dev, 0x2140, 0x00000000);
-	nouveau_irq_unregister(dev, 8);
-}
-
 static bool
 nouveau_fifo_swmthd(struct drm_device *dev, u32 chid, u32 addr, u32 data)
 {
+	struct nouveau_fifo_priv *pfifo = nv_engine(dev, NVOBJ_ENGINE_FIFO);
 	struct drm_nouveau_private *dev_priv = dev->dev_private;
 	struct nouveau_channel *chan = NULL;
 	struct nouveau_gpuobj *obj;
@@ -346,7 +275,7 @@ nouveau_fifo_swmthd(struct drm_device *dev, u32 chid, u32 addr, u32 data)
 	u32 engine;
 
 	spin_lock_irqsave(&dev_priv->channels.lock, flags);
-	if (likely(chid >= 0 && chid < dev_priv->engine.fifo.channels))
+	if (likely(chid >= 0 && chid < pfifo->channels))
 		chan = dev_priv->channels.ptr[chid];
 	if (unlikely(!chan))
 		goto out;
@@ -357,7 +286,6 @@ nouveau_fifo_swmthd(struct drm_device *dev, u32 chid, u32 addr, u32 data)
 		if (unlikely(!obj || obj->engine != NVOBJ_ENGINE_SW))
 			break;
 
-		chan->sw_subchannel[subc] = obj->class;
 		engine = 0x0000000f << (subc * 4);
 
 		nv_mask(dev, NV04_PFIFO_CACHE1_ENGINE, engine, 0x00000000);
@@ -368,7 +296,7 @@ nouveau_fifo_swmthd(struct drm_device *dev, u32 chid, u32 addr, u32 data)
 		if (unlikely(((engine >> (subc * 4)) & 0xf) != 0))
 			break;
 
-		if (!nouveau_gpuobj_mthd_call(chan, chan->sw_subchannel[subc],
+		if (!nouveau_gpuobj_mthd_call(chan, nouveau_software_class(dev),
 					      mthd, data))
 			handled = true;
 		break;
@@ -391,8 +319,8 @@ static const char *nv_dma_state_err(u32 state)
 void
 nv04_fifo_isr(struct drm_device *dev)
 {
+	struct nouveau_fifo_priv *pfifo = nv_engine(dev, NVOBJ_ENGINE_FIFO);
 	struct drm_nouveau_private *dev_priv = dev->dev_private;
-	struct nouveau_engine *engine = &dev_priv->engine;
 	uint32_t status, reassign;
 	int cnt = 0;
 
@@ -402,7 +330,7 @@ nv04_fifo_isr(struct drm_device *dev)
 
 		nv_wr32(dev, NV03_PFIFO_CACHES, 0);
 
-		chid = engine->fifo.channel_id(dev);
+		chid = nv_rd32(dev, NV03_PFIFO_CACHE1_PUSH1) & pfifo->channels;
 		get  = nv_rd32(dev, NV03_PFIFO_CACHE1_GET);
 
 		if (status & NV_PFIFO_INTR_CACHE_ERROR) {
@@ -541,3 +469,38 @@ nv04_fifo_isr(struct drm_device *dev)
 
 	nv_wr32(dev, NV03_PMC_INTR_0, NV_PMC_INTR_0_PFIFO_PENDING);
 }
+
+void
+nv04_fifo_destroy(struct drm_device *dev, int engine)
+{
+	struct drm_nouveau_private *dev_priv = dev->dev_private;
+	struct nv04_fifo_priv *priv = nv_engine(dev, engine);
+
+	nouveau_irq_unregister(dev, 8);
+
+	dev_priv->eng[engine] = NULL;
+	kfree(priv);
+}
+
+int
+nv04_fifo_create(struct drm_device *dev)
+{
+	struct drm_nouveau_private *dev_priv = dev->dev_private;
+	struct nv04_fifo_priv *priv;
+
+	priv = kzalloc(sizeof(*priv), GFP_KERNEL);
+	if (!priv)
+		return -ENOMEM;
+
+	priv->base.base.destroy = nv04_fifo_destroy;
+	priv->base.base.init = nv04_fifo_init;
+	priv->base.base.fini = nv04_fifo_fini;
+	priv->base.base.context_new = nv04_fifo_context_new;
+	priv->base.base.context_del = nv04_fifo_context_del;
+	priv->base.channels = 15;
+	priv->ramfc_desc = nv04_ramfc;
+	dev_priv->eng[NVOBJ_ENGINE_FIFO] = &priv->base.base;
+
+	nouveau_irq_register(dev, 8, nv04_fifo_isr);
+	return 0;
+}
diff --git a/drivers/gpu/drm/nouveau/nv04_graph.c b/drivers/gpu/drm/nouveau/nv04_graph.c
index dbdea8ed392..72f1a62903b 100644
--- a/drivers/gpu/drm/nouveau/nv04_graph.c
+++ b/drivers/gpu/drm/nouveau/nv04_graph.c
@@ -356,12 +356,12 @@ static struct nouveau_channel *
 nv04_graph_channel(struct drm_device *dev)
 {
 	struct drm_nouveau_private *dev_priv = dev->dev_private;
-	int chid = dev_priv->engine.fifo.channels;
+	int chid = 15;
 
 	if (nv_rd32(dev, NV04_PGRAPH_CTX_CONTROL) & 0x00010000)
 		chid = nv_rd32(dev, NV04_PGRAPH_CTX_USER) >> 24;
 
-	if (chid >= dev_priv->engine.fifo.channels)
+	if (chid > 15)
 		return NULL;
 
 	return dev_priv->channels.ptr[chid];
@@ -404,7 +404,6 @@ nv04_graph_load_context(struct nouveau_channel *chan)
 static int
 nv04_graph_unload_context(struct drm_device *dev)
 {
-	struct drm_nouveau_private *dev_priv = dev->dev_private;
 	struct nouveau_channel *chan = NULL;
 	struct graph_state *ctx;
 	uint32_t tmp;
@@ -420,7 +419,7 @@ nv04_graph_unload_context(struct drm_device *dev)
 
 	nv_wr32(dev, NV04_PGRAPH_CTX_CONTROL, 0x10000000);
 	tmp  = nv_rd32(dev, NV04_PGRAPH_CTX_USER) & 0x00ffffff;
-	tmp |= (dev_priv->engine.fifo.channels - 1) << 24;
+	tmp |= 15 << 24;
 	nv_wr32(dev, NV04_PGRAPH_CTX_USER, tmp);
 	return 0;
 }
@@ -495,7 +494,6 @@ nv04_graph_object_new(struct nouveau_channel *chan, int engine,
 static int
 nv04_graph_init(struct drm_device *dev, int engine)
 {
-	struct drm_nouveau_private *dev_priv = dev->dev_private;
 	uint32_t tmp;
 
 	nv_wr32(dev, NV03_PMC_ENABLE, nv_rd32(dev, NV03_PMC_ENABLE) &
@@ -527,7 +525,7 @@ nv04_graph_init(struct drm_device *dev, int engine)
 	nv_wr32(dev, NV04_PGRAPH_STATE        , 0xFFFFFFFF);
 	nv_wr32(dev, NV04_PGRAPH_CTX_CONTROL  , 0x10000100);
 	tmp  = nv_rd32(dev, NV04_PGRAPH_CTX_USER) & 0x00ffffff;
-	tmp |= (dev_priv->engine.fifo.channels - 1) << 24;
+	tmp |= 15 << 24;
 	nv_wr32(dev, NV04_PGRAPH_CTX_USER, tmp);
 
 	/* These don't belong here, they're part of a per-channel context */
@@ -550,28 +548,6 @@ nv04_graph_fini(struct drm_device *dev, int engine, bool suspend)
 	return 0;
 }
 
-static int
-nv04_graph_mthd_set_ref(struct nouveau_channel *chan,
-			u32 class, u32 mthd, u32 data)
-{
-	atomic_set(&chan->fence.last_sequence_irq, data);
-	return 0;
-}
-
-int
-nv04_graph_mthd_page_flip(struct nouveau_channel *chan,
-			  u32 class, u32 mthd, u32 data)
-{
-	struct drm_device *dev = chan->dev;
-	struct nouveau_page_flip_state s;
-
-	if (!nouveau_finish_page_flip(chan, &s))
-		nv_set_crtc_base(dev, s.crtc,
-				 s.offset + s.y * s.pitch + s.x * s.bpp / 8);
-
-	return 0;
-}
-
 /*
  * Software methods, why they are needed, and how they all work:
  *
@@ -1020,7 +996,8 @@ nv04_graph_context_switch(struct drm_device *dev)
 	nv04_graph_unload_context(dev);
 
 	/* Load context for next channel */
-	chid = dev_priv->engine.fifo.channel_id(dev);
+	chid = nv_rd32(dev, NV03_PFIFO_CACHE1_PUSH1) &
+			    NV03_PFIFO_CACHE1_PUSH1_CHID_MASK;
 	chan = dev_priv->channels.ptr[chid];
 	if (chan)
 		nv04_graph_load_context(chan);
@@ -1345,9 +1322,5 @@ nv04_graph_create(struct drm_device *dev)
 	NVOBJ_MTHD (dev, 0x005e, 0x0198, nv04_graph_mthd_bind_surf2d);
 	NVOBJ_MTHD (dev, 0x005e, 0x02fc, nv04_graph_mthd_set_operation);
 
-	/* nvsw */
-	NVOBJ_CLASS(dev, 0x506e, SW);
-	NVOBJ_MTHD (dev, 0x506e, 0x0150, nv04_graph_mthd_set_ref);
-	NVOBJ_MTHD (dev, 0x506e, 0x0500, nv04_graph_mthd_page_flip);
 	return 0;
 }
diff --git a/drivers/gpu/drm/nouveau/nv04_instmem.c b/drivers/gpu/drm/nouveau/nv04_instmem.c
index c1248e0740a..ef7a934a499 100644
--- a/drivers/gpu/drm/nouveau/nv04_instmem.c
+++ b/drivers/gpu/drm/nouveau/nv04_instmem.c
@@ -1,6 +1,8 @@
 #include "drmP.h"
 #include "drm.h"
+
 #include "nouveau_drv.h"
+#include "nouveau_fifo.h"
 #include "nouveau_ramht.h"
 
 /* returns the size of fifo context */
@@ -10,12 +12,15 @@ nouveau_fifo_ctx_size(struct drm_device *dev)
 	struct drm_nouveau_private *dev_priv = dev->dev_private;
 
 	if (dev_priv->chipset >= 0x40)
-		return 128;
+		return 128 * 32;
 	else
 	if (dev_priv->chipset >= 0x17)
-		return 64;
+		return 64 * 32;
+	else
+	if (dev_priv->chipset >= 0x10)
+		return 32 * 32;
 
-	return 32;
+	return 32 * 16;
 }
 
 int nv04_instmem_init(struct drm_device *dev)
@@ -39,14 +44,10 @@ int nv04_instmem_init(struct drm_device *dev)
 		else if (nv44_graph_class(dev))	    rsvd = 0x4980 * vs;
 		else				    rsvd = 0x4a40 * vs;
 		rsvd += 16 * 1024;
-		rsvd *= dev_priv->engine.fifo.channels;
-
-		/* pciegart table */
-		if (pci_is_pcie(dev->pdev))
-			rsvd += 512 * 1024;
+		rsvd *= 32; /* per-channel */
 
-		/* object storage */
-		rsvd += 512 * 1024;
+		rsvd += 512 * 1024; /* pci(e)gart table */
+		rsvd += 512 * 1024; /* object storage */
 
 		dev_priv->ramin_rsvd_vram = round_up(rsvd, 4096);
 	} else {
@@ -71,7 +72,7 @@ int nv04_instmem_init(struct drm_device *dev)
 		return ret;
 
 	/* And RAMFC */
-	length = dev_priv->engine.fifo.channels * nouveau_fifo_ctx_size(dev);
+	length = nouveau_fifo_ctx_size(dev);
 	switch (dev_priv->card_type) {
 	case NV_40:
 		offset = 0x20000;
diff --git a/drivers/gpu/drm/nouveau/nv04_software.c b/drivers/gpu/drm/nouveau/nv04_software.c
new file mode 100644
index 00000000000..0c41abf4877
--- /dev/null
+++ b/drivers/gpu/drm/nouveau/nv04_software.c
@@ -0,0 +1,147 @@
+/*
+ * 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 "drmP.h"
+
+#include "nouveau_drv.h"
+#include "nouveau_ramht.h"
+#include "nouveau_fence.h"
+#include "nouveau_software.h"
+#include "nouveau_hw.h"
+
+struct nv04_software_priv {
+	struct nouveau_software_priv base;
+};
+
+struct nv04_software_chan {
+	struct nouveau_software_chan base;
+};
+
+static int
+mthd_flip(struct nouveau_channel *chan, u32 class, u32 mthd, u32 data)
+{
+
+	struct nouveau_page_flip_state state;
+
+	if (!nouveau_finish_page_flip(chan, &state)) {
+		nv_set_crtc_base(chan->dev, state.crtc, state.offset +
+				 state.y * state.pitch +
+				 state.x * state.bpp / 8);
+	}
+
+	return 0;
+}
+
+static int
+nv04_software_context_new(struct nouveau_channel *chan, int engine)
+{
+	struct nv04_software_chan *pch;
+
+	pch = kzalloc(sizeof(*pch), GFP_KERNEL);
+	if (!pch)
+		return -ENOMEM;
+
+	nouveau_software_context_new(&pch->base);
+	chan->engctx[engine] = pch;
+	return 0;
+}
+
+static void
+nv04_software_context_del(struct nouveau_channel *chan, int engine)
+{
+	struct nv04_software_chan *pch = chan->engctx[engine];
+	chan->engctx[engine] = NULL;
+	kfree(pch);
+}
+
+static int
+nv04_software_object_new(struct nouveau_channel *chan, int engine,
+			 u32 handle, u16 class)
+{
+	struct drm_device *dev = chan->dev;
+	struct nouveau_gpuobj *obj = NULL;
+	int ret;
+
+	ret = nouveau_gpuobj_new(dev, chan, 16, 16, 0, &obj);
+	if (ret)
+		return ret;
+	obj->engine = 0;
+	obj->class  = class;
+
+	ret = nouveau_ramht_insert(chan, handle, obj);
+	nouveau_gpuobj_ref(NULL, &obj);
+	return ret;
+}
+
+static int
+nv04_software_init(struct drm_device *dev, int engine)
+{
+	return 0;
+}
+
+static int
+nv04_software_fini(struct drm_device *dev, int engine, bool suspend)
+{
+	return 0;
+}
+
+static void
+nv04_software_destroy(struct drm_device *dev, int engine)
+{
+	struct nv04_software_priv *psw = nv_engine(dev, engine);
+
+	NVOBJ_ENGINE_DEL(dev, SW);
+	kfree(psw);
+}
+
+int
+nv04_software_create(struct drm_device *dev)
+{
+	struct drm_nouveau_private *dev_priv = dev->dev_private;
+	struct nv04_software_priv *psw;
+
+	psw = kzalloc(sizeof(*psw), GFP_KERNEL);
+	if (!psw)
+		return -ENOMEM;
+
+	psw->base.base.destroy = nv04_software_destroy;
+	psw->base.base.init = nv04_software_init;
+	psw->base.base.fini = nv04_software_fini;
+	psw->base.base.context_new = nv04_software_context_new;
+	psw->base.base.context_del = nv04_software_context_del;
+	psw->base.base.object_new = nv04_software_object_new;
+	nouveau_software_create(&psw->base);
+
+	NVOBJ_ENGINE_ADD(dev, SW, &psw->base.base);
+	if (dev_priv->card_type <= NV_04) {
+		NVOBJ_CLASS(dev, 0x006e, SW);
+		NVOBJ_MTHD (dev, 0x006e, 0x0150, nv04_fence_mthd);
+		NVOBJ_MTHD (dev, 0x006e, 0x0500, mthd_flip);
+	} else {
+		NVOBJ_CLASS(dev, 0x016e, SW);
+		NVOBJ_MTHD (dev, 0x016e, 0x0500, mthd_flip);
+	}
+
+	return 0;
+}
diff --git a/drivers/gpu/drm/nouveau/nv10_fence.c b/drivers/gpu/drm/nouveau/nv10_fence.c
new file mode 100644
index 00000000000..8a1b7500918
--- /dev/null
+++ b/drivers/gpu/drm/nouveau/nv10_fence.c
@@ -0,0 +1,214 @@
+/*
+ * 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 <bskeggs@redhat.com>
+ */
+
+#include "drmP.h"
+#include "nouveau_drv.h"
+#include "nouveau_dma.h"
+#include "nouveau_ramht.h"
+#include "nouveau_fence.h"
+
+struct nv10_fence_chan {
+	struct nouveau_fence_chan base;
+};
+
+struct nv10_fence_priv {
+	struct nouveau_fence_priv base;
+	struct nouveau_bo *bo;
+	spinlock_t lock;
+	u32 sequence;
+};
+
+static int
+nv10_fence_emit(struct nouveau_fence *fence)
+{
+	struct nouveau_channel *chan = fence->channel;
+	int ret = RING_SPACE(chan, 2);
+	if (ret == 0) {
+		BEGIN_NV04(chan, 0, NV10_SUBCHAN_REF_CNT, 1);
+		OUT_RING  (chan, fence->sequence);
+		FIRE_RING (chan);
+	}
+	return ret;
+}
+
+
+static int
+nv10_fence_sync(struct nouveau_fence *fence,
+		struct nouveau_channel *prev, struct nouveau_channel *chan)
+{
+	return -ENODEV;
+}
+
+static int
+nv17_fence_sync(struct nouveau_fence *fence,
+		struct nouveau_channel *prev, struct nouveau_channel *chan)
+{
+	struct nv10_fence_priv *priv = nv_engine(chan->dev, NVOBJ_ENGINE_FENCE);
+	u32 value;
+	int ret;
+
+	if (!mutex_trylock(&prev->mutex))
+		return -EBUSY;
+
+	spin_lock(&priv->lock);
+	value = priv->sequence;
+	priv->sequence += 2;
+	spin_unlock(&priv->lock);
+
+	ret = RING_SPACE(prev, 5);
+	if (!ret) {
+		BEGIN_NV04(prev, 0, NV11_SUBCHAN_DMA_SEMAPHORE, 4);
+		OUT_RING  (prev, NvSema);
+		OUT_RING  (prev, 0);
+		OUT_RING  (prev, value + 0);
+		OUT_RING  (prev, value + 1);
+		FIRE_RING (prev);
+	}
+
+	if (!ret && !(ret = RING_SPACE(chan, 5))) {
+		BEGIN_NV04(chan, 0, NV11_SUBCHAN_DMA_SEMAPHORE, 4);
+		OUT_RING  (chan, NvSema);
+		OUT_RING  (chan, 0);
+		OUT_RING  (chan, value + 1);
+		OUT_RING  (chan, value + 2);
+		FIRE_RING (chan);
+	}
+
+	mutex_unlock(&prev->mutex);
+	return 0;
+}
+
+static u32
+nv10_fence_read(struct nouveau_channel *chan)
+{
+	return nvchan_rd32(chan, 0x0048);
+}
+
+static void
+nv10_fence_context_del(struct nouveau_channel *chan, int engine)
+{
+	struct nv10_fence_chan *fctx = chan->engctx[engine];
+	nouveau_fence_context_del(&fctx->base);
+	chan->engctx[engine] = NULL;
+	kfree(fctx);
+}
+
+static int
+nv10_fence_context_new(struct nouveau_channel *chan, int engine)
+{
+	struct nv10_fence_priv *priv = nv_engine(chan->dev, engine);
+	struct nv10_fence_chan *fctx;
+	struct nouveau_gpuobj *obj;
+	int ret = 0;
+
+	fctx = chan->engctx[engine] = kzalloc(sizeof(*fctx), GFP_KERNEL);
+	if (!fctx)
+		return -ENOMEM;
+
+	nouveau_fence_context_new(&fctx->base);
+
+	if (priv->bo) {
+		struct ttm_mem_reg *mem = &priv->bo->bo.mem;
+
+		ret = nouveau_gpuobj_dma_new(chan, NV_CLASS_DMA_FROM_MEMORY,
+					     mem->start * PAGE_SIZE, mem->size,
+					     NV_MEM_ACCESS_RW,
+					     NV_MEM_TARGET_VRAM, &obj);
+		if (!ret) {
+			ret = nouveau_ramht_insert(chan, NvSema, obj);
+			nouveau_gpuobj_ref(NULL, &obj);
+		}
+	}
+
+	if (ret)
+		nv10_fence_context_del(chan, engine);
+	return ret;
+}
+
+static int
+nv10_fence_fini(struct drm_device *dev, int engine, bool suspend)
+{
+	return 0;
+}
+
+static int
+nv10_fence_init(struct drm_device *dev, int engine)
+{
+	return 0;
+}
+
+static void
+nv10_fence_destroy(struct drm_device *dev, int engine)
+{
+	struct drm_nouveau_private *dev_priv = dev->dev_private;
+	struct nv10_fence_priv *priv = nv_engine(dev, engine);
+
+	nouveau_bo_ref(NULL, &priv->bo);
+	dev_priv->eng[engine] = NULL;
+	kfree(priv);
+}
+
+int
+nv10_fence_create(struct drm_device *dev)
+{
+	struct drm_nouveau_private *dev_priv = dev->dev_private;
+	struct nv10_fence_priv *priv;
+	int ret = 0;
+
+	priv = kzalloc(sizeof(*priv), GFP_KERNEL);
+	if (!priv)
+		return -ENOMEM;
+
+	priv->base.engine.destroy = nv10_fence_destroy;
+	priv->base.engine.init = nv10_fence_init;
+	priv->base.engine.fini = nv10_fence_fini;
+	priv->base.engine.context_new = nv10_fence_context_new;
+	priv->base.engine.context_del = nv10_fence_context_del;
+	priv->base.emit = nv10_fence_emit;
+	priv->base.read = nv10_fence_read;
+	priv->base.sync = nv10_fence_sync;
+	dev_priv->eng[NVOBJ_ENGINE_FENCE] = &priv->base.engine;
+	spin_lock_init(&priv->lock);
+
+	if (dev_priv->chipset >= 0x17) {
+		ret = nouveau_bo_new(dev, 4096, 0x1000, TTM_PL_FLAG_VRAM,
+				     0, 0x0000, NULL, &priv->bo);
+		if (!ret) {
+			ret = nouveau_bo_pin(priv->bo, TTM_PL_FLAG_VRAM);
+			if (!ret)
+				ret = nouveau_bo_map(priv->bo);
+			if (ret)
+				nouveau_bo_ref(NULL, &priv->bo);
+		}
+
+		if (ret == 0) {
+			nouveau_bo_wr32(priv->bo, 0x000, 0x00000000);
+			priv->base.sync = nv17_fence_sync;
+		}
+	}
+
+	if (ret)
+		nv10_fence_destroy(dev, NVOBJ_ENGINE_FENCE);
+	return ret;
+}
diff --git a/drivers/gpu/drm/nouveau/nv10_fifo.c b/drivers/gpu/drm/nouveau/nv10_fifo.c
index d2ecbff4bee..f1fe7d75824 100644
--- a/drivers/gpu/drm/nouveau/nv10_fifo.c
+++ b/drivers/gpu/drm/nouveau/nv10_fifo.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (C) 2007 Ben Skeggs.
+ * Copyright (C) 2012 Ben Skeggs.
  * All Rights Reserved.
  *
  * Permission is hereby granted, free of charge, to any person obtaining
@@ -27,220 +27,112 @@
 #include "drmP.h"
 #include "drm.h"
 #include "nouveau_drv.h"
+#include "nouveau_fifo.h"
+#include "nouveau_util.h"
 #include "nouveau_ramht.h"
 
-#define NV10_RAMFC(c) (dev_priv->ramfc->pinst + ((c) * NV10_RAMFC__SIZE))
-#define NV10_RAMFC__SIZE ((dev_priv->chipset) >= 0x17 ? 64 : 32)
-
-int
-nv10_fifo_channel_id(struct drm_device *dev)
-{
-	return nv_rd32(dev, NV03_PFIFO_CACHE1_PUSH1) &
-			NV10_PFIFO_CACHE1_PUSH1_CHID_MASK;
-}
-
-int
-nv10_fifo_create_context(struct nouveau_channel *chan)
+static struct ramfc_desc {
+	unsigned bits:6;
+	unsigned ctxs:5;
+	unsigned ctxp:8;
+	unsigned regs:5;
+	unsigned regp;
+} nv10_ramfc[] = {
+	{ 32,  0, 0x00,  0, NV04_PFIFO_CACHE1_DMA_PUT },
+	{ 32,  0, 0x04,  0, NV04_PFIFO_CACHE1_DMA_GET },
+	{ 32,  0, 0x08,  0, NV10_PFIFO_CACHE1_REF_CNT },
+	{ 16,  0, 0x0c,  0, NV04_PFIFO_CACHE1_DMA_INSTANCE },
+	{ 16, 16, 0x0c,  0, NV04_PFIFO_CACHE1_DMA_DCOUNT },
+	{ 32,  0, 0x10,  0, NV04_PFIFO_CACHE1_DMA_STATE },
+	{ 32,  0, 0x14,  0, NV04_PFIFO_CACHE1_DMA_FETCH },
+	{ 32,  0, 0x18,  0, NV04_PFIFO_CACHE1_ENGINE },
+	{ 32,  0, 0x1c,  0, NV04_PFIFO_CACHE1_PULL1 },
+	{}
+};
+
+struct nv10_fifo_priv {
+	struct nouveau_fifo_priv base;
+	struct ramfc_desc *ramfc_desc;
+};
+
+struct nv10_fifo_chan {
+	struct nouveau_fifo_chan base;
+	struct nouveau_gpuobj *ramfc;
+};
+
+static int
+nv10_fifo_context_new(struct nouveau_channel *chan, int engine)
 {
-	struct drm_nouveau_private *dev_priv = chan->dev->dev_private;
 	struct drm_device *dev = chan->dev;
-	uint32_t fc = NV10_RAMFC(chan->id);
+	struct drm_nouveau_private *dev_priv = dev->dev_private;
+	struct nv10_fifo_priv *priv = nv_engine(dev, engine);
+	struct nv10_fifo_chan *fctx;
+	unsigned long flags;
 	int ret;
 
-	ret = nouveau_gpuobj_new_fake(dev, NV10_RAMFC(chan->id), ~0,
-				      NV10_RAMFC__SIZE, NVOBJ_FLAG_ZERO_ALLOC |
-				      NVOBJ_FLAG_ZERO_FREE, &chan->ramfc);
-	if (ret)
-		return ret;
+	fctx = chan->engctx[engine] = kzalloc(sizeof(*fctx), GFP_KERNEL);
+	if (!fctx)
+		return -ENOMEM;
 
+	/* map channel control registers */
 	chan->user = ioremap(pci_resource_start(dev->pdev, 0) +
 			     NV03_USER(chan->id), PAGE_SIZE);
-	if (!chan->user)
-		return -ENOMEM;
+	if (!chan->user) {
+		ret = -ENOMEM;
+		goto error;
+	}
 
-	/* Fill entries that are seen filled in dumps of nvidia driver just
-	 * after channel's is put into DMA mode
-	 */
-	nv_wi32(dev, fc +  0, chan->pushbuf_base);
-	nv_wi32(dev, fc +  4, chan->pushbuf_base);
-	nv_wi32(dev, fc + 12, chan->pushbuf->pinst >> 4);
-	nv_wi32(dev, fc + 20, NV_PFIFO_CACHE1_DMA_FETCH_TRIG_128_BYTES |
-			      NV_PFIFO_CACHE1_DMA_FETCH_SIZE_128_BYTES |
-			      NV_PFIFO_CACHE1_DMA_FETCH_MAX_REQS_8 |
+	/* initialise default fifo context */
+	ret = nouveau_gpuobj_new_fake(dev, dev_priv->ramfc->pinst +
+				      chan->id * 32, ~0, 32,
+				      NVOBJ_FLAG_ZERO_FREE, &fctx->ramfc);
+	if (ret)
+		goto error;
+
+	nv_wo32(fctx->ramfc, 0x00, chan->pushbuf_base);
+	nv_wo32(fctx->ramfc, 0x04, chan->pushbuf_base);
+	nv_wo32(fctx->ramfc, 0x08, 0x00000000);
+	nv_wo32(fctx->ramfc, 0x0c, chan->pushbuf->pinst >> 4);
+	nv_wo32(fctx->ramfc, 0x10, 0x00000000);
+	nv_wo32(fctx->ramfc, 0x14, NV_PFIFO_CACHE1_DMA_FETCH_TRIG_128_BYTES |
+				   NV_PFIFO_CACHE1_DMA_FETCH_SIZE_128_BYTES |
 #ifdef __BIG_ENDIAN
-			      NV_PFIFO_CACHE1_BIG_ENDIAN |
+				   NV_PFIFO_CACHE1_BIG_ENDIAN |
 #endif
-			      0);
-
-	/* enable the fifo dma operation */
-	nv_wr32(dev, NV04_PFIFO_MODE,
-		nv_rd32(dev, NV04_PFIFO_MODE) | (1 << chan->id));
-	return 0;
-}
-
-static void
-nv10_fifo_do_load_context(struct drm_device *dev, int chid)
-{
-	struct drm_nouveau_private *dev_priv = dev->dev_private;
-	uint32_t fc = NV10_RAMFC(chid), tmp;
-
-	nv_wr32(dev, NV04_PFIFO_CACHE1_DMA_PUT, nv_ri32(dev, fc + 0));
-	nv_wr32(dev, NV04_PFIFO_CACHE1_DMA_GET, nv_ri32(dev, fc + 4));
-	nv_wr32(dev, NV10_PFIFO_CACHE1_REF_CNT, nv_ri32(dev, fc + 8));
-
-	tmp = nv_ri32(dev, fc + 12);
-	nv_wr32(dev, NV04_PFIFO_CACHE1_DMA_INSTANCE, tmp & 0xFFFF);
-	nv_wr32(dev, NV04_PFIFO_CACHE1_DMA_DCOUNT, tmp >> 16);
-
-	nv_wr32(dev, NV04_PFIFO_CACHE1_DMA_STATE, nv_ri32(dev, fc + 16));
-	nv_wr32(dev, NV04_PFIFO_CACHE1_DMA_FETCH, nv_ri32(dev, fc + 20));
-	nv_wr32(dev, NV04_PFIFO_CACHE1_ENGINE, nv_ri32(dev, fc + 24));
-	nv_wr32(dev, NV04_PFIFO_CACHE1_PULL1, nv_ri32(dev, fc + 28));
-
-	if (dev_priv->chipset < 0x17)
-		goto out;
-
-	nv_wr32(dev, NV10_PFIFO_CACHE1_ACQUIRE_VALUE, nv_ri32(dev, fc + 32));
-	tmp = nv_ri32(dev, fc + 36);
-	nv_wr32(dev, NV10_PFIFO_CACHE1_ACQUIRE_TIMESTAMP, tmp);
-	nv_wr32(dev, NV10_PFIFO_CACHE1_ACQUIRE_TIMEOUT, nv_ri32(dev, fc + 40));
-	nv_wr32(dev, NV10_PFIFO_CACHE1_SEMAPHORE, nv_ri32(dev, fc + 44));
-	nv_wr32(dev, NV10_PFIFO_CACHE1_DMA_SUBROUTINE, nv_ri32(dev, fc + 48));
-
-out:
-	nv_wr32(dev, NV03_PFIFO_CACHE1_GET, 0);
-	nv_wr32(dev, NV03_PFIFO_CACHE1_PUT, 0);
-}
-
-int
-nv10_fifo_load_context(struct nouveau_channel *chan)
-{
-	struct drm_device *dev = chan->dev;
-	uint32_t tmp;
-
-	nv10_fifo_do_load_context(dev, chan->id);
+				   NV_PFIFO_CACHE1_DMA_FETCH_MAX_REQS_8);
+	nv_wo32(fctx->ramfc, 0x18, 0x00000000);
+	nv_wo32(fctx->ramfc, 0x1c, 0x00000000);
 
-	nv_wr32(dev, NV03_PFIFO_CACHE1_PUSH1,
-		     NV03_PFIFO_CACHE1_PUSH1_DMA | chan->id);
-	nv_wr32(dev, NV04_PFIFO_CACHE1_DMA_PUSH, 1);
+	/* enable dma mode on the channel */
+	spin_lock_irqsave(&dev_priv->context_switch_lock, flags);
+	nv_mask(dev, NV04_PFIFO_MODE, (1 << chan->id), (1 << chan->id));
+	spin_unlock_irqrestore(&dev_priv->context_switch_lock, flags);
 
-	/* Reset NV04_PFIFO_CACHE1_DMA_CTL_AT_INFO to INVALID */
-	tmp = nv_rd32(dev, NV04_PFIFO_CACHE1_DMA_CTL) & ~(1 << 31);
-	nv_wr32(dev, NV04_PFIFO_CACHE1_DMA_CTL, tmp);
-
-	return 0;
+error:
+	if (ret)
+		priv->base.base.context_del(chan, engine);
+	return ret;
 }
 
 int
-nv10_fifo_unload_context(struct drm_device *dev)
-{
-	struct drm_nouveau_private *dev_priv = dev->dev_private;
-	struct nouveau_fifo_engine *pfifo = &dev_priv->engine.fifo;
-	uint32_t fc, tmp;
-	int chid;
-
-	chid = pfifo->channel_id(dev);
-	if (chid < 0 || chid >= dev_priv->engine.fifo.channels)
-		return 0;
-	fc = NV10_RAMFC(chid);
-
-	nv_wi32(dev, fc +  0, nv_rd32(dev, NV04_PFIFO_CACHE1_DMA_PUT));
-	nv_wi32(dev, fc +  4, nv_rd32(dev, NV04_PFIFO_CACHE1_DMA_GET));
-	nv_wi32(dev, fc +  8, nv_rd32(dev, NV10_PFIFO_CACHE1_REF_CNT));
-	tmp  = nv_rd32(dev, NV04_PFIFO_CACHE1_DMA_INSTANCE) & 0xFFFF;
-	tmp |= (nv_rd32(dev, NV04_PFIFO_CACHE1_DMA_DCOUNT) << 16);
-	nv_wi32(dev, fc + 12, tmp);
-	nv_wi32(dev, fc + 16, nv_rd32(dev, NV04_PFIFO_CACHE1_DMA_STATE));
-	nv_wi32(dev, fc + 20, nv_rd32(dev, NV04_PFIFO_CACHE1_DMA_FETCH));
-	nv_wi32(dev, fc + 24, nv_rd32(dev, NV04_PFIFO_CACHE1_ENGINE));
-	nv_wi32(dev, fc + 28, nv_rd32(dev, NV04_PFIFO_CACHE1_PULL1));
-
-	if (dev_priv->chipset < 0x17)
-		goto out;
-
-	nv_wi32(dev, fc + 32, nv_rd32(dev, NV10_PFIFO_CACHE1_ACQUIRE_VALUE));
-	tmp = nv_rd32(dev, NV10_PFIFO_CACHE1_ACQUIRE_TIMESTAMP);
-	nv_wi32(dev, fc + 36, tmp);
-	nv_wi32(dev, fc + 40, nv_rd32(dev, NV10_PFIFO_CACHE1_ACQUIRE_TIMEOUT));
-	nv_wi32(dev, fc + 44, nv_rd32(dev, NV10_PFIFO_CACHE1_SEMAPHORE));
-	nv_wi32(dev, fc + 48, nv_rd32(dev, NV04_PFIFO_CACHE1_DMA_GET));
-
-out:
-	nv10_fifo_do_load_context(dev, pfifo->channels - 1);
-	nv_wr32(dev, NV03_PFIFO_CACHE1_PUSH1, pfifo->channels - 1);
-	return 0;
-}
-
-static void
-nv10_fifo_init_reset(struct drm_device *dev)
-{
-	nv_wr32(dev, NV03_PMC_ENABLE,
-		nv_rd32(dev, NV03_PMC_ENABLE) & ~NV_PMC_ENABLE_PFIFO);
-	nv_wr32(dev, NV03_PMC_ENABLE,
-		nv_rd32(dev, NV03_PMC_ENABLE) |  NV_PMC_ENABLE_PFIFO);
-
-	nv_wr32(dev, 0x003224, 0x000f0078);
-	nv_wr32(dev, 0x002044, 0x0101ffff);
-	nv_wr32(dev, 0x002040, 0x000000ff);
-	nv_wr32(dev, 0x002500, 0x00000000);
-	nv_wr32(dev, 0x003000, 0x00000000);
-	nv_wr32(dev, 0x003050, 0x00000000);
-
-	nv_wr32(dev, 0x003258, 0x00000000);
-	nv_wr32(dev, 0x003210, 0x00000000);
-	nv_wr32(dev, 0x003270, 0x00000000);
-}
-
-static void
-nv10_fifo_init_ramxx(struct drm_device *dev)
+nv10_fifo_create(struct drm_device *dev)
 {
 	struct drm_nouveau_private *dev_priv = dev->dev_private;
+	struct nv10_fifo_priv *priv;
 
-	nv_wr32(dev, NV03_PFIFO_RAMHT, (0x03 << 24) /* search 128 */ |
-				       ((dev_priv->ramht->bits - 9) << 16) |
-				       (dev_priv->ramht->gpuobj->pinst >> 8));
-	nv_wr32(dev, NV03_PFIFO_RAMRO, dev_priv->ramro->pinst >> 8);
+	priv = kzalloc(sizeof(*priv), GFP_KERNEL);
+	if (!priv)
+		return -ENOMEM;
 
-	if (dev_priv->chipset < 0x17) {
-		nv_wr32(dev, NV03_PFIFO_RAMFC, dev_priv->ramfc->pinst >> 8);
-	} else {
-		nv_wr32(dev, NV03_PFIFO_RAMFC, (dev_priv->ramfc->pinst >> 8) |
-					       (1 << 16) /* 64 Bytes entry*/);
-		/* XXX nvidia blob set bit 18, 21,23 for nv20 & nv30 */
-	}
-}
+	priv->base.base.destroy = nv04_fifo_destroy;
+	priv->base.base.init = nv04_fifo_init;
+	priv->base.base.fini = nv04_fifo_fini;
+	priv->base.base.context_new = nv10_fifo_context_new;
+	priv->base.base.context_del = nv04_fifo_context_del;
+	priv->base.channels = 31;
+	priv->ramfc_desc = nv10_ramfc;
+	dev_priv->eng[NVOBJ_ENGINE_FIFO] = &priv->base.base;
 
-static void
-nv10_fifo_init_intr(struct drm_device *dev)
-{
 	nouveau_irq_register(dev, 8, nv04_fifo_isr);
-	nv_wr32(dev, 0x002100, 0xffffffff);
-	nv_wr32(dev, 0x002140, 0xffffffff);
-}
-
-int
-nv10_fifo_init(struct drm_device *dev)
-{
-	struct drm_nouveau_private *dev_priv = dev->dev_private;
-	struct nouveau_fifo_engine *pfifo = &dev_priv->engine.fifo;
-	int i;
-
-	nv10_fifo_init_reset(dev);
-	nv10_fifo_init_ramxx(dev);
-
-	nv10_fifo_do_load_context(dev, pfifo->channels - 1);
-	nv_wr32(dev, NV03_PFIFO_CACHE1_PUSH1, pfifo->channels - 1);
-
-	nv10_fifo_init_intr(dev);
-	pfifo->enable(dev);
-	pfifo->reassign(dev, true);
-
-	for (i = 0; i < dev_priv->engine.fifo.channels; i++) {
-		if (dev_priv->channels.ptr[i]) {
-			uint32_t mode = nv_rd32(dev, NV04_PFIFO_MODE);
-			nv_wr32(dev, NV04_PFIFO_MODE, mode | (1 << i));
-		}
-	}
-
 	return 0;
 }
diff --git a/drivers/gpu/drm/nouveau/nv10_graph.c b/drivers/gpu/drm/nouveau/nv10_graph.c
index 7255e4a4d3f..fb1d88a951d 100644
--- a/drivers/gpu/drm/nouveau/nv10_graph.c
+++ b/drivers/gpu/drm/nouveau/nv10_graph.c
@@ -759,7 +759,6 @@ static int
 nv10_graph_unload_context(struct drm_device *dev)
 {
 	struct drm_nouveau_private *dev_priv = dev->dev_private;
-	struct nouveau_fifo_engine *pfifo = &dev_priv->engine.fifo;
 	struct nouveau_channel *chan;
 	struct graph_state *ctx;
 	uint32_t tmp;
@@ -782,7 +781,7 @@ nv10_graph_unload_context(struct drm_device *dev)
 
 	nv_wr32(dev, NV10_PGRAPH_CTX_CONTROL, 0x10000000);
 	tmp  = nv_rd32(dev, NV10_PGRAPH_CTX_USER) & 0x00ffffff;
-	tmp |= (pfifo->channels - 1) << 24;
+	tmp |= 31 << 24;
 	nv_wr32(dev, NV10_PGRAPH_CTX_USER, tmp);
 	return 0;
 }
@@ -822,12 +821,12 @@ struct nouveau_channel *
 nv10_graph_channel(struct drm_device *dev)
 {
 	struct drm_nouveau_private *dev_priv = dev->dev_private;
-	int chid = dev_priv->engine.fifo.channels;
+	int chid = 31;
 
 	if (nv_rd32(dev, NV10_PGRAPH_CTX_CONTROL) & 0x00010000)
 		chid = nv_rd32(dev, NV10_PGRAPH_CTX_USER) >> 24;
 
-	if (chid >= dev_priv->engine.fifo.channels)
+	if (chid >= 31)
 		return NULL;
 
 	return dev_priv->channels.ptr[chid];
@@ -948,7 +947,7 @@ nv10_graph_init(struct drm_device *dev, int engine)
 	nv_wr32(dev, NV10_PGRAPH_STATE, 0xFFFFFFFF);
 
 	tmp  = nv_rd32(dev, NV10_PGRAPH_CTX_USER) & 0x00ffffff;
-	tmp |= (dev_priv->engine.fifo.channels - 1) << 24;
+	tmp |= 31 << 24;
 	nv_wr32(dev, NV10_PGRAPH_CTX_USER, tmp);
 	nv_wr32(dev, NV10_PGRAPH_CTX_CONTROL, 0x10000100);
 	nv_wr32(dev, NV10_PGRAPH_FFINTFC_ST2, 0x08000000);
@@ -1153,10 +1152,6 @@ nv10_graph_create(struct drm_device *dev)
 	NVOBJ_ENGINE_ADD(dev, GR, &pgraph->base);
 	nouveau_irq_register(dev, 12, nv10_graph_isr);
 
-	/* nvsw */
-	NVOBJ_CLASS(dev, 0x506e, SW);
-	NVOBJ_MTHD (dev, 0x506e, 0x0500, nv04_graph_mthd_page_flip);
-
 	NVOBJ_CLASS(dev, 0x0030, GR); /* null */
 	NVOBJ_CLASS(dev, 0x0039, GR); /* m2mf */
 	NVOBJ_CLASS(dev, 0x004a, GR); /* gdirect */
diff --git a/drivers/gpu/drm/nouveau/nv17_fifo.c b/drivers/gpu/drm/nouveau/nv17_fifo.c
new file mode 100644
index 00000000000..d9e482e4abe
--- /dev/null
+++ b/drivers/gpu/drm/nouveau/nv17_fifo.c
@@ -0,0 +1,177 @@
+/*
+ * Copyright (C) 2012 Ben Skeggs.
+ * 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 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.
+ *
+ */
+
+#include "drmP.h"
+#include "drm.h"
+#include "nouveau_drv.h"
+#include "nouveau_fifo.h"
+#include "nouveau_util.h"
+#include "nouveau_ramht.h"
+
+static struct ramfc_desc {
+	unsigned bits:6;
+	unsigned ctxs:5;
+	unsigned ctxp:8;
+	unsigned regs:5;
+	unsigned regp;
+} nv17_ramfc[] = {
+	{ 32,  0, 0x00,  0, NV04_PFIFO_CACHE1_DMA_PUT },
+	{ 32,  0, 0x04,  0, NV04_PFIFO_CACHE1_DMA_GET },
+	{ 32,  0, 0x08,  0, NV10_PFIFO_CACHE1_REF_CNT },
+	{ 16,  0, 0x0c,  0, NV04_PFIFO_CACHE1_DMA_INSTANCE },
+	{ 16, 16, 0x0c,  0, NV04_PFIFO_CACHE1_DMA_DCOUNT },
+	{ 32,  0, 0x10,  0, NV04_PFIFO_CACHE1_DMA_STATE },
+	{ 32,  0, 0x14,  0, NV04_PFIFO_CACHE1_DMA_FETCH },
+	{ 32,  0, 0x18,  0, NV04_PFIFO_CACHE1_ENGINE },
+	{ 32,  0, 0x1c,  0, NV04_PFIFO_CACHE1_PULL1 },
+	{ 32,  0, 0x20,  0, NV10_PFIFO_CACHE1_ACQUIRE_VALUE },
+	{ 32,  0, 0x24,  0, NV10_PFIFO_CACHE1_ACQUIRE_TIMESTAMP },
+	{ 32,  0, 0x28,  0, NV10_PFIFO_CACHE1_ACQUIRE_TIMEOUT },
+	{ 32,  0, 0x2c,  0, NV10_PFIFO_CACHE1_SEMAPHORE },
+	{ 32,  0, 0x30,  0, NV10_PFIFO_CACHE1_DMA_SUBROUTINE },
+	{}
+};
+
+struct nv17_fifo_priv {
+	struct nouveau_fifo_priv base;
+	struct ramfc_desc *ramfc_desc;
+};
+
+struct nv17_fifo_chan {
+	struct nouveau_fifo_chan base;
+	struct nouveau_gpuobj *ramfc;
+};
+
+static int
+nv17_fifo_context_new(struct nouveau_channel *chan, int engine)
+{
+	struct drm_device *dev = chan->dev;
+	struct drm_nouveau_private *dev_priv = dev->dev_private;
+	struct nv17_fifo_priv *priv = nv_engine(dev, engine);
+	struct nv17_fifo_chan *fctx;
+	unsigned long flags;
+	int ret;
+
+	fctx = chan->engctx[engine] = kzalloc(sizeof(*fctx), GFP_KERNEL);
+	if (!fctx)
+		return -ENOMEM;
+
+	/* map channel control registers */
+	chan->user = ioremap(pci_resource_start(dev->pdev, 0) +
+			     NV03_USER(chan->id), PAGE_SIZE);
+	if (!chan->user) {
+		ret = -ENOMEM;
+		goto error;
+	}
+
+	/* initialise default fifo context */
+	ret = nouveau_gpuobj_new_fake(dev, dev_priv->ramfc->pinst +
+				      chan->id * 64, ~0, 64,
+				      NVOBJ_FLAG_ZERO_ALLOC |
+				      NVOBJ_FLAG_ZERO_FREE, &fctx->ramfc);
+	if (ret)
+		goto error;
+
+	nv_wo32(fctx->ramfc, 0x00, chan->pushbuf_base);
+	nv_wo32(fctx->ramfc, 0x04, chan->pushbuf_base);
+	nv_wo32(fctx->ramfc, 0x0c, chan->pushbuf->pinst >> 4);
+	nv_wo32(fctx->ramfc, 0x14, NV_PFIFO_CACHE1_DMA_FETCH_TRIG_128_BYTES |
+				   NV_PFIFO_CACHE1_DMA_FETCH_SIZE_128_BYTES |
+#ifdef __BIG_ENDIAN
+				   NV_PFIFO_CACHE1_BIG_ENDIAN |
+#endif
+				   NV_PFIFO_CACHE1_DMA_FETCH_MAX_REQS_8);
+
+	/* enable dma mode on the channel */
+	spin_lock_irqsave(&dev_priv->context_switch_lock, flags);
+	nv_mask(dev, NV04_PFIFO_MODE, (1 << chan->id), (1 << chan->id));
+	spin_unlock_irqrestore(&dev_priv->context_switch_lock, flags);
+
+error:
+	if (ret)
+		priv->base.base.context_del(chan, engine);
+	return ret;
+}
+
+static int
+nv17_fifo_init(struct drm_device *dev, int engine)
+{
+	struct drm_nouveau_private *dev_priv = dev->dev_private;
+	struct nv17_fifo_priv *priv = nv_engine(dev, engine);
+	int i;
+
+	nv_mask(dev, NV03_PMC_ENABLE, NV_PMC_ENABLE_PFIFO, 0);
+	nv_mask(dev, NV03_PMC_ENABLE, NV_PMC_ENABLE_PFIFO, NV_PMC_ENABLE_PFIFO);
+
+	nv_wr32(dev, NV04_PFIFO_DELAY_0, 0x000000ff);
+	nv_wr32(dev, NV04_PFIFO_DMA_TIMESLICE, 0x0101ffff);
+
+	nv_wr32(dev, NV03_PFIFO_RAMHT, (0x03 << 24) /* search 128 */ |
+				       ((dev_priv->ramht->bits - 9) << 16) |
+				       (dev_priv->ramht->gpuobj->pinst >> 8));
+	nv_wr32(dev, NV03_PFIFO_RAMRO, dev_priv->ramro->pinst >> 8);
+	nv_wr32(dev, NV03_PFIFO_RAMFC, 0x00010000 |
+				       dev_priv->ramfc->pinst >> 8);
+
+	nv_wr32(dev, NV03_PFIFO_CACHE1_PUSH1, priv->base.channels);
+
+	nv_wr32(dev, NV03_PFIFO_INTR_0, 0xffffffff);
+	nv_wr32(dev, NV03_PFIFO_INTR_EN_0, 0xffffffff);
+
+	nv_wr32(dev, NV03_PFIFO_CACHE1_PUSH0, 1);
+	nv_wr32(dev, NV04_PFIFO_CACHE1_PULL0, 1);
+	nv_wr32(dev, NV03_PFIFO_CACHES, 1);
+
+	for (i = 0; i < priv->base.channels; i++) {
+		if (dev_priv->channels.ptr[i])
+			nv_mask(dev, NV04_PFIFO_MODE, (1 << i), (1 << i));
+	}
+
+	return 0;
+}
+
+int
+nv17_fifo_create(struct drm_device *dev)
+{
+	struct drm_nouveau_private *dev_priv = dev->dev_private;
+	struct nv17_fifo_priv *priv;
+
+	priv = kzalloc(sizeof(*priv), GFP_KERNEL);
+	if (!priv)
+		return -ENOMEM;
+
+	priv->base.base.destroy = nv04_fifo_destroy;
+	priv->base.base.init = nv17_fifo_init;
+	priv->base.base.fini = nv04_fifo_fini;
+	priv->base.base.context_new = nv17_fifo_context_new;
+	priv->base.base.context_del = nv04_fifo_context_del;
+	priv->base.channels = 31;
+	priv->ramfc_desc = nv17_ramfc;
+	dev_priv->eng[NVOBJ_ENGINE_FIFO] = &priv->base.base;
+
+	nouveau_irq_register(dev, 8, nv04_fifo_isr);
+	return 0;
+}
diff --git a/drivers/gpu/drm/nouveau/nv20_graph.c b/drivers/gpu/drm/nouveau/nv20_graph.c
index 183e37512ef..e34ea30758f 100644
--- a/drivers/gpu/drm/nouveau/nv20_graph.c
+++ b/drivers/gpu/drm/nouveau/nv20_graph.c
@@ -43,8 +43,6 @@ struct nv20_graph_engine {
 int
 nv20_graph_unload_context(struct drm_device *dev)
 {
-	struct drm_nouveau_private *dev_priv = dev->dev_private;
-	struct nouveau_fifo_engine *pfifo = &dev_priv->engine.fifo;
 	struct nouveau_channel *chan;
 	struct nouveau_gpuobj *grctx;
 	u32 tmp;
@@ -62,7 +60,7 @@ nv20_graph_unload_context(struct drm_device *dev)
 
 	nv_wr32(dev, NV10_PGRAPH_CTX_CONTROL, 0x10000000);
 	tmp  = nv_rd32(dev, NV10_PGRAPH_CTX_USER) & 0x00ffffff;
-	tmp |= (pfifo->channels - 1) << 24;
+	tmp |= 31 << 24;
 	nv_wr32(dev, NV10_PGRAPH_CTX_USER, tmp);
 	return 0;
 }
@@ -796,10 +794,6 @@ nv20_graph_create(struct drm_device *dev)
 	NVOBJ_ENGINE_ADD(dev, GR, &pgraph->base);
 	nouveau_irq_register(dev, 12, nv20_graph_isr);
 
-	/* nvsw */
-	NVOBJ_CLASS(dev, 0x506e, SW);
-	NVOBJ_MTHD (dev, 0x506e, 0x0500, nv04_graph_mthd_page_flip);
-
 	NVOBJ_CLASS(dev, 0x0030, GR); /* null */
 	NVOBJ_CLASS(dev, 0x0039, GR); /* m2mf */
 	NVOBJ_CLASS(dev, 0x004a, GR); /* gdirect */
diff --git a/drivers/gpu/drm/nouveau/nv31_mpeg.c b/drivers/gpu/drm/nouveau/nv31_mpeg.c
index 6f06a0713f0..5f239bf658c 100644
--- a/drivers/gpu/drm/nouveau/nv31_mpeg.c
+++ b/drivers/gpu/drm/nouveau/nv31_mpeg.c
@@ -24,6 +24,7 @@
 
 #include "drmP.h"
 #include "nouveau_drv.h"
+#include "nouveau_fifo.h"
 #include "nouveau_ramht.h"
 
 struct nv31_mpeg_engine {
@@ -208,6 +209,7 @@ nv31_mpeg_mthd_dma(struct nouveau_channel *chan, u32 class, u32 mthd, u32 data)
 static int
 nv31_mpeg_isr_chid(struct drm_device *dev, u32 inst)
 {
+	struct nouveau_fifo_priv *pfifo = nv_engine(dev, NVOBJ_ENGINE_FIFO);
 	struct drm_nouveau_private *dev_priv = dev->dev_private;
 	struct nouveau_gpuobj *ctx;
 	unsigned long flags;
@@ -218,7 +220,7 @@ nv31_mpeg_isr_chid(struct drm_device *dev, u32 inst)
 		return 0;
 
 	spin_lock_irqsave(&dev_priv->channels.lock, flags);
-	for (i = 0; i < dev_priv->engine.fifo.channels; i++) {
+	for (i = 0; i < pfifo->channels; i++) {
 		if (!dev_priv->channels.ptr[i])
 			continue;
 
diff --git a/drivers/gpu/drm/nouveau/nv40_fifo.c b/drivers/gpu/drm/nouveau/nv40_fifo.c
index 68cb2d991c8..cdc818479b0 100644
--- a/drivers/gpu/drm/nouveau/nv40_fifo.c
+++ b/drivers/gpu/drm/nouveau/nv40_fifo.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (C) 2007 Ben Skeggs.
+ * Copyright (C) 2012 Ben Skeggs.
  * All Rights Reserved.
  *
  * Permission is hereby granted, free of charge, to any person obtaining
@@ -25,215 +25,123 @@
  */
 
 #include "drmP.h"
+#include "drm.h"
 #include "nouveau_drv.h"
-#include "nouveau_drm.h"
+#include "nouveau_fifo.h"
+#include "nouveau_util.h"
 #include "nouveau_ramht.h"
 
-#define NV40_RAMFC(c) (dev_priv->ramfc->pinst + ((c) * NV40_RAMFC__SIZE))
-#define NV40_RAMFC__SIZE 128
-
-int
-nv40_fifo_create_context(struct nouveau_channel *chan)
+static struct ramfc_desc {
+	unsigned bits:6;
+	unsigned ctxs:5;
+	unsigned ctxp:8;
+	unsigned regs:5;
+	unsigned regp;
+} nv40_ramfc[] = {
+	{ 32,  0, 0x00,  0, NV04_PFIFO_CACHE1_DMA_PUT },
+	{ 32,  0, 0x04,  0, NV04_PFIFO_CACHE1_DMA_GET },
+	{ 32,  0, 0x08,  0, NV10_PFIFO_CACHE1_REF_CNT },
+	{ 32,  0, 0x0c,  0, NV04_PFIFO_CACHE1_DMA_INSTANCE },
+	{ 32,  0, 0x10,  0, NV04_PFIFO_CACHE1_DMA_DCOUNT },
+	{ 32,  0, 0x14,  0, NV04_PFIFO_CACHE1_DMA_STATE },
+	{ 28,  0, 0x18,  0, NV04_PFIFO_CACHE1_DMA_FETCH },
+	{  2, 28, 0x18, 28, 0x002058 },
+	{ 32,  0, 0x1c,  0, NV04_PFIFO_CACHE1_ENGINE },
+	{ 32,  0, 0x20,  0, NV04_PFIFO_CACHE1_PULL1 },
+	{ 32,  0, 0x24,  0, NV10_PFIFO_CACHE1_ACQUIRE_VALUE },
+	{ 32,  0, 0x28,  0, NV10_PFIFO_CACHE1_ACQUIRE_TIMESTAMP },
+	{ 32,  0, 0x2c,  0, NV10_PFIFO_CACHE1_ACQUIRE_TIMEOUT },
+	{ 32,  0, 0x30,  0, NV10_PFIFO_CACHE1_SEMAPHORE },
+	{ 32,  0, 0x34,  0, NV10_PFIFO_CACHE1_DMA_SUBROUTINE },
+	{ 32,  0, 0x38,  0, NV40_PFIFO_GRCTX_INSTANCE },
+	{ 17,  0, 0x3c,  0, NV04_PFIFO_DMA_TIMESLICE },
+	{ 32,  0, 0x40,  0, 0x0032e4 },
+	{ 32,  0, 0x44,  0, 0x0032e8 },
+	{ 32,  0, 0x4c,  0, 0x002088 },
+	{ 32,  0, 0x50,  0, 0x003300 },
+	{ 32,  0, 0x54,  0, 0x00330c },
+	{}
+};
+
+struct nv40_fifo_priv {
+	struct nouveau_fifo_priv base;
+	struct ramfc_desc *ramfc_desc;
+};
+
+struct nv40_fifo_chan {
+	struct nouveau_fifo_chan base;
+	struct nouveau_gpuobj *ramfc;
+};
+
+static int
+nv40_fifo_context_new(struct nouveau_channel *chan, int engine)
 {
 	struct drm_device *dev = chan->dev;
 	struct drm_nouveau_private *dev_priv = dev->dev_private;
-	uint32_t fc = NV40_RAMFC(chan->id);
+	struct nv40_fifo_priv *priv = nv_engine(dev, engine);
+	struct nv40_fifo_chan *fctx;
 	unsigned long flags;
 	int ret;
 
-	ret = nouveau_gpuobj_new_fake(dev, NV40_RAMFC(chan->id), ~0,
-				      NV40_RAMFC__SIZE, NVOBJ_FLAG_ZERO_ALLOC |
-				      NVOBJ_FLAG_ZERO_FREE, &chan->ramfc);
-	if (ret)
-		return ret;
-
-	chan->user = ioremap(pci_resource_start(dev->pdev, 0) +
-			     NV40_USER(chan->id), PAGE_SIZE);
-	if (!chan->user)
+	fctx = chan->engctx[engine] = kzalloc(sizeof(*fctx), GFP_KERNEL);
+	if (!fctx)
 		return -ENOMEM;
 
-	spin_lock_irqsave(&dev_priv->context_switch_lock, flags);
+	/* map channel control registers */
+	chan->user = ioremap(pci_resource_start(dev->pdev, 0) +
+			     NV03_USER(chan->id), PAGE_SIZE);
+	if (!chan->user) {
+		ret = -ENOMEM;
+		goto error;
+	}
 
-	nv_wi32(dev, fc +  0, chan->pushbuf_base);
-	nv_wi32(dev, fc +  4, chan->pushbuf_base);
-	nv_wi32(dev, fc + 12, chan->pushbuf->pinst >> 4);
-	nv_wi32(dev, fc + 24, NV_PFIFO_CACHE1_DMA_FETCH_TRIG_128_BYTES |
-			      NV_PFIFO_CACHE1_DMA_FETCH_SIZE_128_BYTES |
-			      NV_PFIFO_CACHE1_DMA_FETCH_MAX_REQS_8 |
+	/* initialise default fifo context */
+	ret = nouveau_gpuobj_new_fake(dev, dev_priv->ramfc->pinst +
+				      chan->id * 128, ~0, 128,
+				      NVOBJ_FLAG_ZERO_ALLOC |
+				      NVOBJ_FLAG_ZERO_FREE, &fctx->ramfc);
+	if (ret)
+		goto error;
+
+	nv_wo32(fctx->ramfc, 0x00, chan->pushbuf_base);
+	nv_wo32(fctx->ramfc, 0x04, chan->pushbuf_base);
+	nv_wo32(fctx->ramfc, 0x0c, chan->pushbuf->pinst >> 4);
+	nv_wo32(fctx->ramfc, 0x18, 0x30000000 |
+				   NV_PFIFO_CACHE1_DMA_FETCH_TRIG_128_BYTES |
+				   NV_PFIFO_CACHE1_DMA_FETCH_SIZE_128_BYTES |
 #ifdef __BIG_ENDIAN
-			      NV_PFIFO_CACHE1_BIG_ENDIAN |
+				   NV_PFIFO_CACHE1_BIG_ENDIAN |
 #endif
-			      0x30000000 /* no idea.. */);
-	nv_wi32(dev, fc + 60, 0x0001FFFF);
-
-	/* enable the fifo dma operation */
-	nv_wr32(dev, NV04_PFIFO_MODE,
-		nv_rd32(dev, NV04_PFIFO_MODE) | (1 << chan->id));
+				   NV_PFIFO_CACHE1_DMA_FETCH_MAX_REQS_8);
+	nv_wo32(fctx->ramfc, 0x3c, 0x0001ffff);
 
+	/* enable dma mode on the channel */
+	spin_lock_irqsave(&dev_priv->context_switch_lock, flags);
+	nv_mask(dev, NV04_PFIFO_MODE, (1 << chan->id), (1 << chan->id));
 	spin_unlock_irqrestore(&dev_priv->context_switch_lock, flags);
-	return 0;
-}
-
-static void
-nv40_fifo_do_load_context(struct drm_device *dev, int chid)
-{
-	struct drm_nouveau_private *dev_priv = dev->dev_private;
-	uint32_t fc = NV40_RAMFC(chid), tmp, tmp2;
-
-	nv_wr32(dev, NV04_PFIFO_CACHE1_DMA_PUT, nv_ri32(dev, fc + 0));
-	nv_wr32(dev, NV04_PFIFO_CACHE1_DMA_GET, nv_ri32(dev, fc + 4));
-	nv_wr32(dev, NV10_PFIFO_CACHE1_REF_CNT, nv_ri32(dev, fc + 8));
-	nv_wr32(dev, NV04_PFIFO_CACHE1_DMA_INSTANCE, nv_ri32(dev, fc + 12));
-	nv_wr32(dev, NV04_PFIFO_CACHE1_DMA_DCOUNT, nv_ri32(dev, fc + 16));
-	nv_wr32(dev, NV04_PFIFO_CACHE1_DMA_STATE, nv_ri32(dev, fc + 20));
-
-	/* No idea what 0x2058 is.. */
-	tmp   = nv_ri32(dev, fc + 24);
-	tmp2  = nv_rd32(dev, 0x2058) & 0xFFF;
-	tmp2 |= (tmp & 0x30000000);
-	nv_wr32(dev, 0x2058, tmp2);
-	tmp  &= ~0x30000000;
-	nv_wr32(dev, NV04_PFIFO_CACHE1_DMA_FETCH, tmp);
 
-	nv_wr32(dev, NV04_PFIFO_CACHE1_ENGINE, nv_ri32(dev, fc + 28));
-	nv_wr32(dev, NV04_PFIFO_CACHE1_PULL1, nv_ri32(dev, fc + 32));
-	nv_wr32(dev, NV10_PFIFO_CACHE1_ACQUIRE_VALUE, nv_ri32(dev, fc + 36));
-	tmp = nv_ri32(dev, fc + 40);
-	nv_wr32(dev, NV10_PFIFO_CACHE1_ACQUIRE_TIMESTAMP, tmp);
-	nv_wr32(dev, NV10_PFIFO_CACHE1_ACQUIRE_TIMEOUT, nv_ri32(dev, fc + 44));
-	nv_wr32(dev, NV10_PFIFO_CACHE1_SEMAPHORE, nv_ri32(dev, fc + 48));
-	nv_wr32(dev, NV10_PFIFO_CACHE1_DMA_SUBROUTINE, nv_ri32(dev, fc + 52));
-	nv_wr32(dev, NV40_PFIFO_GRCTX_INSTANCE, nv_ri32(dev, fc + 56));
+	/*XXX: remove this later, need fifo engine context commit hook */
+	nouveau_gpuobj_ref(fctx->ramfc, &chan->ramfc);
 
-	/* Don't clobber the TIMEOUT_ENABLED flag when restoring from RAMFC */
-	tmp  = nv_rd32(dev, NV04_PFIFO_DMA_TIMESLICE) & ~0x1FFFF;
-	tmp |= nv_ri32(dev, fc + 60) & 0x1FFFF;
-	nv_wr32(dev, NV04_PFIFO_DMA_TIMESLICE, tmp);
-
-	nv_wr32(dev, 0x32e4, nv_ri32(dev, fc + 64));
-	/* NVIDIA does this next line twice... */
-	nv_wr32(dev, 0x32e8, nv_ri32(dev, fc + 68));
-	nv_wr32(dev, 0x2088, nv_ri32(dev, fc + 76));
-	nv_wr32(dev, 0x3300, nv_ri32(dev, fc + 80));
-	nv_wr32(dev, 0x330c, nv_ri32(dev, fc + 84));
-
-	nv_wr32(dev, NV03_PFIFO_CACHE1_GET, 0);
-	nv_wr32(dev, NV03_PFIFO_CACHE1_PUT, 0);
-}
-
-int
-nv40_fifo_load_context(struct nouveau_channel *chan)
-{
-	struct drm_device *dev = chan->dev;
-	uint32_t tmp;
-
-	nv40_fifo_do_load_context(dev, chan->id);
-
-	/* Set channel active, and in DMA mode */
-	nv_wr32(dev, NV03_PFIFO_CACHE1_PUSH1,
-		     NV40_PFIFO_CACHE1_PUSH1_DMA | chan->id);
-	nv_wr32(dev, NV04_PFIFO_CACHE1_DMA_PUSH, 1);
-
-	/* Reset DMA_CTL_AT_INFO to INVALID */
-	tmp = nv_rd32(dev, NV04_PFIFO_CACHE1_DMA_CTL) & ~(1 << 31);
-	nv_wr32(dev, NV04_PFIFO_CACHE1_DMA_CTL, tmp);
-
-	return 0;
+error:
+	if (ret)
+		priv->base.base.context_del(chan, engine);
+	return ret;
 }
 
-int
-nv40_fifo_unload_context(struct drm_device *dev)
+static int
+nv40_fifo_init(struct drm_device *dev, int engine)
 {
 	struct drm_nouveau_private *dev_priv = dev->dev_private;
-	struct nouveau_fifo_engine *pfifo = &dev_priv->engine.fifo;
-	uint32_t fc, tmp;
-	int chid;
-
-	chid = pfifo->channel_id(dev);
-	if (chid < 0 || chid >= dev_priv->engine.fifo.channels)
-		return 0;
-	fc = NV40_RAMFC(chid);
-
-	nv_wi32(dev, fc + 0, nv_rd32(dev, NV04_PFIFO_CACHE1_DMA_PUT));
-	nv_wi32(dev, fc + 4, nv_rd32(dev, NV04_PFIFO_CACHE1_DMA_GET));
-	nv_wi32(dev, fc + 8, nv_rd32(dev, NV10_PFIFO_CACHE1_REF_CNT));
-	nv_wi32(dev, fc + 12, nv_rd32(dev, NV04_PFIFO_CACHE1_DMA_INSTANCE));
-	nv_wi32(dev, fc + 16, nv_rd32(dev, NV04_PFIFO_CACHE1_DMA_DCOUNT));
-	nv_wi32(dev, fc + 20, nv_rd32(dev, NV04_PFIFO_CACHE1_DMA_STATE));
-	tmp  = nv_rd32(dev, NV04_PFIFO_CACHE1_DMA_FETCH);
-	tmp |= nv_rd32(dev, 0x2058) & 0x30000000;
-	nv_wi32(dev, fc + 24, tmp);
-	nv_wi32(dev, fc + 28, nv_rd32(dev, NV04_PFIFO_CACHE1_ENGINE));
-	nv_wi32(dev, fc + 32, nv_rd32(dev, NV04_PFIFO_CACHE1_PULL1));
-	nv_wi32(dev, fc + 36, nv_rd32(dev, NV10_PFIFO_CACHE1_ACQUIRE_VALUE));
-	tmp = nv_rd32(dev, NV10_PFIFO_CACHE1_ACQUIRE_TIMESTAMP);
-	nv_wi32(dev, fc + 40, tmp);
-	nv_wi32(dev, fc + 44, nv_rd32(dev, NV10_PFIFO_CACHE1_ACQUIRE_TIMEOUT));
-	nv_wi32(dev, fc + 48, nv_rd32(dev, NV10_PFIFO_CACHE1_SEMAPHORE));
-	/* NVIDIA read 0x3228 first, then write DMA_GET here.. maybe something
-	 * more involved depending on the value of 0x3228?
-	 */
-	nv_wi32(dev, fc + 52, nv_rd32(dev, NV04_PFIFO_CACHE1_DMA_GET));
-	nv_wi32(dev, fc + 56, nv_rd32(dev, NV40_PFIFO_GRCTX_INSTANCE));
-	nv_wi32(dev, fc + 60, nv_rd32(dev, NV04_PFIFO_DMA_TIMESLICE) & 0x1ffff);
-	/* No idea what the below is for exactly, ripped from a mmio-trace */
-	nv_wi32(dev, fc + 64, nv_rd32(dev, NV40_PFIFO_UNK32E4));
-	/* NVIDIA do this next line twice.. bug? */
-	nv_wi32(dev, fc + 68, nv_rd32(dev, 0x32e8));
-	nv_wi32(dev, fc + 76, nv_rd32(dev, 0x2088));
-	nv_wi32(dev, fc + 80, nv_rd32(dev, 0x3300));
-#if 0 /* no real idea which is PUT/GET in UNK_48.. */
-	tmp  = nv_rd32(dev, NV04_PFIFO_CACHE1_GET);
-	tmp |= (nv_rd32(dev, NV04_PFIFO_CACHE1_PUT) << 16);
-	nv_wi32(dev, fc + 72, tmp);
-#endif
-	nv_wi32(dev, fc + 84, nv_rd32(dev, 0x330c));
-
-	nv40_fifo_do_load_context(dev, pfifo->channels - 1);
-	nv_wr32(dev, NV03_PFIFO_CACHE1_PUSH1,
-		     NV40_PFIFO_CACHE1_PUSH1_DMA | (pfifo->channels - 1));
-	return 0;
-}
-
-static void
-nv40_fifo_init_reset(struct drm_device *dev)
-{
+	struct nv40_fifo_priv *priv = nv_engine(dev, engine);
 	int i;
 
-	nv_wr32(dev, NV03_PMC_ENABLE,
-		nv_rd32(dev, NV03_PMC_ENABLE) & ~NV_PMC_ENABLE_PFIFO);
-	nv_wr32(dev, NV03_PMC_ENABLE,
-		nv_rd32(dev, NV03_PMC_ENABLE) |  NV_PMC_ENABLE_PFIFO);
+	nv_mask(dev, NV03_PMC_ENABLE, NV_PMC_ENABLE_PFIFO, 0);
+	nv_mask(dev, NV03_PMC_ENABLE, NV_PMC_ENABLE_PFIFO, NV_PMC_ENABLE_PFIFO);
 
-	nv_wr32(dev, 0x003224, 0x000f0078);
-	nv_wr32(dev, 0x003210, 0x00000000);
-	nv_wr32(dev, 0x003270, 0x00000000);
-	nv_wr32(dev, 0x003240, 0x00000000);
-	nv_wr32(dev, 0x003244, 0x00000000);
-	nv_wr32(dev, 0x003258, 0x00000000);
-	nv_wr32(dev, 0x002504, 0x00000000);
-	for (i = 0; i < 16; i++)
-		nv_wr32(dev, 0x002510 + (i * 4), 0x00000000);
-	nv_wr32(dev, 0x00250c, 0x0000ffff);
-	nv_wr32(dev, 0x002048, 0x00000000);
-	nv_wr32(dev, 0x003228, 0x00000000);
-	nv_wr32(dev, 0x0032e8, 0x00000000);
-	nv_wr32(dev, 0x002410, 0x00000000);
-	nv_wr32(dev, 0x002420, 0x00000000);
-	nv_wr32(dev, 0x002058, 0x00000001);
-	nv_wr32(dev, 0x00221c, 0x00000000);
-	/* something with 0x2084, read/modify/write, no change */
 	nv_wr32(dev, 0x002040, 0x000000ff);
-	nv_wr32(dev, 0x002500, 0x00000000);
-	nv_wr32(dev, 0x003200, 0x00000000);
-
-	nv_wr32(dev, NV04_PFIFO_DMA_TIMESLICE, 0x2101ffff);
-}
-
-static void
-nv40_fifo_init_ramxx(struct drm_device *dev)
-{
-	struct drm_nouveau_private *dev_priv = dev->dev_private;
+	nv_wr32(dev, 0x002044, 0x2101ffff);
+	nv_wr32(dev, 0x002058, 0x00000001);
 
 	nv_wr32(dev, NV03_PFIFO_RAMHT, (0x03 << 24) /* search 128 */ |
 				       ((dev_priv->ramht->bits - 9) << 16) |
@@ -244,64 +152,59 @@ nv40_fifo_init_ramxx(struct drm_device *dev)
 	case 0x47:
 	case 0x49:
 	case 0x4b:
-		nv_wr32(dev, 0x2230, 1);
-		break;
-	default:
-		break;
-	}
-
-	switch (dev_priv->chipset) {
+		nv_wr32(dev, 0x002230, 0x00000001);
 	case 0x40:
 	case 0x41:
 	case 0x42:
 	case 0x43:
 	case 0x45:
-	case 0x47:
 	case 0x48:
-	case 0x49:
-	case 0x4b:
-		nv_wr32(dev, NV40_PFIFO_RAMFC, 0x30002);
+		nv_wr32(dev, 0x002220, 0x00030002);
 		break;
 	default:
-		nv_wr32(dev, 0x2230, 0);
-		nv_wr32(dev, NV40_PFIFO_RAMFC,
-			((dev_priv->vram_size - 512 * 1024 +
-			  dev_priv->ramfc->pinst) >> 16) | (3 << 16));
+		nv_wr32(dev, 0x002230, 0x00000000);
+		nv_wr32(dev, 0x002220, ((dev_priv->vram_size - 512 * 1024 +
+					 dev_priv->ramfc->pinst) >> 16) |
+				       0x00030000);
 		break;
 	}
-}
 
-static void
-nv40_fifo_init_intr(struct drm_device *dev)
-{
-	nouveau_irq_register(dev, 8, nv04_fifo_isr);
-	nv_wr32(dev, 0x002100, 0xffffffff);
-	nv_wr32(dev, 0x002140, 0xffffffff);
+	nv_wr32(dev, NV03_PFIFO_CACHE1_PUSH1, priv->base.channels);
+
+	nv_wr32(dev, NV03_PFIFO_INTR_0, 0xffffffff);
+	nv_wr32(dev, NV03_PFIFO_INTR_EN_0, 0xffffffff);
+
+	nv_wr32(dev, NV03_PFIFO_CACHE1_PUSH0, 1);
+	nv_wr32(dev, NV04_PFIFO_CACHE1_PULL0, 1);
+	nv_wr32(dev, NV03_PFIFO_CACHES, 1);
+
+	for (i = 0; i < priv->base.channels; i++) {
+		if (dev_priv->channels.ptr[i])
+			nv_mask(dev, NV04_PFIFO_MODE, (1 << i), (1 << i));
+	}
+
+	return 0;
 }
 
 int
-nv40_fifo_init(struct drm_device *dev)
+nv40_fifo_create(struct drm_device *dev)
 {
 	struct drm_nouveau_private *dev_priv = dev->dev_private;
-	struct nouveau_fifo_engine *pfifo = &dev_priv->engine.fifo;
-	int i;
-
-	nv40_fifo_init_reset(dev);
-	nv40_fifo_init_ramxx(dev);
+	struct nv40_fifo_priv *priv;
 
-	nv40_fifo_do_load_context(dev, pfifo->channels - 1);
-	nv_wr32(dev, NV03_PFIFO_CACHE1_PUSH1, pfifo->channels - 1);
-
-	nv40_fifo_init_intr(dev);
-	pfifo->enable(dev);
-	pfifo->reassign(dev, true);
+	priv = kzalloc(sizeof(*priv), GFP_KERNEL);
+	if (!priv)
+		return -ENOMEM;
 
-	for (i = 0; i < dev_priv->engine.fifo.channels; i++) {
-		if (dev_priv->channels.ptr[i]) {
-			uint32_t mode = nv_rd32(dev, NV04_PFIFO_MODE);
-			nv_wr32(dev, NV04_PFIFO_MODE, mode | (1 << i));
-		}
-	}
+	priv->base.base.destroy = nv04_fifo_destroy;
+	priv->base.base.init = nv40_fifo_init;
+	priv->base.base.fini = nv04_fifo_fini;
+	priv->base.base.context_new = nv40_fifo_context_new;
+	priv->base.base.context_del = nv04_fifo_context_del;
+	priv->base.channels = 31;
+	priv->ramfc_desc = nv40_ramfc;
+	dev_priv->eng[NVOBJ_ENGINE_FIFO] = &priv->base.base;
 
+	nouveau_irq_register(dev, 8, nv04_fifo_isr);
 	return 0;
 }
diff --git a/drivers/gpu/drm/nouveau/nv40_graph.c b/drivers/gpu/drm/nouveau/nv40_graph.c
index ba14a93d8af..aa9e2df64a2 100644
--- a/drivers/gpu/drm/nouveau/nv40_graph.c
+++ b/drivers/gpu/drm/nouveau/nv40_graph.c
@@ -27,7 +27,7 @@
 #include "drmP.h"
 #include "drm.h"
 #include "nouveau_drv.h"
-#include "nouveau_grctx.h"
+#include "nouveau_fifo.h"
 #include "nouveau_ramht.h"
 
 struct nv40_graph_engine {
@@ -42,7 +42,6 @@ nv40_graph_context_new(struct nouveau_channel *chan, int engine)
 	struct drm_device *dev = chan->dev;
 	struct drm_nouveau_private *dev_priv = dev->dev_private;
 	struct nouveau_gpuobj *grctx = NULL;
-	struct nouveau_grctx ctx = {};
 	unsigned long flags;
 	int ret;
 
@@ -52,11 +51,7 @@ nv40_graph_context_new(struct nouveau_channel *chan, int engine)
 		return ret;
 
 	/* Initialise default context values */
-	ctx.dev = chan->dev;
-	ctx.mode = NOUVEAU_GRCTX_VALS;
-	ctx.data = grctx;
-	nv40_grctx_init(&ctx);
-
+	nv40_grctx_fill(dev, grctx);
 	nv_wo32(grctx, 0, grctx->vinst);
 
 	/* init grctx pointer in ramfc, and on PFIFO if channel is
@@ -184,8 +179,7 @@ nv40_graph_init(struct drm_device *dev, int engine)
 	struct nv40_graph_engine *pgraph = nv_engine(dev, engine);
 	struct drm_nouveau_private *dev_priv = dev->dev_private;
 	struct nouveau_fb_engine *pfb = &dev_priv->engine.fb;
-	struct nouveau_grctx ctx = {};
-	uint32_t vramsz, *cp;
+	uint32_t vramsz;
 	int i, j;
 
 	nv_wr32(dev, NV03_PMC_ENABLE, nv_rd32(dev, NV03_PMC_ENABLE) &
@@ -193,22 +187,8 @@ nv40_graph_init(struct drm_device *dev, int engine)
 	nv_wr32(dev, NV03_PMC_ENABLE, nv_rd32(dev, NV03_PMC_ENABLE) |
 			 NV_PMC_ENABLE_PGRAPH);
 
-	cp = kmalloc(sizeof(*cp) * 256, GFP_KERNEL);
-	if (!cp)
-		return -ENOMEM;
-
-	ctx.dev = dev;
-	ctx.mode = NOUVEAU_GRCTX_PROG;
-	ctx.data = cp;
-	ctx.ctxprog_max = 256;
-	nv40_grctx_init(&ctx);
-	pgraph->grctx_size = ctx.ctxvals_pos * 4;
-
-	nv_wr32(dev, NV40_PGRAPH_CTXCTL_UCODE_INDEX, 0);
-	for (i = 0; i < ctx.ctxprog_len; i++)
-		nv_wr32(dev, NV40_PGRAPH_CTXCTL_UCODE_DATA, cp[i]);
-
-	kfree(cp);
+	/* generate and upload context program */
+	nv40_grctx_init(dev, &pgraph->grctx_size);
 
 	/* No context present currently */
 	nv_wr32(dev, NV40_PGRAPH_CTXCTL_CUR, 0x00000000);
@@ -366,13 +346,14 @@ nv40_graph_fini(struct drm_device *dev, int engine, bool suspend)
 static int
 nv40_graph_isr_chid(struct drm_device *dev, u32 inst)
 {
+	struct nouveau_fifo_priv *pfifo = nv_engine(dev, NVOBJ_ENGINE_FIFO);
 	struct drm_nouveau_private *dev_priv = dev->dev_private;
 	struct nouveau_gpuobj *grctx;
 	unsigned long flags;
 	int i;
 
 	spin_lock_irqsave(&dev_priv->channels.lock, flags);
-	for (i = 0; i < dev_priv->engine.fifo.channels; i++) {
+	for (i = 0; i < pfifo->channels; i++) {
 		if (!dev_priv->channels.ptr[i])
 			continue;
 		grctx = dev_priv->channels.ptr[i]->engctx[NVOBJ_ENGINE_GR];
@@ -460,7 +441,6 @@ nv40_graph_create(struct drm_device *dev)
 	NVOBJ_ENGINE_ADD(dev, GR, &pgraph->base);
 	nouveau_irq_register(dev, 12, nv40_graph_isr);
 
-	NVOBJ_CLASS(dev, 0x506e, SW); /* nvsw */
 	NVOBJ_CLASS(dev, 0x0030, GR); /* null */
 	NVOBJ_CLASS(dev, 0x0039, GR); /* m2mf */
 	NVOBJ_CLASS(dev, 0x004a, GR); /* gdirect */
@@ -483,8 +463,5 @@ nv40_graph_create(struct drm_device *dev)
 	else
 		NVOBJ_CLASS(dev, 0x4097, GR);
 
-	/* nvsw */
-	NVOBJ_CLASS(dev, 0x506e, SW);
-	NVOBJ_MTHD (dev, 0x506e, 0x0500, nv04_graph_mthd_page_flip);
 	return 0;
 }
diff --git a/drivers/gpu/drm/nouveau/nv40_grctx.c b/drivers/gpu/drm/nouveau/nv40_grctx.c
index f70447d131d..be0a74750fb 100644
--- a/drivers/gpu/drm/nouveau/nv40_grctx.c
+++ b/drivers/gpu/drm/nouveau/nv40_grctx.c
@@ -595,8 +595,8 @@ nv40_graph_construct_shader(struct nouveau_grctx *ctx)
 	}
 }
 
-void
-nv40_grctx_init(struct nouveau_grctx *ctx)
+static void
+nv40_grctx_generate(struct nouveau_grctx *ctx)
 {
 	/* decide whether we're loading/unloading the context */
 	cp_bra (ctx, AUTO_SAVE, PENDING, cp_setup_save);
@@ -660,3 +660,31 @@ nv40_grctx_init(struct nouveau_grctx *ctx)
 	cp_out (ctx, CP_END);
 }
 
+void
+nv40_grctx_fill(struct drm_device *dev, struct nouveau_gpuobj *mem)
+{
+	nv40_grctx_generate(&(struct nouveau_grctx) {
+			     .dev = dev,
+			     .mode = NOUVEAU_GRCTX_VALS,
+			     .data = mem,
+			   });
+}
+
+void
+nv40_grctx_init(struct drm_device *dev, u32 *size)
+{
+	u32 ctxprog[256], i;
+	struct nouveau_grctx ctx = {
+		.dev = dev,
+		.mode = NOUVEAU_GRCTX_PROG,
+		.data = ctxprog,
+		.ctxprog_max = ARRAY_SIZE(ctxprog)
+	};
+
+	nv40_grctx_generate(&ctx);
+
+	nv_wr32(dev, NV40_PGRAPH_CTXCTL_UCODE_INDEX, 0);
+	for (i = 0; i < ctx.ctxprog_len; i++)
+		nv_wr32(dev, NV40_PGRAPH_CTXCTL_UCODE_DATA, ctxprog[i]);
+	*size = ctx.ctxvals_pos * 4;
+}
diff --git a/drivers/gpu/drm/nouveau/nv40_pm.c b/drivers/gpu/drm/nouveau/nv40_pm.c
index c7615381c5d..e66273aff49 100644
--- a/drivers/gpu/drm/nouveau/nv40_pm.c
+++ b/drivers/gpu/drm/nouveau/nv40_pm.c
@@ -27,6 +27,7 @@
 #include "nouveau_bios.h"
 #include "nouveau_pm.h"
 #include "nouveau_hw.h"
+#include "nouveau_fifo.h"
 
 #define min2(a,b) ((a) < (b) ? (a) : (b))
 
diff --git a/drivers/gpu/drm/nouveau/nv50_crtc.c b/drivers/gpu/drm/nouveau/nv50_crtc.c
index 701b927998b..97a477b3d52 100644
--- a/drivers/gpu/drm/nouveau/nv50_crtc.c
+++ b/drivers/gpu/drm/nouveau/nv50_crtc.c
@@ -79,15 +79,15 @@ nv50_crtc_blank(struct nouveau_crtc *nv_crtc, bool blanked)
 			NV_ERROR(dev, "no space while blanking crtc\n");
 			return ret;
 		}
-		BEGIN_RING(evo, 0, NV50_EVO_CRTC(index, CLUT_MODE), 2);
+		BEGIN_NV04(evo, 0, NV50_EVO_CRTC(index, CLUT_MODE), 2);
 		OUT_RING(evo, NV50_EVO_CRTC_CLUT_MODE_BLANK);
 		OUT_RING(evo, 0);
 		if (dev_priv->chipset != 0x50) {
-			BEGIN_RING(evo, 0, NV84_EVO_CRTC(index, CLUT_DMA), 1);
+			BEGIN_NV04(evo, 0, NV84_EVO_CRTC(index, CLUT_DMA), 1);
 			OUT_RING(evo, NV84_EVO_CRTC_CLUT_DMA_HANDLE_NONE);
 		}
 
-		BEGIN_RING(evo, 0, NV50_EVO_CRTC(index, FB_DMA), 1);
+		BEGIN_NV04(evo, 0, NV50_EVO_CRTC(index, FB_DMA), 1);
 		OUT_RING(evo, NV50_EVO_CRTC_FB_DMA_HANDLE_NONE);
 	} else {
 		if (nv_crtc->cursor.visible)
@@ -100,20 +100,20 @@ nv50_crtc_blank(struct nouveau_crtc *nv_crtc, bool blanked)
 			NV_ERROR(dev, "no space while unblanking crtc\n");
 			return ret;
 		}
-		BEGIN_RING(evo, 0, NV50_EVO_CRTC(index, CLUT_MODE), 2);
+		BEGIN_NV04(evo, 0, NV50_EVO_CRTC(index, CLUT_MODE), 2);
 		OUT_RING(evo, nv_crtc->lut.depth == 8 ?
 				NV50_EVO_CRTC_CLUT_MODE_OFF :
 				NV50_EVO_CRTC_CLUT_MODE_ON);
 		OUT_RING(evo, nv_crtc->lut.nvbo->bo.offset >> 8);
 		if (dev_priv->chipset != 0x50) {
-			BEGIN_RING(evo, 0, NV84_EVO_CRTC(index, CLUT_DMA), 1);
+			BEGIN_NV04(evo, 0, NV84_EVO_CRTC(index, CLUT_DMA), 1);
 			OUT_RING(evo, NvEvoVRAM);
 		}
 
-		BEGIN_RING(evo, 0, NV50_EVO_CRTC(index, FB_OFFSET), 2);
+		BEGIN_NV04(evo, 0, NV50_EVO_CRTC(index, FB_OFFSET), 2);
 		OUT_RING(evo, nv_crtc->fb.offset >> 8);
 		OUT_RING(evo, 0);
-		BEGIN_RING(evo, 0, NV50_EVO_CRTC(index, FB_DMA), 1);
+		BEGIN_NV04(evo, 0, NV50_EVO_CRTC(index, FB_DMA), 1);
 		if (dev_priv->chipset != 0x50)
 			if (nv_crtc->fb.tile_flags == 0x7a00 ||
 			    nv_crtc->fb.tile_flags == 0xfe00)
@@ -158,10 +158,10 @@ nv50_crtc_set_dither(struct nouveau_crtc *nv_crtc, bool update)
 
 	ret = RING_SPACE(evo, 2 + (update ? 2 : 0));
 	if (ret == 0) {
-		BEGIN_RING(evo, 0, NV50_EVO_CRTC(head, DITHER_CTRL), 1);
+		BEGIN_NV04(evo, 0, NV50_EVO_CRTC(head, DITHER_CTRL), 1);
 		OUT_RING  (evo, mode);
 		if (update) {
-			BEGIN_RING(evo, 0, NV50_EVO_UPDATE, 1);
+			BEGIN_NV04(evo, 0, NV50_EVO_UPDATE, 1);
 			OUT_RING  (evo, 0);
 			FIRE_RING (evo);
 		}
@@ -193,11 +193,11 @@ nv50_crtc_set_color_vibrance(struct nouveau_crtc *nv_crtc, bool update)
 
 	hue = ((nv_crtc->vibrant_hue * 2047) / 100) & 0xfff;
 
-	BEGIN_RING(evo, 0, NV50_EVO_CRTC(nv_crtc->index, COLOR_CTRL), 1);
+	BEGIN_NV04(evo, 0, NV50_EVO_CRTC(nv_crtc->index, COLOR_CTRL), 1);
 	OUT_RING  (evo, (hue << 20) | (vib << 8));
 
 	if (update) {
-		BEGIN_RING(evo, 0, NV50_EVO_UPDATE, 1);
+		BEGIN_NV04(evo, 0, NV50_EVO_UPDATE, 1);
 		OUT_RING  (evo, 0);
 		FIRE_RING (evo);
 	}
@@ -311,9 +311,9 @@ nv50_crtc_set_scale(struct nouveau_crtc *nv_crtc, bool update)
 	if (ret)
 		return ret;
 
-	BEGIN_RING(evo, 0, NV50_EVO_CRTC(nv_crtc->index, SCALE_CTRL), 1);
+	BEGIN_NV04(evo, 0, NV50_EVO_CRTC(nv_crtc->index, SCALE_CTRL), 1);
 	OUT_RING  (evo, ctrl);
-	BEGIN_RING(evo, 0, NV50_EVO_CRTC(nv_crtc->index, SCALE_RES1), 2);
+	BEGIN_NV04(evo, 0, NV50_EVO_CRTC(nv_crtc->index, SCALE_RES1), 2);
 	OUT_RING  (evo, oY << 16 | oX);
 	OUT_RING  (evo, oY << 16 | oX);
 
@@ -383,23 +383,15 @@ nv50_crtc_set_clock(struct drm_device *dev, int head, int pclk)
 static void
 nv50_crtc_destroy(struct drm_crtc *crtc)
 {
-	struct drm_device *dev;
-	struct nouveau_crtc *nv_crtc;
-
-	if (!crtc)
-		return;
-
-	dev = crtc->dev;
-	nv_crtc = nouveau_crtc(crtc);
-
-	NV_DEBUG_KMS(dev, "\n");
+	struct nouveau_crtc *nv_crtc = nouveau_crtc(crtc);
 
-	drm_crtc_cleanup(&nv_crtc->base);
+	NV_DEBUG_KMS(crtc->dev, "\n");
 
 	nouveau_bo_unmap(nv_crtc->lut.nvbo);
 	nouveau_bo_ref(NULL, &nv_crtc->lut.nvbo);
 	nouveau_bo_unmap(nv_crtc->cursor.nvbo);
 	nouveau_bo_ref(NULL, &nv_crtc->cursor.nvbo);
+	drm_crtc_cleanup(&nv_crtc->base);
 	kfree(nv_crtc);
 }
 
@@ -593,7 +585,7 @@ nv50_crtc_do_mode_set_base(struct drm_crtc *crtc,
 		if (ret)
 			return ret;
 
-		BEGIN_RING(evo, 0, NV50_EVO_CRTC(nv_crtc->index, FB_DMA), 1);
+		BEGIN_NV04(evo, 0, NV50_EVO_CRTC(nv_crtc->index, FB_DMA), 1);
 		OUT_RING  (evo, fb->r_dma);
 	}
 
@@ -601,18 +593,18 @@ nv50_crtc_do_mode_set_base(struct drm_crtc *crtc,
 	if (ret)
 		return ret;
 
-	BEGIN_RING(evo, 0, NV50_EVO_CRTC(nv_crtc->index, FB_OFFSET), 5);
+	BEGIN_NV04(evo, 0, NV50_EVO_CRTC(nv_crtc->index, FB_OFFSET), 5);
 	OUT_RING  (evo, nv_crtc->fb.offset >> 8);
 	OUT_RING  (evo, 0);
 	OUT_RING  (evo, (drm_fb->height << 16) | drm_fb->width);
 	OUT_RING  (evo, fb->r_pitch);
 	OUT_RING  (evo, fb->r_format);
 
-	BEGIN_RING(evo, 0, NV50_EVO_CRTC(nv_crtc->index, CLUT_MODE), 1);
+	BEGIN_NV04(evo, 0, NV50_EVO_CRTC(nv_crtc->index, CLUT_MODE), 1);
 	OUT_RING  (evo, fb->base.depth == 8 ?
 		   NV50_EVO_CRTC_CLUT_MODE_OFF : NV50_EVO_CRTC_CLUT_MODE_ON);
 
-	BEGIN_RING(evo, 0, NV50_EVO_CRTC(nv_crtc->index, FB_POS), 1);
+	BEGIN_NV04(evo, 0, NV50_EVO_CRTC(nv_crtc->index, FB_POS), 1);
 	OUT_RING  (evo, (y << 16) | x);
 
 	if (nv_crtc->lut.depth != fb->base.depth) {
@@ -672,23 +664,23 @@ nv50_crtc_mode_set(struct drm_crtc *crtc, struct drm_display_mode *umode,
 
 	ret = RING_SPACE(evo, 18);
 	if (ret == 0) {
-		BEGIN_RING(evo, 0, 0x0804 + head, 2);
+		BEGIN_NV04(evo, 0, 0x0804 + head, 2);
 		OUT_RING  (evo, 0x00800000 | mode->clock);
 		OUT_RING  (evo, (ilace == 2) ? 2 : 0);
-		BEGIN_RING(evo, 0, 0x0810 + head, 6);
+		BEGIN_NV04(evo, 0, 0x0810 + head, 6);
 		OUT_RING  (evo, 0x00000000); /* border colour */
 		OUT_RING  (evo, (vactive << 16) | hactive);
 		OUT_RING  (evo, ( vsynce << 16) | hsynce);
 		OUT_RING  (evo, (vblanke << 16) | hblanke);
 		OUT_RING  (evo, (vblanks << 16) | hblanks);
 		OUT_RING  (evo, (vblan2e << 16) | vblan2s);
-		BEGIN_RING(evo, 0, 0x082c + head, 1);
+		BEGIN_NV04(evo, 0, 0x082c + head, 1);
 		OUT_RING  (evo, 0x00000000);
-		BEGIN_RING(evo, 0, 0x0900 + head, 1);
+		BEGIN_NV04(evo, 0, 0x0900 + head, 1);
 		OUT_RING  (evo, 0x00000311); /* makes sync channel work */
-		BEGIN_RING(evo, 0, 0x08c8 + head, 1);
+		BEGIN_NV04(evo, 0, 0x08c8 + head, 1);
 		OUT_RING  (evo, (umode->vdisplay << 16) | umode->hdisplay);
-		BEGIN_RING(evo, 0, 0x08d4 + head, 1);
+		BEGIN_NV04(evo, 0, 0x08d4 + head, 1);
 		OUT_RING  (evo, 0x00000000); /* screen position */
 	}
 
@@ -755,21 +747,25 @@ nv50_crtc_create(struct drm_device *dev, int index)
 	if (!nv_crtc)
 		return -ENOMEM;
 
+	nv_crtc->index = index;
+	nv_crtc->set_dither = nv50_crtc_set_dither;
+	nv_crtc->set_scale = nv50_crtc_set_scale;
+	nv_crtc->set_color_vibrance = nv50_crtc_set_color_vibrance;
 	nv_crtc->color_vibrance = 50;
 	nv_crtc->vibrant_hue = 0;
-
-	/* Default CLUT parameters, will be activated on the hw upon
-	 * first mode set.
-	 */
+	nv_crtc->lut.depth = 0;
 	for (i = 0; i < 256; i++) {
 		nv_crtc->lut.r[i] = i << 8;
 		nv_crtc->lut.g[i] = i << 8;
 		nv_crtc->lut.b[i] = i << 8;
 	}
-	nv_crtc->lut.depth = 0;
+
+	drm_crtc_init(dev, &nv_crtc->base, &nv50_crtc_funcs);
+	drm_crtc_helper_add(&nv_crtc->base, &nv50_crtc_helper_funcs);
+	drm_mode_crtc_set_gamma_size(&nv_crtc->base, 256);
 
 	ret = nouveau_bo_new(dev, 4096, 0x100, TTM_PL_FLAG_VRAM,
-			     0, 0x0000, &nv_crtc->lut.nvbo);
+			     0, 0x0000, NULL, &nv_crtc->lut.nvbo);
 	if (!ret) {
 		ret = nouveau_bo_pin(nv_crtc->lut.nvbo, TTM_PL_FLAG_VRAM);
 		if (!ret)
@@ -778,24 +774,12 @@ nv50_crtc_create(struct drm_device *dev, int index)
 			nouveau_bo_ref(NULL, &nv_crtc->lut.nvbo);
 	}
 
-	if (ret) {
-		kfree(nv_crtc);
-		return ret;
-	}
-
-	nv_crtc->index = index;
+	if (ret)
+		goto out;
 
-	/* set function pointers */
-	nv_crtc->set_dither = nv50_crtc_set_dither;
-	nv_crtc->set_scale = nv50_crtc_set_scale;
-	nv_crtc->set_color_vibrance = nv50_crtc_set_color_vibrance;
-
-	drm_crtc_init(dev, &nv_crtc->base, &nv50_crtc_funcs);
-	drm_crtc_helper_add(&nv_crtc->base, &nv50_crtc_helper_funcs);
-	drm_mode_crtc_set_gamma_size(&nv_crtc->base, 256);
 
 	ret = nouveau_bo_new(dev, 64*64*4, 0x100, TTM_PL_FLAG_VRAM,
-			     0, 0x0000, &nv_crtc->cursor.nvbo);
+			     0, 0x0000, NULL, &nv_crtc->cursor.nvbo);
 	if (!ret) {
 		ret = nouveau_bo_pin(nv_crtc->cursor.nvbo, TTM_PL_FLAG_VRAM);
 		if (!ret)
@@ -804,6 +788,12 @@ nv50_crtc_create(struct drm_device *dev, int index)
 			nouveau_bo_ref(NULL, &nv_crtc->cursor.nvbo);
 	}
 
+	if (ret)
+		goto out;
+
 	nv50_cursor_init(nv_crtc);
-	return 0;
+out:
+	if (ret)
+		nv50_crtc_destroy(&nv_crtc->base);
+	return ret;
 }
diff --git a/drivers/gpu/drm/nouveau/nv50_cursor.c b/drivers/gpu/drm/nouveau/nv50_cursor.c
index adfc9b607a5..af4ec7bf367 100644
--- a/drivers/gpu/drm/nouveau/nv50_cursor.c
+++ b/drivers/gpu/drm/nouveau/nv50_cursor.c
@@ -53,15 +53,15 @@ nv50_cursor_show(struct nouveau_crtc *nv_crtc, bool update)
 	}
 
 	if (dev_priv->chipset != 0x50) {
-		BEGIN_RING(evo, 0, NV84_EVO_CRTC(nv_crtc->index, CURSOR_DMA), 1);
+		BEGIN_NV04(evo, 0, NV84_EVO_CRTC(nv_crtc->index, CURSOR_DMA), 1);
 		OUT_RING(evo, NvEvoVRAM);
 	}
-	BEGIN_RING(evo, 0, NV50_EVO_CRTC(nv_crtc->index, CURSOR_CTRL), 2);
+	BEGIN_NV04(evo, 0, NV50_EVO_CRTC(nv_crtc->index, CURSOR_CTRL), 2);
 	OUT_RING(evo, NV50_EVO_CRTC_CURSOR_CTRL_SHOW);
 	OUT_RING(evo, nv_crtc->cursor.offset >> 8);
 
 	if (update) {
-		BEGIN_RING(evo, 0, NV50_EVO_UPDATE, 1);
+		BEGIN_NV04(evo, 0, NV50_EVO_UPDATE, 1);
 		OUT_RING(evo, 0);
 		FIRE_RING(evo);
 		nv_crtc->cursor.visible = true;
@@ -86,16 +86,16 @@ nv50_cursor_hide(struct nouveau_crtc *nv_crtc, bool update)
 		NV_ERROR(dev, "no space while hiding cursor\n");
 		return;
 	}
-	BEGIN_RING(evo, 0, NV50_EVO_CRTC(nv_crtc->index, CURSOR_CTRL), 2);
+	BEGIN_NV04(evo, 0, NV50_EVO_CRTC(nv_crtc->index, CURSOR_CTRL), 2);
 	OUT_RING(evo, NV50_EVO_CRTC_CURSOR_CTRL_HIDE);
 	OUT_RING(evo, 0);
 	if (dev_priv->chipset != 0x50) {
-		BEGIN_RING(evo, 0, NV84_EVO_CRTC(nv_crtc->index, CURSOR_DMA), 1);
+		BEGIN_NV04(evo, 0, NV84_EVO_CRTC(nv_crtc->index, CURSOR_DMA), 1);
 		OUT_RING(evo, NV84_EVO_CRTC_CURSOR_DMA_HANDLE_NONE);
 	}
 
 	if (update) {
-		BEGIN_RING(evo, 0, NV50_EVO_UPDATE, 1);
+		BEGIN_NV04(evo, 0, NV50_EVO_UPDATE, 1);
 		OUT_RING(evo, 0);
 		FIRE_RING(evo);
 		nv_crtc->cursor.visible = false;
diff --git a/drivers/gpu/drm/nouveau/nv50_dac.c b/drivers/gpu/drm/nouveau/nv50_dac.c
index 55c56330be6..eb216a446b8 100644
--- a/drivers/gpu/drm/nouveau/nv50_dac.c
+++ b/drivers/gpu/drm/nouveau/nv50_dac.c
@@ -55,9 +55,9 @@ nv50_dac_disconnect(struct drm_encoder *encoder)
 		NV_ERROR(dev, "no space while disconnecting DAC\n");
 		return;
 	}
-	BEGIN_RING(evo, 0, NV50_EVO_DAC(nv_encoder->or, MODE_CTRL), 1);
+	BEGIN_NV04(evo, 0, NV50_EVO_DAC(nv_encoder->or, MODE_CTRL), 1);
 	OUT_RING  (evo, 0);
-	BEGIN_RING(evo, 0, NV50_EVO_UPDATE, 1);
+	BEGIN_NV04(evo, 0, NV50_EVO_UPDATE, 1);
 	OUT_RING  (evo, 0);
 
 	nv_encoder->crtc = NULL;
@@ -240,7 +240,7 @@ nv50_dac_mode_set(struct drm_encoder *encoder, struct drm_display_mode *mode,
 		NV_ERROR(dev, "no space while connecting DAC\n");
 		return;
 	}
-	BEGIN_RING(evo, 0, NV50_EVO_DAC(nv_encoder->or, MODE_CTRL), 2);
+	BEGIN_NV04(evo, 0, NV50_EVO_DAC(nv_encoder->or, MODE_CTRL), 2);
 	OUT_RING(evo, mode_ctl);
 	OUT_RING(evo, mode_ctl2);
 
diff --git a/drivers/gpu/drm/nouveau/nv50_display.c b/drivers/gpu/drm/nouveau/nv50_display.c
index 8b78b9cfa38..5c41612723b 100644
--- a/drivers/gpu/drm/nouveau/nv50_display.c
+++ b/drivers/gpu/drm/nouveau/nv50_display.c
@@ -32,6 +32,7 @@
 #include "nouveau_fb.h"
 #include "nouveau_fbcon.h"
 #include "nouveau_ramht.h"
+#include "nouveau_software.h"
 #include "drm_crtc_helper.h"
 
 static void nv50_display_isr(struct drm_device *);
@@ -140,11 +141,11 @@ nv50_display_sync(struct drm_device *dev)
 
 	ret = RING_SPACE(evo, 6);
 	if (ret == 0) {
-		BEGIN_RING(evo, 0, 0x0084, 1);
+		BEGIN_NV04(evo, 0, 0x0084, 1);
 		OUT_RING  (evo, 0x80000000);
-		BEGIN_RING(evo, 0, 0x0080, 1);
+		BEGIN_NV04(evo, 0, 0x0080, 1);
 		OUT_RING  (evo, 0);
-		BEGIN_RING(evo, 0, 0x0084, 1);
+		BEGIN_NV04(evo, 0, 0x0084, 1);
 		OUT_RING  (evo, 0x00000000);
 
 		nv_wo32(disp->ntfy, 0x000, 0x00000000);
@@ -267,7 +268,7 @@ nv50_display_init(struct drm_device *dev)
 	ret = RING_SPACE(evo, 3);
 	if (ret)
 		return ret;
-	BEGIN_RING(evo, 0, NV50_EVO_UNK84, 2);
+	BEGIN_NV04(evo, 0, NV50_EVO_UNK84, 2);
 	OUT_RING  (evo, NV50_EVO_UNK84_NOTIFY_DISABLED);
 	OUT_RING  (evo, NvEvoSync);
 
@@ -292,7 +293,7 @@ nv50_display_fini(struct drm_device *dev)
 
 	ret = RING_SPACE(evo, 2);
 	if (ret == 0) {
-		BEGIN_RING(evo, 0, NV50_EVO_UPDATE, 1);
+		BEGIN_NV04(evo, 0, NV50_EVO_UPDATE, 1);
 		OUT_RING(evo, 0);
 	}
 	FIRE_RING(evo);
@@ -358,8 +359,11 @@ nv50_display_create(struct drm_device *dev)
 	dev_priv->engine.display.priv = priv;
 
 	/* Create CRTC objects */
-	for (i = 0; i < 2; i++)
-		nv50_crtc_create(dev, i);
+	for (i = 0; i < 2; i++) {
+		ret = nv50_crtc_create(dev, i);
+		if (ret)
+			return ret;
+	}
 
 	/* We setup the encoders from the BIOS table */
 	for (i = 0 ; i < dcb->entries; i++) {
@@ -438,13 +442,13 @@ nv50_display_flip_stop(struct drm_crtc *crtc)
 		return;
 	}
 
-	BEGIN_RING(evo, 0, 0x0084, 1);
+	BEGIN_NV04(evo, 0, 0x0084, 1);
 	OUT_RING  (evo, 0x00000000);
-	BEGIN_RING(evo, 0, 0x0094, 1);
+	BEGIN_NV04(evo, 0, 0x0094, 1);
 	OUT_RING  (evo, 0x00000000);
-	BEGIN_RING(evo, 0, 0x00c0, 1);
+	BEGIN_NV04(evo, 0, 0x00c0, 1);
 	OUT_RING  (evo, 0x00000000);
-	BEGIN_RING(evo, 0, 0x0080, 1);
+	BEGIN_NV04(evo, 0, 0x0080, 1);
 	OUT_RING  (evo, 0x00000000);
 	FIRE_RING (evo);
 }
@@ -474,28 +478,28 @@ nv50_display_flip_next(struct drm_crtc *crtc, struct drm_framebuffer *fb,
 		}
 
 		if (dev_priv->chipset < 0xc0) {
-			BEGIN_RING(chan, 0, 0x0060, 2);
+			BEGIN_NV04(chan, 0, 0x0060, 2);
 			OUT_RING  (chan, NvEvoSema0 + nv_crtc->index);
 			OUT_RING  (chan, dispc->sem.offset);
-			BEGIN_RING(chan, 0, 0x006c, 1);
+			BEGIN_NV04(chan, 0, 0x006c, 1);
 			OUT_RING  (chan, 0xf00d0000 | dispc->sem.value);
-			BEGIN_RING(chan, 0, 0x0064, 2);
+			BEGIN_NV04(chan, 0, 0x0064, 2);
 			OUT_RING  (chan, dispc->sem.offset ^ 0x10);
 			OUT_RING  (chan, 0x74b1e000);
-			BEGIN_RING(chan, 0, 0x0060, 1);
+			BEGIN_NV04(chan, 0, 0x0060, 1);
 			if (dev_priv->chipset < 0x84)
 				OUT_RING  (chan, NvSema);
 			else
 				OUT_RING  (chan, chan->vram_handle);
 		} else {
-			u64 offset = chan->dispc_vma[nv_crtc->index].offset;
+			u64 offset = nvc0_software_crtc(chan, nv_crtc->index);
 			offset += dispc->sem.offset;
-			BEGIN_NVC0(chan, 2, 0, 0x0010, 4);
+			BEGIN_NVC0(chan, 0, 0x0010, 4);
 			OUT_RING  (chan, upper_32_bits(offset));
 			OUT_RING  (chan, lower_32_bits(offset));
 			OUT_RING  (chan, 0xf00d0000 | dispc->sem.value);
 			OUT_RING  (chan, 0x1002);
-			BEGIN_NVC0(chan, 2, 0, 0x0010, 4);
+			BEGIN_NVC0(chan, 0, 0x0010, 4);
 			OUT_RING  (chan, upper_32_bits(offset));
 			OUT_RING  (chan, lower_32_bits(offset ^ 0x10));
 			OUT_RING  (chan, 0x74b1e000);
@@ -508,40 +512,40 @@ nv50_display_flip_next(struct drm_crtc *crtc, struct drm_framebuffer *fb,
 	}
 
 	/* queue the flip on the crtc's "display sync" channel */
-	BEGIN_RING(evo, 0, 0x0100, 1);
+	BEGIN_NV04(evo, 0, 0x0100, 1);
 	OUT_RING  (evo, 0xfffe0000);
 	if (chan) {
-		BEGIN_RING(evo, 0, 0x0084, 1);
+		BEGIN_NV04(evo, 0, 0x0084, 1);
 		OUT_RING  (evo, 0x00000100);
 	} else {
-		BEGIN_RING(evo, 0, 0x0084, 1);
+		BEGIN_NV04(evo, 0, 0x0084, 1);
 		OUT_RING  (evo, 0x00000010);
 		/* allows gamma somehow, PDISP will bitch at you if
 		 * you don't wait for vblank before changing this..
 		 */
-		BEGIN_RING(evo, 0, 0x00e0, 1);
+		BEGIN_NV04(evo, 0, 0x00e0, 1);
 		OUT_RING  (evo, 0x40000000);
 	}
-	BEGIN_RING(evo, 0, 0x0088, 4);
+	BEGIN_NV04(evo, 0, 0x0088, 4);
 	OUT_RING  (evo, dispc->sem.offset);
 	OUT_RING  (evo, 0xf00d0000 | dispc->sem.value);
 	OUT_RING  (evo, 0x74b1e000);
 	OUT_RING  (evo, NvEvoSync);
-	BEGIN_RING(evo, 0, 0x00a0, 2);
+	BEGIN_NV04(evo, 0, 0x00a0, 2);
 	OUT_RING  (evo, 0x00000000);
 	OUT_RING  (evo, 0x00000000);
-	BEGIN_RING(evo, 0, 0x00c0, 1);
+	BEGIN_NV04(evo, 0, 0x00c0, 1);
 	OUT_RING  (evo, nv_fb->r_dma);
-	BEGIN_RING(evo, 0, 0x0110, 2);
+	BEGIN_NV04(evo, 0, 0x0110, 2);
 	OUT_RING  (evo, 0x00000000);
 	OUT_RING  (evo, 0x00000000);
-	BEGIN_RING(evo, 0, 0x0800, 5);
+	BEGIN_NV04(evo, 0, 0x0800, 5);
 	OUT_RING  (evo, nv_fb->nvbo->bo.offset >> 8);
 	OUT_RING  (evo, 0);
 	OUT_RING  (evo, (fb->height << 16) | fb->width);
 	OUT_RING  (evo, nv_fb->r_pitch);
 	OUT_RING  (evo, nv_fb->r_format);
-	BEGIN_RING(evo, 0, 0x0080, 1);
+	BEGIN_NV04(evo, 0, 0x0080, 1);
 	OUT_RING  (evo, 0x00000000);
 	FIRE_RING (evo);
 
@@ -642,20 +646,7 @@ nv50_display_script_select(struct drm_device *dev, struct dcb_entry *dcb,
 static void
 nv50_display_vblank_crtc_handler(struct drm_device *dev, int crtc)
 {
-	struct drm_nouveau_private *dev_priv = dev->dev_private;
-	struct nouveau_channel *chan, *tmp;
-
-	list_for_each_entry_safe(chan, tmp, &dev_priv->vbl_waiting,
-				 nvsw.vbl_wait) {
-		if (chan->nvsw.vblsem_head != crtc)
-			continue;
-
-		nouveau_bo_wr32(chan->notifier_bo, chan->nvsw.vblsem_offset,
-						chan->nvsw.vblsem_rval);
-		list_del(&chan->nvsw.vbl_wait);
-		drm_vblank_put(dev, crtc);
-	}
-
+	nouveau_software_vblank(dev, crtc);
 	drm_handle_vblank(dev, crtc);
 }
 
diff --git a/drivers/gpu/drm/nouveau/nv50_display.h b/drivers/gpu/drm/nouveau/nv50_display.h
index 5d3dd14d283..e9db9b97f04 100644
--- a/drivers/gpu/drm/nouveau/nv50_display.h
+++ b/drivers/gpu/drm/nouveau/nv50_display.h
@@ -33,6 +33,7 @@
 #include "nouveau_dma.h"
 #include "nouveau_reg.h"
 #include "nouveau_crtc.h"
+#include "nouveau_software.h"
 #include "nv50_evo.h"
 
 struct nv50_display_crtc {
diff --git a/drivers/gpu/drm/nouveau/nv50_evo.c b/drivers/gpu/drm/nouveau/nv50_evo.c
index 9b962e989d7..ddcd5559582 100644
--- a/drivers/gpu/drm/nouveau/nv50_evo.c
+++ b/drivers/gpu/drm/nouveau/nv50_evo.c
@@ -117,7 +117,7 @@ nv50_evo_channel_new(struct drm_device *dev, int chid,
 	evo->user_get = 4;
 	evo->user_put = 0;
 
-	ret = nouveau_bo_new(dev, 4096, 0, TTM_PL_FLAG_VRAM, 0, 0,
+	ret = nouveau_bo_new(dev, 4096, 0, TTM_PL_FLAG_VRAM, 0, 0, NULL,
 			     &evo->pushbuf_bo);
 	if (ret == 0)
 		ret = nouveau_bo_pin(evo->pushbuf_bo, TTM_PL_FLAG_VRAM);
@@ -333,7 +333,7 @@ nv50_evo_create(struct drm_device *dev)
 			goto err;
 
 		ret = nouveau_bo_new(dev, 4096, 0x1000, TTM_PL_FLAG_VRAM,
-				     0, 0x0000, &dispc->sem.bo);
+				     0, 0x0000, NULL, &dispc->sem.bo);
 		if (!ret) {
 			ret = nouveau_bo_pin(dispc->sem.bo, TTM_PL_FLAG_VRAM);
 			if (!ret)
diff --git a/drivers/gpu/drm/nouveau/nv50_fb.c b/drivers/gpu/drm/nouveau/nv50_fb.c
index bdd2afe2920..f1e4b9e07d1 100644
--- a/drivers/gpu/drm/nouveau/nv50_fb.c
+++ b/drivers/gpu/drm/nouveau/nv50_fb.c
@@ -2,6 +2,7 @@
 #include "drm.h"
 #include "nouveau_drv.h"
 #include "nouveau_drm.h"
+#include "nouveau_fifo.h"
 
 struct nv50_fb_priv {
 	struct page *r100c08_page;
@@ -212,6 +213,7 @@ static struct nouveau_enum vm_fault[] = {
 void
 nv50_fb_vm_trap(struct drm_device *dev, int display)
 {
+	struct nouveau_fifo_priv *pfifo = nv_engine(dev, NVOBJ_ENGINE_FIFO);
 	struct drm_nouveau_private *dev_priv = dev->dev_private;
 	const struct nouveau_enum *en, *cl;
 	unsigned long flags;
@@ -236,7 +238,7 @@ nv50_fb_vm_trap(struct drm_device *dev, int display)
 	/* lookup channel id */
 	chinst = (trap[2] << 16) | trap[1];
 	spin_lock_irqsave(&dev_priv->channels.lock, flags);
-	for (ch = 0; ch < dev_priv->engine.fifo.channels; ch++) {
+	for (ch = 0; ch < pfifo->channels; ch++) {
 		struct nouveau_channel *chan = dev_priv->channels.ptr[ch];
 
 		if (!chan || !chan->ramin)
diff --git a/drivers/gpu/drm/nouveau/nv50_fbcon.c b/drivers/gpu/drm/nouveau/nv50_fbcon.c
index dc75a720652..e3c8b05dcae 100644
--- a/drivers/gpu/drm/nouveau/nv50_fbcon.c
+++ b/drivers/gpu/drm/nouveau/nv50_fbcon.c
@@ -43,22 +43,22 @@ nv50_fbcon_fillrect(struct fb_info *info, const struct fb_fillrect *rect)
 		return ret;
 
 	if (rect->rop != ROP_COPY) {
-		BEGIN_RING(chan, NvSub2D, 0x02ac, 1);
+		BEGIN_NV04(chan, NvSub2D, 0x02ac, 1);
 		OUT_RING(chan, 1);
 	}
-	BEGIN_RING(chan, NvSub2D, 0x0588, 1);
+	BEGIN_NV04(chan, NvSub2D, 0x0588, 1);
 	if (info->fix.visual == FB_VISUAL_TRUECOLOR ||
 	    info->fix.visual == FB_VISUAL_DIRECTCOLOR)
 		OUT_RING(chan, ((uint32_t *)info->pseudo_palette)[rect->color]);
 	else
 		OUT_RING(chan, rect->color);
-	BEGIN_RING(chan, NvSub2D, 0x0600, 4);
+	BEGIN_NV04(chan, NvSub2D, 0x0600, 4);
 	OUT_RING(chan, rect->dx);
 	OUT_RING(chan, rect->dy);
 	OUT_RING(chan, rect->dx + rect->width);
 	OUT_RING(chan, rect->dy + rect->height);
 	if (rect->rop != ROP_COPY) {
-		BEGIN_RING(chan, NvSub2D, 0x02ac, 1);
+		BEGIN_NV04(chan, NvSub2D, 0x02ac, 1);
 		OUT_RING(chan, 3);
 	}
 	FIRE_RING(chan);
@@ -78,14 +78,14 @@ nv50_fbcon_copyarea(struct fb_info *info, const struct fb_copyarea *region)
 	if (ret)
 		return ret;
 
-	BEGIN_RING(chan, NvSub2D, 0x0110, 1);
+	BEGIN_NV04(chan, NvSub2D, 0x0110, 1);
 	OUT_RING(chan, 0);
-	BEGIN_RING(chan, NvSub2D, 0x08b0, 4);
+	BEGIN_NV04(chan, NvSub2D, 0x08b0, 4);
 	OUT_RING(chan, region->dx);
 	OUT_RING(chan, region->dy);
 	OUT_RING(chan, region->width);
 	OUT_RING(chan, region->height);
-	BEGIN_RING(chan, NvSub2D, 0x08d0, 4);
+	BEGIN_NV04(chan, NvSub2D, 0x08d0, 4);
 	OUT_RING(chan, 0);
 	OUT_RING(chan, region->sx);
 	OUT_RING(chan, 0);
@@ -116,7 +116,7 @@ nv50_fbcon_imageblit(struct fb_info *info, const struct fb_image *image)
 	width = ALIGN(image->width, 32);
 	dwords = (width * image->height) >> 5;
 
-	BEGIN_RING(chan, NvSub2D, 0x0814, 2);
+	BEGIN_NV04(chan, NvSub2D, 0x0814, 2);
 	if (info->fix.visual == FB_VISUAL_TRUECOLOR ||
 	    info->fix.visual == FB_VISUAL_DIRECTCOLOR) {
 		OUT_RING(chan, palette[image->bg_color] | mask);
@@ -125,10 +125,10 @@ nv50_fbcon_imageblit(struct fb_info *info, const struct fb_image *image)
 		OUT_RING(chan, image->bg_color);
 		OUT_RING(chan, image->fg_color);
 	}
-	BEGIN_RING(chan, NvSub2D, 0x0838, 2);
+	BEGIN_NV04(chan, NvSub2D, 0x0838, 2);
 	OUT_RING(chan, image->width);
 	OUT_RING(chan, image->height);
-	BEGIN_RING(chan, NvSub2D, 0x0850, 4);
+	BEGIN_NV04(chan, NvSub2D, 0x0850, 4);
 	OUT_RING(chan, 0);
 	OUT_RING(chan, image->dx);
 	OUT_RING(chan, 0);
@@ -143,7 +143,7 @@ nv50_fbcon_imageblit(struct fb_info *info, const struct fb_image *image)
 
 		dwords -= push;
 
-		BEGIN_RING(chan, NvSub2D, 0x40000860, push);
+		BEGIN_NI04(chan, NvSub2D, 0x0860, push);
 		OUT_RINGp(chan, data, push);
 		data += push;
 	}
@@ -199,60 +199,59 @@ nv50_fbcon_accel_init(struct fb_info *info)
 		return ret;
 	}
 
-	BEGIN_RING(chan, NvSub2D, 0x0000, 1);
+	BEGIN_NV04(chan, NvSub2D, 0x0000, 1);
 	OUT_RING(chan, Nv2D);
-	BEGIN_RING(chan, NvSub2D, 0x0180, 4);
-	OUT_RING(chan, NvNotify0);
+	BEGIN_NV04(chan, NvSub2D, 0x0184, 3);
 	OUT_RING(chan, chan->vram_handle);
 	OUT_RING(chan, chan->vram_handle);
 	OUT_RING(chan, chan->vram_handle);
-	BEGIN_RING(chan, NvSub2D, 0x0290, 1);
+	BEGIN_NV04(chan, NvSub2D, 0x0290, 1);
 	OUT_RING(chan, 0);
-	BEGIN_RING(chan, NvSub2D, 0x0888, 1);
+	BEGIN_NV04(chan, NvSub2D, 0x0888, 1);
 	OUT_RING(chan, 1);
-	BEGIN_RING(chan, NvSub2D, 0x02ac, 1);
+	BEGIN_NV04(chan, NvSub2D, 0x02ac, 1);
 	OUT_RING(chan, 3);
-	BEGIN_RING(chan, NvSub2D, 0x02a0, 1);
+	BEGIN_NV04(chan, NvSub2D, 0x02a0, 1);
 	OUT_RING(chan, 0x55);
-	BEGIN_RING(chan, NvSub2D, 0x08c0, 4);
+	BEGIN_NV04(chan, NvSub2D, 0x08c0, 4);
 	OUT_RING(chan, 0);
 	OUT_RING(chan, 1);
 	OUT_RING(chan, 0);
 	OUT_RING(chan, 1);
-	BEGIN_RING(chan, NvSub2D, 0x0580, 2);
+	BEGIN_NV04(chan, NvSub2D, 0x0580, 2);
 	OUT_RING(chan, 4);
 	OUT_RING(chan, format);
-	BEGIN_RING(chan, NvSub2D, 0x02e8, 2);
+	BEGIN_NV04(chan, NvSub2D, 0x02e8, 2);
 	OUT_RING(chan, 2);
 	OUT_RING(chan, 1);
-	BEGIN_RING(chan, NvSub2D, 0x0804, 1);
+	BEGIN_NV04(chan, NvSub2D, 0x0804, 1);
 	OUT_RING(chan, format);
-	BEGIN_RING(chan, NvSub2D, 0x0800, 1);
+	BEGIN_NV04(chan, NvSub2D, 0x0800, 1);
 	OUT_RING(chan, 1);
-	BEGIN_RING(chan, NvSub2D, 0x0808, 3);
+	BEGIN_NV04(chan, NvSub2D, 0x0808, 3);
 	OUT_RING(chan, 0);
 	OUT_RING(chan, 0);
 	OUT_RING(chan, 1);
-	BEGIN_RING(chan, NvSub2D, 0x081c, 1);
+	BEGIN_NV04(chan, NvSub2D, 0x081c, 1);
 	OUT_RING(chan, 1);
-	BEGIN_RING(chan, NvSub2D, 0x0840, 4);
+	BEGIN_NV04(chan, NvSub2D, 0x0840, 4);
 	OUT_RING(chan, 0);
 	OUT_RING(chan, 1);
 	OUT_RING(chan, 0);
 	OUT_RING(chan, 1);
-	BEGIN_RING(chan, NvSub2D, 0x0200, 2);
+	BEGIN_NV04(chan, NvSub2D, 0x0200, 2);
 	OUT_RING(chan, format);
 	OUT_RING(chan, 1);
-	BEGIN_RING(chan, NvSub2D, 0x0214, 5);
+	BEGIN_NV04(chan, NvSub2D, 0x0214, 5);
 	OUT_RING(chan, info->fix.line_length);
 	OUT_RING(chan, info->var.xres_virtual);
 	OUT_RING(chan, info->var.yres_virtual);
 	OUT_RING(chan, upper_32_bits(fb->vma.offset));
 	OUT_RING(chan, lower_32_bits(fb->vma.offset));
-	BEGIN_RING(chan, NvSub2D, 0x0230, 2);
+	BEGIN_NV04(chan, NvSub2D, 0x0230, 2);
 	OUT_RING(chan, format);
 	OUT_RING(chan, 1);
-	BEGIN_RING(chan, NvSub2D, 0x0244, 5);
+	BEGIN_NV04(chan, NvSub2D, 0x0244, 5);
 	OUT_RING(chan, info->fix.line_length);
 	OUT_RING(chan, info->var.xres_virtual);
 	OUT_RING(chan, info->var.yres_virtual);
diff --git a/drivers/gpu/drm/nouveau/nv50_fifo.c b/drivers/gpu/drm/nouveau/nv50_fifo.c
index 3bc2a565c20..55383b85db0 100644
--- a/drivers/gpu/drm/nouveau/nv50_fifo.c
+++ b/drivers/gpu/drm/nouveau/nv50_fifo.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (C) 2007 Ben Skeggs.
+ * Copyright (C) 2012 Ben Skeggs.
  * All Rights Reserved.
  *
  * Permission is hereby granted, free of charge, to any person obtaining
@@ -27,480 +27,268 @@
 #include "drmP.h"
 #include "drm.h"
 #include "nouveau_drv.h"
+#include "nouveau_fifo.h"
 #include "nouveau_ramht.h"
 #include "nouveau_vm.h"
 
-static void
+struct nv50_fifo_priv {
+	struct nouveau_fifo_priv base;
+	struct nouveau_gpuobj *playlist[2];
+	int cur_playlist;
+};
+
+struct nv50_fifo_chan {
+	struct nouveau_fifo_chan base;
+};
+
+void
 nv50_fifo_playlist_update(struct drm_device *dev)
 {
+	struct nv50_fifo_priv *priv = nv_engine(dev, NVOBJ_ENGINE_FIFO);
 	struct drm_nouveau_private *dev_priv = dev->dev_private;
-	struct nouveau_fifo_engine *pfifo = &dev_priv->engine.fifo;
 	struct nouveau_gpuobj *cur;
-	int i, nr;
-
-	NV_DEBUG(dev, "\n");
+	int i, p;
 
-	cur = pfifo->playlist[pfifo->cur_playlist];
-	pfifo->cur_playlist = !pfifo->cur_playlist;
+	cur = priv->playlist[priv->cur_playlist];
+	priv->cur_playlist = !priv->cur_playlist;
 
-	/* We never schedule channel 0 or 127 */
-	for (i = 1, nr = 0; i < 127; i++) {
-		if (dev_priv->channels.ptr[i] &&
-		    dev_priv->channels.ptr[i]->ramfc) {
-			nv_wo32(cur, (nr * 4), i);
-			nr++;
-		}
+	for (i = 0, p = 0; i < priv->base.channels; i++) {
+		if (nv_rd32(dev, 0x002600 + (i * 4)) & 0x80000000)
+			nv_wo32(cur, p++ * 4, i);
 	}
-	dev_priv->engine.instmem.flush(dev);
-
-	nv_wr32(dev, 0x32f4, cur->vinst >> 12);
-	nv_wr32(dev, 0x32ec, nr);
-	nv_wr32(dev, 0x2500, 0x101);
-}
 
-static void
-nv50_fifo_channel_enable(struct drm_device *dev, int channel)
-{
-	struct drm_nouveau_private *dev_priv = dev->dev_private;
-	struct nouveau_channel *chan = dev_priv->channels.ptr[channel];
-	uint32_t inst;
-
-	NV_DEBUG(dev, "ch%d\n", channel);
-
-	if (dev_priv->chipset == 0x50)
-		inst = chan->ramfc->vinst >> 12;
-	else
-		inst = chan->ramfc->vinst >> 8;
+	dev_priv->engine.instmem.flush(dev);
 
-	nv_wr32(dev, NV50_PFIFO_CTX_TABLE(channel), inst |
-		     NV50_PFIFO_CTX_TABLE_CHANNEL_ENABLED);
+	nv_wr32(dev, 0x0032f4, cur->vinst >> 12);
+	nv_wr32(dev, 0x0032ec, p);
+	nv_wr32(dev, 0x002500, 0x00000101);
 }
 
-static void
-nv50_fifo_channel_disable(struct drm_device *dev, int channel)
+static int
+nv50_fifo_context_new(struct nouveau_channel *chan, int engine)
 {
+	struct nv50_fifo_priv *priv = nv_engine(chan->dev, engine);
+	struct nv50_fifo_chan *fctx;
+	struct drm_device *dev = chan->dev;
 	struct drm_nouveau_private *dev_priv = dev->dev_private;
-	uint32_t inst;
-
-	NV_DEBUG(dev, "ch%d\n", channel);
+	u64 ib_offset = chan->pushbuf_base + chan->dma.ib_base * 4;
+	u64 instance = chan->ramin->vinst >> 12;
+	unsigned long flags;
+	int ret = 0, i;
 
-	if (dev_priv->chipset == 0x50)
-		inst = NV50_PFIFO_CTX_TABLE_INSTANCE_MASK_G80;
-	else
-		inst = NV50_PFIFO_CTX_TABLE_INSTANCE_MASK_G84;
-	nv_wr32(dev, NV50_PFIFO_CTX_TABLE(channel), inst);
-}
+	fctx = chan->engctx[engine] = kzalloc(sizeof(*fctx), GFP_KERNEL);
+	if (!fctx)
+		return -ENOMEM;
+	atomic_inc(&chan->vm->engref[engine]);
 
-static void
-nv50_fifo_init_reset(struct drm_device *dev)
-{
-	uint32_t pmc_e = NV_PMC_ENABLE_PFIFO;
+	chan->user = ioremap(pci_resource_start(dev->pdev, 0) +
+			     NV50_USER(chan->id), PAGE_SIZE);
+	if (!chan->user) {
+		ret = -ENOMEM;
+		goto error;
+	}
 
-	NV_DEBUG(dev, "\n");
+	for (i = 0; i < 0x100; i += 4)
+		nv_wo32(chan->ramin, i, 0x00000000);
+	nv_wo32(chan->ramin, 0x3c, 0x403f6078);
+	nv_wo32(chan->ramin, 0x40, 0x00000000);
+	nv_wo32(chan->ramin, 0x44, 0x01003fff);
+	nv_wo32(chan->ramin, 0x48, chan->pushbuf->cinst >> 4);
+	nv_wo32(chan->ramin, 0x50, lower_32_bits(ib_offset));
+	nv_wo32(chan->ramin, 0x54, upper_32_bits(ib_offset) |
+				   drm_order(chan->dma.ib_max + 1) << 16);
+	nv_wo32(chan->ramin, 0x60, 0x7fffffff);
+	nv_wo32(chan->ramin, 0x78, 0x00000000);
+	nv_wo32(chan->ramin, 0x7c, 0x30000001);
+	nv_wo32(chan->ramin, 0x80, ((chan->ramht->bits - 9) << 27) |
+				   (4 << 24) /* SEARCH_FULL */ |
+				   (chan->ramht->gpuobj->cinst >> 4));
 
-	nv_wr32(dev, NV03_PMC_ENABLE, nv_rd32(dev, NV03_PMC_ENABLE) & ~pmc_e);
-	nv_wr32(dev, NV03_PMC_ENABLE, nv_rd32(dev, NV03_PMC_ENABLE) |  pmc_e);
-}
+	dev_priv->engine.instmem.flush(dev);
 
-static void
-nv50_fifo_init_intr(struct drm_device *dev)
-{
-	NV_DEBUG(dev, "\n");
+	spin_lock_irqsave(&dev_priv->context_switch_lock, flags);
+	nv_wr32(dev, 0x002600 + (chan->id * 4), 0x80000000 | instance);
+	nv50_fifo_playlist_update(dev);
+	spin_unlock_irqrestore(&dev_priv->context_switch_lock, flags);
 
-	nouveau_irq_register(dev, 8, nv04_fifo_isr);
-	nv_wr32(dev, NV03_PFIFO_INTR_0, 0xFFFFFFFF);
-	nv_wr32(dev, NV03_PFIFO_INTR_EN_0, 0xFFFFFFFF);
+error:
+	if (ret)
+		priv->base.base.context_del(chan, engine);
+	return ret;
 }
 
-static void
-nv50_fifo_init_context_table(struct drm_device *dev)
+static bool
+nv50_fifo_kickoff(struct nouveau_channel *chan)
 {
-	struct drm_nouveau_private *dev_priv = dev->dev_private;
-	int i;
-
-	NV_DEBUG(dev, "\n");
-
-	for (i = 0; i < NV50_PFIFO_CTX_TABLE__SIZE; i++) {
-		if (dev_priv->channels.ptr[i])
-			nv50_fifo_channel_enable(dev, i);
-		else
-			nv50_fifo_channel_disable(dev, i);
+	struct drm_device *dev = chan->dev;
+	bool done = true;
+	u32 me;
+
+	/* HW bug workaround:
+	 *
+	 * PFIFO will hang forever if the connected engines don't report
+	 * that they've processed the context switch request.
+	 *
+	 * In order for the kickoff to work, we need to ensure all the
+	 * connected engines are in a state where they can answer.
+	 *
+	 * Newer chipsets don't seem to suffer from this issue, and well,
+	 * there's also a "ignore these engines" bitmask reg we can use
+	 * if we hit the issue there..
+	 */
+
+	/* PME: make sure engine is enabled */
+	me = nv_mask(dev, 0x00b860, 0x00000001, 0x00000001);
+
+	/* do the kickoff... */
+	nv_wr32(dev, 0x0032fc, chan->ramin->vinst >> 12);
+	if (!nv_wait_ne(dev, 0x0032fc, 0xffffffff, 0xffffffff)) {
+		NV_INFO(dev, "PFIFO: channel %d unload timeout\n", chan->id);
+		done = false;
 	}
 
-	nv50_fifo_playlist_update(dev);
+	/* restore any engine states we changed, and exit */
+	nv_wr32(dev, 0x00b860, me);
+	return done;
 }
 
 static void
-nv50_fifo_init_regs__nv(struct drm_device *dev)
-{
-	NV_DEBUG(dev, "\n");
-
-	nv_wr32(dev, 0x250c, 0x6f3cfc34);
-}
-
-static void
-nv50_fifo_init_regs(struct drm_device *dev)
-{
-	NV_DEBUG(dev, "\n");
-
-	nv_wr32(dev, 0x2500, 0);
-	nv_wr32(dev, 0x3250, 0);
-	nv_wr32(dev, 0x3220, 0);
-	nv_wr32(dev, 0x3204, 0);
-	nv_wr32(dev, 0x3210, 0);
-	nv_wr32(dev, 0x3270, 0);
-	nv_wr32(dev, 0x2044, 0x01003fff);
-
-	/* Enable dummy channels setup by nv50_instmem.c */
-	nv50_fifo_channel_enable(dev, 0);
-	nv50_fifo_channel_enable(dev, 127);
-}
-
-int
-nv50_fifo_init(struct drm_device *dev)
+nv50_fifo_context_del(struct nouveau_channel *chan, int engine)
 {
+	struct nv50_fifo_chan *fctx = chan->engctx[engine];
+	struct drm_device *dev = chan->dev;
 	struct drm_nouveau_private *dev_priv = dev->dev_private;
-	struct nouveau_fifo_engine *pfifo = &dev_priv->engine.fifo;
-	int ret;
+	unsigned long flags;
 
-	NV_DEBUG(dev, "\n");
+	/* remove channel from playlist, will context switch if active */
+	spin_lock_irqsave(&dev_priv->context_switch_lock, flags);
+	nv_mask(dev, 0x002600 + (chan->id * 4), 0x80000000, 0x00000000);
+	nv50_fifo_playlist_update(dev);
 
-	if (pfifo->playlist[0]) {
-		pfifo->cur_playlist = !pfifo->cur_playlist;
-		goto just_reset;
-	}
+	/* tell any engines on this channel to unload their contexts */
+	nv50_fifo_kickoff(chan);
 
-	ret = nouveau_gpuobj_new(dev, NULL, 128*4, 0x1000,
-				 NVOBJ_FLAG_ZERO_ALLOC,
-				 &pfifo->playlist[0]);
-	if (ret) {
-		NV_ERROR(dev, "error creating playlist 0: %d\n", ret);
-		return ret;
-	}
+	nv_wr32(dev, 0x002600 + (chan->id * 4), 0x00000000);
+	spin_unlock_irqrestore(&dev_priv->context_switch_lock, flags);
 
-	ret = nouveau_gpuobj_new(dev, NULL, 128*4, 0x1000,
-				 NVOBJ_FLAG_ZERO_ALLOC,
-				 &pfifo->playlist[1]);
-	if (ret) {
-		nouveau_gpuobj_ref(NULL, &pfifo->playlist[0]);
-		NV_ERROR(dev, "error creating playlist 1: %d\n", ret);
-		return ret;
+	/* clean up */
+	if (chan->user) {
+		iounmap(chan->user);
+		chan->user = NULL;
 	}
 
-just_reset:
-	nv50_fifo_init_reset(dev);
-	nv50_fifo_init_intr(dev);
-	nv50_fifo_init_context_table(dev);
-	nv50_fifo_init_regs__nv(dev);
-	nv50_fifo_init_regs(dev);
-	dev_priv->engine.fifo.enable(dev);
-	dev_priv->engine.fifo.reassign(dev, true);
-
-	return 0;
+	atomic_dec(&chan->vm->engref[engine]);
+	chan->engctx[engine] = NULL;
+	kfree(fctx);
 }
 
-void
-nv50_fifo_takedown(struct drm_device *dev)
+static int
+nv50_fifo_init(struct drm_device *dev, int engine)
 {
 	struct drm_nouveau_private *dev_priv = dev->dev_private;
-	struct nouveau_fifo_engine *pfifo = &dev_priv->engine.fifo;
+	u32 instance;
+	int i;
 
-	NV_DEBUG(dev, "\n");
+	nv_mask(dev, 0x000200, 0x00000100, 0x00000000);
+	nv_mask(dev, 0x000200, 0x00000100, 0x00000100);
+	nv_wr32(dev, 0x00250c, 0x6f3cfc34);
+	nv_wr32(dev, 0x002044, 0x01003fff);
 
-	if (!pfifo->playlist[0])
-		return;
+	nv_wr32(dev, 0x002100, 0xffffffff);
+	nv_wr32(dev, 0x002140, 0xffffffff);
 
-	nv_wr32(dev, 0x2140, 0x00000000);
-	nouveau_irq_unregister(dev, 8);
+	for (i = 0; i < 128; i++) {
+		struct nouveau_channel *chan = dev_priv->channels.ptr[i];
+		if (chan && chan->engctx[engine])
+			instance = 0x80000000 | chan->ramin->vinst >> 12;
+		else
+			instance = 0x00000000;
+		nv_wr32(dev, 0x002600 + (i * 4), instance);
+	}
 
-	nouveau_gpuobj_ref(NULL, &pfifo->playlist[0]);
-	nouveau_gpuobj_ref(NULL, &pfifo->playlist[1]);
-}
+	nv50_fifo_playlist_update(dev);
 
-int
-nv50_fifo_channel_id(struct drm_device *dev)
-{
-	return nv_rd32(dev, NV03_PFIFO_CACHE1_PUSH1) &
-			NV50_PFIFO_CACHE1_PUSH1_CHID_MASK;
+	nv_wr32(dev, 0x003200, 1);
+	nv_wr32(dev, 0x003250, 1);
+	nv_wr32(dev, 0x002500, 1);
+	return 0;
 }
 
-int
-nv50_fifo_create_context(struct nouveau_channel *chan)
+static int
+nv50_fifo_fini(struct drm_device *dev, int engine, bool suspend)
 {
-	struct drm_device *dev = chan->dev;
 	struct drm_nouveau_private *dev_priv = dev->dev_private;
-	struct nouveau_gpuobj *ramfc = NULL;
-        uint64_t ib_offset = chan->pushbuf_base + chan->dma.ib_base * 4;
-	unsigned long flags;
-	int ret;
-
-	NV_DEBUG(dev, "ch%d\n", chan->id);
-
-	if (dev_priv->chipset == 0x50) {
-		ret = nouveau_gpuobj_new_fake(dev, chan->ramin->pinst,
-					      chan->ramin->vinst, 0x100,
-					      NVOBJ_FLAG_ZERO_ALLOC |
-					      NVOBJ_FLAG_ZERO_FREE,
-					      &chan->ramfc);
-		if (ret)
-			return ret;
-
-		ret = nouveau_gpuobj_new_fake(dev, chan->ramin->pinst + 0x0400,
-					      chan->ramin->vinst + 0x0400,
-					      4096, 0, &chan->cache);
-		if (ret)
-			return ret;
-	} else {
-		ret = nouveau_gpuobj_new(dev, chan, 0x100, 256,
-					 NVOBJ_FLAG_ZERO_ALLOC |
-					 NVOBJ_FLAG_ZERO_FREE, &chan->ramfc);
-		if (ret)
-			return ret;
-
-		ret = nouveau_gpuobj_new(dev, chan, 4096, 1024,
-					 0, &chan->cache);
-		if (ret)
-			return ret;
-	}
-	ramfc = chan->ramfc;
+	struct nv50_fifo_priv *priv = nv_engine(dev, engine);
+	int i;
 
-	chan->user = ioremap(pci_resource_start(dev->pdev, 0) +
-			     NV50_USER(chan->id), PAGE_SIZE);
-	if (!chan->user)
-		return -ENOMEM;
+	/* set playlist length to zero, fifo will unload context */
+	nv_wr32(dev, 0x0032ec, 0);
 
-	spin_lock_irqsave(&dev_priv->context_switch_lock, flags);
-
-	nv_wo32(ramfc, 0x48, chan->pushbuf->cinst >> 4);
-	nv_wo32(ramfc, 0x80, ((chan->ramht->bits - 9) << 27) |
-			     (4 << 24) /* SEARCH_FULL */ |
-			     (chan->ramht->gpuobj->cinst >> 4));
-	nv_wo32(ramfc, 0x44, 0x01003fff);
-	nv_wo32(ramfc, 0x60, 0x7fffffff);
-	nv_wo32(ramfc, 0x40, 0x00000000);
-	nv_wo32(ramfc, 0x7c, 0x30000001);
-	nv_wo32(ramfc, 0x78, 0x00000000);
-	nv_wo32(ramfc, 0x3c, 0x403f6078);
-	nv_wo32(ramfc, 0x50, lower_32_bits(ib_offset));
-	nv_wo32(ramfc, 0x54, upper_32_bits(ib_offset) |
-                drm_order(chan->dma.ib_max + 1) << 16);
-
-	if (dev_priv->chipset != 0x50) {
-		nv_wo32(chan->ramin, 0, chan->id);
-		nv_wo32(chan->ramin, 4, chan->ramfc->vinst >> 8);
-
-		nv_wo32(ramfc, 0x88, chan->cache->vinst >> 10);
-		nv_wo32(ramfc, 0x98, chan->ramin->vinst >> 12);
+	/* tell all connected engines to unload their contexts */
+	for (i = 0; i < priv->base.channels; i++) {
+		struct nouveau_channel *chan = dev_priv->channels.ptr[i];
+		if (chan && !nv50_fifo_kickoff(chan))
+			return -EBUSY;
 	}
 
-	dev_priv->engine.instmem.flush(dev);
-
-	nv50_fifo_channel_enable(dev, chan->id);
-	nv50_fifo_playlist_update(dev);
-	spin_unlock_irqrestore(&dev_priv->context_switch_lock, flags);
+	nv_wr32(dev, 0x002140, 0);
 	return 0;
 }
 
 void
-nv50_fifo_destroy_context(struct nouveau_channel *chan)
+nv50_fifo_tlb_flush(struct drm_device *dev, int engine)
 {
-	struct drm_device *dev = chan->dev;
-	struct drm_nouveau_private *dev_priv = dev->dev_private;
-	struct nouveau_fifo_engine *pfifo = &dev_priv->engine.fifo;
-	struct nouveau_gpuobj *ramfc = NULL;
-	unsigned long flags;
-
-	NV_DEBUG(dev, "ch%d\n", chan->id);
-
-	spin_lock_irqsave(&dev_priv->context_switch_lock, flags);
-	pfifo->reassign(dev, false);
-
-	/* Unload the context if it's the currently active one */
-	if (pfifo->channel_id(dev) == chan->id) {
-		pfifo->disable(dev);
-		pfifo->unload_context(dev);
-		pfifo->enable(dev);
-	}
-
-	/* This will ensure the channel is seen as disabled. */
-	nouveau_gpuobj_ref(chan->ramfc, &ramfc);
-	nouveau_gpuobj_ref(NULL, &chan->ramfc);
-	nv50_fifo_channel_disable(dev, chan->id);
-
-	/* Dummy channel, also used on ch 127 */
-	if (chan->id == 0)
-		nv50_fifo_channel_disable(dev, 127);
-	nv50_fifo_playlist_update(dev);
-
-	pfifo->reassign(dev, true);
-	spin_unlock_irqrestore(&dev_priv->context_switch_lock, flags);
-
-	/* Free the channel resources */
-	if (chan->user) {
-		iounmap(chan->user);
-		chan->user = NULL;
-	}
-	nouveau_gpuobj_ref(NULL, &ramfc);
-	nouveau_gpuobj_ref(NULL, &chan->cache);
+	nv50_vm_flush_engine(dev, 5);
 }
 
-int
-nv50_fifo_load_context(struct nouveau_channel *chan)
+void
+nv50_fifo_destroy(struct drm_device *dev, int engine)
 {
-	struct drm_device *dev = chan->dev;
 	struct drm_nouveau_private *dev_priv = dev->dev_private;
-	struct nouveau_gpuobj *ramfc = chan->ramfc;
-	struct nouveau_gpuobj *cache = chan->cache;
-	int ptr, cnt;
-
-	NV_DEBUG(dev, "ch%d\n", chan->id);
-
-	nv_wr32(dev, 0x3330, nv_ro32(ramfc, 0x00));
-	nv_wr32(dev, 0x3334, nv_ro32(ramfc, 0x04));
-	nv_wr32(dev, 0x3240, nv_ro32(ramfc, 0x08));
-	nv_wr32(dev, 0x3320, nv_ro32(ramfc, 0x0c));
-	nv_wr32(dev, 0x3244, nv_ro32(ramfc, 0x10));
-	nv_wr32(dev, 0x3328, nv_ro32(ramfc, 0x14));
-	nv_wr32(dev, 0x3368, nv_ro32(ramfc, 0x18));
-	nv_wr32(dev, 0x336c, nv_ro32(ramfc, 0x1c));
-	nv_wr32(dev, 0x3370, nv_ro32(ramfc, 0x20));
-	nv_wr32(dev, 0x3374, nv_ro32(ramfc, 0x24));
-	nv_wr32(dev, 0x3378, nv_ro32(ramfc, 0x28));
-	nv_wr32(dev, 0x337c, nv_ro32(ramfc, 0x2c));
-	nv_wr32(dev, 0x3228, nv_ro32(ramfc, 0x30));
-	nv_wr32(dev, 0x3364, nv_ro32(ramfc, 0x34));
-	nv_wr32(dev, 0x32a0, nv_ro32(ramfc, 0x38));
-	nv_wr32(dev, 0x3224, nv_ro32(ramfc, 0x3c));
-	nv_wr32(dev, 0x324c, nv_ro32(ramfc, 0x40));
-	nv_wr32(dev, 0x2044, nv_ro32(ramfc, 0x44));
-	nv_wr32(dev, 0x322c, nv_ro32(ramfc, 0x48));
-	nv_wr32(dev, 0x3234, nv_ro32(ramfc, 0x4c));
-	nv_wr32(dev, 0x3340, nv_ro32(ramfc, 0x50));
-	nv_wr32(dev, 0x3344, nv_ro32(ramfc, 0x54));
-	nv_wr32(dev, 0x3280, nv_ro32(ramfc, 0x58));
-	nv_wr32(dev, 0x3254, nv_ro32(ramfc, 0x5c));
-	nv_wr32(dev, 0x3260, nv_ro32(ramfc, 0x60));
-	nv_wr32(dev, 0x3264, nv_ro32(ramfc, 0x64));
-	nv_wr32(dev, 0x3268, nv_ro32(ramfc, 0x68));
-	nv_wr32(dev, 0x326c, nv_ro32(ramfc, 0x6c));
-	nv_wr32(dev, 0x32e4, nv_ro32(ramfc, 0x70));
-	nv_wr32(dev, 0x3248, nv_ro32(ramfc, 0x74));
-	nv_wr32(dev, 0x2088, nv_ro32(ramfc, 0x78));
-	nv_wr32(dev, 0x2058, nv_ro32(ramfc, 0x7c));
-	nv_wr32(dev, 0x2210, nv_ro32(ramfc, 0x80));
-
-	cnt = nv_ro32(ramfc, 0x84);
-	for (ptr = 0; ptr < cnt; ptr++) {
-		nv_wr32(dev, NV40_PFIFO_CACHE1_METHOD(ptr),
-			nv_ro32(cache, (ptr * 8) + 0));
-		nv_wr32(dev, NV40_PFIFO_CACHE1_DATA(ptr),
-			nv_ro32(cache, (ptr * 8) + 4));
-	}
-	nv_wr32(dev, NV03_PFIFO_CACHE1_PUT, cnt << 2);
-	nv_wr32(dev, NV03_PFIFO_CACHE1_GET, 0);
-
-	/* guessing that all the 0x34xx regs aren't on NV50 */
-	if (dev_priv->chipset != 0x50) {
-		nv_wr32(dev, 0x340c, nv_ro32(ramfc, 0x88));
-		nv_wr32(dev, 0x3400, nv_ro32(ramfc, 0x8c));
-		nv_wr32(dev, 0x3404, nv_ro32(ramfc, 0x90));
-		nv_wr32(dev, 0x3408, nv_ro32(ramfc, 0x94));
-		nv_wr32(dev, 0x3410, nv_ro32(ramfc, 0x98));
-	}
+	struct nv50_fifo_priv *priv = nv_engine(dev, engine);
 
-	nv_wr32(dev, NV03_PFIFO_CACHE1_PUSH1, chan->id | (1<<16));
-	return 0;
+	nouveau_irq_unregister(dev, 8);
+
+	nouveau_gpuobj_ref(NULL, &priv->playlist[0]);
+	nouveau_gpuobj_ref(NULL, &priv->playlist[1]);
+
+	dev_priv->eng[engine] = NULL;
+	kfree(priv);
 }
 
 int
-nv50_fifo_unload_context(struct drm_device *dev)
+nv50_fifo_create(struct drm_device *dev)
 {
 	struct drm_nouveau_private *dev_priv = dev->dev_private;
-	struct nouveau_fifo_engine *pfifo = &dev_priv->engine.fifo;
-	struct nouveau_gpuobj *ramfc, *cache;
-	struct nouveau_channel *chan = NULL;
-	int chid, get, put, ptr;
-
-	NV_DEBUG(dev, "\n");
-
-	chid = pfifo->channel_id(dev);
-	if (chid < 1 || chid >= dev_priv->engine.fifo.channels - 1)
-		return 0;
-
-	chan = dev_priv->channels.ptr[chid];
-	if (!chan) {
-		NV_ERROR(dev, "Inactive channel on PFIFO: %d\n", chid);
-		return -EINVAL;
-	}
-	NV_DEBUG(dev, "ch%d\n", chan->id);
-	ramfc = chan->ramfc;
-	cache = chan->cache;
-
-	nv_wo32(ramfc, 0x00, nv_rd32(dev, 0x3330));
-	nv_wo32(ramfc, 0x04, nv_rd32(dev, 0x3334));
-	nv_wo32(ramfc, 0x08, nv_rd32(dev, 0x3240));
-	nv_wo32(ramfc, 0x0c, nv_rd32(dev, 0x3320));
-	nv_wo32(ramfc, 0x10, nv_rd32(dev, 0x3244));
-	nv_wo32(ramfc, 0x14, nv_rd32(dev, 0x3328));
-	nv_wo32(ramfc, 0x18, nv_rd32(dev, 0x3368));
-	nv_wo32(ramfc, 0x1c, nv_rd32(dev, 0x336c));
-	nv_wo32(ramfc, 0x20, nv_rd32(dev, 0x3370));
-	nv_wo32(ramfc, 0x24, nv_rd32(dev, 0x3374));
-	nv_wo32(ramfc, 0x28, nv_rd32(dev, 0x3378));
-	nv_wo32(ramfc, 0x2c, nv_rd32(dev, 0x337c));
-	nv_wo32(ramfc, 0x30, nv_rd32(dev, 0x3228));
-	nv_wo32(ramfc, 0x34, nv_rd32(dev, 0x3364));
-	nv_wo32(ramfc, 0x38, nv_rd32(dev, 0x32a0));
-	nv_wo32(ramfc, 0x3c, nv_rd32(dev, 0x3224));
-	nv_wo32(ramfc, 0x40, nv_rd32(dev, 0x324c));
-	nv_wo32(ramfc, 0x44, nv_rd32(dev, 0x2044));
-	nv_wo32(ramfc, 0x48, nv_rd32(dev, 0x322c));
-	nv_wo32(ramfc, 0x4c, nv_rd32(dev, 0x3234));
-	nv_wo32(ramfc, 0x50, nv_rd32(dev, 0x3340));
-	nv_wo32(ramfc, 0x54, nv_rd32(dev, 0x3344));
-	nv_wo32(ramfc, 0x58, nv_rd32(dev, 0x3280));
-	nv_wo32(ramfc, 0x5c, nv_rd32(dev, 0x3254));
-	nv_wo32(ramfc, 0x60, nv_rd32(dev, 0x3260));
-	nv_wo32(ramfc, 0x64, nv_rd32(dev, 0x3264));
-	nv_wo32(ramfc, 0x68, nv_rd32(dev, 0x3268));
-	nv_wo32(ramfc, 0x6c, nv_rd32(dev, 0x326c));
-	nv_wo32(ramfc, 0x70, nv_rd32(dev, 0x32e4));
-	nv_wo32(ramfc, 0x74, nv_rd32(dev, 0x3248));
-	nv_wo32(ramfc, 0x78, nv_rd32(dev, 0x2088));
-	nv_wo32(ramfc, 0x7c, nv_rd32(dev, 0x2058));
-	nv_wo32(ramfc, 0x80, nv_rd32(dev, 0x2210));
-
-	put = (nv_rd32(dev, NV03_PFIFO_CACHE1_PUT) & 0x7ff) >> 2;
-	get = (nv_rd32(dev, NV03_PFIFO_CACHE1_GET) & 0x7ff) >> 2;
-	ptr = 0;
-	while (put != get) {
-		nv_wo32(cache, ptr + 0,
-			nv_rd32(dev, NV40_PFIFO_CACHE1_METHOD(get)));
-		nv_wo32(cache, ptr + 4,
-			nv_rd32(dev, NV40_PFIFO_CACHE1_DATA(get)));
-		get = (get + 1) & 0x1ff;
-		ptr += 8;
-	}
-
-	/* guessing that all the 0x34xx regs aren't on NV50 */
-	if (dev_priv->chipset != 0x50) {
-		nv_wo32(ramfc, 0x84, ptr >> 3);
-		nv_wo32(ramfc, 0x88, nv_rd32(dev, 0x340c));
-		nv_wo32(ramfc, 0x8c, nv_rd32(dev, 0x3400));
-		nv_wo32(ramfc, 0x90, nv_rd32(dev, 0x3404));
-		nv_wo32(ramfc, 0x94, nv_rd32(dev, 0x3408));
-		nv_wo32(ramfc, 0x98, nv_rd32(dev, 0x3410));
-	}
+	struct nv50_fifo_priv *priv;
+	int ret;
 
-	dev_priv->engine.instmem.flush(dev);
+	priv = kzalloc(sizeof(*priv), GFP_KERNEL);
+	if (!priv)
+		return -ENOMEM;
 
-	/*XXX: probably reload ch127 (NULL) state back too */
-	nv_wr32(dev, NV03_PFIFO_CACHE1_PUSH1, 127);
-	return 0;
-}
+	priv->base.base.destroy = nv50_fifo_destroy;
+	priv->base.base.init = nv50_fifo_init;
+	priv->base.base.fini = nv50_fifo_fini;
+	priv->base.base.context_new = nv50_fifo_context_new;
+	priv->base.base.context_del = nv50_fifo_context_del;
+	priv->base.base.tlb_flush = nv50_fifo_tlb_flush;
+	priv->base.channels = 127;
+	dev_priv->eng[NVOBJ_ENGINE_FIFO] = &priv->base.base;
+
+	ret = nouveau_gpuobj_new(dev, NULL, priv->base.channels * 4, 0x1000,
+				 NVOBJ_FLAG_ZERO_ALLOC, &priv->playlist[0]);
+	if (ret)
+		goto error;
+
+	ret = nouveau_gpuobj_new(dev, NULL, priv->base.channels * 4, 0x1000,
+				 NVOBJ_FLAG_ZERO_ALLOC, &priv->playlist[1]);
+	if (ret)
+		goto error;
 
-void
-nv50_fifo_tlb_flush(struct drm_device *dev)
-{
-	nv50_vm_flush_engine(dev, 5);
+	nouveau_irq_register(dev, 8, nv04_fifo_isr);
+error:
+	if (ret)
+		priv->base.base.destroy(dev, NVOBJ_ENGINE_FIFO);
+	return ret;
 }
diff --git a/drivers/gpu/drm/nouveau/nv50_graph.c b/drivers/gpu/drm/nouveau/nv50_graph.c
index 33d5711a918..d9cc2f2638d 100644
--- a/drivers/gpu/drm/nouveau/nv50_graph.c
+++ b/drivers/gpu/drm/nouveau/nv50_graph.c
@@ -27,8 +27,8 @@
 #include "drmP.h"
 #include "drm.h"
 #include "nouveau_drv.h"
+#include "nouveau_fifo.h"
 #include "nouveau_ramht.h"
-#include "nouveau_grctx.h"
 #include "nouveau_dma.h"
 #include "nouveau_vm.h"
 #include "nv50_evo.h"
@@ -40,86 +40,6 @@ struct nv50_graph_engine {
 	u32 grctx_size;
 };
 
-static void
-nv50_graph_fifo_access(struct drm_device *dev, bool enabled)
-{
-	const uint32_t mask = 0x00010001;
-
-	if (enabled)
-		nv_wr32(dev, 0x400500, nv_rd32(dev, 0x400500) | mask);
-	else
-		nv_wr32(dev, 0x400500, nv_rd32(dev, 0x400500) & ~mask);
-}
-
-static struct nouveau_channel *
-nv50_graph_channel(struct drm_device *dev)
-{
-	struct drm_nouveau_private *dev_priv = dev->dev_private;
-	uint32_t inst;
-	int i;
-
-	/* Be sure we're not in the middle of a context switch or bad things
-	 * will happen, such as unloading the wrong pgraph context.
-	 */
-	if (!nv_wait(dev, 0x400300, 0x00000001, 0x00000000))
-		NV_ERROR(dev, "Ctxprog is still running\n");
-
-	inst = nv_rd32(dev, NV50_PGRAPH_CTXCTL_CUR);
-	if (!(inst & NV50_PGRAPH_CTXCTL_CUR_LOADED))
-		return NULL;
-	inst = (inst & NV50_PGRAPH_CTXCTL_CUR_INSTANCE) << 12;
-
-	for (i = 0; i < dev_priv->engine.fifo.channels; i++) {
-		struct nouveau_channel *chan = dev_priv->channels.ptr[i];
-
-		if (chan && chan->ramin && chan->ramin->vinst == inst)
-			return chan;
-	}
-
-	return NULL;
-}
-
-static int
-nv50_graph_do_load_context(struct drm_device *dev, uint32_t inst)
-{
-	uint32_t fifo = nv_rd32(dev, 0x400500);
-
-	nv_wr32(dev, 0x400500, fifo & ~1);
-	nv_wr32(dev, 0x400784, inst);
-	nv_wr32(dev, 0x400824, nv_rd32(dev, 0x400824) | 0x40);
-	nv_wr32(dev, 0x400320, nv_rd32(dev, 0x400320) | 0x11);
-	nv_wr32(dev, 0x400040, 0xffffffff);
-	(void)nv_rd32(dev, 0x400040);
-	nv_wr32(dev, 0x400040, 0x00000000);
-	nv_wr32(dev, 0x400304, nv_rd32(dev, 0x400304) | 1);
-
-	if (nouveau_wait_for_idle(dev))
-		nv_wr32(dev, 0x40032c, inst | (1<<31));
-	nv_wr32(dev, 0x400500, fifo);
-
-	return 0;
-}
-
-static int
-nv50_graph_unload_context(struct drm_device *dev)
-{
-	uint32_t inst;
-
-	inst  = nv_rd32(dev, NV50_PGRAPH_CTXCTL_CUR);
-	if (!(inst & NV50_PGRAPH_CTXCTL_CUR_LOADED))
-		return 0;
-	inst &= NV50_PGRAPH_CTXCTL_CUR_INSTANCE;
-
-	nouveau_wait_for_idle(dev);
-	nv_wr32(dev, 0x400784, inst);
-	nv_wr32(dev, 0x400824, nv_rd32(dev, 0x400824) | 0x20);
-	nv_wr32(dev, 0x400304, nv_rd32(dev, 0x400304) | 0x01);
-	nouveau_wait_for_idle(dev);
-
-	nv_wr32(dev, NV50_PGRAPH_CTXCTL_CUR, inst);
-	return 0;
-}
-
 static int
 nv50_graph_init(struct drm_device *dev, int engine)
 {
@@ -211,12 +131,6 @@ nv50_graph_init(struct drm_device *dev, int engine)
 static int
 nv50_graph_fini(struct drm_device *dev, int engine, bool suspend)
 {
-	nv_mask(dev, 0x400500, 0x00010001, 0x00000000);
-	if (!nv_wait(dev, 0x400700, ~0, 0) && suspend) {
-		nv_mask(dev, 0x400500, 0x00010001, 0x00010001);
-		return -EBUSY;
-	}
-	nv50_graph_unload_context(dev);
 	nv_wr32(dev, 0x40013c, 0x00000000);
 	return 0;
 }
@@ -229,7 +143,6 @@ nv50_graph_context_new(struct nouveau_channel *chan, int engine)
 	struct nouveau_gpuobj *ramin = chan->ramin;
 	struct nouveau_gpuobj *grctx = NULL;
 	struct nv50_graph_engine *pgraph = nv_engine(dev, engine);
-	struct nouveau_grctx ctx = {};
 	int hdr, ret;
 
 	NV_DEBUG(dev, "ch%d\n", chan->id);
@@ -248,11 +161,7 @@ nv50_graph_context_new(struct nouveau_channel *chan, int engine)
 	nv_wo32(ramin, hdr + 0x10, 0);
 	nv_wo32(ramin, hdr + 0x14, 0x00010000);
 
-	ctx.dev = chan->dev;
-	ctx.mode = NOUVEAU_GRCTX_VALS;
-	ctx.data = grctx;
-	nv50_grctx_init(&ctx);
-
+	nv50_grctx_fill(dev, grctx);
 	nv_wo32(grctx, 0x00000, chan->ramin->vinst >> 12);
 
 	dev_priv->engine.instmem.flush(dev);
@@ -268,33 +177,14 @@ nv50_graph_context_del(struct nouveau_channel *chan, int engine)
 	struct nouveau_gpuobj *grctx = chan->engctx[engine];
 	struct drm_device *dev = chan->dev;
 	struct drm_nouveau_private *dev_priv = dev->dev_private;
-	struct nouveau_fifo_engine *pfifo = &dev_priv->engine.fifo;
 	int i, hdr = (dev_priv->chipset == 0x50) ? 0x200 : 0x20;
-	unsigned long flags;
-
-	NV_DEBUG(dev, "ch%d\n", chan->id);
-
-	if (!chan->ramin)
-		return;
-
-	spin_lock_irqsave(&dev_priv->context_switch_lock, flags);
-	pfifo->reassign(dev, false);
-	nv50_graph_fifo_access(dev, false);
-
-	if (nv50_graph_channel(dev) == chan)
-		nv50_graph_unload_context(dev);
 
 	for (i = hdr; i < hdr + 24; i += 4)
 		nv_wo32(chan->ramin, i, 0);
 	dev_priv->engine.instmem.flush(dev);
 
-	nv50_graph_fifo_access(dev, true);
-	pfifo->reassign(dev, true);
-	spin_unlock_irqrestore(&dev_priv->context_switch_lock, flags);
-
-	nouveau_gpuobj_ref(NULL, &grctx);
-
 	atomic_dec(&chan->vm->engref[engine]);
+	nouveau_gpuobj_ref(NULL, &grctx);
 	chan->engctx[engine] = NULL;
 }
 
@@ -325,85 +215,6 @@ nv50_graph_object_new(struct nouveau_channel *chan, int engine,
 }
 
 static void
-nv50_graph_context_switch(struct drm_device *dev)
-{
-	uint32_t inst;
-
-	nv50_graph_unload_context(dev);
-
-	inst  = nv_rd32(dev, NV50_PGRAPH_CTXCTL_NEXT);
-	inst &= NV50_PGRAPH_CTXCTL_NEXT_INSTANCE;
-	nv50_graph_do_load_context(dev, inst);
-
-	nv_wr32(dev, NV40_PGRAPH_INTR_EN, nv_rd32(dev,
-		NV40_PGRAPH_INTR_EN) | NV_PGRAPH_INTR_CONTEXT_SWITCH);
-}
-
-static int
-nv50_graph_nvsw_dma_vblsem(struct nouveau_channel *chan,
-			   u32 class, u32 mthd, u32 data)
-{
-	struct nouveau_gpuobj *gpuobj;
-
-	gpuobj = nouveau_ramht_find(chan, data);
-	if (!gpuobj)
-		return -ENOENT;
-
-	if (nouveau_notifier_offset(gpuobj, NULL))
-		return -EINVAL;
-
-	chan->nvsw.vblsem = gpuobj;
-	chan->nvsw.vblsem_offset = ~0;
-	return 0;
-}
-
-static int
-nv50_graph_nvsw_vblsem_offset(struct nouveau_channel *chan,
-			      u32 class, u32 mthd, u32 data)
-{
-	if (nouveau_notifier_offset(chan->nvsw.vblsem, &data))
-		return -ERANGE;
-
-	chan->nvsw.vblsem_offset = data >> 2;
-	return 0;
-}
-
-static int
-nv50_graph_nvsw_vblsem_release_val(struct nouveau_channel *chan,
-				   u32 class, u32 mthd, u32 data)
-{
-	chan->nvsw.vblsem_rval = data;
-	return 0;
-}
-
-static int
-nv50_graph_nvsw_vblsem_release(struct nouveau_channel *chan,
-			       u32 class, u32 mthd, u32 data)
-{
-	struct drm_device *dev = chan->dev;
-	struct drm_nouveau_private *dev_priv = dev->dev_private;
-
-	if (!chan->nvsw.vblsem || chan->nvsw.vblsem_offset == ~0 || data > 1)
-		return -EINVAL;
-
-	drm_vblank_get(dev, data);
-
-	chan->nvsw.vblsem_head = data;
-	list_add(&chan->nvsw.vbl_wait, &dev_priv->vbl_waiting);
-
-	return 0;
-}
-
-static int
-nv50_graph_nvsw_mthd_page_flip(struct nouveau_channel *chan,
-			       u32 class, u32 mthd, u32 data)
-{
-	nouveau_finish_page_flip(chan, NULL);
-	return 0;
-}
-
-
-static void
 nv50_graph_tlb_flush(struct drm_device *dev, int engine)
 {
 	nv50_vm_flush_engine(dev, 0);
@@ -514,6 +325,7 @@ struct nouveau_enum nv50_data_error_names[] = {
 	{ 0x0000001f, "RT_BPP128_WITH_MS8", NULL },
 	{ 0x00000021, "Z_OUT_OF_BOUNDS", NULL },
 	{ 0x00000023, "XY_OUT_OF_BOUNDS", NULL },
+	{ 0x00000024, "VP_ZERO_INPUTS", NULL },
 	{ 0x00000027, "CP_MORE_PARAMS_THAN_SHARED", NULL },
 	{ 0x00000028, "CP_NO_REG_SPACE_STRIPED", NULL },
 	{ 0x00000029, "CP_NO_REG_SPACE_PACKED", NULL },
@@ -900,13 +712,14 @@ nv50_pgraph_trap_handler(struct drm_device *dev, u32 display, u64 inst, u32 chid
 int
 nv50_graph_isr_chid(struct drm_device *dev, u64 inst)
 {
+	struct nouveau_fifo_priv *pfifo = nv_engine(dev, NVOBJ_ENGINE_FIFO);
 	struct drm_nouveau_private *dev_priv = dev->dev_private;
 	struct nouveau_channel *chan;
 	unsigned long flags;
 	int i;
 
 	spin_lock_irqsave(&dev_priv->channels.lock, flags);
-	for (i = 0; i < dev_priv->engine.fifo.channels; i++) {
+	for (i = 0; i < pfifo->channels; i++) {
 		chan = dev_priv->channels.ptr[i];
 		if (!chan || !chan->ramin)
 			continue;
@@ -939,15 +752,6 @@ nv50_graph_isr(struct drm_device *dev)
 				show &= ~0x00000010;
 		}
 
-		if (stat & 0x00001000) {
-			nv_wr32(dev, 0x400500, 0x00000000);
-			nv_wr32(dev, 0x400100, 0x00001000);
-			nv_mask(dev, 0x40013c, 0x00001000, 0x00000000);
-			nv50_graph_context_switch(dev);
-			stat &= ~0x00001000;
-			show &= ~0x00001000;
-		}
-
 		show = (show && nouveau_ratelimit()) ? show : 0;
 
 		if (show & 0x00100000) {
@@ -996,28 +800,21 @@ nv50_graph_create(struct drm_device *dev)
 {
 	struct drm_nouveau_private *dev_priv = dev->dev_private;
 	struct nv50_graph_engine *pgraph;
-	struct nouveau_grctx ctx = {};
 	int ret;
 
 	pgraph = kzalloc(sizeof(*pgraph),GFP_KERNEL);
 	if (!pgraph)
 		return -ENOMEM;
 
-	ctx.dev = dev;
-	ctx.mode = NOUVEAU_GRCTX_PROG;
-	ctx.data = pgraph->ctxprog;
-	ctx.ctxprog_max = ARRAY_SIZE(pgraph->ctxprog);
-
-	ret = nv50_grctx_init(&ctx);
+	ret = nv50_grctx_init(dev, pgraph->ctxprog, ARRAY_SIZE(pgraph->ctxprog),
+				  &pgraph->ctxprog_size,
+				  &pgraph->grctx_size);
 	if (ret) {
 		NV_ERROR(dev, "PGRAPH: ctxprog build failed\n");
 		kfree(pgraph);
 		return 0;
 	}
 
-	pgraph->grctx_size = ctx.ctxvals_pos * 4;
-	pgraph->ctxprog_size = ctx.ctxprog_len;
-
 	pgraph->base.destroy = nv50_graph_destroy;
 	pgraph->base.init = nv50_graph_init;
 	pgraph->base.fini = nv50_graph_fini;
@@ -1031,14 +828,6 @@ nv50_graph_create(struct drm_device *dev)
 
 	nouveau_irq_register(dev, 12, nv50_graph_isr);
 
-	/* NVSW really doesn't live here... */
-	NVOBJ_CLASS(dev, 0x506e, SW); /* nvsw */
-	NVOBJ_MTHD (dev, 0x506e, 0x018c, nv50_graph_nvsw_dma_vblsem);
-	NVOBJ_MTHD (dev, 0x506e, 0x0400, nv50_graph_nvsw_vblsem_offset);
-	NVOBJ_MTHD (dev, 0x506e, 0x0404, nv50_graph_nvsw_vblsem_release_val);
-	NVOBJ_MTHD (dev, 0x506e, 0x0408, nv50_graph_nvsw_vblsem_release);
-	NVOBJ_MTHD (dev, 0x506e, 0x0500, nv50_graph_nvsw_mthd_page_flip);
-
 	NVOBJ_ENGINE_ADD(dev, GR, &pgraph->base);
 	NVOBJ_CLASS(dev, 0x0030, GR); /* null */
 	NVOBJ_CLASS(dev, 0x5039, GR); /* m2mf */
diff --git a/drivers/gpu/drm/nouveau/nv50_grctx.c b/drivers/gpu/drm/nouveau/nv50_grctx.c
index 4b46d696856..881e22b249f 100644
--- a/drivers/gpu/drm/nouveau/nv50_grctx.c
+++ b/drivers/gpu/drm/nouveau/nv50_grctx.c
@@ -172,8 +172,8 @@ static void nv50_graph_construct_xfer2(struct nouveau_grctx *ctx);
 
 /* Main function: construct the ctxprog skeleton, call the other functions. */
 
-int
-nv50_grctx_init(struct nouveau_grctx *ctx)
+static int
+nv50_grctx_generate(struct nouveau_grctx *ctx)
 {
 	struct drm_nouveau_private *dev_priv = ctx->dev->dev_private;
 
@@ -210,7 +210,7 @@ nv50_grctx_init(struct nouveau_grctx *ctx)
 	cp_name(ctx, cp_check_load);
 	cp_bra (ctx, AUTO_LOAD, PENDING, cp_setup_auto_load);
 	cp_bra (ctx, USER_LOAD, PENDING, cp_setup_load);
-	cp_bra (ctx, ALWAYS, TRUE, cp_exit);
+	cp_bra (ctx, ALWAYS, TRUE, cp_prepare_exit);
 
 	/* setup for context load */
 	cp_name(ctx, cp_setup_auto_load);
@@ -277,6 +277,33 @@ nv50_grctx_init(struct nouveau_grctx *ctx)
 	return 0;
 }
 
+void
+nv50_grctx_fill(struct drm_device *dev, struct nouveau_gpuobj *mem)
+{
+	nv50_grctx_generate(&(struct nouveau_grctx) {
+			     .dev = dev,
+			     .mode = NOUVEAU_GRCTX_VALS,
+			     .data = mem,
+			   });
+}
+
+int
+nv50_grctx_init(struct drm_device *dev, u32 *data, u32 max, u32 *len, u32 *cnt)
+{
+	struct nouveau_grctx ctx = {
+		.dev = dev,
+		.mode = NOUVEAU_GRCTX_PROG,
+		.data = data,
+		.ctxprog_max = max
+	};
+	int ret;
+
+	ret = nv50_grctx_generate(&ctx);
+	*cnt = ctx.ctxvals_pos * 4;
+	*len = ctx.ctxprog_len;
+	return ret;
+}
+
 /*
  * Constructs MMIO part of ctxprog and ctxvals. Just a matter of knowing which
  * registers to save/restore and the default values for them.
diff --git a/drivers/gpu/drm/nouveau/nv50_instmem.c b/drivers/gpu/drm/nouveau/nv50_instmem.c
index a7c12c94a5a..0bba54f1180 100644
--- a/drivers/gpu/drm/nouveau/nv50_instmem.c
+++ b/drivers/gpu/drm/nouveau/nv50_instmem.c
@@ -83,7 +83,7 @@ nv50_channel_new(struct drm_device *dev, u32 size, struct nouveau_vm *vm,
 		return ret;
 	}
 
-	ret = drm_mm_init(&chan->ramin_heap, 0x6000, chan->ramin->size);
+	ret = drm_mm_init(&chan->ramin_heap, 0x6000, chan->ramin->size - 0x6000);
 	if (ret) {
 		nv50_channel_del(&chan);
 		return ret;
diff --git a/drivers/gpu/drm/nouveau/nv50_mpeg.c b/drivers/gpu/drm/nouveau/nv50_mpeg.c
index b57a2d180ad..90e8ed22cfc 100644
--- a/drivers/gpu/drm/nouveau/nv50_mpeg.c
+++ b/drivers/gpu/drm/nouveau/nv50_mpeg.c
@@ -77,27 +77,13 @@ nv50_mpeg_context_new(struct nouveau_channel *chan, int engine)
 static void
 nv50_mpeg_context_del(struct nouveau_channel *chan, int engine)
 {
-	struct drm_nouveau_private *dev_priv = chan->dev->dev_private;
 	struct nouveau_gpuobj *ctx = chan->engctx[engine];
 	struct drm_device *dev = chan->dev;
-	unsigned long flags;
-	u32 inst, i;
-
-	if (!chan->ramin)
-		return;
-
-	inst  = chan->ramin->vinst >> 12;
-	inst |= 0x80000000;
-
-	spin_lock_irqsave(&dev_priv->context_switch_lock, flags);
-	nv_mask(dev, 0x00b32c, 0x00000001, 0x00000000);
-	if (nv_rd32(dev, 0x00b318) == inst)
-		nv_mask(dev, 0x00b318, 0x80000000, 0x00000000);
-	nv_mask(dev, 0x00b32c, 0x00000001, 0x00000001);
-	spin_unlock_irqrestore(&dev_priv->context_switch_lock, flags);
+	int i;
 
 	for (i = 0x00; i <= 0x14; i += 4)
 		nv_wo32(chan->ramin, CTX_PTR(dev, i), 0x00000000);
+
 	nouveau_gpuobj_ref(NULL, &ctx);
 	chan->engctx[engine] = NULL;
 }
@@ -162,7 +148,6 @@ nv50_mpeg_init(struct drm_device *dev, int engine)
 static int
 nv50_mpeg_fini(struct drm_device *dev, int engine, bool suspend)
 {
-	/*XXX: context save for s/r */
 	nv_mask(dev, 0x00b32c, 0x00000001, 0x00000000);
 	nv_wr32(dev, 0x00b140, 0x00000000);
 	return 0;
diff --git a/drivers/gpu/drm/nouveau/nv50_software.c b/drivers/gpu/drm/nouveau/nv50_software.c
new file mode 100644
index 00000000000..114d2517d4a
--- /dev/null
+++ b/drivers/gpu/drm/nouveau/nv50_software.c
@@ -0,0 +1,214 @@
+/*
+ * 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 "drmP.h"
+
+#include "nouveau_drv.h"
+#include "nouveau_ramht.h"
+#include "nouveau_software.h"
+
+#include "nv50_display.h"
+
+struct nv50_software_priv {
+	struct nouveau_software_priv base;
+};
+
+struct nv50_software_chan {
+	struct nouveau_software_chan base;
+	struct {
+		struct nouveau_gpuobj *object;
+	} vblank;
+};
+
+static int
+mthd_dma_vblsem(struct nouveau_channel *chan, u32 class, u32 mthd, u32 data)
+{
+	struct nv50_software_chan *pch = chan->engctx[NVOBJ_ENGINE_SW];
+	struct nouveau_gpuobj *gpuobj;
+
+	gpuobj = nouveau_ramht_find(chan, data);
+	if (!gpuobj)
+		return -ENOENT;
+
+	if (nouveau_notifier_offset(gpuobj, NULL))
+		return -EINVAL;
+
+	pch->vblank.object = gpuobj;
+	pch->base.vblank.offset = ~0;
+	return 0;
+}
+
+static int
+mthd_vblsem_offset(struct nouveau_channel *chan, u32 class, u32 mthd, u32 data)
+{
+	struct nv50_software_chan *pch = chan->engctx[NVOBJ_ENGINE_SW];
+
+	if (nouveau_notifier_offset(pch->vblank.object, &data))
+		return -ERANGE;
+
+	pch->base.vblank.offset = data >> 2;
+	return 0;
+}
+
+static int
+mthd_vblsem_value(struct nouveau_channel *chan, u32 class, u32 mthd, u32 data)
+{
+	struct nv50_software_chan *pch = chan->engctx[NVOBJ_ENGINE_SW];
+	pch->base.vblank.value = data;
+	return 0;
+}
+
+static int
+mthd_vblsem_release(struct nouveau_channel *chan, u32 class, u32 mthd, u32 data)
+{
+	struct nv50_software_priv *psw = nv_engine(chan->dev, NVOBJ_ENGINE_SW);
+	struct nv50_software_chan *pch = chan->engctx[NVOBJ_ENGINE_SW];
+	struct drm_device *dev = chan->dev;
+
+	if (!pch->vblank.object || pch->base.vblank.offset == ~0 || data > 1)
+		return -EINVAL;
+
+	drm_vblank_get(dev, data);
+
+	pch->base.vblank.head = data;
+	list_add(&pch->base.vblank.list, &psw->base.vblank);
+	return 0;
+}
+
+static int
+mthd_flip(struct nouveau_channel *chan, u32 class, u32 mthd, u32 data)
+{
+	nouveau_finish_page_flip(chan, NULL);
+	return 0;
+}
+
+static int
+nv50_software_context_new(struct nouveau_channel *chan, int engine)
+{
+	struct nv50_software_priv *psw = nv_engine(chan->dev, NVOBJ_ENGINE_SW);
+	struct nv50_display *pdisp = nv50_display(chan->dev);
+	struct nv50_software_chan *pch;
+	int ret = 0, i;
+
+	pch = kzalloc(sizeof(*pch), GFP_KERNEL);
+	if (!pch)
+		return -ENOMEM;
+
+	nouveau_software_context_new(&pch->base);
+	pch->base.vblank.bo = chan->notifier_bo;
+	chan->engctx[engine] = pch;
+
+	/* dma objects for display sync channel semaphore blocks */
+	for (i = 0; i < chan->dev->mode_config.num_crtc; i++) {
+		struct nv50_display_crtc *dispc = &pdisp->crtc[i];
+		struct nouveau_gpuobj *obj = NULL;
+
+		ret = nouveau_gpuobj_dma_new(chan, NV_CLASS_DMA_IN_MEMORY,
+					     dispc->sem.bo->bo.offset, 0x1000,
+					     NV_MEM_ACCESS_RW,
+					     NV_MEM_TARGET_VRAM, &obj);
+		if (ret)
+			break;
+
+		ret = nouveau_ramht_insert(chan, NvEvoSema0 + i, obj);
+		nouveau_gpuobj_ref(NULL, &obj);
+	}
+
+	if (ret)
+		psw->base.base.context_del(chan, engine);
+	return ret;
+}
+
+static void
+nv50_software_context_del(struct nouveau_channel *chan, int engine)
+{
+	struct nv50_software_chan *pch = chan->engctx[engine];
+	chan->engctx[engine] = NULL;
+	kfree(pch);
+}
+
+static int
+nv50_software_object_new(struct nouveau_channel *chan, int engine,
+			 u32 handle, u16 class)
+{
+	struct drm_device *dev = chan->dev;
+	struct nouveau_gpuobj *obj = NULL;
+	int ret;
+
+	ret = nouveau_gpuobj_new(dev, chan, 16, 16, 0, &obj);
+	if (ret)
+		return ret;
+	obj->engine = 0;
+	obj->class  = class;
+
+	ret = nouveau_ramht_insert(chan, handle, obj);
+	nouveau_gpuobj_ref(NULL, &obj);
+	return ret;
+}
+
+static int
+nv50_software_init(struct drm_device *dev, int engine)
+{
+	return 0;
+}
+
+static int
+nv50_software_fini(struct drm_device *dev, int engine, bool suspend)
+{
+	return 0;
+}
+
+static void
+nv50_software_destroy(struct drm_device *dev, int engine)
+{
+	struct nv50_software_priv *psw = nv_engine(dev, engine);
+
+	NVOBJ_ENGINE_DEL(dev, SW);
+	kfree(psw);
+}
+
+int
+nv50_software_create(struct drm_device *dev)
+{
+	struct nv50_software_priv *psw = kzalloc(sizeof(*psw), GFP_KERNEL);
+	if (!psw)
+		return -ENOMEM;
+
+	psw->base.base.destroy = nv50_software_destroy;
+	psw->base.base.init = nv50_software_init;
+	psw->base.base.fini = nv50_software_fini;
+	psw->base.base.context_new = nv50_software_context_new;
+	psw->base.base.context_del = nv50_software_context_del;
+	psw->base.base.object_new = nv50_software_object_new;
+	nouveau_software_create(&psw->base);
+
+	NVOBJ_ENGINE_ADD(dev, SW, &psw->base.base);
+	NVOBJ_CLASS(dev, 0x506e, SW);
+	NVOBJ_MTHD (dev, 0x506e, 0x018c, mthd_dma_vblsem);
+	NVOBJ_MTHD (dev, 0x506e, 0x0400, mthd_vblsem_offset);
+	NVOBJ_MTHD (dev, 0x506e, 0x0404, mthd_vblsem_value);
+	NVOBJ_MTHD (dev, 0x506e, 0x0408, mthd_vblsem_release);
+	NVOBJ_MTHD (dev, 0x506e, 0x0500, mthd_flip);
+	return 0;
+}
diff --git a/drivers/gpu/drm/nouveau/nv50_sor.c b/drivers/gpu/drm/nouveau/nv50_sor.c
index 27464021247..a9514eaa74c 100644
--- a/drivers/gpu/drm/nouveau/nv50_sor.c
+++ b/drivers/gpu/drm/nouveau/nv50_sor.c
@@ -242,9 +242,9 @@ nv50_sor_disconnect(struct drm_encoder *encoder)
 		NV_ERROR(dev, "no space while disconnecting SOR\n");
 		return;
 	}
-	BEGIN_RING(evo, 0, NV50_EVO_SOR(nv_encoder->or, MODE_CTRL), 1);
+	BEGIN_NV04(evo, 0, NV50_EVO_SOR(nv_encoder->or, MODE_CTRL), 1);
 	OUT_RING  (evo, 0);
-	BEGIN_RING(evo, 0, NV50_EVO_UPDATE, 1);
+	BEGIN_NV04(evo, 0, NV50_EVO_UPDATE, 1);
 	OUT_RING  (evo, 0);
 
 	nouveau_hdmi_mode_set(encoder, NULL);
@@ -430,7 +430,7 @@ nv50_sor_mode_set(struct drm_encoder *encoder, struct drm_display_mode *umode,
 		nv_encoder->crtc = NULL;
 		return;
 	}
-	BEGIN_RING(evo, 0, NV50_EVO_SOR(nv_encoder->or, MODE_CTRL), 1);
+	BEGIN_NV04(evo, 0, NV50_EVO_SOR(nv_encoder->or, MODE_CTRL), 1);
 	OUT_RING(evo, mode_ctl);
 }
 
diff --git a/drivers/gpu/drm/nouveau/nv50_vm.c b/drivers/gpu/drm/nouveau/nv50_vm.c
index 44fbac9c7d9..179bb42a635 100644
--- a/drivers/gpu/drm/nouveau/nv50_vm.c
+++ b/drivers/gpu/drm/nouveau/nv50_vm.c
@@ -147,7 +147,6 @@ nv50_vm_flush(struct nouveau_vm *vm)
 {
 	struct drm_nouveau_private *dev_priv = vm->dev->dev_private;
 	struct nouveau_instmem_engine *pinstmem = &dev_priv->engine.instmem;
-	struct nouveau_fifo_engine *pfifo = &dev_priv->engine.fifo;
 	int i;
 
 	pinstmem->flush(vm->dev);
@@ -158,7 +157,6 @@ nv50_vm_flush(struct nouveau_vm *vm)
 		return;
 	}
 
-	pfifo->tlb_flush(vm->dev);
 	for (i = 0; i < NVOBJ_ENGINE_NR; i++) {
 		if (atomic_read(&vm->engref[i]))
 			dev_priv->eng[i]->tlb_flush(vm->dev, i);
diff --git a/drivers/gpu/drm/nouveau/nv84_fence.c b/drivers/gpu/drm/nouveau/nv84_fence.c
new file mode 100644
index 00000000000..c2f889b0d34
--- /dev/null
+++ b/drivers/gpu/drm/nouveau/nv84_fence.c
@@ -0,0 +1,177 @@
+/*
+ * 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 "drmP.h"
+#include "nouveau_drv.h"
+#include "nouveau_dma.h"
+#include "nouveau_fifo.h"
+#include "nouveau_ramht.h"
+#include "nouveau_fence.h"
+
+struct nv84_fence_chan {
+	struct nouveau_fence_chan base;
+};
+
+struct nv84_fence_priv {
+	struct nouveau_fence_priv base;
+	struct nouveau_gpuobj *mem;
+};
+
+static int
+nv84_fence_emit(struct nouveau_fence *fence)
+{
+	struct nouveau_channel *chan = fence->channel;
+	int ret = RING_SPACE(chan, 7);
+	if (ret == 0) {
+		BEGIN_NV04(chan, 0, NV11_SUBCHAN_DMA_SEMAPHORE, 1);
+		OUT_RING  (chan, NvSema);
+		BEGIN_NV04(chan, 0, NV84_SUBCHAN_SEMAPHORE_ADDRESS_HIGH, 4);
+		OUT_RING  (chan, upper_32_bits(chan->id * 16));
+		OUT_RING  (chan, lower_32_bits(chan->id * 16));
+		OUT_RING  (chan, fence->sequence);
+		OUT_RING  (chan, NV84_SUBCHAN_SEMAPHORE_TRIGGER_WRITE_LONG);
+		FIRE_RING (chan);
+	}
+	return ret;
+}
+
+
+static int
+nv84_fence_sync(struct nouveau_fence *fence,
+		struct nouveau_channel *prev, struct nouveau_channel *chan)
+{
+	int ret = RING_SPACE(chan, 7);
+	if (ret == 0) {
+		BEGIN_NV04(chan, 0, NV11_SUBCHAN_DMA_SEMAPHORE, 1);
+		OUT_RING  (chan, NvSema);
+		BEGIN_NV04(chan, 0, NV84_SUBCHAN_SEMAPHORE_ADDRESS_HIGH, 4);
+		OUT_RING  (chan, upper_32_bits(prev->id * 16));
+		OUT_RING  (chan, lower_32_bits(prev->id * 16));
+		OUT_RING  (chan, fence->sequence);
+		OUT_RING  (chan, NV84_SUBCHAN_SEMAPHORE_TRIGGER_ACQUIRE_GEQUAL);
+		FIRE_RING (chan);
+	}
+	return ret;
+}
+
+static u32
+nv84_fence_read(struct nouveau_channel *chan)
+{
+	struct nv84_fence_priv *priv = nv_engine(chan->dev, NVOBJ_ENGINE_FENCE);
+	return nv_ro32(priv->mem, chan->id * 16);
+}
+
+static void
+nv84_fence_context_del(struct nouveau_channel *chan, int engine)
+{
+	struct nv84_fence_chan *fctx = chan->engctx[engine];
+	nouveau_fence_context_del(&fctx->base);
+	chan->engctx[engine] = NULL;
+	kfree(fctx);
+}
+
+static int
+nv84_fence_context_new(struct nouveau_channel *chan, int engine)
+{
+	struct nv84_fence_priv *priv = nv_engine(chan->dev, engine);
+	struct nv84_fence_chan *fctx;
+	struct nouveau_gpuobj *obj;
+	int ret;
+
+	fctx = chan->engctx[engine] = kzalloc(sizeof(*fctx), GFP_KERNEL);
+	if (!fctx)
+		return -ENOMEM;
+
+	nouveau_fence_context_new(&fctx->base);
+
+	ret = nouveau_gpuobj_dma_new(chan, NV_CLASS_DMA_FROM_MEMORY,
+				     priv->mem->vinst, priv->mem->size,
+				     NV_MEM_ACCESS_RW,
+				     NV_MEM_TARGET_VRAM, &obj);
+	if (ret == 0) {
+		ret = nouveau_ramht_insert(chan, NvSema, obj);
+		nouveau_gpuobj_ref(NULL, &obj);
+		nv_wo32(priv->mem, chan->id * 16, 0x00000000);
+	}
+
+	if (ret)
+		nv84_fence_context_del(chan, engine);
+	return ret;
+}
+
+static int
+nv84_fence_fini(struct drm_device *dev, int engine, bool suspend)
+{
+	return 0;
+}
+
+static int
+nv84_fence_init(struct drm_device *dev, int engine)
+{
+	return 0;
+}
+
+static void
+nv84_fence_destroy(struct drm_device *dev, int engine)
+{
+	struct drm_nouveau_private *dev_priv = dev->dev_private;
+	struct nv84_fence_priv *priv = nv_engine(dev, engine);
+
+	nouveau_gpuobj_ref(NULL, &priv->mem);
+	dev_priv->eng[engine] = NULL;
+	kfree(priv);
+}
+
+int
+nv84_fence_create(struct drm_device *dev)
+{
+	struct nouveau_fifo_priv *pfifo = nv_engine(dev, NVOBJ_ENGINE_FIFO);
+	struct drm_nouveau_private *dev_priv = dev->dev_private;
+	struct nv84_fence_priv *priv;
+	int ret;
+
+	priv = kzalloc(sizeof(*priv), GFP_KERNEL);
+	if (!priv)
+		return -ENOMEM;
+
+	priv->base.engine.destroy = nv84_fence_destroy;
+	priv->base.engine.init = nv84_fence_init;
+	priv->base.engine.fini = nv84_fence_fini;
+	priv->base.engine.context_new = nv84_fence_context_new;
+	priv->base.engine.context_del = nv84_fence_context_del;
+	priv->base.emit = nv84_fence_emit;
+	priv->base.sync = nv84_fence_sync;
+	priv->base.read = nv84_fence_read;
+	dev_priv->eng[NVOBJ_ENGINE_FENCE] = &priv->base.engine;
+
+	ret = nouveau_gpuobj_new(dev, NULL, 16 * pfifo->channels,
+				 0x1000, 0, &priv->mem);
+	if (ret)
+		goto out;
+
+out:
+	if (ret)
+		nv84_fence_destroy(dev, NVOBJ_ENGINE_FENCE);
+	return ret;
+}
diff --git a/drivers/gpu/drm/nouveau/nv84_fifo.c b/drivers/gpu/drm/nouveau/nv84_fifo.c
new file mode 100644
index 00000000000..cc82d799fc3
--- /dev/null
+++ b/drivers/gpu/drm/nouveau/nv84_fifo.c
@@ -0,0 +1,241 @@
+/*
+ * Copyright (C) 2012 Ben Skeggs.
+ * 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 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.
+ *
+ */
+
+#include "drmP.h"
+#include "drm.h"
+#include "nouveau_drv.h"
+#include "nouveau_fifo.h"
+#include "nouveau_ramht.h"
+#include "nouveau_vm.h"
+
+struct nv84_fifo_priv {
+	struct nouveau_fifo_priv base;
+	struct nouveau_gpuobj *playlist[2];
+	int cur_playlist;
+};
+
+struct nv84_fifo_chan {
+	struct nouveau_fifo_chan base;
+	struct nouveau_gpuobj *ramfc;
+	struct nouveau_gpuobj *cache;
+};
+
+static int
+nv84_fifo_context_new(struct nouveau_channel *chan, int engine)
+{
+	struct nv84_fifo_priv *priv = nv_engine(chan->dev, engine);
+	struct nv84_fifo_chan *fctx;
+	struct drm_device *dev = chan->dev;
+	struct drm_nouveau_private *dev_priv = dev->dev_private;
+        u64 ib_offset = chan->pushbuf_base + chan->dma.ib_base * 4;
+	u64 instance;
+	unsigned long flags;
+	int ret;
+
+	fctx = chan->engctx[engine] = kzalloc(sizeof(*fctx), GFP_KERNEL);
+	if (!fctx)
+		return -ENOMEM;
+	atomic_inc(&chan->vm->engref[engine]);
+
+	chan->user = ioremap(pci_resource_start(dev->pdev, 0) +
+			     NV50_USER(chan->id), PAGE_SIZE);
+	if (!chan->user) {
+		ret = -ENOMEM;
+		goto error;
+	}
+
+	ret = nouveau_gpuobj_new(dev, chan, 256, 256, NVOBJ_FLAG_ZERO_ALLOC |
+				 NVOBJ_FLAG_ZERO_FREE, &fctx->ramfc);
+	if (ret)
+		goto error;
+
+	instance = fctx->ramfc->vinst >> 8;
+
+	ret = nouveau_gpuobj_new(dev, chan, 4096, 1024, 0, &fctx->cache);
+	if (ret)
+		goto error;
+
+	nv_wo32(fctx->ramfc, 0x3c, 0x403f6078);
+	nv_wo32(fctx->ramfc, 0x40, 0x00000000);
+	nv_wo32(fctx->ramfc, 0x44, 0x01003fff);
+	nv_wo32(fctx->ramfc, 0x48, chan->pushbuf->cinst >> 4);
+	nv_wo32(fctx->ramfc, 0x50, lower_32_bits(ib_offset));
+	nv_wo32(fctx->ramfc, 0x54, upper_32_bits(ib_offset) |
+				   drm_order(chan->dma.ib_max + 1) << 16);
+	nv_wo32(fctx->ramfc, 0x60, 0x7fffffff);
+	nv_wo32(fctx->ramfc, 0x78, 0x00000000);
+	nv_wo32(fctx->ramfc, 0x7c, 0x30000001);
+	nv_wo32(fctx->ramfc, 0x80, ((chan->ramht->bits - 9) << 27) |
+				   (4 << 24) /* SEARCH_FULL */ |
+				   (chan->ramht->gpuobj->cinst >> 4));
+	nv_wo32(fctx->ramfc, 0x88, fctx->cache->vinst >> 10);
+	nv_wo32(fctx->ramfc, 0x98, chan->ramin->vinst >> 12);
+
+	nv_wo32(chan->ramin, 0x00, chan->id);
+	nv_wo32(chan->ramin, 0x04, fctx->ramfc->vinst >> 8);
+
+	dev_priv->engine.instmem.flush(dev);
+
+	spin_lock_irqsave(&dev_priv->context_switch_lock, flags);
+	nv_wr32(dev, 0x002600 + (chan->id * 4), 0x80000000 | instance);
+	nv50_fifo_playlist_update(dev);
+	spin_unlock_irqrestore(&dev_priv->context_switch_lock, flags);
+
+error:
+	if (ret)
+		priv->base.base.context_del(chan, engine);
+	return ret;
+}
+
+static void
+nv84_fifo_context_del(struct nouveau_channel *chan, int engine)
+{
+	struct nv84_fifo_chan *fctx = chan->engctx[engine];
+	struct drm_device *dev = chan->dev;
+	struct drm_nouveau_private *dev_priv = dev->dev_private;
+	unsigned long flags;
+
+	/* remove channel from playlist, will context switch if active */
+	spin_lock_irqsave(&dev_priv->context_switch_lock, flags);
+	nv_mask(dev, 0x002600 + (chan->id * 4), 0x80000000, 0x00000000);
+	nv50_fifo_playlist_update(dev);
+
+	/* tell any engines on this channel to unload their contexts */
+	nv_wr32(dev, 0x0032fc, chan->ramin->vinst >> 12);
+	if (!nv_wait_ne(dev, 0x0032fc, 0xffffffff, 0xffffffff))
+		NV_INFO(dev, "PFIFO: channel %d unload timeout\n", chan->id);
+
+	nv_wr32(dev, 0x002600 + (chan->id * 4), 0x00000000);
+	spin_unlock_irqrestore(&dev_priv->context_switch_lock, flags);
+
+	/* clean up */
+	if (chan->user) {
+		iounmap(chan->user);
+		chan->user = NULL;
+	}
+
+	nouveau_gpuobj_ref(NULL, &fctx->ramfc);
+	nouveau_gpuobj_ref(NULL, &fctx->cache);
+
+	atomic_dec(&chan->vm->engref[engine]);
+	chan->engctx[engine] = NULL;
+	kfree(fctx);
+}
+
+static int
+nv84_fifo_init(struct drm_device *dev, int engine)
+{
+	struct drm_nouveau_private *dev_priv = dev->dev_private;
+	struct nv84_fifo_chan *fctx;
+	u32 instance;
+	int i;
+
+	nv_mask(dev, 0x000200, 0x00000100, 0x00000000);
+	nv_mask(dev, 0x000200, 0x00000100, 0x00000100);
+	nv_wr32(dev, 0x00250c, 0x6f3cfc34);
+	nv_wr32(dev, 0x002044, 0x01003fff);
+
+	nv_wr32(dev, 0x002100, 0xffffffff);
+	nv_wr32(dev, 0x002140, 0xffffffff);
+
+	for (i = 0; i < 128; i++) {
+		struct nouveau_channel *chan = dev_priv->channels.ptr[i];
+		if (chan && (fctx = chan->engctx[engine]))
+			instance = 0x80000000 | fctx->ramfc->vinst >> 8;
+		else
+			instance = 0x00000000;
+		nv_wr32(dev, 0x002600 + (i * 4), instance);
+	}
+
+	nv50_fifo_playlist_update(dev);
+
+	nv_wr32(dev, 0x003200, 1);
+	nv_wr32(dev, 0x003250, 1);
+	nv_wr32(dev, 0x002500, 1);
+	return 0;
+}
+
+static int
+nv84_fifo_fini(struct drm_device *dev, int engine, bool suspend)
+{
+	struct drm_nouveau_private *dev_priv = dev->dev_private;
+	struct nv84_fifo_priv *priv = nv_engine(dev, engine);
+	int i;
+
+	/* set playlist length to zero, fifo will unload context */
+	nv_wr32(dev, 0x0032ec, 0);
+
+	/* tell all connected engines to unload their contexts */
+	for (i = 0; i < priv->base.channels; i++) {
+		struct nouveau_channel *chan = dev_priv->channels.ptr[i];
+		if (chan)
+			nv_wr32(dev, 0x0032fc, chan->ramin->vinst >> 12);
+		if (!nv_wait_ne(dev, 0x0032fc, 0xffffffff, 0xffffffff)) {
+			NV_INFO(dev, "PFIFO: channel %d unload timeout\n", i);
+			return -EBUSY;
+		}
+	}
+
+	nv_wr32(dev, 0x002140, 0);
+	return 0;
+}
+
+int
+nv84_fifo_create(struct drm_device *dev)
+{
+	struct drm_nouveau_private *dev_priv = dev->dev_private;
+	struct nv84_fifo_priv *priv;
+	int ret;
+
+	priv = kzalloc(sizeof(*priv), GFP_KERNEL);
+	if (!priv)
+		return -ENOMEM;
+
+	priv->base.base.destroy = nv50_fifo_destroy;
+	priv->base.base.init = nv84_fifo_init;
+	priv->base.base.fini = nv84_fifo_fini;
+	priv->base.base.context_new = nv84_fifo_context_new;
+	priv->base.base.context_del = nv84_fifo_context_del;
+	priv->base.base.tlb_flush = nv50_fifo_tlb_flush;
+	priv->base.channels = 127;
+	dev_priv->eng[NVOBJ_ENGINE_FIFO] = &priv->base.base;
+
+	ret = nouveau_gpuobj_new(dev, NULL, priv->base.channels * 4, 0x1000,
+				 NVOBJ_FLAG_ZERO_ALLOC, &priv->playlist[0]);
+	if (ret)
+		goto error;
+
+	ret = nouveau_gpuobj_new(dev, NULL, priv->base.channels * 4, 0x1000,
+				 NVOBJ_FLAG_ZERO_ALLOC, &priv->playlist[1]);
+	if (ret)
+		goto error;
+
+	nouveau_irq_register(dev, 8, nv04_fifo_isr);
+error:
+	if (ret)
+		priv->base.base.destroy(dev, NVOBJ_ENGINE_FIFO);
+	return ret;
+}
diff --git a/drivers/gpu/drm/nouveau/nv98_crypt.c b/drivers/gpu/drm/nouveau/nv98_crypt.c
index db94ff0a9fa..e25e13fb894 100644
--- a/drivers/gpu/drm/nouveau/nv98_crypt.c
+++ b/drivers/gpu/drm/nouveau/nv98_crypt.c
@@ -23,21 +23,93 @@
  */
 
 #include "drmP.h"
+
 #include "nouveau_drv.h"
 #include "nouveau_util.h"
 #include "nouveau_vm.h"
 #include "nouveau_ramht.h"
 
-struct nv98_crypt_engine {
+#include "nv98_crypt.fuc.h"
+
+struct nv98_crypt_priv {
 	struct nouveau_exec_engine base;
 };
 
+struct nv98_crypt_chan {
+	struct nouveau_gpuobj *mem;
+};
+
 static int
-nv98_crypt_fini(struct drm_device *dev, int engine, bool suspend)
+nv98_crypt_context_new(struct nouveau_channel *chan, int engine)
+{
+	struct drm_device *dev = chan->dev;
+	struct drm_nouveau_private *dev_priv = dev->dev_private;
+	struct nv98_crypt_priv *priv = nv_engine(dev, engine);
+	struct nv98_crypt_chan *cctx;
+	int ret;
+
+	cctx = chan->engctx[engine] = kzalloc(sizeof(*cctx), GFP_KERNEL);
+	if (!cctx)
+		return -ENOMEM;
+
+	atomic_inc(&chan->vm->engref[engine]);
+
+	ret = nouveau_gpuobj_new(dev, chan, 256, 0, NVOBJ_FLAG_ZERO_ALLOC |
+				 NVOBJ_FLAG_ZERO_FREE, &cctx->mem);
+	if (ret)
+		goto error;
+
+	nv_wo32(chan->ramin, 0xa0, 0x00190000);
+	nv_wo32(chan->ramin, 0xa4, cctx->mem->vinst + cctx->mem->size - 1);
+	nv_wo32(chan->ramin, 0xa8, cctx->mem->vinst);
+	nv_wo32(chan->ramin, 0xac, 0x00000000);
+	nv_wo32(chan->ramin, 0xb0, 0x00000000);
+	nv_wo32(chan->ramin, 0xb4, 0x00000000);
+	dev_priv->engine.instmem.flush(dev);
+
+error:
+	if (ret)
+		priv->base.context_del(chan, engine);
+	return ret;
+}
+
+static void
+nv98_crypt_context_del(struct nouveau_channel *chan, int engine)
+{
+	struct nv98_crypt_chan *cctx = chan->engctx[engine];
+	int i;
+
+	for (i = 0xa0; i < 0xb4; i += 4)
+		nv_wo32(chan->ramin, i, 0x00000000);
+
+	nouveau_gpuobj_ref(NULL, &cctx->mem);
+
+	atomic_dec(&chan->vm->engref[engine]);
+	chan->engctx[engine] = NULL;
+	kfree(cctx);
+}
+
+static int
+nv98_crypt_object_new(struct nouveau_channel *chan, int engine,
+		     u32 handle, u16 class)
 {
-	if (!(nv_rd32(dev, 0x000200) & 0x00004000))
-		return 0;
+	struct nv98_crypt_chan *cctx = chan->engctx[engine];
+
+	/* fuc engine doesn't need an object, our ramht code does.. */
+	cctx->mem->engine = 5;
+	cctx->mem->class  = class;
+	return nouveau_ramht_insert(chan, handle, cctx->mem);
+}
 
+static void
+nv98_crypt_tlb_flush(struct drm_device *dev, int engine)
+{
+	nv50_vm_flush_engine(dev, 0x0a);
+}
+
+static int
+nv98_crypt_fini(struct drm_device *dev, int engine, bool suspend)
+{
 	nv_mask(dev, 0x000200, 0x00004000, 0x00000000);
 	return 0;
 }
@@ -45,34 +117,100 @@ nv98_crypt_fini(struct drm_device *dev, int engine, bool suspend)
 static int
 nv98_crypt_init(struct drm_device *dev, int engine)
 {
+	int i;
+
+	/* reset! */
 	nv_mask(dev, 0x000200, 0x00004000, 0x00000000);
 	nv_mask(dev, 0x000200, 0x00004000, 0x00004000);
+
+	/* wait for exit interrupt to signal */
+	nv_wait(dev, 0x087008, 0x00000010, 0x00000010);
+	nv_wr32(dev, 0x087004, 0x00000010);
+
+	/* upload microcode code and data segments */
+	nv_wr32(dev, 0x087ff8, 0x00100000);
+	for (i = 0; i < ARRAY_SIZE(nv98_pcrypt_code); i++)
+		nv_wr32(dev, 0x087ff4, nv98_pcrypt_code[i]);
+
+	nv_wr32(dev, 0x087ff8, 0x00000000);
+	for (i = 0; i < ARRAY_SIZE(nv98_pcrypt_data); i++)
+		nv_wr32(dev, 0x087ff4, nv98_pcrypt_data[i]);
+
+	/* start it running */
+	nv_wr32(dev, 0x08710c, 0x00000000);
+	nv_wr32(dev, 0x087104, 0x00000000); /* ENTRY */
+	nv_wr32(dev, 0x087100, 0x00000002); /* TRIGGER */
 	return 0;
 }
 
+static struct nouveau_enum nv98_crypt_isr_error_name[] = {
+	{ 0x0000, "ILLEGAL_MTHD" },
+	{ 0x0001, "INVALID_BITFIELD" },
+	{ 0x0002, "INVALID_ENUM" },
+	{ 0x0003, "QUERY" },
+	{}
+};
+
+static void
+nv98_crypt_isr(struct drm_device *dev)
+{
+	u32 disp = nv_rd32(dev, 0x08701c);
+	u32 stat = nv_rd32(dev, 0x087008) & disp & ~(disp >> 16);
+	u32 inst = nv_rd32(dev, 0x087050) & 0x3fffffff;
+	u32 ssta = nv_rd32(dev, 0x087040) & 0x0000ffff;
+	u32 addr = nv_rd32(dev, 0x087040) >> 16;
+	u32 mthd = (addr & 0x07ff) << 2;
+	u32 subc = (addr & 0x3800) >> 11;
+	u32 data = nv_rd32(dev, 0x087044);
+	int chid = nv50_graph_isr_chid(dev, inst);
+
+	if (stat & 0x00000040) {
+		NV_INFO(dev, "PCRYPT: DISPATCH_ERROR [");
+		nouveau_enum_print(nv98_crypt_isr_error_name, ssta);
+		printk("] ch %d [0x%08x] subc %d mthd 0x%04x data 0x%08x\n",
+			chid, inst, subc, mthd, data);
+		nv_wr32(dev, 0x087004, 0x00000040);
+		stat &= ~0x00000040;
+	}
+
+	if (stat) {
+		NV_INFO(dev, "PCRYPT: unhandled intr 0x%08x\n", stat);
+		nv_wr32(dev, 0x087004, stat);
+	}
+
+	nv50_fb_vm_trap(dev, 1);
+}
+
 static void
 nv98_crypt_destroy(struct drm_device *dev, int engine)
 {
-	struct nv98_crypt_engine *pcrypt = nv_engine(dev, engine);
+	struct nv98_crypt_priv *priv = nv_engine(dev, engine);
 
+	nouveau_irq_unregister(dev, 14);
 	NVOBJ_ENGINE_DEL(dev, CRYPT);
-
-	kfree(pcrypt);
+	kfree(priv);
 }
 
 int
 nv98_crypt_create(struct drm_device *dev)
 {
-	struct nv98_crypt_engine *pcrypt;
+	struct nv98_crypt_priv *priv;
 
-	pcrypt = kzalloc(sizeof(*pcrypt), GFP_KERNEL);
-	if (!pcrypt)
+	priv = kzalloc(sizeof(*priv), GFP_KERNEL);
+	if (!priv)
 		return -ENOMEM;
 
-	pcrypt->base.destroy = nv98_crypt_destroy;
-	pcrypt->base.init = nv98_crypt_init;
-	pcrypt->base.fini = nv98_crypt_fini;
+	priv->base.destroy = nv98_crypt_destroy;
+	priv->base.init = nv98_crypt_init;
+	priv->base.fini = nv98_crypt_fini;
+	priv->base.context_new = nv98_crypt_context_new;
+	priv->base.context_del = nv98_crypt_context_del;
+	priv->base.object_new = nv98_crypt_object_new;
+	priv->base.tlb_flush = nv98_crypt_tlb_flush;
+
+	nouveau_irq_register(dev, 14, nv98_crypt_isr);
 
-	NVOBJ_ENGINE_ADD(dev, CRYPT, &pcrypt->base);
+	NVOBJ_ENGINE_ADD(dev, CRYPT, &priv->base);
+	NVOBJ_CLASS(dev, 0x88b4, CRYPT);
 	return 0;
 }
diff --git a/drivers/gpu/drm/nouveau/nv98_crypt.fuc b/drivers/gpu/drm/nouveau/nv98_crypt.fuc
new file mode 100644
index 00000000000..7393813044d
--- /dev/null
+++ b/drivers/gpu/drm/nouveau/nv98_crypt.fuc
@@ -0,0 +1,698 @@
+/*
+ *  fuc microcode for nv98 pcrypt engine
+ *  Copyright (C) 2010  Marcin Kościelnicki
+ *
+ *  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 St, Fifth Floor, Boston, MA  02110-1301  USA
+ */
+
+.section #nv98_pcrypt_data
+
+ctx_dma:
+ctx_dma_query:		.b32 0
+ctx_dma_src:		.b32 0
+ctx_dma_dst:		.b32 0
+.equ #dma_count 3
+ctx_query_address_high:	.b32 0
+ctx_query_address_low:	.b32 0
+ctx_query_counter:	.b32 0
+ctx_cond_address_high:	.b32 0
+ctx_cond_address_low:	.b32 0
+ctx_cond_off:		.b32 0
+ctx_src_address_high:	.b32 0
+ctx_src_address_low:	.b32 0
+ctx_dst_address_high:	.b32 0
+ctx_dst_address_low:	.b32 0
+ctx_mode:		.b32 0
+.align 16
+ctx_key:		.skip 16
+ctx_iv:			.skip 16
+
+.align 0x80
+swap:
+.skip 32
+
+.align 8
+common_cmd_dtable:
+.b32 #ctx_query_address_high + 0x20000 ~0xff
+.b32 #ctx_query_address_low + 0x20000 ~0xfffffff0
+.b32 #ctx_query_counter + 0x20000 ~0xffffffff
+.b32 #cmd_query_get + 0x00000 ~1
+.b32 #ctx_cond_address_high + 0x20000 ~0xff
+.b32 #ctx_cond_address_low + 0x20000 ~0xfffffff0
+.b32 #cmd_cond_mode + 0x00000 ~7
+.b32 #cmd_wrcache_flush + 0x00000 ~0
+.equ #common_cmd_max 0x88
+
+
+.align 8
+engine_cmd_dtable:
+.b32 #ctx_key + 0x0 + 0x20000 ~0xffffffff
+.b32 #ctx_key + 0x4 + 0x20000 ~0xffffffff
+.b32 #ctx_key + 0x8 + 0x20000 ~0xffffffff
+.b32 #ctx_key + 0xc + 0x20000 ~0xffffffff
+.b32 #ctx_iv + 0x0 + 0x20000 ~0xffffffff
+.b32 #ctx_iv + 0x4 + 0x20000 ~0xffffffff
+.b32 #ctx_iv + 0x8 + 0x20000 ~0xffffffff
+.b32 #ctx_iv + 0xc + 0x20000 ~0xffffffff
+.b32 #ctx_src_address_high + 0x20000 ~0xff
+.b32 #ctx_src_address_low + 0x20000 ~0xfffffff0
+.b32 #ctx_dst_address_high + 0x20000 ~0xff
+.b32 #ctx_dst_address_low + 0x20000 ~0xfffffff0
+.b32 #crypt_cmd_mode + 0x00000 ~0xf
+.b32 #crypt_cmd_length + 0x10000 ~0x0ffffff0
+.equ #engine_cmd_max 0xce
+
+.align 4
+crypt_dtable:
+.b16 #crypt_copy_prep #crypt_do_inout
+.b16 #crypt_store_prep #crypt_do_out
+.b16 #crypt_ecb_e_prep #crypt_do_inout
+.b16 #crypt_ecb_d_prep #crypt_do_inout
+.b16 #crypt_cbc_e_prep #crypt_do_inout
+.b16 #crypt_cbc_d_prep #crypt_do_inout
+.b16 #crypt_pcbc_e_prep #crypt_do_inout
+.b16 #crypt_pcbc_d_prep #crypt_do_inout
+.b16 #crypt_cfb_e_prep #crypt_do_inout
+.b16 #crypt_cfb_d_prep #crypt_do_inout
+.b16 #crypt_ofb_prep #crypt_do_inout
+.b16 #crypt_ctr_prep #crypt_do_inout
+.b16 #crypt_cbc_mac_prep #crypt_do_in
+.b16 #crypt_cmac_finish_complete_prep #crypt_do_in
+.b16 #crypt_cmac_finish_partial_prep #crypt_do_in
+
+.align 0x100
+
+.section #nv98_pcrypt_code
+
+	// $r0 is always set to 0 in our code - this allows some space savings.
+	clear b32 $r0
+
+	// set up the interrupt handler
+	mov $r1 #ih
+	mov $iv0 $r1
+
+	// init stack pointer
+	mov $sp $r0
+
+	// set interrupt dispatch - route timer, fifo, ctxswitch to i0, others to host
+	movw $r1 0xfff0
+	sethi $r1 0
+	mov $r2 0x400
+	iowr I[$r2 + 0x300] $r1
+
+	// enable the interrupts
+	or $r1 0xc
+	iowr I[$r2] $r1
+
+	// enable fifo access and context switching
+	mov $r1 3
+	mov $r2 0x1200
+	iowr I[$r2] $r1
+
+	// enable i0 delivery
+	bset $flags ie0
+
+	// sleep forver, waking only for interrupts.
+	bset $flags $p0
+	spin:
+	sleep $p0
+	bra #spin
+
+// i0 handler
+ih:
+	// see which interrupts we got
+	iord $r1 I[$r0 + 0x200]
+
+	and $r2 $r1 0x8
+	cmpu b32 $r2 0
+	bra e #noctx
+
+		// context switch... prepare the regs for xfer
+		mov $r2 0x7700
+		mov $xtargets $r2
+		mov $xdbase $r0
+		// 128-byte context.
+		mov $r2 0
+		sethi $r2 0x50000
+
+		// read current channel
+		mov $r3 0x1400
+		iord $r4 I[$r3]
+		// if bit 30 set, it's active, so we have to unload it first.
+		shl b32 $r5 $r4 1
+		cmps b32 $r5 0
+		bra nc #ctxload
+
+			// unload the current channel - save the context
+			xdst $r0 $r2
+			xdwait
+			// and clear bit 30, then write back
+			bclr $r4 0x1e
+			iowr I[$r3] $r4
+			// tell PFIFO we unloaded
+			mov $r4 1
+			iowr I[$r3 + 0x200] $r4
+
+		bra #noctx
+
+		ctxload:
+			// no channel loaded - perhaps we're requested to load one
+			iord $r4 I[$r3 + 0x100]
+			shl b32 $r15 $r4 1
+			cmps b32 $r15 0
+			// if bit 30 of next channel not set, probably PFIFO is just
+			// killing a context. do a faux load, without the active bit.
+			bra nc #dummyload
+
+				// ok, do a real context load.
+				xdld $r0 $r2
+				xdwait
+				mov $r5 #ctx_dma
+				mov $r6 #dma_count - 1
+				ctxload_dma_loop:
+					ld b32 $r7 D[$r5 + $r6 * 4]
+					add b32 $r8 $r6 0x180
+					shl b32 $r8 8
+					iowr I[$r8] $r7
+					sub b32 $r6 1
+				bra nc #ctxload_dma_loop
+
+			dummyload:
+			// tell PFIFO we're done
+			mov $r5 2
+			iowr I[$r3 + 0x200] $r5
+
+	noctx:
+	and $r2 $r1 0x4
+	cmpu b32 $r2 0
+	bra e #nocmd
+
+		// incoming fifo command.
+		mov $r3 0x1900
+		iord $r2 I[$r3 + 0x100]
+		iord $r3 I[$r3]
+		// extract the method
+		and $r4 $r2 0x7ff
+		// shift the addr to proper position if we need to interrupt later
+		shl b32 $r2 0x10
+
+		// mthd 0 and 0x100 [NAME, NOP]: ignore
+		and $r5 $r4 0x7bf
+		cmpu b32 $r5 0
+		bra e #cmddone
+
+		mov $r5 #engine_cmd_dtable - 0xc0 * 8
+		mov $r6 #engine_cmd_max
+		cmpu b32 $r4 0xc0
+		bra nc #dtable_cmd
+		mov $r5 #common_cmd_dtable - 0x80 * 8
+		mov $r6 #common_cmd_max
+		cmpu b32 $r4 0x80
+		bra nc #dtable_cmd
+		cmpu b32 $r4 0x60
+		bra nc #dma_cmd
+		cmpu b32 $r4 0x50
+		bra ne #illegal_mthd
+
+			// mthd 0x140: PM_TRIGGER
+			mov $r2 0x2200
+			clear b32 $r3
+			sethi $r3 0x20000
+			iowr I[$r2] $r3
+			bra #cmddone
+
+		dma_cmd:
+			// mthd 0x180...: DMA_*
+			cmpu b32 $r4 0x60+#dma_count
+			bra nc #illegal_mthd
+			shl b32 $r5 $r4 2
+			add b32 $r5 (#ctx_dma - 0x60 * 4) & 0xffff
+			bset $r3 0x1e
+			st b32 D[$r5] $r3
+			add b32 $r4 0x180 - 0x60
+			shl b32 $r4 8
+			iowr I[$r4] $r3
+			bra #cmddone
+
+		dtable_cmd:
+			cmpu b32 $r4 $r6
+			bra nc #illegal_mthd
+			shl b32 $r4 3
+			add b32 $r4 $r5
+			ld b32 $r5 D[$r4 + 4]
+			and $r5 $r3
+			cmpu b32 $r5 0
+			bra ne #invalid_bitfield
+			ld b16 $r5 D[$r4]
+			ld b16 $r6 D[$r4 + 2]
+			cmpu b32 $r6 2
+			bra e #cmd_setctx
+			ld b32 $r7 D[$r0 + #ctx_cond_off]
+			and $r6 $r7
+			cmpu b32 $r6 1
+			bra e #cmddone
+			call $r5
+			bra $p1 #dispatch_error
+			bra #cmddone
+
+		cmd_setctx:
+			st b32 D[$r5] $r3
+			bra #cmddone
+
+
+		invalid_bitfield:
+			or $r2 1
+		dispatch_error:
+		illegal_mthd:
+			mov $r4 0x1000
+			iowr I[$r4] $r2
+			iowr I[$r4 + 0x100] $r3
+			mov $r4 0x40
+			iowr I[$r0] $r4
+
+			im_loop:
+				iord $r4 I[$r0 + 0x200]
+				and $r4 0x40
+				cmpu b32 $r4 0
+			bra ne #im_loop
+
+		cmddone:
+		// remove the command from FIFO
+		mov $r3 0x1d00
+		mov $r4 1
+		iowr I[$r3] $r4
+
+	nocmd:
+	// ack the processed interrupts
+	and $r1 $r1 0xc
+	iowr I[$r0 + 0x100] $r1
+iret
+
+cmd_query_get:
+	// if bit 0 of param set, trigger interrupt afterwards.
+	setp $p1 $r3
+	or $r2 3
+
+	// read PTIMER, beware of races...
+	mov $r4 0xb00
+	ptimer_retry:
+		iord $r6 I[$r4 + 0x100]
+		iord $r5 I[$r4]
+		iord $r7 I[$r4 + 0x100]
+		cmpu b32 $r6 $r7
+	bra ne #ptimer_retry
+
+	// prepare the query structure
+	ld b32 $r4 D[$r0 + #ctx_query_counter]
+	st b32 D[$r0 + #swap + 0x0] $r4
+	st b32 D[$r0 + #swap + 0x4] $r0
+	st b32 D[$r0 + #swap + 0x8] $r5
+	st b32 D[$r0 + #swap + 0xc] $r6
+
+	// will use target 0, DMA_QUERY.
+	mov $xtargets $r0
+
+	ld b32 $r4 D[$r0 + #ctx_query_address_high]
+	shl b32 $r4 0x18
+	mov $xdbase $r4
+
+	ld b32 $r4 D[$r0 + #ctx_query_address_low]
+	mov $r5 #swap
+	sethi $r5 0x20000
+	xdst $r4 $r5
+	xdwait
+
+	ret
+
+cmd_cond_mode:
+	// if >= 5, INVALID_ENUM
+	bset $flags $p1
+	or $r2 2
+	cmpu b32 $r3 5
+	bra nc #return
+
+	// otherwise, no error.
+	bclr $flags $p1
+
+	// if < 2, no QUERY object is involved
+	cmpu b32 $r3 2
+	bra nc #cmd_cond_mode_queryful
+
+		xor $r3 1
+		st b32 D[$r0 + #ctx_cond_off] $r3
+	return:
+		ret
+
+	cmd_cond_mode_queryful:
+	// ok, will need to pull a QUERY object, prepare offsets
+	ld b32 $r4 D[$r0 + #ctx_cond_address_high]
+	ld b32 $r5 D[$r0 + #ctx_cond_address_low]
+	and $r6 $r5 0xff
+	shr b32 $r5 8
+	shl b32 $r4 0x18
+	or $r4 $r5
+	mov $xdbase $r4
+	mov $xtargets $r0
+
+	// pull the first one
+	mov $r5 #swap
+	sethi $r5 0x20000
+	xdld $r6 $r5
+
+	// if == 2, only a single QUERY is involved...
+	cmpu b32 $r3 2
+	bra ne #cmd_cond_mode_double
+
+		xdwait
+		ld b32 $r4 D[$r0 + #swap + 4]
+		cmpu b32 $r4 0
+		xbit $r4 $flags z
+		st b32 D[$r0 + #ctx_cond_off] $r4
+		ret
+
+	// ok, we'll need to pull second one too
+	cmd_cond_mode_double:
+	add b32 $r6 0x10
+	add b32 $r5 0x10
+	xdld $r6 $r5
+	xdwait
+
+	// compare COUNTERs
+	ld b32 $r5 D[$r0 + #swap + 0x00]
+	ld b32 $r6 D[$r0 + #swap + 0x10]
+	cmpu b32 $r5 $r6
+	xbit $r4 $flags z
+
+	// compare RESen
+	ld b32 $r5 D[$r0 + #swap + 0x04]
+	ld b32 $r6 D[$r0 + #swap + 0x14]
+	cmpu b32 $r5 $r6
+	xbit $r5 $flags z
+	and $r4 $r5
+
+	// and negate or not, depending on mode
+	cmpu b32 $r3 3
+	xbit $r5 $flags z
+	xor $r4 $r5
+	st b32 D[$r0 + #ctx_cond_off] $r4
+	ret
+
+cmd_wrcache_flush:
+	bclr $flags $p1
+	mov $r2 0x2200
+	clear b32 $r3
+	sethi $r3 0x10000
+	iowr I[$r2] $r3
+	ret
+
+crypt_cmd_mode:
+	// if >= 0xf, INVALID_ENUM
+	bset $flags $p1
+	or $r2 2
+	cmpu b32 $r3 0xf
+	bra nc #crypt_cmd_mode_return
+
+		bclr $flags $p1
+		st b32 D[$r0 + #ctx_mode] $r3
+
+	crypt_cmd_mode_return:
+	ret
+
+crypt_cmd_length:
+	// nop if length == 0
+	cmpu b32 $r3 0
+	bra e #crypt_cmd_mode_return
+
+	// init key, IV
+	cxset 3
+	mov $r4 #ctx_key
+	sethi $r4 0x70000
+	xdst $r0 $r4
+	mov $r4 #ctx_iv
+	sethi $r4 0x60000
+	xdst $r0 $r4
+	xdwait
+	ckeyreg $c7
+
+	// prepare the targets
+	mov $r4 0x2100
+	mov $xtargets $r4
+
+	// prepare src address
+	ld b32 $r4 D[$r0 + #ctx_src_address_high]
+	ld b32 $r5 D[$r0 + #ctx_src_address_low]
+	shr b32 $r8 $r5 8
+	shl b32 $r4 0x18
+	or $r4 $r8
+	and $r5 $r5 0xff
+
+	// prepare dst address
+	ld b32 $r6 D[$r0 + #ctx_dst_address_high]
+	ld b32 $r7 D[$r0 + #ctx_dst_address_low]
+	shr b32 $r8 $r7 8
+	shl b32 $r6 0x18
+	or $r6 $r8
+	and $r7 $r7 0xff
+
+	// find the proper prep & do functions
+	ld b32 $r8 D[$r0 + #ctx_mode]
+	shl b32 $r8 2
+
+	// run prep
+	ld b16 $r9 D[$r8 + #crypt_dtable]
+	call $r9
+
+	// do it
+	ld b16 $r9 D[$r8 + #crypt_dtable + 2]
+	call $r9
+	cxset 1
+	xdwait
+	cxset 0x61
+	xdwait
+	xdwait
+
+	// update src address
+	shr b32 $r8 $r4 0x18
+	shl b32 $r9 $r4 8
+	add b32 $r9 $r5
+	adc b32 $r8 0
+	st b32 D[$r0 + #ctx_src_address_high] $r8
+	st b32 D[$r0 + #ctx_src_address_low] $r9
+
+	// update dst address
+	shr b32 $r8 $r6 0x18
+	shl b32 $r9 $r6 8
+	add b32 $r9 $r7
+	adc b32 $r8 0
+	st b32 D[$r0 + #ctx_dst_address_high] $r8
+	st b32 D[$r0 + #ctx_dst_address_low] $r9
+
+	// pull updated IV
+	cxset 2
+	mov $r4 #ctx_iv
+	sethi $r4 0x60000
+	xdld $r0 $r4
+	xdwait
+
+	ret
+
+
+crypt_copy_prep:
+	cs0begin 2
+		cxsin $c0
+		cxsout $c0
+	ret
+
+crypt_store_prep:
+	cs0begin 1
+		cxsout $c6
+	ret
+
+crypt_ecb_e_prep:
+	cs0begin 3
+		cxsin $c0
+		cenc $c0 $c0
+		cxsout $c0
+	ret
+
+crypt_ecb_d_prep:
+	ckexp $c7 $c7
+	cs0begin 3
+		cxsin $c0
+		cdec $c0 $c0
+		cxsout $c0
+	ret
+
+crypt_cbc_e_prep:
+	cs0begin 4
+		cxsin $c0
+		cxor $c6 $c0
+		cenc $c6 $c6
+		cxsout $c6
+	ret
+
+crypt_cbc_d_prep:
+	ckexp $c7 $c7
+	cs0begin 5
+		cmov $c2 $c6
+		cxsin $c6
+		cdec $c0 $c6
+		cxor $c0 $c2
+		cxsout $c0
+	ret
+
+crypt_pcbc_e_prep:
+	cs0begin 5
+		cxsin $c0
+		cxor $c6 $c0
+		cenc $c6 $c6
+		cxsout $c6
+		cxor $c6 $c0
+	ret
+
+crypt_pcbc_d_prep:
+	ckexp $c7 $c7
+	cs0begin 5
+		cxsin $c0
+		cdec $c1 $c0
+		cxor $c6 $c1
+		cxsout $c6
+		cxor $c6 $c0
+	ret
+
+crypt_cfb_e_prep:
+	cs0begin 4
+		cenc $c6 $c6
+		cxsin $c0
+		cxor $c6 $c0
+		cxsout $c6
+	ret
+
+crypt_cfb_d_prep:
+	cs0begin 4
+		cenc $c0 $c6
+		cxsin $c6
+		cxor $c0 $c6
+		cxsout $c0
+	ret
+
+crypt_ofb_prep:
+	cs0begin 4
+		cenc $c6 $c6
+		cxsin $c0
+		cxor $c0 $c6
+		cxsout $c0
+	ret
+
+crypt_ctr_prep:
+	cs0begin 5
+		cenc $c1 $c6
+		cadd $c6 1
+		cxsin $c0
+		cxor $c0 $c1
+		cxsout $c0
+	ret
+
+crypt_cbc_mac_prep:
+	cs0begin 3
+		cxsin $c0
+		cxor $c6 $c0
+		cenc $c6 $c6
+	ret
+
+crypt_cmac_finish_complete_prep:
+	cs0begin 7
+		cxsin $c0
+		cxor $c6 $c0
+		cxor $c0 $c0
+		cenc $c0 $c0
+		cprecmac $c0 $c0
+		cxor $c6 $c0
+		cenc $c6 $c6
+	ret
+
+crypt_cmac_finish_partial_prep:
+	cs0begin 8
+		cxsin $c0
+		cxor $c6 $c0
+		cxor $c0 $c0
+		cenc $c0 $c0
+		cprecmac $c0 $c0
+		cprecmac $c0 $c0
+		cxor $c6 $c0
+		cenc $c6 $c6
+	ret
+
+// TODO
+crypt_do_in:
+	add b32 $r3 $r5
+	mov $xdbase $r4
+	mov $r9 #swap
+	sethi $r9 0x20000
+	crypt_do_in_loop:
+		xdld $r5 $r9
+		xdwait
+		cxset 0x22
+		xdst $r0 $r9
+		cs0exec 1
+		xdwait
+		add b32 $r5 0x10
+		cmpu b32 $r5 $r3
+	bra ne #crypt_do_in_loop
+	cxset 1
+	xdwait
+	ret
+
+crypt_do_out:
+	add b32 $r3 $r7
+	mov $xdbase $r6
+	mov $r9 #swap
+	sethi $r9 0x20000
+	crypt_do_out_loop:
+		cs0exec 1
+		cxset 0x61
+		xdld $r7 $r9
+		xdst $r7 $r9
+		cxset 1
+		xdwait
+		add b32 $r7 0x10
+		cmpu b32 $r7 $r3
+	bra ne #crypt_do_out_loop
+	ret
+
+crypt_do_inout:
+	add b32 $r3 $r5
+	mov $r9 #swap
+	sethi $r9 0x20000
+	crypt_do_inout_loop:
+		mov $xdbase $r4
+		xdld $r5 $r9
+		xdwait
+		cxset 0x21
+		xdst $r0 $r9
+		cs0exec 1
+		cxset 0x61
+		mov $xdbase $r6
+		xdld $r7 $r9
+		xdst $r7 $r9
+		cxset 1
+		xdwait
+		add b32 $r5 0x10
+		add b32 $r7 0x10
+		cmpu b32 $r5 $r3
+	bra ne #crypt_do_inout_loop
+	ret
+
+.align 0x100
diff --git a/drivers/gpu/drm/nouveau/nv98_crypt.fuc.h b/drivers/gpu/drm/nouveau/nv98_crypt.fuc.h
new file mode 100644
index 00000000000..38676c74e6e
--- /dev/null
+++ b/drivers/gpu/drm/nouveau/nv98_crypt.fuc.h
@@ -0,0 +1,584 @@
+uint32_t nv98_pcrypt_data[] = {
+/* 0x0000: ctx_dma */
+/* 0x0000: ctx_dma_query */
+	0x00000000,
+/* 0x0004: ctx_dma_src */
+	0x00000000,
+/* 0x0008: ctx_dma_dst */
+	0x00000000,
+/* 0x000c: ctx_query_address_high */
+	0x00000000,
+/* 0x0010: ctx_query_address_low */
+	0x00000000,
+/* 0x0014: ctx_query_counter */
+	0x00000000,
+/* 0x0018: ctx_cond_address_high */
+	0x00000000,
+/* 0x001c: ctx_cond_address_low */
+	0x00000000,
+/* 0x0020: ctx_cond_off */
+	0x00000000,
+/* 0x0024: ctx_src_address_high */
+	0x00000000,
+/* 0x0028: ctx_src_address_low */
+	0x00000000,
+/* 0x002c: ctx_dst_address_high */
+	0x00000000,
+/* 0x0030: ctx_dst_address_low */
+	0x00000000,
+/* 0x0034: ctx_mode */
+	0x00000000,
+	0x00000000,
+	0x00000000,
+/* 0x0040: ctx_key */
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+/* 0x0050: ctx_iv */
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+/* 0x0080: swap */
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+/* 0x00a0: common_cmd_dtable */
+	0x0002000c,
+	0xffffff00,
+	0x00020010,
+	0x0000000f,
+	0x00020014,
+	0x00000000,
+	0x00000192,
+	0xfffffffe,
+	0x00020018,
+	0xffffff00,
+	0x0002001c,
+	0x0000000f,
+	0x000001d7,
+	0xfffffff8,
+	0x00000260,
+	0xffffffff,
+/* 0x00e0: engine_cmd_dtable */
+	0x00020040,
+	0x00000000,
+	0x00020044,
+	0x00000000,
+	0x00020048,
+	0x00000000,
+	0x0002004c,
+	0x00000000,
+	0x00020050,
+	0x00000000,
+	0x00020054,
+	0x00000000,
+	0x00020058,
+	0x00000000,
+	0x0002005c,
+	0x00000000,
+	0x00020024,
+	0xffffff00,
+	0x00020028,
+	0x0000000f,
+	0x0002002c,
+	0xffffff00,
+	0x00020030,
+	0x0000000f,
+	0x00000271,
+	0xfffffff0,
+	0x00010285,
+	0xf000000f,
+/* 0x0150: crypt_dtable */
+	0x04db0321,
+	0x04b1032f,
+	0x04db0339,
+	0x04db034b,
+	0x04db0361,
+	0x04db0377,
+	0x04db0395,
+	0x04db03af,
+	0x04db03cd,
+	0x04db03e3,
+	0x04db03f9,
+	0x04db040f,
+	0x04830429,
+	0x0483043b,
+	0x0483045d,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+};
+
+uint32_t nv98_pcrypt_code[] = {
+	0x17f004bd,
+	0x0010fe35,
+	0xf10004fe,
+	0xf0fff017,
+	0x27f10013,
+	0x21d00400,
+	0x0c15f0c0,
+	0xf00021d0,
+	0x27f10317,
+	0x21d01200,
+	0x1031f400,
+/* 0x002f: spin */
+	0xf40031f4,
+	0x0ef40028,
+/* 0x0035: ih */
+	0x8001cffd,
+	0xb00812c4,
+	0x0bf40024,
+	0x0027f167,
+	0x002bfe77,
+	0xf00007fe,
+	0x23f00027,
+	0x0037f105,
+	0x0034cf14,
+	0xb0014594,
+	0x18f40055,
+	0x0602fa17,
+	0x4af003f8,
+	0x0034d01e,
+	0xd00147f0,
+	0x0ef48034,
+/* 0x0075: ctxload */
+	0x4034cf33,
+	0xb0014f94,
+	0x18f400f5,
+	0x0502fa21,
+	0x57f003f8,
+	0x0267f000,
+/* 0x008c: ctxload_dma_loop */
+	0xa07856bc,
+	0xb6018068,
+	0x87d00884,
+	0x0162b600,
+/* 0x009f: dummyload */
+	0xf0f018f4,
+	0x35d00257,
+/* 0x00a5: noctx */
+	0x0412c480,
+	0xf50024b0,
+	0xf100df0b,
+	0xcf190037,
+	0x33cf4032,
+	0xff24e400,
+	0x1024b607,
+	0x07bf45e4,
+	0xf50054b0,
+	0xf100b90b,
+	0xf1fae057,
+	0xb000ce67,
+	0x18f4c044,
+	0xa057f14d,
+	0x8867f1fc,
+	0x8044b000,
+	0xb03f18f4,
+	0x18f46044,
+	0x5044b019,
+	0xf1741bf4,
+	0xbd220027,
+	0x0233f034,
+	0xf50023d0,
+/* 0x0103: dma_cmd */
+	0xb000810e,
+	0x18f46344,
+	0x0245945e,
+	0xfe8050b7,
+	0x801e39f0,
+	0x40b70053,
+	0x44b60120,
+	0x0043d008,
+/* 0x0123: dtable_cmd */
+	0xb8600ef4,
+	0x18f40446,
+	0x0344b63e,
+	0x980045bb,
+	0x53fd0145,
+	0x0054b004,
+	0x58291bf4,
+	0x46580045,
+	0x0264b001,
+	0x98170bf4,
+	0x67fd0807,
+	0x0164b004,
+	0xf9300bf4,
+	0x0f01f455,
+/* 0x015b: cmd_setctx */
+	0x80280ef4,
+	0x0ef40053,
+/* 0x0161: invalid_bitfield */
+	0x0125f022,
+/* 0x0164: dispatch_error */
+/* 0x0164: illegal_mthd */
+	0x100047f1,
+	0xd00042d0,
+	0x47f04043,
+	0x0004d040,
+/* 0x0174: im_loop */
+	0xf08004cf,
+	0x44b04044,
+	0xf71bf400,
+/* 0x0180: cmddone */
+	0x1d0037f1,
+	0xd00147f0,
+/* 0x018a: nocmd */
+	0x11c40034,
+	0x4001d00c,
+/* 0x0192: cmd_query_get */
+	0x38f201f8,
+	0x0325f001,
+	0x0b0047f1,
+/* 0x019c: ptimer_retry */
+	0xcf4046cf,
+	0x47cf0045,
+	0x0467b840,
+	0x98f41bf4,
+	0x04800504,
+	0x21008020,
+	0x80220580,
+	0x0bfe2306,
+	0x03049800,
+	0xfe1844b6,
+	0x04980047,
+	0x8057f104,
+	0x0253f000,
+	0xf80645fa,
+/* 0x01d7: cmd_cond_mode */
+	0xf400f803,
+	0x25f00131,
+	0x0534b002,
+	0xf41218f4,
+	0x34b00132,
+	0x0b18f402,
+	0x800136f0,
+/* 0x01f2: return */
+	0x00f80803,
+/* 0x01f4: cmd_cond_mode_queryful */
+	0x98060498,
+	0x56c40705,
+	0x0855b6ff,
+	0xfd1844b6,
+	0x47fe0545,
+	0x000bfe00,
+	0x008057f1,
+	0xfa0253f0,
+	0x34b00565,
+	0x131bf402,
+	0x049803f8,
+	0x0044b021,
+	0x800b4cf0,
+	0x00f80804,
+/* 0x022c: cmd_cond_mode_double */
+	0xb61060b6,
+	0x65fa1050,
+	0x9803f805,
+	0x06982005,
+	0x0456b824,
+	0x980b4cf0,
+	0x06982105,
+	0x0456b825,
+	0xfd0b5cf0,
+	0x34b00445,
+	0x0b5cf003,
+	0x800645fd,
+	0x00f80804,
+/* 0x0260: cmd_wrcache_flush */
+	0xf10132f4,
+	0xbd220027,
+	0x0133f034,
+	0xf80023d0,
+/* 0x0271: crypt_cmd_mode */
+	0x0131f400,
+	0xb00225f0,
+	0x18f40f34,
+	0x0132f409,
+/* 0x0283: crypt_cmd_mode_return */
+	0xf80d0380,
+/* 0x0285: crypt_cmd_length */
+	0x0034b000,
+	0xf4fb0bf4,
+	0x47f0033c,
+	0x0743f040,
+	0xf00604fa,
+	0x43f05047,
+	0x0604fa06,
+	0x3cf503f8,
+	0x47f1c407,
+	0x4bfe2100,
+	0x09049800,
+	0x950a0598,
+	0x44b60858,
+	0x0548fd18,
+	0x98ff55c4,
+	0x07980b06,
+	0x0878950c,
+	0xfd1864b6,
+	0x77c40568,
+	0x0d0898ff,
+	0x580284b6,
+	0x95f9a889,
+	0xf9a98958,
+	0x013cf495,
+	0x3cf403f8,
+	0xf803f861,
+	0x18489503,
+	0xbb084994,
+	0x81b60095,
+	0x09088000,
+	0x950a0980,
+	0x69941868,
+	0x0097bb08,
+	0x800081b6,
+	0x09800b08,
+	0x023cf40c,
+	0xf05047f0,
+	0x04fa0643,
+	0xf803f805,
+/* 0x0321: crypt_copy_prep */
+	0x203cf500,
+	0x003cf594,
+	0x003cf588,
+/* 0x032f: crypt_store_prep */
+	0xf500f88c,
+	0xf594103c,
+	0xf88c063c,
+/* 0x0339: crypt_ecb_e_prep */
+	0x303cf500,
+	0x003cf594,
+	0x003cf588,
+	0x003cf5d0,
+/* 0x034b: crypt_ecb_d_prep */
+	0xf500f88c,
+	0xf5c8773c,
+	0xf594303c,
+	0xf588003c,
+	0xf5d4003c,
+	0xf88c003c,
+/* 0x0361: crypt_cbc_e_prep */
+	0x403cf500,
+	0x003cf594,
+	0x063cf588,
+	0x663cf5ac,
+	0x063cf5d0,
+/* 0x0377: crypt_cbc_d_prep */
+	0xf500f88c,
+	0xf5c8773c,
+	0xf594503c,
+	0xf584623c,
+	0xf588063c,
+	0xf5d4603c,
+	0xf5ac203c,
+	0xf88c003c,
+/* 0x0395: crypt_pcbc_e_prep */
+	0x503cf500,
+	0x003cf594,
+	0x063cf588,
+	0x663cf5ac,
+	0x063cf5d0,
+	0x063cf58c,
+/* 0x03af: crypt_pcbc_d_prep */
+	0xf500f8ac,
+	0xf5c8773c,
+	0xf594503c,
+	0xf588003c,
+	0xf5d4013c,
+	0xf5ac163c,
+	0xf58c063c,
+	0xf8ac063c,
+/* 0x03cd: crypt_cfb_e_prep */
+	0x403cf500,
+	0x663cf594,
+	0x003cf5d0,
+	0x063cf588,
+	0x063cf5ac,
+/* 0x03e3: crypt_cfb_d_prep */
+	0xf500f88c,
+	0xf594403c,
+	0xf5d0603c,
+	0xf588063c,
+	0xf5ac603c,
+	0xf88c003c,
+/* 0x03f9: crypt_ofb_prep */
+	0x403cf500,
+	0x663cf594,
+	0x003cf5d0,
+	0x603cf588,
+	0x003cf5ac,
+/* 0x040f: crypt_ctr_prep */
+	0xf500f88c,
+	0xf594503c,
+	0xf5d0613c,
+	0xf5b0163c,
+	0xf588003c,
+	0xf5ac103c,
+	0xf88c003c,
+/* 0x0429: crypt_cbc_mac_prep */
+	0x303cf500,
+	0x003cf594,
+	0x063cf588,
+	0x663cf5ac,
+/* 0x043b: crypt_cmac_finish_complete_prep */
+	0xf500f8d0,
+	0xf594703c,
+	0xf588003c,
+	0xf5ac063c,
+	0xf5ac003c,
+	0xf5d0003c,
+	0xf5bc003c,
+	0xf5ac063c,
+	0xf8d0663c,
+/* 0x045d: crypt_cmac_finish_partial_prep */
+	0x803cf500,
+	0x003cf594,
+	0x063cf588,
+	0x003cf5ac,
+	0x003cf5ac,
+	0x003cf5d0,
+	0x003cf5bc,
+	0x063cf5bc,
+	0x663cf5ac,
+/* 0x0483: crypt_do_in */
+	0xbb00f8d0,
+	0x47fe0035,
+	0x8097f100,
+	0x0293f000,
+/* 0x0490: crypt_do_in_loop */
+	0xf80559fa,
+	0x223cf403,
+	0xf50609fa,
+	0xf898103c,
+	0x1050b603,
+	0xf40453b8,
+	0x3cf4e91b,
+	0xf803f801,
+/* 0x04b1: crypt_do_out */
+	0x0037bb00,
+	0xf10067fe,
+	0xf0008097,
+/* 0x04be: crypt_do_out_loop */
+	0x3cf50293,
+	0x3cf49810,
+	0x0579fa61,
+	0xf40679fa,
+	0x03f8013c,
+	0xb81070b6,
+	0x1bf40473,
+/* 0x04db: crypt_do_inout */
+	0xbb00f8e8,
+	0x97f10035,
+	0x93f00080,
+/* 0x04e5: crypt_do_inout_loop */
+	0x0047fe02,
+	0xf80559fa,
+	0x213cf403,
+	0xf50609fa,
+	0xf498103c,
+	0x67fe613c,
+	0x0579fa00,
+	0xf40679fa,
+	0x03f8013c,
+	0xb61050b6,
+	0x53b81070,
+	0xd41bf404,
+	0x000000f8,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+};
diff --git a/drivers/gpu/drm/nouveau/nva3_copy.c b/drivers/gpu/drm/nouveau/nva3_copy.c
index 8f356d58e40..0387dc7f4f4 100644
--- a/drivers/gpu/drm/nouveau/nva3_copy.c
+++ b/drivers/gpu/drm/nouveau/nva3_copy.c
@@ -79,29 +79,13 @@ static void
 nva3_copy_context_del(struct nouveau_channel *chan, int engine)
 {
 	struct nouveau_gpuobj *ctx = chan->engctx[engine];
-	struct drm_device *dev = chan->dev;
-	u32 inst;
-
-	inst  = (chan->ramin->vinst >> 12);
-	inst |= 0x40000000;
-
-	/* disable fifo access */
-	nv_wr32(dev, 0x104048, 0x00000000);
-	/* mark channel as unloaded if it's currently active */
-	if (nv_rd32(dev, 0x104050) == inst)
-		nv_mask(dev, 0x104050, 0x40000000, 0x00000000);
-	/* mark next channel as invalid if it's about to be loaded */
-	if (nv_rd32(dev, 0x104054) == inst)
-		nv_mask(dev, 0x104054, 0x40000000, 0x00000000);
-	/* restore fifo access */
-	nv_wr32(dev, 0x104048, 0x00000003);
+	int i;
 
-	for (inst = 0xc0; inst <= 0xd4; inst += 4)
-		nv_wo32(chan->ramin, inst, 0x00000000);
-
-	nouveau_gpuobj_ref(NULL, &ctx);
+	for (i = 0xc0; i <= 0xd4; i += 4)
+		nv_wo32(chan->ramin, i, 0x00000000);
 
 	atomic_dec(&chan->vm->engref[engine]);
+	nouveau_gpuobj_ref(NULL, &ctx);
 	chan->engctx[engine] = ctx;
 }
 
@@ -143,13 +127,6 @@ static int
 nva3_copy_fini(struct drm_device *dev, int engine, bool suspend)
 {
 	nv_mask(dev, 0x104048, 0x00000003, 0x00000000);
-
-	/* trigger fuc context unload */
-	nv_wait(dev, 0x104008, 0x0000000c, 0x00000000);
-	nv_mask(dev, 0x104054, 0x40000000, 0x00000000);
-	nv_wr32(dev, 0x104000, 0x00000008);
-	nv_wait(dev, 0x104008, 0x00000008, 0x00000000);
-
 	nv_wr32(dev, 0x104014, 0xffffffff);
 	return 0;
 }
diff --git a/drivers/gpu/drm/nouveau/nva3_pm.c b/drivers/gpu/drm/nouveau/nva3_pm.c
index 9e636e6ef6d..798829353fb 100644
--- a/drivers/gpu/drm/nouveau/nva3_pm.c
+++ b/drivers/gpu/drm/nouveau/nva3_pm.c
@@ -98,7 +98,9 @@ read_pll(struct drm_device *dev, int clk, u32 pll)
 		sclk = read_clk(dev, 0x10 + clk, false);
 	}
 
-	return sclk * N / (M * P);
+	if (M * P)
+		return sclk * N / (M * P);
+	return 0;
 }
 
 struct creg {
@@ -182,23 +184,26 @@ prog_pll(struct drm_device *dev, int clk, u32 pll, struct creg *reg)
 	const u32 src1 = 0x004160 + (clk * 4);
 	const u32 ctrl = pll + 0;
 	const u32 coef = pll + 4;
-	u32 cntl;
 
 	if (!reg->clk && !reg->pll) {
 		NV_DEBUG(dev, "no clock for %02x\n", clk);
 		return;
 	}
 
-	cntl = nv_rd32(dev, ctrl) & 0xfffffff2;
 	if (reg->pll) {
 		nv_mask(dev, src0, 0x00000101, 0x00000101);
 		nv_wr32(dev, coef, reg->pll);
-		nv_wr32(dev, ctrl, cntl | 0x00000015);
+		nv_mask(dev, ctrl, 0x00000015, 0x00000015);
+		nv_mask(dev, ctrl, 0x00000010, 0x00000000);
+		nv_wait(dev, ctrl, 0x00020000, 0x00020000);
+		nv_mask(dev, ctrl, 0x00000010, 0x00000010);
+		nv_mask(dev, ctrl, 0x00000008, 0x00000000);
 		nv_mask(dev, src1, 0x00000100, 0x00000000);
 		nv_mask(dev, src1, 0x00000001, 0x00000000);
 	} else {
 		nv_mask(dev, src1, 0x003f3141, 0x00000101 | reg->clk);
-		nv_wr32(dev, ctrl, cntl | 0x0000001d);
+		nv_mask(dev, ctrl, 0x00000018, 0x00000018);
+		udelay(20);
 		nv_mask(dev, ctrl, 0x00000001, 0x00000000);
 		nv_mask(dev, src0, 0x00000100, 0x00000000);
 		nv_mask(dev, src0, 0x00000001, 0x00000000);
@@ -230,17 +235,28 @@ nva3_pm_clocks_get(struct drm_device *dev, struct nouveau_pm_level *perflvl)
 }
 
 struct nva3_pm_state {
+	struct nouveau_pm_level *perflvl;
+
 	struct creg nclk;
 	struct creg sclk;
-	struct creg mclk;
 	struct creg vdec;
 	struct creg unka0;
+
+	struct creg mclk;
+	u8 *rammap;
+	u8  rammap_ver;
+	u8  rammap_len;
+	u8 *ramcfg;
+	u8  ramcfg_len;
+	u32 r004018;
+	u32 r100760;
 };
 
 void *
 nva3_pm_clocks_pre(struct drm_device *dev, struct nouveau_pm_level *perflvl)
 {
 	struct nva3_pm_state *info;
+	u8 ramcfg_cnt;
 	int ret;
 
 	info = kzalloc(sizeof(*info), GFP_KERNEL);
@@ -267,6 +283,20 @@ nva3_pm_clocks_pre(struct drm_device *dev, struct nouveau_pm_level *perflvl)
 	if (ret < 0)
 		goto out;
 
+	info->rammap = nouveau_perf_rammap(dev, perflvl->memory,
+					   &info->rammap_ver,
+					   &info->rammap_len,
+					   &ramcfg_cnt, &info->ramcfg_len);
+	if (info->rammap_ver != 0x10 || info->rammap_len < 5)
+		info->rammap = NULL;
+
+	info->ramcfg = nouveau_perf_ramcfg(dev, perflvl->memory,
+					   &info->rammap_ver,
+					   &info->ramcfg_len);
+	if (info->rammap_ver != 0x10)
+		info->ramcfg = NULL;
+
+	info->perflvl = perflvl;
 out:
 	if (ret < 0) {
 		kfree(info);
@@ -287,6 +317,240 @@ nva3_pm_grcp_idle(void *data)
 	return false;
 }
 
+static void
+mclk_precharge(struct nouveau_mem_exec_func *exec)
+{
+	nv_wr32(exec->dev, 0x1002d4, 0x00000001);
+}
+
+static void
+mclk_refresh(struct nouveau_mem_exec_func *exec)
+{
+	nv_wr32(exec->dev, 0x1002d0, 0x00000001);
+}
+
+static void
+mclk_refresh_auto(struct nouveau_mem_exec_func *exec, bool enable)
+{
+	nv_wr32(exec->dev, 0x100210, enable ? 0x80000000 : 0x00000000);
+}
+
+static void
+mclk_refresh_self(struct nouveau_mem_exec_func *exec, bool enable)
+{
+	nv_wr32(exec->dev, 0x1002dc, enable ? 0x00000001 : 0x00000000);
+}
+
+static void
+mclk_wait(struct nouveau_mem_exec_func *exec, u32 nsec)
+{
+	volatile u32 post = nv_rd32(exec->dev, 0); (void)post;
+	udelay((nsec + 500) / 1000);
+}
+
+static u32
+mclk_mrg(struct nouveau_mem_exec_func *exec, int mr)
+{
+	if (mr <= 1)
+		return nv_rd32(exec->dev, 0x1002c0 + ((mr - 0) * 4));
+	if (mr <= 3)
+		return nv_rd32(exec->dev, 0x1002e0 + ((mr - 2) * 4));
+	return 0;
+}
+
+static void
+mclk_mrs(struct nouveau_mem_exec_func *exec, int mr, u32 data)
+{
+	struct drm_nouveau_private *dev_priv = exec->dev->dev_private;
+
+	if (mr <= 1) {
+		if (dev_priv->vram_rank_B)
+			nv_wr32(exec->dev, 0x1002c8 + ((mr - 0) * 4), data);
+		nv_wr32(exec->dev, 0x1002c0 + ((mr - 0) * 4), data);
+	} else
+	if (mr <= 3) {
+		if (dev_priv->vram_rank_B)
+			nv_wr32(exec->dev, 0x1002e8 + ((mr - 2) * 4), data);
+		nv_wr32(exec->dev, 0x1002e0 + ((mr - 2) * 4), data);
+	}
+}
+
+static void
+mclk_clock_set(struct nouveau_mem_exec_func *exec)
+{
+	struct drm_device *dev = exec->dev;
+	struct nva3_pm_state *info = exec->priv;
+	u32 ctrl;
+
+	ctrl = nv_rd32(dev, 0x004000);
+	if (!(ctrl & 0x00000008) && info->mclk.pll) {
+		nv_wr32(dev, 0x004000, (ctrl |=  0x00000008));
+		nv_mask(dev, 0x1110e0, 0x00088000, 0x00088000);
+		nv_wr32(dev, 0x004018, 0x00001000);
+		nv_wr32(dev, 0x004000, (ctrl &= ~0x00000001));
+		nv_wr32(dev, 0x004004, info->mclk.pll);
+		nv_wr32(dev, 0x004000, (ctrl |=  0x00000001));
+		udelay(64);
+		nv_wr32(dev, 0x004018, 0x00005000 | info->r004018);
+		udelay(20);
+	} else
+	if (!info->mclk.pll) {
+		nv_mask(dev, 0x004168, 0x003f3040, info->mclk.clk);
+		nv_wr32(dev, 0x004000, (ctrl |= 0x00000008));
+		nv_mask(dev, 0x1110e0, 0x00088000, 0x00088000);
+		nv_wr32(dev, 0x004018, 0x0000d000 | info->r004018);
+	}
+
+	if (info->rammap) {
+		if (info->ramcfg && (info->rammap[4] & 0x08)) {
+			u32 unk5a0 = (ROM16(info->ramcfg[5]) << 8) |
+				      info->ramcfg[5];
+			u32 unk5a4 = ROM16(info->ramcfg[7]);
+			u32 unk804 = (info->ramcfg[9] & 0xf0) << 16 |
+				     (info->ramcfg[3] & 0x0f) << 16 |
+				     (info->ramcfg[9] & 0x0f) |
+				     0x80000000;
+			nv_wr32(dev, 0x1005a0, unk5a0);
+			nv_wr32(dev, 0x1005a4, unk5a4);
+			nv_wr32(dev, 0x10f804, unk804);
+			nv_mask(dev, 0x10053c, 0x00001000, 0x00000000);
+		} else {
+			nv_mask(dev, 0x10053c, 0x00001000, 0x00001000);
+			nv_mask(dev, 0x10f804, 0x80000000, 0x00000000);
+			nv_mask(dev, 0x100760, 0x22222222, info->r100760);
+			nv_mask(dev, 0x1007a0, 0x22222222, info->r100760);
+			nv_mask(dev, 0x1007e0, 0x22222222, info->r100760);
+		}
+	}
+
+	if (info->mclk.pll) {
+		nv_mask(dev, 0x1110e0, 0x00088000, 0x00011000);
+		nv_wr32(dev, 0x004000, (ctrl &= ~0x00000008));
+	}
+}
+
+static void
+mclk_timing_set(struct nouveau_mem_exec_func *exec)
+{
+	struct drm_device *dev = exec->dev;
+	struct nva3_pm_state *info = exec->priv;
+	struct nouveau_pm_level *perflvl = info->perflvl;
+	int i;
+
+	for (i = 0; i < 9; i++)
+		nv_wr32(dev, 0x100220 + (i * 4), perflvl->timing.reg[i]);
+
+	if (info->ramcfg) {
+		u32 data = (info->ramcfg[2] & 0x08) ? 0x00000000 : 0x00001000;
+		nv_mask(dev, 0x100200, 0x00001000, data);
+	}
+
+	if (info->ramcfg) {
+		u32 unk714 = nv_rd32(dev, 0x100714) & ~0xf0000010;
+		u32 unk718 = nv_rd32(dev, 0x100718) & ~0x00000100;
+		u32 unk71c = nv_rd32(dev, 0x10071c) & ~0x00000100;
+		if ( (info->ramcfg[2] & 0x20))
+			unk714 |= 0xf0000000;
+		if (!(info->ramcfg[2] & 0x04))
+			unk714 |= 0x00000010;
+		nv_wr32(dev, 0x100714, unk714);
+
+		if (info->ramcfg[2] & 0x01)
+			unk71c |= 0x00000100;
+		nv_wr32(dev, 0x10071c, unk71c);
+
+		if (info->ramcfg[2] & 0x02)
+			unk718 |= 0x00000100;
+		nv_wr32(dev, 0x100718, unk718);
+
+		if (info->ramcfg[2] & 0x10)
+			nv_wr32(dev, 0x111100, 0x48000000); /*XXX*/
+	}
+}
+
+static void
+prog_mem(struct drm_device *dev, struct nva3_pm_state *info)
+{
+	struct nouveau_mem_exec_func exec = {
+		.dev = dev,
+		.precharge = mclk_precharge,
+		.refresh = mclk_refresh,
+		.refresh_auto = mclk_refresh_auto,
+		.refresh_self = mclk_refresh_self,
+		.wait = mclk_wait,
+		.mrg = mclk_mrg,
+		.mrs = mclk_mrs,
+		.clock_set = mclk_clock_set,
+		.timing_set = mclk_timing_set,
+		.priv = info
+	};
+	u32 ctrl;
+
+	/* XXX: where the fuck does 750MHz come from? */
+	if (info->perflvl->memory <= 750000) {
+		info->r004018 = 0x10000000;
+		info->r100760 = 0x22222222;
+	}
+
+	ctrl = nv_rd32(dev, 0x004000);
+	if (ctrl & 0x00000008) {
+		if (info->mclk.pll) {
+			nv_mask(dev, 0x004128, 0x00000101, 0x00000101);
+			nv_wr32(dev, 0x004004, info->mclk.pll);
+			nv_wr32(dev, 0x004000, (ctrl |= 0x00000001));
+			nv_wr32(dev, 0x004000, (ctrl &= 0xffffffef));
+			nv_wait(dev, 0x004000, 0x00020000, 0x00020000);
+			nv_wr32(dev, 0x004000, (ctrl |= 0x00000010));
+			nv_wr32(dev, 0x004018, 0x00005000 | info->r004018);
+			nv_wr32(dev, 0x004000, (ctrl |= 0x00000004));
+		}
+	} else {
+		u32 ssel = 0x00000101;
+		if (info->mclk.clk)
+			ssel |= info->mclk.clk;
+		else
+			ssel |= 0x00080000; /* 324MHz, shouldn't matter... */
+		nv_mask(dev, 0x004168, 0x003f3141, ctrl);
+	}
+
+	if (info->ramcfg) {
+		if (info->ramcfg[2] & 0x10) {
+			nv_mask(dev, 0x111104, 0x00000600, 0x00000000);
+		} else {
+			nv_mask(dev, 0x111100, 0x40000000, 0x40000000);
+			nv_mask(dev, 0x111104, 0x00000180, 0x00000000);
+		}
+	}
+	if (info->rammap && !(info->rammap[4] & 0x02))
+		nv_mask(dev, 0x100200, 0x00000800, 0x00000000);
+	nv_wr32(dev, 0x611200, 0x00003300);
+	if (!(info->ramcfg[2] & 0x10))
+		nv_wr32(dev, 0x111100, 0x4c020000); /*XXX*/
+
+	nouveau_mem_exec(&exec, info->perflvl);
+
+	nv_wr32(dev, 0x611200, 0x00003330);
+	if (info->rammap && (info->rammap[4] & 0x02))
+		nv_mask(dev, 0x100200, 0x00000800, 0x00000800);
+	if (info->ramcfg) {
+		if (info->ramcfg[2] & 0x10) {
+			nv_mask(dev, 0x111104, 0x00000180, 0x00000180);
+			nv_mask(dev, 0x111100, 0x40000000, 0x00000000);
+		} else {
+			nv_mask(dev, 0x111104, 0x00000600, 0x00000600);
+		}
+	}
+
+	if (info->mclk.pll) {
+		nv_mask(dev, 0x004168, 0x00000001, 0x00000000);
+		nv_mask(dev, 0x004168, 0x00000100, 0x00000000);
+	} else {
+		nv_mask(dev, 0x004000, 0x00000001, 0x00000000);
+		nv_mask(dev, 0x004128, 0x00000001, 0x00000000);
+		nv_mask(dev, 0x004128, 0x00000100, 0x00000000);
+	}
+}
+
 int
 nva3_pm_clocks_set(struct drm_device *dev, void *pre_state)
 {
@@ -316,18 +580,8 @@ nva3_pm_clocks_set(struct drm_device *dev, void *pre_state)
 	prog_clk(dev, 0x20, &info->unka0);
 	prog_clk(dev, 0x21, &info->vdec);
 
-	if (info->mclk.clk || info->mclk.pll) {
-		nv_wr32(dev, 0x100210, 0);
-		nv_wr32(dev, 0x1002dc, 1);
-		nv_wr32(dev, 0x004018, 0x00001000);
-		prog_pll(dev, 0x02, 0x004000, &info->mclk);
-		if (nv_rd32(dev, 0x4000) & 0x00000008)
-			nv_wr32(dev, 0x004018, 0x1000d000);
-		else
-			nv_wr32(dev, 0x004018, 0x10005000);
-		nv_wr32(dev, 0x1002dc, 0);
-		nv_wr32(dev, 0x100210, 0x80000000);
-	}
+	if (info->mclk.clk || info->mclk.pll)
+		prog_mem(dev, info);
 
 	ret = 0;
 
diff --git a/drivers/gpu/drm/nouveau/nvc0_fbcon.c b/drivers/gpu/drm/nouveau/nvc0_fbcon.c
index a495e48197c..797159e7b7a 100644
--- a/drivers/gpu/drm/nouveau/nvc0_fbcon.c
+++ b/drivers/gpu/drm/nouveau/nvc0_fbcon.c
@@ -43,22 +43,22 @@ nvc0_fbcon_fillrect(struct fb_info *info, const struct fb_fillrect *rect)
 		return ret;
 
 	if (rect->rop != ROP_COPY) {
-		BEGIN_NVC0(chan, 2, NvSub2D, 0x02ac, 1);
+		BEGIN_NVC0(chan, NvSub2D, 0x02ac, 1);
 		OUT_RING  (chan, 1);
 	}
-	BEGIN_NVC0(chan, 2, NvSub2D, 0x0588, 1);
+	BEGIN_NVC0(chan, NvSub2D, 0x0588, 1);
 	if (info->fix.visual == FB_VISUAL_TRUECOLOR ||
 	    info->fix.visual == FB_VISUAL_DIRECTCOLOR)
 		OUT_RING  (chan, ((uint32_t *)info->pseudo_palette)[rect->color]);
 	else
 		OUT_RING  (chan, rect->color);
-	BEGIN_NVC0(chan, 2, NvSub2D, 0x0600, 4);
+	BEGIN_NVC0(chan, NvSub2D, 0x0600, 4);
 	OUT_RING  (chan, rect->dx);
 	OUT_RING  (chan, rect->dy);
 	OUT_RING  (chan, rect->dx + rect->width);
 	OUT_RING  (chan, rect->dy + rect->height);
 	if (rect->rop != ROP_COPY) {
-		BEGIN_NVC0(chan, 2, NvSub2D, 0x02ac, 1);
+		BEGIN_NVC0(chan, NvSub2D, 0x02ac, 1);
 		OUT_RING  (chan, 3);
 	}
 	FIRE_RING(chan);
@@ -78,14 +78,14 @@ nvc0_fbcon_copyarea(struct fb_info *info, const struct fb_copyarea *region)
 	if (ret)
 		return ret;
 
-	BEGIN_NVC0(chan, 2, NvSub2D, 0x0110, 1);
+	BEGIN_NVC0(chan, NvSub2D, 0x0110, 1);
 	OUT_RING  (chan, 0);
-	BEGIN_NVC0(chan, 2, NvSub2D, 0x08b0, 4);
+	BEGIN_NVC0(chan, NvSub2D, 0x08b0, 4);
 	OUT_RING  (chan, region->dx);
 	OUT_RING  (chan, region->dy);
 	OUT_RING  (chan, region->width);
 	OUT_RING  (chan, region->height);
-	BEGIN_NVC0(chan, 2, NvSub2D, 0x08d0, 4);
+	BEGIN_NVC0(chan, NvSub2D, 0x08d0, 4);
 	OUT_RING  (chan, 0);
 	OUT_RING  (chan, region->sx);
 	OUT_RING  (chan, 0);
@@ -116,7 +116,7 @@ nvc0_fbcon_imageblit(struct fb_info *info, const struct fb_image *image)
 	width = ALIGN(image->width, 32);
 	dwords = (width * image->height) >> 5;
 
-	BEGIN_NVC0(chan, 2, NvSub2D, 0x0814, 2);
+	BEGIN_NVC0(chan, NvSub2D, 0x0814, 2);
 	if (info->fix.visual == FB_VISUAL_TRUECOLOR ||
 	    info->fix.visual == FB_VISUAL_DIRECTCOLOR) {
 		OUT_RING  (chan, palette[image->bg_color] | mask);
@@ -125,10 +125,10 @@ nvc0_fbcon_imageblit(struct fb_info *info, const struct fb_image *image)
 		OUT_RING  (chan, image->bg_color);
 		OUT_RING  (chan, image->fg_color);
 	}
-	BEGIN_NVC0(chan, 2, NvSub2D, 0x0838, 2);
+	BEGIN_NVC0(chan, NvSub2D, 0x0838, 2);
 	OUT_RING  (chan, image->width);
 	OUT_RING  (chan, image->height);
-	BEGIN_NVC0(chan, 2, NvSub2D, 0x0850, 4);
+	BEGIN_NVC0(chan, NvSub2D, 0x0850, 4);
 	OUT_RING  (chan, 0);
 	OUT_RING  (chan, image->dx);
 	OUT_RING  (chan, 0);
@@ -143,7 +143,7 @@ nvc0_fbcon_imageblit(struct fb_info *info, const struct fb_image *image)
 
 		dwords -= push;
 
-		BEGIN_NVC0(chan, 6, NvSub2D, 0x0860, push);
+		BEGIN_NIC0(chan, NvSub2D, 0x0860, push);
 		OUT_RINGp(chan, data, push);
 		data += push;
 	}
@@ -200,47 +200,47 @@ nvc0_fbcon_accel_init(struct fb_info *info)
 		return ret;
 	}
 
-	BEGIN_NVC0(chan, 2, NvSub2D, 0x0000, 1);
+	BEGIN_NVC0(chan, NvSub2D, 0x0000, 1);
 	OUT_RING  (chan, 0x0000902d);
-	BEGIN_NVC0(chan, 2, NvSub2D, 0x0104, 2);
+	BEGIN_NVC0(chan, NvSub2D, 0x0104, 2);
 	OUT_RING  (chan, upper_32_bits(chan->notifier_vma.offset));
 	OUT_RING  (chan, lower_32_bits(chan->notifier_vma.offset));
-	BEGIN_NVC0(chan, 2, NvSub2D, 0x0290, 1);
+	BEGIN_NVC0(chan, NvSub2D, 0x0290, 1);
 	OUT_RING  (chan, 0);
-	BEGIN_NVC0(chan, 2, NvSub2D, 0x0888, 1);
+	BEGIN_NVC0(chan, NvSub2D, 0x0888, 1);
 	OUT_RING  (chan, 1);
-	BEGIN_NVC0(chan, 2, NvSub2D, 0x02ac, 1);
+	BEGIN_NVC0(chan, NvSub2D, 0x02ac, 1);
 	OUT_RING  (chan, 3);
-	BEGIN_NVC0(chan, 2, NvSub2D, 0x02a0, 1);
+	BEGIN_NVC0(chan, NvSub2D, 0x02a0, 1);
 	OUT_RING  (chan, 0x55);
-	BEGIN_NVC0(chan, 2, NvSub2D, 0x08c0, 4);
+	BEGIN_NVC0(chan, NvSub2D, 0x08c0, 4);
 	OUT_RING  (chan, 0);
 	OUT_RING  (chan, 1);
 	OUT_RING  (chan, 0);
 	OUT_RING  (chan, 1);
-	BEGIN_NVC0(chan, 2, NvSub2D, 0x0580, 2);
+	BEGIN_NVC0(chan, NvSub2D, 0x0580, 2);
 	OUT_RING  (chan, 4);
 	OUT_RING  (chan, format);
-	BEGIN_NVC0(chan, 2, NvSub2D, 0x02e8, 2);
+	BEGIN_NVC0(chan, NvSub2D, 0x02e8, 2);
 	OUT_RING  (chan, 2);
 	OUT_RING  (chan, 1);
 
-	BEGIN_NVC0(chan, 2, NvSub2D, 0x0804, 1);
+	BEGIN_NVC0(chan, NvSub2D, 0x0804, 1);
 	OUT_RING  (chan, format);
-	BEGIN_NVC0(chan, 2, NvSub2D, 0x0800, 1);
+	BEGIN_NVC0(chan, NvSub2D, 0x0800, 1);
 	OUT_RING  (chan, 1);
-	BEGIN_NVC0(chan, 2, NvSub2D, 0x0808, 3);
+	BEGIN_NVC0(chan, NvSub2D, 0x0808, 3);
 	OUT_RING  (chan, 0);
 	OUT_RING  (chan, 0);
 	OUT_RING  (chan, 1);
-	BEGIN_NVC0(chan, 2, NvSub2D, 0x081c, 1);
+	BEGIN_NVC0(chan, NvSub2D, 0x081c, 1);
 	OUT_RING  (chan, 1);
-	BEGIN_NVC0(chan, 2, NvSub2D, 0x0840, 4);
+	BEGIN_NVC0(chan, NvSub2D, 0x0840, 4);
 	OUT_RING  (chan, 0);
 	OUT_RING  (chan, 1);
 	OUT_RING  (chan, 0);
 	OUT_RING  (chan, 1);
-	BEGIN_NVC0(chan, 2, NvSub2D, 0x0200, 10);
+	BEGIN_NVC0(chan, NvSub2D, 0x0200, 10);
 	OUT_RING  (chan, format);
 	OUT_RING  (chan, 1);
 	OUT_RING  (chan, 0);
@@ -251,7 +251,7 @@ nvc0_fbcon_accel_init(struct fb_info *info)
 	OUT_RING  (chan, info->var.yres_virtual);
 	OUT_RING  (chan, upper_32_bits(fb->vma.offset));
 	OUT_RING  (chan, lower_32_bits(fb->vma.offset));
-	BEGIN_NVC0(chan, 2, NvSub2D, 0x0230, 10);
+	BEGIN_NVC0(chan, NvSub2D, 0x0230, 10);
 	OUT_RING  (chan, format);
 	OUT_RING  (chan, 1);
 	OUT_RING  (chan, 0);
diff --git a/drivers/gpu/drm/nouveau/nvc0_fence.c b/drivers/gpu/drm/nouveau/nvc0_fence.c
new file mode 100644
index 00000000000..47ab388a606
--- /dev/null
+++ b/drivers/gpu/drm/nouveau/nvc0_fence.c
@@ -0,0 +1,184 @@
+/*
+ * 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 "drmP.h"
+#include "nouveau_drv.h"
+#include "nouveau_dma.h"
+#include "nouveau_fifo.h"
+#include "nouveau_ramht.h"
+#include "nouveau_fence.h"
+
+struct nvc0_fence_priv {
+	struct nouveau_fence_priv base;
+	struct nouveau_bo *bo;
+};
+
+struct nvc0_fence_chan {
+	struct nouveau_fence_chan base;
+	struct nouveau_vma vma;
+};
+
+static int
+nvc0_fence_emit(struct nouveau_fence *fence)
+{
+	struct nouveau_channel *chan = fence->channel;
+	struct nvc0_fence_chan *fctx = chan->engctx[NVOBJ_ENGINE_FENCE];
+	u64 addr = fctx->vma.offset + chan->id * 16;
+	int ret;
+
+	ret = RING_SPACE(chan, 5);
+	if (ret == 0) {
+		BEGIN_NVC0(chan, 0, NV84_SUBCHAN_SEMAPHORE_ADDRESS_HIGH, 4);
+		OUT_RING  (chan, upper_32_bits(addr));
+		OUT_RING  (chan, lower_32_bits(addr));
+		OUT_RING  (chan, fence->sequence);
+		OUT_RING  (chan, NV84_SUBCHAN_SEMAPHORE_TRIGGER_WRITE_LONG);
+		FIRE_RING (chan);
+	}
+
+	return ret;
+}
+
+static int
+nvc0_fence_sync(struct nouveau_fence *fence,
+		struct nouveau_channel *prev, struct nouveau_channel *chan)
+{
+	struct nvc0_fence_chan *fctx = chan->engctx[NVOBJ_ENGINE_FENCE];
+	u64 addr = fctx->vma.offset + prev->id * 16;
+	int ret;
+
+	ret = RING_SPACE(chan, 5);
+	if (ret == 0) {
+		BEGIN_NVC0(chan, 0, NV84_SUBCHAN_SEMAPHORE_ADDRESS_HIGH, 4);
+		OUT_RING  (chan, upper_32_bits(addr));
+		OUT_RING  (chan, lower_32_bits(addr));
+		OUT_RING  (chan, fence->sequence);
+		OUT_RING  (chan, NV84_SUBCHAN_SEMAPHORE_TRIGGER_ACQUIRE_GEQUAL |
+				 NVC0_SUBCHAN_SEMAPHORE_TRIGGER_YIELD);
+		FIRE_RING (chan);
+	}
+
+	return ret;
+}
+
+static u32
+nvc0_fence_read(struct nouveau_channel *chan)
+{
+	struct nvc0_fence_priv *priv = nv_engine(chan->dev, NVOBJ_ENGINE_FENCE);
+	return nouveau_bo_rd32(priv->bo, chan->id * 16/4);
+}
+
+static void
+nvc0_fence_context_del(struct nouveau_channel *chan, int engine)
+{
+	struct nvc0_fence_priv *priv = nv_engine(chan->dev, engine);
+	struct nvc0_fence_chan *fctx = chan->engctx[engine];
+
+	nouveau_bo_vma_del(priv->bo, &fctx->vma);
+	nouveau_fence_context_del(&fctx->base);
+	chan->engctx[engine] = NULL;
+	kfree(fctx);
+}
+
+static int
+nvc0_fence_context_new(struct nouveau_channel *chan, int engine)
+{
+	struct nvc0_fence_priv *priv = nv_engine(chan->dev, engine);
+	struct nvc0_fence_chan *fctx;
+	int ret;
+
+	fctx = chan->engctx[engine] = kzalloc(sizeof(*fctx), GFP_KERNEL);
+	if (!fctx)
+		return -ENOMEM;
+
+	nouveau_fence_context_new(&fctx->base);
+
+	ret = nouveau_bo_vma_add(priv->bo, chan->vm, &fctx->vma);
+	if (ret)
+		nvc0_fence_context_del(chan, engine);
+
+	nouveau_bo_wr32(priv->bo, chan->id * 16/4, 0x00000000);
+	return ret;
+}
+
+static int
+nvc0_fence_fini(struct drm_device *dev, int engine, bool suspend)
+{
+	return 0;
+}
+
+static int
+nvc0_fence_init(struct drm_device *dev, int engine)
+{
+	return 0;
+}
+
+static void
+nvc0_fence_destroy(struct drm_device *dev, int engine)
+{
+	struct drm_nouveau_private *dev_priv = dev->dev_private;
+	struct nvc0_fence_priv *priv = nv_engine(dev, engine);
+
+	nouveau_bo_unmap(priv->bo);
+	nouveau_bo_ref(NULL, &priv->bo);
+	dev_priv->eng[engine] = NULL;
+	kfree(priv);
+}
+
+int
+nvc0_fence_create(struct drm_device *dev)
+{
+	struct nouveau_fifo_priv *pfifo = nv_engine(dev, NVOBJ_ENGINE_FIFO);
+	struct drm_nouveau_private *dev_priv = dev->dev_private;
+	struct nvc0_fence_priv *priv;
+	int ret;
+
+	priv = kzalloc(sizeof(*priv), GFP_KERNEL);
+	if (!priv)
+		return -ENOMEM;
+
+	priv->base.engine.destroy = nvc0_fence_destroy;
+	priv->base.engine.init = nvc0_fence_init;
+	priv->base.engine.fini = nvc0_fence_fini;
+	priv->base.engine.context_new = nvc0_fence_context_new;
+	priv->base.engine.context_del = nvc0_fence_context_del;
+	priv->base.emit = nvc0_fence_emit;
+	priv->base.sync = nvc0_fence_sync;
+	priv->base.read = nvc0_fence_read;
+	dev_priv->eng[NVOBJ_ENGINE_FENCE] = &priv->base.engine;
+
+	ret = nouveau_bo_new(dev, 16 * pfifo->channels, 0, TTM_PL_FLAG_VRAM,
+			     0, 0, NULL, &priv->bo);
+	if (ret == 0) {
+		ret = nouveau_bo_pin(priv->bo, TTM_PL_FLAG_VRAM);
+		if (ret == 0)
+			ret = nouveau_bo_map(priv->bo);
+		if (ret)
+			nouveau_bo_ref(NULL, &priv->bo);
+	}
+
+	if (ret)
+		nvc0_fence_destroy(dev, NVOBJ_ENGINE_FENCE);
+	return ret;
+}
diff --git a/drivers/gpu/drm/nouveau/nvc0_fifo.c b/drivers/gpu/drm/nouveau/nvc0_fifo.c
index 50d68a7a137..7d85553d518 100644
--- a/drivers/gpu/drm/nouveau/nvc0_fifo.c
+++ b/drivers/gpu/drm/nouveau/nvc0_fifo.c
@@ -26,10 +26,12 @@
 
 #include "nouveau_drv.h"
 #include "nouveau_mm.h"
+#include "nouveau_fifo.h"
 
 static void nvc0_fifo_isr(struct drm_device *);
 
 struct nvc0_fifo_priv {
+	struct nouveau_fifo_priv base;
 	struct nouveau_gpuobj *playlist[2];
 	int cur_playlist;
 	struct nouveau_vma user_vma;
@@ -37,8 +39,8 @@ struct nvc0_fifo_priv {
 };
 
 struct nvc0_fifo_chan {
+	struct nouveau_fifo_chan base;
 	struct nouveau_gpuobj *user;
-	struct nouveau_gpuobj *ramfc;
 };
 
 static void
@@ -46,8 +48,7 @@ nvc0_fifo_playlist_update(struct drm_device *dev)
 {
 	struct drm_nouveau_private *dev_priv = dev->dev_private;
 	struct nouveau_instmem_engine *pinstmem = &dev_priv->engine.instmem;
-	struct nouveau_fifo_engine *pfifo = &dev_priv->engine.fifo;
-	struct nvc0_fifo_priv *priv = pfifo->priv;
+	struct nvc0_fifo_priv *priv = nv_engine(dev, NVOBJ_ENGINE_FIFO);
 	struct nouveau_gpuobj *cur;
 	int i, p;
 
@@ -69,59 +70,20 @@ nvc0_fifo_playlist_update(struct drm_device *dev)
 		NV_ERROR(dev, "PFIFO - playlist update failed\n");
 }
 
-void
-nvc0_fifo_disable(struct drm_device *dev)
-{
-}
-
-void
-nvc0_fifo_enable(struct drm_device *dev)
-{
-}
-
-bool
-nvc0_fifo_reassign(struct drm_device *dev, bool enable)
-{
-	return false;
-}
-
-bool
-nvc0_fifo_cache_pull(struct drm_device *dev, bool enable)
-{
-	return false;
-}
-
-int
-nvc0_fifo_channel_id(struct drm_device *dev)
-{
-	return 127;
-}
-
-int
-nvc0_fifo_create_context(struct nouveau_channel *chan)
+static int
+nvc0_fifo_context_new(struct nouveau_channel *chan, int engine)
 {
 	struct drm_device *dev = chan->dev;
 	struct drm_nouveau_private *dev_priv = dev->dev_private;
 	struct nouveau_instmem_engine *pinstmem = &dev_priv->engine.instmem;
-	struct nouveau_fifo_engine *pfifo = &dev_priv->engine.fifo;
-	struct nvc0_fifo_priv *priv = pfifo->priv;
-	struct nvc0_fifo_chan *fifoch;
+	struct nvc0_fifo_priv *priv = nv_engine(dev, engine);
+	struct nvc0_fifo_chan *fctx;
 	u64 ib_virt = chan->pushbuf_base + chan->dma.ib_base * 4;
-	int ret;
+	int ret, i;
 
-	chan->fifo_priv = kzalloc(sizeof(*fifoch), GFP_KERNEL);
-	if (!chan->fifo_priv)
+	fctx = chan->engctx[engine] = kzalloc(sizeof(*fctx), GFP_KERNEL);
+	if (!fctx)
 		return -ENOMEM;
-	fifoch = chan->fifo_priv;
-
-	/* allocate vram for control regs, map into polling area */
-	ret = nouveau_gpuobj_new(dev, NULL, 0x1000, 0x1000,
-				 NVOBJ_FLAG_ZERO_ALLOC, &fifoch->user);
-	if (ret)
-		goto error;
-
-	nouveau_vm_map_at(&priv->user_vma, chan->id * 0x1000,
-			  *(struct nouveau_mem **)fifoch->user->node);
 
 	chan->user = ioremap_wc(pci_resource_start(dev->pdev, 1) +
 				priv->user_vma.offset + (chan->id * 0x1000),
@@ -131,176 +93,77 @@ nvc0_fifo_create_context(struct nouveau_channel *chan)
 		goto error;
 	}
 
-	/* ramfc */
-	ret = nouveau_gpuobj_new_fake(dev, chan->ramin->pinst,
-				      chan->ramin->vinst, 0x100,
-				      NVOBJ_FLAG_ZERO_ALLOC, &fifoch->ramfc);
+	/* allocate vram for control regs, map into polling area */
+	ret = nouveau_gpuobj_new(dev, NULL, 0x1000, 0x1000,
+				 NVOBJ_FLAG_ZERO_ALLOC, &fctx->user);
 	if (ret)
 		goto error;
 
-	nv_wo32(fifoch->ramfc, 0x08, lower_32_bits(fifoch->user->vinst));
-	nv_wo32(fifoch->ramfc, 0x0c, upper_32_bits(fifoch->user->vinst));
-	nv_wo32(fifoch->ramfc, 0x10, 0x0000face);
-	nv_wo32(fifoch->ramfc, 0x30, 0xfffff902);
-	nv_wo32(fifoch->ramfc, 0x48, lower_32_bits(ib_virt));
-	nv_wo32(fifoch->ramfc, 0x4c, drm_order(chan->dma.ib_max + 1) << 16 |
+	nouveau_vm_map_at(&priv->user_vma, chan->id * 0x1000,
+			  *(struct nouveau_mem **)fctx->user->node);
+
+	for (i = 0; i < 0x100; i += 4)
+		nv_wo32(chan->ramin, i, 0x00000000);
+	nv_wo32(chan->ramin, 0x08, lower_32_bits(fctx->user->vinst));
+	nv_wo32(chan->ramin, 0x0c, upper_32_bits(fctx->user->vinst));
+	nv_wo32(chan->ramin, 0x10, 0x0000face);
+	nv_wo32(chan->ramin, 0x30, 0xfffff902);
+	nv_wo32(chan->ramin, 0x48, lower_32_bits(ib_virt));
+	nv_wo32(chan->ramin, 0x4c, drm_order(chan->dma.ib_max + 1) << 16 |
 				   upper_32_bits(ib_virt));
-	nv_wo32(fifoch->ramfc, 0x54, 0x00000002);
-	nv_wo32(fifoch->ramfc, 0x84, 0x20400000);
-	nv_wo32(fifoch->ramfc, 0x94, 0x30000001);
-	nv_wo32(fifoch->ramfc, 0x9c, 0x00000100);
-	nv_wo32(fifoch->ramfc, 0xa4, 0x1f1f1f1f);
-	nv_wo32(fifoch->ramfc, 0xa8, 0x1f1f1f1f);
-	nv_wo32(fifoch->ramfc, 0xac, 0x0000001f);
-	nv_wo32(fifoch->ramfc, 0xb8, 0xf8000000);
-	nv_wo32(fifoch->ramfc, 0xf8, 0x10003080); /* 0x002310 */
-	nv_wo32(fifoch->ramfc, 0xfc, 0x10000010); /* 0x002350 */
+	nv_wo32(chan->ramin, 0x54, 0x00000002);
+	nv_wo32(chan->ramin, 0x84, 0x20400000);
+	nv_wo32(chan->ramin, 0x94, 0x30000001);
+	nv_wo32(chan->ramin, 0x9c, 0x00000100);
+	nv_wo32(chan->ramin, 0xa4, 0x1f1f1f1f);
+	nv_wo32(chan->ramin, 0xa8, 0x1f1f1f1f);
+	nv_wo32(chan->ramin, 0xac, 0x0000001f);
+	nv_wo32(chan->ramin, 0xb8, 0xf8000000);
+	nv_wo32(chan->ramin, 0xf8, 0x10003080); /* 0x002310 */
+	nv_wo32(chan->ramin, 0xfc, 0x10000010); /* 0x002350 */
 	pinstmem->flush(dev);
 
 	nv_wr32(dev, 0x003000 + (chan->id * 8), 0xc0000000 |
 						(chan->ramin->vinst >> 12));
 	nv_wr32(dev, 0x003004 + (chan->id * 8), 0x001f0001);
 	nvc0_fifo_playlist_update(dev);
-	return 0;
 
 error:
-	pfifo->destroy_context(chan);
+	if (ret)
+		priv->base.base.context_del(chan, engine);
 	return ret;
 }
 
-void
-nvc0_fifo_destroy_context(struct nouveau_channel *chan)
+static void
+nvc0_fifo_context_del(struct nouveau_channel *chan, int engine)
 {
+	struct nvc0_fifo_chan *fctx = chan->engctx[engine];
 	struct drm_device *dev = chan->dev;
-	struct nvc0_fifo_chan *fifoch;
 
 	nv_mask(dev, 0x003004 + (chan->id * 8), 0x00000001, 0x00000000);
 	nv_wr32(dev, 0x002634, chan->id);
 	if (!nv_wait(dev, 0x0002634, 0xffffffff, chan->id))
 		NV_WARN(dev, "0x2634 != chid: 0x%08x\n", nv_rd32(dev, 0x2634));
-
 	nvc0_fifo_playlist_update(dev);
-
 	nv_wr32(dev, 0x003000 + (chan->id * 8), 0x00000000);
 
+	nouveau_gpuobj_ref(NULL, &fctx->user);
 	if (chan->user) {
 		iounmap(chan->user);
 		chan->user = NULL;
 	}
 
-	fifoch = chan->fifo_priv;
-	chan->fifo_priv = NULL;
-	if (!fifoch)
-		return;
-
-	nouveau_gpuobj_ref(NULL, &fifoch->ramfc);
-	nouveau_gpuobj_ref(NULL, &fifoch->user);
-	kfree(fifoch);
-}
-
-int
-nvc0_fifo_load_context(struct nouveau_channel *chan)
-{
-	return 0;
-}
-
-int
-nvc0_fifo_unload_context(struct drm_device *dev)
-{
-	int i;
-
-	for (i = 0; i < 128; i++) {
-		if (!(nv_rd32(dev, 0x003004 + (i * 8)) & 1))
-			continue;
-
-		nv_mask(dev, 0x003004 + (i * 8), 0x00000001, 0x00000000);
-		nv_wr32(dev, 0x002634, i);
-		if (!nv_wait(dev, 0x002634, 0xffffffff, i)) {
-			NV_INFO(dev, "PFIFO: kick ch %d failed: 0x%08x\n",
-				i, nv_rd32(dev, 0x002634));
-			return -EBUSY;
-		}
-	}
-
-	return 0;
-}
-
-static void
-nvc0_fifo_destroy(struct drm_device *dev)
-{
-	struct drm_nouveau_private *dev_priv = dev->dev_private;
-	struct nouveau_fifo_engine *pfifo = &dev_priv->engine.fifo;
-	struct nvc0_fifo_priv *priv;
-
-	priv = pfifo->priv;
-	if (!priv)
-		return;
-
-	nouveau_vm_put(&priv->user_vma);
-	nouveau_gpuobj_ref(NULL, &priv->playlist[1]);
-	nouveau_gpuobj_ref(NULL, &priv->playlist[0]);
-	kfree(priv);
-}
-
-void
-nvc0_fifo_takedown(struct drm_device *dev)
-{
-	nv_wr32(dev, 0x002140, 0x00000000);
-	nvc0_fifo_destroy(dev);
+	chan->engctx[engine] = NULL;
+	kfree(fctx);
 }
 
 static int
-nvc0_fifo_create(struct drm_device *dev)
-{
-	struct drm_nouveau_private *dev_priv = dev->dev_private;
-	struct nouveau_fifo_engine *pfifo = &dev_priv->engine.fifo;
-	struct nvc0_fifo_priv *priv;
-	int ret;
-
-	priv = kzalloc(sizeof(*priv), GFP_KERNEL);
-	if (!priv)
-		return -ENOMEM;
-	pfifo->priv = priv;
-
-	ret = nouveau_gpuobj_new(dev, NULL, 0x1000, 0x1000, 0,
-				 &priv->playlist[0]);
-	if (ret)
-		goto error;
-
-	ret = nouveau_gpuobj_new(dev, NULL, 0x1000, 0x1000, 0,
-				 &priv->playlist[1]);
-	if (ret)
-		goto error;
-
-	ret = nouveau_vm_get(dev_priv->bar1_vm, pfifo->channels * 0x1000,
-			     12, NV_MEM_ACCESS_RW, &priv->user_vma);
-	if (ret)
-		goto error;
-
-	nouveau_irq_register(dev, 8, nvc0_fifo_isr);
-	NVOBJ_CLASS(dev, 0x506e, SW); /* nvsw */
-	return 0;
-
-error:
-	nvc0_fifo_destroy(dev);
-	return ret;
-}
-
-int
-nvc0_fifo_init(struct drm_device *dev)
+nvc0_fifo_init(struct drm_device *dev, int engine)
 {
 	struct drm_nouveau_private *dev_priv = dev->dev_private;
-	struct nouveau_fifo_engine *pfifo = &dev_priv->engine.fifo;
+	struct nvc0_fifo_priv *priv = nv_engine(dev, engine);
 	struct nouveau_channel *chan;
-	struct nvc0_fifo_priv *priv;
-	int ret, i;
-
-	if (!pfifo->priv) {
-		ret = nvc0_fifo_create(dev);
-		if (ret)
-			return ret;
-	}
-	priv = pfifo->priv;
+	int i;
 
 	/* reset PFIFO, enable all available PSUBFIFO areas */
 	nv_mask(dev, 0x000200, 0x00000100, 0x00000000);
@@ -338,7 +201,7 @@ nvc0_fifo_init(struct drm_device *dev)
 	/* restore PFIFO context table */
 	for (i = 0; i < 128; i++) {
 		chan = dev_priv->channels.ptr[i];
-		if (!chan || !chan->fifo_priv)
+		if (!chan || !chan->engctx[engine])
 			continue;
 
 		nv_wr32(dev, 0x003000 + (i * 8), 0xc0000000 |
@@ -350,6 +213,29 @@ nvc0_fifo_init(struct drm_device *dev)
 	return 0;
 }
 
+static int
+nvc0_fifo_fini(struct drm_device *dev, int engine, bool suspend)
+{
+	int i;
+
+	for (i = 0; i < 128; i++) {
+		if (!(nv_rd32(dev, 0x003004 + (i * 8)) & 1))
+			continue;
+
+		nv_mask(dev, 0x003004 + (i * 8), 0x00000001, 0x00000000);
+		nv_wr32(dev, 0x002634, i);
+		if (!nv_wait(dev, 0x002634, 0xffffffff, i)) {
+			NV_INFO(dev, "PFIFO: kick ch %d failed: 0x%08x\n",
+				i, nv_rd32(dev, 0x002634));
+			return -EBUSY;
+		}
+	}
+
+	nv_wr32(dev, 0x002140, 0x00000000);
+	return 0;
+}
+
+
 struct nouveau_enum nvc0_fifo_fault_unit[] = {
 	{ 0x00, "PGRAPH" },
 	{ 0x03, "PEEPHOLE" },
@@ -439,13 +325,14 @@ nvc0_fifo_isr_vm_fault(struct drm_device *dev, int unit)
 static int
 nvc0_fifo_page_flip(struct drm_device *dev, u32 chid)
 {
+	struct nvc0_fifo_priv *priv = nv_engine(dev, NVOBJ_ENGINE_FIFO);
 	struct drm_nouveau_private *dev_priv = dev->dev_private;
 	struct nouveau_channel *chan = NULL;
 	unsigned long flags;
 	int ret = -EINVAL;
 
 	spin_lock_irqsave(&dev_priv->channels.lock, flags);
-	if (likely(chid >= 0 && chid < dev_priv->engine.fifo.channels)) {
+	if (likely(chid >= 0 && chid < priv->base.channels)) {
 		chan = dev_priv->channels.ptr[chid];
 		if (likely(chan))
 			ret = nouveau_finish_page_flip(chan, NULL);
@@ -534,3 +421,56 @@ nvc0_fifo_isr(struct drm_device *dev)
 		nv_wr32(dev, 0x002140, 0);
 	}
 }
+
+static void
+nvc0_fifo_destroy(struct drm_device *dev, int engine)
+{
+	struct nvc0_fifo_priv *priv = nv_engine(dev, NVOBJ_ENGINE_FIFO);
+	struct drm_nouveau_private *dev_priv = dev->dev_private;
+
+	nouveau_vm_put(&priv->user_vma);
+	nouveau_gpuobj_ref(NULL, &priv->playlist[1]);
+	nouveau_gpuobj_ref(NULL, &priv->playlist[0]);
+
+	dev_priv->eng[engine] = NULL;
+	kfree(priv);
+}
+
+int
+nvc0_fifo_create(struct drm_device *dev)
+{
+	struct drm_nouveau_private *dev_priv = dev->dev_private;
+	struct nvc0_fifo_priv *priv;
+	int ret;
+
+	priv = kzalloc(sizeof(*priv), GFP_KERNEL);
+	if (!priv)
+		return -ENOMEM;
+
+	priv->base.base.destroy = nvc0_fifo_destroy;
+	priv->base.base.init = nvc0_fifo_init;
+	priv->base.base.fini = nvc0_fifo_fini;
+	priv->base.base.context_new = nvc0_fifo_context_new;
+	priv->base.base.context_del = nvc0_fifo_context_del;
+	priv->base.channels = 128;
+	dev_priv->eng[NVOBJ_ENGINE_FIFO] = &priv->base.base;
+
+	ret = nouveau_gpuobj_new(dev, NULL, 4096, 4096, 0, &priv->playlist[0]);
+	if (ret)
+		goto error;
+
+	ret = nouveau_gpuobj_new(dev, NULL, 4096, 4096, 0, &priv->playlist[1]);
+	if (ret)
+		goto error;
+
+	ret = nouveau_vm_get(dev_priv->bar1_vm, priv->base.channels * 0x1000,
+			     12, NV_MEM_ACCESS_RW, &priv->user_vma);
+	if (ret)
+		goto error;
+
+	nouveau_irq_register(dev, 8, nvc0_fifo_isr);
+error:
+	if (ret)
+		priv->base.base.destroy(dev, NVOBJ_ENGINE_FIFO);
+	return ret;
+}
diff --git a/drivers/gpu/drm/nouveau/nvc0_graph.c b/drivers/gpu/drm/nouveau/nvc0_graph.c
index 9066102d115..2a01e6e4772 100644
--- a/drivers/gpu/drm/nouveau/nvc0_graph.c
+++ b/drivers/gpu/drm/nouveau/nvc0_graph.c
@@ -29,6 +29,7 @@
 
 #include "nouveau_drv.h"
 #include "nouveau_mm.h"
+#include "nouveau_fifo.h"
 
 #include "nvc0_graph.h"
 #include "nvc0_grhub.fuc.h"
@@ -620,13 +621,14 @@ nvc0_graph_init(struct drm_device *dev, int engine)
 int
 nvc0_graph_isr_chid(struct drm_device *dev, u64 inst)
 {
+	struct nouveau_fifo_priv *pfifo = nv_engine(dev, NVOBJ_ENGINE_FIFO);
 	struct drm_nouveau_private *dev_priv = dev->dev_private;
 	struct nouveau_channel *chan;
 	unsigned long flags;
 	int i;
 
 	spin_lock_irqsave(&dev_priv->channels.lock, flags);
-	for (i = 0; i < dev_priv->engine.fifo.channels; i++) {
+	for (i = 0; i < pfifo->channels; i++) {
 		chan = dev_priv->channels.ptr[i];
 		if (!chan || !chan->ramin)
 			continue;
diff --git a/drivers/gpu/drm/nouveau/nvc0_pm.c b/drivers/gpu/drm/nouveau/nvc0_pm.c
index ce65f81bb87..7c95c44e288 100644
--- a/drivers/gpu/drm/nouveau/nvc0_pm.c
+++ b/drivers/gpu/drm/nouveau/nvc0_pm.c
@@ -164,7 +164,9 @@ struct nvc0_pm_clock {
 };
 
 struct nvc0_pm_state {
+	struct nouveau_pm_level *perflvl;
 	struct nvc0_pm_clock eng[16];
+	struct nvc0_pm_clock mem;
 };
 
 static u32
@@ -303,6 +305,48 @@ calc_clk(struct drm_device *dev, int clk, struct nvc0_pm_clock *info, u32 freq)
 	return 0;
 }
 
+static int
+calc_mem(struct drm_device *dev, struct nvc0_pm_clock *info, u32 freq)
+{
+	struct pll_lims pll;
+	int N, M, P, ret;
+	u32 ctrl;
+
+	/* mclk pll input freq comes from another pll, make sure it's on */
+	ctrl = nv_rd32(dev, 0x132020);
+	if (!(ctrl & 0x00000001)) {
+		/* if not, program it to 567MHz.  nfi where this value comes
+		 * from - it looks like it's in the pll limits table for
+		 * 132000 but the binary driver ignores all my attempts to
+		 * change this value.
+		 */
+		nv_wr32(dev, 0x137320, 0x00000103);
+		nv_wr32(dev, 0x137330, 0x81200606);
+		nv_wait(dev, 0x132020, 0x00010000, 0x00010000);
+		nv_wr32(dev, 0x132024, 0x0001150f);
+		nv_mask(dev, 0x132020, 0x00000001, 0x00000001);
+		nv_wait(dev, 0x137390, 0x00020000, 0x00020000);
+		nv_mask(dev, 0x132020, 0x00000004, 0x00000004);
+	}
+
+	/* for the moment, until the clock tree is better understood, use
+	 * pll mode for all clock frequencies
+	 */
+	ret = get_pll_limits(dev, 0x132000, &pll);
+	if (ret == 0) {
+		pll.refclk = read_pll(dev, 0x132020);
+		if (pll.refclk) {
+			ret = nva3_calc_pll(dev, &pll, freq, &N, NULL, &M, &P);
+			if (ret > 0) {
+				info->coef = (P << 16) | (N << 8) | M;
+				return 0;
+			}
+		}
+	}
+
+	return -EINVAL;
+}
+
 void *
 nvc0_pm_clocks_pre(struct drm_device *dev, struct nouveau_pm_level *perflvl)
 {
@@ -335,6 +379,15 @@ nvc0_pm_clocks_pre(struct drm_device *dev, struct nouveau_pm_level *perflvl)
 		return ERR_PTR(ret);
 	}
 
+	if (perflvl->memory) {
+		ret = calc_mem(dev, &info->mem, perflvl->memory);
+		if (ret) {
+			kfree(info);
+			return ERR_PTR(ret);
+		}
+	}
+
+	info->perflvl = perflvl;
 	return info;
 }
 
@@ -375,12 +428,148 @@ prog_clk(struct drm_device *dev, int clk, struct nvc0_pm_clock *info)
 	nv_mask(dev, 0x137250 + (clk * 0x04), 0x00003f3f, info->mdiv);
 }
 
+static void
+mclk_precharge(struct nouveau_mem_exec_func *exec)
+{
+}
+
+static void
+mclk_refresh(struct nouveau_mem_exec_func *exec)
+{
+}
+
+static void
+mclk_refresh_auto(struct nouveau_mem_exec_func *exec, bool enable)
+{
+	nv_wr32(exec->dev, 0x10f210, enable ? 0x80000000 : 0x00000000);
+}
+
+static void
+mclk_refresh_self(struct nouveau_mem_exec_func *exec, bool enable)
+{
+}
+
+static void
+mclk_wait(struct nouveau_mem_exec_func *exec, u32 nsec)
+{
+	udelay((nsec + 500) / 1000);
+}
+
+static u32
+mclk_mrg(struct nouveau_mem_exec_func *exec, int mr)
+{
+	struct drm_device *dev = exec->dev;
+	struct drm_nouveau_private *dev_priv = dev->dev_private;
+	if (dev_priv->vram_type != NV_MEM_TYPE_GDDR5) {
+		if (mr <= 1)
+			return nv_rd32(dev, 0x10f300 + ((mr - 0) * 4));
+		return nv_rd32(dev, 0x10f320 + ((mr - 2) * 4));
+	} else {
+		if (mr == 0)
+			return nv_rd32(dev, 0x10f300 + (mr * 4));
+		else
+		if (mr <= 7)
+			return nv_rd32(dev, 0x10f32c + (mr * 4));
+		return nv_rd32(dev, 0x10f34c);
+	}
+}
+
+static void
+mclk_mrs(struct nouveau_mem_exec_func *exec, int mr, u32 data)
+{
+	struct drm_device *dev = exec->dev;
+	struct drm_nouveau_private *dev_priv = dev->dev_private;
+	if (dev_priv->vram_type != NV_MEM_TYPE_GDDR5) {
+		if (mr <= 1) {
+			nv_wr32(dev, 0x10f300 + ((mr - 0) * 4), data);
+			if (dev_priv->vram_rank_B)
+				nv_wr32(dev, 0x10f308 + ((mr - 0) * 4), data);
+		} else
+		if (mr <= 3) {
+			nv_wr32(dev, 0x10f320 + ((mr - 2) * 4), data);
+			if (dev_priv->vram_rank_B)
+				nv_wr32(dev, 0x10f328 + ((mr - 2) * 4), data);
+		}
+	} else {
+		if      (mr ==  0) nv_wr32(dev, 0x10f300 + (mr * 4), data);
+		else if (mr <=  7) nv_wr32(dev, 0x10f32c + (mr * 4), data);
+		else if (mr == 15) nv_wr32(dev, 0x10f34c, data);
+	}
+}
+
+static void
+mclk_clock_set(struct nouveau_mem_exec_func *exec)
+{
+	struct nvc0_pm_state *info = exec->priv;
+	struct drm_device *dev = exec->dev;
+	u32 ctrl = nv_rd32(dev, 0x132000);
+
+	nv_wr32(dev, 0x137360, 0x00000001);
+	nv_wr32(dev, 0x137370, 0x00000000);
+	nv_wr32(dev, 0x137380, 0x00000000);
+	if (ctrl & 0x00000001)
+		nv_wr32(dev, 0x132000, (ctrl &= ~0x00000001));
+
+	nv_wr32(dev, 0x132004, info->mem.coef);
+	nv_wr32(dev, 0x132000, (ctrl |= 0x00000001));
+	nv_wait(dev, 0x137390, 0x00000002, 0x00000002);
+	nv_wr32(dev, 0x132018, 0x00005000);
+
+	nv_wr32(dev, 0x137370, 0x00000001);
+	nv_wr32(dev, 0x137380, 0x00000001);
+	nv_wr32(dev, 0x137360, 0x00000000);
+}
+
+static void
+mclk_timing_set(struct nouveau_mem_exec_func *exec)
+{
+	struct nvc0_pm_state *info = exec->priv;
+	struct nouveau_pm_level *perflvl = info->perflvl;
+	int i;
+
+	for (i = 0; i < 5; i++)
+		nv_wr32(exec->dev, 0x10f290 + (i * 4), perflvl->timing.reg[i]);
+}
+
+static void
+prog_mem(struct drm_device *dev, struct nvc0_pm_state *info)
+{
+	struct drm_nouveau_private *dev_priv = dev->dev_private;
+	struct nouveau_mem_exec_func exec = {
+		.dev = dev,
+		.precharge = mclk_precharge,
+		.refresh = mclk_refresh,
+		.refresh_auto = mclk_refresh_auto,
+		.refresh_self = mclk_refresh_self,
+		.wait = mclk_wait,
+		.mrg = mclk_mrg,
+		.mrs = mclk_mrs,
+		.clock_set = mclk_clock_set,
+		.timing_set = mclk_timing_set,
+		.priv = info
+	};
+
+	if (dev_priv->chipset < 0xd0)
+		nv_wr32(dev, 0x611200, 0x00003300);
+	else
+		nv_wr32(dev, 0x62c000, 0x03030000);
+
+	nouveau_mem_exec(&exec, info->perflvl);
+
+	if (dev_priv->chipset < 0xd0)
+		nv_wr32(dev, 0x611200, 0x00003300);
+	else
+		nv_wr32(dev, 0x62c000, 0x03030300);
+}
 int
 nvc0_pm_clocks_set(struct drm_device *dev, void *data)
 {
 	struct nvc0_pm_state *info = data;
 	int i;
 
+	if (info->mem.coef)
+		prog_mem(dev, info);
+
 	for (i = 0; i < 16; i++) {
 		if (!info->eng[i].freq)
 			continue;
diff --git a/drivers/gpu/drm/nouveau/nvc0_software.c b/drivers/gpu/drm/nouveau/nvc0_software.c
new file mode 100644
index 00000000000..93e8c164fec
--- /dev/null
+++ b/drivers/gpu/drm/nouveau/nvc0_software.c
@@ -0,0 +1,153 @@
+/*
+ * 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 "drmP.h"
+
+#include "nouveau_drv.h"
+#include "nouveau_ramht.h"
+#include "nouveau_software.h"
+
+#include "nv50_display.h"
+
+struct nvc0_software_priv {
+	struct nouveau_software_priv base;
+};
+
+struct nvc0_software_chan {
+	struct nouveau_software_chan base;
+	struct nouveau_vma dispc_vma[4];
+};
+
+u64
+nvc0_software_crtc(struct nouveau_channel *chan, int crtc)
+{
+	struct nvc0_software_chan *pch = chan->engctx[NVOBJ_ENGINE_SW];
+	return pch->dispc_vma[crtc].offset;
+}
+
+static int
+nvc0_software_context_new(struct nouveau_channel *chan, int engine)
+{
+	struct drm_device *dev = chan->dev;
+	struct drm_nouveau_private *dev_priv = dev->dev_private;
+	struct nvc0_software_priv *psw = nv_engine(dev, NVOBJ_ENGINE_SW);
+	struct nvc0_software_chan *pch;
+	int ret = 0, i;
+
+	pch = kzalloc(sizeof(*pch), GFP_KERNEL);
+	if (!pch)
+		return -ENOMEM;
+
+	nouveau_software_context_new(&pch->base);
+	chan->engctx[engine] = pch;
+
+	/* map display semaphore buffers into channel's vm */
+	for (i = 0; !ret && i < dev->mode_config.num_crtc; i++) {
+		struct nouveau_bo *bo;
+		if (dev_priv->card_type >= NV_D0)
+			bo = nvd0_display_crtc_sema(dev, i);
+		else
+			bo = nv50_display(dev)->crtc[i].sem.bo;
+
+		ret = nouveau_bo_vma_add(bo, chan->vm, &pch->dispc_vma[i]);
+	}
+
+	if (ret)
+		psw->base.base.context_del(chan, engine);
+	return ret;
+}
+
+static void
+nvc0_software_context_del(struct nouveau_channel *chan, int engine)
+{
+	struct drm_device *dev = chan->dev;
+	struct drm_nouveau_private *dev_priv = dev->dev_private;
+	struct nvc0_software_chan *pch = chan->engctx[engine];
+	int i;
+
+	if (dev_priv->card_type >= NV_D0) {
+		for (i = 0; i < dev->mode_config.num_crtc; i++) {
+			struct nouveau_bo *bo = nvd0_display_crtc_sema(dev, i);
+			nouveau_bo_vma_del(bo, &pch->dispc_vma[i]);
+		}
+	} else
+	if (dev_priv->card_type >= NV_50) {
+		struct nv50_display *disp = nv50_display(dev);
+		for (i = 0; i < dev->mode_config.num_crtc; i++) {
+			struct nv50_display_crtc *dispc = &disp->crtc[i];
+			nouveau_bo_vma_del(dispc->sem.bo, &pch->dispc_vma[i]);
+		}
+	}
+
+	chan->engctx[engine] = NULL;
+	kfree(pch);
+}
+
+static int
+nvc0_software_object_new(struct nouveau_channel *chan, int engine,
+			 u32 handle, u16 class)
+{
+	return 0;
+}
+
+static int
+nvc0_software_init(struct drm_device *dev, int engine)
+{
+	return 0;
+}
+
+static int
+nvc0_software_fini(struct drm_device *dev, int engine, bool suspend)
+{
+	return 0;
+}
+
+static void
+nvc0_software_destroy(struct drm_device *dev, int engine)
+{
+	struct nvc0_software_priv *psw = nv_engine(dev, engine);
+
+	NVOBJ_ENGINE_DEL(dev, SW);
+	kfree(psw);
+}
+
+int
+nvc0_software_create(struct drm_device *dev)
+{
+	struct nvc0_software_priv *psw = kzalloc(sizeof(*psw), GFP_KERNEL);
+	if (!psw)
+		return -ENOMEM;
+
+	psw->base.base.destroy = nvc0_software_destroy;
+	psw->base.base.init = nvc0_software_init;
+	psw->base.base.fini = nvc0_software_fini;
+	psw->base.base.context_new = nvc0_software_context_new;
+	psw->base.base.context_del = nvc0_software_context_del;
+	psw->base.base.object_new = nvc0_software_object_new;
+	nouveau_software_create(&psw->base);
+
+	NVOBJ_ENGINE_ADD(dev, SW, &psw->base.base);
+	NVOBJ_CLASS(dev, 0x906e, SW);
+	return 0;
+}
diff --git a/drivers/gpu/drm/nouveau/nvd0_display.c b/drivers/gpu/drm/nouveau/nvd0_display.c
index 0247250939e..c486d3ce3c2 100644
--- a/drivers/gpu/drm/nouveau/nvd0_display.c
+++ b/drivers/gpu/drm/nouveau/nvd0_display.c
@@ -33,6 +33,7 @@
 #include "nouveau_crtc.h"
 #include "nouveau_dma.h"
 #include "nouveau_fb.h"
+#include "nouveau_software.h"
 #include "nv50_display.h"
 
 #define EVO_DMA_NR 9
@@ -284,8 +285,6 @@ nvd0_display_flip_next(struct drm_crtc *crtc, struct drm_framebuffer *fb,
 	u32 *push;
 	int ret;
 
-	evo_sync(crtc->dev, EVO_MASTER);
-
 	swap_interval <<= 4;
 	if (swap_interval == 0)
 		swap_interval |= 0x100;
@@ -300,15 +299,16 @@ nvd0_display_flip_next(struct drm_crtc *crtc, struct drm_framebuffer *fb,
 		if (ret)
 			return ret;
 
-		offset  = chan->dispc_vma[nv_crtc->index].offset;
+
+		offset  = nvc0_software_crtc(chan, nv_crtc->index);
 		offset += evo->sem.offset;
 
-		BEGIN_NVC0(chan, 2, 0, NV84_SUBCHAN_SEMAPHORE_ADDRESS_HIGH, 4);
+		BEGIN_NVC0(chan, 0, NV84_SUBCHAN_SEMAPHORE_ADDRESS_HIGH, 4);
 		OUT_RING  (chan, upper_32_bits(offset));
 		OUT_RING  (chan, lower_32_bits(offset));
 		OUT_RING  (chan, 0xf00d0000 | evo->sem.value);
 		OUT_RING  (chan, 0x1002);
-		BEGIN_NVC0(chan, 2, 0, NV84_SUBCHAN_SEMAPHORE_ADDRESS_HIGH, 4);
+		BEGIN_NVC0(chan, 0, NV84_SUBCHAN_SEMAPHORE_ADDRESS_HIGH, 4);
 		OUT_RING  (chan, upper_32_bits(offset));
 		OUT_RING  (chan, lower_32_bits(offset ^ 0x10));
 		OUT_RING  (chan, 0x74b1e000);
@@ -882,7 +882,7 @@ nvd0_crtc_create(struct drm_device *dev, int index)
 	drm_mode_crtc_set_gamma_size(crtc, 256);
 
 	ret = nouveau_bo_new(dev, 64 * 64 * 4, 0x100, TTM_PL_FLAG_VRAM,
-			     0, 0x0000, &nv_crtc->cursor.nvbo);
+			     0, 0x0000, NULL, &nv_crtc->cursor.nvbo);
 	if (!ret) {
 		ret = nouveau_bo_pin(nv_crtc->cursor.nvbo, TTM_PL_FLAG_VRAM);
 		if (!ret)
@@ -895,7 +895,7 @@ nvd0_crtc_create(struct drm_device *dev, int index)
 		goto out;
 
 	ret = nouveau_bo_new(dev, 8192, 0x100, TTM_PL_FLAG_VRAM,
-			     0, 0x0000, &nv_crtc->lut.nvbo);
+			     0, 0x0000, NULL, &nv_crtc->lut.nvbo);
 	if (!ret) {
 		ret = nouveau_bo_pin(nv_crtc->lut.nvbo, TTM_PL_FLAG_VRAM);
 		if (!ret)
@@ -2030,7 +2030,7 @@ nvd0_display_create(struct drm_device *dev)
 
 	/* small shared memory area we use for notifiers and semaphores */
 	ret = nouveau_bo_new(dev, 4096, 0x1000, TTM_PL_FLAG_VRAM,
-			     0, 0x0000, &disp->sync);
+			     0, 0x0000, NULL, &disp->sync);
 	if (!ret) {
 		ret = nouveau_bo_pin(disp->sync, TTM_PL_FLAG_VRAM);
 		if (!ret)
diff --git a/drivers/gpu/drm/nouveau/nve0_fifo.c b/drivers/gpu/drm/nouveau/nve0_fifo.c
new file mode 100644
index 00000000000..1855ecbd843
--- /dev/null
+++ b/drivers/gpu/drm/nouveau/nve0_fifo.c
@@ -0,0 +1,423 @@
+/*
+ * Copyright 2010 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 "drmP.h"
+
+#include "nouveau_drv.h"
+#include "nouveau_mm.h"
+#include "nouveau_fifo.h"
+
+#define NVE0_FIFO_ENGINE_NUM 32
+
+static void nve0_fifo_isr(struct drm_device *);
+
+struct nve0_fifo_engine {
+	struct nouveau_gpuobj *playlist[2];
+	int cur_playlist;
+};
+
+struct nve0_fifo_priv {
+	struct nouveau_fifo_priv base;
+	struct nve0_fifo_engine engine[NVE0_FIFO_ENGINE_NUM];
+	struct {
+		struct nouveau_gpuobj *mem;
+		struct nouveau_vma bar;
+	} user;
+	int spoon_nr;
+};
+
+struct nve0_fifo_chan {
+	struct nouveau_fifo_chan base;
+	u32 engine;
+};
+
+static void
+nve0_fifo_playlist_update(struct drm_device *dev, u32 engine)
+{
+	struct drm_nouveau_private *dev_priv = dev->dev_private;
+	struct nouveau_instmem_engine *pinstmem = &dev_priv->engine.instmem;
+	struct nve0_fifo_priv *priv = nv_engine(dev, NVOBJ_ENGINE_FIFO);
+	struct nve0_fifo_engine *peng = &priv->engine[engine];
+	struct nouveau_gpuobj *cur;
+	u32 match = (engine << 16) | 0x00000001;
+	int ret, i, p;
+
+	cur = peng->playlist[peng->cur_playlist];
+	if (unlikely(cur == NULL)) {
+		ret = nouveau_gpuobj_new(dev, NULL, 0x8000, 0x1000, 0, &cur);
+		if (ret) {
+			NV_ERROR(dev, "PFIFO: playlist alloc failed\n");
+			return;
+		}
+
+		peng->playlist[peng->cur_playlist] = cur;
+	}
+
+	peng->cur_playlist = !peng->cur_playlist;
+
+	for (i = 0, p = 0; i < priv->base.channels; i++) {
+		u32 ctrl = nv_rd32(dev, 0x800004 + (i * 8)) & 0x001f0001;
+		if (ctrl != match)
+			continue;
+		nv_wo32(cur, p + 0, i);
+		nv_wo32(cur, p + 4, 0x00000000);
+		p += 8;
+	}
+	pinstmem->flush(dev);
+
+	nv_wr32(dev, 0x002270, cur->vinst >> 12);
+	nv_wr32(dev, 0x002274, (engine << 20) | (p >> 3));
+	if (!nv_wait(dev, 0x002284 + (engine * 4), 0x00100000, 0x00000000))
+		NV_ERROR(dev, "PFIFO: playlist %d update timeout\n", engine);
+}
+
+static int
+nve0_fifo_context_new(struct nouveau_channel *chan, int engine)
+{
+	struct drm_device *dev = chan->dev;
+	struct drm_nouveau_private *dev_priv = dev->dev_private;
+	struct nouveau_instmem_engine *pinstmem = &dev_priv->engine.instmem;
+	struct nve0_fifo_priv *priv = nv_engine(dev, engine);
+	struct nve0_fifo_chan *fctx;
+	u64 usermem = priv->user.mem->vinst + chan->id * 512;
+	u64 ib_virt = chan->pushbuf_base + chan->dma.ib_base * 4;
+	int ret = 0, i;
+
+	fctx = chan->engctx[engine] = kzalloc(sizeof(*fctx), GFP_KERNEL);
+	if (!fctx)
+		return -ENOMEM;
+
+	fctx->engine = 0; /* PGRAPH */
+
+	/* allocate vram for control regs, map into polling area */
+	chan->user = ioremap_wc(pci_resource_start(dev->pdev, 1) +
+				priv->user.bar.offset + (chan->id * 512), 512);
+	if (!chan->user) {
+		ret = -ENOMEM;
+		goto error;
+	}
+
+	for (i = 0; i < 0x100; i += 4)
+		nv_wo32(chan->ramin, i, 0x00000000);
+	nv_wo32(chan->ramin, 0x08, lower_32_bits(usermem));
+	nv_wo32(chan->ramin, 0x0c, upper_32_bits(usermem));
+	nv_wo32(chan->ramin, 0x10, 0x0000face);
+	nv_wo32(chan->ramin, 0x30, 0xfffff902);
+	nv_wo32(chan->ramin, 0x48, lower_32_bits(ib_virt));
+	nv_wo32(chan->ramin, 0x4c, drm_order(chan->dma.ib_max + 1) << 16 |
+				     upper_32_bits(ib_virt));
+	nv_wo32(chan->ramin, 0x84, 0x20400000);
+	nv_wo32(chan->ramin, 0x94, 0x30000001);
+	nv_wo32(chan->ramin, 0x9c, 0x00000100);
+	nv_wo32(chan->ramin, 0xac, 0x0000001f);
+	nv_wo32(chan->ramin, 0xe4, 0x00000000);
+	nv_wo32(chan->ramin, 0xe8, chan->id);
+	nv_wo32(chan->ramin, 0xf8, 0x10003080); /* 0x002310 */
+	nv_wo32(chan->ramin, 0xfc, 0x10000010); /* 0x002350 */
+	pinstmem->flush(dev);
+
+	nv_wr32(dev, 0x800000 + (chan->id * 8), 0x80000000 |
+						(chan->ramin->vinst >> 12));
+	nv_mask(dev, 0x800004 + (chan->id * 8), 0x00000400, 0x00000400);
+	nve0_fifo_playlist_update(dev, fctx->engine);
+	nv_mask(dev, 0x800004 + (chan->id * 8), 0x00000400, 0x00000400);
+
+error:
+	if (ret)
+		priv->base.base.context_del(chan, engine);
+	return ret;
+}
+
+static void
+nve0_fifo_context_del(struct nouveau_channel *chan, int engine)
+{
+	struct nve0_fifo_chan *fctx = chan->engctx[engine];
+	struct drm_device *dev = chan->dev;
+
+	nv_mask(dev, 0x800004 + (chan->id * 8), 0x00000800, 0x00000800);
+	nv_wr32(dev, 0x002634, chan->id);
+	if (!nv_wait(dev, 0x0002634, 0xffffffff, chan->id))
+		NV_WARN(dev, "0x2634 != chid: 0x%08x\n", nv_rd32(dev, 0x2634));
+	nve0_fifo_playlist_update(dev, fctx->engine);
+	nv_wr32(dev, 0x800000 + (chan->id * 8), 0x00000000);
+
+	if (chan->user) {
+		iounmap(chan->user);
+		chan->user = NULL;
+	}
+
+	chan->engctx[NVOBJ_ENGINE_FIFO] = NULL;
+	kfree(fctx);
+}
+
+static int
+nve0_fifo_init(struct drm_device *dev, int engine)
+{
+	struct drm_nouveau_private *dev_priv = dev->dev_private;
+	struct nve0_fifo_priv *priv = nv_engine(dev, engine);
+	struct nve0_fifo_chan *fctx;
+	int i;
+
+	/* reset PFIFO, enable all available PSUBFIFO areas */
+	nv_mask(dev, 0x000200, 0x00000100, 0x00000000);
+	nv_mask(dev, 0x000200, 0x00000100, 0x00000100);
+	nv_wr32(dev, 0x000204, 0xffffffff);
+
+	priv->spoon_nr = hweight32(nv_rd32(dev, 0x000204));
+	NV_DEBUG(dev, "PFIFO: %d subfifo(s)\n", priv->spoon_nr);
+
+	/* PSUBFIFO[n] */
+	for (i = 0; i < priv->spoon_nr; i++) {
+		nv_mask(dev, 0x04013c + (i * 0x2000), 0x10000100, 0x00000000);
+		nv_wr32(dev, 0x040108 + (i * 0x2000), 0xffffffff); /* INTR */
+		nv_wr32(dev, 0x04010c + (i * 0x2000), 0xfffffeff); /* INTR_EN */
+	}
+
+	nv_wr32(dev, 0x002254, 0x10000000 | priv->user.bar.offset >> 12);
+
+	nv_wr32(dev, 0x002a00, 0xffffffff);
+	nv_wr32(dev, 0x002100, 0xffffffff);
+	nv_wr32(dev, 0x002140, 0xbfffffff);
+
+	/* restore PFIFO context table */
+	for (i = 0; i < priv->base.channels; i++) {
+		struct nouveau_channel *chan = dev_priv->channels.ptr[i];
+		if (!chan || !(fctx = chan->engctx[engine]))
+			continue;
+
+		nv_wr32(dev, 0x800000 + (i * 8), 0x80000000 |
+						 (chan->ramin->vinst >> 12));
+		nv_mask(dev, 0x800004 + (i * 8), 0x00000400, 0x00000400);
+		nve0_fifo_playlist_update(dev, fctx->engine);
+		nv_mask(dev, 0x800004 + (i * 8), 0x00000400, 0x00000400);
+	}
+
+	return 0;
+}
+
+static int
+nve0_fifo_fini(struct drm_device *dev, int engine, bool suspend)
+{
+	struct nve0_fifo_priv *priv = nv_engine(dev, engine);
+	int i;
+
+	for (i = 0; i < priv->base.channels; i++) {
+		if (!(nv_rd32(dev, 0x800004 + (i * 8)) & 1))
+			continue;
+
+		nv_mask(dev, 0x800004 + (i * 8), 0x00000800, 0x00000800);
+		nv_wr32(dev, 0x002634, i);
+		if (!nv_wait(dev, 0x002634, 0xffffffff, i)) {
+			NV_INFO(dev, "PFIFO: kick ch %d failed: 0x%08x\n",
+				i, nv_rd32(dev, 0x002634));
+			return -EBUSY;
+		}
+	}
+
+	nv_wr32(dev, 0x002140, 0x00000000);
+	return 0;
+}
+
+struct nouveau_enum nve0_fifo_fault_unit[] = {
+	{}
+};
+
+struct nouveau_enum nve0_fifo_fault_reason[] = {
+	{ 0x00, "PT_NOT_PRESENT" },
+	{ 0x01, "PT_TOO_SHORT" },
+	{ 0x02, "PAGE_NOT_PRESENT" },
+	{ 0x03, "VM_LIMIT_EXCEEDED" },
+	{ 0x04, "NO_CHANNEL" },
+	{ 0x05, "PAGE_SYSTEM_ONLY" },
+	{ 0x06, "PAGE_READ_ONLY" },
+	{ 0x0a, "COMPRESSED_SYSRAM" },
+	{ 0x0c, "INVALID_STORAGE_TYPE" },
+	{}
+};
+
+struct nouveau_enum nve0_fifo_fault_hubclient[] = {
+	{}
+};
+
+struct nouveau_enum nve0_fifo_fault_gpcclient[] = {
+	{}
+};
+
+struct nouveau_bitfield nve0_fifo_subfifo_intr[] = {
+	{ 0x00200000, "ILLEGAL_MTHD" },
+	{ 0x00800000, "EMPTY_SUBC" },
+	{}
+};
+
+static void
+nve0_fifo_isr_vm_fault(struct drm_device *dev, int unit)
+{
+	u32 inst = nv_rd32(dev, 0x2800 + (unit * 0x10));
+	u32 valo = nv_rd32(dev, 0x2804 + (unit * 0x10));
+	u32 vahi = nv_rd32(dev, 0x2808 + (unit * 0x10));
+	u32 stat = nv_rd32(dev, 0x280c + (unit * 0x10));
+	u32 client = (stat & 0x00001f00) >> 8;
+
+	NV_INFO(dev, "PFIFO: %s fault at 0x%010llx [",
+		(stat & 0x00000080) ? "write" : "read", (u64)vahi << 32 | valo);
+	nouveau_enum_print(nve0_fifo_fault_reason, stat & 0x0000000f);
+	printk("] from ");
+	nouveau_enum_print(nve0_fifo_fault_unit, unit);
+	if (stat & 0x00000040) {
+		printk("/");
+		nouveau_enum_print(nve0_fifo_fault_hubclient, client);
+	} else {
+		printk("/GPC%d/", (stat & 0x1f000000) >> 24);
+		nouveau_enum_print(nve0_fifo_fault_gpcclient, client);
+	}
+	printk(" on channel 0x%010llx\n", (u64)inst << 12);
+}
+
+static void
+nve0_fifo_isr_subfifo_intr(struct drm_device *dev, int unit)
+{
+	u32 stat = nv_rd32(dev, 0x040108 + (unit * 0x2000));
+	u32 addr = nv_rd32(dev, 0x0400c0 + (unit * 0x2000));
+	u32 data = nv_rd32(dev, 0x0400c4 + (unit * 0x2000));
+	u32 chid = nv_rd32(dev, 0x040120 + (unit * 0x2000)) & 0x7f;
+	u32 subc = (addr & 0x00070000);
+	u32 mthd = (addr & 0x00003ffc);
+
+	NV_INFO(dev, "PSUBFIFO %d:", unit);
+	nouveau_bitfield_print(nve0_fifo_subfifo_intr, stat);
+	NV_INFO(dev, "PSUBFIFO %d: ch %d subc %d mthd 0x%04x data 0x%08x\n",
+		unit, chid, subc, mthd, data);
+
+	nv_wr32(dev, 0x0400c0 + (unit * 0x2000), 0x80600008);
+	nv_wr32(dev, 0x040108 + (unit * 0x2000), stat);
+}
+
+static void
+nve0_fifo_isr(struct drm_device *dev)
+{
+	u32 stat = nv_rd32(dev, 0x002100);
+
+	if (stat & 0x00000100) {
+		NV_INFO(dev, "PFIFO: unknown status 0x00000100\n");
+		nv_wr32(dev, 0x002100, 0x00000100);
+		stat &= ~0x00000100;
+	}
+
+	if (stat & 0x10000000) {
+		u32 units = nv_rd32(dev, 0x00259c);
+		u32 u = units;
+
+		while (u) {
+			int i = ffs(u) - 1;
+			nve0_fifo_isr_vm_fault(dev, i);
+			u &= ~(1 << i);
+		}
+
+		nv_wr32(dev, 0x00259c, units);
+		stat &= ~0x10000000;
+	}
+
+	if (stat & 0x20000000) {
+		u32 units = nv_rd32(dev, 0x0025a0);
+		u32 u = units;
+
+		while (u) {
+			int i = ffs(u) - 1;
+			nve0_fifo_isr_subfifo_intr(dev, i);
+			u &= ~(1 << i);
+		}
+
+		nv_wr32(dev, 0x0025a0, units);
+		stat &= ~0x20000000;
+	}
+
+	if (stat & 0x40000000) {
+		NV_INFO(dev, "PFIFO: unknown status 0x40000000\n");
+		nv_mask(dev, 0x002a00, 0x00000000, 0x00000000);
+		stat &= ~0x40000000;
+	}
+
+	if (stat) {
+		NV_INFO(dev, "PFIFO: unhandled status 0x%08x\n", stat);
+		nv_wr32(dev, 0x002100, stat);
+		nv_wr32(dev, 0x002140, 0);
+	}
+}
+
+static void
+nve0_fifo_destroy(struct drm_device *dev, int engine)
+{
+	struct drm_nouveau_private *dev_priv = dev->dev_private;
+	struct nve0_fifo_priv *priv = nv_engine(dev, engine);
+	int i;
+
+	nouveau_vm_put(&priv->user.bar);
+	nouveau_gpuobj_ref(NULL, &priv->user.mem);
+
+	for (i = 0; i < NVE0_FIFO_ENGINE_NUM; i++) {
+		nouveau_gpuobj_ref(NULL, &priv->engine[i].playlist[0]);
+		nouveau_gpuobj_ref(NULL, &priv->engine[i].playlist[1]);
+	}
+
+	dev_priv->eng[engine] = NULL;
+	kfree(priv);
+}
+
+int
+nve0_fifo_create(struct drm_device *dev)
+{
+	struct drm_nouveau_private *dev_priv = dev->dev_private;
+	struct nve0_fifo_priv *priv;
+	int ret;
+
+	priv = kzalloc(sizeof(*priv), GFP_KERNEL);
+	if (!priv)
+		return -ENOMEM;
+
+	priv->base.base.destroy = nve0_fifo_destroy;
+	priv->base.base.init = nve0_fifo_init;
+	priv->base.base.fini = nve0_fifo_fini;
+	priv->base.base.context_new = nve0_fifo_context_new;
+	priv->base.base.context_del = nve0_fifo_context_del;
+	priv->base.channels = 4096;
+	dev_priv->eng[NVOBJ_ENGINE_FIFO] = &priv->base.base;
+
+	ret = nouveau_gpuobj_new(dev, NULL, priv->base.channels * 512, 0x1000,
+				 NVOBJ_FLAG_ZERO_ALLOC, &priv->user.mem);
+	if (ret)
+		goto error;
+
+	ret = nouveau_vm_get(dev_priv->bar1_vm, priv->user.mem->size,
+			     12, NV_MEM_ACCESS_RW, &priv->user.bar);
+	if (ret)
+		goto error;
+
+	nouveau_vm_map(&priv->user.bar, *(struct nouveau_mem **)priv->user.mem->node);
+
+	nouveau_irq_register(dev, 8, nve0_fifo_isr);
+error:
+	if (ret)
+		priv->base.base.destroy(dev, NVOBJ_ENGINE_FIFO);
+	return ret;
+}
diff --git a/drivers/gpu/drm/nouveau/nve0_graph.c b/drivers/gpu/drm/nouveau/nve0_graph.c
new file mode 100644
index 00000000000..8a8051b68f1
--- /dev/null
+++ b/drivers/gpu/drm/nouveau/nve0_graph.c
@@ -0,0 +1,831 @@
+/*
+ * Copyright 2010 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 <linux/firmware.h>
+#include <linux/module.h>
+
+#include "drmP.h"
+
+#include "nouveau_drv.h"
+#include "nouveau_mm.h"
+#include "nouveau_fifo.h"
+
+#include "nve0_graph.h"
+
+static void
+nve0_graph_ctxctl_debug_unit(struct drm_device *dev, u32 base)
+{
+	NV_INFO(dev, "PGRAPH: %06x - done 0x%08x\n", base,
+		nv_rd32(dev, base + 0x400));
+	NV_INFO(dev, "PGRAPH: %06x - stat 0x%08x 0x%08x 0x%08x 0x%08x\n", base,
+		nv_rd32(dev, base + 0x800), nv_rd32(dev, base + 0x804),
+		nv_rd32(dev, base + 0x808), nv_rd32(dev, base + 0x80c));
+	NV_INFO(dev, "PGRAPH: %06x - stat 0x%08x 0x%08x 0x%08x 0x%08x\n", base,
+		nv_rd32(dev, base + 0x810), nv_rd32(dev, base + 0x814),
+		nv_rd32(dev, base + 0x818), nv_rd32(dev, base + 0x81c));
+}
+
+static void
+nve0_graph_ctxctl_debug(struct drm_device *dev)
+{
+	u32 gpcnr = nv_rd32(dev, 0x409604) & 0xffff;
+	u32 gpc;
+
+	nve0_graph_ctxctl_debug_unit(dev, 0x409000);
+	for (gpc = 0; gpc < gpcnr; gpc++)
+		nve0_graph_ctxctl_debug_unit(dev, 0x502000 + (gpc * 0x8000));
+}
+
+static int
+nve0_graph_load_context(struct nouveau_channel *chan)
+{
+	struct drm_device *dev = chan->dev;
+
+	nv_wr32(dev, 0x409840, 0x00000030);
+	nv_wr32(dev, 0x409500, 0x80000000 | chan->ramin->vinst >> 12);
+	nv_wr32(dev, 0x409504, 0x00000003);
+	if (!nv_wait(dev, 0x409800, 0x00000010, 0x00000010))
+		NV_ERROR(dev, "PGRAPH: load_ctx timeout\n");
+
+	return 0;
+}
+
+static int
+nve0_graph_unload_context_to(struct drm_device *dev, u64 chan)
+{
+	nv_wr32(dev, 0x409840, 0x00000003);
+	nv_wr32(dev, 0x409500, 0x80000000 | chan >> 12);
+	nv_wr32(dev, 0x409504, 0x00000009);
+	if (!nv_wait(dev, 0x409800, 0x00000001, 0x00000000)) {
+		NV_ERROR(dev, "PGRAPH: unload_ctx timeout\n");
+		return -EBUSY;
+	}
+
+	return 0;
+}
+
+static int
+nve0_graph_construct_context(struct nouveau_channel *chan)
+{
+	struct drm_nouveau_private *dev_priv = chan->dev->dev_private;
+	struct nve0_graph_priv *priv = nv_engine(chan->dev, NVOBJ_ENGINE_GR);
+	struct nve0_graph_chan *grch = chan->engctx[NVOBJ_ENGINE_GR];
+	struct drm_device *dev = chan->dev;
+	int ret, i;
+	u32 *ctx;
+
+	ctx = kmalloc(priv->grctx_size, GFP_KERNEL);
+	if (!ctx)
+		return -ENOMEM;
+
+	nve0_graph_load_context(chan);
+
+	nv_wo32(grch->grctx, 0x1c, 1);
+	nv_wo32(grch->grctx, 0x20, 0);
+	nv_wo32(grch->grctx, 0x28, 0);
+	nv_wo32(grch->grctx, 0x2c, 0);
+	dev_priv->engine.instmem.flush(dev);
+
+	ret = nve0_grctx_generate(chan);
+	if (ret)
+		goto err;
+
+	ret = nve0_graph_unload_context_to(dev, chan->ramin->vinst);
+	if (ret)
+		goto err;
+
+	for (i = 0; i < priv->grctx_size; i += 4)
+		ctx[i / 4] = nv_ro32(grch->grctx, i);
+
+	priv->grctx_vals = ctx;
+	return 0;
+
+err:
+	kfree(ctx);
+	return ret;
+}
+
+static int
+nve0_graph_create_context_mmio_list(struct nouveau_channel *chan)
+{
+	struct nve0_graph_priv *priv = nv_engine(chan->dev, NVOBJ_ENGINE_GR);
+	struct nve0_graph_chan *grch = chan->engctx[NVOBJ_ENGINE_GR];
+	struct drm_device *dev = chan->dev;
+	u32 magic[GPC_MAX][2];
+	u16 offset = 0x0000;
+	int gpc;
+	int ret;
+
+	ret = nouveau_gpuobj_new(dev, chan, 0x3000, 256, NVOBJ_FLAG_VM,
+				 &grch->unk408004);
+	if (ret)
+		return ret;
+
+	ret = nouveau_gpuobj_new(dev, chan, 0x8000, 256, NVOBJ_FLAG_VM,
+				 &grch->unk40800c);
+	if (ret)
+		return ret;
+
+	ret = nouveau_gpuobj_new(dev, chan, 384 * 1024, 4096,
+				 NVOBJ_FLAG_VM | NVOBJ_FLAG_VM_USER,
+				 &grch->unk418810);
+	if (ret)
+		return ret;
+
+	ret = nouveau_gpuobj_new(dev, chan, 0x1000, 0, NVOBJ_FLAG_VM,
+				 &grch->mmio);
+	if (ret)
+		return ret;
+
+#define mmio(r,v) do {                                                         \
+	nv_wo32(grch->mmio, (grch->mmio_nr * 8) + 0, (r));                     \
+	nv_wo32(grch->mmio, (grch->mmio_nr * 8) + 4, (v));                     \
+	grch->mmio_nr++;                                                       \
+} while (0)
+	mmio(0x40800c, grch->unk40800c->linst >> 8);
+	mmio(0x408010, 0x80000000);
+	mmio(0x419004, grch->unk40800c->linst >> 8);
+	mmio(0x419008, 0x00000000);
+	mmio(0x4064cc, 0x80000000);
+	mmio(0x408004, grch->unk408004->linst >> 8);
+	mmio(0x408008, 0x80000030);
+	mmio(0x418808, grch->unk408004->linst >> 8);
+	mmio(0x41880c, 0x80000030);
+	mmio(0x4064c8, 0x01800600);
+	mmio(0x418810, 0x80000000 | grch->unk418810->linst >> 12);
+	mmio(0x419848, 0x10000000 | grch->unk418810->linst >> 12);
+	mmio(0x405830, 0x02180648);
+	mmio(0x4064c4, 0x0192ffff);
+
+	for (gpc = 0; gpc < priv->gpc_nr; gpc++) {
+		u16 magic0 = 0x0218 * priv->tpc_nr[gpc];
+		u16 magic1 = 0x0648 * priv->tpc_nr[gpc];
+		magic[gpc][0]  = 0x10000000 | (magic0 << 16) | offset;
+		magic[gpc][1]  = 0x00000000 | (magic1 << 16);
+		offset += 0x0324 * priv->tpc_nr[gpc];
+	}
+
+	for (gpc = 0; gpc < priv->gpc_nr; gpc++) {
+		mmio(GPC_UNIT(gpc, 0x30c0), magic[gpc][0]);
+		mmio(GPC_UNIT(gpc, 0x30e4), magic[gpc][1] | offset);
+		offset += 0x07ff * priv->tpc_nr[gpc];
+	}
+
+	mmio(0x17e91c, 0x06060609);
+	mmio(0x17e920, 0x00090a05);
+#undef mmio
+	return 0;
+}
+
+static int
+nve0_graph_context_new(struct nouveau_channel *chan, int engine)
+{
+	struct drm_device *dev = chan->dev;
+	struct drm_nouveau_private *dev_priv = dev->dev_private;
+	struct nouveau_instmem_engine *pinstmem = &dev_priv->engine.instmem;
+	struct nve0_graph_priv *priv = nv_engine(dev, engine);
+	struct nve0_graph_chan *grch;
+	struct nouveau_gpuobj *grctx;
+	int ret, i;
+
+	grch = kzalloc(sizeof(*grch), GFP_KERNEL);
+	if (!grch)
+		return -ENOMEM;
+	chan->engctx[NVOBJ_ENGINE_GR] = grch;
+
+	ret = nouveau_gpuobj_new(dev, chan, priv->grctx_size, 256,
+				 NVOBJ_FLAG_VM | NVOBJ_FLAG_ZERO_ALLOC,
+				 &grch->grctx);
+	if (ret)
+		goto error;
+	grctx = grch->grctx;
+
+	ret = nve0_graph_create_context_mmio_list(chan);
+	if (ret)
+		goto error;
+
+	nv_wo32(chan->ramin, 0x0210, lower_32_bits(grctx->linst) | 4);
+	nv_wo32(chan->ramin, 0x0214, upper_32_bits(grctx->linst));
+	pinstmem->flush(dev);
+
+	if (!priv->grctx_vals) {
+		ret = nve0_graph_construct_context(chan);
+		if (ret)
+			goto error;
+	}
+
+	for (i = 0; i < priv->grctx_size; i += 4)
+		nv_wo32(grctx, i, priv->grctx_vals[i / 4]);
+	nv_wo32(grctx, 0xf4, 0);
+	nv_wo32(grctx, 0xf8, 0);
+	nv_wo32(grctx, 0x10, grch->mmio_nr);
+	nv_wo32(grctx, 0x14, lower_32_bits(grch->mmio->linst));
+	nv_wo32(grctx, 0x18, upper_32_bits(grch->mmio->linst));
+	nv_wo32(grctx, 0x1c, 1);
+	nv_wo32(grctx, 0x20, 0);
+	nv_wo32(grctx, 0x28, 0);
+	nv_wo32(grctx, 0x2c, 0);
+
+	pinstmem->flush(dev);
+	return 0;
+
+error:
+	priv->base.context_del(chan, engine);
+	return ret;
+}
+
+static void
+nve0_graph_context_del(struct nouveau_channel *chan, int engine)
+{
+	struct nve0_graph_chan *grch = chan->engctx[engine];
+
+	nouveau_gpuobj_ref(NULL, &grch->mmio);
+	nouveau_gpuobj_ref(NULL, &grch->unk418810);
+	nouveau_gpuobj_ref(NULL, &grch->unk40800c);
+	nouveau_gpuobj_ref(NULL, &grch->unk408004);
+	nouveau_gpuobj_ref(NULL, &grch->grctx);
+	chan->engctx[engine] = NULL;
+}
+
+static int
+nve0_graph_object_new(struct nouveau_channel *chan, int engine,
+		      u32 handle, u16 class)
+{
+	return 0;
+}
+
+static int
+nve0_graph_fini(struct drm_device *dev, int engine, bool suspend)
+{
+	return 0;
+}
+
+static void
+nve0_graph_init_obj418880(struct drm_device *dev)
+{
+	struct nve0_graph_priv *priv = nv_engine(dev, NVOBJ_ENGINE_GR);
+	int i;
+
+	nv_wr32(dev, GPC_BCAST(0x0880), 0x00000000);
+	nv_wr32(dev, GPC_BCAST(0x08a4), 0x00000000);
+	for (i = 0; i < 4; i++)
+		nv_wr32(dev, GPC_BCAST(0x0888) + (i * 4), 0x00000000);
+	nv_wr32(dev, GPC_BCAST(0x08b4), priv->unk4188b4->vinst >> 8);
+	nv_wr32(dev, GPC_BCAST(0x08b8), priv->unk4188b8->vinst >> 8);
+}
+
+static void
+nve0_graph_init_regs(struct drm_device *dev)
+{
+	nv_wr32(dev, 0x400080, 0x003083c2);
+	nv_wr32(dev, 0x400088, 0x0001ffe7);
+	nv_wr32(dev, 0x40008c, 0x00000000);
+	nv_wr32(dev, 0x400090, 0x00000030);
+	nv_wr32(dev, 0x40013c, 0x003901f7);
+	nv_wr32(dev, 0x400140, 0x00000100);
+	nv_wr32(dev, 0x400144, 0x00000000);
+	nv_wr32(dev, 0x400148, 0x00000110);
+	nv_wr32(dev, 0x400138, 0x00000000);
+	nv_wr32(dev, 0x400130, 0x00000000);
+	nv_wr32(dev, 0x400134, 0x00000000);
+	nv_wr32(dev, 0x400124, 0x00000002);
+}
+
+static void
+nve0_graph_init_units(struct drm_device *dev)
+{
+	nv_wr32(dev, 0x409ffc, 0x00000000);
+	nv_wr32(dev, 0x409c14, 0x00003e3e);
+	nv_wr32(dev, 0x409c24, 0x000f0000);
+
+	nv_wr32(dev, 0x404000, 0xc0000000);
+	nv_wr32(dev, 0x404600, 0xc0000000);
+	nv_wr32(dev, 0x408030, 0xc0000000);
+	nv_wr32(dev, 0x404490, 0xc0000000);
+	nv_wr32(dev, 0x406018, 0xc0000000);
+	nv_wr32(dev, 0x407020, 0xc0000000);
+	nv_wr32(dev, 0x405840, 0xc0000000);
+	nv_wr32(dev, 0x405844, 0x00ffffff);
+
+	nv_mask(dev, 0x419cc0, 0x00000008, 0x00000008);
+	nv_mask(dev, 0x419eb4, 0x00001000, 0x00001000);
+
+}
+
+static void
+nve0_graph_init_gpc_0(struct drm_device *dev)
+{
+	struct nve0_graph_priv *priv = nv_engine(dev, NVOBJ_ENGINE_GR);
+	const u32 magicgpc918 = DIV_ROUND_UP(0x00800000, priv->tpc_total);
+	u32 data[TPC_MAX / 8];
+	u8  tpcnr[GPC_MAX];
+	int i, gpc, tpc;
+
+	nv_wr32(dev, GPC_UNIT(0, 0x3018), 0x00000001);
+
+	memset(data, 0x00, sizeof(data));
+	memcpy(tpcnr, priv->tpc_nr, sizeof(priv->tpc_nr));
+	for (i = 0, gpc = -1; i < priv->tpc_total; i++) {
+		do {
+			gpc = (gpc + 1) % priv->gpc_nr;
+		} while (!tpcnr[gpc]);
+		tpc = priv->tpc_nr[gpc] - tpcnr[gpc]--;
+
+		data[i / 8] |= tpc << ((i % 8) * 4);
+	}
+
+	nv_wr32(dev, GPC_BCAST(0x0980), data[0]);
+	nv_wr32(dev, GPC_BCAST(0x0984), data[1]);
+	nv_wr32(dev, GPC_BCAST(0x0988), data[2]);
+	nv_wr32(dev, GPC_BCAST(0x098c), data[3]);
+
+	for (gpc = 0; gpc < priv->gpc_nr; gpc++) {
+		nv_wr32(dev, GPC_UNIT(gpc, 0x0914), priv->magic_not_rop_nr << 8 |
+						  priv->tpc_nr[gpc]);
+		nv_wr32(dev, GPC_UNIT(gpc, 0x0910), 0x00040000 | priv->tpc_total);
+		nv_wr32(dev, GPC_UNIT(gpc, 0x0918), magicgpc918);
+	}
+
+	nv_wr32(dev, GPC_BCAST(0x1bd4), magicgpc918);
+	nv_wr32(dev, GPC_BCAST(0x08ac), nv_rd32(dev, 0x100800));
+}
+
+static void
+nve0_graph_init_gpc_1(struct drm_device *dev)
+{
+	struct nve0_graph_priv *priv = nv_engine(dev, NVOBJ_ENGINE_GR);
+	int gpc, tpc;
+
+	for (gpc = 0; gpc < priv->gpc_nr; gpc++) {
+		nv_wr32(dev, GPC_UNIT(gpc, 0x3038), 0xc0000000);
+		nv_wr32(dev, GPC_UNIT(gpc, 0x0420), 0xc0000000);
+		nv_wr32(dev, GPC_UNIT(gpc, 0x0900), 0xc0000000);
+		nv_wr32(dev, GPC_UNIT(gpc, 0x1028), 0xc0000000);
+		nv_wr32(dev, GPC_UNIT(gpc, 0x0824), 0xc0000000);
+		for (tpc = 0; tpc < priv->tpc_nr[gpc]; tpc++) {
+			nv_wr32(dev, TPC_UNIT(gpc, tpc, 0x508), 0xffffffff);
+			nv_wr32(dev, TPC_UNIT(gpc, tpc, 0x50c), 0xffffffff);
+			nv_wr32(dev, TPC_UNIT(gpc, tpc, 0x224), 0xc0000000);
+			nv_wr32(dev, TPC_UNIT(gpc, tpc, 0x48c), 0xc0000000);
+			nv_wr32(dev, TPC_UNIT(gpc, tpc, 0x084), 0xc0000000);
+			nv_wr32(dev, TPC_UNIT(gpc, tpc, 0x644), 0x001ffffe);
+			nv_wr32(dev, TPC_UNIT(gpc, tpc, 0x64c), 0x0000000f);
+		}
+		nv_wr32(dev, GPC_UNIT(gpc, 0x2c90), 0xffffffff);
+		nv_wr32(dev, GPC_UNIT(gpc, 0x2c94), 0xffffffff);
+	}
+}
+
+static void
+nve0_graph_init_rop(struct drm_device *dev)
+{
+	struct nve0_graph_priv *priv = nv_engine(dev, NVOBJ_ENGINE_GR);
+	int rop;
+
+	for (rop = 0; rop < priv->rop_nr; rop++) {
+		nv_wr32(dev, ROP_UNIT(rop, 0x144), 0xc0000000);
+		nv_wr32(dev, ROP_UNIT(rop, 0x070), 0xc0000000);
+		nv_wr32(dev, ROP_UNIT(rop, 0x204), 0xffffffff);
+		nv_wr32(dev, ROP_UNIT(rop, 0x208), 0xffffffff);
+	}
+}
+
+static void
+nve0_graph_init_fuc(struct drm_device *dev, u32 fuc_base,
+		    struct nve0_graph_fuc *code, struct nve0_graph_fuc *data)
+{
+	int i;
+
+	nv_wr32(dev, fuc_base + 0x01c0, 0x01000000);
+	for (i = 0; i < data->size / 4; i++)
+		nv_wr32(dev, fuc_base + 0x01c4, data->data[i]);
+
+	nv_wr32(dev, fuc_base + 0x0180, 0x01000000);
+	for (i = 0; i < code->size / 4; i++) {
+		if ((i & 0x3f) == 0)
+			nv_wr32(dev, fuc_base + 0x0188, i >> 6);
+		nv_wr32(dev, fuc_base + 0x0184, code->data[i]);
+	}
+}
+
+static int
+nve0_graph_init_ctxctl(struct drm_device *dev)
+{
+	struct nve0_graph_priv *priv = nv_engine(dev, NVOBJ_ENGINE_GR);
+	u32 r000260;
+
+	/* load fuc microcode */
+	r000260 = nv_mask(dev, 0x000260, 0x00000001, 0x00000000);
+	nve0_graph_init_fuc(dev, 0x409000, &priv->fuc409c, &priv->fuc409d);
+	nve0_graph_init_fuc(dev, 0x41a000, &priv->fuc41ac, &priv->fuc41ad);
+	nv_wr32(dev, 0x000260, r000260);
+
+	/* start both of them running */
+	nv_wr32(dev, 0x409840, 0xffffffff);
+	nv_wr32(dev, 0x41a10c, 0x00000000);
+	nv_wr32(dev, 0x40910c, 0x00000000);
+	nv_wr32(dev, 0x41a100, 0x00000002);
+	nv_wr32(dev, 0x409100, 0x00000002);
+	if (!nv_wait(dev, 0x409800, 0x00000001, 0x00000001))
+		NV_INFO(dev, "0x409800 wait failed\n");
+
+	nv_wr32(dev, 0x409840, 0xffffffff);
+	nv_wr32(dev, 0x409500, 0x7fffffff);
+	nv_wr32(dev, 0x409504, 0x00000021);
+
+	nv_wr32(dev, 0x409840, 0xffffffff);
+	nv_wr32(dev, 0x409500, 0x00000000);
+	nv_wr32(dev, 0x409504, 0x00000010);
+	if (!nv_wait_ne(dev, 0x409800, 0xffffffff, 0x00000000)) {
+		NV_ERROR(dev, "fuc09 req 0x10 timeout\n");
+		return -EBUSY;
+	}
+	priv->grctx_size = nv_rd32(dev, 0x409800);
+
+	nv_wr32(dev, 0x409840, 0xffffffff);
+	nv_wr32(dev, 0x409500, 0x00000000);
+	nv_wr32(dev, 0x409504, 0x00000016);
+	if (!nv_wait_ne(dev, 0x409800, 0xffffffff, 0x00000000)) {
+		NV_ERROR(dev, "fuc09 req 0x16 timeout\n");
+		return -EBUSY;
+	}
+
+	nv_wr32(dev, 0x409840, 0xffffffff);
+	nv_wr32(dev, 0x409500, 0x00000000);
+	nv_wr32(dev, 0x409504, 0x00000025);
+	if (!nv_wait_ne(dev, 0x409800, 0xffffffff, 0x00000000)) {
+		NV_ERROR(dev, "fuc09 req 0x25 timeout\n");
+		return -EBUSY;
+	}
+
+	nv_wr32(dev, 0x409800, 0x00000000);
+	nv_wr32(dev, 0x409500, 0x00000001);
+	nv_wr32(dev, 0x409504, 0x00000030);
+	if (!nv_wait_ne(dev, 0x409800, 0xffffffff, 0x00000000)) {
+		NV_ERROR(dev, "fuc09 req 0x30 timeout\n");
+		return -EBUSY;
+	}
+
+	nv_wr32(dev, 0x409810, 0xb00095c8);
+	nv_wr32(dev, 0x409800, 0x00000000);
+	nv_wr32(dev, 0x409500, 0x00000001);
+	nv_wr32(dev, 0x409504, 0x00000031);
+	if (!nv_wait_ne(dev, 0x409800, 0xffffffff, 0x00000000)) {
+		NV_ERROR(dev, "fuc09 req 0x31 timeout\n");
+		return -EBUSY;
+	}
+
+	nv_wr32(dev, 0x409810, 0x00080420);
+	nv_wr32(dev, 0x409800, 0x00000000);
+	nv_wr32(dev, 0x409500, 0x00000001);
+	nv_wr32(dev, 0x409504, 0x00000032);
+	if (!nv_wait_ne(dev, 0x409800, 0xffffffff, 0x00000000)) {
+		NV_ERROR(dev, "fuc09 req 0x32 timeout\n");
+		return -EBUSY;
+	}
+
+	nv_wr32(dev, 0x409614, 0x00000070);
+	nv_wr32(dev, 0x409614, 0x00000770);
+	nv_wr32(dev, 0x40802c, 0x00000001);
+	return 0;
+}
+
+static int
+nve0_graph_init(struct drm_device *dev, int engine)
+{
+	int ret;
+
+	nv_mask(dev, 0x000200, 0x18001000, 0x00000000);
+	nv_mask(dev, 0x000200, 0x18001000, 0x18001000);
+
+	nve0_graph_init_obj418880(dev);
+	nve0_graph_init_regs(dev);
+	nve0_graph_init_gpc_0(dev);
+
+	nv_wr32(dev, 0x400500, 0x00010001);
+	nv_wr32(dev, 0x400100, 0xffffffff);
+	nv_wr32(dev, 0x40013c, 0xffffffff);
+
+	nve0_graph_init_units(dev);
+	nve0_graph_init_gpc_1(dev);
+	nve0_graph_init_rop(dev);
+
+	nv_wr32(dev, 0x400108, 0xffffffff);
+	nv_wr32(dev, 0x400138, 0xffffffff);
+	nv_wr32(dev, 0x400118, 0xffffffff);
+	nv_wr32(dev, 0x400130, 0xffffffff);
+	nv_wr32(dev, 0x40011c, 0xffffffff);
+	nv_wr32(dev, 0x400134, 0xffffffff);
+	nv_wr32(dev, 0x400054, 0x34ce3464);
+
+	ret = nve0_graph_init_ctxctl(dev);
+	if (ret)
+		return ret;
+
+	return 0;
+}
+
+int
+nve0_graph_isr_chid(struct drm_device *dev, u64 inst)
+{
+	struct nouveau_fifo_priv *pfifo = nv_engine(dev, NVOBJ_ENGINE_FIFO);
+	struct drm_nouveau_private *dev_priv = dev->dev_private;
+	struct nouveau_channel *chan;
+	unsigned long flags;
+	int i;
+
+	spin_lock_irqsave(&dev_priv->channels.lock, flags);
+	for (i = 0; i < pfifo->channels; i++) {
+		chan = dev_priv->channels.ptr[i];
+		if (!chan || !chan->ramin)
+			continue;
+
+		if (inst == chan->ramin->vinst)
+			break;
+	}
+	spin_unlock_irqrestore(&dev_priv->channels.lock, flags);
+	return i;
+}
+
+static void
+nve0_graph_ctxctl_isr(struct drm_device *dev)
+{
+	u32 ustat = nv_rd32(dev, 0x409c18);
+
+	if (ustat & 0x00000001)
+		NV_INFO(dev, "PGRAPH: CTXCTRL ucode error\n");
+	if (ustat & 0x00080000)
+		NV_INFO(dev, "PGRAPH: CTXCTRL watchdog timeout\n");
+	if (ustat & ~0x00080001)
+		NV_INFO(dev, "PGRAPH: CTXCTRL 0x%08x\n", ustat);
+
+	nve0_graph_ctxctl_debug(dev);
+	nv_wr32(dev, 0x409c20, ustat);
+}
+
+static void
+nve0_graph_trap_isr(struct drm_device *dev, int chid)
+{
+	struct nve0_graph_priv *priv = nv_engine(dev, NVOBJ_ENGINE_GR);
+	u32 trap = nv_rd32(dev, 0x400108);
+	int rop;
+
+	if (trap & 0x00000001) {
+		u32 stat = nv_rd32(dev, 0x404000);
+		NV_INFO(dev, "PGRAPH: DISPATCH ch %d 0x%08x\n", chid, stat);
+		nv_wr32(dev, 0x404000, 0xc0000000);
+		nv_wr32(dev, 0x400108, 0x00000001);
+		trap &= ~0x00000001;
+	}
+
+	if (trap & 0x00000010) {
+		u32 stat = nv_rd32(dev, 0x405840);
+		NV_INFO(dev, "PGRAPH: SHADER ch %d 0x%08x\n", chid, stat);
+		nv_wr32(dev, 0x405840, 0xc0000000);
+		nv_wr32(dev, 0x400108, 0x00000010);
+		trap &= ~0x00000010;
+	}
+
+	if (trap & 0x02000000) {
+		for (rop = 0; rop < priv->rop_nr; rop++) {
+			u32 statz = nv_rd32(dev, ROP_UNIT(rop, 0x070));
+			u32 statc = nv_rd32(dev, ROP_UNIT(rop, 0x144));
+			NV_INFO(dev, "PGRAPH: ROP%d ch %d 0x%08x 0x%08x\n",
+				     rop, chid, statz, statc);
+			nv_wr32(dev, ROP_UNIT(rop, 0x070), 0xc0000000);
+			nv_wr32(dev, ROP_UNIT(rop, 0x144), 0xc0000000);
+		}
+		nv_wr32(dev, 0x400108, 0x02000000);
+		trap &= ~0x02000000;
+	}
+
+	if (trap) {
+		NV_INFO(dev, "PGRAPH: TRAP ch %d 0x%08x\n", chid, trap);
+		nv_wr32(dev, 0x400108, trap);
+	}
+}
+
+static void
+nve0_graph_isr(struct drm_device *dev)
+{
+	u64 inst = (u64)(nv_rd32(dev, 0x409b00) & 0x0fffffff) << 12;
+	u32 chid = nve0_graph_isr_chid(dev, inst);
+	u32 stat = nv_rd32(dev, 0x400100);
+	u32 addr = nv_rd32(dev, 0x400704);
+	u32 mthd = (addr & 0x00003ffc);
+	u32 subc = (addr & 0x00070000) >> 16;
+	u32 data = nv_rd32(dev, 0x400708);
+	u32 code = nv_rd32(dev, 0x400110);
+	u32 class = nv_rd32(dev, 0x404200 + (subc * 4));
+
+	if (stat & 0x00000010) {
+		if (nouveau_gpuobj_mthd_call2(dev, chid, class, mthd, data)) {
+			NV_INFO(dev, "PGRAPH: ILLEGAL_MTHD ch %d [0x%010llx] "
+				     "subc %d class 0x%04x mthd 0x%04x "
+				     "data 0x%08x\n",
+				chid, inst, subc, class, mthd, data);
+		}
+		nv_wr32(dev, 0x400100, 0x00000010);
+		stat &= ~0x00000010;
+	}
+
+	if (stat & 0x00000020) {
+		NV_INFO(dev, "PGRAPH: ILLEGAL_CLASS ch %d [0x%010llx] subc %d "
+			     "class 0x%04x mthd 0x%04x data 0x%08x\n",
+			chid, inst, subc, class, mthd, data);
+		nv_wr32(dev, 0x400100, 0x00000020);
+		stat &= ~0x00000020;
+	}
+
+	if (stat & 0x00100000) {
+		NV_INFO(dev, "PGRAPH: DATA_ERROR [");
+		nouveau_enum_print(nv50_data_error_names, code);
+		printk("] ch %d [0x%010llx] subc %d class 0x%04x "
+		       "mthd 0x%04x data 0x%08x\n",
+		       chid, inst, subc, class, mthd, data);
+		nv_wr32(dev, 0x400100, 0x00100000);
+		stat &= ~0x00100000;
+	}
+
+	if (stat & 0x00200000) {
+		nve0_graph_trap_isr(dev, chid);
+		nv_wr32(dev, 0x400100, 0x00200000);
+		stat &= ~0x00200000;
+	}
+
+	if (stat & 0x00080000) {
+		nve0_graph_ctxctl_isr(dev);
+		nv_wr32(dev, 0x400100, 0x00080000);
+		stat &= ~0x00080000;
+	}
+
+	if (stat) {
+		NV_INFO(dev, "PGRAPH: unknown stat 0x%08x\n", stat);
+		nv_wr32(dev, 0x400100, stat);
+	}
+
+	nv_wr32(dev, 0x400500, 0x00010001);
+}
+
+static int
+nve0_graph_create_fw(struct drm_device *dev, const char *fwname,
+		     struct nve0_graph_fuc *fuc)
+{
+	struct drm_nouveau_private *dev_priv = dev->dev_private;
+	const struct firmware *fw;
+	char f[32];
+	int ret;
+
+	snprintf(f, sizeof(f), "nouveau/nv%02x_%s", dev_priv->chipset, fwname);
+	ret = request_firmware(&fw, f, &dev->pdev->dev);
+	if (ret)
+		return ret;
+
+	fuc->size = fw->size;
+	fuc->data = kmemdup(fw->data, fuc->size, GFP_KERNEL);
+	release_firmware(fw);
+	return (fuc->data != NULL) ? 0 : -ENOMEM;
+}
+
+static void
+nve0_graph_destroy_fw(struct nve0_graph_fuc *fuc)
+{
+	if (fuc->data) {
+		kfree(fuc->data);
+		fuc->data = NULL;
+	}
+}
+
+static void
+nve0_graph_destroy(struct drm_device *dev, int engine)
+{
+	struct nve0_graph_priv *priv = nv_engine(dev, engine);
+
+	nve0_graph_destroy_fw(&priv->fuc409c);
+	nve0_graph_destroy_fw(&priv->fuc409d);
+	nve0_graph_destroy_fw(&priv->fuc41ac);
+	nve0_graph_destroy_fw(&priv->fuc41ad);
+
+	nouveau_irq_unregister(dev, 12);
+
+	nouveau_gpuobj_ref(NULL, &priv->unk4188b8);
+	nouveau_gpuobj_ref(NULL, &priv->unk4188b4);
+
+	if (priv->grctx_vals)
+		kfree(priv->grctx_vals);
+
+	NVOBJ_ENGINE_DEL(dev, GR);
+	kfree(priv);
+}
+
+int
+nve0_graph_create(struct drm_device *dev)
+{
+	struct drm_nouveau_private *dev_priv = dev->dev_private;
+	struct nve0_graph_priv *priv;
+	int ret, gpc, i;
+	u32 kepler;
+
+	kepler = nve0_graph_class(dev);
+	if (!kepler) {
+		NV_ERROR(dev, "PGRAPH: unsupported chipset, please report!\n");
+		return 0;
+	}
+
+	priv = kzalloc(sizeof(*priv), GFP_KERNEL);
+	if (!priv)
+		return -ENOMEM;
+
+	priv->base.destroy = nve0_graph_destroy;
+	priv->base.init = nve0_graph_init;
+	priv->base.fini = nve0_graph_fini;
+	priv->base.context_new = nve0_graph_context_new;
+	priv->base.context_del = nve0_graph_context_del;
+	priv->base.object_new = nve0_graph_object_new;
+
+	NVOBJ_ENGINE_ADD(dev, GR, &priv->base);
+	nouveau_irq_register(dev, 12, nve0_graph_isr);
+
+	NV_INFO(dev, "PGRAPH: using external firmware\n");
+	if (nve0_graph_create_fw(dev, "fuc409c", &priv->fuc409c) ||
+	    nve0_graph_create_fw(dev, "fuc409d", &priv->fuc409d) ||
+	    nve0_graph_create_fw(dev, "fuc41ac", &priv->fuc41ac) ||
+	    nve0_graph_create_fw(dev, "fuc41ad", &priv->fuc41ad)) {
+		ret = 0;
+		goto error;
+	}
+
+	ret = nouveau_gpuobj_new(dev, NULL, 0x1000, 256, 0, &priv->unk4188b4);
+	if (ret)
+		goto error;
+
+	ret = nouveau_gpuobj_new(dev, NULL, 0x1000, 256, 0, &priv->unk4188b8);
+	if (ret)
+		goto error;
+
+	for (i = 0; i < 0x1000; i += 4) {
+		nv_wo32(priv->unk4188b4, i, 0x00000010);
+		nv_wo32(priv->unk4188b8, i, 0x00000010);
+	}
+
+	priv->gpc_nr  =  nv_rd32(dev, 0x409604) & 0x0000001f;
+	priv->rop_nr = (nv_rd32(dev, 0x409604) & 0x001f0000) >> 16;
+	for (gpc = 0; gpc < priv->gpc_nr; gpc++) {
+		priv->tpc_nr[gpc] = nv_rd32(dev, GPC_UNIT(gpc, 0x2608));
+		priv->tpc_total += priv->tpc_nr[gpc];
+	}
+
+	switch (dev_priv->chipset) {
+	case 0xe4:
+		if (priv->tpc_total == 8)
+			priv->magic_not_rop_nr = 3;
+		else
+		if (priv->tpc_total == 7)
+			priv->magic_not_rop_nr = 1;
+		break;
+	case 0xe7:
+		priv->magic_not_rop_nr = 1;
+		break;
+	default:
+		break;
+	}
+
+	if (!priv->magic_not_rop_nr) {
+		NV_ERROR(dev, "PGRAPH: unknown config: %d/%d/%d/%d, %d\n",
+			 priv->tpc_nr[0], priv->tpc_nr[1], priv->tpc_nr[2],
+			 priv->tpc_nr[3], priv->rop_nr);
+		priv->magic_not_rop_nr = 0x00;
+	}
+
+	NVOBJ_CLASS(dev, 0xa097, GR); /* subc 0: 3D */
+	NVOBJ_CLASS(dev, 0xa0c0, GR); /* subc 1: COMPUTE */
+	NVOBJ_CLASS(dev, 0xa040, GR); /* subc 2: P2MF */
+	NVOBJ_CLASS(dev, 0x902d, GR); /* subc 3: 2D */
+	NVOBJ_CLASS(dev, 0xa0b5, GR); /* subc 4: COPY */
+	return 0;
+
+error:
+	nve0_graph_destroy(dev, NVOBJ_ENGINE_GR);
+	return ret;
+}
diff --git a/drivers/gpu/drm/nouveau/nve0_graph.h b/drivers/gpu/drm/nouveau/nve0_graph.h
new file mode 100644
index 00000000000..2ba70449ba0
--- /dev/null
+++ b/drivers/gpu/drm/nouveau/nve0_graph.h
@@ -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
+ */
+
+#ifndef __NVE0_GRAPH_H__
+#define __NVE0_GRAPH_H__
+
+#define GPC_MAX 4
+#define TPC_MAX 32
+
+#define ROP_BCAST(r)     (0x408800 + (r))
+#define ROP_UNIT(u, r)   (0x410000 + (u) * 0x400 + (r))
+#define GPC_BCAST(r)     (0x418000 + (r))
+#define GPC_UNIT(t, r)   (0x500000 + (t) * 0x8000 + (r))
+#define TPC_UNIT(t, m, r) (0x504000 + (t) * 0x8000 + (m) * 0x800 + (r))
+
+struct nve0_graph_fuc {
+	u32 *data;
+	u32  size;
+};
+
+struct nve0_graph_priv {
+	struct nouveau_exec_engine base;
+
+	struct nve0_graph_fuc fuc409c;
+	struct nve0_graph_fuc fuc409d;
+	struct nve0_graph_fuc fuc41ac;
+	struct nve0_graph_fuc fuc41ad;
+
+	u8 gpc_nr;
+	u8 rop_nr;
+	u8 tpc_nr[GPC_MAX];
+	u8 tpc_total;
+
+	u32  grctx_size;
+	u32 *grctx_vals;
+	struct nouveau_gpuobj *unk4188b4;
+	struct nouveau_gpuobj *unk4188b8;
+
+	u8 magic_not_rop_nr;
+};
+
+struct nve0_graph_chan {
+	struct nouveau_gpuobj *grctx;
+	struct nouveau_gpuobj *unk408004; /* 0x418810 too */
+	struct nouveau_gpuobj *unk40800c; /* 0x419004 too */
+	struct nouveau_gpuobj *unk418810; /* 0x419848 too */
+	struct nouveau_gpuobj *mmio;
+	int mmio_nr;
+};
+
+int nve0_grctx_generate(struct nouveau_channel *);
+
+/* nve0_graph.c uses this also to determine supported chipsets */
+static inline u32
+nve0_graph_class(struct drm_device *dev)
+{
+	struct drm_nouveau_private *dev_priv = dev->dev_private;
+
+	switch (dev_priv->chipset) {
+	case 0xe4:
+	case 0xe7:
+		return 0xa097;
+	default:
+		return 0;
+	}
+}
+
+#endif
diff --git a/drivers/gpu/drm/nouveau/nve0_grctx.c b/drivers/gpu/drm/nouveau/nve0_grctx.c
new file mode 100644
index 00000000000..d8cb360e92c
--- /dev/null
+++ b/drivers/gpu/drm/nouveau/nve0_grctx.c
@@ -0,0 +1,2777 @@
+/*
+ * Copyright 2010 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 "drmP.h"
+#include "nouveau_drv.h"
+#include "nouveau_mm.h"
+#include "nve0_graph.h"
+
+static void
+nv_icmd(struct drm_device *dev, u32 icmd, u32 data)
+{
+	nv_wr32(dev, 0x400204, data);
+	nv_wr32(dev, 0x400200, icmd);
+	while (nv_rd32(dev, 0x400700) & 0x00000002) {}
+}
+
+static void
+nve0_grctx_generate_icmd(struct drm_device *dev)
+{
+	nv_wr32(dev, 0x400208, 0x80000000);
+	nv_icmd(dev, 0x001000, 0x00000004);
+	nv_icmd(dev, 0x000039, 0x00000000);
+	nv_icmd(dev, 0x00003a, 0x00000000);
+	nv_icmd(dev, 0x00003b, 0x00000000);
+	nv_icmd(dev, 0x0000a9, 0x0000ffff);
+	nv_icmd(dev, 0x000038, 0x0fac6881);
+	nv_icmd(dev, 0x00003d, 0x00000001);
+	nv_icmd(dev, 0x0000e8, 0x00000400);
+	nv_icmd(dev, 0x0000e9, 0x00000400);
+	nv_icmd(dev, 0x0000ea, 0x00000400);
+	nv_icmd(dev, 0x0000eb, 0x00000400);
+	nv_icmd(dev, 0x0000ec, 0x00000400);
+	nv_icmd(dev, 0x0000ed, 0x00000400);
+	nv_icmd(dev, 0x0000ee, 0x00000400);
+	nv_icmd(dev, 0x0000ef, 0x00000400);
+	nv_icmd(dev, 0x000078, 0x00000300);
+	nv_icmd(dev, 0x000079, 0x00000300);
+	nv_icmd(dev, 0x00007a, 0x00000300);
+	nv_icmd(dev, 0x00007b, 0x00000300);
+	nv_icmd(dev, 0x00007c, 0x00000300);
+	nv_icmd(dev, 0x00007d, 0x00000300);
+	nv_icmd(dev, 0x00007e, 0x00000300);
+	nv_icmd(dev, 0x00007f, 0x00000300);
+	nv_icmd(dev, 0x000050, 0x00000011);
+	nv_icmd(dev, 0x000058, 0x00000008);
+	nv_icmd(dev, 0x000059, 0x00000008);
+	nv_icmd(dev, 0x00005a, 0x00000008);
+	nv_icmd(dev, 0x00005b, 0x00000008);
+	nv_icmd(dev, 0x00005c, 0x00000008);
+	nv_icmd(dev, 0x00005d, 0x00000008);
+	nv_icmd(dev, 0x00005e, 0x00000008);
+	nv_icmd(dev, 0x00005f, 0x00000008);
+	nv_icmd(dev, 0x000208, 0x00000001);
+	nv_icmd(dev, 0x000209, 0x00000001);
+	nv_icmd(dev, 0x00020a, 0x00000001);
+	nv_icmd(dev, 0x00020b, 0x00000001);
+	nv_icmd(dev, 0x00020c, 0x00000001);
+	nv_icmd(dev, 0x00020d, 0x00000001);
+	nv_icmd(dev, 0x00020e, 0x00000001);
+	nv_icmd(dev, 0x00020f, 0x00000001);
+	nv_icmd(dev, 0x000081, 0x00000001);
+	nv_icmd(dev, 0x000085, 0x00000004);
+	nv_icmd(dev, 0x000088, 0x00000400);
+	nv_icmd(dev, 0x000090, 0x00000300);
+	nv_icmd(dev, 0x000098, 0x00001001);
+	nv_icmd(dev, 0x0000e3, 0x00000001);
+	nv_icmd(dev, 0x0000da, 0x00000001);
+	nv_icmd(dev, 0x0000f8, 0x00000003);
+	nv_icmd(dev, 0x0000fa, 0x00000001);
+	nv_icmd(dev, 0x00009f, 0x0000ffff);
+	nv_icmd(dev, 0x0000a0, 0x0000ffff);
+	nv_icmd(dev, 0x0000a1, 0x0000ffff);
+	nv_icmd(dev, 0x0000a2, 0x0000ffff);
+	nv_icmd(dev, 0x0000b1, 0x00000001);
+	nv_icmd(dev, 0x0000ad, 0x0000013e);
+	nv_icmd(dev, 0x0000e1, 0x00000010);
+	nv_icmd(dev, 0x000290, 0x00000000);
+	nv_icmd(dev, 0x000291, 0x00000000);
+	nv_icmd(dev, 0x000292, 0x00000000);
+	nv_icmd(dev, 0x000293, 0x00000000);
+	nv_icmd(dev, 0x000294, 0x00000000);
+	nv_icmd(dev, 0x000295, 0x00000000);
+	nv_icmd(dev, 0x000296, 0x00000000);
+	nv_icmd(dev, 0x000297, 0x00000000);
+	nv_icmd(dev, 0x000298, 0x00000000);
+	nv_icmd(dev, 0x000299, 0x00000000);
+	nv_icmd(dev, 0x00029a, 0x00000000);
+	nv_icmd(dev, 0x00029b, 0x00000000);
+	nv_icmd(dev, 0x00029c, 0x00000000);
+	nv_icmd(dev, 0x00029d, 0x00000000);
+	nv_icmd(dev, 0x00029e, 0x00000000);
+	nv_icmd(dev, 0x00029f, 0x00000000);
+	nv_icmd(dev, 0x0003b0, 0x00000000);
+	nv_icmd(dev, 0x0003b1, 0x00000000);
+	nv_icmd(dev, 0x0003b2, 0x00000000);
+	nv_icmd(dev, 0x0003b3, 0x00000000);
+	nv_icmd(dev, 0x0003b4, 0x00000000);
+	nv_icmd(dev, 0x0003b5, 0x00000000);
+	nv_icmd(dev, 0x0003b6, 0x00000000);
+	nv_icmd(dev, 0x0003b7, 0x00000000);
+	nv_icmd(dev, 0x0003b8, 0x00000000);
+	nv_icmd(dev, 0x0003b9, 0x00000000);
+	nv_icmd(dev, 0x0003ba, 0x00000000);
+	nv_icmd(dev, 0x0003bb, 0x00000000);
+	nv_icmd(dev, 0x0003bc, 0x00000000);
+	nv_icmd(dev, 0x0003bd, 0x00000000);
+	nv_icmd(dev, 0x0003be, 0x00000000);
+	nv_icmd(dev, 0x0003bf, 0x00000000);
+	nv_icmd(dev, 0x0002a0, 0x00000000);
+	nv_icmd(dev, 0x0002a1, 0x00000000);
+	nv_icmd(dev, 0x0002a2, 0x00000000);
+	nv_icmd(dev, 0x0002a3, 0x00000000);
+	nv_icmd(dev, 0x0002a4, 0x00000000);
+	nv_icmd(dev, 0x0002a5, 0x00000000);
+	nv_icmd(dev, 0x0002a6, 0x00000000);
+	nv_icmd(dev, 0x0002a7, 0x00000000);
+	nv_icmd(dev, 0x0002a8, 0x00000000);
+	nv_icmd(dev, 0x0002a9, 0x00000000);
+	nv_icmd(dev, 0x0002aa, 0x00000000);
+	nv_icmd(dev, 0x0002ab, 0x00000000);
+	nv_icmd(dev, 0x0002ac, 0x00000000);
+	nv_icmd(dev, 0x0002ad, 0x00000000);
+	nv_icmd(dev, 0x0002ae, 0x00000000);
+	nv_icmd(dev, 0x0002af, 0x00000000);
+	nv_icmd(dev, 0x000420, 0x00000000);
+	nv_icmd(dev, 0x000421, 0x00000000);
+	nv_icmd(dev, 0x000422, 0x00000000);
+	nv_icmd(dev, 0x000423, 0x00000000);
+	nv_icmd(dev, 0x000424, 0x00000000);
+	nv_icmd(dev, 0x000425, 0x00000000);
+	nv_icmd(dev, 0x000426, 0x00000000);
+	nv_icmd(dev, 0x000427, 0x00000000);
+	nv_icmd(dev, 0x000428, 0x00000000);
+	nv_icmd(dev, 0x000429, 0x00000000);
+	nv_icmd(dev, 0x00042a, 0x00000000);
+	nv_icmd(dev, 0x00042b, 0x00000000);
+	nv_icmd(dev, 0x00042c, 0x00000000);
+	nv_icmd(dev, 0x00042d, 0x00000000);
+	nv_icmd(dev, 0x00042e, 0x00000000);
+	nv_icmd(dev, 0x00042f, 0x00000000);
+	nv_icmd(dev, 0x0002b0, 0x00000000);
+	nv_icmd(dev, 0x0002b1, 0x00000000);
+	nv_icmd(dev, 0x0002b2, 0x00000000);
+	nv_icmd(dev, 0x0002b3, 0x00000000);
+	nv_icmd(dev, 0x0002b4, 0x00000000);
+	nv_icmd(dev, 0x0002b5, 0x00000000);
+	nv_icmd(dev, 0x0002b6, 0x00000000);
+	nv_icmd(dev, 0x0002b7, 0x00000000);
+	nv_icmd(dev, 0x0002b8, 0x00000000);
+	nv_icmd(dev, 0x0002b9, 0x00000000);
+	nv_icmd(dev, 0x0002ba, 0x00000000);
+	nv_icmd(dev, 0x0002bb, 0x00000000);
+	nv_icmd(dev, 0x0002bc, 0x00000000);
+	nv_icmd(dev, 0x0002bd, 0x00000000);
+	nv_icmd(dev, 0x0002be, 0x00000000);
+	nv_icmd(dev, 0x0002bf, 0x00000000);
+	nv_icmd(dev, 0x000430, 0x00000000);
+	nv_icmd(dev, 0x000431, 0x00000000);
+	nv_icmd(dev, 0x000432, 0x00000000);
+	nv_icmd(dev, 0x000433, 0x00000000);
+	nv_icmd(dev, 0x000434, 0x00000000);
+	nv_icmd(dev, 0x000435, 0x00000000);
+	nv_icmd(dev, 0x000436, 0x00000000);
+	nv_icmd(dev, 0x000437, 0x00000000);
+	nv_icmd(dev, 0x000438, 0x00000000);
+	nv_icmd(dev, 0x000439, 0x00000000);
+	nv_icmd(dev, 0x00043a, 0x00000000);
+	nv_icmd(dev, 0x00043b, 0x00000000);
+	nv_icmd(dev, 0x00043c, 0x00000000);
+	nv_icmd(dev, 0x00043d, 0x00000000);
+	nv_icmd(dev, 0x00043e, 0x00000000);
+	nv_icmd(dev, 0x00043f, 0x00000000);
+	nv_icmd(dev, 0x0002c0, 0x00000000);
+	nv_icmd(dev, 0x0002c1, 0x00000000);
+	nv_icmd(dev, 0x0002c2, 0x00000000);
+	nv_icmd(dev, 0x0002c3, 0x00000000);
+	nv_icmd(dev, 0x0002c4, 0x00000000);
+	nv_icmd(dev, 0x0002c5, 0x00000000);
+	nv_icmd(dev, 0x0002c6, 0x00000000);
+	nv_icmd(dev, 0x0002c7, 0x00000000);
+	nv_icmd(dev, 0x0002c8, 0x00000000);
+	nv_icmd(dev, 0x0002c9, 0x00000000);
+	nv_icmd(dev, 0x0002ca, 0x00000000);
+	nv_icmd(dev, 0x0002cb, 0x00000000);
+	nv_icmd(dev, 0x0002cc, 0x00000000);
+	nv_icmd(dev, 0x0002cd, 0x00000000);
+	nv_icmd(dev, 0x0002ce, 0x00000000);
+	nv_icmd(dev, 0x0002cf, 0x00000000);
+	nv_icmd(dev, 0x0004d0, 0x00000000);
+	nv_icmd(dev, 0x0004d1, 0x00000000);
+	nv_icmd(dev, 0x0004d2, 0x00000000);
+	nv_icmd(dev, 0x0004d3, 0x00000000);
+	nv_icmd(dev, 0x0004d4, 0x00000000);
+	nv_icmd(dev, 0x0004d5, 0x00000000);
+	nv_icmd(dev, 0x0004d6, 0x00000000);
+	nv_icmd(dev, 0x0004d7, 0x00000000);
+	nv_icmd(dev, 0x0004d8, 0x00000000);
+	nv_icmd(dev, 0x0004d9, 0x00000000);
+	nv_icmd(dev, 0x0004da, 0x00000000);
+	nv_icmd(dev, 0x0004db, 0x00000000);
+	nv_icmd(dev, 0x0004dc, 0x00000000);
+	nv_icmd(dev, 0x0004dd, 0x00000000);
+	nv_icmd(dev, 0x0004de, 0x00000000);
+	nv_icmd(dev, 0x0004df, 0x00000000);
+	nv_icmd(dev, 0x000720, 0x00000000);
+	nv_icmd(dev, 0x000721, 0x00000000);
+	nv_icmd(dev, 0x000722, 0x00000000);
+	nv_icmd(dev, 0x000723, 0x00000000);
+	nv_icmd(dev, 0x000724, 0x00000000);
+	nv_icmd(dev, 0x000725, 0x00000000);
+	nv_icmd(dev, 0x000726, 0x00000000);
+	nv_icmd(dev, 0x000727, 0x00000000);
+	nv_icmd(dev, 0x000728, 0x00000000);
+	nv_icmd(dev, 0x000729, 0x00000000);
+	nv_icmd(dev, 0x00072a, 0x00000000);
+	nv_icmd(dev, 0x00072b, 0x00000000);
+	nv_icmd(dev, 0x00072c, 0x00000000);
+	nv_icmd(dev, 0x00072d, 0x00000000);
+	nv_icmd(dev, 0x00072e, 0x00000000);
+	nv_icmd(dev, 0x00072f, 0x00000000);
+	nv_icmd(dev, 0x0008c0, 0x00000000);
+	nv_icmd(dev, 0x0008c1, 0x00000000);
+	nv_icmd(dev, 0x0008c2, 0x00000000);
+	nv_icmd(dev, 0x0008c3, 0x00000000);
+	nv_icmd(dev, 0x0008c4, 0x00000000);
+	nv_icmd(dev, 0x0008c5, 0x00000000);
+	nv_icmd(dev, 0x0008c6, 0x00000000);
+	nv_icmd(dev, 0x0008c7, 0x00000000);
+	nv_icmd(dev, 0x0008c8, 0x00000000);
+	nv_icmd(dev, 0x0008c9, 0x00000000);
+	nv_icmd(dev, 0x0008ca, 0x00000000);
+	nv_icmd(dev, 0x0008cb, 0x00000000);
+	nv_icmd(dev, 0x0008cc, 0x00000000);
+	nv_icmd(dev, 0x0008cd, 0x00000000);
+	nv_icmd(dev, 0x0008ce, 0x00000000);
+	nv_icmd(dev, 0x0008cf, 0x00000000);
+	nv_icmd(dev, 0x000890, 0x00000000);
+	nv_icmd(dev, 0x000891, 0x00000000);
+	nv_icmd(dev, 0x000892, 0x00000000);
+	nv_icmd(dev, 0x000893, 0x00000000);
+	nv_icmd(dev, 0x000894, 0x00000000);
+	nv_icmd(dev, 0x000895, 0x00000000);
+	nv_icmd(dev, 0x000896, 0x00000000);
+	nv_icmd(dev, 0x000897, 0x00000000);
+	nv_icmd(dev, 0x000898, 0x00000000);
+	nv_icmd(dev, 0x000899, 0x00000000);
+	nv_icmd(dev, 0x00089a, 0x00000000);
+	nv_icmd(dev, 0x00089b, 0x00000000);
+	nv_icmd(dev, 0x00089c, 0x00000000);
+	nv_icmd(dev, 0x00089d, 0x00000000);
+	nv_icmd(dev, 0x00089e, 0x00000000);
+	nv_icmd(dev, 0x00089f, 0x00000000);
+	nv_icmd(dev, 0x0008e0, 0x00000000);
+	nv_icmd(dev, 0x0008e1, 0x00000000);
+	nv_icmd(dev, 0x0008e2, 0x00000000);
+	nv_icmd(dev, 0x0008e3, 0x00000000);
+	nv_icmd(dev, 0x0008e4, 0x00000000);
+	nv_icmd(dev, 0x0008e5, 0x00000000);
+	nv_icmd(dev, 0x0008e6, 0x00000000);
+	nv_icmd(dev, 0x0008e7, 0x00000000);
+	nv_icmd(dev, 0x0008e8, 0x00000000);
+	nv_icmd(dev, 0x0008e9, 0x00000000);
+	nv_icmd(dev, 0x0008ea, 0x00000000);
+	nv_icmd(dev, 0x0008eb, 0x00000000);
+	nv_icmd(dev, 0x0008ec, 0x00000000);
+	nv_icmd(dev, 0x0008ed, 0x00000000);
+	nv_icmd(dev, 0x0008ee, 0x00000000);
+	nv_icmd(dev, 0x0008ef, 0x00000000);
+	nv_icmd(dev, 0x0008a0, 0x00000000);
+	nv_icmd(dev, 0x0008a1, 0x00000000);
+	nv_icmd(dev, 0x0008a2, 0x00000000);
+	nv_icmd(dev, 0x0008a3, 0x00000000);
+	nv_icmd(dev, 0x0008a4, 0x00000000);
+	nv_icmd(dev, 0x0008a5, 0x00000000);
+	nv_icmd(dev, 0x0008a6, 0x00000000);
+	nv_icmd(dev, 0x0008a7, 0x00000000);
+	nv_icmd(dev, 0x0008a8, 0x00000000);
+	nv_icmd(dev, 0x0008a9, 0x00000000);
+	nv_icmd(dev, 0x0008aa, 0x00000000);
+	nv_icmd(dev, 0x0008ab, 0x00000000);
+	nv_icmd(dev, 0x0008ac, 0x00000000);
+	nv_icmd(dev, 0x0008ad, 0x00000000);
+	nv_icmd(dev, 0x0008ae, 0x00000000);
+	nv_icmd(dev, 0x0008af, 0x00000000);
+	nv_icmd(dev, 0x0008f0, 0x00000000);
+	nv_icmd(dev, 0x0008f1, 0x00000000);
+	nv_icmd(dev, 0x0008f2, 0x00000000);
+	nv_icmd(dev, 0x0008f3, 0x00000000);
+	nv_icmd(dev, 0x0008f4, 0x00000000);
+	nv_icmd(dev, 0x0008f5, 0x00000000);
+	nv_icmd(dev, 0x0008f6, 0x00000000);
+	nv_icmd(dev, 0x0008f7, 0x00000000);
+	nv_icmd(dev, 0x0008f8, 0x00000000);
+	nv_icmd(dev, 0x0008f9, 0x00000000);
+	nv_icmd(dev, 0x0008fa, 0x00000000);
+	nv_icmd(dev, 0x0008fb, 0x00000000);
+	nv_icmd(dev, 0x0008fc, 0x00000000);
+	nv_icmd(dev, 0x0008fd, 0x00000000);
+	nv_icmd(dev, 0x0008fe, 0x00000000);
+	nv_icmd(dev, 0x0008ff, 0x00000000);
+	nv_icmd(dev, 0x00094c, 0x000000ff);
+	nv_icmd(dev, 0x00094d, 0xffffffff);
+	nv_icmd(dev, 0x00094e, 0x00000002);
+	nv_icmd(dev, 0x0002ec, 0x00000001);
+	nv_icmd(dev, 0x000303, 0x00000001);
+	nv_icmd(dev, 0x0002e6, 0x00000001);
+	nv_icmd(dev, 0x000466, 0x00000052);
+	nv_icmd(dev, 0x000301, 0x3f800000);
+	nv_icmd(dev, 0x000304, 0x30201000);
+	nv_icmd(dev, 0x000305, 0x70605040);
+	nv_icmd(dev, 0x000306, 0xb8a89888);
+	nv_icmd(dev, 0x000307, 0xf8e8d8c8);
+	nv_icmd(dev, 0x00030a, 0x00ffff00);
+	nv_icmd(dev, 0x00030b, 0x0000001a);
+	nv_icmd(dev, 0x00030c, 0x00000001);
+	nv_icmd(dev, 0x000318, 0x00000001);
+	nv_icmd(dev, 0x000340, 0x00000000);
+	nv_icmd(dev, 0x000375, 0x00000001);
+	nv_icmd(dev, 0x00037d, 0x00000006);
+	nv_icmd(dev, 0x0003a0, 0x00000002);
+	nv_icmd(dev, 0x0003aa, 0x00000001);
+	nv_icmd(dev, 0x0003a9, 0x00000001);
+	nv_icmd(dev, 0x000380, 0x00000001);
+	nv_icmd(dev, 0x000383, 0x00000011);
+	nv_icmd(dev, 0x000360, 0x00000040);
+	nv_icmd(dev, 0x000366, 0x00000000);
+	nv_icmd(dev, 0x000367, 0x00000000);
+	nv_icmd(dev, 0x000368, 0x00000fff);
+	nv_icmd(dev, 0x000370, 0x00000000);
+	nv_icmd(dev, 0x000371, 0x00000000);
+	nv_icmd(dev, 0x000372, 0x000fffff);
+	nv_icmd(dev, 0x00037a, 0x00000012);
+	nv_icmd(dev, 0x000619, 0x00000003);
+	nv_icmd(dev, 0x000811, 0x00000003);
+	nv_icmd(dev, 0x000812, 0x00000004);
+	nv_icmd(dev, 0x000813, 0x00000006);
+	nv_icmd(dev, 0x000814, 0x00000008);
+	nv_icmd(dev, 0x000815, 0x0000000b);
+	nv_icmd(dev, 0x000800, 0x00000001);
+	nv_icmd(dev, 0x000801, 0x00000001);
+	nv_icmd(dev, 0x000802, 0x00000001);
+	nv_icmd(dev, 0x000803, 0x00000001);
+	nv_icmd(dev, 0x000804, 0x00000001);
+	nv_icmd(dev, 0x000805, 0x00000001);
+	nv_icmd(dev, 0x000632, 0x00000001);
+	nv_icmd(dev, 0x000633, 0x00000002);
+	nv_icmd(dev, 0x000634, 0x00000003);
+	nv_icmd(dev, 0x000635, 0x00000004);
+	nv_icmd(dev, 0x000654, 0x3f800000);
+	nv_icmd(dev, 0x000657, 0x3f800000);
+	nv_icmd(dev, 0x000655, 0x3f800000);
+	nv_icmd(dev, 0x000656, 0x3f800000);
+	nv_icmd(dev, 0x0006cd, 0x3f800000);
+	nv_icmd(dev, 0x0007f5, 0x3f800000);
+	nv_icmd(dev, 0x0007dc, 0x39291909);
+	nv_icmd(dev, 0x0007dd, 0x79695949);
+	nv_icmd(dev, 0x0007de, 0xb9a99989);
+	nv_icmd(dev, 0x0007df, 0xf9e9d9c9);
+	nv_icmd(dev, 0x0007e8, 0x00003210);
+	nv_icmd(dev, 0x0007e9, 0x00007654);
+	nv_icmd(dev, 0x0007ea, 0x00000098);
+	nv_icmd(dev, 0x0007ec, 0x39291909);
+	nv_icmd(dev, 0x0007ed, 0x79695949);
+	nv_icmd(dev, 0x0007ee, 0xb9a99989);
+	nv_icmd(dev, 0x0007ef, 0xf9e9d9c9);
+	nv_icmd(dev, 0x0007f0, 0x00003210);
+	nv_icmd(dev, 0x0007f1, 0x00007654);
+	nv_icmd(dev, 0x0007f2, 0x00000098);
+	nv_icmd(dev, 0x0005a5, 0x00000001);
+	nv_icmd(dev, 0x000980, 0x00000000);
+	nv_icmd(dev, 0x000981, 0x00000000);
+	nv_icmd(dev, 0x000982, 0x00000000);
+	nv_icmd(dev, 0x000983, 0x00000000);
+	nv_icmd(dev, 0x000984, 0x00000000);
+	nv_icmd(dev, 0x000985, 0x00000000);
+	nv_icmd(dev, 0x000986, 0x00000000);
+	nv_icmd(dev, 0x000987, 0x00000000);
+	nv_icmd(dev, 0x000988, 0x00000000);
+	nv_icmd(dev, 0x000989, 0x00000000);
+	nv_icmd(dev, 0x00098a, 0x00000000);
+	nv_icmd(dev, 0x00098b, 0x00000000);
+	nv_icmd(dev, 0x00098c, 0x00000000);
+	nv_icmd(dev, 0x00098d, 0x00000000);
+	nv_icmd(dev, 0x00098e, 0x00000000);
+	nv_icmd(dev, 0x00098f, 0x00000000);
+	nv_icmd(dev, 0x000990, 0x00000000);
+	nv_icmd(dev, 0x000991, 0x00000000);
+	nv_icmd(dev, 0x000992, 0x00000000);
+	nv_icmd(dev, 0x000993, 0x00000000);
+	nv_icmd(dev, 0x000994, 0x00000000);
+	nv_icmd(dev, 0x000995, 0x00000000);
+	nv_icmd(dev, 0x000996, 0x00000000);
+	nv_icmd(dev, 0x000997, 0x00000000);
+	nv_icmd(dev, 0x000998, 0x00000000);
+	nv_icmd(dev, 0x000999, 0x00000000);
+	nv_icmd(dev, 0x00099a, 0x00000000);
+	nv_icmd(dev, 0x00099b, 0x00000000);
+	nv_icmd(dev, 0x00099c, 0x00000000);
+	nv_icmd(dev, 0x00099d, 0x00000000);
+	nv_icmd(dev, 0x00099e, 0x00000000);
+	nv_icmd(dev, 0x00099f, 0x00000000);
+	nv_icmd(dev, 0x0009a0, 0x00000000);
+	nv_icmd(dev, 0x0009a1, 0x00000000);
+	nv_icmd(dev, 0x0009a2, 0x00000000);
+	nv_icmd(dev, 0x0009a3, 0x00000000);
+	nv_icmd(dev, 0x0009a4, 0x00000000);
+	nv_icmd(dev, 0x0009a5, 0x00000000);
+	nv_icmd(dev, 0x0009a6, 0x00000000);
+	nv_icmd(dev, 0x0009a7, 0x00000000);
+	nv_icmd(dev, 0x0009a8, 0x00000000);
+	nv_icmd(dev, 0x0009a9, 0x00000000);
+	nv_icmd(dev, 0x0009aa, 0x00000000);
+	nv_icmd(dev, 0x0009ab, 0x00000000);
+	nv_icmd(dev, 0x0009ac, 0x00000000);
+	nv_icmd(dev, 0x0009ad, 0x00000000);
+	nv_icmd(dev, 0x0009ae, 0x00000000);
+	nv_icmd(dev, 0x0009af, 0x00000000);
+	nv_icmd(dev, 0x0009b0, 0x00000000);
+	nv_icmd(dev, 0x0009b1, 0x00000000);
+	nv_icmd(dev, 0x0009b2, 0x00000000);
+	nv_icmd(dev, 0x0009b3, 0x00000000);
+	nv_icmd(dev, 0x0009b4, 0x00000000);
+	nv_icmd(dev, 0x0009b5, 0x00000000);
+	nv_icmd(dev, 0x0009b6, 0x00000000);
+	nv_icmd(dev, 0x0009b7, 0x00000000);
+	nv_icmd(dev, 0x0009b8, 0x00000000);
+	nv_icmd(dev, 0x0009b9, 0x00000000);
+	nv_icmd(dev, 0x0009ba, 0x00000000);
+	nv_icmd(dev, 0x0009bb, 0x00000000);
+	nv_icmd(dev, 0x0009bc, 0x00000000);
+	nv_icmd(dev, 0x0009bd, 0x00000000);
+	nv_icmd(dev, 0x0009be, 0x00000000);
+	nv_icmd(dev, 0x0009bf, 0x00000000);
+	nv_icmd(dev, 0x0009c0, 0x00000000);
+	nv_icmd(dev, 0x0009c1, 0x00000000);
+	nv_icmd(dev, 0x0009c2, 0x00000000);
+	nv_icmd(dev, 0x0009c3, 0x00000000);
+	nv_icmd(dev, 0x0009c4, 0x00000000);
+	nv_icmd(dev, 0x0009c5, 0x00000000);
+	nv_icmd(dev, 0x0009c6, 0x00000000);
+	nv_icmd(dev, 0x0009c7, 0x00000000);
+	nv_icmd(dev, 0x0009c8, 0x00000000);
+	nv_icmd(dev, 0x0009c9, 0x00000000);
+	nv_icmd(dev, 0x0009ca, 0x00000000);
+	nv_icmd(dev, 0x0009cb, 0x00000000);
+	nv_icmd(dev, 0x0009cc, 0x00000000);
+	nv_icmd(dev, 0x0009cd, 0x00000000);
+	nv_icmd(dev, 0x0009ce, 0x00000000);
+	nv_icmd(dev, 0x0009cf, 0x00000000);
+	nv_icmd(dev, 0x0009d0, 0x00000000);
+	nv_icmd(dev, 0x0009d1, 0x00000000);
+	nv_icmd(dev, 0x0009d2, 0x00000000);
+	nv_icmd(dev, 0x0009d3, 0x00000000);
+	nv_icmd(dev, 0x0009d4, 0x00000000);
+	nv_icmd(dev, 0x0009d5, 0x00000000);
+	nv_icmd(dev, 0x0009d6, 0x00000000);
+	nv_icmd(dev, 0x0009d7, 0x00000000);
+	nv_icmd(dev, 0x0009d8, 0x00000000);
+	nv_icmd(dev, 0x0009d9, 0x00000000);
+	nv_icmd(dev, 0x0009da, 0x00000000);
+	nv_icmd(dev, 0x0009db, 0x00000000);
+	nv_icmd(dev, 0x0009dc, 0x00000000);
+	nv_icmd(dev, 0x0009dd, 0x00000000);
+	nv_icmd(dev, 0x0009de, 0x00000000);
+	nv_icmd(dev, 0x0009df, 0x00000000);
+	nv_icmd(dev, 0x0009e0, 0x00000000);
+	nv_icmd(dev, 0x0009e1, 0x00000000);
+	nv_icmd(dev, 0x0009e2, 0x00000000);
+	nv_icmd(dev, 0x0009e3, 0x00000000);
+	nv_icmd(dev, 0x0009e4, 0x00000000);
+	nv_icmd(dev, 0x0009e5, 0x00000000);
+	nv_icmd(dev, 0x0009e6, 0x00000000);
+	nv_icmd(dev, 0x0009e7, 0x00000000);
+	nv_icmd(dev, 0x0009e8, 0x00000000);
+	nv_icmd(dev, 0x0009e9, 0x00000000);
+	nv_icmd(dev, 0x0009ea, 0x00000000);
+	nv_icmd(dev, 0x0009eb, 0x00000000);
+	nv_icmd(dev, 0x0009ec, 0x00000000);
+	nv_icmd(dev, 0x0009ed, 0x00000000);
+	nv_icmd(dev, 0x0009ee, 0x00000000);
+	nv_icmd(dev, 0x0009ef, 0x00000000);
+	nv_icmd(dev, 0x0009f0, 0x00000000);
+	nv_icmd(dev, 0x0009f1, 0x00000000);
+	nv_icmd(dev, 0x0009f2, 0x00000000);
+	nv_icmd(dev, 0x0009f3, 0x00000000);
+	nv_icmd(dev, 0x0009f4, 0x00000000);
+	nv_icmd(dev, 0x0009f5, 0x00000000);
+	nv_icmd(dev, 0x0009f6, 0x00000000);
+	nv_icmd(dev, 0x0009f7, 0x00000000);
+	nv_icmd(dev, 0x0009f8, 0x00000000);
+	nv_icmd(dev, 0x0009f9, 0x00000000);
+	nv_icmd(dev, 0x0009fa, 0x00000000);
+	nv_icmd(dev, 0x0009fb, 0x00000000);
+	nv_icmd(dev, 0x0009fc, 0x00000000);
+	nv_icmd(dev, 0x0009fd, 0x00000000);
+	nv_icmd(dev, 0x0009fe, 0x00000000);
+	nv_icmd(dev, 0x0009ff, 0x00000000);
+	nv_icmd(dev, 0x000468, 0x00000004);
+	nv_icmd(dev, 0x00046c, 0x00000001);
+	nv_icmd(dev, 0x000470, 0x00000000);
+	nv_icmd(dev, 0x000471, 0x00000000);
+	nv_icmd(dev, 0x000472, 0x00000000);
+	nv_icmd(dev, 0x000473, 0x00000000);
+	nv_icmd(dev, 0x000474, 0x00000000);
+	nv_icmd(dev, 0x000475, 0x00000000);
+	nv_icmd(dev, 0x000476, 0x00000000);
+	nv_icmd(dev, 0x000477, 0x00000000);
+	nv_icmd(dev, 0x000478, 0x00000000);
+	nv_icmd(dev, 0x000479, 0x00000000);
+	nv_icmd(dev, 0x00047a, 0x00000000);
+	nv_icmd(dev, 0x00047b, 0x00000000);
+	nv_icmd(dev, 0x00047c, 0x00000000);
+	nv_icmd(dev, 0x00047d, 0x00000000);
+	nv_icmd(dev, 0x00047e, 0x00000000);
+	nv_icmd(dev, 0x00047f, 0x00000000);
+	nv_icmd(dev, 0x000480, 0x00000000);
+	nv_icmd(dev, 0x000481, 0x00000000);
+	nv_icmd(dev, 0x000482, 0x00000000);
+	nv_icmd(dev, 0x000483, 0x00000000);
+	nv_icmd(dev, 0x000484, 0x00000000);
+	nv_icmd(dev, 0x000485, 0x00000000);
+	nv_icmd(dev, 0x000486, 0x00000000);
+	nv_icmd(dev, 0x000487, 0x00000000);
+	nv_icmd(dev, 0x000488, 0x00000000);
+	nv_icmd(dev, 0x000489, 0x00000000);
+	nv_icmd(dev, 0x00048a, 0x00000000);
+	nv_icmd(dev, 0x00048b, 0x00000000);
+	nv_icmd(dev, 0x00048c, 0x00000000);
+	nv_icmd(dev, 0x00048d, 0x00000000);
+	nv_icmd(dev, 0x00048e, 0x00000000);
+	nv_icmd(dev, 0x00048f, 0x00000000);
+	nv_icmd(dev, 0x000490, 0x00000000);
+	nv_icmd(dev, 0x000491, 0x00000000);
+	nv_icmd(dev, 0x000492, 0x00000000);
+	nv_icmd(dev, 0x000493, 0x00000000);
+	nv_icmd(dev, 0x000494, 0x00000000);
+	nv_icmd(dev, 0x000495, 0x00000000);
+	nv_icmd(dev, 0x000496, 0x00000000);
+	nv_icmd(dev, 0x000497, 0x00000000);
+	nv_icmd(dev, 0x000498, 0x00000000);
+	nv_icmd(dev, 0x000499, 0x00000000);
+	nv_icmd(dev, 0x00049a, 0x00000000);
+	nv_icmd(dev, 0x00049b, 0x00000000);
+	nv_icmd(dev, 0x00049c, 0x00000000);
+	nv_icmd(dev, 0x00049d, 0x00000000);
+	nv_icmd(dev, 0x00049e, 0x00000000);
+	nv_icmd(dev, 0x00049f, 0x00000000);
+	nv_icmd(dev, 0x0004a0, 0x00000000);
+	nv_icmd(dev, 0x0004a1, 0x00000000);
+	nv_icmd(dev, 0x0004a2, 0x00000000);
+	nv_icmd(dev, 0x0004a3, 0x00000000);
+	nv_icmd(dev, 0x0004a4, 0x00000000);
+	nv_icmd(dev, 0x0004a5, 0x00000000);
+	nv_icmd(dev, 0x0004a6, 0x00000000);
+	nv_icmd(dev, 0x0004a7, 0x00000000);
+	nv_icmd(dev, 0x0004a8, 0x00000000);
+	nv_icmd(dev, 0x0004a9, 0x00000000);
+	nv_icmd(dev, 0x0004aa, 0x00000000);
+	nv_icmd(dev, 0x0004ab, 0x00000000);
+	nv_icmd(dev, 0x0004ac, 0x00000000);
+	nv_icmd(dev, 0x0004ad, 0x00000000);
+	nv_icmd(dev, 0x0004ae, 0x00000000);
+	nv_icmd(dev, 0x0004af, 0x00000000);
+	nv_icmd(dev, 0x0004b0, 0x00000000);
+	nv_icmd(dev, 0x0004b1, 0x00000000);
+	nv_icmd(dev, 0x0004b2, 0x00000000);
+	nv_icmd(dev, 0x0004b3, 0x00000000);
+	nv_icmd(dev, 0x0004b4, 0x00000000);
+	nv_icmd(dev, 0x0004b5, 0x00000000);
+	nv_icmd(dev, 0x0004b6, 0x00000000);
+	nv_icmd(dev, 0x0004b7, 0x00000000);
+	nv_icmd(dev, 0x0004b8, 0x00000000);
+	nv_icmd(dev, 0x0004b9, 0x00000000);
+	nv_icmd(dev, 0x0004ba, 0x00000000);
+	nv_icmd(dev, 0x0004bb, 0x00000000);
+	nv_icmd(dev, 0x0004bc, 0x00000000);
+	nv_icmd(dev, 0x0004bd, 0x00000000);
+	nv_icmd(dev, 0x0004be, 0x00000000);
+	nv_icmd(dev, 0x0004bf, 0x00000000);
+	nv_icmd(dev, 0x0004c0, 0x00000000);
+	nv_icmd(dev, 0x0004c1, 0x00000000);
+	nv_icmd(dev, 0x0004c2, 0x00000000);
+	nv_icmd(dev, 0x0004c3, 0x00000000);
+	nv_icmd(dev, 0x0004c4, 0x00000000);
+	nv_icmd(dev, 0x0004c5, 0x00000000);
+	nv_icmd(dev, 0x0004c6, 0x00000000);
+	nv_icmd(dev, 0x0004c7, 0x00000000);
+	nv_icmd(dev, 0x0004c8, 0x00000000);
+	nv_icmd(dev, 0x0004c9, 0x00000000);
+	nv_icmd(dev, 0x0004ca, 0x00000000);
+	nv_icmd(dev, 0x0004cb, 0x00000000);
+	nv_icmd(dev, 0x0004cc, 0x00000000);
+	nv_icmd(dev, 0x0004cd, 0x00000000);
+	nv_icmd(dev, 0x0004ce, 0x00000000);
+	nv_icmd(dev, 0x0004cf, 0x00000000);
+	nv_icmd(dev, 0x000510, 0x3f800000);
+	nv_icmd(dev, 0x000511, 0x3f800000);
+	nv_icmd(dev, 0x000512, 0x3f800000);
+	nv_icmd(dev, 0x000513, 0x3f800000);
+	nv_icmd(dev, 0x000514, 0x3f800000);
+	nv_icmd(dev, 0x000515, 0x3f800000);
+	nv_icmd(dev, 0x000516, 0x3f800000);
+	nv_icmd(dev, 0x000517, 0x3f800000);
+	nv_icmd(dev, 0x000518, 0x3f800000);
+	nv_icmd(dev, 0x000519, 0x3f800000);
+	nv_icmd(dev, 0x00051a, 0x3f800000);
+	nv_icmd(dev, 0x00051b, 0x3f800000);
+	nv_icmd(dev, 0x00051c, 0x3f800000);
+	nv_icmd(dev, 0x00051d, 0x3f800000);
+	nv_icmd(dev, 0x00051e, 0x3f800000);
+	nv_icmd(dev, 0x00051f, 0x3f800000);
+	nv_icmd(dev, 0x000520, 0x000002b6);
+	nv_icmd(dev, 0x000529, 0x00000001);
+	nv_icmd(dev, 0x000530, 0xffff0000);
+	nv_icmd(dev, 0x000531, 0xffff0000);
+	nv_icmd(dev, 0x000532, 0xffff0000);
+	nv_icmd(dev, 0x000533, 0xffff0000);
+	nv_icmd(dev, 0x000534, 0xffff0000);
+	nv_icmd(dev, 0x000535, 0xffff0000);
+	nv_icmd(dev, 0x000536, 0xffff0000);
+	nv_icmd(dev, 0x000537, 0xffff0000);
+	nv_icmd(dev, 0x000538, 0xffff0000);
+	nv_icmd(dev, 0x000539, 0xffff0000);
+	nv_icmd(dev, 0x00053a, 0xffff0000);
+	nv_icmd(dev, 0x00053b, 0xffff0000);
+	nv_icmd(dev, 0x00053c, 0xffff0000);
+	nv_icmd(dev, 0x00053d, 0xffff0000);
+	nv_icmd(dev, 0x00053e, 0xffff0000);
+	nv_icmd(dev, 0x00053f, 0xffff0000);
+	nv_icmd(dev, 0x000585, 0x0000003f);
+	nv_icmd(dev, 0x000576, 0x00000003);
+	nv_icmd(dev, 0x00057b, 0x00000059);
+	nv_icmd(dev, 0x000586, 0x00000040);
+	nv_icmd(dev, 0x000582, 0x00000080);
+	nv_icmd(dev, 0x000583, 0x00000080);
+	nv_icmd(dev, 0x0005c2, 0x00000001);
+	nv_icmd(dev, 0x000638, 0x00000001);
+	nv_icmd(dev, 0x000639, 0x00000001);
+	nv_icmd(dev, 0x00063a, 0x00000002);
+	nv_icmd(dev, 0x00063b, 0x00000001);
+	nv_icmd(dev, 0x00063c, 0x00000001);
+	nv_icmd(dev, 0x00063d, 0x00000002);
+	nv_icmd(dev, 0x00063e, 0x00000001);
+	nv_icmd(dev, 0x0008b8, 0x00000001);
+	nv_icmd(dev, 0x0008b9, 0x00000001);
+	nv_icmd(dev, 0x0008ba, 0x00000001);
+	nv_icmd(dev, 0x0008bb, 0x00000001);
+	nv_icmd(dev, 0x0008bc, 0x00000001);
+	nv_icmd(dev, 0x0008bd, 0x00000001);
+	nv_icmd(dev, 0x0008be, 0x00000001);
+	nv_icmd(dev, 0x0008bf, 0x00000001);
+	nv_icmd(dev, 0x000900, 0x00000001);
+	nv_icmd(dev, 0x000901, 0x00000001);
+	nv_icmd(dev, 0x000902, 0x00000001);
+	nv_icmd(dev, 0x000903, 0x00000001);
+	nv_icmd(dev, 0x000904, 0x00000001);
+	nv_icmd(dev, 0x000905, 0x00000001);
+	nv_icmd(dev, 0x000906, 0x00000001);
+	nv_icmd(dev, 0x000907, 0x00000001);
+	nv_icmd(dev, 0x000908, 0x00000002);
+	nv_icmd(dev, 0x000909, 0x00000002);
+	nv_icmd(dev, 0x00090a, 0x00000002);
+	nv_icmd(dev, 0x00090b, 0x00000002);
+	nv_icmd(dev, 0x00090c, 0x00000002);
+	nv_icmd(dev, 0x00090d, 0x00000002);
+	nv_icmd(dev, 0x00090e, 0x00000002);
+	nv_icmd(dev, 0x00090f, 0x00000002);
+	nv_icmd(dev, 0x000910, 0x00000001);
+	nv_icmd(dev, 0x000911, 0x00000001);
+	nv_icmd(dev, 0x000912, 0x00000001);
+	nv_icmd(dev, 0x000913, 0x00000001);
+	nv_icmd(dev, 0x000914, 0x00000001);
+	nv_icmd(dev, 0x000915, 0x00000001);
+	nv_icmd(dev, 0x000916, 0x00000001);
+	nv_icmd(dev, 0x000917, 0x00000001);
+	nv_icmd(dev, 0x000918, 0x00000001);
+	nv_icmd(dev, 0x000919, 0x00000001);
+	nv_icmd(dev, 0x00091a, 0x00000001);
+	nv_icmd(dev, 0x00091b, 0x00000001);
+	nv_icmd(dev, 0x00091c, 0x00000001);
+	nv_icmd(dev, 0x00091d, 0x00000001);
+	nv_icmd(dev, 0x00091e, 0x00000001);
+	nv_icmd(dev, 0x00091f, 0x00000001);
+	nv_icmd(dev, 0x000920, 0x00000002);
+	nv_icmd(dev, 0x000921, 0x00000002);
+	nv_icmd(dev, 0x000922, 0x00000002);
+	nv_icmd(dev, 0x000923, 0x00000002);
+	nv_icmd(dev, 0x000924, 0x00000002);
+	nv_icmd(dev, 0x000925, 0x00000002);
+	nv_icmd(dev, 0x000926, 0x00000002);
+	nv_icmd(dev, 0x000927, 0x00000002);
+	nv_icmd(dev, 0x000928, 0x00000001);
+	nv_icmd(dev, 0x000929, 0x00000001);
+	nv_icmd(dev, 0x00092a, 0x00000001);
+	nv_icmd(dev, 0x00092b, 0x00000001);
+	nv_icmd(dev, 0x00092c, 0x00000001);
+	nv_icmd(dev, 0x00092d, 0x00000001);
+	nv_icmd(dev, 0x00092e, 0x00000001);
+	nv_icmd(dev, 0x00092f, 0x00000001);
+	nv_icmd(dev, 0x000648, 0x00000001);
+	nv_icmd(dev, 0x000649, 0x00000001);
+	nv_icmd(dev, 0x00064a, 0x00000001);
+	nv_icmd(dev, 0x00064b, 0x00000001);
+	nv_icmd(dev, 0x00064c, 0x00000001);
+	nv_icmd(dev, 0x00064d, 0x00000001);
+	nv_icmd(dev, 0x00064e, 0x00000001);
+	nv_icmd(dev, 0x00064f, 0x00000001);
+	nv_icmd(dev, 0x000650, 0x00000001);
+	nv_icmd(dev, 0x000658, 0x0000000f);
+	nv_icmd(dev, 0x0007ff, 0x0000000a);
+	nv_icmd(dev, 0x00066a, 0x40000000);
+	nv_icmd(dev, 0x00066b, 0x10000000);
+	nv_icmd(dev, 0x00066c, 0xffff0000);
+	nv_icmd(dev, 0x00066d, 0xffff0000);
+	nv_icmd(dev, 0x0007af, 0x00000008);
+	nv_icmd(dev, 0x0007b0, 0x00000008);
+	nv_icmd(dev, 0x0007f6, 0x00000001);
+	nv_icmd(dev, 0x0006b2, 0x00000055);
+	nv_icmd(dev, 0x0007ad, 0x00000003);
+	nv_icmd(dev, 0x000937, 0x00000001);
+	nv_icmd(dev, 0x000971, 0x00000008);
+	nv_icmd(dev, 0x000972, 0x00000040);
+	nv_icmd(dev, 0x000973, 0x0000012c);
+	nv_icmd(dev, 0x00097c, 0x00000040);
+	nv_icmd(dev, 0x000979, 0x00000003);
+	nv_icmd(dev, 0x000975, 0x00000020);
+	nv_icmd(dev, 0x000976, 0x00000001);
+	nv_icmd(dev, 0x000977, 0x00000020);
+	nv_icmd(dev, 0x000978, 0x00000001);
+	nv_icmd(dev, 0x000957, 0x00000003);
+	nv_icmd(dev, 0x00095e, 0x20164010);
+	nv_icmd(dev, 0x00095f, 0x00000020);
+	nv_icmd(dev, 0x00097d, 0x00000020);
+	nv_icmd(dev, 0x000683, 0x00000006);
+	nv_icmd(dev, 0x000685, 0x003fffff);
+	nv_icmd(dev, 0x000687, 0x003fffff);
+	nv_icmd(dev, 0x0006a0, 0x00000005);
+	nv_icmd(dev, 0x000840, 0x00400008);
+	nv_icmd(dev, 0x000841, 0x08000080);
+	nv_icmd(dev, 0x000842, 0x00400008);
+	nv_icmd(dev, 0x000843, 0x08000080);
+	nv_icmd(dev, 0x000818, 0x00000000);
+	nv_icmd(dev, 0x000819, 0x00000000);
+	nv_icmd(dev, 0x00081a, 0x00000000);
+	nv_icmd(dev, 0x00081b, 0x00000000);
+	nv_icmd(dev, 0x00081c, 0x00000000);
+	nv_icmd(dev, 0x00081d, 0x00000000);
+	nv_icmd(dev, 0x00081e, 0x00000000);
+	nv_icmd(dev, 0x00081f, 0x00000000);
+	nv_icmd(dev, 0x000848, 0x00000000);
+	nv_icmd(dev, 0x000849, 0x00000000);
+	nv_icmd(dev, 0x00084a, 0x00000000);
+	nv_icmd(dev, 0x00084b, 0x00000000);
+	nv_icmd(dev, 0x00084c, 0x00000000);
+	nv_icmd(dev, 0x00084d, 0x00000000);
+	nv_icmd(dev, 0x00084e, 0x00000000);
+	nv_icmd(dev, 0x00084f, 0x00000000);
+	nv_icmd(dev, 0x000850, 0x00000000);
+	nv_icmd(dev, 0x000851, 0x00000000);
+	nv_icmd(dev, 0x000852, 0x00000000);
+	nv_icmd(dev, 0x000853, 0x00000000);
+	nv_icmd(dev, 0x000854, 0x00000000);
+	nv_icmd(dev, 0x000855, 0x00000000);
+	nv_icmd(dev, 0x000856, 0x00000000);
+	nv_icmd(dev, 0x000857, 0x00000000);
+	nv_icmd(dev, 0x000738, 0x00000000);
+	nv_icmd(dev, 0x0006aa, 0x00000001);
+	nv_icmd(dev, 0x0006ab, 0x00000002);
+	nv_icmd(dev, 0x0006ac, 0x00000080);
+	nv_icmd(dev, 0x0006ad, 0x00000100);
+	nv_icmd(dev, 0x0006ae, 0x00000100);
+	nv_icmd(dev, 0x0006b1, 0x00000011);
+	nv_icmd(dev, 0x0006bb, 0x000000cf);
+	nv_icmd(dev, 0x0006ce, 0x2a712488);
+	nv_icmd(dev, 0x000739, 0x4085c000);
+	nv_icmd(dev, 0x00073a, 0x00000080);
+	nv_icmd(dev, 0x000786, 0x80000100);
+	nv_icmd(dev, 0x00073c, 0x00010100);
+	nv_icmd(dev, 0x00073d, 0x02800000);
+	nv_icmd(dev, 0x000787, 0x000000cf);
+	nv_icmd(dev, 0x00078c, 0x00000008);
+	nv_icmd(dev, 0x000792, 0x00000001);
+	nv_icmd(dev, 0x000794, 0x00000001);
+	nv_icmd(dev, 0x000795, 0x00000001);
+	nv_icmd(dev, 0x000796, 0x00000001);
+	nv_icmd(dev, 0x000797, 0x000000cf);
+	nv_icmd(dev, 0x000836, 0x00000001);
+	nv_icmd(dev, 0x00079a, 0x00000002);
+	nv_icmd(dev, 0x000833, 0x04444480);
+	nv_icmd(dev, 0x0007a1, 0x00000001);
+	nv_icmd(dev, 0x0007a3, 0x00000001);
+	nv_icmd(dev, 0x0007a4, 0x00000001);
+	nv_icmd(dev, 0x0007a5, 0x00000001);
+	nv_icmd(dev, 0x000831, 0x00000004);
+	nv_icmd(dev, 0x000b07, 0x00000002);
+	nv_icmd(dev, 0x000b08, 0x00000100);
+	nv_icmd(dev, 0x000b09, 0x00000100);
+	nv_icmd(dev, 0x000b0a, 0x00000001);
+	nv_icmd(dev, 0x000a04, 0x000000ff);
+	nv_icmd(dev, 0x000a0b, 0x00000040);
+	nv_icmd(dev, 0x00097f, 0x00000100);
+	nv_icmd(dev, 0x000a02, 0x00000001);
+	nv_icmd(dev, 0x000809, 0x00000007);
+	nv_icmd(dev, 0x00c221, 0x00000040);
+	nv_icmd(dev, 0x00c1b0, 0x0000000f);
+	nv_icmd(dev, 0x00c1b1, 0x0000000f);
+	nv_icmd(dev, 0x00c1b2, 0x0000000f);
+	nv_icmd(dev, 0x00c1b3, 0x0000000f);
+	nv_icmd(dev, 0x00c1b4, 0x0000000f);
+	nv_icmd(dev, 0x00c1b5, 0x0000000f);
+	nv_icmd(dev, 0x00c1b6, 0x0000000f);
+	nv_icmd(dev, 0x00c1b7, 0x0000000f);
+	nv_icmd(dev, 0x00c1b8, 0x0fac6881);
+	nv_icmd(dev, 0x00c1b9, 0x00fac688);
+	nv_icmd(dev, 0x00c401, 0x00000001);
+	nv_icmd(dev, 0x00c402, 0x00010001);
+	nv_icmd(dev, 0x00c403, 0x00000001);
+	nv_icmd(dev, 0x00c404, 0x00000001);
+	nv_icmd(dev, 0x00c40e, 0x00000020);
+	nv_icmd(dev, 0x00c500, 0x00000003);
+	nv_icmd(dev, 0x01e100, 0x00000001);
+	nv_icmd(dev, 0x001000, 0x00000002);
+	nv_icmd(dev, 0x0006aa, 0x00000001);
+	nv_icmd(dev, 0x0006ad, 0x00000100);
+	nv_icmd(dev, 0x0006ae, 0x00000100);
+	nv_icmd(dev, 0x0006b1, 0x00000011);
+	nv_icmd(dev, 0x00078c, 0x00000008);
+	nv_icmd(dev, 0x000792, 0x00000001);
+	nv_icmd(dev, 0x000794, 0x00000001);
+	nv_icmd(dev, 0x000795, 0x00000001);
+	nv_icmd(dev, 0x000796, 0x00000001);
+	nv_icmd(dev, 0x000797, 0x000000cf);
+	nv_icmd(dev, 0x00079a, 0x00000002);
+	nv_icmd(dev, 0x000833, 0x04444480);
+	nv_icmd(dev, 0x0007a1, 0x00000001);
+	nv_icmd(dev, 0x0007a3, 0x00000001);
+	nv_icmd(dev, 0x0007a4, 0x00000001);
+	nv_icmd(dev, 0x0007a5, 0x00000001);
+	nv_icmd(dev, 0x000831, 0x00000004);
+	nv_icmd(dev, 0x01e100, 0x00000001);
+	nv_icmd(dev, 0x001000, 0x00000008);
+	nv_icmd(dev, 0x000039, 0x00000000);
+	nv_icmd(dev, 0x00003a, 0x00000000);
+	nv_icmd(dev, 0x00003b, 0x00000000);
+	nv_icmd(dev, 0x000380, 0x00000001);
+	nv_icmd(dev, 0x000366, 0x00000000);
+	nv_icmd(dev, 0x000367, 0x00000000);
+	nv_icmd(dev, 0x000368, 0x00000fff);
+	nv_icmd(dev, 0x000370, 0x00000000);
+	nv_icmd(dev, 0x000371, 0x00000000);
+	nv_icmd(dev, 0x000372, 0x000fffff);
+	nv_icmd(dev, 0x000813, 0x00000006);
+	nv_icmd(dev, 0x000814, 0x00000008);
+	nv_icmd(dev, 0x000957, 0x00000003);
+	nv_icmd(dev, 0x000818, 0x00000000);
+	nv_icmd(dev, 0x000819, 0x00000000);
+	nv_icmd(dev, 0x00081a, 0x00000000);
+	nv_icmd(dev, 0x00081b, 0x00000000);
+	nv_icmd(dev, 0x00081c, 0x00000000);
+	nv_icmd(dev, 0x00081d, 0x00000000);
+	nv_icmd(dev, 0x00081e, 0x00000000);
+	nv_icmd(dev, 0x00081f, 0x00000000);
+	nv_icmd(dev, 0x000848, 0x00000000);
+	nv_icmd(dev, 0x000849, 0x00000000);
+	nv_icmd(dev, 0x00084a, 0x00000000);
+	nv_icmd(dev, 0x00084b, 0x00000000);
+	nv_icmd(dev, 0x00084c, 0x00000000);
+	nv_icmd(dev, 0x00084d, 0x00000000);
+	nv_icmd(dev, 0x00084e, 0x00000000);
+	nv_icmd(dev, 0x00084f, 0x00000000);
+	nv_icmd(dev, 0x000850, 0x00000000);
+	nv_icmd(dev, 0x000851, 0x00000000);
+	nv_icmd(dev, 0x000852, 0x00000000);
+	nv_icmd(dev, 0x000853, 0x00000000);
+	nv_icmd(dev, 0x000854, 0x00000000);
+	nv_icmd(dev, 0x000855, 0x00000000);
+	nv_icmd(dev, 0x000856, 0x00000000);
+	nv_icmd(dev, 0x000857, 0x00000000);
+	nv_icmd(dev, 0x000738, 0x00000000);
+	nv_icmd(dev, 0x000b07, 0x00000002);
+	nv_icmd(dev, 0x000b08, 0x00000100);
+	nv_icmd(dev, 0x000b09, 0x00000100);
+	nv_icmd(dev, 0x000b0a, 0x00000001);
+	nv_icmd(dev, 0x000a04, 0x000000ff);
+	nv_icmd(dev, 0x00097f, 0x00000100);
+	nv_icmd(dev, 0x000a02, 0x00000001);
+	nv_icmd(dev, 0x000809, 0x00000007);
+	nv_icmd(dev, 0x00c221, 0x00000040);
+	nv_icmd(dev, 0x00c401, 0x00000001);
+	nv_icmd(dev, 0x00c402, 0x00010001);
+	nv_icmd(dev, 0x00c403, 0x00000001);
+	nv_icmd(dev, 0x00c404, 0x00000001);
+	nv_icmd(dev, 0x00c40e, 0x00000020);
+	nv_icmd(dev, 0x00c500, 0x00000003);
+	nv_icmd(dev, 0x01e100, 0x00000001);
+	nv_icmd(dev, 0x001000, 0x00000001);
+	nv_icmd(dev, 0x000b07, 0x00000002);
+	nv_icmd(dev, 0x000b08, 0x00000100);
+	nv_icmd(dev, 0x000b09, 0x00000100);
+	nv_icmd(dev, 0x000b0a, 0x00000001);
+	nv_icmd(dev, 0x01e100, 0x00000001);
+	nv_wr32(dev, 0x400208, 0x00000000);
+}
+
+static void
+nv_mthd(struct drm_device *dev, u32 class, u32 mthd, u32 data)
+{
+	nv_wr32(dev, 0x40448c, data);
+	nv_wr32(dev, 0x404488, 0x80000000 | (mthd << 14) | class);
+}
+
+static void
+nve0_grctx_generate_a097(struct drm_device *dev)
+{
+	nv_mthd(dev, 0xa097, 0x0800, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0840, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0880, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x08c0, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0900, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0940, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0980, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x09c0, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0804, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0844, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0884, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x08c4, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0904, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0944, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0984, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x09c4, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0808, 0x00000400);
+	nv_mthd(dev, 0xa097, 0x0848, 0x00000400);
+	nv_mthd(dev, 0xa097, 0x0888, 0x00000400);
+	nv_mthd(dev, 0xa097, 0x08c8, 0x00000400);
+	nv_mthd(dev, 0xa097, 0x0908, 0x00000400);
+	nv_mthd(dev, 0xa097, 0x0948, 0x00000400);
+	nv_mthd(dev, 0xa097, 0x0988, 0x00000400);
+	nv_mthd(dev, 0xa097, 0x09c8, 0x00000400);
+	nv_mthd(dev, 0xa097, 0x080c, 0x00000300);
+	nv_mthd(dev, 0xa097, 0x084c, 0x00000300);
+	nv_mthd(dev, 0xa097, 0x088c, 0x00000300);
+	nv_mthd(dev, 0xa097, 0x08cc, 0x00000300);
+	nv_mthd(dev, 0xa097, 0x090c, 0x00000300);
+	nv_mthd(dev, 0xa097, 0x094c, 0x00000300);
+	nv_mthd(dev, 0xa097, 0x098c, 0x00000300);
+	nv_mthd(dev, 0xa097, 0x09cc, 0x00000300);
+	nv_mthd(dev, 0xa097, 0x0810, 0x000000cf);
+	nv_mthd(dev, 0xa097, 0x0850, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0890, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x08d0, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0910, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0950, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0990, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x09d0, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0814, 0x00000040);
+	nv_mthd(dev, 0xa097, 0x0854, 0x00000040);
+	nv_mthd(dev, 0xa097, 0x0894, 0x00000040);
+	nv_mthd(dev, 0xa097, 0x08d4, 0x00000040);
+	nv_mthd(dev, 0xa097, 0x0914, 0x00000040);
+	nv_mthd(dev, 0xa097, 0x0954, 0x00000040);
+	nv_mthd(dev, 0xa097, 0x0994, 0x00000040);
+	nv_mthd(dev, 0xa097, 0x09d4, 0x00000040);
+	nv_mthd(dev, 0xa097, 0x0818, 0x00000001);
+	nv_mthd(dev, 0xa097, 0x0858, 0x00000001);
+	nv_mthd(dev, 0xa097, 0x0898, 0x00000001);
+	nv_mthd(dev, 0xa097, 0x08d8, 0x00000001);
+	nv_mthd(dev, 0xa097, 0x0918, 0x00000001);
+	nv_mthd(dev, 0xa097, 0x0958, 0x00000001);
+	nv_mthd(dev, 0xa097, 0x0998, 0x00000001);
+	nv_mthd(dev, 0xa097, 0x09d8, 0x00000001);
+	nv_mthd(dev, 0xa097, 0x081c, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x085c, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x089c, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x08dc, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x091c, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x095c, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x099c, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x09dc, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0820, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0860, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x08a0, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x08e0, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0920, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0960, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x09a0, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x09e0, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1c00, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1c10, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1c20, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1c30, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1c40, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1c50, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1c60, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1c70, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1c80, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1c90, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1ca0, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1cb0, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1cc0, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1cd0, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1ce0, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1cf0, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1c04, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1c14, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1c24, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1c34, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1c44, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1c54, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1c64, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1c74, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1c84, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1c94, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1ca4, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1cb4, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1cc4, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1cd4, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1ce4, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1cf4, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1c08, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1c18, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1c28, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1c38, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1c48, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1c58, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1c68, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1c78, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1c88, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1c98, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1ca8, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1cb8, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1cc8, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1cd8, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1ce8, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1cf8, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1c0c, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1c1c, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1c2c, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1c3c, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1c4c, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1c5c, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1c6c, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1c7c, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1c8c, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1c9c, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1cac, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1cbc, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1ccc, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1cdc, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1cec, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1cfc, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1d00, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1d10, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1d20, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1d30, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1d40, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1d50, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1d60, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1d70, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1d80, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1d90, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1da0, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1db0, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1dc0, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1dd0, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1de0, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1df0, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1d04, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1d14, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1d24, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1d34, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1d44, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1d54, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1d64, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1d74, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1d84, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1d94, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1da4, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1db4, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1dc4, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1dd4, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1de4, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1df4, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1d08, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1d18, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1d28, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1d38, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1d48, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1d58, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1d68, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1d78, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1d88, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1d98, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1da8, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1db8, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1dc8, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1dd8, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1de8, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1df8, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1d0c, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1d1c, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1d2c, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1d3c, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1d4c, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1d5c, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1d6c, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1d7c, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1d8c, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1d9c, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1dac, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1dbc, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1dcc, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1ddc, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1dec, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1dfc, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1f00, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1f08, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1f10, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1f18, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1f20, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1f28, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1f30, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1f38, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1f40, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1f48, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1f50, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1f58, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1f60, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1f68, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1f70, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1f78, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1f04, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1f0c, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1f14, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1f1c, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1f24, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1f2c, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1f34, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1f3c, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1f44, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1f4c, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1f54, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1f5c, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1f64, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1f6c, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1f74, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1f7c, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1f80, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1f88, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1f90, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1f98, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1fa0, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1fa8, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1fb0, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1fb8, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1fc0, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1fc8, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1fd0, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1fd8, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1fe0, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1fe8, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1ff0, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1ff8, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1f84, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1f8c, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1f94, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1f9c, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1fa4, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1fac, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1fb4, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1fbc, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1fc4, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1fcc, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1fd4, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1fdc, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1fe4, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1fec, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1ff4, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1ffc, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x2000, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x2040, 0x00000011);
+	nv_mthd(dev, 0xa097, 0x2080, 0x00000020);
+	nv_mthd(dev, 0xa097, 0x20c0, 0x00000030);
+	nv_mthd(dev, 0xa097, 0x2100, 0x00000040);
+	nv_mthd(dev, 0xa097, 0x2140, 0x00000051);
+	nv_mthd(dev, 0xa097, 0x200c, 0x00000001);
+	nv_mthd(dev, 0xa097, 0x204c, 0x00000001);
+	nv_mthd(dev, 0xa097, 0x208c, 0x00000001);
+	nv_mthd(dev, 0xa097, 0x20cc, 0x00000001);
+	nv_mthd(dev, 0xa097, 0x210c, 0x00000001);
+	nv_mthd(dev, 0xa097, 0x214c, 0x00000001);
+	nv_mthd(dev, 0xa097, 0x2010, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x2050, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x2090, 0x00000001);
+	nv_mthd(dev, 0xa097, 0x20d0, 0x00000002);
+	nv_mthd(dev, 0xa097, 0x2110, 0x00000003);
+	nv_mthd(dev, 0xa097, 0x2150, 0x00000004);
+	nv_mthd(dev, 0xa097, 0x0380, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x03a0, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x03c0, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x03e0, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0384, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x03a4, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x03c4, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x03e4, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0388, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x03a8, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x03c8, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x03e8, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x038c, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x03ac, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x03cc, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x03ec, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0700, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0710, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0720, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0730, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0704, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0714, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0724, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0734, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0708, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0718, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0728, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0738, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x2800, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x2804, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x2808, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x280c, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x2810, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x2814, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x2818, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x281c, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x2820, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x2824, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x2828, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x282c, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x2830, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x2834, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x2838, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x283c, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x2840, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x2844, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x2848, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x284c, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x2850, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x2854, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x2858, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x285c, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x2860, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x2864, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x2868, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x286c, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x2870, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x2874, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x2878, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x287c, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x2880, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x2884, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x2888, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x288c, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x2890, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x2894, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x2898, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x289c, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x28a0, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x28a4, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x28a8, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x28ac, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x28b0, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x28b4, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x28b8, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x28bc, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x28c0, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x28c4, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x28c8, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x28cc, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x28d0, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x28d4, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x28d8, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x28dc, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x28e0, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x28e4, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x28e8, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x28ec, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x28f0, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x28f4, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x28f8, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x28fc, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x2900, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x2904, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x2908, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x290c, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x2910, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x2914, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x2918, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x291c, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x2920, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x2924, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x2928, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x292c, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x2930, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x2934, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x2938, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x293c, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x2940, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x2944, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x2948, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x294c, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x2950, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x2954, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x2958, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x295c, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x2960, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x2964, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x2968, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x296c, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x2970, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x2974, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x2978, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x297c, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x2980, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x2984, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x2988, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x298c, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x2990, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x2994, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x2998, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x299c, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x29a0, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x29a4, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x29a8, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x29ac, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x29b0, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x29b4, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x29b8, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x29bc, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x29c0, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x29c4, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x29c8, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x29cc, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x29d0, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x29d4, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x29d8, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x29dc, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x29e0, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x29e4, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x29e8, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x29ec, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x29f0, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x29f4, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x29f8, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x29fc, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0a00, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0a20, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0a40, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0a60, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0a80, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0aa0, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0ac0, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0ae0, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0b00, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0b20, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0b40, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0b60, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0b80, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0ba0, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0bc0, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0be0, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0a04, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0a24, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0a44, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0a64, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0a84, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0aa4, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0ac4, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0ae4, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0b04, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0b24, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0b44, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0b64, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0b84, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0ba4, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0bc4, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0be4, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0a08, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0a28, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0a48, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0a68, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0a88, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0aa8, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0ac8, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0ae8, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0b08, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0b28, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0b48, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0b68, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0b88, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0ba8, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0bc8, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0be8, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0a0c, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0a2c, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0a4c, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0a6c, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0a8c, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0aac, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0acc, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0aec, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0b0c, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0b2c, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0b4c, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0b6c, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0b8c, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0bac, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0bcc, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0bec, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0a10, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0a30, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0a50, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0a70, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0a90, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0ab0, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0ad0, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0af0, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0b10, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0b30, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0b50, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0b70, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0b90, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0bb0, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0bd0, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0bf0, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0a14, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0a34, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0a54, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0a74, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0a94, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0ab4, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0ad4, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0af4, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0b14, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0b34, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0b54, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0b74, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0b94, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0bb4, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0bd4, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0bf4, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0c00, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0c10, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0c20, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0c30, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0c40, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0c50, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0c60, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0c70, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0c80, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0c90, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0ca0, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0cb0, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0cc0, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0cd0, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0ce0, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0cf0, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0c04, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0c14, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0c24, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0c34, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0c44, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0c54, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0c64, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0c74, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0c84, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0c94, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0ca4, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0cb4, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0cc4, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0cd4, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0ce4, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0cf4, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0c08, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0c18, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0c28, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0c38, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0c48, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0c58, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0c68, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0c78, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0c88, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0c98, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0ca8, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0cb8, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0cc8, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0cd8, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0ce8, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0cf8, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0c0c, 0x3f800000);
+	nv_mthd(dev, 0xa097, 0x0c1c, 0x3f800000);
+	nv_mthd(dev, 0xa097, 0x0c2c, 0x3f800000);
+	nv_mthd(dev, 0xa097, 0x0c3c, 0x3f800000);
+	nv_mthd(dev, 0xa097, 0x0c4c, 0x3f800000);
+	nv_mthd(dev, 0xa097, 0x0c5c, 0x3f800000);
+	nv_mthd(dev, 0xa097, 0x0c6c, 0x3f800000);
+	nv_mthd(dev, 0xa097, 0x0c7c, 0x3f800000);
+	nv_mthd(dev, 0xa097, 0x0c8c, 0x3f800000);
+	nv_mthd(dev, 0xa097, 0x0c9c, 0x3f800000);
+	nv_mthd(dev, 0xa097, 0x0cac, 0x3f800000);
+	nv_mthd(dev, 0xa097, 0x0cbc, 0x3f800000);
+	nv_mthd(dev, 0xa097, 0x0ccc, 0x3f800000);
+	nv_mthd(dev, 0xa097, 0x0cdc, 0x3f800000);
+	nv_mthd(dev, 0xa097, 0x0cec, 0x3f800000);
+	nv_mthd(dev, 0xa097, 0x0cfc, 0x3f800000);
+	nv_mthd(dev, 0xa097, 0x0d00, 0xffff0000);
+	nv_mthd(dev, 0xa097, 0x0d08, 0xffff0000);
+	nv_mthd(dev, 0xa097, 0x0d10, 0xffff0000);
+	nv_mthd(dev, 0xa097, 0x0d18, 0xffff0000);
+	nv_mthd(dev, 0xa097, 0x0d20, 0xffff0000);
+	nv_mthd(dev, 0xa097, 0x0d28, 0xffff0000);
+	nv_mthd(dev, 0xa097, 0x0d30, 0xffff0000);
+	nv_mthd(dev, 0xa097, 0x0d38, 0xffff0000);
+	nv_mthd(dev, 0xa097, 0x0d04, 0xffff0000);
+	nv_mthd(dev, 0xa097, 0x0d0c, 0xffff0000);
+	nv_mthd(dev, 0xa097, 0x0d14, 0xffff0000);
+	nv_mthd(dev, 0xa097, 0x0d1c, 0xffff0000);
+	nv_mthd(dev, 0xa097, 0x0d24, 0xffff0000);
+	nv_mthd(dev, 0xa097, 0x0d2c, 0xffff0000);
+	nv_mthd(dev, 0xa097, 0x0d34, 0xffff0000);
+	nv_mthd(dev, 0xa097, 0x0d3c, 0xffff0000);
+	nv_mthd(dev, 0xa097, 0x0e00, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0e10, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0e20, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0e30, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0e40, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0e50, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0e60, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0e70, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0e80, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0e90, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0ea0, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0eb0, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0ec0, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0ed0, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0ee0, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0ef0, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0e04, 0xffff0000);
+	nv_mthd(dev, 0xa097, 0x0e14, 0xffff0000);
+	nv_mthd(dev, 0xa097, 0x0e24, 0xffff0000);
+	nv_mthd(dev, 0xa097, 0x0e34, 0xffff0000);
+	nv_mthd(dev, 0xa097, 0x0e44, 0xffff0000);
+	nv_mthd(dev, 0xa097, 0x0e54, 0xffff0000);
+	nv_mthd(dev, 0xa097, 0x0e64, 0xffff0000);
+	nv_mthd(dev, 0xa097, 0x0e74, 0xffff0000);
+	nv_mthd(dev, 0xa097, 0x0e84, 0xffff0000);
+	nv_mthd(dev, 0xa097, 0x0e94, 0xffff0000);
+	nv_mthd(dev, 0xa097, 0x0ea4, 0xffff0000);
+	nv_mthd(dev, 0xa097, 0x0eb4, 0xffff0000);
+	nv_mthd(dev, 0xa097, 0x0ec4, 0xffff0000);
+	nv_mthd(dev, 0xa097, 0x0ed4, 0xffff0000);
+	nv_mthd(dev, 0xa097, 0x0ee4, 0xffff0000);
+	nv_mthd(dev, 0xa097, 0x0ef4, 0xffff0000);
+	nv_mthd(dev, 0xa097, 0x0e08, 0xffff0000);
+	nv_mthd(dev, 0xa097, 0x0e18, 0xffff0000);
+	nv_mthd(dev, 0xa097, 0x0e28, 0xffff0000);
+	nv_mthd(dev, 0xa097, 0x0e38, 0xffff0000);
+	nv_mthd(dev, 0xa097, 0x0e48, 0xffff0000);
+	nv_mthd(dev, 0xa097, 0x0e58, 0xffff0000);
+	nv_mthd(dev, 0xa097, 0x0e68, 0xffff0000);
+	nv_mthd(dev, 0xa097, 0x0e78, 0xffff0000);
+	nv_mthd(dev, 0xa097, 0x0e88, 0xffff0000);
+	nv_mthd(dev, 0xa097, 0x0e98, 0xffff0000);
+	nv_mthd(dev, 0xa097, 0x0ea8, 0xffff0000);
+	nv_mthd(dev, 0xa097, 0x0eb8, 0xffff0000);
+	nv_mthd(dev, 0xa097, 0x0ec8, 0xffff0000);
+	nv_mthd(dev, 0xa097, 0x0ed8, 0xffff0000);
+	nv_mthd(dev, 0xa097, 0x0ee8, 0xffff0000);
+	nv_mthd(dev, 0xa097, 0x0ef8, 0xffff0000);
+	nv_mthd(dev, 0xa097, 0x0d40, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0d48, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0d50, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0d58, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0d44, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0d4c, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0d54, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0d5c, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1e00, 0x00000001);
+	nv_mthd(dev, 0xa097, 0x1e20, 0x00000001);
+	nv_mthd(dev, 0xa097, 0x1e40, 0x00000001);
+	nv_mthd(dev, 0xa097, 0x1e60, 0x00000001);
+	nv_mthd(dev, 0xa097, 0x1e80, 0x00000001);
+	nv_mthd(dev, 0xa097, 0x1ea0, 0x00000001);
+	nv_mthd(dev, 0xa097, 0x1ec0, 0x00000001);
+	nv_mthd(dev, 0xa097, 0x1ee0, 0x00000001);
+	nv_mthd(dev, 0xa097, 0x1e04, 0x00000001);
+	nv_mthd(dev, 0xa097, 0x1e24, 0x00000001);
+	nv_mthd(dev, 0xa097, 0x1e44, 0x00000001);
+	nv_mthd(dev, 0xa097, 0x1e64, 0x00000001);
+	nv_mthd(dev, 0xa097, 0x1e84, 0x00000001);
+	nv_mthd(dev, 0xa097, 0x1ea4, 0x00000001);
+	nv_mthd(dev, 0xa097, 0x1ec4, 0x00000001);
+	nv_mthd(dev, 0xa097, 0x1ee4, 0x00000001);
+	nv_mthd(dev, 0xa097, 0x1e08, 0x00000002);
+	nv_mthd(dev, 0xa097, 0x1e28, 0x00000002);
+	nv_mthd(dev, 0xa097, 0x1e48, 0x00000002);
+	nv_mthd(dev, 0xa097, 0x1e68, 0x00000002);
+	nv_mthd(dev, 0xa097, 0x1e88, 0x00000002);
+	nv_mthd(dev, 0xa097, 0x1ea8, 0x00000002);
+	nv_mthd(dev, 0xa097, 0x1ec8, 0x00000002);
+	nv_mthd(dev, 0xa097, 0x1ee8, 0x00000002);
+	nv_mthd(dev, 0xa097, 0x1e0c, 0x00000001);
+	nv_mthd(dev, 0xa097, 0x1e2c, 0x00000001);
+	nv_mthd(dev, 0xa097, 0x1e4c, 0x00000001);
+	nv_mthd(dev, 0xa097, 0x1e6c, 0x00000001);
+	nv_mthd(dev, 0xa097, 0x1e8c, 0x00000001);
+	nv_mthd(dev, 0xa097, 0x1eac, 0x00000001);
+	nv_mthd(dev, 0xa097, 0x1ecc, 0x00000001);
+	nv_mthd(dev, 0xa097, 0x1eec, 0x00000001);
+	nv_mthd(dev, 0xa097, 0x1e10, 0x00000001);
+	nv_mthd(dev, 0xa097, 0x1e30, 0x00000001);
+	nv_mthd(dev, 0xa097, 0x1e50, 0x00000001);
+	nv_mthd(dev, 0xa097, 0x1e70, 0x00000001);
+	nv_mthd(dev, 0xa097, 0x1e90, 0x00000001);
+	nv_mthd(dev, 0xa097, 0x1eb0, 0x00000001);
+	nv_mthd(dev, 0xa097, 0x1ed0, 0x00000001);
+	nv_mthd(dev, 0xa097, 0x1ef0, 0x00000001);
+	nv_mthd(dev, 0xa097, 0x1e14, 0x00000002);
+	nv_mthd(dev, 0xa097, 0x1e34, 0x00000002);
+	nv_mthd(dev, 0xa097, 0x1e54, 0x00000002);
+	nv_mthd(dev, 0xa097, 0x1e74, 0x00000002);
+	nv_mthd(dev, 0xa097, 0x1e94, 0x00000002);
+	nv_mthd(dev, 0xa097, 0x1eb4, 0x00000002);
+	nv_mthd(dev, 0xa097, 0x1ed4, 0x00000002);
+	nv_mthd(dev, 0xa097, 0x1ef4, 0x00000002);
+	nv_mthd(dev, 0xa097, 0x1e18, 0x00000001);
+	nv_mthd(dev, 0xa097, 0x1e38, 0x00000001);
+	nv_mthd(dev, 0xa097, 0x1e58, 0x00000001);
+	nv_mthd(dev, 0xa097, 0x1e78, 0x00000001);
+	nv_mthd(dev, 0xa097, 0x1e98, 0x00000001);
+	nv_mthd(dev, 0xa097, 0x1eb8, 0x00000001);
+	nv_mthd(dev, 0xa097, 0x1ed8, 0x00000001);
+	nv_mthd(dev, 0xa097, 0x1ef8, 0x00000001);
+	nv_mthd(dev, 0xa097, 0x3400, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x3404, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x3408, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x340c, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x3410, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x3414, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x3418, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x341c, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x3420, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x3424, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x3428, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x342c, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x3430, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x3434, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x3438, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x343c, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x3440, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x3444, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x3448, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x344c, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x3450, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x3454, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x3458, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x345c, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x3460, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x3464, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x3468, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x346c, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x3470, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x3474, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x3478, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x347c, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x3480, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x3484, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x3488, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x348c, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x3490, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x3494, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x3498, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x349c, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x34a0, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x34a4, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x34a8, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x34ac, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x34b0, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x34b4, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x34b8, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x34bc, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x34c0, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x34c4, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x34c8, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x34cc, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x34d0, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x34d4, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x34d8, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x34dc, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x34e0, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x34e4, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x34e8, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x34ec, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x34f0, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x34f4, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x34f8, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x34fc, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x3500, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x3504, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x3508, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x350c, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x3510, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x3514, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x3518, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x351c, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x3520, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x3524, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x3528, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x352c, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x3530, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x3534, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x3538, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x353c, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x3540, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x3544, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x3548, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x354c, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x3550, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x3554, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x3558, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x355c, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x3560, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x3564, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x3568, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x356c, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x3570, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x3574, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x3578, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x357c, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x3580, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x3584, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x3588, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x358c, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x3590, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x3594, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x3598, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x359c, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x35a0, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x35a4, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x35a8, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x35ac, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x35b0, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x35b4, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x35b8, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x35bc, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x35c0, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x35c4, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x35c8, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x35cc, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x35d0, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x35d4, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x35d8, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x35dc, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x35e0, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x35e4, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x35e8, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x35ec, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x35f0, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x35f4, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x35f8, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x35fc, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x030c, 0x00000001);
+	nv_mthd(dev, 0xa097, 0x1944, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1514, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0d68, 0x0000ffff);
+	nv_mthd(dev, 0xa097, 0x121c, 0x0fac6881);
+	nv_mthd(dev, 0xa097, 0x0fac, 0x00000001);
+	nv_mthd(dev, 0xa097, 0x1538, 0x00000001);
+	nv_mthd(dev, 0xa097, 0x0fe0, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0fe4, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0fe8, 0x00000014);
+	nv_mthd(dev, 0xa097, 0x0fec, 0x00000040);
+	nv_mthd(dev, 0xa097, 0x0ff0, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x179c, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1228, 0x00000400);
+	nv_mthd(dev, 0xa097, 0x122c, 0x00000300);
+	nv_mthd(dev, 0xa097, 0x1230, 0x00010001);
+	nv_mthd(dev, 0xa097, 0x07f8, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x15b4, 0x00000001);
+	nv_mthd(dev, 0xa097, 0x15cc, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1534, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0fb0, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x15d0, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x153c, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x16b4, 0x00000003);
+	nv_mthd(dev, 0xa097, 0x0fbc, 0x0000ffff);
+	nv_mthd(dev, 0xa097, 0x0fc0, 0x0000ffff);
+	nv_mthd(dev, 0xa097, 0x0fc4, 0x0000ffff);
+	nv_mthd(dev, 0xa097, 0x0fc8, 0x0000ffff);
+	nv_mthd(dev, 0xa097, 0x0df8, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0dfc, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1948, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1970, 0x00000001);
+	nv_mthd(dev, 0xa097, 0x161c, 0x000009f0);
+	nv_mthd(dev, 0xa097, 0x0dcc, 0x00000010);
+	nv_mthd(dev, 0xa097, 0x163c, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x15e4, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1160, 0x25e00040);
+	nv_mthd(dev, 0xa097, 0x1164, 0x25e00040);
+	nv_mthd(dev, 0xa097, 0x1168, 0x25e00040);
+	nv_mthd(dev, 0xa097, 0x116c, 0x25e00040);
+	nv_mthd(dev, 0xa097, 0x1170, 0x25e00040);
+	nv_mthd(dev, 0xa097, 0x1174, 0x25e00040);
+	nv_mthd(dev, 0xa097, 0x1178, 0x25e00040);
+	nv_mthd(dev, 0xa097, 0x117c, 0x25e00040);
+	nv_mthd(dev, 0xa097, 0x1180, 0x25e00040);
+	nv_mthd(dev, 0xa097, 0x1184, 0x25e00040);
+	nv_mthd(dev, 0xa097, 0x1188, 0x25e00040);
+	nv_mthd(dev, 0xa097, 0x118c, 0x25e00040);
+	nv_mthd(dev, 0xa097, 0x1190, 0x25e00040);
+	nv_mthd(dev, 0xa097, 0x1194, 0x25e00040);
+	nv_mthd(dev, 0xa097, 0x1198, 0x25e00040);
+	nv_mthd(dev, 0xa097, 0x119c, 0x25e00040);
+	nv_mthd(dev, 0xa097, 0x11a0, 0x25e00040);
+	nv_mthd(dev, 0xa097, 0x11a4, 0x25e00040);
+	nv_mthd(dev, 0xa097, 0x11a8, 0x25e00040);
+	nv_mthd(dev, 0xa097, 0x11ac, 0x25e00040);
+	nv_mthd(dev, 0xa097, 0x11b0, 0x25e00040);
+	nv_mthd(dev, 0xa097, 0x11b4, 0x25e00040);
+	nv_mthd(dev, 0xa097, 0x11b8, 0x25e00040);
+	nv_mthd(dev, 0xa097, 0x11bc, 0x25e00040);
+	nv_mthd(dev, 0xa097, 0x11c0, 0x25e00040);
+	nv_mthd(dev, 0xa097, 0x11c4, 0x25e00040);
+	nv_mthd(dev, 0xa097, 0x11c8, 0x25e00040);
+	nv_mthd(dev, 0xa097, 0x11cc, 0x25e00040);
+	nv_mthd(dev, 0xa097, 0x11d0, 0x25e00040);
+	nv_mthd(dev, 0xa097, 0x11d4, 0x25e00040);
+	nv_mthd(dev, 0xa097, 0x11d8, 0x25e00040);
+	nv_mthd(dev, 0xa097, 0x11dc, 0x25e00040);
+	nv_mthd(dev, 0xa097, 0x1880, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1884, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1888, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x188c, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1890, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1894, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1898, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x189c, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x18a0, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x18a4, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x18a8, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x18ac, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x18b0, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x18b4, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x18b8, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x18bc, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x18c0, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x18c4, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x18c8, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x18cc, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x18d0, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x18d4, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x18d8, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x18dc, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x18e0, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x18e4, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x18e8, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x18ec, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x18f0, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x18f4, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x18f8, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x18fc, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0f84, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0f88, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x17c8, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x17cc, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x17d0, 0x000000ff);
+	nv_mthd(dev, 0xa097, 0x17d4, 0xffffffff);
+	nv_mthd(dev, 0xa097, 0x17d8, 0x00000002);
+	nv_mthd(dev, 0xa097, 0x17dc, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x15f4, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x15f8, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1434, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1438, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0d74, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0dec, 0x00000001);
+	nv_mthd(dev, 0xa097, 0x13a4, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1318, 0x00000001);
+	nv_mthd(dev, 0xa097, 0x1644, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0748, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0de8, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1648, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x12a4, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1120, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1124, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1128, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x112c, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1118, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x164c, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1658, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1910, 0x00000290);
+	nv_mthd(dev, 0xa097, 0x1518, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x165c, 0x00000001);
+	nv_mthd(dev, 0xa097, 0x1520, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1604, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1570, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x13b0, 0x3f800000);
+	nv_mthd(dev, 0xa097, 0x13b4, 0x3f800000);
+	nv_mthd(dev, 0xa097, 0x020c, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1670, 0x30201000);
+	nv_mthd(dev, 0xa097, 0x1674, 0x70605040);
+	nv_mthd(dev, 0xa097, 0x1678, 0xb8a89888);
+	nv_mthd(dev, 0xa097, 0x167c, 0xf8e8d8c8);
+	nv_mthd(dev, 0xa097, 0x166c, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1680, 0x00ffff00);
+	nv_mthd(dev, 0xa097, 0x12d0, 0x00000003);
+	nv_mthd(dev, 0xa097, 0x12d4, 0x00000002);
+	nv_mthd(dev, 0xa097, 0x1684, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1688, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0dac, 0x00001b02);
+	nv_mthd(dev, 0xa097, 0x0db0, 0x00001b02);
+	nv_mthd(dev, 0xa097, 0x0db4, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x168c, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x15bc, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x156c, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x187c, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1110, 0x00000001);
+	nv_mthd(dev, 0xa097, 0x0dc0, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0dc4, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0dc8, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1234, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1690, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x12ac, 0x00000001);
+	nv_mthd(dev, 0xa097, 0x0790, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0794, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0798, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x079c, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x07a0, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x077c, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1000, 0x00000010);
+	nv_mthd(dev, 0xa097, 0x10fc, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1290, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0218, 0x00000010);
+	nv_mthd(dev, 0xa097, 0x12d8, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x12dc, 0x00000010);
+	nv_mthd(dev, 0xa097, 0x0d94, 0x00000001);
+	nv_mthd(dev, 0xa097, 0x155c, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1560, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1564, 0x00000fff);
+	nv_mthd(dev, 0xa097, 0x1574, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1578, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x157c, 0x000fffff);
+	nv_mthd(dev, 0xa097, 0x1354, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1610, 0x00000012);
+	nv_mthd(dev, 0xa097, 0x1608, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x160c, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x260c, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x07ac, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x162c, 0x00000003);
+	nv_mthd(dev, 0xa097, 0x0210, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0320, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0324, 0x3f800000);
+	nv_mthd(dev, 0xa097, 0x0328, 0x3f800000);
+	nv_mthd(dev, 0xa097, 0x032c, 0x3f800000);
+	nv_mthd(dev, 0xa097, 0x0330, 0x3f800000);
+	nv_mthd(dev, 0xa097, 0x0334, 0x3f800000);
+	nv_mthd(dev, 0xa097, 0x0338, 0x3f800000);
+	nv_mthd(dev, 0xa097, 0x0750, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0760, 0x39291909);
+	nv_mthd(dev, 0xa097, 0x0764, 0x79695949);
+	nv_mthd(dev, 0xa097, 0x0768, 0xb9a99989);
+	nv_mthd(dev, 0xa097, 0x076c, 0xf9e9d9c9);
+	nv_mthd(dev, 0xa097, 0x0770, 0x30201000);
+	nv_mthd(dev, 0xa097, 0x0774, 0x70605040);
+	nv_mthd(dev, 0xa097, 0x0778, 0x00009080);
+	nv_mthd(dev, 0xa097, 0x0780, 0x39291909);
+	nv_mthd(dev, 0xa097, 0x0784, 0x79695949);
+	nv_mthd(dev, 0xa097, 0x0788, 0xb9a99989);
+	nv_mthd(dev, 0xa097, 0x078c, 0xf9e9d9c9);
+	nv_mthd(dev, 0xa097, 0x07d0, 0x30201000);
+	nv_mthd(dev, 0xa097, 0x07d4, 0x70605040);
+	nv_mthd(dev, 0xa097, 0x07d8, 0x00009080);
+	nv_mthd(dev, 0xa097, 0x037c, 0x00000001);
+	nv_mthd(dev, 0xa097, 0x0740, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0744, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x2600, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1918, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x191c, 0x00000900);
+	nv_mthd(dev, 0xa097, 0x1920, 0x00000405);
+	nv_mthd(dev, 0xa097, 0x1308, 0x00000001);
+	nv_mthd(dev, 0xa097, 0x1924, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x13ac, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x192c, 0x00000001);
+	nv_mthd(dev, 0xa097, 0x193c, 0x00002c1c);
+	nv_mthd(dev, 0xa097, 0x0d7c, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0f8c, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x02c0, 0x00000001);
+	nv_mthd(dev, 0xa097, 0x1510, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1940, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0ff4, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0ff8, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x194c, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1950, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1968, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1590, 0x0000003f);
+	nv_mthd(dev, 0xa097, 0x07e8, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x07ec, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x07f0, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x07f4, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x196c, 0x00000011);
+	nv_mthd(dev, 0xa097, 0x02e4, 0x0000b001);
+	nv_mthd(dev, 0xa097, 0x036c, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0370, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x197c, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0fcc, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0fd0, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x02d8, 0x00000040);
+	nv_mthd(dev, 0xa097, 0x1980, 0x00000080);
+	nv_mthd(dev, 0xa097, 0x1504, 0x00000080);
+	nv_mthd(dev, 0xa097, 0x1984, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0300, 0x00000001);
+	nv_mthd(dev, 0xa097, 0x13a8, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x12ec, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1310, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1314, 0x00000001);
+	nv_mthd(dev, 0xa097, 0x1380, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1384, 0x00000001);
+	nv_mthd(dev, 0xa097, 0x1388, 0x00000001);
+	nv_mthd(dev, 0xa097, 0x138c, 0x00000001);
+	nv_mthd(dev, 0xa097, 0x1390, 0x00000001);
+	nv_mthd(dev, 0xa097, 0x1394, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x139c, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1398, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1594, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1598, 0x00000001);
+	nv_mthd(dev, 0xa097, 0x159c, 0x00000001);
+	nv_mthd(dev, 0xa097, 0x15a0, 0x00000001);
+	nv_mthd(dev, 0xa097, 0x15a4, 0x00000001);
+	nv_mthd(dev, 0xa097, 0x0f54, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0f58, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0f5c, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x19bc, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0f9c, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0fa0, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x12cc, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x12e8, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x130c, 0x00000001);
+	nv_mthd(dev, 0xa097, 0x1360, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1364, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1368, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x136c, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1370, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1374, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1378, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x137c, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x133c, 0x00000001);
+	nv_mthd(dev, 0xa097, 0x1340, 0x00000001);
+	nv_mthd(dev, 0xa097, 0x1344, 0x00000002);
+	nv_mthd(dev, 0xa097, 0x1348, 0x00000001);
+	nv_mthd(dev, 0xa097, 0x134c, 0x00000001);
+	nv_mthd(dev, 0xa097, 0x1350, 0x00000002);
+	nv_mthd(dev, 0xa097, 0x1358, 0x00000001);
+	nv_mthd(dev, 0xa097, 0x12e4, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x131c, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1320, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1324, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1328, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x19c0, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1140, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x19c4, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x19c8, 0x00001500);
+	nv_mthd(dev, 0xa097, 0x135c, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0f90, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x19e0, 0x00000001);
+	nv_mthd(dev, 0xa097, 0x19e4, 0x00000001);
+	nv_mthd(dev, 0xa097, 0x19e8, 0x00000001);
+	nv_mthd(dev, 0xa097, 0x19ec, 0x00000001);
+	nv_mthd(dev, 0xa097, 0x19f0, 0x00000001);
+	nv_mthd(dev, 0xa097, 0x19f4, 0x00000001);
+	nv_mthd(dev, 0xa097, 0x19f8, 0x00000001);
+	nv_mthd(dev, 0xa097, 0x19fc, 0x00000001);
+	nv_mthd(dev, 0xa097, 0x19cc, 0x00000001);
+	nv_mthd(dev, 0xa097, 0x15b8, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1a00, 0x00001111);
+	nv_mthd(dev, 0xa097, 0x1a04, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1a08, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1a0c, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1a10, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1a14, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1a18, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1a1c, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0d6c, 0xffff0000);
+	nv_mthd(dev, 0xa097, 0x0d70, 0xffff0000);
+	nv_mthd(dev, 0xa097, 0x10f8, 0x00001010);
+	nv_mthd(dev, 0xa097, 0x0d80, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0d84, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0d88, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0d8c, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0d90, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0da0, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x07a4, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x07a8, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1508, 0x80000000);
+	nv_mthd(dev, 0xa097, 0x150c, 0x40000000);
+	nv_mthd(dev, 0xa097, 0x1668, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0318, 0x00000008);
+	nv_mthd(dev, 0xa097, 0x031c, 0x00000008);
+	nv_mthd(dev, 0xa097, 0x0d9c, 0x00000001);
+	nv_mthd(dev, 0xa097, 0x0374, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0378, 0x00000020);
+	nv_mthd(dev, 0xa097, 0x07dc, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x074c, 0x00000055);
+	nv_mthd(dev, 0xa097, 0x1420, 0x00000003);
+	nv_mthd(dev, 0xa097, 0x17bc, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x17c0, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x17c4, 0x00000001);
+	nv_mthd(dev, 0xa097, 0x1008, 0x00000008);
+	nv_mthd(dev, 0xa097, 0x100c, 0x00000040);
+	nv_mthd(dev, 0xa097, 0x1010, 0x0000012c);
+	nv_mthd(dev, 0xa097, 0x0d60, 0x00000040);
+	nv_mthd(dev, 0xa097, 0x075c, 0x00000003);
+	nv_mthd(dev, 0xa097, 0x1018, 0x00000020);
+	nv_mthd(dev, 0xa097, 0x101c, 0x00000001);
+	nv_mthd(dev, 0xa097, 0x1020, 0x00000020);
+	nv_mthd(dev, 0xa097, 0x1024, 0x00000001);
+	nv_mthd(dev, 0xa097, 0x1444, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x1448, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x144c, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0360, 0x20164010);
+	nv_mthd(dev, 0xa097, 0x0364, 0x00000020);
+	nv_mthd(dev, 0xa097, 0x0368, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0de4, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0204, 0x00000006);
+	nv_mthd(dev, 0xa097, 0x0208, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x02cc, 0x003fffff);
+	nv_mthd(dev, 0xa097, 0x02d0, 0x003fffff);
+	nv_mthd(dev, 0xa097, 0x1220, 0x00000005);
+	nv_mthd(dev, 0xa097, 0x0fdc, 0x00000000);
+	nv_mthd(dev, 0xa097, 0x0f98, 0x00400008);
+	nv_mthd(dev, 0xa097, 0x1284, 0x08000080);
+	nv_mthd(dev, 0xa097, 0x1450, 0x00400008);
+	nv_mthd(dev, 0xa097, 0x1454, 0x08000080);
+	nv_mthd(dev, 0xa097, 0x0214, 0x00000000);
+}
+
+static void
+nve0_grctx_generate_902d(struct drm_device *dev)
+{
+	nv_mthd(dev, 0x902d, 0x0200, 0x000000cf);
+	nv_mthd(dev, 0x902d, 0x0204, 0x00000001);
+	nv_mthd(dev, 0x902d, 0x0208, 0x00000020);
+	nv_mthd(dev, 0x902d, 0x020c, 0x00000001);
+	nv_mthd(dev, 0x902d, 0x0210, 0x00000000);
+	nv_mthd(dev, 0x902d, 0x0214, 0x00000080);
+	nv_mthd(dev, 0x902d, 0x0218, 0x00000100);
+	nv_mthd(dev, 0x902d, 0x021c, 0x00000100);
+	nv_mthd(dev, 0x902d, 0x0220, 0x00000000);
+	nv_mthd(dev, 0x902d, 0x0224, 0x00000000);
+	nv_mthd(dev, 0x902d, 0x0230, 0x000000cf);
+	nv_mthd(dev, 0x902d, 0x0234, 0x00000001);
+	nv_mthd(dev, 0x902d, 0x0238, 0x00000020);
+	nv_mthd(dev, 0x902d, 0x023c, 0x00000001);
+	nv_mthd(dev, 0x902d, 0x0244, 0x00000080);
+	nv_mthd(dev, 0x902d, 0x0248, 0x00000100);
+	nv_mthd(dev, 0x902d, 0x024c, 0x00000100);
+	nv_mthd(dev, 0x902d, 0x3410, 0x00000000);
+}
+
+static void
+nve0_graph_generate_unk40xx(struct drm_device *dev)
+{
+	nv_wr32(dev, 0x404010, 0x0);
+	nv_wr32(dev, 0x404014, 0x0);
+	nv_wr32(dev, 0x404018, 0x0);
+	nv_wr32(dev, 0x40401c, 0x0);
+	nv_wr32(dev, 0x404020, 0x0);
+	nv_wr32(dev, 0x404024, 0xe000);
+	nv_wr32(dev, 0x404028, 0x0);
+	nv_wr32(dev, 0x4040a8, 0x0);
+	nv_wr32(dev, 0x4040ac, 0x0);
+	nv_wr32(dev, 0x4040b0, 0x0);
+	nv_wr32(dev, 0x4040b4, 0x0);
+	nv_wr32(dev, 0x4040b8, 0x0);
+	nv_wr32(dev, 0x4040bc, 0x0);
+	nv_wr32(dev, 0x4040c0, 0x0);
+	nv_wr32(dev, 0x4040c4, 0x0);
+	nv_wr32(dev, 0x4040c8, 0xf800008f);
+	nv_wr32(dev, 0x4040d0, 0x0);
+	nv_wr32(dev, 0x4040d4, 0x0);
+	nv_wr32(dev, 0x4040d8, 0x0);
+	nv_wr32(dev, 0x4040dc, 0x0);
+	nv_wr32(dev, 0x4040e0, 0x0);
+	nv_wr32(dev, 0x4040e4, 0x0);
+	nv_wr32(dev, 0x4040e8, 0x1000);
+	nv_wr32(dev, 0x4040f8, 0x0);
+	nv_wr32(dev, 0x404130, 0x0);
+	nv_wr32(dev, 0x404134, 0x0);
+	nv_wr32(dev, 0x404138, 0x20000040);
+	nv_wr32(dev, 0x404150, 0x2e);
+	nv_wr32(dev, 0x404154, 0x400);
+	nv_wr32(dev, 0x404158, 0x200);
+	nv_wr32(dev, 0x404164, 0x55);
+	nv_wr32(dev, 0x4041a0, 0x0);
+	nv_wr32(dev, 0x4041a4, 0x0);
+	nv_wr32(dev, 0x4041a8, 0x0);
+	nv_wr32(dev, 0x4041ac, 0x0);
+	nv_wr32(dev, 0x404200, 0x0);
+	nv_wr32(dev, 0x404204, 0x0);
+	nv_wr32(dev, 0x404208, 0x0);
+	nv_wr32(dev, 0x40420c, 0x0);
+}
+
+static void
+nve0_graph_generate_unk44xx(struct drm_device *dev)
+{
+	nv_wr32(dev, 0x404404, 0x0);
+	nv_wr32(dev, 0x404408, 0x0);
+	nv_wr32(dev, 0x40440c, 0x0);
+	nv_wr32(dev, 0x404410, 0x0);
+	nv_wr32(dev, 0x404414, 0x0);
+	nv_wr32(dev, 0x404418, 0x0);
+	nv_wr32(dev, 0x40441c, 0x0);
+	nv_wr32(dev, 0x404420, 0x0);
+	nv_wr32(dev, 0x404424, 0x0);
+	nv_wr32(dev, 0x404428, 0x0);
+	nv_wr32(dev, 0x40442c, 0x0);
+	nv_wr32(dev, 0x404430, 0x0);
+	nv_wr32(dev, 0x404434, 0x0);
+	nv_wr32(dev, 0x404438, 0x0);
+	nv_wr32(dev, 0x404460, 0x0);
+	nv_wr32(dev, 0x404464, 0x0);
+	nv_wr32(dev, 0x404468, 0xffffff);
+	nv_wr32(dev, 0x40446c, 0x0);
+	nv_wr32(dev, 0x404480, 0x1);
+	nv_wr32(dev, 0x404498, 0x1);
+}
+
+static void
+nve0_graph_generate_unk46xx(struct drm_device *dev)
+{
+	nv_wr32(dev, 0x404604, 0x14);
+	nv_wr32(dev, 0x404608, 0x0);
+	nv_wr32(dev, 0x40460c, 0x3fff);
+	nv_wr32(dev, 0x404610, 0x100);
+	nv_wr32(dev, 0x404618, 0x0);
+	nv_wr32(dev, 0x40461c, 0x0);
+	nv_wr32(dev, 0x404620, 0x0);
+	nv_wr32(dev, 0x404624, 0x0);
+	nv_wr32(dev, 0x40462c, 0x0);
+	nv_wr32(dev, 0x404630, 0x0);
+	nv_wr32(dev, 0x404640, 0x0);
+	nv_wr32(dev, 0x404654, 0x0);
+	nv_wr32(dev, 0x404660, 0x0);
+	nv_wr32(dev, 0x404678, 0x0);
+	nv_wr32(dev, 0x40467c, 0x2);
+	nv_wr32(dev, 0x404680, 0x0);
+	nv_wr32(dev, 0x404684, 0x0);
+	nv_wr32(dev, 0x404688, 0x0);
+	nv_wr32(dev, 0x40468c, 0x0);
+	nv_wr32(dev, 0x404690, 0x0);
+	nv_wr32(dev, 0x404694, 0x0);
+	nv_wr32(dev, 0x404698, 0x0);
+	nv_wr32(dev, 0x40469c, 0x0);
+	nv_wr32(dev, 0x4046a0, 0x7f0080);
+	nv_wr32(dev, 0x4046a4, 0x0);
+	nv_wr32(dev, 0x4046a8, 0x0);
+	nv_wr32(dev, 0x4046ac, 0x0);
+	nv_wr32(dev, 0x4046b0, 0x0);
+	nv_wr32(dev, 0x4046b4, 0x0);
+	nv_wr32(dev, 0x4046b8, 0x0);
+	nv_wr32(dev, 0x4046bc, 0x0);
+	nv_wr32(dev, 0x4046c0, 0x0);
+	nv_wr32(dev, 0x4046c8, 0x0);
+	nv_wr32(dev, 0x4046cc, 0x0);
+	nv_wr32(dev, 0x4046d0, 0x0);
+}
+
+static void
+nve0_graph_generate_unk47xx(struct drm_device *dev)
+{
+	nv_wr32(dev, 0x404700, 0x0);
+	nv_wr32(dev, 0x404704, 0x0);
+	nv_wr32(dev, 0x404708, 0x0);
+	nv_wr32(dev, 0x404718, 0x0);
+	nv_wr32(dev, 0x40471c, 0x0);
+	nv_wr32(dev, 0x404720, 0x0);
+	nv_wr32(dev, 0x404724, 0x0);
+	nv_wr32(dev, 0x404728, 0x0);
+	nv_wr32(dev, 0x40472c, 0x0);
+	nv_wr32(dev, 0x404730, 0x0);
+	nv_wr32(dev, 0x404734, 0x100);
+	nv_wr32(dev, 0x404738, 0x0);
+	nv_wr32(dev, 0x40473c, 0x0);
+	nv_wr32(dev, 0x404744, 0x0);
+	nv_wr32(dev, 0x404748, 0x0);
+	nv_wr32(dev, 0x404754, 0x0);
+}
+
+static void
+nve0_graph_generate_unk58xx(struct drm_device *dev)
+{
+	nv_wr32(dev, 0x405800, 0xf8000bf);
+	nv_wr32(dev, 0x405830, 0x2180648);
+	nv_wr32(dev, 0x405834, 0x8000000);
+	nv_wr32(dev, 0x405838, 0x0);
+	nv_wr32(dev, 0x405854, 0x0);
+	nv_wr32(dev, 0x405870, 0x1);
+	nv_wr32(dev, 0x405874, 0x1);
+	nv_wr32(dev, 0x405878, 0x1);
+	nv_wr32(dev, 0x40587c, 0x1);
+	nv_wr32(dev, 0x405a00, 0x0);
+	nv_wr32(dev, 0x405a04, 0x0);
+	nv_wr32(dev, 0x405a18, 0x0);
+	nv_wr32(dev, 0x405b00, 0x0);
+	nv_wr32(dev, 0x405b10, 0x1000);
+}
+
+static void
+nve0_graph_generate_unk60xx(struct drm_device *dev)
+{
+	nv_wr32(dev, 0x406020, 0x4103c1);
+	nv_wr32(dev, 0x406028, 0x1);
+	nv_wr32(dev, 0x40602c, 0x1);
+	nv_wr32(dev, 0x406030, 0x1);
+	nv_wr32(dev, 0x406034, 0x1);
+}
+
+static void
+nve0_graph_generate_unk64xx(struct drm_device *dev)
+{
+	nv_wr32(dev, 0x4064a8, 0x0);
+	nv_wr32(dev, 0x4064ac, 0x3fff);
+	nv_wr32(dev, 0x4064b4, 0x0);
+	nv_wr32(dev, 0x4064b8, 0x0);
+	nv_wr32(dev, 0x4064c0, 0x801a00f0);
+	nv_wr32(dev, 0x4064c4, 0x192ffff);
+	nv_wr32(dev, 0x4064c8, 0x1800600);
+	nv_wr32(dev, 0x4064cc, 0x0);
+	nv_wr32(dev, 0x4064d0, 0x0);
+	nv_wr32(dev, 0x4064d4, 0x0);
+	nv_wr32(dev, 0x4064d8, 0x0);
+	nv_wr32(dev, 0x4064dc, 0x0);
+	nv_wr32(dev, 0x4064e0, 0x0);
+	nv_wr32(dev, 0x4064e4, 0x0);
+	nv_wr32(dev, 0x4064e8, 0x0);
+	nv_wr32(dev, 0x4064ec, 0x0);
+	nv_wr32(dev, 0x4064fc, 0x22a);
+}
+
+static void
+nve0_graph_generate_unk70xx(struct drm_device *dev)
+{
+	nv_wr32(dev, 0x407040, 0x0);
+}
+
+static void
+nve0_graph_generate_unk78xx(struct drm_device *dev)
+{
+	nv_wr32(dev, 0x407804, 0x23);
+	nv_wr32(dev, 0x40780c, 0xa418820);
+	nv_wr32(dev, 0x407810, 0x62080e6);
+	nv_wr32(dev, 0x407814, 0x20398a4);
+	nv_wr32(dev, 0x407818, 0xe629062);
+	nv_wr32(dev, 0x40781c, 0xa418820);
+	nv_wr32(dev, 0x407820, 0xe6);
+	nv_wr32(dev, 0x4078bc, 0x103);
+}
+
+static void
+nve0_graph_generate_unk80xx(struct drm_device *dev)
+{
+	nv_wr32(dev, 0x408000, 0x0);
+	nv_wr32(dev, 0x408004, 0x0);
+	nv_wr32(dev, 0x408008, 0x30);
+	nv_wr32(dev, 0x40800c, 0x0);
+	nv_wr32(dev, 0x408010, 0x0);
+	nv_wr32(dev, 0x408014, 0x69);
+	nv_wr32(dev, 0x408018, 0xe100e100);
+	nv_wr32(dev, 0x408064, 0x0);
+}
+
+static void
+nve0_graph_generate_unk88xx(struct drm_device *dev)
+{
+	nv_wr32(dev, 0x408800, 0x2802a3c);
+	nv_wr32(dev, 0x408804, 0x40);
+	nv_wr32(dev, 0x408808, 0x1043e005);
+	nv_wr32(dev, 0x408840, 0xb);
+	nv_wr32(dev, 0x408900, 0x3080b801);
+	nv_wr32(dev, 0x408904, 0x62000001);
+	nv_wr32(dev, 0x408908, 0xc8102f);
+	nv_wr32(dev, 0x408980, 0x11d);
+}
+
+static void
+nve0_graph_generate_gpc(struct drm_device *dev)
+{
+	nv_wr32(dev, 0x418380, 0x16);
+	nv_wr32(dev, 0x418400, 0x38004e00);
+	nv_wr32(dev, 0x418404, 0x71e0ffff);
+	nv_wr32(dev, 0x41840c, 0x1008);
+	nv_wr32(dev, 0x418410, 0xfff0fff);
+	nv_wr32(dev, 0x418414, 0x2200fff);
+	nv_wr32(dev, 0x418450, 0x0);
+	nv_wr32(dev, 0x418454, 0x0);
+	nv_wr32(dev, 0x418458, 0x0);
+	nv_wr32(dev, 0x41845c, 0x0);
+	nv_wr32(dev, 0x418460, 0x0);
+	nv_wr32(dev, 0x418464, 0x0);
+	nv_wr32(dev, 0x418468, 0x1);
+	nv_wr32(dev, 0x41846c, 0x0);
+	nv_wr32(dev, 0x418470, 0x0);
+	nv_wr32(dev, 0x418600, 0x1f);
+	nv_wr32(dev, 0x418684, 0xf);
+	nv_wr32(dev, 0x418700, 0x2);
+	nv_wr32(dev, 0x418704, 0x80);
+	nv_wr32(dev, 0x418708, 0x0);
+	nv_wr32(dev, 0x41870c, 0x0);
+	nv_wr32(dev, 0x418710, 0x0);
+	nv_wr32(dev, 0x418800, 0x7006860a);
+	nv_wr32(dev, 0x418808, 0x0);
+	nv_wr32(dev, 0x41880c, 0x0);
+	nv_wr32(dev, 0x418810, 0x0);
+	nv_wr32(dev, 0x418828, 0x44);
+	nv_wr32(dev, 0x418830, 0x10000001);
+	nv_wr32(dev, 0x4188d8, 0x8);
+	nv_wr32(dev, 0x4188e0, 0x1000000);
+	nv_wr32(dev, 0x4188e8, 0x0);
+	nv_wr32(dev, 0x4188ec, 0x0);
+	nv_wr32(dev, 0x4188f0, 0x0);
+	nv_wr32(dev, 0x4188f4, 0x0);
+	nv_wr32(dev, 0x4188f8, 0x0);
+	nv_wr32(dev, 0x4188fc, 0x20100018);
+	nv_wr32(dev, 0x41891c, 0xff00ff);
+	nv_wr32(dev, 0x418924, 0x0);
+	nv_wr32(dev, 0x418928, 0xffff00);
+	nv_wr32(dev, 0x41892c, 0xff00);
+	nv_wr32(dev, 0x418a00, 0x0);
+	nv_wr32(dev, 0x418a04, 0x0);
+	nv_wr32(dev, 0x418a08, 0x0);
+	nv_wr32(dev, 0x418a0c, 0x10000);
+	nv_wr32(dev, 0x418a10, 0x0);
+	nv_wr32(dev, 0x418a14, 0x0);
+	nv_wr32(dev, 0x418a18, 0x0);
+	nv_wr32(dev, 0x418a20, 0x0);
+	nv_wr32(dev, 0x418a24, 0x0);
+	nv_wr32(dev, 0x418a28, 0x0);
+	nv_wr32(dev, 0x418a2c, 0x10000);
+	nv_wr32(dev, 0x418a30, 0x0);
+	nv_wr32(dev, 0x418a34, 0x0);
+	nv_wr32(dev, 0x418a38, 0x0);
+	nv_wr32(dev, 0x418a40, 0x0);
+	nv_wr32(dev, 0x418a44, 0x0);
+	nv_wr32(dev, 0x418a48, 0x0);
+	nv_wr32(dev, 0x418a4c, 0x10000);
+	nv_wr32(dev, 0x418a50, 0x0);
+	nv_wr32(dev, 0x418a54, 0x0);
+	nv_wr32(dev, 0x418a58, 0x0);
+	nv_wr32(dev, 0x418a60, 0x0);
+	nv_wr32(dev, 0x418a64, 0x0);
+	nv_wr32(dev, 0x418a68, 0x0);
+	nv_wr32(dev, 0x418a6c, 0x10000);
+	nv_wr32(dev, 0x418a70, 0x0);
+	nv_wr32(dev, 0x418a74, 0x0);
+	nv_wr32(dev, 0x418a78, 0x0);
+	nv_wr32(dev, 0x418a80, 0x0);
+	nv_wr32(dev, 0x418a84, 0x0);
+	nv_wr32(dev, 0x418a88, 0x0);
+	nv_wr32(dev, 0x418a8c, 0x10000);
+	nv_wr32(dev, 0x418a90, 0x0);
+	nv_wr32(dev, 0x418a94, 0x0);
+	nv_wr32(dev, 0x418a98, 0x0);
+	nv_wr32(dev, 0x418aa0, 0x0);
+	nv_wr32(dev, 0x418aa4, 0x0);
+	nv_wr32(dev, 0x418aa8, 0x0);
+	nv_wr32(dev, 0x418aac, 0x10000);
+	nv_wr32(dev, 0x418ab0, 0x0);
+	nv_wr32(dev, 0x418ab4, 0x0);
+	nv_wr32(dev, 0x418ab8, 0x0);
+	nv_wr32(dev, 0x418ac0, 0x0);
+	nv_wr32(dev, 0x418ac4, 0x0);
+	nv_wr32(dev, 0x418ac8, 0x0);
+	nv_wr32(dev, 0x418acc, 0x10000);
+	nv_wr32(dev, 0x418ad0, 0x0);
+	nv_wr32(dev, 0x418ad4, 0x0);
+	nv_wr32(dev, 0x418ad8, 0x0);
+	nv_wr32(dev, 0x418ae0, 0x0);
+	nv_wr32(dev, 0x418ae4, 0x0);
+	nv_wr32(dev, 0x418ae8, 0x0);
+	nv_wr32(dev, 0x418aec, 0x10000);
+	nv_wr32(dev, 0x418af0, 0x0);
+	nv_wr32(dev, 0x418af4, 0x0);
+	nv_wr32(dev, 0x418af8, 0x0);
+	nv_wr32(dev, 0x418b00, 0x6);
+	nv_wr32(dev, 0x418b08, 0xa418820);
+	nv_wr32(dev, 0x418b0c, 0x62080e6);
+	nv_wr32(dev, 0x418b10, 0x20398a4);
+	nv_wr32(dev, 0x418b14, 0xe629062);
+	nv_wr32(dev, 0x418b18, 0xa418820);
+	nv_wr32(dev, 0x418b1c, 0xe6);
+	nv_wr32(dev, 0x418bb8, 0x103);
+	nv_wr32(dev, 0x418c08, 0x1);
+	nv_wr32(dev, 0x418c10, 0x0);
+	nv_wr32(dev, 0x418c14, 0x0);
+	nv_wr32(dev, 0x418c18, 0x0);
+	nv_wr32(dev, 0x418c1c, 0x0);
+	nv_wr32(dev, 0x418c20, 0x0);
+	nv_wr32(dev, 0x418c24, 0x0);
+	nv_wr32(dev, 0x418c28, 0x0);
+	nv_wr32(dev, 0x418c2c, 0x0);
+	nv_wr32(dev, 0x418c40, 0xffffffff);
+	nv_wr32(dev, 0x418c6c, 0x1);
+	nv_wr32(dev, 0x418c80, 0x20200004);
+	nv_wr32(dev, 0x418c8c, 0x1);
+	nv_wr32(dev, 0x419000, 0x780);
+	nv_wr32(dev, 0x419004, 0x0);
+	nv_wr32(dev, 0x419008, 0x0);
+	nv_wr32(dev, 0x419014, 0x4);
+}
+
+static void
+nve0_graph_generate_tpc(struct drm_device *dev)
+{
+	nv_wr32(dev, 0x419848, 0x0);
+	nv_wr32(dev, 0x419864, 0x129);
+	nv_wr32(dev, 0x419888, 0x0);
+	nv_wr32(dev, 0x419a00, 0xf0);
+	nv_wr32(dev, 0x419a04, 0x1);
+	nv_wr32(dev, 0x419a08, 0x21);
+	nv_wr32(dev, 0x419a0c, 0x20000);
+	nv_wr32(dev, 0x419a10, 0x0);
+	nv_wr32(dev, 0x419a14, 0x200);
+	nv_wr32(dev, 0x419a1c, 0xc000);
+	nv_wr32(dev, 0x419a20, 0x800);
+	nv_wr32(dev, 0x419a30, 0x1);
+	nv_wr32(dev, 0x419ac4, 0x37f440);
+	nv_wr32(dev, 0x419c00, 0xa);
+	nv_wr32(dev, 0x419c04, 0x80000006);
+	nv_wr32(dev, 0x419c08, 0x2);
+	nv_wr32(dev, 0x419c20, 0x0);
+	nv_wr32(dev, 0x419c24, 0x84210);
+	nv_wr32(dev, 0x419c28, 0x3efbefbe);
+	nv_wr32(dev, 0x419ce8, 0x0);
+	nv_wr32(dev, 0x419cf4, 0x3203);
+	nv_wr32(dev, 0x419e04, 0x0);
+	nv_wr32(dev, 0x419e08, 0x0);
+	nv_wr32(dev, 0x419e0c, 0x0);
+	nv_wr32(dev, 0x419e10, 0x402);
+	nv_wr32(dev, 0x419e44, 0x13eff2);
+	nv_wr32(dev, 0x419e48, 0x0);
+	nv_wr32(dev, 0x419e4c, 0x7f);
+	nv_wr32(dev, 0x419e50, 0x0);
+	nv_wr32(dev, 0x419e54, 0x0);
+	nv_wr32(dev, 0x419e58, 0x0);
+	nv_wr32(dev, 0x419e5c, 0x0);
+	nv_wr32(dev, 0x419e60, 0x0);
+	nv_wr32(dev, 0x419e64, 0x0);
+	nv_wr32(dev, 0x419e68, 0x0);
+	nv_wr32(dev, 0x419e6c, 0x0);
+	nv_wr32(dev, 0x419e70, 0x0);
+	nv_wr32(dev, 0x419e74, 0x0);
+	nv_wr32(dev, 0x419e78, 0x0);
+	nv_wr32(dev, 0x419e7c, 0x0);
+	nv_wr32(dev, 0x419e80, 0x0);
+	nv_wr32(dev, 0x419e84, 0x0);
+	nv_wr32(dev, 0x419e88, 0x0);
+	nv_wr32(dev, 0x419e8c, 0x0);
+	nv_wr32(dev, 0x419e90, 0x0);
+	nv_wr32(dev, 0x419e94, 0x0);
+	nv_wr32(dev, 0x419e98, 0x0);
+	nv_wr32(dev, 0x419eac, 0x1fcf);
+	nv_wr32(dev, 0x419eb0, 0xd3f);
+	nv_wr32(dev, 0x419ec8, 0x1304f);
+	nv_wr32(dev, 0x419f30, 0x0);
+	nv_wr32(dev, 0x419f34, 0x0);
+	nv_wr32(dev, 0x419f38, 0x0);
+	nv_wr32(dev, 0x419f3c, 0x0);
+	nv_wr32(dev, 0x419f40, 0x0);
+	nv_wr32(dev, 0x419f44, 0x0);
+	nv_wr32(dev, 0x419f48, 0x0);
+	nv_wr32(dev, 0x419f4c, 0x0);
+	nv_wr32(dev, 0x419f58, 0x0);
+	nv_wr32(dev, 0x419f78, 0xb);
+}
+
+static void
+nve0_graph_generate_tpcunk(struct drm_device *dev)
+{
+	nv_wr32(dev, 0x41be24, 0x6);
+	nv_wr32(dev, 0x41bec0, 0x12180000);
+	nv_wr32(dev, 0x41bec4, 0x37f7f);
+	nv_wr32(dev, 0x41bee4, 0x6480430);
+	nv_wr32(dev, 0x41bf00, 0xa418820);
+	nv_wr32(dev, 0x41bf04, 0x62080e6);
+	nv_wr32(dev, 0x41bf08, 0x20398a4);
+	nv_wr32(dev, 0x41bf0c, 0xe629062);
+	nv_wr32(dev, 0x41bf10, 0xa418820);
+	nv_wr32(dev, 0x41bf14, 0xe6);
+	nv_wr32(dev, 0x41bfd0, 0x900103);
+	nv_wr32(dev, 0x41bfe0, 0x400001);
+	nv_wr32(dev, 0x41bfe4, 0x0);
+}
+
+int
+nve0_grctx_generate(struct nouveau_channel *chan)
+{
+	struct nve0_graph_priv *priv = nv_engine(chan->dev, NVOBJ_ENGINE_GR);
+	struct nve0_graph_chan *grch = chan->engctx[NVOBJ_ENGINE_GR];
+	struct drm_device *dev = chan->dev;
+	u32 data[6] = {}, data2[2] = {}, tmp;
+	u32 tpc_set = 0, tpc_mask = 0;
+	u8 tpcnr[GPC_MAX], a, b;
+	u8 shift, ntpcv;
+	int i, gpc, tpc, id;
+
+	nv_mask(dev, 0x000260, 0x00000001, 0x00000000);
+	nv_wr32(dev, 0x400204, 0x00000000);
+	nv_wr32(dev, 0x400208, 0x00000000);
+
+	nve0_graph_generate_unk40xx(dev);
+	nve0_graph_generate_unk44xx(dev);
+	nve0_graph_generate_unk46xx(dev);
+	nve0_graph_generate_unk47xx(dev);
+	nve0_graph_generate_unk58xx(dev);
+	nve0_graph_generate_unk60xx(dev);
+	nve0_graph_generate_unk64xx(dev);
+	nve0_graph_generate_unk70xx(dev);
+	nve0_graph_generate_unk78xx(dev);
+	nve0_graph_generate_unk80xx(dev);
+	nve0_graph_generate_unk88xx(dev);
+	nve0_graph_generate_gpc(dev);
+	nve0_graph_generate_tpc(dev);
+	nve0_graph_generate_tpcunk(dev);
+
+	nv_wr32(dev, 0x404154, 0x0);
+
+	for (i = 0; i < grch->mmio_nr * 8; i += 8) {
+		u32 reg = nv_ro32(grch->mmio, i + 0);
+		u32 val = nv_ro32(grch->mmio, i + 4);
+		nv_wr32(dev, reg, val);
+	}
+
+	nv_wr32(dev, 0x418c6c, 0x1);
+	nv_wr32(dev, 0x41980c, 0x10);
+	nv_wr32(dev, 0x41be08, 0x4);
+	nv_wr32(dev, 0x4064c0, 0x801a00f0);
+	nv_wr32(dev, 0x405800, 0xf8000bf);
+	nv_wr32(dev, 0x419c00, 0xa);
+
+	for (tpc = 0, id = 0; tpc < 4; tpc++) {
+		for (gpc = 0; gpc < priv->gpc_nr; gpc++) {
+			if (tpc < priv->tpc_nr[gpc]) {
+				nv_wr32(dev, TPC_UNIT(gpc, tpc, 0x0698), id);
+				nv_wr32(dev, TPC_UNIT(gpc, tpc, 0x04e8), id);
+				nv_wr32(dev, GPC_UNIT(gpc, 0x0c10 + tpc * 4), id);
+				nv_wr32(dev, TPC_UNIT(gpc, tpc, 0x0088), id++);
+			}
+
+			nv_wr32(dev, GPC_UNIT(gpc, 0x0c08), priv->tpc_nr[gpc]);
+			nv_wr32(dev, GPC_UNIT(gpc, 0x0c8c), priv->tpc_nr[gpc]);
+		}
+	}
+
+	tmp = 0;
+	for (i = 0; i < priv->gpc_nr; i++)
+		tmp |= priv->tpc_nr[i] << (i * 4);
+	nv_wr32(dev, 0x406028, tmp);
+	nv_wr32(dev, 0x405870, tmp);
+
+	nv_wr32(dev, 0x40602c, 0x0);
+	nv_wr32(dev, 0x405874, 0x0);
+	nv_wr32(dev, 0x406030, 0x0);
+	nv_wr32(dev, 0x405878, 0x0);
+	nv_wr32(dev, 0x406034, 0x0);
+	nv_wr32(dev, 0x40587c, 0x0);
+
+	/* calculate first set of magics */
+	memcpy(tpcnr, priv->tpc_nr, sizeof(priv->tpc_nr));
+
+	gpc = -1;
+	for (tpc = 0; tpc < priv->tpc_total; tpc++) {
+		do {
+			gpc = (gpc + 1) % priv->gpc_nr;
+		} while (!tpcnr[gpc]);
+		tpcnr[gpc]--;
+
+		data[tpc / 6] |= gpc << ((tpc % 6) * 5);
+	}
+
+	for (; tpc < 32; tpc++)
+		data[tpc / 6] |= 7 << ((tpc % 6) * 5);
+
+	/* and the second... */
+	shift = 0;
+	ntpcv = priv->tpc_total;
+	while (!(ntpcv & (1 << 4))) {
+		ntpcv <<= 1;
+		shift++;
+	}
+
+	data2[0]  = ntpcv << 16;
+	data2[0] |= shift << 21;
+	data2[0] |= (((1 << (0 + 5)) % ntpcv) << 24);
+	data2[0] |= priv->tpc_total << 8;
+	data2[0] |= priv->magic_not_rop_nr;
+	for (i = 1; i < 7; i++)
+		data2[1] |= ((1 << (i + 5)) % ntpcv) << ((i - 1) * 5);
+
+	/* and write it all the various parts of PGRAPH */
+	nv_wr32(dev, 0x418bb8, (priv->tpc_total << 8) | priv->magic_not_rop_nr);
+	for (i = 0; i < 6; i++)
+		nv_wr32(dev, 0x418b08 + (i * 4), data[i]);
+
+	nv_wr32(dev, 0x41bfd0, data2[0]);
+	nv_wr32(dev, 0x41bfe4, data2[1]);
+	for (i = 0; i < 6; i++)
+		nv_wr32(dev, 0x41bf00 + (i * 4), data[i]);
+
+	nv_wr32(dev, 0x4078bc, (priv->tpc_total << 8) | priv->magic_not_rop_nr);
+	for (i = 0; i < 6; i++)
+		nv_wr32(dev, 0x40780c + (i * 4), data[i]);
+
+
+	memcpy(tpcnr, priv->tpc_nr, sizeof(priv->tpc_nr));
+	for (gpc = 0; gpc < priv->gpc_nr; gpc++)
+		tpc_mask |= ((1 << priv->tpc_nr[gpc]) - 1) << (gpc * 8);
+
+	for (i = 0, gpc = -1, b = -1; i < 32; i++) {
+		a = (i * (priv->tpc_total - 1)) / 32;
+		if (a != b) {
+			b = a;
+			do {
+				gpc = (gpc + 1) % priv->gpc_nr;
+			} while (!tpcnr[gpc]);
+			tpc = priv->tpc_nr[gpc] - tpcnr[gpc]--;
+
+			tpc_set |= 1 << ((gpc * 8) + tpc);
+		}
+
+		nv_wr32(dev, 0x406800 + (i * 0x20), tpc_set);
+		nv_wr32(dev, 0x406c00 + (i * 0x20), tpc_set ^ tpc_mask);
+	}
+
+	for (i = 0; i < 8; i++)
+		nv_wr32(dev, 0x4064d0 + (i * 0x04), 0x00000000);
+
+	nv_wr32(dev, 0x405b00, 0x201);
+	nv_wr32(dev, 0x408850, 0x2);
+	nv_wr32(dev, 0x408958, 0x2);
+	nv_wr32(dev, 0x419f78, 0xa);
+
+	nve0_grctx_generate_icmd(dev);
+	nve0_grctx_generate_a097(dev);
+	nve0_grctx_generate_902d(dev);
+
+	nv_mask(dev, 0x000260, 0x00000001, 0x00000001);
+	nv_wr32(dev, 0x418800, 0x7026860a); //XXX
+	nv_wr32(dev, 0x41be10, 0x00bb8bc7); //XXX
+	return 0;
+}
diff --git a/drivers/gpu/drm/radeon/Makefile b/drivers/gpu/drm/radeon/Makefile
index 9d83729956f..a6598fd6642 100644
--- a/drivers/gpu/drm/radeon/Makefile
+++ b/drivers/gpu/drm/radeon/Makefile
@@ -70,8 +70,9 @@ radeon-y += radeon_device.o radeon_asic.o radeon_kms.o \
 	r200.o radeon_legacy_tv.o r600_cs.o r600_blit.o r600_blit_shaders.o \
 	r600_blit_kms.o radeon_pm.o atombios_dp.o r600_audio.o r600_hdmi.o \
 	evergreen.o evergreen_cs.o evergreen_blit_shaders.o evergreen_blit_kms.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
+	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
 
 radeon-$(CONFIG_COMPAT) += radeon_ioc32.o
 radeon-$(CONFIG_VGA_SWITCHEROO) += radeon_atpx_handler.o
diff --git a/drivers/gpu/drm/radeon/atombios_crtc.c b/drivers/gpu/drm/radeon/atombios_crtc.c
index af1054f8202..01d77d1554f 100644
--- a/drivers/gpu/drm/radeon/atombios_crtc.c
+++ b/drivers/gpu/drm/radeon/atombios_crtc.c
@@ -591,8 +591,7 @@ static u32 atombios_adjust_pll(struct drm_crtc *crtc,
 		if (encoder->crtc == crtc) {
 			radeon_encoder = to_radeon_encoder(encoder);
 			connector = radeon_get_connector_for_encoder(encoder);
-			/* if (connector && connector->display_info.bpc)
-				bpc = connector->display_info.bpc; */
+			bpc = radeon_get_monitor_bpc(connector);
 			encoder_mode = atombios_get_encoder_mode(encoder);
 			is_duallink = radeon_dig_monitor_is_duallink(encoder, mode->clock);
 			if ((radeon_encoder->devices & (ATOM_DEVICE_LCD_SUPPORT | ATOM_DEVICE_DFP_SUPPORT)) ||
@@ -968,9 +967,7 @@ static void atombios_crtc_set_pll(struct drm_crtc *crtc, struct drm_display_mode
 		struct radeon_connector_atom_dig *dig_connector =
 			radeon_connector->con_priv;
 		int dp_clock;
-
-		/* if (connector->display_info.bpc)
-			bpc = connector->display_info.bpc; */
+		bpc = radeon_get_monitor_bpc(connector);
 
 		switch (encoder_mode) {
 		case ATOM_ENCODER_MODE_DP_MST:
diff --git a/drivers/gpu/drm/radeon/atombios_dp.c b/drivers/gpu/drm/radeon/atombios_dp.c
index c57d85664e7..5131b3b0f7d 100644
--- a/drivers/gpu/drm/radeon/atombios_dp.c
+++ b/drivers/gpu/drm/radeon/atombios_dp.c
@@ -405,13 +405,10 @@ static void dp_get_adjust_train(u8 link_status[DP_LINK_STATUS_SIZE],
 /* get bpc from the EDID */
 static int convert_bpc_to_bpp(int bpc)
 {
-#if 0
 	if (bpc == 0)
 		return 24;
 	else
 		return bpc * 3;
-#endif
-	return 24;
 }
 
 /* get the max pix clock supported by the link rate and lane num */
@@ -463,7 +460,7 @@ static int radeon_dp_get_dp_lane_number(struct drm_connector *connector,
 					u8 dpcd[DP_DPCD_SIZE],
 					int pix_clock)
 {
-	int bpp = convert_bpc_to_bpp(connector->display_info.bpc);
+	int bpp = convert_bpc_to_bpp(radeon_get_monitor_bpc(connector));
 	int max_link_rate = dp_get_max_link_rate(dpcd);
 	int max_lane_num = dp_get_max_lane_number(dpcd);
 	int lane_num;
@@ -482,7 +479,7 @@ static int radeon_dp_get_dp_link_clock(struct drm_connector *connector,
 				       u8 dpcd[DP_DPCD_SIZE],
 				       int pix_clock)
 {
-	int bpp = convert_bpc_to_bpp(connector->display_info.bpc);
+	int bpp = convert_bpc_to_bpp(radeon_get_monitor_bpc(connector));
 	int lane_num, max_pix_clock;
 
 	if (radeon_connector_encoder_get_dp_bridge_encoder_id(connector) ==
@@ -533,6 +530,23 @@ u8 radeon_dp_getsinktype(struct radeon_connector *radeon_connector)
 					 dig_connector->dp_i2c_bus->rec.i2c_id, 0);
 }
 
+static void radeon_dp_probe_oui(struct radeon_connector *radeon_connector)
+{
+	struct radeon_connector_atom_dig *dig_connector = radeon_connector->con_priv;
+	u8 buf[3];
+
+	if (!(dig_connector->dpcd[DP_DOWN_STREAM_PORT_COUNT] & DP_OUI_SUPPORT))
+		return;
+
+	if (radeon_dp_aux_native_read(radeon_connector, DP_SINK_OUI, buf, 3, 0))
+		DRM_DEBUG_KMS("Sink OUI: %02hx%02hx%02hx\n",
+			      buf[0], buf[1], buf[2]);
+
+	if (radeon_dp_aux_native_read(radeon_connector, DP_BRANCH_OUI, buf, 3, 0))
+		DRM_DEBUG_KMS("Branch OUI: %02hx%02hx%02hx\n",
+			      buf[0], buf[1], buf[2]);
+}
+
 bool radeon_dp_getdpcd(struct radeon_connector *radeon_connector)
 {
 	struct radeon_connector_atom_dig *dig_connector = radeon_connector->con_priv;
@@ -546,6 +560,9 @@ bool radeon_dp_getdpcd(struct radeon_connector *radeon_connector)
 		for (i = 0; i < 8; i++)
 			DRM_DEBUG_KMS("%02x ", msg[i]);
 		DRM_DEBUG_KMS("\n");
+
+		radeon_dp_probe_oui(radeon_connector);
+
 		return true;
 	}
 	dig_connector->dpcd[0] = 0;
diff --git a/drivers/gpu/drm/radeon/atombios_encoders.c b/drivers/gpu/drm/radeon/atombios_encoders.c
index 2d39f9977e0..e7b1ec5ae8c 100644
--- a/drivers/gpu/drm/radeon/atombios_encoders.c
+++ b/drivers/gpu/drm/radeon/atombios_encoders.c
@@ -545,7 +545,7 @@ atombios_dig_encoder_setup(struct drm_encoder *encoder, int action, int panel_mo
 		dp_clock = dig_connector->dp_clock;
 		dp_lane_count = dig_connector->dp_lane_count;
 		hpd_id = radeon_connector->hpd.hpd;
-		/* bpc = connector->display_info.bpc; */
+		bpc = radeon_get_monitor_bpc(connector);
 	}
 
 	/* no dig encoder assigned */
@@ -1163,7 +1163,7 @@ atombios_external_encoder_setup(struct drm_encoder *encoder,
 		dp_lane_count = dig_connector->dp_lane_count;
 		connector_object_id =
 			(radeon_connector->connector_object_id & OBJECT_ID_MASK) >> OBJECT_ID_SHIFT;
-		/* bpc = connector->display_info.bpc; */
+		bpc = radeon_get_monitor_bpc(connector);
 	}
 
 	memset(&args, 0, sizeof(args));
@@ -1926,7 +1926,10 @@ radeon_atom_encoder_mode_set(struct drm_encoder *encoder,
 
 	if (atombios_get_encoder_mode(encoder) == ATOM_ENCODER_MODE_HDMI) {
 		r600_hdmi_enable(encoder);
-		r600_hdmi_setmode(encoder, adjusted_mode);
+		if (ASIC_IS_DCE4(rdev))
+			evergreen_hdmi_setmode(encoder, adjusted_mode);
+		else
+			r600_hdmi_setmode(encoder, adjusted_mode);
 	}
 }
 
@@ -2081,6 +2084,7 @@ radeon_atom_ext_encoder_setup_ddc(struct drm_encoder *encoder)
 
 static void radeon_atom_encoder_prepare(struct drm_encoder *encoder)
 {
+	struct radeon_device *rdev = encoder->dev->dev_private;
 	struct radeon_encoder *radeon_encoder = to_radeon_encoder(encoder);
 	struct drm_connector *connector = radeon_get_connector_for_encoder(encoder);
 
@@ -2089,8 +2093,16 @@ static void radeon_atom_encoder_prepare(struct drm_encoder *encoder)
 	    (radeon_encoder_get_dp_bridge_encoder_id(encoder) !=
 	     ENCODER_OBJECT_ID_NONE)) {
 		struct radeon_encoder_atom_dig *dig = radeon_encoder->enc_priv;
-		if (dig)
+		if (dig) {
 			dig->dig_encoder = radeon_atom_pick_dig_encoder(encoder);
+			if (radeon_encoder->active_device & ATOM_DEVICE_DFP_SUPPORT) {
+				if (rdev->family >= CHIP_R600)
+					dig->afmt = rdev->mode_info.afmt[dig->dig_encoder];
+				else
+					/* RS600/690/740 have only 1 afmt block */
+					dig->afmt = rdev->mode_info.afmt[0];
+			}
+		}
 	}
 
 	radeon_atom_output_lock(encoder, true);
diff --git a/drivers/gpu/drm/radeon/evergreen.c b/drivers/gpu/drm/radeon/evergreen.c
index cfa372cb1cb..58991af9050 100644
--- a/drivers/gpu/drm/radeon/evergreen.c
+++ b/drivers/gpu/drm/radeon/evergreen.c
@@ -2424,27 +2424,18 @@ bool evergreen_gpu_is_lockup(struct radeon_device *rdev, struct radeon_ring *rin
 	u32 srbm_status;
 	u32 grbm_status;
 	u32 grbm_status_se0, grbm_status_se1;
-	struct r100_gpu_lockup *lockup = &rdev->config.evergreen.lockup;
-	int r;
 
 	srbm_status = RREG32(SRBM_STATUS);
 	grbm_status = RREG32(GRBM_STATUS);
 	grbm_status_se0 = RREG32(GRBM_STATUS_SE0);
 	grbm_status_se1 = RREG32(GRBM_STATUS_SE1);
 	if (!(grbm_status & GUI_ACTIVE)) {
-		r100_gpu_lockup_update(lockup, ring);
+		radeon_ring_lockup_update(ring);
 		return false;
 	}
 	/* force CP activities */
-	r = radeon_ring_lock(rdev, ring, 2);
-	if (!r) {
-		/* PACKET2 NOP */
-		radeon_ring_write(ring, 0x80000000);
-		radeon_ring_write(ring, 0x80000000);
-		radeon_ring_unlock_commit(rdev, ring);
-	}
-	ring->rptr = RREG32(CP_RB_RPTR);
-	return r100_gpu_cp_is_lockup(rdev, lockup, ring);
+	radeon_ring_force_activity(rdev, ring);
+	return radeon_ring_test_lockup(rdev, ring);
 }
 
 static int evergreen_gpu_soft_reset(struct radeon_device *rdev)
@@ -2594,6 +2585,7 @@ int evergreen_irq_set(struct radeon_device *rdev)
 	u32 hpd1, hpd2, hpd3, hpd4, hpd5, hpd6;
 	u32 grbm_int_cntl = 0;
 	u32 grph1 = 0, grph2 = 0, grph3 = 0, grph4 = 0, grph5 = 0, grph6 = 0;
+	u32 afmt1 = 0, afmt2 = 0, afmt3 = 0, afmt4 = 0, afmt5 = 0, afmt6 = 0;
 
 	if (!rdev->irq.installed) {
 		WARN(1, "Can't enable IRQ/MSI because no handler is installed\n");
@@ -2614,6 +2606,13 @@ int evergreen_irq_set(struct radeon_device *rdev)
 	hpd5 = RREG32(DC_HPD5_INT_CONTROL) & ~DC_HPDx_INT_EN;
 	hpd6 = RREG32(DC_HPD6_INT_CONTROL) & ~DC_HPDx_INT_EN;
 
+	afmt1 = RREG32(AFMT_AUDIO_PACKET_CONTROL + EVERGREEN_CRTC0_REGISTER_OFFSET) & ~AFMT_AZ_FORMAT_WTRIG_MASK;
+	afmt2 = RREG32(AFMT_AUDIO_PACKET_CONTROL + EVERGREEN_CRTC1_REGISTER_OFFSET) & ~AFMT_AZ_FORMAT_WTRIG_MASK;
+	afmt3 = RREG32(AFMT_AUDIO_PACKET_CONTROL + EVERGREEN_CRTC2_REGISTER_OFFSET) & ~AFMT_AZ_FORMAT_WTRIG_MASK;
+	afmt4 = RREG32(AFMT_AUDIO_PACKET_CONTROL + EVERGREEN_CRTC3_REGISTER_OFFSET) & ~AFMT_AZ_FORMAT_WTRIG_MASK;
+	afmt5 = RREG32(AFMT_AUDIO_PACKET_CONTROL + EVERGREEN_CRTC4_REGISTER_OFFSET) & ~AFMT_AZ_FORMAT_WTRIG_MASK;
+	afmt6 = RREG32(AFMT_AUDIO_PACKET_CONTROL + EVERGREEN_CRTC5_REGISTER_OFFSET) & ~AFMT_AZ_FORMAT_WTRIG_MASK;
+
 	if (rdev->family >= CHIP_CAYMAN) {
 		/* enable CP interrupts on all rings */
 		if (rdev->irq.sw_int[RADEON_RING_TYPE_GFX_INDEX]) {
@@ -2690,6 +2689,30 @@ int evergreen_irq_set(struct radeon_device *rdev)
 		DRM_DEBUG("evergreen_irq_set: hpd 6\n");
 		hpd6 |= DC_HPDx_INT_EN;
 	}
+	if (rdev->irq.afmt[0]) {
+		DRM_DEBUG("evergreen_irq_set: hdmi 0\n");
+		afmt1 |= AFMT_AZ_FORMAT_WTRIG_MASK;
+	}
+	if (rdev->irq.afmt[1]) {
+		DRM_DEBUG("evergreen_irq_set: hdmi 1\n");
+		afmt2 |= AFMT_AZ_FORMAT_WTRIG_MASK;
+	}
+	if (rdev->irq.afmt[2]) {
+		DRM_DEBUG("evergreen_irq_set: hdmi 2\n");
+		afmt3 |= AFMT_AZ_FORMAT_WTRIG_MASK;
+	}
+	if (rdev->irq.afmt[3]) {
+		DRM_DEBUG("evergreen_irq_set: hdmi 3\n");
+		afmt4 |= AFMT_AZ_FORMAT_WTRIG_MASK;
+	}
+	if (rdev->irq.afmt[4]) {
+		DRM_DEBUG("evergreen_irq_set: hdmi 4\n");
+		afmt5 |= AFMT_AZ_FORMAT_WTRIG_MASK;
+	}
+	if (rdev->irq.afmt[5]) {
+		DRM_DEBUG("evergreen_irq_set: hdmi 5\n");
+		afmt6 |= AFMT_AZ_FORMAT_WTRIG_MASK;
+	}
 	if (rdev->irq.gui_idle) {
 		DRM_DEBUG("gui idle\n");
 		grbm_int_cntl |= GUI_IDLE_INT_ENABLE;
@@ -2732,6 +2755,13 @@ int evergreen_irq_set(struct radeon_device *rdev)
 	WREG32(DC_HPD5_INT_CONTROL, hpd5);
 	WREG32(DC_HPD6_INT_CONTROL, hpd6);
 
+	WREG32(AFMT_AUDIO_PACKET_CONTROL + EVERGREEN_CRTC0_REGISTER_OFFSET, afmt1);
+	WREG32(AFMT_AUDIO_PACKET_CONTROL + EVERGREEN_CRTC1_REGISTER_OFFSET, afmt2);
+	WREG32(AFMT_AUDIO_PACKET_CONTROL + EVERGREEN_CRTC2_REGISTER_OFFSET, afmt3);
+	WREG32(AFMT_AUDIO_PACKET_CONTROL + EVERGREEN_CRTC3_REGISTER_OFFSET, afmt4);
+	WREG32(AFMT_AUDIO_PACKET_CONTROL + EVERGREEN_CRTC4_REGISTER_OFFSET, afmt5);
+	WREG32(AFMT_AUDIO_PACKET_CONTROL + EVERGREEN_CRTC5_REGISTER_OFFSET, afmt6);
+
 	return 0;
 }
 
@@ -2756,6 +2786,13 @@ static void evergreen_irq_ack(struct radeon_device *rdev)
 		rdev->irq.stat_regs.evergreen.d6grph_int = RREG32(GRPH_INT_STATUS + EVERGREEN_CRTC5_REGISTER_OFFSET);
 	}
 
+	rdev->irq.stat_regs.evergreen.afmt_status1 = RREG32(AFMT_STATUS + EVERGREEN_CRTC0_REGISTER_OFFSET);
+	rdev->irq.stat_regs.evergreen.afmt_status2 = RREG32(AFMT_STATUS + EVERGREEN_CRTC1_REGISTER_OFFSET);
+	rdev->irq.stat_regs.evergreen.afmt_status3 = RREG32(AFMT_STATUS + EVERGREEN_CRTC2_REGISTER_OFFSET);
+	rdev->irq.stat_regs.evergreen.afmt_status4 = RREG32(AFMT_STATUS + EVERGREEN_CRTC3_REGISTER_OFFSET);
+	rdev->irq.stat_regs.evergreen.afmt_status5 = RREG32(AFMT_STATUS + EVERGREEN_CRTC4_REGISTER_OFFSET);
+	rdev->irq.stat_regs.evergreen.afmt_status6 = RREG32(AFMT_STATUS + EVERGREEN_CRTC5_REGISTER_OFFSET);
+
 	if (rdev->irq.stat_regs.evergreen.d1grph_int & GRPH_PFLIP_INT_OCCURRED)
 		WREG32(GRPH_INT_STATUS + EVERGREEN_CRTC0_REGISTER_OFFSET, GRPH_PFLIP_INT_CLEAR);
 	if (rdev->irq.stat_regs.evergreen.d2grph_int & GRPH_PFLIP_INT_OCCURRED)
@@ -2829,6 +2866,36 @@ static void evergreen_irq_ack(struct radeon_device *rdev)
 		tmp |= DC_HPDx_INT_ACK;
 		WREG32(DC_HPD6_INT_CONTROL, tmp);
 	}
+	if (rdev->irq.stat_regs.evergreen.afmt_status1 & AFMT_AZ_FORMAT_WTRIG) {
+		tmp = RREG32(AFMT_AUDIO_PACKET_CONTROL + EVERGREEN_CRTC0_REGISTER_OFFSET);
+		tmp |= AFMT_AZ_FORMAT_WTRIG_ACK;
+		WREG32(AFMT_AUDIO_PACKET_CONTROL + EVERGREEN_CRTC0_REGISTER_OFFSET, tmp);
+	}
+	if (rdev->irq.stat_regs.evergreen.afmt_status2 & AFMT_AZ_FORMAT_WTRIG) {
+		tmp = RREG32(AFMT_AUDIO_PACKET_CONTROL + EVERGREEN_CRTC1_REGISTER_OFFSET);
+		tmp |= AFMT_AZ_FORMAT_WTRIG_ACK;
+		WREG32(AFMT_AUDIO_PACKET_CONTROL + EVERGREEN_CRTC1_REGISTER_OFFSET, tmp);
+	}
+	if (rdev->irq.stat_regs.evergreen.afmt_status3 & AFMT_AZ_FORMAT_WTRIG) {
+		tmp = RREG32(AFMT_AUDIO_PACKET_CONTROL + EVERGREEN_CRTC2_REGISTER_OFFSET);
+		tmp |= AFMT_AZ_FORMAT_WTRIG_ACK;
+		WREG32(AFMT_AUDIO_PACKET_CONTROL + EVERGREEN_CRTC2_REGISTER_OFFSET, tmp);
+	}
+	if (rdev->irq.stat_regs.evergreen.afmt_status4 & AFMT_AZ_FORMAT_WTRIG) {
+		tmp = RREG32(AFMT_AUDIO_PACKET_CONTROL + EVERGREEN_CRTC3_REGISTER_OFFSET);
+		tmp |= AFMT_AZ_FORMAT_WTRIG_ACK;
+		WREG32(AFMT_AUDIO_PACKET_CONTROL + EVERGREEN_CRTC3_REGISTER_OFFSET, tmp);
+	}
+	if (rdev->irq.stat_regs.evergreen.afmt_status5 & AFMT_AZ_FORMAT_WTRIG) {
+		tmp = RREG32(AFMT_AUDIO_PACKET_CONTROL + EVERGREEN_CRTC4_REGISTER_OFFSET);
+		tmp |= AFMT_AZ_FORMAT_WTRIG_ACK;
+		WREG32(AFMT_AUDIO_PACKET_CONTROL + EVERGREEN_CRTC4_REGISTER_OFFSET, tmp);
+	}
+	if (rdev->irq.stat_regs.evergreen.afmt_status6 & AFMT_AZ_FORMAT_WTRIG) {
+		tmp = RREG32(AFMT_AUDIO_PACKET_CONTROL + EVERGREEN_CRTC5_REGISTER_OFFSET);
+		tmp |= AFMT_AZ_FORMAT_WTRIG_ACK;
+		WREG32(AFMT_AUDIO_PACKET_CONTROL + EVERGREEN_CRTC5_REGISTER_OFFSET, tmp);
+	}
 }
 
 void evergreen_irq_disable(struct radeon_device *rdev)
@@ -2878,6 +2945,7 @@ int evergreen_irq_process(struct radeon_device *rdev)
 	u32 ring_index;
 	unsigned long flags;
 	bool queue_hotplug = false;
+	bool queue_hdmi = false;
 
 	if (!rdev->ih.enabled || rdev->shutdown)
 		return IRQ_NONE;
@@ -3111,6 +3179,55 @@ restart_ih:
 				break;
 			}
 			break;
+		case 44: /* hdmi */
+			switch (src_data) {
+			case 0:
+				if (rdev->irq.stat_regs.evergreen.afmt_status1 & AFMT_AZ_FORMAT_WTRIG) {
+					rdev->irq.stat_regs.evergreen.afmt_status1 &= ~AFMT_AZ_FORMAT_WTRIG;
+					queue_hdmi = true;
+					DRM_DEBUG("IH: HDMI0\n");
+				}
+				break;
+			case 1:
+				if (rdev->irq.stat_regs.evergreen.afmt_status2 & AFMT_AZ_FORMAT_WTRIG) {
+					rdev->irq.stat_regs.evergreen.afmt_status2 &= ~AFMT_AZ_FORMAT_WTRIG;
+					queue_hdmi = true;
+					DRM_DEBUG("IH: HDMI1\n");
+				}
+				break;
+			case 2:
+				if (rdev->irq.stat_regs.evergreen.afmt_status3 & AFMT_AZ_FORMAT_WTRIG) {
+					rdev->irq.stat_regs.evergreen.afmt_status3 &= ~AFMT_AZ_FORMAT_WTRIG;
+					queue_hdmi = true;
+					DRM_DEBUG("IH: HDMI2\n");
+				}
+				break;
+			case 3:
+				if (rdev->irq.stat_regs.evergreen.afmt_status4 & AFMT_AZ_FORMAT_WTRIG) {
+					rdev->irq.stat_regs.evergreen.afmt_status4 &= ~AFMT_AZ_FORMAT_WTRIG;
+					queue_hdmi = true;
+					DRM_DEBUG("IH: HDMI3\n");
+				}
+				break;
+			case 4:
+				if (rdev->irq.stat_regs.evergreen.afmt_status5 & AFMT_AZ_FORMAT_WTRIG) {
+					rdev->irq.stat_regs.evergreen.afmt_status5 &= ~AFMT_AZ_FORMAT_WTRIG;
+					queue_hdmi = true;
+					DRM_DEBUG("IH: HDMI4\n");
+				}
+				break;
+			case 5:
+				if (rdev->irq.stat_regs.evergreen.afmt_status6 & AFMT_AZ_FORMAT_WTRIG) {
+					rdev->irq.stat_regs.evergreen.afmt_status6 &= ~AFMT_AZ_FORMAT_WTRIG;
+					queue_hdmi = true;
+					DRM_DEBUG("IH: HDMI5\n");
+				}
+				break;
+			default:
+				DRM_ERROR("Unhandled interrupt: %d %d\n", src_id, src_data);
+				break;
+			}
+			break;
 		case 176: /* CP_INT in ring buffer */
 		case 177: /* CP_INT in IB1 */
 		case 178: /* CP_INT in IB2 */
@@ -3154,6 +3271,8 @@ restart_ih:
 		goto restart_ih;
 	if (queue_hotplug)
 		schedule_work(&rdev->hotplug_work);
+	if (queue_hdmi)
+		schedule_work(&rdev->audio_work);
 	rdev->ih.rptr = rptr;
 	WREG32(IH_RB_RPTR, rdev->ih.rptr);
 	spin_unlock_irqrestore(&rdev->ih.lock, flags);
@@ -3248,12 +3367,9 @@ static int evergreen_startup(struct radeon_device *rdev)
 	if (r)
 		return r;
 
-	r = radeon_ib_test(rdev, RADEON_RING_TYPE_GFX_INDEX, &rdev->ring[RADEON_RING_TYPE_GFX_INDEX]);
-	if (r) {
-		DRM_ERROR("radeon: failed testing IB (%d).\n", r);
-		rdev->accel_working = false;
+	r = radeon_ib_ring_tests(rdev);
+	if (r)
 		return r;
-	}
 
 	r = r600_audio_init(rdev);
 	if (r) {
@@ -3319,10 +3435,6 @@ int evergreen_init(struct radeon_device *rdev)
 {
 	int r;
 
-	/* This don't do much */
-	r = radeon_gem_init(rdev);
-	if (r)
-		return r;
 	/* Read BIOS */
 	if (!radeon_get_bios(rdev)) {
 		if (ASIC_IS_AVIVO(rdev))
@@ -3434,7 +3546,6 @@ void evergreen_fini(struct radeon_device *rdev)
 	evergreen_pcie_gart_fini(rdev);
 	r600_vram_scratch_fini(rdev);
 	radeon_gem_fini(rdev);
-	radeon_semaphore_driver_fini(rdev);
 	radeon_fence_driver_fini(rdev);
 	radeon_agp_fini(rdev);
 	radeon_bo_fini(rdev);
diff --git a/drivers/gpu/drm/radeon/evergreen_blit_kms.c b/drivers/gpu/drm/radeon/evergreen_blit_kms.c
index 222acd2d33d..1e96bd458cf 100644
--- a/drivers/gpu/drm/radeon/evergreen_blit_kms.c
+++ b/drivers/gpu/drm/radeon/evergreen_blit_kms.c
@@ -637,7 +637,6 @@ int evergreen_blit_init(struct radeon_device *rdev)
 	if (rdev->r600_blit.shader_obj)
 		goto done;
 
-	mutex_init(&rdev->r600_blit.mutex);
 	rdev->r600_blit.state_offset = 0;
 
 	if (rdev->family < CHIP_CAYMAN)
@@ -669,7 +668,7 @@ int evergreen_blit_init(struct radeon_device *rdev)
 	obj_size = ALIGN(obj_size, 256);
 
 	r = radeon_bo_create(rdev, obj_size, PAGE_SIZE, true, RADEON_GEM_DOMAIN_VRAM,
-				&rdev->r600_blit.shader_obj);
+			     NULL, &rdev->r600_blit.shader_obj);
 	if (r) {
 		DRM_ERROR("evergreen failed to allocate shader\n");
 		return r;
diff --git a/drivers/gpu/drm/radeon/evergreen_cs.c b/drivers/gpu/drm/radeon/evergreen_cs.c
index 70089d32b80..4e7dd2b4843 100644
--- a/drivers/gpu/drm/radeon/evergreen_cs.c
+++ b/drivers/gpu/drm/radeon/evergreen_cs.c
@@ -1057,7 +1057,7 @@ static int evergreen_cs_packet_parse_vline(struct radeon_cs_parser *p)
 	uint32_t header, h_idx, reg, wait_reg_mem_info;
 	volatile uint32_t *ib;
 
-	ib = p->ib->ptr;
+	ib = p->ib.ptr;
 
 	/* parse the WAIT_REG_MEM */
 	r = evergreen_cs_packet_parse(p, &wait_reg_mem, p->idx);
@@ -1215,7 +1215,7 @@ static int evergreen_cs_check_reg(struct radeon_cs_parser *p, u32 reg, u32 idx)
 		if (!(evergreen_reg_safe_bm[i] & m))
 			return 0;
 	}
-	ib = p->ib->ptr;
+	ib = p->ib.ptr;
 	switch (reg) {
 	/* force following reg to 0 in an attempt to disable out buffer
 	 * which will need us to better understand how it works to perform
@@ -1896,7 +1896,7 @@ static int evergreen_packet3_check(struct radeon_cs_parser *p,
 	u32 idx_value;
 
 	track = (struct evergreen_cs_track *)p->track;
-	ib = p->ib->ptr;
+	ib = p->ib.ptr;
 	idx = pkt->idx + 1;
 	idx_value = radeon_get_ib_value(p, idx);
 
@@ -2610,8 +2610,8 @@ int evergreen_cs_parse(struct radeon_cs_parser *p)
 		}
 	} while (p->idx < p->chunks[p->chunk_ib_idx].length_dw);
 #if 0
-	for (r = 0; r < p->ib->length_dw; r++) {
-		printk(KERN_INFO "%05d  0x%08X\n", r, p->ib->ptr[r]);
+	for (r = 0; r < p->ib.length_dw; r++) {
+		printk(KERN_INFO "%05d  0x%08X\n", r, p->ib.ptr[r]);
 		mdelay(1);
 	}
 #endif
diff --git a/drivers/gpu/drm/radeon/evergreen_hdmi.c b/drivers/gpu/drm/radeon/evergreen_hdmi.c
new file mode 100644
index 00000000000..a51f880985f
--- /dev/null
+++ b/drivers/gpu/drm/radeon/evergreen_hdmi.c
@@ -0,0 +1,216 @@
+/*
+ * Copyright 2008 Advanced Micro Devices, Inc.
+ * Copyright 2008 Red Hat Inc.
+ * Copyright 2009 Christian König.
+ *
+ * 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: Christian König
+ *          Rafał Miłecki
+ */
+#include "drmP.h"
+#include "radeon_drm.h"
+#include "radeon.h"
+#include "radeon_asic.h"
+#include "evergreend.h"
+#include "atom.h"
+
+/*
+ * update the N and CTS parameters for a given pixel clock rate
+ */
+static void evergreen_hdmi_update_ACR(struct drm_encoder *encoder, uint32_t clock)
+{
+	struct drm_device *dev = encoder->dev;
+	struct radeon_device *rdev = dev->dev_private;
+	struct radeon_hdmi_acr acr = r600_hdmi_acr(clock);
+	struct radeon_encoder *radeon_encoder = to_radeon_encoder(encoder);
+	struct radeon_encoder_atom_dig *dig = radeon_encoder->enc_priv;
+	uint32_t offset = dig->afmt->offset;
+
+	WREG32(HDMI_ACR_32_0 + offset, HDMI_ACR_CTS_32(acr.cts_32khz));
+	WREG32(HDMI_ACR_32_1 + offset, acr.n_32khz);
+
+	WREG32(HDMI_ACR_44_0 + offset, HDMI_ACR_CTS_44(acr.cts_44_1khz));
+	WREG32(HDMI_ACR_44_1 + offset, acr.n_44_1khz);
+
+	WREG32(HDMI_ACR_48_0 + offset, HDMI_ACR_CTS_48(acr.cts_48khz));
+	WREG32(HDMI_ACR_48_1 + offset, acr.n_48khz);
+}
+
+/*
+ * calculate the crc for a given info frame
+ */
+static void evergreen_hdmi_infoframe_checksum(uint8_t packetType,
+					 uint8_t versionNumber,
+					 uint8_t length,
+					 uint8_t *frame)
+{
+	int i;
+	frame[0] = packetType + versionNumber + length;
+	for (i = 1; i <= length; i++)
+		frame[0] += frame[i];
+	frame[0] = 0x100 - frame[0];
+}
+
+/*
+ * build a HDMI Video Info Frame
+ */
+static void evergreen_hdmi_videoinfoframe(
+	struct drm_encoder *encoder,
+	uint8_t color_format,
+	int active_information_present,
+	uint8_t active_format_aspect_ratio,
+	uint8_t scan_information,
+	uint8_t colorimetry,
+	uint8_t ex_colorimetry,
+	uint8_t quantization,
+	int ITC,
+	uint8_t picture_aspect_ratio,
+	uint8_t video_format_identification,
+	uint8_t pixel_repetition,
+	uint8_t non_uniform_picture_scaling,
+	uint8_t bar_info_data_valid,
+	uint16_t top_bar,
+	uint16_t bottom_bar,
+	uint16_t left_bar,
+	uint16_t right_bar
+)
+{
+	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 = dig->afmt->offset;
+
+	uint8_t frame[14];
+
+	frame[0x0] = 0;
+	frame[0x1] =
+		(scan_information & 0x3) |
+		((bar_info_data_valid & 0x3) << 2) |
+		((active_information_present & 0x1) << 4) |
+		((color_format & 0x3) << 5);
+	frame[0x2] =
+		(active_format_aspect_ratio & 0xF) |
+		((picture_aspect_ratio & 0x3) << 4) |
+		((colorimetry & 0x3) << 6);
+	frame[0x3] =
+		(non_uniform_picture_scaling & 0x3) |
+		((quantization & 0x3) << 2) |
+		((ex_colorimetry & 0x7) << 4) |
+		((ITC & 0x1) << 7);
+	frame[0x4] = (video_format_identification & 0x7F);
+	frame[0x5] = (pixel_repetition & 0xF);
+	frame[0x6] = (top_bar & 0xFF);
+	frame[0x7] = (top_bar >> 8);
+	frame[0x8] = (bottom_bar & 0xFF);
+	frame[0x9] = (bottom_bar >> 8);
+	frame[0xA] = (left_bar & 0xFF);
+	frame[0xB] = (left_bar >> 8);
+	frame[0xC] = (right_bar & 0xFF);
+	frame[0xD] = (right_bar >> 8);
+
+	evergreen_hdmi_infoframe_checksum(0x82, 0x02, 0x0D, frame);
+	/* Our header values (type, version, length) should be alright, Intel
+	 * is using the same. Checksum function also seems to be OK, it works
+	 * fine for audio infoframe. However calculated value is always lower
+	 * by 2 in comparison to fglrx. It breaks displaying anything in case
+	 * of TVs that strictly check the checksum. Hack it manually here to
+	 * workaround this issue. */
+	frame[0x0] += 2;
+
+	WREG32(AFMT_AVI_INFO0 + offset,
+		frame[0x0] | (frame[0x1] << 8) | (frame[0x2] << 16) | (frame[0x3] << 24));
+	WREG32(AFMT_AVI_INFO1 + offset,
+		frame[0x4] | (frame[0x5] << 8) | (frame[0x6] << 16) | (frame[0x7] << 24));
+	WREG32(AFMT_AVI_INFO2 + offset,
+		frame[0x8] | (frame[0x9] << 8) | (frame[0xA] << 16) | (frame[0xB] << 24));
+	WREG32(AFMT_AVI_INFO3 + offset,
+		frame[0xC] | (frame[0xD] << 8));
+}
+
+/*
+ * update the info frames with the data from the current display mode
+ */
+void evergreen_hdmi_setmode(struct drm_encoder *encoder, struct drm_display_mode *mode)
+{
+	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_DCE5(rdev))
+		return;
+
+	/* Silent, r600_hdmi_enable will raise WARN for us */
+	if (!dig->afmt->enabled)
+		return;
+	offset = dig->afmt->offset;
+
+	r600_audio_set_clock(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 */
+
+	WREG32(AFMT_INFOFRAME_CONTROL0 + offset,
+	       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 */
+
+	evergreen_hdmi_videoinfoframe(encoder, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
+				      0, 0, 0, 0, 0, 0);
+
+	evergreen_hdmi_update_ACR(encoder, mode->clock);
+
+	/* it's unknown what these bits do excatly, but it's indeed quite useful for debugging */
+	WREG32(AFMT_RAMP_CONTROL0 + offset, 0x00FFFFFF);
+	WREG32(AFMT_RAMP_CONTROL1 + offset, 0x007FFFFF);
+	WREG32(AFMT_RAMP_CONTROL2 + offset, 0x00000001);
+	WREG32(AFMT_RAMP_CONTROL3 + offset, 0x00000001);
+}
diff --git a/drivers/gpu/drm/radeon/evergreen_reg.h b/drivers/gpu/drm/radeon/evergreen_reg.h
index 96c10b3991a..8beac106502 100644
--- a/drivers/gpu/drm/radeon/evergreen_reg.h
+++ b/drivers/gpu/drm/radeon/evergreen_reg.h
@@ -232,6 +232,4 @@
 /* HDMI blocks at 0x7030, 0x7c30, 0x10830, 0x11430, 0x12030, 0x12c30 */
 #define EVERGREEN_HDMI_BASE				0x7030
 
-#define EVERGREEN_HDMI_CONFIG_OFFSET			0xf0
-
 #endif
diff --git a/drivers/gpu/drm/radeon/evergreend.h b/drivers/gpu/drm/radeon/evergreend.h
index b4eefc355f1..79130bfd1d6 100644
--- a/drivers/gpu/drm/radeon/evergreend.h
+++ b/drivers/gpu/drm/radeon/evergreend.h
@@ -112,6 +112,226 @@
 #define	CP_SEM_INCOMPLETE_TIMER_CNTL			0x85C8
 #define	CP_DEBUG					0xC1FC
 
+/* Audio clocks */
+#define DCCG_AUDIO_DTO_SOURCE             0x05ac
+#       define DCCG_AUDIO_DTO0_SOURCE_SEL(x) ((x) << 0) /* crtc0 - crtc5 */
+#       define DCCG_AUDIO_DTO_SEL         (1 << 4) /* 0=dto0 1=dto1 */
+
+#define DCCG_AUDIO_DTO0_PHASE             0x05b0
+#define DCCG_AUDIO_DTO0_MODULE            0x05b4
+#define DCCG_AUDIO_DTO0_LOAD              0x05b8
+#define DCCG_AUDIO_DTO0_CNTL              0x05bc
+
+#define DCCG_AUDIO_DTO1_PHASE             0x05c0
+#define DCCG_AUDIO_DTO1_MODULE            0x05c4
+#define DCCG_AUDIO_DTO1_LOAD              0x05c8
+#define DCCG_AUDIO_DTO1_CNTL              0x05cc
+
+/* DCE 4.0 AFMT */
+#define HDMI_CONTROL                         0x7030
+#       define HDMI_KEEPOUT_MODE             (1 << 0)
+#       define HDMI_PACKET_GEN_VERSION       (1 << 4) /* 0 = r6xx compat */
+#       define HDMI_ERROR_ACK                (1 << 8)
+#       define HDMI_ERROR_MASK               (1 << 9)
+#       define HDMI_DEEP_COLOR_ENABLE        (1 << 24)
+#       define HDMI_DEEP_COLOR_DEPTH         (((x) & 3) << 28)
+#       define HDMI_24BIT_DEEP_COLOR         0
+#       define HDMI_30BIT_DEEP_COLOR         1
+#       define HDMI_36BIT_DEEP_COLOR         2
+#define HDMI_STATUS                          0x7034
+#       define HDMI_ACTIVE_AVMUTE            (1 << 0)
+#       define HDMI_AUDIO_PACKET_ERROR       (1 << 16)
+#       define HDMI_VBI_PACKET_ERROR         (1 << 20)
+#define HDMI_AUDIO_PACKET_CONTROL            0x7038
+#       define HDMI_AUDIO_DELAY_EN(x)        (((x) & 3) << 4)
+#       define HDMI_AUDIO_PACKETS_PER_LINE(x)  (((x) & 0x1f) << 16)
+#define HDMI_ACR_PACKET_CONTROL              0x703c
+#       define HDMI_ACR_SEND                 (1 << 0)
+#       define HDMI_ACR_CONT                 (1 << 1)
+#       define HDMI_ACR_SELECT(x)            (((x) & 3) << 4)
+#       define HDMI_ACR_HW                   0
+#       define HDMI_ACR_32                   1
+#       define HDMI_ACR_44                   2
+#       define HDMI_ACR_48                   3
+#       define HDMI_ACR_SOURCE               (1 << 8) /* 0 - hw; 1 - cts value */
+#       define HDMI_ACR_AUTO_SEND            (1 << 12)
+#       define HDMI_ACR_N_MULTIPLE(x)        (((x) & 7) << 16)
+#       define HDMI_ACR_X1                   1
+#       define HDMI_ACR_X2                   2
+#       define HDMI_ACR_X4                   4
+#       define HDMI_ACR_AUDIO_PRIORITY       (1 << 31)
+#define HDMI_VBI_PACKET_CONTROL              0x7040
+#       define HDMI_NULL_SEND                (1 << 0)
+#       define HDMI_GC_SEND                  (1 << 4)
+#       define HDMI_GC_CONT                  (1 << 5) /* 0 - once; 1 - every frame */
+#define HDMI_INFOFRAME_CONTROL0              0x7044
+#       define HDMI_AVI_INFO_SEND            (1 << 0)
+#       define HDMI_AVI_INFO_CONT            (1 << 1)
+#       define HDMI_AUDIO_INFO_SEND          (1 << 4)
+#       define HDMI_AUDIO_INFO_CONT          (1 << 5)
+#       define HDMI_MPEG_INFO_SEND           (1 << 8)
+#       define HDMI_MPEG_INFO_CONT           (1 << 9)
+#define HDMI_INFOFRAME_CONTROL1              0x7048
+#       define HDMI_AVI_INFO_LINE(x)         (((x) & 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
+#       define HDMI_GENERIC0_SEND            (1 << 0)
+#       define HDMI_GENERIC0_CONT            (1 << 1)
+#       define HDMI_GENERIC1_SEND            (1 << 4)
+#       define HDMI_GENERIC1_CONT            (1 << 5)
+#       define HDMI_GENERIC0_LINE(x)         (((x) & 0x3f) << 16)
+#       define HDMI_GENERIC1_LINE(x)         (((x) & 0x3f) << 24)
+#define HDMI_GC                              0x7058
+#       define HDMI_GC_AVMUTE                (1 << 0)
+#       define HDMI_GC_AVMUTE_CONT           (1 << 2)
+#define AFMT_AUDIO_PACKET_CONTROL2           0x705c
+#       define AFMT_AUDIO_LAYOUT_OVRD        (1 << 0)
+#       define AFMT_AUDIO_LAYOUT_SELECT      (1 << 1)
+#       define AFMT_60958_CS_SOURCE          (1 << 4)
+#       define AFMT_AUDIO_CHANNEL_ENABLE(x)  (((x) & 0xff) << 8)
+#       define AFMT_DP_AUDIO_STREAM_ID(x)    (((x) & 0xff) << 16)
+#define AFMT_AVI_INFO0                       0x7084
+#       define AFMT_AVI_INFO_CHECKSUM(x)     (((x) & 0xff) << 0)
+#       define AFMT_AVI_INFO_S(x)            (((x) & 3) << 8)
+#       define AFMT_AVI_INFO_B(x)            (((x) & 3) << 10)
+#       define AFMT_AVI_INFO_A(x)            (((x) & 1) << 12)
+#       define AFMT_AVI_INFO_Y(x)            (((x) & 3) << 13)
+#       define AFMT_AVI_INFO_Y_RGB           0
+#       define AFMT_AVI_INFO_Y_YCBCR422      1
+#       define AFMT_AVI_INFO_Y_YCBCR444      2
+#       define AFMT_AVI_INFO_Y_A_B_S(x)      (((x) & 0xff) << 8)
+#       define AFMT_AVI_INFO_R(x)            (((x) & 0xf) << 16)
+#       define AFMT_AVI_INFO_M(x)            (((x) & 0x3) << 20)
+#       define AFMT_AVI_INFO_C(x)            (((x) & 0x3) << 22)
+#       define AFMT_AVI_INFO_C_M_R(x)        (((x) & 0xff) << 16)
+#       define AFMT_AVI_INFO_SC(x)           (((x) & 0x3) << 24)
+#       define AFMT_AVI_INFO_Q(x)            (((x) & 0x3) << 26)
+#       define AFMT_AVI_INFO_EC(x)           (((x) & 0x3) << 28)
+#       define AFMT_AVI_INFO_ITC(x)          (((x) & 0x1) << 31)
+#       define AFMT_AVI_INFO_ITC_EC_Q_SC(x)  (((x) & 0xff) << 24)
+#define AFMT_AVI_INFO1                       0x7088
+#       define AFMT_AVI_INFO_VIC(x)          (((x) & 0x7f) << 0) /* don't use avi infoframe v1 */
+#       define AFMT_AVI_INFO_PR(x)           (((x) & 0xf) << 8) /* don't use avi infoframe v1 */
+#       define AFMT_AVI_INFO_CN(x)           (((x) & 0x3) << 12)
+#       define AFMT_AVI_INFO_YQ(x)           (((x) & 0x3) << 14)
+#       define AFMT_AVI_INFO_TOP(x)          (((x) & 0xffff) << 16)
+#define AFMT_AVI_INFO2                       0x708c
+#       define AFMT_AVI_INFO_BOTTOM(x)       (((x) & 0xffff) << 0)
+#       define AFMT_AVI_INFO_LEFT(x)         (((x) & 0xffff) << 16)
+#define AFMT_AVI_INFO3                       0x7090
+#       define AFMT_AVI_INFO_RIGHT(x)        (((x) & 0xffff) << 0)
+#       define AFMT_AVI_INFO_VERSION(x)      (((x) & 3) << 24)
+#define AFMT_MPEG_INFO0                      0x7094
+#       define AFMT_MPEG_INFO_CHECKSUM(x)    (((x) & 0xff) << 0)
+#       define AFMT_MPEG_INFO_MB0(x)         (((x) & 0xff) << 8)
+#       define AFMT_MPEG_INFO_MB1(x)         (((x) & 0xff) << 16)
+#       define AFMT_MPEG_INFO_MB2(x)         (((x) & 0xff) << 24)
+#define AFMT_MPEG_INFO1                      0x7098
+#       define AFMT_MPEG_INFO_MB3(x)         (((x) & 0xff) << 0)
+#       define AFMT_MPEG_INFO_MF(x)          (((x) & 3) << 8)
+#       define AFMT_MPEG_INFO_FR(x)          (((x) & 1) << 12)
+#define AFMT_GENERIC0_HDR                    0x709c
+#define AFMT_GENERIC0_0                      0x70a0
+#define AFMT_GENERIC0_1                      0x70a4
+#define AFMT_GENERIC0_2                      0x70a8
+#define AFMT_GENERIC0_3                      0x70ac
+#define AFMT_GENERIC0_4                      0x70b0
+#define AFMT_GENERIC0_5                      0x70b4
+#define AFMT_GENERIC0_6                      0x70b8
+#define AFMT_GENERIC1_HDR                    0x70bc
+#define AFMT_GENERIC1_0                      0x70c0
+#define AFMT_GENERIC1_1                      0x70c4
+#define AFMT_GENERIC1_2                      0x70c8
+#define AFMT_GENERIC1_3                      0x70cc
+#define AFMT_GENERIC1_4                      0x70d0
+#define AFMT_GENERIC1_5                      0x70d4
+#define AFMT_GENERIC1_6                      0x70d8
+#define HDMI_ACR_32_0                        0x70dc
+#       define HDMI_ACR_CTS_32(x)            (((x) & 0xfffff) << 12)
+#define HDMI_ACR_32_1                        0x70e0
+#       define HDMI_ACR_N_32(x)              (((x) & 0xfffff) << 0)
+#define HDMI_ACR_44_0                        0x70e4
+#       define HDMI_ACR_CTS_44(x)            (((x) & 0xfffff) << 12)
+#define HDMI_ACR_44_1                        0x70e8
+#       define HDMI_ACR_N_44(x)              (((x) & 0xfffff) << 0)
+#define HDMI_ACR_48_0                        0x70ec
+#       define HDMI_ACR_CTS_48(x)            (((x) & 0xfffff) << 12)
+#define HDMI_ACR_48_1                        0x70f0
+#       define HDMI_ACR_N_48(x)              (((x) & 0xfffff) << 0)
+#define HDMI_ACR_STATUS_0                    0x70f4
+#define HDMI_ACR_STATUS_1                    0x70f8
+#define AFMT_AUDIO_INFO0                     0x70fc
+#       define AFMT_AUDIO_INFO_CHECKSUM(x)   (((x) & 0xff) << 0)
+#       define AFMT_AUDIO_INFO_CC(x)         (((x) & 7) << 8)
+#       define AFMT_AUDIO_INFO_CT(x)         (((x) & 0xf) << 11)
+#       define AFMT_AUDIO_INFO_CHECKSUM_OFFSET(x)   (((x) & 0xff) << 16)
+#       define AFMT_AUDIO_INFO_CXT(x)        (((x) & 0x1f) << 24)
+#define AFMT_AUDIO_INFO1                     0x7100
+#       define AFMT_AUDIO_INFO_CA(x)         (((x) & 0xff) << 0)
+#       define AFMT_AUDIO_INFO_LSV(x)        (((x) & 0xf) << 11)
+#       define AFMT_AUDIO_INFO_DM_INH(x)     (((x) & 1) << 15)
+#       define AFMT_AUDIO_INFO_DM_INH_LSV(x) (((x) & 0xff) << 8)
+#       define AFMT_AUDIO_INFO_LFEBPL(x)     (((x) & 3) << 16)
+#define AFMT_60958_0                         0x7104
+#       define AFMT_60958_CS_A(x)            (((x) & 1) << 0)
+#       define AFMT_60958_CS_B(x)            (((x) & 1) << 1)
+#       define AFMT_60958_CS_C(x)            (((x) & 1) << 2)
+#       define AFMT_60958_CS_D(x)            (((x) & 3) << 3)
+#       define AFMT_60958_CS_MODE(x)         (((x) & 3) << 6)
+#       define AFMT_60958_CS_CATEGORY_CODE(x)      (((x) & 0xff) << 8)
+#       define AFMT_60958_CS_SOURCE_NUMBER(x)      (((x) & 0xf) << 16)
+#       define AFMT_60958_CS_CHANNEL_NUMBER_L(x)   (((x) & 0xf) << 20)
+#       define AFMT_60958_CS_SAMPLING_FREQUENCY(x) (((x) & 0xf) << 24)
+#       define AFMT_60958_CS_CLOCK_ACCURACY(x)     (((x) & 3) << 28)
+#define AFMT_60958_1                         0x7108
+#       define AFMT_60958_CS_WORD_LENGTH(x)  (((x) & 0xf) << 0)
+#       define AFMT_60958_CS_ORIGINAL_SAMPLING_FREQUENCY(x)   (((x) & 0xf) << 4)
+#       define AFMT_60958_CS_VALID_L(x)      (((x) & 1) << 16)
+#       define AFMT_60958_CS_VALID_R(x)      (((x) & 1) << 18)
+#       define AFMT_60958_CS_CHANNEL_NUMBER_R(x)   (((x) & 0xf) << 20)
+#define AFMT_AUDIO_CRC_CONTROL               0x710c
+#       define AFMT_AUDIO_CRC_EN             (1 << 0)
+#define AFMT_RAMP_CONTROL0                   0x7110
+#       define AFMT_RAMP_MAX_COUNT(x)        (((x) & 0xffffff) << 0)
+#       define AFMT_RAMP_DATA_SIGN           (1 << 31)
+#define AFMT_RAMP_CONTROL1                   0x7114
+#       define AFMT_RAMP_MIN_COUNT(x)        (((x) & 0xffffff) << 0)
+#       define AFMT_AUDIO_TEST_CH_DISABLE(x) (((x) & 0xff) << 24)
+#define AFMT_RAMP_CONTROL2                   0x7118
+#       define AFMT_RAMP_INC_COUNT(x)        (((x) & 0xffffff) << 0)
+#define AFMT_RAMP_CONTROL3                   0x711c
+#       define AFMT_RAMP_DEC_COUNT(x)        (((x) & 0xffffff) << 0)
+#define AFMT_60958_2                         0x7120
+#       define AFMT_60958_CS_CHANNEL_NUMBER_2(x)   (((x) & 0xf) << 0)
+#       define AFMT_60958_CS_CHANNEL_NUMBER_3(x)   (((x) & 0xf) << 4)
+#       define AFMT_60958_CS_CHANNEL_NUMBER_4(x)   (((x) & 0xf) << 8)
+#       define AFMT_60958_CS_CHANNEL_NUMBER_5(x)   (((x) & 0xf) << 12)
+#       define AFMT_60958_CS_CHANNEL_NUMBER_6(x)   (((x) & 0xf) << 16)
+#       define AFMT_60958_CS_CHANNEL_NUMBER_7(x)   (((x) & 0xf) << 20)
+#define AFMT_STATUS                          0x7128
+#       define AFMT_AUDIO_ENABLE             (1 << 4)
+#       define AFMT_AUDIO_HBR_ENABLE         (1 << 8)
+#       define AFMT_AZ_FORMAT_WTRIG          (1 << 28)
+#       define AFMT_AZ_FORMAT_WTRIG_INT      (1 << 29)
+#       define AFMT_AZ_AUDIO_ENABLE_CHG      (1 << 30)
+#define AFMT_AUDIO_PACKET_CONTROL            0x712c
+#       define AFMT_AUDIO_SAMPLE_SEND        (1 << 0)
+#       define AFMT_RESET_FIFO_WHEN_AUDIO_DIS (1 << 11) /* set to 1 */
+#       define AFMT_AUDIO_TEST_EN            (1 << 12)
+#       define AFMT_AUDIO_CHANNEL_SWAP       (1 << 24)
+#       define AFMT_60958_CS_UPDATE          (1 << 26)
+#       define AFMT_AZ_AUDIO_ENABLE_CHG_MASK (1 << 27)
+#       define AFMT_AZ_FORMAT_WTRIG_MASK     (1 << 28)
+#       define AFMT_AZ_FORMAT_WTRIG_ACK      (1 << 29)
+#       define AFMT_AZ_AUDIO_ENABLE_CHG_ACK  (1 << 30)
+#define AFMT_VBI_PACKET_CONTROL              0x7130
+#       define AFMT_GENERIC0_UPDATE          (1 << 2)
+#define AFMT_INFOFRAME_CONTROL0              0x7134
+#       define AFMT_AUDIO_INFO_SOURCE        (1 << 6) /* 0 - sound block; 1 - afmt regs */
+#       define AFMT_AUDIO_INFO_UPDATE        (1 << 7)
+#       define AFMT_MPEG_INFO_UPDATE         (1 << 10)
+#define AFMT_GENERIC0_7                      0x7138
 
 #define	GC_USER_SHADER_PIPE_CONFIG			0x8954
 #define		INACTIVE_QD_PIPES(x)				((x) << 8)
diff --git a/drivers/gpu/drm/radeon/ni.c b/drivers/gpu/drm/radeon/ni.c
index a48ca53fcd6..b01c2dd627b 100644
--- a/drivers/gpu/drm/radeon/ni.c
+++ b/drivers/gpu/drm/radeon/ni.c
@@ -1392,35 +1392,6 @@ int cayman_cp_resume(struct radeon_device *rdev)
 	return 0;
 }
 
-bool cayman_gpu_is_lockup(struct radeon_device *rdev, struct radeon_ring *ring)
-{
-	u32 srbm_status;
-	u32 grbm_status;
-	u32 grbm_status_se0, grbm_status_se1;
-	struct r100_gpu_lockup *lockup = &rdev->config.cayman.lockup;
-	int r;
-
-	srbm_status = RREG32(SRBM_STATUS);
-	grbm_status = RREG32(GRBM_STATUS);
-	grbm_status_se0 = RREG32(GRBM_STATUS_SE0);
-	grbm_status_se1 = RREG32(GRBM_STATUS_SE1);
-	if (!(grbm_status & GUI_ACTIVE)) {
-		r100_gpu_lockup_update(lockup, ring);
-		return false;
-	}
-	/* force CP activities */
-	r = radeon_ring_lock(rdev, ring, 2);
-	if (!r) {
-		/* PACKET2 NOP */
-		radeon_ring_write(ring, 0x80000000);
-		radeon_ring_write(ring, 0x80000000);
-		radeon_ring_unlock_commit(rdev, ring);
-	}
-	/* XXX deal with CP0,1,2 */
-	ring->rptr = RREG32(ring->rptr_reg);
-	return r100_gpu_cp_is_lockup(rdev, lockup, ring);
-}
-
 static int cayman_gpu_soft_reset(struct radeon_device *rdev)
 {
 	struct evergreen_mc_save save;
@@ -1601,12 +1572,9 @@ static int cayman_startup(struct radeon_device *rdev)
 	if (r)
 		return r;
 
-	r = radeon_ib_test(rdev, RADEON_RING_TYPE_GFX_INDEX, &rdev->ring[RADEON_RING_TYPE_GFX_INDEX]);
-	if (r) {
-		DRM_ERROR("radeon: failed testing IB (%d).\n", r);
-		rdev->accel_working = false;
+	r = radeon_ib_ring_tests(rdev);
+	if (r)
 		return r;
-	}
 
 	r = radeon_vm_manager_start(rdev);
 	if (r)
@@ -1661,10 +1629,6 @@ int cayman_init(struct radeon_device *rdev)
 	struct radeon_ring *ring = &rdev->ring[RADEON_RING_TYPE_GFX_INDEX];
 	int r;
 
-	/* This don't do much */
-	r = radeon_gem_init(rdev);
-	if (r)
-		return r;
 	/* Read BIOS */
 	if (!radeon_get_bios(rdev)) {
 		if (ASIC_IS_AVIVO(rdev))
@@ -1776,7 +1740,6 @@ void cayman_fini(struct radeon_device *rdev)
 	cayman_pcie_gart_fini(rdev);
 	r600_vram_scratch_fini(rdev);
 	radeon_gem_fini(rdev);
-	radeon_semaphore_driver_fini(rdev);
 	radeon_fence_driver_fini(rdev);
 	radeon_bo_fini(rdev);
 	radeon_atombios_fini(rdev);
diff --git a/drivers/gpu/drm/radeon/r100.c b/drivers/gpu/drm/radeon/r100.c
index fe33d35dae8..fb44e7e4908 100644
--- a/drivers/gpu/drm/radeon/r100.c
+++ b/drivers/gpu/drm/radeon/r100.c
@@ -139,9 +139,9 @@ int r100_reloc_pitch_offset(struct radeon_cs_parser *p,
 		}
 
 		tmp |= tile_flags;
-		p->ib->ptr[idx] = (value & 0x3fc00000) | tmp;
+		p->ib.ptr[idx] = (value & 0x3fc00000) | tmp;
 	} else
-		p->ib->ptr[idx] = (value & 0xffc00000) | tmp;
+		p->ib.ptr[idx] = (value & 0xffc00000) | tmp;
 	return 0;
 }
 
@@ -156,7 +156,7 @@ int r100_packet3_load_vbpntr(struct radeon_cs_parser *p,
 	volatile uint32_t *ib;
 	u32 idx_value;
 
-	ib = p->ib->ptr;
+	ib = p->ib.ptr;
 	track = (struct r100_cs_track *)p->track;
 	c = radeon_get_ib_value(p, idx++) & 0x1F;
 	if (c > 16) {
@@ -660,7 +660,7 @@ int r100_pci_gart_enable(struct radeon_device *rdev)
 	tmp = RREG32(RADEON_AIC_CNTL) | RADEON_PCIGART_TRANSLATE_EN;
 	WREG32(RADEON_AIC_CNTL, tmp);
 	r100_pci_gart_tlb_flush(rdev);
-	DRM_INFO("PCIE GART of %uM enabled (table at 0x%016llX).\n",
+	DRM_INFO("PCI GART of %uM enabled (table at 0x%016llX).\n",
 		 (unsigned)(rdev->mc.gtt_size >> 20),
 		 (unsigned long long)rdev->gart.table_addr);
 	rdev->gart.ready = true;
@@ -1180,6 +1180,10 @@ int r100_cp_init(struct radeon_device *rdev, unsigned ring_size)
 	WREG32(RADEON_CP_RB_WPTR_DELAY, 0);
 	WREG32(RADEON_CP_CSQ_MODE, 0x00004D4D);
 	WREG32(RADEON_CP_CSQ_CNTL, RADEON_CSQ_PRIBM_INDBM);
+
+	/* at this point everything should be setup correctly to enable master */
+	pci_set_master(rdev->pdev);
+
 	radeon_ring_start(rdev, RADEON_RING_TYPE_GFX_INDEX, &rdev->ring[RADEON_RING_TYPE_GFX_INDEX]);
 	r = radeon_ring_test(rdev, RADEON_RING_TYPE_GFX_INDEX, ring);
 	if (r) {
@@ -1271,7 +1275,7 @@ void r100_cs_dump_packet(struct radeon_cs_parser *p,
 	unsigned i;
 	unsigned idx;
 
-	ib = p->ib->ptr;
+	ib = p->ib.ptr;
 	idx = pkt->idx;
 	for (i = 0; i <= (pkt->count + 1); i++, idx++) {
 		DRM_INFO("ib[%d]=0x%08X\n", idx, ib[idx]);
@@ -1350,7 +1354,7 @@ int r100_cs_packet_parse_vline(struct radeon_cs_parser *p)
 	uint32_t header, h_idx, reg;
 	volatile uint32_t *ib;
 
-	ib = p->ib->ptr;
+	ib = p->ib.ptr;
 
 	/* parse the wait until */
 	r = r100_cs_packet_parse(p, &waitreloc, p->idx);
@@ -1529,7 +1533,7 @@ static int r100_packet0_check(struct radeon_cs_parser *p,
 	u32 tile_flags = 0;
 	u32 idx_value;
 
-	ib = p->ib->ptr;
+	ib = p->ib.ptr;
 	track = (struct r100_cs_track *)p->track;
 
 	idx_value = radeon_get_ib_value(p, idx);
@@ -1885,7 +1889,7 @@ static int r100_packet3_check(struct radeon_cs_parser *p,
 	volatile uint32_t *ib;
 	int r;
 
-	ib = p->ib->ptr;
+	ib = p->ib.ptr;
 	idx = pkt->idx + 1;
 	track = (struct r100_cs_track *)p->track;
 	switch (pkt->opcode) {
@@ -2004,6 +2008,8 @@ int r100_cs_parse(struct radeon_cs_parser *p)
 	int r;
 
 	track = kzalloc(sizeof(*track), GFP_KERNEL);
+	if (!track)
+		return -ENOMEM;
 	r100_cs_track_clear(p->rdev, track);
 	p->track = track;
 	do {
@@ -2155,79 +2161,18 @@ int r100_mc_wait_for_idle(struct radeon_device *rdev)
 	return -1;
 }
 
-void r100_gpu_lockup_update(struct r100_gpu_lockup *lockup, struct radeon_ring *ring)
-{
-	lockup->last_cp_rptr = ring->rptr;
-	lockup->last_jiffies = jiffies;
-}
-
-/**
- * r100_gpu_cp_is_lockup() - check if CP is lockup by recording information
- * @rdev:	radeon device structure
- * @lockup:	r100_gpu_lockup structure holding CP lockup tracking informations
- * @cp:		radeon_cp structure holding CP information
- *
- * We don't need to initialize the lockup tracking information as we will either
- * have CP rptr to a different value of jiffies wrap around which will force
- * initialization of the lockup tracking informations.
- *
- * A possible false positivie is if we get call after while and last_cp_rptr ==
- * the current CP rptr, even if it's unlikely it might happen. To avoid this
- * if the elapsed time since last call is bigger than 2 second than we return
- * false and update the tracking information. Due to this the caller must call
- * r100_gpu_cp_is_lockup several time in less than 2sec for lockup to be reported
- * the fencing code should be cautious about that.
- *
- * Caller should write to the ring to force CP to do something so we don't get
- * false positive when CP is just gived nothing to do.
- *
- **/
-bool r100_gpu_cp_is_lockup(struct radeon_device *rdev, struct r100_gpu_lockup *lockup, struct radeon_ring *ring)
-{
-	unsigned long cjiffies, elapsed;
-
-	cjiffies = jiffies;
-	if (!time_after(cjiffies, lockup->last_jiffies)) {
-		/* likely a wrap around */
-		lockup->last_cp_rptr = ring->rptr;
-		lockup->last_jiffies = jiffies;
-		return false;
-	}
-	if (ring->rptr != lockup->last_cp_rptr) {
-		/* CP is still working no lockup */
-		lockup->last_cp_rptr = ring->rptr;
-		lockup->last_jiffies = jiffies;
-		return false;
-	}
-	elapsed = jiffies_to_msecs(cjiffies - lockup->last_jiffies);
-	if (elapsed >= 10000) {
-		dev_err(rdev->dev, "GPU lockup CP stall for more than %lumsec\n", elapsed);
-		return true;
-	}
-	/* give a chance to the GPU ... */
-	return false;
-}
-
 bool r100_gpu_is_lockup(struct radeon_device *rdev, struct radeon_ring *ring)
 {
 	u32 rbbm_status;
-	int r;
 
 	rbbm_status = RREG32(R_000E40_RBBM_STATUS);
 	if (!G_000E40_GUI_ACTIVE(rbbm_status)) {
-		r100_gpu_lockup_update(&rdev->config.r100.lockup, ring);
+		radeon_ring_lockup_update(ring);
 		return false;
 	}
 	/* force CP activities */
-	r = radeon_ring_lock(rdev, ring, 2);
-	if (!r) {
-		/* PACKET2 NOP */
-		radeon_ring_write(ring, 0x80000000);
-		radeon_ring_write(ring, 0x80000000);
-		radeon_ring_unlock_commit(rdev, ring);
-	}
-	ring->rptr = RREG32(ring->rptr_reg);
-	return r100_gpu_cp_is_lockup(rdev, &rdev->config.r100.lockup, ring);
+	radeon_ring_force_activity(rdev, ring);
+	return radeon_ring_test_lockup(rdev, ring);
 }
 
 void r100_bm_disable(struct radeon_device *rdev)
@@ -2296,7 +2241,6 @@ int r100_asic_reset(struct radeon_device *rdev)
 	if (G_000E40_SE_BUSY(status) || G_000E40_RE_BUSY(status) ||
 		G_000E40_TAM_BUSY(status) || G_000E40_PB_BUSY(status)) {
 		dev_err(rdev->dev, "failed to reset GPU\n");
-		rdev->gpu_lockup = true;
 		ret = -1;
 	} else
 		dev_info(rdev->dev, "GPU reset succeed\n");
@@ -3742,7 +3686,7 @@ void r100_ring_ib_execute(struct radeon_device *rdev, struct radeon_ib *ib)
 
 int r100_ib_test(struct radeon_device *rdev, struct radeon_ring *ring)
 {
-	struct radeon_ib *ib;
+	struct radeon_ib ib;
 	uint32_t scratch;
 	uint32_t tmp = 0;
 	unsigned i;
@@ -3758,22 +3702,22 @@ int r100_ib_test(struct radeon_device *rdev, struct radeon_ring *ring)
 	if (r) {
 		return r;
 	}
-	ib->ptr[0] = PACKET0(scratch, 0);
-	ib->ptr[1] = 0xDEADBEEF;
-	ib->ptr[2] = PACKET2(0);
-	ib->ptr[3] = PACKET2(0);
-	ib->ptr[4] = PACKET2(0);
-	ib->ptr[5] = PACKET2(0);
-	ib->ptr[6] = PACKET2(0);
-	ib->ptr[7] = PACKET2(0);
-	ib->length_dw = 8;
-	r = radeon_ib_schedule(rdev, ib);
+	ib.ptr[0] = PACKET0(scratch, 0);
+	ib.ptr[1] = 0xDEADBEEF;
+	ib.ptr[2] = PACKET2(0);
+	ib.ptr[3] = PACKET2(0);
+	ib.ptr[4] = PACKET2(0);
+	ib.ptr[5] = PACKET2(0);
+	ib.ptr[6] = PACKET2(0);
+	ib.ptr[7] = PACKET2(0);
+	ib.length_dw = 8;
+	r = radeon_ib_schedule(rdev, &ib);
 	if (r) {
 		radeon_scratch_free(rdev, scratch);
 		radeon_ib_free(rdev, &ib);
 		return r;
 	}
-	r = radeon_fence_wait(ib->fence, false);
+	r = radeon_fence_wait(ib.fence, false);
 	if (r) {
 		return r;
 	}
@@ -3965,12 +3909,9 @@ static int r100_startup(struct radeon_device *rdev)
 	if (r)
 		return r;
 
-	r = radeon_ib_test(rdev, RADEON_RING_TYPE_GFX_INDEX, &rdev->ring[RADEON_RING_TYPE_GFX_INDEX]);
-	if (r) {
-		dev_err(rdev->dev, "failed testing IB (%d).\n", r);
-		rdev->accel_working = false;
+	r = radeon_ib_ring_tests(rdev);
+	if (r)
 		return r;
-	}
 
 	return 0;
 }
diff --git a/drivers/gpu/drm/radeon/r200.c b/drivers/gpu/drm/radeon/r200.c
index a59cc474d53..a26144d0120 100644
--- a/drivers/gpu/drm/radeon/r200.c
+++ b/drivers/gpu/drm/radeon/r200.c
@@ -154,7 +154,7 @@ int r200_packet0_check(struct radeon_cs_parser *p,
 	u32 tile_flags = 0;
 	u32 idx_value;
 
-	ib = p->ib->ptr;
+	ib = p->ib.ptr;
 	track = (struct r100_cs_track *)p->track;
 	idx_value = radeon_get_ib_value(p, idx);
 	switch (reg) {
diff --git a/drivers/gpu/drm/radeon/r300.c b/drivers/gpu/drm/radeon/r300.c
index fa14383f9ca..97722a33e51 100644
--- a/drivers/gpu/drm/radeon/r300.c
+++ b/drivers/gpu/drm/radeon/r300.c
@@ -377,28 +377,6 @@ void r300_gpu_init(struct radeon_device *rdev)
 		 rdev->num_gb_pipes, rdev->num_z_pipes);
 }
 
-bool r300_gpu_is_lockup(struct radeon_device *rdev, struct radeon_ring *ring)
-{
-	u32 rbbm_status;
-	int r;
-
-	rbbm_status = RREG32(R_000E40_RBBM_STATUS);
-	if (!G_000E40_GUI_ACTIVE(rbbm_status)) {
-		r100_gpu_lockup_update(&rdev->config.r300.lockup, ring);
-		return false;
-	}
-	/* force CP activities */
-	r = radeon_ring_lock(rdev, ring, 2);
-	if (!r) {
-		/* PACKET2 NOP */
-		radeon_ring_write(ring, 0x80000000);
-		radeon_ring_write(ring, 0x80000000);
-		radeon_ring_unlock_commit(rdev, ring);
-	}
-	ring->rptr = RREG32(RADEON_CP_RB_RPTR);
-	return r100_gpu_cp_is_lockup(rdev, &rdev->config.r300.lockup, ring);
-}
-
 int r300_asic_reset(struct radeon_device *rdev)
 {
 	struct r100_mc_save save;
@@ -449,7 +427,6 @@ int r300_asic_reset(struct radeon_device *rdev)
 	/* Check if GPU is idle */
 	if (G_000E40_GA_BUSY(status) || G_000E40_VAP_BUSY(status)) {
 		dev_err(rdev->dev, "failed to reset GPU\n");
-		rdev->gpu_lockup = true;
 		ret = -1;
 	} else
 		dev_info(rdev->dev, "GPU reset succeed\n");
@@ -627,7 +604,7 @@ static int r300_packet0_check(struct radeon_cs_parser *p,
 	int r;
 	u32 idx_value;
 
-	ib = p->ib->ptr;
+	ib = p->ib.ptr;
 	track = (struct r100_cs_track *)p->track;
 	idx_value = radeon_get_ib_value(p, idx);
 
@@ -1169,7 +1146,7 @@ static int r300_packet3_check(struct radeon_cs_parser *p,
 	unsigned idx;
 	int r;
 
-	ib = p->ib->ptr;
+	ib = p->ib.ptr;
 	idx = pkt->idx + 1;
 	track = (struct r100_cs_track *)p->track;
 	switch(pkt->opcode) {
@@ -1418,12 +1395,9 @@ static int r300_startup(struct radeon_device *rdev)
 	if (r)
 		return r;
 
-	r = radeon_ib_test(rdev, RADEON_RING_TYPE_GFX_INDEX, &rdev->ring[RADEON_RING_TYPE_GFX_INDEX]);
-	if (r) {
-		dev_err(rdev->dev, "failed testing IB (%d).\n", r);
-		rdev->accel_working = false;
+	r = radeon_ib_ring_tests(rdev);
+	if (r)
 		return r;
-	}
 
 	return 0;
 }
diff --git a/drivers/gpu/drm/radeon/r420.c b/drivers/gpu/drm/radeon/r420.c
index f3fcaacfea0..99137be7a30 100644
--- a/drivers/gpu/drm/radeon/r420.c
+++ b/drivers/gpu/drm/radeon/r420.c
@@ -279,12 +279,9 @@ static int r420_startup(struct radeon_device *rdev)
 	if (r)
 		return r;
 
-	r = radeon_ib_test(rdev, RADEON_RING_TYPE_GFX_INDEX, &rdev->ring[RADEON_RING_TYPE_GFX_INDEX]);
-	if (r) {
-		dev_err(rdev->dev, "failed testing IB (%d).\n", r);
-		rdev->accel_working = false;
+	r = radeon_ib_ring_tests(rdev);
+	if (r)
 		return r;
-	}
 
 	return 0;
 }
diff --git a/drivers/gpu/drm/radeon/r520.c b/drivers/gpu/drm/radeon/r520.c
index ebcc15b03c9..b5cf8375cd2 100644
--- a/drivers/gpu/drm/radeon/r520.c
+++ b/drivers/gpu/drm/radeon/r520.c
@@ -207,12 +207,10 @@ static int r520_startup(struct radeon_device *rdev)
 	if (r)
 		return r;
 
-	r = radeon_ib_test(rdev, RADEON_RING_TYPE_GFX_INDEX, &rdev->ring[RADEON_RING_TYPE_GFX_INDEX]);
-	if (r) {
-		dev_err(rdev->dev, "failed testing IB (%d).\n", r);
-		rdev->accel_working = false;
+	r = radeon_ib_ring_tests(rdev);
+	if (r)
 		return r;
-	}
+
 	return 0;
 }
 
diff --git a/drivers/gpu/drm/radeon/r600.c b/drivers/gpu/drm/radeon/r600.c
index c8187c4b6ae..f388a1d73b6 100644
--- a/drivers/gpu/drm/radeon/r600.c
+++ b/drivers/gpu/drm/radeon/r600.c
@@ -713,6 +713,14 @@ void r600_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
+			 */
+			continue;
+		}
 		if (ASIC_IS_DCE3(rdev)) {
 			u32 tmp = DC_HPDx_CONNECTION_TIMER(0x9c4) | DC_HPDx_RX_INT_TIMER(0xfa);
 			if (ASIC_IS_DCE32(rdev))
@@ -1223,7 +1231,7 @@ int r600_vram_scratch_init(struct radeon_device *rdev)
 	if (rdev->vram_scratch.robj == NULL) {
 		r = radeon_bo_create(rdev, RADEON_GPU_PAGE_SIZE,
 				     PAGE_SIZE, true, RADEON_GEM_DOMAIN_VRAM,
-				     &rdev->vram_scratch.robj);
+				     NULL, &rdev->vram_scratch.robj);
 		if (r) {
 			return r;
 		}
@@ -1350,31 +1358,17 @@ bool r600_gpu_is_lockup(struct radeon_device *rdev, struct radeon_ring *ring)
 	u32 srbm_status;
 	u32 grbm_status;
 	u32 grbm_status2;
-	struct r100_gpu_lockup *lockup;
-	int r;
-
-	if (rdev->family >= CHIP_RV770)
-		lockup = &rdev->config.rv770.lockup;
-	else
-		lockup = &rdev->config.r600.lockup;
 
 	srbm_status = RREG32(R_000E50_SRBM_STATUS);
 	grbm_status = RREG32(R_008010_GRBM_STATUS);
 	grbm_status2 = RREG32(R_008014_GRBM_STATUS2);
 	if (!G_008010_GUI_ACTIVE(grbm_status)) {
-		r100_gpu_lockup_update(lockup, ring);
+		radeon_ring_lockup_update(ring);
 		return false;
 	}
 	/* force CP activities */
-	r = radeon_ring_lock(rdev, ring, 2);
-	if (!r) {
-		/* PACKET2 NOP */
-		radeon_ring_write(ring, 0x80000000);
-		radeon_ring_write(ring, 0x80000000);
-		radeon_ring_unlock_commit(rdev, ring);
-	}
-	ring->rptr = RREG32(ring->rptr_reg);
-	return r100_gpu_cp_is_lockup(rdev, lockup, ring);
+	radeon_ring_force_activity(rdev, ring);
+	return radeon_ring_test_lockup(rdev, ring);
 }
 
 int r600_asic_reset(struct radeon_device *rdev)
@@ -2377,20 +2371,15 @@ int r600_copy_blit(struct radeon_device *rdev,
 		   unsigned num_gpu_pages,
 		   struct radeon_fence *fence)
 {
+	struct radeon_sa_bo *vb = NULL;
 	int r;
 
-	mutex_lock(&rdev->r600_blit.mutex);
-	rdev->r600_blit.vb_ib = NULL;
-	r = r600_blit_prepare_copy(rdev, num_gpu_pages);
+	r = r600_blit_prepare_copy(rdev, num_gpu_pages, &vb);
 	if (r) {
-		if (rdev->r600_blit.vb_ib)
-			radeon_ib_free(rdev, &rdev->r600_blit.vb_ib);
-		mutex_unlock(&rdev->r600_blit.mutex);
 		return r;
 	}
-	r600_kms_blit_copy(rdev, src_offset, dst_offset, num_gpu_pages);
-	r600_blit_done_copy(rdev, fence);
-	mutex_unlock(&rdev->r600_blit.mutex);
+	r600_kms_blit_copy(rdev, src_offset, dst_offset, num_gpu_pages, vb);
+	r600_blit_done_copy(rdev, fence, vb);
 	return 0;
 }
 
@@ -2494,12 +2483,9 @@ int r600_startup(struct radeon_device *rdev)
 	if (r)
 		return r;
 
-	r = radeon_ib_test(rdev, RADEON_RING_TYPE_GFX_INDEX, &rdev->ring[RADEON_RING_TYPE_GFX_INDEX]);
-	if (r) {
-		DRM_ERROR("radeon: failed testing IB (%d).\n", r);
-		rdev->accel_working = false;
+	r = radeon_ib_ring_tests(rdev);
+	if (r)
 		return r;
-	}
 
 	return 0;
 }
@@ -2574,10 +2560,6 @@ int r600_init(struct radeon_device *rdev)
 	if (r600_debugfs_mc_info_init(rdev)) {
 		DRM_ERROR("Failed to register debugfs file for mc !\n");
 	}
-	/* This don't do much */
-	r = radeon_gem_init(rdev);
-	if (r)
-		return r;
 	/* Read BIOS */
 	if (!radeon_get_bios(rdev)) {
 		if (ASIC_IS_AVIVO(rdev))
@@ -2675,7 +2657,6 @@ void r600_fini(struct radeon_device *rdev)
 	r600_vram_scratch_fini(rdev);
 	radeon_agp_fini(rdev);
 	radeon_gem_fini(rdev);
-	radeon_semaphore_driver_fini(rdev);
 	radeon_fence_driver_fini(rdev);
 	radeon_bo_fini(rdev);
 	radeon_atombios_fini(rdev);
@@ -2704,7 +2685,7 @@ void r600_ring_ib_execute(struct radeon_device *rdev, struct radeon_ib *ib)
 
 int r600_ib_test(struct radeon_device *rdev, struct radeon_ring *ring)
 {
-	struct radeon_ib *ib;
+	struct radeon_ib ib;
 	uint32_t scratch;
 	uint32_t tmp = 0;
 	unsigned i;
@@ -2722,18 +2703,18 @@ int r600_ib_test(struct radeon_device *rdev, struct radeon_ring *ring)
 		DRM_ERROR("radeon: failed to get ib (%d).\n", r);
 		return r;
 	}
-	ib->ptr[0] = PACKET3(PACKET3_SET_CONFIG_REG, 1);
-	ib->ptr[1] = ((scratch - PACKET3_SET_CONFIG_REG_OFFSET) >> 2);
-	ib->ptr[2] = 0xDEADBEEF;
-	ib->length_dw = 3;
-	r = radeon_ib_schedule(rdev, ib);
+	ib.ptr[0] = PACKET3(PACKET3_SET_CONFIG_REG, 1);
+	ib.ptr[1] = ((scratch - PACKET3_SET_CONFIG_REG_OFFSET) >> 2);
+	ib.ptr[2] = 0xDEADBEEF;
+	ib.length_dw = 3;
+	r = radeon_ib_schedule(rdev, &ib);
 	if (r) {
 		radeon_scratch_free(rdev, scratch);
 		radeon_ib_free(rdev, &ib);
 		DRM_ERROR("radeon: failed to schedule ib (%d).\n", r);
 		return r;
 	}
-	r = radeon_fence_wait(ib->fence, false);
+	r = radeon_fence_wait(ib.fence, false);
 	if (r) {
 		DRM_ERROR("radeon: fence wait failed (%d).\n", r);
 		return r;
@@ -2745,7 +2726,7 @@ int r600_ib_test(struct radeon_device *rdev, struct radeon_ring *ring)
 		DRM_UDELAY(1);
 	}
 	if (i < rdev->usec_timeout) {
-		DRM_INFO("ib test on ring %d succeeded in %u usecs\n", ib->fence->ring, i);
+		DRM_INFO("ib test on ring %d succeeded in %u usecs\n", ib.fence->ring, i);
 	} else {
 		DRM_ERROR("radeon: ib test failed (scratch(0x%04X)=0x%08X)\n",
 			  scratch, tmp);
@@ -2788,7 +2769,7 @@ int r600_ih_ring_alloc(struct radeon_device *rdev)
 		r = radeon_bo_create(rdev, rdev->ih.ring_size,
 				     PAGE_SIZE, true,
 				     RADEON_GEM_DOMAIN_GTT,
-				     &rdev->ih.ring_obj);
+				     NULL, &rdev->ih.ring_obj);
 		if (r) {
 			DRM_ERROR("radeon: failed to create ih ring buffer (%d).\n", r);
 			return r;
@@ -2968,6 +2949,15 @@ static void r600_disable_interrupt_state(struct radeon_device *rdev)
 			WREG32(DC_HPD5_INT_CONTROL, tmp);
 			tmp = RREG32(DC_HPD6_INT_CONTROL) & DC_HPDx_INT_POLARITY;
 			WREG32(DC_HPD6_INT_CONTROL, tmp);
+			tmp = RREG32(AFMT_AUDIO_PACKET_CONTROL + DCE3_HDMI_OFFSET0) & ~HDMI0_AZ_FORMAT_WTRIG_MASK;
+			WREG32(AFMT_AUDIO_PACKET_CONTROL + DCE3_HDMI_OFFSET0, tmp);
+			tmp = RREG32(AFMT_AUDIO_PACKET_CONTROL + DCE3_HDMI_OFFSET1) & ~HDMI0_AZ_FORMAT_WTRIG_MASK;
+			WREG32(AFMT_AUDIO_PACKET_CONTROL + DCE3_HDMI_OFFSET1, tmp);
+		} else {
+			tmp = RREG32(HDMI0_AUDIO_PACKET_CONTROL) & ~HDMI0_AZ_FORMAT_WTRIG_MASK;
+			WREG32(HDMI0_AUDIO_PACKET_CONTROL, tmp);
+			tmp = RREG32(DCE3_HDMI1_AUDIO_PACKET_CONTROL) & ~HDMI0_AZ_FORMAT_WTRIG_MASK;
+			WREG32(DCE3_HDMI1_AUDIO_PACKET_CONTROL, tmp);
 		}
 	} else {
 		WREG32(DACA_AUTODETECT_INT_CONTROL, 0);
@@ -2978,6 +2968,10 @@ static void r600_disable_interrupt_state(struct radeon_device *rdev)
 		WREG32(DC_HOT_PLUG_DETECT2_INT_CONTROL, tmp);
 		tmp = RREG32(DC_HOT_PLUG_DETECT3_INT_CONTROL) & DC_HOT_PLUG_DETECTx_INT_POLARITY;
 		WREG32(DC_HOT_PLUG_DETECT3_INT_CONTROL, tmp);
+		tmp = RREG32(HDMI0_AUDIO_PACKET_CONTROL) & ~HDMI0_AZ_FORMAT_WTRIG_MASK;
+		WREG32(HDMI0_AUDIO_PACKET_CONTROL, tmp);
+		tmp = RREG32(HDMI1_AUDIO_PACKET_CONTROL) & ~HDMI0_AZ_FORMAT_WTRIG_MASK;
+		WREG32(HDMI1_AUDIO_PACKET_CONTROL, tmp);
 	}
 }
 
@@ -3047,6 +3041,9 @@ int r600_irq_init(struct radeon_device *rdev)
 	else
 		r600_disable_interrupt_state(rdev);
 
+	/* at this point everything should be setup correctly to enable master */
+	pci_set_master(rdev->pdev);
+
 	/* enable irqs */
 	r600_enable_interrupts(rdev);
 
@@ -3071,7 +3068,7 @@ int r600_irq_set(struct radeon_device *rdev)
 	u32 mode_int = 0;
 	u32 hpd1, hpd2, hpd3, hpd4 = 0, hpd5 = 0, hpd6 = 0;
 	u32 grbm_int_cntl = 0;
-	u32 hdmi1, hdmi2;
+	u32 hdmi0, hdmi1;
 	u32 d1grph = 0, d2grph = 0;
 
 	if (!rdev->irq.installed) {
@@ -3086,9 +3083,7 @@ int r600_irq_set(struct radeon_device *rdev)
 		return 0;
 	}
 
-	hdmi1 = RREG32(R600_HDMI_BLOCK1 + R600_HDMI_CNTL) & ~R600_HDMI_INT_EN;
 	if (ASIC_IS_DCE3(rdev)) {
-		hdmi2 = RREG32(R600_HDMI_BLOCK3 + R600_HDMI_CNTL) & ~R600_HDMI_INT_EN;
 		hpd1 = RREG32(DC_HPD1_INT_CONTROL) & ~DC_HPDx_INT_EN;
 		hpd2 = RREG32(DC_HPD2_INT_CONTROL) & ~DC_HPDx_INT_EN;
 		hpd3 = RREG32(DC_HPD3_INT_CONTROL) & ~DC_HPDx_INT_EN;
@@ -3096,12 +3091,18 @@ int r600_irq_set(struct radeon_device *rdev)
 		if (ASIC_IS_DCE32(rdev)) {
 			hpd5 = RREG32(DC_HPD5_INT_CONTROL) & ~DC_HPDx_INT_EN;
 			hpd6 = RREG32(DC_HPD6_INT_CONTROL) & ~DC_HPDx_INT_EN;
+			hdmi0 = RREG32(AFMT_AUDIO_PACKET_CONTROL + DCE3_HDMI_OFFSET0) & ~AFMT_AZ_FORMAT_WTRIG_MASK;
+			hdmi1 = RREG32(AFMT_AUDIO_PACKET_CONTROL + DCE3_HDMI_OFFSET1) & ~AFMT_AZ_FORMAT_WTRIG_MASK;
+		} else {
+			hdmi0 = RREG32(HDMI0_AUDIO_PACKET_CONTROL) & ~HDMI0_AZ_FORMAT_WTRIG_MASK;
+			hdmi1 = RREG32(DCE3_HDMI1_AUDIO_PACKET_CONTROL) & ~HDMI0_AZ_FORMAT_WTRIG_MASK;
 		}
 	} else {
-		hdmi2 = RREG32(R600_HDMI_BLOCK2 + R600_HDMI_CNTL) & ~R600_HDMI_INT_EN;
 		hpd1 = RREG32(DC_HOT_PLUG_DETECT1_INT_CONTROL) & ~DC_HPDx_INT_EN;
 		hpd2 = RREG32(DC_HOT_PLUG_DETECT2_INT_CONTROL) & ~DC_HPDx_INT_EN;
 		hpd3 = RREG32(DC_HOT_PLUG_DETECT3_INT_CONTROL) & ~DC_HPDx_INT_EN;
+		hdmi0 = RREG32(HDMI0_AUDIO_PACKET_CONTROL) & ~HDMI0_AZ_FORMAT_WTRIG_MASK;
+		hdmi1 = RREG32(HDMI1_AUDIO_PACKET_CONTROL) & ~HDMI0_AZ_FORMAT_WTRIG_MASK;
 	}
 
 	if (rdev->irq.sw_int[RADEON_RING_TYPE_GFX_INDEX]) {
@@ -3143,13 +3144,13 @@ int r600_irq_set(struct radeon_device *rdev)
 		DRM_DEBUG("r600_irq_set: hpd 6\n");
 		hpd6 |= DC_HPDx_INT_EN;
 	}
-	if (rdev->irq.hdmi[0]) {
-		DRM_DEBUG("r600_irq_set: hdmi 1\n");
-		hdmi1 |= R600_HDMI_INT_EN;
+	if (rdev->irq.afmt[0]) {
+		DRM_DEBUG("r600_irq_set: hdmi 0\n");
+		hdmi0 |= HDMI0_AZ_FORMAT_WTRIG_MASK;
 	}
-	if (rdev->irq.hdmi[1]) {
-		DRM_DEBUG("r600_irq_set: hdmi 2\n");
-		hdmi2 |= R600_HDMI_INT_EN;
+	if (rdev->irq.afmt[1]) {
+		DRM_DEBUG("r600_irq_set: hdmi 0\n");
+		hdmi1 |= HDMI0_AZ_FORMAT_WTRIG_MASK;
 	}
 	if (rdev->irq.gui_idle) {
 		DRM_DEBUG("gui idle\n");
@@ -3161,9 +3162,7 @@ int r600_irq_set(struct radeon_device *rdev)
 	WREG32(D1GRPH_INTERRUPT_CONTROL, d1grph);
 	WREG32(D2GRPH_INTERRUPT_CONTROL, d2grph);
 	WREG32(GRBM_INT_CNTL, grbm_int_cntl);
-	WREG32(R600_HDMI_BLOCK1 + R600_HDMI_CNTL, hdmi1);
 	if (ASIC_IS_DCE3(rdev)) {
-		WREG32(R600_HDMI_BLOCK3 + R600_HDMI_CNTL, hdmi2);
 		WREG32(DC_HPD1_INT_CONTROL, hpd1);
 		WREG32(DC_HPD2_INT_CONTROL, hpd2);
 		WREG32(DC_HPD3_INT_CONTROL, hpd3);
@@ -3171,12 +3170,18 @@ int r600_irq_set(struct radeon_device *rdev)
 		if (ASIC_IS_DCE32(rdev)) {
 			WREG32(DC_HPD5_INT_CONTROL, hpd5);
 			WREG32(DC_HPD6_INT_CONTROL, hpd6);
+			WREG32(AFMT_AUDIO_PACKET_CONTROL + DCE3_HDMI_OFFSET0, hdmi0);
+			WREG32(AFMT_AUDIO_PACKET_CONTROL + DCE3_HDMI_OFFSET1, hdmi1);
+		} else {
+			WREG32(HDMI0_AUDIO_PACKET_CONTROL, hdmi0);
+			WREG32(DCE3_HDMI1_AUDIO_PACKET_CONTROL, hdmi1);
 		}
 	} else {
-		WREG32(R600_HDMI_BLOCK2 + R600_HDMI_CNTL, hdmi2);
 		WREG32(DC_HOT_PLUG_DETECT1_INT_CONTROL, hpd1);
 		WREG32(DC_HOT_PLUG_DETECT2_INT_CONTROL, hpd2);
 		WREG32(DC_HOT_PLUG_DETECT3_INT_CONTROL, hpd3);
+		WREG32(HDMI0_AUDIO_PACKET_CONTROL, hdmi0);
+		WREG32(HDMI1_AUDIO_PACKET_CONTROL, hdmi1);
 	}
 
 	return 0;
@@ -3190,10 +3195,19 @@ static void r600_irq_ack(struct radeon_device *rdev)
 		rdev->irq.stat_regs.r600.disp_int = RREG32(DCE3_DISP_INTERRUPT_STATUS);
 		rdev->irq.stat_regs.r600.disp_int_cont = RREG32(DCE3_DISP_INTERRUPT_STATUS_CONTINUE);
 		rdev->irq.stat_regs.r600.disp_int_cont2 = RREG32(DCE3_DISP_INTERRUPT_STATUS_CONTINUE2);
+		if (ASIC_IS_DCE32(rdev)) {
+			rdev->irq.stat_regs.r600.hdmi0_status = RREG32(AFMT_STATUS + DCE3_HDMI_OFFSET0);
+			rdev->irq.stat_regs.r600.hdmi1_status = RREG32(AFMT_STATUS + DCE3_HDMI_OFFSET1);
+		} else {
+			rdev->irq.stat_regs.r600.hdmi0_status = RREG32(HDMI0_STATUS);
+			rdev->irq.stat_regs.r600.hdmi1_status = RREG32(DCE3_HDMI1_STATUS);
+		}
 	} else {
 		rdev->irq.stat_regs.r600.disp_int = RREG32(DISP_INTERRUPT_STATUS);
 		rdev->irq.stat_regs.r600.disp_int_cont = RREG32(DISP_INTERRUPT_STATUS_CONTINUE);
 		rdev->irq.stat_regs.r600.disp_int_cont2 = 0;
+		rdev->irq.stat_regs.r600.hdmi0_status = RREG32(HDMI0_STATUS);
+		rdev->irq.stat_regs.r600.hdmi1_status = RREG32(HDMI1_STATUS);
 	}
 	rdev->irq.stat_regs.r600.d1grph_int = RREG32(D1GRPH_INTERRUPT_STATUS);
 	rdev->irq.stat_regs.r600.d2grph_int = RREG32(D2GRPH_INTERRUPT_STATUS);
@@ -3259,17 +3273,32 @@ static void r600_irq_ack(struct radeon_device *rdev)
 			tmp |= DC_HPDx_INT_ACK;
 			WREG32(DC_HPD6_INT_CONTROL, tmp);
 		}
-	}
-	if (RREG32(R600_HDMI_BLOCK1 + R600_HDMI_STATUS) & R600_HDMI_INT_PENDING) {
-		WREG32_P(R600_HDMI_BLOCK1 + R600_HDMI_CNTL, R600_HDMI_INT_ACK, ~R600_HDMI_INT_ACK);
-	}
-	if (ASIC_IS_DCE3(rdev)) {
-		if (RREG32(R600_HDMI_BLOCK3 + R600_HDMI_STATUS) & R600_HDMI_INT_PENDING) {
-			WREG32_P(R600_HDMI_BLOCK3 + R600_HDMI_CNTL, R600_HDMI_INT_ACK, ~R600_HDMI_INT_ACK);
+		if (rdev->irq.stat_regs.r600.hdmi0_status & AFMT_AZ_FORMAT_WTRIG) {
+			tmp = RREG32(AFMT_AUDIO_PACKET_CONTROL + DCE3_HDMI_OFFSET0);
+			tmp |= AFMT_AZ_FORMAT_WTRIG_ACK;
+			WREG32(AFMT_AUDIO_PACKET_CONTROL + DCE3_HDMI_OFFSET0, tmp);
+		}
+		if (rdev->irq.stat_regs.r600.hdmi1_status & AFMT_AZ_FORMAT_WTRIG) {
+			tmp = RREG32(AFMT_AUDIO_PACKET_CONTROL + DCE3_HDMI_OFFSET1);
+			tmp |= AFMT_AZ_FORMAT_WTRIG_ACK;
+			WREG32(AFMT_AUDIO_PACKET_CONTROL + DCE3_HDMI_OFFSET1, tmp);
 		}
 	} else {
-		if (RREG32(R600_HDMI_BLOCK2 + R600_HDMI_STATUS) & R600_HDMI_INT_PENDING) {
-			WREG32_P(R600_HDMI_BLOCK2 + R600_HDMI_CNTL, R600_HDMI_INT_ACK, ~R600_HDMI_INT_ACK);
+		if (rdev->irq.stat_regs.r600.hdmi0_status & HDMI0_AZ_FORMAT_WTRIG) {
+			tmp = RREG32(HDMI0_AUDIO_PACKET_CONTROL);
+			tmp |= HDMI0_AZ_FORMAT_WTRIG_ACK;
+			WREG32(HDMI0_AUDIO_PACKET_CONTROL, tmp);
+		}
+		if (rdev->irq.stat_regs.r600.hdmi1_status & HDMI0_AZ_FORMAT_WTRIG) {
+			if (ASIC_IS_DCE3(rdev)) {
+				tmp = RREG32(DCE3_HDMI1_AUDIO_PACKET_CONTROL);
+				tmp |= HDMI0_AZ_FORMAT_WTRIG_ACK;
+				WREG32(DCE3_HDMI1_AUDIO_PACKET_CONTROL, tmp);
+			} else {
+				tmp = RREG32(HDMI1_AUDIO_PACKET_CONTROL);
+				tmp |= HDMI0_AZ_FORMAT_WTRIG_ACK;
+				WREG32(HDMI1_AUDIO_PACKET_CONTROL, tmp);
+			}
 		}
 	}
 }
@@ -3345,6 +3374,7 @@ int r600_irq_process(struct radeon_device *rdev)
 	u32 ring_index;
 	unsigned long flags;
 	bool queue_hotplug = false;
+	bool queue_hdmi = false;
 
 	if (!rdev->ih.enabled || rdev->shutdown)
 		return IRQ_NONE;
@@ -3480,9 +3510,26 @@ restart_ih:
 				break;
 			}
 			break;
-		case 21: /* HDMI */
-			DRM_DEBUG("IH: HDMI: 0x%x\n", src_data);
-			r600_audio_schedule_polling(rdev);
+		case 21: /* hdmi */
+			switch (src_data) {
+			case 4:
+				if (rdev->irq.stat_regs.r600.hdmi0_status & HDMI0_AZ_FORMAT_WTRIG) {
+					rdev->irq.stat_regs.r600.hdmi0_status &= ~HDMI0_AZ_FORMAT_WTRIG;
+					queue_hdmi = true;
+					DRM_DEBUG("IH: HDMI0\n");
+				}
+				break;
+			case 5:
+				if (rdev->irq.stat_regs.r600.hdmi1_status & HDMI0_AZ_FORMAT_WTRIG) {
+					rdev->irq.stat_regs.r600.hdmi1_status &= ~HDMI0_AZ_FORMAT_WTRIG;
+					queue_hdmi = true;
+					DRM_DEBUG("IH: HDMI1\n");
+				}
+				break;
+			default:
+				DRM_ERROR("Unhandled interrupt: %d %d\n", src_id, src_data);
+				break;
+			}
 			break;
 		case 176: /* CP_INT in ring buffer */
 		case 177: /* CP_INT in IB1 */
@@ -3514,6 +3561,8 @@ restart_ih:
 		goto restart_ih;
 	if (queue_hotplug)
 		schedule_work(&rdev->hotplug_work);
+	if (queue_hdmi)
+		schedule_work(&rdev->audio_work);
 	rdev->ih.rptr = rptr;
 	WREG32(IH_RB_RPTR, rdev->ih.rptr);
 	spin_unlock_irqrestore(&rdev->ih.lock, flags);
diff --git a/drivers/gpu/drm/radeon/r600_audio.c b/drivers/gpu/drm/radeon/r600_audio.c
index ba66f3093d4..7c4fa77f018 100644
--- a/drivers/gpu/drm/radeon/r600_audio.c
+++ b/drivers/gpu/drm/radeon/r600_audio.c
@@ -29,7 +29,28 @@
 #include "radeon_asic.h"
 #include "atom.h"
 
-#define AUDIO_TIMER_INTERVALL 100 /* 1/10 sekund should be enough */
+/*
+ * check if enc_priv stores radeon_encoder_atom_dig
+ */
+static bool radeon_dig_encoder(struct drm_encoder *encoder)
+{
+	struct radeon_encoder *radeon_encoder = to_radeon_encoder(encoder);
+	switch (radeon_encoder->encoder_id) {
+	case ENCODER_OBJECT_ID_INTERNAL_LVDS:
+	case ENCODER_OBJECT_ID_INTERNAL_TMDS1:
+	case ENCODER_OBJECT_ID_INTERNAL_KLDSCP_TMDS1:
+	case ENCODER_OBJECT_ID_INTERNAL_LVTM1:
+	case ENCODER_OBJECT_ID_INTERNAL_DVO1:
+	case ENCODER_OBJECT_ID_INTERNAL_KLDSCP_DVO1:
+	case ENCODER_OBJECT_ID_INTERNAL_DDI:
+	case ENCODER_OBJECT_ID_INTERNAL_UNIPHY:
+	case ENCODER_OBJECT_ID_INTERNAL_KLDSCP_LVTMA:
+	case ENCODER_OBJECT_ID_INTERNAL_UNIPHY1:
+	case ENCODER_OBJECT_ID_INTERNAL_UNIPHY2:
+		return true;
+	}
+	return false;
+}
 
 /*
  * check if the chipset is supported
@@ -42,118 +63,85 @@ static int r600_audio_chipset_supported(struct radeon_device *rdev)
 		|| rdev->family == CHIP_RS740;
 }
 
-/*
- * current number of channels
- */
-int r600_audio_channels(struct radeon_device *rdev)
+struct r600_audio r600_audio_status(struct radeon_device *rdev)
 {
-	return (RREG32(R600_AUDIO_RATE_BPS_CHANNEL) & 0x7) + 1;
-}
+	struct r600_audio status;
+	uint32_t value;
 
-/*
- * current bits per sample
- */
-int r600_audio_bits_per_sample(struct radeon_device *rdev)
-{
-	uint32_t value = (RREG32(R600_AUDIO_RATE_BPS_CHANNEL) & 0xF0) >> 4;
-	switch (value) {
-	case 0x0: return  8;
-	case 0x1: return 16;
-	case 0x2: return 20;
-	case 0x3: return 24;
-	case 0x4: return 32;
-	}
+	value = RREG32(R600_AUDIO_RATE_BPS_CHANNEL);
 
-	dev_err(rdev->dev, "Unknown bits per sample 0x%x using 16 instead\n",
-		(int)value);
+	/* number of channels */
+	status.channels = (value & 0x7) + 1;
 
-	return 16;
-}
-
-/*
- * current sampling rate in HZ
- */
-int r600_audio_rate(struct radeon_device *rdev)
-{
-	uint32_t value = RREG32(R600_AUDIO_RATE_BPS_CHANNEL);
-	uint32_t result;
+	/* bits per sample */
+	switch ((value & 0xF0) >> 4) {
+	case 0x0:
+		status.bits_per_sample = 8;
+		break;
+	case 0x1:
+		status.bits_per_sample = 16;
+		break;
+	case 0x2:
+		status.bits_per_sample = 20;
+		break;
+	case 0x3:
+		status.bits_per_sample = 24;
+		break;
+	case 0x4:
+		status.bits_per_sample = 32;
+		break;
+	default:
+		dev_err(rdev->dev, "Unknown bits per sample 0x%x, using 16\n",
+			(int)value);
+		status.bits_per_sample = 16;
+	}
 
+	/* current sampling rate in HZ */
 	if (value & 0x4000)
-		result = 44100;
+		status.rate = 44100;
 	else
-		result = 48000;
+		status.rate = 48000;
+	status.rate *= ((value >> 11) & 0x7) + 1;
+	status.rate /= ((value >> 8) & 0x7) + 1;
 
-	result *= ((value >> 11) & 0x7) + 1;
-	result /= ((value >> 8) & 0x7) + 1;
+	value = RREG32(R600_AUDIO_STATUS_BITS);
 
-	return result;
-}
+	/* iec 60958 status bits */
+	status.status_bits = value & 0xff;
 
-/*
- * iec 60958 status bits
- */
-uint8_t r600_audio_status_bits(struct radeon_device *rdev)
-{
-	return RREG32(R600_AUDIO_STATUS_BITS) & 0xff;
-}
+	/* iec 60958 category code */
+	status.category_code = (value >> 8) & 0xff;
 
-/*
- * iec 60958 category code
- */
-uint8_t r600_audio_category_code(struct radeon_device *rdev)
-{
-	return (RREG32(R600_AUDIO_STATUS_BITS) >> 8) & 0xff;
-}
-
-/*
- * schedule next audio update event
- */
-void r600_audio_schedule_polling(struct radeon_device *rdev)
-{
-	mod_timer(&rdev->audio_timer,
-		jiffies + msecs_to_jiffies(AUDIO_TIMER_INTERVALL));
+	return status;
 }
 
 /*
  * update all hdmi interfaces with current audio parameters
  */
-static void r600_audio_update_hdmi(unsigned long param)
+void r600_audio_update_hdmi(struct work_struct *work)
 {
-	struct radeon_device *rdev = (struct radeon_device *)param;
+	struct radeon_device *rdev = container_of(work, struct radeon_device,
+						  audio_work);
 	struct drm_device *dev = rdev->ddev;
-
-	int channels = r600_audio_channels(rdev);
-	int rate = r600_audio_rate(rdev);
-	int bps = r600_audio_bits_per_sample(rdev);
-	uint8_t status_bits = r600_audio_status_bits(rdev);
-	uint8_t category_code = r600_audio_category_code(rdev);
-
+	struct r600_audio audio_status = r600_audio_status(rdev);
 	struct drm_encoder *encoder;
-	int changes = 0, still_going = 0;
-
-	changes |= channels != rdev->audio_channels;
-	changes |= rate != rdev->audio_rate;
-	changes |= bps != rdev->audio_bits_per_sample;
-	changes |= status_bits != rdev->audio_status_bits;
-	changes |= category_code != rdev->audio_category_code;
-
-	if (changes) {
-		rdev->audio_channels = channels;
-		rdev->audio_rate = rate;
-		rdev->audio_bits_per_sample = bps;
-		rdev->audio_status_bits = status_bits;
-		rdev->audio_category_code = category_code;
+	bool changed = false;
+
+	if (rdev->audio_status.channels != audio_status.channels ||
+	    rdev->audio_status.rate != audio_status.rate ||
+	    rdev->audio_status.bits_per_sample != audio_status.bits_per_sample ||
+	    rdev->audio_status.status_bits != audio_status.status_bits ||
+	    rdev->audio_status.category_code != audio_status.category_code) {
+		rdev->audio_status = audio_status;
+		changed = true;
 	}
 
 	list_for_each_entry(encoder, &dev->mode_config.encoder_list, head) {
-		struct radeon_encoder *radeon_encoder = to_radeon_encoder(encoder);
-		still_going |= radeon_encoder->audio_polling_active;
-		if (changes || r600_hdmi_buffer_status_changed(encoder))
+		if (!radeon_dig_encoder(encoder))
+			continue;
+		if (changed || r600_hdmi_buffer_status_changed(encoder))
 			r600_hdmi_update_audio_settings(encoder);
 	}
-
-	if (still_going)
-		r600_audio_schedule_polling(rdev);
 }
 
 /*
@@ -177,7 +165,7 @@ static void r600_audio_engine_enable(struct radeon_device *rdev, bool enable)
 }
 
 /*
- * initialize the audio vars and register the update timer
+ * initialize the audio vars
  */
 int r600_audio_init(struct radeon_device *rdev)
 {
@@ -186,51 +174,16 @@ int r600_audio_init(struct radeon_device *rdev)
 
 	r600_audio_engine_enable(rdev, true);
 
-	rdev->audio_channels = -1;
-	rdev->audio_rate = -1;
-	rdev->audio_bits_per_sample = -1;
-	rdev->audio_status_bits = 0;
-	rdev->audio_category_code = 0;
-
-	setup_timer(
-		&rdev->audio_timer,
-		r600_audio_update_hdmi,
-		(unsigned long)rdev);
+	rdev->audio_status.channels = -1;
+	rdev->audio_status.rate = -1;
+	rdev->audio_status.bits_per_sample = -1;
+	rdev->audio_status.status_bits = 0;
+	rdev->audio_status.category_code = 0;
 
 	return 0;
 }
 
 /*
- * enable the polling timer, to check for status changes
- */
-void r600_audio_enable_polling(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);
-
-	DRM_DEBUG("r600_audio_enable_polling: %d\n",
-		radeon_encoder->audio_polling_active);
-	if (radeon_encoder->audio_polling_active)
-		return;
-
-	radeon_encoder->audio_polling_active = 1;
-	if (rdev->audio_enabled)
-		mod_timer(&rdev->audio_timer, jiffies + 1);
-}
-
-/*
- * disable the polling timer, so we get no more status updates
- */
-void r600_audio_disable_polling(struct drm_encoder *encoder)
-{
-	struct radeon_encoder *radeon_encoder = to_radeon_encoder(encoder);
-	DRM_DEBUG("r600_audio_disable_polling: %d\n",
-		radeon_encoder->audio_polling_active);
-	radeon_encoder->audio_polling_active = 0;
-}
-
-/*
  * atach the audio codec to the clock source of the encoder
  */
 void r600_audio_set_clock(struct drm_encoder *encoder, int clock)
@@ -297,7 +250,5 @@ void r600_audio_fini(struct radeon_device *rdev)
 	if (!rdev->audio_enabled)
 		return;
 
-	del_timer(&rdev->audio_timer);
-
 	r600_audio_engine_enable(rdev, false);
 }
diff --git a/drivers/gpu/drm/radeon/r600_blit_kms.c b/drivers/gpu/drm/radeon/r600_blit_kms.c
index db38f587f27..03b6e0d3d50 100644
--- a/drivers/gpu/drm/radeon/r600_blit_kms.c
+++ b/drivers/gpu/drm/radeon/r600_blit_kms.c
@@ -513,7 +513,6 @@ int r600_blit_init(struct radeon_device *rdev)
 	rdev->r600_blit.primitives.set_default_state = set_default_state;
 
 	rdev->r600_blit.ring_size_common = 40; /* shaders + def state */
-	rdev->r600_blit.ring_size_common += 16; /* fence emit for VB IB */
 	rdev->r600_blit.ring_size_common += 5; /* done copy */
 	rdev->r600_blit.ring_size_common += 16; /* fence emit for done copy */
 
@@ -528,7 +527,6 @@ int r600_blit_init(struct radeon_device *rdev)
 	if (rdev->r600_blit.shader_obj)
 		goto done;
 
-	mutex_init(&rdev->r600_blit.mutex);
 	rdev->r600_blit.state_offset = 0;
 
 	if (rdev->family >= CHIP_RV770)
@@ -554,7 +552,7 @@ int r600_blit_init(struct radeon_device *rdev)
 	obj_size = ALIGN(obj_size, 256);
 
 	r = radeon_bo_create(rdev, obj_size, PAGE_SIZE, true, RADEON_GEM_DOMAIN_VRAM,
-				&rdev->r600_blit.shader_obj);
+			     NULL, &rdev->r600_blit.shader_obj);
 	if (r) {
 		DRM_ERROR("r600 failed to allocate shader\n");
 		return r;
@@ -621,27 +619,6 @@ void r600_blit_fini(struct radeon_device *rdev)
 	radeon_bo_unref(&rdev->r600_blit.shader_obj);
 }
 
-static int r600_vb_ib_get(struct radeon_device *rdev, unsigned size)
-{
-	int r;
-	r = radeon_ib_get(rdev, RADEON_RING_TYPE_GFX_INDEX,
-			  &rdev->r600_blit.vb_ib, size);
-	if (r) {
-		DRM_ERROR("failed to get IB for vertex buffer\n");
-		return r;
-	}
-
-	rdev->r600_blit.vb_total = size;
-	rdev->r600_blit.vb_used = 0;
-	return 0;
-}
-
-static void r600_vb_ib_put(struct radeon_device *rdev)
-{
-	radeon_fence_emit(rdev, rdev->r600_blit.vb_ib->fence);
-	radeon_ib_free(rdev, &rdev->r600_blit.vb_ib);
-}
-
 static unsigned r600_blit_create_rect(unsigned num_gpu_pages,
 				      int *width, int *height, int max_dim)
 {
@@ -688,7 +665,8 @@ static unsigned r600_blit_create_rect(unsigned num_gpu_pages,
 }
 
 
-int r600_blit_prepare_copy(struct radeon_device *rdev, unsigned num_gpu_pages)
+int r600_blit_prepare_copy(struct radeon_device *rdev, unsigned num_gpu_pages,
+			   struct radeon_sa_bo **vb)
 {
 	struct radeon_ring *ring = &rdev->ring[RADEON_RING_TYPE_GFX_INDEX];
 	int r;
@@ -705,46 +683,54 @@ int r600_blit_prepare_copy(struct radeon_device *rdev, unsigned num_gpu_pages)
 	}
 
 	/* 48 bytes for vertex per loop */
-	r = r600_vb_ib_get(rdev, (num_loops*48)+256);
-	if (r)
+	r = radeon_sa_bo_new(rdev, &rdev->ring_tmp_bo, vb,
+			     (num_loops*48)+256, 256, true);
+	if (r) {
 		return r;
+	}
 
 	/* calculate number of loops correctly */
 	ring_size = num_loops * dwords_per_loop;
 	ring_size += rdev->r600_blit.ring_size_common;
 	r = radeon_ring_lock(rdev, ring, ring_size);
-	if (r)
+	if (r) {
+		radeon_sa_bo_free(rdev, vb, NULL);
 		return r;
+	}
 
 	rdev->r600_blit.primitives.set_default_state(rdev);
 	rdev->r600_blit.primitives.set_shaders(rdev);
 	return 0;
 }
 
-void r600_blit_done_copy(struct radeon_device *rdev, struct radeon_fence *fence)
+void r600_blit_done_copy(struct radeon_device *rdev, struct radeon_fence *fence,
+			 struct radeon_sa_bo *vb)
 {
+	struct radeon_ring *ring = &rdev->ring[RADEON_RING_TYPE_GFX_INDEX];
 	int r;
 
-	if (rdev->r600_blit.vb_ib)
-		r600_vb_ib_put(rdev);
-
-	if (fence)
-		r = radeon_fence_emit(rdev, fence);
+	r = radeon_fence_emit(rdev, fence);
+	if (r) {
+		radeon_ring_unlock_undo(rdev, ring);
+		return;
+	}
 
-	radeon_ring_unlock_commit(rdev, &rdev->ring[RADEON_RING_TYPE_GFX_INDEX]);
+	radeon_ring_unlock_commit(rdev, ring);
+	radeon_sa_bo_free(rdev, &vb, fence);
 }
 
 void r600_kms_blit_copy(struct radeon_device *rdev,
 			u64 src_gpu_addr, u64 dst_gpu_addr,
-			unsigned num_gpu_pages)
+			unsigned num_gpu_pages,
+			struct radeon_sa_bo *vb)
 {
 	u64 vb_gpu_addr;
-	u32 *vb;
+	u32 *vb_cpu_addr;
 
-	DRM_DEBUG("emitting copy %16llx %16llx %d %d\n",
-		  src_gpu_addr, dst_gpu_addr,
-		  num_gpu_pages, rdev->r600_blit.vb_used);
-	vb = (u32 *)(rdev->r600_blit.vb_ib->ptr + rdev->r600_blit.vb_used);
+	DRM_DEBUG("emitting copy %16llx %16llx %d\n",
+		  src_gpu_addr, dst_gpu_addr, num_gpu_pages);
+	vb_cpu_addr = (u32 *)radeon_sa_bo_cpu_addr(vb);
+	vb_gpu_addr = radeon_sa_bo_gpu_addr(vb);
 
 	while (num_gpu_pages) {
 		int w, h;
@@ -756,39 +742,34 @@ void r600_kms_blit_copy(struct radeon_device *rdev,
 		size_in_bytes = pages_per_loop * RADEON_GPU_PAGE_SIZE;
 		DRM_DEBUG("rectangle w=%d h=%d\n", w, h);
 
-		if ((rdev->r600_blit.vb_used + 48) > rdev->r600_blit.vb_total) {
-			WARN_ON(1);
-		}
-
-		vb[0] = 0;
-		vb[1] = 0;
-		vb[2] = 0;
-		vb[3] = 0;
+		vb_cpu_addr[0] = 0;
+		vb_cpu_addr[1] = 0;
+		vb_cpu_addr[2] = 0;
+		vb_cpu_addr[3] = 0;
 
-		vb[4] = 0;
-		vb[5] = i2f(h);
-		vb[6] = 0;
-		vb[7] = i2f(h);
+		vb_cpu_addr[4] = 0;
+		vb_cpu_addr[5] = i2f(h);
+		vb_cpu_addr[6] = 0;
+		vb_cpu_addr[7] = i2f(h);
 
-		vb[8] = i2f(w);
-		vb[9] = i2f(h);
-		vb[10] = i2f(w);
-		vb[11] = i2f(h);
+		vb_cpu_addr[8] = i2f(w);
+		vb_cpu_addr[9] = i2f(h);
+		vb_cpu_addr[10] = i2f(w);
+		vb_cpu_addr[11] = i2f(h);
 
 		rdev->r600_blit.primitives.set_tex_resource(rdev, FMT_8_8_8_8,
 							    w, h, w, src_gpu_addr, size_in_bytes);
 		rdev->r600_blit.primitives.set_render_target(rdev, COLOR_8_8_8_8,
 							     w, h, dst_gpu_addr);
 		rdev->r600_blit.primitives.set_scissors(rdev, 0, 0, w, h);
-		vb_gpu_addr = rdev->r600_blit.vb_ib->gpu_addr + rdev->r600_blit.vb_used;
 		rdev->r600_blit.primitives.set_vtx_resource(rdev, vb_gpu_addr);
 		rdev->r600_blit.primitives.draw_auto(rdev);
 		rdev->r600_blit.primitives.cp_set_surface_sync(rdev,
 				    PACKET3_CB_ACTION_ENA | PACKET3_CB0_DEST_BASE_ENA,
 				    size_in_bytes, dst_gpu_addr);
 
-		vb += 12;
-		rdev->r600_blit.vb_used += 4*12;
+		vb_cpu_addr += 12;
+		vb_gpu_addr += 4*12;
 		src_gpu_addr += size_in_bytes;
 		dst_gpu_addr += size_in_bytes;
 		num_gpu_pages -= pages_per_loop;
diff --git a/drivers/gpu/drm/radeon/r600_cs.c b/drivers/gpu/drm/radeon/r600_cs.c
index b8e12af304a..0133f5f09bd 100644
--- a/drivers/gpu/drm/radeon/r600_cs.c
+++ b/drivers/gpu/drm/radeon/r600_cs.c
@@ -345,7 +345,7 @@ static int r600_cs_track_validate_cb(struct radeon_cs_parser *p, int i)
 	u32 height, height_align, pitch, pitch_align, depth_align;
 	u64 base_offset, base_align;
 	struct array_mode_checker array_check;
-	volatile u32 *ib = p->ib->ptr;
+	volatile u32 *ib = p->ib.ptr;
 	unsigned array_mode;
 	u32 format;
 
@@ -471,7 +471,7 @@ static int r600_cs_track_validate_db(struct radeon_cs_parser *p)
 	u64 base_offset, base_align;
 	struct array_mode_checker array_check;
 	int array_mode;
-	volatile u32 *ib = p->ib->ptr;
+	volatile u32 *ib = p->ib.ptr;
 
 
 	if (track->db_bo == NULL) {
@@ -961,7 +961,7 @@ static int r600_cs_packet_parse_vline(struct radeon_cs_parser *p)
 	uint32_t header, h_idx, reg, wait_reg_mem_info;
 	volatile uint32_t *ib;
 
-	ib = p->ib->ptr;
+	ib = p->ib.ptr;
 
 	/* parse the WAIT_REG_MEM */
 	r = r600_cs_packet_parse(p, &wait_reg_mem, p->idx);
@@ -1110,7 +1110,7 @@ static int r600_cs_check_reg(struct radeon_cs_parser *p, u32 reg, u32 idx)
 	m = 1 << ((reg >> 2) & 31);
 	if (!(r600_reg_safe_bm[i] & m))
 		return 0;
-	ib = p->ib->ptr;
+	ib = p->ib.ptr;
 	switch (reg) {
 	/* force following reg to 0 in an attempt to disable out buffer
 	 * which will need us to better understand how it works to perform
@@ -1714,7 +1714,7 @@ static int r600_packet3_check(struct radeon_cs_parser *p,
 	u32 idx_value;
 
 	track = (struct r600_cs_track *)p->track;
-	ib = p->ib->ptr;
+	ib = p->ib.ptr;
 	idx = pkt->idx + 1;
 	idx_value = radeon_get_ib_value(p, idx);
 
@@ -2249,8 +2249,8 @@ int r600_cs_parse(struct radeon_cs_parser *p)
 		}
 	} while (p->idx < p->chunks[p->chunk_ib_idx].length_dw);
 #if 0
-	for (r = 0; r < p->ib->length_dw; r++) {
-		printk(KERN_INFO "%05d  0x%08X\n", r, p->ib->ptr[r]);
+	for (r = 0; r < p->ib.length_dw; r++) {
+		printk(KERN_INFO "%05d  0x%08X\n", r, p->ib.ptr[r]);
 		mdelay(1);
 	}
 #endif
@@ -2298,7 +2298,6 @@ int r600_cs_legacy(struct drm_device *dev, void *data, struct drm_file *filp,
 {
 	struct radeon_cs_parser parser;
 	struct radeon_cs_chunk *ib_chunk;
-	struct radeon_ib fake_ib;
 	struct r600_cs_track *track;
 	int r;
 
@@ -2314,9 +2313,8 @@ int r600_cs_legacy(struct drm_device *dev, void *data, struct drm_file *filp,
 	parser.dev = &dev->pdev->dev;
 	parser.rdev = NULL;
 	parser.family = family;
-	parser.ib = &fake_ib;
 	parser.track = track;
-	fake_ib.ptr = ib;
+	parser.ib.ptr = ib;
 	r = radeon_cs_parser_init(&parser, data);
 	if (r) {
 		DRM_ERROR("Failed to initialize parser !\n");
@@ -2333,8 +2331,8 @@ int r600_cs_legacy(struct drm_device *dev, void *data, struct drm_file *filp,
 	 * input memory (cached) and write to the IB (which can be
 	 * uncached). */
 	ib_chunk = &parser.chunks[parser.chunk_ib_idx];
-	parser.ib->length_dw = ib_chunk->length_dw;
-	*l = parser.ib->length_dw;
+	parser.ib.length_dw = ib_chunk->length_dw;
+	*l = parser.ib.length_dw;
 	r = r600_cs_parse(&parser);
 	if (r) {
 		DRM_ERROR("Invalid command stream !\n");
diff --git a/drivers/gpu/drm/radeon/r600_hdmi.c b/drivers/gpu/drm/radeon/r600_hdmi.c
index 0b592067145..226379e00ac 100644
--- a/drivers/gpu/drm/radeon/r600_hdmi.c
+++ b/drivers/gpu/drm/radeon/r600_hdmi.c
@@ -27,6 +27,7 @@
 #include "radeon_drm.h"
 #include "radeon.h"
 #include "radeon_asic.h"
+#include "r600d.h"
 #include "atom.h"
 
 /*
@@ -52,19 +53,7 @@ enum r600_hdmi_iec_status_bits {
 	AUDIO_STATUS_LEVEL        = 0x80
 };
 
-struct {
-	uint32_t Clock;
-
-	int N_32kHz;
-	int CTS_32kHz;
-
-	int N_44_1kHz;
-	int CTS_44_1kHz;
-
-	int N_48kHz;
-	int CTS_48kHz;
-
-} r600_hdmi_ACR[] = {
+struct radeon_hdmi_acr r600_hdmi_predefined_acr[] = {
     /*	     32kHz	  44.1kHz	48kHz    */
     /* Clock      N     CTS      N     CTS      N     CTS */
     {  25174,  4576,  28125,  7007,  31250,  6864,  28125 }, /*  25,20/1.001 MHz */
@@ -83,7 +72,7 @@ struct {
 /*
  * calculate CTS value if it's not found in the table
  */
-static void r600_hdmi_calc_CTS(uint32_t clock, int *CTS, int N, int freq)
+static void r600_hdmi_calc_cts(uint32_t clock, int *CTS, int N, int freq)
 {
 	if (*CTS == 0)
 		*CTS = clock * N / (128 * freq) * 1000;
@@ -91,6 +80,24 @@ static void r600_hdmi_calc_CTS(uint32_t clock, int *CTS, int N, int freq)
 		  N, *CTS, freq);
 }
 
+struct radeon_hdmi_acr r600_hdmi_acr(uint32_t clock)
+{
+	struct radeon_hdmi_acr res;
+	u8 i;
+
+	for (i = 0; r600_hdmi_predefined_acr[i].clock != clock &&
+	     r600_hdmi_predefined_acr[i].clock != 0; i++)
+		;
+	res = r600_hdmi_predefined_acr[i];
+
+	/* In case some CTS are missing */
+	r600_hdmi_calc_cts(clock, &res.cts_32khz, res.n_32khz, 32000);
+	r600_hdmi_calc_cts(clock, &res.cts_44_1khz, res.n_44_1khz, 44100);
+	r600_hdmi_calc_cts(clock, &res.cts_48khz, res.n_48khz, 48000);
+
+	return res;
+}
+
 /*
  * update the N and CTS parameters for a given pixel clock rate
  */
@@ -98,30 +105,19 @@ static void r600_hdmi_update_ACR(struct drm_encoder *encoder, uint32_t clock)
 {
 	struct drm_device *dev = encoder->dev;
 	struct radeon_device *rdev = dev->dev_private;
-	uint32_t offset = to_radeon_encoder(encoder)->hdmi_offset;
-	int CTS;
-	int N;
-	int i;
+	struct radeon_hdmi_acr acr = r600_hdmi_acr(clock);
+	struct radeon_encoder *radeon_encoder = to_radeon_encoder(encoder);
+	struct radeon_encoder_atom_dig *dig = radeon_encoder->enc_priv;
+	uint32_t offset = dig->afmt->offset;
+
+	WREG32(HDMI0_ACR_32_0 + offset, HDMI0_ACR_CTS_32(acr.cts_32khz));
+	WREG32(HDMI0_ACR_32_1 + offset, acr.n_32khz);
 
-	for (i = 0; r600_hdmi_ACR[i].Clock != clock && r600_hdmi_ACR[i].Clock != 0; i++);
-
-	CTS = r600_hdmi_ACR[i].CTS_32kHz;
-	N = r600_hdmi_ACR[i].N_32kHz;
-	r600_hdmi_calc_CTS(clock, &CTS, N, 32000);
-	WREG32(offset+R600_HDMI_32kHz_CTS, CTS << 12);
-	WREG32(offset+R600_HDMI_32kHz_N, N);
-
-	CTS = r600_hdmi_ACR[i].CTS_44_1kHz;
-	N = r600_hdmi_ACR[i].N_44_1kHz;
-	r600_hdmi_calc_CTS(clock, &CTS, N, 44100);
-	WREG32(offset+R600_HDMI_44_1kHz_CTS, CTS << 12);
-	WREG32(offset+R600_HDMI_44_1kHz_N, N);
-
-	CTS = r600_hdmi_ACR[i].CTS_48kHz;
-	N = r600_hdmi_ACR[i].N_48kHz;
-	r600_hdmi_calc_CTS(clock, &CTS, N, 48000);
-	WREG32(offset+R600_HDMI_48kHz_CTS, CTS << 12);
-	WREG32(offset+R600_HDMI_48kHz_N, N);
+	WREG32(HDMI0_ACR_44_0 + offset, HDMI0_ACR_CTS_44(acr.cts_44_1khz));
+	WREG32(HDMI0_ACR_44_1 + offset, acr.n_44_1khz);
+
+	WREG32(HDMI0_ACR_48_0 + offset, HDMI0_ACR_CTS_48(acr.cts_48khz));
+	WREG32(HDMI0_ACR_48_1 + offset, acr.n_48khz);
 }
 
 /*
@@ -165,7 +161,9 @@ static void r600_hdmi_videoinfoframe(
 {
 	struct drm_device *dev = encoder->dev;
 	struct radeon_device *rdev = dev->dev_private;
-	uint32_t offset = to_radeon_encoder(encoder)->hdmi_offset;
+	struct radeon_encoder *radeon_encoder = to_radeon_encoder(encoder);
+	struct radeon_encoder_atom_dig *dig = radeon_encoder->enc_priv;
+	uint32_t offset = dig->afmt->offset;
 
 	uint8_t frame[14];
 
@@ -204,13 +202,13 @@ static void r600_hdmi_videoinfoframe(
 	 * workaround this issue. */
 	frame[0x0] += 2;
 
-	WREG32(offset+R600_HDMI_VIDEOINFOFRAME_0,
+	WREG32(HDMI0_AVI_INFO0 + offset,
 		frame[0x0] | (frame[0x1] << 8) | (frame[0x2] << 16) | (frame[0x3] << 24));
-	WREG32(offset+R600_HDMI_VIDEOINFOFRAME_1,
+	WREG32(HDMI0_AVI_INFO1 + offset,
 		frame[0x4] | (frame[0x5] << 8) | (frame[0x6] << 16) | (frame[0x7] << 24));
-	WREG32(offset+R600_HDMI_VIDEOINFOFRAME_2,
+	WREG32(HDMI0_AVI_INFO2 + offset,
 		frame[0x8] | (frame[0x9] << 8) | (frame[0xA] << 16) | (frame[0xB] << 24));
-	WREG32(offset+R600_HDMI_VIDEOINFOFRAME_3,
+	WREG32(HDMI0_AVI_INFO3 + offset,
 		frame[0xC] | (frame[0xD] << 8));
 }
 
@@ -231,7 +229,9 @@ static void r600_hdmi_audioinfoframe(
 {
 	struct drm_device *dev = encoder->dev;
 	struct radeon_device *rdev = dev->dev_private;
-	uint32_t offset = to_radeon_encoder(encoder)->hdmi_offset;
+	struct radeon_encoder *radeon_encoder = to_radeon_encoder(encoder);
+	struct radeon_encoder_atom_dig *dig = radeon_encoder->enc_priv;
+	uint32_t offset = dig->afmt->offset;
 
 	uint8_t frame[11];
 
@@ -249,22 +249,24 @@ static void r600_hdmi_audioinfoframe(
 
 	r600_hdmi_infoframe_checksum(0x84, 0x01, 0x0A, frame);
 
-	WREG32(offset+R600_HDMI_AUDIOINFOFRAME_0,
+	WREG32(HDMI0_AUDIO_INFO0 + offset,
 		frame[0x0] | (frame[0x1] << 8) | (frame[0x2] << 16) | (frame[0x3] << 24));
-	WREG32(offset+R600_HDMI_AUDIOINFOFRAME_1,
+	WREG32(HDMI0_AUDIO_INFO1 + offset,
 		frame[0x4] | (frame[0x5] << 8) | (frame[0x6] << 16) | (frame[0x8] << 24));
 }
 
 /*
  * test if audio buffer is filled enough to start playing
  */
-static int r600_hdmi_is_audio_buffer_filled(struct drm_encoder *encoder)
+static bool r600_hdmi_is_audio_buffer_filled(struct drm_encoder *encoder)
 {
 	struct drm_device *dev = encoder->dev;
 	struct radeon_device *rdev = dev->dev_private;
-	uint32_t offset = to_radeon_encoder(encoder)->hdmi_offset;
+	struct radeon_encoder *radeon_encoder = to_radeon_encoder(encoder);
+	struct radeon_encoder_atom_dig *dig = radeon_encoder->enc_priv;
+	uint32_t offset = dig->afmt->offset;
 
-	return (RREG32(offset+R600_HDMI_STATUS) & 0x10) != 0;
+	return (RREG32(HDMI0_STATUS + offset) & 0x10) != 0;
 }
 
 /*
@@ -273,14 +275,15 @@ static int r600_hdmi_is_audio_buffer_filled(struct drm_encoder *encoder)
 int r600_hdmi_buffer_status_changed(struct drm_encoder *encoder)
 {
 	struct radeon_encoder *radeon_encoder = to_radeon_encoder(encoder);
+	struct radeon_encoder_atom_dig *dig = radeon_encoder->enc_priv;
 	int status, result;
 
-	if (!radeon_encoder->hdmi_offset)
+	if (!dig->afmt || !dig->afmt->enabled)
 		return 0;
 
 	status = r600_hdmi_is_audio_buffer_filled(encoder);
-	result = radeon_encoder->hdmi_buffer_status != status;
-	radeon_encoder->hdmi_buffer_status = status;
+	result = dig->afmt->last_buffer_filled_status != status;
+	dig->afmt->last_buffer_filled_status = status;
 
 	return result;
 }
@@ -288,26 +291,23 @@ int r600_hdmi_buffer_status_changed(struct drm_encoder *encoder)
 /*
  * write the audio workaround status to the hardware
  */
-void r600_hdmi_audio_workaround(struct drm_encoder *encoder)
+static void r600_hdmi_audio_workaround(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);
-	uint32_t offset = radeon_encoder->hdmi_offset;
-
-	if (!offset)
-		return;
-
-	if (!radeon_encoder->hdmi_audio_workaround ||
-		r600_hdmi_is_audio_buffer_filled(encoder)) {
-
-		/* disable audio workaround */
-		WREG32_P(offset+R600_HDMI_CNTL, 0x00000001, ~0x00001001);
-
-	} else {
-		/* enable audio workaround */
-		WREG32_P(offset+R600_HDMI_CNTL, 0x00001001, ~0x00001001);
-	}
+	struct radeon_encoder_atom_dig *dig = radeon_encoder->enc_priv;
+	uint32_t offset = dig->afmt->offset;
+	bool hdmi_audio_workaround = false; /* FIXME */
+	u32 value;
+
+	if (!hdmi_audio_workaround ||
+	    r600_hdmi_is_audio_buffer_filled(encoder))
+		value = 0; /* disable workaround */
+	else
+		value = HDMI0_AUDIO_TEST_EN; /* enable workaround */
+	WREG32_P(HDMI0_AUDIO_PACKET_CONTROL + offset,
+		 value, ~HDMI0_AUDIO_TEST_EN);
 }
 
 
@@ -318,39 +318,75 @@ void r600_hdmi_setmode(struct drm_encoder *encoder, struct drm_display_mode *mod
 {
 	struct drm_device *dev = encoder->dev;
 	struct radeon_device *rdev = dev->dev_private;
-	uint32_t offset = to_radeon_encoder(encoder)->hdmi_offset;
+	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_DCE5(rdev))
 		return;
 
-	if (!offset)
+	/* Silent, r600_hdmi_enable will raise WARN for us */
+	if (!dig->afmt->enabled)
 		return;
+	offset = dig->afmt->offset;
 
 	r600_audio_set_clock(encoder, mode->clock);
 
-	WREG32(offset+R600_HDMI_UNKNOWN_0, 0x1000);
-	WREG32(offset+R600_HDMI_UNKNOWN_1, 0x0);
-	WREG32(offset+R600_HDMI_UNKNOWN_2, 0x1000);
+	WREG32(HDMI0_VBI_PACKET_CONTROL + offset,
+	       HDMI0_NULL_SEND); /* send null packets when required */
 
-	r600_hdmi_update_ACR(encoder, mode->clock);
+	WREG32(HDMI0_AUDIO_CRC_CONTROL + offset, 0x1000);
+
+	if (ASIC_IS_DCE32(rdev)) {
+		WREG32(HDMI0_AUDIO_PACKET_CONTROL + offset,
+		       HDMI0_AUDIO_DELAY_EN(1) | /* default audio delay */
+		       HDMI0_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 */
+	} else {
+		WREG32(HDMI0_AUDIO_PACKET_CONTROL + offset,
+		       HDMI0_AUDIO_SAMPLE_SEND | /* send audio packets */
+		       HDMI0_AUDIO_DELAY_EN(1) | /* default audio delay */
+		       HDMI0_AUDIO_SEND_MAX_PACKETS | /* send NULL packets if no audio is available */
+		       HDMI0_AUDIO_PACKETS_PER_LINE(3) | /* should be suffient for all audio modes and small enough for all hblanks */
+		       HDMI0_60958_CS_UPDATE); /* allow 60958 channel status fields to be updated */
+	}
+
+	WREG32(HDMI0_ACR_PACKET_CONTROL + offset,
+	       HDMI0_ACR_AUTO_SEND | /* allow hw to sent ACR packets when required */
+	       HDMI0_ACR_SOURCE); /* select SW CTS value */
+
+	WREG32(HDMI0_VBI_PACKET_CONTROL + offset,
+	       HDMI0_NULL_SEND | /* send null packets when required */
+	       HDMI0_GC_SEND | /* send general control packets */
+	       HDMI0_GC_CONT); /* send general control packets every frame */
 
-	WREG32(offset+R600_HDMI_VIDEOCNTL, 0x13);
+	/* TODO: HDMI0_AUDIO_INFO_UPDATE */
+	WREG32(HDMI0_INFOFRAME_CONTROL0 + offset,
+	       HDMI0_AVI_INFO_SEND | /* enable AVI info frames */
+	       HDMI0_AVI_INFO_CONT | /* send AVI info frames every frame/field */
+	       HDMI0_AUDIO_INFO_SEND | /* enable audio info frames (frames won't be set until audio is enabled) */
+	       HDMI0_AUDIO_INFO_CONT); /* send audio info frames every frame/field */
 
-	WREG32(offset+R600_HDMI_VERSION, 0x202);
+	WREG32(HDMI0_INFOFRAME_CONTROL1 + offset,
+	       HDMI0_AVI_INFO_LINE(2) | /* anything other than 0 */
+	       HDMI0_AUDIO_INFO_LINE(2)); /* anything other than 0 */
+
+	WREG32(HDMI0_GC + offset, 0); /* unset HDMI0_GC_AVMUTE */
 
 	r600_hdmi_videoinfoframe(encoder, RGB, 0, 0, 0, 0,
 		0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0);
 
+	r600_hdmi_update_ACR(encoder, mode->clock);
+
 	/* it's unknown what these bits do excatly, but it's indeed quite useful for debugging */
-	WREG32(offset+R600_HDMI_AUDIO_DEBUG_0, 0x00FFFFFF);
-	WREG32(offset+R600_HDMI_AUDIO_DEBUG_1, 0x007FFFFF);
-	WREG32(offset+R600_HDMI_AUDIO_DEBUG_2, 0x00000001);
-	WREG32(offset+R600_HDMI_AUDIO_DEBUG_3, 0x00000001);
+	WREG32(HDMI0_RAMP_CONTROL0 + offset, 0x00FFFFFF);
+	WREG32(HDMI0_RAMP_CONTROL1 + offset, 0x007FFFFF);
+	WREG32(HDMI0_RAMP_CONTROL2 + offset, 0x00000001);
+	WREG32(HDMI0_RAMP_CONTROL3 + offset, 0x00000001);
 
 	r600_hdmi_audio_workaround(encoder);
-
-	/* audio packets per line, does anyone know how to calc this ? */
-	WREG32_P(offset+R600_HDMI_CNTL, 0x00040000, ~0x001F0000);
 }
 
 /*
@@ -360,145 +396,82 @@ void r600_hdmi_update_audio_settings(struct drm_encoder *encoder)
 {
 	struct drm_device *dev = encoder->dev;
 	struct radeon_device *rdev = dev->dev_private;
-	uint32_t offset = to_radeon_encoder(encoder)->hdmi_offset;
-
-	int channels = r600_audio_channels(rdev);
-	int rate = r600_audio_rate(rdev);
-	int bps = r600_audio_bits_per_sample(rdev);
-	uint8_t status_bits = r600_audio_status_bits(rdev);
-	uint8_t category_code = r600_audio_category_code(rdev);
-
+	struct radeon_encoder *radeon_encoder = to_radeon_encoder(encoder);
+	struct radeon_encoder_atom_dig *dig = radeon_encoder->enc_priv;
+	struct r600_audio audio = r600_audio_status(rdev);
+	uint32_t offset;
 	uint32_t iec;
 
-	if (!offset)
+	if (!dig->afmt || !dig->afmt->enabled)
 		return;
+	offset = dig->afmt->offset;
 
 	DRM_DEBUG("%s with %d channels, %d Hz sampling rate, %d bits per sample,\n",
 		 r600_hdmi_is_audio_buffer_filled(encoder) ? "playing" : "stopped",
-		channels, rate, bps);
+		  audio.channels, audio.rate, audio.bits_per_sample);
 	DRM_DEBUG("0x%02X IEC60958 status bits and 0x%02X category code\n",
-		  (int)status_bits, (int)category_code);
+		  (int)audio.status_bits, (int)audio.category_code);
 
 	iec = 0;
-	if (status_bits & AUDIO_STATUS_PROFESSIONAL)
+	if (audio.status_bits & AUDIO_STATUS_PROFESSIONAL)
 		iec |= 1 << 0;
-	if (status_bits & AUDIO_STATUS_NONAUDIO)
+	if (audio.status_bits & AUDIO_STATUS_NONAUDIO)
 		iec |= 1 << 1;
-	if (status_bits & AUDIO_STATUS_COPYRIGHT)
+	if (audio.status_bits & AUDIO_STATUS_COPYRIGHT)
 		iec |= 1 << 2;
-	if (status_bits & AUDIO_STATUS_EMPHASIS)
+	if (audio.status_bits & AUDIO_STATUS_EMPHASIS)
 		iec |= 1 << 3;
 
-	iec |= category_code << 8;
-
-	switch (rate) {
-	case  32000: iec |= 0x3 << 24; break;
-	case  44100: iec |= 0x0 << 24; break;
-	case  88200: iec |= 0x8 << 24; break;
-	case 176400: iec |= 0xc << 24; break;
-	case  48000: iec |= 0x2 << 24; break;
-	case  96000: iec |= 0xa << 24; break;
-	case 192000: iec |= 0xe << 24; break;
+	iec |= HDMI0_60958_CS_CATEGORY_CODE(audio.category_code);
+
+	switch (audio.rate) {
+	case 32000:
+		iec |= HDMI0_60958_CS_SAMPLING_FREQUENCY(0x3);
+		break;
+	case 44100:
+		iec |= HDMI0_60958_CS_SAMPLING_FREQUENCY(0x0);
+		break;
+	case 48000:
+		iec |= HDMI0_60958_CS_SAMPLING_FREQUENCY(0x2);
+		break;
+	case 88200:
+		iec |= HDMI0_60958_CS_SAMPLING_FREQUENCY(0x8);
+		break;
+	case 96000:
+		iec |= HDMI0_60958_CS_SAMPLING_FREQUENCY(0xa);
+		break;
+	case 176400:
+		iec |= HDMI0_60958_CS_SAMPLING_FREQUENCY(0xc);
+		break;
+	case 192000:
+		iec |= HDMI0_60958_CS_SAMPLING_FREQUENCY(0xe);
+		break;
 	}
 
-	WREG32(offset+R600_HDMI_IEC60958_1, iec);
+	WREG32(HDMI0_60958_0 + offset, iec);
 
 	iec = 0;
-	switch (bps) {
-	case 16: iec |= 0x2; break;
-	case 20: iec |= 0x3; break;
-	case 24: iec |= 0xb; break;
+	switch (audio.bits_per_sample) {
+	case 16:
+		iec |= HDMI0_60958_CS_WORD_LENGTH(0x2);
+		break;
+	case 20:
+		iec |= HDMI0_60958_CS_WORD_LENGTH(0x3);
+		break;
+	case 24:
+		iec |= HDMI0_60958_CS_WORD_LENGTH(0xb);
+		break;
 	}
-	if (status_bits & AUDIO_STATUS_V)
+	if (audio.status_bits & AUDIO_STATUS_V)
 		iec |= 0x5 << 16;
+	WREG32_P(HDMI0_60958_1 + offset, iec, ~0x5000f);
 
-	WREG32_P(offset+R600_HDMI_IEC60958_2, iec, ~0x5000f);
-
-	/* 0x021 or 0x031 sets the audio frame length */
-	WREG32(offset+R600_HDMI_AUDIOCNTL, 0x31);
-	r600_hdmi_audioinfoframe(encoder, channels-1, 0, 0, 0, 0, 0, 0, 0);
+	r600_hdmi_audioinfoframe(encoder, audio.channels - 1, 0, 0, 0, 0, 0, 0,
+				 0);
 
 	r600_hdmi_audio_workaround(encoder);
 }
 
-static int r600_hdmi_find_free_block(struct drm_device *dev)
-{
-	struct radeon_device *rdev = dev->dev_private;
-	struct drm_encoder *encoder;
-	struct radeon_encoder *radeon_encoder;
-	bool free_blocks[3] = { true, true, true };
-
-	list_for_each_entry(encoder, &dev->mode_config.encoder_list, head) {
-		radeon_encoder = to_radeon_encoder(encoder);
-		switch (radeon_encoder->hdmi_offset) {
-		case R600_HDMI_BLOCK1:
-			free_blocks[0] = false;
-			break;
-		case R600_HDMI_BLOCK2:
-			free_blocks[1] = false;
-			break;
-		case R600_HDMI_BLOCK3:
-			free_blocks[2] = false;
-			break;
-		}
-	}
-
-	if (rdev->family == CHIP_RS600 || rdev->family == CHIP_RS690 ||
-	    rdev->family == CHIP_RS740) {
-		return free_blocks[0] ? R600_HDMI_BLOCK1 : 0;
-	} else if (rdev->family >= CHIP_R600) {
-		if (free_blocks[0])
-			return R600_HDMI_BLOCK1;
-		else if (free_blocks[1])
-			return R600_HDMI_BLOCK2;
-	}
-	return 0;
-}
-
-static void r600_hdmi_assign_block(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;
-
-	u16 eg_offsets[] = {
-		EVERGREEN_CRTC0_REGISTER_OFFSET,
-		EVERGREEN_CRTC1_REGISTER_OFFSET,
-		EVERGREEN_CRTC2_REGISTER_OFFSET,
-		EVERGREEN_CRTC3_REGISTER_OFFSET,
-		EVERGREEN_CRTC4_REGISTER_OFFSET,
-		EVERGREEN_CRTC5_REGISTER_OFFSET,
-	};
-
-	if (!dig) {
-		dev_err(rdev->dev, "Enabling HDMI on non-dig encoder\n");
-		return;
-	}
-
-	if (ASIC_IS_DCE5(rdev)) {
-		/* TODO */
-	} else if (ASIC_IS_DCE4(rdev)) {
-		if (dig->dig_encoder >= ARRAY_SIZE(eg_offsets)) {
-			dev_err(rdev->dev, "Enabling HDMI on unknown dig\n");
-			return;
-		}
-		radeon_encoder->hdmi_offset = EVERGREEN_HDMI_BASE +
-						eg_offsets[dig->dig_encoder];
-		radeon_encoder->hdmi_config_offset = radeon_encoder->hdmi_offset
-						+ EVERGREEN_HDMI_CONFIG_OFFSET;
-	} else if (ASIC_IS_DCE3(rdev)) {
-		radeon_encoder->hdmi_offset = dig->dig_encoder ?
-			R600_HDMI_BLOCK3 : R600_HDMI_BLOCK1;
-		if (ASIC_IS_DCE32(rdev))
-			radeon_encoder->hdmi_config_offset = dig->dig_encoder ?
-				R600_HDMI_CONFIG2 : R600_HDMI_CONFIG1;
-	} else if (rdev->family >= CHIP_R600 || rdev->family == CHIP_RS600 ||
-		   rdev->family == CHIP_RS690 || rdev->family == CHIP_RS740) {
-		radeon_encoder->hdmi_offset = r600_hdmi_find_free_block(dev);
-	}
-}
-
 /*
  * enable the HDMI engine
  */
@@ -507,64 +480,57 @@ void r600_hdmi_enable(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;
+	u32 hdmi;
 
 	if (ASIC_IS_DCE5(rdev))
 		return;
 
-	if (!radeon_encoder->hdmi_offset) {
-		r600_hdmi_assign_block(encoder);
-		if (!radeon_encoder->hdmi_offset) {
-			dev_warn(rdev->dev, "Could not find HDMI block for "
-				"0x%x encoder\n", radeon_encoder->encoder_id);
-			return;
-		}
-	}
+	/* Silent, r600_hdmi_enable will raise WARN for us */
+	if (dig->afmt->enabled)
+		return;
+	offset = dig->afmt->offset;
 
-	offset = radeon_encoder->hdmi_offset;
-	if (ASIC_IS_DCE5(rdev)) {
-		/* TODO */
-	} else if (ASIC_IS_DCE4(rdev)) {
-		WREG32_P(radeon_encoder->hdmi_config_offset + 0xc, 0x1, ~0x1);
-	} else if (ASIC_IS_DCE32(rdev)) {
-		WREG32_P(radeon_encoder->hdmi_config_offset + 0x4, 0x1, ~0x1);
-	} else if (ASIC_IS_DCE3(rdev)) {
-		/* TODO */
-	} else if (rdev->family >= CHIP_R600) {
+	/* Older chipsets require setting HDMI and routing manually */
+	if (rdev->family >= CHIP_R600 && !ASIC_IS_DCE3(rdev)) {
+		hdmi = HDMI0_ERROR_ACK | 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);
-			WREG32(offset + R600_HDMI_ENABLE, 0x101);
+			hdmi |= HDMI0_STREAM(HDMI0_STREAM_TMDSA);
 			break;
 		case ENCODER_OBJECT_ID_INTERNAL_LVTM1:
 			WREG32_P(AVIVO_LVTMA_CNTL, AVIVO_LVTMA_CNTL_HDMI_EN,
 				 ~AVIVO_LVTMA_CNTL_HDMI_EN);
-			WREG32(offset + R600_HDMI_ENABLE, 0x105);
+			hdmi |= HDMI0_STREAM(HDMI0_STREAM_LVTMA);
+			break;
+		case ENCODER_OBJECT_ID_INTERNAL_DDI:
+			WREG32_P(DDIA_CNTL, DDIA_HDMI_EN, ~DDIA_HDMI_EN);
+			hdmi |= HDMI0_STREAM(HDMI0_STREAM_DDIA);
+			break;
+		case ENCODER_OBJECT_ID_INTERNAL_KLDSCP_DVO1:
+			hdmi |= HDMI0_STREAM(HDMI0_STREAM_DVOA);
 			break;
 		default:
-			dev_err(rdev->dev, "Unknown HDMI output type\n");
+			dev_err(rdev->dev, "Invalid encoder for HDMI: 0x%X\n",
+				radeon_encoder->encoder_id);
 			break;
 		}
+		WREG32(HDMI0_CONTROL + offset, hdmi);
 	}
 
-	if (rdev->irq.installed
-	    && rdev->family != CHIP_RS600
-	    && rdev->family != CHIP_RS690
-	    && rdev->family != CHIP_RS740
-	    && !ASIC_IS_DCE4(rdev)) {
+	if (rdev->irq.installed) {
 		/* if irq is available use it */
-		rdev->irq.hdmi[offset == R600_HDMI_BLOCK1 ? 0 : 1] = true;
+		rdev->irq.afmt[dig->afmt->id] = true;
 		radeon_irq_set(rdev);
-
-		r600_audio_disable_polling(encoder);
-	} else {
-		/* if not fallback to polling */
-		r600_audio_enable_polling(encoder);
 	}
 
+	dig->afmt->enabled = true;
+
 	DRM_DEBUG("Enabling HDMI interface @ 0x%04X for encoder 0x%x\n",
-		radeon_encoder->hdmi_offset, radeon_encoder->encoder_id);
+		  offset, radeon_encoder->encoder_id);
 }
 
 /*
@@ -575,51 +541,51 @@ 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_DCE5(rdev))
 		return;
 
-	offset = radeon_encoder->hdmi_offset;
-	if (!offset) {
-		dev_err(rdev->dev, "Disabling not enabled HDMI\n");
+	/* Called for ATOM_ENCODER_MODE_HDMI only */
+	if (!dig || !dig->afmt) {
+		WARN_ON(1);
 		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);
+		  offset, radeon_encoder->encoder_id);
 
 	/* disable irq */
-	rdev->irq.hdmi[offset == R600_HDMI_BLOCK1 ? 0 : 1] = false;
+	rdev->irq.afmt[dig->afmt->id] = false;
 	radeon_irq_set(rdev);
 
-	/* disable polling */
-	r600_audio_disable_polling(encoder);
-
-	if (ASIC_IS_DCE5(rdev)) {
-		/* TODO */
-	} else if (ASIC_IS_DCE4(rdev)) {
-		WREG32_P(radeon_encoder->hdmi_config_offset + 0xc, 0, ~0x1);
-	} else if (ASIC_IS_DCE32(rdev)) {
-		WREG32_P(radeon_encoder->hdmi_config_offset + 0x4, 0, ~0x1);
-	} else if (rdev->family >= CHIP_R600 && !ASIC_IS_DCE3(rdev)) {
+	/* 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);
-			WREG32(offset + R600_HDMI_ENABLE, 0);
 			break;
 		case ENCODER_OBJECT_ID_INTERNAL_LVTM1:
 			WREG32_P(AVIVO_LVTMA_CNTL, 0,
 				 ~AVIVO_LVTMA_CNTL_HDMI_EN);
-			WREG32(offset + R600_HDMI_ENABLE, 0);
+			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, "Unknown HDMI output type\n");
+			dev_err(rdev->dev, "Invalid encoder for HDMI: 0x%X\n",
+				radeon_encoder->encoder_id);
 			break;
 		}
+		WREG32(HDMI0_CONTROL + offset, HDMI0_ERROR_ACK);
 	}
 
-	radeon_encoder->hdmi_offset = 0;
-	radeon_encoder->hdmi_config_offset = 0;
+	dig->afmt->enabled = false;
 }
diff --git a/drivers/gpu/drm/radeon/r600_reg.h b/drivers/gpu/drm/radeon/r600_reg.h
index f869897c745..2b960cb5c18 100644
--- a/drivers/gpu/drm/radeon/r600_reg.h
+++ b/drivers/gpu/drm/radeon/r600_reg.h
@@ -156,45 +156,10 @@
 #define R600_AUDIO_PIN_WIDGET_CNTL        0x73d4
 #define R600_AUDIO_STATUS_BITS            0x73d8
 
-/* HDMI base register addresses */
-#define R600_HDMI_BLOCK1                  0x7400
-#define R600_HDMI_BLOCK2                  0x7700
-#define R600_HDMI_BLOCK3                  0x7800
-
-/* HDMI registers */
-#define R600_HDMI_ENABLE                0x00
-#define R600_HDMI_STATUS                0x04
-#       define R600_HDMI_INT_PENDING    (1 << 29)
-#define R600_HDMI_CNTL                  0x08
-#       define R600_HDMI_INT_EN         (1 << 28)
-#       define R600_HDMI_INT_ACK        (1 << 29)
-#define R600_HDMI_UNKNOWN_0             0x0C
-#define R600_HDMI_AUDIOCNTL             0x10
-#define R600_HDMI_VIDEOCNTL             0x14
-#define R600_HDMI_VERSION               0x18
-#define R600_HDMI_UNKNOWN_1             0x28
-#define R600_HDMI_VIDEOINFOFRAME_0      0x54
-#define R600_HDMI_VIDEOINFOFRAME_1      0x58
-#define R600_HDMI_VIDEOINFOFRAME_2      0x5c
-#define R600_HDMI_VIDEOINFOFRAME_3      0x60
-#define R600_HDMI_32kHz_CTS             0xac
-#define R600_HDMI_32kHz_N               0xb0
-#define R600_HDMI_44_1kHz_CTS           0xb4
-#define R600_HDMI_44_1kHz_N             0xb8
-#define R600_HDMI_48kHz_CTS             0xbc
-#define R600_HDMI_48kHz_N               0xc0
-#define R600_HDMI_AUDIOINFOFRAME_0      0xcc
-#define R600_HDMI_AUDIOINFOFRAME_1      0xd0
-#define R600_HDMI_IEC60958_1            0xd4
-#define R600_HDMI_IEC60958_2            0xd8
-#define R600_HDMI_UNKNOWN_2             0xdc
-#define R600_HDMI_AUDIO_DEBUG_0         0xe0
-#define R600_HDMI_AUDIO_DEBUG_1         0xe4
-#define R600_HDMI_AUDIO_DEBUG_2         0xe8
-#define R600_HDMI_AUDIO_DEBUG_3         0xec
-
-/* HDMI additional config base register addresses */
-#define R600_HDMI_CONFIG1                 0x7600
-#define R600_HDMI_CONFIG2                 0x7a00
+#define DCE2_HDMI_OFFSET0		(0x7400 - 0x7400)
+#define DCE2_HDMI_OFFSET1		(0x7700 - 0x7400)
+/* DCE3.2 second instance starts at 0x7800 */
+#define DCE3_HDMI_OFFSET0		(0x7400 - 0x7400)
+#define DCE3_HDMI_OFFSET1		(0x7800 - 0x7400)
 
 #endif
diff --git a/drivers/gpu/drm/radeon/r600d.h b/drivers/gpu/drm/radeon/r600d.h
index 59f9c993cc3..15bd3b21624 100644
--- a/drivers/gpu/drm/radeon/r600d.h
+++ b/drivers/gpu/drm/radeon/r600d.h
@@ -824,6 +824,239 @@
 #       define TARGET_LINK_SPEED_MASK                     (0xf << 0)
 #       define SELECTABLE_DEEMPHASIS                      (1 << 6)
 
+/* Audio clocks */
+#define DCCG_AUDIO_DTO0_PHASE             0x0514
+#define DCCG_AUDIO_DTO0_MODULE            0x0518
+#define DCCG_AUDIO_DTO0_LOAD              0x051c
+#       define DTO_LOAD                   (1 << 31)
+#define DCCG_AUDIO_DTO0_CNTL              0x0520
+
+#define DCCG_AUDIO_DTO1_PHASE             0x0524
+#define DCCG_AUDIO_DTO1_MODULE            0x0528
+#define DCCG_AUDIO_DTO1_LOAD              0x052c
+#define DCCG_AUDIO_DTO1_CNTL              0x0530
+
+#define DCCG_AUDIO_DTO_SELECT             0x0534
+
+/* digital blocks */
+#define TMDSA_CNTL                       0x7880
+#       define TMDSA_HDMI_EN             (1 << 2)
+#define LVTMA_CNTL                       0x7a80
+#       define LVTMA_HDMI_EN             (1 << 2)
+#define DDIA_CNTL                        0x7200
+#       define DDIA_HDMI_EN              (1 << 2)
+#define DIG0_CNTL                        0x75a0
+#       define DIG_MODE(x)               (((x) & 7) << 8)
+#       define DIG_MODE_DP               0
+#       define DIG_MODE_LVDS             1
+#       define DIG_MODE_TMDS_DVI         2
+#       define DIG_MODE_TMDS_HDMI        3
+#       define DIG_MODE_SDVO             4
+#define DIG1_CNTL                        0x79a0
+
+/* rs6xx/rs740 and r6xx share the same HDMI blocks, however, rs6xx has only one
+ * instance of the blocks while r6xx has 2.  DCE 3.0 cards are slightly
+ * different due to the new DIG blocks, but also have 2 instances.
+ * DCE 3.0 HDMI blocks are part of each DIG encoder.
+ */
+
+/* rs6xx/rs740/r6xx/dce3 */
+#define HDMI0_CONTROL                0x7400
+/* rs6xx/rs740/r6xx */
+#       define HDMI0_ENABLE          (1 << 0)
+#       define HDMI0_STREAM(x)       (((x) & 3) << 2)
+#       define HDMI0_STREAM_TMDSA    0
+#       define HDMI0_STREAM_LVTMA    1
+#       define HDMI0_STREAM_DVOA     2
+#       define HDMI0_STREAM_DDIA     3
+/* rs6xx/r6xx/dce3 */
+#       define HDMI0_ERROR_ACK       (1 << 8)
+#       define HDMI0_ERROR_MASK      (1 << 9)
+#define HDMI0_STATUS                 0x7404
+#       define HDMI0_ACTIVE_AVMUTE   (1 << 0)
+#       define HDMI0_AUDIO_ENABLE    (1 << 4)
+#       define HDMI0_AZ_FORMAT_WTRIG     (1 << 28)
+#       define HDMI0_AZ_FORMAT_WTRIG_INT (1 << 29)
+#define HDMI0_AUDIO_PACKET_CONTROL   0x7408
+#       define HDMI0_AUDIO_SAMPLE_SEND  (1 << 0)
+#       define HDMI0_AUDIO_DELAY_EN(x)  (((x) & 3) << 4)
+#       define HDMI0_AUDIO_SEND_MAX_PACKETS  (1 << 8)
+#       define HDMI0_AUDIO_TEST_EN         (1 << 12)
+#       define HDMI0_AUDIO_PACKETS_PER_LINE(x)  (((x) & 0x1f) << 16)
+#       define HDMI0_AUDIO_CHANNEL_SWAP    (1 << 24)
+#       define HDMI0_60958_CS_UPDATE       (1 << 26)
+#       define HDMI0_AZ_FORMAT_WTRIG_MASK  (1 << 28)
+#       define HDMI0_AZ_FORMAT_WTRIG_ACK   (1 << 29)
+#define HDMI0_AUDIO_CRC_CONTROL      0x740c
+#       define HDMI0_AUDIO_CRC_EN    (1 << 0)
+#define HDMI0_VBI_PACKET_CONTROL     0x7410
+#       define HDMI0_NULL_SEND       (1 << 0)
+#       define HDMI0_GC_SEND         (1 << 4)
+#       define HDMI0_GC_CONT         (1 << 5) /* 0 - once; 1 - every frame */
+#define HDMI0_INFOFRAME_CONTROL0     0x7414
+#       define HDMI0_AVI_INFO_SEND   (1 << 0)
+#       define HDMI0_AVI_INFO_CONT   (1 << 1)
+#       define HDMI0_AUDIO_INFO_SEND (1 << 4)
+#       define HDMI0_AUDIO_INFO_CONT (1 << 5)
+#       define HDMI0_AUDIO_INFO_SOURCE (1 << 6) /* 0 - sound block; 1 - hmdi regs */
+#       define HDMI0_AUDIO_INFO_UPDATE (1 << 7)
+#       define HDMI0_MPEG_INFO_SEND  (1 << 8)
+#       define HDMI0_MPEG_INFO_CONT  (1 << 9)
+#       define HDMI0_MPEG_INFO_UPDATE  (1 << 10)
+#define HDMI0_INFOFRAME_CONTROL1     0x7418
+#       define HDMI0_AVI_INFO_LINE(x)  (((x) & 0x3f) << 0)
+#       define HDMI0_AUDIO_INFO_LINE(x)  (((x) & 0x3f) << 8)
+#       define HDMI0_MPEG_INFO_LINE(x)  (((x) & 0x3f) << 16)
+#define HDMI0_GENERIC_PACKET_CONTROL 0x741c
+#       define HDMI0_GENERIC0_SEND   (1 << 0)
+#       define HDMI0_GENERIC0_CONT   (1 << 1)
+#       define HDMI0_GENERIC0_UPDATE (1 << 2)
+#       define HDMI0_GENERIC1_SEND   (1 << 4)
+#       define HDMI0_GENERIC1_CONT   (1 << 5)
+#       define HDMI0_GENERIC0_LINE(x)  (((x) & 0x3f) << 16)
+#       define HDMI0_GENERIC1_LINE(x)  (((x) & 0x3f) << 24)
+#define HDMI0_GC                     0x7428
+#       define HDMI0_GC_AVMUTE       (1 << 0)
+#define HDMI0_AVI_INFO0              0x7454
+#       define HDMI0_AVI_INFO_CHECKSUM(x)  (((x) & 0xff) << 0)
+#       define HDMI0_AVI_INFO_S(x)   (((x) & 3) << 8)
+#       define HDMI0_AVI_INFO_B(x)   (((x) & 3) << 10)
+#       define HDMI0_AVI_INFO_A(x)   (((x) & 1) << 12)
+#       define HDMI0_AVI_INFO_Y(x)   (((x) & 3) << 13)
+#       define HDMI0_AVI_INFO_Y_RGB       0
+#       define HDMI0_AVI_INFO_Y_YCBCR422  1
+#       define HDMI0_AVI_INFO_Y_YCBCR444  2
+#       define HDMI0_AVI_INFO_Y_A_B_S(x)   (((x) & 0xff) << 8)
+#       define HDMI0_AVI_INFO_R(x)   (((x) & 0xf) << 16)
+#       define HDMI0_AVI_INFO_M(x)   (((x) & 0x3) << 20)
+#       define HDMI0_AVI_INFO_C(x)   (((x) & 0x3) << 22)
+#       define HDMI0_AVI_INFO_C_M_R(x)   (((x) & 0xff) << 16)
+#       define HDMI0_AVI_INFO_SC(x)  (((x) & 0x3) << 24)
+#       define HDMI0_AVI_INFO_ITC_EC_Q_SC(x)  (((x) & 0xff) << 24)
+#define HDMI0_AVI_INFO1              0x7458
+#       define HDMI0_AVI_INFO_VIC(x) (((x) & 0x7f) << 0) /* don't use avi infoframe v1 */
+#       define HDMI0_AVI_INFO_PR(x)  (((x) & 0xf) << 8) /* don't use avi infoframe v1 */
+#       define HDMI0_AVI_INFO_TOP(x) (((x) & 0xffff) << 16)
+#define HDMI0_AVI_INFO2              0x745c
+#       define HDMI0_AVI_INFO_BOTTOM(x)  (((x) & 0xffff) << 0)
+#       define HDMI0_AVI_INFO_LEFT(x)    (((x) & 0xffff) << 16)
+#define HDMI0_AVI_INFO3              0x7460
+#       define HDMI0_AVI_INFO_RIGHT(x)    (((x) & 0xffff) << 0)
+#       define HDMI0_AVI_INFO_VERSION(x)  (((x) & 3) << 24)
+#define HDMI0_MPEG_INFO0             0x7464
+#       define HDMI0_MPEG_INFO_CHECKSUM(x)  (((x) & 0xff) << 0)
+#       define HDMI0_MPEG_INFO_MB0(x)  (((x) & 0xff) << 8)
+#       define HDMI0_MPEG_INFO_MB1(x)  (((x) & 0xff) << 16)
+#       define HDMI0_MPEG_INFO_MB2(x)  (((x) & 0xff) << 24)
+#define HDMI0_MPEG_INFO1             0x7468
+#       define HDMI0_MPEG_INFO_MB3(x)  (((x) & 0xff) << 0)
+#       define HDMI0_MPEG_INFO_MF(x)   (((x) & 3) << 8)
+#       define HDMI0_MPEG_INFO_FR(x)   (((x) & 1) << 12)
+#define HDMI0_GENERIC0_HDR           0x746c
+#define HDMI0_GENERIC0_0             0x7470
+#define HDMI0_GENERIC0_1             0x7474
+#define HDMI0_GENERIC0_2             0x7478
+#define HDMI0_GENERIC0_3             0x747c
+#define HDMI0_GENERIC0_4             0x7480
+#define HDMI0_GENERIC0_5             0x7484
+#define HDMI0_GENERIC0_6             0x7488
+#define HDMI0_GENERIC1_HDR           0x748c
+#define HDMI0_GENERIC1_0             0x7490
+#define HDMI0_GENERIC1_1             0x7494
+#define HDMI0_GENERIC1_2             0x7498
+#define HDMI0_GENERIC1_3             0x749c
+#define HDMI0_GENERIC1_4             0x74a0
+#define HDMI0_GENERIC1_5             0x74a4
+#define HDMI0_GENERIC1_6             0x74a8
+#define HDMI0_ACR_32_0               0x74ac
+#       define HDMI0_ACR_CTS_32(x)   (((x) & 0xfffff) << 12)
+#define HDMI0_ACR_32_1               0x74b0
+#       define HDMI0_ACR_N_32(x)   (((x) & 0xfffff) << 0)
+#define HDMI0_ACR_44_0               0x74b4
+#       define HDMI0_ACR_CTS_44(x)   (((x) & 0xfffff) << 12)
+#define HDMI0_ACR_44_1               0x74b8
+#       define HDMI0_ACR_N_44(x)   (((x) & 0xfffff) << 0)
+#define HDMI0_ACR_48_0               0x74bc
+#       define HDMI0_ACR_CTS_48(x)   (((x) & 0xfffff) << 12)
+#define HDMI0_ACR_48_1               0x74c0
+#       define HDMI0_ACR_N_48(x)   (((x) & 0xfffff) << 0)
+#define HDMI0_ACR_STATUS_0           0x74c4
+#define HDMI0_ACR_STATUS_1           0x74c8
+#define HDMI0_AUDIO_INFO0            0x74cc
+#       define HDMI0_AUDIO_INFO_CHECKSUM(x)  (((x) & 0xff) << 0)
+#       define HDMI0_AUDIO_INFO_CC(x)  (((x) & 7) << 8)
+#define HDMI0_AUDIO_INFO1            0x74d0
+#       define HDMI0_AUDIO_INFO_CA(x)  (((x) & 0xff) << 0)
+#       define HDMI0_AUDIO_INFO_LSV(x)  (((x) & 0xf) << 11)
+#       define HDMI0_AUDIO_INFO_DM_INH(x)  (((x) & 1) << 15)
+#       define HDMI0_AUDIO_INFO_DM_INH_LSV(x)  (((x) & 0xff) << 8)
+#define HDMI0_60958_0                0x74d4
+#       define HDMI0_60958_CS_A(x)   (((x) & 1) << 0)
+#       define HDMI0_60958_CS_B(x)   (((x) & 1) << 1)
+#       define HDMI0_60958_CS_C(x)   (((x) & 1) << 2)
+#       define HDMI0_60958_CS_D(x)   (((x) & 3) << 3)
+#       define HDMI0_60958_CS_MODE(x)   (((x) & 3) << 6)
+#       define HDMI0_60958_CS_CATEGORY_CODE(x)      (((x) & 0xff) << 8)
+#       define HDMI0_60958_CS_SOURCE_NUMBER(x)      (((x) & 0xf) << 16)
+#       define HDMI0_60958_CS_CHANNEL_NUMBER_L(x)   (((x) & 0xf) << 20)
+#       define HDMI0_60958_CS_SAMPLING_FREQUENCY(x) (((x) & 0xf) << 24)
+#       define HDMI0_60958_CS_CLOCK_ACCURACY(x)     (((x) & 3) << 28)
+#define HDMI0_60958_1                0x74d8
+#       define HDMI0_60958_CS_WORD_LENGTH(x)        (((x) & 0xf) << 0)
+#       define HDMI0_60958_CS_ORIGINAL_SAMPLING_FREQUENCY(x)   (((x) & 0xf) << 4)
+#       define HDMI0_60958_CS_VALID_L(x)   (((x) & 1) << 16)
+#       define HDMI0_60958_CS_VALID_R(x)   (((x) & 1) << 18)
+#       define HDMI0_60958_CS_CHANNEL_NUMBER_R(x)   (((x) & 0xf) << 20)
+#define HDMI0_ACR_PACKET_CONTROL     0x74dc
+#       define HDMI0_ACR_SEND        (1 << 0)
+#       define HDMI0_ACR_CONT        (1 << 1)
+#       define HDMI0_ACR_SELECT(x)   (((x) & 3) << 4)
+#       define HDMI0_ACR_HW          0
+#       define HDMI0_ACR_32          1
+#       define HDMI0_ACR_44          2
+#       define HDMI0_ACR_48          3
+#       define HDMI0_ACR_SOURCE      (1 << 8) /* 0 - hw; 1 - cts value */
+#       define HDMI0_ACR_AUTO_SEND   (1 << 12)
+#define HDMI0_RAMP_CONTROL0          0x74e0
+#       define HDMI0_RAMP_MAX_COUNT(x)   (((x) & 0xffffff) << 0)
+#define HDMI0_RAMP_CONTROL1          0x74e4
+#       define HDMI0_RAMP_MIN_COUNT(x)   (((x) & 0xffffff) << 0)
+#define HDMI0_RAMP_CONTROL2          0x74e8
+#       define HDMI0_RAMP_INC_COUNT(x)   (((x) & 0xffffff) << 0)
+#define HDMI0_RAMP_CONTROL3          0x74ec
+#       define HDMI0_RAMP_DEC_COUNT(x)   (((x) & 0xffffff) << 0)
+/* HDMI0_60958_2 is r7xx only */
+#define HDMI0_60958_2                0x74f0
+#       define HDMI0_60958_CS_CHANNEL_NUMBER_2(x)   (((x) & 0xf) << 0)
+#       define HDMI0_60958_CS_CHANNEL_NUMBER_3(x)   (((x) & 0xf) << 4)
+#       define HDMI0_60958_CS_CHANNEL_NUMBER_4(x)   (((x) & 0xf) << 8)
+#       define HDMI0_60958_CS_CHANNEL_NUMBER_5(x)   (((x) & 0xf) << 12)
+#       define HDMI0_60958_CS_CHANNEL_NUMBER_6(x)   (((x) & 0xf) << 16)
+#       define HDMI0_60958_CS_CHANNEL_NUMBER_7(x)   (((x) & 0xf) << 20)
+/* r6xx only; second instance starts at 0x7700 */
+#define HDMI1_CONTROL                0x7700
+#define HDMI1_STATUS                 0x7704
+#define HDMI1_AUDIO_PACKET_CONTROL   0x7708
+/* DCE3; second instance starts at 0x7800 NOT 0x7700 */
+#define DCE3_HDMI1_CONTROL                0x7800
+#define DCE3_HDMI1_STATUS                 0x7804
+#define DCE3_HDMI1_AUDIO_PACKET_CONTROL   0x7808
+/* DCE3.2 (for interrupts) */
+#define AFMT_STATUS                          0x7600
+#       define AFMT_AUDIO_ENABLE             (1 << 4)
+#       define AFMT_AZ_FORMAT_WTRIG          (1 << 28)
+#       define AFMT_AZ_FORMAT_WTRIG_INT      (1 << 29)
+#       define AFMT_AZ_AUDIO_ENABLE_CHG      (1 << 30)
+#define AFMT_AUDIO_PACKET_CONTROL            0x7604
+#       define AFMT_AUDIO_SAMPLE_SEND        (1 << 0)
+#       define AFMT_AUDIO_TEST_EN            (1 << 12)
+#       define AFMT_AUDIO_CHANNEL_SWAP       (1 << 24)
+#       define AFMT_60958_CS_UPDATE          (1 << 26)
+#       define AFMT_AZ_AUDIO_ENABLE_CHG_MASK (1 << 27)
+#       define AFMT_AZ_FORMAT_WTRIG_MASK     (1 << 28)
+#       define AFMT_AZ_FORMAT_WTRIG_ACK      (1 << 29)
+#       define AFMT_AZ_AUDIO_ENABLE_CHG_ACK  (1 << 30)
+
 /*
  * PM4
  */
diff --git a/drivers/gpu/drm/radeon/radeon.h b/drivers/gpu/drm/radeon/radeon.h
index 138b95216d8..1dc3a4aba02 100644
--- a/drivers/gpu/drm/radeon/radeon.h
+++ b/drivers/gpu/drm/radeon/radeon.h
@@ -94,33 +94,38 @@ extern int radeon_disp_priority;
 extern int radeon_hw_i2c;
 extern int radeon_pcie_gen2;
 extern int radeon_msi;
+extern int radeon_lockup_timeout;
 
 /*
  * Copy from radeon_drv.h so we don't have to include both and have conflicting
  * symbol;
  */
-#define RADEON_MAX_USEC_TIMEOUT		100000	/* 100 ms */
-#define RADEON_FENCE_JIFFIES_TIMEOUT	(HZ / 2)
+#define RADEON_MAX_USEC_TIMEOUT			100000	/* 100 ms */
+#define RADEON_FENCE_JIFFIES_TIMEOUT		(HZ / 2)
 /* RADEON_IB_POOL_SIZE must be a power of 2 */
-#define RADEON_IB_POOL_SIZE		16
-#define RADEON_DEBUGFS_MAX_COMPONENTS	32
-#define RADEONFB_CONN_LIMIT		4
-#define RADEON_BIOS_NUM_SCRATCH		8
+#define RADEON_IB_POOL_SIZE			16
+#define RADEON_DEBUGFS_MAX_COMPONENTS		32
+#define RADEONFB_CONN_LIMIT			4
+#define RADEON_BIOS_NUM_SCRATCH			8
 
 /* max number of rings */
-#define RADEON_NUM_RINGS 3
+#define RADEON_NUM_RINGS			3
+
+/* fence seq are set to this number when signaled */
+#define RADEON_FENCE_SIGNALED_SEQ		0LL
+#define RADEON_FENCE_NOTEMITED_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
 
 /* hardcode those limit for now */
-#define RADEON_VA_RESERVED_SIZE		(8 << 20)
-#define RADEON_IB_VM_MAX_SIZE		(64 << 10)
+#define RADEON_VA_RESERVED_SIZE			(8 << 20)
+#define RADEON_IB_VM_MAX_SIZE			(64 << 10)
 
 /*
  * Errata workarounds.
@@ -253,28 +258,20 @@ struct radeon_fence_driver {
 	uint32_t			scratch_reg;
 	uint64_t			gpu_addr;
 	volatile uint32_t		*cpu_addr;
-	atomic_t			seq;
-	uint32_t			last_seq;
-	unsigned long			last_jiffies;
-	unsigned long			last_timeout;
-	wait_queue_head_t		queue;
-	struct list_head		created;
-	struct list_head		emitted;
-	struct list_head		signaled;
+	/* seq is protected by ring emission lock */
+	uint64_t			seq;
+	atomic64_t			last_seq;
+	unsigned long			last_activity;
 	bool				initialized;
 };
 
 struct radeon_fence {
 	struct radeon_device		*rdev;
 	struct kref			kref;
-	struct list_head		list;
 	/* protected by radeon_fence.lock */
-	uint32_t			seq;
-	bool				emitted;
-	bool				signaled;
+	uint64_t			seq;
 	/* RB, DMA, etc. */
-	int				ring;
-	struct radeon_semaphore		*semaphore;
+	unsigned			ring;
 };
 
 int radeon_fence_driver_start_ring(struct radeon_device *rdev, int ring);
@@ -285,11 +282,14 @@ int radeon_fence_emit(struct radeon_device *rdev, struct radeon_fence *fence);
 void radeon_fence_process(struct radeon_device *rdev, int ring);
 bool radeon_fence_signaled(struct radeon_fence *fence);
 int radeon_fence_wait(struct radeon_fence *fence, bool interruptible);
-int radeon_fence_wait_next(struct radeon_device *rdev, int ring);
-int radeon_fence_wait_last(struct radeon_device *rdev, int ring);
+int radeon_fence_wait_next_locked(struct radeon_device *rdev, int ring);
+int radeon_fence_wait_empty_locked(struct radeon_device *rdev, int ring);
+int radeon_fence_wait_any(struct radeon_device *rdev,
+			  struct radeon_fence **fences,
+			  bool intr);
 struct radeon_fence *radeon_fence_ref(struct radeon_fence *fence);
 void radeon_fence_unref(struct radeon_fence **fence);
-int radeon_fence_count_emitted(struct radeon_device *rdev, int ring);
+unsigned radeon_fence_count_emitted(struct radeon_device *rdev, int ring);
 
 /*
  * Tiling registers
@@ -382,8 +382,11 @@ struct radeon_bo_list {
  * alignment).
  */
 struct radeon_sa_manager {
+	spinlock_t		lock;
 	struct radeon_bo	*bo;
-	struct list_head	sa_bo;
+	struct list_head	*hole;
+	struct list_head	flist[RADEON_NUM_RINGS];
+	struct list_head	olist;
 	unsigned		size;
 	uint64_t		gpu_addr;
 	void			*cpu_ptr;
@@ -394,10 +397,12 @@ struct radeon_sa_bo;
 
 /* sub-allocation buffer */
 struct radeon_sa_bo {
-	struct list_head		list;
+	struct list_head		olist;
+	struct list_head		flist;
 	struct radeon_sa_manager	*manager;
-	unsigned			offset;
-	unsigned			size;
+	unsigned			soffset;
+	unsigned			eoffset;
+	struct radeon_fence		*fence;
 };
 
 /*
@@ -428,42 +433,26 @@ int radeon_mode_dumb_destroy(struct drm_file *file_priv,
 /*
  * Semaphores.
  */
-struct radeon_ring;
-
-#define	RADEON_SEMAPHORE_BO_SIZE	256
-
-struct radeon_semaphore_driver {
-	rwlock_t			lock;
-	struct list_head		bo;
-};
-
-struct radeon_semaphore_bo;
-
 /* everything here is constant */
 struct radeon_semaphore {
-	struct list_head		list;
+	struct radeon_sa_bo		*sa_bo;
+	signed				waiters;
 	uint64_t			gpu_addr;
-	uint32_t			*cpu_ptr;
-	struct radeon_semaphore_bo	*bo;
 };
 
-struct radeon_semaphore_bo {
-	struct list_head		list;
-	struct radeon_ib		*ib;
-	struct list_head		free;
-	struct radeon_semaphore		semaphores[RADEON_SEMAPHORE_BO_SIZE/8];
-	unsigned			nused;
-};
-
-void radeon_semaphore_driver_fini(struct radeon_device *rdev);
 int radeon_semaphore_create(struct radeon_device *rdev,
 			    struct radeon_semaphore **semaphore);
 void radeon_semaphore_emit_signal(struct radeon_device *rdev, int ring,
 				  struct radeon_semaphore *semaphore);
 void radeon_semaphore_emit_wait(struct radeon_device *rdev, int ring,
 				struct radeon_semaphore *semaphore);
+int radeon_semaphore_sync_rings(struct radeon_device *rdev,
+				struct radeon_semaphore *semaphore,
+				bool sync_to[RADEON_NUM_RINGS],
+				int dst_ring);
 void radeon_semaphore_free(struct radeon_device *rdev,
-			   struct radeon_semaphore *semaphore);
+			   struct radeon_semaphore *semaphore,
+			   struct radeon_fence *fence);
 
 /*
  * GART structures, functions & helpers
@@ -560,6 +549,7 @@ struct radeon_unpin_work {
 
 struct r500_irq_stat_regs {
 	u32 disp_int;
+	u32 hdmi0_status;
 };
 
 struct r600_irq_stat_regs {
@@ -568,6 +558,8 @@ struct r600_irq_stat_regs {
 	u32 disp_int_cont2;
 	u32 d1grph_int;
 	u32 d2grph_int;
+	u32 hdmi0_status;
+	u32 hdmi1_status;
 };
 
 struct evergreen_irq_stat_regs {
@@ -583,6 +575,12 @@ struct evergreen_irq_stat_regs {
 	u32 d4grph_int;
 	u32 d5grph_int;
 	u32 d6grph_int;
+	u32 afmt_status1;
+	u32 afmt_status2;
+	u32 afmt_status3;
+	u32 afmt_status4;
+	u32 afmt_status5;
+	u32 afmt_status6;
 };
 
 union radeon_irq_stat_regs {
@@ -593,7 +591,7 @@ union radeon_irq_stat_regs {
 
 #define RADEON_MAX_HPD_PINS 6
 #define RADEON_MAX_CRTCS 6
-#define RADEON_MAX_HDMI_BLOCKS 2
+#define RADEON_MAX_AFMT_BLOCKS 6
 
 struct radeon_irq {
 	bool		installed;
@@ -605,7 +603,7 @@ struct radeon_irq {
 	bool            gui_idle;
 	bool            gui_idle_acked;
 	wait_queue_head_t	idle_queue;
-	bool		hdmi[RADEON_MAX_HDMI_BLOCKS];
+	bool		afmt[RADEON_MAX_AFMT_BLOCKS];
 	spinlock_t sw_lock;
 	int sw_refcount[RADEON_NUM_RINGS];
 	union radeon_irq_stat_regs stat_regs;
@@ -625,26 +623,14 @@ void radeon_irq_kms_pflip_irq_put(struct radeon_device *rdev, int crtc);
  */
 
 struct radeon_ib {
-	struct radeon_sa_bo	sa_bo;
-	unsigned		idx;
-	uint32_t		length_dw;
-	uint64_t		gpu_addr;
-	uint32_t		*ptr;
-	struct radeon_fence	*fence;
-	unsigned		vm_id;
-	bool			is_const_ib;
-};
-
-/*
- * locking -
- * mutex protects scheduled_ibs, ready, alloc_bm
- */
-struct radeon_ib_pool {
-	struct radeon_mutex		mutex;
-	struct radeon_sa_manager	sa_manager;
-	struct radeon_ib		ibs[RADEON_IB_POOL_SIZE];
-	bool				ready;
-	unsigned			head_id;
+	struct radeon_sa_bo		*sa_bo;
+	uint32_t			length_dw;
+	uint64_t			gpu_addr;
+	uint32_t			*ptr;
+	struct radeon_fence		*fence;
+	unsigned			vm_id;
+	bool				is_const_ib;
+	struct radeon_semaphore		*semaphore;
 };
 
 struct radeon_ring {
@@ -659,10 +645,11 @@ struct radeon_ring {
 	unsigned		ring_size;
 	unsigned		ring_free_dw;
 	int			count_dw;
+	unsigned long		last_activity;
+	unsigned		last_rptr;
 	uint64_t		gpu_addr;
 	uint32_t		align_mask;
 	uint32_t		ptr_mask;
-	struct mutex		mutex;
 	bool			ready;
 	u32			ptr_reg_shift;
 	u32			ptr_reg_mask;
@@ -679,7 +666,7 @@ struct radeon_vm {
 	unsigned			last_pfn;
 	u64				pt_gpu_addr;
 	u64				*pt;
-	struct radeon_sa_bo		sa_bo;
+	struct radeon_sa_bo		*sa_bo;
 	struct mutex			mutex;
 	/* last fence for cs using this vm */
 	struct radeon_fence		*fence;
@@ -756,7 +743,6 @@ struct r600_blit_cp_primitives {
 };
 
 struct r600_blit {
-	struct mutex		mutex;
 	struct radeon_bo	*shader_obj;
 	struct r600_blit_cp_primitives primitives;
 	int max_dim;
@@ -766,8 +752,6 @@ struct r600_blit {
 	u32 vs_offset, ps_offset;
 	u32 state_offset;
 	u32 state_len;
-	u32 vb_used, vb_total;
-	struct radeon_ib *vb_ib;
 };
 
 void r600_blit_suspend(struct radeon_device *rdev);
@@ -785,14 +769,14 @@ struct si_rlc {
 };
 
 int radeon_ib_get(struct radeon_device *rdev, int ring,
-		  struct radeon_ib **ib, unsigned size);
-void radeon_ib_free(struct radeon_device *rdev, struct radeon_ib **ib);
-bool radeon_ib_try_free(struct radeon_device *rdev, struct radeon_ib *ib);
+		  struct radeon_ib *ib, unsigned size);
+void radeon_ib_free(struct radeon_device *rdev, struct radeon_ib *ib);
 int radeon_ib_schedule(struct radeon_device *rdev, struct radeon_ib *ib);
 int radeon_ib_pool_init(struct radeon_device *rdev);
 void radeon_ib_pool_fini(struct radeon_device *rdev);
 int radeon_ib_pool_start(struct radeon_device *rdev);
 int radeon_ib_pool_suspend(struct radeon_device *rdev);
+int radeon_ib_ring_tests(struct radeon_device *rdev);
 /* Ring access between begin & end cannot sleep */
 int radeon_ring_index(struct radeon_device *rdev, struct radeon_ring *cp);
 void radeon_ring_free_size(struct radeon_device *rdev, struct radeon_ring *cp);
@@ -800,8 +784,12 @@ int radeon_ring_alloc(struct radeon_device *rdev, struct radeon_ring *cp, unsign
 int radeon_ring_lock(struct radeon_device *rdev, struct radeon_ring *cp, unsigned ndw);
 void radeon_ring_commit(struct radeon_device *rdev, struct radeon_ring *cp);
 void radeon_ring_unlock_commit(struct radeon_device *rdev, struct radeon_ring *cp);
+void radeon_ring_undo(struct radeon_ring *ring);
 void radeon_ring_unlock_undo(struct radeon_device *rdev, struct radeon_ring *cp);
 int radeon_ring_test(struct radeon_device *rdev, struct radeon_ring *cp);
+void radeon_ring_force_activity(struct radeon_device *rdev, struct radeon_ring *ring);
+void radeon_ring_lockup_update(struct radeon_ring *ring);
+bool radeon_ring_test_lockup(struct radeon_device *rdev, struct radeon_ring *ring);
 int radeon_ring_init(struct radeon_device *rdev, struct radeon_ring *cp, unsigned ring_size,
 		     unsigned rptr_offs, unsigned rptr_reg, unsigned wptr_reg,
 		     u32 ptr_reg_shift, u32 ptr_reg_mask, u32 nop);
@@ -850,8 +838,8 @@ struct radeon_cs_parser {
 	int			chunk_relocs_idx;
 	int			chunk_flags_idx;
 	int			chunk_const_ib_idx;
-	struct radeon_ib	*ib;
-	struct radeon_ib	*const_ib;
+	struct radeon_ib	ib;
+	struct radeon_ib	const_ib;
 	void			*track;
 	unsigned		family;
 	int			parser_error;
@@ -1105,6 +1093,14 @@ int radeon_pm_get_type_index(struct radeon_device *rdev,
 			     enum radeon_pm_state_type ps_type,
 			     int instance);
 
+struct r600_audio {
+	int			channels;
+	int			rate;
+	int			bits_per_sample;
+	u8			status_bits;
+	u8			category_code;
+};
+
 /*
  * Benchmarking
  */
@@ -1144,7 +1140,6 @@ struct radeon_asic {
 	int (*resume)(struct radeon_device *rdev);
 	int (*suspend)(struct radeon_device *rdev);
 	void (*vga_set_state)(struct radeon_device *rdev, bool state);
-	bool (*gpu_is_lockup)(struct radeon_device *rdev, struct radeon_ring *cp);
 	int (*asic_reset)(struct radeon_device *rdev);
 	/* ioctl hw specific callback. Some hw might want to perform special
 	 * operation on specific ioctl. For instance on wait idle some hw
@@ -1173,6 +1168,7 @@ struct radeon_asic {
 		void (*ring_start)(struct radeon_device *rdev, struct radeon_ring *cp);
 		int (*ring_test)(struct radeon_device *rdev, struct radeon_ring *cp);
 		int (*ib_test)(struct radeon_device *rdev, struct radeon_ring *cp);
+		bool (*is_lockup)(struct radeon_device *rdev, struct radeon_ring *cp);
 	} ring[RADEON_NUM_RINGS];
 	/* irqs */
 	struct {
@@ -1251,16 +1247,10 @@ struct radeon_asic {
 /*
  * Asic structures
  */
-struct r100_gpu_lockup {
-	unsigned long	last_jiffies;
-	u32		last_cp_rptr;
-};
-
 struct r100_asic {
 	const unsigned		*reg_safe_bm;
 	unsigned		reg_safe_bm_size;
 	u32			hdp_cntl;
-	struct r100_gpu_lockup	lockup;
 };
 
 struct r300_asic {
@@ -1268,7 +1258,6 @@ struct r300_asic {
 	unsigned		reg_safe_bm_size;
 	u32			resync_scratch;
 	u32			hdp_cntl;
-	struct r100_gpu_lockup	lockup;
 };
 
 struct r600_asic {
@@ -1290,7 +1279,6 @@ struct r600_asic {
 	unsigned		tiling_group_size;
 	unsigned		tile_config;
 	unsigned		backend_map;
-	struct r100_gpu_lockup	lockup;
 };
 
 struct rv770_asic {
@@ -1316,7 +1304,6 @@ struct rv770_asic {
 	unsigned		tiling_group_size;
 	unsigned		tile_config;
 	unsigned		backend_map;
-	struct r100_gpu_lockup	lockup;
 };
 
 struct evergreen_asic {
@@ -1343,7 +1330,6 @@ struct evergreen_asic {
 	unsigned tiling_group_size;
 	unsigned tile_config;
 	unsigned backend_map;
-	struct r100_gpu_lockup	lockup;
 };
 
 struct cayman_asic {
@@ -1382,7 +1368,6 @@ struct cayman_asic {
 	unsigned multi_gpu_tile_size;
 
 	unsigned tile_config;
-	struct r100_gpu_lockup	lockup;
 };
 
 struct si_asic {
@@ -1413,7 +1398,6 @@ struct si_asic {
 	unsigned multi_gpu_tile_size;
 
 	unsigned tile_config;
-	struct r100_gpu_lockup	lockup;
 };
 
 union radeon_asic_config {
@@ -1516,11 +1500,12 @@ struct radeon_device {
 	struct radeon_mode_info		mode_info;
 	struct radeon_scratch		scratch;
 	struct radeon_mman		mman;
-	rwlock_t			fence_lock;
 	struct radeon_fence_driver	fence_drv[RADEON_NUM_RINGS];
-	struct radeon_semaphore_driver	semaphore_drv;
+	wait_queue_head_t		fence_queue;
+	struct mutex			ring_lock;
 	struct radeon_ring		ring[RADEON_NUM_RINGS];
-	struct radeon_ib_pool		ib_pool;
+	bool				ib_pool_ready;
+	struct radeon_sa_manager	ring_tmp_bo;
 	struct radeon_irq		irq;
 	struct radeon_asic		*asic;
 	struct radeon_gem		gem;
@@ -1529,7 +1514,6 @@ struct radeon_device {
 	struct radeon_mutex		cs_mutex;
 	struct radeon_wb		wb;
 	struct radeon_dummy_page	dummy_page;
-	bool				gpu_lockup;
 	bool				shutdown;
 	bool				suspend;
 	bool				need_dma32;
@@ -1546,19 +1530,12 @@ struct radeon_device {
 	struct r600_ih ih; /* r6/700 interrupt ring */
 	struct si_rlc rlc;
 	struct work_struct hotplug_work;
+	struct work_struct audio_work;
 	int num_crtc; /* number of crtcs */
 	struct mutex dc_hw_i2c_mutex; /* display controller hw i2c mutex */
 	struct mutex vram_mutex;
-
-	/* audio stuff */
-	bool			audio_enabled;
-	struct timer_list	audio_timer;
-	int			audio_channels;
-	int			audio_rate;
-	int			audio_bits_per_sample;
-	uint8_t			audio_status_bits;
-	uint8_t			audio_category_code;
-
+	bool audio_enabled;
+	struct r600_audio audio_status; /* audio stuff */
 	struct notifier_block acpi_nb;
 	/* only one userspace can use Hyperz features or CMASK at a time */
 	struct drm_file *hyperz_filp;
@@ -1730,7 +1707,6 @@ void radeon_ring_write(struct radeon_ring *ring, uint32_t v);
 #define radeon_suspend(rdev) (rdev)->asic->suspend((rdev))
 #define radeon_cs_parse(rdev, r, p) (rdev)->asic->ring[(r)].cs_parse((p))
 #define radeon_vga_set_state(rdev, state) (rdev)->asic->vga_set_state((rdev), (state))
-#define radeon_gpu_is_lockup(rdev, cp) (rdev)->asic->gpu_is_lockup((rdev), (cp))
 #define radeon_asic_reset(rdev) (rdev)->asic->asic_reset((rdev))
 #define radeon_gart_tlb_flush(rdev) (rdev)->asic->gart.tlb_flush((rdev))
 #define radeon_gart_set_page(rdev, i, p) (rdev)->asic->gart.set_page((rdev), (i), (p))
@@ -1739,6 +1715,7 @@ void radeon_ring_write(struct radeon_ring *ring, uint32_t v);
 #define radeon_ib_test(rdev, r, cp) (rdev)->asic->ring[(r)].ib_test((rdev), (cp))
 #define radeon_ring_ib_execute(rdev, r, ib) (rdev)->asic->ring[(r)].ib_execute((rdev), (ib))
 #define radeon_ring_ib_parse(rdev, r, ib) (rdev)->asic->ring[(r)].ib_parse((rdev), (ib))
+#define radeon_ring_is_lockup(rdev, r, cp) (rdev)->asic->ring[(r)].is_lockup((rdev), (cp))
 #define radeon_irq_set(rdev) (rdev)->asic->irq.set((rdev))
 #define radeon_irq_process(rdev) (rdev)->asic->irq.process((rdev))
 #define radeon_get_vblank_counter(rdev, crtc) (rdev)->asic->display.get_vblank_counter((rdev), (crtc))
@@ -1828,6 +1805,8 @@ int radeon_vm_bo_rmv(struct radeon_device *rdev,
 		     struct radeon_vm *vm,
 		     struct radeon_bo *bo);
 
+/* audio */
+void r600_audio_update_hdmi(struct work_struct *work);
 
 /*
  * R600 vram scratch functions
@@ -1848,10 +1827,32 @@ int r600_fmt_get_nblocksy(u32 format, u32 h);
 /*
  * r600 functions used by radeon_encoder.c
  */
+struct radeon_hdmi_acr {
+	u32 clock;
+
+	int n_32khz;
+	int cts_32khz;
+
+	int n_44_1khz;
+	int cts_44_1khz;
+
+	int n_48khz;
+	int cts_48khz;
+
+};
+
+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);
 
+/*
+ * 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 be4dc2ff0e4..f533df5f7d5 100644
--- a/drivers/gpu/drm/radeon/radeon_asic.c
+++ b/drivers/gpu/drm/radeon/radeon_asic.c
@@ -134,7 +134,6 @@ static struct radeon_asic r100_asic = {
 	.suspend = &r100_suspend,
 	.resume = &r100_resume,
 	.vga_set_state = &r100_vga_set_state,
-	.gpu_is_lockup = &r100_gpu_is_lockup,
 	.asic_reset = &r100_asic_reset,
 	.ioctl_wait_idle = NULL,
 	.gui_idle = &r100_gui_idle,
@@ -152,6 +151,7 @@ static struct radeon_asic r100_asic = {
 			.ring_start = &r100_ring_start,
 			.ring_test = &r100_ring_test,
 			.ib_test = &r100_ib_test,
+			.is_lockup = &r100_gpu_is_lockup,
 		}
 	},
 	.irq = {
@@ -208,7 +208,6 @@ static struct radeon_asic r200_asic = {
 	.suspend = &r100_suspend,
 	.resume = &r100_resume,
 	.vga_set_state = &r100_vga_set_state,
-	.gpu_is_lockup = &r100_gpu_is_lockup,
 	.asic_reset = &r100_asic_reset,
 	.ioctl_wait_idle = NULL,
 	.gui_idle = &r100_gui_idle,
@@ -226,6 +225,7 @@ static struct radeon_asic r200_asic = {
 			.ring_start = &r100_ring_start,
 			.ring_test = &r100_ring_test,
 			.ib_test = &r100_ib_test,
+			.is_lockup = &r100_gpu_is_lockup,
 		}
 	},
 	.irq = {
@@ -282,7 +282,6 @@ static struct radeon_asic r300_asic = {
 	.suspend = &r300_suspend,
 	.resume = &r300_resume,
 	.vga_set_state = &r100_vga_set_state,
-	.gpu_is_lockup = &r300_gpu_is_lockup,
 	.asic_reset = &r300_asic_reset,
 	.ioctl_wait_idle = NULL,
 	.gui_idle = &r100_gui_idle,
@@ -300,6 +299,7 @@ static struct radeon_asic r300_asic = {
 			.ring_start = &r300_ring_start,
 			.ring_test = &r100_ring_test,
 			.ib_test = &r100_ib_test,
+			.is_lockup = &r100_gpu_is_lockup,
 		}
 	},
 	.irq = {
@@ -356,7 +356,6 @@ static struct radeon_asic r300_asic_pcie = {
 	.suspend = &r300_suspend,
 	.resume = &r300_resume,
 	.vga_set_state = &r100_vga_set_state,
-	.gpu_is_lockup = &r300_gpu_is_lockup,
 	.asic_reset = &r300_asic_reset,
 	.ioctl_wait_idle = NULL,
 	.gui_idle = &r100_gui_idle,
@@ -374,6 +373,7 @@ static struct radeon_asic r300_asic_pcie = {
 			.ring_start = &r300_ring_start,
 			.ring_test = &r100_ring_test,
 			.ib_test = &r100_ib_test,
+			.is_lockup = &r100_gpu_is_lockup,
 		}
 	},
 	.irq = {
@@ -430,7 +430,6 @@ static struct radeon_asic r420_asic = {
 	.suspend = &r420_suspend,
 	.resume = &r420_resume,
 	.vga_set_state = &r100_vga_set_state,
-	.gpu_is_lockup = &r300_gpu_is_lockup,
 	.asic_reset = &r300_asic_reset,
 	.ioctl_wait_idle = NULL,
 	.gui_idle = &r100_gui_idle,
@@ -448,6 +447,7 @@ static struct radeon_asic r420_asic = {
 			.ring_start = &r300_ring_start,
 			.ring_test = &r100_ring_test,
 			.ib_test = &r100_ib_test,
+			.is_lockup = &r100_gpu_is_lockup,
 		}
 	},
 	.irq = {
@@ -504,7 +504,6 @@ static struct radeon_asic rs400_asic = {
 	.suspend = &rs400_suspend,
 	.resume = &rs400_resume,
 	.vga_set_state = &r100_vga_set_state,
-	.gpu_is_lockup = &r300_gpu_is_lockup,
 	.asic_reset = &r300_asic_reset,
 	.ioctl_wait_idle = NULL,
 	.gui_idle = &r100_gui_idle,
@@ -522,6 +521,7 @@ static struct radeon_asic rs400_asic = {
 			.ring_start = &r300_ring_start,
 			.ring_test = &r100_ring_test,
 			.ib_test = &r100_ib_test,
+			.is_lockup = &r100_gpu_is_lockup,
 		}
 	},
 	.irq = {
@@ -578,7 +578,6 @@ static struct radeon_asic rs600_asic = {
 	.suspend = &rs600_suspend,
 	.resume = &rs600_resume,
 	.vga_set_state = &r100_vga_set_state,
-	.gpu_is_lockup = &r300_gpu_is_lockup,
 	.asic_reset = &rs600_asic_reset,
 	.ioctl_wait_idle = NULL,
 	.gui_idle = &r100_gui_idle,
@@ -596,6 +595,7 @@ static struct radeon_asic rs600_asic = {
 			.ring_start = &r300_ring_start,
 			.ring_test = &r100_ring_test,
 			.ib_test = &r100_ib_test,
+			.is_lockup = &r100_gpu_is_lockup,
 		}
 	},
 	.irq = {
@@ -652,7 +652,6 @@ static struct radeon_asic rs690_asic = {
 	.suspend = &rs690_suspend,
 	.resume = &rs690_resume,
 	.vga_set_state = &r100_vga_set_state,
-	.gpu_is_lockup = &r300_gpu_is_lockup,
 	.asic_reset = &rs600_asic_reset,
 	.ioctl_wait_idle = NULL,
 	.gui_idle = &r100_gui_idle,
@@ -670,6 +669,7 @@ static struct radeon_asic rs690_asic = {
 			.ring_start = &r300_ring_start,
 			.ring_test = &r100_ring_test,
 			.ib_test = &r100_ib_test,
+			.is_lockup = &r100_gpu_is_lockup,
 		}
 	},
 	.irq = {
@@ -726,7 +726,6 @@ static struct radeon_asic rv515_asic = {
 	.suspend = &rv515_suspend,
 	.resume = &rv515_resume,
 	.vga_set_state = &r100_vga_set_state,
-	.gpu_is_lockup = &r300_gpu_is_lockup,
 	.asic_reset = &rs600_asic_reset,
 	.ioctl_wait_idle = NULL,
 	.gui_idle = &r100_gui_idle,
@@ -744,6 +743,7 @@ static struct radeon_asic rv515_asic = {
 			.ring_start = &rv515_ring_start,
 			.ring_test = &r100_ring_test,
 			.ib_test = &r100_ib_test,
+			.is_lockup = &r100_gpu_is_lockup,
 		}
 	},
 	.irq = {
@@ -800,7 +800,6 @@ static struct radeon_asic r520_asic = {
 	.suspend = &rv515_suspend,
 	.resume = &r520_resume,
 	.vga_set_state = &r100_vga_set_state,
-	.gpu_is_lockup = &r300_gpu_is_lockup,
 	.asic_reset = &rs600_asic_reset,
 	.ioctl_wait_idle = NULL,
 	.gui_idle = &r100_gui_idle,
@@ -818,6 +817,7 @@ static struct radeon_asic r520_asic = {
 			.ring_start = &rv515_ring_start,
 			.ring_test = &r100_ring_test,
 			.ib_test = &r100_ib_test,
+			.is_lockup = &r100_gpu_is_lockup,
 		}
 	},
 	.irq = {
@@ -874,7 +874,6 @@ static struct radeon_asic r600_asic = {
 	.suspend = &r600_suspend,
 	.resume = &r600_resume,
 	.vga_set_state = &r600_vga_set_state,
-	.gpu_is_lockup = &r600_gpu_is_lockup,
 	.asic_reset = &r600_asic_reset,
 	.ioctl_wait_idle = r600_ioctl_wait_idle,
 	.gui_idle = &r600_gui_idle,
@@ -891,6 +890,7 @@ static struct radeon_asic r600_asic = {
 			.cs_parse = &r600_cs_parse,
 			.ring_test = &r600_ring_test,
 			.ib_test = &r600_ib_test,
+			.is_lockup = &r600_gpu_is_lockup,
 		}
 	},
 	.irq = {
@@ -946,7 +946,6 @@ static struct radeon_asic rs780_asic = {
 	.fini = &r600_fini,
 	.suspend = &r600_suspend,
 	.resume = &r600_resume,
-	.gpu_is_lockup = &r600_gpu_is_lockup,
 	.vga_set_state = &r600_vga_set_state,
 	.asic_reset = &r600_asic_reset,
 	.ioctl_wait_idle = r600_ioctl_wait_idle,
@@ -964,6 +963,7 @@ static struct radeon_asic rs780_asic = {
 			.cs_parse = &r600_cs_parse,
 			.ring_test = &r600_ring_test,
 			.ib_test = &r600_ib_test,
+			.is_lockup = &r600_gpu_is_lockup,
 		}
 	},
 	.irq = {
@@ -1020,7 +1020,6 @@ static struct radeon_asic rv770_asic = {
 	.suspend = &rv770_suspend,
 	.resume = &rv770_resume,
 	.asic_reset = &r600_asic_reset,
-	.gpu_is_lockup = &r600_gpu_is_lockup,
 	.vga_set_state = &r600_vga_set_state,
 	.ioctl_wait_idle = r600_ioctl_wait_idle,
 	.gui_idle = &r600_gui_idle,
@@ -1037,6 +1036,7 @@ static struct radeon_asic rv770_asic = {
 			.cs_parse = &r600_cs_parse,
 			.ring_test = &r600_ring_test,
 			.ib_test = &r600_ib_test,
+			.is_lockup = &r600_gpu_is_lockup,
 		}
 	},
 	.irq = {
@@ -1092,7 +1092,6 @@ static struct radeon_asic evergreen_asic = {
 	.fini = &evergreen_fini,
 	.suspend = &evergreen_suspend,
 	.resume = &evergreen_resume,
-	.gpu_is_lockup = &evergreen_gpu_is_lockup,
 	.asic_reset = &evergreen_asic_reset,
 	.vga_set_state = &r600_vga_set_state,
 	.ioctl_wait_idle = r600_ioctl_wait_idle,
@@ -1110,6 +1109,7 @@ static struct radeon_asic evergreen_asic = {
 			.cs_parse = &evergreen_cs_parse,
 			.ring_test = &r600_ring_test,
 			.ib_test = &r600_ib_test,
+			.is_lockup = &evergreen_gpu_is_lockup,
 		}
 	},
 	.irq = {
@@ -1165,7 +1165,6 @@ static struct radeon_asic sumo_asic = {
 	.fini = &evergreen_fini,
 	.suspend = &evergreen_suspend,
 	.resume = &evergreen_resume,
-	.gpu_is_lockup = &evergreen_gpu_is_lockup,
 	.asic_reset = &evergreen_asic_reset,
 	.vga_set_state = &r600_vga_set_state,
 	.ioctl_wait_idle = r600_ioctl_wait_idle,
@@ -1183,6 +1182,7 @@ static struct radeon_asic sumo_asic = {
 			.cs_parse = &evergreen_cs_parse,
 			.ring_test = &r600_ring_test,
 			.ib_test = &r600_ib_test,
+			.is_lockup = &evergreen_gpu_is_lockup,
 		},
 	},
 	.irq = {
@@ -1238,7 +1238,6 @@ static struct radeon_asic btc_asic = {
 	.fini = &evergreen_fini,
 	.suspend = &evergreen_suspend,
 	.resume = &evergreen_resume,
-	.gpu_is_lockup = &evergreen_gpu_is_lockup,
 	.asic_reset = &evergreen_asic_reset,
 	.vga_set_state = &r600_vga_set_state,
 	.ioctl_wait_idle = r600_ioctl_wait_idle,
@@ -1256,6 +1255,7 @@ static struct radeon_asic btc_asic = {
 			.cs_parse = &evergreen_cs_parse,
 			.ring_test = &r600_ring_test,
 			.ib_test = &r600_ib_test,
+			.is_lockup = &evergreen_gpu_is_lockup,
 		}
 	},
 	.irq = {
@@ -1321,7 +1321,6 @@ static struct radeon_asic cayman_asic = {
 	.fini = &cayman_fini,
 	.suspend = &cayman_suspend,
 	.resume = &cayman_resume,
-	.gpu_is_lockup = &cayman_gpu_is_lockup,
 	.asic_reset = &cayman_asic_reset,
 	.vga_set_state = &r600_vga_set_state,
 	.ioctl_wait_idle = r600_ioctl_wait_idle,
@@ -1340,6 +1339,7 @@ static struct radeon_asic cayman_asic = {
 			.cs_parse = &evergreen_cs_parse,
 			.ring_test = &r600_ring_test,
 			.ib_test = &r600_ib_test,
+			.is_lockup = &evergreen_gpu_is_lockup,
 		},
 		[CAYMAN_RING_TYPE_CP1_INDEX] = {
 			.ib_execute = &cayman_ring_ib_execute,
@@ -1349,6 +1349,7 @@ static struct radeon_asic cayman_asic = {
 			.cs_parse = &evergreen_cs_parse,
 			.ring_test = &r600_ring_test,
 			.ib_test = &r600_ib_test,
+			.is_lockup = &evergreen_gpu_is_lockup,
 		},
 		[CAYMAN_RING_TYPE_CP2_INDEX] = {
 			.ib_execute = &cayman_ring_ib_execute,
@@ -1358,6 +1359,7 @@ static struct radeon_asic cayman_asic = {
 			.cs_parse = &evergreen_cs_parse,
 			.ring_test = &r600_ring_test,
 			.ib_test = &r600_ib_test,
+			.is_lockup = &evergreen_gpu_is_lockup,
 		}
 	},
 	.irq = {
@@ -1413,7 +1415,6 @@ static struct radeon_asic trinity_asic = {
 	.fini = &cayman_fini,
 	.suspend = &cayman_suspend,
 	.resume = &cayman_resume,
-	.gpu_is_lockup = &cayman_gpu_is_lockup,
 	.asic_reset = &cayman_asic_reset,
 	.vga_set_state = &r600_vga_set_state,
 	.ioctl_wait_idle = r600_ioctl_wait_idle,
@@ -1432,6 +1433,7 @@ static struct radeon_asic trinity_asic = {
 			.cs_parse = &evergreen_cs_parse,
 			.ring_test = &r600_ring_test,
 			.ib_test = &r600_ib_test,
+			.is_lockup = &evergreen_gpu_is_lockup,
 		},
 		[CAYMAN_RING_TYPE_CP1_INDEX] = {
 			.ib_execute = &cayman_ring_ib_execute,
@@ -1441,6 +1443,7 @@ static struct radeon_asic trinity_asic = {
 			.cs_parse = &evergreen_cs_parse,
 			.ring_test = &r600_ring_test,
 			.ib_test = &r600_ib_test,
+			.is_lockup = &evergreen_gpu_is_lockup,
 		},
 		[CAYMAN_RING_TYPE_CP2_INDEX] = {
 			.ib_execute = &cayman_ring_ib_execute,
@@ -1450,6 +1453,7 @@ static struct radeon_asic trinity_asic = {
 			.cs_parse = &evergreen_cs_parse,
 			.ring_test = &r600_ring_test,
 			.ib_test = &r600_ib_test,
+			.is_lockup = &evergreen_gpu_is_lockup,
 		}
 	},
 	.irq = {
@@ -1515,7 +1519,6 @@ static struct radeon_asic si_asic = {
 	.fini = &si_fini,
 	.suspend = &si_suspend,
 	.resume = &si_resume,
-	.gpu_is_lockup = &si_gpu_is_lockup,
 	.asic_reset = &si_asic_reset,
 	.vga_set_state = &r600_vga_set_state,
 	.ioctl_wait_idle = r600_ioctl_wait_idle,
@@ -1534,6 +1537,7 @@ static struct radeon_asic si_asic = {
 			.cs_parse = NULL,
 			.ring_test = &r600_ring_test,
 			.ib_test = &r600_ib_test,
+			.is_lockup = &si_gpu_is_lockup,
 		},
 		[CAYMAN_RING_TYPE_CP1_INDEX] = {
 			.ib_execute = &si_ring_ib_execute,
@@ -1543,6 +1547,7 @@ static struct radeon_asic si_asic = {
 			.cs_parse = NULL,
 			.ring_test = &r600_ring_test,
 			.ib_test = &r600_ib_test,
+			.is_lockup = &si_gpu_is_lockup,
 		},
 		[CAYMAN_RING_TYPE_CP2_INDEX] = {
 			.ib_execute = &si_ring_ib_execute,
@@ -1552,6 +1557,7 @@ static struct radeon_asic si_asic = {
 			.cs_parse = NULL,
 			.ring_test = &r600_ring_test,
 			.ib_test = &r600_ib_test,
+			.is_lockup = &si_gpu_is_lockup,
 		}
 	},
 	.irq = {
diff --git a/drivers/gpu/drm/radeon/radeon_asic.h b/drivers/gpu/drm/radeon/radeon_asic.h
index 3d9f9f1d8f9..e76a941ef14 100644
--- a/drivers/gpu/drm/radeon/radeon_asic.h
+++ b/drivers/gpu/drm/radeon/radeon_asic.h
@@ -103,11 +103,6 @@ int r100_pci_gart_enable(struct radeon_device *rdev);
 void r100_pci_gart_disable(struct radeon_device *rdev);
 int r100_debugfs_mc_info_init(struct radeon_device *rdev);
 int r100_gui_wait_for_idle(struct radeon_device *rdev);
-void r100_gpu_lockup_update(struct r100_gpu_lockup *lockup,
-			    struct radeon_ring *cp);
-bool r100_gpu_cp_is_lockup(struct radeon_device *rdev,
-			   struct r100_gpu_lockup *lockup,
-			   struct radeon_ring *cp);
 void r100_ib_fini(struct radeon_device *rdev);
 int r100_ib_test(struct radeon_device *rdev, struct radeon_ring *ring);
 void r100_irq_disable(struct radeon_device *rdev);
@@ -159,7 +154,6 @@ extern int r300_init(struct radeon_device *rdev);
 extern void r300_fini(struct radeon_device *rdev);
 extern int r300_suspend(struct radeon_device *rdev);
 extern int r300_resume(struct radeon_device *rdev);
-extern bool r300_gpu_is_lockup(struct radeon_device *rdev, struct radeon_ring *cp);
 extern int r300_asic_reset(struct radeon_device *rdev);
 extern void r300_ring_start(struct radeon_device *rdev, struct radeon_ring *ring);
 extern void r300_fence_ring_emit(struct radeon_device *rdev,
@@ -362,26 +356,20 @@ 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);
-int r600_audio_tmds_index(struct drm_encoder *encoder);
 void r600_audio_set_clock(struct drm_encoder *encoder, int clock);
-int r600_audio_channels(struct radeon_device *rdev);
-int r600_audio_bits_per_sample(struct radeon_device *rdev);
-int r600_audio_rate(struct radeon_device *rdev);
-uint8_t r600_audio_status_bits(struct radeon_device *rdev);
-uint8_t r600_audio_category_code(struct radeon_device *rdev);
-void r600_audio_schedule_polling(struct radeon_device *rdev);
-void r600_audio_enable_polling(struct drm_encoder *encoder);
-void r600_audio_disable_polling(struct drm_encoder *encoder);
+struct r600_audio r600_audio_status(struct radeon_device *rdev);
 void r600_audio_fini(struct radeon_device *rdev);
-void r600_hdmi_init(struct drm_encoder *encoder);
 int r600_hdmi_buffer_status_changed(struct drm_encoder *encoder);
 void r600_hdmi_update_audio_settings(struct drm_encoder *encoder);
 /* r600 blit */
-int r600_blit_prepare_copy(struct radeon_device *rdev, unsigned num_gpu_pages);
-void r600_blit_done_copy(struct radeon_device *rdev, struct radeon_fence *fence);
+int r600_blit_prepare_copy(struct radeon_device *rdev, unsigned num_gpu_pages,
+			   struct radeon_sa_bo **vb);
+void r600_blit_done_copy(struct radeon_device *rdev, struct radeon_fence *fence,
+			 struct radeon_sa_bo *vb);
 void r600_kms_blit_copy(struct radeon_device *rdev,
 			u64 src_gpu_addr, u64 dst_gpu_addr,
-			unsigned num_gpu_pages);
+			unsigned num_gpu_pages,
+			struct radeon_sa_bo *vb);
 int r600_mc_wait_for_idle(struct radeon_device *rdev);
 
 /*
@@ -446,7 +434,6 @@ int cayman_init(struct radeon_device *rdev);
 void cayman_fini(struct radeon_device *rdev);
 int cayman_suspend(struct radeon_device *rdev);
 int cayman_resume(struct radeon_device *rdev);
-bool cayman_gpu_is_lockup(struct radeon_device *rdev, struct radeon_ring *cp);
 int cayman_asic_reset(struct radeon_device *rdev);
 void cayman_ring_ib_execute(struct radeon_device *rdev, struct radeon_ib *ib);
 int cayman_vm_init(struct radeon_device *rdev);
diff --git a/drivers/gpu/drm/radeon/radeon_benchmark.c b/drivers/gpu/drm/radeon/radeon_benchmark.c
index fef7b722b05..364f5b1a04b 100644
--- a/drivers/gpu/drm/radeon/radeon_benchmark.c
+++ b/drivers/gpu/drm/radeon/radeon_benchmark.c
@@ -103,7 +103,7 @@ static void radeon_benchmark_move(struct radeon_device *rdev, unsigned size,
 	int time;
 
 	n = RADEON_BENCHMARK_ITERATIONS;
-	r = radeon_bo_create(rdev, size, PAGE_SIZE, true, sdomain, &sobj);
+	r = radeon_bo_create(rdev, size, PAGE_SIZE, true, sdomain, NULL, &sobj);
 	if (r) {
 		goto out_cleanup;
 	}
@@ -115,7 +115,7 @@ static void radeon_benchmark_move(struct radeon_device *rdev, unsigned size,
 	if (r) {
 		goto out_cleanup;
 	}
-	r = radeon_bo_create(rdev, size, PAGE_SIZE, true, ddomain, &dobj);
+	r = radeon_bo_create(rdev, size, PAGE_SIZE, true, ddomain, NULL, &dobj);
 	if (r) {
 		goto out_cleanup;
 	}
diff --git a/drivers/gpu/drm/radeon/radeon_combios.c b/drivers/gpu/drm/radeon/radeon_combios.c
index 2cad9fde92f..576f4f6919f 100644
--- a/drivers/gpu/drm/radeon/radeon_combios.c
+++ b/drivers/gpu/drm/radeon/radeon_combios.c
@@ -1561,6 +1561,11 @@ bool radeon_get_legacy_connector_info_from_table(struct drm_device *dev)
 			   (rdev->pdev->subsystem_device == 0x4150)) {
 			/* Mac G5 tower 9600 */
 			rdev->mode_info.connector_table = CT_MAC_G5_9600;
+		} else if ((rdev->pdev->device == 0x4c66) &&
+			   (rdev->pdev->subsystem_vendor == 0x1002) &&
+			   (rdev->pdev->subsystem_device == 0x4c66)) {
+			/* SAM440ep RV250 embedded board */
+			rdev->mode_info.connector_table = CT_SAM440EP;
 		} else
 #endif /* CONFIG_PPC_PMAC */
 #ifdef CONFIG_PPC64
@@ -2134,6 +2139,67 @@ bool radeon_get_legacy_connector_info_from_table(struct drm_device *dev)
 					    CONNECTOR_OBJECT_ID_SVIDEO,
 					    &hpd);
 		break;
+	case CT_SAM440EP:
+		DRM_INFO("Connector Table: %d (SAM440ep embedded board)\n",
+			 rdev->mode_info.connector_table);
+		/* LVDS */
+		ddc_i2c = combios_setup_i2c_bus(rdev, DDC_NONE_DETECTED, 0, 0);
+		hpd.hpd = RADEON_HPD_NONE;
+		radeon_add_legacy_encoder(dev,
+					  radeon_get_encoder_enum(dev,
+								ATOM_DEVICE_LCD1_SUPPORT,
+								0),
+					  ATOM_DEVICE_LCD1_SUPPORT);
+		radeon_add_legacy_connector(dev, 0, ATOM_DEVICE_LCD1_SUPPORT,
+					    DRM_MODE_CONNECTOR_LVDS, &ddc_i2c,
+					    CONNECTOR_OBJECT_ID_LVDS,
+					    &hpd);
+		/* DVI-I - secondary dac, int tmds */
+		ddc_i2c = combios_setup_i2c_bus(rdev, DDC_DVI, 0, 0);
+		hpd.hpd = RADEON_HPD_1; /* ??? */
+		radeon_add_legacy_encoder(dev,
+					  radeon_get_encoder_enum(dev,
+								ATOM_DEVICE_DFP1_SUPPORT,
+								0),
+					  ATOM_DEVICE_DFP1_SUPPORT);
+		radeon_add_legacy_encoder(dev,
+					  radeon_get_encoder_enum(dev,
+								ATOM_DEVICE_CRT2_SUPPORT,
+								2),
+					  ATOM_DEVICE_CRT2_SUPPORT);
+		radeon_add_legacy_connector(dev, 1,
+					    ATOM_DEVICE_DFP1_SUPPORT |
+					    ATOM_DEVICE_CRT2_SUPPORT,
+					    DRM_MODE_CONNECTOR_DVII, &ddc_i2c,
+					    CONNECTOR_OBJECT_ID_SINGLE_LINK_DVI_I,
+					    &hpd);
+		/* VGA - primary dac */
+		ddc_i2c = combios_setup_i2c_bus(rdev, DDC_VGA, 0, 0);
+		hpd.hpd = RADEON_HPD_NONE;
+		radeon_add_legacy_encoder(dev,
+					  radeon_get_encoder_enum(dev,
+								ATOM_DEVICE_CRT1_SUPPORT,
+								1),
+					  ATOM_DEVICE_CRT1_SUPPORT);
+		radeon_add_legacy_connector(dev, 2,
+					    ATOM_DEVICE_CRT1_SUPPORT,
+					    DRM_MODE_CONNECTOR_VGA, &ddc_i2c,
+					    CONNECTOR_OBJECT_ID_VGA,
+					    &hpd);
+		/* TV - TV DAC */
+		ddc_i2c.valid = false;
+		hpd.hpd = RADEON_HPD_NONE;
+		radeon_add_legacy_encoder(dev,
+					  radeon_get_encoder_enum(dev,
+								ATOM_DEVICE_TV1_SUPPORT,
+								2),
+					  ATOM_DEVICE_TV1_SUPPORT);
+		radeon_add_legacy_connector(dev, 3, ATOM_DEVICE_TV1_SUPPORT,
+					    DRM_MODE_CONNECTOR_SVIDEO,
+					    &ddc_i2c,
+					    CONNECTOR_OBJECT_ID_SVIDEO,
+					    &hpd);
+		break;
 	default:
 		DRM_INFO("Connector table: %d (invalid)\n",
 			 rdev->mode_info.connector_table);
diff --git a/drivers/gpu/drm/radeon/radeon_connectors.c b/drivers/gpu/drm/radeon/radeon_connectors.c
index 3c2e7a000a2..2914c5761cf 100644
--- a/drivers/gpu/drm/radeon/radeon_connectors.c
+++ b/drivers/gpu/drm/radeon/radeon_connectors.c
@@ -84,6 +84,62 @@ static void radeon_property_change_mode(struct drm_encoder *encoder)
 					 crtc->x, crtc->y, crtc->fb);
 	}
 }
+
+int radeon_get_monitor_bpc(struct drm_connector *connector)
+{
+	struct drm_device *dev = connector->dev;
+	struct radeon_device *rdev = dev->dev_private;
+	struct radeon_connector *radeon_connector = to_radeon_connector(connector);
+	struct radeon_connector_atom_dig *dig_connector;
+	int bpc = 8;
+
+	switch (connector->connector_type) {
+	case DRM_MODE_CONNECTOR_DVII:
+	case DRM_MODE_CONNECTOR_HDMIB:
+		if (radeon_connector->use_digital) {
+			if (drm_detect_hdmi_monitor(radeon_connector->edid)) {
+				if (connector->display_info.bpc)
+					bpc = connector->display_info.bpc;
+			}
+		}
+		break;
+	case DRM_MODE_CONNECTOR_DVID:
+	case DRM_MODE_CONNECTOR_HDMIA:
+		if (drm_detect_hdmi_monitor(radeon_connector->edid)) {
+			if (connector->display_info.bpc)
+				bpc = connector->display_info.bpc;
+		}
+		break;
+	case DRM_MODE_CONNECTOR_DisplayPort:
+		dig_connector = radeon_connector->con_priv;
+		if ((dig_connector->dp_sink_type == CONNECTOR_OBJECT_ID_DISPLAYPORT) ||
+		    (dig_connector->dp_sink_type == CONNECTOR_OBJECT_ID_eDP) ||
+		    drm_detect_hdmi_monitor(radeon_connector->edid)) {
+			if (connector->display_info.bpc)
+				bpc = connector->display_info.bpc;
+		}
+		break;
+	case DRM_MODE_CONNECTOR_eDP:
+	case DRM_MODE_CONNECTOR_LVDS:
+		if (connector->display_info.bpc)
+			bpc = connector->display_info.bpc;
+		else if (ASIC_IS_DCE41(rdev) || ASIC_IS_DCE5(rdev)) {
+			struct drm_connector_helper_funcs *connector_funcs =
+				connector->helper_private;
+			struct drm_encoder *encoder = connector_funcs->best_encoder(connector);
+			struct radeon_encoder *radeon_encoder = to_radeon_encoder(encoder);
+			struct radeon_encoder_atom_dig *dig = radeon_encoder->enc_priv;
+
+			if (dig->lcd_misc & ATOM_PANEL_MISC_V13_6BIT_PER_COLOR)
+				bpc = 6;
+			else if (dig->lcd_misc & ATOM_PANEL_MISC_V13_8BIT_PER_COLOR)
+				bpc = 8;
+		}
+		break;
+	}
+	return bpc;
+}
+
 static void
 radeon_connector_update_scratch_regs(struct drm_connector *connector, enum drm_connector_status status)
 {
diff --git a/drivers/gpu/drm/radeon/radeon_cp.c b/drivers/gpu/drm/radeon/radeon_cp.c
index 0ebb7d4796f..ef67e181377 100644
--- a/drivers/gpu/drm/radeon/radeon_cp.c
+++ b/drivers/gpu/drm/radeon/radeon_cp.c
@@ -1827,14 +1827,10 @@ void radeon_do_release(struct drm_device * dev)
 			r600_do_cleanup_cp(dev);
 		else
 			radeon_do_cleanup_cp(dev);
-		if (dev_priv->me_fw) {
-			release_firmware(dev_priv->me_fw);
-			dev_priv->me_fw = NULL;
-		}
-		if (dev_priv->pfp_fw) {
-			release_firmware(dev_priv->pfp_fw);
-			dev_priv->pfp_fw = NULL;
-		}
+		release_firmware(dev_priv->me_fw);
+		dev_priv->me_fw = NULL;
+		release_firmware(dev_priv->pfp_fw);
+		dev_priv->pfp_fw = NULL;
 	}
 }
 
diff --git a/drivers/gpu/drm/radeon/radeon_cs.c b/drivers/gpu/drm/radeon/radeon_cs.c
index 5cac8327833..c7d64a73903 100644
--- a/drivers/gpu/drm/radeon/radeon_cs.c
+++ b/drivers/gpu/drm/radeon/radeon_cs.c
@@ -118,44 +118,33 @@ static int radeon_cs_get_ring(struct radeon_cs_parser *p, u32 ring, s32 priority
 static int radeon_cs_sync_rings(struct radeon_cs_parser *p)
 {
 	bool sync_to_ring[RADEON_NUM_RINGS] = { };
+	bool need_sync = false;
 	int i, r;
 
 	for (i = 0; i < p->nrelocs; i++) {
+		struct radeon_fence *fence;
+
 		if (!p->relocs[i].robj || !p->relocs[i].robj->tbo.sync_obj)
 			continue;
 
-		if (!(p->relocs[i].flags & RADEON_RELOC_DONT_SYNC)) {
-			struct radeon_fence *fence = p->relocs[i].robj->tbo.sync_obj;
-			if (!radeon_fence_signaled(fence)) {
-				sync_to_ring[fence->ring] = true;
-			}
+		fence = p->relocs[i].robj->tbo.sync_obj;
+		if (fence->ring != p->ring && !radeon_fence_signaled(fence)) {
+			sync_to_ring[fence->ring] = true;
+			need_sync = true;
 		}
 	}
 
-	for (i = 0; i < RADEON_NUM_RINGS; ++i) {
-		/* no need to sync to our own or unused rings */
-		if (i == p->ring || !sync_to_ring[i] || !p->rdev->ring[i].ready)
-			continue;
-
-		if (!p->ib->fence->semaphore) {
-			r = radeon_semaphore_create(p->rdev, &p->ib->fence->semaphore);
-			if (r)
-				return r;
-		}
-
-		r = radeon_ring_lock(p->rdev, &p->rdev->ring[i], 3);
-		if (r)
-			return r;
-		radeon_semaphore_emit_signal(p->rdev, i, p->ib->fence->semaphore);
-		radeon_ring_unlock_commit(p->rdev, &p->rdev->ring[i]);
+	if (!need_sync) {
+		return 0;
+	}
 
-		r = radeon_ring_lock(p->rdev, &p->rdev->ring[p->ring], 3);
-		if (r)
-			return r;
-		radeon_semaphore_emit_wait(p->rdev, p->ring, p->ib->fence->semaphore);
-		radeon_ring_unlock_commit(p->rdev, &p->rdev->ring[p->ring]);
+	r = radeon_semaphore_create(p->rdev, &p->ib.semaphore);
+	if (r) {
+		return r;
 	}
-	return 0;
+
+	return radeon_semaphore_sync_rings(p->rdev, p->ib.semaphore,
+					   sync_to_ring, p->ring);
 }
 
 int radeon_cs_parser_init(struct radeon_cs_parser *p, void *data)
@@ -172,6 +161,10 @@ int radeon_cs_parser_init(struct radeon_cs_parser *p, void *data)
 	/* get chunks */
 	INIT_LIST_HEAD(&p->validated);
 	p->idx = 0;
+	p->ib.sa_bo = NULL;
+	p->ib.semaphore = NULL;
+	p->const_ib.sa_bo = NULL;
+	p->const_ib.semaphore = NULL;
 	p->chunk_ib_idx = -1;
 	p->chunk_relocs_idx = -1;
 	p->chunk_flags_idx = -1;
@@ -278,11 +271,16 @@ int radeon_cs_parser_init(struct radeon_cs_parser *p, void *data)
 				  p->chunks[p->chunk_ib_idx].length_dw);
 			return -EINVAL;
 		}
-		p->chunks[p->chunk_ib_idx].kpage[0] = kmalloc(PAGE_SIZE, GFP_KERNEL);
-		p->chunks[p->chunk_ib_idx].kpage[1] = kmalloc(PAGE_SIZE, GFP_KERNEL);
-		if (p->chunks[p->chunk_ib_idx].kpage[0] == NULL ||
-		    p->chunks[p->chunk_ib_idx].kpage[1] == NULL)
-			return -ENOMEM;
+		if ((p->rdev->flags & RADEON_IS_AGP)) {
+			p->chunks[p->chunk_ib_idx].kpage[0] = kmalloc(PAGE_SIZE, GFP_KERNEL);
+			p->chunks[p->chunk_ib_idx].kpage[1] = kmalloc(PAGE_SIZE, GFP_KERNEL);
+			if (p->chunks[p->chunk_ib_idx].kpage[0] == NULL ||
+			    p->chunks[p->chunk_ib_idx].kpage[1] == NULL) {
+				kfree(p->chunks[i].kpage[0]);
+				kfree(p->chunks[i].kpage[1]);
+				return -ENOMEM;
+			}
+		}
 		p->chunks[p->chunk_ib_idx].kpage_idx[0] = -1;
 		p->chunks[p->chunk_ib_idx].kpage_idx[1] = -1;
 		p->chunks[p->chunk_ib_idx].last_copied_page = -1;
@@ -305,10 +303,9 @@ static void radeon_cs_parser_fini(struct radeon_cs_parser *parser, int error)
 {
 	unsigned i;
 
-
-	if (!error && parser->ib)
+	if (!error)
 		ttm_eu_fence_buffer_objects(&parser->validated,
-					    parser->ib->fence);
+					    parser->ib.fence);
 	else
 		ttm_eu_backoff_reservation(&parser->validated);
 
@@ -323,12 +320,15 @@ static void radeon_cs_parser_fini(struct radeon_cs_parser *parser, int error)
 	kfree(parser->relocs_ptr);
 	for (i = 0; i < parser->nchunks; i++) {
 		kfree(parser->chunks[i].kdata);
-		kfree(parser->chunks[i].kpage[0]);
-		kfree(parser->chunks[i].kpage[1]);
+		if ((parser->rdev->flags & RADEON_IS_AGP)) {
+			kfree(parser->chunks[i].kpage[0]);
+			kfree(parser->chunks[i].kpage[1]);
+		}
 	}
 	kfree(parser->chunks);
 	kfree(parser->chunks_array);
 	radeon_ib_free(parser->rdev, &parser->ib);
+	radeon_ib_free(parser->rdev, &parser->const_ib);
 }
 
 static int radeon_cs_ib_chunk(struct radeon_device *rdev,
@@ -354,7 +354,7 @@ static int radeon_cs_ib_chunk(struct radeon_device *rdev,
 		DRM_ERROR("Failed to get ib !\n");
 		return r;
 	}
-	parser->ib->length_dw = ib_chunk->length_dw;
+	parser->ib.length_dw = ib_chunk->length_dw;
 	r = radeon_cs_parse(rdev, parser->ring, parser);
 	if (r || parser->parser_error) {
 		DRM_ERROR("Invalid command stream !\n");
@@ -369,8 +369,8 @@ static int radeon_cs_ib_chunk(struct radeon_device *rdev,
 	if (r) {
 		DRM_ERROR("Failed to synchronize rings !\n");
 	}
-	parser->ib->vm_id = 0;
-	r = radeon_ib_schedule(rdev, parser->ib);
+	parser->ib.vm_id = 0;
+	r = radeon_ib_schedule(rdev, &parser->ib);
 	if (r) {
 		DRM_ERROR("Failed to schedule IB !\n");
 	}
@@ -421,14 +421,14 @@ static int radeon_cs_ib_vm_chunk(struct radeon_device *rdev,
 			DRM_ERROR("Failed to get const ib !\n");
 			return r;
 		}
-		parser->const_ib->is_const_ib = true;
-		parser->const_ib->length_dw = ib_chunk->length_dw;
+		parser->const_ib.is_const_ib = true;
+		parser->const_ib.length_dw = ib_chunk->length_dw;
 		/* Copy the packet into the IB */
-		if (DRM_COPY_FROM_USER(parser->const_ib->ptr, ib_chunk->user_ptr,
+		if (DRM_COPY_FROM_USER(parser->const_ib.ptr, ib_chunk->user_ptr,
 				       ib_chunk->length_dw * 4)) {
 			return -EFAULT;
 		}
-		r = radeon_ring_ib_parse(rdev, parser->ring, parser->const_ib);
+		r = radeon_ring_ib_parse(rdev, parser->ring, &parser->const_ib);
 		if (r) {
 			return r;
 		}
@@ -445,13 +445,13 @@ static int radeon_cs_ib_vm_chunk(struct radeon_device *rdev,
 		DRM_ERROR("Failed to get ib !\n");
 		return r;
 	}
-	parser->ib->length_dw = ib_chunk->length_dw;
+	parser->ib.length_dw = ib_chunk->length_dw;
 	/* Copy the packet into the IB */
-	if (DRM_COPY_FROM_USER(parser->ib->ptr, ib_chunk->user_ptr,
+	if (DRM_COPY_FROM_USER(parser->ib.ptr, ib_chunk->user_ptr,
 			       ib_chunk->length_dw * 4)) {
 		return -EFAULT;
 	}
-	r = radeon_ring_ib_parse(rdev, parser->ring, parser->ib);
+	r = radeon_ring_ib_parse(rdev, parser->ring, &parser->ib);
 	if (r) {
 		return r;
 	}
@@ -472,34 +472,44 @@ static int radeon_cs_ib_vm_chunk(struct radeon_device *rdev,
 
 	if ((rdev->family >= CHIP_TAHITI) &&
 	    (parser->chunk_const_ib_idx != -1)) {
-		parser->const_ib->vm_id = vm->id;
+		parser->const_ib.vm_id = vm->id;
 		/* ib pool is bind at 0 in virtual address space to gpu_addr is the
 		 * offset inside the pool bo
 		 */
-		parser->const_ib->gpu_addr = parser->const_ib->sa_bo.offset;
-		r = radeon_ib_schedule(rdev, parser->const_ib);
+		parser->const_ib.gpu_addr = parser->const_ib.sa_bo->soffset;
+		r = radeon_ib_schedule(rdev, &parser->const_ib);
 		if (r)
 			goto out;
 	}
 
-	parser->ib->vm_id = vm->id;
+	parser->ib.vm_id = vm->id;
 	/* ib pool is bind at 0 in virtual address space to gpu_addr is the
 	 * offset inside the pool bo
 	 */
-	parser->ib->gpu_addr = parser->ib->sa_bo.offset;
-	parser->ib->is_const_ib = false;
-	r = radeon_ib_schedule(rdev, parser->ib);
+	parser->ib.gpu_addr = parser->ib.sa_bo->soffset;
+	parser->ib.is_const_ib = false;
+	r = radeon_ib_schedule(rdev, &parser->ib);
 out:
 	if (!r) {
 		if (vm->fence) {
 			radeon_fence_unref(&vm->fence);
 		}
-		vm->fence = radeon_fence_ref(parser->ib->fence);
+		vm->fence = radeon_fence_ref(parser->ib.fence);
 	}
 	mutex_unlock(&fpriv->vm.mutex);
 	return r;
 }
 
+static int radeon_cs_handle_lockup(struct radeon_device *rdev, int r)
+{
+	if (r == -EDEADLK) {
+		r = radeon_gpu_reset(rdev);
+		if (!r)
+			r = -EAGAIN;
+	}
+	return r;
+}
+
 int radeon_cs_ioctl(struct drm_device *dev, void *data, struct drm_file *filp)
 {
 	struct radeon_device *rdev = dev->dev_private;
@@ -521,6 +531,7 @@ int radeon_cs_ioctl(struct drm_device *dev, void *data, struct drm_file *filp)
 	if (r) {
 		DRM_ERROR("Failed to initialize parser !\n");
 		radeon_cs_parser_fini(&parser, r);
+		r = radeon_cs_handle_lockup(rdev, r);
 		radeon_mutex_unlock(&rdev->cs_mutex);
 		return r;
 	}
@@ -529,6 +540,7 @@ int radeon_cs_ioctl(struct drm_device *dev, void *data, struct drm_file *filp)
 		if (r != -ERESTARTSYS)
 			DRM_ERROR("Failed to parse relocation %d!\n", r);
 		radeon_cs_parser_fini(&parser, r);
+		r = radeon_cs_handle_lockup(rdev, r);
 		radeon_mutex_unlock(&rdev->cs_mutex);
 		return r;
 	}
@@ -542,6 +554,7 @@ int radeon_cs_ioctl(struct drm_device *dev, void *data, struct drm_file *filp)
 	}
 out:
 	radeon_cs_parser_fini(&parser, r);
+	r = radeon_cs_handle_lockup(rdev, r);
 	radeon_mutex_unlock(&rdev->cs_mutex);
 	return r;
 }
@@ -559,7 +572,7 @@ int radeon_cs_finish_pages(struct radeon_cs_parser *p)
 				size = PAGE_SIZE;
 		}
 		
-		if (DRM_COPY_FROM_USER(p->ib->ptr + (i * (PAGE_SIZE/4)),
+		if (DRM_COPY_FROM_USER(p->ib.ptr + (i * (PAGE_SIZE/4)),
 				       ibc->user_ptr + (i * PAGE_SIZE),
 				       size))
 			return -EFAULT;
@@ -573,9 +586,10 @@ int radeon_cs_update_pages(struct radeon_cs_parser *p, int pg_idx)
 	struct radeon_cs_chunk *ibc = &p->chunks[p->chunk_ib_idx];
 	int i;
 	int size = PAGE_SIZE;
+	bool copy1 = (p->rdev->flags & RADEON_IS_AGP) ? false : true;
 
 	for (i = ibc->last_copied_page + 1; i < pg_idx; i++) {
-		if (DRM_COPY_FROM_USER(p->ib->ptr + (i * (PAGE_SIZE/4)),
+		if (DRM_COPY_FROM_USER(p->ib.ptr + (i * (PAGE_SIZE/4)),
 				       ibc->user_ptr + (i * PAGE_SIZE),
 				       PAGE_SIZE)) {
 			p->parser_error = -EFAULT;
@@ -583,14 +597,16 @@ int radeon_cs_update_pages(struct radeon_cs_parser *p, int pg_idx)
 		}
 	}
 
-	new_page = ibc->kpage_idx[0] < ibc->kpage_idx[1] ? 0 : 1;
-
 	if (pg_idx == ibc->last_page_index) {
 		size = (ibc->length_dw * 4) % PAGE_SIZE;
-			if (size == 0)
-				size = PAGE_SIZE;
+		if (size == 0)
+			size = PAGE_SIZE;
 	}
 
+	new_page = ibc->kpage_idx[0] < ibc->kpage_idx[1] ? 0 : 1;
+	if (copy1)
+		ibc->kpage[new_page] = p->ib.ptr + (pg_idx * (PAGE_SIZE / 4));
+
 	if (DRM_COPY_FROM_USER(ibc->kpage[new_page],
 			       ibc->user_ptr + (pg_idx * PAGE_SIZE),
 			       size)) {
@@ -598,8 +614,9 @@ int radeon_cs_update_pages(struct radeon_cs_parser *p, int pg_idx)
 		return 0;
 	}
 
-	/* copy to IB here */
-	memcpy((void *)(p->ib->ptr+(pg_idx*(PAGE_SIZE/4))), ibc->kpage[new_page], size);
+	/* copy to IB for non single case */
+	if (!copy1)
+		memcpy((void *)(p->ib.ptr+(pg_idx*(PAGE_SIZE/4))), ibc->kpage[new_page], size);
 
 	ibc->last_copied_page = pg_idx;
 	ibc->kpage_idx[new_page] = pg_idx;
diff --git a/drivers/gpu/drm/radeon/radeon_device.c b/drivers/gpu/drm/radeon/radeon_device.c
index 5992502a344..066c98b888a 100644
--- a/drivers/gpu/drm/radeon/radeon_device.c
+++ b/drivers/gpu/drm/radeon/radeon_device.c
@@ -193,7 +193,7 @@ int radeon_wb_init(struct radeon_device *rdev)
 
 	if (rdev->wb.wb_obj == NULL) {
 		r = radeon_bo_create(rdev, RADEON_GPU_PAGE_SIZE, PAGE_SIZE, true,
-				RADEON_GEM_DOMAIN_GTT, &rdev->wb.wb_obj);
+				     RADEON_GEM_DOMAIN_GTT, NULL, &rdev->wb.wb_obj);
 		if (r) {
 			dev_warn(rdev->dev, "(%d) create WB bo failed\n", r);
 			return r;
@@ -225,9 +225,9 @@ int radeon_wb_init(struct radeon_device *rdev)
 	/* disable event_write fences */
 	rdev->wb.use_event = false;
 	/* disabled via module param */
-	if (radeon_no_wb == 1)
+	if (radeon_no_wb == 1) {
 		rdev->wb.enabled = false;
-	else {
+	} else {
 		if (rdev->flags & RADEON_IS_AGP) {
 			/* often unreliable on AGP */
 			rdev->wb.enabled = false;
@@ -237,8 +237,9 @@ int radeon_wb_init(struct radeon_device *rdev)
 		} else {
 			rdev->wb.enabled = true;
 			/* event_write fences are only available on r600+ */
-			if (rdev->family >= CHIP_R600)
+			if (rdev->family >= CHIP_R600) {
 				rdev->wb.use_event = true;
+			}
 		}
 	}
 	/* always use writeback/events on NI, APUs */
@@ -696,6 +697,11 @@ static bool radeon_switcheroo_can_switch(struct pci_dev *pdev)
 	return can_switch;
 }
 
+static const struct vga_switcheroo_client_ops radeon_switcheroo_ops = {
+	.set_gpu_state = radeon_switcheroo_set_state,
+	.reprobe = NULL,
+	.can_switch = radeon_switcheroo_can_switch,
+};
 
 int radeon_device_init(struct radeon_device *rdev,
 		       struct drm_device *ddev,
@@ -714,7 +720,6 @@ int radeon_device_init(struct radeon_device *rdev,
 	rdev->is_atom_bios = false;
 	rdev->usec_timeout = RADEON_MAX_USEC_TIMEOUT;
 	rdev->mc.gtt_size = radeon_gart_size * 1024 * 1024;
-	rdev->gpu_lockup = false;
 	rdev->accel_working = false;
 
 	DRM_INFO("initializing kernel modesetting (%s 0x%04X:0x%04X 0x%04X:0x%04X).\n",
@@ -724,21 +729,18 @@ int radeon_device_init(struct radeon_device *rdev,
 	/* mutex initialization are all done here so we
 	 * can recall function without having locking issues */
 	radeon_mutex_init(&rdev->cs_mutex);
-	radeon_mutex_init(&rdev->ib_pool.mutex);
-	for (i = 0; i < RADEON_NUM_RINGS; ++i)
-		mutex_init(&rdev->ring[i].mutex);
+	mutex_init(&rdev->ring_lock);
 	mutex_init(&rdev->dc_hw_i2c_mutex);
 	if (rdev->family >= CHIP_R600)
 		spin_lock_init(&rdev->ih.lock);
 	mutex_init(&rdev->gem.mutex);
 	mutex_init(&rdev->pm.mutex);
 	mutex_init(&rdev->vram_mutex);
-	rwlock_init(&rdev->fence_lock);
-	rwlock_init(&rdev->semaphore_drv.lock);
-	INIT_LIST_HEAD(&rdev->gem.objects);
 	init_waitqueue_head(&rdev->irq.vblank_queue);
 	init_waitqueue_head(&rdev->irq.idle_queue);
-	INIT_LIST_HEAD(&rdev->semaphore_drv.bo);
+	r = radeon_gem_init(rdev);
+	if (r)
+		return r;
 	/* initialize vm here */
 	rdev->vm_manager.use_bitmap = 1;
 	rdev->vm_manager.max_pfn = 1 << 20;
@@ -814,10 +816,7 @@ int radeon_device_init(struct radeon_device *rdev,
 	/* this will fail for cards that aren't VGA class devices, just
 	 * ignore it */
 	vga_client_register(rdev->pdev, rdev, NULL, radeon_vga_set_decode);
-	vga_switcheroo_register_client(rdev->pdev,
-				       radeon_switcheroo_set_state,
-				       NULL,
-				       radeon_switcheroo_can_switch);
+	vga_switcheroo_register_client(rdev->pdev, &radeon_switcheroo_ops);
 
 	r = radeon_init(rdev);
 	if (r)
@@ -914,9 +913,12 @@ int radeon_suspend_kms(struct drm_device *dev, pm_message_t state)
 	}
 	/* evict vram memory */
 	radeon_bo_evict_vram(rdev);
+
+	mutex_lock(&rdev->ring_lock);
 	/* wait for gpu to finish processing current batch */
 	for (i = 0; i < RADEON_NUM_RINGS; i++)
-		radeon_fence_wait_last(rdev, i);
+		radeon_fence_wait_empty_locked(rdev, i);
+	mutex_unlock(&rdev->ring_lock);
 
 	radeon_save_bios_scratch_regs(rdev);
 
@@ -955,7 +957,6 @@ int radeon_resume_kms(struct drm_device *dev)
 		console_unlock();
 		return -1;
 	}
-	pci_set_master(dev->pdev);
 	/* resume AGP if in use */
 	radeon_agp_resume(rdev);
 	radeon_resume(rdev);
@@ -988,9 +989,6 @@ int radeon_gpu_reset(struct radeon_device *rdev)
 	int r;
 	int resched;
 
-	/* Prevent CS ioctl from interfering */
-	radeon_mutex_lock(&rdev->cs_mutex);
-
 	radeon_save_bios_scratch_regs(rdev);
 	/* block TTM */
 	resched = ttm_bo_lock_delayed_workqueue(&rdev->mman.bdev);
@@ -1005,8 +1003,6 @@ int radeon_gpu_reset(struct radeon_device *rdev)
 		ttm_bo_unlock_delayed_workqueue(&rdev->mman.bdev, resched);
 	}
 
-	radeon_mutex_unlock(&rdev->cs_mutex);
-
 	if (r) {
 		/* bad news, how to tell it to userspace ? */
 		dev_info(rdev->dev, "GPU reset failed\n");
diff --git a/drivers/gpu/drm/radeon/radeon_display.c b/drivers/gpu/drm/radeon/radeon_display.c
index 0a1d4bd65ed..64a008d1449 100644
--- a/drivers/gpu/drm/radeon/radeon_display.c
+++ b/drivers/gpu/drm/radeon/radeon_display.c
@@ -573,24 +573,6 @@ static const char *encoder_names[37] = {
 	"INTERNAL_VCE"
 };
 
-static const char *connector_names[15] = {
-	"Unknown",
-	"VGA",
-	"DVI-I",
-	"DVI-D",
-	"DVI-A",
-	"Composite",
-	"S-video",
-	"LVDS",
-	"Component",
-	"DIN",
-	"DisplayPort",
-	"HDMI-A",
-	"HDMI-B",
-	"TV",
-	"eDP",
-};
-
 static const char *hpd_names[6] = {
 	"HPD1",
 	"HPD2",
@@ -613,7 +595,7 @@ static void radeon_print_display_setup(struct drm_device *dev)
 	list_for_each_entry(connector, &dev->mode_config.connector_list, head) {
 		radeon_connector = to_radeon_connector(connector);
 		DRM_INFO("Connector %d:\n", i);
-		DRM_INFO("  %s\n", connector_names[connector->connector_type]);
+		DRM_INFO("  %s\n", drm_get_connector_name(connector));
 		if (radeon_connector->hpd.hpd != RADEON_HPD_NONE)
 			DRM_INFO("  %s\n", hpd_names[radeon_connector->hpd.hpd]);
 		if (radeon_connector->ddc_bus) {
@@ -1243,6 +1225,93 @@ void radeon_update_display_priority(struct radeon_device *rdev)
 
 }
 
+/*
+ * Allocate hdmi structs and determine register offsets
+ */
+static void radeon_afmt_init(struct radeon_device *rdev)
+{
+	int i;
+
+	for (i = 0; i < RADEON_MAX_AFMT_BLOCKS; i++)
+		rdev->mode_info.afmt[i] = NULL;
+
+	if (ASIC_IS_DCE6(rdev)) {
+		/* todo */
+	} else if (ASIC_IS_DCE4(rdev)) {
+		/* DCE4/5 has 6 audio blocks tied to DIG encoders */
+		/* DCE4.1 has 2 audio blocks tied to DIG encoders */
+		rdev->mode_info.afmt[0] = kzalloc(sizeof(struct radeon_afmt), GFP_KERNEL);
+		if (rdev->mode_info.afmt[0]) {
+			rdev->mode_info.afmt[0]->offset = EVERGREEN_CRTC0_REGISTER_OFFSET;
+			rdev->mode_info.afmt[0]->id = 0;
+		}
+		rdev->mode_info.afmt[1] = kzalloc(sizeof(struct radeon_afmt), GFP_KERNEL);
+		if (rdev->mode_info.afmt[1]) {
+			rdev->mode_info.afmt[1]->offset = EVERGREEN_CRTC1_REGISTER_OFFSET;
+			rdev->mode_info.afmt[1]->id = 1;
+		}
+		if (!ASIC_IS_DCE41(rdev)) {
+			rdev->mode_info.afmt[2] = kzalloc(sizeof(struct radeon_afmt), GFP_KERNEL);
+			if (rdev->mode_info.afmt[2]) {
+				rdev->mode_info.afmt[2]->offset = EVERGREEN_CRTC2_REGISTER_OFFSET;
+				rdev->mode_info.afmt[2]->id = 2;
+			}
+			rdev->mode_info.afmt[3] = kzalloc(sizeof(struct radeon_afmt), GFP_KERNEL);
+			if (rdev->mode_info.afmt[3]) {
+				rdev->mode_info.afmt[3]->offset = EVERGREEN_CRTC3_REGISTER_OFFSET;
+				rdev->mode_info.afmt[3]->id = 3;
+			}
+			rdev->mode_info.afmt[4] = kzalloc(sizeof(struct radeon_afmt), GFP_KERNEL);
+			if (rdev->mode_info.afmt[4]) {
+				rdev->mode_info.afmt[4]->offset = EVERGREEN_CRTC4_REGISTER_OFFSET;
+				rdev->mode_info.afmt[4]->id = 4;
+			}
+			rdev->mode_info.afmt[5] = kzalloc(sizeof(struct radeon_afmt), GFP_KERNEL);
+			if (rdev->mode_info.afmt[5]) {
+				rdev->mode_info.afmt[5]->offset = EVERGREEN_CRTC5_REGISTER_OFFSET;
+				rdev->mode_info.afmt[5]->id = 5;
+			}
+		}
+	} else if (ASIC_IS_DCE3(rdev)) {
+		/* DCE3.x has 2 audio blocks tied to DIG encoders */
+		rdev->mode_info.afmt[0] = kzalloc(sizeof(struct radeon_afmt), GFP_KERNEL);
+		if (rdev->mode_info.afmt[0]) {
+			rdev->mode_info.afmt[0]->offset = DCE3_HDMI_OFFSET0;
+			rdev->mode_info.afmt[0]->id = 0;
+		}
+		rdev->mode_info.afmt[1] = kzalloc(sizeof(struct radeon_afmt), GFP_KERNEL);
+		if (rdev->mode_info.afmt[1]) {
+			rdev->mode_info.afmt[1]->offset = DCE3_HDMI_OFFSET1;
+			rdev->mode_info.afmt[1]->id = 1;
+		}
+	} else if (ASIC_IS_DCE2(rdev)) {
+		/* DCE2 has at least 1 routable audio block */
+		rdev->mode_info.afmt[0] = kzalloc(sizeof(struct radeon_afmt), GFP_KERNEL);
+		if (rdev->mode_info.afmt[0]) {
+			rdev->mode_info.afmt[0]->offset = DCE2_HDMI_OFFSET0;
+			rdev->mode_info.afmt[0]->id = 0;
+		}
+		/* r6xx has 2 routable audio blocks */
+		if (rdev->family >= CHIP_R600) {
+			rdev->mode_info.afmt[1] = kzalloc(sizeof(struct radeon_afmt), GFP_KERNEL);
+			if (rdev->mode_info.afmt[1]) {
+				rdev->mode_info.afmt[1]->offset = DCE2_HDMI_OFFSET1;
+				rdev->mode_info.afmt[1]->id = 1;
+			}
+		}
+	}
+}
+
+static void radeon_afmt_fini(struct radeon_device *rdev)
+{
+	int i;
+
+	for (i = 0; i < RADEON_MAX_AFMT_BLOCKS; i++) {
+		kfree(rdev->mode_info.afmt[i]);
+		rdev->mode_info.afmt[i] = NULL;
+	}
+}
+
 int radeon_modeset_init(struct radeon_device *rdev)
 {
 	int i;
@@ -1251,7 +1320,7 @@ int radeon_modeset_init(struct radeon_device *rdev)
 	drm_mode_config_init(rdev->ddev);
 	rdev->mode_info.mode_config_initialized = true;
 
-	rdev->ddev->mode_config.funcs = (void *)&radeon_mode_funcs;
+	rdev->ddev->mode_config.funcs = &radeon_mode_funcs;
 
 	if (ASIC_IS_DCE5(rdev)) {
 		rdev->ddev->mode_config.max_width = 16384;
@@ -1303,6 +1372,9 @@ int radeon_modeset_init(struct radeon_device *rdev)
 	/* initialize hpd */
 	radeon_hpd_init(rdev);
 
+	/* setup afmt */
+	radeon_afmt_init(rdev);
+
 	/* Initialize power management */
 	radeon_pm_init(rdev);
 
@@ -1319,6 +1391,7 @@ void radeon_modeset_fini(struct radeon_device *rdev)
 	radeon_pm_fini(rdev);
 
 	if (rdev->mode_info.mode_config_initialized) {
+		radeon_afmt_fini(rdev);
 		drm_kms_helper_poll_fini(rdev->ddev);
 		radeon_hpd_fini(rdev);
 		drm_mode_config_cleanup(rdev->ddev);
diff --git a/drivers/gpu/drm/radeon/radeon_drv.c b/drivers/gpu/drm/radeon/radeon_drv.c
index ef7bb3f6eca..f0bb2b543b1 100644
--- a/drivers/gpu/drm/radeon/radeon_drv.c
+++ b/drivers/gpu/drm/radeon/radeon_drv.c
@@ -105,6 +105,11 @@ int radeon_mode_dumb_create(struct drm_file *file_priv,
 int radeon_mode_dumb_destroy(struct drm_file *file_priv,
 			     struct drm_device *dev,
 			     uint32_t handle);
+struct dma_buf *radeon_gem_prime_export(struct drm_device *dev,
+					struct drm_gem_object *obj,
+					int flags);
+struct drm_gem_object *radeon_gem_prime_import(struct drm_device *dev,
+					       struct dma_buf *dma_buf);
 
 #if defined(CONFIG_DEBUG_FS)
 int radeon_debugfs_init(struct drm_minor *minor);
@@ -128,6 +133,7 @@ int radeon_disp_priority = 0;
 int radeon_hw_i2c = 0;
 int radeon_pcie_gen2 = 0;
 int radeon_msi = -1;
+int radeon_lockup_timeout = 10000;
 
 MODULE_PARM_DESC(no_wb, "Disable AGP writeback for scratch registers");
 module_param_named(no_wb, radeon_no_wb, int, 0444);
@@ -177,6 +183,9 @@ module_param_named(pcie_gen2, radeon_pcie_gen2, int, 0444);
 MODULE_PARM_DESC(msi, "MSI support (1 = enable, 0 = disable, -1 = auto)");
 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);
+
 static int radeon_suspend(struct drm_device *dev, pm_message_t state)
 {
 	drm_radeon_private_t *dev_priv = dev->dev_private;
@@ -329,7 +338,8 @@ static const struct file_operations radeon_driver_kms_fops = {
 static struct drm_driver kms_driver = {
 	.driver_features =
 	    DRIVER_USE_AGP | DRIVER_USE_MTRR | DRIVER_PCI_DMA | DRIVER_SG |
-	    DRIVER_HAVE_IRQ | DRIVER_HAVE_DMA | DRIVER_IRQ_SHARED | DRIVER_GEM,
+	    DRIVER_HAVE_IRQ | DRIVER_HAVE_DMA | DRIVER_IRQ_SHARED | DRIVER_GEM |
+	    DRIVER_PRIME,
 	.dev_priv_size = 0,
 	.load = radeon_driver_load_kms,
 	.firstopen = radeon_driver_firstopen_kms,
@@ -364,6 +374,12 @@ static struct drm_driver kms_driver = {
 	.dumb_map_offset = radeon_mode_dumb_mmap,
 	.dumb_destroy = radeon_mode_dumb_destroy,
 	.fops = &radeon_driver_kms_fops,
+
+	.prime_handle_to_fd = drm_gem_prime_handle_to_fd,
+	.prime_fd_to_handle = drm_gem_prime_fd_to_handle,
+	.gem_prime_export = radeon_gem_prime_export,
+	.gem_prime_import = radeon_gem_prime_import,
+
 	.name = DRIVER_NAME,
 	.desc = DRIVER_DESC,
 	.date = DRIVER_DATE,
diff --git a/drivers/gpu/drm/radeon/radeon_fence.c b/drivers/gpu/drm/radeon/radeon_fence.c
index 4bd36a354fb..11f5f402d22 100644
--- a/drivers/gpu/drm/radeon/radeon_fence.c
+++ b/drivers/gpu/drm/radeon/radeon_fence.c
@@ -63,98 +63,82 @@ static u32 radeon_fence_read(struct radeon_device *rdev, int ring)
 
 int radeon_fence_emit(struct radeon_device *rdev, struct radeon_fence *fence)
 {
-	unsigned long irq_flags;
-
-	write_lock_irqsave(&rdev->fence_lock, irq_flags);
-	if (fence->emitted) {
-		write_unlock_irqrestore(&rdev->fence_lock, irq_flags);
+	/* we are protected by the ring emission mutex */
+	if (fence->seq && fence->seq < RADEON_FENCE_NOTEMITED_SEQ) {
 		return 0;
 	}
-	fence->seq = atomic_add_return(1, &rdev->fence_drv[fence->ring].seq);
-	if (!rdev->ring[fence->ring].ready)
-		/* FIXME: cp is not running assume everythings is done right
-		 * away
-		 */
-		radeon_fence_write(rdev, fence->seq, fence->ring);
-	else
-		radeon_fence_ring_emit(rdev, fence->ring, fence);
-
+	fence->seq = ++rdev->fence_drv[fence->ring].seq;
+	radeon_fence_ring_emit(rdev, fence->ring, fence);
 	trace_radeon_fence_emit(rdev->ddev, fence->seq);
-	fence->emitted = true;
-	list_move_tail(&fence->list, &rdev->fence_drv[fence->ring].emitted);
-	write_unlock_irqrestore(&rdev->fence_lock, irq_flags);
 	return 0;
 }
 
-static bool radeon_fence_poll_locked(struct radeon_device *rdev, int ring)
+void radeon_fence_process(struct radeon_device *rdev, int ring)
 {
-	struct radeon_fence *fence;
-	struct list_head *i, *n;
-	uint32_t seq;
+	uint64_t seq, last_seq;
+	unsigned count_loop = 0;
 	bool wake = false;
-	unsigned long cjiffies;
 
-	seq = radeon_fence_read(rdev, ring);
-	if (seq != rdev->fence_drv[ring].last_seq) {
-		rdev->fence_drv[ring].last_seq = seq;
-		rdev->fence_drv[ring].last_jiffies = jiffies;
-		rdev->fence_drv[ring].last_timeout = RADEON_FENCE_JIFFIES_TIMEOUT;
-	} else {
-		cjiffies = jiffies;
-		if (time_after(cjiffies, rdev->fence_drv[ring].last_jiffies)) {
-			cjiffies -= rdev->fence_drv[ring].last_jiffies;
-			if (time_after(rdev->fence_drv[ring].last_timeout, cjiffies)) {
-				/* update the timeout */
-				rdev->fence_drv[ring].last_timeout -= cjiffies;
-			} else {
-				/* the 500ms timeout is elapsed we should test
-				 * for GPU lockup
-				 */
-				rdev->fence_drv[ring].last_timeout = 1;
-			}
-		} else {
-			/* wrap around update last jiffies, we will just wait
-			 * a little longer
-			 */
-			rdev->fence_drv[ring].last_jiffies = cjiffies;
+	/* Note there is a scenario here for an infinite loop but it's
+	 * very unlikely to happen. For it to happen, the current polling
+	 * process need to be interrupted by another process and another
+	 * process needs to update the last_seq btw the atomic read and
+	 * xchg of the current process.
+	 *
+	 * More over for this to go in infinite loop there need to be
+	 * continuously new fence signaled ie radeon_fence_read needs
+	 * to return a different value each time for both the currently
+	 * polling process and the other process that xchg the last_seq
+	 * btw atomic read and xchg of the current process. And the
+	 * value the other process set as last seq must be higher than
+	 * the seq value we just read. Which means that current process
+	 * need to be interrupted after radeon_fence_read and before
+	 * atomic xchg.
+	 *
+	 * To be even more safe we count the number of time we loop and
+	 * we bail after 10 loop just accepting the fact that we might
+	 * have temporarly set the last_seq not to the true real last
+	 * seq but to an older one.
+	 */
+	last_seq = atomic64_read(&rdev->fence_drv[ring].last_seq);
+	do {
+		seq = radeon_fence_read(rdev, ring);
+		seq |= last_seq & 0xffffffff00000000LL;
+		if (seq < last_seq) {
+			seq += 0x100000000LL;
 		}
-		return false;
-	}
-	n = NULL;
-	list_for_each(i, &rdev->fence_drv[ring].emitted) {
-		fence = list_entry(i, struct radeon_fence, list);
-		if (fence->seq == seq) {
-			n = i;
+
+		if (seq == last_seq) {
 			break;
 		}
-	}
-	/* all fence previous to this one are considered as signaled */
-	if (n) {
-		i = n;
-		do {
-			n = i->prev;
-			list_move_tail(i, &rdev->fence_drv[ring].signaled);
-			fence = list_entry(i, struct radeon_fence, list);
-			fence->signaled = true;
-			i = n;
-		} while (i != &rdev->fence_drv[ring].emitted);
+		/* If we loop over we don't want to return without
+		 * checking if a fence is signaled as it means that the
+		 * seq we just read is different from the previous on.
+		 */
 		wake = true;
+		last_seq = seq;
+		if ((count_loop++) > 10) {
+			/* We looped over too many time leave with the
+			 * fact that we might have set an older fence
+			 * seq then the current real last seq as signaled
+			 * by the hw.
+			 */
+			break;
+		}
+	} while (atomic64_xchg(&rdev->fence_drv[ring].last_seq, seq) > seq);
+
+	if (wake) {
+		rdev->fence_drv[ring].last_activity = jiffies;
+		wake_up_all(&rdev->fence_queue);
 	}
-	return wake;
 }
 
 static void radeon_fence_destroy(struct kref *kref)
 {
-	unsigned long irq_flags;
-        struct radeon_fence *fence;
+	struct radeon_fence *fence;
 
 	fence = container_of(kref, struct radeon_fence, kref);
-	write_lock_irqsave(&fence->rdev->fence_lock, irq_flags);
-	list_del(&fence->list);
-	fence->emitted = false;
-	write_unlock_irqrestore(&fence->rdev->fence_lock, irq_flags);
-	if (fence->semaphore)
-		radeon_semaphore_free(fence->rdev, fence->semaphore);
+	fence->seq = RADEON_FENCE_NOTEMITED_SEQ;
 	kfree(fence);
 }
 
@@ -162,171 +146,342 @@ int radeon_fence_create(struct radeon_device *rdev,
 			struct radeon_fence **fence,
 			int ring)
 {
-	unsigned long irq_flags;
-
 	*fence = kmalloc(sizeof(struct radeon_fence), GFP_KERNEL);
 	if ((*fence) == NULL) {
 		return -ENOMEM;
 	}
 	kref_init(&((*fence)->kref));
 	(*fence)->rdev = rdev;
-	(*fence)->emitted = false;
-	(*fence)->signaled = false;
-	(*fence)->seq = 0;
+	(*fence)->seq = RADEON_FENCE_NOTEMITED_SEQ;
 	(*fence)->ring = ring;
-	(*fence)->semaphore = NULL;
-	INIT_LIST_HEAD(&(*fence)->list);
-
-	write_lock_irqsave(&rdev->fence_lock, irq_flags);
-	list_add_tail(&(*fence)->list, &rdev->fence_drv[ring].created);
-	write_unlock_irqrestore(&rdev->fence_lock, irq_flags);
 	return 0;
 }
 
-bool radeon_fence_signaled(struct radeon_fence *fence)
+static bool radeon_fence_seq_signaled(struct radeon_device *rdev,
+				      u64 seq, unsigned ring)
 {
-	unsigned long irq_flags;
-	bool signaled = false;
-
-	if (!fence)
+	if (atomic64_read(&rdev->fence_drv[ring].last_seq) >= seq) {
 		return true;
-
-	if (fence->rdev->gpu_lockup)
+	}
+	/* poll new last sequence at least once */
+	radeon_fence_process(rdev, ring);
+	if (atomic64_read(&rdev->fence_drv[ring].last_seq) >= seq) {
 		return true;
+	}
+	return false;
+}
 
-	write_lock_irqsave(&fence->rdev->fence_lock, irq_flags);
-	signaled = fence->signaled;
-	/* if we are shuting down report all fence as signaled */
-	if (fence->rdev->shutdown) {
-		signaled = true;
+bool radeon_fence_signaled(struct radeon_fence *fence)
+{
+	if (!fence) {
+		return true;
 	}
-	if (!fence->emitted) {
+	if (fence->seq == RADEON_FENCE_NOTEMITED_SEQ) {
 		WARN(1, "Querying an unemitted fence : %p !\n", fence);
-		signaled = true;
+		return true;
 	}
-	if (!signaled) {
-		radeon_fence_poll_locked(fence->rdev, fence->ring);
-		signaled = fence->signaled;
+	if (fence->seq == RADEON_FENCE_SIGNALED_SEQ) {
+		return true;
+	}
+	if (radeon_fence_seq_signaled(fence->rdev, fence->seq, fence->ring)) {
+		fence->seq = RADEON_FENCE_SIGNALED_SEQ;
+		return true;
 	}
-	write_unlock_irqrestore(&fence->rdev->fence_lock, irq_flags);
-	return signaled;
+	return false;
+}
+
+static int radeon_fence_wait_seq(struct radeon_device *rdev, u64 target_seq,
+				 unsigned ring, bool intr, bool lock_ring)
+{
+	unsigned long timeout, last_activity;
+	uint64_t seq;
+	unsigned i;
+	bool signaled;
+	int r;
+
+	while (target_seq > atomic64_read(&rdev->fence_drv[ring].last_seq)) {
+		if (!rdev->ring[ring].ready) {
+			return -EBUSY;
+		}
+
+		timeout = jiffies - RADEON_FENCE_JIFFIES_TIMEOUT;
+		if (time_after(rdev->fence_drv[ring].last_activity, timeout)) {
+			/* the normal case, timeout is somewhere before last_activity */
+			timeout = rdev->fence_drv[ring].last_activity - timeout;
+		} else {
+			/* either jiffies wrapped around, or no fence was signaled in the last 500ms
+			 * anyway we will just wait for the minimum amount and then check for a lockup
+			 */
+			timeout = 1;
+		}
+		seq = atomic64_read(&rdev->fence_drv[ring].last_seq);
+		/* Save current last activity valuee, used to check for GPU lockups */
+		last_activity = rdev->fence_drv[ring].last_activity;
+
+		trace_radeon_fence_wait_begin(rdev->ddev, seq);
+		radeon_irq_kms_sw_irq_get(rdev, ring);
+		if (intr) {
+			r = wait_event_interruptible_timeout(rdev->fence_queue,
+				(signaled = radeon_fence_seq_signaled(rdev, target_seq, ring)),
+				timeout);
+                } else {
+			r = wait_event_timeout(rdev->fence_queue,
+				(signaled = radeon_fence_seq_signaled(rdev, target_seq, ring)),
+				timeout);
+		}
+		radeon_irq_kms_sw_irq_put(rdev, ring);
+		if (unlikely(r < 0)) {
+			return r;
+		}
+		trace_radeon_fence_wait_end(rdev->ddev, seq);
+
+		if (unlikely(!signaled)) {
+			/* we were interrupted for some reason and fence
+			 * isn't signaled yet, resume waiting */
+			if (r) {
+				continue;
+			}
+
+			/* check if sequence value has changed since last_activity */
+			if (seq != atomic64_read(&rdev->fence_drv[ring].last_seq)) {
+				continue;
+			}
+
+			if (lock_ring) {
+				mutex_lock(&rdev->ring_lock);
+			}
+
+			/* test if somebody else has already decided that this is a lockup */
+			if (last_activity != rdev->fence_drv[ring].last_activity) {
+				if (lock_ring) {
+					mutex_unlock(&rdev->ring_lock);
+				}
+				continue;
+			}
+
+			if (radeon_ring_is_lockup(rdev, ring, &rdev->ring[ring])) {
+				/* good news we believe it's a lockup */
+				dev_warn(rdev->dev, "GPU lockup (waiting for 0x%016llx last fence id 0x%016llx)\n",
+					 target_seq, seq);
+
+				/* change last activity so nobody else think there is a lockup */
+				for (i = 0; i < RADEON_NUM_RINGS; ++i) {
+					rdev->fence_drv[i].last_activity = jiffies;
+				}
+
+				/* mark the ring as not ready any more */
+				rdev->ring[ring].ready = false;
+				if (lock_ring) {
+					mutex_unlock(&rdev->ring_lock);
+				}
+				return -EDEADLK;
+			}
+
+			if (lock_ring) {
+				mutex_unlock(&rdev->ring_lock);
+			}
+		}
+	}
+	return 0;
 }
 
 int radeon_fence_wait(struct radeon_fence *fence, bool intr)
 {
-	struct radeon_device *rdev;
-	unsigned long irq_flags, timeout;
-	u32 seq;
 	int r;
 
 	if (fence == NULL) {
 		WARN(1, "Querying an invalid fence : %p !\n", fence);
-		return 0;
+		return -EINVAL;
 	}
-	rdev = fence->rdev;
-	if (radeon_fence_signaled(fence)) {
-		return 0;
+
+	r = radeon_fence_wait_seq(fence->rdev, fence->seq,
+				  fence->ring, intr, true);
+	if (r) {
+		return r;
 	}
-	timeout = rdev->fence_drv[fence->ring].last_timeout;
-retry:
-	/* save current sequence used to check for GPU lockup */
-	seq = rdev->fence_drv[fence->ring].last_seq;
-	trace_radeon_fence_wait_begin(rdev->ddev, seq);
-	if (intr) {
-		radeon_irq_kms_sw_irq_get(rdev, fence->ring);
-		r = wait_event_interruptible_timeout(rdev->fence_drv[fence->ring].queue,
-				radeon_fence_signaled(fence), timeout);
-		radeon_irq_kms_sw_irq_put(rdev, fence->ring);
-		if (unlikely(r < 0)) {
-			return r;
+	fence->seq = RADEON_FENCE_SIGNALED_SEQ;
+	return 0;
+}
+
+bool radeon_fence_any_seq_signaled(struct radeon_device *rdev, u64 *seq)
+{
+	unsigned i;
+
+	for (i = 0; i < RADEON_NUM_RINGS; ++i) {
+		if (seq[i] && radeon_fence_seq_signaled(rdev, seq[i], i)) {
+			return true;
 		}
-	} else {
-		radeon_irq_kms_sw_irq_get(rdev, fence->ring);
-		r = wait_event_timeout(rdev->fence_drv[fence->ring].queue,
-			 radeon_fence_signaled(fence), timeout);
-		radeon_irq_kms_sw_irq_put(rdev, fence->ring);
 	}
-	trace_radeon_fence_wait_end(rdev->ddev, seq);
-	if (unlikely(!radeon_fence_signaled(fence))) {
-		/* we were interrupted for some reason and fence isn't
-		 * isn't signaled yet, resume wait
-		 */
-		if (r) {
-			timeout = r;
-			goto retry;
+	return false;
+}
+
+static int radeon_fence_wait_any_seq(struct radeon_device *rdev,
+				     u64 *target_seq, bool intr)
+{
+	unsigned long timeout, last_activity, tmp;
+	unsigned i, ring = RADEON_NUM_RINGS;
+	bool signaled;
+	int r;
+
+	for (i = 0, last_activity = 0; i < RADEON_NUM_RINGS; ++i) {
+		if (!target_seq[i]) {
+			continue;
+		}
+
+		/* use the most recent one as indicator */
+		if (time_after(rdev->fence_drv[i].last_activity, last_activity)) {
+			last_activity = rdev->fence_drv[i].last_activity;
 		}
-		/* don't protect read access to rdev->fence_drv[t].last_seq
-		 * if we experiencing a lockup the value doesn't change
+
+		/* For lockup detection just pick the lowest ring we are
+		 * actively waiting for
 		 */
-		if (seq == rdev->fence_drv[fence->ring].last_seq &&
-		    radeon_gpu_is_lockup(rdev, &rdev->ring[fence->ring])) {
-			/* good news we believe it's a lockup */
-			printk(KERN_WARNING "GPU lockup (waiting for 0x%08X last fence id 0x%08X)\n",
-			     fence->seq, seq);
-			/* FIXME: what should we do ? marking everyone
-			 * as signaled for now
+		if (i < ring) {
+			ring = i;
+		}
+	}
+
+	/* nothing to wait for ? */
+	if (ring == RADEON_NUM_RINGS) {
+		return 0;
+	}
+
+	while (!radeon_fence_any_seq_signaled(rdev, target_seq)) {
+		timeout = jiffies - RADEON_FENCE_JIFFIES_TIMEOUT;
+		if (time_after(last_activity, timeout)) {
+			/* the normal case, timeout is somewhere before last_activity */
+			timeout = last_activity - timeout;
+		} else {
+			/* either jiffies wrapped around, or no fence was signaled in the last 500ms
+			 * anyway we will just wait for the minimum amount and then check for a lockup
 			 */
-			rdev->gpu_lockup = true;
-			r = radeon_gpu_reset(rdev);
-			if (r)
-				return r;
-			radeon_fence_write(rdev, fence->seq, fence->ring);
-			rdev->gpu_lockup = false;
+			timeout = 1;
+		}
+
+		trace_radeon_fence_wait_begin(rdev->ddev, target_seq[ring]);
+		for (i = 0; i < RADEON_NUM_RINGS; ++i) {
+			if (target_seq[i]) {
+				radeon_irq_kms_sw_irq_get(rdev, i);
+			}
+		}
+		if (intr) {
+			r = wait_event_interruptible_timeout(rdev->fence_queue,
+				(signaled = radeon_fence_any_seq_signaled(rdev, target_seq)),
+				timeout);
+		} else {
+			r = wait_event_timeout(rdev->fence_queue,
+				(signaled = radeon_fence_any_seq_signaled(rdev, target_seq)),
+				timeout);
+		}
+		for (i = 0; i < RADEON_NUM_RINGS; ++i) {
+			if (target_seq[i]) {
+				radeon_irq_kms_sw_irq_put(rdev, i);
+			}
+		}
+		if (unlikely(r < 0)) {
+			return r;
+		}
+		trace_radeon_fence_wait_end(rdev->ddev, target_seq[ring]);
+
+		if (unlikely(!signaled)) {
+			/* we were interrupted for some reason and fence
+			 * isn't signaled yet, resume waiting */
+			if (r) {
+				continue;
+			}
+
+			mutex_lock(&rdev->ring_lock);
+			for (i = 0, tmp = 0; i < RADEON_NUM_RINGS; ++i) {
+				if (time_after(rdev->fence_drv[i].last_activity, tmp)) {
+					tmp = rdev->fence_drv[i].last_activity;
+				}
+			}
+			/* test if somebody else has already decided that this is a lockup */
+			if (last_activity != tmp) {
+				last_activity = tmp;
+				mutex_unlock(&rdev->ring_lock);
+				continue;
+			}
+
+			if (radeon_ring_is_lockup(rdev, ring, &rdev->ring[ring])) {
+				/* good news we believe it's a lockup */
+				dev_warn(rdev->dev, "GPU lockup (waiting for 0x%016llx)\n",
+					 target_seq[ring]);
+
+				/* change last activity so nobody else think there is a lockup */
+				for (i = 0; i < RADEON_NUM_RINGS; ++i) {
+					rdev->fence_drv[i].last_activity = jiffies;
+				}
+
+				/* mark the ring as not ready any more */
+				rdev->ring[ring].ready = false;
+				mutex_unlock(&rdev->ring_lock);
+				return -EDEADLK;
+			}
+			mutex_unlock(&rdev->ring_lock);
 		}
-		timeout = RADEON_FENCE_JIFFIES_TIMEOUT;
-		write_lock_irqsave(&rdev->fence_lock, irq_flags);
-		rdev->fence_drv[fence->ring].last_timeout = RADEON_FENCE_JIFFIES_TIMEOUT;
-		rdev->fence_drv[fence->ring].last_jiffies = jiffies;
-		write_unlock_irqrestore(&rdev->fence_lock, irq_flags);
-		goto retry;
 	}
 	return 0;
 }
 
-int radeon_fence_wait_next(struct radeon_device *rdev, int ring)
+int radeon_fence_wait_any(struct radeon_device *rdev,
+			  struct radeon_fence **fences,
+			  bool intr)
 {
-	unsigned long irq_flags;
-	struct radeon_fence *fence;
+	uint64_t seq[RADEON_NUM_RINGS];
+	unsigned i;
 	int r;
 
-	if (rdev->gpu_lockup) {
-		return 0;
+	for (i = 0; i < RADEON_NUM_RINGS; ++i) {
+		seq[i] = 0;
+
+		if (!fences[i]) {
+			continue;
+		}
+
+		if (fences[i]->seq == RADEON_FENCE_SIGNALED_SEQ) {
+			/* something was allready signaled */
+			return 0;
+		}
+
+		if (fences[i]->seq < RADEON_FENCE_NOTEMITED_SEQ) {
+			seq[i] = fences[i]->seq;
+		}
 	}
-	write_lock_irqsave(&rdev->fence_lock, irq_flags);
-	if (list_empty(&rdev->fence_drv[ring].emitted)) {
-		write_unlock_irqrestore(&rdev->fence_lock, irq_flags);
-		return 0;
+
+	r = radeon_fence_wait_any_seq(rdev, seq, intr);
+	if (r) {
+		return r;
 	}
-	fence = list_entry(rdev->fence_drv[ring].emitted.next,
-			   struct radeon_fence, list);
-	radeon_fence_ref(fence);
-	write_unlock_irqrestore(&rdev->fence_lock, irq_flags);
-	r = radeon_fence_wait(fence, false);
-	radeon_fence_unref(&fence);
-	return r;
+	return 0;
 }
 
-int radeon_fence_wait_last(struct radeon_device *rdev, int ring)
+int radeon_fence_wait_next_locked(struct radeon_device *rdev, int ring)
 {
-	unsigned long irq_flags;
-	struct radeon_fence *fence;
-	int r;
-
-	if (rdev->gpu_lockup) {
-		return 0;
+	uint64_t seq;
+
+	/* We are not protected by ring lock when reading current seq but
+	 * it's ok as worst case is we return to early while we could have
+	 * wait.
+	 */
+	seq = atomic64_read(&rdev->fence_drv[ring].last_seq) + 1ULL;
+	if (seq >= rdev->fence_drv[ring].seq) {
+		/* nothing to wait for, last_seq is
+		   already the last emited fence */
+		return -ENOENT;
 	}
-	write_lock_irqsave(&rdev->fence_lock, irq_flags);
-	if (list_empty(&rdev->fence_drv[ring].emitted)) {
-		write_unlock_irqrestore(&rdev->fence_lock, irq_flags);
-		return 0;
-	}
-	fence = list_entry(rdev->fence_drv[ring].emitted.prev,
-			   struct radeon_fence, list);
-	radeon_fence_ref(fence);
-	write_unlock_irqrestore(&rdev->fence_lock, irq_flags);
-	r = radeon_fence_wait(fence, false);
-	radeon_fence_unref(&fence);
-	return r;
+	return radeon_fence_wait_seq(rdev, seq, ring, false, false);
+}
+
+int radeon_fence_wait_empty_locked(struct radeon_device *rdev, int ring)
+{
+	/* We are not protected by ring lock when reading current seq
+	 * but it's ok as wait empty is call from place where no more
+	 * activity can be scheduled so there won't be concurrent access
+	 * to seq value.
+	 */
+	return radeon_fence_wait_seq(rdev, rdev->fence_drv[ring].seq,
+				     ring, false, false);
 }
 
 struct radeon_fence *radeon_fence_ref(struct radeon_fence *fence)
@@ -345,49 +500,27 @@ void radeon_fence_unref(struct radeon_fence **fence)
 	}
 }
 
-void radeon_fence_process(struct radeon_device *rdev, int ring)
-{
-	unsigned long irq_flags;
-	bool wake;
-
-	write_lock_irqsave(&rdev->fence_lock, irq_flags);
-	wake = radeon_fence_poll_locked(rdev, ring);
-	write_unlock_irqrestore(&rdev->fence_lock, irq_flags);
-	if (wake) {
-		wake_up_all(&rdev->fence_drv[ring].queue);
-	}
-}
-
-int radeon_fence_count_emitted(struct radeon_device *rdev, int ring)
+unsigned radeon_fence_count_emitted(struct radeon_device *rdev, int ring)
 {
-	unsigned long irq_flags;
-	int not_processed = 0;
-
-	read_lock_irqsave(&rdev->fence_lock, irq_flags);
-	if (!rdev->fence_drv[ring].initialized) {
-		read_unlock_irqrestore(&rdev->fence_lock, irq_flags);
-		return 0;
+	uint64_t emitted;
+
+	/* We are not protected by ring lock when reading the last sequence
+	 * but it's ok to report slightly wrong fence count here.
+	 */
+	radeon_fence_process(rdev, ring);
+	emitted = rdev->fence_drv[ring].seq - atomic64_read(&rdev->fence_drv[ring].last_seq);
+	/* to avoid 32bits warp around */
+	if (emitted > 0x10000000) {
+		emitted = 0x10000000;
 	}
-
-	if (!list_empty(&rdev->fence_drv[ring].emitted)) {
-		struct list_head *ptr;
-		list_for_each(ptr, &rdev->fence_drv[ring].emitted) {
-			/* count up to 3, that's enought info */
-			if (++not_processed >= 3)
-				break;
-		}
-	}
-	read_unlock_irqrestore(&rdev->fence_lock, irq_flags);
-	return not_processed;
+	return (unsigned)emitted;
 }
 
 int radeon_fence_driver_start_ring(struct radeon_device *rdev, int ring)
 {
-	unsigned long irq_flags;
 	uint64_t index;
 	int r;
 
-	write_lock_irqsave(&rdev->fence_lock, irq_flags);
 	radeon_scratch_free(rdev, rdev->fence_drv[ring].scratch_reg);
 	if (rdev->wb.use_event) {
 		rdev->fence_drv[ring].scratch_reg = 0;
@@ -396,7 +529,6 @@ int radeon_fence_driver_start_ring(struct radeon_device *rdev, int ring)
 		r = radeon_scratch_get(rdev, &rdev->fence_drv[ring].scratch_reg);
 		if (r) {
 			dev_err(rdev->dev, "fence failed to get scratch register\n");
-			write_unlock_irqrestore(&rdev->fence_lock, irq_flags);
 			return r;
 		}
 		index = RADEON_WB_SCRATCH_OFFSET +
@@ -405,11 +537,10 @@ int radeon_fence_driver_start_ring(struct radeon_device *rdev, int ring)
 	}
 	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, atomic_read(&rdev->fence_drv[ring].seq), ring);
+	radeon_fence_write(rdev, rdev->fence_drv[ring].seq, ring);
 	rdev->fence_drv[ring].initialized = true;
-	DRM_INFO("fence driver on ring %d use gpu addr 0x%08Lx and cpu addr 0x%p\n",
+	dev_info(rdev->dev, "fence driver on ring %d use gpu addr 0x%016llx and cpu addr 0x%p\n",
 		 ring, rdev->fence_drv[ring].gpu_addr, rdev->fence_drv[ring].cpu_addr);
-	write_unlock_irqrestore(&rdev->fence_lock, irq_flags);
 	return 0;
 }
 
@@ -418,24 +549,20 @@ static void radeon_fence_driver_init_ring(struct radeon_device *rdev, int ring)
 	rdev->fence_drv[ring].scratch_reg = -1;
 	rdev->fence_drv[ring].cpu_addr = NULL;
 	rdev->fence_drv[ring].gpu_addr = 0;
-	atomic_set(&rdev->fence_drv[ring].seq, 0);
-	INIT_LIST_HEAD(&rdev->fence_drv[ring].created);
-	INIT_LIST_HEAD(&rdev->fence_drv[ring].emitted);
-	INIT_LIST_HEAD(&rdev->fence_drv[ring].signaled);
-	init_waitqueue_head(&rdev->fence_drv[ring].queue);
+	rdev->fence_drv[ring].seq = 0;
+	atomic64_set(&rdev->fence_drv[ring].last_seq, 0);
+	rdev->fence_drv[ring].last_activity = jiffies;
 	rdev->fence_drv[ring].initialized = false;
 }
 
 int radeon_fence_driver_init(struct radeon_device *rdev)
 {
-	unsigned long irq_flags;
 	int ring;
 
-	write_lock_irqsave(&rdev->fence_lock, irq_flags);
+	init_waitqueue_head(&rdev->fence_queue);
 	for (ring = 0; ring < RADEON_NUM_RINGS; ring++) {
 		radeon_fence_driver_init_ring(rdev, ring);
 	}
-	write_unlock_irqrestore(&rdev->fence_lock, irq_flags);
 	if (radeon_debugfs_fence_init(rdev)) {
 		dev_err(rdev->dev, "fence debugfs file creation failed\n");
 	}
@@ -444,19 +571,18 @@ int radeon_fence_driver_init(struct radeon_device *rdev)
 
 void radeon_fence_driver_fini(struct radeon_device *rdev)
 {
-	unsigned long irq_flags;
 	int ring;
 
+	mutex_lock(&rdev->ring_lock);
 	for (ring = 0; ring < RADEON_NUM_RINGS; ring++) {
 		if (!rdev->fence_drv[ring].initialized)
 			continue;
-		radeon_fence_wait_last(rdev, ring);
-		wake_up_all(&rdev->fence_drv[ring].queue);
-		write_lock_irqsave(&rdev->fence_lock, irq_flags);
+		radeon_fence_wait_empty_locked(rdev, ring);
+		wake_up_all(&rdev->fence_queue);
 		radeon_scratch_free(rdev, rdev->fence_drv[ring].scratch_reg);
-		write_unlock_irqrestore(&rdev->fence_lock, irq_flags);
 		rdev->fence_drv[ring].initialized = false;
 	}
+	mutex_unlock(&rdev->ring_lock);
 }
 
 
@@ -469,7 +595,6 @@ static int radeon_debugfs_fence_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_fence *fence;
 	int i;
 
 	for (i = 0; i < RADEON_NUM_RINGS; ++i) {
@@ -477,14 +602,10 @@ static int radeon_debugfs_fence_info(struct seq_file *m, void *data)
 			continue;
 
 		seq_printf(m, "--- ring %d ---\n", i);
-		seq_printf(m, "Last signaled fence 0x%08X\n",
-			   radeon_fence_read(rdev, i));
-		if (!list_empty(&rdev->fence_drv[i].emitted)) {
-			fence = list_entry(rdev->fence_drv[i].emitted.prev,
-					   struct radeon_fence, list);
-			seq_printf(m, "Last emitted fence %p with 0x%08X\n",
-				   fence,  fence->seq);
-		}
+		seq_printf(m, "Last signaled fence 0x%016llx\n",
+			   (unsigned long long)atomic64_read(&rdev->fence_drv[i].last_seq));
+		seq_printf(m, "Last emitted  0x%016llx\n",
+			   rdev->fence_drv[i].seq);
 	}
 	return 0;
 }
diff --git a/drivers/gpu/drm/radeon/radeon_gart.c b/drivers/gpu/drm/radeon/radeon_gart.c
index c58a036233f..79db56e6c2a 100644
--- a/drivers/gpu/drm/radeon/radeon_gart.c
+++ b/drivers/gpu/drm/radeon/radeon_gart.c
@@ -80,7 +80,7 @@ int radeon_gart_table_vram_alloc(struct radeon_device *rdev)
 	if (rdev->gart.robj == NULL) {
 		r = radeon_bo_create(rdev, rdev->gart.table_size,
 				     PAGE_SIZE, true, RADEON_GEM_DOMAIN_VRAM,
-				     &rdev->gart.robj);
+				     NULL, &rdev->gart.robj);
 		if (r) {
 			return r;
 		}
@@ -326,7 +326,7 @@ static void radeon_vm_unbind_locked(struct radeon_device *rdev,
 	rdev->vm_manager.use_bitmap &= ~(1 << vm->id);
 	list_del_init(&vm->list);
 	vm->id = -1;
-	radeon_sa_bo_free(rdev, &vm->sa_bo);
+	radeon_sa_bo_free(rdev, &vm->sa_bo, NULL);
 	vm->pt = NULL;
 
 	list_for_each_entry(bo_va, &vm->va, vm_list) {
@@ -395,7 +395,7 @@ int radeon_vm_bind(struct radeon_device *rdev, struct radeon_vm *vm)
 retry:
 	r = radeon_sa_bo_new(rdev, &rdev->vm_manager.sa_manager, &vm->sa_bo,
 			     RADEON_GPU_PAGE_ALIGN(vm->last_pfn * 8),
-			     RADEON_GPU_PAGE_SIZE);
+			     RADEON_GPU_PAGE_SIZE, false);
 	if (r) {
 		if (list_empty(&rdev->vm_manager.lru_vm)) {
 			return r;
@@ -404,10 +404,8 @@ retry:
 		radeon_vm_unbind(rdev, vm_evict);
 		goto retry;
 	}
-	vm->pt = rdev->vm_manager.sa_manager.cpu_ptr;
-	vm->pt += (vm->sa_bo.offset >> 3);
-	vm->pt_gpu_addr = rdev->vm_manager.sa_manager.gpu_addr;
-	vm->pt_gpu_addr += vm->sa_bo.offset;
+	vm->pt = radeon_sa_bo_cpu_addr(vm->sa_bo);
+	vm->pt_gpu_addr = radeon_sa_bo_gpu_addr(vm->sa_bo);
 	memset(vm->pt, 0, RADEON_GPU_PAGE_ALIGN(vm->last_pfn * 8));
 
 retry_id:
@@ -428,14 +426,14 @@ retry_id:
 	/* do hw bind */
 	r = rdev->vm_manager.funcs->bind(rdev, vm, id);
 	if (r) {
-		radeon_sa_bo_free(rdev, &vm->sa_bo);
+		radeon_sa_bo_free(rdev, &vm->sa_bo, NULL);
 		return r;
 	}
 	rdev->vm_manager.use_bitmap |= 1 << id;
 	vm->id = id;
 	list_add_tail(&vm->list, &rdev->vm_manager.lru_vm);
-	return radeon_vm_bo_update_pte(rdev, vm, rdev->ib_pool.sa_manager.bo,
-				       &rdev->ib_pool.sa_manager.bo->tbo.mem);
+	return radeon_vm_bo_update_pte(rdev, vm, rdev->ring_tmp_bo.bo,
+				       &rdev->ring_tmp_bo.bo->tbo.mem);
 }
 
 /* object have to be reserved */
@@ -551,7 +549,7 @@ int radeon_vm_bo_update_pte(struct radeon_device *rdev,
 
 	/* nothing to do if vm isn't bound */
 	if (vm->id == -1)
-		return 0;;
+		return 0;
 
 	bo_va = radeon_bo_va(bo, vm);
 	if (bo_va == NULL) {
@@ -633,7 +631,7 @@ int radeon_vm_init(struct radeon_device *rdev, struct radeon_vm *vm)
 	/* map the ib pool buffer at 0 in virtual address space, set
 	 * read only
 	 */
-	r = radeon_vm_bo_add(rdev, vm, rdev->ib_pool.sa_manager.bo, 0,
+	r = radeon_vm_bo_add(rdev, vm, rdev->ring_tmp_bo.bo, 0,
 			     RADEON_VM_PAGE_READABLE | RADEON_VM_PAGE_SNOOPED);
 	return r;
 }
@@ -650,12 +648,12 @@ void radeon_vm_fini(struct radeon_device *rdev, struct radeon_vm *vm)
 	radeon_mutex_unlock(&rdev->cs_mutex);
 
 	/* remove all bo */
-	r = radeon_bo_reserve(rdev->ib_pool.sa_manager.bo, false);
+	r = radeon_bo_reserve(rdev->ring_tmp_bo.bo, false);
 	if (!r) {
-		bo_va = radeon_bo_va(rdev->ib_pool.sa_manager.bo, vm);
+		bo_va = radeon_bo_va(rdev->ring_tmp_bo.bo, vm);
 		list_del_init(&bo_va->bo_list);
 		list_del_init(&bo_va->vm_list);
-		radeon_bo_unreserve(rdev->ib_pool.sa_manager.bo);
+		radeon_bo_unreserve(rdev->ring_tmp_bo.bo);
 		kfree(bo_va);
 	}
 	if (!list_empty(&vm->va)) {
diff --git a/drivers/gpu/drm/radeon/radeon_gem.c b/drivers/gpu/drm/radeon/radeon_gem.c
index c7008b5210f..f28bd4b7ef9 100644
--- a/drivers/gpu/drm/radeon/radeon_gem.c
+++ b/drivers/gpu/drm/radeon/radeon_gem.c
@@ -42,6 +42,8 @@ void radeon_gem_object_free(struct drm_gem_object *gobj)
 	struct radeon_bo *robj = gem_to_radeon_bo(gobj);
 
 	if (robj) {
+		if (robj->gem_base.import_attach)
+			drm_prime_gem_destroy(&robj->gem_base, robj->tbo.sg);
 		radeon_bo_unref(&robj);
 	}
 }
@@ -59,7 +61,7 @@ int radeon_gem_object_create(struct radeon_device *rdev, int size,
 	if (alignment < PAGE_SIZE) {
 		alignment = PAGE_SIZE;
 	}
-	r = radeon_bo_create(rdev, size, alignment, kernel, initial_domain, &robj);
+	r = radeon_bo_create(rdev, size, alignment, kernel, initial_domain, NULL, &robj);
 	if (r) {
 		if (r != -ERESTARTSYS)
 			DRM_ERROR("Failed to allocate GEM object (%d, %d, %u, %d)\n",
@@ -91,7 +93,7 @@ int radeon_gem_set_domain(struct drm_gem_object *gobj,
 	}
 	if (!domain) {
 		/* Do nothings */
-		printk(KERN_WARNING "Set domain withou domain !\n");
+		printk(KERN_WARNING "Set domain without domain !\n");
 		return 0;
 	}
 	if (domain == RADEON_GEM_DOMAIN_CPU) {
@@ -154,6 +156,17 @@ void radeon_gem_object_close(struct drm_gem_object *obj,
 	radeon_bo_unreserve(rbo);
 }
 
+static int radeon_gem_handle_lockup(struct radeon_device *rdev, int r)
+{
+	if (r == -EDEADLK) {
+		radeon_mutex_lock(&rdev->cs_mutex);
+		r = radeon_gpu_reset(rdev);
+		if (!r)
+			r = -EAGAIN;
+		radeon_mutex_unlock(&rdev->cs_mutex);
+	}
+	return r;
+}
 
 /*
  * GEM ioctls.
@@ -210,12 +223,14 @@ int radeon_gem_create_ioctl(struct drm_device *dev, void *data,
 					args->initial_domain, false,
 					false, &gobj);
 	if (r) {
+		r = radeon_gem_handle_lockup(rdev, r);
 		return r;
 	}
 	r = drm_gem_handle_create(filp, gobj, &handle);
 	/* drop reference from allocate - handle holds it now */
 	drm_gem_object_unreference_unlocked(gobj);
 	if (r) {
+		r = radeon_gem_handle_lockup(rdev, r);
 		return r;
 	}
 	args->handle = handle;
@@ -245,6 +260,7 @@ int radeon_gem_set_domain_ioctl(struct drm_device *dev, void *data,
 	r = radeon_gem_set_domain(gobj, args->read_domains, args->write_domain);
 
 	drm_gem_object_unreference_unlocked(gobj);
+	r = radeon_gem_handle_lockup(robj->rdev, r);
 	return r;
 }
 
@@ -301,6 +317,7 @@ int radeon_gem_busy_ioctl(struct drm_device *dev, void *data,
 		break;
 	}
 	drm_gem_object_unreference_unlocked(gobj);
+	r = radeon_gem_handle_lockup(robj->rdev, r);
 	return r;
 }
 
@@ -322,6 +339,7 @@ int radeon_gem_wait_idle_ioctl(struct drm_device *dev, void *data,
 	if (robj->rdev->asic->ioctl_wait_idle)
 		robj->rdev->asic->ioctl_wait_idle(robj->rdev, robj);
 	drm_gem_object_unreference_unlocked(gobj);
+	r = radeon_gem_handle_lockup(robj->rdev, r);
 	return r;
 }
 
diff --git a/drivers/gpu/drm/radeon/radeon_irq_kms.c b/drivers/gpu/drm/radeon/radeon_irq_kms.c
index 65060b77c80..5df58d1aba0 100644
--- a/drivers/gpu/drm/radeon/radeon_irq_kms.c
+++ b/drivers/gpu/drm/radeon/radeon_irq_kms.c
@@ -73,6 +73,7 @@ void radeon_driver_irq_preinstall_kms(struct drm_device *dev)
 	for (i = 0; i < RADEON_MAX_CRTCS; i++) {
 		rdev->irq.crtc_vblank_int[i] = false;
 		rdev->irq.pflip[i] = false;
+		rdev->irq.afmt[i] = false;
 	}
 	radeon_irq_set(rdev);
 	/* Clear bits */
@@ -108,6 +109,7 @@ void radeon_driver_irq_uninstall_kms(struct drm_device *dev)
 	for (i = 0; i < RADEON_MAX_CRTCS; i++) {
 		rdev->irq.crtc_vblank_int[i] = false;
 		rdev->irq.pflip[i] = false;
+		rdev->irq.afmt[i] = false;
 	}
 	radeon_irq_set(rdev);
 }
@@ -170,6 +172,7 @@ int radeon_irq_kms_init(struct radeon_device *rdev)
 	int r = 0;
 
 	INIT_WORK(&rdev->hotplug_work, radeon_hotplug_work_func);
+	INIT_WORK(&rdev->audio_work, r600_audio_update_hdmi);
 
 	spin_lock_init(&rdev->irq.sw_lock);
 	for (i = 0; i < rdev->num_crtc; i++)
diff --git a/drivers/gpu/drm/radeon/radeon_kms.c b/drivers/gpu/drm/radeon/radeon_kms.c
index 3c2628b14d5..f1016a5820d 100644
--- a/drivers/gpu/drm/radeon/radeon_kms.c
+++ b/drivers/gpu/drm/radeon/radeon_kms.c
@@ -57,8 +57,6 @@ int radeon_driver_load_kms(struct drm_device *dev, unsigned long flags)
 	}
 	dev->dev_private = (void *)rdev;
 
-	pci_set_master(dev->pdev);
-
 	/* update BUS flag */
 	if (drm_pci_device_is_agp(dev)) {
 		flags |= RADEON_IS_AGP;
diff --git a/drivers/gpu/drm/radeon/radeon_legacy_encoders.c b/drivers/gpu/drm/radeon/radeon_legacy_encoders.c
index 42db254f6bb..a0c82229e8f 100644
--- a/drivers/gpu/drm/radeon/radeon_legacy_encoders.c
+++ b/drivers/gpu/drm/radeon/radeon_legacy_encoders.c
@@ -369,6 +369,7 @@ void radeon_legacy_backlight_init(struct radeon_encoder *radeon_encoder,
 		goto error;
 	}
 
+	memset(&props, 0, sizeof(props));
 	props.max_brightness = MAX_RADEON_LEVEL;
 	props.type = BACKLIGHT_RAW;
 	bd = backlight_device_register("radeon_bl", &drm_connector->kdev,
diff --git a/drivers/gpu/drm/radeon/radeon_mode.h b/drivers/gpu/drm/radeon/radeon_mode.h
index f7eb5d8b9fd..5b10ffd7bb2 100644
--- a/drivers/gpu/drm/radeon/radeon_mode.h
+++ b/drivers/gpu/drm/radeon/radeon_mode.h
@@ -210,6 +210,7 @@ enum radeon_connector_table {
 	CT_RN50_POWER,
 	CT_MAC_X800,
 	CT_MAC_G5_9600,
+	CT_SAM440EP
 };
 
 enum radeon_dvo_chip {
@@ -219,12 +220,20 @@ enum radeon_dvo_chip {
 
 struct radeon_fbdev;
 
+struct radeon_afmt {
+	bool enabled;
+	int offset;
+	bool last_buffer_filled_status;
+	int id;
+};
+
 struct radeon_mode_info {
 	struct atom_context *atom_context;
 	struct card_info *atom_card_info;
 	enum radeon_connector_table connector_table;
 	bool mode_config_initialized;
 	struct radeon_crtc *crtcs[6];
+	struct radeon_afmt *afmt[6];
 	/* DVI-I properties */
 	struct drm_property *coherent_mode_property;
 	/* DAC enable load detect */
@@ -363,6 +372,7 @@ struct radeon_encoder_atom_dig {
 	int dpms_mode;
 	uint8_t backlight_level;
 	int panel_mode;
+	struct radeon_afmt *afmt;
 };
 
 struct radeon_encoder_atom_dac {
@@ -384,10 +394,6 @@ struct radeon_encoder {
 	struct drm_display_mode native_mode;
 	void *enc_priv;
 	int audio_polling_active;
-	int hdmi_offset;
-	int hdmi_config_offset;
-	int hdmi_audio_workaround;
-	int hdmi_buffer_status;
 	bool is_ext_encoder;
 	u16 caps;
 };
@@ -476,6 +482,7 @@ extern u16 radeon_encoder_get_dp_bridge_encoder_id(struct drm_encoder *encoder);
 extern u16 radeon_connector_encoder_get_dp_bridge_encoder_id(struct drm_connector *connector);
 extern bool radeon_connector_encoder_is_hbr2(struct drm_connector *connector);
 extern bool radeon_connector_is_dp12_capable(struct drm_connector *connector);
+extern int radeon_get_monitor_bpc(struct drm_connector *connector);
 
 extern void radeon_connector_hotplug(struct drm_connector *connector);
 extern int radeon_dp_mode_valid_helper(struct drm_connector *connector,
diff --git a/drivers/gpu/drm/radeon/radeon_object.c b/drivers/gpu/drm/radeon/radeon_object.c
index df6a4dbd93f..830f1a7b486 100644
--- a/drivers/gpu/drm/radeon/radeon_object.c
+++ b/drivers/gpu/drm/radeon/radeon_object.c
@@ -104,7 +104,7 @@ void radeon_ttm_placement_from_domain(struct radeon_bo *rbo, u32 domain)
 
 int radeon_bo_create(struct radeon_device *rdev,
 		     unsigned long size, int byte_align, bool kernel, u32 domain,
-		     struct radeon_bo **bo_ptr)
+		     struct sg_table *sg, struct radeon_bo **bo_ptr)
 {
 	struct radeon_bo *bo;
 	enum ttm_bo_type type;
@@ -120,6 +120,8 @@ int radeon_bo_create(struct radeon_device *rdev,
 	}
 	if (kernel) {
 		type = ttm_bo_type_kernel;
+	} else if (sg) {
+		type = ttm_bo_type_sg;
 	} else {
 		type = ttm_bo_type_device;
 	}
@@ -155,7 +157,7 @@ retry:
 	mutex_lock(&rdev->vram_mutex);
 	r = ttm_bo_init(&rdev->mman.bdev, &bo->tbo, size, type,
 			&bo->placement, page_align, 0, !kernel, NULL,
-			acc_size, &radeon_ttm_bo_destroy);
+			acc_size, sg, &radeon_ttm_bo_destroy);
 	mutex_unlock(&rdev->vram_mutex);
 	if (unlikely(r != 0)) {
 		if (r != -ERESTARTSYS) {
diff --git a/drivers/gpu/drm/radeon/radeon_object.h b/drivers/gpu/drm/radeon/radeon_object.h
index f9104be88d7..17fb99f177c 100644
--- a/drivers/gpu/drm/radeon/radeon_object.h
+++ b/drivers/gpu/drm/radeon/radeon_object.h
@@ -111,9 +111,10 @@ extern int radeon_bo_wait(struct radeon_bo *bo, u32 *mem_type,
 			  bool no_wait);
 
 extern int radeon_bo_create(struct radeon_device *rdev,
-				unsigned long size, int byte_align,
-				bool kernel, u32 domain,
-				struct radeon_bo **bo_ptr);
+			    unsigned long size, int byte_align,
+			    bool kernel, u32 domain,
+			    struct sg_table *sg,
+			    struct radeon_bo **bo_ptr);
 extern int radeon_bo_kmap(struct radeon_bo *bo, void **ptr);
 extern void radeon_bo_kunmap(struct radeon_bo *bo);
 extern void radeon_bo_unref(struct radeon_bo **bo);
@@ -146,6 +147,17 @@ extern struct radeon_bo_va *radeon_bo_va(struct radeon_bo *rbo,
 /*
  * sub allocation
  */
+
+static inline uint64_t radeon_sa_bo_gpu_addr(struct radeon_sa_bo *sa_bo)
+{
+	return sa_bo->manager->gpu_addr + sa_bo->soffset;
+}
+
+static inline void * radeon_sa_bo_cpu_addr(struct radeon_sa_bo *sa_bo)
+{
+	return sa_bo->manager->cpu_ptr + sa_bo->soffset;
+}
+
 extern int radeon_sa_bo_manager_init(struct radeon_device *rdev,
 				     struct radeon_sa_manager *sa_manager,
 				     unsigned size, u32 domain);
@@ -157,9 +169,15 @@ extern int radeon_sa_bo_manager_suspend(struct radeon_device *rdev,
 					struct radeon_sa_manager *sa_manager);
 extern int radeon_sa_bo_new(struct radeon_device *rdev,
 			    struct radeon_sa_manager *sa_manager,
-			    struct radeon_sa_bo *sa_bo,
-			    unsigned size, unsigned align);
+			    struct radeon_sa_bo **sa_bo,
+			    unsigned size, unsigned align, bool block);
 extern void radeon_sa_bo_free(struct radeon_device *rdev,
-			      struct radeon_sa_bo *sa_bo);
+			      struct radeon_sa_bo **sa_bo,
+			      struct radeon_fence *fence);
+#if defined(CONFIG_DEBUG_FS)
+extern void radeon_sa_bo_dump_debug_info(struct radeon_sa_manager *sa_manager,
+					 struct seq_file *m);
+#endif
+
 
 #endif
diff --git a/drivers/gpu/drm/radeon/radeon_pm.c b/drivers/gpu/drm/radeon/radeon_pm.c
index caa55d68f31..08825548ee6 100644
--- a/drivers/gpu/drm/radeon/radeon_pm.c
+++ b/drivers/gpu/drm/radeon/radeon_pm.c
@@ -252,10 +252,7 @@ static void radeon_pm_set_clocks(struct radeon_device *rdev)
 
 	mutex_lock(&rdev->ddev->struct_mutex);
 	mutex_lock(&rdev->vram_mutex);
-	for (i = 0; i < RADEON_NUM_RINGS; ++i) {
-		if (rdev->ring[i].ring_obj)
-			mutex_lock(&rdev->ring[i].mutex);
-	}
+	mutex_lock(&rdev->ring_lock);
 
 	/* gui idle int has issues on older chips it seems */
 	if (rdev->family >= CHIP_R600) {
@@ -273,13 +270,7 @@ static void radeon_pm_set_clocks(struct radeon_device *rdev)
 	} else {
 		struct radeon_ring *ring = &rdev->ring[RADEON_RING_TYPE_GFX_INDEX];
 		if (ring->ready) {
-			struct radeon_fence *fence;
-			radeon_ring_alloc(rdev, ring, 64);
-			radeon_fence_create(rdev, &fence, radeon_ring_index(rdev, ring));
-			radeon_fence_emit(rdev, fence);
-			radeon_ring_commit(rdev, ring);
-			radeon_fence_wait(fence, false);
-			radeon_fence_unref(&fence);
+			radeon_fence_wait_empty_locked(rdev, RADEON_RING_TYPE_GFX_INDEX);
 		}
 	}
 	radeon_unmap_vram_bos(rdev);
@@ -311,10 +302,7 @@ static void radeon_pm_set_clocks(struct radeon_device *rdev)
 
 	rdev->pm.dynpm_planned_action = DYNPM_ACTION_NONE;
 
-	for (i = 0; i < RADEON_NUM_RINGS; ++i) {
-		if (rdev->ring[i].ring_obj)
-			mutex_unlock(&rdev->ring[i].mutex);
-	}
+	mutex_unlock(&rdev->ring_lock);
 	mutex_unlock(&rdev->vram_mutex);
 	mutex_unlock(&rdev->ddev->struct_mutex);
 }
diff --git a/drivers/gpu/drm/radeon/radeon_prime.c b/drivers/gpu/drm/radeon/radeon_prime.c
new file mode 100644
index 00000000000..b8f835d8ecb
--- /dev/null
+++ b/drivers/gpu/drm/radeon/radeon_prime.c
@@ -0,0 +1,176 @@
+/*
+ * Copyright 2012 Advanced Micro Devices, 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.
+ *
+ * based on nouveau_prime.c
+ *
+ * Authors: Alex Deucher
+ */
+#include "drmP.h"
+#include "drm.h"
+
+#include "radeon.h"
+#include "radeon_drm.h"
+
+#include <linux/dma-buf.h>
+
+static struct sg_table *radeon_gem_map_dma_buf(struct dma_buf_attachment *attachment,
+					       enum dma_data_direction dir)
+{
+	struct radeon_bo *bo = attachment->dmabuf->priv;
+	struct drm_device *dev = bo->rdev->ddev;
+	int npages = bo->tbo.num_pages;
+	struct sg_table *sg;
+	int nents;
+
+	mutex_lock(&dev->struct_mutex);
+	sg = drm_prime_pages_to_sg(bo->tbo.ttm->pages, npages);
+	nents = dma_map_sg(attachment->dev, sg->sgl, sg->nents, dir);
+	mutex_unlock(&dev->struct_mutex);
+	return sg;
+}
+
+static void radeon_gem_unmap_dma_buf(struct dma_buf_attachment *attachment,
+				     struct sg_table *sg, enum dma_data_direction dir)
+{
+	dma_unmap_sg(attachment->dev, sg->sgl, sg->nents, dir);
+	sg_free_table(sg);
+	kfree(sg);
+}
+
+static void radeon_gem_dmabuf_release(struct dma_buf *dma_buf)
+{
+	struct radeon_bo *bo = dma_buf->priv;
+
+	if (bo->gem_base.export_dma_buf == dma_buf) {
+		DRM_ERROR("unreference dmabuf %p\n", &bo->gem_base);
+		bo->gem_base.export_dma_buf = NULL;
+		drm_gem_object_unreference_unlocked(&bo->gem_base);
+	}
+}
+
+static void *radeon_gem_kmap_atomic(struct dma_buf *dma_buf, unsigned long page_num)
+{
+	return NULL;
+}
+
+static void radeon_gem_kunmap_atomic(struct dma_buf *dma_buf, unsigned long page_num, void *addr)
+{
+
+}
+static void *radeon_gem_kmap(struct dma_buf *dma_buf, unsigned long page_num)
+{
+	return NULL;
+}
+
+static void radeon_gem_kunmap(struct dma_buf *dma_buf, unsigned long page_num, void *addr)
+{
+
+}
+
+const static struct dma_buf_ops radeon_dmabuf_ops =  {
+	.map_dma_buf = radeon_gem_map_dma_buf,
+	.unmap_dma_buf = radeon_gem_unmap_dma_buf,
+	.release = radeon_gem_dmabuf_release,
+	.kmap = radeon_gem_kmap,
+	.kmap_atomic = radeon_gem_kmap_atomic,
+	.kunmap = radeon_gem_kunmap,
+	.kunmap_atomic = radeon_gem_kunmap_atomic,
+};
+
+static int radeon_prime_create(struct drm_device *dev,
+			       size_t size,
+			       struct sg_table *sg,
+			       struct radeon_bo **pbo)
+{
+	struct radeon_device *rdev = dev->dev_private;
+	struct radeon_bo *bo;
+	int ret;
+
+	ret = radeon_bo_create(rdev, size, PAGE_SIZE, false,
+			       RADEON_GEM_DOMAIN_GTT, sg, pbo);
+	if (ret)
+		return ret;
+	bo = *pbo;
+	bo->gem_base.driver_private = bo;
+
+	mutex_lock(&rdev->gem.mutex);
+	list_add_tail(&bo->list, &rdev->gem.objects);
+	mutex_unlock(&rdev->gem.mutex);
+
+	return 0;
+}
+
+struct dma_buf *radeon_gem_prime_export(struct drm_device *dev,
+					struct drm_gem_object *obj,
+					int flags)
+{
+	struct radeon_bo *bo = gem_to_radeon_bo(obj);
+	int ret = 0;
+
+	/* pin buffer into GTT */
+	ret = radeon_bo_pin(bo, RADEON_GEM_DOMAIN_GTT, NULL);
+	if (ret)
+		return ERR_PTR(ret);
+
+	return dma_buf_export(bo, &radeon_dmabuf_ops, obj->size, flags);
+}
+
+struct drm_gem_object *radeon_gem_prime_import(struct drm_device *dev,
+					       struct dma_buf *dma_buf)
+{
+	struct dma_buf_attachment *attach;
+	struct sg_table *sg;
+	struct radeon_bo *bo;
+	int ret;
+
+	if (dma_buf->ops == &radeon_dmabuf_ops) {
+		bo = dma_buf->priv;
+		if (bo->gem_base.dev == dev) {
+			drm_gem_object_reference(&bo->gem_base);
+			return &bo->gem_base;
+		}
+	}
+
+	/* need to attach */
+	attach = dma_buf_attach(dma_buf, dev->dev);
+	if (IS_ERR(attach))
+		return ERR_CAST(attach);
+
+	sg = dma_buf_map_attachment(attach, DMA_BIDIRECTIONAL);
+	if (IS_ERR(sg)) {
+		ret = PTR_ERR(sg);
+		goto fail_detach;
+	}
+
+	ret = radeon_prime_create(dev, dma_buf->size, sg, &bo);
+	if (ret)
+		goto fail_unmap;
+
+	bo->gem_base.import_attach = attach;
+
+	return &bo->gem_base;
+
+fail_unmap:
+	dma_buf_unmap_attachment(attach, sg, DMA_BIDIRECTIONAL);
+fail_detach:
+	dma_buf_detach(dma_buf, attach);
+	return ERR_PTR(ret);
+}
diff --git a/drivers/gpu/drm/radeon/radeon_ring.c b/drivers/gpu/drm/radeon/radeon_ring.c
index cc33b3d7c33..493a7be7530 100644
--- a/drivers/gpu/drm/radeon/radeon_ring.c
+++ b/drivers/gpu/drm/radeon/radeon_ring.c
@@ -24,6 +24,7 @@
  * Authors: Dave Airlie
  *          Alex Deucher
  *          Jerome Glisse
+ *          Christian König
  */
 #include <linux/seq_file.h>
 #include <linux/slab.h>
@@ -33,8 +34,10 @@
 #include "radeon.h"
 #include "atom.h"
 
-int radeon_debugfs_ib_init(struct radeon_device *rdev);
-int radeon_debugfs_ring_init(struct radeon_device *rdev);
+/*
+ * IB.
+ */
+int radeon_debugfs_sa_init(struct radeon_device *rdev);
 
 u32 radeon_get_ib_value(struct radeon_cs_parser *p, int idx)
 {
@@ -61,123 +64,37 @@ u32 radeon_get_ib_value(struct radeon_cs_parser *p, int idx)
 	return idx_value;
 }
 
-void radeon_ring_write(struct radeon_ring *ring, uint32_t v)
-{
-#if DRM_DEBUG_CODE
-	if (ring->count_dw <= 0) {
-		DRM_ERROR("radeon: writting more dword to ring than expected !\n");
-	}
-#endif
-	ring->ring[ring->wptr++] = v;
-	ring->wptr &= ring->ptr_mask;
-	ring->count_dw--;
-	ring->ring_free_dw--;
-}
-
-/*
- * IB.
- */
-bool radeon_ib_try_free(struct radeon_device *rdev, struct radeon_ib *ib)
-{
-	bool done = false;
-
-	/* only free ib which have been emited */
-	if (ib->fence && ib->fence->emitted) {
-		if (radeon_fence_signaled(ib->fence)) {
-			radeon_fence_unref(&ib->fence);
-			radeon_sa_bo_free(rdev, &ib->sa_bo);
-			done = true;
-		}
-	}
-	return done;
-}
-
 int radeon_ib_get(struct radeon_device *rdev, int ring,
-		  struct radeon_ib **ib, unsigned size)
+		  struct radeon_ib *ib, unsigned size)
 {
-	struct radeon_fence *fence;
-	unsigned cretry = 0;
-	int r = 0, i, idx;
-
-	*ib = NULL;
-	/* align size on 256 bytes */
-	size = ALIGN(size, 256);
+	int r;
 
-	r = radeon_fence_create(rdev, &fence, ring);
+	r = radeon_sa_bo_new(rdev, &rdev->ring_tmp_bo, &ib->sa_bo, size, 256, true);
 	if (r) {
-		dev_err(rdev->dev, "failed to create fence for new IB\n");
+		dev_err(rdev->dev, "failed to get a new IB (%d)\n", r);
 		return r;
 	}
-
-	radeon_mutex_lock(&rdev->ib_pool.mutex);
-	idx = rdev->ib_pool.head_id;
-retry:
-	if (cretry > 5) {
-		dev_err(rdev->dev, "failed to get an ib after 5 retry\n");
-		radeon_mutex_unlock(&rdev->ib_pool.mutex);
-		radeon_fence_unref(&fence);
-		return -ENOMEM;
-	}
-	cretry++;
-	for (i = 0; i < RADEON_IB_POOL_SIZE; i++) {
-		radeon_ib_try_free(rdev, &rdev->ib_pool.ibs[idx]);
-		if (rdev->ib_pool.ibs[idx].fence == NULL) {
-			r = radeon_sa_bo_new(rdev, &rdev->ib_pool.sa_manager,
-					     &rdev->ib_pool.ibs[idx].sa_bo,
-					     size, 256);
-			if (!r) {
-				*ib = &rdev->ib_pool.ibs[idx];
-				(*ib)->ptr = rdev->ib_pool.sa_manager.cpu_ptr;
-				(*ib)->ptr += ((*ib)->sa_bo.offset >> 2);
-				(*ib)->gpu_addr = rdev->ib_pool.sa_manager.gpu_addr;
-				(*ib)->gpu_addr += (*ib)->sa_bo.offset;
-				(*ib)->fence = fence;
-				(*ib)->vm_id = 0;
-				(*ib)->is_const_ib = false;
-				/* ib are most likely to be allocated in a ring fashion
-				 * thus rdev->ib_pool.head_id should be the id of the
-				 * oldest ib
-				 */
-				rdev->ib_pool.head_id = (1 + idx);
-				rdev->ib_pool.head_id &= (RADEON_IB_POOL_SIZE - 1);
-				radeon_mutex_unlock(&rdev->ib_pool.mutex);
-				return 0;
-			}
-		}
-		idx = (idx + 1) & (RADEON_IB_POOL_SIZE - 1);
-	}
-	/* this should be rare event, ie all ib scheduled none signaled yet.
-	 */
-	for (i = 0; i < RADEON_IB_POOL_SIZE; i++) {
-		if (rdev->ib_pool.ibs[idx].fence && rdev->ib_pool.ibs[idx].fence->emitted) {
-			r = radeon_fence_wait(rdev->ib_pool.ibs[idx].fence, false);
-			if (!r) {
-				goto retry;
-			}
-			/* an error happened */
-			break;
-		}
-		idx = (idx + 1) & (RADEON_IB_POOL_SIZE - 1);
+	r = radeon_fence_create(rdev, &ib->fence, ring);
+	if (r) {
+		dev_err(rdev->dev, "failed to create fence for new IB (%d)\n", r);
+		radeon_sa_bo_free(rdev, &ib->sa_bo, NULL);
+		return r;
 	}
-	radeon_mutex_unlock(&rdev->ib_pool.mutex);
-	radeon_fence_unref(&fence);
-	return r;
+
+	ib->ptr = radeon_sa_bo_cpu_addr(ib->sa_bo);
+	ib->gpu_addr = radeon_sa_bo_gpu_addr(ib->sa_bo);
+	ib->vm_id = 0;
+	ib->is_const_ib = false;
+	ib->semaphore = NULL;
+
+	return 0;
 }
 
-void radeon_ib_free(struct radeon_device *rdev, struct radeon_ib **ib)
+void radeon_ib_free(struct radeon_device *rdev, struct radeon_ib *ib)
 {
-	struct radeon_ib *tmp = *ib;
-
-	*ib = NULL;
-	if (tmp == NULL) {
-		return;
-	}
-	radeon_mutex_lock(&rdev->ib_pool.mutex);
-	if (tmp->fence && !tmp->fence->emitted) {
-		radeon_sa_bo_free(rdev, &tmp->sa_bo);
-		radeon_fence_unref(&tmp->fence);
-	}
-	radeon_mutex_unlock(&rdev->ib_pool.mutex);
+	radeon_semaphore_free(rdev, ib->semaphore, ib->fence);
+	radeon_sa_bo_free(rdev, &ib->sa_bo, ib->fence);
+	radeon_fence_unref(&ib->fence);
 }
 
 int radeon_ib_schedule(struct radeon_device *rdev, struct radeon_ib *ib)
@@ -187,14 +104,14 @@ int radeon_ib_schedule(struct radeon_device *rdev, struct radeon_ib *ib)
 
 	if (!ib->length_dw || !ring->ready) {
 		/* TODO: Nothings in the ib we should report. */
-		DRM_ERROR("radeon: couldn't schedule IB(%u).\n", ib->idx);
+		dev_err(rdev->dev, "couldn't schedule ib\n");
 		return -EINVAL;
 	}
 
 	/* 64 dwords should be enough for fence too */
 	r = radeon_ring_lock(rdev, ring, 64);
 	if (r) {
-		DRM_ERROR("radeon: scheduling IB failed (%d).\n", r);
+		dev_err(rdev->dev, "scheduling IB failed (%d).\n", r);
 		return r;
 	}
 	radeon_ring_ib_execute(rdev, ib->fence->ring, ib);
@@ -205,74 +122,90 @@ int radeon_ib_schedule(struct radeon_device *rdev, struct radeon_ib *ib)
 
 int radeon_ib_pool_init(struct radeon_device *rdev)
 {
-	struct radeon_sa_manager tmp;
-	int i, r;
+	int r;
 
-	r = radeon_sa_bo_manager_init(rdev, &tmp,
+	if (rdev->ib_pool_ready) {
+		return 0;
+	}
+	r = radeon_sa_bo_manager_init(rdev, &rdev->ring_tmp_bo,
 				      RADEON_IB_POOL_SIZE*64*1024,
 				      RADEON_GEM_DOMAIN_GTT);
 	if (r) {
 		return r;
 	}
-
-	radeon_mutex_lock(&rdev->ib_pool.mutex);
-	if (rdev->ib_pool.ready) {
-		radeon_mutex_unlock(&rdev->ib_pool.mutex);
-		radeon_sa_bo_manager_fini(rdev, &tmp);
-		return 0;
-	}
-
-	rdev->ib_pool.sa_manager = tmp;
-	INIT_LIST_HEAD(&rdev->ib_pool.sa_manager.sa_bo);
-	for (i = 0; i < RADEON_IB_POOL_SIZE; i++) {
-		rdev->ib_pool.ibs[i].fence = NULL;
-		rdev->ib_pool.ibs[i].idx = i;
-		rdev->ib_pool.ibs[i].length_dw = 0;
-		INIT_LIST_HEAD(&rdev->ib_pool.ibs[i].sa_bo.list);
-	}
-	rdev->ib_pool.head_id = 0;
-	rdev->ib_pool.ready = true;
-	DRM_INFO("radeon: ib pool ready.\n");
-
-	if (radeon_debugfs_ib_init(rdev)) {
-		DRM_ERROR("Failed to register debugfs file for IB !\n");
-	}
-	if (radeon_debugfs_ring_init(rdev)) {
-		DRM_ERROR("Failed to register debugfs file for rings !\n");
+	rdev->ib_pool_ready = true;
+	if (radeon_debugfs_sa_init(rdev)) {
+		dev_err(rdev->dev, "failed to register debugfs file for SA\n");
 	}
-	radeon_mutex_unlock(&rdev->ib_pool.mutex);
 	return 0;
 }
 
 void radeon_ib_pool_fini(struct radeon_device *rdev)
 {
-	unsigned i;
-
-	radeon_mutex_lock(&rdev->ib_pool.mutex);
-	if (rdev->ib_pool.ready) {
-		for (i = 0; i < RADEON_IB_POOL_SIZE; i++) {
-			radeon_sa_bo_free(rdev, &rdev->ib_pool.ibs[i].sa_bo);
-			radeon_fence_unref(&rdev->ib_pool.ibs[i].fence);
-		}
-		radeon_sa_bo_manager_fini(rdev, &rdev->ib_pool.sa_manager);
-		rdev->ib_pool.ready = false;
+	if (rdev->ib_pool_ready) {
+		radeon_sa_bo_manager_fini(rdev, &rdev->ring_tmp_bo);
+		rdev->ib_pool_ready = false;
 	}
-	radeon_mutex_unlock(&rdev->ib_pool.mutex);
 }
 
 int radeon_ib_pool_start(struct radeon_device *rdev)
 {
-	return radeon_sa_bo_manager_start(rdev, &rdev->ib_pool.sa_manager);
+	return radeon_sa_bo_manager_start(rdev, &rdev->ring_tmp_bo);
 }
 
 int radeon_ib_pool_suspend(struct radeon_device *rdev)
 {
-	return radeon_sa_bo_manager_suspend(rdev, &rdev->ib_pool.sa_manager);
+	return radeon_sa_bo_manager_suspend(rdev, &rdev->ring_tmp_bo);
+}
+
+int radeon_ib_ring_tests(struct radeon_device *rdev)
+{
+	unsigned i;
+	int r;
+
+	for (i = 0; i < RADEON_NUM_RINGS; ++i) {
+		struct radeon_ring *ring = &rdev->ring[i];
+
+		if (!ring->ready)
+			continue;
+
+		r = radeon_ib_test(rdev, i, ring);
+		if (r) {
+			ring->ready = false;
+
+			if (i == RADEON_RING_TYPE_GFX_INDEX) {
+				/* oh, oh, that's really bad */
+				DRM_ERROR("radeon: failed testing IB on GFX ring (%d).\n", r);
+		                rdev->accel_working = false;
+				return r;
+
+			} else {
+				/* still not good, but we can live with it */
+				DRM_ERROR("radeon: failed testing IB on ring %d (%d).\n", i, r);
+			}
+		}
+	}
+	return 0;
 }
 
 /*
  * Ring.
  */
+int radeon_debugfs_ring_init(struct radeon_device *rdev, struct radeon_ring *ring);
+
+void radeon_ring_write(struct radeon_ring *ring, uint32_t v)
+{
+#if DRM_DEBUG_CODE
+	if (ring->count_dw <= 0) {
+		DRM_ERROR("radeon: writting more dword to ring than expected !\n");
+	}
+#endif
+	ring->ring[ring->wptr++] = v;
+	ring->wptr &= ring->ptr_mask;
+	ring->count_dw--;
+	ring->ring_free_dw--;
+}
+
 int radeon_ring_index(struct radeon_device *rdev, struct radeon_ring *ring)
 {
 	/* r1xx-r5xx only has CP ring */
@@ -319,7 +252,7 @@ int radeon_ring_alloc(struct radeon_device *rdev, struct radeon_ring *ring, unsi
 		if (ndw < ring->ring_free_dw) {
 			break;
 		}
-		r = radeon_fence_wait_next(rdev, radeon_ring_index(rdev, ring));
+		r = radeon_fence_wait_next_locked(rdev, radeon_ring_index(rdev, ring));
 		if (r)
 			return r;
 	}
@@ -332,10 +265,10 @@ int radeon_ring_lock(struct radeon_device *rdev, struct radeon_ring *ring, unsig
 {
 	int r;
 
-	mutex_lock(&ring->mutex);
+	mutex_lock(&rdev->ring_lock);
 	r = radeon_ring_alloc(rdev, ring, ndw);
 	if (r) {
-		mutex_unlock(&ring->mutex);
+		mutex_unlock(&rdev->ring_lock);
 		return r;
 	}
 	return 0;
@@ -360,13 +293,85 @@ void radeon_ring_commit(struct radeon_device *rdev, struct radeon_ring *ring)
 void radeon_ring_unlock_commit(struct radeon_device *rdev, struct radeon_ring *ring)
 {
 	radeon_ring_commit(rdev, ring);
-	mutex_unlock(&ring->mutex);
+	mutex_unlock(&rdev->ring_lock);
 }
 
-void radeon_ring_unlock_undo(struct radeon_device *rdev, struct radeon_ring *ring)
+void radeon_ring_undo(struct radeon_ring *ring)
 {
 	ring->wptr = ring->wptr_old;
-	mutex_unlock(&ring->mutex);
+}
+
+void radeon_ring_unlock_undo(struct radeon_device *rdev, struct radeon_ring *ring)
+{
+	radeon_ring_undo(ring);
+	mutex_unlock(&rdev->ring_lock);
+}
+
+void radeon_ring_force_activity(struct radeon_device *rdev, struct radeon_ring *ring)
+{
+	int r;
+
+	radeon_ring_free_size(rdev, ring);
+	if (ring->rptr == ring->wptr) {
+		r = radeon_ring_alloc(rdev, ring, 1);
+		if (!r) {
+			radeon_ring_write(ring, ring->nop);
+			radeon_ring_commit(rdev, ring);
+		}
+	}
+}
+
+void radeon_ring_lockup_update(struct radeon_ring *ring)
+{
+	ring->last_rptr = ring->rptr;
+	ring->last_activity = jiffies;
+}
+
+/**
+ * radeon_ring_test_lockup() - check if ring is lockedup by recording information
+ * @rdev:       radeon device structure
+ * @ring:       radeon_ring structure holding ring information
+ *
+ * We don't need to initialize the lockup tracking information as we will either
+ * have CP rptr to a different value of jiffies wrap around which will force
+ * initialization of the lockup tracking informations.
+ *
+ * A possible false positivie is if we get call after while and last_cp_rptr ==
+ * the current CP rptr, even if it's unlikely it might happen. To avoid this
+ * if the elapsed time since last call is bigger than 2 second than we return
+ * false and update the tracking information. Due to this the caller must call
+ * radeon_ring_test_lockup several time in less than 2sec for lockup to be reported
+ * the fencing code should be cautious about that.
+ *
+ * Caller should write to the ring to force CP to do something so we don't get
+ * false positive when CP is just gived nothing to do.
+ *
+ **/
+bool radeon_ring_test_lockup(struct radeon_device *rdev, struct radeon_ring *ring)
+{
+	unsigned long cjiffies, elapsed;
+	uint32_t rptr;
+
+	cjiffies = jiffies;
+	if (!time_after(cjiffies, ring->last_activity)) {
+		/* likely a wrap around */
+		radeon_ring_lockup_update(ring);
+		return false;
+	}
+	rptr = RREG32(ring->rptr_reg);
+	ring->rptr = (rptr & ring->ptr_reg_mask) >> ring->ptr_reg_shift;
+	if (ring->rptr != ring->last_rptr) {
+		/* CP is still working no lockup */
+		radeon_ring_lockup_update(ring);
+		return false;
+	}
+	elapsed = jiffies_to_msecs(cjiffies - ring->last_activity);
+	if (radeon_lockup_timeout && elapsed >= radeon_lockup_timeout) {
+		dev_err(rdev->dev, "GPU lockup CP stall for more than %lumsec\n", elapsed);
+		return true;
+	}
+	/* give a chance to the GPU ... */
+	return false;
 }
 
 int radeon_ring_init(struct radeon_device *rdev, struct radeon_ring *ring, unsigned ring_size,
@@ -385,8 +390,8 @@ int radeon_ring_init(struct radeon_device *rdev, struct radeon_ring *ring, unsig
 	/* Allocate ring buffer */
 	if (ring->ring_obj == NULL) {
 		r = radeon_bo_create(rdev, ring->ring_size, PAGE_SIZE, true,
-					RADEON_GEM_DOMAIN_GTT,
-					&ring->ring_obj);
+				     RADEON_GEM_DOMAIN_GTT,
+				     NULL, &ring->ring_obj);
 		if (r) {
 			dev_err(rdev->dev, "(%d) ring create failed\n", r);
 			return r;
@@ -411,6 +416,9 @@ int radeon_ring_init(struct radeon_device *rdev, struct radeon_ring *ring, unsig
 	}
 	ring->ptr_mask = (ring->ring_size / 4) - 1;
 	ring->ring_free_dw = ring->ring_size / 4;
+	if (radeon_debugfs_ring_init(rdev, ring)) {
+		DRM_ERROR("Failed to register debugfs file for rings !\n");
+	}
 	return 0;
 }
 
@@ -419,11 +427,12 @@ void radeon_ring_fini(struct radeon_device *rdev, struct radeon_ring *ring)
 	int r;
 	struct radeon_bo *ring_obj;
 
-	mutex_lock(&ring->mutex);
+	mutex_lock(&rdev->ring_lock);
 	ring_obj = ring->ring_obj;
+	ring->ready = false;
 	ring->ring = NULL;
 	ring->ring_obj = NULL;
-	mutex_unlock(&ring->mutex);
+	mutex_unlock(&rdev->ring_lock);
 
 	if (ring_obj) {
 		r = radeon_bo_reserve(ring_obj, false);
@@ -476,59 +485,48 @@ static struct drm_info_list radeon_debugfs_ring_info_list[] = {
 	{"radeon_ring_cp2", radeon_debugfs_ring_info, 0, &cayman_ring_type_cp2_index},
 };
 
-static int radeon_debugfs_ib_info(struct seq_file *m, void *data)
+static int radeon_debugfs_sa_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_ib *ib = &rdev->ib_pool.ibs[*((unsigned*)node->info_ent->data)];
-	unsigned i;
 
-	if (ib == NULL) {
-		return 0;
-	}
-	seq_printf(m, "IB %04u\n", ib->idx);
-	seq_printf(m, "IB fence %p\n", ib->fence);
-	seq_printf(m, "IB size %05u dwords\n", ib->length_dw);
-	for (i = 0; i < ib->length_dw; i++) {
-		seq_printf(m, "[%05u]=0x%08X\n", i, ib->ptr[i]);
-	}
+	radeon_sa_bo_dump_debug_info(&rdev->ring_tmp_bo, m);
+
 	return 0;
+
 }
 
-static struct drm_info_list radeon_debugfs_ib_list[RADEON_IB_POOL_SIZE];
-static char radeon_debugfs_ib_names[RADEON_IB_POOL_SIZE][32];
-static unsigned radeon_debugfs_ib_idx[RADEON_IB_POOL_SIZE];
+static struct drm_info_list radeon_debugfs_sa_list[] = {
+        {"radeon_sa_info", &radeon_debugfs_sa_info, 0, NULL},
+};
+
 #endif
 
-int radeon_debugfs_ring_init(struct radeon_device *rdev)
+int radeon_debugfs_ring_init(struct radeon_device *rdev, struct radeon_ring *ring)
 {
 #if defined(CONFIG_DEBUG_FS)
-	if (rdev->family >= CHIP_CAYMAN)
-		return radeon_debugfs_add_files(rdev, radeon_debugfs_ring_info_list,
-						ARRAY_SIZE(radeon_debugfs_ring_info_list));
-	else
-		return radeon_debugfs_add_files(rdev, radeon_debugfs_ring_info_list, 1);
-#else
-	return 0;
+	unsigned i;
+	for (i = 0; i < ARRAY_SIZE(radeon_debugfs_ring_info_list); ++i) {
+		struct drm_info_list *info = &radeon_debugfs_ring_info_list[i];
+		int ridx = *(int*)radeon_debugfs_ring_info_list[i].data;
+		unsigned r;
+
+		if (&rdev->ring[ridx] != ring)
+			continue;
+
+		r = radeon_debugfs_add_files(rdev, info, 1);
+		if (r)
+			return r;
+	}
 #endif
+	return 0;
 }
 
-int radeon_debugfs_ib_init(struct radeon_device *rdev)
+int radeon_debugfs_sa_init(struct radeon_device *rdev)
 {
 #if defined(CONFIG_DEBUG_FS)
-	unsigned i;
-
-	for (i = 0; i < RADEON_IB_POOL_SIZE; i++) {
-		sprintf(radeon_debugfs_ib_names[i], "radeon_ib_%04u", i);
-		radeon_debugfs_ib_idx[i] = i;
-		radeon_debugfs_ib_list[i].name = radeon_debugfs_ib_names[i];
-		radeon_debugfs_ib_list[i].show = &radeon_debugfs_ib_info;
-		radeon_debugfs_ib_list[i].driver_features = 0;
-		radeon_debugfs_ib_list[i].data = &radeon_debugfs_ib_idx[i];
-	}
-	return radeon_debugfs_add_files(rdev, radeon_debugfs_ib_list,
-					RADEON_IB_POOL_SIZE);
+	return radeon_debugfs_add_files(rdev, radeon_debugfs_sa_list, 1);
 #else
 	return 0;
 #endif
diff --git a/drivers/gpu/drm/radeon/radeon_sa.c b/drivers/gpu/drm/radeon/radeon_sa.c
index 4cce47e7dc0..32059b74572 100644
--- a/drivers/gpu/drm/radeon/radeon_sa.c
+++ b/drivers/gpu/drm/radeon/radeon_sa.c
@@ -27,23 +27,45 @@
  * Authors:
  *    Jerome Glisse <glisse@freedesktop.org>
  */
+/* Algorithm:
+ *
+ * We store the last allocated bo in "hole", we always try to allocate
+ * after the last allocated bo. Principle is that in a linear GPU ring
+ * progression was is after last is the oldest bo we allocated and thus
+ * the first one that should no longer be in use by the GPU.
+ *
+ * If it's not the case we skip over the bo after last to the closest
+ * done bo if such one exist. If none exist and we are not asked to
+ * block we report failure to allocate.
+ *
+ * If we are asked to block we wait on all the oldest fence of all
+ * rings. We just wait for any of those fence to complete.
+ */
 #include "drmP.h"
 #include "drm.h"
 #include "radeon.h"
 
+static void radeon_sa_bo_remove_locked(struct radeon_sa_bo *sa_bo);
+static void radeon_sa_bo_try_free(struct radeon_sa_manager *sa_manager);
+
 int radeon_sa_bo_manager_init(struct radeon_device *rdev,
 			      struct radeon_sa_manager *sa_manager,
 			      unsigned size, u32 domain)
 {
-	int r;
+	int i, r;
 
+	spin_lock_init(&sa_manager->lock);
 	sa_manager->bo = NULL;
 	sa_manager->size = size;
 	sa_manager->domain = domain;
-	INIT_LIST_HEAD(&sa_manager->sa_bo);
+	sa_manager->hole = &sa_manager->olist;
+	INIT_LIST_HEAD(&sa_manager->olist);
+	for (i = 0; i < RADEON_NUM_RINGS; ++i) {
+		INIT_LIST_HEAD(&sa_manager->flist[i]);
+	}
 
 	r = radeon_bo_create(rdev, size, RADEON_GPU_PAGE_SIZE, true,
-			     RADEON_GEM_DOMAIN_CPU, &sa_manager->bo);
+			     RADEON_GEM_DOMAIN_CPU, NULL, &sa_manager->bo);
 	if (r) {
 		dev_err(rdev->dev, "(%d) failed to allocate bo for manager\n", r);
 		return r;
@@ -57,11 +79,15 @@ void radeon_sa_bo_manager_fini(struct radeon_device *rdev,
 {
 	struct radeon_sa_bo *sa_bo, *tmp;
 
-	if (!list_empty(&sa_manager->sa_bo)) {
-		dev_err(rdev->dev, "sa_manager is not empty, clearing anyway\n");
+	if (!list_empty(&sa_manager->olist)) {
+		sa_manager->hole = &sa_manager->olist,
+		radeon_sa_bo_try_free(sa_manager);
+		if (!list_empty(&sa_manager->olist)) {
+			dev_err(rdev->dev, "sa_manager is not empty, clearing anyway\n");
+		}
 	}
-	list_for_each_entry_safe(sa_bo, tmp, &sa_manager->sa_bo, list) {
-		list_del_init(&sa_bo->list);
+	list_for_each_entry_safe(sa_bo, tmp, &sa_manager->olist, olist) {
+		radeon_sa_bo_remove_locked(sa_bo);
 	}
 	radeon_bo_unref(&sa_manager->bo);
 	sa_manager->size = 0;
@@ -113,77 +139,248 @@ int radeon_sa_bo_manager_suspend(struct radeon_device *rdev,
 	return r;
 }
 
-/*
- * Principe is simple, we keep a list of sub allocation in offset
- * order (first entry has offset == 0, last entry has the highest
- * offset).
- *
- * When allocating new object we first check if there is room at
- * the end total_size - (last_object_offset + last_object_size) >=
- * alloc_size. If so we allocate new object there.
- *
- * When there is not enough room at the end, we start waiting for
- * each sub object until we reach object_offset+object_size >=
- * alloc_size, this object then become the sub object we return.
- *
- * Alignment can't be bigger than page size
- */
+static void radeon_sa_bo_remove_locked(struct radeon_sa_bo *sa_bo)
+{
+	struct radeon_sa_manager *sa_manager = sa_bo->manager;
+	if (sa_manager->hole == &sa_bo->olist) {
+		sa_manager->hole = sa_bo->olist.prev;
+	}
+	list_del_init(&sa_bo->olist);
+	list_del_init(&sa_bo->flist);
+	radeon_fence_unref(&sa_bo->fence);
+	kfree(sa_bo);
+}
+
+static void radeon_sa_bo_try_free(struct radeon_sa_manager *sa_manager)
+{
+	struct radeon_sa_bo *sa_bo, *tmp;
+
+	if (sa_manager->hole->next == &sa_manager->olist)
+		return;
+
+	sa_bo = list_entry(sa_manager->hole->next, struct radeon_sa_bo, olist);
+	list_for_each_entry_safe_from(sa_bo, tmp, &sa_manager->olist, olist) {
+		if (sa_bo->fence == NULL || !radeon_fence_signaled(sa_bo->fence)) {
+			return;
+		}
+		radeon_sa_bo_remove_locked(sa_bo);
+	}
+}
+
+static inline unsigned radeon_sa_bo_hole_soffset(struct radeon_sa_manager *sa_manager)
+{
+	struct list_head *hole = sa_manager->hole;
+
+	if (hole != &sa_manager->olist) {
+		return list_entry(hole, struct radeon_sa_bo, olist)->eoffset;
+	}
+	return 0;
+}
+
+static inline unsigned radeon_sa_bo_hole_eoffset(struct radeon_sa_manager *sa_manager)
+{
+	struct list_head *hole = sa_manager->hole;
+
+	if (hole->next != &sa_manager->olist) {
+		return list_entry(hole->next, struct radeon_sa_bo, olist)->soffset;
+	}
+	return sa_manager->size;
+}
+
+static bool radeon_sa_bo_try_alloc(struct radeon_sa_manager *sa_manager,
+				   struct radeon_sa_bo *sa_bo,
+				   unsigned size, unsigned align)
+{
+	unsigned soffset, eoffset, wasted;
+
+	soffset = radeon_sa_bo_hole_soffset(sa_manager);
+	eoffset = radeon_sa_bo_hole_eoffset(sa_manager);
+	wasted = (align - (soffset % align)) % align;
+
+	if ((eoffset - soffset) >= (size + wasted)) {
+		soffset += wasted;
+
+		sa_bo->manager = sa_manager;
+		sa_bo->soffset = soffset;
+		sa_bo->eoffset = soffset + size;
+		list_add(&sa_bo->olist, sa_manager->hole);
+		INIT_LIST_HEAD(&sa_bo->flist);
+		sa_manager->hole = &sa_bo->olist;
+		return true;
+	}
+	return false;
+}
+
+static bool radeon_sa_bo_next_hole(struct radeon_sa_manager *sa_manager,
+				   struct radeon_fence **fences,
+				   unsigned *tries)
+{
+	struct radeon_sa_bo *best_bo = NULL;
+	unsigned i, soffset, best, tmp;
+
+	/* if hole points to the end of the buffer */
+	if (sa_manager->hole->next == &sa_manager->olist) {
+		/* try again with its beginning */
+		sa_manager->hole = &sa_manager->olist;
+		return true;
+	}
+
+	soffset = radeon_sa_bo_hole_soffset(sa_manager);
+	/* to handle wrap around we add sa_manager->size */
+	best = sa_manager->size * 2;
+	/* go over all fence list and try to find the closest sa_bo
+	 * of the current last
+	 */
+	for (i = 0; i < RADEON_NUM_RINGS; ++i) {
+		struct radeon_sa_bo *sa_bo;
+
+		if (list_empty(&sa_manager->flist[i])) {
+			continue;
+		}
+
+		sa_bo = list_first_entry(&sa_manager->flist[i],
+					 struct radeon_sa_bo, flist);
+
+		if (!radeon_fence_signaled(sa_bo->fence)) {
+			fences[i] = sa_bo->fence;
+			continue;
+		}
+
+		/* limit the number of tries each ring gets */
+		if (tries[i] > 2) {
+			continue;
+		}
+
+		tmp = sa_bo->soffset;
+		if (tmp < soffset) {
+			/* wrap around, pretend it's after */
+			tmp += sa_manager->size;
+		}
+		tmp -= soffset;
+		if (tmp < best) {
+			/* this sa bo is the closest one */
+			best = tmp;
+			best_bo = sa_bo;
+		}
+	}
+
+	if (best_bo) {
+		++tries[best_bo->fence->ring];
+		sa_manager->hole = best_bo->olist.prev;
+
+		/* we knew that this one is signaled,
+		   so it's save to remote it */
+		radeon_sa_bo_remove_locked(best_bo);
+		return true;
+	}
+	return false;
+}
+
 int radeon_sa_bo_new(struct radeon_device *rdev,
 		     struct radeon_sa_manager *sa_manager,
-		     struct radeon_sa_bo *sa_bo,
-		     unsigned size, unsigned align)
+		     struct radeon_sa_bo **sa_bo,
+		     unsigned size, unsigned align, bool block)
 {
-	struct radeon_sa_bo *tmp;
-	struct list_head *head;
-	unsigned offset = 0, wasted = 0;
+	struct radeon_fence *fences[RADEON_NUM_RINGS];
+	unsigned tries[RADEON_NUM_RINGS];
+	int i, r = -ENOMEM;
 
 	BUG_ON(align > RADEON_GPU_PAGE_SIZE);
 	BUG_ON(size > sa_manager->size);
 
-	/* no one ? */
-	head = sa_manager->sa_bo.prev;
-	if (list_empty(&sa_manager->sa_bo)) {
-		goto out;
+	*sa_bo = kmalloc(sizeof(struct radeon_sa_bo), GFP_KERNEL);
+	if ((*sa_bo) == NULL) {
+		return -ENOMEM;
 	}
+	(*sa_bo)->manager = sa_manager;
+	(*sa_bo)->fence = NULL;
+	INIT_LIST_HEAD(&(*sa_bo)->olist);
+	INIT_LIST_HEAD(&(*sa_bo)->flist);
 
-	/* look for a hole big enough */
-	offset = 0;
-	list_for_each_entry(tmp, &sa_manager->sa_bo, list) {
-		/* room before this object ? */
-		if ((tmp->offset - offset) >= size) {
-			head = tmp->list.prev;
-			goto out;
+	spin_lock(&sa_manager->lock);
+	do {
+		for (i = 0; i < RADEON_NUM_RINGS; ++i) {
+			fences[i] = NULL;
+			tries[i] = 0;
 		}
-		offset = tmp->offset + tmp->size;
-		wasted = offset % align;
-		if (wasted) {
-			wasted = align - wasted;
+
+		do {
+			radeon_sa_bo_try_free(sa_manager);
+
+			if (radeon_sa_bo_try_alloc(sa_manager, *sa_bo,
+						   size, align)) {
+				spin_unlock(&sa_manager->lock);
+				return 0;
+			}
+
+			/* see if we can skip over some allocations */
+		} while (radeon_sa_bo_next_hole(sa_manager, fences, tries));
+
+		if (block) {
+			spin_unlock(&sa_manager->lock);
+			r = radeon_fence_wait_any(rdev, fences, false);
+			spin_lock(&sa_manager->lock);
+			if (r) {
+				/* if we have nothing to wait for we
+				   are practically out of memory */
+				if (r == -ENOENT) {
+					r = -ENOMEM;
+				}
+				goto out_err;
+			}
 		}
-		offset += wasted;
-	}
-	/* room at the end ? */
-	head = sa_manager->sa_bo.prev;
-	tmp = list_entry(head, struct radeon_sa_bo, list);
-	offset = tmp->offset + tmp->size;
-	wasted = offset % align;
-	if (wasted) {
-		wasted = align - wasted;
-	}
-	offset += wasted;
-	if ((sa_manager->size - offset) < size) {
-		/* failed to find somethings big enough */
-		return -ENOMEM;
+	} while (block);
+
+out_err:
+	spin_unlock(&sa_manager->lock);
+	kfree(*sa_bo);
+	*sa_bo = NULL;
+	return r;
+}
+
+void radeon_sa_bo_free(struct radeon_device *rdev, struct radeon_sa_bo **sa_bo,
+		       struct radeon_fence *fence)
+{
+	struct radeon_sa_manager *sa_manager;
+
+	if (sa_bo == NULL || *sa_bo == NULL) {
+		return;
 	}
 
-out:
-	sa_bo->manager = sa_manager;
-	sa_bo->offset = offset;
-	sa_bo->size = size;
-	list_add(&sa_bo->list, head);
-	return 0;
+	sa_manager = (*sa_bo)->manager;
+	spin_lock(&sa_manager->lock);
+	if (fence && fence->seq && fence->seq < RADEON_FENCE_NOTEMITED_SEQ) {
+		(*sa_bo)->fence = radeon_fence_ref(fence);
+		list_add_tail(&(*sa_bo)->flist,
+			      &sa_manager->flist[fence->ring]);
+	} else {
+		radeon_sa_bo_remove_locked(*sa_bo);
+	}
+	spin_unlock(&sa_manager->lock);
+	*sa_bo = NULL;
 }
 
-void radeon_sa_bo_free(struct radeon_device *rdev, struct radeon_sa_bo *sa_bo)
+#if defined(CONFIG_DEBUG_FS)
+void radeon_sa_bo_dump_debug_info(struct radeon_sa_manager *sa_manager,
+				  struct seq_file *m)
 {
-	list_del_init(&sa_bo->list);
+	struct radeon_sa_bo *i;
+
+	spin_lock(&sa_manager->lock);
+	list_for_each_entry(i, &sa_manager->olist, olist) {
+		if (&i->olist == sa_manager->hole) {
+			seq_printf(m, ">");
+		} else {
+			seq_printf(m, " ");
+		}
+		seq_printf(m, "[0x%08x 0x%08x] size %8d",
+			   i->soffset, i->eoffset, i->eoffset - i->soffset);
+		if (i->fence) {
+			seq_printf(m, " protected by 0x%016llx on ring %d",
+				   i->fence->seq, i->fence->ring);
+		}
+		seq_printf(m, "\n");
+	}
+	spin_unlock(&sa_manager->lock);
 }
+#endif
diff --git a/drivers/gpu/drm/radeon/radeon_semaphore.c b/drivers/gpu/drm/radeon/radeon_semaphore.c
index 61dd4e3c920..e2ace5dce11 100644
--- a/drivers/gpu/drm/radeon/radeon_semaphore.c
+++ b/drivers/gpu/drm/radeon/radeon_semaphore.c
@@ -31,148 +31,107 @@
 #include "drm.h"
 #include "radeon.h"
 
-static int radeon_semaphore_add_bo(struct radeon_device *rdev)
-{
-	struct radeon_semaphore_bo *bo;
-	unsigned long irq_flags;
-	uint64_t gpu_addr;
-	uint32_t *cpu_ptr;
-	int r, i;
-
-
-	bo = kmalloc(sizeof(struct radeon_semaphore_bo), GFP_KERNEL);
-	if (bo == NULL) {
-		return -ENOMEM;
-	}
-	INIT_LIST_HEAD(&bo->free);
-	INIT_LIST_HEAD(&bo->list);
-	bo->nused = 0;
-
-	r = radeon_ib_get(rdev, 0, &bo->ib, RADEON_SEMAPHORE_BO_SIZE);
-	if (r) {
-		dev_err(rdev->dev, "failed to get a bo after 5 retry\n");
-		kfree(bo);
-		return r;
-	}
-	gpu_addr = rdev->ib_pool.sa_manager.gpu_addr;
-	gpu_addr += bo->ib->sa_bo.offset;
-	cpu_ptr = rdev->ib_pool.sa_manager.cpu_ptr;
-	cpu_ptr += (bo->ib->sa_bo.offset >> 2);
-	for (i = 0; i < (RADEON_SEMAPHORE_BO_SIZE/8); i++) {
-		bo->semaphores[i].gpu_addr = gpu_addr;
-		bo->semaphores[i].cpu_ptr = cpu_ptr;
-		bo->semaphores[i].bo = bo;
-		list_add_tail(&bo->semaphores[i].list, &bo->free);
-		gpu_addr += 8;
-		cpu_ptr += 2;
-	}
-	write_lock_irqsave(&rdev->semaphore_drv.lock, irq_flags);
-	list_add_tail(&bo->list, &rdev->semaphore_drv.bo);
-	write_unlock_irqrestore(&rdev->semaphore_drv.lock, irq_flags);
-	return 0;
-}
-
-static void radeon_semaphore_del_bo_locked(struct radeon_device *rdev,
-					   struct radeon_semaphore_bo *bo)
-{
-	radeon_sa_bo_free(rdev, &bo->ib->sa_bo);
-	radeon_fence_unref(&bo->ib->fence);
-	list_del(&bo->list);
-	kfree(bo);
-}
-
-void radeon_semaphore_shrink_locked(struct radeon_device *rdev)
-{
-	struct radeon_semaphore_bo *bo, *n;
-
-	if (list_empty(&rdev->semaphore_drv.bo)) {
-		return;
-	}
-	/* only shrink if first bo has free semaphore */
-	bo = list_first_entry(&rdev->semaphore_drv.bo, struct radeon_semaphore_bo, list);
-	if (list_empty(&bo->free)) {
-		return;
-	}
-	list_for_each_entry_safe_continue(bo, n, &rdev->semaphore_drv.bo, list) {
-		if (bo->nused)
-			continue;
-		radeon_semaphore_del_bo_locked(rdev, bo);
-	}
-}
 
 int radeon_semaphore_create(struct radeon_device *rdev,
 			    struct radeon_semaphore **semaphore)
 {
-	struct radeon_semaphore_bo *bo;
-	unsigned long irq_flags;
-	bool do_retry = true;
 	int r;
 
-retry:
-	*semaphore = NULL;
-	write_lock_irqsave(&rdev->semaphore_drv.lock, irq_flags);
-	list_for_each_entry(bo, &rdev->semaphore_drv.bo, list) {
-		if (list_empty(&bo->free))
-			continue;
-		*semaphore = list_first_entry(&bo->free, struct radeon_semaphore, list);
-		(*semaphore)->cpu_ptr[0] = 0;
-		(*semaphore)->cpu_ptr[1] = 0;
-		list_del(&(*semaphore)->list);
-		bo->nused++;
-		break;
-	}
-	write_unlock_irqrestore(&rdev->semaphore_drv.lock, irq_flags);
-
+	*semaphore = kmalloc(sizeof(struct radeon_semaphore), GFP_KERNEL);
 	if (*semaphore == NULL) {
-		if (do_retry) {
-			do_retry = false;
-			r = radeon_semaphore_add_bo(rdev);
-			if (r)
-				return r;
-			goto retry;
-		}
 		return -ENOMEM;
 	}
-
+	r = radeon_sa_bo_new(rdev, &rdev->ring_tmp_bo,
+			     &(*semaphore)->sa_bo, 8, 8, true);
+	if (r) {
+		kfree(*semaphore);
+		*semaphore = NULL;
+		return r;
+	}
+	(*semaphore)->waiters = 0;
+	(*semaphore)->gpu_addr = radeon_sa_bo_gpu_addr((*semaphore)->sa_bo);
+	*((uint64_t*)radeon_sa_bo_cpu_addr((*semaphore)->sa_bo)) = 0;
 	return 0;
 }
 
 void radeon_semaphore_emit_signal(struct radeon_device *rdev, int ring,
 			          struct radeon_semaphore *semaphore)
 {
+	--semaphore->waiters;
 	radeon_semaphore_ring_emit(rdev, ring, &rdev->ring[ring], semaphore, false);
 }
 
 void radeon_semaphore_emit_wait(struct radeon_device *rdev, int ring,
 			        struct radeon_semaphore *semaphore)
 {
+	++semaphore->waiters;
 	radeon_semaphore_ring_emit(rdev, ring, &rdev->ring[ring], semaphore, true);
 }
 
-void radeon_semaphore_free(struct radeon_device *rdev,
-			   struct radeon_semaphore *semaphore)
+int radeon_semaphore_sync_rings(struct radeon_device *rdev,
+				struct radeon_semaphore *semaphore,
+				bool sync_to[RADEON_NUM_RINGS],
+				int dst_ring)
 {
-	unsigned long irq_flags;
+	int i = 0, r;
+
+	mutex_lock(&rdev->ring_lock);
+	r = radeon_ring_alloc(rdev, &rdev->ring[dst_ring], RADEON_NUM_RINGS * 8);
+	if (r) {
+		goto error;
+	}
+
+	for (i = 0; i < RADEON_NUM_RINGS; ++i) {
+		/* no need to sync to our own or unused rings */
+		if (!sync_to[i] || i == dst_ring)
+			continue;
 
-	write_lock_irqsave(&rdev->semaphore_drv.lock, irq_flags);
-	semaphore->bo->nused--;
-	list_add_tail(&semaphore->list, &semaphore->bo->free);
-	radeon_semaphore_shrink_locked(rdev);
-	write_unlock_irqrestore(&rdev->semaphore_drv.lock, irq_flags);
+		/* prevent GPU deadlocks */
+		if (!rdev->ring[i].ready) {
+			dev_err(rdev->dev, "Trying to sync to a disabled ring!");
+			r = -EINVAL;
+			goto error;
+		}
+
+		r = radeon_ring_alloc(rdev, &rdev->ring[i], 8);
+		if (r) {
+			goto error;
+		}
+
+		radeon_semaphore_emit_signal(rdev, i, semaphore);
+		radeon_semaphore_emit_wait(rdev, dst_ring, semaphore);
+
+		radeon_ring_commit(rdev, &rdev->ring[i]);
+	}
+
+	radeon_ring_commit(rdev, &rdev->ring[dst_ring]);
+	mutex_unlock(&rdev->ring_lock);
+
+	return 0;
+
+error:
+	/* unlock all locks taken so far */
+	for (--i; i >= 0; --i) {
+		if (sync_to[i] || i == dst_ring) {
+			radeon_ring_undo(&rdev->ring[i]);
+		}
+	}
+	radeon_ring_undo(&rdev->ring[dst_ring]);
+	mutex_unlock(&rdev->ring_lock);
+	return r;
 }
 
-void radeon_semaphore_driver_fini(struct radeon_device *rdev)
+void radeon_semaphore_free(struct radeon_device *rdev,
+			   struct radeon_semaphore *semaphore,
+			   struct radeon_fence *fence)
 {
-	struct radeon_semaphore_bo *bo, *n;
-	unsigned long irq_flags;
-
-	write_lock_irqsave(&rdev->semaphore_drv.lock, irq_flags);
-	/* we force to free everything */
-	list_for_each_entry_safe(bo, n, &rdev->semaphore_drv.bo, list) {
-		if (!list_empty(&bo->free)) {
-			dev_err(rdev->dev, "still in use semaphore\n");
-		}
-		radeon_semaphore_del_bo_locked(rdev, bo);
+	if (semaphore == NULL) {
+		return;
+	}
+	if (semaphore->waiters > 0) {
+		dev_err(rdev->dev, "semaphore %p has more waiters than signalers,"
+			" hardware lockup imminent!\n", semaphore);
 	}
-	write_unlock_irqrestore(&rdev->semaphore_drv.lock, irq_flags);
+	radeon_sa_bo_free(rdev, &semaphore->sa_bo, fence);
+	kfree(semaphore);
 }
diff --git a/drivers/gpu/drm/radeon/radeon_test.c b/drivers/gpu/drm/radeon/radeon_test.c
index dc5dcf483aa..efff929ea49 100644
--- a/drivers/gpu/drm/radeon/radeon_test.c
+++ b/drivers/gpu/drm/radeon/radeon_test.c
@@ -59,7 +59,7 @@ void radeon_test_moves(struct radeon_device *rdev)
 	}
 
 	r = radeon_bo_create(rdev, size, PAGE_SIZE, true, RADEON_GEM_DOMAIN_VRAM,
-				&vram_obj);
+			     NULL, &vram_obj);
 	if (r) {
 		DRM_ERROR("Failed to create VRAM object\n");
 		goto out_cleanup;
@@ -78,7 +78,7 @@ void radeon_test_moves(struct radeon_device *rdev)
 		void **vram_start, **vram_end;
 
 		r = radeon_bo_create(rdev, size, PAGE_SIZE, true,
-					 RADEON_GEM_DOMAIN_GTT, gtt_obj + i);
+				     RADEON_GEM_DOMAIN_GTT, NULL, gtt_obj + i);
 		if (r) {
 			DRM_ERROR("Failed to create GTT object %d\n", i);
 			goto out_cleanup;
@@ -317,7 +317,7 @@ void radeon_test_ring_sync(struct radeon_device *rdev,
 
 out_cleanup:
 	if (semaphore)
-		radeon_semaphore_free(rdev, semaphore);
+		radeon_semaphore_free(rdev, semaphore, NULL);
 
 	if (fence1)
 		radeon_fence_unref(&fence1);
@@ -437,7 +437,7 @@ void radeon_test_ring_sync2(struct radeon_device *rdev,
 
 out_cleanup:
 	if (semaphore)
-		radeon_semaphore_free(rdev, semaphore);
+		radeon_semaphore_free(rdev, semaphore, NULL);
 
 	if (fenceA)
 		radeon_fence_unref(&fenceA);
diff --git a/drivers/gpu/drm/radeon/radeon_ttm.c b/drivers/gpu/drm/radeon/radeon_ttm.c
index f493c6403af..c94a2257761 100644
--- a/drivers/gpu/drm/radeon/radeon_ttm.c
+++ b/drivers/gpu/drm/radeon/radeon_ttm.c
@@ -222,8 +222,9 @@ static int radeon_move_blit(struct ttm_buffer_object *bo,
 {
 	struct radeon_device *rdev;
 	uint64_t old_start, new_start;
-	struct radeon_fence *fence;
-	int r, i;
+	struct radeon_fence *fence, *old_fence;
+	struct radeon_semaphore *sem = NULL;
+	int r;
 
 	rdev = radeon_get_rdev(bo->bdev);
 	r = radeon_fence_create(rdev, &fence, radeon_copy_ring_index(rdev));
@@ -242,6 +243,7 @@ static int radeon_move_blit(struct ttm_buffer_object *bo,
 		break;
 	default:
 		DRM_ERROR("Unknown placement %d\n", old_mem->mem_type);
+		radeon_fence_unref(&fence);
 		return -EINVAL;
 	}
 	switch (new_mem->mem_type) {
@@ -253,42 +255,36 @@ static int radeon_move_blit(struct ttm_buffer_object *bo,
 		break;
 	default:
 		DRM_ERROR("Unknown placement %d\n", old_mem->mem_type);
+		radeon_fence_unref(&fence);
 		return -EINVAL;
 	}
 	if (!rdev->ring[radeon_copy_ring_index(rdev)].ready) {
 		DRM_ERROR("Trying to move memory with ring turned off.\n");
+		radeon_fence_unref(&fence);
 		return -EINVAL;
 	}
 
 	BUILD_BUG_ON((PAGE_SIZE % RADEON_GPU_PAGE_SIZE) != 0);
 
 	/* sync other rings */
-	if (rdev->family >= CHIP_R600) {
-		for (i = 0; i < RADEON_NUM_RINGS; ++i) {
-			/* no need to sync to our own or unused rings */
-			if (i == radeon_copy_ring_index(rdev) || !rdev->ring[i].ready)
-				continue;
-
-			if (!fence->semaphore) {
-				r = radeon_semaphore_create(rdev, &fence->semaphore);
-				/* FIXME: handle semaphore error */
-				if (r)
-					continue;
-			}
+	old_fence = bo->sync_obj;
+	if (old_fence && old_fence->ring != fence->ring
+	    && !radeon_fence_signaled(old_fence)) {
+		bool sync_to_ring[RADEON_NUM_RINGS] = { };
+		sync_to_ring[old_fence->ring] = true;
+
+		r = radeon_semaphore_create(rdev, &sem);
+		if (r) {
+			radeon_fence_unref(&fence);
+			return r;
+		}
 
-			r = radeon_ring_lock(rdev, &rdev->ring[i], 3);
-			/* FIXME: handle ring lock error */
-			if (r)
-				continue;
-			radeon_semaphore_emit_signal(rdev, i, fence->semaphore);
-			radeon_ring_unlock_commit(rdev, &rdev->ring[i]);
-
-			r = radeon_ring_lock(rdev, &rdev->ring[radeon_copy_ring_index(rdev)], 3);
-			/* FIXME: handle ring lock error */
-			if (r)
-				continue;
-			radeon_semaphore_emit_wait(rdev, radeon_copy_ring_index(rdev), fence->semaphore);
-			radeon_ring_unlock_commit(rdev, &rdev->ring[radeon_copy_ring_index(rdev)]);
+		r = radeon_semaphore_sync_rings(rdev, sem,
+						sync_to_ring, fence->ring);
+		if (r) {
+			radeon_semaphore_free(rdev, sem, NULL);
+			radeon_fence_unref(&fence);
+			return r;
 		}
 	}
 
@@ -298,6 +294,7 @@ static int radeon_move_blit(struct ttm_buffer_object *bo,
 	/* FIXME: handle copy error */
 	r = ttm_bo_move_accel_cleanup(bo, (void *)fence, NULL,
 				      evict, no_wait_reserve, no_wait_gpu, new_mem);
+	radeon_semaphore_free(rdev, sem, fence);
 	radeon_fence_unref(&fence);
 	return r;
 }
@@ -614,10 +611,18 @@ static int radeon_ttm_tt_populate(struct ttm_tt *ttm)
 	struct radeon_ttm_tt *gtt = (void *)ttm;
 	unsigned i;
 	int r;
+	bool slave = !!(ttm->page_flags & TTM_PAGE_FLAG_SG);
 
 	if (ttm->state != tt_unpopulated)
 		return 0;
 
+	if (slave && ttm->sg) {
+		drm_prime_sg_to_page_addr_arrays(ttm->sg, ttm->pages,
+						 gtt->ttm.dma_address, ttm->num_pages);
+		ttm->state = tt_unbound;
+		return 0;
+	}
+
 	rdev = radeon_get_rdev(ttm->bdev);
 #if __OS_HAS_AGP
 	if (rdev->flags & RADEON_IS_AGP) {
@@ -658,6 +663,10 @@ static void radeon_ttm_tt_unpopulate(struct ttm_tt *ttm)
 	struct radeon_device *rdev;
 	struct radeon_ttm_tt *gtt = (void *)ttm;
 	unsigned i;
+	bool slave = !!(ttm->page_flags & TTM_PAGE_FLAG_SG);
+
+	if (slave)
+		return;
 
 	rdev = radeon_get_rdev(ttm->bdev);
 #if __OS_HAS_AGP
@@ -729,8 +738,8 @@ int radeon_ttm_init(struct radeon_device *rdev)
 		return r;
 	}
 	r = radeon_bo_create(rdev, 256 * 1024, PAGE_SIZE, true,
-				RADEON_GEM_DOMAIN_VRAM,
-				&rdev->stollen_vga_memory);
+			     RADEON_GEM_DOMAIN_VRAM,
+			     NULL, &rdev->stollen_vga_memory);
 	if (r) {
 		return r;
 	}
diff --git a/drivers/gpu/drm/radeon/rs400.c b/drivers/gpu/drm/radeon/rs400.c
index 4cf381b3a6d..a464eb5e2df 100644
--- a/drivers/gpu/drm/radeon/rs400.c
+++ b/drivers/gpu/drm/radeon/rs400.c
@@ -430,12 +430,9 @@ static int rs400_startup(struct radeon_device *rdev)
 	if (r)
 		return r;
 
-	r = radeon_ib_test(rdev, RADEON_RING_TYPE_GFX_INDEX, &rdev->ring[RADEON_RING_TYPE_GFX_INDEX]);
-	if (r) {
-		dev_err(rdev->dev, "failed testing IB (%d).\n", r);
-		rdev->accel_working = false;
+	r = radeon_ib_ring_tests(rdev);
+	if (r)
 		return r;
-	}
 
 	return 0;
 }
diff --git a/drivers/gpu/drm/radeon/rs600.c b/drivers/gpu/drm/radeon/rs600.c
index d25cf869d08..25f9eef12c4 100644
--- a/drivers/gpu/drm/radeon/rs600.c
+++ b/drivers/gpu/drm/radeon/rs600.c
@@ -396,7 +396,6 @@ int rs600_asic_reset(struct radeon_device *rdev)
 	/* Check if GPU is idle */
 	if (G_000E40_GA_BUSY(status) || G_000E40_VAP_BUSY(status)) {
 		dev_err(rdev->dev, "failed to reset GPU\n");
-		rdev->gpu_lockup = true;
 		ret = -1;
 	} else
 		dev_info(rdev->dev, "GPU reset succeed\n");
@@ -553,6 +552,12 @@ int rs600_irq_set(struct radeon_device *rdev)
 		~S_007D08_DC_HOT_PLUG_DETECT1_INT_EN(1);
 	u32 hpd2 = RREG32(R_007D18_DC_HOT_PLUG_DETECT2_INT_CONTROL) &
 		~S_007D18_DC_HOT_PLUG_DETECT2_INT_EN(1);
+	u32 hdmi0;
+	if (ASIC_IS_DCE2(rdev))
+		hdmi0 = RREG32(R_007408_HDMI0_AUDIO_PACKET_CONTROL) &
+			~S_007408_HDMI0_AZ_FORMAT_WTRIG_MASK(1);
+	else
+		hdmi0 = 0;
 
 	if (!rdev->irq.installed) {
 		WARN(1, "Can't enable IRQ/MSI because no handler is installed\n");
@@ -579,10 +584,15 @@ int rs600_irq_set(struct radeon_device *rdev)
 	if (rdev->irq.hpd[1]) {
 		hpd2 |= S_007D18_DC_HOT_PLUG_DETECT2_INT_EN(1);
 	}
+	if (rdev->irq.afmt[0]) {
+		hdmi0 |= S_007408_HDMI0_AZ_FORMAT_WTRIG_MASK(1);
+	}
 	WREG32(R_000040_GEN_INT_CNTL, tmp);
 	WREG32(R_006540_DxMODE_INT_MASK, mode_int);
 	WREG32(R_007D08_DC_HOT_PLUG_DETECT1_INT_CONTROL, hpd1);
 	WREG32(R_007D18_DC_HOT_PLUG_DETECT2_INT_CONTROL, hpd2);
+	if (ASIC_IS_DCE2(rdev))
+		WREG32(R_007408_HDMI0_AUDIO_PACKET_CONTROL, hdmi0);
 	return 0;
 }
 
@@ -622,6 +632,17 @@ static inline u32 rs600_irq_ack(struct radeon_device *rdev)
 		rdev->irq.stat_regs.r500.disp_int = 0;
 	}
 
+	if (ASIC_IS_DCE2(rdev)) {
+		rdev->irq.stat_regs.r500.hdmi0_status = RREG32(R_007404_HDMI0_STATUS) &
+			S_007404_HDMI0_AZ_FORMAT_WTRIG(1);
+		if (G_007404_HDMI0_AZ_FORMAT_WTRIG(rdev->irq.stat_regs.r500.hdmi0_status)) {
+			tmp = RREG32(R_007408_HDMI0_AUDIO_PACKET_CONTROL);
+			tmp |= S_007408_HDMI0_AZ_FORMAT_WTRIG_ACK(1);
+			WREG32(R_007408_HDMI0_AUDIO_PACKET_CONTROL, tmp);
+		}
+	} else
+		rdev->irq.stat_regs.r500.hdmi0_status = 0;
+
 	if (irqs) {
 		WREG32(R_000044_GEN_INT_STATUS, irqs);
 	}
@@ -630,6 +651,9 @@ static inline u32 rs600_irq_ack(struct radeon_device *rdev)
 
 void rs600_irq_disable(struct radeon_device *rdev)
 {
+	u32 hdmi0 = RREG32(R_007408_HDMI0_AUDIO_PACKET_CONTROL) &
+		~S_007408_HDMI0_AZ_FORMAT_WTRIG_MASK(1);
+	WREG32(R_007408_HDMI0_AUDIO_PACKET_CONTROL, hdmi0);
 	WREG32(R_000040_GEN_INT_CNTL, 0);
 	WREG32(R_006540_DxMODE_INT_MASK, 0);
 	/* Wait and acknowledge irq */
@@ -641,15 +665,20 @@ int rs600_irq_process(struct radeon_device *rdev)
 {
 	u32 status, msi_rearm;
 	bool queue_hotplug = false;
+	bool queue_hdmi = false;
 
 	/* reset gui idle ack.  the status bit is broken */
 	rdev->irq.gui_idle_acked = false;
 
 	status = rs600_irq_ack(rdev);
-	if (!status && !rdev->irq.stat_regs.r500.disp_int) {
+	if (!status &&
+	    !rdev->irq.stat_regs.r500.disp_int &&
+	    !rdev->irq.stat_regs.r500.hdmi0_status) {
 		return IRQ_NONE;
 	}
-	while (status || rdev->irq.stat_regs.r500.disp_int) {
+	while (status ||
+	       rdev->irq.stat_regs.r500.disp_int ||
+	       rdev->irq.stat_regs.r500.hdmi0_status) {
 		/* SW interrupt */
 		if (G_000044_SW_INT(status)) {
 			radeon_fence_process(rdev, RADEON_RING_TYPE_GFX_INDEX);
@@ -687,12 +716,18 @@ int rs600_irq_process(struct radeon_device *rdev)
 			queue_hotplug = true;
 			DRM_DEBUG("HPD2\n");
 		}
+		if (G_007404_HDMI0_AZ_FORMAT_WTRIG(rdev->irq.stat_regs.r500.hdmi0_status)) {
+			queue_hdmi = true;
+			DRM_DEBUG("HDMI0\n");
+		}
 		status = rs600_irq_ack(rdev);
 	}
 	/* reset gui idle ack.  the status bit is broken */
 	rdev->irq.gui_idle_acked = false;
 	if (queue_hotplug)
 		schedule_work(&rdev->hotplug_work);
+	if (queue_hdmi)
+		schedule_work(&rdev->audio_work);
 	if (rdev->msi_enabled) {
 		switch (rdev->family) {
 		case CHIP_RS600:
@@ -883,12 +918,9 @@ static int rs600_startup(struct radeon_device *rdev)
 	if (r)
 		return r;
 
-	r = radeon_ib_test(rdev, RADEON_RING_TYPE_GFX_INDEX, &rdev->ring[RADEON_RING_TYPE_GFX_INDEX]);
-	if (r) {
-		dev_err(rdev->dev, "failed testing IB (%d).\n", r);
-		rdev->accel_working = false;
+	r = radeon_ib_ring_tests(rdev);
+	if (r)
 		return r;
-	}
 
 	return 0;
 }
diff --git a/drivers/gpu/drm/radeon/rs600d.h b/drivers/gpu/drm/radeon/rs600d.h
index a27c13ac47c..f1f89414dc6 100644
--- a/drivers/gpu/drm/radeon/rs600d.h
+++ b/drivers/gpu/drm/radeon/rs600d.h
@@ -485,6 +485,20 @@
 #define   S_007D18_DC_HOT_PLUG_DETECT2_INT_EN(x)       (((x) & 0x1) << 16)
 #define   G_007D18_DC_HOT_PLUG_DETECT2_INT_EN(x)       (((x) >> 16) & 0x1)
 #define   C_007D18_DC_HOT_PLUG_DETECT2_INT_EN          0xFFFEFFFF
+#define R_007404_HDMI0_STATUS                          0x007404
+#define   S_007404_HDMI0_AZ_FORMAT_WTRIG(x)            (((x) & 0x1) << 28)
+#define   G_007404_HDMI0_AZ_FORMAT_WTRIG(x)            (((x) >> 28) & 0x1)
+#define   C_007404_HDMI0_AZ_FORMAT_WTRIG               0xEFFFFFFF
+#define   S_007404_HDMI0_AZ_FORMAT_WTRIG_INT(x)        (((x) & 0x1) << 29)
+#define   G_007404_HDMI0_AZ_FORMAT_WTRIG_INT(x)        (((x) >> 29) & 0x1)
+#define   C_007404_HDMI0_AZ_FORMAT_WTRIG_INT           0xDFFFFFFF
+#define R_007408_HDMI0_AUDIO_PACKET_CONTROL            0x007408
+#define   S_007408_HDMI0_AZ_FORMAT_WTRIG_MASK(x)       (((x) & 0x1) << 28)
+#define   G_007408_HDMI0_AZ_FORMAT_WTRIG_MASK(x)       (((x) >> 28) & 0x1)
+#define   C_007408_HDMI0_AZ_FORMAT_WTRIG_MASK          0xEFFFFFFF
+#define   S_007408_HDMI0_AZ_FORMAT_WTRIG_ACK(x)        (((x) & 0x1) << 29)
+#define   G_007408_HDMI0_AZ_FORMAT_WTRIG_ACK(x)        (((x) >> 29) & 0x1)
+#define   C_007408_HDMI0_AZ_FORMAT_WTRIG_ACK           0xDFFFFFFF
 
 /* MC registers */
 #define R_000000_MC_STATUS                           0x000000
diff --git a/drivers/gpu/drm/radeon/rs690.c b/drivers/gpu/drm/radeon/rs690.c
index f2c3b9d75f1..3277ddecfe9 100644
--- a/drivers/gpu/drm/radeon/rs690.c
+++ b/drivers/gpu/drm/radeon/rs690.c
@@ -647,12 +647,9 @@ static int rs690_startup(struct radeon_device *rdev)
 	if (r)
 		return r;
 
-	r = radeon_ib_test(rdev, RADEON_RING_TYPE_GFX_INDEX, &rdev->ring[RADEON_RING_TYPE_GFX_INDEX]);
-	if (r) {
-		dev_err(rdev->dev, "failed testing IB (%d).\n", r);
-		rdev->accel_working = false;
+	r = radeon_ib_ring_tests(rdev);
+	if (r)
 		return r;
-	}
 
 	return 0;
 }
diff --git a/drivers/gpu/drm/radeon/rv515.c b/drivers/gpu/drm/radeon/rv515.c
index d8d78fe1794..7f08cedb533 100644
--- a/drivers/gpu/drm/radeon/rv515.c
+++ b/drivers/gpu/drm/radeon/rv515.c
@@ -412,12 +412,10 @@ static int rv515_startup(struct radeon_device *rdev)
 	if (r)
 		return r;
 
-	r = radeon_ib_test(rdev, RADEON_RING_TYPE_GFX_INDEX, &rdev->ring[RADEON_RING_TYPE_GFX_INDEX]);
-	if (r) {
-		dev_err(rdev->dev, "failed testing IB (%d).\n", r);
-		rdev->accel_working = false;
+	r = radeon_ib_ring_tests(rdev);
+	if (r)
 		return r;
-	}
+
 	return 0;
 }
 
diff --git a/drivers/gpu/drm/radeon/rv770.c b/drivers/gpu/drm/radeon/rv770.c
index cdab1aeaed6..c2f473bc13b 100644
--- a/drivers/gpu/drm/radeon/rv770.c
+++ b/drivers/gpu/drm/radeon/rv770.c
@@ -1114,12 +1114,9 @@ static int rv770_startup(struct radeon_device *rdev)
 	if (r)
 		return r;
 
-	r = radeon_ib_test(rdev, RADEON_RING_TYPE_GFX_INDEX, &rdev->ring[RADEON_RING_TYPE_GFX_INDEX]);
-	if (r) {
-		dev_err(rdev->dev, "IB test failed (%d).\n", r);
-		rdev->accel_working = false;
+	r = radeon_ib_ring_tests(rdev);
+	if (r)
 		return r;
-	}
 
 	return 0;
 }
@@ -1178,10 +1175,6 @@ int rv770_init(struct radeon_device *rdev)
 {
 	int r;
 
-	/* This don't do much */
-	r = radeon_gem_init(rdev);
-	if (r)
-		return r;
 	/* Read BIOS */
 	if (!radeon_get_bios(rdev)) {
 		if (ASIC_IS_AVIVO(rdev))
@@ -1281,7 +1274,6 @@ void rv770_fini(struct radeon_device *rdev)
 	rv770_pcie_gart_fini(rdev);
 	r600_vram_scratch_fini(rdev);
 	radeon_gem_fini(rdev);
-	radeon_semaphore_driver_fini(rdev);
 	radeon_fence_driver_fini(rdev);
 	radeon_agp_fini(rdev);
 	radeon_bo_fini(rdev);
diff --git a/drivers/gpu/drm/radeon/rv770d.h b/drivers/gpu/drm/radeon/rv770d.h
index 79fa588e9ed..9c549f702f2 100644
--- a/drivers/gpu/drm/radeon/rv770d.h
+++ b/drivers/gpu/drm/radeon/rv770d.h
@@ -353,6 +353,197 @@
 
 #define	SRBM_STATUS				        0x0E50
 
+/* DCE 3.2 HDMI */
+#define HDMI_CONTROL                         0x7400
+#       define HDMI_KEEPOUT_MODE             (1 << 0)
+#       define HDMI_PACKET_GEN_VERSION       (1 << 4) /* 0 = r6xx compat */
+#       define HDMI_ERROR_ACK                (1 << 8)
+#       define HDMI_ERROR_MASK               (1 << 9)
+#define HDMI_STATUS                          0x7404
+#       define HDMI_ACTIVE_AVMUTE            (1 << 0)
+#       define HDMI_AUDIO_PACKET_ERROR       (1 << 16)
+#       define HDMI_VBI_PACKET_ERROR         (1 << 20)
+#define HDMI_AUDIO_PACKET_CONTROL            0x7408
+#       define HDMI_AUDIO_DELAY_EN(x)        (((x) & 3) << 4)
+#       define HDMI_AUDIO_PACKETS_PER_LINE(x)  (((x) & 0x1f) << 16)
+#define HDMI_ACR_PACKET_CONTROL              0x740c
+#       define HDMI_ACR_SEND                 (1 << 0)
+#       define HDMI_ACR_CONT                 (1 << 1)
+#       define HDMI_ACR_SELECT(x)            (((x) & 3) << 4)
+#       define HDMI_ACR_HW                   0
+#       define HDMI_ACR_32                   1
+#       define HDMI_ACR_44                   2
+#       define HDMI_ACR_48                   3
+#       define HDMI_ACR_SOURCE               (1 << 8) /* 0 - hw; 1 - cts value */
+#       define HDMI_ACR_AUTO_SEND            (1 << 12)
+#define HDMI_VBI_PACKET_CONTROL              0x7410
+#       define HDMI_NULL_SEND                (1 << 0)
+#       define HDMI_GC_SEND                  (1 << 4)
+#       define HDMI_GC_CONT                  (1 << 5) /* 0 - once; 1 - every frame */
+#define HDMI_INFOFRAME_CONTROL0              0x7414
+#       define HDMI_AVI_INFO_SEND            (1 << 0)
+#       define HDMI_AVI_INFO_CONT            (1 << 1)
+#       define HDMI_AUDIO_INFO_SEND          (1 << 4)
+#       define HDMI_AUDIO_INFO_CONT          (1 << 5)
+#       define HDMI_MPEG_INFO_SEND           (1 << 8)
+#       define HDMI_MPEG_INFO_CONT           (1 << 9)
+#define HDMI_INFOFRAME_CONTROL1              0x7418
+#       define HDMI_AVI_INFO_LINE(x)         (((x) & 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          0x741c
+#       define HDMI_GENERIC0_SEND            (1 << 0)
+#       define HDMI_GENERIC0_CONT            (1 << 1)
+#       define HDMI_GENERIC1_SEND            (1 << 4)
+#       define HDMI_GENERIC1_CONT            (1 << 5)
+#       define HDMI_GENERIC0_LINE(x)         (((x) & 0x3f) << 16)
+#       define HDMI_GENERIC1_LINE(x)         (((x) & 0x3f) << 24)
+#define HDMI_GC                              0x7428
+#       define HDMI_GC_AVMUTE                (1 << 0)
+#define AFMT_AUDIO_PACKET_CONTROL2           0x742c
+#       define AFMT_AUDIO_LAYOUT_OVRD        (1 << 0)
+#       define AFMT_AUDIO_LAYOUT_SELECT      (1 << 1)
+#       define AFMT_60958_CS_SOURCE          (1 << 4)
+#       define AFMT_AUDIO_CHANNEL_ENABLE(x)  (((x) & 0xff) << 8)
+#       define AFMT_DP_AUDIO_STREAM_ID(x)    (((x) & 0xff) << 16)
+#define AFMT_AVI_INFO0                       0x7454
+#       define AFMT_AVI_INFO_CHECKSUM(x)     (((x) & 0xff) << 0)
+#       define AFMT_AVI_INFO_S(x)            (((x) & 3) << 8)
+#       define AFMT_AVI_INFO_B(x)            (((x) & 3) << 10)
+#       define AFMT_AVI_INFO_A(x)            (((x) & 1) << 12)
+#       define AFMT_AVI_INFO_Y(x)            (((x) & 3) << 13)
+#       define AFMT_AVI_INFO_Y_RGB           0
+#       define AFMT_AVI_INFO_Y_YCBCR422      1
+#       define AFMT_AVI_INFO_Y_YCBCR444      2
+#       define AFMT_AVI_INFO_Y_A_B_S(x)      (((x) & 0xff) << 8)
+#       define AFMT_AVI_INFO_R(x)            (((x) & 0xf) << 16)
+#       define AFMT_AVI_INFO_M(x)            (((x) & 0x3) << 20)
+#       define AFMT_AVI_INFO_C(x)            (((x) & 0x3) << 22)
+#       define AFMT_AVI_INFO_C_M_R(x)        (((x) & 0xff) << 16)
+#       define AFMT_AVI_INFO_SC(x)           (((x) & 0x3) << 24)
+#       define AFMT_AVI_INFO_Q(x)            (((x) & 0x3) << 26)
+#       define AFMT_AVI_INFO_EC(x)           (((x) & 0x3) << 28)
+#       define AFMT_AVI_INFO_ITC(x)          (((x) & 0x1) << 31)
+#       define AFMT_AVI_INFO_ITC_EC_Q_SC(x)  (((x) & 0xff) << 24)
+#define AFMT_AVI_INFO1                       0x7458
+#       define AFMT_AVI_INFO_VIC(x)          (((x) & 0x7f) << 0) /* don't use avi infoframe v1 */
+#       define AFMT_AVI_INFO_PR(x)           (((x) & 0xf) << 8) /* don't use avi infoframe v1 */
+#       define AFMT_AVI_INFO_TOP(x)          (((x) & 0xffff) << 16)
+#define AFMT_AVI_INFO2                       0x745c
+#       define AFMT_AVI_INFO_BOTTOM(x)       (((x) & 0xffff) << 0)
+#       define AFMT_AVI_INFO_LEFT(x)         (((x) & 0xffff) << 16)
+#define AFMT_AVI_INFO3                       0x7460
+#       define AFMT_AVI_INFO_RIGHT(x)        (((x) & 0xffff) << 0)
+#       define AFMT_AVI_INFO_VERSION(x)      (((x) & 3) << 24)
+#define AFMT_MPEG_INFO0                      0x7464
+#       define AFMT_MPEG_INFO_CHECKSUM(x)    (((x) & 0xff) << 0)
+#       define AFMT_MPEG_INFO_MB0(x)         (((x) & 0xff) << 8)
+#       define AFMT_MPEG_INFO_MB1(x)         (((x) & 0xff) << 16)
+#       define AFMT_MPEG_INFO_MB2(x)         (((x) & 0xff) << 24)
+#define AFMT_MPEG_INFO1                      0x7468
+#       define AFMT_MPEG_INFO_MB3(x)         (((x) & 0xff) << 0)
+#       define AFMT_MPEG_INFO_MF(x)          (((x) & 3) << 8)
+#       define AFMT_MPEG_INFO_FR(x)          (((x) & 1) << 12)
+#define AFMT_GENERIC0_HDR                    0x746c
+#define AFMT_GENERIC0_0                      0x7470
+#define AFMT_GENERIC0_1                      0x7474
+#define AFMT_GENERIC0_2                      0x7478
+#define AFMT_GENERIC0_3                      0x747c
+#define AFMT_GENERIC0_4                      0x7480
+#define AFMT_GENERIC0_5                      0x7484
+#define AFMT_GENERIC0_6                      0x7488
+#define AFMT_GENERIC1_HDR                    0x748c
+#define AFMT_GENERIC1_0                      0x7490
+#define AFMT_GENERIC1_1                      0x7494
+#define AFMT_GENERIC1_2                      0x7498
+#define AFMT_GENERIC1_3                      0x749c
+#define AFMT_GENERIC1_4                      0x74a0
+#define AFMT_GENERIC1_5                      0x74a4
+#define AFMT_GENERIC1_6                      0x74a8
+#define HDMI_ACR_32_0                        0x74ac
+#       define HDMI_ACR_CTS_32(x)            (((x) & 0xfffff) << 12)
+#define HDMI_ACR_32_1                        0x74b0
+#       define HDMI_ACR_N_32(x)              (((x) & 0xfffff) << 0)
+#define HDMI_ACR_44_0                        0x74b4
+#       define HDMI_ACR_CTS_44(x)            (((x) & 0xfffff) << 12)
+#define HDMI_ACR_44_1                        0x74b8
+#       define HDMI_ACR_N_44(x)              (((x) & 0xfffff) << 0)
+#define HDMI_ACR_48_0                        0x74bc
+#       define HDMI_ACR_CTS_48(x)            (((x) & 0xfffff) << 12)
+#define HDMI_ACR_48_1                        0x74c0
+#       define HDMI_ACR_N_48(x)              (((x) & 0xfffff) << 0)
+#define HDMI_ACR_STATUS_0                    0x74c4
+#define HDMI_ACR_STATUS_1                    0x74c8
+#define AFMT_AUDIO_INFO0                     0x74cc
+#       define AFMT_AUDIO_INFO_CHECKSUM(x)   (((x) & 0xff) << 0)
+#       define AFMT_AUDIO_INFO_CC(x)         (((x) & 7) << 8)
+#       define AFMT_AUDIO_INFO_CHECKSUM_OFFSET(x)   (((x) & 0xff) << 16)
+#define AFMT_AUDIO_INFO1                     0x74d0
+#       define AFMT_AUDIO_INFO_CA(x)         (((x) & 0xff) << 0)
+#       define AFMT_AUDIO_INFO_LSV(x)        (((x) & 0xf) << 11)
+#       define AFMT_AUDIO_INFO_DM_INH(x)     (((x) & 1) << 15)
+#       define AFMT_AUDIO_INFO_DM_INH_LSV(x) (((x) & 0xff) << 8)
+#define AFMT_60958_0                         0x74d4
+#       define AFMT_60958_CS_A(x)            (((x) & 1) << 0)
+#       define AFMT_60958_CS_B(x)            (((x) & 1) << 1)
+#       define AFMT_60958_CS_C(x)            (((x) & 1) << 2)
+#       define AFMT_60958_CS_D(x)            (((x) & 3) << 3)
+#       define AFMT_60958_CS_MODE(x)         (((x) & 3) << 6)
+#       define AFMT_60958_CS_CATEGORY_CODE(x)      (((x) & 0xff) << 8)
+#       define AFMT_60958_CS_SOURCE_NUMBER(x)      (((x) & 0xf) << 16)
+#       define AFMT_60958_CS_CHANNEL_NUMBER_L(x)   (((x) & 0xf) << 20)
+#       define AFMT_60958_CS_SAMPLING_FREQUENCY(x) (((x) & 0xf) << 24)
+#       define AFMT_60958_CS_CLOCK_ACCURACY(x)     (((x) & 3) << 28)
+#define AFMT_60958_1                         0x74d8
+#       define AFMT_60958_CS_WORD_LENGTH(x)  (((x) & 0xf) << 0)
+#       define AFMT_60958_CS_ORIGINAL_SAMPLING_FREQUENCY(x)   (((x) & 0xf) << 4)
+#       define AFMT_60958_CS_VALID_L(x)      (((x) & 1) << 16)
+#       define AFMT_60958_CS_VALID_R(x)      (((x) & 1) << 18)
+#       define AFMT_60958_CS_CHANNEL_NUMBER_R(x)   (((x) & 0xf) << 20)
+#define AFMT_AUDIO_CRC_CONTROL               0x74dc
+#       define AFMT_AUDIO_CRC_EN             (1 << 0)
+#define AFMT_RAMP_CONTROL0                   0x74e0
+#       define AFMT_RAMP_MAX_COUNT(x)        (((x) & 0xffffff) << 0)
+#       define AFMT_RAMP_DATA_SIGN           (1 << 31)
+#define AFMT_RAMP_CONTROL1                   0x74e4
+#       define AFMT_RAMP_MIN_COUNT(x)        (((x) & 0xffffff) << 0)
+#       define AFMT_AUDIO_TEST_CH_DISABLE(x) (((x) & 0xff) << 24)
+#define AFMT_RAMP_CONTROL2                   0x74e8
+#       define AFMT_RAMP_INC_COUNT(x)        (((x) & 0xffffff) << 0)
+#define AFMT_RAMP_CONTROL3                   0x74ec
+#       define AFMT_RAMP_DEC_COUNT(x)        (((x) & 0xffffff) << 0)
+#define AFMT_60958_2                         0x74f0
+#       define AFMT_60958_CS_CHANNEL_NUMBER_2(x)   (((x) & 0xf) << 0)
+#       define AFMT_60958_CS_CHANNEL_NUMBER_3(x)   (((x) & 0xf) << 4)
+#       define AFMT_60958_CS_CHANNEL_NUMBER_4(x)   (((x) & 0xf) << 8)
+#       define AFMT_60958_CS_CHANNEL_NUMBER_5(x)   (((x) & 0xf) << 12)
+#       define AFMT_60958_CS_CHANNEL_NUMBER_6(x)   (((x) & 0xf) << 16)
+#       define AFMT_60958_CS_CHANNEL_NUMBER_7(x)   (((x) & 0xf) << 20)
+#define AFMT_STATUS                          0x7600
+#       define AFMT_AUDIO_ENABLE             (1 << 4)
+#       define AFMT_AZ_FORMAT_WTRIG          (1 << 28)
+#       define AFMT_AZ_FORMAT_WTRIG_INT      (1 << 29)
+#       define AFMT_AZ_AUDIO_ENABLE_CHG      (1 << 30)
+#define AFMT_AUDIO_PACKET_CONTROL            0x7604
+#       define AFMT_AUDIO_SAMPLE_SEND        (1 << 0)
+#       define AFMT_AUDIO_TEST_EN            (1 << 12)
+#       define AFMT_AUDIO_CHANNEL_SWAP       (1 << 24)
+#       define AFMT_60958_CS_UPDATE          (1 << 26)
+#       define AFMT_AZ_AUDIO_ENABLE_CHG_MASK (1 << 27)
+#       define AFMT_AZ_FORMAT_WTRIG_MASK     (1 << 28)
+#       define AFMT_AZ_FORMAT_WTRIG_ACK      (1 << 29)
+#       define AFMT_AZ_AUDIO_ENABLE_CHG_ACK  (1 << 30)
+#define AFMT_VBI_PACKET_CONTROL              0x7608
+#       define AFMT_GENERIC0_UPDATE          (1 << 2)
+#define AFMT_INFOFRAME_CONTROL0              0x760c
+#       define AFMT_AUDIO_INFO_SOURCE        (1 << 6) /* 0 - sound block; 1 - hmdi regs */
+#       define AFMT_AUDIO_INFO_UPDATE        (1 << 7)
+#       define AFMT_MPEG_INFO_UPDATE         (1 << 10)
+#define AFMT_GENERIC0_7                      0x7610
+/* second instance starts at 0x7800 */
+#define HDMI_OFFSET0                      (0x7400 - 0x7400)
+#define HDMI_OFFSET1                      (0x7800 - 0x7400)
+
 #define D1GRPH_PRIMARY_SURFACE_ADDRESS                    0x6110
 #define D1GRPH_PRIMARY_SURFACE_ADDRESS_HIGH               0x6914
 #define D2GRPH_PRIMARY_SURFACE_ADDRESS_HIGH               0x6114
diff --git a/drivers/gpu/drm/radeon/si.c b/drivers/gpu/drm/radeon/si.c
index 27bda986fc2..549732e56ca 100644
--- a/drivers/gpu/drm/radeon/si.c
+++ b/drivers/gpu/drm/radeon/si.c
@@ -2217,8 +2217,6 @@ bool si_gpu_is_lockup(struct radeon_device *rdev, struct radeon_ring *ring)
 	u32 srbm_status;
 	u32 grbm_status, grbm_status2;
 	u32 grbm_status_se0, grbm_status_se1;
-	struct r100_gpu_lockup *lockup = &rdev->config.si.lockup;
-	int r;
 
 	srbm_status = RREG32(SRBM_STATUS);
 	grbm_status = RREG32(GRBM_STATUS);
@@ -2226,20 +2224,12 @@ bool si_gpu_is_lockup(struct radeon_device *rdev, struct radeon_ring *ring)
 	grbm_status_se0 = RREG32(GRBM_STATUS_SE0);
 	grbm_status_se1 = RREG32(GRBM_STATUS_SE1);
 	if (!(grbm_status & GUI_ACTIVE)) {
-		r100_gpu_lockup_update(lockup, ring);
+		radeon_ring_lockup_update(ring);
 		return false;
 	}
 	/* force CP activities */
-	r = radeon_ring_lock(rdev, ring, 2);
-	if (!r) {
-		/* PACKET2 NOP */
-		radeon_ring_write(ring, 0x80000000);
-		radeon_ring_write(ring, 0x80000000);
-		radeon_ring_unlock_commit(rdev, ring);
-	}
-	/* XXX deal with CP0,1,2 */
-	ring->rptr = RREG32(ring->rptr_reg);
-	return r100_gpu_cp_is_lockup(rdev, lockup, ring);
+	radeon_ring_force_activity(rdev, ring);
+	return radeon_ring_test_lockup(rdev, ring);
 }
 
 static int si_gpu_soft_reset(struct radeon_device *rdev)
@@ -2275,6 +2265,7 @@ static int si_gpu_soft_reset(struct radeon_device *rdev)
 		      SOFT_RESET_GDS |
 		      SOFT_RESET_PA |
 		      SOFT_RESET_SC |
+		      SOFT_RESET_BCI |
 		      SOFT_RESET_SPI |
 		      SOFT_RESET_SX |
 		      SOFT_RESET_TC |
@@ -2985,7 +2976,8 @@ int si_rlc_init(struct radeon_device *rdev)
 	/* save restore block */
 	if (rdev->rlc.save_restore_obj == NULL) {
 		r = radeon_bo_create(rdev, RADEON_GPU_PAGE_SIZE, PAGE_SIZE, true,
-				RADEON_GEM_DOMAIN_VRAM, &rdev->rlc.save_restore_obj);
+				     RADEON_GEM_DOMAIN_VRAM, NULL,
+				     &rdev->rlc.save_restore_obj);
 		if (r) {
 			dev_warn(rdev->dev, "(%d) create RLC sr bo failed\n", r);
 			return r;
@@ -3009,7 +3001,8 @@ int si_rlc_init(struct radeon_device *rdev)
 	/* clear state block */
 	if (rdev->rlc.clear_state_obj == NULL) {
 		r = radeon_bo_create(rdev, RADEON_GPU_PAGE_SIZE, PAGE_SIZE, true,
-				RADEON_GEM_DOMAIN_VRAM, &rdev->rlc.clear_state_obj);
+				     RADEON_GEM_DOMAIN_VRAM, NULL,
+				     &rdev->rlc.clear_state_obj);
 		if (r) {
 			dev_warn(rdev->dev, "(%d) create RLC c bo failed\n", r);
 			si_rlc_fini(rdev);
@@ -3216,6 +3209,8 @@ static int si_irq_init(struct radeon_device *rdev)
 	/* force the active interrupt state to all disabled */
 	si_disable_interrupt_state(rdev);
 
+	pci_set_master(rdev->pdev);
+
 	/* enable irqs */
 	si_enable_interrupts(rdev);
 
@@ -3994,10 +3989,6 @@ int si_init(struct radeon_device *rdev)
 	struct radeon_ring *ring = &rdev->ring[RADEON_RING_TYPE_GFX_INDEX];
 	int r;
 
-	/* This don't do much */
-	r = radeon_gem_init(rdev);
-	if (r)
-		return r;
 	/* Read BIOS */
 	if (!radeon_get_bios(rdev)) {
 		if (ASIC_IS_AVIVO(rdev))
@@ -4117,7 +4108,6 @@ void si_fini(struct radeon_device *rdev)
 	si_pcie_gart_fini(rdev);
 	r600_vram_scratch_fini(rdev);
 	radeon_gem_fini(rdev);
-	radeon_semaphore_driver_fini(rdev);
 	radeon_fence_driver_fini(rdev);
 	radeon_bo_fini(rdev);
 	radeon_atombios_fini(rdev);
diff --git a/drivers/gpu/drm/savage/savage_bci.c b/drivers/gpu/drm/savage/savage_bci.c
index cb1ee4e0050..6eb507a5d13 100644
--- a/drivers/gpu/drm/savage/savage_bci.c
+++ b/drivers/gpu/drm/savage/savage_bci.c
@@ -735,7 +735,7 @@ static int savage_do_init_bci(struct drm_device * dev, drm_savage_init_t * init)
 			return -EINVAL;
 		}
 		drm_core_ioremap(dev->agp_buffer_map, dev);
-		if (!dev->agp_buffer_map) {
+		if (!dev->agp_buffer_map->handle) {
 			DRM_ERROR("failed to ioremap DMA buffer region!\n");
 			savage_do_cleanup_bci(dev);
 			return -ENOMEM;
diff --git a/drivers/gpu/drm/ttm/ttm_bo.c b/drivers/gpu/drm/ttm/ttm_bo.c
index 1f5c67c579c..36792bd4da7 100644
--- a/drivers/gpu/drm/ttm/ttm_bo.c
+++ b/drivers/gpu/drm/ttm/ttm_bo.c
@@ -343,6 +343,16 @@ static int ttm_bo_add_ttm(struct ttm_buffer_object *bo, bool zero_alloc)
 		if (unlikely(bo->ttm == NULL))
 			ret = -ENOMEM;
 		break;
+	case ttm_bo_type_sg:
+		bo->ttm = bdev->driver->ttm_tt_create(bdev, bo->num_pages << PAGE_SHIFT,
+						      page_flags | TTM_PAGE_FLAG_SG,
+						      glob->dummy_read_page);
+		if (unlikely(bo->ttm == NULL)) {
+			ret = -ENOMEM;
+			break;
+		}
+		bo->ttm->sg = bo->sg;
+		break;
 	default:
 		pr_err("Illegal buffer object type\n");
 		ret = -EINVAL;
@@ -1169,6 +1179,7 @@ int ttm_bo_init(struct ttm_bo_device *bdev,
 		bool interruptible,
 		struct file *persistent_swap_storage,
 		size_t acc_size,
+		struct sg_table *sg,
 		void (*destroy) (struct ttm_buffer_object *))
 {
 	int ret = 0;
@@ -1223,6 +1234,7 @@ int ttm_bo_init(struct ttm_bo_device *bdev,
 	bo->seq_valid = false;
 	bo->persistent_swap_storage = persistent_swap_storage;
 	bo->acc_size = acc_size;
+	bo->sg = sg;
 	atomic_inc(&bo->glob->bo_count);
 
 	ret = ttm_bo_check_placement(bo, placement);
@@ -1233,7 +1245,8 @@ int ttm_bo_init(struct ttm_bo_device *bdev,
 	 * For ttm_bo_type_device buffers, allocate
 	 * address space from the device.
 	 */
-	if (bo->type == ttm_bo_type_device) {
+	if (bo->type == ttm_bo_type_device ||
+	    bo->type == ttm_bo_type_sg) {
 		ret = ttm_bo_setup_vm(bo);
 		if (ret)
 			goto out_err;
@@ -1312,7 +1325,7 @@ int ttm_bo_create(struct ttm_bo_device *bdev,
 
 	ret = ttm_bo_init(bdev, bo, size, type, placement, page_alignment,
 				buffer_start, interruptible,
-				persistent_swap_storage, acc_size, NULL);
+			  persistent_swap_storage, acc_size, NULL, NULL);
 	if (likely(ret == 0))
 		*p_bo = bo;
 
diff --git a/drivers/gpu/drm/udl/udl_drv.c b/drivers/gpu/drm/udl/udl_drv.c
index 53673907a6a..4d02c46a942 100644
--- a/drivers/gpu/drm/udl/udl_drv.c
+++ b/drivers/gpu/drm/udl/udl_drv.c
@@ -38,7 +38,7 @@ static void udl_usb_disconnect(struct usb_interface *interface)
 	drm_unplug_dev(dev);
 }
 
-static struct vm_operations_struct udl_gem_vm_ops = {
+static const struct vm_operations_struct udl_gem_vm_ops = {
 	.fault = udl_gem_fault,
 	.open = drm_gem_vm_open,
 	.close = drm_gem_vm_close,
@@ -57,7 +57,7 @@ static const struct file_operations udl_driver_fops = {
 };
 
 static struct drm_driver driver = {
-	.driver_features = DRIVER_MODESET | DRIVER_GEM,
+	.driver_features = DRIVER_MODESET | DRIVER_GEM | DRIVER_PRIME,
 	.load = udl_driver_load,
 	.unload = udl_driver_unload,
 
@@ -70,6 +70,10 @@ static struct drm_driver driver = {
 	.dumb_map_offset = udl_gem_mmap,
 	.dumb_destroy = udl_dumb_destroy,
 	.fops = &udl_driver_fops,
+
+	.prime_fd_to_handle = drm_gem_prime_fd_to_handle,
+	.gem_prime_import = udl_gem_prime_import,
+
 	.name = DRIVER_NAME,
 	.desc = DRIVER_DESC,
 	.date = DRIVER_DATE,
diff --git a/drivers/gpu/drm/udl/udl_drv.h b/drivers/gpu/drm/udl/udl_drv.h
index 96820d03a30..fccd361f7b5 100644
--- a/drivers/gpu/drm/udl/udl_drv.h
+++ b/drivers/gpu/drm/udl/udl_drv.h
@@ -66,6 +66,7 @@ struct udl_gem_object {
 	struct drm_gem_object base;
 	struct page **pages;
 	void *vmapping;
+	struct sg_table *sg;
 };
 
 #define to_udl_bo(x) container_of(x, struct udl_gem_object, base)
@@ -118,6 +119,8 @@ int udl_gem_init_object(struct drm_gem_object *obj);
 void udl_gem_free_object(struct drm_gem_object *gem_obj);
 struct udl_gem_object *udl_gem_alloc_object(struct drm_device *dev,
 					    size_t size);
+struct drm_gem_object *udl_gem_prime_import(struct drm_device *dev,
+				struct dma_buf *dma_buf);
 
 int udl_gem_vmap(struct udl_gem_object *obj);
 void udl_gem_vunmap(struct udl_gem_object *obj);
diff --git a/drivers/gpu/drm/udl/udl_fb.c b/drivers/gpu/drm/udl/udl_fb.c
index 4d9c3a5d8a4..a029ee39b0c 100644
--- a/drivers/gpu/drm/udl/udl_fb.c
+++ b/drivers/gpu/drm/udl/udl_fb.c
@@ -593,11 +593,20 @@ udl_fb_user_fb_create(struct drm_device *dev,
 	struct drm_gem_object *obj;
 	struct udl_framebuffer *ufb;
 	int ret;
+	uint32_t size;
 
 	obj = drm_gem_object_lookup(dev, file, mode_cmd->handles[0]);
 	if (obj == NULL)
 		return ERR_PTR(-ENOENT);
 
+	size = mode_cmd->pitches[0] * mode_cmd->height;
+	size = ALIGN(size, PAGE_SIZE);
+
+	if (size > obj->size) {
+		DRM_ERROR("object size not sufficient for fb %d %zu %d %d\n", size, obj->size, mode_cmd->pitches[0], mode_cmd->height);
+		return ERR_PTR(-ENOMEM);
+	}
+
 	ufb = kzalloc(sizeof(*ufb), GFP_KERNEL);
 	if (ufb == NULL)
 		return ERR_PTR(-ENOMEM);
diff --git a/drivers/gpu/drm/udl/udl_gem.c b/drivers/gpu/drm/udl/udl_gem.c
index 92f19ef329b..40efd32f7dc 100644
--- a/drivers/gpu/drm/udl/udl_gem.c
+++ b/drivers/gpu/drm/udl/udl_gem.c
@@ -9,6 +9,7 @@
 #include "drmP.h"
 #include "udl_drv.h"
 #include <linux/shmem_fs.h>
+#include <linux/dma-buf.h>
 
 struct udl_gem_object *udl_gem_alloc_object(struct drm_device *dev,
 					    size_t size)
@@ -161,6 +162,12 @@ static void udl_gem_put_pages(struct udl_gem_object *obj)
 	int page_count = obj->base.size / PAGE_SIZE;
 	int i;
 
+	if (obj->base.import_attach) {
+		drm_free_large(obj->pages);
+		obj->pages = NULL;
+		return;
+	}
+
 	for (i = 0; i < page_count; i++)
 		page_cache_release(obj->pages[i]);
 
@@ -195,6 +202,9 @@ void udl_gem_free_object(struct drm_gem_object *gem_obj)
 {
 	struct udl_gem_object *obj = to_udl_bo(gem_obj);
 
+	if (gem_obj->import_attach)
+		drm_prime_gem_destroy(gem_obj, obj->sg);
+
 	if (obj->vmapping)
 		udl_gem_vunmap(obj);
 
@@ -239,3 +249,68 @@ unlock:
 	mutex_unlock(&dev->struct_mutex);
 	return ret;
 }
+
+static int udl_prime_create(struct drm_device *dev,
+			    size_t size,
+			    struct sg_table *sg,
+			    struct udl_gem_object **obj_p)
+{
+	struct udl_gem_object *obj;
+	int npages;
+	int i;
+	struct scatterlist *iter;
+
+	npages = size / PAGE_SIZE;
+
+	*obj_p = NULL;
+	obj = udl_gem_alloc_object(dev, npages * PAGE_SIZE);
+	if (!obj)
+		return -ENOMEM;
+
+	obj->sg = sg;
+	obj->pages = drm_malloc_ab(npages, sizeof(struct page *));
+	if (obj->pages == NULL) {
+		DRM_ERROR("obj pages is NULL %d\n", npages);
+		return -ENOMEM;
+	}
+
+	drm_prime_sg_to_page_addr_arrays(sg, obj->pages, NULL, npages);
+
+	*obj_p = obj;
+	return 0;
+}
+
+struct drm_gem_object *udl_gem_prime_import(struct drm_device *dev,
+				struct dma_buf *dma_buf)
+{
+	struct dma_buf_attachment *attach;
+	struct sg_table *sg;
+	struct udl_gem_object *uobj;
+	int ret;
+
+	/* need to attach */
+	attach = dma_buf_attach(dma_buf, dev->dev);
+	if (IS_ERR(attach))
+		return ERR_PTR(PTR_ERR(attach));
+
+	sg = dma_buf_map_attachment(attach, DMA_BIDIRECTIONAL);
+	if (IS_ERR(sg)) {
+		ret = PTR_ERR(sg);
+		goto fail_detach;
+	}
+
+	ret = udl_prime_create(dev, dma_buf->size, sg, &uobj);
+	if (ret) {
+		goto fail_unmap;
+	}
+
+	uobj->base.import_attach = attach;
+
+	return &uobj->base;
+
+fail_unmap:
+	dma_buf_unmap_attachment(attach, sg, DMA_BIDIRECTIONAL);
+fail_detach:
+	dma_buf_detach(dma_buf, attach);
+	return ERR_PTR(ret);
+}
diff --git a/drivers/gpu/drm/udl/udl_modeset.c b/drivers/gpu/drm/udl/udl_modeset.c
index b3ecb3d12a1..0d7816789da 100644
--- a/drivers/gpu/drm/udl/udl_modeset.c
+++ b/drivers/gpu/drm/udl/udl_modeset.c
@@ -395,7 +395,7 @@ int udl_modeset_init(struct drm_device *dev)
 	dev->mode_config.prefer_shadow = 0;
 	dev->mode_config.preferred_depth = 24;
 
-	dev->mode_config.funcs = (void *)&udl_mode_funcs;
+	dev->mode_config.funcs = &udl_mode_funcs;
 
 	drm_mode_create_dirty_info_property(dev);
 
diff --git a/drivers/gpu/drm/vmwgfx/vmwgfx_kms.c b/drivers/gpu/drm/vmwgfx/vmwgfx_kms.c
index 2286d47e502..6b0078ffa76 100644
--- a/drivers/gpu/drm/vmwgfx/vmwgfx_kms.c
+++ b/drivers/gpu/drm/vmwgfx/vmwgfx_kms.c
@@ -1178,7 +1178,7 @@ err_out:
 	return &vfb->base;
 }
 
-static struct drm_mode_config_funcs vmw_kms_funcs = {
+static const struct drm_mode_config_funcs vmw_kms_funcs = {
 	.fb_create = vmw_kms_fb_create,
 };
 
diff --git a/drivers/gpu/drm/vmwgfx/vmwgfx_resource.c b/drivers/gpu/drm/vmwgfx/vmwgfx_resource.c
index a37abb581cb..22bf9a21ec7 100644
--- a/drivers/gpu/drm/vmwgfx/vmwgfx_resource.c
+++ b/drivers/gpu/drm/vmwgfx/vmwgfx_resource.c
@@ -1567,7 +1567,7 @@ int vmw_dmabuf_init(struct vmw_private *dev_priv,
 	ret = ttm_bo_init(bdev, &vmw_bo->base, size,
 			  ttm_bo_type_device, placement,
 			  0, 0, interruptible,
-			  NULL, acc_size, bo_free);
+			  NULL, acc_size, NULL, bo_free);
 	return ret;
 }
 
diff --git a/drivers/gpu/vga/Kconfig b/drivers/gpu/vga/Kconfig
index 96c83a9a76b..f34838839b0 100644
--- a/drivers/gpu/vga/Kconfig
+++ b/drivers/gpu/vga/Kconfig
@@ -21,6 +21,7 @@ config VGA_SWITCHEROO
 	bool "Laptop Hybrid Graphics - GPU switching support"
 	depends on X86
 	depends on ACPI
+	select VGA_ARB
 	help
 	  Many laptops released in 2008/9/10 have two GPUs with a multiplexer
 	  to switch between them. This adds support for dynamic switching when
diff --git a/drivers/gpu/vga/vga_switcheroo.c b/drivers/gpu/vga/vga_switcheroo.c
index 58434e804d9..38f9534ac51 100644
--- a/drivers/gpu/vga/vga_switcheroo.c
+++ b/drivers/gpu/vga/vga_switcheroo.c
@@ -28,15 +28,16 @@
 #include <linux/pci.h>
 #include <linux/vga_switcheroo.h>
 
+#include <linux/vgaarb.h>
+
 struct vga_switcheroo_client {
 	struct pci_dev *pdev;
 	struct fb_info *fb_info;
 	int pwr_state;
-	void (*set_gpu_state)(struct pci_dev *pdev, enum vga_switcheroo_state);
-	void (*reprobe)(struct pci_dev *pdev);
-	bool (*can_switch)(struct pci_dev *pdev);
+	const struct vga_switcheroo_client_ops *ops;
 	int id;
 	bool active;
+	struct list_head list;
 };
 
 static DEFINE_MUTEX(vgasr_mutex);
@@ -51,16 +52,23 @@ struct vgasr_priv {
 	struct dentry *switch_file;
 
 	int registered_clients;
-	struct vga_switcheroo_client clients[VGA_SWITCHEROO_MAX_CLIENTS];
+	struct list_head clients;
 
 	struct vga_switcheroo_handler *handler;
 };
 
+#define ID_BIT_AUDIO		0x100
+#define client_is_audio(c)	((c)->id & ID_BIT_AUDIO)
+#define client_is_vga(c)	((c)->id == -1 || !client_is_audio(c))
+#define client_id(c)		((c)->id & ~ID_BIT_AUDIO)
+
 static int vga_switcheroo_debugfs_init(struct vgasr_priv *priv);
 static void vga_switcheroo_debugfs_fini(struct vgasr_priv *priv);
 
 /* only one switcheroo per system */
-static struct vgasr_priv vgasr_priv;
+static struct vgasr_priv vgasr_priv = {
+	.clients = LIST_HEAD_INIT(vgasr_priv.clients),
+};
 
 int vga_switcheroo_register_handler(struct vga_switcheroo_handler *handler)
 {
@@ -86,72 +94,119 @@ EXPORT_SYMBOL(vga_switcheroo_unregister_handler);
 
 static void vga_switcheroo_enable(void)
 {
-	int i;
 	int ret;
+	struct vga_switcheroo_client *client;
+
 	/* call the handler to init */
 	vgasr_priv.handler->init();
 
-	for (i = 0; i < VGA_SWITCHEROO_MAX_CLIENTS; i++) {
-		ret = vgasr_priv.handler->get_client_id(vgasr_priv.clients[i].pdev);
+	list_for_each_entry(client, &vgasr_priv.clients, list) {
+		if (client->id != -1)
+			continue;
+		ret = vgasr_priv.handler->get_client_id(client->pdev);
 		if (ret < 0)
 			return;
 
-		vgasr_priv.clients[i].id = ret;
+		client->id = ret;
 	}
 	vga_switcheroo_debugfs_init(&vgasr_priv);
 	vgasr_priv.active = true;
 }
 
-int vga_switcheroo_register_client(struct pci_dev *pdev,
-				   void (*set_gpu_state)(struct pci_dev *pdev, enum vga_switcheroo_state),
-				   void (*reprobe)(struct pci_dev *pdev),
-				   bool (*can_switch)(struct pci_dev *pdev))
+static int register_client(struct pci_dev *pdev,
+			   const struct vga_switcheroo_client_ops *ops,
+			   int id, bool active)
 {
-	int index;
+	struct vga_switcheroo_client *client;
+
+	client = kzalloc(sizeof(*client), GFP_KERNEL);
+	if (!client)
+		return -ENOMEM;
+
+	client->pwr_state = VGA_SWITCHEROO_ON;
+	client->pdev = pdev;
+	client->ops = ops;
+	client->id = id;
+	client->active = active;
 
 	mutex_lock(&vgasr_mutex);
-	/* don't do IGD vs DIS here */
-	if (vgasr_priv.registered_clients & 1)
-		index = 1;
-	else
-		index = 0;
-
-	vgasr_priv.clients[index].pwr_state = VGA_SWITCHEROO_ON;
-	vgasr_priv.clients[index].pdev = pdev;
-	vgasr_priv.clients[index].set_gpu_state = set_gpu_state;
-	vgasr_priv.clients[index].reprobe = reprobe;
-	vgasr_priv.clients[index].can_switch = can_switch;
-	vgasr_priv.clients[index].id = -1;
-	if (pdev->resource[PCI_ROM_RESOURCE].flags & IORESOURCE_ROM_SHADOW)
-		vgasr_priv.clients[index].active = true;
-
-	vgasr_priv.registered_clients |= (1 << index);
+	list_add_tail(&client->list, &vgasr_priv.clients);
+	if (client_is_vga(client))
+		vgasr_priv.registered_clients++;
 
 	/* if we get two clients + handler */
-	if (vgasr_priv.registered_clients == 0x3 && vgasr_priv.handler) {
+	if (!vgasr_priv.active &&
+	    vgasr_priv.registered_clients == 2 && vgasr_priv.handler) {
 		printk(KERN_INFO "vga_switcheroo: enabled\n");
 		vga_switcheroo_enable();
 	}
 	mutex_unlock(&vgasr_mutex);
 	return 0;
 }
+
+int vga_switcheroo_register_client(struct pci_dev *pdev,
+				   const struct vga_switcheroo_client_ops *ops)
+{
+	return register_client(pdev, ops, -1,
+			       pdev == vga_default_device());
+}
 EXPORT_SYMBOL(vga_switcheroo_register_client);
 
+int vga_switcheroo_register_audio_client(struct pci_dev *pdev,
+					 const struct vga_switcheroo_client_ops *ops,
+					 int id, bool active)
+{
+	return register_client(pdev, ops, id | ID_BIT_AUDIO, active);
+}
+EXPORT_SYMBOL(vga_switcheroo_register_audio_client);
+
+static struct vga_switcheroo_client *
+find_client_from_pci(struct list_head *head, struct pci_dev *pdev)
+{
+	struct vga_switcheroo_client *client;
+	list_for_each_entry(client, head, list)
+		if (client->pdev == pdev)
+			return client;
+	return NULL;
+}
+
+static struct vga_switcheroo_client *
+find_client_from_id(struct list_head *head, int client_id)
+{
+	struct vga_switcheroo_client *client;
+	list_for_each_entry(client, head, list)
+		if (client->id == client_id)
+			return client;
+	return NULL;
+}
+
+static struct vga_switcheroo_client *
+find_active_client(struct list_head *head)
+{
+	struct vga_switcheroo_client *client;
+	list_for_each_entry(client, head, list)
+		if (client->active && client_is_vga(client))
+			return client;
+	return NULL;
+}
+
 void vga_switcheroo_unregister_client(struct pci_dev *pdev)
 {
-	int i;
+	struct vga_switcheroo_client *client;
 
 	mutex_lock(&vgasr_mutex);
-	for (i = 0; i < VGA_SWITCHEROO_MAX_CLIENTS; i++) {
-		if (vgasr_priv.clients[i].pdev == pdev) {
-			vgasr_priv.registered_clients &= ~(1 << i);
-			break;
-		}
+	client = find_client_from_pci(&vgasr_priv.clients, pdev);
+	if (client) {
+		if (client_is_vga(client))
+			vgasr_priv.registered_clients--;
+		list_del(&client->list);
+		kfree(client);
+	}
+	if (vgasr_priv.active && vgasr_priv.registered_clients < 2) {
+		printk(KERN_INFO "vga_switcheroo: disabled\n");
+		vga_switcheroo_debugfs_fini(&vgasr_priv);
+		vgasr_priv.active = false;
 	}
-
-	printk(KERN_INFO "vga_switcheroo: disabled\n");
-	vga_switcheroo_debugfs_fini(&vgasr_priv);
-	vgasr_priv.active = false;
 	mutex_unlock(&vgasr_mutex);
 }
 EXPORT_SYMBOL(vga_switcheroo_unregister_client);
@@ -159,29 +214,29 @@ EXPORT_SYMBOL(vga_switcheroo_unregister_client);
 void vga_switcheroo_client_fb_set(struct pci_dev *pdev,
 				 struct fb_info *info)
 {
-	int i;
+	struct vga_switcheroo_client *client;
 
 	mutex_lock(&vgasr_mutex);
-	for (i = 0; i < VGA_SWITCHEROO_MAX_CLIENTS; i++) {
-		if (vgasr_priv.clients[i].pdev == pdev) {
-			vgasr_priv.clients[i].fb_info = info;
-			break;
-		}
-	}
+	client = find_client_from_pci(&vgasr_priv.clients, pdev);
+	if (client)
+		client->fb_info = info;
 	mutex_unlock(&vgasr_mutex);
 }
 EXPORT_SYMBOL(vga_switcheroo_client_fb_set);
 
 static int vga_switcheroo_show(struct seq_file *m, void *v)
 {
-	int i;
+	struct vga_switcheroo_client *client;
+	int i = 0;
 	mutex_lock(&vgasr_mutex);
-	for (i = 0; i < VGA_SWITCHEROO_MAX_CLIENTS; i++) {
-		seq_printf(m, "%d:%s:%c:%s:%s\n", i,
-			   vgasr_priv.clients[i].id == VGA_SWITCHEROO_DIS ? "DIS" : "IGD",
-			   vgasr_priv.clients[i].active ? '+' : ' ',
-			   vgasr_priv.clients[i].pwr_state ? "Pwr" : "Off",
-			   pci_name(vgasr_priv.clients[i].pdev));
+	list_for_each_entry(client, &vgasr_priv.clients, list) {
+		seq_printf(m, "%d:%s%s:%c:%s:%s\n", i,
+			   client_id(client) == VGA_SWITCHEROO_DIS ? "DIS" : "IGD",
+			   client_is_vga(client) ? "" : "-Audio",
+			   client->active ? '+' : ' ',
+			   client->pwr_state ? "Pwr" : "Off",
+			   pci_name(client->pdev));
+		i++;
 	}
 	mutex_unlock(&vgasr_mutex);
 	return 0;
@@ -197,7 +252,7 @@ static int vga_switchon(struct vga_switcheroo_client *client)
 	if (vgasr_priv.handler->power_state)
 		vgasr_priv.handler->power_state(client->id, VGA_SWITCHEROO_ON);
 	/* call the driver callback to turn on device */
-	client->set_gpu_state(client->pdev, VGA_SWITCHEROO_ON);
+	client->ops->set_gpu_state(client->pdev, VGA_SWITCHEROO_ON);
 	client->pwr_state = VGA_SWITCHEROO_ON;
 	return 0;
 }
@@ -205,34 +260,39 @@ static int vga_switchon(struct vga_switcheroo_client *client)
 static int vga_switchoff(struct vga_switcheroo_client *client)
 {
 	/* call the driver callback to turn off device */
-	client->set_gpu_state(client->pdev, VGA_SWITCHEROO_OFF);
+	client->ops->set_gpu_state(client->pdev, VGA_SWITCHEROO_OFF);
 	if (vgasr_priv.handler->power_state)
 		vgasr_priv.handler->power_state(client->id, VGA_SWITCHEROO_OFF);
 	client->pwr_state = VGA_SWITCHEROO_OFF;
 	return 0;
 }
 
+static void set_audio_state(int id, int state)
+{
+	struct vga_switcheroo_client *client;
+
+	client = find_client_from_id(&vgasr_priv.clients, id | ID_BIT_AUDIO);
+	if (client && client->pwr_state != state) {
+		client->ops->set_gpu_state(client->pdev, state);
+		client->pwr_state = state;
+	}
+}
+
 /* stage one happens before delay */
 static int vga_switchto_stage1(struct vga_switcheroo_client *new_client)
 {
-	int i;
-	struct vga_switcheroo_client *active = NULL;
+	struct vga_switcheroo_client *active;
 
-	for (i = 0; i < VGA_SWITCHEROO_MAX_CLIENTS; i++) {
-		if (vgasr_priv.clients[i].active == true) {
-			active = &vgasr_priv.clients[i];
-			break;
-		}
-	}
+	active = find_active_client(&vgasr_priv.clients);
 	if (!active)
 		return 0;
 
 	if (new_client->pwr_state == VGA_SWITCHEROO_OFF)
 		vga_switchon(new_client);
 
-	/* swap shadow resource to denote boot VGA device has changed so X starts on new device */
-	active->pdev->resource[PCI_ROM_RESOURCE].flags &= ~IORESOURCE_ROM_SHADOW;
-	new_client->pdev->resource[PCI_ROM_RESOURCE].flags |= IORESOURCE_ROM_SHADOW;
+	vga_set_default_device(new_client->pdev);
+	set_audio_state(new_client->id, VGA_SWITCHEROO_ON);
+
 	return 0;
 }
 
@@ -240,15 +300,9 @@ static int vga_switchto_stage1(struct vga_switcheroo_client *new_client)
 static int vga_switchto_stage2(struct vga_switcheroo_client *new_client)
 {
 	int ret;
-	int i;
-	struct vga_switcheroo_client *active = NULL;
+	struct vga_switcheroo_client *active;
 
-	for (i = 0; i < VGA_SWITCHEROO_MAX_CLIENTS; i++) {
-		if (vgasr_priv.clients[i].active == true) {
-			active = &vgasr_priv.clients[i];
-			break;
-		}
-	}
+	active = find_active_client(&vgasr_priv.clients);
 	if (!active)
 		return 0;
 
@@ -264,8 +318,10 @@ static int vga_switchto_stage2(struct vga_switcheroo_client *new_client)
 	if (ret)
 		return ret;
 
-	if (new_client->reprobe)
-		new_client->reprobe(new_client->pdev);
+	if (new_client->ops->reprobe)
+		new_client->ops->reprobe(new_client->pdev);
+
+	set_audio_state(active->id, VGA_SWITCHEROO_OFF);
 
 	if (active->pwr_state == VGA_SWITCHEROO_ON)
 		vga_switchoff(active);
@@ -274,13 +330,26 @@ static int vga_switchto_stage2(struct vga_switcheroo_client *new_client)
 	return 0;
 }
 
+static bool check_can_switch(void)
+{
+	struct vga_switcheroo_client *client;
+
+	list_for_each_entry(client, &vgasr_priv.clients, list) {
+		if (!client->ops->can_switch(client->pdev)) {
+			printk(KERN_ERR "vga_switcheroo: client %x refused switch\n", client->id);
+			return false;
+		}
+	}
+	return true;
+}
+
 static ssize_t
 vga_switcheroo_debugfs_write(struct file *filp, const char __user *ubuf,
 			     size_t cnt, loff_t *ppos)
 {
 	char usercmd[64];
 	const char *pdev_name;
-	int i, ret;
+	int ret;
 	bool delay = false, can_switch;
 	bool just_mux = false;
 	int client_id = -1;
@@ -301,21 +370,21 @@ vga_switcheroo_debugfs_write(struct file *filp, const char __user *ubuf,
 
 	/* pwr off the device not in use */
 	if (strncmp(usercmd, "OFF", 3) == 0) {
-		for (i = 0; i < VGA_SWITCHEROO_MAX_CLIENTS; i++) {
-			if (vgasr_priv.clients[i].active)
+		list_for_each_entry(client, &vgasr_priv.clients, list) {
+			if (client->active)
 				continue;
-			if (vgasr_priv.clients[i].pwr_state == VGA_SWITCHEROO_ON)
-				vga_switchoff(&vgasr_priv.clients[i]);
+			if (client->pwr_state == VGA_SWITCHEROO_ON)
+				vga_switchoff(client);
 		}
 		goto out;
 	}
 	/* pwr on the device not in use */
 	if (strncmp(usercmd, "ON", 2) == 0) {
-		for (i = 0; i < VGA_SWITCHEROO_MAX_CLIENTS; i++) {
-			if (vgasr_priv.clients[i].active)
+		list_for_each_entry(client, &vgasr_priv.clients, list) {
+			if (client->active)
 				continue;
-			if (vgasr_priv.clients[i].pwr_state == VGA_SWITCHEROO_OFF)
-				vga_switchon(&vgasr_priv.clients[i]);
+			if (client->pwr_state == VGA_SWITCHEROO_OFF)
+				vga_switchon(client);
 		}
 		goto out;
 	}
@@ -348,13 +417,9 @@ vga_switcheroo_debugfs_write(struct file *filp, const char __user *ubuf,
 
 	if (client_id == -1)
 		goto out;
-
-	for (i = 0; i < VGA_SWITCHEROO_MAX_CLIENTS; i++) {
-		if (vgasr_priv.clients[i].id == client_id) {
-			client = &vgasr_priv.clients[i];
-			break;
-		}
-	}
+	client = find_client_from_id(&vgasr_priv.clients, client_id);
+	if (!client)
+		goto out;
 
 	vgasr_priv.delayed_switch_active = false;
 
@@ -363,23 +428,16 @@ vga_switcheroo_debugfs_write(struct file *filp, const char __user *ubuf,
 		goto out;
 	}
 
-	if (client->active == true)
+	if (client->active)
 		goto out;
 
 	/* okay we want a switch - test if devices are willing to switch */
-	can_switch = true;
-	for (i = 0; i < VGA_SWITCHEROO_MAX_CLIENTS; i++) {
-		can_switch = vgasr_priv.clients[i].can_switch(vgasr_priv.clients[i].pdev);
-		if (can_switch == false) {
-			printk(KERN_ERR "vga_switcheroo: client %d refused switch\n", i);
-			break;
-		}
-	}
+	can_switch = check_can_switch();
 
 	if (can_switch == false && delay == false)
 		goto out;
 
-	if (can_switch == true) {
+	if (can_switch) {
 		pdev_name = pci_name(client->pdev);
 		ret = vga_switchto_stage1(client);
 		if (ret)
@@ -451,10 +509,8 @@ fail:
 
 int vga_switcheroo_process_delayed_switch(void)
 {
-	struct vga_switcheroo_client *client = NULL;
+	struct vga_switcheroo_client *client;
 	const char *pdev_name;
-	bool can_switch = true;
-	int i;
 	int ret;
 	int err = -EINVAL;
 
@@ -464,17 +520,9 @@ int vga_switcheroo_process_delayed_switch(void)
 
 	printk(KERN_INFO "vga_switcheroo: processing delayed switch to %d\n", vgasr_priv.delayed_client_id);
 
-	for (i = 0; i < VGA_SWITCHEROO_MAX_CLIENTS; i++) {
-		if (vgasr_priv.clients[i].id == vgasr_priv.delayed_client_id)
-			client = &vgasr_priv.clients[i];
-		can_switch = vgasr_priv.clients[i].can_switch(vgasr_priv.clients[i].pdev);
-		if (can_switch == false) {
-			printk(KERN_ERR "vga_switcheroo: client %d refused switch\n", i);
-			break;
-		}
-	}
-
-	if (can_switch == false || client == NULL)
+	client = find_client_from_id(&vgasr_priv.clients,
+				     vgasr_priv.delayed_client_id);
+	if (!client || !check_can_switch())
 		goto err;
 
 	pdev_name = pci_name(client->pdev);
diff --git a/drivers/gpu/vga/vgaarb.c b/drivers/gpu/vga/vgaarb.c
index 111d956d8e7..3df8fc0ec01 100644
--- a/drivers/gpu/vga/vgaarb.c
+++ b/drivers/gpu/vga/vgaarb.c
@@ -136,6 +136,13 @@ struct pci_dev *vga_default_device(void)
 {
 	return vga_default;
 }
+
+EXPORT_SYMBOL_GPL(vga_default_device);
+
+void vga_set_default_device(struct pci_dev *pdev)
+{
+	vga_default = pdev;
+}
 #endif
 
 static inline void vga_irq_set_state(struct vga_device *vgadev, bool state)
@@ -605,10 +612,12 @@ static bool vga_arbiter_del_pci_device(struct pci_dev *pdev)
 		goto bail;
 	}
 
+#ifndef __ARCH_HAS_VGA_DEFAULT_DEVICE
 	if (vga_default == pdev) {
 		pci_dev_put(vga_default);
 		vga_default = NULL;
 	}
+#endif
 
 	if (vgadev->decodes & (VGA_RSRC_LEGACY_IO | VGA_RSRC_LEGACY_MEM))
 		vga_decode_count--;
diff --git a/drivers/hid/Kconfig b/drivers/hid/Kconfig
index ffddcba32af..034c80a10f1 100644
--- a/drivers/hid/Kconfig
+++ b/drivers/hid/Kconfig
@@ -32,9 +32,13 @@ config HID
 	  If unsure, say Y.
 
 config HID_BATTERY_STRENGTH
-	bool
+	bool "Battery level reporting for HID devices"
 	depends on HID && POWER_SUPPLY && HID = POWER_SUPPLY
 	default n
+	---help---
+	This option adds support of reporting battery strength (for HID devices
+	that support this feature) through power_supply class so that userspace
+	tools, such as upower, can display it.
 
 config HIDRAW
 	bool "/dev/hidraw raw HID device support"
@@ -60,6 +64,18 @@ source "drivers/hid/usbhid/Kconfig"
 menu "Special HID drivers"
 	depends on HID
 
+config HID_GENERIC
+	tristate "Generic HID driver"
+	depends on HID
+	default y
+	---help---
+	Support for generic HID devices.
+
+	To compile this driver as a module, choose M here: the module
+	will be called hid-generic.
+
+	If unsure, say Y.
+
 config HID_A4TECH
 	tristate "A4 tech mice" if EXPERT
 	depends on USB_HID
@@ -92,6 +108,12 @@ config HID_APPLE
 	Say Y here if you want support for keyboards of	Apple iBooks, PowerBooks,
 	MacBooks, MacBook Pros and Apple Aluminum.
 
+config HID_AUREAL
+	tristate "Aureal"
+	depends on USB_HID
+	---help---
+	Support for Aureal Cy se W-01RN Remote Controller and other Aureal derived remotes.
+
 config HID_BELKIN
 	tristate "Belkin Flip KVM and Wireless keyboard" if EXPERT
 	depends on USB_HID
@@ -448,7 +470,7 @@ config HID_PICOLCD_FB
 	select FB_SYS_FOPS
 	---help---
 	  Provide access to PicoLCD's 256x64 monochrome display via a
-	  frambuffer device.
+	  framebuffer device.
 
 config HID_PICOLCD_BACKLIGHT
 	bool "Backlight control" if EXPERT
@@ -595,16 +617,10 @@ config THRUSTMASTER_FF
 config HID_WACOM
 	tristate "Wacom Bluetooth devices support"
 	depends on BT_HIDP
-	---help---
-	Support for Wacom Graphire Bluetooth tablet.
-
-config HID_WACOM_POWER_SUPPLY
-	bool "Wacom Bluetooth devices power supply status support"
-	depends on HID_WACOM
+	depends on LEDS_CLASS
 	select POWER_SUPPLY
 	---help---
-	  Say Y here if you want to enable power supply status monitoring for
-	  Wacom Bluetooth devices.
+	Support for Wacom Graphire Bluetooth and Intuos4 WL tablets.
 
 config HID_WIIMOTE
 	tristate "Nintendo Wii Remote support"
diff --git a/drivers/hid/Makefile b/drivers/hid/Makefile
index 22f1d16cd79..ca6cc9f0485 100644
--- a/drivers/hid/Makefile
+++ b/drivers/hid/Makefile
@@ -9,6 +9,8 @@ endif
 
 obj-$(CONFIG_HID)		+= hid.o
 
+obj-$(CONFIG_HID_GENERIC)	+= hid-generic.o
+
 hid-$(CONFIG_HIDRAW)		+= hidraw.o
 
 hid-logitech-y		:= hid-lg.o
@@ -36,6 +38,7 @@ endif
 obj-$(CONFIG_HID_A4TECH)	+= hid-a4tech.o
 obj-$(CONFIG_HID_ACRUX)		+= hid-axff.o
 obj-$(CONFIG_HID_APPLE)		+= hid-apple.o
+obj-$(CONFIG_HID_AUREAL)        += hid-aureal.o
 obj-$(CONFIG_HID_BELKIN)	+= hid-belkin.o
 obj-$(CONFIG_HID_CHERRY)	+= hid-cherry.o
 obj-$(CONFIG_HID_CHICONY)	+= hid-chicony.o
diff --git a/drivers/hid/hid-apple.c b/drivers/hid/hid-apple.c
index 299d2387112..fa10f847f7d 100644
--- a/drivers/hid/hid-apple.c
+++ b/drivers/hid/hid-apple.c
@@ -234,7 +234,7 @@ static int hidinput_apple_event(struct hid_device *hid, struct input_dev *input,
 		}
 	}
 
-        if (iso_layout) {
+	if (iso_layout) {
 		if (asc->quirks & APPLE_ISO_KEYBOARD) {
 			trans = apple_find_translation(apple_iso_keyboard, usage->code);
 			if (trans) {
@@ -458,6 +458,9 @@ static const struct hid_device_id apple_devices[] = {
 	{ HID_BLUETOOTH_DEVICE(USB_VENDOR_ID_APPLE, USB_DEVICE_ID_APPLE_ALU_WIRELESS_2011_ISO),
 		.driver_data = APPLE_NUMLOCK_EMULATION | APPLE_HAS_FN |
 			APPLE_ISO_KEYBOARD },
+	{ HID_BLUETOOTH_DEVICE(USB_VENDOR_ID_APPLE,
+				USB_DEVICE_ID_APPLE_ALU_WIRELESS_2011_ANSI),
+		.driver_data = APPLE_NUMLOCK_EMULATION | APPLE_HAS_FN },
 	{ HID_BLUETOOTH_DEVICE(USB_VENDOR_ID_APPLE, USB_DEVICE_ID_APPLE_ALU_WIRELESS_JIS),
 		.driver_data = APPLE_NUMLOCK_EMULATION | APPLE_HAS_FN },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_APPLE, USB_DEVICE_ID_APPLE_WELLSPRING_ANSI),
diff --git a/drivers/hid/hid-aureal.c b/drivers/hid/hid-aureal.c
new file mode 100644
index 00000000000..ba64b041b8b
--- /dev/null
+++ b/drivers/hid/hid-aureal.c
@@ -0,0 +1,54 @@
+/*
+ *  HID driver for Aureal Cy se W-01RN USB_V3.1 devices
+ *
+ *  Copyright (c) 2010 Franco Catrin <fcatrin@gmail.com>
+ *  Copyright (c) 2010 Ben Cropley <bcropley@internode.on.net>
+ *
+ *  Based on HID sunplus driver by
+ *  Copyright (c) 1999 Andreas Gal
+ *  Copyright (c) 2000-2005 Vojtech Pavlik <vojtech@suse.cz>
+ *  Copyright (c) 2005 Michael Haboustak <mike-@cinci.rr.com> for Concept2, Inc
+ *  Copyright (c) 2006-2007 Jiri Kosina
+ *  Copyright (c) 2007 Paul Walmsley
+ *  Copyright (c) 2008 Jiri Slaby
+ */
+#include <linux/device.h>
+#include <linux/hid.h>
+#include <linux/module.h>
+
+#include "hid-ids.h"
+
+static __u8 *aureal_report_fixup(struct hid_device *hdev, __u8 *rdesc,
+		unsigned int *rsize)
+{
+	if (*rsize >= 54 && rdesc[52] == 0x25 && rdesc[53] == 0x01) {
+		dev_info(&hdev->dev, "fixing Aureal Cy se W-01RN USB_V3.1 report descriptor.\n");
+		rdesc[53] = 0x65;
+	} return rdesc;
+}
+
+static const struct hid_device_id aureal_devices[] = {
+	{ HID_USB_DEVICE(USB_VENDOR_ID_AUREAL, USB_DEVICE_ID_AUREAL_W01RN) },
+	{ }
+};
+MODULE_DEVICE_TABLE(hid, aureal_devices);
+
+static struct hid_driver aureal_driver = {
+	.name = "aureal",
+	.id_table = aureal_devices,
+	.report_fixup = aureal_report_fixup,
+};
+
+static int __init aureal_init(void)
+{
+	return hid_register_driver(&aureal_driver);
+}
+
+static void __exit aureal_exit(void)
+{
+	hid_unregister_driver(&aureal_driver);
+}
+
+module_init(aureal_init);
+module_exit(aureal_exit);
+MODULE_LICENSE("GPL");
diff --git a/drivers/hid/hid-core.c b/drivers/hid/hid-core.c
index 4da66b4b977..8e3a6b26147 100644
--- a/drivers/hid/hid-core.c
+++ b/drivers/hid/hid-core.c
@@ -230,9 +230,16 @@ static int hid_add_field(struct hid_parser *parser, unsigned report_type, unsign
 		return -1;
 	}
 
-	if (parser->global.logical_maximum < parser->global.logical_minimum) {
-		hid_err(parser->device, "logical range invalid %d %d\n",
-				parser->global.logical_minimum, parser->global.logical_maximum);
+	/* Handle both signed and unsigned cases properly */
+	if ((parser->global.logical_minimum < 0 &&
+		parser->global.logical_maximum <
+		parser->global.logical_minimum) ||
+		(parser->global.logical_minimum >= 0 &&
+		(__u32)parser->global.logical_maximum <
+		(__u32)parser->global.logical_minimum)) {
+		dbg_hid("logical range invalid 0x%x 0x%x\n",
+			parser->global.logical_minimum,
+			parser->global.logical_maximum);
 		return -1;
 	}
 
@@ -546,12 +553,11 @@ static void hid_free_report(struct hid_report *report)
 }
 
 /*
- * Free a device structure, all reports, and all fields.
+ * Close report. This function returns the device
+ * state to the point prior to hid_open_report().
  */
-
-static void hid_device_release(struct device *dev)
+static void hid_close_report(struct hid_device *device)
 {
-	struct hid_device *device = container_of(dev, struct hid_device, dev);
 	unsigned i, j;
 
 	for (i = 0; i < HID_REPORT_TYPES; i++) {
@@ -562,11 +568,34 @@ static void hid_device_release(struct device *dev)
 			if (report)
 				hid_free_report(report);
 		}
+		memset(report_enum, 0, sizeof(*report_enum));
+		INIT_LIST_HEAD(&report_enum->report_list);
 	}
 
 	kfree(device->rdesc);
+	device->rdesc = NULL;
+	device->rsize = 0;
+
 	kfree(device->collection);
-	kfree(device);
+	device->collection = NULL;
+	device->collection_size = 0;
+	device->maxcollection = 0;
+	device->maxapplication = 0;
+
+	device->status &= ~HID_STAT_PARSED;
+}
+
+/*
+ * Free a device structure, all reports, and all fields.
+ */
+
+static void hid_device_release(struct device *dev)
+{
+	struct hid_device *hid = container_of(dev, struct hid_device, dev);
+
+	hid_close_report(hid);
+	kfree(hid->dev_rdesc);
+	kfree(hid);
 }
 
 /*
@@ -636,6 +665,60 @@ static u8 *fetch_item(__u8 *start, __u8 *end, struct hid_item *item)
 	return NULL;
 }
 
+static void hid_scan_usage(struct hid_device *hid, u32 usage)
+{
+	if (usage == HID_DG_CONTACTID)
+		hid->group = HID_GROUP_MULTITOUCH;
+}
+
+/*
+ * Scan a report descriptor before the device is added to the bus.
+ * Sets device groups and other properties that determine what driver
+ * to load.
+ */
+static int hid_scan_report(struct hid_device *hid)
+{
+	unsigned int page = 0, delim = 0;
+	__u8 *start = hid->dev_rdesc;
+	__u8 *end = start + hid->dev_rsize;
+	unsigned int u, u_min = 0, u_max = 0;
+	struct hid_item item;
+
+	hid->group = HID_GROUP_GENERIC;
+	while ((start = fetch_item(start, end, &item)) != NULL) {
+		if (item.format != HID_ITEM_FORMAT_SHORT)
+			return -EINVAL;
+		if (item.type == HID_ITEM_TYPE_GLOBAL) {
+			if (item.tag == HID_GLOBAL_ITEM_TAG_USAGE_PAGE)
+				page = item_udata(&item) << 16;
+		} else if (item.type == HID_ITEM_TYPE_LOCAL) {
+			if (delim > 1)
+				break;
+			u = item_udata(&item);
+			if (item.size <= 2)
+				u += page;
+			switch (item.tag) {
+			case HID_LOCAL_ITEM_TAG_DELIMITER:
+				delim += !!u;
+				break;
+			case HID_LOCAL_ITEM_TAG_USAGE:
+				hid_scan_usage(hid, u);
+				break;
+			case HID_LOCAL_ITEM_TAG_USAGE_MINIMUM:
+				u_min = u;
+				break;
+			case HID_LOCAL_ITEM_TAG_USAGE_MAXIMUM:
+				u_max = u;
+				for (u = u_min; u <= u_max; u++)
+					hid_scan_usage(hid, u);
+				break;
+			}
+		}
+	}
+
+	return 0;
+}
+
 /**
  * hid_parse_report - parse device report
  *
@@ -643,15 +726,37 @@ static u8 *fetch_item(__u8 *start, __u8 *end, struct hid_item *item)
  * @start: report start
  * @size: report size
  *
+ * Allocate the device report as read by the bus driver. This function should
+ * only be called from parse() in ll drivers.
+ */
+int hid_parse_report(struct hid_device *hid, __u8 *start, unsigned size)
+{
+	hid->dev_rdesc = kmemdup(start, size, GFP_KERNEL);
+	if (!hid->dev_rdesc)
+		return -ENOMEM;
+	hid->dev_rsize = size;
+	return 0;
+}
+EXPORT_SYMBOL_GPL(hid_parse_report);
+
+/**
+ * hid_open_report - open a driver-specific device report
+ *
+ * @device: hid device
+ *
  * Parse a report description into a hid_device structure. Reports are
  * enumerated, fields are attached to these reports.
  * 0 returned on success, otherwise nonzero error value.
+ *
+ * This function (or the equivalent hid_parse() macro) should only be
+ * called from probe() in drivers, before starting the device.
  */
-int hid_parse_report(struct hid_device *device, __u8 *start,
-		unsigned size)
+int hid_open_report(struct hid_device *device)
 {
 	struct hid_parser *parser;
 	struct hid_item item;
+	unsigned int size;
+	__u8 *start;
 	__u8 *end;
 	int ret;
 	static int (*dispatch_type[])(struct hid_parser *parser,
@@ -662,6 +767,14 @@ int hid_parse_report(struct hid_device *device, __u8 *start,
 		hid_parser_reserved
 	};
 
+	if (WARN_ON(device->status & HID_STAT_PARSED))
+		return -EBUSY;
+
+	start = device->dev_rdesc;
+	if (WARN_ON(!start))
+		return -ENODEV;
+	size = device->dev_rsize;
+
 	if (device->driver->report_fixup)
 		start = device->driver->report_fixup(device, start, &size);
 
@@ -679,6 +792,15 @@ int hid_parse_report(struct hid_device *device, __u8 *start,
 	parser->device = device;
 
 	end = start + size;
+
+	device->collection = kcalloc(HID_DEFAULT_NUM_COLLECTIONS,
+				     sizeof(struct hid_collection), GFP_KERNEL);
+	if (!device->collection) {
+		ret = -ENOMEM;
+		goto err;
+	}
+	device->collection_size = HID_DEFAULT_NUM_COLLECTIONS;
+
 	ret = -EINVAL;
 	while ((start = fetch_item(start, end, &item)) != NULL) {
 
@@ -704,6 +826,7 @@ int hid_parse_report(struct hid_device *device, __u8 *start,
 				goto err;
 			}
 			vfree(parser);
+			device->status |= HID_STAT_PARSED;
 			return 0;
 		}
 	}
@@ -711,9 +834,10 @@ int hid_parse_report(struct hid_device *device, __u8 *start,
 	hid_err(device, "item fetching failed at offset %d\n", (int)(end - start));
 err:
 	vfree(parser);
+	hid_close_report(device);
 	return ret;
 }
-EXPORT_SYMBOL_GPL(hid_parse_report);
+EXPORT_SYMBOL_GPL(hid_open_report);
 
 /*
  * Convert a signed n-bit integer to signed 32-bit integer. Common
@@ -1032,7 +1156,7 @@ static struct hid_report *hid_get_report(struct hid_report_enum *report_enum,
 	return report;
 }
 
-void hid_report_raw_event(struct hid_device *hid, int type, u8 *data, int size,
+int hid_report_raw_event(struct hid_device *hid, int type, u8 *data, int size,
 		int interrupt)
 {
 	struct hid_report_enum *report_enum = hid->report_enum + type;
@@ -1040,10 +1164,11 @@ void hid_report_raw_event(struct hid_device *hid, int type, u8 *data, int size,
 	unsigned int a;
 	int rsize, csize = size;
 	u8 *cdata = data;
+	int ret = 0;
 
 	report = hid_get_report(report_enum, data);
 	if (!report)
-		return;
+		goto out;
 
 	if (report_enum->numbered) {
 		cdata++;
@@ -1063,14 +1188,19 @@ void hid_report_raw_event(struct hid_device *hid, int type, u8 *data, int size,
 
 	if ((hid->claimed & HID_CLAIMED_HIDDEV) && hid->hiddev_report_event)
 		hid->hiddev_report_event(hid, report);
-	if (hid->claimed & HID_CLAIMED_HIDRAW)
-		hidraw_report_event(hid, data, size);
+	if (hid->claimed & HID_CLAIMED_HIDRAW) {
+		ret = hidraw_report_event(hid, data, size);
+		if (ret)
+			goto out;
+	}
 
 	for (a = 0; a < report->maxfield; a++)
 		hid_input_field(hid, report->field[a], cdata, interrupt);
 
 	if (hid->claimed & HID_CLAIMED_INPUT)
 		hidinput_report_event(hid, report);
+out:
+	return ret;
 }
 EXPORT_SYMBOL_GPL(hid_report_raw_event);
 
@@ -1147,7 +1277,7 @@ nomem:
 		}
 	}
 
-	hid_report_raw_event(hid, type, data, size, interrupt);
+	ret = hid_report_raw_event(hid, type, data, size, interrupt);
 
 unlock:
 	up(&hid->driver_lock);
@@ -1158,7 +1288,8 @@ EXPORT_SYMBOL_GPL(hid_input_report);
 static bool hid_match_one_id(struct hid_device *hdev,
 		const struct hid_device_id *id)
 {
-	return id->bus == hdev->bus &&
+	return (id->bus == HID_BUS_ANY || id->bus == hdev->bus) &&
+		(id->group == HID_GROUP_ANY || id->group == hdev->group) &&
 		(id->vendor == HID_ANY_ID || id->vendor == hdev->vendor) &&
 		(id->product == HID_ANY_ID || id->product == hdev->product);
 }
@@ -1234,10 +1365,6 @@ int hid_connect(struct hid_device *hdev, unsigned int connect_mask)
 	if ((connect_mask & HID_CONNECT_HIDINPUT) && !hidinput_connect(hdev,
 				connect_mask & HID_CONNECT_HIDINPUT_FORCE))
 		hdev->claimed |= HID_CLAIMED_INPUT;
-	if (hdev->quirks & HID_QUIRK_MULTITOUCH) {
-		/* this device should be handled by hid-multitouch, skip it */
-		return -ENODEV;
-	}
 
 	if ((connect_mask & HID_CONNECT_HIDDEV) && hdev->hiddev_connect &&
 			!hdev->hiddev_connect(hdev,
@@ -1314,13 +1441,10 @@ EXPORT_SYMBOL_GPL(hid_disconnect);
 
 /* a list of devices for which there is a specialized driver on HID bus */
 static const struct hid_device_id hid_have_special_driver[] = {
-	{ HID_USB_DEVICE(USB_VENDOR_ID_3M, USB_DEVICE_ID_3M1968) },
-	{ HID_USB_DEVICE(USB_VENDOR_ID_3M, USB_DEVICE_ID_3M2256) },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_A4TECH, USB_DEVICE_ID_A4TECH_WCP32PU) },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_A4TECH, USB_DEVICE_ID_A4TECH_X5_005D) },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_A4TECH, USB_DEVICE_ID_A4TECH_RP_649) },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_ACRUX, 0x0802) },
-	{ HID_USB_DEVICE(USB_VENDOR_ID_ACTIONSTAR, USB_DEVICE_ID_ACTIONSTAR_1011) },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_APPLE, USB_DEVICE_ID_APPLE_ATV_IRCONTROL) },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_APPLE, USB_DEVICE_ID_APPLE_IRCONTROL4) },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_APPLE, USB_DEVICE_ID_APPLE_MIGHTYMOUSE) },
@@ -1385,60 +1509,33 @@ static const struct hid_device_id hid_have_special_driver[] = {
 	{ HID_BLUETOOTH_DEVICE(USB_VENDOR_ID_APPLE, USB_DEVICE_ID_APPLE_ALU_WIRELESS_2011_ISO) },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_APPLE, USB_DEVICE_ID_APPLE_FOUNTAIN_TP_ONLY) },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_APPLE, USB_DEVICE_ID_APPLE_GEYSER1_TP_ONLY) },
-	{ HID_USB_DEVICE(USB_VENDOR_ID_ASUS, USB_DEVICE_ID_ASUS_T91MT) },
-	{ HID_USB_DEVICE(USB_VENDOR_ID_ASUS, USB_DEVICE_ID_ASUSTEK_MULTITOUCH_YFO) },
+	{ HID_USB_DEVICE(USB_VENDOR_ID_AUREAL, USB_DEVICE_ID_AUREAL_W01RN) },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_BELKIN, USB_DEVICE_ID_FLIP_KVM) },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_BTC, USB_DEVICE_ID_BTC_EMPREX_REMOTE) },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_BTC, USB_DEVICE_ID_BTC_EMPREX_REMOTE_2) },
-	{ HID_USB_DEVICE(USB_VENDOR_ID_CANDO, USB_DEVICE_ID_CANDO_PIXCIR_MULTI_TOUCH) },
-	{ HID_USB_DEVICE(USB_VENDOR_ID_CANDO, USB_DEVICE_ID_CANDO_MULTI_TOUCH) },
-	{ HID_USB_DEVICE(USB_VENDOR_ID_CANDO, USB_DEVICE_ID_CANDO_MULTI_TOUCH_10_1) },
-	{ HID_USB_DEVICE(USB_VENDOR_ID_CANDO, USB_DEVICE_ID_CANDO_MULTI_TOUCH_11_6) },
-	{ HID_USB_DEVICE(USB_VENDOR_ID_CANDO, USB_DEVICE_ID_CANDO_MULTI_TOUCH_15_6) },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_CHERRY, USB_DEVICE_ID_CHERRY_CYMOTION) },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_CHERRY, USB_DEVICE_ID_CHERRY_CYMOTION_SOLAR) },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_CHICONY, USB_DEVICE_ID_CHICONY_TACTICAL_PAD) },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_CHICONY, USB_DEVICE_ID_CHICONY_WIRELESS) },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_CHICONY, USB_DEVICE_ID_CHICONY_WIRELESS2) },
-	{ HID_USB_DEVICE(USB_VENDOR_ID_CHUNGHWAT, USB_DEVICE_ID_CHUNGHWAT_MULTITOUCH) },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_CREATIVELABS, USB_DEVICE_ID_PRODIKEYS_PCMIDI) },
-	{ HID_USB_DEVICE(USB_VENDOR_ID_CVTOUCH, USB_DEVICE_ID_CVTOUCH_SCREEN) },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_CYPRESS, USB_DEVICE_ID_CYPRESS_BARCODE_1) },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_CYPRESS, USB_DEVICE_ID_CYPRESS_BARCODE_2) },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_CYPRESS, USB_DEVICE_ID_CYPRESS_BARCODE_3) },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_CYPRESS, USB_DEVICE_ID_CYPRESS_MOUSE) },
-	{ HID_USB_DEVICE(USB_VENDOR_ID_CYPRESS, USB_DEVICE_ID_CYPRESS_TRUETOUCH) },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_DRAGONRISE, 0x0006) },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_DRAGONRISE, 0x0011) },
-	{ HID_USB_DEVICE(USB_VENDOR_ID_DWAV, USB_DEVICE_ID_DWAV_EGALAX_MULTITOUCH_480D) },
-	{ HID_USB_DEVICE(USB_VENDOR_ID_DWAV, USB_DEVICE_ID_DWAV_EGALAX_MULTITOUCH_480E) },
-	{ HID_USB_DEVICE(USB_VENDOR_ID_DWAV, USB_DEVICE_ID_DWAV_EGALAX_MULTITOUCH_720C) },
-	{ HID_USB_DEVICE(USB_VENDOR_ID_DWAV, USB_DEVICE_ID_DWAV_EGALAX_MULTITOUCH_7224) },
-	{ HID_USB_DEVICE(USB_VENDOR_ID_DWAV, USB_DEVICE_ID_DWAV_EGALAX_MULTITOUCH_725E) },
-	{ HID_USB_DEVICE(USB_VENDOR_ID_DWAV, USB_DEVICE_ID_DWAV_EGALAX_MULTITOUCH_726B) },
-	{ HID_USB_DEVICE(USB_VENDOR_ID_DWAV, USB_DEVICE_ID_DWAV_EGALAX_MULTITOUCH_72A1) },
-	{ HID_USB_DEVICE(USB_VENDOR_ID_DWAV, USB_DEVICE_ID_DWAV_EGALAX_MULTITOUCH_7302) },
-	{ HID_USB_DEVICE(USB_VENDOR_ID_DWAV, USB_DEVICE_ID_DWAV_EGALAX_MULTITOUCH_A001) },
 	{ HID_BLUETOOTH_DEVICE(USB_VENDOR_ID_ELECOM, USB_DEVICE_ID_ELECOM_BM084) },
-	{ HID_USB_DEVICE(USB_VENDOR_ID_ELO, USB_DEVICE_ID_ELO_TS2515) },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_EMS, USB_DEVICE_ID_EMS_TRIO_LINKER_PLUS_II) },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_EZKEY, USB_DEVICE_ID_BTC_8193) },
-	{ HID_BLUETOOTH_DEVICE(USB_VENDOR_ID_FRUCTEL, USB_DEVICE_ID_GAMETEL_MT_MODE) },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_GAMERON, USB_DEVICE_ID_GAMERON_DUAL_PSX_ADAPTOR) },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_GAMERON, USB_DEVICE_ID_GAMERON_DUAL_PCS_ADAPTOR) },
-	{ HID_USB_DEVICE(USB_VENDOR_ID_GENERAL_TOUCH, USB_DEVICE_ID_GENERAL_TOUCH_WIN7_TWOFINGERS) },
-	{ HID_USB_DEVICE(USB_VENDOR_ID_GOODTOUCH, USB_DEVICE_ID_GOODTOUCH_000f) },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_GREENASIA, 0x0003) },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_GREENASIA, 0x0012) },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_GYRATION, USB_DEVICE_ID_GYRATION_REMOTE) },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_GYRATION, USB_DEVICE_ID_GYRATION_REMOTE_2) },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_GYRATION, USB_DEVICE_ID_GYRATION_REMOTE_3) },
-	{ HID_USB_DEVICE(USB_VENDOR_ID_HANVON, USB_DEVICE_ID_HANVON_MULTITOUCH) },
-	{ HID_USB_DEVICE(USB_VENDOR_ID_HANVON_ALT, USB_DEVICE_ID_HANVON_ALT_MULTITOUCH) },
- 	{ HID_USB_DEVICE(USB_VENDOR_ID_IDEACOM, USB_DEVICE_ID_IDEACOM_IDC6650) },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_HOLTEK, USB_DEVICE_ID_HOLTEK_ON_LINE_GRIP) },
-	{ HID_USB_DEVICE(USB_VENDOR_ID_ILITEK, USB_DEVICE_ID_ILITEK_MULTITOUCH) },
-	{ HID_USB_DEVICE(USB_VENDOR_ID_IRTOUCHSYSTEMS, USB_DEVICE_ID_IRTOUCH_INFRARED_USB) },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_KENSINGTON, USB_DEVICE_ID_KS_SLIMBLADE) },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_KEYTOUCH, USB_DEVICE_ID_KEYTOUCH_IEC) },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_KYE, USB_DEVICE_ID_KYE_ERGO_525V) },
@@ -1447,7 +1544,6 @@ static const struct hid_device_id hid_have_special_driver[] = {
 	{ HID_USB_DEVICE(USB_VENDOR_ID_KYE, USB_DEVICE_ID_KYE_EASYPEN_M610X) },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_LABTEC, USB_DEVICE_ID_LABTEC_WIRELESS_KEYBOARD) },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_LCPOWER, USB_DEVICE_ID_LCPOWER_LC1000 ) },
-	{ HID_USB_DEVICE(USB_VENDOR_ID_LG, USB_DEVICE_ID_LG_MULTITOUCH) },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_LOGITECH, USB_DEVICE_ID_MX3000_RECEIVER) },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_LOGITECH, USB_DEVICE_ID_S510_RECEIVER) },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_LOGITECH, USB_DEVICE_ID_S510_RECEIVER_2) },
@@ -1480,8 +1576,6 @@ static const struct hid_device_id hid_have_special_driver[] = {
 	{ HID_USB_DEVICE(USB_VENDOR_ID_LOGITECH, USB_DEVICE_ID_LOGITECH_RUMBLEPAD2) },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_LOGITECH, USB_DEVICE_ID_SPACETRAVELLER) },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_LOGITECH, USB_DEVICE_ID_SPACENAVIGATOR) },
-	{ HID_USB_DEVICE(USB_VENDOR_ID_LUMIO, USB_DEVICE_ID_CRYSTALTOUCH) },
-	{ HID_USB_DEVICE(USB_VENDOR_ID_LUMIO, USB_DEVICE_ID_CRYSTALTOUCH_DUAL) },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_MICROCHIP, USB_DEVICE_ID_PICOLCD) },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_MICROCHIP, USB_DEVICE_ID_PICOLCD_BOOTLOADER) },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_MICROSOFT, USB_DEVICE_ID_MS_COMFORT_MOUSE_4500) },
@@ -1513,15 +1607,8 @@ static const struct hid_device_id hid_have_special_driver[] = {
 	{ HID_USB_DEVICE(USB_VENDOR_ID_NTRIG, USB_DEVICE_ID_NTRIG_TOUCH_SCREEN_18) },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_ORTEK, USB_DEVICE_ID_ORTEK_PKB1700) },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_ORTEK, USB_DEVICE_ID_ORTEK_WKB2000) },
-	{ HID_USB_DEVICE(USB_VENDOR_ID_PANASONIC, USB_DEVICE_ID_PANABOARD_UBT780) },
-	{ HID_USB_DEVICE(USB_VENDOR_ID_PANASONIC, USB_DEVICE_ID_PANABOARD_UBT880) },
-	{ HID_USB_DEVICE(USB_VENDOR_ID_PENMOUNT, USB_DEVICE_ID_PENMOUNT_PCI) },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_PETALYNX, USB_DEVICE_ID_PETALYNX_MAXTER_REMOTE) },
-	{ HID_USB_DEVICE(USB_VENDOR_ID_PIXART, USB_DEVICE_ID_PIXART_OPTICAL_TOUCH_SCREEN) },
-	{ HID_USB_DEVICE(USB_VENDOR_ID_PIXART, USB_DEVICE_ID_PIXART_OPTICAL_TOUCH_SCREEN1) },
-	{ HID_USB_DEVICE(USB_VENDOR_ID_PIXART, USB_DEVICE_ID_PIXART_OPTICAL_TOUCH_SCREEN2) },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_PRIMAX, USB_DEVICE_ID_PRIMAX_KEYBOARD) },
-	{ HID_USB_DEVICE(USB_VENDOR_ID_QUANTA, USB_DEVICE_ID_QUANTA_OPTICAL_TOUCH) },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_QUANTA, USB_DEVICE_ID_PIXART_IMAGING_INC_OPTICAL_TOUCH_SCREEN) },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_ROCCAT, USB_DEVICE_ID_ROCCAT_KONE) },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_ROCCAT, USB_DEVICE_ID_ROCCAT_ARVO) },
@@ -1538,9 +1625,6 @@ static const struct hid_device_id hid_have_special_driver[] = {
 	{ HID_USB_DEVICE(USB_VENDOR_ID_SONY, USB_DEVICE_ID_SONY_NAVIGATION_CONTROLLER) },
 	{ HID_BLUETOOTH_DEVICE(USB_VENDOR_ID_SONY, USB_DEVICE_ID_SONY_PS3_CONTROLLER) },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_SONY, USB_DEVICE_ID_SONY_VAIO_VGX_MOUSE) },
-	{ HID_USB_DEVICE(USB_VENDOR_ID_STANTUM, USB_DEVICE_ID_MTP) },
-	{ HID_USB_DEVICE(USB_VENDOR_ID_STANTUM_STM, USB_DEVICE_ID_MTP_STM) },
-	{ HID_USB_DEVICE(USB_VENDOR_ID_STANTUM_SITRONIX, USB_DEVICE_ID_MTP_SITRONIX) },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_SUNPLUS, USB_DEVICE_ID_SUNPLUS_WDESKTOP) },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_THRUSTMASTER, 0xb300) },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_THRUSTMASTER, 0xb304) },
@@ -1554,16 +1638,13 @@ static const struct hid_device_id hid_have_special_driver[] = {
 	{ HID_USB_DEVICE(USB_VENDOR_ID_TIVO, USB_DEVICE_ID_TIVO_SLIDE) },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_TOPSEED, USB_DEVICE_ID_TOPSEED_CYBERLINK) },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_TOPSEED2, USB_DEVICE_ID_TOPSEED2_RF_COMBO) },
-	{ HID_USB_DEVICE(USB_VENDOR_ID_TOUCH_INTL, USB_DEVICE_ID_TOUCH_INTL_MULTI_TOUCH) },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_TWINHAN, USB_DEVICE_ID_TWINHAN_IR_REMOTE) },
-	{ HID_USB_DEVICE(USB_VENDOR_ID_TURBOX, USB_DEVICE_ID_TURBOX_TOUCHSCREEN_MOSART) },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_UCLOGIC, USB_DEVICE_ID_UCLOGIC_TABLET_PF1209) },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_UCLOGIC, USB_DEVICE_ID_UCLOGIC_TABLET_WP4030U) },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_UCLOGIC, USB_DEVICE_ID_UCLOGIC_TABLET_WP5540U) },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_UCLOGIC, USB_DEVICE_ID_UCLOGIC_TABLET_WP8060U) },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_UCLOGIC, USB_DEVICE_ID_UCLOGIC_TABLET_WP1062) },
-	{ HID_USB_DEVICE(USB_VENDOR_ID_UNITEC, USB_DEVICE_ID_UNITEC_USB_TOUCH_0709) },
-	{ HID_USB_DEVICE(USB_VENDOR_ID_UNITEC, USB_DEVICE_ID_UNITEC_USB_TOUCH_0A19) },
+	{ HID_USB_DEVICE(USB_VENDOR_ID_UCLOGIC, USB_DEVICE_ID_UCLOGIC_WIRELESS_TABLET_TWHL850) },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_WISEGROUP, USB_DEVICE_ID_SMARTJOY_PLUS) },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_WISEGROUP, USB_DEVICE_ID_SUPER_JOY_BOX_3) },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_WISEGROUP, USB_DEVICE_ID_DUAL_USB_JOYPAD) },
@@ -1578,16 +1659,7 @@ static const struct hid_device_id hid_have_special_driver[] = {
 	{ HID_USB_DEVICE(USB_VENDOR_ID_WALTOP, USB_DEVICE_ID_WALTOP_PID_0038) },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_WALTOP, USB_DEVICE_ID_WALTOP_MEDIA_TABLET_10_6_INCH) },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_WALTOP, USB_DEVICE_ID_WALTOP_MEDIA_TABLET_14_1_INCH) },
-	{ HID_USB_DEVICE(USB_VENDOR_ID_XAT, USB_DEVICE_ID_XAT_CSR) },
-	{ HID_USB_DEVICE(USB_VENDOR_ID_XIROKU, USB_DEVICE_ID_XIROKU_SPX) },
-	{ HID_USB_DEVICE(USB_VENDOR_ID_XIROKU, USB_DEVICE_ID_XIROKU_MPX) },
-	{ HID_USB_DEVICE(USB_VENDOR_ID_XIROKU, USB_DEVICE_ID_XIROKU_CSR) },
-	{ HID_USB_DEVICE(USB_VENDOR_ID_XIROKU, USB_DEVICE_ID_XIROKU_SPX1) },
-	{ HID_USB_DEVICE(USB_VENDOR_ID_XIROKU, USB_DEVICE_ID_XIROKU_MPX1) },
-	{ HID_USB_DEVICE(USB_VENDOR_ID_XIROKU, USB_DEVICE_ID_XIROKU_CSR1) },
-	{ HID_USB_DEVICE(USB_VENDOR_ID_XIROKU, USB_DEVICE_ID_XIROKU_SPX2) },
-	{ HID_USB_DEVICE(USB_VENDOR_ID_XIROKU, USB_DEVICE_ID_XIROKU_MPX2) },
-	{ HID_USB_DEVICE(USB_VENDOR_ID_XIROKU, USB_DEVICE_ID_XIROKU_CSR2) },
+	{ HID_USB_DEVICE(USB_VENDOR_ID_WALTOP, USB_DEVICE_ID_WALTOP_SIRIUS_BATTERY_FREE_TABLET) },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_X_TENSIONS, USB_DEVICE_ID_SPEEDLINK_VAD_CEZANNE) },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_ZEROPLUS, 0x0005) },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_ZEROPLUS, 0x0030) },
@@ -1631,6 +1703,7 @@ static ssize_t store_new_id(struct device_driver *drv, const char *buf,
 		return -ENOMEM;
 
 	dynid->id.bus = bus;
+	dynid->id.group = HID_GROUP_ANY;
 	dynid->id.vendor = vendor;
 	dynid->id.product = product;
 	dynid->id.driver_data = driver_data;
@@ -1679,18 +1752,7 @@ static int hid_bus_match(struct device *dev, struct device_driver *drv)
 	struct hid_driver *hdrv = container_of(drv, struct hid_driver, driver);
 	struct hid_device *hdev = container_of(dev, struct hid_device, dev);
 
-	if ((hdev->quirks & HID_QUIRK_MULTITOUCH) &&
-		!strncmp(hdrv->name, "hid-multitouch", 14))
-		return 1;
-
-	if (!hid_match_device(hdev, hdrv))
-		return 0;
-
-	/* generic wants all that don't have specialized driver */
-	if (!strncmp(hdrv->name, "generic-", 8) && !hid_ignore_special_drivers)
-		return !hid_match_id(hdev, hid_have_special_driver);
-
-	return 1;
+	return hid_match_device(hdev, hdrv) != NULL;
 }
 
 static int hid_device_probe(struct device *dev)
@@ -1707,23 +1769,22 @@ static int hid_device_probe(struct device *dev)
 	if (!hdev->driver) {
 		id = hid_match_device(hdev, hdrv);
 		if (id == NULL) {
-			if (!((hdev->quirks & HID_QUIRK_MULTITOUCH) &&
-				!strncmp(hdrv->name, "hid-multitouch", 14))) {
-				ret = -ENODEV;
-				goto unlock;
-			}
+			ret = -ENODEV;
+			goto unlock;
 		}
 
 		hdev->driver = hdrv;
 		if (hdrv->probe) {
 			ret = hdrv->probe(hdev, id);
 		} else { /* default probe */
-			ret = hid_parse(hdev);
+			ret = hid_open_report(hdev);
 			if (!ret)
 				ret = hid_hw_start(hdev, HID_CONNECT_DEFAULT);
 		}
-		if (ret)
+		if (ret) {
+			hid_close_report(hdev);
 			hdev->driver = NULL;
+		}
 	}
 unlock:
 	up(&hdev->driver_lock);
@@ -1744,6 +1805,7 @@ static int hid_device_remove(struct device *dev)
 			hdrv->remove(hdev);
 		else /* default remove */
 			hid_hw_stop(hdev);
+		hid_close_report(hdev);
 		hdev->driver = NULL;
 	}
 
@@ -1751,6 +1813,23 @@ static int hid_device_remove(struct device *dev)
 	return 0;
 }
 
+static ssize_t modalias_show(struct device *dev, struct device_attribute *a,
+			     char *buf)
+{
+	struct hid_device *hdev = container_of(dev, struct hid_device, dev);
+	int len;
+
+	len = snprintf(buf, PAGE_SIZE, "hid:b%04Xg%04Xv%08Xp%08X\n",
+		       hdev->bus, hdev->group, hdev->vendor, hdev->product);
+
+	return (len >= PAGE_SIZE) ? (PAGE_SIZE - 1) : len;
+}
+
+static struct device_attribute hid_dev_attrs[] = {
+	__ATTR_RO(modalias),
+	__ATTR_NULL,
+};
+
 static int hid_uevent(struct device *dev, struct kobj_uevent_env *env)
 {
 	struct hid_device *hdev = container_of(dev, struct hid_device, dev);
@@ -1768,8 +1847,8 @@ static int hid_uevent(struct device *dev, struct kobj_uevent_env *env)
 	if (add_uevent_var(env, "HID_UNIQ=%s", hdev->uniq))
 		return -ENOMEM;
 
-	if (add_uevent_var(env, "MODALIAS=hid:b%04Xv%08Xp%08X",
-			hdev->bus, hdev->vendor, hdev->product))
+	if (add_uevent_var(env, "MODALIAS=hid:b%04Xg%04Xv%08Xp%08X",
+			   hdev->bus, hdev->group, hdev->vendor, hdev->product))
 		return -ENOMEM;
 
 	return 0;
@@ -1777,6 +1856,7 @@ static int hid_uevent(struct device *dev, struct kobj_uevent_env *env)
 
 static struct bus_type hid_bus_type = {
 	.name		= "hid",
+	.dev_attrs	= hid_dev_attrs,
 	.match		= hid_bus_match,
 	.probe		= hid_device_probe,
 	.remove		= hid_device_remove,
@@ -2075,6 +2155,26 @@ int hid_add_device(struct hid_device *hdev)
             && (hid_ignore(hdev) || (hdev->quirks & HID_QUIRK_IGNORE)))
 		return -ENODEV;
 
+	/*
+	 * Read the device report descriptor once and use as template
+	 * for the driver-specific modifications.
+	 */
+	ret = hdev->ll_driver->parse(hdev);
+	if (ret)
+		return ret;
+	if (!hdev->dev_rdesc)
+		return -ENODEV;
+
+	/*
+	 * Scan generic devices for group information
+	 */
+	if (hid_ignore_special_drivers ||
+	    !hid_match_id(hdev, hid_have_special_driver)) {
+		ret = hid_scan_report(hdev);
+		if (ret)
+			hid_warn(hdev, "bad device descriptor (%d)\n", ret);
+	}
+
 	/* XXX hack, any other cleaner solution after the driver core
 	 * is converted to allow more than 20 bytes as the device name? */
 	dev_set_name(&hdev->dev, "%04X:%04X:%04X.%04X", hdev->bus,
@@ -2103,7 +2203,6 @@ EXPORT_SYMBOL_GPL(hid_add_device);
 struct hid_device *hid_allocate_device(void)
 {
 	struct hid_device *hdev;
-	unsigned int i;
 	int ret = -ENOMEM;
 
 	hdev = kzalloc(sizeof(*hdev), GFP_KERNEL);
@@ -2114,23 +2213,13 @@ struct hid_device *hid_allocate_device(void)
 	hdev->dev.release = hid_device_release;
 	hdev->dev.bus = &hid_bus_type;
 
-	hdev->collection = kcalloc(HID_DEFAULT_NUM_COLLECTIONS,
-			sizeof(struct hid_collection), GFP_KERNEL);
-	if (hdev->collection == NULL)
-		goto err;
-	hdev->collection_size = HID_DEFAULT_NUM_COLLECTIONS;
-
-	for (i = 0; i < HID_REPORT_TYPES; i++)
-		INIT_LIST_HEAD(&hdev->report_enum[i].report_list);
+	hid_close_report(hdev);
 
 	init_waitqueue_head(&hdev->debug_wait);
 	INIT_LIST_HEAD(&hdev->debug_list);
 	sema_init(&hdev->driver_lock, 1);
 
 	return hdev;
-err:
-	put_device(&hdev->dev);
-	return ERR_PTR(ret);
 }
 EXPORT_SYMBOL_GPL(hid_allocate_device);
 
@@ -2141,6 +2230,9 @@ static void hid_remove_device(struct hid_device *hdev)
 		hid_debug_unregister(hdev);
 		hdev->status &= ~HID_STAT_ADDED;
 	}
+	kfree(hdev->dev_rdesc);
+	hdev->dev_rdesc = NULL;
+	hdev->dev_rsize = 0;
 }
 
 /**
diff --git a/drivers/hid/hid-generic.c b/drivers/hid/hid-generic.c
new file mode 100644
index 00000000000..a8b3148e03a
--- /dev/null
+++ b/drivers/hid/hid-generic.c
@@ -0,0 +1,53 @@
+/*
+ *  HID support for Linux
+ *
+ *  Copyright (c) 1999 Andreas Gal
+ *  Copyright (c) 2000-2005 Vojtech Pavlik <vojtech@suse.cz>
+ *  Copyright (c) 2005 Michael Haboustak <mike-@cinci.rr.com> for Concept2, Inc
+ *  Copyright (c) 2007-2008 Oliver Neukum
+ *  Copyright (c) 2006-2012 Jiri Kosina
+ *  Copyright (c) 2012 Henrik Rydberg
+ */
+
+/*
+ * 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/slab.h>
+#include <linux/kernel.h>
+#include <asm/unaligned.h>
+#include <asm/byteorder.h>
+
+#include <linux/hid.h>
+
+static const struct hid_device_id hid_table[] = {
+	{ HID_DEVICE(HID_BUS_ANY, HID_GROUP_GENERIC, HID_ANY_ID, HID_ANY_ID) },
+	{ }
+};
+MODULE_DEVICE_TABLE(hid, hid_table);
+
+static struct hid_driver hid_generic = {
+	.name = "hid-generic",
+	.id_table = hid_table,
+};
+
+static int __init hid_init(void)
+{
+	return hid_register_driver(&hid_generic);
+}
+
+static void __exit hid_exit(void)
+{
+	hid_unregister_driver(&hid_generic);
+}
+
+module_init(hid_init);
+module_exit(hid_exit);
+
+MODULE_AUTHOR("Henrik Rydberg");
+MODULE_DESCRIPTION("HID generic driver");
+MODULE_LICENSE("GPL");
diff --git a/drivers/hid/hid-hyperv.c b/drivers/hid/hid-hyperv.c
index 406632472c1..3d62781b899 100644
--- a/drivers/hid/hid-hyperv.c
+++ b/drivers/hid/hid-hyperv.c
@@ -430,6 +430,15 @@ cleanup:
 	return ret;
 }
 
+static int mousevsc_hid_parse(struct hid_device *hid)
+{
+	struct hv_device *dev = hid_get_drvdata(hid);
+	struct mousevsc_dev *input_dev = hv_get_drvdata(dev);
+
+	return hid_parse_report(hid, input_dev->report_desc,
+				input_dev->report_desc_size);
+}
+
 static int mousevsc_hid_open(struct hid_device *hid)
 {
 	return 0;
@@ -449,6 +458,7 @@ static void mousevsc_hid_stop(struct hid_device *hid)
 }
 
 static struct hid_ll_driver mousevsc_ll_driver = {
+	.parse = mousevsc_hid_parse,
 	.open = mousevsc_hid_open,
 	.close = mousevsc_hid_close,
 	.start = mousevsc_hid_start,
@@ -506,13 +516,14 @@ static int mousevsc_probe(struct hv_device *device,
 
 	sprintf(hid_dev->name, "%s", "Microsoft Vmbus HID-compliant Mouse");
 
+	hid_set_drvdata(hid_dev, device);
+
 	ret = hid_add_device(hid_dev);
 	if (ret)
 		goto probe_err1;
 
-	ret = hid_parse_report(hid_dev, input_dev->report_desc,
-				input_dev->report_desc_size);
 
+	ret = hid_parse(hid_dev);
 	if (ret) {
 		hid_err(hid_dev, "parse failed\n");
 		goto probe_err2;
diff --git a/drivers/hid/hid-ids.h b/drivers/hid/hid-ids.h
index e39aecb1f9f..9373f535dfe 100644
--- a/drivers/hid/hid-ids.h
+++ b/drivers/hid/hid-ids.h
@@ -154,9 +154,15 @@
 #define USB_DEVICE_ID_ATMEL_MULTITOUCH	0x211c
 #define USB_DEVICE_ID_ATMEL_MXT_DIGITIZER	0x2118
 
+#define USB_VENDOR_ID_AUREAL		0x0755
+#define USB_DEVICE_ID_AUREAL_W01RN	0x2626
+
 #define USB_VENDOR_ID_AVERMEDIA		0x07ca
 #define USB_DEVICE_ID_AVER_FM_MR800	0xb800
 
+#define USB_VENDOR_ID_BAANTO		0x2453
+#define USB_DEVICE_ID_BAANTO_MT_190W2	0x0100
+
 #define USB_VENDOR_ID_BELKIN		0x050d
 #define USB_DEVICE_ID_FLIP_KVM		0x3201
 
@@ -726,6 +732,7 @@
 #define USB_DEVICE_ID_UCLOGIC_TABLET_WP5540U	0x0004
 #define USB_DEVICE_ID_UCLOGIC_TABLET_WP8060U	0x0005
 #define USB_DEVICE_ID_UCLOGIC_TABLET_WP1062	0x0064
+#define USB_DEVICE_ID_UCLOGIC_WIRELESS_TABLET_TWHL850	0x0522
 
 #define USB_VENDOR_ID_UNITEC	0x227d
 #define USB_DEVICE_ID_UNITEC_USB_TOUCH_0709	0x0709
@@ -749,6 +756,7 @@
 #define USB_DEVICE_ID_WALTOP_PID_0038			0x0038
 #define USB_DEVICE_ID_WALTOP_MEDIA_TABLET_10_6_INCH	0x0501
 #define USB_DEVICE_ID_WALTOP_MEDIA_TABLET_14_1_INCH	0x0500
+#define USB_DEVICE_ID_WALTOP_SIRIUS_BATTERY_FREE_TABLET	0x0502
 
 #define USB_VENDOR_ID_WISEGROUP		0x0925
 #define USB_DEVICE_ID_SMARTJOY_PLUS	0x0005
diff --git a/drivers/hid/hid-input.c b/drivers/hid/hid-input.c
index 002781c5a61..132b0019365 100644
--- a/drivers/hid/hid-input.c
+++ b/drivers/hid/hid-input.c
@@ -225,7 +225,10 @@ static __s32 hidinput_calc_abs_res(const struct hid_field *field, __u16 code)
 	 * Verify and convert units.
 	 * See HID specification v1.11 6.2.2.7 Global Items for unit decoding
 	 */
-	if (code == ABS_X || code == ABS_Y || code == ABS_Z) {
+	switch (code) {
+	case ABS_X:
+	case ABS_Y:
+	case ABS_Z:
 		if (field->unit == 0x11) {		/* If centimeters */
 			/* Convert to millimeters */
 			unit_exponent += 1;
@@ -239,7 +242,13 @@ static __s32 hidinput_calc_abs_res(const struct hid_field *field, __u16 code)
 		} else {
 			return 0;
 		}
-	} else if (code == ABS_RX || code == ABS_RY || code == ABS_RZ) {
+		break;
+
+	case ABS_RX:
+	case ABS_RY:
+	case ABS_RZ:
+	case ABS_TILT_X:
+	case ABS_TILT_Y:
 		if (field->unit == 0x14) {		/* If degrees */
 			/* Convert to radians */
 			prev = logical_extents;
@@ -250,7 +259,9 @@ static __s32 hidinput_calc_abs_res(const struct hid_field *field, __u16 code)
 		} else if (field->unit != 0x12) {	/* If not radians */
 			return 0;
 		}
-	} else {
+		break;
+
+	default:
 		return 0;
 	}
 
@@ -623,6 +634,14 @@ static void hidinput_configure_usage(struct hid_input *hidinput, struct hid_fiel
 			map_key_clear(BTN_TOOL_RUBBER);
 			break;
 
+		case 0x3d: /* X Tilt */
+			map_abs_clear(ABS_TILT_X);
+			break;
+
+		case 0x3e: /* Y Tilt */
+			map_abs_clear(ABS_TILT_Y);
+			break;
+
 		case 0x33: /* Touch */
 		case 0x42: /* TipSwitch */
 		case 0x43: /* TipSwitch2 */
@@ -638,10 +657,6 @@ static void hidinput_configure_usage(struct hid_input *hidinput, struct hid_fiel
 			map_key_clear(BTN_STYLUS2);
 			break;
 
-		case 0x51: /* ContactID */
-			device->quirks |= HID_QUIRK_MULTITOUCH;
-			goto unknown;
-
 		default:  goto unknown;
 		}
 		break;
@@ -1208,13 +1223,6 @@ int hidinput_connect(struct hid_device *hid, unsigned int force)
 		}
 	}
 
-	if (hid->quirks & HID_QUIRK_MULTITOUCH) {
-		/* generic hid does not know how to handle multitouch devices */
-		if (hidinput)
-			goto out_cleanup;
-		goto out_unwind;
-	}
-
 	if (hidinput && input_register_device(hidinput->input))
 		goto out_cleanup;
 
diff --git a/drivers/hid/hid-lg.c b/drivers/hid/hid-lg.c
index e7a7bd1eb34..fc37ed6b108 100644
--- a/drivers/hid/hid-lg.c
+++ b/drivers/hid/hid-lg.c
@@ -109,23 +109,23 @@ static __u8 dfp_rdesc_fixed[] = {
 static __u8 *lg_report_fixup(struct hid_device *hdev, __u8 *rdesc,
 		unsigned int *rsize)
 {
-	unsigned long quirks = (unsigned long)hid_get_drvdata(hdev);
+	struct lg_drv_data *drv_data = (struct lg_drv_data *)hid_get_drvdata(hdev);
 
-	if ((quirks & LG_RDESC) && *rsize >= 90 && rdesc[83] == 0x26 &&
+	if ((drv_data->quirks & LG_RDESC) && *rsize >= 90 && rdesc[83] == 0x26 &&
 			rdesc[84] == 0x8c && rdesc[85] == 0x02) {
 		hid_info(hdev,
 			 "fixing up Logitech keyboard report descriptor\n");
 		rdesc[84] = rdesc[89] = 0x4d;
 		rdesc[85] = rdesc[90] = 0x10;
 	}
-	if ((quirks & LG_RDESC_REL_ABS) && *rsize >= 50 &&
+	if ((drv_data->quirks & LG_RDESC_REL_ABS) && *rsize >= 50 &&
 			rdesc[32] == 0x81 && rdesc[33] == 0x06 &&
 			rdesc[49] == 0x81 && rdesc[50] == 0x06) {
 		hid_info(hdev,
 			 "fixing up rel/abs in Logitech report descriptor\n");
 		rdesc[33] = rdesc[50] = 0x02;
 	}
-	if ((quirks & LG_FF4) && *rsize >= 101 &&
+	if ((drv_data->quirks & LG_FF4) && *rsize >= 101 &&
 			rdesc[41] == 0x95 && rdesc[42] == 0x0B &&
 			rdesc[47] == 0x05 && rdesc[48] == 0x09) {
 		hid_info(hdev, "fixing up Logitech Speed Force Wireless button descriptor\n");
@@ -278,7 +278,7 @@ static int lg_input_mapping(struct hid_device *hdev, struct hid_input *hi,
 		  0,  0,  0,  0,  0,183,184,185,186,187,
 		188,189,190,191,192,193,194,  0,  0,  0
 	};
-	unsigned long quirks = (unsigned long)hid_get_drvdata(hdev);
+	struct lg_drv_data *drv_data = (struct lg_drv_data *)hid_get_drvdata(hdev);
 	unsigned int hid = usage->hid;
 
 	if (hdev->product == USB_DEVICE_ID_LOGITECH_RECEIVER &&
@@ -289,7 +289,7 @@ static int lg_input_mapping(struct hid_device *hdev, struct hid_input *hi,
 			lg_dinovo_mapping(hi, usage, bit, max))
 		return 1;
 
-	if ((quirks & LG_WIRELESS) && lg_wireless_mapping(hi, usage, bit, max))
+	if ((drv_data->quirks & LG_WIRELESS) && lg_wireless_mapping(hi, usage, bit, max))
 		return 1;
 
 	if ((hid & HID_USAGE_PAGE) != HID_UP_BUTTON)
@@ -299,11 +299,11 @@ static int lg_input_mapping(struct hid_device *hdev, struct hid_input *hi,
 
 	/* Special handling for Logitech Cordless Desktop */
 	if (field->application == HID_GD_MOUSE) {
-		if ((quirks & LG_IGNORE_DOUBLED_WHEEL) &&
+		if ((drv_data->quirks & LG_IGNORE_DOUBLED_WHEEL) &&
 				(hid == 7 || hid == 8))
 			return -1;
 	} else {
-		if ((quirks & LG_EXPANDED_KEYMAP) &&
+		if ((drv_data->quirks & LG_EXPANDED_KEYMAP) &&
 				hid < ARRAY_SIZE(e_keymap) &&
 				e_keymap[hid] != 0) {
 			hid_map_usage(hi, usage, bit, max, EV_KEY,
@@ -319,13 +319,13 @@ static int lg_input_mapped(struct hid_device *hdev, struct hid_input *hi,
 		struct hid_field *field, struct hid_usage *usage,
 		unsigned long **bit, int *max)
 {
-	unsigned long quirks = (unsigned long)hid_get_drvdata(hdev);
+	struct lg_drv_data *drv_data = (struct lg_drv_data *)hid_get_drvdata(hdev);
 
-	if ((quirks & LG_BAD_RELATIVE_KEYS) && usage->type == EV_KEY &&
+	if ((drv_data->quirks & LG_BAD_RELATIVE_KEYS) && usage->type == EV_KEY &&
 			(field->flags & HID_MAIN_ITEM_RELATIVE))
 		field->flags &= ~HID_MAIN_ITEM_RELATIVE;
 
-	if ((quirks & LG_DUPLICATE_USAGES) && (usage->type == EV_KEY ||
+	if ((drv_data->quirks & LG_DUPLICATE_USAGES) && (usage->type == EV_KEY ||
 			 usage->type == EV_REL || usage->type == EV_ABS))
 		clear_bit(usage->code, *bit);
 
@@ -335,9 +335,9 @@ static int lg_input_mapped(struct hid_device *hdev, struct hid_input *hi,
 static int lg_event(struct hid_device *hdev, struct hid_field *field,
 		struct hid_usage *usage, __s32 value)
 {
-	unsigned long quirks = (unsigned long)hid_get_drvdata(hdev);
+	struct lg_drv_data *drv_data = (struct lg_drv_data *)hid_get_drvdata(hdev);
 
-	if ((quirks & LG_INVERT_HWHEEL) && usage->code == REL_HWHEEL) {
+	if ((drv_data->quirks & LG_INVERT_HWHEEL) && usage->code == REL_HWHEEL) {
 		input_event(field->hidinput->input, usage->type, usage->code,
 				-value);
 		return 1;
@@ -348,13 +348,20 @@ static int lg_event(struct hid_device *hdev, struct hid_field *field,
 
 static int lg_probe(struct hid_device *hdev, const struct hid_device_id *id)
 {
-	unsigned long quirks = id->driver_data;
 	unsigned int connect_mask = HID_CONNECT_DEFAULT;
+	struct lg_drv_data *drv_data;
 	int ret;
 
-	hid_set_drvdata(hdev, (void *)quirks);
+	drv_data = kzalloc(sizeof(struct lg_drv_data), GFP_KERNEL);
+	if (!drv_data) {
+		hid_err(hdev, "Insufficient memory, cannot allocate driver data\n");
+		return -ENOMEM;
+	}
+	drv_data->quirks = id->driver_data;
+	
+	hid_set_drvdata(hdev, (void *)drv_data);
 
-	if (quirks & LG_NOGET)
+	if (drv_data->quirks & LG_NOGET)
 		hdev->quirks |= HID_QUIRK_NOGET;
 
 	ret = hid_parse(hdev);
@@ -363,7 +370,7 @@ static int lg_probe(struct hid_device *hdev, const struct hid_device_id *id)
 		goto err_free;
 	}
 
-	if (quirks & (LG_FF | LG_FF2 | LG_FF3 | LG_FF4))
+	if (drv_data->quirks & (LG_FF | LG_FF2 | LG_FF3 | LG_FF4))
 		connect_mask &= ~HID_CONNECT_FF;
 
 	ret = hid_hw_start(hdev, connect_mask);
@@ -392,27 +399,29 @@ static int lg_probe(struct hid_device *hdev, const struct hid_device_id *id)
 		}
 	}
 
-	if (quirks & LG_FF)
+	if (drv_data->quirks & LG_FF)
 		lgff_init(hdev);
-	if (quirks & LG_FF2)
+	if (drv_data->quirks & LG_FF2)
 		lg2ff_init(hdev);
-	if (quirks & LG_FF3)
+	if (drv_data->quirks & LG_FF3)
 		lg3ff_init(hdev);
-	if (quirks & LG_FF4)
+	if (drv_data->quirks & LG_FF4)
 		lg4ff_init(hdev);
 
 	return 0;
 err_free:
+	kfree(drv_data);
 	return ret;
 }
 
 static void lg_remove(struct hid_device *hdev)
 {
-	unsigned long quirks = (unsigned long)hid_get_drvdata(hdev);
-	if(quirks & LG_FF4)
+	struct lg_drv_data *drv_data = (struct lg_drv_data *)hid_get_drvdata(hdev);
+	if (drv_data->quirks & LG_FF4)
 		lg4ff_deinit(hdev);
 
 	hid_hw_stop(hdev);
+	kfree(drv_data);
 }
 
 static const struct hid_device_id lg_devices[] = {
diff --git a/drivers/hid/hid-lg.h b/drivers/hid/hid-lg.h
index 4b097286dc7..d64cf8d2751 100644
--- a/drivers/hid/hid-lg.h
+++ b/drivers/hid/hid-lg.h
@@ -1,6 +1,11 @@
 #ifndef __HID_LG_H
 #define __HID_LG_H
 
+struct lg_drv_data {
+	unsigned long quirks;
+	void *device_props;	/* Device specific properties */
+};
+
 #ifdef CONFIG_LOGITECH_FF
 int lgff_init(struct hid_device *hdev);
 #else
diff --git a/drivers/hid/hid-lg4ff.c b/drivers/hid/hid-lg4ff.c
index 6ecc9e22044..f3390ee6105 100644
--- a/drivers/hid/hid-lg4ff.c
+++ b/drivers/hid/hid-lg4ff.c
@@ -1,7 +1,8 @@
 /*
- *  Force feedback support for Logitech Speed Force Wireless
+ *  Force feedback support for Logitech Gaming Wheels
  *
- *  http://wiibrew.org/wiki/Logitech_USB_steering_wheel
+ *  Including G27, G25, DFP, DFGT, FFEX, Momo, Momo2 &
+ *  Speed Force Wireless (WiiWheel)
  *
  *  Copyright (c) 2010 Simon Wood <simon@mungewell.org>
  */
@@ -51,20 +52,18 @@ static ssize_t lg4ff_range_store(struct device *dev, struct device_attribute *at
 
 static DEVICE_ATTR(range, S_IRWXU | S_IRWXG | S_IRWXO, lg4ff_range_show, lg4ff_range_store);
 
-static bool list_inited;
-
 struct lg4ff_device_entry {
-	char  *device_id;	/* Use name in respective kobject structure's address as the ID */
 	__u16 range;
 	__u16 min_range;
 	__u16 max_range;
-	__u8  leds;
+#ifdef CONFIG_LEDS_CLASS
+	__u8  led_state;
+	struct led_classdev *led[5];
+#endif
 	struct list_head list;
 	void (*set_range)(struct hid_device *hid, u16 range);
 };
 
-static struct lg4ff_device_entry device_list;
-
 static const signed short lg4ff_wheel_effects[] = {
 	FF_CONSTANT,
 	FF_AUTOCENTER,
@@ -285,18 +284,20 @@ static void hid_lg4ff_switch_native(struct hid_device *hid, const struct lg4ff_n
 /* Read current range and display it in terminal */
 static ssize_t lg4ff_range_show(struct device *dev, struct device_attribute *attr, char *buf)
 {
-	struct lg4ff_device_entry *uninitialized_var(entry);
-	struct list_head *h;
 	struct hid_device *hid = to_hid_device(dev);
+	struct lg4ff_device_entry *entry;
+	struct lg_drv_data *drv_data;
 	size_t count;
 
-	list_for_each(h, &device_list.list) {
-		entry = list_entry(h, struct lg4ff_device_entry, list);
-		if (strcmp(entry->device_id, (&hid->dev)->kobj.name) == 0)
-			break;
+	drv_data = hid_get_drvdata(hid);
+	if (!drv_data) {
+		hid_err(hid, "Private driver data not found!\n");
+		return 0;
 	}
-	if (h == &device_list.list) {
-		dbg_hid("Device not found!");
+
+	entry = drv_data->device_props;
+	if (!entry) {
+		hid_err(hid, "Device properties not found!\n");
 		return 0;
 	}
 
@@ -308,19 +309,21 @@ static ssize_t lg4ff_range_show(struct device *dev, struct device_attribute *att
  * according to the type of the wheel */
 static ssize_t lg4ff_range_store(struct device *dev, struct device_attribute *attr, const char *buf, size_t count)
 {
-	struct lg4ff_device_entry *uninitialized_var(entry);
-	struct list_head *h;
 	struct hid_device *hid = to_hid_device(dev);
+	struct lg4ff_device_entry *entry;
+	struct lg_drv_data *drv_data;
 	__u16 range = simple_strtoul(buf, NULL, 10);
 
-	list_for_each(h, &device_list.list) {
-		entry = list_entry(h, struct lg4ff_device_entry, list);
-		if (strcmp(entry->device_id, (&hid->dev)->kobj.name) == 0)
-			break;
+	drv_data = hid_get_drvdata(hid);
+	if (!drv_data) {
+		hid_err(hid, "Private driver data not found!\n");
+		return 0;
 	}
-	if (h == &device_list.list) {
-		dbg_hid("Device not found!");
-		return count;
+
+	entry = drv_data->device_props;
+	if (!entry) {
+		hid_err(hid, "Device properties not found!\n");
+		return 0;
 	}
 
 	if (range == 0)
@@ -336,6 +339,88 @@ static ssize_t lg4ff_range_store(struct device *dev, struct device_attribute *at
 	return count;
 }
 
+#ifdef CONFIG_LEDS_CLASS
+static void lg4ff_set_leds(struct hid_device *hid, __u8 leds)
+{
+	struct list_head *report_list = &hid->report_enum[HID_OUTPUT_REPORT].report_list;
+	struct hid_report *report = list_entry(report_list->next, struct hid_report, list);
+
+	report->field[0]->value[0] = 0xf8;
+	report->field[0]->value[1] = 0x12;
+	report->field[0]->value[2] = leds;
+	report->field[0]->value[3] = 0x00;
+	report->field[0]->value[4] = 0x00;
+	report->field[0]->value[5] = 0x00;
+	report->field[0]->value[6] = 0x00;
+	usbhid_submit_report(hid, report, USB_DIR_OUT);
+}
+
+static void lg4ff_led_set_brightness(struct led_classdev *led_cdev,
+			enum led_brightness value)
+{
+	struct device *dev = led_cdev->dev->parent;
+	struct hid_device *hid = container_of(dev, struct hid_device, dev);
+	struct lg_drv_data *drv_data = (struct lg_drv_data *)hid_get_drvdata(hid);
+	struct lg4ff_device_entry *entry;
+	int i, state = 0;
+
+	if (!drv_data) {
+		hid_err(hid, "Device data not found.");
+		return;
+	}
+
+	entry = (struct lg4ff_device_entry *)drv_data->device_props;
+
+	if (!entry) {
+		hid_err(hid, "Device properties not found.");
+		return;
+	}
+
+	for (i = 0; i < 5; i++) {
+		if (led_cdev != entry->led[i])
+			continue;
+		state = (entry->led_state >> i) & 1;
+		if (value == LED_OFF && state) {
+			entry->led_state &= ~(1 << i);
+			lg4ff_set_leds(hid, entry->led_state);
+		} else if (value != LED_OFF && !state) {
+			entry->led_state |= 1 << i;
+			lg4ff_set_leds(hid, entry->led_state);
+		}
+		break;
+	}
+}
+
+static enum led_brightness lg4ff_led_get_brightness(struct led_classdev *led_cdev)
+{
+	struct device *dev = led_cdev->dev->parent;
+	struct hid_device *hid = container_of(dev, struct hid_device, dev);
+	struct lg_drv_data *drv_data = (struct lg_drv_data *)hid_get_drvdata(hid);
+	struct lg4ff_device_entry *entry;
+	int i, value = 0;
+
+	if (!drv_data) {
+		hid_err(hid, "Device data not found.");
+		return LED_OFF;
+	}
+
+	entry = (struct lg4ff_device_entry *)drv_data->device_props;
+
+	if (!entry) {
+		hid_err(hid, "Device properties not found.");
+		return LED_OFF;
+	}
+
+	for (i = 0; i < 5; i++)
+		if (led_cdev == entry->led[i]) {
+			value = (entry->led_state >> i) & 1;
+			break;
+		}
+
+	return value ? LED_FULL : LED_OFF;
+}
+#endif
+
 int lg4ff_init(struct hid_device *hid)
 {
 	struct hid_input *hidinput = list_entry(hid->inputs.next, struct hid_input, list);
@@ -344,6 +429,7 @@ int lg4ff_init(struct hid_device *hid)
 	struct hid_report *report;
 	struct hid_field *field;
 	struct lg4ff_device_entry *entry;
+	struct lg_drv_data *drv_data;
 	struct usb_device_descriptor *udesc;
 	int error, i, j;
 	__u16 bcdDevice, rev_maj, rev_min;
@@ -423,28 +509,24 @@ int lg4ff_init(struct hid_device *hid)
 		dev->ff->set_autocenter(dev, 0);
 	}
 
-		/* Initialize device_list if this is the first device to handle by lg4ff */
-	if (!list_inited) {
-		INIT_LIST_HEAD(&device_list.list);
-		list_inited = 1;
+	/* Get private driver data */
+	drv_data = hid_get_drvdata(hid);
+	if (!drv_data) {
+		hid_err(hid, "Cannot add device, private driver data not allocated\n");
+		return -1;
 	}
 
-	/* Add the device to device_list */
+	/* Initialize device properties */
 	entry = kzalloc(sizeof(struct lg4ff_device_entry), GFP_KERNEL);
 	if (!entry) {
-		hid_err(hid, "Cannot add device, insufficient memory.\n");
-		return -ENOMEM;
-	}
-	entry->device_id = kstrdup((&hid->dev)->kobj.name, GFP_KERNEL);
-	if (!entry->device_id) {
-		hid_err(hid, "Cannot set device_id, insufficient memory.\n");
-		kfree(entry);
+		hid_err(hid, "Cannot add device, insufficient memory to allocate device properties.\n");
 		return -ENOMEM;
 	}
+	drv_data->device_props = entry;
+
 	entry->min_range = lg4ff_devices[i].min_range;
 	entry->max_range = lg4ff_devices[i].max_range;
 	entry->set_range = lg4ff_devices[i].set_range;
-	list_add(&entry->list, &device_list.list);
 
 	/* Create sysfs interface */
 	error = device_create_file(&hid->dev, &dev_attr_range);
@@ -457,32 +539,100 @@ int lg4ff_init(struct hid_device *hid)
 	if (entry->set_range != NULL)
 		entry->set_range(hid, entry->range);
 
-	hid_info(hid, "Force feedback for Logitech Speed Force Wireless by Simon Wood <simon@mungewell.org>\n");
+#ifdef CONFIG_LEDS_CLASS
+	/* register led subsystem - G27 only */
+	entry->led_state = 0;
+	for (j = 0; j < 5; j++)
+		entry->led[j] = NULL;
+
+	if (lg4ff_devices[i].product_id == USB_DEVICE_ID_LOGITECH_G27_WHEEL) {
+		struct led_classdev *led;
+		size_t name_sz;
+		char *name;
+
+		lg4ff_set_leds(hid, 0);
+
+		name_sz = strlen(dev_name(&hid->dev)) + 8;
+
+		for (j = 0; j < 5; j++) {
+			led = kzalloc(sizeof(struct led_classdev)+name_sz, GFP_KERNEL);
+			if (!led) {
+				hid_err(hid, "can't allocate memory for LED %d\n", j);
+				goto err;
+			}
+
+			name = (void *)(&led[1]);
+			snprintf(name, name_sz, "%s::RPM%d", dev_name(&hid->dev), j+1);
+			led->name = name;
+			led->brightness = 0;
+			led->max_brightness = 1;
+			led->brightness_get = lg4ff_led_get_brightness;
+			led->brightness_set = lg4ff_led_set_brightness;
+
+			entry->led[j] = led;
+			error = led_classdev_register(&hid->dev, led);
+
+			if (error) {
+				hid_err(hid, "failed to register LED %d. Aborting.\n", j);
+err:
+				/* Deregister LEDs (if any) */
+				for (j = 0; j < 5; j++) {
+					led = entry->led[j];
+					entry->led[j] = NULL;
+					if (!led)
+						continue;
+					led_classdev_unregister(led);
+					kfree(led);
+				}
+				goto out;	/* Let the driver continue without LEDs */
+			}
+		}
+	}
+out:
+#endif
+	hid_info(hid, "Force feedback support for Logitech Gaming Wheels\n");
 	return 0;
 }
 
 int lg4ff_deinit(struct hid_device *hid)
 {
-	bool found = 0;
 	struct lg4ff_device_entry *entry;
-	struct list_head *h, *g;
-	list_for_each_safe(h, g, &device_list.list) {
-		entry = list_entry(h, struct lg4ff_device_entry, list);
-		if (strcmp(entry->device_id, (&hid->dev)->kobj.name) == 0) {
-			list_del(h);
-			kfree(entry->device_id);
-			kfree(entry);
-			found = 1;
-			break;
-		}
-	}
+	struct lg_drv_data *drv_data;
+
+	device_remove_file(&hid->dev, &dev_attr_range);
 
-	if (!found) {
-		dbg_hid("Device entry not found!\n");
+	drv_data = hid_get_drvdata(hid);
+	if (!drv_data) {
+		hid_err(hid, "Error while deinitializing device, no private driver data.\n");
+		return -1;
+	}
+	entry = drv_data->device_props;
+	if (!entry) {
+		hid_err(hid, "Error while deinitializing device, no device properties data.\n");
 		return -1;
 	}
 
-	device_remove_file(&hid->dev, &dev_attr_range);
+#ifdef CONFIG_LEDS_CLASS
+	{
+		int j;
+		struct led_classdev *led;
+
+		/* Deregister LEDs (if any) */
+		for (j = 0; j < 5; j++) {
+
+			led = entry->led[j];
+			entry->led[j] = NULL;
+			if (!led)
+				continue;
+			led_classdev_unregister(led);
+			kfree(led);
+		}
+	}
+#endif
+
+	/* Deallocate memory */
+	kfree(entry);
+
 	dbg_hid("Device successfully unregistered\n");
 	return 0;
 }
diff --git a/drivers/hid/hid-logitech-dj.c b/drivers/hid/hid-logitech-dj.c
index 2b56efcbdf6..5e8a7ed4234 100644
--- a/drivers/hid/hid-logitech-dj.c
+++ b/drivers/hid/hid-logitech-dj.c
@@ -26,6 +26,7 @@
 #include <linux/hid.h>
 #include <linux/module.h>
 #include <linux/usb.h>
+#include <asm/unaligned.h>
 #include "usbhid/usbhid.h"
 #include "hid-ids.h"
 #include "hid-logitech-dj.h"
@@ -155,6 +156,14 @@ static const char media_descriptor[] = {
 /* Maximum size of all defined hid reports in bytes (including report id) */
 #define MAX_REPORT_SIZE 8
 
+/* Make sure all descriptors are present here */
+#define MAX_RDESC_SIZE				\
+	(sizeof(kbd_descriptor) +		\
+	 sizeof(mse_descriptor) +		\
+	 sizeof(consumer_descriptor) +		\
+	 sizeof(syscontrol_descriptor) +	\
+	 sizeof(media_descriptor))
+
 /* Number of possible hid report types that can be created by this driver.
  *
  * Right now, RF report types have the same report types (or report id's)
@@ -265,8 +274,8 @@ static void logi_dj_recv_add_djhid_device(struct dj_receiver_dev *djrcv_dev,
 		goto dj_device_allocate_fail;
 	}
 
-	dj_dev->reports_supported = le32_to_cpu(
-		dj_report->report_params[DEVICE_PAIRED_RF_REPORT_TYPE]);
+	dj_dev->reports_supported = get_unaligned_le32(
+		dj_report->report_params + DEVICE_PAIRED_RF_REPORT_TYPE);
 	dj_dev->hdev = dj_hiddev;
 	dj_dev->dj_receiver_dev = djrcv_dev;
 	dj_dev->device_index = dj_report->device_index;
@@ -473,9 +482,17 @@ static int logi_dj_output_hidraw_report(struct hid_device *hid, u8 * buf,
 	return 0;
 }
 
+static void rdcat(char **rdesc, unsigned int *rsize, const char *data, unsigned int size)
+{
+	memcpy(*rdesc + *rsize, data, size);
+	*rsize += size;
+}
+
 static int logi_dj_ll_parse(struct hid_device *hid)
 {
 	struct dj_device *djdev = hid->driver_data;
+	unsigned int rsize = 0;
+	char *rdesc;
 	int retval;
 
 	dbg_hid("%s\n", __func__);
@@ -483,70 +500,38 @@ static int logi_dj_ll_parse(struct hid_device *hid)
 	djdev->hdev->version = 0x0111;
 	djdev->hdev->country = 0x00;
 
+	rdesc = kmalloc(MAX_RDESC_SIZE, GFP_KERNEL);
+	if (!rdesc)
+		return -ENOMEM;
+
 	if (djdev->reports_supported & STD_KEYBOARD) {
 		dbg_hid("%s: sending a kbd descriptor, reports_supported: %x\n",
 			__func__, djdev->reports_supported);
-		retval = hid_parse_report(hid,
-					  (u8 *) kbd_descriptor,
-					  sizeof(kbd_descriptor));
-		if (retval) {
-			dbg_hid("%s: sending a kbd descriptor, hid_parse failed"
-				" error: %d\n", __func__, retval);
-			return retval;
-		}
+		rdcat(&rdesc, &rsize, kbd_descriptor, sizeof(kbd_descriptor));
 	}
 
 	if (djdev->reports_supported & STD_MOUSE) {
 		dbg_hid("%s: sending a mouse descriptor, reports_supported: "
 			"%x\n", __func__, djdev->reports_supported);
-		retval = hid_parse_report(hid,
-					  (u8 *) mse_descriptor,
-					  sizeof(mse_descriptor));
-		if (retval) {
-			dbg_hid("%s: sending a mouse descriptor, hid_parse "
-				"failed error: %d\n", __func__, retval);
-			return retval;
-		}
+		rdcat(&rdesc, &rsize, mse_descriptor, sizeof(mse_descriptor));
 	}
 
 	if (djdev->reports_supported & MULTIMEDIA) {
 		dbg_hid("%s: sending a multimedia report descriptor: %x\n",
 			__func__, djdev->reports_supported);
-		retval = hid_parse_report(hid,
-					  (u8 *) consumer_descriptor,
-					  sizeof(consumer_descriptor));
-		if (retval) {
-			dbg_hid("%s: sending a consumer_descriptor, hid_parse "
-				"failed error: %d\n", __func__, retval);
-			return retval;
-		}
+		rdcat(&rdesc, &rsize, consumer_descriptor, sizeof(consumer_descriptor));
 	}
 
 	if (djdev->reports_supported & POWER_KEYS) {
 		dbg_hid("%s: sending a power keys report descriptor: %x\n",
 			__func__, djdev->reports_supported);
-		retval = hid_parse_report(hid,
-					  (u8 *) syscontrol_descriptor,
-					  sizeof(syscontrol_descriptor));
-		if (retval) {
-			dbg_hid("%s: sending a syscontrol_descriptor, "
-				"hid_parse failed error: %d\n",
-				__func__, retval);
-			return retval;
-		}
+		rdcat(&rdesc, &rsize, syscontrol_descriptor, sizeof(syscontrol_descriptor));
 	}
 
 	if (djdev->reports_supported & MEDIA_CENTER) {
 		dbg_hid("%s: sending a media center report descriptor: %x\n",
 			__func__, djdev->reports_supported);
-		retval = hid_parse_report(hid,
-					  (u8 *) media_descriptor,
-					  sizeof(media_descriptor));
-		if (retval) {
-			dbg_hid("%s: sending a media_descriptor, hid_parse "
-				"failed error: %d\n", __func__, retval);
-			return retval;
-		}
+		rdcat(&rdesc, &rsize, media_descriptor, sizeof(media_descriptor));
 	}
 
 	if (djdev->reports_supported & KBD_LEDS) {
@@ -554,7 +539,10 @@ static int logi_dj_ll_parse(struct hid_device *hid)
 			__func__, djdev->reports_supported);
 	}
 
-	return 0;
+	retval = hid_parse_report(hid, rdesc, rsize);
+	kfree(rdesc);
+
+	return retval;
 }
 
 static int logi_dj_ll_input_event(struct input_dev *dev, unsigned int type,
diff --git a/drivers/hid/hid-multitouch.c b/drivers/hid/hid-multitouch.c
index 1d5b94167b5..6e3332a9997 100644
--- a/drivers/hid/hid-multitouch.c
+++ b/drivers/hid/hid-multitouch.c
@@ -70,9 +70,16 @@ struct mt_class {
 	bool is_indirect;	/* true for touchpads */
 };
 
+struct mt_fields {
+	unsigned usages[HID_MAX_FIELDS];
+	unsigned int length;
+};
+
 struct mt_device {
 	struct mt_slot curdata;	/* placeholder of incoming data */
 	struct mt_class mtclass;	/* our mt device class */
+	struct mt_fields *fields;	/* temporary placeholder for storing the
+					   multitouch fields */
 	unsigned last_field_index;	/* last field index of the report */
 	unsigned last_slot_field;	/* the last field of a slot */
 	__s8 inputmode;		/* InputMode HID feature, -1 if non-existent */
@@ -110,6 +117,9 @@ struct mt_device {
 
 #define MT_DEFAULT_MAXCONTACT	10
 
+#define MT_USB_DEVICE(v, p)	HID_DEVICE(BUS_USB, HID_GROUP_MULTITOUCH, v, p)
+#define MT_BT_DEVICE(v, p)	HID_DEVICE(BUS_BLUETOOTH, HID_GROUP_MULTITOUCH, v, p)
+
 /*
  * these device-dependent functions determine what slot corresponds
  * to a valid contact that was just read.
@@ -275,11 +285,15 @@ static void set_abs(struct input_dev *input, unsigned int code,
 	input_set_abs_params(input, code, fmin, fmax, fuzz, 0);
 }
 
-static void set_last_slot_field(struct hid_usage *usage, struct mt_device *td,
+static void mt_store_field(struct hid_usage *usage, struct mt_device *td,
 		struct hid_input *hi)
 {
-	if (!test_bit(usage->hid, hi->input->absbit))
-		td->last_slot_field = usage->hid;
+	struct mt_fields *f = td->fields;
+
+	if (f->length >= HID_MAX_FIELDS)
+		return;
+
+	f->usages[f->length++] = usage->hid;
 }
 
 static int mt_input_mapping(struct hid_device *hdev, struct hid_input *hi,
@@ -330,7 +344,7 @@ static int mt_input_mapping(struct hid_device *hdev, struct hid_input *hi,
 				cls->sn_move);
 			/* touchscreen emulation */
 			set_abs(hi->input, ABS_X, field, cls->sn_move);
-			set_last_slot_field(usage, td, hi);
+			mt_store_field(usage, td, hi);
 			td->last_field_index = field->index;
 			return 1;
 		case HID_GD_Y:
@@ -340,7 +354,7 @@ static int mt_input_mapping(struct hid_device *hdev, struct hid_input *hi,
 				cls->sn_move);
 			/* touchscreen emulation */
 			set_abs(hi->input, ABS_Y, field, cls->sn_move);
-			set_last_slot_field(usage, td, hi);
+			mt_store_field(usage, td, hi);
 			td->last_field_index = field->index;
 			return 1;
 		}
@@ -349,24 +363,24 @@ static int mt_input_mapping(struct hid_device *hdev, struct hid_input *hi,
 	case HID_UP_DIGITIZER:
 		switch (usage->hid) {
 		case HID_DG_INRANGE:
-			set_last_slot_field(usage, td, hi);
+			mt_store_field(usage, td, hi);
 			td->last_field_index = field->index;
 			return 1;
 		case HID_DG_CONFIDENCE:
-			set_last_slot_field(usage, td, hi);
+			mt_store_field(usage, td, hi);
 			td->last_field_index = field->index;
 			return 1;
 		case HID_DG_TIPSWITCH:
 			hid_map_usage(hi, usage, bit, max, EV_KEY, BTN_TOUCH);
 			input_set_capability(hi->input, EV_KEY, BTN_TOUCH);
-			set_last_slot_field(usage, td, hi);
+			mt_store_field(usage, td, hi);
 			td->last_field_index = field->index;
 			return 1;
 		case HID_DG_CONTACTID:
 			if (!td->maxcontacts)
 				td->maxcontacts = MT_DEFAULT_MAXCONTACT;
 			input_mt_init_slots(hi->input, td->maxcontacts);
-			td->last_slot_field = usage->hid;
+			mt_store_field(usage, td, hi);
 			td->last_field_index = field->index;
 			td->touches_by_report++;
 			return 1;
@@ -375,7 +389,7 @@ static int mt_input_mapping(struct hid_device *hdev, struct hid_input *hi,
 					EV_ABS, ABS_MT_TOUCH_MAJOR);
 			set_abs(hi->input, ABS_MT_TOUCH_MAJOR, field,
 				cls->sn_width);
-			set_last_slot_field(usage, td, hi);
+			mt_store_field(usage, td, hi);
 			td->last_field_index = field->index;
 			return 1;
 		case HID_DG_HEIGHT:
@@ -385,7 +399,7 @@ static int mt_input_mapping(struct hid_device *hdev, struct hid_input *hi,
 				cls->sn_height);
 			input_set_abs_params(hi->input,
 					ABS_MT_ORIENTATION, 0, 1, 0, 0);
-			set_last_slot_field(usage, td, hi);
+			mt_store_field(usage, td, hi);
 			td->last_field_index = field->index;
 			return 1;
 		case HID_DG_TIPPRESSURE:
@@ -396,7 +410,7 @@ static int mt_input_mapping(struct hid_device *hdev, struct hid_input *hi,
 			/* touchscreen emulation */
 			set_abs(hi->input, ABS_PRESSURE, field,
 				cls->sn_pressure);
-			set_last_slot_field(usage, td, hi);
+			mt_store_field(usage, td, hi);
 			td->last_field_index = field->index;
 			return 1;
 		case HID_DG_CONTACTCOUNT:
@@ -635,6 +649,31 @@ static void mt_set_maxcontacts(struct hid_device *hdev)
 	}
 }
 
+static void mt_post_parse_default_settings(struct mt_device *td)
+{
+	__s32 quirks = td->mtclass.quirks;
+
+	/* unknown serial device needs special quirks */
+	if (td->touches_by_report == 1) {
+		quirks |= MT_QUIRK_ALWAYS_VALID;
+		quirks &= ~MT_QUIRK_NOT_SEEN_MEANS_UP;
+		quirks &= ~MT_QUIRK_VALID_IS_INRANGE;
+		quirks &= ~MT_QUIRK_VALID_IS_CONFIDENCE;
+	}
+
+	td->mtclass.quirks = quirks;
+}
+
+static void mt_post_parse(struct mt_device *td)
+{
+	struct mt_fields *f = td->fields;
+
+	if (td->touches_by_report > 0) {
+		int field_count_per_touch = f->length / td->touches_by_report;
+		td->last_slot_field = f->usages[field_count_per_touch - 1];
+	}
+}
+
 static int mt_probe(struct hid_device *hdev, const struct hid_device_id *id)
 {
 	int ret, i;
@@ -654,7 +693,6 @@ static int mt_probe(struct hid_device *hdev, const struct hid_device_id *id)
 	 * that emit events over several HID messages.
 	 */
 	hdev->quirks |= HID_QUIRK_NO_INPUT_SYNC;
-	hdev->quirks &= ~HID_QUIRK_MULTITOUCH;
 
 	td = kzalloc(sizeof(struct mt_device), GFP_KERNEL);
 	if (!td) {
@@ -666,6 +704,13 @@ static int mt_probe(struct hid_device *hdev, const struct hid_device_id *id)
 	td->maxcontact_report_id = -1;
 	hid_set_drvdata(hdev, td);
 
+	td->fields = kzalloc(sizeof(struct mt_fields), GFP_KERNEL);
+	if (!td->fields) {
+		dev_err(&hdev->dev, "cannot allocate multitouch fields data\n");
+		ret = -ENOMEM;
+		goto fail;
+	}
+
 	ret = hid_parse(hdev);
 	if (ret != 0)
 		goto fail;
@@ -674,14 +719,10 @@ static int mt_probe(struct hid_device *hdev, const struct hid_device_id *id)
 	if (ret)
 		goto fail;
 
-	if (!id && td->touches_by_report == 1) {
-		/* the device has been sent by hid-generic */
-		mtclass = &td->mtclass;
-		mtclass->quirks |= MT_QUIRK_ALWAYS_VALID;
-		mtclass->quirks &= ~MT_QUIRK_NOT_SEEN_MEANS_UP;
-		mtclass->quirks &= ~MT_QUIRK_VALID_IS_INRANGE;
-		mtclass->quirks &= ~MT_QUIRK_VALID_IS_CONFIDENCE;
-	}
+	mt_post_parse(td);
+
+	if (id->vendor == HID_ANY_ID && id->product == HID_ANY_ID)
+		mt_post_parse_default_settings(td);
 
 	td->slots = kzalloc(td->maxcontacts * sizeof(struct mt_slot),
 				GFP_KERNEL);
@@ -697,9 +738,13 @@ static int mt_probe(struct hid_device *hdev, const struct hid_device_id *id)
 	mt_set_maxcontacts(hdev);
 	mt_set_input_mode(hdev);
 
+	kfree(td->fields);
+	td->fields = NULL;
+
 	return 0;
 
 fail:
+	kfree(td->fields);
 	kfree(td);
 	return ret;
 }
@@ -727,50 +772,54 @@ static const struct hid_device_id mt_devices[] = {
 
 	/* 3M panels */
 	{ .driver_data = MT_CLS_3M,
-		HID_USB_DEVICE(USB_VENDOR_ID_3M,
+		MT_USB_DEVICE(USB_VENDOR_ID_3M,
 			USB_DEVICE_ID_3M1968) },
 	{ .driver_data = MT_CLS_3M,
-		HID_USB_DEVICE(USB_VENDOR_ID_3M,
+		MT_USB_DEVICE(USB_VENDOR_ID_3M,
 			USB_DEVICE_ID_3M2256) },
 	{ .driver_data = MT_CLS_3M,
-		HID_USB_DEVICE(USB_VENDOR_ID_3M,
+		MT_USB_DEVICE(USB_VENDOR_ID_3M,
 			USB_DEVICE_ID_3M3266) },
 
 	/* ActionStar panels */
 	{ .driver_data = MT_CLS_DEFAULT,
-		HID_USB_DEVICE(USB_VENDOR_ID_ACTIONSTAR,
+		MT_USB_DEVICE(USB_VENDOR_ID_ACTIONSTAR,
 			USB_DEVICE_ID_ACTIONSTAR_1011) },
 
 	/* Atmel panels */
 	{ .driver_data = MT_CLS_SERIAL,
-		HID_USB_DEVICE(USB_VENDOR_ID_ATMEL,
+		MT_USB_DEVICE(USB_VENDOR_ID_ATMEL,
 			USB_DEVICE_ID_ATMEL_MULTITOUCH) },
 	{ .driver_data = MT_CLS_SERIAL,
-		HID_USB_DEVICE(USB_VENDOR_ID_ATMEL,
+		MT_USB_DEVICE(USB_VENDOR_ID_ATMEL,
 			USB_DEVICE_ID_ATMEL_MXT_DIGITIZER) },
 
+	/* Baanto multitouch devices */
+	{ .driver_data = MT_CLS_DEFAULT,
+		MT_USB_DEVICE(USB_VENDOR_ID_BAANTO,
+			USB_DEVICE_ID_BAANTO_MT_190W2) },
 	/* Cando panels */
 	{ .driver_data = MT_CLS_DUAL_INRANGE_CONTACTNUMBER,
-		HID_USB_DEVICE(USB_VENDOR_ID_CANDO,
+		MT_USB_DEVICE(USB_VENDOR_ID_CANDO,
 			USB_DEVICE_ID_CANDO_MULTI_TOUCH) },
 	{ .driver_data = MT_CLS_DUAL_INRANGE_CONTACTNUMBER,
-		HID_USB_DEVICE(USB_VENDOR_ID_CANDO,
+		MT_USB_DEVICE(USB_VENDOR_ID_CANDO,
 			USB_DEVICE_ID_CANDO_MULTI_TOUCH_10_1) },
 	{ .driver_data = MT_CLS_DUAL_INRANGE_CONTACTNUMBER,
-		HID_USB_DEVICE(USB_VENDOR_ID_CANDO,
+		MT_USB_DEVICE(USB_VENDOR_ID_CANDO,
 			USB_DEVICE_ID_CANDO_MULTI_TOUCH_11_6) },
 	{ .driver_data = MT_CLS_DUAL_INRANGE_CONTACTNUMBER,
-		HID_USB_DEVICE(USB_VENDOR_ID_CANDO,
+		MT_USB_DEVICE(USB_VENDOR_ID_CANDO,
 			USB_DEVICE_ID_CANDO_MULTI_TOUCH_15_6) },
 
 	/* Chunghwa Telecom touch panels */
 	{  .driver_data = MT_CLS_DEFAULT,
-		HID_USB_DEVICE(USB_VENDOR_ID_CHUNGHWAT,
+		MT_USB_DEVICE(USB_VENDOR_ID_CHUNGHWAT,
 			USB_DEVICE_ID_CHUNGHWAT_MULTITOUCH) },
 
 	/* CVTouch panels */
 	{ .driver_data = MT_CLS_DEFAULT,
-		HID_USB_DEVICE(USB_VENDOR_ID_CVTOUCH,
+		MT_USB_DEVICE(USB_VENDOR_ID_CVTOUCH,
 			USB_DEVICE_ID_CVTOUCH_SCREEN) },
 
 	/* Cypress panel */
@@ -780,225 +829,227 @@ static const struct hid_device_id mt_devices[] = {
 
 	/* eGalax devices (resistive) */
 	{ .driver_data = MT_CLS_EGALAX,
-		HID_USB_DEVICE(USB_VENDOR_ID_DWAV,
+		MT_USB_DEVICE(USB_VENDOR_ID_DWAV,
 			USB_DEVICE_ID_DWAV_EGALAX_MULTITOUCH_480D) },
 	{ .driver_data = MT_CLS_EGALAX,
-		HID_USB_DEVICE(USB_VENDOR_ID_DWAV,
+		MT_USB_DEVICE(USB_VENDOR_ID_DWAV,
 			USB_DEVICE_ID_DWAV_EGALAX_MULTITOUCH_480E) },
 
 	/* eGalax devices (capacitive) */
 	{ .driver_data = MT_CLS_EGALAX,
-		HID_USB_DEVICE(USB_VENDOR_ID_DWAV,
+		MT_USB_DEVICE(USB_VENDOR_ID_DWAV,
 			USB_DEVICE_ID_DWAV_EGALAX_MULTITOUCH_720C) },
 	{ .driver_data = MT_CLS_EGALAX_SERIAL,
-		HID_USB_DEVICE(USB_VENDOR_ID_DWAV,
+		MT_USB_DEVICE(USB_VENDOR_ID_DWAV,
 			USB_DEVICE_ID_DWAV_EGALAX_MULTITOUCH_7207) },
 	{ .driver_data = MT_CLS_EGALAX_SERIAL,
-		HID_USB_DEVICE(USB_VENDOR_ID_DWAV,
+		MT_USB_DEVICE(USB_VENDOR_ID_DWAV,
 			USB_DEVICE_ID_DWAV_EGALAX_MULTITOUCH_725E) },
 	{ .driver_data = MT_CLS_EGALAX_SERIAL,
-		HID_USB_DEVICE(USB_VENDOR_ID_DWAV,
+		MT_USB_DEVICE(USB_VENDOR_ID_DWAV,
 			USB_DEVICE_ID_DWAV_EGALAX_MULTITOUCH_7224) },
 	{ .driver_data = MT_CLS_EGALAX_SERIAL,
-		HID_USB_DEVICE(USB_VENDOR_ID_DWAV,
+		MT_USB_DEVICE(USB_VENDOR_ID_DWAV,
 			USB_DEVICE_ID_DWAV_EGALAX_MULTITOUCH_722A) },
 	{ .driver_data = MT_CLS_EGALAX,
-		HID_USB_DEVICE(USB_VENDOR_ID_DWAV,
+		MT_USB_DEVICE(USB_VENDOR_ID_DWAV,
 			USB_DEVICE_ID_DWAV_EGALAX_MULTITOUCH_726B) },
 	{ .driver_data = MT_CLS_EGALAX_SERIAL,
-		HID_USB_DEVICE(USB_VENDOR_ID_DWAV,
+		MT_USB_DEVICE(USB_VENDOR_ID_DWAV,
 			USB_DEVICE_ID_DWAV_EGALAX_MULTITOUCH_7262) },
 	{ .driver_data = MT_CLS_EGALAX,
-		HID_USB_DEVICE(USB_VENDOR_ID_DWAV,
+		MT_USB_DEVICE(USB_VENDOR_ID_DWAV,
 			USB_DEVICE_ID_DWAV_EGALAX_MULTITOUCH_72A1) },
 	{ .driver_data = MT_CLS_EGALAX_SERIAL,
-		HID_USB_DEVICE(USB_VENDOR_ID_DWAV,
+		MT_USB_DEVICE(USB_VENDOR_ID_DWAV,
 			USB_DEVICE_ID_DWAV_EGALAX_MULTITOUCH_72AA) },
 	{ .driver_data = MT_CLS_EGALAX,
-		HID_USB_DEVICE(USB_VENDOR_ID_DWAV,
+		MT_USB_DEVICE(USB_VENDOR_ID_DWAV,
 			USB_DEVICE_ID_DWAV_EGALAX_MULTITOUCH_72FA) },
 	{ .driver_data = MT_CLS_EGALAX,
-		HID_USB_DEVICE(USB_VENDOR_ID_DWAV,
+		MT_USB_DEVICE(USB_VENDOR_ID_DWAV,
 			USB_DEVICE_ID_DWAV_EGALAX_MULTITOUCH_7302) },
 	{ .driver_data = MT_CLS_EGALAX_SERIAL,
-		HID_USB_DEVICE(USB_VENDOR_ID_DWAV,
+		MT_USB_DEVICE(USB_VENDOR_ID_DWAV,
 			USB_DEVICE_ID_DWAV_EGALAX_MULTITOUCH_7349) },
 	{ .driver_data = MT_CLS_EGALAX_SERIAL,
-		HID_USB_DEVICE(USB_VENDOR_ID_DWAV,
+		MT_USB_DEVICE(USB_VENDOR_ID_DWAV,
 			USB_DEVICE_ID_DWAV_EGALAX_MULTITOUCH_A001) },
 
 	/* Elo TouchSystems IntelliTouch Plus panel */
 	{ .driver_data = MT_CLS_DUAL_NSMU_CONTACTID,
-		HID_USB_DEVICE(USB_VENDOR_ID_ELO,
+		MT_USB_DEVICE(USB_VENDOR_ID_ELO,
 			USB_DEVICE_ID_ELO_TS2515) },
 
 	/* GeneralTouch panel */
 	{ .driver_data = MT_CLS_DUAL_INRANGE_CONTACTNUMBER,
-		HID_USB_DEVICE(USB_VENDOR_ID_GENERAL_TOUCH,
+		MT_USB_DEVICE(USB_VENDOR_ID_GENERAL_TOUCH,
 			USB_DEVICE_ID_GENERAL_TOUCH_WIN7_TWOFINGERS) },
 
 	/* Gametel game controller */
 	{ .driver_data = MT_CLS_DEFAULT,
-		HID_BLUETOOTH_DEVICE(USB_VENDOR_ID_FRUCTEL,
+		MT_BT_DEVICE(USB_VENDOR_ID_FRUCTEL,
 			USB_DEVICE_ID_GAMETEL_MT_MODE) },
 
 	/* GoodTouch panels */
 	{ .driver_data = MT_CLS_DEFAULT,
-		HID_USB_DEVICE(USB_VENDOR_ID_GOODTOUCH,
+		MT_USB_DEVICE(USB_VENDOR_ID_GOODTOUCH,
 			USB_DEVICE_ID_GOODTOUCH_000f) },
 
 	/* Hanvon panels */
 	{ .driver_data = MT_CLS_DUAL_INRANGE_CONTACTID,
-		HID_USB_DEVICE(USB_VENDOR_ID_HANVON_ALT,
+		MT_USB_DEVICE(USB_VENDOR_ID_HANVON_ALT,
 			USB_DEVICE_ID_HANVON_ALT_MULTITOUCH) },
 
 	/* Ideacom panel */
 	{ .driver_data = MT_CLS_SERIAL,
-		HID_USB_DEVICE(USB_VENDOR_ID_IDEACOM,
+		MT_USB_DEVICE(USB_VENDOR_ID_IDEACOM,
 			USB_DEVICE_ID_IDEACOM_IDC6650) },
 	{ .driver_data = MT_CLS_SERIAL,
-		HID_USB_DEVICE(USB_VENDOR_ID_IDEACOM,
+		MT_USB_DEVICE(USB_VENDOR_ID_IDEACOM,
 			USB_DEVICE_ID_IDEACOM_IDC6651) },
 
 	/* Ilitek dual touch panel */
 	{  .driver_data = MT_CLS_DEFAULT,
-		HID_USB_DEVICE(USB_VENDOR_ID_ILITEK,
+		MT_USB_DEVICE(USB_VENDOR_ID_ILITEK,
 			USB_DEVICE_ID_ILITEK_MULTITOUCH) },
 
 	/* IRTOUCH panels */
 	{ .driver_data = MT_CLS_DUAL_INRANGE_CONTACTID,
-		HID_USB_DEVICE(USB_VENDOR_ID_IRTOUCHSYSTEMS,
+		MT_USB_DEVICE(USB_VENDOR_ID_IRTOUCHSYSTEMS,
 			USB_DEVICE_ID_IRTOUCH_INFRARED_USB) },
 
 	/* LG Display panels */
 	{ .driver_data = MT_CLS_DEFAULT,
-		HID_USB_DEVICE(USB_VENDOR_ID_LG,
+		MT_USB_DEVICE(USB_VENDOR_ID_LG,
 			USB_DEVICE_ID_LG_MULTITOUCH) },
 
 	/* Lumio panels */
 	{ .driver_data = MT_CLS_CONFIDENCE_MINUS_ONE,
-		HID_USB_DEVICE(USB_VENDOR_ID_LUMIO,
+		MT_USB_DEVICE(USB_VENDOR_ID_LUMIO,
 			USB_DEVICE_ID_CRYSTALTOUCH) },
 	{ .driver_data = MT_CLS_CONFIDENCE_MINUS_ONE,
-		HID_USB_DEVICE(USB_VENDOR_ID_LUMIO,
+		MT_USB_DEVICE(USB_VENDOR_ID_LUMIO,
 			USB_DEVICE_ID_CRYSTALTOUCH_DUAL) },
 
 	/* MosArt panels */
 	{ .driver_data = MT_CLS_CONFIDENCE_MINUS_ONE,
-		HID_USB_DEVICE(USB_VENDOR_ID_ASUS,
+		MT_USB_DEVICE(USB_VENDOR_ID_ASUS,
 			USB_DEVICE_ID_ASUS_T91MT)},
 	{ .driver_data = MT_CLS_CONFIDENCE_MINUS_ONE,
-		HID_USB_DEVICE(USB_VENDOR_ID_ASUS,
+		MT_USB_DEVICE(USB_VENDOR_ID_ASUS,
 			USB_DEVICE_ID_ASUSTEK_MULTITOUCH_YFO) },
 	{ .driver_data = MT_CLS_CONFIDENCE_MINUS_ONE,
-		HID_USB_DEVICE(USB_VENDOR_ID_TURBOX,
+		MT_USB_DEVICE(USB_VENDOR_ID_TURBOX,
 			USB_DEVICE_ID_TURBOX_TOUCHSCREEN_MOSART) },
 
 	/* Panasonic panels */
 	{ .driver_data = MT_CLS_PANASONIC,
-		HID_USB_DEVICE(USB_VENDOR_ID_PANASONIC,
+		MT_USB_DEVICE(USB_VENDOR_ID_PANASONIC,
 			USB_DEVICE_ID_PANABOARD_UBT780) },
 	{ .driver_data = MT_CLS_PANASONIC,
-		HID_USB_DEVICE(USB_VENDOR_ID_PANASONIC,
+		MT_USB_DEVICE(USB_VENDOR_ID_PANASONIC,
 			USB_DEVICE_ID_PANABOARD_UBT880) },
 
 	/* PenMount panels */
 	{ .driver_data = MT_CLS_CONFIDENCE,
-		HID_USB_DEVICE(USB_VENDOR_ID_PENMOUNT,
+		MT_USB_DEVICE(USB_VENDOR_ID_PENMOUNT,
 			USB_DEVICE_ID_PENMOUNT_PCI) },
 
 	/* PixArt optical touch screen */
 	{ .driver_data = MT_CLS_INRANGE_CONTACTNUMBER,
-		HID_USB_DEVICE(USB_VENDOR_ID_PIXART,
+		MT_USB_DEVICE(USB_VENDOR_ID_PIXART,
 			USB_DEVICE_ID_PIXART_OPTICAL_TOUCH_SCREEN) },
 	{ .driver_data = MT_CLS_INRANGE_CONTACTNUMBER,
-		HID_USB_DEVICE(USB_VENDOR_ID_PIXART,
+		MT_USB_DEVICE(USB_VENDOR_ID_PIXART,
 			USB_DEVICE_ID_PIXART_OPTICAL_TOUCH_SCREEN1) },
 	{ .driver_data = MT_CLS_INRANGE_CONTACTNUMBER,
-		HID_USB_DEVICE(USB_VENDOR_ID_PIXART,
+		MT_USB_DEVICE(USB_VENDOR_ID_PIXART,
 			USB_DEVICE_ID_PIXART_OPTICAL_TOUCH_SCREEN2) },
 
 	/* PixCir-based panels */
 	{ .driver_data = MT_CLS_DUAL_INRANGE_CONTACTID,
-		HID_USB_DEVICE(USB_VENDOR_ID_HANVON,
+		MT_USB_DEVICE(USB_VENDOR_ID_HANVON,
 			USB_DEVICE_ID_HANVON_MULTITOUCH) },
 	{ .driver_data = MT_CLS_DUAL_INRANGE_CONTACTID,
-		HID_USB_DEVICE(USB_VENDOR_ID_CANDO,
+		MT_USB_DEVICE(USB_VENDOR_ID_CANDO,
 			USB_DEVICE_ID_CANDO_PIXCIR_MULTI_TOUCH) },
 
 	/* Quanta-based panels */
 	{ .driver_data = MT_CLS_CONFIDENCE_CONTACT_ID,
-		HID_USB_DEVICE(USB_VENDOR_ID_QUANTA,
+		MT_USB_DEVICE(USB_VENDOR_ID_QUANTA,
 			USB_DEVICE_ID_QUANTA_OPTICAL_TOUCH) },
 	{ .driver_data = MT_CLS_CONFIDENCE_CONTACT_ID,
-		HID_USB_DEVICE(USB_VENDOR_ID_QUANTA,
+		MT_USB_DEVICE(USB_VENDOR_ID_QUANTA,
 			USB_DEVICE_ID_QUANTA_OPTICAL_TOUCH_3001) },
 	{ .driver_data = MT_CLS_CONFIDENCE_CONTACT_ID,
-		HID_USB_DEVICE(USB_VENDOR_ID_QUANTA,
+		MT_USB_DEVICE(USB_VENDOR_ID_QUANTA,
 			USB_DEVICE_ID_QUANTA_OPTICAL_TOUCH_3008) },
 
 	/* Stantum panels */
 	{ .driver_data = MT_CLS_CONFIDENCE,
-		HID_USB_DEVICE(USB_VENDOR_ID_STANTUM,
+		MT_USB_DEVICE(USB_VENDOR_ID_STANTUM,
 			USB_DEVICE_ID_MTP)},
 	{ .driver_data = MT_CLS_CONFIDENCE,
-		HID_USB_DEVICE(USB_VENDOR_ID_STANTUM_STM,
+		MT_USB_DEVICE(USB_VENDOR_ID_STANTUM_STM,
 			USB_DEVICE_ID_MTP_STM)},
 	{ .driver_data = MT_CLS_CONFIDENCE,
-		HID_USB_DEVICE(USB_VENDOR_ID_STANTUM_SITRONIX,
+		MT_USB_DEVICE(USB_VENDOR_ID_STANTUM_SITRONIX,
 			USB_DEVICE_ID_MTP_SITRONIX)},
 
 	/* TopSeed panels */
 	{ .driver_data = MT_CLS_TOPSEED,
-		HID_USB_DEVICE(USB_VENDOR_ID_TOPSEED2,
+		MT_USB_DEVICE(USB_VENDOR_ID_TOPSEED2,
 			USB_DEVICE_ID_TOPSEED2_PERIPAD_701) },
 
 	/* Touch International panels */
 	{ .driver_data = MT_CLS_DEFAULT,
-		HID_USB_DEVICE(USB_VENDOR_ID_TOUCH_INTL,
+		MT_USB_DEVICE(USB_VENDOR_ID_TOUCH_INTL,
 			USB_DEVICE_ID_TOUCH_INTL_MULTI_TOUCH) },
 
 	/* Unitec panels */
 	{ .driver_data = MT_CLS_DEFAULT,
-		HID_USB_DEVICE(USB_VENDOR_ID_UNITEC,
+		MT_USB_DEVICE(USB_VENDOR_ID_UNITEC,
 			USB_DEVICE_ID_UNITEC_USB_TOUCH_0709) },
 	{ .driver_data = MT_CLS_DEFAULT,
-		HID_USB_DEVICE(USB_VENDOR_ID_UNITEC,
+		MT_USB_DEVICE(USB_VENDOR_ID_UNITEC,
 			USB_DEVICE_ID_UNITEC_USB_TOUCH_0A19) },
 	/* XAT */
 	{ .driver_data = MT_CLS_DEFAULT,
-		HID_USB_DEVICE(USB_VENDOR_ID_XAT,
+		MT_USB_DEVICE(USB_VENDOR_ID_XAT,
 			USB_DEVICE_ID_XAT_CSR) },
 
 	/* Xiroku */
 	{ .driver_data = MT_CLS_DEFAULT,
-		HID_USB_DEVICE(USB_VENDOR_ID_XIROKU,
+		MT_USB_DEVICE(USB_VENDOR_ID_XIROKU,
 			USB_DEVICE_ID_XIROKU_SPX) },
 	{ .driver_data = MT_CLS_DEFAULT,
-		HID_USB_DEVICE(USB_VENDOR_ID_XIROKU,
+		MT_USB_DEVICE(USB_VENDOR_ID_XIROKU,
 			USB_DEVICE_ID_XIROKU_MPX) },
 	{ .driver_data = MT_CLS_DEFAULT,
-		HID_USB_DEVICE(USB_VENDOR_ID_XIROKU,
+		MT_USB_DEVICE(USB_VENDOR_ID_XIROKU,
 			USB_DEVICE_ID_XIROKU_CSR) },
 	{ .driver_data = MT_CLS_DEFAULT,
-		HID_USB_DEVICE(USB_VENDOR_ID_XIROKU,
+		MT_USB_DEVICE(USB_VENDOR_ID_XIROKU,
 			USB_DEVICE_ID_XIROKU_SPX1) },
 	{ .driver_data = MT_CLS_DEFAULT,
-		HID_USB_DEVICE(USB_VENDOR_ID_XIROKU,
+		MT_USB_DEVICE(USB_VENDOR_ID_XIROKU,
 			USB_DEVICE_ID_XIROKU_MPX1) },
 	{ .driver_data = MT_CLS_DEFAULT,
-		HID_USB_DEVICE(USB_VENDOR_ID_XIROKU,
+		MT_USB_DEVICE(USB_VENDOR_ID_XIROKU,
 			USB_DEVICE_ID_XIROKU_CSR1) },
 	{ .driver_data = MT_CLS_DEFAULT,
-		HID_USB_DEVICE(USB_VENDOR_ID_XIROKU,
+		MT_USB_DEVICE(USB_VENDOR_ID_XIROKU,
 			USB_DEVICE_ID_XIROKU_SPX2) },
 	{ .driver_data = MT_CLS_DEFAULT,
-		HID_USB_DEVICE(USB_VENDOR_ID_XIROKU,
+		MT_USB_DEVICE(USB_VENDOR_ID_XIROKU,
 			USB_DEVICE_ID_XIROKU_MPX2) },
 	{ .driver_data = MT_CLS_DEFAULT,
-		HID_USB_DEVICE(USB_VENDOR_ID_XIROKU,
+		MT_USB_DEVICE(USB_VENDOR_ID_XIROKU,
 			USB_DEVICE_ID_XIROKU_CSR2) },
 
+	/* Generic MT device */
+	{ HID_DEVICE(HID_BUS_ANY, HID_GROUP_MULTITOUCH, HID_ANY_ID, HID_ANY_ID) },
 	{ }
 };
 MODULE_DEVICE_TABLE(hid, mt_devices);
diff --git a/drivers/hid/hid-uclogic.c b/drivers/hid/hid-uclogic.c
index 1f112891033..3aba02be1f2 100644
--- a/drivers/hid/hid-uclogic.c
+++ b/drivers/hid/hid-uclogic.c
@@ -14,6 +14,7 @@
 #include <linux/device.h>
 #include <linux/hid.h>
 #include <linux/module.h>
+#include <linux/usb.h>
 
 #include "hid-ids.h"
 
@@ -352,9 +353,125 @@ static __u8 pf1209_rdesc_fixed[] = {
 	0xC0                /*  End Collection                      */
 };
 
+/*
+ * See TWHL850 description, device and HID report descriptors at
+ * http://sf.net/apps/mediawiki/digimend/?title=UC-Logic_Wireless_Tablet_TWHL850
+ */
+
+/* Size of the original descriptors of TWHL850 tablet */
+#define TWHL850_RDESC_ORIG_SIZE0	182
+#define TWHL850_RDESC_ORIG_SIZE1	161
+#define TWHL850_RDESC_ORIG_SIZE2	92
+
+/* Fixed PID 0522 tablet report descriptor, interface 0 (stylus) */
+static __u8 twhl850_rdesc_fixed0[] = {
+	0x05, 0x0D,         /*  Usage Page (Digitizer),             */
+	0x09, 0x02,         /*  Usage (Pen),                        */
+	0xA1, 0x01,         /*  Collection (Application),           */
+	0x85, 0x09,         /*      Report ID (9),                  */
+	0x09, 0x20,         /*      Usage (Stylus),                 */
+	0xA0,               /*      Collection (Physical),          */
+	0x14,               /*          Logical Minimum (0),        */
+	0x25, 0x01,         /*          Logical Maximum (1),        */
+	0x75, 0x01,         /*          Report Size (1),            */
+	0x95, 0x03,         /*          Report Count (3),           */
+	0x09, 0x42,         /*          Usage (Tip Switch),         */
+	0x09, 0x44,         /*          Usage (Barrel Switch),      */
+	0x09, 0x46,         /*          Usage (Tablet Pick),        */
+	0x81, 0x02,         /*          Input (Variable),           */
+	0x81, 0x03,         /*          Input (Constant, Variable), */
+	0x95, 0x01,         /*          Report Count (1),           */
+	0x09, 0x32,         /*          Usage (In Range),           */
+	0x81, 0x02,         /*          Input (Variable),           */
+	0x81, 0x03,         /*          Input (Constant, Variable), */
+	0x75, 0x10,         /*          Report Size (16),           */
+	0xA4,               /*          Push,                       */
+	0x05, 0x01,         /*          Usage Page (Desktop),       */
+	0x65, 0x13,         /*          Unit (Inch),                */
+	0x55, 0xFD,         /*          Unit Exponent (-3),         */
+	0x34,               /*          Physical Minimum (0),       */
+	0x09, 0x30,         /*          Usage (X),                  */
+	0x46, 0x40, 0x1F,   /*          Physical Maximum (8000),    */
+	0x26, 0x00, 0x7D,   /*          Logical Maximum (32000),    */
+	0x81, 0x02,         /*          Input (Variable),           */
+	0x09, 0x31,         /*          Usage (Y),                  */
+	0x46, 0x88, 0x13,   /*          Physical Maximum (5000),    */
+	0x26, 0x20, 0x4E,   /*          Logical Maximum (20000),    */
+	0x81, 0x02,         /*          Input (Variable),           */
+	0xB4,               /*          Pop,                        */
+	0x09, 0x30,         /*          Usage (Tip Pressure),       */
+	0x26, 0xFF, 0x03,   /*          Logical Maximum (1023),     */
+	0x81, 0x02,         /*          Input (Variable),           */
+	0xC0,               /*      End Collection,                 */
+	0xC0                /*  End Collection                      */
+};
+
+/* Fixed PID 0522 tablet report descriptor, interface 1 (mouse) */
+static __u8 twhl850_rdesc_fixed1[] = {
+	0x05, 0x01,         /*  Usage Page (Desktop),               */
+	0x09, 0x02,         /*  Usage (Mouse),                      */
+	0xA1, 0x01,         /*  Collection (Application),           */
+	0x85, 0x01,         /*      Report ID (1),                  */
+	0x09, 0x01,         /*      Usage (Pointer),                */
+	0xA0,               /*      Collection (Physical),          */
+	0x05, 0x09,         /*          Usage Page (Button),        */
+	0x75, 0x01,         /*          Report Size (1),            */
+	0x95, 0x03,         /*          Report Count (3),           */
+	0x19, 0x01,         /*          Usage Minimum (01h),        */
+	0x29, 0x03,         /*          Usage Maximum (03h),        */
+	0x14,               /*          Logical Minimum (0),        */
+	0x25, 0x01,         /*          Logical Maximum (1),        */
+	0x81, 0x02,         /*          Input (Variable),           */
+	0x95, 0x05,         /*          Report Count (5),           */
+	0x81, 0x03,         /*          Input (Constant, Variable), */
+	0x05, 0x01,         /*          Usage Page (Desktop),       */
+	0x09, 0x30,         /*          Usage (X),                  */
+	0x09, 0x31,         /*          Usage (Y),                  */
+	0x16, 0x00, 0x80,   /*          Logical Minimum (-32768),   */
+	0x26, 0xFF, 0x7F,   /*          Logical Maximum (32767),    */
+	0x75, 0x10,         /*          Report Size (16),           */
+	0x95, 0x02,         /*          Report Count (2),           */
+	0x81, 0x06,         /*          Input (Variable, Relative), */
+	0x09, 0x38,         /*          Usage (Wheel),              */
+	0x15, 0xFF,         /*          Logical Minimum (-1),       */
+	0x25, 0x01,         /*          Logical Maximum (1),        */
+	0x95, 0x01,         /*          Report Count (1),           */
+	0x75, 0x08,         /*          Report Size (8),            */
+	0x81, 0x06,         /*          Input (Variable, Relative), */
+	0x81, 0x03,         /*          Input (Constant, Variable), */
+	0xC0,               /*      End Collection,                 */
+	0xC0                /*  End Collection                      */
+};
+
+/* Fixed PID 0522 tablet report descriptor, interface 2 (frame buttons) */
+static __u8 twhl850_rdesc_fixed2[] = {
+	0x05, 0x01,         /*  Usage Page (Desktop),               */
+	0x09, 0x06,         /*  Usage (Keyboard),                   */
+	0xA1, 0x01,         /*  Collection (Application),           */
+	0x85, 0x03,         /*      Report ID (3),                  */
+	0x05, 0x07,         /*      Usage Page (Keyboard),          */
+	0x14,               /*      Logical Minimum (0),            */
+	0x19, 0xE0,         /*      Usage Minimum (KB Leftcontrol), */
+	0x29, 0xE7,         /*      Usage Maximum (KB Right GUI),   */
+	0x25, 0x01,         /*      Logical Maximum (1),            */
+	0x75, 0x01,         /*      Report Size (1),                */
+	0x95, 0x08,         /*      Report Count (8),               */
+	0x81, 0x02,         /*      Input (Variable),               */
+	0x18,               /*      Usage Minimum (None),           */
+	0x29, 0xFF,         /*      Usage Maximum (FFh),            */
+	0x26, 0xFF, 0x00,   /*      Logical Maximum (255),          */
+	0x75, 0x08,         /*      Report Size (8),                */
+	0x95, 0x06,         /*      Report Count (6),               */
+	0x80,               /*      Input,                          */
+	0xC0                /*  End Collection                      */
+};
+
 static __u8 *uclogic_report_fixup(struct hid_device *hdev, __u8 *rdesc,
 					unsigned int *rsize)
 {
+	struct usb_interface *iface = to_usb_interface(hdev->dev.parent);
+	__u8 iface_num = iface->cur_altsetting->desc.bInterfaceNumber;
+
 	switch (hdev->product) {
 	case USB_DEVICE_ID_UCLOGIC_TABLET_PF1209:
 		if (*rsize == PF1209_RDESC_ORIG_SIZE) {
@@ -386,6 +503,28 @@ static __u8 *uclogic_report_fixup(struct hid_device *hdev, __u8 *rdesc,
 			*rsize = sizeof(wp1062_rdesc_fixed);
 		}
 		break;
+	case USB_DEVICE_ID_UCLOGIC_WIRELESS_TABLET_TWHL850:
+		switch (iface_num) {
+		case 0:
+			if (*rsize == TWHL850_RDESC_ORIG_SIZE0) {
+				rdesc = twhl850_rdesc_fixed0;
+				*rsize = sizeof(twhl850_rdesc_fixed0);
+			}
+			break;
+		case 1:
+			if (*rsize == TWHL850_RDESC_ORIG_SIZE1) {
+				rdesc = twhl850_rdesc_fixed1;
+				*rsize = sizeof(twhl850_rdesc_fixed1);
+			}
+			break;
+		case 2:
+			if (*rsize == TWHL850_RDESC_ORIG_SIZE2) {
+				rdesc = twhl850_rdesc_fixed2;
+				*rsize = sizeof(twhl850_rdesc_fixed2);
+			}
+			break;
+		}
+		break;
 	}
 
 	return rdesc;
@@ -402,6 +541,8 @@ static const struct hid_device_id uclogic_devices[] = {
 				USB_DEVICE_ID_UCLOGIC_TABLET_WP8060U) },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_UCLOGIC,
 				USB_DEVICE_ID_UCLOGIC_TABLET_WP1062) },
+	{ HID_USB_DEVICE(USB_VENDOR_ID_UCLOGIC,
+				USB_DEVICE_ID_UCLOGIC_WIRELESS_TABLET_TWHL850) },
 	{ }
 };
 MODULE_DEVICE_TABLE(hid, uclogic_devices);
diff --git a/drivers/hid/hid-wacom.c b/drivers/hid/hid-wacom.c
index 067e2963314..fe23a1eb586 100644
--- a/drivers/hid/hid-wacom.c
+++ b/drivers/hid/hid-wacom.c
@@ -24,15 +24,16 @@
 #include <linux/device.h>
 #include <linux/hid.h>
 #include <linux/module.h>
+#include <linux/leds.h>
 #include <linux/slab.h>
-#ifdef CONFIG_HID_WACOM_POWER_SUPPLY
 #include <linux/power_supply.h>
-#endif
 
 #include "hid-ids.h"
 
 #define PAD_DEVICE_ID	0x0F
 
+#define WAC_CMD_LED_CONTROL     0x20
+
 struct wacom_data {
 	__u16 tool;
 	__u16 butstate;
@@ -41,16 +42,20 @@ struct wacom_data {
 	__u32 id;
 	__u32 serial;
 	unsigned char high_speed;
-#ifdef CONFIG_HID_WACOM_POWER_SUPPLY
-	int battery_capacity;
+	__u8 battery_capacity;
+	__u8 power_raw;
+	__u8 ps_connected;
 	struct power_supply battery;
 	struct power_supply ac;
-#endif
+	__u8 led_selector;
+	struct led_classdev *leds[4];
 };
 
-#ifdef CONFIG_HID_WACOM_POWER_SUPPLY
-/*percent of battery capacity, 0 means AC online*/
-static unsigned short batcap[8] = { 1, 15, 25, 35, 50, 70, 100, 0 };
+/*percent of battery capacity for Graphire
+  8th value means AC online and show 100% capacity */
+static unsigned short batcap_gr[8] = { 1, 15, 25, 35, 50, 70, 100, 100 };
+/*percent of battery capacity for Intuos4 WL, AC has a separate bit*/
+static unsigned short batcap_i4[8] = { 1, 15, 30, 45, 60, 70, 85, 100 };
 
 static enum power_supply_property wacom_battery_props[] = {
 	POWER_SUPPLY_PROP_PRESENT,
@@ -64,13 +69,123 @@ static enum power_supply_property wacom_ac_props[] = {
 	POWER_SUPPLY_PROP_SCOPE,
 };
 
+static void wacom_leds_set_brightness(struct led_classdev *led_dev,
+						enum led_brightness value)
+{
+	struct device *dev = led_dev->dev->parent;
+	struct hid_device *hdev;
+	struct wacom_data *wdata;
+	unsigned char *buf;
+	__u8 led = 0;
+	int i;
+
+	hdev = container_of(dev, struct hid_device, dev);
+	wdata = hid_get_drvdata(hdev);
+	for (i = 0; i < 4; ++i) {
+		if (wdata->leds[i] == led_dev)
+			wdata->led_selector = i;
+	}
+
+	led = wdata->led_selector | 0x04;
+	buf = kzalloc(9, GFP_KERNEL);
+	if (buf) {
+		buf[0] = WAC_CMD_LED_CONTROL;
+		buf[1] = led;
+		buf[2] = value;
+		hdev->hid_output_raw_report(hdev, buf, 9, HID_FEATURE_REPORT);
+		kfree(buf);
+	}
+
+	return;
+}
+
+static enum led_brightness wacom_leds_get_brightness(struct led_classdev *led_dev)
+{
+	struct wacom_data *wdata;
+	struct device *dev = led_dev->dev->parent;
+	int value = 0;
+	int i;
+
+	wdata = hid_get_drvdata(container_of(dev, struct hid_device, dev));
+
+	for (i = 0; i < 4; ++i) {
+		if (wdata->leds[i] == led_dev) {
+			value = wdata->leds[i]->brightness;
+			break;
+		}
+	}
+
+	return value;
+}
+
+
+static int wacom_initialize_leds(struct hid_device *hdev)
+{
+	struct wacom_data *wdata = hid_get_drvdata(hdev);
+	struct led_classdev *led;
+	struct device *dev = &hdev->dev;
+	size_t namesz = strlen(dev_name(dev)) + 12;
+	char *name;
+	int i, ret;
+
+	wdata->led_selector = 0;
+
+	for (i = 0; i < 4; i++) {
+		led = kzalloc(sizeof(struct led_classdev) + namesz, GFP_KERNEL);
+		if (!led) {
+			hid_warn(hdev,
+				 "can't allocate memory for LED selector\n");
+			ret = -ENOMEM;
+			goto err;
+		}
+
+		name = (void *)&led[1];
+		snprintf(name, namesz, "%s:selector:%d", dev_name(dev), i);
+		led->name = name;
+		led->brightness = 0;
+		led->max_brightness = 127;
+		led->brightness_get = wacom_leds_get_brightness;
+		led->brightness_set = wacom_leds_set_brightness;
+
+		wdata->leds[i] = led;
+
+		ret = led_classdev_register(dev, wdata->leds[i]);
+
+		if (ret) {
+			wdata->leds[i] = NULL;
+			kfree(led);
+			hid_warn(hdev, "can't register LED\n");
+			goto err;
+		}
+	}
+
+err:
+	return ret;
+}
+
+static void wacom_destroy_leds(struct hid_device *hdev)
+{
+	struct wacom_data *wdata = hid_get_drvdata(hdev);
+	struct led_classdev *led;
+	int i;
+
+	for (i = 0; i < 4; ++i) {
+		if (wdata->leds[i]) {
+			led = wdata->leds[i];
+			wdata->leds[i] = NULL;
+			led_classdev_unregister(led);
+			kfree(led);
+		}
+	}
+
+}
+
 static int wacom_battery_get_property(struct power_supply *psy,
 				enum power_supply_property psp,
 				union power_supply_propval *val)
 {
 	struct wacom_data *wdata = container_of(psy,
 					struct wacom_data, battery);
-	int power_state = batcap[wdata->battery_capacity];
 	int ret = 0;
 
 	switch (psp) {
@@ -81,11 +196,7 @@ static int wacom_battery_get_property(struct power_supply *psy,
 		val->intval = POWER_SUPPLY_SCOPE_DEVICE;
 		break;
 	case POWER_SUPPLY_PROP_CAPACITY:
-		/* show 100% battery capacity when charging */
-		if (power_state == 0)
-			val->intval = 100;
-		else
-			val->intval = power_state;
+		val->intval = wdata->battery_capacity;
 		break;
 	default:
 		ret = -EINVAL;
@@ -99,17 +210,13 @@ static int wacom_ac_get_property(struct power_supply *psy,
 				union power_supply_propval *val)
 {
 	struct wacom_data *wdata = container_of(psy, struct wacom_data, ac);
-	int power_state = batcap[wdata->battery_capacity];
 	int ret = 0;
 
 	switch (psp) {
 	case POWER_SUPPLY_PROP_PRESENT:
 		/* fall through */
 	case POWER_SUPPLY_PROP_ONLINE:
-		if (power_state == 0)
-			val->intval = 1;
-		else
-			val->intval = 0;
+		val->intval = wdata->ps_connected;
 		break;
 	case POWER_SUPPLY_PROP_SCOPE:
 		val->intval = POWER_SUPPLY_SCOPE_DEVICE;
@@ -120,41 +227,16 @@ static int wacom_ac_get_property(struct power_supply *psy,
 	}
 	return ret;
 }
-#endif
-
-static void wacom_set_features(struct hid_device *hdev)
-{
-	int ret;
-	__u8 rep_data[2];
-
-	/*set high speed, tablet mode*/
-	rep_data[0] = 0x03;
-	rep_data[1] = 0x20;
-	ret = hdev->hid_output_raw_report(hdev, rep_data, 2,
-				HID_FEATURE_REPORT);
-	return;
-}
 
-static void wacom_poke(struct hid_device *hdev, u8 speed)
+static void wacom_set_features(struct hid_device *hdev, u8 speed)
 {
 	struct wacom_data *wdata = hid_get_drvdata(hdev);
 	int limit, ret;
-	char rep_data[2];
-
-	rep_data[0] = 0x03 ; rep_data[1] = 0x00;
-	limit = 3;
-	do {
-		ret = hdev->hid_output_raw_report(hdev, rep_data, 2,
-				HID_FEATURE_REPORT);
-	} while (ret < 0 && limit-- > 0);
-
-	if (ret >= 0) {
-		if (speed == 0)
-			rep_data[0] = 0x05;
-		else
-			rep_data[0] = 0x06;
+	__u8 rep_data[2];
 
-		rep_data[1] = 0x00;
+	switch (hdev->product) {
+	case USB_DEVICE_ID_WACOM_GRAPHIRE_BLUETOOTH:
+		rep_data[0] = 0x03 ; rep_data[1] = 0x00;
 		limit = 3;
 		do {
 			ret = hdev->hid_output_raw_report(hdev, rep_data, 2,
@@ -162,17 +244,47 @@ static void wacom_poke(struct hid_device *hdev, u8 speed)
 		} while (ret < 0 && limit-- > 0);
 
 		if (ret >= 0) {
-			wdata->high_speed = speed;
-			return;
+			if (speed == 0)
+				rep_data[0] = 0x05;
+			else
+				rep_data[0] = 0x06;
+
+			rep_data[1] = 0x00;
+			limit = 3;
+			do {
+				ret = hdev->hid_output_raw_report(hdev,
+					rep_data, 2, HID_FEATURE_REPORT);
+			} while (ret < 0 && limit-- > 0);
+
+			if (ret >= 0) {
+				wdata->high_speed = speed;
+				return;
+			}
 		}
+
+		/*
+		 * Note that if the raw queries fail, it's not a hard failure
+		 * and it is safe to continue
+		 */
+		hid_warn(hdev, "failed to poke device, command %d, err %d\n",
+			 rep_data[0], ret);
+		break;
+	case USB_DEVICE_ID_WACOM_INTUOS4_BLUETOOTH:
+		if (speed == 1)
+			wdata->features &= ~0x20;
+		else
+			wdata->features |= 0x20;
+
+		rep_data[0] = 0x03;
+		rep_data[1] = wdata->features;
+
+		ret = hdev->hid_output_raw_report(hdev, rep_data, 2,
+					HID_FEATURE_REPORT);
+		if (ret >= 0)
+			wdata->high_speed = speed;
+		break;
 	}
 
-	/*
-	 * Note that if the raw queries fail, it's not a hard failure and it
-	 * is safe to continue
-	 */
-	hid_warn(hdev, "failed to poke device, command %d, err %d\n",
-		 rep_data[0], ret);
 	return;
 }
 
@@ -196,7 +308,7 @@ static ssize_t wacom_store_speed(struct device *dev,
 		return -EINVAL;
 
 	if (new_speed == 0 || new_speed == 1) {
-		wacom_poke(hdev, new_speed);
+		wacom_set_features(hdev, new_speed);
 		return strnlen(buf, PAGE_SIZE);
 	} else
 		return -EINVAL;
@@ -310,12 +422,16 @@ static int wacom_gr_parse_report(struct hid_device *hdev,
 		input_sync(input);
 	}
 
-#ifdef CONFIG_HID_WACOM_POWER_SUPPLY
-	/* Store current battery capacity */
+	/* Store current battery capacity and power supply state*/
 	rw = (data[7] >> 2 & 0x07);
-	if (rw != wdata->battery_capacity)
-		wdata->battery_capacity = rw;
-#endif
+	if (rw != wdata->power_raw) {
+		wdata->power_raw = rw;
+		wdata->battery_capacity = batcap_gr[rw];
+		if (rw == 7)
+			wdata->ps_connected = 1;
+		else
+			wdata->ps_connected = 0;
+	}
 	return 1;
 }
 
@@ -369,6 +485,7 @@ static void wacom_i4_parse_pen_report(struct wacom_data *wdata,
 {
 	__u16 x, y, pressure;
 	__u8 distance;
+	__u8 tilt_x, tilt_y;
 
 	switch (data[1]) {
 	case 0x80: /* Out of proximity report */
@@ -405,6 +522,8 @@ static void wacom_i4_parse_pen_report(struct wacom_data *wdata,
 		pressure = (data[6] << 3) | ((data[7] & 0xC0) >> 5)
 			| (data[1] & 0x01);
 		distance = (data[9] >> 2) & 0x3f;
+		tilt_x = ((data[7] << 1) & 0x7e) | (data[8] >> 7);
+		tilt_y = data[8] & 0x7f;
 
 		input_report_key(input, BTN_TOUCH, pressure > 1);
 
@@ -415,6 +534,8 @@ static void wacom_i4_parse_pen_report(struct wacom_data *wdata,
 		input_report_abs(input, ABS_Y, y);
 		input_report_abs(input, ABS_PRESSURE, pressure);
 		input_report_abs(input, ABS_DISTANCE, distance);
+		input_report_abs(input, ABS_TILT_X, tilt_x);
+		input_report_abs(input, ABS_TILT_Y, tilt_y);
 		input_report_abs(input, ABS_MISC, wdata->id);
 		input_event(input, EV_MSC, MSC_SERIAL, wdata->serial);
 		input_report_key(input, wdata->tool, 1);
@@ -455,6 +576,7 @@ static int wacom_raw_event(struct hid_device *hdev, struct hid_report *report,
 	struct input_dev *input;
 	unsigned char *data = (unsigned char *) raw_data;
 	int i;
+	__u8 power_raw;
 
 	if (!(hdev->claimed & HID_CLAIMED_INPUT))
 		return 0;
@@ -462,13 +584,15 @@ static int wacom_raw_event(struct hid_device *hdev, struct hid_report *report,
 	hidinput = list_entry(hdev->inputs.next, struct hid_input, list);
 	input = hidinput->input;
 
-	/* Check if this is a tablet report */
-	if (data[0] != 0x03)
-		return 0;
-
 	switch (hdev->product) {
 	case USB_DEVICE_ID_WACOM_GRAPHIRE_BLUETOOTH:
-		return wacom_gr_parse_report(hdev, wdata, input, data);
+		if (data[0] == 0x03) {
+			return wacom_gr_parse_report(hdev, wdata, input, data);
+		} else {
+			hid_err(hdev, "Unknown report: %d,%d size:%d\n",
+					data[0], data[1], size);
+			return 0;
+		}
 		break;
 	case USB_DEVICE_ID_WACOM_INTUOS4_BLUETOOTH:
 		i = 1;
@@ -482,6 +606,13 @@ static int wacom_raw_event(struct hid_device *hdev, struct hid_report *report,
 			wacom_i4_parse_report(hdev, wdata, input, data + i);
 			i += 10;
 			wacom_i4_parse_report(hdev, wdata, input, data + i);
+			power_raw = data[i+10];
+			if (power_raw != wdata->power_raw) {
+				wdata->power_raw = power_raw;
+				wdata->battery_capacity = batcap_i4[power_raw & 0x07];
+				wdata->ps_connected = power_raw & 0x08;
+			}
+
 			break;
 		default:
 			hid_err(hdev, "Unknown report: %d,%d size:%d\n",
@@ -546,6 +677,8 @@ static int wacom_input_mapped(struct hid_device *hdev, struct hid_input *hi,
 		input_set_abs_params(input, ABS_Y, 0, 25400, 4, 0);
 		input_set_abs_params(input, ABS_PRESSURE, 0, 2047, 0, 0);
 		input_set_abs_params(input, ABS_DISTANCE, 0, 63, 0, 0);
+		input_set_abs_params(input, ABS_TILT_X, 0, 127, 0, 0);
+		input_set_abs_params(input, ABS_TILT_Y, 0, 127, 0, 0);
 		break;
 	}
 
@@ -584,19 +717,19 @@ static int wacom_probe(struct hid_device *hdev,
 		hid_warn(hdev,
 			 "can't create sysfs speed attribute err: %d\n", ret);
 
-	switch (hdev->product) {
-	case USB_DEVICE_ID_WACOM_GRAPHIRE_BLUETOOTH:
-		/* Set Wacom mode 2 with high reporting speed */
-		wacom_poke(hdev, 1);
-		break;
-	case USB_DEVICE_ID_WACOM_INTUOS4_BLUETOOTH:
+	wdata->features = 0;
+	wacom_set_features(hdev, 1);
+
+	if (hdev->product == USB_DEVICE_ID_WACOM_INTUOS4_BLUETOOTH) {
 		sprintf(hdev->name, "%s", "Wacom Intuos4 WL");
-		wdata->features = 0;
-		wacom_set_features(hdev);
-		break;
+		ret = wacom_initialize_leds(hdev);
+		if (ret) {
+			hid_warn(hdev,
+				 "can't create led attribute, err: %d\n", ret);
+			goto destroy_leds;
+		}
 	}
 
-#ifdef CONFIG_HID_WACOM_POWER_SUPPLY
 	wdata->battery.properties = wacom_battery_props;
 	wdata->battery.num_properties = ARRAY_SIZE(wacom_battery_props);
 	wdata->battery.get_property = wacom_battery_get_property;
@@ -629,16 +762,15 @@ static int wacom_probe(struct hid_device *hdev,
 	}
 
 	power_supply_powers(&wdata->ac, &hdev->dev);
-#endif
 	return 0;
 
-#ifdef CONFIG_HID_WACOM_POWER_SUPPLY
 err_ac:
 	power_supply_unregister(&wdata->battery);
 err_battery:
 	device_remove_file(&hdev->dev, &dev_attr_speed);
 	hid_hw_stop(hdev);
-#endif
+destroy_leds:
+	wacom_destroy_leds(hdev);
 err_free:
 	kfree(wdata);
 	return ret;
@@ -646,16 +778,14 @@ err_free:
 
 static void wacom_remove(struct hid_device *hdev)
 {
-#ifdef CONFIG_HID_WACOM_POWER_SUPPLY
 	struct wacom_data *wdata = hid_get_drvdata(hdev);
-#endif
+
+	wacom_destroy_leds(hdev);
 	device_remove_file(&hdev->dev, &dev_attr_speed);
 	hid_hw_stop(hdev);
 
-#ifdef CONFIG_HID_WACOM_POWER_SUPPLY
 	power_supply_unregister(&wdata->battery);
 	power_supply_unregister(&wdata->ac);
-#endif
 	kfree(hid_get_drvdata(hdev));
 }
 
@@ -693,5 +823,5 @@ static void __exit wacom_exit(void)
 
 module_init(wacom_init);
 module_exit(wacom_exit);
+MODULE_DESCRIPTION("Driver for Wacom Graphire Bluetooth and Wacom Intuos4 WL");
 MODULE_LICENSE("GPL");
-
diff --git a/drivers/hid/hid-waltop.c b/drivers/hid/hid-waltop.c
index 2cfd95c4467..745e4e9a8cf 100644
--- a/drivers/hid/hid-waltop.c
+++ b/drivers/hid/hid-waltop.c
@@ -502,28 +502,146 @@ static __u8 media_tablet_14_1_inch_rdesc_fixed[] = {
 	0xC0                /*  End Collection                      */
 };
 
-struct waltop_state {
-	u8 pressure0;
-	u8 pressure1;
+/*
+ * See Sirius Battery Free Tablet description, device and HID report descriptors
+ * at
+ * http://sf.net/apps/mediawiki/digimend/?title=Waltop_Sirius_Battery_Free_Tablet
+ */
+
+/* Size of the original report descriptor of Sirius Battery Free Tablet */
+#define SIRIUS_BATTERY_FREE_TABLET_RDESC_ORIG_SIZE	335
+
+/* Fixed Sirius Battery Free Tablet descriptor */
+static __u8 sirius_battery_free_tablet_rdesc_fixed[] = {
+	0x05, 0x0D,         /*  Usage Page (Digitizer),             */
+	0x09, 0x02,         /*  Usage (Pen),                        */
+	0xA1, 0x01,         /*  Collection (Application),           */
+	0x85, 0x10,         /*      Report ID (16),                 */
+	0x09, 0x20,         /*      Usage (Stylus),                 */
+	0xA0,               /*      Collection (Physical),          */
+	0x95, 0x01,         /*          Report Count (1),           */
+	0x15, 0x01,         /*          Logical Minimum (1),        */
+	0x25, 0x03,         /*          Logical Maximum (3),        */
+	0x75, 0x02,         /*          Report Size (2),            */
+	0x09, 0x42,         /*          Usage (Tip Switch),         */
+	0x09, 0x44,         /*          Usage (Barrel Switch),      */
+	0x09, 0x46,         /*          Usage (Tablet Pick),        */
+	0x80,               /*          Input,                      */
+	0x14,               /*          Logical Minimum (0),        */
+	0x25, 0x01,         /*          Logical Maximum (1),        */
+	0x75, 0x01,         /*          Report Size (1),            */
+	0x09, 0x3C,         /*          Usage (Invert),             */
+	0x81, 0x02,         /*          Input (Variable),           */
+	0x81, 0x03,         /*          Input (Constant, Variable), */
+	0x09, 0x32,         /*          Usage (In Range),           */
+	0x81, 0x02,         /*          Input (Variable),           */
+	0x95, 0x03,         /*          Report Count (3),           */
+	0x81, 0x03,         /*          Input (Constant, Variable), */
+	0xA4,               /*          Push,                       */
+	0x05, 0x01,         /*          Usage Page (Desktop),       */
+	0x55, 0xFD,         /*          Unit Exponent (-3),         */
+	0x65, 0x13,         /*          Unit (Inch),                */
+	0x34,               /*          Physical Minimum (0),       */
+	0x14,               /*          Logical Minimum (0),        */
+	0x75, 0x10,         /*          Report Size (16),           */
+	0x95, 0x01,         /*          Report Count (1),           */
+	0x46, 0x10, 0x27,   /*          Physical Maximum (10000),   */
+	0x26, 0x20, 0x4E,   /*          Logical Maximum (20000),    */
+	0x09, 0x30,         /*          Usage (X),                  */
+	0x81, 0x02,         /*          Input (Variable),           */
+	0x46, 0x70, 0x17,   /*          Physical Maximum (6000),    */
+	0x26, 0xE0, 0x2E,   /*          Logical Maximum (12000),    */
+	0x09, 0x31,         /*          Usage (Y),                  */
+	0x81, 0x02,         /*          Input (Variable),           */
+	0xB4,               /*          Pop,                        */
+	0x75, 0x10,         /*          Report Size (16),           */
+	0x95, 0x01,         /*          Report Count (1),           */
+	0x14,               /*          Logical Minimum (0),        */
+	0x26, 0xFF, 0x03,   /*          Logical Maximum (1023),     */
+	0x09, 0x30,         /*          Usage (Tip Pressure),       */
+	0x81, 0x02,         /*          Input (Variable),           */
+	0xA4,               /*          Push,                       */
+	0x55, 0xFE,         /*          Unit Exponent (-2),         */
+	0x65, 0x12,         /*          Unit (Radians),             */
+	0x35, 0x97,         /*          Physical Minimum (-105),    */
+	0x45, 0x69,         /*          Physical Maximum (105),     */
+	0x15, 0x97,         /*          Logical Minimum (-105),     */
+	0x25, 0x69,         /*          Logical Maximum (105),      */
+	0x75, 0x08,         /*          Report Size (8),            */
+	0x95, 0x02,         /*          Report Count (2),           */
+	0x09, 0x3D,         /*          Usage (X Tilt),             */
+	0x09, 0x3E,         /*          Usage (Y Tilt),             */
+	0x81, 0x02,         /*          Input (Variable),           */
+	0xB4,               /*          Pop,                        */
+	0xC0,               /*      End Collection,                 */
+	0xC0,               /*  End Collection,                     */
+	0x05, 0x01,         /*  Usage Page (Desktop),               */
+	0x09, 0x02,         /*  Usage (Mouse),                      */
+	0xA1, 0x01,         /*  Collection (Application),           */
+	0x85, 0x01,         /*      Report ID (1),                  */
+	0x09, 0x01,         /*      Usage (Pointer),                */
+	0xA0,               /*      Collection (Physical),          */
+	0x75, 0x08,         /*          Report Size (8),            */
+	0x95, 0x03,         /*          Report Count (3),           */
+	0x81, 0x03,         /*          Input (Constant, Variable), */
+	0x09, 0x38,         /*          Usage (Wheel),              */
+	0x15, 0xFF,         /*          Logical Minimum (-1),       */
+	0x25, 0x01,         /*          Logical Maximum (1),        */
+	0x75, 0x08,         /*          Report Size (8),            */
+	0x95, 0x01,         /*          Report Count (1),           */
+	0x81, 0x06,         /*          Input (Variable, Relative), */
+	0x75, 0x08,         /*          Report Size (8),            */
+	0x95, 0x03,         /*          Report Count (3),           */
+	0x81, 0x03,         /*          Input (Constant, Variable), */
+	0xC0,               /*      End Collection,                 */
+	0xC0,               /*  End Collection,                     */
+	0x05, 0x01,         /*  Usage Page (Desktop),               */
+	0x09, 0x06,         /*  Usage (Keyboard),                   */
+	0xA1, 0x01,         /*  Collection (Application),           */
+	0x85, 0x0D,         /*      Report ID (13),                 */
+	0x05, 0x07,         /*      Usage Page (Keyboard),          */
+	0x19, 0xE0,         /*      Usage Minimum (KB Leftcontrol), */
+	0x29, 0xE7,         /*      Usage Maximum (KB Right GUI),   */
+	0x14,               /*      Logical Minimum (0),            */
+	0x25, 0x01,         /*      Logical Maximum (1),            */
+	0x75, 0x01,         /*      Report Size (1),                */
+	0x95, 0x08,         /*      Report Count (8),               */
+	0x81, 0x02,         /*      Input (Variable),               */
+	0x75, 0x08,         /*      Report Size (8),                */
+	0x95, 0x01,         /*      Report Count (1),               */
+	0x81, 0x01,         /*      Input (Constant),               */
+	0x18,               /*      Usage Minimum (None),           */
+	0x29, 0x65,         /*      Usage Maximum (KB Application), */
+	0x14,               /*      Logical Minimum (0),            */
+	0x25, 0x65,         /*      Logical Maximum (101),          */
+	0x75, 0x08,         /*      Report Size (8),                */
+	0x95, 0x05,         /*      Report Count (5),               */
+	0x80,               /*      Input,                          */
+	0xC0,               /*  End Collection,                     */
+	0x05, 0x0C,         /*  Usage Page (Consumer),              */
+	0x09, 0x01,         /*  Usage (Consumer Control),           */
+	0xA1, 0x01,         /*  Collection (Application),           */
+	0x85, 0x0C,         /*      Report ID (12),                 */
+	0x09, 0xE9,         /*      Usage (Volume Inc),             */
+	0x09, 0xEA,         /*      Usage (Volume Dec),             */
+	0x14,               /*      Logical Minimum (0),            */
+	0x25, 0x01,         /*      Logical Maximum (1),            */
+	0x75, 0x01,         /*      Report Size (1),                */
+	0x95, 0x02,         /*      Report Count (2),               */
+	0x81, 0x02,         /*      Input (Variable),               */
+	0x75, 0x06,         /*      Report Size (6),                */
+	0x95, 0x01,         /*      Report Count (1),               */
+	0x81, 0x03,         /*      Input (Constant, Variable),     */
+	0x75, 0x10,         /*      Report Size (16),               */
+	0x95, 0x03,         /*      Report Count (3),               */
+	0x81, 0x03,         /*      Input (Constant, Variable),     */
+	0xC0                /*  End Collection                      */
 };
 
 static int waltop_probe(struct hid_device *hdev,
 			const struct hid_device_id *id)
 {
 	int ret;
-	struct waltop_state *s;
-
-	s = kzalloc(sizeof(*s), GFP_KERNEL);
-	if (s == NULL) {
-		hid_err(hdev, "can't allocate device state\n");
-		ret = -ENOMEM;
-		goto err;
-	}
-
-	s->pressure0 = 0;
-	s->pressure1 = 0;
-
-	hid_set_drvdata(hdev, s);
 
 	ret = hid_parse(hdev);
 	if (ret) {
@@ -539,7 +657,6 @@ static int waltop_probe(struct hid_device *hdev,
 
 	return 0;
 err:
-	kfree(s);
 	return ret;
 }
 
@@ -583,6 +700,12 @@ static __u8 *waltop_report_fixup(struct hid_device *hdev, __u8 *rdesc,
 			*rsize = sizeof(media_tablet_14_1_inch_rdesc_fixed);
 		}
 		break;
+	case USB_DEVICE_ID_WALTOP_SIRIUS_BATTERY_FREE_TABLET:
+		if (*rsize == SIRIUS_BATTERY_FREE_TABLET_RDESC_ORIG_SIZE) {
+			rdesc = sirius_battery_free_tablet_rdesc_fixed;
+			*rsize = sizeof(sirius_battery_free_tablet_rdesc_fixed);
+		}
+		break;
 	}
 	return rdesc;
 }
@@ -590,39 +713,72 @@ static __u8 *waltop_report_fixup(struct hid_device *hdev, __u8 *rdesc,
 static int waltop_raw_event(struct hid_device *hdev, struct hid_report *report,
 		     u8 *data, int size)
 {
-	/* If this is a pen input report of a tablet with PID 0038 */
-	if (hdev->product == USB_DEVICE_ID_WALTOP_PID_0038 &&
-	    report->type == HID_INPUT_REPORT &&
-	    report->id == 16 &&
-	    size == 8) {
-		struct waltop_state *s = hid_get_drvdata(hdev);
-
+	/* If this is a pen input report */
+	if (report->type == HID_INPUT_REPORT && report->id == 16 && size >= 8) {
 		/*
-		 * Ignore maximum pressure reported when a barrel button is
-		 * pressed.
+		 * Ignore reported pressure when a barrel button is pressed,
+		 * because it is rarely correct.
 		 */
 
 		/* If a barrel button is pressed */
 		if ((data[1] & 0xF) > 1) {
-			/* Use the last known pressure */
-			data[6] = s->pressure0;
-			data[7] = s->pressure1;
-		} else {
-			/* Remember reported pressure */
-			s->pressure0 = data[6];
-			s->pressure1 = data[7];
+			/* Report zero pressure */
+			data[6] = 0;
+			data[7] = 0;
 		}
 	}
 
+	/* If this is a pen input report of Sirius Battery Free Tablet */
+	if (hdev->product == USB_DEVICE_ID_WALTOP_SIRIUS_BATTERY_FREE_TABLET &&
+	    report->type == HID_INPUT_REPORT &&
+	    report->id == 16 &&
+	    size == 10) {
+		/*
+		 * The tablet reports tilt as roughly sin(a)*21 (18 means 60
+		 * degrees).
+		 *
+		 * This array stores angles as radians * 100, corresponding to
+		 * reported values up to 60 degrees, as expected by userspace.
+		 */
+		static const s8 tilt_to_radians[] = {
+			0, 5, 10, 14, 19, 24, 29, 34, 40, 45,
+			50, 56, 62, 68, 74, 81, 88, 96, 105
+		};
+
+		s8 tilt_x = (s8)data[8];
+		s8 tilt_y = (s8)data[9];
+		s8 sign_x = tilt_x >= 0 ? 1 : -1;
+		s8 sign_y = tilt_y >= 0 ? 1 : -1;
+
+		tilt_x *= sign_x;
+		tilt_y *= sign_y;
+
+		/*
+		 * Reverse the Y Tilt direction to match the HID standard and
+		 * userspace expectations. See HID Usage Tables v1.12 16.3.2
+		 * Tilt Orientation.
+		 */
+		sign_y *= -1;
+
+		/*
+		 * This effectively clamps reported tilt to 60 degrees - the
+		 * range expected by userspace
+		 */
+		if (tilt_x > ARRAY_SIZE(tilt_to_radians) - 1)
+			tilt_x = ARRAY_SIZE(tilt_to_radians) - 1;
+		if (tilt_y > ARRAY_SIZE(tilt_to_radians) - 1)
+			tilt_y = ARRAY_SIZE(tilt_to_radians) - 1;
+
+		data[8] = tilt_to_radians[tilt_x] * sign_x;
+		data[9] = tilt_to_radians[tilt_y] * sign_y;
+	}
+
 	return 0;
 }
 
 static void waltop_remove(struct hid_device *hdev)
 {
-	struct waltop_state *s = hid_get_drvdata(hdev);
-
 	hid_hw_stop(hdev);
-	kfree(s);
 }
 
 static const struct hid_device_id waltop_devices[] = {
@@ -638,6 +794,8 @@ static const struct hid_device_id waltop_devices[] = {
 				USB_DEVICE_ID_WALTOP_MEDIA_TABLET_10_6_INCH) },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_WALTOP,
 				USB_DEVICE_ID_WALTOP_MEDIA_TABLET_14_1_INCH) },
+	{ HID_USB_DEVICE(USB_VENDOR_ID_WALTOP,
+			 USB_DEVICE_ID_WALTOP_SIRIUS_BATTERY_FREE_TABLET) },
 	{ }
 };
 MODULE_DEVICE_TABLE(hid, waltop_devices);
diff --git a/drivers/hid/hid-wiimote-core.c b/drivers/hid/hid-wiimote-core.c
index cac3589b1ed..84e2fbec5fb 100644
--- a/drivers/hid/hid-wiimote-core.c
+++ b/drivers/hid/hid-wiimote-core.c
@@ -769,7 +769,7 @@ static void __ir_to_input(struct wiimote_data *wdata, const __u8 *ir,
 
 	/*
 	 * Basic IR data is encoded into 3 bytes. The first two bytes are the
-	 * upper 8 bit of the X/Y data, the 3rd byte contains the lower 2 bits
+	 * lower 8 bit of the X/Y data, the 3rd byte contains the upper 2 bits
 	 * of both.
 	 * If data is packed, then the 3rd byte is put first and slightly
 	 * reordered. This allows to interleave packed and non-packed data to
@@ -778,17 +778,11 @@ static void __ir_to_input(struct wiimote_data *wdata, const __u8 *ir,
 	 */
 
 	if (packed) {
-		x = ir[1] << 2;
-		y = ir[2] << 2;
-
-		x |= ir[0] & 0x3;
-		y |= (ir[0] >> 2) & 0x3;
+		x = ir[1] | ((ir[0] & 0x03) << 8);
+		y = ir[2] | ((ir[0] & 0x0c) << 6);
 	} else {
-		x = ir[0] << 2;
-		y = ir[1] << 2;
-
-		x |= (ir[2] >> 4) & 0x3;
-		y |= (ir[2] >> 6) & 0x3;
+		x = ir[0] | ((ir[2] & 0x30) << 4);
+		y = ir[1] | ((ir[2] & 0xc0) << 2);
 	}
 
 	input_report_abs(wdata->ir, xid, x);
diff --git a/drivers/hid/hidraw.c b/drivers/hid/hidraw.c
index cf7d6d58e79..36fa77b40ff 100644
--- a/drivers/hid/hidraw.c
+++ b/drivers/hid/hidraw.c
@@ -87,11 +87,13 @@ static ssize_t hidraw_read(struct file *file, char __user *buffer, size_t count,
 		len = list->buffer[list->tail].len > count ?
 			count : list->buffer[list->tail].len;
 
-		if (copy_to_user(buffer, list->buffer[list->tail].value, len)) {
-			ret = -EFAULT;
-			goto out;
+		if (list->buffer[list->tail].value) {
+			if (copy_to_user(buffer, list->buffer[list->tail].value, len)) {
+				ret = -EFAULT;
+				goto out;
+			}
+			ret = len;
 		}
-		ret = len;
 
 		kfree(list->buffer[list->tail].value);
 		list->tail = (list->tail + 1) & (HIDRAW_BUFFER_SIZE - 1);
@@ -437,19 +439,24 @@ static const struct file_operations hidraw_ops = {
 	.llseek =	noop_llseek,
 };
 
-void hidraw_report_event(struct hid_device *hid, u8 *data, int len)
+int hidraw_report_event(struct hid_device *hid, u8 *data, int len)
 {
 	struct hidraw *dev = hid->hidraw;
 	struct hidraw_list *list;
+	int ret = 0;
 
 	list_for_each_entry(list, &dev->list, node) {
-		list->buffer[list->head].value = kmemdup(data, len, GFP_ATOMIC);
+		if (!(list->buffer[list->head].value = kmemdup(data, len, GFP_ATOMIC))) {
+			ret = -ENOMEM;
+			break;
+		}
 		list->buffer[list->head].len = len;
 		list->head = (list->head + 1) & (HIDRAW_BUFFER_SIZE - 1);
 		kill_fasync(&list->fasync, SIGIO, POLL_IN);
 	}
 
 	wake_up_interruptible(&dev->wait);
+	return ret;
 }
 EXPORT_SYMBOL_GPL(hidraw_report_event);
 
diff --git a/drivers/hid/usbhid/hid-core.c b/drivers/hid/usbhid/hid-core.c
index 5bf91dbad59..482f936fc29 100644
--- a/drivers/hid/usbhid/hid-core.c
+++ b/drivers/hid/usbhid/hid-core.c
@@ -28,6 +28,7 @@
 #include <linux/input.h>
 #include <linux/wait.h>
 #include <linux/workqueue.h>
+#include <linux/string.h>
 
 #include <linux/usb.h>
 
@@ -86,8 +87,13 @@ static int hid_start_in(struct hid_device *hid)
 			!test_bit(HID_REPORTED_IDLE, &usbhid->iofl) &&
 			!test_and_set_bit(HID_IN_RUNNING, &usbhid->iofl)) {
 		rc = usb_submit_urb(usbhid->urbin, GFP_ATOMIC);
-		if (rc != 0)
+		if (rc != 0) {
 			clear_bit(HID_IN_RUNNING, &usbhid->iofl);
+			if (rc == -ENOSPC)
+				set_bit(HID_NO_BANDWIDTH, &usbhid->iofl);
+		} else {
+			clear_bit(HID_NO_BANDWIDTH, &usbhid->iofl);
+		}
 	}
 	spin_unlock_irqrestore(&usbhid->lock, flags);
 	return rc;
@@ -173,8 +179,10 @@ static void hid_io_error(struct hid_device *hid)
 
 	if (time_after(jiffies, usbhid->stop_retry)) {
 
-		/* Retries failed, so do a port reset */
-		if (!test_and_set_bit(HID_RESET_PENDING, &usbhid->iofl)) {
+		/* Retries failed, so do a port reset unless we lack bandwidth*/
+		if (test_bit(HID_NO_BANDWIDTH, &usbhid->iofl)
+		     && !test_and_set_bit(HID_RESET_PENDING, &usbhid->iofl)) {
+
 			schedule_work(&usbhid->reset_work);
 			goto done;
 		}
@@ -203,7 +211,7 @@ static int usbhid_restart_out_queue(struct usbhid_device *usbhid)
 		return 0;
 
 	if ((kicked = (usbhid->outhead != usbhid->outtail))) {
-		dbg("Kicking head %d tail %d", usbhid->outhead, usbhid->outtail);
+		hid_dbg(hid, "Kicking head %d tail %d", usbhid->outhead, usbhid->outtail);
 
 		r = usb_autopm_get_interface_async(usbhid->intf);
 		if (r < 0)
@@ -230,7 +238,7 @@ static int usbhid_restart_ctrl_queue(struct usbhid_device *usbhid)
 		return 0;
 
 	if ((kicked = (usbhid->ctrlhead != usbhid->ctrltail))) {
-		dbg("Kicking head %d tail %d", usbhid->ctrlhead, usbhid->ctrltail);
+		hid_dbg(hid, "Kicking head %d tail %d", usbhid->ctrlhead, usbhid->ctrltail);
 
 		r = usb_autopm_get_interface_async(usbhid->intf);
 		if (r < 0)
@@ -399,6 +407,16 @@ static int hid_submit_ctrl(struct hid_device *hid)
  * Output interrupt completion handler.
  */
 
+static int irq_out_pump_restart(struct hid_device *hid)
+{
+	struct usbhid_device *usbhid = hid->driver_data;
+
+	if (usbhid->outhead != usbhid->outtail)
+		return hid_submit_out(hid);
+	else
+		return -1;
+}
+
 static void hid_irq_out(struct urb *urb)
 {
 	struct hid_device *hid = urb->context;
@@ -428,7 +446,7 @@ static void hid_irq_out(struct urb *urb)
 	else
 		usbhid->outtail = (usbhid->outtail + 1) & (HID_OUTPUT_FIFO_SIZE - 1);
 
-	if (usbhid->outhead != usbhid->outtail && !hid_submit_out(hid)) {
+	if (!irq_out_pump_restart(hid)) {
 		/* Successfully submitted next urb in queue */
 		spin_unlock_irqrestore(&usbhid->lock, flags);
 		return;
@@ -443,6 +461,15 @@ static void hid_irq_out(struct urb *urb)
 /*
  * Control pipe completion handler.
  */
+static int ctrl_pump_restart(struct hid_device *hid)
+{
+	struct usbhid_device *usbhid = hid->driver_data;
+
+	if (usbhid->ctrlhead != usbhid->ctrltail)
+		return hid_submit_ctrl(hid);
+	else
+		return -1;
+}
 
 static void hid_ctrl(struct urb *urb)
 {
@@ -476,7 +503,7 @@ static void hid_ctrl(struct urb *urb)
 	else
 		usbhid->ctrltail = (usbhid->ctrltail + 1) & (HID_CONTROL_FIFO_SIZE - 1);
 
-	if (usbhid->ctrlhead != usbhid->ctrltail && !hid_submit_ctrl(hid)) {
+	if (!ctrl_pump_restart(hid)) {
 		/* Successfully submitted next urb in queue */
 		spin_unlock(&usbhid->lock);
 		return;
@@ -535,11 +562,27 @@ static void __usbhid_submit_report(struct hid_device *hid, struct hid_report *re
 			 * the queue is known to run
 			 * but an earlier request may be stuck
 			 * we may need to time out
-			 * no race because this is called under
+			 * no race because the URB is blocked under
 			 * spinlock
 			 */
-			if (time_after(jiffies, usbhid->last_out + HZ * 5))
+			if (time_after(jiffies, usbhid->last_out + HZ * 5)) {
+				usb_block_urb(usbhid->urbout);
+				/* drop lock to not deadlock if the callback is called */
+				spin_unlock(&usbhid->lock);
 				usb_unlink_urb(usbhid->urbout);
+				spin_lock(&usbhid->lock);
+				usb_unblock_urb(usbhid->urbout);
+				/*
+				 * if the unlinking has already completed
+				 * the pump will have been stopped
+				 * it must be restarted now
+				 */
+				if (!test_bit(HID_OUT_RUNNING, &usbhid->iofl))
+					if (!irq_out_pump_restart(hid))
+						set_bit(HID_OUT_RUNNING, &usbhid->iofl);
+
+
+			}
 		}
 		return;
 	}
@@ -583,11 +626,25 @@ static void __usbhid_submit_report(struct hid_device *hid, struct hid_report *re
 		 * the queue is known to run
 		 * but an earlier request may be stuck
 		 * we may need to time out
-		 * no race because this is called under
+		 * no race because the URB is blocked under
 		 * spinlock
 		 */
-		if (time_after(jiffies, usbhid->last_ctrl + HZ * 5))
+		if (time_after(jiffies, usbhid->last_ctrl + HZ * 5)) {
+			usb_block_urb(usbhid->urbctrl);
+			/* drop lock to not deadlock if the callback is called */
+			spin_unlock(&usbhid->lock);
 			usb_unlink_urb(usbhid->urbctrl);
+			spin_lock(&usbhid->lock);
+			usb_unblock_urb(usbhid->urbctrl);
+			/*
+			 * if the unlinking has already completed
+			 * the pump will have been stopped
+			 * it must be restarted now
+			 */
+			if (!test_bit(HID_CTRL_RUNNING, &usbhid->iofl))
+				if (!ctrl_pump_restart(hid))
+					set_bit(HID_CTRL_RUNNING, &usbhid->iofl);
+		}
 	}
 }
 
@@ -700,7 +757,7 @@ static int hid_get_class_descriptor(struct usb_device *dev, int ifnum,
 int usbhid_open(struct hid_device *hid)
 {
 	struct usbhid_device *usbhid = hid->driver_data;
-	int res;
+	int res = 0;
 
 	mutex_lock(&hid_open_mut);
 	if (!hid->open++) {
@@ -708,17 +765,27 @@ int usbhid_open(struct hid_device *hid)
 		/* the device must be awake to reliably request remote wakeup */
 		if (res < 0) {
 			hid->open--;
-			mutex_unlock(&hid_open_mut);
-			return -EIO;
+			res = -EIO;
+			goto done;
 		}
 		usbhid->intf->needs_remote_wakeup = 1;
-		if (hid_start_in(hid))
-			hid_io_error(hid);
- 
+		res = hid_start_in(hid);
+		if (res) {
+			if (res != -ENOSPC) {
+				hid_io_error(hid);
+				res = 0;
+			} else {
+				/* no use opening if resources are insufficient */
+				hid->open--;
+				res = -EBUSY;
+				usbhid->intf->needs_remote_wakeup = 0;
+			}
+		}
 		usb_autopm_put_interface(usbhid->intf);
 	}
+done:
 	mutex_unlock(&hid_open_mut);
-	return 0;
+	return res;
 }
 
 void usbhid_close(struct hid_device *hid)
@@ -1347,7 +1414,34 @@ static int hid_post_reset(struct usb_interface *intf)
 	struct usb_device *dev = interface_to_usbdev (intf);
 	struct hid_device *hid = usb_get_intfdata(intf);
 	struct usbhid_device *usbhid = hid->driver_data;
+	struct usb_host_interface *interface = intf->cur_altsetting;
 	int status;
+	char *rdesc;
+
+	/* Fetch and examine the HID report descriptor. If this
+	 * has changed, then rebind. Since usbcore's check of the
+	 * configuration descriptors passed, we already know that
+	 * the size of the HID report descriptor has not changed.
+	 */
+	rdesc = kmalloc(hid->rsize, GFP_KERNEL);
+	if (!rdesc) {
+		dbg_hid("couldn't allocate rdesc memory (post_reset)\n");
+		return 1;
+	}
+	status = hid_get_class_descriptor(dev,
+				interface->desc.bInterfaceNumber,
+				HID_DT_REPORT, rdesc, hid->rsize);
+	if (status < 0) {
+		dbg_hid("reading report descriptor failed (post_reset)\n");
+		kfree(rdesc);
+		return 1;
+	}
+	status = memcmp(rdesc, hid->rdesc, hid->rsize);
+	kfree(rdesc);
+	if (status != 0) {
+		dbg_hid("report descriptor changed\n");
+		return 1;
+	}
 
 	spin_lock_irq(&usbhid->lock);
 	clear_bit(HID_RESET_PENDING, &usbhid->iofl);
@@ -1504,28 +1598,15 @@ static struct usb_driver hid_driver = {
 	.supports_autosuspend = 1,
 };
 
-static const struct hid_device_id hid_usb_table[] = {
-	{ HID_USB_DEVICE(HID_ANY_ID, HID_ANY_ID) },
-	{ }
-};
-
 struct usb_interface *usbhid_find_interface(int minor)
 {
 	return usb_find_interface(&hid_driver, minor);
 }
 
-static struct hid_driver hid_usb_driver = {
-	.name = "generic-usb",
-	.id_table = hid_usb_table,
-};
-
 static int __init hid_init(void)
 {
 	int retval = -ENOMEM;
 
-	retval = hid_register_driver(&hid_usb_driver);
-	if (retval)
-		goto hid_register_fail;
 	retval = usbhid_quirks_init(quirks_param);
 	if (retval)
 		goto usbhid_quirks_init_fail;
@@ -1538,8 +1619,6 @@ static int __init hid_init(void)
 usb_register_fail:
 	usbhid_quirks_exit();
 usbhid_quirks_init_fail:
-	hid_unregister_driver(&hid_usb_driver);
-hid_register_fail:
 	return retval;
 }
 
@@ -1547,7 +1626,6 @@ static void __exit hid_exit(void)
 {
 	usb_deregister(&hid_driver);
 	usbhid_quirks_exit();
-	hid_unregister_driver(&hid_usb_driver);
 }
 
 module_init(hid_init);
diff --git a/drivers/hid/usbhid/hid-quirks.c b/drivers/hid/usbhid/hid-quirks.c
index 782c63955f2..0597ee604f6 100644
--- a/drivers/hid/usbhid/hid-quirks.c
+++ b/drivers/hid/usbhid/hid-quirks.c
@@ -88,6 +88,7 @@ static const struct hid_blacklist {
 	{ USB_VENDOR_ID_UCLOGIC, USB_DEVICE_ID_UCLOGIC_TABLET_WP8060U, HID_QUIRK_MULTI_INPUT },
 	{ USB_VENDOR_ID_WALTOP, USB_DEVICE_ID_WALTOP_MEDIA_TABLET_10_6_INCH, HID_QUIRK_MULTI_INPUT },
 	{ USB_VENDOR_ID_WALTOP, USB_DEVICE_ID_WALTOP_MEDIA_TABLET_14_1_INCH, HID_QUIRK_MULTI_INPUT },
+	{ USB_VENDOR_ID_WALTOP, USB_DEVICE_ID_WALTOP_SIRIUS_BATTERY_FREE_TABLET, HID_QUIRK_MULTI_INPUT },
 	{ USB_VENDOR_ID_WISEGROUP, USB_DEVICE_ID_QUAD_USB_JOYPAD, HID_QUIRK_NOGET | HID_QUIRK_MULTI_INPUT },
 
 	{ USB_VENDOR_ID_WISEGROUP_LTD2, USB_DEVICE_ID_SMARTJOY_DUAL_PLUS, HID_QUIRK_NOGET | HID_QUIRK_MULTI_INPUT },
diff --git a/drivers/hid/usbhid/hiddev.c b/drivers/hid/usbhid/hiddev.c
index b1ec0e2aeb5..14599e25679 100644
--- a/drivers/hid/usbhid/hiddev.c
+++ b/drivers/hid/usbhid/hiddev.c
@@ -34,6 +34,7 @@
 #include <linux/hid.h>
 #include <linux/hiddev.h>
 #include <linux/compat.h>
+#include <linux/vmalloc.h>
 #include "usbhid.h"
 
 #ifdef CONFIG_USB_DYNAMIC_MINORS
@@ -250,13 +251,13 @@ static int hiddev_release(struct inode * inode, struct file * file)
 		} else {
 			mutex_unlock(&list->hiddev->existancelock);
 			kfree(list->hiddev);
-			kfree(list);
+			vfree(list);
 			return 0;
 		}
 	}
 
 	mutex_unlock(&list->hiddev->existancelock);
-	kfree(list);
+	vfree(list);
 
 	return 0;
 }
@@ -278,7 +279,7 @@ static int hiddev_open(struct inode *inode, struct file *file)
 	hid = usb_get_intfdata(intf);
 	hiddev = hid->hiddev;
 
-	if (!(list = kzalloc(sizeof(struct hiddev_list), GFP_KERNEL)))
+	if (!(list = vzalloc(sizeof(struct hiddev_list))))
 		return -ENOMEM;
 	mutex_init(&list->thread_lock);
 	list->hiddev = hiddev;
@@ -322,7 +323,7 @@ bail_unlock:
 	mutex_unlock(&hiddev->existancelock);
 bail:
 	file->private_data = NULL;
-	kfree(list);
+	vfree(list);
 	return res;
 }
 
diff --git a/drivers/hid/usbhid/usbhid.h b/drivers/hid/usbhid/usbhid.h
index cb8f703efde..1883d7b9487 100644
--- a/drivers/hid/usbhid/usbhid.h
+++ b/drivers/hid/usbhid/usbhid.h
@@ -55,6 +55,7 @@ struct usb_interface *usbhid_find_interface(int minor);
 #define HID_STARTED		8
 #define HID_REPORTED_IDLE	9
 #define HID_KEYS_PRESSED	10
+#define HID_NO_BANDWIDTH	11
 
 /*
  * USB-specific HID struct, to be pointed to
diff --git a/drivers/hid/usbhid/usbmouse.c b/drivers/hid/usbhid/usbmouse.c
index 0f6be45d43d..bf16d72dc37 100644
--- a/drivers/hid/usbhid/usbmouse.c
+++ b/drivers/hid/usbhid/usbmouse.c
@@ -92,9 +92,10 @@ static void usb_mouse_irq(struct urb *urb)
 resubmit:
 	status = usb_submit_urb (urb, GFP_ATOMIC);
 	if (status)
-		err ("can't resubmit intr, %s-%s/input0, status %d",
-				mouse->usbdev->bus->bus_name,
-				mouse->usbdev->devpath, status);
+		dev_err(&mouse->usbdev->dev,
+			"can't resubmit intr, %s-%s/input0, status %d\n",
+			mouse->usbdev->bus->bus_name,
+			mouse->usbdev->devpath, status);
 }
 
 static int usb_mouse_open(struct input_dev *dev)
diff --git a/drivers/hv/channel_mgmt.c b/drivers/hv/channel_mgmt.c
index 9ffbfc575a0..2b8b8d4558d 100644
--- a/drivers/hv/channel_mgmt.c
+++ b/drivers/hv/channel_mgmt.c
@@ -46,40 +46,61 @@ struct vmbus_channel_message_table_entry {
  *
  * @icmsghdrp is of type &struct icmsg_hdr.
  * @negop is of type &struct icmsg_negotiate.
- * Set up and fill in default negotiate response message. This response can
- * come from both the vmbus driver and the hv_utils driver. The current api
- * will respond properly to both Windows 2008 and Windows 2008-R2 operating
- * systems.
+ * Set up and fill in default negotiate response message.
+ *
+ * The max_fw_version specifies the maximum framework version that
+ * we can support and max _srv_version specifies the maximum service
+ * version we can support. A special value MAX_SRV_VER can be
+ * specified to indicate that we can handle the maximum version
+ * exposed by the host.
  *
  * Mainly used by Hyper-V drivers.
  */
 void vmbus_prep_negotiate_resp(struct icmsg_hdr *icmsghdrp,
-			       struct icmsg_negotiate *negop, u8 *buf)
+				struct icmsg_negotiate *negop, u8 *buf,
+				int max_fw_version, int max_srv_version)
 {
-	if (icmsghdrp->icmsgtype == ICMSGTYPE_NEGOTIATE) {
-		icmsghdrp->icmsgsize = 0x10;
-
-		negop = (struct icmsg_negotiate *)&buf[
-			sizeof(struct vmbuspipe_hdr) +
-			sizeof(struct icmsg_hdr)];
-
-		if (negop->icframe_vercnt == 2 &&
-		   negop->icversion_data[1].major == 3) {
-			negop->icversion_data[0].major = 3;
-			negop->icversion_data[0].minor = 0;
-			negop->icversion_data[1].major = 3;
-			negop->icversion_data[1].minor = 0;
-		} else {
-			negop->icversion_data[0].major = 1;
-			negop->icversion_data[0].minor = 0;
-			negop->icversion_data[1].major = 1;
-			negop->icversion_data[1].minor = 0;
-		}
+	int icframe_vercnt;
+	int icmsg_vercnt;
+	int i;
+
+	icmsghdrp->icmsgsize = 0x10;
+
+	negop = (struct icmsg_negotiate *)&buf[
+		sizeof(struct vmbuspipe_hdr) +
+		sizeof(struct icmsg_hdr)];
+
+	icframe_vercnt = negop->icframe_vercnt;
+	icmsg_vercnt = negop->icmsg_vercnt;
+
+	/*
+	 * Select the framework version number we will
+	 * support.
+	 */
+
+	for (i = 0; i < negop->icframe_vercnt; i++) {
+		if (negop->icversion_data[i].major <= max_fw_version)
+			icframe_vercnt = negop->icversion_data[i].major;
+	}
 
-		negop->icframe_vercnt = 1;
-		negop->icmsg_vercnt = 1;
+	for (i = negop->icframe_vercnt;
+		 (i < negop->icframe_vercnt + negop->icmsg_vercnt); i++) {
+		if (negop->icversion_data[i].major <= max_srv_version)
+			icmsg_vercnt = negop->icversion_data[i].major;
 	}
+
+	/*
+	 * Respond with the maximum framework and service
+	 * version numbers we can support.
+	 */
+	negop->icframe_vercnt = 1;
+	negop->icmsg_vercnt = 1;
+	negop->icversion_data[0].major = icframe_vercnt;
+	negop->icversion_data[0].minor = 0;
+	negop->icversion_data[1].major = icmsg_vercnt;
+	negop->icversion_data[1].minor = 0;
 }
+
 EXPORT_SYMBOL_GPL(vmbus_prep_negotiate_resp);
 
 /*
diff --git a/drivers/hv/hv.c b/drivers/hv/hv.c
index 15956bd48b4..86f8885aeb4 100644
--- a/drivers/hv/hv.c
+++ b/drivers/hv/hv.c
@@ -252,7 +252,7 @@ void hv_cleanup(void)
  *
  * This involves a hypercall.
  */
-u16 hv_post_message(union hv_connection_id connection_id,
+int hv_post_message(union hv_connection_id connection_id,
 		  enum hv_message_type message_type,
 		  void *payload, size_t payload_size)
 {
diff --git a/drivers/hv/hv_kvp.c b/drivers/hv/hv_kvp.c
index 6186025209c..0012eed6d87 100644
--- a/drivers/hv/hv_kvp.c
+++ b/drivers/hv/hv_kvp.c
@@ -394,7 +394,8 @@ void hv_kvp_onchannelcallback(void *context)
 			sizeof(struct vmbuspipe_hdr)];
 
 		if (icmsghdrp->icmsgtype == ICMSGTYPE_NEGOTIATE) {
-			vmbus_prep_negotiate_resp(icmsghdrp, negop, recv_buffer);
+			vmbus_prep_negotiate_resp(icmsghdrp, negop,
+				 recv_buffer, MAX_SRV_VER, MAX_SRV_VER);
 		} else {
 			kvp_msg = (struct hv_kvp_msg *)&recv_buffer[
 				sizeof(struct vmbuspipe_hdr) +
diff --git a/drivers/hv/hv_util.c b/drivers/hv/hv_util.c
index dbb8b8eec21..d3ac6a40118 100644
--- a/drivers/hv/hv_util.c
+++ b/drivers/hv/hv_util.c
@@ -70,7 +70,8 @@ static void shutdown_onchannelcallback(void *context)
 			sizeof(struct vmbuspipe_hdr)];
 
 		if (icmsghdrp->icmsgtype == ICMSGTYPE_NEGOTIATE) {
-			vmbus_prep_negotiate_resp(icmsghdrp, negop, shut_txf_buf);
+			vmbus_prep_negotiate_resp(icmsghdrp, negop,
+					shut_txf_buf, MAX_SRV_VER, MAX_SRV_VER);
 		} else {
 			shutdown_msg =
 				(struct shutdown_msg_data *)&shut_txf_buf[
@@ -195,7 +196,8 @@ static void timesync_onchannelcallback(void *context)
 				sizeof(struct vmbuspipe_hdr)];
 
 		if (icmsghdrp->icmsgtype == ICMSGTYPE_NEGOTIATE) {
-			vmbus_prep_negotiate_resp(icmsghdrp, NULL, time_txf_buf);
+			vmbus_prep_negotiate_resp(icmsghdrp, NULL, time_txf_buf,
+						MAX_SRV_VER, MAX_SRV_VER);
 		} else {
 			timedatap = (struct ictimesync_data *)&time_txf_buf[
 				sizeof(struct vmbuspipe_hdr) +
@@ -234,7 +236,8 @@ static void heartbeat_onchannelcallback(void *context)
 				sizeof(struct vmbuspipe_hdr)];
 
 		if (icmsghdrp->icmsgtype == ICMSGTYPE_NEGOTIATE) {
-			vmbus_prep_negotiate_resp(icmsghdrp, NULL, hbeat_txf_buf);
+			vmbus_prep_negotiate_resp(icmsghdrp, NULL,
+				hbeat_txf_buf, MAX_SRV_VER, MAX_SRV_VER);
 		} else {
 			heartbeat_msg =
 				(struct heartbeat_msg_data *)&hbeat_txf_buf[
diff --git a/drivers/hv/hyperv_vmbus.h b/drivers/hv/hyperv_vmbus.h
index 699f0d8e59e..b9426a6592e 100644
--- a/drivers/hv/hyperv_vmbus.h
+++ b/drivers/hv/hyperv_vmbus.h
@@ -495,7 +495,7 @@ extern int hv_init(void);
 
 extern void hv_cleanup(void);
 
-extern u16 hv_post_message(union hv_connection_id connection_id,
+extern int hv_post_message(union hv_connection_id connection_id,
 			 enum hv_message_type message_type,
 			 void *payload, size_t payload_size);
 
diff --git a/drivers/hv/ring_buffer.c b/drivers/hv/ring_buffer.c
index 8af25a097d7..7233c88f01b 100644
--- a/drivers/hv/ring_buffer.c
+++ b/drivers/hv/ring_buffer.c
@@ -30,37 +30,6 @@
 #include "hyperv_vmbus.h"
 
 
-/* #defines */
-
-
-/* Amount of space to write to */
-#define BYTES_AVAIL_TO_WRITE(r, w, z) \
-	((w) >= (r)) ? ((z) - ((w) - (r))) : ((r) - (w))
-
-
-/*
- *
- * hv_get_ringbuffer_availbytes()
- *
- * Get number of bytes available to read and to write to
- * for the specified ring buffer
- */
-static inline void
-hv_get_ringbuffer_availbytes(struct hv_ring_buffer_info *rbi,
-			  u32 *read, u32 *write)
-{
-	u32 read_loc, write_loc;
-
-	smp_read_barrier_depends();
-
-	/* Capture the read/write indices before they changed */
-	read_loc = rbi->ring_buffer->read_index;
-	write_loc = rbi->ring_buffer->write_index;
-
-	*write = BYTES_AVAIL_TO_WRITE(read_loc, write_loc, rbi->ring_datasize);
-	*read = rbi->ring_datasize - *write;
-}
-
 /*
  * hv_get_next_write_location()
  *
diff --git a/drivers/hwmon/Kconfig b/drivers/hwmon/Kconfig
index 8deedc1b984..7cd9bf42108 100644
--- a/drivers/hwmon/Kconfig
+++ b/drivers/hwmon/Kconfig
@@ -318,7 +318,7 @@ config SENSORS_EXYNOS4_TMU
 	tristate "Temperature sensor on Samsung EXYNOS4"
 	depends on ARCH_EXYNOS4
 	help
-	  If you say yes here you get support for TMU (Thermal Managment
+	  If you say yes here you get support for TMU (Thermal Management
 	  Unit) on SAMSUNG EXYNOS4 series of SoC.
 
 	  This driver can also be built as a module. If so, the module
@@ -1102,6 +1102,19 @@ config SENSORS_AMC6821
 	  This driver can also be build as a module.  If so, the module
 	  will be called amc6821.
 
+config SENSORS_INA2XX
+	tristate "Texas Instruments INA219, INA226"
+	depends on I2C && EXPERIMENTAL
+	help
+	  If you say yes here you get support for INA219 and INA226 power
+	  monitor chips.
+
+	  The INA2xx driver is configured for the default configuration of
+	  the part as described in the datasheet.
+	  Default value for Rshunt is 10 mOhms.
+	  This driver can also be built as a module.  If so, the module
+	  will be called ina2xx.
+
 config SENSORS_THMC50
 	tristate "Texas Instruments THMC50 / Analog Devices ADM1022"
 	depends on I2C
diff --git a/drivers/hwmon/Makefile b/drivers/hwmon/Makefile
index 6d3f11f7181..e1eeac13b85 100644
--- a/drivers/hwmon/Makefile
+++ b/drivers/hwmon/Makefile
@@ -62,6 +62,7 @@ obj-$(CONFIG_SENSORS_ULTRA45)	+= ultra45_env.o
 obj-$(CONFIG_SENSORS_I5K_AMB)	+= i5k_amb.o
 obj-$(CONFIG_SENSORS_IBMAEM)	+= ibmaem.o
 obj-$(CONFIG_SENSORS_IBMPEX)	+= ibmpex.o
+obj-$(CONFIG_SENSORS_INA2XX)	+= ina2xx.o
 obj-$(CONFIG_SENSORS_IT87)	+= it87.o
 obj-$(CONFIG_SENSORS_JC42)	+= jc42.o
 obj-$(CONFIG_SENSORS_JZ4740)	+= jz4740-hwmon.o
diff --git a/drivers/hwmon/acpi_power_meter.c b/drivers/hwmon/acpi_power_meter.c
index 9140236a018..34ad5a27a7e 100644
--- a/drivers/hwmon/acpi_power_meter.c
+++ b/drivers/hwmon/acpi_power_meter.c
@@ -107,15 +107,7 @@ struct acpi_power_meter_resource {
 	struct kobject		*holders_dir;
 };
 
-struct ro_sensor_template {
-	char *label;
-	ssize_t (*show)(struct device *dev,
-			struct device_attribute *devattr,
-			char *buf);
-	int index;
-};
-
-struct rw_sensor_template {
+struct sensor_template {
 	char *label;
 	ssize_t (*show)(struct device *dev,
 			struct device_attribute *devattr,
@@ -469,52 +461,67 @@ static ssize_t show_name(struct device *dev,
 	return sprintf(buf, "%s\n", ACPI_POWER_METER_NAME);
 }
 
+#define RO_SENSOR_TEMPLATE(_label, _show, _index)	\
+	{						\
+		.label = _label,			\
+		.show  = _show,				\
+		.index = _index,			\
+	}
+
+#define RW_SENSOR_TEMPLATE(_label, _show, _set, _index)	\
+	{						\
+		.label = _label,			\
+		.show  = _show,				\
+		.set   = _set,				\
+		.index = _index,			\
+	}
+
 /* Sensor descriptions.  If you add a sensor, update NUM_SENSORS above! */
-static struct ro_sensor_template meter_ro_attrs[] = {
-{POWER_AVERAGE_NAME, show_power, 0},
-{"power1_accuracy", show_accuracy, 0},
-{"power1_average_interval_min", show_val, 0},
-{"power1_average_interval_max", show_val, 1},
-{"power1_is_battery", show_val, 5},
-{NULL, NULL, 0},
+static struct sensor_template meter_attrs[] = {
+	RO_SENSOR_TEMPLATE(POWER_AVERAGE_NAME, show_power, 0),
+	RO_SENSOR_TEMPLATE("power1_accuracy", show_accuracy, 0),
+	RO_SENSOR_TEMPLATE("power1_average_interval_min", show_val, 0),
+	RO_SENSOR_TEMPLATE("power1_average_interval_max", show_val, 1),
+	RO_SENSOR_TEMPLATE("power1_is_battery", show_val, 5),
+	RW_SENSOR_TEMPLATE(POWER_AVG_INTERVAL_NAME, show_avg_interval,
+		set_avg_interval, 0),
+	{},
 };
 
-static struct rw_sensor_template meter_rw_attrs[] = {
-{POWER_AVG_INTERVAL_NAME, show_avg_interval, set_avg_interval, 0},
-{NULL, NULL, NULL, 0},
+static struct sensor_template misc_cap_attrs[] = {
+	RO_SENSOR_TEMPLATE("power1_cap_min", show_val, 2),
+	RO_SENSOR_TEMPLATE("power1_cap_max", show_val, 3),
+	RO_SENSOR_TEMPLATE("power1_cap_hyst", show_val, 4),
+	RO_SENSOR_TEMPLATE(POWER_ALARM_NAME, show_val, 6),
+	{},
 };
 
-static struct ro_sensor_template misc_cap_attrs[] = {
-{"power1_cap_min", show_val, 2},
-{"power1_cap_max", show_val, 3},
-{"power1_cap_hyst", show_val, 4},
-{POWER_ALARM_NAME, show_val, 6},
-{NULL, NULL, 0},
+static struct sensor_template ro_cap_attrs[] = {
+	RO_SENSOR_TEMPLATE(POWER_CAP_NAME, show_cap, 0),
+	{},
 };
 
-static struct ro_sensor_template ro_cap_attrs[] = {
-{POWER_CAP_NAME, show_cap, 0},
-{NULL, NULL, 0},
+static struct sensor_template rw_cap_attrs[] = {
+	RW_SENSOR_TEMPLATE(POWER_CAP_NAME, show_cap, set_cap, 0),
+	{},
 };
 
-static struct rw_sensor_template rw_cap_attrs[] = {
-{POWER_CAP_NAME, show_cap, set_cap, 0},
-{NULL, NULL, NULL, 0},
+static struct sensor_template trip_attrs[] = {
+	RW_SENSOR_TEMPLATE("power1_average_min", show_val, set_trip, 7),
+	RW_SENSOR_TEMPLATE("power1_average_max", show_val, set_trip, 8),
+	{},
 };
 
-static struct rw_sensor_template trip_attrs[] = {
-{"power1_average_min", show_val, set_trip, 7},
-{"power1_average_max", show_val, set_trip, 8},
-{NULL, NULL, NULL, 0},
+static struct sensor_template misc_attrs[] = {
+	RO_SENSOR_TEMPLATE("name", show_name, 0),
+	RO_SENSOR_TEMPLATE("power1_model_number", show_str, 0),
+	RO_SENSOR_TEMPLATE("power1_oem_info", show_str, 2),
+	RO_SENSOR_TEMPLATE("power1_serial_number", show_str, 1),
+	{},
 };
 
-static struct ro_sensor_template misc_attrs[] = {
-{"name", show_name, 0},
-{"power1_model_number", show_str, 0},
-{"power1_oem_info", show_str, 2},
-{"power1_serial_number", show_str, 1},
-{NULL, NULL, 0},
-};
+#undef RO_SENSOR_TEMPLATE
+#undef RW_SENSOR_TEMPLATE
 
 /* Read power domain data */
 static void remove_domain_devices(struct acpi_power_meter_resource *resource)
@@ -619,49 +626,24 @@ end:
 }
 
 /* Registration and deregistration */
-static int register_ro_attrs(struct acpi_power_meter_resource *resource,
-			     struct ro_sensor_template *ro)
+static int register_attrs(struct acpi_power_meter_resource *resource,
+			  struct sensor_template *attrs)
 {
 	struct device *dev = &resource->acpi_dev->dev;
 	struct sensor_device_attribute *sensors =
 		&resource->sensors[resource->num_sensors];
 	int res = 0;
 
-	while (ro->label) {
-		sensors->dev_attr.attr.name = ro->label;
+	while (attrs->label) {
+		sensors->dev_attr.attr.name = attrs->label;
 		sensors->dev_attr.attr.mode = S_IRUGO;
-		sensors->dev_attr.show = ro->show;
-		sensors->index = ro->index;
+		sensors->dev_attr.show = attrs->show;
+		sensors->index = attrs->index;
 
-		sysfs_attr_init(&sensors->dev_attr.attr);
-		res = device_create_file(dev, &sensors->dev_attr);
-		if (res) {
-			sensors->dev_attr.attr.name = NULL;
-			goto error;
+		if (attrs->set) {
+			sensors->dev_attr.attr.mode |= S_IWUSR;
+			sensors->dev_attr.store = attrs->set;
 		}
-		sensors++;
-		resource->num_sensors++;
-		ro++;
-	}
-
-error:
-	return res;
-}
-
-static int register_rw_attrs(struct acpi_power_meter_resource *resource,
-			     struct rw_sensor_template *rw)
-{
-	struct device *dev = &resource->acpi_dev->dev;
-	struct sensor_device_attribute *sensors =
-		&resource->sensors[resource->num_sensors];
-	int res = 0;
-
-	while (rw->label) {
-		sensors->dev_attr.attr.name = rw->label;
-		sensors->dev_attr.attr.mode = S_IRUGO | S_IWUSR;
-		sensors->dev_attr.show = rw->show;
-		sensors->dev_attr.store = rw->set;
-		sensors->index = rw->index;
 
 		sysfs_attr_init(&sensors->dev_attr.attr);
 		res = device_create_file(dev, &sensors->dev_attr);
@@ -671,7 +653,7 @@ static int register_rw_attrs(struct acpi_power_meter_resource *resource,
 		}
 		sensors++;
 		resource->num_sensors++;
-		rw++;
+		attrs++;
 	}
 
 error:
@@ -703,10 +685,7 @@ static int setup_attrs(struct acpi_power_meter_resource *resource)
 		return res;
 
 	if (resource->caps.flags & POWER_METER_CAN_MEASURE) {
-		res = register_ro_attrs(resource, meter_ro_attrs);
-		if (res)
-			goto error;
-		res = register_rw_attrs(resource, meter_rw_attrs);
+		res = register_attrs(resource, meter_attrs);
 		if (res)
 			goto error;
 	}
@@ -718,28 +697,27 @@ static int setup_attrs(struct acpi_power_meter_resource *resource)
 			goto skip_unsafe_cap;
 		}
 
-		if (resource->caps.configurable_cap) {
-			res = register_rw_attrs(resource, rw_cap_attrs);
-			if (res)
-				goto error;
-		} else {
-			res = register_ro_attrs(resource, ro_cap_attrs);
-			if (res)
-				goto error;
-		}
-		res = register_ro_attrs(resource, misc_cap_attrs);
+		if (resource->caps.configurable_cap)
+			res = register_attrs(resource, rw_cap_attrs);
+		else
+			res = register_attrs(resource, ro_cap_attrs);
+
+		if (res)
+			goto error;
+
+		res = register_attrs(resource, misc_cap_attrs);
 		if (res)
 			goto error;
 	}
-skip_unsafe_cap:
 
+skip_unsafe_cap:
 	if (resource->caps.flags & POWER_METER_CAN_TRIP) {
-		res = register_rw_attrs(resource, trip_attrs);
+		res = register_attrs(resource, trip_attrs);
 		if (res)
 			goto error;
 	}
 
-	res = register_ro_attrs(resource, misc_attrs);
+	res = register_attrs(resource, misc_attrs);
 	if (res)
 		goto error;
 
diff --git a/drivers/hwmon/ad7314.c b/drivers/hwmon/ad7314.c
index f85ce70d967..cfec802cf9c 100644
--- a/drivers/hwmon/ad7314.c
+++ b/drivers/hwmon/ad7314.c
@@ -18,21 +18,14 @@
 #include <linux/hwmon-sysfs.h>
 
 /*
- * AD7314 power mode
- */
-#define AD7314_PD		0x2000
-
-/*
  * AD7314 temperature masks
  */
-#define AD7314_TEMP_SIGN		0x200
 #define AD7314_TEMP_MASK		0x7FE0
-#define AD7314_TEMP_OFFSET		5
+#define AD7314_TEMP_SHIFT		5
 
 /*
  * ADT7301 and ADT7302 temperature masks
  */
-#define ADT7301_TEMP_SIGN		0x2000
 #define ADT7301_TEMP_MASK		0x3FFF
 
 enum ad7314_variant {
@@ -73,7 +66,7 @@ static ssize_t ad7314_show_temperature(struct device *dev,
 		return ret;
 	switch (spi_get_device_id(chip->spi_dev)->driver_data) {
 	case ad7314:
-		data = (ret & AD7314_TEMP_MASK) >> AD7314_TEMP_OFFSET;
+		data = (ret & AD7314_TEMP_MASK) >> AD7314_TEMP_SHIFT;
 		data = (data << 6) >> 6;
 
 		return sprintf(buf, "%d\n", 250 * data);
diff --git a/drivers/hwmon/fam15h_power.c b/drivers/hwmon/fam15h_power.c
index e8e18cab1fb..6b13f1a4dc2 100644
--- a/drivers/hwmon/fam15h_power.c
+++ b/drivers/hwmon/fam15h_power.c
@@ -257,15 +257,4 @@ static struct pci_driver fam15h_power_driver = {
 	.remove = __devexit_p(fam15h_power_remove),
 };
 
-static int __init fam15h_power_init(void)
-{
-	return pci_register_driver(&fam15h_power_driver);
-}
-
-static void __exit fam15h_power_exit(void)
-{
-	pci_unregister_driver(&fam15h_power_driver);
-}
-
-module_init(fam15h_power_init)
-module_exit(fam15h_power_exit)
+module_pci_driver(fam15h_power_driver);
diff --git a/drivers/hwmon/ina2xx.c b/drivers/hwmon/ina2xx.c
new file mode 100644
index 00000000000..7f3f4a38572
--- /dev/null
+++ b/drivers/hwmon/ina2xx.c
@@ -0,0 +1,368 @@
+/*
+ * Driver for Texas Instruments INA219, INA226 power monitor chips
+ *
+ * INA219:
+ * Zero Drift Bi-Directional Current/Power Monitor with I2C Interface
+ * Datasheet: http://www.ti.com/product/ina219
+ *
+ * INA226:
+ * Bi-Directional Current/Power Monitor with I2C Interface
+ * Datasheet: http://www.ti.com/product/ina226
+ *
+ * Copyright (C) 2012 Lothar Felten <l-felten@ti.com>
+ * Thanks to Jan Volkering
+ *
+ * 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.
+ */
+
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/init.h>
+#include <linux/err.h>
+#include <linux/slab.h>
+#include <linux/i2c.h>
+#include <linux/hwmon.h>
+#include <linux/hwmon-sysfs.h>
+
+#include <linux/platform_data/ina2xx.h>
+
+/* common register definitions */
+#define INA2XX_CONFIG			0x00
+#define INA2XX_SHUNT_VOLTAGE		0x01 /* readonly */
+#define INA2XX_BUS_VOLTAGE		0x02 /* readonly */
+#define INA2XX_POWER			0x03 /* readonly */
+#define INA2XX_CURRENT			0x04 /* readonly */
+#define INA2XX_CALIBRATION		0x05
+
+/* INA226 register definitions */
+#define INA226_MASK_ENABLE		0x06
+#define INA226_ALERT_LIMIT		0x07
+#define INA226_DIE_ID			0xFF
+
+
+/* register count */
+#define INA219_REGISTERS		6
+#define INA226_REGISTERS		8
+
+#define INA2XX_MAX_REGISTERS		8
+
+/* settings - depend on use case */
+#define INA219_CONFIG_DEFAULT		0x399F	/* PGA=8 */
+#define INA226_CONFIG_DEFAULT		0x4527	/* averages=16 */
+
+/* worst case is 68.10 ms (~14.6Hz, ina219) */
+#define INA2XX_CONVERSION_RATE		15
+
+enum ina2xx_ids { ina219, ina226 };
+
+struct ina2xx_data {
+	struct device *hwmon_dev;
+
+	struct mutex update_lock;
+	bool valid;
+	unsigned long last_updated;
+
+	int kind;
+	int registers;
+	u16 regs[INA2XX_MAX_REGISTERS];
+};
+
+int ina2xx_read_word(struct i2c_client *client, int reg)
+{
+	int val = i2c_smbus_read_word_data(client, reg);
+	if (unlikely(val < 0)) {
+		dev_dbg(&client->dev,
+			"Failed to read register: %d\n", reg);
+		return val;
+	}
+	return be16_to_cpu(val);
+}
+
+void ina2xx_write_word(struct i2c_client *client, int reg, int data)
+{
+	i2c_smbus_write_word_data(client, reg, cpu_to_be16(data));
+}
+
+static struct ina2xx_data *ina2xx_update_device(struct device *dev)
+{
+	struct i2c_client *client = to_i2c_client(dev);
+	struct ina2xx_data *data = i2c_get_clientdata(client);
+	struct ina2xx_data *ret = data;
+
+	mutex_lock(&data->update_lock);
+
+	if (time_after(jiffies, data->last_updated +
+		       HZ / INA2XX_CONVERSION_RATE) || !data->valid) {
+
+		int i;
+
+		dev_dbg(&client->dev, "Starting ina2xx update\n");
+
+		/* Read all registers */
+		for (i = 0; i < data->registers; i++) {
+			int rv = ina2xx_read_word(client, i);
+			if (rv < 0) {
+				ret = ERR_PTR(rv);
+				goto abort;
+			}
+			data->regs[i] = rv;
+		}
+		data->last_updated = jiffies;
+		data->valid = 1;
+	}
+abort:
+	mutex_unlock(&data->update_lock);
+	return ret;
+}
+
+static int ina219_get_value(struct ina2xx_data *data, u8 reg)
+{
+	/*
+	 * calculate exact value for the given register
+	 * we assume default power-on reset settings:
+	 * bus voltage range 32V
+	 * gain = /8
+	 * adc 1 & 2 -> conversion time 532uS
+	 * mode is continuous shunt and bus
+	 * calibration value is INA219_CALIBRATION_VALUE
+	 */
+	int val = data->regs[reg];
+
+	switch (reg) {
+	case INA2XX_SHUNT_VOLTAGE:
+		/* LSB=10uV. Convert to mV. */
+		val = DIV_ROUND_CLOSEST(val, 100);
+		break;
+	case INA2XX_BUS_VOLTAGE:
+		/* LSB=4mV. Register is not right aligned, convert to mV. */
+		val = (val >> 3) * 4;
+		break;
+	case INA2XX_POWER:
+		/* LSB=20mW. Convert to uW */
+		val = val * 20 * 1000;
+		break;
+	case INA2XX_CURRENT:
+		/* LSB=1mA (selected). Is in mA */
+		break;
+	default:
+		/* programmer goofed */
+		WARN_ON_ONCE(1);
+		val = 0;
+		break;
+	}
+
+	return val;
+}
+
+static int ina226_get_value(struct ina2xx_data *data, u8 reg)
+{
+	/*
+	 * calculate exact value for the given register
+	 * we assume default power-on reset settings:
+	 * bus voltage range 32V
+	 * gain = /8
+	 * adc 1 & 2 -> conversion time 532uS
+	 * mode is continuous shunt and bus
+	 * calibration value is INA226_CALIBRATION_VALUE
+	 */
+	int val = data->regs[reg];
+
+	switch (reg) {
+	case INA2XX_SHUNT_VOLTAGE:
+		/* LSB=2.5uV. Convert to mV. */
+		val = DIV_ROUND_CLOSEST(val, 400);
+		break;
+	case INA2XX_BUS_VOLTAGE:
+		/* LSB=1.25mV. Convert to mV. */
+		val = val + DIV_ROUND_CLOSEST(val, 4);
+		break;
+	case INA2XX_POWER:
+		/* LSB=25mW. Convert to uW */
+		val = val * 25 * 1000;
+		break;
+	case INA2XX_CURRENT:
+		/* LSB=1mA (selected). Is in mA */
+		break;
+	default:
+		/* programmer goofed */
+		WARN_ON_ONCE(1);
+		val = 0;
+		break;
+	}
+
+	return val;
+}
+
+static ssize_t ina2xx_show_value(struct device *dev,
+				 struct device_attribute *da, char *buf)
+{
+	struct sensor_device_attribute *attr = to_sensor_dev_attr(da);
+	struct ina2xx_data *data = ina2xx_update_device(dev);
+	int value = 0;
+
+	if (IS_ERR(data))
+		return PTR_ERR(data);
+
+	switch (data->kind) {
+	case ina219:
+		value = ina219_get_value(data, attr->index);
+		break;
+	case ina226:
+		value = ina226_get_value(data, attr->index);
+		break;
+	default:
+		WARN_ON_ONCE(1);
+		break;
+	}
+	return snprintf(buf, PAGE_SIZE, "%d\n", value);
+}
+
+/* shunt voltage */
+static SENSOR_DEVICE_ATTR(in0_input, S_IRUGO, \
+	ina2xx_show_value, NULL, INA2XX_SHUNT_VOLTAGE);
+
+/* bus voltage */
+static SENSOR_DEVICE_ATTR(in1_input, S_IRUGO, \
+	ina2xx_show_value, NULL, INA2XX_BUS_VOLTAGE);
+
+/* calculated current */
+static SENSOR_DEVICE_ATTR(curr1_input, S_IRUGO, \
+	ina2xx_show_value, NULL, INA2XX_CURRENT);
+
+/* calculated power */
+static SENSOR_DEVICE_ATTR(power1_input, S_IRUGO, \
+	ina2xx_show_value, NULL, INA2XX_POWER);
+
+/* pointers to created device attributes */
+static struct attribute *ina2xx_attributes[] = {
+	&sensor_dev_attr_in0_input.dev_attr.attr,
+	&sensor_dev_attr_in1_input.dev_attr.attr,
+	&sensor_dev_attr_curr1_input.dev_attr.attr,
+	&sensor_dev_attr_power1_input.dev_attr.attr,
+	NULL,
+};
+
+static const struct attribute_group ina2xx_group = {
+	.attrs = ina2xx_attributes,
+};
+
+static int ina2xx_probe(struct i2c_client *client,
+			const struct i2c_device_id *id)
+{
+	struct i2c_adapter *adapter = client->adapter;
+	struct ina2xx_data *data;
+	struct ina2xx_platform_data *pdata;
+	int ret = 0;
+	long shunt = 10000; /* default shunt value 10mOhms */
+
+	if (!i2c_check_functionality(adapter, I2C_FUNC_SMBUS_WORD_DATA))
+		return -ENODEV;
+
+	data = devm_kzalloc(&client->dev, sizeof(*data), GFP_KERNEL);
+	if (!data)
+		return -ENOMEM;
+
+	if (client->dev.platform_data) {
+		pdata =
+		  (struct ina2xx_platform_data *)client->dev.platform_data;
+		shunt = pdata->shunt_uohms;
+	}
+
+	if (shunt <= 0)
+		return -ENODEV;
+
+	/* set the device type */
+	data->kind = id->driver_data;
+
+	switch (data->kind) {
+	case ina219:
+		/* device configuration */
+		ina2xx_write_word(client, INA2XX_CONFIG, INA219_CONFIG_DEFAULT);
+
+		/* set current LSB to 1mA, shunt is in uOhms */
+		/* (equation 13 in datasheet) */
+		ina2xx_write_word(client, INA2XX_CALIBRATION, 40960000 / shunt);
+		dev_info(&client->dev,
+			 "power monitor INA219 (Rshunt = %li uOhm)\n", shunt);
+		data->registers = INA219_REGISTERS;
+		break;
+	case ina226:
+		/* device configuration */
+		ina2xx_write_word(client, INA2XX_CONFIG, INA226_CONFIG_DEFAULT);
+
+		/* set current LSB to 1mA, shunt is in uOhms */
+		/* (equation 1 in datasheet)*/
+		ina2xx_write_word(client, INA2XX_CALIBRATION, 5120000 / shunt);
+		dev_info(&client->dev,
+			 "power monitor INA226 (Rshunt = %li uOhm)\n", shunt);
+		data->registers = INA226_REGISTERS;
+		break;
+	default:
+		/* unknown device id */
+		return -ENODEV;
+	}
+
+	i2c_set_clientdata(client, data);
+	mutex_init(&data->update_lock);
+
+	ret = sysfs_create_group(&client->dev.kobj, &ina2xx_group);
+	if (ret)
+		return ret;
+
+	data->hwmon_dev = hwmon_device_register(&client->dev);
+	if (IS_ERR(data->hwmon_dev)) {
+		ret = PTR_ERR(data->hwmon_dev);
+		goto out_err_hwmon;
+	}
+
+	return 0;
+
+out_err_hwmon:
+	sysfs_remove_group(&client->dev.kobj, &ina2xx_group);
+	return ret;
+}
+
+static int ina2xx_remove(struct i2c_client *client)
+{
+	struct ina2xx_data *data = i2c_get_clientdata(client);
+
+	hwmon_device_unregister(data->hwmon_dev);
+	sysfs_remove_group(&client->dev.kobj, &ina2xx_group);
+
+	return 0;
+}
+
+static const struct i2c_device_id ina2xx_id[] = {
+	{ "ina219", ina219 },
+	{ "ina226", ina226 },
+	{ }
+};
+MODULE_DEVICE_TABLE(i2c, ina2xx_id);
+
+static struct i2c_driver ina2xx_driver = {
+	.driver = {
+		.name	= "ina2xx",
+	},
+	.probe		= ina2xx_probe,
+	.remove		= ina2xx_remove,
+	.id_table	= ina2xx_id,
+};
+
+static int __init ina2xx_init(void)
+{
+	return i2c_add_driver(&ina2xx_driver);
+}
+
+static void __exit ina2xx_exit(void)
+{
+	i2c_del_driver(&ina2xx_driver);
+}
+
+MODULE_AUTHOR("Lothar Felten <l-felten@ti.com>");
+MODULE_DESCRIPTION("ina2xx driver");
+MODULE_LICENSE("GPL");
+
+module_init(ina2xx_init);
+module_exit(ina2xx_exit);
diff --git a/drivers/hwmon/it87.c b/drivers/hwmon/it87.c
index 0b204e4cf51..e7701d99f8e 100644
--- a/drivers/hwmon/it87.c
+++ b/drivers/hwmon/it87.c
@@ -19,6 +19,8 @@
  *            IT8726F  Super I/O chip w/LPC interface
  *            IT8728F  Super I/O chip w/LPC interface
  *            IT8758E  Super I/O chip w/LPC interface
+ *            IT8782F  Super I/O chip w/LPC interface
+ *            IT8783E/F Super I/O chip w/LPC interface
  *            Sis950   A clone of the IT8705F
  *
  *  Copyright (C) 2001 Chris Gauthron
@@ -59,7 +61,8 @@
 
 #define DRVNAME "it87"
 
-enum chips { it87, it8712, it8716, it8718, it8720, it8721, it8728 };
+enum chips { it87, it8712, it8716, it8718, it8720, it8721, it8728, it8782,
+	     it8783 };
 
 static unsigned short force_id;
 module_param(force_id, ushort, 0);
@@ -137,13 +140,18 @@ static inline void superio_exit(void)
 #define IT8721F_DEVID 0x8721
 #define IT8726F_DEVID 0x8726
 #define IT8728F_DEVID 0x8728
+#define IT8782F_DEVID 0x8782
+#define IT8783E_DEVID 0x8783
 #define IT87_ACT_REG  0x30
 #define IT87_BASE_REG 0x60
 
 /* Logical device 7 registers (IT8712F and later) */
+#define IT87_SIO_GPIO1_REG	0x25
 #define IT87_SIO_GPIO3_REG	0x27
 #define IT87_SIO_GPIO5_REG	0x29
+#define IT87_SIO_PINX1_REG	0x2a	/* Pin selection */
 #define IT87_SIO_PINX2_REG	0x2c	/* Pin selection */
+#define IT87_SIO_SPI_REG	0xef	/* SPI function pin select */
 #define IT87_SIO_VID_REG	0xfc	/* VID value */
 #define IT87_SIO_BEEP_PIN_REG	0xf6	/* Beep pin mapping */
 
@@ -210,6 +218,7 @@ static const u8 IT87_REG_FANX_MIN[]	= { 0x1b, 0x1c, 0x1d, 0x85, 0x87 };
 
 #define IT87_REG_VIN_ENABLE    0x50
 #define IT87_REG_TEMP_ENABLE   0x51
+#define IT87_REG_TEMP_EXTRA    0x55
 #define IT87_REG_BEEP_ENABLE   0x5c
 
 #define IT87_REG_CHIPID        0x58
@@ -226,9 +235,11 @@ struct it87_sio_data {
 	u8 beep_pin;
 	u8 internal;	/* Internal sensors can be labeled */
 	/* Features skipped based on config or DMI */
+	u16 skip_in;
 	u8 skip_vid;
 	u8 skip_fan;
 	u8 skip_pwm;
+	u8 skip_temp;
 };
 
 /*
@@ -253,6 +264,7 @@ struct it87_data {
 	u8 has_fan;		/* Bitfield, fans enabled */
 	u16 fan[5];		/* Register values, possibly combined */
 	u16 fan_min[5];		/* Register values, possibly combined */
+	u8 has_temp;		/* Bitfield, temp sensors enabled */
 	s8 temp[3];		/* Register value */
 	s8 temp_high[3];	/* Register value */
 	s8 temp_low[3];		/* Register value */
@@ -304,31 +316,23 @@ static inline int has_newer_autopwm(const struct it87_data *data)
 	    || data->type == it8728;
 }
 
-static u8 in_to_reg(const struct it87_data *data, int nr, long val)
+static int adc_lsb(const struct it87_data *data, int nr)
 {
-	long lsb;
-
-	if (has_12mv_adc(data)) {
-		if (data->in_scaled & (1 << nr))
-			lsb = 24;
-		else
-			lsb = 12;
-	} else
-		lsb = 16;
+	int lsb = has_12mv_adc(data) ? 12 : 16;
+	if (data->in_scaled & (1 << nr))
+		lsb <<= 1;
+	return lsb;
+}
 
-	val = DIV_ROUND_CLOSEST(val, lsb);
+static u8 in_to_reg(const struct it87_data *data, int nr, long val)
+{
+	val = DIV_ROUND_CLOSEST(val, adc_lsb(data, nr));
 	return SENSORS_LIMIT(val, 0, 255);
 }
 
 static int in_from_reg(const struct it87_data *data, int nr, int val)
 {
-	if (has_12mv_adc(data)) {
-		if (data->in_scaled & (1 << nr))
-			return val * 24;
-		else
-			return val * 12;
-	} else
-		return val * 16;
+	return val * adc_lsb(data, nr);
 }
 
 static inline u8 FAN_TO_REG(long rpm, int div)
@@ -407,7 +411,9 @@ static inline int has_16bit_fans(const struct it87_data *data)
 	    || data->type == it8718
 	    || data->type == it8720
 	    || data->type == it8721
-	    || data->type == it8728;
+	    || data->type == it8728
+	    || data->type == it8782
+	    || data->type == it8783;
 }
 
 static inline int has_old_autopwm(const struct it87_data *data)
@@ -1369,57 +1375,103 @@ static ssize_t show_name(struct device *dev, struct device_attribute
 }
 static DEVICE_ATTR(name, S_IRUGO, show_name, NULL);
 
-static struct attribute *it87_attributes[] = {
+static struct attribute *it87_attributes_in[9][5] = {
+{
 	&sensor_dev_attr_in0_input.dev_attr.attr,
-	&sensor_dev_attr_in1_input.dev_attr.attr,
-	&sensor_dev_attr_in2_input.dev_attr.attr,
-	&sensor_dev_attr_in3_input.dev_attr.attr,
-	&sensor_dev_attr_in4_input.dev_attr.attr,
-	&sensor_dev_attr_in5_input.dev_attr.attr,
-	&sensor_dev_attr_in6_input.dev_attr.attr,
-	&sensor_dev_attr_in7_input.dev_attr.attr,
-	&sensor_dev_attr_in8_input.dev_attr.attr,
 	&sensor_dev_attr_in0_min.dev_attr.attr,
-	&sensor_dev_attr_in1_min.dev_attr.attr,
-	&sensor_dev_attr_in2_min.dev_attr.attr,
-	&sensor_dev_attr_in3_min.dev_attr.attr,
-	&sensor_dev_attr_in4_min.dev_attr.attr,
-	&sensor_dev_attr_in5_min.dev_attr.attr,
-	&sensor_dev_attr_in6_min.dev_attr.attr,
-	&sensor_dev_attr_in7_min.dev_attr.attr,
 	&sensor_dev_attr_in0_max.dev_attr.attr,
-	&sensor_dev_attr_in1_max.dev_attr.attr,
-	&sensor_dev_attr_in2_max.dev_attr.attr,
-	&sensor_dev_attr_in3_max.dev_attr.attr,
-	&sensor_dev_attr_in4_max.dev_attr.attr,
-	&sensor_dev_attr_in5_max.dev_attr.attr,
-	&sensor_dev_attr_in6_max.dev_attr.attr,
-	&sensor_dev_attr_in7_max.dev_attr.attr,
 	&sensor_dev_attr_in0_alarm.dev_attr.attr,
+	NULL
+}, {
+	&sensor_dev_attr_in1_input.dev_attr.attr,
+	&sensor_dev_attr_in1_min.dev_attr.attr,
+	&sensor_dev_attr_in1_max.dev_attr.attr,
 	&sensor_dev_attr_in1_alarm.dev_attr.attr,
+	NULL
+}, {
+	&sensor_dev_attr_in2_input.dev_attr.attr,
+	&sensor_dev_attr_in2_min.dev_attr.attr,
+	&sensor_dev_attr_in2_max.dev_attr.attr,
 	&sensor_dev_attr_in2_alarm.dev_attr.attr,
+	NULL
+}, {
+	&sensor_dev_attr_in3_input.dev_attr.attr,
+	&sensor_dev_attr_in3_min.dev_attr.attr,
+	&sensor_dev_attr_in3_max.dev_attr.attr,
 	&sensor_dev_attr_in3_alarm.dev_attr.attr,
+	NULL
+}, {
+	&sensor_dev_attr_in4_input.dev_attr.attr,
+	&sensor_dev_attr_in4_min.dev_attr.attr,
+	&sensor_dev_attr_in4_max.dev_attr.attr,
 	&sensor_dev_attr_in4_alarm.dev_attr.attr,
+	NULL
+}, {
+	&sensor_dev_attr_in5_input.dev_attr.attr,
+	&sensor_dev_attr_in5_min.dev_attr.attr,
+	&sensor_dev_attr_in5_max.dev_attr.attr,
 	&sensor_dev_attr_in5_alarm.dev_attr.attr,
+	NULL
+}, {
+	&sensor_dev_attr_in6_input.dev_attr.attr,
+	&sensor_dev_attr_in6_min.dev_attr.attr,
+	&sensor_dev_attr_in6_max.dev_attr.attr,
 	&sensor_dev_attr_in6_alarm.dev_attr.attr,
+	NULL
+}, {
+	&sensor_dev_attr_in7_input.dev_attr.attr,
+	&sensor_dev_attr_in7_min.dev_attr.attr,
+	&sensor_dev_attr_in7_max.dev_attr.attr,
 	&sensor_dev_attr_in7_alarm.dev_attr.attr,
+	NULL
+}, {
+	&sensor_dev_attr_in8_input.dev_attr.attr,
+	NULL
+} };
 
+static const struct attribute_group it87_group_in[9] = {
+	{ .attrs = it87_attributes_in[0] },
+	{ .attrs = it87_attributes_in[1] },
+	{ .attrs = it87_attributes_in[2] },
+	{ .attrs = it87_attributes_in[3] },
+	{ .attrs = it87_attributes_in[4] },
+	{ .attrs = it87_attributes_in[5] },
+	{ .attrs = it87_attributes_in[6] },
+	{ .attrs = it87_attributes_in[7] },
+	{ .attrs = it87_attributes_in[8] },
+};
+
+static struct attribute *it87_attributes_temp[3][6] = {
+{
 	&sensor_dev_attr_temp1_input.dev_attr.attr,
-	&sensor_dev_attr_temp2_input.dev_attr.attr,
-	&sensor_dev_attr_temp3_input.dev_attr.attr,
 	&sensor_dev_attr_temp1_max.dev_attr.attr,
-	&sensor_dev_attr_temp2_max.dev_attr.attr,
-	&sensor_dev_attr_temp3_max.dev_attr.attr,
 	&sensor_dev_attr_temp1_min.dev_attr.attr,
-	&sensor_dev_attr_temp2_min.dev_attr.attr,
-	&sensor_dev_attr_temp3_min.dev_attr.attr,
 	&sensor_dev_attr_temp1_type.dev_attr.attr,
-	&sensor_dev_attr_temp2_type.dev_attr.attr,
-	&sensor_dev_attr_temp3_type.dev_attr.attr,
 	&sensor_dev_attr_temp1_alarm.dev_attr.attr,
+	NULL
+} , {
+	&sensor_dev_attr_temp2_input.dev_attr.attr,
+	&sensor_dev_attr_temp2_max.dev_attr.attr,
+	&sensor_dev_attr_temp2_min.dev_attr.attr,
+	&sensor_dev_attr_temp2_type.dev_attr.attr,
 	&sensor_dev_attr_temp2_alarm.dev_attr.attr,
+	NULL
+} , {
+	&sensor_dev_attr_temp3_input.dev_attr.attr,
+	&sensor_dev_attr_temp3_max.dev_attr.attr,
+	&sensor_dev_attr_temp3_min.dev_attr.attr,
+	&sensor_dev_attr_temp3_type.dev_attr.attr,
 	&sensor_dev_attr_temp3_alarm.dev_attr.attr,
+	NULL
+} };
+
+static const struct attribute_group it87_group_temp[3] = {
+	{ .attrs = it87_attributes_temp[0] },
+	{ .attrs = it87_attributes_temp[1] },
+	{ .attrs = it87_attributes_temp[2] },
+};
 
+static struct attribute *it87_attributes[] = {
 	&dev_attr_alarms.attr,
 	&sensor_dev_attr_intrusion0_alarm.dev_attr.attr,
 	&dev_attr_name.attr,
@@ -1430,7 +1482,7 @@ static const struct attribute_group it87_group = {
 	.attrs = it87_attributes,
 };
 
-static struct attribute *it87_attributes_beep[] = {
+static struct attribute *it87_attributes_in_beep[] = {
 	&sensor_dev_attr_in0_beep.dev_attr.attr,
 	&sensor_dev_attr_in1_beep.dev_attr.attr,
 	&sensor_dev_attr_in2_beep.dev_attr.attr,
@@ -1439,15 +1491,13 @@ static struct attribute *it87_attributes_beep[] = {
 	&sensor_dev_attr_in5_beep.dev_attr.attr,
 	&sensor_dev_attr_in6_beep.dev_attr.attr,
 	&sensor_dev_attr_in7_beep.dev_attr.attr,
+	NULL
+};
 
+static struct attribute *it87_attributes_temp_beep[] = {
 	&sensor_dev_attr_temp1_beep.dev_attr.attr,
 	&sensor_dev_attr_temp2_beep.dev_attr.attr,
 	&sensor_dev_attr_temp3_beep.dev_attr.attr,
-	NULL
-};
-
-static const struct attribute_group it87_group_beep = {
-	.attrs = it87_attributes_beep,
 };
 
 static struct attribute *it87_attributes_fan16[5][3+1] = { {
@@ -1651,6 +1701,12 @@ static int __init it87_find(unsigned short *address,
 	case IT8728F_DEVID:
 		sio_data->type = it8728;
 		break;
+	case IT8782F_DEVID:
+		sio_data->type = it8782;
+		break;
+	case IT8783E_DEVID:
+		sio_data->type = it8783;
+		break;
 	case 0xffff:	/* No device at all */
 		goto exit;
 	default:
@@ -1686,16 +1742,86 @@ static int __init it87_find(unsigned short *address,
 		/* The IT8705F has a different LD number for GPIO */
 		superio_select(5);
 		sio_data->beep_pin = superio_inb(IT87_SIO_BEEP_PIN_REG) & 0x3f;
+	} else if (sio_data->type == it8783) {
+		int reg25, reg27, reg2A, reg2C, regEF;
+
+		sio_data->skip_vid = 1;	/* No VID */
+
+		superio_select(GPIO);
+
+		reg25 = superio_inb(IT87_SIO_GPIO1_REG);
+		reg27 = superio_inb(IT87_SIO_GPIO3_REG);
+		reg2A = superio_inb(IT87_SIO_PINX1_REG);
+		reg2C = superio_inb(IT87_SIO_PINX2_REG);
+		regEF = superio_inb(IT87_SIO_SPI_REG);
+
+		/* Check if fan3 is there or not */
+		if ((reg27 & (1 << 0)) || !(reg2C & (1 << 2)))
+			sio_data->skip_fan |= (1 << 2);
+		if ((reg25 & (1 << 4))
+		    || (!(reg2A & (1 << 1)) && (regEF & (1 << 0))))
+			sio_data->skip_pwm |= (1 << 2);
+
+		/* Check if fan2 is there or not */
+		if (reg27 & (1 << 7))
+			sio_data->skip_fan |= (1 << 1);
+		if (reg27 & (1 << 3))
+			sio_data->skip_pwm |= (1 << 1);
+
+		/* VIN5 */
+		if ((reg27 & (1 << 0)) || (reg2C & (1 << 2)))
+			sio_data->skip_in |= (1 << 5); /* No VIN5 */
+
+		/* VIN6 */
+		if (reg27 & (1 << 1))
+			sio_data->skip_in |= (1 << 6); /* No VIN6 */
+
+		/*
+		 * VIN7
+		 * Does not depend on bit 2 of Reg2C, contrary to datasheet.
+		 */
+		if (reg27 & (1 << 2)) {
+			/*
+			 * The data sheet is a bit unclear regarding the
+			 * internal voltage divider for VCCH5V. It says
+			 * "This bit enables and switches VIN7 (pin 91) to the
+			 * internal voltage divider for VCCH5V".
+			 * This is different to other chips, where the internal
+			 * voltage divider would connect VIN7 to an internal
+			 * voltage source. Maybe that is the case here as well.
+			 *
+			 * Since we don't know for sure, re-route it if that is
+			 * not the case, and ask the user to report if the
+			 * resulting voltage is sane.
+			 */
+			if (!(reg2C & (1 << 1))) {
+				reg2C |= (1 << 1);
+				superio_outb(IT87_SIO_PINX2_REG, reg2C);
+				pr_notice("Routing internal VCCH5V to in7.\n");
+			}
+			pr_notice("in7 routed to internal voltage divider, with external pin disabled.\n");
+			pr_notice("Please report if it displays a reasonable voltage.\n");
+		}
+
+		if (reg2C & (1 << 0))
+			sio_data->internal |= (1 << 0);
+		if (reg2C & (1 << 1))
+			sio_data->internal |= (1 << 1);
+
+		sio_data->beep_pin = superio_inb(IT87_SIO_BEEP_PIN_REG) & 0x3f;
+
 	} else {
 		int reg;
+		bool uart6;
 
 		superio_select(GPIO);
 
 		reg = superio_inb(IT87_SIO_GPIO3_REG);
-		if (sio_data->type == it8721 || sio_data->type == it8728) {
+		if (sio_data->type == it8721 || sio_data->type == it8728 ||
+		    sio_data->type == it8782) {
 			/*
-			 * The IT8721F/IT8758E doesn't have VID pins at all,
-			 * not sure about the IT8728F.
+			 * IT8721F/IT8758E, and IT8782F don't have VID pins
+			 * at all, not sure about the IT8728F.
 			 */
 			sio_data->skip_vid = 1;
 		} else {
@@ -1724,6 +1850,9 @@ static int __init it87_find(unsigned short *address,
 			sio_data->vid_value = superio_inb(IT87_SIO_VID_REG);
 
 		reg = superio_inb(IT87_SIO_PINX2_REG);
+
+		uart6 = sio_data->type == it8782 && (reg & (1 << 2));
+
 		/*
 		 * The IT8720F has no VIN7 pin, so VCCH should always be
 		 * routed internally to VIN7 with an internal divider.
@@ -1733,8 +1862,12 @@ static int __init it87_find(unsigned short *address,
 		 * configured, even though the IT8720F datasheet claims
 		 * that the internal routing of VCCH to VIN7 is the default
 		 * setting. So we force the internal routing in this case.
+		 *
+		 * On IT8782F, VIN7 is multiplexed with one of the UART6 pins.
+		 * If UART6 is enabled, re-route VIN7 to the internal divider
+		 * if that is not already the case.
 		 */
-		if (sio_data->type == it8720 && !(reg & (1 << 1))) {
+		if ((sio_data->type == it8720 || uart6) && !(reg & (1 << 1))) {
 			reg |= (1 << 1);
 			superio_outb(IT87_SIO_PINX2_REG, reg);
 			pr_notice("Routing internal VCCH to in7\n");
@@ -1745,6 +1878,20 @@ static int __init it87_find(unsigned short *address,
 		    sio_data->type == it8728)
 			sio_data->internal |= (1 << 1);
 
+		/*
+		 * On IT8782F, UART6 pins overlap with VIN5, VIN6, and VIN7.
+		 * While VIN7 can be routed to the internal voltage divider,
+		 * VIN5 and VIN6 are not available if UART6 is enabled.
+		 *
+		 * Also, temp3 is not available if UART6 is enabled and TEMPIN3
+		 * is the temperature source. Since we can not read the
+		 * temperature source here, skip_temp is preliminary.
+		 */
+		if (uart6) {
+			sio_data->skip_in |= (1 << 5) | (1 << 6);
+			sio_data->skip_temp |= (1 << 2);
+		}
+
 		sio_data->beep_pin = superio_inb(IT87_SIO_BEEP_PIN_REG) & 0x3f;
 	}
 	if (sio_data->beep_pin)
@@ -1782,8 +1929,22 @@ static void it87_remove_files(struct device *dev)
 	int i;
 
 	sysfs_remove_group(&dev->kobj, &it87_group);
-	if (sio_data->beep_pin)
-		sysfs_remove_group(&dev->kobj, &it87_group_beep);
+	for (i = 0; i < 9; i++) {
+		if (sio_data->skip_in & (1 << i))
+			continue;
+		sysfs_remove_group(&dev->kobj, &it87_group_in[i]);
+		if (it87_attributes_in_beep[i])
+			sysfs_remove_file(&dev->kobj,
+					  it87_attributes_in_beep[i]);
+	}
+	for (i = 0; i < 3; i++) {
+		if (!(data->has_temp & (1 << i)))
+			continue;
+		sysfs_remove_group(&dev->kobj, &it87_group_temp[i]);
+		if (sio_data->beep_pin)
+			sysfs_remove_file(&dev->kobj,
+					  it87_attributes_temp_beep[i]);
+	}
 	for (i = 0; i < 5; i++) {
 		if (!(data->has_fan & (1 << i)))
 			continue;
@@ -1823,22 +1984,22 @@ static int __devinit it87_probe(struct platform_device *pdev)
 		"it8720",
 		"it8721",
 		"it8728",
+		"it8782",
+		"it8783",
 	};
 
 	res = platform_get_resource(pdev, IORESOURCE_IO, 0);
-	if (!request_region(res->start, IT87_EC_EXTENT, DRVNAME)) {
+	if (!devm_request_region(&pdev->dev, res->start, IT87_EC_EXTENT,
+				 DRVNAME)) {
 		dev_err(dev, "Failed to request region 0x%lx-0x%lx\n",
 			(unsigned long)res->start,
 			(unsigned long)(res->start + IT87_EC_EXTENT - 1));
-		err = -EBUSY;
-		goto ERROR0;
+		return -EBUSY;
 	}
 
-	data = kzalloc(sizeof(struct it87_data), GFP_KERNEL);
-	if (!data) {
-		err = -ENOMEM;
-		goto ERROR1;
-	}
+	data = devm_kzalloc(&pdev->dev, sizeof(struct it87_data), GFP_KERNEL);
+	if (!data)
+		return -ENOMEM;
 
 	data->addr = res->start;
 	data->type = sio_data->type;
@@ -1847,10 +2008,8 @@ static int __devinit it87_probe(struct platform_device *pdev)
 
 	/* Now, we do the remaining detection. */
 	if ((it87_read_value(data, IT87_REG_CONFIG) & 0x80)
-	 || it87_read_value(data, IT87_REG_CHIPID) != 0x90) {
-		err = -ENODEV;
-		goto ERROR2;
-	}
+	 || it87_read_value(data, IT87_REG_CHIPID) != 0x90)
+		return -ENODEV;
 
 	platform_set_drvdata(pdev, data);
 
@@ -1867,6 +2026,18 @@ static int __devinit it87_probe(struct platform_device *pdev)
 			data->in_scaled |= (1 << 7);	/* in7 is VSB */
 		if (sio_data->internal & (1 << 2))
 			data->in_scaled |= (1 << 8);	/* in8 is Vbat */
+	} else if (sio_data->type == it8782 || sio_data->type == it8783) {
+		if (sio_data->internal & (1 << 0))
+			data->in_scaled |= (1 << 3);	/* in3 is VCC5V */
+		if (sio_data->internal & (1 << 1))
+			data->in_scaled |= (1 << 7);	/* in7 is VCCH5V */
+	}
+
+	data->has_temp = 0x07;
+	if (sio_data->skip_temp & (1 << 2)) {
+		if (sio_data->type == it8782
+		    && !(it87_read_value(data, IT87_REG_TEMP_EXTRA) & 0x80))
+			data->has_temp &= ~(1 << 2);
 	}
 
 	/* Initialize the IT87 chip */
@@ -1875,12 +2046,34 @@ static int __devinit it87_probe(struct platform_device *pdev)
 	/* Register sysfs hooks */
 	err = sysfs_create_group(&dev->kobj, &it87_group);
 	if (err)
-		goto ERROR2;
+		return err;
 
-	if (sio_data->beep_pin) {
-		err = sysfs_create_group(&dev->kobj, &it87_group_beep);
+	for (i = 0; i < 9; i++) {
+		if (sio_data->skip_in & (1 << i))
+			continue;
+		err = sysfs_create_group(&dev->kobj, &it87_group_in[i]);
 		if (err)
-			goto ERROR4;
+			goto error;
+		if (sio_data->beep_pin && it87_attributes_in_beep[i]) {
+			err = sysfs_create_file(&dev->kobj,
+						it87_attributes_in_beep[i]);
+			if (err)
+				goto error;
+		}
+	}
+
+	for (i = 0; i < 3; i++) {
+		if (!(data->has_temp & (1 << i)))
+			continue;
+		err = sysfs_create_group(&dev->kobj, &it87_group_temp[i]);
+		if (err)
+			goto error;
+		if (sio_data->beep_pin) {
+			err = sysfs_create_file(&dev->kobj,
+						it87_attributes_temp_beep[i]);
+			if (err)
+				goto error;
+		}
 	}
 
 	/* Do not create fan files for disabled fans */
@@ -1891,13 +2084,13 @@ static int __devinit it87_probe(struct platform_device *pdev)
 			continue;
 		err = sysfs_create_group(&dev->kobj, &fan_group[i]);
 		if (err)
-			goto ERROR4;
+			goto error;
 
 		if (sio_data->beep_pin) {
 			err = sysfs_create_file(&dev->kobj,
 						it87_attributes_fan_beep[i]);
 			if (err)
-				goto ERROR4;
+				goto error;
 			if (!fan_beep_need_rw)
 				continue;
 
@@ -1922,14 +2115,14 @@ static int __devinit it87_probe(struct platform_device *pdev)
 			err = sysfs_create_group(&dev->kobj,
 						 &it87_group_pwm[i]);
 			if (err)
-				goto ERROR4;
+				goto error;
 
 			if (!has_old_autopwm(data))
 				continue;
 			err = sysfs_create_group(&dev->kobj,
 						 &it87_group_autopwm[i]);
 			if (err)
-				goto ERROR4;
+				goto error;
 		}
 	}
 
@@ -1939,7 +2132,7 @@ static int __devinit it87_probe(struct platform_device *pdev)
 		data->vid = sio_data->vid_value;
 		err = sysfs_create_group(&dev->kobj, &it87_group_vid);
 		if (err)
-			goto ERROR4;
+			goto error;
 	}
 
 	/* Export labels for internal sensors */
@@ -1949,25 +2142,19 @@ static int __devinit it87_probe(struct platform_device *pdev)
 		err = sysfs_create_file(&dev->kobj,
 					it87_attributes_label[i]);
 		if (err)
-			goto ERROR4;
+			goto error;
 	}
 
 	data->hwmon_dev = hwmon_device_register(dev);
 	if (IS_ERR(data->hwmon_dev)) {
 		err = PTR_ERR(data->hwmon_dev);
-		goto ERROR4;
+		goto error;
 	}
 
 	return 0;
 
-ERROR4:
+error:
 	it87_remove_files(dev);
-ERROR2:
-	platform_set_drvdata(pdev, NULL);
-	kfree(data);
-ERROR1:
-	release_region(res->start, IT87_EC_EXTENT);
-ERROR0:
 	return err;
 }
 
@@ -1978,10 +2165,6 @@ static int __devexit it87_remove(struct platform_device *pdev)
 	hwmon_device_unregister(data->hwmon_dev);
 	it87_remove_files(&pdev->dev);
 
-	release_region(data->addr, IT87_EC_EXTENT);
-	platform_set_drvdata(pdev, NULL);
-	kfree(data);
-
 	return 0;
 }
 
@@ -2143,8 +2326,9 @@ static void __devinit it87_init_device(struct platform_device *pdev)
 			it87_write_value(data, IT87_REG_FAN_16BIT,
 					 tmp | 0x07);
 		}
-		/* IT8705F only supports three fans. */
-		if (data->type != it87) {
+		/* IT8705F, IT8782F, and IT8783E/F only support three fans. */
+		if (data->type != it87 && data->type != it8782 &&
+		    data->type != it8783) {
 			if (tmp & (1 << 4))
 				data->has_fan |= (1 << 3); /* fan4 enabled */
 			if (tmp & (1 << 5))
@@ -2233,6 +2417,8 @@ static struct it87_data *it87_update_device(struct device *dev)
 			}
 		}
 		for (i = 0; i < 3; i++) {
+			if (!(data->has_temp & (1 << i)))
+				continue;
 			data->temp[i] =
 				it87_read_value(data, IT87_REG_TEMP(i));
 			data->temp_high[i] =
diff --git a/drivers/hwmon/k10temp.c b/drivers/hwmon/k10temp.c
index 307bb325dde..7356b5ec8f6 100644
--- a/drivers/hwmon/k10temp.c
+++ b/drivers/hwmon/k10temp.c
@@ -225,15 +225,4 @@ static struct pci_driver k10temp_driver = {
 	.remove = __devexit_p(k10temp_remove),
 };
 
-static int __init k10temp_init(void)
-{
-	return pci_register_driver(&k10temp_driver);
-}
-
-static void __exit k10temp_exit(void)
-{
-	pci_unregister_driver(&k10temp_driver);
-}
-
-module_init(k10temp_init)
-module_exit(k10temp_exit)
+module_pci_driver(k10temp_driver);
diff --git a/drivers/hwmon/k8temp.c b/drivers/hwmon/k8temp.c
index 57510198875..35aac82ee8e 100644
--- a/drivers/hwmon/k8temp.c
+++ b/drivers/hwmon/k8temp.c
@@ -339,19 +339,8 @@ static struct pci_driver k8temp_driver = {
 	.remove = __devexit_p(k8temp_remove),
 };
 
-static int __init k8temp_init(void)
-{
-	return pci_register_driver(&k8temp_driver);
-}
-
-static void __exit k8temp_exit(void)
-{
-	pci_unregister_driver(&k8temp_driver);
-}
+module_pci_driver(k8temp_driver);
 
 MODULE_AUTHOR("Rudolf Marek <r.marek@assembler.cz>");
 MODULE_DESCRIPTION("AMD K8 core temperature monitor");
 MODULE_LICENSE("GPL");
-
-module_init(k8temp_init)
-module_exit(k8temp_exit)
diff --git a/drivers/hwmon/ntc_thermistor.c b/drivers/hwmon/ntc_thermistor.c
index 9b382ec2c3b..6da9696e182 100644
--- a/drivers/hwmon/ntc_thermistor.c
+++ b/drivers/hwmon/ntc_thermistor.c
@@ -134,8 +134,7 @@ static inline u64 div64_u64_safe(u64 dividend, u64 divisor)
 	return div64_u64(dividend, divisor);
 }
 
-static unsigned int get_ohm_of_thermistor(struct ntc_data *data,
-		unsigned int uV)
+static int get_ohm_of_thermistor(struct ntc_data *data, unsigned int uV)
 {
 	struct ntc_thermistor_platform_data *pdata = data->pdata;
 	u64 mV = uV / 1000;
@@ -146,12 +145,12 @@ static unsigned int get_ohm_of_thermistor(struct ntc_data *data,
 
 	if (mV == 0) {
 		if (pdata->connect == NTC_CONNECTED_POSITIVE)
-			return UINT_MAX;
+			return INT_MAX;
 		return 0;
 	}
 	if (mV >= pmV)
 		return (pdata->connect == NTC_CONNECTED_POSITIVE) ?
-			0 : UINT_MAX;
+			0 : INT_MAX;
 
 	if (pdata->connect == NTC_CONNECTED_POSITIVE && puO == 0)
 		N = div64_u64_safe(pdO * (pmV - mV), mV);
@@ -163,113 +162,109 @@ static unsigned int get_ohm_of_thermistor(struct ntc_data *data,
 	else
 		N = div64_u64_safe(pdO * puO * mV, pdO * (pmV - mV) - puO * mV);
 
-	return (unsigned int) N;
+	if (N > INT_MAX)
+		N = INT_MAX;
+	return N;
 }
 
-static int lookup_comp(struct ntc_data *data,
-		unsigned int ohm, int *i_low, int *i_high)
+static void lookup_comp(struct ntc_data *data, unsigned int ohm,
+			int *i_low, int *i_high)
 {
-	int start, end, mid = -1;
+	int start, end, mid;
+
+	/*
+	 * Handle special cases: Resistance is higher than or equal to
+	 * resistance in first table entry, or resistance is lower or equal
+	 * to resistance in last table entry.
+	 * In these cases, return i_low == i_high, either pointing to the
+	 * beginning or to the end of the table depending on the condition.
+	 */
+	if (ohm >= data->comp[0].ohm) {
+		*i_low = 0;
+		*i_high = 0;
+		return;
+	}
+	if (ohm <= data->comp[data->n_comp - 1].ohm) {
+		*i_low = data->n_comp - 1;
+		*i_high = data->n_comp - 1;
+		return;
+	}
 
 	/* Do a binary search on compensation table */
 	start = 0;
 	end = data->n_comp;
-
-	while (end > start) {
+	while (start < end) {
 		mid = start + (end - start) / 2;
-		if (data->comp[mid].ohm < ohm)
+		/*
+		 * start <= mid < end
+		 * data->comp[start].ohm > ohm >= data->comp[end].ohm
+		 *
+		 * We could check for "ohm == data->comp[mid].ohm" here, but
+		 * that is a quite unlikely condition, and we would have to
+		 * check again after updating start. Check it at the end instead
+		 * for simplicity.
+		 */
+		if (ohm >= data->comp[mid].ohm) {
 			end = mid;
-		else if (data->comp[mid].ohm > ohm)
-			start = mid + 1;
-		else
-			break;
-	}
-
-	if (mid == 0) {
-		if (data->comp[mid].ohm > ohm) {
-			*i_high = mid;
-			*i_low = mid + 1;
-			return 0;
-		} else {
-			*i_low = mid;
-			*i_high = -1;
-			return -EINVAL;
-		}
-	}
-	if (mid == (data->n_comp - 1)) {
-		if (data->comp[mid].ohm <= ohm) {
-			*i_low = mid;
-			*i_high = mid - 1;
-			return 0;
 		} else {
-			*i_low = -1;
-			*i_high = mid;
-			return -EINVAL;
+			start = mid + 1;
+			/*
+			 * ohm >= data->comp[start].ohm might be true here,
+			 * since we set start to mid + 1. In that case, we are
+			 * done. We could keep going, but the condition is quite
+			 * likely to occur, so it is worth checking for it.
+			 */
+			if (ohm >= data->comp[start].ohm)
+				end = start;
 		}
+		/*
+		 * start <= end
+		 * data->comp[start].ohm >= ohm >= data->comp[end].ohm
+		 */
 	}
-
-	if (data->comp[mid].ohm <= ohm) {
-		*i_low = mid;
-		*i_high = mid - 1;
-	} else {
-		*i_low = mid + 1;
-		*i_high = mid;
-	}
-
-	return 0;
+	/*
+	 * start == end
+	 * ohm >= data->comp[end].ohm
+	 */
+	*i_low = end;
+	if (ohm == data->comp[end].ohm)
+		*i_high = end;
+	else
+		*i_high = end - 1;
 }
 
-static int get_temp_mC(struct ntc_data *data, unsigned int ohm, int *temp)
+static int get_temp_mC(struct ntc_data *data, unsigned int ohm)
 {
 	int low, high;
-	int ret;
+	int temp;
 
-	ret = lookup_comp(data, ohm, &low, &high);
-	if (ret) {
+	lookup_comp(data, ohm, &low, &high);
+	if (low == high) {
 		/* Unable to use linear approximation */
-		if (low != -1)
-			*temp = data->comp[low].temp_C * 1000;
-		else if (high != -1)
-			*temp = data->comp[high].temp_C * 1000;
-		else
-			return ret;
+		temp = data->comp[low].temp_C * 1000;
 	} else {
-		*temp = data->comp[low].temp_C * 1000 +
+		temp = data->comp[low].temp_C * 1000 +
 			((data->comp[high].temp_C - data->comp[low].temp_C) *
 			 1000 * ((int)ohm - (int)data->comp[low].ohm)) /
 			((int)data->comp[high].ohm - (int)data->comp[low].ohm);
 	}
-
-	return 0;
+	return temp;
 }
 
-static int ntc_thermistor_read(struct ntc_data *data, int *temp)
+static int ntc_thermistor_get_ohm(struct ntc_data *data)
 {
-	int ret;
-	int read_ohm, read_uV;
-	unsigned int ohm = 0;
-
-	if (data->pdata->read_ohm) {
-		read_ohm = data->pdata->read_ohm();
-		if (read_ohm < 0)
-			return read_ohm;
-		ohm = (unsigned int)read_ohm;
-	}
+	int read_uV;
+
+	if (data->pdata->read_ohm)
+		return data->pdata->read_ohm();
 
 	if (data->pdata->read_uV) {
 		read_uV = data->pdata->read_uV();
 		if (read_uV < 0)
 			return read_uV;
-		ohm = get_ohm_of_thermistor(data, (unsigned int)read_uV);
-	}
-
-	ret = get_temp_mC(data, ohm, temp);
-	if (ret) {
-		dev_dbg(data->dev, "Sensor reading function not available.\n");
-		return ret;
+		return get_ohm_of_thermistor(data, read_uV);
 	}
-
-	return 0;
+	return -EINVAL;
 }
 
 static ssize_t ntc_show_name(struct device *dev,
@@ -290,12 +285,13 @@ static ssize_t ntc_show_temp(struct device *dev,
 		struct device_attribute *attr, char *buf)
 {
 	struct ntc_data *data = dev_get_drvdata(dev);
-	int temp, ret;
+	int ohm;
 
-	ret = ntc_thermistor_read(data, &temp);
-	if (ret)
-		return ret;
-	return sprintf(buf, "%d\n", temp);
+	ohm = ntc_thermistor_get_ohm(data);
+	if (ohm < 0)
+		return ohm;
+
+	return sprintf(buf, "%d\n", get_temp_mC(data, ohm));
 }
 
 static SENSOR_DEVICE_ATTR(temp1_type, S_IRUGO, ntc_show_type, NULL, 0);
@@ -326,14 +322,14 @@ static int __devinit ntc_thermistor_probe(struct platform_device *pdev)
 
 	/* Either one of the two is required. */
 	if (!pdata->read_uV && !pdata->read_ohm) {
-		dev_err(&pdev->dev, "Both read_uV and read_ohm missing."
-				"Need either one of the two.\n");
+		dev_err(&pdev->dev,
+			"Both read_uV and read_ohm missing. Need either one of the two.\n");
 		return -EINVAL;
 	}
 
 	if (pdata->read_uV && pdata->read_ohm) {
-		dev_warn(&pdev->dev, "Only one of read_uV and read_ohm "
-				"is needed; ignoring read_uV.\n");
+		dev_warn(&pdev->dev,
+			 "Only one of read_uV and read_ohm is needed; ignoring read_uV.\n");
 		pdata->read_uV = NULL;
 	}
 
@@ -344,12 +340,12 @@ static int __devinit ntc_thermistor_probe(struct platform_device *pdev)
 				 NTC_CONNECTED_POSITIVE) ||
 				(pdata->connect != NTC_CONNECTED_POSITIVE &&
 				 pdata->connect != NTC_CONNECTED_GROUND))) {
-		dev_err(&pdev->dev, "Required data to use read_uV not "
-				"supplied.\n");
+		dev_err(&pdev->dev,
+			"Required data to use read_uV not supplied.\n");
 		return -EINVAL;
 	}
 
-	data = kzalloc(sizeof(struct ntc_data), GFP_KERNEL);
+	data = devm_kzalloc(&pdev->dev, sizeof(struct ntc_data), GFP_KERNEL);
 	if (!data)
 		return -ENOMEM;
 
@@ -370,8 +366,7 @@ static int __devinit ntc_thermistor_probe(struct platform_device *pdev)
 		dev_err(&pdev->dev, "Unknown device type: %lu(%s)\n",
 				pdev->id_entry->driver_data,
 				pdev->id_entry->name);
-		ret = -EINVAL;
-		goto err;
+		return -EINVAL;
 	}
 
 	platform_set_drvdata(pdev, data);
@@ -379,13 +374,13 @@ static int __devinit ntc_thermistor_probe(struct platform_device *pdev)
 	ret = sysfs_create_group(&data->dev->kobj, &ntc_attr_group);
 	if (ret) {
 		dev_err(data->dev, "unable to create sysfs files\n");
-		goto err;
+		return ret;
 	}
 
 	data->hwmon_dev = hwmon_device_register(data->dev);
-	if (IS_ERR_OR_NULL(data->hwmon_dev)) {
+	if (IS_ERR(data->hwmon_dev)) {
 		dev_err(data->dev, "unable to register as hwmon device.\n");
-		ret = -EINVAL;
+		ret = PTR_ERR(data->hwmon_dev);
 		goto err_after_sysfs;
 	}
 
@@ -395,8 +390,6 @@ static int __devinit ntc_thermistor_probe(struct platform_device *pdev)
 	return 0;
 err_after_sysfs:
 	sysfs_remove_group(&data->dev->kobj, &ntc_attr_group);
-err:
-	kfree(data);
 	return ret;
 }
 
@@ -408,8 +401,6 @@ static int __devexit ntc_thermistor_remove(struct platform_device *pdev)
 	sysfs_remove_group(&data->dev->kobj, &ntc_attr_group);
 	platform_set_drvdata(pdev, NULL);
 
-	kfree(data);
-
 	return 0;
 }
 
diff --git a/drivers/i2c/Kconfig b/drivers/i2c/Kconfig
index 5f13c62e64b..5a3bb3d738d 100644
--- a/drivers/i2c/Kconfig
+++ b/drivers/i2c/Kconfig
@@ -49,7 +49,6 @@ config I2C_CHARDEV
 
 config I2C_MUX
 	tristate "I2C bus multiplexing support"
-	depends on EXPERIMENTAL
 	help
 	  Say Y here if you want the I2C core to support the ability to
 	  handle multiplexed I2C bus topologies, by presenting each
diff --git a/drivers/i2c/busses/Kconfig b/drivers/i2c/busses/Kconfig
index d2c5095deea..7244c8be606 100644
--- a/drivers/i2c/busses/Kconfig
+++ b/drivers/i2c/busses/Kconfig
@@ -351,7 +351,7 @@ config I2C_DAVINCI
 	  For details please see http://www.ti.com/davinci
 
 config I2C_DESIGNWARE_PLATFORM
-	tristate "Synopsys DesignWare Platfrom"
+	tristate "Synopsys DesignWare Platform"
 	depends on HAVE_CLK
 	help
 	  If you say yes to this option, support will be included for the
@@ -445,20 +445,6 @@ config I2C_IOP3XX
 	  This driver can also be built as a module.  If so, the module
 	  will be called i2c-iop3xx.
 
-config I2C_IXP2000
-	tristate "IXP2000 GPIO-Based I2C Interface (DEPRECATED)"
-	depends on ARCH_IXP2000
-	select I2C_ALGOBIT
-	help
-	  Say Y here if you have an Intel IXP2000 (2400, 2800, 2850) based
-	  system and are using GPIO lines for an I2C bus.
-
-	  This support is also available as a module. If so, the module
-	  will be called i2c-ixp2000.
-
-	  This driver is deprecated and will be dropped soon. Use i2c-gpio
-	  instead.
-
 config I2C_MPC
 	tristate "MPC107/824x/85xx/512x/52xx/83xx/86xx"
 	depends on PPC
@@ -483,6 +469,7 @@ config I2C_MV64XXX
 config I2C_MXS
 	tristate "Freescale i.MX28 I2C interface"
 	depends on SOC_IMX28
+	select STMP_DEVICE
 	help
 	  Say Y here if you want to use the I2C bus controller on
 	  the Freescale i.MX28 processors.
diff --git a/drivers/i2c/busses/Makefile b/drivers/i2c/busses/Makefile
index 569567b0d02..ce3c2be7fb4 100644
--- a/drivers/i2c/busses/Makefile
+++ b/drivers/i2c/busses/Makefile
@@ -44,7 +44,6 @@ obj-$(CONFIG_I2C_IBM_IIC)	+= i2c-ibm_iic.o
 obj-$(CONFIG_I2C_IMX)		+= i2c-imx.o
 obj-$(CONFIG_I2C_INTEL_MID)	+= i2c-intel-mid.o
 obj-$(CONFIG_I2C_IOP3XX)	+= i2c-iop3xx.o
-obj-$(CONFIG_I2C_IXP2000)	+= i2c-ixp2000.o
 obj-$(CONFIG_I2C_MPC)		+= i2c-mpc.o
 obj-$(CONFIG_I2C_MV64XXX)	+= i2c-mv64xxx.o
 obj-$(CONFIG_I2C_MXS)		+= i2c-mxs.o
diff --git a/drivers/i2c/busses/i2c-davinci.c b/drivers/i2c/busses/i2c-davinci.c
index a76d85fa3ad..79b4bcb3b85 100644
--- a/drivers/i2c/busses/i2c-davinci.c
+++ b/drivers/i2c/busses/i2c-davinci.c
@@ -755,7 +755,7 @@ static int davinci_i2c_remove(struct platform_device *pdev)
 	dev->clk = NULL;
 
 	davinci_i2c_write_reg(dev, DAVINCI_I2C_MDR_REG, 0);
-	free_irq(IRQ_I2C, dev);
+	free_irq(dev->irq, dev);
 	iounmap(dev->base);
 	kfree(dev);
 
diff --git a/drivers/i2c/busses/i2c-designware-core.c b/drivers/i2c/busses/i2c-designware-core.c
index df879924100..1e48bec80ed 100644
--- a/drivers/i2c/busses/i2c-designware-core.c
+++ b/drivers/i2c/busses/i2c-designware-core.c
@@ -164,9 +164,15 @@ static char *abort_sources[] = {
 
 u32 dw_readl(struct dw_i2c_dev *dev, int offset)
 {
-	u32 value = readl(dev->base + offset);
+	u32 value;
 
-	if (dev->swab)
+	if (dev->accessor_flags & ACCESS_16BIT)
+		value = readw(dev->base + offset) |
+			(readw(dev->base + offset + 2) << 16);
+	else
+		value = readl(dev->base + offset);
+
+	if (dev->accessor_flags & ACCESS_SWAP)
 		return swab32(value);
 	else
 		return value;
@@ -174,10 +180,15 @@ u32 dw_readl(struct dw_i2c_dev *dev, int offset)
 
 void dw_writel(struct dw_i2c_dev *dev, u32 b, int offset)
 {
-	if (dev->swab)
+	if (dev->accessor_flags & ACCESS_SWAP)
 		b = swab32(b);
 
-	writel(b, dev->base + offset);
+	if (dev->accessor_flags & ACCESS_16BIT) {
+		writew((u16)b, dev->base + offset);
+		writew((u16)(b >> 16), dev->base + offset + 2);
+	} else {
+		writel(b, dev->base + offset);
+	}
 }
 
 static u32
@@ -251,14 +262,14 @@ int i2c_dw_init(struct dw_i2c_dev *dev)
 
 	input_clock_khz = dev->get_clk_rate_khz(dev);
 
-	/* Configure register endianess access */
 	reg = dw_readl(dev, DW_IC_COMP_TYPE);
 	if (reg == ___constant_swab32(DW_IC_COMP_TYPE_VALUE)) {
-		dev->swab = 1;
-		reg = DW_IC_COMP_TYPE_VALUE;
-	}
-
-	if (reg != DW_IC_COMP_TYPE_VALUE) {
+		/* Configure register endianess access */
+		dev->accessor_flags |= ACCESS_SWAP;
+	} else if (reg == (DW_IC_COMP_TYPE_VALUE & 0x0000ffff)) {
+		/* Configure register access mode 16bit */
+		dev->accessor_flags |= ACCESS_16BIT;
+	} else if (reg != DW_IC_COMP_TYPE_VALUE) {
 		dev_err(dev->dev, "Unknown Synopsys component type: "
 			"0x%08x\n", reg);
 		return -ENODEV;
diff --git a/drivers/i2c/busses/i2c-designware-core.h b/drivers/i2c/busses/i2c-designware-core.h
index 02d1a2ddd85..9c1840ee09c 100644
--- a/drivers/i2c/busses/i2c-designware-core.h
+++ b/drivers/i2c/busses/i2c-designware-core.h
@@ -82,7 +82,7 @@ struct dw_i2c_dev {
 	unsigned int		status;
 	u32			abort_source;
 	int			irq;
-	int			swab;
+	u32			accessor_flags;
 	struct i2c_adapter	adapter;
 	u32			functionality;
 	u32			master_cfg;
@@ -90,6 +90,9 @@ struct dw_i2c_dev {
 	unsigned int		rx_fifo_depth;
 };
 
+#define ACCESS_SWAP		0x00000001
+#define ACCESS_16BIT		0x00000002
+
 extern u32 dw_readl(struct dw_i2c_dev *dev, int offset);
 extern void dw_writel(struct dw_i2c_dev *dev, u32 b, int offset);
 extern int i2c_dw_init(struct dw_i2c_dev *dev);
diff --git a/drivers/i2c/busses/i2c-designware-platdrv.c b/drivers/i2c/busses/i2c-designware-platdrv.c
index 4ba589ab861..0506fef8dc0 100644
--- a/drivers/i2c/busses/i2c-designware-platdrv.c
+++ b/drivers/i2c/busses/i2c-designware-platdrv.c
@@ -36,6 +36,7 @@
 #include <linux/interrupt.h>
 #include <linux/of_i2c.h>
 #include <linux/platform_device.h>
+#include <linux/pm.h>
 #include <linux/io.h>
 #include <linux/slab.h>
 #include "i2c-designware-core.h"
@@ -95,7 +96,7 @@ static int __devinit dw_i2c_probe(struct platform_device *pdev)
 		r = -ENODEV;
 		goto err_free_mem;
 	}
-	clk_enable(dev->clk);
+	clk_prepare_enable(dev->clk);
 
 	dev->functionality =
 		I2C_FUNC_I2C |
@@ -155,7 +156,7 @@ err_free_irq:
 err_iounmap:
 	iounmap(dev->base);
 err_unuse_clocks:
-	clk_disable(dev->clk);
+	clk_disable_unprepare(dev->clk);
 	clk_put(dev->clk);
 	dev->clk = NULL;
 err_free_mem:
@@ -177,7 +178,7 @@ static int __devexit dw_i2c_remove(struct platform_device *pdev)
 	i2c_del_adapter(&dev->adapter);
 	put_device(&pdev->dev);
 
-	clk_disable(dev->clk);
+	clk_disable_unprepare(dev->clk);
 	clk_put(dev->clk);
 	dev->clk = NULL;
 
@@ -198,6 +199,31 @@ static const struct of_device_id dw_i2c_of_match[] = {
 MODULE_DEVICE_TABLE(of, dw_i2c_of_match);
 #endif
 
+#ifdef CONFIG_PM
+static int dw_i2c_suspend(struct device *dev)
+{
+	struct platform_device *pdev = to_platform_device(dev);
+	struct dw_i2c_dev *i_dev = platform_get_drvdata(pdev);
+
+	clk_disable_unprepare(i_dev->clk);
+
+	return 0;
+}
+
+static int dw_i2c_resume(struct device *dev)
+{
+	struct platform_device *pdev = to_platform_device(dev);
+	struct dw_i2c_dev *i_dev = platform_get_drvdata(pdev);
+
+	clk_prepare_enable(i_dev->clk);
+	i2c_dw_init(i_dev);
+
+	return 0;
+}
+#endif
+
+static SIMPLE_DEV_PM_OPS(dw_i2c_dev_pm_ops, dw_i2c_suspend, dw_i2c_resume);
+
 /* work with hotplug and coldplug */
 MODULE_ALIAS("platform:i2c_designware");
 
@@ -207,6 +233,7 @@ static struct platform_driver dw_i2c_driver = {
 		.name	= "i2c_designware",
 		.owner	= THIS_MODULE,
 		.of_match_table = of_match_ptr(dw_i2c_of_match),
+		.pm	= &dw_i2c_dev_pm_ops,
 	},
 };
 
diff --git a/drivers/i2c/busses/i2c-eg20t.c b/drivers/i2c/busses/i2c-eg20t.c
index c811289b61e..2f74ae872e1 100644
--- a/drivers/i2c/busses/i2c-eg20t.c
+++ b/drivers/i2c/busses/i2c-eg20t.c
@@ -263,11 +263,6 @@ static void pch_i2c_init(struct i2c_algo_pch_data *adap)
 	init_waitqueue_head(&pch_event);
 }
 
-static inline bool ktime_lt(const ktime_t cmp1, const ktime_t cmp2)
-{
-	return cmp1.tv64 < cmp2.tv64;
-}
-
 /**
  * pch_i2c_wait_for_bus_idle() - check the status of bus.
  * @adap:	Pointer to struct i2c_algo_pch_data.
@@ -317,33 +312,6 @@ static void pch_i2c_start(struct i2c_algo_pch_data *adap)
 }
 
 /**
- * pch_i2c_wait_for_xfer_complete() - initiates a wait for the tx complete event
- * @adap:	Pointer to struct i2c_algo_pch_data.
- */
-static s32 pch_i2c_wait_for_xfer_complete(struct i2c_algo_pch_data *adap)
-{
-	long ret;
-	ret = wait_event_timeout(pch_event,
-			(adap->pch_event_flag != 0), msecs_to_jiffies(1000));
-
-	if (ret == 0) {
-		pch_err(adap, "timeout: %x\n", adap->pch_event_flag);
-		adap->pch_event_flag = 0;
-		return -ETIMEDOUT;
-	}
-
-	if (adap->pch_event_flag & I2C_ERROR_MASK) {
-		pch_err(adap, "error bits set: %x\n", adap->pch_event_flag);
-		adap->pch_event_flag = 0;
-		return -EIO;
-	}
-
-	adap->pch_event_flag = 0;
-
-	return 0;
-}
-
-/**
  * pch_i2c_getack() - to confirm ACK/NACK
  * @adap:	Pointer to struct i2c_algo_pch_data.
  */
@@ -373,6 +341,40 @@ static void pch_i2c_stop(struct i2c_algo_pch_data *adap)
 	pch_clrbit(adap->pch_base_address, PCH_I2CCTL, PCH_START);
 }
 
+static int pch_i2c_wait_for_check_xfer(struct i2c_algo_pch_data *adap)
+{
+	long ret;
+
+	ret = wait_event_timeout(pch_event,
+			(adap->pch_event_flag != 0), msecs_to_jiffies(1000));
+	if (!ret) {
+		pch_err(adap, "%s:wait-event timeout\n", __func__);
+		adap->pch_event_flag = 0;
+		pch_i2c_stop(adap);
+		pch_i2c_init(adap);
+		return -ETIMEDOUT;
+	}
+
+	if (adap->pch_event_flag & I2C_ERROR_MASK) {
+		pch_err(adap, "Lost Arbitration\n");
+		adap->pch_event_flag = 0;
+		pch_clrbit(adap->pch_base_address, PCH_I2CSR, I2CMAL_BIT);
+		pch_clrbit(adap->pch_base_address, PCH_I2CSR, I2CMIF_BIT);
+		pch_i2c_init(adap);
+		return -EAGAIN;
+	}
+
+	adap->pch_event_flag = 0;
+
+	if (pch_i2c_getack(adap)) {
+		pch_dbg(adap, "Receive NACK for slave address"
+			"setting\n");
+		return -EIO;
+	}
+
+	return 0;
+}
+
 /**
  * pch_i2c_repstart() - generate repeated start condition in normal mode
  * @adap:	Pointer to struct i2c_algo_pch_data.
@@ -427,27 +429,12 @@ static s32 pch_i2c_writebytes(struct i2c_adapter *i2c_adap,
 		if (first)
 			pch_i2c_start(adap);
 
-		rtn = pch_i2c_wait_for_xfer_complete(adap);
-		if (rtn == 0) {
-			if (pch_i2c_getack(adap)) {
-				pch_dbg(adap, "Receive NACK for slave address"
-					"setting\n");
-				return -EIO;
-			}
-			addr_8_lsb = (addr & I2C_ADDR_MSK);
-			iowrite32(addr_8_lsb, p + PCH_I2CDR);
-		} else if (rtn == -EIO) { /* Arbitration Lost */
-			pch_err(adap, "Lost Arbitration\n");
-			pch_clrbit(adap->pch_base_address, PCH_I2CSR,
-				   I2CMAL_BIT);
-			pch_clrbit(adap->pch_base_address, PCH_I2CSR,
-				   I2CMIF_BIT);
-			pch_i2c_init(adap);
-			return -EAGAIN;
-		} else { /* wait-event timeout */
-			pch_i2c_stop(adap);
-			return -ETIME;
-		}
+		rtn = pch_i2c_wait_for_check_xfer(adap);
+		if (rtn)
+			return rtn;
+
+		addr_8_lsb = (addr & I2C_ADDR_MSK);
+		iowrite32(addr_8_lsb, p + PCH_I2CDR);
 	} else {
 		/* set 7 bit slave address and R/W bit as 0 */
 		iowrite32(addr << 1, p + PCH_I2CDR);
@@ -455,44 +442,21 @@ static s32 pch_i2c_writebytes(struct i2c_adapter *i2c_adap,
 			pch_i2c_start(adap);
 	}
 
-	rtn = pch_i2c_wait_for_xfer_complete(adap);
-	if (rtn == 0) {
-		if (pch_i2c_getack(adap)) {
-			pch_dbg(adap, "Receive NACK for slave address"
-				"setting\n");
-			return -EIO;
-		}
-	} else if (rtn == -EIO) { /* Arbitration Lost */
-		pch_err(adap, "Lost Arbitration\n");
-		pch_clrbit(adap->pch_base_address, PCH_I2CSR, I2CMAL_BIT);
-		pch_clrbit(adap->pch_base_address, PCH_I2CSR, I2CMIF_BIT);
-		pch_i2c_init(adap);
-		return -EAGAIN;
-	} else { /* wait-event timeout */
-		pch_i2c_stop(adap);
-		return -ETIME;
-	}
+	rtn = pch_i2c_wait_for_check_xfer(adap);
+	if (rtn)
+		return rtn;
 
 	for (wrcount = 0; wrcount < length; ++wrcount) {
 		/* write buffer value to I2C data register */
 		iowrite32(buf[wrcount], p + PCH_I2CDR);
 		pch_dbg(adap, "writing %x to Data register\n", buf[wrcount]);
 
-		rtn = pch_i2c_wait_for_xfer_complete(adap);
-		if (rtn == 0) {
-			if (pch_i2c_getack(adap)) {
-				pch_dbg(adap, "Receive NACK for slave address"
-					"setting\n");
-				return -EIO;
-			}
-			pch_clrbit(adap->pch_base_address, PCH_I2CSR,
-				   I2CMCF_BIT);
-			pch_clrbit(adap->pch_base_address, PCH_I2CSR,
-				   I2CMIF_BIT);
-		} else { /* wait-event timeout */
-			pch_i2c_stop(adap);
-			return -ETIME;
-		}
+		rtn = pch_i2c_wait_for_check_xfer(adap);
+		if (rtn)
+			return rtn;
+
+		pch_clrbit(adap->pch_base_address, PCH_I2CSR, I2CMCF_BIT);
+		pch_clrbit(adap->pch_base_address, PCH_I2CSR, I2CMIF_BIT);
 	}
 
 	/* check if this is the last message */
@@ -580,50 +544,21 @@ static s32 pch_i2c_readbytes(struct i2c_adapter *i2c_adap, struct i2c_msg *msgs,
 		if (first)
 			pch_i2c_start(adap);
 
-		rtn = pch_i2c_wait_for_xfer_complete(adap);
-		if (rtn == 0) {
-			if (pch_i2c_getack(adap)) {
-				pch_dbg(adap, "Receive NACK for slave address"
-					"setting\n");
-				return -EIO;
-			}
-			addr_8_lsb = (addr & I2C_ADDR_MSK);
-			iowrite32(addr_8_lsb, p + PCH_I2CDR);
-		} else if (rtn == -EIO) { /* Arbitration Lost */
-			pch_err(adap, "Lost Arbitration\n");
-			pch_clrbit(adap->pch_base_address, PCH_I2CSR,
-				   I2CMAL_BIT);
-			pch_clrbit(adap->pch_base_address, PCH_I2CSR,
-				   I2CMIF_BIT);
-			pch_i2c_init(adap);
-			return -EAGAIN;
-		} else { /* wait-event timeout */
-			pch_i2c_stop(adap);
-			return -ETIME;
-		}
+		rtn = pch_i2c_wait_for_check_xfer(adap);
+		if (rtn)
+			return rtn;
+
+		addr_8_lsb = (addr & I2C_ADDR_MSK);
+		iowrite32(addr_8_lsb, p + PCH_I2CDR);
+
 		pch_i2c_restart(adap);
-		rtn = pch_i2c_wait_for_xfer_complete(adap);
-		if (rtn == 0) {
-			if (pch_i2c_getack(adap)) {
-				pch_dbg(adap, "Receive NACK for slave address"
-					"setting\n");
-				return -EIO;
-			}
-			addr_2_msb |= I2C_RD;
-			iowrite32(addr_2_msb | TEN_BIT_ADDR_MASK,
-				  p + PCH_I2CDR);
-		} else if (rtn == -EIO) { /* Arbitration Lost */
-			pch_err(adap, "Lost Arbitration\n");
-			pch_clrbit(adap->pch_base_address, PCH_I2CSR,
-				   I2CMAL_BIT);
-			pch_clrbit(adap->pch_base_address, PCH_I2CSR,
-				   I2CMIF_BIT);
-			pch_i2c_init(adap);
-			return -EAGAIN;
-		} else { /* wait-event timeout */
-			pch_i2c_stop(adap);
-			return -ETIME;
-		}
+
+		rtn = pch_i2c_wait_for_check_xfer(adap);
+		if (rtn)
+			return rtn;
+
+		addr_2_msb |= I2C_RD;
+		iowrite32(addr_2_msb | TEN_BIT_ADDR_MASK, p + PCH_I2CDR);
 	} else {
 		/* 7 address bits + R/W bit */
 		addr = (((addr) << 1) | (I2C_RD));
@@ -634,23 +569,9 @@ static s32 pch_i2c_readbytes(struct i2c_adapter *i2c_adap, struct i2c_msg *msgs,
 	if (first)
 		pch_i2c_start(adap);
 
-	rtn = pch_i2c_wait_for_xfer_complete(adap);
-	if (rtn == 0) {
-		if (pch_i2c_getack(adap)) {
-			pch_dbg(adap, "Receive NACK for slave address"
-				"setting\n");
-			return -EIO;
-		}
-	} else if (rtn == -EIO) { /* Arbitration Lost */
-		pch_err(adap, "Lost Arbitration\n");
-		pch_clrbit(adap->pch_base_address, PCH_I2CSR, I2CMAL_BIT);
-		pch_clrbit(adap->pch_base_address, PCH_I2CSR, I2CMIF_BIT);
-		pch_i2c_init(adap);
-		return -EAGAIN;
-	} else { /* wait-event timeout */
-		pch_i2c_stop(adap);
-		return -ETIME;
-	}
+	rtn = pch_i2c_wait_for_check_xfer(adap);
+	if (rtn)
+		return rtn;
 
 	if (length == 0) {
 		pch_i2c_stop(adap);
@@ -669,18 +590,9 @@ static s32 pch_i2c_readbytes(struct i2c_adapter *i2c_adap, struct i2c_msg *msgs,
 			if (loop != 1)
 				read_index++;
 
-			rtn = pch_i2c_wait_for_xfer_complete(adap);
-			if (rtn == 0) {
-				if (pch_i2c_getack(adap)) {
-					pch_dbg(adap, "Receive NACK for slave"
-						"address setting\n");
-					return -EIO;
-				}
-			} else { /* wait-event timeout */
-				pch_i2c_stop(adap);
-				return -ETIME;
-			}
-
+			rtn = pch_i2c_wait_for_check_xfer(adap);
+			if (rtn)
+				return rtn;
 		}	/* end for */
 
 		pch_i2c_sendnack(adap);
@@ -690,17 +602,9 @@ static s32 pch_i2c_readbytes(struct i2c_adapter *i2c_adap, struct i2c_msg *msgs,
 		if (length != 1)
 			read_index++;
 
-		rtn = pch_i2c_wait_for_xfer_complete(adap);
-		if (rtn == 0) {
-			if (pch_i2c_getack(adap)) {
-				pch_dbg(adap, "Receive NACK for slave"
-					"address setting\n");
-				return -EIO;
-			}
-		} else { /* wait-event timeout */
-			pch_i2c_stop(adap);
-			return -ETIME;
-		}
+		rtn = pch_i2c_wait_for_check_xfer(adap);
+		if (rtn)
+			return rtn;
 
 		if (last)
 			pch_i2c_stop(adap);
@@ -790,7 +694,7 @@ static s32 pch_i2c_xfer(struct i2c_adapter *i2c_adap,
 
 	ret = mutex_lock_interruptible(&pch_mutex);
 	if (ret)
-		return -ERESTARTSYS;
+		return ret;
 
 	if (adap->p_adapter_info->pch_i2c_suspended) {
 		mutex_unlock(&pch_mutex);
@@ -909,7 +813,7 @@ static int __devinit pch_i2c_probe(struct pci_dev *pdev,
 
 		pch_adap->owner = THIS_MODULE;
 		pch_adap->class = I2C_CLASS_HWMON;
-		strcpy(pch_adap->name, KBUILD_MODNAME);
+		strlcpy(pch_adap->name, KBUILD_MODNAME, sizeof(pch_adap->name));
 		pch_adap->algo = &pch_algorithm;
 		pch_adap->algo_data = &adap_info->pch_data[i];
 
@@ -963,7 +867,7 @@ static void __devexit pch_i2c_remove(struct pci_dev *pdev)
 		pci_iounmap(pdev, adap_info->pch_data[0].pch_base_address);
 
 	for (i = 0; i < adap_info->ch_num; i++)
-		adap_info->pch_data[i].pch_base_address = 0;
+		adap_info->pch_data[i].pch_base_address = NULL;
 
 	pci_set_drvdata(pdev, NULL);
 
diff --git a/drivers/i2c/busses/i2c-gpio.c b/drivers/i2c/busses/i2c-gpio.c
index c0330a41db0..e62d2d93862 100644
--- a/drivers/i2c/busses/i2c-gpio.c
+++ b/drivers/i2c/busses/i2c-gpio.c
@@ -190,12 +190,7 @@ static int __devinit i2c_gpio_probe(struct platform_device *pdev)
 	adap->dev.parent = &pdev->dev;
 	adap->dev.of_node = pdev->dev.of_node;
 
-	/*
-	 * If "dev->id" is negative we consider it as zero.
-	 * The reason to do so is to avoid sysfs names that only make
-	 * sense when there are multiple adapters.
-	 */
-	adap->nr = (pdev->id != -1) ? pdev->id : 0;
+	adap->nr = pdev->id;
 	ret = i2c_bit_add_numbered_bus(adap);
 	if (ret)
 		goto err_add_bus;
diff --git a/drivers/i2c/busses/i2c-imx.c b/drivers/i2c/busses/i2c-imx.c
index dfb84b7ee55..8d6b504d65c 100644
--- a/drivers/i2c/busses/i2c-imx.c
+++ b/drivers/i2c/busses/i2c-imx.c
@@ -51,6 +51,7 @@
 #include <linux/of.h>
 #include <linux/of_device.h>
 #include <linux/of_i2c.h>
+#include <linux/pinctrl/consumer.h>
 
 #include <mach/irqs.h>
 #include <mach/hardware.h>
@@ -470,6 +471,7 @@ static int __init i2c_imx_probe(struct platform_device *pdev)
 	struct imx_i2c_struct *i2c_imx;
 	struct resource *res;
 	struct imxi2c_platform_data *pdata = pdev->dev.platform_data;
+	struct pinctrl *pinctrl;
 	void __iomem *base;
 	resource_size_t res_size;
 	int irq, bitrate;
@@ -510,7 +512,7 @@ static int __init i2c_imx_probe(struct platform_device *pdev)
 	}
 
 	/* Setup i2c_imx driver structure */
-	strcpy(i2c_imx->adapter.name, pdev->name);
+	strlcpy(i2c_imx->adapter.name, pdev->name, sizeof(i2c_imx->adapter.name));
 	i2c_imx->adapter.owner		= THIS_MODULE;
 	i2c_imx->adapter.algo		= &i2c_imx_algo;
 	i2c_imx->adapter.dev.parent	= &pdev->dev;
@@ -520,6 +522,12 @@ static int __init i2c_imx_probe(struct platform_device *pdev)
 	i2c_imx->base			= base;
 	i2c_imx->res			= res;
 
+	pinctrl = devm_pinctrl_get_select_default(&pdev->dev);
+	if (IS_ERR(pinctrl)) {
+		ret = PTR_ERR(pinctrl);
+		goto fail3;
+	}
+
 	/* Get I2C clock */
 	i2c_imx->clk = clk_get(&pdev->dev, "i2c_clk");
 	if (IS_ERR(i2c_imx->clk)) {
diff --git a/drivers/i2c/busses/i2c-ixp2000.c b/drivers/i2c/busses/i2c-ixp2000.c
deleted file mode 100644
index 5d263f9014d..00000000000
--- a/drivers/i2c/busses/i2c-ixp2000.c
+++ /dev/null
@@ -1,157 +0,0 @@
-/*
- * drivers/i2c/busses/i2c-ixp2000.c
- *
- * I2C adapter for IXP2000 systems using GPIOs for I2C bus
- *
- * Author: Deepak Saxena <dsaxena@plexity.net>
- * Based on IXDP2400 code by: Naeem M. Afzal <naeem.m.afzal@intel.com>
- * Made generic by: Jeff Daly <jeffrey.daly@intel.com>
- *
- * Copyright (c) 2003-2004 MontaVista Software Inc.
- *
- * 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.
- *
- * From Jeff Daly:
- *
- * I2C adapter driver for Intel IXDP2xxx platforms. This should work for any
- * IXP2000 platform if it uses the HW GPIO in the same manner.  Basically, 
- * SDA and SCL GPIOs have external pullups.  Setting the respective GPIO to 
- * an input will make the signal a '1' via the pullup.  Setting them to 
- * outputs will pull them down. 
- *
- * The GPIOs are open drain signals and are used as configuration strap inputs
- * during power-up so there's generally a buffer on the board that needs to be 
- * 'enabled' to drive the GPIOs.
- */
-
-#include <linux/kernel.h>
-#include <linux/init.h>
-#include <linux/platform_device.h>
-#include <linux/module.h>
-#include <linux/i2c.h>
-#include <linux/i2c-algo-bit.h>
-#include <linux/slab.h>
-
-#include <mach/hardware.h>	/* Pick up IXP2000-specific bits */
-#include <mach/gpio-ixp2000.h>
-
-static inline int ixp2000_scl_pin(void *data)
-{
-	return ((struct ixp2000_i2c_pins*)data)->scl_pin;
-}
-
-static inline int ixp2000_sda_pin(void *data)
-{
-	return ((struct ixp2000_i2c_pins*)data)->sda_pin;
-}
-
-
-static void ixp2000_bit_setscl(void *data, int val)
-{
-	int i = 5000;
-
-	if (val) {
-		gpio_line_config(ixp2000_scl_pin(data), GPIO_IN);
-		while(!gpio_line_get(ixp2000_scl_pin(data)) && i--);
-	} else {
-		gpio_line_config(ixp2000_scl_pin(data), GPIO_OUT);
-	}
-}
-
-static void ixp2000_bit_setsda(void *data, int val)
-{
-	if (val) {
-		gpio_line_config(ixp2000_sda_pin(data), GPIO_IN);
-	} else {
-		gpio_line_config(ixp2000_sda_pin(data), GPIO_OUT);
-	}
-}
-
-static int ixp2000_bit_getscl(void *data)
-{
-	return gpio_line_get(ixp2000_scl_pin(data));
-}
-
-static int ixp2000_bit_getsda(void *data)
-{
-	return gpio_line_get(ixp2000_sda_pin(data));
-}
-
-struct ixp2000_i2c_data {
-	struct ixp2000_i2c_pins *gpio_pins;
-	struct i2c_adapter adapter;
-	struct i2c_algo_bit_data algo_data;
-};
-
-static int ixp2000_i2c_remove(struct platform_device *plat_dev)
-{
-	struct ixp2000_i2c_data *drv_data = platform_get_drvdata(plat_dev);
-
-	platform_set_drvdata(plat_dev, NULL);
-
-	i2c_del_adapter(&drv_data->adapter);
-
-	kfree(drv_data);
-
-	return 0;
-}
-
-static int ixp2000_i2c_probe(struct platform_device *plat_dev)
-{
-	int err;
-	struct ixp2000_i2c_pins *gpio = plat_dev->dev.platform_data;
-	struct ixp2000_i2c_data *drv_data = 
-		kzalloc(sizeof(struct ixp2000_i2c_data), GFP_KERNEL);
-
-	if (!drv_data)
-		return -ENOMEM;
-	drv_data->gpio_pins = gpio;
-
-	drv_data->algo_data.data = gpio;
-	drv_data->algo_data.setsda = ixp2000_bit_setsda;
-	drv_data->algo_data.setscl = ixp2000_bit_setscl;
-	drv_data->algo_data.getsda = ixp2000_bit_getsda;
-	drv_data->algo_data.getscl = ixp2000_bit_getscl;
-	drv_data->algo_data.udelay = 6;
-	drv_data->algo_data.timeout = HZ;
-
-	strlcpy(drv_data->adapter.name, plat_dev->dev.driver->name,
-		sizeof(drv_data->adapter.name));
-	drv_data->adapter.algo_data = &drv_data->algo_data,
-
-	drv_data->adapter.dev.parent = &plat_dev->dev;
-
-	gpio_line_config(gpio->sda_pin, GPIO_IN);
-	gpio_line_config(gpio->scl_pin, GPIO_IN);
-	gpio_line_set(gpio->scl_pin, 0);
-	gpio_line_set(gpio->sda_pin, 0);
-
-	if ((err = i2c_bit_add_bus(&drv_data->adapter)) != 0) {
-		dev_err(&plat_dev->dev, "Could not install, error %d\n", err);
-		kfree(drv_data);
-		return err;
-	} 
-
-	platform_set_drvdata(plat_dev, drv_data);
-
-	return 0;
-}
-
-static struct platform_driver ixp2000_i2c_driver = {
-	.probe		= ixp2000_i2c_probe,
-	.remove		= ixp2000_i2c_remove,
-	.driver		= {
-		.name	= "IXP2000-I2C",
-		.owner	= THIS_MODULE,
-	},
-};
-
-module_platform_driver(ixp2000_i2c_driver);
-
-MODULE_AUTHOR ("Deepak Saxena <dsaxena@plexity.net>");
-MODULE_DESCRIPTION("IXP2000 GPIO-based I2C bus driver");
-MODULE_LICENSE("GPL");
-MODULE_ALIAS("platform:IXP2000-I2C");
-
diff --git a/drivers/i2c/busses/i2c-mpc.c b/drivers/i2c/busses/i2c-mpc.c
index 206caacd30d..b76731edbf1 100644
--- a/drivers/i2c/busses/i2c-mpc.c
+++ b/drivers/i2c/busses/i2c-mpc.c
@@ -64,6 +64,9 @@ struct mpc_i2c {
 	struct i2c_adapter adap;
 	int irq;
 	u32 real_clk;
+#ifdef CONFIG_PM
+	u8 fdr, dfsrr;
+#endif
 };
 
 struct mpc_i2c_divider {
@@ -703,6 +706,30 @@ static int __devexit fsl_i2c_remove(struct platform_device *op)
 	return 0;
 };
 
+#ifdef CONFIG_PM
+static int mpc_i2c_suspend(struct device *dev)
+{
+	struct mpc_i2c *i2c = dev_get_drvdata(dev);
+
+	i2c->fdr = readb(i2c->base + MPC_I2C_FDR);
+	i2c->dfsrr = readb(i2c->base + MPC_I2C_DFSRR);
+
+	return 0;
+}
+
+static int mpc_i2c_resume(struct device *dev)
+{
+	struct mpc_i2c *i2c = dev_get_drvdata(dev);
+
+	writeb(i2c->fdr, i2c->base + MPC_I2C_FDR);
+	writeb(i2c->dfsrr, i2c->base + MPC_I2C_DFSRR);
+
+	return 0;
+}
+
+SIMPLE_DEV_PM_OPS(mpc_i2c_pm_ops, mpc_i2c_suspend, mpc_i2c_resume);
+#endif
+
 static struct mpc_i2c_data mpc_i2c_data_512x __devinitdata = {
 	.setup = mpc_i2c_setup_512x,
 };
@@ -747,6 +774,9 @@ static struct platform_driver mpc_i2c_driver = {
 		.owner = THIS_MODULE,
 		.name = DRV_NAME,
 		.of_match_table = mpc_i2c_of_match,
+#ifdef CONFIG_PM
+		.pm = &mpc_i2c_pm_ops,
+#endif
 	},
 };
 
diff --git a/drivers/i2c/busses/i2c-mxs.c b/drivers/i2c/busses/i2c-mxs.c
index 76b8af44f63..04eb441b6ce 100644
--- a/drivers/i2c/busses/i2c-mxs.c
+++ b/drivers/i2c/busses/i2c-mxs.c
@@ -26,8 +26,11 @@
 #include <linux/platform_device.h>
 #include <linux/jiffies.h>
 #include <linux/io.h>
-
-#include <mach/common.h>
+#include <linux/pinctrl/consumer.h>
+#include <linux/stmp_device.h>
+#include <linux/of.h>
+#include <linux/of_device.h>
+#include <linux/of_i2c.h>
 
 #define DRIVER_NAME "mxs-i2c"
 
@@ -111,13 +114,9 @@ struct mxs_i2c_dev {
 	struct i2c_adapter adapter;
 };
 
-/*
- * TODO: check if calls to here are really needed. If not, we could get rid of
- * mxs_reset_block and the mach-dependency. Needs an I2C analyzer, probably.
- */
 static void mxs_i2c_reset(struct mxs_i2c_dev *i2c)
 {
-	mxs_reset_block(i2c->regs);
+	stmp_reset_block(i2c->regs);
 	writel(MXS_I2C_IRQ_MASK << 8, i2c->regs + MXS_I2C_CTRL1_SET);
 	writel(MXS_I2C_QUEUECTRL_PIO_QUEUE_MODE,
 			i2c->regs + MXS_I2C_QUEUECTRL_SET);
@@ -325,10 +324,15 @@ static int __devinit mxs_i2c_probe(struct platform_device *pdev)
 	struct device *dev = &pdev->dev;
 	struct mxs_i2c_dev *i2c;
 	struct i2c_adapter *adap;
+	struct pinctrl *pinctrl;
 	struct resource *res;
 	resource_size_t res_size;
 	int err, irq;
 
+	pinctrl = devm_pinctrl_get_select_default(dev);
+	if (IS_ERR(pinctrl))
+		return PTR_ERR(pinctrl);
+
 	i2c = devm_kzalloc(dev, sizeof(struct mxs_i2c_dev), GFP_KERNEL);
 	if (!i2c)
 		return -ENOMEM;
@@ -365,6 +369,7 @@ static int __devinit mxs_i2c_probe(struct platform_device *pdev)
 	adap->algo = &mxs_i2c_algo;
 	adap->dev.parent = dev;
 	adap->nr = pdev->id;
+	adap->dev.of_node = pdev->dev.of_node;
 	i2c_set_adapdata(adap, i2c);
 	err = i2c_add_numbered_adapter(adap);
 	if (err) {
@@ -374,6 +379,8 @@ static int __devinit mxs_i2c_probe(struct platform_device *pdev)
 		return err;
 	}
 
+	of_i2c_register_devices(adap);
+
 	return 0;
 }
 
@@ -393,10 +400,17 @@ static int __devexit mxs_i2c_remove(struct platform_device *pdev)
 	return 0;
 }
 
+static const struct of_device_id mxs_i2c_dt_ids[] = {
+	{ .compatible = "fsl,imx28-i2c", },
+	{ /* sentinel */ }
+};
+MODULE_DEVICE_TABLE(of, mxs_i2c_dt_ids);
+
 static struct platform_driver mxs_i2c_driver = {
 	.driver = {
 		   .name = DRIVER_NAME,
 		   .owner = THIS_MODULE,
+		   .of_match_table = mxs_i2c_dt_ids,
 		   },
 	.remove = __devexit_p(mxs_i2c_remove),
 };
diff --git a/drivers/i2c/busses/i2c-ocores.c b/drivers/i2c/busses/i2c-ocores.c
index 18068dee48f..75194c579b6 100644
--- a/drivers/i2c/busses/i2c-ocores.c
+++ b/drivers/i2c/busses/i2c-ocores.c
@@ -55,6 +55,7 @@
 #include <linux/i2c-ocores.h>
 #include <linux/slab.h>
 #include <linux/io.h>
+#include <linux/of_i2c.h>
 
 struct ocores_i2c {
 	void __iomem *base;
@@ -343,6 +344,8 @@ static int __devinit ocores_i2c_probe(struct platform_device *pdev)
 	if (pdata) {
 		for (i = 0; i < pdata->num_devices; i++)
 			i2c_new_device(&i2c->adap, pdata->devices + i);
+	} else {
+		of_i2c_register_devices(&i2c->adap);
 	}
 
 	return 0;
diff --git a/drivers/i2c/busses/i2c-pca-platform.c b/drivers/i2c/busses/i2c-pca-platform.c
index 2adbf1a8fde..675878f49f7 100644
--- a/drivers/i2c/busses/i2c-pca-platform.c
+++ b/drivers/i2c/busses/i2c-pca-platform.c
@@ -171,7 +171,7 @@ static int __devinit i2c_pca_pf_probe(struct platform_device *pdev)
 	i2c->io_size = resource_size(res);
 	i2c->irq = irq;
 
-	i2c->adap.nr = pdev->id >= 0 ? pdev->id : 0;
+	i2c->adap.nr = pdev->id;
 	i2c->adap.owner = THIS_MODULE;
 	snprintf(i2c->adap.name, sizeof(i2c->adap.name),
 		 "PCA9564/PCA9665 at 0x%08lx",
diff --git a/drivers/i2c/busses/i2c-pnx.c b/drivers/i2c/busses/i2c-pnx.c
index eb8ad538c79..99389d2eae5 100644
--- a/drivers/i2c/busses/i2c-pnx.c
+++ b/drivers/i2c/busses/i2c-pnx.c
@@ -23,16 +23,61 @@
 #include <linux/err.h>
 #include <linux/clk.h>
 #include <linux/slab.h>
+#include <linux/of_i2c.h>
+
+#define I2C_PNX_TIMEOUT_DEFAULT		10 /* msec */
+#define I2C_PNX_SPEED_KHZ_DEFAULT	100
+#define I2C_PNX_REGION_SIZE		0x100
+
+enum {
+	mstatus_tdi = 0x00000001,
+	mstatus_afi = 0x00000002,
+	mstatus_nai = 0x00000004,
+	mstatus_drmi = 0x00000008,
+	mstatus_active = 0x00000020,
+	mstatus_scl = 0x00000040,
+	mstatus_sda = 0x00000080,
+	mstatus_rff = 0x00000100,
+	mstatus_rfe = 0x00000200,
+	mstatus_tff = 0x00000400,
+	mstatus_tfe = 0x00000800,
+};
 
-#include <mach/hardware.h>
-#include <mach/i2c.h>
+enum {
+	mcntrl_tdie = 0x00000001,
+	mcntrl_afie = 0x00000002,
+	mcntrl_naie = 0x00000004,
+	mcntrl_drmie = 0x00000008,
+	mcntrl_daie = 0x00000020,
+	mcntrl_rffie = 0x00000040,
+	mcntrl_tffie = 0x00000080,
+	mcntrl_reset = 0x00000100,
+	mcntrl_cdbmode = 0x00000400,
+};
 
-#define I2C_PNX_TIMEOUT		10 /* msec */
-#define I2C_PNX_SPEED_KHZ	100
-#define I2C_PNX_REGION_SIZE	0x100
+enum {
+	rw_bit = 1 << 0,
+	start_bit = 1 << 8,
+	stop_bit = 1 << 9,
+};
 
-static inline int wait_timeout(long timeout, struct i2c_pnx_algo_data *data)
+#define I2C_REG_RX(a)	((a)->ioaddr)		/* Rx FIFO reg (RO) */
+#define I2C_REG_TX(a)	((a)->ioaddr)		/* Tx FIFO reg (WO) */
+#define I2C_REG_STS(a)	((a)->ioaddr + 0x04)	/* Status reg (RO) */
+#define I2C_REG_CTL(a)	((a)->ioaddr + 0x08)	/* Ctl reg */
+#define I2C_REG_CKL(a)	((a)->ioaddr + 0x0c)	/* Clock divider low */
+#define I2C_REG_CKH(a)	((a)->ioaddr + 0x10)	/* Clock divider high */
+#define I2C_REG_ADR(a)	((a)->ioaddr + 0x14)	/* I2C address */
+#define I2C_REG_RFL(a)	((a)->ioaddr + 0x18)	/* Rx FIFO level (RO) */
+#define I2C_REG_TFL(a)	((a)->ioaddr + 0x1c)	/* Tx FIFO level (RO) */
+#define I2C_REG_RXB(a)	((a)->ioaddr + 0x20)	/* Num of bytes Rx-ed (RO) */
+#define I2C_REG_TXB(a)	((a)->ioaddr + 0x24)	/* Num of bytes Tx-ed (RO) */
+#define I2C_REG_TXS(a)	((a)->ioaddr + 0x28)	/* Tx slave FIFO (RO) */
+#define I2C_REG_STFL(a)	((a)->ioaddr + 0x2c)	/* Tx slave FIFO level (RO) */
+
+static inline int wait_timeout(struct i2c_pnx_algo_data *data)
 {
+	long timeout = data->timeout;
 	while (timeout > 0 &&
 			(ioread32(I2C_REG_STS(data)) & mstatus_active)) {
 		mdelay(1);
@@ -41,8 +86,9 @@ static inline int wait_timeout(long timeout, struct i2c_pnx_algo_data *data)
 	return (timeout <= 0);
 }
 
-static inline int wait_reset(long timeout, struct i2c_pnx_algo_data *data)
+static inline int wait_reset(struct i2c_pnx_algo_data *data)
 {
+	long timeout = data->timeout;
 	while (timeout > 0 &&
 			(ioread32(I2C_REG_CTL(data)) & mcntrl_reset)) {
 		mdelay(1);
@@ -54,7 +100,7 @@ static inline int wait_reset(long timeout, struct i2c_pnx_algo_data *data)
 static inline void i2c_pnx_arm_timer(struct i2c_pnx_algo_data *alg_data)
 {
 	struct timer_list *timer = &alg_data->mif.timer;
-	unsigned long expires = msecs_to_jiffies(I2C_PNX_TIMEOUT);
+	unsigned long expires = msecs_to_jiffies(alg_data->timeout);
 
 	if (expires <= 1)
 		expires = 2;
@@ -92,7 +138,7 @@ static int i2c_pnx_start(unsigned char slave_addr,
 	}
 
 	/* First, make sure bus is idle */
-	if (wait_timeout(I2C_PNX_TIMEOUT, alg_data)) {
+	if (wait_timeout(alg_data)) {
 		/* Somebody else is monopolizing the bus */
 		dev_err(&alg_data->adapter.dev,
 			"%s: Bus busy. Slave addr = %02x, cntrl = %x, stat = %x\n",
@@ -185,7 +231,7 @@ static int i2c_pnx_master_xmit(struct i2c_pnx_algo_data *alg_data)
 		if (alg_data->mif.len == 0) {
 			if (alg_data->last) {
 				/* Wait until the STOP is seen. */
-				if (wait_timeout(I2C_PNX_TIMEOUT, alg_data))
+				if (wait_timeout(alg_data))
 					dev_err(&alg_data->adapter.dev,
 						"The bus is still active after timeout\n");
 			}
@@ -283,7 +329,7 @@ static int i2c_pnx_master_rcv(struct i2c_pnx_algo_data *alg_data)
 		if (alg_data->mif.len == 0) {
 			if (alg_data->last)
 				/* Wait until the STOP is seen. */
-				if (wait_timeout(I2C_PNX_TIMEOUT, alg_data))
+				if (wait_timeout(alg_data))
 					dev_err(&alg_data->adapter.dev,
 						"The bus is still active after timeout\n");
 
@@ -399,7 +445,7 @@ static void i2c_pnx_timeout(unsigned long data)
 
 	ctl |= mcntrl_reset;
 	iowrite32(ctl, I2C_REG_CTL(alg_data));
-	wait_reset(I2C_PNX_TIMEOUT, alg_data);
+	wait_reset(alg_data);
 	alg_data->mif.ret = -EIO;
 	complete(&alg_data->mif.complete);
 }
@@ -414,18 +460,18 @@ static inline void bus_reset_if_active(struct i2c_pnx_algo_data *alg_data)
 			alg_data->adapter.name);
 		iowrite32(ioread32(I2C_REG_CTL(alg_data)) | mcntrl_reset,
 			  I2C_REG_CTL(alg_data));
-		wait_reset(I2C_PNX_TIMEOUT, alg_data);
+		wait_reset(alg_data);
 	} else if (!(stat & mstatus_rfe) || !(stat & mstatus_tfe)) {
 		/* If there is data in the fifo's after transfer,
 		 * flush fifo's by reset.
 		 */
 		iowrite32(ioread32(I2C_REG_CTL(alg_data)) | mcntrl_reset,
 			  I2C_REG_CTL(alg_data));
-		wait_reset(I2C_PNX_TIMEOUT, alg_data);
+		wait_reset(alg_data);
 	} else if (stat & mstatus_nai) {
 		iowrite32(ioread32(I2C_REG_CTL(alg_data)) | mcntrl_reset,
 			  I2C_REG_CTL(alg_data));
-		wait_reset(I2C_PNX_TIMEOUT, alg_data);
+		wait_reset(alg_data);
 	}
 }
 
@@ -568,14 +614,8 @@ static int __devinit i2c_pnx_probe(struct platform_device *pdev)
 	int ret = 0;
 	struct i2c_pnx_algo_data *alg_data;
 	unsigned long freq;
-	struct i2c_pnx_data *i2c_pnx = pdev->dev.platform_data;
-
-	if (!i2c_pnx || !i2c_pnx->name) {
-		dev_err(&pdev->dev, "%s: no platform data supplied\n",
-		       __func__);
-		ret = -EINVAL;
-		goto out;
-	}
+	struct resource *res;
+	u32 speed = I2C_PNX_SPEED_KHZ_DEFAULT * 1000;
 
 	alg_data = kzalloc(sizeof(*alg_data), GFP_KERNEL);
 	if (!alg_data) {
@@ -585,14 +625,27 @@ static int __devinit i2c_pnx_probe(struct platform_device *pdev)
 
 	platform_set_drvdata(pdev, alg_data);
 
-	strlcpy(alg_data->adapter.name, i2c_pnx->name,
-		sizeof(alg_data->adapter.name));
 	alg_data->adapter.dev.parent = &pdev->dev;
 	alg_data->adapter.algo = &pnx_algorithm;
 	alg_data->adapter.algo_data = alg_data;
 	alg_data->adapter.nr = pdev->id;
-	alg_data->i2c_pnx = i2c_pnx;
 
+	alg_data->timeout = I2C_PNX_TIMEOUT_DEFAULT;
+#ifdef CONFIG_OF
+	alg_data->adapter.dev.of_node = of_node_get(pdev->dev.of_node);
+	if (pdev->dev.of_node) {
+		of_property_read_u32(pdev->dev.of_node, "clock-frequency",
+				     &speed);
+		/*
+		 * At this point, it is planned to add an OF timeout property.
+		 * As soon as there is a consensus about how to call and handle
+		 * this, sth. like the following can be put here:
+		 *
+		 * of_property_read_u32(pdev->dev.of_node, "timeout",
+		 *                      &alg_data->timeout);
+		 */
+	}
+#endif
 	alg_data->clk = clk_get(&pdev->dev, NULL);
 	if (IS_ERR(alg_data->clk)) {
 		ret = PTR_ERR(alg_data->clk);
@@ -603,17 +656,27 @@ static int __devinit i2c_pnx_probe(struct platform_device *pdev)
 	alg_data->mif.timer.function = i2c_pnx_timeout;
 	alg_data->mif.timer.data = (unsigned long)alg_data;
 
+	snprintf(alg_data->adapter.name, sizeof(alg_data->adapter.name),
+		 "%s", pdev->name);
+
 	/* Register I/O resource */
-	if (!request_mem_region(i2c_pnx->base, I2C_PNX_REGION_SIZE,
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	if (!res) {
+		dev_err(&pdev->dev, "Unable to get mem resource.\n");
+		ret = -EBUSY;
+		goto out_clkget;
+	}
+	if (!request_mem_region(res->start, I2C_PNX_REGION_SIZE,
 				pdev->name)) {
 		dev_err(&pdev->dev,
 		       "I/O region 0x%08x for I2C already in use.\n",
-		       i2c_pnx->base);
-		ret = -ENODEV;
+		       res->start);
+		ret = -ENOMEM;
 		goto out_clkget;
 	}
 
-	alg_data->ioaddr = ioremap(i2c_pnx->base, I2C_PNX_REGION_SIZE);
+	alg_data->base = res->start;
+	alg_data->ioaddr = ioremap(res->start, I2C_PNX_REGION_SIZE);
 	if (!alg_data->ioaddr) {
 		dev_err(&pdev->dev, "Couldn't ioremap I2C I/O region\n");
 		ret = -ENOMEM;
@@ -637,20 +700,25 @@ static int __devinit i2c_pnx_probe(struct platform_device *pdev)
 	 * the deglitching filter length.
 	 */
 
-	tmp = ((freq / 1000) / I2C_PNX_SPEED_KHZ) / 2 - 2;
+	tmp = (freq / speed) / 2 - 2;
 	if (tmp > 0x3FF)
 		tmp = 0x3FF;
 	iowrite32(tmp, I2C_REG_CKH(alg_data));
 	iowrite32(tmp, I2C_REG_CKL(alg_data));
 
 	iowrite32(mcntrl_reset, I2C_REG_CTL(alg_data));
-	if (wait_reset(I2C_PNX_TIMEOUT, alg_data)) {
+	if (wait_reset(alg_data)) {
 		ret = -ENODEV;
 		goto out_clock;
 	}
 	init_completion(&alg_data->mif.complete);
 
-	ret = request_irq(i2c_pnx->irq, i2c_pnx_interrupt,
+	alg_data->irq = platform_get_irq(pdev, 0);
+	if (alg_data->irq < 0) {
+		dev_err(&pdev->dev, "Failed to get IRQ from platform resource\n");
+		goto out_irq;
+	}
+	ret = request_irq(alg_data->irq, i2c_pnx_interrupt,
 			0, pdev->name, alg_data);
 	if (ret)
 		goto out_clock;
@@ -662,39 +730,39 @@ static int __devinit i2c_pnx_probe(struct platform_device *pdev)
 		goto out_irq;
 	}
 
+	of_i2c_register_devices(&alg_data->adapter);
+
 	dev_dbg(&pdev->dev, "%s: Master at %#8x, irq %d.\n",
-	       alg_data->adapter.name, i2c_pnx->base, i2c_pnx->irq);
+		alg_data->adapter.name, res->start, alg_data->irq);
 
 	return 0;
 
 out_irq:
-	free_irq(i2c_pnx->irq, alg_data);
+	free_irq(alg_data->irq, alg_data);
 out_clock:
 	clk_disable(alg_data->clk);
 out_unmap:
 	iounmap(alg_data->ioaddr);
 out_release:
-	release_mem_region(i2c_pnx->base, I2C_PNX_REGION_SIZE);
+	release_mem_region(res->start, I2C_PNX_REGION_SIZE);
 out_clkget:
 	clk_put(alg_data->clk);
 out_drvdata:
 	kfree(alg_data);
 err_kzalloc:
 	platform_set_drvdata(pdev, NULL);
-out:
 	return ret;
 }
 
 static int __devexit i2c_pnx_remove(struct platform_device *pdev)
 {
 	struct i2c_pnx_algo_data *alg_data = platform_get_drvdata(pdev);
-	struct i2c_pnx_data *i2c_pnx = alg_data->i2c_pnx;
 
-	free_irq(i2c_pnx->irq, alg_data);
+	free_irq(alg_data->irq, alg_data);
 	i2c_del_adapter(&alg_data->adapter);
 	clk_disable(alg_data->clk);
 	iounmap(alg_data->ioaddr);
-	release_mem_region(i2c_pnx->base, I2C_PNX_REGION_SIZE);
+	release_mem_region(alg_data->base, I2C_PNX_REGION_SIZE);
 	clk_put(alg_data->clk);
 	kfree(alg_data);
 	platform_set_drvdata(pdev, NULL);
@@ -702,10 +770,19 @@ static int __devexit i2c_pnx_remove(struct platform_device *pdev)
 	return 0;
 }
 
+#ifdef CONFIG_OF
+static const struct of_device_id i2c_pnx_of_match[] = {
+	{ .compatible = "nxp,pnx-i2c" },
+	{ },
+};
+MODULE_DEVICE_TABLE(of, i2c_pnx_of_match);
+#endif
+
 static struct platform_driver i2c_pnx_driver = {
 	.driver = {
 		.name = "pnx-i2c",
 		.owner = THIS_MODULE,
+		.of_match_table = of_match_ptr(i2c_pnx_of_match),
 	},
 	.probe = i2c_pnx_probe,
 	.remove = __devexit_p(i2c_pnx_remove),
diff --git a/drivers/i2c/busses/i2c-powermac.c b/drivers/i2c/busses/i2c-powermac.c
index 7b397c6f607..31c47e18d83 100644
--- a/drivers/i2c/busses/i2c-powermac.c
+++ b/drivers/i2c/busses/i2c-powermac.c
@@ -227,6 +227,72 @@ static int __devexit i2c_powermac_remove(struct platform_device *dev)
 	return 0;
 }
 
+static void __devinit i2c_powermac_register_devices(struct i2c_adapter *adap,
+						    struct pmac_i2c_bus *bus)
+{
+	struct i2c_client *newdev;
+	struct device_node *node;
+
+	for_each_child_of_node(adap->dev.of_node, node) {
+		struct i2c_board_info info = {};
+		struct dev_archdata dev_ad = {};
+		const __be32 *reg;
+		char tmp[16];
+		u32 addr;
+		int len;
+
+		/* Get address & channel */
+		reg = of_get_property(node, "reg", &len);
+		if (!reg || (len < sizeof(int))) {
+			dev_err(&adap->dev, "i2c-powermac: invalid reg on %s\n",
+				node->full_name);
+			continue;
+		}
+		addr = be32_to_cpup(reg);
+
+		/* Multibus setup, check channel */
+		if (!pmac_i2c_match_adapter(node, adap))
+			continue;
+
+		dev_dbg(&adap->dev, "i2c-powermac: register %s\n",
+			node->full_name);
+
+		/* Make up a modalias. Note: we to _NOT_ want the standard
+		 * i2c drivers to match with any of our powermac stuff
+		 * unless they have been specifically modified to handle
+		 * it on a case by case basis. For example, for thermal
+		 * control, things like lm75 etc... shall match with their
+		 * corresponding windfarm drivers, _NOT_ the generic ones,
+		 * so we force a prefix of AAPL, onto the modalias to
+		 * make that happen
+		 */
+		if (of_modalias_node(node, tmp, sizeof(tmp)) < 0) {
+			dev_err(&adap->dev, "i2c-powermac: modalias failure"
+				" on %s\n", node->full_name);
+			continue;
+		}
+		snprintf(info.type, sizeof(info.type), "MAC,%s", tmp);
+
+		/* Fill out the rest of the info structure */
+		info.addr = (addr & 0xff) >> 1;
+		info.irq = irq_of_parse_and_map(node, 0);
+		info.of_node = of_node_get(node);
+		info.archdata = &dev_ad;
+
+		newdev = i2c_new_device(adap, &info);
+		if (!newdev) {
+			dev_err(&adap->dev, "i2c-powermac: Failure to register"
+				" %s\n", node->full_name);
+			of_node_put(node);
+			/* We do not dispose of the interrupt mapping on
+			 * purpose. It's not necessary (interrupt cannot be
+			 * re-used) and somebody else might have grabbed it
+			 * via direct DT lookup so let's not bother
+			 */
+			continue;
+		}
+	}
+}
 
 static int __devinit i2c_powermac_probe(struct platform_device *dev)
 {
@@ -272,6 +338,7 @@ static int __devinit i2c_powermac_probe(struct platform_device *dev)
 	adapter->algo = &i2c_powermac_algorithm;
 	i2c_set_adapdata(adapter, bus);
 	adapter->dev.parent = &dev->dev;
+	adapter->dev.of_node = dev->dev.of_node;
 	rc = i2c_add_adapter(adapter);
 	if (rc) {
 		printk(KERN_ERR "i2c-powermac: Adapter %s registration "
@@ -281,33 +348,10 @@ static int __devinit i2c_powermac_probe(struct platform_device *dev)
 
 	printk(KERN_INFO "PowerMac i2c bus %s registered\n", adapter->name);
 
-	if (!strncmp(basename, "uni-n", 5)) {
-		struct device_node *np;
-		const u32 *prop;
-		struct i2c_board_info info;
-
-		/* Instantiate I2C motion sensor if present */
-		np = of_find_node_by_name(NULL, "accelerometer");
-		if (np && of_device_is_compatible(np, "AAPL,accelerometer_1") &&
-		    (prop = of_get_property(np, "reg", NULL))) {
-			int i2c_bus;
-			const char *tmp_bus;
-
-			/* look for bus either using "reg" or by path */
-			tmp_bus = strstr(np->full_name, "/i2c-bus@");
-			if (tmp_bus)
-				i2c_bus = *(tmp_bus + 9) - '0';
-			else
-				i2c_bus = ((*prop) >> 8) & 0x0f;
-
-			if (pmac_i2c_get_channel(bus) == i2c_bus) {
-				memset(&info, 0, sizeof(struct i2c_board_info));
-				info.addr = ((*prop) & 0xff) >> 1;
-				strlcpy(info.type, "ams", I2C_NAME_SIZE);
-				i2c_new_device(adapter, &info);
-			}
-		}
-	}
+	/* Cannot use of_i2c_register_devices() due to Apple device-tree
+	 * funkyness
+	 */
+	i2c_powermac_register_devices(adapter, bus);
 
 	return rc;
 }
diff --git a/drivers/i2c/busses/i2c-pxa.c b/drivers/i2c/busses/i2c-pxa.c
index f6733267fa9..a997c7d3f95 100644
--- a/drivers/i2c/busses/i2c-pxa.c
+++ b/drivers/i2c/busses/i2c-pxa.c
@@ -1131,11 +1131,6 @@ static int i2c_pxa_probe(struct platform_device *dev)
 	spin_lock_init(&i2c->lock);
 	init_waitqueue_head(&i2c->wait);
 
-	/*
-	 * If "dev->id" is negative we consider it as zero.
-	 * The reason to do so is to avoid sysfs names that only make
-	 * sense when there are multiple adapters.
-	 */
 	i2c->adap.nr = dev->id;
 	snprintf(i2c->adap.name, sizeof(i2c->adap.name), "pxa_i2c-i2c.%u",
 		 i2c->adap.nr);
diff --git a/drivers/i2c/busses/i2c-s3c2410.c b/drivers/i2c/busses/i2c-s3c2410.c
index 737f7218a32..fa0b1349087 100644
--- a/drivers/i2c/busses/i2c-s3c2410.c
+++ b/drivers/i2c/busses/i2c-s3c2410.c
@@ -44,8 +44,12 @@
 #include <plat/regs-iic.h>
 #include <plat/iic.h>
 
-/* i2c controller state */
+/* Treat S3C2410 as baseline hardware, anything else is supported via quirks */
+#define QUIRK_S3C2440		(1 << 0)
+#define QUIRK_HDMIPHY		(1 << 1)
+#define QUIRK_NO_GPIO		(1 << 2)
 
+/* i2c controller state */
 enum s3c24xx_i2c_state {
 	STATE_IDLE,
 	STATE_START,
@@ -54,14 +58,10 @@ enum s3c24xx_i2c_state {
 	STATE_STOP
 };
 
-enum s3c24xx_i2c_type {
-	TYPE_S3C2410,
-	TYPE_S3C2440,
-};
-
 struct s3c24xx_i2c {
 	spinlock_t		lock;
 	wait_queue_head_t	wait;
+	unsigned int            quirks;
 	unsigned int		suspended:1;
 
 	struct i2c_msg		*msg;
@@ -88,26 +88,45 @@ struct s3c24xx_i2c {
 #endif
 };
 
-/* default platform data removed, dev should always carry data. */
+static struct platform_device_id s3c24xx_driver_ids[] = {
+	{
+		.name		= "s3c2410-i2c",
+		.driver_data	= 0,
+	}, {
+		.name		= "s3c2440-i2c",
+		.driver_data	= QUIRK_S3C2440,
+	}, {
+		.name		= "s3c2440-hdmiphy-i2c",
+		.driver_data	= QUIRK_S3C2440 | QUIRK_HDMIPHY | QUIRK_NO_GPIO,
+	}, { },
+};
+MODULE_DEVICE_TABLE(platform, s3c24xx_driver_ids);
+
+#ifdef CONFIG_OF
+static const struct of_device_id s3c24xx_i2c_match[] = {
+	{ .compatible = "samsung,s3c2410-i2c", .data = (void *)0 },
+	{ .compatible = "samsung,s3c2440-i2c", .data = (void *)QUIRK_S3C2440 },
+	{ .compatible = "samsung,s3c2440-hdmiphy-i2c",
+	  .data = (void *)(QUIRK_S3C2440 | QUIRK_HDMIPHY | QUIRK_NO_GPIO) },
+	{},
+};
+MODULE_DEVICE_TABLE(of, s3c24xx_i2c_match);
+#endif
 
-/* s3c24xx_i2c_is2440()
+/* s3c24xx_get_device_quirks
  *
- * return true is this is an s3c2440
+ * Get controller type either from device tree or platform device variant.
 */
 
-static inline int s3c24xx_i2c_is2440(struct s3c24xx_i2c *i2c)
+static inline unsigned int s3c24xx_get_device_quirks(struct platform_device *pdev)
 {
-	struct platform_device *pdev = to_platform_device(i2c->dev);
-	enum s3c24xx_i2c_type type;
-
-#ifdef CONFIG_OF
-	if (i2c->dev->of_node)
-		return of_device_is_compatible(i2c->dev->of_node,
-				"samsung,s3c2440-i2c");
-#endif
+	if (pdev->dev.of_node) {
+		const struct of_device_id *match;
+		match = of_match_node(&s3c24xx_i2c_match, pdev->dev.of_node);
+		return (unsigned int)match->data;
+	}
 
-	type = platform_get_device_id(pdev)->driver_data;
-	return type == TYPE_S3C2440;
+	return platform_get_device_id(pdev)->driver_data;
 }
 
 /* s3c24xx_i2c_master_complete
@@ -471,6 +490,13 @@ static int s3c24xx_i2c_set_master(struct s3c24xx_i2c *i2c)
 	unsigned long iicstat;
 	int timeout = 400;
 
+	/* the timeout for HDMIPHY is reduced to 10 ms because
+	 * the hangup is expected to happen, so waiting 400 ms
+	 * causes only unnecessary system hangup
+	 */
+	if (i2c->quirks & QUIRK_HDMIPHY)
+		timeout = 10;
+
 	while (timeout-- > 0) {
 		iicstat = readl(i2c->regs + S3C2410_IICSTAT);
 
@@ -480,6 +506,15 @@ static int s3c24xx_i2c_set_master(struct s3c24xx_i2c *i2c)
 		msleep(1);
 	}
 
+	/* hang-up of bus dedicated for HDMIPHY occurred, resetting */
+	if (i2c->quirks & QUIRK_HDMIPHY) {
+		writel(0, i2c->regs + S3C2410_IICCON);
+		writel(0, i2c->regs + S3C2410_IICSTAT);
+		writel(0, i2c->regs + S3C2410_IICDS);
+
+		return 0;
+	}
+
 	return -ETIMEDOUT;
 }
 
@@ -676,7 +711,7 @@ static int s3c24xx_i2c_clockrate(struct s3c24xx_i2c *i2c, unsigned int *got)
 
 	writel(iiccon, i2c->regs + S3C2410_IICCON);
 
-	if (s3c24xx_i2c_is2440(i2c)) {
+	if (i2c->quirks & QUIRK_S3C2440) {
 		unsigned long sda_delay;
 
 		if (pdata->sda_delay) {
@@ -761,6 +796,9 @@ static int s3c24xx_i2c_parse_dt_gpio(struct s3c24xx_i2c *i2c)
 {
 	int idx, gpio, ret;
 
+	if (i2c->quirks & QUIRK_NO_GPIO)
+		return 0;
+
 	for (idx = 0; idx < 2; idx++) {
 		gpio = of_get_gpio(i2c->dev->of_node, idx);
 		if (!gpio_is_valid(gpio)) {
@@ -785,6 +823,10 @@ free_gpio:
 static void s3c24xx_i2c_dt_gpio_free(struct s3c24xx_i2c *i2c)
 {
 	unsigned int idx;
+
+	if (i2c->quirks & QUIRK_NO_GPIO)
+		return;
+
 	for (idx = 0; idx < 2; idx++)
 		gpio_free(i2c->gpios[idx]);
 }
@@ -906,6 +948,7 @@ static int s3c24xx_i2c_probe(struct platform_device *pdev)
 		goto err_noclk;
 	}
 
+	i2c->quirks = s3c24xx_get_device_quirks(pdev);
 	if (pdata)
 		memcpy(i2c->pdata, pdata, sizeof(*pdata));
 	else
@@ -1110,28 +1153,6 @@ static const struct dev_pm_ops s3c24xx_i2c_dev_pm_ops = {
 
 /* device driver for platform bus bits */
 
-static struct platform_device_id s3c24xx_driver_ids[] = {
-	{
-		.name		= "s3c2410-i2c",
-		.driver_data	= TYPE_S3C2410,
-	}, {
-		.name		= "s3c2440-i2c",
-		.driver_data	= TYPE_S3C2440,
-	}, { },
-};
-MODULE_DEVICE_TABLE(platform, s3c24xx_driver_ids);
-
-#ifdef CONFIG_OF
-static const struct of_device_id s3c24xx_i2c_match[] = {
-	{ .compatible = "samsung,s3c2410-i2c" },
-	{ .compatible = "samsung,s3c2440-i2c" },
-	{},
-};
-MODULE_DEVICE_TABLE(of, s3c24xx_i2c_match);
-#else
-#define s3c24xx_i2c_match NULL
-#endif
-
 static struct platform_driver s3c24xx_i2c_driver = {
 	.probe		= s3c24xx_i2c_probe,
 	.remove		= s3c24xx_i2c_remove,
@@ -1140,7 +1161,7 @@ static struct platform_driver s3c24xx_i2c_driver = {
 		.owner	= THIS_MODULE,
 		.name	= "s3c-i2c",
 		.pm	= S3C24XX_DEV_PM_OPS,
-		.of_match_table = s3c24xx_i2c_match,
+		.of_match_table = of_match_ptr(s3c24xx_i2c_match),
 	},
 };
 
diff --git a/drivers/i2c/busses/i2c-s6000.c b/drivers/i2c/busses/i2c-s6000.c
index c64ba736f48..b76a29d1f8e 100644
--- a/drivers/i2c/busses/i2c-s6000.c
+++ b/drivers/i2c/busses/i2c-s6000.c
@@ -3,7 +3,7 @@
  *
  * Description: Driver for S6000 Family I2C Interface
  * Copyright (c) 2008 emlix GmbH
- * Author:	Oskar Schirmer <os@emlix.com>
+ * Author:	Oskar Schirmer <oskar@scara.com>
  *
  * Partially based on i2c-bfin-twi.c driver by <sonic.zhang@analog.com>
  * Copyright (c) 2005-2007 Analog Devices, Inc.
diff --git a/drivers/i2c/busses/i2c-s6000.h b/drivers/i2c/busses/i2c-s6000.h
index ff23b81ded4..4936f9f2256 100644
--- a/drivers/i2c/busses/i2c-s6000.h
+++ b/drivers/i2c/busses/i2c-s6000.h
@@ -6,7 +6,7 @@
  * for more details.
  *
  * Copyright (C) 2008 Emlix GmbH <info@emlix.com>
- * Author:	Oskar Schirmer <os@emlix.com>
+ * Author:	Oskar Schirmer <oskar@scara.com>
  */
 
 #ifndef __DRIVERS_I2C_BUSSES_I2C_S6000_H
diff --git a/drivers/i2c/busses/i2c-sh_mobile.c b/drivers/i2c/busses/i2c-sh_mobile.c
index 675c9692d14..8110ca45f34 100644
--- a/drivers/i2c/busses/i2c-sh_mobile.c
+++ b/drivers/i2c/busses/i2c-sh_mobile.c
@@ -27,6 +27,7 @@
 #include <linux/platform_device.h>
 #include <linux/interrupt.h>
 #include <linux/i2c.h>
+#include <linux/of_i2c.h>
 #include <linux/err.h>
 #include <linux/pm_runtime.h>
 #include <linux/clk.h>
@@ -653,6 +654,7 @@ static int sh_mobile_i2c_probe(struct platform_device *dev)
 	adap->dev.parent = &dev->dev;
 	adap->retries = 5;
 	adap->nr = dev->id;
+	adap->dev.of_node = dev->dev.of_node;
 
 	strlcpy(adap->name, dev->name, sizeof(adap->name));
 
@@ -667,6 +669,8 @@ static int sh_mobile_i2c_probe(struct platform_device *dev)
 
 	dev_info(&dev->dev, "I2C adapter %d with bus speed %lu Hz\n",
 		 adap->nr, pd->bus_speed);
+
+	of_i2c_register_devices(adap);
 	return 0;
 
  err_all:
@@ -710,11 +714,18 @@ static const struct dev_pm_ops sh_mobile_i2c_dev_pm_ops = {
 	.runtime_resume = sh_mobile_i2c_runtime_nop,
 };
 
+static const struct of_device_id sh_mobile_i2c_dt_ids[] __devinitconst = {
+	{ .compatible = "renesas,rmobile-iic", },
+	{},
+};
+MODULE_DEVICE_TABLE(of, sh_mobile_i2c_dt_ids);
+
 static struct platform_driver sh_mobile_i2c_driver = {
 	.driver		= {
 		.name		= "i2c-sh_mobile",
 		.owner		= THIS_MODULE,
 		.pm		= &sh_mobile_i2c_dev_pm_ops,
+		.of_match_table = sh_mobile_i2c_dt_ids,
 	},
 	.probe		= sh_mobile_i2c_probe,
 	.remove		= sh_mobile_i2c_remove,
diff --git a/drivers/i2c/busses/i2c-tegra.c b/drivers/i2c/busses/i2c-tegra.c
index 55e5ea62cce..8b2e555a956 100644
--- a/drivers/i2c/busses/i2c-tegra.c
+++ b/drivers/i2c/busses/i2c-tegra.c
@@ -401,8 +401,6 @@ static irqreturn_t tegra_i2c_isr(int irq, void *dev_id)
 			disable_irq_nosync(i2c_dev->irq);
 			i2c_dev->irq_disabled = 1;
 		}
-
-		complete(&i2c_dev->msg_complete);
 		goto err;
 	}
 
@@ -411,7 +409,6 @@ static irqreturn_t tegra_i2c_isr(int irq, void *dev_id)
 			i2c_dev->msg_err |= I2C_ERR_NO_ACK;
 		if (status & I2C_INT_ARBITRATION_LOST)
 			i2c_dev->msg_err |= I2C_ERR_ARBITRATION_LOST;
-		complete(&i2c_dev->msg_complete);
 		goto err;
 	}
 
@@ -429,14 +426,14 @@ static irqreturn_t tegra_i2c_isr(int irq, void *dev_id)
 			tegra_i2c_mask_irq(i2c_dev, I2C_INT_TX_FIFO_DATA_REQ);
 	}
 
+	i2c_writel(i2c_dev, status, I2C_INT_STATUS);
+	if (i2c_dev->is_dvc)
+		dvc_writel(i2c_dev, DVC_STATUS_I2C_DONE_INTR, DVC_STATUS);
+
 	if (status & I2C_INT_PACKET_XFER_COMPLETE) {
 		BUG_ON(i2c_dev->msg_buf_remaining);
 		complete(&i2c_dev->msg_complete);
 	}
-
-	i2c_writel(i2c_dev, status, I2C_INT_STATUS);
-	if (i2c_dev->is_dvc)
-		dvc_writel(i2c_dev, DVC_STATUS_I2C_DONE_INTR, DVC_STATUS);
 	return IRQ_HANDLED;
 err:
 	/* An error occurred, mask all interrupts */
@@ -446,6 +443,8 @@ err:
 	i2c_writel(i2c_dev, status, I2C_INT_STATUS);
 	if (i2c_dev->is_dvc)
 		dvc_writel(i2c_dev, DVC_STATUS_I2C_DONE_INTR, DVC_STATUS);
+
+	complete(&i2c_dev->msg_complete);
 	return IRQ_HANDLED;
 }
 
@@ -476,12 +475,15 @@ static int tegra_i2c_xfer_msg(struct tegra_i2c_dev *i2c_dev,
 	packet_header = msg->len - 1;
 	i2c_writel(i2c_dev, packet_header, I2C_TX_FIFO);
 
-	packet_header = msg->addr << I2C_HEADER_SLAVE_ADDR_SHIFT;
-	packet_header |= I2C_HEADER_IE_ENABLE;
+	packet_header = I2C_HEADER_IE_ENABLE;
 	if (!stop)
 		packet_header |= I2C_HEADER_REPEAT_START;
-	if (msg->flags & I2C_M_TEN)
+	if (msg->flags & I2C_M_TEN) {
+		packet_header |= msg->addr;
 		packet_header |= I2C_HEADER_10BIT_ADDR;
+	} else {
+		packet_header |= msg->addr << I2C_HEADER_SLAVE_ADDR_SHIFT;
+	}
 	if (msg->flags & I2C_M_IGNORE_NAK)
 		packet_header |= I2C_HEADER_CONT_ON_NAK;
 	if (msg->flags & I2C_M_RD)
@@ -557,7 +559,7 @@ static int tegra_i2c_xfer(struct i2c_adapter *adap, struct i2c_msg msgs[],
 
 static u32 tegra_i2c_func(struct i2c_adapter *adap)
 {
-	return I2C_FUNC_I2C | I2C_FUNC_SMBUS_EMUL;
+	return I2C_FUNC_I2C | I2C_FUNC_SMBUS_EMUL | I2C_FUNC_10BIT_ADDR;
 }
 
 static const struct i2c_algorithm tegra_i2c_algo = {
diff --git a/drivers/i2c/busses/i2c-versatile.c b/drivers/i2c/busses/i2c-versatile.c
index f585aead50c..eec20db6246 100644
--- a/drivers/i2c/busses/i2c-versatile.c
+++ b/drivers/i2c/busses/i2c-versatile.c
@@ -104,13 +104,8 @@ static int i2c_versatile_probe(struct platform_device *dev)
 	i2c->algo = i2c_versatile_algo;
 	i2c->algo.data = i2c;
 
-	if (dev->id >= 0) {
-		/* static bus numbering */
-		i2c->adap.nr = dev->id;
-		ret = i2c_bit_add_numbered_bus(&i2c->adap);
-	} else
-		/* dynamic bus numbering */
-		ret = i2c_bit_add_bus(&i2c->adap);
+	i2c->adap.nr = dev->id;
+	ret = i2c_bit_add_numbered_bus(&i2c->adap);
 	if (ret >= 0) {
 		platform_set_drvdata(dev, i2c);
 		of_i2c_register_devices(&i2c->adap);
diff --git a/drivers/i2c/busses/i2c-xiic.c b/drivers/i2c/busses/i2c-xiic.c
index 2bded7647ef..641d0e5e330 100644
--- a/drivers/i2c/busses/i2c-xiic.c
+++ b/drivers/i2c/busses/i2c-xiic.c
@@ -40,6 +40,7 @@
 #include <linux/i2c-xiic.h>
 #include <linux/io.h>
 #include <linux/slab.h>
+#include <linux/of_i2c.h>
 
 #define DRIVER_NAME "xiic-i2c"
 
@@ -705,8 +706,6 @@ static int __devinit xiic_i2c_probe(struct platform_device *pdev)
 		goto resource_missing;
 
 	pdata = (struct xiic_i2c_platform_data *) pdev->dev.platform_data;
-	if (!pdata)
-		return -EINVAL;
 
 	i2c = kzalloc(sizeof(*i2c), GFP_KERNEL);
 	if (!i2c)
@@ -730,6 +729,7 @@ static int __devinit xiic_i2c_probe(struct platform_device *pdev)
 	i2c->adap = xiic_adapter;
 	i2c_set_adapdata(&i2c->adap, i2c);
 	i2c->adap.dev.parent = &pdev->dev;
+	i2c->adap.dev.of_node = pdev->dev.of_node;
 
 	xiic_reinit(i2c);
 
@@ -748,9 +748,13 @@ static int __devinit xiic_i2c_probe(struct platform_device *pdev)
 		goto add_adapter_failed;
 	}
 
-	/* add in known devices to the bus */
-	for (i = 0; i < pdata->num_devices; i++)
-		i2c_new_device(&i2c->adap, pdata->devices + i);
+	if (pdata) {
+		/* add in known devices to the bus */
+		for (i = 0; i < pdata->num_devices; i++)
+			i2c_new_device(&i2c->adap, pdata->devices + i);
+	}
+
+	of_i2c_register_devices(&i2c->adap);
 
 	return 0;
 
@@ -795,12 +799,21 @@ static int __devexit xiic_i2c_remove(struct platform_device* pdev)
 	return 0;
 }
 
+#if defined(CONFIG_OF)
+static const struct of_device_id xiic_of_match[] __devinitconst = {
+	{ .compatible = "xlnx,xps-iic-2.00.a", },
+	{},
+};
+MODULE_DEVICE_TABLE(of, xiic_of_match);
+#endif
+
 static struct platform_driver xiic_i2c_driver = {
 	.probe   = xiic_i2c_probe,
 	.remove  = __devexit_p(xiic_i2c_remove),
 	.driver  = {
 		.owner = THIS_MODULE,
 		.name = DRIVER_NAME,
+		.of_match_table = of_match_ptr(xiic_of_match),
 	},
 };
 
diff --git a/drivers/i2c/i2c-core.c b/drivers/i2c/i2c-core.c
index feb7dc35918..a6ad32bc0a9 100644
--- a/drivers/i2c/i2c-core.c
+++ b/drivers/i2c/i2c-core.c
@@ -772,6 +772,23 @@ struct device_type i2c_adapter_type = {
 };
 EXPORT_SYMBOL_GPL(i2c_adapter_type);
 
+/**
+ * i2c_verify_adapter - return parameter as i2c_adapter or NULL
+ * @dev: device, probably from some driver model iterator
+ *
+ * When traversing the driver model tree, perhaps using driver model
+ * iterators like @device_for_each_child(), you can't assume very much
+ * about the nodes you find.  Use this function to avoid oopses caused
+ * by wrongly treating some non-I2C device as an i2c_adapter.
+ */
+struct i2c_adapter *i2c_verify_adapter(struct device *dev)
+{
+	return (dev->type == &i2c_adapter_type)
+			? to_i2c_adapter(dev)
+			: NULL;
+}
+EXPORT_SYMBOL(i2c_verify_adapter);
+
 #ifdef CONFIG_I2C_COMPAT
 static struct class_compat *i2c_adapter_compat_class;
 #endif
diff --git a/drivers/i2c/i2c-mux.c b/drivers/i2c/i2c-mux.c
index d7a4833be41..1038c381aea 100644
--- a/drivers/i2c/i2c-mux.c
+++ b/drivers/i2c/i2c-mux.c
@@ -24,6 +24,8 @@
 #include <linux/slab.h>
 #include <linux/i2c.h>
 #include <linux/i2c-mux.h>
+#include <linux/of.h>
+#include <linux/of_i2c.h>
 
 /* multiplexer per channel data */
 struct i2c_mux_priv {
@@ -31,11 +33,11 @@ struct i2c_mux_priv {
 	struct i2c_algorithm algo;
 
 	struct i2c_adapter *parent;
-	void *mux_dev;	/* the mux chip/device */
+	void *mux_priv;	/* the mux chip/device */
 	u32  chan_id;	/* the channel id */
 
-	int (*select)(struct i2c_adapter *, void *mux_dev, u32 chan_id);
-	int (*deselect)(struct i2c_adapter *, void *mux_dev, u32 chan_id);
+	int (*select)(struct i2c_adapter *, void *mux_priv, u32 chan_id);
+	int (*deselect)(struct i2c_adapter *, void *mux_priv, u32 chan_id);
 };
 
 static int i2c_mux_master_xfer(struct i2c_adapter *adap,
@@ -47,11 +49,11 @@ static int i2c_mux_master_xfer(struct i2c_adapter *adap,
 
 	/* Switch to the right mux port and perform the transfer. */
 
-	ret = priv->select(parent, priv->mux_dev, priv->chan_id);
+	ret = priv->select(parent, priv->mux_priv, priv->chan_id);
 	if (ret >= 0)
 		ret = parent->algo->master_xfer(parent, msgs, num);
 	if (priv->deselect)
-		priv->deselect(parent, priv->mux_dev, priv->chan_id);
+		priv->deselect(parent, priv->mux_priv, priv->chan_id);
 
 	return ret;
 }
@@ -67,12 +69,12 @@ static int i2c_mux_smbus_xfer(struct i2c_adapter *adap,
 
 	/* Select the right mux port and perform the transfer. */
 
-	ret = priv->select(parent, priv->mux_dev, priv->chan_id);
+	ret = priv->select(parent, priv->mux_priv, priv->chan_id);
 	if (ret >= 0)
 		ret = parent->algo->smbus_xfer(parent, addr, flags,
 					read_write, command, size, data);
 	if (priv->deselect)
-		priv->deselect(parent, priv->mux_dev, priv->chan_id);
+		priv->deselect(parent, priv->mux_priv, priv->chan_id);
 
 	return ret;
 }
@@ -87,7 +89,8 @@ static u32 i2c_mux_functionality(struct i2c_adapter *adap)
 }
 
 struct i2c_adapter *i2c_add_mux_adapter(struct i2c_adapter *parent,
-				void *mux_dev, u32 force_nr, u32 chan_id,
+				struct device *mux_dev,
+				void *mux_priv, u32 force_nr, u32 chan_id,
 				int (*select) (struct i2c_adapter *,
 					       void *, u32),
 				int (*deselect) (struct i2c_adapter *,
@@ -102,7 +105,7 @@ struct i2c_adapter *i2c_add_mux_adapter(struct i2c_adapter *parent,
 
 	/* Set up private adapter data */
 	priv->parent = parent;
-	priv->mux_dev = mux_dev;
+	priv->mux_priv = mux_priv;
 	priv->chan_id = chan_id;
 	priv->select = select;
 	priv->deselect = deselect;
@@ -124,6 +127,25 @@ struct i2c_adapter *i2c_add_mux_adapter(struct i2c_adapter *parent,
 	priv->adap.algo_data = priv;
 	priv->adap.dev.parent = &parent->dev;
 
+	/*
+	 * Try to populate the mux adapter's of_node, expands to
+	 * nothing if !CONFIG_OF.
+	 */
+	if (mux_dev->of_node) {
+		struct device_node *child;
+		u32 reg;
+
+		for_each_child_of_node(mux_dev->of_node, child) {
+			ret = of_property_read_u32(child, "reg", &reg);
+			if (ret)
+				continue;
+			if (chan_id == reg) {
+				priv->adap.dev.of_node = child;
+				break;
+			}
+		}
+	}
+
 	if (force_nr) {
 		priv->adap.nr = force_nr;
 		ret = i2c_add_numbered_adapter(&priv->adap);
@@ -141,6 +163,8 @@ struct i2c_adapter *i2c_add_mux_adapter(struct i2c_adapter *parent,
 	dev_info(&parent->dev, "Added multiplexed i2c bus %d\n",
 		 i2c_adapter_id(&priv->adap));
 
+	of_i2c_register_devices(&priv->adap);
+
 	return &priv->adap;
 }
 EXPORT_SYMBOL_GPL(i2c_add_mux_adapter);
diff --git a/drivers/i2c/muxes/Kconfig b/drivers/i2c/muxes/Kconfig
index 90b7a016389..beb2491db27 100644
--- a/drivers/i2c/muxes/Kconfig
+++ b/drivers/i2c/muxes/Kconfig
@@ -15,7 +15,7 @@ config I2C_MUX_GPIO
 	  through GPIO pins.
 
 	  This driver can also be built as a module.  If so, the module
-	  will be called gpio-i2cmux.
+	  will be called i2c-mux-gpio.
 
 config I2C_MUX_PCA9541
 	tristate "NXP PCA9541 I2C Master Selector"
@@ -25,7 +25,7 @@ config I2C_MUX_PCA9541
 	  I2C Master Selector.
 
 	  This driver can also be built as a module.  If so, the module
-	  will be called pca9541.
+	  will be called i2c-mux-pca9541.
 
 config I2C_MUX_PCA954x
 	tristate "Philips PCA954x I2C Mux/switches"
@@ -35,6 +35,6 @@ config I2C_MUX_PCA954x
 	  I2C mux/switch devices.
 
 	  This driver can also be built as a module.  If so, the module
-	  will be called pca954x.
+	  will be called i2c-mux-pca954x.
 
 endmenu
diff --git a/drivers/i2c/muxes/Makefile b/drivers/i2c/muxes/Makefile
index 4640436ea61..5826249b29c 100644
--- a/drivers/i2c/muxes/Makefile
+++ b/drivers/i2c/muxes/Makefile
@@ -1,8 +1,8 @@
 #
 # Makefile for multiplexer I2C chip drivers.
 
-obj-$(CONFIG_I2C_MUX_GPIO)	+= gpio-i2cmux.o
-obj-$(CONFIG_I2C_MUX_PCA9541)	+= pca9541.o
-obj-$(CONFIG_I2C_MUX_PCA954x)	+= pca954x.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
 
 ccflags-$(CONFIG_I2C_DEBUG_BUS) := -DDEBUG
diff --git a/drivers/i2c/muxes/gpio-i2cmux.c b/drivers/i2c/muxes/i2c-mux-gpio.c
index e5fa695eb0f..68b1f8ec343 100644
--- a/drivers/i2c/muxes/gpio-i2cmux.c
+++ b/drivers/i2c/muxes/i2c-mux-gpio.c
@@ -10,7 +10,7 @@
 
 #include <linux/i2c.h>
 #include <linux/i2c-mux.h>
-#include <linux/gpio-i2cmux.h>
+#include <linux/i2c-mux-gpio.h>
 #include <linux/platform_device.h>
 #include <linux/init.h>
 #include <linux/module.h>
@@ -20,10 +20,10 @@
 struct gpiomux {
 	struct i2c_adapter *parent;
 	struct i2c_adapter **adap; /* child busses */
-	struct gpio_i2cmux_platform_data data;
+	struct i2c_mux_gpio_platform_data data;
 };
 
-static void gpiomux_set(const struct gpiomux *mux, unsigned val)
+static void i2c_mux_gpio_set(const struct gpiomux *mux, unsigned val)
 {
 	int i;
 
@@ -31,28 +31,28 @@ static void gpiomux_set(const struct gpiomux *mux, unsigned val)
 		gpio_set_value(mux->data.gpios[i], val & (1 << i));
 }
 
-static int gpiomux_select(struct i2c_adapter *adap, void *data, u32 chan)
+static int i2c_mux_gpio_select(struct i2c_adapter *adap, void *data, u32 chan)
 {
 	struct gpiomux *mux = data;
 
-	gpiomux_set(mux, mux->data.values[chan]);
+	i2c_mux_gpio_set(mux, mux->data.values[chan]);
 
 	return 0;
 }
 
-static int gpiomux_deselect(struct i2c_adapter *adap, void *data, u32 chan)
+static int i2c_mux_gpio_deselect(struct i2c_adapter *adap, void *data, u32 chan)
 {
 	struct gpiomux *mux = data;
 
-	gpiomux_set(mux, mux->data.idle);
+	i2c_mux_gpio_set(mux, mux->data.idle);
 
 	return 0;
 }
 
-static int __devinit gpiomux_probe(struct platform_device *pdev)
+static int __devinit i2c_mux_gpio_probe(struct platform_device *pdev)
 {
 	struct gpiomux *mux;
-	struct gpio_i2cmux_platform_data *pdata;
+	struct i2c_mux_gpio_platform_data *pdata;
 	struct i2c_adapter *parent;
 	int (*deselect) (struct i2c_adapter *, void *, u32);
 	unsigned initial_state;
@@ -86,16 +86,16 @@ static int __devinit gpiomux_probe(struct platform_device *pdev)
 		goto alloc_failed2;
 	}
 
-	if (pdata->idle != GPIO_I2CMUX_NO_IDLE) {
+	if (pdata->idle != I2C_MUX_GPIO_NO_IDLE) {
 		initial_state = pdata->idle;
-		deselect = gpiomux_deselect;
+		deselect = i2c_mux_gpio_deselect;
 	} else {
 		initial_state = pdata->values[0];
 		deselect = NULL;
 	}
 
 	for (i = 0; i < pdata->n_gpios; i++) {
-		ret = gpio_request(pdata->gpios[i], "gpio-i2cmux");
+		ret = gpio_request(pdata->gpios[i], "i2c-mux-gpio");
 		if (ret)
 			goto err_request_gpio;
 		gpio_direction_output(pdata->gpios[i],
@@ -105,8 +105,8 @@ static int __devinit gpiomux_probe(struct platform_device *pdev)
 	for (i = 0; i < pdata->n_values; i++) {
 		u32 nr = pdata->base_nr ? (pdata->base_nr + i) : 0;
 
-		mux->adap[i] = i2c_add_mux_adapter(parent, mux, nr, i,
-						   gpiomux_select, deselect);
+		mux->adap[i] = i2c_add_mux_adapter(parent, &pdev->dev, mux, nr, i,
+						   i2c_mux_gpio_select, deselect);
 		if (!mux->adap[i]) {
 			ret = -ENODEV;
 			dev_err(&pdev->dev, "Failed to add adapter %d\n", i);
@@ -137,7 +137,7 @@ alloc_failed:
 	return ret;
 }
 
-static int __devexit gpiomux_remove(struct platform_device *pdev)
+static int __devexit i2c_mux_gpio_remove(struct platform_device *pdev)
 {
 	struct gpiomux *mux = platform_get_drvdata(pdev);
 	int i;
@@ -156,18 +156,18 @@ static int __devexit gpiomux_remove(struct platform_device *pdev)
 	return 0;
 }
 
-static struct platform_driver gpiomux_driver = {
-	.probe	= gpiomux_probe,
-	.remove	= __devexit_p(gpiomux_remove),
+static struct platform_driver i2c_mux_gpio_driver = {
+	.probe	= i2c_mux_gpio_probe,
+	.remove	= __devexit_p(i2c_mux_gpio_remove),
 	.driver	= {
 		.owner	= THIS_MODULE,
-		.name	= "gpio-i2cmux",
+		.name	= "i2c-mux-gpio",
 	},
 };
 
-module_platform_driver(gpiomux_driver);
+module_platform_driver(i2c_mux_gpio_driver);
 
 MODULE_DESCRIPTION("GPIO-based I2C multiplexer driver");
 MODULE_AUTHOR("Peter Korsgaard <peter.korsgaard@barco.com>");
 MODULE_LICENSE("GPL");
-MODULE_ALIAS("platform:gpio-i2cmux");
+MODULE_ALIAS("platform:i2c-mux-gpio");
diff --git a/drivers/i2c/muxes/pca9541.c b/drivers/i2c/muxes/i2c-mux-pca9541.c
index e0df9b6c66b..8aacde1516a 100644
--- a/drivers/i2c/muxes/pca9541.c
+++ b/drivers/i2c/muxes/i2c-mux-pca9541.c
@@ -353,7 +353,8 @@ static int pca9541_probe(struct i2c_client *client,
 	force = 0;
 	if (pdata)
 		force = pdata->modes[0].adap_id;
-	data->mux_adap = i2c_add_mux_adapter(adap, client, force, 0,
+	data->mux_adap = i2c_add_mux_adapter(adap, &client->dev, client,
+					     force, 0,
 					     pca9541_select_chan,
 					     pca9541_release_chan);
 
diff --git a/drivers/i2c/muxes/pca954x.c b/drivers/i2c/muxes/i2c-mux-pca954x.c
index 0e37ef27aa1..f2dfe0d8fcc 100644
--- a/drivers/i2c/muxes/pca954x.c
+++ b/drivers/i2c/muxes/i2c-mux-pca954x.c
@@ -226,7 +226,7 @@ static int pca954x_probe(struct i2c_client *client,
 		}
 
 		data->virt_adaps[num] =
-			i2c_add_mux_adapter(adap, client,
+			i2c_add_mux_adapter(adap, &client->dev, client,
 				force, num, pca954x_select_chan,
 				(pdata && pdata->modes[num].deselect_on_exit)
 					? pca954x_deselect_mux : NULL);
diff --git a/drivers/ieee802154/Kconfig b/drivers/ieee802154/Kconfig
index 9b9f43aa2f8..15c06407370 100644
--- a/drivers/ieee802154/Kconfig
+++ b/drivers/ieee802154/Kconfig
@@ -19,4 +19,12 @@ config IEEE802154_FAKEHARD
 
           This driver can also be built as a module. To do so say M here.
 	  The module will be called 'fakehard'.
+config IEEE802154_FAKELB
+	depends on IEEE802154_DRIVERS && MAC802154
+	tristate "IEEE 802.15.4 loopback driver"
+	---help---
+	  Say Y here to enable the fake driver that can emulate a net
+	  of several interconnected radio devices.
 
+	  This driver can also be built as a module. To do so say M here.
+	  The module will be called 'fakelb'.
diff --git a/drivers/ieee802154/Makefile b/drivers/ieee802154/Makefile
index 800a3894af0..ea784ea6f0f 100644
--- a/drivers/ieee802154/Makefile
+++ b/drivers/ieee802154/Makefile
@@ -1 +1,2 @@
 obj-$(CONFIG_IEEE802154_FAKEHARD) += fakehard.o
+obj-$(CONFIG_IEEE802154_FAKELB) += fakelb.o
diff --git a/drivers/ieee802154/fakelb.c b/drivers/ieee802154/fakelb.c
new file mode 100644
index 00000000000..e7456fcd091
--- /dev/null
+++ b/drivers/ieee802154/fakelb.c
@@ -0,0 +1,294 @@
+/*
+ * Loopback IEEE 802.15.4 interface
+ *
+ * Copyright 2007-2012 Siemens AG
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2
+ * as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with this program; if not, write to the Free Software Foundation, Inc.,
+ * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
+ *
+ * Written by:
+ * Sergey Lapin <slapin@ossfans.org>
+ * Dmitry Eremin-Solenikov <dbaryshkov@gmail.com>
+ * Alexander Smirnov <alex.bluesman.smirnov@gmail.com>
+ */
+
+#include <linux/module.h>
+#include <linux/timer.h>
+#include <linux/platform_device.h>
+#include <linux/netdevice.h>
+#include <linux/spinlock.h>
+#include <net/mac802154.h>
+#include <net/wpan-phy.h>
+
+static int numlbs = 1;
+
+struct fakelb_dev_priv {
+	struct ieee802154_dev *dev;
+
+	struct list_head list;
+	struct fakelb_priv *fake;
+
+	spinlock_t lock;
+	bool working;
+};
+
+struct fakelb_priv {
+	struct list_head list;
+	rwlock_t lock;
+};
+
+static int
+fakelb_hw_ed(struct ieee802154_dev *dev, u8 *level)
+{
+	might_sleep();
+	BUG_ON(!level);
+	*level = 0xbe;
+
+	return 0;
+}
+
+static int
+fakelb_hw_channel(struct ieee802154_dev *dev, int page, int channel)
+{
+	pr_debug("set channel to %d\n", channel);
+
+	might_sleep();
+	dev->phy->current_page = page;
+	dev->phy->current_channel = channel;
+
+	return 0;
+}
+
+static void
+fakelb_hw_deliver(struct fakelb_dev_priv *priv, struct sk_buff *skb)
+{
+	struct sk_buff *newskb;
+
+	spin_lock(&priv->lock);
+	if (priv->working) {
+		newskb = pskb_copy(skb, GFP_ATOMIC);
+		ieee802154_rx_irqsafe(priv->dev, newskb, 0xcc);
+	}
+	spin_unlock(&priv->lock);
+}
+
+static int
+fakelb_hw_xmit(struct ieee802154_dev *dev, struct sk_buff *skb)
+{
+	struct fakelb_dev_priv *priv = dev->priv;
+	struct fakelb_priv *fake = priv->fake;
+
+	might_sleep();
+
+	read_lock_bh(&fake->lock);
+	if (priv->list.next == priv->list.prev) {
+		/* we are the only one device */
+		fakelb_hw_deliver(priv, skb);
+	} else {
+		struct fakelb_dev_priv *dp;
+		list_for_each_entry(dp, &priv->fake->list, list) {
+			if (dp != priv &&
+			    (dp->dev->phy->current_channel ==
+			     priv->dev->phy->current_channel))
+				fakelb_hw_deliver(dp, skb);
+		}
+	}
+	read_unlock_bh(&fake->lock);
+
+	return 0;
+}
+
+static int
+fakelb_hw_start(struct ieee802154_dev *dev) {
+	struct fakelb_dev_priv *priv = dev->priv;
+	int ret = 0;
+
+	spin_lock(&priv->lock);
+	if (priv->working)
+		ret = -EBUSY;
+	else
+		priv->working = 1;
+	spin_unlock(&priv->lock);
+
+	return ret;
+}
+
+static void
+fakelb_hw_stop(struct ieee802154_dev *dev) {
+	struct fakelb_dev_priv *priv = dev->priv;
+
+	spin_lock(&priv->lock);
+	priv->working = 0;
+	spin_unlock(&priv->lock);
+}
+
+static struct ieee802154_ops fakelb_ops = {
+	.owner = THIS_MODULE,
+	.xmit = fakelb_hw_xmit,
+	.ed = fakelb_hw_ed,
+	.set_channel = fakelb_hw_channel,
+	.start = fakelb_hw_start,
+	.stop = fakelb_hw_stop,
+};
+
+/* Number of dummy devices to be set up by this module. */
+module_param(numlbs, int, 0);
+MODULE_PARM_DESC(numlbs, " number of pseudo devices");
+
+static int fakelb_add_one(struct device *dev, struct fakelb_priv *fake)
+{
+	struct fakelb_dev_priv *priv;
+	int err;
+	struct ieee802154_dev *ieee;
+
+	ieee = ieee802154_alloc_device(sizeof(*priv), &fakelb_ops);
+	if (!ieee)
+		return -ENOMEM;
+
+	priv = ieee->priv;
+	priv->dev = ieee;
+
+	/* 868 MHz BPSK	802.15.4-2003 */
+	ieee->phy->channels_supported[0] |= 1;
+	/* 915 MHz BPSK	802.15.4-2003 */
+	ieee->phy->channels_supported[0] |= 0x7fe;
+	/* 2.4 GHz O-QPSK 802.15.4-2003 */
+	ieee->phy->channels_supported[0] |= 0x7FFF800;
+	/* 868 MHz ASK 802.15.4-2006 */
+	ieee->phy->channels_supported[1] |= 1;
+	/* 915 MHz ASK 802.15.4-2006 */
+	ieee->phy->channels_supported[1] |= 0x7fe;
+	/* 868 MHz O-QPSK 802.15.4-2006 */
+	ieee->phy->channels_supported[2] |= 1;
+	/* 915 MHz O-QPSK 802.15.4-2006 */
+	ieee->phy->channels_supported[2] |= 0x7fe;
+	/* 2.4 GHz CSS 802.15.4a-2007 */
+	ieee->phy->channels_supported[3] |= 0x3fff;
+	/* UWB Sub-gigahertz 802.15.4a-2007 */
+	ieee->phy->channels_supported[4] |= 1;
+	/* UWB Low band 802.15.4a-2007 */
+	ieee->phy->channels_supported[4] |= 0x1e;
+	/* UWB High band 802.15.4a-2007 */
+	ieee->phy->channels_supported[4] |= 0xffe0;
+	/* 750 MHz O-QPSK 802.15.4c-2009 */
+	ieee->phy->channels_supported[5] |= 0xf;
+	/* 750 MHz MPSK 802.15.4c-2009 */
+	ieee->phy->channels_supported[5] |= 0xf0;
+	/* 950 MHz BPSK 802.15.4d-2009 */
+	ieee->phy->channels_supported[6] |= 0x3ff;
+	/* 950 MHz GFSK 802.15.4d-2009 */
+	ieee->phy->channels_supported[6] |= 0x3ffc00;
+
+	INIT_LIST_HEAD(&priv->list);
+	priv->fake = fake;
+
+	spin_lock_init(&priv->lock);
+
+	ieee->parent = dev;
+
+	err = ieee802154_register_device(ieee);
+	if (err)
+		goto err_reg;
+
+	write_lock_bh(&fake->lock);
+	list_add_tail(&priv->list, &fake->list);
+	write_unlock_bh(&fake->lock);
+
+	return 0;
+
+err_reg:
+	ieee802154_free_device(priv->dev);
+	return err;
+}
+
+static void fakelb_del(struct fakelb_dev_priv *priv)
+{
+	write_lock_bh(&priv->fake->lock);
+	list_del(&priv->list);
+	write_unlock_bh(&priv->fake->lock);
+
+	ieee802154_unregister_device(priv->dev);
+	ieee802154_free_device(priv->dev);
+}
+
+static int __devinit fakelb_probe(struct platform_device *pdev)
+{
+	struct fakelb_priv *priv;
+	struct fakelb_dev_priv *dp;
+	int err = -ENOMEM;
+	int i;
+
+	priv = kzalloc(sizeof(struct fakelb_priv), GFP_KERNEL);
+	if (!priv)
+		goto err_alloc;
+
+	INIT_LIST_HEAD(&priv->list);
+	rwlock_init(&priv->lock);
+
+	for (i = 0; i < numlbs; i++) {
+		err = fakelb_add_one(&pdev->dev, priv);
+		if (err < 0)
+			goto err_slave;
+	}
+
+	platform_set_drvdata(pdev, priv);
+	dev_info(&pdev->dev, "added ieee802154 hardware\n");
+	return 0;
+
+err_slave:
+	list_for_each_entry(dp, &priv->list, list)
+		fakelb_del(dp);
+	kfree(priv);
+err_alloc:
+	return err;
+}
+
+static int __devexit fakelb_remove(struct platform_device *pdev)
+{
+	struct fakelb_priv *priv = platform_get_drvdata(pdev);
+	struct fakelb_dev_priv *dp, *temp;
+
+	list_for_each_entry_safe(dp, temp, &priv->list, list)
+		fakelb_del(dp);
+	kfree(priv);
+
+	return 0;
+}
+
+static struct platform_device *ieee802154fake_dev;
+
+static struct platform_driver ieee802154fake_driver = {
+	.probe = fakelb_probe,
+	.remove = __devexit_p(fakelb_remove),
+	.driver = {
+			.name = "ieee802154fakelb",
+			.owner = THIS_MODULE,
+	},
+};
+
+static __init int fakelb_init_module(void)
+{
+	ieee802154fake_dev = platform_device_register_simple(
+			     "ieee802154fakelb", -1, NULL, 0);
+	return platform_driver_register(&ieee802154fake_driver);
+}
+
+static __exit void fake_remove_module(void)
+{
+	platform_driver_unregister(&ieee802154fake_driver);
+	platform_device_unregister(ieee802154fake_dev);
+}
+
+module_init(fakelb_init_module);
+module_exit(fake_remove_module);
+MODULE_LICENSE("GPL");
diff --git a/drivers/iio/Kconfig b/drivers/iio/Kconfig
new file mode 100644
index 00000000000..56eecefcec7
--- /dev/null
+++ b/drivers/iio/Kconfig
@@ -0,0 +1,54 @@
+#
+# Industrial I/O subsytem configuration
+#
+
+menuconfig IIO
+	tristate "Industrial I/O support"
+	depends on GENERIC_HARDIRQS
+	help
+	  The industrial I/O subsystem provides a unified framework for
+	  drivers for many different types of embedded sensors using a
+	  number of different physical interfaces (i2c, spi, etc). See
+	  Documentation/iio for more information.
+
+if IIO
+
+config IIO_BUFFER
+	bool "Enable buffer support within IIO"
+	help
+	  Provide core support for various buffer based data
+	  acquisition methods.
+
+if IIO_BUFFER
+
+config IIO_KFIFO_BUF
+	select IIO_TRIGGER
+	tristate "Industrial I/O buffering based on kfifo"
+	help
+	  A simple fifo based on kfifo.  Use this if you want a fifo
+	  rather than a ring buffer. Note that this currently provides
+	  no buffer events so it is up to userspace to work out how
+	  often to read from the buffer.
+
+endif # IIO_BUFFER
+
+config IIO_TRIGGER
+	boolean "Enable triggered sampling support"
+	help
+	  Provides IIO core support for triggers.  Currently these
+	  are used to initialize capture of samples to push into
+	  ring buffers.  The triggers are effectively a 'capture
+	  data now' interrupt.
+
+config IIO_CONSUMERS_PER_TRIGGER
+       int "Maximum number of consumers per trigger"
+       depends on IIO_TRIGGER
+       default "2"
+       help
+	This value controls the maximum number of consumers that a
+	given trigger may handle. Default is 2.
+
+source "drivers/iio/adc/Kconfig"
+source "drivers/iio/amplifiers/Kconfig"
+
+endif # IIO
diff --git a/drivers/iio/Makefile b/drivers/iio/Makefile
new file mode 100644
index 00000000000..e425afd1480
--- /dev/null
+++ b/drivers/iio/Makefile
@@ -0,0 +1,13 @@
+#
+# Makefile for the industrial I/O core.
+#
+
+obj-$(CONFIG_IIO) += industrialio.o
+industrialio-y := industrialio-core.o industrialio-event.o inkern.o
+industrialio-$(CONFIG_IIO_BUFFER) += industrialio-buffer.o
+industrialio-$(CONFIG_IIO_TRIGGER) += industrialio-trigger.o
+
+obj-$(CONFIG_IIO_KFIFO_BUF) += kfifo_buf.o
+
+obj-y += adc/
+obj-y += amplifiers/
diff --git a/drivers/iio/adc/Kconfig b/drivers/iio/adc/Kconfig
new file mode 100644
index 00000000000..9a0df8123cc
--- /dev/null
+++ b/drivers/iio/adc/Kconfig
@@ -0,0 +1,16 @@
+#
+# ADC drivers
+#
+menu "Analog to digital converters"
+
+config AT91_ADC
+	tristate "Atmel AT91 ADC"
+	depends on ARCH_AT91
+	select IIO_BUFFER
+	select IIO_KFIFO_BUF
+	select IIO_TRIGGER
+	select SYSFS
+	help
+	  Say yes here to build support for Atmel AT91 ADC.
+
+endmenu
diff --git a/drivers/iio/adc/Makefile b/drivers/iio/adc/Makefile
new file mode 100644
index 00000000000..175c8d41ea9
--- /dev/null
+++ b/drivers/iio/adc/Makefile
@@ -0,0 +1,5 @@
+#
+# Makefile for IIO ADC drivers
+#
+
+obj-$(CONFIG_AT91_ADC) += at91_adc.o
diff --git a/drivers/iio/adc/at91_adc.c b/drivers/iio/adc/at91_adc.c
new file mode 100644
index 00000000000..f18a95d8025
--- /dev/null
+++ b/drivers/iio/adc/at91_adc.c
@@ -0,0 +1,802 @@
+/*
+ * Driver for the ADC present in the Atmel AT91 evaluation boards.
+ *
+ * Copyright 2011 Free Electrons
+ *
+ * Licensed under the GPLv2 or later.
+ */
+
+#include <linux/bitmap.h>
+#include <linux/bitops.h>
+#include <linux/clk.h>
+#include <linux/err.h>
+#include <linux/io.h>
+#include <linux/interrupt.h>
+#include <linux/jiffies.h>
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/of.h>
+#include <linux/of_device.h>
+#include <linux/platform_device.h>
+#include <linux/sched.h>
+#include <linux/slab.h>
+#include <linux/wait.h>
+
+#include <linux/platform_data/at91_adc.h>
+
+#include <linux/iio/iio.h>
+#include <linux/iio/buffer.h>
+#include <linux/iio/kfifo_buf.h>
+#include <linux/iio/trigger.h>
+#include <linux/iio/trigger_consumer.h>
+
+#include <mach/at91_adc.h>
+
+#define AT91_ADC_CHAN(st, ch) \
+	(st->registers->channel_base + (ch * 4))
+#define at91_adc_readl(st, reg) \
+	(readl_relaxed(st->reg_base + reg))
+#define at91_adc_writel(st, reg, val) \
+	(writel_relaxed(val, st->reg_base + reg))
+
+struct at91_adc_state {
+	struct clk		*adc_clk;
+	u16			*buffer;
+	unsigned long		channels_mask;
+	struct clk		*clk;
+	bool			done;
+	int			irq;
+	bool			irq_enabled;
+	u16			last_value;
+	struct mutex		lock;
+	u8			num_channels;
+	void __iomem		*reg_base;
+	struct at91_adc_reg_desc *registers;
+	u8			startup_time;
+	struct iio_trigger	**trig;
+	struct at91_adc_trigger	*trigger_list;
+	u32			trigger_number;
+	bool			use_external;
+	u32			vref_mv;
+	wait_queue_head_t	wq_data_avail;
+};
+
+static irqreturn_t at91_adc_trigger_handler(int irq, void *p)
+{
+	struct iio_poll_func *pf = p;
+	struct iio_dev *idev = pf->indio_dev;
+	struct at91_adc_state *st = iio_priv(idev);
+	struct iio_buffer *buffer = idev->buffer;
+	int i, j = 0;
+
+	for (i = 0; i < idev->masklength; i++) {
+		if (!test_bit(i, idev->active_scan_mask))
+			continue;
+		st->buffer[j] = at91_adc_readl(st, AT91_ADC_CHAN(st, i));
+		j++;
+	}
+
+	if (idev->scan_timestamp) {
+		s64 *timestamp = (s64 *)((u8 *)st->buffer +
+					ALIGN(j, sizeof(s64)));
+		*timestamp = pf->timestamp;
+	}
+
+	buffer->access->store_to(buffer, (u8 *)st->buffer, pf->timestamp);
+
+	iio_trigger_notify_done(idev->trig);
+	st->irq_enabled = true;
+
+	/* Needed to ACK the DRDY interruption */
+	at91_adc_readl(st, AT91_ADC_LCDR);
+
+	enable_irq(st->irq);
+
+	return IRQ_HANDLED;
+}
+
+static irqreturn_t at91_adc_eoc_trigger(int irq, void *private)
+{
+	struct iio_dev *idev = private;
+	struct at91_adc_state *st = iio_priv(idev);
+	u32 status = at91_adc_readl(st, st->registers->status_register);
+
+	if (!(status & st->registers->drdy_mask))
+		return IRQ_HANDLED;
+
+	if (iio_buffer_enabled(idev)) {
+		disable_irq_nosync(irq);
+		st->irq_enabled = false;
+		iio_trigger_poll(idev->trig, iio_get_time_ns());
+	} else {
+		st->last_value = at91_adc_readl(st, AT91_ADC_LCDR);
+		st->done = true;
+		wake_up_interruptible(&st->wq_data_avail);
+	}
+
+	return IRQ_HANDLED;
+}
+
+static int at91_adc_channel_init(struct iio_dev *idev)
+{
+	struct at91_adc_state *st = iio_priv(idev);
+	struct iio_chan_spec *chan_array, *timestamp;
+	int bit, idx = 0;
+
+	idev->num_channels = bitmap_weight(&st->channels_mask,
+					   st->num_channels) + 1;
+
+	chan_array = devm_kzalloc(&idev->dev,
+				  ((idev->num_channels + 1) *
+					sizeof(struct iio_chan_spec)),
+				  GFP_KERNEL);
+
+	if (!chan_array)
+		return -ENOMEM;
+
+	for_each_set_bit(bit, &st->channels_mask, st->num_channels) {
+		struct iio_chan_spec *chan = chan_array + idx;
+
+		chan->type = IIO_VOLTAGE;
+		chan->indexed = 1;
+		chan->channel = bit;
+		chan->scan_index = idx;
+		chan->scan_type.sign = 'u';
+		chan->scan_type.realbits = 10;
+		chan->scan_type.storagebits = 16;
+		chan->info_mask = IIO_CHAN_INFO_SCALE_SHARED_BIT |
+			IIO_CHAN_INFO_RAW_SEPARATE_BIT;
+		idx++;
+	}
+	timestamp = chan_array + idx;
+
+	timestamp->type = IIO_TIMESTAMP;
+	timestamp->channel = -1;
+	timestamp->scan_index = idx;
+	timestamp->scan_type.sign = 's';
+	timestamp->scan_type.realbits = 64;
+	timestamp->scan_type.storagebits = 64;
+
+	idev->channels = chan_array;
+	return idev->num_channels;
+}
+
+static u8 at91_adc_get_trigger_value_by_name(struct iio_dev *idev,
+					     struct at91_adc_trigger *triggers,
+					     const char *trigger_name)
+{
+	struct at91_adc_state *st = iio_priv(idev);
+	u8 value = 0;
+	int i;
+
+	for (i = 0; i < st->trigger_number; i++) {
+		char *name = kasprintf(GFP_KERNEL,
+				"%s-dev%d-%s",
+				idev->name,
+				idev->id,
+				triggers[i].name);
+		if (!name)
+			return -ENOMEM;
+
+		if (strcmp(trigger_name, name) == 0) {
+			value = triggers[i].value;
+			kfree(name);
+			break;
+		}
+
+		kfree(name);
+	}
+
+	return value;
+}
+
+static int at91_adc_configure_trigger(struct iio_trigger *trig, bool state)
+{
+	struct iio_dev *idev = trig->private_data;
+	struct at91_adc_state *st = iio_priv(idev);
+	struct iio_buffer *buffer = idev->buffer;
+	struct at91_adc_reg_desc *reg = st->registers;
+	u32 status = at91_adc_readl(st, reg->trigger_register);
+	u8 value;
+	u8 bit;
+
+	value = at91_adc_get_trigger_value_by_name(idev,
+						   st->trigger_list,
+						   idev->trig->name);
+	if (value == 0)
+		return -EINVAL;
+
+	if (state) {
+		st->buffer = kmalloc(idev->scan_bytes, GFP_KERNEL);
+		if (st->buffer == NULL)
+			return -ENOMEM;
+
+		at91_adc_writel(st, reg->trigger_register,
+				status | value);
+
+		for_each_set_bit(bit, buffer->scan_mask,
+				 st->num_channels) {
+			struct iio_chan_spec const *chan = idev->channels + bit;
+			at91_adc_writel(st, AT91_ADC_CHER,
+					AT91_ADC_CH(chan->channel));
+		}
+
+		at91_adc_writel(st, AT91_ADC_IER, reg->drdy_mask);
+
+	} else {
+		at91_adc_writel(st, AT91_ADC_IDR, reg->drdy_mask);
+
+		at91_adc_writel(st, reg->trigger_register,
+				status & ~value);
+
+		for_each_set_bit(bit, buffer->scan_mask,
+				 st->num_channels) {
+			struct iio_chan_spec const *chan = idev->channels + bit;
+			at91_adc_writel(st, AT91_ADC_CHDR,
+					AT91_ADC_CH(chan->channel));
+		}
+		kfree(st->buffer);
+	}
+
+	return 0;
+}
+
+static const struct iio_trigger_ops at91_adc_trigger_ops = {
+	.owner = THIS_MODULE,
+	.set_trigger_state = &at91_adc_configure_trigger,
+};
+
+static struct iio_trigger *at91_adc_allocate_trigger(struct iio_dev *idev,
+						     struct at91_adc_trigger *trigger)
+{
+	struct iio_trigger *trig;
+	int ret;
+
+	trig = iio_trigger_alloc("%s-dev%d-%s", idev->name,
+				 idev->id, trigger->name);
+	if (trig == NULL)
+		return NULL;
+
+	trig->dev.parent = idev->dev.parent;
+	trig->private_data = idev;
+	trig->ops = &at91_adc_trigger_ops;
+
+	ret = iio_trigger_register(trig);
+	if (ret)
+		return NULL;
+
+	return trig;
+}
+
+static int at91_adc_trigger_init(struct iio_dev *idev)
+{
+	struct at91_adc_state *st = iio_priv(idev);
+	int i, ret;
+
+	st->trig = devm_kzalloc(&idev->dev,
+				st->trigger_number * sizeof(st->trig),
+				GFP_KERNEL);
+
+	if (st->trig == NULL) {
+		ret = -ENOMEM;
+		goto error_ret;
+	}
+
+	for (i = 0; i < st->trigger_number; i++) {
+		if (st->trigger_list[i].is_external && !(st->use_external))
+			continue;
+
+		st->trig[i] = at91_adc_allocate_trigger(idev,
+							st->trigger_list + i);
+		if (st->trig[i] == NULL) {
+			dev_err(&idev->dev,
+				"Could not allocate trigger %d\n", i);
+			ret = -ENOMEM;
+			goto error_trigger;
+		}
+	}
+
+	return 0;
+
+error_trigger:
+	for (i--; i >= 0; i--) {
+		iio_trigger_unregister(st->trig[i]);
+		iio_trigger_free(st->trig[i]);
+	}
+error_ret:
+	return ret;
+}
+
+static void at91_adc_trigger_remove(struct iio_dev *idev)
+{
+	struct at91_adc_state *st = iio_priv(idev);
+	int i;
+
+	for (i = 0; i < st->trigger_number; i++) {
+		iio_trigger_unregister(st->trig[i]);
+		iio_trigger_free(st->trig[i]);
+	}
+}
+
+static const struct iio_buffer_setup_ops at91_adc_buffer_ops = {
+	.preenable = &iio_sw_buffer_preenable,
+	.postenable = &iio_triggered_buffer_postenable,
+	.predisable = &iio_triggered_buffer_predisable,
+};
+
+static int at91_adc_buffer_init(struct iio_dev *idev)
+{
+	int ret;
+
+	idev->buffer = iio_kfifo_allocate(idev);
+	if (!idev->buffer) {
+		ret = -ENOMEM;
+		goto error_ret;
+	}
+
+	idev->pollfunc = iio_alloc_pollfunc(&iio_pollfunc_store_time,
+					    &at91_adc_trigger_handler,
+					    IRQF_ONESHOT,
+					    idev,
+					    "%s-consumer%d",
+					    idev->name,
+					    idev->id);
+	if (idev->pollfunc == NULL) {
+		ret = -ENOMEM;
+		goto error_pollfunc;
+	}
+
+	idev->setup_ops = &at91_adc_buffer_ops;
+	idev->modes |= INDIO_BUFFER_TRIGGERED;
+
+	ret = iio_buffer_register(idev,
+				  idev->channels,
+				  idev->num_channels);
+	if (ret)
+		goto error_register;
+
+	return 0;
+
+error_register:
+	iio_dealloc_pollfunc(idev->pollfunc);
+error_pollfunc:
+	iio_kfifo_free(idev->buffer);
+error_ret:
+	return ret;
+}
+
+static void at91_adc_buffer_remove(struct iio_dev *idev)
+{
+	iio_buffer_unregister(idev);
+	iio_dealloc_pollfunc(idev->pollfunc);
+	iio_kfifo_free(idev->buffer);
+}
+
+static int at91_adc_read_raw(struct iio_dev *idev,
+			     struct iio_chan_spec const *chan,
+			     int *val, int *val2, long mask)
+{
+	struct at91_adc_state *st = iio_priv(idev);
+	int ret;
+
+	switch (mask) {
+	case IIO_CHAN_INFO_RAW:
+		mutex_lock(&st->lock);
+
+		at91_adc_writel(st, AT91_ADC_CHER,
+				AT91_ADC_CH(chan->channel));
+		at91_adc_writel(st, AT91_ADC_IER, st->registers->drdy_mask);
+		at91_adc_writel(st, AT91_ADC_CR, AT91_ADC_START);
+
+		ret = wait_event_interruptible_timeout(st->wq_data_avail,
+						       st->done,
+						       msecs_to_jiffies(1000));
+		if (ret == 0)
+			return -ETIMEDOUT;
+		else if (ret < 0)
+			return ret;
+
+		*val = st->last_value;
+
+		at91_adc_writel(st, AT91_ADC_CHDR,
+				AT91_ADC_CH(chan->channel));
+		at91_adc_writel(st, AT91_ADC_IDR, st->registers->drdy_mask);
+
+		st->last_value = 0;
+		st->done = false;
+		mutex_unlock(&st->lock);
+		return IIO_VAL_INT;
+
+	case IIO_CHAN_INFO_SCALE:
+		*val = (st->vref_mv * 1000) >> chan->scan_type.realbits;
+		*val2 = 0;
+		return IIO_VAL_INT_PLUS_MICRO;
+	default:
+		break;
+	}
+	return -EINVAL;
+}
+
+static int at91_adc_probe_dt(struct at91_adc_state *st,
+			     struct platform_device *pdev)
+{
+	struct iio_dev *idev = iio_priv_to_dev(st);
+	struct device_node *node = pdev->dev.of_node;
+	struct device_node *trig_node;
+	int i = 0, ret;
+	u32 prop;
+
+	if (!node)
+		return -EINVAL;
+
+	st->use_external = of_property_read_bool(node, "atmel,adc-use-external-triggers");
+
+	if (of_property_read_u32(node, "atmel,adc-channels-used", &prop)) {
+		dev_err(&idev->dev, "Missing adc-channels-used property in the DT.\n");
+		ret = -EINVAL;
+		goto error_ret;
+	}
+	st->channels_mask = prop;
+
+	if (of_property_read_u32(node, "atmel,adc-num-channels", &prop)) {
+		dev_err(&idev->dev, "Missing adc-num-channels property in the DT.\n");
+		ret = -EINVAL;
+		goto error_ret;
+	}
+	st->num_channels = prop;
+
+	if (of_property_read_u32(node, "atmel,adc-startup-time", &prop)) {
+		dev_err(&idev->dev, "Missing adc-startup-time property in the DT.\n");
+		ret = -EINVAL;
+		goto error_ret;
+	}
+	st->startup_time = prop;
+
+
+	if (of_property_read_u32(node, "atmel,adc-vref", &prop)) {
+		dev_err(&idev->dev, "Missing adc-vref property in the DT.\n");
+		ret = -EINVAL;
+		goto error_ret;
+	}
+	st->vref_mv = prop;
+
+	st->registers = devm_kzalloc(&idev->dev,
+				     sizeof(struct at91_adc_reg_desc),
+				     GFP_KERNEL);
+	if (!st->registers) {
+		dev_err(&idev->dev, "Could not allocate register memory.\n");
+		ret = -ENOMEM;
+		goto error_ret;
+	}
+
+	if (of_property_read_u32(node, "atmel,adc-channel-base", &prop)) {
+		dev_err(&idev->dev, "Missing adc-channel-base property in the DT.\n");
+		ret = -EINVAL;
+		goto error_ret;
+	}
+	st->registers->channel_base = prop;
+
+	if (of_property_read_u32(node, "atmel,adc-drdy-mask", &prop)) {
+		dev_err(&idev->dev, "Missing adc-drdy-mask property in the DT.\n");
+		ret = -EINVAL;
+		goto error_ret;
+	}
+	st->registers->drdy_mask = prop;
+
+	if (of_property_read_u32(node, "atmel,adc-status-register", &prop)) {
+		dev_err(&idev->dev, "Missing adc-status-register property in the DT.\n");
+		ret = -EINVAL;
+		goto error_ret;
+	}
+	st->registers->status_register = prop;
+
+	if (of_property_read_u32(node, "atmel,adc-trigger-register", &prop)) {
+		dev_err(&idev->dev, "Missing adc-trigger-register property in the DT.\n");
+		ret = -EINVAL;
+		goto error_ret;
+	}
+	st->registers->trigger_register = prop;
+
+	st->trigger_number = of_get_child_count(node);
+	st->trigger_list = devm_kzalloc(&idev->dev, st->trigger_number *
+					sizeof(struct at91_adc_trigger),
+					GFP_KERNEL);
+	if (!st->trigger_list) {
+		dev_err(&idev->dev, "Could not allocate trigger list memory.\n");
+		ret = -ENOMEM;
+		goto error_ret;
+	}
+
+	for_each_child_of_node(node, trig_node) {
+		struct at91_adc_trigger *trig = st->trigger_list + i;
+		const char *name;
+
+		if (of_property_read_string(trig_node, "trigger-name", &name)) {
+			dev_err(&idev->dev, "Missing trigger-name property in the DT.\n");
+			ret = -EINVAL;
+			goto error_ret;
+		}
+	        trig->name = name;
+
+		if (of_property_read_u32(trig_node, "trigger-value", &prop)) {
+			dev_err(&idev->dev, "Missing trigger-value property in the DT.\n");
+			ret = -EINVAL;
+			goto error_ret;
+		}
+	        trig->value = prop;
+		trig->is_external = of_property_read_bool(trig_node, "trigger-external");
+		i++;
+	}
+
+	return 0;
+
+error_ret:
+	return ret;
+}
+
+static int at91_adc_probe_pdata(struct at91_adc_state *st,
+				struct platform_device *pdev)
+{
+	struct at91_adc_data *pdata = pdev->dev.platform_data;
+
+	if (!pdata)
+		return -EINVAL;
+
+	st->use_external = pdata->use_external_triggers;
+	st->vref_mv = pdata->vref;
+	st->channels_mask = pdata->channels_used;
+	st->num_channels = pdata->num_channels;
+	st->startup_time = pdata->startup_time;
+	st->trigger_number = pdata->trigger_number;
+	st->trigger_list = pdata->trigger_list;
+	st->registers = pdata->registers;
+
+	return 0;
+}
+
+static const struct iio_info at91_adc_info = {
+	.driver_module = THIS_MODULE,
+	.read_raw = &at91_adc_read_raw,
+};
+
+static int __devinit at91_adc_probe(struct platform_device *pdev)
+{
+	unsigned int prsc, mstrclk, ticks, adc_clk;
+	int ret;
+	struct iio_dev *idev;
+	struct at91_adc_state *st;
+	struct resource *res;
+
+	idev = iio_device_alloc(sizeof(struct at91_adc_state));
+	if (idev == NULL) {
+		ret = -ENOMEM;
+		goto error_ret;
+	}
+
+	st = iio_priv(idev);
+
+	if (pdev->dev.of_node)
+		ret = at91_adc_probe_dt(st, pdev);
+	else
+		ret = at91_adc_probe_pdata(st, pdev);
+
+	if (ret) {
+		dev_err(&pdev->dev, "No platform data available.\n");
+		ret = -EINVAL;
+		goto error_free_device;
+	}
+
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	if (!res) {
+		dev_err(&pdev->dev, "No resource defined\n");
+		ret = -ENXIO;
+		goto error_ret;
+	}
+
+	platform_set_drvdata(pdev, idev);
+
+	idev->dev.parent = &pdev->dev;
+	idev->name = dev_name(&pdev->dev);
+	idev->modes = INDIO_DIRECT_MODE;
+	idev->info = &at91_adc_info;
+
+	st->irq = platform_get_irq(pdev, 0);
+	if (st->irq < 0) {
+		dev_err(&pdev->dev, "No IRQ ID is designated\n");
+		ret = -ENODEV;
+		goto error_free_device;
+	}
+
+	if (!request_mem_region(res->start, resource_size(res),
+				"AT91 adc registers")) {
+		dev_err(&pdev->dev, "Resources are unavailable.\n");
+		ret = -EBUSY;
+		goto error_free_device;
+	}
+
+	st->reg_base = ioremap(res->start, resource_size(res));
+	if (!st->reg_base) {
+		dev_err(&pdev->dev, "Failed to map registers.\n");
+		ret = -ENOMEM;
+		goto error_release_mem;
+	}
+
+	/*
+	 * Disable all IRQs before setting up the handler
+	 */
+	at91_adc_writel(st, AT91_ADC_CR, AT91_ADC_SWRST);
+	at91_adc_writel(st, AT91_ADC_IDR, 0xFFFFFFFF);
+	ret = request_irq(st->irq,
+			  at91_adc_eoc_trigger,
+			  0,
+			  pdev->dev.driver->name,
+			  idev);
+	if (ret) {
+		dev_err(&pdev->dev, "Failed to allocate IRQ.\n");
+		goto error_unmap_reg;
+	}
+
+	st->clk = clk_get(&pdev->dev, "adc_clk");
+	if (IS_ERR(st->clk)) {
+		dev_err(&pdev->dev, "Failed to get the clock.\n");
+		ret = PTR_ERR(st->clk);
+		goto error_free_irq;
+	}
+
+	ret = clk_prepare(st->clk);
+	if (ret) {
+		dev_err(&pdev->dev, "Could not prepare the clock.\n");
+		goto error_free_clk;
+	}
+
+	ret = clk_enable(st->clk);
+	if (ret) {
+		dev_err(&pdev->dev, "Could not enable the clock.\n");
+		goto error_unprepare_clk;
+	}
+
+	st->adc_clk = clk_get(&pdev->dev, "adc_op_clk");
+	if (IS_ERR(st->adc_clk)) {
+		dev_err(&pdev->dev, "Failed to get the ADC clock.\n");
+		ret = PTR_ERR(st->clk);
+		goto error_disable_clk;
+	}
+
+	ret = clk_prepare(st->adc_clk);
+	if (ret) {
+		dev_err(&pdev->dev, "Could not prepare the ADC clock.\n");
+		goto error_free_adc_clk;
+	}
+
+	ret = clk_enable(st->adc_clk);
+	if (ret) {
+		dev_err(&pdev->dev, "Could not enable the ADC clock.\n");
+		goto error_unprepare_adc_clk;
+	}
+
+	/*
+	 * Prescaler rate computation using the formula from the Atmel's
+	 * datasheet : ADC Clock = MCK / ((Prescaler + 1) * 2), ADC Clock being
+	 * specified by the electrical characteristics of the board.
+	 */
+	mstrclk = clk_get_rate(st->clk);
+	adc_clk = clk_get_rate(st->adc_clk);
+	prsc = (mstrclk / (2 * adc_clk)) - 1;
+
+	if (!st->startup_time) {
+		dev_err(&pdev->dev, "No startup time available.\n");
+		ret = -EINVAL;
+		goto error_disable_adc_clk;
+	}
+
+	/*
+	 * Number of ticks needed to cover the startup time of the ADC as
+	 * defined in the electrical characteristics of the board, divided by 8.
+	 * The formula thus is : Startup Time = (ticks + 1) * 8 / ADC Clock
+	 */
+	ticks = round_up((st->startup_time * adc_clk /
+			  1000000) - 1, 8) / 8;
+	at91_adc_writel(st, AT91_ADC_MR,
+			(AT91_ADC_PRESCAL_(prsc) & AT91_ADC_PRESCAL) |
+			(AT91_ADC_STARTUP_(ticks) & AT91_ADC_STARTUP));
+
+	/* Setup the ADC channels available on the board */
+	ret = at91_adc_channel_init(idev);
+	if (ret < 0) {
+		dev_err(&pdev->dev, "Couldn't initialize the channels.\n");
+		goto error_disable_adc_clk;
+	}
+
+	init_waitqueue_head(&st->wq_data_avail);
+	mutex_init(&st->lock);
+
+	ret = at91_adc_buffer_init(idev);
+	if (ret < 0) {
+		dev_err(&pdev->dev, "Couldn't initialize the buffer.\n");
+		goto error_disable_adc_clk;
+	}
+
+	ret = at91_adc_trigger_init(idev);
+	if (ret < 0) {
+		dev_err(&pdev->dev, "Couldn't setup the triggers.\n");
+		goto error_unregister_buffer;
+	}
+
+	ret = iio_device_register(idev);
+	if (ret < 0) {
+		dev_err(&pdev->dev, "Couldn't register the device.\n");
+		goto error_remove_triggers;
+	}
+
+	return 0;
+
+error_remove_triggers:
+	at91_adc_trigger_remove(idev);
+error_unregister_buffer:
+	at91_adc_buffer_remove(idev);
+error_disable_adc_clk:
+	clk_disable(st->adc_clk);
+error_unprepare_adc_clk:
+	clk_unprepare(st->adc_clk);
+error_free_adc_clk:
+	clk_put(st->adc_clk);
+error_disable_clk:
+	clk_disable(st->clk);
+error_unprepare_clk:
+	clk_unprepare(st->clk);
+error_free_clk:
+	clk_put(st->clk);
+error_free_irq:
+	free_irq(st->irq, idev);
+error_unmap_reg:
+	iounmap(st->reg_base);
+error_release_mem:
+	release_mem_region(res->start, resource_size(res));
+error_free_device:
+	iio_device_free(idev);
+error_ret:
+	return ret;
+}
+
+static int __devexit at91_adc_remove(struct platform_device *pdev)
+{
+	struct iio_dev *idev = platform_get_drvdata(pdev);
+	struct resource *res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	struct at91_adc_state *st = iio_priv(idev);
+
+	iio_device_unregister(idev);
+	at91_adc_trigger_remove(idev);
+	at91_adc_buffer_remove(idev);
+	clk_disable_unprepare(st->adc_clk);
+	clk_put(st->adc_clk);
+	clk_disable(st->clk);
+	clk_unprepare(st->clk);
+	clk_put(st->clk);
+	free_irq(st->irq, idev);
+	iounmap(st->reg_base);
+	release_mem_region(res->start, resource_size(res));
+	iio_device_free(idev);
+
+	return 0;
+}
+
+static const struct of_device_id at91_adc_dt_ids[] = {
+	{ .compatible = "atmel,at91sam9260-adc" },
+	{},
+};
+MODULE_DEVICE_TABLE(of, at91_adc_dt_ids);
+
+static struct platform_driver at91_adc_driver = {
+	.probe = at91_adc_probe,
+	.remove = __devexit_p(at91_adc_remove),
+	.driver = {
+		   .name = "at91_adc",
+		   .of_match_table = of_match_ptr(at91_adc_dt_ids),
+	},
+};
+
+module_platform_driver(at91_adc_driver);
+
+MODULE_LICENSE("GPL");
+MODULE_DESCRIPTION("Atmel AT91 ADC Driver");
+MODULE_AUTHOR("Maxime Ripard <maxime.ripard@free-electrons.com>");
diff --git a/drivers/iio/amplifiers/Kconfig b/drivers/iio/amplifiers/Kconfig
new file mode 100644
index 00000000000..05d707ed7d4
--- /dev/null
+++ b/drivers/iio/amplifiers/Kconfig
@@ -0,0 +1,17 @@
+#
+# Gain Amplifiers, etc.
+#
+menu "Amplifiers"
+
+config AD8366
+	tristate "Analog Devices AD8366 VGA"
+	depends on SPI
+	select BITREVERSE
+	help
+	  Say yes here to build support for Analog Devices AD8366
+	  SPI Dual-Digital Variable Gain Amplifier (VGA).
+
+	  To compile this driver as a module, choose M here: the
+	  module will be called ad8366.
+
+endmenu
diff --git a/drivers/iio/amplifiers/Makefile b/drivers/iio/amplifiers/Makefile
new file mode 100644
index 00000000000..a6ca366908e
--- /dev/null
+++ b/drivers/iio/amplifiers/Makefile
@@ -0,0 +1,5 @@
+#
+# Makefile iio/amplifiers
+#
+
+obj-$(CONFIG_AD8366) += ad8366.o
diff --git a/drivers/iio/amplifiers/ad8366.c b/drivers/iio/amplifiers/ad8366.c
new file mode 100644
index 00000000000..d8281cdbfc4
--- /dev/null
+++ b/drivers/iio/amplifiers/ad8366.c
@@ -0,0 +1,222 @@
+/*
+ * AD8366 SPI Dual-Digital Variable Gain Amplifier (VGA)
+ *
+ * Copyright 2012 Analog Devices Inc.
+ *
+ * Licensed under the GPL-2.
+ */
+
+#include <linux/device.h>
+#include <linux/kernel.h>
+#include <linux/slab.h>
+#include <linux/sysfs.h>
+#include <linux/spi/spi.h>
+#include <linux/regulator/consumer.h>
+#include <linux/err.h>
+#include <linux/module.h>
+#include <linux/bitrev.h>
+
+#include <linux/iio/iio.h>
+#include <linux/iio/sysfs.h>
+
+struct ad8366_state {
+	struct spi_device	*spi;
+	struct regulator	*reg;
+	unsigned char		ch[2];
+	/*
+	 * DMA (thus cache coherency maintenance) requires the
+	 * transfer buffers to live in their own cache lines.
+	 */
+	unsigned char		data[2] ____cacheline_aligned;
+};
+
+static int ad8366_write(struct iio_dev *indio_dev,
+			unsigned char ch_a, char unsigned ch_b)
+{
+	struct ad8366_state *st = iio_priv(indio_dev);
+	int ret;
+
+	ch_a = bitrev8(ch_a & 0x3F);
+	ch_b = bitrev8(ch_b & 0x3F);
+
+	st->data[0] = ch_b >> 4;
+	st->data[1] = (ch_b << 4) | (ch_a >> 2);
+
+	ret = spi_write(st->spi, st->data, ARRAY_SIZE(st->data));
+	if (ret < 0)
+		dev_err(&indio_dev->dev, "write failed (%d)", ret);
+
+	return ret;
+}
+
+static int ad8366_read_raw(struct iio_dev *indio_dev,
+			   struct iio_chan_spec const *chan,
+			   int *val,
+			   int *val2,
+			   long m)
+{
+	struct ad8366_state *st = iio_priv(indio_dev);
+	int ret;
+	unsigned code;
+
+	mutex_lock(&indio_dev->mlock);
+	switch (m) {
+	case IIO_CHAN_INFO_HARDWAREGAIN:
+		code = st->ch[chan->channel];
+
+		/* Values in dB */
+		code = code * 253 + 4500;
+		*val = code / 1000;
+		*val2 = (code % 1000) * 1000;
+
+		ret = IIO_VAL_INT_PLUS_MICRO_DB;
+		break;
+	default:
+		ret = -EINVAL;
+	}
+	mutex_unlock(&indio_dev->mlock);
+
+	return ret;
+};
+
+static int ad8366_write_raw(struct iio_dev *indio_dev,
+			    struct iio_chan_spec const *chan,
+			    int val,
+			    int val2,
+			    long mask)
+{
+	struct ad8366_state *st = iio_priv(indio_dev);
+	unsigned code;
+	int ret;
+
+	if (val < 0 || val2 < 0)
+		return -EINVAL;
+
+	/* Values in dB */
+	code = (((u8)val * 1000) + ((u32)val2 / 1000));
+
+	if (code > 20500 || code < 4500)
+		return -EINVAL;
+
+	code = (code - 4500) / 253;
+
+	mutex_lock(&indio_dev->mlock);
+	switch (mask) {
+	case IIO_CHAN_INFO_HARDWAREGAIN:
+		st->ch[chan->channel] = code;
+		ret = ad8366_write(indio_dev, st->ch[0], st->ch[1]);
+		break;
+	default:
+		ret = -EINVAL;
+	}
+	mutex_unlock(&indio_dev->mlock);
+
+	return ret;
+}
+
+static const struct iio_info ad8366_info = {
+	.read_raw = &ad8366_read_raw,
+	.write_raw = &ad8366_write_raw,
+	.driver_module = THIS_MODULE,
+};
+
+#define AD8366_CHAN(_channel) {				\
+	.type = IIO_VOLTAGE,				\
+	.output = 1,					\
+	.indexed = 1,					\
+	.channel = _channel,				\
+	.info_mask = IIO_CHAN_INFO_HARDWAREGAIN_SEPARATE_BIT,\
+}
+
+static const struct iio_chan_spec ad8366_channels[] = {
+	AD8366_CHAN(0),
+	AD8366_CHAN(1),
+};
+
+static int __devinit ad8366_probe(struct spi_device *spi)
+{
+	struct iio_dev *indio_dev;
+	struct ad8366_state *st;
+	int ret;
+
+	indio_dev = iio_device_alloc(sizeof(*st));
+	if (indio_dev == NULL)
+		return -ENOMEM;
+
+	st = iio_priv(indio_dev);
+
+	st->reg = regulator_get(&spi->dev, "vcc");
+	if (!IS_ERR(st->reg)) {
+		ret = regulator_enable(st->reg);
+		if (ret)
+			goto error_put_reg;
+	}
+
+	spi_set_drvdata(spi, indio_dev);
+	st->spi = spi;
+
+	indio_dev->dev.parent = &spi->dev;
+	indio_dev->name = spi_get_device_id(spi)->name;
+	indio_dev->info = &ad8366_info;
+	indio_dev->modes = INDIO_DIRECT_MODE;
+	indio_dev->channels = ad8366_channels;
+	indio_dev->num_channels = ARRAY_SIZE(ad8366_channels);
+
+	ret = iio_device_register(indio_dev);
+	if (ret)
+		goto error_disable_reg;
+
+	ad8366_write(indio_dev, 0 , 0);
+
+	return 0;
+
+error_disable_reg:
+	if (!IS_ERR(st->reg))
+		regulator_disable(st->reg);
+error_put_reg:
+	if (!IS_ERR(st->reg))
+		regulator_put(st->reg);
+
+	iio_device_free(indio_dev);
+
+	return ret;
+}
+
+static int __devexit ad8366_remove(struct spi_device *spi)
+{
+	struct iio_dev *indio_dev = spi_get_drvdata(spi);
+	struct ad8366_state *st = iio_priv(indio_dev);
+	struct regulator *reg = st->reg;
+
+	iio_device_unregister(indio_dev);
+
+	if (!IS_ERR(reg)) {
+		regulator_disable(reg);
+		regulator_put(reg);
+	}
+
+	iio_device_free(indio_dev);
+
+	return 0;
+}
+
+static const struct spi_device_id ad8366_id[] = {
+	{"ad8366", 0},
+	{}
+};
+
+static struct spi_driver ad8366_driver = {
+	.driver = {
+		.name	= KBUILD_MODNAME,
+		.owner	= THIS_MODULE,
+	},
+	.probe		= ad8366_probe,
+	.remove		= __devexit_p(ad8366_remove),
+	.id_table	= ad8366_id,
+};
+
+module_spi_driver(ad8366_driver);
+
+MODULE_AUTHOR("Michael Hennerich <hennerich@blackfin.uclinux.org>");
+MODULE_DESCRIPTION("Analog Devices AD8366 VGA");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/staging/iio/iio_core.h b/drivers/iio/iio_core.h
index c9dfcba0bac..f652e6ae5a3 100644
--- a/drivers/staging/iio/iio_core.h
+++ b/drivers/iio/iio_core.h
@@ -12,6 +12,12 @@
 
 #ifndef _IIO_CORE_H_
 #define _IIO_CORE_H_
+#include <linux/kernel.h>
+#include <linux/device.h>
+
+struct iio_chan_spec;
+struct iio_dev;
+
 
 int __iio_add_chan_devattr(const char *postfix,
 			   struct iio_chan_spec const *chan,
diff --git a/drivers/staging/iio/iio_core_trigger.h b/drivers/iio/iio_core_trigger.h
index 6f7c56fcbe7..6f7c56fcbe7 100644
--- a/drivers/staging/iio/iio_core_trigger.h
+++ b/drivers/iio/iio_core_trigger.h
diff --git a/drivers/staging/iio/industrialio-buffer.c b/drivers/iio/industrialio-buffer.c
index 386ba760f3f..ac185b8694b 100644
--- a/drivers/staging/iio/industrialio-buffer.c
+++ b/drivers/iio/industrialio-buffer.c
@@ -21,10 +21,10 @@
 #include <linux/slab.h>
 #include <linux/poll.h>
 
-#include "iio.h"
+#include <linux/iio/iio.h>
 #include "iio_core.h"
-#include "sysfs.h"
-#include "buffer.h"
+#include <linux/iio/sysfs.h>
+#include <linux/iio/buffer.h>
 
 static const char * const iio_endian_prefix[] = {
 	[IIO_BE] = "be",
@@ -105,7 +105,7 @@ static ssize_t iio_scan_el_show(struct device *dev,
 				char *buf)
 {
 	int ret;
-	struct iio_dev *indio_dev = dev_get_drvdata(dev);
+	struct iio_dev *indio_dev = dev_to_iio_dev(dev);
 
 	ret = test_bit(to_iio_dev_attr(attr)->address,
 		       indio_dev->buffer->scan_mask);
@@ -124,13 +124,15 @@ static ssize_t iio_scan_el_store(struct device *dev,
 				 const char *buf,
 				 size_t len)
 {
-	int ret = 0;
+	int ret;
 	bool state;
-	struct iio_dev *indio_dev = dev_get_drvdata(dev);
+	struct iio_dev *indio_dev = dev_to_iio_dev(dev);
 	struct iio_buffer *buffer = indio_dev->buffer;
 	struct iio_dev_attr *this_attr = to_iio_dev_attr(attr);
 
-	state = !(buf[0] == '0');
+	ret = strtobool(buf, &state);
+	if (ret < 0)
+		return ret;
 	mutex_lock(&indio_dev->mlock);
 	if (iio_buffer_enabled(indio_dev)) {
 		ret = -EBUSY;
@@ -160,7 +162,7 @@ static ssize_t iio_scan_el_ts_show(struct device *dev,
 				   struct device_attribute *attr,
 				   char *buf)
 {
-	struct iio_dev *indio_dev = dev_get_drvdata(dev);
+	struct iio_dev *indio_dev = dev_to_iio_dev(dev);
 	return sprintf(buf, "%d\n", indio_dev->buffer->scan_timestamp);
 }
 
@@ -169,17 +171,21 @@ static ssize_t iio_scan_el_ts_store(struct device *dev,
 				    const char *buf,
 				    size_t len)
 {
-	int ret = 0;
-	struct iio_dev *indio_dev = dev_get_drvdata(dev);
+	int ret;
+	struct iio_dev *indio_dev = dev_to_iio_dev(dev);
 	bool state;
 
-	state = !(buf[0] == '0');
+	ret = strtobool(buf, &state);
+	if (ret < 0)
+		return ret;
+
 	mutex_lock(&indio_dev->mlock);
 	if (iio_buffer_enabled(indio_dev)) {
 		ret = -EBUSY;
 		goto error_ret;
 	}
 	indio_dev->buffer->scan_timestamp = state;
+	indio_dev->scan_timestamp = state;
 error_ret:
 	mutex_unlock(&indio_dev->mlock);
 
@@ -291,7 +297,7 @@ int iio_buffer_register(struct iio_dev *indio_dev,
 				goto error_cleanup_dynamic;
 			attrcount += ret;
 			if (channels[i].type == IIO_TIMESTAMP)
-				buffer->scan_index_timestamp =
+				indio_dev->scan_index_timestamp =
 					channels[i].scan_index;
 		}
 		if (indio_dev->masklength && buffer->scan_mask == NULL) {
@@ -346,7 +352,7 @@ ssize_t iio_buffer_read_length(struct device *dev,
 			       struct device_attribute *attr,
 			       char *buf)
 {
-	struct iio_dev *indio_dev = dev_get_drvdata(dev);
+	struct iio_dev *indio_dev = dev_to_iio_dev(dev);
 	struct iio_buffer *buffer = indio_dev->buffer;
 
 	if (buffer->access->get_length)
@@ -364,7 +370,7 @@ ssize_t iio_buffer_write_length(struct device *dev,
 {
 	int ret;
 	ulong val;
-	struct iio_dev *indio_dev = dev_get_drvdata(dev);
+	struct iio_dev *indio_dev = dev_to_iio_dev(dev);
 	struct iio_buffer *buffer = indio_dev->buffer;
 
 	ret = strict_strtoul(buf, 10, &val);
@@ -397,7 +403,7 @@ ssize_t iio_buffer_store_enable(struct device *dev,
 	int ret;
 	bool requested_state, current_state;
 	int previous_mode;
-	struct iio_dev *indio_dev = dev_get_drvdata(dev);
+	struct iio_dev *indio_dev = dev_to_iio_dev(dev);
 	struct iio_buffer *buffer = indio_dev->buffer;
 
 	mutex_lock(&indio_dev->mlock);
@@ -483,7 +489,7 @@ ssize_t iio_buffer_show_enable(struct device *dev,
 			       struct device_attribute *attr,
 			       char *buf)
 {
-	struct iio_dev *indio_dev = dev_get_drvdata(dev);
+	struct iio_dev *indio_dev = dev_to_iio_dev(dev);
 	return sprintf(buf, "%d\n", iio_buffer_enabled(indio_dev));
 }
 EXPORT_SYMBOL(iio_buffer_show_enable);
@@ -503,30 +509,41 @@ static const unsigned long *iio_scan_mask_match(const unsigned long *av_masks,
 	return NULL;
 }
 
-int iio_sw_buffer_preenable(struct iio_dev *indio_dev)
+static int iio_compute_scan_bytes(struct iio_dev *indio_dev, const long *mask,
+				  bool timestamp)
 {
-	struct iio_buffer *buffer = indio_dev->buffer;
 	const struct iio_chan_spec *ch;
 	unsigned bytes = 0;
 	int length, i;
-	dev_dbg(&indio_dev->dev, "%s\n", __func__);
 
 	/* How much space will the demuxed element take? */
-	for_each_set_bit(i, buffer->scan_mask,
+	for_each_set_bit(i, mask,
 			 indio_dev->masklength) {
 		ch = iio_find_channel_from_si(indio_dev, i);
-		length = ch->scan_type.storagebits/8;
+		length = ch->scan_type.storagebits / 8;
 		bytes = ALIGN(bytes, length);
 		bytes += length;
 	}
-	if (buffer->scan_timestamp) {
+	if (timestamp) {
 		ch = iio_find_channel_from_si(indio_dev,
-					      buffer->scan_index_timestamp);
-		length = ch->scan_type.storagebits/8;
+					      indio_dev->scan_index_timestamp);
+		length = ch->scan_type.storagebits / 8;
 		bytes = ALIGN(bytes, length);
 		bytes += length;
 	}
-	buffer->access->set_bytes_per_datum(buffer, bytes);
+	return bytes;
+}
+
+int iio_sw_buffer_preenable(struct iio_dev *indio_dev)
+{
+	struct iio_buffer *buffer = indio_dev->buffer;
+	dev_dbg(&indio_dev->dev, "%s\n", __func__);
+
+	/* How much space will the demuxed element take? */
+	indio_dev->scan_bytes =
+		iio_compute_scan_bytes(indio_dev, buffer->scan_mask,
+				       buffer->scan_timestamp);
+	buffer->access->set_bytes_per_datum(buffer, indio_dev->scan_bytes);
 
 	/* What scan mask do we actually have ?*/
 	if (indio_dev->available_scan_masks)
@@ -638,19 +655,25 @@ int iio_push_to_buffer(struct iio_buffer *buffer, unsigned char *data,
 }
 EXPORT_SYMBOL_GPL(iio_push_to_buffer);
 
+static void iio_buffer_demux_free(struct iio_buffer *buffer)
+{
+	struct iio_demux_table *p, *q;
+	list_for_each_entry_safe(p, q, &buffer->demux_list, l) {
+		list_del(&p->l);
+		kfree(p);
+	}
+}
+
 int iio_update_demux(struct iio_dev *indio_dev)
 {
 	const struct iio_chan_spec *ch;
 	struct iio_buffer *buffer = indio_dev->buffer;
 	int ret, in_ind = -1, out_ind, length;
 	unsigned in_loc = 0, out_loc = 0;
-	struct iio_demux_table *p, *q;
+	struct iio_demux_table *p;
 
 	/* Clear out any old demux */
-	list_for_each_entry_safe(p, q, &buffer->demux_list, l) {
-		list_del(&p->l);
-		kfree(p);
-	}
+	iio_buffer_demux_free(buffer);
 	kfree(buffer->demux_bounce);
 	buffer->demux_bounce = NULL;
 
@@ -704,7 +727,7 @@ int iio_update_demux(struct iio_dev *indio_dev)
 			goto error_clear_mux_table;
 		}
 		ch = iio_find_channel_from_si(indio_dev,
-			buffer->scan_index_timestamp);
+			indio_dev->scan_index_timestamp);
 		length = ch->scan_type.storagebits/8;
 		if (out_loc % length)
 			out_loc += length - out_loc % length;
@@ -725,10 +748,8 @@ int iio_update_demux(struct iio_dev *indio_dev)
 	return 0;
 
 error_clear_mux_table:
-	list_for_each_entry_safe(p, q, &buffer->demux_list, l) {
-		list_del(&p->l);
-		kfree(p);
-	}
+	iio_buffer_demux_free(buffer);
+
 	return ret;
 }
 EXPORT_SYMBOL_GPL(iio_update_demux);
diff --git a/drivers/staging/iio/industrialio-core.c b/drivers/iio/industrialio-core.c
index d303bfbff27..1ddd8861c71 100644
--- a/drivers/staging/iio/industrialio-core.c
+++ b/drivers/iio/industrialio-core.c
@@ -23,11 +23,11 @@
 #include <linux/slab.h>
 #include <linux/anon_inodes.h>
 #include <linux/debugfs.h>
-#include "iio.h"
+#include <linux/iio/iio.h>
 #include "iio_core.h"
 #include "iio_core_trigger.h"
-#include "sysfs.h"
-#include "events.h"
+#include <linux/iio/sysfs.h>
+#include <linux/iio/events.h>
 
 /* IDA to assign each registered device a unique id*/
 static DEFINE_IDA(iio_ida);
@@ -42,11 +42,6 @@ EXPORT_SYMBOL(iio_bus_type);
 
 static struct dentry *iio_debugfs_dentry;
 
-static const char * const iio_data_type_name[] = {
-	[IIO_RAW] = "raw",
-	[IIO_PROCESSED] = "input",
-};
-
 static const char * const iio_direction[] = {
 	[0] = "in",
 	[1] = "out",
@@ -68,6 +63,7 @@ static const char * const iio_chan_type_name_spec[] = {
 	[IIO_ANGL] = "angl",
 	[IIO_TIMESTAMP] = "timestamp",
 	[IIO_CAPACITANCE] = "capacitance",
+	[IIO_ALTVOLTAGE] = "altvoltage",
 };
 
 static const char * const iio_modifier_names[] = {
@@ -80,6 +76,8 @@ static const char * const iio_modifier_names[] = {
 
 /* relies on pairs of these shared then separate */
 static const char * const iio_chan_info_postfix[] = {
+	[IIO_CHAN_INFO_RAW] = "raw",
+	[IIO_CHAN_INFO_PROCESSED] = "input",
 	[IIO_CHAN_INFO_SCALE] = "scale",
 	[IIO_CHAN_INFO_OFFSET] = "offset",
 	[IIO_CHAN_INFO_CALIBSCALE] = "calibscale",
@@ -90,6 +88,10 @@ static const char * const iio_chan_info_postfix[] = {
 	[IIO_CHAN_INFO_AVERAGE_RAW] = "mean_raw",
 	[IIO_CHAN_INFO_LOW_PASS_FILTER_3DB_FREQUENCY]
 	= "filter_low_pass_3db_frequency",
+	[IIO_CHAN_INFO_SAMP_FREQ] = "sampling_frequency",
+	[IIO_CHAN_INFO_FREQUENCY] = "frequency",
+	[IIO_CHAN_INFO_PHASE] = "phase",
+	[IIO_CHAN_INFO_HARDWAREGAIN] = "hardwaregain",
 };
 
 const struct iio_chan_spec
@@ -151,14 +153,6 @@ static void __exit iio_exit(void)
 }
 
 #if defined(CONFIG_DEBUG_FS)
-static int iio_debugfs_open(struct inode *inode, struct file *file)
-{
-	if (inode->i_private)
-		file->private_data = inode->i_private;
-
-	return 0;
-}
-
 static ssize_t iio_debugfs_read_reg(struct file *file, char __user *userbuf,
 			      size_t count, loff_t *ppos)
 {
@@ -217,7 +211,7 @@ static ssize_t iio_debugfs_write_reg(struct file *file,
 }
 
 static const struct file_operations iio_debugfs_reg_fops = {
-	.open = iio_debugfs_open,
+	.open = simple_open,
 	.read = iio_debugfs_read_reg,
 	.write = iio_debugfs_write_reg,
 };
@@ -234,15 +228,12 @@ static int iio_device_register_debugfs(struct iio_dev *indio_dev)
 	if (indio_dev->info->debugfs_reg_access == NULL)
 		return 0;
 
-	if (IS_ERR(iio_debugfs_dentry))
+	if (!iio_debugfs_dentry)
 		return 0;
 
 	indio_dev->debugfs_dentry =
 		debugfs_create_dir(dev_name(&indio_dev->dev),
 				   iio_debugfs_dentry);
-	if (IS_ERR(indio_dev->debugfs_dentry))
-		return PTR_ERR(indio_dev->debugfs_dentry);
-
 	if (indio_dev->debugfs_dentry == NULL) {
 		dev_warn(indio_dev->dev.parent,
 			 "Failed to create debugfs directory\n");
@@ -274,13 +265,13 @@ static ssize_t iio_read_channel_ext_info(struct device *dev,
 				     struct device_attribute *attr,
 				     char *buf)
 {
-	struct iio_dev *indio_dev = dev_get_drvdata(dev);
+	struct iio_dev *indio_dev = dev_to_iio_dev(dev);
 	struct iio_dev_attr *this_attr = to_iio_dev_attr(attr);
 	const struct iio_chan_spec_ext_info *ext_info;
 
 	ext_info = &this_attr->c->ext_info[this_attr->address];
 
-	return ext_info->read(indio_dev, this_attr->c, buf);
+	return ext_info->read(indio_dev, ext_info->private, this_attr->c, buf);
 }
 
 static ssize_t iio_write_channel_ext_info(struct device *dev,
@@ -288,42 +279,50 @@ static ssize_t iio_write_channel_ext_info(struct device *dev,
 				     const char *buf,
 					 size_t len)
 {
-	struct iio_dev *indio_dev = dev_get_drvdata(dev);
+	struct iio_dev *indio_dev = dev_to_iio_dev(dev);
 	struct iio_dev_attr *this_attr = to_iio_dev_attr(attr);
 	const struct iio_chan_spec_ext_info *ext_info;
 
 	ext_info = &this_attr->c->ext_info[this_attr->address];
 
-	return ext_info->write(indio_dev, this_attr->c, buf, len);
+	return ext_info->write(indio_dev, ext_info->private,
+			       this_attr->c, buf, len);
 }
 
 static ssize_t iio_read_channel_info(struct device *dev,
 				     struct device_attribute *attr,
 				     char *buf)
 {
-	struct iio_dev *indio_dev = dev_get_drvdata(dev);
+	struct iio_dev *indio_dev = dev_to_iio_dev(dev);
 	struct iio_dev_attr *this_attr = to_iio_dev_attr(attr);
 	int val, val2;
+	bool scale_db = false;
 	int ret = indio_dev->info->read_raw(indio_dev, this_attr->c,
 					    &val, &val2, this_attr->address);
 
 	if (ret < 0)
 		return ret;
 
-	if (ret == IIO_VAL_INT)
+	switch (ret) {
+	case IIO_VAL_INT:
 		return sprintf(buf, "%d\n", val);
-	else if (ret == IIO_VAL_INT_PLUS_MICRO) {
+	case IIO_VAL_INT_PLUS_MICRO_DB:
+		scale_db = true;
+	case IIO_VAL_INT_PLUS_MICRO:
 		if (val2 < 0)
-			return sprintf(buf, "-%d.%06u\n", val, -val2);
+			return sprintf(buf, "-%d.%06u%s\n", val, -val2,
+				scale_db ? " dB" : "");
 		else
-			return sprintf(buf, "%d.%06u\n", val, val2);
-	} else if (ret == IIO_VAL_INT_PLUS_NANO) {
+			return sprintf(buf, "%d.%06u%s\n", val, val2,
+				scale_db ? " dB" : "");
+	case IIO_VAL_INT_PLUS_NANO:
 		if (val2 < 0)
 			return sprintf(buf, "-%d.%09u\n", val, -val2);
 		else
 			return sprintf(buf, "%d.%09u\n", val, val2);
-	} else
+	default:
 		return 0;
+	}
 }
 
 static ssize_t iio_write_channel_info(struct device *dev,
@@ -331,7 +330,7 @@ static ssize_t iio_write_channel_info(struct device *dev,
 				      const char *buf,
 				      size_t len)
 {
-	struct iio_dev *indio_dev = dev_get_drvdata(dev);
+	struct iio_dev *indio_dev = dev_to_iio_dev(dev);
 	struct iio_dev_attr *this_attr = to_iio_dev_attr(attr);
 	int ret, integer = 0, fract = 0, fract_mult = 100000;
 	bool integer_part = true, negative = false;
@@ -575,25 +574,12 @@ error_ret:
 static int iio_device_add_channel_sysfs(struct iio_dev *indio_dev,
 					struct iio_chan_spec const *chan)
 {
-	int ret, i, attrcount = 0;
+	int ret, attrcount = 0;
+	int i;
 	const struct iio_chan_spec_ext_info *ext_info;
 
 	if (chan->channel < 0)
 		return 0;
-
-	ret = __iio_add_chan_devattr(iio_data_type_name[chan->processed_val],
-				     chan,
-				     &iio_read_channel_info,
-				     (chan->output ?
-				      &iio_write_channel_info : NULL),
-				     0,
-				     0,
-				     &indio_dev->dev,
-				     &indio_dev->channel_attr_list);
-	if (ret)
-		goto error_ret;
-	attrcount++;
-
 	for_each_set_bit(i, &chan->info_mask, sizeof(long)*8) {
 		ret = __iio_add_chan_devattr(iio_chan_info_postfix[i/2],
 					     chan,
@@ -652,7 +638,7 @@ static ssize_t iio_show_dev_name(struct device *dev,
 				 struct device_attribute *attr,
 				 char *buf)
 {
-	struct iio_dev *indio_dev = dev_get_drvdata(dev);
+	struct iio_dev *indio_dev = dev_to_iio_dev(dev);
 	return sprintf(buf, "%s\n", indio_dev->name);
 }
 
@@ -738,7 +724,7 @@ static void iio_device_unregister_sysfs(struct iio_dev *indio_dev)
 
 static void iio_dev_release(struct device *device)
 {
-	struct iio_dev *indio_dev = container_of(device, struct iio_dev, dev);
+	struct iio_dev *indio_dev = dev_to_iio_dev(device);
 	cdev_del(&indio_dev->chrdev);
 	if (indio_dev->modes & INDIO_BUFFER_TRIGGERED)
 		iio_device_unregister_trigger_consumer(indio_dev);
@@ -752,7 +738,7 @@ static struct device_type iio_dev_type = {
 	.release = iio_dev_release,
 };
 
-struct iio_dev *iio_allocate_device(int sizeof_priv)
+struct iio_dev *iio_device_alloc(int sizeof_priv)
 {
 	struct iio_dev *dev;
 	size_t alloc_size;
@@ -788,16 +774,16 @@ struct iio_dev *iio_allocate_device(int sizeof_priv)
 
 	return dev;
 }
-EXPORT_SYMBOL(iio_allocate_device);
+EXPORT_SYMBOL(iio_device_alloc);
 
-void iio_free_device(struct iio_dev *dev)
+void iio_device_free(struct iio_dev *dev)
 {
 	if (dev) {
 		ida_simple_remove(&iio_ida, dev->id);
 		kfree(dev);
 	}
 }
-EXPORT_SYMBOL(iio_free_device);
+EXPORT_SYMBOL(iio_device_free);
 
 /**
  * iio_chrdev_open() - chrdev file open for buffer access and ioctls
diff --git a/drivers/staging/iio/industrialio-event.c b/drivers/iio/industrialio-event.c
index 5fdf739e38f..b49059de5d0 100644
--- a/drivers/staging/iio/industrialio-event.c
+++ b/drivers/iio/industrialio-event.c
@@ -20,10 +20,10 @@
 #include <linux/slab.h>
 #include <linux/uaccess.h>
 #include <linux/wait.h>
-#include "iio.h"
+#include <linux/iio/iio.h>
 #include "iio_core.h"
-#include "sysfs.h"
-#include "events.h"
+#include <linux/iio/sysfs.h>
+#include <linux/iio/events.h>
 
 /**
  * struct iio_event_interface - chrdev interface for an event line
@@ -186,7 +186,7 @@ static ssize_t iio_ev_state_store(struct device *dev,
 				  const char *buf,
 				  size_t len)
 {
-	struct iio_dev *indio_dev = dev_get_drvdata(dev);
+	struct iio_dev *indio_dev = dev_to_iio_dev(dev);
 	struct iio_dev_attr *this_attr = to_iio_dev_attr(attr);
 	int ret;
 	bool val;
@@ -205,7 +205,7 @@ static ssize_t iio_ev_state_show(struct device *dev,
 				 struct device_attribute *attr,
 				 char *buf)
 {
-	struct iio_dev *indio_dev = dev_get_drvdata(dev);
+	struct iio_dev *indio_dev = dev_to_iio_dev(dev);
 	struct iio_dev_attr *this_attr = to_iio_dev_attr(attr);
 	int val = indio_dev->info->read_event_config(indio_dev,
 						     this_attr->address);
@@ -220,7 +220,7 @@ static ssize_t iio_ev_value_show(struct device *dev,
 				 struct device_attribute *attr,
 				 char *buf)
 {
-	struct iio_dev *indio_dev = dev_get_drvdata(dev);
+	struct iio_dev *indio_dev = dev_to_iio_dev(dev);
 	struct iio_dev_attr *this_attr = to_iio_dev_attr(attr);
 	int val, ret;
 
@@ -237,7 +237,7 @@ static ssize_t iio_ev_value_store(struct device *dev,
 				  const char *buf,
 				  size_t len)
 {
-	struct iio_dev *indio_dev = dev_get_drvdata(dev);
+	struct iio_dev *indio_dev = dev_to_iio_dev(dev);
 	struct iio_dev_attr *this_attr = to_iio_dev_attr(attr);
 	unsigned long val;
 	int ret;
diff --git a/drivers/staging/iio/industrialio-trigger.c b/drivers/iio/industrialio-trigger.c
index 47ecadd4818..0f582df75a1 100644
--- a/drivers/staging/iio/industrialio-trigger.c
+++ b/drivers/iio/industrialio-trigger.c
@@ -15,11 +15,11 @@
 #include <linux/list.h>
 #include <linux/slab.h>
 
-#include "iio.h"
-#include "trigger.h"
+#include <linux/iio/iio.h>
+#include <linux/iio/trigger.h>
 #include "iio_core.h"
 #include "iio_core_trigger.h"
-#include "trigger_consumer.h"
+#include <linux/iio/trigger_consumer.h>
 
 /* RFC - Question of approach
  * Make the common case (single sensor single trigger)
@@ -310,7 +310,7 @@ static ssize_t iio_trigger_read_current(struct device *dev,
 					struct device_attribute *attr,
 					char *buf)
 {
-	struct iio_dev *indio_dev = dev_get_drvdata(dev);
+	struct iio_dev *indio_dev = dev_to_iio_dev(dev);
 
 	if (indio_dev->trig)
 		return sprintf(buf, "%s\n", indio_dev->trig->name);
@@ -329,7 +329,7 @@ static ssize_t iio_trigger_write_current(struct device *dev,
 					 const char *buf,
 					 size_t len)
 {
-	struct iio_dev *indio_dev = dev_get_drvdata(dev);
+	struct iio_dev *indio_dev = dev_to_iio_dev(dev);
 	struct iio_trigger *oldtrig = indio_dev->trig;
 	struct iio_trigger *trig;
 	int ret;
@@ -360,9 +360,9 @@ static ssize_t iio_trigger_write_current(struct device *dev,
 	indio_dev->trig = trig;
 
 	if (oldtrig && indio_dev->trig != oldtrig)
-		iio_put_trigger(oldtrig);
+		iio_trigger_put(oldtrig);
 	if (indio_dev->trig)
-		iio_get_trigger(indio_dev->trig);
+		iio_trigger_get(indio_dev->trig);
 
 	return len;
 }
@@ -426,7 +426,7 @@ static void iio_trig_subirqunmask(struct irq_data *d)
 	trig->subirqs[d->irq - trig->subirq_base].enabled = true;
 }
 
-struct iio_trigger *iio_allocate_trigger(const char *fmt, ...)
+struct iio_trigger *iio_trigger_alloc(const char *fmt, ...)
 {
 	va_list vargs;
 	struct iio_trigger *trig;
@@ -472,14 +472,14 @@ struct iio_trigger *iio_allocate_trigger(const char *fmt, ...)
 	}
 	return trig;
 }
-EXPORT_SYMBOL(iio_allocate_trigger);
+EXPORT_SYMBOL(iio_trigger_alloc);
 
-void iio_free_trigger(struct iio_trigger *trig)
+void iio_trigger_free(struct iio_trigger *trig)
 {
 	if (trig)
 		put_device(&trig->dev);
 }
-EXPORT_SYMBOL(iio_free_trigger);
+EXPORT_SYMBOL(iio_trigger_free);
 
 void iio_device_register_trigger_consumer(struct iio_dev *indio_dev)
 {
@@ -491,7 +491,7 @@ void iio_device_unregister_trigger_consumer(struct iio_dev *indio_dev)
 {
 	/* Clean up and associated but not attached triggers references */
 	if (indio_dev->trig)
-		iio_put_trigger(indio_dev->trig);
+		iio_trigger_put(indio_dev->trig);
 }
 
 int iio_triggered_buffer_postenable(struct iio_dev *indio_dev)
diff --git a/drivers/staging/iio/inkern.c b/drivers/iio/inkern.c
index ef07a02bf54..922645893dc 100644
--- a/drivers/staging/iio/inkern.c
+++ b/drivers/iio/inkern.c
@@ -11,11 +11,11 @@
 #include <linux/slab.h>
 #include <linux/mutex.h>
 
-#include "iio.h"
+#include <linux/iio/iio.h>
 #include "iio_core.h"
-#include "machine.h"
-#include "driver.h"
-#include "consumer.h"
+#include <linux/iio/machine.h>
+#include <linux/iio/driver.h>
+#include <linux/iio/consumer.h>
 
 struct iio_map_internal {
 	struct iio_dev *indio_dev;
diff --git a/drivers/staging/iio/kfifo_buf.c b/drivers/iio/kfifo_buf.c
index 9f3bd59c0e7..6bf9d05f484 100644
--- a/drivers/staging/iio/kfifo_buf.c
+++ b/drivers/iio/kfifo_buf.c
@@ -5,8 +5,7 @@
 #include <linux/workqueue.h>
 #include <linux/kfifo.h>
 #include <linux/mutex.h>
-
-#include "kfifo_buf.h"
+#include <linux/iio/kfifo_buf.h>
 
 struct iio_kfifo {
 	struct iio_buffer buffer;
diff --git a/drivers/infiniband/Kconfig b/drivers/infiniband/Kconfig
index eb0add311dc..a0f29c1d03b 100644
--- a/drivers/infiniband/Kconfig
+++ b/drivers/infiniband/Kconfig
@@ -51,6 +51,7 @@ source "drivers/infiniband/hw/cxgb3/Kconfig"
 source "drivers/infiniband/hw/cxgb4/Kconfig"
 source "drivers/infiniband/hw/mlx4/Kconfig"
 source "drivers/infiniband/hw/nes/Kconfig"
+source "drivers/infiniband/hw/ocrdma/Kconfig"
 
 source "drivers/infiniband/ulp/ipoib/Kconfig"
 
diff --git a/drivers/infiniband/Makefile b/drivers/infiniband/Makefile
index a3b2d8eac86..bf846a14b9d 100644
--- a/drivers/infiniband/Makefile
+++ b/drivers/infiniband/Makefile
@@ -8,6 +8,7 @@ obj-$(CONFIG_INFINIBAND_CXGB3)		+= hw/cxgb3/
 obj-$(CONFIG_INFINIBAND_CXGB4)		+= hw/cxgb4/
 obj-$(CONFIG_MLX4_INFINIBAND)		+= hw/mlx4/
 obj-$(CONFIG_INFINIBAND_NES)		+= hw/nes/
+obj-$(CONFIG_INFINIBAND_OCRDMA)		+= hw/ocrdma/
 obj-$(CONFIG_INFINIBAND_IPOIB)		+= ulp/ipoib/
 obj-$(CONFIG_INFINIBAND_SRP)		+= ulp/srp/
 obj-$(CONFIG_INFINIBAND_SRPT)		+= ulp/srpt/
diff --git a/drivers/infiniband/core/cma.c b/drivers/infiniband/core/cma.c
index e3e470fecaa..55d5642eb10 100644
--- a/drivers/infiniband/core/cma.c
+++ b/drivers/infiniband/core/cma.c
@@ -42,6 +42,7 @@
 #include <linux/inetdevice.h>
 #include <linux/slab.h>
 #include <linux/module.h>
+#include <net/route.h>
 
 #include <net/tcp.h>
 #include <net/ipv6.h>
@@ -1218,13 +1219,13 @@ static int cma_req_handler(struct ib_cm_id *cm_id, struct ib_cm_event *ib_event)
 	}
 	if (!conn_id) {
 		ret = -ENOMEM;
-		goto out;
+		goto err1;
 	}
 
 	mutex_lock_nested(&conn_id->handler_mutex, SINGLE_DEPTH_NESTING);
 	ret = cma_acquire_dev(conn_id);
 	if (ret)
-		goto release_conn_id;
+		goto err2;
 
 	conn_id->cm_id.ib = cm_id;
 	cm_id->context = conn_id;
@@ -1236,31 +1237,33 @@ static int cma_req_handler(struct ib_cm_id *cm_id, struct ib_cm_event *ib_event)
 	 */
 	atomic_inc(&conn_id->refcount);
 	ret = conn_id->id.event_handler(&conn_id->id, &event);
-	if (!ret) {
-		/*
-		 * Acquire mutex to prevent user executing rdma_destroy_id()
-		 * while we're accessing the cm_id.
-		 */
-		mutex_lock(&lock);
-		if (cma_comp(conn_id, RDMA_CM_CONNECT) && (conn_id->id.qp_type != IB_QPT_UD))
-			ib_send_cm_mra(cm_id, CMA_CM_MRA_SETTING, NULL, 0);
-		mutex_unlock(&lock);
-		mutex_unlock(&conn_id->handler_mutex);
-		cma_deref_id(conn_id);
-		goto out;
-	}
+	if (ret)
+		goto err3;
+
+	/*
+	 * Acquire mutex to prevent user executing rdma_destroy_id()
+	 * while we're accessing the cm_id.
+	 */
+	mutex_lock(&lock);
+	if (cma_comp(conn_id, RDMA_CM_CONNECT) && (conn_id->id.qp_type != IB_QPT_UD))
+		ib_send_cm_mra(cm_id, CMA_CM_MRA_SETTING, NULL, 0);
+	mutex_unlock(&lock);
+	mutex_unlock(&conn_id->handler_mutex);
+	mutex_unlock(&listen_id->handler_mutex);
 	cma_deref_id(conn_id);
+	return 0;
 
+err3:
+	cma_deref_id(conn_id);
 	/* Destroy the CM ID by returning a non-zero value. */
 	conn_id->cm_id.ib = NULL;
-
-release_conn_id:
+err2:
 	cma_exch(conn_id, RDMA_CM_DESTROYING);
 	mutex_unlock(&conn_id->handler_mutex);
-	rdma_destroy_id(&conn_id->id);
-
-out:
+err1:
 	mutex_unlock(&listen_id->handler_mutex);
+	if (conn_id)
+		rdma_destroy_id(&conn_id->id);
 	return ret;
 }
 
@@ -1826,7 +1829,10 @@ static int cma_resolve_iboe_route(struct rdma_id_private *id_priv)
 	route->path_rec->reversible = 1;
 	route->path_rec->pkey = cpu_to_be16(0xffff);
 	route->path_rec->mtu_selector = IB_SA_EQ;
-	route->path_rec->sl = id_priv->tos >> 5;
+	route->path_rec->sl = netdev_get_prio_tc_map(
+			ndev->priv_flags & IFF_802_1Q_VLAN ?
+				vlan_dev_real_dev(ndev) : ndev,
+			rt_tos2priority(id_priv->tos));
 
 	route->path_rec->mtu = iboe_get_mtu(ndev->mtu);
 	route->path_rec->rate_selector = IB_SA_EQ;
diff --git a/drivers/infiniband/core/netlink.c b/drivers/infiniband/core/netlink.c
index 396e2937030..e497dfbee43 100644
--- a/drivers/infiniband/core/netlink.c
+++ b/drivers/infiniband/core/netlink.c
@@ -125,7 +125,8 @@ int ibnl_put_attr(struct sk_buff *skb, struct nlmsghdr *nlh,
 	unsigned char *prev_tail;
 
 	prev_tail = skb_tail_pointer(skb);
-	NLA_PUT(skb, type, len, data);
+	if (nla_put(skb, type, len, data))
+		goto nla_put_failure;
 	nlh->nlmsg_len += skb_tail_pointer(skb) - prev_tail;
 	return 0;
 
diff --git a/drivers/infiniband/core/ucma.c b/drivers/infiniband/core/ucma.c
index 5861cdb22b7..8002ae642cf 100644
--- a/drivers/infiniband/core/ucma.c
+++ b/drivers/infiniband/core/ucma.c
@@ -66,12 +66,6 @@ static ctl_table ucma_ctl_table[] = {
 	{ }
 };
 
-static struct ctl_path ucma_ctl_path[] = {
-	{ .procname = "net" },
-	{ .procname = "rdma_ucm" },
-	{ }
-};
-
 struct ucma_file {
 	struct mutex		mut;
 	struct file		*filp;
@@ -1392,7 +1386,7 @@ static int __init ucma_init(void)
 		goto err1;
 	}
 
-	ucma_ctl_table_hdr = register_sysctl_paths(ucma_ctl_path, ucma_ctl_table);
+	ucma_ctl_table_hdr = register_net_sysctl(&init_net, "net/rdma_ucm", ucma_ctl_table);
 	if (!ucma_ctl_table_hdr) {
 		printk(KERN_ERR "rdma_ucm: couldn't register sysctl paths\n");
 		ret = -ENOMEM;
@@ -1408,7 +1402,7 @@ err1:
 
 static void __exit ucma_cleanup(void)
 {
-	unregister_sysctl_table(ucma_ctl_table_hdr);
+	unregister_net_sysctl_table(ucma_ctl_table_hdr);
 	device_remove_file(ucma_misc.this_device, &dev_attr_abi_version);
 	misc_deregister(&ucma_misc);
 	idr_destroy(&ctx_idr);
diff --git a/drivers/infiniband/core/umem.c b/drivers/infiniband/core/umem.c
index 71f0c0f7df9..a8411232207 100644
--- a/drivers/infiniband/core/umem.c
+++ b/drivers/infiniband/core/umem.c
@@ -269,7 +269,7 @@ void ib_umem_release(struct ib_umem *umem)
 	} else
 		down_write(&mm->mmap_sem);
 
-	current->mm->locked_vm -= diff;
+	current->mm->pinned_vm -= diff;
 	up_write(&mm->mmap_sem);
 	mmput(mm);
 	kfree(umem);
diff --git a/drivers/infiniband/core/uverbs_cmd.c b/drivers/infiniband/core/uverbs_cmd.c
index 4d27e4c3fe3..f9d0d7c413a 100644
--- a/drivers/infiniband/core/uverbs_cmd.c
+++ b/drivers/infiniband/core/uverbs_cmd.c
@@ -41,13 +41,18 @@
 
 #include "uverbs.h"
 
-static struct lock_class_key pd_lock_key;
-static struct lock_class_key mr_lock_key;
-static struct lock_class_key cq_lock_key;
-static struct lock_class_key qp_lock_key;
-static struct lock_class_key ah_lock_key;
-static struct lock_class_key srq_lock_key;
-static struct lock_class_key xrcd_lock_key;
+struct uverbs_lock_class {
+	struct lock_class_key	key;
+	char			name[16];
+};
+
+static struct uverbs_lock_class pd_lock_class	= { .name = "PD-uobj" };
+static struct uverbs_lock_class mr_lock_class	= { .name = "MR-uobj" };
+static struct uverbs_lock_class cq_lock_class	= { .name = "CQ-uobj" };
+static struct uverbs_lock_class qp_lock_class	= { .name = "QP-uobj" };
+static struct uverbs_lock_class ah_lock_class	= { .name = "AH-uobj" };
+static struct uverbs_lock_class srq_lock_class	= { .name = "SRQ-uobj" };
+static struct uverbs_lock_class xrcd_lock_class = { .name = "XRCD-uobj" };
 
 #define INIT_UDATA(udata, ibuf, obuf, ilen, olen)			\
 	do {								\
@@ -83,13 +88,13 @@ static struct lock_class_key xrcd_lock_key;
  */
 
 static void init_uobj(struct ib_uobject *uobj, u64 user_handle,
-		      struct ib_ucontext *context, struct lock_class_key *key)
+		      struct ib_ucontext *context, struct uverbs_lock_class *c)
 {
 	uobj->user_handle = user_handle;
 	uobj->context     = context;
 	kref_init(&uobj->ref);
 	init_rwsem(&uobj->mutex);
-	lockdep_set_class(&uobj->mutex, key);
+	lockdep_set_class_and_name(&uobj->mutex, &c->key, c->name);
 	uobj->live        = 0;
 }
 
@@ -522,7 +527,7 @@ ssize_t ib_uverbs_alloc_pd(struct ib_uverbs_file *file,
 	if (!uobj)
 		return -ENOMEM;
 
-	init_uobj(uobj, 0, file->ucontext, &pd_lock_key);
+	init_uobj(uobj, 0, file->ucontext, &pd_lock_class);
 	down_write(&uobj->mutex);
 
 	pd = file->device->ib_dev->alloc_pd(file->device->ib_dev,
@@ -750,7 +755,7 @@ ssize_t ib_uverbs_open_xrcd(struct ib_uverbs_file *file,
 		goto err_tree_mutex_unlock;
 	}
 
-	init_uobj(&obj->uobject, 0, file->ucontext, &xrcd_lock_key);
+	init_uobj(&obj->uobject, 0, file->ucontext, &xrcd_lock_class);
 
 	down_write(&obj->uobject.mutex);
 
@@ -947,7 +952,7 @@ ssize_t ib_uverbs_reg_mr(struct ib_uverbs_file *file,
 	if (!uobj)
 		return -ENOMEM;
 
-	init_uobj(uobj, 0, file->ucontext, &mr_lock_key);
+	init_uobj(uobj, 0, file->ucontext, &mr_lock_class);
 	down_write(&uobj->mutex);
 
 	pd = idr_read_pd(cmd.pd_handle, file->ucontext);
@@ -1115,7 +1120,7 @@ ssize_t ib_uverbs_create_cq(struct ib_uverbs_file *file,
 	if (!obj)
 		return -ENOMEM;
 
-	init_uobj(&obj->uobject, cmd.user_handle, file->ucontext, &cq_lock_key);
+	init_uobj(&obj->uobject, cmd.user_handle, file->ucontext, &cq_lock_class);
 	down_write(&obj->uobject.mutex);
 
 	if (cmd.comp_channel >= 0) {
@@ -1399,6 +1404,9 @@ ssize_t ib_uverbs_create_qp(struct ib_uverbs_file *file,
 	if (copy_from_user(&cmd, buf, sizeof cmd))
 		return -EFAULT;
 
+	if (cmd.qp_type == IB_QPT_RAW_PACKET && !capable(CAP_NET_RAW))
+		return -EPERM;
+
 	INIT_UDATA(&udata, buf + sizeof cmd,
 		   (unsigned long) cmd.response + sizeof resp,
 		   in_len - sizeof cmd, out_len - sizeof resp);
@@ -1407,7 +1415,7 @@ ssize_t ib_uverbs_create_qp(struct ib_uverbs_file *file,
 	if (!obj)
 		return -ENOMEM;
 
-	init_uobj(&obj->uevent.uobject, cmd.user_handle, file->ucontext, &qp_lock_key);
+	init_uobj(&obj->uevent.uobject, cmd.user_handle, file->ucontext, &qp_lock_class);
 	down_write(&obj->uevent.uobject.mutex);
 
 	if (cmd.qp_type == IB_QPT_XRC_TGT) {
@@ -1418,13 +1426,6 @@ ssize_t ib_uverbs_create_qp(struct ib_uverbs_file *file,
 		}
 		device = xrcd->device;
 	} else {
-		pd  = idr_read_pd(cmd.pd_handle, file->ucontext);
-		scq = idr_read_cq(cmd.send_cq_handle, file->ucontext, 0);
-		if (!pd || !scq) {
-			ret = -EINVAL;
-			goto err_put;
-		}
-
 		if (cmd.qp_type == IB_QPT_XRC_INI) {
 			cmd.max_recv_wr = cmd.max_recv_sge = 0;
 		} else {
@@ -1435,13 +1436,24 @@ ssize_t ib_uverbs_create_qp(struct ib_uverbs_file *file,
 					goto err_put;
 				}
 			}
-			rcq = (cmd.recv_cq_handle == cmd.send_cq_handle) ?
-			       scq : idr_read_cq(cmd.recv_cq_handle, file->ucontext, 1);
-			if (!rcq) {
-				ret = -EINVAL;
-				goto err_put;
+
+			if (cmd.recv_cq_handle != cmd.send_cq_handle) {
+				rcq = idr_read_cq(cmd.recv_cq_handle, file->ucontext, 0);
+				if (!rcq) {
+					ret = -EINVAL;
+					goto err_put;
+				}
 			}
 		}
+
+		scq = idr_read_cq(cmd.send_cq_handle, file->ucontext, !!rcq);
+		rcq = rcq ?: scq;
+		pd  = idr_read_pd(cmd.pd_handle, file->ucontext);
+		if (!pd || !scq) {
+			ret = -EINVAL;
+			goto err_put;
+		}
+
 		device = pd->device;
 	}
 
@@ -1585,7 +1597,7 @@ ssize_t ib_uverbs_open_qp(struct ib_uverbs_file *file,
 	if (!obj)
 		return -ENOMEM;
 
-	init_uobj(&obj->uevent.uobject, cmd.user_handle, file->ucontext, &qp_lock_key);
+	init_uobj(&obj->uevent.uobject, cmd.user_handle, file->ucontext, &qp_lock_class);
 	down_write(&obj->uevent.uobject.mutex);
 
 	xrcd = idr_read_xrcd(cmd.pd_handle, file->ucontext, &xrcd_uobj);
@@ -2272,7 +2284,7 @@ ssize_t ib_uverbs_create_ah(struct ib_uverbs_file *file,
 	if (!uobj)
 		return -ENOMEM;
 
-	init_uobj(uobj, cmd.user_handle, file->ucontext, &ah_lock_key);
+	init_uobj(uobj, cmd.user_handle, file->ucontext, &ah_lock_class);
 	down_write(&uobj->mutex);
 
 	pd = idr_read_pd(cmd.pd_handle, file->ucontext);
@@ -2476,30 +2488,30 @@ static int __uverbs_create_xsrq(struct ib_uverbs_file *file,
 	if (!obj)
 		return -ENOMEM;
 
-	init_uobj(&obj->uevent.uobject, cmd->user_handle, file->ucontext, &srq_lock_key);
+	init_uobj(&obj->uevent.uobject, cmd->user_handle, file->ucontext, &srq_lock_class);
 	down_write(&obj->uevent.uobject.mutex);
 
-	pd  = idr_read_pd(cmd->pd_handle, file->ucontext);
-	if (!pd) {
-		ret = -EINVAL;
-		goto err;
-	}
-
 	if (cmd->srq_type == IB_SRQT_XRC) {
-		attr.ext.xrc.cq  = idr_read_cq(cmd->cq_handle, file->ucontext, 0);
-		if (!attr.ext.xrc.cq) {
-			ret = -EINVAL;
-			goto err_put_pd;
-		}
-
 		attr.ext.xrc.xrcd  = idr_read_xrcd(cmd->xrcd_handle, file->ucontext, &xrcd_uobj);
 		if (!attr.ext.xrc.xrcd) {
 			ret = -EINVAL;
-			goto err_put_cq;
+			goto err;
 		}
 
 		obj->uxrcd = container_of(xrcd_uobj, struct ib_uxrcd_object, uobject);
 		atomic_inc(&obj->uxrcd->refcnt);
+
+		attr.ext.xrc.cq  = idr_read_cq(cmd->cq_handle, file->ucontext, 0);
+		if (!attr.ext.xrc.cq) {
+			ret = -EINVAL;
+			goto err_put_xrcd;
+		}
+	}
+
+	pd  = idr_read_pd(cmd->pd_handle, file->ucontext);
+	if (!pd) {
+		ret = -EINVAL;
+		goto err_put_cq;
 	}
 
 	attr.event_handler  = ib_uverbs_srq_event_handler;
@@ -2576,17 +2588,17 @@ err_destroy:
 	ib_destroy_srq(srq);
 
 err_put:
-	if (cmd->srq_type == IB_SRQT_XRC) {
-		atomic_dec(&obj->uxrcd->refcnt);
-		put_uobj_read(xrcd_uobj);
-	}
+	put_pd_read(pd);
 
 err_put_cq:
 	if (cmd->srq_type == IB_SRQT_XRC)
 		put_cq_read(attr.ext.xrc.cq);
 
-err_put_pd:
-	put_pd_read(pd);
+err_put_xrcd:
+	if (cmd->srq_type == IB_SRQT_XRC) {
+		atomic_dec(&obj->uxrcd->refcnt);
+		put_uobj_read(xrcd_uobj);
+	}
 
 err:
 	put_uobj_write(&obj->uevent.uobject);
diff --git a/drivers/infiniband/core/verbs.c b/drivers/infiniband/core/verbs.c
index 575b78045aa..30f199e8579 100644
--- a/drivers/infiniband/core/verbs.c
+++ b/drivers/infiniband/core/verbs.c
@@ -479,6 +479,7 @@ static const struct {
 				[IB_QPT_UD]  = (IB_QP_PKEY_INDEX		|
 						IB_QP_PORT			|
 						IB_QP_QKEY),
+				[IB_QPT_RAW_PACKET] = IB_QP_PORT,
 				[IB_QPT_UC]  = (IB_QP_PKEY_INDEX		|
 						IB_QP_PORT			|
 						IB_QP_ACCESS_FLAGS),
@@ -1183,23 +1184,33 @@ EXPORT_SYMBOL(ib_dealloc_fmr);
 
 int ib_attach_mcast(struct ib_qp *qp, union ib_gid *gid, u16 lid)
 {
+	int ret;
+
 	if (!qp->device->attach_mcast)
 		return -ENOSYS;
 	if (gid->raw[0] != 0xff || qp->qp_type != IB_QPT_UD)
 		return -EINVAL;
 
-	return qp->device->attach_mcast(qp, gid, lid);
+	ret = qp->device->attach_mcast(qp, gid, lid);
+	if (!ret)
+		atomic_inc(&qp->usecnt);
+	return ret;
 }
 EXPORT_SYMBOL(ib_attach_mcast);
 
 int ib_detach_mcast(struct ib_qp *qp, union ib_gid *gid, u16 lid)
 {
+	int ret;
+
 	if (!qp->device->detach_mcast)
 		return -ENOSYS;
 	if (gid->raw[0] != 0xff || qp->qp_type != IB_QPT_UD)
 		return -EINVAL;
 
-	return qp->device->detach_mcast(qp, gid, lid);
+	ret = qp->device->detach_mcast(qp, gid, lid);
+	if (!ret)
+		atomic_dec(&qp->usecnt);
+	return ret;
 }
 EXPORT_SYMBOL(ib_detach_mcast);
 
diff --git a/drivers/infiniband/hw/cxgb4/Makefile b/drivers/infiniband/hw/cxgb4/Makefile
index 46b878ca2c3..e11cf729994 100644
--- a/drivers/infiniband/hw/cxgb4/Makefile
+++ b/drivers/infiniband/hw/cxgb4/Makefile
@@ -2,4 +2,4 @@ ccflags-y := -Idrivers/net/ethernet/chelsio/cxgb4
 
 obj-$(CONFIG_INFINIBAND_CXGB4) += iw_cxgb4.o
 
-iw_cxgb4-y :=  device.o cm.o provider.o mem.o cq.o qp.o resource.o ev.o
+iw_cxgb4-y :=  device.o cm.o provider.o mem.o cq.o qp.o resource.o ev.o id_table.o
diff --git a/drivers/infiniband/hw/cxgb4/cm.c b/drivers/infiniband/hw/cxgb4/cm.c
index 92b4c2b0308..55ab284e22f 100644
--- a/drivers/infiniband/hw/cxgb4/cm.c
+++ b/drivers/infiniband/hw/cxgb4/cm.c
@@ -1362,7 +1362,10 @@ static int abort_rpl(struct c4iw_dev *dev, struct sk_buff *skb)
 
 	ep = lookup_tid(t, tid);
 	PDBG("%s ep %p tid %u\n", __func__, ep, ep->hwtid);
-	BUG_ON(!ep);
+	if (!ep) {
+		printk(KERN_WARNING MOD "Abort rpl to freed endpoint\n");
+		return 0;
+	}
 	mutex_lock(&ep->com.mutex);
 	switch (ep->com.state) {
 	case ABORTING:
@@ -1410,6 +1413,24 @@ static int act_open_rpl(struct c4iw_dev *dev, struct sk_buff *skb)
 		return 0;
 	}
 
+	/*
+	 * Log interesting failures.
+	 */
+	switch (status) {
+	case CPL_ERR_CONN_RESET:
+	case CPL_ERR_CONN_TIMEDOUT:
+		break;
+	default:
+		printk(KERN_INFO MOD "Active open failure - "
+		       "atid %u status %u errno %d %pI4:%u->%pI4:%u\n",
+		       atid, status, status2errno(status),
+		       &ep->com.local_addr.sin_addr.s_addr,
+		       ntohs(ep->com.local_addr.sin_port),
+		       &ep->com.remote_addr.sin_addr.s_addr,
+		       ntohs(ep->com.remote_addr.sin_port));
+		break;
+	}
+
 	connect_reply_upcall(ep, status2errno(status));
 	state_set(&ep->com, DEAD);
 
@@ -1593,7 +1614,7 @@ static int import_ep(struct c4iw_ep *ep, __be32 peer_ip, struct dst_entry *dst,
 					n, n->dev, 0);
 		if (!ep->l2t)
 			goto out;
-		ep->mtu = dst_mtu(ep->dst);
+		ep->mtu = dst_mtu(dst);
 		ep->tx_chan = cxgb4_port_chan(n->dev);
 		ep->smac_idx = (cxgb4_port_viid(n->dev) & 0x7F) << 1;
 		step = cdev->rdev.lldi.ntxq /
@@ -2656,6 +2677,12 @@ static int peer_abort_intr(struct c4iw_dev *dev, struct sk_buff *skb)
 	unsigned int tid = GET_TID(req);
 
 	ep = lookup_tid(t, tid);
+	if (!ep) {
+		printk(KERN_WARNING MOD
+		       "Abort on non-existent endpoint, tid %d\n", tid);
+		kfree_skb(skb);
+		return 0;
+	}
 	if (is_neg_adv_abort(req->status)) {
 		PDBG("%s neg_adv_abort ep %p tid %u\n", __func__, ep,
 		     ep->hwtid);
@@ -2667,11 +2694,8 @@ static int peer_abort_intr(struct c4iw_dev *dev, struct sk_buff *skb)
 
 	/*
 	 * Wake up any threads in rdma_init() or rdma_fini().
-	 * However, this is not needed if com state is just
-	 * MPA_REQ_SENT
 	 */
-	if (ep->com.state != MPA_REQ_SENT)
-		c4iw_wake_up(&ep->com.wr_wait, -ECONNRESET);
+	c4iw_wake_up(&ep->com.wr_wait, -ECONNRESET);
 	sched(dev, skb);
 	return 0;
 }
diff --git a/drivers/infiniband/hw/cxgb4/device.c b/drivers/infiniband/hw/cxgb4/device.c
index 6d0df6ec161..cb4ecd78370 100644
--- a/drivers/infiniband/hw/cxgb4/device.c
+++ b/drivers/infiniband/hw/cxgb4/device.c
@@ -32,6 +32,7 @@
 #include <linux/module.h>
 #include <linux/moduleparam.h>
 #include <linux/debugfs.h>
+#include <linux/vmalloc.h>
 
 #include <rdma/ib_verbs.h>
 
@@ -44,6 +45,12 @@ MODULE_DESCRIPTION("Chelsio T4 RDMA Driver");
 MODULE_LICENSE("Dual BSD/GPL");
 MODULE_VERSION(DRV_VERSION);
 
+struct uld_ctx {
+	struct list_head entry;
+	struct cxgb4_lld_info lldi;
+	struct c4iw_dev *dev;
+};
+
 static LIST_HEAD(uld_ctx_list);
 static DEFINE_MUTEX(dev_mutex);
 
@@ -115,7 +122,7 @@ static int qp_release(struct inode *inode, struct file *file)
 		printk(KERN_INFO "%s null qpd?\n", __func__);
 		return 0;
 	}
-	kfree(qpd->buf);
+	vfree(qpd->buf);
 	kfree(qpd);
 	return 0;
 }
@@ -139,7 +146,7 @@ static int qp_open(struct inode *inode, struct file *file)
 	spin_unlock_irq(&qpd->devp->lock);
 
 	qpd->bufsize = count * 128;
-	qpd->buf = kmalloc(qpd->bufsize, GFP_KERNEL);
+	qpd->buf = vmalloc(qpd->bufsize);
 	if (!qpd->buf) {
 		ret = -ENOMEM;
 		goto err1;
@@ -240,6 +247,81 @@ static const struct file_operations stag_debugfs_fops = {
 	.llseek  = default_llseek,
 };
 
+static char *db_state_str[] = {"NORMAL", "FLOW_CONTROL", "RECOVERY"};
+
+static int stats_show(struct seq_file *seq, void *v)
+{
+	struct c4iw_dev *dev = seq->private;
+
+	seq_printf(seq, "   Object: %10s %10s %10s %10s\n", "Total", "Current",
+		   "Max", "Fail");
+	seq_printf(seq, "     PDID: %10llu %10llu %10llu %10llu\n",
+			dev->rdev.stats.pd.total, dev->rdev.stats.pd.cur,
+			dev->rdev.stats.pd.max, dev->rdev.stats.pd.fail);
+	seq_printf(seq, "      QID: %10llu %10llu %10llu %10llu\n",
+			dev->rdev.stats.qid.total, dev->rdev.stats.qid.cur,
+			dev->rdev.stats.qid.max, dev->rdev.stats.qid.fail);
+	seq_printf(seq, "   TPTMEM: %10llu %10llu %10llu %10llu\n",
+			dev->rdev.stats.stag.total, dev->rdev.stats.stag.cur,
+			dev->rdev.stats.stag.max, dev->rdev.stats.stag.fail);
+	seq_printf(seq, "   PBLMEM: %10llu %10llu %10llu %10llu\n",
+			dev->rdev.stats.pbl.total, dev->rdev.stats.pbl.cur,
+			dev->rdev.stats.pbl.max, dev->rdev.stats.pbl.fail);
+	seq_printf(seq, "   RQTMEM: %10llu %10llu %10llu %10llu\n",
+			dev->rdev.stats.rqt.total, dev->rdev.stats.rqt.cur,
+			dev->rdev.stats.rqt.max, dev->rdev.stats.rqt.fail);
+	seq_printf(seq, "  OCQPMEM: %10llu %10llu %10llu %10llu\n",
+			dev->rdev.stats.ocqp.total, dev->rdev.stats.ocqp.cur,
+			dev->rdev.stats.ocqp.max, dev->rdev.stats.ocqp.fail);
+	seq_printf(seq, "  DB FULL: %10llu\n", dev->rdev.stats.db_full);
+	seq_printf(seq, " DB EMPTY: %10llu\n", dev->rdev.stats.db_empty);
+	seq_printf(seq, "  DB DROP: %10llu\n", dev->rdev.stats.db_drop);
+	seq_printf(seq, " DB State: %s Transitions %llu\n",
+		   db_state_str[dev->db_state],
+		   dev->rdev.stats.db_state_transitions);
+	return 0;
+}
+
+static int stats_open(struct inode *inode, struct file *file)
+{
+	return single_open(file, stats_show, inode->i_private);
+}
+
+static ssize_t stats_clear(struct file *file, const char __user *buf,
+		size_t count, loff_t *pos)
+{
+	struct c4iw_dev *dev = ((struct seq_file *)file->private_data)->private;
+
+	mutex_lock(&dev->rdev.stats.lock);
+	dev->rdev.stats.pd.max = 0;
+	dev->rdev.stats.pd.fail = 0;
+	dev->rdev.stats.qid.max = 0;
+	dev->rdev.stats.qid.fail = 0;
+	dev->rdev.stats.stag.max = 0;
+	dev->rdev.stats.stag.fail = 0;
+	dev->rdev.stats.pbl.max = 0;
+	dev->rdev.stats.pbl.fail = 0;
+	dev->rdev.stats.rqt.max = 0;
+	dev->rdev.stats.rqt.fail = 0;
+	dev->rdev.stats.ocqp.max = 0;
+	dev->rdev.stats.ocqp.fail = 0;
+	dev->rdev.stats.db_full = 0;
+	dev->rdev.stats.db_empty = 0;
+	dev->rdev.stats.db_drop = 0;
+	dev->rdev.stats.db_state_transitions = 0;
+	mutex_unlock(&dev->rdev.stats.lock);
+	return count;
+}
+
+static const struct file_operations stats_debugfs_fops = {
+	.owner   = THIS_MODULE,
+	.open    = stats_open,
+	.release = single_release,
+	.read    = seq_read,
+	.llseek  = seq_lseek,
+	.write   = stats_clear,
+};
+
 static int setup_debugfs(struct c4iw_dev *devp)
 {
 	struct dentry *de;
@@ -256,6 +338,12 @@ static int setup_debugfs(struct c4iw_dev *devp)
 				 (void *)devp, &stag_debugfs_fops);
 	if (de && de->d_inode)
 		de->d_inode->i_size = 4096;
+
+	de = debugfs_create_file("stats", S_IWUSR, devp->debugfs_root,
+			(void *)devp, &stats_debugfs_fops);
+	if (de && de->d_inode)
+		de->d_inode->i_size = 4096;
+
 	return 0;
 }
 
@@ -269,9 +357,13 @@ void c4iw_release_dev_ucontext(struct c4iw_rdev *rdev,
 	list_for_each_safe(pos, nxt, &uctx->qpids) {
 		entry = list_entry(pos, struct c4iw_qid_list, entry);
 		list_del_init(&entry->entry);
-		if (!(entry->qid & rdev->qpmask))
-			c4iw_put_resource(&rdev->resource.qid_fifo, entry->qid,
-					  &rdev->resource.qid_fifo_lock);
+		if (!(entry->qid & rdev->qpmask)) {
+			c4iw_put_resource(&rdev->resource.qid_table,
+					  entry->qid);
+			mutex_lock(&rdev->stats.lock);
+			rdev->stats.qid.cur -= rdev->qpmask + 1;
+			mutex_unlock(&rdev->stats.lock);
+		}
 		kfree(entry);
 	}
 
@@ -332,6 +424,13 @@ static int c4iw_rdev_open(struct c4iw_rdev *rdev)
 		goto err1;
 	}
 
+	rdev->stats.pd.total = T4_MAX_NUM_PD;
+	rdev->stats.stag.total = rdev->lldi.vr->stag.size;
+	rdev->stats.pbl.total = rdev->lldi.vr->pbl.size;
+	rdev->stats.rqt.total = rdev->lldi.vr->rq.size;
+	rdev->stats.ocqp.total = rdev->lldi.vr->ocq.size;
+	rdev->stats.qid.total = rdev->lldi.vr->qp.size;
+
 	err = c4iw_init_resource(rdev, c4iw_num_stags(rdev), T4_MAX_NUM_PD);
 	if (err) {
 		printk(KERN_ERR MOD "error %d initializing resources\n", err);
@@ -370,12 +469,6 @@ static void c4iw_rdev_close(struct c4iw_rdev *rdev)
 	c4iw_destroy_resource(&rdev->resource);
 }
 
-struct uld_ctx {
-	struct list_head entry;
-	struct cxgb4_lld_info lldi;
-	struct c4iw_dev *dev;
-};
-
 static void c4iw_dealloc(struct uld_ctx *ctx)
 {
 	c4iw_rdev_close(&ctx->dev->rdev);
@@ -440,6 +533,8 @@ static struct c4iw_dev *c4iw_alloc(const struct cxgb4_lld_info *infop)
 	idr_init(&devp->qpidr);
 	idr_init(&devp->mmidr);
 	spin_lock_init(&devp->lock);
+	mutex_init(&devp->rdev.stats.lock);
+	mutex_init(&devp->db_mutex);
 
 	if (c4iw_debugfs_root) {
 		devp->debugfs_root = debugfs_create_dir(
@@ -585,11 +680,234 @@ static int c4iw_uld_state_change(void *handle, enum cxgb4_state new_state)
 	return 0;
 }
 
+static int disable_qp_db(int id, void *p, void *data)
+{
+	struct c4iw_qp *qp = p;
+
+	t4_disable_wq_db(&qp->wq);
+	return 0;
+}
+
+static void stop_queues(struct uld_ctx *ctx)
+{
+	spin_lock_irq(&ctx->dev->lock);
+	if (ctx->dev->db_state == NORMAL) {
+		ctx->dev->rdev.stats.db_state_transitions++;
+		ctx->dev->db_state = FLOW_CONTROL;
+		idr_for_each(&ctx->dev->qpidr, disable_qp_db, NULL);
+	}
+	spin_unlock_irq(&ctx->dev->lock);
+}
+
+static int enable_qp_db(int id, void *p, void *data)
+{
+	struct c4iw_qp *qp = p;
+
+	t4_enable_wq_db(&qp->wq);
+	return 0;
+}
+
+static void resume_queues(struct uld_ctx *ctx)
+{
+	spin_lock_irq(&ctx->dev->lock);
+	if (ctx->dev->qpcnt <= db_fc_threshold &&
+	    ctx->dev->db_state == FLOW_CONTROL) {
+		ctx->dev->db_state = NORMAL;
+		ctx->dev->rdev.stats.db_state_transitions++;
+		idr_for_each(&ctx->dev->qpidr, enable_qp_db, NULL);
+	}
+	spin_unlock_irq(&ctx->dev->lock);
+}
+
+struct qp_list {
+	unsigned idx;
+	struct c4iw_qp **qps;
+};
+
+static int add_and_ref_qp(int id, void *p, void *data)
+{
+	struct qp_list *qp_listp = data;
+	struct c4iw_qp *qp = p;
+
+	c4iw_qp_add_ref(&qp->ibqp);
+	qp_listp->qps[qp_listp->idx++] = qp;
+	return 0;
+}
+
+static int count_qps(int id, void *p, void *data)
+{
+	unsigned *countp = data;
+	(*countp)++;
+	return 0;
+}
+
+static void deref_qps(struct qp_list qp_list)
+{
+	int idx;
+
+	for (idx = 0; idx < qp_list.idx; idx++)
+		c4iw_qp_rem_ref(&qp_list.qps[idx]->ibqp);
+}
+
+static void recover_lost_dbs(struct uld_ctx *ctx, struct qp_list *qp_list)
+{
+	int idx;
+	int ret;
+
+	for (idx = 0; idx < qp_list->idx; idx++) {
+		struct c4iw_qp *qp = qp_list->qps[idx];
+
+		ret = cxgb4_sync_txq_pidx(qp->rhp->rdev.lldi.ports[0],
+					  qp->wq.sq.qid,
+					  t4_sq_host_wq_pidx(&qp->wq),
+					  t4_sq_wq_size(&qp->wq));
+		if (ret) {
+			printk(KERN_ERR MOD "%s: Fatal error - "
+			       "DB overflow recovery failed - "
+			       "error syncing SQ qid %u\n",
+			       pci_name(ctx->lldi.pdev), qp->wq.sq.qid);
+			return;
+		}
+
+		ret = cxgb4_sync_txq_pidx(qp->rhp->rdev.lldi.ports[0],
+					  qp->wq.rq.qid,
+					  t4_rq_host_wq_pidx(&qp->wq),
+					  t4_rq_wq_size(&qp->wq));
+
+		if (ret) {
+			printk(KERN_ERR MOD "%s: Fatal error - "
+			       "DB overflow recovery failed - "
+			       "error syncing RQ qid %u\n",
+			       pci_name(ctx->lldi.pdev), qp->wq.rq.qid);
+			return;
+		}
+
+		/* Wait for the dbfifo to drain */
+		while (cxgb4_dbfifo_count(qp->rhp->rdev.lldi.ports[0], 1) > 0) {
+			set_current_state(TASK_UNINTERRUPTIBLE);
+			schedule_timeout(usecs_to_jiffies(10));
+		}
+	}
+}
+
+static void recover_queues(struct uld_ctx *ctx)
+{
+	int count = 0;
+	struct qp_list qp_list;
+	int ret;
+
+	/* lock out kernel db ringers */
+	mutex_lock(&ctx->dev->db_mutex);
+
+	/* put all queues in to recovery mode */
+	spin_lock_irq(&ctx->dev->lock);
+	ctx->dev->db_state = RECOVERY;
+	ctx->dev->rdev.stats.db_state_transitions++;
+	idr_for_each(&ctx->dev->qpidr, disable_qp_db, NULL);
+	spin_unlock_irq(&ctx->dev->lock);
+
+	/* slow everybody down */
+	set_current_state(TASK_UNINTERRUPTIBLE);
+	schedule_timeout(usecs_to_jiffies(1000));
+
+	/* Wait for the dbfifo to completely drain. */
+	while (cxgb4_dbfifo_count(ctx->dev->rdev.lldi.ports[0], 1) > 0) {
+		set_current_state(TASK_UNINTERRUPTIBLE);
+		schedule_timeout(usecs_to_jiffies(10));
+	}
+
+	/* flush the SGE contexts */
+	ret = cxgb4_flush_eq_cache(ctx->dev->rdev.lldi.ports[0]);
+	if (ret) {
+		printk(KERN_ERR MOD "%s: Fatal error - DB overflow recovery failed\n",
+		       pci_name(ctx->lldi.pdev));
+		goto out;
+	}
+
+	/* Count active queues so we can build a list of queues to recover */
+	spin_lock_irq(&ctx->dev->lock);
+	idr_for_each(&ctx->dev->qpidr, count_qps, &count);
+
+	qp_list.qps = kzalloc(count * sizeof *qp_list.qps, GFP_ATOMIC);
+	if (!qp_list.qps) {
+		printk(KERN_ERR MOD "%s: Fatal error - DB overflow recovery failed\n",
+		       pci_name(ctx->lldi.pdev));
+		spin_unlock_irq(&ctx->dev->lock);
+		goto out;
+	}
+	qp_list.idx = 0;
+
+	/* add and ref each qp so it doesn't get freed */
+	idr_for_each(&ctx->dev->qpidr, add_and_ref_qp, &qp_list);
+
+	spin_unlock_irq(&ctx->dev->lock);
+
+	/* now traverse the list in a safe context to recover the db state*/
+	recover_lost_dbs(ctx, &qp_list);
+
+	/* we're almost done!  deref the qps and clean up */
+	deref_qps(qp_list);
+	kfree(qp_list.qps);
+
+	/* Wait for the dbfifo to completely drain again */
+	while (cxgb4_dbfifo_count(ctx->dev->rdev.lldi.ports[0], 1) > 0) {
+		set_current_state(TASK_UNINTERRUPTIBLE);
+		schedule_timeout(usecs_to_jiffies(10));
+	}
+
+	/* resume the queues */
+	spin_lock_irq(&ctx->dev->lock);
+	if (ctx->dev->qpcnt > db_fc_threshold)
+		ctx->dev->db_state = FLOW_CONTROL;
+	else {
+		ctx->dev->db_state = NORMAL;
+		idr_for_each(&ctx->dev->qpidr, enable_qp_db, NULL);
+	}
+	ctx->dev->rdev.stats.db_state_transitions++;
+	spin_unlock_irq(&ctx->dev->lock);
+
+out:
+	/* start up kernel db ringers again */
+	mutex_unlock(&ctx->dev->db_mutex);
+}
+
+static int c4iw_uld_control(void *handle, enum cxgb4_control control, ...)
+{
+	struct uld_ctx *ctx = handle;
+
+	switch (control) {
+	case CXGB4_CONTROL_DB_FULL:
+		stop_queues(ctx);
+		mutex_lock(&ctx->dev->rdev.stats.lock);
+		ctx->dev->rdev.stats.db_full++;
+		mutex_unlock(&ctx->dev->rdev.stats.lock);
+		break;
+	case CXGB4_CONTROL_DB_EMPTY:
+		resume_queues(ctx);
+		mutex_lock(&ctx->dev->rdev.stats.lock);
+		ctx->dev->rdev.stats.db_empty++;
+		mutex_unlock(&ctx->dev->rdev.stats.lock);
+		break;
+	case CXGB4_CONTROL_DB_DROP:
+		recover_queues(ctx);
+		mutex_lock(&ctx->dev->rdev.stats.lock);
+		ctx->dev->rdev.stats.db_drop++;
+		mutex_unlock(&ctx->dev->rdev.stats.lock);
+		break;
+	default:
+		printk(KERN_WARNING MOD "%s: unknown control cmd %u\n",
+		       pci_name(ctx->lldi.pdev), control);
+		break;
+	}
+	return 0;
+}
+
 static struct cxgb4_uld_info c4iw_uld_info = {
 	.name = DRV_NAME,
 	.add = c4iw_uld_add,
 	.rx_handler = c4iw_uld_rx_handler,
 	.state_change = c4iw_uld_state_change,
+	.control = c4iw_uld_control,
 };
 
 static int __init c4iw_init_module(void)
diff --git a/drivers/infiniband/hw/cxgb4/ev.c b/drivers/infiniband/hw/cxgb4/ev.c
index 397cb36cf10..cf2f6b47617 100644
--- a/drivers/infiniband/hw/cxgb4/ev.c
+++ b/drivers/infiniband/hw/cxgb4/ev.c
@@ -84,7 +84,7 @@ void c4iw_ev_dispatch(struct c4iw_dev *dev, struct t4_cqe *err_cqe)
 	struct c4iw_qp *qhp;
 	u32 cqid;
 
-	spin_lock(&dev->lock);
+	spin_lock_irq(&dev->lock);
 	qhp = get_qhp(dev, CQE_QPID(err_cqe));
 	if (!qhp) {
 		printk(KERN_ERR MOD "BAD AE qpid 0x%x opcode %d "
@@ -93,7 +93,7 @@ void c4iw_ev_dispatch(struct c4iw_dev *dev, struct t4_cqe *err_cqe)
 		       CQE_OPCODE(err_cqe), CQE_STATUS(err_cqe),
 		       CQE_TYPE(err_cqe), CQE_WRID_HI(err_cqe),
 		       CQE_WRID_LOW(err_cqe));
-		spin_unlock(&dev->lock);
+		spin_unlock_irq(&dev->lock);
 		goto out;
 	}
 
@@ -109,13 +109,13 @@ void c4iw_ev_dispatch(struct c4iw_dev *dev, struct t4_cqe *err_cqe)
 		       CQE_OPCODE(err_cqe), CQE_STATUS(err_cqe),
 		       CQE_TYPE(err_cqe), CQE_WRID_HI(err_cqe),
 		       CQE_WRID_LOW(err_cqe));
-		spin_unlock(&dev->lock);
+		spin_unlock_irq(&dev->lock);
 		goto out;
 	}
 
 	c4iw_qp_add_ref(&qhp->ibqp);
 	atomic_inc(&chp->refcnt);
-	spin_unlock(&dev->lock);
+	spin_unlock_irq(&dev->lock);
 
 	/* Bad incoming write */
 	if (RQ_TYPE(err_cqe) &&
diff --git a/drivers/infiniband/hw/cxgb4/id_table.c b/drivers/infiniband/hw/cxgb4/id_table.c
new file mode 100644
index 00000000000..f95e5df30db
--- /dev/null
+++ b/drivers/infiniband/hw/cxgb4/id_table.c
@@ -0,0 +1,112 @@
+/*
+ * Copyright (c) 2011 Chelsio Communications.  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/kernel.h>
+#include <linux/random.h>
+#include "iw_cxgb4.h"
+
+#define RANDOM_SKIP 16
+
+/*
+ * Trivial bitmap-based allocator. If the random flag is set, the
+ * allocator is designed to:
+ * - pseudo-randomize the id returned such that it is not trivially predictable.
+ * - avoid reuse of recently used id (at the expense of predictability)
+ */
+u32 c4iw_id_alloc(struct c4iw_id_table *alloc)
+{
+	unsigned long flags;
+	u32 obj;
+
+	spin_lock_irqsave(&alloc->lock, flags);
+
+	obj = find_next_zero_bit(alloc->table, alloc->max, alloc->last);
+	if (obj >= alloc->max)
+		obj = find_first_zero_bit(alloc->table, alloc->max);
+
+	if (obj < alloc->max) {
+		if (alloc->flags & C4IW_ID_TABLE_F_RANDOM)
+			alloc->last += random32() % RANDOM_SKIP;
+		else
+			alloc->last = obj + 1;
+		if (alloc->last >= alloc->max)
+			alloc->last = 0;
+		set_bit(obj, alloc->table);
+		obj += alloc->start;
+	} else
+		obj = -1;
+
+	spin_unlock_irqrestore(&alloc->lock, flags);
+	return obj;
+}
+
+void c4iw_id_free(struct c4iw_id_table *alloc, u32 obj)
+{
+	unsigned long flags;
+
+	obj -= alloc->start;
+	BUG_ON((int)obj < 0);
+
+	spin_lock_irqsave(&alloc->lock, flags);
+	clear_bit(obj, alloc->table);
+	spin_unlock_irqrestore(&alloc->lock, flags);
+}
+
+int c4iw_id_table_alloc(struct c4iw_id_table *alloc, u32 start, u32 num,
+			u32 reserved, u32 flags)
+{
+	int i;
+
+	alloc->start = start;
+	alloc->flags = flags;
+	if (flags & C4IW_ID_TABLE_F_RANDOM)
+		alloc->last = random32() % RANDOM_SKIP;
+	else
+		alloc->last = 0;
+	alloc->max  = num;
+	spin_lock_init(&alloc->lock);
+	alloc->table = kmalloc(BITS_TO_LONGS(num) * sizeof(long),
+				GFP_KERNEL);
+	if (!alloc->table)
+		return -ENOMEM;
+
+	bitmap_zero(alloc->table, num);
+	if (!(alloc->flags & C4IW_ID_TABLE_F_EMPTY))
+		for (i = 0; i < reserved; ++i)
+			set_bit(i, alloc->table);
+
+	return 0;
+}
+
+void c4iw_id_table_free(struct c4iw_id_table *alloc)
+{
+	kfree(alloc->table);
+}
diff --git a/drivers/infiniband/hw/cxgb4/iw_cxgb4.h b/drivers/infiniband/hw/cxgb4/iw_cxgb4.h
index 1357c5bf209..9beb3a9f033 100644
--- a/drivers/infiniband/hw/cxgb4/iw_cxgb4.h
+++ b/drivers/infiniband/hw/cxgb4/iw_cxgb4.h
@@ -45,7 +45,6 @@
 #include <linux/kref.h>
 #include <linux/timer.h>
 #include <linux/io.h>
-#include <linux/kfifo.h>
 
 #include <asm/byteorder.h>
 
@@ -79,13 +78,22 @@ static inline void *cplhdr(struct sk_buff *skb)
 	return skb->data;
 }
 
+#define C4IW_ID_TABLE_F_RANDOM 1       /* Pseudo-randomize the id's returned */
+#define C4IW_ID_TABLE_F_EMPTY  2       /* Table is initially empty */
+
+struct c4iw_id_table {
+	u32 flags;
+	u32 start;              /* logical minimal id */
+	u32 last;               /* hint for find */
+	u32 max;
+	spinlock_t lock;
+	unsigned long *table;
+};
+
 struct c4iw_resource {
-	struct kfifo tpt_fifo;
-	spinlock_t tpt_fifo_lock;
-	struct kfifo qid_fifo;
-	spinlock_t qid_fifo_lock;
-	struct kfifo pdid_fifo;
-	spinlock_t pdid_fifo_lock;
+	struct c4iw_id_table tpt_table;
+	struct c4iw_id_table qid_table;
+	struct c4iw_id_table pdid_table;
 };
 
 struct c4iw_qid_list {
@@ -103,6 +111,27 @@ enum c4iw_rdev_flags {
 	T4_FATAL_ERROR = (1<<0),
 };
 
+struct c4iw_stat {
+	u64 total;
+	u64 cur;
+	u64 max;
+	u64 fail;
+};
+
+struct c4iw_stats {
+	struct mutex lock;
+	struct c4iw_stat qid;
+	struct c4iw_stat pd;
+	struct c4iw_stat stag;
+	struct c4iw_stat pbl;
+	struct c4iw_stat rqt;
+	struct c4iw_stat ocqp;
+	u64  db_full;
+	u64  db_empty;
+	u64  db_drop;
+	u64  db_state_transitions;
+};
+
 struct c4iw_rdev {
 	struct c4iw_resource resource;
 	unsigned long qpshift;
@@ -117,6 +146,7 @@ struct c4iw_rdev {
 	struct cxgb4_lld_info lldi;
 	unsigned long oc_mw_pa;
 	void __iomem *oc_mw_kva;
+	struct c4iw_stats stats;
 };
 
 static inline int c4iw_fatal_error(struct c4iw_rdev *rdev)
@@ -175,6 +205,12 @@ static inline int c4iw_wait_for_reply(struct c4iw_rdev *rdev,
 	return wr_waitp->ret;
 }
 
+enum db_state {
+	NORMAL = 0,
+	FLOW_CONTROL = 1,
+	RECOVERY = 2
+};
+
 struct c4iw_dev {
 	struct ib_device ibdev;
 	struct c4iw_rdev rdev;
@@ -183,7 +219,10 @@ struct c4iw_dev {
 	struct idr qpidr;
 	struct idr mmidr;
 	spinlock_t lock;
+	struct mutex db_mutex;
 	struct dentry *debugfs_root;
+	enum db_state db_state;
+	int qpcnt;
 };
 
 static inline struct c4iw_dev *to_c4iw_dev(struct ib_device *ibdev)
@@ -211,29 +250,57 @@ static inline struct c4iw_mr *get_mhp(struct c4iw_dev *rhp, u32 mmid)
 	return idr_find(&rhp->mmidr, mmid);
 }
 
-static inline int insert_handle(struct c4iw_dev *rhp, struct idr *idr,
-				void *handle, u32 id)
+static inline int _insert_handle(struct c4iw_dev *rhp, struct idr *idr,
+				 void *handle, u32 id, int lock)
 {
 	int ret;
 	int newid;
 
 	do {
-		if (!idr_pre_get(idr, GFP_KERNEL))
+		if (!idr_pre_get(idr, lock ? GFP_KERNEL : GFP_ATOMIC))
 			return -ENOMEM;
-		spin_lock_irq(&rhp->lock);
+		if (lock)
+			spin_lock_irq(&rhp->lock);
 		ret = idr_get_new_above(idr, handle, id, &newid);
-		BUG_ON(newid != id);
-		spin_unlock_irq(&rhp->lock);
+		BUG_ON(!ret && newid != id);
+		if (lock)
+			spin_unlock_irq(&rhp->lock);
 	} while (ret == -EAGAIN);
 
 	return ret;
 }
 
-static inline void remove_handle(struct c4iw_dev *rhp, struct idr *idr, u32 id)
+static inline int insert_handle(struct c4iw_dev *rhp, struct idr *idr,
+				void *handle, u32 id)
+{
+	return _insert_handle(rhp, idr, handle, id, 1);
+}
+
+static inline int insert_handle_nolock(struct c4iw_dev *rhp, struct idr *idr,
+				       void *handle, u32 id)
+{
+	return _insert_handle(rhp, idr, handle, id, 0);
+}
+
+static inline void _remove_handle(struct c4iw_dev *rhp, struct idr *idr,
+				   u32 id, int lock)
 {
-	spin_lock_irq(&rhp->lock);
+	if (lock)
+		spin_lock_irq(&rhp->lock);
 	idr_remove(idr, id);
-	spin_unlock_irq(&rhp->lock);
+	if (lock)
+		spin_unlock_irq(&rhp->lock);
+}
+
+static inline void remove_handle(struct c4iw_dev *rhp, struct idr *idr, u32 id)
+{
+	_remove_handle(rhp, idr, id, 1);
+}
+
+static inline void remove_handle_nolock(struct c4iw_dev *rhp,
+					 struct idr *idr, u32 id)
+{
+	_remove_handle(rhp, idr, id, 0);
 }
 
 struct c4iw_pd {
@@ -353,6 +420,8 @@ struct c4iw_qp_attributes {
 	struct c4iw_ep *llp_stream_handle;
 	u8 layer_etype;
 	u8 ecode;
+	u16 sq_db_inc;
+	u16 rq_db_inc;
 };
 
 struct c4iw_qp {
@@ -427,6 +496,8 @@ static inline void insert_mmap(struct c4iw_ucontext *ucontext,
 
 enum c4iw_qp_attr_mask {
 	C4IW_QP_ATTR_NEXT_STATE = 1 << 0,
+	C4IW_QP_ATTR_SQ_DB = 1<<1,
+	C4IW_QP_ATTR_RQ_DB = 1<<2,
 	C4IW_QP_ATTR_ENABLE_RDMA_READ = 1 << 7,
 	C4IW_QP_ATTR_ENABLE_RDMA_WRITE = 1 << 8,
 	C4IW_QP_ATTR_ENABLE_RDMA_BIND = 1 << 9,
@@ -480,6 +551,23 @@ static inline int c4iw_convert_state(enum ib_qp_state ib_state)
 	}
 }
 
+static inline int to_ib_qp_state(int c4iw_qp_state)
+{
+	switch (c4iw_qp_state) {
+	case C4IW_QP_STATE_IDLE:
+		return IB_QPS_INIT;
+	case C4IW_QP_STATE_RTS:
+		return IB_QPS_RTS;
+	case C4IW_QP_STATE_CLOSING:
+		return IB_QPS_SQD;
+	case C4IW_QP_STATE_TERMINATE:
+		return IB_QPS_SQE;
+	case C4IW_QP_STATE_ERROR:
+		return IB_QPS_ERR;
+	}
+	return IB_QPS_ERR;
+}
+
 static inline u32 c4iw_ib_to_tpt_access(int a)
 {
 	return (a & IB_ACCESS_REMOTE_WRITE ? FW_RI_MEM_ACCESS_REM_WRITE : 0) |
@@ -693,14 +781,20 @@ static inline int compute_wscale(int win)
 	return wscale;
 }
 
+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,
+			u32 reserved, u32 flags);
+void c4iw_id_table_free(struct c4iw_id_table *alloc);
+
 typedef int (*c4iw_handler_func)(struct c4iw_dev *dev, struct sk_buff *skb);
 
 int c4iw_ep_redirect(void *ctx, struct dst_entry *old, struct dst_entry *new,
 		     struct l2t_entry *l2t);
 void c4iw_put_qpid(struct c4iw_rdev *rdev, u32 qpid,
 		   struct c4iw_dev_ucontext *uctx);
-u32 c4iw_get_resource(struct kfifo *fifo, spinlock_t *lock);
-void c4iw_put_resource(struct kfifo *fifo, u32 entry, spinlock_t *lock);
+u32 c4iw_get_resource(struct c4iw_id_table *id_table);
+void c4iw_put_resource(struct c4iw_id_table *id_table, u32 entry);
 int c4iw_init_resource(struct c4iw_rdev *rdev, u32 nr_tpt, u32 nr_pdid);
 int c4iw_init_ctrl_qp(struct c4iw_rdev *rdev);
 int c4iw_pblpool_create(struct c4iw_rdev *rdev);
@@ -769,6 +863,8 @@ struct ib_qp *c4iw_create_qp(struct ib_pd *pd,
 			     struct ib_udata *udata);
 int c4iw_ib_modify_qp(struct ib_qp *ibqp, struct ib_qp_attr *attr,
 				 int attr_mask, struct ib_udata *udata);
+int c4iw_ib_query_qp(struct ib_qp *ibqp, struct ib_qp_attr *attr,
+		     int attr_mask, struct ib_qp_init_attr *init_attr);
 struct ib_qp *c4iw_get_qp(struct ib_device *dev, int qpn);
 u32 c4iw_rqtpool_alloc(struct c4iw_rdev *rdev, int size);
 void c4iw_rqtpool_free(struct c4iw_rdev *rdev, u32 addr, int size);
@@ -797,5 +893,7 @@ void c4iw_ev_dispatch(struct c4iw_dev *dev, struct t4_cqe *err_cqe);
 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;
+
 
 #endif
diff --git a/drivers/infiniband/hw/cxgb4/mem.c b/drivers/infiniband/hw/cxgb4/mem.c
index 40c835309e4..57e07c61ace 100644
--- a/drivers/infiniband/hw/cxgb4/mem.c
+++ b/drivers/infiniband/hw/cxgb4/mem.c
@@ -131,10 +131,14 @@ static int write_tpt_entry(struct c4iw_rdev *rdev, u32 reset_tpt_entry,
 	stag_idx = (*stag) >> 8;
 
 	if ((!reset_tpt_entry) && (*stag == T4_STAG_UNSET)) {
-		stag_idx = c4iw_get_resource(&rdev->resource.tpt_fifo,
-					     &rdev->resource.tpt_fifo_lock);
+		stag_idx = c4iw_get_resource(&rdev->resource.tpt_table);
 		if (!stag_idx)
 			return -ENOMEM;
+		mutex_lock(&rdev->stats.lock);
+		rdev->stats.stag.cur += 32;
+		if (rdev->stats.stag.cur > rdev->stats.stag.max)
+			rdev->stats.stag.max = rdev->stats.stag.cur;
+		mutex_unlock(&rdev->stats.lock);
 		*stag = (stag_idx << 8) | (atomic_inc_return(&key) & 0xff);
 	}
 	PDBG("%s stag_state 0x%0x type 0x%0x pdid 0x%0x, stag_idx 0x%x\n",
@@ -165,9 +169,12 @@ static int write_tpt_entry(struct c4iw_rdev *rdev, u32 reset_tpt_entry,
 				(rdev->lldi.vr->stag.start >> 5),
 				sizeof(tpt), &tpt);
 
-	if (reset_tpt_entry)
-		c4iw_put_resource(&rdev->resource.tpt_fifo, stag_idx,
-				  &rdev->resource.tpt_fifo_lock);
+	if (reset_tpt_entry) {
+		c4iw_put_resource(&rdev->resource.tpt_table, stag_idx);
+		mutex_lock(&rdev->stats.lock);
+		rdev->stats.stag.cur -= 32;
+		mutex_unlock(&rdev->stats.lock);
+	}
 	return err;
 }
 
@@ -686,8 +693,8 @@ int c4iw_dealloc_mw(struct ib_mw *mw)
 	mhp = to_c4iw_mw(mw);
 	rhp = mhp->rhp;
 	mmid = (mw->rkey) >> 8;
-	deallocate_window(&rhp->rdev, mhp->attr.stag);
 	remove_handle(rhp, &rhp->mmidr, mmid);
+	deallocate_window(&rhp->rdev, mhp->attr.stag);
 	kfree(mhp);
 	PDBG("%s ib_mw %p mmid 0x%x ptr %p\n", __func__, mw, mmid, mhp);
 	return 0;
@@ -789,12 +796,12 @@ int c4iw_dereg_mr(struct ib_mr *ib_mr)
 	mhp = to_c4iw_mr(ib_mr);
 	rhp = mhp->rhp;
 	mmid = mhp->attr.stag >> 8;
+	remove_handle(rhp, &rhp->mmidr, mmid);
 	dereg_mem(&rhp->rdev, mhp->attr.stag, mhp->attr.pbl_size,
 		       mhp->attr.pbl_addr);
 	if (mhp->attr.pbl_size)
 		c4iw_pblpool_free(&mhp->rhp->rdev, mhp->attr.pbl_addr,
 				  mhp->attr.pbl_size << 3);
-	remove_handle(rhp, &rhp->mmidr, mmid);
 	if (mhp->kva)
 		kfree((void *) (unsigned long) mhp->kva);
 	if (mhp->umem)
diff --git a/drivers/infiniband/hw/cxgb4/provider.c b/drivers/infiniband/hw/cxgb4/provider.c
index be1c18f4440..e084fdc6da7 100644
--- a/drivers/infiniband/hw/cxgb4/provider.c
+++ b/drivers/infiniband/hw/cxgb4/provider.c
@@ -188,8 +188,10 @@ static int c4iw_deallocate_pd(struct ib_pd *pd)
 	php = to_c4iw_pd(pd);
 	rhp = php->rhp;
 	PDBG("%s ibpd %p pdid 0x%x\n", __func__, pd, php->pdid);
-	c4iw_put_resource(&rhp->rdev.resource.pdid_fifo, php->pdid,
-			  &rhp->rdev.resource.pdid_fifo_lock);
+	c4iw_put_resource(&rhp->rdev.resource.pdid_table, php->pdid);
+	mutex_lock(&rhp->rdev.stats.lock);
+	rhp->rdev.stats.pd.cur--;
+	mutex_unlock(&rhp->rdev.stats.lock);
 	kfree(php);
 	return 0;
 }
@@ -204,14 +206,12 @@ static struct ib_pd *c4iw_allocate_pd(struct ib_device *ibdev,
 
 	PDBG("%s ibdev %p\n", __func__, ibdev);
 	rhp = (struct c4iw_dev *) ibdev;
-	pdid =  c4iw_get_resource(&rhp->rdev.resource.pdid_fifo,
-				  &rhp->rdev.resource.pdid_fifo_lock);
+	pdid =  c4iw_get_resource(&rhp->rdev.resource.pdid_table);
 	if (!pdid)
 		return ERR_PTR(-EINVAL);
 	php = kzalloc(sizeof(*php), GFP_KERNEL);
 	if (!php) {
-		c4iw_put_resource(&rhp->rdev.resource.pdid_fifo, pdid,
-				  &rhp->rdev.resource.pdid_fifo_lock);
+		c4iw_put_resource(&rhp->rdev.resource.pdid_table, pdid);
 		return ERR_PTR(-ENOMEM);
 	}
 	php->pdid = pdid;
@@ -222,6 +222,11 @@ static struct ib_pd *c4iw_allocate_pd(struct ib_device *ibdev,
 			return ERR_PTR(-EFAULT);
 		}
 	}
+	mutex_lock(&rhp->rdev.stats.lock);
+	rhp->rdev.stats.pd.cur++;
+	if (rhp->rdev.stats.pd.cur > rhp->rdev.stats.pd.max)
+		rhp->rdev.stats.pd.max = rhp->rdev.stats.pd.cur;
+	mutex_unlock(&rhp->rdev.stats.lock);
 	PDBG("%s pdid 0x%0x ptr 0x%p\n", __func__, pdid, php);
 	return &php->ibpd;
 }
@@ -438,6 +443,7 @@ int c4iw_register_device(struct c4iw_dev *dev)
 	    (1ull << IB_USER_VERBS_CMD_REQ_NOTIFY_CQ) |
 	    (1ull << IB_USER_VERBS_CMD_CREATE_QP) |
 	    (1ull << IB_USER_VERBS_CMD_MODIFY_QP) |
+	    (1ull << IB_USER_VERBS_CMD_QUERY_QP) |
 	    (1ull << IB_USER_VERBS_CMD_POLL_CQ) |
 	    (1ull << IB_USER_VERBS_CMD_DESTROY_QP) |
 	    (1ull << IB_USER_VERBS_CMD_POST_SEND) |
@@ -460,6 +466,7 @@ int c4iw_register_device(struct c4iw_dev *dev)
 	dev->ibdev.destroy_ah = c4iw_ah_destroy;
 	dev->ibdev.create_qp = c4iw_create_qp;
 	dev->ibdev.modify_qp = c4iw_ib_modify_qp;
+	dev->ibdev.query_qp = c4iw_ib_query_qp;
 	dev->ibdev.destroy_qp = c4iw_destroy_qp;
 	dev->ibdev.create_cq = c4iw_create_cq;
 	dev->ibdev.destroy_cq = c4iw_destroy_cq;
diff --git a/drivers/infiniband/hw/cxgb4/qp.c b/drivers/infiniband/hw/cxgb4/qp.c
index 5f940aeaab1..45aedf1d933 100644
--- a/drivers/infiniband/hw/cxgb4/qp.c
+++ b/drivers/infiniband/hw/cxgb4/qp.c
@@ -34,10 +34,19 @@
 
 #include "iw_cxgb4.h"
 
+static int db_delay_usecs = 1;
+module_param(db_delay_usecs, int, 0644);
+MODULE_PARM_DESC(db_delay_usecs, "Usecs to delay awaiting db fifo to drain");
+
 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;
+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)");
+
 static void set_state(struct c4iw_qp *qhp, enum c4iw_qp_state state)
 {
 	unsigned long flag;
@@ -1128,6 +1137,35 @@ out:
 	return ret;
 }
 
+/*
+ * Called by the library when the qp has user dbs disabled due to
+ * a DB_FULL condition.  This function will single-thread all user
+ * DB rings to avoid overflowing the hw db-fifo.
+ */
+static int ring_kernel_db(struct c4iw_qp *qhp, u32 qid, u16 inc)
+{
+	int delay = db_delay_usecs;
+
+	mutex_lock(&qhp->rhp->db_mutex);
+	do {
+
+		/*
+		 * The interrupt threshold is dbfifo_int_thresh << 6. So
+		 * make sure we don't cross that and generate an interrupt.
+		 */
+		if (cxgb4_dbfifo_count(qhp->rhp->rdev.lldi.ports[0], 1) <
+		    (qhp->rhp->rdev.lldi.dbfifo_int_thresh << 5)) {
+			writel(V_QID(qid) | V_PIDX(inc), qhp->wq.db);
+			break;
+		}
+		set_current_state(TASK_UNINTERRUPTIBLE);
+		schedule_timeout(usecs_to_jiffies(delay));
+		delay = min(delay << 1, 2000);
+	} while (1);
+	mutex_unlock(&qhp->rhp->db_mutex);
+	return 0;
+}
+
 int c4iw_modify_qp(struct c4iw_dev *rhp, struct c4iw_qp *qhp,
 		   enum c4iw_qp_attr_mask mask,
 		   struct c4iw_qp_attributes *attrs,
@@ -1176,6 +1214,15 @@ int c4iw_modify_qp(struct c4iw_dev *rhp, struct c4iw_qp *qhp,
 		qhp->attr = newattr;
 	}
 
+	if (mask & C4IW_QP_ATTR_SQ_DB) {
+		ret = ring_kernel_db(qhp, qhp->wq.sq.qid, attrs->sq_db_inc);
+		goto out;
+	}
+	if (mask & C4IW_QP_ATTR_RQ_DB) {
+		ret = ring_kernel_db(qhp, qhp->wq.rq.qid, attrs->rq_db_inc);
+		goto out;
+	}
+
 	if (!(mask & C4IW_QP_ATTR_NEXT_STATE))
 		goto out;
 	if (qhp->attr.state == attrs->next_state)
@@ -1352,6 +1399,14 @@ out:
 	return ret;
 }
 
+static int enable_qp_db(int id, void *p, void *data)
+{
+	struct c4iw_qp *qp = p;
+
+	t4_enable_wq_db(&qp->wq);
+	return 0;
+}
+
 int c4iw_destroy_qp(struct ib_qp *ib_qp)
 {
 	struct c4iw_dev *rhp;
@@ -1369,7 +1424,16 @@ int c4iw_destroy_qp(struct ib_qp *ib_qp)
 		c4iw_modify_qp(rhp, qhp, C4IW_QP_ATTR_NEXT_STATE, &attrs, 0);
 	wait_event(qhp->wait, !qhp->ep);
 
-	remove_handle(rhp, &rhp->qpidr, qhp->wq.sq.qid);
+	spin_lock_irq(&rhp->lock);
+	remove_handle_nolock(rhp, &rhp->qpidr, qhp->wq.sq.qid);
+	rhp->qpcnt--;
+	BUG_ON(rhp->qpcnt < 0);
+	if (rhp->qpcnt <= db_fc_threshold && rhp->db_state == FLOW_CONTROL) {
+		rhp->rdev.stats.db_state_transitions++;
+		rhp->db_state = NORMAL;
+		idr_for_each(&rhp->qpidr, enable_qp_db, NULL);
+	}
+	spin_unlock_irq(&rhp->lock);
 	atomic_dec(&qhp->refcnt);
 	wait_event(qhp->wait, !atomic_read(&qhp->refcnt));
 
@@ -1383,6 +1447,14 @@ int c4iw_destroy_qp(struct ib_qp *ib_qp)
 	return 0;
 }
 
+static int disable_qp_db(int id, void *p, void *data)
+{
+	struct c4iw_qp *qp = p;
+
+	t4_disable_wq_db(&qp->wq);
+	return 0;
+}
+
 struct ib_qp *c4iw_create_qp(struct ib_pd *pd, struct ib_qp_init_attr *attrs,
 			     struct ib_udata *udata)
 {
@@ -1469,7 +1541,16 @@ struct ib_qp *c4iw_create_qp(struct ib_pd *pd, struct ib_qp_init_attr *attrs,
 	init_waitqueue_head(&qhp->wait);
 	atomic_set(&qhp->refcnt, 1);
 
-	ret = insert_handle(rhp, &rhp->qpidr, qhp, qhp->wq.sq.qid);
+	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->rdev.stats.db_state_transitions++;
+		rhp->db_state = FLOW_CONTROL;
+		idr_for_each(&rhp->qpidr, disable_qp_db, NULL);
+	}
+	ret = insert_handle_nolock(rhp, &rhp->qpidr, qhp, qhp->wq.sq.qid);
+	spin_unlock_irq(&rhp->lock);
 	if (ret)
 		goto err2;
 
@@ -1613,6 +1694,15 @@ int c4iw_ib_modify_qp(struct ib_qp *ibqp, struct ib_qp_attr *attr,
 			 C4IW_QP_ATTR_ENABLE_RDMA_WRITE |
 			 C4IW_QP_ATTR_ENABLE_RDMA_BIND) : 0;
 
+	/*
+	 * Use SQ_PSN and RQ_PSN to pass in IDX_INC values for
+	 * ringing the queue db when we're in DB_FULL mode.
+	 */
+	attrs.sq_db_inc = attr->sq_psn;
+	attrs.rq_db_inc = attr->rq_psn;
+	mask |= (attr_mask & IB_QP_SQ_PSN) ? C4IW_QP_ATTR_SQ_DB : 0;
+	mask |= (attr_mask & IB_QP_RQ_PSN) ? C4IW_QP_ATTR_RQ_DB : 0;
+
 	return c4iw_modify_qp(rhp, qhp, mask, &attrs, 0);
 }
 
@@ -1621,3 +1711,14 @@ struct ib_qp *c4iw_get_qp(struct ib_device *dev, int qpn)
 	PDBG("%s ib_dev %p qpn 0x%x\n", __func__, dev, qpn);
 	return (struct ib_qp *)get_qhp(to_c4iw_dev(dev), qpn);
 }
+
+int c4iw_ib_query_qp(struct ib_qp *ibqp, struct ib_qp_attr *attr,
+		     int attr_mask, struct ib_qp_init_attr *init_attr)
+{
+	struct c4iw_qp *qhp = to_c4iw_qp(ibqp);
+
+	memset(attr, 0, sizeof *attr);
+	memset(init_attr, 0, sizeof *init_attr);
+	attr->qp_state = to_ib_qp_state(qhp->attr.state);
+	return 0;
+}
diff --git a/drivers/infiniband/hw/cxgb4/resource.c b/drivers/infiniband/hw/cxgb4/resource.c
index 407ff392415..cdef4d7fb6d 100644
--- a/drivers/infiniband/hw/cxgb4/resource.c
+++ b/drivers/infiniband/hw/cxgb4/resource.c
@@ -30,96 +30,25 @@
  * SOFTWARE.
  */
 /* Crude resource management */
-#include <linux/kernel.h>
-#include <linux/random.h>
-#include <linux/slab.h>
-#include <linux/kfifo.h>
 #include <linux/spinlock.h>
-#include <linux/errno.h>
 #include <linux/genalloc.h>
 #include <linux/ratelimit.h>
 #include "iw_cxgb4.h"
 
-#define RANDOM_SIZE 16
-
-static int __c4iw_init_resource_fifo(struct kfifo *fifo,
-				   spinlock_t *fifo_lock,
-				   u32 nr, u32 skip_low,
-				   u32 skip_high,
-				   int random)
-{
-	u32 i, j, entry = 0, idx;
-	u32 random_bytes;
-	u32 rarray[16];
-	spin_lock_init(fifo_lock);
-
-	if (kfifo_alloc(fifo, nr * sizeof(u32), GFP_KERNEL))
-		return -ENOMEM;
-
-	for (i = 0; i < skip_low + skip_high; i++)
-		kfifo_in(fifo, (unsigned char *) &entry, sizeof(u32));
-	if (random) {
-		j = 0;
-		random_bytes = random32();
-		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();
-			}
-			idx = (random_bytes >> (j * 2)) & 0xF;
-			kfifo_in(fifo,
-				(unsigned char *) &rarray[idx],
-				sizeof(u32));
-			rarray[idx] = i;
-			j++;
-		}
-		for (i = 0; i < RANDOM_SIZE; i++)
-			kfifo_in(fifo,
-				(unsigned char *) &rarray[i],
-				sizeof(u32));
-	} else
-		for (i = skip_low; i < nr - skip_high; i++)
-			kfifo_in(fifo, (unsigned char *) &i, sizeof(u32));
-
-	for (i = 0; i < skip_low + skip_high; i++)
-		if (kfifo_out_locked(fifo, (unsigned char *) &entry,
-				     sizeof(u32), fifo_lock))
-			break;
-	return 0;
-}
-
-static int c4iw_init_resource_fifo(struct kfifo *fifo, spinlock_t * fifo_lock,
-				   u32 nr, u32 skip_low, u32 skip_high)
-{
-	return __c4iw_init_resource_fifo(fifo, fifo_lock, nr, skip_low,
-					  skip_high, 0);
-}
-
-static int c4iw_init_resource_fifo_random(struct kfifo *fifo,
-				   spinlock_t *fifo_lock,
-				   u32 nr, u32 skip_low, u32 skip_high)
-{
-	return __c4iw_init_resource_fifo(fifo, fifo_lock, nr, skip_low,
-					  skip_high, 1);
-}
-
-static int c4iw_init_qid_fifo(struct c4iw_rdev *rdev)
+static int c4iw_init_qid_table(struct c4iw_rdev *rdev)
 {
 	u32 i;
 
-	spin_lock_init(&rdev->resource.qid_fifo_lock);
-
-	if (kfifo_alloc(&rdev->resource.qid_fifo, rdev->lldi.vr->qp.size *
-			sizeof(u32), GFP_KERNEL))
+	if (c4iw_id_table_alloc(&rdev->resource.qid_table,
+				rdev->lldi.vr->qp.start,
+				rdev->lldi.vr->qp.size,
+				rdev->lldi.vr->qp.size, 0))
 		return -ENOMEM;
 
 	for (i = rdev->lldi.vr->qp.start;
-	     i < rdev->lldi.vr->qp.start + rdev->lldi.vr->qp.size; i++)
+		i < rdev->lldi.vr->qp.start + rdev->lldi.vr->qp.size; i++)
 		if (!(i & rdev->qpmask))
-			kfifo_in(&rdev->resource.qid_fifo,
-				    (unsigned char *) &i, sizeof(u32));
+			c4iw_id_free(&rdev->resource.qid_table, i);
 	return 0;
 }
 
@@ -127,44 +56,42 @@ static int c4iw_init_qid_fifo(struct c4iw_rdev *rdev)
 int c4iw_init_resource(struct c4iw_rdev *rdev, u32 nr_tpt, u32 nr_pdid)
 {
 	int err = 0;
-	err = c4iw_init_resource_fifo_random(&rdev->resource.tpt_fifo,
-					     &rdev->resource.tpt_fifo_lock,
-					     nr_tpt, 1, 0);
+	err = c4iw_id_table_alloc(&rdev->resource.tpt_table, 0, nr_tpt, 1,
+					C4IW_ID_TABLE_F_RANDOM);
 	if (err)
 		goto tpt_err;
-	err = c4iw_init_qid_fifo(rdev);
+	err = c4iw_init_qid_table(rdev);
 	if (err)
 		goto qid_err;
-	err = c4iw_init_resource_fifo(&rdev->resource.pdid_fifo,
-				      &rdev->resource.pdid_fifo_lock,
-				      nr_pdid, 1, 0);
+	err = c4iw_id_table_alloc(&rdev->resource.pdid_table, 0,
+					nr_pdid, 1, 0);
 	if (err)
 		goto pdid_err;
 	return 0;
-pdid_err:
-	kfifo_free(&rdev->resource.qid_fifo);
-qid_err:
-	kfifo_free(&rdev->resource.tpt_fifo);
-tpt_err:
+ pdid_err:
+	c4iw_id_table_free(&rdev->resource.qid_table);
+ qid_err:
+	c4iw_id_table_free(&rdev->resource.tpt_table);
+ tpt_err:
 	return -ENOMEM;
 }
 
 /*
  * returns 0 if no resource available
  */
-u32 c4iw_get_resource(struct kfifo *fifo, spinlock_t *lock)
+u32 c4iw_get_resource(struct c4iw_id_table *id_table)
 {
 	u32 entry;
-	if (kfifo_out_locked(fifo, (unsigned char *) &entry, sizeof(u32), lock))
-		return entry;
-	else
+	entry = c4iw_id_alloc(id_table);
+	if (entry == (u32)(-1))
 		return 0;
+	return entry;
 }
 
-void c4iw_put_resource(struct kfifo *fifo, u32 entry, spinlock_t *lock)
+void c4iw_put_resource(struct c4iw_id_table *id_table, u32 entry)
 {
 	PDBG("%s entry 0x%x\n", __func__, entry);
-	kfifo_in_locked(fifo, (unsigned char *) &entry, sizeof(u32), lock);
+	c4iw_id_free(id_table, entry);
 }
 
 u32 c4iw_get_cqid(struct c4iw_rdev *rdev, struct c4iw_dev_ucontext *uctx)
@@ -181,10 +108,12 @@ u32 c4iw_get_cqid(struct c4iw_rdev *rdev, struct c4iw_dev_ucontext *uctx)
 		qid = entry->qid;
 		kfree(entry);
 	} else {
-		qid = c4iw_get_resource(&rdev->resource.qid_fifo,
-					&rdev->resource.qid_fifo_lock);
+		qid = c4iw_get_resource(&rdev->resource.qid_table);
 		if (!qid)
 			goto out;
+		mutex_lock(&rdev->stats.lock);
+		rdev->stats.qid.cur += rdev->qpmask + 1;
+		mutex_unlock(&rdev->stats.lock);
 		for (i = qid+1; i & rdev->qpmask; i++) {
 			entry = kmalloc(sizeof *entry, GFP_KERNEL);
 			if (!entry)
@@ -213,6 +142,10 @@ u32 c4iw_get_cqid(struct c4iw_rdev *rdev, struct c4iw_dev_ucontext *uctx)
 out:
 	mutex_unlock(&uctx->lock);
 	PDBG("%s qid 0x%x\n", __func__, qid);
+	mutex_lock(&rdev->stats.lock);
+	if (rdev->stats.qid.cur > rdev->stats.qid.max)
+		rdev->stats.qid.max = rdev->stats.qid.cur;
+	mutex_unlock(&rdev->stats.lock);
 	return qid;
 }
 
@@ -245,10 +178,12 @@ u32 c4iw_get_qpid(struct c4iw_rdev *rdev, struct c4iw_dev_ucontext *uctx)
 		qid = entry->qid;
 		kfree(entry);
 	} else {
-		qid = c4iw_get_resource(&rdev->resource.qid_fifo,
-					&rdev->resource.qid_fifo_lock);
+		qid = c4iw_get_resource(&rdev->resource.qid_table);
 		if (!qid)
 			goto out;
+		mutex_lock(&rdev->stats.lock);
+		rdev->stats.qid.cur += rdev->qpmask + 1;
+		mutex_unlock(&rdev->stats.lock);
 		for (i = qid+1; i & rdev->qpmask; i++) {
 			entry = kmalloc(sizeof *entry, GFP_KERNEL);
 			if (!entry)
@@ -277,6 +212,10 @@ u32 c4iw_get_qpid(struct c4iw_rdev *rdev, struct c4iw_dev_ucontext *uctx)
 out:
 	mutex_unlock(&uctx->lock);
 	PDBG("%s qid 0x%x\n", __func__, qid);
+	mutex_lock(&rdev->stats.lock);
+	if (rdev->stats.qid.cur > rdev->stats.qid.max)
+		rdev->stats.qid.max = rdev->stats.qid.cur;
+	mutex_unlock(&rdev->stats.lock);
 	return qid;
 }
 
@@ -297,9 +236,9 @@ void c4iw_put_qpid(struct c4iw_rdev *rdev, u32 qid,
 
 void c4iw_destroy_resource(struct c4iw_resource *rscp)
 {
-	kfifo_free(&rscp->tpt_fifo);
-	kfifo_free(&rscp->qid_fifo);
-	kfifo_free(&rscp->pdid_fifo);
+	c4iw_id_table_free(&rscp->tpt_table);
+	c4iw_id_table_free(&rscp->qid_table);
+	c4iw_id_table_free(&rscp->pdid_table);
 }
 
 /*
@@ -312,15 +251,23 @@ u32 c4iw_pblpool_alloc(struct c4iw_rdev *rdev, int size)
 {
 	unsigned long addr = gen_pool_alloc(rdev->pbl_pool, size);
 	PDBG("%s addr 0x%x size %d\n", __func__, (u32)addr, size);
-	if (!addr)
-		printk_ratelimited(KERN_WARNING MOD "%s: Out of PBL memory\n",
-		       pci_name(rdev->lldi.pdev));
+	mutex_lock(&rdev->stats.lock);
+	if (addr) {
+		rdev->stats.pbl.cur += roundup(size, 1 << MIN_PBL_SHIFT);
+		if (rdev->stats.pbl.cur > rdev->stats.pbl.max)
+			rdev->stats.pbl.max = rdev->stats.pbl.cur;
+	} else
+		rdev->stats.pbl.fail++;
+	mutex_unlock(&rdev->stats.lock);
 	return (u32)addr;
 }
 
 void c4iw_pblpool_free(struct c4iw_rdev *rdev, u32 addr, int size)
 {
 	PDBG("%s addr 0x%x size %d\n", __func__, addr, size);
+	mutex_lock(&rdev->stats.lock);
+	rdev->stats.pbl.cur -= roundup(size, 1 << MIN_PBL_SHIFT);
+	mutex_unlock(&rdev->stats.lock);
 	gen_pool_free(rdev->pbl_pool, (unsigned long)addr, size);
 }
 
@@ -377,12 +324,23 @@ u32 c4iw_rqtpool_alloc(struct c4iw_rdev *rdev, int size)
 	if (!addr)
 		printk_ratelimited(KERN_WARNING MOD "%s: Out of RQT memory\n",
 		       pci_name(rdev->lldi.pdev));
+	mutex_lock(&rdev->stats.lock);
+	if (addr) {
+		rdev->stats.rqt.cur += roundup(size << 6, 1 << MIN_RQT_SHIFT);
+		if (rdev->stats.rqt.cur > rdev->stats.rqt.max)
+			rdev->stats.rqt.max = rdev->stats.rqt.cur;
+	} else
+		rdev->stats.rqt.fail++;
+	mutex_unlock(&rdev->stats.lock);
 	return (u32)addr;
 }
 
 void c4iw_rqtpool_free(struct c4iw_rdev *rdev, u32 addr, int size)
 {
 	PDBG("%s addr 0x%x size %d\n", __func__, addr, size << 6);
+	mutex_lock(&rdev->stats.lock);
+	rdev->stats.rqt.cur -= roundup(size << 6, 1 << MIN_RQT_SHIFT);
+	mutex_unlock(&rdev->stats.lock);
 	gen_pool_free(rdev->rqt_pool, (unsigned long)addr, size << 6);
 }
 
@@ -433,12 +391,22 @@ u32 c4iw_ocqp_pool_alloc(struct c4iw_rdev *rdev, int size)
 {
 	unsigned long addr = gen_pool_alloc(rdev->ocqp_pool, size);
 	PDBG("%s addr 0x%x size %d\n", __func__, (u32)addr, size);
+	if (addr) {
+		mutex_lock(&rdev->stats.lock);
+		rdev->stats.ocqp.cur += roundup(size, 1 << MIN_OCQP_SHIFT);
+		if (rdev->stats.ocqp.cur > rdev->stats.ocqp.max)
+			rdev->stats.ocqp.max = rdev->stats.ocqp.cur;
+		mutex_unlock(&rdev->stats.lock);
+	}
 	return (u32)addr;
 }
 
 void c4iw_ocqp_pool_free(struct c4iw_rdev *rdev, u32 addr, int size)
 {
 	PDBG("%s addr 0x%x size %d\n", __func__, addr, size);
+	mutex_lock(&rdev->stats.lock);
+	rdev->stats.ocqp.cur -= roundup(size, 1 << MIN_OCQP_SHIFT);
+	mutex_unlock(&rdev->stats.lock);
 	gen_pool_free(rdev->ocqp_pool, (unsigned long)addr, size);
 }
 
diff --git a/drivers/infiniband/hw/cxgb4/t4.h b/drivers/infiniband/hw/cxgb4/t4.h
index c0221eec881..16f26ab2930 100644
--- a/drivers/infiniband/hw/cxgb4/t4.h
+++ b/drivers/infiniband/hw/cxgb4/t4.h
@@ -62,6 +62,10 @@ struct t4_status_page {
 	__be16 pidx;
 	u8 qp_err;	/* flit 1 - sw owns */
 	u8 db_off;
+	u8 pad;
+	u16 host_wq_pidx;
+	u16 host_cidx;
+	u16 host_pidx;
 };
 
 #define T4_EQ_ENTRY_SIZE 64
@@ -375,6 +379,16 @@ static inline void t4_rq_consume(struct t4_wq *wq)
 		wq->rq.cidx = 0;
 }
 
+static inline u16 t4_rq_host_wq_pidx(struct t4_wq *wq)
+{
+	return wq->rq.queue[wq->rq.size].status.host_wq_pidx;
+}
+
+static inline u16 t4_rq_wq_size(struct t4_wq *wq)
+{
+		return wq->rq.size * T4_RQ_NUM_SLOTS;
+}
+
 static inline int t4_sq_onchip(struct t4_sq *sq)
 {
 	return sq->flags & T4_SQ_ONCHIP;
@@ -412,6 +426,16 @@ static inline void t4_sq_consume(struct t4_wq *wq)
 		wq->sq.cidx = 0;
 }
 
+static inline u16 t4_sq_host_wq_pidx(struct t4_wq *wq)
+{
+	return wq->sq.queue[wq->sq.size].status.host_wq_pidx;
+}
+
+static inline u16 t4_sq_wq_size(struct t4_wq *wq)
+{
+		return wq->sq.size * T4_SQ_NUM_SLOTS;
+}
+
 static inline void t4_ring_sq_db(struct t4_wq *wq, u16 inc)
 {
 	wmb();
diff --git a/drivers/infiniband/hw/cxgb4/user.h b/drivers/infiniband/hw/cxgb4/user.h
index e6669d54770..32b754c35ab 100644
--- a/drivers/infiniband/hw/cxgb4/user.h
+++ b/drivers/infiniband/hw/cxgb4/user.h
@@ -32,7 +32,7 @@
 #ifndef __C4IW_USER_H__
 #define __C4IW_USER_H__
 
-#define C4IW_UVERBS_ABI_VERSION	1
+#define C4IW_UVERBS_ABI_VERSION	2
 
 /*
  * Make sure that all structs defined in this file remain laid out so
diff --git a/drivers/infiniband/hw/ipath/ipath_iba6110.c b/drivers/infiniband/hw/ipath/ipath_iba6110.c
index 1d7aea132a0..7cc305488a3 100644
--- a/drivers/infiniband/hw/ipath/ipath_iba6110.c
+++ b/drivers/infiniband/hw/ipath/ipath_iba6110.c
@@ -596,8 +596,7 @@ static void ipath_ht_handle_hwerrors(struct ipath_devdata *dd, char *msg,
 
 	ipath_format_hwerrors(hwerrs,
 			      ipath_6110_hwerror_msgs,
-			      sizeof(ipath_6110_hwerror_msgs) /
-			      sizeof(ipath_6110_hwerror_msgs[0]),
+			      ARRAY_SIZE(ipath_6110_hwerror_msgs),
 			      msg, msgl);
 
 	if (hwerrs & (_IPATH_HTLINK0_CRCBITS | _IPATH_HTLINK1_CRCBITS))
diff --git a/drivers/infiniband/hw/ipath/ipath_intr.c b/drivers/infiniband/hw/ipath/ipath_intr.c
index c0a03ac03ee..26dfbc8ee0f 100644
--- a/drivers/infiniband/hw/ipath/ipath_intr.c
+++ b/drivers/infiniband/hw/ipath/ipath_intr.c
@@ -209,8 +209,7 @@ void ipath_format_hwerrors(u64 hwerrs,
 {
 	int i;
 	const int glen =
-	    sizeof(ipath_generic_hwerror_msgs) /
-	    sizeof(ipath_generic_hwerror_msgs[0]);
+	    ARRAY_SIZE(ipath_generic_hwerror_msgs);
 
 	for (i=0; i<glen; i++) {
 		if (hwerrs & ipath_generic_hwerror_msgs[i].mask) {
diff --git a/drivers/infiniband/hw/mlx4/cq.c b/drivers/infiniband/hw/mlx4/cq.c
index 77c8cb4c507..6d4ef71cbcd 100644
--- a/drivers/infiniband/hw/mlx4/cq.c
+++ b/drivers/infiniband/hw/mlx4/cq.c
@@ -50,7 +50,7 @@ static void mlx4_ib_cq_event(struct mlx4_cq *cq, enum mlx4_event type)
 	struct ib_cq *ibcq;
 
 	if (type != MLX4_EVENT_TYPE_CQ_ERROR) {
-		printk(KERN_WARNING "mlx4_ib: Unexpected event type %d "
+		pr_warn("Unexpected event type %d "
 		       "on CQ %06x\n", type, cq->cqn);
 		return;
 	}
@@ -222,6 +222,9 @@ struct ib_cq *mlx4_ib_create_cq(struct ib_device *ibdev, int entries, int vector
 		uar = &dev->priv_uar;
 	}
 
+	if (dev->eq_table)
+		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);
 	if (err)
@@ -463,7 +466,7 @@ static void dump_cqe(void *cqe)
 {
 	__be32 *buf = cqe;
 
-	printk(KERN_DEBUG "CQE contents %08x %08x %08x %08x %08x %08x %08x %08x\n",
+	pr_debug("CQE contents %08x %08x %08x %08x %08x %08x %08x %08x\n",
 	       be32_to_cpu(buf[0]), be32_to_cpu(buf[1]), be32_to_cpu(buf[2]),
 	       be32_to_cpu(buf[3]), be32_to_cpu(buf[4]), be32_to_cpu(buf[5]),
 	       be32_to_cpu(buf[6]), be32_to_cpu(buf[7]));
@@ -473,7 +476,7 @@ static void mlx4_ib_handle_error_cqe(struct mlx4_err_cqe *cqe,
 				     struct ib_wc *wc)
 {
 	if (cqe->syndrome == MLX4_CQE_SYNDROME_LOCAL_QP_OP_ERR) {
-		printk(KERN_DEBUG "local QP operation err "
+		pr_debug("local QP operation err "
 		       "(QPN %06x, WQE index %x, vendor syndrome %02x, "
 		       "opcode = %02x)\n",
 		       be32_to_cpu(cqe->my_qpn), be16_to_cpu(cqe->wqe_index),
@@ -576,7 +579,7 @@ repoll:
 
 	if (unlikely((cqe->owner_sr_opcode & MLX4_CQE_OPCODE_MASK) == MLX4_OPCODE_NOP &&
 		     is_send)) {
-		printk(KERN_WARNING "Completion for NOP opcode detected!\n");
+		pr_warn("Completion for NOP opcode detected!\n");
 		return -EINVAL;
 	}
 
@@ -606,7 +609,7 @@ repoll:
 		mqp = __mlx4_qp_lookup(to_mdev(cq->ibcq.device)->dev,
 				       be32_to_cpu(cqe->vlan_my_qpn));
 		if (unlikely(!mqp)) {
-			printk(KERN_WARNING "CQ %06x with entry for unknown QPN %06x\n",
+			pr_warn("CQ %06x with entry for unknown QPN %06x\n",
 			       cq->mcq.cqn, be32_to_cpu(cqe->vlan_my_qpn) & MLX4_CQE_QPN_MASK);
 			return -EINVAL;
 		}
diff --git a/drivers/infiniband/hw/mlx4/main.c b/drivers/infiniband/hw/mlx4/main.c
index b948b6dd5d5..ee1c577238f 100644
--- a/drivers/infiniband/hw/mlx4/main.c
+++ b/drivers/infiniband/hw/mlx4/main.c
@@ -789,7 +789,7 @@ static int mlx4_ib_mcg_detach(struct ib_qp *ibqp, union ib_gid *gid, u16 lid)
 		list_del(&ge->list);
 		kfree(ge);
 	} else
-		printk(KERN_WARNING "could not find mgid entry\n");
+		pr_warn("could not find mgid entry\n");
 
 	mutex_unlock(&mqp->mutex);
 
@@ -902,7 +902,7 @@ static void update_gids_task(struct work_struct *work)
 
 	mailbox = mlx4_alloc_cmd_mailbox(dev);
 	if (IS_ERR(mailbox)) {
-		printk(KERN_WARNING "update gid table failed %ld\n", PTR_ERR(mailbox));
+		pr_warn("update gid table failed %ld\n", PTR_ERR(mailbox));
 		return;
 	}
 
@@ -913,7 +913,7 @@ static void update_gids_task(struct work_struct *work)
 		       1, MLX4_CMD_SET_PORT, MLX4_CMD_TIME_CLASS_B,
 		       MLX4_CMD_NATIVE);
 	if (err)
-		printk(KERN_WARNING "set port command failed\n");
+		pr_warn("set port command failed\n");
 	else {
 		memcpy(gw->dev->iboe.gid_table[gw->port - 1], gw->gids, sizeof gw->gids);
 		event.device = &gw->dev->ib_dev;
@@ -1076,18 +1076,98 @@ static int mlx4_ib_netdev_event(struct notifier_block *this, unsigned long event
 	return NOTIFY_DONE;
 }
 
+static void mlx4_ib_alloc_eqs(struct mlx4_dev *dev, struct mlx4_ib_dev *ibdev)
+{
+	char name[32];
+	int eq_per_port = 0;
+	int added_eqs = 0;
+	int total_eqs = 0;
+	int i, j, eq;
+
+	/* Init eq table */
+	ibdev->eq_table = NULL;
+	ibdev->eq_added = 0;
+
+	/* Legacy mode? */
+	if (dev->caps.comp_pool == 0)
+		return;
+
+	eq_per_port = rounddown_pow_of_two(dev->caps.comp_pool/
+					dev->caps.num_ports);
+
+	/* Init eq table */
+	added_eqs = 0;
+	mlx4_foreach_port(i, dev, MLX4_PORT_TYPE_IB)
+		added_eqs += eq_per_port;
+
+	total_eqs = dev->caps.num_comp_vectors + added_eqs;
+
+	ibdev->eq_table = kzalloc(total_eqs * sizeof(int), GFP_KERNEL);
+	if (!ibdev->eq_table)
+		return;
+
+	ibdev->eq_added = added_eqs;
+
+	eq = 0;
+	mlx4_foreach_port(i, dev, MLX4_PORT_TYPE_IB) {
+		for (j = 0; j < eq_per_port; j++) {
+			sprintf(name, "mlx4-ib-%d-%d@%s",
+				i, j, dev->pdev->bus->name);
+			/* Set IRQ for specific name (per ring) */
+			if (mlx4_assign_eq(dev, name, &ibdev->eq_table[eq])) {
+				/* Use legacy (same as mlx4_en driver) */
+				pr_warn("Can't allocate EQ %d; reverting to legacy\n", eq);
+				ibdev->eq_table[eq] =
+					(eq % dev->caps.num_comp_vectors);
+			}
+			eq++;
+		}
+	}
+
+	/* Fill the reset of the vector with legacy EQ */
+	for (i = 0, eq = added_eqs; i < dev->caps.num_comp_vectors; i++)
+		ibdev->eq_table[eq++] = i;
+
+	/* Advertise the new number of EQs to clients */
+	ibdev->ib_dev.num_comp_vectors = total_eqs;
+}
+
+static void mlx4_ib_free_eqs(struct mlx4_dev *dev, struct mlx4_ib_dev *ibdev)
+{
+	int i;
+	int total_eqs;
+
+	/* Reset the advertised EQ number */
+	ibdev->ib_dev.num_comp_vectors = dev->caps.num_comp_vectors;
+
+	/* Free only the added eqs */
+	for (i = 0; i < ibdev->eq_added; i++) {
+		/* Don't free legacy eqs if used */
+		if (ibdev->eq_table[i] <= dev->caps.num_comp_vectors)
+			continue;
+		mlx4_release_eq(dev, ibdev->eq_table[i]);
+	}
+
+	total_eqs = dev->caps.num_comp_vectors + ibdev->eq_added;
+	memset(ibdev->eq_table, 0, total_eqs * sizeof(int));
+	kfree(ibdev->eq_table);
+
+	ibdev->eq_table = NULL;
+	ibdev->eq_added = 0;
+}
+
 static void *mlx4_ib_add(struct mlx4_dev *dev)
 {
 	struct mlx4_ib_dev *ibdev;
 	int num_ports = 0;
-	int i;
+	int i, j;
 	int err;
 	struct mlx4_ib_iboe *iboe;
 
-	printk_once(KERN_INFO "%s", mlx4_ib_version);
+	pr_info_once("%s", mlx4_ib_version);
 
 	if (mlx4_is_mfunc(dev)) {
-		printk(KERN_WARNING "IB not yet supported in SRIOV\n");
+		pr_warn("IB not yet supported in SRIOV\n");
 		return NULL;
 	}
 
@@ -1210,6 +1290,8 @@ static void *mlx4_ib_add(struct mlx4_dev *dev)
 			(1ull << IB_USER_VERBS_CMD_CLOSE_XRCD);
 	}
 
+	mlx4_ib_alloc_eqs(dev, ibdev);
+
 	spin_lock_init(&iboe->lock);
 
 	if (init_node_data(ibdev))
@@ -1241,9 +1323,9 @@ static void *mlx4_ib_add(struct mlx4_dev *dev)
 			goto err_reg;
 	}
 
-	for (i = 0; i < ARRAY_SIZE(mlx4_class_attributes); ++i) {
+	for (j = 0; j < ARRAY_SIZE(mlx4_class_attributes); ++j) {
 		if (device_create_file(&ibdev->ib_dev.dev,
-				       mlx4_class_attributes[i]))
+				       mlx4_class_attributes[j]))
 			goto err_notif;
 	}
 
@@ -1253,7 +1335,7 @@ static void *mlx4_ib_add(struct mlx4_dev *dev)
 
 err_notif:
 	if (unregister_netdevice_notifier(&ibdev->iboe.nb))
-		printk(KERN_WARNING "failure unregistering notifier\n");
+		pr_warn("failure unregistering notifier\n");
 	flush_workqueue(wq);
 
 err_reg:
@@ -1288,7 +1370,7 @@ static void mlx4_ib_remove(struct mlx4_dev *dev, void *ibdev_ptr)
 	ib_unregister_device(&ibdev->ib_dev);
 	if (ibdev->iboe.nb.notifier_call) {
 		if (unregister_netdevice_notifier(&ibdev->iboe.nb))
-			printk(KERN_WARNING "failure unregistering notifier\n");
+			pr_warn("failure unregistering notifier\n");
 		ibdev->iboe.nb.notifier_call = NULL;
 	}
 	iounmap(ibdev->uar_map);
@@ -1298,6 +1380,8 @@ static void mlx4_ib_remove(struct mlx4_dev *dev, void *ibdev_ptr)
 	mlx4_foreach_port(p, dev, MLX4_PORT_TYPE_IB)
 		mlx4_CLOSE_PORT(dev, p);
 
+	mlx4_ib_free_eqs(dev, ibdev);
+
 	mlx4_uar_free(dev, &ibdev->priv_uar);
 	mlx4_pd_free(dev, ibdev->priv_pdn);
 	ib_dealloc_device(&ibdev->ib_dev);
diff --git a/drivers/infiniband/hw/mlx4/mlx4_ib.h b/drivers/infiniband/hw/mlx4/mlx4_ib.h
index ed80345c99a..e62297cc77c 100644
--- a/drivers/infiniband/hw/mlx4/mlx4_ib.h
+++ b/drivers/infiniband/hw/mlx4/mlx4_ib.h
@@ -202,6 +202,8 @@ struct mlx4_ib_dev {
 	bool			ib_active;
 	struct mlx4_ib_iboe	iboe;
 	int			counters[MLX4_MAX_PORTS];
+	int		       *eq_table;
+	int			eq_added;
 };
 
 static inline struct mlx4_ib_dev *to_mdev(struct ib_device *ibdev)
diff --git a/drivers/infiniband/hw/mlx4/mr.c b/drivers/infiniband/hw/mlx4/mr.c
index dca55b19a6f..bbaf6176f20 100644
--- a/drivers/infiniband/hw/mlx4/mr.c
+++ b/drivers/infiniband/hw/mlx4/mr.c
@@ -338,7 +338,7 @@ int mlx4_ib_unmap_fmr(struct list_head *fmr_list)
 
 	err = mlx4_SYNC_TPT(mdev);
 	if (err)
-		printk(KERN_WARNING "mlx4_ib: SYNC_TPT error %d when "
+		pr_warn("SYNC_TPT error %d when "
 		       "unmapping FMRs\n", err);
 
 	return 0;
diff --git a/drivers/infiniband/hw/mlx4/qp.c b/drivers/infiniband/hw/mlx4/qp.c
index 3a784896662..ceb33327091 100644
--- a/drivers/infiniband/hw/mlx4/qp.c
+++ b/drivers/infiniband/hw/mlx4/qp.c
@@ -84,6 +84,11 @@ enum {
 	MLX4_IB_CACHE_LINE_SIZE	= 64,
 };
 
+enum {
+	MLX4_RAW_QP_MTU		= 7,
+	MLX4_RAW_QP_MSGMAX	= 31,
+};
+
 static const __be32 mlx4_ib_opcode[] = {
 	[IB_WR_SEND]				= cpu_to_be32(MLX4_OPCODE_SEND),
 	[IB_WR_LSO]				= cpu_to_be32(MLX4_OPCODE_LSO),
@@ -256,7 +261,7 @@ static void mlx4_ib_qp_event(struct mlx4_qp *qp, enum mlx4_event type)
 			event.event = IB_EVENT_QP_ACCESS_ERR;
 			break;
 		default:
-			printk(KERN_WARNING "mlx4_ib: Unexpected event type %d "
+			pr_warn("Unexpected event type %d "
 			       "on QP %06x\n", type, qp->qpn);
 			return;
 		}
@@ -573,7 +578,12 @@ static int create_qp_common(struct mlx4_ib_dev *dev, struct ib_pd *pd,
 	if (sqpn) {
 		qpn = sqpn;
 	} else {
-		err = mlx4_qp_reserve_range(dev->dev, 1, 1, &qpn);
+		/* Raw packet QPNs must be aligned to 8 bits. If not, the WQE
+		 * BlueFlame setup flow wrongly causes VLAN insertion. */
+		if (init_attr->qp_type == IB_QPT_RAW_PACKET)
+			err = mlx4_qp_reserve_range(dev->dev, 1, 1 << 8, &qpn);
+		else
+			err = mlx4_qp_reserve_range(dev->dev, 1, 1, &qpn);
 		if (err)
 			goto err_wrid;
 	}
@@ -715,7 +725,7 @@ static void destroy_qp_common(struct mlx4_ib_dev *dev, struct mlx4_ib_qp *qp,
 	if (qp->state != IB_QPS_RESET)
 		if (mlx4_qp_modify(dev->dev, NULL, to_mlx4_state(qp->state),
 				   MLX4_QP_STATE_RST, NULL, 0, 0, &qp->mqp))
-			printk(KERN_WARNING "mlx4_ib: modify QP %06x to RESET failed.\n",
+			pr_warn("modify QP %06x to RESET failed.\n",
 			       qp->mqp.qpn);
 
 	get_cqs(qp, &send_cq, &recv_cq);
@@ -791,6 +801,7 @@ struct ib_qp *mlx4_ib_create_qp(struct ib_pd *pd,
 	case IB_QPT_RC:
 	case IB_QPT_UC:
 	case IB_QPT_UD:
+	case IB_QPT_RAW_PACKET:
 	{
 		qp = kzalloc(sizeof *qp, GFP_KERNEL);
 		if (!qp)
@@ -872,7 +883,8 @@ static int to_mlx4_st(enum ib_qp_type type)
 	case IB_QPT_XRC_INI:
 	case IB_QPT_XRC_TGT:	return MLX4_QP_ST_XRC;
 	case IB_QPT_SMI:
-	case IB_QPT_GSI:	return MLX4_QP_ST_MLX;
+	case IB_QPT_GSI:
+	case IB_QPT_RAW_PACKET:	return MLX4_QP_ST_MLX;
 	default:		return -1;
 	}
 }
@@ -946,7 +958,7 @@ static int mlx4_set_path(struct mlx4_ib_dev *dev, const struct ib_ah_attr *ah,
 
 	if (ah->ah_flags & IB_AH_GRH) {
 		if (ah->grh.sgid_index >= dev->dev->caps.gid_table_len[port]) {
-			printk(KERN_ERR "sgid_index (%u) too large. max is %d\n",
+			pr_err("sgid_index (%u) too large. max is %d\n",
 			       ah->grh.sgid_index, dev->dev->caps.gid_table_len[port] - 1);
 			return -1;
 		}
@@ -1042,6 +1054,8 @@ static int __mlx4_ib_modify_qp(struct ib_qp *ibqp,
 
 	if (ibqp->qp_type == IB_QPT_GSI || ibqp->qp_type == IB_QPT_SMI)
 		context->mtu_msgmax = (IB_MTU_4096 << 5) | 11;
+	else if (ibqp->qp_type == IB_QPT_RAW_PACKET)
+		context->mtu_msgmax = (MLX4_RAW_QP_MTU << 5) | MLX4_RAW_QP_MSGMAX;
 	else if (ibqp->qp_type == IB_QPT_UD) {
 		if (qp->flags & MLX4_IB_QP_LSO)
 			context->mtu_msgmax = (IB_MTU_4096 << 5) |
@@ -1050,7 +1064,7 @@ static int __mlx4_ib_modify_qp(struct ib_qp *ibqp,
 			context->mtu_msgmax = (IB_MTU_4096 << 5) | 12;
 	} else if (attr_mask & IB_QP_PATH_MTU) {
 		if (attr->path_mtu < IB_MTU_256 || attr->path_mtu > IB_MTU_4096) {
-			printk(KERN_ERR "path MTU (%u) is invalid\n",
+			pr_err("path MTU (%u) is invalid\n",
 			       attr->path_mtu);
 			goto out;
 		}
@@ -1200,7 +1214,8 @@ static int __mlx4_ib_modify_qp(struct ib_qp *ibqp,
 	if (cur_state == IB_QPS_INIT &&
 	    new_state == IB_QPS_RTR  &&
 	    (ibqp->qp_type == IB_QPT_GSI || ibqp->qp_type == IB_QPT_SMI ||
-	     ibqp->qp_type == IB_QPT_UD)) {
+	     ibqp->qp_type == IB_QPT_UD ||
+	     ibqp->qp_type == IB_QPT_RAW_PACKET)) {
 		context->pri_path.sched_queue = (qp->port - 1) << 6;
 		if (is_qp0(dev, qp))
 			context->pri_path.sched_queue |= MLX4_IB_DEFAULT_QP0_SCHED_QUEUE;
@@ -1266,7 +1281,7 @@ static int __mlx4_ib_modify_qp(struct ib_qp *ibqp,
 	if (is_qp0(dev, qp)) {
 		if (cur_state != IB_QPS_RTR && new_state == IB_QPS_RTR)
 			if (mlx4_INIT_PORT(dev->dev, qp->port))
-				printk(KERN_WARNING "INIT_PORT failed for port %d\n",
+				pr_warn("INIT_PORT failed for port %d\n",
 				       qp->port);
 
 		if (cur_state != IB_QPS_RESET && cur_state != IB_QPS_ERR &&
@@ -1319,6 +1334,11 @@ int mlx4_ib_modify_qp(struct ib_qp *ibqp, struct ib_qp_attr *attr,
 		goto out;
 	}
 
+	if ((attr_mask & IB_QP_PORT) && (ibqp->qp_type == IB_QPT_RAW_PACKET) &&
+	    (rdma_port_get_link_layer(&dev->ib_dev, attr->port_num) !=
+	     IB_LINK_LAYER_ETHERNET))
+		goto out;
+
 	if (attr_mask & IB_QP_PKEY_INDEX) {
 		int p = attr_mask & IB_QP_PORT ? attr->port_num : qp->port;
 		if (attr->pkey_index >= dev->dev->caps.pkey_table_len[p])
@@ -1424,6 +1444,9 @@ static int build_mlx_header(struct mlx4_ib_sqp *sqp, struct ib_send_wr *wr,
 
 	if (is_eth) {
 		u8 *smac;
+		u16 pcp = (be32_to_cpu(ah->av.ib.sl_tclass_flowlabel) >> 29) << 13;
+
+		mlx->sched_prio = cpu_to_be16(pcp);
 
 		memcpy(sqp->ud_header.eth.dmac_h, ah->av.eth.mac, 6);
 		/* FIXME: cache smac value? */
@@ -1434,10 +1457,7 @@ static int build_mlx_header(struct mlx4_ib_sqp *sqp, struct ib_send_wr *wr,
 		if (!is_vlan) {
 			sqp->ud_header.eth.type = cpu_to_be16(MLX4_IB_IBOE_ETHERTYPE);
 		} else {
-			u16 pcp;
-
 			sqp->ud_header.vlan.type = cpu_to_be16(MLX4_IB_IBOE_ETHERTYPE);
-			pcp = (be32_to_cpu(ah->av.ib.sl_tclass_flowlabel) >> 29) << 13;
 			sqp->ud_header.vlan.tag = cpu_to_be16(vlan | pcp);
 		}
 	} else {
@@ -1460,16 +1480,16 @@ static int build_mlx_header(struct mlx4_ib_sqp *sqp, struct ib_send_wr *wr,
 	header_size = ib_ud_header_pack(&sqp->ud_header, sqp->header_buf);
 
 	if (0) {
-		printk(KERN_ERR "built UD header of size %d:\n", header_size);
+		pr_err("built UD header of size %d:\n", header_size);
 		for (i = 0; i < header_size / 4; ++i) {
 			if (i % 8 == 0)
-				printk("  [%02x] ", i * 4);
-			printk(" %08x",
-			       be32_to_cpu(((__be32 *) sqp->header_buf)[i]));
+				pr_err("  [%02x] ", i * 4);
+			pr_cont(" %08x",
+				be32_to_cpu(((__be32 *) sqp->header_buf)[i]));
 			if ((i + 1) % 8 == 0)
-				printk("\n");
+				pr_cont("\n");
 		}
-		printk("\n");
+		pr_err("\n");
 	}
 
 	/*
diff --git a/drivers/infiniband/hw/mlx4/srq.c b/drivers/infiniband/hw/mlx4/srq.c
index 39542f3703b..60c5fb025fc 100644
--- a/drivers/infiniband/hw/mlx4/srq.c
+++ b/drivers/infiniband/hw/mlx4/srq.c
@@ -59,7 +59,7 @@ static void mlx4_ib_srq_event(struct mlx4_srq *srq, enum mlx4_event type)
 			event.event = IB_EVENT_SRQ_ERR;
 			break;
 		default:
-			printk(KERN_WARNING "mlx4_ib: Unexpected event type %d "
+			pr_warn("Unexpected event type %d "
 			       "on SRQ %06x\n", type, srq->srqn);
 			return;
 		}
diff --git a/drivers/infiniband/hw/nes/nes_cm.c b/drivers/infiniband/hw/nes/nes_cm.c
index 71edfbbcce1..020e95c4c4b 100644
--- a/drivers/infiniband/hw/nes/nes_cm.c
+++ b/drivers/infiniband/hw/nes/nes_cm.c
@@ -2884,7 +2884,8 @@ static int nes_cm_disconn_true(struct nes_qp *nesqp)
 			ibevent.device = nesqp->ibqp.device;
 			ibevent.event = nesqp->terminate_eventtype;
 			ibevent.element.qp = &nesqp->ibqp;
-			nesqp->ibqp.event_handler(&ibevent, nesqp->ibqp.qp_context);
+			if (nesqp->ibqp.event_handler)
+				nesqp->ibqp.event_handler(&ibevent, nesqp->ibqp.qp_context);
 		}
 	}
 
@@ -3320,6 +3321,10 @@ int nes_connect(struct iw_cm_id *cm_id, struct iw_cm_conn_param *conn_param)
 
 	nesqp->private_data_len = conn_param->private_data_len;
 	nesqp->nesqp_context->ird_ord_sizes |= cpu_to_le32((u32)conn_param->ord);
+	/* space for rdma0 read msg */
+	if (conn_param->ord == 0)
+		nesqp->nesqp_context->ird_ord_sizes |= cpu_to_le32(1);
+
 	nes_debug(NES_DBG_CM, "requested ord = 0x%08X.\n", (u32)conn_param->ord);
 	nes_debug(NES_DBG_CM, "mpa private data len =%u\n",
 		  conn_param->private_data_len);
diff --git a/drivers/infiniband/hw/ocrdma/Kconfig b/drivers/infiniband/hw/ocrdma/Kconfig
new file mode 100644
index 00000000000..b5b6056c851
--- /dev/null
+++ b/drivers/infiniband/hw/ocrdma/Kconfig
@@ -0,0 +1,8 @@
+config INFINIBAND_OCRDMA
+	tristate "Emulex One Connect HCA support"
+	depends on ETHERNET && NETDEVICES && PCI && (IPV6 || IPV6=n)
+	select NET_VENDOR_EMULEX
+	select BE2NET
+	---help---
+	  This driver provides low-level InfiniBand over Ethernet
+	  support for Emulex One Connect host channel adapters (HCAs).
diff --git a/drivers/infiniband/hw/ocrdma/Makefile b/drivers/infiniband/hw/ocrdma/Makefile
new file mode 100644
index 00000000000..06a5bed12e4
--- /dev/null
+++ b/drivers/infiniband/hw/ocrdma/Makefile
@@ -0,0 +1,5 @@
+ccflags-y := -Idrivers/net/ethernet/emulex/benet
+
+obj-$(CONFIG_INFINIBAND_OCRDMA)	+= ocrdma.o
+
+ocrdma-y :=	ocrdma_main.o ocrdma_verbs.o ocrdma_hw.o ocrdma_ah.o
diff --git a/drivers/infiniband/hw/ocrdma/ocrdma.h b/drivers/infiniband/hw/ocrdma/ocrdma.h
new file mode 100644
index 00000000000..85a69c95855
--- /dev/null
+++ b/drivers/infiniband/hw/ocrdma/ocrdma.h
@@ -0,0 +1,393 @@
+/*******************************************************************
+ * This file is part of the Emulex RoCE Device Driver for          *
+ * RoCE (RDMA over Converged Ethernet) adapters.                   *
+ * Copyright (C) 2008-2012 Emulex. All rights reserved.            *
+ * EMULEX and SLI are trademarks of Emulex.                        *
+ * www.emulex.com                                                  *
+ *                                                                 *
+ * 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. *
+ * ALL EXPRESS OR IMPLIED CONDITIONS, REPRESENTATIONS AND          *
+ * WARRANTIES, INCLUDING ANY IMPLIED WARRANTY OF MERCHANTABILITY,  *
+ * FITNESS FOR A PARTICULAR PURPOSE, OR NON-INFRINGEMENT, ARE      *
+ * DISCLAIMED, EXCEPT TO THE EXTENT THAT SUCH DISCLAIMERS ARE HELD *
+ * TO BE LEGALLY INVALID.  See the GNU General Public License for  *
+ * more details, a copy of which can be found in the file COPYING  *
+ * included with this package.                                     *
+ *
+ * Contact Information:
+ * linux-drivers@emulex.com
+ *
+ * Emulex
+ * 3333 Susan Street
+ * Costa Mesa, CA 92626
+ *******************************************************************/
+
+#ifndef __OCRDMA_H__
+#define __OCRDMA_H__
+
+#include <linux/mutex.h>
+#include <linux/list.h>
+#include <linux/spinlock.h>
+#include <linux/pci.h>
+
+#include <rdma/ib_verbs.h>
+#include <rdma/ib_user_verbs.h>
+
+#include <be_roce.h>
+#include "ocrdma_sli.h"
+
+#define OCRDMA_ROCE_DEV_VERSION "1.0.0"
+#define OCRDMA_NODE_DESC "Emulex OneConnect RoCE HCA"
+
+#define ocrdma_err(format, arg...) printk(KERN_ERR format, ##arg)
+
+#define OCRDMA_MAX_AH 512
+
+#define OCRDMA_UVERBS(CMD_NAME) (1ull << IB_USER_VERBS_CMD_##CMD_NAME)
+
+struct ocrdma_dev_attr {
+	u8 fw_ver[32];
+	u32 vendor_id;
+	u32 device_id;
+	u16 max_pd;
+	u16 max_cq;
+	u16 max_cqe;
+	u16 max_qp;
+	u16 max_wqe;
+	u16 max_rqe;
+	u32 max_inline_data;
+	int max_send_sge;
+	int max_recv_sge;
+	int max_mr;
+	u64 max_mr_size;
+	u32 max_num_mr_pbl;
+	int max_fmr;
+	int max_map_per_fmr;
+	int max_pages_per_frmr;
+	u16 max_ord_per_qp;
+	u16 max_ird_per_qp;
+
+	int device_cap_flags;
+	u8 cq_overflow_detect;
+	u8 srq_supported;
+
+	u32 wqe_size;
+	u32 rqe_size;
+	u32 ird_page_size;
+	u8 local_ca_ack_delay;
+	u8 ird;
+	u8 num_ird_pages;
+};
+
+struct ocrdma_pbl {
+	void *va;
+	dma_addr_t pa;
+};
+
+struct ocrdma_queue_info {
+	void *va;
+	dma_addr_t dma;
+	u32 size;
+	u16 len;
+	u16 entry_size;		/* Size of an element in the queue */
+	u16 id;			/* qid, where to ring the doorbell. */
+	u16 head, tail;
+	bool created;
+	atomic_t used;		/* Number of valid elements in the queue */
+};
+
+struct ocrdma_eq {
+	struct ocrdma_queue_info q;
+	u32 vector;
+	int cq_cnt;
+	struct ocrdma_dev *dev;
+	char irq_name[32];
+};
+
+struct ocrdma_mq {
+	struct ocrdma_queue_info sq;
+	struct ocrdma_queue_info cq;
+	bool rearm_cq;
+};
+
+struct mqe_ctx {
+	struct mutex lock; /* for serializing mailbox commands on MQ */
+	wait_queue_head_t cmd_wait;
+	u32 tag;
+	u16 cqe_status;
+	u16 ext_status;
+	bool cmd_done;
+};
+
+struct ocrdma_dev {
+	struct ib_device ibdev;
+	struct ocrdma_dev_attr attr;
+
+	struct mutex dev_lock; /* provides syncronise access to device data */
+	spinlock_t flush_q_lock ____cacheline_aligned;
+
+	struct ocrdma_cq **cq_tbl;
+	struct ocrdma_qp **qp_tbl;
+
+	struct ocrdma_eq meq;
+	struct ocrdma_eq *qp_eq_tbl;
+	int eq_cnt;
+	u16 base_eqid;
+	u16 max_eq;
+
+	union ib_gid *sgid_tbl;
+	/* provided synchronization to sgid table for
+	 * updating gid entries triggered by notifier.
+	 */
+	spinlock_t sgid_lock;
+
+	int gsi_qp_created;
+	struct ocrdma_cq *gsi_sqcq;
+	struct ocrdma_cq *gsi_rqcq;
+
+	struct {
+		struct ocrdma_av *va;
+		dma_addr_t pa;
+		u32 size;
+		u32 num_ah;
+		/* provide synchronization for av
+		 * entry allocations.
+		 */
+		spinlock_t lock;
+		u32 ahid;
+		struct ocrdma_pbl pbl;
+	} av_tbl;
+
+	void *mbx_cmd;
+	struct ocrdma_mq mq;
+	struct mqe_ctx mqe_ctx;
+
+	struct be_dev_info nic_info;
+
+	struct list_head entry;
+	struct rcu_head rcu;
+	int id;
+};
+
+struct ocrdma_cq {
+	struct ib_cq ibcq;
+	struct ocrdma_dev *dev;
+	struct ocrdma_cqe *va;
+	u32 phase;
+	u32 getp;	/* pointer to pending wrs to
+			 * return to stack, wrap arounds
+			 * at max_hw_cqe
+			 */
+	u32 max_hw_cqe;
+	bool phase_change;
+	bool armed, solicited;
+	bool arm_needed;
+
+	spinlock_t cq_lock ____cacheline_aligned; /* provide synchronization
+						   * to cq polling
+						   */
+	/* syncronizes cq completion handler invoked from multiple context */
+	spinlock_t comp_handler_lock ____cacheline_aligned;
+	u16 id;
+	u16 eqn;
+
+	struct ocrdma_ucontext *ucontext;
+	dma_addr_t pa;
+	u32 len;
+	atomic_t use_cnt;
+
+	/* head of all qp's sq and rq for which cqes need to be flushed
+	 * by the software.
+	 */
+	struct list_head sq_head, rq_head;
+};
+
+struct ocrdma_pd {
+	struct ib_pd ibpd;
+	struct ocrdma_dev *dev;
+	struct ocrdma_ucontext *uctx;
+	atomic_t use_cnt;
+	u32 id;
+	int num_dpp_qp;
+	u32 dpp_page;
+	bool dpp_enabled;
+};
+
+struct ocrdma_ah {
+	struct ib_ah ibah;
+	struct ocrdma_dev *dev;
+	struct ocrdma_av *av;
+	u16 sgid_index;
+	u32 id;
+};
+
+struct ocrdma_qp_hwq_info {
+	u8 *va;			/* virtual address */
+	u32 max_sges;
+	u32 head, tail;
+	u32 entry_size;
+	u32 max_cnt;
+	u32 max_wqe_idx;
+	u32 free_delta;
+	u16 dbid;		/* qid, where to ring the doorbell. */
+	u32 len;
+	dma_addr_t pa;
+};
+
+struct ocrdma_srq {
+	struct ib_srq ibsrq;
+	struct ocrdma_dev *dev;
+	u8 __iomem *db;
+	/* provide synchronization to multiple context(s) posting rqe */
+	spinlock_t q_lock ____cacheline_aligned;
+
+	struct ocrdma_qp_hwq_info rq;
+	struct ocrdma_pd *pd;
+	atomic_t use_cnt;
+	u32 id;
+	u64 *rqe_wr_id_tbl;
+	u32 *idx_bit_fields;
+	u32 bit_fields_len;
+};
+
+struct ocrdma_qp {
+	struct ib_qp ibqp;
+	struct ocrdma_dev *dev;
+
+	u8 __iomem *sq_db;
+	/* provide synchronization to multiple context(s) posting wqe, rqe */
+	spinlock_t q_lock ____cacheline_aligned;
+	struct ocrdma_qp_hwq_info sq;
+	struct {
+		uint64_t wrid;
+		uint16_t dpp_wqe_idx;
+		uint16_t dpp_wqe;
+		uint8_t  signaled;
+		uint8_t  rsvd[3];
+	} *wqe_wr_id_tbl;
+	u32 max_inline_data;
+	struct ocrdma_cq *sq_cq;
+	/* list maintained per CQ to flush SQ errors */
+	struct list_head sq_entry;
+
+	u8 __iomem *rq_db;
+	struct ocrdma_qp_hwq_info rq;
+	u64 *rqe_wr_id_tbl;
+	struct ocrdma_cq *rq_cq;
+	struct ocrdma_srq *srq;
+	/* list maintained per CQ to flush RQ errors */
+	struct list_head rq_entry;
+
+	enum ocrdma_qp_state state;	/*  QP state */
+	int cap_flags;
+	u32 max_ord, max_ird;
+
+	u32 id;
+	struct ocrdma_pd *pd;
+
+	enum ib_qp_type qp_type;
+
+	int sgid_idx;
+	u32 qkey;
+	bool dpp_enabled;
+	u8 *ird_q_va;
+};
+
+#define OCRDMA_GET_NUM_POSTED_SHIFT_VAL(qp) \
+	(((qp->dev->nic_info.dev_family == OCRDMA_GEN2_FAMILY) && \
+		(qp->id < 64)) ? 24 : 16)
+
+struct ocrdma_hw_mr {
+	struct ocrdma_dev *dev;
+	u32 lkey;
+	u8 fr_mr;
+	u8 remote_atomic;
+	u8 remote_rd;
+	u8 remote_wr;
+	u8 local_rd;
+	u8 local_wr;
+	u8 mw_bind;
+	u8 rsvd;
+	u64 len;
+	struct ocrdma_pbl *pbl_table;
+	u32 num_pbls;
+	u32 num_pbes;
+	u32 pbl_size;
+	u32 pbe_size;
+	u64 fbo;
+	u64 va;
+};
+
+struct ocrdma_mr {
+	struct ib_mr ibmr;
+	struct ib_umem *umem;
+	struct ocrdma_hw_mr hwmr;
+	struct ocrdma_pd *pd;
+};
+
+struct ocrdma_ucontext {
+	struct ib_ucontext ibucontext;
+	struct ocrdma_dev *dev;
+
+	struct list_head mm_head;
+	struct mutex mm_list_lock; /* protects list entries of mm type */
+	struct {
+		u32 *va;
+		dma_addr_t pa;
+		u32 len;
+	} ah_tbl;
+};
+
+struct ocrdma_mm {
+	struct {
+		u64 phy_addr;
+		unsigned long len;
+	} key;
+	struct list_head entry;
+};
+
+static inline struct ocrdma_dev *get_ocrdma_dev(struct ib_device *ibdev)
+{
+	return container_of(ibdev, struct ocrdma_dev, ibdev);
+}
+
+static inline struct ocrdma_ucontext *get_ocrdma_ucontext(struct ib_ucontext
+							  *ibucontext)
+{
+	return container_of(ibucontext, struct ocrdma_ucontext, ibucontext);
+}
+
+static inline struct ocrdma_pd *get_ocrdma_pd(struct ib_pd *ibpd)
+{
+	return container_of(ibpd, struct ocrdma_pd, ibpd);
+}
+
+static inline struct ocrdma_cq *get_ocrdma_cq(struct ib_cq *ibcq)
+{
+	return container_of(ibcq, struct ocrdma_cq, ibcq);
+}
+
+static inline struct ocrdma_qp *get_ocrdma_qp(struct ib_qp *ibqp)
+{
+	return container_of(ibqp, struct ocrdma_qp, ibqp);
+}
+
+static inline struct ocrdma_mr *get_ocrdma_mr(struct ib_mr *ibmr)
+{
+	return container_of(ibmr, struct ocrdma_mr, ibmr);
+}
+
+static inline struct ocrdma_ah *get_ocrdma_ah(struct ib_ah *ibah)
+{
+	return container_of(ibah, struct ocrdma_ah, ibah);
+}
+
+static inline struct ocrdma_srq *get_ocrdma_srq(struct ib_srq *ibsrq)
+{
+	return container_of(ibsrq, struct ocrdma_srq, ibsrq);
+}
+
+#endif
diff --git a/drivers/infiniband/hw/ocrdma/ocrdma_abi.h b/drivers/infiniband/hw/ocrdma/ocrdma_abi.h
new file mode 100644
index 00000000000..a411a4e3193
--- /dev/null
+++ b/drivers/infiniband/hw/ocrdma/ocrdma_abi.h
@@ -0,0 +1,134 @@
+/*******************************************************************
+ * This file is part of the Emulex RoCE Device Driver for          *
+ * RoCE (RDMA over Converged Ethernet) adapters.                   *
+ * Copyright (C) 2008-2012 Emulex. All rights reserved.            *
+ * EMULEX and SLI are trademarks of Emulex.                        *
+ * www.emulex.com                                                  *
+ *                                                                 *
+ * 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. *
+ * ALL EXPRESS OR IMPLIED CONDITIONS, REPRESENTATIONS AND          *
+ * WARRANTIES, INCLUDING ANY IMPLIED WARRANTY OF MERCHANTABILITY,  *
+ * FITNESS FOR A PARTICULAR PURPOSE, OR NON-INFRINGEMENT, ARE      *
+ * DISCLAIMED, EXCEPT TO THE EXTENT THAT SUCH DISCLAIMERS ARE HELD *
+ * TO BE LEGALLY INVALID.  See the GNU General Public License for  *
+ * more details, a copy of which can be found in the file COPYING  *
+ * included with this package.                                     *
+ *
+ * Contact Information:
+ * linux-drivers@emulex.com
+ *
+ * Emulex
+ * 3333 Susan Street
+ * Costa Mesa, CA 92626
+ *******************************************************************/
+
+#ifndef __OCRDMA_ABI_H__
+#define __OCRDMA_ABI_H__
+
+struct ocrdma_alloc_ucontext_resp {
+	u32 dev_id;
+	u32 wqe_size;
+	u32 max_inline_data;
+	u32 dpp_wqe_size;
+	u64 ah_tbl_page;
+	u32 ah_tbl_len;
+	u32 rsvd;
+	u8 fw_ver[32];
+	u32 rqe_size;
+	u64 rsvd1;
+} __packed;
+
+/* user kernel communication data structures. */
+struct ocrdma_alloc_pd_ureq {
+	u64 rsvd1;
+} __packed;
+
+struct ocrdma_alloc_pd_uresp {
+	u32 id;
+	u32 dpp_enabled;
+	u32 dpp_page_addr_hi;
+	u32 dpp_page_addr_lo;
+	u64 rsvd1;
+} __packed;
+
+struct ocrdma_create_cq_ureq {
+	u32 dpp_cq;
+	u32 rsvd;
+} __packed;
+
+#define MAX_CQ_PAGES 8
+struct ocrdma_create_cq_uresp {
+	u32 cq_id;
+	u32 page_size;
+	u32 num_pages;
+	u32 max_hw_cqe;
+	u64 page_addr[MAX_CQ_PAGES];
+	u64 db_page_addr;
+	u32 db_page_size;
+	u32 phase_change;
+	u64 rsvd1;
+	u64 rsvd2;
+} __packed;
+
+#define MAX_QP_PAGES 8
+#define MAX_UD_AV_PAGES 8
+
+struct ocrdma_create_qp_ureq {
+	u8 enable_dpp_cq;
+	u8 rsvd;
+	u16 dpp_cq_id;
+	u32 rsvd1;
+};
+
+struct ocrdma_create_qp_uresp {
+	u16 qp_id;
+	u16 sq_dbid;
+	u16 rq_dbid;
+	u16 resv0;
+	u32 sq_page_size;
+	u32 rq_page_size;
+	u32 num_sq_pages;
+	u32 num_rq_pages;
+	u64 sq_page_addr[MAX_QP_PAGES];
+	u64 rq_page_addr[MAX_QP_PAGES];
+	u64 db_page_addr;
+	u32 db_page_size;
+	u32 dpp_credit;
+	u32 dpp_offset;
+	u32 rsvd1;
+	u32 num_wqe_allocated;
+	u32 num_rqe_allocated;
+	u32 free_wqe_delta;
+	u32 free_rqe_delta;
+	u32 db_sq_offset;
+	u32 db_rq_offset;
+	u32 db_shift;
+	u64 rsvd2;
+	u64 rsvd3;
+} __packed;
+
+struct ocrdma_create_srq_uresp {
+	u16 rq_dbid;
+	u16 resv0;
+	u32 resv1;
+
+	u32 rq_page_size;
+	u32 num_rq_pages;
+
+	u64 rq_page_addr[MAX_QP_PAGES];
+	u64 db_page_addr;
+
+	u32 db_page_size;
+	u32 num_rqe_allocated;
+	u32 db_rq_offset;
+	u32 db_shift;
+
+	u32 free_rqe_delta;
+	u32 rsvd2;
+	u64 rsvd3;
+} __packed;
+
+#endif				/* __OCRDMA_ABI_H__ */
diff --git a/drivers/infiniband/hw/ocrdma/ocrdma_ah.c b/drivers/infiniband/hw/ocrdma/ocrdma_ah.c
new file mode 100644
index 00000000000..a877a8ed790
--- /dev/null
+++ b/drivers/infiniband/hw/ocrdma/ocrdma_ah.c
@@ -0,0 +1,172 @@
+/*******************************************************************
+ * This file is part of the Emulex RoCE Device Driver for          *
+ * RoCE (RDMA over Converged Ethernet) adapters.                   *
+ * Copyright (C) 2008-2012 Emulex. All rights reserved.            *
+ * EMULEX and SLI are trademarks of Emulex.                        *
+ * www.emulex.com                                                  *
+ *                                                                 *
+ * 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. *
+ * ALL EXPRESS OR IMPLIED CONDITIONS, REPRESENTATIONS AND          *
+ * WARRANTIES, INCLUDING ANY IMPLIED WARRANTY OF MERCHANTABILITY,  *
+ * FITNESS FOR A PARTICULAR PURPOSE, OR NON-INFRINGEMENT, ARE      *
+ * DISCLAIMED, EXCEPT TO THE EXTENT THAT SUCH DISCLAIMERS ARE HELD *
+ * TO BE LEGALLY INVALID.  See the GNU General Public License for  *
+ * more details, a copy of which can be found in the file COPYING  *
+ * included with this package.                                     *
+ *
+ * Contact Information:
+ * linux-drivers@emulex.com
+ *
+ * Emulex
+ * 3333 Susan Street
+ * Costa Mesa, CA 92626
+ *******************************************************************/
+
+#include <net/neighbour.h>
+#include <net/netevent.h>
+
+#include <rdma/ib_addr.h>
+#include <rdma/ib_cache.h>
+
+#include "ocrdma.h"
+#include "ocrdma_verbs.h"
+#include "ocrdma_ah.h"
+#include "ocrdma_hw.h"
+
+static inline int set_av_attr(struct ocrdma_ah *ah,
+				struct ib_ah_attr *attr, int pdid)
+{
+	int status = 0;
+	u16 vlan_tag; bool vlan_enabled = false;
+	struct ocrdma_dev *dev = ah->dev;
+	struct ocrdma_eth_vlan eth;
+	struct ocrdma_grh grh;
+	int eth_sz;
+
+	memset(&eth, 0, sizeof(eth));
+	memset(&grh, 0, sizeof(grh));
+
+	ah->sgid_index = attr->grh.sgid_index;
+
+	vlan_tag = rdma_get_vlan_id(&attr->grh.dgid);
+	if (vlan_tag && (vlan_tag < 0x1000)) {
+		eth.eth_type = cpu_to_be16(0x8100);
+		eth.roce_eth_type = cpu_to_be16(OCRDMA_ROCE_ETH_TYPE);
+		vlan_tag |= (attr->sl & 7) << 13;
+		eth.vlan_tag = cpu_to_be16(vlan_tag);
+		eth_sz = sizeof(struct ocrdma_eth_vlan);
+		vlan_enabled = true;
+	} else {
+		eth.eth_type = cpu_to_be16(OCRDMA_ROCE_ETH_TYPE);
+		eth_sz = sizeof(struct ocrdma_eth_basic);
+	}
+	memcpy(&eth.smac[0], &dev->nic_info.mac_addr[0], ETH_ALEN);
+	status = ocrdma_resolve_dgid(dev, &attr->grh.dgid, &eth.dmac[0]);
+	if (status)
+		return status;
+	status = ocrdma_query_gid(&dev->ibdev, 1, attr->grh.sgid_index,
+			(union ib_gid *)&grh.sgid[0]);
+	if (status)
+		return status;
+
+	grh.tclass_flow = cpu_to_be32((6 << 28) |
+			(attr->grh.traffic_class << 24) |
+			attr->grh.flow_label);
+	/* 0x1b is next header value in GRH */
+	grh.pdid_hoplimit = cpu_to_be32((pdid << 16) |
+			(0x1b << 8) | attr->grh.hop_limit);
+
+	memcpy(&grh.dgid[0], attr->grh.dgid.raw, sizeof(attr->grh.dgid.raw));
+	memcpy(&ah->av->eth_hdr, &eth, eth_sz);
+	memcpy((u8 *)ah->av + eth_sz, &grh, sizeof(struct ocrdma_grh));
+	if (vlan_enabled)
+		ah->av->valid |= OCRDMA_AV_VLAN_VALID;
+	return status;
+}
+
+struct ib_ah *ocrdma_create_ah(struct ib_pd *ibpd, struct ib_ah_attr *attr)
+{
+	u32 *ahid_addr;
+	int status;
+	struct ocrdma_ah *ah;
+	struct ocrdma_pd *pd = get_ocrdma_pd(ibpd);
+	struct ocrdma_dev *dev = pd->dev;
+
+	if (!(attr->ah_flags & IB_AH_GRH))
+		return ERR_PTR(-EINVAL);
+
+	ah = kzalloc(sizeof *ah, GFP_ATOMIC);
+	if (!ah)
+		return ERR_PTR(-ENOMEM);
+	ah->dev = pd->dev;
+
+	status = ocrdma_alloc_av(dev, ah);
+	if (status)
+		goto av_err;
+	status = set_av_attr(ah, attr, pd->id);
+	if (status)
+		goto av_conf_err;
+
+	/* if pd is for the user process, pass the ah_id to user space */
+	if ((pd->uctx) && (pd->uctx->ah_tbl.va)) {
+		ahid_addr = pd->uctx->ah_tbl.va + attr->dlid;
+		*ahid_addr = ah->id;
+	}
+	return &ah->ibah;
+
+av_conf_err:
+	ocrdma_free_av(dev, ah);
+av_err:
+	kfree(ah);
+	return ERR_PTR(status);
+}
+
+int ocrdma_destroy_ah(struct ib_ah *ibah)
+{
+	struct ocrdma_ah *ah = get_ocrdma_ah(ibah);
+	ocrdma_free_av(ah->dev, ah);
+	kfree(ah);
+	return 0;
+}
+
+int ocrdma_query_ah(struct ib_ah *ibah, struct ib_ah_attr *attr)
+{
+	struct ocrdma_ah *ah = get_ocrdma_ah(ibah);
+	struct ocrdma_av *av = ah->av;
+	struct ocrdma_grh *grh;
+	attr->ah_flags |= IB_AH_GRH;
+	if (ah->av->valid & Bit(1)) {
+		grh = (struct ocrdma_grh *)((u8 *)ah->av +
+				sizeof(struct ocrdma_eth_vlan));
+		attr->sl = be16_to_cpu(av->eth_hdr.vlan_tag) >> 13;
+	} else {
+		grh = (struct ocrdma_grh *)((u8 *)ah->av +
+					sizeof(struct ocrdma_eth_basic));
+		attr->sl = 0;
+	}
+	memcpy(&attr->grh.dgid.raw[0], &grh->dgid[0], sizeof(grh->dgid));
+	attr->grh.sgid_index = ah->sgid_index;
+	attr->grh.hop_limit = be32_to_cpu(grh->pdid_hoplimit) & 0xff;
+	attr->grh.traffic_class = be32_to_cpu(grh->tclass_flow) >> 24;
+	attr->grh.flow_label = be32_to_cpu(grh->tclass_flow) & 0x00ffffffff;
+	return 0;
+}
+
+int ocrdma_modify_ah(struct ib_ah *ibah, struct ib_ah_attr *attr)
+{
+	/* modify_ah is unsupported */
+	return -ENOSYS;
+}
+
+int ocrdma_process_mad(struct ib_device *ibdev,
+		       int process_mad_flags,
+		       u8 port_num,
+		       struct ib_wc *in_wc,
+		       struct ib_grh *in_grh,
+		       struct ib_mad *in_mad, struct ib_mad *out_mad)
+{
+	return IB_MAD_RESULT_SUCCESS;
+}
diff --git a/drivers/infiniband/hw/ocrdma/ocrdma_ah.h b/drivers/infiniband/hw/ocrdma/ocrdma_ah.h
new file mode 100644
index 00000000000..8ac49e7f96d
--- /dev/null
+++ b/drivers/infiniband/hw/ocrdma/ocrdma_ah.h
@@ -0,0 +1,42 @@
+/*******************************************************************
+ * This file is part of the Emulex RoCE Device Driver for          *
+ * RoCE (RDMA over Converged Ethernet) adapters.                   *
+ * Copyright (C) 2008-2012 Emulex. All rights reserved.            *
+ * EMULEX and SLI are trademarks of Emulex.                        *
+ * www.emulex.com                                                  *
+ *                                                                 *
+ * 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. *
+ * ALL EXPRESS OR IMPLIED CONDITIONS, REPRESENTATIONS AND          *
+ * WARRANTIES, INCLUDING ANY IMPLIED WARRANTY OF MERCHANTABILITY,  *
+ * FITNESS FOR A PARTICULAR PURPOSE, OR NON-INFRINGEMENT, ARE      *
+ * DISCLAIMED, EXCEPT TO THE EXTENT THAT SUCH DISCLAIMERS ARE HELD *
+ * TO BE LEGALLY INVALID.  See the GNU General Public License for  *
+ * more details, a copy of which can be found in the file COPYING  *
+ * included with this package.                                     *
+ *
+ * Contact Information:
+ * linux-drivers@emulex.com
+ *
+ * Emulex
+ * 3333 Susan Street
+ * Costa Mesa, CA 92626
+ *******************************************************************/
+
+#ifndef __OCRDMA_AH_H__
+#define __OCRDMA_AH_H__
+
+struct ib_ah *ocrdma_create_ah(struct ib_pd *, struct ib_ah_attr *);
+int ocrdma_destroy_ah(struct ib_ah *);
+int ocrdma_query_ah(struct ib_ah *, struct ib_ah_attr *);
+int ocrdma_modify_ah(struct ib_ah *, struct ib_ah_attr *);
+
+int ocrdma_process_mad(struct ib_device *,
+		       int process_mad_flags,
+		       u8 port_num,
+		       struct ib_wc *in_wc,
+		       struct ib_grh *in_grh,
+		       struct ib_mad *in_mad, struct ib_mad *out_mad);
+#endif				/* __OCRDMA_AH_H__ */
diff --git a/drivers/infiniband/hw/ocrdma/ocrdma_hw.c b/drivers/infiniband/hw/ocrdma/ocrdma_hw.c
new file mode 100644
index 00000000000..9b204b1ba33
--- /dev/null
+++ b/drivers/infiniband/hw/ocrdma/ocrdma_hw.c
@@ -0,0 +1,2640 @@
+/*******************************************************************
+ * This file is part of the Emulex RoCE Device Driver for          *
+ * RoCE (RDMA over Converged Ethernet) CNA Adapters.              *
+ * Copyright (C) 2008-2012 Emulex. All rights reserved.            *
+ * EMULEX and SLI are trademarks of Emulex.                        *
+ * www.emulex.com                                                  *
+ *                                                                 *
+ * 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. *
+ * ALL EXPRESS OR IMPLIED CONDITIONS, REPRESENTATIONS AND          *
+ * WARRANTIES, INCLUDING ANY IMPLIED WARRANTY OF MERCHANTABILITY,  *
+ * FITNESS FOR A PARTICULAR PURPOSE, OR NON-INFRINGEMENT, ARE      *
+ * DISCLAIMED, EXCEPT TO THE EXTENT THAT SUCH DISCLAIMERS ARE HELD *
+ * TO BE LEGALLY INVALID.  See the GNU General Public License for  *
+ * more details, a copy of which can be found in the file COPYING  *
+ * included with this package.                                     *
+ *
+ * Contact Information:
+ * linux-drivers@emulex.com
+ *
+ * Emulex
+ * 3333 Susan Street
+ * Costa Mesa, CA 92626
+ *******************************************************************/
+
+#include <linux/sched.h>
+#include <linux/interrupt.h>
+#include <linux/log2.h>
+#include <linux/dma-mapping.h>
+
+#include <rdma/ib_verbs.h>
+#include <rdma/ib_user_verbs.h>
+#include <rdma/ib_addr.h>
+
+#include "ocrdma.h"
+#include "ocrdma_hw.h"
+#include "ocrdma_verbs.h"
+#include "ocrdma_ah.h"
+
+enum mbx_status {
+	OCRDMA_MBX_STATUS_FAILED		= 1,
+	OCRDMA_MBX_STATUS_ILLEGAL_FIELD		= 3,
+	OCRDMA_MBX_STATUS_OOR			= 100,
+	OCRDMA_MBX_STATUS_INVALID_PD		= 101,
+	OCRDMA_MBX_STATUS_PD_INUSE		= 102,
+	OCRDMA_MBX_STATUS_INVALID_CQ		= 103,
+	OCRDMA_MBX_STATUS_INVALID_QP		= 104,
+	OCRDMA_MBX_STATUS_INVALID_LKEY		= 105,
+	OCRDMA_MBX_STATUS_ORD_EXCEEDS		= 106,
+	OCRDMA_MBX_STATUS_IRD_EXCEEDS		= 107,
+	OCRDMA_MBX_STATUS_SENDQ_WQE_EXCEEDS	= 108,
+	OCRDMA_MBX_STATUS_RECVQ_RQE_EXCEEDS	= 109,
+	OCRDMA_MBX_STATUS_SGE_SEND_EXCEEDS	= 110,
+	OCRDMA_MBX_STATUS_SGE_WRITE_EXCEEDS	= 111,
+	OCRDMA_MBX_STATUS_SGE_RECV_EXCEEDS	= 112,
+	OCRDMA_MBX_STATUS_INVALID_STATE_CHANGE	= 113,
+	OCRDMA_MBX_STATUS_MW_BOUND		= 114,
+	OCRDMA_MBX_STATUS_INVALID_VA		= 115,
+	OCRDMA_MBX_STATUS_INVALID_LENGTH	= 116,
+	OCRDMA_MBX_STATUS_INVALID_FBO		= 117,
+	OCRDMA_MBX_STATUS_INVALID_ACC_RIGHTS	= 118,
+	OCRDMA_MBX_STATUS_INVALID_PBE_SIZE	= 119,
+	OCRDMA_MBX_STATUS_INVALID_PBL_ENTRY	= 120,
+	OCRDMA_MBX_STATUS_INVALID_PBL_SHIFT	= 121,
+	OCRDMA_MBX_STATUS_INVALID_SRQ_ID	= 129,
+	OCRDMA_MBX_STATUS_SRQ_ERROR		= 133,
+	OCRDMA_MBX_STATUS_RQE_EXCEEDS		= 134,
+	OCRDMA_MBX_STATUS_MTU_EXCEEDS		= 135,
+	OCRDMA_MBX_STATUS_MAX_QP_EXCEEDS	= 136,
+	OCRDMA_MBX_STATUS_SRQ_LIMIT_EXCEEDS	= 137,
+	OCRDMA_MBX_STATUS_SRQ_SIZE_UNDERUNS	= 138,
+	OCRDMA_MBX_STATUS_QP_BOUND		= 130,
+	OCRDMA_MBX_STATUS_INVALID_CHANGE	= 139,
+	OCRDMA_MBX_STATUS_ATOMIC_OPS_UNSUP	= 140,
+	OCRDMA_MBX_STATUS_INVALID_RNR_NAK_TIMER	= 141,
+	OCRDMA_MBX_STATUS_MW_STILL_BOUND	= 142,
+	OCRDMA_MBX_STATUS_PKEY_INDEX_INVALID	= 143,
+	OCRDMA_MBX_STATUS_PKEY_INDEX_EXCEEDS	= 144
+};
+
+enum additional_status {
+	OCRDMA_MBX_ADDI_STATUS_INSUFFICIENT_RESOURCES = 22
+};
+
+enum cqe_status {
+	OCRDMA_MBX_CQE_STATUS_INSUFFICIENT_PRIVILEDGES	= 1,
+	OCRDMA_MBX_CQE_STATUS_INVALID_PARAMETER		= 2,
+	OCRDMA_MBX_CQE_STATUS_INSUFFICIENT_RESOURCES	= 3,
+	OCRDMA_MBX_CQE_STATUS_QUEUE_FLUSHING		= 4,
+	OCRDMA_MBX_CQE_STATUS_DMA_FAILED		= 5
+};
+
+static inline void *ocrdma_get_eqe(struct ocrdma_eq *eq)
+{
+	return (u8 *)eq->q.va + (eq->q.tail * sizeof(struct ocrdma_eqe));
+}
+
+static inline void ocrdma_eq_inc_tail(struct ocrdma_eq *eq)
+{
+	eq->q.tail = (eq->q.tail + 1) & (OCRDMA_EQ_LEN - 1);
+}
+
+static inline void *ocrdma_get_mcqe(struct ocrdma_dev *dev)
+{
+	struct ocrdma_mcqe *cqe = (struct ocrdma_mcqe *)
+	    ((u8 *) dev->mq.cq.va +
+	     (dev->mq.cq.tail * sizeof(struct ocrdma_mcqe)));
+
+	if (!(le32_to_cpu(cqe->valid_ae_cmpl_cons) & OCRDMA_MCQE_VALID_MASK))
+		return NULL;
+	return cqe;
+}
+
+static inline void ocrdma_mcq_inc_tail(struct ocrdma_dev *dev)
+{
+	dev->mq.cq.tail = (dev->mq.cq.tail + 1) & (OCRDMA_MQ_CQ_LEN - 1);
+}
+
+static inline struct ocrdma_mqe *ocrdma_get_mqe(struct ocrdma_dev *dev)
+{
+	return (struct ocrdma_mqe *)((u8 *) dev->mq.sq.va +
+				     (dev->mq.sq.head *
+				      sizeof(struct ocrdma_mqe)));
+}
+
+static inline void ocrdma_mq_inc_head(struct ocrdma_dev *dev)
+{
+	dev->mq.sq.head = (dev->mq.sq.head + 1) & (OCRDMA_MQ_LEN - 1);
+	atomic_inc(&dev->mq.sq.used);
+}
+
+static inline void *ocrdma_get_mqe_rsp(struct ocrdma_dev *dev)
+{
+	return (void *)((u8 *) dev->mq.sq.va +
+			(dev->mqe_ctx.tag * sizeof(struct ocrdma_mqe)));
+}
+
+enum ib_qp_state get_ibqp_state(enum ocrdma_qp_state qps)
+{
+	switch (qps) {
+	case OCRDMA_QPS_RST:
+		return IB_QPS_RESET;
+	case OCRDMA_QPS_INIT:
+		return IB_QPS_INIT;
+	case OCRDMA_QPS_RTR:
+		return IB_QPS_RTR;
+	case OCRDMA_QPS_RTS:
+		return IB_QPS_RTS;
+	case OCRDMA_QPS_SQD:
+	case OCRDMA_QPS_SQ_DRAINING:
+		return IB_QPS_SQD;
+	case OCRDMA_QPS_SQE:
+		return IB_QPS_SQE;
+	case OCRDMA_QPS_ERR:
+		return IB_QPS_ERR;
+	};
+	return IB_QPS_ERR;
+}
+
+static enum ocrdma_qp_state get_ocrdma_qp_state(enum ib_qp_state qps)
+{
+	switch (qps) {
+	case IB_QPS_RESET:
+		return OCRDMA_QPS_RST;
+	case IB_QPS_INIT:
+		return OCRDMA_QPS_INIT;
+	case IB_QPS_RTR:
+		return OCRDMA_QPS_RTR;
+	case IB_QPS_RTS:
+		return OCRDMA_QPS_RTS;
+	case IB_QPS_SQD:
+		return OCRDMA_QPS_SQD;
+	case IB_QPS_SQE:
+		return OCRDMA_QPS_SQE;
+	case IB_QPS_ERR:
+		return OCRDMA_QPS_ERR;
+	};
+	return OCRDMA_QPS_ERR;
+}
+
+static int ocrdma_get_mbx_errno(u32 status)
+{
+	int err_num = -EFAULT;
+	u8 mbox_status = (status & OCRDMA_MBX_RSP_STATUS_MASK) >>
+					OCRDMA_MBX_RSP_STATUS_SHIFT;
+	u8 add_status = (status & OCRDMA_MBX_RSP_ASTATUS_MASK) >>
+					OCRDMA_MBX_RSP_ASTATUS_SHIFT;
+
+	switch (mbox_status) {
+	case OCRDMA_MBX_STATUS_OOR:
+	case OCRDMA_MBX_STATUS_MAX_QP_EXCEEDS:
+		err_num = -EAGAIN;
+		break;
+
+	case OCRDMA_MBX_STATUS_INVALID_PD:
+	case OCRDMA_MBX_STATUS_INVALID_CQ:
+	case OCRDMA_MBX_STATUS_INVALID_SRQ_ID:
+	case OCRDMA_MBX_STATUS_INVALID_QP:
+	case OCRDMA_MBX_STATUS_INVALID_CHANGE:
+	case OCRDMA_MBX_STATUS_MTU_EXCEEDS:
+	case OCRDMA_MBX_STATUS_INVALID_RNR_NAK_TIMER:
+	case OCRDMA_MBX_STATUS_PKEY_INDEX_INVALID:
+	case OCRDMA_MBX_STATUS_PKEY_INDEX_EXCEEDS:
+	case OCRDMA_MBX_STATUS_ILLEGAL_FIELD:
+	case OCRDMA_MBX_STATUS_INVALID_PBL_ENTRY:
+	case OCRDMA_MBX_STATUS_INVALID_LKEY:
+	case OCRDMA_MBX_STATUS_INVALID_VA:
+	case OCRDMA_MBX_STATUS_INVALID_LENGTH:
+	case OCRDMA_MBX_STATUS_INVALID_FBO:
+	case OCRDMA_MBX_STATUS_INVALID_ACC_RIGHTS:
+	case OCRDMA_MBX_STATUS_INVALID_PBE_SIZE:
+	case OCRDMA_MBX_STATUS_ATOMIC_OPS_UNSUP:
+	case OCRDMA_MBX_STATUS_SRQ_ERROR:
+	case OCRDMA_MBX_STATUS_SRQ_SIZE_UNDERUNS:
+		err_num = -EINVAL;
+		break;
+
+	case OCRDMA_MBX_STATUS_PD_INUSE:
+	case OCRDMA_MBX_STATUS_QP_BOUND:
+	case OCRDMA_MBX_STATUS_MW_STILL_BOUND:
+	case OCRDMA_MBX_STATUS_MW_BOUND:
+		err_num = -EBUSY;
+		break;
+
+	case OCRDMA_MBX_STATUS_RECVQ_RQE_EXCEEDS:
+	case OCRDMA_MBX_STATUS_SGE_RECV_EXCEEDS:
+	case OCRDMA_MBX_STATUS_RQE_EXCEEDS:
+	case OCRDMA_MBX_STATUS_SRQ_LIMIT_EXCEEDS:
+	case OCRDMA_MBX_STATUS_ORD_EXCEEDS:
+	case OCRDMA_MBX_STATUS_IRD_EXCEEDS:
+	case OCRDMA_MBX_STATUS_SENDQ_WQE_EXCEEDS:
+	case OCRDMA_MBX_STATUS_SGE_SEND_EXCEEDS:
+	case OCRDMA_MBX_STATUS_SGE_WRITE_EXCEEDS:
+		err_num = -ENOBUFS;
+		break;
+
+	case OCRDMA_MBX_STATUS_FAILED:
+		switch (add_status) {
+		case OCRDMA_MBX_ADDI_STATUS_INSUFFICIENT_RESOURCES:
+			err_num = -EAGAIN;
+			break;
+		}
+	default:
+		err_num = -EFAULT;
+	}
+	return err_num;
+}
+
+static int ocrdma_get_mbx_cqe_errno(u16 cqe_status)
+{
+	int err_num = -EINVAL;
+
+	switch (cqe_status) {
+	case OCRDMA_MBX_CQE_STATUS_INSUFFICIENT_PRIVILEDGES:
+		err_num = -EPERM;
+		break;
+	case OCRDMA_MBX_CQE_STATUS_INVALID_PARAMETER:
+		err_num = -EINVAL;
+		break;
+	case OCRDMA_MBX_CQE_STATUS_INSUFFICIENT_RESOURCES:
+	case OCRDMA_MBX_CQE_STATUS_QUEUE_FLUSHING:
+		err_num = -EAGAIN;
+		break;
+	case OCRDMA_MBX_CQE_STATUS_DMA_FAILED:
+		err_num = -EIO;
+		break;
+	}
+	return err_num;
+}
+
+void ocrdma_ring_cq_db(struct ocrdma_dev *dev, u16 cq_id, bool armed,
+		       bool solicited, u16 cqe_popped)
+{
+	u32 val = cq_id & OCRDMA_DB_CQ_RING_ID_MASK;
+
+	val |= ((cq_id & OCRDMA_DB_CQ_RING_ID_EXT_MASK) <<
+	     OCRDMA_DB_CQ_RING_ID_EXT_MASK_SHIFT);
+
+	if (armed)
+		val |= (1 << OCRDMA_DB_CQ_REARM_SHIFT);
+	if (solicited)
+		val |= (1 << OCRDMA_DB_CQ_SOLICIT_SHIFT);
+	val |= (cqe_popped << OCRDMA_DB_CQ_NUM_POPPED_SHIFT);
+	iowrite32(val, dev->nic_info.db + OCRDMA_DB_CQ_OFFSET);
+}
+
+static void ocrdma_ring_mq_db(struct ocrdma_dev *dev)
+{
+	u32 val = 0;
+
+	val |= dev->mq.sq.id & OCRDMA_MQ_ID_MASK;
+	val |= 1 << OCRDMA_MQ_NUM_MQE_SHIFT;
+	iowrite32(val, dev->nic_info.db + OCRDMA_DB_MQ_OFFSET);
+}
+
+static void ocrdma_ring_eq_db(struct ocrdma_dev *dev, u16 eq_id,
+			      bool arm, bool clear_int, u16 num_eqe)
+{
+	u32 val = 0;
+
+	val |= eq_id & OCRDMA_EQ_ID_MASK;
+	val |= ((eq_id & OCRDMA_EQ_ID_EXT_MASK) << OCRDMA_EQ_ID_EXT_MASK_SHIFT);
+	if (arm)
+		val |= (1 << OCRDMA_REARM_SHIFT);
+	if (clear_int)
+		val |= (1 << OCRDMA_EQ_CLR_SHIFT);
+	val |= (1 << OCRDMA_EQ_TYPE_SHIFT);
+	val |= (num_eqe << OCRDMA_NUM_EQE_SHIFT);
+	iowrite32(val, dev->nic_info.db + OCRDMA_DB_EQ_OFFSET);
+}
+
+static void ocrdma_init_mch(struct ocrdma_mbx_hdr *cmd_hdr,
+			    u8 opcode, u8 subsys, u32 cmd_len)
+{
+	cmd_hdr->subsys_op = (opcode | (subsys << OCRDMA_MCH_SUBSYS_SHIFT));
+	cmd_hdr->timeout = 20; /* seconds */
+	cmd_hdr->cmd_len = cmd_len - sizeof(struct ocrdma_mbx_hdr);
+}
+
+static void *ocrdma_init_emb_mqe(u8 opcode, u32 cmd_len)
+{
+	struct ocrdma_mqe *mqe;
+
+	mqe = kzalloc(sizeof(struct ocrdma_mqe), GFP_KERNEL);
+	if (!mqe)
+		return NULL;
+	mqe->hdr.spcl_sge_cnt_emb |=
+		(OCRDMA_MQE_EMBEDDED << OCRDMA_MQE_HDR_EMB_SHIFT) &
+					OCRDMA_MQE_HDR_EMB_MASK;
+	mqe->hdr.pyld_len = cmd_len - sizeof(struct ocrdma_mqe_hdr);
+
+	ocrdma_init_mch(&mqe->u.emb_req.mch, opcode, OCRDMA_SUBSYS_ROCE,
+			mqe->hdr.pyld_len);
+	return mqe;
+}
+
+static void ocrdma_free_q(struct ocrdma_dev *dev, struct ocrdma_queue_info *q)
+{
+	dma_free_coherent(&dev->nic_info.pdev->dev, q->size, q->va, q->dma);
+}
+
+static int ocrdma_alloc_q(struct ocrdma_dev *dev,
+			  struct ocrdma_queue_info *q, u16 len, u16 entry_size)
+{
+	memset(q, 0, sizeof(*q));
+	q->len = len;
+	q->entry_size = entry_size;
+	q->size = len * entry_size;
+	q->va = dma_alloc_coherent(&dev->nic_info.pdev->dev, q->size,
+				   &q->dma, GFP_KERNEL);
+	if (!q->va)
+		return -ENOMEM;
+	memset(q->va, 0, q->size);
+	return 0;
+}
+
+static void ocrdma_build_q_pages(struct ocrdma_pa *q_pa, int cnt,
+					dma_addr_t host_pa, int hw_page_size)
+{
+	int i;
+
+	for (i = 0; i < cnt; i++) {
+		q_pa[i].lo = (u32) (host_pa & 0xffffffff);
+		q_pa[i].hi = (u32) upper_32_bits(host_pa);
+		host_pa += hw_page_size;
+	}
+}
+
+static void ocrdma_assign_eq_vect_gen2(struct ocrdma_dev *dev,
+				       struct ocrdma_eq *eq)
+{
+	/* assign vector and update vector id for next EQ */
+	eq->vector = dev->nic_info.msix.start_vector;
+	dev->nic_info.msix.start_vector += 1;
+}
+
+static void ocrdma_free_eq_vect_gen2(struct ocrdma_dev *dev)
+{
+	/* this assumes that EQs are freed in exactly reverse order
+	 * as its allocation.
+	 */
+	dev->nic_info.msix.start_vector -= 1;
+}
+
+static int ocrdma_mbx_delete_q(struct ocrdma_dev *dev, struct ocrdma_queue_info *q,
+			       int queue_type)
+{
+	u8 opcode = 0;
+	int status;
+	struct ocrdma_delete_q_req *cmd = dev->mbx_cmd;
+
+	switch (queue_type) {
+	case QTYPE_MCCQ:
+		opcode = OCRDMA_CMD_DELETE_MQ;
+		break;
+	case QTYPE_CQ:
+		opcode = OCRDMA_CMD_DELETE_CQ;
+		break;
+	case QTYPE_EQ:
+		opcode = OCRDMA_CMD_DELETE_EQ;
+		break;
+	default:
+		BUG();
+	}
+	memset(cmd, 0, sizeof(*cmd));
+	ocrdma_init_mch(&cmd->req, opcode, OCRDMA_SUBSYS_COMMON, sizeof(*cmd));
+	cmd->id = q->id;
+
+	status = be_roce_mcc_cmd(dev->nic_info.netdev,
+				 cmd, sizeof(*cmd), NULL, NULL);
+	if (!status)
+		q->created = false;
+	return status;
+}
+
+static int ocrdma_mbx_create_eq(struct ocrdma_dev *dev, struct ocrdma_eq *eq)
+{
+	int status;
+	struct ocrdma_create_eq_req *cmd = dev->mbx_cmd;
+	struct ocrdma_create_eq_rsp *rsp = dev->mbx_cmd;
+
+	memset(cmd, 0, sizeof(*cmd));
+	ocrdma_init_mch(&cmd->req, OCRDMA_CMD_CREATE_EQ, OCRDMA_SUBSYS_COMMON,
+			sizeof(*cmd));
+	if (dev->nic_info.dev_family == OCRDMA_GEN2_FAMILY)
+		cmd->req.rsvd_version = 0;
+	else
+		cmd->req.rsvd_version = 2;
+
+	cmd->num_pages = 4;
+	cmd->valid = OCRDMA_CREATE_EQ_VALID;
+	cmd->cnt = 4 << OCRDMA_CREATE_EQ_CNT_SHIFT;
+
+	ocrdma_build_q_pages(&cmd->pa[0], cmd->num_pages, eq->q.dma,
+			     PAGE_SIZE_4K);
+	status = be_roce_mcc_cmd(dev->nic_info.netdev, cmd, sizeof(*cmd), NULL,
+				 NULL);
+	if (!status) {
+		eq->q.id = rsp->vector_eqid & 0xffff;
+		if (dev->nic_info.dev_family == OCRDMA_GEN2_FAMILY)
+			ocrdma_assign_eq_vect_gen2(dev, eq);
+		else {
+			eq->vector = (rsp->vector_eqid >> 16) & 0xffff;
+			dev->nic_info.msix.start_vector += 1;
+		}
+		eq->q.created = true;
+	}
+	return status;
+}
+
+static int ocrdma_create_eq(struct ocrdma_dev *dev,
+			    struct ocrdma_eq *eq, u16 q_len)
+{
+	int status;
+
+	status = ocrdma_alloc_q(dev, &eq->q, OCRDMA_EQ_LEN,
+				sizeof(struct ocrdma_eqe));
+	if (status)
+		return status;
+
+	status = ocrdma_mbx_create_eq(dev, eq);
+	if (status)
+		goto mbx_err;
+	eq->dev = dev;
+	ocrdma_ring_eq_db(dev, eq->q.id, true, true, 0);
+
+	return 0;
+mbx_err:
+	ocrdma_free_q(dev, &eq->q);
+	return status;
+}
+
+static int ocrdma_get_irq(struct ocrdma_dev *dev, struct ocrdma_eq *eq)
+{
+	int irq;
+
+	if (dev->nic_info.intr_mode == BE_INTERRUPT_MODE_INTX)
+		irq = dev->nic_info.pdev->irq;
+	else
+		irq = dev->nic_info.msix.vector_list[eq->vector];
+	return irq;
+}
+
+static void _ocrdma_destroy_eq(struct ocrdma_dev *dev, struct ocrdma_eq *eq)
+{
+	if (eq->q.created) {
+		ocrdma_mbx_delete_q(dev, &eq->q, QTYPE_EQ);
+		if (dev->nic_info.dev_family == OCRDMA_GEN2_FAMILY)
+			ocrdma_free_eq_vect_gen2(dev);
+		ocrdma_free_q(dev, &eq->q);
+	}
+}
+
+static void ocrdma_destroy_eq(struct ocrdma_dev *dev, struct ocrdma_eq *eq)
+{
+	int irq;
+
+	/* disarm EQ so that interrupts are not generated
+	 * during freeing and EQ delete is in progress.
+	 */
+	ocrdma_ring_eq_db(dev, eq->q.id, false, false, 0);
+
+	irq = ocrdma_get_irq(dev, eq);
+	free_irq(irq, eq);
+	_ocrdma_destroy_eq(dev, eq);
+}
+
+static void ocrdma_destroy_qp_eqs(struct ocrdma_dev *dev)
+{
+	int i;
+
+	/* deallocate the data path eqs */
+	for (i = 0; i < dev->eq_cnt; i++)
+		ocrdma_destroy_eq(dev, &dev->qp_eq_tbl[i]);
+}
+
+static int ocrdma_mbx_mq_cq_create(struct ocrdma_dev *dev,
+				   struct ocrdma_queue_info *cq,
+				   struct ocrdma_queue_info *eq)
+{
+	struct ocrdma_create_cq_cmd *cmd = dev->mbx_cmd;
+	struct ocrdma_create_cq_cmd_rsp *rsp = dev->mbx_cmd;
+	int status;
+
+	memset(cmd, 0, sizeof(*cmd));
+	ocrdma_init_mch(&cmd->req, OCRDMA_CMD_CREATE_CQ,
+			OCRDMA_SUBSYS_COMMON, sizeof(*cmd));
+
+	cmd->pgsz_pgcnt = PAGES_4K_SPANNED(cq->va, cq->size);
+	cmd->ev_cnt_flags = OCRDMA_CREATE_CQ_DEF_FLAGS;
+	cmd->eqn = (eq->id << OCRDMA_CREATE_CQ_EQID_SHIFT);
+
+	ocrdma_build_q_pages(&cmd->pa[0], cmd->pgsz_pgcnt,
+			     cq->dma, PAGE_SIZE_4K);
+	status = be_roce_mcc_cmd(dev->nic_info.netdev,
+				 cmd, sizeof(*cmd), NULL, NULL);
+	if (!status) {
+		cq->id = (rsp->cq_id & OCRDMA_CREATE_CQ_RSP_CQ_ID_MASK);
+		cq->created = true;
+	}
+	return status;
+}
+
+static u32 ocrdma_encoded_q_len(int q_len)
+{
+	u32 len_encoded = fls(q_len);	/* log2(len) + 1 */
+
+	if (len_encoded == 16)
+		len_encoded = 0;
+	return len_encoded;
+}
+
+static int ocrdma_mbx_create_mq(struct ocrdma_dev *dev,
+				struct ocrdma_queue_info *mq,
+				struct ocrdma_queue_info *cq)
+{
+	int num_pages, status;
+	struct ocrdma_create_mq_req *cmd = dev->mbx_cmd;
+	struct ocrdma_create_mq_rsp *rsp = dev->mbx_cmd;
+	struct ocrdma_pa *pa;
+
+	memset(cmd, 0, sizeof(*cmd));
+	num_pages = PAGES_4K_SPANNED(mq->va, mq->size);
+
+	if (dev->nic_info.dev_family == OCRDMA_GEN2_FAMILY) {
+		ocrdma_init_mch(&cmd->req, OCRDMA_CMD_CREATE_MQ,
+				OCRDMA_SUBSYS_COMMON, sizeof(*cmd));
+		cmd->v0.pages = num_pages;
+		cmd->v0.async_cqid_valid = OCRDMA_CREATE_MQ_ASYNC_CQ_VALID;
+		cmd->v0.async_cqid_valid = (cq->id << 1);
+		cmd->v0.cqid_ringsize |= (ocrdma_encoded_q_len(mq->len) <<
+					     OCRDMA_CREATE_MQ_RING_SIZE_SHIFT);
+		cmd->v0.cqid_ringsize |=
+			(cq->id << OCRDMA_CREATE_MQ_V0_CQ_ID_SHIFT);
+		cmd->v0.valid = OCRDMA_CREATE_MQ_VALID;
+		pa = &cmd->v0.pa[0];
+	} else {
+		ocrdma_init_mch(&cmd->req, OCRDMA_CMD_CREATE_MQ_EXT,
+				OCRDMA_SUBSYS_COMMON, sizeof(*cmd));
+		cmd->req.rsvd_version = 1;
+		cmd->v1.cqid_pages = num_pages;
+		cmd->v1.cqid_pages |= (cq->id << OCRDMA_CREATE_MQ_CQ_ID_SHIFT);
+		cmd->v1.async_cqid_valid = OCRDMA_CREATE_MQ_ASYNC_CQ_VALID;
+		cmd->v1.async_event_bitmap = Bit(20);
+		cmd->v1.async_cqid_ringsize = cq->id;
+		cmd->v1.async_cqid_ringsize |= (ocrdma_encoded_q_len(mq->len) <<
+					     OCRDMA_CREATE_MQ_RING_SIZE_SHIFT);
+		cmd->v1.valid = OCRDMA_CREATE_MQ_VALID;
+		pa = &cmd->v1.pa[0];
+	}
+	ocrdma_build_q_pages(pa, num_pages, mq->dma, PAGE_SIZE_4K);
+	status = be_roce_mcc_cmd(dev->nic_info.netdev,
+				 cmd, sizeof(*cmd), NULL, NULL);
+	if (!status) {
+		mq->id = rsp->id;
+		mq->created = true;
+	}
+	return status;
+}
+
+static int ocrdma_create_mq(struct ocrdma_dev *dev)
+{
+	int status;
+
+	/* Alloc completion queue for Mailbox queue */
+	status = ocrdma_alloc_q(dev, &dev->mq.cq, OCRDMA_MQ_CQ_LEN,
+				sizeof(struct ocrdma_mcqe));
+	if (status)
+		goto alloc_err;
+
+	status = ocrdma_mbx_mq_cq_create(dev, &dev->mq.cq, &dev->meq.q);
+	if (status)
+		goto mbx_cq_free;
+
+	memset(&dev->mqe_ctx, 0, sizeof(dev->mqe_ctx));
+	init_waitqueue_head(&dev->mqe_ctx.cmd_wait);
+	mutex_init(&dev->mqe_ctx.lock);
+
+	/* Alloc Mailbox queue */
+	status = ocrdma_alloc_q(dev, &dev->mq.sq, OCRDMA_MQ_LEN,
+				sizeof(struct ocrdma_mqe));
+	if (status)
+		goto mbx_cq_destroy;
+	status = ocrdma_mbx_create_mq(dev, &dev->mq.sq, &dev->mq.cq);
+	if (status)
+		goto mbx_q_free;
+	ocrdma_ring_cq_db(dev, dev->mq.cq.id, true, false, 0);
+	return 0;
+
+mbx_q_free:
+	ocrdma_free_q(dev, &dev->mq.sq);
+mbx_cq_destroy:
+	ocrdma_mbx_delete_q(dev, &dev->mq.cq, QTYPE_CQ);
+mbx_cq_free:
+	ocrdma_free_q(dev, &dev->mq.cq);
+alloc_err:
+	return status;
+}
+
+static void ocrdma_destroy_mq(struct ocrdma_dev *dev)
+{
+	struct ocrdma_queue_info *mbxq, *cq;
+
+	/* mqe_ctx lock synchronizes with any other pending cmds. */
+	mutex_lock(&dev->mqe_ctx.lock);
+	mbxq = &dev->mq.sq;
+	if (mbxq->created) {
+		ocrdma_mbx_delete_q(dev, mbxq, QTYPE_MCCQ);
+		ocrdma_free_q(dev, mbxq);
+	}
+	mutex_unlock(&dev->mqe_ctx.lock);
+
+	cq = &dev->mq.cq;
+	if (cq->created) {
+		ocrdma_mbx_delete_q(dev, cq, QTYPE_CQ);
+		ocrdma_free_q(dev, cq);
+	}
+}
+
+static void ocrdma_process_qpcat_error(struct ocrdma_dev *dev,
+				       struct ocrdma_qp *qp)
+{
+	enum ib_qp_state new_ib_qps = IB_QPS_ERR;
+	enum ib_qp_state old_ib_qps;
+
+	if (qp == NULL)
+		BUG();
+	ocrdma_qp_state_machine(qp, new_ib_qps, &old_ib_qps);
+}
+
+static void ocrdma_dispatch_ibevent(struct ocrdma_dev *dev,
+				    struct ocrdma_ae_mcqe *cqe)
+{
+	struct ocrdma_qp *qp = NULL;
+	struct ocrdma_cq *cq = NULL;
+	struct ib_event ib_evt;
+	int cq_event = 0;
+	int qp_event = 1;
+	int srq_event = 0;
+	int dev_event = 0;
+	int type = (cqe->valid_ae_event & OCRDMA_AE_MCQE_EVENT_TYPE_MASK) >>
+	    OCRDMA_AE_MCQE_EVENT_TYPE_SHIFT;
+
+	if (cqe->qpvalid_qpid & OCRDMA_AE_MCQE_QPVALID)
+		qp = dev->qp_tbl[cqe->qpvalid_qpid & OCRDMA_AE_MCQE_QPID_MASK];
+	if (cqe->cqvalid_cqid & OCRDMA_AE_MCQE_CQVALID)
+		cq = dev->cq_tbl[cqe->cqvalid_cqid & OCRDMA_AE_MCQE_CQID_MASK];
+
+	ib_evt.device = &dev->ibdev;
+
+	switch (type) {
+	case OCRDMA_CQ_ERROR:
+		ib_evt.element.cq = &cq->ibcq;
+		ib_evt.event = IB_EVENT_CQ_ERR;
+		cq_event = 1;
+		qp_event = 0;
+		break;
+	case OCRDMA_CQ_OVERRUN_ERROR:
+		ib_evt.element.cq = &cq->ibcq;
+		ib_evt.event = IB_EVENT_CQ_ERR;
+		break;
+	case OCRDMA_CQ_QPCAT_ERROR:
+		ib_evt.element.qp = &qp->ibqp;
+		ib_evt.event = IB_EVENT_QP_FATAL;
+		ocrdma_process_qpcat_error(dev, qp);
+		break;
+	case OCRDMA_QP_ACCESS_ERROR:
+		ib_evt.element.qp = &qp->ibqp;
+		ib_evt.event = IB_EVENT_QP_ACCESS_ERR;
+		break;
+	case OCRDMA_QP_COMM_EST_EVENT:
+		ib_evt.element.qp = &qp->ibqp;
+		ib_evt.event = IB_EVENT_COMM_EST;
+		break;
+	case OCRDMA_SQ_DRAINED_EVENT:
+		ib_evt.element.qp = &qp->ibqp;
+		ib_evt.event = IB_EVENT_SQ_DRAINED;
+		break;
+	case OCRDMA_DEVICE_FATAL_EVENT:
+		ib_evt.element.port_num = 1;
+		ib_evt.event = IB_EVENT_DEVICE_FATAL;
+		qp_event = 0;
+		dev_event = 1;
+		break;
+	case OCRDMA_SRQCAT_ERROR:
+		ib_evt.element.srq = &qp->srq->ibsrq;
+		ib_evt.event = IB_EVENT_SRQ_ERR;
+		srq_event = 1;
+		qp_event = 0;
+		break;
+	case OCRDMA_SRQ_LIMIT_EVENT:
+		ib_evt.element.srq = &qp->srq->ibsrq;
+		ib_evt.event = IB_EVENT_QP_LAST_WQE_REACHED;
+		srq_event = 1;
+		qp_event = 0;
+		break;
+	case OCRDMA_QP_LAST_WQE_EVENT:
+		ib_evt.element.qp = &qp->ibqp;
+		ib_evt.event = IB_EVENT_QP_LAST_WQE_REACHED;
+		break;
+	default:
+		cq_event = 0;
+		qp_event = 0;
+		srq_event = 0;
+		dev_event = 0;
+		ocrdma_err("%s() unknown type=0x%x\n", __func__, type);
+		break;
+	}
+
+	if (qp_event) {
+		if (qp->ibqp.event_handler)
+			qp->ibqp.event_handler(&ib_evt, qp->ibqp.qp_context);
+	} else if (cq_event) {
+		if (cq->ibcq.event_handler)
+			cq->ibcq.event_handler(&ib_evt, cq->ibcq.cq_context);
+	} else if (srq_event) {
+		if (qp->srq->ibsrq.event_handler)
+			qp->srq->ibsrq.event_handler(&ib_evt,
+						     qp->srq->ibsrq.
+						     srq_context);
+	} else if (dev_event)
+		ib_dispatch_event(&ib_evt);
+
+}
+
+static void ocrdma_process_acqe(struct ocrdma_dev *dev, void *ae_cqe)
+{
+	/* async CQE processing */
+	struct ocrdma_ae_mcqe *cqe = ae_cqe;
+	u32 evt_code = (cqe->valid_ae_event & OCRDMA_AE_MCQE_EVENT_CODE_MASK) >>
+			OCRDMA_AE_MCQE_EVENT_CODE_SHIFT;
+
+	if (evt_code == OCRDMA_ASYNC_EVE_CODE)
+		ocrdma_dispatch_ibevent(dev, cqe);
+	else
+		ocrdma_err("%s(%d) invalid evt code=0x%x\n",
+			   __func__, dev->id, evt_code);
+}
+
+static void ocrdma_process_mcqe(struct ocrdma_dev *dev, struct ocrdma_mcqe *cqe)
+{
+	if (dev->mqe_ctx.tag == cqe->tag_lo && dev->mqe_ctx.cmd_done == false) {
+		dev->mqe_ctx.cqe_status = (cqe->status &
+		     OCRDMA_MCQE_STATUS_MASK) >> OCRDMA_MCQE_STATUS_SHIFT;
+		dev->mqe_ctx.ext_status =
+		    (cqe->status & OCRDMA_MCQE_ESTATUS_MASK)
+		    >> OCRDMA_MCQE_ESTATUS_SHIFT;
+		dev->mqe_ctx.cmd_done = true;
+		wake_up(&dev->mqe_ctx.cmd_wait);
+	} else
+		ocrdma_err("%s() cqe for invalid tag0x%x.expected=0x%x\n",
+			   __func__, cqe->tag_lo, dev->mqe_ctx.tag);
+}
+
+static int ocrdma_mq_cq_handler(struct ocrdma_dev *dev, u16 cq_id)
+{
+	u16 cqe_popped = 0;
+	struct ocrdma_mcqe *cqe;
+
+	while (1) {
+		cqe = ocrdma_get_mcqe(dev);
+		if (cqe == NULL)
+			break;
+		ocrdma_le32_to_cpu(cqe, sizeof(*cqe));
+		cqe_popped += 1;
+		if (cqe->valid_ae_cmpl_cons & OCRDMA_MCQE_AE_MASK)
+			ocrdma_process_acqe(dev, cqe);
+		else if (cqe->valid_ae_cmpl_cons & OCRDMA_MCQE_CMPL_MASK)
+			ocrdma_process_mcqe(dev, cqe);
+		else
+			ocrdma_err("%s() cqe->compl is not set.\n", __func__);
+		memset(cqe, 0, sizeof(struct ocrdma_mcqe));
+		ocrdma_mcq_inc_tail(dev);
+	}
+	ocrdma_ring_cq_db(dev, dev->mq.cq.id, true, false, cqe_popped);
+	return 0;
+}
+
+static void ocrdma_qp_buddy_cq_handler(struct ocrdma_dev *dev,
+				       struct ocrdma_cq *cq)
+{
+	unsigned long flags;
+	struct ocrdma_qp *qp;
+	bool buddy_cq_found = false;
+	/* Go through list of QPs in error state which are using this CQ
+	 * and invoke its callback handler to trigger CQE processing for
+	 * error/flushed CQE. It is rare to find more than few entries in
+	 * this list as most consumers stops after getting error CQE.
+	 * List is traversed only once when a matching buddy cq found for a QP.
+	 */
+	spin_lock_irqsave(&dev->flush_q_lock, flags);
+	list_for_each_entry(qp, &cq->sq_head, sq_entry) {
+		if (qp->srq)
+			continue;
+		/* if wq and rq share the same cq, than comp_handler
+		 * is already invoked.
+		 */
+		if (qp->sq_cq == qp->rq_cq)
+			continue;
+		/* if completion came on sq, rq's cq is buddy cq.
+		 * if completion came on rq, sq's cq is buddy cq.
+		 */
+		if (qp->sq_cq == cq)
+			cq = qp->rq_cq;
+		else
+			cq = qp->sq_cq;
+		buddy_cq_found = true;
+		break;
+	}
+	spin_unlock_irqrestore(&dev->flush_q_lock, flags);
+	if (buddy_cq_found == false)
+		return;
+	if (cq->ibcq.comp_handler) {
+		spin_lock_irqsave(&cq->comp_handler_lock, flags);
+		(*cq->ibcq.comp_handler) (&cq->ibcq, cq->ibcq.cq_context);
+		spin_unlock_irqrestore(&cq->comp_handler_lock, flags);
+	}
+}
+
+static void ocrdma_qp_cq_handler(struct ocrdma_dev *dev, u16 cq_idx)
+{
+	unsigned long flags;
+	struct ocrdma_cq *cq;
+
+	if (cq_idx >= OCRDMA_MAX_CQ)
+		BUG();
+
+	cq = dev->cq_tbl[cq_idx];
+	if (cq == NULL) {
+		ocrdma_err("%s%d invalid id=0x%x\n", __func__, dev->id, cq_idx);
+		return;
+	}
+	spin_lock_irqsave(&cq->cq_lock, flags);
+	cq->armed = false;
+	cq->solicited = false;
+	spin_unlock_irqrestore(&cq->cq_lock, flags);
+
+	ocrdma_ring_cq_db(dev, cq->id, false, false, 0);
+
+	if (cq->ibcq.comp_handler) {
+		spin_lock_irqsave(&cq->comp_handler_lock, flags);
+		(*cq->ibcq.comp_handler) (&cq->ibcq, cq->ibcq.cq_context);
+		spin_unlock_irqrestore(&cq->comp_handler_lock, flags);
+	}
+	ocrdma_qp_buddy_cq_handler(dev, cq);
+}
+
+static void ocrdma_cq_handler(struct ocrdma_dev *dev, u16 cq_id)
+{
+	/* process the MQ-CQE. */
+	if (cq_id == dev->mq.cq.id)
+		ocrdma_mq_cq_handler(dev, cq_id);
+	else
+		ocrdma_qp_cq_handler(dev, cq_id);
+}
+
+static irqreturn_t ocrdma_irq_handler(int irq, void *handle)
+{
+	struct ocrdma_eq *eq = handle;
+	struct ocrdma_dev *dev = eq->dev;
+	struct ocrdma_eqe eqe;
+	struct ocrdma_eqe *ptr;
+	u16 eqe_popped = 0;
+	u16 cq_id;
+	while (1) {
+		ptr = ocrdma_get_eqe(eq);
+		eqe = *ptr;
+		ocrdma_le32_to_cpu(&eqe, sizeof(eqe));
+		if ((eqe.id_valid & OCRDMA_EQE_VALID_MASK) == 0)
+			break;
+		eqe_popped += 1;
+		ptr->id_valid = 0;
+		/* check whether its CQE or not. */
+		if ((eqe.id_valid & OCRDMA_EQE_FOR_CQE_MASK) == 0) {
+			cq_id = eqe.id_valid >> OCRDMA_EQE_RESOURCE_ID_SHIFT;
+			ocrdma_cq_handler(dev, cq_id);
+		}
+		ocrdma_eq_inc_tail(eq);
+	}
+	ocrdma_ring_eq_db(dev, eq->q.id, true, true, eqe_popped);
+	/* Ring EQ doorbell with num_popped to 0 to enable interrupts again. */
+	if (dev->nic_info.intr_mode == BE_INTERRUPT_MODE_INTX)
+		ocrdma_ring_eq_db(dev, eq->q.id, true, true, 0);
+	return IRQ_HANDLED;
+}
+
+static void ocrdma_post_mqe(struct ocrdma_dev *dev, struct ocrdma_mqe *cmd)
+{
+	struct ocrdma_mqe *mqe;
+
+	dev->mqe_ctx.tag = dev->mq.sq.head;
+	dev->mqe_ctx.cmd_done = false;
+	mqe = ocrdma_get_mqe(dev);
+	cmd->hdr.tag_lo = dev->mq.sq.head;
+	ocrdma_copy_cpu_to_le32(mqe, cmd, sizeof(*mqe));
+	/* make sure descriptor is written before ringing doorbell */
+	wmb();
+	ocrdma_mq_inc_head(dev);
+	ocrdma_ring_mq_db(dev);
+}
+
+static int ocrdma_wait_mqe_cmpl(struct ocrdma_dev *dev)
+{
+	long status;
+	/* 30 sec timeout */
+	status = wait_event_timeout(dev->mqe_ctx.cmd_wait,
+				    (dev->mqe_ctx.cmd_done != false),
+				    msecs_to_jiffies(30000));
+	if (status)
+		return 0;
+	else
+		return -1;
+}
+
+/* issue a mailbox command on the MQ */
+static int ocrdma_mbx_cmd(struct ocrdma_dev *dev, struct ocrdma_mqe *mqe)
+{
+	int status = 0;
+	u16 cqe_status, ext_status;
+	struct ocrdma_mqe *rsp;
+
+	mutex_lock(&dev->mqe_ctx.lock);
+	ocrdma_post_mqe(dev, mqe);
+	status = ocrdma_wait_mqe_cmpl(dev);
+	if (status)
+		goto mbx_err;
+	cqe_status = dev->mqe_ctx.cqe_status;
+	ext_status = dev->mqe_ctx.ext_status;
+	rsp = ocrdma_get_mqe_rsp(dev);
+	ocrdma_copy_le32_to_cpu(mqe, rsp, (sizeof(*mqe)));
+	if (cqe_status || ext_status) {
+		ocrdma_err
+		    ("%s() opcode=0x%x, cqe_status=0x%x, ext_status=0x%x\n",
+		     __func__,
+		     (rsp->u.rsp.subsys_op & OCRDMA_MBX_RSP_OPCODE_MASK) >>
+		     OCRDMA_MBX_RSP_OPCODE_SHIFT, cqe_status, ext_status);
+		status = ocrdma_get_mbx_cqe_errno(cqe_status);
+		goto mbx_err;
+	}
+	if (mqe->u.rsp.status & OCRDMA_MBX_RSP_STATUS_MASK)
+		status = ocrdma_get_mbx_errno(mqe->u.rsp.status);
+mbx_err:
+	mutex_unlock(&dev->mqe_ctx.lock);
+	return status;
+}
+
+static void ocrdma_get_attr(struct ocrdma_dev *dev,
+			      struct ocrdma_dev_attr *attr,
+			      struct ocrdma_mbx_query_config *rsp)
+{
+	int max_q_mem;
+
+	attr->max_pd =
+	    (rsp->max_pd_ca_ack_delay & OCRDMA_MBX_QUERY_CFG_MAX_PD_MASK) >>
+	    OCRDMA_MBX_QUERY_CFG_MAX_PD_SHIFT;
+	attr->max_qp =
+	    (rsp->qp_srq_cq_ird_ord & OCRDMA_MBX_QUERY_CFG_MAX_QP_MASK) >>
+	    OCRDMA_MBX_QUERY_CFG_MAX_QP_SHIFT;
+	attr->max_send_sge = ((rsp->max_write_send_sge &
+			       OCRDMA_MBX_QUERY_CFG_MAX_SEND_SGE_MASK) >>
+			      OCRDMA_MBX_QUERY_CFG_MAX_SEND_SGE_SHIFT);
+	attr->max_recv_sge = (rsp->max_write_send_sge &
+			      OCRDMA_MBX_QUERY_CFG_MAX_SEND_SGE_MASK) >>
+	    OCRDMA_MBX_QUERY_CFG_MAX_SEND_SGE_SHIFT;
+	attr->max_ord_per_qp = (rsp->max_ird_ord_per_qp &
+				OCRDMA_MBX_QUERY_CFG_MAX_ORD_PER_QP_MASK) >>
+	    OCRDMA_MBX_QUERY_CFG_MAX_ORD_PER_QP_SHIFT;
+	attr->max_ird_per_qp = (rsp->max_ird_ord_per_qp &
+				OCRDMA_MBX_QUERY_CFG_MAX_IRD_PER_QP_MASK) >>
+	    OCRDMA_MBX_QUERY_CFG_MAX_IRD_PER_QP_SHIFT;
+	attr->cq_overflow_detect = (rsp->qp_srq_cq_ird_ord &
+				    OCRDMA_MBX_QUERY_CFG_CQ_OVERFLOW_MASK) >>
+	    OCRDMA_MBX_QUERY_CFG_CQ_OVERFLOW_SHIFT;
+	attr->srq_supported = (rsp->qp_srq_cq_ird_ord &
+			       OCRDMA_MBX_QUERY_CFG_SRQ_SUPPORTED_MASK) >>
+	    OCRDMA_MBX_QUERY_CFG_SRQ_SUPPORTED_SHIFT;
+	attr->local_ca_ack_delay = (rsp->max_pd_ca_ack_delay &
+				    OCRDMA_MBX_QUERY_CFG_CA_ACK_DELAY_MASK) >>
+	    OCRDMA_MBX_QUERY_CFG_CA_ACK_DELAY_SHIFT;
+	attr->max_mr = rsp->max_mr;
+	attr->max_mr_size = ~0ull;
+	attr->max_fmr = 0;
+	attr->max_pages_per_frmr = rsp->max_pages_per_frmr;
+	attr->max_num_mr_pbl = rsp->max_num_mr_pbl;
+	attr->max_cqe = rsp->max_cq_cqes_per_cq &
+			OCRDMA_MBX_QUERY_CFG_MAX_CQES_PER_CQ_MASK;
+	attr->wqe_size = ((rsp->wqe_rqe_stride_max_dpp_cqs &
+		OCRDMA_MBX_QUERY_CFG_MAX_WQE_SIZE_MASK) >>
+		OCRDMA_MBX_QUERY_CFG_MAX_WQE_SIZE_OFFSET) *
+		OCRDMA_WQE_STRIDE;
+	attr->rqe_size = ((rsp->wqe_rqe_stride_max_dpp_cqs &
+		OCRDMA_MBX_QUERY_CFG_MAX_RQE_SIZE_MASK) >>
+		OCRDMA_MBX_QUERY_CFG_MAX_RQE_SIZE_OFFSET) *
+		OCRDMA_WQE_STRIDE;
+	attr->max_inline_data =
+	    attr->wqe_size - (sizeof(struct ocrdma_hdr_wqe) +
+			      sizeof(struct ocrdma_sge));
+	max_q_mem = OCRDMA_Q_PAGE_BASE_SIZE << (OCRDMA_MAX_Q_PAGE_SIZE_CNT - 1);
+	/* hw can queue one less then the configured size,
+	 * so publish less by one to stack.
+	 */
+	if (dev->nic_info.dev_family == OCRDMA_GEN2_FAMILY) {
+		dev->attr.max_wqe = max_q_mem / dev->attr.wqe_size;
+		attr->ird = 1;
+		attr->ird_page_size = OCRDMA_MIN_Q_PAGE_SIZE;
+		attr->num_ird_pages = MAX_OCRDMA_IRD_PAGES;
+	} else
+		dev->attr.max_wqe = (max_q_mem / dev->attr.wqe_size) - 1;
+	dev->attr.max_rqe = (max_q_mem / dev->attr.rqe_size) - 1;
+}
+
+static int ocrdma_check_fw_config(struct ocrdma_dev *dev,
+				   struct ocrdma_fw_conf_rsp *conf)
+{
+	u32 fn_mode;
+
+	fn_mode = conf->fn_mode & OCRDMA_FN_MODE_RDMA;
+	if (fn_mode != OCRDMA_FN_MODE_RDMA)
+		return -EINVAL;
+	dev->base_eqid = conf->base_eqid;
+	dev->max_eq = conf->max_eq;
+	dev->attr.max_cq = OCRDMA_MAX_CQ - 1;
+	return 0;
+}
+
+/* can be issued only during init time. */
+static int ocrdma_mbx_query_fw_ver(struct ocrdma_dev *dev)
+{
+	int status = -ENOMEM;
+	struct ocrdma_mqe *cmd;
+	struct ocrdma_fw_ver_rsp *rsp;
+
+	cmd = ocrdma_init_emb_mqe(OCRDMA_CMD_GET_FW_VER, sizeof(*cmd));
+	if (!cmd)
+		return -ENOMEM;
+	ocrdma_init_mch((struct ocrdma_mbx_hdr *)&cmd->u.cmd[0],
+			OCRDMA_CMD_GET_FW_VER,
+			OCRDMA_SUBSYS_COMMON, sizeof(*cmd));
+
+	status = ocrdma_mbx_cmd(dev, (struct ocrdma_mqe *)cmd);
+	if (status)
+		goto mbx_err;
+	rsp = (struct ocrdma_fw_ver_rsp *)cmd;
+	memset(&dev->attr.fw_ver[0], 0, sizeof(dev->attr.fw_ver));
+	memcpy(&dev->attr.fw_ver[0], &rsp->running_ver[0],
+	       sizeof(rsp->running_ver));
+	ocrdma_le32_to_cpu(dev->attr.fw_ver, sizeof(rsp->running_ver));
+mbx_err:
+	kfree(cmd);
+	return status;
+}
+
+/* can be issued only during init time. */
+static int ocrdma_mbx_query_fw_config(struct ocrdma_dev *dev)
+{
+	int status = -ENOMEM;
+	struct ocrdma_mqe *cmd;
+	struct ocrdma_fw_conf_rsp *rsp;
+
+	cmd = ocrdma_init_emb_mqe(OCRDMA_CMD_GET_FW_CONFIG, sizeof(*cmd));
+	if (!cmd)
+		return -ENOMEM;
+	ocrdma_init_mch((struct ocrdma_mbx_hdr *)&cmd->u.cmd[0],
+			OCRDMA_CMD_GET_FW_CONFIG,
+			OCRDMA_SUBSYS_COMMON, sizeof(*cmd));
+	status = ocrdma_mbx_cmd(dev, (struct ocrdma_mqe *)cmd);
+	if (status)
+		goto mbx_err;
+	rsp = (struct ocrdma_fw_conf_rsp *)cmd;
+	status = ocrdma_check_fw_config(dev, rsp);
+mbx_err:
+	kfree(cmd);
+	return status;
+}
+
+static int ocrdma_mbx_query_dev(struct ocrdma_dev *dev)
+{
+	int status = -ENOMEM;
+	struct ocrdma_mbx_query_config *rsp;
+	struct ocrdma_mqe *cmd;
+
+	cmd = ocrdma_init_emb_mqe(OCRDMA_CMD_QUERY_CONFIG, sizeof(*cmd));
+	if (!cmd)
+		return status;
+	status = ocrdma_mbx_cmd(dev, (struct ocrdma_mqe *)cmd);
+	if (status)
+		goto mbx_err;
+	rsp = (struct ocrdma_mbx_query_config *)cmd;
+	ocrdma_get_attr(dev, &dev->attr, rsp);
+mbx_err:
+	kfree(cmd);
+	return status;
+}
+
+int ocrdma_mbx_alloc_pd(struct ocrdma_dev *dev, struct ocrdma_pd *pd)
+{
+	int status = -ENOMEM;
+	struct ocrdma_alloc_pd *cmd;
+	struct ocrdma_alloc_pd_rsp *rsp;
+
+	cmd = ocrdma_init_emb_mqe(OCRDMA_CMD_ALLOC_PD, sizeof(*cmd));
+	if (!cmd)
+		return status;
+	if (pd->dpp_enabled)
+		cmd->enable_dpp_rsvd |= OCRDMA_ALLOC_PD_ENABLE_DPP;
+	status = ocrdma_mbx_cmd(dev, (struct ocrdma_mqe *)cmd);
+	if (status)
+		goto mbx_err;
+	rsp = (struct ocrdma_alloc_pd_rsp *)cmd;
+	pd->id = rsp->dpp_page_pdid & OCRDMA_ALLOC_PD_RSP_PDID_MASK;
+	if (rsp->dpp_page_pdid & OCRDMA_ALLOC_PD_RSP_DPP) {
+		pd->dpp_enabled = true;
+		pd->dpp_page = rsp->dpp_page_pdid >>
+				OCRDMA_ALLOC_PD_RSP_DPP_PAGE_SHIFT;
+	} else {
+		pd->dpp_enabled = false;
+		pd->num_dpp_qp = 0;
+	}
+mbx_err:
+	kfree(cmd);
+	return status;
+}
+
+int ocrdma_mbx_dealloc_pd(struct ocrdma_dev *dev, struct ocrdma_pd *pd)
+{
+	int status = -ENOMEM;
+	struct ocrdma_dealloc_pd *cmd;
+
+	cmd = ocrdma_init_emb_mqe(OCRDMA_CMD_DEALLOC_PD, sizeof(*cmd));
+	if (!cmd)
+		return status;
+	cmd->id = pd->id;
+	status = ocrdma_mbx_cmd(dev, (struct ocrdma_mqe *)cmd);
+	kfree(cmd);
+	return status;
+}
+
+static int ocrdma_build_q_conf(u32 *num_entries, int entry_size,
+			       int *num_pages, int *page_size)
+{
+	int i;
+	int mem_size;
+
+	*num_entries = roundup_pow_of_two(*num_entries);
+	mem_size = *num_entries * entry_size;
+	/* find the possible lowest possible multiplier */
+	for (i = 0; i < OCRDMA_MAX_Q_PAGE_SIZE_CNT; i++) {
+		if (mem_size <= (OCRDMA_Q_PAGE_BASE_SIZE << i))
+			break;
+	}
+	if (i >= OCRDMA_MAX_Q_PAGE_SIZE_CNT)
+		return -EINVAL;
+	mem_size = roundup(mem_size,
+		       ((OCRDMA_Q_PAGE_BASE_SIZE << i) / OCRDMA_MAX_Q_PAGES));
+	*num_pages =
+	    mem_size / ((OCRDMA_Q_PAGE_BASE_SIZE << i) / OCRDMA_MAX_Q_PAGES);
+	*page_size = ((OCRDMA_Q_PAGE_BASE_SIZE << i) / OCRDMA_MAX_Q_PAGES);
+	*num_entries = mem_size / entry_size;
+	return 0;
+}
+
+static int ocrdma_mbx_create_ah_tbl(struct ocrdma_dev *dev)
+{
+	int i ;
+	int status = 0;
+	int max_ah;
+	struct ocrdma_create_ah_tbl *cmd;
+	struct ocrdma_create_ah_tbl_rsp *rsp;
+	struct pci_dev *pdev = dev->nic_info.pdev;
+	dma_addr_t pa;
+	struct ocrdma_pbe *pbes;
+
+	cmd = ocrdma_init_emb_mqe(OCRDMA_CMD_CREATE_AH_TBL, sizeof(*cmd));
+	if (!cmd)
+		return status;
+
+	max_ah = OCRDMA_MAX_AH;
+	dev->av_tbl.size = sizeof(struct ocrdma_av) * max_ah;
+
+	/* number of PBEs in PBL */
+	cmd->ah_conf = (OCRDMA_AH_TBL_PAGES <<
+				OCRDMA_CREATE_AH_NUM_PAGES_SHIFT) &
+				OCRDMA_CREATE_AH_NUM_PAGES_MASK;
+
+	/* page size */
+	for (i = 0; i < OCRDMA_MAX_Q_PAGE_SIZE_CNT; i++) {
+		if (PAGE_SIZE == (OCRDMA_MIN_Q_PAGE_SIZE << i))
+			break;
+	}
+	cmd->ah_conf |= (i << OCRDMA_CREATE_AH_PAGE_SIZE_SHIFT) &
+				OCRDMA_CREATE_AH_PAGE_SIZE_MASK;
+
+	/* ah_entry size */
+	cmd->ah_conf |= (sizeof(struct ocrdma_av) <<
+				OCRDMA_CREATE_AH_ENTRY_SIZE_SHIFT) &
+				OCRDMA_CREATE_AH_ENTRY_SIZE_MASK;
+
+	dev->av_tbl.pbl.va = dma_alloc_coherent(&pdev->dev, PAGE_SIZE,
+						&dev->av_tbl.pbl.pa,
+						GFP_KERNEL);
+	if (dev->av_tbl.pbl.va == NULL)
+		goto mem_err;
+
+	dev->av_tbl.va = dma_alloc_coherent(&pdev->dev, dev->av_tbl.size,
+					    &pa, GFP_KERNEL);
+	if (dev->av_tbl.va == NULL)
+		goto mem_err_ah;
+	dev->av_tbl.pa = pa;
+	dev->av_tbl.num_ah = max_ah;
+	memset(dev->av_tbl.va, 0, dev->av_tbl.size);
+
+	pbes = (struct ocrdma_pbe *)dev->av_tbl.pbl.va;
+	for (i = 0; i < dev->av_tbl.size / OCRDMA_MIN_Q_PAGE_SIZE; i++) {
+		pbes[i].pa_lo = (u32) (pa & 0xffffffff);
+		pbes[i].pa_hi = (u32) upper_32_bits(pa);
+		pa += PAGE_SIZE;
+	}
+	cmd->tbl_addr[0].lo = (u32)(dev->av_tbl.pbl.pa & 0xFFFFFFFF);
+	cmd->tbl_addr[0].hi = (u32)upper_32_bits(dev->av_tbl.pbl.pa);
+	status = ocrdma_mbx_cmd(dev, (struct ocrdma_mqe *)cmd);
+	if (status)
+		goto mbx_err;
+	rsp = (struct ocrdma_create_ah_tbl_rsp *)cmd;
+	dev->av_tbl.ahid = rsp->ahid & 0xFFFF;
+	kfree(cmd);
+	return 0;
+
+mbx_err:
+	dma_free_coherent(&pdev->dev, dev->av_tbl.size, dev->av_tbl.va,
+			  dev->av_tbl.pa);
+	dev->av_tbl.va = NULL;
+mem_err_ah:
+	dma_free_coherent(&pdev->dev, PAGE_SIZE, dev->av_tbl.pbl.va,
+			  dev->av_tbl.pbl.pa);
+	dev->av_tbl.pbl.va = NULL;
+	dev->av_tbl.size = 0;
+mem_err:
+	kfree(cmd);
+	return status;
+}
+
+static void ocrdma_mbx_delete_ah_tbl(struct ocrdma_dev *dev)
+{
+	struct ocrdma_delete_ah_tbl *cmd;
+	struct pci_dev *pdev = dev->nic_info.pdev;
+
+	if (dev->av_tbl.va == NULL)
+		return;
+
+	cmd = ocrdma_init_emb_mqe(OCRDMA_CMD_DELETE_AH_TBL, sizeof(*cmd));
+	if (!cmd)
+		return;
+	cmd->ahid = dev->av_tbl.ahid;
+
+	ocrdma_mbx_cmd(dev, (struct ocrdma_mqe *)cmd);
+	dma_free_coherent(&pdev->dev, dev->av_tbl.size, dev->av_tbl.va,
+			  dev->av_tbl.pa);
+	dma_free_coherent(&pdev->dev, PAGE_SIZE, dev->av_tbl.pbl.va,
+			  dev->av_tbl.pbl.pa);
+	kfree(cmd);
+}
+
+/* Multiple CQs uses the EQ. This routine returns least used
+ * EQ to associate with CQ. This will distributes the interrupt
+ * processing and CPU load to associated EQ, vector and so to that CPU.
+ */
+static u16 ocrdma_bind_eq(struct ocrdma_dev *dev)
+{
+	int i, selected_eq = 0, cq_cnt = 0;
+	u16 eq_id;
+
+	mutex_lock(&dev->dev_lock);
+	cq_cnt = dev->qp_eq_tbl[0].cq_cnt;
+	eq_id = dev->qp_eq_tbl[0].q.id;
+	/* find the EQ which is has the least number of
+	 * CQs associated with it.
+	 */
+	for (i = 0; i < dev->eq_cnt; i++) {
+		if (dev->qp_eq_tbl[i].cq_cnt < cq_cnt) {
+			cq_cnt = dev->qp_eq_tbl[i].cq_cnt;
+			eq_id = dev->qp_eq_tbl[i].q.id;
+			selected_eq = i;
+		}
+	}
+	dev->qp_eq_tbl[selected_eq].cq_cnt += 1;
+	mutex_unlock(&dev->dev_lock);
+	return eq_id;
+}
+
+static void ocrdma_unbind_eq(struct ocrdma_dev *dev, u16 eq_id)
+{
+	int i;
+
+	mutex_lock(&dev->dev_lock);
+	for (i = 0; i < dev->eq_cnt; i++) {
+		if (dev->qp_eq_tbl[i].q.id != eq_id)
+			continue;
+		dev->qp_eq_tbl[i].cq_cnt -= 1;
+		break;
+	}
+	mutex_unlock(&dev->dev_lock);
+}
+
+int ocrdma_mbx_create_cq(struct ocrdma_dev *dev, struct ocrdma_cq *cq,
+			 int entries, int dpp_cq)
+{
+	int status = -ENOMEM; int max_hw_cqe;
+	struct pci_dev *pdev = dev->nic_info.pdev;
+	struct ocrdma_create_cq *cmd;
+	struct ocrdma_create_cq_rsp *rsp;
+	u32 hw_pages, cqe_size, page_size, cqe_count;
+
+	if (dpp_cq)
+		return -EINVAL;
+	if (entries > dev->attr.max_cqe) {
+		ocrdma_err("%s(%d) max_cqe=0x%x, requester_cqe=0x%x\n",
+			   __func__, dev->id, dev->attr.max_cqe, entries);
+		return -EINVAL;
+	}
+	if (dpp_cq && (dev->nic_info.dev_family != OCRDMA_GEN2_FAMILY))
+		return -EINVAL;
+
+	if (dpp_cq) {
+		cq->max_hw_cqe = 1;
+		max_hw_cqe = 1;
+		cqe_size = OCRDMA_DPP_CQE_SIZE;
+		hw_pages = 1;
+	} else {
+		cq->max_hw_cqe = dev->attr.max_cqe;
+		max_hw_cqe = dev->attr.max_cqe;
+		cqe_size = sizeof(struct ocrdma_cqe);
+		hw_pages = OCRDMA_CREATE_CQ_MAX_PAGES;
+	}
+
+	cq->len = roundup(max_hw_cqe * cqe_size, OCRDMA_MIN_Q_PAGE_SIZE);
+
+	cmd = ocrdma_init_emb_mqe(OCRDMA_CMD_CREATE_CQ, sizeof(*cmd));
+	if (!cmd)
+		return -ENOMEM;
+	ocrdma_init_mch(&cmd->cmd.req, OCRDMA_CMD_CREATE_CQ,
+			OCRDMA_SUBSYS_COMMON, sizeof(*cmd));
+	cq->va = dma_alloc_coherent(&pdev->dev, cq->len, &cq->pa, GFP_KERNEL);
+	if (!cq->va) {
+		status = -ENOMEM;
+		goto mem_err;
+	}
+	memset(cq->va, 0, cq->len);
+	page_size = cq->len / hw_pages;
+	cmd->cmd.pgsz_pgcnt = (page_size / OCRDMA_MIN_Q_PAGE_SIZE) <<
+					OCRDMA_CREATE_CQ_PAGE_SIZE_SHIFT;
+	cmd->cmd.pgsz_pgcnt |= hw_pages;
+	cmd->cmd.ev_cnt_flags = OCRDMA_CREATE_CQ_DEF_FLAGS;
+
+	if (dev->eq_cnt < 0)
+		goto eq_err;
+	cq->eqn = ocrdma_bind_eq(dev);
+	cmd->cmd.req.rsvd_version = OCRDMA_CREATE_CQ_VER2;
+	cqe_count = cq->len / cqe_size;
+	if (cqe_count > 1024)
+		/* Set cnt to 3 to indicate more than 1024 cq entries */
+		cmd->cmd.ev_cnt_flags |= (0x3 << OCRDMA_CREATE_CQ_CNT_SHIFT);
+	else {
+		u8 count = 0;
+		switch (cqe_count) {
+		case 256:
+			count = 0;
+			break;
+		case 512:
+			count = 1;
+			break;
+		case 1024:
+			count = 2;
+			break;
+		default:
+			goto mbx_err;
+		}
+		cmd->cmd.ev_cnt_flags |= (count << OCRDMA_CREATE_CQ_CNT_SHIFT);
+	}
+	/* shared eq between all the consumer cqs. */
+	cmd->cmd.eqn = cq->eqn;
+	if (dev->nic_info.dev_family == OCRDMA_GEN2_FAMILY) {
+		if (dpp_cq)
+			cmd->cmd.pgsz_pgcnt |= OCRDMA_CREATE_CQ_DPP <<
+				OCRDMA_CREATE_CQ_TYPE_SHIFT;
+		cq->phase_change = false;
+		cmd->cmd.cqe_count = (cq->len / cqe_size);
+	} else {
+		cmd->cmd.cqe_count = (cq->len / cqe_size) - 1;
+		cmd->cmd.ev_cnt_flags |= OCRDMA_CREATE_CQ_FLAGS_AUTO_VALID;
+		cq->phase_change = true;
+	}
+
+	ocrdma_build_q_pages(&cmd->cmd.pa[0], hw_pages, cq->pa, page_size);
+	status = ocrdma_mbx_cmd(dev, (struct ocrdma_mqe *)cmd);
+	if (status)
+		goto mbx_err;
+
+	rsp = (struct ocrdma_create_cq_rsp *)cmd;
+	cq->id = (u16) (rsp->rsp.cq_id & OCRDMA_CREATE_CQ_RSP_CQ_ID_MASK);
+	kfree(cmd);
+	return 0;
+mbx_err:
+	ocrdma_unbind_eq(dev, cq->eqn);
+eq_err:
+	dma_free_coherent(&pdev->dev, cq->len, cq->va, cq->pa);
+mem_err:
+	kfree(cmd);
+	return status;
+}
+
+int ocrdma_mbx_destroy_cq(struct ocrdma_dev *dev, struct ocrdma_cq *cq)
+{
+	int status = -ENOMEM;
+	struct ocrdma_destroy_cq *cmd;
+
+	cmd = ocrdma_init_emb_mqe(OCRDMA_CMD_DELETE_CQ, sizeof(*cmd));
+	if (!cmd)
+		return status;
+	ocrdma_init_mch(&cmd->req, OCRDMA_CMD_DELETE_CQ,
+			OCRDMA_SUBSYS_COMMON, sizeof(*cmd));
+
+	cmd->bypass_flush_qid |=
+	    (cq->id << OCRDMA_DESTROY_CQ_QID_SHIFT) &
+	    OCRDMA_DESTROY_CQ_QID_MASK;
+
+	ocrdma_unbind_eq(dev, cq->eqn);
+	status = ocrdma_mbx_cmd(dev, (struct ocrdma_mqe *)cmd);
+	if (status)
+		goto mbx_err;
+	dma_free_coherent(&dev->nic_info.pdev->dev, cq->len, cq->va, cq->pa);
+mbx_err:
+	kfree(cmd);
+	return status;
+}
+
+int ocrdma_mbx_alloc_lkey(struct ocrdma_dev *dev, struct ocrdma_hw_mr *hwmr,
+			  u32 pdid, int addr_check)
+{
+	int status = -ENOMEM;
+	struct ocrdma_alloc_lkey *cmd;
+	struct ocrdma_alloc_lkey_rsp *rsp;
+
+	cmd = ocrdma_init_emb_mqe(OCRDMA_CMD_ALLOC_LKEY, sizeof(*cmd));
+	if (!cmd)
+		return status;
+	cmd->pdid = pdid;
+	cmd->pbl_sz_flags |= addr_check;
+	cmd->pbl_sz_flags |= (hwmr->fr_mr << OCRDMA_ALLOC_LKEY_FMR_SHIFT);
+	cmd->pbl_sz_flags |=
+	    (hwmr->remote_wr << OCRDMA_ALLOC_LKEY_REMOTE_WR_SHIFT);
+	cmd->pbl_sz_flags |=
+	    (hwmr->remote_rd << OCRDMA_ALLOC_LKEY_REMOTE_RD_SHIFT);
+	cmd->pbl_sz_flags |=
+	    (hwmr->local_wr << OCRDMA_ALLOC_LKEY_LOCAL_WR_SHIFT);
+	cmd->pbl_sz_flags |=
+	    (hwmr->remote_atomic << OCRDMA_ALLOC_LKEY_REMOTE_ATOMIC_SHIFT);
+	cmd->pbl_sz_flags |=
+	    (hwmr->num_pbls << OCRDMA_ALLOC_LKEY_PBL_SIZE_SHIFT);
+
+	status = ocrdma_mbx_cmd(dev, (struct ocrdma_mqe *)cmd);
+	if (status)
+		goto mbx_err;
+	rsp = (struct ocrdma_alloc_lkey_rsp *)cmd;
+	hwmr->lkey = rsp->lrkey;
+mbx_err:
+	kfree(cmd);
+	return status;
+}
+
+int ocrdma_mbx_dealloc_lkey(struct ocrdma_dev *dev, int fr_mr, u32 lkey)
+{
+	int status = -ENOMEM;
+	struct ocrdma_dealloc_lkey *cmd;
+
+	cmd = ocrdma_init_emb_mqe(OCRDMA_CMD_DEALLOC_LKEY, sizeof(*cmd));
+	if (!cmd)
+		return -ENOMEM;
+	cmd->lkey = lkey;
+	cmd->rsvd_frmr = fr_mr ? 1 : 0;
+	status = ocrdma_mbx_cmd(dev, (struct ocrdma_mqe *)cmd);
+	if (status)
+		goto mbx_err;
+mbx_err:
+	kfree(cmd);
+	return status;
+}
+
+static int ocrdma_mbx_reg_mr(struct ocrdma_dev *dev, struct ocrdma_hw_mr *hwmr,
+			     u32 pdid, u32 pbl_cnt, u32 pbe_size, u32 last)
+{
+	int status = -ENOMEM;
+	int i;
+	struct ocrdma_reg_nsmr *cmd;
+	struct ocrdma_reg_nsmr_rsp *rsp;
+
+	cmd = ocrdma_init_emb_mqe(OCRDMA_CMD_REGISTER_NSMR, sizeof(*cmd));
+	if (!cmd)
+		return -ENOMEM;
+	cmd->num_pbl_pdid =
+	    pdid | (hwmr->num_pbls << OCRDMA_REG_NSMR_NUM_PBL_SHIFT);
+
+	cmd->flags_hpage_pbe_sz |= (hwmr->remote_wr <<
+				    OCRDMA_REG_NSMR_REMOTE_WR_SHIFT);
+	cmd->flags_hpage_pbe_sz |= (hwmr->remote_rd <<
+				    OCRDMA_REG_NSMR_REMOTE_RD_SHIFT);
+	cmd->flags_hpage_pbe_sz |= (hwmr->local_wr <<
+				    OCRDMA_REG_NSMR_LOCAL_WR_SHIFT);
+	cmd->flags_hpage_pbe_sz |= (hwmr->remote_atomic <<
+				    OCRDMA_REG_NSMR_REMOTE_ATOMIC_SHIFT);
+	cmd->flags_hpage_pbe_sz |= (hwmr->mw_bind <<
+				    OCRDMA_REG_NSMR_BIND_MEMWIN_SHIFT);
+	cmd->flags_hpage_pbe_sz |= (last << OCRDMA_REG_NSMR_LAST_SHIFT);
+
+	cmd->flags_hpage_pbe_sz |= (hwmr->pbe_size / OCRDMA_MIN_HPAGE_SIZE);
+	cmd->flags_hpage_pbe_sz |= (hwmr->pbl_size / OCRDMA_MIN_HPAGE_SIZE) <<
+					OCRDMA_REG_NSMR_HPAGE_SIZE_SHIFT;
+	cmd->totlen_low = hwmr->len;
+	cmd->totlen_high = upper_32_bits(hwmr->len);
+	cmd->fbo_low = (u32) (hwmr->fbo & 0xffffffff);
+	cmd->fbo_high = (u32) upper_32_bits(hwmr->fbo);
+	cmd->va_loaddr = (u32) hwmr->va;
+	cmd->va_hiaddr = (u32) upper_32_bits(hwmr->va);
+
+	for (i = 0; i < pbl_cnt; i++) {
+		cmd->pbl[i].lo = (u32) (hwmr->pbl_table[i].pa & 0xffffffff);
+		cmd->pbl[i].hi = upper_32_bits(hwmr->pbl_table[i].pa);
+	}
+	status = ocrdma_mbx_cmd(dev, (struct ocrdma_mqe *)cmd);
+	if (status)
+		goto mbx_err;
+	rsp = (struct ocrdma_reg_nsmr_rsp *)cmd;
+	hwmr->lkey = rsp->lrkey;
+mbx_err:
+	kfree(cmd);
+	return status;
+}
+
+static int ocrdma_mbx_reg_mr_cont(struct ocrdma_dev *dev,
+				  struct ocrdma_hw_mr *hwmr, u32 pbl_cnt,
+				  u32 pbl_offset, u32 last)
+{
+	int status = -ENOMEM;
+	int i;
+	struct ocrdma_reg_nsmr_cont *cmd;
+
+	cmd = ocrdma_init_emb_mqe(OCRDMA_CMD_REGISTER_NSMR_CONT, sizeof(*cmd));
+	if (!cmd)
+		return -ENOMEM;
+	cmd->lrkey = hwmr->lkey;
+	cmd->num_pbl_offset = (pbl_cnt << OCRDMA_REG_NSMR_CONT_NUM_PBL_SHIFT) |
+	    (pbl_offset & OCRDMA_REG_NSMR_CONT_PBL_SHIFT_MASK);
+	cmd->last = last << OCRDMA_REG_NSMR_CONT_LAST_SHIFT;
+
+	for (i = 0; i < pbl_cnt; i++) {
+		cmd->pbl[i].lo =
+		    (u32) (hwmr->pbl_table[i + pbl_offset].pa & 0xffffffff);
+		cmd->pbl[i].hi =
+		    upper_32_bits(hwmr->pbl_table[i + pbl_offset].pa);
+	}
+	status = ocrdma_mbx_cmd(dev, (struct ocrdma_mqe *)cmd);
+	if (status)
+		goto mbx_err;
+mbx_err:
+	kfree(cmd);
+	return status;
+}
+
+int ocrdma_reg_mr(struct ocrdma_dev *dev,
+		  struct ocrdma_hw_mr *hwmr, u32 pdid, int acc)
+{
+	int status;
+	u32 last = 0;
+	u32 cur_pbl_cnt, pbl_offset;
+	u32 pending_pbl_cnt = hwmr->num_pbls;
+
+	pbl_offset = 0;
+	cur_pbl_cnt = min(pending_pbl_cnt, MAX_OCRDMA_NSMR_PBL);
+	if (cur_pbl_cnt == pending_pbl_cnt)
+		last = 1;
+
+	status = ocrdma_mbx_reg_mr(dev, hwmr, pdid,
+				   cur_pbl_cnt, hwmr->pbe_size, last);
+	if (status) {
+		ocrdma_err("%s() status=%d\n", __func__, status);
+		return status;
+	}
+	/* if there is no more pbls to register then exit. */
+	if (last)
+		return 0;
+
+	while (!last) {
+		pbl_offset += cur_pbl_cnt;
+		pending_pbl_cnt -= cur_pbl_cnt;
+		cur_pbl_cnt = min(pending_pbl_cnt, MAX_OCRDMA_NSMR_PBL);
+		/* if we reach the end of the pbls, then need to set the last
+		 * bit, indicating no more pbls to register for this memory key.
+		 */
+		if (cur_pbl_cnt == pending_pbl_cnt)
+			last = 1;
+
+		status = ocrdma_mbx_reg_mr_cont(dev, hwmr, cur_pbl_cnt,
+						pbl_offset, last);
+		if (status)
+			break;
+	}
+	if (status)
+		ocrdma_err("%s() err. status=%d\n", __func__, status);
+
+	return status;
+}
+
+bool ocrdma_is_qp_in_sq_flushlist(struct ocrdma_cq *cq, struct ocrdma_qp *qp)
+{
+	struct ocrdma_qp *tmp;
+	bool found = false;
+	list_for_each_entry(tmp, &cq->sq_head, sq_entry) {
+		if (qp == tmp) {
+			found = true;
+			break;
+		}
+	}
+	return found;
+}
+
+bool ocrdma_is_qp_in_rq_flushlist(struct ocrdma_cq *cq, struct ocrdma_qp *qp)
+{
+	struct ocrdma_qp *tmp;
+	bool found = false;
+	list_for_each_entry(tmp, &cq->rq_head, rq_entry) {
+		if (qp == tmp) {
+			found = true;
+			break;
+		}
+	}
+	return found;
+}
+
+void ocrdma_flush_qp(struct ocrdma_qp *qp)
+{
+	bool found;
+	unsigned long flags;
+
+	spin_lock_irqsave(&qp->dev->flush_q_lock, flags);
+	found = ocrdma_is_qp_in_sq_flushlist(qp->sq_cq, qp);
+	if (!found)
+		list_add_tail(&qp->sq_entry, &qp->sq_cq->sq_head);
+	if (!qp->srq) {
+		found = ocrdma_is_qp_in_rq_flushlist(qp->rq_cq, qp);
+		if (!found)
+			list_add_tail(&qp->rq_entry, &qp->rq_cq->rq_head);
+	}
+	spin_unlock_irqrestore(&qp->dev->flush_q_lock, flags);
+}
+
+int ocrdma_qp_state_machine(struct ocrdma_qp *qp, enum ib_qp_state new_ib_state,
+			    enum ib_qp_state *old_ib_state)
+{
+	unsigned long flags;
+	int status = 0;
+	enum ocrdma_qp_state new_state;
+	new_state = get_ocrdma_qp_state(new_ib_state);
+
+	/* sync with wqe and rqe posting */
+	spin_lock_irqsave(&qp->q_lock, flags);
+
+	if (old_ib_state)
+		*old_ib_state = get_ibqp_state(qp->state);
+	if (new_state == qp->state) {
+		spin_unlock_irqrestore(&qp->q_lock, flags);
+		return 1;
+	}
+
+	switch (qp->state) {
+	case OCRDMA_QPS_RST:
+		switch (new_state) {
+		case OCRDMA_QPS_RST:
+		case OCRDMA_QPS_INIT:
+			break;
+		default:
+			status = -EINVAL;
+			break;
+		};
+		break;
+	case OCRDMA_QPS_INIT:
+		/* qps: INIT->XXX */
+		switch (new_state) {
+		case OCRDMA_QPS_INIT:
+		case OCRDMA_QPS_RTR:
+			break;
+		case OCRDMA_QPS_ERR:
+			ocrdma_flush_qp(qp);
+			break;
+		default:
+			status = -EINVAL;
+			break;
+		};
+		break;
+	case OCRDMA_QPS_RTR:
+		/* qps: RTS->XXX */
+		switch (new_state) {
+		case OCRDMA_QPS_RTS:
+			break;
+		case OCRDMA_QPS_ERR:
+			ocrdma_flush_qp(qp);
+			break;
+		default:
+			status = -EINVAL;
+			break;
+		};
+		break;
+	case OCRDMA_QPS_RTS:
+		/* qps: RTS->XXX */
+		switch (new_state) {
+		case OCRDMA_QPS_SQD:
+		case OCRDMA_QPS_SQE:
+			break;
+		case OCRDMA_QPS_ERR:
+			ocrdma_flush_qp(qp);
+			break;
+		default:
+			status = -EINVAL;
+			break;
+		};
+		break;
+	case OCRDMA_QPS_SQD:
+		/* qps: SQD->XXX */
+		switch (new_state) {
+		case OCRDMA_QPS_RTS:
+		case OCRDMA_QPS_SQE:
+		case OCRDMA_QPS_ERR:
+			break;
+		default:
+			status = -EINVAL;
+			break;
+		};
+		break;
+	case OCRDMA_QPS_SQE:
+		switch (new_state) {
+		case OCRDMA_QPS_RTS:
+		case OCRDMA_QPS_ERR:
+			break;
+		default:
+			status = -EINVAL;
+			break;
+		};
+		break;
+	case OCRDMA_QPS_ERR:
+		/* qps: ERR->XXX */
+		switch (new_state) {
+		case OCRDMA_QPS_RST:
+			break;
+		default:
+			status = -EINVAL;
+			break;
+		};
+		break;
+	default:
+		status = -EINVAL;
+		break;
+	};
+	if (!status)
+		qp->state = new_state;
+
+	spin_unlock_irqrestore(&qp->q_lock, flags);
+	return status;
+}
+
+static u32 ocrdma_set_create_qp_mbx_access_flags(struct ocrdma_qp *qp)
+{
+	u32 flags = 0;
+	if (qp->cap_flags & OCRDMA_QP_INB_RD)
+		flags |= OCRDMA_CREATE_QP_REQ_INB_RDEN_MASK;
+	if (qp->cap_flags & OCRDMA_QP_INB_WR)
+		flags |= OCRDMA_CREATE_QP_REQ_INB_WREN_MASK;
+	if (qp->cap_flags & OCRDMA_QP_MW_BIND)
+		flags |= OCRDMA_CREATE_QP_REQ_BIND_MEMWIN_MASK;
+	if (qp->cap_flags & OCRDMA_QP_LKEY0)
+		flags |= OCRDMA_CREATE_QP_REQ_ZERO_LKEYEN_MASK;
+	if (qp->cap_flags & OCRDMA_QP_FAST_REG)
+		flags |= OCRDMA_CREATE_QP_REQ_FMR_EN_MASK;
+	return flags;
+}
+
+static int ocrdma_set_create_qp_sq_cmd(struct ocrdma_create_qp_req *cmd,
+					struct ib_qp_init_attr *attrs,
+					struct ocrdma_qp *qp)
+{
+	int status;
+	u32 len, hw_pages, hw_page_size;
+	dma_addr_t pa;
+	struct ocrdma_dev *dev = qp->dev;
+	struct pci_dev *pdev = dev->nic_info.pdev;
+	u32 max_wqe_allocated;
+	u32 max_sges = attrs->cap.max_send_sge;
+
+	max_wqe_allocated = attrs->cap.max_send_wr;
+	/* need to allocate one extra to for GEN1 family */
+	if (dev->nic_info.dev_family != OCRDMA_GEN2_FAMILY)
+		max_wqe_allocated += 1;
+
+	status = ocrdma_build_q_conf(&max_wqe_allocated,
+		dev->attr.wqe_size, &hw_pages, &hw_page_size);
+	if (status) {
+		ocrdma_err("%s() req. max_send_wr=0x%x\n", __func__,
+			   max_wqe_allocated);
+		return -EINVAL;
+	}
+	qp->sq.max_cnt = max_wqe_allocated;
+	len = (hw_pages * hw_page_size);
+
+	qp->sq.va = dma_alloc_coherent(&pdev->dev, len, &pa, GFP_KERNEL);
+	if (!qp->sq.va)
+		return -EINVAL;
+	memset(qp->sq.va, 0, len);
+	qp->sq.len = len;
+	qp->sq.pa = pa;
+	qp->sq.entry_size = dev->attr.wqe_size;
+	ocrdma_build_q_pages(&cmd->wq_addr[0], hw_pages, pa, hw_page_size);
+
+	cmd->type_pgsz_pdn |= (ilog2(hw_page_size / OCRDMA_MIN_Q_PAGE_SIZE)
+				<< OCRDMA_CREATE_QP_REQ_SQ_PAGE_SIZE_SHIFT);
+	cmd->num_wq_rq_pages |= (hw_pages <<
+				 OCRDMA_CREATE_QP_REQ_NUM_WQ_PAGES_SHIFT) &
+	    OCRDMA_CREATE_QP_REQ_NUM_WQ_PAGES_MASK;
+	cmd->max_sge_send_write |= (max_sges <<
+				    OCRDMA_CREATE_QP_REQ_MAX_SGE_SEND_SHIFT) &
+	    OCRDMA_CREATE_QP_REQ_MAX_SGE_SEND_MASK;
+	cmd->max_sge_send_write |= (max_sges <<
+				    OCRDMA_CREATE_QP_REQ_MAX_SGE_WRITE_SHIFT) &
+					OCRDMA_CREATE_QP_REQ_MAX_SGE_WRITE_MASK;
+	cmd->max_wqe_rqe |= (ilog2(qp->sq.max_cnt) <<
+			     OCRDMA_CREATE_QP_REQ_MAX_WQE_SHIFT) &
+				OCRDMA_CREATE_QP_REQ_MAX_WQE_MASK;
+	cmd->wqe_rqe_size |= (dev->attr.wqe_size <<
+			      OCRDMA_CREATE_QP_REQ_WQE_SIZE_SHIFT) &
+				OCRDMA_CREATE_QP_REQ_WQE_SIZE_MASK;
+	return 0;
+}
+
+static int ocrdma_set_create_qp_rq_cmd(struct ocrdma_create_qp_req *cmd,
+					struct ib_qp_init_attr *attrs,
+					struct ocrdma_qp *qp)
+{
+	int status;
+	u32 len, hw_pages, hw_page_size;
+	dma_addr_t pa = 0;
+	struct ocrdma_dev *dev = qp->dev;
+	struct pci_dev *pdev = dev->nic_info.pdev;
+	u32 max_rqe_allocated = attrs->cap.max_recv_wr + 1;
+
+	status = ocrdma_build_q_conf(&max_rqe_allocated, dev->attr.rqe_size,
+				     &hw_pages, &hw_page_size);
+	if (status) {
+		ocrdma_err("%s() req. max_recv_wr=0x%x\n", __func__,
+			   attrs->cap.max_recv_wr + 1);
+		return status;
+	}
+	qp->rq.max_cnt = max_rqe_allocated;
+	len = (hw_pages * hw_page_size);
+
+	qp->rq.va = dma_alloc_coherent(&pdev->dev, len, &pa, GFP_KERNEL);
+	if (!qp->rq.va)
+		return status;
+	memset(qp->rq.va, 0, len);
+	qp->rq.pa = pa;
+	qp->rq.len = len;
+	qp->rq.entry_size = dev->attr.rqe_size;
+
+	ocrdma_build_q_pages(&cmd->rq_addr[0], hw_pages, pa, hw_page_size);
+	cmd->type_pgsz_pdn |= (ilog2(hw_page_size / OCRDMA_MIN_Q_PAGE_SIZE) <<
+		OCRDMA_CREATE_QP_REQ_RQ_PAGE_SIZE_SHIFT);
+	cmd->num_wq_rq_pages |=
+	    (hw_pages << OCRDMA_CREATE_QP_REQ_NUM_RQ_PAGES_SHIFT) &
+	    OCRDMA_CREATE_QP_REQ_NUM_RQ_PAGES_MASK;
+	cmd->max_sge_recv_flags |= (attrs->cap.max_recv_sge <<
+				OCRDMA_CREATE_QP_REQ_MAX_SGE_RECV_SHIFT) &
+				OCRDMA_CREATE_QP_REQ_MAX_SGE_RECV_MASK;
+	cmd->max_wqe_rqe |= (ilog2(qp->rq.max_cnt) <<
+				OCRDMA_CREATE_QP_REQ_MAX_RQE_SHIFT) &
+				OCRDMA_CREATE_QP_REQ_MAX_RQE_MASK;
+	cmd->wqe_rqe_size |= (dev->attr.rqe_size <<
+			OCRDMA_CREATE_QP_REQ_RQE_SIZE_SHIFT) &
+			OCRDMA_CREATE_QP_REQ_RQE_SIZE_MASK;
+	return 0;
+}
+
+static void ocrdma_set_create_qp_dpp_cmd(struct ocrdma_create_qp_req *cmd,
+					 struct ocrdma_pd *pd,
+					 struct ocrdma_qp *qp,
+					 u8 enable_dpp_cq, u16 dpp_cq_id)
+{
+	pd->num_dpp_qp--;
+	qp->dpp_enabled = true;
+	cmd->max_sge_recv_flags |= OCRDMA_CREATE_QP_REQ_ENABLE_DPP_MASK;
+	if (!enable_dpp_cq)
+		return;
+	cmd->max_sge_recv_flags |= OCRDMA_CREATE_QP_REQ_ENABLE_DPP_MASK;
+	cmd->dpp_credits_cqid = dpp_cq_id;
+	cmd->dpp_credits_cqid |= OCRDMA_CREATE_QP_REQ_DPP_CREDIT_LIMIT <<
+					OCRDMA_CREATE_QP_REQ_DPP_CREDIT_SHIFT;
+}
+
+static int ocrdma_set_create_qp_ird_cmd(struct ocrdma_create_qp_req *cmd,
+					struct ocrdma_qp *qp)
+{
+	struct ocrdma_dev *dev = qp->dev;
+	struct pci_dev *pdev = dev->nic_info.pdev;
+	dma_addr_t pa = 0;
+	int ird_page_size = dev->attr.ird_page_size;
+	int ird_q_len = dev->attr.num_ird_pages * ird_page_size;
+
+	if (dev->attr.ird == 0)
+		return 0;
+
+	qp->ird_q_va = dma_alloc_coherent(&pdev->dev, ird_q_len,
+					&pa, GFP_KERNEL);
+	if (!qp->ird_q_va)
+		return -ENOMEM;
+	memset(qp->ird_q_va, 0, ird_q_len);
+	ocrdma_build_q_pages(&cmd->ird_addr[0], dev->attr.num_ird_pages,
+			     pa, ird_page_size);
+	return 0;
+}
+
+static void ocrdma_get_create_qp_rsp(struct ocrdma_create_qp_rsp *rsp,
+				     struct ocrdma_qp *qp,
+				     struct ib_qp_init_attr *attrs,
+				     u16 *dpp_offset, u16 *dpp_credit_lmt)
+{
+	u32 max_wqe_allocated, max_rqe_allocated;
+	qp->id = rsp->qp_id & OCRDMA_CREATE_QP_RSP_QP_ID_MASK;
+	qp->rq.dbid = rsp->sq_rq_id & OCRDMA_CREATE_QP_RSP_RQ_ID_MASK;
+	qp->sq.dbid = rsp->sq_rq_id >> OCRDMA_CREATE_QP_RSP_SQ_ID_SHIFT;
+	qp->max_ird = rsp->max_ord_ird & OCRDMA_CREATE_QP_RSP_MAX_IRD_MASK;
+	qp->max_ord = (rsp->max_ord_ird >> OCRDMA_CREATE_QP_RSP_MAX_ORD_SHIFT);
+	qp->dpp_enabled = false;
+	if (rsp->dpp_response & OCRDMA_CREATE_QP_RSP_DPP_ENABLED_MASK) {
+		qp->dpp_enabled = true;
+		*dpp_credit_lmt = (rsp->dpp_response &
+				OCRDMA_CREATE_QP_RSP_DPP_CREDITS_MASK) >>
+				OCRDMA_CREATE_QP_RSP_DPP_CREDITS_SHIFT;
+		*dpp_offset = (rsp->dpp_response &
+				OCRDMA_CREATE_QP_RSP_DPP_PAGE_OFFSET_MASK) >>
+				OCRDMA_CREATE_QP_RSP_DPP_PAGE_OFFSET_SHIFT;
+	}
+	max_wqe_allocated =
+		rsp->max_wqe_rqe >> OCRDMA_CREATE_QP_RSP_MAX_WQE_SHIFT;
+	max_wqe_allocated = 1 << max_wqe_allocated;
+	max_rqe_allocated = 1 << ((u16)rsp->max_wqe_rqe);
+
+	if (qp->dev->nic_info.dev_family == OCRDMA_GEN2_FAMILY) {
+		qp->sq.free_delta = 0;
+		qp->rq.free_delta = 1;
+	} else
+		qp->sq.free_delta = 1;
+
+	qp->sq.max_cnt = max_wqe_allocated;
+	qp->sq.max_wqe_idx = max_wqe_allocated - 1;
+
+	if (!attrs->srq) {
+		qp->rq.max_cnt = max_rqe_allocated;
+		qp->rq.max_wqe_idx = max_rqe_allocated - 1;
+		qp->rq.free_delta = 1;
+	}
+}
+
+int ocrdma_mbx_create_qp(struct ocrdma_qp *qp, struct ib_qp_init_attr *attrs,
+			 u8 enable_dpp_cq, u16 dpp_cq_id, u16 *dpp_offset,
+			 u16 *dpp_credit_lmt)
+{
+	int status = -ENOMEM;
+	u32 flags = 0;
+	struct ocrdma_dev *dev = qp->dev;
+	struct ocrdma_pd *pd = qp->pd;
+	struct pci_dev *pdev = dev->nic_info.pdev;
+	struct ocrdma_cq *cq;
+	struct ocrdma_create_qp_req *cmd;
+	struct ocrdma_create_qp_rsp *rsp;
+	int qptype;
+
+	switch (attrs->qp_type) {
+	case IB_QPT_GSI:
+		qptype = OCRDMA_QPT_GSI;
+		break;
+	case IB_QPT_RC:
+		qptype = OCRDMA_QPT_RC;
+		break;
+	case IB_QPT_UD:
+		qptype = OCRDMA_QPT_UD;
+		break;
+	default:
+		return -EINVAL;
+	};
+
+	cmd = ocrdma_init_emb_mqe(OCRDMA_CMD_CREATE_QP, sizeof(*cmd));
+	if (!cmd)
+		return status;
+	cmd->type_pgsz_pdn |= (qptype << OCRDMA_CREATE_QP_REQ_QPT_SHIFT) &
+						OCRDMA_CREATE_QP_REQ_QPT_MASK;
+	status = ocrdma_set_create_qp_sq_cmd(cmd, attrs, qp);
+	if (status)
+		goto sq_err;
+
+	if (attrs->srq) {
+		struct ocrdma_srq *srq = get_ocrdma_srq(attrs->srq);
+		cmd->max_sge_recv_flags |= OCRDMA_CREATE_QP_REQ_USE_SRQ_MASK;
+		cmd->rq_addr[0].lo = srq->id;
+		qp->srq = srq;
+	} else {
+		status = ocrdma_set_create_qp_rq_cmd(cmd, attrs, qp);
+		if (status)
+			goto rq_err;
+	}
+
+	status = ocrdma_set_create_qp_ird_cmd(cmd, qp);
+	if (status)
+		goto mbx_err;
+
+	cmd->type_pgsz_pdn |= (pd->id << OCRDMA_CREATE_QP_REQ_PD_ID_SHIFT) &
+				OCRDMA_CREATE_QP_REQ_PD_ID_MASK;
+
+	flags = ocrdma_set_create_qp_mbx_access_flags(qp);
+
+	cmd->max_sge_recv_flags |= flags;
+	cmd->max_ord_ird |= (dev->attr.max_ord_per_qp <<
+			     OCRDMA_CREATE_QP_REQ_MAX_ORD_SHIFT) &
+				OCRDMA_CREATE_QP_REQ_MAX_ORD_MASK;
+	cmd->max_ord_ird |= (dev->attr.max_ird_per_qp <<
+			     OCRDMA_CREATE_QP_REQ_MAX_IRD_SHIFT) &
+				OCRDMA_CREATE_QP_REQ_MAX_IRD_MASK;
+	cq = get_ocrdma_cq(attrs->send_cq);
+	cmd->wq_rq_cqid |= (cq->id << OCRDMA_CREATE_QP_REQ_WQ_CQID_SHIFT) &
+				OCRDMA_CREATE_QP_REQ_WQ_CQID_MASK;
+	qp->sq_cq = cq;
+	cq = get_ocrdma_cq(attrs->recv_cq);
+	cmd->wq_rq_cqid |= (cq->id << OCRDMA_CREATE_QP_REQ_RQ_CQID_SHIFT) &
+				OCRDMA_CREATE_QP_REQ_RQ_CQID_MASK;
+	qp->rq_cq = cq;
+
+	if (pd->dpp_enabled && attrs->cap.max_inline_data && pd->num_dpp_qp &&
+	    (attrs->cap.max_inline_data <= dev->attr.max_inline_data))
+		ocrdma_set_create_qp_dpp_cmd(cmd, pd, qp, enable_dpp_cq,
+					     dpp_cq_id);
+
+	status = ocrdma_mbx_cmd(dev, (struct ocrdma_mqe *)cmd);
+	if (status)
+		goto mbx_err;
+	rsp = (struct ocrdma_create_qp_rsp *)cmd;
+	ocrdma_get_create_qp_rsp(rsp, qp, attrs, dpp_offset, dpp_credit_lmt);
+	qp->state = OCRDMA_QPS_RST;
+	kfree(cmd);
+	return 0;
+mbx_err:
+	if (qp->rq.va)
+		dma_free_coherent(&pdev->dev, qp->rq.len, qp->rq.va, qp->rq.pa);
+rq_err:
+	ocrdma_err("%s(%d) rq_err\n", __func__, dev->id);
+	dma_free_coherent(&pdev->dev, qp->sq.len, qp->sq.va, qp->sq.pa);
+sq_err:
+	ocrdma_err("%s(%d) sq_err\n", __func__, dev->id);
+	kfree(cmd);
+	return status;
+}
+
+int ocrdma_mbx_query_qp(struct ocrdma_dev *dev, struct ocrdma_qp *qp,
+			struct ocrdma_qp_params *param)
+{
+	int status = -ENOMEM;
+	struct ocrdma_query_qp *cmd;
+	struct ocrdma_query_qp_rsp *rsp;
+
+	cmd = ocrdma_init_emb_mqe(OCRDMA_CMD_QUERY_QP, sizeof(*cmd));
+	if (!cmd)
+		return status;
+	cmd->qp_id = qp->id;
+	status = ocrdma_mbx_cmd(dev, (struct ocrdma_mqe *)cmd);
+	if (status)
+		goto mbx_err;
+	rsp = (struct ocrdma_query_qp_rsp *)cmd;
+	memcpy(param, &rsp->params, sizeof(struct ocrdma_qp_params));
+mbx_err:
+	kfree(cmd);
+	return status;
+}
+
+int ocrdma_resolve_dgid(struct ocrdma_dev *dev, union ib_gid *dgid,
+			u8 *mac_addr)
+{
+	struct in6_addr in6;
+
+	memcpy(&in6, dgid, sizeof in6);
+	if (rdma_is_multicast_addr(&in6))
+		rdma_get_mcast_mac(&in6, mac_addr);
+	else if (rdma_link_local_addr(&in6))
+		rdma_get_ll_mac(&in6, mac_addr);
+	else {
+		ocrdma_err("%s() fail to resolve mac_addr.\n", __func__);
+		return -EINVAL;
+	}
+	return 0;
+}
+
+static void ocrdma_set_av_params(struct ocrdma_qp *qp,
+				struct ocrdma_modify_qp *cmd,
+				struct ib_qp_attr *attrs)
+{
+	struct ib_ah_attr *ah_attr = &attrs->ah_attr;
+	union ib_gid sgid;
+	u32 vlan_id;
+	u8 mac_addr[6];
+	if ((ah_attr->ah_flags & IB_AH_GRH) == 0)
+		return;
+	cmd->params.tclass_sq_psn |=
+	    (ah_attr->grh.traffic_class << OCRDMA_QP_PARAMS_TCLASS_SHIFT);
+	cmd->params.rnt_rc_sl_fl |=
+	    (ah_attr->grh.flow_label & OCRDMA_QP_PARAMS_FLOW_LABEL_MASK);
+	cmd->params.hop_lmt_rq_psn |=
+	    (ah_attr->grh.hop_limit << OCRDMA_QP_PARAMS_HOP_LMT_SHIFT);
+	cmd->flags |= OCRDMA_QP_PARA_FLOW_LBL_VALID;
+	memcpy(&cmd->params.dgid[0], &ah_attr->grh.dgid.raw[0],
+	       sizeof(cmd->params.dgid));
+	ocrdma_query_gid(&qp->dev->ibdev, 1,
+			 ah_attr->grh.sgid_index, &sgid);
+	qp->sgid_idx = ah_attr->grh.sgid_index;
+	memcpy(&cmd->params.sgid[0], &sgid.raw[0], sizeof(cmd->params.sgid));
+	ocrdma_resolve_dgid(qp->dev, &ah_attr->grh.dgid, &mac_addr[0]);
+	cmd->params.dmac_b0_to_b3 = mac_addr[0] | (mac_addr[1] << 8) |
+				(mac_addr[2] << 16) | (mac_addr[3] << 24);
+	/* convert them to LE format. */
+	ocrdma_cpu_to_le32(&cmd->params.dgid[0], sizeof(cmd->params.dgid));
+	ocrdma_cpu_to_le32(&cmd->params.sgid[0], sizeof(cmd->params.sgid));
+	cmd->params.vlan_dmac_b4_to_b5 = mac_addr[4] | (mac_addr[5] << 8);
+	vlan_id = rdma_get_vlan_id(&sgid);
+	if (vlan_id && (vlan_id < 0x1000)) {
+		cmd->params.vlan_dmac_b4_to_b5 |=
+		    vlan_id << OCRDMA_QP_PARAMS_VLAN_SHIFT;
+		cmd->flags |= OCRDMA_QP_PARA_VLAN_EN_VALID;
+	}
+}
+
+static int ocrdma_set_qp_params(struct ocrdma_qp *qp,
+				struct ocrdma_modify_qp *cmd,
+				struct ib_qp_attr *attrs, int attr_mask,
+				enum ib_qp_state old_qps)
+{
+	int status = 0;
+	struct net_device *netdev = qp->dev->nic_info.netdev;
+	int eth_mtu = iboe_get_mtu(netdev->mtu);
+
+	if (attr_mask & IB_QP_PKEY_INDEX) {
+		cmd->params.path_mtu_pkey_indx |= (attrs->pkey_index &
+					    OCRDMA_QP_PARAMS_PKEY_INDEX_MASK);
+		cmd->flags |= OCRDMA_QP_PARA_PKEY_VALID;
+	}
+	if (attr_mask & IB_QP_QKEY) {
+		qp->qkey = attrs->qkey;
+		cmd->params.qkey = attrs->qkey;
+		cmd->flags |= OCRDMA_QP_PARA_QKEY_VALID;
+	}
+	if (attr_mask & IB_QP_AV)
+		ocrdma_set_av_params(qp, cmd, attrs);
+	else if (qp->qp_type == IB_QPT_GSI || qp->qp_type == IB_QPT_UD) {
+		/* set the default mac address for UD, GSI QPs */
+		cmd->params.dmac_b0_to_b3 = qp->dev->nic_info.mac_addr[0] |
+			(qp->dev->nic_info.mac_addr[1] << 8) |
+			(qp->dev->nic_info.mac_addr[2] << 16) |
+			(qp->dev->nic_info.mac_addr[3] << 24);
+		cmd->params.vlan_dmac_b4_to_b5 = qp->dev->nic_info.mac_addr[4] |
+					(qp->dev->nic_info.mac_addr[5] << 8);
+	}
+	if ((attr_mask & IB_QP_EN_SQD_ASYNC_NOTIFY) &&
+	    attrs->en_sqd_async_notify) {
+		cmd->params.max_sge_recv_flags |=
+			OCRDMA_QP_PARAMS_FLAGS_SQD_ASYNC;
+		cmd->flags |= OCRDMA_QP_PARA_DST_QPN_VALID;
+	}
+	if (attr_mask & IB_QP_DEST_QPN) {
+		cmd->params.ack_to_rnr_rtc_dest_qpn |= (attrs->dest_qp_num &
+				OCRDMA_QP_PARAMS_DEST_QPN_MASK);
+		cmd->flags |= OCRDMA_QP_PARA_DST_QPN_VALID;
+	}
+	if (attr_mask & IB_QP_PATH_MTU) {
+		if (ib_mtu_enum_to_int(eth_mtu) <
+		    ib_mtu_enum_to_int(attrs->path_mtu)) {
+			status = -EINVAL;
+			goto pmtu_err;
+		}
+		cmd->params.path_mtu_pkey_indx |=
+		    (ib_mtu_enum_to_int(attrs->path_mtu) <<
+		     OCRDMA_QP_PARAMS_PATH_MTU_SHIFT) &
+		    OCRDMA_QP_PARAMS_PATH_MTU_MASK;
+		cmd->flags |= OCRDMA_QP_PARA_PMTU_VALID;
+	}
+	if (attr_mask & IB_QP_TIMEOUT) {
+		cmd->params.ack_to_rnr_rtc_dest_qpn |= attrs->timeout <<
+		    OCRDMA_QP_PARAMS_ACK_TIMEOUT_SHIFT;
+		cmd->flags |= OCRDMA_QP_PARA_ACK_TO_VALID;
+	}
+	if (attr_mask & IB_QP_RETRY_CNT) {
+		cmd->params.rnt_rc_sl_fl |= (attrs->retry_cnt <<
+				      OCRDMA_QP_PARAMS_RETRY_CNT_SHIFT) &
+		    OCRDMA_QP_PARAMS_RETRY_CNT_MASK;
+		cmd->flags |= OCRDMA_QP_PARA_RETRY_CNT_VALID;
+	}
+	if (attr_mask & IB_QP_MIN_RNR_TIMER) {
+		cmd->params.rnt_rc_sl_fl |= (attrs->min_rnr_timer <<
+				      OCRDMA_QP_PARAMS_RNR_NAK_TIMER_SHIFT) &
+		    OCRDMA_QP_PARAMS_RNR_NAK_TIMER_MASK;
+		cmd->flags |= OCRDMA_QP_PARA_RNT_VALID;
+	}
+	if (attr_mask & IB_QP_RNR_RETRY) {
+		cmd->params.ack_to_rnr_rtc_dest_qpn |= (attrs->rnr_retry <<
+			OCRDMA_QP_PARAMS_RNR_RETRY_CNT_SHIFT)
+			& OCRDMA_QP_PARAMS_RNR_RETRY_CNT_MASK;
+		cmd->flags |= OCRDMA_QP_PARA_RRC_VALID;
+	}
+	if (attr_mask & IB_QP_SQ_PSN) {
+		cmd->params.tclass_sq_psn |= (attrs->sq_psn & 0x00ffffff);
+		cmd->flags |= OCRDMA_QP_PARA_SQPSN_VALID;
+	}
+	if (attr_mask & IB_QP_RQ_PSN) {
+		cmd->params.hop_lmt_rq_psn |= (attrs->rq_psn & 0x00ffffff);
+		cmd->flags |= OCRDMA_QP_PARA_RQPSN_VALID;
+	}
+	if (attr_mask & IB_QP_MAX_QP_RD_ATOMIC) {
+		if (attrs->max_rd_atomic > qp->dev->attr.max_ord_per_qp) {
+			status = -EINVAL;
+			goto pmtu_err;
+		}
+		qp->max_ord = attrs->max_rd_atomic;
+		cmd->flags |= OCRDMA_QP_PARA_MAX_ORD_VALID;
+	}
+	if (attr_mask & IB_QP_MAX_DEST_RD_ATOMIC) {
+		if (attrs->max_dest_rd_atomic > qp->dev->attr.max_ird_per_qp) {
+			status = -EINVAL;
+			goto pmtu_err;
+		}
+		qp->max_ird = attrs->max_dest_rd_atomic;
+		cmd->flags |= OCRDMA_QP_PARA_MAX_IRD_VALID;
+	}
+	cmd->params.max_ord_ird = (qp->max_ord <<
+				OCRDMA_QP_PARAMS_MAX_ORD_SHIFT) |
+				(qp->max_ird & OCRDMA_QP_PARAMS_MAX_IRD_MASK);
+pmtu_err:
+	return status;
+}
+
+int ocrdma_mbx_modify_qp(struct ocrdma_dev *dev, struct ocrdma_qp *qp,
+			 struct ib_qp_attr *attrs, int attr_mask,
+			 enum ib_qp_state old_qps)
+{
+	int status = -ENOMEM;
+	struct ocrdma_modify_qp *cmd;
+
+	cmd = ocrdma_init_emb_mqe(OCRDMA_CMD_MODIFY_QP, sizeof(*cmd));
+	if (!cmd)
+		return status;
+
+	cmd->params.id = qp->id;
+	cmd->flags = 0;
+	if (attr_mask & IB_QP_STATE) {
+		cmd->params.max_sge_recv_flags |=
+		    (get_ocrdma_qp_state(attrs->qp_state) <<
+		     OCRDMA_QP_PARAMS_STATE_SHIFT) &
+		    OCRDMA_QP_PARAMS_STATE_MASK;
+		cmd->flags |= OCRDMA_QP_PARA_QPS_VALID;
+	} else
+		cmd->params.max_sge_recv_flags |=
+		    (qp->state << OCRDMA_QP_PARAMS_STATE_SHIFT) &
+		    OCRDMA_QP_PARAMS_STATE_MASK;
+	status = ocrdma_set_qp_params(qp, cmd, attrs, attr_mask, old_qps);
+	if (status)
+		goto mbx_err;
+	status = ocrdma_mbx_cmd(dev, (struct ocrdma_mqe *)cmd);
+	if (status)
+		goto mbx_err;
+
+mbx_err:
+	kfree(cmd);
+	return status;
+}
+
+int ocrdma_mbx_destroy_qp(struct ocrdma_dev *dev, struct ocrdma_qp *qp)
+{
+	int status = -ENOMEM;
+	struct ocrdma_destroy_qp *cmd;
+	struct pci_dev *pdev = dev->nic_info.pdev;
+
+	cmd = ocrdma_init_emb_mqe(OCRDMA_CMD_DELETE_QP, sizeof(*cmd));
+	if (!cmd)
+		return status;
+	cmd->qp_id = qp->id;
+	status = ocrdma_mbx_cmd(dev, (struct ocrdma_mqe *)cmd);
+	if (status)
+		goto mbx_err;
+
+mbx_err:
+	kfree(cmd);
+	if (qp->sq.va)
+		dma_free_coherent(&pdev->dev, qp->sq.len, qp->sq.va, qp->sq.pa);
+	if (!qp->srq && qp->rq.va)
+		dma_free_coherent(&pdev->dev, qp->rq.len, qp->rq.va, qp->rq.pa);
+	if (qp->dpp_enabled)
+		qp->pd->num_dpp_qp++;
+	return status;
+}
+
+int ocrdma_mbx_create_srq(struct ocrdma_srq *srq,
+			  struct ib_srq_init_attr *srq_attr,
+			  struct ocrdma_pd *pd)
+{
+	int status = -ENOMEM;
+	int hw_pages, hw_page_size;
+	int len;
+	struct ocrdma_create_srq_rsp *rsp;
+	struct ocrdma_create_srq *cmd;
+	dma_addr_t pa;
+	struct ocrdma_dev *dev = srq->dev;
+	struct pci_dev *pdev = dev->nic_info.pdev;
+	u32 max_rqe_allocated;
+
+	cmd = ocrdma_init_emb_mqe(OCRDMA_CMD_CREATE_SRQ, sizeof(*cmd));
+	if (!cmd)
+		return status;
+
+	cmd->pgsz_pdid = pd->id & OCRDMA_CREATE_SRQ_PD_ID_MASK;
+	max_rqe_allocated = srq_attr->attr.max_wr + 1;
+	status = ocrdma_build_q_conf(&max_rqe_allocated,
+				dev->attr.rqe_size,
+				&hw_pages, &hw_page_size);
+	if (status) {
+		ocrdma_err("%s() req. max_wr=0x%x\n", __func__,
+			   srq_attr->attr.max_wr);
+		status = -EINVAL;
+		goto ret;
+	}
+	len = hw_pages * hw_page_size;
+	srq->rq.va = dma_alloc_coherent(&pdev->dev, len, &pa, GFP_KERNEL);
+	if (!srq->rq.va) {
+		status = -ENOMEM;
+		goto ret;
+	}
+	ocrdma_build_q_pages(&cmd->rq_addr[0], hw_pages, pa, hw_page_size);
+
+	srq->rq.entry_size = dev->attr.rqe_size;
+	srq->rq.pa = pa;
+	srq->rq.len = len;
+	srq->rq.max_cnt = max_rqe_allocated;
+
+	cmd->max_sge_rqe = ilog2(max_rqe_allocated);
+	cmd->max_sge_rqe |= srq_attr->attr.max_sge <<
+				OCRDMA_CREATE_SRQ_MAX_SGE_RECV_SHIFT;
+
+	cmd->pgsz_pdid |= (ilog2(hw_page_size / OCRDMA_MIN_Q_PAGE_SIZE)
+		<< OCRDMA_CREATE_SRQ_PG_SZ_SHIFT);
+	cmd->pages_rqe_sz |= (dev->attr.rqe_size
+		<< OCRDMA_CREATE_SRQ_RQE_SIZE_SHIFT)
+		& OCRDMA_CREATE_SRQ_RQE_SIZE_MASK;
+	cmd->pages_rqe_sz |= hw_pages << OCRDMA_CREATE_SRQ_NUM_RQ_PAGES_SHIFT;
+
+	status = ocrdma_mbx_cmd(dev, (struct ocrdma_mqe *)cmd);
+	if (status)
+		goto mbx_err;
+	rsp = (struct ocrdma_create_srq_rsp *)cmd;
+	srq->id = rsp->id;
+	srq->rq.dbid = rsp->id;
+	max_rqe_allocated = ((rsp->max_sge_rqe_allocated &
+		OCRDMA_CREATE_SRQ_RSP_MAX_RQE_ALLOCATED_MASK) >>
+		OCRDMA_CREATE_SRQ_RSP_MAX_RQE_ALLOCATED_SHIFT);
+	max_rqe_allocated = (1 << max_rqe_allocated);
+	srq->rq.max_cnt = max_rqe_allocated;
+	srq->rq.max_wqe_idx = max_rqe_allocated - 1;
+	srq->rq.max_sges = (rsp->max_sge_rqe_allocated &
+		OCRDMA_CREATE_SRQ_RSP_MAX_SGE_RECV_ALLOCATED_MASK) >>
+		OCRDMA_CREATE_SRQ_RSP_MAX_SGE_RECV_ALLOCATED_SHIFT;
+	goto ret;
+mbx_err:
+	dma_free_coherent(&pdev->dev, srq->rq.len, srq->rq.va, pa);
+ret:
+	kfree(cmd);
+	return status;
+}
+
+int ocrdma_mbx_modify_srq(struct ocrdma_srq *srq, struct ib_srq_attr *srq_attr)
+{
+	int status = -ENOMEM;
+	struct ocrdma_modify_srq *cmd;
+	cmd = ocrdma_init_emb_mqe(OCRDMA_CMD_CREATE_SRQ, sizeof(*cmd));
+	if (!cmd)
+		return status;
+	cmd->id = srq->id;
+	cmd->limit_max_rqe |= srq_attr->srq_limit <<
+	    OCRDMA_MODIFY_SRQ_LIMIT_SHIFT;
+	status = ocrdma_mbx_cmd(srq->dev, (struct ocrdma_mqe *)cmd);
+	kfree(cmd);
+	return status;
+}
+
+int ocrdma_mbx_query_srq(struct ocrdma_srq *srq, struct ib_srq_attr *srq_attr)
+{
+	int status = -ENOMEM;
+	struct ocrdma_query_srq *cmd;
+	cmd = ocrdma_init_emb_mqe(OCRDMA_CMD_CREATE_SRQ, sizeof(*cmd));
+	if (!cmd)
+		return status;
+	cmd->id = srq->rq.dbid;
+	status = ocrdma_mbx_cmd(srq->dev, (struct ocrdma_mqe *)cmd);
+	if (status == 0) {
+		struct ocrdma_query_srq_rsp *rsp =
+		    (struct ocrdma_query_srq_rsp *)cmd;
+		srq_attr->max_sge =
+		    rsp->srq_lmt_max_sge &
+		    OCRDMA_QUERY_SRQ_RSP_MAX_SGE_RECV_MASK;
+		srq_attr->max_wr =
+		    rsp->max_rqe_pdid >> OCRDMA_QUERY_SRQ_RSP_MAX_RQE_SHIFT;
+		srq_attr->srq_limit = rsp->srq_lmt_max_sge >>
+		    OCRDMA_QUERY_SRQ_RSP_SRQ_LIMIT_SHIFT;
+	}
+	kfree(cmd);
+	return status;
+}
+
+int ocrdma_mbx_destroy_srq(struct ocrdma_dev *dev, struct ocrdma_srq *srq)
+{
+	int status = -ENOMEM;
+	struct ocrdma_destroy_srq *cmd;
+	struct pci_dev *pdev = dev->nic_info.pdev;
+	cmd = ocrdma_init_emb_mqe(OCRDMA_CMD_DELETE_SRQ, sizeof(*cmd));
+	if (!cmd)
+		return status;
+	cmd->id = srq->id;
+	status = ocrdma_mbx_cmd(srq->dev, (struct ocrdma_mqe *)cmd);
+	if (srq->rq.va)
+		dma_free_coherent(&pdev->dev, srq->rq.len,
+				  srq->rq.va, srq->rq.pa);
+	kfree(cmd);
+	return status;
+}
+
+int ocrdma_alloc_av(struct ocrdma_dev *dev, struct ocrdma_ah *ah)
+{
+	int i;
+	int status = -EINVAL;
+	struct ocrdma_av *av;
+	unsigned long flags;
+
+	av = dev->av_tbl.va;
+	spin_lock_irqsave(&dev->av_tbl.lock, flags);
+	for (i = 0; i < dev->av_tbl.num_ah; i++) {
+		if (av->valid == 0) {
+			av->valid = OCRDMA_AV_VALID;
+			ah->av = av;
+			ah->id = i;
+			status = 0;
+			break;
+		}
+		av++;
+	}
+	if (i == dev->av_tbl.num_ah)
+		status = -EAGAIN;
+	spin_unlock_irqrestore(&dev->av_tbl.lock, flags);
+	return status;
+}
+
+int ocrdma_free_av(struct ocrdma_dev *dev, struct ocrdma_ah *ah)
+{
+	unsigned long flags;
+	spin_lock_irqsave(&dev->av_tbl.lock, flags);
+	ah->av->valid = 0;
+	spin_unlock_irqrestore(&dev->av_tbl.lock, flags);
+	return 0;
+}
+
+static int ocrdma_create_mq_eq(struct ocrdma_dev *dev)
+{
+	int status;
+	int irq;
+	unsigned long flags = 0;
+	int num_eq = 0;
+
+	if (dev->nic_info.intr_mode == BE_INTERRUPT_MODE_INTX)
+		flags = IRQF_SHARED;
+	else {
+		num_eq = dev->nic_info.msix.num_vectors -
+				dev->nic_info.msix.start_vector;
+		/* minimum two vectors/eq are required for rdma to work.
+		 * one for control path and one for data path.
+		 */
+		if (num_eq < 2)
+			return -EBUSY;
+	}
+
+	status = ocrdma_create_eq(dev, &dev->meq, OCRDMA_EQ_LEN);
+	if (status)
+		return status;
+	sprintf(dev->meq.irq_name, "ocrdma_mq%d", dev->id);
+	irq = ocrdma_get_irq(dev, &dev->meq);
+	status = request_irq(irq, ocrdma_irq_handler, flags, dev->meq.irq_name,
+			     &dev->meq);
+	if (status)
+		_ocrdma_destroy_eq(dev, &dev->meq);
+	return status;
+}
+
+static int ocrdma_create_qp_eqs(struct ocrdma_dev *dev)
+{
+	int num_eq, i, status = 0;
+	int irq;
+	unsigned long flags = 0;
+
+	num_eq = dev->nic_info.msix.num_vectors -
+			dev->nic_info.msix.start_vector;
+	if (dev->nic_info.intr_mode == BE_INTERRUPT_MODE_INTX) {
+		num_eq = 1;
+		flags = IRQF_SHARED;
+	} else
+		num_eq = min_t(u32, num_eq, num_online_cpus());
+	dev->qp_eq_tbl = kzalloc(sizeof(struct ocrdma_eq) * num_eq, GFP_KERNEL);
+	if (!dev->qp_eq_tbl)
+		return -ENOMEM;
+
+	for (i = 0; i < num_eq; i++) {
+		status = ocrdma_create_eq(dev, &dev->qp_eq_tbl[i],
+					  OCRDMA_EQ_LEN);
+		if (status) {
+			status = -EINVAL;
+			break;
+		}
+		sprintf(dev->qp_eq_tbl[i].irq_name, "ocrdma_qp%d-%d",
+			dev->id, i);
+		irq = ocrdma_get_irq(dev, &dev->qp_eq_tbl[i]);
+		status = request_irq(irq, ocrdma_irq_handler, flags,
+				     dev->qp_eq_tbl[i].irq_name,
+				     &dev->qp_eq_tbl[i]);
+		if (status) {
+			_ocrdma_destroy_eq(dev, &dev->qp_eq_tbl[i]);
+			status = -EINVAL;
+			break;
+		}
+		dev->eq_cnt += 1;
+	}
+	/* one eq is sufficient for data path to work */
+	if (dev->eq_cnt >= 1)
+		return 0;
+	if (status)
+		ocrdma_destroy_qp_eqs(dev);
+	return status;
+}
+
+int ocrdma_init_hw(struct ocrdma_dev *dev)
+{
+	int status;
+	/* set up control path eq */
+	status = ocrdma_create_mq_eq(dev);
+	if (status)
+		return status;
+	/* set up data path eq */
+	status = ocrdma_create_qp_eqs(dev);
+	if (status)
+		goto qpeq_err;
+	status = ocrdma_create_mq(dev);
+	if (status)
+		goto mq_err;
+	status = ocrdma_mbx_query_fw_config(dev);
+	if (status)
+		goto conf_err;
+	status = ocrdma_mbx_query_dev(dev);
+	if (status)
+		goto conf_err;
+	status = ocrdma_mbx_query_fw_ver(dev);
+	if (status)
+		goto conf_err;
+	status = ocrdma_mbx_create_ah_tbl(dev);
+	if (status)
+		goto conf_err;
+	return 0;
+
+conf_err:
+	ocrdma_destroy_mq(dev);
+mq_err:
+	ocrdma_destroy_qp_eqs(dev);
+qpeq_err:
+	ocrdma_destroy_eq(dev, &dev->meq);
+	ocrdma_err("%s() status=%d\n", __func__, status);
+	return status;
+}
+
+void ocrdma_cleanup_hw(struct ocrdma_dev *dev)
+{
+	ocrdma_mbx_delete_ah_tbl(dev);
+
+	/* cleanup the data path eqs */
+	ocrdma_destroy_qp_eqs(dev);
+
+	/* cleanup the control path */
+	ocrdma_destroy_mq(dev);
+	ocrdma_destroy_eq(dev, &dev->meq);
+}
diff --git a/drivers/infiniband/hw/ocrdma/ocrdma_hw.h b/drivers/infiniband/hw/ocrdma/ocrdma_hw.h
new file mode 100644
index 00000000000..be5db77404d
--- /dev/null
+++ b/drivers/infiniband/hw/ocrdma/ocrdma_hw.h
@@ -0,0 +1,132 @@
+/*******************************************************************
+ * This file is part of the Emulex RoCE Device Driver for          *
+ * RoCE (RDMA over Converged Ethernet) CNA Adapters.              *
+ * Copyright (C) 2008-2012 Emulex. All rights reserved.            *
+ * EMULEX and SLI are trademarks of Emulex.                        *
+ * www.emulex.com                                                  *
+ *                                                                 *
+ * 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. *
+ * ALL EXPRESS OR IMPLIED CONDITIONS, REPRESENTATIONS AND          *
+ * WARRANTIES, INCLUDING ANY IMPLIED WARRANTY OF MERCHANTABILITY,  *
+ * FITNESS FOR A PARTICULAR PURPOSE, OR NON-INFRINGEMENT, ARE      *
+ * DISCLAIMED, EXCEPT TO THE EXTENT THAT SUCH DISCLAIMERS ARE HELD *
+ * TO BE LEGALLY INVALID.  See the GNU General Public License for  *
+ * more details, a copy of which can be found in the file COPYING  *
+ * included with this package.                                     *
+ *
+ * Contact Information:
+ * linux-drivers@emulex.com
+ *
+ * Emulex
+ * 3333 Susan Street
+ * Costa Mesa, CA 92626
+ *******************************************************************/
+
+#ifndef __OCRDMA_HW_H__
+#define __OCRDMA_HW_H__
+
+#include "ocrdma_sli.h"
+
+static inline void ocrdma_cpu_to_le32(void *dst, u32 len)
+{
+#ifdef __BIG_ENDIAN
+	int i = 0;
+	u32 *src_ptr = dst;
+	u32 *dst_ptr = dst;
+	for (; i < (len / 4); i++)
+		*(dst_ptr + i) = cpu_to_le32p(src_ptr + i);
+#endif
+}
+
+static inline void ocrdma_le32_to_cpu(void *dst, u32 len)
+{
+#ifdef __BIG_ENDIAN
+	int i = 0;
+	u32 *src_ptr = dst;
+	u32 *dst_ptr = dst;
+	for (; i < (len / sizeof(u32)); i++)
+		*(dst_ptr + i) = le32_to_cpu(*(src_ptr + i));
+#endif
+}
+
+static inline void ocrdma_copy_cpu_to_le32(void *dst, void *src, u32 len)
+{
+#ifdef __BIG_ENDIAN
+	int i = 0;
+	u32 *src_ptr = src;
+	u32 *dst_ptr = dst;
+	for (; i < (len / sizeof(u32)); i++)
+		*(dst_ptr + i) = cpu_to_le32p(src_ptr + i);
+#else
+	memcpy(dst, src, len);
+#endif
+}
+
+static inline void ocrdma_copy_le32_to_cpu(void *dst, void *src, u32 len)
+{
+#ifdef __BIG_ENDIAN
+	int i = 0;
+	u32 *src_ptr = src;
+	u32 *dst_ptr = dst;
+	for (; i < len / sizeof(u32); i++)
+		*(dst_ptr + i) = le32_to_cpu(*(src_ptr + i));
+#else
+	memcpy(dst, src, len);
+#endif
+}
+
+int ocrdma_init_hw(struct ocrdma_dev *);
+void ocrdma_cleanup_hw(struct ocrdma_dev *);
+
+enum ib_qp_state get_ibqp_state(enum ocrdma_qp_state qps);
+void ocrdma_ring_cq_db(struct ocrdma_dev *, u16 cq_id, bool armed,
+		       bool solicited, u16 cqe_popped);
+
+/* verbs specific mailbox commands */
+int ocrdma_query_config(struct ocrdma_dev *,
+			struct ocrdma_mbx_query_config *config);
+int ocrdma_resolve_dgid(struct ocrdma_dev *, union ib_gid *dgid, u8 *mac_addr);
+
+int ocrdma_mbx_alloc_pd(struct ocrdma_dev *, struct ocrdma_pd *);
+int ocrdma_mbx_dealloc_pd(struct ocrdma_dev *, struct ocrdma_pd *);
+
+int ocrdma_mbx_alloc_lkey(struct ocrdma_dev *, struct ocrdma_hw_mr *hwmr,
+			  u32 pd_id, int addr_check);
+int ocrdma_mbx_dealloc_lkey(struct ocrdma_dev *, int fmr, u32 lkey);
+
+int ocrdma_reg_mr(struct ocrdma_dev *, struct ocrdma_hw_mr *hwmr,
+			u32 pd_id, int acc);
+int ocrdma_mbx_create_cq(struct ocrdma_dev *, struct ocrdma_cq *,
+				int entries, int dpp_cq);
+int ocrdma_mbx_destroy_cq(struct ocrdma_dev *, struct ocrdma_cq *);
+
+int ocrdma_mbx_create_qp(struct ocrdma_qp *, struct ib_qp_init_attr *attrs,
+			 u8 enable_dpp_cq, u16 dpp_cq_id, u16 *dpp_offset,
+			 u16 *dpp_credit_lmt);
+int ocrdma_mbx_modify_qp(struct ocrdma_dev *, struct ocrdma_qp *,
+			 struct ib_qp_attr *attrs, int attr_mask,
+			 enum ib_qp_state old_qps);
+int ocrdma_mbx_query_qp(struct ocrdma_dev *, struct ocrdma_qp *,
+			struct ocrdma_qp_params *param);
+int ocrdma_mbx_destroy_qp(struct ocrdma_dev *, struct ocrdma_qp *);
+
+int ocrdma_mbx_create_srq(struct ocrdma_srq *,
+			  struct ib_srq_init_attr *,
+			  struct ocrdma_pd *);
+int ocrdma_mbx_modify_srq(struct ocrdma_srq *, struct ib_srq_attr *);
+int ocrdma_mbx_query_srq(struct ocrdma_srq *, struct ib_srq_attr *);
+int ocrdma_mbx_destroy_srq(struct ocrdma_dev *, struct ocrdma_srq *);
+
+int ocrdma_alloc_av(struct ocrdma_dev *, struct ocrdma_ah *);
+int ocrdma_free_av(struct ocrdma_dev *, struct ocrdma_ah *);
+
+int ocrdma_qp_state_machine(struct ocrdma_qp *, enum ib_qp_state new_state,
+			    enum ib_qp_state *old_ib_state);
+bool ocrdma_is_qp_in_sq_flushlist(struct ocrdma_cq *, struct ocrdma_qp *);
+bool ocrdma_is_qp_in_rq_flushlist(struct ocrdma_cq *, struct ocrdma_qp *);
+void ocrdma_flush_qp(struct ocrdma_qp *);
+
+#endif				/* __OCRDMA_HW_H__ */
diff --git a/drivers/infiniband/hw/ocrdma/ocrdma_main.c b/drivers/infiniband/hw/ocrdma/ocrdma_main.c
new file mode 100644
index 00000000000..a20d16eaae7
--- /dev/null
+++ b/drivers/infiniband/hw/ocrdma/ocrdma_main.c
@@ -0,0 +1,577 @@
+/*******************************************************************
+ * This file is part of the Emulex RoCE Device Driver for          *
+ * RoCE (RDMA over Converged Ethernet) adapters.                   *
+ * Copyright (C) 2008-2012 Emulex. All rights reserved.            *
+ * EMULEX and SLI are trademarks of Emulex.                        *
+ * www.emulex.com                                                  *
+ *                                                                 *
+ * 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. *
+ * ALL EXPRESS OR IMPLIED CONDITIONS, REPRESENTATIONS AND          *
+ * WARRANTIES, INCLUDING ANY IMPLIED WARRANTY OF MERCHANTABILITY,  *
+ * FITNESS FOR A PARTICULAR PURPOSE, OR NON-INFRINGEMENT, ARE      *
+ * DISCLAIMED, EXCEPT TO THE EXTENT THAT SUCH DISCLAIMERS ARE HELD *
+ * TO BE LEGALLY INVALID.  See the GNU General Public License for  *
+ * more details, a copy of which can be found in the file COPYING  *
+ * included with this package.                                     *
+ *
+ * Contact Information:
+ * linux-drivers@emulex.com
+ *
+ * Emulex
+ * 3333 Susan Street
+ * Costa Mesa, CA 92626
+ *******************************************************************/
+
+#include <linux/module.h>
+#include <linux/version.h>
+#include <linux/idr.h>
+#include <rdma/ib_verbs.h>
+#include <rdma/ib_user_verbs.h>
+#include <rdma/ib_addr.h>
+
+#include <linux/netdevice.h>
+#include <net/addrconf.h>
+
+#include "ocrdma.h"
+#include "ocrdma_verbs.h"
+#include "ocrdma_ah.h"
+#include "be_roce.h"
+#include "ocrdma_hw.h"
+
+MODULE_VERSION(OCRDMA_ROCE_DEV_VERSION);
+MODULE_DESCRIPTION("Emulex RoCE HCA Driver");
+MODULE_AUTHOR("Emulex Corporation");
+MODULE_LICENSE("GPL");
+
+static LIST_HEAD(ocrdma_dev_list);
+static DEFINE_SPINLOCK(ocrdma_devlist_lock);
+static DEFINE_IDR(ocrdma_dev_id);
+
+static union ib_gid ocrdma_zero_sgid;
+
+static int ocrdma_get_instance(void)
+{
+	int instance = 0;
+
+	/* Assign an unused number */
+	if (!idr_pre_get(&ocrdma_dev_id, GFP_KERNEL))
+		return -1;
+	if (idr_get_new(&ocrdma_dev_id, NULL, &instance))
+		return -1;
+	return instance;
+}
+
+void ocrdma_get_guid(struct ocrdma_dev *dev, u8 *guid)
+{
+	u8 mac_addr[6];
+
+	memcpy(&mac_addr[0], &dev->nic_info.mac_addr[0], ETH_ALEN);
+	guid[0] = mac_addr[0] ^ 2;
+	guid[1] = mac_addr[1];
+	guid[2] = mac_addr[2];
+	guid[3] = 0xff;
+	guid[4] = 0xfe;
+	guid[5] = mac_addr[3];
+	guid[6] = mac_addr[4];
+	guid[7] = mac_addr[5];
+}
+
+static void ocrdma_build_sgid_mac(union ib_gid *sgid, unsigned char *mac_addr,
+				  bool is_vlan, u16 vlan_id)
+{
+	sgid->global.subnet_prefix = cpu_to_be64(0xfe80000000000000LL);
+	sgid->raw[8] = mac_addr[0] ^ 2;
+	sgid->raw[9] = mac_addr[1];
+	sgid->raw[10] = mac_addr[2];
+	if (is_vlan) {
+		sgid->raw[11] = vlan_id >> 8;
+		sgid->raw[12] = vlan_id & 0xff;
+	} else {
+		sgid->raw[11] = 0xff;
+		sgid->raw[12] = 0xfe;
+	}
+	sgid->raw[13] = mac_addr[3];
+	sgid->raw[14] = mac_addr[4];
+	sgid->raw[15] = mac_addr[5];
+}
+
+static void ocrdma_add_sgid(struct ocrdma_dev *dev, unsigned char *mac_addr,
+			    bool is_vlan, u16 vlan_id)
+{
+	int i;
+	bool found = false;
+	union ib_gid new_sgid;
+	int free_idx = OCRDMA_MAX_SGID;
+	unsigned long flags;
+
+	memset(&ocrdma_zero_sgid, 0, sizeof(union ib_gid));
+
+	ocrdma_build_sgid_mac(&new_sgid, mac_addr, is_vlan, vlan_id);
+
+	spin_lock_irqsave(&dev->sgid_lock, flags);
+	for (i = 0; i < OCRDMA_MAX_SGID; i++) {
+		if (!memcmp(&dev->sgid_tbl[i], &ocrdma_zero_sgid,
+			    sizeof(union ib_gid))) {
+			/* found free entry */
+			if (!found) {
+				free_idx = i;
+				found = true;
+				break;
+			}
+		} else if (!memcmp(&dev->sgid_tbl[i], &new_sgid,
+				   sizeof(union ib_gid))) {
+			/* entry already present, no addition is required. */
+			spin_unlock_irqrestore(&dev->sgid_lock, flags);
+			return;
+		}
+	}
+	/* if entry doesn't exist and if table has some space, add entry */
+	if (found)
+		memcpy(&dev->sgid_tbl[free_idx], &new_sgid,
+		       sizeof(union ib_gid));
+	spin_unlock_irqrestore(&dev->sgid_lock, flags);
+}
+
+static bool ocrdma_del_sgid(struct ocrdma_dev *dev, unsigned char *mac_addr,
+			    bool is_vlan, u16 vlan_id)
+{
+	int found = false;
+	int i;
+	union ib_gid sgid;
+	unsigned long flags;
+
+	ocrdma_build_sgid_mac(&sgid, mac_addr, is_vlan, vlan_id);
+
+	spin_lock_irqsave(&dev->sgid_lock, flags);
+	/* first is default sgid, which cannot be deleted. */
+	for (i = 1; i < OCRDMA_MAX_SGID; i++) {
+		if (!memcmp(&dev->sgid_tbl[i], &sgid, sizeof(union ib_gid))) {
+			/* found matching entry */
+			memset(&dev->sgid_tbl[i], 0, sizeof(union ib_gid));
+			found = true;
+			break;
+		}
+	}
+	spin_unlock_irqrestore(&dev->sgid_lock, flags);
+	return found;
+}
+
+static void ocrdma_add_default_sgid(struct ocrdma_dev *dev)
+{
+	/* GID Index 0 - Invariant manufacturer-assigned EUI-64 */
+	union ib_gid *sgid = &dev->sgid_tbl[0];
+
+	sgid->global.subnet_prefix = cpu_to_be64(0xfe80000000000000LL);
+	ocrdma_get_guid(dev, &sgid->raw[8]);
+}
+
+static int ocrdma_build_sgid_tbl(struct ocrdma_dev *dev)
+{
+	struct net_device *netdev, *tmp;
+	u16 vlan_id;
+	bool is_vlan;
+
+	netdev = dev->nic_info.netdev;
+
+	ocrdma_add_default_sgid(dev);
+
+	rcu_read_lock();
+	for_each_netdev_rcu(&init_net, tmp) {
+		if (netdev == tmp || vlan_dev_real_dev(tmp) == netdev) {
+			if (!netif_running(tmp) || !netif_oper_up(tmp))
+				continue;
+			if (netdev != tmp) {
+				vlan_id = vlan_dev_vlan_id(tmp);
+				is_vlan = true;
+			} else {
+				is_vlan = false;
+				vlan_id = 0;
+				tmp = netdev;
+			}
+			ocrdma_add_sgid(dev, tmp->dev_addr, is_vlan, vlan_id);
+		}
+	}
+	rcu_read_unlock();
+	return 0;
+}
+
+#if defined(CONFIG_IPV6) || defined(CONFIG_IPV6_MODULE)
+
+static int ocrdma_inet6addr_event(struct notifier_block *notifier,
+				  unsigned long event, void *ptr)
+{
+	struct inet6_ifaddr *ifa = (struct inet6_ifaddr *)ptr;
+	struct net_device *event_netdev = ifa->idev->dev;
+	struct net_device *netdev = NULL;
+	struct ib_event gid_event;
+	struct ocrdma_dev *dev;
+	bool found = false;
+	bool is_vlan = false;
+	u16 vid = 0;
+
+	netdev = vlan_dev_real_dev(event_netdev);
+	if (netdev != event_netdev) {
+		is_vlan = true;
+		vid = vlan_dev_vlan_id(event_netdev);
+	}
+	rcu_read_lock();
+	list_for_each_entry_rcu(dev, &ocrdma_dev_list, entry) {
+		if (dev->nic_info.netdev == netdev) {
+			found = true;
+			break;
+		}
+	}
+	rcu_read_unlock();
+
+	if (!found)
+		return NOTIFY_DONE;
+	if (!rdma_link_local_addr((struct in6_addr *)&ifa->addr))
+		return NOTIFY_DONE;
+
+	mutex_lock(&dev->dev_lock);
+	switch (event) {
+	case NETDEV_UP:
+		ocrdma_add_sgid(dev, netdev->dev_addr, is_vlan, vid);
+		break;
+	case NETDEV_DOWN:
+		found = ocrdma_del_sgid(dev, netdev->dev_addr, is_vlan, vid);
+		if (found) {
+			/* found the matching entry, notify
+			 * the consumers about it
+			 */
+			gid_event.device = &dev->ibdev;
+			gid_event.element.port_num = 1;
+			gid_event.event = IB_EVENT_GID_CHANGE;
+			ib_dispatch_event(&gid_event);
+		}
+		break;
+	default:
+		break;
+	}
+	mutex_unlock(&dev->dev_lock);
+	return NOTIFY_OK;
+}
+
+static struct notifier_block ocrdma_inet6addr_notifier = {
+	.notifier_call = ocrdma_inet6addr_event
+};
+
+#endif /* IPV6 */
+
+static enum rdma_link_layer ocrdma_link_layer(struct ib_device *device,
+					      u8 port_num)
+{
+	return IB_LINK_LAYER_ETHERNET;
+}
+
+static int ocrdma_register_device(struct ocrdma_dev *dev)
+{
+	strlcpy(dev->ibdev.name, "ocrdma%d", IB_DEVICE_NAME_MAX);
+	ocrdma_get_guid(dev, (u8 *)&dev->ibdev.node_guid);
+	memcpy(dev->ibdev.node_desc, OCRDMA_NODE_DESC,
+	       sizeof(OCRDMA_NODE_DESC));
+	dev->ibdev.owner = THIS_MODULE;
+	dev->ibdev.uverbs_cmd_mask =
+	    OCRDMA_UVERBS(GET_CONTEXT) |
+	    OCRDMA_UVERBS(QUERY_DEVICE) |
+	    OCRDMA_UVERBS(QUERY_PORT) |
+	    OCRDMA_UVERBS(ALLOC_PD) |
+	    OCRDMA_UVERBS(DEALLOC_PD) |
+	    OCRDMA_UVERBS(REG_MR) |
+	    OCRDMA_UVERBS(DEREG_MR) |
+	    OCRDMA_UVERBS(CREATE_COMP_CHANNEL) |
+	    OCRDMA_UVERBS(CREATE_CQ) |
+	    OCRDMA_UVERBS(RESIZE_CQ) |
+	    OCRDMA_UVERBS(DESTROY_CQ) |
+	    OCRDMA_UVERBS(REQ_NOTIFY_CQ) |
+	    OCRDMA_UVERBS(CREATE_QP) |
+	    OCRDMA_UVERBS(MODIFY_QP) |
+	    OCRDMA_UVERBS(QUERY_QP) |
+	    OCRDMA_UVERBS(DESTROY_QP) |
+	    OCRDMA_UVERBS(POLL_CQ) |
+	    OCRDMA_UVERBS(POST_SEND) |
+	    OCRDMA_UVERBS(POST_RECV);
+
+	dev->ibdev.uverbs_cmd_mask |=
+	    OCRDMA_UVERBS(CREATE_AH) |
+	     OCRDMA_UVERBS(MODIFY_AH) |
+	     OCRDMA_UVERBS(QUERY_AH) |
+	     OCRDMA_UVERBS(DESTROY_AH);
+
+	dev->ibdev.node_type = RDMA_NODE_IB_CA;
+	dev->ibdev.phys_port_cnt = 1;
+	dev->ibdev.num_comp_vectors = 1;
+
+	/* mandatory verbs. */
+	dev->ibdev.query_device = ocrdma_query_device;
+	dev->ibdev.query_port = ocrdma_query_port;
+	dev->ibdev.modify_port = ocrdma_modify_port;
+	dev->ibdev.query_gid = ocrdma_query_gid;
+	dev->ibdev.get_link_layer = ocrdma_link_layer;
+	dev->ibdev.alloc_pd = ocrdma_alloc_pd;
+	dev->ibdev.dealloc_pd = ocrdma_dealloc_pd;
+
+	dev->ibdev.create_cq = ocrdma_create_cq;
+	dev->ibdev.destroy_cq = ocrdma_destroy_cq;
+	dev->ibdev.resize_cq = ocrdma_resize_cq;
+
+	dev->ibdev.create_qp = ocrdma_create_qp;
+	dev->ibdev.modify_qp = ocrdma_modify_qp;
+	dev->ibdev.query_qp = ocrdma_query_qp;
+	dev->ibdev.destroy_qp = ocrdma_destroy_qp;
+
+	dev->ibdev.query_pkey = ocrdma_query_pkey;
+	dev->ibdev.create_ah = ocrdma_create_ah;
+	dev->ibdev.destroy_ah = ocrdma_destroy_ah;
+	dev->ibdev.query_ah = ocrdma_query_ah;
+	dev->ibdev.modify_ah = ocrdma_modify_ah;
+
+	dev->ibdev.poll_cq = ocrdma_poll_cq;
+	dev->ibdev.post_send = ocrdma_post_send;
+	dev->ibdev.post_recv = ocrdma_post_recv;
+	dev->ibdev.req_notify_cq = ocrdma_arm_cq;
+
+	dev->ibdev.get_dma_mr = ocrdma_get_dma_mr;
+	dev->ibdev.dereg_mr = ocrdma_dereg_mr;
+	dev->ibdev.reg_user_mr = ocrdma_reg_user_mr;
+
+	/* mandatory to support user space verbs consumer. */
+	dev->ibdev.alloc_ucontext = ocrdma_alloc_ucontext;
+	dev->ibdev.dealloc_ucontext = ocrdma_dealloc_ucontext;
+	dev->ibdev.mmap = ocrdma_mmap;
+	dev->ibdev.dma_device = &dev->nic_info.pdev->dev;
+
+	dev->ibdev.process_mad = ocrdma_process_mad;
+
+	if (dev->nic_info.dev_family == OCRDMA_GEN2_FAMILY) {
+		dev->ibdev.uverbs_cmd_mask |=
+		     OCRDMA_UVERBS(CREATE_SRQ) |
+		     OCRDMA_UVERBS(MODIFY_SRQ) |
+		     OCRDMA_UVERBS(QUERY_SRQ) |
+		     OCRDMA_UVERBS(DESTROY_SRQ) |
+		     OCRDMA_UVERBS(POST_SRQ_RECV);
+
+		dev->ibdev.create_srq = ocrdma_create_srq;
+		dev->ibdev.modify_srq = ocrdma_modify_srq;
+		dev->ibdev.query_srq = ocrdma_query_srq;
+		dev->ibdev.destroy_srq = ocrdma_destroy_srq;
+		dev->ibdev.post_srq_recv = ocrdma_post_srq_recv;
+	}
+	return ib_register_device(&dev->ibdev, NULL);
+}
+
+static int ocrdma_alloc_resources(struct ocrdma_dev *dev)
+{
+	mutex_init(&dev->dev_lock);
+	dev->sgid_tbl = kzalloc(sizeof(union ib_gid) *
+				OCRDMA_MAX_SGID, GFP_KERNEL);
+	if (!dev->sgid_tbl)
+		goto alloc_err;
+	spin_lock_init(&dev->sgid_lock);
+
+	dev->cq_tbl = kzalloc(sizeof(struct ocrdma_cq *) *
+			      OCRDMA_MAX_CQ, GFP_KERNEL);
+	if (!dev->cq_tbl)
+		goto alloc_err;
+
+	if (dev->attr.max_qp) {
+		dev->qp_tbl = kzalloc(sizeof(struct ocrdma_qp *) *
+				      OCRDMA_MAX_QP, GFP_KERNEL);
+		if (!dev->qp_tbl)
+			goto alloc_err;
+	}
+	spin_lock_init(&dev->av_tbl.lock);
+	spin_lock_init(&dev->flush_q_lock);
+	return 0;
+alloc_err:
+	ocrdma_err("%s(%d) error.\n", __func__, dev->id);
+	return -ENOMEM;
+}
+
+static void ocrdma_free_resources(struct ocrdma_dev *dev)
+{
+	kfree(dev->qp_tbl);
+	kfree(dev->cq_tbl);
+	kfree(dev->sgid_tbl);
+}
+
+static struct ocrdma_dev *ocrdma_add(struct be_dev_info *dev_info)
+{
+	int status = 0;
+	struct ocrdma_dev *dev;
+
+	dev = (struct ocrdma_dev *)ib_alloc_device(sizeof(struct ocrdma_dev));
+	if (!dev) {
+		ocrdma_err("Unable to allocate ib device\n");
+		return NULL;
+	}
+	dev->mbx_cmd = kzalloc(sizeof(struct ocrdma_mqe_emb_cmd), GFP_KERNEL);
+	if (!dev->mbx_cmd)
+		goto idr_err;
+
+	memcpy(&dev->nic_info, dev_info, sizeof(*dev_info));
+	dev->id = ocrdma_get_instance();
+	if (dev->id < 0)
+		goto idr_err;
+
+	status = ocrdma_init_hw(dev);
+	if (status)
+		goto init_err;
+
+	status = ocrdma_alloc_resources(dev);
+	if (status)
+		goto alloc_err;
+
+	status = ocrdma_build_sgid_tbl(dev);
+	if (status)
+		goto alloc_err;
+
+	status = ocrdma_register_device(dev);
+	if (status)
+		goto alloc_err;
+
+	spin_lock(&ocrdma_devlist_lock);
+	list_add_tail_rcu(&dev->entry, &ocrdma_dev_list);
+	spin_unlock(&ocrdma_devlist_lock);
+	return dev;
+
+alloc_err:
+	ocrdma_free_resources(dev);
+	ocrdma_cleanup_hw(dev);
+init_err:
+	idr_remove(&ocrdma_dev_id, dev->id);
+idr_err:
+	kfree(dev->mbx_cmd);
+	ib_dealloc_device(&dev->ibdev);
+	ocrdma_err("%s() leaving. ret=%d\n", __func__, status);
+	return NULL;
+}
+
+static void ocrdma_remove_free(struct rcu_head *rcu)
+{
+	struct ocrdma_dev *dev = container_of(rcu, struct ocrdma_dev, rcu);
+
+	ocrdma_free_resources(dev);
+	ocrdma_cleanup_hw(dev);
+
+	idr_remove(&ocrdma_dev_id, dev->id);
+	kfree(dev->mbx_cmd);
+	ib_dealloc_device(&dev->ibdev);
+}
+
+static void ocrdma_remove(struct ocrdma_dev *dev)
+{
+	/* first unregister with stack to stop all the active traffic
+	 * of the registered clients.
+	 */
+	ib_unregister_device(&dev->ibdev);
+
+	spin_lock(&ocrdma_devlist_lock);
+	list_del_rcu(&dev->entry);
+	spin_unlock(&ocrdma_devlist_lock);
+	call_rcu(&dev->rcu, ocrdma_remove_free);
+}
+
+static int ocrdma_open(struct ocrdma_dev *dev)
+{
+	struct ib_event port_event;
+
+	port_event.event = IB_EVENT_PORT_ACTIVE;
+	port_event.element.port_num = 1;
+	port_event.device = &dev->ibdev;
+	ib_dispatch_event(&port_event);
+	return 0;
+}
+
+static int ocrdma_close(struct ocrdma_dev *dev)
+{
+	int i;
+	struct ocrdma_qp *qp, **cur_qp;
+	struct ib_event err_event;
+	struct ib_qp_attr attrs;
+	int attr_mask = IB_QP_STATE;
+
+	attrs.qp_state = IB_QPS_ERR;
+	mutex_lock(&dev->dev_lock);
+	if (dev->qp_tbl) {
+		cur_qp = dev->qp_tbl;
+		for (i = 0; i < OCRDMA_MAX_QP; i++) {
+			qp = cur_qp[i];
+			if (qp) {
+				/* change the QP state to ERROR */
+				_ocrdma_modify_qp(&qp->ibqp, &attrs, attr_mask);
+
+				err_event.event = IB_EVENT_QP_FATAL;
+				err_event.element.qp = &qp->ibqp;
+				err_event.device = &dev->ibdev;
+				ib_dispatch_event(&err_event);
+			}
+		}
+	}
+	mutex_unlock(&dev->dev_lock);
+
+	err_event.event = IB_EVENT_PORT_ERR;
+	err_event.element.port_num = 1;
+	err_event.device = &dev->ibdev;
+	ib_dispatch_event(&err_event);
+	return 0;
+}
+
+/* event handling via NIC driver ensures that all the NIC specific
+ * initialization done before RoCE driver notifies
+ * event to stack.
+ */
+static void ocrdma_event_handler(struct ocrdma_dev *dev, u32 event)
+{
+	switch (event) {
+	case BE_DEV_UP:
+		ocrdma_open(dev);
+		break;
+	case BE_DEV_DOWN:
+		ocrdma_close(dev);
+		break;
+	};
+}
+
+static struct ocrdma_driver ocrdma_drv = {
+	.name			= "ocrdma_driver",
+	.add			= ocrdma_add,
+	.remove			= ocrdma_remove,
+	.state_change_handler	= ocrdma_event_handler,
+};
+
+static void ocrdma_unregister_inet6addr_notifier(void)
+{
+#if defined(CONFIG_IPV6) || defined(CONFIG_IPV6_MODULE)
+	unregister_inet6addr_notifier(&ocrdma_inet6addr_notifier);
+#endif
+}
+
+static int __init ocrdma_init_module(void)
+{
+	int status;
+
+#if defined(CONFIG_IPV6) || defined(CONFIG_IPV6_MODULE)
+	status = register_inet6addr_notifier(&ocrdma_inet6addr_notifier);
+	if (status)
+		return status;
+#endif
+
+	status = be_roce_register_driver(&ocrdma_drv);
+	if (status)
+		ocrdma_unregister_inet6addr_notifier();
+
+	return status;
+}
+
+static void __exit ocrdma_exit_module(void)
+{
+	be_roce_unregister_driver(&ocrdma_drv);
+	ocrdma_unregister_inet6addr_notifier();
+}
+
+module_init(ocrdma_init_module);
+module_exit(ocrdma_exit_module);
diff --git a/drivers/infiniband/hw/ocrdma/ocrdma_sli.h b/drivers/infiniband/hw/ocrdma/ocrdma_sli.h
new file mode 100644
index 00000000000..7fd80cc0f03
--- /dev/null
+++ b/drivers/infiniband/hw/ocrdma/ocrdma_sli.h
@@ -0,0 +1,1672 @@
+/*******************************************************************
+ * This file is part of the Emulex RoCE Device Driver for          *
+ * RoCE (RDMA over Converged Ethernet) adapters.                   *
+ * Copyright (C) 2008-2012 Emulex. All rights reserved.            *
+ * EMULEX and SLI are trademarks of Emulex.                        *
+ * www.emulex.com                                                  *
+ *                                                                 *
+ * 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. *
+ * ALL EXPRESS OR IMPLIED CONDITIONS, REPRESENTATIONS AND          *
+ * WARRANTIES, INCLUDING ANY IMPLIED WARRANTY OF MERCHANTABILITY,  *
+ * FITNESS FOR A PARTICULAR PURPOSE, OR NON-INFRINGEMENT, ARE      *
+ * DISCLAIMED, EXCEPT TO THE EXTENT THAT SUCH DISCLAIMERS ARE HELD *
+ * TO BE LEGALLY INVALID.  See the GNU General Public License for  *
+ * more details, a copy of which can be found in the file COPYING  *
+ * included with this package.                                     *
+ *
+ * Contact Information:
+ * linux-drivers@emulex.com
+ *
+ * Emulex
+ * 3333 Susan Street
+ * Costa Mesa, CA 92626
+ *******************************************************************/
+
+#ifndef __OCRDMA_SLI_H__
+#define __OCRDMA_SLI_H__
+
+#define Bit(_b) (1 << (_b))
+
+#define OCRDMA_GEN1_FAMILY	0xB
+#define OCRDMA_GEN2_FAMILY	0x2
+
+#define OCRDMA_SUBSYS_ROCE 10
+enum {
+	OCRDMA_CMD_QUERY_CONFIG = 1,
+	OCRDMA_CMD_ALLOC_PD,
+	OCRDMA_CMD_DEALLOC_PD,
+
+	OCRDMA_CMD_CREATE_AH_TBL,
+	OCRDMA_CMD_DELETE_AH_TBL,
+
+	OCRDMA_CMD_CREATE_QP,
+	OCRDMA_CMD_QUERY_QP,
+	OCRDMA_CMD_MODIFY_QP,
+	OCRDMA_CMD_DELETE_QP,
+
+	OCRDMA_CMD_RSVD1,
+	OCRDMA_CMD_ALLOC_LKEY,
+	OCRDMA_CMD_DEALLOC_LKEY,
+	OCRDMA_CMD_REGISTER_NSMR,
+	OCRDMA_CMD_REREGISTER_NSMR,
+	OCRDMA_CMD_REGISTER_NSMR_CONT,
+	OCRDMA_CMD_QUERY_NSMR,
+	OCRDMA_CMD_ALLOC_MW,
+	OCRDMA_CMD_QUERY_MW,
+
+	OCRDMA_CMD_CREATE_SRQ,
+	OCRDMA_CMD_QUERY_SRQ,
+	OCRDMA_CMD_MODIFY_SRQ,
+	OCRDMA_CMD_DELETE_SRQ,
+
+	OCRDMA_CMD_ATTACH_MCAST,
+	OCRDMA_CMD_DETACH_MCAST,
+
+	OCRDMA_CMD_MAX
+};
+
+#define OCRDMA_SUBSYS_COMMON 1
+enum {
+	OCRDMA_CMD_CREATE_CQ		= 12,
+	OCRDMA_CMD_CREATE_EQ		= 13,
+	OCRDMA_CMD_CREATE_MQ		= 21,
+	OCRDMA_CMD_GET_FW_VER		= 35,
+	OCRDMA_CMD_DELETE_MQ		= 53,
+	OCRDMA_CMD_DELETE_CQ		= 54,
+	OCRDMA_CMD_DELETE_EQ		= 55,
+	OCRDMA_CMD_GET_FW_CONFIG	= 58,
+	OCRDMA_CMD_CREATE_MQ_EXT	= 90
+};
+
+enum {
+	QTYPE_EQ	= 1,
+	QTYPE_CQ	= 2,
+	QTYPE_MCCQ	= 3
+};
+
+#define OCRDMA_MAX_SGID (8)
+
+#define OCRDMA_MAX_QP    2048
+#define OCRDMA_MAX_CQ    2048
+
+enum {
+	OCRDMA_DB_RQ_OFFSET		= 0xE0,
+	OCRDMA_DB_GEN2_RQ1_OFFSET	= 0x100,
+	OCRDMA_DB_GEN2_RQ2_OFFSET	= 0xC0,
+	OCRDMA_DB_SQ_OFFSET		= 0x60,
+	OCRDMA_DB_GEN2_SQ_OFFSET	= 0x1C0,
+	OCRDMA_DB_SRQ_OFFSET		= OCRDMA_DB_RQ_OFFSET,
+	OCRDMA_DB_GEN2_SRQ_OFFSET	= OCRDMA_DB_GEN2_RQ1_OFFSET,
+	OCRDMA_DB_CQ_OFFSET		= 0x120,
+	OCRDMA_DB_EQ_OFFSET		= OCRDMA_DB_CQ_OFFSET,
+	OCRDMA_DB_MQ_OFFSET		= 0x140
+};
+
+#define OCRDMA_DB_CQ_RING_ID_MASK       0x3FF	/* bits 0 - 9 */
+#define OCRDMA_DB_CQ_RING_ID_EXT_MASK  0x0C00	/* bits 10-11 of qid at 12-11 */
+/* qid #2 msbits at 12-11 */
+#define OCRDMA_DB_CQ_RING_ID_EXT_MASK_SHIFT  0x1
+#define OCRDMA_DB_CQ_NUM_POPPED_SHIFT       (16)	/* bits 16 - 28 */
+/* Rearm bit */
+#define OCRDMA_DB_CQ_REARM_SHIFT        (29)	/* bit 29 */
+/* solicited bit */
+#define OCRDMA_DB_CQ_SOLICIT_SHIFT   (31)	/* bit 31 */
+
+#define OCRDMA_EQ_ID_MASK		0x1FF	/* bits 0 - 8 */
+#define OCRDMA_EQ_ID_EXT_MASK		0x3e00	/* bits 9-13 */
+#define OCRDMA_EQ_ID_EXT_MASK_SHIFT	(2)	/* qid bits 9-13 at 11-15 */
+
+/* Clear the interrupt for this eq */
+#define OCRDMA_EQ_CLR_SHIFT			(9)	/* bit 9 */
+/* Must be 1 */
+#define OCRDMA_EQ_TYPE_SHIFT		(10)	/* bit 10 */
+/* Number of event entries processed */
+#define OCRDMA_NUM_EQE_SHIFT		(16)	/* bits 16 - 28 */
+/* Rearm bit */
+#define OCRDMA_REARM_SHIFT		(29)	/* bit 29 */
+
+#define OCRDMA_MQ_ID_MASK		0x7FF	/* bits 0 - 10 */
+/* Number of entries posted */
+#define OCRDMA_MQ_NUM_MQE_SHIFT	(16)	/* bits 16 - 29 */
+
+#define OCRDMA_MIN_HPAGE_SIZE (4096)
+
+#define OCRDMA_MIN_Q_PAGE_SIZE (4096)
+#define OCRDMA_MAX_Q_PAGES     (8)
+
+/*
+# 0: 4K Bytes
+# 1: 8K Bytes
+# 2: 16K Bytes
+# 3: 32K Bytes
+# 4: 64K Bytes
+*/
+#define OCRDMA_MAX_Q_PAGE_SIZE_CNT (5)
+#define OCRDMA_Q_PAGE_BASE_SIZE (OCRDMA_MIN_Q_PAGE_SIZE * OCRDMA_MAX_Q_PAGES)
+
+#define MAX_OCRDMA_QP_PAGES      (8)
+#define OCRDMA_MAX_WQE_MEM_SIZE (MAX_OCRDMA_QP_PAGES * OCRDMA_MIN_HQ_PAGE_SIZE)
+
+#define OCRDMA_CREATE_CQ_MAX_PAGES (4)
+#define OCRDMA_DPP_CQE_SIZE (4)
+
+#define OCRDMA_GEN2_MAX_CQE 1024
+#define OCRDMA_GEN2_CQ_PAGE_SIZE 4096
+#define OCRDMA_GEN2_WQE_SIZE 256
+#define OCRDMA_MAX_CQE  4095
+#define OCRDMA_CQ_PAGE_SIZE 16384
+#define OCRDMA_WQE_SIZE 128
+#define OCRDMA_WQE_STRIDE 8
+#define OCRDMA_WQE_ALIGN_BYTES 16
+
+#define MAX_OCRDMA_SRQ_PAGES MAX_OCRDMA_QP_PAGES
+
+enum {
+	OCRDMA_MCH_OPCODE_SHIFT	= 0,
+	OCRDMA_MCH_OPCODE_MASK	= 0xFF,
+	OCRDMA_MCH_SUBSYS_SHIFT	= 8,
+	OCRDMA_MCH_SUBSYS_MASK	= 0xFF00
+};
+
+/* mailbox cmd header */
+struct ocrdma_mbx_hdr {
+	u32 subsys_op;
+	u32 timeout;		/* in seconds */
+	u32 cmd_len;
+	u32 rsvd_version;
+} __packed;
+
+enum {
+	OCRDMA_MBX_RSP_OPCODE_SHIFT	= 0,
+	OCRDMA_MBX_RSP_OPCODE_MASK	= 0xFF,
+	OCRDMA_MBX_RSP_SUBSYS_SHIFT	= 8,
+	OCRDMA_MBX_RSP_SUBSYS_MASK	= 0xFF << OCRDMA_MBX_RSP_SUBSYS_SHIFT,
+
+	OCRDMA_MBX_RSP_STATUS_SHIFT	= 0,
+	OCRDMA_MBX_RSP_STATUS_MASK	= 0xFF,
+	OCRDMA_MBX_RSP_ASTATUS_SHIFT	= 8,
+	OCRDMA_MBX_RSP_ASTATUS_MASK	= 0xFF << OCRDMA_MBX_RSP_ASTATUS_SHIFT
+};
+
+/* mailbox cmd response */
+struct ocrdma_mbx_rsp {
+	u32 subsys_op;
+	u32 status;
+	u32 rsp_len;
+	u32 add_rsp_len;
+} __packed;
+
+enum {
+	OCRDMA_MQE_EMBEDDED	= 1,
+	OCRDMA_MQE_NONEMBEDDED	= 0
+};
+
+struct ocrdma_mqe_sge {
+	u32 pa_lo;
+	u32 pa_hi;
+	u32 len;
+} __packed;
+
+enum {
+	OCRDMA_MQE_HDR_EMB_SHIFT	= 0,
+	OCRDMA_MQE_HDR_EMB_MASK		= Bit(0),
+	OCRDMA_MQE_HDR_SGE_CNT_SHIFT	= 3,
+	OCRDMA_MQE_HDR_SGE_CNT_MASK	= 0x1F << OCRDMA_MQE_HDR_SGE_CNT_SHIFT,
+	OCRDMA_MQE_HDR_SPECIAL_SHIFT	= 24,
+	OCRDMA_MQE_HDR_SPECIAL_MASK	= 0xFF << OCRDMA_MQE_HDR_SPECIAL_SHIFT
+};
+
+struct ocrdma_mqe_hdr {
+	u32 spcl_sge_cnt_emb;
+	u32 pyld_len;
+	u32 tag_lo;
+	u32 tag_hi;
+	u32 rsvd3;
+} __packed;
+
+struct ocrdma_mqe_emb_cmd {
+	struct ocrdma_mbx_hdr mch;
+	u8 pyld[220];
+} __packed;
+
+struct ocrdma_mqe {
+	struct ocrdma_mqe_hdr hdr;
+	union {
+		struct ocrdma_mqe_emb_cmd emb_req;
+		struct {
+			struct ocrdma_mqe_sge sge[19];
+		} nonemb_req;
+		u8 cmd[236];
+		struct ocrdma_mbx_rsp rsp;
+	} u;
+} __packed;
+
+#define OCRDMA_EQ_LEN       4096
+#define OCRDMA_MQ_CQ_LEN    256
+#define OCRDMA_MQ_LEN       128
+
+#define PAGE_SHIFT_4K		12
+#define PAGE_SIZE_4K		(1 << PAGE_SHIFT_4K)
+
+/* Returns number of pages spanned by the data starting at the given addr */
+#define PAGES_4K_SPANNED(_address, size) \
+	((u32)((((size_t)(_address) & (PAGE_SIZE_4K - 1)) +	\
+			(size) + (PAGE_SIZE_4K - 1)) >> PAGE_SHIFT_4K))
+
+struct ocrdma_delete_q_req {
+	struct ocrdma_mbx_hdr req;
+	u32 id;
+} __packed;
+
+struct ocrdma_pa {
+	u32 lo;
+	u32 hi;
+} __packed;
+
+#define MAX_OCRDMA_EQ_PAGES (8)
+struct ocrdma_create_eq_req {
+	struct ocrdma_mbx_hdr req;
+	u32 num_pages;
+	u32 valid;
+	u32 cnt;
+	u32 delay;
+	u32 rsvd;
+	struct ocrdma_pa pa[MAX_OCRDMA_EQ_PAGES];
+} __packed;
+
+enum {
+	OCRDMA_CREATE_EQ_VALID	= Bit(29),
+	OCRDMA_CREATE_EQ_CNT_SHIFT	= 26,
+	OCRDMA_CREATE_CQ_DELAY_SHIFT	= 13,
+};
+
+struct ocrdma_create_eq_rsp {
+	struct ocrdma_mbx_rsp rsp;
+	u32 vector_eqid;
+};
+
+#define OCRDMA_EQ_MINOR_OTHER (0x1)
+
+enum {
+	OCRDMA_MCQE_STATUS_SHIFT	= 0,
+	OCRDMA_MCQE_STATUS_MASK		= 0xFFFF,
+	OCRDMA_MCQE_ESTATUS_SHIFT	= 16,
+	OCRDMA_MCQE_ESTATUS_MASK	= 0xFFFF << OCRDMA_MCQE_ESTATUS_SHIFT,
+	OCRDMA_MCQE_CONS_SHIFT		= 27,
+	OCRDMA_MCQE_CONS_MASK		= Bit(27),
+	OCRDMA_MCQE_CMPL_SHIFT		= 28,
+	OCRDMA_MCQE_CMPL_MASK		= Bit(28),
+	OCRDMA_MCQE_AE_SHIFT		= 30,
+	OCRDMA_MCQE_AE_MASK		= Bit(30),
+	OCRDMA_MCQE_VALID_SHIFT		= 31,
+	OCRDMA_MCQE_VALID_MASK		= Bit(31)
+};
+
+struct ocrdma_mcqe {
+	u32 status;
+	u32 tag_lo;
+	u32 tag_hi;
+	u32 valid_ae_cmpl_cons;
+} __packed;
+
+enum {
+	OCRDMA_AE_MCQE_QPVALID		= Bit(31),
+	OCRDMA_AE_MCQE_QPID_MASK	= 0xFFFF,
+
+	OCRDMA_AE_MCQE_CQVALID		= Bit(31),
+	OCRDMA_AE_MCQE_CQID_MASK	= 0xFFFF,
+	OCRDMA_AE_MCQE_VALID		= Bit(31),
+	OCRDMA_AE_MCQE_AE		= Bit(30),
+	OCRDMA_AE_MCQE_EVENT_TYPE_SHIFT	= 16,
+	OCRDMA_AE_MCQE_EVENT_TYPE_MASK	=
+					0xFF << OCRDMA_AE_MCQE_EVENT_TYPE_SHIFT,
+	OCRDMA_AE_MCQE_EVENT_CODE_SHIFT	= 8,
+	OCRDMA_AE_MCQE_EVENT_CODE_MASK	=
+					0xFF << OCRDMA_AE_MCQE_EVENT_CODE_SHIFT
+};
+struct ocrdma_ae_mcqe {
+	u32 qpvalid_qpid;
+	u32 cqvalid_cqid;
+	u32 evt_tag;
+	u32 valid_ae_event;
+} __packed;
+
+enum {
+	OCRDMA_AE_MPA_MCQE_REQ_ID_SHIFT		= 16,
+	OCRDMA_AE_MPA_MCQE_REQ_ID_MASK		= 0xFFFF <<
+					OCRDMA_AE_MPA_MCQE_REQ_ID_SHIFT,
+
+	OCRDMA_AE_MPA_MCQE_EVENT_CODE_SHIFT	= 8,
+	OCRDMA_AE_MPA_MCQE_EVENT_CODE_MASK	= 0xFF <<
+					OCRDMA_AE_MPA_MCQE_EVENT_CODE_SHIFT,
+	OCRDMA_AE_MPA_MCQE_EVENT_TYPE_SHIFT	= 16,
+	OCRDMA_AE_MPA_MCQE_EVENT_TYPE_MASK	= 0xFF <<
+					OCRDMA_AE_MPA_MCQE_EVENT_TYPE_SHIFT,
+	OCRDMA_AE_MPA_MCQE_EVENT_AE_SHIFT	= 30,
+	OCRDMA_AE_MPA_MCQE_EVENT_AE_MASK	= Bit(30),
+	OCRDMA_AE_MPA_MCQE_EVENT_VALID_SHIFT	= 31,
+	OCRDMA_AE_MPA_MCQE_EVENT_VALID_MASK	= Bit(31)
+};
+
+struct ocrdma_ae_mpa_mcqe {
+	u32 req_id;
+	u32 w1;
+	u32 w2;
+	u32 valid_ae_event;
+} __packed;
+
+enum {
+	OCRDMA_AE_QP_MCQE_NEW_QP_STATE_SHIFT	= 0,
+	OCRDMA_AE_QP_MCQE_NEW_QP_STATE_MASK	= 0xFFFF,
+	OCRDMA_AE_QP_MCQE_QP_ID_SHIFT		= 16,
+	OCRDMA_AE_QP_MCQE_QP_ID_MASK		= 0xFFFF <<
+						OCRDMA_AE_QP_MCQE_QP_ID_SHIFT,
+
+	OCRDMA_AE_QP_MCQE_EVENT_CODE_SHIFT	= 8,
+	OCRDMA_AE_QP_MCQE_EVENT_CODE_MASK	= 0xFF <<
+				OCRDMA_AE_QP_MCQE_EVENT_CODE_SHIFT,
+	OCRDMA_AE_QP_MCQE_EVENT_TYPE_SHIFT	= 16,
+	OCRDMA_AE_QP_MCQE_EVENT_TYPE_MASK	= 0xFF <<
+				OCRDMA_AE_QP_MCQE_EVENT_TYPE_SHIFT,
+	OCRDMA_AE_QP_MCQE_EVENT_AE_SHIFT	= 30,
+	OCRDMA_AE_QP_MCQE_EVENT_AE_MASK		= Bit(30),
+	OCRDMA_AE_QP_MCQE_EVENT_VALID_SHIFT	= 31,
+	OCRDMA_AE_QP_MCQE_EVENT_VALID_MASK	= Bit(31)
+};
+
+struct ocrdma_ae_qp_mcqe {
+	u32 qp_id_state;
+	u32 w1;
+	u32 w2;
+	u32 valid_ae_event;
+} __packed;
+
+#define OCRDMA_ASYNC_EVE_CODE 0x14
+
+enum OCRDMA_ASYNC_EVENT_TYPE {
+	OCRDMA_CQ_ERROR			= 0x00,
+	OCRDMA_CQ_OVERRUN_ERROR		= 0x01,
+	OCRDMA_CQ_QPCAT_ERROR		= 0x02,
+	OCRDMA_QP_ACCESS_ERROR		= 0x03,
+	OCRDMA_QP_COMM_EST_EVENT	= 0x04,
+	OCRDMA_SQ_DRAINED_EVENT		= 0x05,
+	OCRDMA_DEVICE_FATAL_EVENT	= 0x08,
+	OCRDMA_SRQCAT_ERROR		= 0x0E,
+	OCRDMA_SRQ_LIMIT_EVENT		= 0x0F,
+	OCRDMA_QP_LAST_WQE_EVENT	= 0x10
+};
+
+/* mailbox command request and responses */
+enum {
+	OCRDMA_MBX_QUERY_CFG_CQ_OVERFLOW_SHIFT		= 2,
+	OCRDMA_MBX_QUERY_CFG_CQ_OVERFLOW_MASK		= Bit(2),
+	OCRDMA_MBX_QUERY_CFG_SRQ_SUPPORTED_SHIFT	= 3,
+	OCRDMA_MBX_QUERY_CFG_SRQ_SUPPORTED_MASK		= Bit(3),
+	OCRDMA_MBX_QUERY_CFG_MAX_QP_SHIFT		= 8,
+	OCRDMA_MBX_QUERY_CFG_MAX_QP_MASK		= 0xFFFFFF <<
+				OCRDMA_MBX_QUERY_CFG_MAX_QP_SHIFT,
+
+	OCRDMA_MBX_QUERY_CFG_MAX_PD_SHIFT		= 16,
+	OCRDMA_MBX_QUERY_CFG_MAX_PD_MASK		= 0xFFFF <<
+					OCRDMA_MBX_QUERY_CFG_MAX_PD_SHIFT,
+	OCRDMA_MBX_QUERY_CFG_CA_ACK_DELAY_SHIFT		= 8,
+	OCRDMA_MBX_QUERY_CFG_CA_ACK_DELAY_MASK		= 0xFF <<
+				OCRDMA_MBX_QUERY_CFG_CA_ACK_DELAY_SHIFT,
+
+	OCRDMA_MBX_QUERY_CFG_MAX_SEND_SGE_SHIFT		= 0,
+	OCRDMA_MBX_QUERY_CFG_MAX_SEND_SGE_MASK		= 0xFFFF,
+
+	OCRDMA_MBX_QUERY_CFG_MAX_ORD_PER_QP_SHIFT	= 0,
+	OCRDMA_MBX_QUERY_CFG_MAX_ORD_PER_QP_MASK	= 0xFFFF,
+	OCRDMA_MBX_QUERY_CFG_MAX_IRD_PER_QP_SHIFT	= 16,
+	OCRDMA_MBX_QUERY_CFG_MAX_IRD_PER_QP_MASK	= 0xFFFF <<
+				OCRDMA_MBX_QUERY_CFG_MAX_IRD_PER_QP_SHIFT,
+
+	OCRDMA_MBX_QUERY_CFG_MAX_WQE_SIZE_OFFSET	= 24,
+	OCRDMA_MBX_QUERY_CFG_MAX_WQE_SIZE_MASK		= 0xFF <<
+				OCRDMA_MBX_QUERY_CFG_MAX_WQE_SIZE_OFFSET,
+	OCRDMA_MBX_QUERY_CFG_MAX_RQE_SIZE_OFFSET	= 16,
+	OCRDMA_MBX_QUERY_CFG_MAX_RQE_SIZE_MASK		= 0xFF <<
+				OCRDMA_MBX_QUERY_CFG_MAX_RQE_SIZE_OFFSET,
+	OCRDMA_MBX_QUERY_CFG_MAX_DPP_CQES_OFFSET	= 0,
+	OCRDMA_MBX_QUERY_CFG_MAX_DPP_CQES_MASK		= 0xFFFF <<
+				OCRDMA_MBX_QUERY_CFG_MAX_DPP_CQES_OFFSET,
+
+	OCRDMA_MBX_QUERY_CFG_MAX_SRQ_OFFSET		= 16,
+	OCRDMA_MBX_QUERY_CFG_MAX_SRQ_MASK		= 0xFFFF <<
+				OCRDMA_MBX_QUERY_CFG_MAX_SRQ_OFFSET,
+	OCRDMA_MBX_QUERY_CFG_MAX_RPIR_QPS_OFFSET	= 0,
+	OCRDMA_MBX_QUERY_CFG_MAX_RPIR_QPS_MASK		= 0xFFFF <<
+				OCRDMA_MBX_QUERY_CFG_MAX_RPIR_QPS_OFFSET,
+
+	OCRDMA_MBX_QUERY_CFG_MAX_DPP_PDS_OFFSET		= 16,
+	OCRDMA_MBX_QUERY_CFG_MAX_DPP_PDS_MASK		= 0xFFFF <<
+				OCRDMA_MBX_QUERY_CFG_MAX_DPP_PDS_OFFSET,
+	OCRDMA_MBX_QUERY_CFG_MAX_DPP_CREDITS_OFFSET	= 0,
+	OCRDMA_MBX_QUERY_CFG_MAX_DPP_CREDITS_MASK	= 0xFFFF <<
+				OCRDMA_MBX_QUERY_CFG_MAX_DPP_CREDITS_OFFSET,
+
+	OCRDMA_MBX_QUERY_CFG_MAX_DPP_QPS_OFFSET		= 0,
+	OCRDMA_MBX_QUERY_CFG_MAX_DPP_QPS_MASK		= 0xFFFF <<
+				OCRDMA_MBX_QUERY_CFG_MAX_DPP_QPS_OFFSET,
+
+	OCRDMA_MBX_QUERY_CFG_MAX_WQES_PER_WQ_OFFSET	= 16,
+	OCRDMA_MBX_QUERY_CFG_MAX_WQES_PER_WQ_MASK	= 0xFFFF <<
+				OCRDMA_MBX_QUERY_CFG_MAX_WQES_PER_WQ_OFFSET,
+	OCRDMA_MBX_QUERY_CFG_MAX_RQES_PER_RQ_OFFSET	= 0,
+	OCRDMA_MBX_QUERY_CFG_MAX_RQES_PER_RQ_MASK	= 0xFFFF <<
+				OCRDMA_MBX_QUERY_CFG_MAX_WQES_PER_WQ_OFFSET,
+
+	OCRDMA_MBX_QUERY_CFG_MAX_CQ_OFFSET		= 16,
+	OCRDMA_MBX_QUERY_CFG_MAX_CQ_MASK		= 0xFFFF <<
+				OCRDMA_MBX_QUERY_CFG_MAX_CQ_OFFSET,
+	OCRDMA_MBX_QUERY_CFG_MAX_CQES_PER_CQ_OFFSET	= 0,
+	OCRDMA_MBX_QUERY_CFG_MAX_CQES_PER_CQ_MASK	= 0xFFFF <<
+				OCRDMA_MBX_QUERY_CFG_MAX_CQES_PER_CQ_OFFSET,
+
+	OCRDMA_MBX_QUERY_CFG_MAX_SRQ_RQE_OFFSET		= 16,
+	OCRDMA_MBX_QUERY_CFG_MAX_SRQ_RQE_MASK		= 0xFFFF <<
+				OCRDMA_MBX_QUERY_CFG_MAX_SRQ_RQE_OFFSET,
+	OCRDMA_MBX_QUERY_CFG_MAX_SRQ_SGE_OFFSET		= 0,
+	OCRDMA_MBX_QUERY_CFG_MAX_SRQ_SGE_MASK		= 0xFFFF <<
+				OCRDMA_MBX_QUERY_CFG_MAX_SRQ_SGE_OFFSET,
+};
+
+struct ocrdma_mbx_query_config {
+	struct ocrdma_mqe_hdr hdr;
+	struct ocrdma_mbx_rsp rsp;
+	u32 qp_srq_cq_ird_ord;
+	u32 max_pd_ca_ack_delay;
+	u32 max_write_send_sge;
+	u32 max_ird_ord_per_qp;
+	u32 max_shared_ird_ord;
+	u32 max_mr;
+	u64 max_mr_size;
+	u32 max_num_mr_pbl;
+	u32 max_mw;
+	u32 max_fmr;
+	u32 max_pages_per_frmr;
+	u32 max_mcast_group;
+	u32 max_mcast_qp_attach;
+	u32 max_total_mcast_qp_attach;
+	u32 wqe_rqe_stride_max_dpp_cqs;
+	u32 max_srq_rpir_qps;
+	u32 max_dpp_pds_credits;
+	u32 max_dpp_credits_pds_per_pd;
+	u32 max_wqes_rqes_per_q;
+	u32 max_cq_cqes_per_cq;
+	u32 max_srq_rqe_sge;
+} __packed;
+
+struct ocrdma_fw_ver_rsp {
+	struct ocrdma_mqe_hdr hdr;
+	struct ocrdma_mbx_rsp rsp;
+
+	u8 running_ver[32];
+} __packed;
+
+struct ocrdma_fw_conf_rsp {
+	struct ocrdma_mqe_hdr hdr;
+	struct ocrdma_mbx_rsp rsp;
+
+	u32 config_num;
+	u32 asic_revision;
+	u32 phy_port;
+	u32 fn_mode;
+	struct {
+		u32 mode;
+		u32 nic_wqid_base;
+		u32 nic_wq_tot;
+		u32 prot_wqid_base;
+		u32 prot_wq_tot;
+		u32 prot_rqid_base;
+		u32 prot_rqid_tot;
+		u32 rsvd[6];
+	} ulp[2];
+	u32 fn_capabilities;
+	u32 rsvd1;
+	u32 rsvd2;
+	u32 base_eqid;
+	u32 max_eq;
+
+} __packed;
+
+enum {
+	OCRDMA_FN_MODE_RDMA	= 0x4
+};
+
+enum {
+	OCRDMA_CREATE_CQ_VER2			= 2,
+
+	OCRDMA_CREATE_CQ_PAGE_CNT_MASK		= 0xFFFF,
+	OCRDMA_CREATE_CQ_PAGE_SIZE_SHIFT	= 16,
+	OCRDMA_CREATE_CQ_PAGE_SIZE_MASK		= 0xFF,
+
+	OCRDMA_CREATE_CQ_COALESCWM_SHIFT	= 12,
+	OCRDMA_CREATE_CQ_COALESCWM_MASK		= Bit(13) | Bit(12),
+	OCRDMA_CREATE_CQ_FLAGS_NODELAY		= Bit(14),
+	OCRDMA_CREATE_CQ_FLAGS_AUTO_VALID	= Bit(15),
+
+	OCRDMA_CREATE_CQ_EQ_ID_MASK		= 0xFFFF,
+	OCRDMA_CREATE_CQ_CQE_COUNT_MASK		= 0xFFFF
+};
+
+enum {
+	OCRDMA_CREATE_CQ_VER0			= 0,
+	OCRDMA_CREATE_CQ_DPP			= 1,
+	OCRDMA_CREATE_CQ_TYPE_SHIFT		= 24,
+	OCRDMA_CREATE_CQ_EQID_SHIFT		= 22,
+
+	OCRDMA_CREATE_CQ_CNT_SHIFT		= 27,
+	OCRDMA_CREATE_CQ_FLAGS_VALID		= Bit(29),
+	OCRDMA_CREATE_CQ_FLAGS_EVENTABLE	= Bit(31),
+	OCRDMA_CREATE_CQ_DEF_FLAGS		= OCRDMA_CREATE_CQ_FLAGS_VALID |
+					OCRDMA_CREATE_CQ_FLAGS_EVENTABLE |
+					OCRDMA_CREATE_CQ_FLAGS_NODELAY
+};
+
+struct ocrdma_create_cq_cmd {
+	struct ocrdma_mbx_hdr req;
+	u32 pgsz_pgcnt;
+	u32 ev_cnt_flags;
+	u32 eqn;
+	u32 cqe_count;
+	u32 rsvd6;
+	struct ocrdma_pa pa[OCRDMA_CREATE_CQ_MAX_PAGES];
+};
+
+struct ocrdma_create_cq {
+	struct ocrdma_mqe_hdr hdr;
+	struct ocrdma_create_cq_cmd cmd;
+} __packed;
+
+enum {
+	OCRDMA_CREATE_CQ_RSP_CQ_ID_MASK	= 0xFFFF
+};
+
+struct ocrdma_create_cq_cmd_rsp {
+	struct ocrdma_mbx_rsp rsp;
+	u32 cq_id;
+} __packed;
+
+struct ocrdma_create_cq_rsp {
+	struct ocrdma_mqe_hdr hdr;
+	struct ocrdma_create_cq_cmd_rsp rsp;
+} __packed;
+
+enum {
+	OCRDMA_CREATE_MQ_V0_CQ_ID_SHIFT		= 22,
+	OCRDMA_CREATE_MQ_CQ_ID_SHIFT		= 16,
+	OCRDMA_CREATE_MQ_RING_SIZE_SHIFT	= 16,
+	OCRDMA_CREATE_MQ_VALID			= Bit(31),
+	OCRDMA_CREATE_MQ_ASYNC_CQ_VALID		= Bit(0)
+};
+
+struct ocrdma_create_mq_v0 {
+	u32 pages;
+	u32 cqid_ringsize;
+	u32 valid;
+	u32 async_cqid_valid;
+	u32 rsvd;
+	struct ocrdma_pa pa[8];
+} __packed;
+
+struct ocrdma_create_mq_v1 {
+	u32 cqid_pages;
+	u32 async_event_bitmap;
+	u32 async_cqid_ringsize;
+	u32 valid;
+	u32 async_cqid_valid;
+	u32 rsvd;
+	struct ocrdma_pa pa[8];
+} __packed;
+
+struct ocrdma_create_mq_req {
+	struct ocrdma_mbx_hdr req;
+	union {
+		struct ocrdma_create_mq_v0 v0;
+		struct ocrdma_create_mq_v1 v1;
+	};
+} __packed;
+
+struct ocrdma_create_mq_rsp {
+	struct ocrdma_mbx_rsp rsp;
+	u32 id;
+} __packed;
+
+enum {
+	OCRDMA_DESTROY_CQ_QID_SHIFT			= 0,
+	OCRDMA_DESTROY_CQ_QID_MASK			= 0xFFFF,
+	OCRDMA_DESTROY_CQ_QID_BYPASS_FLUSH_SHIFT	= 16,
+	OCRDMA_DESTROY_CQ_QID_BYPASS_FLUSH_MASK		= 0xFFFF <<
+				OCRDMA_DESTROY_CQ_QID_BYPASS_FLUSH_SHIFT
+};
+
+struct ocrdma_destroy_cq {
+	struct ocrdma_mqe_hdr hdr;
+	struct ocrdma_mbx_hdr req;
+
+	u32 bypass_flush_qid;
+} __packed;
+
+struct ocrdma_destroy_cq_rsp {
+	struct ocrdma_mqe_hdr hdr;
+	struct ocrdma_mbx_rsp rsp;
+} __packed;
+
+enum {
+	OCRDMA_QPT_GSI	= 1,
+	OCRDMA_QPT_RC	= 2,
+	OCRDMA_QPT_UD	= 4,
+};
+
+enum {
+	OCRDMA_CREATE_QP_REQ_PD_ID_SHIFT	= 0,
+	OCRDMA_CREATE_QP_REQ_PD_ID_MASK		= 0xFFFF,
+	OCRDMA_CREATE_QP_REQ_SQ_PAGE_SIZE_SHIFT	= 16,
+	OCRDMA_CREATE_QP_REQ_RQ_PAGE_SIZE_SHIFT	= 19,
+	OCRDMA_CREATE_QP_REQ_QPT_SHIFT		= 29,
+	OCRDMA_CREATE_QP_REQ_QPT_MASK		= Bit(31) | Bit(30) | Bit(29),
+
+	OCRDMA_CREATE_QP_REQ_MAX_RQE_SHIFT	= 0,
+	OCRDMA_CREATE_QP_REQ_MAX_RQE_MASK	= 0xFFFF,
+	OCRDMA_CREATE_QP_REQ_MAX_WQE_SHIFT	= 16,
+	OCRDMA_CREATE_QP_REQ_MAX_WQE_MASK	= 0xFFFF <<
+					OCRDMA_CREATE_QP_REQ_MAX_WQE_SHIFT,
+
+	OCRDMA_CREATE_QP_REQ_MAX_SGE_WRITE_SHIFT	= 0,
+	OCRDMA_CREATE_QP_REQ_MAX_SGE_WRITE_MASK		= 0xFFFF,
+	OCRDMA_CREATE_QP_REQ_MAX_SGE_SEND_SHIFT		= 16,
+	OCRDMA_CREATE_QP_REQ_MAX_SGE_SEND_MASK		= 0xFFFF <<
+					OCRDMA_CREATE_QP_REQ_MAX_SGE_SEND_SHIFT,
+
+	OCRDMA_CREATE_QP_REQ_FMR_EN_SHIFT		= 0,
+	OCRDMA_CREATE_QP_REQ_FMR_EN_MASK		= Bit(0),
+	OCRDMA_CREATE_QP_REQ_ZERO_LKEYEN_SHIFT		= 1,
+	OCRDMA_CREATE_QP_REQ_ZERO_LKEYEN_MASK		= Bit(1),
+	OCRDMA_CREATE_QP_REQ_BIND_MEMWIN_SHIFT		= 2,
+	OCRDMA_CREATE_QP_REQ_BIND_MEMWIN_MASK		= Bit(2),
+	OCRDMA_CREATE_QP_REQ_INB_WREN_SHIFT		= 3,
+	OCRDMA_CREATE_QP_REQ_INB_WREN_MASK		= Bit(3),
+	OCRDMA_CREATE_QP_REQ_INB_RDEN_SHIFT		= 4,
+	OCRDMA_CREATE_QP_REQ_INB_RDEN_MASK		= Bit(4),
+	OCRDMA_CREATE_QP_REQ_USE_SRQ_SHIFT		= 5,
+	OCRDMA_CREATE_QP_REQ_USE_SRQ_MASK		= Bit(5),
+	OCRDMA_CREATE_QP_REQ_ENABLE_RPIR_SHIFT		= 6,
+	OCRDMA_CREATE_QP_REQ_ENABLE_RPIR_MASK		= Bit(6),
+	OCRDMA_CREATE_QP_REQ_ENABLE_DPP_SHIFT		= 7,
+	OCRDMA_CREATE_QP_REQ_ENABLE_DPP_MASK		= Bit(7),
+	OCRDMA_CREATE_QP_REQ_ENABLE_DPP_CQ_SHIFT	= 8,
+	OCRDMA_CREATE_QP_REQ_ENABLE_DPP_CQ_MASK		= Bit(8),
+	OCRDMA_CREATE_QP_REQ_MAX_SGE_RECV_SHIFT		= 16,
+	OCRDMA_CREATE_QP_REQ_MAX_SGE_RECV_MASK		= 0xFFFF <<
+				OCRDMA_CREATE_QP_REQ_MAX_SGE_RECV_SHIFT,
+
+	OCRDMA_CREATE_QP_REQ_MAX_IRD_SHIFT		= 0,
+	OCRDMA_CREATE_QP_REQ_MAX_IRD_MASK		= 0xFFFF,
+	OCRDMA_CREATE_QP_REQ_MAX_ORD_SHIFT		= 16,
+	OCRDMA_CREATE_QP_REQ_MAX_ORD_MASK		= 0xFFFF <<
+				OCRDMA_CREATE_QP_REQ_MAX_ORD_SHIFT,
+
+	OCRDMA_CREATE_QP_REQ_NUM_RQ_PAGES_SHIFT		= 0,
+	OCRDMA_CREATE_QP_REQ_NUM_RQ_PAGES_MASK		= 0xFFFF,
+	OCRDMA_CREATE_QP_REQ_NUM_WQ_PAGES_SHIFT		= 16,
+	OCRDMA_CREATE_QP_REQ_NUM_WQ_PAGES_MASK		= 0xFFFF <<
+				OCRDMA_CREATE_QP_REQ_NUM_WQ_PAGES_SHIFT,
+
+	OCRDMA_CREATE_QP_REQ_RQE_SIZE_SHIFT		= 0,
+	OCRDMA_CREATE_QP_REQ_RQE_SIZE_MASK		= 0xFFFF,
+	OCRDMA_CREATE_QP_REQ_WQE_SIZE_SHIFT		= 16,
+	OCRDMA_CREATE_QP_REQ_WQE_SIZE_MASK		= 0xFFFF <<
+				OCRDMA_CREATE_QP_REQ_WQE_SIZE_SHIFT,
+
+	OCRDMA_CREATE_QP_REQ_RQ_CQID_SHIFT		= 0,
+	OCRDMA_CREATE_QP_REQ_RQ_CQID_MASK		= 0xFFFF,
+	OCRDMA_CREATE_QP_REQ_WQ_CQID_SHIFT		= 16,
+	OCRDMA_CREATE_QP_REQ_WQ_CQID_MASK		= 0xFFFF <<
+				OCRDMA_CREATE_QP_REQ_WQ_CQID_SHIFT,
+
+	OCRDMA_CREATE_QP_REQ_DPP_CQPID_SHIFT		= 0,
+	OCRDMA_CREATE_QP_REQ_DPP_CQPID_MASK		= 0xFFFF,
+	OCRDMA_CREATE_QP_REQ_DPP_CREDIT_SHIFT		= 16,
+	OCRDMA_CREATE_QP_REQ_DPP_CREDIT_MASK		= 0xFFFF <<
+				OCRDMA_CREATE_QP_REQ_DPP_CREDIT_SHIFT
+};
+
+enum {
+	OCRDMA_CREATE_QP_REQ_DPP_CREDIT_LIMIT	= 16,
+	OCRDMA_CREATE_QP_RSP_DPP_PAGE_SHIFT	= 1
+};
+
+#define MAX_OCRDMA_IRD_PAGES 4
+
+enum ocrdma_qp_flags {
+	OCRDMA_QP_MW_BIND	= 1,
+	OCRDMA_QP_LKEY0		= (1 << 1),
+	OCRDMA_QP_FAST_REG	= (1 << 2),
+	OCRDMA_QP_INB_RD	= (1 << 6),
+	OCRDMA_QP_INB_WR	= (1 << 7),
+};
+
+enum ocrdma_qp_state {
+	OCRDMA_QPS_RST		= 0,
+	OCRDMA_QPS_INIT		= 1,
+	OCRDMA_QPS_RTR		= 2,
+	OCRDMA_QPS_RTS		= 3,
+	OCRDMA_QPS_SQE		= 4,
+	OCRDMA_QPS_SQ_DRAINING	= 5,
+	OCRDMA_QPS_ERR		= 6,
+	OCRDMA_QPS_SQD		= 7
+};
+
+struct ocrdma_create_qp_req {
+	struct ocrdma_mqe_hdr hdr;
+	struct ocrdma_mbx_hdr req;
+
+	u32 type_pgsz_pdn;
+	u32 max_wqe_rqe;
+	u32 max_sge_send_write;
+	u32 max_sge_recv_flags;
+	u32 max_ord_ird;
+	u32 num_wq_rq_pages;
+	u32 wqe_rqe_size;
+	u32 wq_rq_cqid;
+	struct ocrdma_pa wq_addr[MAX_OCRDMA_QP_PAGES];
+	struct ocrdma_pa rq_addr[MAX_OCRDMA_QP_PAGES];
+	u32 dpp_credits_cqid;
+	u32 rpir_lkey;
+	struct ocrdma_pa ird_addr[MAX_OCRDMA_IRD_PAGES];
+} __packed;
+
+enum {
+	OCRDMA_CREATE_QP_RSP_QP_ID_SHIFT		= 0,
+	OCRDMA_CREATE_QP_RSP_QP_ID_MASK			= 0xFFFF,
+
+	OCRDMA_CREATE_QP_RSP_MAX_RQE_SHIFT		= 0,
+	OCRDMA_CREATE_QP_RSP_MAX_RQE_MASK		= 0xFFFF,
+	OCRDMA_CREATE_QP_RSP_MAX_WQE_SHIFT		= 16,
+	OCRDMA_CREATE_QP_RSP_MAX_WQE_MASK		= 0xFFFF <<
+				OCRDMA_CREATE_QP_RSP_MAX_WQE_SHIFT,
+
+	OCRDMA_CREATE_QP_RSP_MAX_SGE_WRITE_SHIFT	= 0,
+	OCRDMA_CREATE_QP_RSP_MAX_SGE_WRITE_MASK		= 0xFFFF,
+	OCRDMA_CREATE_QP_RSP_MAX_SGE_SEND_SHIFT		= 16,
+	OCRDMA_CREATE_QP_RSP_MAX_SGE_SEND_MASK		= 0xFFFF <<
+				OCRDMA_CREATE_QP_RSP_MAX_SGE_SEND_SHIFT,
+
+	OCRDMA_CREATE_QP_RSP_MAX_SGE_RECV_SHIFT		= 16,
+	OCRDMA_CREATE_QP_RSP_MAX_SGE_RECV_MASK		= 0xFFFF <<
+				OCRDMA_CREATE_QP_RSP_MAX_SGE_RECV_SHIFT,
+
+	OCRDMA_CREATE_QP_RSP_MAX_IRD_SHIFT		= 0,
+	OCRDMA_CREATE_QP_RSP_MAX_IRD_MASK		= 0xFFFF,
+	OCRDMA_CREATE_QP_RSP_MAX_ORD_SHIFT		= 16,
+	OCRDMA_CREATE_QP_RSP_MAX_ORD_MASK		= 0xFFFF <<
+				OCRDMA_CREATE_QP_RSP_MAX_ORD_SHIFT,
+
+	OCRDMA_CREATE_QP_RSP_RQ_ID_SHIFT		= 0,
+	OCRDMA_CREATE_QP_RSP_RQ_ID_MASK			= 0xFFFF,
+	OCRDMA_CREATE_QP_RSP_SQ_ID_SHIFT		= 16,
+	OCRDMA_CREATE_QP_RSP_SQ_ID_MASK			= 0xFFFF <<
+				OCRDMA_CREATE_QP_RSP_SQ_ID_SHIFT,
+
+	OCRDMA_CREATE_QP_RSP_DPP_ENABLED_MASK		= Bit(0),
+	OCRDMA_CREATE_QP_RSP_DPP_PAGE_OFFSET_SHIFT	= 1,
+	OCRDMA_CREATE_QP_RSP_DPP_PAGE_OFFSET_MASK	= 0x7FFF <<
+				OCRDMA_CREATE_QP_RSP_DPP_PAGE_OFFSET_SHIFT,
+	OCRDMA_CREATE_QP_RSP_DPP_CREDITS_SHIFT		= 16,
+	OCRDMA_CREATE_QP_RSP_DPP_CREDITS_MASK		= 0xFFFF <<
+				OCRDMA_CREATE_QP_RSP_DPP_CREDITS_SHIFT,
+};
+
+struct ocrdma_create_qp_rsp {
+	struct ocrdma_mqe_hdr hdr;
+	struct ocrdma_mbx_rsp rsp;
+
+	u32 qp_id;
+	u32 max_wqe_rqe;
+	u32 max_sge_send_write;
+	u32 max_sge_recv;
+	u32 max_ord_ird;
+	u32 sq_rq_id;
+	u32 dpp_response;
+} __packed;
+
+struct ocrdma_destroy_qp {
+	struct ocrdma_mqe_hdr hdr;
+	struct ocrdma_mbx_hdr req;
+	u32 qp_id;
+} __packed;
+
+struct ocrdma_destroy_qp_rsp {
+	struct ocrdma_mqe_hdr hdr;
+	struct ocrdma_mbx_rsp rsp;
+} __packed;
+
+enum {
+	OCRDMA_MODIFY_QP_ID_SHIFT	= 0,
+	OCRDMA_MODIFY_QP_ID_MASK	= 0xFFFF,
+
+	OCRDMA_QP_PARA_QPS_VALID	= Bit(0),
+	OCRDMA_QP_PARA_SQD_ASYNC_VALID	= Bit(1),
+	OCRDMA_QP_PARA_PKEY_VALID	= Bit(2),
+	OCRDMA_QP_PARA_QKEY_VALID	= Bit(3),
+	OCRDMA_QP_PARA_PMTU_VALID	= Bit(4),
+	OCRDMA_QP_PARA_ACK_TO_VALID	= Bit(5),
+	OCRDMA_QP_PARA_RETRY_CNT_VALID	= Bit(6),
+	OCRDMA_QP_PARA_RRC_VALID	= Bit(7),
+	OCRDMA_QP_PARA_RQPSN_VALID	= Bit(8),
+	OCRDMA_QP_PARA_MAX_IRD_VALID	= Bit(9),
+	OCRDMA_QP_PARA_MAX_ORD_VALID	= Bit(10),
+	OCRDMA_QP_PARA_RNT_VALID	= Bit(11),
+	OCRDMA_QP_PARA_SQPSN_VALID	= Bit(12),
+	OCRDMA_QP_PARA_DST_QPN_VALID	= Bit(13),
+	OCRDMA_QP_PARA_MAX_WQE_VALID	= Bit(14),
+	OCRDMA_QP_PARA_MAX_RQE_VALID	= Bit(15),
+	OCRDMA_QP_PARA_SGE_SEND_VALID	= Bit(16),
+	OCRDMA_QP_PARA_SGE_RECV_VALID	= Bit(17),
+	OCRDMA_QP_PARA_SGE_WR_VALID	= Bit(18),
+	OCRDMA_QP_PARA_INB_RDEN_VALID	= Bit(19),
+	OCRDMA_QP_PARA_INB_WREN_VALID	= Bit(20),
+	OCRDMA_QP_PARA_FLOW_LBL_VALID	= Bit(21),
+	OCRDMA_QP_PARA_BIND_EN_VALID	= Bit(22),
+	OCRDMA_QP_PARA_ZLKEY_EN_VALID	= Bit(23),
+	OCRDMA_QP_PARA_FMR_EN_VALID	= Bit(24),
+	OCRDMA_QP_PARA_INBAT_EN_VALID	= Bit(25),
+	OCRDMA_QP_PARA_VLAN_EN_VALID	= Bit(26),
+
+	OCRDMA_MODIFY_QP_FLAGS_RD	= Bit(0),
+	OCRDMA_MODIFY_QP_FLAGS_WR	= Bit(1),
+	OCRDMA_MODIFY_QP_FLAGS_SEND	= Bit(2),
+	OCRDMA_MODIFY_QP_FLAGS_ATOMIC	= Bit(3)
+};
+
+enum {
+	OCRDMA_QP_PARAMS_SRQ_ID_SHIFT		= 0,
+	OCRDMA_QP_PARAMS_SRQ_ID_MASK		= 0xFFFF,
+
+	OCRDMA_QP_PARAMS_MAX_RQE_SHIFT		= 0,
+	OCRDMA_QP_PARAMS_MAX_RQE_MASK		= 0xFFFF,
+	OCRDMA_QP_PARAMS_MAX_WQE_SHIFT		= 16,
+	OCRDMA_QP_PARAMS_MAX_WQE_MASK		= 0xFFFF <<
+	    OCRDMA_QP_PARAMS_MAX_WQE_SHIFT,
+
+	OCRDMA_QP_PARAMS_MAX_SGE_WRITE_SHIFT	= 0,
+	OCRDMA_QP_PARAMS_MAX_SGE_WRITE_MASK	= 0xFFFF,
+	OCRDMA_QP_PARAMS_MAX_SGE_SEND_SHIFT	= 16,
+	OCRDMA_QP_PARAMS_MAX_SGE_SEND_MASK	= 0xFFFF <<
+					OCRDMA_QP_PARAMS_MAX_SGE_SEND_SHIFT,
+
+	OCRDMA_QP_PARAMS_FLAGS_FMR_EN		= Bit(0),
+	OCRDMA_QP_PARAMS_FLAGS_LKEY_0_EN	= Bit(1),
+	OCRDMA_QP_PARAMS_FLAGS_BIND_MW_EN	= Bit(2),
+	OCRDMA_QP_PARAMS_FLAGS_INBWR_EN		= Bit(3),
+	OCRDMA_QP_PARAMS_FLAGS_INBRD_EN		= Bit(4),
+	OCRDMA_QP_PARAMS_STATE_SHIFT		= 5,
+	OCRDMA_QP_PARAMS_STATE_MASK		= Bit(5) | Bit(6) | Bit(7),
+	OCRDMA_QP_PARAMS_FLAGS_SQD_ASYNC	= Bit(8),
+	OCRDMA_QP_PARAMS_FLAGS_INB_ATEN		= Bit(9),
+	OCRDMA_QP_PARAMS_MAX_SGE_RECV_SHIFT	= 16,
+	OCRDMA_QP_PARAMS_MAX_SGE_RECV_MASK	= 0xFFFF <<
+					OCRDMA_QP_PARAMS_MAX_SGE_RECV_SHIFT,
+
+	OCRDMA_QP_PARAMS_MAX_IRD_SHIFT		= 0,
+	OCRDMA_QP_PARAMS_MAX_IRD_MASK		= 0xFFFF,
+	OCRDMA_QP_PARAMS_MAX_ORD_SHIFT		= 16,
+	OCRDMA_QP_PARAMS_MAX_ORD_MASK		= 0xFFFF <<
+					OCRDMA_QP_PARAMS_MAX_ORD_SHIFT,
+
+	OCRDMA_QP_PARAMS_RQ_CQID_SHIFT		= 0,
+	OCRDMA_QP_PARAMS_RQ_CQID_MASK		= 0xFFFF,
+	OCRDMA_QP_PARAMS_WQ_CQID_SHIFT		= 16,
+	OCRDMA_QP_PARAMS_WQ_CQID_MASK		= 0xFFFF <<
+					OCRDMA_QP_PARAMS_WQ_CQID_SHIFT,
+
+	OCRDMA_QP_PARAMS_RQ_PSN_SHIFT		= 0,
+	OCRDMA_QP_PARAMS_RQ_PSN_MASK		= 0xFFFFFF,
+	OCRDMA_QP_PARAMS_HOP_LMT_SHIFT		= 24,
+	OCRDMA_QP_PARAMS_HOP_LMT_MASK		= 0xFF <<
+					OCRDMA_QP_PARAMS_HOP_LMT_SHIFT,
+
+	OCRDMA_QP_PARAMS_SQ_PSN_SHIFT		= 0,
+	OCRDMA_QP_PARAMS_SQ_PSN_MASK		= 0xFFFFFF,
+	OCRDMA_QP_PARAMS_TCLASS_SHIFT		= 24,
+	OCRDMA_QP_PARAMS_TCLASS_MASK		= 0xFF <<
+					OCRDMA_QP_PARAMS_TCLASS_SHIFT,
+
+	OCRDMA_QP_PARAMS_DEST_QPN_SHIFT		= 0,
+	OCRDMA_QP_PARAMS_DEST_QPN_MASK		= 0xFFFFFF,
+	OCRDMA_QP_PARAMS_RNR_RETRY_CNT_SHIFT	= 24,
+	OCRDMA_QP_PARAMS_RNR_RETRY_CNT_MASK	= 0x7 <<
+					OCRDMA_QP_PARAMS_RNR_RETRY_CNT_SHIFT,
+	OCRDMA_QP_PARAMS_ACK_TIMEOUT_SHIFT	= 27,
+	OCRDMA_QP_PARAMS_ACK_TIMEOUT_MASK	= 0x1F <<
+					OCRDMA_QP_PARAMS_ACK_TIMEOUT_SHIFT,
+
+	OCRDMA_QP_PARAMS_PKEY_IDNEX_SHIFT	= 0,
+	OCRDMA_QP_PARAMS_PKEY_INDEX_MASK	= 0xFFFF,
+	OCRDMA_QP_PARAMS_PATH_MTU_SHIFT		= 18,
+	OCRDMA_QP_PARAMS_PATH_MTU_MASK		= 0x3FFF <<
+					OCRDMA_QP_PARAMS_PATH_MTU_SHIFT,
+
+	OCRDMA_QP_PARAMS_FLOW_LABEL_SHIFT	= 0,
+	OCRDMA_QP_PARAMS_FLOW_LABEL_MASK	= 0xFFFFF,
+	OCRDMA_QP_PARAMS_SL_SHIFT		= 20,
+	OCRDMA_QP_PARAMS_SL_MASK		= 0xF <<
+					OCRDMA_QP_PARAMS_SL_SHIFT,
+	OCRDMA_QP_PARAMS_RETRY_CNT_SHIFT	= 24,
+	OCRDMA_QP_PARAMS_RETRY_CNT_MASK		= 0x7 <<
+					OCRDMA_QP_PARAMS_RETRY_CNT_SHIFT,
+	OCRDMA_QP_PARAMS_RNR_NAK_TIMER_SHIFT	= 27,
+	OCRDMA_QP_PARAMS_RNR_NAK_TIMER_MASK	= 0x1F <<
+					OCRDMA_QP_PARAMS_RNR_NAK_TIMER_SHIFT,
+
+	OCRDMA_QP_PARAMS_DMAC_B4_TO_B5_SHIFT	= 0,
+	OCRDMA_QP_PARAMS_DMAC_B4_TO_B5_MASK	= 0xFFFF,
+	OCRDMA_QP_PARAMS_VLAN_SHIFT		= 16,
+	OCRDMA_QP_PARAMS_VLAN_MASK		= 0xFFFF <<
+					OCRDMA_QP_PARAMS_VLAN_SHIFT
+};
+
+struct ocrdma_qp_params {
+	u32 id;
+	u32 max_wqe_rqe;
+	u32 max_sge_send_write;
+	u32 max_sge_recv_flags;
+	u32 max_ord_ird;
+	u32 wq_rq_cqid;
+	u32 hop_lmt_rq_psn;
+	u32 tclass_sq_psn;
+	u32 ack_to_rnr_rtc_dest_qpn;
+	u32 path_mtu_pkey_indx;
+	u32 rnt_rc_sl_fl;
+	u8 sgid[16];
+	u8 dgid[16];
+	u32 dmac_b0_to_b3;
+	u32 vlan_dmac_b4_to_b5;
+	u32 qkey;
+} __packed;
+
+
+struct ocrdma_modify_qp {
+	struct ocrdma_mqe_hdr hdr;
+	struct ocrdma_mbx_hdr req;
+
+	struct ocrdma_qp_params params;
+	u32 flags;
+	u32 rdma_flags;
+	u32 num_outstanding_atomic_rd;
+} __packed;
+
+enum {
+	OCRDMA_MODIFY_QP_RSP_MAX_RQE_SHIFT	= 0,
+	OCRDMA_MODIFY_QP_RSP_MAX_RQE_MASK	= 0xFFFF,
+	OCRDMA_MODIFY_QP_RSP_MAX_WQE_SHIFT	= 16,
+	OCRDMA_MODIFY_QP_RSP_MAX_WQE_MASK	= 0xFFFF <<
+					OCRDMA_MODIFY_QP_RSP_MAX_WQE_SHIFT,
+
+	OCRDMA_MODIFY_QP_RSP_MAX_IRD_SHIFT	= 0,
+	OCRDMA_MODIFY_QP_RSP_MAX_IRD_MASK	= 0xFFFF,
+	OCRDMA_MODIFY_QP_RSP_MAX_ORD_SHIFT	= 16,
+	OCRDMA_MODIFY_QP_RSP_MAX_ORD_MASK	= 0xFFFF <<
+					OCRDMA_MODIFY_QP_RSP_MAX_ORD_SHIFT
+};
+struct ocrdma_modify_qp_rsp {
+	struct ocrdma_mqe_hdr hdr;
+	struct ocrdma_mbx_rsp rsp;
+
+	u32 max_wqe_rqe;
+	u32 max_ord_ird;
+} __packed;
+
+struct ocrdma_query_qp {
+	struct ocrdma_mqe_hdr hdr;
+	struct ocrdma_mbx_hdr req;
+
+#define OCRDMA_QUERY_UP_QP_ID_SHIFT 0
+#define OCRDMA_QUERY_UP_QP_ID_MASK   0xFFFFFF
+	u32 qp_id;
+} __packed;
+
+struct ocrdma_query_qp_rsp {
+	struct ocrdma_mqe_hdr hdr;
+	struct ocrdma_mbx_rsp rsp;
+	struct ocrdma_qp_params params;
+} __packed;
+
+enum {
+	OCRDMA_CREATE_SRQ_PD_ID_SHIFT		= 0,
+	OCRDMA_CREATE_SRQ_PD_ID_MASK		= 0xFFFF,
+	OCRDMA_CREATE_SRQ_PG_SZ_SHIFT		= 16,
+	OCRDMA_CREATE_SRQ_PG_SZ_MASK		= 0x3 <<
+					OCRDMA_CREATE_SRQ_PG_SZ_SHIFT,
+
+	OCRDMA_CREATE_SRQ_MAX_RQE_SHIFT		= 0,
+	OCRDMA_CREATE_SRQ_MAX_SGE_RECV_SHIFT	= 16,
+	OCRDMA_CREATE_SRQ_MAX_SGE_RECV_MASK	= 0xFFFF <<
+					OCRDMA_CREATE_SRQ_MAX_SGE_RECV_SHIFT,
+
+	OCRDMA_CREATE_SRQ_RQE_SIZE_SHIFT	= 0,
+	OCRDMA_CREATE_SRQ_RQE_SIZE_MASK		= 0xFFFF,
+	OCRDMA_CREATE_SRQ_NUM_RQ_PAGES_SHIFT	= 16,
+	OCRDMA_CREATE_SRQ_NUM_RQ_PAGES_MASK	= 0xFFFF <<
+					OCRDMA_CREATE_SRQ_NUM_RQ_PAGES_SHIFT
+};
+
+struct ocrdma_create_srq {
+	struct ocrdma_mqe_hdr hdr;
+	struct ocrdma_mbx_hdr req;
+
+	u32 pgsz_pdid;
+	u32 max_sge_rqe;
+	u32 pages_rqe_sz;
+	struct ocrdma_pa rq_addr[MAX_OCRDMA_SRQ_PAGES];
+} __packed;
+
+enum {
+	OCRDMA_CREATE_SRQ_RSP_SRQ_ID_SHIFT			= 0,
+	OCRDMA_CREATE_SRQ_RSP_SRQ_ID_MASK			= 0xFFFFFF,
+
+	OCRDMA_CREATE_SRQ_RSP_MAX_RQE_ALLOCATED_SHIFT		= 0,
+	OCRDMA_CREATE_SRQ_RSP_MAX_RQE_ALLOCATED_MASK		= 0xFFFF,
+	OCRDMA_CREATE_SRQ_RSP_MAX_SGE_RECV_ALLOCATED_SHIFT	= 16,
+	OCRDMA_CREATE_SRQ_RSP_MAX_SGE_RECV_ALLOCATED_MASK	= 0xFFFF <<
+			OCRDMA_CREATE_SRQ_RSP_MAX_SGE_RECV_ALLOCATED_SHIFT
+};
+
+struct ocrdma_create_srq_rsp {
+	struct ocrdma_mqe_hdr hdr;
+	struct ocrdma_mbx_rsp rsp;
+
+	u32 id;
+	u32 max_sge_rqe_allocated;
+} __packed;
+
+enum {
+	OCRDMA_MODIFY_SRQ_ID_SHIFT	= 0,
+	OCRDMA_MODIFY_SRQ_ID_MASK	= 0xFFFFFF,
+
+	OCRDMA_MODIFY_SRQ_MAX_RQE_SHIFT	= 0,
+	OCRDMA_MODIFY_SRQ_MAX_RQE_MASK	= 0xFFFF,
+	OCRDMA_MODIFY_SRQ_LIMIT_SHIFT	= 16,
+	OCRDMA_MODIFY_SRQ__LIMIT_MASK	= 0xFFFF <<
+					OCRDMA_MODIFY_SRQ_LIMIT_SHIFT
+};
+
+struct ocrdma_modify_srq {
+	struct ocrdma_mqe_hdr hdr;
+	struct ocrdma_mbx_rsp rep;
+
+	u32 id;
+	u32 limit_max_rqe;
+} __packed;
+
+enum {
+	OCRDMA_QUERY_SRQ_ID_SHIFT	= 0,
+	OCRDMA_QUERY_SRQ_ID_MASK	= 0xFFFFFF
+};
+
+struct ocrdma_query_srq {
+	struct ocrdma_mqe_hdr hdr;
+	struct ocrdma_mbx_rsp req;
+
+	u32 id;
+} __packed;
+
+enum {
+	OCRDMA_QUERY_SRQ_RSP_PD_ID_SHIFT	= 0,
+	OCRDMA_QUERY_SRQ_RSP_PD_ID_MASK		= 0xFFFF,
+	OCRDMA_QUERY_SRQ_RSP_MAX_RQE_SHIFT	= 16,
+	OCRDMA_QUERY_SRQ_RSP_MAX_RQE_MASK	= 0xFFFF <<
+					OCRDMA_QUERY_SRQ_RSP_MAX_RQE_SHIFT,
+
+	OCRDMA_QUERY_SRQ_RSP_MAX_SGE_RECV_SHIFT	= 0,
+	OCRDMA_QUERY_SRQ_RSP_MAX_SGE_RECV_MASK	= 0xFFFF,
+	OCRDMA_QUERY_SRQ_RSP_SRQ_LIMIT_SHIFT	= 16,
+	OCRDMA_QUERY_SRQ_RSP_SRQ_LIMIT_MASK	= 0xFFFF <<
+					OCRDMA_QUERY_SRQ_RSP_SRQ_LIMIT_SHIFT
+};
+
+struct ocrdma_query_srq_rsp {
+	struct ocrdma_mqe_hdr hdr;
+	struct ocrdma_mbx_rsp req;
+
+	u32 max_rqe_pdid;
+	u32 srq_lmt_max_sge;
+} __packed;
+
+enum {
+	OCRDMA_DESTROY_SRQ_ID_SHIFT	= 0,
+	OCRDMA_DESTROY_SRQ_ID_MASK	= 0xFFFFFF
+};
+
+struct ocrdma_destroy_srq {
+	struct ocrdma_mqe_hdr hdr;
+	struct ocrdma_mbx_rsp req;
+
+	u32 id;
+} __packed;
+
+enum {
+	OCRDMA_ALLOC_PD_ENABLE_DPP	= BIT(16),
+	OCRDMA_PD_MAX_DPP_ENABLED_QP	= 8,
+	OCRDMA_DPP_PAGE_SIZE		= 4096
+};
+
+struct ocrdma_alloc_pd {
+	struct ocrdma_mqe_hdr hdr;
+	struct ocrdma_mbx_hdr req;
+	u32 enable_dpp_rsvd;
+} __packed;
+
+enum {
+	OCRDMA_ALLOC_PD_RSP_DPP			= Bit(16),
+	OCRDMA_ALLOC_PD_RSP_DPP_PAGE_SHIFT	= 20,
+	OCRDMA_ALLOC_PD_RSP_PDID_MASK		= 0xFFFF,
+};
+
+struct ocrdma_alloc_pd_rsp {
+	struct ocrdma_mqe_hdr hdr;
+	struct ocrdma_mbx_rsp rsp;
+	u32 dpp_page_pdid;
+} __packed;
+
+struct ocrdma_dealloc_pd {
+	struct ocrdma_mqe_hdr hdr;
+	struct ocrdma_mbx_hdr req;
+	u32 id;
+} __packed;
+
+struct ocrdma_dealloc_pd_rsp {
+	struct ocrdma_mqe_hdr hdr;
+	struct ocrdma_mbx_rsp rsp;
+} __packed;
+
+enum {
+	OCRDMA_ADDR_CHECK_ENABLE	= 1,
+	OCRDMA_ADDR_CHECK_DISABLE	= 0
+};
+
+enum {
+	OCRDMA_ALLOC_LKEY_PD_ID_SHIFT		= 0,
+	OCRDMA_ALLOC_LKEY_PD_ID_MASK		= 0xFFFF,
+
+	OCRDMA_ALLOC_LKEY_ADDR_CHECK_SHIFT	= 0,
+	OCRDMA_ALLOC_LKEY_ADDR_CHECK_MASK	= Bit(0),
+	OCRDMA_ALLOC_LKEY_FMR_SHIFT		= 1,
+	OCRDMA_ALLOC_LKEY_FMR_MASK		= Bit(1),
+	OCRDMA_ALLOC_LKEY_REMOTE_INV_SHIFT	= 2,
+	OCRDMA_ALLOC_LKEY_REMOTE_INV_MASK	= Bit(2),
+	OCRDMA_ALLOC_LKEY_REMOTE_WR_SHIFT	= 3,
+	OCRDMA_ALLOC_LKEY_REMOTE_WR_MASK	= Bit(3),
+	OCRDMA_ALLOC_LKEY_REMOTE_RD_SHIFT	= 4,
+	OCRDMA_ALLOC_LKEY_REMOTE_RD_MASK	= Bit(4),
+	OCRDMA_ALLOC_LKEY_LOCAL_WR_SHIFT	= 5,
+	OCRDMA_ALLOC_LKEY_LOCAL_WR_MASK		= Bit(5),
+	OCRDMA_ALLOC_LKEY_REMOTE_ATOMIC_MASK	= Bit(6),
+	OCRDMA_ALLOC_LKEY_REMOTE_ATOMIC_SHIFT	= 6,
+	OCRDMA_ALLOC_LKEY_PBL_SIZE_SHIFT	= 16,
+	OCRDMA_ALLOC_LKEY_PBL_SIZE_MASK		= 0xFFFF <<
+						OCRDMA_ALLOC_LKEY_PBL_SIZE_SHIFT
+};
+
+struct ocrdma_alloc_lkey {
+	struct ocrdma_mqe_hdr hdr;
+	struct ocrdma_mbx_hdr req;
+
+	u32 pdid;
+	u32 pbl_sz_flags;
+} __packed;
+
+struct ocrdma_alloc_lkey_rsp {
+	struct ocrdma_mqe_hdr hdr;
+	struct ocrdma_mbx_rsp rsp;
+
+	u32 lrkey;
+	u32 num_pbl_rsvd;
+} __packed;
+
+struct ocrdma_dealloc_lkey {
+	struct ocrdma_mqe_hdr hdr;
+	struct ocrdma_mbx_hdr req;
+
+	u32 lkey;
+	u32 rsvd_frmr;
+} __packed;
+
+struct ocrdma_dealloc_lkey_rsp {
+	struct ocrdma_mqe_hdr hdr;
+	struct ocrdma_mbx_rsp rsp;
+} __packed;
+
+#define MAX_OCRDMA_NSMR_PBL    (u32)22
+#define MAX_OCRDMA_PBL_SIZE     65536
+#define MAX_OCRDMA_PBL_PER_LKEY	32767
+
+enum {
+	OCRDMA_REG_NSMR_LRKEY_INDEX_SHIFT	= 0,
+	OCRDMA_REG_NSMR_LRKEY_INDEX_MASK	= 0xFFFFFF,
+	OCRDMA_REG_NSMR_LRKEY_SHIFT		= 24,
+	OCRDMA_REG_NSMR_LRKEY_MASK		= 0xFF <<
+					OCRDMA_REG_NSMR_LRKEY_SHIFT,
+
+	OCRDMA_REG_NSMR_PD_ID_SHIFT		= 0,
+	OCRDMA_REG_NSMR_PD_ID_MASK		= 0xFFFF,
+	OCRDMA_REG_NSMR_NUM_PBL_SHIFT		= 16,
+	OCRDMA_REG_NSMR_NUM_PBL_MASK		= 0xFFFF <<
+					OCRDMA_REG_NSMR_NUM_PBL_SHIFT,
+
+	OCRDMA_REG_NSMR_PBE_SIZE_SHIFT		= 0,
+	OCRDMA_REG_NSMR_PBE_SIZE_MASK		= 0xFFFF,
+	OCRDMA_REG_NSMR_HPAGE_SIZE_SHIFT	= 16,
+	OCRDMA_REG_NSMR_HPAGE_SIZE_MASK		= 0xFF <<
+					OCRDMA_REG_NSMR_HPAGE_SIZE_SHIFT,
+	OCRDMA_REG_NSMR_BIND_MEMWIN_SHIFT	= 24,
+	OCRDMA_REG_NSMR_BIND_MEMWIN_MASK	= Bit(24),
+	OCRDMA_REG_NSMR_ZB_SHIFT		= 25,
+	OCRDMA_REG_NSMR_ZB_SHIFT_MASK		= Bit(25),
+	OCRDMA_REG_NSMR_REMOTE_INV_SHIFT	= 26,
+	OCRDMA_REG_NSMR_REMOTE_INV_MASK		= Bit(26),
+	OCRDMA_REG_NSMR_REMOTE_WR_SHIFT		= 27,
+	OCRDMA_REG_NSMR_REMOTE_WR_MASK		= Bit(27),
+	OCRDMA_REG_NSMR_REMOTE_RD_SHIFT		= 28,
+	OCRDMA_REG_NSMR_REMOTE_RD_MASK		= Bit(28),
+	OCRDMA_REG_NSMR_LOCAL_WR_SHIFT		= 29,
+	OCRDMA_REG_NSMR_LOCAL_WR_MASK		= Bit(29),
+	OCRDMA_REG_NSMR_REMOTE_ATOMIC_SHIFT	= 30,
+	OCRDMA_REG_NSMR_REMOTE_ATOMIC_MASK	= Bit(30),
+	OCRDMA_REG_NSMR_LAST_SHIFT		= 31,
+	OCRDMA_REG_NSMR_LAST_MASK		= Bit(31)
+};
+
+struct ocrdma_reg_nsmr {
+	struct ocrdma_mqe_hdr hdr;
+	struct ocrdma_mbx_hdr cmd;
+
+	u32 lrkey_key_index;
+	u32 num_pbl_pdid;
+	u32 flags_hpage_pbe_sz;
+	u32 totlen_low;
+	u32 totlen_high;
+	u32 fbo_low;
+	u32 fbo_high;
+	u32 va_loaddr;
+	u32 va_hiaddr;
+	struct ocrdma_pa pbl[MAX_OCRDMA_NSMR_PBL];
+} __packed;
+
+enum {
+	OCRDMA_REG_NSMR_CONT_PBL_SHIFT		= 0,
+	OCRDMA_REG_NSMR_CONT_PBL_SHIFT_MASK	= 0xFFFF,
+	OCRDMA_REG_NSMR_CONT_NUM_PBL_SHIFT	= 16,
+	OCRDMA_REG_NSMR_CONT_NUM_PBL_MASK	= 0xFFFF <<
+					OCRDMA_REG_NSMR_CONT_NUM_PBL_SHIFT,
+
+	OCRDMA_REG_NSMR_CONT_LAST_SHIFT		= 31,
+	OCRDMA_REG_NSMR_CONT_LAST_MASK		= Bit(31)
+};
+
+struct ocrdma_reg_nsmr_cont {
+	struct ocrdma_mqe_hdr hdr;
+	struct ocrdma_mbx_hdr cmd;
+
+	u32 lrkey;
+	u32 num_pbl_offset;
+	u32 last;
+
+	struct ocrdma_pa pbl[MAX_OCRDMA_NSMR_PBL];
+} __packed;
+
+struct ocrdma_pbe {
+	u32 pa_hi;
+	u32 pa_lo;
+} __packed;
+
+enum {
+	OCRDMA_REG_NSMR_RSP_NUM_PBL_SHIFT	= 16,
+	OCRDMA_REG_NSMR_RSP_NUM_PBL_MASK	= 0xFFFF0000
+};
+struct ocrdma_reg_nsmr_rsp {
+	struct ocrdma_mqe_hdr hdr;
+	struct ocrdma_mbx_rsp rsp;
+
+	u32 lrkey;
+	u32 num_pbl;
+} __packed;
+
+enum {
+	OCRDMA_REG_NSMR_CONT_RSP_LRKEY_INDEX_SHIFT	= 0,
+	OCRDMA_REG_NSMR_CONT_RSP_LRKEY_INDEX_MASK	= 0xFFFFFF,
+	OCRDMA_REG_NSMR_CONT_RSP_LRKEY_SHIFT		= 24,
+	OCRDMA_REG_NSMR_CONT_RSP_LRKEY_MASK		= 0xFF <<
+					OCRDMA_REG_NSMR_CONT_RSP_LRKEY_SHIFT,
+
+	OCRDMA_REG_NSMR_CONT_RSP_NUM_PBL_SHIFT		= 16,
+	OCRDMA_REG_NSMR_CONT_RSP_NUM_PBL_MASK		= 0xFFFF <<
+					OCRDMA_REG_NSMR_CONT_RSP_NUM_PBL_SHIFT
+};
+
+struct ocrdma_reg_nsmr_cont_rsp {
+	struct ocrdma_mqe_hdr hdr;
+	struct ocrdma_mbx_rsp rsp;
+
+	u32 lrkey_key_index;
+	u32 num_pbl;
+} __packed;
+
+enum {
+	OCRDMA_ALLOC_MW_PD_ID_SHIFT	= 0,
+	OCRDMA_ALLOC_MW_PD_ID_MASK	= 0xFFFF
+};
+
+struct ocrdma_alloc_mw {
+	struct ocrdma_mqe_hdr hdr;
+	struct ocrdma_mbx_hdr req;
+
+	u32 pdid;
+} __packed;
+
+enum {
+	OCRDMA_ALLOC_MW_RSP_LRKEY_INDEX_SHIFT	= 0,
+	OCRDMA_ALLOC_MW_RSP_LRKEY_INDEX_MASK	= 0xFFFFFF
+};
+
+struct ocrdma_alloc_mw_rsp {
+	struct ocrdma_mqe_hdr hdr;
+	struct ocrdma_mbx_rsp rsp;
+
+	u32 lrkey_index;
+} __packed;
+
+struct ocrdma_attach_mcast {
+	struct ocrdma_mqe_hdr hdr;
+	struct ocrdma_mbx_hdr req;
+	u32 qp_id;
+	u8 mgid[16];
+	u32 mac_b0_to_b3;
+	u32 vlan_mac_b4_to_b5;
+} __packed;
+
+struct ocrdma_attach_mcast_rsp {
+	struct ocrdma_mqe_hdr hdr;
+	struct ocrdma_mbx_rsp rsp;
+} __packed;
+
+struct ocrdma_detach_mcast {
+	struct ocrdma_mqe_hdr hdr;
+	struct ocrdma_mbx_hdr req;
+	u32 qp_id;
+	u8 mgid[16];
+	u32 mac_b0_to_b3;
+	u32 vlan_mac_b4_to_b5;
+} __packed;
+
+struct ocrdma_detach_mcast_rsp {
+	struct ocrdma_mqe_hdr hdr;
+	struct ocrdma_mbx_rsp rsp;
+} __packed;
+
+enum {
+	OCRDMA_CREATE_AH_NUM_PAGES_SHIFT	= 19,
+	OCRDMA_CREATE_AH_NUM_PAGES_MASK		= 0xF <<
+					OCRDMA_CREATE_AH_NUM_PAGES_SHIFT,
+
+	OCRDMA_CREATE_AH_PAGE_SIZE_SHIFT	= 16,
+	OCRDMA_CREATE_AH_PAGE_SIZE_MASK		= 0x7 <<
+					OCRDMA_CREATE_AH_PAGE_SIZE_SHIFT,
+
+	OCRDMA_CREATE_AH_ENTRY_SIZE_SHIFT	= 23,
+	OCRDMA_CREATE_AH_ENTRY_SIZE_MASK	= 0x1FF <<
+					OCRDMA_CREATE_AH_ENTRY_SIZE_SHIFT,
+};
+
+#define OCRDMA_AH_TBL_PAGES 8
+
+struct ocrdma_create_ah_tbl {
+	struct ocrdma_mqe_hdr hdr;
+	struct ocrdma_mbx_hdr req;
+
+	u32 ah_conf;
+	struct ocrdma_pa tbl_addr[8];
+} __packed;
+
+struct ocrdma_create_ah_tbl_rsp {
+	struct ocrdma_mqe_hdr hdr;
+	struct ocrdma_mbx_rsp rsp;
+	u32 ahid;
+} __packed;
+
+struct ocrdma_delete_ah_tbl {
+	struct ocrdma_mqe_hdr hdr;
+	struct ocrdma_mbx_hdr req;
+	u32 ahid;
+} __packed;
+
+struct ocrdma_delete_ah_tbl_rsp {
+	struct ocrdma_mqe_hdr hdr;
+	struct ocrdma_mbx_rsp rsp;
+} __packed;
+
+enum {
+	OCRDMA_EQE_VALID_SHIFT		= 0,
+	OCRDMA_EQE_VALID_MASK		= Bit(0),
+	OCRDMA_EQE_FOR_CQE_MASK		= 0xFFFE,
+	OCRDMA_EQE_RESOURCE_ID_SHIFT	= 16,
+	OCRDMA_EQE_RESOURCE_ID_MASK	= 0xFFFF <<
+				OCRDMA_EQE_RESOURCE_ID_SHIFT,
+};
+
+struct ocrdma_eqe {
+	u32 id_valid;
+} __packed;
+
+enum OCRDMA_CQE_STATUS {
+	OCRDMA_CQE_SUCCESS = 0,
+	OCRDMA_CQE_LOC_LEN_ERR,
+	OCRDMA_CQE_LOC_QP_OP_ERR,
+	OCRDMA_CQE_LOC_EEC_OP_ERR,
+	OCRDMA_CQE_LOC_PROT_ERR,
+	OCRDMA_CQE_WR_FLUSH_ERR,
+	OCRDMA_CQE_MW_BIND_ERR,
+	OCRDMA_CQE_BAD_RESP_ERR,
+	OCRDMA_CQE_LOC_ACCESS_ERR,
+	OCRDMA_CQE_REM_INV_REQ_ERR,
+	OCRDMA_CQE_REM_ACCESS_ERR,
+	OCRDMA_CQE_REM_OP_ERR,
+	OCRDMA_CQE_RETRY_EXC_ERR,
+	OCRDMA_CQE_RNR_RETRY_EXC_ERR,
+	OCRDMA_CQE_LOC_RDD_VIOL_ERR,
+	OCRDMA_CQE_REM_INV_RD_REQ_ERR,
+	OCRDMA_CQE_REM_ABORT_ERR,
+	OCRDMA_CQE_INV_EECN_ERR,
+	OCRDMA_CQE_INV_EEC_STATE_ERR,
+	OCRDMA_CQE_FATAL_ERR,
+	OCRDMA_CQE_RESP_TIMEOUT_ERR,
+	OCRDMA_CQE_GENERAL_ERR
+};
+
+enum {
+	/* w0 */
+	OCRDMA_CQE_WQEIDX_SHIFT		= 0,
+	OCRDMA_CQE_WQEIDX_MASK		= 0xFFFF,
+
+	/* w1 */
+	OCRDMA_CQE_UD_XFER_LEN_SHIFT	= 16,
+	OCRDMA_CQE_PKEY_SHIFT		= 0,
+	OCRDMA_CQE_PKEY_MASK		= 0xFFFF,
+
+	/* w2 */
+	OCRDMA_CQE_QPN_SHIFT		= 0,
+	OCRDMA_CQE_QPN_MASK		= 0x0000FFFF,
+
+	OCRDMA_CQE_BUFTAG_SHIFT		= 16,
+	OCRDMA_CQE_BUFTAG_MASK		= 0xFFFF << OCRDMA_CQE_BUFTAG_SHIFT,
+
+	/* w3 */
+	OCRDMA_CQE_UD_STATUS_SHIFT	= 24,
+	OCRDMA_CQE_UD_STATUS_MASK	= 0x7 << OCRDMA_CQE_UD_STATUS_SHIFT,
+	OCRDMA_CQE_STATUS_SHIFT		= 16,
+	OCRDMA_CQE_STATUS_MASK		= 0xFF << OCRDMA_CQE_STATUS_SHIFT,
+	OCRDMA_CQE_VALID		= Bit(31),
+	OCRDMA_CQE_INVALIDATE		= Bit(30),
+	OCRDMA_CQE_QTYPE		= Bit(29),
+	OCRDMA_CQE_IMM			= Bit(28),
+	OCRDMA_CQE_WRITE_IMM		= Bit(27),
+	OCRDMA_CQE_QTYPE_SQ		= 0,
+	OCRDMA_CQE_QTYPE_RQ		= 1,
+	OCRDMA_CQE_SRCQP_MASK		= 0xFFFFFF
+};
+
+struct ocrdma_cqe {
+	union {
+		/* w0 to w2 */
+		struct {
+			u32 wqeidx;
+			u32 bytes_xfered;
+			u32 qpn;
+		} wq;
+		struct {
+			u32 lkey_immdt;
+			u32 rxlen;
+			u32 buftag_qpn;
+		} rq;
+		struct {
+			u32 lkey_immdt;
+			u32 rxlen_pkey;
+			u32 buftag_qpn;
+		} ud;
+		struct {
+			u32 word_0;
+			u32 word_1;
+			u32 qpn;
+		} cmn;
+	};
+	u32 flags_status_srcqpn;	/* w3 */
+} __packed;
+
+#define is_cqe_valid(cq, cqe) \
+	(((le32_to_cpu(cqe->flags_status_srcqpn) & OCRDMA_CQE_VALID)\
+	== cq->phase) ? 1 : 0)
+#define is_cqe_for_sq(cqe) \
+	((le32_to_cpu(cqe->flags_status_srcqpn) & OCRDMA_CQE_QTYPE) ? 0 : 1)
+#define is_cqe_for_rq(cqe) \
+	((le32_to_cpu(cqe->flags_status_srcqpn) & OCRDMA_CQE_QTYPE) ? 1 : 0)
+#define is_cqe_invalidated(cqe) \
+	((le32_to_cpu(cqe->flags_status_srcqpn) & OCRDMA_CQE_INVALIDATE) ? \
+	1 : 0)
+#define is_cqe_imm(cqe) \
+	((le32_to_cpu(cqe->flags_status_srcqpn) & OCRDMA_CQE_IMM) ? 1 : 0)
+#define is_cqe_wr_imm(cqe) \
+	((le32_to_cpu(cqe->flags_status_srcqpn) & OCRDMA_CQE_WRITE_IMM) ? 1 : 0)
+
+struct ocrdma_sge {
+	u32 addr_hi;
+	u32 addr_lo;
+	u32 lrkey;
+	u32 len;
+} __packed;
+
+enum {
+	OCRDMA_FLAG_SIG		= 0x1,
+	OCRDMA_FLAG_INV		= 0x2,
+	OCRDMA_FLAG_FENCE_L	= 0x4,
+	OCRDMA_FLAG_FENCE_R	= 0x8,
+	OCRDMA_FLAG_SOLICIT	= 0x10,
+	OCRDMA_FLAG_IMM		= 0x20,
+
+	/* Stag flags */
+	OCRDMA_LKEY_FLAG_LOCAL_WR	= 0x1,
+	OCRDMA_LKEY_FLAG_REMOTE_RD	= 0x2,
+	OCRDMA_LKEY_FLAG_REMOTE_WR	= 0x4,
+	OCRDMA_LKEY_FLAG_VATO		= 0x8,
+};
+
+enum OCRDMA_WQE_OPCODE {
+	OCRDMA_WRITE		= 0x06,
+	OCRDMA_READ		= 0x0C,
+	OCRDMA_RESV0		= 0x02,
+	OCRDMA_SEND		= 0x00,
+	OCRDMA_CMP_SWP		= 0x14,
+	OCRDMA_BIND_MW		= 0x10,
+	OCRDMA_RESV1		= 0x0A,
+	OCRDMA_LKEY_INV		= 0x15,
+	OCRDMA_FETCH_ADD	= 0x13,
+	OCRDMA_POST_RQ		= 0x12
+};
+
+enum {
+	OCRDMA_TYPE_INLINE	= 0x0,
+	OCRDMA_TYPE_LKEY	= 0x1,
+};
+
+enum {
+	OCRDMA_WQE_OPCODE_SHIFT		= 0,
+	OCRDMA_WQE_OPCODE_MASK		= 0x0000001F,
+	OCRDMA_WQE_FLAGS_SHIFT		= 5,
+	OCRDMA_WQE_TYPE_SHIFT		= 16,
+	OCRDMA_WQE_TYPE_MASK		= 0x00030000,
+	OCRDMA_WQE_SIZE_SHIFT		= 18,
+	OCRDMA_WQE_SIZE_MASK		= 0xFF,
+	OCRDMA_WQE_NXT_WQE_SIZE_SHIFT	= 25,
+
+	OCRDMA_WQE_LKEY_FLAGS_SHIFT	= 0,
+	OCRDMA_WQE_LKEY_FLAGS_MASK	= 0xF
+};
+
+/* header WQE for all the SQ and RQ operations */
+struct ocrdma_hdr_wqe {
+	u32 cw;
+	union {
+		u32 rsvd_tag;
+		u32 rsvd_lkey_flags;
+	};
+	union {
+		u32 immdt;
+		u32 lkey;
+	};
+	u32 total_len;
+} __packed;
+
+struct ocrdma_ewqe_ud_hdr {
+	u32 rsvd_dest_qpn;
+	u32 qkey;
+	u32 rsvd_ahid;
+	u32 rsvd;
+} __packed;
+
+struct ocrdma_eth_basic {
+	u8 dmac[6];
+	u8 smac[6];
+	__be16 eth_type;
+} __packed;
+
+struct ocrdma_eth_vlan {
+	u8 dmac[6];
+	u8 smac[6];
+	__be16 eth_type;
+	__be16 vlan_tag;
+#define OCRDMA_ROCE_ETH_TYPE 0x8915
+	__be16 roce_eth_type;
+} __packed;
+
+struct ocrdma_grh {
+	__be32	tclass_flow;
+	__be32	pdid_hoplimit;
+	u8	sgid[16];
+	u8	dgid[16];
+	u16	rsvd;
+} __packed;
+
+#define OCRDMA_AV_VALID		Bit(0)
+#define OCRDMA_AV_VLAN_VALID	Bit(1)
+
+struct ocrdma_av {
+	struct ocrdma_eth_vlan eth_hdr;
+	struct ocrdma_grh grh;
+	u32 valid;
+} __packed;
+
+#endif				/* __OCRDMA_SLI_H__ */
diff --git a/drivers/infiniband/hw/ocrdma/ocrdma_verbs.c b/drivers/infiniband/hw/ocrdma/ocrdma_verbs.c
new file mode 100644
index 00000000000..e9f74d1b48f
--- /dev/null
+++ b/drivers/infiniband/hw/ocrdma/ocrdma_verbs.c
@@ -0,0 +1,2537 @@
+/*******************************************************************
+ * This file is part of the Emulex RoCE Device Driver for          *
+ * RoCE (RDMA over Converged Ethernet) adapters.                   *
+ * Copyright (C) 2008-2012 Emulex. All rights reserved.            *
+ * EMULEX and SLI are trademarks of Emulex.                        *
+ * www.emulex.com                                                  *
+ *                                                                 *
+ * 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. *
+ * ALL EXPRESS OR IMPLIED CONDITIONS, REPRESENTATIONS AND          *
+ * WARRANTIES, INCLUDING ANY IMPLIED WARRANTY OF MERCHANTABILITY,  *
+ * FITNESS FOR A PARTICULAR PURPOSE, OR NON-INFRINGEMENT, ARE      *
+ * DISCLAIMED, EXCEPT TO THE EXTENT THAT SUCH DISCLAIMERS ARE HELD *
+ * TO BE LEGALLY INVALID.  See the GNU General Public License for  *
+ * more details, a copy of which can be found in the file COPYING  *
+ * included with this package.                                     *
+ *
+ * Contact Information:
+ * linux-drivers@emulex.com
+ *
+ * Emulex
+ * 3333 Susan Street
+ * Costa Mesa, CA 92626
+ *******************************************************************/
+
+#include <linux/dma-mapping.h>
+#include <rdma/ib_verbs.h>
+#include <rdma/ib_user_verbs.h>
+#include <rdma/iw_cm.h>
+#include <rdma/ib_umem.h>
+#include <rdma/ib_addr.h>
+
+#include "ocrdma.h"
+#include "ocrdma_hw.h"
+#include "ocrdma_verbs.h"
+#include "ocrdma_abi.h"
+
+int ocrdma_query_pkey(struct ib_device *ibdev, u8 port, u16 index, u16 *pkey)
+{
+	if (index > 1)
+		return -EINVAL;
+
+	*pkey = 0xffff;
+	return 0;
+}
+
+int ocrdma_query_gid(struct ib_device *ibdev, u8 port,
+		     int index, union ib_gid *sgid)
+{
+	struct ocrdma_dev *dev;
+
+	dev = get_ocrdma_dev(ibdev);
+	memset(sgid, 0, sizeof(*sgid));
+	if (index > OCRDMA_MAX_SGID)
+		return -EINVAL;
+
+	memcpy(sgid, &dev->sgid_tbl[index], sizeof(*sgid));
+
+	return 0;
+}
+
+int ocrdma_query_device(struct ib_device *ibdev, struct ib_device_attr *attr)
+{
+	struct ocrdma_dev *dev = get_ocrdma_dev(ibdev);
+
+	memset(attr, 0, sizeof *attr);
+	memcpy(&attr->fw_ver, &dev->attr.fw_ver[0],
+	       min(sizeof(dev->attr.fw_ver), sizeof(attr->fw_ver)));
+	ocrdma_get_guid(dev, (u8 *)&attr->sys_image_guid);
+	attr->max_mr_size = ~0ull;
+	attr->page_size_cap = 0xffff000;
+	attr->vendor_id = dev->nic_info.pdev->vendor;
+	attr->vendor_part_id = dev->nic_info.pdev->device;
+	attr->hw_ver = 0;
+	attr->max_qp = dev->attr.max_qp;
+	attr->max_ah = dev->attr.max_qp;
+	attr->max_qp_wr = dev->attr.max_wqe;
+
+	attr->device_cap_flags = IB_DEVICE_CURR_QP_STATE_MOD |
+					IB_DEVICE_RC_RNR_NAK_GEN |
+					IB_DEVICE_SHUTDOWN_PORT |
+					IB_DEVICE_SYS_IMAGE_GUID |
+					IB_DEVICE_LOCAL_DMA_LKEY;
+	attr->max_sge = dev->attr.max_send_sge;
+	attr->max_sge_rd = dev->attr.max_send_sge;
+	attr->max_cq = dev->attr.max_cq;
+	attr->max_cqe = dev->attr.max_cqe;
+	attr->max_mr = dev->attr.max_mr;
+	attr->max_mw = 0;
+	attr->max_pd = dev->attr.max_pd;
+	attr->atomic_cap = 0;
+	attr->max_fmr = 0;
+	attr->max_map_per_fmr = 0;
+	attr->max_qp_rd_atom =
+	    min(dev->attr.max_ord_per_qp, dev->attr.max_ird_per_qp);
+	attr->max_qp_init_rd_atom = dev->attr.max_ord_per_qp;
+	attr->max_srq = (dev->attr.max_qp - 1);
+	attr->max_srq_sge = attr->max_sge;
+	attr->max_srq_wr = dev->attr.max_rqe;
+	attr->local_ca_ack_delay = dev->attr.local_ca_ack_delay;
+	attr->max_fast_reg_page_list_len = 0;
+	attr->max_pkeys = 1;
+	return 0;
+}
+
+int ocrdma_query_port(struct ib_device *ibdev,
+		      u8 port, struct ib_port_attr *props)
+{
+	enum ib_port_state port_state;
+	struct ocrdma_dev *dev;
+	struct net_device *netdev;
+
+	dev = get_ocrdma_dev(ibdev);
+	if (port > 1) {
+		ocrdma_err("%s(%d) invalid_port=0x%x\n", __func__,
+			   dev->id, port);
+		return -EINVAL;
+	}
+	netdev = dev->nic_info.netdev;
+	if (netif_running(netdev) && netif_oper_up(netdev)) {
+		port_state = IB_PORT_ACTIVE;
+		props->phys_state = 5;
+	} else {
+		port_state = IB_PORT_DOWN;
+		props->phys_state = 3;
+	}
+	props->max_mtu = IB_MTU_4096;
+	props->active_mtu = iboe_get_mtu(netdev->mtu);
+	props->lid = 0;
+	props->lmc = 0;
+	props->sm_lid = 0;
+	props->sm_sl = 0;
+	props->state = port_state;
+	props->port_cap_flags =
+	    IB_PORT_CM_SUP |
+	    IB_PORT_REINIT_SUP |
+	    IB_PORT_DEVICE_MGMT_SUP | IB_PORT_VENDOR_CLASS_SUP;
+	props->gid_tbl_len = OCRDMA_MAX_SGID;
+	props->pkey_tbl_len = 1;
+	props->bad_pkey_cntr = 0;
+	props->qkey_viol_cntr = 0;
+	props->active_width = IB_WIDTH_1X;
+	props->active_speed = 4;
+	props->max_msg_sz = 0x80000000;
+	props->max_vl_num = 4;
+	return 0;
+}
+
+int ocrdma_modify_port(struct ib_device *ibdev, u8 port, int mask,
+		       struct ib_port_modify *props)
+{
+	struct ocrdma_dev *dev;
+
+	dev = get_ocrdma_dev(ibdev);
+	if (port > 1) {
+		ocrdma_err("%s(%d) invalid_port=0x%x\n", __func__,
+			   dev->id, port);
+		return -EINVAL;
+	}
+	return 0;
+}
+
+static int ocrdma_add_mmap(struct ocrdma_ucontext *uctx, u64 phy_addr,
+			   unsigned long len)
+{
+	struct ocrdma_mm *mm;
+
+	mm = kzalloc(sizeof(*mm), GFP_KERNEL);
+	if (mm == NULL)
+		return -ENOMEM;
+	mm->key.phy_addr = phy_addr;
+	mm->key.len = len;
+	INIT_LIST_HEAD(&mm->entry);
+
+	mutex_lock(&uctx->mm_list_lock);
+	list_add_tail(&mm->entry, &uctx->mm_head);
+	mutex_unlock(&uctx->mm_list_lock);
+	return 0;
+}
+
+static void ocrdma_del_mmap(struct ocrdma_ucontext *uctx, u64 phy_addr,
+			    unsigned long len)
+{
+	struct ocrdma_mm *mm, *tmp;
+
+	mutex_lock(&uctx->mm_list_lock);
+	list_for_each_entry_safe(mm, tmp, &uctx->mm_head, entry) {
+		if (len != mm->key.len || phy_addr != mm->key.phy_addr)
+			continue;
+
+		list_del(&mm->entry);
+		kfree(mm);
+		break;
+	}
+	mutex_unlock(&uctx->mm_list_lock);
+}
+
+static bool ocrdma_search_mmap(struct ocrdma_ucontext *uctx, u64 phy_addr,
+			      unsigned long len)
+{
+	bool found = false;
+	struct ocrdma_mm *mm;
+
+	mutex_lock(&uctx->mm_list_lock);
+	list_for_each_entry(mm, &uctx->mm_head, entry) {
+		if (len != mm->key.len || phy_addr != mm->key.phy_addr)
+			continue;
+
+		found = true;
+		break;
+	}
+	mutex_unlock(&uctx->mm_list_lock);
+	return found;
+}
+
+struct ib_ucontext *ocrdma_alloc_ucontext(struct ib_device *ibdev,
+					  struct ib_udata *udata)
+{
+	int status;
+	struct ocrdma_ucontext *ctx;
+	struct ocrdma_alloc_ucontext_resp resp;
+	struct ocrdma_dev *dev = get_ocrdma_dev(ibdev);
+	struct pci_dev *pdev = dev->nic_info.pdev;
+	u32 map_len = roundup(sizeof(u32) * 2048, PAGE_SIZE);
+
+	if (!udata)
+		return ERR_PTR(-EFAULT);
+	ctx = kzalloc(sizeof(*ctx), GFP_KERNEL);
+	if (!ctx)
+		return ERR_PTR(-ENOMEM);
+	ctx->dev = dev;
+	INIT_LIST_HEAD(&ctx->mm_head);
+	mutex_init(&ctx->mm_list_lock);
+
+	ctx->ah_tbl.va = dma_alloc_coherent(&pdev->dev, map_len,
+					    &ctx->ah_tbl.pa, GFP_KERNEL);
+	if (!ctx->ah_tbl.va) {
+		kfree(ctx);
+		return ERR_PTR(-ENOMEM);
+	}
+	memset(ctx->ah_tbl.va, 0, map_len);
+	ctx->ah_tbl.len = map_len;
+
+	resp.ah_tbl_len = ctx->ah_tbl.len;
+	resp.ah_tbl_page = ctx->ah_tbl.pa;
+
+	status = ocrdma_add_mmap(ctx, resp.ah_tbl_page, resp.ah_tbl_len);
+	if (status)
+		goto map_err;
+	resp.dev_id = dev->id;
+	resp.max_inline_data = dev->attr.max_inline_data;
+	resp.wqe_size = dev->attr.wqe_size;
+	resp.rqe_size = dev->attr.rqe_size;
+	resp.dpp_wqe_size = dev->attr.wqe_size;
+	resp.rsvd = 0;
+
+	memcpy(resp.fw_ver, dev->attr.fw_ver, sizeof(resp.fw_ver));
+	status = ib_copy_to_udata(udata, &resp, sizeof(resp));
+	if (status)
+		goto cpy_err;
+	return &ctx->ibucontext;
+
+cpy_err:
+	ocrdma_del_mmap(ctx, ctx->ah_tbl.pa, ctx->ah_tbl.len);
+map_err:
+	dma_free_coherent(&pdev->dev, ctx->ah_tbl.len, ctx->ah_tbl.va,
+			  ctx->ah_tbl.pa);
+	kfree(ctx);
+	return ERR_PTR(status);
+}
+
+int ocrdma_dealloc_ucontext(struct ib_ucontext *ibctx)
+{
+	struct ocrdma_mm *mm, *tmp;
+	struct ocrdma_ucontext *uctx = get_ocrdma_ucontext(ibctx);
+	struct pci_dev *pdev = uctx->dev->nic_info.pdev;
+
+	ocrdma_del_mmap(uctx, uctx->ah_tbl.pa, uctx->ah_tbl.len);
+	dma_free_coherent(&pdev->dev, uctx->ah_tbl.len, uctx->ah_tbl.va,
+			  uctx->ah_tbl.pa);
+
+	list_for_each_entry_safe(mm, tmp, &uctx->mm_head, entry) {
+		list_del(&mm->entry);
+		kfree(mm);
+	}
+	kfree(uctx);
+	return 0;
+}
+
+int ocrdma_mmap(struct ib_ucontext *context, struct vm_area_struct *vma)
+{
+	struct ocrdma_ucontext *ucontext = get_ocrdma_ucontext(context);
+	struct ocrdma_dev *dev = ucontext->dev;
+	unsigned long vm_page = vma->vm_pgoff << PAGE_SHIFT;
+	u64 unmapped_db = (u64) dev->nic_info.unmapped_db;
+	unsigned long len = (vma->vm_end - vma->vm_start);
+	int status = 0;
+	bool found;
+
+	if (vma->vm_start & (PAGE_SIZE - 1))
+		return -EINVAL;
+	found = ocrdma_search_mmap(ucontext, vma->vm_pgoff << PAGE_SHIFT, len);
+	if (!found)
+		return -EINVAL;
+
+	if ((vm_page >= unmapped_db) && (vm_page <= (unmapped_db +
+		dev->nic_info.db_total_size)) &&
+		(len <=	dev->nic_info.db_page_size)) {
+		/* doorbell mapping */
+		status = io_remap_pfn_range(vma, vma->vm_start, vma->vm_pgoff,
+					    len, vma->vm_page_prot);
+	} else if (dev->nic_info.dpp_unmapped_len &&
+		(vm_page >= (u64) dev->nic_info.dpp_unmapped_addr) &&
+		(vm_page <= (u64) (dev->nic_info.dpp_unmapped_addr +
+			dev->nic_info.dpp_unmapped_len)) &&
+		(len <= dev->nic_info.dpp_unmapped_len)) {
+		/* dpp area mapping */
+		vma->vm_page_prot = pgprot_writecombine(vma->vm_page_prot);
+		status = io_remap_pfn_range(vma, vma->vm_start, vma->vm_pgoff,
+					    len, vma->vm_page_prot);
+	} else {
+		/* queue memory mapping */
+		status = remap_pfn_range(vma, vma->vm_start,
+					 vma->vm_pgoff, len, vma->vm_page_prot);
+	}
+	return status;
+}
+
+static int ocrdma_copy_pd_uresp(struct ocrdma_pd *pd,
+				struct ib_ucontext *ib_ctx,
+				struct ib_udata *udata)
+{
+	int status;
+	u64 db_page_addr;
+	u64 dpp_page_addr = 0;
+	u32 db_page_size;
+	struct ocrdma_alloc_pd_uresp rsp;
+	struct ocrdma_ucontext *uctx = get_ocrdma_ucontext(ib_ctx);
+
+	rsp.id = pd->id;
+	rsp.dpp_enabled = pd->dpp_enabled;
+	db_page_addr = pd->dev->nic_info.unmapped_db +
+			(pd->id * pd->dev->nic_info.db_page_size);
+	db_page_size = pd->dev->nic_info.db_page_size;
+
+	status = ocrdma_add_mmap(uctx, db_page_addr, db_page_size);
+	if (status)
+		return status;
+
+	if (pd->dpp_enabled) {
+		dpp_page_addr = pd->dev->nic_info.dpp_unmapped_addr +
+				(pd->id * OCRDMA_DPP_PAGE_SIZE);
+		status = ocrdma_add_mmap(uctx, dpp_page_addr,
+				 OCRDMA_DPP_PAGE_SIZE);
+		if (status)
+			goto dpp_map_err;
+		rsp.dpp_page_addr_hi = upper_32_bits(dpp_page_addr);
+		rsp.dpp_page_addr_lo = dpp_page_addr;
+	}
+
+	status = ib_copy_to_udata(udata, &rsp, sizeof(rsp));
+	if (status)
+		goto ucopy_err;
+
+	pd->uctx = uctx;
+	return 0;
+
+ucopy_err:
+	if (pd->dpp_enabled)
+		ocrdma_del_mmap(pd->uctx, dpp_page_addr, OCRDMA_DPP_PAGE_SIZE);
+dpp_map_err:
+	ocrdma_del_mmap(pd->uctx, db_page_addr, db_page_size);
+	return status;
+}
+
+struct ib_pd *ocrdma_alloc_pd(struct ib_device *ibdev,
+			      struct ib_ucontext *context,
+			      struct ib_udata *udata)
+{
+	struct ocrdma_dev *dev = get_ocrdma_dev(ibdev);
+	struct ocrdma_pd *pd;
+	int status;
+
+	pd = kzalloc(sizeof(*pd), GFP_KERNEL);
+	if (!pd)
+		return ERR_PTR(-ENOMEM);
+	pd->dev = dev;
+	if (udata && context) {
+		pd->dpp_enabled = (dev->nic_info.dev_family ==
+					OCRDMA_GEN2_FAMILY) ? true : false;
+		pd->num_dpp_qp =
+			pd->dpp_enabled ? OCRDMA_PD_MAX_DPP_ENABLED_QP : 0;
+	}
+	status = ocrdma_mbx_alloc_pd(dev, pd);
+	if (status) {
+		kfree(pd);
+		return ERR_PTR(status);
+	}
+	atomic_set(&pd->use_cnt, 0);
+
+	if (udata && context) {
+		status = ocrdma_copy_pd_uresp(pd, context, udata);
+		if (status)
+			goto err;
+	}
+	return &pd->ibpd;
+
+err:
+	ocrdma_dealloc_pd(&pd->ibpd);
+	return ERR_PTR(status);
+}
+
+int ocrdma_dealloc_pd(struct ib_pd *ibpd)
+{
+	struct ocrdma_pd *pd = get_ocrdma_pd(ibpd);
+	struct ocrdma_dev *dev = pd->dev;
+	int status;
+	u64 usr_db;
+
+	if (atomic_read(&pd->use_cnt)) {
+		ocrdma_err("%s(%d) pd=0x%x is in use.\n",
+			   __func__, dev->id, pd->id);
+		status = -EFAULT;
+		goto dealloc_err;
+	}
+	status = ocrdma_mbx_dealloc_pd(dev, pd);
+	if (pd->uctx) {
+		u64 dpp_db = dev->nic_info.dpp_unmapped_addr +
+		    (pd->id * OCRDMA_DPP_PAGE_SIZE);
+		if (pd->dpp_enabled)
+			ocrdma_del_mmap(pd->uctx, dpp_db, OCRDMA_DPP_PAGE_SIZE);
+		usr_db = dev->nic_info.unmapped_db +
+		    (pd->id * dev->nic_info.db_page_size);
+		ocrdma_del_mmap(pd->uctx, usr_db, dev->nic_info.db_page_size);
+	}
+	kfree(pd);
+dealloc_err:
+	return status;
+}
+
+static struct ocrdma_mr *ocrdma_alloc_lkey(struct ib_pd *ibpd,
+					   int acc, u32 num_pbls,
+					   u32 addr_check)
+{
+	int status;
+	struct ocrdma_mr *mr;
+	struct ocrdma_pd *pd = get_ocrdma_pd(ibpd);
+	struct ocrdma_dev *dev = pd->dev;
+
+	if (acc & IB_ACCESS_REMOTE_WRITE && !(acc & IB_ACCESS_LOCAL_WRITE)) {
+		ocrdma_err("%s(%d) leaving err, invalid access rights\n",
+			   __func__, dev->id);
+		return ERR_PTR(-EINVAL);
+	}
+
+	mr = kzalloc(sizeof(*mr), GFP_KERNEL);
+	if (!mr)
+		return ERR_PTR(-ENOMEM);
+	mr->hwmr.dev = dev;
+	mr->hwmr.fr_mr = 0;
+	mr->hwmr.local_rd = 1;
+	mr->hwmr.remote_rd = (acc & IB_ACCESS_REMOTE_READ) ? 1 : 0;
+	mr->hwmr.remote_wr = (acc & IB_ACCESS_REMOTE_WRITE) ? 1 : 0;
+	mr->hwmr.local_wr = (acc & IB_ACCESS_LOCAL_WRITE) ? 1 : 0;
+	mr->hwmr.mw_bind = (acc & IB_ACCESS_MW_BIND) ? 1 : 0;
+	mr->hwmr.remote_atomic = (acc & IB_ACCESS_REMOTE_ATOMIC) ? 1 : 0;
+	mr->hwmr.num_pbls = num_pbls;
+
+	status = ocrdma_mbx_alloc_lkey(dev, &mr->hwmr, pd->id, addr_check);
+	if (status) {
+		kfree(mr);
+		return ERR_PTR(-ENOMEM);
+	}
+	mr->pd = pd;
+	atomic_inc(&pd->use_cnt);
+	mr->ibmr.lkey = mr->hwmr.lkey;
+	if (mr->hwmr.remote_wr || mr->hwmr.remote_rd)
+		mr->ibmr.rkey = mr->hwmr.lkey;
+	return mr;
+}
+
+struct ib_mr *ocrdma_get_dma_mr(struct ib_pd *ibpd, int acc)
+{
+	struct ocrdma_mr *mr;
+
+	mr = ocrdma_alloc_lkey(ibpd, acc, 0, OCRDMA_ADDR_CHECK_DISABLE);
+	if (IS_ERR(mr))
+		return ERR_CAST(mr);
+
+	return &mr->ibmr;
+}
+
+static void ocrdma_free_mr_pbl_tbl(struct ocrdma_dev *dev,
+				   struct ocrdma_hw_mr *mr)
+{
+	struct pci_dev *pdev = dev->nic_info.pdev;
+	int i = 0;
+
+	if (mr->pbl_table) {
+		for (i = 0; i < mr->num_pbls; i++) {
+			if (!mr->pbl_table[i].va)
+				continue;
+			dma_free_coherent(&pdev->dev, mr->pbl_size,
+					  mr->pbl_table[i].va,
+					  mr->pbl_table[i].pa);
+		}
+		kfree(mr->pbl_table);
+		mr->pbl_table = NULL;
+	}
+}
+
+static int ocrdma_get_pbl_info(struct ocrdma_mr *mr, u32 num_pbes)
+{
+	u32 num_pbls = 0;
+	u32 idx = 0;
+	int status = 0;
+	u32 pbl_size;
+
+	do {
+		pbl_size = OCRDMA_MIN_HPAGE_SIZE * (1 << idx);
+		if (pbl_size > MAX_OCRDMA_PBL_SIZE) {
+			status = -EFAULT;
+			break;
+		}
+		num_pbls = roundup(num_pbes, (pbl_size / sizeof(u64)));
+		num_pbls = num_pbls / (pbl_size / sizeof(u64));
+		idx++;
+	} while (num_pbls >= mr->hwmr.dev->attr.max_num_mr_pbl);
+
+	mr->hwmr.num_pbes = num_pbes;
+	mr->hwmr.num_pbls = num_pbls;
+	mr->hwmr.pbl_size = pbl_size;
+	return status;
+}
+
+static int ocrdma_build_pbl_tbl(struct ocrdma_dev *dev, struct ocrdma_hw_mr *mr)
+{
+	int status = 0;
+	int i;
+	u32 dma_len = mr->pbl_size;
+	struct pci_dev *pdev = dev->nic_info.pdev;
+	void *va;
+	dma_addr_t pa;
+
+	mr->pbl_table = kzalloc(sizeof(struct ocrdma_pbl) *
+				mr->num_pbls, GFP_KERNEL);
+
+	if (!mr->pbl_table)
+		return -ENOMEM;
+
+	for (i = 0; i < mr->num_pbls; i++) {
+		va = dma_alloc_coherent(&pdev->dev, dma_len, &pa, GFP_KERNEL);
+		if (!va) {
+			ocrdma_free_mr_pbl_tbl(dev, mr);
+			status = -ENOMEM;
+			break;
+		}
+		memset(va, 0, dma_len);
+		mr->pbl_table[i].va = va;
+		mr->pbl_table[i].pa = pa;
+	}
+	return status;
+}
+
+static void build_user_pbes(struct ocrdma_dev *dev, struct ocrdma_mr *mr,
+			    u32 num_pbes)
+{
+	struct ocrdma_pbe *pbe;
+	struct ib_umem_chunk *chunk;
+	struct ocrdma_pbl *pbl_tbl = mr->hwmr.pbl_table;
+	struct ib_umem *umem = mr->umem;
+	int i, shift, pg_cnt, pages, pbe_cnt, total_num_pbes = 0;
+
+	if (!mr->hwmr.num_pbes)
+		return;
+
+	pbe = (struct ocrdma_pbe *)pbl_tbl->va;
+	pbe_cnt = 0;
+
+	shift = ilog2(umem->page_size);
+
+	list_for_each_entry(chunk, &umem->chunk_list, list) {
+		/* get all the dma regions from the chunk. */
+		for (i = 0; i < chunk->nmap; i++) {
+			pages = sg_dma_len(&chunk->page_list[i]) >> shift;
+			for (pg_cnt = 0; pg_cnt < pages; pg_cnt++) {
+				/* store the page address in pbe */
+				pbe->pa_lo =
+				    cpu_to_le32(sg_dma_address
+						(&chunk->page_list[i]) +
+						(umem->page_size * pg_cnt));
+				pbe->pa_hi =
+				    cpu_to_le32(upper_32_bits
+						((sg_dma_address
+						  (&chunk->page_list[i]) +
+						  umem->page_size * pg_cnt)));
+				pbe_cnt += 1;
+				total_num_pbes += 1;
+				pbe++;
+
+				/* if done building pbes, issue the mbx cmd. */
+				if (total_num_pbes == num_pbes)
+					return;
+
+				/* if the given pbl is full storing the pbes,
+				 * move to next pbl.
+				 */
+				if (pbe_cnt ==
+					(mr->hwmr.pbl_size / sizeof(u64))) {
+					pbl_tbl++;
+					pbe = (struct ocrdma_pbe *)pbl_tbl->va;
+					pbe_cnt = 0;
+				}
+			}
+		}
+	}
+}
+
+struct ib_mr *ocrdma_reg_user_mr(struct ib_pd *ibpd, u64 start, u64 len,
+				 u64 usr_addr, int acc, struct ib_udata *udata)
+{
+	int status = -ENOMEM;
+	struct ocrdma_dev *dev;
+	struct ocrdma_mr *mr;
+	struct ocrdma_pd *pd;
+	u32 num_pbes;
+
+	pd = get_ocrdma_pd(ibpd);
+	dev = pd->dev;
+
+	if (acc & IB_ACCESS_REMOTE_WRITE && !(acc & IB_ACCESS_LOCAL_WRITE))
+		return ERR_PTR(-EINVAL);
+
+	mr = kzalloc(sizeof(*mr), GFP_KERNEL);
+	if (!mr)
+		return ERR_PTR(status);
+	mr->hwmr.dev = dev;
+	mr->umem = ib_umem_get(ibpd->uobject->context, start, len, acc, 0);
+	if (IS_ERR(mr->umem)) {
+		status = -EFAULT;
+		goto umem_err;
+	}
+	num_pbes = ib_umem_page_count(mr->umem);
+	status = ocrdma_get_pbl_info(mr, num_pbes);
+	if (status)
+		goto umem_err;
+
+	mr->hwmr.pbe_size = mr->umem->page_size;
+	mr->hwmr.fbo = mr->umem->offset;
+	mr->hwmr.va = usr_addr;
+	mr->hwmr.len = len;
+	mr->hwmr.remote_wr = (acc & IB_ACCESS_REMOTE_WRITE) ? 1 : 0;
+	mr->hwmr.remote_rd = (acc & IB_ACCESS_REMOTE_READ) ? 1 : 0;
+	mr->hwmr.local_wr = (acc & IB_ACCESS_LOCAL_WRITE) ? 1 : 0;
+	mr->hwmr.local_rd = 1;
+	mr->hwmr.remote_atomic = (acc & IB_ACCESS_REMOTE_ATOMIC) ? 1 : 0;
+	status = ocrdma_build_pbl_tbl(dev, &mr->hwmr);
+	if (status)
+		goto umem_err;
+	build_user_pbes(dev, mr, num_pbes);
+	status = ocrdma_reg_mr(dev, &mr->hwmr, pd->id, acc);
+	if (status)
+		goto mbx_err;
+	mr->pd = pd;
+	atomic_inc(&pd->use_cnt);
+	mr->ibmr.lkey = mr->hwmr.lkey;
+	if (mr->hwmr.remote_wr || mr->hwmr.remote_rd)
+		mr->ibmr.rkey = mr->hwmr.lkey;
+
+	return &mr->ibmr;
+
+mbx_err:
+	ocrdma_free_mr_pbl_tbl(dev, &mr->hwmr);
+umem_err:
+	kfree(mr);
+	return ERR_PTR(status);
+}
+
+int ocrdma_dereg_mr(struct ib_mr *ib_mr)
+{
+	struct ocrdma_mr *mr = get_ocrdma_mr(ib_mr);
+	struct ocrdma_dev *dev = mr->hwmr.dev;
+	int status;
+
+	status = ocrdma_mbx_dealloc_lkey(dev, mr->hwmr.fr_mr, mr->hwmr.lkey);
+
+	if (mr->hwmr.fr_mr == 0)
+		ocrdma_free_mr_pbl_tbl(dev, &mr->hwmr);
+
+	atomic_dec(&mr->pd->use_cnt);
+	/* it could be user registered memory. */
+	if (mr->umem)
+		ib_umem_release(mr->umem);
+	kfree(mr);
+	return status;
+}
+
+static int ocrdma_copy_cq_uresp(struct ocrdma_cq *cq, struct ib_udata *udata,
+				struct ib_ucontext *ib_ctx)
+{
+	int status;
+	struct ocrdma_ucontext *uctx;
+	struct ocrdma_create_cq_uresp uresp;
+
+	uresp.cq_id = cq->id;
+	uresp.page_size = cq->len;
+	uresp.num_pages = 1;
+	uresp.max_hw_cqe = cq->max_hw_cqe;
+	uresp.page_addr[0] = cq->pa;
+	uresp.db_page_addr = cq->dev->nic_info.unmapped_db;
+	uresp.db_page_size = cq->dev->nic_info.db_page_size;
+	uresp.phase_change = cq->phase_change ? 1 : 0;
+	status = ib_copy_to_udata(udata, &uresp, sizeof(uresp));
+	if (status) {
+		ocrdma_err("%s(%d) copy error cqid=0x%x.\n",
+			   __func__, cq->dev->id, cq->id);
+		goto err;
+	}
+	uctx = get_ocrdma_ucontext(ib_ctx);
+	status = ocrdma_add_mmap(uctx, uresp.db_page_addr, uresp.db_page_size);
+	if (status)
+		goto err;
+	status = ocrdma_add_mmap(uctx, uresp.page_addr[0], uresp.page_size);
+	if (status) {
+		ocrdma_del_mmap(uctx, uresp.db_page_addr, uresp.db_page_size);
+		goto err;
+	}
+	cq->ucontext = uctx;
+err:
+	return status;
+}
+
+struct ib_cq *ocrdma_create_cq(struct ib_device *ibdev, int entries, int vector,
+			       struct ib_ucontext *ib_ctx,
+			       struct ib_udata *udata)
+{
+	struct ocrdma_cq *cq;
+	struct ocrdma_dev *dev = get_ocrdma_dev(ibdev);
+	int status;
+	struct ocrdma_create_cq_ureq ureq;
+
+	if (udata) {
+		if (ib_copy_from_udata(&ureq, udata, sizeof(ureq)))
+			return ERR_PTR(-EFAULT);
+	} else
+		ureq.dpp_cq = 0;
+	cq = kzalloc(sizeof(*cq), GFP_KERNEL);
+	if (!cq)
+		return ERR_PTR(-ENOMEM);
+
+	spin_lock_init(&cq->cq_lock);
+	spin_lock_init(&cq->comp_handler_lock);
+	atomic_set(&cq->use_cnt, 0);
+	INIT_LIST_HEAD(&cq->sq_head);
+	INIT_LIST_HEAD(&cq->rq_head);
+	cq->dev = dev;
+
+	status = ocrdma_mbx_create_cq(dev, cq, entries, ureq.dpp_cq);
+	if (status) {
+		kfree(cq);
+		return ERR_PTR(status);
+	}
+	if (ib_ctx) {
+		status = ocrdma_copy_cq_uresp(cq, udata, ib_ctx);
+		if (status)
+			goto ctx_err;
+	}
+	cq->phase = OCRDMA_CQE_VALID;
+	cq->arm_needed = true;
+	dev->cq_tbl[cq->id] = cq;
+
+	return &cq->ibcq;
+
+ctx_err:
+	ocrdma_mbx_destroy_cq(dev, cq);
+	kfree(cq);
+	return ERR_PTR(status);
+}
+
+int ocrdma_resize_cq(struct ib_cq *ibcq, int new_cnt,
+		     struct ib_udata *udata)
+{
+	int status = 0;
+	struct ocrdma_cq *cq = get_ocrdma_cq(ibcq);
+
+	if (new_cnt < 1 || new_cnt > cq->max_hw_cqe) {
+		status = -EINVAL;
+		return status;
+	}
+	ibcq->cqe = new_cnt;
+	return status;
+}
+
+int ocrdma_destroy_cq(struct ib_cq *ibcq)
+{
+	int status;
+	struct ocrdma_cq *cq = get_ocrdma_cq(ibcq);
+	struct ocrdma_dev *dev = cq->dev;
+
+	if (atomic_read(&cq->use_cnt))
+		return -EINVAL;
+
+	status = ocrdma_mbx_destroy_cq(dev, cq);
+
+	if (cq->ucontext) {
+		ocrdma_del_mmap(cq->ucontext, (u64) cq->pa, cq->len);
+		ocrdma_del_mmap(cq->ucontext, dev->nic_info.unmapped_db,
+				dev->nic_info.db_page_size);
+	}
+	dev->cq_tbl[cq->id] = NULL;
+
+	kfree(cq);
+	return status;
+}
+
+static int ocrdma_add_qpn_map(struct ocrdma_dev *dev, struct ocrdma_qp *qp)
+{
+	int status = -EINVAL;
+
+	if (qp->id < OCRDMA_MAX_QP && dev->qp_tbl[qp->id] == NULL) {
+		dev->qp_tbl[qp->id] = qp;
+		status = 0;
+	}
+	return status;
+}
+
+static void ocrdma_del_qpn_map(struct ocrdma_dev *dev, struct ocrdma_qp *qp)
+{
+	dev->qp_tbl[qp->id] = NULL;
+}
+
+static int ocrdma_check_qp_params(struct ib_pd *ibpd, struct ocrdma_dev *dev,
+				  struct ib_qp_init_attr *attrs)
+{
+	if (attrs->qp_type != IB_QPT_GSI &&
+	    attrs->qp_type != IB_QPT_RC &&
+	    attrs->qp_type != IB_QPT_UD) {
+		ocrdma_err("%s(%d) unsupported qp type=0x%x requested\n",
+			   __func__, dev->id, attrs->qp_type);
+		return -EINVAL;
+	}
+	if (attrs->cap.max_send_wr > dev->attr.max_wqe) {
+		ocrdma_err("%s(%d) unsupported send_wr=0x%x requested\n",
+			   __func__, dev->id, attrs->cap.max_send_wr);
+		ocrdma_err("%s(%d) supported send_wr=0x%x\n",
+			   __func__, dev->id, dev->attr.max_wqe);
+		return -EINVAL;
+	}
+	if (!attrs->srq && (attrs->cap.max_recv_wr > dev->attr.max_rqe)) {
+		ocrdma_err("%s(%d) unsupported recv_wr=0x%x requested\n",
+			   __func__, dev->id, attrs->cap.max_recv_wr);
+		ocrdma_err("%s(%d) supported recv_wr=0x%x\n",
+			   __func__, dev->id, dev->attr.max_rqe);
+		return -EINVAL;
+	}
+	if (attrs->cap.max_inline_data > dev->attr.max_inline_data) {
+		ocrdma_err("%s(%d) unsupported inline data size=0x%x"
+			   " requested\n", __func__, dev->id,
+			   attrs->cap.max_inline_data);
+		ocrdma_err("%s(%d) supported inline data size=0x%x\n",
+			   __func__, dev->id, dev->attr.max_inline_data);
+		return -EINVAL;
+	}
+	if (attrs->cap.max_send_sge > dev->attr.max_send_sge) {
+		ocrdma_err("%s(%d) unsupported send_sge=0x%x requested\n",
+			   __func__, dev->id, attrs->cap.max_send_sge);
+		ocrdma_err("%s(%d) supported send_sge=0x%x\n",
+			   __func__, dev->id, dev->attr.max_send_sge);
+		return -EINVAL;
+	}
+	if (attrs->cap.max_recv_sge > dev->attr.max_recv_sge) {
+		ocrdma_err("%s(%d) unsupported recv_sge=0x%x requested\n",
+			   __func__, dev->id, attrs->cap.max_recv_sge);
+		ocrdma_err("%s(%d) supported recv_sge=0x%x\n",
+			   __func__, dev->id, dev->attr.max_recv_sge);
+		return -EINVAL;
+	}
+	/* unprivileged user space cannot create special QP */
+	if (ibpd->uobject && attrs->qp_type == IB_QPT_GSI) {
+		ocrdma_err
+		    ("%s(%d) Userspace can't create special QPs of type=0x%x\n",
+		     __func__, dev->id, attrs->qp_type);
+		return -EINVAL;
+	}
+	/* allow creating only one GSI type of QP */
+	if (attrs->qp_type == IB_QPT_GSI && dev->gsi_qp_created) {
+		ocrdma_err("%s(%d) GSI special QPs already created.\n",
+			   __func__, dev->id);
+		return -EINVAL;
+	}
+	/* verify consumer QPs are not trying to use GSI QP's CQ */
+	if ((attrs->qp_type != IB_QPT_GSI) && (dev->gsi_qp_created)) {
+		if ((dev->gsi_sqcq == get_ocrdma_cq(attrs->send_cq)) ||
+		    (dev->gsi_sqcq == get_ocrdma_cq(attrs->send_cq))) {
+			ocrdma_err("%s(%d) Consumer QP cannot use GSI CQs.\n",
+				   __func__, dev->id);
+			return -EINVAL;
+		}
+	}
+	return 0;
+}
+
+static int ocrdma_copy_qp_uresp(struct ocrdma_qp *qp,
+				struct ib_udata *udata, int dpp_offset,
+				int dpp_credit_lmt, int srq)
+{
+	int status = 0;
+	u64 usr_db;
+	struct ocrdma_create_qp_uresp uresp;
+	struct ocrdma_dev *dev = qp->dev;
+	struct ocrdma_pd *pd = qp->pd;
+
+	memset(&uresp, 0, sizeof(uresp));
+	usr_db = dev->nic_info.unmapped_db +
+			(pd->id * dev->nic_info.db_page_size);
+	uresp.qp_id = qp->id;
+	uresp.sq_dbid = qp->sq.dbid;
+	uresp.num_sq_pages = 1;
+	uresp.sq_page_size = qp->sq.len;
+	uresp.sq_page_addr[0] = qp->sq.pa;
+	uresp.num_wqe_allocated = qp->sq.max_cnt;
+	if (!srq) {
+		uresp.rq_dbid = qp->rq.dbid;
+		uresp.num_rq_pages = 1;
+		uresp.rq_page_size = qp->rq.len;
+		uresp.rq_page_addr[0] = qp->rq.pa;
+		uresp.num_rqe_allocated = qp->rq.max_cnt;
+	}
+	uresp.db_page_addr = usr_db;
+	uresp.db_page_size = dev->nic_info.db_page_size;
+	if (dev->nic_info.dev_family == OCRDMA_GEN2_FAMILY) {
+		uresp.db_sq_offset = OCRDMA_DB_GEN2_SQ_OFFSET;
+		uresp.db_rq_offset = ((qp->id & 0xFFFF) < 128) ?
+			OCRDMA_DB_GEN2_RQ1_OFFSET : OCRDMA_DB_GEN2_RQ2_OFFSET;
+		uresp.db_shift = (qp->id < 128) ? 24 : 16;
+	} else {
+		uresp.db_sq_offset = OCRDMA_DB_SQ_OFFSET;
+		uresp.db_rq_offset = OCRDMA_DB_RQ_OFFSET;
+		uresp.db_shift = 16;
+	}
+	uresp.free_wqe_delta = qp->sq.free_delta;
+	uresp.free_rqe_delta = qp->rq.free_delta;
+
+	if (qp->dpp_enabled) {
+		uresp.dpp_credit = dpp_credit_lmt;
+		uresp.dpp_offset = dpp_offset;
+	}
+	status = ib_copy_to_udata(udata, &uresp, sizeof(uresp));
+	if (status) {
+		ocrdma_err("%s(%d) user copy error.\n", __func__, dev->id);
+		goto err;
+	}
+	status = ocrdma_add_mmap(pd->uctx, uresp.sq_page_addr[0],
+				 uresp.sq_page_size);
+	if (status)
+		goto err;
+
+	if (!srq) {
+		status = ocrdma_add_mmap(pd->uctx, uresp.rq_page_addr[0],
+					 uresp.rq_page_size);
+		if (status)
+			goto rq_map_err;
+	}
+	return status;
+rq_map_err:
+	ocrdma_del_mmap(pd->uctx, uresp.sq_page_addr[0], uresp.sq_page_size);
+err:
+	return status;
+}
+
+static void ocrdma_set_qp_db(struct ocrdma_dev *dev, struct ocrdma_qp *qp,
+			     struct ocrdma_pd *pd)
+{
+	if (dev->nic_info.dev_family == OCRDMA_GEN2_FAMILY) {
+		qp->sq_db = dev->nic_info.db +
+			(pd->id * dev->nic_info.db_page_size) +
+			OCRDMA_DB_GEN2_SQ_OFFSET;
+		qp->rq_db = dev->nic_info.db +
+			(pd->id * dev->nic_info.db_page_size) +
+			((qp->id < 128) ?
+			OCRDMA_DB_GEN2_RQ1_OFFSET : OCRDMA_DB_GEN2_RQ2_OFFSET);
+	} else {
+		qp->sq_db = dev->nic_info.db +
+			(pd->id * dev->nic_info.db_page_size) +
+			OCRDMA_DB_SQ_OFFSET;
+		qp->rq_db = dev->nic_info.db +
+			(pd->id * dev->nic_info.db_page_size) +
+			OCRDMA_DB_RQ_OFFSET;
+	}
+}
+
+static int ocrdma_alloc_wr_id_tbl(struct ocrdma_qp *qp)
+{
+	qp->wqe_wr_id_tbl =
+	    kzalloc(sizeof(*(qp->wqe_wr_id_tbl)) * qp->sq.max_cnt,
+		    GFP_KERNEL);
+	if (qp->wqe_wr_id_tbl == NULL)
+		return -ENOMEM;
+	qp->rqe_wr_id_tbl =
+	    kzalloc(sizeof(u64) * qp->rq.max_cnt, GFP_KERNEL);
+	if (qp->rqe_wr_id_tbl == NULL)
+		return -ENOMEM;
+
+	return 0;
+}
+
+static void ocrdma_set_qp_init_params(struct ocrdma_qp *qp,
+				      struct ocrdma_pd *pd,
+				      struct ib_qp_init_attr *attrs)
+{
+	qp->pd = pd;
+	spin_lock_init(&qp->q_lock);
+	INIT_LIST_HEAD(&qp->sq_entry);
+	INIT_LIST_HEAD(&qp->rq_entry);
+
+	qp->qp_type = attrs->qp_type;
+	qp->cap_flags = OCRDMA_QP_INB_RD | OCRDMA_QP_INB_WR;
+	qp->max_inline_data = attrs->cap.max_inline_data;
+	qp->sq.max_sges = attrs->cap.max_send_sge;
+	qp->rq.max_sges = attrs->cap.max_recv_sge;
+	qp->state = OCRDMA_QPS_RST;
+}
+
+static void ocrdma_set_qp_use_cnt(struct ocrdma_qp *qp, struct ocrdma_pd *pd)
+{
+	atomic_inc(&pd->use_cnt);
+	atomic_inc(&qp->sq_cq->use_cnt);
+	atomic_inc(&qp->rq_cq->use_cnt);
+	if (qp->srq)
+		atomic_inc(&qp->srq->use_cnt);
+	qp->ibqp.qp_num = qp->id;
+}
+
+static void ocrdma_store_gsi_qp_cq(struct ocrdma_dev *dev,
+				   struct ib_qp_init_attr *attrs)
+{
+	if (attrs->qp_type == IB_QPT_GSI) {
+		dev->gsi_qp_created = 1;
+		dev->gsi_sqcq = get_ocrdma_cq(attrs->send_cq);
+		dev->gsi_rqcq = get_ocrdma_cq(attrs->recv_cq);
+	}
+}
+
+struct ib_qp *ocrdma_create_qp(struct ib_pd *ibpd,
+			       struct ib_qp_init_attr *attrs,
+			       struct ib_udata *udata)
+{
+	int status;
+	struct ocrdma_pd *pd = get_ocrdma_pd(ibpd);
+	struct ocrdma_qp *qp;
+	struct ocrdma_dev *dev = pd->dev;
+	struct ocrdma_create_qp_ureq ureq;
+	u16 dpp_credit_lmt, dpp_offset;
+
+	status = ocrdma_check_qp_params(ibpd, dev, attrs);
+	if (status)
+		goto gen_err;
+
+	memset(&ureq, 0, sizeof(ureq));
+	if (udata) {
+		if (ib_copy_from_udata(&ureq, udata, sizeof(ureq)))
+			return ERR_PTR(-EFAULT);
+	}
+	qp = kzalloc(sizeof(*qp), GFP_KERNEL);
+	if (!qp) {
+		status = -ENOMEM;
+		goto gen_err;
+	}
+	qp->dev = dev;
+	ocrdma_set_qp_init_params(qp, pd, attrs);
+
+	mutex_lock(&dev->dev_lock);
+	status = ocrdma_mbx_create_qp(qp, attrs, ureq.enable_dpp_cq,
+					ureq.dpp_cq_id,
+					&dpp_offset, &dpp_credit_lmt);
+	if (status)
+		goto mbx_err;
+
+	/* user space QP's wr_id table are managed in library */
+	if (udata == NULL) {
+		qp->cap_flags |= (OCRDMA_QP_MW_BIND | OCRDMA_QP_LKEY0 |
+				  OCRDMA_QP_FAST_REG);
+		status = ocrdma_alloc_wr_id_tbl(qp);
+		if (status)
+			goto map_err;
+	}
+
+	status = ocrdma_add_qpn_map(dev, qp);
+	if (status)
+		goto map_err;
+	ocrdma_set_qp_db(dev, qp, pd);
+	if (udata) {
+		status = ocrdma_copy_qp_uresp(qp, udata, dpp_offset,
+					      dpp_credit_lmt,
+					      (attrs->srq != NULL));
+		if (status)
+			goto cpy_err;
+	}
+	ocrdma_store_gsi_qp_cq(dev, attrs);
+	ocrdma_set_qp_use_cnt(qp, pd);
+	mutex_unlock(&dev->dev_lock);
+	return &qp->ibqp;
+
+cpy_err:
+	ocrdma_del_qpn_map(dev, qp);
+map_err:
+	ocrdma_mbx_destroy_qp(dev, qp);
+mbx_err:
+	mutex_unlock(&dev->dev_lock);
+	kfree(qp->wqe_wr_id_tbl);
+	kfree(qp->rqe_wr_id_tbl);
+	kfree(qp);
+	ocrdma_err("%s(%d) error=%d\n", __func__, dev->id, status);
+gen_err:
+	return ERR_PTR(status);
+}
+
+int _ocrdma_modify_qp(struct ib_qp *ibqp, struct ib_qp_attr *attr,
+		      int attr_mask)
+{
+	int status = 0;
+	struct ocrdma_qp *qp;
+	struct ocrdma_dev *dev;
+	enum ib_qp_state old_qps;
+
+	qp = get_ocrdma_qp(ibqp);
+	dev = qp->dev;
+	if (attr_mask & IB_QP_STATE)
+		status = ocrdma_qp_state_machine(qp, attr->qp_state, &old_qps);
+	/* if new and previous states are same hw doesn't need to
+	 * know about it.
+	 */
+	if (status < 0)
+		return status;
+	status = ocrdma_mbx_modify_qp(dev, qp, attr, attr_mask, old_qps);
+	return status;
+}
+
+int ocrdma_modify_qp(struct ib_qp *ibqp, struct ib_qp_attr *attr,
+		     int attr_mask, struct ib_udata *udata)
+{
+	unsigned long flags;
+	int status = -EINVAL;
+	struct ocrdma_qp *qp;
+	struct ocrdma_dev *dev;
+	enum ib_qp_state old_qps, new_qps;
+
+	qp = get_ocrdma_qp(ibqp);
+	dev = qp->dev;
+
+	/* syncronize with multiple context trying to change, retrive qps */
+	mutex_lock(&dev->dev_lock);
+	/* syncronize with wqe, rqe posting and cqe processing contexts */
+	spin_lock_irqsave(&qp->q_lock, flags);
+	old_qps = get_ibqp_state(qp->state);
+	if (attr_mask & IB_QP_STATE)
+		new_qps = attr->qp_state;
+	else
+		new_qps = old_qps;
+	spin_unlock_irqrestore(&qp->q_lock, flags);
+
+	if (!ib_modify_qp_is_ok(old_qps, new_qps, ibqp->qp_type, attr_mask)) {
+		ocrdma_err("%s(%d) invalid attribute mask=0x%x specified for "
+			   "qpn=0x%x of type=0x%x old_qps=0x%x, new_qps=0x%x\n",
+			   __func__, dev->id, attr_mask, qp->id, ibqp->qp_type,
+			   old_qps, new_qps);
+		goto param_err;
+	}
+
+	status = _ocrdma_modify_qp(ibqp, attr, attr_mask);
+	if (status > 0)
+		status = 0;
+param_err:
+	mutex_unlock(&dev->dev_lock);
+	return status;
+}
+
+static enum ib_mtu ocrdma_mtu_int_to_enum(u16 mtu)
+{
+	switch (mtu) {
+	case 256:
+		return IB_MTU_256;
+	case 512:
+		return IB_MTU_512;
+	case 1024:
+		return IB_MTU_1024;
+	case 2048:
+		return IB_MTU_2048;
+	case 4096:
+		return IB_MTU_4096;
+	default:
+		return IB_MTU_1024;
+	}
+}
+
+static int ocrdma_to_ib_qp_acc_flags(int qp_cap_flags)
+{
+	int ib_qp_acc_flags = 0;
+
+	if (qp_cap_flags & OCRDMA_QP_INB_WR)
+		ib_qp_acc_flags |= IB_ACCESS_REMOTE_WRITE;
+	if (qp_cap_flags & OCRDMA_QP_INB_RD)
+		ib_qp_acc_flags |= IB_ACCESS_LOCAL_WRITE;
+	return ib_qp_acc_flags;
+}
+
+int ocrdma_query_qp(struct ib_qp *ibqp,
+		    struct ib_qp_attr *qp_attr,
+		    int attr_mask, struct ib_qp_init_attr *qp_init_attr)
+{
+	int status;
+	u32 qp_state;
+	struct ocrdma_qp_params params;
+	struct ocrdma_qp *qp = get_ocrdma_qp(ibqp);
+	struct ocrdma_dev *dev = qp->dev;
+
+	memset(&params, 0, sizeof(params));
+	mutex_lock(&dev->dev_lock);
+	status = ocrdma_mbx_query_qp(dev, qp, &params);
+	mutex_unlock(&dev->dev_lock);
+	if (status)
+		goto mbx_err;
+	qp_attr->qp_state = get_ibqp_state(IB_QPS_INIT);
+	qp_attr->cur_qp_state = get_ibqp_state(IB_QPS_INIT);
+	qp_attr->path_mtu =
+		ocrdma_mtu_int_to_enum(params.path_mtu_pkey_indx &
+				OCRDMA_QP_PARAMS_PATH_MTU_MASK) >>
+				OCRDMA_QP_PARAMS_PATH_MTU_SHIFT;
+	qp_attr->path_mig_state = IB_MIG_MIGRATED;
+	qp_attr->rq_psn = params.hop_lmt_rq_psn & OCRDMA_QP_PARAMS_RQ_PSN_MASK;
+	qp_attr->sq_psn = params.tclass_sq_psn & OCRDMA_QP_PARAMS_SQ_PSN_MASK;
+	qp_attr->dest_qp_num =
+	    params.ack_to_rnr_rtc_dest_qpn & OCRDMA_QP_PARAMS_DEST_QPN_MASK;
+
+	qp_attr->qp_access_flags = ocrdma_to_ib_qp_acc_flags(qp->cap_flags);
+	qp_attr->cap.max_send_wr = qp->sq.max_cnt - 1;
+	qp_attr->cap.max_recv_wr = qp->rq.max_cnt - 1;
+	qp_attr->cap.max_send_sge = qp->sq.max_sges;
+	qp_attr->cap.max_recv_sge = qp->rq.max_sges;
+	qp_attr->cap.max_inline_data = dev->attr.max_inline_data;
+	qp_init_attr->cap = qp_attr->cap;
+	memcpy(&qp_attr->ah_attr.grh.dgid, &params.dgid[0],
+	       sizeof(params.dgid));
+	qp_attr->ah_attr.grh.flow_label = params.rnt_rc_sl_fl &
+	    OCRDMA_QP_PARAMS_FLOW_LABEL_MASK;
+	qp_attr->ah_attr.grh.sgid_index = qp->sgid_idx;
+	qp_attr->ah_attr.grh.hop_limit = (params.hop_lmt_rq_psn &
+					  OCRDMA_QP_PARAMS_HOP_LMT_MASK) >>
+						OCRDMA_QP_PARAMS_HOP_LMT_SHIFT;
+	qp_attr->ah_attr.grh.traffic_class = (params.tclass_sq_psn &
+					      OCRDMA_QP_PARAMS_SQ_PSN_MASK) >>
+						OCRDMA_QP_PARAMS_TCLASS_SHIFT;
+
+	qp_attr->ah_attr.ah_flags = IB_AH_GRH;
+	qp_attr->ah_attr.port_num = 1;
+	qp_attr->ah_attr.sl = (params.rnt_rc_sl_fl &
+			       OCRDMA_QP_PARAMS_SL_MASK) >>
+				OCRDMA_QP_PARAMS_SL_SHIFT;
+	qp_attr->timeout = (params.ack_to_rnr_rtc_dest_qpn &
+			    OCRDMA_QP_PARAMS_ACK_TIMEOUT_MASK) >>
+				OCRDMA_QP_PARAMS_ACK_TIMEOUT_SHIFT;
+	qp_attr->rnr_retry = (params.ack_to_rnr_rtc_dest_qpn &
+			      OCRDMA_QP_PARAMS_RNR_RETRY_CNT_MASK) >>
+				OCRDMA_QP_PARAMS_RNR_RETRY_CNT_SHIFT;
+	qp_attr->retry_cnt =
+	    (params.rnt_rc_sl_fl & OCRDMA_QP_PARAMS_RETRY_CNT_MASK) >>
+		OCRDMA_QP_PARAMS_RETRY_CNT_SHIFT;
+	qp_attr->min_rnr_timer = 0;
+	qp_attr->pkey_index = 0;
+	qp_attr->port_num = 1;
+	qp_attr->ah_attr.src_path_bits = 0;
+	qp_attr->ah_attr.static_rate = 0;
+	qp_attr->alt_pkey_index = 0;
+	qp_attr->alt_port_num = 0;
+	qp_attr->alt_timeout = 0;
+	memset(&qp_attr->alt_ah_attr, 0, sizeof(qp_attr->alt_ah_attr));
+	qp_state = (params.max_sge_recv_flags & OCRDMA_QP_PARAMS_STATE_MASK) >>
+		    OCRDMA_QP_PARAMS_STATE_SHIFT;
+	qp_attr->sq_draining = (qp_state == OCRDMA_QPS_SQ_DRAINING) ? 1 : 0;
+	qp_attr->max_dest_rd_atomic =
+	    params.max_ord_ird >> OCRDMA_QP_PARAMS_MAX_ORD_SHIFT;
+	qp_attr->max_rd_atomic =
+	    params.max_ord_ird & OCRDMA_QP_PARAMS_MAX_IRD_MASK;
+	qp_attr->en_sqd_async_notify = (params.max_sge_recv_flags &
+				OCRDMA_QP_PARAMS_FLAGS_SQD_ASYNC) ? 1 : 0;
+mbx_err:
+	return status;
+}
+
+static void ocrdma_srq_toggle_bit(struct ocrdma_srq *srq, int idx)
+{
+	int i = idx / 32;
+	unsigned int mask = (1 << (idx % 32));
+
+	if (srq->idx_bit_fields[i] & mask)
+		srq->idx_bit_fields[i] &= ~mask;
+	else
+		srq->idx_bit_fields[i] |= mask;
+}
+
+static int ocrdma_hwq_free_cnt(struct ocrdma_qp_hwq_info *q)
+{
+	int free_cnt;
+	if (q->head >= q->tail)
+		free_cnt = (q->max_cnt - q->head) + q->tail;
+	else
+		free_cnt = q->tail - q->head;
+	if (q->free_delta)
+		free_cnt -= q->free_delta;
+	return free_cnt;
+}
+
+static int is_hw_sq_empty(struct ocrdma_qp *qp)
+{
+	return (qp->sq.tail == qp->sq.head &&
+		ocrdma_hwq_free_cnt(&qp->sq) ? 1 : 0);
+}
+
+static int is_hw_rq_empty(struct ocrdma_qp *qp)
+{
+	return (qp->rq.tail == qp->rq.head) ? 1 : 0;
+}
+
+static void *ocrdma_hwq_head(struct ocrdma_qp_hwq_info *q)
+{
+	return q->va + (q->head * q->entry_size);
+}
+
+static void *ocrdma_hwq_head_from_idx(struct ocrdma_qp_hwq_info *q,
+				      u32 idx)
+{
+	return q->va + (idx * q->entry_size);
+}
+
+static void ocrdma_hwq_inc_head(struct ocrdma_qp_hwq_info *q)
+{
+	q->head = (q->head + 1) & q->max_wqe_idx;
+}
+
+static void ocrdma_hwq_inc_tail(struct ocrdma_qp_hwq_info *q)
+{
+	q->tail = (q->tail + 1) & q->max_wqe_idx;
+}
+
+/* discard the cqe for a given QP */
+static void ocrdma_discard_cqes(struct ocrdma_qp *qp, struct ocrdma_cq *cq)
+{
+	unsigned long cq_flags;
+	unsigned long flags;
+	int discard_cnt = 0;
+	u32 cur_getp, stop_getp;
+	struct ocrdma_cqe *cqe;
+	u32 qpn = 0;
+
+	spin_lock_irqsave(&cq->cq_lock, cq_flags);
+
+	/* traverse through the CQEs in the hw CQ,
+	 * find the matching CQE for a given qp,
+	 * mark the matching one discarded by clearing qpn.
+	 * ring the doorbell in the poll_cq() as
+	 * we don't complete out of order cqe.
+	 */
+
+	cur_getp = cq->getp;
+	/* find upto when do we reap the cq. */
+	stop_getp = cur_getp;
+	do {
+		if (is_hw_sq_empty(qp) && (!qp->srq && is_hw_rq_empty(qp)))
+			break;
+
+		cqe = cq->va + cur_getp;
+		/* if (a) done reaping whole hw cq, or
+		 *    (b) qp_xq becomes empty.
+		 * then exit
+		 */
+		qpn = cqe->cmn.qpn & OCRDMA_CQE_QPN_MASK;
+		/* if previously discarded cqe found, skip that too. */
+		/* check for matching qp */
+		if (qpn == 0 || qpn != qp->id)
+			goto skip_cqe;
+
+		/* mark cqe discarded so that it is not picked up later
+		 * in the poll_cq().
+		 */
+		discard_cnt += 1;
+		cqe->cmn.qpn = 0;
+		if (is_cqe_for_sq(cqe))
+			ocrdma_hwq_inc_tail(&qp->sq);
+		else {
+			if (qp->srq) {
+				spin_lock_irqsave(&qp->srq->q_lock, flags);
+				ocrdma_hwq_inc_tail(&qp->srq->rq);
+				ocrdma_srq_toggle_bit(qp->srq, cur_getp);
+				spin_unlock_irqrestore(&qp->srq->q_lock, flags);
+
+			} else
+				ocrdma_hwq_inc_tail(&qp->rq);
+		}
+skip_cqe:
+		cur_getp = (cur_getp + 1) % cq->max_hw_cqe;
+	} while (cur_getp != stop_getp);
+	spin_unlock_irqrestore(&cq->cq_lock, cq_flags);
+}
+
+static void ocrdma_del_flush_qp(struct ocrdma_qp *qp)
+{
+	int found = false;
+	unsigned long flags;
+	struct ocrdma_dev *dev = qp->dev;
+	/* sync with any active CQ poll */
+
+	spin_lock_irqsave(&dev->flush_q_lock, flags);
+	found = ocrdma_is_qp_in_sq_flushlist(qp->sq_cq, qp);
+	if (found)
+		list_del(&qp->sq_entry);
+	if (!qp->srq) {
+		found = ocrdma_is_qp_in_rq_flushlist(qp->rq_cq, qp);
+		if (found)
+			list_del(&qp->rq_entry);
+	}
+	spin_unlock_irqrestore(&dev->flush_q_lock, flags);
+}
+
+int ocrdma_destroy_qp(struct ib_qp *ibqp)
+{
+	int status;
+	struct ocrdma_pd *pd;
+	struct ocrdma_qp *qp;
+	struct ocrdma_dev *dev;
+	struct ib_qp_attr attrs;
+	int attr_mask = IB_QP_STATE;
+	unsigned long flags;
+
+	qp = get_ocrdma_qp(ibqp);
+	dev = qp->dev;
+
+	attrs.qp_state = IB_QPS_ERR;
+	pd = qp->pd;
+
+	/* change the QP state to ERROR */
+	_ocrdma_modify_qp(ibqp, &attrs, attr_mask);
+
+	/* ensure that CQEs for newly created QP (whose id may be same with
+	 * one which just getting destroyed are same), dont get
+	 * discarded until the old CQEs are discarded.
+	 */
+	mutex_lock(&dev->dev_lock);
+	status = ocrdma_mbx_destroy_qp(dev, qp);
+
+	/*
+	 * acquire CQ lock while destroy is in progress, in order to
+	 * protect against proessing in-flight CQEs for this QP.
+	 */
+	spin_lock_irqsave(&qp->sq_cq->cq_lock, flags);
+	if (qp->rq_cq && (qp->rq_cq != qp->sq_cq))
+		spin_lock(&qp->rq_cq->cq_lock);
+
+	ocrdma_del_qpn_map(dev, qp);
+
+	if (qp->rq_cq && (qp->rq_cq != qp->sq_cq))
+		spin_unlock(&qp->rq_cq->cq_lock);
+	spin_unlock_irqrestore(&qp->sq_cq->cq_lock, flags);
+
+	if (!pd->uctx) {
+		ocrdma_discard_cqes(qp, qp->sq_cq);
+		ocrdma_discard_cqes(qp, qp->rq_cq);
+	}
+	mutex_unlock(&dev->dev_lock);
+
+	if (pd->uctx) {
+		ocrdma_del_mmap(pd->uctx, (u64) qp->sq.pa, qp->sq.len);
+		if (!qp->srq)
+			ocrdma_del_mmap(pd->uctx, (u64) qp->rq.pa, qp->rq.len);
+	}
+
+	ocrdma_del_flush_qp(qp);
+
+	atomic_dec(&qp->pd->use_cnt);
+	atomic_dec(&qp->sq_cq->use_cnt);
+	atomic_dec(&qp->rq_cq->use_cnt);
+	if (qp->srq)
+		atomic_dec(&qp->srq->use_cnt);
+	kfree(qp->wqe_wr_id_tbl);
+	kfree(qp->rqe_wr_id_tbl);
+	kfree(qp);
+	return status;
+}
+
+static int ocrdma_copy_srq_uresp(struct ocrdma_srq *srq, struct ib_udata *udata)
+{
+	int status;
+	struct ocrdma_create_srq_uresp uresp;
+
+	uresp.rq_dbid = srq->rq.dbid;
+	uresp.num_rq_pages = 1;
+	uresp.rq_page_addr[0] = srq->rq.pa;
+	uresp.rq_page_size = srq->rq.len;
+	uresp.db_page_addr = srq->dev->nic_info.unmapped_db +
+	    (srq->pd->id * srq->dev->nic_info.db_page_size);
+	uresp.db_page_size = srq->dev->nic_info.db_page_size;
+	uresp.num_rqe_allocated = srq->rq.max_cnt;
+	uresp.free_rqe_delta = 1;
+	if (srq->dev->nic_info.dev_family == OCRDMA_GEN2_FAMILY) {
+		uresp.db_rq_offset = OCRDMA_DB_GEN2_RQ1_OFFSET;
+		uresp.db_shift = 24;
+	} else {
+		uresp.db_rq_offset = OCRDMA_DB_RQ_OFFSET;
+		uresp.db_shift = 16;
+	}
+
+	status = ib_copy_to_udata(udata, &uresp, sizeof(uresp));
+	if (status)
+		return status;
+	status = ocrdma_add_mmap(srq->pd->uctx, uresp.rq_page_addr[0],
+				 uresp.rq_page_size);
+	if (status)
+		return status;
+	return status;
+}
+
+struct ib_srq *ocrdma_create_srq(struct ib_pd *ibpd,
+				 struct ib_srq_init_attr *init_attr,
+				 struct ib_udata *udata)
+{
+	int status = -ENOMEM;
+	struct ocrdma_pd *pd = get_ocrdma_pd(ibpd);
+	struct ocrdma_dev *dev = pd->dev;
+	struct ocrdma_srq *srq;
+
+	if (init_attr->attr.max_sge > dev->attr.max_recv_sge)
+		return ERR_PTR(-EINVAL);
+	if (init_attr->attr.max_wr > dev->attr.max_rqe)
+		return ERR_PTR(-EINVAL);
+
+	srq = kzalloc(sizeof(*srq), GFP_KERNEL);
+	if (!srq)
+		return ERR_PTR(status);
+
+	spin_lock_init(&srq->q_lock);
+	srq->dev = dev;
+	srq->pd = pd;
+	srq->db = dev->nic_info.db + (pd->id * dev->nic_info.db_page_size);
+	status = ocrdma_mbx_create_srq(srq, init_attr, pd);
+	if (status)
+		goto err;
+
+	if (udata == NULL) {
+		srq->rqe_wr_id_tbl = kzalloc(sizeof(u64) * srq->rq.max_cnt,
+			    GFP_KERNEL);
+		if (srq->rqe_wr_id_tbl == NULL)
+			goto arm_err;
+
+		srq->bit_fields_len = (srq->rq.max_cnt / 32) +
+		    (srq->rq.max_cnt % 32 ? 1 : 0);
+		srq->idx_bit_fields =
+		    kmalloc(srq->bit_fields_len * sizeof(u32), GFP_KERNEL);
+		if (srq->idx_bit_fields == NULL)
+			goto arm_err;
+		memset(srq->idx_bit_fields, 0xff,
+		       srq->bit_fields_len * sizeof(u32));
+	}
+
+	if (init_attr->attr.srq_limit) {
+		status = ocrdma_mbx_modify_srq(srq, &init_attr->attr);
+		if (status)
+			goto arm_err;
+	}
+
+	atomic_set(&srq->use_cnt, 0);
+	if (udata) {
+		status = ocrdma_copy_srq_uresp(srq, udata);
+		if (status)
+			goto arm_err;
+	}
+
+	atomic_inc(&pd->use_cnt);
+	return &srq->ibsrq;
+
+arm_err:
+	ocrdma_mbx_destroy_srq(dev, srq);
+err:
+	kfree(srq->rqe_wr_id_tbl);
+	kfree(srq->idx_bit_fields);
+	kfree(srq);
+	return ERR_PTR(status);
+}
+
+int ocrdma_modify_srq(struct ib_srq *ibsrq,
+		      struct ib_srq_attr *srq_attr,
+		      enum ib_srq_attr_mask srq_attr_mask,
+		      struct ib_udata *udata)
+{
+	int status = 0;
+	struct ocrdma_srq *srq;
+
+	srq = get_ocrdma_srq(ibsrq);
+	if (srq_attr_mask & IB_SRQ_MAX_WR)
+		status = -EINVAL;
+	else
+		status = ocrdma_mbx_modify_srq(srq, srq_attr);
+	return status;
+}
+
+int ocrdma_query_srq(struct ib_srq *ibsrq, struct ib_srq_attr *srq_attr)
+{
+	int status;
+	struct ocrdma_srq *srq;
+
+	srq = get_ocrdma_srq(ibsrq);
+	status = ocrdma_mbx_query_srq(srq, srq_attr);
+	return status;
+}
+
+int ocrdma_destroy_srq(struct ib_srq *ibsrq)
+{
+	int status;
+	struct ocrdma_srq *srq;
+	struct ocrdma_dev *dev;
+
+	srq = get_ocrdma_srq(ibsrq);
+	dev = srq->dev;
+	if (atomic_read(&srq->use_cnt)) {
+		ocrdma_err("%s(%d) err, srq=0x%x in use\n",
+			   __func__, dev->id, srq->id);
+		return -EAGAIN;
+	}
+
+	status = ocrdma_mbx_destroy_srq(dev, srq);
+
+	if (srq->pd->uctx)
+		ocrdma_del_mmap(srq->pd->uctx, (u64) srq->rq.pa, srq->rq.len);
+
+	atomic_dec(&srq->pd->use_cnt);
+	kfree(srq->idx_bit_fields);
+	kfree(srq->rqe_wr_id_tbl);
+	kfree(srq);
+	return status;
+}
+
+/* unprivileged verbs and their support functions. */
+static void ocrdma_build_ud_hdr(struct ocrdma_qp *qp,
+				struct ocrdma_hdr_wqe *hdr,
+				struct ib_send_wr *wr)
+{
+	struct ocrdma_ewqe_ud_hdr *ud_hdr =
+		(struct ocrdma_ewqe_ud_hdr *)(hdr + 1);
+	struct ocrdma_ah *ah = get_ocrdma_ah(wr->wr.ud.ah);
+
+	ud_hdr->rsvd_dest_qpn = wr->wr.ud.remote_qpn;
+	if (qp->qp_type == IB_QPT_GSI)
+		ud_hdr->qkey = qp->qkey;
+	else
+		ud_hdr->qkey = wr->wr.ud.remote_qkey;
+	ud_hdr->rsvd_ahid = ah->id;
+}
+
+static void ocrdma_build_sges(struct ocrdma_hdr_wqe *hdr,
+			      struct ocrdma_sge *sge, int num_sge,
+			      struct ib_sge *sg_list)
+{
+	int i;
+
+	for (i = 0; i < num_sge; i++) {
+		sge[i].lrkey = sg_list[i].lkey;
+		sge[i].addr_lo = sg_list[i].addr;
+		sge[i].addr_hi = upper_32_bits(sg_list[i].addr);
+		sge[i].len = sg_list[i].length;
+		hdr->total_len += sg_list[i].length;
+	}
+	if (num_sge == 0)
+		memset(sge, 0, sizeof(*sge));
+}
+
+static int ocrdma_build_inline_sges(struct ocrdma_qp *qp,
+				    struct ocrdma_hdr_wqe *hdr,
+				    struct ocrdma_sge *sge,
+				    struct ib_send_wr *wr, u32 wqe_size)
+{
+	if (wr->send_flags & IB_SEND_INLINE) {
+		if (wr->sg_list[0].length > qp->max_inline_data) {
+			ocrdma_err("%s() supported_len=0x%x,"
+				" unspported len req=0x%x\n", __func__,
+				qp->max_inline_data, wr->sg_list[0].length);
+			return -EINVAL;
+		}
+		memcpy(sge,
+		       (void *)(unsigned long)wr->sg_list[0].addr,
+		       wr->sg_list[0].length);
+		hdr->total_len = wr->sg_list[0].length;
+		wqe_size += roundup(hdr->total_len, OCRDMA_WQE_ALIGN_BYTES);
+		hdr->cw |= (OCRDMA_TYPE_INLINE << OCRDMA_WQE_TYPE_SHIFT);
+	} else {
+		ocrdma_build_sges(hdr, sge, wr->num_sge, wr->sg_list);
+		if (wr->num_sge)
+			wqe_size += (wr->num_sge * sizeof(struct ocrdma_sge));
+		else
+			wqe_size += sizeof(struct ocrdma_sge);
+		hdr->cw |= (OCRDMA_TYPE_LKEY << OCRDMA_WQE_TYPE_SHIFT);
+	}
+	hdr->cw |= ((wqe_size / OCRDMA_WQE_STRIDE) << OCRDMA_WQE_SIZE_SHIFT);
+	return 0;
+}
+
+static int ocrdma_build_send(struct ocrdma_qp *qp, struct ocrdma_hdr_wqe *hdr,
+			     struct ib_send_wr *wr)
+{
+	int status;
+	struct ocrdma_sge *sge;
+	u32 wqe_size = sizeof(*hdr);
+
+	if (qp->qp_type == IB_QPT_UD || qp->qp_type == IB_QPT_GSI) {
+		ocrdma_build_ud_hdr(qp, hdr, wr);
+		sge = (struct ocrdma_sge *)(hdr + 2);
+		wqe_size += sizeof(struct ocrdma_ewqe_ud_hdr);
+	} else
+		sge = (struct ocrdma_sge *)(hdr + 1);
+
+	status = ocrdma_build_inline_sges(qp, hdr, sge, wr, wqe_size);
+	return status;
+}
+
+static int ocrdma_build_write(struct ocrdma_qp *qp, struct ocrdma_hdr_wqe *hdr,
+			      struct ib_send_wr *wr)
+{
+	int status;
+	struct ocrdma_sge *ext_rw = (struct ocrdma_sge *)(hdr + 1);
+	struct ocrdma_sge *sge = ext_rw + 1;
+	u32 wqe_size = sizeof(*hdr) + sizeof(*ext_rw);
+
+	status = ocrdma_build_inline_sges(qp, hdr, sge, wr, wqe_size);
+	if (status)
+		return status;
+	ext_rw->addr_lo = wr->wr.rdma.remote_addr;
+	ext_rw->addr_hi = upper_32_bits(wr->wr.rdma.remote_addr);
+	ext_rw->lrkey = wr->wr.rdma.rkey;
+	ext_rw->len = hdr->total_len;
+	return 0;
+}
+
+static void ocrdma_build_read(struct ocrdma_qp *qp, struct ocrdma_hdr_wqe *hdr,
+			      struct ib_send_wr *wr)
+{
+	struct ocrdma_sge *ext_rw = (struct ocrdma_sge *)(hdr + 1);
+	struct ocrdma_sge *sge = ext_rw + 1;
+	u32 wqe_size = ((wr->num_sge + 1) * sizeof(struct ocrdma_sge)) +
+	    sizeof(struct ocrdma_hdr_wqe);
+
+	ocrdma_build_sges(hdr, sge, wr->num_sge, wr->sg_list);
+	hdr->cw |= ((wqe_size / OCRDMA_WQE_STRIDE) << OCRDMA_WQE_SIZE_SHIFT);
+	hdr->cw |= (OCRDMA_READ << OCRDMA_WQE_OPCODE_SHIFT);
+	hdr->cw |= (OCRDMA_TYPE_LKEY << OCRDMA_WQE_TYPE_SHIFT);
+
+	ext_rw->addr_lo = wr->wr.rdma.remote_addr;
+	ext_rw->addr_hi = upper_32_bits(wr->wr.rdma.remote_addr);
+	ext_rw->lrkey = wr->wr.rdma.rkey;
+	ext_rw->len = hdr->total_len;
+}
+
+static void ocrdma_ring_sq_db(struct ocrdma_qp *qp)
+{
+	u32 val = qp->sq.dbid | (1 << 16);
+
+	iowrite32(val, qp->sq_db);
+}
+
+int ocrdma_post_send(struct ib_qp *ibqp, struct ib_send_wr *wr,
+		     struct ib_send_wr **bad_wr)
+{
+	int status = 0;
+	struct ocrdma_qp *qp = get_ocrdma_qp(ibqp);
+	struct ocrdma_hdr_wqe *hdr;
+	unsigned long flags;
+
+	spin_lock_irqsave(&qp->q_lock, flags);
+	if (qp->state != OCRDMA_QPS_RTS && qp->state != OCRDMA_QPS_SQD) {
+		spin_unlock_irqrestore(&qp->q_lock, flags);
+		return -EINVAL;
+	}
+
+	while (wr) {
+		if (ocrdma_hwq_free_cnt(&qp->sq) == 0 ||
+		    wr->num_sge > qp->sq.max_sges) {
+			status = -ENOMEM;
+			break;
+		}
+		hdr = ocrdma_hwq_head(&qp->sq);
+		hdr->cw = 0;
+		if (wr->send_flags & IB_SEND_SIGNALED)
+			hdr->cw |= (OCRDMA_FLAG_SIG << OCRDMA_WQE_FLAGS_SHIFT);
+		if (wr->send_flags & IB_SEND_FENCE)
+			hdr->cw |=
+			    (OCRDMA_FLAG_FENCE_L << OCRDMA_WQE_FLAGS_SHIFT);
+		if (wr->send_flags & IB_SEND_SOLICITED)
+			hdr->cw |=
+			    (OCRDMA_FLAG_SOLICIT << OCRDMA_WQE_FLAGS_SHIFT);
+		hdr->total_len = 0;
+		switch (wr->opcode) {
+		case IB_WR_SEND_WITH_IMM:
+			hdr->cw |= (OCRDMA_FLAG_IMM << OCRDMA_WQE_FLAGS_SHIFT);
+			hdr->immdt = ntohl(wr->ex.imm_data);
+		case IB_WR_SEND:
+			hdr->cw |= (OCRDMA_SEND << OCRDMA_WQE_OPCODE_SHIFT);
+			ocrdma_build_send(qp, hdr, wr);
+			break;
+		case IB_WR_SEND_WITH_INV:
+			hdr->cw |= (OCRDMA_FLAG_INV << OCRDMA_WQE_FLAGS_SHIFT);
+			hdr->cw |= (OCRDMA_SEND << OCRDMA_WQE_OPCODE_SHIFT);
+			hdr->lkey = wr->ex.invalidate_rkey;
+			status = ocrdma_build_send(qp, hdr, wr);
+			break;
+		case IB_WR_RDMA_WRITE_WITH_IMM:
+			hdr->cw |= (OCRDMA_FLAG_IMM << OCRDMA_WQE_FLAGS_SHIFT);
+			hdr->immdt = ntohl(wr->ex.imm_data);
+		case IB_WR_RDMA_WRITE:
+			hdr->cw |= (OCRDMA_WRITE << OCRDMA_WQE_OPCODE_SHIFT);
+			status = ocrdma_build_write(qp, hdr, wr);
+			break;
+		case IB_WR_RDMA_READ_WITH_INV:
+			hdr->cw |= (OCRDMA_FLAG_INV << OCRDMA_WQE_FLAGS_SHIFT);
+		case IB_WR_RDMA_READ:
+			ocrdma_build_read(qp, hdr, wr);
+			break;
+		case IB_WR_LOCAL_INV:
+			hdr->cw |=
+			    (OCRDMA_LKEY_INV << OCRDMA_WQE_OPCODE_SHIFT);
+			hdr->cw |= (sizeof(struct ocrdma_hdr_wqe) /
+				OCRDMA_WQE_STRIDE) << OCRDMA_WQE_SIZE_SHIFT;
+			hdr->lkey = wr->ex.invalidate_rkey;
+			break;
+		default:
+			status = -EINVAL;
+			break;
+		}
+		if (status) {
+			*bad_wr = wr;
+			break;
+		}
+		if (wr->send_flags & IB_SEND_SIGNALED)
+			qp->wqe_wr_id_tbl[qp->sq.head].signaled = 1;
+		else
+			qp->wqe_wr_id_tbl[qp->sq.head].signaled = 0;
+		qp->wqe_wr_id_tbl[qp->sq.head].wrid = wr->wr_id;
+		ocrdma_cpu_to_le32(hdr, ((hdr->cw >> OCRDMA_WQE_SIZE_SHIFT) &
+				   OCRDMA_WQE_SIZE_MASK) * OCRDMA_WQE_STRIDE);
+		/* make sure wqe is written before adapter can access it */
+		wmb();
+		/* inform hw to start processing it */
+		ocrdma_ring_sq_db(qp);
+
+		/* update pointer, counter for next wr */
+		ocrdma_hwq_inc_head(&qp->sq);
+		wr = wr->next;
+	}
+	spin_unlock_irqrestore(&qp->q_lock, flags);
+	return status;
+}
+
+static void ocrdma_ring_rq_db(struct ocrdma_qp *qp)
+{
+	u32 val = qp->rq.dbid | (1 << OCRDMA_GET_NUM_POSTED_SHIFT_VAL(qp));
+
+	iowrite32(val, qp->rq_db);
+}
+
+static void ocrdma_build_rqe(struct ocrdma_hdr_wqe *rqe, struct ib_recv_wr *wr,
+			     u16 tag)
+{
+	u32 wqe_size = 0;
+	struct ocrdma_sge *sge;
+	if (wr->num_sge)
+		wqe_size = (wr->num_sge * sizeof(*sge)) + sizeof(*rqe);
+	else
+		wqe_size = sizeof(*sge) + sizeof(*rqe);
+
+	rqe->cw = ((wqe_size / OCRDMA_WQE_STRIDE) <<
+				OCRDMA_WQE_SIZE_SHIFT);
+	rqe->cw |= (OCRDMA_FLAG_SIG << OCRDMA_WQE_FLAGS_SHIFT);
+	rqe->cw |= (OCRDMA_TYPE_LKEY << OCRDMA_WQE_TYPE_SHIFT);
+	rqe->total_len = 0;
+	rqe->rsvd_tag = tag;
+	sge = (struct ocrdma_sge *)(rqe + 1);
+	ocrdma_build_sges(rqe, sge, wr->num_sge, wr->sg_list);
+	ocrdma_cpu_to_le32(rqe, wqe_size);
+}
+
+int ocrdma_post_recv(struct ib_qp *ibqp, struct ib_recv_wr *wr,
+		     struct ib_recv_wr **bad_wr)
+{
+	int status = 0;
+	unsigned long flags;
+	struct ocrdma_qp *qp = get_ocrdma_qp(ibqp);
+	struct ocrdma_hdr_wqe *rqe;
+
+	spin_lock_irqsave(&qp->q_lock, flags);
+	if (qp->state == OCRDMA_QPS_RST || qp->state == OCRDMA_QPS_ERR) {
+		spin_unlock_irqrestore(&qp->q_lock, flags);
+		*bad_wr = wr;
+		return -EINVAL;
+	}
+	while (wr) {
+		if (ocrdma_hwq_free_cnt(&qp->rq) == 0 ||
+		    wr->num_sge > qp->rq.max_sges) {
+			*bad_wr = wr;
+			status = -ENOMEM;
+			break;
+		}
+		rqe = ocrdma_hwq_head(&qp->rq);
+		ocrdma_build_rqe(rqe, wr, 0);
+
+		qp->rqe_wr_id_tbl[qp->rq.head] = wr->wr_id;
+		/* make sure rqe is written before adapter can access it */
+		wmb();
+
+		/* inform hw to start processing it */
+		ocrdma_ring_rq_db(qp);
+
+		/* update pointer, counter for next wr */
+		ocrdma_hwq_inc_head(&qp->rq);
+		wr = wr->next;
+	}
+	spin_unlock_irqrestore(&qp->q_lock, flags);
+	return status;
+}
+
+/* cqe for srq's rqe can potentially arrive out of order.
+ * index gives the entry in the shadow table where to store
+ * the wr_id. tag/index is returned in cqe to reference back
+ * for a given rqe.
+ */
+static int ocrdma_srq_get_idx(struct ocrdma_srq *srq)
+{
+	int row = 0;
+	int indx = 0;
+
+	for (row = 0; row < srq->bit_fields_len; row++) {
+		if (srq->idx_bit_fields[row]) {
+			indx = ffs(srq->idx_bit_fields[row]);
+			indx = (row * 32) + (indx - 1);
+			if (indx >= srq->rq.max_cnt)
+				BUG();
+			ocrdma_srq_toggle_bit(srq, indx);
+			break;
+		}
+	}
+
+	if (row == srq->bit_fields_len)
+		BUG();
+	return indx;
+}
+
+static void ocrdma_ring_srq_db(struct ocrdma_srq *srq)
+{
+	u32 val = srq->rq.dbid | (1 << 16);
+
+	iowrite32(val, srq->db + OCRDMA_DB_GEN2_SRQ_OFFSET);
+}
+
+int ocrdma_post_srq_recv(struct ib_srq *ibsrq, struct ib_recv_wr *wr,
+			 struct ib_recv_wr **bad_wr)
+{
+	int status = 0;
+	unsigned long flags;
+	struct ocrdma_srq *srq;
+	struct ocrdma_hdr_wqe *rqe;
+	u16 tag;
+
+	srq = get_ocrdma_srq(ibsrq);
+
+	spin_lock_irqsave(&srq->q_lock, flags);
+	while (wr) {
+		if (ocrdma_hwq_free_cnt(&srq->rq) == 0 ||
+		    wr->num_sge > srq->rq.max_sges) {
+			status = -ENOMEM;
+			*bad_wr = wr;
+			break;
+		}
+		tag = ocrdma_srq_get_idx(srq);
+		rqe = ocrdma_hwq_head(&srq->rq);
+		ocrdma_build_rqe(rqe, wr, tag);
+
+		srq->rqe_wr_id_tbl[tag] = wr->wr_id;
+		/* make sure rqe is written before adapter can perform DMA */
+		wmb();
+		/* inform hw to start processing it */
+		ocrdma_ring_srq_db(srq);
+		/* update pointer, counter for next wr */
+		ocrdma_hwq_inc_head(&srq->rq);
+		wr = wr->next;
+	}
+	spin_unlock_irqrestore(&srq->q_lock, flags);
+	return status;
+}
+
+static enum ib_wc_status ocrdma_to_ibwc_err(u16 status)
+{
+	enum ib_wc_status ibwc_status = IB_WC_GENERAL_ERR;
+
+	switch (status) {
+	case OCRDMA_CQE_GENERAL_ERR:
+		ibwc_status = IB_WC_GENERAL_ERR;
+		break;
+	case OCRDMA_CQE_LOC_LEN_ERR:
+		ibwc_status = IB_WC_LOC_LEN_ERR;
+		break;
+	case OCRDMA_CQE_LOC_QP_OP_ERR:
+		ibwc_status = IB_WC_LOC_QP_OP_ERR;
+		break;
+	case OCRDMA_CQE_LOC_EEC_OP_ERR:
+		ibwc_status = IB_WC_LOC_EEC_OP_ERR;
+		break;
+	case OCRDMA_CQE_LOC_PROT_ERR:
+		ibwc_status = IB_WC_LOC_PROT_ERR;
+		break;
+	case OCRDMA_CQE_WR_FLUSH_ERR:
+		ibwc_status = IB_WC_WR_FLUSH_ERR;
+		break;
+	case OCRDMA_CQE_MW_BIND_ERR:
+		ibwc_status = IB_WC_MW_BIND_ERR;
+		break;
+	case OCRDMA_CQE_BAD_RESP_ERR:
+		ibwc_status = IB_WC_BAD_RESP_ERR;
+		break;
+	case OCRDMA_CQE_LOC_ACCESS_ERR:
+		ibwc_status = IB_WC_LOC_ACCESS_ERR;
+		break;
+	case OCRDMA_CQE_REM_INV_REQ_ERR:
+		ibwc_status = IB_WC_REM_INV_REQ_ERR;
+		break;
+	case OCRDMA_CQE_REM_ACCESS_ERR:
+		ibwc_status = IB_WC_REM_ACCESS_ERR;
+		break;
+	case OCRDMA_CQE_REM_OP_ERR:
+		ibwc_status = IB_WC_REM_OP_ERR;
+		break;
+	case OCRDMA_CQE_RETRY_EXC_ERR:
+		ibwc_status = IB_WC_RETRY_EXC_ERR;
+		break;
+	case OCRDMA_CQE_RNR_RETRY_EXC_ERR:
+		ibwc_status = IB_WC_RNR_RETRY_EXC_ERR;
+		break;
+	case OCRDMA_CQE_LOC_RDD_VIOL_ERR:
+		ibwc_status = IB_WC_LOC_RDD_VIOL_ERR;
+		break;
+	case OCRDMA_CQE_REM_INV_RD_REQ_ERR:
+		ibwc_status = IB_WC_REM_INV_RD_REQ_ERR;
+		break;
+	case OCRDMA_CQE_REM_ABORT_ERR:
+		ibwc_status = IB_WC_REM_ABORT_ERR;
+		break;
+	case OCRDMA_CQE_INV_EECN_ERR:
+		ibwc_status = IB_WC_INV_EECN_ERR;
+		break;
+	case OCRDMA_CQE_INV_EEC_STATE_ERR:
+		ibwc_status = IB_WC_INV_EEC_STATE_ERR;
+		break;
+	case OCRDMA_CQE_FATAL_ERR:
+		ibwc_status = IB_WC_FATAL_ERR;
+		break;
+	case OCRDMA_CQE_RESP_TIMEOUT_ERR:
+		ibwc_status = IB_WC_RESP_TIMEOUT_ERR;
+		break;
+	default:
+		ibwc_status = IB_WC_GENERAL_ERR;
+		break;
+	};
+	return ibwc_status;
+}
+
+static void ocrdma_update_wc(struct ocrdma_qp *qp, struct ib_wc *ibwc,
+		      u32 wqe_idx)
+{
+	struct ocrdma_hdr_wqe *hdr;
+	struct ocrdma_sge *rw;
+	int opcode;
+
+	hdr = ocrdma_hwq_head_from_idx(&qp->sq, wqe_idx);
+
+	ibwc->wr_id = qp->wqe_wr_id_tbl[wqe_idx].wrid;
+	/* Undo the hdr->cw swap */
+	opcode = le32_to_cpu(hdr->cw) & OCRDMA_WQE_OPCODE_MASK;
+	switch (opcode) {
+	case OCRDMA_WRITE:
+		ibwc->opcode = IB_WC_RDMA_WRITE;
+		break;
+	case OCRDMA_READ:
+		rw = (struct ocrdma_sge *)(hdr + 1);
+		ibwc->opcode = IB_WC_RDMA_READ;
+		ibwc->byte_len = rw->len;
+		break;
+	case OCRDMA_SEND:
+		ibwc->opcode = IB_WC_SEND;
+		break;
+	case OCRDMA_LKEY_INV:
+		ibwc->opcode = IB_WC_LOCAL_INV;
+		break;
+	default:
+		ibwc->status = IB_WC_GENERAL_ERR;
+		ocrdma_err("%s() invalid opcode received = 0x%x\n",
+			   __func__, hdr->cw & OCRDMA_WQE_OPCODE_MASK);
+		break;
+	};
+}
+
+static void ocrdma_set_cqe_status_flushed(struct ocrdma_qp *qp,
+						struct ocrdma_cqe *cqe)
+{
+	if (is_cqe_for_sq(cqe)) {
+		cqe->flags_status_srcqpn = cpu_to_le32(le32_to_cpu(
+				cqe->flags_status_srcqpn) &
+					~OCRDMA_CQE_STATUS_MASK);
+		cqe->flags_status_srcqpn = cpu_to_le32(le32_to_cpu(
+				cqe->flags_status_srcqpn) |
+				(OCRDMA_CQE_WR_FLUSH_ERR <<
+					OCRDMA_CQE_STATUS_SHIFT));
+	} else {
+		if (qp->qp_type == IB_QPT_UD || qp->qp_type == IB_QPT_GSI) {
+			cqe->flags_status_srcqpn = cpu_to_le32(le32_to_cpu(
+					cqe->flags_status_srcqpn) &
+						~OCRDMA_CQE_UD_STATUS_MASK);
+			cqe->flags_status_srcqpn = cpu_to_le32(le32_to_cpu(
+					cqe->flags_status_srcqpn) |
+					(OCRDMA_CQE_WR_FLUSH_ERR <<
+						OCRDMA_CQE_UD_STATUS_SHIFT));
+		} else {
+			cqe->flags_status_srcqpn = cpu_to_le32(le32_to_cpu(
+					cqe->flags_status_srcqpn) &
+						~OCRDMA_CQE_STATUS_MASK);
+			cqe->flags_status_srcqpn = cpu_to_le32(le32_to_cpu(
+					cqe->flags_status_srcqpn) |
+					(OCRDMA_CQE_WR_FLUSH_ERR <<
+						OCRDMA_CQE_STATUS_SHIFT));
+		}
+	}
+}
+
+static bool ocrdma_update_err_cqe(struct ib_wc *ibwc, struct ocrdma_cqe *cqe,
+				  struct ocrdma_qp *qp, int status)
+{
+	bool expand = false;
+
+	ibwc->byte_len = 0;
+	ibwc->qp = &qp->ibqp;
+	ibwc->status = ocrdma_to_ibwc_err(status);
+
+	ocrdma_flush_qp(qp);
+	ocrdma_qp_state_machine(qp, IB_QPS_ERR, NULL);
+
+	/* if wqe/rqe pending for which cqe needs to be returned,
+	 * trigger inflating it.
+	 */
+	if (!is_hw_rq_empty(qp) || !is_hw_sq_empty(qp)) {
+		expand = true;
+		ocrdma_set_cqe_status_flushed(qp, cqe);
+	}
+	return expand;
+}
+
+static int ocrdma_update_err_rcqe(struct ib_wc *ibwc, struct ocrdma_cqe *cqe,
+				  struct ocrdma_qp *qp, int status)
+{
+	ibwc->opcode = IB_WC_RECV;
+	ibwc->wr_id = qp->rqe_wr_id_tbl[qp->rq.tail];
+	ocrdma_hwq_inc_tail(&qp->rq);
+
+	return ocrdma_update_err_cqe(ibwc, cqe, qp, status);
+}
+
+static int ocrdma_update_err_scqe(struct ib_wc *ibwc, struct ocrdma_cqe *cqe,
+				  struct ocrdma_qp *qp, int status)
+{
+	ocrdma_update_wc(qp, ibwc, qp->sq.tail);
+	ocrdma_hwq_inc_tail(&qp->sq);
+
+	return ocrdma_update_err_cqe(ibwc, cqe, qp, status);
+}
+
+
+static bool ocrdma_poll_err_scqe(struct ocrdma_qp *qp,
+				 struct ocrdma_cqe *cqe, struct ib_wc *ibwc,
+				 bool *polled, bool *stop)
+{
+	bool expand;
+	int status = (le32_to_cpu(cqe->flags_status_srcqpn) &
+		OCRDMA_CQE_STATUS_MASK) >> OCRDMA_CQE_STATUS_SHIFT;
+
+	/* when hw sq is empty, but rq is not empty, so we continue
+	 * to keep the cqe in order to get the cq event again.
+	 */
+	if (is_hw_sq_empty(qp) && !is_hw_rq_empty(qp)) {
+		/* when cq for rq and sq is same, it is safe to return
+		 * flush cqe for RQEs.
+		 */
+		if (!qp->srq && (qp->sq_cq == qp->rq_cq)) {
+			*polled = true;
+			status = OCRDMA_CQE_WR_FLUSH_ERR;
+			expand = ocrdma_update_err_rcqe(ibwc, cqe, qp, status);
+		} else {
+			/* stop processing further cqe as this cqe is used for
+			 * triggering cq event on buddy cq of RQ.
+			 * When QP is destroyed, this cqe will be removed
+			 * from the cq's hardware q.
+			 */
+			*polled = false;
+			*stop = true;
+			expand = false;
+		}
+	} else {
+		*polled = true;
+		expand = ocrdma_update_err_scqe(ibwc, cqe, qp, status);
+	}
+	return expand;
+}
+
+static bool ocrdma_poll_success_scqe(struct ocrdma_qp *qp,
+				     struct ocrdma_cqe *cqe,
+				     struct ib_wc *ibwc, bool *polled)
+{
+	bool expand = false;
+	int tail = qp->sq.tail;
+	u32 wqe_idx;
+
+	if (!qp->wqe_wr_id_tbl[tail].signaled) {
+		expand = true;	/* CQE cannot be consumed yet */
+		*polled = false;    /* WC cannot be consumed yet */
+	} else {
+		ibwc->status = IB_WC_SUCCESS;
+		ibwc->wc_flags = 0;
+		ibwc->qp = &qp->ibqp;
+		ocrdma_update_wc(qp, ibwc, tail);
+		*polled = true;
+		wqe_idx = le32_to_cpu(cqe->wq.wqeidx) &	OCRDMA_CQE_WQEIDX_MASK;
+		if (tail != wqe_idx)
+			expand = true; /* Coalesced CQE can't be consumed yet */
+	}
+	ocrdma_hwq_inc_tail(&qp->sq);
+	return expand;
+}
+
+static bool ocrdma_poll_scqe(struct ocrdma_qp *qp, struct ocrdma_cqe *cqe,
+			     struct ib_wc *ibwc, bool *polled, bool *stop)
+{
+	int status;
+	bool expand;
+
+	status = (le32_to_cpu(cqe->flags_status_srcqpn) &
+		OCRDMA_CQE_STATUS_MASK) >> OCRDMA_CQE_STATUS_SHIFT;
+
+	if (status == OCRDMA_CQE_SUCCESS)
+		expand = ocrdma_poll_success_scqe(qp, cqe, ibwc, polled);
+	else
+		expand = ocrdma_poll_err_scqe(qp, cqe, ibwc, polled, stop);
+	return expand;
+}
+
+static int ocrdma_update_ud_rcqe(struct ib_wc *ibwc, struct ocrdma_cqe *cqe)
+{
+	int status;
+
+	status = (le32_to_cpu(cqe->flags_status_srcqpn) &
+		OCRDMA_CQE_UD_STATUS_MASK) >> OCRDMA_CQE_UD_STATUS_SHIFT;
+	ibwc->src_qp = le32_to_cpu(cqe->flags_status_srcqpn) &
+						OCRDMA_CQE_SRCQP_MASK;
+	ibwc->pkey_index = le32_to_cpu(cqe->ud.rxlen_pkey) &
+						OCRDMA_CQE_PKEY_MASK;
+	ibwc->wc_flags = IB_WC_GRH;
+	ibwc->byte_len = (le32_to_cpu(cqe->ud.rxlen_pkey) >>
+					OCRDMA_CQE_UD_XFER_LEN_SHIFT);
+	return status;
+}
+
+static void ocrdma_update_free_srq_cqe(struct ib_wc *ibwc,
+				       struct ocrdma_cqe *cqe,
+				       struct ocrdma_qp *qp)
+{
+	unsigned long flags;
+	struct ocrdma_srq *srq;
+	u32 wqe_idx;
+
+	srq = get_ocrdma_srq(qp->ibqp.srq);
+	wqe_idx = le32_to_cpu(cqe->rq.buftag_qpn) >> OCRDMA_CQE_BUFTAG_SHIFT;
+	ibwc->wr_id = srq->rqe_wr_id_tbl[wqe_idx];
+	spin_lock_irqsave(&srq->q_lock, flags);
+	ocrdma_srq_toggle_bit(srq, wqe_idx);
+	spin_unlock_irqrestore(&srq->q_lock, flags);
+	ocrdma_hwq_inc_tail(&srq->rq);
+}
+
+static bool ocrdma_poll_err_rcqe(struct ocrdma_qp *qp, struct ocrdma_cqe *cqe,
+				struct ib_wc *ibwc, bool *polled, bool *stop,
+				int status)
+{
+	bool expand;
+
+	/* when hw_rq is empty, but wq is not empty, so continue
+	 * to keep the cqe to get the cq event again.
+	 */
+	if (is_hw_rq_empty(qp) && !is_hw_sq_empty(qp)) {
+		if (!qp->srq && (qp->sq_cq == qp->rq_cq)) {
+			*polled = true;
+			status = OCRDMA_CQE_WR_FLUSH_ERR;
+			expand = ocrdma_update_err_scqe(ibwc, cqe, qp, status);
+		} else {
+			*polled = false;
+			*stop = true;
+			expand = false;
+		}
+	} else
+		expand = ocrdma_update_err_rcqe(ibwc, cqe, qp, status);
+	return expand;
+}
+
+static void ocrdma_poll_success_rcqe(struct ocrdma_qp *qp,
+				     struct ocrdma_cqe *cqe, struct ib_wc *ibwc)
+{
+	ibwc->opcode = IB_WC_RECV;
+	ibwc->qp = &qp->ibqp;
+	ibwc->status = IB_WC_SUCCESS;
+
+	if (qp->qp_type == IB_QPT_UD || qp->qp_type == IB_QPT_GSI)
+		ocrdma_update_ud_rcqe(ibwc, cqe);
+	else
+		ibwc->byte_len = le32_to_cpu(cqe->rq.rxlen);
+
+	if (is_cqe_imm(cqe)) {
+		ibwc->ex.imm_data = htonl(le32_to_cpu(cqe->rq.lkey_immdt));
+		ibwc->wc_flags |= IB_WC_WITH_IMM;
+	} else if (is_cqe_wr_imm(cqe)) {
+		ibwc->opcode = IB_WC_RECV_RDMA_WITH_IMM;
+		ibwc->ex.imm_data = htonl(le32_to_cpu(cqe->rq.lkey_immdt));
+		ibwc->wc_flags |= IB_WC_WITH_IMM;
+	} else if (is_cqe_invalidated(cqe)) {
+		ibwc->ex.invalidate_rkey = le32_to_cpu(cqe->rq.lkey_immdt);
+		ibwc->wc_flags |= IB_WC_WITH_INVALIDATE;
+	}
+	if (qp->ibqp.srq)
+		ocrdma_update_free_srq_cqe(ibwc, cqe, qp);
+	else {
+		ibwc->wr_id = qp->rqe_wr_id_tbl[qp->rq.tail];
+		ocrdma_hwq_inc_tail(&qp->rq);
+	}
+}
+
+static bool ocrdma_poll_rcqe(struct ocrdma_qp *qp, struct ocrdma_cqe *cqe,
+			     struct ib_wc *ibwc, bool *polled, bool *stop)
+{
+	int status;
+	bool expand = false;
+
+	ibwc->wc_flags = 0;
+	if (qp->qp_type == IB_QPT_UD || qp->qp_type == IB_QPT_GSI)
+		status = (le32_to_cpu(cqe->flags_status_srcqpn) &
+					OCRDMA_CQE_UD_STATUS_MASK) >>
+					OCRDMA_CQE_UD_STATUS_SHIFT;
+	else
+		status = (le32_to_cpu(cqe->flags_status_srcqpn) &
+			     OCRDMA_CQE_STATUS_MASK) >> OCRDMA_CQE_STATUS_SHIFT;
+
+	if (status == OCRDMA_CQE_SUCCESS) {
+		*polled = true;
+		ocrdma_poll_success_rcqe(qp, cqe, ibwc);
+	} else {
+		expand = ocrdma_poll_err_rcqe(qp, cqe, ibwc, polled, stop,
+					      status);
+	}
+	return expand;
+}
+
+static void ocrdma_change_cq_phase(struct ocrdma_cq *cq, struct ocrdma_cqe *cqe,
+				   u16 cur_getp)
+{
+	if (cq->phase_change) {
+		if (cur_getp == 0)
+			cq->phase = (~cq->phase & OCRDMA_CQE_VALID);
+	} else
+		/* clear valid bit */
+		cqe->flags_status_srcqpn = 0;
+}
+
+static int ocrdma_poll_hwcq(struct ocrdma_cq *cq, int num_entries,
+			    struct ib_wc *ibwc)
+{
+	u16 qpn = 0;
+	int i = 0;
+	bool expand = false;
+	int polled_hw_cqes = 0;
+	struct ocrdma_qp *qp = NULL;
+	struct ocrdma_dev *dev = cq->dev;
+	struct ocrdma_cqe *cqe;
+	u16 cur_getp; bool polled = false; bool stop = false;
+
+	cur_getp = cq->getp;
+	while (num_entries) {
+		cqe = cq->va + cur_getp;
+		/* check whether valid cqe or not */
+		if (!is_cqe_valid(cq, cqe))
+			break;
+		qpn = (le32_to_cpu(cqe->cmn.qpn) & OCRDMA_CQE_QPN_MASK);
+		/* ignore discarded cqe */
+		if (qpn == 0)
+			goto skip_cqe;
+		qp = dev->qp_tbl[qpn];
+		BUG_ON(qp == NULL);
+
+		if (is_cqe_for_sq(cqe)) {
+			expand = ocrdma_poll_scqe(qp, cqe, ibwc, &polled,
+						  &stop);
+		} else {
+			expand = ocrdma_poll_rcqe(qp, cqe, ibwc, &polled,
+						  &stop);
+		}
+		if (expand)
+			goto expand_cqe;
+		if (stop)
+			goto stop_cqe;
+		/* clear qpn to avoid duplicate processing by discard_cqe() */
+		cqe->cmn.qpn = 0;
+skip_cqe:
+		polled_hw_cqes += 1;
+		cur_getp = (cur_getp + 1) % cq->max_hw_cqe;
+		ocrdma_change_cq_phase(cq, cqe, cur_getp);
+expand_cqe:
+		if (polled) {
+			num_entries -= 1;
+			i += 1;
+			ibwc = ibwc + 1;
+			polled = false;
+		}
+	}
+stop_cqe:
+	cq->getp = cur_getp;
+	if (polled_hw_cqes || expand || stop) {
+		ocrdma_ring_cq_db(dev, cq->id, cq->armed, cq->solicited,
+				  polled_hw_cqes);
+	}
+	return i;
+}
+
+/* insert error cqe if the QP's SQ or RQ's CQ matches the CQ under poll. */
+static int ocrdma_add_err_cqe(struct ocrdma_cq *cq, int num_entries,
+			      struct ocrdma_qp *qp, struct ib_wc *ibwc)
+{
+	int err_cqes = 0;
+
+	while (num_entries) {
+		if (is_hw_sq_empty(qp) && is_hw_rq_empty(qp))
+			break;
+		if (!is_hw_sq_empty(qp) && qp->sq_cq == cq) {
+			ocrdma_update_wc(qp, ibwc, qp->sq.tail);
+			ocrdma_hwq_inc_tail(&qp->sq);
+		} else if (!is_hw_rq_empty(qp) && qp->rq_cq == cq) {
+			ibwc->wr_id = qp->rqe_wr_id_tbl[qp->rq.tail];
+			ocrdma_hwq_inc_tail(&qp->rq);
+		} else
+			return err_cqes;
+		ibwc->byte_len = 0;
+		ibwc->status = IB_WC_WR_FLUSH_ERR;
+		ibwc = ibwc + 1;
+		err_cqes += 1;
+		num_entries -= 1;
+	}
+	return err_cqes;
+}
+
+int ocrdma_poll_cq(struct ib_cq *ibcq, int num_entries, struct ib_wc *wc)
+{
+	int cqes_to_poll = num_entries;
+	struct ocrdma_cq *cq = NULL;
+	unsigned long flags;
+	struct ocrdma_dev *dev;
+	int num_os_cqe = 0, err_cqes = 0;
+	struct ocrdma_qp *qp;
+
+	cq = get_ocrdma_cq(ibcq);
+	dev = cq->dev;
+
+	/* poll cqes from adapter CQ */
+	spin_lock_irqsave(&cq->cq_lock, flags);
+	num_os_cqe = ocrdma_poll_hwcq(cq, cqes_to_poll, wc);
+	spin_unlock_irqrestore(&cq->cq_lock, flags);
+	cqes_to_poll -= num_os_cqe;
+
+	if (cqes_to_poll) {
+		wc = wc + num_os_cqe;
+		/* adapter returns single error cqe when qp moves to
+		 * error state. So insert error cqes with wc_status as
+		 * FLUSHED for pending WQEs and RQEs of QP's SQ and RQ
+		 * respectively which uses this CQ.
+		 */
+		spin_lock_irqsave(&dev->flush_q_lock, flags);
+		list_for_each_entry(qp, &cq->sq_head, sq_entry) {
+			if (cqes_to_poll == 0)
+				break;
+			err_cqes = ocrdma_add_err_cqe(cq, cqes_to_poll, qp, wc);
+			cqes_to_poll -= err_cqes;
+			num_os_cqe += err_cqes;
+			wc = wc + err_cqes;
+		}
+		spin_unlock_irqrestore(&dev->flush_q_lock, flags);
+	}
+	return num_os_cqe;
+}
+
+int ocrdma_arm_cq(struct ib_cq *ibcq, enum ib_cq_notify_flags cq_flags)
+{
+	struct ocrdma_cq *cq;
+	unsigned long flags;
+	struct ocrdma_dev *dev;
+	u16 cq_id;
+	u16 cur_getp;
+	struct ocrdma_cqe *cqe;
+
+	cq = get_ocrdma_cq(ibcq);
+	cq_id = cq->id;
+	dev = cq->dev;
+
+	spin_lock_irqsave(&cq->cq_lock, flags);
+	if (cq_flags & IB_CQ_NEXT_COMP || cq_flags & IB_CQ_SOLICITED)
+		cq->armed = true;
+	if (cq_flags & IB_CQ_SOLICITED)
+		cq->solicited = true;
+
+	cur_getp = cq->getp;
+	cqe = cq->va + cur_getp;
+
+	/* check whether any valid cqe exist or not, if not then safe to
+	 * arm. If cqe is not yet consumed, then let it get consumed and then
+	 * we arm it to avoid false interrupts.
+	 */
+	if (!is_cqe_valid(cq, cqe) || cq->arm_needed) {
+		cq->arm_needed = false;
+		ocrdma_ring_cq_db(dev, cq_id, cq->armed, cq->solicited, 0);
+	}
+	spin_unlock_irqrestore(&cq->cq_lock, flags);
+	return 0;
+}
diff --git a/drivers/infiniband/hw/ocrdma/ocrdma_verbs.h b/drivers/infiniband/hw/ocrdma/ocrdma_verbs.h
new file mode 100644
index 00000000000..e6483439f25
--- /dev/null
+++ b/drivers/infiniband/hw/ocrdma/ocrdma_verbs.h
@@ -0,0 +1,94 @@
+/*******************************************************************
+ * This file is part of the Emulex RoCE Device Driver for          *
+ * RoCE (RDMA over Converged Ethernet) adapters.                   *
+ * Copyright (C) 2008-2012 Emulex. All rights reserved.            *
+ * EMULEX and SLI are trademarks of Emulex.                        *
+ * www.emulex.com                                                  *
+ *                                                                 *
+ * 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. *
+ * ALL EXPRESS OR IMPLIED CONDITIONS, REPRESENTATIONS AND          *
+ * WARRANTIES, INCLUDING ANY IMPLIED WARRANTY OF MERCHANTABILITY,  *
+ * FITNESS FOR A PARTICULAR PURPOSE, OR NON-INFRINGEMENT, ARE      *
+ * DISCLAIMED, EXCEPT TO THE EXTENT THAT SUCH DISCLAIMERS ARE HELD *
+ * TO BE LEGALLY INVALID.  See the GNU General Public License for  *
+ * more details, a copy of which can be found in the file COPYING  *
+ * included with this package.                                     *
+ *
+ * Contact Information:
+ * linux-drivers@emulex.com
+ *
+ * Emulex
+ * 3333 Susan Street
+ * Costa Mesa, CA 92626
+ *******************************************************************/
+
+#ifndef __OCRDMA_VERBS_H__
+#define __OCRDMA_VERBS_H__
+
+#include <linux/version.h>
+int ocrdma_post_send(struct ib_qp *, struct ib_send_wr *,
+		     struct ib_send_wr **bad_wr);
+int ocrdma_post_recv(struct ib_qp *, struct ib_recv_wr *,
+		     struct ib_recv_wr **bad_wr);
+
+int ocrdma_poll_cq(struct ib_cq *, int num_entries, struct ib_wc *wc);
+int ocrdma_arm_cq(struct ib_cq *, enum ib_cq_notify_flags flags);
+
+int ocrdma_query_device(struct ib_device *, struct ib_device_attr *props);
+int ocrdma_query_port(struct ib_device *, u8 port, struct ib_port_attr *props);
+int ocrdma_modify_port(struct ib_device *, u8 port, int mask,
+		       struct ib_port_modify *props);
+
+void ocrdma_get_guid(struct ocrdma_dev *, u8 *guid);
+int ocrdma_query_gid(struct ib_device *, u8 port,
+		     int index, union ib_gid *gid);
+int ocrdma_query_pkey(struct ib_device *, u8 port, u16 index, u16 *pkey);
+
+struct ib_ucontext *ocrdma_alloc_ucontext(struct ib_device *,
+					  struct ib_udata *);
+int ocrdma_dealloc_ucontext(struct ib_ucontext *);
+
+int ocrdma_mmap(struct ib_ucontext *, struct vm_area_struct *vma);
+
+struct ib_pd *ocrdma_alloc_pd(struct ib_device *,
+			      struct ib_ucontext *, struct ib_udata *);
+int ocrdma_dealloc_pd(struct ib_pd *pd);
+
+struct ib_cq *ocrdma_create_cq(struct ib_device *, int entries, int vector,
+			       struct ib_ucontext *, struct ib_udata *);
+int ocrdma_resize_cq(struct ib_cq *, int cqe, struct ib_udata *);
+int ocrdma_destroy_cq(struct ib_cq *);
+
+struct ib_qp *ocrdma_create_qp(struct ib_pd *,
+			       struct ib_qp_init_attr *attrs,
+			       struct ib_udata *);
+int _ocrdma_modify_qp(struct ib_qp *, struct ib_qp_attr *attr,
+		      int attr_mask);
+int ocrdma_modify_qp(struct ib_qp *, struct ib_qp_attr *attr,
+		     int attr_mask, struct ib_udata *udata);
+int ocrdma_query_qp(struct ib_qp *,
+		    struct ib_qp_attr *qp_attr,
+		    int qp_attr_mask, struct ib_qp_init_attr *);
+int ocrdma_destroy_qp(struct ib_qp *);
+
+struct ib_srq *ocrdma_create_srq(struct ib_pd *, struct ib_srq_init_attr *,
+				 struct ib_udata *);
+int ocrdma_modify_srq(struct ib_srq *, struct ib_srq_attr *,
+		      enum ib_srq_attr_mask, struct ib_udata *);
+int ocrdma_query_srq(struct ib_srq *, struct ib_srq_attr *);
+int ocrdma_destroy_srq(struct ib_srq *);
+int ocrdma_post_srq_recv(struct ib_srq *, struct ib_recv_wr *,
+			 struct ib_recv_wr **bad_recv_wr);
+
+int ocrdma_dereg_mr(struct ib_mr *);
+struct ib_mr *ocrdma_get_dma_mr(struct ib_pd *, int acc);
+struct ib_mr *ocrdma_reg_kernel_mr(struct ib_pd *,
+				   struct ib_phys_buf *buffer_list,
+				   int num_phys_buf, int acc, u64 *iova_start);
+struct ib_mr *ocrdma_reg_user_mr(struct ib_pd *, u64 start, u64 length,
+				 u64 virt, int acc, struct ib_udata *);
+
+#endif				/* __OCRDMA_VERBS_H__ */
diff --git a/drivers/infiniband/hw/qib/qib.h b/drivers/infiniband/hw/qib/qib.h
index 6b811e3e8bd..7e62f413714 100644
--- a/drivers/infiniband/hw/qib/qib.h
+++ b/drivers/infiniband/hw/qib/qib.h
@@ -530,8 +530,6 @@ struct qib_pportdata {
 	/* qib_lflags driver is waiting for */
 	u32 state_wanted;
 	spinlock_t lflags_lock;
-	/* number of (port-specific) interrupts for this port -- saturates... */
-	u32 int_counter;
 
 	/* ref count for each pkey */
 	atomic_t pkeyrefs[4];
@@ -543,24 +541,26 @@ struct qib_pportdata {
 	u64 *statusp;
 
 	/* SendDMA related entries */
-	spinlock_t            sdma_lock;
-	struct qib_sdma_state sdma_state;
-	unsigned long         sdma_buf_jiffies;
+
+	/* read mostly */
 	struct qib_sdma_desc *sdma_descq;
+	struct qib_sdma_state sdma_state;
+	dma_addr_t       sdma_descq_phys;
+	volatile __le64 *sdma_head_dma; /* DMA'ed by chip */
+	dma_addr_t       sdma_head_phys;
+	u16                   sdma_descq_cnt;
+
+	/* read/write using lock */
+	spinlock_t            sdma_lock ____cacheline_aligned_in_smp;
+	struct list_head      sdma_activelist;
 	u64                   sdma_descq_added;
 	u64                   sdma_descq_removed;
-	u16                   sdma_descq_cnt;
 	u16                   sdma_descq_tail;
 	u16                   sdma_descq_head;
-	u16                   sdma_next_intr;
-	u16                   sdma_reset_wait;
 	u8                    sdma_generation;
-	struct tasklet_struct sdma_sw_clean_up_task;
-	struct list_head      sdma_activelist;
 
-	dma_addr_t       sdma_descq_phys;
-	volatile __le64 *sdma_head_dma; /* DMA'ed by chip */
-	dma_addr_t       sdma_head_phys;
+	struct tasklet_struct sdma_sw_clean_up_task
+		____cacheline_aligned_in_smp;
 
 	wait_queue_head_t state_wait; /* for state_wanted */
 
@@ -873,7 +873,14 @@ struct qib_devdata {
 	 * pio_writing.
 	 */
 	spinlock_t pioavail_lock;
-
+	/*
+	 * index of last buffer to optimize search for next
+	 */
+	u32 last_pio;
+	/*
+	 * min kernel pio buffer to optimize search
+	 */
+	u32 min_kernel_pio;
 	/*
 	 * Shadow copies of registers; size indicates read access size.
 	 * Most of them are readonly, but some are write-only register,
diff --git a/drivers/infiniband/hw/qib/qib_driver.c b/drivers/infiniband/hw/qib/qib_driver.c
index 6fc9365ba8a..8895cfec501 100644
--- a/drivers/infiniband/hw/qib/qib_driver.c
+++ b/drivers/infiniband/hw/qib/qib_driver.c
@@ -38,6 +38,7 @@
 #include <linux/netdevice.h>
 #include <linux/vmalloc.h>
 #include <linux/module.h>
+#include <linux/prefetch.h>
 
 #include "qib.h"
 
@@ -481,8 +482,10 @@ u32 qib_kreceive(struct qib_ctxtdata *rcd, u32 *llic, u32 *npkts)
 			etail = qib_hdrget_index(rhf_addr);
 			updegr = 1;
 			if (tlen > sizeof(*hdr) ||
-			    etype >= RCVHQ_RCV_TYPE_NON_KD)
+			    etype >= RCVHQ_RCV_TYPE_NON_KD) {
 				ebuf = qib_get_egrbuf(rcd, etail);
+				prefetch_range(ebuf, tlen - sizeof(*hdr));
+			}
 		}
 		if (!eflags) {
 			u16 lrh_len = be16_to_cpu(hdr->lrh[2]) << 2;
diff --git a/drivers/infiniband/hw/qib/qib_iba6120.c b/drivers/infiniband/hw/qib/qib_iba6120.c
index d0c64d51481..4d352b90750 100644
--- a/drivers/infiniband/hw/qib/qib_iba6120.c
+++ b/drivers/infiniband/hw/qib/qib_iba6120.c
@@ -3132,6 +3132,7 @@ static void get_6120_chip_params(struct qib_devdata *dd)
 	val = qib_read_kreg64(dd, kr_sendpiobufcnt);
 	dd->piobcnt2k = val & ~0U;
 	dd->piobcnt4k = val >> 32;
+	dd->last_pio = dd->piobcnt4k + dd->piobcnt2k - 1;
 	/* these may be adjusted in init_chip_wc_pat() */
 	dd->pio2kbase = (u32 __iomem *)
 		(((char __iomem *)dd->kregbase) + dd->pio2k_bufbase);
diff --git a/drivers/infiniband/hw/qib/qib_iba7220.c b/drivers/infiniband/hw/qib/qib_iba7220.c
index 3c722f79d6f..86a0ba7ca0c 100644
--- a/drivers/infiniband/hw/qib/qib_iba7220.c
+++ b/drivers/infiniband/hw/qib/qib_iba7220.c
@@ -4157,6 +4157,7 @@ static int qib_init_7220_variables(struct qib_devdata *dd)
 		dd->cspec->sdmabufcnt;
 	dd->lastctxt_piobuf = dd->cspec->lastbuf_for_pio - sbufs;
 	dd->cspec->lastbuf_for_pio--; /* range is <= , not < */
+	dd->last_pio = dd->cspec->lastbuf_for_pio;
 	dd->pbufsctxt = dd->lastctxt_piobuf /
 		(dd->cfgctxts - dd->first_user_ctxt);
 
diff --git a/drivers/infiniband/hw/qib/qib_iba7322.c b/drivers/infiniband/hw/qib/qib_iba7322.c
index 060b9606446..c881e744c09 100644
--- a/drivers/infiniband/hw/qib/qib_iba7322.c
+++ b/drivers/infiniband/hw/qib/qib_iba7322.c
@@ -6379,6 +6379,7 @@ static int qib_init_7322_variables(struct qib_devdata *dd)
 		dd->cspec->sdmabufcnt;
 	dd->lastctxt_piobuf = dd->cspec->lastbuf_for_pio - sbufs;
 	dd->cspec->lastbuf_for_pio--; /* range is <= , not < */
+	dd->last_pio = dd->cspec->lastbuf_for_pio;
 	dd->pbufsctxt = (dd->cfgctxts > dd->first_user_ctxt) ?
 		dd->lastctxt_piobuf / (dd->cfgctxts - dd->first_user_ctxt) : 0;
 
@@ -7708,7 +7709,7 @@ static int serdes_7322_init_new(struct qib_pportdata *ppd)
 	ibsd_wr_allchans(ppd, 5, 0, BMASK(0, 0));
 	msleep(20);
 	/*       Set Frequency Loop Bandwidth */
-	ibsd_wr_allchans(ppd, 2, (7 << 5), BMASK(8, 5));
+	ibsd_wr_allchans(ppd, 2, (15 << 5), BMASK(8, 5));
 	/*       Enable Frequency Loop */
 	ibsd_wr_allchans(ppd, 2, (1 << 4), BMASK(4, 4));
 	/*       Set Timing Loop Bandwidth */
diff --git a/drivers/infiniband/hw/qib/qib_init.c b/drivers/infiniband/hw/qib/qib_init.c
index cf0cd30adc8..dc14e100a7f 100644
--- a/drivers/infiniband/hw/qib/qib_init.c
+++ b/drivers/infiniband/hw/qib/qib_init.c
@@ -102,6 +102,8 @@ void qib_set_ctxtcnt(struct qib_devdata *dd)
 		dd->cfgctxts = qib_cfgctxts;
 	else
 		dd->cfgctxts = dd->ctxtcnt;
+	dd->freectxts = (dd->first_user_ctxt > dd->cfgctxts) ? 0 :
+		dd->cfgctxts - dd->first_user_ctxt;
 }
 
 /*
@@ -402,7 +404,6 @@ static void enable_chip(struct qib_devdata *dd)
 		if (rcd)
 			dd->f_rcvctrl(rcd->ppd, rcvmask, i);
 	}
-	dd->freectxts = dd->cfgctxts - dd->first_user_ctxt;
 }
 
 static void verify_interrupt(unsigned long opaque)
diff --git a/drivers/infiniband/hw/qib/qib_mad.c b/drivers/infiniband/hw/qib/qib_mad.c
index c4ff788823b..43390217a02 100644
--- a/drivers/infiniband/hw/qib/qib_mad.c
+++ b/drivers/infiniband/hw/qib/qib_mad.c
@@ -396,6 +396,7 @@ static int get_linkdowndefaultstate(struct qib_pportdata *ppd)
 
 static int check_mkey(struct qib_ibport *ibp, struct ib_smp *smp, int mad_flags)
 {
+	int valid_mkey = 0;
 	int ret = 0;
 
 	/* Is the mkey in the process of expiring? */
@@ -406,23 +407,36 @@ static int check_mkey(struct qib_ibport *ibp, struct ib_smp *smp, int mad_flags)
 		ibp->mkeyprot = 0;
 	}
 
-	/* M_Key checking depends on Portinfo:M_Key_protect_bits */
-	if ((mad_flags & IB_MAD_IGNORE_MKEY) == 0 && ibp->mkey != 0 &&
-	    ibp->mkey != smp->mkey &&
-	    (smp->method == IB_MGMT_METHOD_SET ||
-	     smp->method == IB_MGMT_METHOD_TRAP_REPRESS ||
-	     (smp->method == IB_MGMT_METHOD_GET && ibp->mkeyprot >= 2))) {
-		if (ibp->mkey_violations != 0xFFFF)
-			++ibp->mkey_violations;
-		if (!ibp->mkey_lease_timeout && ibp->mkey_lease_period)
-			ibp->mkey_lease_timeout = jiffies +
-				ibp->mkey_lease_period * HZ;
-		/* Generate a trap notice. */
-		qib_bad_mkey(ibp, smp);
-		ret = IB_MAD_RESULT_SUCCESS | IB_MAD_RESULT_CONSUMED;
-	} else if (ibp->mkey_lease_timeout)
+	if ((mad_flags & IB_MAD_IGNORE_MKEY) ||  ibp->mkey == 0 ||
+	    ibp->mkey == smp->mkey)
+		valid_mkey = 1;
+
+	/* Unset lease timeout on any valid Get/Set/TrapRepress */
+	if (valid_mkey && ibp->mkey_lease_timeout &&
+	    (smp->method == IB_MGMT_METHOD_GET ||
+	     smp->method == IB_MGMT_METHOD_SET ||
+	     smp->method == IB_MGMT_METHOD_TRAP_REPRESS))
 		ibp->mkey_lease_timeout = 0;
 
+	if (!valid_mkey) {
+		switch (smp->method) {
+		case IB_MGMT_METHOD_GET:
+			/* Bad mkey not a violation below level 2 */
+			if (ibp->mkeyprot < 2)
+				break;
+		case IB_MGMT_METHOD_SET:
+		case IB_MGMT_METHOD_TRAP_REPRESS:
+			if (ibp->mkey_violations != 0xFFFF)
+				++ibp->mkey_violations;
+			if (!ibp->mkey_lease_timeout && ibp->mkey_lease_period)
+				ibp->mkey_lease_timeout = jiffies +
+					ibp->mkey_lease_period * HZ;
+			/* Generate a trap notice. */
+			qib_bad_mkey(ibp, smp);
+			ret = 1;
+		}
+	}
+
 	return ret;
 }
 
@@ -450,6 +464,7 @@ static int subn_get_portinfo(struct ib_smp *smp, struct ib_device *ibdev,
 			ibp = to_iport(ibdev, port_num);
 			ret = check_mkey(ibp, smp, 0);
 			if (ret)
+				ret = IB_MAD_RESULT_FAILURE;
 				goto bail;
 		}
 	}
@@ -631,7 +646,7 @@ static int subn_set_portinfo(struct ib_smp *smp, struct ib_device *ibdev,
 	struct qib_devdata *dd;
 	struct qib_pportdata *ppd;
 	struct qib_ibport *ibp;
-	char clientrereg = 0;
+	u8 clientrereg = (pip->clientrereg_resv_subnetto & 0x80);
 	unsigned long flags;
 	u16 lid, smlid;
 	u8 lwe;
@@ -781,12 +796,6 @@ static int subn_set_portinfo(struct ib_smp *smp, struct ib_device *ibdev,
 
 	ibp->subnet_timeout = pip->clientrereg_resv_subnetto & 0x1F;
 
-	if (pip->clientrereg_resv_subnetto & 0x80) {
-		clientrereg = 1;
-		event.event = IB_EVENT_CLIENT_REREGISTER;
-		ib_dispatch_event(&event);
-	}
-
 	/*
 	 * Do the port state change now that the other link parameters
 	 * have been set.
@@ -844,10 +853,15 @@ static int subn_set_portinfo(struct ib_smp *smp, struct ib_device *ibdev,
 		smp->status |= IB_SMP_INVALID_FIELD;
 	}
 
+	if (clientrereg) {
+		event.event = IB_EVENT_CLIENT_REREGISTER;
+		ib_dispatch_event(&event);
+	}
+
 	ret = subn_get_portinfo(smp, ibdev, port);
 
-	if (clientrereg)
-		pip->clientrereg_resv_subnetto |= 0x80;
+	/* restore re-reg bit per o14-12.2.1 */
+	pip->clientrereg_resv_subnetto |= clientrereg;
 
 	goto get_only;
 
@@ -1835,6 +1849,7 @@ static int process_subn(struct ib_device *ibdev, int mad_flags,
 		    port_num && port_num <= ibdev->phys_port_cnt &&
 		    port != port_num)
 			(void) check_mkey(to_iport(ibdev, port_num), smp, 0);
+		ret = IB_MAD_RESULT_FAILURE;
 		goto bail;
 	}
 
diff --git a/drivers/infiniband/hw/qib/qib_qp.c b/drivers/infiniband/hw/qib/qib_qp.c
index 7e7e16fbee9..1ce56b51ab1 100644
--- a/drivers/infiniband/hw/qib/qib_qp.c
+++ b/drivers/infiniband/hw/qib/qib_qp.c
@@ -1038,6 +1038,11 @@ struct ib_qp *qib_create_qp(struct ib_pd *ibpd,
 			goto bail_swq;
 		}
 		RCU_INIT_POINTER(qp->next, NULL);
+		qp->s_hdr = kzalloc(sizeof(*qp->s_hdr), GFP_KERNEL);
+		if (!qp->s_hdr) {
+			ret = ERR_PTR(-ENOMEM);
+			goto bail_qp;
+		}
 		qp->timeout_jiffies =
 			usecs_to_jiffies((4096UL * (1UL << qp->timeout)) /
 				1000UL);
@@ -1159,6 +1164,7 @@ bail_ip:
 		vfree(qp->r_rq.wq);
 	free_qpn(&dev->qpn_table, qp->ibqp.qp_num);
 bail_qp:
+	kfree(qp->s_hdr);
 	kfree(qp);
 bail_swq:
 	vfree(swq);
@@ -1214,6 +1220,7 @@ int qib_destroy_qp(struct ib_qp *ibqp)
 	else
 		vfree(qp->r_rq.wq);
 	vfree(qp->s_wq);
+	kfree(qp->s_hdr);
 	kfree(qp);
 	return 0;
 }
diff --git a/drivers/infiniband/hw/qib/qib_rc.c b/drivers/infiniband/hw/qib/qib_rc.c
index 765b4cbaa02..b641416148e 100644
--- a/drivers/infiniband/hw/qib/qib_rc.c
+++ b/drivers/infiniband/hw/qib/qib_rc.c
@@ -244,9 +244,9 @@ int qib_make_rc_req(struct qib_qp *qp)
 	int ret = 0;
 	int delta;
 
-	ohdr = &qp->s_hdr.u.oth;
+	ohdr = &qp->s_hdr->u.oth;
 	if (qp->remote_ah_attr.ah_flags & IB_AH_GRH)
-		ohdr = &qp->s_hdr.u.l.oth;
+		ohdr = &qp->s_hdr->u.l.oth;
 
 	/*
 	 * The lock is needed to synchronize between the sending tasklet,
diff --git a/drivers/infiniband/hw/qib/qib_ruc.c b/drivers/infiniband/hw/qib/qib_ruc.c
index b4b37e47321..c0ee7e095d8 100644
--- a/drivers/infiniband/hw/qib/qib_ruc.c
+++ b/drivers/infiniband/hw/qib/qib_ruc.c
@@ -688,17 +688,17 @@ void qib_make_ruc_header(struct qib_qp *qp, struct qib_other_headers *ohdr,
 	nwords = (qp->s_cur_size + extra_bytes) >> 2;
 	lrh0 = QIB_LRH_BTH;
 	if (unlikely(qp->remote_ah_attr.ah_flags & IB_AH_GRH)) {
-		qp->s_hdrwords += qib_make_grh(ibp, &qp->s_hdr.u.l.grh,
+		qp->s_hdrwords += qib_make_grh(ibp, &qp->s_hdr->u.l.grh,
 					       &qp->remote_ah_attr.grh,
 					       qp->s_hdrwords, nwords);
 		lrh0 = QIB_LRH_GRH;
 	}
 	lrh0 |= ibp->sl_to_vl[qp->remote_ah_attr.sl] << 12 |
 		qp->remote_ah_attr.sl << 4;
-	qp->s_hdr.lrh[0] = cpu_to_be16(lrh0);
-	qp->s_hdr.lrh[1] = cpu_to_be16(qp->remote_ah_attr.dlid);
-	qp->s_hdr.lrh[2] = cpu_to_be16(qp->s_hdrwords + nwords + SIZE_OF_CRC);
-	qp->s_hdr.lrh[3] = cpu_to_be16(ppd_from_ibp(ibp)->lid |
+	qp->s_hdr->lrh[0] = cpu_to_be16(lrh0);
+	qp->s_hdr->lrh[1] = cpu_to_be16(qp->remote_ah_attr.dlid);
+	qp->s_hdr->lrh[2] = cpu_to_be16(qp->s_hdrwords + nwords + SIZE_OF_CRC);
+	qp->s_hdr->lrh[3] = cpu_to_be16(ppd_from_ibp(ibp)->lid |
 				       qp->remote_ah_attr.src_path_bits);
 	bth0 |= qib_get_pkey(ibp, qp->s_pkey_index);
 	bth0 |= extra_bytes << 20;
@@ -758,7 +758,7 @@ void qib_do_send(struct work_struct *work)
 			 * If the packet cannot be sent now, return and
 			 * the send tasklet will be woken up later.
 			 */
-			if (qib_verbs_send(qp, &qp->s_hdr, qp->s_hdrwords,
+			if (qib_verbs_send(qp, qp->s_hdr, qp->s_hdrwords,
 					   qp->s_cur_sge, qp->s_cur_size))
 				break;
 			/* Record that s_hdr is empty. */
diff --git a/drivers/infiniband/hw/qib/qib_sysfs.c b/drivers/infiniband/hw/qib/qib_sysfs.c
index dae51604cfc..dd9cd49d097 100644
--- a/drivers/infiniband/hw/qib/qib_sysfs.c
+++ b/drivers/infiniband/hw/qib/qib_sysfs.c
@@ -503,8 +503,11 @@ static ssize_t show_nctxts(struct device *device,
 	struct qib_devdata *dd = dd_from_dev(dev);
 
 	/* Return the number of user ports (contexts) available. */
-	return scnprintf(buf, PAGE_SIZE, "%u\n", dd->cfgctxts -
-		dd->first_user_ctxt);
+	/* The calculation below deals with a special case where
+	 * cfgctxts is set to 1 on a single-port board. */
+	return scnprintf(buf, PAGE_SIZE, "%u\n",
+			(dd->first_user_ctxt > dd->cfgctxts) ? 0 :
+			(dd->cfgctxts - dd->first_user_ctxt));
 }
 
 static ssize_t show_nfreectxts(struct device *device,
diff --git a/drivers/infiniband/hw/qib/qib_tx.c b/drivers/infiniband/hw/qib/qib_tx.c
index 1bf626c4017..31d3561400a 100644
--- a/drivers/infiniband/hw/qib/qib_tx.c
+++ b/drivers/infiniband/hw/qib/qib_tx.c
@@ -295,6 +295,7 @@ u32 __iomem *qib_getsendbuf_range(struct qib_devdata *dd, u32 *pbufnum,
 
 	nbufs = last - first + 1; /* number in range to check */
 	if (dd->upd_pio_shadow) {
+update_shadow:
 		/*
 		 * Minor optimization.  If we had no buffers on last call,
 		 * start out by doing the update; continue and do scan even
@@ -304,37 +305,39 @@ u32 __iomem *qib_getsendbuf_range(struct qib_devdata *dd, u32 *pbufnum,
 		updated++;
 	}
 	i = first;
-rescan:
 	/*
 	 * While test_and_set_bit() is atomic, we do that and then the
 	 * change_bit(), and the pair is not.  See if this is the cause
 	 * of the remaining armlaunch errors.
 	 */
 	spin_lock_irqsave(&dd->pioavail_lock, flags);
+	if (dd->last_pio >= first && dd->last_pio <= last)
+		i = dd->last_pio + 1;
+	if (!first)
+		/* adjust to min possible  */
+		nbufs = last - dd->min_kernel_pio + 1;
 	for (j = 0; j < nbufs; j++, i++) {
 		if (i > last)
-			i = first;
+			i = !first ? dd->min_kernel_pio : first;
 		if (__test_and_set_bit((2 * i) + 1, shadow))
 			continue;
 		/* flip generation bit */
 		__change_bit(2 * i, shadow);
 		/* remember that the buffer can be written to now */
 		__set_bit(i, dd->pio_writing);
+		if (!first && first != last) /* first == last on VL15, avoid */
+			dd->last_pio = i;
 		break;
 	}
 	spin_unlock_irqrestore(&dd->pioavail_lock, flags);
 
 	if (j == nbufs) {
-		if (!updated) {
+		if (!updated)
 			/*
 			 * First time through; shadow exhausted, but may be
 			 * buffers available, try an update and then rescan.
 			 */
-			update_send_bufs(dd);
-			updated++;
-			i = first;
-			goto rescan;
-		}
+			goto update_shadow;
 		no_send_bufs(dd);
 		buf = NULL;
 	} else {
@@ -422,14 +425,20 @@ void qib_chg_pioavailkernel(struct qib_devdata *dd, unsigned start,
 				__clear_bit(QLOGIC_IB_SENDPIOAVAIL_CHECK_SHIFT
 					    + start, dd->pioavailshadow);
 			__set_bit(start, dd->pioavailkernel);
+			if ((start >> 1) < dd->min_kernel_pio)
+				dd->min_kernel_pio = start >> 1;
 		} else {
 			__set_bit(start + QLOGIC_IB_SENDPIOAVAIL_BUSY_SHIFT,
 				  dd->pioavailshadow);
 			__clear_bit(start, dd->pioavailkernel);
+			if ((start >> 1) > dd->min_kernel_pio)
+				dd->min_kernel_pio = start >> 1;
 		}
 		start += 2;
 	}
 
+	if (dd->min_kernel_pio > 0 && dd->last_pio < dd->min_kernel_pio - 1)
+		dd->last_pio = dd->min_kernel_pio - 1;
 	spin_unlock_irqrestore(&dd->pioavail_lock, flags);
 
 	dd->f_txchk_change(dd, ostart, len, avail, rcd);
diff --git a/drivers/infiniband/hw/qib/qib_uc.c b/drivers/infiniband/hw/qib/qib_uc.c
index 7ce2ac2ed21..ce7387ff5d9 100644
--- a/drivers/infiniband/hw/qib/qib_uc.c
+++ b/drivers/infiniband/hw/qib/qib_uc.c
@@ -72,9 +72,9 @@ int qib_make_uc_req(struct qib_qp *qp)
 		goto done;
 	}
 
-	ohdr = &qp->s_hdr.u.oth;
+	ohdr = &qp->s_hdr->u.oth;
 	if (qp->remote_ah_attr.ah_flags & IB_AH_GRH)
-		ohdr = &qp->s_hdr.u.l.oth;
+		ohdr = &qp->s_hdr->u.l.oth;
 
 	/* header size in 32-bit words LRH+BTH = (8+12)/4. */
 	hwords = 5;
diff --git a/drivers/infiniband/hw/qib/qib_ud.c b/drivers/infiniband/hw/qib/qib_ud.c
index 828609fa4d2..a468bf2d446 100644
--- a/drivers/infiniband/hw/qib/qib_ud.c
+++ b/drivers/infiniband/hw/qib/qib_ud.c
@@ -321,11 +321,11 @@ int qib_make_ud_req(struct qib_qp *qp)
 
 	if (ah_attr->ah_flags & IB_AH_GRH) {
 		/* Header size in 32-bit words. */
-		qp->s_hdrwords += qib_make_grh(ibp, &qp->s_hdr.u.l.grh,
+		qp->s_hdrwords += qib_make_grh(ibp, &qp->s_hdr->u.l.grh,
 					       &ah_attr->grh,
 					       qp->s_hdrwords, nwords);
 		lrh0 = QIB_LRH_GRH;
-		ohdr = &qp->s_hdr.u.l.oth;
+		ohdr = &qp->s_hdr->u.l.oth;
 		/*
 		 * Don't worry about sending to locally attached multicast
 		 * QPs.  It is unspecified by the spec. what happens.
@@ -333,7 +333,7 @@ int qib_make_ud_req(struct qib_qp *qp)
 	} else {
 		/* Header size in 32-bit words. */
 		lrh0 = QIB_LRH_BTH;
-		ohdr = &qp->s_hdr.u.oth;
+		ohdr = &qp->s_hdr->u.oth;
 	}
 	if (wqe->wr.opcode == IB_WR_SEND_WITH_IMM) {
 		qp->s_hdrwords++;
@@ -346,15 +346,15 @@ int qib_make_ud_req(struct qib_qp *qp)
 		lrh0 |= 0xF000; /* Set VL (see ch. 13.5.3.1) */
 	else
 		lrh0 |= ibp->sl_to_vl[ah_attr->sl] << 12;
-	qp->s_hdr.lrh[0] = cpu_to_be16(lrh0);
-	qp->s_hdr.lrh[1] = cpu_to_be16(ah_attr->dlid);  /* DEST LID */
-	qp->s_hdr.lrh[2] = cpu_to_be16(qp->s_hdrwords + nwords + SIZE_OF_CRC);
+	qp->s_hdr->lrh[0] = cpu_to_be16(lrh0);
+	qp->s_hdr->lrh[1] = cpu_to_be16(ah_attr->dlid);  /* DEST LID */
+	qp->s_hdr->lrh[2] = cpu_to_be16(qp->s_hdrwords + nwords + SIZE_OF_CRC);
 	lid = ppd->lid;
 	if (lid) {
 		lid |= ah_attr->src_path_bits & ((1 << ppd->lmc) - 1);
-		qp->s_hdr.lrh[3] = cpu_to_be16(lid);
+		qp->s_hdr->lrh[3] = cpu_to_be16(lid);
 	} else
-		qp->s_hdr.lrh[3] = IB_LID_PERMISSIVE;
+		qp->s_hdr->lrh[3] = IB_LID_PERMISSIVE;
 	if (wqe->wr.send_flags & IB_SEND_SOLICITED)
 		bth0 |= IB_BTH_SOLICITED;
 	bth0 |= extra_bytes << 20;
diff --git a/drivers/infiniband/hw/qib/qib_verbs.h b/drivers/infiniband/hw/qib/qib_verbs.h
index 0c19ef0c412..48760602465 100644
--- a/drivers/infiniband/hw/qib/qib_verbs.h
+++ b/drivers/infiniband/hw/qib/qib_verbs.h
@@ -367,9 +367,10 @@ struct qib_rwq {
 
 struct qib_rq {
 	struct qib_rwq *wq;
-	spinlock_t lock; /* protect changes in this struct */
 	u32 size;               /* size of RWQE array */
 	u8 max_sge;
+	spinlock_t lock /* protect changes in this struct */
+		____cacheline_aligned_in_smp;
 };
 
 struct qib_srq {
@@ -412,31 +413,75 @@ struct qib_ack_entry {
  */
 struct qib_qp {
 	struct ib_qp ibqp;
-	struct qib_qp *next;            /* link list for QPN hash table */
-	struct qib_qp *timer_next;      /* link list for qib_ib_timer() */
-	struct list_head iowait;        /* link for wait PIO buf */
-	struct list_head rspwait;       /* link for waititing to respond */
+	/* read mostly fields above and below */
 	struct ib_ah_attr remote_ah_attr;
 	struct ib_ah_attr alt_ah_attr;
-	struct qib_ib_header s_hdr;     /* next packet header to send */
-	atomic_t refcount;
-	wait_queue_head_t wait;
-	wait_queue_head_t wait_dma;
-	struct timer_list s_timer;
-	struct work_struct s_work;
+	struct qib_qp *next;            /* link list for QPN hash table */
+	struct qib_swqe *s_wq;  /* send work queue */
 	struct qib_mmap_info *ip;
+	struct qib_ib_header *s_hdr;     /* next packet header to send */
+	unsigned long timeout_jiffies;  /* computed from timeout */
+
+	enum ib_mtu path_mtu;
+	u32 remote_qpn;
+	u32 pmtu;		/* decoded from path_mtu */
+	u32 qkey;               /* QKEY for this QP (for UD or RD) */
+	u32 s_size;             /* send work queue size */
+	u32 s_rnr_timeout;      /* number of milliseconds for RNR timeout */
+
+	u8 state;               /* QP state */
+	u8 qp_access_flags;
+	u8 alt_timeout;         /* Alternate path timeout for this QP */
+	u8 timeout;             /* Timeout for this QP */
+	u8 s_srate;
+	u8 s_mig_state;
+	u8 port_num;
+	u8 s_pkey_index;        /* PKEY index to use */
+	u8 s_alt_pkey_index;    /* Alternate path PKEY index to use */
+	u8 r_max_rd_atomic;     /* max number of RDMA read/atomic to receive */
+	u8 s_max_rd_atomic;     /* max number of RDMA read/atomic to send */
+	u8 s_retry_cnt;         /* number of times to retry */
+	u8 s_rnr_retry_cnt;
+	u8 r_min_rnr_timer;     /* retry timeout value for RNR NAKs */
+	u8 s_max_sge;           /* size of s_wq->sg_list */
+	u8 s_draining;
+
+	/* start of read/write fields */
+
+	atomic_t refcount ____cacheline_aligned_in_smp;
+	wait_queue_head_t wait;
+
+
+	struct qib_ack_entry s_ack_queue[QIB_MAX_RDMA_ATOMIC + 1]
+		____cacheline_aligned_in_smp;
+	struct qib_sge_state s_rdma_read_sge;
+
+	spinlock_t r_lock ____cacheline_aligned_in_smp;      /* used for APM */
+	unsigned long r_aflags;
+	u64 r_wr_id;            /* ID for current receive WQE */
+	u32 r_ack_psn;          /* PSN for next ACK or atomic ACK */
+	u32 r_len;              /* total length of r_sge */
+	u32 r_rcv_len;          /* receive data len processed */
+	u32 r_psn;              /* expected rcv packet sequence number */
+	u32 r_msn;              /* message sequence number */
+
+	u8 r_state;             /* opcode of last packet received */
+	u8 r_flags;
+	u8 r_head_ack_queue;    /* index into s_ack_queue[] */
+
+	struct list_head rspwait;       /* link for waititing to respond */
+
+	struct qib_sge_state r_sge;     /* current receive data */
+	struct qib_rq r_rq;             /* receive work queue */
+
+	spinlock_t s_lock ____cacheline_aligned_in_smp;
 	struct qib_sge_state *s_cur_sge;
+	u32 s_flags;
 	struct qib_verbs_txreq *s_tx;
-	struct qib_mregion *s_rdma_mr;
+	struct qib_swqe *s_wqe;
 	struct qib_sge_state s_sge;     /* current send request data */
-	struct qib_ack_entry s_ack_queue[QIB_MAX_RDMA_ATOMIC + 1];
-	struct qib_sge_state s_ack_rdma_sge;
-	struct qib_sge_state s_rdma_read_sge;
-	struct qib_sge_state r_sge;     /* current receive data */
-	spinlock_t r_lock;      /* used for APM */
-	spinlock_t s_lock;
+	struct qib_mregion *s_rdma_mr;
 	atomic_t s_dma_busy;
-	u32 s_flags;
 	u32 s_cur_size;         /* size of send packet in bytes */
 	u32 s_len;              /* total length of s_sge */
 	u32 s_rdma_read_len;    /* total length of s_rdma_read_sge */
@@ -447,60 +492,34 @@ struct qib_qp {
 	u32 s_psn;              /* current packet sequence number */
 	u32 s_ack_rdma_psn;     /* PSN for sending RDMA read responses */
 	u32 s_ack_psn;          /* PSN for acking sends and RDMA writes */
-	u32 s_rnr_timeout;      /* number of milliseconds for RNR timeout */
-	u32 r_ack_psn;          /* PSN for next ACK or atomic ACK */
-	u64 r_wr_id;            /* ID for current receive WQE */
-	unsigned long r_aflags;
-	u32 r_len;              /* total length of r_sge */
-	u32 r_rcv_len;          /* receive data len processed */
-	u32 r_psn;              /* expected rcv packet sequence number */
-	u32 r_msn;              /* message sequence number */
+	u32 s_head;             /* new entries added here */
+	u32 s_tail;             /* next entry to process */
+	u32 s_cur;              /* current work queue entry */
+	u32 s_acked;            /* last un-ACK'ed entry */
+	u32 s_last;             /* last completed entry */
+	u32 s_ssn;              /* SSN of tail entry */
+	u32 s_lsn;              /* limit sequence number (credit) */
 	u16 s_hdrwords;         /* size of s_hdr in 32 bit words */
 	u16 s_rdma_ack_cnt;
-	u8 state;               /* QP state */
 	u8 s_state;             /* opcode of last packet sent */
 	u8 s_ack_state;         /* opcode of packet to ACK */
 	u8 s_nak_state;         /* non-zero if NAK is pending */
-	u8 r_state;             /* opcode of last packet received */
 	u8 r_nak_state;         /* non-zero if NAK is pending */
-	u8 r_min_rnr_timer;     /* retry timeout value for RNR NAKs */
-	u8 r_flags;
-	u8 r_max_rd_atomic;     /* max number of RDMA read/atomic to receive */
-	u8 r_head_ack_queue;    /* index into s_ack_queue[] */
-	u8 qp_access_flags;
-	u8 s_max_sge;           /* size of s_wq->sg_list */
-	u8 s_retry_cnt;         /* number of times to retry */
-	u8 s_rnr_retry_cnt;
 	u8 s_retry;             /* requester retry counter */
 	u8 s_rnr_retry;         /* requester RNR retry counter */
-	u8 s_pkey_index;        /* PKEY index to use */
-	u8 s_alt_pkey_index;    /* Alternate path PKEY index to use */
-	u8 s_max_rd_atomic;     /* max number of RDMA read/atomic to send */
 	u8 s_num_rd_atomic;     /* number of RDMA read/atomic pending */
 	u8 s_tail_ack_queue;    /* index into s_ack_queue[] */
-	u8 s_srate;
-	u8 s_draining;
-	u8 s_mig_state;
-	u8 timeout;             /* Timeout for this QP */
-	u8 alt_timeout;         /* Alternate path timeout for this QP */
-	u8 port_num;
-	enum ib_mtu path_mtu;
-	u32 pmtu;		/* decoded from path_mtu */
-	u32 remote_qpn;
-	u32 qkey;               /* QKEY for this QP (for UD or RD) */
-	u32 s_size;             /* send work queue size */
-	u32 s_head;             /* new entries added here */
-	u32 s_tail;             /* next entry to process */
-	u32 s_cur;              /* current work queue entry */
-	u32 s_acked;            /* last un-ACK'ed entry */
-	u32 s_last;             /* last completed entry */
-	u32 s_ssn;              /* SSN of tail entry */
-	u32 s_lsn;              /* limit sequence number (credit) */
-	unsigned long timeout_jiffies;  /* computed from timeout */
-	struct qib_swqe *s_wq;  /* send work queue */
-	struct qib_swqe *s_wqe;
-	struct qib_rq r_rq;             /* receive work queue */
-	struct qib_sge r_sg_list[0];    /* verified SGEs */
+
+	struct qib_sge_state s_ack_rdma_sge;
+	struct timer_list s_timer;
+	struct list_head iowait;        /* link for wait PIO buf */
+
+	struct work_struct s_work;
+
+	wait_queue_head_t wait_dma;
+
+	struct qib_sge r_sg_list[0] /* verified SGEs */
+		____cacheline_aligned_in_smp;
 };
 
 /*
diff --git a/drivers/infiniband/ulp/iser/iscsi_iser.c b/drivers/infiniband/ulp/iser/iscsi_iser.c
index db43b311716..0ab8c9cc3a7 100644
--- a/drivers/infiniband/ulp/iser/iscsi_iser.c
+++ b/drivers/infiniband/ulp/iser/iscsi_iser.c
@@ -573,10 +573,9 @@ iscsi_iser_ep_connect(struct Scsi_Host *shost, struct sockaddr *dst_addr,
 
 	err = iser_connect(ib_conn, NULL, (struct sockaddr_in *)dst_addr,
 			   non_blocking);
-	if (err) {
-		iscsi_destroy_endpoint(ep);
+	if (err)
 		return ERR_PTR(err);
-	}
+
 	return ep;
 }
 
diff --git a/drivers/infiniband/ulp/iser/iser_verbs.c b/drivers/infiniband/ulp/iser/iser_verbs.c
index 14224ba44fd..2dddabd8fcf 100644
--- a/drivers/infiniband/ulp/iser/iser_verbs.c
+++ b/drivers/infiniband/ulp/iser/iser_verbs.c
@@ -613,8 +613,9 @@ id_failure:
 	ib_conn->cma_id = NULL;
 addr_failure:
 	ib_conn->state = ISER_CONN_DOWN;
+	iser_conn_put(ib_conn, 1); /* deref ib conn's cma id */
 connect_failure:
-	iser_conn_release(ib_conn, 1);
+	iser_conn_put(ib_conn, 1); /* deref ib conn deallocate */
 	return err;
 }
 
diff --git a/drivers/infiniband/ulp/srpt/ib_srpt.c b/drivers/infiniband/ulp/srpt/ib_srpt.c
index daf21b89999..5f6b7f63cde 100644
--- a/drivers/infiniband/ulp/srpt/ib_srpt.c
+++ b/drivers/infiniband/ulp/srpt/ib_srpt.c
@@ -1099,9 +1099,8 @@ static int srpt_map_sg_to_ib_sge(struct srpt_rdma_ch *ch,
 	dir = cmd->data_direction;
 	BUG_ON(dir == DMA_NONE);
 
-	transport_do_task_sg_chain(cmd);
-	ioctx->sg = sg = sg_orig = cmd->t_tasks_sg_chained;
-	ioctx->sg_cnt = sg_cnt = cmd->t_tasks_sg_chained_no;
+	ioctx->sg = sg = sg_orig = cmd->t_data_sg;
+	ioctx->sg_cnt = sg_cnt = cmd->t_data_nents;
 
 	count = ib_dma_map_sg(ch->sport->sdev->device, sg, sg_cnt,
 			      opposite_dma_dir(dir));
@@ -1769,7 +1768,7 @@ static int srpt_handle_cmd(struct srpt_rdma_ch *ch,
 		kref_put(&send_ioctx->kref, srpt_put_send_ioctx_kref);
 		goto send_sense;
 	}
-	ret = transport_generic_allocate_tasks(cmd, srp_cmd->cdb);
+	ret = target_setup_cmd_from_cdb(cmd, srp_cmd->cdb);
 	if (ret < 0) {
 		kref_put(&send_ioctx->kref, srpt_put_send_ioctx_kref);
 		if (cmd->se_cmd_flags & SCF_SCSI_RESERVATION_CONFLICT) {
@@ -4004,9 +4003,6 @@ static int __init srpt_init_module(void)
 
 	srpt_target->tf_ops = srpt_template;
 
-	/* Enable SG chaining */
-	srpt_target->tf_ops.task_sg_chaining = true;
-
 	/*
 	 * Set up default attribute lists.
 	 */
diff --git a/drivers/input/Kconfig b/drivers/input/Kconfig
index 33259798081..55f7e57d4e4 100644
--- a/drivers/input/Kconfig
+++ b/drivers/input/Kconfig
@@ -25,10 +25,6 @@ config INPUT
 
 if INPUT
 
-config INPUT_OF_MATRIX_KEYMAP
-	depends on USE_OF
-	bool
-
 config INPUT_FF_MEMLESS
 	tristate "Support for memoryless force-feedback devices"
 	help
@@ -68,6 +64,19 @@ config INPUT_SPARSEKMAP
 	  To compile this driver as a module, choose M here: the
 	  module will be called sparse-keymap.
 
+config INPUT_MATRIXKMAP
+	tristate "Matrix keymap support library"
+	help
+	  Say Y here if you are using a driver for an input
+	  device that uses matrix keymap. This option is only
+	  useful for out-of-tree drivers since in-tree drivers
+	  select it automatically.
+
+	  If unsure, say N.
+
+	  To compile this driver as a module, choose M here: the
+	  module will be called matrix-keymap.
+
 comment "Userland interfaces"
 
 config INPUT_MOUSEDEV
diff --git a/drivers/input/Makefile b/drivers/input/Makefile
index b173a13a73c..5ca3f631497 100644
--- a/drivers/input/Makefile
+++ b/drivers/input/Makefile
@@ -10,6 +10,7 @@ input-core-y := input.o input-compat.o input-mt.o ff-core.o
 obj-$(CONFIG_INPUT_FF_MEMLESS)	+= ff-memless.o
 obj-$(CONFIG_INPUT_POLLDEV)	+= input-polldev.o
 obj-$(CONFIG_INPUT_SPARSEKMAP)	+= sparse-keymap.o
+obj-$(CONFIG_INPUT_MATRIXKMAP)	+= matrix-keymap.o
 
 obj-$(CONFIG_INPUT_MOUSEDEV)	+= mousedev.o
 obj-$(CONFIG_INPUT_JOYDEV)	+= joydev.o
@@ -24,4 +25,3 @@ obj-$(CONFIG_INPUT_TOUCHSCREEN)	+= touchscreen/
 obj-$(CONFIG_INPUT_MISC)	+= misc/
 
 obj-$(CONFIG_INPUT_APMPOWER)	+= apm-power.o
-obj-$(CONFIG_INPUT_OF_MATRIX_KEYMAP) += of_keymap.o
diff --git a/drivers/input/evdev.c b/drivers/input/evdev.c
index 4b2e10d5d64..6c58bfff01a 100644
--- a/drivers/input/evdev.c
+++ b/drivers/input/evdev.c
@@ -180,7 +180,10 @@ static int evdev_grab(struct evdev *evdev, struct evdev_client *client)
 
 static int evdev_ungrab(struct evdev *evdev, struct evdev_client *client)
 {
-	if (evdev->grab != client)
+	struct evdev_client *grab = rcu_dereference_protected(evdev->grab,
+					lockdep_is_held(&evdev->mutex));
+
+	if (grab != client)
 		return  -EINVAL;
 
 	rcu_assign_pointer(evdev->grab, NULL);
@@ -259,8 +262,7 @@ static int evdev_release(struct inode *inode, struct file *file)
 	struct evdev *evdev = client->evdev;
 
 	mutex_lock(&evdev->mutex);
-	if (evdev->grab == client)
-		evdev_ungrab(evdev, client);
+	evdev_ungrab(evdev, client);
 	mutex_unlock(&evdev->mutex);
 
 	evdev_detach_client(evdev, client);
@@ -343,7 +345,7 @@ static ssize_t evdev_write(struct file *file, const char __user *buffer,
 	struct input_event event;
 	int retval = 0;
 
-	if (count < input_event_size())
+	if (count != 0 && count < input_event_size())
 		return -EINVAL;
 
 	retval = mutex_lock_interruptible(&evdev->mutex);
@@ -355,7 +357,8 @@ static ssize_t evdev_write(struct file *file, const char __user *buffer,
 		goto out;
 	}
 
-	do {
+	while (retval + input_event_size() <= count) {
+
 		if (input_event_from_user(buffer + retval, &event)) {
 			retval = -EFAULT;
 			goto out;
@@ -364,7 +367,7 @@ static ssize_t evdev_write(struct file *file, const char __user *buffer,
 
 		input_inject_event(&evdev->handle,
 				   event.type, event.code, event.value);
-	} while (retval + input_event_size() <= count);
+	}
 
  out:
 	mutex_unlock(&evdev->mutex);
@@ -395,35 +398,49 @@ static ssize_t evdev_read(struct file *file, char __user *buffer,
 	struct evdev_client *client = file->private_data;
 	struct evdev *evdev = client->evdev;
 	struct input_event event;
-	int retval = 0;
+	size_t read = 0;
+	int error;
 
-	if (count < input_event_size())
+	if (count != 0 && count < input_event_size())
 		return -EINVAL;
 
-	if (!(file->f_flags & O_NONBLOCK)) {
-		retval = wait_event_interruptible(evdev->wait,
-				client->packet_head != client->tail ||
-				!evdev->exist);
-		if (retval)
-			return retval;
-	}
+	for (;;) {
+		if (!evdev->exist)
+			return -ENODEV;
 
-	if (!evdev->exist)
-		return -ENODEV;
+		if (client->packet_head == client->tail &&
+		    (file->f_flags & O_NONBLOCK))
+			return -EAGAIN;
 
-	while (retval + input_event_size() <= count &&
-	       evdev_fetch_next_event(client, &event)) {
+		/*
+		 * count == 0 is special - no IO is done but we check
+		 * for error conditions (see above).
+		 */
+		if (count == 0)
+			break;
 
-		if (input_event_to_user(buffer + retval, &event))
-			return -EFAULT;
+		while (read + input_event_size() <= count &&
+		       evdev_fetch_next_event(client, &event)) {
 
-		retval += input_event_size();
-	}
+			if (input_event_to_user(buffer + read, &event))
+				return -EFAULT;
 
-	if (retval == 0 && (file->f_flags & O_NONBLOCK))
-		return -EAGAIN;
+			read += input_event_size();
+		}
 
-	return retval;
+		if (read)
+			break;
+
+		if (!(file->f_flags & O_NONBLOCK)) {
+			error = wait_event_interruptible(evdev->wait,
+					client->packet_head != client->tail ||
+					!evdev->exist);
+			if (error)
+				return error;
+		}
+	}
+
+	return read;
 }
 
 /* No kernel lock - fine */
diff --git a/drivers/input/ff-memless.c b/drivers/input/ff-memless.c
index 117a59aaa70..5f558851d64 100644
--- a/drivers/input/ff-memless.c
+++ b/drivers/input/ff-memless.c
@@ -31,8 +31,7 @@
 #include <linux/mutex.h>
 #include <linux/spinlock.h>
 #include <linux/jiffies.h>
-
-#include "fixp-arith.h"
+#include <linux/fixp-arith.h>
 
 MODULE_LICENSE("GPL");
 MODULE_AUTHOR("Anssi Hannula <anssi.hannula@gmail.com>");
diff --git a/drivers/input/fixp-arith.h b/drivers/input/fixp-arith.h
deleted file mode 100644
index 3089d738232..00000000000
--- a/drivers/input/fixp-arith.h
+++ /dev/null
@@ -1,87 +0,0 @@
-#ifndef _FIXP_ARITH_H
-#define _FIXP_ARITH_H
-
-/*
- * Simplistic fixed-point arithmetics.
- * Hmm, I'm probably duplicating some code :(
- *
- * Copyright (c) 2002 Johann Deneux
- */
-
-/*
- * 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
- *
- * Should you need to contact me, the author, you can do so by
- * e-mail - mail your message to <johann.deneux@gmail.com>
- */
-
-#include <linux/types.h>
-
-/* The type representing fixed-point values */
-typedef s16 fixp_t;
-
-#define FRAC_N 8
-#define FRAC_MASK ((1<<FRAC_N)-1)
-
-/* Not to be used directly. Use fixp_{cos,sin} */
-static const fixp_t cos_table[46] = {
-	0x0100,	0x00FF,	0x00FF,	0x00FE,	0x00FD,	0x00FC,	0x00FA,	0x00F8,
-	0x00F6,	0x00F3,	0x00F0,	0x00ED,	0x00E9,	0x00E6,	0x00E2,	0x00DD,
-	0x00D9,	0x00D4,	0x00CF,	0x00C9,	0x00C4,	0x00BE,	0x00B8,	0x00B1,
-	0x00AB,	0x00A4,	0x009D,	0x0096,	0x008F,	0x0087,	0x0080,	0x0078,
-	0x0070,	0x0068,	0x005F,	0x0057,	0x004F,	0x0046,	0x003D,	0x0035,
-	0x002C,	0x0023,	0x001A,	0x0011,	0x0008, 0x0000
-};
-
-
-/* a: 123 -> 123.0 */
-static inline fixp_t fixp_new(s16 a)
-{
-	return a<<FRAC_N;
-}
-
-/* a: 0xFFFF -> -1.0
-      0x8000 -> 1.0
-      0x0000 -> 0.0
-*/
-static inline fixp_t fixp_new16(s16 a)
-{
-	return ((s32)a)>>(16-FRAC_N);
-}
-
-static inline fixp_t fixp_cos(unsigned int degrees)
-{
-	int quadrant = (degrees / 90) & 3;
-	unsigned int i = degrees % 90;
-
-	if (quadrant == 1 || quadrant == 3)
-		i = 90 - i;
-
-	i >>= 1;
-
-	return (quadrant == 1 || quadrant == 2)? -cos_table[i] : cos_table[i];
-}
-
-static inline fixp_t fixp_sin(unsigned int degrees)
-{
-	return -fixp_cos(degrees + 90);
-}
-
-static inline fixp_t fixp_mult(fixp_t a, fixp_t b)
-{
-	return ((s32)(a*b))>>FRAC_N;
-}
-
-#endif
diff --git a/drivers/input/gameport/emu10k1-gp.c b/drivers/input/gameport/emu10k1-gp.c
index 422aa0a6b77..daceafe7ee7 100644
--- a/drivers/input/gameport/emu10k1-gp.c
+++ b/drivers/input/gameport/emu10k1-gp.c
@@ -125,15 +125,4 @@ static struct pci_driver emu_driver = {
         .remove =       __devexit_p(emu_remove),
 };
 
-static int __init emu_init(void)
-{
-	return pci_register_driver(&emu_driver);
-}
-
-static void __exit emu_exit(void)
-{
-	pci_unregister_driver(&emu_driver);
-}
-
-module_init(emu_init);
-module_exit(emu_exit);
+module_pci_driver(emu_driver);
diff --git a/drivers/input/gameport/fm801-gp.c b/drivers/input/gameport/fm801-gp.c
index a3b70ff2101..48ad3829ff2 100644
--- a/drivers/input/gameport/fm801-gp.c
+++ b/drivers/input/gameport/fm801-gp.c
@@ -144,6 +144,7 @@ static const struct pci_device_id fm801_gp_id_table[] = {
 	{ PCI_VENDOR_ID_FORTEMEDIA, PCI_DEVICE_ID_FM801_GP, PCI_ANY_ID, PCI_ANY_ID, 0, 0, 0  },
 	{ 0 }
 };
+MODULE_DEVICE_TABLE(pci, fm801_gp_id_table);
 
 static struct pci_driver fm801_gp_driver = {
 	.name =		"FM801_gameport",
@@ -152,20 +153,7 @@ static struct pci_driver fm801_gp_driver = {
 	.remove =	__devexit_p(fm801_gp_remove),
 };
 
-static int __init fm801_gp_init(void)
-{
-	return pci_register_driver(&fm801_gp_driver);
-}
-
-static void __exit fm801_gp_exit(void)
-{
-	pci_unregister_driver(&fm801_gp_driver);
-}
-
-module_init(fm801_gp_init);
-module_exit(fm801_gp_exit);
-
-MODULE_DEVICE_TABLE(pci, fm801_gp_id_table);
+module_pci_driver(fm801_gp_driver);
 
 MODULE_DESCRIPTION("FM801 gameport driver");
 MODULE_AUTHOR("Takashi Iwai <tiwai@suse.de>");
diff --git a/drivers/input/joystick/a3d.c b/drivers/input/joystick/a3d.c
index 1639ab2b94b..85bc8dc07cf 100644
--- a/drivers/input/joystick/a3d.c
+++ b/drivers/input/joystick/a3d.c
@@ -413,15 +413,4 @@ static struct gameport_driver a3d_drv = {
 	.disconnect	= a3d_disconnect,
 };
 
-static int __init a3d_init(void)
-{
-	return gameport_register_driver(&a3d_drv);
-}
-
-static void __exit a3d_exit(void)
-{
-	gameport_unregister_driver(&a3d_drv);
-}
-
-module_init(a3d_init);
-module_exit(a3d_exit);
+module_gameport_driver(a3d_drv);
diff --git a/drivers/input/joystick/adi.c b/drivers/input/joystick/adi.c
index b992fbf91f2..0cbfd2dfabf 100644
--- a/drivers/input/joystick/adi.c
+++ b/drivers/input/joystick/adi.c
@@ -557,10 +557,6 @@ static void adi_disconnect(struct gameport *gameport)
 	kfree(port);
 }
 
-/*
- * The gameport device structure.
- */
-
 static struct gameport_driver adi_drv = {
 	.driver		= {
 		.name	= "adi",
@@ -570,15 +566,4 @@ static struct gameport_driver adi_drv = {
 	.disconnect	= adi_disconnect,
 };
 
-static int __init adi_init(void)
-{
-	return gameport_register_driver(&adi_drv);
-}
-
-static void __exit adi_exit(void)
-{
-	gameport_unregister_driver(&adi_drv);
-}
-
-module_init(adi_init);
-module_exit(adi_exit);
+module_gameport_driver(adi_drv);
diff --git a/drivers/input/joystick/cobra.c b/drivers/input/joystick/cobra.c
index 3497b87c3d0..65367e44d71 100644
--- a/drivers/input/joystick/cobra.c
+++ b/drivers/input/joystick/cobra.c
@@ -261,15 +261,4 @@ static struct gameport_driver cobra_drv = {
 	.disconnect	= cobra_disconnect,
 };
 
-static int __init cobra_init(void)
-{
-	return gameport_register_driver(&cobra_drv);
-}
-
-static void __exit cobra_exit(void)
-{
-	gameport_unregister_driver(&cobra_drv);
-}
-
-module_init(cobra_init);
-module_exit(cobra_exit);
+module_gameport_driver(cobra_drv);
diff --git a/drivers/input/joystick/gf2k.c b/drivers/input/joystick/gf2k.c
index 0536b1b2f01..ab1cf288200 100644
--- a/drivers/input/joystick/gf2k.c
+++ b/drivers/input/joystick/gf2k.c
@@ -373,15 +373,4 @@ static struct gameport_driver gf2k_drv = {
 	.disconnect	= gf2k_disconnect,
 };
 
-static int __init gf2k_init(void)
-{
-	return gameport_register_driver(&gf2k_drv);
-}
-
-static void __exit gf2k_exit(void)
-{
-	gameport_unregister_driver(&gf2k_drv);
-}
-
-module_init(gf2k_init);
-module_exit(gf2k_exit);
+module_gameport_driver(gf2k_drv);
diff --git a/drivers/input/joystick/grip.c b/drivers/input/joystick/grip.c
index fc55899ba6c..9e1beff57c3 100644
--- a/drivers/input/joystick/grip.c
+++ b/drivers/input/joystick/grip.c
@@ -424,15 +424,4 @@ static struct gameport_driver grip_drv = {
 	.disconnect	= grip_disconnect,
 };
 
-static int __init grip_init(void)
-{
-	return gameport_register_driver(&grip_drv);
-}
-
-static void __exit grip_exit(void)
-{
-	gameport_unregister_driver(&grip_drv);
-}
-
-module_init(grip_init);
-module_exit(grip_exit);
+module_gameport_driver(grip_drv);
diff --git a/drivers/input/joystick/grip_mp.c b/drivers/input/joystick/grip_mp.c
index 2d47baf4776..c0f9c7b7eb4 100644
--- a/drivers/input/joystick/grip_mp.c
+++ b/drivers/input/joystick/grip_mp.c
@@ -687,15 +687,4 @@ static struct gameport_driver grip_drv = {
 	.disconnect	= grip_disconnect,
 };
 
-static int __init grip_init(void)
-{
-	return gameport_register_driver(&grip_drv);
-}
-
-static void __exit grip_exit(void)
-{
-	gameport_unregister_driver(&grip_drv);
-}
-
-module_init(grip_init);
-module_exit(grip_exit);
+module_gameport_driver(grip_drv);
diff --git a/drivers/input/joystick/guillemot.c b/drivers/input/joystick/guillemot.c
index 4058d4b272f..55196f730af 100644
--- a/drivers/input/joystick/guillemot.c
+++ b/drivers/input/joystick/guillemot.c
@@ -281,15 +281,4 @@ static struct gameport_driver guillemot_drv = {
 	.disconnect	= guillemot_disconnect,
 };
 
-static int __init guillemot_init(void)
-{
-	return gameport_register_driver(&guillemot_drv);
-}
-
-static void __exit guillemot_exit(void)
-{
-	gameport_unregister_driver(&guillemot_drv);
-}
-
-module_init(guillemot_init);
-module_exit(guillemot_exit);
+module_gameport_driver(guillemot_drv);
diff --git a/drivers/input/joystick/iforce/iforce-main.c b/drivers/input/joystick/iforce/iforce-main.c
index 405febd94f2..daeeb4c7e3b 100644
--- a/drivers/input/joystick/iforce/iforce-main.c
+++ b/drivers/input/joystick/iforce/iforce-main.c
@@ -317,7 +317,8 @@ int iforce_init_device(struct iforce *iforce)
 			break;
 
 	if (i == 20) { /* 5 seconds */
-		err("Timeout waiting for response from device.");
+		dev_err(&input_dev->dev,
+			"Timeout waiting for response from device.\n");
 		error = -ENODEV;
 		goto fail;
 	}
diff --git a/drivers/input/joystick/iforce/iforce-packets.c b/drivers/input/joystick/iforce/iforce-packets.c
index a17b5001600..08f98f2eaf8 100644
--- a/drivers/input/joystick/iforce/iforce-packets.c
+++ b/drivers/input/joystick/iforce/iforce-packets.c
@@ -257,7 +257,8 @@ int iforce_get_id_packet(struct iforce *iforce, char *packet)
 
 		status = usb_submit_urb(iforce->ctrl, GFP_ATOMIC);
 		if (status) {
-			err("usb_submit_urb failed %d", status);
+			dev_err(&iforce->intf->dev,
+				"usb_submit_urb failed %d\n", status);
 			return -1;
 		}
 
@@ -265,12 +266,14 @@ int iforce_get_id_packet(struct iforce *iforce, char *packet)
 			iforce->ctrl->status != -EINPROGRESS, HZ);
 
 		if (iforce->ctrl->status) {
-			dbg("iforce->ctrl->status = %d", iforce->ctrl->status);
+			dev_dbg(&iforce->intf->dev,
+				"iforce->ctrl->status = %d\n",
+				iforce->ctrl->status);
 			usb_unlink_urb(iforce->ctrl);
 			return -1;
 		}
 #else
-		dbg("iforce_get_id_packet: iforce->bus = USB!");
+		printk(KERN_DEBUG "iforce_get_id_packet: iforce->bus = USB!\n");
 #endif
 		}
 		break;
@@ -289,12 +292,15 @@ int iforce_get_id_packet(struct iforce *iforce, char *packet)
 			return -1;
 		}
 #else
-		err("iforce_get_id_packet: iforce->bus = SERIO!");
+		dev_err(&iforce->dev->dev,
+			"iforce_get_id_packet: iforce->bus = SERIO!\n");
 #endif
 		break;
 
 	default:
-		err("iforce_get_id_packet: iforce->bus = %d", iforce->bus);
+		dev_err(&iforce->dev->dev,
+			"iforce_get_id_packet: iforce->bus = %d\n",
+			iforce->bus);
 		break;
 	}
 
diff --git a/drivers/input/joystick/iforce/iforce-usb.c b/drivers/input/joystick/iforce/iforce-usb.c
index 6c96631ae5d..d96aa27dfcd 100644
--- a/drivers/input/joystick/iforce/iforce-usb.c
+++ b/drivers/input/joystick/iforce/iforce-usb.c
@@ -64,7 +64,7 @@ void iforce_usb_xmit(struct iforce *iforce)
 
 	if ( (n=usb_submit_urb(iforce->out, GFP_ATOMIC)) ) {
 		clear_bit(IFORCE_XMIT_RUNNING, iforce->xmit_flags);
-		dev_warn(&iforce->dev->dev, "usb_submit_urb failed %d\n", n);
+		dev_warn(&iforce->intf->dev, "usb_submit_urb failed %d\n", n);
 	}
 
 	/* The IFORCE_XMIT_RUNNING bit is not cleared here. That's intended.
@@ -76,6 +76,7 @@ void iforce_usb_xmit(struct iforce *iforce)
 static void iforce_usb_irq(struct urb *urb)
 {
 	struct iforce *iforce = urb->context;
+	struct device *dev = &iforce->intf->dev;
 	int status;
 
 	switch (urb->status) {
@@ -86,11 +87,12 @@ static void iforce_usb_irq(struct urb *urb)
 	case -ENOENT:
 	case -ESHUTDOWN:
 		/* this urb is terminated, clean up */
-		dbg("%s - urb shutting down with status: %d",
-		    __func__, urb->status);
+		dev_dbg(dev, "%s - urb shutting down with status: %d\n",
+			__func__, urb->status);
 		return;
 	default:
-		dbg("%s - urb has status of: %d", __func__, urb->status);
+		dev_dbg(dev, "%s - urb has status of: %d\n",
+			__func__, urb->status);
 		goto exit;
 	}
 
@@ -100,8 +102,8 @@ static void iforce_usb_irq(struct urb *urb)
 exit:
 	status = usb_submit_urb (urb, GFP_ATOMIC);
 	if (status)
-		err ("%s - usb_submit_urb failed with result %d",
-		     __func__, status);
+		dev_err(dev, "%s - usb_submit_urb failed with result %d\n",
+			__func__, status);
 }
 
 static void iforce_usb_out(struct urb *urb)
@@ -110,7 +112,8 @@ static void iforce_usb_out(struct urb *urb)
 
 	if (urb->status) {
 		clear_bit(IFORCE_XMIT_RUNNING, iforce->xmit_flags);
-		dbg("urb->status %d, exiting", urb->status);
+		dev_dbg(&iforce->intf->dev, "urb->status %d, exiting\n",
+			urb->status);
 		return;
 	}
 
@@ -155,6 +158,7 @@ static int iforce_usb_probe(struct usb_interface *intf,
 
 	iforce->bus = IFORCE_USB;
 	iforce->usbdev = dev;
+	iforce->intf = intf;
 
 	iforce->cr.bRequestType = USB_TYPE_VENDOR | USB_DIR_IN | USB_RECIP_INTERFACE;
 	iforce->cr.wIndex = 0;
diff --git a/drivers/input/joystick/iforce/iforce.h b/drivers/input/joystick/iforce/iforce.h
index 9f494b75848..b1d7d9b0eb8 100644
--- a/drivers/input/joystick/iforce/iforce.h
+++ b/drivers/input/joystick/iforce/iforce.h
@@ -115,6 +115,7 @@ struct iforce {
 #endif
 #ifdef CONFIG_JOYSTICK_IFORCE_USB
 	struct usb_device *usbdev;	/* USB transfer */
+	struct usb_interface *intf;
 	struct urb *irq, *out, *ctrl;
 	struct usb_ctrlrequest cr;
 #endif
diff --git a/drivers/input/joystick/interact.c b/drivers/input/joystick/interact.c
index 16fb19d1ca2..88c22623a2e 100644
--- a/drivers/input/joystick/interact.c
+++ b/drivers/input/joystick/interact.c
@@ -311,15 +311,4 @@ static struct gameport_driver interact_drv = {
 	.disconnect	= interact_disconnect,
 };
 
-static int __init interact_init(void)
-{
-	return gameport_register_driver(&interact_drv);
-}
-
-static void __exit interact_exit(void)
-{
-	gameport_unregister_driver(&interact_drv);
-}
-
-module_init(interact_init);
-module_exit(interact_exit);
+module_gameport_driver(interact_drv);
diff --git a/drivers/input/joystick/joydump.c b/drivers/input/joystick/joydump.c
index cd894a0564a..7eb878bab96 100644
--- a/drivers/input/joystick/joydump.c
+++ b/drivers/input/joystick/joydump.c
@@ -159,15 +159,4 @@ static struct gameport_driver joydump_drv = {
 	.disconnect	= joydump_disconnect,
 };
 
-static int __init joydump_init(void)
-{
-	return gameport_register_driver(&joydump_drv);
-}
-
-static void __exit joydump_exit(void)
-{
-	gameport_unregister_driver(&joydump_drv);
-}
-
-module_init(joydump_init);
-module_exit(joydump_exit);
+module_gameport_driver(joydump_drv);
diff --git a/drivers/input/joystick/magellan.c b/drivers/input/joystick/magellan.c
index 40e40780747..9fb153eef2f 100644
--- a/drivers/input/joystick/magellan.c
+++ b/drivers/input/joystick/magellan.c
@@ -222,19 +222,4 @@ static struct serio_driver magellan_drv = {
 	.disconnect	= magellan_disconnect,
 };
 
-/*
- * The functions for inserting/removing us as a module.
- */
-
-static int __init magellan_init(void)
-{
-	return serio_register_driver(&magellan_drv);
-}
-
-static void __exit magellan_exit(void)
-{
-	serio_unregister_driver(&magellan_drv);
-}
-
-module_init(magellan_init);
-module_exit(magellan_exit);
+module_serio_driver(magellan_drv);
diff --git a/drivers/input/joystick/sidewinder.c b/drivers/input/joystick/sidewinder.c
index b8d86115644..04c69af3714 100644
--- a/drivers/input/joystick/sidewinder.c
+++ b/drivers/input/joystick/sidewinder.c
@@ -820,15 +820,4 @@ static struct gameport_driver sw_drv = {
 	.disconnect	= sw_disconnect,
 };
 
-static int __init sw_init(void)
-{
-	return gameport_register_driver(&sw_drv);
-}
-
-static void __exit sw_exit(void)
-{
-	gameport_unregister_driver(&sw_drv);
-}
-
-module_init(sw_init);
-module_exit(sw_exit);
+module_gameport_driver(sw_drv);
diff --git a/drivers/input/joystick/spaceball.c b/drivers/input/joystick/spaceball.c
index 0cd9b29356a..80a7b27a457 100644
--- a/drivers/input/joystick/spaceball.c
+++ b/drivers/input/joystick/spaceball.c
@@ -296,19 +296,4 @@ static struct serio_driver spaceball_drv = {
 	.disconnect	= spaceball_disconnect,
 };
 
-/*
- * The functions for inserting/removing us as a module.
- */
-
-static int __init spaceball_init(void)
-{
-	return serio_register_driver(&spaceball_drv);
-}
-
-static void __exit spaceball_exit(void)
-{
-	serio_unregister_driver(&spaceball_drv);
-}
-
-module_init(spaceball_init);
-module_exit(spaceball_exit);
+module_serio_driver(spaceball_drv);
diff --git a/drivers/input/joystick/spaceorb.c b/drivers/input/joystick/spaceorb.c
index a694bf8e557..a41f291652e 100644
--- a/drivers/input/joystick/spaceorb.c
+++ b/drivers/input/joystick/spaceorb.c
@@ -237,19 +237,4 @@ static struct serio_driver spaceorb_drv = {
 	.disconnect	= spaceorb_disconnect,
 };
 
-/*
- * The functions for inserting/removing us as a module.
- */
-
-static int __init spaceorb_init(void)
-{
-	return serio_register_driver(&spaceorb_drv);
-}
-
-static void __exit spaceorb_exit(void)
-{
-	serio_unregister_driver(&spaceorb_drv);
-}
-
-module_init(spaceorb_init);
-module_exit(spaceorb_exit);
+module_serio_driver(spaceorb_drv);
diff --git a/drivers/input/joystick/stinger.c b/drivers/input/joystick/stinger.c
index e0db9f5e4b4..0f51a60e14a 100644
--- a/drivers/input/joystick/stinger.c
+++ b/drivers/input/joystick/stinger.c
@@ -208,19 +208,4 @@ static struct serio_driver stinger_drv = {
 	.disconnect	= stinger_disconnect,
 };
 
-/*
- * The functions for inserting/removing us as a module.
- */
-
-static int __init stinger_init(void)
-{
-	return serio_register_driver(&stinger_drv);
-}
-
-static void __exit stinger_exit(void)
-{
-	serio_unregister_driver(&stinger_drv);
-}
-
-module_init(stinger_init);
-module_exit(stinger_exit);
+module_serio_driver(stinger_drv);
diff --git a/drivers/input/joystick/tmdc.c b/drivers/input/joystick/tmdc.c
index d6c60980711..5ef9bcdb034 100644
--- a/drivers/input/joystick/tmdc.c
+++ b/drivers/input/joystick/tmdc.c
@@ -436,15 +436,4 @@ static struct gameport_driver tmdc_drv = {
 	.disconnect	= tmdc_disconnect,
 };
 
-static int __init tmdc_init(void)
-{
-	return gameport_register_driver(&tmdc_drv);
-}
-
-static void __exit tmdc_exit(void)
-{
-	gameport_unregister_driver(&tmdc_drv);
-}
-
-module_init(tmdc_init);
-module_exit(tmdc_exit);
+module_gameport_driver(tmdc_drv);
diff --git a/drivers/input/joystick/twidjoy.c b/drivers/input/joystick/twidjoy.c
index 3f4ec73c955..2556a819357 100644
--- a/drivers/input/joystick/twidjoy.c
+++ b/drivers/input/joystick/twidjoy.c
@@ -257,19 +257,4 @@ static struct serio_driver twidjoy_drv = {
 	.disconnect	= twidjoy_disconnect,
 };
 
-/*
- * The functions for inserting/removing us as a module.
- */
-
-static int __init twidjoy_init(void)
-{
-	return serio_register_driver(&twidjoy_drv);
-}
-
-static void __exit twidjoy_exit(void)
-{
-	serio_unregister_driver(&twidjoy_drv);
-}
-
-module_init(twidjoy_init);
-module_exit(twidjoy_exit);
+module_serio_driver(twidjoy_drv);
diff --git a/drivers/input/joystick/warrior.c b/drivers/input/joystick/warrior.c
index f72c83e15e6..23b3071abb6 100644
--- a/drivers/input/joystick/warrior.c
+++ b/drivers/input/joystick/warrior.c
@@ -217,19 +217,4 @@ static struct serio_driver warrior_drv = {
 	.disconnect	= warrior_disconnect,
 };
 
-/*
- * The functions for inserting/removing us as a module.
- */
-
-static int __init warrior_init(void)
-{
-	return serio_register_driver(&warrior_drv);
-}
-
-static void __exit warrior_exit(void)
-{
-	serio_unregister_driver(&warrior_drv);
-}
-
-module_init(warrior_init);
-module_exit(warrior_exit);
+module_serio_driver(warrior_drv);
diff --git a/drivers/input/joystick/xpad.c b/drivers/input/joystick/xpad.c
index fd7a0d5bc94..ee16fb67b7a 100644
--- a/drivers/input/joystick/xpad.c
+++ b/drivers/input/joystick/xpad.c
@@ -252,6 +252,7 @@ MODULE_DEVICE_TABLE (usb, xpad_table);
 struct usb_xpad {
 	struct input_dev *dev;		/* input device interface */
 	struct usb_device *udev;	/* usb device */
+	struct usb_interface *intf;	/* usb interface */
 
 	int pad_present;
 
@@ -457,6 +458,7 @@ static void xpad360w_process_packet(struct usb_xpad *xpad, u16 cmd, unsigned cha
 static void xpad_irq_in(struct urb *urb)
 {
 	struct usb_xpad *xpad = urb->context;
+	struct device *dev = &xpad->intf->dev;
 	int retval, status;
 
 	status = urb->status;
@@ -469,11 +471,11 @@ static void xpad_irq_in(struct urb *urb)
 	case -ENOENT:
 	case -ESHUTDOWN:
 		/* this urb is terminated, clean up */
-		dbg("%s - urb shutting down with status: %d",
+		dev_dbg(dev, "%s - urb shutting down with status: %d\n",
 			__func__, status);
 		return;
 	default:
-		dbg("%s - nonzero urb status received: %d",
+		dev_dbg(dev, "%s - nonzero urb status received: %d\n",
 			__func__, status);
 		goto exit;
 	}
@@ -492,12 +494,15 @@ static void xpad_irq_in(struct urb *urb)
 exit:
 	retval = usb_submit_urb(urb, GFP_ATOMIC);
 	if (retval)
-		err ("%s - usb_submit_urb failed with result %d",
-		     __func__, retval);
+		dev_err(dev, "%s - usb_submit_urb failed with result %d\n",
+			__func__, retval);
 }
 
 static void xpad_bulk_out(struct urb *urb)
 {
+	struct usb_xpad *xpad = urb->context;
+	struct device *dev = &xpad->intf->dev;
+
 	switch (urb->status) {
 	case 0:
 		/* success */
@@ -506,16 +511,20 @@ static void xpad_bulk_out(struct urb *urb)
 	case -ENOENT:
 	case -ESHUTDOWN:
 		/* this urb is terminated, clean up */
-		dbg("%s - urb shutting down with status: %d", __func__, urb->status);
+		dev_dbg(dev, "%s - urb shutting down with status: %d\n",
+			__func__, urb->status);
 		break;
 	default:
-		dbg("%s - nonzero urb status received: %d", __func__, urb->status);
+		dev_dbg(dev, "%s - nonzero urb status received: %d\n",
+			__func__, urb->status);
 	}
 }
 
 #if defined(CONFIG_JOYSTICK_XPAD_FF) || defined(CONFIG_JOYSTICK_XPAD_LEDS)
 static void xpad_irq_out(struct urb *urb)
 {
+	struct usb_xpad *xpad = urb->context;
+	struct device *dev = &xpad->intf->dev;
 	int retval, status;
 
 	status = urb->status;
@@ -529,19 +538,21 @@ static void xpad_irq_out(struct urb *urb)
 	case -ENOENT:
 	case -ESHUTDOWN:
 		/* this urb is terminated, clean up */
-		dbg("%s - urb shutting down with status: %d", __func__, status);
+		dev_dbg(dev, "%s - urb shutting down with status: %d\n",
+			__func__, status);
 		return;
 
 	default:
-		dbg("%s - nonzero urb status received: %d", __func__, status);
+		dev_dbg(dev, "%s - nonzero urb status received: %d\n",
+			__func__, status);
 		goto exit;
 	}
 
 exit:
 	retval = usb_submit_urb(urb, GFP_ATOMIC);
 	if (retval)
-		err("%s - usb_submit_urb failed with result %d",
-		    __func__, retval);
+		dev_err(dev, "%s - usb_submit_urb failed with result %d\n",
+			__func__, retval);
 }
 
 static int xpad_init_output(struct usb_interface *intf, struct usb_xpad *xpad)
@@ -654,7 +665,8 @@ static int xpad_play_effect(struct input_dev *dev, void *data, struct ff_effect
 			return usb_submit_urb(xpad->irq_out, GFP_ATOMIC);
 
 		default:
-			dbg("%s - rumble command sent to unsupported xpad type: %d",
+			dev_dbg(&xpad->dev->dev,
+				"%s - rumble command sent to unsupported xpad type: %d\n",
 				__func__, xpad->xtype);
 			return -1;
 		}
@@ -844,6 +856,7 @@ static int xpad_probe(struct usb_interface *intf, const struct usb_device_id *id
 	}
 
 	xpad->udev = udev;
+	xpad->intf = intf;
 	xpad->mapping = xpad_device[i].mapping;
 	xpad->xtype = xpad_device[i].xtype;
 
diff --git a/drivers/input/joystick/zhenhua.c b/drivers/input/joystick/zhenhua.c
index b5853125c89..c4de4388fd7 100644
--- a/drivers/input/joystick/zhenhua.c
+++ b/drivers/input/joystick/zhenhua.c
@@ -225,19 +225,4 @@ static struct serio_driver zhenhua_drv = {
 	.disconnect	= zhenhua_disconnect,
 };
 
-/*
- * The functions for inserting/removing us as a module.
- */
-
-static int __init zhenhua_init(void)
-{
-	return serio_register_driver(&zhenhua_drv);
-}
-
-static void __exit zhenhua_exit(void)
-{
-	serio_unregister_driver(&zhenhua_drv);
-}
-
-module_init(zhenhua_init);
-module_exit(zhenhua_exit);
+module_serio_driver(zhenhua_drv);
diff --git a/drivers/input/keyboard/Kconfig b/drivers/input/keyboard/Kconfig
index f354813a13e..c0e11ecc646 100644
--- a/drivers/input/keyboard/Kconfig
+++ b/drivers/input/keyboard/Kconfig
@@ -166,6 +166,7 @@ config KEYBOARD_LKKBD
 config KEYBOARD_EP93XX
 	tristate "EP93xx Matrix Keypad support"
 	depends on ARCH_EP93XX
+	select INPUT_MATRIXKMAP
 	help
 	  Say Y here to enable the matrix keypad on the Cirrus EP93XX.
 
@@ -224,6 +225,7 @@ config KEYBOARD_TCA6416
 config KEYBOARD_TCA8418
 	tristate "TCA8418 Keypad Support"
 	depends on I2C
+	select INPUT_MATRIXKMAP
 	help
 	  This driver implements basic keypad functionality
 	  for keys connected through TCA8418 keypad decoder.
@@ -240,6 +242,7 @@ config KEYBOARD_TCA8418
 config KEYBOARD_MATRIX
 	tristate "GPIO driven matrix keypad support"
 	depends on GENERIC_GPIO
+	select INPUT_MATRIXKMAP
 	help
 	  Enable support for GPIO driven matrix keypad.
 
@@ -309,6 +312,17 @@ config KEYBOARD_LM8323
 	  To compile this driver as a module, choose M here: the
 	  module will be called lm8323.
 
+config KEYBOARD_LM8333
+	tristate "LM8333 keypad chip"
+	depends on I2C
+	select INPUT_MATRIXKMAP
+	help
+	  If you say yes here you get support for the National Semiconductor
+	  LM8333 keypad controller.
+
+	  To compile this driver as a module, choose M here: the
+	  module will be called lm8333.
+
 config KEYBOARD_LOCOMO
 	tristate "LoCoMo Keyboard Support"
 	depends on SHARP_LOCOMO
@@ -366,6 +380,7 @@ config KEYBOARD_MPR121
 config KEYBOARD_IMX
 	tristate "IMX keypad support"
 	depends on ARCH_MXC
+	select INPUT_MATRIXKMAP
 	help
 	  Enable support for IMX keypad port.
 
@@ -384,6 +399,7 @@ config KEYBOARD_NEWTON
 config KEYBOARD_NOMADIK
 	tristate "ST-Ericsson Nomadik SKE keyboard"
 	depends on PLAT_NOMADIK
+	select INPUT_MATRIXKMAP
 	help
 	  Say Y here if you want to use a keypad provided on the SKE controller
 	  used on the Ux500 and Nomadik platforms
@@ -394,7 +410,7 @@ config KEYBOARD_NOMADIK
 config KEYBOARD_TEGRA
 	tristate "NVIDIA Tegra internal matrix keyboard controller support"
 	depends on ARCH_TEGRA
-	select INPUT_OF_MATRIX_KEYMAP if USE_OF
+	select INPUT_MATRIXKMAP
 	help
 	  Say Y here if you want to use a matrix keyboard connected directly
 	  to the internal keyboard controller on Tegra SoCs.
@@ -432,6 +448,7 @@ config KEYBOARD_PXA930_ROTARY
 config KEYBOARD_PMIC8XXX
 	tristate "Qualcomm PMIC8XXX keypad support"
 	depends on MFD_PM8XXX
+	select INPUT_MATRIXKMAP
 	help
 	  Say Y here if you want to enable the driver for the PMIC8XXX
 	  keypad provided as a reference design from Qualcomm. This is intended
@@ -443,6 +460,7 @@ config KEYBOARD_PMIC8XXX
 config KEYBOARD_SAMSUNG
 	tristate "Samsung keypad support"
 	depends on HAVE_CLK
+	select INPUT_MATRIXKMAP
 	help
 	  Say Y here if you want to use the keypad on your Samsung mobile
 	  device.
@@ -485,6 +503,7 @@ config KEYBOARD_SH_KEYSC
 config KEYBOARD_STMPE
 	tristate "STMPE keypad support"
 	depends on MFD_STMPE
+	select INPUT_MATRIXKMAP
 	help
 	  Say Y here if you want to use the keypad controller on STMPE I/O
 	  expanders.
@@ -505,6 +524,7 @@ config KEYBOARD_DAVINCI
 config KEYBOARD_OMAP
 	tristate "TI OMAP keypad support"
 	depends on (ARCH_OMAP1 || ARCH_OMAP2)
+	select INPUT_MATRIXKMAP
 	help
 	  Say Y here if you want to use the OMAP keypad.
 
@@ -512,9 +532,10 @@ config KEYBOARD_OMAP
 	  module will be called omap-keypad.
 
 config KEYBOARD_OMAP4
-	tristate "TI OMAP4 keypad support"
+	tristate "TI OMAP4+ keypad support"
+	select INPUT_MATRIXKMAP
 	help
-	  Say Y here if you want to use the OMAP4 keypad.
+	  Say Y here if you want to use the OMAP4+ keypad.
 
 	  To compile this driver as a module, choose M here: the
 	  module will be called omap4-keypad.
@@ -522,6 +543,7 @@ config KEYBOARD_OMAP4
 config KEYBOARD_SPEAR
 	tristate "ST SPEAR keyboard support"
 	depends on PLAT_SPEAR
+	select INPUT_MATRIXKMAP
 	help
 	  Say Y here if you want to use the SPEAR keyboard.
 
@@ -531,6 +553,7 @@ config KEYBOARD_SPEAR
 config KEYBOARD_TC3589X
 	tristate "TC3589X Keypad support"
 	depends on MFD_TC3589X
+	select INPUT_MATRIXKMAP
 	help
 	  Say Y here if you want to use the keypad controller on
 	  TC35892/3 I/O expander.
@@ -541,6 +564,7 @@ config KEYBOARD_TC3589X
 config KEYBOARD_TNETV107X
 	tristate "TI TNETV107X keypad support"
 	depends on ARCH_DAVINCI_TNETV107X
+	select INPUT_MATRIXKMAP
 	help
 	  Say Y here if you want to use the TNETV107X keypad.
 
@@ -550,6 +574,7 @@ config KEYBOARD_TNETV107X
 config KEYBOARD_TWL4030
 	tristate "TI TWL4030/TWL5030/TPS659x0 keypad support"
 	depends on TWL4030_CORE
+	select INPUT_MATRIXKMAP
 	help
 	  Say Y here if your board use the keypad controller on
 	  TWL4030 family chips.  It's safe to say enable this
@@ -573,6 +598,7 @@ config KEYBOARD_XTKBD
 config KEYBOARD_W90P910
 	tristate "W90P910 Matrix Keypad support"
 	depends on ARCH_W90X900
+	select INPUT_MATRIXKMAP
 	help
 	  Say Y here to enable the matrix keypad on evaluation board
 	  based on W90P910.
diff --git a/drivers/input/keyboard/Makefile b/drivers/input/keyboard/Makefile
index df7061f1291..b03b02456a8 100644
--- a/drivers/input/keyboard/Makefile
+++ b/drivers/input/keyboard/Makefile
@@ -24,6 +24,7 @@ obj-$(CONFIG_KEYBOARD_HP6XX)		+= jornada680_kbd.o
 obj-$(CONFIG_KEYBOARD_HP7XX)		+= jornada720_kbd.o
 obj-$(CONFIG_KEYBOARD_LKKBD)		+= lkkbd.o
 obj-$(CONFIG_KEYBOARD_LM8323)		+= lm8323.o
+obj-$(CONFIG_KEYBOARD_LM8333)		+= lm8333.o
 obj-$(CONFIG_KEYBOARD_LOCOMO)		+= locomokbd.o
 obj-$(CONFIG_KEYBOARD_MAPLE)		+= maple_keyb.o
 obj-$(CONFIG_KEYBOARD_MATRIX)		+= matrix_keypad.o
diff --git a/drivers/input/keyboard/adp5588-keys.c b/drivers/input/keyboard/adp5588-keys.c
index 39ebffac207..b083bf10f13 100644
--- a/drivers/input/keyboard/adp5588-keys.c
+++ b/drivers/input/keyboard/adp5588-keys.c
@@ -197,6 +197,7 @@ static int __devinit adp5588_gpio_add(struct adp5588_kpad *kpad)
 	kpad->gc.base = gpio_data->gpio_start;
 	kpad->gc.label = kpad->client->name;
 	kpad->gc.owner = THIS_MODULE;
+	kpad->gc.names = gpio_data->names;
 
 	mutex_init(&kpad->gpio_lock);
 
diff --git a/drivers/input/keyboard/atkbd.c b/drivers/input/keyboard/atkbd.c
index e05a2e7073c..add5ffd9fe2 100644
--- a/drivers/input/keyboard/atkbd.c
+++ b/drivers/input/keyboard/atkbd.c
@@ -433,7 +433,7 @@ static irqreturn_t atkbd_interrupt(struct serio *serio, unsigned char data,
 		if (printk_ratelimit())
 			dev_warn(&serio->dev,
 				 "Spurious %s on %s. "
-				 "Some program might be trying access hardware directly.\n",
+				 "Some program might be trying to access hardware directly.\n",
 				 data == ATKBD_RET_ACK ? "ACK" : "NAK", serio->phys);
 		goto out;
 	case ATKBD_RET_ERR:
diff --git a/drivers/input/keyboard/ep93xx_keypad.c b/drivers/input/keyboard/ep93xx_keypad.c
index 0ba69f3fcb5..c46fc818546 100644
--- a/drivers/input/keyboard/ep93xx_keypad.c
+++ b/drivers/input/keyboard/ep93xx_keypad.c
@@ -182,16 +182,10 @@ static void ep93xx_keypad_close(struct input_dev *pdev)
 }
 
 
-#ifdef CONFIG_PM
-/*
- * NOTE: I don't know if this is correct, or will work on the ep93xx.
- *
- * None of the existing ep93xx drivers have power management support.
- * But, this is basically what the pxa27x_keypad driver does.
- */
-static int ep93xx_keypad_suspend(struct platform_device *pdev,
-				 pm_message_t state)
+#ifdef CONFIG_PM_SLEEP
+static int ep93xx_keypad_suspend(struct device *dev)
 {
+	struct platform_device *pdev = to_platform_device(dev);
 	struct ep93xx_keypad *keypad = platform_get_drvdata(pdev);
 	struct input_dev *input_dev = keypad->input_dev;
 
@@ -210,8 +204,9 @@ static int ep93xx_keypad_suspend(struct platform_device *pdev,
 	return 0;
 }
 
-static int ep93xx_keypad_resume(struct platform_device *pdev)
+static int ep93xx_keypad_resume(struct device *dev)
 {
+	struct platform_device *pdev = to_platform_device(dev);
 	struct ep93xx_keypad *keypad = platform_get_drvdata(pdev);
 	struct input_dev *input_dev = keypad->input_dev;
 
@@ -232,10 +227,10 @@ static int ep93xx_keypad_resume(struct platform_device *pdev)
 
 	return 0;
 }
-#else	/* !CONFIG_PM */
-#define ep93xx_keypad_suspend	NULL
-#define ep93xx_keypad_resume	NULL
-#endif	/* !CONFIG_PM */
+#endif
+
+static SIMPLE_DEV_PM_OPS(ep93xx_keypad_pm_ops,
+			 ep93xx_keypad_suspend, ep93xx_keypad_resume);
 
 static int __devinit ep93xx_keypad_probe(struct platform_device *pdev)
 {
@@ -308,19 +303,16 @@ static int __devinit ep93xx_keypad_probe(struct platform_device *pdev)
 	input_dev->open = ep93xx_keypad_open;
 	input_dev->close = ep93xx_keypad_close;
 	input_dev->dev.parent = &pdev->dev;
-	input_dev->keycode = keypad->keycodes;
-	input_dev->keycodesize = sizeof(keypad->keycodes[0]);
-	input_dev->keycodemax = ARRAY_SIZE(keypad->keycodes);
 
-	input_set_drvdata(input_dev, keypad);
+	err = matrix_keypad_build_keymap(keymap_data, NULL,
+					 EP93XX_MATRIX_ROWS, EP93XX_MATRIX_COLS,
+					 keypad->keycodes, input_dev);
+	if (err)
+		goto failed_free_dev;
 
-	input_dev->evbit[0] = BIT_MASK(EV_KEY);
 	if (keypad->pdata->flags & EP93XX_KEYPAD_AUTOREPEAT)
-		input_dev->evbit[0] |= BIT_MASK(EV_REP);
-
-	matrix_keypad_build_keymap(keymap_data, 3,
-				   input_dev->keycode, input_dev->keybit);
-	platform_set_drvdata(pdev, keypad);
+		__set_bit(EV_REP, input_dev->evbit);
+	input_set_drvdata(input_dev, keypad);
 
 	err = request_irq(keypad->irq, ep93xx_keypad_irq_handler,
 			  0, pdev->name, keypad);
@@ -331,6 +323,7 @@ static int __devinit ep93xx_keypad_probe(struct platform_device *pdev)
 	if (err)
 		goto failed_free_irq;
 
+	platform_set_drvdata(pdev, keypad);
 	device_init_wakeup(&pdev->dev, 1);
 
 	return 0;
@@ -384,11 +377,10 @@ static struct platform_driver ep93xx_keypad_driver = {
 	.driver		= {
 		.name	= "ep93xx-keypad",
 		.owner	= THIS_MODULE,
+		.pm	= &ep93xx_keypad_pm_ops,
 	},
 	.probe		= ep93xx_keypad_probe,
 	.remove		= __devexit_p(ep93xx_keypad_remove),
-	.suspend	= ep93xx_keypad_suspend,
-	.resume		= ep93xx_keypad_resume,
 };
 module_platform_driver(ep93xx_keypad_driver);
 
diff --git a/drivers/input/keyboard/hil_kbd.c b/drivers/input/keyboard/hil_kbd.c
index fed31e0947a..589e3c258f3 100644
--- a/drivers/input/keyboard/hil_kbd.c
+++ b/drivers/input/keyboard/hil_kbd.c
@@ -583,15 +583,4 @@ static struct serio_driver hil_serio_drv = {
 	.interrupt	= hil_dev_interrupt
 };
 
-static int __init hil_dev_init(void)
-{
-	return serio_register_driver(&hil_serio_drv);
-}
-
-static void __exit hil_dev_exit(void)
-{
-	serio_unregister_driver(&hil_serio_drv);
-}
-
-module_init(hil_dev_init);
-module_exit(hil_dev_exit);
+module_serio_driver(hil_serio_drv);
diff --git a/drivers/input/keyboard/imx_keypad.c b/drivers/input/keyboard/imx_keypad.c
index fb87b3bcadb..6ee7421e232 100644
--- a/drivers/input/keyboard/imx_keypad.c
+++ b/drivers/input/keyboard/imx_keypad.c
@@ -481,7 +481,7 @@ static int __devinit imx_keypad_probe(struct platform_device *pdev)
 	}
 
 	if (keypad->rows_en_mask > ((1 << MAX_MATRIX_KEY_ROWS) - 1) ||
-	   keypad->cols_en_mask > ((1 << MAX_MATRIX_KEY_COLS) - 1)) {
+	    keypad->cols_en_mask > ((1 << MAX_MATRIX_KEY_COLS) - 1)) {
 		dev_err(&pdev->dev,
 			"invalid key data (too many rows or colums)\n");
 		error = -EINVAL;
@@ -496,14 +496,17 @@ static int __devinit imx_keypad_probe(struct platform_device *pdev)
 	input_dev->dev.parent = &pdev->dev;
 	input_dev->open = imx_keypad_open;
 	input_dev->close = imx_keypad_close;
-	input_dev->evbit[0] = BIT_MASK(EV_KEY) | BIT_MASK(EV_REP);
-	input_dev->keycode = keypad->keycodes;
-	input_dev->keycodesize = sizeof(keypad->keycodes[0]);
-	input_dev->keycodemax = ARRAY_SIZE(keypad->keycodes);
 
-	matrix_keypad_build_keymap(keymap_data, MATRIX_ROW_SHIFT,
-				keypad->keycodes, input_dev->keybit);
+	error = matrix_keypad_build_keymap(keymap_data, NULL,
+					   MAX_MATRIX_KEY_ROWS,
+					   MAX_MATRIX_KEY_COLS,
+					   keypad->keycodes, input_dev);
+	if (error) {
+		dev_err(&pdev->dev, "failed to build keymap\n");
+		goto failed_clock_put;
+	}
 
+	__set_bit(EV_REP, input_dev->evbit);
 	input_set_capability(input_dev, EV_MSC, MSC_SCAN);
 	input_set_drvdata(input_dev, keypad);
 
diff --git a/drivers/input/keyboard/lkkbd.c b/drivers/input/keyboard/lkkbd.c
index fa9bb6d235e..fc0a63c2f27 100644
--- a/drivers/input/keyboard/lkkbd.c
+++ b/drivers/input/keyboard/lkkbd.c
@@ -731,19 +731,4 @@ static struct serio_driver lkkbd_drv = {
 	.interrupt	= lkkbd_interrupt,
 };
 
-/*
- * The functions for insering/removing us as a module.
- */
-static int __init lkkbd_init(void)
-{
-	return serio_register_driver(&lkkbd_drv);
-}
-
-static void __exit lkkbd_exit(void)
-{
-	serio_unregister_driver(&lkkbd_drv);
-}
-
-module_init(lkkbd_init);
-module_exit(lkkbd_exit);
-
+module_serio_driver(lkkbd_drv);
diff --git a/drivers/input/keyboard/lm8333.c b/drivers/input/keyboard/lm8333.c
new file mode 100644
index 00000000000..ca168a6679d
--- /dev/null
+++ b/drivers/input/keyboard/lm8333.c
@@ -0,0 +1,235 @@
+/*
+ * LM8333 keypad driver
+ * Copyright (C) 2012 Wolfram Sang, Pengutronix <w.sang@pengutronix.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.
+ */
+
+#include <linux/module.h>
+#include <linux/slab.h>
+#include <linux/irq.h>
+#include <linux/i2c.h>
+#include <linux/interrupt.h>
+#include <linux/input/matrix_keypad.h>
+#include <linux/input/lm8333.h>
+
+#define LM8333_FIFO_READ		0x20
+#define LM8333_DEBOUNCE			0x22
+#define LM8333_READ_INT			0xD0
+#define LM8333_ACTIVE			0xE4
+#define LM8333_READ_ERROR		0xF0
+
+#define LM8333_KEYPAD_IRQ		(1 << 0)
+#define LM8333_ERROR_IRQ		(1 << 3)
+
+#define LM8333_ERROR_KEYOVR		0x04
+#define LM8333_ERROR_FIFOOVR		0x40
+
+#define LM8333_FIFO_TRANSFER_SIZE	16
+
+#define LM8333_NUM_ROWS		8
+#define LM8333_NUM_COLS		16
+#define LM8333_ROW_SHIFT	4
+
+struct lm8333 {
+	struct i2c_client *client;
+	struct input_dev *input;
+	unsigned short keycodes[LM8333_NUM_ROWS << LM8333_ROW_SHIFT];
+};
+
+/* The accessors try twice because the first access may be needed for wakeup */
+#define LM8333_READ_RETRIES 2
+
+int lm8333_read8(struct lm8333 *lm8333, u8 cmd)
+{
+	int retries = 0, ret;
+
+	do {
+		ret = i2c_smbus_read_byte_data(lm8333->client, cmd);
+	} while (ret < 0 && retries++ < LM8333_READ_RETRIES);
+
+	return ret;
+}
+
+int lm8333_write8(struct lm8333 *lm8333, u8 cmd, u8 val)
+{
+	int retries = 0, ret;
+
+	do {
+		ret = i2c_smbus_write_byte_data(lm8333->client, cmd, val);
+	} while (ret < 0 && retries++ < LM8333_READ_RETRIES);
+
+	return ret;
+}
+
+int lm8333_read_block(struct lm8333 *lm8333, u8 cmd, u8 len, u8 *buf)
+{
+	int retries = 0, ret;
+
+	do {
+		ret = i2c_smbus_read_i2c_block_data(lm8333->client,
+						    cmd, len, buf);
+	} while (ret < 0 && retries++ < LM8333_READ_RETRIES);
+
+	return ret;
+}
+
+static void lm8333_key_handler(struct lm8333 *lm8333)
+{
+	struct input_dev *input = lm8333->input;
+	u8 keys[LM8333_FIFO_TRANSFER_SIZE];
+	u8 code, pressed;
+	int i, ret;
+
+	ret = lm8333_read_block(lm8333, LM8333_FIFO_READ,
+				LM8333_FIFO_TRANSFER_SIZE, keys);
+	if (ret != LM8333_FIFO_TRANSFER_SIZE) {
+		dev_err(&lm8333->client->dev,
+			"Error %d while reading FIFO\n", ret);
+		return;
+	}
+
+	for (i = 0; keys[i] && i < LM8333_FIFO_TRANSFER_SIZE; i++) {
+		pressed = keys[i] & 0x80;
+		code = keys[i] & 0x7f;
+
+		input_event(input, EV_MSC, MSC_SCAN, code);
+		input_report_key(input, lm8333->keycodes[code], pressed);
+	}
+
+	input_sync(input);
+}
+
+static irqreturn_t lm8333_irq_thread(int irq, void *data)
+{
+	struct lm8333 *lm8333 = data;
+	u8 status = lm8333_read8(lm8333, LM8333_READ_INT);
+
+	if (!status)
+		return IRQ_NONE;
+
+	if (status & LM8333_ERROR_IRQ) {
+		u8 err = lm8333_read8(lm8333, LM8333_READ_ERROR);
+
+		if (err & (LM8333_ERROR_KEYOVR | LM8333_ERROR_FIFOOVR)) {
+			u8 dummy[LM8333_FIFO_TRANSFER_SIZE];
+
+			lm8333_read_block(lm8333, LM8333_FIFO_READ,
+					LM8333_FIFO_TRANSFER_SIZE, dummy);
+		}
+		dev_err(&lm8333->client->dev, "Got error %02x\n", err);
+	}
+
+	if (status & LM8333_KEYPAD_IRQ)
+		lm8333_key_handler(lm8333);
+
+	return IRQ_HANDLED;
+}
+
+static int __devinit lm8333_probe(struct i2c_client *client,
+				  const struct i2c_device_id *id)
+{
+	const struct lm8333_platform_data *pdata = client->dev.platform_data;
+	struct lm8333 *lm8333;
+	struct input_dev *input;
+	int err, active_time;
+
+	if (!pdata)
+		return -EINVAL;
+
+	active_time = pdata->active_time ?: 500;
+	if (active_time / 3 <= pdata->debounce_time / 3) {
+		dev_err(&client->dev, "Active time not big enough!\n");
+		return -EINVAL;
+	}
+
+	lm8333 = kzalloc(sizeof(*lm8333), GFP_KERNEL);
+	input = input_allocate_device();
+	if (!lm8333 || !input) {
+		err = -ENOMEM;
+		goto free_mem;
+	}
+
+	lm8333->client = client;
+	lm8333->input = input;
+
+	input->name = client->name;
+	input->dev.parent = &client->dev;
+	input->id.bustype = BUS_I2C;
+
+	input_set_capability(input, EV_MSC, MSC_SCAN);
+
+	err = matrix_keypad_build_keymap(pdata->matrix_data, NULL,
+					 LM8333_NUM_ROWS, LM8333_NUM_COLS,
+					 lm8333->keycodes, input);
+	if (err)
+		goto free_mem;
+
+	if (pdata->debounce_time) {
+		err = lm8333_write8(lm8333, LM8333_DEBOUNCE,
+				    pdata->debounce_time / 3);
+		if (err)
+			dev_warn(&client->dev, "Unable to set debounce time\n");
+	}
+
+	if (pdata->active_time) {
+		err = lm8333_write8(lm8333, LM8333_ACTIVE,
+				    pdata->active_time / 3);
+		if (err)
+			dev_warn(&client->dev, "Unable to set active time\n");
+	}
+
+	err = request_threaded_irq(client->irq, NULL, lm8333_irq_thread,
+				   IRQF_TRIGGER_FALLING | IRQF_ONESHOT,
+				   "lm8333", lm8333);
+	if (err)
+		goto free_mem;
+
+	err = input_register_device(input);
+	if (err)
+		goto free_irq;
+
+	i2c_set_clientdata(client, lm8333);
+	return 0;
+
+ free_irq:
+	free_irq(client->irq, lm8333);
+ free_mem:
+	input_free_device(input);
+	kfree(lm8333);
+	return err;
+}
+
+static int __devexit lm8333_remove(struct i2c_client *client)
+{
+	struct lm8333 *lm8333 = i2c_get_clientdata(client);
+
+	free_irq(client->irq, lm8333);
+	input_unregister_device(lm8333->input);
+	kfree(lm8333);
+
+	return 0;
+}
+
+static const struct i2c_device_id lm8333_id[] = {
+	{ "lm8333", 0 },
+	{ }
+};
+MODULE_DEVICE_TABLE(i2c, lm8333_id);
+
+static struct i2c_driver lm8333_driver = {
+	.driver = {
+		.name		= "lm8333",
+		.owner		= THIS_MODULE,
+	},
+	.probe		= lm8333_probe,
+	.remove		= __devexit_p(lm8333_remove),
+	.id_table	= lm8333_id,
+};
+module_i2c_driver(lm8333_driver);
+
+MODULE_AUTHOR("Wolfram Sang <w.sang@pengutronix.de>");
+MODULE_DESCRIPTION("LM8333 keyboard driver");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/input/keyboard/matrix_keypad.c b/drivers/input/keyboard/matrix_keypad.c
index 9b223d73de3..18b72372028 100644
--- a/drivers/input/keyboard/matrix_keypad.c
+++ b/drivers/input/keyboard/matrix_keypad.c
@@ -27,7 +27,6 @@
 struct matrix_keypad {
 	const struct matrix_keypad_platform_data *pdata;
 	struct input_dev *input_dev;
-	unsigned short *keycodes;
 	unsigned int row_shift;
 
 	DECLARE_BITMAP(disabled_gpios, MATRIX_MAX_ROWS);
@@ -38,6 +37,8 @@ struct matrix_keypad {
 	bool scan_pending;
 	bool stopped;
 	bool gpio_all_disabled;
+
+	unsigned short keycodes[];
 };
 
 /*
@@ -224,7 +225,7 @@ static void matrix_keypad_stop(struct input_dev *dev)
 	disable_row_irqs(keypad);
 }
 
-#ifdef CONFIG_PM
+#ifdef CONFIG_PM_SLEEP
 static void matrix_keypad_enable_wakeup(struct matrix_keypad *keypad)
 {
 	const struct matrix_keypad_platform_data *pdata = keypad->pdata;
@@ -293,16 +294,16 @@ static int matrix_keypad_resume(struct device *dev)
 
 	return 0;
 }
-
-static const SIMPLE_DEV_PM_OPS(matrix_keypad_pm_ops,
-				matrix_keypad_suspend, matrix_keypad_resume);
 #endif
 
-static int __devinit init_matrix_gpio(struct platform_device *pdev,
-					struct matrix_keypad *keypad)
+static SIMPLE_DEV_PM_OPS(matrix_keypad_pm_ops,
+			 matrix_keypad_suspend, matrix_keypad_resume);
+
+static int __devinit matrix_keypad_init_gpio(struct platform_device *pdev,
+					     struct matrix_keypad *keypad)
 {
 	const struct matrix_keypad_platform_data *pdata = keypad->pdata;
-	int i, err = -EINVAL;
+	int i, err;
 
 	/* initialized strobe lines as outputs, activated */
 	for (i = 0; i < pdata->num_col_gpios; i++) {
@@ -348,8 +349,7 @@ static int __devinit init_matrix_gpio(struct platform_device *pdev,
 					"matrix-keypad", keypad);
 			if (err) {
 				dev_err(&pdev->dev,
-					"Unable to acquire interrupt "
-					"for GPIO line %i\n",
+					"Unable to acquire interrupt for GPIO line %i\n",
 					pdata->row_gpios[i]);
 				goto err_free_irqs;
 			}
@@ -375,14 +375,33 @@ err_free_cols:
 	return err;
 }
 
+static void matrix_keypad_free_gpio(struct matrix_keypad *keypad)
+{
+	const struct matrix_keypad_platform_data *pdata = keypad->pdata;
+	int i;
+
+	if (pdata->clustered_irq > 0) {
+		free_irq(pdata->clustered_irq, keypad);
+	} else {
+		for (i = 0; i < pdata->num_row_gpios; i++)
+			free_irq(gpio_to_irq(pdata->row_gpios[i]), keypad);
+	}
+
+	for (i = 0; i < pdata->num_row_gpios; i++)
+		gpio_free(pdata->row_gpios[i]);
+
+	for (i = 0; i < pdata->num_col_gpios; i++)
+		gpio_free(pdata->col_gpios[i]);
+}
+
 static int __devinit matrix_keypad_probe(struct platform_device *pdev)
 {
 	const struct matrix_keypad_platform_data *pdata;
 	const struct matrix_keymap_data *keymap_data;
 	struct matrix_keypad *keypad;
 	struct input_dev *input_dev;
-	unsigned short *keycodes;
 	unsigned int row_shift;
+	size_t keymap_size;
 	int err;
 
 	pdata = pdev->dev.platform_data;
@@ -398,20 +417,18 @@ static int __devinit matrix_keypad_probe(struct platform_device *pdev)
 	}
 
 	row_shift = get_count_order(pdata->num_col_gpios);
-
-	keypad = kzalloc(sizeof(struct matrix_keypad), GFP_KERNEL);
-	keycodes = kzalloc((pdata->num_row_gpios << row_shift) *
-				sizeof(*keycodes),
-			   GFP_KERNEL);
+	keymap_size = (pdata->num_row_gpios << row_shift) *
+			sizeof(keypad->keycodes[0]);
+	keypad = kzalloc(sizeof(struct matrix_keypad) + keymap_size,
+			 GFP_KERNEL);
 	input_dev = input_allocate_device();
-	if (!keypad || !keycodes || !input_dev) {
+	if (!keypad || !input_dev) {
 		err = -ENOMEM;
 		goto err_free_mem;
 	}
 
 	keypad->input_dev = input_dev;
 	keypad->pdata = pdata;
-	keypad->keycodes = keycodes;
 	keypad->row_shift = row_shift;
 	keypad->stopped = true;
 	INIT_DELAYED_WORK(&keypad->work, matrix_keypad_scan);
@@ -420,38 +437,38 @@ static int __devinit matrix_keypad_probe(struct platform_device *pdev)
 	input_dev->name		= pdev->name;
 	input_dev->id.bustype	= BUS_HOST;
 	input_dev->dev.parent	= &pdev->dev;
-	input_dev->evbit[0]	= BIT_MASK(EV_KEY);
-	if (!pdata->no_autorepeat)
-		input_dev->evbit[0] |= BIT_MASK(EV_REP);
 	input_dev->open		= matrix_keypad_start;
 	input_dev->close	= matrix_keypad_stop;
 
-	input_dev->keycode	= keycodes;
-	input_dev->keycodesize	= sizeof(*keycodes);
-	input_dev->keycodemax	= pdata->num_row_gpios << row_shift;
-
-	matrix_keypad_build_keymap(keymap_data, row_shift,
-				   input_dev->keycode, input_dev->keybit);
+	err = matrix_keypad_build_keymap(keymap_data, NULL,
+					 pdata->num_row_gpios,
+					 pdata->num_col_gpios,
+					 keypad->keycodes, input_dev);
+	if (err)
+		goto err_free_mem;
 
+	if (!pdata->no_autorepeat)
+		__set_bit(EV_REP, input_dev->evbit);
 	input_set_capability(input_dev, EV_MSC, MSC_SCAN);
 	input_set_drvdata(input_dev, keypad);
 
-	err = init_matrix_gpio(pdev, keypad);
+	err = matrix_keypad_init_gpio(pdev, keypad);
 	if (err)
 		goto err_free_mem;
 
 	err = input_register_device(keypad->input_dev);
 	if (err)
-		goto err_free_mem;
+		goto err_free_gpio;
 
 	device_init_wakeup(&pdev->dev, pdata->wakeup);
 	platform_set_drvdata(pdev, keypad);
 
 	return 0;
 
+err_free_gpio:
+	matrix_keypad_free_gpio(keypad);
 err_free_mem:
 	input_free_device(input_dev);
-	kfree(keycodes);
 	kfree(keypad);
 	return err;
 }
@@ -459,29 +476,15 @@ err_free_mem:
 static int __devexit matrix_keypad_remove(struct platform_device *pdev)
 {
 	struct matrix_keypad *keypad = platform_get_drvdata(pdev);
-	const struct matrix_keypad_platform_data *pdata = keypad->pdata;
-	int i;
 
 	device_init_wakeup(&pdev->dev, 0);
 
-	if (pdata->clustered_irq > 0) {
-		free_irq(pdata->clustered_irq, keypad);
-	} else {
-		for (i = 0; i < pdata->num_row_gpios; i++)
-			free_irq(gpio_to_irq(pdata->row_gpios[i]), keypad);
-	}
-
-	for (i = 0; i < pdata->num_row_gpios; i++)
-		gpio_free(pdata->row_gpios[i]);
-
-	for (i = 0; i < pdata->num_col_gpios; i++)
-		gpio_free(pdata->col_gpios[i]);
-
+	matrix_keypad_free_gpio(keypad);
 	input_unregister_device(keypad->input_dev);
-	platform_set_drvdata(pdev, NULL);
-	kfree(keypad->keycodes);
 	kfree(keypad);
 
+	platform_set_drvdata(pdev, NULL);
+
 	return 0;
 }
 
@@ -491,9 +494,7 @@ static struct platform_driver matrix_keypad_driver = {
 	.driver		= {
 		.name	= "matrix-keypad",
 		.owner	= THIS_MODULE,
-#ifdef CONFIG_PM
 		.pm	= &matrix_keypad_pm_ops,
-#endif
 	},
 };
 module_platform_driver(matrix_keypad_driver);
diff --git a/drivers/input/keyboard/newtonkbd.c b/drivers/input/keyboard/newtonkbd.c
index 48d1cab0aa1..f971898ad59 100644
--- a/drivers/input/keyboard/newtonkbd.c
+++ b/drivers/input/keyboard/newtonkbd.c
@@ -166,15 +166,4 @@ static struct serio_driver nkbd_drv = {
 	.disconnect	= nkbd_disconnect,
 };
 
-static int __init nkbd_init(void)
-{
-	return serio_register_driver(&nkbd_drv);
-}
-
-static void __exit nkbd_exit(void)
-{
-	serio_unregister_driver(&nkbd_drv);
-}
-
-module_init(nkbd_init);
-module_exit(nkbd_exit);
+module_serio_driver(nkbd_drv);
diff --git a/drivers/input/keyboard/nomadik-ske-keypad.c b/drivers/input/keyboard/nomadik-ske-keypad.c
index 101e245944e..4ea4341a68c 100644
--- a/drivers/input/keyboard/nomadik-ske-keypad.c
+++ b/drivers/input/keyboard/nomadik-ske-keypad.c
@@ -39,7 +39,8 @@
 #define SKE_KPRISA	(0x1 << 2)
 
 #define SKE_KEYPAD_ROW_SHIFT	3
-#define SKE_KPD_KEYMAP_SIZE	(8 * 8)
+#define SKE_KPD_NUM_ROWS	8
+#define SKE_KPD_NUM_COLS	8
 
 /* keypad auto scan registers */
 #define SKE_ASR0	0x20
@@ -63,7 +64,7 @@ struct ske_keypad {
 	void __iomem *reg_base;
 	struct input_dev *input;
 	const struct ske_keypad_platform_data *board;
-	unsigned short keymap[SKE_KPD_KEYMAP_SIZE];
+	unsigned short keymap[SKE_KPD_NUM_ROWS * SKE_KPD_NUM_COLS];
 	struct clk *clk;
 	spinlock_t ske_keypad_lock;
 };
@@ -261,19 +262,18 @@ static int __init ske_keypad_probe(struct platform_device *pdev)
 	input->name = "ux500-ske-keypad";
 	input->dev.parent = &pdev->dev;
 
-	input->keycode = keypad->keymap;
-	input->keycodesize = sizeof(keypad->keymap[0]);
-	input->keycodemax = ARRAY_SIZE(keypad->keymap);
+	error = matrix_keypad_build_keymap(plat->keymap_data, NULL,
+					   SKE_KPD_NUM_ROWS, SKE_KPD_NUM_COLS,
+					   keypad->keymap, input);
+	if (error) {
+		dev_err(&pdev->dev, "Failed to build keymap\n");
+		goto err_iounmap;
+	}
 
 	input_set_capability(input, EV_MSC, MSC_SCAN);
-
-	__set_bit(EV_KEY, input->evbit);
 	if (!plat->no_autorepeat)
 		__set_bit(EV_REP, input->evbit);
 
-	matrix_keypad_build_keymap(plat->keymap_data, SKE_KEYPAD_ROW_SHIFT,
-			input->keycode, input->keybit);
-
 	clk_enable(keypad->clk);
 
 	/* go through board initialization helpers */
diff --git a/drivers/input/keyboard/omap-keypad.c b/drivers/input/keyboard/omap-keypad.c
index 6b630d9d3df..a0222db4dc8 100644
--- a/drivers/input/keyboard/omap-keypad.c
+++ b/drivers/input/keyboard/omap-keypad.c
@@ -61,6 +61,7 @@ struct omap_kp {
 	unsigned int cols;
 	unsigned long delay;
 	unsigned int debounce;
+	unsigned short keymap[];
 };
 
 static DECLARE_TASKLET_DISABLED(kp_tasklet, omap_kp_tasklet, 0);
@@ -316,13 +317,6 @@ static int __devinit omap_kp_probe(struct platform_device *pdev)
 	if (!cpu_is_omap24xx())
 		omap_writew(1, OMAP1_MPUIO_BASE + OMAP_MPUIO_KBD_MASKIT);
 
-	input_dev->keycode      = &omap_kp[1];
-	input_dev->keycodesize  = sizeof(unsigned short);
-	input_dev->keycodemax   = keycodemax;
-
-	if (pdata->rep)
-		__set_bit(EV_REP, input_dev->evbit);
-
 	if (pdata->delay)
 		omap_kp->delay = pdata->delay;
 
@@ -371,9 +365,6 @@ static int __devinit omap_kp_probe(struct platform_device *pdev)
 		goto err2;
 
 	/* setup input device */
-	__set_bit(EV_KEY, input_dev->evbit);
-	matrix_keypad_build_keymap(pdata->keymap_data, row_shift,
-			input_dev->keycode, input_dev->keybit);
 	input_dev->name = "omap-keypad";
 	input_dev->phys = "omap-keypad/input0";
 	input_dev->dev.parent = &pdev->dev;
@@ -383,6 +374,15 @@ static int __devinit omap_kp_probe(struct platform_device *pdev)
 	input_dev->id.product = 0x0001;
 	input_dev->id.version = 0x0100;
 
+	if (pdata->rep)
+		__set_bit(EV_REP, input_dev->evbit);
+
+	ret = matrix_keypad_build_keymap(pdata->keymap_data, NULL,
+					 pdata->rows, pdata->cols,
+					 omap_kp->keymap, input_dev);
+	if (ret < 0)
+		goto err3;
+
 	ret = input_register_device(omap_kp->input);
 	if (ret < 0) {
 		printk(KERN_ERR "Unable to register omap-keypad input device\n");
diff --git a/drivers/input/keyboard/omap4-keypad.c b/drivers/input/keyboard/omap4-keypad.c
index e809ac095a3..aed5f6999ce 100644
--- a/drivers/input/keyboard/omap4-keypad.c
+++ b/drivers/input/keyboard/omap4-keypad.c
@@ -68,19 +68,52 @@
 
 #define OMAP4_MASK_IRQSTATUSDISABLE	0xFFFF
 
+enum {
+	KBD_REVISION_OMAP4 = 0,
+	KBD_REVISION_OMAP5,
+};
+
 struct omap4_keypad {
 	struct input_dev *input;
 
 	void __iomem *base;
-	int irq;
+	unsigned int irq;
 
 	unsigned int rows;
 	unsigned int cols;
+	u32 reg_offset;
+	u32 irqreg_offset;
 	unsigned int row_shift;
 	unsigned char key_state[8];
 	unsigned short keymap[];
 };
 
+static int kbd_readl(struct omap4_keypad *keypad_data, u32 offset)
+{
+	return __raw_readl(keypad_data->base +
+				keypad_data->reg_offset + offset);
+}
+
+static void kbd_writel(struct omap4_keypad *keypad_data, u32 offset, u32 value)
+{
+	__raw_writel(value,
+		     keypad_data->base + keypad_data->reg_offset + offset);
+}
+
+static int kbd_read_irqreg(struct omap4_keypad *keypad_data, u32 offset)
+{
+	return __raw_readl(keypad_data->base +
+				keypad_data->irqreg_offset + offset);
+}
+
+static void kbd_write_irqreg(struct omap4_keypad *keypad_data,
+			     u32 offset, u32 value)
+{
+	__raw_writel(value,
+		     keypad_data->base + keypad_data->irqreg_offset + offset);
+}
+
+
 /* Interrupt handler */
 static irqreturn_t omap4_keypad_interrupt(int irq, void *dev_id)
 {
@@ -91,12 +124,11 @@ static irqreturn_t omap4_keypad_interrupt(int irq, void *dev_id)
 	u32 *new_state = (u32 *) key_state;
 
 	/* Disable interrupts */
-	__raw_writel(OMAP4_VAL_IRQDISABLE,
-		     keypad_data->base + OMAP4_KBD_IRQENABLE);
+	kbd_write_irqreg(keypad_data, OMAP4_KBD_IRQENABLE,
+			 OMAP4_VAL_IRQDISABLE);
 
-	*new_state = __raw_readl(keypad_data->base + OMAP4_KBD_FULLCODE31_0);
-	*(new_state + 1) = __raw_readl(keypad_data->base
-						+ OMAP4_KBD_FULLCODE63_32);
+	*new_state = kbd_readl(keypad_data, OMAP4_KBD_FULLCODE31_0);
+	*(new_state + 1) = kbd_readl(keypad_data, OMAP4_KBD_FULLCODE63_32);
 
 	for (row = 0; row < keypad_data->rows; row++) {
 		changed = key_state[row] ^ keypad_data->key_state[row];
@@ -121,12 +153,13 @@ static irqreturn_t omap4_keypad_interrupt(int irq, void *dev_id)
 		sizeof(keypad_data->key_state));
 
 	/* clear pending interrupts */
-	__raw_writel(__raw_readl(keypad_data->base + OMAP4_KBD_IRQSTATUS),
-			keypad_data->base + OMAP4_KBD_IRQSTATUS);
+	kbd_write_irqreg(keypad_data, OMAP4_KBD_IRQSTATUS,
+			 kbd_read_irqreg(keypad_data, OMAP4_KBD_IRQSTATUS));
 
 	/* enable interrupts */
-	__raw_writel(OMAP4_DEF_IRQENABLE_EVENTEN | OMAP4_DEF_IRQENABLE_LONGKEY,
-			keypad_data->base + OMAP4_KBD_IRQENABLE);
+	kbd_write_irqreg(keypad_data, OMAP4_KBD_IRQENABLE,
+		OMAP4_DEF_IRQENABLE_EVENTEN |
+				OMAP4_DEF_IRQENABLE_LONGKEY);
 
 	return IRQ_HANDLED;
 }
@@ -139,16 +172,17 @@ static int omap4_keypad_open(struct input_dev *input)
 
 	disable_irq(keypad_data->irq);
 
-	__raw_writel(OMAP4_VAL_FUNCTIONALCFG,
-			keypad_data->base + OMAP4_KBD_CTRL);
-	__raw_writel(OMAP4_VAL_DEBOUNCINGTIME,
-			keypad_data->base + OMAP4_KBD_DEBOUNCINGTIME);
-	__raw_writel(OMAP4_VAL_IRQDISABLE,
-			keypad_data->base + OMAP4_KBD_IRQSTATUS);
-	__raw_writel(OMAP4_DEF_IRQENABLE_EVENTEN | OMAP4_DEF_IRQENABLE_LONGKEY,
-			keypad_data->base + OMAP4_KBD_IRQENABLE);
-	__raw_writel(OMAP4_DEF_WUP_EVENT_ENA | OMAP4_DEF_WUP_LONG_KEY_ENA,
-			keypad_data->base + OMAP4_KBD_WAKEUPENABLE);
+	kbd_writel(keypad_data, OMAP4_KBD_CTRL,
+			OMAP4_VAL_FUNCTIONALCFG);
+	kbd_writel(keypad_data, OMAP4_KBD_DEBOUNCINGTIME,
+			OMAP4_VAL_DEBOUNCINGTIME);
+	kbd_write_irqreg(keypad_data, OMAP4_KBD_IRQSTATUS,
+			OMAP4_VAL_IRQDISABLE);
+	kbd_write_irqreg(keypad_data, OMAP4_KBD_IRQENABLE,
+			OMAP4_DEF_IRQENABLE_EVENTEN |
+				OMAP4_DEF_IRQENABLE_LONGKEY);
+	kbd_writel(keypad_data, OMAP4_KBD_WAKEUPENABLE,
+			OMAP4_DEF_WUP_EVENT_ENA | OMAP4_DEF_WUP_LONG_KEY_ENA);
 
 	enable_irq(keypad_data->irq);
 
@@ -162,12 +196,12 @@ static void omap4_keypad_close(struct input_dev *input)
 	disable_irq(keypad_data->irq);
 
 	/* Disable interrupts */
-	__raw_writel(OMAP4_VAL_IRQDISABLE,
-		     keypad_data->base + OMAP4_KBD_IRQENABLE);
+	kbd_write_irqreg(keypad_data, OMAP4_KBD_IRQENABLE,
+			 OMAP4_VAL_IRQDISABLE);
 
 	/* clear pending interrupts */
-	__raw_writel(__raw_readl(keypad_data->base + OMAP4_KBD_IRQSTATUS),
-			keypad_data->base + OMAP4_KBD_IRQSTATUS);
+	kbd_write_irqreg(keypad_data, OMAP4_KBD_IRQSTATUS,
+			 kbd_read_irqreg(keypad_data, OMAP4_KBD_IRQSTATUS));
 
 	enable_irq(keypad_data->irq);
 
@@ -182,6 +216,7 @@ static int __devinit omap4_keypad_probe(struct platform_device *pdev)
 	struct resource *res;
 	resource_size_t size;
 	unsigned int row_shift, max_keys;
+	int rev;
 	int irq;
 	int error;
 
@@ -241,11 +276,40 @@ static int __devinit omap4_keypad_probe(struct platform_device *pdev)
 	keypad_data->rows = pdata->rows;
 	keypad_data->cols = pdata->cols;
 
+	/*
+	* Enable clocks for the keypad module so that we can read
+	* revision register.
+	*/
+	pm_runtime_enable(&pdev->dev);
+	error = pm_runtime_get_sync(&pdev->dev);
+	if (error) {
+		dev_err(&pdev->dev, "pm_runtime_get_sync() failed\n");
+		goto err_unmap;
+	}
+	rev = __raw_readl(keypad_data->base + OMAP4_KBD_REVISION);
+	rev &= 0x03 << 30;
+	rev >>= 30;
+	switch (rev) {
+	case KBD_REVISION_OMAP4:
+		keypad_data->reg_offset = 0x00;
+		keypad_data->irqreg_offset = 0x00;
+		break;
+	case KBD_REVISION_OMAP5:
+		keypad_data->reg_offset = 0x10;
+		keypad_data->irqreg_offset = 0x0c;
+		break;
+	default:
+		dev_err(&pdev->dev,
+			"Keypad reports unsupported revision %d", rev);
+		error = -EINVAL;
+		goto err_pm_put_sync;
+	}
+
 	/* input device allocation */
 	keypad_data->input = input_dev = input_allocate_device();
 	if (!input_dev) {
 		error = -ENOMEM;
-		goto err_unmap;
+		goto err_pm_put_sync;
 	}
 
 	input_dev->name = pdev->name;
@@ -258,20 +322,19 @@ static int __devinit omap4_keypad_probe(struct platform_device *pdev)
 	input_dev->open = omap4_keypad_open;
 	input_dev->close = omap4_keypad_close;
 
-	input_dev->keycode	= keypad_data->keymap;
-	input_dev->keycodesize	= sizeof(keypad_data->keymap[0]);
-	input_dev->keycodemax	= max_keys;
+	error = matrix_keypad_build_keymap(pdata->keymap_data, NULL,
+					   pdata->rows, pdata->cols,
+					   keypad_data->keymap, input_dev);
+	if (error) {
+		dev_err(&pdev->dev, "failed to build keymap\n");
+		goto err_free_input;
+	}
 
-	__set_bit(EV_KEY, input_dev->evbit);
 	__set_bit(EV_REP, input_dev->evbit);
-
 	input_set_capability(input_dev, EV_MSC, MSC_SCAN);
 
 	input_set_drvdata(input_dev, keypad_data);
 
-	matrix_keypad_build_keymap(pdata->keymap_data, row_shift,
-			input_dev->keycode, input_dev->keybit);
-
 	error = request_irq(keypad_data->irq, omap4_keypad_interrupt,
 			     IRQF_TRIGGER_RISING,
 			     "omap4-keypad", keypad_data);
@@ -280,7 +343,7 @@ static int __devinit omap4_keypad_probe(struct platform_device *pdev)
 		goto err_free_input;
 	}
 
-	pm_runtime_enable(&pdev->dev);
+	pm_runtime_put_sync(&pdev->dev);
 
 	error = input_register_device(keypad_data->input);
 	if (error < 0) {
@@ -296,6 +359,8 @@ err_pm_disable:
 	free_irq(keypad_data->irq, keypad_data);
 err_free_input:
 	input_free_device(input_dev);
+err_pm_put_sync:
+	pm_runtime_put_sync(&pdev->dev);
 err_unmap:
 	iounmap(keypad_data->base);
 err_release_mem:
diff --git a/drivers/input/keyboard/pmic8xxx-keypad.c b/drivers/input/keyboard/pmic8xxx-keypad.c
index 01a1c9f8a38..52c34657d30 100644
--- a/drivers/input/keyboard/pmic8xxx-keypad.c
+++ b/drivers/input/keyboard/pmic8xxx-keypad.c
@@ -626,21 +626,21 @@ static int __devinit pmic8xxx_kp_probe(struct platform_device *pdev)
 	kp->input->id.product	= 0x0001;
 	kp->input->id.vendor	= 0x0001;
 
-	kp->input->evbit[0]	= BIT_MASK(EV_KEY);
-
-	if (pdata->rep)
-		__set_bit(EV_REP, kp->input->evbit);
-
-	kp->input->keycode	= kp->keycodes;
-	kp->input->keycodemax	= PM8XXX_MATRIX_MAX_SIZE;
-	kp->input->keycodesize	= sizeof(kp->keycodes);
 	kp->input->open		= pmic8xxx_kp_open;
 	kp->input->close	= pmic8xxx_kp_close;
 
-	matrix_keypad_build_keymap(keymap_data, PM8XXX_ROW_SHIFT,
-					kp->input->keycode, kp->input->keybit);
+	rc = matrix_keypad_build_keymap(keymap_data, NULL,
+					PM8XXX_MAX_ROWS, PM8XXX_MAX_COLS,
+					kp->keycodes, kp->input);
+	if (rc) {
+		dev_err(&pdev->dev, "failed to build keymap\n");
+		goto err_get_irq;
+	}
 
+	if (pdata->rep)
+		__set_bit(EV_REP, kp->input->evbit);
 	input_set_capability(kp->input, EV_MSC, MSC_SCAN);
+
 	input_set_drvdata(kp->input, kp);
 
 	/* initialize keypad state */
diff --git a/drivers/input/keyboard/pxa27x_keypad.c b/drivers/input/keyboard/pxa27x_keypad.c
index 29fe1b2be1c..7f7b72464a3 100644
--- a/drivers/input/keyboard/pxa27x_keypad.c
+++ b/drivers/input/keyboard/pxa27x_keypad.c
@@ -311,7 +311,15 @@ static void pxa27x_keypad_scan_direct(struct pxa27x_keypad *keypad)
 	if (pdata->enable_rotary0 || pdata->enable_rotary1)
 		pxa27x_keypad_scan_rotary(keypad);
 
-	new_state = KPDK_DK(kpdk) & keypad->direct_key_mask;
+	/*
+	 * The KPDR_DK only output the key pin level, so it relates to board,
+	 * and low level may be active.
+	 */
+	if (pdata->direct_key_low_active)
+		new_state = ~KPDK_DK(kpdk) & keypad->direct_key_mask;
+	else
+		new_state = KPDK_DK(kpdk) & keypad->direct_key_mask;
+
 	bits_changed = keypad->direct_key_state ^ new_state;
 
 	if (bits_changed == 0)
@@ -383,7 +391,14 @@ static void pxa27x_keypad_config(struct pxa27x_keypad *keypad)
 	if (pdata->direct_key_num > direct_key_num)
 		direct_key_num = pdata->direct_key_num;
 
-	keypad->direct_key_mask = ((2 << direct_key_num) - 1) & ~mask;
+	/*
+	 * Direct keys usage may not start from KP_DKIN0, check the platfrom
+	 * mask data to config the specific.
+	 */
+	if (pdata->direct_key_mask)
+		keypad->direct_key_mask = pdata->direct_key_mask;
+	else
+		keypad->direct_key_mask = ((1 << direct_key_num) - 1) & ~mask;
 
 	/* enable direct key */
 	if (direct_key_num)
@@ -399,7 +414,7 @@ static int pxa27x_keypad_open(struct input_dev *dev)
 	struct pxa27x_keypad *keypad = input_get_drvdata(dev);
 
 	/* Enable unit clock */
-	clk_enable(keypad->clk);
+	clk_prepare_enable(keypad->clk);
 	pxa27x_keypad_config(keypad);
 
 	return 0;
@@ -410,7 +425,7 @@ static void pxa27x_keypad_close(struct input_dev *dev)
 	struct pxa27x_keypad *keypad = input_get_drvdata(dev);
 
 	/* Disable clock unit */
-	clk_disable(keypad->clk);
+	clk_disable_unprepare(keypad->clk);
 }
 
 #ifdef CONFIG_PM
@@ -419,10 +434,14 @@ static int pxa27x_keypad_suspend(struct device *dev)
 	struct platform_device *pdev = to_platform_device(dev);
 	struct pxa27x_keypad *keypad = platform_get_drvdata(pdev);
 
-	clk_disable(keypad->clk);
-
+	/*
+	 * If the keypad is used a wake up source, clock can not be disabled.
+	 * Or it can not detect the key pressing.
+	 */
 	if (device_may_wakeup(&pdev->dev))
 		enable_irq_wake(keypad->irq);
+	else
+		clk_disable_unprepare(keypad->clk);
 
 	return 0;
 }
@@ -433,19 +452,24 @@ static int pxa27x_keypad_resume(struct device *dev)
 	struct pxa27x_keypad *keypad = platform_get_drvdata(pdev);
 	struct input_dev *input_dev = keypad->input_dev;
 
-	if (device_may_wakeup(&pdev->dev))
+	/*
+	 * If the keypad is used as wake up source, the clock is not turned
+	 * off. So do not need configure it again.
+	 */
+	if (device_may_wakeup(&pdev->dev)) {
 		disable_irq_wake(keypad->irq);
+	} else {
+		mutex_lock(&input_dev->mutex);
 
-	mutex_lock(&input_dev->mutex);
+		if (input_dev->users) {
+			/* Enable unit clock */
+			clk_prepare_enable(keypad->clk);
+			pxa27x_keypad_config(keypad);
+		}
 
-	if (input_dev->users) {
-		/* Enable unit clock */
-		clk_enable(keypad->clk);
-		pxa27x_keypad_config(keypad);
+		mutex_unlock(&input_dev->mutex);
 	}
 
-	mutex_unlock(&input_dev->mutex);
-
 	return 0;
 }
 
diff --git a/drivers/input/keyboard/samsung-keypad.c b/drivers/input/keyboard/samsung-keypad.c
index 2391ae884fe..a061ba603a2 100644
--- a/drivers/input/keyboard/samsung-keypad.c
+++ b/drivers/input/keyboard/samsung-keypad.c
@@ -454,23 +454,23 @@ static int __devinit samsung_keypad_probe(struct platform_device *pdev)
 	input_dev->name = pdev->name;
 	input_dev->id.bustype = BUS_HOST;
 	input_dev->dev.parent = &pdev->dev;
-	input_set_drvdata(input_dev, keypad);
 
 	input_dev->open = samsung_keypad_open;
 	input_dev->close = samsung_keypad_close;
 
-	input_dev->evbit[0] = BIT_MASK(EV_KEY);
-	if (!pdata->no_autorepeat)
-		input_dev->evbit[0] |= BIT_MASK(EV_REP);
+	error = matrix_keypad_build_keymap(keymap_data, NULL,
+					   pdata->rows, pdata->cols,
+					   keypad->keycodes, input_dev);
+	if (error) {
+		dev_err(&pdev->dev, "failed to build keymap\n");
+		goto err_put_clk;
+	}
 
 	input_set_capability(input_dev, EV_MSC, MSC_SCAN);
+	if (!pdata->no_autorepeat)
+		__set_bit(EV_REP, input_dev->evbit);
 
-	input_dev->keycode = keypad->keycodes;
-	input_dev->keycodesize = sizeof(keypad->keycodes[0]);
-	input_dev->keycodemax = pdata->rows << row_shift;
-
-	matrix_keypad_build_keymap(keymap_data, row_shift,
-			input_dev->keycode, input_dev->keybit);
+	input_set_drvdata(input_dev, keypad);
 
 	keypad->irq = platform_get_irq(pdev, 0);
 	if (keypad->irq < 0) {
diff --git a/drivers/input/keyboard/spear-keyboard.c b/drivers/input/keyboard/spear-keyboard.c
index 3b6b528f02f..6f287f7e153 100644
--- a/drivers/input/keyboard/spear-keyboard.c
+++ b/drivers/input/keyboard/spear-keyboard.c
@@ -19,6 +19,7 @@
 #include <linux/irq.h>
 #include <linux/kernel.h>
 #include <linux/module.h>
+#include <linux/of.h>
 #include <linux/platform_device.h>
 #include <linux/pm_wakeup.h>
 #include <linux/slab.h>
@@ -49,7 +50,9 @@
 #define KEY_VALUE	0x00FFFFFF
 #define ROW_MASK	0xF0
 #define COLUMN_MASK	0x0F
-#define ROW_SHIFT	4
+#define NUM_ROWS	16
+#define NUM_COLS	16
+
 #define KEY_MATRIX_SHIFT	6
 
 struct spear_kbd {
@@ -60,7 +63,8 @@ struct spear_kbd {
 	unsigned int irq;
 	unsigned int mode;
 	unsigned short last_key;
-	unsigned short keycodes[256];
+	unsigned short keycodes[NUM_ROWS * NUM_COLS];
+	bool rep;
 };
 
 static irqreturn_t spear_kbd_interrupt(int irq, void *dev_id)
@@ -136,27 +140,49 @@ static void spear_kbd_close(struct input_dev *dev)
 	kbd->last_key = KEY_RESERVED;
 }
 
-static int __devinit spear_kbd_probe(struct platform_device *pdev)
+#ifdef CONFIG_OF
+static int __devinit spear_kbd_parse_dt(struct platform_device *pdev,
+                                        struct spear_kbd *kbd)
 {
-	const struct kbd_platform_data *pdata = pdev->dev.platform_data;
-	const struct matrix_keymap_data *keymap;
-	struct spear_kbd *kbd;
-	struct input_dev *input_dev;
-	struct resource *res;
-	int irq;
+	struct device_node *np = pdev->dev.of_node;
 	int error;
+	u32 val;
 
-	if (!pdata) {
-		dev_err(&pdev->dev, "Invalid platform data\n");
+	if (!np) {
+		dev_err(&pdev->dev, "Missing DT data\n");
 		return -EINVAL;
 	}
 
-	keymap = pdata->keymap;
-	if (!keymap) {
-		dev_err(&pdev->dev, "no keymap defined\n");
-		return -EINVAL;
+	if (of_property_read_bool(np, "autorepeat"))
+		kbd->rep = true;
+
+	error = of_property_read_u32(np, "st,mode", &val);
+	if (error) {
+		dev_err(&pdev->dev, "DT: Invalid or missing mode\n");
+		return error;
 	}
 
+	kbd->mode = val;
+	return 0;
+}
+#else
+static inline int spear_kbd_parse_dt(struct platform_device *pdev,
+				     struct spear_kbd *kbd)
+{
+	return -ENOSYS;
+}
+#endif
+
+static int __devinit spear_kbd_probe(struct platform_device *pdev)
+{
+	struct kbd_platform_data *pdata = dev_get_platdata(&pdev->dev);
+	const struct matrix_keymap_data *keymap = pdata ? pdata->keymap : NULL;
+	struct spear_kbd *kbd;
+	struct input_dev *input_dev;
+	struct resource *res;
+	int irq;
+	int error;
+
 	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
 	if (!res) {
 		dev_err(&pdev->dev, "no keyboard resource defined\n");
@@ -179,7 +205,15 @@ static int __devinit spear_kbd_probe(struct platform_device *pdev)
 
 	kbd->input = input_dev;
 	kbd->irq = irq;
-	kbd->mode = pdata->mode;
+
+	if (!pdata) {
+		error = spear_kbd_parse_dt(pdev, kbd);
+		if (error)
+			goto err_free_mem;
+	} else {
+		kbd->mode = pdata->mode;
+		kbd->rep = pdata->rep;
+	}
 
 	kbd->res = request_mem_region(res->start, resource_size(res),
 				      pdev->name);
@@ -212,18 +246,17 @@ static int __devinit spear_kbd_probe(struct platform_device *pdev)
 	input_dev->open = spear_kbd_open;
 	input_dev->close = spear_kbd_close;
 
-	__set_bit(EV_KEY, input_dev->evbit);
-	if (pdata->rep)
+	error = matrix_keypad_build_keymap(keymap, NULL, NUM_ROWS, NUM_COLS,
+					   kbd->keycodes, input_dev);
+	if (error) {
+		dev_err(&pdev->dev, "Failed to build keymap\n");
+		goto err_put_clk;
+	}
+
+	if (kbd->rep)
 		__set_bit(EV_REP, input_dev->evbit);
 	input_set_capability(input_dev, EV_MSC, MSC_SCAN);
 
-	input_dev->keycode = kbd->keycodes;
-	input_dev->keycodesize = sizeof(kbd->keycodes[0]);
-	input_dev->keycodemax = ARRAY_SIZE(kbd->keycodes);
-
-	matrix_keypad_build_keymap(keymap, ROW_SHIFT,
-			input_dev->keycode, input_dev->keybit);
-
 	input_set_drvdata(input_dev, kbd);
 
 	error = request_irq(irq, spear_kbd_interrupt, 0, "keyboard", kbd);
@@ -317,6 +350,14 @@ static int spear_kbd_resume(struct device *dev)
 
 static SIMPLE_DEV_PM_OPS(spear_kbd_pm_ops, spear_kbd_suspend, spear_kbd_resume);
 
+#ifdef CONFIG_OF
+static const struct of_device_id spear_kbd_id_table[] = {
+	{ .compatible = "st,spear300-kbd" },
+	{}
+};
+MODULE_DEVICE_TABLE(of, spear_kbd_id_table);
+#endif
+
 static struct platform_driver spear_kbd_driver = {
 	.probe		= spear_kbd_probe,
 	.remove		= __devexit_p(spear_kbd_remove),
@@ -324,6 +365,7 @@ static struct platform_driver spear_kbd_driver = {
 		.name	= "keyboard",
 		.owner	= THIS_MODULE,
 		.pm	= &spear_kbd_pm_ops,
+		.of_match_table = of_match_ptr(spear_kbd_id_table),
 	},
 };
 module_platform_driver(spear_kbd_driver);
diff --git a/drivers/input/keyboard/stmpe-keypad.c b/drivers/input/keyboard/stmpe-keypad.c
index 9397cf9c625..470a8778dec 100644
--- a/drivers/input/keyboard/stmpe-keypad.c
+++ b/drivers/input/keyboard/stmpe-keypad.c
@@ -289,19 +289,17 @@ static int __devinit stmpe_keypad_probe(struct platform_device *pdev)
 	input->id.bustype = BUS_I2C;
 	input->dev.parent = &pdev->dev;
 
-	input_set_capability(input, EV_MSC, MSC_SCAN);
+	ret = matrix_keypad_build_keymap(plat->keymap_data, NULL,
+					 STMPE_KEYPAD_MAX_ROWS,
+					 STMPE_KEYPAD_MAX_COLS,
+					 keypad->keymap, input);
+	if (ret)
+		goto out_freeinput;
 
-	__set_bit(EV_KEY, input->evbit);
+	input_set_capability(input, EV_MSC, MSC_SCAN);
 	if (!plat->no_autorepeat)
 		__set_bit(EV_REP, input->evbit);
 
-	input->keycode = keypad->keymap;
-	input->keycodesize = sizeof(keypad->keymap[0]);
-	input->keycodemax = ARRAY_SIZE(keypad->keymap);
-
-	matrix_keypad_build_keymap(plat->keymap_data, STMPE_KEYPAD_ROW_SHIFT,
-				   input->keycode, input->keybit);
-
 	for (i = 0; i < plat->keymap_data->keymap_size; i++) {
 		unsigned int key = plat->keymap_data->keymap[i];
 
diff --git a/drivers/input/keyboard/stowaway.c b/drivers/input/keyboard/stowaway.c
index 7437219370b..cc612c5d542 100644
--- a/drivers/input/keyboard/stowaway.c
+++ b/drivers/input/keyboard/stowaway.c
@@ -170,15 +170,4 @@ static struct serio_driver skbd_drv = {
 	.disconnect	= skbd_disconnect,
 };
 
-static int __init skbd_init(void)
-{
-	return serio_register_driver(&skbd_drv);
-}
-
-static void __exit skbd_exit(void)
-{
-	serio_unregister_driver(&skbd_drv);
-}
-
-module_init(skbd_init);
-module_exit(skbd_exit);
+module_serio_driver(skbd_drv);
diff --git a/drivers/input/keyboard/sunkbd.c b/drivers/input/keyboard/sunkbd.c
index a99a04b03ee..5f836b1638c 100644
--- a/drivers/input/keyboard/sunkbd.c
+++ b/drivers/input/keyboard/sunkbd.c
@@ -369,19 +369,4 @@ static struct serio_driver sunkbd_drv = {
 	.disconnect	= sunkbd_disconnect,
 };
 
-/*
- * The functions for insering/removing us as a module.
- */
-
-static int __init sunkbd_init(void)
-{
-	return serio_register_driver(&sunkbd_drv);
-}
-
-static void __exit sunkbd_exit(void)
-{
-	serio_unregister_driver(&sunkbd_drv);
-}
-
-module_init(sunkbd_init);
-module_exit(sunkbd_exit);
+module_serio_driver(sunkbd_drv);
diff --git a/drivers/input/keyboard/tc3589x-keypad.c b/drivers/input/keyboard/tc3589x-keypad.c
index 2dee3e4e7c6..7d498e69850 100644
--- a/drivers/input/keyboard/tc3589x-keypad.c
+++ b/drivers/input/keyboard/tc3589x-keypad.c
@@ -78,7 +78,7 @@
  * @input:      pointer to input device object
  * @board:      keypad platform device
  * @krow:	number of rows
- * @kcol:	number of coloumns
+ * @kcol:	number of columns
  * @keymap:     matrix scan code table for keycodes
  * @keypad_stopped: holds keypad status
  */
@@ -96,21 +96,15 @@ static int tc3589x_keypad_init_key_hardware(struct tc_keypad *keypad)
 {
 	int ret;
 	struct tc3589x *tc3589x = keypad->tc3589x;
-	u8 settle_time = keypad->board->settle_time;
-	u8 dbounce_period = keypad->board->debounce_period;
-	u8 rows = keypad->board->krow & 0xf;	/* mask out the nibble */
-	u8 column = keypad->board->kcol & 0xf;	/* mask out the nibble */
-
-	/* validate platform configurations */
-	if (keypad->board->kcol > TC3589x_MAX_KPCOL ||
-	    keypad->board->krow > TC3589x_MAX_KPROW ||
-	    keypad->board->debounce_period > TC3589x_MAX_DEBOUNCE_SETTLE ||
-	    keypad->board->settle_time > TC3589x_MAX_DEBOUNCE_SETTLE)
+	const struct tc3589x_keypad_platform_data *board = keypad->board;
+
+	/* validate platform configuration */
+	if (board->kcol > TC3589x_MAX_KPCOL || board->krow > TC3589x_MAX_KPROW)
 		return -EINVAL;
 
 	/* configure KBDSIZE 4 LSbits for cols and 4 MSbits for rows */
 	ret = tc3589x_reg_write(tc3589x, TC3589x_KBDSIZE,
-			(rows << KP_ROW_SHIFT) | column);
+			(board->krow << KP_ROW_SHIFT) | board->kcol);
 	if (ret < 0)
 		return ret;
 
@@ -124,12 +118,14 @@ static int tc3589x_keypad_init_key_hardware(struct tc_keypad *keypad)
 		return ret;
 
 	/* Configure settle time */
-	ret = tc3589x_reg_write(tc3589x, TC3589x_KBDSETTLE_REG, settle_time);
+	ret = tc3589x_reg_write(tc3589x, TC3589x_KBDSETTLE_REG,
+				board->settle_time);
 	if (ret < 0)
 		return ret;
 
 	/* Configure debounce time */
-	ret = tc3589x_reg_write(tc3589x, TC3589x_KBDBOUNCE, dbounce_period);
+	ret = tc3589x_reg_write(tc3589x, TC3589x_KBDBOUNCE,
+				board->debounce_period);
 	if (ret < 0)
 		return ret;
 
@@ -337,23 +333,22 @@ static int __devinit tc3589x_keypad_probe(struct platform_device *pdev)
 	input->name = pdev->name;
 	input->dev.parent = &pdev->dev;
 
-	input->keycode = keypad->keymap;
-	input->keycodesize = sizeof(keypad->keymap[0]);
-	input->keycodemax = ARRAY_SIZE(keypad->keymap);
-
 	input->open = tc3589x_keypad_open;
 	input->close = tc3589x_keypad_close;
 
-	input_set_drvdata(input, keypad);
+	error = matrix_keypad_build_keymap(plat->keymap_data, NULL,
+					   TC3589x_MAX_KPROW, TC3589x_MAX_KPCOL,
+					   keypad->keymap, input);
+	if (error) {
+		dev_err(&pdev->dev, "Failed to build keymap\n");
+		goto err_free_mem;
+	}
 
 	input_set_capability(input, EV_MSC, MSC_SCAN);
-
-	__set_bit(EV_KEY, input->evbit);
 	if (!plat->no_autorepeat)
 		__set_bit(EV_REP, input->evbit);
 
-	matrix_keypad_build_keymap(plat->keymap_data, 0x3,
-			input->keycode, input->keybit);
+	input_set_drvdata(input, keypad);
 
 	error = request_threaded_irq(irq, NULL,
 			tc3589x_keypad_irq, plat->irqtype,
diff --git a/drivers/input/keyboard/tca8418_keypad.c b/drivers/input/keyboard/tca8418_keypad.c
index 958ec107bfb..5f87b28b319 100644
--- a/drivers/input/keyboard/tca8418_keypad.c
+++ b/drivers/input/keyboard/tca8418_keypad.c
@@ -342,21 +342,20 @@ static int __devinit tca8418_keypad_probe(struct i2c_client *client,
 	input->id.product = 0x001;
 	input->id.version = 0x0001;
 
-	input->keycode     = keypad_data->keymap;
-	input->keycodesize = sizeof(keypad_data->keymap[0]);
-	input->keycodemax  = max_keys;
+	error = matrix_keypad_build_keymap(pdata->keymap_data, NULL,
+					   pdata->rows, pdata->cols,
+					   keypad_data->keymap, input);
+	if (error) {
+		dev_dbg(&client->dev, "Failed to build keymap\n");
+		goto fail2;
+	}
 
-	__set_bit(EV_KEY, input->evbit);
 	if (pdata->rep)
 		__set_bit(EV_REP, input->evbit);
-
 	input_set_capability(input, EV_MSC, MSC_SCAN);
 
 	input_set_drvdata(input, keypad_data);
 
-	matrix_keypad_build_keymap(pdata->keymap_data, row_shift,
-			input->keycode, input->keybit);
-
 	if (pdata->irq_is_gpio)
 		client->irq = gpio_to_irq(client->irq);
 
diff --git a/drivers/input/keyboard/tegra-kbc.c b/drivers/input/keyboard/tegra-kbc.c
index fe4ac95ca6c..4ffe64d5310 100644
--- a/drivers/input/keyboard/tegra-kbc.c
+++ b/drivers/input/keyboard/tegra-kbc.c
@@ -619,8 +619,8 @@ tegra_kbc_check_pin_cfg(const struct tegra_kbc_platform_data *pdata,
 }
 
 #ifdef CONFIG_OF
-static struct tegra_kbc_platform_data * __devinit
-tegra_kbc_dt_parse_pdata(struct platform_device *pdev)
+static struct tegra_kbc_platform_data * __devinit tegra_kbc_dt_parse_pdata(
+	struct platform_device *pdev)
 {
 	struct tegra_kbc_platform_data *pdata;
 	struct device_node *np = pdev->dev.of_node;
@@ -660,10 +660,6 @@ tegra_kbc_dt_parse_pdata(struct platform_device *pdev)
 		pdata->pin_cfg[KBC_MAX_ROW + i].type = PIN_CFG_COL;
 	}
 
-	pdata->keymap_data = matrix_keyboard_of_fill_keymap(np, "linux,keymap");
-
-	/* FIXME: Add handling of linux,fn-keymap here */
-
 	return pdata;
 }
 #else
@@ -674,10 +670,36 @@ static inline struct tegra_kbc_platform_data *tegra_kbc_dt_parse_pdata(
 }
 #endif
 
+static int __devinit tegra_kbd_setup_keymap(struct tegra_kbc *kbc)
+{
+	const struct tegra_kbc_platform_data *pdata = kbc->pdata;
+	const struct matrix_keymap_data *keymap_data = pdata->keymap_data;
+	unsigned int keymap_rows = KBC_MAX_KEY;
+	int retval;
+
+	if (keymap_data && pdata->use_fn_map)
+		keymap_rows *= 2;
+
+	retval = matrix_keypad_build_keymap(keymap_data, NULL,
+					    keymap_rows, KBC_MAX_COL,
+					    kbc->keycode, kbc->idev);
+	if (retval == -ENOSYS || retval == -ENOENT) {
+		/*
+		 * If there is no OF support in kernel or keymap
+		 * property is missing, use default keymap.
+		 */
+		retval = matrix_keypad_build_keymap(
+					&tegra_kbc_default_keymap_data, NULL,
+					keymap_rows, KBC_MAX_COL,
+					kbc->keycode, kbc->idev);
+	}
+
+	return retval;
+}
+
 static int __devinit tegra_kbc_probe(struct platform_device *pdev)
 {
 	const struct tegra_kbc_platform_data *pdata = pdev->dev.platform_data;
-	const struct matrix_keymap_data *keymap_data;
 	struct tegra_kbc *kbc;
 	struct input_dev *input_dev;
 	struct resource *res;
@@ -757,29 +779,26 @@ static int __devinit tegra_kbc_probe(struct platform_device *pdev)
 	kbc->repoll_dly = KBC_ROW_SCAN_DLY + scan_time_rows + pdata->repeat_cnt;
 	kbc->repoll_dly = DIV_ROUND_UP(kbc->repoll_dly, KBC_CYCLE_MS);
 
+	kbc->wakeup_key = pdata->wakeup_key;
+	kbc->use_fn_map = pdata->use_fn_map;
+	kbc->use_ghost_filter = pdata->use_ghost_filter;
+
 	input_dev->name = pdev->name;
 	input_dev->id.bustype = BUS_HOST;
 	input_dev->dev.parent = &pdev->dev;
 	input_dev->open = tegra_kbc_open;
 	input_dev->close = tegra_kbc_close;
 
-	input_set_drvdata(input_dev, kbc);
+	err = tegra_kbd_setup_keymap(kbc);
+	if (err) {
+		dev_err(&pdev->dev, "failed to setup keymap\n");
+		goto err_put_clk;
+	}
 
-	input_dev->evbit[0] = BIT_MASK(EV_KEY) | BIT_MASK(EV_REP);
+	__set_bit(EV_REP, input_dev->evbit);
 	input_set_capability(input_dev, EV_MSC, MSC_SCAN);
 
-	input_dev->keycode = kbc->keycode;
-	input_dev->keycodesize = sizeof(kbc->keycode[0]);
-	input_dev->keycodemax = KBC_MAX_KEY;
-	if (pdata->use_fn_map)
-		input_dev->keycodemax *= 2;
-
-	kbc->use_fn_map = pdata->use_fn_map;
-	kbc->use_ghost_filter = pdata->use_ghost_filter;
-	keymap_data = pdata->keymap_data ?: &tegra_kbc_default_keymap_data;
-	matrix_keypad_build_keymap(keymap_data, KBC_ROW_SHIFT,
-				   input_dev->keycode, input_dev->keybit);
-	kbc->wakeup_key = pdata->wakeup_key;
+	input_set_drvdata(input_dev, kbc);
 
 	err = request_irq(kbc->irq, tegra_kbc_isr,
 			  IRQF_NO_SUSPEND | IRQF_TRIGGER_HIGH, pdev->name, kbc);
@@ -799,9 +818,6 @@ static int __devinit tegra_kbc_probe(struct platform_device *pdev)
 	platform_set_drvdata(pdev, kbc);
 	device_init_wakeup(&pdev->dev, pdata->wakeup);
 
-	if (!pdev->dev.platform_data)
-		matrix_keyboard_of_free_keymap(pdata->keymap_data);
-
 	return 0;
 
 err_free_irq:
@@ -816,10 +832,8 @@ err_free_mem:
 	input_free_device(input_dev);
 	kfree(kbc);
 err_free_pdata:
-	if (!pdev->dev.platform_data) {
-		matrix_keyboard_of_free_keymap(pdata->keymap_data);
+	if (!pdev->dev.platform_data)
 		kfree(pdata);
-	}
 
 	return err;
 }
diff --git a/drivers/input/keyboard/tnetv107x-keypad.c b/drivers/input/keyboard/tnetv107x-keypad.c
index fb39c94b6fd..a4a445fb702 100644
--- a/drivers/input/keyboard/tnetv107x-keypad.c
+++ b/drivers/input/keyboard/tnetv107x-keypad.c
@@ -247,15 +247,11 @@ static int __devinit keypad_probe(struct platform_device *pdev)
 		error = -ENOMEM;
 		goto error_input;
 	}
-	input_set_drvdata(kp->input_dev, kp);
 
 	kp->input_dev->name	  = pdev->name;
 	kp->input_dev->dev.parent = &pdev->dev;
 	kp->input_dev->open	  = keypad_start;
 	kp->input_dev->close	  = keypad_stop;
-	kp->input_dev->evbit[0]	  = BIT_MASK(EV_KEY);
-	if (!pdata->no_autorepeat)
-		kp->input_dev->evbit[0] |= BIT_MASK(EV_REP);
 
 	clk_enable(kp->clk);
 	rev = keypad_read(kp, rev);
@@ -264,15 +260,20 @@ static int __devinit keypad_probe(struct platform_device *pdev)
 	kp->input_dev->id.version = ((rev >> 16) & 0xfff);
 	clk_disable(kp->clk);
 
-	kp->input_dev->keycode     = kp->keycodes;
-	kp->input_dev->keycodesize = sizeof(kp->keycodes[0]);
-	kp->input_dev->keycodemax  = kp->rows << kp->row_shift;
-
-	matrix_keypad_build_keymap(keymap_data, kp->row_shift, kp->keycodes,
-				   kp->input_dev->keybit);
+	error = matrix_keypad_build_keymap(keymap_data, NULL,
+					   kp->rows, kp->cols,
+					   kp->keycodes, kp->input_dev);
+	if (error) {
+		dev_err(dev, "Failed to build keymap\n");
+		goto error_reg;
+	}
 
+	if (!pdata->no_autorepeat)
+		kp->input_dev->evbit[0] |= BIT_MASK(EV_REP);
 	input_set_capability(kp->input_dev, EV_MSC, MSC_SCAN);
 
+	input_set_drvdata(kp->input_dev, kp);
+
 	error = input_register_device(kp->input_dev);
 	if (error < 0) {
 		dev_err(dev, "Could not register input device\n");
diff --git a/drivers/input/keyboard/twl4030_keypad.c b/drivers/input/keyboard/twl4030_keypad.c
index 67bec14e8b9..a2c6f79aa10 100644
--- a/drivers/input/keyboard/twl4030_keypad.c
+++ b/drivers/input/keyboard/twl4030_keypad.c
@@ -361,14 +361,6 @@ static int __devinit twl4030_kp_probe(struct platform_device *pdev)
 	kp->irq = platform_get_irq(pdev, 0);
 
 	/* setup input device */
-	__set_bit(EV_KEY, input->evbit);
-
-	/* Enable auto repeat feature of Linux input subsystem */
-	if (pdata->rep)
-		__set_bit(EV_REP, input->evbit);
-
-	input_set_capability(input, EV_MSC, MSC_SCAN);
-
 	input->name		= "TWL4030 Keypad";
 	input->phys		= "twl4030_keypad/input0";
 	input->dev.parent	= &pdev->dev;
@@ -378,12 +370,19 @@ static int __devinit twl4030_kp_probe(struct platform_device *pdev)
 	input->id.product	= 0x0001;
 	input->id.version	= 0x0003;
 
-	input->keycode		= kp->keymap;
-	input->keycodesize	= sizeof(kp->keymap[0]);
-	input->keycodemax	= ARRAY_SIZE(kp->keymap);
+	error = matrix_keypad_build_keymap(keymap_data, NULL,
+					   TWL4030_MAX_ROWS,
+					   1 << TWL4030_ROW_SHIFT,
+					   kp->keymap, input);
+	if (error) {
+		dev_err(kp->dbg_dev, "Failed to build keymap\n");
+		goto err1;
+	}
 
-	matrix_keypad_build_keymap(keymap_data, TWL4030_ROW_SHIFT,
-				   input->keycode, input->keybit);
+	input_set_capability(input, EV_MSC, MSC_SCAN);
+	/* Enable auto repeat feature of Linux input subsystem */
+	if (pdata->rep)
+		__set_bit(EV_REP, input->evbit);
 
 	error = input_register_device(input);
 	if (error) {
diff --git a/drivers/input/keyboard/w90p910_keypad.c b/drivers/input/keyboard/w90p910_keypad.c
index 99bbb7e775a..085ede4d972 100644
--- a/drivers/input/keyboard/w90p910_keypad.c
+++ b/drivers/input/keyboard/w90p910_keypad.c
@@ -42,7 +42,8 @@
 #define KGET_RAW(n)		(((n) & KEY0R) >> 3)
 #define KGET_COLUMN(n)		((n) & KEY0C)
 
-#define W90P910_MAX_KEY_NUM	(8 * 8)
+#define W90P910_NUM_ROWS	8
+#define W90P910_NUM_COLS	8
 #define W90P910_ROW_SHIFT	3
 
 struct w90p910_keypad {
@@ -51,7 +52,7 @@ struct w90p910_keypad {
 	struct input_dev *input_dev;
 	void __iomem *mmio_base;
 	int irq;
-	unsigned short keymap[W90P910_MAX_KEY_NUM];
+	unsigned short keymap[W90P910_NUM_ROWS * W90P910_NUM_COLS];
 };
 
 static void w90p910_keypad_scan_matrix(struct w90p910_keypad *keypad,
@@ -190,17 +191,13 @@ static int __devinit w90p910_keypad_probe(struct platform_device *pdev)
 	input_dev->close = w90p910_keypad_close;
 	input_dev->dev.parent = &pdev->dev;
 
-	input_dev->keycode = keypad->keymap;
-	input_dev->keycodesize = sizeof(keypad->keymap[0]);
-	input_dev->keycodemax = ARRAY_SIZE(keypad->keymap);
-
-	input_set_drvdata(input_dev, keypad);
-
-	input_dev->evbit[0] = BIT_MASK(EV_KEY) | BIT_MASK(EV_REP);
-	input_set_capability(input_dev, EV_MSC, MSC_SCAN);
-
-	matrix_keypad_build_keymap(keymap_data, W90P910_ROW_SHIFT,
-				   input_dev->keycode, input_dev->keybit);
+	error = matrix_keypad_build_keymap(keymap_data, NULL,
+					   W90P910_NUM_ROWS, W90P910_NUM_COLS,
+					   keypad->keymap, input_dev);
+	if (error) {
+		dev_err(&pdev->dev, "failed to build keymap\n");
+		goto failed_put_clk;
+	}
 
 	error = request_irq(keypad->irq, w90p910_keypad_irq_handler,
 			    0, pdev->name, keypad);
@@ -209,6 +206,10 @@ static int __devinit w90p910_keypad_probe(struct platform_device *pdev)
 		goto failed_put_clk;
 	}
 
+	__set_bit(EV_REP, input_dev->evbit);
+	input_set_capability(input_dev, EV_MSC, MSC_SCAN);
+	input_set_drvdata(input_dev, keypad);
+
 	/* Register the input device */
 	error = input_register_device(input_dev);
 	if (error) {
diff --git a/drivers/input/keyboard/xtkbd.c b/drivers/input/keyboard/xtkbd.c
index 37b01d777a4..d050d9d0011 100644
--- a/drivers/input/keyboard/xtkbd.c
+++ b/drivers/input/keyboard/xtkbd.c
@@ -169,15 +169,4 @@ static struct serio_driver xtkbd_drv = {
 	.disconnect	= xtkbd_disconnect,
 };
 
-static int __init xtkbd_init(void)
-{
-	return serio_register_driver(&xtkbd_drv);
-}
-
-static void __exit xtkbd_exit(void)
-{
-	serio_unregister_driver(&xtkbd_drv);
-}
-
-module_init(xtkbd_init);
-module_exit(xtkbd_exit);
+module_serio_driver(xtkbd_drv);
diff --git a/drivers/input/matrix-keymap.c b/drivers/input/matrix-keymap.c
new file mode 100644
index 00000000000..443ad64b7f2
--- /dev/null
+++ b/drivers/input/matrix-keymap.c
@@ -0,0 +1,163 @@
+/*
+ * Helpers for matrix keyboard bindings
+ *
+ * Copyright (C) 2012 Google, Inc
+ *
+ * Author:
+ *	Olof Johansson <olof@lixom.net>
+ *
+ * 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/kernel.h>
+#include <linux/types.h>
+#include <linux/input.h>
+#include <linux/of.h>
+#include <linux/export.h>
+#include <linux/input/matrix_keypad.h>
+
+static bool matrix_keypad_map_key(struct input_dev *input_dev,
+				  unsigned int rows, unsigned int cols,
+				  unsigned int row_shift, unsigned int key)
+{
+	unsigned short *keymap = input_dev->keycode;
+	unsigned int row = KEY_ROW(key);
+	unsigned int col = KEY_COL(key);
+	unsigned short code = KEY_VAL(key);
+
+	if (row >= rows || col >= cols) {
+		dev_err(input_dev->dev.parent,
+			"%s: invalid keymap entry 0x%x (row: %d, col: %d, rows: %d, cols: %d)\n",
+			__func__, key, row, col, rows, cols);
+		return false;
+	}
+
+	keymap[MATRIX_SCAN_CODE(row, col, row_shift)] = code;
+	__set_bit(code, input_dev->keybit);
+
+	return true;
+}
+
+#ifdef CONFIG_OF
+static int matrix_keypad_parse_of_keymap(const char *propname,
+					 unsigned int rows, unsigned int cols,
+					 struct input_dev *input_dev)
+{
+	struct device *dev = input_dev->dev.parent;
+	struct device_node *np = dev->of_node;
+	unsigned int row_shift = get_count_order(cols);
+	unsigned int max_keys = rows << row_shift;
+	unsigned int proplen, i, size;
+	const __be32 *prop;
+
+	if (!np)
+		return -ENOENT;
+
+	if (!propname)
+		propname = "linux,keymap";
+
+	prop = of_get_property(np, propname, &proplen);
+	if (!prop) {
+		dev_err(dev, "OF: %s property not defined in %s\n",
+			propname, np->full_name);
+		return -ENOENT;
+	}
+
+	if (proplen % sizeof(u32)) {
+		dev_err(dev, "OF: Malformed keycode property %s in %s\n",
+			propname, np->full_name);
+		return -EINVAL;
+	}
+
+	size = proplen / sizeof(u32);
+	if (size > max_keys) {
+		dev_err(dev, "OF: %s size overflow\n", propname);
+		return -EINVAL;
+	}
+
+	for (i = 0; i < size; i++) {
+		unsigned int key = be32_to_cpup(prop + i);
+
+		if (!matrix_keypad_map_key(input_dev, rows, cols,
+					   row_shift, key))
+			return -EINVAL;
+	}
+
+	return 0;
+}
+#else
+static int matrix_keypad_parse_of_keymap(const char *propname,
+					 unsigned int rows, unsigned int cols,
+					 struct input_dev *input_dev)
+{
+	return -ENOSYS;
+}
+#endif
+
+/**
+ * matrix_keypad_build_keymap - convert platform keymap into matrix keymap
+ * @keymap_data: keymap supplied by the platform code
+ * @keymap_name: name of device tree property containing keymap (if device
+ *	tree support is enabled).
+ * @rows: number of rows in target keymap array
+ * @cols: number of cols in target keymap array
+ * @keymap: expanded version of keymap that is suitable for use by
+ * matrix keyboard driver
+ * @input_dev: input devices for which we are setting up the keymap
+ *
+ * This function converts platform keymap (encoded with KEY() macro) into
+ * an array of keycodes that is suitable for using in a standard matrix
+ * keyboard driver that uses row and col as indices.
+ *
+ * If @keymap_data is not supplied and device tree support is enabled
+ * it will attempt load the keymap from property specified by @keymap_name
+ * argument (or "linux,keymap" if @keymap_name is %NULL).
+ *
+ * Callers are expected to set up input_dev->dev.parent before calling this
+ * function.
+ */
+int matrix_keypad_build_keymap(const struct matrix_keymap_data *keymap_data,
+			       const char *keymap_name,
+			       unsigned int rows, unsigned int cols,
+			       unsigned short *keymap,
+			       struct input_dev *input_dev)
+{
+	unsigned int row_shift = get_count_order(cols);
+	int i;
+	int error;
+
+	input_dev->keycode = keymap;
+	input_dev->keycodesize = sizeof(*keymap);
+	input_dev->keycodemax = rows << row_shift;
+
+	__set_bit(EV_KEY, input_dev->evbit);
+
+	if (keymap_data) {
+		for (i = 0; i < keymap_data->keymap_size; i++) {
+			unsigned int key = keymap_data->keymap[i];
+
+			if (!matrix_keypad_map_key(input_dev, rows, cols,
+						   row_shift, key))
+				return -EINVAL;
+		}
+	} else {
+		error = matrix_keypad_parse_of_keymap(keymap_name, rows, cols,
+						      input_dev);
+		if (error)
+			return error;
+	}
+
+	__clear_bit(KEY_RESERVED, input_dev->keybit);
+
+	return 0;
+}
+EXPORT_SYMBOL(matrix_keypad_build_keymap);
diff --git a/drivers/input/misc/cm109.c b/drivers/input/misc/cm109.c
index ab860511f01..082684e7f39 100644
--- a/drivers/input/misc/cm109.c
+++ b/drivers/input/misc/cm109.c
@@ -327,7 +327,9 @@ static void cm109_submit_buzz_toggle(struct cm109_dev *dev)
 
 	error = usb_submit_urb(dev->urb_ctl, GFP_ATOMIC);
 	if (error)
-		err("%s: usb_submit_urb (urb_ctl) failed %d", __func__, error);
+		dev_err(&dev->intf->dev,
+			"%s: usb_submit_urb (urb_ctl) failed %d\n",
+			__func__, error);
 }
 
 /*
@@ -339,7 +341,7 @@ static void cm109_urb_irq_callback(struct urb *urb)
 	const int status = urb->status;
 	int error;
 
-	dev_dbg(&urb->dev->dev, "### URB IRQ: [0x%02x 0x%02x 0x%02x 0x%02x] keybit=0x%02x\n",
+	dev_dbg(&dev->intf->dev, "### URB IRQ: [0x%02x 0x%02x 0x%02x 0x%02x] keybit=0x%02x\n",
 	     dev->irq_data->byte[0],
 	     dev->irq_data->byte[1],
 	     dev->irq_data->byte[2],
@@ -349,7 +351,7 @@ static void cm109_urb_irq_callback(struct urb *urb)
 	if (status) {
 		if (status == -ESHUTDOWN)
 			return;
-		err("%s: urb status %d", __func__, status);
+		dev_err(&dev->intf->dev, "%s: urb status %d\n", __func__, status);
 	}
 
 	/* Special keys */
@@ -396,7 +398,8 @@ static void cm109_urb_irq_callback(struct urb *urb)
 
 		error = usb_submit_urb(dev->urb_ctl, GFP_ATOMIC);
 		if (error)
-			err("%s: usb_submit_urb (urb_ctl) failed %d",
+			dev_err(&dev->intf->dev,
+				"%s: usb_submit_urb (urb_ctl) failed %d\n",
 				__func__, error);
 	}
 
@@ -409,14 +412,14 @@ static void cm109_urb_ctl_callback(struct urb *urb)
 	const int status = urb->status;
 	int error;
 
-	dev_dbg(&urb->dev->dev, "### URB CTL: [0x%02x 0x%02x 0x%02x 0x%02x]\n",
+	dev_dbg(&dev->intf->dev, "### URB CTL: [0x%02x 0x%02x 0x%02x 0x%02x]\n",
 	     dev->ctl_data->byte[0],
 	     dev->ctl_data->byte[1],
 	     dev->ctl_data->byte[2],
 	     dev->ctl_data->byte[3]);
 
 	if (status)
-		err("%s: urb status %d", __func__, status);
+		dev_err(&dev->intf->dev, "%s: urb status %d\n", __func__, status);
 
 	spin_lock(&dev->ctl_submit_lock);
 
@@ -433,7 +436,8 @@ static void cm109_urb_ctl_callback(struct urb *urb)
 			dev->irq_urb_pending = 1;
 			error = usb_submit_urb(dev->urb_irq, GFP_ATOMIC);
 			if (error)
-				err("%s: usb_submit_urb (urb_irq) failed %d",
+				dev_err(&dev->intf->dev,
+					"%s: usb_submit_urb (urb_irq) failed %d\n",
 					__func__, error);
 		}
 	}
@@ -476,7 +480,8 @@ static void cm109_toggle_buzzer_sync(struct cm109_dev *dev, int on)
 				dev->ctl_data,
 				USB_PKT_LEN, USB_CTRL_SET_TIMEOUT);
 	if (error < 0 && error != -EINTR)
-		err("%s: usb_control_msg() failed %d", __func__, error);
+		dev_err(&dev->intf->dev, "%s: usb_control_msg() failed %d\n",
+			__func__, error);
 }
 
 static void cm109_stop_traffic(struct cm109_dev *dev)
@@ -518,8 +523,8 @@ static int cm109_input_open(struct input_dev *idev)
 
 	error = usb_autopm_get_interface(dev->intf);
 	if (error < 0) {
-		err("%s - cannot autoresume, result %d",
-		    __func__, error);
+		dev_err(&idev->dev, "%s - cannot autoresume, result %d\n",
+			__func__, error);
 		return error;
 	}
 
@@ -537,7 +542,8 @@ static int cm109_input_open(struct input_dev *idev)
 
 	error = usb_submit_urb(dev->urb_ctl, GFP_KERNEL);
 	if (error)
-		err("%s: usb_submit_urb (urb_ctl) failed %d", __func__, error);
+		dev_err(&dev->intf->dev, "%s: usb_submit_urb (urb_ctl) failed %d\n",
+			__func__, error);
 	else
 		dev->open = 1;
 
@@ -573,7 +579,7 @@ static int cm109_input_ev(struct input_dev *idev, unsigned int type,
 {
 	struct cm109_dev *dev = input_get_drvdata(idev);
 
-	dev_dbg(&dev->udev->dev,
+	dev_dbg(&dev->intf->dev,
 		"input_ev: type=%u code=%u value=%d\n", type, code, value);
 
 	if (type != EV_SND)
@@ -710,7 +716,8 @@ static int cm109_usb_probe(struct usb_interface *intf,
 	pipe = usb_rcvintpipe(udev, endpoint->bEndpointAddress);
 	ret = usb_maxpacket(udev, pipe, usb_pipeout(pipe));
 	if (ret != USB_PKT_LEN)
-		err("invalid payload size %d, expected %d", ret, USB_PKT_LEN);
+		dev_err(&intf->dev, "invalid payload size %d, expected %d\n",
+			ret, USB_PKT_LEN);
 
 	/* initialise irq urb */
 	usb_fill_int_urb(dev->urb_irq, udev, pipe, dev->irq_data,
diff --git a/drivers/input/misc/cma3000_d0x.c b/drivers/input/misc/cma3000_d0x.c
index 06517e60e50..a3735a01e9f 100644
--- a/drivers/input/misc/cma3000_d0x.c
+++ b/drivers/input/misc/cma3000_d0x.c
@@ -318,7 +318,7 @@ struct cma3000_accl_data *cma3000_init(struct device *dev, int irq,
 	mutex_init(&data->mutex);
 
 	data->mode = pdata->mode;
-	if (data->mode < CMAMODE_DEFAULT || data->mode > CMAMODE_POFF) {
+	if (data->mode > CMAMODE_POFF) {
 		data->mode = CMAMODE_MOTDET;
 		dev_warn(dev,
 			 "Invalid mode specified, assuming Motion Detect\n");
diff --git a/drivers/input/misc/keyspan_remote.c b/drivers/input/misc/keyspan_remote.c
index d99151a8bf1..290fa5f97de 100644
--- a/drivers/input/misc/keyspan_remote.c
+++ b/drivers/input/misc/keyspan_remote.c
@@ -157,7 +157,7 @@ static int keyspan_load_tester(struct usb_keyspan* dev, int bits_needed)
 	 * though so it's not too big a deal
 	 */
 	if (dev->data.pos >= dev->data.len) {
-		dev_dbg(&dev->udev->dev,
+		dev_dbg(&dev->interface->dev,
 			"%s - Error ran out of data. pos: %d, len: %d\n",
 			__func__, dev->data.pos, dev->data.len);
 		return -1;
@@ -267,7 +267,9 @@ static void keyspan_check_data(struct usb_keyspan *remote)
 				remote->data.tester = remote->data.tester >> 6;
 				remote->data.bits_left -= 6;
 			} else {
-				err("%s - Unknown sequence found in system data.\n", __func__);
+				dev_err(&remote->interface->dev,
+					"%s - Unknown sequence found in system data.\n",
+					__func__);
 				remote->stage = 0;
 				return;
 			}
@@ -286,7 +288,9 @@ static void keyspan_check_data(struct usb_keyspan *remote)
 				remote->data.tester = remote->data.tester >> 6;
 				remote->data.bits_left -= 6;
 			} else {
-				err("%s - Unknown sequence found in button data.\n", __func__);
+				dev_err(&remote->interface->dev,
+					"%s - Unknown sequence found in button data.\n",
+					__func__);
 				remote->stage = 0;
 				return;
 			}
@@ -302,7 +306,9 @@ static void keyspan_check_data(struct usb_keyspan *remote)
 			remote->data.tester = remote->data.tester >> 6;
 			remote->data.bits_left -= 6;
 		} else {
-			err("%s - Error in message, invalid toggle.\n", __func__);
+			dev_err(&remote->interface->dev,
+				"%s - Error in message, invalid toggle.\n",
+				__func__);
 			remote->stage = 0;
 			return;
 		}
@@ -312,10 +318,11 @@ static void keyspan_check_data(struct usb_keyspan *remote)
 			remote->data.tester = remote->data.tester >> 5;
 			remote->data.bits_left -= 5;
 		} else {
-			err("Bad message received, no stop bit found.\n");
+			dev_err(&remote->interface->dev,
+				"Bad message received, no stop bit found.\n");
 		}
 
-		dev_dbg(&remote->udev->dev,
+		dev_dbg(&remote->interface->dev,
 			"%s found valid message: system: %d, button: %d, toggle: %d\n",
 			__func__, message.system, message.button, message.toggle);
 
@@ -397,7 +404,9 @@ static void keyspan_irq_recv(struct urb *urb)
 resubmit:
 	retval = usb_submit_urb(urb, GFP_ATOMIC);
 	if (retval)
-		err ("%s - usb_submit_urb failed with result: %d", __func__, retval);
+		dev_err(&dev->interface->dev,
+			"%s - usb_submit_urb failed with result: %d\n",
+			__func__, retval);
 }
 
 static int keyspan_open(struct input_dev *dev)
diff --git a/drivers/input/misc/mpu3050.c b/drivers/input/misc/mpu3050.c
index 5403c571b6a..306f84c2d8f 100644
--- a/drivers/input/misc/mpu3050.c
+++ b/drivers/input/misc/mpu3050.c
@@ -367,7 +367,7 @@ static int __devinit mpu3050_probe(struct i2c_client *client,
 
 	error = request_threaded_irq(client->irq,
 				     NULL, mpu3050_interrupt_thread,
-				     IRQF_TRIGGER_RISING,
+				     IRQF_TRIGGER_RISING | IRQF_ONESHOT,
 				     "mpu3050", sensor);
 	if (error) {
 		dev_err(&client->dev,
diff --git a/drivers/input/misc/powermate.c b/drivers/input/misc/powermate.c
index 538f7049ec6..49c0c3ebd32 100644
--- a/drivers/input/misc/powermate.c
+++ b/drivers/input/misc/powermate.c
@@ -65,6 +65,7 @@ struct powermate_device {
 	struct urb *irq, *config;
 	struct usb_ctrlrequest *configcr;
 	struct usb_device *udev;
+	struct usb_interface *intf;
 	struct input_dev *input;
 	spinlock_t lock;
 	int static_brightness;
@@ -85,6 +86,7 @@ static void powermate_config_complete(struct urb *urb);
 static void powermate_irq(struct urb *urb)
 {
 	struct powermate_device *pm = urb->context;
+	struct device *dev = &pm->intf->dev;
 	int retval;
 
 	switch (urb->status) {
@@ -95,10 +97,12 @@ static void powermate_irq(struct urb *urb)
 	case -ENOENT:
 	case -ESHUTDOWN:
 		/* this urb is terminated, clean up */
-		dbg("%s - urb shutting down with status: %d", __func__, urb->status);
+		dev_dbg(dev, "%s - urb shutting down with status: %d\n",
+			__func__, urb->status);
 		return;
 	default:
-		dbg("%s - nonzero urb status received: %d", __func__, urb->status);
+		dev_dbg(dev, "%s - nonzero urb status received: %d\n",
+			__func__, urb->status);
 		goto exit;
 	}
 
@@ -110,8 +114,8 @@ static void powermate_irq(struct urb *urb)
 exit:
 	retval = usb_submit_urb (urb, GFP_ATOMIC);
 	if (retval)
-		err ("%s - usb_submit_urb failed with result %d",
-		     __func__, retval);
+		dev_err(dev, "%s - usb_submit_urb failed with result: %d\n",
+			__func__, retval);
 }
 
 /* Decide if we need to issue a control message and do so. Must be called with pm->lock taken */
@@ -330,6 +334,7 @@ static int powermate_probe(struct usb_interface *intf, const struct usb_device_i
 		goto fail3;
 
 	pm->udev = udev;
+	pm->intf = intf;
 	pm->input = input_dev;
 
 	usb_make_path(udev, pm->phys, sizeof(pm->phys));
diff --git a/drivers/input/misc/twl6040-vibra.c b/drivers/input/misc/twl6040-vibra.c
index 14e94f56cb7..c34f6c0371c 100644
--- a/drivers/input/misc/twl6040-vibra.c
+++ b/drivers/input/misc/twl6040-vibra.c
@@ -27,6 +27,7 @@
  */
 #include <linux/module.h>
 #include <linux/platform_device.h>
+#include <linux/of.h>
 #include <linux/workqueue.h>
 #include <linux/input.h>
 #include <linux/mfd/twl6040.h>
@@ -258,10 +259,13 @@ static SIMPLE_DEV_PM_OPS(twl6040_vibra_pm_ops, twl6040_vibra_suspend, NULL);
 static int __devinit twl6040_vibra_probe(struct platform_device *pdev)
 {
 	struct twl6040_vibra_data *pdata = pdev->dev.platform_data;
+	struct device_node *node = pdev->dev.of_node;
 	struct vibra_info *info;
+	int vddvibl_uV = 0;
+	int vddvibr_uV = 0;
 	int ret;
 
-	if (!pdata) {
+	if (!pdata && !node) {
 		dev_err(&pdev->dev, "platform_data not available\n");
 		return -EINVAL;
 	}
@@ -273,11 +277,26 @@ static int __devinit twl6040_vibra_probe(struct platform_device *pdev)
 	}
 
 	info->dev = &pdev->dev;
+
 	info->twl6040 = dev_get_drvdata(pdev->dev.parent);
-	info->vibldrv_res = pdata->vibldrv_res;
-	info->vibrdrv_res = pdata->vibrdrv_res;
-	info->viblmotor_res = pdata->viblmotor_res;
-	info->vibrmotor_res = pdata->vibrmotor_res;
+	if (pdata) {
+		info->vibldrv_res = pdata->vibldrv_res;
+		info->vibrdrv_res = pdata->vibrdrv_res;
+		info->viblmotor_res = pdata->viblmotor_res;
+		info->vibrmotor_res = pdata->vibrmotor_res;
+		vddvibl_uV = pdata->vddvibl_uV;
+		vddvibr_uV = pdata->vddvibr_uV;
+	} else {
+		of_property_read_u32(node, "vibldrv_res", &info->vibldrv_res);
+		of_property_read_u32(node, "vibrdrv_res", &info->vibrdrv_res);
+		of_property_read_u32(node, "viblmotor_res",
+				     &info->viblmotor_res);
+		of_property_read_u32(node, "vibrmotor_res",
+				     &info->vibrmotor_res);
+		of_property_read_u32(node, "vddvibl_uV", &vddvibl_uV);
+		of_property_read_u32(node, "vddvibr_uV", &vddvibr_uV);
+	}
+
 	if ((!info->vibldrv_res && !info->viblmotor_res) ||
 	    (!info->vibrdrv_res && !info->vibrmotor_res)) {
 		dev_err(info->dev, "invalid vibra driver/motor resistance\n");
@@ -339,10 +358,9 @@ static int __devinit twl6040_vibra_probe(struct platform_device *pdev)
 		goto err_regulator;
 	}
 
-	if (pdata->vddvibl_uV) {
+	if (vddvibl_uV) {
 		ret = regulator_set_voltage(info->supplies[0].consumer,
-					    pdata->vddvibl_uV,
-					    pdata->vddvibl_uV);
+					    vddvibl_uV, vddvibl_uV);
 		if (ret) {
 			dev_err(info->dev, "failed to set VDDVIBL volt %d\n",
 				ret);
@@ -350,10 +368,9 @@ static int __devinit twl6040_vibra_probe(struct platform_device *pdev)
 		}
 	}
 
-	if (pdata->vddvibr_uV) {
+	if (vddvibr_uV) {
 		ret = regulator_set_voltage(info->supplies[1].consumer,
-					    pdata->vddvibr_uV,
-					    pdata->vddvibr_uV);
+					    vddvibr_uV, vddvibr_uV);
 		if (ret) {
 			dev_err(info->dev, "failed to set VDDVIBR volt %d\n",
 				ret);
@@ -401,6 +418,12 @@ static int __devexit twl6040_vibra_remove(struct platform_device *pdev)
 	return 0;
 }
 
+static const struct of_device_id twl6040_vibra_of_match[] = {
+	{.compatible = "ti,twl6040-vibra", },
+	{ },
+};
+MODULE_DEVICE_TABLE(of, twl6040_vibra_of_match);
+
 static struct platform_driver twl6040_vibra_driver = {
 	.probe		= twl6040_vibra_probe,
 	.remove		= __devexit_p(twl6040_vibra_remove),
@@ -408,6 +431,7 @@ static struct platform_driver twl6040_vibra_driver = {
 		.name	= "twl6040-vibra",
 		.owner	= THIS_MODULE,
 		.pm	= &twl6040_vibra_pm_ops,
+		.of_match_table = twl6040_vibra_of_match,
 	},
 };
 module_platform_driver(twl6040_vibra_driver);
diff --git a/drivers/input/misc/wm831x-on.c b/drivers/input/misc/wm831x-on.c
index 47f18d6bce4..6790a812a1d 100644
--- a/drivers/input/misc/wm831x-on.c
+++ b/drivers/input/misc/wm831x-on.c
@@ -73,7 +73,7 @@ static int __devinit wm831x_on_probe(struct platform_device *pdev)
 {
 	struct wm831x *wm831x = dev_get_drvdata(pdev->dev.parent);
 	struct wm831x_on *wm831x_on;
-	int irq = platform_get_irq(pdev, 0);
+	int irq = wm831x_irq(wm831x, platform_get_irq(pdev, 0));
 	int ret;
 
 	wm831x_on = kzalloc(sizeof(struct wm831x_on), GFP_KERNEL);
diff --git a/drivers/input/misc/yealink.c b/drivers/input/misc/yealink.c
index f4776e7f8c1..285a5bd6cbc 100644
--- a/drivers/input/misc/yealink.c
+++ b/drivers/input/misc/yealink.c
@@ -101,6 +101,7 @@ static const struct lcd_segment_map {
 struct yealink_dev {
 	struct input_dev *idev;		/* input device */
 	struct usb_device *udev;	/* usb device */
+	struct usb_interface *intf;	/* usb interface */
 
 	/* irq input channel */
 	struct yld_ctl_packet	*irq_data;
@@ -428,7 +429,8 @@ static void urb_irq_callback(struct urb *urb)
 	int ret, status = urb->status;
 
 	if (status)
-		err("%s - urb status %d", __func__, status);
+		dev_err(&yld->intf->dev, "%s - urb status %d\n",
+			__func__, status);
 
 	switch (yld->irq_data->cmd) {
 	case CMD_KEYPRESS:
@@ -437,13 +439,15 @@ static void urb_irq_callback(struct urb *urb)
 		break;
 
 	case CMD_SCANCODE:
-		dbg("get scancode %x", yld->irq_data->data[0]);
+		dev_dbg(&yld->intf->dev, "get scancode %x\n",
+			yld->irq_data->data[0]);
 
 		report_key(yld, map_p1k_to_key(yld->irq_data->data[0]));
 		break;
 
 	default:
-		err("unexpected response %x", yld->irq_data->cmd);
+		dev_err(&yld->intf->dev, "unexpected response %x\n",
+			yld->irq_data->cmd);
 	}
 
 	yealink_do_idle_tasks(yld);
@@ -451,7 +455,9 @@ static void urb_irq_callback(struct urb *urb)
 	if (!yld->shutdown) {
 		ret = usb_submit_urb(yld->urb_ctl, GFP_ATOMIC);
 		if (ret && ret != -EPERM)
-			err("%s - usb_submit_urb failed %d", __func__, ret);
+			dev_err(&yld->intf->dev,
+				"%s - usb_submit_urb failed %d\n",
+				__func__, ret);
 	}
 }
 
@@ -461,7 +467,8 @@ static void urb_ctl_callback(struct urb *urb)
 	int ret = 0, status = urb->status;
 
 	if (status)
-		err("%s - urb status %d", __func__, status);
+		dev_err(&yld->intf->dev, "%s - urb status %d\n",
+			__func__, status);
 
 	switch (yld->ctl_data->cmd) {
 	case CMD_KEYPRESS:
@@ -479,7 +486,8 @@ static void urb_ctl_callback(struct urb *urb)
 	}
 
 	if (ret && ret != -EPERM)
-		err("%s - usb_submit_urb failed %d", __func__, ret);
+		dev_err(&yld->intf->dev, "%s - usb_submit_urb failed %d\n",
+			__func__, ret);
 }
 
 /*******************************************************************************
@@ -511,7 +519,7 @@ static int input_open(struct input_dev *dev)
 	struct yealink_dev *yld = input_get_drvdata(dev);
 	int i, ret;
 
-	dbg("%s", __func__);
+	dev_dbg(&yld->intf->dev, "%s\n", __func__);
 
 	/* force updates to device */
 	for (i = 0; i<sizeof(yld->master); i++)
@@ -526,8 +534,9 @@ static int input_open(struct input_dev *dev)
 	yld->ctl_data->size	= 10;
 	yld->ctl_data->sum	= 0x100-CMD_INIT-10;
 	if ((ret = usb_submit_urb(yld->urb_ctl, GFP_KERNEL)) != 0) {
-		dbg("%s - usb_submit_urb failed with result %d",
-		     __func__, ret);
+		dev_dbg(&yld->intf->dev,
+			"%s - usb_submit_urb failed with result %d\n",
+			__func__, ret);
 		return ret;
 	}
 	return 0;
@@ -876,6 +885,7 @@ static int usb_probe(struct usb_interface *intf, const struct usb_device_id *id)
 		return -ENOMEM;
 
 	yld->udev = udev;
+	yld->intf = intf;
 
 	yld->idev = input_dev = input_allocate_device();
 	if (!input_dev)
@@ -909,7 +919,8 @@ static int usb_probe(struct usb_interface *intf, const struct usb_device_id *id)
 	pipe = usb_rcvintpipe(udev, endpoint->bEndpointAddress);
 	ret = usb_maxpacket(udev, pipe, usb_pipeout(pipe));
 	if (ret != USB_PKT_LEN)
-		err("invalid payload size %d, expected %zd", ret, USB_PKT_LEN);
+		dev_err(&intf->dev, "invalid payload size %d, expected %zd\n",
+			ret, USB_PKT_LEN);
 
 	/* initialise irq urb */
 	usb_fill_int_urb(yld->urb_irq, udev, pipe, yld->irq_data,
diff --git a/drivers/input/mouse/Kconfig b/drivers/input/mouse/Kconfig
index 9b8db821d5f..cd6268cf7cd 100644
--- a/drivers/input/mouse/Kconfig
+++ b/drivers/input/mouse/Kconfig
@@ -339,4 +339,16 @@ config MOUSE_SYNAPTICS_USB
 	  To compile this driver as a module, choose M here: the
 	  module will be called synaptics_usb.
 
+config MOUSE_NAVPOINT_PXA27x
+	tristate "Synaptics NavPoint (PXA27x SSP/SPI)"
+	depends on PXA27x && PXA_SSP
+	help
+	  This driver adds support for the Synaptics NavPoint touchpad connected
+	  to a PXA27x SSP port in SPI slave mode. The device emulates a mouse;
+	  a tap or tap-and-a-half drag gesture emulates the left mouse button.
+	  For example, use the xf86-input-evdev driver for an X pointing device.
+
+	  To compile this driver as a module, choose M here: the
+	  module will be called navpoint.
+
 endif
diff --git a/drivers/input/mouse/Makefile b/drivers/input/mouse/Makefile
index 4718effeb8d..46ba7556fd4 100644
--- a/drivers/input/mouse/Makefile
+++ b/drivers/input/mouse/Makefile
@@ -12,6 +12,7 @@ obj-$(CONFIG_MOUSE_GPIO)		+= gpio_mouse.o
 obj-$(CONFIG_MOUSE_INPORT)		+= inport.o
 obj-$(CONFIG_MOUSE_LOGIBM)		+= logibm.o
 obj-$(CONFIG_MOUSE_MAPLE)		+= maplemouse.o
+obj-$(CONFIG_MOUSE_NAVPOINT_PXA27x)	+= navpoint.o
 obj-$(CONFIG_MOUSE_PC110PAD)		+= pc110pad.o
 obj-$(CONFIG_MOUSE_PS2)			+= psmouse.o
 obj-$(CONFIG_MOUSE_PXA930_TRKBALL)	+= pxa930_trkball.o
diff --git a/drivers/input/mouse/alps.c b/drivers/input/mouse/alps.c
index 4c6a72d3d48..4a1347e91bd 100644
--- a/drivers/input/mouse/alps.c
+++ b/drivers/input/mouse/alps.c
@@ -553,10 +553,7 @@ static void alps_process_touchpad_packet_v3(struct psmouse *psmouse)
 
 	alps_report_semi_mt_data(dev, fingers, x1, y1, x2, y2);
 
-	input_report_key(dev, BTN_TOOL_FINGER, fingers == 1);
-	input_report_key(dev, BTN_TOOL_DOUBLETAP, fingers == 2);
-	input_report_key(dev, BTN_TOOL_TRIPLETAP, fingers == 3);
-	input_report_key(dev, BTN_TOOL_QUADTAP, fingers == 4);
+	input_mt_report_finger_count(dev, fingers);
 
 	input_report_key(dev, BTN_LEFT, left);
 	input_report_key(dev, BTN_RIGHT, right);
@@ -604,10 +601,54 @@ static void alps_process_packet_v3(struct psmouse *psmouse)
 
 static void alps_process_packet_v4(struct psmouse *psmouse)
 {
+	struct alps_data *priv = psmouse->private;
 	unsigned char *packet = psmouse->packet;
 	struct input_dev *dev = psmouse->dev;
+	int offset;
 	int x, y, z;
 	int left, right;
+	int x1, y1, x2, y2;
+	int fingers = 0;
+	unsigned int x_bitmap, y_bitmap;
+
+	/*
+	 * v4 has a 6-byte encoding for bitmap data, but this data is
+	 * broken up between 3 normal packets. Use priv->multi_packet to
+	 * track our position in the bitmap packet.
+	 */
+	if (packet[6] & 0x40) {
+		/* sync, reset position */
+		priv->multi_packet = 0;
+	}
+
+	if (WARN_ON_ONCE(priv->multi_packet > 2))
+		return;
+
+	offset = 2 * priv->multi_packet;
+	priv->multi_data[offset] = packet[6];
+	priv->multi_data[offset + 1] = packet[7];
+
+	if (++priv->multi_packet > 2) {
+		priv->multi_packet = 0;
+
+		x_bitmap = ((priv->multi_data[2] & 0x1f) << 10) |
+			   ((priv->multi_data[3] & 0x60) << 3) |
+			   ((priv->multi_data[0] & 0x3f) << 2) |
+			   ((priv->multi_data[1] & 0x60) >> 5);
+		y_bitmap = ((priv->multi_data[5] & 0x01) << 10) |
+			   ((priv->multi_data[3] & 0x1f) << 5) |
+			    (priv->multi_data[1] & 0x1f);
+
+		fingers = alps_process_bitmap(x_bitmap, y_bitmap,
+					      &x1, &y1, &x2, &y2);
+
+		/* Store MT data.*/
+		priv->fingers = fingers;
+		priv->x1 = x1;
+		priv->x2 = x2;
+		priv->y1 = y1;
+		priv->y2 = y2;
+	}
 
 	left = packet[4] & 0x01;
 	right = packet[4] & 0x02;
@@ -617,21 +658,41 @@ static void alps_process_packet_v4(struct psmouse *psmouse)
 	y = ((packet[2] & 0x7f) << 4) | (packet[3] & 0x0f);
 	z = packet[5] & 0x7f;
 
+	/*
+	 * If there were no contacts in the bitmap, use ST
+	 * points in MT reports.
+	 * If there were two contacts or more, report MT data.
+	 */
+	if (priv->fingers < 2) {
+		x1 = x;
+		y1 = y;
+		fingers = z > 0 ? 1 : 0;
+	} else {
+		fingers = priv->fingers;
+		x1 = priv->x1;
+		x2 = priv->x2;
+		y1 = priv->y1;
+		y2 = priv->y2;
+	}
+
 	if (z >= 64)
 		input_report_key(dev, BTN_TOUCH, 1);
 	else
 		input_report_key(dev, BTN_TOUCH, 0);
 
+	alps_report_semi_mt_data(dev, fingers, x1, y1, x2, y2);
+
+	input_mt_report_finger_count(dev, fingers);
+
+	input_report_key(dev, BTN_LEFT, left);
+	input_report_key(dev, BTN_RIGHT, right);
+
 	if (z > 0) {
 		input_report_abs(dev, ABS_X, x);
 		input_report_abs(dev, ABS_Y, y);
 	}
 	input_report_abs(dev, ABS_PRESSURE, z);
 
-	input_report_key(dev, BTN_TOOL_FINGER, z > 0);
-	input_report_key(dev, BTN_LEFT, left);
-	input_report_key(dev, BTN_RIGHT, right);
-
 	input_sync(dev);
 }
 
@@ -1557,6 +1618,7 @@ int alps_init(struct psmouse *psmouse)
 		input_set_abs_params(dev1, ABS_Y, 0, 767, 0, 0);
 		break;
 	case ALPS_PROTO_V3:
+	case ALPS_PROTO_V4:
 		set_bit(INPUT_PROP_SEMI_MT, dev1->propbit);
 		input_mt_init_slots(dev1, 2);
 		input_set_abs_params(dev1, ABS_MT_POSITION_X, 0, ALPS_V3_X_MAX, 0, 0);
@@ -1565,8 +1627,7 @@ int alps_init(struct psmouse *psmouse)
 		set_bit(BTN_TOOL_DOUBLETAP, dev1->keybit);
 		set_bit(BTN_TOOL_TRIPLETAP, dev1->keybit);
 		set_bit(BTN_TOOL_QUADTAP, dev1->keybit);
-		/* fall through */
-	case ALPS_PROTO_V4:
+
 		input_set_abs_params(dev1, ABS_X, 0, ALPS_V3_X_MAX, 0, 0);
 		input_set_abs_params(dev1, ABS_Y, 0, ALPS_V3_Y_MAX, 0, 0);
 		break;
diff --git a/drivers/input/mouse/alps.h b/drivers/input/mouse/alps.h
index a00a4ab92a0..ae1ac354c77 100644
--- a/drivers/input/mouse/alps.h
+++ b/drivers/input/mouse/alps.h
@@ -39,6 +39,8 @@ struct alps_data {
 	int prev_fin;			/* Finger bit from previous packet */
 	int multi_packet;		/* Multi-packet data in progress */
 	unsigned char multi_data[6];	/* Saved multi-packet data */
+	int x1, x2, y1, y2;		/* Coordinates from last MT report */
+	int fingers;			/* Number of fingers from MT report */
 	u8 quirks;
 	struct timer_list timer;
 };
diff --git a/drivers/input/mouse/appletouch.c b/drivers/input/mouse/appletouch.c
index 0acbc7d50d0..e42f1fa8cdc 100644
--- a/drivers/input/mouse/appletouch.c
+++ b/drivers/input/mouse/appletouch.c
@@ -195,6 +195,7 @@ enum atp_status_bits {
 struct atp {
 	char			phys[64];
 	struct usb_device	*udev;		/* usb device */
+	struct usb_interface	*intf;		/* usb interface */
 	struct urb		*urb;		/* usb request block */
 	u8			*data;		/* transferred data */
 	struct input_dev	*input;		/* input dev */
@@ -253,8 +254,9 @@ MODULE_PARM_DESC(debug, "Activate debugging output");
  * packets (Report ID 2). This code changes device mode, so it
  * sends raw sensor reports (Report ID 5).
  */
-static int atp_geyser_init(struct usb_device *udev)
+static int atp_geyser_init(struct atp *dev)
 {
+	struct usb_device *udev = dev->udev;
 	char *data;
 	int size;
 	int i;
@@ -262,7 +264,7 @@ static int atp_geyser_init(struct usb_device *udev)
 
 	data = kmalloc(8, GFP_KERNEL);
 	if (!data) {
-		err("Out of memory");
+		dev_err(&dev->intf->dev, "Out of memory\n");
 		return -ENOMEM;
 	}
 
@@ -277,7 +279,7 @@ static int atp_geyser_init(struct usb_device *udev)
 		for (i = 0; i < 8; i++)
 			dprintk("appletouch[%d]: %d\n", i, data[i]);
 
-		err("Failed to read mode from device.");
+		dev_err(&dev->intf->dev, "Failed to read mode from device.\n");
 		ret = -EIO;
 		goto out_free;
 	}
@@ -296,7 +298,7 @@ static int atp_geyser_init(struct usb_device *udev)
 		for (i = 0; i < 8; i++)
 			dprintk("appletouch[%d]: %d\n", i, data[i]);
 
-		err("Failed to request geyser raw mode");
+		dev_err(&dev->intf->dev, "Failed to request geyser raw mode\n");
 		ret = -EIO;
 		goto out_free;
 	}
@@ -313,16 +315,16 @@ out_free:
 static void atp_reinit(struct work_struct *work)
 {
 	struct atp *dev = container_of(work, struct atp, work);
-	struct usb_device *udev = dev->udev;
 	int retval;
 
 	dprintk("appletouch: putting appletouch to sleep (reinit)\n");
-	atp_geyser_init(udev);
+	atp_geyser_init(dev);
 
 	retval = usb_submit_urb(dev->urb, GFP_ATOMIC);
 	if (retval)
-		err("atp_reinit: usb_submit_urb failed with error %d",
-		    retval);
+		dev_err(&dev->intf->dev,
+			"atp_reinit: usb_submit_urb failed with error %d\n",
+			retval);
 }
 
 static int atp_calculate_abs(int *xy_sensors, int nb_sensors, int fact,
@@ -400,6 +402,7 @@ static inline void atp_report_fingers(struct input_dev *input, int fingers)
 static int atp_status_check(struct urb *urb)
 {
 	struct atp *dev = urb->context;
+	struct usb_interface *intf = dev->intf;
 
 	switch (urb->status) {
 	case 0:
@@ -407,8 +410,8 @@ static int atp_status_check(struct urb *urb)
 		break;
 	case -EOVERFLOW:
 		if (!dev->overflow_warned) {
-			printk(KERN_WARNING "appletouch: OVERFLOW with data "
-				"length %d, actual length is %d\n",
+			dev_warn(&intf->dev,
+				"appletouch: OVERFLOW with data length %d, actual length is %d\n",
 				dev->info->datalen, dev->urb->actual_length);
 			dev->overflow_warned = true;
 		}
@@ -416,13 +419,15 @@ static int atp_status_check(struct urb *urb)
 	case -ENOENT:
 	case -ESHUTDOWN:
 		/* This urb is terminated, clean up */
-		dbg("atp_complete: urb shutting down with status: %d",
-		    urb->status);
+		dev_dbg(&intf->dev,
+			"atp_complete: urb shutting down with status: %d\n",
+			urb->status);
 		return ATP_URB_STATUS_ERROR_FATAL;
 
 	default:
-		dbg("atp_complete: nonzero urb status received: %d",
-		    urb->status);
+		dev_dbg(&intf->dev,
+			"atp_complete: nonzero urb status received: %d\n",
+			urb->status);
 		return ATP_URB_STATUS_ERROR;
 	}
 
@@ -445,7 +450,8 @@ static void atp_detect_size(struct atp *dev)
 	for (i = dev->info->xsensors; i < ATP_XSENSORS; i++) {
 		if (dev->xy_cur[i]) {
 
-			printk(KERN_INFO "appletouch: 17\" model detected.\n");
+			dev_info(&dev->intf->dev,
+				"appletouch: 17\" model detected.\n");
 
 			input_set_abs_params(dev->input, ABS_X, 0,
 					     (dev->info->xsensors_17 - 1) *
@@ -588,8 +594,9 @@ static void atp_complete_geyser_1_2(struct urb *urb)
  exit:
 	retval = usb_submit_urb(dev->urb, GFP_ATOMIC);
 	if (retval)
-		err("atp_complete: usb_submit_urb failed with result %d",
-		    retval);
+		dev_err(&dev->intf->dev,
+			"atp_complete: usb_submit_urb failed with result %d\n",
+			retval);
 }
 
 /* Interrupt function for older touchpads: GEYSER3/GEYSER4 */
@@ -722,8 +729,9 @@ static void atp_complete_geyser_3_4(struct urb *urb)
  exit:
 	retval = usb_submit_urb(dev->urb, GFP_ATOMIC);
 	if (retval)
-		err("atp_complete: usb_submit_urb failed with result %d",
-		    retval);
+		dev_err(&dev->intf->dev,
+			"atp_complete: usb_submit_urb failed with result %d\n",
+			retval);
 }
 
 static int atp_open(struct input_dev *input)
@@ -748,14 +756,12 @@ static void atp_close(struct input_dev *input)
 
 static int atp_handle_geyser(struct atp *dev)
 {
-	struct usb_device *udev = dev->udev;
-
 	if (dev->info != &fountain_info) {
 		/* switch to raw sensor mode */
-		if (atp_geyser_init(udev))
+		if (atp_geyser_init(dev))
 			return -EIO;
 
-		printk(KERN_INFO "appletouch: Geyser mode initialized.\n");
+		dev_info(&dev->intf->dev, "Geyser mode initialized.\n");
 	}
 
 	return 0;
@@ -785,7 +791,7 @@ static int atp_probe(struct usb_interface *iface,
 		}
 	}
 	if (!int_in_endpointAddr) {
-		err("Could not find int-in endpoint");
+		dev_err(&iface->dev, "Could not find int-in endpoint\n");
 		return -EIO;
 	}
 
@@ -793,11 +799,12 @@ static int atp_probe(struct usb_interface *iface,
 	dev = kzalloc(sizeof(struct atp), GFP_KERNEL);
 	input_dev = input_allocate_device();
 	if (!dev || !input_dev) {
-		err("Out of memory");
+		dev_err(&iface->dev, "Out of memory\n");
 		goto err_free_devs;
 	}
 
 	dev->udev = udev;
+	dev->intf = iface;
 	dev->input = input_dev;
 	dev->info = info;
 	dev->overflow_warned = false;
@@ -886,7 +893,7 @@ static void atp_disconnect(struct usb_interface *iface)
 		usb_free_urb(dev->urb);
 		kfree(dev);
 	}
-	printk(KERN_INFO "input: appletouch disconnected\n");
+	dev_info(&iface->dev, "input: appletouch disconnected\n");
 }
 
 static int atp_recover(struct atp *dev)
diff --git a/drivers/input/mouse/bcm5974.c b/drivers/input/mouse/bcm5974.c
index f9e2758b9f4..2cf681d98c0 100644
--- a/drivers/input/mouse/bcm5974.c
+++ b/drivers/input/mouse/bcm5974.c
@@ -584,7 +584,7 @@ static int bcm5974_wellspring_mode(struct bcm5974 *dev, bool on)
 	int retval = 0, size;
 
 	if (!data) {
-		err("bcm5974: out of memory");
+		dev_err(&dev->intf->dev, "out of memory\n");
 		retval = -ENOMEM;
 		goto out;
 	}
@@ -597,7 +597,7 @@ static int bcm5974_wellspring_mode(struct bcm5974 *dev, bool on)
 			BCM5974_WELLSPRING_MODE_REQUEST_INDEX, data, 8, 5000);
 
 	if (size != 8) {
-		err("bcm5974: could not read from device");
+		dev_err(&dev->intf->dev, "could not read from device\n");
 		retval = -EIO;
 		goto out;
 	}
@@ -615,7 +615,7 @@ static int bcm5974_wellspring_mode(struct bcm5974 *dev, bool on)
 			BCM5974_WELLSPRING_MODE_REQUEST_INDEX, data, 8, 5000);
 
 	if (size != 8) {
-		err("bcm5974: could not write to device");
+		dev_err(&dev->intf->dev, "could not write to device\n");
 		retval = -EIO;
 		goto out;
 	}
@@ -631,6 +631,7 @@ static int bcm5974_wellspring_mode(struct bcm5974 *dev, bool on)
 static void bcm5974_irq_button(struct urb *urb)
 {
 	struct bcm5974 *dev = urb->context;
+	struct usb_interface *intf = dev->intf;
 	int error;
 
 	switch (urb->status) {
@@ -640,10 +641,11 @@ static void bcm5974_irq_button(struct urb *urb)
 	case -ECONNRESET:
 	case -ENOENT:
 	case -ESHUTDOWN:
-		dbg("bcm5974: button urb shutting down: %d", urb->status);
+		dev_dbg(&intf->dev, "button urb shutting down: %d\n",
+			urb->status);
 		return;
 	default:
-		dbg("bcm5974: button urb status: %d", urb->status);
+		dev_dbg(&intf->dev, "button urb status: %d\n", urb->status);
 		goto exit;
 	}
 
@@ -654,12 +656,13 @@ static void bcm5974_irq_button(struct urb *urb)
 exit:
 	error = usb_submit_urb(dev->bt_urb, GFP_ATOMIC);
 	if (error)
-		err("bcm5974: button urb failed: %d", error);
+		dev_err(&intf->dev, "button urb failed: %d\n", error);
 }
 
 static void bcm5974_irq_trackpad(struct urb *urb)
 {
 	struct bcm5974 *dev = urb->context;
+	struct usb_interface *intf = dev->intf;
 	int error;
 
 	switch (urb->status) {
@@ -669,10 +672,11 @@ static void bcm5974_irq_trackpad(struct urb *urb)
 	case -ECONNRESET:
 	case -ENOENT:
 	case -ESHUTDOWN:
-		dbg("bcm5974: trackpad urb shutting down: %d", urb->status);
+		dev_dbg(&intf->dev, "trackpad urb shutting down: %d\n",
+			urb->status);
 		return;
 	default:
-		dbg("bcm5974: trackpad urb status: %d", urb->status);
+		dev_dbg(&intf->dev, "trackpad urb status: %d\n", urb->status);
 		goto exit;
 	}
 
@@ -687,7 +691,7 @@ static void bcm5974_irq_trackpad(struct urb *urb)
 exit:
 	error = usb_submit_urb(dev->tp_urb, GFP_ATOMIC);
 	if (error)
-		err("bcm5974: trackpad urb failed: %d", error);
+		dev_err(&intf->dev, "trackpad urb failed: %d\n", error);
 }
 
 /*
@@ -833,7 +837,7 @@ static int bcm5974_probe(struct usb_interface *iface,
 	dev = kzalloc(sizeof(struct bcm5974), GFP_KERNEL);
 	input_dev = input_allocate_device();
 	if (!dev || !input_dev) {
-		err("bcm5974: out of memory");
+		dev_err(&iface->dev, "out of memory\n");
 		goto err_free_devs;
 	}
 
diff --git a/drivers/input/mouse/navpoint.c b/drivers/input/mouse/navpoint.c
new file mode 100644
index 00000000000..c29ae7654d5
--- /dev/null
+++ b/drivers/input/mouse/navpoint.c
@@ -0,0 +1,369 @@
+/*
+ * Synaptics NavPoint (PXA27x SSP/SPI) driver.
+ *
+ * Copyright (C) 2012 Paul Parsons <lost.distance@yahoo.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/init.h>
+#include <linux/module.h>
+#include <linux/platform_device.h>
+#include <linux/clk.h>
+#include <linux/delay.h>
+#include <linux/gpio.h>
+#include <linux/input.h>
+#include <linux/input/navpoint.h>
+#include <linux/interrupt.h>
+#include <linux/mutex.h>
+#include <linux/pxa2xx_ssp.h>
+#include <linux/slab.h>
+
+/*
+ * Synaptics Modular Embedded Protocol: Module Packet Format.
+ * Module header byte 2:0 = Length (# bytes that follow)
+ * Module header byte 4:3 = Control
+ * Module header byte 7:5 = Module Address
+ */
+#define HEADER_LENGTH(byte)	((byte) & 0x07)
+#define HEADER_CONTROL(byte)	(((byte) >> 3) & 0x03)
+#define HEADER_ADDRESS(byte)	((byte) >> 5)
+
+struct navpoint {
+	struct ssp_device	*ssp;
+	struct input_dev	*input;
+	struct device		*dev;
+	int			gpio;
+	int			index;
+	u8			data[1 + HEADER_LENGTH(0xff)];
+};
+
+/*
+ * Initialization values for SSCR0_x, SSCR1_x, SSSR_x.
+ */
+static const u32 sscr0 = 0
+	| SSCR0_TUM		/* TIM = 1; No TUR interrupts */
+	| SSCR0_RIM		/* RIM = 1; No ROR interrupts */
+	| SSCR0_SSE		/* SSE = 1; SSP enabled */
+	| SSCR0_Motorola	/* FRF = 0; Motorola SPI */
+	| SSCR0_DataSize(16)	/* DSS = 15; Data size = 16-bit */
+	;
+static const u32 sscr1 = 0
+	| SSCR1_SCFR		/* SCFR = 1; SSPSCLK only during transfers */
+	| SSCR1_SCLKDIR		/* SCLKDIR = 1; Slave mode */
+	| SSCR1_SFRMDIR		/* SFRMDIR = 1; Slave mode */
+	| SSCR1_RWOT		/* RWOT = 1; Receive without transmit mode */
+	| SSCR1_RxTresh(1)	/* RFT = 0; Receive FIFO threshold = 1 */
+	| SSCR1_SPH		/* SPH = 1; SSPSCLK inactive 0.5 + 1 cycles */
+	| SSCR1_RIE		/* RIE = 1; Receive FIFO interrupt enabled */
+	;
+static const u32 sssr = 0
+	| SSSR_BCE		/* BCE = 1; Clear BCE */
+	| SSSR_TUR		/* TUR = 1; Clear TUR */
+	| SSSR_EOC		/* EOC = 1; Clear EOC */
+	| SSSR_TINT		/* TINT = 1; Clear TINT */
+	| SSSR_PINT		/* PINT = 1; Clear PINT */
+	| SSSR_ROR		/* ROR = 1; Clear ROR */
+	;
+
+/*
+ * MEP Query $22: Touchpad Coordinate Range Query is not supported by
+ * the NavPoint module, so sampled values provide the default limits.
+ */
+#define NAVPOINT_X_MIN		1278
+#define NAVPOINT_X_MAX		5340
+#define NAVPOINT_Y_MIN		1572
+#define NAVPOINT_Y_MAX		4396
+#define NAVPOINT_PRESSURE_MIN	0
+#define NAVPOINT_PRESSURE_MAX	255
+
+static void navpoint_packet(struct navpoint *navpoint)
+{
+	int finger;
+	int gesture;
+	int x, y, z;
+
+	switch (navpoint->data[0]) {
+	case 0xff:	/* Garbage (packet?) between reset and Hello packet */
+	case 0x00:	/* Module 0, NULL packet */
+		break;
+
+	case 0x0e:	/* Module 0, Absolute packet */
+		finger = (navpoint->data[1] & 0x01);
+		gesture = (navpoint->data[1] & 0x02);
+		x = ((navpoint->data[2] & 0x1f) << 8) | navpoint->data[3];
+		y = ((navpoint->data[4] & 0x1f) << 8) | navpoint->data[5];
+		z = navpoint->data[6];
+		input_report_key(navpoint->input, BTN_TOUCH, finger);
+		input_report_abs(navpoint->input, ABS_X, x);
+		input_report_abs(navpoint->input, ABS_Y, y);
+		input_report_abs(navpoint->input, ABS_PRESSURE, z);
+		input_report_key(navpoint->input, BTN_TOOL_FINGER, finger);
+		input_report_key(navpoint->input, BTN_LEFT, gesture);
+		input_sync(navpoint->input);
+		break;
+
+	case 0x19:	/* Module 0, Hello packet */
+		if ((navpoint->data[1] & 0xf0) == 0x10)
+			break;
+		/* FALLTHROUGH */
+	default:
+		dev_warn(navpoint->dev,
+			 "spurious packet: data=0x%02x,0x%02x,...\n",
+			 navpoint->data[0], navpoint->data[1]);
+		break;
+	}
+}
+
+static irqreturn_t navpoint_irq(int irq, void *dev_id)
+{
+	struct navpoint *navpoint = dev_id;
+	struct ssp_device *ssp = navpoint->ssp;
+	irqreturn_t ret = IRQ_NONE;
+	u32 status;
+
+	status = pxa_ssp_read_reg(ssp, SSSR);
+	if (status & sssr) {
+		dev_warn(navpoint->dev,
+			 "unexpected interrupt: status=0x%08x\n", status);
+		pxa_ssp_write_reg(ssp, SSSR, (status & sssr));
+		ret = IRQ_HANDLED;
+	}
+
+	while (status & SSSR_RNE) {
+		u32 data;
+
+		data = pxa_ssp_read_reg(ssp, SSDR);
+		navpoint->data[navpoint->index + 0] = (data >> 8);
+		navpoint->data[navpoint->index + 1] = data;
+		navpoint->index += 2;
+		if (HEADER_LENGTH(navpoint->data[0]) < navpoint->index) {
+			navpoint_packet(navpoint);
+			navpoint->index = 0;
+		}
+		status = pxa_ssp_read_reg(ssp, SSSR);
+		ret = IRQ_HANDLED;
+	}
+
+	return ret;
+}
+
+static void navpoint_up(struct navpoint *navpoint)
+{
+	struct ssp_device *ssp = navpoint->ssp;
+	int timeout;
+
+	clk_prepare_enable(ssp->clk);
+
+	pxa_ssp_write_reg(ssp, SSCR1, sscr1);
+	pxa_ssp_write_reg(ssp, SSSR, sssr);
+	pxa_ssp_write_reg(ssp, SSTO, 0);
+	pxa_ssp_write_reg(ssp, SSCR0, sscr0);	/* SSCR0_SSE written last */
+
+	/* Wait until SSP port is ready for slave clock operations */
+	for (timeout = 100; timeout != 0; --timeout) {
+		if (!(pxa_ssp_read_reg(ssp, SSSR) & SSSR_CSS))
+			break;
+		msleep(1);
+	}
+
+	if (timeout == 0)
+		dev_err(navpoint->dev,
+			"timeout waiting for SSSR[CSS] to clear\n");
+
+	if (gpio_is_valid(navpoint->gpio))
+		gpio_set_value(navpoint->gpio, 1);
+}
+
+static void navpoint_down(struct navpoint *navpoint)
+{
+	struct ssp_device *ssp = navpoint->ssp;
+
+	if (gpio_is_valid(navpoint->gpio))
+		gpio_set_value(navpoint->gpio, 0);
+
+	pxa_ssp_write_reg(ssp, SSCR0, 0);
+
+	clk_disable_unprepare(ssp->clk);
+}
+
+static int navpoint_open(struct input_dev *input)
+{
+	struct navpoint *navpoint = input_get_drvdata(input);
+
+	navpoint_up(navpoint);
+
+	return 0;
+}
+
+static void navpoint_close(struct input_dev *input)
+{
+	struct navpoint *navpoint = input_get_drvdata(input);
+
+	navpoint_down(navpoint);
+}
+
+static int __devinit navpoint_probe(struct platform_device *pdev)
+{
+	const struct navpoint_platform_data *pdata =
+					dev_get_platdata(&pdev->dev);
+	struct ssp_device *ssp;
+	struct input_dev *input;
+	struct navpoint *navpoint;
+	int error;
+
+	if (!pdata) {
+		dev_err(&pdev->dev, "no platform data\n");
+		return -EINVAL;
+	}
+
+	if (gpio_is_valid(pdata->gpio)) {
+		error = gpio_request_one(pdata->gpio, GPIOF_OUT_INIT_LOW,
+					 "SYNAPTICS_ON");
+		if (error)
+			return error;
+	}
+
+	ssp = pxa_ssp_request(pdata->port, pdev->name);
+	if (!ssp) {
+		error = -ENODEV;
+		goto err_free_gpio;
+	}
+
+	/* HaRET does not disable devices before jumping into Linux */
+	if (pxa_ssp_read_reg(ssp, SSCR0) & SSCR0_SSE) {
+		pxa_ssp_write_reg(ssp, SSCR0, 0);
+		dev_warn(&pdev->dev, "ssp%d already enabled\n", pdata->port);
+	}
+
+	navpoint = kzalloc(sizeof(*navpoint), GFP_KERNEL);
+	input = input_allocate_device();
+	if (!navpoint || !input) {
+		error = -ENOMEM;
+		goto err_free_mem;
+	}
+
+	navpoint->ssp = ssp;
+	navpoint->input = input;
+	navpoint->dev = &pdev->dev;
+	navpoint->gpio = pdata->gpio;
+
+	input->name = pdev->name;
+	input->dev.parent = &pdev->dev;
+
+	__set_bit(EV_KEY, input->evbit);
+	__set_bit(EV_ABS, input->evbit);
+	__set_bit(BTN_LEFT, input->keybit);
+	__set_bit(BTN_TOUCH, input->keybit);
+	__set_bit(BTN_TOOL_FINGER, input->keybit);
+
+	input_set_abs_params(input, ABS_X,
+			     NAVPOINT_X_MIN, NAVPOINT_X_MAX, 0, 0);
+	input_set_abs_params(input, ABS_Y,
+			     NAVPOINT_Y_MIN, NAVPOINT_Y_MAX, 0, 0);
+	input_set_abs_params(input, ABS_PRESSURE,
+			     NAVPOINT_PRESSURE_MIN, NAVPOINT_PRESSURE_MAX,
+			     0, 0);
+
+	input->open = navpoint_open;
+	input->close = navpoint_close;
+
+	input_set_drvdata(input, navpoint);
+
+	error = request_irq(ssp->irq, navpoint_irq, 0, pdev->name, navpoint);
+	if (error)
+		goto err_free_mem;
+
+	error = input_register_device(input);
+	if (error)
+		goto err_free_irq;
+
+	platform_set_drvdata(pdev, navpoint);
+	dev_dbg(&pdev->dev, "ssp%d, irq %d\n", pdata->port, ssp->irq);
+
+	return 0;
+
+err_free_irq:
+	free_irq(ssp->irq, &pdev->dev);
+err_free_mem:
+	input_free_device(input);
+	kfree(navpoint);
+	pxa_ssp_free(ssp);
+err_free_gpio:
+	if (gpio_is_valid(pdata->gpio))
+		gpio_free(pdata->gpio);
+
+	return error;
+}
+
+static int __devexit navpoint_remove(struct platform_device *pdev)
+{
+	const struct navpoint_platform_data *pdata =
+					dev_get_platdata(&pdev->dev);
+	struct navpoint *navpoint = platform_get_drvdata(pdev);
+	struct ssp_device *ssp = navpoint->ssp;
+
+	free_irq(ssp->irq, navpoint);
+
+	input_unregister_device(navpoint->input);
+	kfree(navpoint);
+
+	pxa_ssp_free(ssp);
+
+	if (gpio_is_valid(pdata->gpio))
+		gpio_free(pdata->gpio);
+
+	return 0;
+}
+
+#ifdef CONFIG_PM_SLEEP
+static int navpoint_suspend(struct device *dev)
+{
+	struct platform_device *pdev = to_platform_device(dev);
+	struct navpoint *navpoint = platform_get_drvdata(pdev);
+	struct input_dev *input = navpoint->input;
+
+	mutex_lock(&input->mutex);
+	if (input->users)
+		navpoint_down(navpoint);
+	mutex_unlock(&input->mutex);
+
+	return 0;
+}
+
+static int navpoint_resume(struct device *dev)
+{
+	struct platform_device *pdev = to_platform_device(dev);
+	struct navpoint *navpoint = platform_get_drvdata(pdev);
+	struct input_dev *input = navpoint->input;
+
+	mutex_lock(&input->mutex);
+	if (input->users)
+		navpoint_up(navpoint);
+	mutex_unlock(&input->mutex);
+
+	return 0;
+}
+#endif
+
+static SIMPLE_DEV_PM_OPS(navpoint_pm_ops, navpoint_suspend, navpoint_resume);
+
+static struct platform_driver navpoint_driver = {
+	.probe		= navpoint_probe,
+	.remove		= __devexit_p(navpoint_remove),
+	.driver = {
+		.name	= "navpoint",
+		.owner	= THIS_MODULE,
+		.pm	= &navpoint_pm_ops,
+	},
+};
+
+module_platform_driver(navpoint_driver);
+
+MODULE_AUTHOR("Paul Parsons <lost.distance@yahoo.com>");
+MODULE_DESCRIPTION("Synaptics NavPoint (PXA27x SSP/SPI) driver");
+MODULE_LICENSE("GPL");
+MODULE_ALIAS("platform:navpoint");
diff --git a/drivers/input/mouse/sentelic.c b/drivers/input/mouse/sentelic.c
index 661a0ca3b3d..3f5649f1908 100644
--- a/drivers/input/mouse/sentelic.c
+++ b/drivers/input/mouse/sentelic.c
@@ -41,7 +41,7 @@
 #define	GET_ABS_Y(packet)	((packet[2] << 2) | (packet[3] & 0x03))
 
 /** Driver version. */
-static const char fsp_drv_ver[] = "1.0.0-K";
+static const char fsp_drv_ver[] = "1.1.0-K";
 
 /*
  * Make sure that the value being sent to FSP will not conflict with
@@ -303,6 +303,27 @@ static int fsp_get_revision(struct psmouse *psmouse, int *rev)
 	return 0;
 }
 
+static int fsp_get_sn(struct psmouse *psmouse, int *sn)
+{
+	int v0, v1, v2;
+	int rc = -EIO;
+
+	/* production number since Cx is available at: 0x0b40 ~ 0x0b42 */
+	if (fsp_page_reg_write(psmouse, FSP_PAGE_0B))
+		goto out;
+	if (fsp_reg_read(psmouse, FSP_REG_SN0, &v0))
+		goto out;
+	if (fsp_reg_read(psmouse, FSP_REG_SN1, &v1))
+		goto out;
+	if (fsp_reg_read(psmouse, FSP_REG_SN2, &v2))
+		goto out;
+	*sn = (v0 << 16) | (v1 << 8) | v2;
+	rc = 0;
+out:
+	fsp_page_reg_write(psmouse, FSP_PAGE_DEFAULT);
+	return rc;
+}
+
 static int fsp_get_buttons(struct psmouse *psmouse, int *btn)
 {
 	static const int buttons[] = {
@@ -1000,16 +1021,21 @@ static int fsp_reconnect(struct psmouse *psmouse)
 int fsp_init(struct psmouse *psmouse)
 {
 	struct fsp_data *priv;
-	int ver, rev;
+	int ver, rev, sn = 0;
 	int error;
 
 	if (fsp_get_version(psmouse, &ver) ||
 	    fsp_get_revision(psmouse, &rev)) {
 		return -ENODEV;
 	}
+	if (ver >= FSP_VER_STL3888_C0) {
+		/* firmware information is only available since C0 */
+		fsp_get_sn(psmouse, &sn);
+	}
 
-	psmouse_info(psmouse, "Finger Sensing Pad, hw: %d.%d.%d, sw: %s\n",
-		     ver >> 4, ver & 0x0F, rev, fsp_drv_ver);
+	psmouse_info(psmouse,
+		     "Finger Sensing Pad, hw: %d.%d.%d, sn: %x, sw: %s\n",
+		     ver >> 4, ver & 0x0F, rev, sn, fsp_drv_ver);
 
 	psmouse->private = priv = kzalloc(sizeof(struct fsp_data), GFP_KERNEL);
 	if (!priv)
diff --git a/drivers/input/mouse/sentelic.h b/drivers/input/mouse/sentelic.h
index 334de19e5dd..aa697ece405 100644
--- a/drivers/input/mouse/sentelic.h
+++ b/drivers/input/mouse/sentelic.h
@@ -65,6 +65,14 @@
 #define	FSP_BIT_SWC1_GST_GRP1	BIT(6)
 #define	FSP_BIT_SWC1_BX_COMPAT	BIT(7)
 
+#define	FSP_PAGE_0B		(0x0b)
+#define	FSP_PAGE_82		(0x82)
+#define	FSP_PAGE_DEFAULT	FSP_PAGE_82
+
+#define	FSP_REG_SN0		(0x40)
+#define	FSP_REG_SN1		(0x41)
+#define	FSP_REG_SN2		(0x42)
+
 /* Finger-sensing Pad packet formating related definitions */
 
 /* absolute packet type */
diff --git a/drivers/input/mouse/sermouse.c b/drivers/input/mouse/sermouse.c
index 17ff137b9bd..d5928fd0c91 100644
--- a/drivers/input/mouse/sermouse.c
+++ b/drivers/input/mouse/sermouse.c
@@ -355,15 +355,4 @@ static struct serio_driver sermouse_drv = {
 	.disconnect	= sermouse_disconnect,
 };
 
-static int __init sermouse_init(void)
-{
-	return serio_register_driver(&sermouse_drv);
-}
-
-static void __exit sermouse_exit(void)
-{
-	serio_unregister_driver(&sermouse_drv);
-}
-
-module_init(sermouse_init);
-module_exit(sermouse_exit);
+module_serio_driver(sermouse_drv);
diff --git a/drivers/input/mouse/synaptics.c b/drivers/input/mouse/synaptics.c
index a4b14a41cbf..c703d53be3a 100644
--- a/drivers/input/mouse/synaptics.c
+++ b/drivers/input/mouse/synaptics.c
@@ -45,16 +45,6 @@
 #define YMIN_NOMINAL 1408
 #define YMAX_NOMINAL 4448
 
-/*
- * Synaptics touchpads report the y coordinate from bottom to top, which is
- * opposite from what userspace expects.
- * This function is used to invert y before reporting.
- */
-static int synaptics_invert_y(int y)
-{
-	return YMAX_NOMINAL + YMIN_NOMINAL - y;
-}
-
 
 /*****************************************************************************
  *	Stuff we need even when we do not want native Synaptics support
@@ -112,6 +102,16 @@ void synaptics_reset(struct psmouse *psmouse)
  ****************************************************************************/
 
 /*
+ * Synaptics touchpads report the y coordinate from bottom to top, which is
+ * opposite from what userspace expects.
+ * This function is used to invert y before reporting.
+ */
+static int synaptics_invert_y(int y)
+{
+	return YMAX_NOMINAL + YMIN_NOMINAL - y;
+}
+
+/*
  * Send a command to the synpatics touchpad by special commands
  */
 static int synaptics_send_cmd(struct psmouse *psmouse, unsigned char c, unsigned char *param)
diff --git a/drivers/input/mouse/vsxxxaa.c b/drivers/input/mouse/vsxxxaa.c
index eb9a3cfbeef..e900d465aaf 100644
--- a/drivers/input/mouse/vsxxxaa.c
+++ b/drivers/input/mouse/vsxxxaa.c
@@ -548,16 +548,4 @@ static struct serio_driver vsxxxaa_drv = {
 	.disconnect	= vsxxxaa_disconnect,
 };
 
-static int __init vsxxxaa_init(void)
-{
-	return serio_register_driver(&vsxxxaa_drv);
-}
-
-static void __exit vsxxxaa_exit(void)
-{
-	serio_unregister_driver(&vsxxxaa_drv);
-}
-
-module_init(vsxxxaa_init);
-module_exit(vsxxxaa_exit);
-
+module_serio_driver(vsxxxaa_drv);
diff --git a/drivers/input/of_keymap.c b/drivers/input/of_keymap.c
deleted file mode 100644
index 061493d5768..00000000000
--- a/drivers/input/of_keymap.c
+++ /dev/null
@@ -1,87 +0,0 @@
-/*
- * Helpers for open firmware matrix keyboard bindings
- *
- * Copyright (C) 2012 Google, Inc
- *
- * Author:
- *	Olof Johansson <olof@lixom.net>
- *
- * 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/types.h>
-#include <linux/input.h>
-#include <linux/of.h>
-#include <linux/input/matrix_keypad.h>
-#include <linux/export.h>
-#include <linux/gfp.h>
-#include <linux/slab.h>
-
-struct matrix_keymap_data *
-matrix_keyboard_of_fill_keymap(struct device_node *np,
-			       const char *propname)
-{
-	struct matrix_keymap_data *kd;
-	u32 *keymap;
-	int proplen, i;
-	const __be32 *prop;
-
-	if (!np)
-		return NULL;
-
-	if (!propname)
-		propname = "linux,keymap";
-
-	prop = of_get_property(np, propname, &proplen);
-	if (!prop)
-		return NULL;
-
-	if (proplen % sizeof(u32)) {
-		pr_warn("Malformed keymap property %s in %s\n",
-			propname, np->full_name);
-		return NULL;
-	}
-
-	kd = kzalloc(sizeof(*kd), GFP_KERNEL);
-	if (!kd)
-		return NULL;
-
-	kd->keymap = keymap = kzalloc(proplen, GFP_KERNEL);
-	if (!kd->keymap) {
-		kfree(kd);
-		return NULL;
-	}
-
-	kd->keymap_size = proplen / sizeof(u32);
-
-	for (i = 0; i < kd->keymap_size; i++) {
-		u32 tmp = be32_to_cpup(prop + i);
-		int key_code, row, col;
-
-		row = (tmp >> 24) & 0xff;
-		col = (tmp >> 16) & 0xff;
-		key_code = tmp & 0xffff;
-		keymap[i] = KEY(row, col, key_code);
-	}
-
-	return kd;
-}
-EXPORT_SYMBOL_GPL(matrix_keyboard_of_fill_keymap);
-
-void matrix_keyboard_of_free_keymap(const struct matrix_keymap_data *kd)
-{
-	if (kd) {
-		kfree(kd->keymap);
-		kfree(kd);
-	}
-}
-EXPORT_SYMBOL_GPL(matrix_keyboard_of_free_keymap);
diff --git a/drivers/input/serio/pcips2.c b/drivers/input/serio/pcips2.c
index 43494742541..0c42497aaaf 100644
--- a/drivers/input/serio/pcips2.c
+++ b/drivers/input/serio/pcips2.c
@@ -206,6 +206,7 @@ static const struct pci_device_id pcips2_ids[] = {
 	},
 	{ 0, }
 };
+MODULE_DEVICE_TABLE(pci, pcips2_ids);
 
 static struct pci_driver pcips2_driver = {
 	.name			= "pcips2",
@@ -214,20 +215,8 @@ static struct pci_driver pcips2_driver = {
 	.remove			= __devexit_p(pcips2_remove),
 };
 
-static int __init pcips2_init(void)
-{
-	return pci_register_driver(&pcips2_driver);
-}
-
-static void __exit pcips2_exit(void)
-{
-	pci_unregister_driver(&pcips2_driver);
-}
-
-module_init(pcips2_init);
-module_exit(pcips2_exit);
+module_pci_driver(pcips2_driver);
 
 MODULE_LICENSE("GPL");
 MODULE_AUTHOR("Russell King <rmk@arm.linux.org.uk>");
 MODULE_DESCRIPTION("PCI PS/2 keyboard/mouse driver");
-MODULE_DEVICE_TABLE(pci, pcips2_ids);
diff --git a/drivers/input/serio/ps2mult.c b/drivers/input/serio/ps2mult.c
index 15aa81c9f1f..a76fb64f03d 100644
--- a/drivers/input/serio/ps2mult.c
+++ b/drivers/input/serio/ps2mult.c
@@ -304,15 +304,4 @@ static struct serio_driver ps2mult_drv = {
 	.reconnect	= ps2mult_reconnect,
 };
 
-static int __init ps2mult_init(void)
-{
-	return serio_register_driver(&ps2mult_drv);
-}
-
-static void __exit ps2mult_exit(void)
-{
-	serio_unregister_driver(&ps2mult_drv);
-}
-
-module_init(ps2mult_init);
-module_exit(ps2mult_exit);
+module_serio_driver(ps2mult_drv);
diff --git a/drivers/input/serio/serio_raw.c b/drivers/input/serio/serio_raw.c
index 4494233d331..59df2e7317a 100644
--- a/drivers/input/serio/serio_raw.c
+++ b/drivers/input/serio/serio_raw.c
@@ -165,31 +165,38 @@ static ssize_t serio_raw_read(struct file *file, char __user *buffer,
 	struct serio_raw *serio_raw = client->serio_raw;
 	char uninitialized_var(c);
 	ssize_t read = 0;
-	int retval;
+	int error;
 
-	if (serio_raw->dead)
-		return -ENODEV;
+	for (;;) {
+		if (serio_raw->dead)
+			return -ENODEV;
 
-	if (serio_raw->head == serio_raw->tail && (file->f_flags & O_NONBLOCK))
-		return -EAGAIN;
+		if (serio_raw->head == serio_raw->tail &&
+		    (file->f_flags & O_NONBLOCK))
+			return -EAGAIN;
 
-	retval = wait_event_interruptible(serio_raw->wait,
-			serio_raw->head != serio_raw->tail || serio_raw->dead);
-	if (retval)
-		return retval;
+		if (count == 0)
+			break;
 
-	if (serio_raw->dead)
-		return -ENODEV;
+		while (read < count && serio_raw_fetch_byte(serio_raw, &c)) {
+			if (put_user(c, buffer++))
+				return -EFAULT;
+			read++;
+		}
 
-	while (read < count && serio_raw_fetch_byte(serio_raw, &c)) {
-		if (put_user(c, buffer++)) {
-			retval = -EFAULT;
+		if (read)
 			break;
+
+		if (!(file->f_flags & O_NONBLOCK)) {
+			error = wait_event_interruptible(serio_raw->wait,
+					serio_raw->head != serio_raw->tail ||
+					serio_raw->dead);
+			if (error)
+				return error;
 		}
-		read++;
 	}
 
-	return read ?: retval;
+	return read;
 }
 
 static ssize_t serio_raw_write(struct file *file, const char __user *buffer,
@@ -197,8 +204,7 @@ static ssize_t serio_raw_write(struct file *file, const char __user *buffer,
 {
 	struct serio_raw_client *client = file->private_data;
 	struct serio_raw *serio_raw = client->serio_raw;
-	ssize_t written = 0;
-	int retval;
+	int retval = 0;
 	unsigned char c;
 
 	retval = mutex_lock_interruptible(&serio_raw_mutex);
@@ -218,16 +224,20 @@ static ssize_t serio_raw_write(struct file *file, const char __user *buffer,
 			retval = -EFAULT;
 			goto out;
 		}
+
 		if (serio_write(serio_raw->serio, c)) {
-			retval = -EIO;
+			/* Either signal error or partial write */
+			if (retval == 0)
+				retval = -EIO;
 			goto out;
 		}
-		written++;
+
+		retval++;
 	}
 
 out:
 	mutex_unlock(&serio_raw_mutex);
-	return written ?: retval;
+	return retval;
 }
 
 static unsigned int serio_raw_poll(struct file *file, poll_table *wait)
@@ -432,15 +442,4 @@ static struct serio_driver serio_raw_drv = {
 	.manual_bind	= true,
 };
 
-static int __init serio_raw_init(void)
-{
-	return serio_register_driver(&serio_raw_drv);
-}
-
-static void __exit serio_raw_exit(void)
-{
-	serio_unregister_driver(&serio_raw_drv);
-}
-
-module_init(serio_raw_init);
-module_exit(serio_raw_exit);
+module_serio_driver(serio_raw_drv);
diff --git a/drivers/input/serio/xilinx_ps2.c b/drivers/input/serio/xilinx_ps2.c
index d96d4c2a76a..1e983bec7d8 100644
--- a/drivers/input/serio/xilinx_ps2.c
+++ b/drivers/input/serio/xilinx_ps2.c
@@ -73,7 +73,8 @@ struct xps2data {
 	spinlock_t lock;
 	void __iomem *base_address;	/* virt. address of control registers */
 	unsigned int flags;
-	struct serio serio;		/* serio */
+	struct serio *serio;		/* serio */
+	struct device *dev;
 };
 
 /************************************/
@@ -119,7 +120,7 @@ static irqreturn_t xps2_interrupt(int irq, void *dev_id)
 
 	/* Check which interrupt is active */
 	if (intr_sr & XPS2_IPIXR_RX_OVF)
-		dev_warn(drvdata->serio.dev.parent, "receive overrun error\n");
+		dev_warn(drvdata->dev, "receive overrun error\n");
 
 	if (intr_sr & XPS2_IPIXR_RX_ERR)
 		drvdata->flags |= SERIO_PARITY;
@@ -132,10 +133,10 @@ static irqreturn_t xps2_interrupt(int irq, void *dev_id)
 
 		/* Error, if a byte is not received */
 		if (status) {
-			dev_err(drvdata->serio.dev.parent,
+			dev_err(drvdata->dev,
 				"wrong rcvd byte count (%d)\n", status);
 		} else {
-			serio_interrupt(&drvdata->serio, c, drvdata->flags);
+			serio_interrupt(drvdata->serio, c, drvdata->flags);
 			drvdata->flags = 0;
 		}
 	}
@@ -193,7 +194,7 @@ static int sxps2_open(struct serio *pserio)
 	error = request_irq(drvdata->irq, &xps2_interrupt, 0,
 				DRIVER_NAME, drvdata);
 	if (error) {
-		dev_err(drvdata->serio.dev.parent,
+		dev_err(drvdata->dev,
 			"Couldn't allocate interrupt %d\n", drvdata->irq);
 		return error;
 	}
@@ -259,15 +260,16 @@ static int __devinit xps2_of_probe(struct platform_device *ofdev)
 	}
 
 	drvdata = kzalloc(sizeof(struct xps2data), GFP_KERNEL);
-	if (!drvdata) {
-		dev_err(dev, "Couldn't allocate device private record\n");
-		return -ENOMEM;
+	serio = kzalloc(sizeof(struct serio), GFP_KERNEL);
+	if (!drvdata || !serio) {
+		error = -ENOMEM;
+		goto failed1;
 	}
 
-	dev_set_drvdata(dev, drvdata);
-
 	spin_lock_init(&drvdata->lock);
 	drvdata->irq = r_irq.start;
+	drvdata->serio = serio;
+	drvdata->dev = dev;
 
 	phys_addr = r_mem.start;
 	remap_size = resource_size(&r_mem);
@@ -298,7 +300,6 @@ static int __devinit xps2_of_probe(struct platform_device *ofdev)
 		 (unsigned long long)phys_addr, drvdata->base_address,
 		 drvdata->irq);
 
-	serio = &drvdata->serio;
 	serio->id.type = SERIO_8042;
 	serio->write = sxps2_write;
 	serio->open = sxps2_open;
@@ -312,13 +313,14 @@ static int __devinit xps2_of_probe(struct platform_device *ofdev)
 
 	serio_register_port(serio);
 
+	platform_set_drvdata(ofdev, drvdata);
 	return 0;		/* success */
 
 failed2:
 	release_mem_region(phys_addr, remap_size);
 failed1:
+	kfree(serio);
 	kfree(drvdata);
-	dev_set_drvdata(dev, NULL);
 
 	return error;
 }
@@ -333,22 +335,21 @@ failed1:
  */
 static int __devexit xps2_of_remove(struct platform_device *of_dev)
 {
-	struct device *dev = &of_dev->dev;
-	struct xps2data *drvdata = dev_get_drvdata(dev);
+	struct xps2data *drvdata = platform_get_drvdata(of_dev);
 	struct resource r_mem; /* IO mem resources */
 
-	serio_unregister_port(&drvdata->serio);
+	serio_unregister_port(drvdata->serio);
 	iounmap(drvdata->base_address);
 
 	/* Get iospace of the device */
 	if (of_address_to_resource(of_dev->dev.of_node, 0, &r_mem))
-		dev_err(dev, "invalid address\n");
+		dev_err(drvdata->dev, "invalid address\n");
 	else
 		release_mem_region(r_mem.start, resource_size(&r_mem));
 
 	kfree(drvdata);
 
-	dev_set_drvdata(dev, NULL);
+	platform_set_drvdata(of_dev, NULL);
 
 	return 0;
 }
diff --git a/drivers/input/tablet/acecad.c b/drivers/input/tablet/acecad.c
index f8b0b1df913..e062ec899ca 100644
--- a/drivers/input/tablet/acecad.c
+++ b/drivers/input/tablet/acecad.c
@@ -51,6 +51,7 @@ struct usb_acecad {
 	char name[128];
 	char phys[64];
 	struct usb_device *usbdev;
+	struct usb_interface *intf;
 	struct input_dev *input;
 	struct urb *irq;
 
@@ -63,6 +64,7 @@ static void usb_acecad_irq(struct urb *urb)
 	struct usb_acecad *acecad = urb->context;
 	unsigned char *data = acecad->data;
 	struct input_dev *dev = acecad->input;
+	struct usb_interface *intf = acecad->intf;
 	int prox, status;
 
 	switch (urb->status) {
@@ -73,10 +75,12 @@ static void usb_acecad_irq(struct urb *urb)
 	case -ENOENT:
 	case -ESHUTDOWN:
 		/* this urb is terminated, clean up */
-		dbg("%s - urb shutting down with status: %d", __func__, urb->status);
+		dev_dbg(&intf->dev, "%s - urb shutting down with status: %d\n",
+			__func__, urb->status);
 		return;
 	default:
-		dbg("%s - nonzero urb status received: %d", __func__, urb->status);
+		dev_dbg(&intf->dev, "%s - nonzero urb status received: %d\n",
+			__func__, urb->status);
 		goto resubmit;
 	}
 
@@ -105,8 +109,10 @@ static void usb_acecad_irq(struct urb *urb)
 resubmit:
 	status = usb_submit_urb(urb, GFP_ATOMIC);
 	if (status)
-		err("can't resubmit intr, %s-%s/input0, status %d",
-			acecad->usbdev->bus->bus_name, acecad->usbdev->devpath, status);
+		dev_err(&intf->dev,
+			"can't resubmit intr, %s-%s/input0, status %d\n",
+			acecad->usbdev->bus->bus_name,
+			acecad->usbdev->devpath, status);
 }
 
 static int usb_acecad_open(struct input_dev *dev)
@@ -168,6 +174,7 @@ static int usb_acecad_probe(struct usb_interface *intf, const struct usb_device_
 	}
 
 	acecad->usbdev = dev;
+	acecad->intf = intf;
 	acecad->input = input_dev;
 
 	if (dev->manufacturer)
diff --git a/drivers/input/tablet/aiptek.c b/drivers/input/tablet/aiptek.c
index 205d16aab44..ee83c3904ee 100644
--- a/drivers/input/tablet/aiptek.c
+++ b/drivers/input/tablet/aiptek.c
@@ -309,6 +309,7 @@ struct aiptek_settings {
 struct aiptek {
 	struct input_dev *inputdev;		/* input device struct           */
 	struct usb_device *usbdev;		/* usb device struct             */
+	struct usb_interface *intf;		/* usb interface struct          */
 	struct urb *urb;			/* urb for incoming reports      */
 	dma_addr_t data_dma;			/* our dma stuffage              */
 	struct aiptek_features features;	/* tablet's array of features    */
@@ -435,6 +436,7 @@ static void aiptek_irq(struct urb *urb)
 	struct aiptek *aiptek = urb->context;
 	unsigned char *data = aiptek->data;
 	struct input_dev *inputdev = aiptek->inputdev;
+	struct usb_interface *intf = aiptek->intf;
 	int jitterable = 0;
 	int retval, macro, x, y, z, left, right, middle, p, dv, tip, bs, pck;
 
@@ -447,13 +449,13 @@ static void aiptek_irq(struct urb *urb)
 	case -ENOENT:
 	case -ESHUTDOWN:
 		/* This urb is terminated, clean up */
-		dbg("%s - urb shutting down with status: %d",
-		    __func__, urb->status);
+		dev_dbg(&intf->dev, "%s - urb shutting down with status: %d\n",
+			__func__, urb->status);
 		return;
 
 	default:
-		dbg("%s - nonzero urb status received: %d",
-		    __func__, urb->status);
+		dev_dbg(&intf->dev, "%s - nonzero urb status received: %d\n",
+			__func__, urb->status);
 		goto exit;
 	}
 
@@ -785,7 +787,7 @@ static void aiptek_irq(struct urb *urb)
 				 1 | AIPTEK_REPORT_TOOL_UNKNOWN);
 		input_sync(inputdev);
 	} else {
-		dbg("Unknown report %d", data[0]);
+		dev_dbg(&intf->dev, "Unknown report %d\n", data[0]);
 	}
 
 	/* Jitter may occur when the user presses a button on the stlyus
@@ -811,8 +813,9 @@ static void aiptek_irq(struct urb *urb)
 exit:
 	retval = usb_submit_urb(urb, GFP_ATOMIC);
 	if (retval != 0) {
-		err("%s - usb_submit_urb failed with result %d",
-		    __func__, retval);
+		dev_err(&intf->dev,
+			"%s - usb_submit_urb failed with result %d\n",
+			__func__, retval);
 	}
 }
 
@@ -912,8 +915,9 @@ aiptek_command(struct aiptek *aiptek, unsigned char command, unsigned char data)
 
 	if ((ret =
 	     aiptek_set_report(aiptek, 3, 2, buf, sizeof_buf)) != sizeof_buf) {
-		dbg("aiptek_program: failed, tried to send: 0x%02x 0x%02x",
-		    command, data);
+		dev_dbg(&aiptek->intf->dev,
+			"aiptek_program: failed, tried to send: 0x%02x 0x%02x\n",
+			command, data);
 	}
 	kfree(buf);
 	return ret < 0 ? ret : 0;
@@ -947,8 +951,9 @@ aiptek_query(struct aiptek *aiptek, unsigned char command, unsigned char data)
 
 	if ((ret =
 	     aiptek_get_report(aiptek, 3, 2, buf, sizeof_buf)) != sizeof_buf) {
-		dbg("aiptek_query failed: returned 0x%02x 0x%02x 0x%02x",
-		    buf[0], buf[1], buf[2]);
+		dev_dbg(&aiptek->intf->dev,
+			"aiptek_query failed: returned 0x%02x 0x%02x 0x%02x\n",
+			buf[0], buf[1], buf[2]);
 		ret = -EIO;
 	} else {
 		ret = get_unaligned_le16(buf + 1);
@@ -1726,6 +1731,7 @@ aiptek_probe(struct usb_interface *intf, const struct usb_device_id *id)
 
 	aiptek->inputdev = inputdev;
 	aiptek->usbdev = usbdev;
+	aiptek->intf = intf;
 	aiptek->ifnum = intf->altsetting[0].desc.bInterfaceNumber;
 	aiptek->inDelay = 0;
 	aiptek->endDelay = 0;
@@ -1856,7 +1862,7 @@ aiptek_probe(struct usb_interface *intf, const struct usb_device_id *id)
 	if (i == ARRAY_SIZE(speeds)) {
 		dev_info(&intf->dev,
 			 "Aiptek tried all speeds, no sane response\n");
-		goto fail2;
+		goto fail3;
 	}
 
 	/* Associate this driver's struct with the usb interface.
diff --git a/drivers/input/tablet/gtco.c b/drivers/input/tablet/gtco.c
index 89a297801dc..29e01ab6859 100644
--- a/drivers/input/tablet/gtco.c
+++ b/drivers/input/tablet/gtco.c
@@ -2,8 +2,6 @@
 
 GTCO digitizer USB driver
 
-Use the err() and dbg() macros from usb.h for system logging
-
 TO CHECK:  Is pressure done right on report 5?
 
 Copyright (C) 2006  GTCO CalComp
@@ -108,6 +106,7 @@ struct gtco {
 
 	struct input_dev  *inputdevice; /* input device struct pointer  */
 	struct usb_device *usbdev; /* the usb device for this device */
+	struct usb_interface *intf;	/* the usb interface for this device */
 	struct urb        *urbinfo;	 /* urb for incoming reports      */
 	dma_addr_t        buf_dma;  /* dma addr of the data buffer*/
 	unsigned char *   buffer;   /* databuffer for reports */
@@ -202,6 +201,7 @@ struct hid_descriptor
 static void parse_hid_report_descriptor(struct gtco *device, char * report,
 					int length)
 {
+	struct device *ddev = &device->intf->dev;
 	int   x, i = 0;
 
 	/* Tag primitive vars */
@@ -228,7 +228,7 @@ static void parse_hid_report_descriptor(struct gtco *device, char * report,
 	char  indentstr[10] = "";
 
 
-	dbg("======>>>>>>PARSE<<<<<<======");
+	dev_dbg(ddev, "======>>>>>>PARSE<<<<<<======\n");
 
 	/* Walk  this report and pull out the info we need */
 	while (i < length) {
@@ -277,11 +277,11 @@ static void parse_hid_report_descriptor(struct gtco *device, char * report,
 				else if (data == 3)
 					strcpy(globtype, "Var|Const");
 
-				dbg("::::: Saving Report: %d input #%d Max: 0x%X(%d) Min:0x%X(%d) of %d bits",
-				    globalval[TAG_GLOB_REPORT_ID], inputnum,
-				    globalval[TAG_GLOB_LOG_MAX], globalval[TAG_GLOB_LOG_MAX],
-				    globalval[TAG_GLOB_LOG_MIN], globalval[TAG_GLOB_LOG_MIN],
-				    globalval[TAG_GLOB_REPORT_SZ] * globalval[TAG_GLOB_REPORT_CNT]);
+				dev_dbg(ddev, "::::: Saving Report: %d input #%d Max: 0x%X(%d) Min:0x%X(%d) of %d bits\n",
+					globalval[TAG_GLOB_REPORT_ID], inputnum,
+					globalval[TAG_GLOB_LOG_MAX], globalval[TAG_GLOB_LOG_MAX],
+					globalval[TAG_GLOB_LOG_MIN], globalval[TAG_GLOB_LOG_MIN],
+					globalval[TAG_GLOB_REPORT_SZ] * globalval[TAG_GLOB_REPORT_CNT]);
 
 
 				/*
@@ -292,7 +292,7 @@ static void parse_hid_report_descriptor(struct gtco *device, char * report,
 				 */
 				switch (inputnum) {
 				case 0:  /* X coord */
-					dbg("GER: X Usage: 0x%x", usage);
+					dev_dbg(ddev, "GER: X Usage: 0x%x\n", usage);
 					if (device->max_X == 0) {
 						device->max_X = globalval[TAG_GLOB_LOG_MAX];
 						device->min_X = globalval[TAG_GLOB_LOG_MIN];
@@ -300,7 +300,7 @@ static void parse_hid_report_descriptor(struct gtco *device, char * report,
 					break;
 
 				case 1:  /* Y coord */
-					dbg("GER: Y Usage: 0x%x", usage);
+					dev_dbg(ddev, "GER: Y Usage: 0x%x\n", usage);
 					if (device->max_Y == 0) {
 						device->max_Y = globalval[TAG_GLOB_LOG_MAX];
 						device->min_Y = globalval[TAG_GLOB_LOG_MIN];
@@ -350,10 +350,10 @@ static void parse_hid_report_descriptor(struct gtco *device, char * report,
 				maintype = 'S';
 
 				if (data == 0) {
-					dbg("======>>>>>> Physical");
+					dev_dbg(ddev, "======>>>>>> Physical\n");
 					strcpy(globtype, "Physical");
 				} else
-					dbg("======>>>>>>");
+					dev_dbg(ddev, "======>>>>>>\n");
 
 				/* Indent the debug output */
 				indent++;
@@ -368,7 +368,7 @@ static void parse_hid_report_descriptor(struct gtco *device, char * report,
 				break;
 
 			case TAG_MAIN_COL_END:
-				dbg("<<<<<<======");
+				dev_dbg(ddev, "<<<<<<======\n");
 				maintype = 'E';
 				indent--;
 				for (x = 0; x < indent; x++)
@@ -384,18 +384,18 @@ static void parse_hid_report_descriptor(struct gtco *device, char * report,
 
 			switch (size) {
 			case 1:
-				dbg("%sMAINTAG:(%d) %c SIZE: %d Data: %s 0x%x",
-				    indentstr, tag, maintype, size, globtype, data);
+				dev_dbg(ddev, "%sMAINTAG:(%d) %c SIZE: %d Data: %s 0x%x\n",
+					indentstr, tag, maintype, size, globtype, data);
 				break;
 
 			case 2:
-				dbg("%sMAINTAG:(%d) %c SIZE: %d Data: %s 0x%x",
-				    indentstr, tag, maintype, size, globtype, data16);
+				dev_dbg(ddev, "%sMAINTAG:(%d) %c SIZE: %d Data: %s 0x%x\n",
+					indentstr, tag, maintype, size, globtype, data16);
 				break;
 
 			case 4:
-				dbg("%sMAINTAG:(%d) %c SIZE: %d Data: %s 0x%x",
-				    indentstr, tag, maintype, size, globtype, data32);
+				dev_dbg(ddev, "%sMAINTAG:(%d) %c SIZE: %d Data: %s 0x%x\n",
+					indentstr, tag, maintype, size, globtype, data32);
 				break;
 			}
 			break;
@@ -465,26 +465,26 @@ static void parse_hid_report_descriptor(struct gtco *device, char * report,
 			if (tag < TAG_GLOB_MAX) {
 				switch (size) {
 				case 1:
-					dbg("%sGLOBALTAG:%s(%d) SIZE: %d Data: 0x%x",
-					    indentstr, globtype, tag, size, data);
+					dev_dbg(ddev, "%sGLOBALTAG:%s(%d) SIZE: %d Data: 0x%x\n",
+						indentstr, globtype, tag, size, data);
 					globalval[tag] = data;
 					break;
 
 				case 2:
-					dbg("%sGLOBALTAG:%s(%d) SIZE: %d Data: 0x%x",
-					    indentstr, globtype, tag, size, data16);
+					dev_dbg(ddev, "%sGLOBALTAG:%s(%d) SIZE: %d Data: 0x%x\n",
+						indentstr, globtype, tag, size, data16);
 					globalval[tag] = data16;
 					break;
 
 				case 4:
-					dbg("%sGLOBALTAG:%s(%d) SIZE: %d Data: 0x%x",
-					    indentstr, globtype, tag, size, data32);
+					dev_dbg(ddev, "%sGLOBALTAG:%s(%d) SIZE: %d Data: 0x%x\n",
+						indentstr, globtype, tag, size, data32);
 					globalval[tag] = data32;
 					break;
 				}
 			} else {
-				dbg("%sGLOBALTAG: ILLEGAL TAG:%d SIZE: %d ",
-				    indentstr, tag, size);
+				dev_dbg(ddev, "%sGLOBALTAG: ILLEGAL TAG:%d SIZE: %d\n",
+					indentstr, tag, size);
 			}
 			break;
 
@@ -511,18 +511,18 @@ static void parse_hid_report_descriptor(struct gtco *device, char * report,
 
 			switch (size) {
 			case 1:
-				dbg("%sLOCALTAG:(%d) %s SIZE: %d Data: 0x%x",
-				    indentstr, tag, globtype, size, data);
+				dev_dbg(ddev, "%sLOCALTAG:(%d) %s SIZE: %d Data: 0x%x\n",
+					indentstr, tag, globtype, size, data);
 				break;
 
 			case 2:
-				dbg("%sLOCALTAG:(%d) %s SIZE: %d Data: 0x%x",
-				    indentstr, tag, globtype, size, data16);
+				dev_dbg(ddev, "%sLOCALTAG:(%d) %s SIZE: %d Data: 0x%x\n",
+					indentstr, tag, globtype, size, data16);
 				break;
 
 			case 4:
-				dbg("%sLOCALTAG:(%d) %s SIZE: %d Data: 0x%x",
-				    indentstr, tag, globtype, size, data32);
+				dev_dbg(ddev, "%sLOCALTAG:(%d) %s SIZE: %d Data: 0x%x\n",
+					indentstr, tag, globtype, size, data32);
 				break;
 			}
 
@@ -714,8 +714,9 @@ static void gtco_urb_callback(struct urb *urbinfo)
 				 * the rest as 0
 				 */
 				val = device->buffer[5] & MASK_BUTTON;
-				dbg("======>>>>>>REPORT 1: val 0x%X(%d)",
-				    val, val);
+				dev_dbg(&device->intf->dev,
+					"======>>>>>>REPORT 1: val 0x%X(%d)\n",
+					val, val);
 
 				/*
 				 * We don't apply any meaning to the button
@@ -808,7 +809,8 @@ static void gtco_urb_callback(struct urb *urbinfo)
  resubmit:
 	rc = usb_submit_urb(urbinfo, GFP_ATOMIC);
 	if (rc != 0)
-		err("usb_submit_urb failed rc=0x%x", rc);
+		dev_err(&device->intf->dev,
+			"usb_submit_urb failed rc=0x%x\n", rc);
 }
 
 /*
@@ -838,7 +840,7 @@ static int gtco_probe(struct usb_interface *usbinterface,
 	gtco = kzalloc(sizeof(struct gtco), GFP_KERNEL);
 	input_dev = input_allocate_device();
 	if (!gtco || !input_dev) {
-		err("No more memory");
+		dev_err(&usbinterface->dev, "No more memory\n");
 		error = -ENOMEM;
 		goto err_free_devs;
 	}
@@ -848,12 +850,13 @@ static int gtco_probe(struct usb_interface *usbinterface,
 
 	/* Save interface information */
 	gtco->usbdev = usb_get_dev(interface_to_usbdev(usbinterface));
+	gtco->intf = usbinterface;
 
 	/* Allocate some data for incoming reports */
 	gtco->buffer = usb_alloc_coherent(gtco->usbdev, REPORT_MAX_SIZE,
 					  GFP_KERNEL, &gtco->buf_dma);
 	if (!gtco->buffer) {
-		err("No more memory for us buffers");
+		dev_err(&usbinterface->dev, "No more memory for us buffers\n");
 		error = -ENOMEM;
 		goto err_free_devs;
 	}
@@ -861,7 +864,7 @@ static int gtco_probe(struct usb_interface *usbinterface,
 	/* Allocate URB for reports */
 	gtco->urbinfo = usb_alloc_urb(0, GFP_KERNEL);
 	if (!gtco->urbinfo) {
-		err("Failed to allocate URB");
+		dev_err(&usbinterface->dev, "Failed to allocate URB\n");
 		error = -ENOMEM;
 		goto err_free_buf;
 	}
@@ -873,14 +876,14 @@ static int gtco_probe(struct usb_interface *usbinterface,
 	endpoint = &usbinterface->altsetting[0].endpoint[0].desc;
 
 	/* Some debug */
-	dbg("gtco # interfaces: %d", usbinterface->num_altsetting);
-	dbg("num endpoints:     %d", usbinterface->cur_altsetting->desc.bNumEndpoints);
-	dbg("interface class:   %d", usbinterface->cur_altsetting->desc.bInterfaceClass);
-	dbg("endpoint: attribute:0x%x type:0x%x", endpoint->bmAttributes, endpoint->bDescriptorType);
+	dev_dbg(&usbinterface->dev, "gtco # interfaces: %d\n", usbinterface->num_altsetting);
+	dev_dbg(&usbinterface->dev, "num endpoints:     %d\n", usbinterface->cur_altsetting->desc.bNumEndpoints);
+	dev_dbg(&usbinterface->dev, "interface class:   %d\n", usbinterface->cur_altsetting->desc.bInterfaceClass);
+	dev_dbg(&usbinterface->dev, "endpoint: attribute:0x%x type:0x%x\n", endpoint->bmAttributes, endpoint->bDescriptorType);
 	if (usb_endpoint_xfer_int(endpoint))
-		dbg("endpoint: we have interrupt endpoint\n");
+		dev_dbg(&usbinterface->dev, "endpoint: we have interrupt endpoint\n");
 
-	dbg("endpoint extra len:%d ", usbinterface->altsetting[0].extralen);
+	dev_dbg(&usbinterface->dev, "endpoint extra len:%d\n", usbinterface->altsetting[0].extralen);
 
 	/*
 	 * Find the HID descriptor so we can find out the size of the
@@ -888,17 +891,19 @@ static int gtco_probe(struct usb_interface *usbinterface,
 	 */
 	if (usb_get_extra_descriptor(usbinterface->cur_altsetting,
 				     HID_DEVICE_TYPE, &hid_desc) != 0){
-		err("Can't retrieve exta USB descriptor to get hid report descriptor length");
+		dev_err(&usbinterface->dev,
+			"Can't retrieve exta USB descriptor to get hid report descriptor length\n");
 		error = -EIO;
 		goto err_free_urb;
 	}
 
-	dbg("Extra descriptor success: type:%d  len:%d",
-	    hid_desc->bDescriptorType,  hid_desc->wDescriptorLength);
+	dev_dbg(&usbinterface->dev,
+		"Extra descriptor success: type:%d  len:%d\n",
+		hid_desc->bDescriptorType,  hid_desc->wDescriptorLength);
 
 	report = kzalloc(le16_to_cpu(hid_desc->wDescriptorLength), GFP_KERNEL);
 	if (!report) {
-		err("No more memory for report");
+		dev_err(&usbinterface->dev, "No more memory for report\n");
 		error = -ENOMEM;
 		goto err_free_urb;
 	}
@@ -915,7 +920,7 @@ static int gtco_probe(struct usb_interface *usbinterface,
 					 le16_to_cpu(hid_desc->wDescriptorLength),
 					 5000); /* 5 secs */
 
-		dbg("usb_control_msg result: %d", result);
+		dev_dbg(&usbinterface->dev, "usb_control_msg result: %d\n", result);
 		if (result == le16_to_cpu(hid_desc->wDescriptorLength)) {
 			parse_hid_report_descriptor(gtco, report, result);
 			break;
@@ -926,8 +931,9 @@ static int gtco_probe(struct usb_interface *usbinterface,
 
 	/* If we didn't get the report, fail */
 	if (result != le16_to_cpu(hid_desc->wDescriptorLength)) {
-		err("Failed to get HID Report Descriptor of size: %d",
-		    hid_desc->wDescriptorLength);
+		dev_err(&usbinterface->dev,
+			"Failed to get HID Report Descriptor of size: %d\n",
+			hid_desc->wDescriptorLength);
 		error = -EIO;
 		goto err_free_urb;
 	}
diff --git a/drivers/input/tablet/kbtab.c b/drivers/input/tablet/kbtab.c
index 85a5b40333a..3fba74b9b60 100644
--- a/drivers/input/tablet/kbtab.c
+++ b/drivers/input/tablet/kbtab.c
@@ -33,6 +33,7 @@ struct kbtab {
 	dma_addr_t data_dma;
 	struct input_dev *dev;
 	struct usb_device *usbdev;
+	struct usb_interface *intf;
 	struct urb *irq;
 	char phys[32];
 };
@@ -53,10 +54,14 @@ static void kbtab_irq(struct urb *urb)
 	case -ENOENT:
 	case -ESHUTDOWN:
 		/* this urb is terminated, clean up */
-		dbg("%s - urb shutting down with status: %d", __func__, urb->status);
+		dev_dbg(&kbtab->intf->dev,
+			"%s - urb shutting down with status: %d\n",
+			__func__, urb->status);
 		return;
 	default:
-		dbg("%s - nonzero urb status received: %d", __func__, urb->status);
+		dev_dbg(&kbtab->intf->dev,
+			"%s - nonzero urb status received: %d\n",
+			__func__, urb->status);
 		goto exit;
 	}
 
@@ -80,8 +85,9 @@ static void kbtab_irq(struct urb *urb)
  exit:
 	retval = usb_submit_urb(urb, GFP_ATOMIC);
 	if (retval)
-		err("%s - usb_submit_urb failed with result %d",
-		     __func__, retval);
+		dev_err(&kbtab->intf->dev,
+			"%s - usb_submit_urb failed with result %d\n",
+			__func__, retval);
 }
 
 static struct usb_device_id kbtab_ids[] = {
@@ -131,6 +137,7 @@ static int kbtab_probe(struct usb_interface *intf, const struct usb_device_id *i
 		goto fail2;
 
 	kbtab->usbdev = dev;
+	kbtab->intf = intf;
 	kbtab->dev = input_dev;
 
 	usb_make_path(dev, kbtab->phys, sizeof(kbtab->phys));
diff --git a/drivers/input/tablet/wacom.h b/drivers/input/tablet/wacom.h
index b4842d0e61d..b79d45198d8 100644
--- a/drivers/input/tablet/wacom.h
+++ b/drivers/input/tablet/wacom.h
@@ -135,6 +135,6 @@ extern const struct usb_device_id wacom_ids[];
 
 void wacom_wac_irq(struct wacom_wac *wacom_wac, size_t len);
 void wacom_setup_device_quirks(struct wacom_features *features);
-void wacom_setup_input_capabilities(struct input_dev *input_dev,
-				    struct wacom_wac *wacom_wac);
+int wacom_setup_input_capabilities(struct input_dev *input_dev,
+				   struct wacom_wac *wacom_wac);
 #endif
diff --git a/drivers/input/tablet/wacom_sys.c b/drivers/input/tablet/wacom_sys.c
index 0d269212931..cad5602d3ce 100644
--- a/drivers/input/tablet/wacom_sys.c
+++ b/drivers/input/tablet/wacom_sys.c
@@ -28,6 +28,7 @@
 #define HID_USAGE_Y_TILT		0x3e
 #define HID_USAGE_FINGER		0x22
 #define HID_USAGE_STYLUS		0x20
+#define HID_USAGE_CONTACTMAX		0x55
 #define HID_COLLECTION			0xa1
 #define HID_COLLECTION_LOGICAL		0x02
 #define HID_COLLECTION_END		0xc0
@@ -99,6 +100,7 @@ static int wacom_set_report(struct usb_interface *intf, u8 type, u8 id,
 static void wacom_sys_irq(struct urb *urb)
 {
 	struct wacom *wacom = urb->context;
+	struct device *dev = &wacom->intf->dev;
 	int retval;
 
 	switch (urb->status) {
@@ -109,10 +111,12 @@ static void wacom_sys_irq(struct urb *urb)
 	case -ENOENT:
 	case -ESHUTDOWN:
 		/* this urb is terminated, clean up */
-		dbg("%s - urb shutting down with status: %d", __func__, urb->status);
+		dev_dbg(dev, "%s - urb shutting down with status: %d\n",
+			__func__, urb->status);
 		return;
 	default:
-		dbg("%s - nonzero urb status received: %d", __func__, urb->status);
+		dev_dbg(dev, "%s - nonzero urb status received: %d\n",
+			__func__, urb->status);
 		goto exit;
 	}
 
@@ -122,8 +126,8 @@ static void wacom_sys_irq(struct urb *urb)
 	usb_mark_last_busy(wacom->usbdev);
 	retval = usb_submit_urb(urb, GFP_ATOMIC);
 	if (retval)
-		err ("%s - usb_submit_urb failed with result %d",
-		     __func__, retval);
+		dev_err(dev, "%s - usb_submit_urb failed with result %d\n",
+			__func__, retval);
 }
 
 static int wacom_open(struct input_dev *dev)
@@ -201,6 +205,27 @@ static int wacom_parse_logical_collection(unsigned char *report,
 	return length;
 }
 
+static void wacom_retrieve_report_data(struct usb_interface *intf,
+				       struct wacom_features *features)
+{
+	int result = 0;
+	unsigned char *rep_data;
+
+	rep_data = kmalloc(2, GFP_KERNEL);
+	if (rep_data) {
+
+		rep_data[0] = 12;
+		result = wacom_get_report(intf, WAC_HID_FEATURE_REPORT,
+					  rep_data[0], &rep_data, 2,
+					  WAC_MSG_RETRIES);
+
+		if (result >= 0 && rep_data[1] > 2)
+			features->touch_max = rep_data[1];
+
+		kfree(rep_data);
+	}
+}
+
 /*
  * Interface Descriptor of wacom devices can be incomplete and
  * inconsistent so wacom_features table is used to store stylus
@@ -233,6 +258,9 @@ static int wacom_parse_logical_collection(unsigned char *report,
  * 3rd gen Bamboo Touch no longer define a Digitizer-Finger Pysical
  * Collection. Instead they define a Logical Collection with a single
  * Logical Maximum for both X and Y.
+ *
+ * Intuos5 touch interface does not contain useful data. We deal with
+ * this after returning from this function.
  */
 static int wacom_parse_hid(struct usb_interface *intf,
 			   struct hid_descriptor *hid_desc,
@@ -292,6 +320,10 @@ static int wacom_parse_hid(struct usb_interface *intf,
 							/* need to reset back */
 							features->pktlen = WACOM_PKGLEN_TPC2FG;
 						}
+
+						if (features->type == MTSCREEN)
+							features->pktlen = WACOM_PKGLEN_MTOUCH;
+
 						if (features->type == BAMBOO_PT) {
 							/* need to reset back */
 							features->pktlen = WACOM_PKGLEN_BBTOUCH;
@@ -324,18 +356,15 @@ static int wacom_parse_hid(struct usb_interface *intf,
 			case HID_USAGE_Y:
 				if (usage == WCM_DESKTOP) {
 					if (finger) {
-						features->device_type = BTN_TOOL_FINGER;
-						if (features->type == TABLETPC2FG) {
-							/* need to reset back */
-							features->pktlen = WACOM_PKGLEN_TPC2FG;
+						int type = features->type;
+
+						if (type == TABLETPC2FG || type == MTSCREEN) {
 							features->y_max =
 								get_unaligned_le16(&report[i + 3]);
 							features->y_phy =
 								get_unaligned_le16(&report[i + 6]);
 							i += 7;
-						} else if (features->type == BAMBOO_PT) {
-							/* need to reset back */
-							features->pktlen = WACOM_PKGLEN_BBTOUCH;
+						} else if (type == BAMBOO_PT) {
 							features->y_phy =
 								get_unaligned_le16(&report[i + 3]);
 							features->y_max =
@@ -349,10 +378,6 @@ static int wacom_parse_hid(struct usb_interface *intf,
 							i += 4;
 						}
 					} else if (pen) {
-						/* penabled only accepts exact bytes of data */
-						if (features->type == TABLETPC2FG)
-							features->pktlen = WACOM_PKGLEN_GRAPHIRE;
-						features->device_type = BTN_TOOL_PEN;
 						features->y_max =
 							get_unaligned_le16(&report[i + 3]);
 						i += 4;
@@ -374,6 +399,11 @@ static int wacom_parse_hid(struct usb_interface *intf,
 				pen = 1;
 				i++;
 				break;
+
+			case HID_USAGE_CONTACTMAX:
+				wacom_retrieve_report_data(intf, features);
+				i++;
+				break;
 			}
 			break;
 
@@ -410,22 +440,29 @@ static int wacom_query_tablet_data(struct usb_interface *intf, struct wacom_feat
 	if (!rep_data)
 		return error;
 
-	/* ask to report tablet data if it is MT Tablet PC or
-	 * not a Tablet PC */
-	if (features->type == TABLETPC2FG) {
-		do {
-			rep_data[0] = 3;
-			rep_data[1] = 4;
-			rep_data[2] = 0;
-			rep_data[3] = 0;
-			report_id = 3;
-			error = wacom_set_report(intf, WAC_HID_FEATURE_REPORT,
-						 report_id, rep_data, 4, 1);
-			if (error >= 0)
-				error = wacom_get_report(intf,
-						WAC_HID_FEATURE_REPORT,
-						report_id, rep_data, 4, 1);
-		} while ((error < 0 || rep_data[1] != 4) && limit++ < WAC_MSG_RETRIES);
+	/* ask to report Wacom data */
+	if (features->device_type == BTN_TOOL_FINGER) {
+		/* if it is an MT Tablet PC touch */
+		if (features->type == TABLETPC2FG ||
+		    features->type == MTSCREEN) {
+			do {
+				rep_data[0] = 3;
+				rep_data[1] = 4;
+				rep_data[2] = 0;
+				rep_data[3] = 0;
+				report_id = 3;
+				error = wacom_set_report(intf,
+							 WAC_HID_FEATURE_REPORT,
+							 report_id,
+							 rep_data, 4, 1);
+				if (error >= 0)
+					error = wacom_get_report(intf,
+							WAC_HID_FEATURE_REPORT,
+							report_id,
+							rep_data, 4, 1);
+			} while ((error < 0 || rep_data[1] != 4) &&
+				 limit++ < WAC_MSG_RETRIES);
+		}
 	} else if (features->type != TABLETPC &&
 		   features->type != WIRELESS &&
 		   features->device_type == BTN_TOOL_PEN) {
@@ -447,7 +484,7 @@ static int wacom_query_tablet_data(struct usb_interface *intf, struct wacom_feat
 }
 
 static int wacom_retrieve_hid_descriptor(struct usb_interface *intf,
-		struct wacom_features *features)
+					 struct wacom_features *features)
 {
 	int error = 0;
 	struct usb_host_interface *interface = intf->cur_altsetting;
@@ -475,16 +512,21 @@ static int wacom_retrieve_hid_descriptor(struct usb_interface *intf,
 		}
 	}
 
-	/* only Tablet PCs and Bamboo P&T need to retrieve the info */
-	if ((features->type != TABLETPC) && (features->type != TABLETPC2FG) &&
-	    (features->type != BAMBOO_PT))
+	/* only devices that support touch need to retrieve the info */
+	if (features->type != TABLETPC &&
+	    features->type != TABLETPC2FG &&
+	    features->type != BAMBOO_PT &&
+	    features->type != MTSCREEN) {
 		goto out;
+	}
 
-	if (usb_get_extra_descriptor(interface, HID_DEVICET_HID, &hid_desc)) {
-		if (usb_get_extra_descriptor(&interface->endpoint[0],
-				HID_DEVICET_REPORT, &hid_desc)) {
-			printk("wacom: can not retrieve extra class descriptor\n");
-			error = 1;
+	error = usb_get_extra_descriptor(interface, HID_DEVICET_HID, &hid_desc);
+	if (error) {
+		error = usb_get_extra_descriptor(&interface->endpoint[0],
+						 HID_DEVICET_REPORT, &hid_desc);
+		if (error) {
+			dev_err(&intf->dev,
+				"can not retrieve extra class descriptor\n");
 			goto out;
 		}
 	}
@@ -574,23 +616,39 @@ static void wacom_remove_shared_data(struct wacom_wac *wacom)
 static int wacom_led_control(struct wacom *wacom)
 {
 	unsigned char *buf;
-	int retval, led = 0;
+	int retval;
 
 	buf = kzalloc(9, GFP_KERNEL);
 	if (!buf)
 		return -ENOMEM;
 
-	if (wacom->wacom_wac.features.type == WACOM_21UX2 ||
-	    wacom->wacom_wac.features.type == WACOM_24HD)
-		led = (wacom->led.select[1] << 4) | 0x40;
-
-	led |=  wacom->led.select[0] | 0x4;
-
-	buf[0] = WAC_CMD_LED_CONTROL;
-	buf[1] = led;
-	buf[2] = wacom->led.llv;
-	buf[3] = wacom->led.hlv;
-	buf[4] = wacom->led.img_lum;
+	if (wacom->wacom_wac.features.type >= INTUOS5S &&
+	    wacom->wacom_wac.features.type <= INTUOS5L)	{
+		/*
+		 * Touch Ring and crop mark LED luminance may take on
+		 * one of four values:
+		 *    0 = Low; 1 = Medium; 2 = High; 3 = Off
+		 */
+		int ring_led = wacom->led.select[0] & 0x03;
+		int ring_lum = (((wacom->led.llv & 0x60) >> 5) - 1) & 0x03;
+		int crop_lum = 0;
+
+		buf[0] = WAC_CMD_LED_CONTROL;
+		buf[1] = (crop_lum << 4) | (ring_lum << 2) | (ring_led);
+	}
+	else {
+		int led = wacom->led.select[0] | 0x4;
+
+		if (wacom->wacom_wac.features.type == WACOM_21UX2 ||
+		    wacom->wacom_wac.features.type == WACOM_24HD)
+			led |= (wacom->led.select[1] << 4) | 0x40;
+
+		buf[0] = WAC_CMD_LED_CONTROL;
+		buf[1] = led;
+		buf[2] = wacom->led.llv;
+		buf[3] = wacom->led.hlv;
+		buf[4] = wacom->led.img_lum;
+	}
 
 	retval = wacom_set_report(wacom->intf, 0x03, WAC_CMD_LED_CONTROL,
 				  buf, 9, WAC_CMD_RETRIES);
@@ -783,6 +841,17 @@ static struct attribute_group intuos4_led_attr_group = {
 	.attrs = intuos4_led_attrs,
 };
 
+static struct attribute *intuos5_led_attrs[] = {
+	&dev_attr_status0_luminance.attr,
+	&dev_attr_status_led0_select.attr,
+	NULL
+};
+
+static struct attribute_group intuos5_led_attr_group = {
+	.name = "wacom_led",
+	.attrs = intuos5_led_attrs,
+};
+
 static int wacom_initialize_leds(struct wacom *wacom)
 {
 	int error;
@@ -812,6 +881,19 @@ static int wacom_initialize_leds(struct wacom *wacom)
 					   &cintiq_led_attr_group);
 		break;
 
+	case INTUOS5S:
+	case INTUOS5:
+	case INTUOS5L:
+		wacom->led.select[0] = 0;
+		wacom->led.select[1] = 0;
+		wacom->led.llv = 32;
+		wacom->led.hlv = 0;
+		wacom->led.img_lum = 0;
+
+		error = sysfs_create_group(&wacom->intf->dev.kobj,
+					   &intuos5_led_attr_group);
+		break;
+
 	default:
 		return 0;
 	}
@@ -840,6 +922,13 @@ static void wacom_destroy_leds(struct wacom *wacom)
 		sysfs_remove_group(&wacom->intf->dev.kobj,
 				   &cintiq_led_attr_group);
 		break;
+
+	case INTUOS5S:
+	case INTUOS5:
+	case INTUOS5L:
+		sysfs_remove_group(&wacom->intf->dev.kobj,
+				   &intuos5_led_attr_group);
+		break;
 	}
 }
 
@@ -901,8 +990,10 @@ static int wacom_register_input(struct wacom *wacom)
 	int error;
 
 	input_dev = input_allocate_device();
-	if (!input_dev)
-		return -ENOMEM;
+	if (!input_dev) {
+		error = -ENOMEM;
+		goto fail1;
+	}
 
 	input_dev->name = wacom_wac->name;
 	input_dev->dev.parent = &intf->dev;
@@ -912,14 +1003,20 @@ static int wacom_register_input(struct wacom *wacom)
 	input_set_drvdata(input_dev, wacom);
 
 	wacom_wac->input = input_dev;
-	wacom_setup_input_capabilities(input_dev, wacom_wac);
+	error = wacom_setup_input_capabilities(input_dev, wacom_wac);
+	if (error)
+		goto fail1;
 
 	error = input_register_device(input_dev);
-	if (error) {
-		input_free_device(input_dev);
-		wacom_wac->input = NULL;
-	}
+	if (error)
+		goto fail2;
 
+	return 0;
+
+fail2:
+	input_free_device(input_dev);
+	wacom_wac->input = NULL;
+fail1:
 	return error;
 }
 
@@ -938,22 +1035,22 @@ static void wacom_wireless_work(struct work_struct *work)
 	wacom = usb_get_intfdata(usbdev->config->interface[1]);
 	if (wacom->wacom_wac.input)
 		input_unregister_device(wacom->wacom_wac.input);
-	wacom->wacom_wac.input = 0;
+	wacom->wacom_wac.input = NULL;
 
 	/* Touch interface */
 	wacom = usb_get_intfdata(usbdev->config->interface[2]);
 	if (wacom->wacom_wac.input)
 		input_unregister_device(wacom->wacom_wac.input);
-	wacom->wacom_wac.input = 0;
+	wacom->wacom_wac.input = NULL;
 
 	if (wacom_wac->pid == 0) {
-		printk(KERN_INFO "wacom: wireless tablet disconnected\n");
+		dev_info(&wacom->intf->dev, "wireless tablet disconnected\n");
 	} else {
 		const struct usb_device_id *id = wacom_ids;
 
-		printk(KERN_INFO
-		       "wacom: wireless tablet connected with PID %x\n",
-		       wacom_wac->pid);
+		dev_info(&wacom->intf->dev,
+			 "wireless tablet connected with PID %x\n",
+			 wacom_wac->pid);
 
 		while (id->match_flags) {
 			if (id->idVendor == USB_VENDOR_ID_WACOM &&
@@ -963,8 +1060,8 @@ static void wacom_wireless_work(struct work_struct *work)
 		}
 
 		if (!id->match_flags) {
-			printk(KERN_INFO
-			       "wacom: ignorning unknown PID.\n");
+			dev_info(&wacom->intf->dev,
+				 "ignoring unknown PID.\n");
 			return;
 		}
 
@@ -1035,11 +1132,33 @@ static int wacom_probe(struct usb_interface *intf, const struct usb_device_id *i
 
 	endpoint = &intf->cur_altsetting->endpoint[0].desc;
 
-	/* Retrieve the physical and logical size for OEM devices */
+	/* Retrieve the physical and logical size for touch devices */
 	error = wacom_retrieve_hid_descriptor(intf, features);
 	if (error)
 		goto fail3;
 
+	/*
+	 * Intuos5 has no useful data about its touch interface in its
+	 * HID descriptor. If this is the touch interface (wMaxPacketSize
+	 * of WACOM_PKGLEN_BBTOUCH3), override the table values.
+	 */
+	if (features->type >= INTUOS5S && features->type <= INTUOS5L) {
+		if (endpoint->wMaxPacketSize == WACOM_PKGLEN_BBTOUCH3) {
+			features->device_type = BTN_TOOL_FINGER;
+			features->pktlen = WACOM_PKGLEN_BBTOUCH3;
+
+			features->x_phy =
+				(features->x_max * 100) / features->x_resolution;
+			features->y_phy =
+				(features->y_max * 100) / features->y_resolution;
+
+			features->x_max = 4096;
+			features->y_max = 4096;
+		} else {
+			features->device_type = BTN_TOOL_PEN;
+		}
+	}
+
 	wacom_setup_device_quirks(features);
 
 	strlcpy(wacom_wac->name, features->name, sizeof(wacom_wac->name));
diff --git a/drivers/input/tablet/wacom_wac.c b/drivers/input/tablet/wacom_wac.c
index cecd35c8f0b..004bc1bb154 100644
--- a/drivers/input/tablet/wacom_wac.c
+++ b/drivers/input/tablet/wacom_wac.c
@@ -61,7 +61,8 @@ static int wacom_penpartner_irq(struct wacom_wac *wacom)
 		break;
 
 	default:
-		printk(KERN_INFO "wacom_penpartner_irq: received unknown report #%d\n", data[0]);
+		dev_dbg(input->dev.parent,
+			"%s: received unknown report #%d\n", __func__, data[0]);
 		return 0;
         }
 
@@ -76,7 +77,8 @@ static int wacom_pl_irq(struct wacom_wac *wacom)
 	int prox, pressure;
 
 	if (data[0] != WACOM_REPORT_PENABLED) {
-		dbg("wacom_pl_irq: received unknown report #%d", data[0]);
+		dev_dbg(input->dev.parent,
+			"%s: received unknown report #%d\n", __func__, data[0]);
 		return 0;
 	}
 
@@ -146,7 +148,8 @@ static int wacom_ptu_irq(struct wacom_wac *wacom)
 	struct input_dev *input = wacom->input;
 
 	if (data[0] != WACOM_REPORT_PENABLED) {
-		printk(KERN_INFO "wacom_ptu_irq: received unknown report #%d\n", data[0]);
+		dev_dbg(input->dev.parent,
+			"%s: received unknown report #%d\n", __func__, data[0]);
 		return 0;
 	}
 
@@ -175,7 +178,8 @@ static int wacom_dtu_irq(struct wacom_wac *wacom)
 	struct input_dev *input = wacom->input;
 	int prox = data[1] & 0x20, pressure;
 
-	dbg("wacom_dtu_irq: received report #%d", data[0]);
+	dev_dbg(input->dev.parent,
+		"%s: received report #%d", __func__, data[0]);
 
 	if (prox) {
 		/* Going into proximity select tool */
@@ -211,7 +215,8 @@ static int wacom_graphire_irq(struct wacom_wac *wacom)
 	int retval = 0;
 
 	if (data[0] != WACOM_REPORT_PENABLED) {
-		dbg("wacom_graphire_irq: received unknown report #%d", data[0]);
+		dev_dbg(input->dev.parent,
+			"%s: received unknown report #%d\n", __func__, data[0]);
 		goto exit;
 	}
 
@@ -321,6 +326,9 @@ static int wacom_intuos_inout(struct wacom_wac *wacom)
 
 	/* Enter report */
 	if ((data[1] & 0xfc) == 0xc0) {
+		if (features->type >= INTUOS5S && features->type <= INTUOS5L)
+			wacom->shared->stylus_in_proximity = true;
+
 		/* serial number of the tool */
 		wacom->serial[idx] = ((data[3] & 0x0f) << 28) +
 			(data[4] << 20) + (data[5] << 12) +
@@ -406,6 +414,9 @@ static int wacom_intuos_inout(struct wacom_wac *wacom)
 
 	/* Exit report */
 	if ((data[1] & 0xfe) == 0x80) {
+		if (features->type >= INTUOS5S && features->type <= INTUOS5L)
+			wacom->shared->stylus_in_proximity = false;
+
 		/*
 		 * Reset all states otherwise we lose the initial states
 		 * when in-prox next time
@@ -452,6 +463,7 @@ static void wacom_intuos_general(struct wacom_wac *wacom)
 	if ((data[1] & 0xb8) == 0xa0) {
 		t = (data[6] << 2) | ((data[7] >> 6) & 3);
 		if ((features->type >= INTUOS4S && features->type <= INTUOS4L) ||
+                    (features->type >= INTUOS5S && features->type <= INTUOS5L) ||
 		    features->type == WACOM_21UX2 || features->type == WACOM_24HD) {
 			t = (t << 1) | (data[1] & 1);
 		}
@@ -482,9 +494,13 @@ static int wacom_intuos_irq(struct wacom_wac *wacom)
 	unsigned int t;
 	int idx = 0, result;
 
-	if (data[0] != WACOM_REPORT_PENABLED && data[0] != WACOM_REPORT_INTUOSREAD
-		&& data[0] != WACOM_REPORT_INTUOSWRITE && data[0] != WACOM_REPORT_INTUOSPAD) {
-		dbg("wacom_intuos_irq: received unknown report #%d", data[0]);
+	if (data[0] != WACOM_REPORT_PENABLED &&
+	    data[0] != WACOM_REPORT_INTUOSREAD &&
+	    data[0] != WACOM_REPORT_INTUOSWRITE &&
+	    data[0] != WACOM_REPORT_INTUOSPAD &&
+	    data[0] != WACOM_REPORT_INTUOS5PAD) {
+		dev_dbg(input->dev.parent,
+			"%s: received unknown report #%d\n", __func__, data[0]);
                 return 0;
 	}
 
@@ -493,7 +509,7 @@ static int wacom_intuos_irq(struct wacom_wac *wacom)
 		idx = data[1] & 0x01;
 
 	/* pad packets. Works as a second tool and is always in prox */
-	if (data[0] == WACOM_REPORT_INTUOSPAD) {
+	if (data[0] == WACOM_REPORT_INTUOSPAD || data[0] == WACOM_REPORT_INTUOS5PAD) {
 		if (features->type >= INTUOS4S && features->type <= INTUOS4L) {
 			input_report_key(input, BTN_0, (data[2] & 0x01));
 			input_report_key(input, BTN_1, (data[3] & 0x01));
@@ -569,6 +585,34 @@ static int wacom_intuos_irq(struct wacom_wac *wacom)
 				input_report_key(input, wacom->tool[1], 0);
 				input_report_abs(input, ABS_MISC, 0);
 			}
+		} else if (features->type >= INTUOS5S && features->type <= INTUOS5L) {
+			int i;
+
+			/* Touch ring mode switch has no capacitive sensor */
+			input_report_key(input, BTN_0, (data[3] & 0x01));
+
+			/*
+			 * ExpressKeys on Intuos5 have a capacitive sensor in
+			 * addition to the mechanical switch. Switch data is
+			 * stored in data[4], capacitive data in data[5].
+			 */
+			for (i = 0; i < 8; i++)
+				input_report_key(input, BTN_1 + i, data[4] & (1 << i));
+
+			if (data[2] & 0x80) {
+				input_report_abs(input, ABS_WHEEL, (data[2] & 0x7f));
+			} else {
+				/* Out of proximity, clear wheel value. */
+				input_report_abs(input, ABS_WHEEL, 0);
+			}
+
+			if (data[2] | (data[3] & 0x01) | data[4]) {
+				input_report_key(input, wacom->tool[1], 1);
+				input_report_abs(input, ABS_MISC, PAD_DEVICE_ID);
+			} else {
+				input_report_key(input, wacom->tool[1], 0);
+				input_report_abs(input, ABS_MISC, 0);
+			}
 		} else {
 			if (features->type == WACOM_21UX2) {
 				input_report_key(input, BTN_0, (data[5] & 0x01));
@@ -632,7 +676,9 @@ static int wacom_intuos_irq(struct wacom_wac *wacom)
 	    (features->type == INTUOS3 ||
 	     features->type == INTUOS3S ||
 	     features->type == INTUOS4 ||
-	     features->type == INTUOS4S)) {
+	     features->type == INTUOS4S ||
+	     features->type == INTUOS5 ||
+	     features->type == INTUOS5S)) {
 
 		return 0;
 	}
@@ -685,7 +731,8 @@ static int wacom_intuos_irq(struct wacom_wac *wacom)
 
 		} else if (wacom->tool[idx] == BTN_TOOL_MOUSE) {
 			/* I4 mouse */
-			if (features->type >= INTUOS4S && features->type <= INTUOS4L) {
+			if ((features->type >= INTUOS4S && features->type <= INTUOS4L) ||
+			    (features->type >= INTUOS5S && features->type <= INTUOS5L)) {
 				input_report_key(input, BTN_LEFT,   data[6] & 0x01);
 				input_report_key(input, BTN_MIDDLE, data[6] & 0x02);
 				input_report_key(input, BTN_RIGHT,  data[6] & 0x04);
@@ -712,7 +759,7 @@ static int wacom_intuos_irq(struct wacom_wac *wacom)
 				}
 			}
 		} else if ((features->type < INTUOS3S || features->type == INTUOS3L ||
-				features->type == INTUOS4L) &&
+				features->type == INTUOS4L || features->type == INTUOS5L) &&
 			   wacom->tool[idx] == BTN_TOOL_LENS) {
 			/* Lens cursor packets */
 			input_report_key(input, BTN_LEFT,   data[8] & 0x01);
@@ -729,6 +776,72 @@ static int wacom_intuos_irq(struct wacom_wac *wacom)
 	return 1;
 }
 
+static int find_slot_from_contactid(struct wacom_wac *wacom, int contactid)
+{
+	int touch_max = wacom->features.touch_max;
+	int i;
+
+	if (!wacom->slots)
+		return -1;
+
+	for (i = 0; i < touch_max; ++i) {
+		if (wacom->slots[i] == contactid)
+			return i;
+	}
+	for (i = 0; i < touch_max; ++i) {
+		if (wacom->slots[i] == -1)
+			return i;
+	}
+	return -1;
+}
+
+static int wacom_mt_touch(struct wacom_wac *wacom)
+{
+	struct input_dev *input = wacom->input;
+	char *data = wacom->data;
+	int i;
+	int current_num_contacts = data[2];
+	int contacts_to_send = 0;
+
+	/*
+	 * First packet resets the counter since only the first
+	 * packet in series will have non-zero current_num_contacts.
+	 */
+	if (current_num_contacts)
+		wacom->num_contacts_left = current_num_contacts;
+
+	/* There are at most 5 contacts per packet */
+	contacts_to_send = min(5, wacom->num_contacts_left);
+
+	for (i = 0; i < contacts_to_send; i++) {
+		int offset = (WACOM_BYTES_PER_MT_PACKET * i) + 3;
+		bool touch = data[offset] & 0x1;
+		int id = le16_to_cpup((__le16 *)&data[offset + 1]);
+		int slot = find_slot_from_contactid(wacom, id);
+
+		if (slot < 0)
+			continue;
+
+		input_mt_slot(input, slot);
+		input_mt_report_slot_state(input, MT_TOOL_FINGER, touch);
+		if (touch) {
+			int x = le16_to_cpup((__le16 *)&data[offset + 7]);
+			int y = le16_to_cpup((__le16 *)&data[offset + 9]);
+			input_report_abs(input, ABS_MT_POSITION_X, x);
+			input_report_abs(input, ABS_MT_POSITION_Y, y);
+		}
+		wacom->slots[slot] = touch ? id : -1;
+	}
+
+	input_mt_report_pointer_emulation(input, true);
+
+	wacom->num_contacts_left -= contacts_to_send;
+	if (wacom->num_contacts_left < 0)
+		wacom->num_contacts_left = 0;
+
+	return 1;
+}
+
 static int wacom_tpc_mt_touch(struct wacom_wac *wacom)
 {
 	struct input_dev *input = wacom->input;
@@ -767,6 +880,9 @@ static int wacom_tpc_single_touch(struct wacom_wac *wacom, size_t len)
 	bool prox;
 	int x = 0, y = 0;
 
+	if (wacom->features.touch_max > 1 || len > WACOM_PKGLEN_TPC2FG)
+		return 0;
+
 	if (!wacom->shared->stylus_in_proximity) {
 		if (len == WACOM_PKGLEN_TPC1FG) {
 			prox = data[0] & 0x01;
@@ -830,14 +946,15 @@ static int wacom_tpc_irq(struct wacom_wac *wacom, size_t len)
 {
 	char *data = wacom->data;
 
-	dbg("wacom_tpc_irq: received report #%d", data[0]);
+	dev_dbg(wacom->input->dev.parent,
+		"%s: received report #%d\n", __func__, data[0]);
 
 	switch (len) {
 	case WACOM_PKGLEN_TPC1FG:
-		 return wacom_tpc_single_touch(wacom, len);
+		return wacom_tpc_single_touch(wacom, len);
 
 	case WACOM_PKGLEN_TPC2FG:
- 		return wacom_tpc_mt_touch(wacom);
+		return wacom_tpc_mt_touch(wacom);
 
 	default:
 		switch (data[0]) {
@@ -846,6 +963,9 @@ static int wacom_tpc_irq(struct wacom_wac *wacom, size_t len)
 		case WACOM_REPORT_TPCST:
 			return wacom_tpc_single_touch(wacom, len);
 
+		case WACOM_REPORT_TPCMT:
+			return wacom_mt_touch(wacom);
+
 		case WACOM_REPORT_PENABLED:
 			return wacom_tpc_pen(wacom);
 		}
@@ -1114,8 +1234,18 @@ void wacom_wac_irq(struct wacom_wac *wacom_wac, size_t len)
 		sync = wacom_intuos_irq(wacom_wac);
 		break;
 
+	case INTUOS5S:
+	case INTUOS5:
+	case INTUOS5L:
+		if (len == WACOM_PKGLEN_BBTOUCH3)
+			sync = wacom_bpt3_touch(wacom_wac);
+		else
+			sync = wacom_intuos_irq(wacom_wac);
+		break;
+
 	case TABLETPC:
 	case TABLETPC2FG:
+	case MTSCREEN:
 		sync = wacom_tpc_irq(wacom_wac, len);
 		break;
 
@@ -1188,7 +1318,9 @@ void wacom_setup_device_quirks(struct wacom_features *features)
 
 	/* these device have multiple inputs */
 	if (features->type == TABLETPC || features->type == TABLETPC2FG ||
-	    features->type == BAMBOO_PT || features->type == WIRELESS)
+	    features->type == BAMBOO_PT || features->type == WIRELESS ||
+	    (features->type >= INTUOS5S && features->type <= INTUOS5L) ||
+	    features->type == MTSCREEN)
 		features->quirks |= WACOM_QUIRK_MULTI_INPUT;
 
 	/* quirk for bamboo touch with 2 low res touches */
@@ -1219,8 +1351,8 @@ static unsigned int wacom_calculate_touch_res(unsigned int logical_max,
        return (logical_max * 100) / physical_max;
 }
 
-void wacom_setup_input_capabilities(struct input_dev *input_dev,
-				    struct wacom_wac *wacom_wac)
+int wacom_setup_input_capabilities(struct input_dev *input_dev,
+				   struct wacom_wac *wacom_wac)
 {
 	struct wacom_features *features = &wacom_wac->features;
 	int i;
@@ -1355,6 +1487,50 @@ void wacom_setup_input_capabilities(struct input_dev *input_dev,
 		wacom_setup_intuos(wacom_wac);
 		break;
 
+	case INTUOS5:
+	case INTUOS5L:
+		if (features->device_type == BTN_TOOL_PEN) {
+			__set_bit(BTN_7, input_dev->keybit);
+			__set_bit(BTN_8, input_dev->keybit);
+		}
+		/* fall through */
+
+	case INTUOS5S:
+		__set_bit(INPUT_PROP_POINTER, input_dev->propbit);
+
+		if (features->device_type == BTN_TOOL_PEN) {
+			for (i = 0; i < 7; i++)
+				__set_bit(BTN_0 + i, input_dev->keybit);
+
+			input_set_abs_params(input_dev, ABS_DISTANCE, 0,
+					      features->distance_max,
+					      0, 0);
+
+			input_set_abs_params(input_dev, ABS_Z, -900, 899, 0, 0);
+
+			wacom_setup_intuos(wacom_wac);
+		} else if (features->device_type == BTN_TOOL_FINGER) {
+			__clear_bit(ABS_MISC, input_dev->absbit);
+
+			__set_bit(BTN_TOOL_FINGER, input_dev->keybit);
+			__set_bit(BTN_TOOL_DOUBLETAP, input_dev->keybit);
+			__set_bit(BTN_TOOL_TRIPLETAP, input_dev->keybit);
+			__set_bit(BTN_TOOL_QUADTAP, input_dev->keybit);
+
+			input_mt_init_slots(input_dev, features->touch_max);
+
+			input_set_abs_params(input_dev, ABS_MT_TOUCH_MAJOR,
+			                     0, 255, 0, 0);
+
+			input_set_abs_params(input_dev, ABS_MT_POSITION_X,
+					     0, features->x_max,
+					     features->x_fuzz, 0);
+			input_set_abs_params(input_dev, ABS_MT_POSITION_Y,
+					     0, features->y_max,
+					     features->y_fuzz, 0);
+		}
+		break;
+
 	case INTUOS4:
 	case INTUOS4L:
 		__set_bit(BTN_7, input_dev->keybit);
@@ -1372,9 +1548,19 @@ void wacom_setup_input_capabilities(struct input_dev *input_dev,
 		break;
 
 	case TABLETPC2FG:
+	case MTSCREEN:
 		if (features->device_type == BTN_TOOL_FINGER) {
 
-			input_mt_init_slots(input_dev, 2);
+			wacom_wac->slots = kmalloc(features->touch_max *
+							sizeof(int),
+						   GFP_KERNEL);
+			if (!wacom_wac->slots)
+				return -ENOMEM;
+
+			for (i = 0; i < features->touch_max; i++)
+				wacom_wac->slots[i] = -1;
+
+			input_mt_init_slots(input_dev, features->touch_max);
 			input_set_abs_params(input_dev, ABS_MT_TOOL_TYPE,
 					0, MT_TOOL_MAX, 0, 0);
 			input_set_abs_params(input_dev, ABS_MT_POSITION_X,
@@ -1429,6 +1615,7 @@ void wacom_setup_input_capabilities(struct input_dev *input_dev,
 
 			__set_bit(BTN_TOOL_FINGER, input_dev->keybit);
 			__set_bit(BTN_TOOL_DOUBLETAP, input_dev->keybit);
+			input_mt_init_slots(input_dev, features->touch_max);
 
 			if (features->pktlen == WACOM_PKGLEN_BBTOUCH3) {
 				__set_bit(BTN_TOOL_TRIPLETAP,
@@ -1436,13 +1623,9 @@ void wacom_setup_input_capabilities(struct input_dev *input_dev,
 				__set_bit(BTN_TOOL_QUADTAP,
 					  input_dev->keybit);
 
-				input_mt_init_slots(input_dev, 16);
-
 				input_set_abs_params(input_dev,
 						     ABS_MT_TOUCH_MAJOR,
 						     0, 255, 0, 0);
-			} else {
-				input_mt_init_slots(input_dev, 2);
 			}
 
 			input_set_abs_params(input_dev, ABS_MT_POSITION_X,
@@ -1462,6 +1645,7 @@ void wacom_setup_input_capabilities(struct input_dev *input_dev,
 		}
 		break;
 	}
+	return 0;
 }
 
 static const struct wacom_features wacom_features_0x00 =
@@ -1629,6 +1813,24 @@ static const struct wacom_features wacom_features_0xBB =
 static const struct wacom_features wacom_features_0xBC =
 	{ "Wacom Intuos4 WL",     WACOM_PKGLEN_INTUOS,    40840, 25400, 2047,
 	  63, INTUOS4, WACOM_INTUOS3_RES, WACOM_INTUOS3_RES };
+static const struct wacom_features wacom_features_0x26 =
+	{ "Wacom Intuos5 touch S", WACOM_PKGLEN_INTUOS,  31496, 19685, 2047,
+	  63, INTUOS5S, WACOM_INTUOS3_RES, WACOM_INTUOS3_RES,
+	  .touch_max = 16 };
+static const struct wacom_features wacom_features_0x27 =
+	{ "Wacom Intuos5 touch M", WACOM_PKGLEN_INTUOS,  44704, 27940, 2047,
+	  63, INTUOS5, WACOM_INTUOS3_RES, WACOM_INTUOS3_RES,
+	  .touch_max = 16 };
+static const struct wacom_features wacom_features_0x28 =
+	{ "Wacom Intuos5 touch L", WACOM_PKGLEN_INTUOS, 65024, 40640, 2047,
+	  63, INTUOS5L, WACOM_INTUOS3_RES, WACOM_INTUOS3_RES,
+	  .touch_max = 16 };
+static const struct wacom_features wacom_features_0x29 =
+	{ "Wacom Intuos5 S", WACOM_PKGLEN_INTUOS,  31496, 19685, 2047,
+	  63, INTUOS5S, WACOM_INTUOS3_RES, WACOM_INTUOS3_RES };
+static const struct wacom_features wacom_features_0x2A =
+	{ "Wacom Intuos5 M", WACOM_PKGLEN_INTUOS,  44704, 27940, 2047,
+	  63, INTUOS5, WACOM_INTUOS3_RES, WACOM_INTUOS3_RES };
 static const struct wacom_features wacom_features_0xF4 =
 	{ "Wacom Cintiq 24HD",    WACOM_PKGLEN_INTUOS,   104480, 65600, 2047,
 	  63, WACOM_24HD, WACOM_INTUOS3_RES, WACOM_INTUOS3_RES };
@@ -1670,13 +1872,19 @@ static const struct wacom_features wacom_features_0x9F =
 	  0, TABLETPC, WACOM_INTUOS_RES, WACOM_INTUOS_RES };
 static const struct wacom_features wacom_features_0xE2 =
 	{ "Wacom ISDv4 E2",       WACOM_PKGLEN_TPC2FG,    26202, 16325,  255,
-	  0, TABLETPC2FG, WACOM_INTUOS_RES, WACOM_INTUOS_RES };
+	  0, TABLETPC2FG, WACOM_INTUOS_RES, WACOM_INTUOS_RES,
+	  .touch_max = 2 };
 static const struct wacom_features wacom_features_0xE3 =
 	{ "Wacom ISDv4 E3",       WACOM_PKGLEN_TPC2FG,    26202, 16325,  255,
-	  0, TABLETPC2FG, WACOM_INTUOS_RES, WACOM_INTUOS_RES };
+	  0, TABLETPC2FG, WACOM_INTUOS_RES, WACOM_INTUOS_RES,
+	  .touch_max = 2 };
+static const struct wacom_features wacom_features_0xE5 =
+	{ "Wacom ISDv4 E5",       WACOM_PKGLEN_MTOUCH,    26202, 16325,  255,
+	  0, MTSCREEN, WACOM_INTUOS_RES, WACOM_INTUOS_RES };
 static const struct wacom_features wacom_features_0xE6 =
 	{ "Wacom ISDv4 E6",       WACOM_PKGLEN_TPC2FG,    27760, 15694,  255,
-	  0, TABLETPC2FG, WACOM_INTUOS_RES, WACOM_INTUOS_RES };
+	  0, TABLETPC2FG, WACOM_INTUOS_RES, WACOM_INTUOS_RES,
+	.touch_max = 2 };
 static const struct wacom_features wacom_features_0xEC =
 	{ "Wacom ISDv4 EC",       WACOM_PKGLEN_GRAPHIRE,  25710, 14500,  255,
 	  0, TABLETPC,    WACOM_INTUOS_RES, WACOM_INTUOS_RES };
@@ -1685,19 +1893,22 @@ static const struct wacom_features wacom_features_0x47 =
 	  31, INTUOS, WACOM_INTUOS_RES, WACOM_INTUOS_RES };
 static const struct wacom_features wacom_features_0x84 =
 	{ "Wacom Wireless Receiver", WACOM_PKGLEN_WIRELESS, 0, 0, 0,
-	  0, WIRELESS, 0, 0 };
+	  0, WIRELESS, 0, 0, .touch_max = 16 };
 static const struct wacom_features wacom_features_0xD0 =
 	{ "Wacom Bamboo 2FG",     WACOM_PKGLEN_BBFUN,     14720,  9200, 1023,
-	  31, BAMBOO_PT, WACOM_INTUOS_RES, WACOM_INTUOS_RES };
+	  31, BAMBOO_PT, WACOM_INTUOS_RES, WACOM_INTUOS_RES,
+	  .touch_max = 2 };
 static const struct wacom_features wacom_features_0xD1 =
 	{ "Wacom Bamboo 2FG 4x5", WACOM_PKGLEN_BBFUN,     14720,  9200, 1023,
-	  31, BAMBOO_PT, WACOM_INTUOS_RES, WACOM_INTUOS_RES };
+	  31, BAMBOO_PT, WACOM_INTUOS_RES, WACOM_INTUOS_RES,
+	  .touch_max = 2 };
 static const struct wacom_features wacom_features_0xD2 =
 	{ "Wacom Bamboo Craft",   WACOM_PKGLEN_BBFUN,     14720,  9200, 1023,
 	  31, BAMBOO_PT, WACOM_INTUOS_RES, WACOM_INTUOS_RES };
 static const struct wacom_features wacom_features_0xD3 =
 	{ "Wacom Bamboo 2FG 6x8", WACOM_PKGLEN_BBFUN,     21648, 13700, 1023,
-	  31, BAMBOO_PT, WACOM_INTUOS_RES, WACOM_INTUOS_RES };
+	  31, BAMBOO_PT, WACOM_INTUOS_RES, WACOM_INTUOS_RES,
+	  .touch_max = 2 };
 static const struct wacom_features wacom_features_0xD4 =
 	{ "Wacom Bamboo Pen",     WACOM_PKGLEN_BBFUN,     14720,  9200, 1023,
 	  31, BAMBOO_PT, WACOM_INTUOS_RES, WACOM_INTUOS_RES };
@@ -1706,28 +1917,35 @@ static const struct wacom_features wacom_features_0xD5 =
 	  31, BAMBOO_PT, WACOM_INTUOS_RES, WACOM_INTUOS_RES };
 static const struct wacom_features wacom_features_0xD6 =
 	{ "Wacom BambooPT 2FG 4x5", WACOM_PKGLEN_BBFUN,   14720,  9200, 1023,
-	  31, BAMBOO_PT, WACOM_INTUOS_RES, WACOM_INTUOS_RES };
+	  31, BAMBOO_PT, WACOM_INTUOS_RES, WACOM_INTUOS_RES,
+	  .touch_max = 2 };
 static const struct wacom_features wacom_features_0xD7 =
 	{ "Wacom BambooPT 2FG Small", WACOM_PKGLEN_BBFUN, 14720,  9200, 1023,
-	  31, BAMBOO_PT, WACOM_INTUOS_RES, WACOM_INTUOS_RES };
+	  31, BAMBOO_PT, WACOM_INTUOS_RES, WACOM_INTUOS_RES,
+	  .touch_max = 2 };
 static const struct wacom_features wacom_features_0xD8 =
 	{ "Wacom Bamboo Comic 2FG", WACOM_PKGLEN_BBFUN,   21648, 13700, 1023,
-	  31, BAMBOO_PT, WACOM_INTUOS_RES, WACOM_INTUOS_RES };
+	  31, BAMBOO_PT, WACOM_INTUOS_RES, WACOM_INTUOS_RES,
+	  .touch_max = 2 };
 static const struct wacom_features wacom_features_0xDA =
 	{ "Wacom Bamboo 2FG 4x5 SE", WACOM_PKGLEN_BBFUN,  14720,  9200, 1023,
-	  31, BAMBOO_PT, WACOM_INTUOS_RES, WACOM_INTUOS_RES };
+	  31, BAMBOO_PT, WACOM_INTUOS_RES, WACOM_INTUOS_RES,
+	  .touch_max = 2 };
 static struct wacom_features wacom_features_0xDB =
 	{ "Wacom Bamboo 2FG 6x8 SE", WACOM_PKGLEN_BBFUN,  21648, 13700, 1023,
-	  31, BAMBOO_PT, WACOM_INTUOS_RES, WACOM_INTUOS_RES };
+	  31, BAMBOO_PT, WACOM_INTUOS_RES, WACOM_INTUOS_RES,
+	  .touch_max = 2 };
 static const struct wacom_features wacom_features_0xDD =
         { "Wacom Bamboo Connect", WACOM_PKGLEN_BBPEN,     14720,  9200, 1023,
           31, BAMBOO_PT, WACOM_INTUOS_RES, WACOM_INTUOS_RES };
 static const struct wacom_features wacom_features_0xDE =
         { "Wacom Bamboo 16FG 4x5", WACOM_PKGLEN_BBPEN,    14720,  9200, 1023,
-          31, BAMBOO_PT, WACOM_INTUOS_RES, WACOM_INTUOS_RES };
+	  31, BAMBOO_PT, WACOM_INTUOS_RES, WACOM_INTUOS_RES,
+	  .touch_max = 16 };
 static const struct wacom_features wacom_features_0xDF =
         { "Wacom Bamboo 16FG 6x8", WACOM_PKGLEN_BBPEN,    21648, 13700, 1023,
-          31, BAMBOO_PT, WACOM_INTUOS_RES, WACOM_INTUOS_RES };
+	  31, BAMBOO_PT, WACOM_INTUOS_RES, WACOM_INTUOS_RES,
+	  .touch_max = 16 };
 static const struct wacom_features wacom_features_0x6004 =
 	{ "ISD-V4",               WACOM_PKGLEN_GRAPHIRE,  12800,  8000,  255,
 	  0, TABLETPC, WACOM_INTUOS_RES, WACOM_INTUOS_RES };
@@ -1801,6 +2019,11 @@ const struct usb_device_id wacom_ids[] = {
 	{ USB_DEVICE_WACOM(0xBA) },
 	{ USB_DEVICE_WACOM(0xBB) },
 	{ USB_DEVICE_WACOM(0xBC) },
+	{ USB_DEVICE_WACOM(0x26) },
+	{ USB_DEVICE_WACOM(0x27) },
+	{ USB_DEVICE_WACOM(0x28) },
+	{ USB_DEVICE_WACOM(0x29) },
+	{ USB_DEVICE_WACOM(0x2A) },
 	{ USB_DEVICE_WACOM(0x3F) },
 	{ USB_DEVICE_WACOM(0xC5) },
 	{ USB_DEVICE_WACOM(0xC6) },
@@ -1836,6 +2059,7 @@ const struct usb_device_id wacom_ids[] = {
 	{ USB_DEVICE_WACOM(0x9F) },
 	{ USB_DEVICE_WACOM(0xE2) },
 	{ USB_DEVICE_WACOM(0xE3) },
+	{ USB_DEVICE_WACOM(0xE5) },
 	{ USB_DEVICE_WACOM(0xE6) },
 	{ USB_DEVICE_WACOM(0xEC) },
 	{ USB_DEVICE_WACOM(0x47) },
diff --git a/drivers/input/tablet/wacom_wac.h b/drivers/input/tablet/wacom_wac.h
index ba5a334e54d..78fbd3f4200 100644
--- a/drivers/input/tablet/wacom_wac.h
+++ b/drivers/input/tablet/wacom_wac.h
@@ -25,6 +25,10 @@
 #define WACOM_PKGLEN_BBTOUCH3	64
 #define WACOM_PKGLEN_BBPEN	10
 #define WACOM_PKGLEN_WIRELESS	32
+#define WACOM_PKGLEN_MTOUCH	62
+
+/* wacom data size per MT contact */
+#define WACOM_BYTES_PER_MT_PACKET	11
 
 /* device IDs */
 #define STYLUS_DEVICE_ID	0x02
@@ -38,8 +42,10 @@
 #define WACOM_REPORT_INTUOSREAD		5
 #define WACOM_REPORT_INTUOSWRITE	6
 #define WACOM_REPORT_INTUOSPAD		12
+#define WACOM_REPORT_INTUOS5PAD		3
 #define WACOM_REPORT_TPC1FG		6
 #define WACOM_REPORT_TPC2FG		13
+#define WACOM_REPORT_TPCMT		13
 #define WACOM_REPORT_TPCHID		15
 #define WACOM_REPORT_TPCST		16
 
@@ -65,6 +71,9 @@ enum {
 	INTUOS4S,
 	INTUOS4,
 	INTUOS4L,
+	INTUOS5S,
+	INTUOS5,
+	INTUOS5L,
 	WACOM_24HD,
 	WACOM_21UX2,
 	CINTIQ,
@@ -72,6 +81,7 @@ enum {
 	WACOM_MO,
 	TABLETPC,
 	TABLETPC2FG,
+	MTSCREEN,
 	MAX_TYPE
 };
 
@@ -95,6 +105,7 @@ struct wacom_features {
 	int pressure_fuzz;
 	int distance_fuzz;
 	unsigned quirks;
+	unsigned touch_max;
 };
 
 struct wacom_shared {
@@ -113,6 +124,8 @@ struct wacom_wac {
 	struct input_dev *input;
 	int pid;
 	int battery_capacity;
+	int num_contacts_left;
+	int *slots;
 };
 
 #endif
diff --git a/drivers/input/touchscreen/Kconfig b/drivers/input/touchscreen/Kconfig
index 2a2141915aa..98d263504ee 100644
--- a/drivers/input/touchscreen/Kconfig
+++ b/drivers/input/touchscreen/Kconfig
@@ -187,6 +187,23 @@ config TOUCHSCREEN_DA9034
 	  Say Y here to enable the support for the touchscreen found
 	  on Dialog Semiconductor DA9034 PMIC.
 
+	  If unsure, say N.
+
+	  To compile this driver as a module, choose M here: the
+	  module will be called da9034-ts.
+
+config TOUCHSCREEN_DA9052
+	tristate "Dialog DA9052/DA9053 TSI"
+	depends on PMIC_DA9052
+	help
+	  Say Y here to support the touchscreen found on Dialog Semiconductor
+	  DA9052-BC and DA9053-AA/Bx PMICs.
+
+	  If unsure, say N.
+
+	  To compile this driver as a module, choose M here: the
+	  module will be called da9052_tsi.
+
 config TOUCHSCREEN_DYNAPRO
 	tristate "Dynapro serial touchscreen"
 	select SERIO
@@ -306,6 +323,18 @@ config TOUCHSCREEN_WACOM_W8001
 	  To compile this driver as a module, choose M here: the
 	  module will be called wacom_w8001.
 
+config TOUCHSCREEN_WACOM_I2C
+	tristate "Wacom Tablet support (I2C)"
+	depends on I2C
+	help
+	  Say Y here if you want to use the I2C version of the Wacom
+	  Pen Tablet.
+
+	  If unsure, say N.
+
+	  To compile this driver as a module, choose M here: the module
+	  will be called wacom_i2c.
+
 config TOUCHSCREEN_LPC32XX
 	tristate "LPC32XX touchscreen controller"
 	depends on ARCH_LPC32XX
@@ -489,10 +518,10 @@ config TOUCHSCREEN_TI_TSCADC
 
 config TOUCHSCREEN_ATMEL_TSADCC
 	tristate "Atmel Touchscreen Interface"
-	depends on ARCH_AT91SAM9RL || ARCH_AT91SAM9G45
+	depends on ARCH_AT91
 	help
 	  Say Y here if you have a 4-wire touchscreen connected to the
-          ADC Controller on your Atmel SoC (such as the AT91SAM9RL).
+          ADC Controller on your Atmel SoC.
 
 	  If unsure, say N.
 
@@ -635,6 +664,7 @@ config TOUCHSCREEN_USB_COMPOSITE
 	  - Zytronic controllers
 	  - Elo TouchSystems 2700 IntelliTouch
 	  - EasyTouch USB Touch Controller from Data Modul
+	  - e2i (Mimo monitors)
 
 	  Have a look at <http://linux.chapter7.ch/touchkit/> for
 	  a usage description and the required user-space stuff.
@@ -721,7 +751,7 @@ config TOUCHSCREEN_USB_ELO
 
 config TOUCHSCREEN_USB_E2I
 	default y
-	bool "e2i Touchscreen controller (e.g. from Mimo 740)"
+	bool "e2i Touchscreen controller (e.g. from Mimo 740)" if EXPERT
 	depends on TOUCHSCREEN_USB_COMPOSITE
 
 config TOUCHSCREEN_USB_ZYTRONIC
@@ -744,7 +774,7 @@ config TOUCHSCREEN_USB_EASYTOUCH
 	bool "EasyTouch USB Touch controller device support" if EMBEDDED
 	depends on TOUCHSCREEN_USB_COMPOSITE
 	help
-	  Say Y here if you have a EasyTouch USB Touch controller device support.
+	  Say Y here if you have an EasyTouch USB Touch controller.
 	  If unsure, say N.
 
 config TOUCHSCREEN_TOUCHIT213
diff --git a/drivers/input/touchscreen/Makefile b/drivers/input/touchscreen/Makefile
index 3d5cf8cbf89..eb8bfe1c1a4 100644
--- a/drivers/input/touchscreen/Makefile
+++ b/drivers/input/touchscreen/Makefile
@@ -22,6 +22,7 @@ obj-$(CONFIG_TOUCHSCREEN_CYTTSP_CORE)	+= cyttsp_core.o
 obj-$(CONFIG_TOUCHSCREEN_CYTTSP_I2C)	+= cyttsp_i2c.o
 obj-$(CONFIG_TOUCHSCREEN_CYTTSP_SPI)	+= cyttsp_spi.o
 obj-$(CONFIG_TOUCHSCREEN_DA9034)	+= da9034-ts.o
+obj-$(CONFIG_TOUCHSCREEN_DA9052)	+= da9052_tsi.o
 obj-$(CONFIG_TOUCHSCREEN_DYNAPRO)	+= dynapro.o
 obj-$(CONFIG_TOUCHSCREEN_HAMPSHIRE)	+= hampshire.o
 obj-$(CONFIG_TOUCHSCREEN_GUNZE)		+= gunze.o
@@ -59,6 +60,7 @@ obj-$(CONFIG_TOUCHSCREEN_TSC2005)	+= tsc2005.o
 obj-$(CONFIG_TOUCHSCREEN_TSC2007)	+= tsc2007.o
 obj-$(CONFIG_TOUCHSCREEN_UCB1400)	+= ucb1400_ts.o
 obj-$(CONFIG_TOUCHSCREEN_WACOM_W8001)	+= wacom_w8001.o
+obj-$(CONFIG_TOUCHSCREEN_WACOM_I2C)	+= wacom_i2c.o
 obj-$(CONFIG_TOUCHSCREEN_WM831X)	+= wm831x-ts.o
 obj-$(CONFIG_TOUCHSCREEN_WM97XX)	+= wm97xx-ts.o
 wm97xx-ts-$(CONFIG_TOUCHSCREEN_WM9705)	+= wm9705.o
diff --git a/drivers/input/touchscreen/atmel_mxt_ts.c b/drivers/input/touchscreen/atmel_mxt_ts.c
index 19d4ea65ea0..42e645062c2 100644
--- a/drivers/input/touchscreen/atmel_mxt_ts.c
+++ b/drivers/input/touchscreen/atmel_mxt_ts.c
@@ -236,7 +236,6 @@ struct mxt_object {
 struct mxt_message {
 	u8 reportid;
 	u8 message[7];
-	u8 checksum;
 };
 
 struct mxt_finger {
@@ -326,17 +325,12 @@ static bool mxt_object_writable(unsigned int type)
 }
 
 static void mxt_dump_message(struct device *dev,
-				  struct mxt_message *message)
+			     struct mxt_message *message)
 {
-	dev_dbg(dev, "reportid:\t0x%x\n", message->reportid);
-	dev_dbg(dev, "message1:\t0x%x\n", message->message[0]);
-	dev_dbg(dev, "message2:\t0x%x\n", message->message[1]);
-	dev_dbg(dev, "message3:\t0x%x\n", message->message[2]);
-	dev_dbg(dev, "message4:\t0x%x\n", message->message[3]);
-	dev_dbg(dev, "message5:\t0x%x\n", message->message[4]);
-	dev_dbg(dev, "message6:\t0x%x\n", message->message[5]);
-	dev_dbg(dev, "message7:\t0x%x\n", message->message[6]);
-	dev_dbg(dev, "checksum:\t0x%x\n", message->checksum);
+	dev_dbg(dev, "reportid: %u\tmessage: %02x %02x %02x %02x %02x %02x %02x\n",
+		message->reportid, message->message[0], message->message[1],
+		message->message[2], message->message[3], message->message[4],
+		message->message[5], message->message[6]);
 }
 
 static int mxt_check_bootloader(struct i2c_client *client,
@@ -506,7 +500,7 @@ static int mxt_write_object(struct mxt_data *data,
 	u16 reg;
 
 	object = mxt_get_object(data, type);
-	if (!object)
+	if (!object || offset >= object->size + 1)
 		return -EINVAL;
 
 	reg = object->start_address;
@@ -1049,8 +1043,8 @@ static ssize_t mxt_update_fw_store(struct device *dev,
 	return count;
 }
 
-static DEVICE_ATTR(object, 0444, mxt_object_show, NULL);
-static DEVICE_ATTR(update_fw, 0664, NULL, mxt_update_fw_store);
+static DEVICE_ATTR(object, S_IRUGO, mxt_object_show, NULL);
+static DEVICE_ATTR(update_fw, S_IWUSR, NULL, mxt_update_fw_store);
 
 static struct attribute *mxt_attrs[] = {
 	&dev_attr_object.attr,
@@ -1201,7 +1195,7 @@ static int __devexit mxt_remove(struct i2c_client *client)
 	return 0;
 }
 
-#ifdef CONFIG_PM
+#ifdef CONFIG_PM_SLEEP
 static int mxt_suspend(struct device *dev)
 {
 	struct i2c_client *client = to_i2c_client(dev);
@@ -1239,13 +1233,10 @@ static int mxt_resume(struct device *dev)
 
 	return 0;
 }
-
-static const struct dev_pm_ops mxt_pm_ops = {
-	.suspend	= mxt_suspend,
-	.resume		= mxt_resume,
-};
 #endif
 
+static SIMPLE_DEV_PM_OPS(mxt_pm_ops, mxt_suspend, mxt_resume);
+
 static const struct i2c_device_id mxt_id[] = {
 	{ "qt602240_ts", 0 },
 	{ "atmel_mxt_ts", 0 },
@@ -1258,9 +1249,7 @@ static struct i2c_driver mxt_driver = {
 	.driver = {
 		.name	= "atmel_mxt_ts",
 		.owner	= THIS_MODULE,
-#ifdef CONFIG_PM
 		.pm	= &mxt_pm_ops,
-#endif
 	},
 	.probe		= mxt_probe,
 	.remove		= __devexit_p(mxt_remove),
diff --git a/drivers/input/touchscreen/da9052_tsi.c b/drivers/input/touchscreen/da9052_tsi.c
new file mode 100644
index 00000000000..e8df341090c
--- /dev/null
+++ b/drivers/input/touchscreen/da9052_tsi.c
@@ -0,0 +1,370 @@
+/*
+ * TSI driver for Dialog DA9052
+ *
+ * Copyright(c) 2012 Dialog Semiconductor Ltd.
+ *
+ * Author: David Dajun Chen <dchen@diasemi.com>
+ *
+ *  This program is free software; you can redistribute  it and/or modify it
+ *  under  the terms of  the GNU General  Public License as published by the
+ *  Free Software Foundation;  either version 2 of the  License, or (at your
+ *  option) any later version.
+ *
+ */
+#include <linux/module.h>
+#include <linux/input.h>
+#include <linux/delay.h>
+#include <linux/platform_device.h>
+#include <linux/interrupt.h>
+
+#include <linux/mfd/da9052/reg.h>
+#include <linux/mfd/da9052/da9052.h>
+
+#define TSI_PEN_DOWN_STATUS 0x40
+
+struct da9052_tsi {
+	struct da9052 *da9052;
+	struct input_dev *dev;
+	struct delayed_work ts_pen_work;
+	struct mutex mutex;
+	unsigned int irq_pendwn;
+	unsigned int irq_datardy;
+	bool stopped;
+	bool adc_on;
+};
+
+static void da9052_ts_adc_toggle(struct da9052_tsi *tsi, bool on)
+{
+	da9052_reg_update(tsi->da9052, DA9052_TSI_CONT_A_REG, 1 << 0, on);
+	tsi->adc_on = on;
+}
+
+static irqreturn_t da9052_ts_pendwn_irq(int irq, void *data)
+{
+	struct da9052_tsi *tsi = data;
+
+	if (!tsi->stopped) {
+		/* Mask PEN_DOWN event and unmask TSI_READY event */
+		disable_irq_nosync(tsi->irq_pendwn);
+		enable_irq(tsi->irq_datardy);
+
+		da9052_ts_adc_toggle(tsi, true);
+
+		schedule_delayed_work(&tsi->ts_pen_work, HZ / 50);
+	}
+
+	return IRQ_HANDLED;
+}
+
+static void da9052_ts_read(struct da9052_tsi *tsi)
+{
+	struct input_dev *input = tsi->dev;
+	int ret;
+	u16 x, y, z;
+	u8 v;
+
+	ret = da9052_reg_read(tsi->da9052, DA9052_TSI_X_MSB_REG);
+	if (ret < 0)
+		return;
+
+	x = (u16) ret;
+
+	ret = da9052_reg_read(tsi->da9052, DA9052_TSI_Y_MSB_REG);
+	if (ret < 0)
+		return;
+
+	y = (u16) ret;
+
+	ret = da9052_reg_read(tsi->da9052, DA9052_TSI_Z_MSB_REG);
+	if (ret < 0)
+		return;
+
+	z = (u16) ret;
+
+	ret = da9052_reg_read(tsi->da9052, DA9052_TSI_LSB_REG);
+	if (ret < 0)
+		return;
+
+	v = (u8) ret;
+
+	x = ((x << 2) & 0x3fc) | (v & 0x3);
+	y = ((y << 2) & 0x3fc) | ((v & 0xc) >> 2);
+	z = ((z << 2) & 0x3fc) | ((v & 0x30) >> 4);
+
+	input_report_key(input, BTN_TOUCH, 1);
+	input_report_abs(input, ABS_X, x);
+	input_report_abs(input, ABS_Y, y);
+	input_report_abs(input, ABS_PRESSURE, z);
+	input_sync(input);
+}
+
+static irqreturn_t da9052_ts_datardy_irq(int irq, void *data)
+{
+	struct da9052_tsi *tsi = data;
+
+	da9052_ts_read(tsi);
+
+	return IRQ_HANDLED;
+}
+
+static void da9052_ts_pen_work(struct work_struct *work)
+{
+	struct da9052_tsi *tsi = container_of(work, struct da9052_tsi,
+					      ts_pen_work.work);
+	if (!tsi->stopped) {
+		int ret = da9052_reg_read(tsi->da9052, DA9052_TSI_LSB_REG);
+		if (ret < 0 || (ret & TSI_PEN_DOWN_STATUS)) {
+			/* Pen is still DOWN (or read error) */
+			schedule_delayed_work(&tsi->ts_pen_work, HZ / 50);
+		} else {
+			struct input_dev *input = tsi->dev;
+
+			/* Pen UP */
+			da9052_ts_adc_toggle(tsi, false);
+
+			/* Report Pen UP */
+			input_report_key(input, BTN_TOUCH, 0);
+			input_report_abs(input, ABS_PRESSURE, 0);
+			input_sync(input);
+
+			/*
+			 * FIXME: Fixes the unhandled irq issue when quick
+			 * pen down and pen up events occurs
+			 */
+			ret = da9052_reg_update(tsi->da9052,
+						DA9052_EVENT_B_REG, 0xC0, 0xC0);
+			if (ret < 0)
+				return;
+
+			/* Mask TSI_READY event and unmask PEN_DOWN event */
+			disable_irq(tsi->irq_datardy);
+			enable_irq(tsi->irq_pendwn);
+		}
+	}
+}
+
+static int __devinit da9052_ts_configure_gpio(struct da9052 *da9052)
+{
+	int error;
+
+	error = da9052_reg_update(da9052, DA9052_GPIO_2_3_REG, 0x30, 0);
+	if (error < 0)
+		return error;
+
+	error = da9052_reg_update(da9052, DA9052_GPIO_4_5_REG, 0x33, 0);
+	if (error < 0)
+		return error;
+
+	error = da9052_reg_update(da9052, DA9052_GPIO_6_7_REG, 0x33, 0);
+	if (error < 0)
+		return error;
+
+	return 0;
+}
+
+static int __devinit da9052_configure_tsi(struct da9052_tsi *tsi)
+{
+	int error;
+
+	error = da9052_ts_configure_gpio(tsi->da9052);
+	if (error)
+		return error;
+
+	/* Measure TSI sample every 1ms */
+	error = da9052_reg_update(tsi->da9052, DA9052_ADC_CONT_REG,
+				  1 << 6, 1 << 6);
+	if (error < 0)
+		return error;
+
+	/* TSI_DELAY: 3 slots, TSI_SKIP: 0 slots, TSI_MODE: XYZP */
+	error = da9052_reg_update(tsi->da9052, DA9052_TSI_CONT_A_REG, 0xFC, 0xC0);
+	if (error < 0)
+		return error;
+
+	/* Supply TSIRef through LD09 */
+	error = da9052_reg_write(tsi->da9052, DA9052_LDO9_REG, 0x59);
+	if (error < 0)
+		return error;
+
+	return 0;
+}
+
+static int da9052_ts_input_open(struct input_dev *input_dev)
+{
+	struct da9052_tsi *tsi = input_get_drvdata(input_dev);
+
+	tsi->stopped = false;
+	mb();
+
+	/* Unmask PEN_DOWN event */
+	enable_irq(tsi->irq_pendwn);
+
+	/* Enable Pen Detect Circuit */
+	return da9052_reg_update(tsi->da9052, DA9052_TSI_CONT_A_REG,
+				 1 << 1, 1 << 1);
+}
+
+static void da9052_ts_input_close(struct input_dev *input_dev)
+{
+	struct da9052_tsi *tsi = input_get_drvdata(input_dev);
+
+	tsi->stopped = true;
+	mb();
+	disable_irq(tsi->irq_pendwn);
+	cancel_delayed_work_sync(&tsi->ts_pen_work);
+
+	if (tsi->adc_on) {
+		disable_irq(tsi->irq_datardy);
+		da9052_ts_adc_toggle(tsi, false);
+
+		/*
+		 * If ADC was on that means that pendwn IRQ was disabled
+		 * twice and we need to enable it to keep enable/disable
+		 * counter balanced. IRQ is still off though.
+		 */
+		enable_irq(tsi->irq_pendwn);
+	}
+
+	/* Disable Pen Detect Circuit */
+	da9052_reg_update(tsi->da9052, DA9052_TSI_CONT_A_REG, 1 << 1, 0);
+}
+
+static int __devinit da9052_ts_probe(struct platform_device *pdev)
+{
+	struct da9052 *da9052;
+	struct da9052_tsi *tsi;
+	struct input_dev *input_dev;
+	int irq_pendwn;
+	int irq_datardy;
+	int error;
+
+	da9052 = dev_get_drvdata(pdev->dev.parent);
+	if (!da9052)
+		return -EINVAL;
+
+	irq_pendwn = platform_get_irq_byname(pdev, "PENDWN");
+	irq_datardy = platform_get_irq_byname(pdev, "TSIRDY");
+	if (irq_pendwn < 0 || irq_datardy < 0) {
+		dev_err(da9052->dev, "Unable to determine device interrupts\n");
+		return -ENXIO;
+	}
+
+	tsi = kzalloc(sizeof(struct da9052_tsi), GFP_KERNEL);
+	input_dev = input_allocate_device();
+	if (!tsi || !input_dev) {
+		error = -ENOMEM;
+		goto err_free_mem;
+	}
+
+	tsi->da9052 = da9052;
+	tsi->dev = input_dev;
+	tsi->irq_pendwn = da9052->irq_base + irq_pendwn;
+	tsi->irq_datardy = da9052->irq_base + irq_datardy;
+	tsi->stopped = true;
+	INIT_DELAYED_WORK(&tsi->ts_pen_work, da9052_ts_pen_work);
+
+	input_dev->id.version = 0x0101;
+	input_dev->id.vendor = 0x15B6;
+	input_dev->id.product = 0x9052;
+	input_dev->name = "Dialog DA9052 TouchScreen Driver";
+	input_dev->dev.parent = &pdev->dev;
+	input_dev->open = da9052_ts_input_open;
+	input_dev->close = da9052_ts_input_close;
+
+	__set_bit(EV_ABS, input_dev->evbit);
+	__set_bit(EV_KEY, input_dev->evbit);
+	__set_bit(BTN_TOUCH, input_dev->keybit);
+
+	input_set_abs_params(input_dev, ABS_X, 0, 1023, 0, 0);
+	input_set_abs_params(input_dev, ABS_Y, 0, 1023, 0, 0);
+	input_set_abs_params(input_dev, ABS_PRESSURE, 0, 1023, 0, 0);
+
+	input_set_drvdata(input_dev, tsi);
+
+	/* Disable Pen Detect Circuit */
+	da9052_reg_update(tsi->da9052, DA9052_TSI_CONT_A_REG, 1 << 1, 0);
+
+	/* Disable ADC */
+	da9052_ts_adc_toggle(tsi, false);
+
+	error = request_threaded_irq(tsi->irq_pendwn,
+				     NULL, da9052_ts_pendwn_irq,
+				     IRQF_TRIGGER_LOW | IRQF_ONESHOT,
+				     "PENDWN", tsi);
+	if (error) {
+		dev_err(tsi->da9052->dev,
+			"Failed to register PENDWN IRQ %d, error = %d\n",
+			tsi->irq_pendwn, error);
+		goto err_free_mem;
+	}
+
+	error = request_threaded_irq(tsi->irq_datardy,
+				     NULL, da9052_ts_datardy_irq,
+				     IRQF_TRIGGER_LOW | IRQF_ONESHOT,
+				     "TSIRDY", tsi);
+	if (error) {
+		dev_err(tsi->da9052->dev,
+			"Failed to register TSIRDY IRQ %d, error = %d\n",
+			tsi->irq_datardy, error);
+		goto err_free_pendwn_irq;
+	}
+
+	/* Mask PEN_DOWN and TSI_READY events */
+	disable_irq(tsi->irq_pendwn);
+	disable_irq(tsi->irq_datardy);
+
+	error = da9052_configure_tsi(tsi);
+	if (error)
+		goto err_free_datardy_irq;
+
+	error = input_register_device(tsi->dev);
+	if (error)
+		goto err_free_datardy_irq;
+
+	platform_set_drvdata(pdev, tsi);
+
+	return 0;
+
+err_free_datardy_irq:
+	free_irq(tsi->irq_datardy, tsi);
+err_free_pendwn_irq:
+	free_irq(tsi->irq_pendwn, tsi);
+err_free_mem:
+	kfree(tsi);
+	input_free_device(input_dev);
+
+	return error;
+}
+
+static int  __devexit da9052_ts_remove(struct platform_device *pdev)
+{
+	struct da9052_tsi *tsi = platform_get_drvdata(pdev);
+
+	da9052_reg_write(tsi->da9052, DA9052_LDO9_REG, 0x19);
+
+	free_irq(tsi->irq_pendwn, tsi);
+	free_irq(tsi->irq_datardy, tsi);
+
+	input_unregister_device(tsi->dev);
+	kfree(tsi);
+
+	platform_set_drvdata(pdev, NULL);
+
+	return 0;
+}
+
+static struct platform_driver da9052_tsi_driver = {
+	.probe	= da9052_ts_probe,
+	.remove	= __devexit_p(da9052_ts_remove),
+	.driver	= {
+		.name	= "da9052-tsi",
+		.owner	= THIS_MODULE,
+	},
+};
+
+module_platform_driver(da9052_tsi_driver);
+
+MODULE_DESCRIPTION("Touchscreen driver for Dialog Semiconductor DA9052");
+MODULE_AUTHOR("Anthony Olech <Anthony.Olech@diasemi.com>");
+MODULE_LICENSE("GPL");
+MODULE_ALIAS("platform:da9052-tsi");
diff --git a/drivers/input/touchscreen/dynapro.c b/drivers/input/touchscreen/dynapro.c
index 455353908bd..1809677a651 100644
--- a/drivers/input/touchscreen/dynapro.c
+++ b/drivers/input/touchscreen/dynapro.c
@@ -188,19 +188,4 @@ static struct serio_driver dynapro_drv = {
 	.disconnect	= dynapro_disconnect,
 };
 
-/*
- * The functions for inserting/removing us as a module.
- */
-
-static int __init dynapro_init(void)
-{
-	return serio_register_driver(&dynapro_drv);
-}
-
-static void __exit dynapro_exit(void)
-{
-	serio_unregister_driver(&dynapro_drv);
-}
-
-module_init(dynapro_init);
-module_exit(dynapro_exit);
+module_serio_driver(dynapro_drv);
diff --git a/drivers/input/touchscreen/elo.c b/drivers/input/touchscreen/elo.c
index 486d31ba9c0..957423d1471 100644
--- a/drivers/input/touchscreen/elo.c
+++ b/drivers/input/touchscreen/elo.c
@@ -405,19 +405,4 @@ static struct serio_driver elo_drv = {
 	.disconnect	= elo_disconnect,
 };
 
-/*
- * The functions for inserting/removing us as a module.
- */
-
-static int __init elo_init(void)
-{
-	return serio_register_driver(&elo_drv);
-}
-
-static void __exit elo_exit(void)
-{
-	serio_unregister_driver(&elo_drv);
-}
-
-module_init(elo_init);
-module_exit(elo_exit);
+module_serio_driver(elo_drv);
diff --git a/drivers/input/touchscreen/fujitsu_ts.c b/drivers/input/touchscreen/fujitsu_ts.c
index 80b21800355..10794ddbdf5 100644
--- a/drivers/input/touchscreen/fujitsu_ts.c
+++ b/drivers/input/touchscreen/fujitsu_ts.c
@@ -175,15 +175,4 @@ static struct serio_driver fujitsu_drv = {
 	.disconnect	= fujitsu_disconnect,
 };
 
-static int __init fujitsu_init(void)
-{
-	return serio_register_driver(&fujitsu_drv);
-}
-
-static void __exit fujitsu_exit(void)
-{
-	serio_unregister_driver(&fujitsu_drv);
-}
-
-module_init(fujitsu_init);
-module_exit(fujitsu_exit);
+module_serio_driver(fujitsu_drv);
diff --git a/drivers/input/touchscreen/gunze.c b/drivers/input/touchscreen/gunze.c
index a54f90e02ab..41c71766bf1 100644
--- a/drivers/input/touchscreen/gunze.c
+++ b/drivers/input/touchscreen/gunze.c
@@ -186,19 +186,4 @@ static struct serio_driver gunze_drv = {
 	.disconnect	= gunze_disconnect,
 };
 
-/*
- * The functions for inserting/removing us as a module.
- */
-
-static int __init gunze_init(void)
-{
-	return serio_register_driver(&gunze_drv);
-}
-
-static void __exit gunze_exit(void)
-{
-	serio_unregister_driver(&gunze_drv);
-}
-
-module_init(gunze_init);
-module_exit(gunze_exit);
+module_serio_driver(gunze_drv);
diff --git a/drivers/input/touchscreen/h3600_ts_input.c b/drivers/input/touchscreen/h3600_ts_input.c
index 6107e563e68..b9e8686a6f1 100644
--- a/drivers/input/touchscreen/h3600_ts_input.c
+++ b/drivers/input/touchscreen/h3600_ts_input.c
@@ -476,19 +476,4 @@ static struct serio_driver h3600ts_drv = {
 	.disconnect	= h3600ts_disconnect,
 };
 
-/*
- * The functions for inserting/removing us as a module.
- */
-
-static int __init h3600ts_init(void)
-{
-	return serio_register_driver(&h3600ts_drv);
-}
-
-static void __exit h3600ts_exit(void)
-{
-	serio_unregister_driver(&h3600ts_drv);
-}
-
-module_init(h3600ts_init);
-module_exit(h3600ts_exit);
+module_serio_driver(h3600ts_drv);
diff --git a/drivers/input/touchscreen/hampshire.c b/drivers/input/touchscreen/hampshire.c
index 2da6cc31bb2..0cc47ea98ac 100644
--- a/drivers/input/touchscreen/hampshire.c
+++ b/drivers/input/touchscreen/hampshire.c
@@ -187,19 +187,4 @@ static struct serio_driver hampshire_drv = {
 	.disconnect	= hampshire_disconnect,
 };
 
-/*
- * The functions for inserting/removing us as a module.
- */
-
-static int __init hampshire_init(void)
-{
-	return serio_register_driver(&hampshire_drv);
-}
-
-static void __exit hampshire_exit(void)
-{
-	serio_unregister_driver(&hampshire_drv);
-}
-
-module_init(hampshire_init);
-module_exit(hampshire_exit);
+module_serio_driver(hampshire_drv);
diff --git a/drivers/input/touchscreen/inexio.c b/drivers/input/touchscreen/inexio.c
index 192ade0a0fb..a29c99c3224 100644
--- a/drivers/input/touchscreen/inexio.c
+++ b/drivers/input/touchscreen/inexio.c
@@ -189,19 +189,4 @@ static struct serio_driver inexio_drv = {
 	.disconnect	= inexio_disconnect,
 };
 
-/*
- * The functions for inserting/removing us as a module.
- */
-
-static int __init inexio_init(void)
-{
-	return serio_register_driver(&inexio_drv);
-}
-
-static void __exit inexio_exit(void)
-{
-	serio_unregister_driver(&inexio_drv);
-}
-
-module_init(inexio_init);
-module_exit(inexio_exit);
+module_serio_driver(inexio_drv);
diff --git a/drivers/input/touchscreen/lpc32xx_ts.c b/drivers/input/touchscreen/lpc32xx_ts.c
index afcd0691ec6..4c2b8ed3bf1 100644
--- a/drivers/input/touchscreen/lpc32xx_ts.c
+++ b/drivers/input/touchscreen/lpc32xx_ts.c
@@ -22,6 +22,7 @@
 #include <linux/clk.h>
 #include <linux/io.h>
 #include <linux/slab.h>
+#include <linux/of.h>
 
 /*
  * Touchscreen controller register offsets
@@ -383,6 +384,14 @@ static const struct dev_pm_ops lpc32xx_ts_pm_ops = {
 #define LPC32XX_TS_PM_OPS NULL
 #endif
 
+#ifdef CONFIG_OF
+static struct of_device_id lpc32xx_tsc_of_match[] = {
+	{ .compatible = "nxp,lpc3220-tsc", },
+	{ },
+};
+MODULE_DEVICE_TABLE(of, lpc32xx_tsc_of_match);
+#endif
+
 static struct platform_driver lpc32xx_ts_driver = {
 	.probe		= lpc32xx_ts_probe,
 	.remove		= __devexit_p(lpc32xx_ts_remove),
@@ -390,6 +399,7 @@ static struct platform_driver lpc32xx_ts_driver = {
 		.name	= MOD_NAME,
 		.owner	= THIS_MODULE,
 		.pm	= LPC32XX_TS_PM_OPS,
+		.of_match_table = of_match_ptr(lpc32xx_tsc_of_match),
 	},
 };
 module_platform_driver(lpc32xx_ts_driver);
diff --git a/drivers/input/touchscreen/mtouch.c b/drivers/input/touchscreen/mtouch.c
index 9077228418b..eb66b7c37c2 100644
--- a/drivers/input/touchscreen/mtouch.c
+++ b/drivers/input/touchscreen/mtouch.c
@@ -202,19 +202,4 @@ static struct serio_driver mtouch_drv = {
 	.disconnect	= mtouch_disconnect,
 };
 
-/*
- * The functions for inserting/removing us as a module.
- */
-
-static int __init mtouch_init(void)
-{
-	return serio_register_driver(&mtouch_drv);
-}
-
-static void __exit mtouch_exit(void)
-{
-	serio_unregister_driver(&mtouch_drv);
-}
-
-module_init(mtouch_init);
-module_exit(mtouch_exit);
+module_serio_driver(mtouch_drv);
diff --git a/drivers/input/touchscreen/penmount.c b/drivers/input/touchscreen/penmount.c
index 4c012fb2b01..4ccde45b9da 100644
--- a/drivers/input/touchscreen/penmount.c
+++ b/drivers/input/touchscreen/penmount.c
@@ -317,19 +317,4 @@ static struct serio_driver pm_drv = {
 	.disconnect	= pm_disconnect,
 };
 
-/*
- * The functions for inserting/removing us as a module.
- */
-
-static int __init pm_init(void)
-{
-	return serio_register_driver(&pm_drv);
-}
-
-static void __exit pm_exit(void)
-{
-	serio_unregister_driver(&pm_drv);
-}
-
-module_init(pm_init);
-module_exit(pm_exit);
+module_serio_driver(pm_drv);
diff --git a/drivers/input/touchscreen/st1232.c b/drivers/input/touchscreen/st1232.c
index cbbf71b2269..6cb68a1981b 100644
--- a/drivers/input/touchscreen/st1232.c
+++ b/drivers/input/touchscreen/st1232.c
@@ -218,7 +218,7 @@ static int __devexit st1232_ts_remove(struct i2c_client *client)
 	return 0;
 }
 
-#ifdef CONFIG_PM
+#ifdef CONFIG_PM_SLEEP
 static int st1232_ts_suspend(struct device *dev)
 {
 	struct i2c_client *client = to_i2c_client(dev);
@@ -243,18 +243,25 @@ static int st1232_ts_resume(struct device *dev)
 	return 0;
 }
 
-static const struct dev_pm_ops st1232_ts_pm_ops = {
-	.suspend	= st1232_ts_suspend,
-	.resume		= st1232_ts_resume,
-};
 #endif
 
+static SIMPLE_DEV_PM_OPS(st1232_ts_pm_ops,
+			 st1232_ts_suspend, st1232_ts_resume);
+
 static const struct i2c_device_id st1232_ts_id[] = {
 	{ ST1232_TS_NAME, 0 },
 	{ }
 };
 MODULE_DEVICE_TABLE(i2c, st1232_ts_id);
 
+#ifdef CONFIG_OF
+static const struct of_device_id st1232_ts_dt_ids[] __devinitconst = {
+	{ .compatible = "sitronix,st1232", },
+	{ }
+};
+MODULE_DEVICE_TABLE(of, st1232_ts_dt_ids);
+#endif
+
 static struct i2c_driver st1232_ts_driver = {
 	.probe		= st1232_ts_probe,
 	.remove		= __devexit_p(st1232_ts_remove),
@@ -262,9 +269,8 @@ static struct i2c_driver st1232_ts_driver = {
 	.driver = {
 		.name	= ST1232_TS_NAME,
 		.owner	= THIS_MODULE,
-#ifdef CONFIG_PM
+		.of_match_table = of_match_ptr(st1232_ts_dt_ids),
 		.pm	= &st1232_ts_pm_ops,
-#endif
 	},
 };
 
diff --git a/drivers/input/touchscreen/touchit213.c b/drivers/input/touchscreen/touchit213.c
index d1297ba19da..5f29e5b8e1c 100644
--- a/drivers/input/touchscreen/touchit213.c
+++ b/drivers/input/touchscreen/touchit213.c
@@ -216,19 +216,4 @@ static struct serio_driver touchit213_drv = {
 	.disconnect	= touchit213_disconnect,
 };
 
-/*
- * The functions for inserting/removing us as a module.
- */
-
-static int __init touchit213_init(void)
-{
-	return serio_register_driver(&touchit213_drv);
-}
-
-static void __exit touchit213_exit(void)
-{
-	serio_unregister_driver(&touchit213_drv);
-}
-
-module_init(touchit213_init);
-module_exit(touchit213_exit);
+module_serio_driver(touchit213_drv);
diff --git a/drivers/input/touchscreen/touchright.c b/drivers/input/touchscreen/touchright.c
index 3a5c142c2a7..8a2887daf19 100644
--- a/drivers/input/touchscreen/touchright.c
+++ b/drivers/input/touchscreen/touchright.c
@@ -176,19 +176,4 @@ static struct serio_driver tr_drv = {
 	.disconnect	= tr_disconnect,
 };
 
-/*
- * The functions for inserting/removing us as a module.
- */
-
-static int __init tr_init(void)
-{
-	return serio_register_driver(&tr_drv);
-}
-
-static void __exit tr_exit(void)
-{
-	serio_unregister_driver(&tr_drv);
-}
-
-module_init(tr_init);
-module_exit(tr_exit);
+module_serio_driver(tr_drv);
diff --git a/drivers/input/touchscreen/touchwin.c b/drivers/input/touchscreen/touchwin.c
index 763a656a59f..588cdcb839d 100644
--- a/drivers/input/touchscreen/touchwin.c
+++ b/drivers/input/touchscreen/touchwin.c
@@ -183,19 +183,4 @@ static struct serio_driver tw_drv = {
 	.disconnect	= tw_disconnect,
 };
 
-/*
- * The functions for inserting/removing us as a module.
- */
-
-static int __init tw_init(void)
-{
-	return serio_register_driver(&tw_drv);
-}
-
-static void __exit tw_exit(void)
-{
-	serio_unregister_driver(&tw_drv);
-}
-
-module_init(tw_init);
-module_exit(tw_exit);
+module_serio_driver(tw_drv);
diff --git a/drivers/input/touchscreen/tsc40.c b/drivers/input/touchscreen/tsc40.c
index 29d5ed4dd31..63209aaa55f 100644
--- a/drivers/input/touchscreen/tsc40.c
+++ b/drivers/input/touchscreen/tsc40.c
@@ -167,17 +167,7 @@ static struct serio_driver tsc_drv = {
 	.disconnect     = tsc_disconnect,
 };
 
-static int __init tsc_ser_init(void)
-{
-	return serio_register_driver(&tsc_drv);
-}
-module_init(tsc_ser_init);
-
-static void __exit tsc_exit(void)
-{
-	serio_unregister_driver(&tsc_drv);
-}
-module_exit(tsc_exit);
+module_serio_driver(tsc_drv);
 
 MODULE_AUTHOR("Sebastian Andrzej Siewior <bigeasy@linutronix.de>");
 MODULE_DESCRIPTION(DRIVER_DESC);
diff --git a/drivers/input/touchscreen/usbtouchscreen.c b/drivers/input/touchscreen/usbtouchscreen.c
index 22cd96f58c9..e32709e0dd6 100644
--- a/drivers/input/touchscreen/usbtouchscreen.c
+++ b/drivers/input/touchscreen/usbtouchscreen.c
@@ -269,8 +269,9 @@ static int e2i_init(struct usbtouch_usb *usbtouch)
 	                      0x01, 0x02, 0x0000, 0x0081,
 	                      NULL, 0, USB_CTRL_SET_TIMEOUT);
 
-	dbg("%s - usb_control_msg - E2I_RESET - bytes|err: %d",
-	    __func__, ret);
+	dev_dbg(&usbtouch->interface->dev,
+		"%s - usb_control_msg - E2I_RESET - bytes|err: %d\n",
+		__func__, ret);
 	return ret;
 }
 
@@ -425,8 +426,9 @@ static int mtouch_init(struct usbtouch_usb *usbtouch)
 	                      MTOUCHUSB_RESET,
 	                      USB_DIR_OUT | USB_TYPE_VENDOR | USB_RECIP_DEVICE,
 	                      1, 0, NULL, 0, USB_CTRL_SET_TIMEOUT);
-	dbg("%s - usb_control_msg - MTOUCHUSB_RESET - bytes|err: %d",
-	    __func__, ret);
+	dev_dbg(&usbtouch->interface->dev,
+		"%s - usb_control_msg - MTOUCHUSB_RESET - bytes|err: %d\n",
+		__func__, ret);
 	if (ret < 0)
 		return ret;
 	msleep(150);
@@ -436,8 +438,9 @@ static int mtouch_init(struct usbtouch_usb *usbtouch)
 				      MTOUCHUSB_ASYNC_REPORT,
 				      USB_DIR_OUT | USB_TYPE_VENDOR | USB_RECIP_DEVICE,
 				      1, 1, NULL, 0, USB_CTRL_SET_TIMEOUT);
-		dbg("%s - usb_control_msg - MTOUCHUSB_ASYNC_REPORT - bytes|err: %d",
-		    __func__, ret);
+		dev_dbg(&usbtouch->interface->dev,
+			"%s - usb_control_msg - MTOUCHUSB_ASYNC_REPORT - bytes|err: %d\n",
+			__func__, ret);
 		if (ret >= 0)
 			break;
 		if (ret != -EPIPE)
@@ -737,27 +740,29 @@ static int jastec_read_data(struct usbtouch_usb *dev, unsigned char *pkt)
 #ifdef CONFIG_TOUCHSCREEN_USB_ZYTRONIC
 static int zytronic_read_data(struct usbtouch_usb *dev, unsigned char *pkt)
 {
+	struct usb_interface *intf = dev->interface;
+
 	switch (pkt[0]) {
 	case 0x3A: /* command response */
-		dbg("%s: Command response %d", __func__, pkt[1]);
+		dev_dbg(&intf->dev, "%s: Command response %d\n", __func__, pkt[1]);
 		break;
 
 	case 0xC0: /* down */
 		dev->x = (pkt[1] & 0x7f) | ((pkt[2] & 0x07) << 7);
 		dev->y = (pkt[3] & 0x7f) | ((pkt[4] & 0x07) << 7);
 		dev->touch = 1;
-		dbg("%s: down %d,%d", __func__, dev->x, dev->y);
+		dev_dbg(&intf->dev, "%s: down %d,%d\n", __func__, dev->x, dev->y);
 		return 1;
 
 	case 0x80: /* up */
 		dev->x = (pkt[1] & 0x7f) | ((pkt[2] & 0x07) << 7);
 		dev->y = (pkt[3] & 0x7f) | ((pkt[4] & 0x07) << 7);
 		dev->touch = 0;
-		dbg("%s: up %d,%d", __func__, dev->x, dev->y);
+		dev_dbg(&intf->dev, "%s: up %d,%d\n", __func__, dev->x, dev->y);
 		return 1;
 
 	default:
-		dbg("%s: Unknown return %d", __func__, pkt[0]);
+		dev_dbg(&intf->dev, "%s: Unknown return %d\n", __func__, pkt[0]);
 		break;
 	}
 
@@ -812,7 +817,8 @@ static int nexio_alloc(struct usbtouch_usb *usbtouch)
 
 	priv->ack = usb_alloc_urb(0, GFP_KERNEL);
 	if (!priv->ack) {
-		dbg("%s - usb_alloc_urb failed: usbtouch->ack", __func__);
+		dev_dbg(&usbtouch->interface->dev,
+			"%s - usb_alloc_urb failed: usbtouch->ack\n", __func__);
 		goto err_ack_buf;
 	}
 
@@ -1349,6 +1355,7 @@ out_flush_buf:
 static void usbtouch_irq(struct urb *urb)
 {
 	struct usbtouch_usb *usbtouch = urb->context;
+	struct device *dev = &usbtouch->interface->dev;
 	int retval;
 
 	switch (urb->status) {
@@ -1357,20 +1364,21 @@ static void usbtouch_irq(struct urb *urb)
 		break;
 	case -ETIME:
 		/* this urb is timing out */
-		dbg("%s - urb timed out - was the device unplugged?",
-		    __func__);
+		dev_dbg(dev,
+			"%s - urb timed out - was the device unplugged?\n",
+			__func__);
 		return;
 	case -ECONNRESET:
 	case -ENOENT:
 	case -ESHUTDOWN:
 	case -EPIPE:
 		/* this urb is terminated, clean up */
-		dbg("%s - urb shutting down with status: %d",
-		    __func__, urb->status);
+		dev_dbg(dev, "%s - urb shutting down with status: %d\n",
+			__func__, urb->status);
 		return;
 	default:
-		dbg("%s - nonzero urb status received: %d",
-		    __func__, urb->status);
+		dev_dbg(dev, "%s - nonzero urb status received: %d\n",
+			__func__, urb->status);
 		goto exit;
 	}
 
@@ -1380,8 +1388,8 @@ exit:
 	usb_mark_last_busy(interface_to_usbdev(usbtouch->interface));
 	retval = usb_submit_urb(urb, GFP_ATOMIC);
 	if (retval)
-		err("%s - usb_submit_urb failed with result: %d",
-		    __func__, retval);
+		dev_err(dev, "%s - usb_submit_urb failed with result: %d\n",
+			__func__, retval);
 }
 
 static int usbtouch_open(struct input_dev *input)
@@ -1456,8 +1464,9 @@ static int usbtouch_reset_resume(struct usb_interface *intf)
 	if (usbtouch->type->init) {
 		err = usbtouch->type->init(usbtouch);
 		if (err) {
-			dbg("%s - type->init() failed, err: %d",
-			    __func__, err);
+			dev_dbg(&intf->dev,
+				"%s - type->init() failed, err: %d\n",
+				__func__, err);
 			return err;
 		}
 	}
@@ -1532,7 +1541,8 @@ static int usbtouch_probe(struct usb_interface *intf,
 
 	usbtouch->irq = usb_alloc_urb(0, GFP_KERNEL);
 	if (!usbtouch->irq) {
-		dbg("%s - usb_alloc_urb failed: usbtouch->irq", __func__);
+		dev_dbg(&intf->dev,
+			"%s - usb_alloc_urb failed: usbtouch->irq\n", __func__);
 		goto out_free_buffers;
 	}
 
@@ -1594,7 +1604,9 @@ static int usbtouch_probe(struct usb_interface *intf,
 	if (type->alloc) {
 		err = type->alloc(usbtouch);
 		if (err) {
-			dbg("%s - type->alloc() failed, err: %d", __func__, err);
+			dev_dbg(&intf->dev,
+				"%s - type->alloc() failed, err: %d\n",
+				__func__, err);
 			goto out_free_urb;
 		}
 	}
@@ -1603,14 +1615,18 @@ static int usbtouch_probe(struct usb_interface *intf,
 	if (type->init) {
 		err = type->init(usbtouch);
 		if (err) {
-			dbg("%s - type->init() failed, err: %d", __func__, err);
+			dev_dbg(&intf->dev,
+				"%s - type->init() failed, err: %d\n",
+				__func__, err);
 			goto out_do_exit;
 		}
 	}
 
 	err = input_register_device(usbtouch->input);
 	if (err) {
-		dbg("%s - input_register_device failed, err: %d", __func__, err);
+		dev_dbg(&intf->dev,
+			"%s - input_register_device failed, err: %d\n",
+			__func__, err);
 		goto out_do_exit;
 	}
 
@@ -1622,8 +1638,9 @@ static int usbtouch_probe(struct usb_interface *intf,
 		err = usb_submit_urb(usbtouch->irq, GFP_KERNEL);
 		if (err) {
 			usb_autopm_put_interface(intf);
-			err("%s - usb_submit_urb failed with result: %d",
-			    __func__, err);
+			dev_err(&intf->dev,
+				"%s - usb_submit_urb failed with result: %d\n",
+				__func__, err);
 			goto out_unregister_input;
 		}
 	}
@@ -1650,12 +1667,12 @@ static void usbtouch_disconnect(struct usb_interface *intf)
 {
 	struct usbtouch_usb *usbtouch = usb_get_intfdata(intf);
 
-	dbg("%s - called", __func__);
-
 	if (!usbtouch)
 		return;
 
-	dbg("%s - usbtouch is initialized, cleaning up", __func__);
+	dev_dbg(&intf->dev,
+		"%s - usbtouch is initialized, cleaning up\n", __func__);
+
 	usb_set_intfdata(intf, NULL);
 	/* this will stop IO via close */
 	input_unregister_device(usbtouch->input);
diff --git a/drivers/input/touchscreen/wacom_i2c.c b/drivers/input/touchscreen/wacom_i2c.c
new file mode 100644
index 00000000000..35572575d34
--- /dev/null
+++ b/drivers/input/touchscreen/wacom_i2c.c
@@ -0,0 +1,282 @@
+/*
+ * Wacom Penabled Driver for I2C
+ *
+ * Copyright (c) 2011 Tatsunosuke Tobita, Wacom.
+ * <tobita.tatsunosuke@wacom.co.jp>
+ *
+ * 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 of 2 of the License,
+ * or (at your option) any later version.
+ */
+
+#include <linux/module.h>
+#include <linux/input.h>
+#include <linux/i2c.h>
+#include <linux/slab.h>
+#include <linux/irq.h>
+#include <linux/interrupt.h>
+#include <linux/gpio.h>
+#include <asm/unaligned.h>
+
+#define WACOM_CMD_QUERY0	0x04
+#define WACOM_CMD_QUERY1	0x00
+#define WACOM_CMD_QUERY2	0x33
+#define WACOM_CMD_QUERY3	0x02
+#define WACOM_CMD_THROW0	0x05
+#define WACOM_CMD_THROW1	0x00
+#define WACOM_QUERY_SIZE	19
+#define WACOM_RETRY_CNT		100
+
+struct wacom_features {
+	int x_max;
+	int y_max;
+	int pressure_max;
+	char fw_version;
+};
+
+struct wacom_i2c {
+	struct i2c_client *client;
+	struct input_dev *input;
+	u8 data[WACOM_QUERY_SIZE];
+};
+
+static int wacom_query_device(struct i2c_client *client,
+			      struct wacom_features *features)
+{
+	int ret;
+	u8 cmd1[] = { WACOM_CMD_QUERY0, WACOM_CMD_QUERY1,
+			WACOM_CMD_QUERY2, WACOM_CMD_QUERY3 };
+	u8 cmd2[] = { WACOM_CMD_THROW0, WACOM_CMD_THROW1 };
+	u8 data[WACOM_QUERY_SIZE];
+	struct i2c_msg msgs[] = {
+		{
+			.addr = client->addr,
+			.flags = 0,
+			.len = sizeof(cmd1),
+			.buf = cmd1,
+		},
+		{
+			.addr = client->addr,
+			.flags = 0,
+			.len = sizeof(cmd2),
+			.buf = cmd2,
+		},
+		{
+			.addr = client->addr,
+			.flags = I2C_M_RD,
+			.len = sizeof(data),
+			.buf = data,
+		},
+	};
+
+	ret = i2c_transfer(client->adapter, msgs, ARRAY_SIZE(msgs));
+	if (ret < 0)
+		return ret;
+	if (ret != ARRAY_SIZE(msgs))
+		return -EIO;
+
+	features->x_max = get_unaligned_le16(&data[3]);
+	features->y_max = get_unaligned_le16(&data[5]);
+	features->pressure_max = get_unaligned_le16(&data[11]);
+	features->fw_version = get_unaligned_le16(&data[13]);
+
+	dev_dbg(&client->dev,
+		"x_max:%d, y_max:%d, pressure:%d, fw:%d\n",
+		features->x_max, features->y_max,
+		features->pressure_max, features->fw_version);
+
+	return 0;
+}
+
+static irqreturn_t wacom_i2c_irq(int irq, void *dev_id)
+{
+	struct wacom_i2c *wac_i2c = dev_id;
+	struct input_dev *input = wac_i2c->input;
+	u8 *data = wac_i2c->data;
+	unsigned int x, y, pressure;
+	unsigned char tsw, f1, f2, ers;
+	int error;
+
+	error = i2c_master_recv(wac_i2c->client,
+				wac_i2c->data, sizeof(wac_i2c->data));
+	if (error < 0)
+		goto out;
+
+	tsw = data[3] & 0x01;
+	ers = data[3] & 0x04;
+	f1 = data[3] & 0x02;
+	f2 = data[3] & 0x10;
+	x = le16_to_cpup((__le16 *)&data[4]);
+	y = le16_to_cpup((__le16 *)&data[6]);
+	pressure = le16_to_cpup((__le16 *)&data[8]);
+
+	input_report_key(input, BTN_TOUCH, tsw || ers);
+	input_report_key(input, BTN_TOOL_PEN, tsw);
+	input_report_key(input, BTN_TOOL_RUBBER, ers);
+	input_report_key(input, BTN_STYLUS, f1);
+	input_report_key(input, BTN_STYLUS2, f2);
+	input_report_abs(input, ABS_X, x);
+	input_report_abs(input, ABS_Y, y);
+	input_report_abs(input, ABS_PRESSURE, pressure);
+	input_sync(input);
+
+out:
+	return IRQ_HANDLED;
+}
+
+static int wacom_i2c_open(struct input_dev *dev)
+{
+	struct wacom_i2c *wac_i2c = input_get_drvdata(dev);
+	struct i2c_client *client = wac_i2c->client;
+
+	enable_irq(client->irq);
+
+	return 0;
+}
+
+static void wacom_i2c_close(struct input_dev *dev)
+{
+	struct wacom_i2c *wac_i2c = input_get_drvdata(dev);
+	struct i2c_client *client = wac_i2c->client;
+
+	disable_irq(client->irq);
+}
+
+static int __devinit wacom_i2c_probe(struct i2c_client *client,
+				     const struct i2c_device_id *id)
+{
+	struct wacom_i2c *wac_i2c;
+	struct input_dev *input;
+	struct wacom_features features;
+	int error;
+
+	if (!i2c_check_functionality(client->adapter, I2C_FUNC_I2C)) {
+		dev_err(&client->dev, "i2c_check_functionality error\n");
+		return -EIO;
+	}
+
+	error = wacom_query_device(client, &features);
+	if (error)
+		return error;
+
+	wac_i2c = kzalloc(sizeof(*wac_i2c), GFP_KERNEL);
+	input = input_allocate_device();
+	if (!wac_i2c || !input) {
+		error = -ENOMEM;
+		goto err_free_mem;
+	}
+
+	wac_i2c->client = client;
+	wac_i2c->input = input;
+
+	input->name = "Wacom I2C Digitizer";
+	input->id.bustype = BUS_I2C;
+	input->id.vendor = 0x56a;
+	input->id.version = features.fw_version;
+	input->dev.parent = &client->dev;
+	input->open = wacom_i2c_open;
+	input->close = wacom_i2c_close;
+
+	input->evbit[0] |= BIT_MASK(EV_KEY) | BIT_MASK(EV_ABS);
+
+	__set_bit(BTN_TOOL_PEN, input->keybit);
+	__set_bit(BTN_TOOL_RUBBER, input->keybit);
+	__set_bit(BTN_STYLUS, input->keybit);
+	__set_bit(BTN_STYLUS2, input->keybit);
+	__set_bit(BTN_TOUCH, input->keybit);
+
+	input_set_abs_params(input, ABS_X, 0, features.x_max, 0, 0);
+	input_set_abs_params(input, ABS_Y, 0, features.y_max, 0, 0);
+	input_set_abs_params(input, ABS_PRESSURE,
+			     0, features.pressure_max, 0, 0);
+
+	input_set_drvdata(input, wac_i2c);
+
+	error = request_threaded_irq(client->irq, NULL, wacom_i2c_irq,
+				     IRQF_TRIGGER_LOW | IRQF_ONESHOT,
+				     "wacom_i2c", wac_i2c);
+	if (error) {
+		dev_err(&client->dev,
+			"Failed to enable IRQ, error: %d\n", error);
+		goto err_free_mem;
+	}
+
+	/* Disable the IRQ, we'll enable it in wac_i2c_open() */
+	disable_irq(client->irq);
+
+	error = input_register_device(wac_i2c->input);
+	if (error) {
+		dev_err(&client->dev,
+			"Failed to register input device, error: %d\n", error);
+		goto err_free_irq;
+	}
+
+	i2c_set_clientdata(client, wac_i2c);
+	return 0;
+
+err_free_irq:
+	free_irq(client->irq, wac_i2c);
+err_free_mem:
+	input_free_device(input);
+	kfree(wac_i2c);
+
+	return error;
+}
+
+static int __devexit wacom_i2c_remove(struct i2c_client *client)
+{
+	struct wacom_i2c *wac_i2c = i2c_get_clientdata(client);
+
+	free_irq(client->irq, wac_i2c);
+	input_unregister_device(wac_i2c->input);
+	kfree(wac_i2c);
+
+	return 0;
+}
+
+#ifdef CONFIG_PM_SLEEP
+static int wacom_i2c_suspend(struct device *dev)
+{
+	struct i2c_client *client = to_i2c_client(dev);
+
+	disable_irq(client->irq);
+
+	return 0;
+}
+
+static int wacom_i2c_resume(struct device *dev)
+{
+	struct i2c_client *client = to_i2c_client(dev);
+
+	enable_irq(client->irq);
+
+	return 0;
+}
+#endif
+
+static SIMPLE_DEV_PM_OPS(wacom_i2c_pm, wacom_i2c_suspend, wacom_i2c_resume);
+
+static const struct i2c_device_id wacom_i2c_id[] = {
+	{ "WAC_I2C_EMR", 0 },
+	{ },
+};
+MODULE_DEVICE_TABLE(i2c, wacom_i2c_id);
+
+static struct i2c_driver wacom_i2c_driver = {
+	.driver	= {
+		.name	= "wacom_i2c",
+		.owner	= THIS_MODULE,
+		.pm	= &wacom_i2c_pm,
+	},
+
+	.probe		= wacom_i2c_probe,
+	.remove		= __devexit_p(wacom_i2c_remove),
+	.id_table	= wacom_i2c_id,
+};
+module_i2c_driver(wacom_i2c_driver);
+
+MODULE_AUTHOR("Tatsunosuke Tobita <tobita.tatsunosuke@wacom.co.jp>");
+MODULE_DESCRIPTION("WACOM EMR I2C Driver");
+MODULE_LICENSE("GPL");
diff --git a/drivers/input/touchscreen/wacom_w8001.c b/drivers/input/touchscreen/wacom_w8001.c
index 1569a3934ab..8f9ad2f893b 100644
--- a/drivers/input/touchscreen/wacom_w8001.c
+++ b/drivers/input/touchscreen/wacom_w8001.c
@@ -594,15 +594,4 @@ static struct serio_driver w8001_drv = {
 	.disconnect	= w8001_disconnect,
 };
 
-static int __init w8001_init(void)
-{
-	return serio_register_driver(&w8001_drv);
-}
-
-static void __exit w8001_exit(void)
-{
-	serio_unregister_driver(&w8001_drv);
-}
-
-module_init(w8001_init);
-module_exit(w8001_exit);
+module_serio_driver(w8001_drv);
diff --git a/drivers/input/touchscreen/wm831x-ts.c b/drivers/input/touchscreen/wm831x-ts.c
index 4bc851a9dc3..e83410721e3 100644
--- a/drivers/input/touchscreen/wm831x-ts.c
+++ b/drivers/input/touchscreen/wm831x-ts.c
@@ -260,15 +260,16 @@ static __devinit int wm831x_ts_probe(struct platform_device *pdev)
 	 * If we have a direct IRQ use it, otherwise use the interrupt
 	 * from the WM831x IRQ controller.
 	 */
+	wm831x_ts->data_irq = wm831x_irq(wm831x,
+					 platform_get_irq_byname(pdev,
+								 "TCHDATA"));
 	if (pdata && pdata->data_irq)
 		wm831x_ts->data_irq = pdata->data_irq;
-	else
-		wm831x_ts->data_irq = platform_get_irq_byname(pdev, "TCHDATA");
 
+	wm831x_ts->pd_irq = wm831x_irq(wm831x,
+				       platform_get_irq_byname(pdev, "TCHPD"));
 	if (pdata && pdata->pd_irq)
 		wm831x_ts->pd_irq = pdata->pd_irq;
-	else
-		wm831x_ts->pd_irq = platform_get_irq_byname(pdev, "TCHPD");
 
 	if (pdata)
 		wm831x_ts->pressure = pdata->pressure;
diff --git a/drivers/iommu/Kconfig b/drivers/iommu/Kconfig
index 3bd9fff5c58..34089372753 100644
--- a/drivers/iommu/Kconfig
+++ b/drivers/iommu/Kconfig
@@ -43,7 +43,7 @@ config AMD_IOMMU
 	  With this option you can enable support for AMD IOMMU hardware in
 	  your system. An IOMMU is a hardware component which provides
 	  remapping of DMA memory accesses from devices. With an AMD IOMMU you
-	  can isolate the the DMA memory of different devices and protect the
+	  can isolate the DMA memory of different devices and protect the
 	  system from misbehaving device drivers or hardware.
 
 	  You can find out if your system has an AMD IOMMU if you look into
@@ -67,7 +67,7 @@ config AMD_IOMMU_V2
 	---help---
 	  This option enables support for the AMD IOMMUv2 features of the IOMMU
 	  hardware. Select this option if you want to use devices that support
-	  the the PCI PRI and PASID interface.
+	  the PCI PRI and PASID interface.
 
 # Intel IOMMU support
 config DMAR_TABLE
@@ -162,4 +162,25 @@ config TEGRA_IOMMU_SMMU
 	  space through the SMMU (System Memory Management Unit)
 	  hardware included on Tegra SoCs.
 
+config EXYNOS_IOMMU
+	bool "Exynos IOMMU Support"
+	depends on ARCH_EXYNOS && EXYNOS_DEV_SYSMMU
+	select IOMMU_API
+	help
+	  Support for the IOMMU(System MMU) of Samsung Exynos application
+	  processor family. This enables H/W multimedia accellerators to see
+	  non-linear physical memory chunks as a linear memory in their
+	  address spaces
+
+	  If unsure, say N here.
+
+config EXYNOS_IOMMU_DEBUG
+	bool "Debugging log for Exynos IOMMU"
+	depends on EXYNOS_IOMMU
+	help
+	  Select this to see the detailed log message that shows what
+	  happens in the IOMMU driver
+
+	  Say N unless you need kernel log message for IOMMU debugging
+
 endif # IOMMU_SUPPORT
diff --git a/drivers/iommu/Makefile b/drivers/iommu/Makefile
index 7ad7a3bc124..76e54ef796d 100644
--- a/drivers/iommu/Makefile
+++ b/drivers/iommu/Makefile
@@ -4,9 +4,10 @@ obj-$(CONFIG_AMD_IOMMU) += amd_iommu.o amd_iommu_init.o
 obj-$(CONFIG_AMD_IOMMU_V2) += amd_iommu_v2.o
 obj-$(CONFIG_DMAR_TABLE) += dmar.o
 obj-$(CONFIG_INTEL_IOMMU) += iova.o intel-iommu.o
-obj-$(CONFIG_IRQ_REMAP) += intr_remapping.o
+obj-$(CONFIG_IRQ_REMAP) += intel_irq_remapping.o irq_remapping.o
 obj-$(CONFIG_OMAP_IOMMU) += omap-iommu.o
 obj-$(CONFIG_OMAP_IOVMM) += omap-iovmm.o
 obj-$(CONFIG_OMAP_IOMMU_DEBUG) += omap-iommu-debug.o
 obj-$(CONFIG_TEGRA_IOMMU_GART) += tegra-gart.o
 obj-$(CONFIG_TEGRA_IOMMU_SMMU) += tegra-smmu.o
+obj-$(CONFIG_EXYNOS_IOMMU) += exynos-iommu.o
diff --git a/drivers/iommu/dmar.c b/drivers/iommu/dmar.c
index 35c1e17fce1..3a74e4410fc 100644
--- a/drivers/iommu/dmar.c
+++ b/drivers/iommu/dmar.c
@@ -36,6 +36,7 @@
 #include <linux/tboot.h>
 #include <linux/dmi.h>
 #include <linux/slab.h>
+#include <asm/irq_remapping.h>
 #include <asm/iommu_table.h>
 
 #define PREFIX "DMAR: "
@@ -555,7 +556,7 @@ int __init detect_intel_iommu(void)
 
 		dmar = (struct acpi_table_dmar *) dmar_tbl;
 
-		if (ret && intr_remapping_enabled && cpu_has_x2apic &&
+		if (ret && irq_remapping_enabled && cpu_has_x2apic &&
 		    dmar->flags & 0x1)
 			printk(KERN_INFO
 			       "Queued invalidation will be enabled to support x2apic and Intr-remapping.\n");
@@ -1041,7 +1042,7 @@ static const char *dma_remap_fault_reasons[] =
 	"non-zero reserved fields in PTE",
 };
 
-static const char *intr_remap_fault_reasons[] =
+static const char *irq_remap_fault_reasons[] =
 {
 	"Detected reserved fields in the decoded interrupt-remapped request",
 	"Interrupt index exceeded the interrupt-remapping table size",
@@ -1056,10 +1057,10 @@ static const char *intr_remap_fault_reasons[] =
 
 const char *dmar_get_fault_reason(u8 fault_reason, int *fault_type)
 {
-	if (fault_reason >= 0x20 && (fault_reason <= 0x20 +
-				     ARRAY_SIZE(intr_remap_fault_reasons))) {
+	if (fault_reason >= 0x20 && (fault_reason - 0x20 <
+					ARRAY_SIZE(irq_remap_fault_reasons))) {
 		*fault_type = INTR_REMAP;
-		return intr_remap_fault_reasons[fault_reason - 0x20];
+		return irq_remap_fault_reasons[fault_reason - 0x20];
 	} else if (fault_reason < ARRAY_SIZE(dma_remap_fault_reasons)) {
 		*fault_type = DMA_REMAP;
 		return dma_remap_fault_reasons[fault_reason];
diff --git a/drivers/iommu/exynos-iommu.c b/drivers/iommu/exynos-iommu.c
new file mode 100644
index 00000000000..9a114b9ff17
--- /dev/null
+++ b/drivers/iommu/exynos-iommu.c
@@ -0,0 +1,1076 @@
+/* linux/drivers/iommu/exynos_iommu.c
+ *
+ * Copyright (c) 2011 Samsung Electronics Co., Ltd.
+ *		http://www.samsung.com
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ */
+
+#ifdef CONFIG_EXYNOS_IOMMU_DEBUG
+#define DEBUG
+#endif
+
+#include <linux/io.h>
+#include <linux/interrupt.h>
+#include <linux/platform_device.h>
+#include <linux/slab.h>
+#include <linux/pm_runtime.h>
+#include <linux/clk.h>
+#include <linux/err.h>
+#include <linux/mm.h>
+#include <linux/iommu.h>
+#include <linux/errno.h>
+#include <linux/list.h>
+#include <linux/memblock.h>
+#include <linux/export.h>
+
+#include <asm/cacheflush.h>
+#include <asm/pgtable.h>
+
+#include <mach/sysmmu.h>
+
+/* We does not consider super section mapping (16MB) */
+#define SECT_ORDER 20
+#define LPAGE_ORDER 16
+#define SPAGE_ORDER 12
+
+#define SECT_SIZE (1 << SECT_ORDER)
+#define LPAGE_SIZE (1 << LPAGE_ORDER)
+#define SPAGE_SIZE (1 << SPAGE_ORDER)
+
+#define SECT_MASK (~(SECT_SIZE - 1))
+#define LPAGE_MASK (~(LPAGE_SIZE - 1))
+#define SPAGE_MASK (~(SPAGE_SIZE - 1))
+
+#define lv1ent_fault(sent) (((*(sent) & 3) == 0) || ((*(sent) & 3) == 3))
+#define lv1ent_page(sent) ((*(sent) & 3) == 1)
+#define lv1ent_section(sent) ((*(sent) & 3) == 2)
+
+#define lv2ent_fault(pent) ((*(pent) & 3) == 0)
+#define lv2ent_small(pent) ((*(pent) & 2) == 2)
+#define lv2ent_large(pent) ((*(pent) & 3) == 1)
+
+#define section_phys(sent) (*(sent) & SECT_MASK)
+#define section_offs(iova) ((iova) & 0xFFFFF)
+#define lpage_phys(pent) (*(pent) & LPAGE_MASK)
+#define lpage_offs(iova) ((iova) & 0xFFFF)
+#define spage_phys(pent) (*(pent) & SPAGE_MASK)
+#define spage_offs(iova) ((iova) & 0xFFF)
+
+#define lv1ent_offset(iova) ((iova) >> SECT_ORDER)
+#define lv2ent_offset(iova) (((iova) & 0xFF000) >> SPAGE_ORDER)
+
+#define NUM_LV1ENTRIES 4096
+#define NUM_LV2ENTRIES 256
+
+#define LV2TABLE_SIZE (NUM_LV2ENTRIES * sizeof(long))
+
+#define SPAGES_PER_LPAGE (LPAGE_SIZE / SPAGE_SIZE)
+
+#define lv2table_base(sent) (*(sent) & 0xFFFFFC00)
+
+#define mk_lv1ent_sect(pa) ((pa) | 2)
+#define mk_lv1ent_page(pa) ((pa) | 1)
+#define mk_lv2ent_lpage(pa) ((pa) | 1)
+#define mk_lv2ent_spage(pa) ((pa) | 2)
+
+#define CTRL_ENABLE	0x5
+#define CTRL_BLOCK	0x7
+#define CTRL_DISABLE	0x0
+
+#define REG_MMU_CTRL		0x000
+#define REG_MMU_CFG		0x004
+#define REG_MMU_STATUS		0x008
+#define REG_MMU_FLUSH		0x00C
+#define REG_MMU_FLUSH_ENTRY	0x010
+#define REG_PT_BASE_ADDR	0x014
+#define REG_INT_STATUS		0x018
+#define REG_INT_CLEAR		0x01C
+
+#define REG_PAGE_FAULT_ADDR	0x024
+#define REG_AW_FAULT_ADDR	0x028
+#define REG_AR_FAULT_ADDR	0x02C
+#define REG_DEFAULT_SLAVE_ADDR	0x030
+
+#define REG_MMU_VERSION		0x034
+
+#define REG_PB0_SADDR		0x04C
+#define REG_PB0_EADDR		0x050
+#define REG_PB1_SADDR		0x054
+#define REG_PB1_EADDR		0x058
+
+static unsigned long *section_entry(unsigned long *pgtable, unsigned long iova)
+{
+	return pgtable + lv1ent_offset(iova);
+}
+
+static unsigned long *page_entry(unsigned long *sent, unsigned long iova)
+{
+	return (unsigned long *)__va(lv2table_base(sent)) + lv2ent_offset(iova);
+}
+
+enum exynos_sysmmu_inttype {
+	SYSMMU_PAGEFAULT,
+	SYSMMU_AR_MULTIHIT,
+	SYSMMU_AW_MULTIHIT,
+	SYSMMU_BUSERROR,
+	SYSMMU_AR_SECURITY,
+	SYSMMU_AR_ACCESS,
+	SYSMMU_AW_SECURITY,
+	SYSMMU_AW_PROTECTION, /* 7 */
+	SYSMMU_FAULT_UNKNOWN,
+	SYSMMU_FAULTS_NUM
+};
+
+/*
+ * @itype: type of fault.
+ * @pgtable_base: the physical address of page table base. This is 0 if @itype
+ *                is SYSMMU_BUSERROR.
+ * @fault_addr: the device (virtual) address that the System MMU tried to
+ *             translated. This is 0 if @itype is SYSMMU_BUSERROR.
+ */
+typedef int (*sysmmu_fault_handler_t)(enum exynos_sysmmu_inttype itype,
+			unsigned long pgtable_base, unsigned long fault_addr);
+
+static unsigned short fault_reg_offset[SYSMMU_FAULTS_NUM] = {
+	REG_PAGE_FAULT_ADDR,
+	REG_AR_FAULT_ADDR,
+	REG_AW_FAULT_ADDR,
+	REG_DEFAULT_SLAVE_ADDR,
+	REG_AR_FAULT_ADDR,
+	REG_AR_FAULT_ADDR,
+	REG_AW_FAULT_ADDR,
+	REG_AW_FAULT_ADDR
+};
+
+static char *sysmmu_fault_name[SYSMMU_FAULTS_NUM] = {
+	"PAGE FAULT",
+	"AR MULTI-HIT FAULT",
+	"AW MULTI-HIT FAULT",
+	"BUS ERROR",
+	"AR SECURITY PROTECTION FAULT",
+	"AR ACCESS PROTECTION FAULT",
+	"AW SECURITY PROTECTION FAULT",
+	"AW ACCESS PROTECTION FAULT",
+	"UNKNOWN FAULT"
+};
+
+struct exynos_iommu_domain {
+	struct list_head clients; /* list of sysmmu_drvdata.node */
+	unsigned long *pgtable; /* lv1 page table, 16KB */
+	short *lv2entcnt; /* free lv2 entry counter for each section */
+	spinlock_t lock; /* lock for this structure */
+	spinlock_t pgtablelock; /* lock for modifying page table @ pgtable */
+};
+
+struct sysmmu_drvdata {
+	struct list_head node; /* entry of exynos_iommu_domain.clients */
+	struct device *sysmmu;	/* System MMU's device descriptor */
+	struct device *dev;	/* Owner of system MMU */
+	char *dbgname;
+	int nsfrs;
+	void __iomem **sfrbases;
+	struct clk *clk[2];
+	int activations;
+	rwlock_t lock;
+	struct iommu_domain *domain;
+	sysmmu_fault_handler_t fault_handler;
+	unsigned long pgtable;
+};
+
+static bool set_sysmmu_active(struct sysmmu_drvdata *data)
+{
+	/* return true if the System MMU was not active previously
+	   and it needs to be initialized */
+	return ++data->activations == 1;
+}
+
+static bool set_sysmmu_inactive(struct sysmmu_drvdata *data)
+{
+	/* return true if the System MMU is needed to be disabled */
+	BUG_ON(data->activations < 1);
+	return --data->activations == 0;
+}
+
+static bool is_sysmmu_active(struct sysmmu_drvdata *data)
+{
+	return data->activations > 0;
+}
+
+static void sysmmu_unblock(void __iomem *sfrbase)
+{
+	__raw_writel(CTRL_ENABLE, sfrbase + REG_MMU_CTRL);
+}
+
+static bool sysmmu_block(void __iomem *sfrbase)
+{
+	int i = 120;
+
+	__raw_writel(CTRL_BLOCK, sfrbase + REG_MMU_CTRL);
+	while ((i > 0) && !(__raw_readl(sfrbase + REG_MMU_STATUS) & 1))
+		--i;
+
+	if (!(__raw_readl(sfrbase + REG_MMU_STATUS) & 1)) {
+		sysmmu_unblock(sfrbase);
+		return false;
+	}
+
+	return true;
+}
+
+static void __sysmmu_tlb_invalidate(void __iomem *sfrbase)
+{
+	__raw_writel(0x1, sfrbase + REG_MMU_FLUSH);
+}
+
+static void __sysmmu_tlb_invalidate_entry(void __iomem *sfrbase,
+						unsigned long iova)
+{
+	__raw_writel((iova & SPAGE_MASK) | 1, sfrbase + REG_MMU_FLUSH_ENTRY);
+}
+
+static void __sysmmu_set_ptbase(void __iomem *sfrbase,
+				       unsigned long pgd)
+{
+	__raw_writel(0x1, sfrbase + REG_MMU_CFG); /* 16KB LV1, LRU */
+	__raw_writel(pgd, sfrbase + REG_PT_BASE_ADDR);
+
+	__sysmmu_tlb_invalidate(sfrbase);
+}
+
+static void __sysmmu_set_prefbuf(void __iomem *sfrbase, unsigned long base,
+						unsigned long size, int idx)
+{
+	__raw_writel(base, sfrbase + REG_PB0_SADDR + idx * 8);
+	__raw_writel(size - 1 + base,  sfrbase + REG_PB0_EADDR + idx * 8);
+}
+
+void exynos_sysmmu_set_prefbuf(struct device *dev,
+				unsigned long base0, unsigned long size0,
+				unsigned long base1, unsigned long size1)
+{
+	struct sysmmu_drvdata *data = dev_get_drvdata(dev->archdata.iommu);
+	unsigned long flags;
+	int i;
+
+	BUG_ON((base0 + size0) <= base0);
+	BUG_ON((size1 > 0) && ((base1 + size1) <= base1));
+
+	read_lock_irqsave(&data->lock, flags);
+	if (!is_sysmmu_active(data))
+		goto finish;
+
+	for (i = 0; i < data->nsfrs; i++) {
+		if ((readl(data->sfrbases[i] + REG_MMU_VERSION) >> 28) == 3) {
+			if (!sysmmu_block(data->sfrbases[i]))
+				continue;
+
+			if (size1 == 0) {
+				if (size0 <= SZ_128K) {
+					base1 = base0;
+					size1 = size0;
+				} else {
+					size1 = size0 -
+						ALIGN(size0 / 2, SZ_64K);
+					size0 = size0 - size1;
+					base1 = base0 + size0;
+				}
+			}
+
+			__sysmmu_set_prefbuf(
+					data->sfrbases[i], base0, size0, 0);
+			__sysmmu_set_prefbuf(
+					data->sfrbases[i], base1, size1, 1);
+
+			sysmmu_unblock(data->sfrbases[i]);
+		}
+	}
+finish:
+	read_unlock_irqrestore(&data->lock, flags);
+}
+
+static void __set_fault_handler(struct sysmmu_drvdata *data,
+					sysmmu_fault_handler_t handler)
+{
+	unsigned long flags;
+
+	write_lock_irqsave(&data->lock, flags);
+	data->fault_handler = handler;
+	write_unlock_irqrestore(&data->lock, flags);
+}
+
+void exynos_sysmmu_set_fault_handler(struct device *dev,
+					sysmmu_fault_handler_t handler)
+{
+	struct sysmmu_drvdata *data = dev_get_drvdata(dev->archdata.iommu);
+
+	__set_fault_handler(data, handler);
+}
+
+static int default_fault_handler(enum exynos_sysmmu_inttype itype,
+		     unsigned long pgtable_base, unsigned long fault_addr)
+{
+	unsigned long *ent;
+
+	if ((itype >= SYSMMU_FAULTS_NUM) || (itype < SYSMMU_PAGEFAULT))
+		itype = SYSMMU_FAULT_UNKNOWN;
+
+	pr_err("%s occured at 0x%lx(Page table base: 0x%lx)\n",
+			sysmmu_fault_name[itype], fault_addr, pgtable_base);
+
+	ent = section_entry(__va(pgtable_base), fault_addr);
+	pr_err("\tLv1 entry: 0x%lx\n", *ent);
+
+	if (lv1ent_page(ent)) {
+		ent = page_entry(ent, fault_addr);
+		pr_err("\t Lv2 entry: 0x%lx\n", *ent);
+	}
+
+	pr_err("Generating Kernel OOPS... because it is unrecoverable.\n");
+
+	BUG();
+
+	return 0;
+}
+
+static irqreturn_t exynos_sysmmu_irq(int irq, void *dev_id)
+{
+	/* SYSMMU is in blocked when interrupt occurred. */
+	struct sysmmu_drvdata *data = dev_id;
+	struct resource *irqres;
+	struct platform_device *pdev;
+	enum exynos_sysmmu_inttype itype;
+	unsigned long addr = -1;
+
+	int i, ret = -ENOSYS;
+
+	read_lock(&data->lock);
+
+	WARN_ON(!is_sysmmu_active(data));
+
+	pdev = to_platform_device(data->sysmmu);
+	for (i = 0; i < (pdev->num_resources / 2); i++) {
+		irqres = platform_get_resource(pdev, IORESOURCE_IRQ, i);
+		if (irqres && ((int)irqres->start == irq))
+			break;
+	}
+
+	if (i == pdev->num_resources) {
+		itype = SYSMMU_FAULT_UNKNOWN;
+	} else {
+		itype = (enum exynos_sysmmu_inttype)
+			__ffs(__raw_readl(data->sfrbases[i] + REG_INT_STATUS));
+		if (WARN_ON(!((itype >= 0) && (itype < SYSMMU_FAULT_UNKNOWN))))
+			itype = SYSMMU_FAULT_UNKNOWN;
+		else
+			addr = __raw_readl(
+				data->sfrbases[i] + fault_reg_offset[itype]);
+	}
+
+	if (data->domain)
+		ret = report_iommu_fault(data->domain, data->dev,
+				addr, itype);
+
+	if ((ret == -ENOSYS) && data->fault_handler) {
+		unsigned long base = data->pgtable;
+		if (itype != SYSMMU_FAULT_UNKNOWN)
+			base = __raw_readl(
+					data->sfrbases[i] + REG_PT_BASE_ADDR);
+		ret = data->fault_handler(itype, base, addr);
+	}
+
+	if (!ret && (itype != SYSMMU_FAULT_UNKNOWN))
+		__raw_writel(1 << itype, data->sfrbases[i] + REG_INT_CLEAR);
+	else
+		dev_dbg(data->sysmmu, "(%s) %s is not handled.\n",
+				data->dbgname, sysmmu_fault_name[itype]);
+
+	if (itype != SYSMMU_FAULT_UNKNOWN)
+		sysmmu_unblock(data->sfrbases[i]);
+
+	read_unlock(&data->lock);
+
+	return IRQ_HANDLED;
+}
+
+static bool __exynos_sysmmu_disable(struct sysmmu_drvdata *data)
+{
+	unsigned long flags;
+	bool disabled = false;
+	int i;
+
+	write_lock_irqsave(&data->lock, flags);
+
+	if (!set_sysmmu_inactive(data))
+		goto finish;
+
+	for (i = 0; i < data->nsfrs; i++)
+		__raw_writel(CTRL_DISABLE, data->sfrbases[i] + REG_MMU_CTRL);
+
+	if (data->clk[1])
+		clk_disable(data->clk[1]);
+	if (data->clk[0])
+		clk_disable(data->clk[0]);
+
+	disabled = true;
+	data->pgtable = 0;
+	data->domain = NULL;
+finish:
+	write_unlock_irqrestore(&data->lock, flags);
+
+	if (disabled)
+		dev_dbg(data->sysmmu, "(%s) Disabled\n", data->dbgname);
+	else
+		dev_dbg(data->sysmmu, "(%s) %d times left to be disabled\n",
+					data->dbgname, data->activations);
+
+	return disabled;
+}
+
+/* __exynos_sysmmu_enable: Enables System MMU
+ *
+ * returns -error if an error occurred and System MMU is not enabled,
+ * 0 if the System MMU has been just enabled and 1 if System MMU was already
+ * enabled before.
+ */
+static int __exynos_sysmmu_enable(struct sysmmu_drvdata *data,
+			unsigned long pgtable, struct iommu_domain *domain)
+{
+	int i, ret = 0;
+	unsigned long flags;
+
+	write_lock_irqsave(&data->lock, flags);
+
+	if (!set_sysmmu_active(data)) {
+		if (WARN_ON(pgtable != data->pgtable)) {
+			ret = -EBUSY;
+			set_sysmmu_inactive(data);
+		} else {
+			ret = 1;
+		}
+
+		dev_dbg(data->sysmmu, "(%s) Already enabled\n", data->dbgname);
+		goto finish;
+	}
+
+	if (data->clk[0])
+		clk_enable(data->clk[0]);
+	if (data->clk[1])
+		clk_enable(data->clk[1]);
+
+	data->pgtable = pgtable;
+
+	for (i = 0; i < data->nsfrs; i++) {
+		__sysmmu_set_ptbase(data->sfrbases[i], pgtable);
+
+		if ((readl(data->sfrbases[i] + REG_MMU_VERSION) >> 28) == 3) {
+			/* System MMU version is 3.x */
+			__raw_writel((1 << 12) | (2 << 28),
+					data->sfrbases[i] + REG_MMU_CFG);
+			__sysmmu_set_prefbuf(data->sfrbases[i], 0, -1, 0);
+			__sysmmu_set_prefbuf(data->sfrbases[i], 0, -1, 1);
+		}
+
+		__raw_writel(CTRL_ENABLE, data->sfrbases[i] + REG_MMU_CTRL);
+	}
+
+	data->domain = domain;
+
+	dev_dbg(data->sysmmu, "(%s) Enabled\n", data->dbgname);
+finish:
+	write_unlock_irqrestore(&data->lock, flags);
+
+	return ret;
+}
+
+int exynos_sysmmu_enable(struct device *dev, unsigned long pgtable)
+{
+	struct sysmmu_drvdata *data = dev_get_drvdata(dev->archdata.iommu);
+	int ret;
+
+	BUG_ON(!memblock_is_memory(pgtable));
+
+	ret = pm_runtime_get_sync(data->sysmmu);
+	if (ret < 0) {
+		dev_dbg(data->sysmmu, "(%s) Failed to enable\n", data->dbgname);
+		return ret;
+	}
+
+	ret = __exynos_sysmmu_enable(data, pgtable, NULL);
+	if (WARN_ON(ret < 0)) {
+		pm_runtime_put(data->sysmmu);
+		dev_err(data->sysmmu,
+			"(%s) Already enabled with page table %#lx\n",
+			data->dbgname, data->pgtable);
+	} else {
+		data->dev = dev;
+	}
+
+	return ret;
+}
+
+bool exynos_sysmmu_disable(struct device *dev)
+{
+	struct sysmmu_drvdata *data = dev_get_drvdata(dev->archdata.iommu);
+	bool disabled;
+
+	disabled = __exynos_sysmmu_disable(data);
+	pm_runtime_put(data->sysmmu);
+
+	return disabled;
+}
+
+static void sysmmu_tlb_invalidate_entry(struct device *dev, unsigned long iova)
+{
+	unsigned long flags;
+	struct sysmmu_drvdata *data = dev_get_drvdata(dev->archdata.iommu);
+
+	read_lock_irqsave(&data->lock, flags);
+
+	if (is_sysmmu_active(data)) {
+		int i;
+		for (i = 0; i < data->nsfrs; i++) {
+			if (sysmmu_block(data->sfrbases[i])) {
+				__sysmmu_tlb_invalidate_entry(
+						data->sfrbases[i], iova);
+				sysmmu_unblock(data->sfrbases[i]);
+			}
+		}
+	} else {
+		dev_dbg(data->sysmmu,
+			"(%s) Disabled. Skipping invalidating TLB.\n",
+			data->dbgname);
+	}
+
+	read_unlock_irqrestore(&data->lock, flags);
+}
+
+void exynos_sysmmu_tlb_invalidate(struct device *dev)
+{
+	unsigned long flags;
+	struct sysmmu_drvdata *data = dev_get_drvdata(dev->archdata.iommu);
+
+	read_lock_irqsave(&data->lock, flags);
+
+	if (is_sysmmu_active(data)) {
+		int i;
+		for (i = 0; i < data->nsfrs; i++) {
+			if (sysmmu_block(data->sfrbases[i])) {
+				__sysmmu_tlb_invalidate(data->sfrbases[i]);
+				sysmmu_unblock(data->sfrbases[i]);
+			}
+		}
+	} else {
+		dev_dbg(data->sysmmu,
+			"(%s) Disabled. Skipping invalidating TLB.\n",
+			data->dbgname);
+	}
+
+	read_unlock_irqrestore(&data->lock, flags);
+}
+
+static int exynos_sysmmu_probe(struct platform_device *pdev)
+{
+	int i, ret;
+	struct device *dev;
+	struct sysmmu_drvdata *data;
+
+	dev = &pdev->dev;
+
+	data = kzalloc(sizeof(*data), GFP_KERNEL);
+	if (!data) {
+		dev_dbg(dev, "Not enough memory\n");
+		ret = -ENOMEM;
+		goto err_alloc;
+	}
+
+	ret = dev_set_drvdata(dev, data);
+	if (ret) {
+		dev_dbg(dev, "Unabled to initialize driver data\n");
+		goto err_init;
+	}
+
+	data->nsfrs = pdev->num_resources / 2;
+	data->sfrbases = kmalloc(sizeof(*data->sfrbases) * data->nsfrs,
+								GFP_KERNEL);
+	if (data->sfrbases == NULL) {
+		dev_dbg(dev, "Not enough memory\n");
+		ret = -ENOMEM;
+		goto err_init;
+	}
+
+	for (i = 0; i < data->nsfrs; i++) {
+		struct resource *res;
+		res = platform_get_resource(pdev, IORESOURCE_MEM, i);
+		if (!res) {
+			dev_dbg(dev, "Unable to find IOMEM region\n");
+			ret = -ENOENT;
+			goto err_res;
+		}
+
+		data->sfrbases[i] = ioremap(res->start, resource_size(res));
+		if (!data->sfrbases[i]) {
+			dev_dbg(dev, "Unable to map IOMEM @ PA:%#x\n",
+							res->start);
+			ret = -ENOENT;
+			goto err_res;
+		}
+	}
+
+	for (i = 0; i < data->nsfrs; i++) {
+		ret = platform_get_irq(pdev, i);
+		if (ret <= 0) {
+			dev_dbg(dev, "Unable to find IRQ resource\n");
+			goto err_irq;
+		}
+
+		ret = request_irq(ret, exynos_sysmmu_irq, 0,
+					dev_name(dev), data);
+		if (ret) {
+			dev_dbg(dev, "Unabled to register interrupt handler\n");
+			goto err_irq;
+		}
+	}
+
+	if (dev_get_platdata(dev)) {
+		char *deli, *beg;
+		struct sysmmu_platform_data *platdata = dev_get_platdata(dev);
+
+		beg = platdata->clockname;
+
+		for (deli = beg; (*deli != '\0') && (*deli != ','); deli++)
+			/* NOTHING */;
+
+		if (*deli == '\0')
+			deli = NULL;
+		else
+			*deli = '\0';
+
+		data->clk[0] = clk_get(dev, beg);
+		if (IS_ERR(data->clk[0])) {
+			data->clk[0] = NULL;
+			dev_dbg(dev, "No clock descriptor registered\n");
+		}
+
+		if (data->clk[0] && deli) {
+			*deli = ',';
+			data->clk[1] = clk_get(dev, deli + 1);
+			if (IS_ERR(data->clk[1]))
+				data->clk[1] = NULL;
+		}
+
+		data->dbgname = platdata->dbgname;
+	}
+
+	data->sysmmu = dev;
+	rwlock_init(&data->lock);
+	INIT_LIST_HEAD(&data->node);
+
+	__set_fault_handler(data, &default_fault_handler);
+
+	if (dev->parent)
+		pm_runtime_enable(dev);
+
+	dev_dbg(dev, "(%s) Initialized\n", data->dbgname);
+	return 0;
+err_irq:
+	while (i-- > 0) {
+		int irq;
+
+		irq = platform_get_irq(pdev, i);
+		free_irq(irq, data);
+	}
+err_res:
+	while (data->nsfrs-- > 0)
+		iounmap(data->sfrbases[data->nsfrs]);
+	kfree(data->sfrbases);
+err_init:
+	kfree(data);
+err_alloc:
+	dev_err(dev, "Failed to initialize\n");
+	return ret;
+}
+
+static struct platform_driver exynos_sysmmu_driver = {
+	.probe		= exynos_sysmmu_probe,
+	.driver		= {
+		.owner		= THIS_MODULE,
+		.name		= "exynos-sysmmu",
+	}
+};
+
+static inline void pgtable_flush(void *vastart, void *vaend)
+{
+	dmac_flush_range(vastart, vaend);
+	outer_flush_range(virt_to_phys(vastart),
+				virt_to_phys(vaend));
+}
+
+static int exynos_iommu_domain_init(struct iommu_domain *domain)
+{
+	struct exynos_iommu_domain *priv;
+
+	priv = kzalloc(sizeof(*priv), GFP_KERNEL);
+	if (!priv)
+		return -ENOMEM;
+
+	priv->pgtable = (unsigned long *)__get_free_pages(
+						GFP_KERNEL | __GFP_ZERO, 2);
+	if (!priv->pgtable)
+		goto err_pgtable;
+
+	priv->lv2entcnt = (short *)__get_free_pages(
+						GFP_KERNEL | __GFP_ZERO, 1);
+	if (!priv->lv2entcnt)
+		goto err_counter;
+
+	pgtable_flush(priv->pgtable, priv->pgtable + NUM_LV1ENTRIES);
+
+	spin_lock_init(&priv->lock);
+	spin_lock_init(&priv->pgtablelock);
+	INIT_LIST_HEAD(&priv->clients);
+
+	domain->priv = priv;
+	return 0;
+
+err_counter:
+	free_pages((unsigned long)priv->pgtable, 2);
+err_pgtable:
+	kfree(priv);
+	return -ENOMEM;
+}
+
+static void exynos_iommu_domain_destroy(struct iommu_domain *domain)
+{
+	struct exynos_iommu_domain *priv = domain->priv;
+	struct sysmmu_drvdata *data;
+	unsigned long flags;
+	int i;
+
+	WARN_ON(!list_empty(&priv->clients));
+
+	spin_lock_irqsave(&priv->lock, flags);
+
+	list_for_each_entry(data, &priv->clients, node) {
+		while (!exynos_sysmmu_disable(data->dev))
+			; /* until System MMU is actually disabled */
+	}
+
+	spin_unlock_irqrestore(&priv->lock, flags);
+
+	for (i = 0; i < NUM_LV1ENTRIES; i++)
+		if (lv1ent_page(priv->pgtable + i))
+			kfree(__va(lv2table_base(priv->pgtable + i)));
+
+	free_pages((unsigned long)priv->pgtable, 2);
+	free_pages((unsigned long)priv->lv2entcnt, 1);
+	kfree(domain->priv);
+	domain->priv = NULL;
+}
+
+static int exynos_iommu_attach_device(struct iommu_domain *domain,
+				   struct device *dev)
+{
+	struct sysmmu_drvdata *data = dev_get_drvdata(dev->archdata.iommu);
+	struct exynos_iommu_domain *priv = domain->priv;
+	unsigned long flags;
+	int ret;
+
+	ret = pm_runtime_get_sync(data->sysmmu);
+	if (ret < 0)
+		return ret;
+
+	ret = 0;
+
+	spin_lock_irqsave(&priv->lock, flags);
+
+	ret = __exynos_sysmmu_enable(data, __pa(priv->pgtable), domain);
+
+	if (ret == 0) {
+		/* 'data->node' must not be appeared in priv->clients */
+		BUG_ON(!list_empty(&data->node));
+		data->dev = dev;
+		list_add_tail(&data->node, &priv->clients);
+	}
+
+	spin_unlock_irqrestore(&priv->lock, flags);
+
+	if (ret < 0) {
+		dev_err(dev, "%s: Failed to attach IOMMU with pgtable %#lx\n",
+				__func__, __pa(priv->pgtable));
+		pm_runtime_put(data->sysmmu);
+	} else if (ret > 0) {
+		dev_dbg(dev, "%s: IOMMU with pgtable 0x%lx already attached\n",
+					__func__, __pa(priv->pgtable));
+	} else {
+		dev_dbg(dev, "%s: Attached new IOMMU with pgtable 0x%lx\n",
+					__func__, __pa(priv->pgtable));
+	}
+
+	return ret;
+}
+
+static void exynos_iommu_detach_device(struct iommu_domain *domain,
+				    struct device *dev)
+{
+	struct sysmmu_drvdata *data = dev_get_drvdata(dev->archdata.iommu);
+	struct exynos_iommu_domain *priv = domain->priv;
+	struct list_head *pos;
+	unsigned long flags;
+	bool found = false;
+
+	spin_lock_irqsave(&priv->lock, flags);
+
+	list_for_each(pos, &priv->clients) {
+		if (list_entry(pos, struct sysmmu_drvdata, node) == data) {
+			found = true;
+			break;
+		}
+	}
+
+	if (!found)
+		goto finish;
+
+	if (__exynos_sysmmu_disable(data)) {
+		dev_dbg(dev, "%s: Detached IOMMU with pgtable %#lx\n",
+					__func__, __pa(priv->pgtable));
+		list_del(&data->node);
+		INIT_LIST_HEAD(&data->node);
+
+	} else {
+		dev_dbg(dev, "%s: Detaching IOMMU with pgtable %#lx delayed",
+					__func__, __pa(priv->pgtable));
+	}
+
+finish:
+	spin_unlock_irqrestore(&priv->lock, flags);
+
+	if (found)
+		pm_runtime_put(data->sysmmu);
+}
+
+static unsigned long *alloc_lv2entry(unsigned long *sent, unsigned long iova,
+					short *pgcounter)
+{
+	if (lv1ent_fault(sent)) {
+		unsigned long *pent;
+
+		pent = kzalloc(LV2TABLE_SIZE, GFP_ATOMIC);
+		BUG_ON((unsigned long)pent & (LV2TABLE_SIZE - 1));
+		if (!pent)
+			return NULL;
+
+		*sent = mk_lv1ent_page(__pa(pent));
+		*pgcounter = NUM_LV2ENTRIES;
+		pgtable_flush(pent, pent + NUM_LV2ENTRIES);
+		pgtable_flush(sent, sent + 1);
+	}
+
+	return page_entry(sent, iova);
+}
+
+static int lv1set_section(unsigned long *sent, phys_addr_t paddr, short *pgcnt)
+{
+	if (lv1ent_section(sent))
+		return -EADDRINUSE;
+
+	if (lv1ent_page(sent)) {
+		if (*pgcnt != NUM_LV2ENTRIES)
+			return -EADDRINUSE;
+
+		kfree(page_entry(sent, 0));
+
+		*pgcnt = 0;
+	}
+
+	*sent = mk_lv1ent_sect(paddr);
+
+	pgtable_flush(sent, sent + 1);
+
+	return 0;
+}
+
+static int lv2set_page(unsigned long *pent, phys_addr_t paddr, size_t size,
+								short *pgcnt)
+{
+	if (size == SPAGE_SIZE) {
+		if (!lv2ent_fault(pent))
+			return -EADDRINUSE;
+
+		*pent = mk_lv2ent_spage(paddr);
+		pgtable_flush(pent, pent + 1);
+		*pgcnt -= 1;
+	} else { /* size == LPAGE_SIZE */
+		int i;
+		for (i = 0; i < SPAGES_PER_LPAGE; i++, pent++) {
+			if (!lv2ent_fault(pent)) {
+				memset(pent, 0, sizeof(*pent) * i);
+				return -EADDRINUSE;
+			}
+
+			*pent = mk_lv2ent_lpage(paddr);
+		}
+		pgtable_flush(pent - SPAGES_PER_LPAGE, pent);
+		*pgcnt -= SPAGES_PER_LPAGE;
+	}
+
+	return 0;
+}
+
+static int exynos_iommu_map(struct iommu_domain *domain, unsigned long iova,
+			 phys_addr_t paddr, size_t size, int prot)
+{
+	struct exynos_iommu_domain *priv = domain->priv;
+	unsigned long *entry;
+	unsigned long flags;
+	int ret = -ENOMEM;
+
+	BUG_ON(priv->pgtable == NULL);
+
+	spin_lock_irqsave(&priv->pgtablelock, flags);
+
+	entry = section_entry(priv->pgtable, iova);
+
+	if (size == SECT_SIZE) {
+		ret = lv1set_section(entry, paddr,
+					&priv->lv2entcnt[lv1ent_offset(iova)]);
+	} else {
+		unsigned long *pent;
+
+		pent = alloc_lv2entry(entry, iova,
+					&priv->lv2entcnt[lv1ent_offset(iova)]);
+
+		if (!pent)
+			ret = -ENOMEM;
+		else
+			ret = lv2set_page(pent, paddr, size,
+					&priv->lv2entcnt[lv1ent_offset(iova)]);
+	}
+
+	if (ret) {
+		pr_debug("%s: Failed to map iova 0x%lx/0x%x bytes\n",
+							__func__, iova, size);
+	}
+
+	spin_unlock_irqrestore(&priv->pgtablelock, flags);
+
+	return ret;
+}
+
+static size_t exynos_iommu_unmap(struct iommu_domain *domain,
+					       unsigned long iova, size_t size)
+{
+	struct exynos_iommu_domain *priv = domain->priv;
+	struct sysmmu_drvdata *data;
+	unsigned long flags;
+	unsigned long *ent;
+
+	BUG_ON(priv->pgtable == NULL);
+
+	spin_lock_irqsave(&priv->pgtablelock, flags);
+
+	ent = section_entry(priv->pgtable, iova);
+
+	if (lv1ent_section(ent)) {
+		BUG_ON(size < SECT_SIZE);
+
+		*ent = 0;
+		pgtable_flush(ent, ent + 1);
+		size = SECT_SIZE;
+		goto done;
+	}
+
+	if (unlikely(lv1ent_fault(ent))) {
+		if (size > SECT_SIZE)
+			size = SECT_SIZE;
+		goto done;
+	}
+
+	/* lv1ent_page(sent) == true here */
+
+	ent = page_entry(ent, iova);
+
+	if (unlikely(lv2ent_fault(ent))) {
+		size = SPAGE_SIZE;
+		goto done;
+	}
+
+	if (lv2ent_small(ent)) {
+		*ent = 0;
+		size = SPAGE_SIZE;
+		priv->lv2entcnt[lv1ent_offset(iova)] += 1;
+		goto done;
+	}
+
+	/* lv1ent_large(ent) == true here */
+	BUG_ON(size < LPAGE_SIZE);
+
+	memset(ent, 0, sizeof(*ent) * SPAGES_PER_LPAGE);
+
+	size = LPAGE_SIZE;
+	priv->lv2entcnt[lv1ent_offset(iova)] += SPAGES_PER_LPAGE;
+done:
+	spin_unlock_irqrestore(&priv->pgtablelock, flags);
+
+	spin_lock_irqsave(&priv->lock, flags);
+	list_for_each_entry(data, &priv->clients, node)
+		sysmmu_tlb_invalidate_entry(data->dev, iova);
+	spin_unlock_irqrestore(&priv->lock, flags);
+
+
+	return size;
+}
+
+static phys_addr_t exynos_iommu_iova_to_phys(struct iommu_domain *domain,
+					  unsigned long iova)
+{
+	struct exynos_iommu_domain *priv = domain->priv;
+	unsigned long *entry;
+	unsigned long flags;
+	phys_addr_t phys = 0;
+
+	spin_lock_irqsave(&priv->pgtablelock, flags);
+
+	entry = section_entry(priv->pgtable, iova);
+
+	if (lv1ent_section(entry)) {
+		phys = section_phys(entry) + section_offs(iova);
+	} else if (lv1ent_page(entry)) {
+		entry = page_entry(entry, iova);
+
+		if (lv2ent_large(entry))
+			phys = lpage_phys(entry) + lpage_offs(iova);
+		else if (lv2ent_small(entry))
+			phys = spage_phys(entry) + spage_offs(iova);
+	}
+
+	spin_unlock_irqrestore(&priv->pgtablelock, flags);
+
+	return phys;
+}
+
+static struct iommu_ops exynos_iommu_ops = {
+	.domain_init = &exynos_iommu_domain_init,
+	.domain_destroy = &exynos_iommu_domain_destroy,
+	.attach_dev = &exynos_iommu_attach_device,
+	.detach_dev = &exynos_iommu_detach_device,
+	.map = &exynos_iommu_map,
+	.unmap = &exynos_iommu_unmap,
+	.iova_to_phys = &exynos_iommu_iova_to_phys,
+	.pgsize_bitmap = SECT_SIZE | LPAGE_SIZE | SPAGE_SIZE,
+};
+
+static int __init exynos_iommu_init(void)
+{
+	int ret;
+
+	ret = platform_driver_register(&exynos_sysmmu_driver);
+
+	if (ret == 0)
+		bus_set_iommu(&platform_bus_type, &exynos_iommu_ops);
+
+	return ret;
+}
+subsys_initcall(exynos_iommu_init);
diff --git a/drivers/iommu/intel-iommu.c b/drivers/iommu/intel-iommu.c
index f93d5ac8f81..b12af2ff8c5 100644
--- a/drivers/iommu/intel-iommu.c
+++ b/drivers/iommu/intel-iommu.c
@@ -42,6 +42,7 @@
 #include <linux/dmi.h>
 #include <linux/pci-ats.h>
 #include <linux/memblock.h>
+#include <asm/irq_remapping.h>
 #include <asm/cacheflush.h>
 #include <asm/iommu.h>
 
@@ -1906,6 +1907,15 @@ static void iommu_detach_dev(struct intel_iommu *iommu, u8 bus, u8 devfn)
 	iommu->flush.flush_iotlb(iommu, 0, 0, 0, DMA_TLB_GLOBAL_FLUSH);
 }
 
+static inline void unlink_domain_info(struct device_domain_info *info)
+{
+	assert_spin_locked(&device_domain_lock);
+	list_del(&info->link);
+	list_del(&info->global);
+	if (info->dev)
+		info->dev->dev.archdata.iommu = NULL;
+}
+
 static void domain_remove_dev_info(struct dmar_domain *domain)
 {
 	struct device_domain_info *info;
@@ -1916,10 +1926,7 @@ static void domain_remove_dev_info(struct dmar_domain *domain)
 	while (!list_empty(&domain->devices)) {
 		info = list_entry(domain->devices.next,
 			struct device_domain_info, link);
-		list_del(&info->link);
-		list_del(&info->global);
-		if (info->dev)
-			info->dev->dev.archdata.iommu = NULL;
+		unlink_domain_info(info);
 		spin_unlock_irqrestore(&device_domain_lock, flags);
 
 		iommu_disable_dev_iotlb(info);
@@ -2286,12 +2293,6 @@ static int domain_add_dev_info(struct dmar_domain *domain,
 	if (!info)
 		return -ENOMEM;
 
-	ret = domain_context_mapping(domain, pdev, translation);
-	if (ret) {
-		free_devinfo_mem(info);
-		return ret;
-	}
-
 	info->segment = pci_domain_nr(pdev->bus);
 	info->bus = pdev->bus->number;
 	info->devfn = pdev->devfn;
@@ -2304,6 +2305,15 @@ static int domain_add_dev_info(struct dmar_domain *domain,
 	pdev->dev.archdata.iommu = info;
 	spin_unlock_irqrestore(&device_domain_lock, flags);
 
+	ret = domain_context_mapping(domain, pdev, translation);
+	if (ret) {
+		spin_lock_irqsave(&device_domain_lock, flags);
+		unlink_domain_info(info);
+		spin_unlock_irqrestore(&device_domain_lock, flags);
+		free_devinfo_mem(info);
+		return ret;
+	}
+
 	return 0;
 }
 
@@ -3727,10 +3737,7 @@ static void domain_remove_one_dev_info(struct dmar_domain *domain,
 		if (info->segment == pci_domain_nr(pdev->bus) &&
 		    info->bus == pdev->bus->number &&
 		    info->devfn == pdev->devfn) {
-			list_del(&info->link);
-			list_del(&info->global);
-			if (info->dev)
-				info->dev->dev.archdata.iommu = NULL;
+			unlink_domain_info(info);
 			spin_unlock_irqrestore(&device_domain_lock, flags);
 
 			iommu_disable_dev_iotlb(info);
@@ -3785,11 +3792,7 @@ static void vm_domain_remove_all_dev_info(struct dmar_domain *domain)
 	while (!list_empty(&domain->devices)) {
 		info = list_entry(domain->devices.next,
 			struct device_domain_info, link);
-		list_del(&info->link);
-		list_del(&info->global);
-		if (info->dev)
-			info->dev->dev.archdata.iommu = NULL;
-
+		unlink_domain_info(info);
 		spin_unlock_irqrestore(&device_domain_lock, flags1);
 
 		iommu_disable_dev_iotlb(info);
@@ -4082,7 +4085,7 @@ static int intel_iommu_domain_has_cap(struct iommu_domain *domain,
 	if (cap == IOMMU_CAP_CACHE_COHERENCY)
 		return dmar_domain->iommu_snooping;
 	if (cap == IOMMU_CAP_INTR_REMAP)
-		return intr_remapping_enabled;
+		return irq_remapping_enabled;
 
 	return 0;
 }
diff --git a/drivers/iommu/intr_remapping.c b/drivers/iommu/intel_irq_remapping.c
index 6777ca04947..6d347064b8b 100644
--- a/drivers/iommu/intr_remapping.c
+++ b/drivers/iommu/intel_irq_remapping.c
@@ -10,49 +10,33 @@
 #include <asm/smp.h>
 #include <asm/cpu.h>
 #include <linux/intel-iommu.h>
-#include "intr_remapping.h"
 #include <acpi/acpi.h>
+#include <asm/irq_remapping.h>
 #include <asm/pci-direct.h>
+#include <asm/msidef.h>
 
-static struct ioapic_scope ir_ioapic[MAX_IO_APICS];
-static struct hpet_scope ir_hpet[MAX_HPET_TBS];
-static int ir_ioapic_num, ir_hpet_num;
-int intr_remapping_enabled;
-
-static int disable_intremap;
-static int disable_sourceid_checking;
-static int no_x2apic_optout;
+#include "irq_remapping.h"
 
-static __init int setup_nointremap(char *str)
-{
-	disable_intremap = 1;
-	return 0;
-}
-early_param("nointremap", setup_nointremap);
+struct ioapic_scope {
+	struct intel_iommu *iommu;
+	unsigned int id;
+	unsigned int bus;	/* PCI bus number */
+	unsigned int devfn;	/* PCI devfn number */
+};
 
-static __init int setup_intremap(char *str)
-{
-	if (!str)
-		return -EINVAL;
+struct hpet_scope {
+	struct intel_iommu *iommu;
+	u8 id;
+	unsigned int bus;
+	unsigned int devfn;
+};
 
-	while (*str) {
-		if (!strncmp(str, "on", 2))
-			disable_intremap = 0;
-		else if (!strncmp(str, "off", 3))
-			disable_intremap = 1;
-		else if (!strncmp(str, "nosid", 5))
-			disable_sourceid_checking = 1;
-		else if (!strncmp(str, "no_x2apic_optout", 16))
-			no_x2apic_optout = 1;
-
-		str += strcspn(str, ",");
-		while (*str == ',')
-			str++;
-	}
+#define IR_X2APIC_MODE(mode) (mode ? (1 << 11) : 0)
+#define IRTE_DEST(dest) ((x2apic_mode) ? dest : dest << 8)
 
-	return 0;
-}
-early_param("intremap", setup_intremap);
+static struct ioapic_scope ir_ioapic[MAX_IO_APICS];
+static struct hpet_scope ir_hpet[MAX_HPET_TBS];
+static int ir_ioapic_num, ir_hpet_num;
 
 static DEFINE_RAW_SPINLOCK(irq_2_ir_lock);
 
@@ -80,7 +64,7 @@ int get_irte(int irq, struct irte *entry)
 	return 0;
 }
 
-int alloc_irte(struct intel_iommu *iommu, int irq, u16 count)
+static int alloc_irte(struct intel_iommu *iommu, int irq, u16 count)
 {
 	struct ir_table *table = iommu->ir_table;
 	struct irq_2_iommu *irq_iommu = irq_2_iommu(irq);
@@ -152,7 +136,7 @@ static int qi_flush_iec(struct intel_iommu *iommu, int index, int mask)
 	return qi_submit_sync(&desc, iommu);
 }
 
-int map_irq_to_irte_handle(int irq, u16 *sub_handle)
+static int map_irq_to_irte_handle(int irq, u16 *sub_handle)
 {
 	struct irq_2_iommu *irq_iommu = irq_2_iommu(irq);
 	unsigned long flags;
@@ -168,7 +152,7 @@ int map_irq_to_irte_handle(int irq, u16 *sub_handle)
 	return index;
 }
 
-int set_irte_irq(int irq, struct intel_iommu *iommu, u16 index, u16 subhandle)
+static int set_irte_irq(int irq, struct intel_iommu *iommu, u16 index, u16 subhandle)
 {
 	struct irq_2_iommu *irq_iommu = irq_2_iommu(irq);
 	unsigned long flags;
@@ -188,7 +172,7 @@ int set_irte_irq(int irq, struct intel_iommu *iommu, u16 index, u16 subhandle)
 	return 0;
 }
 
-int modify_irte(int irq, struct irte *irte_modified)
+static int modify_irte(int irq, struct irte *irte_modified)
 {
 	struct irq_2_iommu *irq_iommu = irq_2_iommu(irq);
 	struct intel_iommu *iommu;
@@ -216,7 +200,7 @@ int modify_irte(int irq, struct irte *irte_modified)
 	return rc;
 }
 
-struct intel_iommu *map_hpet_to_ir(u8 hpet_id)
+static struct intel_iommu *map_hpet_to_ir(u8 hpet_id)
 {
 	int i;
 
@@ -226,7 +210,7 @@ struct intel_iommu *map_hpet_to_ir(u8 hpet_id)
 	return NULL;
 }
 
-struct intel_iommu *map_ioapic_to_ir(int apic)
+static struct intel_iommu *map_ioapic_to_ir(int apic)
 {
 	int i;
 
@@ -236,7 +220,7 @@ struct intel_iommu *map_ioapic_to_ir(int apic)
 	return NULL;
 }
 
-struct intel_iommu *map_dev_to_ir(struct pci_dev *dev)
+static struct intel_iommu *map_dev_to_ir(struct pci_dev *dev)
 {
 	struct dmar_drhd_unit *drhd;
 
@@ -270,7 +254,7 @@ static int clear_entries(struct irq_2_iommu *irq_iommu)
 	return qi_flush_iec(iommu, index, irq_iommu->irte_mask);
 }
 
-int free_irte(int irq)
+static int free_irte(int irq)
 {
 	struct irq_2_iommu *irq_iommu = irq_2_iommu(irq);
 	unsigned long flags;
@@ -328,7 +312,7 @@ static void set_irte_sid(struct irte *irte, unsigned int svt,
 	irte->sid = sid;
 }
 
-int set_ioapic_sid(struct irte *irte, int apic)
+static int set_ioapic_sid(struct irte *irte, int apic)
 {
 	int i;
 	u16 sid = 0;
@@ -353,7 +337,7 @@ int set_ioapic_sid(struct irte *irte, int apic)
 	return 0;
 }
 
-int set_hpet_sid(struct irte *irte, u8 id)
+static int set_hpet_sid(struct irte *irte, u8 id)
 {
 	int i;
 	u16 sid = 0;
@@ -383,7 +367,7 @@ int set_hpet_sid(struct irte *irte, u8 id)
 	return 0;
 }
 
-int set_msi_sid(struct irte *irte, struct pci_dev *dev)
+static int set_msi_sid(struct irte *irte, struct pci_dev *dev)
 {
 	struct pci_dev *bridge;
 
@@ -410,7 +394,7 @@ int set_msi_sid(struct irte *irte, struct pci_dev *dev)
 	return 0;
 }
 
-static void iommu_set_intr_remapping(struct intel_iommu *iommu, int mode)
+static void iommu_set_irq_remapping(struct intel_iommu *iommu, int mode)
 {
 	u64 addr;
 	u32 sts;
@@ -450,7 +434,7 @@ static void iommu_set_intr_remapping(struct intel_iommu *iommu, int mode)
 }
 
 
-static int setup_intr_remapping(struct intel_iommu *iommu, int mode)
+static int intel_setup_irq_remapping(struct intel_iommu *iommu, int mode)
 {
 	struct ir_table *ir_table;
 	struct page *pages;
@@ -473,14 +457,14 @@ static int setup_intr_remapping(struct intel_iommu *iommu, int mode)
 
 	ir_table->base = page_address(pages);
 
-	iommu_set_intr_remapping(iommu, mode);
+	iommu_set_irq_remapping(iommu, mode);
 	return 0;
 }
 
 /*
  * Disable Interrupt Remapping.
  */
-static void iommu_disable_intr_remapping(struct intel_iommu *iommu)
+static void iommu_disable_irq_remapping(struct intel_iommu *iommu)
 {
 	unsigned long flags;
 	u32 sts;
@@ -519,11 +503,11 @@ static int __init dmar_x2apic_optout(void)
 	return dmar->flags & DMAR_X2APIC_OPT_OUT;
 }
 
-int __init intr_remapping_supported(void)
+static int __init intel_irq_remapping_supported(void)
 {
 	struct dmar_drhd_unit *drhd;
 
-	if (disable_intremap)
+	if (disable_irq_remap)
 		return 0;
 
 	if (!dmar_ir_support())
@@ -539,7 +523,7 @@ int __init intr_remapping_supported(void)
 	return 1;
 }
 
-int __init enable_intr_remapping(void)
+static int __init intel_enable_irq_remapping(void)
 {
 	struct dmar_drhd_unit *drhd;
 	int setup = 0;
@@ -577,7 +561,7 @@ int __init enable_intr_remapping(void)
 		 * Disable intr remapping and queued invalidation, if already
 		 * enabled prior to OS handover.
 		 */
-		iommu_disable_intr_remapping(iommu);
+		iommu_disable_irq_remapping(iommu);
 
 		dmar_disable_qi(iommu);
 	}
@@ -623,7 +607,7 @@ int __init enable_intr_remapping(void)
 		if (!ecap_ir_support(iommu->ecap))
 			continue;
 
-		if (setup_intr_remapping(iommu, eim))
+		if (intel_setup_irq_remapping(iommu, eim))
 			goto error;
 
 		setup = 1;
@@ -632,7 +616,7 @@ int __init enable_intr_remapping(void)
 	if (!setup)
 		goto error;
 
-	intr_remapping_enabled = 1;
+	irq_remapping_enabled = 1;
 	pr_info("Enabled IRQ remapping in %s mode\n", eim ? "x2apic" : "xapic");
 
 	return eim ? IRQ_REMAP_X2APIC_MODE : IRQ_REMAP_XAPIC_MODE;
@@ -775,14 +759,14 @@ int __init parse_ioapics_under_ir(void)
 
 int __init ir_dev_scope_init(void)
 {
-	if (!intr_remapping_enabled)
+	if (!irq_remapping_enabled)
 		return 0;
 
 	return dmar_dev_scope_init();
 }
 rootfs_initcall(ir_dev_scope_init);
 
-void disable_intr_remapping(void)
+static void disable_irq_remapping(void)
 {
 	struct dmar_drhd_unit *drhd;
 	struct intel_iommu *iommu = NULL;
@@ -794,11 +778,11 @@ void disable_intr_remapping(void)
 		if (!ecap_ir_support(iommu->ecap))
 			continue;
 
-		iommu_disable_intr_remapping(iommu);
+		iommu_disable_irq_remapping(iommu);
 	}
 }
 
-int reenable_intr_remapping(int eim)
+static int reenable_irq_remapping(int eim)
 {
 	struct dmar_drhd_unit *drhd;
 	int setup = 0;
@@ -816,7 +800,7 @@ int reenable_intr_remapping(int eim)
 			continue;
 
 		/* Set up interrupt remapping for iommu.*/
-		iommu_set_intr_remapping(iommu, eim);
+		iommu_set_irq_remapping(iommu, eim);
 		setup = 1;
 	}
 
@@ -832,3 +816,254 @@ error:
 	return -1;
 }
 
+static void prepare_irte(struct irte *irte, int vector,
+			 unsigned int dest)
+{
+	memset(irte, 0, sizeof(*irte));
+
+	irte->present = 1;
+	irte->dst_mode = apic->irq_dest_mode;
+	/*
+	 * Trigger mode in the IRTE will always be edge, and for IO-APIC, the
+	 * actual level or edge trigger will be setup in the IO-APIC
+	 * RTE. This will help simplify level triggered irq migration.
+	 * For more details, see the comments (in io_apic.c) explainig IO-APIC
+	 * irq migration in the presence of interrupt-remapping.
+	*/
+	irte->trigger_mode = 0;
+	irte->dlvry_mode = apic->irq_delivery_mode;
+	irte->vector = vector;
+	irte->dest_id = IRTE_DEST(dest);
+	irte->redir_hint = 1;
+}
+
+static int intel_setup_ioapic_entry(int irq,
+				    struct IO_APIC_route_entry *route_entry,
+				    unsigned int destination, int vector,
+				    struct io_apic_irq_attr *attr)
+{
+	int ioapic_id = mpc_ioapic_id(attr->ioapic);
+	struct intel_iommu *iommu = map_ioapic_to_ir(ioapic_id);
+	struct IR_IO_APIC_route_entry *entry;
+	struct irte irte;
+	int index;
+
+	if (!iommu) {
+		pr_warn("No mapping iommu for ioapic %d\n", ioapic_id);
+		return -ENODEV;
+	}
+
+	entry = (struct IR_IO_APIC_route_entry *)route_entry;
+
+	index = alloc_irte(iommu, irq, 1);
+	if (index < 0) {
+		pr_warn("Failed to allocate IRTE for ioapic %d\n", ioapic_id);
+		return -ENOMEM;
+	}
+
+	prepare_irte(&irte, vector, destination);
+
+	/* Set source-id of interrupt request */
+	set_ioapic_sid(&irte, ioapic_id);
+
+	modify_irte(irq, &irte);
+
+	apic_printk(APIC_VERBOSE, KERN_DEBUG "IOAPIC[%d]: "
+		"Set IRTE entry (P:%d FPD:%d Dst_Mode:%d "
+		"Redir_hint:%d Trig_Mode:%d Dlvry_Mode:%X "
+		"Avail:%X Vector:%02X Dest:%08X "
+		"SID:%04X SQ:%X SVT:%X)\n",
+		attr->ioapic, irte.present, irte.fpd, irte.dst_mode,
+		irte.redir_hint, irte.trigger_mode, irte.dlvry_mode,
+		irte.avail, irte.vector, irte.dest_id,
+		irte.sid, irte.sq, irte.svt);
+
+	memset(entry, 0, sizeof(*entry));
+
+	entry->index2	= (index >> 15) & 0x1;
+	entry->zero	= 0;
+	entry->format	= 1;
+	entry->index	= (index & 0x7fff);
+	/*
+	 * IO-APIC RTE will be configured with virtual vector.
+	 * irq handler will do the explicit EOI to the io-apic.
+	 */
+	entry->vector	= attr->ioapic_pin;
+	entry->mask	= 0;			/* enable IRQ */
+	entry->trigger	= attr->trigger;
+	entry->polarity	= attr->polarity;
+
+	/* Mask level triggered irqs.
+	 * Use IRQ_DELAYED_DISABLE for edge triggered irqs.
+	 */
+	if (attr->trigger)
+		entry->mask = 1;
+
+	return 0;
+}
+
+#ifdef CONFIG_SMP
+/*
+ * Migrate the IO-APIC irq in the presence of intr-remapping.
+ *
+ * For both level and edge triggered, irq migration is a simple atomic
+ * update(of vector and cpu destination) of IRTE and flush the hardware cache.
+ *
+ * For level triggered, we eliminate the io-apic RTE modification (with the
+ * updated vector information), by using a virtual vector (io-apic pin number).
+ * Real vector that is used for interrupting cpu will be coming from
+ * the interrupt-remapping table entry.
+ *
+ * As the migration is a simple atomic update of IRTE, the same mechanism
+ * is used to migrate MSI irq's in the presence of interrupt-remapping.
+ */
+static int
+intel_ioapic_set_affinity(struct irq_data *data, const struct cpumask *mask,
+			  bool force)
+{
+	struct irq_cfg *cfg = data->chip_data;
+	unsigned int dest, irq = data->irq;
+	struct irte irte;
+
+	if (!cpumask_intersects(mask, cpu_online_mask))
+		return -EINVAL;
+
+	if (get_irte(irq, &irte))
+		return -EBUSY;
+
+	if (assign_irq_vector(irq, cfg, mask))
+		return -EBUSY;
+
+	dest = apic->cpu_mask_to_apicid_and(cfg->domain, mask);
+
+	irte.vector = cfg->vector;
+	irte.dest_id = IRTE_DEST(dest);
+
+	/*
+	 * Atomically updates the IRTE with the new destination, vector
+	 * and flushes the interrupt entry cache.
+	 */
+	modify_irte(irq, &irte);
+
+	/*
+	 * After this point, all the interrupts will start arriving
+	 * at the new destination. So, time to cleanup the previous
+	 * vector allocation.
+	 */
+	if (cfg->move_in_progress)
+		send_cleanup_vector(cfg);
+
+	cpumask_copy(data->affinity, mask);
+	return 0;
+}
+#endif
+
+static void intel_compose_msi_msg(struct pci_dev *pdev,
+				  unsigned int irq, unsigned int dest,
+				  struct msi_msg *msg, u8 hpet_id)
+{
+	struct irq_cfg *cfg;
+	struct irte irte;
+	u16 sub_handle = 0;
+	int ir_index;
+
+	cfg = irq_get_chip_data(irq);
+
+	ir_index = map_irq_to_irte_handle(irq, &sub_handle);
+	BUG_ON(ir_index == -1);
+
+	prepare_irte(&irte, cfg->vector, dest);
+
+	/* Set source-id of interrupt request */
+	if (pdev)
+		set_msi_sid(&irte, pdev);
+	else
+		set_hpet_sid(&irte, hpet_id);
+
+	modify_irte(irq, &irte);
+
+	msg->address_hi = MSI_ADDR_BASE_HI;
+	msg->data = sub_handle;
+	msg->address_lo = MSI_ADDR_BASE_LO | MSI_ADDR_IR_EXT_INT |
+			  MSI_ADDR_IR_SHV |
+			  MSI_ADDR_IR_INDEX1(ir_index) |
+			  MSI_ADDR_IR_INDEX2(ir_index);
+}
+
+/*
+ * Map the PCI dev to the corresponding remapping hardware unit
+ * and allocate 'nvec' consecutive interrupt-remapping table entries
+ * in it.
+ */
+static int intel_msi_alloc_irq(struct pci_dev *dev, int irq, int nvec)
+{
+	struct intel_iommu *iommu;
+	int index;
+
+	iommu = map_dev_to_ir(dev);
+	if (!iommu) {
+		printk(KERN_ERR
+		       "Unable to map PCI %s to iommu\n", pci_name(dev));
+		return -ENOENT;
+	}
+
+	index = alloc_irte(iommu, irq, nvec);
+	if (index < 0) {
+		printk(KERN_ERR
+		       "Unable to allocate %d IRTE for PCI %s\n", nvec,
+		       pci_name(dev));
+		return -ENOSPC;
+	}
+	return index;
+}
+
+static int intel_msi_setup_irq(struct pci_dev *pdev, unsigned int irq,
+			       int index, int sub_handle)
+{
+	struct intel_iommu *iommu;
+
+	iommu = map_dev_to_ir(pdev);
+	if (!iommu)
+		return -ENOENT;
+	/*
+	 * setup the mapping between the irq and the IRTE
+	 * base index, the sub_handle pointing to the
+	 * appropriate interrupt remap table entry.
+	 */
+	set_irte_irq(irq, iommu, index, sub_handle);
+
+	return 0;
+}
+
+static int intel_setup_hpet_msi(unsigned int irq, unsigned int id)
+{
+	struct intel_iommu *iommu = map_hpet_to_ir(id);
+	int index;
+
+	if (!iommu)
+		return -1;
+
+	index = alloc_irte(iommu, irq, 1);
+	if (index < 0)
+		return -1;
+
+	return 0;
+}
+
+struct irq_remap_ops intel_irq_remap_ops = {
+	.supported		= intel_irq_remapping_supported,
+	.prepare		= dmar_table_init,
+	.enable			= intel_enable_irq_remapping,
+	.disable		= disable_irq_remapping,
+	.reenable		= reenable_irq_remapping,
+	.enable_faulting	= enable_drhd_fault_handling,
+	.setup_ioapic_entry	= intel_setup_ioapic_entry,
+#ifdef CONFIG_SMP
+	.set_affinity		= intel_ioapic_set_affinity,
+#endif
+	.free_irq		= free_irte,
+	.compose_msi_msg	= intel_compose_msi_msg,
+	.msi_alloc_irq		= intel_msi_alloc_irq,
+	.msi_setup_irq		= intel_msi_setup_irq,
+	.setup_hpet_msi		= intel_setup_hpet_msi,
+};
diff --git a/drivers/iommu/intr_remapping.h b/drivers/iommu/intr_remapping.h
deleted file mode 100644
index 5662fecfee6..00000000000
--- a/drivers/iommu/intr_remapping.h
+++ /dev/null
@@ -1,17 +0,0 @@
-#include <linux/intel-iommu.h>
-
-struct ioapic_scope {
-	struct intel_iommu *iommu;
-	unsigned int id;
-	unsigned int bus;	/* PCI bus number */
-	unsigned int devfn;	/* PCI devfn number */
-};
-
-struct hpet_scope {
-	struct intel_iommu *iommu;
-	u8 id;
-	unsigned int bus;
-	unsigned int devfn;
-};
-
-#define IR_X2APIC_MODE(mode) (mode ? (1 << 11) : 0)
diff --git a/drivers/iommu/irq_remapping.c b/drivers/iommu/irq_remapping.c
new file mode 100644
index 00000000000..40cda8e98d8
--- /dev/null
+++ b/drivers/iommu/irq_remapping.c
@@ -0,0 +1,166 @@
+#include <linux/kernel.h>
+#include <linux/string.h>
+#include <linux/errno.h>
+
+#include "irq_remapping.h"
+
+int irq_remapping_enabled;
+
+int disable_irq_remap;
+int disable_sourceid_checking;
+int no_x2apic_optout;
+
+static struct irq_remap_ops *remap_ops;
+
+static __init int setup_nointremap(char *str)
+{
+	disable_irq_remap = 1;
+	return 0;
+}
+early_param("nointremap", setup_nointremap);
+
+static __init int setup_irqremap(char *str)
+{
+	if (!str)
+		return -EINVAL;
+
+	while (*str) {
+		if (!strncmp(str, "on", 2))
+			disable_irq_remap = 0;
+		else if (!strncmp(str, "off", 3))
+			disable_irq_remap = 1;
+		else if (!strncmp(str, "nosid", 5))
+			disable_sourceid_checking = 1;
+		else if (!strncmp(str, "no_x2apic_optout", 16))
+			no_x2apic_optout = 1;
+
+		str += strcspn(str, ",");
+		while (*str == ',')
+			str++;
+	}
+
+	return 0;
+}
+early_param("intremap", setup_irqremap);
+
+void __init setup_irq_remapping_ops(void)
+{
+	remap_ops = &intel_irq_remap_ops;
+}
+
+int irq_remapping_supported(void)
+{
+	if (disable_irq_remap)
+		return 0;
+
+	if (!remap_ops || !remap_ops->supported)
+		return 0;
+
+	return remap_ops->supported();
+}
+
+int __init irq_remapping_prepare(void)
+{
+	if (!remap_ops || !remap_ops->prepare)
+		return -ENODEV;
+
+	return remap_ops->prepare();
+}
+
+int __init irq_remapping_enable(void)
+{
+	if (!remap_ops || !remap_ops->enable)
+		return -ENODEV;
+
+	return remap_ops->enable();
+}
+
+void irq_remapping_disable(void)
+{
+	if (!remap_ops || !remap_ops->disable)
+		return;
+
+	remap_ops->disable();
+}
+
+int irq_remapping_reenable(int mode)
+{
+	if (!remap_ops || !remap_ops->reenable)
+		return 0;
+
+	return remap_ops->reenable(mode);
+}
+
+int __init irq_remap_enable_fault_handling(void)
+{
+	if (!remap_ops || !remap_ops->enable_faulting)
+		return -ENODEV;
+
+	return remap_ops->enable_faulting();
+}
+
+int setup_ioapic_remapped_entry(int irq,
+				struct IO_APIC_route_entry *entry,
+				unsigned int destination, int vector,
+				struct io_apic_irq_attr *attr)
+{
+	if (!remap_ops || !remap_ops->setup_ioapic_entry)
+		return -ENODEV;
+
+	return remap_ops->setup_ioapic_entry(irq, entry, destination,
+					     vector, attr);
+}
+
+#ifdef CONFIG_SMP
+int set_remapped_irq_affinity(struct irq_data *data, const struct cpumask *mask,
+			      bool force)
+{
+	if (!remap_ops || !remap_ops->set_affinity)
+		return 0;
+
+	return remap_ops->set_affinity(data, mask, force);
+}
+#endif
+
+void free_remapped_irq(int irq)
+{
+	if (!remap_ops || !remap_ops->free_irq)
+		return;
+
+	remap_ops->free_irq(irq);
+}
+
+void compose_remapped_msi_msg(struct pci_dev *pdev,
+			      unsigned int irq, unsigned int dest,
+			      struct msi_msg *msg, u8 hpet_id)
+{
+	if (!remap_ops || !remap_ops->compose_msi_msg)
+		return;
+
+	remap_ops->compose_msi_msg(pdev, irq, dest, msg, hpet_id);
+}
+
+int msi_alloc_remapped_irq(struct pci_dev *pdev, int irq, int nvec)
+{
+	if (!remap_ops || !remap_ops->msi_alloc_irq)
+		return -ENODEV;
+
+	return remap_ops->msi_alloc_irq(pdev, irq, nvec);
+}
+
+int msi_setup_remapped_irq(struct pci_dev *pdev, unsigned int irq,
+			   int index, int sub_handle)
+{
+	if (!remap_ops || !remap_ops->msi_setup_irq)
+		return -ENODEV;
+
+	return remap_ops->msi_setup_irq(pdev, irq, index, sub_handle);
+}
+
+int setup_hpet_msi_remapped(unsigned int irq, unsigned int id)
+{
+	if (!remap_ops || !remap_ops->setup_hpet_msi)
+		return -ENODEV;
+
+	return remap_ops->setup_hpet_msi(irq, id);
+}
diff --git a/drivers/iommu/irq_remapping.h b/drivers/iommu/irq_remapping.h
new file mode 100644
index 00000000000..be9d72950c5
--- /dev/null
+++ b/drivers/iommu/irq_remapping.h
@@ -0,0 +1,90 @@
+/*
+ * Copyright (C) 2012 Advanced Micro Devices, Inc.
+ * Author: Joerg Roedel <joerg.roedel@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.
+ *
+ * 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 header file contains stuff that is shared between different interrupt
+ * remapping drivers but with no need to be visible outside of the IOMMU layer.
+ */
+
+#ifndef __IRQ_REMAPPING_H
+#define __IRQ_REMAPPING_H
+
+#ifdef CONFIG_IRQ_REMAP
+
+struct IO_APIC_route_entry;
+struct io_apic_irq_attr;
+struct irq_data;
+struct cpumask;
+struct pci_dev;
+struct msi_msg;
+
+extern int disable_irq_remap;
+extern int disable_sourceid_checking;
+extern int no_x2apic_optout;
+
+struct irq_remap_ops {
+	/* Check whether Interrupt Remapping is supported */
+	int (*supported)(void);
+
+	/* Initializes hardware and makes it ready for remapping interrupts */
+	int  (*prepare)(void);
+
+	/* Enables the remapping hardware */
+	int  (*enable)(void);
+
+	/* Disables the remapping hardware */
+	void (*disable)(void);
+
+	/* Reenables the remapping hardware */
+	int  (*reenable)(int);
+
+	/* Enable fault handling */
+	int  (*enable_faulting)(void);
+
+	/* IO-APIC setup routine */
+	int (*setup_ioapic_entry)(int irq, struct IO_APIC_route_entry *,
+				  unsigned int, int,
+				  struct io_apic_irq_attr *);
+
+#ifdef CONFIG_SMP
+	/* Set the CPU affinity of a remapped interrupt */
+	int (*set_affinity)(struct irq_data *data, const struct cpumask *mask,
+			    bool force);
+#endif
+
+	/* Free an IRQ */
+	int (*free_irq)(int);
+
+	/* Create MSI msg to use for interrupt remapping */
+	void (*compose_msi_msg)(struct pci_dev *,
+				unsigned int, unsigned int,
+				struct msi_msg *, u8);
+
+	/* Allocate remapping resources for MSI */
+	int (*msi_alloc_irq)(struct pci_dev *, int, int);
+
+	/* Setup the remapped MSI irq */
+	int (*msi_setup_irq)(struct pci_dev *, unsigned int, int, int);
+
+	/* Setup interrupt remapping for an HPET MSI */
+	int (*setup_hpet_msi)(unsigned int, unsigned int);
+};
+
+extern struct irq_remap_ops intel_irq_remap_ops;
+
+#endif /* CONFIG_IRQ_REMAP */
+
+#endif /* __IRQ_REMAPPING_H */
diff --git a/drivers/isdn/capi/capi.c b/drivers/isdn/capi/capi.c
index b902794bbf0..38c4bd87b2c 100644
--- a/drivers/isdn/capi/capi.c
+++ b/drivers/isdn/capi/capi.c
@@ -336,11 +336,6 @@ static inline void
 capincci_alloc_minor(struct capidev *cdev, struct capincci *np) { }
 static inline void capincci_free_minor(struct capincci *np) { }
 
-static inline unsigned int capincci_minor_opencount(struct capincci *np)
-{
-	return 0;
-}
-
 #endif /* !CONFIG_ISDN_CAPI_MIDDLEWARE */
 
 static struct capincci *capincci_alloc(struct capidev *cdev, u32 ncci)
@@ -372,6 +367,7 @@ static void capincci_free(struct capidev *cdev, u32 ncci)
 		}
 }
 
+#ifdef CONFIG_ISDN_CAPI_MIDDLEWARE
 static struct capincci *capincci_find(struct capidev *cdev, u32 ncci)
 {
 	struct capincci *np;
@@ -382,7 +378,6 @@ static struct capincci *capincci_find(struct capidev *cdev, u32 ncci)
 	return NULL;
 }
 
-#ifdef CONFIG_ISDN_CAPI_MIDDLEWARE
 /* -------- handle data queue --------------------------------------- */
 
 static struct sk_buff *
@@ -578,8 +573,8 @@ static void capi_recv_message(struct capi20_appl *ap, struct sk_buff *skb)
 	struct tty_struct *tty;
 	struct capiminor *mp;
 	u16 datahandle;
-#endif /* CONFIG_ISDN_CAPI_MIDDLEWARE */
 	struct capincci *np;
+#endif /* CONFIG_ISDN_CAPI_MIDDLEWARE */
 
 	mutex_lock(&cdev->lock);
 
@@ -597,6 +592,12 @@ static void capi_recv_message(struct capi20_appl *ap, struct sk_buff *skb)
 		goto unlock_out;
 	}
 
+#ifndef CONFIG_ISDN_CAPI_MIDDLEWARE
+	skb_queue_tail(&cdev->recvqueue, skb);
+	wake_up_interruptible(&cdev->recvwait);
+
+#else /* CONFIG_ISDN_CAPI_MIDDLEWARE */
+
 	np = capincci_find(cdev, CAPIMSG_CONTROL(skb->data));
 	if (!np) {
 		printk(KERN_ERR "BUG: capi_signal: ncci not found\n");
@@ -605,12 +606,6 @@ static void capi_recv_message(struct capi20_appl *ap, struct sk_buff *skb)
 		goto unlock_out;
 	}
 
-#ifndef CONFIG_ISDN_CAPI_MIDDLEWARE
-	skb_queue_tail(&cdev->recvqueue, skb);
-	wake_up_interruptible(&cdev->recvwait);
-
-#else /* CONFIG_ISDN_CAPI_MIDDLEWARE */
-
 	mp = np->minorp;
 	if (!mp) {
 		skb_queue_tail(&cdev->recvqueue, skb);
@@ -786,7 +781,6 @@ register_out:
 		return retval;
 
 	case CAPI_GET_VERSION:
-	{
 		if (copy_from_user(&data.contr, argp,
 				   sizeof(data.contr)))
 			return -EFAULT;
@@ -796,11 +790,9 @@ register_out:
 		if (copy_to_user(argp, &data.version,
 				 sizeof(data.version)))
 			return -EFAULT;
-	}
-	return 0;
+		return 0;
 
 	case CAPI_GET_SERIAL:
-	{
 		if (copy_from_user(&data.contr, argp,
 				   sizeof(data.contr)))
 			return -EFAULT;
@@ -810,10 +802,9 @@ register_out:
 		if (copy_to_user(argp, data.serial,
 				 sizeof(data.serial)))
 			return -EFAULT;
-	}
-	return 0;
+		return 0;
+
 	case CAPI_GET_PROFILE:
-	{
 		if (copy_from_user(&data.contr, argp,
 				   sizeof(data.contr)))
 			return -EFAULT;
@@ -837,11 +828,9 @@ register_out:
 		}
 		if (retval)
 			return -EFAULT;
-	}
-	return 0;
+		return 0;
 
 	case CAPI_GET_MANUFACTURER:
-	{
 		if (copy_from_user(&data.contr, argp,
 				   sizeof(data.contr)))
 			return -EFAULT;
@@ -853,8 +842,8 @@ register_out:
 				 sizeof(data.manufacturer)))
 			return -EFAULT;
 
-	}
-	return 0;
+		return 0;
+
 	case CAPI_GET_ERRCODE:
 		data.errcode = cdev->errcode;
 		cdev->errcode = CAPI_NOERROR;
@@ -870,8 +859,7 @@ register_out:
 			return 0;
 		return -ENXIO;
 
-	case CAPI_MANUFACTURER_CMD:
-	{
+	case CAPI_MANUFACTURER_CMD: {
 		struct capi_manufacturer_cmd mcmd;
 		if (!capable(CAP_SYS_ADMIN))
 			return -EPERM;
@@ -879,8 +867,6 @@ register_out:
 			return -EFAULT;
 		return capi20_manufacturer(mcmd.cmd, mcmd.data);
 	}
-	return 0;
-
 	case CAPI_SET_FLAGS:
 	case CAPI_CLR_FLAGS: {
 		unsigned userflags;
@@ -902,6 +888,11 @@ register_out:
 			return -EFAULT;
 		return 0;
 
+#ifndef CONFIG_ISDN_CAPI_MIDDLEWARE
+	case CAPI_NCCI_OPENCOUNT:
+		return 0;
+
+#else /* CONFIG_ISDN_CAPI_MIDDLEWARE */
 	case CAPI_NCCI_OPENCOUNT: {
 		struct capincci *nccip;
 		unsigned ncci;
@@ -918,7 +909,6 @@ register_out:
 		return count;
 	}
 
-#ifdef CONFIG_ISDN_CAPI_MIDDLEWARE
 	case CAPI_NCCI_GETUNIT: {
 		struct capincci *nccip;
 		struct capiminor *mp;
diff --git a/drivers/isdn/capi/capidrv.c b/drivers/isdn/capi/capidrv.c
index 6f5016b479f..832bc807ed2 100644
--- a/drivers/isdn/capi/capidrv.c
+++ b/drivers/isdn/capi/capidrv.c
@@ -1593,7 +1593,7 @@ static int capidrv_command(isdn_ctrl *c, capidrv_contr *card)
 		return capidrv_ioctl(c, card);
 
 	switch (c->command) {
-	case ISDN_CMD_DIAL:{
+	case ISDN_CMD_DIAL: {
 		u8 calling[ISDN_MSNLEN + 3];
 		u8 called[ISDN_MSNLEN + 2];
 
@@ -2072,7 +2072,8 @@ static int capidrv_addcontr(u16 contr, struct capi_profile *profp)
 	card->interface.writebuf_skb = if_sendbuf;
 	card->interface.writecmd = NULL;
 	card->interface.readstat = if_readstat;
-	card->interface.features = ISDN_FEATURE_L2_HDLC |
+	card->interface.features =
+		ISDN_FEATURE_L2_HDLC |
 		ISDN_FEATURE_L2_TRANS |
 		ISDN_FEATURE_L3_TRANS |
 		ISDN_FEATURE_P_UNKNOWN |
@@ -2080,7 +2081,8 @@ static int capidrv_addcontr(u16 contr, struct capi_profile *profp)
 		ISDN_FEATURE_L2_X75UI |
 		ISDN_FEATURE_L2_X75BUI;
 	if (profp->support1 & (1 << 2))
-		card->interface.features |= ISDN_FEATURE_L2_V11096 |
+		card->interface.features |=
+			ISDN_FEATURE_L2_V11096 |
 			ISDN_FEATURE_L2_V11019 |
 			ISDN_FEATURE_L2_V11038;
 	if (profp->support1 & (1 << 8))
diff --git a/drivers/isdn/gigaset/bas-gigaset.c b/drivers/isdn/gigaset/bas-gigaset.c
index afa080258bf..52758870894 100644
--- a/drivers/isdn/gigaset/bas-gigaset.c
+++ b/drivers/isdn/gigaset/bas-gigaset.c
@@ -148,6 +148,7 @@ static struct usb_driver gigaset_usb_driver = {
 	.reset_resume =	gigaset_post_reset,
 	.pre_reset =	gigaset_pre_reset,
 	.post_reset =	gigaset_post_reset,
+	.disable_hub_initiated_lpm = 1,
 };
 
 /* get message text for usb_submit_urb return code
@@ -410,10 +411,10 @@ static void check_pending(struct bas_cardstate *ucs)
 		if (!(ucs->basstate & BS_RESETTING))
 			ucs->pending = 0;
 		break;
-		/*
-		 * HD_READ_ATMESSAGE and HD_WRITE_ATMESSAGE are handled separately
-		 * and should never end up here
-		 */
+	/*
+	 * HD_READ_ATMESSAGE and HD_WRITE_ATMESSAGE are handled separately
+	 * and should never end up here
+	 */
 	default:
 		dev_warn(&ucs->interface->dev,
 			 "unknown pending request 0x%02x cleared\n",
@@ -877,8 +878,7 @@ static void read_iso_callback(struct urb *urb)
 		for (i = 0; i < BAS_NUMFRAMES; i++) {
 			ubc->isoinlost += urb->iso_frame_desc[i].actual_length;
 			if (unlikely(urb->iso_frame_desc[i].status != 0 &&
-				     urb->iso_frame_desc[i].status !=
-				     -EINPROGRESS))
+				     urb->iso_frame_desc[i].status != -EINPROGRESS))
 				ubc->loststatus = urb->iso_frame_desc[i].status;
 			urb->iso_frame_desc[i].status = 0;
 			urb->iso_frame_desc[i].actual_length = 0;
@@ -2078,16 +2078,14 @@ static int gigaset_brkchars(struct cardstate *cs, const unsigned char buf[6])
 /* Free hardware dependent part of the B channel structure
  * parameter:
  *	bcs	B channel structure
- * return value:
- *	!=0 on success
  */
-static int gigaset_freebcshw(struct bc_state *bcs)
+static void gigaset_freebcshw(struct bc_state *bcs)
 {
 	struct bas_bc_state *ubc = bcs->hw.bas;
 	int i;
 
 	if (!ubc)
-		return 0;
+		return;
 
 	/* kill URBs and tasklets before freeing - better safe than sorry */
 	ubc->running = 0;
@@ -2105,14 +2103,13 @@ static int gigaset_freebcshw(struct bc_state *bcs)
 	kfree(ubc->isooutbuf);
 	kfree(ubc);
 	bcs->hw.bas = NULL;
-	return 1;
 }
 
 /* Initialize hardware dependent part of the B channel structure
  * parameter:
  *	bcs	B channel structure
  * return value:
- *	!=0 on success
+ *	0 on success, error code < 0 on failure
  */
 static int gigaset_initbcshw(struct bc_state *bcs)
 {
@@ -2122,7 +2119,7 @@ static int gigaset_initbcshw(struct bc_state *bcs)
 	bcs->hw.bas = ubc = kmalloc(sizeof(struct bas_bc_state), GFP_KERNEL);
 	if (!ubc) {
 		pr_err("out of memory\n");
-		return 0;
+		return -ENOMEM;
 	}
 
 	ubc->running = 0;
@@ -2139,7 +2136,7 @@ static int gigaset_initbcshw(struct bc_state *bcs)
 		pr_err("out of memory\n");
 		kfree(ubc);
 		bcs->hw.bas = NULL;
-		return 0;
+		return -ENOMEM;
 	}
 	tasklet_init(&ubc->sent_tasklet,
 		     write_iso_tasklet, (unsigned long) bcs);
@@ -2164,7 +2161,7 @@ static int gigaset_initbcshw(struct bc_state *bcs)
 	ubc->stolen0s = 0;
 	tasklet_init(&ubc->rcvd_tasklet,
 		     read_iso_tasklet, (unsigned long) bcs);
-	return 1;
+	return 0;
 }
 
 static void gigaset_reinitbcshw(struct bc_state *bcs)
@@ -2187,6 +2184,12 @@ static void gigaset_freecshw(struct cardstate *cs)
 	cs->hw.bas = NULL;
 }
 
+/* Initialize hardware dependent part of the cardstate structure
+ * parameter:
+ *	cs	cardstate structure
+ * return value:
+ *	0 on success, error code < 0 on failure
+ */
 static int gigaset_initcshw(struct cardstate *cs)
 {
 	struct bas_cardstate *ucs;
@@ -2194,13 +2197,13 @@ static int gigaset_initcshw(struct cardstate *cs)
 	cs->hw.bas = ucs = kmalloc(sizeof *ucs, GFP_KERNEL);
 	if (!ucs) {
 		pr_err("out of memory\n");
-		return 0;
+		return -ENOMEM;
 	}
 	ucs->int_in_buf = kmalloc(IP_MSGSIZE, GFP_KERNEL);
 	if (!ucs->int_in_buf) {
 		kfree(ucs);
 		pr_err("out of memory\n");
-		return 0;
+		return -ENOMEM;
 	}
 
 	ucs->urb_cmd_in = NULL;
@@ -2219,7 +2222,7 @@ static int gigaset_initcshw(struct cardstate *cs)
 	init_waitqueue_head(&ucs->waitqueue);
 	INIT_WORK(&ucs->int_in_wq, int_in_work);
 
-	return 1;
+	return 0;
 }
 
 /* freeurbs
@@ -2379,18 +2382,20 @@ static int gigaset_probe(struct usb_interface *interface,
 	/* save address of controller structure */
 	usb_set_intfdata(interface, cs);
 
-	if (!gigaset_start(cs))
+	rc = gigaset_start(cs);
+	if (rc < 0)
 		goto error;
 
 	return 0;
 
 allocerr:
 	dev_err(cs->dev, "could not allocate URBs\n");
+	rc = -ENOMEM;
 error:
 	freeurbs(cs);
 	usb_set_intfdata(interface, NULL);
 	gigaset_freecs(cs);
-	return -ENODEV;
+	return rc;
 }
 
 /* gigaset_disconnect
diff --git a/drivers/isdn/gigaset/capi.c b/drivers/isdn/gigaset/capi.c
index 343b5c80cb7..27e4a3e21d6 100644
--- a/drivers/isdn/gigaset/capi.c
+++ b/drivers/isdn/gigaset/capi.c
@@ -14,6 +14,7 @@
 #include "gigaset.h"
 #include <linux/proc_fs.h>
 #include <linux/seq_file.h>
+#include <linux/ratelimit.h>
 #include <linux/isdn/capilli.h>
 #include <linux/isdn/capicmd.h>
 #include <linux/isdn/capiutil.h>
@@ -108,51 +109,35 @@ static struct {
 	u8 *bc;
 	u8 *hlc;
 } cip2bchlc[] = {
-	[1] = { "8090A3", NULL },
-	/* Speech (A-law) */
-	[2] = { "8890", NULL },
-	/* Unrestricted digital information */
-	[3] = { "8990", NULL },
-	/* Restricted digital information */
-	[4] = { "9090A3", NULL },
-	/* 3,1 kHz audio (A-law) */
-	[5] = { "9190", NULL },
-	/* 7 kHz audio */
-	[6] = { "9890", NULL },
-	/* Video */
-	[7] = { "88C0C6E6", NULL },
-	/* Packet mode */
-	[8] = { "8890218F", NULL },
-	/* 56 kbit/s rate adaptation */
-	[9] = { "9190A5", NULL },
-	/* Unrestricted digital information with tones/announcements */
-	[16] = { "8090A3", "9181" },
-	/* Telephony */
-	[17] = { "9090A3", "9184" },
-	/* Group 2/3 facsimile */
-	[18] = { "8890", "91A1" },
-	/* Group 4 facsimile Class 1 */
-	[19] = { "8890", "91A4" },
-	/* Teletex service basic and mixed mode
-	   and Group 4 facsimile service Classes II and III */
-	[20] = { "8890", "91A8" },
-	/* Teletex service basic and processable mode */
-	[21] = { "8890", "91B1" },
-	/* Teletex service basic mode */
-	[22] = { "8890", "91B2" },
-	/* International interworking for Videotex */
-	[23] = { "8890", "91B5" },
-	/* Telex */
-	[24] = { "8890", "91B8" },
-	/* Message Handling Systems in accordance with X.400 */
-	[25] = { "8890", "91C1" },
-	/* OSI application in accordance with X.200 */
-	[26] = { "9190A5", "9181" },
-	/* 7 kHz telephony */
-	[27] = { "9190A5", "916001" },
-	/* Video telephony, first connection */
-	[28] = { "8890", "916002" },
-	/* Video telephony, second connection */
+	[1] = { "8090A3", NULL },	/* Speech (A-law) */
+	[2] = { "8890", NULL },		/* Unrestricted digital information */
+	[3] = { "8990", NULL },		/* Restricted digital information */
+	[4] = { "9090A3", NULL },	/* 3,1 kHz audio (A-law) */
+	[5] = { "9190", NULL },		/* 7 kHz audio */
+	[6] = { "9890", NULL },		/* Video */
+	[7] = { "88C0C6E6", NULL },	/* Packet mode */
+	[8] = { "8890218F", NULL },	/* 56 kbit/s rate adaptation */
+	[9] = { "9190A5", NULL },	/* Unrestricted digital information
+					 * with tones/announcements */
+	[16] = { "8090A3", "9181" },	/* Telephony */
+	[17] = { "9090A3", "9184" },	/* Group 2/3 facsimile */
+	[18] = { "8890", "91A1" },	/* Group 4 facsimile Class 1 */
+	[19] = { "8890", "91A4" },	/* Teletex service basic and mixed mode
+					 * and Group 4 facsimile service
+					 * Classes II and III */
+	[20] = { "8890", "91A8" },	/* Teletex service basic and
+					 * processable mode */
+	[21] = { "8890", "91B1" },	/* Teletex service basic mode */
+	[22] = { "8890", "91B2" },	/* International interworking for
+					 * Videotex */
+	[23] = { "8890", "91B5" },	/* Telex */
+	[24] = { "8890", "91B8" },	/* Message Handling Systems
+					 * in accordance with X.400 */
+	[25] = { "8890", "91C1" },	/* OSI application
+					 * in accordance with X.200 */
+	[26] = { "9190A5", "9181" },	/* 7 kHz telephony */
+	[27] = { "9190A5", "916001" },	/* Video telephony, first connection */
+	[28] = { "8890", "916002" },	/* Video telephony, second connection */
 };
 
 /*
@@ -223,10 +208,14 @@ get_appl(struct gigaset_capi_ctr *iif, u16 appl)
 static inline void dump_cmsg(enum debuglevel level, const char *tag, _cmsg *p)
 {
 #ifdef CONFIG_GIGASET_DEBUG
+	/* dump at most 20 messages in 20 secs */
+	static DEFINE_RATELIMIT_STATE(msg_dump_ratelimit, 20 * HZ, 20);
 	_cdebbuf *cdb;
 
 	if (!(gigaset_debuglevel & level))
 		return;
+	if (!___ratelimit(&msg_dump_ratelimit, tag))
+		return;
 
 	cdb = capi_cmsg2str(p);
 	if (cdb) {
@@ -1192,7 +1181,9 @@ static void do_facility_req(struct gigaset_capi_ctr *iif,
 			confparam[3] = 2;	/* length */
 			capimsg_setu16(confparam, 4, CapiSuccess);
 			break;
-			/* ToDo: add supported services */
+
+		/* ToDo: add supported services */
+
 		default:
 			dev_notice(cs->dev,
 				   "%s: unsupported supplementary service function 0x%04x\n",
@@ -1766,7 +1757,8 @@ static void do_connect_b3_req(struct gigaset_capi_ctr *iif,
 
 	/* NCPI parameter: not applicable for B3 Transparent */
 	ignore_cstruct_param(cs, cmsg->NCPI, "CONNECT_B3_REQ", "NCPI");
-	send_conf(iif, ap, skb, (cmsg->NCPI && cmsg->NCPI[0]) ?
+	send_conf(iif, ap, skb,
+		  (cmsg->NCPI && cmsg->NCPI[0]) ?
 		  CapiNcpiNotSupportedByProtocol : CapiSuccess);
 }
 
@@ -1882,6 +1874,9 @@ static void do_disconnect_req(struct gigaset_capi_ctr *iif,
 
 	/* check for active logical connection */
 	if (bcs->apconnstate >= APCONN_ACTIVE) {
+		/* clear it */
+		bcs->apconnstate = APCONN_SETUP;
+
 		/*
 		 * emit DISCONNECT_B3_IND with cause 0x3301
 		 * use separate cmsg structure, as the content of iif->acmsg
@@ -1906,6 +1901,7 @@ static void do_disconnect_req(struct gigaset_capi_ctr *iif,
 		}
 		capi_cmsg2message(b3cmsg,
 				  __skb_put(b3skb, CAPI_DISCONNECT_B3_IND_BASELEN));
+		dump_cmsg(DEBUG_CMD, __func__, b3cmsg);
 		kfree(b3cmsg);
 		capi_ctr_handle_message(&iif->ctr, ap->id, b3skb);
 	}
@@ -1966,7 +1962,8 @@ static void do_disconnect_b3_req(struct gigaset_capi_ctr *iif,
 	/* NCPI parameter: not applicable for B3 Transparent */
 	ignore_cstruct_param(cs, cmsg->NCPI,
 			     "DISCONNECT_B3_REQ", "NCPI");
-	send_conf(iif, ap, skb, (cmsg->NCPI && cmsg->NCPI[0]) ?
+	send_conf(iif, ap, skb,
+		  (cmsg->NCPI && cmsg->NCPI[0]) ?
 		  CapiNcpiNotSupportedByProtocol : CapiSuccess);
 }
 
@@ -2059,12 +2056,6 @@ static void do_reset_b3_req(struct gigaset_capi_ctr *iif,
 }
 
 /*
- * dump unsupported/ignored messages at most twice per minute,
- * some apps send those very frequently
- */
-static unsigned long ignored_msg_dump_time;
-
-/*
  * unsupported CAPI message handler
  */
 static void do_unsupported(struct gigaset_capi_ctr *iif,
@@ -2073,8 +2064,7 @@ static void do_unsupported(struct gigaset_capi_ctr *iif,
 {
 	/* decode message */
 	capi_message2cmsg(&iif->acmsg, skb->data);
-	if (printk_timed_ratelimit(&ignored_msg_dump_time, 30 * 1000))
-		dump_cmsg(DEBUG_CMD, __func__, &iif->acmsg);
+	dump_cmsg(DEBUG_CMD, __func__, &iif->acmsg);
 	send_conf(iif, ap, skb, CapiMessageNotSupportedInCurrentState);
 }
 
@@ -2085,11 +2075,9 @@ static void do_nothing(struct gigaset_capi_ctr *iif,
 		       struct gigaset_capi_appl *ap,
 		       struct sk_buff *skb)
 {
-	if (printk_timed_ratelimit(&ignored_msg_dump_time, 30 * 1000)) {
-		/* decode message */
-		capi_message2cmsg(&iif->acmsg, skb->data);
-		dump_cmsg(DEBUG_CMD, __func__, &iif->acmsg);
-	}
+	/* decode message */
+	capi_message2cmsg(&iif->acmsg, skb->data);
+	dump_cmsg(DEBUG_CMD, __func__, &iif->acmsg);
 	dev_kfree_skb_any(skb);
 }
 
@@ -2358,7 +2346,7 @@ static const struct file_operations gigaset_proc_fops = {
  * @cs:		device descriptor structure.
  * @isdnid:	device name.
  *
- * Return value: 1 for success, 0 for failure
+ * Return value: 0 on success, error code < 0 on failure
  */
 int gigaset_isdn_regdev(struct cardstate *cs, const char *isdnid)
 {
@@ -2368,7 +2356,7 @@ int gigaset_isdn_regdev(struct cardstate *cs, const char *isdnid)
 	iif = kmalloc(sizeof(*iif), GFP_KERNEL);
 	if (!iif) {
 		pr_err("%s: out of memory\n", __func__);
-		return 0;
+		return -ENOMEM;
 	}
 
 	/* prepare controller structure */
@@ -2392,12 +2380,12 @@ int gigaset_isdn_regdev(struct cardstate *cs, const char *isdnid)
 	if (rc) {
 		pr_err("attach_capi_ctr failed (%d)\n", rc);
 		kfree(iif);
-		return 0;
+		return rc;
 	}
 
 	cs->iif = iif;
 	cs->hw_hdr_len = CAPI_DATA_B3_REQ_LEN;
-	return 1;
+	return 0;
 }
 
 /**
diff --git a/drivers/isdn/gigaset/common.c b/drivers/isdn/gigaset/common.c
index 76792707f99..aa41485bc59 100644
--- a/drivers/isdn/gigaset/common.c
+++ b/drivers/isdn/gigaset/common.c
@@ -194,13 +194,13 @@ int gigaset_get_channel(struct bc_state *bcs)
 		gig_dbg(DEBUG_CHANNEL, "could not allocate channel %d",
 			bcs->channel);
 		spin_unlock_irqrestore(&bcs->cs->lock, flags);
-		return 0;
+		return -EBUSY;
 	}
 	++bcs->use_count;
 	bcs->busy = 1;
 	gig_dbg(DEBUG_CHANNEL, "allocated channel %d", bcs->channel);
 	spin_unlock_irqrestore(&bcs->cs->lock, flags);
-	return 1;
+	return 0;
 }
 
 struct bc_state *gigaset_get_free_channel(struct cardstate *cs)
@@ -258,7 +258,7 @@ int gigaset_get_channels(struct cardstate *cs)
 			spin_unlock_irqrestore(&cs->lock, flags);
 			gig_dbg(DEBUG_CHANNEL,
 				"could not allocate all channels");
-			return 0;
+			return -EBUSY;
 		}
 	for (i = 0; i < cs->channels; ++i)
 		++cs->bcs[i].use_count;
@@ -266,7 +266,7 @@ int gigaset_get_channels(struct cardstate *cs)
 
 	gig_dbg(DEBUG_CHANNEL, "allocated all channels");
 
-	return 1;
+	return 0;
 }
 
 void gigaset_free_channels(struct cardstate *cs)
@@ -362,7 +362,7 @@ struct event_t *gigaset_add_event(struct cardstate *cs,
 }
 EXPORT_SYMBOL_GPL(gigaset_add_event);
 
-static void free_strings(struct at_state_t *at_state)
+static void clear_at_state(struct at_state_t *at_state)
 {
 	int i;
 
@@ -372,18 +372,13 @@ static void free_strings(struct at_state_t *at_state)
 	}
 }
 
-static void clear_at_state(struct at_state_t *at_state)
-{
-	free_strings(at_state);
-}
-
-static void dealloc_at_states(struct cardstate *cs)
+static void dealloc_temp_at_states(struct cardstate *cs)
 {
 	struct at_state_t *cur, *next;
 
 	list_for_each_entry_safe(cur, next, &cs->temp_at_states, list) {
 		list_del(&cur->list);
-		free_strings(cur);
+		clear_at_state(cur);
 		kfree(cur);
 	}
 }
@@ -393,8 +388,7 @@ static void gigaset_freebcs(struct bc_state *bcs)
 	int i;
 
 	gig_dbg(DEBUG_INIT, "freeing bcs[%d]->hw", bcs->channel);
-	if (!bcs->cs->ops->freebcshw(bcs))
-		gig_dbg(DEBUG_INIT, "failed");
+	bcs->cs->ops->freebcshw(bcs);
 
 	gig_dbg(DEBUG_INIT, "clearing bcs[%d]->at_state", bcs->channel);
 	clear_at_state(&bcs->at_state);
@@ -512,7 +506,7 @@ void gigaset_freecs(struct cardstate *cs)
 	case 1: /* error when registering to LL */
 		gig_dbg(DEBUG_INIT, "clearing at_state");
 		clear_at_state(&cs->at_state);
-		dealloc_at_states(cs);
+		dealloc_temp_at_states(cs);
 
 		/* fall through */
 	case 0:	/* error in basic setup */
@@ -571,6 +565,8 @@ static void gigaset_inbuf_init(struct inbuf_t *inbuf, struct cardstate *cs)
  * @inbuf:	buffer structure.
  * @src:	received data.
  * @numbytes:	number of bytes received.
+ *
+ * Return value: !=0 if some data was appended
  */
 int gigaset_fill_inbuf(struct inbuf_t *inbuf, const unsigned char *src,
 		       unsigned numbytes)
@@ -614,8 +610,8 @@ int gigaset_fill_inbuf(struct inbuf_t *inbuf, const unsigned char *src,
 EXPORT_SYMBOL_GPL(gigaset_fill_inbuf);
 
 /* Initialize the b-channel structure */
-static struct bc_state *gigaset_initbcs(struct bc_state *bcs,
-					struct cardstate *cs, int channel)
+static int gigaset_initbcs(struct bc_state *bcs, struct cardstate *cs,
+			   int channel)
 {
 	int i;
 
@@ -654,11 +650,7 @@ static struct bc_state *gigaset_initbcs(struct bc_state *bcs,
 	bcs->apconnstate = 0;
 
 	gig_dbg(DEBUG_INIT, "  setting up bcs[%d]->hw", channel);
-	if (cs->ops->initbcshw(bcs))
-		return bcs;
-
-	gig_dbg(DEBUG_INIT, "  failed");
-	return NULL;
+	return cs->ops->initbcshw(bcs);
 }
 
 /**
@@ -757,7 +749,7 @@ struct cardstate *gigaset_initcs(struct gigaset_driver *drv, int channels,
 	cs->cmdbytes = 0;
 
 	gig_dbg(DEBUG_INIT, "setting up iif");
-	if (!gigaset_isdn_regdev(cs, modulename)) {
+	if (gigaset_isdn_regdev(cs, modulename) < 0) {
 		pr_err("error registering ISDN device\n");
 		goto error;
 	}
@@ -765,7 +757,7 @@ struct cardstate *gigaset_initcs(struct gigaset_driver *drv, int channels,
 	make_valid(cs, VALID_ID);
 	++cs->cs_init;
 	gig_dbg(DEBUG_INIT, "setting up hw");
-	if (!cs->ops->initcshw(cs))
+	if (cs->ops->initcshw(cs) < 0)
 		goto error;
 
 	++cs->cs_init;
@@ -779,7 +771,7 @@ struct cardstate *gigaset_initcs(struct gigaset_driver *drv, int channels,
 	/* set up channel data structures */
 	for (i = 0; i < channels; ++i) {
 		gig_dbg(DEBUG_INIT, "setting up bcs[%d]", i);
-		if (!gigaset_initbcs(cs->bcs + i, cs, i)) {
+		if (gigaset_initbcs(cs->bcs + i, cs, i) < 0) {
 			pr_err("could not allocate channel %d data\n", i);
 			goto error;
 		}
@@ -848,8 +840,7 @@ static void cleanup_cs(struct cardstate *cs)
 	cs->mstate = MS_UNINITIALIZED;
 
 	clear_at_state(&cs->at_state);
-	dealloc_at_states(cs);
-	free_strings(&cs->at_state);
+	dealloc_temp_at_states(cs);
 	gigaset_at_init(&cs->at_state, NULL, cs, 0);
 
 	cs->inbuf->inputstate = INS_command;
@@ -875,7 +866,7 @@ static void cleanup_cs(struct cardstate *cs)
 
 	for (i = 0; i < cs->channels; ++i) {
 		gigaset_freebcs(cs->bcs + i);
-		if (!gigaset_initbcs(cs->bcs + i, cs, i))
+		if (gigaset_initbcs(cs->bcs + i, cs, i) < 0)
 			pr_err("could not allocate channel %d data\n", i);
 	}
 
@@ -896,14 +887,14 @@ static void cleanup_cs(struct cardstate *cs)
  * waiting for completion of the initialization.
  *
  * Return value:
- *	1 - success, 0 - error
+ *	0 on success, error code < 0 on failure
  */
 int gigaset_start(struct cardstate *cs)
 {
 	unsigned long flags;
 
 	if (mutex_lock_interruptible(&cs->mutex))
-		return 0;
+		return -EBUSY;
 
 	spin_lock_irqsave(&cs->lock, flags);
 	cs->connected = 1;
@@ -927,11 +918,11 @@ int gigaset_start(struct cardstate *cs)
 	wait_event(cs->waitqueue, !cs->waiting);
 
 	mutex_unlock(&cs->mutex);
-	return 1;
+	return 0;
 
 error:
 	mutex_unlock(&cs->mutex);
-	return 0;
+	return -ENOMEM;
 }
 EXPORT_SYMBOL_GPL(gigaset_start);
 
@@ -943,7 +934,7 @@ EXPORT_SYMBOL_GPL(gigaset_start);
  * waiting for completion of the shutdown.
  *
  * Return value:
- *	0 - success, -1 - error (no device associated)
+ *	0 - success, -ENODEV - error (no device associated)
  */
 int gigaset_shutdown(struct cardstate *cs)
 {
@@ -951,7 +942,7 @@ int gigaset_shutdown(struct cardstate *cs)
 
 	if (!(cs->flags & VALID_MINOR)) {
 		mutex_unlock(&cs->mutex);
-		return -1;
+		return -ENODEV;
 	}
 
 	cs->waiting = 1;
diff --git a/drivers/isdn/gigaset/dummyll.c b/drivers/isdn/gigaset/dummyll.c
index 19b1c779d50..570c2d53b84 100644
--- a/drivers/isdn/gigaset/dummyll.c
+++ b/drivers/isdn/gigaset/dummyll.c
@@ -60,7 +60,7 @@ void gigaset_isdn_stop(struct cardstate *cs)
 
 int gigaset_isdn_regdev(struct cardstate *cs, const char *isdnid)
 {
-	return 1;
+	return 0;
 }
 
 void gigaset_isdn_unregdev(struct cardstate *cs)
diff --git a/drivers/isdn/gigaset/ev-layer.c b/drivers/isdn/gigaset/ev-layer.c
index 624a8256a77..2e6963dc740 100644
--- a/drivers/isdn/gigaset/ev-layer.c
+++ b/drivers/isdn/gigaset/ev-layer.c
@@ -153,103 +153,104 @@ struct reply_t gigaset_tab_nocid[] =
  * action, command */
 
 /* initialize device, set cid mode if possible */
-	{RSP_INIT,	 -1,  -1, SEQ_INIT,		100,  1, {ACT_TIMEOUT} },
+	{RSP_INIT,	 -1,  -1, SEQ_INIT,	100,  1, {ACT_TIMEOUT} },
 
-	{EV_TIMEOUT,	100, 100, -1,			101,  3, {0},	"Z\r"},
-	{RSP_OK,	101, 103, -1,			120,  5, {ACT_GETSTRING},
-	 "+GMR\r"},
+	{EV_TIMEOUT,	100, 100, -1,		101,  3, {0},	"Z\r"},
+	{RSP_OK,	101, 103, -1,		120,  5, {ACT_GETSTRING},
+								"+GMR\r"},
 
-	{EV_TIMEOUT,	101, 101, -1,			102,  5, {0},	"Z\r"},
-	{RSP_ERROR,	101, 101, -1,			102,  5, {0},	"Z\r"},
+	{EV_TIMEOUT,	101, 101, -1,		102,  5, {0},	"Z\r"},
+	{RSP_ERROR,	101, 101, -1,		102,  5, {0},	"Z\r"},
 
-	{EV_TIMEOUT,	102, 102, -1,			108,  5, {ACT_SETDLE1},
-	 "^SDLE=0\r"},
-	{RSP_OK,	108, 108, -1,			104, -1},
-	{RSP_ZDLE,	104, 104,  0,			103,  5, {0},	"Z\r"},
-	{EV_TIMEOUT,	104, 104, -1,			  0,  0, {ACT_FAILINIT} },
-	{RSP_ERROR,	108, 108, -1,			  0,  0, {ACT_FAILINIT} },
+	{EV_TIMEOUT,	102, 102, -1,		108,  5, {ACT_SETDLE1},
+								"^SDLE=0\r"},
+	{RSP_OK,	108, 108, -1,		104, -1},
+	{RSP_ZDLE,	104, 104,  0,		103,  5, {0},	"Z\r"},
+	{EV_TIMEOUT,	104, 104, -1,		  0,  0, {ACT_FAILINIT} },
+	{RSP_ERROR,	108, 108, -1,		  0,  0, {ACT_FAILINIT} },
 
-	{EV_TIMEOUT,	108, 108, -1,			105,  2, {ACT_SETDLE0,
-								  ACT_HUPMODEM,
-								  ACT_TIMEOUT} },
-	{EV_TIMEOUT,	105, 105, -1,			103,  5, {0},	"Z\r"},
+	{EV_TIMEOUT,	108, 108, -1,		105,  2, {ACT_SETDLE0,
+							  ACT_HUPMODEM,
+							  ACT_TIMEOUT} },
+	{EV_TIMEOUT,	105, 105, -1,		103,  5, {0},	"Z\r"},
 
-	{RSP_ERROR,	102, 102, -1,			107,  5, {0},	"^GETPRE\r"},
-	{RSP_OK,	107, 107, -1,			  0,  0, {ACT_CONFIGMODE} },
-	{RSP_ERROR,	107, 107, -1,			  0,  0, {ACT_FAILINIT} },
-	{EV_TIMEOUT,	107, 107, -1,			  0,  0, {ACT_FAILINIT} },
+	{RSP_ERROR,	102, 102, -1,		107,  5, {0},	"^GETPRE\r"},
+	{RSP_OK,	107, 107, -1,		  0,  0, {ACT_CONFIGMODE} },
+	{RSP_ERROR,	107, 107, -1,		  0,  0, {ACT_FAILINIT} },
+	{EV_TIMEOUT,	107, 107, -1,		  0,  0, {ACT_FAILINIT} },
 
-	{RSP_ERROR,	103, 103, -1,			  0,  0, {ACT_FAILINIT} },
-	{EV_TIMEOUT,	103, 103, -1,			  0,  0, {ACT_FAILINIT} },
+	{RSP_ERROR,	103, 103, -1,		  0,  0, {ACT_FAILINIT} },
+	{EV_TIMEOUT,	103, 103, -1,		  0,  0, {ACT_FAILINIT} },
 
-	{RSP_STRING,	120, 120, -1,			121, -1, {ACT_SETVER} },
+	{RSP_STRING,	120, 120, -1,		121, -1, {ACT_SETVER} },
 
-	{EV_TIMEOUT,	120, 121, -1,			  0,  0, {ACT_FAILVER,
-								  ACT_INIT} },
-	{RSP_ERROR,	120, 121, -1,			  0,  0, {ACT_FAILVER,
-								  ACT_INIT} },
-	{RSP_OK,	121, 121, -1,			  0,  0, {ACT_GOTVER,
-								  ACT_INIT} },
+	{EV_TIMEOUT,	120, 121, -1,		  0,  0, {ACT_FAILVER,
+							  ACT_INIT} },
+	{RSP_ERROR,	120, 121, -1,		  0,  0, {ACT_FAILVER,
+							  ACT_INIT} },
+	{RSP_OK,	121, 121, -1,		  0,  0, {ACT_GOTVER,
+							  ACT_INIT} },
+	{RSP_NONE,	121, 121, -1,		120,  0, {ACT_GETSTRING} },
 
 /* leave dle mode */
-	{RSP_INIT,	  0,   0, SEQ_DLE0,		201,  5, {0},	"^SDLE=0\r"},
-	{RSP_OK,	201, 201, -1,			202, -1},
-	{RSP_ZDLE,	202, 202,  0,			  0,  0, {ACT_DLE0} },
-	{RSP_NODEV,	200, 249, -1,			  0,  0, {ACT_FAKEDLE0} },
-	{RSP_ERROR,	200, 249, -1,			  0,  0, {ACT_FAILDLE0} },
-	{EV_TIMEOUT,	200, 249, -1,			  0,  0, {ACT_FAILDLE0} },
+	{RSP_INIT,	  0,   0, SEQ_DLE0,	201,  5, {0},	"^SDLE=0\r"},
+	{RSP_OK,	201, 201, -1,		202, -1},
+	{RSP_ZDLE,	202, 202,  0,		  0,  0, {ACT_DLE0} },
+	{RSP_NODEV,	200, 249, -1,		  0,  0, {ACT_FAKEDLE0} },
+	{RSP_ERROR,	200, 249, -1,		  0,  0, {ACT_FAILDLE0} },
+	{EV_TIMEOUT,	200, 249, -1,		  0,  0, {ACT_FAILDLE0} },
 
 /* enter dle mode */
-	{RSP_INIT,	  0,   0, SEQ_DLE1,		251,  5, {0},	"^SDLE=1\r"},
-	{RSP_OK,	251, 251, -1,			252, -1},
-	{RSP_ZDLE,	252, 252,  1,			  0,  0, {ACT_DLE1} },
-	{RSP_ERROR,	250, 299, -1,			  0,  0, {ACT_FAILDLE1} },
-	{EV_TIMEOUT,	250, 299, -1,			  0,  0, {ACT_FAILDLE1} },
+	{RSP_INIT,	  0,   0, SEQ_DLE1,	251,  5, {0},	"^SDLE=1\r"},
+	{RSP_OK,	251, 251, -1,		252, -1},
+	{RSP_ZDLE,	252, 252,  1,		  0,  0, {ACT_DLE1} },
+	{RSP_ERROR,	250, 299, -1,		  0,  0, {ACT_FAILDLE1} },
+	{EV_TIMEOUT,	250, 299, -1,		  0,  0, {ACT_FAILDLE1} },
 
 /* incoming call */
-	{RSP_RING,	 -1,  -1, -1,			 -1, -1, {ACT_RING} },
+	{RSP_RING,	 -1,  -1, -1,		 -1, -1, {ACT_RING} },
 
 /* get cid */
-	{RSP_INIT,	  0,   0, SEQ_CID,		301,  5, {0},	"^SGCI?\r"},
-	{RSP_OK,	301, 301, -1,			302, -1},
-	{RSP_ZGCI,	302, 302, -1,			  0,  0, {ACT_CID} },
-	{RSP_ERROR,	301, 349, -1,			  0,  0, {ACT_FAILCID} },
-	{EV_TIMEOUT,	301, 349, -1,			  0,  0, {ACT_FAILCID} },
+	{RSP_INIT,	  0,   0, SEQ_CID,	301,  5, {0},	"^SGCI?\r"},
+	{RSP_OK,	301, 301, -1,		302, -1},
+	{RSP_ZGCI,	302, 302, -1,		  0,  0, {ACT_CID} },
+	{RSP_ERROR,	301, 349, -1,		  0,  0, {ACT_FAILCID} },
+	{EV_TIMEOUT,	301, 349, -1,		  0,  0, {ACT_FAILCID} },
 
 /* enter cid mode */
-	{RSP_INIT,	  0,   0, SEQ_CIDMODE,		150,  5, {0},	"^SGCI=1\r"},
-	{RSP_OK,	150, 150, -1,			  0,  0, {ACT_CMODESET} },
-	{RSP_ERROR,	150, 150, -1,			  0,  0, {ACT_FAILCMODE} },
-	{EV_TIMEOUT,	150, 150, -1,			  0,  0, {ACT_FAILCMODE} },
+	{RSP_INIT,	  0,   0, SEQ_CIDMODE,	150,  5, {0},	"^SGCI=1\r"},
+	{RSP_OK,	150, 150, -1,		  0,  0, {ACT_CMODESET} },
+	{RSP_ERROR,	150, 150, -1,		  0,  0, {ACT_FAILCMODE} },
+	{EV_TIMEOUT,	150, 150, -1,		  0,  0, {ACT_FAILCMODE} },
 
 /* leave cid mode */
-	{RSP_INIT,	  0,   0, SEQ_UMMODE,		160,  5, {0},	"Z\r"},
-	{RSP_OK,	160, 160, -1,			  0,  0, {ACT_UMODESET} },
-	{RSP_ERROR,	160, 160, -1,			  0,  0, {ACT_FAILUMODE} },
-	{EV_TIMEOUT,	160, 160, -1,			  0,  0, {ACT_FAILUMODE} },
+	{RSP_INIT,	  0,   0, SEQ_UMMODE,	160,  5, {0},	"Z\r"},
+	{RSP_OK,	160, 160, -1,		  0,  0, {ACT_UMODESET} },
+	{RSP_ERROR,	160, 160, -1,		  0,  0, {ACT_FAILUMODE} },
+	{EV_TIMEOUT,	160, 160, -1,		  0,  0, {ACT_FAILUMODE} },
 
 /* abort getting cid */
-	{RSP_INIT,	  0,   0, SEQ_NOCID,		  0,  0, {ACT_ABORTCID} },
+	{RSP_INIT,	  0,   0, SEQ_NOCID,	  0,  0, {ACT_ABORTCID} },
 
 /* reset */
-	{RSP_INIT,	  0,   0, SEQ_SHUTDOWN,		504,  5, {0},	"Z\r"},
-	{RSP_OK,	504, 504, -1,			  0,  0, {ACT_SDOWN} },
-	{RSP_ERROR,	501, 599, -1,			  0,  0, {ACT_FAILSDOWN} },
-	{EV_TIMEOUT,	501, 599, -1,			  0,  0, {ACT_FAILSDOWN} },
-	{RSP_NODEV,	501, 599, -1,			  0,  0, {ACT_FAKESDOWN} },
-
-	{EV_PROC_CIDMODE, -1, -1, -1,			 -1, -1, {ACT_PROC_CIDMODE} },
-	{EV_IF_LOCK,	 -1,  -1, -1,			 -1, -1, {ACT_IF_LOCK} },
-	{EV_IF_VER,	 -1,  -1, -1,			 -1, -1, {ACT_IF_VER} },
-	{EV_START,	 -1,  -1, -1,			 -1, -1, {ACT_START} },
-	{EV_STOP,	 -1,  -1, -1,			 -1, -1, {ACT_STOP} },
-	{EV_SHUTDOWN,	 -1,  -1, -1,			 -1, -1, {ACT_SHUTDOWN} },
+	{RSP_INIT,	  0,   0, SEQ_SHUTDOWN,	504,  5, {0},	"Z\r"},
+	{RSP_OK,	504, 504, -1,		  0,  0, {ACT_SDOWN} },
+	{RSP_ERROR,	501, 599, -1,		  0,  0, {ACT_FAILSDOWN} },
+	{EV_TIMEOUT,	501, 599, -1,		  0,  0, {ACT_FAILSDOWN} },
+	{RSP_NODEV,	501, 599, -1,		  0,  0, {ACT_FAKESDOWN} },
+
+	{EV_PROC_CIDMODE, -1, -1, -1,		 -1, -1, {ACT_PROC_CIDMODE} },
+	{EV_IF_LOCK,	 -1,  -1, -1,		 -1, -1, {ACT_IF_LOCK} },
+	{EV_IF_VER,	 -1,  -1, -1,		 -1, -1, {ACT_IF_VER} },
+	{EV_START,	 -1,  -1, -1,		 -1, -1, {ACT_START} },
+	{EV_STOP,	 -1,  -1, -1,		 -1, -1, {ACT_STOP} },
+	{EV_SHUTDOWN,	 -1,  -1, -1,		 -1, -1, {ACT_SHUTDOWN} },
 
 /* misc. */
-	{RSP_ERROR,	 -1,  -1, -1,			 -1, -1, {ACT_ERROR} },
-	{RSP_ZCAU,	 -1,  -1, -1,			 -1, -1, {ACT_ZCAU} },
-	{RSP_NONE,	 -1,  -1, -1,			 -1, -1, {ACT_DEBUG} },
-	{RSP_ANY,	 -1,  -1, -1,			 -1, -1, {ACT_WARN} },
+	{RSP_ERROR,	 -1,  -1, -1,		 -1, -1, {ACT_ERROR} },
+	{RSP_ZCAU,	 -1,  -1, -1,		 -1, -1, {ACT_ZCAU} },
+	{RSP_NONE,	 -1,  -1, -1,		 -1, -1, {ACT_DEBUG} },
+	{RSP_ANY,	 -1,  -1, -1,		 -1, -1, {ACT_WARN} },
 	{RSP_LAST}
 };
 
@@ -261,90 +262,90 @@ struct reply_t gigaset_tab_cid[] =
  * action, command */
 
 /* dial */
-	{EV_DIAL,	 -1,  -1, -1,			 -1, -1, {ACT_DIAL} },
-	{RSP_INIT,	  0,   0, SEQ_DIAL,		601,  5, {ACT_CMD + AT_BC} },
-	{RSP_OK,	601, 601, -1,			603,  5, {ACT_CMD + AT_PROTO} },
-	{RSP_OK,	603, 603, -1,			604,  5, {ACT_CMD + AT_TYPE} },
-	{RSP_OK,	604, 604, -1,			605,  5, {ACT_CMD + AT_MSN} },
-	{RSP_NULL,	605, 605, -1,			606,  5, {ACT_CMD + AT_CLIP} },
-	{RSP_OK,	605, 605, -1,			606,  5, {ACT_CMD + AT_CLIP} },
-	{RSP_NULL,	606, 606, -1,			607,  5, {ACT_CMD + AT_ISO} },
-	{RSP_OK,	606, 606, -1,			607,  5, {ACT_CMD + AT_ISO} },
-	{RSP_OK,	607, 607, -1,			608,  5, {0},	"+VLS=17\r"},
-	{RSP_OK,	608, 608, -1,			609, -1},
-	{RSP_ZSAU,	609, 609, ZSAU_PROCEEDING,	610,  5, {ACT_CMD + AT_DIAL} },
-	{RSP_OK,	610, 610, -1,			650,  0, {ACT_DIALING} },
-
-	{RSP_ERROR,	601, 610, -1,			  0,  0, {ACT_ABORTDIAL} },
-	{EV_TIMEOUT,	601, 610, -1,			  0,  0, {ACT_ABORTDIAL} },
+	{EV_DIAL,	 -1,  -1, -1,		 -1, -1, {ACT_DIAL} },
+	{RSP_INIT,	  0,   0, SEQ_DIAL,	601,  5, {ACT_CMD + AT_BC} },
+	{RSP_OK,	601, 601, -1,		603,  5, {ACT_CMD + AT_PROTO} },
+	{RSP_OK,	603, 603, -1,		604,  5, {ACT_CMD + AT_TYPE} },
+	{RSP_OK,	604, 604, -1,		605,  5, {ACT_CMD + AT_MSN} },
+	{RSP_NULL,	605, 605, -1,		606,  5, {ACT_CMD + AT_CLIP} },
+	{RSP_OK,	605, 605, -1,		606,  5, {ACT_CMD + AT_CLIP} },
+	{RSP_NULL,	606, 606, -1,		607,  5, {ACT_CMD + AT_ISO} },
+	{RSP_OK,	606, 606, -1,		607,  5, {ACT_CMD + AT_ISO} },
+	{RSP_OK,	607, 607, -1,		608,  5, {0},	"+VLS=17\r"},
+	{RSP_OK,	608, 608, -1,		609, -1},
+	{RSP_ZSAU,	609, 609, ZSAU_PROCEEDING, 610, 5, {ACT_CMD + AT_DIAL} },
+	{RSP_OK,	610, 610, -1,		650,  0, {ACT_DIALING} },
+
+	{RSP_ERROR,	601, 610, -1,		  0,  0, {ACT_ABORTDIAL} },
+	{EV_TIMEOUT,	601, 610, -1,		  0,  0, {ACT_ABORTDIAL} },
 
 /* optional dialing responses */
-	{EV_BC_OPEN,	650, 650, -1,			651, -1},
-	{RSP_ZVLS,	609, 651, 17,			 -1, -1, {ACT_DEBUG} },
-	{RSP_ZCTP,	610, 651, -1,			 -1, -1, {ACT_DEBUG} },
-	{RSP_ZCPN,	610, 651, -1,			 -1, -1, {ACT_DEBUG} },
-	{RSP_ZSAU,	650, 651, ZSAU_CALL_DELIVERED,	 -1, -1, {ACT_DEBUG} },
+	{EV_BC_OPEN,	650, 650, -1,		651, -1},
+	{RSP_ZVLS,	609, 651, 17,		 -1, -1, {ACT_DEBUG} },
+	{RSP_ZCTP,	610, 651, -1,		 -1, -1, {ACT_DEBUG} },
+	{RSP_ZCPN,	610, 651, -1,		 -1, -1, {ACT_DEBUG} },
+	{RSP_ZSAU,	650, 651, ZSAU_CALL_DELIVERED, -1, -1, {ACT_DEBUG} },
 
 /* connect */
-	{RSP_ZSAU,	650, 650, ZSAU_ACTIVE,		800, -1, {ACT_CONNECT} },
-	{RSP_ZSAU,	651, 651, ZSAU_ACTIVE,		800, -1, {ACT_CONNECT,
-								  ACT_NOTIFY_BC_UP} },
-	{RSP_ZSAU,	750, 750, ZSAU_ACTIVE,		800, -1, {ACT_CONNECT} },
-	{RSP_ZSAU,	751, 751, ZSAU_ACTIVE,		800, -1, {ACT_CONNECT,
-								  ACT_NOTIFY_BC_UP} },
-	{EV_BC_OPEN,	800, 800, -1,			800, -1, {ACT_NOTIFY_BC_UP} },
+	{RSP_ZSAU,	650, 650, ZSAU_ACTIVE,	800, -1, {ACT_CONNECT} },
+	{RSP_ZSAU,	651, 651, ZSAU_ACTIVE,	800, -1, {ACT_CONNECT,
+							  ACT_NOTIFY_BC_UP} },
+	{RSP_ZSAU,	750, 750, ZSAU_ACTIVE,	800, -1, {ACT_CONNECT} },
+	{RSP_ZSAU,	751, 751, ZSAU_ACTIVE,	800, -1, {ACT_CONNECT,
+							  ACT_NOTIFY_BC_UP} },
+	{EV_BC_OPEN,	800, 800, -1,		800, -1, {ACT_NOTIFY_BC_UP} },
 
 /* remote hangup */
-	{RSP_ZSAU,	650, 651, ZSAU_DISCONNECT_IND,	  0,  0, {ACT_REMOTEREJECT} },
-	{RSP_ZSAU,	750, 751, ZSAU_DISCONNECT_IND,	  0,  0, {ACT_REMOTEHUP} },
-	{RSP_ZSAU,	800, 800, ZSAU_DISCONNECT_IND,	  0,  0, {ACT_REMOTEHUP} },
+	{RSP_ZSAU,	650, 651, ZSAU_DISCONNECT_IND, 0, 0, {ACT_REMOTEREJECT} },
+	{RSP_ZSAU,	750, 751, ZSAU_DISCONNECT_IND, 0, 0, {ACT_REMOTEHUP} },
+	{RSP_ZSAU,	800, 800, ZSAU_DISCONNECT_IND, 0, 0, {ACT_REMOTEHUP} },
 
 /* hangup */
-	{EV_HUP,	 -1,  -1, -1,			 -1, -1, {ACT_HUP} },
-	{RSP_INIT,	 -1,  -1, SEQ_HUP,		401,  5, {0},	"+VLS=0\r"},
-	{RSP_OK,	401, 401, -1,			402,  5},
-	{RSP_ZVLS,	402, 402,  0,			403,  5},
-	{RSP_ZSAU,	403, 403, ZSAU_DISCONNECT_REQ,	 -1, -1, {ACT_DEBUG} },
-	{RSP_ZSAU,	403, 403, ZSAU_NULL,		  0,  0, {ACT_DISCONNECT} },
-	{RSP_NODEV,	401, 403, -1,			  0,  0, {ACT_FAKEHUP} },
-	{RSP_ERROR,	401, 401, -1,			  0,  0, {ACT_ABORTHUP} },
-	{EV_TIMEOUT,	401, 403, -1,			  0,  0, {ACT_ABORTHUP} },
-
-	{EV_BC_CLOSED,	  0,   0, -1,			  0, -1, {ACT_NOTIFY_BC_DOWN} },
+	{EV_HUP,	 -1,  -1, -1,		 -1, -1, {ACT_HUP} },
+	{RSP_INIT,	 -1,  -1, SEQ_HUP,	401,  5, {0},	"+VLS=0\r"},
+	{RSP_OK,	401, 401, -1,		402,  5},
+	{RSP_ZVLS,	402, 402,  0,		403,  5},
+	{RSP_ZSAU,	403, 403, ZSAU_DISCONNECT_REQ, -1, -1, {ACT_DEBUG} },
+	{RSP_ZSAU,	403, 403, ZSAU_NULL,	  0,  0, {ACT_DISCONNECT} },
+	{RSP_NODEV,	401, 403, -1,		  0,  0, {ACT_FAKEHUP} },
+	{RSP_ERROR,	401, 401, -1,		  0,  0, {ACT_ABORTHUP} },
+	{EV_TIMEOUT,	401, 403, -1,		  0,  0, {ACT_ABORTHUP} },
+
+	{EV_BC_CLOSED,	  0,   0, -1,		  0, -1, {ACT_NOTIFY_BC_DOWN} },
 
 /* ring */
-	{RSP_ZBC,	700, 700, -1,			 -1, -1, {0} },
-	{RSP_ZHLC,	700, 700, -1,			 -1, -1, {0} },
-	{RSP_NMBR,	700, 700, -1,			 -1, -1, {0} },
-	{RSP_ZCPN,	700, 700, -1,			 -1, -1, {0} },
-	{RSP_ZCTP,	700, 700, -1,			 -1, -1, {0} },
-	{EV_TIMEOUT,	700, 700, -1,			720, 720, {ACT_ICALL} },
-	{EV_BC_CLOSED,	720, 720, -1,			  0, -1, {ACT_NOTIFY_BC_DOWN} },
+	{RSP_ZBC,	700, 700, -1,		 -1, -1, {0} },
+	{RSP_ZHLC,	700, 700, -1,		 -1, -1, {0} },
+	{RSP_NMBR,	700, 700, -1,		 -1, -1, {0} },
+	{RSP_ZCPN,	700, 700, -1,		 -1, -1, {0} },
+	{RSP_ZCTP,	700, 700, -1,		 -1, -1, {0} },
+	{EV_TIMEOUT,	700, 700, -1,		720, 720, {ACT_ICALL} },
+	{EV_BC_CLOSED,	720, 720, -1,		  0, -1, {ACT_NOTIFY_BC_DOWN} },
 
 /*accept icall*/
-	{EV_ACCEPT,	 -1,  -1, -1,			 -1, -1, {ACT_ACCEPT} },
-	{RSP_INIT,	720, 720, SEQ_ACCEPT,		721,  5, {ACT_CMD + AT_PROTO} },
-	{RSP_OK,	721, 721, -1,			722,  5, {ACT_CMD + AT_ISO} },
-	{RSP_OK,	722, 722, -1,			723,  5, {0},	"+VLS=17\r"},
-	{RSP_OK,	723, 723, -1,			724,  5, {0} },
-	{RSP_ZVLS,	724, 724, 17,			750, 50, {ACT_ACCEPTED} },
-	{RSP_ERROR,	721, 729, -1,			  0,  0, {ACT_ABORTACCEPT} },
-	{EV_TIMEOUT,	721, 729, -1,			  0,  0, {ACT_ABORTACCEPT} },
-	{RSP_ZSAU,	700, 729, ZSAU_NULL,		  0,  0, {ACT_ABORTACCEPT} },
-	{RSP_ZSAU,	700, 729, ZSAU_ACTIVE,		  0,  0, {ACT_ABORTACCEPT} },
-	{RSP_ZSAU,	700, 729, ZSAU_DISCONNECT_IND,	  0,  0, {ACT_ABORTACCEPT} },
-
-	{EV_BC_OPEN,	750, 750, -1,			751, -1},
-	{EV_TIMEOUT,	750, 751, -1,			  0,  0, {ACT_CONNTIMEOUT} },
+	{EV_ACCEPT,	 -1,  -1, -1,		 -1, -1, {ACT_ACCEPT} },
+	{RSP_INIT,	720, 720, SEQ_ACCEPT,	721,  5, {ACT_CMD + AT_PROTO} },
+	{RSP_OK,	721, 721, -1,		722,  5, {ACT_CMD + AT_ISO} },
+	{RSP_OK,	722, 722, -1,		723,  5, {0},	"+VLS=17\r"},
+	{RSP_OK,	723, 723, -1,		724,  5, {0} },
+	{RSP_ZVLS,	724, 724, 17,		750, 50, {ACT_ACCEPTED} },
+	{RSP_ERROR,	721, 729, -1,		  0,  0, {ACT_ABORTACCEPT} },
+	{EV_TIMEOUT,	721, 729, -1,		  0,  0, {ACT_ABORTACCEPT} },
+	{RSP_ZSAU,	700, 729, ZSAU_NULL,	  0,  0, {ACT_ABORTACCEPT} },
+	{RSP_ZSAU,	700, 729, ZSAU_ACTIVE,	  0,  0, {ACT_ABORTACCEPT} },
+	{RSP_ZSAU,	700, 729, ZSAU_DISCONNECT_IND, 0, 0, {ACT_ABORTACCEPT} },
+
+	{EV_BC_OPEN,	750, 750, -1,		751, -1},
+	{EV_TIMEOUT,	750, 751, -1,		  0,  0, {ACT_CONNTIMEOUT} },
 
 /* B channel closed (general case) */
-	{EV_BC_CLOSED,	 -1,  -1, -1,			 -1, -1, {ACT_NOTIFY_BC_DOWN} },
+	{EV_BC_CLOSED,	 -1,  -1, -1,		 -1, -1, {ACT_NOTIFY_BC_DOWN} },
 
 /* misc. */
-	{RSP_ZCON,	 -1,  -1, -1,			 -1, -1, {ACT_DEBUG} },
-	{RSP_ZCAU,	 -1,  -1, -1,			 -1, -1, {ACT_ZCAU} },
-	{RSP_NONE,	 -1,  -1, -1,			 -1, -1, {ACT_DEBUG} },
-	{RSP_ANY,	 -1,  -1, -1,			 -1, -1, {ACT_WARN} },
+	{RSP_ZCON,	 -1,  -1, -1,		 -1, -1, {ACT_DEBUG} },
+	{RSP_ZCAU,	 -1,  -1, -1,		 -1, -1, {ACT_ZCAU} },
+	{RSP_NONE,	 -1,  -1, -1,		 -1, -1, {ACT_DEBUG} },
+	{RSP_ANY,	 -1,  -1, -1,		 -1, -1, {ACT_WARN} },
 	{RSP_LAST}
 };
 
@@ -648,16 +649,16 @@ static void disconnect(struct at_state_t **at_state_p)
 static inline struct at_state_t *get_free_channel(struct cardstate *cs,
 						  int cid)
 /* cids: >0: siemens-cid
-   0: without cid
-   -1: no cid assigned yet
-*/
+ *        0: without cid
+ *       -1: no cid assigned yet
+ */
 {
 	unsigned long flags;
 	int i;
 	struct at_state_t *ret;
 
 	for (i = 0; i < cs->channels; ++i)
-		if (gigaset_get_channel(cs->bcs + i)) {
+		if (gigaset_get_channel(cs->bcs + i) >= 0) {
 			ret = &cs->bcs[i].at_state;
 			ret->cid = cid;
 			return ret;
@@ -922,18 +923,18 @@ static void do_stop(struct cardstate *cs)
  * channel >= 0: getting cid for the channel failed
  * channel < 0:  entering cid mode failed
  *
- * returns 0 on failure
+ * returns 0 on success, <0 on failure
  */
 static int reinit_and_retry(struct cardstate *cs, int channel)
 {
 	int i;
 
 	if (--cs->retry_count <= 0)
-		return 0;
+		return -EFAULT;
 
 	for (i = 0; i < cs->channels; ++i)
 		if (cs->bcs[i].at_state.cid > 0)
-			return 0;
+			return -EBUSY;
 
 	if (channel < 0)
 		dev_warn(cs->dev,
@@ -944,7 +945,7 @@ static int reinit_and_retry(struct cardstate *cs, int channel)
 		cs->bcs[channel].at_state.pending_commands |= PC_CID;
 	}
 	schedule_init(cs, MS_INIT);
-	return 1;
+	return 0;
 }
 
 static int at_state_invalid(struct cardstate *cs,
@@ -1015,7 +1016,7 @@ static int do_lock(struct cardstate *cs)
 			if (cs->bcs[i].at_state.pending_commands)
 				return -EBUSY;
 
-		if (!gigaset_get_channels(cs))
+		if (gigaset_get_channels(cs) < 0)
 			return -EBUSY;
 
 		break;
@@ -1124,7 +1125,7 @@ static void do_action(int action, struct cardstate *cs,
 			init_failed(cs, M_UNKNOWN);
 			break;
 		}
-		if (!reinit_and_retry(cs, -1))
+		if (reinit_and_retry(cs, -1) < 0)
 			schedule_init(cs, MS_RECOVER);
 		break;
 	case ACT_FAILUMODE:
@@ -1267,7 +1268,7 @@ static void do_action(int action, struct cardstate *cs,
 	case ACT_FAILCID:
 		cs->cur_at_seq = SEQ_NONE;
 		channel = cs->curchannel;
-		if (!reinit_and_retry(cs, channel)) {
+		if (reinit_and_retry(cs, channel) < 0) {
 			dev_warn(cs->dev,
 				 "Could not get a call ID. Cannot dial.\n");
 			at_state2 = &cs->bcs[channel].at_state;
@@ -1314,8 +1315,9 @@ static void do_action(int action, struct cardstate *cs,
 		s = ev->ptr;
 
 		if (!strcmp(s, "OK")) {
+			/* OK without version string: assume old response */
 			*p_genresp = 1;
-			*p_resp_code = RSP_ERROR;
+			*p_resp_code = RSP_NONE;
 			break;
 		}
 
@@ -1372,7 +1374,8 @@ static void do_action(int action, struct cardstate *cs,
 			 ev->parameter, at_state->ConState);
 		break;
 
-		/* events from the LL */
+	/* events from the LL */
+
 	case ACT_DIAL:
 		start_dial(at_state, ev->ptr, ev->parameter);
 		break;
@@ -1385,7 +1388,8 @@ static void do_action(int action, struct cardstate *cs,
 		cs->commands_pending = 1;
 		break;
 
-		/* hotplug events */
+	/* hotplug events */
+
 	case ACT_STOP:
 		do_stop(cs);
 		break;
@@ -1393,7 +1397,8 @@ static void do_action(int action, struct cardstate *cs,
 		do_start(cs);
 		break;
 
-		/* events from the interface */
+	/* events from the interface */
+
 	case ACT_IF_LOCK:
 		cs->cmd_result = ev->parameter ? do_lock(cs) : do_unlock(cs);
 		cs->waiting = 0;
@@ -1412,7 +1417,8 @@ static void do_action(int action, struct cardstate *cs,
 		wake_up(&cs->waitqueue);
 		break;
 
-		/* events from the proc file system */
+	/* events from the proc file system */
+
 	case ACT_PROC_CIDMODE:
 		spin_lock_irqsave(&cs->lock, flags);
 		if (ev->parameter != cs->cidmode) {
@@ -1431,7 +1437,8 @@ static void do_action(int action, struct cardstate *cs,
 		wake_up(&cs->waitqueue);
 		break;
 
-		/* events from the hardware drivers */
+	/* events from the hardware drivers */
+
 	case ACT_NOTIFY_BC_DOWN:
 		bchannel_down(bcs);
 		break;
diff --git a/drivers/isdn/gigaset/gigaset.h b/drivers/isdn/gigaset/gigaset.h
index 1dc25131e67..8e2fc8f31d1 100644
--- a/drivers/isdn/gigaset/gigaset.h
+++ b/drivers/isdn/gigaset/gigaset.h
@@ -163,8 +163,8 @@ void gigaset_dbg_buffer(enum debuglevel level, const unsigned char *msg,
 #define BAS_LOWFRAME	5	/* "    "    with negative flow control */
 #define BAS_CORRFRAMES	4	/* flow control multiplicator */
 
-#define BAS_INBUFSIZE	(BAS_MAXFRAME * BAS_NUMFRAMES)
-/* size of isoc in buf per URB */
+#define BAS_INBUFSIZE	(BAS_MAXFRAME * BAS_NUMFRAMES)	/* size of isoc in buf
+							 * per URB */
 #define BAS_OUTBUFSIZE	4096		/* size of common isoc out buffer */
 #define BAS_OUTBUFPAD	BAS_MAXFRAME	/* size of pad area for isoc out buf */
 
@@ -471,18 +471,18 @@ struct cardstate {
 					   for */
 	int commands_pending;		/* flag(s) in xxx.commands_pending have
 					   been set */
-	struct tasklet_struct event_tasklet;
-	/* tasklet for serializing AT commands.
-	 * Scheduled
-	 *   -> for modem reponses (and
-	 *      incoming data for M10x)
-	 *   -> on timeout
-	 *   -> after setting bits in
-	 *      xxx.at_state.pending_command
-	 *      (e.g. command from LL) */
-	struct tasklet_struct write_tasklet;
-	/* tasklet for serial output
-	 * (not used in base driver) */
+	struct tasklet_struct
+		event_tasklet;		/* tasklet for serializing AT commands.
+					 * Scheduled
+					 *   -> for modem reponses (and
+					 *      incoming data for M10x)
+					 *   -> on timeout
+					 *   -> after setting bits in
+					 *      xxx.at_state.pending_command
+					 *      (e.g. command from LL) */
+	struct tasklet_struct
+		write_tasklet;		/* tasklet for serial output
+					 * (not used in base driver) */
 
 	/* event queue */
 	struct event_t events[MAX_EVENTS];
@@ -583,7 +583,7 @@ struct gigaset_ops {
 	int (*initbcshw)(struct bc_state *bcs);
 
 	/* Called by gigaset_freecs() for freeing bcs->hw.xxx */
-	int (*freebcshw)(struct bc_state *bcs);
+	void (*freebcshw)(struct bc_state *bcs);
 
 	/* Called by gigaset_bchannel_down() for resetting bcs->hw.xxx */
 	void (*reinitbcshw)(struct bc_state *bcs);
diff --git a/drivers/isdn/gigaset/i4l.c b/drivers/isdn/gigaset/i4l.c
index 0f13eb1de65..2d75329007f 100644
--- a/drivers/isdn/gigaset/i4l.c
+++ b/drivers/isdn/gigaset/i4l.c
@@ -229,7 +229,7 @@ static int command_from_LL(isdn_ctrl *cntrl)
 			return -EINVAL;
 		}
 		bcs = cs->bcs + ch;
-		if (!gigaset_get_channel(bcs)) {
+		if (gigaset_get_channel(bcs) < 0) {
 			dev_err(cs->dev, "ISDN_CMD_DIAL: channel not free\n");
 			return -EBUSY;
 		}
@@ -618,7 +618,7 @@ void gigaset_isdn_stop(struct cardstate *cs)
  * @cs:		device descriptor structure.
  * @isdnid:	device name.
  *
- * Return value: 1 for success, 0 for failure
+ * Return value: 0 on success, error code < 0 on failure
  */
 int gigaset_isdn_regdev(struct cardstate *cs, const char *isdnid)
 {
@@ -627,14 +627,14 @@ int gigaset_isdn_regdev(struct cardstate *cs, const char *isdnid)
 	iif = kmalloc(sizeof *iif, GFP_KERNEL);
 	if (!iif) {
 		pr_err("out of memory\n");
-		return 0;
+		return -ENOMEM;
 	}
 
 	if (snprintf(iif->id, sizeof iif->id, "%s_%u", isdnid, cs->minor_index)
 	    >= sizeof iif->id) {
 		pr_err("ID too long: %s\n", isdnid);
 		kfree(iif);
-		return 0;
+		return -EINVAL;
 	}
 
 	iif->owner = THIS_MODULE;
@@ -656,13 +656,13 @@ int gigaset_isdn_regdev(struct cardstate *cs, const char *isdnid)
 	if (!register_isdn(iif)) {
 		pr_err("register_isdn failed\n");
 		kfree(iif);
-		return 0;
+		return -EINVAL;
 	}
 
 	cs->iif = iif;
 	cs->myid = iif->channels;		/* Set my device id */
 	cs->hw_hdr_len = HW_HDR_LEN;
-	return 1;
+	return 0;
 }
 
 /**
diff --git a/drivers/isdn/gigaset/isocdata.c b/drivers/isdn/gigaset/isocdata.c
index a351c16705b..bc29f1d52a2 100644
--- a/drivers/isdn/gigaset/isocdata.c
+++ b/drivers/isdn/gigaset/isocdata.c
@@ -56,7 +56,7 @@ static inline int isowbuf_freebytes(struct isowbuf_t *iwb)
 
 /* start writing
  * acquire the write semaphore
- * return true if acquired, false if busy
+ * return 0 if acquired, <0 if busy
  */
 static inline int isowbuf_startwrite(struct isowbuf_t *iwb)
 {
@@ -64,12 +64,12 @@ static inline int isowbuf_startwrite(struct isowbuf_t *iwb)
 		atomic_inc(&iwb->writesem);
 		gig_dbg(DEBUG_ISO, "%s: couldn't acquire iso write semaphore",
 			__func__);
-		return 0;
+		return -EBUSY;
 	}
 	gig_dbg(DEBUG_ISO,
 		"%s: acquired iso write semaphore, data[write]=%02x, nbits=%d",
 		__func__, iwb->data[iwb->write], iwb->wbits);
-	return 1;
+	return 0;
 }
 
 /* finish writing
@@ -158,7 +158,7 @@ int gigaset_isowbuf_getbytes(struct isowbuf_t *iwb, int size)
 		/* no wraparound in valid data */
 		if (limit >= write) {
 			/* append idle frame */
-			if (!isowbuf_startwrite(iwb))
+			if (isowbuf_startwrite(iwb) < 0)
 				return -EBUSY;
 			/* write position could have changed */
 			write = iwb->write;
@@ -403,7 +403,7 @@ static inline int hdlc_buildframe(struct isowbuf_t *iwb,
 	unsigned char c;
 
 	if (isowbuf_freebytes(iwb) < count + count / 5 + 6 ||
-	    !isowbuf_startwrite(iwb)) {
+	    isowbuf_startwrite(iwb) < 0) {
 		gig_dbg(DEBUG_ISO, "%s: %d bytes free -> -EAGAIN",
 			__func__, isowbuf_freebytes(iwb));
 		return -EAGAIN;
@@ -457,7 +457,7 @@ static inline int trans_buildframe(struct isowbuf_t *iwb,
 		return iwb->write;
 
 	if (isowbuf_freebytes(iwb) < count ||
-	    !isowbuf_startwrite(iwb)) {
+	    isowbuf_startwrite(iwb) < 0) {
 		gig_dbg(DEBUG_ISO, "can't put %d bytes", count);
 		return -EAGAIN;
 	}
diff --git a/drivers/isdn/gigaset/ser-gigaset.c b/drivers/isdn/gigaset/ser-gigaset.c
index 6f3fd4cf437..8c91fd5eb6f 100644
--- a/drivers/isdn/gigaset/ser-gigaset.c
+++ b/drivers/isdn/gigaset/ser-gigaset.c
@@ -340,17 +340,16 @@ static int gigaset_initbcshw(struct bc_state *bcs)
 {
 	/* unused */
 	bcs->hw.ser = NULL;
-	return 1;
+	return 0;
 }
 
 /*
  * Free B channel structure
  * Called by "gigaset_freebcs" in common.c
  */
-static int gigaset_freebcshw(struct bc_state *bcs)
+static void gigaset_freebcshw(struct bc_state *bcs)
 {
 	/* unused */
-	return 1;
 }
 
 /*
@@ -398,7 +397,7 @@ static int gigaset_initcshw(struct cardstate *cs)
 	scs = kzalloc(sizeof(struct ser_cardstate), GFP_KERNEL);
 	if (!scs) {
 		pr_err("out of memory\n");
-		return 0;
+		return -ENOMEM;
 	}
 	cs->hw.ser = scs;
 
@@ -410,13 +409,13 @@ static int gigaset_initcshw(struct cardstate *cs)
 		pr_err("error %d registering platform device\n", rc);
 		kfree(cs->hw.ser);
 		cs->hw.ser = NULL;
-		return 0;
+		return rc;
 	}
 	dev_set_drvdata(&cs->hw.ser->dev.dev, cs);
 
 	tasklet_init(&cs->write_tasklet,
 		     gigaset_modem_fill, (unsigned long) cs);
-	return 1;
+	return 0;
 }
 
 /*
@@ -503,6 +502,7 @@ static int
 gigaset_tty_open(struct tty_struct *tty)
 {
 	struct cardstate *cs;
+	int rc;
 
 	gig_dbg(DEBUG_INIT, "Starting HLL for Gigaset M101");
 
@@ -515,8 +515,10 @@ gigaset_tty_open(struct tty_struct *tty)
 
 	/* allocate memory for our device state and initialize it */
 	cs = gigaset_initcs(driver, 1, 1, 0, cidmode, GIGASET_MODULENAME);
-	if (!cs)
+	if (!cs) {
+		rc = -ENODEV;
 		goto error;
+	}
 
 	cs->dev = &cs->hw.ser->dev.dev;
 	cs->hw.ser->tty = tty;
@@ -530,7 +532,8 @@ gigaset_tty_open(struct tty_struct *tty)
 	 */
 	if (startmode == SM_LOCKED)
 		cs->mstate = MS_LOCKED;
-	if (!gigaset_start(cs)) {
+	rc = gigaset_start(cs);
+	if (rc < 0) {
 		tasklet_kill(&cs->write_tasklet);
 		goto error;
 	}
@@ -542,7 +545,7 @@ error:
 	gig_dbg(DEBUG_INIT, "Startup of HLL failed");
 	tty->disc_data = NULL;
 	gigaset_freecs(cs);
-	return -ENODEV;
+	return rc;
 }
 
 /*
diff --git a/drivers/isdn/gigaset/usb-gigaset.c b/drivers/isdn/gigaset/usb-gigaset.c
index 049da67f639..d0a41cb0cf6 100644
--- a/drivers/isdn/gigaset/usb-gigaset.c
+++ b/drivers/isdn/gigaset/usb-gigaset.c
@@ -124,6 +124,7 @@ static struct usb_driver gigaset_usb_driver = {
 	.reset_resume =	gigaset_resume,
 	.pre_reset =	gigaset_pre_reset,
 	.post_reset =	gigaset_resume,
+	.disable_hub_initiated_lpm = 1,
 };
 
 struct usb_cardstate {
@@ -549,10 +550,9 @@ static int gigaset_brkchars(struct cardstate *cs, const unsigned char buf[6])
 			       0, 0, &buf, 6, 2000);
 }
 
-static int gigaset_freebcshw(struct bc_state *bcs)
+static void gigaset_freebcshw(struct bc_state *bcs)
 {
 	/* unused */
-	return 1;
 }
 
 /* Initialize the b-channel structure */
@@ -560,7 +560,7 @@ static int gigaset_initbcshw(struct bc_state *bcs)
 {
 	/* unused */
 	bcs->hw.usb = NULL;
-	return 1;
+	return 0;
 }
 
 static void gigaset_reinitbcshw(struct bc_state *bcs)
@@ -582,7 +582,7 @@ static int gigaset_initcshw(struct cardstate *cs)
 		kmalloc(sizeof(struct usb_cardstate), GFP_KERNEL);
 	if (!ucs) {
 		pr_err("out of memory\n");
-		return 0;
+		return -ENOMEM;
 	}
 
 	ucs->bchars[0] = 0;
@@ -597,7 +597,7 @@ static int gigaset_initcshw(struct cardstate *cs)
 	tasklet_init(&cs->write_tasklet,
 		     gigaset_modem_fill, (unsigned long) cs);
 
-	return 1;
+	return 0;
 }
 
 /* Send data from current skb to the device. */
@@ -766,9 +766,9 @@ static int gigaset_probe(struct usb_interface *interface,
 	if (startmode == SM_LOCKED)
 		cs->mstate = MS_LOCKED;
 
-	if (!gigaset_start(cs)) {
+	retval = gigaset_start(cs);
+	if (retval < 0) {
 		tasklet_kill(&cs->write_tasklet);
-		retval = -ENODEV;
 		goto error;
 	}
 	return 0;
@@ -898,8 +898,10 @@ static int __init usb_gigaset_init(void)
 	driver = gigaset_initdriver(GIGASET_MINOR, GIGASET_MINORS,
 				    GIGASET_MODULENAME, GIGASET_DEVNAME,
 				    &ops, THIS_MODULE);
-	if (driver == NULL)
+	if (driver == NULL) {
+		result = -ENOMEM;
 		goto error;
+	}
 
 	/* register this driver with the USB subsystem */
 	result = usb_register(&gigaset_usb_driver);
@@ -915,7 +917,7 @@ error:
 	if (driver)
 		gigaset_freedriver(driver);
 	driver = NULL;
-	return -1;
+	return result;
 }
 
 /*
diff --git a/drivers/isdn/hardware/eicon/capifunc.c b/drivers/isdn/hardware/eicon/capifunc.c
index a576f32e663..7a0bdbdd87e 100644
--- a/drivers/isdn/hardware/eicon/capifunc.c
+++ b/drivers/isdn/hardware/eicon/capifunc.c
@@ -1120,7 +1120,7 @@ int fax_head_line_time(char *buffer)
 /*
  * init (alloc) main structures
  */
-static int DIVA_INIT_FUNCTION init_main_structs(void)
+static int __init init_main_structs(void)
 {
 	if (!(mapped_msg = (CAPI_MSG *) diva_os_malloc(0, MAX_MSG_SIZE))) {
 		DBG_ERR(("init: failed alloc mapped_msg."))
@@ -1181,7 +1181,7 @@ static void do_api_remove_start(void)
 /*
  * init
  */
-int DIVA_INIT_FUNCTION init_capifunc(void)
+int __init init_capifunc(void)
 {
 	diva_os_initialize_spin_lock(&api_lock, "capifunc");
 	memset(ControllerMap, 0, MAX_DESCRIPTORS + 1);
@@ -1209,7 +1209,7 @@ int DIVA_INIT_FUNCTION init_capifunc(void)
 /*
  * finit
  */
-void DIVA_EXIT_FUNCTION finit_capifunc(void)
+void __exit finit_capifunc(void)
 {
 	do_api_remove_start();
 	divacapi_disconnect_didd();
diff --git a/drivers/isdn/hardware/eicon/capimain.c b/drivers/isdn/hardware/eicon/capimain.c
index eabe0fa1b62..997d46abf5b 100644
--- a/drivers/isdn/hardware/eicon/capimain.c
+++ b/drivers/isdn/hardware/eicon/capimain.c
@@ -118,7 +118,7 @@ void diva_os_set_controller_struct(struct capi_ctr *ctrl)
 /*
  * module init
  */
-static int DIVA_INIT_FUNCTION divacapi_init(void)
+static int __init divacapi_init(void)
 {
 	char tmprev[32];
 	int ret = 0;
@@ -144,7 +144,7 @@ static int DIVA_INIT_FUNCTION divacapi_init(void)
 /*
  * module exit
  */
-static void DIVA_EXIT_FUNCTION divacapi_exit(void)
+static void __exit divacapi_exit(void)
 {
 	finit_capifunc();
 	printk(KERN_INFO "%s: module unloaded.\n", DRIVERLNAME);
diff --git a/drivers/isdn/hardware/eicon/diddfunc.c b/drivers/isdn/hardware/eicon/diddfunc.c
index c4c8220c9d7..b0b23ed8b37 100644
--- a/drivers/isdn/hardware/eicon/diddfunc.c
+++ b/drivers/isdn/hardware/eicon/diddfunc.c
@@ -47,7 +47,7 @@ static void *didd_callback(void *context, DESCRIPTOR *adapter,
 /*
  * connect to didd
  */
-static int DIVA_INIT_FUNCTION connect_didd(void)
+static int __init connect_didd(void)
 {
 	int x = 0;
 	int dadapter = 0;
@@ -79,7 +79,7 @@ static int DIVA_INIT_FUNCTION connect_didd(void)
 /*
  * disconnect from didd
  */
-static void DIVA_EXIT_FUNCTION disconnect_didd(void)
+static void __exit disconnect_didd(void)
 {
 	IDI_SYNC_REQ req;
 
@@ -92,7 +92,7 @@ static void DIVA_EXIT_FUNCTION disconnect_didd(void)
 /*
  * init
  */
-int DIVA_INIT_FUNCTION diddfunc_init(void)
+int __init diddfunc_init(void)
 {
 	diva_didd_load_time_init();
 
@@ -107,7 +107,7 @@ int DIVA_INIT_FUNCTION diddfunc_init(void)
 /*
  * finit
  */
-void DIVA_EXIT_FUNCTION diddfunc_finit(void)
+void __exit diddfunc_finit(void)
 {
 	DbgDeregister();
 	disconnect_didd();
diff --git a/drivers/isdn/hardware/eicon/diva_didd.c b/drivers/isdn/hardware/eicon/diva_didd.c
index d1d3de03cce..fab6ccfb00d 100644
--- a/drivers/isdn/hardware/eicon/diva_didd.c
+++ b/drivers/isdn/hardware/eicon/diva_didd.c
@@ -91,7 +91,7 @@ static const struct file_operations divadidd_proc_fops = {
 	.release	= single_release,
 };
 
-static int DIVA_INIT_FUNCTION create_proc(void)
+static int __init create_proc(void)
 {
 	proc_net_eicon = proc_mkdir("eicon", init_net.proc_net);
 
@@ -109,7 +109,7 @@ static void remove_proc(void)
 	remove_proc_entry("eicon", init_net.proc_net);
 }
 
-static int DIVA_INIT_FUNCTION divadidd_init(void)
+static int __init divadidd_init(void)
 {
 	char tmprev[32];
 	int ret = 0;
@@ -141,7 +141,7 @@ out:
 	return (ret);
 }
 
-static void DIVA_EXIT_FUNCTION divadidd_exit(void)
+static void __exit divadidd_exit(void)
 {
 	diddfunc_finit();
 	remove_proc();
diff --git a/drivers/isdn/hardware/eicon/divamnt.c b/drivers/isdn/hardware/eicon/divamnt.c
index ffa0c31be74..48db08d0bb3 100644
--- a/drivers/isdn/hardware/eicon/divamnt.c
+++ b/drivers/isdn/hardware/eicon/divamnt.c
@@ -184,7 +184,7 @@ static void divas_maint_unregister_chrdev(void)
 	unregister_chrdev(major, DEVNAME);
 }
 
-static int DIVA_INIT_FUNCTION divas_maint_register_chrdev(void)
+static int __init divas_maint_register_chrdev(void)
 {
 	if ((major = register_chrdev(0, DEVNAME, &divas_maint_fops)) < 0)
 	{
@@ -207,7 +207,7 @@ void diva_maint_wakeup_read(void)
 /*
  *  Driver Load
  */
-static int DIVA_INIT_FUNCTION maint_init(void)
+static int __init maint_init(void)
 {
 	char tmprev[50];
 	int ret = 0;
@@ -245,7 +245,7 @@ out:
 /*
 **  Driver Unload
 */
-static void DIVA_EXIT_FUNCTION maint_exit(void)
+static void __exit maint_exit(void)
 {
 	divas_maint_unregister_chrdev();
 	mntfunc_finit();
diff --git a/drivers/isdn/hardware/eicon/divasfunc.c b/drivers/isdn/hardware/eicon/divasfunc.c
index 60aaf958095..4be5f881477 100644
--- a/drivers/isdn/hardware/eicon/divasfunc.c
+++ b/drivers/isdn/hardware/eicon/divasfunc.c
@@ -153,7 +153,7 @@ static void *didd_callback(void *context, DESCRIPTOR *adapter,
 /*
  * connect to didd
  */
-static int DIVA_INIT_FUNCTION connect_didd(void)
+static int __init connect_didd(void)
 {
 	int x = 0;
 	int dadapter = 0;
@@ -209,7 +209,7 @@ static void disconnect_didd(void)
 /*
  * init
  */
-int DIVA_INIT_FUNCTION divasfunc_init(int dbgmask)
+int __init divasfunc_init(int dbgmask)
 {
 	char *version;
 
diff --git a/drivers/isdn/hardware/eicon/divasi.c b/drivers/isdn/hardware/eicon/divasi.c
index a5c8f90b3b3..4103a8c178d 100644
--- a/drivers/isdn/hardware/eicon/divasi.c
+++ b/drivers/isdn/hardware/eicon/divasi.c
@@ -114,7 +114,7 @@ static const struct file_operations um_idi_proc_fops = {
 	.release	= single_release,
 };
 
-static int DIVA_INIT_FUNCTION create_um_idi_proc(void)
+static int __init create_um_idi_proc(void)
 {
 	um_idi_proc_entry = proc_create(DRIVERLNAME, S_IRUGO, proc_net_eicon,
 					&um_idi_proc_fops);
@@ -146,7 +146,7 @@ static void divas_idi_unregister_chrdev(void)
 	unregister_chrdev(major, DEVNAME);
 }
 
-static int DIVA_INIT_FUNCTION divas_idi_register_chrdev(void)
+static int __init divas_idi_register_chrdev(void)
 {
 	if ((major = register_chrdev(0, DEVNAME, &divas_idi_fops)) < 0)
 	{
@@ -161,7 +161,7 @@ static int DIVA_INIT_FUNCTION divas_idi_register_chrdev(void)
 /*
 ** Driver Load
 */
-static int DIVA_INIT_FUNCTION divasi_init(void)
+static int __init divasi_init(void)
 {
 	char tmprev[50];
 	int ret = 0;
@@ -202,7 +202,7 @@ out:
 /*
 ** Driver Unload
 */
-static void DIVA_EXIT_FUNCTION divasi_exit(void)
+static void __exit divasi_exit(void)
 {
 	idifunc_finit();
 	remove_um_idi_proc();
diff --git a/drivers/isdn/hardware/eicon/divasmain.c b/drivers/isdn/hardware/eicon/divasmain.c
index 7eaab06276f..ca6d276bb25 100644
--- a/drivers/isdn/hardware/eicon/divasmain.c
+++ b/drivers/isdn/hardware/eicon/divasmain.c
@@ -673,7 +673,7 @@ static void divas_unregister_chrdev(void)
 	unregister_chrdev(major, DEVNAME);
 }
 
-static int DIVA_INIT_FUNCTION divas_register_chrdev(void)
+static int __init divas_register_chrdev(void)
 {
 	if ((major = register_chrdev(0, DEVNAME, &divas_fops)) < 0)
 	{
@@ -767,7 +767,7 @@ static void __devexit divas_remove_one(struct pci_dev *pdev)
 /* --------------------------------------------------------------------------
    Driver Load / Startup
    -------------------------------------------------------------------------- */
-static int DIVA_INIT_FUNCTION divas_init(void)
+static int __init divas_init(void)
 {
 	char tmprev[50];
 	int ret = 0;
@@ -831,7 +831,7 @@ out:
 /* --------------------------------------------------------------------------
    Driver Unload
    -------------------------------------------------------------------------- */
-static void DIVA_EXIT_FUNCTION divas_exit(void)
+static void __exit divas_exit(void)
 {
 	pci_unregister_driver(&diva_pci_driver);
 	remove_divas_proc();
diff --git a/drivers/isdn/hardware/eicon/idifunc.c b/drivers/isdn/hardware/eicon/idifunc.c
index d153e3cdecf..fef6586fe5a 100644
--- a/drivers/isdn/hardware/eicon/idifunc.c
+++ b/drivers/isdn/hardware/eicon/idifunc.c
@@ -133,7 +133,7 @@ static void um_remove_card(DESCRIPTOR *d)
 /*
  * remove all adapter
  */
-static void DIVA_EXIT_FUNCTION remove_all_idi_proc(void)
+static void __exit remove_all_idi_proc(void)
 {
 	udiva_card *card;
 	diva_os_spin_lock_magic_t old_irql;
@@ -181,7 +181,7 @@ static void *didd_callback(void *context, DESCRIPTOR *adapter,
 /*
  * connect DIDD
  */
-static int DIVA_INIT_FUNCTION connect_didd(void)
+static int __init connect_didd(void)
 {
 	int x = 0;
 	int dadapter = 0;
@@ -225,7 +225,7 @@ static int DIVA_INIT_FUNCTION connect_didd(void)
 /*
  *  Disconnect from DIDD
  */
-static void DIVA_EXIT_FUNCTION disconnect_didd(void)
+static void __exit disconnect_didd(void)
 {
 	IDI_SYNC_REQ req;
 
@@ -240,7 +240,7 @@ static void DIVA_EXIT_FUNCTION disconnect_didd(void)
 /*
  * init
  */
-int DIVA_INIT_FUNCTION idifunc_init(void)
+int __init idifunc_init(void)
 {
 	diva_os_initialize_spin_lock(&ll_lock, "idifunc");
 
@@ -260,7 +260,7 @@ int DIVA_INIT_FUNCTION idifunc_init(void)
 /*
  * finit
  */
-void DIVA_EXIT_FUNCTION idifunc_finit(void)
+void __exit idifunc_finit(void)
 {
 	diva_user_mode_idi_finit();
 	disconnect_didd();
diff --git a/drivers/isdn/hardware/eicon/mntfunc.c b/drivers/isdn/hardware/eicon/mntfunc.c
index d6072607305..1cd9affb605 100644
--- a/drivers/isdn/hardware/eicon/mntfunc.c
+++ b/drivers/isdn/hardware/eicon/mntfunc.c
@@ -72,7 +72,7 @@ static void *didd_callback(void *context, DESCRIPTOR *adapter,
 /*
  * connect to didd
  */
-static int DIVA_INIT_FUNCTION connect_didd(void)
+static int __init connect_didd(void)
 {
 	int x = 0;
 	int dadapter = 0;
@@ -114,7 +114,7 @@ static int DIVA_INIT_FUNCTION connect_didd(void)
 /*
  * disconnect from didd
  */
-static void DIVA_EXIT_FUNCTION disconnect_didd(void)
+static void __exit disconnect_didd(void)
 {
 	IDI_SYNC_REQ req;
 
@@ -300,7 +300,7 @@ int maint_read_write(void __user *buf, int count)
 /*
  *  init
  */
-int DIVA_INIT_FUNCTION mntfunc_init(int *buffer_length, void **buffer,
+int __init mntfunc_init(int *buffer_length, void **buffer,
 				    unsigned long diva_dbg_mem)
 {
 	if (*buffer_length < 64) {
@@ -348,7 +348,7 @@ int DIVA_INIT_FUNCTION mntfunc_init(int *buffer_length, void **buffer,
 /*
  *  exit
  */
-void DIVA_EXIT_FUNCTION mntfunc_finit(void)
+void __exit mntfunc_finit(void)
 {
 	void *buffer;
 	int i = 100;
diff --git a/drivers/isdn/hardware/eicon/platform.h b/drivers/isdn/hardware/eicon/platform.h
index 7331c3b14a5..b2edb7590dd 100644
--- a/drivers/isdn/hardware/eicon/platform.h
+++ b/drivers/isdn/hardware/eicon/platform.h
@@ -38,9 +38,6 @@
 #define DIVA_NO_DEBUGLIB
 #endif
 
-#define DIVA_INIT_FUNCTION  __init
-#define DIVA_EXIT_FUNCTION  __exit
-
 #define DIVA_USER_MODE_CARD_CONFIG 1
 #define	USE_EXTENDED_DEBUGS 1
 
diff --git a/drivers/isdn/hardware/mISDN/avmfritz.c b/drivers/isdn/hardware/mISDN/avmfritz.c
index c0b8c960ee3..c08fc605e56 100644
--- a/drivers/isdn/hardware/mISDN/avmfritz.c
+++ b/drivers/isdn/hardware/mISDN/avmfritz.c
@@ -30,7 +30,7 @@
 #include "ipac.h"
 
 
-#define AVMFRITZ_REV	"2.1"
+#define AVMFRITZ_REV	"2.3"
 
 static int AVM_cnt;
 static int debug;
@@ -69,6 +69,7 @@ enum {
 #define HDLC_MODE_TRANS		0x02
 #define HDLC_MODE_CCR_7		0x04
 #define HDLC_MODE_CCR_16	0x08
+#define HDLC_FIFO_SIZE_128	0x20
 #define HDLC_MODE_TESTLOOP	0x80
 
 #define HDLC_INT_XPR		0x80
@@ -80,13 +81,16 @@ enum {
 #define HDLC_STAT_RDO		0x10
 #define HDLC_STAT_CRCVFRRAB	0x0E
 #define HDLC_STAT_CRCVFR	0x06
-#define HDLC_STAT_RML_MASK	0x3f00
+#define HDLC_STAT_RML_MASK_V1	0x3f00
+#define HDLC_STAT_RML_MASK_V2	0x7f00
 
 #define HDLC_CMD_XRS		0x80
 #define HDLC_CMD_XME		0x01
 #define HDLC_CMD_RRS		0x20
 #define HDLC_CMD_XML_MASK	0x3f00
-#define HDLC_FIFO_SIZE		32
+
+#define HDLC_FIFO_SIZE_V1	32
+#define HDLC_FIFO_SIZE_V2	128
 
 /* Fritz PCI v2.0 */
 
@@ -346,11 +350,14 @@ modehdlc(struct bchannel *bch, int protocol)
 {
 	struct fritzcard *fc = bch->hw;
 	struct hdlc_hw *hdlc;
+	u8 mode;
 
 	hdlc = &fc->hdlc[(bch->nr - 1) & 1];
 	pr_debug("%s: hdlc %c protocol %x-->%x ch %d\n", fc->name,
 		 '@' + bch->nr, bch->state, protocol, bch->nr);
 	hdlc->ctrl.ctrl = 0;
+	mode = (fc->type == AVM_FRITZ_PCIV2) ? HDLC_FIFO_SIZE_128 : 0;
+
 	switch (protocol) {
 	case -1: /* used for init */
 		bch->state = -1;
@@ -358,7 +365,7 @@ modehdlc(struct bchannel *bch, int protocol)
 		if (bch->state == ISDN_P_NONE)
 			break;
 		hdlc->ctrl.sr.cmd  = HDLC_CMD_XRS | HDLC_CMD_RRS;
-		hdlc->ctrl.sr.mode = HDLC_MODE_TRANS;
+		hdlc->ctrl.sr.mode = mode | HDLC_MODE_TRANS;
 		write_ctrl(bch, 5);
 		bch->state = ISDN_P_NONE;
 		test_and_clear_bit(FLG_HDLC, &bch->Flags);
@@ -367,7 +374,7 @@ modehdlc(struct bchannel *bch, int protocol)
 	case ISDN_P_B_RAW:
 		bch->state = protocol;
 		hdlc->ctrl.sr.cmd  = HDLC_CMD_XRS | HDLC_CMD_RRS;
-		hdlc->ctrl.sr.mode = HDLC_MODE_TRANS;
+		hdlc->ctrl.sr.mode = mode | HDLC_MODE_TRANS;
 		write_ctrl(bch, 5);
 		hdlc->ctrl.sr.cmd = HDLC_CMD_XRS;
 		write_ctrl(bch, 1);
@@ -377,7 +384,7 @@ modehdlc(struct bchannel *bch, int protocol)
 	case ISDN_P_B_HDLC:
 		bch->state = protocol;
 		hdlc->ctrl.sr.cmd  = HDLC_CMD_XRS | HDLC_CMD_RRS;
-		hdlc->ctrl.sr.mode = HDLC_MODE_ITF_FLG;
+		hdlc->ctrl.sr.mode = mode | HDLC_MODE_ITF_FLG;
 		write_ctrl(bch, 5);
 		hdlc->ctrl.sr.cmd = HDLC_CMD_XRS;
 		write_ctrl(bch, 1);
@@ -397,39 +404,40 @@ hdlc_empty_fifo(struct bchannel *bch, int count)
 	u32 *ptr;
 	u8 *p;
 	u32  val, addr;
-	int cnt = 0;
+	int cnt;
 	struct fritzcard *fc = bch->hw;
 
 	pr_debug("%s: %s %d\n", fc->name, __func__, count);
-	if (!bch->rx_skb) {
-		bch->rx_skb = mI_alloc_skb(bch->maxlen, GFP_ATOMIC);
-		if (!bch->rx_skb) {
-			pr_info("%s: B receive out of memory\n",
-				fc->name);
+	if (test_bit(FLG_RX_OFF, &bch->Flags)) {
+		p = NULL;
+		bch->dropcnt += count;
+	} else {
+		cnt = bchannel_get_rxbuf(bch, count);
+		if (cnt < 0) {
+			pr_warning("%s.B%d: No bufferspace for %d bytes\n",
+				   fc->name, bch->nr, count);
 			return;
 		}
+		p = skb_put(bch->rx_skb, count);
 	}
-	if ((bch->rx_skb->len + count) > bch->maxlen) {
-		pr_debug("%s: overrun %d\n", fc->name,
-			 bch->rx_skb->len + count);
-		return;
-	}
-	p = skb_put(bch->rx_skb, count);
 	ptr = (u32 *)p;
-	if (AVM_FRITZ_PCIV2 == fc->type)
+	if (fc->type == AVM_FRITZ_PCIV2)
 		addr = fc->addr + (bch->nr == 2 ?
 				   AVM_HDLC_FIFO_2 : AVM_HDLC_FIFO_1);
 	else {
 		addr = fc->addr + CHIP_WINDOW;
 		outl(bch->nr == 2 ? AVM_HDLC_2 : AVM_HDLC_1, fc->addr);
 	}
+	cnt = 0;
 	while (cnt < count) {
 		val = le32_to_cpu(inl(addr));
-		put_unaligned(val, ptr);
-		ptr++;
+		if (p) {
+			put_unaligned(val, ptr);
+			ptr++;
+		}
 		cnt += 4;
 	}
-	if (debug & DEBUG_HW_BFIFO) {
+	if (p && (debug & DEBUG_HW_BFIFO)) {
 		snprintf(fc->log, LOG_SIZE, "B%1d-recv %s %d ",
 			 bch->nr, fc->name, count);
 		print_hex_dump_bytes(fc->log, DUMP_PREFIX_OFFSET, p, count);
@@ -441,30 +449,43 @@ hdlc_fill_fifo(struct bchannel *bch)
 {
 	struct fritzcard *fc = bch->hw;
 	struct hdlc_hw *hdlc;
-	int count, cnt = 0;
+	int count, fs, cnt = 0, idx, fillempty = 0;
 	u8 *p;
 	u32 *ptr, val, addr;
 
-	hdlc = &fc->hdlc[(bch->nr - 1) & 1];
-	if (!bch->tx_skb)
-		return;
-	count = bch->tx_skb->len - bch->tx_idx;
-	if (count <= 0)
-		return;
-	p = bch->tx_skb->data + bch->tx_idx;
+	idx = (bch->nr - 1) & 1;
+	hdlc = &fc->hdlc[idx];
+	fs = (fc->type == AVM_FRITZ_PCIV2) ?
+		HDLC_FIFO_SIZE_V2 : HDLC_FIFO_SIZE_V1;
+	if (!bch->tx_skb) {
+		if (!test_bit(FLG_TX_EMPTY, &bch->Flags))
+			return;
+		count = fs;
+		p = bch->fill;
+		fillempty = 1;
+	} else {
+		count = bch->tx_skb->len - bch->tx_idx;
+		if (count <= 0)
+			return;
+		p = bch->tx_skb->data + bch->tx_idx;
+	}
 	hdlc->ctrl.sr.cmd &= ~HDLC_CMD_XME;
-	if (count > HDLC_FIFO_SIZE) {
-		count = HDLC_FIFO_SIZE;
+	if (count > fs) {
+		count = fs;
 	} else {
 		if (test_bit(FLG_HDLC, &bch->Flags))
 			hdlc->ctrl.sr.cmd |= HDLC_CMD_XME;
 	}
-	pr_debug("%s: %s %d/%d/%d", fc->name, __func__, count,
-		 bch->tx_idx, bch->tx_skb->len);
 	ptr = (u32 *)p;
-	bch->tx_idx += count;
-	hdlc->ctrl.sr.xml = ((count == HDLC_FIFO_SIZE) ? 0 : count);
-	if (AVM_FRITZ_PCIV2 == fc->type) {
+	if (fillempty) {
+		pr_debug("%s.B%d: %d/%d/%d", fc->name, bch->nr, count,
+			 bch->tx_idx, bch->tx_skb->len);
+		bch->tx_idx += count;
+	} else {
+		pr_debug("%s.B%d: fillempty %d\n", fc->name, bch->nr, count);
+	}
+	hdlc->ctrl.sr.xml = ((count == fs) ? 0 : count);
+	if (fc->type == AVM_FRITZ_PCIV2) {
 		__write_ctrl_pciv2(fc, hdlc, bch->nr);
 		addr = fc->addr + (bch->nr == 2 ?
 				   AVM_HDLC_FIFO_2 : AVM_HDLC_FIFO_1);
@@ -472,13 +493,21 @@ hdlc_fill_fifo(struct bchannel *bch)
 		__write_ctrl_pci(fc, hdlc, bch->nr);
 		addr = fc->addr + CHIP_WINDOW;
 	}
-	while (cnt < count) {
-		val = get_unaligned(ptr);
-		outl(cpu_to_le32(val), addr);
-		ptr++;
-		cnt += 4;
+	if (fillempty) {
+		while (cnt < count) {
+			/* all bytes the same - no worry about endian */
+			outl(*ptr, addr);
+			cnt += 4;
+		}
+	} else {
+		while (cnt < count) {
+			val = get_unaligned(ptr);
+			outl(cpu_to_le32(val), addr);
+			ptr++;
+			cnt += 4;
+		}
 	}
-	if (debug & DEBUG_HW_BFIFO) {
+	if ((debug & DEBUG_HW_BFIFO) && !fillempty) {
 		snprintf(fc->log, LOG_SIZE, "B%1d-send %s %d ",
 			 bch->nr, fc->name, count);
 		print_hex_dump_bytes(fc->log, DUMP_PREFIX_OFFSET, p, count);
@@ -488,17 +517,17 @@ hdlc_fill_fifo(struct bchannel *bch)
 static void
 HDLC_irq_xpr(struct bchannel *bch)
 {
-	if (bch->tx_skb && bch->tx_idx < bch->tx_skb->len)
+	if (bch->tx_skb && bch->tx_idx < bch->tx_skb->len) {
 		hdlc_fill_fifo(bch);
-	else {
-		if (bch->tx_skb) {
-			/* send confirm, on trans, free on hdlc. */
-			if (test_bit(FLG_TRANSPARENT, &bch->Flags))
-				confirm_Bsend(bch);
+	} else {
+		if (bch->tx_skb)
 			dev_kfree_skb(bch->tx_skb);
-		}
-		if (get_next_bframe(bch))
+		if (get_next_bframe(bch)) {
 			hdlc_fill_fifo(bch);
+			test_and_clear_bit(FLG_TX_EMPTY, &bch->Flags);
+		} else if (test_bit(FLG_TX_EMPTY, &bch->Flags)) {
+			hdlc_fill_fifo(bch);
+		}
 	}
 }
 
@@ -506,13 +535,23 @@ static void
 HDLC_irq(struct bchannel *bch, u32 stat)
 {
 	struct fritzcard *fc = bch->hw;
-	int		len;
+	int		len, fs;
+	u32		rmlMask;
 	struct hdlc_hw	*hdlc;
 
 	hdlc = &fc->hdlc[(bch->nr - 1) & 1];
 	pr_debug("%s: ch%d stat %#x\n", fc->name, bch->nr, stat);
+	if (fc->type == AVM_FRITZ_PCIV2) {
+		rmlMask = HDLC_STAT_RML_MASK_V2;
+		fs = HDLC_FIFO_SIZE_V2;
+	} else {
+		rmlMask = HDLC_STAT_RML_MASK_V1;
+		fs = HDLC_FIFO_SIZE_V1;
+	}
 	if (stat & HDLC_INT_RPR) {
 		if (stat & HDLC_STAT_RDO) {
+			pr_warning("%s: ch%d stat %x RDO\n",
+				   fc->name, bch->nr, stat);
 			hdlc->ctrl.sr.xml = 0;
 			hdlc->ctrl.sr.cmd |= HDLC_CMD_RRS;
 			write_ctrl(bch, 1);
@@ -521,21 +560,21 @@ HDLC_irq(struct bchannel *bch, u32 stat)
 			if (bch->rx_skb)
 				skb_trim(bch->rx_skb, 0);
 		} else {
-			len = (stat & HDLC_STAT_RML_MASK) >> 8;
+			len = (stat & rmlMask) >> 8;
 			if (!len)
-				len = 32;
+				len = fs;
 			hdlc_empty_fifo(bch, len);
 			if (!bch->rx_skb)
 				goto handle_tx;
-			if ((stat & HDLC_STAT_RME) || test_bit(FLG_TRANSPARENT,
-							       &bch->Flags)) {
-				if (((stat & HDLC_STAT_CRCVFRRAB) ==
-				     HDLC_STAT_CRCVFR) ||
-				    test_bit(FLG_TRANSPARENT, &bch->Flags)) {
-					recv_Bchannel(bch, 0);
+			if (test_bit(FLG_TRANSPARENT, &bch->Flags)) {
+				recv_Bchannel(bch, 0, false);
+			} else if (stat & HDLC_STAT_RME) {
+				if ((stat & HDLC_STAT_CRCVFRRAB) ==
+				    HDLC_STAT_CRCVFR) {
+					recv_Bchannel(bch, 0, false);
 				} else {
-					pr_debug("%s: got invalid frame\n",
-						 fc->name);
+					pr_warning("%s: got invalid frame\n",
+						   fc->name);
 					skb_trim(bch->rx_skb, 0);
 				}
 			}
@@ -547,16 +586,13 @@ handle_tx:
 		 * restart transmitting the whole frame on HDLC
 		 * in transparent mode we send the next data
 		 */
-		if (bch->tx_skb)
-			pr_debug("%s: ch%d XDU len(%d) idx(%d) Flags(%lx)\n",
-				 fc->name, bch->nr, bch->tx_skb->len,
-				 bch->tx_idx, bch->Flags);
-		else
-			pr_debug("%s: ch%d XDU no tx_skb Flags(%lx)\n",
-				 fc->name, bch->nr, bch->Flags);
+		pr_warning("%s: ch%d stat %x XDU %s\n", fc->name, bch->nr,
+			   stat, bch->tx_skb ? "tx_skb" : "no tx_skb");
 		if (bch->tx_skb && bch->tx_skb->len) {
 			if (!test_bit(FLG_TRANSPARENT, &bch->Flags))
 				bch->tx_idx = 0;
+		} else if (test_bit(FLG_FILLEMPTY, &bch->Flags)) {
+			test_and_set_bit(FLG_TX_EMPTY, &bch->Flags);
 		}
 		hdlc->ctrl.sr.xml = 0;
 		hdlc->ctrl.sr.cmd |= HDLC_CMD_XRS;
@@ -659,22 +695,17 @@ avm_l2l1B(struct mISDNchannel *ch, struct sk_buff *skb)
 	struct fritzcard *fc = bch->hw;
 	int ret = -EINVAL;
 	struct mISDNhead *hh = mISDN_HEAD_P(skb);
-	u32 id;
-	u_long flags;
+	unsigned long flags;
 
 	switch (hh->prim) {
 	case PH_DATA_REQ:
 		spin_lock_irqsave(&fc->lock, flags);
 		ret = bchannel_senddata(bch, skb);
 		if (ret > 0) { /* direct TX */
-			id = hh->id; /* skb can be freed */
 			hdlc_fill_fifo(bch);
 			ret = 0;
-			spin_unlock_irqrestore(&fc->lock, flags);
-			if (!test_bit(FLG_TRANSPARENT, &bch->Flags))
-				queue_ch_frame(ch, PH_DATA_CNF, id, NULL);
-		} else
-			spin_unlock_irqrestore(&fc->lock, flags);
+		}
+		spin_unlock_irqrestore(&fc->lock, flags);
 		return ret;
 	case PH_ACTIVATE_REQ:
 		spin_lock_irqsave(&fc->lock, flags);
@@ -783,7 +814,7 @@ init_card(struct fritzcard *fc)
 		inithdlc(fc);
 		enable_hwirq(fc);
 		/* RESET Receiver and Transmitter */
-		if (AVM_FRITZ_PCIV2 == fc->type) {
+		if (fc->type == AVM_FRITZ_PCIV2) {
 			WriteISAC_V2(fc, ISACX_MASK, 0);
 			WriteISAC_V2(fc, ISACX_CMDRD, 0x41);
 		} else {
@@ -810,21 +841,7 @@ init_card(struct fritzcard *fc)
 static int
 channel_bctrl(struct bchannel *bch, struct mISDN_ctrl_req *cq)
 {
-	int ret = 0;
-	struct fritzcard *fc = bch->hw;
-
-	switch (cq->op) {
-	case MISDN_CTRL_GETOP:
-		cq->op = 0;
-		break;
-		/* Nothing implemented yet */
-	case MISDN_CTRL_FILL_EMPTY:
-	default:
-		pr_info("%s: %s unknown Op %x\n", fc->name, __func__, cq->op);
-		ret = -EINVAL;
-		break;
-	}
-	return ret;
+	return mISDN_ctrl_bchannel(bch, cq);
 }
 
 static int
@@ -839,14 +856,10 @@ avm_bctrl(struct mISDNchannel *ch, u32 cmd, void *arg)
 	switch (cmd) {
 	case CLOSE_CHANNEL:
 		test_and_clear_bit(FLG_OPEN, &bch->Flags);
-		if (test_bit(FLG_ACTIVE, &bch->Flags)) {
-			spin_lock_irqsave(&fc->lock, flags);
-			mISDN_freebchannel(bch);
-			test_and_clear_bit(FLG_TX_BUSY, &bch->Flags);
-			test_and_clear_bit(FLG_ACTIVE, &bch->Flags);
-			modehdlc(bch, ISDN_P_NONE);
-			spin_unlock_irqrestore(&fc->lock, flags);
-		}
+		spin_lock_irqsave(&fc->lock, flags);
+		mISDN_freebchannel(bch);
+		modehdlc(bch, ISDN_P_NONE);
+		spin_unlock_irqrestore(&fc->lock, flags);
 		ch->protocol = ISDN_P_NONE;
 		ch->peer = NULL;
 		module_put(THIS_MODULE);
@@ -868,7 +881,7 @@ channel_ctrl(struct fritzcard  *fc, struct mISDN_ctrl_req *cq)
 
 	switch (cq->op) {
 	case MISDN_CTRL_GETOP:
-		cq->op = MISDN_CTRL_LOOP;
+		cq->op = MISDN_CTRL_LOOP | MISDN_CTRL_L1_TIMER3;
 		break;
 	case MISDN_CTRL_LOOP:
 		/* cq->channel: 0 disable, 1 B1 loop 2 B2 loop, 3 both */
@@ -878,6 +891,9 @@ channel_ctrl(struct fritzcard  *fc, struct mISDN_ctrl_req *cq)
 		}
 		ret = fc->isac.ctrl(&fc->isac, HW_TESTLOOP, cq->channel);
 		break;
+	case MISDN_CTRL_L1_TIMER3:
+		ret = fc->isac.ctrl(&fc->isac, HW_TIMER3_VALUE, cq->p1);
+		break;
 	default:
 		pr_info("%s: %s unknown Op %x\n", fc->name, __func__, cq->op);
 		ret = -EINVAL;
@@ -898,7 +914,6 @@ open_bchannel(struct fritzcard *fc, struct channel_req *rq)
 	bch = &fc->bch[rq->adr.channel - 1];
 	if (test_and_set_bit(FLG_OPEN, &bch->Flags))
 		return -EBUSY; /* b-channel can be only open once */
-	test_and_clear_bit(FLG_FILLEMPTY, &bch->Flags);
 	bch->ch.protocol = rq->protocol;
 	rq->ch = &bch->ch;
 	return 0;
@@ -1021,6 +1036,7 @@ static int __devinit
 setup_instance(struct fritzcard *card)
 {
 	int i, err;
+	unsigned short minsize;
 	u_long flags;
 
 	snprintf(card->name, MISDN_MAX_IDLEN - 1, "AVM.%d", AVM_cnt + 1);
@@ -1040,7 +1056,11 @@ setup_instance(struct fritzcard *card)
 	for (i = 0; i < 2; i++) {
 		card->bch[i].nr = i + 1;
 		set_channelmap(i + 1, card->isac.dch.dev.channelmap);
-		mISDN_initbchannel(&card->bch[i], MAX_DATA_MEM);
+		if (AVM_FRITZ_PCIV2 == card->type)
+			minsize = HDLC_FIFO_SIZE_V2;
+		else
+			minsize = HDLC_FIFO_SIZE_V1;
+		mISDN_initbchannel(&card->bch[i], MAX_DATA_MEM, minsize);
 		card->bch[i].hw = card;
 		card->bch[i].ch.send = avm_l2l1B;
 		card->bch[i].ch.ctrl = avm_bctrl;
diff --git a/drivers/isdn/hardware/mISDN/hfc_multi.h b/drivers/isdn/hardware/mISDN/hfc_multi.h
index b0588acbb47..c601f880141 100644
--- a/drivers/isdn/hardware/mISDN/hfc_multi.h
+++ b/drivers/isdn/hardware/mISDN/hfc_multi.h
@@ -205,18 +205,22 @@ struct hfc_multi {
 
 	u_int		slots;	/* number of PCM slots */
 	u_int		leds;	/* type of leds */
-	u_int		ledcount; /* used to animate leds */
 	u_long		ledstate; /* save last state of leds */
 	int		opticalsupport; /* has the e1 board */
 					/* an optical Interface */
-	int		dslot;	/* channel # of d-channel (E1) default 16 */
+
+	u_int		bmask[32]; /* bitmask of bchannels for port */
+	u_char		dnum[32]; /* array of used dchannel numbers for port */
+	u_char		created[32]; /* what port is created */
+	u_int		activity_tx; /* if there is data TX / RX */
+	u_int		activity_rx; /* bitmask according to port number */
+				     /* (will be cleared after */
+				     /* showing led-states) */
+	u_int		flash[8]; /* counter for flashing 8 leds on activity */
 
 	u_long		wdcount;	/* every 500 ms we need to */
 					/* send the watchdog a signal */
 	u_char		wdbyte; /* watchdog toggle byte */
-	u_int		activity[8];	/* if there is any action on this */
-					/* port (will be cleared after */
-					/* showing led-states) */
 	int		e1_state; /* keep track of last state */
 	int		e1_getclock; /* if sync is retrieved from interface */
 	int		syncronized; /* keep track of existing sync interface */
@@ -233,7 +237,6 @@ struct hfc_multi {
 	 * the bch->channel is equvalent to the hfc-channel
 	 */
 	struct hfc_chan	chan[32];
-	u_char		created[8]; /* what port is created */
 	signed char	slot_owner[256]; /* owner channel of slot */
 };
 
diff --git a/drivers/isdn/hardware/mISDN/hfcmulti.c b/drivers/isdn/hardware/mISDN/hfcmulti.c
index 033223180b5..5e402cf2e79 100644
--- a/drivers/isdn/hardware/mISDN/hfcmulti.c
+++ b/drivers/isdn/hardware/mISDN/hfcmulti.c
@@ -103,14 +103,26 @@
  *	Omit this value, if all cards are interconnected or none is connected.
  *	If unsure, don't give this parameter.
  *
- * dslot:
- *	NOTE: only one dslot value must be given for every card.
- *	Also this value must be given for non-E1 cards. If omitted, the E1
- *	card has D-channel on time slot 16, which is default.
- *	If 1..15 or 17..31, an alternate time slot is used for D-channel.
- *	In this case, the application must be able to handle this.
- *	If -1 is given, the D-channel is disabled and all 31 slots can be used
- *	for B-channel. (only for specific applications)
+ * dmask and bmask:
+ *	NOTE: One dmask value must be given for every HFC-E1 card.
+ *	If omitted, the E1 card has D-channel on time slot 16, which is default.
+ *	dmask is a 32 bit mask. The bit must be set for an alternate time slot.
+ *	If multiple bits are set, multiple virtual card fragments are created.
+ *	For each bit set, a bmask value must be given. Each bit on the bmask
+ *	value stands for a B-channel. The bmask may not overlap with dmask or
+ *	with other bmask values for that card.
+ *	Example: dmask=0x00020002 bmask=0x0000fffc,0xfffc0000
+ *		This will create one fragment with D-channel on slot 1 with
+ *		B-channels on slots 2..15, and a second fragment with D-channel
+ *		on slot 17 with B-channels on slot 18..31. Slot 16 is unused.
+ *	If bit 0 is set (dmask=0x00000001) the D-channel is on slot 0 and will
+ *	not function.
+ *	Example: dmask=0x00000001 bmask=0xfffffffe
+ *		This will create a port with all 31 usable timeslots as
+ *		B-channels.
+ *	If no bits are set on bmask, no B-channel is created for that fragment.
+ *	Example: dmask=0xfffffffe bmask=0,0,0,0.... (31 0-values for bmask)
+ *		This will create 31 ports with one D-channel only.
  *	If you don't know how to use it, you don't need it!
  *
  * iomode:
@@ -172,6 +184,7 @@
 
 #define	MAX_CARDS	8
 #define	MAX_PORTS	(8 * MAX_CARDS)
+#define	MAX_FRAGS	(32 * MAX_CARDS)
 
 static LIST_HEAD(HFClist);
 static spinlock_t HFClock; /* global hfc list lock */
@@ -203,7 +216,8 @@ static int nt_t1_count[] = { 3840, 1920, 960, 480, 240, 120, 60, 30  };
 
 static uint	type[MAX_CARDS];
 static int	pcm[MAX_CARDS];
-static int	dslot[MAX_CARDS];
+static uint	dmask[MAX_CARDS];
+static uint	bmask[MAX_FRAGS];
 static uint	iomode[MAX_CARDS];
 static uint	port[MAX_PORTS];
 static uint	debug;
@@ -218,7 +232,7 @@ static uint	clockdelay_nt = CLKDEL_NT;
 #define HWID_MINIP16	3
 static uint	hwid = HWID_NONE;
 
-static int	HFC_cnt, Port_cnt, PCM_cnt = 99;
+static int	HFC_cnt, E1_cnt, bmask_cnt, Port_cnt, PCM_cnt = 99;
 
 MODULE_AUTHOR("Andreas Eversberg");
 MODULE_LICENSE("GPL");
@@ -231,7 +245,8 @@ module_param(clockdelay_te, uint, S_IRUGO | S_IWUSR);
 module_param(clockdelay_nt, uint, S_IRUGO | S_IWUSR);
 module_param_array(type, uint, NULL, S_IRUGO | S_IWUSR);
 module_param_array(pcm, int, NULL, S_IRUGO | S_IWUSR);
-module_param_array(dslot, int, NULL, S_IRUGO | S_IWUSR);
+module_param_array(dmask, uint, NULL, S_IRUGO | S_IWUSR);
+module_param_array(bmask, uint, NULL, S_IRUGO | S_IWUSR);
 module_param_array(iomode, uint, NULL, S_IRUGO | S_IWUSR);
 module_param_array(port, uint, NULL, S_IRUGO | S_IWUSR);
 module_param(hwid, uint, S_IRUGO | S_IWUSR); /* The hardware ID */
@@ -1156,7 +1171,7 @@ init_chip(struct hfc_multi *hc)
 	hc->DTMFbase = 0x1000;
 	if (test_bit(HFC_CHIP_EXRAM_128, &hc->chip)) {
 		if (debug & DEBUG_HFCMULTI_INIT)
-			printk(KERN_DEBUG "%s: changing to 128K extenal RAM\n",
+			printk(KERN_DEBUG "%s: changing to 128K external RAM\n",
 			       __func__);
 		hc->hw.r_ctrl |= V_EXT_RAM;
 		hc->hw.r_ram_sz = 1;
@@ -1167,7 +1182,7 @@ init_chip(struct hfc_multi *hc)
 	}
 	if (test_bit(HFC_CHIP_EXRAM_512, &hc->chip)) {
 		if (debug & DEBUG_HFCMULTI_INIT)
-			printk(KERN_DEBUG "%s: changing to 512K extenal RAM\n",
+			printk(KERN_DEBUG "%s: changing to 512K external RAM\n",
 			       __func__);
 		hc->hw.r_ctrl |= V_EXT_RAM;
 		hc->hw.r_ram_sz = 2;
@@ -1607,40 +1622,46 @@ hfcmulti_leds(struct hfc_multi *hc)
 	struct dchannel *dch;
 	int led[4];
 
-	hc->ledcount += poll;
-	if (hc->ledcount > 4096) {
-		hc->ledcount -= 4096;
-		hc->ledstate = 0xAFFEAFFE;
-	}
-
 	switch (hc->leds) {
 	case 1: /* HFC-E1 OEM */
-		/* 2 red blinking: NT mode deactivate
-		 * 2 red steady:   TE mode deactivate
-		 * left green:     L1 active
-		 * left red:       frame sync, but no L1
-		 * right green:    L2 active
+		/* 2 red steady:       LOS
+		 * 1 red steady:       L1 not active
+		 * 2 green steady:     L1 active
+		 * 1st green flashing: activity on TX
+		 * 2nd green flashing: activity on RX
 		 */
-		if (hc->chan[hc->dslot].sync != 2) { /* no frame sync */
-			if (hc->chan[hc->dslot].dch->dev.D.protocol
-			    != ISDN_P_NT_E1) {
-				led[0] = 1;
+		led[0] = 0;
+		led[1] = 0;
+		led[2] = 0;
+		led[3] = 0;
+		dch = hc->chan[hc->dnum[0]].dch;
+		if (dch) {
+			if (hc->chan[hc->dnum[0]].los)
 				led[1] = 1;
-			} else if (hc->ledcount >> 11) {
+			if (hc->e1_state != 1) {
 				led[0] = 1;
-				led[1] = 1;
+				hc->flash[2] = 0;
+				hc->flash[3] = 0;
 			} else {
-				led[0] = 0;
-				led[1] = 0;
+				led[2] = 1;
+				led[3] = 1;
+				if (!hc->flash[2] && hc->activity_tx)
+					hc->flash[2] = poll;
+				if (!hc->flash[3] && hc->activity_rx)
+					hc->flash[3] = poll;
+				if (hc->flash[2] && hc->flash[2] < 1024)
+					led[2] = 0;
+				if (hc->flash[3] && hc->flash[3] < 1024)
+					led[3] = 0;
+				if (hc->flash[2] >= 2048)
+					hc->flash[2] = 0;
+				if (hc->flash[3] >= 2048)
+					hc->flash[3] = 0;
+				if (hc->flash[2])
+					hc->flash[2] += poll;
+				if (hc->flash[3])
+					hc->flash[3] += poll;
 			}
-			led[2] = 0;
-			led[3] = 0;
-		} else { /* with frame sync */
-			/* TODO make it work */
-			led[0] = 0;
-			led[1] = 0;
-			led[2] = 0;
-			led[3] = 1;
 		}
 		leds = (led[0] | (led[1]<<2) | (led[2]<<1) | (led[3]<<3))^0xF;
 		/* leds are inverted */
@@ -1651,9 +1672,9 @@ hfcmulti_leds(struct hfc_multi *hc)
 		break;
 
 	case 2: /* HFC-4S OEM */
-		/* red blinking = PH_DEACTIVATE NT Mode
-		 * red steady   = PH_DEACTIVATE TE Mode
-		 * green steady = PH_ACTIVATE
+		/* red steady:     PH_DEACTIVATE
+		 * green steady:   PH_ACTIVATE
+		 * green flashing: activity on TX
 		 */
 		for (i = 0; i < 4; i++) {
 			state = 0;
@@ -1669,17 +1690,20 @@ hfcmulti_leds(struct hfc_multi *hc)
 			if (state) {
 				if (state == active) {
 					led[i] = 1; /* led green */
-				} else
-					if (dch->dev.D.protocol == ISDN_P_TE_S0)
-						/* TE mode: led red */
-						led[i] = 2;
-					else
-						if (hc->ledcount >> 11)
-							/* led red */
-							led[i] = 2;
-						else
-							/* led off */
-							led[i] = 0;
+					hc->activity_tx |= hc->activity_rx;
+					if (!hc->flash[i] &&
+						(hc->activity_tx & (1 << i)))
+							hc->flash[i] = poll;
+					if (hc->flash[i] && hc->flash[i] < 1024)
+						led[i] = 0; /* led off */
+					if (hc->flash[i] >= 2048)
+						hc->flash[i] = 0;
+					if (hc->flash[i])
+						hc->flash[i] += poll;
+				} else {
+					led[i] = 2; /* led red */
+					hc->flash[i] = 0;
+				}
 			} else
 				led[i] = 0; /* led off */
 		}
@@ -1712,9 +1736,9 @@ hfcmulti_leds(struct hfc_multi *hc)
 		break;
 
 	case 3: /* HFC 1S/2S Beronet */
-		/* red blinking = PH_DEACTIVATE NT Mode
-		 * red steady   = PH_DEACTIVATE TE Mode
-		 * green steady = PH_ACTIVATE
+		/* red steady:     PH_DEACTIVATE
+		 * green steady:   PH_ACTIVATE
+		 * green flashing: activity on TX
 		 */
 		for (i = 0; i < 2; i++) {
 			state = 0;
@@ -1730,22 +1754,23 @@ hfcmulti_leds(struct hfc_multi *hc)
 			if (state) {
 				if (state == active) {
 					led[i] = 1; /* led green */
-				} else
-					if (dch->dev.D.protocol == ISDN_P_TE_S0)
-						/* TE mode: led red */
-						led[i] = 2;
-					else
-						if (hc->ledcount >> 11)
-							/* led red */
-							led[i] = 2;
-						else
-							/* led off */
-							led[i] = 0;
+					hc->activity_tx |= hc->activity_rx;
+					if (!hc->flash[i] &&
+						(hc->activity_tx & (1 << i)))
+							hc->flash[i] = poll;
+					if (hc->flash[i] < 1024)
+						led[i] = 0; /* led off */
+					if (hc->flash[i] >= 2048)
+						hc->flash[i] = 0;
+					if (hc->flash[i])
+						hc->flash[i] += poll;
+				} else {
+					led[i] = 2; /* led red */
+					hc->flash[i] = 0;
+				}
 			} else
 				led[i] = 0; /* led off */
 		}
-
-
 		leds = (led[0] > 0) | ((led[1] > 0) << 1) | ((led[0]&1) << 2)
 			| ((led[1]&1) << 3);
 		if (leds != (int)hc->ledstate) {
@@ -1757,8 +1782,11 @@ hfcmulti_leds(struct hfc_multi *hc)
 		}
 		break;
 	case 8: /* HFC 8S+ Beronet */
-		lled = 0;
-
+		/* off:      PH_DEACTIVATE
+		 * steady:   PH_ACTIVATE
+		 * flashing: activity on TX
+		 */
+		lled = 0xff; /* leds off */
 		for (i = 0; i < 8; i++) {
 			state = 0;
 			active = -1;
@@ -1772,14 +1800,20 @@ hfcmulti_leds(struct hfc_multi *hc)
 			}
 			if (state) {
 				if (state == active) {
-					lled |= 0 << i;
+					lled &= ~(1 << i); /* led on */
+					hc->activity_tx |= hc->activity_rx;
+					if (!hc->flash[i] &&
+						(hc->activity_tx & (1 << i)))
+							hc->flash[i] = poll;
+					if (hc->flash[i] < 1024)
+						lled |= 1 << i; /* led off */
+					if (hc->flash[i] >= 2048)
+						hc->flash[i] = 0;
+					if (hc->flash[i])
+						hc->flash[i] += poll;
 				} else
-					if (hc->ledcount >> 11)
-						lled |= 0 << i;
-					else
-						lled |= 1 << i;
-			} else
-				lled |= 1 << i;
+					hc->flash[i] = 0;
+			}
 		}
 		leddw = lled << 24 | lled << 16 | lled << 8 | lled;
 		if (leddw != hc->ledstate) {
@@ -1794,6 +1828,8 @@ hfcmulti_leds(struct hfc_multi *hc)
 		}
 		break;
 	}
+	hc->activity_tx = 0;
+	hc->activity_rx = 0;
 }
 /*
  * read dtmf coefficients
@@ -2093,7 +2129,8 @@ next_frame:
 	*txpending = 1;
 
 	/* show activity */
-	hc->activity[hc->chan[ch].port] = 1;
+	if (dch)
+		hc->activity_tx |= 1 << hc->chan[ch].port;
 
 	/* fill fifo to what we have left */
 	ii = len;
@@ -2129,13 +2166,9 @@ next_frame:
 		HFC_wait_nodebug(hc);
 	}
 
-	/* send confirm, since get_net_bframe will not do it with trans */
-	if (bch && test_bit(FLG_TRANSPARENT, &bch->Flags))
-		confirm_Bsend(bch);
-
-	/* check for next frame */
 	dev_kfree_skb(*sp);
-	if (bch && get_next_bframe(bch)) { /* hdlc is confirmed here */
+	/* check for next frame */
+	if (bch && get_next_bframe(bch)) {
 		len = (*sp)->len;
 		goto next_frame;
 	}
@@ -2163,24 +2196,20 @@ hfcmulti_rx(struct hfc_multi *hc, int ch)
 	int f1 = 0, f2 = 0; /* = 0, to make GCC happy */
 	int again = 0;
 	struct	bchannel *bch;
-	struct  dchannel *dch;
+	struct  dchannel *dch = NULL;
 	struct sk_buff	*skb, **sp = NULL;
 	int	maxlen;
 
 	bch = hc->chan[ch].bch;
-	dch = hc->chan[ch].dch;
-	if ((!dch) && (!bch))
-		return;
-	if (dch) {
+	if (bch) {
+		if (!test_bit(FLG_ACTIVE, &bch->Flags))
+			return;
+	} else if (hc->chan[ch].dch) {
+		dch = hc->chan[ch].dch;
 		if (!test_bit(FLG_ACTIVE, &dch->Flags))
 			return;
-		sp = &dch->rx_skb;
-		maxlen = dch->maxlen;
 	} else {
-		if (!test_bit(FLG_ACTIVE, &bch->Flags))
-			return;
-		sp = &bch->rx_skb;
-		maxlen = bch->maxlen;
+		return;
 	}
 next_frame:
 	/* on first AND before getting next valid frame, R_FIFO must be written
@@ -2195,8 +2224,11 @@ next_frame:
 	HFC_wait_nodebug(hc);
 
 	/* ignore if rx is off BUT change fifo (above) to start pending TX */
-	if (hc->chan[ch].rx_off)
+	if (hc->chan[ch].rx_off) {
+		if (bch)
+			bch->dropcnt += poll; /* not exact but fair enough */
 		return;
+	}
 
 	if (dch || test_bit(FLG_HDLC, &bch->Flags)) {
 		f1 = HFC_inb_nodebug(hc, A_F1);
@@ -2227,16 +2259,30 @@ next_frame:
 	if (Zsize <= 0)
 		return;
 
-	if (*sp == NULL) {
-		*sp = mI_alloc_skb(maxlen + 3, GFP_ATOMIC);
-		if (*sp == NULL) {
-			printk(KERN_DEBUG "%s: No mem for rx_skb\n",
-			       __func__);
+	if (bch) {
+		maxlen = bchannel_get_rxbuf(bch, Zsize);
+		if (maxlen < 0) {
+			pr_warning("card%d.B%d: No bufferspace for %d bytes\n",
+				   hc->id + 1, bch->nr, Zsize);
 			return;
 		}
+		sp = &bch->rx_skb;
+		maxlen = bch->maxlen;
+	} else { /* Dchannel */
+		sp = &dch->rx_skb;
+		maxlen = dch->maxlen + 3;
+		if (*sp == NULL) {
+			*sp = mI_alloc_skb(maxlen, GFP_ATOMIC);
+			if (*sp == NULL) {
+				pr_warning("card%d: No mem for dch rx_skb\n",
+					   hc->id + 1);
+				return;
+			}
+		}
 	}
 	/* show activity */
-	hc->activity[hc->chan[ch].port] = 1;
+	if (dch)
+		hc->activity_rx |= 1 << hc->chan[ch].port;
 
 	/* empty fifo with what we have */
 	if (dch || test_bit(FLG_HDLC, &bch->Flags)) {
@@ -2247,7 +2293,7 @@ next_frame:
 			       Zsize, z1, z2, (f1 == f2) ? "fragment" : "COMPLETE",
 			       f1, f2, Zsize + (*sp)->len, again);
 		/* HDLC */
-		if ((Zsize + (*sp)->len) > (maxlen + 3)) {
+		if ((Zsize + (*sp)->len) > maxlen) {
 			if (debug & DEBUG_HFCMULTI_FIFO)
 				printk(KERN_DEBUG
 				       "%s(card %d): hdlc-frame too large.\n",
@@ -2309,7 +2355,7 @@ next_frame:
 			if (dch)
 				recv_Dchannel(dch);
 			else
-				recv_Bchannel(bch, MISDN_ID_ANY);
+				recv_Bchannel(bch, MISDN_ID_ANY, false);
 			*sp = skb;
 			again++;
 			goto next_frame;
@@ -2317,32 +2363,14 @@ next_frame:
 		/* there is an incomplete frame */
 	} else {
 		/* transparent */
-		if (Zsize > skb_tailroom(*sp))
-			Zsize = skb_tailroom(*sp);
 		hc->read_fifo(hc, skb_put(*sp, Zsize), Zsize);
-		if (((*sp)->len) < MISDN_COPY_SIZE) {
-			skb = *sp;
-			*sp = mI_alloc_skb(skb->len, GFP_ATOMIC);
-			if (*sp) {
-				memcpy(skb_put(*sp, skb->len),
-				       skb->data, skb->len);
-				skb_trim(skb, 0);
-			} else {
-				printk(KERN_DEBUG "%s: No mem\n", __func__);
-				*sp = skb;
-				skb = NULL;
-			}
-		} else {
-			skb = NULL;
-		}
 		if (debug & DEBUG_HFCMULTI_FIFO)
 			printk(KERN_DEBUG
 			       "%s(card %d): fifo(%d) reading %d bytes "
 			       "(z1=%04x, z2=%04x) TRANS\n",
 			       __func__, hc->id + 1, ch, Zsize, z1, z2);
 		/* only bch is transparent */
-		recv_Bchannel(bch, hc->chan[ch].Zfill);
-		*sp = skb;
+		recv_Bchannel(bch, hc->chan[ch].Zfill, false);
 	}
 }
 
@@ -2430,55 +2458,55 @@ handle_timer_irq(struct hfc_multi *hc)
 			}
 		}
 	if (hc->ctype == HFC_TYPE_E1 && hc->created[0]) {
-		dch = hc->chan[hc->dslot].dch;
-		if (test_bit(HFC_CFG_REPORT_LOS, &hc->chan[hc->dslot].cfg)) {
-			/* LOS */
-			temp = HFC_inb_nodebug(hc, R_SYNC_STA) & V_SIG_LOS;
-			if (!temp && hc->chan[hc->dslot].los)
+		dch = hc->chan[hc->dnum[0]].dch;
+		/* LOS */
+		temp = HFC_inb_nodebug(hc, R_SYNC_STA) & V_SIG_LOS;
+		hc->chan[hc->dnum[0]].los = temp;
+		if (test_bit(HFC_CFG_REPORT_LOS, &hc->chan[hc->dnum[0]].cfg)) {
+			if (!temp && hc->chan[hc->dnum[0]].los)
 				signal_state_up(dch, L1_SIGNAL_LOS_ON,
 						"LOS detected");
-			if (temp && !hc->chan[hc->dslot].los)
+			if (temp && !hc->chan[hc->dnum[0]].los)
 				signal_state_up(dch, L1_SIGNAL_LOS_OFF,
 						"LOS gone");
-			hc->chan[hc->dslot].los = temp;
 		}
-		if (test_bit(HFC_CFG_REPORT_AIS, &hc->chan[hc->dslot].cfg)) {
+		if (test_bit(HFC_CFG_REPORT_AIS, &hc->chan[hc->dnum[0]].cfg)) {
 			/* AIS */
 			temp = HFC_inb_nodebug(hc, R_SYNC_STA) & V_AIS;
-			if (!temp && hc->chan[hc->dslot].ais)
+			if (!temp && hc->chan[hc->dnum[0]].ais)
 				signal_state_up(dch, L1_SIGNAL_AIS_ON,
 						"AIS detected");
-			if (temp && !hc->chan[hc->dslot].ais)
+			if (temp && !hc->chan[hc->dnum[0]].ais)
 				signal_state_up(dch, L1_SIGNAL_AIS_OFF,
 						"AIS gone");
-			hc->chan[hc->dslot].ais = temp;
+			hc->chan[hc->dnum[0]].ais = temp;
 		}
-		if (test_bit(HFC_CFG_REPORT_SLIP, &hc->chan[hc->dslot].cfg)) {
+		if (test_bit(HFC_CFG_REPORT_SLIP, &hc->chan[hc->dnum[0]].cfg)) {
 			/* SLIP */
 			temp = HFC_inb_nodebug(hc, R_SLIP) & V_FOSLIP_RX;
-			if (!temp && hc->chan[hc->dslot].slip_rx)
+			if (!temp && hc->chan[hc->dnum[0]].slip_rx)
 				signal_state_up(dch, L1_SIGNAL_SLIP_RX,
 						" bit SLIP detected RX");
-			hc->chan[hc->dslot].slip_rx = temp;
+			hc->chan[hc->dnum[0]].slip_rx = temp;
 			temp = HFC_inb_nodebug(hc, R_SLIP) & V_FOSLIP_TX;
-			if (!temp && hc->chan[hc->dslot].slip_tx)
+			if (!temp && hc->chan[hc->dnum[0]].slip_tx)
 				signal_state_up(dch, L1_SIGNAL_SLIP_TX,
 						" bit SLIP detected TX");
-			hc->chan[hc->dslot].slip_tx = temp;
+			hc->chan[hc->dnum[0]].slip_tx = temp;
 		}
-		if (test_bit(HFC_CFG_REPORT_RDI, &hc->chan[hc->dslot].cfg)) {
+		if (test_bit(HFC_CFG_REPORT_RDI, &hc->chan[hc->dnum[0]].cfg)) {
 			/* RDI */
 			temp = HFC_inb_nodebug(hc, R_RX_SL0_0) & V_A;
-			if (!temp && hc->chan[hc->dslot].rdi)
+			if (!temp && hc->chan[hc->dnum[0]].rdi)
 				signal_state_up(dch, L1_SIGNAL_RDI_ON,
 						"RDI detected");
-			if (temp && !hc->chan[hc->dslot].rdi)
+			if (temp && !hc->chan[hc->dnum[0]].rdi)
 				signal_state_up(dch, L1_SIGNAL_RDI_OFF,
 						"RDI gone");
-			hc->chan[hc->dslot].rdi = temp;
+			hc->chan[hc->dnum[0]].rdi = temp;
 		}
 		temp = HFC_inb_nodebug(hc, R_JATT_DIR);
-		switch (hc->chan[hc->dslot].sync) {
+		switch (hc->chan[hc->dnum[0]].sync) {
 		case 0:
 			if ((temp & 0x60) == 0x60) {
 				if (debug & DEBUG_HFCMULTI_SYNC)
@@ -2487,10 +2515,10 @@ handle_timer_irq(struct hfc_multi *hc)
 					       "in clock sync\n",
 					       __func__, hc->id);
 				HFC_outb(hc, R_RX_OFF,
-					 hc->chan[hc->dslot].jitter | V_RX_INIT);
+				    hc->chan[hc->dnum[0]].jitter | V_RX_INIT);
 				HFC_outb(hc, R_TX_OFF,
-					 hc->chan[hc->dslot].jitter | V_RX_INIT);
-				hc->chan[hc->dslot].sync = 1;
+				    hc->chan[hc->dnum[0]].jitter | V_RX_INIT);
+				hc->chan[hc->dnum[0]].sync = 1;
 				goto check_framesync;
 			}
 			break;
@@ -2501,7 +2529,7 @@ handle_timer_irq(struct hfc_multi *hc)
 					       "%s: (id=%d) E1 "
 					       "lost clock sync\n",
 					       __func__, hc->id);
-				hc->chan[hc->dslot].sync = 0;
+				hc->chan[hc->dnum[0]].sync = 0;
 				break;
 			}
 		check_framesync:
@@ -2512,7 +2540,7 @@ handle_timer_irq(struct hfc_multi *hc)
 					       "%s: (id=%d) E1 "
 					       "now in frame sync\n",
 					       __func__, hc->id);
-				hc->chan[hc->dslot].sync = 2;
+				hc->chan[hc->dnum[0]].sync = 2;
 			}
 			break;
 		case 2:
@@ -2522,7 +2550,7 @@ handle_timer_irq(struct hfc_multi *hc)
 					       "%s: (id=%d) E1 lost "
 					       "clock & frame sync\n",
 					       __func__, hc->id);
-				hc->chan[hc->dslot].sync = 0;
+				hc->chan[hc->dnum[0]].sync = 0;
 				break;
 			}
 			temp = HFC_inb_nodebug(hc, R_SYNC_STA);
@@ -2532,7 +2560,7 @@ handle_timer_irq(struct hfc_multi *hc)
 					       "%s: (id=%d) E1 "
 					       "lost frame sync\n",
 					       __func__, hc->id);
-				hc->chan[hc->dslot].sync = 1;
+				hc->chan[hc->dnum[0]].sync = 1;
 			}
 			break;
 		}
@@ -2673,7 +2701,7 @@ hfcmulti_interrupt(int intno, void *dev_id)
 	int			i;
 	void __iomem		*plx_acc;
 	u_short			wval;
-	u_char			e1_syncsta, temp;
+	u_char			e1_syncsta, temp, temp2;
 	u_long			flags;
 
 	if (!hc) {
@@ -2748,7 +2776,7 @@ hfcmulti_interrupt(int intno, void *dev_id)
 		if (r_irq_misc & V_STA_IRQ) {
 			if (hc->ctype == HFC_TYPE_E1) {
 				/* state machine */
-				dch = hc->chan[hc->dslot].dch;
+				dch = hc->chan[hc->dnum[0]].dch;
 				e1_syncsta = HFC_inb_nodebug(hc, R_SYNC_STA);
 				if (test_bit(HFC_CHIP_PLXSD, &hc->chip)
 				    && hc->e1_getclock) {
@@ -2758,23 +2786,26 @@ hfcmulti_interrupt(int intno, void *dev_id)
 						hc->syncronized = 0;
 				}
 				/* undocumented: status changes during read */
-				dch->state = HFC_inb_nodebug(hc, R_E1_RD_STA);
-				while (dch->state != (temp =
+				temp = HFC_inb_nodebug(hc, R_E1_RD_STA);
+				while (temp != (temp2 =
 						      HFC_inb_nodebug(hc, R_E1_RD_STA))) {
 					if (debug & DEBUG_HFCMULTI_STATE)
 						printk(KERN_DEBUG "%s: reread "
 						       "STATE because %d!=%d\n",
-						       __func__, temp,
-						       dch->state);
-					dch->state = temp; /* repeat */
+						    __func__, temp, temp2);
+					temp = temp2; /* repeat */
 				}
-				dch->state = HFC_inb_nodebug(hc, R_E1_RD_STA)
-					& 0x7;
-				schedule_event(dch, FLG_PHCHANGE);
+				/* broadcast state change to all fragments */
 				if (debug & DEBUG_HFCMULTI_STATE)
 					printk(KERN_DEBUG
 					       "%s: E1 (id=%d) newstate %x\n",
-					       __func__, hc->id, dch->state);
+					    __func__, hc->id, temp & 0x7);
+				for (i = 0; i < hc->ports; i++) {
+					dch = hc->chan[hc->dnum[i]].dch;
+					dch->state = temp & 0x7;
+					schedule_event(dch, FLG_PHCHANGE);
+				}
+
 				if (test_bit(HFC_CHIP_PLXSD, &hc->chip))
 					plxsd_checksync(hc, 0);
 			}
@@ -3018,8 +3049,10 @@ mode_hfcmulti(struct hfc_multi *hc, int ch, int protocol, int slot_tx,
 			HFC_outb(hc, A_CON_HDLC, 0x20 | V_HDLC_TRP | V_IFF);
 			HFC_outb(hc, A_SUBCH_CFG, 0);
 			HFC_outb(hc, A_IRQ_MSK, 0);
-			HFC_outb(hc, R_INC_RES_FIFO, V_RES_F);
-			HFC_wait(hc);
+			if (hc->chan[ch].protocol != protocol) {
+				HFC_outb(hc, R_INC_RES_FIFO, V_RES_F);
+				HFC_wait(hc);
+			}
 			HFC_outb(hc, R_SLOT, ((((ch / 4) * 8) +
 					       ((ch % 4) * 4) + 1) << 1) | 1);
 			HFC_outb(hc, A_SL_CFG, 0x80 | 0x20 | (ch << 1) | 1);
@@ -3039,8 +3072,10 @@ mode_hfcmulti(struct hfc_multi *hc, int ch, int protocol, int slot_tx,
 			HFC_outb(hc, A_CON_HDLC, 0x20 | V_HDLC_TRP | V_IFF);
 			HFC_outb(hc, A_SUBCH_CFG, 0);
 			HFC_outb(hc, A_IRQ_MSK, 0);
-			HFC_outb(hc, R_INC_RES_FIFO, V_RES_F);
-			HFC_wait(hc);
+			if (hc->chan[ch].protocol != protocol) {
+				HFC_outb(hc, R_INC_RES_FIFO, V_RES_F);
+				HFC_wait(hc);
+			}
 			/* tx silence */
 			HFC_outb_nodebug(hc, A_FIFO_DATA0_NOINC, hc->silence);
 			HFC_outb(hc, R_SLOT, (((ch / 4) * 8) +
@@ -3059,8 +3094,10 @@ mode_hfcmulti(struct hfc_multi *hc, int ch, int protocol, int slot_tx,
 					 V_HDLC_TRP | V_IFF);
 			HFC_outb(hc, A_SUBCH_CFG, 0);
 			HFC_outb(hc, A_IRQ_MSK, 0);
-			HFC_outb(hc, R_INC_RES_FIFO, V_RES_F);
-			HFC_wait(hc);
+			if (hc->chan[ch].protocol != protocol) {
+				HFC_outb(hc, R_INC_RES_FIFO, V_RES_F);
+				HFC_wait(hc);
+			}
 			/* tx silence */
 			HFC_outb_nodebug(hc, A_FIFO_DATA0_NOINC, hc->silence);
 			/* enable RX fifo */
@@ -3075,8 +3112,10 @@ mode_hfcmulti(struct hfc_multi *hc, int ch, int protocol, int slot_tx,
 					 V_HDLC_TRP);
 			HFC_outb(hc, A_SUBCH_CFG, 0);
 			HFC_outb(hc, A_IRQ_MSK, 0);
-			HFC_outb(hc, R_INC_RES_FIFO, V_RES_F);
-			HFC_wait(hc);
+			if (hc->chan[ch].protocol != protocol) {
+				HFC_outb(hc, R_INC_RES_FIFO, V_RES_F);
+				HFC_wait(hc);
+			}
 		}
 		if (hc->ctype != HFC_TYPE_E1) {
 			hc->hw.a_st_ctrl0[hc->chan[ch].port] |=
@@ -3433,8 +3472,7 @@ handle_bmsg(struct mISDNchannel *ch, struct sk_buff *skb)
 	struct hfc_multi	*hc = bch->hw;
 	int			ret = -EINVAL;
 	struct mISDNhead	*hh = mISDN_HEAD_P(skb);
-	unsigned int		id;
-	u_long			flags;
+	unsigned long		flags;
 
 	switch (hh->prim) {
 	case PH_DATA_REQ:
@@ -3443,19 +3481,13 @@ handle_bmsg(struct mISDNchannel *ch, struct sk_buff *skb)
 		spin_lock_irqsave(&hc->lock, flags);
 		ret = bchannel_senddata(bch, skb);
 		if (ret > 0) { /* direct TX */
-			id = hh->id; /* skb can be freed */
 			hfcmulti_tx(hc, bch->slot);
 			ret = 0;
 			/* start fifo */
 			HFC_outb_nodebug(hc, R_FIFO, 0);
 			HFC_wait_nodebug(hc);
-			if (!test_bit(FLG_TRANSPARENT, &bch->Flags)) {
-				spin_unlock_irqrestore(&hc->lock, flags);
-				queue_ch_frame(ch, PH_DATA_CNF, id, NULL);
-			} else
-				spin_unlock_irqrestore(&hc->lock, flags);
-		} else
-			spin_unlock_irqrestore(&hc->lock, flags);
+		}
+		spin_unlock_irqrestore(&hc->lock, flags);
 		return ret;
 	case PH_ACTIVATE_REQ:
 		if (debug & DEBUG_HFCMULTI_MSG)
@@ -3545,10 +3577,11 @@ channel_bctrl(struct bchannel *bch, struct mISDN_ctrl_req *cq)
 
 	switch (cq->op) {
 	case MISDN_CTRL_GETOP:
-		cq->op = MISDN_CTRL_HFC_OP | MISDN_CTRL_HW_FEATURES_OP
-			| MISDN_CTRL_RX_OFF | MISDN_CTRL_FILL_EMPTY;
+		ret = mISDN_ctrl_bchannel(bch, cq);
+		cq->op |= MISDN_CTRL_HFC_OP | MISDN_CTRL_HW_FEATURES_OP;
 		break;
 	case MISDN_CTRL_RX_OFF: /* turn off / on rx stream */
+		ret = mISDN_ctrl_bchannel(bch, cq);
 		hc->chan[bch->slot].rx_off = !!cq->p1;
 		if (!hc->chan[bch->slot].rx_off) {
 			/* reset fifo on rx on */
@@ -3561,11 +3594,10 @@ channel_bctrl(struct bchannel *bch, struct mISDN_ctrl_req *cq)
 			printk(KERN_DEBUG "%s: RX_OFF request (nr=%d off=%d)\n",
 			       __func__, bch->nr, hc->chan[bch->slot].rx_off);
 		break;
-	case MISDN_CTRL_FILL_EMPTY: /* fill fifo, if empty */
-		test_and_set_bit(FLG_FILLEMPTY, &bch->Flags);
-		if (debug & DEBUG_HFCMULTI_MSG)
-			printk(KERN_DEBUG "%s: FILL_EMPTY request (nr=%d "
-			       "off=%d)\n", __func__, bch->nr, !!cq->p1);
+	case MISDN_CTRL_FILL_EMPTY:
+		ret = mISDN_ctrl_bchannel(bch, cq);
+		hc->silence = bch->fill[0];
+		memset(hc->silence_data, hc->silence, sizeof(hc->silence_data));
 		break;
 	case MISDN_CTRL_HW_FEATURES: /* fill features structure */
 		if (debug & DEBUG_HFCMULTI_MSG)
@@ -3654,9 +3686,7 @@ channel_bctrl(struct bchannel *bch, struct mISDN_ctrl_req *cq)
 			ret = -EINVAL;
 		break;
 	default:
-		printk(KERN_WARNING "%s: unknown Op %x\n",
-		       __func__, cq->op);
-		ret = -EINVAL;
+		ret = mISDN_ctrl_bchannel(bch, cq);
 		break;
 	}
 	return ret;
@@ -3676,8 +3706,7 @@ hfcm_bctrl(struct mISDNchannel *ch, u_int cmd, void *arg)
 	switch (cmd) {
 	case CLOSE_CHANNEL:
 		test_and_clear_bit(FLG_OPEN, &bch->Flags);
-		if (test_bit(FLG_ACTIVE, &bch->Flags))
-			deactivate_bchannel(bch); /* locked there */
+		deactivate_bchannel(bch); /* locked there */
 		ch->protocol = ISDN_P_NONE;
 		ch->peer = NULL;
 		module_put(THIS_MODULE);
@@ -3839,31 +3868,37 @@ hfcmulti_initmode(struct dchannel *dch)
 	if (debug & DEBUG_HFCMULTI_INIT)
 		printk(KERN_DEBUG "%s: entered\n", __func__);
 
+	i = dch->slot;
+	pt = hc->chan[i].port;
 	if (hc->ctype == HFC_TYPE_E1) {
-		hc->chan[hc->dslot].slot_tx = -1;
-		hc->chan[hc->dslot].slot_rx = -1;
-		hc->chan[hc->dslot].conf = -1;
-		if (hc->dslot) {
-			mode_hfcmulti(hc, hc->dslot, dch->dev.D.protocol,
+		/* E1 */
+		hc->chan[hc->dnum[pt]].slot_tx = -1;
+		hc->chan[hc->dnum[pt]].slot_rx = -1;
+		hc->chan[hc->dnum[pt]].conf = -1;
+		if (hc->dnum[pt]) {
+			mode_hfcmulti(hc, dch->slot, dch->dev.D.protocol,
 				      -1, 0, -1, 0);
 			dch->timer.function = (void *) hfcmulti_dbusy_timer;
 			dch->timer.data = (long) dch;
 			init_timer(&dch->timer);
 		}
 		for (i = 1; i <= 31; i++) {
-			if (i == hc->dslot)
+			if (!((1 << i) & hc->bmask[pt])) /* skip unused chan */
 				continue;
 			hc->chan[i].slot_tx = -1;
 			hc->chan[i].slot_rx = -1;
 			hc->chan[i].conf = -1;
 			mode_hfcmulti(hc, i, ISDN_P_NONE, -1, 0, -1, 0);
 		}
-		/* E1 */
-		if (test_bit(HFC_CFG_REPORT_LOS, &hc->chan[hc->dslot].cfg)) {
+	}
+	if (hc->ctype == HFC_TYPE_E1 && pt == 0) {
+		/* E1, port 0 */
+		dch = hc->chan[hc->dnum[0]].dch;
+		if (test_bit(HFC_CFG_REPORT_LOS, &hc->chan[hc->dnum[0]].cfg)) {
 			HFC_outb(hc, R_LOS0, 255); /* 2 ms */
 			HFC_outb(hc, R_LOS1, 255); /* 512 ms */
 		}
-		if (test_bit(HFC_CFG_OPTICAL, &hc->chan[hc->dslot].cfg)) {
+		if (test_bit(HFC_CFG_OPTICAL, &hc->chan[hc->dnum[0]].cfg)) {
 			HFC_outb(hc, R_RX0, 0);
 			hc->hw.r_tx0 = 0 | V_OUT_EN;
 		} else {
@@ -3876,12 +3911,12 @@ hfcmulti_initmode(struct dchannel *dch)
 		HFC_outb(hc, R_TX_FR0, 0x00);
 		HFC_outb(hc, R_TX_FR1, 0xf8);
 
-		if (test_bit(HFC_CFG_CRC4, &hc->chan[hc->dslot].cfg))
+		if (test_bit(HFC_CFG_CRC4, &hc->chan[hc->dnum[0]].cfg))
 			HFC_outb(hc, R_TX_FR2, V_TX_MF | V_TX_E | V_NEG_E);
 
 		HFC_outb(hc, R_RX_FR0, V_AUTO_RESYNC | V_AUTO_RECO | 0);
 
-		if (test_bit(HFC_CFG_CRC4, &hc->chan[hc->dslot].cfg))
+		if (test_bit(HFC_CFG_CRC4, &hc->chan[hc->dnum[0]].cfg))
 			HFC_outb(hc, R_RX_FR1, V_RX_MF | V_RX_MF_SYNC);
 
 		if (dch->dev.D.protocol == ISDN_P_NT_E1) {
@@ -3944,13 +3979,14 @@ hfcmulti_initmode(struct dchannel *dch)
 			hc->syncronized = 0;
 			plxsd_checksync(hc, 0);
 		}
-	} else {
-		i = dch->slot;
+	}
+	if (hc->ctype != HFC_TYPE_E1) {
+		/* ST */
 		hc->chan[i].slot_tx = -1;
 		hc->chan[i].slot_rx = -1;
 		hc->chan[i].conf = -1;
 		mode_hfcmulti(hc, i, dch->dev.D.protocol, -1, 0, -1, 0);
-		dch->timer.function = (void *)hfcmulti_dbusy_timer;
+		dch->timer.function = (void *) hfcmulti_dbusy_timer;
 		dch->timer.data = (long) dch;
 		init_timer(&dch->timer);
 		hc->chan[i - 2].slot_tx = -1;
@@ -3961,8 +3997,6 @@ hfcmulti_initmode(struct dchannel *dch)
 		hc->chan[i - 1].slot_rx = -1;
 		hc->chan[i - 1].conf = -1;
 		mode_hfcmulti(hc, i - 1, ISDN_P_NONE, -1, 0, -1, 0);
-		/* ST */
-		pt = hc->chan[i].port;
 		/* select interface */
 		HFC_outb(hc, R_ST_SEL, pt);
 		/* undocumented: delay after R_ST_SEL */
@@ -4054,14 +4088,9 @@ open_dchannel(struct hfc_multi *hc, struct dchannel *dch,
 		hfcmulti_initmode(dch);
 		spin_unlock_irqrestore(&hc->lock, flags);
 	}
-
-	if (((rq->protocol == ISDN_P_NT_S0) && (dch->state == 3)) ||
-	    ((rq->protocol == ISDN_P_TE_S0) && (dch->state == 7)) ||
-	    ((rq->protocol == ISDN_P_NT_E1) && (dch->state == 1)) ||
-	    ((rq->protocol == ISDN_P_TE_E1) && (dch->state == 1))) {
+	if (test_bit(FLG_ACTIVE, &dch->Flags))
 		_queue_data(&dch->dev.D, PH_ACTIVATE_IND, MISDN_ID_ANY,
 			    0, NULL, GFP_KERNEL);
-	}
 	rq->ch = &dch->dev.D;
 	if (!try_module_get(THIS_MODULE))
 		printk(KERN_WARNING "%s:cannot get module\n", __func__);
@@ -4091,7 +4120,6 @@ open_bchannel(struct hfc_multi *hc, struct dchannel *dch,
 	}
 	if (test_and_set_bit(FLG_OPEN, &bch->Flags))
 		return -EBUSY; /* b-channel can be only open once */
-	test_and_clear_bit(FLG_FILLEMPTY, &bch->Flags);
 	bch->ch.protocol = rq->protocol;
 	hc->chan[ch].rx_off = 0;
 	rq->ch = &bch->ch;
@@ -4112,7 +4140,7 @@ channel_dctrl(struct dchannel *dch, struct mISDN_ctrl_req *cq)
 
 	switch (cq->op) {
 	case MISDN_CTRL_GETOP:
-		cq->op = MISDN_CTRL_HFC_OP;
+		cq->op = MISDN_CTRL_HFC_OP | MISDN_CTRL_L1_TIMER3;
 		break;
 	case MISDN_CTRL_HFC_WD_INIT: /* init the watchdog */
 		wd_cnt = cq->p1 & 0xf;
@@ -4142,6 +4170,9 @@ channel_dctrl(struct dchannel *dch, struct mISDN_ctrl_req *cq)
 			       __func__);
 		HFC_outb(hc, R_BERT_WD_MD, hc->hw.r_bert_wd_md | V_WD_RES);
 		break;
+	case MISDN_CTRL_L1_TIMER3:
+		ret = l1_event(dch->l1, HW_TIMER3_VALUE | (cq->p1 & 0xff));
+		break;
 	default:
 		printk(KERN_WARNING "%s: unknown Op %x\n",
 		       __func__, cq->op);
@@ -4545,6 +4576,8 @@ release_port(struct hfc_multi *hc, struct dchannel *dch)
 		}
 		/* free channels */
 		for (i = 0; i <= 31; i++) {
+			if (!((1 << i) & hc->bmask[pt])) /* skip unused chan */
+				continue;
 			if (hc->chan[i].bch) {
 				if (debug & DEBUG_HFCMULTI_INIT)
 					printk(KERN_DEBUG
@@ -4600,7 +4633,8 @@ release_port(struct hfc_multi *hc, struct dchannel *dch)
 	spin_unlock_irqrestore(&hc->lock, flags);
 
 	if (debug & DEBUG_HFCMULTI_INIT)
-		printk(KERN_DEBUG "%s: free port %d channel D\n", __func__, pt);
+		printk(KERN_DEBUG "%s: free port %d channel D(%d)\n", __func__,
+			pt+1, ci);
 	mISDN_freedchannel(dch);
 	kfree(dch);
 
@@ -4622,15 +4656,19 @@ release_card(struct hfc_multi *hc)
 	if (hc->iclock)
 		mISDN_unregister_clock(hc->iclock);
 
-	/* disable irq */
+	/* disable and free irq */
 	spin_lock_irqsave(&hc->lock, flags);
 	disable_hwirq(hc);
 	spin_unlock_irqrestore(&hc->lock, flags);
 	udelay(1000);
+	if (hc->irq) {
+		if (debug & DEBUG_HFCMULTI_INIT)
+			printk(KERN_DEBUG "%s: free irq %d (hc=%p)\n",
+			    __func__, hc->irq, hc);
+		free_irq(hc->irq, hc);
+		hc->irq = 0;
 
-	/* dimm leds */
-	if (hc->leds)
-		hfcmulti_leds(hc);
+	}
 
 	/* disable D-channels & B-channels */
 	if (debug & DEBUG_HFCMULTI_INIT)
@@ -4641,15 +4679,11 @@ release_card(struct hfc_multi *hc)
 			release_port(hc, hc->chan[ch].dch);
 	}
 
-	/* release hardware & irq */
-	if (hc->irq) {
-		if (debug & DEBUG_HFCMULTI_INIT)
-			printk(KERN_DEBUG "%s: free irq %d\n",
-			       __func__, hc->irq);
-		free_irq(hc->irq, hc);
-		hc->irq = 0;
+	/* dimm leds */
+	if (hc->leds)
+		hfcmulti_leds(hc);
 
-	}
+	/* release hardware */
 	release_io_hfcmulti(hc);
 
 	if (debug & DEBUG_HFCMULTI_INIT)
@@ -4667,61 +4701,9 @@ release_card(struct hfc_multi *hc)
 		       __func__);
 }
 
-static int
-init_e1_port(struct hfc_multi *hc, struct hm_map *m)
+static void
+init_e1_port_hw(struct hfc_multi *hc, struct hm_map *m)
 {
-	struct dchannel	*dch;
-	struct bchannel	*bch;
-	int		ch, ret = 0;
-	char		name[MISDN_MAX_IDLEN];
-
-	dch = kzalloc(sizeof(struct dchannel), GFP_KERNEL);
-	if (!dch)
-		return -ENOMEM;
-	dch->debug = debug;
-	mISDN_initdchannel(dch, MAX_DFRAME_LEN_L1, ph_state_change);
-	dch->hw = hc;
-	dch->dev.Dprotocols = (1 << ISDN_P_TE_E1) | (1 << ISDN_P_NT_E1);
-	dch->dev.Bprotocols = (1 << (ISDN_P_B_RAW & ISDN_P_B_MASK)) |
-		(1 << (ISDN_P_B_HDLC & ISDN_P_B_MASK));
-	dch->dev.D.send = handle_dmsg;
-	dch->dev.D.ctrl = hfcm_dctrl;
-	dch->dev.nrbchan = (hc->dslot) ? 30 : 31;
-	dch->slot = hc->dslot;
-	hc->chan[hc->dslot].dch = dch;
-	hc->chan[hc->dslot].port = 0;
-	hc->chan[hc->dslot].nt_timer = -1;
-	for (ch = 1; ch <= 31; ch++) {
-		if (ch == hc->dslot) /* skip dchannel */
-			continue;
-		bch = kzalloc(sizeof(struct bchannel), GFP_KERNEL);
-		if (!bch) {
-			printk(KERN_ERR "%s: no memory for bchannel\n",
-			       __func__);
-			ret = -ENOMEM;
-			goto free_chan;
-		}
-		hc->chan[ch].coeff = kzalloc(512, GFP_KERNEL);
-		if (!hc->chan[ch].coeff) {
-			printk(KERN_ERR "%s: no memory for coeffs\n",
-			       __func__);
-			ret = -ENOMEM;
-			kfree(bch);
-			goto free_chan;
-		}
-		bch->nr = ch;
-		bch->slot = ch;
-		bch->debug = debug;
-		mISDN_initbchannel(bch, MAX_DATA_MEM);
-		bch->hw = hc;
-		bch->ch.send = handle_bmsg;
-		bch->ch.ctrl = hfcm_bctrl;
-		bch->ch.nr = ch;
-		list_add(&bch->ch.list, &dch->dev.bchannels);
-		hc->chan[ch].bch = bch;
-		hc->chan[ch].port = 0;
-		set_channelmap(bch->nr, dch->dev.channelmap);
-	}
 	/* set optical line type */
 	if (port[Port_cnt] & 0x001) {
 		if (!m->opticalsupport)  {
@@ -4737,7 +4719,7 @@ init_e1_port(struct hfc_multi *hc, struct hm_map *m)
 				       __func__,
 				       HFC_cnt + 1, 1);
 			test_and_set_bit(HFC_CFG_OPTICAL,
-					 &hc->chan[hc->dslot].cfg);
+			    &hc->chan[hc->dnum[0]].cfg);
 		}
 	}
 	/* set LOS report */
@@ -4747,7 +4729,7 @@ init_e1_port(struct hfc_multi *hc, struct hm_map *m)
 			       "LOS report: card(%d) port(%d)\n",
 			       __func__, HFC_cnt + 1, 1);
 		test_and_set_bit(HFC_CFG_REPORT_LOS,
-				 &hc->chan[hc->dslot].cfg);
+		    &hc->chan[hc->dnum[0]].cfg);
 	}
 	/* set AIS report */
 	if (port[Port_cnt] & 0x008) {
@@ -4756,7 +4738,7 @@ init_e1_port(struct hfc_multi *hc, struct hm_map *m)
 			       "AIS report: card(%d) port(%d)\n",
 			       __func__, HFC_cnt + 1, 1);
 		test_and_set_bit(HFC_CFG_REPORT_AIS,
-				 &hc->chan[hc->dslot].cfg);
+		    &hc->chan[hc->dnum[0]].cfg);
 	}
 	/* set SLIP report */
 	if (port[Port_cnt] & 0x010) {
@@ -4766,7 +4748,7 @@ init_e1_port(struct hfc_multi *hc, struct hm_map *m)
 			       "card(%d) port(%d)\n",
 			       __func__, HFC_cnt + 1, 1);
 		test_and_set_bit(HFC_CFG_REPORT_SLIP,
-				 &hc->chan[hc->dslot].cfg);
+		    &hc->chan[hc->dnum[0]].cfg);
 	}
 	/* set RDI report */
 	if (port[Port_cnt] & 0x020) {
@@ -4776,7 +4758,7 @@ init_e1_port(struct hfc_multi *hc, struct hm_map *m)
 			       "card(%d) port(%d)\n",
 			       __func__, HFC_cnt + 1, 1);
 		test_and_set_bit(HFC_CFG_REPORT_RDI,
-				 &hc->chan[hc->dslot].cfg);
+		    &hc->chan[hc->dnum[0]].cfg);
 	}
 	/* set CRC-4 Mode */
 	if (!(port[Port_cnt] & 0x100)) {
@@ -4785,7 +4767,7 @@ init_e1_port(struct hfc_multi *hc, struct hm_map *m)
 			       " card(%d) port(%d)\n",
 			       __func__, HFC_cnt + 1, 1);
 		test_and_set_bit(HFC_CFG_CRC4,
-				 &hc->chan[hc->dslot].cfg);
+		    &hc->chan[hc->dnum[0]].cfg);
 	} else {
 		if (debug & DEBUG_HFCMULTI_INIT)
 			printk(KERN_DEBUG "%s: PORT turn off CRC4"
@@ -4817,20 +4799,85 @@ init_e1_port(struct hfc_multi *hc, struct hm_map *m)
 	}
 	/* set elastic jitter buffer */
 	if (port[Port_cnt] & 0x3000) {
-		hc->chan[hc->dslot].jitter = (port[Port_cnt]>>12) & 0x3;
+		hc->chan[hc->dnum[0]].jitter = (port[Port_cnt]>>12) & 0x3;
 		if (debug & DEBUG_HFCMULTI_INIT)
 			printk(KERN_DEBUG
 			       "%s: PORT set elastic "
 			       "buffer to %d: card(%d) port(%d)\n",
-			       __func__, hc->chan[hc->dslot].jitter,
+			    __func__, hc->chan[hc->dnum[0]].jitter,
 			       HFC_cnt + 1, 1);
 	} else
-		hc->chan[hc->dslot].jitter = 2; /* default */
-	snprintf(name, MISDN_MAX_IDLEN - 1, "hfc-e1.%d", HFC_cnt + 1);
+		hc->chan[hc->dnum[0]].jitter = 2; /* default */
+}
+
+static int
+init_e1_port(struct hfc_multi *hc, struct hm_map *m, int pt)
+{
+	struct dchannel	*dch;
+	struct bchannel	*bch;
+	int		ch, ret = 0;
+	char		name[MISDN_MAX_IDLEN];
+	int		bcount = 0;
+
+	dch = kzalloc(sizeof(struct dchannel), GFP_KERNEL);
+	if (!dch)
+		return -ENOMEM;
+	dch->debug = debug;
+	mISDN_initdchannel(dch, MAX_DFRAME_LEN_L1, ph_state_change);
+	dch->hw = hc;
+	dch->dev.Dprotocols = (1 << ISDN_P_TE_E1) | (1 << ISDN_P_NT_E1);
+	dch->dev.Bprotocols = (1 << (ISDN_P_B_RAW & ISDN_P_B_MASK)) |
+	    (1 << (ISDN_P_B_HDLC & ISDN_P_B_MASK));
+	dch->dev.D.send = handle_dmsg;
+	dch->dev.D.ctrl = hfcm_dctrl;
+	dch->slot = hc->dnum[pt];
+	hc->chan[hc->dnum[pt]].dch = dch;
+	hc->chan[hc->dnum[pt]].port = pt;
+	hc->chan[hc->dnum[pt]].nt_timer = -1;
+	for (ch = 1; ch <= 31; ch++) {
+		if (!((1 << ch) & hc->bmask[pt])) /* skip unused channel */
+			continue;
+		bch = kzalloc(sizeof(struct bchannel), GFP_KERNEL);
+		if (!bch) {
+			printk(KERN_ERR "%s: no memory for bchannel\n",
+			    __func__);
+			ret = -ENOMEM;
+			goto free_chan;
+		}
+		hc->chan[ch].coeff = kzalloc(512, GFP_KERNEL);
+		if (!hc->chan[ch].coeff) {
+			printk(KERN_ERR "%s: no memory for coeffs\n",
+			    __func__);
+			ret = -ENOMEM;
+			kfree(bch);
+			goto free_chan;
+		}
+		bch->nr = ch;
+		bch->slot = ch;
+		bch->debug = debug;
+		mISDN_initbchannel(bch, MAX_DATA_MEM, poll >> 1);
+		bch->hw = hc;
+		bch->ch.send = handle_bmsg;
+		bch->ch.ctrl = hfcm_bctrl;
+		bch->ch.nr = ch;
+		list_add(&bch->ch.list, &dch->dev.bchannels);
+		hc->chan[ch].bch = bch;
+		hc->chan[ch].port = pt;
+		set_channelmap(bch->nr, dch->dev.channelmap);
+		bcount++;
+	}
+	dch->dev.nrbchan = bcount;
+	if (pt == 0)
+		init_e1_port_hw(hc, m);
+	if (hc->ports > 1)
+		snprintf(name, MISDN_MAX_IDLEN - 1, "hfc-e1.%d-%d",
+				HFC_cnt + 1, pt+1);
+	else
+		snprintf(name, MISDN_MAX_IDLEN - 1, "hfc-e1.%d", HFC_cnt + 1);
 	ret = mISDN_register_device(&dch->dev, &hc->pci_dev->dev, name);
 	if (ret)
 		goto free_chan;
-	hc->created[0] = 1;
+	hc->created[pt] = 1;
 	return ret;
 free_chan:
 	release_port(hc, dch);
@@ -4881,7 +4928,7 @@ init_multi_port(struct hfc_multi *hc, int pt)
 		bch->nr = ch + 1;
 		bch->slot = i + ch;
 		bch->debug = debug;
-		mISDN_initbchannel(bch, MAX_DATA_MEM);
+		mISDN_initbchannel(bch, MAX_DATA_MEM, poll >> 1);
 		bch->hw = hc;
 		bch->ch.send = handle_bmsg;
 		bch->ch.ctrl = hfcm_bctrl;
@@ -4963,7 +5010,8 @@ hfcmulti_init(struct hm_map *m, struct pci_dev *pdev,
 	struct hfc_multi	*hc;
 	u_long		flags;
 	u_char		dips = 0, pmj = 0; /* dip settings, port mode Jumpers */
-	int		i;
+	int		i, ch;
+	u_int		maskcheck;
 
 	if (HFC_cnt >= MAX_CARDS) {
 		printk(KERN_ERR "too many cards (max=%d).\n",
@@ -4997,18 +5045,36 @@ hfcmulti_init(struct hm_map *m, struct pci_dev *pdev,
 	hc->id = HFC_cnt;
 	hc->pcm = pcm[HFC_cnt];
 	hc->io_mode = iomode[HFC_cnt];
-	if (dslot[HFC_cnt] < 0 && hc->ctype == HFC_TYPE_E1) {
-		hc->dslot = 0;
-		printk(KERN_INFO "HFC-E1 card has disabled D-channel, but "
-		       "31 B-channels\n");
-	}
-	if (dslot[HFC_cnt] > 0 && dslot[HFC_cnt] < 32
-	    && hc->ctype == HFC_TYPE_E1) {
-		hc->dslot = dslot[HFC_cnt];
-		printk(KERN_INFO "HFC-E1 card has alternating D-channel on "
-		       "time slot %d\n", dslot[HFC_cnt]);
-	} else
-		hc->dslot = 16;
+	if (hc->ctype == HFC_TYPE_E1 && dmask[E1_cnt]) {
+		/* fragment card */
+		pt = 0;
+		maskcheck = 0;
+		for (ch = 0; ch <= 31; ch++) {
+			if (!((1 << ch) & dmask[E1_cnt]))
+				continue;
+			hc->dnum[pt] = ch;
+			hc->bmask[pt] = bmask[bmask_cnt++];
+			if ((maskcheck & hc->bmask[pt])
+			 || (dmask[E1_cnt] & hc->bmask[pt])) {
+				printk(KERN_INFO
+				       "HFC-E1 #%d has overlapping B-channels on fragment #%d\n",
+				       E1_cnt + 1, pt);
+				return -EINVAL;
+			}
+			maskcheck |= hc->bmask[pt];
+			printk(KERN_INFO
+			       "HFC-E1 #%d uses D-channel on slot %d and a B-channel map of 0x%08x\n",
+				E1_cnt + 1, ch, hc->bmask[pt]);
+			pt++;
+		}
+		hc->ports = pt;
+	}
+	if (hc->ctype == HFC_TYPE_E1 && !dmask[E1_cnt]) {
+		/* default card layout */
+		hc->dnum[0] = 16;
+		hc->bmask[0] = 0xfffefffe;
+		hc->ports = 1;
+	}
 
 	/* set chip specific features */
 	hc->masterclk = -1;
@@ -5091,23 +5157,33 @@ hfcmulti_init(struct hm_map *m, struct pci_dev *pdev,
 			goto free_card;
 		}
 		if (hc->ctype == HFC_TYPE_E1)
-			ret_err = init_e1_port(hc, m);
+			ret_err = init_e1_port(hc, m, pt);
 		else
 			ret_err = init_multi_port(hc, pt);
 		if (debug & DEBUG_HFCMULTI_INIT)
 			printk(KERN_DEBUG
-			       "%s: Registering D-channel, card(%d) port(%d)"
+			    "%s: Registering D-channel, card(%d) port(%d) "
 			       "result %d\n",
-			       __func__, HFC_cnt + 1, pt, ret_err);
+			    __func__, HFC_cnt + 1, pt + 1, ret_err);
 
 		if (ret_err) {
 			while (pt) { /* release already registered ports */
 				pt--;
-				release_port(hc, hc->chan[(pt << 2) + 2].dch);
+				if (hc->ctype == HFC_TYPE_E1)
+					release_port(hc,
+						hc->chan[hc->dnum[pt]].dch);
+				else
+					release_port(hc,
+						hc->chan[(pt << 2) + 2].dch);
 			}
 			goto free_card;
 		}
-		Port_cnt++;
+		if (hc->ctype != HFC_TYPE_E1)
+			Port_cnt++; /* for each S0 port */
+	}
+	if (hc->ctype == HFC_TYPE_E1) {
+		Port_cnt++; /* for each E1 port */
+		E1_cnt++;
 	}
 
 	/* disp switches */
diff --git a/drivers/isdn/hardware/mISDN/hfcpci.c b/drivers/isdn/hardware/mISDN/hfcpci.c
index e2c83a2d769..81363ffa535 100644
--- a/drivers/isdn/hardware/mISDN/hfcpci.c
+++ b/drivers/isdn/hardware/mISDN/hfcpci.c
@@ -453,7 +453,7 @@ hfcpci_empty_bfifo(struct bchannel *bch, struct bzfifo *bz,
 		}
 		bz->za[new_f2].z2 = cpu_to_le16(new_z2);
 		bz->f2 = new_f2;	/* next buffer */
-		recv_Bchannel(bch, MISDN_ID_ANY);
+		recv_Bchannel(bch, MISDN_ID_ANY, false);
 	}
 }
 
@@ -565,11 +565,6 @@ hfcpci_empty_fifo_trans(struct bchannel *bch, struct bzfifo *rxbz,
 	if (new_z2 >= (B_FIFO_SIZE + B_SUB_VAL))
 		new_z2 -= B_FIFO_SIZE;	/* buffer wrap */
 
-	if (fcnt_rx > MAX_DATA_SIZE) {	/* flush, if oversized */
-		*z2r = cpu_to_le16(new_z2);		/* new position */
-		return;
-	}
-
 	fcnt_tx = le16_to_cpu(*z2t) - le16_to_cpu(*z1t);
 	if (fcnt_tx <= 0)
 		fcnt_tx += B_FIFO_SIZE;
@@ -577,8 +572,16 @@ hfcpci_empty_fifo_trans(struct bchannel *bch, struct bzfifo *rxbz,
 	fcnt_tx = B_FIFO_SIZE - fcnt_tx;
 	/* remaining bytes to send (bytes in tx-fifo) */
 
-	bch->rx_skb = mI_alloc_skb(fcnt_rx, GFP_ATOMIC);
-	if (bch->rx_skb) {
+	if (test_bit(FLG_RX_OFF, &bch->Flags)) {
+		bch->dropcnt += fcnt_rx;
+		*z2r = cpu_to_le16(new_z2);
+		return;
+	}
+	maxlen = bchannel_get_rxbuf(bch, fcnt_rx);
+	if (maxlen < 0) {
+		pr_warning("B%d: No bufferspace for %d bytes\n",
+			   bch->nr, fcnt_rx);
+	} else {
 		ptr = skb_put(bch->rx_skb, fcnt_rx);
 		if (le16_to_cpu(*z2r) + fcnt_rx <= B_FIFO_SIZE + B_SUB_VAL)
 			maxlen = fcnt_rx;	/* complete transfer */
@@ -596,10 +599,8 @@ hfcpci_empty_fifo_trans(struct bchannel *bch, struct bzfifo *rxbz,
 			ptr1 = bdata;	/* start of buffer */
 			memcpy(ptr, ptr1, fcnt_rx);	/* rest */
 		}
-		recv_Bchannel(bch, fcnt_tx); /* bch, id */
-	} else
-		printk(KERN_WARNING "HFCPCI: receive out of memory\n");
-
+		recv_Bchannel(bch, fcnt_tx, false); /* bch, id, !force */
+	}
 	*z2r = cpu_to_le16(new_z2);		/* new position */
 }
 
@@ -760,9 +761,14 @@ hfcpci_fill_fifo(struct bchannel *bch)
 
 	if ((bch->debug & DEBUG_HW_BCHANNEL) && !(bch->debug & DEBUG_HW_BFIFO))
 		printk(KERN_DEBUG "%s\n", __func__);
-	if ((!bch->tx_skb) || bch->tx_skb->len <= 0)
-		return;
-	count = bch->tx_skb->len - bch->tx_idx;
+	if ((!bch->tx_skb) || bch->tx_skb->len == 0) {
+		if (!test_bit(FLG_FILLEMPTY, &bch->Flags) &&
+		    !test_bit(FLG_TRANSPARENT, &bch->Flags))
+			return;
+		count = HFCPCI_FILLEMPTY;
+	} else {
+		count = bch->tx_skb->len - bch->tx_idx;
+	}
 	if ((bch->nr & 2) && (!hc->hw.bswapped)) {
 		bz = &((union fifo_area *)(hc->hw.fifos))->b_chans.txbz_b2;
 		bdata = ((union fifo_area *)(hc->hw.fifos))->b_chans.txdat_b2;
@@ -781,16 +787,10 @@ hfcpci_fill_fifo(struct bchannel *bch)
 		fcnt = le16_to_cpu(*z2t) - le16_to_cpu(*z1t);
 		if (fcnt <= 0)
 			fcnt += B_FIFO_SIZE;
-		/* fcnt contains available bytes in fifo */
-		fcnt = B_FIFO_SIZE - fcnt;
-		/* remaining bytes to send (bytes in fifo) */
-
-		/* "fill fifo if empty" feature */
-		if (test_bit(FLG_FILLEMPTY, &bch->Flags) && !fcnt) {
-			/* printk(KERN_DEBUG "%s: buffer empty, so we have "
-			   "underrun\n", __func__); */
-			/* fill buffer, to prevent future underrun */
-			count = HFCPCI_FILLEMPTY;
+		if (test_bit(FLG_FILLEMPTY, &bch->Flags)) {
+			/* fcnt contains available bytes in fifo */
+			if (count > fcnt)
+				count = fcnt;
 			new_z1 = le16_to_cpu(*z1t) + count;
 			/* new buffer Position */
 			if (new_z1 >= (B_FIFO_SIZE + B_SUB_VAL))
@@ -802,17 +802,20 @@ hfcpci_fill_fifo(struct bchannel *bch)
 				printk(KERN_DEBUG "hfcpci_FFt fillempty "
 				       "fcnt(%d) maxl(%d) nz1(%x) dst(%p)\n",
 				       fcnt, maxlen, new_z1, dst);
-			fcnt += count;
 			if (maxlen > count)
 				maxlen = count;		/* limit size */
-			memset(dst, 0x2a, maxlen);	/* first copy */
+			memset(dst, bch->fill[0], maxlen); /* first copy */
 			count -= maxlen;		/* remaining bytes */
 			if (count) {
 				dst = bdata;		/* start of buffer */
-				memset(dst, 0x2a, count);
+				memset(dst, bch->fill[0], count);
 			}
 			*z1t = cpu_to_le16(new_z1);	/* now send data */
+			return;
 		}
+		/* fcnt contains available bytes in fifo */
+		fcnt = B_FIFO_SIZE - fcnt;
+		/* remaining bytes to send (bytes in fifo) */
 
 	next_t_frame:
 		count = bch->tx_skb->len - bch->tx_idx;
@@ -849,9 +852,6 @@ hfcpci_fill_fifo(struct bchannel *bch)
 		*z1t = cpu_to_le16(new_z1);	/* now send data */
 		if (bch->tx_idx < bch->tx_skb->len)
 			return;
-		/* send confirm, on trans, free on hdlc. */
-		if (test_bit(FLG_TRANSPARENT, &bch->Flags))
-			confirm_Bsend(bch);
 		dev_kfree_skb(bch->tx_skb);
 		if (get_next_bframe(bch))
 			goto next_t_frame;
@@ -1533,24 +1533,7 @@ deactivate_bchannel(struct bchannel *bch)
 static int
 channel_bctrl(struct bchannel *bch, struct mISDN_ctrl_req *cq)
 {
-	int	ret = 0;
-
-	switch (cq->op) {
-	case MISDN_CTRL_GETOP:
-		cq->op = MISDN_CTRL_FILL_EMPTY;
-		break;
-	case MISDN_CTRL_FILL_EMPTY: /* fill fifo, if empty */
-		test_and_set_bit(FLG_FILLEMPTY, &bch->Flags);
-		if (debug & DEBUG_HW_OPEN)
-			printk(KERN_DEBUG "%s: FILL_EMPTY request (nr=%d "
-			       "off=%d)\n", __func__, bch->nr, !!cq->p1);
-		break;
-	default:
-		printk(KERN_WARNING "%s: unknown Op %x\n", __func__, cq->op);
-		ret = -EINVAL;
-		break;
-	}
-	return ret;
+	return mISDN_ctrl_bchannel(bch, cq);
 }
 static int
 hfc_bctrl(struct mISDNchannel *ch, u_int cmd, void *arg)
@@ -1581,8 +1564,7 @@ hfc_bctrl(struct mISDNchannel *ch, u_int cmd, void *arg)
 		break;
 	case CLOSE_CHANNEL:
 		test_and_clear_bit(FLG_OPEN, &bch->Flags);
-		if (test_bit(FLG_ACTIVE, &bch->Flags))
-			deactivate_bchannel(bch);
+		deactivate_bchannel(bch);
 		ch->protocol = ISDN_P_NONE;
 		ch->peer = NULL;
 		module_put(THIS_MODULE);
@@ -1692,22 +1674,17 @@ hfcpci_l2l1B(struct mISDNchannel *ch, struct sk_buff *skb)
 	struct hfc_pci		*hc = bch->hw;
 	int			ret = -EINVAL;
 	struct mISDNhead	*hh = mISDN_HEAD_P(skb);
-	unsigned int		id;
-	u_long			flags;
+	unsigned long		flags;
 
 	switch (hh->prim) {
 	case PH_DATA_REQ:
 		spin_lock_irqsave(&hc->lock, flags);
 		ret = bchannel_senddata(bch, skb);
 		if (ret > 0) { /* direct TX */
-			id = hh->id; /* skb can be freed */
 			hfcpci_fill_fifo(bch);
 			ret = 0;
-			spin_unlock_irqrestore(&hc->lock, flags);
-			if (!test_bit(FLG_TRANSPARENT, &bch->Flags))
-				queue_ch_frame(ch, PH_DATA_CNF, id, NULL);
-		} else
-			spin_unlock_irqrestore(&hc->lock, flags);
+		}
+		spin_unlock_irqrestore(&hc->lock, flags);
 		return ret;
 	case PH_ACTIVATE_REQ:
 		spin_lock_irqsave(&hc->lock, flags);
@@ -1819,7 +1796,7 @@ channel_ctrl(struct hfc_pci *hc, struct mISDN_ctrl_req *cq)
 	switch (cq->op) {
 	case MISDN_CTRL_GETOP:
 		cq->op = MISDN_CTRL_LOOP | MISDN_CTRL_CONNECT |
-			MISDN_CTRL_DISCONNECT;
+			 MISDN_CTRL_DISCONNECT | MISDN_CTRL_L1_TIMER3;
 		break;
 	case MISDN_CTRL_LOOP:
 		/* channel 0 disabled loop */
@@ -1896,6 +1873,9 @@ channel_ctrl(struct hfc_pci *hc, struct mISDN_ctrl_req *cq)
 		Write_hfc(hc, HFCPCI_CONNECT, hc->hw.conn);
 		hc->hw.trm &= 0x7f;	/* disable IOM-loop */
 		break;
+	case MISDN_CTRL_L1_TIMER3:
+		ret = l1_event(hc->dch.l1, HW_TIMER3_VALUE | (cq->p1 & 0xff));
+		break;
 	default:
 		printk(KERN_WARNING "%s: unknown Op %x\n",
 		       __func__, cq->op);
@@ -1969,7 +1949,6 @@ open_bchannel(struct hfc_pci *hc, struct channel_req *rq)
 	bch = &hc->bch[rq->adr.channel - 1];
 	if (test_and_set_bit(FLG_OPEN, &bch->Flags))
 		return -EBUSY; /* b-channel can be only open once */
-	test_and_clear_bit(FLG_FILLEMPTY, &bch->Flags);
 	bch->ch.protocol = rq->protocol;
 	rq->ch = &bch->ch; /* TODO: E-channel */
 	if (!try_module_get(THIS_MODULE))
@@ -2121,7 +2100,7 @@ setup_card(struct hfc_pci *card)
 		card->bch[i].nr = i + 1;
 		set_channelmap(i + 1, card->dch.dev.channelmap);
 		card->bch[i].debug = debug;
-		mISDN_initbchannel(&card->bch[i], MAX_DATA_MEM);
+		mISDN_initbchannel(&card->bch[i], MAX_DATA_MEM, poll >> 1);
 		card->bch[i].hw = card;
 		card->bch[i].ch.send = hfcpci_l2l1B;
 		card->bch[i].ch.ctrl = hfc_bctrl;
diff --git a/drivers/isdn/hardware/mISDN/hfcsusb.c b/drivers/isdn/hardware/mISDN/hfcsusb.c
index 8cde2a0538a..c65c3440cd7 100644
--- a/drivers/isdn/hardware/mISDN/hfcsusb.c
+++ b/drivers/isdn/hardware/mISDN/hfcsusb.c
@@ -226,19 +226,12 @@ hfcusb_l2l1B(struct mISDNchannel *ch, struct sk_buff *skb)
 		if (debug & DBG_HFC_CALL_TRACE)
 			printk(KERN_DEBUG "%s: %s PH_DATA_REQ ret(%i)\n",
 			       hw->name, __func__, ret);
-		if (ret > 0) {
-			/*
-			 * other l1 drivers don't send early confirms on
-			 * transp data, but hfcsusb does because tx_next
-			 * skb is needed in tx_iso_complete()
-			 */
-			queue_ch_frame(ch, PH_DATA_CNF, hh->id, NULL);
+		if (ret > 0)
 			ret = 0;
-		}
 		return ret;
 	case PH_ACTIVATE_REQ:
 		if (!test_and_set_bit(FLG_ACTIVE, &bch->Flags)) {
-			hfcsusb_start_endpoint(hw, bch->nr);
+			hfcsusb_start_endpoint(hw, bch->nr - 1);
 			ret = hfcsusb_setup_bch(bch, ch->protocol);
 		} else
 			ret = 0;
@@ -498,16 +491,9 @@ open_bchannel(struct hfcsusb *hw, struct channel_req *rq)
 	bch = &hw->bch[rq->adr.channel - 1];
 	if (test_and_set_bit(FLG_OPEN, &bch->Flags))
 		return -EBUSY; /* b-channel can be only open once */
-	test_and_clear_bit(FLG_FILLEMPTY, &bch->Flags);
 	bch->ch.protocol = rq->protocol;
 	rq->ch = &bch->ch;
 
-	/* start USB endpoint for bchannel */
-	if (rq->adr.channel  == 1)
-		hfcsusb_start_endpoint(hw, HFC_CHAN_B1);
-	else
-		hfcsusb_start_endpoint(hw, HFC_CHAN_B2);
-
 	if (!try_module_get(THIS_MODULE))
 		printk(KERN_WARNING "%s: %s:cannot get module\n",
 		       hw->name, __func__);
@@ -819,24 +805,7 @@ hfcsusb_ph_command(struct hfcsusb *hw, u_char command)
 static int
 channel_bctrl(struct bchannel *bch, struct mISDN_ctrl_req *cq)
 {
-	int	ret = 0;
-
-	switch (cq->op) {
-	case MISDN_CTRL_GETOP:
-		cq->op = MISDN_CTRL_FILL_EMPTY;
-		break;
-	case MISDN_CTRL_FILL_EMPTY: /* fill fifo, if empty */
-		test_and_set_bit(FLG_FILLEMPTY, &bch->Flags);
-		if (debug & DEBUG_HW_OPEN)
-			printk(KERN_DEBUG "%s: FILL_EMPTY request (nr=%d "
-			       "off=%d)\n", __func__, bch->nr, !!cq->p1);
-		break;
-	default:
-		printk(KERN_WARNING "%s: unknown Op %x\n", __func__, cq->op);
-		ret = -EINVAL;
-		break;
-	}
-	return ret;
+	return mISDN_ctrl_bchannel(bch, cq);
 }
 
 /* collect data from incoming interrupt or isochron USB data */
@@ -873,7 +842,21 @@ hfcsusb_rx_frame(struct usb_fifo *fifo, __u8 *data, unsigned int len,
 		hdlc = 1;
 	}
 	if (fifo->bch) {
+		if (test_bit(FLG_RX_OFF, &fifo->bch->Flags)) {
+			fifo->bch->dropcnt += len;
+			spin_unlock(&hw->lock);
+			return;
+		}
+		maxlen = bchannel_get_rxbuf(fifo->bch, len);
 		rx_skb = fifo->bch->rx_skb;
+		if (maxlen < 0) {
+			if (rx_skb)
+				skb_trim(rx_skb, 0);
+			pr_warning("%s.B%d: No bufferspace for %d bytes\n",
+				   hw->name, fifo->bch->nr, len);
+			spin_unlock(&hw->lock);
+			return;
+		}
 		maxlen = fifo->bch->maxlen;
 		hdlc = test_bit(FLG_HDLC, &fifo->bch->Flags);
 	}
@@ -883,25 +866,22 @@ hfcsusb_rx_frame(struct usb_fifo *fifo, __u8 *data, unsigned int len,
 		hdlc = 1;
 	}
 
-	if (!rx_skb) {
-		rx_skb = mI_alloc_skb(maxlen, GFP_ATOMIC);
-		if (rx_skb) {
-			if (fifo->dch)
-				fifo->dch->rx_skb = rx_skb;
-			if (fifo->bch)
-				fifo->bch->rx_skb = rx_skb;
-			if (fifo->ech)
-				fifo->ech->rx_skb = rx_skb;
-			skb_trim(rx_skb, 0);
-		} else {
-			printk(KERN_DEBUG "%s: %s: No mem for rx_skb\n",
-			       hw->name, __func__);
-			spin_unlock(&hw->lock);
-			return;
-		}
-	}
-
 	if (fifo->dch || fifo->ech) {
+		if (!rx_skb) {
+			rx_skb = mI_alloc_skb(maxlen, GFP_ATOMIC);
+			if (rx_skb) {
+				if (fifo->dch)
+					fifo->dch->rx_skb = rx_skb;
+				if (fifo->ech)
+					fifo->ech->rx_skb = rx_skb;
+				skb_trim(rx_skb, 0);
+			} else {
+				printk(KERN_DEBUG "%s: %s: No mem for rx_skb\n",
+				       hw->name, __func__);
+				spin_unlock(&hw->lock);
+				return;
+			}
+		}
 		/* D/E-Channel SKB range check */
 		if ((rx_skb->len + len) >= MAX_DFRAME_LEN_L1) {
 			printk(KERN_DEBUG "%s: %s: sbk mem exceeded "
@@ -911,16 +891,6 @@ hfcsusb_rx_frame(struct usb_fifo *fifo, __u8 *data, unsigned int len,
 			spin_unlock(&hw->lock);
 			return;
 		}
-	} else if (fifo->bch) {
-		/* B-Channel SKB range check */
-		if ((rx_skb->len + len) >= (MAX_BCH_SIZE + 3)) {
-			printk(KERN_DEBUG "%s: %s: sbk mem exceeded "
-			       "for fifo(%d) HFCUSB_B_RX\n",
-			       hw->name, __func__, fifon);
-			skb_trim(rx_skb, 0);
-			spin_unlock(&hw->lock);
-			return;
-		}
 	}
 
 	memcpy(skb_put(rx_skb, len), data, len);
@@ -948,7 +918,8 @@ hfcsusb_rx_frame(struct usb_fifo *fifo, __u8 *data, unsigned int len,
 				if (fifo->dch)
 					recv_Dchannel(fifo->dch);
 				if (fifo->bch)
-					recv_Bchannel(fifo->bch, MISDN_ID_ANY);
+					recv_Bchannel(fifo->bch, MISDN_ID_ANY,
+						      0);
 				if (fifo->ech)
 					recv_Echannel(fifo->ech,
 						      &hw->dch);
@@ -969,8 +940,7 @@ hfcsusb_rx_frame(struct usb_fifo *fifo, __u8 *data, unsigned int len,
 		}
 	} else {
 		/* deliver transparent data to layer2 */
-		if (rx_skb->len >= poll)
-			recv_Bchannel(fifo->bch, MISDN_ID_ANY);
+		recv_Bchannel(fifo->bch, MISDN_ID_ANY, false);
 	}
 	spin_unlock(&hw->lock);
 }
@@ -1200,8 +1170,8 @@ tx_iso_complete(struct urb *urb)
 	int k, tx_offset, num_isoc_packets, sink, remain, current_len,
 		errcode, hdlc, i;
 	int *tx_idx;
-	int frame_complete, fifon, status;
-	__u8 threshbit;
+	int frame_complete, fifon, status, fillempty = 0;
+	__u8 threshbit, *p;
 
 	spin_lock(&hw->lock);
 	if (fifo->stop_gracefull) {
@@ -1219,6 +1189,9 @@ tx_iso_complete(struct urb *urb)
 		tx_skb = fifo->bch->tx_skb;
 		tx_idx = &fifo->bch->tx_idx;
 		hdlc = test_bit(FLG_HDLC, &fifo->bch->Flags);
+		if (!tx_skb && !hdlc &&
+		    test_bit(FLG_FILLEMPTY, &fifo->bch->Flags))
+			fillempty = 1;
 	} else {
 		printk(KERN_DEBUG "%s: %s: neither BCH nor DCH\n",
 		       hw->name, __func__);
@@ -1277,6 +1250,8 @@ tx_iso_complete(struct urb *urb)
 			/* Generate next ISO Packets */
 			if (tx_skb)
 				remain = tx_skb->len - *tx_idx;
+			else if (fillempty)
+				remain = 15; /* > not complete */
 			else
 				remain = 0;
 
@@ -1307,15 +1282,20 @@ tx_iso_complete(struct urb *urb)
 				}
 
 				/* copy tx data to iso-urb buffer */
-				memcpy(context_iso_urb->buffer + tx_offset + 1,
-				       (tx_skb->data + *tx_idx), current_len);
-				*tx_idx += current_len;
-
+				p = context_iso_urb->buffer + tx_offset + 1;
+				if (fillempty) {
+					memset(p, fifo->bch->fill[0],
+					       current_len);
+				} else {
+					memcpy(p, (tx_skb->data + *tx_idx),
+					       current_len);
+					*tx_idx += current_len;
+				}
 				urb->iso_frame_desc[k].offset = tx_offset;
 				urb->iso_frame_desc[k].length = current_len + 1;
 
 				/* USB data log for every D ISO out */
-				if ((fifon == HFCUSB_D_RX) &&
+				if ((fifon == HFCUSB_D_RX) && !fillempty &&
 				    (debug & DBG_HFC_USB_VERBOSE)) {
 					printk(KERN_DEBUG
 					       "%s: %s (%d/%d) offs(%d) len(%d) ",
@@ -1365,12 +1345,8 @@ tx_iso_complete(struct urb *urb)
 				if (fifo->dch && get_next_dframe(fifo->dch))
 					tx_skb = fifo->dch->tx_skb;
 				else if (fifo->bch &&
-					 get_next_bframe(fifo->bch)) {
-					if (test_bit(FLG_TRANSPARENT,
-						     &fifo->bch->Flags))
-						confirm_Bsend(fifo->bch);
+					 get_next_bframe(fifo->bch))
 					tx_skb = fifo->bch->tx_skb;
-				}
 			}
 		}
 		errcode = usb_submit_urb(urb, GFP_ATOMIC);
@@ -1812,7 +1788,7 @@ deactivate_bchannel(struct bchannel *bch)
 	mISDN_clear_bchannel(bch);
 	spin_unlock_irqrestore(&hw->lock, flags);
 	hfcsusb_setup_bch(bch, ISDN_P_NONE);
-	hfcsusb_stop_endpoint(hw, bch->nr);
+	hfcsusb_stop_endpoint(hw, bch->nr - 1);
 }
 
 /*
@@ -1836,8 +1812,7 @@ hfc_bctrl(struct mISDNchannel *ch, u_int cmd, void *arg)
 
 	case CLOSE_CHANNEL:
 		test_and_clear_bit(FLG_OPEN, &bch->Flags);
-		if (test_bit(FLG_ACTIVE, &bch->Flags))
-			deactivate_bchannel(bch);
+		deactivate_bchannel(bch);
 		ch->protocol = ISDN_P_NONE;
 		ch->peer = NULL;
 		module_put(THIS_MODULE);
@@ -1883,7 +1858,7 @@ setup_instance(struct hfcsusb *hw, struct device *parent)
 		hw->bch[i].nr = i + 1;
 		set_channelmap(i + 1, hw->dch.dev.channelmap);
 		hw->bch[i].debug = debug;
-		mISDN_initbchannel(&hw->bch[i], MAX_DATA_MEM);
+		mISDN_initbchannel(&hw->bch[i], MAX_DATA_MEM, poll >> 1);
 		hw->bch[i].hw = hw;
 		hw->bch[i].ch.send = hfcusb_l2l1B;
 		hw->bch[i].ch.ctrl = hfc_bctrl;
@@ -2151,6 +2126,7 @@ static struct usb_driver hfcsusb_drv = {
 	.id_table = hfcsusb_idtab,
 	.probe = hfcsusb_probe,
 	.disconnect = hfcsusb_disconnect,
+	.disable_hub_initiated_lpm = 1,
 };
 
 module_usb_driver(hfcsusb_drv);
diff --git a/drivers/isdn/hardware/mISDN/hfcsusb.h b/drivers/isdn/hardware/mISDN/hfcsusb.h
index cb1231b08f7..4157311d569 100644
--- a/drivers/isdn/hardware/mISDN/hfcsusb.h
+++ b/drivers/isdn/hardware/mISDN/hfcsusb.h
@@ -410,6 +410,12 @@ static struct usb_device_id hfcsusb_idtab[] = {
 			{LED_SCHEME1, {0x88, -64, -32, -16},
 					"ZyXEL OMNI.NET USB II"}),
 	},
+	{
+		USB_DEVICE(0x1ae7, 0x0525),
+		.driver_info = (unsigned long) &((struct hfcsusb_vdata)
+			{LED_SCHEME1, {0x88, -64, -32, -16},
+					"X-Tensions USB ISDN TA XC-525"}),
+	},
 	{ }
 };
 
diff --git a/drivers/isdn/hardware/mISDN/mISDNipac.c b/drivers/isdn/hardware/mISDN/mISDNipac.c
index 884369f09ca..752e0825591 100644
--- a/drivers/isdn/hardware/mISDN/mISDNipac.c
+++ b/drivers/isdn/hardware/mISDN/mISDNipac.c
@@ -603,10 +603,11 @@ isac_l1hw(struct mISDNchannel *ch, struct sk_buff *skb)
 }
 
 static int
-isac_ctrl(struct isac_hw *isac, u32 cmd, u_long para)
+isac_ctrl(struct isac_hw *isac, u32 cmd, unsigned long para)
 {
 	u8 tl = 0;
-	u_long flags;
+	unsigned long flags;
+	int ret = 0;
 
 	switch (cmd) {
 	case HW_TESTLOOP:
@@ -626,12 +627,15 @@ isac_ctrl(struct isac_hw *isac, u32 cmd, u_long para)
 		}
 		spin_unlock_irqrestore(isac->hwlock, flags);
 		break;
+	case HW_TIMER3_VALUE:
+		ret = l1_event(isac->dch.l1, HW_TIMER3_VALUE | (para & 0xff));
+		break;
 	default:
 		pr_debug("%s: %s unknown command %x %lx\n", isac->name,
 			 __func__, cmd, para);
-		return -1;
+		ret = -1;
 	}
-	return 0;
+	return ret;
 }
 
 static int
@@ -929,22 +933,21 @@ static void
 hscx_empty_fifo(struct hscx_hw *hscx, u8 count)
 {
 	u8 *p;
+	int maxlen;
 
 	pr_debug("%s: B%1d %d\n", hscx->ip->name, hscx->bch.nr, count);
-	if (!hscx->bch.rx_skb) {
-		hscx->bch.rx_skb = mI_alloc_skb(hscx->bch.maxlen, GFP_ATOMIC);
-		if (!hscx->bch.rx_skb) {
-			pr_info("%s: B receive out of memory\n",
-				hscx->ip->name);
-			hscx_cmdr(hscx, 0x80); /* RMC */
-			return;
-		}
+	if (test_bit(FLG_RX_OFF, &hscx->bch.Flags)) {
+		hscx->bch.dropcnt += count;
+		hscx_cmdr(hscx, 0x80); /* RMC */
+		return;
 	}
-	if ((hscx->bch.rx_skb->len + count) > hscx->bch.maxlen) {
-		pr_debug("%s: overrun %d\n", hscx->ip->name,
-			 hscx->bch.rx_skb->len + count);
-		skb_trim(hscx->bch.rx_skb, 0);
+	maxlen = bchannel_get_rxbuf(&hscx->bch, count);
+	if (maxlen < 0) {
 		hscx_cmdr(hscx, 0x80); /* RMC */
+		if (hscx->bch.rx_skb)
+			skb_trim(hscx->bch.rx_skb, 0);
+		pr_warning("%s.B%d: No bufferspace for %d bytes\n",
+			   hscx->ip->name, hscx->bch.nr, count);
 		return;
 	}
 	p = skb_put(hscx->bch.rx_skb, count);
@@ -971,22 +974,28 @@ hscx_fill_fifo(struct hscx_hw *hscx)
 	int count, more;
 	u8 *p;
 
-	if (!hscx->bch.tx_skb)
-		return;
-	count = hscx->bch.tx_skb->len - hscx->bch.tx_idx;
-	if (count <= 0)
-		return;
-	p = hscx->bch.tx_skb->data + hscx->bch.tx_idx;
-
-	more = test_bit(FLG_TRANSPARENT, &hscx->bch.Flags) ? 1 : 0;
-	if (count > hscx->fifo_size) {
+	if (!hscx->bch.tx_skb) {
+		if (!test_bit(FLG_TX_EMPTY, &hscx->bch.Flags))
+			return;
 		count = hscx->fifo_size;
 		more = 1;
-	}
-	pr_debug("%s: B%1d %d/%d/%d\n", hscx->ip->name, hscx->bch.nr, count,
-		 hscx->bch.tx_idx, hscx->bch.tx_skb->len);
-	hscx->bch.tx_idx += count;
+		p = hscx->log;
+		memset(p, hscx->bch.fill[0], count);
+	} else {
+		count = hscx->bch.tx_skb->len - hscx->bch.tx_idx;
+		if (count <= 0)
+			return;
+		p = hscx->bch.tx_skb->data + hscx->bch.tx_idx;
 
+		more = test_bit(FLG_TRANSPARENT, &hscx->bch.Flags) ? 1 : 0;
+		if (count > hscx->fifo_size) {
+			count = hscx->fifo_size;
+			more = 1;
+		}
+		pr_debug("%s: B%1d %d/%d/%d\n", hscx->ip->name, hscx->bch.nr,
+			 count, hscx->bch.tx_idx, hscx->bch.tx_skb->len);
+		hscx->bch.tx_idx += count;
+	}
 	if (hscx->ip->type & IPAC_TYPE_IPACX)
 		hscx->ip->write_fifo(hscx->ip->hw,
 				     hscx->off + IPACX_XFIFOB, p, count);
@@ -997,7 +1006,7 @@ hscx_fill_fifo(struct hscx_hw *hscx)
 	}
 	hscx_cmdr(hscx, more ? 0x08 : 0x0a);
 
-	if (hscx->bch.debug & DEBUG_HW_BFIFO) {
+	if (hscx->bch.tx_skb && (hscx->bch.debug & DEBUG_HW_BFIFO)) {
 		snprintf(hscx->log, 64, "B%1d-send %s %d ",
 			 hscx->bch.nr, hscx->ip->name, count);
 		print_hex_dump_bytes(hscx->log, DUMP_PREFIX_OFFSET, p, count);
@@ -1007,17 +1016,17 @@ hscx_fill_fifo(struct hscx_hw *hscx)
 static void
 hscx_xpr(struct hscx_hw *hx)
 {
-	if (hx->bch.tx_skb && hx->bch.tx_idx < hx->bch.tx_skb->len)
+	if (hx->bch.tx_skb && hx->bch.tx_idx < hx->bch.tx_skb->len) {
 		hscx_fill_fifo(hx);
-	else {
-		if (hx->bch.tx_skb) {
-			/* send confirm, on trans, free on hdlc. */
-			if (test_bit(FLG_TRANSPARENT, &hx->bch.Flags))
-				confirm_Bsend(&hx->bch);
+	} else {
+		if (hx->bch.tx_skb)
 			dev_kfree_skb(hx->bch.tx_skb);
-		}
-		if (get_next_bframe(&hx->bch))
+		if (get_next_bframe(&hx->bch)) {
+			hscx_fill_fifo(hx);
+			test_and_clear_bit(FLG_TX_EMPTY, &hx->bch.Flags);
+		} else if (test_bit(FLG_TX_EMPTY, &hx->bch.Flags)) {
 			hscx_fill_fifo(hx);
+		}
 	}
 }
 
@@ -1069,7 +1078,7 @@ ipac_rme(struct hscx_hw *hx)
 		skb_trim(hx->bch.rx_skb, 0);
 	} else {
 		skb_trim(hx->bch.rx_skb, hx->bch.rx_skb->len - 1);
-		recv_Bchannel(&hx->bch, 0);
+		recv_Bchannel(&hx->bch, 0, false);
 	}
 }
 
@@ -1120,11 +1129,8 @@ ipac_irq(struct hscx_hw *hx, u8 ista)
 
 	if (istab & IPACX_B_RPF) {
 		hscx_empty_fifo(hx, hx->fifo_size);
-		if (test_bit(FLG_TRANSPARENT, &hx->bch.Flags)) {
-			/* receive transparent audio data */
-			if (hx->bch.rx_skb)
-				recv_Bchannel(&hx->bch, 0);
-		}
+		if (test_bit(FLG_TRANSPARENT, &hx->bch.Flags))
+			recv_Bchannel(&hx->bch, 0, false);
 	}
 
 	if (istab & IPACX_B_RFO) {
@@ -1137,7 +1143,9 @@ ipac_irq(struct hscx_hw *hx, u8 ista)
 
 	if (istab & IPACX_B_XDU) {
 		if (test_bit(FLG_TRANSPARENT, &hx->bch.Flags)) {
-			hscx_fill_fifo(hx);
+			if (test_bit(FLG_FILLEMPTY, &hx->bch.Flags))
+				test_and_set_bit(FLG_TX_EMPTY, &hx->bch.Flags);
+			hscx_xpr(hx);
 			return;
 		}
 		pr_debug("%s: B%1d XDU error at len %d\n", hx->ip->name,
@@ -1338,22 +1346,17 @@ hscx_l2l1(struct mISDNchannel *ch, struct sk_buff *skb)
 	struct hscx_hw	*hx = container_of(bch, struct hscx_hw, bch);
 	int ret = -EINVAL;
 	struct mISDNhead *hh = mISDN_HEAD_P(skb);
-	u32 id;
-	u_long flags;
+	unsigned long flags;
 
 	switch (hh->prim) {
 	case PH_DATA_REQ:
 		spin_lock_irqsave(hx->ip->hwlock, flags);
 		ret = bchannel_senddata(bch, skb);
 		if (ret > 0) { /* direct TX */
-			id = hh->id; /* skb can be freed */
 			ret = 0;
 			hscx_fill_fifo(hx);
-			spin_unlock_irqrestore(hx->ip->hwlock, flags);
-			if (!test_bit(FLG_TRANSPARENT, &bch->Flags))
-				queue_ch_frame(ch, PH_DATA_CNF, id, NULL);
-		} else
-			spin_unlock_irqrestore(hx->ip->hwlock, flags);
+		}
+		spin_unlock_irqrestore(hx->ip->hwlock, flags);
 		return ret;
 	case PH_ACTIVATE_REQ:
 		spin_lock_irqsave(hx->ip->hwlock, flags);
@@ -1388,20 +1391,7 @@ hscx_l2l1(struct mISDNchannel *ch, struct sk_buff *skb)
 static int
 channel_bctrl(struct bchannel *bch, struct mISDN_ctrl_req *cq)
 {
-	int	ret = 0;
-
-	switch (cq->op) {
-	case MISDN_CTRL_GETOP:
-		cq->op = 0;
-		break;
-		/* Nothing implemented yet */
-	case MISDN_CTRL_FILL_EMPTY:
-	default:
-		pr_info("%s: unknown Op %x\n", __func__, cq->op);
-		ret = -EINVAL;
-		break;
-	}
-	return ret;
+	return mISDN_ctrl_bchannel(bch, cq);
 }
 
 static int
@@ -1416,15 +1406,10 @@ hscx_bctrl(struct mISDNchannel *ch, u32 cmd, void *arg)
 	switch (cmd) {
 	case CLOSE_CHANNEL:
 		test_and_clear_bit(FLG_OPEN, &bch->Flags);
-		if (test_bit(FLG_ACTIVE, &bch->Flags)) {
-			spin_lock_irqsave(hx->ip->hwlock, flags);
-			mISDN_freebchannel(bch);
-			hscx_mode(hx, ISDN_P_NONE);
-			spin_unlock_irqrestore(hx->ip->hwlock, flags);
-		} else {
-			skb_queue_purge(&bch->rqueue);
-			bch->rcount = 0;
-		}
+		spin_lock_irqsave(hx->ip->hwlock, flags);
+		mISDN_freebchannel(bch);
+		hscx_mode(hx, ISDN_P_NONE);
+		spin_unlock_irqrestore(hx->ip->hwlock, flags);
 		ch->protocol = ISDN_P_NONE;
 		ch->peer = NULL;
 		module_put(hx->ip->owner);
@@ -1526,7 +1511,7 @@ channel_ctrl(struct ipac_hw *ipac, struct mISDN_ctrl_req *cq)
 
 	switch (cq->op) {
 	case MISDN_CTRL_GETOP:
-		cq->op = MISDN_CTRL_LOOP;
+		cq->op = MISDN_CTRL_LOOP | MISDN_CTRL_L1_TIMER3;
 		break;
 	case MISDN_CTRL_LOOP:
 		/* cq->channel: 0 disable, 1 B1 loop 2 B2 loop, 3 both */
@@ -1536,6 +1521,9 @@ channel_ctrl(struct ipac_hw *ipac, struct mISDN_ctrl_req *cq)
 		}
 		ret = ipac->ctrl(ipac, HW_TESTLOOP, cq->channel);
 		break;
+	case MISDN_CTRL_L1_TIMER3:
+		ret = ipac->isac.ctrl(&ipac->isac, HW_TIMER3_VALUE, cq->p1);
+		break;
 	default:
 		pr_info("%s: unknown CTRL OP %x\n", ipac->name, cq->op);
 		ret = -EINVAL;
@@ -1621,7 +1609,8 @@ mISDNipac_init(struct ipac_hw *ipac, void *hw)
 		set_channelmap(i + 1, ipac->isac.dch.dev.channelmap);
 		list_add(&ipac->hscx[i].bch.ch.list,
 			 &ipac->isac.dch.dev.bchannels);
-		mISDN_initbchannel(&ipac->hscx[i].bch, MAX_DATA_MEM);
+		mISDN_initbchannel(&ipac->hscx[i].bch, MAX_DATA_MEM,
+				   ipac->hscx[i].fifo_size);
 		ipac->hscx[i].bch.ch.nr = i + 1;
 		ipac->hscx[i].bch.ch.send = &hscx_l2l1;
 		ipac->hscx[i].bch.ch.ctrl = hscx_bctrl;
diff --git a/drivers/isdn/hardware/mISDN/mISDNisar.c b/drivers/isdn/hardware/mISDN/mISDNisar.c
index 9a6da6edcfa..be5973ded6d 100644
--- a/drivers/isdn/hardware/mISDN/mISDNisar.c
+++ b/drivers/isdn/hardware/mISDN/mISDNisar.c
@@ -421,13 +421,19 @@ deliver_status(struct isar_ch *ch, int status)
 static inline void
 isar_rcv_frame(struct isar_ch *ch)
 {
-	u8		*ptr;
+	u8	*ptr;
+	int	maxlen;
 
 	if (!ch->is->clsb) {
 		pr_debug("%s; ISAR zero len frame\n", ch->is->name);
 		ch->is->write_reg(ch->is->hw, ISAR_IIA, 0);
 		return;
 	}
+	if (test_bit(FLG_RX_OFF, &ch->bch.Flags)) {
+		ch->bch.dropcnt += ch->is->clsb;
+		ch->is->write_reg(ch->is->hw, ISAR_IIA, 0);
+		return;
+	}
 	switch (ch->bch.state) {
 	case ISDN_P_NONE:
 		pr_debug("%s: ISAR protocol 0 spurious IIS_RDATA %x/%x/%x\n",
@@ -437,36 +443,22 @@ isar_rcv_frame(struct isar_ch *ch)
 	case ISDN_P_B_RAW:
 	case ISDN_P_B_L2DTMF:
 	case ISDN_P_B_MODEM_ASYNC:
-		if (!ch->bch.rx_skb) {
-			ch->bch.rx_skb = mI_alloc_skb(ch->bch.maxlen,
-						      GFP_ATOMIC);
-			if (unlikely(!ch->bch.rx_skb)) {
-				pr_info("%s: B receive out of memory\n",
-					ch->is->name);
-				ch->is->write_reg(ch->is->hw, ISAR_IIA, 0);
-				break;
-			}
+		maxlen = bchannel_get_rxbuf(&ch->bch, ch->is->clsb);
+		if (maxlen < 0) {
+			pr_warning("%s.B%d: No bufferspace for %d bytes\n",
+				   ch->is->name, ch->bch.nr, ch->is->clsb);
+			ch->is->write_reg(ch->is->hw, ISAR_IIA, 0);
+			break;
 		}
 		rcv_mbox(ch->is, skb_put(ch->bch.rx_skb, ch->is->clsb));
-		recv_Bchannel(&ch->bch, 0);
+		recv_Bchannel(&ch->bch, 0, false);
 		break;
 	case ISDN_P_B_HDLC:
-		if (!ch->bch.rx_skb) {
-			ch->bch.rx_skb = mI_alloc_skb(ch->bch.maxlen,
-						      GFP_ATOMIC);
-			if (unlikely(!ch->bch.rx_skb)) {
-				pr_info("%s: B receive out of memory\n",
-					ch->is->name);
-				ch->is->write_reg(ch->is->hw, ISAR_IIA, 0);
-				break;
-			}
-		}
-		if ((ch->bch.rx_skb->len + ch->is->clsb) >
-		    (ch->bch.maxlen + 2)) {
-			pr_debug("%s: incoming packet too large\n",
-				 ch->is->name);
+		maxlen = bchannel_get_rxbuf(&ch->bch, ch->is->clsb);
+		if (maxlen < 0) {
+			pr_warning("%s.B%d: No bufferspace for %d bytes\n",
+				   ch->is->name, ch->bch.nr, ch->is->clsb);
 			ch->is->write_reg(ch->is->hw, ISAR_IIA, 0);
-			skb_trim(ch->bch.rx_skb, 0);
 			break;
 		}
 		if (ch->is->cmsb & HDLC_ERROR) {
@@ -494,7 +486,7 @@ isar_rcv_frame(struct isar_ch *ch)
 				break;
 			}
 			skb_trim(ch->bch.rx_skb, ch->bch.rx_skb->len - 2);
-			recv_Bchannel(&ch->bch, 0);
+			recv_Bchannel(&ch->bch, 0, false);
 		}
 		break;
 	case ISDN_P_B_T30_FAX:
@@ -530,7 +522,7 @@ isar_rcv_frame(struct isar_ch *ch)
 				ch->state = STFAX_ESCAPE;
 				/* set_skb_flag(skb, DF_NOMOREDATA); */
 			}
-			recv_Bchannel(&ch->bch, 0);
+			recv_Bchannel(&ch->bch, 0, false);
 			if (ch->is->cmsb & SART_NMD)
 				deliver_status(ch, HW_MOD_NOCARR);
 			break;
@@ -570,7 +562,7 @@ isar_rcv_frame(struct isar_ch *ch)
 				break;
 			}
 			skb_trim(ch->bch.rx_skb, ch->bch.rx_skb->len - 2);
-			recv_Bchannel(&ch->bch, 0);
+			recv_Bchannel(&ch->bch, 0, false);
 		}
 		if (ch->is->cmsb & SART_NMD) { /* ABORT */
 			pr_debug("%s: isar_rcv_frame: no more data\n",
@@ -598,16 +590,25 @@ isar_fill_fifo(struct isar_ch *ch)
 	u8 msb;
 	u8 *ptr;
 
-	pr_debug("%s: ch%d  tx_skb %p tx_idx %d\n",
-		 ch->is->name, ch->bch.nr, ch->bch.tx_skb, ch->bch.tx_idx);
-	if (!ch->bch.tx_skb)
+	pr_debug("%s: ch%d  tx_skb %d tx_idx %d\n", ch->is->name, ch->bch.nr,
+		 ch->bch.tx_skb ? ch->bch.tx_skb->len : -1, ch->bch.tx_idx);
+	if (!(ch->is->bstat &
+	      (ch->dpath == 1 ? BSTAT_RDM1 : BSTAT_RDM2)))
+		return;
+	if (!ch->bch.tx_skb) {
+		if (!test_bit(FLG_TX_EMPTY, &ch->bch.Flags) ||
+		    (ch->bch.state != ISDN_P_B_RAW))
+			return;
+		count = ch->mml;
+		/* use the card buffer */
+		memset(ch->is->buf, ch->bch.fill[0], count);
+		send_mbox(ch->is, SET_DPS(ch->dpath) | ISAR_HIS_SDATA,
+			  0, count, ch->is->buf);
 		return;
+	}
 	count = ch->bch.tx_skb->len - ch->bch.tx_idx;
 	if (count <= 0)
 		return;
-	if (!(ch->is->bstat &
-	      (ch->dpath == 1 ? BSTAT_RDM1 : BSTAT_RDM2)))
-		return;
 	if (count > ch->mml) {
 		msb = 0;
 		count = ch->mml;
@@ -686,9 +687,9 @@ sel_bch_isar(struct isar_hw *isar, u8 dpath)
 static void
 send_next(struct isar_ch *ch)
 {
-	pr_debug("%s: %s ch%d tx_skb %p tx_idx %d\n",
-		 ch->is->name, __func__, ch->bch.nr,
-		 ch->bch.tx_skb, ch->bch.tx_idx);
+	pr_debug("%s: %s ch%d tx_skb %d tx_idx %d\n", ch->is->name, __func__,
+		 ch->bch.nr, ch->bch.tx_skb ? ch->bch.tx_skb->len : -1,
+		 ch->bch.tx_idx);
 	if (ch->bch.state == ISDN_P_B_T30_FAX) {
 		if (ch->cmd == PCTRL_CMD_FTH) {
 			if (test_bit(FLG_LASTDATA, &ch->bch.Flags)) {
@@ -702,15 +703,14 @@ send_next(struct isar_ch *ch)
 			}
 		}
 	}
-	if (ch->bch.tx_skb) {
-		/* send confirm, on trans, free on hdlc. */
-		if (test_bit(FLG_TRANSPARENT, &ch->bch.Flags))
-			confirm_Bsend(&ch->bch);
+	if (ch->bch.tx_skb)
 		dev_kfree_skb(ch->bch.tx_skb);
-	}
-	if (get_next_bframe(&ch->bch))
+	if (get_next_bframe(&ch->bch)) {
 		isar_fill_fifo(ch);
-	else {
+		test_and_clear_bit(FLG_TX_EMPTY, &ch->bch.Flags);
+	} else if (test_bit(FLG_TX_EMPTY, &ch->bch.Flags)) {
+		isar_fill_fifo(ch);
+	} else {
 		if (test_and_clear_bit(FLG_DLEETX, &ch->bch.Flags)) {
 			if (test_and_clear_bit(FLG_LASTDATA,
 					       &ch->bch.Flags)) {
@@ -724,6 +724,8 @@ send_next(struct isar_ch *ch)
 			} else {
 				deliver_status(ch, HW_MOD_CONNECT);
 			}
+		} else if (test_bit(FLG_FILLEMPTY, &ch->bch.Flags)) {
+			test_and_set_bit(FLG_TX_EMPTY, &ch->bch.Flags);
 		}
 	}
 }
@@ -1487,14 +1489,10 @@ isar_l2l1(struct mISDNchannel *ch, struct sk_buff *skb)
 		spin_lock_irqsave(ich->is->hwlock, flags);
 		ret = bchannel_senddata(bch, skb);
 		if (ret > 0) { /* direct TX */
-			id = hh->id; /* skb can be freed */
 			ret = 0;
 			isar_fill_fifo(ich);
-			spin_unlock_irqrestore(ich->is->hwlock, flags);
-			if (!test_bit(FLG_TRANSPARENT, &bch->Flags))
-				queue_ch_frame(ch, PH_DATA_CNF, id, NULL);
-		} else
-			spin_unlock_irqrestore(ich->is->hwlock, flags);
+		}
+		spin_unlock_irqrestore(ich->is->hwlock, flags);
 		return ret;
 	case PH_ACTIVATE_REQ:
 		spin_lock_irqsave(ich->is->hwlock, flags);
@@ -1575,20 +1573,7 @@ isar_l2l1(struct mISDNchannel *ch, struct sk_buff *skb)
 static int
 channel_bctrl(struct bchannel *bch, struct mISDN_ctrl_req *cq)
 {
-	int	ret = 0;
-
-	switch (cq->op) {
-	case MISDN_CTRL_GETOP:
-		cq->op = 0;
-		break;
-		/* Nothing implemented yet */
-	case MISDN_CTRL_FILL_EMPTY:
-	default:
-		pr_info("%s: unknown Op %x\n", __func__, cq->op);
-		ret = -EINVAL;
-		break;
-	}
-	return ret;
+	return mISDN_ctrl_bchannel(bch, cq);
 }
 
 static int
@@ -1603,15 +1588,10 @@ isar_bctrl(struct mISDNchannel *ch, u32 cmd, void *arg)
 	switch (cmd) {
 	case CLOSE_CHANNEL:
 		test_and_clear_bit(FLG_OPEN, &bch->Flags);
-		if (test_bit(FLG_ACTIVE, &bch->Flags)) {
-			spin_lock_irqsave(ich->is->hwlock, flags);
-			mISDN_freebchannel(bch);
-			modeisar(ich, ISDN_P_NONE);
-			spin_unlock_irqrestore(ich->is->hwlock, flags);
-		} else {
-			skb_queue_purge(&bch->rqueue);
-			bch->rcount = 0;
-		}
+		spin_lock_irqsave(ich->is->hwlock, flags);
+		mISDN_freebchannel(bch);
+		modeisar(ich, ISDN_P_NONE);
+		spin_unlock_irqrestore(ich->is->hwlock, flags);
 		ch->protocol = ISDN_P_NONE;
 		ch->peer = NULL;
 		module_put(ich->is->owner);
@@ -1677,7 +1657,6 @@ isar_open(struct isar_hw *isar, struct channel_req *rq)
 	bch = &isar->ch[rq->adr.channel - 1].bch;
 	if (test_and_set_bit(FLG_OPEN, &bch->Flags))
 		return -EBUSY; /* b-channel can be only open once */
-	test_and_clear_bit(FLG_FILLEMPTY, &bch->Flags);
 	bch->ch.protocol = rq->protocol;
 	rq->ch = &bch->ch;
 	return 0;
@@ -1691,7 +1670,7 @@ mISDNisar_init(struct isar_hw *isar, void *hw)
 	isar->hw = hw;
 	for (i = 0; i < 2; i++) {
 		isar->ch[i].bch.nr = i + 1;
-		mISDN_initbchannel(&isar->ch[i].bch, MAX_DATA_MEM);
+		mISDN_initbchannel(&isar->ch[i].bch, MAX_DATA_MEM, 32);
 		isar->ch[i].bch.ch.nr = i + 1;
 		isar->ch[i].bch.ch.send = &isar_l2l1;
 		isar->ch[i].bch.ch.ctrl = isar_bctrl;
diff --git a/drivers/isdn/hardware/mISDN/netjet.c b/drivers/isdn/hardware/mISDN/netjet.c
index c726e09d098..c3e3e768627 100644
--- a/drivers/isdn/hardware/mISDN/netjet.c
+++ b/drivers/isdn/hardware/mISDN/netjet.c
@@ -386,24 +386,20 @@ read_dma(struct tiger_ch *bc, u32 idx, int cnt)
 			bc->bch.nr, idx);
 	}
 	bc->lastrx = idx;
-	if (!bc->bch.rx_skb) {
-		bc->bch.rx_skb = mI_alloc_skb(bc->bch.maxlen, GFP_ATOMIC);
-		if (!bc->bch.rx_skb) {
-			pr_info("%s: B%1d receive out of memory\n",
-				card->name, bc->bch.nr);
-			return;
-		}
+	if (test_bit(FLG_RX_OFF, &bc->bch.Flags)) {
+		bc->bch.dropcnt += cnt;
+		return;
 	}
-
-	if (test_bit(FLG_TRANSPARENT, &bc->bch.Flags)) {
-		if ((bc->bch.rx_skb->len + cnt) > bc->bch.maxlen) {
-			pr_debug("%s: B%1d overrun %d\n", card->name,
-				 bc->bch.nr, bc->bch.rx_skb->len + cnt);
-			skb_trim(bc->bch.rx_skb, 0);
-			return;
-		}
+	stat = bchannel_get_rxbuf(&bc->bch, cnt);
+	/* only transparent use the count here, HDLC overun is detected later */
+	if (stat == ENOMEM) {
+		pr_warning("%s.B%d: No memory for %d bytes\n",
+			   card->name, bc->bch.nr, cnt);
+		return;
+	}
+	if (test_bit(FLG_TRANSPARENT, &bc->bch.Flags))
 		p = skb_put(bc->bch.rx_skb, cnt);
-	} else
+	else
 		p = bc->hrbuf;
 
 	for (i = 0; i < cnt; i++) {
@@ -414,48 +410,45 @@ read_dma(struct tiger_ch *bc, u32 idx, int cnt)
 			idx = 0;
 		p[i] = val & 0xff;
 	}
+
+	if (test_bit(FLG_TRANSPARENT, &bc->bch.Flags)) {
+		recv_Bchannel(&bc->bch, 0, false);
+		return;
+	}
+
 	pn = bc->hrbuf;
-next_frame:
-	if (test_bit(FLG_HDLC, &bc->bch.Flags)) {
+	while (cnt > 0) {
 		stat = isdnhdlc_decode(&bc->hrecv, pn, cnt, &i,
 				       bc->bch.rx_skb->data, bc->bch.maxlen);
-		if (stat > 0) /* valid frame received */
+		if (stat > 0) { /* valid frame received */
 			p = skb_put(bc->bch.rx_skb, stat);
-		else if (stat == -HDLC_CRC_ERROR)
+			if (debug & DEBUG_HW_BFIFO) {
+				snprintf(card->log, LOG_SIZE,
+					 "B%1d-recv %s %d ", bc->bch.nr,
+					 card->name, stat);
+				print_hex_dump_bytes(card->log,
+						     DUMP_PREFIX_OFFSET, p,
+						     stat);
+			}
+			recv_Bchannel(&bc->bch, 0, false);
+			stat = bchannel_get_rxbuf(&bc->bch, bc->bch.maxlen);
+			if (stat < 0) {
+				pr_warning("%s.B%d: No memory for %d bytes\n",
+					   card->name, bc->bch.nr, cnt);
+				return;
+			}
+		} else if (stat == -HDLC_CRC_ERROR) {
 			pr_info("%s: B%1d receive frame CRC error\n",
 				card->name, bc->bch.nr);
-		else if (stat == -HDLC_FRAMING_ERROR)
+		} else if (stat == -HDLC_FRAMING_ERROR) {
 			pr_info("%s: B%1d receive framing error\n",
 				card->name, bc->bch.nr);
-		else if (stat == -HDLC_LENGTH_ERROR)
+		} else if (stat == -HDLC_LENGTH_ERROR) {
 			pr_info("%s: B%1d receive frame too long (> %d)\n",
 				card->name, bc->bch.nr, bc->bch.maxlen);
-	} else
-		stat = cnt;
-
-	if (stat > 0) {
-		if (debug & DEBUG_HW_BFIFO) {
-			snprintf(card->log, LOG_SIZE, "B%1d-recv %s %d ",
-				 bc->bch.nr, card->name, stat);
-			print_hex_dump_bytes(card->log, DUMP_PREFIX_OFFSET,
-					     p, stat);
 		}
-		recv_Bchannel(&bc->bch, 0);
-	}
-	if (test_bit(FLG_HDLC, &bc->bch.Flags)) {
 		pn += i;
 		cnt -= i;
-		if (!bc->bch.rx_skb) {
-			bc->bch.rx_skb = mI_alloc_skb(bc->bch.maxlen,
-						      GFP_ATOMIC);
-			if (!bc->bch.rx_skb) {
-				pr_info("%s: B%1d receive out of memory\n",
-					card->name, bc->bch.nr);
-				return;
-			}
-		}
-		if (cnt > 0)
-			goto next_frame;
 	}
 }
 
@@ -544,22 +537,31 @@ static void
 fill_dma(struct tiger_ch *bc)
 {
 	struct tiger_hw *card = bc->bch.hw;
-	int count, i;
-	u32 m, v;
+	int count, i, fillempty = 0;
+	u32 m, v, n = 0;
 	u8  *p;
 
 	if (bc->free == 0)
 		return;
-	count = bc->bch.tx_skb->len - bc->bch.tx_idx;
-	if (count <= 0)
-		return;
-	pr_debug("%s: %s B%1d %d/%d/%d/%d state %x idx %d/%d\n", card->name,
-		 __func__, bc->bch.nr, count, bc->free, bc->bch.tx_idx,
-		 bc->bch.tx_skb->len, bc->txstate, bc->idx, card->send.idx);
+	if (!bc->bch.tx_skb) {
+		if (!test_bit(FLG_TX_EMPTY, &bc->bch.Flags))
+			return;
+		fillempty = 1;
+		count = card->send.size >> 1;
+		p = bc->bch.fill;
+	} else {
+		count = bc->bch.tx_skb->len - bc->bch.tx_idx;
+		if (count <= 0)
+			return;
+		pr_debug("%s: %s B%1d %d/%d/%d/%d state %x idx %d/%d\n",
+			 card->name, __func__, bc->bch.nr, count, bc->free,
+			 bc->bch.tx_idx, bc->bch.tx_skb->len, bc->txstate,
+			 bc->idx, card->send.idx);
+		p = bc->bch.tx_skb->data + bc->bch.tx_idx;
+	}
 	if (bc->txstate & (TX_IDLE | TX_INIT | TX_UNDERRUN))
 		resync(bc, card);
-	p = bc->bch.tx_skb->data + bc->bch.tx_idx;
-	if (test_bit(FLG_HDLC, &bc->bch.Flags)) {
+	if (test_bit(FLG_HDLC, &bc->bch.Flags) && !fillempty) {
 		count = isdnhdlc_encode(&bc->hsend, p, count, &i,
 					bc->hsbuf, bc->free);
 		pr_debug("%s: B%1d hdlc encoded %d in %d\n", card->name,
@@ -570,17 +572,33 @@ fill_dma(struct tiger_ch *bc)
 	} else {
 		if (count > bc->free)
 			count = bc->free;
-		bc->bch.tx_idx += count;
+		if (!fillempty)
+			bc->bch.tx_idx += count;
 		bc->free -= count;
 	}
 	m = (bc->bch.nr & 1) ? 0xffffff00 : 0xffff00ff;
-	for (i = 0; i < count; i++) {
-		if (bc->idx >= card->send.size)
-			bc->idx = 0;
-		v = card->send.start[bc->idx];
-		v &= m;
-		v |= (bc->bch.nr & 1) ? (u32)(p[i]) : ((u32)(p[i])) << 8;
-		card->send.start[bc->idx++] = v;
+	if (fillempty) {
+		n = p[0];
+		if (!(bc->bch.nr & 1))
+			n <<= 8;
+		for (i = 0; i < count; i++) {
+			if (bc->idx >= card->send.size)
+				bc->idx = 0;
+			v = card->send.start[bc->idx];
+			v &= m;
+			v |= n;
+			card->send.start[bc->idx++] = v;
+		}
+	} else {
+		for (i = 0; i < count; i++) {
+			if (bc->idx >= card->send.size)
+				bc->idx = 0;
+			v = card->send.start[bc->idx];
+			v &= m;
+			n = p[i];
+			v |= (bc->bch.nr & 1) ? n : n << 8;
+			card->send.start[bc->idx++] = v;
+		}
 	}
 	if (debug & DEBUG_HW_BFIFO) {
 		snprintf(card->log, LOG_SIZE, "B%1d-send %s %d ",
@@ -595,21 +613,26 @@ fill_dma(struct tiger_ch *bc)
 static int
 bc_next_frame(struct tiger_ch *bc)
 {
-	if (bc->bch.tx_skb && bc->bch.tx_idx < bc->bch.tx_skb->len)
+	int ret = 1;
+
+	if (bc->bch.tx_skb && bc->bch.tx_idx < bc->bch.tx_skb->len) {
 		fill_dma(bc);
-	else {
-		if (bc->bch.tx_skb) {
-			/* send confirm, on trans, free on hdlc. */
-			if (test_bit(FLG_TRANSPARENT, &bc->bch.Flags))
-				confirm_Bsend(&bc->bch);
+	} else {
+		if (bc->bch.tx_skb)
 			dev_kfree_skb(bc->bch.tx_skb);
-		}
-		if (get_next_bframe(&bc->bch))
+		if (get_next_bframe(&bc->bch)) {
 			fill_dma(bc);
-		else
-			return 0;
+			test_and_clear_bit(FLG_TX_EMPTY, &bc->bch.Flags);
+		} else if (test_bit(FLG_TX_EMPTY, &bc->bch.Flags)) {
+			fill_dma(bc);
+		} else if (test_bit(FLG_FILLEMPTY, &bc->bch.Flags)) {
+			test_and_set_bit(FLG_TX_EMPTY, &bc->bch.Flags);
+			ret = 0;
+		} else {
+			ret = 0;
+		}
 	}
-	return 1;
+	return ret;
 }
 
 static void
@@ -732,22 +755,17 @@ nj_l2l1B(struct mISDNchannel *ch, struct sk_buff *skb)
 	struct tiger_ch *bc = container_of(bch, struct tiger_ch, bch);
 	struct tiger_hw *card = bch->hw;
 	struct mISDNhead *hh = mISDN_HEAD_P(skb);
-	u32 id;
-	u_long flags;
+	unsigned long flags;
 
 	switch (hh->prim) {
 	case PH_DATA_REQ:
 		spin_lock_irqsave(&card->lock, flags);
 		ret = bchannel_senddata(bch, skb);
 		if (ret > 0) { /* direct TX */
-			id = hh->id; /* skb can be freed */
 			fill_dma(bc);
 			ret = 0;
-			spin_unlock_irqrestore(&card->lock, flags);
-			if (!test_bit(FLG_TRANSPARENT, &bch->Flags))
-				queue_ch_frame(ch, PH_DATA_CNF, id, NULL);
-		} else
-			spin_unlock_irqrestore(&card->lock, flags);
+		}
+		spin_unlock_irqrestore(&card->lock, flags);
 		return ret;
 	case PH_ACTIVATE_REQ:
 		spin_lock_irqsave(&card->lock, flags);
@@ -778,21 +796,7 @@ nj_l2l1B(struct mISDNchannel *ch, struct sk_buff *skb)
 static int
 channel_bctrl(struct tiger_ch *bc, struct mISDN_ctrl_req *cq)
 {
-	int ret = 0;
-	struct tiger_hw *card  = bc->bch.hw;
-
-	switch (cq->op) {
-	case MISDN_CTRL_GETOP:
-		cq->op = 0;
-		break;
-		/* Nothing implemented yet */
-	case MISDN_CTRL_FILL_EMPTY:
-	default:
-		pr_info("%s: %s unknown Op %x\n", card->name, __func__, cq->op);
-		ret = -EINVAL;
-		break;
-	}
-	return ret;
+	return mISDN_ctrl_bchannel(&bc->bch, cq);
 }
 
 static int
@@ -808,14 +812,10 @@ nj_bctrl(struct mISDNchannel *ch, u32 cmd, void *arg)
 	switch (cmd) {
 	case CLOSE_CHANNEL:
 		test_and_clear_bit(FLG_OPEN, &bch->Flags);
-		if (test_bit(FLG_ACTIVE, &bch->Flags)) {
-			spin_lock_irqsave(&card->lock, flags);
-			mISDN_freebchannel(bch);
-			test_and_clear_bit(FLG_TX_BUSY, &bch->Flags);
-			test_and_clear_bit(FLG_ACTIVE, &bch->Flags);
-			mode_tiger(bc, ISDN_P_NONE);
-			spin_unlock_irqrestore(&card->lock, flags);
-		}
+		spin_lock_irqsave(&card->lock, flags);
+		mISDN_freebchannel(bch);
+		mode_tiger(bc, ISDN_P_NONE);
+		spin_unlock_irqrestore(&card->lock, flags);
 		ch->protocol = ISDN_P_NONE;
 		ch->peer = NULL;
 		module_put(THIS_MODULE);
@@ -837,7 +837,7 @@ channel_ctrl(struct tiger_hw *card, struct mISDN_ctrl_req *cq)
 
 	switch (cq->op) {
 	case MISDN_CTRL_GETOP:
-		cq->op = MISDN_CTRL_LOOP;
+		cq->op = MISDN_CTRL_LOOP | MISDN_CTRL_L1_TIMER3;
 		break;
 	case MISDN_CTRL_LOOP:
 		/* cq->channel: 0 disable, 1 B1 loop 2 B2 loop, 3 both */
@@ -847,6 +847,9 @@ channel_ctrl(struct tiger_hw *card, struct mISDN_ctrl_req *cq)
 		}
 		ret = card->isac.ctrl(&card->isac, HW_TESTLOOP, cq->channel);
 		break;
+	case MISDN_CTRL_L1_TIMER3:
+		ret = card->isac.ctrl(&card->isac, HW_TIMER3_VALUE, cq->p1);
+		break;
 	default:
 		pr_info("%s: %s unknown Op %x\n", card->name, __func__, cq->op);
 		ret = -EINVAL;
@@ -1027,7 +1030,8 @@ setup_instance(struct tiger_hw *card)
 	for (i = 0; i < 2; i++) {
 		card->bc[i].bch.nr = i + 1;
 		set_channelmap(i + 1, card->isac.dch.dev.channelmap);
-		mISDN_initbchannel(&card->bc[i].bch, MAX_DATA_MEM);
+		mISDN_initbchannel(&card->bc[i].bch, MAX_DATA_MEM,
+				   NJ_DMA_RXSIZE >> 1);
 		card->bc[i].bch.hw = card;
 		card->bc[i].bch.ch.send = nj_l2l1B;
 		card->bc[i].bch.ch.ctrl = nj_bctrl;
diff --git a/drivers/isdn/hardware/mISDN/speedfax.c b/drivers/isdn/hardware/mISDN/speedfax.c
index 04689935148..93f344d74e5 100644
--- a/drivers/isdn/hardware/mISDN/speedfax.c
+++ b/drivers/isdn/hardware/mISDN/speedfax.c
@@ -224,7 +224,7 @@ channel_ctrl(struct sfax_hw  *sf, struct mISDN_ctrl_req *cq)
 
 	switch (cq->op) {
 	case MISDN_CTRL_GETOP:
-		cq->op = MISDN_CTRL_LOOP;
+		cq->op = MISDN_CTRL_LOOP | MISDN_CTRL_L1_TIMER3;
 		break;
 	case MISDN_CTRL_LOOP:
 		/* cq->channel: 0 disable, 1 B1 loop 2 B2 loop, 3 both */
@@ -234,6 +234,9 @@ channel_ctrl(struct sfax_hw  *sf, struct mISDN_ctrl_req *cq)
 		}
 		ret = sf->isac.ctrl(&sf->isac, HW_TESTLOOP, cq->channel);
 		break;
+	case MISDN_CTRL_L1_TIMER3:
+		ret = sf->isac.ctrl(&sf->isac, HW_TIMER3_VALUE, cq->p1);
+		break;
 	default:
 		pr_info("%s: unknown Op %x\n", sf->name, cq->op);
 		ret = -EINVAL;
diff --git a/drivers/isdn/hardware/mISDN/w6692.c b/drivers/isdn/hardware/mISDN/w6692.c
index 2183357f079..26a86b84609 100644
--- a/drivers/isdn/hardware/mISDN/w6692.c
+++ b/drivers/isdn/hardware/mISDN/w6692.c
@@ -465,6 +465,7 @@ W6692_empty_Bfifo(struct w6692_ch *wch, int count)
 {
 	struct w6692_hw *card = wch->bch.hw;
 	u8 *ptr;
+	int maxlen;
 
 	pr_debug("%s: empty_Bfifo %d\n", card->name, count);
 	if (unlikely(wch->bch.state == ISDN_P_NONE)) {
@@ -474,20 +475,18 @@ W6692_empty_Bfifo(struct w6692_ch *wch, int count)
 			skb_trim(wch->bch.rx_skb, 0);
 		return;
 	}
-	if (!wch->bch.rx_skb) {
-		wch->bch.rx_skb = mI_alloc_skb(wch->bch.maxlen, GFP_ATOMIC);
-		if (unlikely(!wch->bch.rx_skb)) {
-			pr_info("%s: B receive out of memory\n", card->name);
-			WriteW6692B(wch, W_B_CMDR, W_B_CMDR_RACK |
-				    W_B_CMDR_RACT);
-			return;
-		}
+	if (test_bit(FLG_RX_OFF, &wch->bch.Flags)) {
+		wch->bch.dropcnt += count;
+		WriteW6692B(wch, W_B_CMDR, W_B_CMDR_RACK | W_B_CMDR_RACT);
+		return;
 	}
-	if (wch->bch.rx_skb->len + count > wch->bch.maxlen) {
-		pr_debug("%s: empty_Bfifo incoming packet too large\n",
-			 card->name);
+	maxlen = bchannel_get_rxbuf(&wch->bch, count);
+	if (maxlen < 0) {
 		WriteW6692B(wch, W_B_CMDR, W_B_CMDR_RACK | W_B_CMDR_RACT);
-		skb_trim(wch->bch.rx_skb, 0);
+		if (wch->bch.rx_skb)
+			skb_trim(wch->bch.rx_skb, 0);
+		pr_warning("%s.B%d: No bufferspace for %d bytes\n",
+			   card->name, wch->bch.nr, count);
 		return;
 	}
 	ptr = skb_put(wch->bch.rx_skb, count);
@@ -504,16 +503,22 @@ static void
 W6692_fill_Bfifo(struct w6692_ch *wch)
 {
 	struct w6692_hw *card = wch->bch.hw;
-	int count;
+	int count, fillempty = 0;
 	u8 *ptr, cmd = W_B_CMDR_RACT | W_B_CMDR_XMS;
 
 	pr_debug("%s: fill Bfifo\n", card->name);
-	if (!wch->bch.tx_skb)
-		return;
-	count = wch->bch.tx_skb->len - wch->bch.tx_idx;
-	if (count <= 0)
-		return;
-	ptr = wch->bch.tx_skb->data + wch->bch.tx_idx;
+	if (!wch->bch.tx_skb) {
+		if (!test_bit(FLG_TX_EMPTY, &wch->bch.Flags))
+			return;
+		ptr = wch->bch.fill;
+		count = W_B_FIFO_THRESH;
+		fillempty = 1;
+	} else {
+		count = wch->bch.tx_skb->len - wch->bch.tx_idx;
+		if (count <= 0)
+			return;
+		ptr = wch->bch.tx_skb->data + wch->bch.tx_idx;
+	}
 	if (count > W_B_FIFO_THRESH)
 		count = W_B_FIFO_THRESH;
 	else if (test_bit(FLG_HDLC, &wch->bch.Flags))
@@ -522,9 +527,16 @@ W6692_fill_Bfifo(struct w6692_ch *wch)
 	pr_debug("%s: fill Bfifo%d/%d\n", card->name,
 		 count, wch->bch.tx_idx);
 	wch->bch.tx_idx += count;
-	outsb(wch->addr + W_B_XFIFO, ptr, count);
+	if (fillempty) {
+		while (count > 0) {
+			outsb(wch->addr + W_B_XFIFO, ptr, MISDN_BCH_FILL_SIZE);
+			count -= MISDN_BCH_FILL_SIZE;
+		}
+	} else {
+		outsb(wch->addr + W_B_XFIFO, ptr, count);
+	}
 	WriteW6692B(wch, W_B_CMDR, cmd);
-	if (debug & DEBUG_HW_DFIFO) {
+	if ((debug & DEBUG_HW_BFIFO) && !fillempty) {
 		snprintf(card->log, 63, "B%1d-send %s %d ",
 			 wch->bch.nr, card->name, count);
 		print_hex_dump_bytes(card->log, DUMP_PREFIX_OFFSET, ptr, count);
@@ -638,17 +650,17 @@ w6692_mode(struct w6692_ch *wch, u32 pr)
 static void
 send_next(struct w6692_ch *wch)
 {
-	if (wch->bch.tx_skb && wch->bch.tx_idx < wch->bch.tx_skb->len)
+	if (wch->bch.tx_skb && wch->bch.tx_idx < wch->bch.tx_skb->len) {
 		W6692_fill_Bfifo(wch);
-	else {
-		if (wch->bch.tx_skb) {
-			/* send confirm, on trans, free on hdlc. */
-			if (test_bit(FLG_TRANSPARENT, &wch->bch.Flags))
-				confirm_Bsend(&wch->bch);
+	} else {
+		if (wch->bch.tx_skb)
 			dev_kfree_skb(wch->bch.tx_skb);
-		}
-		if (get_next_bframe(&wch->bch))
+		if (get_next_bframe(&wch->bch)) {
+			W6692_fill_Bfifo(wch);
+			test_and_clear_bit(FLG_TX_EMPTY, &wch->bch.Flags);
+		} else if (test_bit(FLG_TX_EMPTY, &wch->bch.Flags)) {
 			W6692_fill_Bfifo(wch);
+		}
 	}
 }
 
@@ -698,7 +710,7 @@ W6692B_interrupt(struct w6692_hw *card, int ch)
 			if (count == 0)
 				count = W_B_FIFO_THRESH;
 			W6692_empty_Bfifo(wch, count);
-			recv_Bchannel(&wch->bch, 0);
+			recv_Bchannel(&wch->bch, 0, false);
 		}
 	}
 	if (stat & W_B_EXI_RMR) {
@@ -714,9 +726,8 @@ W6692B_interrupt(struct w6692_hw *card, int ch)
 				    W_B_CMDR_RRST | W_B_CMDR_RACT);
 		} else {
 			W6692_empty_Bfifo(wch, W_B_FIFO_THRESH);
-			if (test_bit(FLG_TRANSPARENT, &wch->bch.Flags) &&
-			    wch->bch.rx_skb && (wch->bch.rx_skb->len > 0))
-				recv_Bchannel(&wch->bch, 0);
+			if (test_bit(FLG_TRANSPARENT, &wch->bch.Flags))
+				recv_Bchannel(&wch->bch, 0, false);
 		}
 	}
 	if (stat & W_B_EXI_RDOV) {
@@ -738,8 +749,8 @@ W6692B_interrupt(struct w6692_hw *card, int ch)
 				 wch->bch.nr, star);
 		}
 		if (star & W_B_STAR_XDOW) {
-			pr_debug("%s: B%d XDOW proto=%x\n", card->name,
-				 wch->bch.nr, wch->bch.state);
+			pr_warning("%s: B%d XDOW proto=%x\n", card->name,
+				   wch->bch.nr, wch->bch.state);
 #ifdef ERROR_STATISTIC
 			wch->bch.err_xdu++;
 #endif
@@ -752,20 +763,21 @@ W6692B_interrupt(struct w6692_hw *card, int ch)
 			}
 		}
 		send_next(wch);
-		if (stat & W_B_EXI_XDUN)
+		if (star & W_B_STAR_XDOW)
 			return; /* handle XDOW only once */
 	}
 	if (stat & W_B_EXI_XDUN) {
-		pr_debug("%s: B%d XDUN proto=%x\n", card->name,
-			 wch->bch.nr, wch->bch.state);
+		pr_warning("%s: B%d XDUN proto=%x\n", card->name,
+			   wch->bch.nr, wch->bch.state);
 #ifdef ERROR_STATISTIC
 		wch->bch.err_xdu++;
 #endif
-		WriteW6692B(wch, W_B_CMDR, W_B_CMDR_XRST | W_B_CMDR_RACT);
-		/* resend */
+		/* resend - no XRST needed */
 		if (wch->bch.tx_skb) {
 			if (!test_bit(FLG_TRANSPARENT, &wch->bch.Flags))
 				wch->bch.tx_idx = 0;
+		} else if (test_bit(FLG_FILLEMPTY, &wch->bch.Flags)) {
+			test_and_set_bit(FLG_TX_EMPTY, &wch->bch.Flags);
 		}
 		send_next(wch);
 	}
@@ -944,22 +956,17 @@ w6692_l2l1B(struct mISDNchannel *ch, struct sk_buff *skb)
 	struct w6692_hw *card = bch->hw;
 	int ret = -EINVAL;
 	struct mISDNhead *hh = mISDN_HEAD_P(skb);
-	u32 id;
-	u_long flags;
+	unsigned long flags;
 
 	switch (hh->prim) {
 	case PH_DATA_REQ:
 		spin_lock_irqsave(&card->lock, flags);
 		ret = bchannel_senddata(bch, skb);
 		if (ret > 0) { /* direct TX */
-			id = hh->id; /* skb can be freed */
 			ret = 0;
 			W6692_fill_Bfifo(bc);
-			spin_unlock_irqrestore(&card->lock, flags);
-			if (!test_bit(FLG_TRANSPARENT, &bch->Flags))
-				queue_ch_frame(ch, PH_DATA_CNF, id, NULL);
-		} else
-			spin_unlock_irqrestore(&card->lock, flags);
+		}
+		spin_unlock_irqrestore(&card->lock, flags);
 		return ret;
 	case PH_ACTIVATE_REQ:
 		spin_lock_irqsave(&card->lock, flags);
@@ -994,20 +1001,7 @@ w6692_l2l1B(struct mISDNchannel *ch, struct sk_buff *skb)
 static int
 channel_bctrl(struct bchannel *bch, struct mISDN_ctrl_req *cq)
 {
-	int	ret = 0;
-
-	switch (cq->op) {
-	case MISDN_CTRL_GETOP:
-		cq->op = 0;
-		break;
-		/* Nothing implemented yet */
-	case MISDN_CTRL_FILL_EMPTY:
-	default:
-		pr_info("%s: unknown Op %x\n", __func__, cq->op);
-		ret = -EINVAL;
-		break;
-	}
-	return ret;
+	return mISDN_ctrl_bchannel(bch, cq);
 }
 
 static int
@@ -1022,7 +1016,6 @@ open_bchannel(struct w6692_hw *card, struct channel_req *rq)
 	bch = &card->bc[rq->adr.channel - 1].bch;
 	if (test_and_set_bit(FLG_OPEN, &bch->Flags))
 		return -EBUSY; /* b-channel can be only open once */
-	test_and_clear_bit(FLG_FILLEMPTY, &bch->Flags);
 	bch->ch.protocol = rq->protocol;
 	rq->ch = &bch->ch;
 	return 0;
@@ -1035,7 +1028,10 @@ channel_ctrl(struct w6692_hw *card, struct mISDN_ctrl_req *cq)
 
 	switch (cq->op) {
 	case MISDN_CTRL_GETOP:
-		cq->op = 0;
+		cq->op = MISDN_CTRL_L1_TIMER3;
+		break;
+	case MISDN_CTRL_L1_TIMER3:
+		ret = l1_event(card->dch.l1, HW_TIMER3_VALUE | (cq->p1 & 0xff));
 		break;
 	default:
 		pr_info("%s: unknown CTRL OP %x\n", card->name, cq->op);
@@ -1058,15 +1054,10 @@ w6692_bctrl(struct mISDNchannel *ch, u32 cmd, void *arg)
 	switch (cmd) {
 	case CLOSE_CHANNEL:
 		test_and_clear_bit(FLG_OPEN, &bch->Flags);
-		if (test_bit(FLG_ACTIVE, &bch->Flags)) {
-			spin_lock_irqsave(&card->lock, flags);
-			mISDN_freebchannel(bch);
-			w6692_mode(bc, ISDN_P_NONE);
-			spin_unlock_irqrestore(&card->lock, flags);
-		} else {
-			skb_queue_purge(&bch->rqueue);
-			bch->rcount = 0;
-		}
+		spin_lock_irqsave(&card->lock, flags);
+		mISDN_freebchannel(bch);
+		w6692_mode(bc, ISDN_P_NONE);
+		spin_unlock_irqrestore(&card->lock, flags);
 		ch->protocol = ISDN_P_NONE;
 		ch->peer = NULL;
 		module_put(THIS_MODULE);
@@ -1320,7 +1311,8 @@ setup_instance(struct w6692_hw *card)
 	card->dch.hw = card;
 	card->dch.dev.nrbchan = 2;
 	for (i = 0; i < 2; i++) {
-		mISDN_initbchannel(&card->bc[i].bch, MAX_DATA_MEM);
+		mISDN_initbchannel(&card->bc[i].bch, MAX_DATA_MEM,
+				   W_B_FIFO_THRESH);
 		card->bc[i].bch.hw = card;
 		card->bc[i].bch.nr = i + 1;
 		card->bc[i].bch.ch.nr = i + 1;
diff --git a/drivers/isdn/hisax/hfc_usb.c b/drivers/isdn/hisax/hfc_usb.c
index 62c65bdefd8..84f9c810307 100644
--- a/drivers/isdn/hisax/hfc_usb.c
+++ b/drivers/isdn/hisax/hfc_usb.c
@@ -1568,6 +1568,7 @@ static struct usb_driver hfc_drv = {
 	.id_table = hfcusb_idtab,
 	.probe = hfc_usb_probe,
 	.disconnect = hfc_usb_disconnect,
+	.disable_hub_initiated_lpm = 1,
 };
 
 static void __exit
diff --git a/drivers/isdn/hisax/st5481_init.c b/drivers/isdn/hisax/st5481_init.c
index 100296e20dc..54ef9e4f8cb 100644
--- a/drivers/isdn/hisax/st5481_init.c
+++ b/drivers/isdn/hisax/st5481_init.c
@@ -182,6 +182,7 @@ static struct usb_driver st5481_usb_driver = {
 	.probe =	probe_st5481,
 	.disconnect =	disconnect_st5481,
 	.id_table =	st5481_ids,
+	.disable_hub_initiated_lpm = 1,
 };
 
 static int __init st5481_usb_init(void)
diff --git a/drivers/isdn/hysdn/hysdn_proclog.c b/drivers/isdn/hysdn/hysdn_proclog.c
index ba91333e3e4..88e4f0ee073 100644
--- a/drivers/isdn/hysdn/hysdn_proclog.c
+++ b/drivers/isdn/hysdn/hysdn_proclog.c
@@ -156,17 +156,9 @@ static ssize_t
 hysdn_log_write(struct file *file, const char __user *buf, size_t count, loff_t *off)
 {
 	int rc;
-	unsigned char valbuf[128];
 	hysdn_card *card = file->private_data;
 
-	if (count > (sizeof(valbuf) - 1))
-		count = sizeof(valbuf) - 1;	/* limit length */
-	if (copy_from_user(valbuf, buf, count))
-		return (-EFAULT);	/* copy failed */
-
-	valbuf[count] = 0;	/* terminating 0 */
-
-	rc = kstrtoul(valbuf, 0, &card->debug_flags);
+	rc = kstrtoul_from_user(buf, count, 0, &card->debug_flags);
 	if (rc < 0)
 		return rc;
 	hysdn_addlog(card, "debug set to 0x%lx", card->debug_flags);
diff --git a/drivers/isdn/i4l/isdn_bsdcomp.c b/drivers/isdn/i4l/isdn_bsdcomp.c
index c59e8d2c067..8837ac5a492 100644
--- a/drivers/isdn/i4l/isdn_bsdcomp.c
+++ b/drivers/isdn/i4l/isdn_bsdcomp.c
@@ -612,7 +612,7 @@ static int bsd_compress(void *state, struct sk_buff *skb_in, struct sk_buff *skb
 		db->n_bits++;
 
 	/* If output length is too large then this is an incompressible frame. */
-	if (!skb_out || (skb_out && skb_out->len >= skb_in->len)) {
+	if (!skb_out || skb_out->len >= skb_in->len) {
 		++db->incomp_count;
 		db->incomp_bytes += isize;
 		return 0;
diff --git a/drivers/isdn/i4l/isdn_common.c b/drivers/isdn/i4l/isdn_common.c
index d9f5524593f..8c610fa6782 100644
--- a/drivers/isdn/i4l/isdn_common.c
+++ b/drivers/isdn/i4l/isdn_common.c
@@ -46,7 +46,6 @@ static DEFINE_MUTEX(isdn_mutex);
 static char *isdn_revision = "$Revision: 1.1.2.3 $";
 
 extern char *isdn_net_revision;
-extern char *isdn_tty_revision;
 #ifdef CONFIG_ISDN_PPP
 extern char *isdn_ppp_revision;
 #else
@@ -2327,8 +2326,6 @@ static int __init isdn_init(void)
 		dev->chanmap[i] = -1;
 		dev->m_idx[i] = -1;
 		strcpy(dev->num[i], "???");
-		init_waitqueue_head(&dev->mdm.info[i].open_wait);
-		init_waitqueue_head(&dev->mdm.info[i].close_wait);
 	}
 	if (register_chrdev(ISDN_MAJOR, "isdn", &isdn_fops)) {
 		printk(KERN_WARNING "isdn: Could not register control devices\n");
@@ -2353,8 +2350,6 @@ static int __init isdn_init(void)
 
 	strcpy(tmprev, isdn_revision);
 	printk(KERN_NOTICE "ISDN subsystem Rev: %s/", isdn_getrev(tmprev));
-	strcpy(tmprev, isdn_tty_revision);
-	printk("%s/", isdn_getrev(tmprev));
 	strcpy(tmprev, isdn_net_revision);
 	printk("%s/", isdn_getrev(tmprev));
 	strcpy(tmprev, isdn_ppp_revision);
diff --git a/drivers/isdn/i4l/isdn_tty.c b/drivers/isdn/i4l/isdn_tty.c
index 3831abdbc66..7bc50670d7d 100644
--- a/drivers/isdn/i4l/isdn_tty.c
+++ b/drivers/isdn/i4l/isdn_tty.c
@@ -1,5 +1,4 @@
-/* $Id: isdn_tty.c,v 1.1.2.3 2004/02/10 01:07:13 keil Exp $
- *
+/*
  * Linux ISDN subsystem, tty functions and AT-command emulator (linklevel).
  *
  * Copyright 1994-1999  by Fritz Elfert (fritz@isdn4linux.de)
@@ -12,6 +11,7 @@
 #undef ISDN_TTY_STAT_DEBUG
 
 #include <linux/isdn.h>
+#include <linux/serial.h> /* ASYNC_* flags */
 #include <linux/slab.h>
 #include <linux/delay.h>
 #include <linux/mutex.h>
@@ -48,9 +48,6 @@ static int bit2si[8] =
 static int si2bit[8] =
 {4, 1, 4, 4, 4, 4, 4, 4};
 
-char *isdn_tty_revision = "$Revision: 1.1.2.3 $";
-
-
 /* isdn_tty_try_read() is called from within isdn_tty_rcv_skb()
  * to stuff incoming data directly into a tty's flip-buffer. This
  * is done to speed up tty-receiving if the receive-queue is empty.
@@ -68,49 +65,54 @@ isdn_tty_try_read(modem_info *info, struct sk_buff *skb)
 	struct tty_struct *tty;
 	char last;
 
-	if (info->online) {
-		if ((tty = info->tty)) {
-			if (info->mcr & UART_MCR_RTS) {
-				len = skb->len
+	if (!info->online)
+		return 0;
+
+	tty = info->port.tty;
+	if (!tty)
+		return 0;
+
+	if (!(info->mcr & UART_MCR_RTS))
+		return 0;
+
+	len = skb->len
 #ifdef CONFIG_ISDN_AUDIO
-					+ ISDN_AUDIO_SKB_DLECOUNT(skb)
+		+ ISDN_AUDIO_SKB_DLECOUNT(skb)
 #endif
-					;
+		;
+
+	c = tty_buffer_request_room(tty, len);
+	if (c < len)
+		return 0;
 
-				c = tty_buffer_request_room(tty, len);
-				if (c >= len) {
-#ifdef CONFIG_ISDN_AUDIO
-					if (ISDN_AUDIO_SKB_DLECOUNT(skb)) {
-						int l = skb->len;
-						unsigned char *dp = skb->data;
-						while (--l) {
-							if (*dp == DLE)
-								tty_insert_flip_char(tty, DLE, 0);
-							tty_insert_flip_char(tty, *dp++, 0);
-						}
-						if (*dp == DLE)
-							tty_insert_flip_char(tty, DLE, 0);
-						last = *dp;
-					} else {
-#endif
-						if (len > 1)
-							tty_insert_flip_string(tty, skb->data, len - 1);
-						last = skb->data[len - 1];
 #ifdef CONFIG_ISDN_AUDIO
-					}
+	if (ISDN_AUDIO_SKB_DLECOUNT(skb)) {
+		int l = skb->len;
+		unsigned char *dp = skb->data;
+		while (--l) {
+			if (*dp == DLE)
+				tty_insert_flip_char(tty, DLE, 0);
+			tty_insert_flip_char(tty, *dp++, 0);
+		}
+		if (*dp == DLE)
+			tty_insert_flip_char(tty, DLE, 0);
+		last = *dp;
+	} else {
 #endif
-					if (info->emu.mdmreg[REG_CPPP] & BIT_CPPP)
-						tty_insert_flip_char(tty, last, 0xFF);
-					else
-						tty_insert_flip_char(tty, last, TTY_NORMAL);
-					tty_flip_buffer_push(tty);
-					kfree_skb(skb);
-					return 1;
-				}
-			}
-		}
+		if (len > 1)
+			tty_insert_flip_string(tty, skb->data, len - 1);
+		last = skb->data[len - 1];
+#ifdef CONFIG_ISDN_AUDIO
 	}
-	return 0;
+#endif
+	if (info->emu.mdmreg[REG_CPPP] & BIT_CPPP)
+		tty_insert_flip_char(tty, last, 0xFF);
+	else
+		tty_insert_flip_char(tty, last, TTY_NORMAL);
+	tty_flip_buffer_push(tty);
+	kfree_skb(skb);
+
+	return 1;
 }
 
 /* isdn_tty_readmodem() is called periodically from within timer-interrupt.
@@ -128,35 +130,39 @@ isdn_tty_readmodem(void)
 	modem_info *info;
 
 	for (i = 0; i < ISDN_MAX_CHANNELS; i++) {
-		if ((midx = dev->m_idx[i]) >= 0) {
-			info = &dev->mdm.info[midx];
-			if (info->online) {
-				r = 0;
+		midx = dev->m_idx[i];
+		if (midx < 0)
+			continue;
+
+		info = &dev->mdm.info[midx];
+		if (!info->online)
+			continue;
+
+		r = 0;
 #ifdef CONFIG_ISDN_AUDIO
-				isdn_audio_eval_dtmf(info);
-				if ((info->vonline & 1) && (info->emu.vpar[1]))
-					isdn_audio_eval_silence(info);
-#endif
-				if ((tty = info->tty)) {
-					if (info->mcr & UART_MCR_RTS) {
-						/* CISCO AsyncPPP Hack */
-						if (!(info->emu.mdmreg[REG_CPPP] & BIT_CPPP))
-							r = isdn_readbchan_tty(info->isdn_driver, info->isdn_channel, tty, 0);
-						else
-							r = isdn_readbchan_tty(info->isdn_driver, info->isdn_channel, tty, 1);
-						if (r)
-							tty_flip_buffer_push(tty);
-					} else
-						r = 1;
-				} else
-					r = 1;
-				if (r) {
-					info->rcvsched = 0;
-					resched = 1;
-				} else
-					info->rcvsched = 1;
-			}
-		}
+		isdn_audio_eval_dtmf(info);
+		if ((info->vonline & 1) && (info->emu.vpar[1]))
+			isdn_audio_eval_silence(info);
+#endif
+		tty = info->port.tty;
+		if (tty) {
+			if (info->mcr & UART_MCR_RTS) {
+				/* CISCO AsyncPPP Hack */
+				if (!(info->emu.mdmreg[REG_CPPP] & BIT_CPPP))
+					r = isdn_readbchan_tty(info->isdn_driver, info->isdn_channel, tty, 0);
+				else
+					r = isdn_readbchan_tty(info->isdn_driver, info->isdn_channel, tty, 1);
+				if (r)
+					tty_flip_buffer_push(tty);
+			} else
+				r = 1;
+		} else
+			r = 1;
+		if (r) {
+			info->rcvsched = 0;
+			resched = 1;
+		} else
+			info->rcvsched = 1;
 	}
 	if (!resched)
 		isdn_timer_ctrl(ISDN_TIMER_MODEMREAD, 0);
@@ -294,7 +300,7 @@ isdn_tty_tint(modem_info *info)
 	len = skb->len;
 	if ((slen = isdn_writebuf_skb_stub(info->isdn_driver,
 					   info->isdn_channel, 1, skb)) == len) {
-		struct tty_struct *tty = info->tty;
+		struct tty_struct *tty = info->port.tty;
 		info->send_outstanding++;
 		info->msr &= ~UART_MSR_CTS;
 		info->lsr &= ~UART_LSR_TEMT;
@@ -327,7 +333,7 @@ isdn_tty_countDLE(unsigned char *buf, int len)
 static int
 isdn_tty_handleDLEdown(modem_info *info, atemu *m, int len)
 {
-	unsigned char *p = &info->xmit_buf[info->xmit_count];
+	unsigned char *p = &info->port.xmit_buf[info->xmit_count];
 	int count = 0;
 
 	while (len > 0) {
@@ -471,7 +477,7 @@ isdn_tty_senddown(modem_info *info)
 		return;
 	}
 	skb_reserve(skb, skb_res);
-	memcpy(skb_put(skb, buflen), info->xmit_buf, buflen);
+	memcpy(skb_put(skb, buflen), info->port.xmit_buf, buflen);
 	info->xmit_count = 0;
 #ifdef CONFIG_ISDN_AUDIO
 	if (info->vonline & 2) {
@@ -699,7 +705,7 @@ isdn_tty_modem_hup(modem_info *info, int local)
 	printk(KERN_DEBUG "Mhup ttyI%d\n", info->line);
 #endif
 	info->rcvsched = 0;
-	isdn_tty_flush_buffer(info->tty);
+	isdn_tty_flush_buffer(info->port.tty);
 	if (info->online) {
 		info->last_lhup = local;
 		info->online = 0;
@@ -997,20 +1003,21 @@ isdn_tty_paranoia_check(modem_info *info, char *name, const char *routine)
 static void
 isdn_tty_change_speed(modem_info *info)
 {
+	struct tty_port *port = &info->port;
 	uint cflag,
 		cval,
 		quot;
 	int i;
 
-	if (!info->tty || !info->tty->termios)
+	if (!port->tty || !port->tty->termios)
 		return;
-	cflag = info->tty->termios->c_cflag;
+	cflag = port->tty->termios->c_cflag;
 
 	quot = i = cflag & CBAUD;
 	if (i & CBAUDEX) {
 		i &= ~CBAUDEX;
 		if (i < 1 || i > 2)
-			info->tty->termios->c_cflag &= ~CBAUDEX;
+			port->tty->termios->c_cflag &= ~CBAUDEX;
 		else
 			i += 15;
 	}
@@ -1040,20 +1047,20 @@ isdn_tty_change_speed(modem_info *info)
 
 	/* CTS flow control flag and modem status interrupts */
 	if (cflag & CRTSCTS) {
-		info->flags |= ISDN_ASYNC_CTS_FLOW;
+		port->flags |= ASYNC_CTS_FLOW;
 	} else
-		info->flags &= ~ISDN_ASYNC_CTS_FLOW;
+		port->flags &= ~ASYNC_CTS_FLOW;
 	if (cflag & CLOCAL)
-		info->flags &= ~ISDN_ASYNC_CHECK_CD;
+		port->flags &= ~ASYNC_CHECK_CD;
 	else {
-		info->flags |= ISDN_ASYNC_CHECK_CD;
+		port->flags |= ASYNC_CHECK_CD;
 	}
 }
 
 static int
 isdn_tty_startup(modem_info *info)
 {
-	if (info->flags & ISDN_ASYNC_INITIALIZED)
+	if (info->port.flags & ASYNC_INITIALIZED)
 		return 0;
 	isdn_lock_drivers();
 #ifdef ISDN_DEBUG_MODEM_OPEN
@@ -1063,14 +1070,14 @@ isdn_tty_startup(modem_info *info)
 	 * Now, initialize the UART
 	 */
 	info->mcr = UART_MCR_DTR | UART_MCR_RTS | UART_MCR_OUT2;
-	if (info->tty)
-		clear_bit(TTY_IO_ERROR, &info->tty->flags);
+	if (info->port.tty)
+		clear_bit(TTY_IO_ERROR, &info->port.tty->flags);
 	/*
 	 * and set the speed of the serial port
 	 */
 	isdn_tty_change_speed(info);
 
-	info->flags |= ISDN_ASYNC_INITIALIZED;
+	info->port.flags |= ASYNC_INITIALIZED;
 	info->msr |= (UART_MSR_DSR | UART_MSR_CTS);
 	info->send_outstanding = 0;
 	return 0;
@@ -1083,14 +1090,14 @@ isdn_tty_startup(modem_info *info)
 static void
 isdn_tty_shutdown(modem_info *info)
 {
-	if (!(info->flags & ISDN_ASYNC_INITIALIZED))
+	if (!(info->port.flags & ASYNC_INITIALIZED))
 		return;
 #ifdef ISDN_DEBUG_MODEM_OPEN
 	printk(KERN_DEBUG "Shutting down isdnmodem port %d ....\n", info->line);
 #endif
 	isdn_unlock_drivers();
 	info->msr &= ~UART_MSR_RI;
-	if (!info->tty || (info->tty->termios->c_cflag & HUPCL)) {
+	if (!info->port.tty || (info->port.tty->termios->c_cflag & HUPCL)) {
 		info->mcr &= ~(UART_MCR_DTR | UART_MCR_RTS);
 		if (info->emu.mdmreg[REG_DTRHUP] & BIT_DTRHUP) {
 			isdn_tty_modem_reset_regs(info, 0);
@@ -1100,10 +1107,10 @@ isdn_tty_shutdown(modem_info *info)
 			isdn_tty_modem_hup(info, 1);
 		}
 	}
-	if (info->tty)
-		set_bit(TTY_IO_ERROR, &info->tty->flags);
+	if (info->port.tty)
+		set_bit(TTY_IO_ERROR, &info->port.tty->flags);
 
-	info->flags &= ~ISDN_ASYNC_INITIALIZED;
+	info->port.flags &= ~ASYNC_INITIALIZED;
 }
 
 /* isdn_tty_write() is the main send-routine. It is called from the upper
@@ -1146,7 +1153,7 @@ isdn_tty_write(struct tty_struct *tty, const u_char *buf, int count)
 				isdn_tty_check_esc(buf, m->mdmreg[REG_ESC], c,
 						   &(m->pluscount),
 						   &(m->lastplus));
-			memcpy(&(info->xmit_buf[info->xmit_count]), buf, c);
+			memcpy(&info->port.xmit_buf[info->xmit_count], buf, c);
 #ifdef CONFIG_ISDN_AUDIO
 			if (info->vonline) {
 				int cc = isdn_tty_handleDLEdown(info, m, c);
@@ -1478,107 +1485,6 @@ isdn_tty_set_termios(struct tty_struct *tty, struct ktermios *old_termios)
  * isdn_tty_open() and friends
  * ------------------------------------------------------------
  */
-static int
-isdn_tty_block_til_ready(struct tty_struct *tty, struct file *filp, modem_info *info)
-{
-	DECLARE_WAITQUEUE(wait, NULL);
-	int do_clocal = 0;
-	int retval;
-
-	/*
-	 * If the device is in the middle of being closed, then block
-	 * until it's done, and then try again.
-	 */
-	if (tty_hung_up_p(filp) ||
-	    (info->flags & ISDN_ASYNC_CLOSING)) {
-		if (info->flags & ISDN_ASYNC_CLOSING)
-			interruptible_sleep_on(&info->close_wait);
-#ifdef MODEM_DO_RESTART
-		if (info->flags & ISDN_ASYNC_HUP_NOTIFY)
-			return -EAGAIN;
-		else
-			return -ERESTARTSYS;
-#else
-		return -EAGAIN;
-#endif
-	}
-	/*
-	 * If non-blocking mode is set, then make the check up front
-	 * and then exit.
-	 */
-	if ((filp->f_flags & O_NONBLOCK) ||
-	    (tty->flags & (1 << TTY_IO_ERROR))) {
-		if (info->flags & ISDN_ASYNC_CALLOUT_ACTIVE)
-			return -EBUSY;
-		info->flags |= ISDN_ASYNC_NORMAL_ACTIVE;
-		return 0;
-	}
-	if (info->flags & ISDN_ASYNC_CALLOUT_ACTIVE) {
-		if (info->normal_termios.c_cflag & CLOCAL)
-			do_clocal = 1;
-	} else {
-		if (tty->termios->c_cflag & CLOCAL)
-			do_clocal = 1;
-	}
-	/*
-	 * Block waiting for the carrier detect and the line to become
-	 * free (i.e., not in use by the callout).  While we are in
-	 * this loop, info->count is dropped by one, so that
-	 * isdn_tty_close() knows when to free things.  We restore it upon
-	 * exit, either normal or abnormal.
-	 */
-	retval = 0;
-	add_wait_queue(&info->open_wait, &wait);
-#ifdef ISDN_DEBUG_MODEM_OPEN
-	printk(KERN_DEBUG "isdn_tty_block_til_ready before block: ttyi%d, count = %d\n",
-	       info->line, info->count);
-#endif
-	if (!(tty_hung_up_p(filp)))
-		info->count--;
-	info->blocked_open++;
-	while (1) {
-		set_current_state(TASK_INTERRUPTIBLE);
-		if (tty_hung_up_p(filp) ||
-		    !(info->flags & ISDN_ASYNC_INITIALIZED)) {
-#ifdef MODEM_DO_RESTART
-			if (info->flags & ISDN_ASYNC_HUP_NOTIFY)
-				retval = -EAGAIN;
-			else
-				retval = -ERESTARTSYS;
-#else
-			retval = -EAGAIN;
-#endif
-			break;
-		}
-		if (!(info->flags & ISDN_ASYNC_CALLOUT_ACTIVE) &&
-		    !(info->flags & ISDN_ASYNC_CLOSING) &&
-		    (do_clocal || (info->msr & UART_MSR_DCD))) {
-			break;
-		}
-		if (signal_pending(current)) {
-			retval = -ERESTARTSYS;
-			break;
-		}
-#ifdef ISDN_DEBUG_MODEM_OPEN
-		printk(KERN_DEBUG "isdn_tty_block_til_ready blocking: ttyi%d, count = %d\n",
-		       info->line, info->count);
-#endif
-		schedule();
-	}
-	current->state = TASK_RUNNING;
-	remove_wait_queue(&info->open_wait, &wait);
-	if (!tty_hung_up_p(filp))
-		info->count++;
-	info->blocked_open--;
-#ifdef ISDN_DEBUG_MODEM_OPEN
-	printk(KERN_DEBUG "isdn_tty_block_til_ready after blocking: ttyi%d, count = %d\n",
-	       info->line, info->count);
-#endif
-	if (retval)
-		return retval;
-	info->flags |= ISDN_ASYNC_NORMAL_ACTIVE;
-	return 0;
-}
 
 /*
  * This routine is called whenever a serial port is opened.  It
@@ -1589,23 +1495,22 @@ isdn_tty_block_til_ready(struct tty_struct *tty, struct file *filp, modem_info *
 static int
 isdn_tty_open(struct tty_struct *tty, struct file *filp)
 {
+	struct tty_port *port;
 	modem_info *info;
 	int retval;
 
 	info = &dev->mdm.info[tty->index];
 	if (isdn_tty_paranoia_check(info, tty->name, "isdn_tty_open"))
 		return -ENODEV;
-	if (!try_module_get(info->owner)) {
-		printk(KERN_WARNING "%s: cannot reserve module\n", __func__);
-		return -ENODEV;
-	}
+	port = &info->port;
 #ifdef ISDN_DEBUG_MODEM_OPEN
 	printk(KERN_DEBUG "isdn_tty_open %s, count = %d\n", tty->name,
-	       info->count);
+	       port->count);
 #endif
-	info->count++;
+	port->count++;
 	tty->driver_data = info;
-	info->tty = tty;
+	port->tty = tty;
+	tty->port = port;
 	/*
 	 * Start up serial port
 	 */
@@ -1614,15 +1519,13 @@ isdn_tty_open(struct tty_struct *tty, struct file *filp)
 #ifdef ISDN_DEBUG_MODEM_OPEN
 		printk(KERN_DEBUG "isdn_tty_open return after startup\n");
 #endif
-		module_put(info->owner);
 		return retval;
 	}
-	retval = isdn_tty_block_til_ready(tty, filp, info);
+	retval = tty_port_block_til_ready(port, tty, filp);
 	if (retval) {
 #ifdef ISDN_DEBUG_MODEM_OPEN
 		printk(KERN_DEBUG "isdn_tty_open return after isdn_tty_block_til_ready \n");
 #endif
-		module_put(info->owner);
 		return retval;
 	}
 #ifdef ISDN_DEBUG_MODEM_OPEN
@@ -1639,6 +1542,7 @@ static void
 isdn_tty_close(struct tty_struct *tty, struct file *filp)
 {
 	modem_info *info = (modem_info *) tty->driver_data;
+	struct tty_port *port = &info->port;
 	ulong timeout;
 
 	if (!info || isdn_tty_paranoia_check(info, tty->name, "isdn_tty_close"))
@@ -1649,7 +1553,7 @@ isdn_tty_close(struct tty_struct *tty, struct file *filp)
 #endif
 		return;
 	}
-	if ((tty->count == 1) && (info->count != 1)) {
+	if ((tty->count == 1) && (port->count != 1)) {
 		/*
 		 * Uh, oh.  tty->count is 1, which means that the tty
 		 * structure will be freed.  Info->count should always
@@ -1658,30 +1562,21 @@ isdn_tty_close(struct tty_struct *tty, struct file *filp)
 		 * serial port won't be shutdown.
 		 */
 		printk(KERN_ERR "isdn_tty_close: bad port count; tty->count is 1, "
-		       "info->count is %d\n", info->count);
-		info->count = 1;
+		       "info->count is %d\n", port->count);
+		port->count = 1;
 	}
-	if (--info->count < 0) {
+	if (--port->count < 0) {
 		printk(KERN_ERR "isdn_tty_close: bad port count for ttyi%d: %d\n",
-		       info->line, info->count);
-		info->count = 0;
+		       info->line, port->count);
+		port->count = 0;
 	}
-	if (info->count) {
+	if (port->count) {
 #ifdef ISDN_DEBUG_MODEM_OPEN
 		printk(KERN_DEBUG "isdn_tty_close after info->count != 0\n");
 #endif
-		module_put(info->owner);
 		return;
 	}
-	info->flags |= ISDN_ASYNC_CLOSING;
-	/*
-	 * Save the termios structure, since this port may have
-	 * separate termios for callout and dialin.
-	 */
-	if (info->flags & ISDN_ASYNC_NORMAL_ACTIVE)
-		info->normal_termios = *tty->termios;
-	if (info->flags & ISDN_ASYNC_CALLOUT_ACTIVE)
-		info->callout_termios = *tty->termios;
+	port->flags |= ASYNC_CLOSING;
 
 	tty->closing = 1;
 	/*
@@ -1690,7 +1585,7 @@ isdn_tty_close(struct tty_struct *tty, struct file *filp)
 	 * interrupt driver to stop checking the data ready bit in the
 	 * line status register.
 	 */
-	if (info->flags & ISDN_ASYNC_INITIALIZED) {
+	if (port->flags & ASYNC_INITIALIZED) {
 		tty_wait_until_sent_from_close(tty, 3000);	/* 30 seconds timeout */
 		/*
 		 * Before we drop DTR, make sure the UART transmitter
@@ -1708,16 +1603,10 @@ isdn_tty_close(struct tty_struct *tty, struct file *filp)
 	isdn_tty_shutdown(info);
 	isdn_tty_flush_buffer(tty);
 	tty_ldisc_flush(tty);
-	info->tty = NULL;
+	port->tty = NULL;
 	info->ncarrier = 0;
-	tty->closing = 0;
-	module_put(info->owner);
-	if (info->blocked_open) {
-		msleep_interruptible(500);
-		wake_up_interruptible(&info->open_wait);
-	}
-	info->flags &= ~(ISDN_ASYNC_NORMAL_ACTIVE | ISDN_ASYNC_CLOSING);
-	wake_up_interruptible(&info->close_wait);
+
+	tty_port_close_end(port, tty);
 #ifdef ISDN_DEBUG_MODEM_OPEN
 	printk(KERN_DEBUG "isdn_tty_close normal exit\n");
 #endif
@@ -1730,14 +1619,15 @@ static void
 isdn_tty_hangup(struct tty_struct *tty)
 {
 	modem_info *info = (modem_info *) tty->driver_data;
+	struct tty_port *port = &info->port;
 
 	if (isdn_tty_paranoia_check(info, tty->name, "isdn_tty_hangup"))
 		return;
 	isdn_tty_shutdown(info);
-	info->count = 0;
-	info->flags &= ~(ISDN_ASYNC_NORMAL_ACTIVE | ISDN_ASYNC_CALLOUT_ACTIVE);
-	info->tty = NULL;
-	wake_up_interruptible(&info->open_wait);
+	port->count = 0;
+	port->flags &= ~ASYNC_NORMAL_ACTIVE;
+	port->tty = NULL;
+	wake_up_interruptible(&port->open_wait);
 }
 
 /* This routine initializes all emulator-data.
@@ -1864,6 +1754,16 @@ static const struct tty_operations modem_ops = {
 	.tiocmset = isdn_tty_tiocmset,
 };
 
+static int isdn_tty_carrier_raised(struct tty_port *port)
+{
+	modem_info *info = container_of(port, modem_info, port);
+	return info->msr & UART_MSR_DCD;
+}
+
+static const struct tty_port_operations isdn_tty_port_ops = {
+	.carrier_raised = isdn_tty_carrier_raised,
+};
+
 int
 isdn_tty_modem_init(void)
 {
@@ -1899,9 +1799,8 @@ isdn_tty_modem_init(void)
 			goto err_unregister;
 		}
 #endif
-#ifdef MODULE
-		info->owner = THIS_MODULE;
-#endif
+		tty_port_init(&info->port);
+		info->port.ops = &isdn_tty_port_ops;
 		spin_lock_init(&info->readlock);
 		sprintf(info->last_cause, "0000");
 		sprintf(info->last_num, "none");
@@ -1913,12 +1812,7 @@ isdn_tty_modem_init(void)
 		isdn_tty_modem_reset_regs(info, 1);
 		info->magic = ISDN_ASYNC_MAGIC;
 		info->line = i;
-		info->tty = NULL;
 		info->x_char = 0;
-		info->count = 0;
-		info->blocked_open = 0;
-		init_waitqueue_head(&info->open_wait);
-		init_waitqueue_head(&info->close_wait);
 		info->isdn_driver = -1;
 		info->isdn_channel = -1;
 		info->drv_index = -1;
@@ -1930,13 +1824,15 @@ isdn_tty_modem_init(void)
 #ifdef CONFIG_ISDN_AUDIO
 		skb_queue_head_init(&info->dtmf_queue);
 #endif
-		if (!(info->xmit_buf = kmalloc(ISDN_SERIAL_XMIT_MAX + 5, GFP_KERNEL))) {
+		info->port.xmit_buf = kmalloc(ISDN_SERIAL_XMIT_MAX + 5,
+				GFP_KERNEL);
+		if (!info->port.xmit_buf) {
 			printk(KERN_ERR "Could not allocate modem xmit-buffer\n");
 			retval = -ENOMEM;
 			goto err_unregister;
 		}
 		/* Make room for T.70 header */
-		info->xmit_buf += 4;
+		info->port.xmit_buf += 4;
 	}
 	return 0;
 err_unregister:
@@ -1945,7 +1841,7 @@ err_unregister:
 #ifdef CONFIG_ISDN_TTY_FAX
 		kfree(info->fax);
 #endif
-		kfree(info->xmit_buf - 4);
+		kfree(info->port.xmit_buf - 4);
 	}
 	tty_unregister_driver(m->tty_modem);
 err:
@@ -1966,7 +1862,7 @@ isdn_tty_exit(void)
 #ifdef CONFIG_ISDN_TTY_FAX
 		kfree(info->fax);
 #endif
-		kfree(info->xmit_buf - 4);
+		kfree(info->port.xmit_buf - 4);
 	}
 	tty_unregister_driver(dev->mdm.tty_modem);
 	put_tty_driver(dev->mdm.tty_modem);
@@ -2068,7 +1964,7 @@ isdn_tty_find_icall(int di, int ch, setup_parm *setup)
 	for (i = 0; i < ISDN_MAX_CHANNELS; i++) {
 		modem_info *info = &dev->mdm.info[i];
 
-		if (info->count == 0)
+		if (info->port.count == 0)
 			continue;
 		if ((info->emu.mdmreg[REG_SI1] & si2bit[si1]) &&  /* SI1 is matching */
 		    (info->emu.mdmreg[REG_SI2] == si2))	{         /* SI2 is matching */
@@ -2076,12 +1972,12 @@ isdn_tty_find_icall(int di, int ch, setup_parm *setup)
 #ifdef ISDN_DEBUG_MODEM_ICALL
 			printk(KERN_DEBUG "m_fi: match1 wret=%d\n", wret);
 			printk(KERN_DEBUG "m_fi: idx=%d flags=%08lx drv=%d ch=%d usg=%d\n", idx,
-			       info->flags, info->isdn_driver, info->isdn_channel,
-			       dev->usage[idx]);
+			       info->port.flags, info->isdn_driver,
+			       info->isdn_channel, dev->usage[idx]);
 #endif
 			if (
 #ifndef FIX_FILE_TRANSFER
-				(info->flags & ISDN_ASYNC_NORMAL_ACTIVE) &&
+				(info->port.flags & ASYNC_NORMAL_ACTIVE) &&
 #endif
 				(info->isdn_driver == -1) &&
 				(info->isdn_channel == -1) &&
@@ -2120,8 +2016,7 @@ isdn_tty_find_icall(int di, int ch, setup_parm *setup)
 	return (wret == 2) ? 3 : 0;
 }
 
-#define TTY_IS_ACTIVE(info)						\
-	(info->flags & (ISDN_ASYNC_NORMAL_ACTIVE | ISDN_ASYNC_CALLOUT_ACTIVE))
+#define TTY_IS_ACTIVE(info)	(info->port.flags & ASYNC_NORMAL_ACTIVE)
 
 int
 isdn_tty_stat_callback(int i, isdn_ctrl *c)
@@ -2212,9 +2107,9 @@ isdn_tty_stat_callback(int i, isdn_ctrl *c)
 			 * for incoming call of this device when
 			 * DCD follow the state of incoming carrier
 			 */
-			if (info->blocked_open &&
+			if (info->port.blocked_open &&
 			    (info->emu.mdmreg[REG_DCD] & BIT_DCD)) {
-				wake_up_interruptible(&info->open_wait);
+				wake_up_interruptible(&info->port.open_wait);
 			}
 
 			/* Schedule CONNECT-Message to any tty
@@ -2222,7 +2117,8 @@ isdn_tty_stat_callback(int i, isdn_ctrl *c)
 			 * set DCD-bit of its modem-status.
 			 */
 			if (TTY_IS_ACTIVE(info) ||
-			    (info->blocked_open && (info->emu.mdmreg[REG_DCD] & BIT_DCD))) {
+			    (info->port.blocked_open &&
+			     (info->emu.mdmreg[REG_DCD] & BIT_DCD))) {
 				info->msr |= UART_MSR_DCD;
 				info->emu.charge = 0;
 				if (info->dialing & 0xf)
@@ -2339,8 +2235,8 @@ isdn_tty_at_cout(char *msg, modem_info *info)
 	l = strlen(msg);
 
 	spin_lock_irqsave(&info->readlock, flags);
-	tty = info->tty;
-	if ((info->flags & ISDN_ASYNC_CLOSING) || (!tty)) {
+	tty = info->port.tty;
+	if ((info->port.flags & ASYNC_CLOSING) || (!tty)) {
 		spin_unlock_irqrestore(&info->readlock, flags);
 		return;
 	}
@@ -2490,15 +2386,15 @@ isdn_tty_modem_result(int code, modem_info *info)
 	case RESULT_NO_CARRIER:
 #ifdef ISDN_DEBUG_MODEM_HUP
 		printk(KERN_DEBUG "modem_result: NO CARRIER %d %d\n",
-		       (info->flags & ISDN_ASYNC_CLOSING),
-		       (!info->tty));
+		       (info->port.flags & ASYNC_CLOSING),
+		       (!info->port.tty));
 #endif
 		m->mdmreg[REG_RINGCNT] = 0;
 		del_timer(&info->nc_timer);
 		info->ncarrier = 0;
-		if ((info->flags & ISDN_ASYNC_CLOSING) || (!info->tty)) {
+		if ((info->port.flags & ASYNC_CLOSING) || (!info->port.tty))
 			return;
-		}
+
 #ifdef CONFIG_ISDN_AUDIO
 		if (info->vonline & 1) {
 #ifdef ISDN_DEBUG_MODEM_VOICE
@@ -2629,14 +2525,11 @@ isdn_tty_modem_result(int code, modem_info *info)
 		}
 	}
 	if (code == RESULT_NO_CARRIER) {
-		if ((info->flags & ISDN_ASYNC_CLOSING) || (!info->tty)) {
+		if ((info->port.flags & ASYNC_CLOSING) || (!info->port.tty))
 			return;
-		}
-		if ((info->flags & ISDN_ASYNC_CHECK_CD) &&
-		    (!((info->flags & ISDN_ASYNC_CALLOUT_ACTIVE) &&
-		       (info->flags & ISDN_ASYNC_CALLOUT_NOHUP)))) {
-			tty_hangup(info->tty);
-		}
+
+		if (info->port.flags & ASYNC_CHECK_CD)
+			tty_hangup(info->port.tty);
 	}
 }
 
@@ -3803,19 +3696,19 @@ isdn_tty_modem_escape(void)
 	int midx;
 
 	for (i = 0; i < ISDN_MAX_CHANNELS; i++)
-		if (USG_MODEM(dev->usage[i]))
-			if ((midx = dev->m_idx[i]) >= 0) {
-				modem_info *info = &dev->mdm.info[midx];
-				if (info->online) {
-					ton = 1;
-					if ((info->emu.pluscount == 3) &&
-					    time_after(jiffies , info->emu.lastplus + PLUSWAIT2)) {
-						info->emu.pluscount = 0;
-						info->online = 0;
-						isdn_tty_modem_result(RESULT_OK, info);
-					}
+		if (USG_MODEM(dev->usage[i]) && (midx = dev->m_idx[i]) >= 0) {
+			modem_info *info = &dev->mdm.info[midx];
+			if (info->online) {
+				ton = 1;
+				if ((info->emu.pluscount == 3) &&
+				    time_after(jiffies,
+					    info->emu.lastplus + PLUSWAIT2)) {
+					info->emu.pluscount = 0;
+					info->online = 0;
+					isdn_tty_modem_result(RESULT_OK, info);
 				}
 			}
+		}
 	isdn_timer_ctrl(ISDN_TIMER_MODEMPLUS, ton);
 }
 
@@ -3873,15 +3766,14 @@ isdn_tty_carrier_timeout(void)
 
 	for (i = 0; i < ISDN_MAX_CHANNELS; i++) {
 		modem_info *info = &dev->mdm.info[i];
-		if (info->dialing) {
-			if (info->emu.carrierwait++ > info->emu.mdmreg[REG_WAITC]) {
-				info->dialing = 0;
-				isdn_tty_modem_result(RESULT_NO_CARRIER, info);
-				isdn_tty_modem_hup(info, 1);
-			}
-			else
-				ton = 1;
-		}
+		if (!info->dialing)
+			continue;
+		if (info->emu.carrierwait++ > info->emu.mdmreg[REG_WAITC]) {
+			info->dialing = 0;
+			isdn_tty_modem_result(RESULT_NO_CARRIER, info);
+			isdn_tty_modem_hup(info, 1);
+		} else
+			ton = 1;
 	}
 	isdn_timer_ctrl(ISDN_TIMER_CARRIER, ton);
 }
diff --git a/drivers/isdn/mISDN/core.c b/drivers/isdn/mISDN/core.c
index a24530f05db..c401634c00e 100644
--- a/drivers/isdn/mISDN/core.c
+++ b/drivers/isdn/mISDN/core.c
@@ -355,6 +355,22 @@ mISDN_unregister_Bprotocol(struct Bprotocol *bp)
 }
 EXPORT_SYMBOL(mISDN_unregister_Bprotocol);
 
+static const char *msg_no_channel = "<no channel>";
+static const char *msg_no_stack = "<no stack>";
+static const char *msg_no_stackdev = "<no stack device>";
+
+const char *mISDNDevName4ch(struct mISDNchannel *ch)
+{
+	if (!ch)
+		return msg_no_channel;
+	if (!ch->st)
+		return msg_no_stack;
+	if (!ch->st->dev)
+		return msg_no_stackdev;
+	return dev_name(&ch->st->dev->dev);
+};
+EXPORT_SYMBOL(mISDNDevName4ch);
+
 static int
 mISDNInit(void)
 {
diff --git a/drivers/isdn/mISDN/dsp.h b/drivers/isdn/mISDN/dsp.h
index afe4173ae00..fc1733a0884 100644
--- a/drivers/isdn/mISDN/dsp.h
+++ b/drivers/isdn/mISDN/dsp.h
@@ -76,7 +76,9 @@ extern u8 dsp_silence;
 #define MAX_SECONDS_JITTER_CHECK 5
 
 extern struct timer_list dsp_spl_tl;
-extern u32 dsp_spl_jiffies;
+
+/* the datatype need to match jiffies datatype */
+extern unsigned long dsp_spl_jiffies;
 
 /* the structure of conferences:
  *
diff --git a/drivers/isdn/mISDN/dsp_cmx.c b/drivers/isdn/mISDN/dsp_cmx.c
index 334feab060a..a4f05c54c32 100644
--- a/drivers/isdn/mISDN/dsp_cmx.c
+++ b/drivers/isdn/mISDN/dsp_cmx.c
@@ -742,8 +742,8 @@ dsp_cmx_hardware(struct dsp_conf *conf, struct dsp *dsp)
 					       member->dsp->pcm_slot_tx,
 					       member->dsp->pcm_bank_tx,
 					       member->dsp->pcm_bank_rx);
-				conf->hardware = 0;
-				conf->software = 1;
+				conf->hardware = 1;
+				conf->software = tx_data;
 				return;
 			}
 			/* find a new slot */
@@ -834,8 +834,8 @@ dsp_cmx_hardware(struct dsp_conf *conf, struct dsp *dsp)
 					       nextm->dsp->name,
 					       member->dsp->pcm_slot_tx,
 					       member->dsp->pcm_slot_rx);
-				conf->hardware = 0;
-				conf->software = 1;
+				conf->hardware = 1;
+				conf->software = tx_data;
 				return;
 			}
 			/* find two new slot */
@@ -939,8 +939,11 @@ dsp_cmx_hardware(struct dsp_conf *conf, struct dsp *dsp)
 	/* for more than two members.. */
 
 	/* if all members already have the same conference */
-	if (all_conf)
+	if (all_conf) {
+		conf->hardware = 1;
+		conf->software = tx_data;
 		return;
+	}
 
 	/*
 	 * if there is an existing conference, but not all members have joined
@@ -1013,6 +1016,8 @@ dsp_cmx_hardware(struct dsp_conf *conf, struct dsp *dsp)
 			dsp_cmx_hw_message(member->dsp,
 					   MISDN_CTRL_HFC_CONF_JOIN, current_conf, 0, 0, 0);
 		}
+		conf->hardware = 1;
+		conf->software = tx_data;
 		return;
 	}
 
@@ -1328,7 +1333,7 @@ dsp_cmx_send_member(struct dsp *dsp, int len, s32 *c, int members)
 		}
 		if (dsp->conf && dsp->conf->software && dsp->conf->hardware)
 			tx_data_only = 1;
-		if (dsp->conf->software && dsp->echo.hardware)
+		if (dsp->echo.software && dsp->echo.hardware)
 			tx_data_only = 1;
 	}
 
@@ -1619,7 +1624,7 @@ send_packet:
 
 static u32	jittercount; /* counter for jitter check */
 struct timer_list dsp_spl_tl;
-u32	dsp_spl_jiffies; /* calculate the next time to fire */
+unsigned long	dsp_spl_jiffies; /* calculate the next time to fire */
 static u16	dsp_count; /* last sample count */
 static int	dsp_count_valid; /* if we have last sample count */
 
diff --git a/drivers/isdn/mISDN/dsp_core.c b/drivers/isdn/mISDN/dsp_core.c
index 2ac2d7a25a9..28c99c623bc 100644
--- a/drivers/isdn/mISDN/dsp_core.c
+++ b/drivers/isdn/mISDN/dsp_core.c
@@ -268,6 +268,7 @@ dsp_fill_empty(struct dsp *dsp)
 	}
 	cq.op = MISDN_CTRL_FILL_EMPTY;
 	cq.p1 = 1;
+	cq.p2 = dsp_silence;
 	if (dsp->ch.peer->ctrl(dsp->ch.peer, CONTROL_CHANNEL, &cq)) {
 		printk(KERN_DEBUG "%s: CONTROL_CHANNEL failed\n",
 		       __func__);
diff --git a/drivers/isdn/mISDN/dsp_dtmf.c b/drivers/isdn/mISDN/dsp_dtmf.c
index 887860bdc63..642f30be5ce 100644
--- a/drivers/isdn/mISDN/dsp_dtmf.c
+++ b/drivers/isdn/mISDN/dsp_dtmf.c
@@ -222,16 +222,25 @@ coefficients:
 		goto storedigit;
 	}
 
-	if (dsp_debug & DEBUG_DSP_DTMFCOEFF)
+	if (dsp_debug & DEBUG_DSP_DTMFCOEFF) {
+		s32 tresh_100 = tresh/100;
+
+		if (tresh_100 == 0) {
+			tresh_100 = 1;
+			printk(KERN_DEBUG
+				"tresh(%d) too small set tresh/100 to 1\n",
+				tresh);
+		}
 		printk(KERN_DEBUG "a %3d %3d %3d %3d %3d %3d %3d %3d"
 		       " tr:%3d r %3d %3d %3d %3d %3d %3d %3d %3d\n",
 		       result[0] / 10000, result[1] / 10000, result[2] / 10000,
 		       result[3] / 10000, result[4] / 10000, result[5] / 10000,
 		       result[6] / 10000, result[7] / 10000, tresh / 10000,
-		       result[0] / (tresh / 100), result[1] / (tresh / 100),
-		       result[2] / (tresh / 100), result[3] / (tresh / 100),
-		       result[4] / (tresh / 100), result[5] / (tresh / 100),
-		       result[6] / (tresh / 100), result[7] / (tresh / 100));
+		       result[0] / (tresh_100), result[1] / (tresh_100),
+		       result[2] / (tresh_100), result[3] / (tresh_100),
+		       result[4] / (tresh_100), result[5] / (tresh_100),
+		       result[6] / (tresh_100), result[7] / (tresh_100));
+	}
 
 	/* calc digit (lowgroup/highgroup) */
 	lowgroup = -1;
diff --git a/drivers/isdn/mISDN/hwchannel.c b/drivers/isdn/mISDN/hwchannel.c
index c74c363554c..ef34fd40867 100644
--- a/drivers/isdn/mISDN/hwchannel.c
+++ b/drivers/isdn/mISDN/hwchannel.c
@@ -81,10 +81,16 @@ mISDN_initdchannel(struct dchannel *ch, int maxlen, void *phf)
 EXPORT_SYMBOL(mISDN_initdchannel);
 
 int
-mISDN_initbchannel(struct bchannel *ch, int maxlen)
+mISDN_initbchannel(struct bchannel *ch, unsigned short maxlen,
+		   unsigned short minlen)
 {
 	ch->Flags = 0;
+	ch->minlen = minlen;
+	ch->next_minlen = minlen;
+	ch->init_minlen = minlen;
 	ch->maxlen = maxlen;
+	ch->next_maxlen = maxlen;
+	ch->init_maxlen = maxlen;
 	ch->hw = NULL;
 	ch->rx_skb = NULL;
 	ch->tx_skb = NULL;
@@ -134,6 +140,14 @@ mISDN_clear_bchannel(struct bchannel *ch)
 	test_and_clear_bit(FLG_TX_BUSY, &ch->Flags);
 	test_and_clear_bit(FLG_TX_NEXT, &ch->Flags);
 	test_and_clear_bit(FLG_ACTIVE, &ch->Flags);
+	test_and_clear_bit(FLG_FILLEMPTY, &ch->Flags);
+	test_and_clear_bit(FLG_TX_EMPTY, &ch->Flags);
+	test_and_clear_bit(FLG_RX_OFF, &ch->Flags);
+	ch->dropcnt = 0;
+	ch->minlen = ch->init_minlen;
+	ch->next_minlen = ch->init_minlen;
+	ch->maxlen = ch->init_maxlen;
+	ch->next_maxlen = ch->init_maxlen;
 }
 EXPORT_SYMBOL(mISDN_clear_bchannel);
 
@@ -148,6 +162,51 @@ mISDN_freebchannel(struct bchannel *ch)
 }
 EXPORT_SYMBOL(mISDN_freebchannel);
 
+int
+mISDN_ctrl_bchannel(struct bchannel *bch, struct mISDN_ctrl_req *cq)
+{
+	int ret = 0;
+
+	switch (cq->op) {
+	case MISDN_CTRL_GETOP:
+		cq->op = MISDN_CTRL_RX_BUFFER | MISDN_CTRL_FILL_EMPTY |
+			 MISDN_CTRL_RX_OFF;
+		break;
+	case MISDN_CTRL_FILL_EMPTY:
+		if (cq->p1) {
+			memset(bch->fill, cq->p2 & 0xff, MISDN_BCH_FILL_SIZE);
+			test_and_set_bit(FLG_FILLEMPTY, &bch->Flags);
+		} else {
+			test_and_clear_bit(FLG_FILLEMPTY, &bch->Flags);
+		}
+		break;
+	case MISDN_CTRL_RX_OFF:
+		/* read back dropped byte count */
+		cq->p2 = bch->dropcnt;
+		if (cq->p1)
+			test_and_set_bit(FLG_RX_OFF, &bch->Flags);
+		else
+			test_and_clear_bit(FLG_RX_OFF, &bch->Flags);
+		bch->dropcnt = 0;
+		break;
+	case MISDN_CTRL_RX_BUFFER:
+		if (cq->p2 > MISDN_CTRL_RX_SIZE_IGNORE)
+			bch->next_maxlen = cq->p2;
+		if (cq->p1 > MISDN_CTRL_RX_SIZE_IGNORE)
+			bch->next_minlen = cq->p1;
+		/* we return the old values */
+		cq->p1 = bch->minlen;
+		cq->p2 = bch->maxlen;
+		break;
+	default:
+		pr_info("mISDN unhandled control %x operation\n", cq->op);
+		ret = -EINVAL;
+		break;
+	}
+	return ret;
+}
+EXPORT_SYMBOL(mISDN_ctrl_bchannel);
+
 static inline u_int
 get_sapi_tei(u_char *p)
 {
@@ -197,24 +256,37 @@ recv_Echannel(struct dchannel *ech, struct dchannel *dch)
 EXPORT_SYMBOL(recv_Echannel);
 
 void
-recv_Bchannel(struct bchannel *bch, unsigned int id)
+recv_Bchannel(struct bchannel *bch, unsigned int id, bool force)
 {
 	struct mISDNhead *hh;
 
-	hh = mISDN_HEAD_P(bch->rx_skb);
-	hh->prim = PH_DATA_IND;
-	hh->id = id;
-	if (bch->rcount >= 64) {
-		printk(KERN_WARNING "B-channel %p receive queue overflow, "
-		       "flushing!\n", bch);
-		skb_queue_purge(&bch->rqueue);
-		bch->rcount = 0;
+	/* if allocation did fail upper functions still may call us */
+	if (unlikely(!bch->rx_skb))
 		return;
+	if (unlikely(!bch->rx_skb->len)) {
+		/* we have no data to send - this may happen after recovery
+		 * from overflow or too small allocation.
+		 * We need to free the buffer here */
+		dev_kfree_skb(bch->rx_skb);
+		bch->rx_skb = NULL;
+	} else {
+		if (test_bit(FLG_TRANSPARENT, &bch->Flags) &&
+		    (bch->rx_skb->len < bch->minlen) && !force)
+				return;
+		hh = mISDN_HEAD_P(bch->rx_skb);
+		hh->prim = PH_DATA_IND;
+		hh->id = id;
+		if (bch->rcount >= 64) {
+			printk(KERN_WARNING
+			       "B%d receive queue overflow - flushing!\n",
+			       bch->nr);
+			skb_queue_purge(&bch->rqueue);
+		}
+		bch->rcount++;
+		skb_queue_tail(&bch->rqueue, bch->rx_skb);
+		bch->rx_skb = NULL;
+		schedule_event(bch, FLG_RECVQUEUE);
 	}
-	bch->rcount++;
-	skb_queue_tail(&bch->rqueue, bch->rx_skb);
-	bch->rx_skb = NULL;
-	schedule_event(bch, FLG_RECVQUEUE);
 }
 EXPORT_SYMBOL(recv_Bchannel);
 
@@ -272,7 +344,7 @@ get_next_dframe(struct dchannel *dch)
 }
 EXPORT_SYMBOL(get_next_dframe);
 
-void
+static void
 confirm_Bsend(struct bchannel *bch)
 {
 	struct sk_buff	*skb;
@@ -294,7 +366,6 @@ confirm_Bsend(struct bchannel *bch)
 	skb_queue_tail(&bch->rqueue, skb);
 	schedule_event(bch, FLG_RECVQUEUE);
 }
-EXPORT_SYMBOL(confirm_Bsend);
 
 int
 get_next_bframe(struct bchannel *bch)
@@ -305,8 +376,8 @@ get_next_bframe(struct bchannel *bch)
 		if (bch->tx_skb) {
 			bch->next_skb = NULL;
 			test_and_clear_bit(FLG_TX_NEXT, &bch->Flags);
-			if (!test_bit(FLG_TRANSPARENT, &bch->Flags))
-				confirm_Bsend(bch); /* not for transparent */
+			/* confirm imediately to allow next data */
+			confirm_Bsend(bch);
 			return 1;
 		} else {
 			test_and_clear_bit(FLG_TX_NEXT, &bch->Flags);
@@ -395,7 +466,62 @@ bchannel_senddata(struct bchannel *ch, struct sk_buff *skb)
 		/* write to fifo */
 		ch->tx_skb = skb;
 		ch->tx_idx = 0;
+		confirm_Bsend(ch);
 		return 1;
 	}
 }
 EXPORT_SYMBOL(bchannel_senddata);
+
+/* The function allocates a new receive skb on demand with a size for the
+ * requirements of the current protocol. It returns the tailroom of the
+ * receive skb or an error.
+ */
+int
+bchannel_get_rxbuf(struct bchannel *bch, int reqlen)
+{
+	int len;
+
+	if (bch->rx_skb) {
+		len = skb_tailroom(bch->rx_skb);
+		if (len < reqlen) {
+			pr_warning("B%d no space for %d (only %d) bytes\n",
+				   bch->nr, reqlen, len);
+			if (test_bit(FLG_TRANSPARENT, &bch->Flags)) {
+				/* send what we have now and try a new buffer */
+				recv_Bchannel(bch, 0, true);
+			} else {
+				/* on HDLC we have to drop too big frames */
+				return -EMSGSIZE;
+			}
+		} else {
+			return len;
+		}
+	}
+	/* update current min/max length first */
+	if (unlikely(bch->maxlen != bch->next_maxlen))
+		bch->maxlen = bch->next_maxlen;
+	if (unlikely(bch->minlen != bch->next_minlen))
+		bch->minlen = bch->next_minlen;
+	if (unlikely(reqlen > bch->maxlen))
+		return -EMSGSIZE;
+	if (test_bit(FLG_TRANSPARENT, &bch->Flags)) {
+		if (reqlen >= bch->minlen) {
+			len = reqlen;
+		} else {
+			len = 2 * bch->minlen;
+			if (len > bch->maxlen)
+				len = bch->maxlen;
+		}
+	} else {
+		/* with HDLC we do not know the length yet */
+		len = bch->maxlen;
+	}
+	bch->rx_skb = mI_alloc_skb(len, GFP_ATOMIC);
+	if (!bch->rx_skb) {
+		pr_warning("B%d receive no memory for %d bytes\n",
+			   bch->nr, len);
+		len = -ENOMEM;
+	}
+	return len;
+}
+EXPORT_SYMBOL(bchannel_get_rxbuf);
diff --git a/drivers/isdn/mISDN/l1oip_core.c b/drivers/isdn/mISDN/l1oip_core.c
index 0f88acf1185..db50f788855 100644
--- a/drivers/isdn/mISDN/l1oip_core.c
+++ b/drivers/isdn/mISDN/l1oip_core.c
@@ -1420,7 +1420,7 @@ init_card(struct l1oip *hc, int pri, int bundle)
 		bch->nr = i + ch;
 		bch->slot = i + ch;
 		bch->debug = debug;
-		mISDN_initbchannel(bch, MAX_DATA_MEM);
+		mISDN_initbchannel(bch, MAX_DATA_MEM, 0);
 		bch->hw = hc;
 		bch->ch.send = handle_bmsg;
 		bch->ch.ctrl = l1oip_bctrl;
diff --git a/drivers/isdn/mISDN/layer1.c b/drivers/isdn/mISDN/layer1.c
index 0fc49b37551..bebc57b7213 100644
--- a/drivers/isdn/mISDN/layer1.c
+++ b/drivers/isdn/mISDN/layer1.c
@@ -28,13 +28,15 @@ static u_int *debug;
 struct layer1 {
 	u_long Flags;
 	struct FsmInst l1m;
-	struct FsmTimer timer;
+	struct FsmTimer timer3;
+	struct FsmTimer timerX;
 	int delay;
+	int t3_value;
 	struct dchannel *dch;
 	dchannel_l1callback *dcb;
 };
 
-#define TIMER3_VALUE 7000
+#define TIMER3_DEFAULT_VALUE	7000
 
 static
 struct Fsm l1fsm_s = {NULL, 0, 0, NULL, NULL};
@@ -134,7 +136,7 @@ l1_deact_req_s(struct FsmInst *fi, int event, void *arg)
 	struct layer1 *l1 = fi->userdata;
 
 	mISDN_FsmChangeState(fi, ST_L1_F3);
-	mISDN_FsmRestartTimer(&l1->timer, 550, EV_TIMER_DEACT, NULL, 2);
+	mISDN_FsmRestartTimer(&l1->timerX, 550, EV_TIMER_DEACT, NULL, 2);
 	test_and_set_bit(FLG_L1_DEACTTIMER, &l1->Flags);
 }
 
@@ -179,11 +181,11 @@ l1_info4_ind(struct FsmInst *fi, int event, void *arg)
 	mISDN_FsmChangeState(fi, ST_L1_F7);
 	l1->dcb(l1->dch, INFO3_P8);
 	if (test_and_clear_bit(FLG_L1_DEACTTIMER, &l1->Flags))
-		mISDN_FsmDelTimer(&l1->timer, 4);
+		mISDN_FsmDelTimer(&l1->timerX, 4);
 	if (!test_bit(FLG_L1_ACTIVATED, &l1->Flags)) {
 		if (test_and_clear_bit(FLG_L1_T3RUN, &l1->Flags))
-			mISDN_FsmDelTimer(&l1->timer, 3);
-		mISDN_FsmRestartTimer(&l1->timer, 110, EV_TIMER_ACT, NULL, 2);
+			mISDN_FsmDelTimer(&l1->timer3, 3);
+		mISDN_FsmRestartTimer(&l1->timerX, 110, EV_TIMER_ACT, NULL, 2);
 		test_and_set_bit(FLG_L1_ACTTIMER, &l1->Flags);
 	}
 }
@@ -201,7 +203,7 @@ l1_timer3(struct FsmInst *fi, int event, void *arg)
 	}
 	if (l1->l1m.state != ST_L1_F6) {
 		mISDN_FsmChangeState(fi, ST_L1_F3);
-		l1->dcb(l1->dch, HW_POWERUP_REQ);
+		/* do not force anything here, we need send INFO 0 */
 	}
 }
 
@@ -233,8 +235,9 @@ l1_activate_s(struct FsmInst *fi, int event, void *arg)
 {
 	struct layer1 *l1 = fi->userdata;
 
-	mISDN_FsmRestartTimer(&l1->timer, TIMER3_VALUE, EV_TIMER3, NULL, 2);
+	mISDN_FsmRestartTimer(&l1->timer3, l1->t3_value, EV_TIMER3, NULL, 2);
 	test_and_set_bit(FLG_L1_T3RUN, &l1->Flags);
+	/* Tell HW to send INFO 1 */
 	l1->dcb(l1->dch, HW_RESET_REQ);
 }
 
@@ -302,7 +305,8 @@ static struct FsmNode L1SFnList[] =
 
 static void
 release_l1(struct layer1 *l1) {
-	mISDN_FsmDelTimer(&l1->timer, 0);
+	mISDN_FsmDelTimer(&l1->timerX, 0);
+	mISDN_FsmDelTimer(&l1->timer3, 0);
 	if (l1->dch)
 		l1->dch->l1 = NULL;
 	module_put(THIS_MODULE);
@@ -356,6 +360,16 @@ l1_event(struct layer1 *l1, u_int event)
 		release_l1(l1);
 		break;
 	default:
+		if ((event & ~HW_TIMER3_VMASK) == HW_TIMER3_VALUE) {
+			int val = event & HW_TIMER3_VMASK;
+
+			if (val < 5)
+				val = 5;
+			if (val > 30)
+				val = 30;
+			l1->t3_value = val;
+			break;
+		}
 		if (*debug & DEBUG_L1)
 			printk(KERN_DEBUG "%s %x unhandled\n",
 			       __func__, event);
@@ -377,13 +391,15 @@ create_l1(struct dchannel *dch, dchannel_l1callback *dcb) {
 	nl1->l1m.fsm = &l1fsm_s;
 	nl1->l1m.state = ST_L1_F3;
 	nl1->Flags = 0;
+	nl1->t3_value = TIMER3_DEFAULT_VALUE;
 	nl1->l1m.debug = *debug & DEBUG_L1_FSM;
 	nl1->l1m.userdata = nl1;
 	nl1->l1m.userint = 0;
 	nl1->l1m.printdebug = l1m_debug;
 	nl1->dch = dch;
 	nl1->dcb = dcb;
-	mISDN_FsmInitTimer(&nl1->l1m, &nl1->timer);
+	mISDN_FsmInitTimer(&nl1->l1m, &nl1->timer3);
+	mISDN_FsmInitTimer(&nl1->l1m, &nl1->timerX);
 	__module_get(THIS_MODULE);
 	dch->l1 = nl1;
 	return 0;
diff --git a/drivers/isdn/mISDN/layer2.c b/drivers/isdn/mISDN/layer2.c
index 39d7375fa55..0dc8abca140 100644
--- a/drivers/isdn/mISDN/layer2.c
+++ b/drivers/isdn/mISDN/layer2.c
@@ -58,6 +58,8 @@ enum {
 	EV_L1_DEACTIVATE,
 	EV_L2_T200,
 	EV_L2_T203,
+	EV_L2_T200I,
+	EV_L2_T203I,
 	EV_L2_SET_OWN_BUSY,
 	EV_L2_CLEAR_OWN_BUSY,
 	EV_L2_FRAME_ERROR,
@@ -86,6 +88,8 @@ static char *strL2Event[] =
 	"EV_L1_DEACTIVATE",
 	"EV_L2_T200",
 	"EV_L2_T203",
+	"EV_L2_T200I",
+	"EV_L2_T203I",
 	"EV_L2_SET_OWN_BUSY",
 	"EV_L2_CLEAR_OWN_BUSY",
 	"EV_L2_FRAME_ERROR",
@@ -106,8 +110,8 @@ l2m_debug(struct FsmInst *fi, char *fmt, ...)
 	vaf.fmt = fmt;
 	vaf.va = &va;
 
-	printk(KERN_DEBUG "l2 (sapi %d tei %d): %pV\n",
-	       l2->sapi, l2->tei, &vaf);
+	printk(KERN_DEBUG "%s l2 (sapi %d tei %d): %pV\n",
+	       mISDNDevName4ch(&l2->ch), l2->sapi, l2->tei, &vaf);
 
 	va_end(va);
 }
@@ -150,7 +154,8 @@ l2up(struct layer2 *l2, u_int prim, struct sk_buff *skb)
 	mISDN_HEAD_ID(skb) = (l2->ch.nr << 16) | l2->ch.addr;
 	err = l2->up->send(l2->up, skb);
 	if (err) {
-		printk(KERN_WARNING "%s: err=%d\n", __func__, err);
+		printk(KERN_WARNING "%s: dev %s err=%d\n", __func__,
+		       mISDNDevName4ch(&l2->ch), err);
 		dev_kfree_skb(skb);
 	}
 }
@@ -174,7 +179,8 @@ l2up_create(struct layer2 *l2, u_int prim, int len, void *arg)
 		memcpy(skb_put(skb, len), arg, len);
 	err = l2->up->send(l2->up, skb);
 	if (err) {
-		printk(KERN_WARNING "%s: err=%d\n", __func__, err);
+		printk(KERN_WARNING "%s: dev %s err=%d\n", __func__,
+		       mISDNDevName4ch(&l2->ch), err);
 		dev_kfree_skb(skb);
 	}
 }
@@ -185,7 +191,8 @@ l2down_skb(struct layer2 *l2, struct sk_buff *skb) {
 
 	ret = l2->ch.recv(l2->ch.peer, skb);
 	if (ret && (*debug & DEBUG_L2_RECV))
-		printk(KERN_DEBUG "l2down_skb: ret(%d)\n", ret);
+		printk(KERN_DEBUG "l2down_skb: dev %s ret(%d)\n",
+		       mISDNDevName4ch(&l2->ch), ret);
 	return ret;
 }
 
@@ -276,12 +283,37 @@ ph_data_confirm(struct layer2 *l2, struct mISDNhead *hh, struct sk_buff *skb) {
 	return ret;
 }
 
+static void
+l2_timeout(struct FsmInst *fi, int event, void *arg)
+{
+	struct layer2 *l2 = fi->userdata;
+	struct sk_buff *skb;
+	struct mISDNhead *hh;
+
+	skb = mI_alloc_skb(0, GFP_ATOMIC);
+	if (!skb) {
+		printk(KERN_WARNING "%s: L2(%d,%d) nr:%x timer %s no skb\n",
+		       mISDNDevName4ch(&l2->ch), l2->sapi, l2->tei,
+		       l2->ch.nr, event == EV_L2_T200 ? "T200" : "T203");
+		return;
+	}
+	hh = mISDN_HEAD_P(skb);
+	hh->prim = event == EV_L2_T200 ? DL_TIMER200_IND : DL_TIMER203_IND;
+	hh->id = l2->ch.nr;
+	if (*debug & DEBUG_TIMER)
+		printk(KERN_DEBUG "%s: L2(%d,%d) nr:%x timer %s expired\n",
+		       mISDNDevName4ch(&l2->ch), l2->sapi, l2->tei,
+		       l2->ch.nr, event == EV_L2_T200 ? "T200" : "T203");
+	if (l2->ch.st)
+		l2->ch.st->own.recv(&l2->ch.st->own, skb);
+}
+
 static int
 l2mgr(struct layer2 *l2, u_int prim, void *arg) {
 	long c = (long)arg;
 
-	printk(KERN_WARNING
-	       "l2mgr: addr:%x prim %x %c\n", l2->id, prim, (char)c);
+	printk(KERN_WARNING "l2mgr: dev %s addr:%x prim %x %c\n",
+	       mISDNDevName4ch(&l2->ch), l2->id, prim, (char)c);
 	if (test_bit(FLG_LAPD, &l2->flag) &&
 	    !test_bit(FLG_FIXED_TEI, &l2->flag)) {
 		switch (c) {
@@ -603,8 +635,8 @@ send_uframe(struct layer2 *l2, struct sk_buff *skb, u_char cmd, u_char cr)
 	else {
 		skb = mI_alloc_skb(i, GFP_ATOMIC);
 		if (!skb) {
-			printk(KERN_WARNING "%s: can't alloc skbuff\n",
-			       __func__);
+			printk(KERN_WARNING "%s: can't alloc skbuff in %s\n",
+			       mISDNDevName4ch(&l2->ch), __func__);
 			return;
 		}
 	}
@@ -1089,8 +1121,8 @@ enquiry_cr(struct layer2 *l2, u_char typ, u_char cr, u_char pf)
 		tmp[i++] = (l2->vr << 5) | typ | (pf ? 0x10 : 0);
 	skb = mI_alloc_skb(i, GFP_ATOMIC);
 	if (!skb) {
-		printk(KERN_WARNING
-		       "isdnl2 can't alloc sbbuff for enquiry_cr\n");
+		printk(KERN_WARNING "%s: isdnl2 can't alloc sbbuff in %s\n",
+		       mISDNDevName4ch(&l2->ch), __func__);
 		return;
 	}
 	memcpy(skb_put(skb, i), tmp, i);
@@ -1150,7 +1182,7 @@ invoke_retransmission(struct layer2 *l2, unsigned int nr)
 			else
 				printk(KERN_WARNING
 				       "%s: windowar[%d] is NULL\n",
-				       __func__, p1);
+				       mISDNDevName4ch(&l2->ch), p1);
 			l2->windowar[p1] = NULL;
 		}
 		mISDN_FsmEvent(&l2->l2m, EV_L2_ACK_PULL, NULL);
@@ -1461,8 +1493,8 @@ l2_pull_iqueue(struct FsmInst *fi, int event, void *arg)
 		p1 = (l2->vs - l2->va) % 8;
 	p1 = (p1 + l2->sow) % l2->window;
 	if (l2->windowar[p1]) {
-		printk(KERN_WARNING "isdnl2 try overwrite ack queue entry %d\n",
-		       p1);
+		printk(KERN_WARNING "%s: l2 try overwrite ack queue entry %d\n",
+		       mISDNDevName4ch(&l2->ch), p1);
 		dev_kfree_skb(l2->windowar[p1]);
 	}
 	l2->windowar[p1] = skb;
@@ -1482,12 +1514,14 @@ l2_pull_iqueue(struct FsmInst *fi, int event, void *arg)
 		memcpy(skb_push(nskb, i), header, i);
 	else {
 		printk(KERN_WARNING
-		       "isdnl2 pull_iqueue skb header(%d/%d) too short\n", i, p1);
+		       "%s: L2 pull_iqueue skb header(%d/%d) too short\n",
+		       mISDNDevName4ch(&l2->ch), i, p1);
 		oskb = nskb;
 		nskb = mI_alloc_skb(oskb->len + i, GFP_ATOMIC);
 		if (!nskb) {
 			dev_kfree_skb(oskb);
-			printk(KERN_WARNING "%s: no skb mem\n", __func__);
+			printk(KERN_WARNING "%s: no skb mem in %s\n",
+			       mISDNDevName4ch(&l2->ch), __func__);
 			return;
 		}
 		memcpy(skb_put(nskb, i), header, i);
@@ -1814,11 +1848,16 @@ static struct FsmNode L2FnList[] =
 	{ST_L2_8, EV_L2_SUPER, l2_st8_got_super},
 	{ST_L2_7, EV_L2_I, l2_got_iframe},
 	{ST_L2_8, EV_L2_I, l2_got_iframe},
-	{ST_L2_5, EV_L2_T200, l2_st5_tout_200},
-	{ST_L2_6, EV_L2_T200, l2_st6_tout_200},
-	{ST_L2_7, EV_L2_T200, l2_st7_tout_200},
-	{ST_L2_8, EV_L2_T200, l2_st8_tout_200},
-	{ST_L2_7, EV_L2_T203, l2_st7_tout_203},
+	{ST_L2_5, EV_L2_T200, l2_timeout},
+	{ST_L2_6, EV_L2_T200, l2_timeout},
+	{ST_L2_7, EV_L2_T200, l2_timeout},
+	{ST_L2_8, EV_L2_T200, l2_timeout},
+	{ST_L2_7, EV_L2_T203, l2_timeout},
+	{ST_L2_5, EV_L2_T200I, l2_st5_tout_200},
+	{ST_L2_6, EV_L2_T200I, l2_st6_tout_200},
+	{ST_L2_7, EV_L2_T200I, l2_st7_tout_200},
+	{ST_L2_8, EV_L2_T200I, l2_st8_tout_200},
+	{ST_L2_7, EV_L2_T203I, l2_st7_tout_203},
 	{ST_L2_7, EV_L2_ACK_PULL, l2_pull_iqueue},
 	{ST_L2_7, EV_L2_SET_OWN_BUSY, l2_set_own_busy},
 	{ST_L2_8, EV_L2_SET_OWN_BUSY, l2_set_own_busy},
@@ -1858,7 +1897,8 @@ ph_data_indication(struct layer2 *l2, struct mISDNhead *hh, struct sk_buff *skb)
 		ptei = *datap++;
 		if ((psapi & 1) || !(ptei & 1)) {
 			printk(KERN_WARNING
-			       "l2 D-channel frame wrong EA0/EA1\n");
+			       "%s l2 D-channel frame wrong EA0/EA1\n",
+			       mISDNDevName4ch(&l2->ch));
 			return ret;
 		}
 		psapi >>= 2;
@@ -1867,7 +1907,8 @@ ph_data_indication(struct layer2 *l2, struct mISDNhead *hh, struct sk_buff *skb)
 			/* not our business */
 			if (*debug & DEBUG_L2)
 				printk(KERN_DEBUG "%s: sapi %d/%d mismatch\n",
-				       __func__, psapi, l2->sapi);
+				       mISDNDevName4ch(&l2->ch), psapi,
+				       l2->sapi);
 			dev_kfree_skb(skb);
 			return 0;
 		}
@@ -1875,7 +1916,7 @@ ph_data_indication(struct layer2 *l2, struct mISDNhead *hh, struct sk_buff *skb)
 			/* not our business */
 			if (*debug & DEBUG_L2)
 				printk(KERN_DEBUG "%s: tei %d/%d mismatch\n",
-				       __func__, ptei, l2->tei);
+				       mISDNDevName4ch(&l2->ch), ptei, l2->tei);
 			dev_kfree_skb(skb);
 			return 0;
 		}
@@ -1916,7 +1957,8 @@ ph_data_indication(struct layer2 *l2, struct mISDNhead *hh, struct sk_buff *skb)
 	} else
 		c = 'L';
 	if (c) {
-		printk(KERN_WARNING "l2 D-channel frame error %c\n", c);
+		printk(KERN_WARNING "%s:l2 D-channel frame error %c\n",
+		       mISDNDevName4ch(&l2->ch), c);
 		mISDN_FsmEvent(&l2->l2m, EV_L2_FRAME_ERROR, (void *)(long)c);
 	}
 	return ret;
@@ -1930,8 +1972,17 @@ l2_send(struct mISDNchannel *ch, struct sk_buff *skb)
 	int			ret = -EINVAL;
 
 	if (*debug & DEBUG_L2_RECV)
-		printk(KERN_DEBUG "%s: prim(%x) id(%x) sapi(%d) tei(%d)\n",
-		       __func__, hh->prim, hh->id, l2->sapi, l2->tei);
+		printk(KERN_DEBUG "%s: %s prim(%x) id(%x) sapi(%d) tei(%d)\n",
+		       __func__, mISDNDevName4ch(&l2->ch), hh->prim, hh->id,
+		       l2->sapi, l2->tei);
+	if (hh->prim == DL_INTERN_MSG) {
+		struct mISDNhead *chh = hh + 1; /* saved copy */
+
+		*hh = *chh;
+		if (*debug & DEBUG_L2_RECV)
+			printk(KERN_DEBUG "%s: prim(%x) id(%x) internal msg\n",
+				mISDNDevName4ch(&l2->ch), hh->prim, hh->id);
+	}
 	switch (hh->prim) {
 	case PH_DATA_IND:
 		ret = ph_data_indication(l2, hh, skb);
@@ -1987,6 +2038,12 @@ l2_send(struct mISDNchannel *ch, struct sk_buff *skb)
 		ret = mISDN_FsmEvent(&l2->l2m, EV_L2_DL_RELEASE_REQ,
 				     skb);
 		break;
+	case DL_TIMER200_IND:
+		mISDN_FsmEvent(&l2->l2m, EV_L2_T200I, NULL);
+		break;
+	case DL_TIMER203_IND:
+		mISDN_FsmEvent(&l2->l2m, EV_L2_T203I, NULL);
+		break;
 	default:
 		if (*debug & DEBUG_L2)
 			l2m_debug(&l2->l2m, "l2 unknown pr %04x",
@@ -2005,7 +2062,8 @@ tei_l2(struct layer2 *l2, u_int cmd, u_long arg)
 	int		ret = -EINVAL;
 
 	if (*debug & DEBUG_L2_TEI)
-		printk(KERN_DEBUG "%s: cmd(%x)\n", __func__, cmd);
+		printk(KERN_DEBUG "%s: cmd(%x) in %s\n",
+		       mISDNDevName4ch(&l2->ch), cmd, __func__);
 	switch (cmd) {
 	case (MDL_ASSIGN_REQ):
 		ret = mISDN_FsmEvent(&l2->l2m, EV_L2_MDL_ASSIGN, (void *)arg);
@@ -2018,7 +2076,8 @@ tei_l2(struct layer2 *l2, u_int cmd, u_long arg)
 		break;
 	case (MDL_ERROR_RSP):
 		/* ETS 300-125 5.3.2.1 Test: TC13010 */
-		printk(KERN_NOTICE "MDL_ERROR|REQ (tei_l2)\n");
+		printk(KERN_NOTICE "%s: MDL_ERROR|REQ (tei_l2)\n",
+		       mISDNDevName4ch(&l2->ch));
 		ret = mISDN_FsmEvent(&l2->l2m, EV_L2_MDL_ERROR, NULL);
 		break;
 	}
@@ -2050,7 +2109,8 @@ l2_ctrl(struct mISDNchannel *ch, u_int cmd, void *arg)
 	u_int			info;
 
 	if (*debug & DEBUG_L2_CTRL)
-		printk(KERN_DEBUG "%s:(%x)\n", __func__, cmd);
+		printk(KERN_DEBUG "%s: %s cmd(%x)\n",
+		       mISDNDevName4ch(ch), __func__, cmd);
 
 	switch (cmd) {
 	case OPEN_CHANNEL:
diff --git a/drivers/isdn/mISDN/tei.c b/drivers/isdn/mISDN/tei.c
index ba2bc0c776e..be88728f110 100644
--- a/drivers/isdn/mISDN/tei.c
+++ b/drivers/isdn/mISDN/tei.c
@@ -790,18 +790,23 @@ tei_ph_data_ind(struct teimgr *tm, u_int mt, u_char *dp, int len)
 static struct layer2 *
 create_new_tei(struct manager *mgr, int tei, int sapi)
 {
-	u_long		opt = 0;
-	u_long		flags;
-	int		id;
-	struct layer2	*l2;
+	unsigned long		opt = 0;
+	unsigned long		flags;
+	int			id;
+	struct layer2		*l2;
+	struct channel_req	rq;
 
 	if (!mgr->up)
 		return NULL;
 	if ((tei >= 0) && (tei < 64))
 		test_and_set_bit(OPTION_L2_FIXEDTEI, &opt);
-	if (mgr->ch.st->dev->Dprotocols
-	    & ((1 << ISDN_P_TE_E1) | (1 << ISDN_P_NT_E1)))
+	if (mgr->ch.st->dev->Dprotocols & ((1 << ISDN_P_TE_E1) |
+	    (1 << ISDN_P_NT_E1))) {
 		test_and_set_bit(OPTION_L2_PMX, &opt);
+		rq.protocol = ISDN_P_NT_E1;
+	} else {
+		rq.protocol = ISDN_P_NT_S0;
+	}
 	l2 = create_l2(mgr->up, ISDN_P_LAPD_NT, opt, tei, sapi);
 	if (!l2) {
 		printk(KERN_WARNING "%s:no memory for layer2\n", __func__);
@@ -836,6 +841,14 @@ create_new_tei(struct manager *mgr, int tei, int sapi)
 		l2->ch.recv = mgr->ch.recv;
 		l2->ch.peer = mgr->ch.peer;
 		l2->ch.ctrl(&l2->ch, OPEN_CHANNEL, NULL);
+		/* We need open here L1 for the manager as well (refcounting) */
+		rq.adr.dev = mgr->ch.st->dev->id;
+		id = mgr->ch.st->own.ctrl(&mgr->ch.st->own, OPEN_CHANNEL, &rq);
+		if (id < 0) {
+			printk(KERN_WARNING "%s: cannot open L1\n", __func__);
+			l2->ch.ctrl(&l2->ch, CLOSE_CHANNEL, NULL);
+			l2 = NULL;
+		}
 	}
 	return l2;
 }
@@ -978,10 +991,11 @@ TEIrelease(struct layer2 *l2)
 static int
 create_teimgr(struct manager *mgr, struct channel_req *crq)
 {
-	struct layer2	*l2;
-	u_long		opt = 0;
-	u_long		flags;
-	int		id;
+	struct layer2		*l2;
+	unsigned long		opt = 0;
+	unsigned long		flags;
+	int			id;
+	struct channel_req	l1rq;
 
 	if (*debug & DEBUG_L2_TEI)
 		printk(KERN_DEBUG "%s: %s proto(%x) adr(%d %d %d %d)\n",
@@ -1016,6 +1030,7 @@ create_teimgr(struct manager *mgr, struct channel_req *crq)
 		if (crq->protocol == ISDN_P_LAPD_TE)
 			test_and_set_bit(MGR_OPT_USER, &mgr->options);
 	}
+	l1rq.adr = crq->adr;
 	if (mgr->ch.st->dev->Dprotocols
 	    & ((1 << ISDN_P_TE_E1) | (1 << ISDN_P_NT_E1)))
 		test_and_set_bit(OPTION_L2_PMX, &opt);
@@ -1023,6 +1038,8 @@ create_teimgr(struct manager *mgr, struct channel_req *crq)
 		mgr->up = crq->ch;
 		id = DL_INFO_L2_CONNECT;
 		teiup_create(mgr, DL_INFORMATION_IND, sizeof(id), &id);
+		if (test_bit(MGR_PH_ACTIVE, &mgr->options))
+			teiup_create(mgr, PH_ACTIVATE_IND, 0, NULL);
 		crq->ch = NULL;
 		if (!list_empty(&mgr->layer2)) {
 			read_lock_irqsave(&mgr->lock, flags);
@@ -1053,24 +1070,34 @@ create_teimgr(struct manager *mgr, struct channel_req *crq)
 		l2->tm->tei_m.fsm = &teifsmu;
 		l2->tm->tei_m.state = ST_TEI_NOP;
 		l2->tm->tval = 1000; /* T201  1 sec */
+		if (test_bit(OPTION_L2_PMX, &opt))
+			l1rq.protocol = ISDN_P_TE_E1;
+		else
+			l1rq.protocol = ISDN_P_TE_S0;
 	} else {
 		l2->tm->tei_m.fsm = &teifsmn;
 		l2->tm->tei_m.state = ST_TEI_NOP;
 		l2->tm->tval = 2000; /* T202  2 sec */
+		if (test_bit(OPTION_L2_PMX, &opt))
+			l1rq.protocol = ISDN_P_NT_E1;
+		else
+			l1rq.protocol = ISDN_P_NT_S0;
 	}
 	mISDN_FsmInitTimer(&l2->tm->tei_m, &l2->tm->timer);
 	write_lock_irqsave(&mgr->lock, flags);
 	id = get_free_id(mgr);
 	list_add_tail(&l2->list, &mgr->layer2);
 	write_unlock_irqrestore(&mgr->lock, flags);
-	if (id < 0) {
-		l2->ch.ctrl(&l2->ch, CLOSE_CHANNEL, NULL);
-	} else {
+	if (id >= 0) {
 		l2->ch.nr = id;
 		l2->up->nr = id;
 		crq->ch = &l2->ch;
-		id = 0;
+		/* We need open here L1 for the manager as well (refcounting) */
+		id = mgr->ch.st->own.ctrl(&mgr->ch.st->own, OPEN_CHANNEL,
+					  &l1rq);
 	}
+	if (id < 0)
+		l2->ch.ctrl(&l2->ch, CLOSE_CHANNEL, NULL);
 	return id;
 }
 
@@ -1096,12 +1123,16 @@ mgr_send(struct mISDNchannel *ch, struct sk_buff *skb)
 		break;
 	case PH_ACTIVATE_IND:
 		test_and_set_bit(MGR_PH_ACTIVE, &mgr->options);
+		if (mgr->up)
+			teiup_create(mgr, PH_ACTIVATE_IND, 0, NULL);
 		mISDN_FsmEvent(&mgr->deact, EV_ACTIVATE_IND, NULL);
 		do_send(mgr);
 		ret = 0;
 		break;
 	case PH_DEACTIVATE_IND:
 		test_and_clear_bit(MGR_PH_ACTIVE, &mgr->options);
+		if (mgr->up)
+			teiup_create(mgr, PH_DEACTIVATE_IND, 0, NULL);
 		mISDN_FsmEvent(&mgr->deact, EV_DEACTIVATE_IND, NULL);
 		ret = 0;
 		break;
@@ -1263,7 +1294,7 @@ static int
 mgr_bcast(struct mISDNchannel *ch, struct sk_buff *skb)
 {
 	struct manager		*mgr = container_of(ch, struct manager, bcast);
-	struct mISDNhead	*hh = mISDN_HEAD_P(skb);
+	struct mISDNhead	*hhc, *hh = mISDN_HEAD_P(skb);
 	struct sk_buff		*cskb = NULL;
 	struct layer2		*l2;
 	u_long			flags;
@@ -1278,10 +1309,17 @@ mgr_bcast(struct mISDNchannel *ch, struct sk_buff *skb)
 				skb = NULL;
 			} else {
 				if (!cskb)
-					cskb = skb_copy(skb, GFP_KERNEL);
+					cskb = skb_copy(skb, GFP_ATOMIC);
 			}
 			if (cskb) {
-				ret = l2->ch.send(&l2->ch, cskb);
+				hhc = mISDN_HEAD_P(cskb);
+				/* save original header behind normal header */
+				hhc++;
+				*hhc = *hh;
+				hhc--;
+				hhc->prim = DL_INTERN_MSG;
+				hhc->id = l2->ch.nr;
+				ret = ch->st->own.recv(&ch->st->own, cskb);
 				if (ret) {
 					if (*debug & DEBUG_SEND_ERR)
 						printk(KERN_DEBUG
diff --git a/drivers/leds/Kconfig b/drivers/leds/Kconfig
index ff4b8cfda58..04cb8c88d74 100644
--- a/drivers/leds/Kconfig
+++ b/drivers/leds/Kconfig
@@ -50,6 +50,19 @@ config LEDS_LM3530
 	  controlled manually or using PWM input or using ambient
 	  light automatically.
 
+config LEDS_LM3533
+	tristate "LED support for LM3533"
+	depends on LEDS_CLASS
+	depends on MFD_LM3533
+	help
+	  This option enables support for the LEDs on National Semiconductor /
+	  TI LM3533 Lighting Power chips.
+
+	  The LEDs can be controlled directly, through PWM input, or by the
+	  ambient-light-sensor interface. The chip supports
+	  hardware-accelerated blinking with maximum on and off periods of 9.8
+	  and 77 seconds respectively.
+
 config LEDS_LOCOMO
 	tristate "LED Support for Locomo device"
 	depends on LEDS_CLASS
@@ -259,6 +272,14 @@ config LEDS_DA903X
 	  This option enables support for on-chip LED drivers found
 	  on Dialog Semiconductor DA9030/DA9034 PMICs.
 
+config LEDS_DA9052
+	tristate "Dialog DA9052/DA9053 LEDS"
+	depends on LEDS_CLASS
+	depends on PMIC_DA9052
+	help
+	  This option enables support for on-chip LED drivers found
+	  on Dialog Semiconductor DA9052-BC and DA9053-AA/Bx PMICs.
+
 config LEDS_DAC124S085
 	tristate "LED Support for DAC124S085 SPI DAC"
 	depends on LEDS_CLASS
@@ -471,4 +492,12 @@ config LEDS_TRIGGER_DEFAULT_ON
 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 hadrware.
+	  If unsure, say Y.
+
 endif # NEW_LEDS
diff --git a/drivers/leds/Makefile b/drivers/leds/Makefile
index 890481cb09f..f8958cd6cf6 100644
--- a/drivers/leds/Makefile
+++ b/drivers/leds/Makefile
@@ -10,6 +10,7 @@ obj-$(CONFIG_LEDS_ATMEL_PWM)		+= leds-atmel-pwm.o
 obj-$(CONFIG_LEDS_BD2802)		+= leds-bd2802.o
 obj-$(CONFIG_LEDS_LOCOMO)		+= leds-locomo.o
 obj-$(CONFIG_LEDS_LM3530)		+= leds-lm3530.o
+obj-$(CONFIG_LEDS_LM3533)		+= leds-lm3533.o
 obj-$(CONFIG_LEDS_MIKROTIK_RB532)	+= leds-rb532.o
 obj-$(CONFIG_LEDS_S3C24XX)		+= leds-s3c24xx.o
 obj-$(CONFIG_LEDS_NET48XX)		+= leds-net48xx.o
@@ -31,6 +32,7 @@ obj-$(CONFIG_LEDS_FSG)			+= leds-fsg.o
 obj-$(CONFIG_LEDS_PCA955X)		+= leds-pca955x.o
 obj-$(CONFIG_LEDS_PCA9633)		+= leds-pca9633.o
 obj-$(CONFIG_LEDS_DA903X)		+= leds-da903x.o
+obj-$(CONFIG_LEDS_DA9052)		+= leds-da9052.o
 obj-$(CONFIG_LEDS_WM831X_STATUS)	+= leds-wm831x-status.o
 obj-$(CONFIG_LEDS_WM8350)		+= leds-wm8350.o
 obj-$(CONFIG_LEDS_PWM)			+= leds-pwm.o
@@ -56,3 +58,4 @@ 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_DEFAULT_ON)	+= ledtrig-default-on.o
+obj-$(CONFIG_LEDS_TRIGGER_TRANSIENT)	+= ledtrig-transient.o
diff --git a/drivers/leds/led-class.c b/drivers/leds/led-class.c
index 5bff8439dc6..8ee92c81aec 100644
--- a/drivers/leds/led-class.c
+++ b/drivers/leds/led-class.c
@@ -44,23 +44,18 @@ static ssize_t led_brightness_store(struct device *dev,
 		struct device_attribute *attr, const char *buf, size_t size)
 {
 	struct led_classdev *led_cdev = dev_get_drvdata(dev);
+	unsigned long state;
 	ssize_t ret = -EINVAL;
-	char *after;
-	unsigned long state = simple_strtoul(buf, &after, 10);
-	size_t count = after - buf;
 
-	if (isspace(*after))
-		count++;
+	ret = kstrtoul(buf, 10, &state);
+	if (ret)
+		return ret;
 
-	if (count == size) {
-		ret = count;
+	if (state == LED_OFF)
+		led_trigger_remove(led_cdev);
+	led_set_brightness(led_cdev, state);
 
-		if (state == LED_OFF)
-			led_trigger_remove(led_cdev);
-		led_set_brightness(led_cdev, state);
-	}
-
-	return ret;
+	return size;
 }
 
 static ssize_t led_max_brightness_show(struct device *dev,
diff --git a/drivers/leds/leds-da9052.c b/drivers/leds/leds-da9052.c
new file mode 100644
index 00000000000..58a5244c437
--- /dev/null
+++ b/drivers/leds/leds-da9052.c
@@ -0,0 +1,214 @@
+/*
+ * LED Driver for Dialog DA9052 PMICs.
+ *
+ * Copyright(c) 2012 Dialog Semiconductor Ltd.
+ *
+ * Author: David Dajun Chen <dchen@diasemi.com>
+ *
+ *  This program is free software; you can redistribute  it and/or modify it
+ *  under  the terms of  the GNU General  Public License as published by the
+ *  Free Software Foundation;  either version 2 of the  License, or (at your
+ *  option) any later version.
+ *
+ */
+
+#include <linux/module.h>
+#include <linux/kernel.h>
+#include <linux/init.h>
+#include <linux/platform_device.h>
+#include <linux/leds.h>
+#include <linux/workqueue.h>
+#include <linux/slab.h>
+
+#include <linux/mfd/da9052/reg.h>
+#include <linux/mfd/da9052/da9052.h>
+#include <linux/mfd/da9052/pdata.h>
+
+#define DA9052_OPENDRAIN_OUTPUT	2
+#define DA9052_SET_HIGH_LVL_OUTPUT	(1 << 3)
+#define DA9052_MASK_UPPER_NIBBLE	0xF0
+#define DA9052_MASK_LOWER_NIBBLE	0x0F
+#define DA9052_NIBBLE_SHIFT		4
+#define DA9052_MAX_BRIGHTNESS		0x5f
+
+struct da9052_led {
+	struct led_classdev cdev;
+	struct work_struct work;
+	struct da9052 *da9052;
+	unsigned char led_index;
+	unsigned char id;
+	int brightness;
+};
+
+static unsigned char led_reg[] = {
+	DA9052_LED_CONT_4_REG,
+	DA9052_LED_CONT_5_REG,
+};
+
+static int da9052_set_led_brightness(struct da9052_led *led)
+{
+	u8 val;
+	int error;
+
+	val = (led->brightness & 0x7f) | DA9052_LED_CONT_DIM;
+
+	error = da9052_reg_write(led->da9052, led_reg[led->led_index], val);
+	if (error < 0)
+		dev_err(led->da9052->dev, "Failed to set led brightness, %d\n",
+			error);
+	return error;
+}
+
+static void da9052_led_work(struct work_struct *work)
+{
+	struct da9052_led *led = container_of(work, struct da9052_led, work);
+
+	da9052_set_led_brightness(led);
+}
+
+static void da9052_led_set(struct led_classdev *led_cdev,
+			   enum led_brightness value)
+{
+	struct da9052_led *led;
+
+	led = container_of(led_cdev, struct da9052_led, cdev);
+	led->brightness = value;
+	schedule_work(&led->work);
+}
+
+static int da9052_configure_leds(struct da9052 *da9052)
+{
+	int error;
+	unsigned char register_value = DA9052_OPENDRAIN_OUTPUT
+				       | DA9052_SET_HIGH_LVL_OUTPUT;
+
+	error = da9052_reg_update(da9052, DA9052_GPIO_14_15_REG,
+				  DA9052_MASK_LOWER_NIBBLE,
+				  register_value);
+
+	if (error < 0) {
+		dev_err(da9052->dev, "Failed to write GPIO 14-15 reg, %d\n",
+			error);
+		return error;
+	}
+
+	error = da9052_reg_update(da9052, DA9052_GPIO_14_15_REG,
+				  DA9052_MASK_UPPER_NIBBLE,
+				  register_value << DA9052_NIBBLE_SHIFT);
+	if (error < 0)
+		dev_err(da9052->dev, "Failed to write GPIO 14-15 reg, %d\n",
+			error);
+
+	return error;
+}
+
+static int __devinit da9052_led_probe(struct platform_device *pdev)
+{
+	struct da9052_pdata *pdata;
+	struct da9052 *da9052;
+	struct led_platform_data *pled;
+	struct da9052_led *led = NULL;
+	int error = -ENODEV;
+	int i;
+
+	da9052 = dev_get_drvdata(pdev->dev.parent);
+	pdata = da9052->dev->platform_data;
+	if (pdata == NULL) {
+		dev_err(&pdev->dev, "No platform data\n");
+		goto err;
+	}
+
+	pled = pdata->pled;
+	if (pled == NULL) {
+		dev_err(&pdev->dev, "No platform data for LED\n");
+		goto err;
+	}
+
+	led = devm_kzalloc(&pdev->dev,
+			   sizeof(struct da9052_led) * pled->num_leds,
+			   GFP_KERNEL);
+	if (led == NULL) {
+		dev_err(&pdev->dev, "Failed to alloc memory\n");
+		error = -ENOMEM;
+		goto err;
+	}
+
+	for (i = 0; i < pled->num_leds; i++) {
+		led[i].cdev.name = pled->leds[i].name;
+		led[i].cdev.brightness_set = da9052_led_set;
+		led[i].cdev.brightness = LED_OFF;
+		led[i].cdev.max_brightness = DA9052_MAX_BRIGHTNESS;
+		led[i].brightness = LED_OFF;
+		led[i].led_index = pled->leds[i].flags;
+		led[i].da9052 = dev_get_drvdata(pdev->dev.parent);
+		INIT_WORK(&led[i].work, da9052_led_work);
+
+		error = led_classdev_register(pdev->dev.parent, &led[i].cdev);
+		if (error) {
+			dev_err(&pdev->dev, "Failed to register led %d\n",
+				led[i].led_index);
+			goto err_register;
+		}
+
+		error = da9052_set_led_brightness(&led[i]);
+		if (error) {
+			dev_err(&pdev->dev, "Unable to init led %d\n",
+				led[i].led_index);
+			continue;
+		}
+	}
+	error = da9052_configure_leds(led->da9052);
+	if (error) {
+		dev_err(&pdev->dev, "Failed to configure GPIO LED%d\n", error);
+		goto err_register;
+	}
+
+	platform_set_drvdata(pdev, led);
+
+	return 0;
+
+err_register:
+	for (i = i - 1; i >= 0; i--) {
+		led_classdev_unregister(&led[i].cdev);
+		cancel_work_sync(&led[i].work);
+	}
+err:
+	return error;
+}
+
+static int __devexit da9052_led_remove(struct platform_device *pdev)
+{
+	struct da9052_led *led = platform_get_drvdata(pdev);
+	struct da9052_pdata *pdata;
+	struct da9052 *da9052;
+	struct led_platform_data *pled;
+	int i;
+
+	da9052 = dev_get_drvdata(pdev->dev.parent);
+	pdata = da9052->dev->platform_data;
+	pled = pdata->pled;
+
+	for (i = 0; i < pled->num_leds; i++) {
+		led[i].brightness = 0;
+		da9052_set_led_brightness(&led[i]);
+		led_classdev_unregister(&led[i].cdev);
+		cancel_work_sync(&led[i].work);
+	}
+
+	return 0;
+}
+
+static struct platform_driver da9052_led_driver = {
+	.driver		= {
+		.name	= "da9052-leds",
+		.owner	= THIS_MODULE,
+	},
+	.probe		= da9052_led_probe,
+	.remove		= __devexit_p(da9052_led_remove),
+};
+
+module_platform_driver(da9052_led_driver);
+
+MODULE_AUTHOR("Dialog Semiconductor Ltd <dchen@diasemi.com>");
+MODULE_DESCRIPTION("LED driver for Dialog DA9052 PMIC");
+MODULE_LICENSE("GPL");
diff --git a/drivers/leds/leds-lm3530.c b/drivers/leds/leds-lm3530.c
index 968fd5fef4f..84ba6de8039 100644
--- a/drivers/leds/leds-lm3530.c
+++ b/drivers/leds/leds-lm3530.c
@@ -113,6 +113,18 @@ struct lm3530_data {
 	bool enable;
 };
 
+/*
+ * struct lm3530_als_data
+ * @config  : value of ALS configuration register
+ * @imp_sel : value of ALS resistor select register
+ * @zone    : values of ALS ZB(Zone Boundary) registers
+ */
+struct lm3530_als_data {
+	u8 config;
+	u8 imp_sel;
+	u8 zones[LM3530_ALS_ZB_MAX];
+};
+
 static const u8 lm3530_reg[LM3530_REG_MAX] = {
 	LM3530_GEN_CONFIG,
 	LM3530_ALS_CONFIG,
@@ -141,29 +153,65 @@ static int lm3530_get_mode_from_str(const char *str)
 	return -1;
 }
 
+static void lm3530_als_configure(struct lm3530_platform_data *pdata,
+				struct lm3530_als_data *als)
+{
+	int i;
+	u32 als_vmin, als_vmax, als_vstep;
+
+	if (pdata->als_vmax == 0) {
+		pdata->als_vmin = 0;
+		pdata->als_vmax = LM3530_ALS_WINDOW_mV;
+	}
+
+	als_vmin = pdata->als_vmin;
+	als_vmax = pdata->als_vmax;
+
+	if ((als_vmax - als_vmin) > LM3530_ALS_WINDOW_mV)
+		pdata->als_vmax = als_vmax = als_vmin + LM3530_ALS_WINDOW_mV;
+
+	/* n zone boundary makes n+1 zones */
+	als_vstep = (als_vmax - als_vmin) / (LM3530_ALS_ZB_MAX + 1);
+
+	for (i = 0; i < LM3530_ALS_ZB_MAX; i++)
+		als->zones[i] = (((als_vmin + LM3530_ALS_OFFSET_mV) +
+			als_vstep + (i * als_vstep)) * LED_FULL) / 1000;
+
+	als->config =
+		(pdata->als_avrg_time << LM3530_ALS_AVG_TIME_SHIFT) |
+		(LM3530_ENABLE_ALS) |
+		(pdata->als_input_mode << LM3530_ALS_SEL_SHIFT);
+
+	als->imp_sel =
+		(pdata->als1_resistor_sel << LM3530_ALS1_IMP_SHIFT) |
+		(pdata->als2_resistor_sel << LM3530_ALS2_IMP_SHIFT);
+}
+
 static int lm3530_init_registers(struct lm3530_data *drvdata)
 {
 	int ret = 0;
 	int i;
 	u8 gen_config;
-	u8 als_config = 0;
 	u8 brt_ramp;
-	u8 als_imp_sel = 0;
 	u8 brightness;
 	u8 reg_val[LM3530_REG_MAX];
-	u8 zones[LM3530_ALS_ZB_MAX];
-	u32 als_vmin, als_vmax, als_vstep;
 	struct lm3530_platform_data *pdata = drvdata->pdata;
 	struct i2c_client *client = drvdata->client;
 	struct lm3530_pwm_data *pwm = &pdata->pwm_data;
+	struct lm3530_als_data als;
+
+	memset(&als, 0, sizeof(struct lm3530_als_data));
 
 	gen_config = (pdata->brt_ramp_law << LM3530_RAMP_LAW_SHIFT) |
 			((pdata->max_current & 7) << LM3530_MAX_CURR_SHIFT);
 
 	switch (drvdata->mode) {
 	case LM3530_BL_MODE_MANUAL:
+		gen_config |= LM3530_ENABLE_I2C;
+		break;
 	case LM3530_BL_MODE_ALS:
 		gen_config |= LM3530_ENABLE_I2C;
+		lm3530_als_configure(pdata, &als);
 		break;
 	case LM3530_BL_MODE_PWM:
 		gen_config |= LM3530_ENABLE_PWM | LM3530_ENABLE_PWM_SIMPLE |
@@ -171,38 +219,6 @@ static int lm3530_init_registers(struct lm3530_data *drvdata)
 		break;
 	}
 
-	if (drvdata->mode == LM3530_BL_MODE_ALS) {
-		if (pdata->als_vmax == 0) {
-			pdata->als_vmin = 0;
-			pdata->als_vmax = LM3530_ALS_WINDOW_mV;
-		}
-
-		als_vmin = pdata->als_vmin;
-		als_vmax = pdata->als_vmax;
-
-		if ((als_vmax - als_vmin) > LM3530_ALS_WINDOW_mV)
-			pdata->als_vmax = als_vmax =
-				als_vmin + LM3530_ALS_WINDOW_mV;
-
-		/* n zone boundary makes n+1 zones */
-		als_vstep = (als_vmax - als_vmin) / (LM3530_ALS_ZB_MAX + 1);
-
-		for (i = 0; i < LM3530_ALS_ZB_MAX; i++)
-			zones[i] = (((als_vmin + LM3530_ALS_OFFSET_mV) +
-					als_vstep + (i * als_vstep)) * LED_FULL)
-					/ 1000;
-
-		als_config =
-			(pdata->als_avrg_time << LM3530_ALS_AVG_TIME_SHIFT) |
-			(LM3530_ENABLE_ALS) |
-			(pdata->als_input_mode << LM3530_ALS_SEL_SHIFT);
-
-		als_imp_sel =
-			(pdata->als1_resistor_sel << LM3530_ALS1_IMP_SHIFT) |
-			(pdata->als2_resistor_sel << LM3530_ALS2_IMP_SHIFT);
-
-	}
-
 	brt_ramp = (pdata->brt_ramp_fall << LM3530_BRT_RAMP_FALL_SHIFT) |
 			(pdata->brt_ramp_rise << LM3530_BRT_RAMP_RISE_SHIFT);
 
@@ -215,14 +231,14 @@ static int lm3530_init_registers(struct lm3530_data *drvdata)
 		brightness = drvdata->led_dev.max_brightness;
 
 	reg_val[0] = gen_config;	/* LM3530_GEN_CONFIG */
-	reg_val[1] = als_config;	/* LM3530_ALS_CONFIG */
+	reg_val[1] = als.config;	/* LM3530_ALS_CONFIG */
 	reg_val[2] = brt_ramp;		/* LM3530_BRT_RAMP_RATE */
-	reg_val[3] = als_imp_sel;	/* LM3530_ALS_IMP_SELECT */
+	reg_val[3] = als.imp_sel;	/* LM3530_ALS_IMP_SELECT */
 	reg_val[4] = brightness;	/* LM3530_BRT_CTRL_REG */
-	reg_val[5] = zones[0];		/* LM3530_ALS_ZB0_REG */
-	reg_val[6] = zones[1];		/* LM3530_ALS_ZB1_REG */
-	reg_val[7] = zones[2];		/* LM3530_ALS_ZB2_REG */
-	reg_val[8] = zones[3];		/* LM3530_ALS_ZB3_REG */
+	reg_val[5] = als.zones[0];	/* LM3530_ALS_ZB0_REG */
+	reg_val[6] = als.zones[1];	/* LM3530_ALS_ZB1_REG */
+	reg_val[7] = als.zones[2];	/* LM3530_ALS_ZB2_REG */
+	reg_val[8] = als.zones[3];	/* LM3530_ALS_ZB3_REG */
 	reg_val[9] = LM3530_DEF_ZT_0;	/* LM3530_ALS_Z0T_REG */
 	reg_val[10] = LM3530_DEF_ZT_1;	/* LM3530_ALS_Z1T_REG */
 	reg_val[11] = LM3530_DEF_ZT_2;	/* LM3530_ALS_Z2T_REG */
diff --git a/drivers/leds/leds-lm3533.c b/drivers/leds/leds-lm3533.c
new file mode 100644
index 00000000000..f56b6e7ffda
--- /dev/null
+++ b/drivers/leds/leds-lm3533.c
@@ -0,0 +1,785 @@
+/*
+ * leds-lm3533.c -- LM3533 LED driver
+ *
+ * Copyright (C) 2011-2012 Texas Instruments
+ *
+ * Author: Johan Hovold <jhovold@gmail.com>
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under  the terms of the GNU General  Public License as published by the
+ * Free Software Foundation;  either version 2 of the License, or (at your
+ * option) any later version.
+ */
+
+#include <linux/module.h>
+#include <linux/init.h>
+#include <linux/leds.h>
+#include <linux/mfd/core.h>
+#include <linux/mutex.h>
+#include <linux/platform_device.h>
+#include <linux/slab.h>
+#include <linux/workqueue.h>
+
+#include <linux/mfd/lm3533.h>
+
+
+#define LM3533_LVCTRLBANK_MIN		2
+#define LM3533_LVCTRLBANK_MAX		5
+#define LM3533_LVCTRLBANK_COUNT		4
+#define LM3533_RISEFALLTIME_MAX		7
+#define LM3533_ALS_CHANNEL_LV_MIN	1
+#define LM3533_ALS_CHANNEL_LV_MAX	2
+
+#define LM3533_REG_CTRLBANK_BCONF_BASE		0x1b
+#define LM3533_REG_PATTERN_ENABLE		0x28
+#define LM3533_REG_PATTERN_LOW_TIME_BASE	0x71
+#define LM3533_REG_PATTERN_HIGH_TIME_BASE	0x72
+#define LM3533_REG_PATTERN_RISETIME_BASE	0x74
+#define LM3533_REG_PATTERN_FALLTIME_BASE	0x75
+
+#define LM3533_REG_PATTERN_STEP			0x10
+
+#define LM3533_REG_CTRLBANK_BCONF_MAPPING_MASK		0x04
+#define LM3533_REG_CTRLBANK_BCONF_ALS_EN_MASK		0x02
+#define LM3533_REG_CTRLBANK_BCONF_ALS_CHANNEL_MASK	0x01
+
+#define LM3533_LED_FLAG_PATTERN_ENABLE		1
+
+
+struct lm3533_led {
+	struct lm3533 *lm3533;
+	struct lm3533_ctrlbank cb;
+	struct led_classdev cdev;
+	int id;
+
+	struct mutex mutex;
+	unsigned long flags;
+
+	struct work_struct work;
+	u8 new_brightness;
+};
+
+
+static inline struct lm3533_led *to_lm3533_led(struct led_classdev *cdev)
+{
+	return container_of(cdev, struct lm3533_led, cdev);
+}
+
+static inline int lm3533_led_get_ctrlbank_id(struct lm3533_led *led)
+{
+	return led->id + 2;
+}
+
+static inline u8 lm3533_led_get_lv_reg(struct lm3533_led *led, u8 base)
+{
+	return base + led->id;
+}
+
+static inline u8 lm3533_led_get_pattern(struct lm3533_led *led)
+{
+	return led->id;
+}
+
+static inline u8 lm3533_led_get_pattern_reg(struct lm3533_led *led,
+								u8 base)
+{
+	return base + lm3533_led_get_pattern(led) * LM3533_REG_PATTERN_STEP;
+}
+
+static int lm3533_led_pattern_enable(struct lm3533_led *led, int enable)
+{
+	u8 mask;
+	u8 val;
+	int pattern;
+	int state;
+	int ret = 0;
+
+	dev_dbg(led->cdev.dev, "%s - %d\n", __func__, enable);
+
+	mutex_lock(&led->mutex);
+
+	state = test_bit(LM3533_LED_FLAG_PATTERN_ENABLE, &led->flags);
+	if ((enable && state) || (!enable && !state))
+		goto out;
+
+	pattern = lm3533_led_get_pattern(led);
+	mask = 1 << (2 * pattern);
+
+	if (enable)
+		val = mask;
+	else
+		val = 0;
+
+	ret = lm3533_update(led->lm3533, LM3533_REG_PATTERN_ENABLE, val, mask);
+	if (ret) {
+		dev_err(led->cdev.dev, "failed to enable pattern %d (%d)\n",
+							pattern, enable);
+		goto out;
+	}
+
+	__change_bit(LM3533_LED_FLAG_PATTERN_ENABLE, &led->flags);
+out:
+	mutex_unlock(&led->mutex);
+
+	return ret;
+}
+
+static void lm3533_led_work(struct work_struct *work)
+{
+	struct lm3533_led *led = container_of(work, struct lm3533_led, work);
+
+	dev_dbg(led->cdev.dev, "%s - %u\n", __func__, led->new_brightness);
+
+	if (led->new_brightness == 0)
+		lm3533_led_pattern_enable(led, 0);	/* disable blink */
+
+	lm3533_ctrlbank_set_brightness(&led->cb, led->new_brightness);
+}
+
+static void lm3533_led_set(struct led_classdev *cdev,
+						enum led_brightness value)
+{
+	struct lm3533_led *led = to_lm3533_led(cdev);
+
+	dev_dbg(led->cdev.dev, "%s - %d\n", __func__, value);
+
+	led->new_brightness = value;
+	schedule_work(&led->work);
+}
+
+static enum led_brightness lm3533_led_get(struct led_classdev *cdev)
+{
+	struct lm3533_led *led = to_lm3533_led(cdev);
+	u8 val;
+	int ret;
+
+	ret = lm3533_ctrlbank_get_brightness(&led->cb, &val);
+	if (ret)
+		return ret;
+
+	dev_dbg(led->cdev.dev, "%s - %u\n", __func__, val);
+
+	return val;
+}
+
+/* Pattern generator defines (delays in us). */
+#define LM3533_LED_DELAY1_VMIN	0x00
+#define LM3533_LED_DELAY2_VMIN	0x3d
+#define LM3533_LED_DELAY3_VMIN	0x80
+
+#define LM3533_LED_DELAY1_VMAX	(LM3533_LED_DELAY2_VMIN - 1)
+#define LM3533_LED_DELAY2_VMAX	(LM3533_LED_DELAY3_VMIN - 1)
+#define LM3533_LED_DELAY3_VMAX	0xff
+
+#define LM3533_LED_DELAY1_TMIN	16384U
+#define LM3533_LED_DELAY2_TMIN	1130496U
+#define LM3533_LED_DELAY3_TMIN	10305536U
+
+#define LM3533_LED_DELAY1_TMAX	999424U
+#define LM3533_LED_DELAY2_TMAX	9781248U
+#define LM3533_LED_DELAY3_TMAX	76890112U
+
+/* t_step = (t_max - t_min) / (v_max - v_min) */
+#define LM3533_LED_DELAY1_TSTEP	16384
+#define LM3533_LED_DELAY2_TSTEP	131072
+#define LM3533_LED_DELAY3_TSTEP	524288
+
+/* Delay limits for hardware accelerated blinking (in ms). */
+#define LM3533_LED_DELAY_ON_MAX \
+	((LM3533_LED_DELAY2_TMAX + LM3533_LED_DELAY2_TSTEP / 2) / 1000)
+#define LM3533_LED_DELAY_OFF_MAX \
+	((LM3533_LED_DELAY3_TMAX + LM3533_LED_DELAY3_TSTEP / 2) / 1000)
+
+/*
+ * Returns linear map of *t from [t_min,t_max] to [v_min,v_max] with a step
+ * size of t_step, where
+ *
+ *	t_step = (t_max - t_min) / (v_max - v_min)
+ *
+ * and updates *t to reflect the mapped value.
+ */
+static u8 time_to_val(unsigned *t, unsigned t_min, unsigned t_step,
+							u8 v_min, u8 v_max)
+{
+	unsigned val;
+
+	val = (*t + t_step / 2 - t_min) / t_step + v_min;
+
+	*t = t_step * (val - v_min) + t_min;
+
+	return (u8)val;
+}
+
+/*
+ * Returns time code corresponding to *delay (in ms) and updates *delay to
+ * reflect actual hardware delay.
+ *
+ * Hardware supports 256 discrete delay times, divided into three groups with
+ * the following ranges and step-sizes:
+ *
+ *	[   16,   999]	[0x00, 0x3e]	step  16 ms
+ *	[ 1130,  9781]	[0x3d, 0x7f]	step 131 ms
+ *	[10306, 76890]	[0x80, 0xff]	step 524 ms
+ *
+ * Note that delay group 3 is only available for delay_off.
+ */
+static u8 lm3533_led_get_hw_delay(unsigned *delay)
+{
+	unsigned t;
+	u8 val;
+
+	t = *delay * 1000;
+
+	if (t >= (LM3533_LED_DELAY2_TMAX + LM3533_LED_DELAY3_TMIN) / 2) {
+		t = clamp(t, LM3533_LED_DELAY3_TMIN, LM3533_LED_DELAY3_TMAX);
+		val = time_to_val(&t,	LM3533_LED_DELAY3_TMIN,
+					LM3533_LED_DELAY3_TSTEP,
+					LM3533_LED_DELAY3_VMIN,
+					LM3533_LED_DELAY3_VMAX);
+	} else if (t >= (LM3533_LED_DELAY1_TMAX + LM3533_LED_DELAY2_TMIN) / 2) {
+		t = clamp(t, LM3533_LED_DELAY2_TMIN, LM3533_LED_DELAY2_TMAX);
+		val = time_to_val(&t,	LM3533_LED_DELAY2_TMIN,
+					LM3533_LED_DELAY2_TSTEP,
+					LM3533_LED_DELAY2_VMIN,
+					LM3533_LED_DELAY2_VMAX);
+	} else {
+		t = clamp(t, LM3533_LED_DELAY1_TMIN, LM3533_LED_DELAY1_TMAX);
+		val = time_to_val(&t,	LM3533_LED_DELAY1_TMIN,
+					LM3533_LED_DELAY1_TSTEP,
+					LM3533_LED_DELAY1_VMIN,
+					LM3533_LED_DELAY1_VMAX);
+	}
+
+	*delay = (t + 500) / 1000;
+
+	return val;
+}
+
+/*
+ * Set delay register base to *delay (in ms) and update *delay to reflect
+ * actual hardware delay used.
+ */
+static u8 lm3533_led_delay_set(struct lm3533_led *led, u8 base,
+							unsigned long *delay)
+{
+	unsigned t;
+	u8 val;
+	u8 reg;
+	int ret;
+
+	t = (unsigned)*delay;
+
+	/* Delay group 3 is only available for low time (delay off). */
+	if (base != LM3533_REG_PATTERN_LOW_TIME_BASE)
+		t = min(t, LM3533_LED_DELAY2_TMAX / 1000);
+
+	val = lm3533_led_get_hw_delay(&t);
+
+	dev_dbg(led->cdev.dev, "%s - %lu: %u (0x%02x)\n", __func__,
+							*delay, t, val);
+	reg = lm3533_led_get_pattern_reg(led, base);
+	ret = lm3533_write(led->lm3533, reg, val);
+	if (ret)
+		dev_err(led->cdev.dev, "failed to set delay (%02x)\n", reg);
+
+	*delay = t;
+
+	return ret;
+}
+
+static int lm3533_led_delay_on_set(struct lm3533_led *led, unsigned long *t)
+{
+	return lm3533_led_delay_set(led, LM3533_REG_PATTERN_HIGH_TIME_BASE, t);
+}
+
+static int lm3533_led_delay_off_set(struct lm3533_led *led, unsigned long *t)
+{
+	return lm3533_led_delay_set(led, LM3533_REG_PATTERN_LOW_TIME_BASE, t);
+}
+
+static int lm3533_led_blink_set(struct led_classdev *cdev,
+				unsigned long *delay_on,
+				unsigned long *delay_off)
+{
+	struct lm3533_led *led = to_lm3533_led(cdev);
+	int ret;
+
+	dev_dbg(led->cdev.dev, "%s - on = %lu, off = %lu\n", __func__,
+							*delay_on, *delay_off);
+
+	if (*delay_on > LM3533_LED_DELAY_ON_MAX ||
+					*delay_off > LM3533_LED_DELAY_OFF_MAX)
+		return -EINVAL;
+
+	if (*delay_on == 0 && *delay_off == 0) {
+		*delay_on = 500;
+		*delay_off = 500;
+	}
+
+	ret = lm3533_led_delay_on_set(led, delay_on);
+	if (ret)
+		return ret;
+
+	ret = lm3533_led_delay_off_set(led, delay_off);
+	if (ret)
+		return ret;
+
+	return lm3533_led_pattern_enable(led, 1);
+}
+
+static ssize_t show_id(struct device *dev,
+				struct device_attribute *attr, char *buf)
+{
+	struct led_classdev *led_cdev = dev_get_drvdata(dev);
+	struct lm3533_led *led = to_lm3533_led(led_cdev);
+
+	return scnprintf(buf, PAGE_SIZE, "%d\n", led->id);
+}
+
+/*
+ * Pattern generator rise/fall times:
+ *
+ *   0 - 2048 us (default)
+ *   1 - 262 ms
+ *   2 - 524 ms
+ *   3 - 1.049 s
+ *   4 - 2.097 s
+ *   5 - 4.194 s
+ *   6 - 8.389 s
+ *   7 - 16.78 s
+ */
+static ssize_t show_risefalltime(struct device *dev,
+					struct device_attribute *attr,
+					char *buf, u8 base)
+{
+	struct led_classdev *led_cdev = dev_get_drvdata(dev);
+	struct lm3533_led *led = to_lm3533_led(led_cdev);
+	ssize_t ret;
+	u8 reg;
+	u8 val;
+
+	reg = lm3533_led_get_pattern_reg(led, base);
+	ret = lm3533_read(led->lm3533, reg, &val);
+	if (ret)
+		return ret;
+
+	return scnprintf(buf, PAGE_SIZE, "%x\n", val);
+}
+
+static ssize_t show_risetime(struct device *dev,
+				struct device_attribute *attr, char *buf)
+{
+	return show_risefalltime(dev, attr, buf,
+					LM3533_REG_PATTERN_RISETIME_BASE);
+}
+
+static ssize_t show_falltime(struct device *dev,
+				struct device_attribute *attr, char *buf)
+{
+	return show_risefalltime(dev, attr, buf,
+					LM3533_REG_PATTERN_FALLTIME_BASE);
+}
+
+static ssize_t store_risefalltime(struct device *dev,
+					struct device_attribute *attr,
+					const char *buf, size_t len, u8 base)
+{
+	struct led_classdev *led_cdev = dev_get_drvdata(dev);
+	struct lm3533_led *led = to_lm3533_led(led_cdev);
+	u8 val;
+	u8 reg;
+	int ret;
+
+	if (kstrtou8(buf, 0, &val) || val > LM3533_RISEFALLTIME_MAX)
+		return -EINVAL;
+
+	reg = lm3533_led_get_pattern_reg(led, base);
+	ret = lm3533_write(led->lm3533, reg, val);
+	if (ret)
+		return ret;
+
+	return len;
+}
+
+static ssize_t store_risetime(struct device *dev,
+					struct device_attribute *attr,
+					const char *buf, size_t len)
+{
+	return store_risefalltime(dev, attr, buf, len,
+					LM3533_REG_PATTERN_RISETIME_BASE);
+}
+
+static ssize_t store_falltime(struct device *dev,
+					struct device_attribute *attr,
+					const char *buf, size_t len)
+{
+	return store_risefalltime(dev, attr, buf, len,
+					LM3533_REG_PATTERN_FALLTIME_BASE);
+}
+
+static ssize_t show_als_channel(struct device *dev,
+				struct device_attribute *attr, char *buf)
+{
+	struct led_classdev *led_cdev = dev_get_drvdata(dev);
+	struct lm3533_led *led = to_lm3533_led(led_cdev);
+	unsigned channel;
+	u8 reg;
+	u8 val;
+	int ret;
+
+	reg = lm3533_led_get_lv_reg(led, LM3533_REG_CTRLBANK_BCONF_BASE);
+	ret = lm3533_read(led->lm3533, reg, &val);
+	if (ret)
+		return ret;
+
+	channel = (val & LM3533_REG_CTRLBANK_BCONF_ALS_CHANNEL_MASK) + 1;
+
+	return scnprintf(buf, PAGE_SIZE, "%u\n", channel);
+}
+
+static ssize_t store_als_channel(struct device *dev,
+					struct device_attribute *attr,
+					const char *buf, size_t len)
+{
+	struct led_classdev *led_cdev = dev_get_drvdata(dev);
+	struct lm3533_led *led = to_lm3533_led(led_cdev);
+	unsigned channel;
+	u8 reg;
+	u8 val;
+	u8 mask;
+	int ret;
+
+	if (kstrtouint(buf, 0, &channel))
+		return -EINVAL;
+
+	if (channel < LM3533_ALS_CHANNEL_LV_MIN ||
+					channel > LM3533_ALS_CHANNEL_LV_MAX)
+		return -EINVAL;
+
+	reg = lm3533_led_get_lv_reg(led, LM3533_REG_CTRLBANK_BCONF_BASE);
+	mask = LM3533_REG_CTRLBANK_BCONF_ALS_CHANNEL_MASK;
+	val = channel - 1;
+
+	ret = lm3533_update(led->lm3533, reg, val, mask);
+	if (ret)
+		return ret;
+
+	return len;
+}
+
+static ssize_t show_als_en(struct device *dev,
+				struct device_attribute *attr, char *buf)
+{
+	struct led_classdev *led_cdev = dev_get_drvdata(dev);
+	struct lm3533_led *led = to_lm3533_led(led_cdev);
+	bool enable;
+	u8 reg;
+	u8 val;
+	int ret;
+
+	reg = lm3533_led_get_lv_reg(led, LM3533_REG_CTRLBANK_BCONF_BASE);
+	ret = lm3533_read(led->lm3533, reg, &val);
+	if (ret)
+		return ret;
+
+	enable = val & LM3533_REG_CTRLBANK_BCONF_ALS_EN_MASK;
+
+	return scnprintf(buf, PAGE_SIZE, "%d\n", enable);
+}
+
+static ssize_t store_als_en(struct device *dev,
+					struct device_attribute *attr,
+					const char *buf, size_t len)
+{
+	struct led_classdev *led_cdev = dev_get_drvdata(dev);
+	struct lm3533_led *led = to_lm3533_led(led_cdev);
+	unsigned enable;
+	u8 reg;
+	u8 mask;
+	u8 val;
+	int ret;
+
+	if (kstrtouint(buf, 0, &enable))
+		return -EINVAL;
+
+	reg = lm3533_led_get_lv_reg(led, LM3533_REG_CTRLBANK_BCONF_BASE);
+	mask = LM3533_REG_CTRLBANK_BCONF_ALS_EN_MASK;
+
+	if (enable)
+		val = mask;
+	else
+		val = 0;
+
+	ret = lm3533_update(led->lm3533, reg, val, mask);
+	if (ret)
+		return ret;
+
+	return len;
+}
+
+static ssize_t show_linear(struct device *dev,
+				struct device_attribute *attr, char *buf)
+{
+	struct led_classdev *led_cdev = dev_get_drvdata(dev);
+	struct lm3533_led *led = to_lm3533_led(led_cdev);
+	u8 reg;
+	u8 val;
+	int linear;
+	int ret;
+
+	reg = lm3533_led_get_lv_reg(led, LM3533_REG_CTRLBANK_BCONF_BASE);
+	ret = lm3533_read(led->lm3533, reg, &val);
+	if (ret)
+		return ret;
+
+	if (val & LM3533_REG_CTRLBANK_BCONF_MAPPING_MASK)
+		linear = 1;
+	else
+		linear = 0;
+
+	return scnprintf(buf, PAGE_SIZE, "%x\n", linear);
+}
+
+static ssize_t store_linear(struct device *dev,
+					struct device_attribute *attr,
+					const char *buf, size_t len)
+{
+	struct led_classdev *led_cdev = dev_get_drvdata(dev);
+	struct lm3533_led *led = to_lm3533_led(led_cdev);
+	unsigned long linear;
+	u8 reg;
+	u8 mask;
+	u8 val;
+	int ret;
+
+	if (kstrtoul(buf, 0, &linear))
+		return -EINVAL;
+
+	reg = lm3533_led_get_lv_reg(led, LM3533_REG_CTRLBANK_BCONF_BASE);
+	mask = LM3533_REG_CTRLBANK_BCONF_MAPPING_MASK;
+
+	if (linear)
+		val = mask;
+	else
+		val = 0;
+
+	ret = lm3533_update(led->lm3533, reg, val, mask);
+	if (ret)
+		return ret;
+
+	return len;
+}
+
+static ssize_t show_pwm(struct device *dev,
+					struct device_attribute *attr,
+					char *buf)
+{
+	struct led_classdev *led_cdev = dev_get_drvdata(dev);
+	struct lm3533_led *led = to_lm3533_led(led_cdev);
+	u8 val;
+	int ret;
+
+	ret = lm3533_ctrlbank_get_pwm(&led->cb, &val);
+	if (ret)
+		return ret;
+
+	return scnprintf(buf, PAGE_SIZE, "%u\n", val);
+}
+
+static ssize_t store_pwm(struct device *dev,
+					struct device_attribute *attr,
+					const char *buf, size_t len)
+{
+	struct led_classdev *led_cdev = dev_get_drvdata(dev);
+	struct lm3533_led *led = to_lm3533_led(led_cdev);
+	u8 val;
+	int ret;
+
+	if (kstrtou8(buf, 0, &val))
+		return -EINVAL;
+
+	ret = lm3533_ctrlbank_set_pwm(&led->cb, val);
+	if (ret)
+		return ret;
+
+	return len;
+}
+
+static LM3533_ATTR_RW(als_channel);
+static LM3533_ATTR_RW(als_en);
+static LM3533_ATTR_RW(falltime);
+static LM3533_ATTR_RO(id);
+static LM3533_ATTR_RW(linear);
+static LM3533_ATTR_RW(pwm);
+static LM3533_ATTR_RW(risetime);
+
+static struct attribute *lm3533_led_attributes[] = {
+	&dev_attr_als_channel.attr,
+	&dev_attr_als_en.attr,
+	&dev_attr_falltime.attr,
+	&dev_attr_id.attr,
+	&dev_attr_linear.attr,
+	&dev_attr_pwm.attr,
+	&dev_attr_risetime.attr,
+	NULL,
+};
+
+static umode_t lm3533_led_attr_is_visible(struct kobject *kobj,
+					     struct attribute *attr, int n)
+{
+	struct device *dev = container_of(kobj, struct device, kobj);
+	struct led_classdev *led_cdev = dev_get_drvdata(dev);
+	struct lm3533_led *led = to_lm3533_led(led_cdev);
+	umode_t mode = attr->mode;
+
+	if (attr == &dev_attr_als_channel.attr ||
+					attr == &dev_attr_als_en.attr) {
+		if (!led->lm3533->have_als)
+			mode = 0;
+	}
+
+	return mode;
+};
+
+static struct attribute_group lm3533_led_attribute_group = {
+	.is_visible	= lm3533_led_attr_is_visible,
+	.attrs		= lm3533_led_attributes
+};
+
+static int __devinit lm3533_led_setup(struct lm3533_led *led,
+					struct lm3533_led_platform_data *pdata)
+{
+	int ret;
+
+	ret = lm3533_ctrlbank_set_max_current(&led->cb, pdata->max_current);
+	if (ret)
+		return ret;
+
+	return lm3533_ctrlbank_set_pwm(&led->cb, pdata->pwm);
+}
+
+static int __devinit lm3533_led_probe(struct platform_device *pdev)
+{
+	struct lm3533 *lm3533;
+	struct lm3533_led_platform_data *pdata;
+	struct lm3533_led *led;
+	int ret;
+
+	dev_dbg(&pdev->dev, "%s\n", __func__);
+
+	lm3533 = dev_get_drvdata(pdev->dev.parent);
+	if (!lm3533)
+		return -EINVAL;
+
+	pdata = pdev->dev.platform_data;
+	if (!pdata) {
+		dev_err(&pdev->dev, "no platform data\n");
+		return -EINVAL;
+	}
+
+	if (pdev->id < 0 || pdev->id >= LM3533_LVCTRLBANK_COUNT) {
+		dev_err(&pdev->dev, "illegal LED id %d\n", pdev->id);
+		return -EINVAL;
+	}
+
+	led = devm_kzalloc(&pdev->dev, sizeof(*led), GFP_KERNEL);
+	if (!led)
+		return -ENOMEM;
+
+	led->lm3533 = lm3533;
+	led->cdev.name = pdata->name;
+	led->cdev.default_trigger = pdata->default_trigger;
+	led->cdev.brightness_set = lm3533_led_set;
+	led->cdev.brightness_get = lm3533_led_get;
+	led->cdev.blink_set = lm3533_led_blink_set;
+	led->cdev.brightness = LED_OFF;
+	led->id = pdev->id;
+
+	mutex_init(&led->mutex);
+	INIT_WORK(&led->work, lm3533_led_work);
+
+	/* The class framework makes a callback to get brightness during
+	 * registration so use parent device (for error reporting) until
+	 * registered.
+	 */
+	led->cb.lm3533 = lm3533;
+	led->cb.id = lm3533_led_get_ctrlbank_id(led);
+	led->cb.dev = lm3533->dev;
+
+	platform_set_drvdata(pdev, led);
+
+	ret = led_classdev_register(pdev->dev.parent, &led->cdev);
+	if (ret) {
+		dev_err(&pdev->dev, "failed to register LED %d\n", pdev->id);
+		return ret;
+	}
+
+	led->cb.dev = led->cdev.dev;
+
+	ret = sysfs_create_group(&led->cdev.dev->kobj,
+						&lm3533_led_attribute_group);
+	if (ret < 0) {
+		dev_err(&pdev->dev, "failed to create sysfs attributes\n");
+		goto err_unregister;
+	}
+
+	ret = lm3533_led_setup(led, pdata);
+	if (ret)
+		goto err_sysfs_remove;
+
+	ret = lm3533_ctrlbank_enable(&led->cb);
+	if (ret)
+		goto err_sysfs_remove;
+
+	return 0;
+
+err_sysfs_remove:
+	sysfs_remove_group(&led->cdev.dev->kobj, &lm3533_led_attribute_group);
+err_unregister:
+	led_classdev_unregister(&led->cdev);
+	flush_work_sync(&led->work);
+
+	return ret;
+}
+
+static int __devexit lm3533_led_remove(struct platform_device *pdev)
+{
+	struct lm3533_led *led = platform_get_drvdata(pdev);
+
+	dev_dbg(&pdev->dev, "%s\n", __func__);
+
+	lm3533_ctrlbank_disable(&led->cb);
+	sysfs_remove_group(&led->cdev.dev->kobj, &lm3533_led_attribute_group);
+	led_classdev_unregister(&led->cdev);
+	flush_work_sync(&led->work);
+
+	return 0;
+}
+
+static void lm3533_led_shutdown(struct platform_device *pdev)
+{
+
+	struct lm3533_led *led = platform_get_drvdata(pdev);
+
+	dev_dbg(&pdev->dev, "%s\n", __func__);
+
+	lm3533_ctrlbank_disable(&led->cb);
+	lm3533_led_set(&led->cdev, LED_OFF);		/* disable blink */
+	flush_work_sync(&led->work);
+}
+
+static struct platform_driver lm3533_led_driver = {
+	.driver = {
+		.name = "lm3533-leds",
+		.owner = THIS_MODULE,
+	},
+	.probe		= lm3533_led_probe,
+	.remove		= __devexit_p(lm3533_led_remove),
+	.shutdown	= lm3533_led_shutdown,
+};
+module_platform_driver(lm3533_led_driver);
+
+MODULE_AUTHOR("Johan Hovold <jhovold@gmail.com>");
+MODULE_DESCRIPTION("LM3533 LED driver");
+MODULE_LICENSE("GPL");
+MODULE_ALIAS("platform:lm3533-leds");
diff --git a/drivers/leds/leds-lp5521.c b/drivers/leds/leds-lp5521.c
index 410a723b869..23815624f35 100644
--- a/drivers/leds/leds-lp5521.c
+++ b/drivers/leds/leds-lp5521.c
@@ -193,9 +193,14 @@ static int lp5521_load_program(struct lp5521_engine *eng, const u8 *pattern)
 
 	/* move current engine to direct mode and remember the state */
 	ret = lp5521_set_engine_mode(eng, LP5521_CMD_DIRECT);
+	if (ret)
+		return ret;
+
 	/* Mode change requires min 500 us delay. 1 - 2 ms  with margin */
 	usleep_range(1000, 2000);
-	ret |= lp5521_read(client, LP5521_REG_OP_MODE, &mode);
+	ret = lp5521_read(client, LP5521_REG_OP_MODE, &mode);
+	if (ret)
+		return ret;
 
 	/* For loading, all the engines to load mode */
 	lp5521_write(client, LP5521_REG_OP_MODE, LP5521_CMD_DIRECT);
@@ -211,8 +216,7 @@ static int lp5521_load_program(struct lp5521_engine *eng, const u8 *pattern)
 				LP5521_PROG_MEM_SIZE,
 				pattern);
 
-	ret |= lp5521_write(client, LP5521_REG_OP_MODE, mode);
-	return ret;
+	return lp5521_write(client, LP5521_REG_OP_MODE, mode);
 }
 
 static int lp5521_set_led_current(struct lp5521_chip *chip, int led, u8 curr)
@@ -785,7 +789,7 @@ static int __devinit lp5521_probe(struct i2c_client *client,
 	 * LP5521_REG_ENABLE register will not have any effect - strange!
 	 */
 	ret = lp5521_read(client, LP5521_REG_R_CURRENT, &buf);
-	if (buf != LP5521_REG_R_CURR_DEFAULT) {
+	if (ret || buf != LP5521_REG_R_CURR_DEFAULT) {
 		dev_err(&client->dev, "error in resetting chip\n");
 		goto fail2;
 	}
diff --git a/drivers/leds/leds-mc13783.c b/drivers/leds/leds-mc13783.c
index 8bc49154155..4cc6a2e3df3 100644
--- a/drivers/leds/leds-mc13783.c
+++ b/drivers/leds/leds-mc13783.c
@@ -280,7 +280,7 @@ static int __devinit mc13783_led_probe(struct platform_device *pdev)
 		return -EINVAL;
 	}
 
-	led = kzalloc(sizeof(*led) * pdata->num_leds, GFP_KERNEL);
+	led = kcalloc(pdata->num_leds, sizeof(*led), GFP_KERNEL);
 	if (led == NULL) {
 		dev_err(&pdev->dev, "failed to alloc memory\n");
 		return -ENOMEM;
diff --git a/drivers/leds/leds-netxbig.c b/drivers/leds/leds-netxbig.c
index d8433f2d53b..73973fdbd8b 100644
--- a/drivers/leds/leds-netxbig.c
+++ b/drivers/leds/leds-netxbig.c
@@ -112,7 +112,7 @@ err_free_addr:
 	return err;
 }
 
-static void __devexit gpio_ext_free(struct netxbig_gpio_ext *gpio_ext)
+static void gpio_ext_free(struct netxbig_gpio_ext *gpio_ext)
 {
 	int i;
 
@@ -294,7 +294,7 @@ static ssize_t netxbig_led_sata_show(struct device *dev,
 
 static DEVICE_ATTR(sata, 0644, netxbig_led_sata_show, netxbig_led_sata_store);
 
-static void __devexit delete_netxbig_led(struct netxbig_led_data *led_dat)
+static void delete_netxbig_led(struct netxbig_led_data *led_dat)
 {
 	if (led_dat->mode_val[NETXBIG_LED_SATA] != NETXBIG_LED_INVALID_MODE)
 		device_remove_file(led_dat->cdev.dev, &dev_attr_sata);
diff --git a/drivers/leds/leds-ns2.c b/drivers/leds/leds-ns2.c
index 2f0a14421a7..01cf89ec694 100644
--- a/drivers/leds/leds-ns2.c
+++ b/drivers/leds/leds-ns2.c
@@ -255,7 +255,7 @@ err_free_cmd:
 	return ret;
 }
 
-static void __devexit delete_ns2_led(struct ns2_led_data *led_dat)
+static void delete_ns2_led(struct ns2_led_data *led_dat)
 {
 	device_remove_file(led_dat->cdev.dev, &dev_attr_sata);
 	led_classdev_unregister(&led_dat->cdev);
diff --git a/drivers/leds/leds-pca955x.c b/drivers/leds/leds-pca955x.c
index dcc3bc3d38d..5f462dbf0db 100644
--- a/drivers/leds/leds-pca955x.c
+++ b/drivers/leds/leds-pca955x.c
@@ -101,11 +101,16 @@ static const struct i2c_device_id pca955x_id[] = {
 };
 MODULE_DEVICE_TABLE(i2c, pca955x_id);
 
-struct pca955x_led {
+struct pca955x {
+	struct mutex lock;
+	struct pca955x_led *leds;
 	struct pca955x_chipdef	*chipdef;
 	struct i2c_client	*client;
+};
+
+struct pca955x_led {
+	struct pca955x	*pca955x;
 	struct work_struct	work;
-	spinlock_t		lock;
 	enum led_brightness	brightness;
 	struct led_classdev	led_cdev;
 	int			led_num;	/* 0 .. 15 potentially */
@@ -140,7 +145,7 @@ static inline u8 pca955x_ledsel(u8 oldval, int led_num, int state)
  */
 static void pca955x_write_psc(struct i2c_client *client, int n, u8 val)
 {
-	struct pca955x_led *pca955x = i2c_get_clientdata(client);
+	struct pca955x *pca955x = i2c_get_clientdata(client);
 
 	i2c_smbus_write_byte_data(client,
 		pca95xx_num_input_regs(pca955x->chipdef->bits) + 2*n,
@@ -156,7 +161,7 @@ static void pca955x_write_psc(struct i2c_client *client, int n, u8 val)
  */
 static void pca955x_write_pwm(struct i2c_client *client, int n, u8 val)
 {
-	struct pca955x_led *pca955x = i2c_get_clientdata(client);
+	struct pca955x *pca955x = i2c_get_clientdata(client);
 
 	i2c_smbus_write_byte_data(client,
 		pca95xx_num_input_regs(pca955x->chipdef->bits) + 1 + 2*n,
@@ -169,7 +174,7 @@ static void pca955x_write_pwm(struct i2c_client *client, int n, u8 val)
  */
 static void pca955x_write_ls(struct i2c_client *client, int n, u8 val)
 {
-	struct pca955x_led *pca955x = i2c_get_clientdata(client);
+	struct pca955x *pca955x = i2c_get_clientdata(client);
 
 	i2c_smbus_write_byte_data(client,
 		pca95xx_num_input_regs(pca955x->chipdef->bits) + 4 + n,
@@ -182,7 +187,7 @@ static void pca955x_write_ls(struct i2c_client *client, int n, u8 val)
  */
 static u8 pca955x_read_ls(struct i2c_client *client, int n)
 {
-	struct pca955x_led *pca955x = i2c_get_clientdata(client);
+	struct pca955x *pca955x = i2c_get_clientdata(client);
 
 	return (u8) i2c_smbus_read_byte_data(client,
 		pca95xx_num_input_regs(pca955x->chipdef->bits) + 4 + n);
@@ -190,18 +195,23 @@ static u8 pca955x_read_ls(struct i2c_client *client, int n)
 
 static void pca955x_led_work(struct work_struct *work)
 {
-	struct pca955x_led *pca955x;
+	struct pca955x_led *pca955x_led;
+	struct pca955x *pca955x;
 	u8 ls;
 	int chip_ls;	/* which LSx to use (0-3 potentially) */
 	int ls_led;	/* which set of bits within LSx to use (0-3) */
 
-	pca955x = container_of(work, struct pca955x_led, work);
-	chip_ls = pca955x->led_num / 4;
-	ls_led = pca955x->led_num % 4;
+	pca955x_led = container_of(work, struct pca955x_led, work);
+	pca955x = pca955x_led->pca955x;
+
+	chip_ls = pca955x_led->led_num / 4;
+	ls_led = pca955x_led->led_num % 4;
+
+	mutex_lock(&pca955x->lock);
 
 	ls = pca955x_read_ls(pca955x->client, chip_ls);
 
-	switch (pca955x->brightness) {
+	switch (pca955x_led->brightness) {
 	case LED_FULL:
 		ls = pca955x_ledsel(ls, ls_led, PCA955X_LS_LED_ON);
 		break;
@@ -219,12 +229,15 @@ static void pca955x_led_work(struct work_struct *work)
 		 * OFF, HALF, or FULL.  But, this is probably better than
 		 * just turning off for all other values.
 		 */
-		pca955x_write_pwm(pca955x->client, 1, 255-pca955x->brightness);
+		pca955x_write_pwm(pca955x->client, 1,
+				255 - pca955x_led->brightness);
 		ls = pca955x_ledsel(ls, ls_led, PCA955X_LS_BLINK1);
 		break;
 	}
 
 	pca955x_write_ls(pca955x->client, chip_ls, ls);
+
+	mutex_unlock(&pca955x->lock);
 }
 
 static void pca955x_led_set(struct led_classdev *led_cdev, enum led_brightness value)
@@ -233,7 +246,6 @@ static void pca955x_led_set(struct led_classdev *led_cdev, enum led_brightness v
 
 	pca955x = container_of(led_cdev, struct pca955x_led, led_cdev);
 
-	spin_lock(&pca955x->lock);
 	pca955x->brightness = value;
 
 	/*
@@ -241,14 +253,13 @@ static void pca955x_led_set(struct led_classdev *led_cdev, enum led_brightness v
 	 * can sleep.
 	 */
 	schedule_work(&pca955x->work);
-
-	spin_unlock(&pca955x->lock);
 }
 
 static int __devinit pca955x_probe(struct i2c_client *client,
 					const struct i2c_device_id *id)
 {
-	struct pca955x_led *pca955x;
+	struct pca955x *pca955x;
+	struct pca955x_led *pca955x_led;
 	struct pca955x_chipdef *chip;
 	struct i2c_adapter *adapter;
 	struct led_platform_data *pdata;
@@ -282,39 +293,48 @@ static int __devinit pca955x_probe(struct i2c_client *client,
 		}
 	}
 
-	pca955x = kzalloc(sizeof(*pca955x) * chip->bits, GFP_KERNEL);
+	pca955x = kzalloc(sizeof(*pca955x), GFP_KERNEL);
 	if (!pca955x)
 		return -ENOMEM;
 
+	pca955x->leds = kzalloc(sizeof(*pca955x_led) * chip->bits, GFP_KERNEL);
+	if (!pca955x->leds) {
+		err = -ENOMEM;
+		goto exit_nomem;
+	}
+
 	i2c_set_clientdata(client, pca955x);
 
+	mutex_init(&pca955x->lock);
+	pca955x->client = client;
+	pca955x->chipdef = chip;
+
 	for (i = 0; i < chip->bits; i++) {
-		pca955x[i].chipdef = chip;
-		pca955x[i].client = client;
-		pca955x[i].led_num = i;
+		pca955x_led = &pca955x->leds[i];
+		pca955x_led->led_num = i;
+		pca955x_led->pca955x = pca955x;
 
 		/* Platform data can specify LED names and default triggers */
 		if (pdata) {
 			if (pdata->leds[i].name)
-				snprintf(pca955x[i].name,
-					 sizeof(pca955x[i].name), "pca955x:%s",
-					 pdata->leds[i].name);
+				snprintf(pca955x_led->name,
+					sizeof(pca955x_led->name), "pca955x:%s",
+					pdata->leds[i].name);
 			if (pdata->leds[i].default_trigger)
-				pca955x[i].led_cdev.default_trigger =
+				pca955x_led->led_cdev.default_trigger =
 					pdata->leds[i].default_trigger;
 		} else {
-			snprintf(pca955x[i].name, sizeof(pca955x[i].name),
+			snprintf(pca955x_led->name, sizeof(pca955x_led->name),
 				 "pca955x:%d", i);
 		}
 
-		spin_lock_init(&pca955x[i].lock);
-
-		pca955x[i].led_cdev.name = pca955x[i].name;
-		pca955x[i].led_cdev.brightness_set = pca955x_led_set;
+		pca955x_led->led_cdev.name = pca955x_led->name;
+		pca955x_led->led_cdev.brightness_set = pca955x_led_set;
 
-		INIT_WORK(&pca955x[i].work, pca955x_led_work);
+		INIT_WORK(&pca955x_led->work, pca955x_led_work);
 
-		err = led_classdev_register(&client->dev, &pca955x[i].led_cdev);
+		err = led_classdev_register(&client->dev,
+					&pca955x_led->led_cdev);
 		if (err < 0)
 			goto exit;
 	}
@@ -337,10 +357,12 @@ static int __devinit pca955x_probe(struct i2c_client *client,
 
 exit:
 	while (i--) {
-		led_classdev_unregister(&pca955x[i].led_cdev);
-		cancel_work_sync(&pca955x[i].work);
+		led_classdev_unregister(&pca955x->leds[i].led_cdev);
+		cancel_work_sync(&pca955x->leds[i].work);
 	}
 
+	kfree(pca955x->leds);
+exit_nomem:
 	kfree(pca955x);
 
 	return err;
@@ -348,14 +370,15 @@ exit:
 
 static int __devexit pca955x_remove(struct i2c_client *client)
 {
-	struct pca955x_led *pca955x = i2c_get_clientdata(client);
+	struct pca955x *pca955x = i2c_get_clientdata(client);
 	int i;
 
 	for (i = 0; i < pca955x->chipdef->bits; i++) {
-		led_classdev_unregister(&pca955x[i].led_cdev);
-		cancel_work_sync(&pca955x[i].work);
+		led_classdev_unregister(&pca955x->leds[i].led_cdev);
+		cancel_work_sync(&pca955x->leds[i].work);
 	}
 
+	kfree(pca955x->leds);
 	kfree(pca955x);
 
 	return 0;
diff --git a/drivers/leds/ledtrig-backlight.c b/drivers/leds/ledtrig-backlight.c
index 2b513a2ad7d..e2726867c5d 100644
--- a/drivers/leds/ledtrig-backlight.c
+++ b/drivers/leds/ledtrig-backlight.c
@@ -120,6 +120,7 @@ static void bl_trig_activate(struct led_classdev *led)
 	ret = fb_register_client(&n->notifier);
 	if (ret)
 		dev_err(led->dev, "unable to register backlight trigger\n");
+	led->activated = true;
 
 	return;
 
@@ -133,10 +134,11 @@ static void bl_trig_deactivate(struct led_classdev *led)
 	struct bl_trig_notifier *n =
 		(struct bl_trig_notifier *) led->trigger_data;
 
-	if (n) {
+	if (led->activated) {
 		device_remove_file(led->dev, &dev_attr_inverted);
 		fb_unregister_client(&n->notifier);
 		kfree(n);
+		led->activated = false;
 	}
 }
 
diff --git a/drivers/leds/ledtrig-gpio.c b/drivers/leds/ledtrig-gpio.c
index ecc4bf3f37a..f057c101b89 100644
--- a/drivers/leds/ledtrig-gpio.c
+++ b/drivers/leds/ledtrig-gpio.c
@@ -200,6 +200,7 @@ static void gpio_trig_activate(struct led_classdev *led)
 	gpio_data->led = led;
 	led->trigger_data = gpio_data;
 	INIT_WORK(&gpio_data->work, gpio_trig_work);
+	led->activated = true;
 
 	return;
 
@@ -217,7 +218,7 @@ static void gpio_trig_deactivate(struct led_classdev *led)
 {
 	struct gpio_trig_data *gpio_data = led->trigger_data;
 
-	if (gpio_data) {
+	if (led->activated) {
 		device_remove_file(led->dev, &dev_attr_gpio);
 		device_remove_file(led->dev, &dev_attr_inverted);
 		device_remove_file(led->dev, &dev_attr_desired_brightness);
@@ -225,6 +226,7 @@ static void gpio_trig_deactivate(struct led_classdev *led)
 		if (gpio_data->gpio != 0)
 			free_irq(gpio_to_irq(gpio_data->gpio), led);
 		kfree(gpio_data);
+		led->activated = false;
 	}
 }
 
diff --git a/drivers/leds/ledtrig-heartbeat.c b/drivers/leds/ledtrig-heartbeat.c
index 759c0bba4a8..41dc76db431 100644
--- a/drivers/leds/ledtrig-heartbeat.c
+++ b/drivers/leds/ledtrig-heartbeat.c
@@ -18,6 +18,7 @@
 #include <linux/timer.h>
 #include <linux/sched.h>
 #include <linux/leds.h>
+#include <linux/reboot.h>
 #include "leds.h"
 
 struct heartbeat_trig_data {
@@ -83,15 +84,17 @@ static void heartbeat_trig_activate(struct led_classdev *led_cdev)
 		    led_heartbeat_function, (unsigned long) led_cdev);
 	heartbeat_data->phase = 0;
 	led_heartbeat_function(heartbeat_data->timer.data);
+	led_cdev->activated = true;
 }
 
 static void heartbeat_trig_deactivate(struct led_classdev *led_cdev)
 {
 	struct heartbeat_trig_data *heartbeat_data = led_cdev->trigger_data;
 
-	if (heartbeat_data) {
+	if (led_cdev->activated) {
 		del_timer_sync(&heartbeat_data->timer);
 		kfree(heartbeat_data);
+		led_cdev->activated = false;
 	}
 }
 
@@ -101,13 +104,38 @@ static struct led_trigger heartbeat_led_trigger = {
 	.deactivate = heartbeat_trig_deactivate,
 };
 
+static int heartbeat_reboot_notifier(struct notifier_block *nb,
+				     unsigned long code, void *unused)
+{
+	led_trigger_unregister(&heartbeat_led_trigger);
+	return NOTIFY_DONE;
+}
+
+static struct notifier_block heartbeat_reboot_nb = {
+	.notifier_call = heartbeat_reboot_notifier,
+};
+
+static struct notifier_block heartbeat_panic_nb = {
+	.notifier_call = heartbeat_reboot_notifier,
+};
+
 static int __init heartbeat_trig_init(void)
 {
-	return led_trigger_register(&heartbeat_led_trigger);
+	int rc = led_trigger_register(&heartbeat_led_trigger);
+
+	if (!rc) {
+		atomic_notifier_chain_register(&panic_notifier_list,
+					       &heartbeat_panic_nb);
+		register_reboot_notifier(&heartbeat_reboot_nb);
+	}
+	return rc;
 }
 
 static void __exit heartbeat_trig_exit(void)
 {
+	unregister_reboot_notifier(&heartbeat_reboot_nb);
+	atomic_notifier_chain_unregister(&panic_notifier_list,
+					 &heartbeat_panic_nb);
 	led_trigger_unregister(&heartbeat_led_trigger);
 }
 
diff --git a/drivers/leds/ledtrig-timer.c b/drivers/leds/ledtrig-timer.c
index 328c64c0841..9010f7abaf2 100644
--- a/drivers/leds/ledtrig-timer.c
+++ b/drivers/leds/ledtrig-timer.c
@@ -31,21 +31,17 @@ static ssize_t led_delay_on_store(struct device *dev,
 		struct device_attribute *attr, const char *buf, size_t size)
 {
 	struct led_classdev *led_cdev = dev_get_drvdata(dev);
-	int ret = -EINVAL;
-	char *after;
-	unsigned long state = simple_strtoul(buf, &after, 10);
-	size_t count = after - buf;
-
-	if (isspace(*after))
-		count++;
-
-	if (count == size) {
-		led_blink_set(led_cdev, &state, &led_cdev->blink_delay_off);
-		led_cdev->blink_delay_on = state;
-		ret = count;
-	}
+	unsigned long state;
+	ssize_t ret = -EINVAL;
+
+	ret = kstrtoul(buf, 10, &state);
+	if (ret)
+		return ret;
 
-	return ret;
+	led_blink_set(led_cdev, &state, &led_cdev->blink_delay_off);
+	led_cdev->blink_delay_on = state;
+
+	return size;
 }
 
 static ssize_t led_delay_off_show(struct device *dev,
@@ -60,21 +56,17 @@ static ssize_t led_delay_off_store(struct device *dev,
 		struct device_attribute *attr, const char *buf, size_t size)
 {
 	struct led_classdev *led_cdev = dev_get_drvdata(dev);
-	int ret = -EINVAL;
-	char *after;
-	unsigned long state = simple_strtoul(buf, &after, 10);
-	size_t count = after - buf;
-
-	if (isspace(*after))
-		count++;
-
-	if (count == size) {
-		led_blink_set(led_cdev, &led_cdev->blink_delay_on, &state);
-		led_cdev->blink_delay_off = state;
-		ret = count;
-	}
+	unsigned long state;
+	ssize_t ret = -EINVAL;
 
-	return ret;
+	ret = kstrtoul(buf, 10, &state);
+	if (ret)
+		return ret;
+
+	led_blink_set(led_cdev, &led_cdev->blink_delay_on, &state);
+	led_cdev->blink_delay_off = state;
+
+	return size;
 }
 
 static DEVICE_ATTR(delay_on, 0644, led_delay_on_show, led_delay_on_store);
@@ -95,8 +87,7 @@ static void timer_trig_activate(struct led_classdev *led_cdev)
 
 	led_blink_set(led_cdev, &led_cdev->blink_delay_on,
 		      &led_cdev->blink_delay_off);
-
-	led_cdev->trigger_data = (void *)1;
+	led_cdev->activated = true;
 
 	return;
 
@@ -106,9 +97,10 @@ err_out_delayon:
 
 static void timer_trig_deactivate(struct led_classdev *led_cdev)
 {
-	if (led_cdev->trigger_data) {
+	if (led_cdev->activated) {
 		device_remove_file(led_cdev->dev, &dev_attr_delay_on);
 		device_remove_file(led_cdev->dev, &dev_attr_delay_off);
+		led_cdev->activated = false;
 	}
 
 	/* Stop blinking */
diff --git a/drivers/leds/ledtrig-transient.c b/drivers/leds/ledtrig-transient.c
new file mode 100644
index 00000000000..83179f435e1
--- /dev/null
+++ b/drivers/leds/ledtrig-transient.c
@@ -0,0 +1,237 @@
+/*
+ * LED Kernel Transient Trigger
+ *
+ * Copyright (C) 2012 Shuah Khan <shuahkhan@gmail.com>
+ *
+ * Based on Richard Purdie's ledtrig-timer.c and Atsushi Nemoto's
+ * ledtrig-heartbeat.c
+ * Design and use-case input from Jonas Bonn <jonas@southpole.se> and
+ * Neil Brown <neilb@suse.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.
+ *
+ */
+/*
+ * Transient trigger allows one shot timer activation. Please refer to
+ * Documentation/leds/ledtrig-transient.txt for details
+*/
+
+#include <linux/module.h>
+#include <linux/kernel.h>
+#include <linux/init.h>
+#include <linux/device.h>
+#include <linux/slab.h>
+#include <linux/timer.h>
+#include <linux/leds.h>
+#include "leds.h"
+
+struct transient_trig_data {
+	int activate;
+	int state;
+	int restore_state;
+	unsigned long duration;
+	struct timer_list timer;
+};
+
+static void transient_timer_function(unsigned long data)
+{
+	struct led_classdev *led_cdev = (struct led_classdev *) data;
+	struct transient_trig_data *transient_data = led_cdev->trigger_data;
+
+	transient_data->activate = 0;
+	led_set_brightness(led_cdev, transient_data->restore_state);
+}
+
+static ssize_t transient_activate_show(struct device *dev,
+		struct device_attribute *attr, char *buf)
+{
+	struct led_classdev *led_cdev = dev_get_drvdata(dev);
+	struct transient_trig_data *transient_data = led_cdev->trigger_data;
+
+	return sprintf(buf, "%d\n", transient_data->activate);
+}
+
+static ssize_t transient_activate_store(struct device *dev,
+		struct device_attribute *attr, const char *buf, size_t size)
+{
+	struct led_classdev *led_cdev = dev_get_drvdata(dev);
+	struct transient_trig_data *transient_data = led_cdev->trigger_data;
+	unsigned long state;
+	ssize_t ret;
+
+	ret = kstrtoul(buf, 10, &state);
+	if (ret)
+		return ret;
+
+	if (state != 1 && state != 0)
+		return -EINVAL;
+
+	/* cancel the running timer */
+	if (state == 0 && transient_data->activate == 1) {
+		del_timer(&transient_data->timer);
+		transient_data->activate = state;
+		led_set_brightness(led_cdev, transient_data->restore_state);
+		return size;
+	}
+
+	/* start timer if there is no active timer */
+	if (state == 1 && transient_data->activate == 0 &&
+	    transient_data->duration != 0) {
+		transient_data->activate = state;
+		led_set_brightness(led_cdev, transient_data->state);
+		transient_data->restore_state =
+		    (transient_data->state == LED_FULL) ? LED_OFF : LED_FULL;
+		mod_timer(&transient_data->timer,
+			  jiffies + transient_data->duration);
+	}
+
+	/* state == 0 && transient_data->activate == 0
+		timer is not active - just return */
+	/* state == 1 && transient_data->activate == 1
+		timer is already active - just return */
+
+	return size;
+}
+
+static ssize_t transient_duration_show(struct device *dev,
+		struct device_attribute *attr, char *buf)
+{
+	struct led_classdev *led_cdev = dev_get_drvdata(dev);
+	struct transient_trig_data *transient_data = led_cdev->trigger_data;
+
+	return sprintf(buf, "%lu\n", transient_data->duration);
+}
+
+static ssize_t transient_duration_store(struct device *dev,
+		struct device_attribute *attr, const char *buf, size_t size)
+{
+	struct led_classdev *led_cdev = dev_get_drvdata(dev);
+	struct transient_trig_data *transient_data = led_cdev->trigger_data;
+	unsigned long state;
+	ssize_t ret;
+
+	ret = kstrtoul(buf, 10, &state);
+	if (ret)
+		return ret;
+
+	transient_data->duration = state;
+	return size;
+}
+
+static ssize_t transient_state_show(struct device *dev,
+		struct device_attribute *attr, char *buf)
+{
+	struct led_classdev *led_cdev = dev_get_drvdata(dev);
+	struct transient_trig_data *transient_data = led_cdev->trigger_data;
+	int state;
+
+	state = (transient_data->state == LED_FULL) ? 1 : 0;
+	return sprintf(buf, "%d\n", state);
+}
+
+static ssize_t transient_state_store(struct device *dev,
+		struct device_attribute *attr, const char *buf, size_t size)
+{
+	struct led_classdev *led_cdev = dev_get_drvdata(dev);
+	struct transient_trig_data *transient_data = led_cdev->trigger_data;
+	unsigned long state;
+	ssize_t ret;
+
+	ret = kstrtoul(buf, 10, &state);
+	if (ret)
+		return ret;
+
+	if (state != 1 && state != 0)
+		return -EINVAL;
+
+	transient_data->state = (state == 1) ? LED_FULL : LED_OFF;
+	return size;
+}
+
+static DEVICE_ATTR(activate, 0644, transient_activate_show,
+		   transient_activate_store);
+static DEVICE_ATTR(duration, 0644, transient_duration_show,
+		   transient_duration_store);
+static DEVICE_ATTR(state, 0644, transient_state_show, transient_state_store);
+
+static void transient_trig_activate(struct led_classdev *led_cdev)
+{
+	int rc;
+	struct transient_trig_data *tdata;
+
+	tdata = kzalloc(sizeof(struct transient_trig_data), GFP_KERNEL);
+	if (!tdata) {
+		dev_err(led_cdev->dev,
+			"unable to allocate transient trigger\n");
+		return;
+	}
+	led_cdev->trigger_data = tdata;
+
+	rc = device_create_file(led_cdev->dev, &dev_attr_activate);
+	if (rc)
+		goto err_out;
+
+	rc = device_create_file(led_cdev->dev, &dev_attr_duration);
+	if (rc)
+		goto err_out_duration;
+
+	rc = device_create_file(led_cdev->dev, &dev_attr_state);
+	if (rc)
+		goto err_out_state;
+
+	setup_timer(&tdata->timer, transient_timer_function,
+		    (unsigned long) led_cdev);
+	led_cdev->activated = true;
+
+	return;
+
+err_out_state:
+	device_remove_file(led_cdev->dev, &dev_attr_duration);
+err_out_duration:
+	device_remove_file(led_cdev->dev, &dev_attr_activate);
+err_out:
+	dev_err(led_cdev->dev, "unable to register transient trigger\n");
+	led_cdev->trigger_data = NULL;
+	kfree(tdata);
+}
+
+static void transient_trig_deactivate(struct led_classdev *led_cdev)
+{
+	struct transient_trig_data *transient_data = led_cdev->trigger_data;
+
+	if (led_cdev->activated) {
+		del_timer_sync(&transient_data->timer);
+		led_set_brightness(led_cdev, transient_data->restore_state);
+		device_remove_file(led_cdev->dev, &dev_attr_activate);
+		device_remove_file(led_cdev->dev, &dev_attr_duration);
+		device_remove_file(led_cdev->dev, &dev_attr_state);
+		led_cdev->trigger_data = NULL;
+		led_cdev->activated = false;
+		kfree(transient_data);
+	}
+}
+
+static struct led_trigger transient_trigger = {
+	.name     = "transient",
+	.activate = transient_trig_activate,
+	.deactivate = transient_trig_deactivate,
+};
+
+static int __init transient_trig_init(void)
+{
+	return led_trigger_register(&transient_trigger);
+}
+
+static void __exit transient_trig_exit(void)
+{
+	led_trigger_unregister(&transient_trigger);
+}
+
+module_init(transient_trig_init);
+module_exit(transient_trig_exit);
+
+MODULE_AUTHOR("Shuah Khan <shuahkhan@gmail.com>");
+MODULE_DESCRIPTION("Transient LED trigger");
+MODULE_LICENSE("GPL");
diff --git a/drivers/macintosh/Kconfig b/drivers/macintosh/Kconfig
index fa51af11c6f..a555da64224 100644
--- a/drivers/macintosh/Kconfig
+++ b/drivers/macintosh/Kconfig
@@ -204,11 +204,14 @@ config THERM_ADT746X
 	  better fan behaviour by default, and some manual control.
 
 config THERM_PM72
-	tristate "Support for thermal management on PowerMac G5"
+	tristate "Support for thermal management on PowerMac G5 (AGP)"
 	depends on I2C && I2C_POWERMAC && PPC_PMAC64
+	default n
 	help
 	  This driver provides thermostat and fan control for the desktop
-	  G5 machines. 
+	  G5 machines.
+
+	  This is deprecated, use windfarm instead.
 
 config WINDFARM
 	tristate "New PowerMac thermal control infrastructure"
@@ -221,6 +224,22 @@ config WINDFARM_PM81
 	help
 	  This driver provides thermal control for the iMacG5
 
+config WINDFARM_PM72
+	tristate "Support for thermal management on PowerMac G5 (AGP)"
+	depends on WINDFARM && I2C && CPU_FREQ_PMAC64 && ADB_PMU
+	select I2C_POWERMAC
+	help
+	  This driver provides thermal control for the PowerMac G5
+	  "AGP" variants (PowerMac 7,2 and 7,3)
+
+config WINDFARM_RM31
+	tristate "Support for thermal management on Xserve G5"
+	depends on WINDFARM && I2C && CPU_FREQ_PMAC64 && ADB_PMU
+	select I2C_POWERMAC
+	help
+	  This driver provides thermal control for the Xserve G5
+	  (RackMac3,1)
+
 config WINDFARM_PM91
 	tristate "Support for thermal management on PowerMac9,1"
 	depends on WINDFARM && I2C && CPU_FREQ_PMAC64 && PMAC_SMU
diff --git a/drivers/macintosh/Makefile b/drivers/macintosh/Makefile
index 6652a6ebb6f..6753b65f8ed 100644
--- a/drivers/macintosh/Makefile
+++ b/drivers/macintosh/Makefile
@@ -29,6 +29,20 @@ obj-$(CONFIG_THERM_PM72)	+= therm_pm72.o
 obj-$(CONFIG_THERM_WINDTUNNEL)	+= therm_windtunnel.o
 obj-$(CONFIG_THERM_ADT746X)	+= therm_adt746x.o
 obj-$(CONFIG_WINDFARM)	        += windfarm_core.o
+obj-$(CONFIG_WINDFARM_PM72)     += windfarm_fcu_controls.o \
+				   windfarm_ad7417_sensor.o \
+				   windfarm_lm75_sensor.o \
+				   windfarm_max6690_sensor.o \
+				   windfarm_pid.o \
+				   windfarm_cpufreq_clamp.o \
+				   windfarm_pm72.o
+obj-$(CONFIG_WINDFARM_RM31)     += windfarm_fcu_controls.o \
+				   windfarm_ad7417_sensor.o \
+				   windfarm_lm75_sensor.o \
+				   windfarm_lm87_sensor.o \
+				   windfarm_pid.o \
+				   windfarm_cpufreq_clamp.o \
+				   windfarm_rm31.o
 obj-$(CONFIG_WINDFARM_PM81)     += windfarm_smu_controls.o \
 				   windfarm_smu_sensors.o \
 				   windfarm_lm75_sensor.o windfarm_pid.o \
diff --git a/drivers/macintosh/ams/ams-i2c.c b/drivers/macintosh/ams/ams-i2c.c
index abeecd27b48..978eda8d667 100644
--- a/drivers/macintosh/ams/ams-i2c.c
+++ b/drivers/macintosh/ams/ams-i2c.c
@@ -65,7 +65,7 @@ static int ams_i2c_probe(struct i2c_client *client,
 static int ams_i2c_remove(struct i2c_client *client);
 
 static const struct i2c_device_id ams_id[] = {
-	{ "ams", 0 },
+	{ "MAC,accelerometer_1", 0 },
 	{ }
 };
 MODULE_DEVICE_TABLE(i2c, ams_id);
diff --git a/drivers/macintosh/therm_adt746x.c b/drivers/macintosh/therm_adt746x.c
index fc71723cbc4..f433521a6f9 100644
--- a/drivers/macintosh/therm_adt746x.c
+++ b/drivers/macintosh/therm_adt746x.c
@@ -47,7 +47,7 @@ static u8 FAN_SPD_SET[2] = {0x30, 0x31};
 
 static u8 default_limits_local[3] = {70, 50, 70};    /* local, sensor1, sensor2 */
 static u8 default_limits_chip[3] = {80, 65, 80};    /* local, sensor1, sensor2 */
-static const char *sensor_location[3];
+static const char *sensor_location[3] = { "?", "?", "?" };
 
 static int limit_adjust;
 static int fan_speed = -1;
@@ -79,18 +79,16 @@ struct thermostat {
 	int			last_speed[2];
 	int			last_var[2];
 	int			pwm_inv[2];
+	struct task_struct	*thread;
+	struct platform_device	*pdev;
+	enum {
+		ADT7460,
+		ADT7467
+	}			type;
 };
 
-static enum {ADT7460, ADT7467} therm_type;
-static int therm_bus, therm_address;
-static struct platform_device * of_dev;
-static struct thermostat* thermostat;
-static struct task_struct *thread_therm = NULL;
-
 static void write_both_fan_speed(struct thermostat *th, int speed);
 static void write_fan_speed(struct thermostat *th, int speed, int fan);
-static void thermostat_create_files(void);
-static void thermostat_remove_files(void);
 
 static int
 write_reg(struct thermostat* th, int reg, u8 data)
@@ -126,66 +124,6 @@ read_reg(struct thermostat* th, int reg)
 	return data;
 }
 
-static struct i2c_driver thermostat_driver;
-
-static int
-attach_thermostat(struct i2c_adapter *adapter)
-{
-	unsigned long bus_no;
-	struct i2c_board_info info;
-	struct i2c_client *client;
-
-	if (strncmp(adapter->name, "uni-n", 5))
-		return -ENODEV;
-	bus_no = simple_strtoul(adapter->name + 6, NULL, 10);
-	if (bus_no != therm_bus)
-		return -ENODEV;
-
-	memset(&info, 0, sizeof(struct i2c_board_info));
-	strlcpy(info.type, "therm_adt746x", I2C_NAME_SIZE);
-	info.addr = therm_address;
-	client = i2c_new_device(adapter, &info);
-	if (!client)
-		return -ENODEV;
-
-	/*
-	 * Let i2c-core delete that device on driver removal.
-	 * This is safe because i2c-core holds the core_lock mutex for us.
-	 */
-	list_add_tail(&client->detected, &thermostat_driver.clients);
-	return 0;
-}
-
-static int
-remove_thermostat(struct i2c_client *client)
-{
-	struct thermostat *th = i2c_get_clientdata(client);
-	int i;
-	
-	thermostat_remove_files();
-
-	if (thread_therm != NULL) {
-		kthread_stop(thread_therm);
-	}
-
-	printk(KERN_INFO "adt746x: Putting max temperatures back from "
-			 "%d, %d, %d to %d, %d, %d\n",
-		th->limits[0], th->limits[1], th->limits[2],
-		th->initial_limits[0], th->initial_limits[1],
-		th->initial_limits[2]);
-
-	for (i = 0; i < 3; i++)
-		write_reg(th, LIMIT_REG[i], th->initial_limits[i]);
-
-	write_both_fan_speed(th, -1);
-
-	thermostat = NULL;
-
-	kfree(th);
-
-	return 0;
-}
-
 static int read_fan_speed(struct thermostat *th, u8 addr)
 {
 	u8 tmp[2];
@@ -203,7 +141,7 @@ static int read_fan_speed(struct thermostat *th, u8 addr)
 static void write_both_fan_speed(struct thermostat *th, int speed)
 {
 	write_fan_speed(th, speed, 0);
-	if (therm_type == ADT7460)
+	if (th->type == ADT7460)
 		write_fan_speed(th, speed, 1);
 }
 
@@ -216,7 +154,7 @@ static void write_fan_speed(struct thermostat *th, int speed, int fan)
 	else if (speed < -1) 
 		speed = 0;
 	
-	if (therm_type == ADT7467 && fan == 1)
+	if (th->type == ADT7467 && fan == 1)
 		return;
 	
 	if (th->last_speed[fan] != speed) {
@@ -239,7 +177,7 @@ static void write_fan_speed(struct thermostat *th, int speed, int fan)
 		write_reg(th, FAN_SPD_SET[fan], speed);
 	} else {
 		/* back to automatic */
-		if(therm_type == ADT7460) {
+		if(th->type == ADT7460) {
 			manual = read_reg(th,
 				MANUAL_MODE[fan]) & (~MANUAL_MASK);
 			manual &= ~INVERT_MASK;
@@ -293,7 +231,7 @@ static void update_fans_speed (struct thermostat *th)
 	/* we don't care about local sensor, so we start at sensor 1 */
 	for (i = 1; i < 3; i++) {
 		int started = 0;
-		int fan_number = (therm_type == ADT7460 && i == 2);
+		int fan_number = (th->type == ADT7460 && i == 2);
 		int var = th->temps[i] - th->limits[i];
 
 		if (var > -1) {
@@ -370,116 +308,22 @@ static int monitor_task(void *arg)
 
 static void set_limit(struct thermostat *th, int i)
 {
-		/* Set sensor1 limit higher to avoid powerdowns */
-		th->limits[i] = default_limits_chip[i] + limit_adjust;
-		write_reg(th, LIMIT_REG[i], th->limits[i]);
+	/* Set sensor1 limit higher to avoid powerdowns */
+	th->limits[i] = default_limits_chip[i] + limit_adjust;
+	write_reg(th, LIMIT_REG[i], th->limits[i]);
 		
-		/* set our limits to normal */
-		th->limits[i] = default_limits_local[i] + limit_adjust;
+	/* set our limits to normal */
+	th->limits[i] = default_limits_local[i] + limit_adjust;
 }
 
-static int probe_thermostat(struct i2c_client *client,
-			    const struct i2c_device_id *id)
-{
-	struct thermostat* th;
-	int rc;
-	int i;
-
-	if (thermostat)
-		return 0;
-
-	th = kzalloc(sizeof(struct thermostat), GFP_KERNEL);
-	if (!th)
-		return -ENOMEM;
-
-	i2c_set_clientdata(client, th);
-	th->clt = client;
-
-	rc = read_reg(th, CONFIG_REG);
-	if (rc < 0) {
-		dev_err(&client->dev, "Thermostat failed to read config!\n");
-		kfree(th);
-		return -ENODEV;
-	}
-
-	/* force manual control to start the fan quieter */
-	if (fan_speed == -1)
-		fan_speed = 64;
-	
-	if(therm_type == ADT7460) {
-		printk(KERN_INFO "adt746x: ADT7460 initializing\n");
-		/* The 7460 needs to be started explicitly */
-		write_reg(th, CONFIG_REG, 1);
-	} else
-		printk(KERN_INFO "adt746x: ADT7467 initializing\n");
-
-	for (i = 0; i < 3; i++) {
-		th->initial_limits[i] = read_reg(th, LIMIT_REG[i]);
-		set_limit(th, i);
-	}
-
-	printk(KERN_INFO "adt746x: Lowering max temperatures from %d, %d, %d"
-			 " to %d, %d, %d\n",
-			 th->initial_limits[0], th->initial_limits[1],
-			 th->initial_limits[2], th->limits[0], th->limits[1],
-			 th->limits[2]);
-
-	thermostat = th;
-
-	/* record invert bit status because fw can corrupt it after suspend */
-	th->pwm_inv[0] = read_reg(th, MANUAL_MODE[0]) & INVERT_MASK;
-	th->pwm_inv[1] = read_reg(th, MANUAL_MODE[1]) & INVERT_MASK;
-
-	/* be sure to really write fan speed the first time */
-	th->last_speed[0] = -2;
-	th->last_speed[1] = -2;
-	th->last_var[0] = -80;
-	th->last_var[1] = -80;
-
-	if (fan_speed != -1) {
-		/* manual mode, stop fans */
-		write_both_fan_speed(th, 0);
-	} else {
-		/* automatic mode */
-		write_both_fan_speed(th, -1);
-	}
-	
-	thread_therm = kthread_run(monitor_task, th, "kfand");
-
-	if (thread_therm == ERR_PTR(-ENOMEM)) {
-		printk(KERN_INFO "adt746x: Kthread creation failed\n");
-		thread_therm = NULL;
-		return -ENOMEM;
-	}
-
-	thermostat_create_files();
-
-	return 0;
+#define BUILD_SHOW_FUNC_INT(name, data)				\
+static ssize_t show_##name(struct device *dev, struct device_attribute *attr, char *buf)	\
+{								\
+	struct thermostat *th = dev_get_drvdata(dev);		\
+	return sprintf(buf, "%d\n", data);			\
 }
 
-static const struct i2c_device_id therm_adt746x_id[] = {
-	{ "therm_adt746x", 0 },
-	{ }
-};
-
-static struct i2c_driver thermostat_driver = {
-	.driver = {
-		.name	= "therm_adt746x",
-	},
-	.attach_adapter	= attach_thermostat,
-	.probe = probe_thermostat,
-	.remove = remove_thermostat,
-	.id_table = therm_adt746x_id,
-};
-
-/* 
- * Now, unfortunately, sysfs doesn't give us a nice void * we could
- * pass around to the attribute functions, so we don't really have
- * choice but implement a bunch of them...
- *
- * FIXME, it does now...
- */
-#define BUILD_SHOW_FUNC_INT(name, data)				\
+#define BUILD_SHOW_FUNC_INT_LITE(name, data)				\
 static ssize_t show_##name(struct device *dev, struct device_attribute *attr, char *buf)	\
 {								\
 	return sprintf(buf, "%d\n", data);			\
@@ -494,22 +338,24 @@ static ssize_t show_##name(struct device *dev, struct device_attribute *attr, ch
 #define BUILD_SHOW_FUNC_FAN(name, data)				\
 static ssize_t show_##name(struct device *dev, struct device_attribute *attr, char *buf)       \
 {								\
+	struct thermostat *th = dev_get_drvdata(dev);		\
 	return sprintf(buf, "%d (%d rpm)\n", 			\
-		thermostat->last_speed[data],			\
-		read_fan_speed(thermostat, FAN_SPEED[data])	\
+		th->last_speed[data],				\
+		read_fan_speed(th, FAN_SPEED[data])		\
 		);						\
 }
 
 #define BUILD_STORE_FUNC_DEG(name, data)			\
 static ssize_t store_##name(struct device *dev, struct device_attribute *attr, const char *buf, size_t n) \
 {								\
+	struct thermostat *th = dev_get_drvdata(dev);		\
 	int val;						\
 	int i;							\
 	val = simple_strtol(buf, NULL, 10);			\
 	printk(KERN_INFO "Adjusting limits by %d degrees\n", val);	\
 	limit_adjust = val;					\
 	for (i=0; i < 3; i++)					\
-		set_limit(thermostat, i);			\
+		set_limit(th, i);				\
 	return n;						\
 }
 
@@ -525,20 +371,21 @@ static ssize_t store_##name(struct device *dev, struct device_attribute *attr, c
 	return n;						\
 }
 
-BUILD_SHOW_FUNC_INT(sensor1_temperature,	 (read_reg(thermostat, TEMP_REG[1])))
-BUILD_SHOW_FUNC_INT(sensor2_temperature,	 (read_reg(thermostat, TEMP_REG[2])))
-BUILD_SHOW_FUNC_INT(sensor1_limit,		 thermostat->limits[1])
-BUILD_SHOW_FUNC_INT(sensor2_limit,		 thermostat->limits[2])
+BUILD_SHOW_FUNC_INT(sensor1_temperature,	 (read_reg(th, TEMP_REG[1])))
+BUILD_SHOW_FUNC_INT(sensor2_temperature,	 (read_reg(th, TEMP_REG[2])))
+BUILD_SHOW_FUNC_INT(sensor1_limit,		 th->limits[1])
+BUILD_SHOW_FUNC_INT(sensor2_limit,		 th->limits[2])
 BUILD_SHOW_FUNC_STR(sensor1_location,		 sensor_location[1])
 BUILD_SHOW_FUNC_STR(sensor2_location,		 sensor_location[2])
 
-BUILD_SHOW_FUNC_INT(specified_fan_speed, fan_speed)
+BUILD_SHOW_FUNC_INT_LITE(specified_fan_speed, fan_speed)
+BUILD_STORE_FUNC_INT(specified_fan_speed,fan_speed)
+
 BUILD_SHOW_FUNC_FAN(sensor1_fan_speed,	 0)
 BUILD_SHOW_FUNC_FAN(sensor2_fan_speed,	 1)
 
-BUILD_STORE_FUNC_INT(specified_fan_speed,fan_speed)
-BUILD_SHOW_FUNC_INT(limit_adjust,	 limit_adjust)
-BUILD_STORE_FUNC_DEG(limit_adjust,	 thermostat)
+BUILD_SHOW_FUNC_INT_LITE(limit_adjust,	 limit_adjust)
+BUILD_STORE_FUNC_DEG(limit_adjust,	 th)
 		
 static DEVICE_ATTR(sensor1_temperature,	S_IRUGO,
 		   show_sensor1_temperature,NULL);
@@ -564,53 +411,77 @@ static DEVICE_ATTR(sensor2_fan_speed,	S_IRUGO,
 static DEVICE_ATTR(limit_adjust,	S_IRUSR|S_IWUSR|S_IRGRP|S_IROTH,
 		   show_limit_adjust,	store_limit_adjust);
 
-
-static int __init
-thermostat_init(void)
+static void thermostat_create_files(struct thermostat *th)
 {
-	struct device_node* np;
-	const u32 *prop;
-	int i = 0, offset = 0;
+	struct device_node *np = th->clt->dev.of_node;
+	struct device *dev;
+	int err;
 
-	np = of_find_node_by_name(NULL, "fan");
-	if (!np)
-		return -ENODEV;
-	if (of_device_is_compatible(np, "adt7460"))
-		therm_type = ADT7460;
-	else if (of_device_is_compatible(np, "adt7467"))
-		therm_type = ADT7467;
-	else {
-		of_node_put(np);
-		return -ENODEV;
-	}
+	/* To maintain ABI compatibility with userspace, create
+	 * the old style platform driver and attach the attributes
+	 * to it here
+	 */
+	th->pdev = of_platform_device_create(np, "temperatures", NULL);
+	if (!th->pdev)
+		return;
+	dev = &th->pdev->dev;
+	dev_set_drvdata(dev, th);
+	err = device_create_file(dev, &dev_attr_sensor1_temperature);
+	err |= device_create_file(dev, &dev_attr_sensor2_temperature);
+	err |= device_create_file(dev, &dev_attr_sensor1_limit);
+	err |= device_create_file(dev, &dev_attr_sensor2_limit);
+	err |= device_create_file(dev, &dev_attr_sensor1_location);
+	err |= device_create_file(dev, &dev_attr_sensor2_location);
+	err |= device_create_file(dev, &dev_attr_limit_adjust);
+	err |= device_create_file(dev, &dev_attr_specified_fan_speed);
+	err |= device_create_file(dev, &dev_attr_sensor1_fan_speed);
+	if(th->type == ADT7460)
+		err |= device_create_file(dev, &dev_attr_sensor2_fan_speed);
+	if (err)
+		printk(KERN_WARNING
+			"Failed to create temperature attribute file(s).\n");
+}
 
-	prop = of_get_property(np, "hwsensor-params-version", NULL);
-	printk(KERN_INFO "adt746x: version %d (%ssupported)\n", *prop,
-			 (*prop == 1)?"":"un");
-	if (*prop != 1) {
-		of_node_put(np);
-		return -ENODEV;
-	}
+static void thermostat_remove_files(struct thermostat *th)
+{
+	struct device *dev;
 
-	prop = of_get_property(np, "reg", NULL);
-	if (!prop) {
-		of_node_put(np);
-		return -ENODEV;
-	}
+	if (!th->pdev)
+		return;
+	dev = &th->pdev->dev;
+	device_remove_file(dev, &dev_attr_sensor1_temperature);
+	device_remove_file(dev, &dev_attr_sensor2_temperature);
+	device_remove_file(dev, &dev_attr_sensor1_limit);
+	device_remove_file(dev, &dev_attr_sensor2_limit);
+	device_remove_file(dev, &dev_attr_sensor1_location);
+	device_remove_file(dev, &dev_attr_sensor2_location);
+	device_remove_file(dev, &dev_attr_limit_adjust);
+	device_remove_file(dev, &dev_attr_specified_fan_speed);
+	device_remove_file(dev, &dev_attr_sensor1_fan_speed);	
+	if (th->type == ADT7460)
+		device_remove_file(dev, &dev_attr_sensor2_fan_speed);
+	of_device_unregister(th->pdev);
 
-	/* look for bus either by path or using "reg" */
-	if (strstr(np->full_name, "/i2c-bus@") != NULL) {
-		const char *tmp_bus = (strstr(np->full_name, "/i2c-bus@") + 9);
-		therm_bus = tmp_bus[0]-'0';
-	} else {
-		therm_bus = ((*prop) >> 8) & 0x0f;
-	}
+}
 
-	therm_address = ((*prop) & 0xff) >> 1;
+static int probe_thermostat(struct i2c_client *client,
+			    const struct i2c_device_id *id)
+{
+	struct device_node *np = client->dev.of_node;
+	struct thermostat* th;
+	const __be32 *prop;
+	int i, rc, vers, offset = 0;
 
-	printk(KERN_INFO "adt746x: Thermostat bus: %d, address: 0x%02x, "
-			 "limit_adjust: %d, fan_speed: %d\n",
-			 therm_bus, therm_address, limit_adjust, fan_speed);
+	if (!np)
+		return -ENXIO;
+	prop = of_get_property(np, "hwsensor-params-version", NULL);
+	if (!prop)
+		return -ENXIO;
+	vers = be32_to_cpup(prop);
+	printk(KERN_INFO "adt746x: version %d (%ssupported)\n",
+	       vers, vers == 1 ? "" : "un");
+	if (vers != 1)
+		return -ENXIO;
 
 	if (of_get_property(np, "hwsensor-location", NULL)) {
 		for (i = 0; i < 3; i++) {
@@ -623,72 +494,129 @@ thermostat_init(void)
 			printk(KERN_INFO "sensor %d: %s\n", i, sensor_location[i]);
 			offset += strlen(sensor_location[i]) + 1;
 		}
-	} else {
-		sensor_location[0] = "?";
-		sensor_location[1] = "?";
-		sensor_location[2] = "?";
 	}
 
-	of_dev = of_platform_device_create(np, "temperatures", NULL);
-	of_node_put(np);
+	th = kzalloc(sizeof(struct thermostat), GFP_KERNEL);
+	if (!th)
+		return -ENOMEM;
+
+	i2c_set_clientdata(client, th);
+	th->clt = client;
+	th->type = id->driver_data;
 
-	if (of_dev == NULL) {
-		printk(KERN_ERR "Can't register temperatures device !\n");
+	rc = read_reg(th, CONFIG_REG);
+	if (rc < 0) {
+		dev_err(&client->dev, "Thermostat failed to read config!\n");
+		kfree(th);
 		return -ENODEV;
 	}
 
-#ifndef CONFIG_I2C_POWERMAC
-	request_module("i2c-powermac");
-#endif
+	/* force manual control to start the fan quieter */
+	if (fan_speed == -1)
+		fan_speed = 64;
+	
+	if (th->type == ADT7460) {
+		printk(KERN_INFO "adt746x: ADT7460 initializing\n");
+		/* The 7460 needs to be started explicitly */
+		write_reg(th, CONFIG_REG, 1);
+	} else
+		printk(KERN_INFO "adt746x: ADT7467 initializing\n");
 
-	return i2c_add_driver(&thermostat_driver);
+	for (i = 0; i < 3; i++) {
+		th->initial_limits[i] = read_reg(th, LIMIT_REG[i]);
+		set_limit(th, i);
+	}
+
+	printk(KERN_INFO "adt746x: Lowering max temperatures from %d, %d, %d"
+			 " to %d, %d, %d\n",
+			 th->initial_limits[0], th->initial_limits[1],
+			 th->initial_limits[2], th->limits[0], th->limits[1],
+			 th->limits[2]);
+
+	/* record invert bit status because fw can corrupt it after suspend */
+	th->pwm_inv[0] = read_reg(th, MANUAL_MODE[0]) & INVERT_MASK;
+	th->pwm_inv[1] = read_reg(th, MANUAL_MODE[1]) & INVERT_MASK;
+
+	/* be sure to really write fan speed the first time */
+	th->last_speed[0] = -2;
+	th->last_speed[1] = -2;
+	th->last_var[0] = -80;
+	th->last_var[1] = -80;
+
+	if (fan_speed != -1) {
+		/* manual mode, stop fans */
+		write_both_fan_speed(th, 0);
+	} else {
+		/* automatic mode */
+		write_both_fan_speed(th, -1);
+	}
+	
+	th->thread = kthread_run(monitor_task, th, "kfand");
+	if (th->thread == ERR_PTR(-ENOMEM)) {
+		printk(KERN_INFO "adt746x: Kthread creation failed\n");
+		th->thread = NULL;
+		return -ENOMEM;
+	}
+
+	thermostat_create_files(th);
+
+	return 0;
 }
 
-static void thermostat_create_files(void)
+static int remove_thermostat(struct i2c_client *client)
 {
-	int err;
+	struct thermostat *th = i2c_get_clientdata(client);
+	int i;
+	
+	thermostat_remove_files(th);
 
-	err = device_create_file(&of_dev->dev, &dev_attr_sensor1_temperature);
-	err |= device_create_file(&of_dev->dev, &dev_attr_sensor2_temperature);
-	err |= device_create_file(&of_dev->dev, &dev_attr_sensor1_limit);
-	err |= device_create_file(&of_dev->dev, &dev_attr_sensor2_limit);
-	err |= device_create_file(&of_dev->dev, &dev_attr_sensor1_location);
-	err |= device_create_file(&of_dev->dev, &dev_attr_sensor2_location);
-	err |= device_create_file(&of_dev->dev, &dev_attr_limit_adjust);
-	err |= device_create_file(&of_dev->dev, &dev_attr_specified_fan_speed);
-	err |= device_create_file(&of_dev->dev, &dev_attr_sensor1_fan_speed);
-	if(therm_type == ADT7460)
-		err |= device_create_file(&of_dev->dev, &dev_attr_sensor2_fan_speed);
-	if (err)
-		printk(KERN_WARNING
-			"Failed to create temperature attribute file(s).\n");
+	if (th->thread != NULL)
+		kthread_stop(th->thread);
+
+	printk(KERN_INFO "adt746x: Putting max temperatures back from "
+			 "%d, %d, %d to %d, %d, %d\n",
+		th->limits[0], th->limits[1], th->limits[2],
+		th->initial_limits[0], th->initial_limits[1],
+		th->initial_limits[2]);
+
+	for (i = 0; i < 3; i++)
+		write_reg(th, LIMIT_REG[i], th->initial_limits[i]);
+
+	write_both_fan_speed(th, -1);
+
+	kfree(th);
+
+	return 0;
 }
 
-static void thermostat_remove_files(void)
+static const struct i2c_device_id therm_adt746x_id[] = {
+	{ "MAC,adt7460", ADT7460 },
+	{ "MAC,adt7467", ADT7467 },
+	{ }
+};
+MODULE_DEVICE_TABLE(i2c, therm_adt746x_id);
+
+static struct i2c_driver thermostat_driver = {
+	.driver = {
+		.name	= "therm_adt746x",
+	},
+	.probe = probe_thermostat,
+	.remove = remove_thermostat,
+	.id_table = therm_adt746x_id,
+};
+
+static int __init thermostat_init(void)
 {
-	if (of_dev) {
-		device_remove_file(&of_dev->dev, &dev_attr_sensor1_temperature);
-		device_remove_file(&of_dev->dev, &dev_attr_sensor2_temperature);
-		device_remove_file(&of_dev->dev, &dev_attr_sensor1_limit);
-		device_remove_file(&of_dev->dev, &dev_attr_sensor2_limit);
-		device_remove_file(&of_dev->dev, &dev_attr_sensor1_location);
-		device_remove_file(&of_dev->dev, &dev_attr_sensor2_location);
-		device_remove_file(&of_dev->dev, &dev_attr_limit_adjust);
-		device_remove_file(&of_dev->dev, &dev_attr_specified_fan_speed);
-		device_remove_file(&of_dev->dev, &dev_attr_sensor1_fan_speed);
-
-		if(therm_type == ADT7460)
-			device_remove_file(&of_dev->dev,
-					   &dev_attr_sensor2_fan_speed);
+#ifndef CONFIG_I2C_POWERMAC
+	request_module("i2c-powermac");
+#endif
 
-	}
+	return i2c_add_driver(&thermostat_driver);
 }
 
-static void __exit
-thermostat_exit(void)
+static void __exit thermostat_exit(void)
 {
 	i2c_del_driver(&thermostat_driver);
-	of_device_unregister(of_dev);
 }
 
 module_init(thermostat_init);
diff --git a/drivers/macintosh/windfarm.h b/drivers/macintosh/windfarm.h
index 7a2482cc26a..028cdac2d33 100644
--- a/drivers/macintosh/windfarm.h
+++ b/drivers/macintosh/windfarm.h
@@ -17,7 +17,7 @@
 #include <linux/device.h>
 
 /* Display a 16.16 fixed point value */
-#define FIX32TOPRINT(f)	((f) >> 16),((((f) & 0xffff) * 1000) >> 16)
+#define FIX32TOPRINT(f)	(((s32)(f)) >> 16),(((((s32)(f)) & 0xffff) * 1000) >> 16)
 
 /*
  * Control objects
@@ -35,12 +35,13 @@ struct wf_control_ops {
 };
 
 struct wf_control {
-	struct list_head	link;
-	struct wf_control_ops	*ops;
-	char			*name;
-	int			type;
-	struct kref		ref;
-	struct device_attribute	attr;
+	struct list_head		link;
+	const struct wf_control_ops	*ops;
+	const char			*name;
+	int				type;
+	struct kref			ref;
+	struct device_attribute		attr;
+	void				*priv;
 };
 
 #define WF_CONTROL_TYPE_GENERIC		0
@@ -72,6 +73,26 @@ static inline int wf_control_set_min(struct wf_control *ct)
 	return ct->ops->set_value(ct, vmin);
 }
 
+static inline int wf_control_set(struct wf_control *ct, s32 val)
+{
+	return ct->ops->set_value(ct, val);
+}
+
+static inline int wf_control_get(struct wf_control *ct, s32 *val)
+{
+	return ct->ops->get_value(ct, val);
+}
+
+static inline s32 wf_control_get_min(struct wf_control *ct)
+{
+	return ct->ops->get_min(ct);
+}
+
+static inline s32 wf_control_get_max(struct wf_control *ct)
+{
+	return ct->ops->get_max(ct);
+}
+
 /*
  * Sensor objects
  */
@@ -85,11 +106,12 @@ struct wf_sensor_ops {
 };
 
 struct wf_sensor {
-	struct list_head	link;
-	struct wf_sensor_ops	*ops;
-	char			*name;
-	struct kref		ref;
-	struct device_attribute	attr;
+	struct list_head		link;
+	const struct wf_sensor_ops	*ops;
+	const char			*name;
+	struct kref			ref;
+	struct device_attribute		attr;
+	void				*priv;
 };
 
 /* Same lifetime rules as controls */
@@ -99,6 +121,11 @@ extern struct wf_sensor * wf_find_sensor(const char *name);
 extern int wf_get_sensor(struct wf_sensor *sr);
 extern void wf_put_sensor(struct wf_sensor *sr);
 
+static inline int wf_sensor_get(struct wf_sensor *sr, s32 *val)
+{
+	return sr->ops->get_value(sr, val);
+}
+
 /* For use by clients. Note that we are a bit racy here since
  * notifier_block doesn't have a module owner field. I may fix
  * it one day ...
diff --git a/drivers/macintosh/windfarm_ad7417_sensor.c b/drivers/macintosh/windfarm_ad7417_sensor.c
new file mode 100644
index 00000000000..ac3f243b9c5
--- /dev/null
+++ b/drivers/macintosh/windfarm_ad7417_sensor.c
@@ -0,0 +1,347 @@
+/*
+ * Windfarm PowerMac thermal control. AD7417 sensors
+ *
+ * Copyright 2012 Benjamin Herrenschmidt, IBM Corp.
+ *
+ * Released under the term of the GNU GPL v2.
+ */
+
+#include <linux/types.h>
+#include <linux/errno.h>
+#include <linux/kernel.h>
+#include <linux/delay.h>
+#include <linux/slab.h>
+#include <linux/init.h>
+#include <linux/wait.h>
+#include <linux/i2c.h>
+#include <asm/prom.h>
+#include <asm/machdep.h>
+#include <asm/io.h>
+#include <asm/sections.h>
+
+#include "windfarm.h"
+#include "windfarm_mpu.h"
+
+#define VERSION "1.0"
+
+struct wf_ad7417_priv {
+	struct kref		ref;
+	struct i2c_client	*i2c;
+	u8			config;
+	u8			cpu;
+	const struct mpu_data	*mpu;
+	struct wf_sensor	sensors[5];
+	struct mutex		lock;
+};
+
+static int wf_ad7417_temp_get(struct wf_sensor *sr, s32 *value)
+{
+	struct wf_ad7417_priv *pv = sr->priv;
+	u8 buf[2];
+	s16 raw;
+	int rc;
+
+	*value = 0;
+	mutex_lock(&pv->lock);
+
+	/* Read temp register */
+	buf[0] = 0;
+	rc = i2c_master_send(pv->i2c, buf, 1);
+	if (rc < 0)
+		goto error;
+	rc = i2c_master_recv(pv->i2c, buf, 2);
+	if (rc < 0)
+		goto error;
+
+	/* Read a a 16-bit signed value */
+	raw = be16_to_cpup((__le16 *)buf);
+
+	/* Convert 8.8-bit to 16.16 fixed point */
+	*value = ((s32)raw) << 8;
+
+	mutex_unlock(&pv->lock);
+	return 0;
+
+error:
+	mutex_unlock(&pv->lock);
+	return -1;
+}
+
+/*
+ * Scaling factors for the AD7417 ADC converters (except
+ * for the CPU diode which is obtained from the EEPROM).
+ * Those values are obtained from the property list of
+ * the darwin driver
+ */
+#define ADC_12V_CURRENT_SCALE	0x0320	/* _AD2 */
+#define ADC_CPU_VOLTAGE_SCALE	0x00a0	/* _AD3 */
+#define ADC_CPU_CURRENT_SCALE	0x1f40	/* _AD4 */
+
+static void wf_ad7417_adc_convert(struct wf_ad7417_priv *pv,
+				  int chan, s32 raw, s32 *value)
+{
+	switch(chan) {
+	case 1: /* Diode */
+		*value = (raw * (s32)pv->mpu->mdiode +
+			((s32)pv->mpu->bdiode << 12)) >> 2;
+		break;
+	case 2: /* 12v current */
+		*value = raw * ADC_12V_CURRENT_SCALE;
+		break;
+	case 3: /* core voltage */
+		*value = raw * ADC_CPU_VOLTAGE_SCALE;
+		break;
+	case 4: /* core current */
+		*value = raw * ADC_CPU_CURRENT_SCALE;
+		break;
+	}
+}
+
+static int wf_ad7417_adc_get(struct wf_sensor *sr, s32 *value)
+{
+	struct wf_ad7417_priv *pv = sr->priv;
+	int chan = sr - pv->sensors;
+	int i, rc;
+	u8 buf[2];
+	u16 raw;
+
+	*value = 0;
+	mutex_lock(&pv->lock);
+	for (i = 0; i < 10; i++) {
+		/* Set channel */
+		buf[0] = 1;
+		buf[1] = (pv->config & 0x1f) | (chan << 5);
+		rc = i2c_master_send(pv->i2c, buf, 2);
+		if (rc < 0)
+			goto error;
+
+		/* Wait for conversion */
+		msleep(1);
+
+		/* Switch to data register */
+		buf[0] = 4;
+		rc = i2c_master_send(pv->i2c, buf, 1);
+		if (rc < 0)
+			goto error;
+
+		/* Read result */
+		rc = i2c_master_recv(pv->i2c, buf, 2);
+		if (rc < 0)
+			goto error;
+
+		/* Read a a 16-bit signed value */
+		raw = be16_to_cpup((__le16 *)buf) >> 6;
+		wf_ad7417_adc_convert(pv, chan, raw, value);
+
+		dev_vdbg(&pv->i2c->dev, "ADC chan %d [%s]"
+			 " raw value: 0x%x, conv to: 0x%08x\n",
+			 chan, sr->name, raw, *value);
+
+		mutex_unlock(&pv->lock);
+		return 0;
+
+	error:
+		dev_dbg(&pv->i2c->dev,
+			  "Error reading ADC, try %d...\n", i);
+		if (i < 9)
+			msleep(10);
+	}
+	mutex_unlock(&pv->lock);
+	return -1;
+}
+
+static void wf_ad7417_release(struct kref *ref)
+{
+	struct wf_ad7417_priv *pv = container_of(ref,
+						 struct wf_ad7417_priv, ref);
+	kfree(pv);
+}
+
+static void wf_ad7417_sensor_release(struct wf_sensor *sr)
+{
+	struct wf_ad7417_priv *pv = sr->priv;
+
+	kfree(sr->name);
+	kref_put(&pv->ref, wf_ad7417_release);
+}
+
+static const struct wf_sensor_ops wf_ad7417_temp_ops = {
+	.get_value	= wf_ad7417_temp_get,
+	.release	= wf_ad7417_sensor_release,
+	.owner		= THIS_MODULE,
+};
+
+static const struct wf_sensor_ops wf_ad7417_adc_ops = {
+	.get_value	= wf_ad7417_adc_get,
+	.release	= wf_ad7417_sensor_release,
+	.owner		= THIS_MODULE,
+};
+
+static void __devinit wf_ad7417_add_sensor(struct wf_ad7417_priv *pv,
+					   int index, const char *name,
+					   const struct wf_sensor_ops *ops)
+{
+	pv->sensors[index].name = kasprintf(GFP_KERNEL, "%s-%d", name, pv->cpu);
+	pv->sensors[index].priv = pv;
+	pv->sensors[index].ops = ops;
+	if (!wf_register_sensor(&pv->sensors[index]))
+		kref_get(&pv->ref);
+}
+
+static void __devinit wf_ad7417_init_chip(struct wf_ad7417_priv *pv)
+{
+	int rc;
+	u8 buf[2];
+	u8 config = 0;
+
+	/*
+	 * Read ADC the configuration register and cache it. We
+	 * also make sure Config2 contains proper values, I've seen
+	 * cases where we got stale grabage in there, thus preventing
+	 * proper reading of conv. values
+	 */
+
+	/* Clear Config2 */
+	buf[0] = 5;
+	buf[1] = 0;
+	i2c_master_send(pv->i2c, buf, 2);
+
+	/* Read & cache Config1 */
+	buf[0] = 1;
+	rc = i2c_master_send(pv->i2c, buf, 1);
+	if (rc > 0) {
+		rc = i2c_master_recv(pv->i2c, buf, 1);
+		if (rc > 0) {
+			config = buf[0];
+
+			dev_dbg(&pv->i2c->dev, "ADC config reg: %02x\n",
+				config);
+
+			/* Disable shutdown mode */
+			config &= 0xfe;
+			buf[0] = 1;
+			buf[1] = config;
+			rc = i2c_master_send(pv->i2c, buf, 2);
+		}
+	}
+	if (rc <= 0)
+		dev_err(&pv->i2c->dev, "Error reading ADC config\n");
+
+	pv->config = config;
+}
+
+static int __devinit wf_ad7417_probe(struct i2c_client *client,
+				     const struct i2c_device_id *id)
+{
+	struct wf_ad7417_priv *pv;
+	const struct mpu_data *mpu;
+	const char *loc;
+	int cpu_nr;
+
+	loc = of_get_property(client->dev.of_node, "hwsensor-location", NULL);
+	if (!loc) {
+		dev_warn(&client->dev, "Missing hwsensor-location property!\n");
+		return -ENXIO;
+	}
+
+	/*
+	 * Identify which CPU we belong to by looking at the first entry
+	 * in the hwsensor-location list
+	 */
+	if (!strncmp(loc, "CPU A", 5))
+		cpu_nr = 0;
+	else if (!strncmp(loc, "CPU B", 5))
+		cpu_nr = 1;
+	else {
+		pr_err("wf_ad7417: Can't identify location %s\n", loc);
+		return -ENXIO;
+	}
+	mpu = wf_get_mpu(cpu_nr);
+	if (!mpu) {
+		dev_err(&client->dev, "Failed to retrieve MPU data\n");
+		return -ENXIO;
+	}
+
+	pv = kzalloc(sizeof(struct wf_ad7417_priv), GFP_KERNEL);
+	if (pv == NULL)
+		return -ENODEV;
+
+	kref_init(&pv->ref);
+	mutex_init(&pv->lock);
+	pv->i2c = client;
+	pv->cpu = cpu_nr;
+	pv->mpu = mpu;
+	dev_set_drvdata(&client->dev, pv);
+
+	/* Initialize the chip */
+	wf_ad7417_init_chip(pv);
+
+	/*
+	 * We cannot rely on Apple device-tree giving us child
+	 * node with the names of the individual sensors so we
+	 * just hard code what we know about them
+	 */
+	wf_ad7417_add_sensor(pv, 0, "cpu-amb-temp", &wf_ad7417_temp_ops);
+	wf_ad7417_add_sensor(pv, 1, "cpu-diode-temp", &wf_ad7417_adc_ops);
+	wf_ad7417_add_sensor(pv, 2, "cpu-12v-current", &wf_ad7417_adc_ops);
+	wf_ad7417_add_sensor(pv, 3, "cpu-voltage", &wf_ad7417_adc_ops);
+	wf_ad7417_add_sensor(pv, 4, "cpu-current", &wf_ad7417_adc_ops);
+
+	return 0;
+}
+
+static int __devexit wf_ad7417_remove(struct i2c_client *client)
+{
+	struct wf_ad7417_priv *pv = dev_get_drvdata(&client->dev);
+	int i;
+
+	/* Mark client detached */
+	pv->i2c = NULL;
+
+	/* Release sensor */
+	for (i = 0; i < 5; i++)
+		wf_unregister_sensor(&pv->sensors[i]);
+
+	kref_put(&pv->ref, wf_ad7417_release);
+
+	return 0;
+}
+
+static const struct i2c_device_id wf_ad7417_id[] = {
+	{ "MAC,ad7417", 0 },
+	{ }
+};
+MODULE_DEVICE_TABLE(i2c, wf_ad7417_id);
+
+static struct i2c_driver wf_ad7417_driver = {
+	.driver = {
+		.name	= "wf_ad7417",
+	},
+	.probe		= wf_ad7417_probe,
+	.remove		= wf_ad7417_remove,
+	.id_table	= wf_ad7417_id,
+};
+
+static int __devinit wf_ad7417_init(void)
+{
+	/* This is only supported on these machines */
+	if (!of_machine_is_compatible("PowerMac7,2") &&
+	    !of_machine_is_compatible("PowerMac7,3") &&
+	    !of_machine_is_compatible("RackMac3,1"))
+		return -ENODEV;
+
+	return i2c_add_driver(&wf_ad7417_driver);
+}
+
+static void __devexit wf_ad7417_exit(void)
+{
+	i2c_del_driver(&wf_ad7417_driver);
+}
+
+module_init(wf_ad7417_init);
+module_exit(wf_ad7417_exit);
+
+MODULE_AUTHOR("Benjamin Herrenschmidt <benh@kernel.crashing.org>");
+MODULE_DESCRIPTION("ad7417 sensor driver for PowerMacs");
+MODULE_LICENSE("GPL");
+
diff --git a/drivers/macintosh/windfarm_core.c b/drivers/macintosh/windfarm_core.c
index ce8897933a8..3ee198b6584 100644
--- a/drivers/macintosh/windfarm_core.c
+++ b/drivers/macintosh/windfarm_core.c
@@ -164,13 +164,27 @@ static ssize_t wf_show_control(struct device *dev,
 			       struct device_attribute *attr, char *buf)
 {
 	struct wf_control *ctrl = container_of(attr, struct wf_control, attr);
+	const char *typestr;
 	s32 val = 0;
 	int err;
 
 	err = ctrl->ops->get_value(ctrl, &val);
-	if (err < 0)
+	if (err < 0) {
+		if (err == -EFAULT)
+			return sprintf(buf, "<HW FAULT>\n");
 		return err;
-	return sprintf(buf, "%d\n", val);
+	}
+	switch(ctrl->type) {
+	case WF_CONTROL_RPM_FAN:
+		typestr = " RPM";
+		break;
+	case WF_CONTROL_PWM_FAN:
+		typestr = " %";
+		break;
+	default:
+		typestr = "";
+	}
+	return sprintf(buf, "%d%s\n", val, typestr);
 }
 
 /* This is really only for debugging... */
@@ -470,11 +484,6 @@ static int __init windfarm_core_init(void)
 {
 	DBG("wf: core loaded\n");
 
-	/* Don't register on old machines that use therm_pm72 for now */
-	if (of_machine_is_compatible("PowerMac7,2") ||
-	    of_machine_is_compatible("PowerMac7,3") ||
-	    of_machine_is_compatible("RackMac3,1"))
-		return -ENODEV;
 	platform_device_register(&wf_platform_device);
 	return 0;
 }
diff --git a/drivers/macintosh/windfarm_cpufreq_clamp.c b/drivers/macintosh/windfarm_cpufreq_clamp.c
index 1a77a7c97d0..72d1fdfe02a 100644
--- a/drivers/macintosh/windfarm_cpufreq_clamp.c
+++ b/drivers/macintosh/windfarm_cpufreq_clamp.c
@@ -75,12 +75,6 @@ static int __init wf_cpufreq_clamp_init(void)
 {
 	struct wf_control *clamp;
 
-	/* Don't register on old machines that use therm_pm72 for now */
-	if (of_machine_is_compatible("PowerMac7,2") ||
-	    of_machine_is_compatible("PowerMac7,3") ||
-	    of_machine_is_compatible("RackMac3,1"))
-		return -ENODEV;
-
 	clamp = kmalloc(sizeof(struct wf_control), GFP_KERNEL);
 	if (clamp == NULL)
 		return -ENOMEM;
diff --git a/drivers/macintosh/windfarm_fcu_controls.c b/drivers/macintosh/windfarm_fcu_controls.c
new file mode 100644
index 00000000000..b3411edb324
--- /dev/null
+++ b/drivers/macintosh/windfarm_fcu_controls.c
@@ -0,0 +1,613 @@
+/*
+ * Windfarm PowerMac thermal control. FCU fan control
+ *
+ * Copyright 2012 Benjamin Herrenschmidt, IBM Corp.
+ *
+ * Released under the term of the GNU GPL v2.
+ */
+#undef DEBUG
+
+#include <linux/types.h>
+#include <linux/errno.h>
+#include <linux/kernel.h>
+#include <linux/delay.h>
+#include <linux/slab.h>
+#include <linux/init.h>
+#include <linux/wait.h>
+#include <linux/i2c.h>
+#include <asm/prom.h>
+#include <asm/machdep.h>
+#include <asm/io.h>
+#include <asm/sections.h>
+
+#include "windfarm.h"
+#include "windfarm_mpu.h"
+
+#define VERSION "1.0"
+
+#ifdef DEBUG
+#define DBG(args...)	printk(args)
+#else
+#define DBG(args...)	do { } while(0)
+#endif
+
+/*
+ * This option is "weird" :) Basically, if you define this to 1
+ * the control loop for the RPMs fans (not PWMs) will apply the
+ * correction factor obtained from the PID to the actual RPM
+ * speed read from the FCU.
+ *
+ * If you define the below constant to 0, then it will be
+ * applied to the setpoint RPM speed, that is basically the
+ * speed we proviously "asked" for.
+ *
+ * I'm using 0 for now which is what therm_pm72 used to do and
+ * what Darwin -apparently- does based on observed behaviour.
+ */
+#define RPM_PID_USE_ACTUAL_SPEED	0
+
+/* Default min/max for pumps */
+#define CPU_PUMP_OUTPUT_MAX		3200
+#define CPU_PUMP_OUTPUT_MIN		1250
+
+#define FCU_FAN_RPM		0
+#define FCU_FAN_PWM		1
+
+struct wf_fcu_priv {
+	struct kref		ref;
+	struct i2c_client	*i2c;
+	struct mutex		lock;
+	struct list_head	fan_list;
+	int			rpm_shift;
+};
+
+struct wf_fcu_fan {
+	struct list_head	link;
+	int			id;
+	s32			min, max, target;
+	struct wf_fcu_priv	*fcu_priv;
+	struct wf_control	ctrl;
+};
+
+static void wf_fcu_release(struct kref *ref)
+{
+	struct wf_fcu_priv *pv = container_of(ref, struct wf_fcu_priv, ref);
+
+	kfree(pv);
+}
+
+static void wf_fcu_fan_release(struct wf_control *ct)
+{
+	struct wf_fcu_fan *fan = ct->priv;
+
+	kref_put(&fan->fcu_priv->ref, wf_fcu_release);
+	kfree(fan);
+}
+
+static int wf_fcu_read_reg(struct wf_fcu_priv *pv, int reg,
+			   unsigned char *buf, int nb)
+{
+	int tries, nr, nw;
+
+	mutex_lock(&pv->lock);
+
+	buf[0] = reg;
+	tries = 0;
+	for (;;) {
+		nw = i2c_master_send(pv->i2c, buf, 1);
+		if (nw > 0 || (nw < 0 && nw != -EIO) || tries >= 100)
+			break;
+		msleep(10);
+		++tries;
+	}
+	if (nw <= 0) {
+		pr_err("Failure writing address to FCU: %d", nw);
+		nr = nw;
+		goto bail;
+	}
+	tries = 0;
+	for (;;) {
+		nr = i2c_master_recv(pv->i2c, buf, nb);
+		if (nr > 0 || (nr < 0 && nr != -ENODEV) || tries >= 100)
+			break;
+		msleep(10);
+		++tries;
+	}
+	if (nr <= 0)
+		pr_err("wf_fcu: Failure reading data from FCU: %d", nw);
+ bail:
+	mutex_unlock(&pv->lock);
+	return nr;
+}
+
+static int wf_fcu_write_reg(struct wf_fcu_priv *pv, int reg,
+			    const unsigned char *ptr, int nb)
+{
+	int tries, nw;
+	unsigned char buf[16];
+
+	buf[0] = reg;
+	memcpy(buf+1, ptr, nb);
+	++nb;
+	tries = 0;
+	for (;;) {
+		nw = i2c_master_send(pv->i2c, buf, nb);
+		if (nw > 0 || (nw < 0 && nw != -EIO) || tries >= 100)
+			break;
+		msleep(10);
+		++tries;
+	}
+	if (nw < 0)
+		pr_err("wf_fcu: Failure writing to FCU: %d", nw);
+	return nw;
+}
+
+static int wf_fcu_fan_set_rpm(struct wf_control *ct, s32 value)
+{
+	struct wf_fcu_fan *fan = ct->priv;
+	struct wf_fcu_priv *pv = fan->fcu_priv;
+	int rc, shift = pv->rpm_shift;
+	unsigned char buf[2];
+
+	if (value < fan->min)
+		value = fan->min;
+	if (value > fan->max)
+		value = fan->max;
+
+	fan->target = value;
+
+	buf[0] = value >> (8 - shift);
+	buf[1] = value << shift;
+	rc = wf_fcu_write_reg(pv, 0x10 + (fan->id * 2), buf, 2);
+	if (rc < 0)
+		return -EIO;
+	return 0;
+}
+
+static int wf_fcu_fan_get_rpm(struct wf_control *ct, s32 *value)
+{
+	struct wf_fcu_fan *fan = ct->priv;
+	struct wf_fcu_priv *pv = fan->fcu_priv;
+	int rc, reg_base, shift = pv->rpm_shift;
+	unsigned char failure;
+	unsigned char active;
+	unsigned char buf[2];
+
+	rc = wf_fcu_read_reg(pv, 0xb, &failure, 1);
+	if (rc != 1)
+		return -EIO;
+	if ((failure & (1 << fan->id)) != 0)
+		return -EFAULT;
+	rc = wf_fcu_read_reg(pv, 0xd, &active, 1);
+	if (rc != 1)
+		return -EIO;
+	if ((active & (1 << fan->id)) == 0)
+		return -ENXIO;
+
+	/* Programmed value or real current speed */
+#if RPM_PID_USE_ACTUAL_SPEED
+	reg_base = 0x11;
+#else
+	reg_base = 0x10;
+#endif
+	rc = wf_fcu_read_reg(pv, reg_base + (fan->id * 2), buf, 2);
+	if (rc != 2)
+		return -EIO;
+
+	*value = (buf[0] << (8 - shift)) | buf[1] >> shift;
+
+	return 0;
+}
+
+static int wf_fcu_fan_set_pwm(struct wf_control *ct, s32 value)
+{
+	struct wf_fcu_fan *fan = ct->priv;
+	struct wf_fcu_priv *pv = fan->fcu_priv;
+	unsigned char buf[2];
+	int rc;
+
+	if (value < fan->min)
+		value = fan->min;
+	if (value > fan->max)
+		value = fan->max;
+
+	fan->target = value;
+
+	value = (value * 2559) / 1000;
+	buf[0] = value;
+	rc = wf_fcu_write_reg(pv, 0x30 + (fan->id * 2), buf, 1);
+	if (rc < 0)
+		return -EIO;
+	return 0;
+}
+
+static int wf_fcu_fan_get_pwm(struct wf_control *ct, s32 *value)
+{
+	struct wf_fcu_fan *fan = ct->priv;
+	struct wf_fcu_priv *pv = fan->fcu_priv;
+	unsigned char failure;
+	unsigned char active;
+	unsigned char buf[2];
+	int rc;
+
+	rc = wf_fcu_read_reg(pv, 0x2b, &failure, 1);
+	if (rc != 1)
+		return -EIO;
+	if ((failure & (1 << fan->id)) != 0)
+		return -EFAULT;
+	rc = wf_fcu_read_reg(pv, 0x2d, &active, 1);
+	if (rc != 1)
+		return -EIO;
+	if ((active & (1 << fan->id)) == 0)
+		return -ENXIO;
+
+	rc = wf_fcu_read_reg(pv, 0x30 + (fan->id * 2), buf, 1);
+	if (rc != 1)
+		return -EIO;
+
+	*value = (((s32)buf[0]) * 1000) / 2559;
+
+	return 0;
+}
+
+static s32 wf_fcu_fan_min(struct wf_control *ct)
+{
+	struct wf_fcu_fan *fan = ct->priv;
+
+	return fan->min;
+}
+
+static s32 wf_fcu_fan_max(struct wf_control *ct)
+{
+	struct wf_fcu_fan *fan = ct->priv;
+
+	return fan->max;
+}
+
+static const struct wf_control_ops wf_fcu_fan_rpm_ops = {
+	.set_value	= wf_fcu_fan_set_rpm,
+	.get_value	= wf_fcu_fan_get_rpm,
+	.get_min	= wf_fcu_fan_min,
+	.get_max	= wf_fcu_fan_max,
+	.release	= wf_fcu_fan_release,
+	.owner		= THIS_MODULE,
+};
+
+static const struct wf_control_ops wf_fcu_fan_pwm_ops = {
+	.set_value	= wf_fcu_fan_set_pwm,
+	.get_value	= wf_fcu_fan_get_pwm,
+	.get_min	= wf_fcu_fan_min,
+	.get_max	= wf_fcu_fan_max,
+	.release	= wf_fcu_fan_release,
+	.owner		= THIS_MODULE,
+};
+
+static void __devinit wf_fcu_get_pump_minmax(struct wf_fcu_fan *fan)
+{
+	const struct mpu_data *mpu = wf_get_mpu(0);
+	u16 pump_min = 0, pump_max = 0xffff;
+	u16 tmp[4];
+
+	/* Try to fetch pumps min/max infos from eeprom */
+	if (mpu) {
+		memcpy(&tmp, mpu->processor_part_num, 8);
+		if (tmp[0] != 0xffff && tmp[1] != 0xffff) {
+			pump_min = max(pump_min, tmp[0]);
+			pump_max = min(pump_max, tmp[1]);
+		}
+		if (tmp[2] != 0xffff && tmp[3] != 0xffff) {
+			pump_min = max(pump_min, tmp[2]);
+			pump_max = min(pump_max, tmp[3]);
+		}
+	}
+
+	/* Double check the values, this _IS_ needed as the EEPROM on
+	 * some dual 2.5Ghz G5s seem, at least, to have both min & max
+	 * same to the same value ... (grrrr)
+	 */
+	if (pump_min == pump_max || pump_min == 0 || pump_max == 0xffff) {
+		pump_min = CPU_PUMP_OUTPUT_MIN;
+		pump_max = CPU_PUMP_OUTPUT_MAX;
+	}
+
+	fan->min = pump_min;
+	fan->max = pump_max;
+
+	DBG("wf_fcu: pump min/max for %s set to: [%d..%d] RPM\n",
+	    fan->ctrl.name, pump_min, pump_max);
+}
+
+static void __devinit wf_fcu_get_rpmfan_minmax(struct wf_fcu_fan *fan)
+{
+	struct wf_fcu_priv *pv = fan->fcu_priv;
+	const struct mpu_data *mpu0 = wf_get_mpu(0);
+	const struct mpu_data *mpu1 = wf_get_mpu(1);
+
+	/* Default */
+	fan->min = 2400 >> pv->rpm_shift;
+	fan->max = 56000 >> pv->rpm_shift;
+
+	/* CPU fans have min/max in MPU */
+	if (mpu0 && !strcmp(fan->ctrl.name, "cpu-front-fan-0")) {
+		fan->min = max(fan->min, (s32)mpu0->rminn_intake_fan);
+		fan->max = min(fan->max, (s32)mpu0->rmaxn_intake_fan);
+		goto bail;
+	}
+	if (mpu1 && !strcmp(fan->ctrl.name, "cpu-front-fan-1")) {
+		fan->min = max(fan->min, (s32)mpu1->rminn_intake_fan);
+		fan->max = min(fan->max, (s32)mpu1->rmaxn_intake_fan);
+		goto bail;
+	}
+	if (mpu0 && !strcmp(fan->ctrl.name, "cpu-rear-fan-0")) {
+		fan->min = max(fan->min, (s32)mpu0->rminn_exhaust_fan);
+		fan->max = min(fan->max, (s32)mpu0->rmaxn_exhaust_fan);
+		goto bail;
+	}
+	if (mpu1 && !strcmp(fan->ctrl.name, "cpu-rear-fan-1")) {
+		fan->min = max(fan->min, (s32)mpu1->rminn_exhaust_fan);
+		fan->max = min(fan->max, (s32)mpu1->rmaxn_exhaust_fan);
+		goto bail;
+	}
+	/* Rackmac variants, we just use mpu0 intake */
+	if (!strncmp(fan->ctrl.name, "cpu-fan", 7)) {
+		fan->min = max(fan->min, (s32)mpu0->rminn_intake_fan);
+		fan->max = min(fan->max, (s32)mpu0->rmaxn_intake_fan);
+		goto bail;
+	}
+ bail:
+	DBG("wf_fcu: fan min/max for %s set to: [%d..%d] RPM\n",
+	    fan->ctrl.name, fan->min, fan->max);
+}
+
+static void __devinit wf_fcu_add_fan(struct wf_fcu_priv *pv,
+				     const char *name,
+				     int type, int id)
+{
+	struct wf_fcu_fan *fan;
+
+	fan = kzalloc(sizeof(*fan), GFP_KERNEL);
+	if (!fan)
+		return;
+	fan->fcu_priv = pv;
+	fan->id = id;
+	fan->ctrl.name = name;
+	fan->ctrl.priv = fan;
+
+	/* min/max is oddball but the code comes from
+	 * therm_pm72 which seems to work so ...
+	 */
+	if (type == FCU_FAN_RPM) {
+		if (!strncmp(name, "cpu-pump", strlen("cpu-pump")))
+			wf_fcu_get_pump_minmax(fan);
+		else
+			wf_fcu_get_rpmfan_minmax(fan);
+		fan->ctrl.type = WF_CONTROL_RPM_FAN;
+		fan->ctrl.ops = &wf_fcu_fan_rpm_ops;
+	} else {
+		fan->min = 10;
+		fan->max = 100;
+		fan->ctrl.type = WF_CONTROL_PWM_FAN;
+		fan->ctrl.ops = &wf_fcu_fan_pwm_ops;
+	}
+
+	if (wf_register_control(&fan->ctrl)) {
+		pr_err("wf_fcu: Failed to register fan %s\n", name);
+		kfree(fan);
+		return;
+	}
+	list_add(&fan->link, &pv->fan_list);
+	kref_get(&pv->ref);
+}
+
+static void __devinit wf_fcu_lookup_fans(struct wf_fcu_priv *pv)
+{
+	/* Translation of device-tree location properties to
+	 * windfarm fan names
+	 */
+	static const struct {
+		const char *dt_name;	/* Device-tree name */
+		const char *ct_name;	/* Control name */
+	} loc_trans[] = {
+		{ "BACKSIDE",		"backside-fan",		},
+		{ "SYS CTRLR FAN",	"backside-fan",		},
+		{ "DRIVE BAY",		"drive-bay-fan",	},
+		{ "SLOT",		"slots-fan",		},
+		{ "PCI FAN",		"slots-fan",		},
+		{ "CPU A INTAKE",	"cpu-front-fan-0",	},
+		{ "CPU A EXHAUST",	"cpu-rear-fan-0",	},
+		{ "CPU B INTAKE",	"cpu-front-fan-1",	},
+		{ "CPU B EXHAUST",	"cpu-rear-fan-1",	},
+		{ "CPU A PUMP",		"cpu-pump-0",		},
+		{ "CPU B PUMP",		"cpu-pump-1",		},
+		{ "CPU A 1",		"cpu-fan-a-0",		},
+		{ "CPU A 2",		"cpu-fan-b-0",		},
+		{ "CPU A 3",		"cpu-fan-c-0",		},
+		{ "CPU B 1",		"cpu-fan-a-1",		},
+		{ "CPU B 2",		"cpu-fan-b-1",		},
+		{ "CPU B 3",		"cpu-fan-c-1",		},
+	};
+	struct device_node *np = NULL, *fcu = pv->i2c->dev.of_node;
+	int i;
+
+	DBG("Looking up FCU controls in device-tree...\n");
+
+	while ((np = of_get_next_child(fcu, np)) != NULL) {
+		int id, type = -1;
+		const char *loc;
+		const char *name;
+		const u32 *reg;
+
+		DBG(" control: %s, type: %s\n", np->name, np->type);
+
+		/* Detect control type */
+		if (!strcmp(np->type, "fan-rpm-control") ||
+		    !strcmp(np->type, "fan-rpm"))
+			type = FCU_FAN_RPM;
+		if (!strcmp(np->type, "fan-pwm-control") ||
+		    !strcmp(np->type, "fan-pwm"))
+			type = FCU_FAN_PWM;
+		/* Only care about fans for now */
+		if (type == -1)
+			continue;
+
+		/* Lookup for a matching location */
+		loc = of_get_property(np, "location", NULL);
+		reg = of_get_property(np, "reg", NULL);
+		if (loc == NULL || reg == NULL)
+			continue;
+		DBG(" matching location: %s, reg: 0x%08x\n", loc, *reg);
+
+		for (i = 0; i < ARRAY_SIZE(loc_trans); i++) {
+			if (strncmp(loc, loc_trans[i].dt_name,
+				    strlen(loc_trans[i].dt_name)))
+				continue;
+			name = loc_trans[i].ct_name;
+
+			DBG(" location match, name: %s\n", name);
+
+			if (type == FCU_FAN_RPM)
+				id = ((*reg) - 0x10) / 2;
+			else
+				id = ((*reg) - 0x30) / 2;
+			if (id > 7) {
+				pr_warning("wf_fcu: Can't parse "
+				       "fan ID in device-tree for %s\n",
+					   np->full_name);
+				break;
+			}
+			wf_fcu_add_fan(pv, name, type, id);
+			break;
+		}
+	}
+}
+
+static void __devinit wf_fcu_default_fans(struct wf_fcu_priv *pv)
+{
+	/* We only support the default fans for PowerMac7,2 */
+	if (!of_machine_is_compatible("PowerMac7,2"))
+		return;
+
+	wf_fcu_add_fan(pv, "backside-fan",	FCU_FAN_PWM, 1);
+	wf_fcu_add_fan(pv, "drive-bay-fan",	FCU_FAN_RPM, 2);
+	wf_fcu_add_fan(pv, "slots-fan",		FCU_FAN_PWM, 2);
+	wf_fcu_add_fan(pv, "cpu-front-fan-0",	FCU_FAN_RPM, 3);
+	wf_fcu_add_fan(pv, "cpu-rear-fan-0",	FCU_FAN_RPM, 4);
+	wf_fcu_add_fan(pv, "cpu-front-fan-1",	FCU_FAN_RPM, 5);
+	wf_fcu_add_fan(pv, "cpu-rear-fan-1",	FCU_FAN_RPM, 6);
+}
+
+static int __devinit wf_fcu_init_chip(struct wf_fcu_priv *pv)
+{
+	unsigned char buf = 0xff;
+	int rc;
+
+	rc = wf_fcu_write_reg(pv, 0xe, &buf, 1);
+	if (rc < 0)
+		return -EIO;
+	rc = wf_fcu_write_reg(pv, 0x2e, &buf, 1);
+	if (rc < 0)
+		return -EIO;
+	rc = wf_fcu_read_reg(pv, 0, &buf, 1);
+	if (rc < 0)
+		return -EIO;
+	pv->rpm_shift = (buf == 1) ? 2 : 3;
+
+	pr_debug("wf_fcu: FCU Initialized, RPM fan shift is %d\n",
+		 pv->rpm_shift);
+
+	return 0;
+}
+
+static int __devinit wf_fcu_probe(struct i2c_client *client,
+				  const struct i2c_device_id *id)
+{
+	struct wf_fcu_priv *pv;
+
+	pv = kzalloc(sizeof(*pv), GFP_KERNEL);
+	if (!pv)
+		return -ENOMEM;
+
+	kref_init(&pv->ref);
+	mutex_init(&pv->lock);
+	INIT_LIST_HEAD(&pv->fan_list);
+	pv->i2c = client;
+
+	/*
+	 * First we must start the FCU which will query the
+	 * shift value to apply to RPMs
+	 */
+	if (wf_fcu_init_chip(pv)) {
+		pr_err("wf_fcu: Initialization failed !\n");
+		kfree(pv);
+		return -ENXIO;
+	}
+
+	/* First lookup fans in the device-tree */
+	wf_fcu_lookup_fans(pv);
+
+	/*
+	 * Older machines don't have the device-tree entries
+	 * we are looking for, just hard code the list
+	 */
+	if (list_empty(&pv->fan_list))
+		wf_fcu_default_fans(pv);
+
+	/* Still no fans ? FAIL */
+	if (list_empty(&pv->fan_list)) {
+		pr_err("wf_fcu: Failed to find fans for your machine\n");
+		kfree(pv);
+		return -ENODEV;
+	}
+
+	dev_set_drvdata(&client->dev, pv);
+
+	return 0;
+}
+
+static int __devexit wf_fcu_remove(struct i2c_client *client)
+{
+	struct wf_fcu_priv *pv = dev_get_drvdata(&client->dev);
+	struct wf_fcu_fan *fan;
+
+	while (!list_empty(&pv->fan_list)) {
+		fan = list_first_entry(&pv->fan_list, struct wf_fcu_fan, link);
+		list_del(&fan->link);
+		wf_unregister_control(&fan->ctrl);
+	}
+	kref_put(&pv->ref, wf_fcu_release);
+	return 0;
+}
+
+static const struct i2c_device_id wf_fcu_id[] = {
+	{ "MAC,fcu", 0 },
+	{ }
+};
+MODULE_DEVICE_TABLE(i2c, wf_fcu_id);
+
+static struct i2c_driver wf_fcu_driver = {
+	.driver = {
+		.name	= "wf_fcu",
+	},
+	.probe		= wf_fcu_probe,
+	.remove		= wf_fcu_remove,
+	.id_table	= wf_fcu_id,
+};
+
+static int __init wf_fcu_init(void)
+{
+	return i2c_add_driver(&wf_fcu_driver);
+}
+
+static void __exit wf_fcu_exit(void)
+{
+	i2c_del_driver(&wf_fcu_driver);
+}
+
+
+module_init(wf_fcu_init);
+module_exit(wf_fcu_exit);
+
+MODULE_AUTHOR("Benjamin Herrenschmidt <benh@kernel.crashing.org>");
+MODULE_DESCRIPTION("FCU control objects for PowerMacs thermal control");
+MODULE_LICENSE("GPL");
+
diff --git a/drivers/macintosh/windfarm_lm75_sensor.c b/drivers/macintosh/windfarm_lm75_sensor.c
index 4d6a90a1372..b0c2d3695b3 100644
--- a/drivers/macintosh/windfarm_lm75_sensor.c
+++ b/drivers/macintosh/windfarm_lm75_sensor.c
@@ -23,7 +23,7 @@
 
 #include "windfarm.h"
 
-#define VERSION "0.2"
+#define VERSION "1.0"
 
 #undef DEBUG
 
@@ -36,8 +36,8 @@
 struct wf_lm75_sensor {
 	int			ds1775 : 1;
 	int			inited : 1;
-	struct 	i2c_client	*i2c;
-	struct 	wf_sensor	sens;
+	struct i2c_client	*i2c;
+	struct wf_sensor	sens;
 };
 #define wf_to_lm75(c) container_of(c, struct wf_lm75_sensor, sens)
 
@@ -90,40 +90,19 @@ static struct wf_sensor_ops wf_lm75_ops = {
 
 static int wf_lm75_probe(struct i2c_client *client,
 			 const struct i2c_device_id *id)
-{
+{	
 	struct wf_lm75_sensor *lm;
-	int rc;
-
-	lm = kzalloc(sizeof(struct wf_lm75_sensor), GFP_KERNEL);
-	if (lm == NULL)
-		return -ENODEV;
-
-	lm->inited = 0;
-	lm->ds1775 = id->driver_data;
-	lm->i2c = client;
-	lm->sens.name = client->dev.platform_data;
-	lm->sens.ops = &wf_lm75_ops;
-	i2c_set_clientdata(client, lm);
-
-	rc = wf_register_sensor(&lm->sens);
-	if (rc)
-		kfree(lm);
-
-	return rc;
-}
-
-static struct i2c_driver wf_lm75_driver;
-
-static struct i2c_client *wf_lm75_create(struct i2c_adapter *adapter,
-					     u8 addr, int ds1775,
-					     const char *loc)
-{
-	struct i2c_board_info info;
-	struct i2c_client *client;
-	char *name;
+	int rc, ds1775 = id->driver_data;
+	const char *name, *loc;
 
 	DBG("wf_lm75: creating  %s device at address 0x%02x\n",
-	    ds1775 ? "ds1775" : "lm75", addr);
+	    ds1775 ? "ds1775" : "lm75", client->addr);
+
+	loc = of_get_property(client->dev.of_node, "hwsensor-location", NULL);
+	if (!loc) {
+		dev_warn(&client->dev, "Missing hwsensor-location property!\n");
+		return -ENXIO;
+	}
 
 	/* Usual rant about sensor names not beeing very consistent in
 	 * the device-tree, oh well ...
@@ -137,68 +116,31 @@ static struct i2c_client *wf_lm75_create(struct i2c_adapter *adapter,
 		name = "optical-drive-temp";
 	else if (!strcmp(loc, "HD Temp"))
 		name = "hard-drive-temp";
+	else if (!strcmp(loc, "PCI SLOTS"))
+		name = "slots-temp";
+	else if (!strcmp(loc, "CPU A INLET"))
+		name = "cpu-inlet-temp-0";
+	else if (!strcmp(loc, "CPU B INLET"))
+		name = "cpu-inlet-temp-1";
 	else
-		goto fail;
-
-	memset(&info, 0, sizeof(struct i2c_board_info));
-	info.addr = (addr >> 1) & 0x7f;
-	info.platform_data = name;
-	strlcpy(info.type, ds1775 ? "wf_ds1775" : "wf_lm75", I2C_NAME_SIZE);
-
-	client = i2c_new_device(adapter, &info);
-	if (client == NULL) {
-		printk(KERN_ERR "windfarm: failed to attach %s %s to i2c\n",
-		       ds1775 ? "ds1775" : "lm75", name);
-		goto fail;
-	}
-
-	/*
-	 * Let i2c-core delete that device on driver removal.
-	 * This is safe because i2c-core holds the core_lock mutex for us.
-	 */
-	list_add_tail(&client->detected, &wf_lm75_driver.clients);
-	return client;
- fail:
-	return NULL;
-}
-
-static int wf_lm75_attach(struct i2c_adapter *adapter)
-{
-	struct device_node *busnode, *dev;
-	struct pmac_i2c_bus *bus;
+		return -ENXIO;
+ 	
 
-	DBG("wf_lm75: adapter %s detected\n", adapter->name);
-
-	bus = pmac_i2c_adapter_to_bus(adapter);
-	if (bus == NULL)
+	lm = kzalloc(sizeof(struct wf_lm75_sensor), GFP_KERNEL);
+	if (lm == NULL)
 		return -ENODEV;
-	busnode = pmac_i2c_get_bus_node(bus);
 
-	DBG("wf_lm75: bus found, looking for device...\n");
-
-	/* Now look for lm75(s) in there */
-	for (dev = NULL;
-	     (dev = of_get_next_child(busnode, dev)) != NULL;) {
-		const char *loc =
-			of_get_property(dev, "hwsensor-location", NULL);
-		u8 addr;
+	lm->inited = 0;
+	lm->ds1775 = ds1775;
+	lm->i2c = client;
+	lm->sens.name = (char *)name; /* XXX fix constness in structure */
+	lm->sens.ops = &wf_lm75_ops;
+	i2c_set_clientdata(client, lm);
 
-		/* We must re-match the adapter in order to properly check
-		 * the channel on multibus setups
-		 */
-		if (!pmac_i2c_match_adapter(dev, adapter))
-			continue;
-		addr = pmac_i2c_get_dev_addr(dev);
-		if (loc == NULL || addr == 0)
-			continue;
-		/* real lm75 */
-		if (of_device_is_compatible(dev, "lm75"))
-			wf_lm75_create(adapter, addr, 0, loc);
-		/* ds1775 (compatible, better resolution */
-		else if (of_device_is_compatible(dev, "ds1775"))
-			wf_lm75_create(adapter, addr, 1, loc);
-	}
-	return 0;
+	rc = wf_register_sensor(&lm->sens);
+	if (rc)
+		kfree(lm);
+	return rc;
 }
 
 static int wf_lm75_remove(struct i2c_client *client)
@@ -217,16 +159,16 @@ static int wf_lm75_remove(struct i2c_client *client)
 }
 
 static const struct i2c_device_id wf_lm75_id[] = {
-	{ "wf_lm75", 0 },
-	{ "wf_ds1775", 1 },
+	{ "MAC,lm75", 0 },
+	{ "MAC,ds1775", 1 },
 	{ }
 };
+MODULE_DEVICE_TABLE(i2c, wf_lm75_id);
 
 static struct i2c_driver wf_lm75_driver = {
 	.driver = {
 		.name	= "wf_lm75",
 	},
-	.attach_adapter	= wf_lm75_attach,
 	.probe		= wf_lm75_probe,
 	.remove		= wf_lm75_remove,
 	.id_table	= wf_lm75_id,
@@ -234,11 +176,6 @@ static struct i2c_driver wf_lm75_driver = {
 
 static int __init wf_lm75_sensor_init(void)
 {
-	/* Don't register on old machines that use therm_pm72 for now */
-	if (of_machine_is_compatible("PowerMac7,2") ||
-	    of_machine_is_compatible("PowerMac7,3") ||
-	    of_machine_is_compatible("RackMac3,1"))
-		return -ENODEV;
 	return i2c_add_driver(&wf_lm75_driver);
 }
 
diff --git a/drivers/macintosh/windfarm_lm87_sensor.c b/drivers/macintosh/windfarm_lm87_sensor.c
new file mode 100644
index 00000000000..c071aab79dd
--- /dev/null
+++ b/drivers/macintosh/windfarm_lm87_sensor.c
@@ -0,0 +1,201 @@
+/*
+ * Windfarm PowerMac thermal control. LM87 sensor
+ *
+ * Copyright 2012 Benjamin Herrenschmidt, IBM Corp.
+ *
+ * Released under the term of the GNU GPL v2.
+ *
+ */
+
+#include <linux/types.h>
+#include <linux/errno.h>
+#include <linux/kernel.h>
+#include <linux/delay.h>
+#include <linux/slab.h>
+#include <linux/init.h>
+#include <linux/wait.h>
+#include <linux/i2c.h>
+#include <asm/prom.h>
+#include <asm/machdep.h>
+#include <asm/io.h>
+#include <asm/sections.h>
+#include <asm/pmac_low_i2c.h>
+
+#include "windfarm.h"
+
+#define VERSION "1.0"
+
+#undef DEBUG
+
+#ifdef DEBUG
+#define DBG(args...)	printk(args)
+#else
+#define DBG(args...)	do { } while(0)
+#endif
+
+struct wf_lm87_sensor {
+	struct i2c_client	*i2c;
+	struct wf_sensor	sens;
+};
+#define wf_to_lm87(c) container_of(c, struct wf_lm87_sensor, sens)
+
+
+static int wf_lm87_read_reg(struct i2c_client *chip, int reg)
+{
+	int rc, tries = 0;
+	u8 buf;
+
+	for (;;) {
+		/* Set address */
+		buf = (u8)reg;
+		rc = i2c_master_send(chip, &buf, 1);
+		if (rc <= 0)
+			goto error;
+		rc = i2c_master_recv(chip, &buf, 1);
+		if (rc <= 0)
+			goto error;
+		return (int)buf;
+	error:
+		DBG("wf_lm87: Error reading LM87, retrying...\n");
+		if (++tries > 10) {
+			printk(KERN_ERR "wf_lm87: Error reading LM87 !\n");
+			return -EIO;
+		}
+		msleep(10);
+	}
+}
+
+static int wf_lm87_get(struct wf_sensor *sr, s32 *value)
+{
+	struct wf_lm87_sensor *lm = sr->priv;
+	s32 temp;
+
+	if (lm->i2c == NULL)
+		return -ENODEV;
+
+#define LM87_INT_TEMP		0x27
+
+	/* Read temperature register */
+	temp = wf_lm87_read_reg(lm->i2c, LM87_INT_TEMP);
+	if (temp < 0)
+		return temp;
+	*value = temp << 16;
+
+	return 0;
+}
+
+static void wf_lm87_release(struct wf_sensor *sr)
+{
+	struct wf_lm87_sensor *lm = wf_to_lm87(sr);
+
+	kfree(lm);
+}
+
+static struct wf_sensor_ops wf_lm87_ops = {
+	.get_value	= wf_lm87_get,
+	.release	= wf_lm87_release,
+	.owner		= THIS_MODULE,
+};
+
+static int wf_lm87_probe(struct i2c_client *client,
+			 const struct i2c_device_id *id)
+{	
+	struct wf_lm87_sensor *lm;
+	const char *name = NULL, *loc;
+	struct device_node *np = NULL;
+	int rc;
+
+	/*
+	 * The lm87 contains a whole pile of sensors, additionally,
+	 * the Xserve G5 has several lm87's. However, for now we only
+	 * care about the internal temperature sensor
+	 */
+	while ((np = of_get_next_child(client->dev.of_node, np)) != NULL) {
+		if (strcmp(np->name, "int-temp"))
+			continue;
+		loc = of_get_property(np, "location", NULL);
+		if (!loc)
+			continue;
+		if (strstr(loc, "DIMM"))
+			name = "dimms-temp";
+		else if (strstr(loc, "Processors"))
+			name = "between-cpus-temp";
+		if (name) {
+			of_node_put(np);
+			break;
+		}
+	}
+	if (!name) {
+		pr_warning("wf_lm87: Unsupported sensor %s\n",
+			   client->dev.of_node->full_name);
+		return -ENODEV;
+	}
+
+	lm = kzalloc(sizeof(struct wf_lm87_sensor), GFP_KERNEL);
+	if (lm == NULL)
+		return -ENODEV;
+
+	lm->i2c = client;
+	lm->sens.name = name;
+	lm->sens.ops = &wf_lm87_ops;
+	lm->sens.priv = lm;
+	i2c_set_clientdata(client, lm);
+
+	rc = wf_register_sensor(&lm->sens);
+	if (rc)
+		kfree(lm);
+	return rc;
+}
+
+static int wf_lm87_remove(struct i2c_client *client)
+{
+	struct wf_lm87_sensor *lm = i2c_get_clientdata(client);
+
+	DBG("wf_lm87: i2c detatch called for %s\n", lm->sens.name);
+
+	/* Mark client detached */
+	lm->i2c = NULL;
+
+	/* release sensor */
+	wf_unregister_sensor(&lm->sens);
+
+	return 0;
+}
+
+static const struct i2c_device_id wf_lm87_id[] = {
+	{ "MAC,lm87cimt", 0 },
+	{ }
+};
+MODULE_DEVICE_TABLE(i2c, wf_lm87_id);
+
+static struct i2c_driver wf_lm87_driver = {
+	.driver = {
+		.name	= "wf_lm87",
+	},
+	.probe		= wf_lm87_probe,
+	.remove		= wf_lm87_remove,
+	.id_table	= wf_lm87_id,
+};
+
+static int __init wf_lm87_sensor_init(void)
+{
+	/* We only support this on the Xserve */
+	if (!of_machine_is_compatible("RackMac3,1"))
+		return -ENODEV;
+
+	return i2c_add_driver(&wf_lm87_driver);
+}
+
+static void __exit wf_lm87_sensor_exit(void)
+{
+	i2c_del_driver(&wf_lm87_driver);
+}
+
+
+module_init(wf_lm87_sensor_init);
+module_exit(wf_lm87_sensor_exit);
+
+MODULE_AUTHOR("Benjamin Herrenschmidt <benh@kernel.crashing.org>");
+MODULE_DESCRIPTION("LM87 sensor objects for PowerMacs thermal control");
+MODULE_LICENSE("GPL");
+
diff --git a/drivers/macintosh/windfarm_max6690_sensor.c b/drivers/macintosh/windfarm_max6690_sensor.c
index 8204113268f..371b058d2f7 100644
--- a/drivers/macintosh/windfarm_max6690_sensor.c
+++ b/drivers/macintosh/windfarm_max6690_sensor.c
@@ -16,7 +16,7 @@
 
 #include "windfarm.h"
 
-#define VERSION "0.2"
+#define VERSION "1.0"
 
 /* This currently only exports the external temperature sensor,
    since that's all the control loops need. */
@@ -64,9 +64,29 @@ static struct wf_sensor_ops wf_max6690_ops = {
 static int wf_max6690_probe(struct i2c_client *client,
 			    const struct i2c_device_id *id)
 {
+	const char *name, *loc;
 	struct wf_6690_sensor *max;
 	int rc;
 
+	loc = of_get_property(client->dev.of_node, "hwsensor-location", NULL);
+	if (!loc) {
+		dev_warn(&client->dev, "Missing hwsensor-location property!\n");
+		return -ENXIO;
+	}
+
+	/*
+	 * We only expose the external temperature register for
+	 * now as this is all we need for our control loops
+	 */
+	if (!strcmp(loc, "BACKSIDE") || !strcmp(loc, "SYS CTRLR AMBIENT"))
+		name = "backside-temp";
+	else if (!strcmp(loc, "NB Ambient"))
+		name = "north-bridge-temp";
+	else if (!strcmp(loc, "GPU Ambient"))
+		name = "gpu-temp";
+	else
+		return -ENXIO;
+
 	max = kzalloc(sizeof(struct wf_6690_sensor), GFP_KERNEL);
 	if (max == NULL) {
 		printk(KERN_ERR "windfarm: Couldn't create MAX6690 sensor: "
@@ -75,90 +95,16 @@ static int wf_max6690_probe(struct i2c_client *client,
 	}
 
 	max->i2c = client;
-	max->sens.name = client->dev.platform_data;
+	max->sens.name = (char *)name; /* XXX fix constness in structure */
 	max->sens.ops = &wf_max6690_ops;
 	i2c_set_clientdata(client, max);
 
 	rc = wf_register_sensor(&max->sens);
-	if (rc) {
+	if (rc)
 		kfree(max);
-	}
-
 	return rc;
 }
 
-static struct i2c_driver wf_max6690_driver;
-
-static struct i2c_client *wf_max6690_create(struct i2c_adapter *adapter,
-					    u8 addr, const char *loc)
-{
-	struct i2c_board_info info;
-	struct i2c_client *client;
-	char *name;
-
-	if (!strcmp(loc, "BACKSIDE"))
-		name = "backside-temp";
-	else if (!strcmp(loc, "NB Ambient"))
-		name = "north-bridge-temp";
-	else if (!strcmp(loc, "GPU Ambient"))
-		name = "gpu-temp";
-	else
-		goto fail;
-
-	memset(&info, 0, sizeof(struct i2c_board_info));
-	info.addr = addr >> 1;
-	info.platform_data = name;
-	strlcpy(info.type, "wf_max6690", I2C_NAME_SIZE);
-
-	client = i2c_new_device(adapter, &info);
-	if (client == NULL) {
-		printk(KERN_ERR "windfarm: failed to attach MAX6690 sensor\n");
-		goto fail;
-	}
-
-	/*
-	 * Let i2c-core delete that device on driver removal.
-	 * This is safe because i2c-core holds the core_lock mutex for us.
-	 */
-	list_add_tail(&client->detected, &wf_max6690_driver.clients);
-	return client;
-
- fail:
-	return NULL;
-}
-
-static int wf_max6690_attach(struct i2c_adapter *adapter)
-{
-	struct device_node *busnode, *dev = NULL;
-	struct pmac_i2c_bus *bus;
-	const char *loc;
-
-	bus = pmac_i2c_adapter_to_bus(adapter);
-	if (bus == NULL)
-		return -ENODEV;
-	busnode = pmac_i2c_get_bus_node(bus);
-
-	while ((dev = of_get_next_child(busnode, dev)) != NULL) {
-		u8 addr;
-
-		/* We must re-match the adapter in order to properly check
-		 * the channel on multibus setups
-		 */
-		if (!pmac_i2c_match_adapter(dev, adapter))
-			continue;
-		if (!of_device_is_compatible(dev, "max6690"))
-			continue;
-		addr = pmac_i2c_get_dev_addr(dev);
-		loc = of_get_property(dev, "hwsensor-location", NULL);
-		if (loc == NULL || addr == 0)
-			continue;
-		printk("found max6690, loc=%s addr=0x%02x\n", loc, addr);
-		wf_max6690_create(adapter, addr, loc);
-	}
-
-	return 0;
-}
-
 static int wf_max6690_remove(struct i2c_client *client)
 {
 	struct wf_6690_sensor *max = i2c_get_clientdata(client);
@@ -170,15 +116,15 @@ static int wf_max6690_remove(struct i2c_client *client)
 }
 
 static const struct i2c_device_id wf_max6690_id[] = {
-	{ "wf_max6690", 0 },
+	{ "MAC,max6690", 0 },
 	{ }
 };
+MODULE_DEVICE_TABLE(i2c, wf_max6690_id);
 
 static struct i2c_driver wf_max6690_driver = {
 	.driver = {
 		.name		= "wf_max6690",
 	},
-	.attach_adapter	= wf_max6690_attach,
 	.probe		= wf_max6690_probe,
 	.remove		= wf_max6690_remove,
 	.id_table	= wf_max6690_id,
@@ -186,11 +132,6 @@ static struct i2c_driver wf_max6690_driver = {
 
 static int __init wf_max6690_sensor_init(void)
 {
-	/* Don't register on old machines that use therm_pm72 for now */
-	if (of_machine_is_compatible("PowerMac7,2") ||
-	    of_machine_is_compatible("PowerMac7,3") ||
-	    of_machine_is_compatible("RackMac3,1"))
-		return -ENODEV;
 	return i2c_add_driver(&wf_max6690_driver);
 }
 
diff --git a/drivers/macintosh/windfarm_mpu.h b/drivers/macintosh/windfarm_mpu.h
new file mode 100644
index 00000000000..046edc8c2ec
--- /dev/null
+++ b/drivers/macintosh/windfarm_mpu.h
@@ -0,0 +1,105 @@
+/*
+ * Windfarm PowerMac thermal control
+ *
+ * Copyright 2012 Benjamin Herrenschmidt, IBM Corp.
+ *
+ * Released under the term of the GNU GPL v2.
+ */
+
+#ifndef __WINDFARM_MPU_H
+#define __WINDFARM_MPU_H
+
+typedef unsigned short fu16;
+typedef int fs32;
+typedef short fs16;
+
+/* Definition of the MPU data structure which contains per CPU
+ * calibration information (among others) for the G5 machines
+ */
+struct mpu_data
+{
+	u8	signature;		/* 0x00 - EEPROM sig. */
+	u8	bytes_used;		/* 0x01 - Bytes used in eeprom (160 ?) */
+	u8	size;			/* 0x02 - EEPROM size (256 ?) */
+	u8	version;		/* 0x03 - EEPROM version */
+	u32	data_revision;		/* 0x04 - Dataset revision */
+	u8	processor_bin_code[3];	/* 0x08 - Processor BIN code */
+	u8	bin_code_expansion;	/* 0x0b - ??? (padding ?) */
+	u8	processor_num;		/* 0x0c - Number of CPUs on this MPU */
+	u8	input_mul_bus_div;	/* 0x0d - Clock input multiplier/bus divider */
+	u8	reserved1[2];		/* 0x0e - */
+	u32	input_clk_freq_high;	/* 0x10 - Input clock frequency high */
+	u8	cpu_nb_target_cycles;	/* 0x14 - ??? */
+	u8	cpu_statlat;		/* 0x15 - ??? */
+	u8	cpu_snooplat;		/* 0x16 - ??? */
+	u8	cpu_snoopacc;		/* 0x17 - ??? */
+	u8	nb_paamwin;		/* 0x18 - ??? */
+	u8	nb_statlat;		/* 0x19 - ??? */
+	u8	nb_snooplat;		/* 0x1a - ??? */
+	u8	nb_snoopwin;		/* 0x1b - ??? */
+	u8	api_bus_mode;		/* 0x1c - ??? */
+	u8	reserved2[3];		/* 0x1d - */
+	u32	input_clk_freq_low;	/* 0x20 - Input clock frequency low */
+	u8	processor_card_slot;	/* 0x24 - Processor card slot number */
+	u8	reserved3[2];		/* 0x25 - */
+	u8	padjmax;       		/* 0x27 - Max power adjustment (Not in OF!) */
+	u8	ttarget;		/* 0x28 - Target temperature */
+	u8	tmax;			/* 0x29 - Max temperature */
+	u8	pmaxh;			/* 0x2a - Max power */
+	u8	tguardband;		/* 0x2b - Guardband temp ??? Hist. len in OSX */
+	fs32	pid_gp;			/* 0x2c - PID proportional gain */
+	fs32	pid_gr;			/* 0x30 - PID reset gain */
+	fs32	pid_gd;			/* 0x34 - PID derivative gain */
+	fu16	voph;			/* 0x38 - Vop High */
+	fu16	vopl;			/* 0x3a - Vop Low */
+	fs16	nactual_die;		/* 0x3c - nActual Die */
+	fs16	nactual_heatsink;	/* 0x3e - nActual Heatsink */
+	fs16	nactual_system;		/* 0x40 - nActual System */
+	u16	calibration_flags;	/* 0x42 - Calibration flags */
+	fu16	mdiode;			/* 0x44 - Diode M value (scaling factor) */
+	fs16	bdiode;			/* 0x46 - Diode B value (offset) */
+	fs32	theta_heat_sink;	/* 0x48 - Theta heat sink */
+	u16	rminn_intake_fan;	/* 0x4c - Intake fan min RPM */
+	u16	rmaxn_intake_fan;	/* 0x4e - Intake fan max RPM */
+	u16	rminn_exhaust_fan;	/* 0x50 - Exhaust fan min RPM */
+	u16	rmaxn_exhaust_fan;	/* 0x52 - Exhaust fan max RPM */
+	u8	processor_part_num[8];	/* 0x54 - Processor part number XX pumps min/max */
+	u32	processor_lot_num;	/* 0x5c - Processor lot number */
+	u8	orig_card_sernum[0x10];	/* 0x60 - Card original serial number */
+	u8	curr_card_sernum[0x10];	/* 0x70 - Card current serial number */
+	u8	mlb_sernum[0x18];	/* 0x80 - MLB serial number */
+	u32	checksum1;		/* 0x98 - */
+	u32	checksum2;		/* 0x9c - */	
+}; /* Total size = 0xa0 */
+
+static inline const struct mpu_data *wf_get_mpu(int cpu)
+{
+	struct device_node *np;
+	char nodename[64];
+	const void *data;
+	int len;
+
+	/*
+	 * prom.c routine for finding a node by path is a bit brain dead
+	 * and requires exact @xxx unit numbers. This is a bit ugly but
+	 * will work for these machines
+	 */
+	sprintf(nodename, "/u3@0,f8000000/i2c@f8001000/cpuid@a%d", cpu ? 2 : 0);
+	np = of_find_node_by_path(nodename);
+	if (!np)
+		return NULL;
+	data = of_get_property(np, "cpuid", &len);	
+	of_node_put(np);
+	if (!data)
+		return NULL;
+
+	/*
+	 * We are naughty, we have dropped the reference to the device
+	 * node and still return a pointer to the content. We know we
+	 * can do that though as this is only ever called on PowerMac
+	 * which cannot remove those nodes
+	 */
+	return data;
+}
+
+#endif /*  __WINDFARM_MPU_H */
diff --git a/drivers/macintosh/windfarm_pm72.c b/drivers/macintosh/windfarm_pm72.c
new file mode 100644
index 00000000000..84ac913d7e3
--- /dev/null
+++ b/drivers/macintosh/windfarm_pm72.c
@@ -0,0 +1,847 @@
+/*
+ * Windfarm PowerMac thermal control.
+ * Control loops for PowerMac7,2 and 7,3
+ *
+ * Copyright (C) 2012 Benjamin Herrenschmidt, IBM Corp.
+ *
+ * Use and redistribute under the terms of the GNU GPL v2.
+ */
+#include <linux/types.h>
+#include <linux/errno.h>
+#include <linux/kernel.h>
+#include <linux/device.h>
+#include <linux/platform_device.h>
+#include <linux/reboot.h>
+#include <asm/prom.h>
+#include <asm/smu.h>
+
+#include "windfarm.h"
+#include "windfarm_pid.h"
+#include "windfarm_mpu.h"
+
+#define VERSION "1.0"
+
+#undef DEBUG
+#undef LOTSA_DEBUG
+
+#ifdef DEBUG
+#define DBG(args...)	printk(args)
+#else
+#define DBG(args...)	do { } while(0)
+#endif
+
+#ifdef LOTSA_DEBUG
+#define DBG_LOTS(args...)	printk(args)
+#else
+#define DBG_LOTS(args...)	do { } while(0)
+#endif
+
+/* define this to force CPU overtemp to 60 degree, useful for testing
+ * the overtemp code
+ */
+#undef HACKED_OVERTEMP
+
+/* We currently only handle 2 chips */
+#define NR_CHIPS	2
+#define NR_CPU_FANS	3 * NR_CHIPS
+
+/* Controls and sensors */
+static struct wf_sensor *sens_cpu_temp[NR_CHIPS];
+static struct wf_sensor *sens_cpu_volts[NR_CHIPS];
+static struct wf_sensor *sens_cpu_amps[NR_CHIPS];
+static struct wf_sensor *backside_temp;
+static struct wf_sensor *drives_temp;
+
+static struct wf_control *cpu_front_fans[NR_CHIPS];
+static struct wf_control *cpu_rear_fans[NR_CHIPS];
+static struct wf_control *cpu_pumps[NR_CHIPS];
+static struct wf_control *backside_fan;
+static struct wf_control *drives_fan;
+static struct wf_control *slots_fan;
+static struct wf_control *cpufreq_clamp;
+
+/* We keep a temperature history for average calculation of 180s */
+#define CPU_TEMP_HIST_SIZE	180
+
+/* Fixed speed for slot fan */
+#define	SLOTS_FAN_DEFAULT_PWM	40
+
+/* Scale value for CPU intake fans */
+#define CPU_INTAKE_SCALE	0x0000f852
+
+/* PID loop state */
+static const struct mpu_data *cpu_mpu_data[NR_CHIPS];
+static struct wf_cpu_pid_state cpu_pid[NR_CHIPS];
+static bool cpu_pid_combined;
+static u32 cpu_thist[CPU_TEMP_HIST_SIZE];
+static int cpu_thist_pt;
+static s64 cpu_thist_total;
+static s32 cpu_all_tmax = 100 << 16;
+static struct wf_pid_state backside_pid;
+static int backside_tick;
+static struct wf_pid_state drives_pid;
+static int drives_tick;
+
+static int nr_chips;
+static bool have_all_controls;
+static bool have_all_sensors;
+static bool started;
+
+static int failure_state;
+#define FAILURE_SENSOR		1
+#define FAILURE_FAN		2
+#define FAILURE_PERM		4
+#define FAILURE_LOW_OVERTEMP	8
+#define FAILURE_HIGH_OVERTEMP	16
+
+/* Overtemp values */
+#define LOW_OVER_AVERAGE	0
+#define LOW_OVER_IMMEDIATE	(10 << 16)
+#define LOW_OVER_CLEAR		((-10) << 16)
+#define HIGH_OVER_IMMEDIATE	(14 << 16)
+#define HIGH_OVER_AVERAGE	(10 << 16)
+#define HIGH_OVER_IMMEDIATE	(14 << 16)
+
+
+static void cpu_max_all_fans(void)
+{
+	int i;
+
+	/* We max all CPU fans in case of a sensor error. We also do the
+	 * cpufreq clamping now, even if it's supposedly done later by the
+	 * generic code anyway, we do it earlier here to react faster
+	 */
+	if (cpufreq_clamp)
+		wf_control_set_max(cpufreq_clamp);
+	for (i = 0; i < nr_chips; i++) {
+		if (cpu_front_fans[i])
+			wf_control_set_max(cpu_front_fans[i]);
+		if (cpu_rear_fans[i])
+			wf_control_set_max(cpu_rear_fans[i]);
+		if (cpu_pumps[i])
+			wf_control_set_max(cpu_pumps[i]);
+	}
+}
+
+static int cpu_check_overtemp(s32 temp)
+{
+	int new_state = 0;
+	s32 t_avg, t_old;
+	static bool first = true;
+
+	/* First check for immediate overtemps */
+	if (temp >= (cpu_all_tmax + LOW_OVER_IMMEDIATE)) {
+		new_state |= FAILURE_LOW_OVERTEMP;
+		if ((failure_state & FAILURE_LOW_OVERTEMP) == 0)
+			printk(KERN_ERR "windfarm: Overtemp due to immediate CPU"
+			       " temperature !\n");
+	}
+	if (temp >= (cpu_all_tmax + HIGH_OVER_IMMEDIATE)) {
+		new_state |= FAILURE_HIGH_OVERTEMP;
+		if ((failure_state & FAILURE_HIGH_OVERTEMP) == 0)
+			printk(KERN_ERR "windfarm: Critical overtemp due to"
+			       " immediate CPU temperature !\n");
+	}
+
+	/*
+	 * The first time around, initialize the array with the first
+	 * temperature reading
+	 */
+	if (first) {
+		int i;
+
+		cpu_thist_total = 0;
+		for (i = 0; i < CPU_TEMP_HIST_SIZE; i++) {
+			cpu_thist[i] = temp;
+			cpu_thist_total += temp;
+		}
+		first = false;
+	}
+
+	/*
+	 * We calculate a history of max temperatures and use that for the
+	 * overtemp management
+	 */
+	t_old = cpu_thist[cpu_thist_pt];
+	cpu_thist[cpu_thist_pt] = temp;
+	cpu_thist_pt = (cpu_thist_pt + 1) % CPU_TEMP_HIST_SIZE;
+	cpu_thist_total -= t_old;
+	cpu_thist_total += temp;
+	t_avg = cpu_thist_total / CPU_TEMP_HIST_SIZE;
+
+	DBG_LOTS("  t_avg = %d.%03d (out: %d.%03d, in: %d.%03d)\n",
+		 FIX32TOPRINT(t_avg), FIX32TOPRINT(t_old), FIX32TOPRINT(temp));
+
+	/* Now check for average overtemps */
+	if (t_avg >= (cpu_all_tmax + LOW_OVER_AVERAGE)) {
+		new_state |= FAILURE_LOW_OVERTEMP;
+		if ((failure_state & FAILURE_LOW_OVERTEMP) == 0)
+			printk(KERN_ERR "windfarm: Overtemp due to average CPU"
+			       " temperature !\n");
+	}
+	if (t_avg >= (cpu_all_tmax + HIGH_OVER_AVERAGE)) {
+		new_state |= FAILURE_HIGH_OVERTEMP;
+		if ((failure_state & FAILURE_HIGH_OVERTEMP) == 0)
+			printk(KERN_ERR "windfarm: Critical overtemp due to"
+			       " average CPU temperature !\n");
+	}
+
+	/* Now handle overtemp conditions. We don't currently use the windfarm
+	 * overtemp handling core as it's not fully suited to the needs of those
+	 * new machine. This will be fixed later.
+	 */
+	if (new_state) {
+		/* High overtemp -> immediate shutdown */
+		if (new_state & FAILURE_HIGH_OVERTEMP)
+			machine_power_off();
+		if ((failure_state & new_state) != new_state)
+			cpu_max_all_fans();
+		failure_state |= new_state;
+	} else if ((failure_state & FAILURE_LOW_OVERTEMP) &&
+		   (temp < (cpu_all_tmax + LOW_OVER_CLEAR))) {
+		printk(KERN_ERR "windfarm: Overtemp condition cleared !\n");
+		failure_state &= ~FAILURE_LOW_OVERTEMP;
+	}
+
+	return failure_state & (FAILURE_LOW_OVERTEMP | FAILURE_HIGH_OVERTEMP);
+}
+
+static int read_one_cpu_vals(int cpu, s32 *temp, s32 *power)
+{
+	s32 dtemp, volts, amps;
+	int rc;
+
+	/* Get diode temperature */
+	rc = wf_sensor_get(sens_cpu_temp[cpu], &dtemp);
+	if (rc) {
+		DBG("  CPU%d: temp reading error !\n", cpu);
+		return -EIO;
+	}
+	DBG_LOTS("  CPU%d: temp   = %d.%03d\n", cpu, FIX32TOPRINT((dtemp)));
+	*temp = dtemp;
+
+	/* Get voltage */
+	rc = wf_sensor_get(sens_cpu_volts[cpu], &volts);
+	if (rc) {
+		DBG("  CPU%d, volts reading error !\n", cpu);
+		return -EIO;
+	}
+	DBG_LOTS("  CPU%d: volts  = %d.%03d\n", cpu, FIX32TOPRINT((volts)));
+
+	/* Get current */
+	rc = wf_sensor_get(sens_cpu_amps[cpu], &amps);
+	if (rc) {
+		DBG("  CPU%d, current reading error !\n", cpu);
+		return -EIO;
+	}
+	DBG_LOTS("  CPU%d: amps   = %d.%03d\n", cpu, FIX32TOPRINT((amps)));
+
+	/* Calculate power */
+
+	/* Scale voltage and current raw sensor values according to fixed scales
+	 * obtained in Darwin and calculate power from I and V
+	 */
+	*power = (((u64)volts) * ((u64)amps)) >> 16;
+
+	DBG_LOTS("  CPU%d: power  = %d.%03d\n", cpu, FIX32TOPRINT((*power)));
+
+	return 0;
+
+}
+
+static void cpu_fans_tick_split(void)
+{
+	int err, cpu;
+	s32 intake, temp, power, t_max = 0;
+
+	DBG_LOTS("* cpu fans_tick_split()\n");
+
+	for (cpu = 0; cpu < nr_chips; ++cpu) {
+		struct wf_cpu_pid_state *sp = &cpu_pid[cpu];
+
+		/* Read current speed */
+		wf_control_get(cpu_rear_fans[cpu], &sp->target);
+
+		DBG_LOTS("  CPU%d: cur_target = %d RPM\n", cpu, sp->target);
+
+		err = read_one_cpu_vals(cpu, &temp, &power);
+		if (err) {
+			failure_state |= FAILURE_SENSOR;
+			cpu_max_all_fans();
+			return;
+		}
+
+		/* Keep track of highest temp */
+		t_max = max(t_max, temp);
+
+		/* Handle possible overtemps */
+		if (cpu_check_overtemp(t_max))
+			return;
+
+		/* Run PID */
+		wf_cpu_pid_run(sp, power, temp);
+
+		DBG_LOTS("  CPU%d: target = %d RPM\n", cpu, sp->target);
+
+		/* Apply result directly to exhaust fan */
+		err = wf_control_set(cpu_rear_fans[cpu], sp->target);
+		if (err) {
+			pr_warning("wf_pm72: Fan %s reports error %d\n",
+			       cpu_rear_fans[cpu]->name, err);
+			failure_state |= FAILURE_FAN;
+			break;
+		}
+
+		/* Scale result for intake fan */
+		intake = (sp->target * CPU_INTAKE_SCALE) >> 16;
+		DBG_LOTS("  CPU%d: intake = %d RPM\n", cpu, intake);
+		err = wf_control_set(cpu_front_fans[cpu], intake);
+		if (err) {
+			pr_warning("wf_pm72: Fan %s reports error %d\n",
+			       cpu_front_fans[cpu]->name, err);
+			failure_state |= FAILURE_FAN;
+			break;
+		}
+	}
+}
+
+static void cpu_fans_tick_combined(void)
+{
+	s32 temp0, power0, temp1, power1, t_max = 0;
+	s32 temp, power, intake, pump;
+	struct wf_control *pump0, *pump1;
+	struct wf_cpu_pid_state *sp = &cpu_pid[0];
+	int err, cpu;
+
+	DBG_LOTS("* cpu fans_tick_combined()\n");
+
+	/* Read current speed from cpu 0 */
+	wf_control_get(cpu_rear_fans[0], &sp->target);
+
+	DBG_LOTS("  CPUs: cur_target = %d RPM\n", sp->target);
+
+	/* Read values for both CPUs */
+	err = read_one_cpu_vals(0, &temp0, &power0);
+	if (err) {
+		failure_state |= FAILURE_SENSOR;
+		cpu_max_all_fans();
+		return;
+	}
+	err = read_one_cpu_vals(1, &temp1, &power1);
+	if (err) {
+		failure_state |= FAILURE_SENSOR;
+		cpu_max_all_fans();
+		return;
+	}
+
+	/* Keep track of highest temp */
+	t_max = max(t_max, max(temp0, temp1));
+
+	/* Handle possible overtemps */
+	if (cpu_check_overtemp(t_max))
+		return;
+
+	/* Use the max temp & power of both */
+	temp = max(temp0, temp1);
+	power = max(power0, power1);
+
+	/* Run PID */
+	wf_cpu_pid_run(sp, power, temp);
+
+	/* Scale result for intake fan */
+	intake = (sp->target * CPU_INTAKE_SCALE) >> 16;
+
+	/* Same deal with pump speed */
+	pump0 = cpu_pumps[0];
+	pump1 = cpu_pumps[1];
+	if (!pump0) {
+		pump0 = pump1;
+		pump1 = NULL;
+	}
+	pump = (sp->target * wf_control_get_max(pump0)) /
+		cpu_mpu_data[0]->rmaxn_exhaust_fan;
+
+	DBG_LOTS("  CPUs: target = %d RPM\n", sp->target);
+	DBG_LOTS("  CPUs: intake = %d RPM\n", intake);
+	DBG_LOTS("  CPUs: pump   = %d RPM\n", pump);
+
+	for (cpu = 0; cpu < nr_chips; cpu++) {
+		err = wf_control_set(cpu_rear_fans[cpu], sp->target);
+		if (err) {
+			pr_warning("wf_pm72: Fan %s reports error %d\n",
+				   cpu_rear_fans[cpu]->name, err);
+			failure_state |= FAILURE_FAN;
+		}
+		err = wf_control_set(cpu_front_fans[cpu], intake);
+		if (err) {
+			pr_warning("wf_pm72: Fan %s reports error %d\n",
+				   cpu_front_fans[cpu]->name, err);
+			failure_state |= FAILURE_FAN;
+		}
+		err = 0;
+		if (cpu_pumps[cpu])
+			err = wf_control_set(cpu_pumps[cpu], pump);
+		if (err) {
+			pr_warning("wf_pm72: Pump %s reports error %d\n",
+				   cpu_pumps[cpu]->name, err);
+			failure_state |= FAILURE_FAN;
+		}
+	}
+}
+
+/* Implementation... */
+static int cpu_setup_pid(int cpu)
+{
+	struct wf_cpu_pid_param pid;
+	const struct mpu_data *mpu = cpu_mpu_data[cpu];
+	s32 tmax, ttarget, ptarget;
+	int fmin, fmax, hsize;
+
+	/* Get PID params from the appropriate MPU EEPROM */
+	tmax = mpu->tmax << 16;
+	ttarget = mpu->ttarget << 16;
+	ptarget = ((s32)(mpu->pmaxh - mpu->padjmax)) << 16;
+
+	DBG("wf_72: CPU%d ttarget = %d.%03d, tmax = %d.%03d\n",
+	    cpu, FIX32TOPRINT(ttarget), FIX32TOPRINT(tmax));
+
+	/* We keep a global tmax for overtemp calculations */
+	if (tmax < cpu_all_tmax)
+		cpu_all_tmax = tmax;
+
+	/* Set PID min/max by using the rear fan min/max */
+	fmin = wf_control_get_min(cpu_rear_fans[cpu]);
+	fmax = wf_control_get_max(cpu_rear_fans[cpu]);
+	DBG("wf_72: CPU%d max RPM range = [%d..%d]\n", cpu, fmin, fmax);
+
+	/* History size */
+	hsize = min_t(int, mpu->tguardband, WF_PID_MAX_HISTORY);
+	DBG("wf_72: CPU%d history size = %d\n", cpu, hsize);
+
+	/* Initialize PID loop */
+	pid.interval	= 1;	/* seconds */
+	pid.history_len = hsize;
+	pid.gd		= mpu->pid_gd;
+	pid.gp		= mpu->pid_gp;
+	pid.gr		= mpu->pid_gr;
+	pid.tmax	= tmax;
+	pid.ttarget	= ttarget;
+	pid.pmaxadj	= ptarget;
+	pid.min		= fmin;
+	pid.max		= fmax;
+
+	wf_cpu_pid_init(&cpu_pid[cpu], &pid);
+	cpu_pid[cpu].target = 1000;
+
+	return 0;
+}
+
+/* Backside/U3 fan */
+static struct wf_pid_param backside_u3_param = {
+	.interval	= 5,
+	.history_len	= 2,
+	.gd		= 40 << 20,
+	.gp		= 5 << 20,
+	.gr		= 0,
+	.itarget	= 65 << 16,
+	.additive	= 1,
+	.min		= 20,
+	.max		= 100,
+};
+
+static struct wf_pid_param backside_u3h_param = {
+	.interval	= 5,
+	.history_len	= 2,
+	.gd		= 20 << 20,
+	.gp		= 5 << 20,
+	.gr		= 0,
+	.itarget	= 75 << 16,
+	.additive	= 1,
+	.min		= 20,
+	.max		= 100,
+};
+
+static void backside_fan_tick(void)
+{
+	s32 temp;
+	int speed;
+	int err;
+
+	if (!backside_fan || !backside_temp || !backside_tick)
+		return;
+	if (--backside_tick > 0)
+		return;
+	backside_tick = backside_pid.param.interval;
+
+	DBG_LOTS("* backside fans tick\n");
+
+	/* Update fan speed from actual fans */
+	err = wf_control_get(backside_fan, &speed);
+	if (!err)
+		backside_pid.target = speed;
+
+	err = wf_sensor_get(backside_temp, &temp);
+	if (err) {
+		printk(KERN_WARNING "windfarm: U4 temp sensor error %d\n",
+		       err);
+		failure_state |= FAILURE_SENSOR;
+		wf_control_set_max(backside_fan);
+		return;
+	}
+	speed = wf_pid_run(&backside_pid, temp);
+
+	DBG_LOTS("backside PID temp=%d.%.3d speed=%d\n",
+		 FIX32TOPRINT(temp), speed);
+
+	err = wf_control_set(backside_fan, speed);
+	if (err) {
+		printk(KERN_WARNING "windfarm: backside fan error %d\n", err);
+		failure_state |= FAILURE_FAN;
+	}
+}
+
+static void backside_setup_pid(void)
+{
+	/* first time initialize things */
+	s32 fmin = wf_control_get_min(backside_fan);
+	s32 fmax = wf_control_get_max(backside_fan);
+	struct wf_pid_param param;
+	struct device_node *u3;
+	int u3h = 1; /* conservative by default */
+
+	u3 = of_find_node_by_path("/u3@0,f8000000");
+	if (u3 != NULL) {
+		const u32 *vers = of_get_property(u3, "device-rev", NULL);
+		if (vers)
+			if (((*vers) & 0x3f) < 0x34)
+				u3h = 0;
+		of_node_put(u3);
+	}
+
+	param = u3h ? backside_u3h_param : backside_u3_param;
+
+	param.min = max(param.min, fmin);
+	param.max = min(param.max, fmax);
+	wf_pid_init(&backside_pid, &param);
+	backside_tick = 1;
+
+	pr_info("wf_pm72: Backside control loop started.\n");
+}
+
+/* Drive bay fan */
+static const struct wf_pid_param drives_param = {
+	.interval	= 5,
+	.history_len	= 2,
+	.gd		= 30 << 20,
+	.gp		= 5 << 20,
+	.gr		= 0,
+	.itarget	= 40 << 16,
+	.additive	= 1,
+	.min		= 300,
+	.max		= 4000,
+};
+
+static void drives_fan_tick(void)
+{
+	s32 temp;
+	int speed;
+	int err;
+
+	if (!drives_fan || !drives_temp || !drives_tick)
+		return;
+	if (--drives_tick > 0)
+		return;
+	drives_tick = drives_pid.param.interval;
+
+	DBG_LOTS("* drives fans tick\n");
+
+	/* Update fan speed from actual fans */
+	err = wf_control_get(drives_fan, &speed);
+	if (!err)
+		drives_pid.target = speed;
+
+	err = wf_sensor_get(drives_temp, &temp);
+	if (err) {
+		pr_warning("wf_pm72: drive bay temp sensor error %d\n", err);
+		failure_state |= FAILURE_SENSOR;
+		wf_control_set_max(drives_fan);
+		return;
+	}
+	speed = wf_pid_run(&drives_pid, temp);
+
+	DBG_LOTS("drives PID temp=%d.%.3d speed=%d\n",
+		 FIX32TOPRINT(temp), speed);
+
+	err = wf_control_set(drives_fan, speed);
+	if (err) {
+		printk(KERN_WARNING "windfarm: drive bay fan error %d\n", err);
+		failure_state |= FAILURE_FAN;
+	}
+}
+
+static void drives_setup_pid(void)
+{
+	/* first time initialize things */
+	s32 fmin = wf_control_get_min(drives_fan);
+	s32 fmax = wf_control_get_max(drives_fan);
+	struct wf_pid_param param = drives_param;
+
+	param.min = max(param.min, fmin);
+	param.max = min(param.max, fmax);
+	wf_pid_init(&drives_pid, &param);
+	drives_tick = 1;
+
+	pr_info("wf_pm72: Drive bay control loop started.\n");
+}
+
+static void set_fail_state(void)
+{
+	cpu_max_all_fans();
+
+	if (backside_fan)
+		wf_control_set_max(backside_fan);
+	if (slots_fan)
+		wf_control_set_max(slots_fan);
+	if (drives_fan)
+		wf_control_set_max(drives_fan);
+}
+
+static void pm72_tick(void)
+{
+	int i, last_failure;
+
+	if (!started) {
+		started = 1;
+		printk(KERN_INFO "windfarm: CPUs control loops started.\n");
+		for (i = 0; i < nr_chips; ++i) {
+			if (cpu_setup_pid(i) < 0) {
+				failure_state = FAILURE_PERM;
+				set_fail_state();
+				break;
+			}
+		}
+		DBG_LOTS("cpu_all_tmax=%d.%03d\n", FIX32TOPRINT(cpu_all_tmax));
+
+		backside_setup_pid();
+		drives_setup_pid();
+
+		/*
+		 * We don't have the right stuff to drive the PCI fan
+		 * so we fix it to a default value
+		 */
+		wf_control_set(slots_fan, SLOTS_FAN_DEFAULT_PWM);
+
+#ifdef HACKED_OVERTEMP
+		cpu_all_tmax = 60 << 16;
+#endif
+	}
+
+	/* Permanent failure, bail out */
+	if (failure_state & FAILURE_PERM)
+		return;
+
+	/*
+	 * Clear all failure bits except low overtemp which will be eventually
+	 * cleared by the control loop itself
+	 */
+	last_failure = failure_state;
+	failure_state &= FAILURE_LOW_OVERTEMP;
+	if (cpu_pid_combined)
+		cpu_fans_tick_combined();
+	else
+		cpu_fans_tick_split();
+	backside_fan_tick();
+	drives_fan_tick();
+
+	DBG_LOTS("  last_failure: 0x%x, failure_state: %x\n",
+		 last_failure, failure_state);
+
+	/* Check for failures. Any failure causes cpufreq clamping */
+	if (failure_state && last_failure == 0 && cpufreq_clamp)
+		wf_control_set_max(cpufreq_clamp);
+	if (failure_state == 0 && last_failure && cpufreq_clamp)
+		wf_control_set_min(cpufreq_clamp);
+
+	/* That's it for now, we might want to deal with other failures
+	 * differently in the future though
+	 */
+}
+
+static void pm72_new_control(struct wf_control *ct)
+{
+	bool all_controls;
+	bool had_pump = cpu_pumps[0] || cpu_pumps[1];
+
+	if (!strcmp(ct->name, "cpu-front-fan-0"))
+		cpu_front_fans[0] = ct;
+	else if (!strcmp(ct->name, "cpu-front-fan-1"))
+		cpu_front_fans[1] = ct;
+	else if (!strcmp(ct->name, "cpu-rear-fan-0"))
+		cpu_rear_fans[0] = ct;
+	else if (!strcmp(ct->name, "cpu-rear-fan-1"))
+		cpu_rear_fans[1] = ct;
+	else if (!strcmp(ct->name, "cpu-pump-0"))
+		cpu_pumps[0] = ct;
+	else if (!strcmp(ct->name, "cpu-pump-1"))
+		cpu_pumps[1] = ct;
+	else if (!strcmp(ct->name, "backside-fan"))
+		backside_fan = ct;
+	else if (!strcmp(ct->name, "slots-fan"))
+		slots_fan = ct;
+	else if (!strcmp(ct->name, "drive-bay-fan"))
+		drives_fan = ct;
+	else if (!strcmp(ct->name, "cpufreq-clamp"))
+		cpufreq_clamp = ct;
+
+	all_controls =
+		cpu_front_fans[0] &&
+		cpu_rear_fans[0] &&
+		backside_fan &&
+		slots_fan &&
+		drives_fan;
+	if (nr_chips > 1)
+		all_controls &=
+			cpu_front_fans[1] &&
+			cpu_rear_fans[1];
+	have_all_controls = all_controls;
+
+	if ((cpu_pumps[0] || cpu_pumps[1]) && !had_pump) {
+		pr_info("wf_pm72: Liquid cooling pump(s) detected,"
+			" using new algorithm !\n");
+		cpu_pid_combined = true;
+	}
+}
+
+
+static void pm72_new_sensor(struct wf_sensor *sr)
+{
+	bool all_sensors;
+
+	if (!strcmp(sr->name, "cpu-diode-temp-0"))
+		sens_cpu_temp[0] = sr;
+	else if (!strcmp(sr->name, "cpu-diode-temp-1"))
+		sens_cpu_temp[1] = sr;
+	else if (!strcmp(sr->name, "cpu-voltage-0"))
+		sens_cpu_volts[0] = sr;
+	else if (!strcmp(sr->name, "cpu-voltage-1"))
+		sens_cpu_volts[1] = sr;
+	else if (!strcmp(sr->name, "cpu-current-0"))
+		sens_cpu_amps[0] = sr;
+	else if (!strcmp(sr->name, "cpu-current-1"))
+		sens_cpu_amps[1] = sr;
+	else if (!strcmp(sr->name, "backside-temp"))
+		backside_temp = sr;
+	else if (!strcmp(sr->name, "hd-temp"))
+		drives_temp = sr;
+
+	all_sensors =
+		sens_cpu_temp[0] &&
+		sens_cpu_volts[0] &&
+		sens_cpu_amps[0] &&
+		backside_temp &&
+		drives_temp;
+	if (nr_chips > 1)
+		all_sensors &=
+			sens_cpu_temp[1] &&
+			sens_cpu_volts[1] &&
+			sens_cpu_amps[1];
+
+	have_all_sensors = all_sensors;
+}
+
+static int pm72_wf_notify(struct notifier_block *self,
+			  unsigned long event, void *data)
+{
+	switch (event) {
+	case WF_EVENT_NEW_SENSOR:
+		pm72_new_sensor(data);
+		break;
+	case WF_EVENT_NEW_CONTROL:
+		pm72_new_control(data);
+		break;
+	case WF_EVENT_TICK:
+		if (have_all_controls && have_all_sensors)
+			pm72_tick();
+	}
+	return 0;
+}
+
+static struct notifier_block pm72_events = {
+	.notifier_call = pm72_wf_notify,
+};
+
+static int wf_pm72_probe(struct platform_device *dev)
+{
+	wf_register_client(&pm72_events);
+	return 0;
+}
+
+static int __devexit wf_pm72_remove(struct platform_device *dev)
+{
+	wf_unregister_client(&pm72_events);
+
+	/* should release all sensors and controls */
+	return 0;
+}
+
+static struct platform_driver wf_pm72_driver = {
+	.probe	= wf_pm72_probe,
+	.remove	= wf_pm72_remove,
+	.driver	= {
+		.name = "windfarm",
+		.owner	= THIS_MODULE,
+	},
+};
+
+static int __init wf_pm72_init(void)
+{
+	struct device_node *cpu;
+	int i;
+
+	if (!of_machine_is_compatible("PowerMac7,2") &&
+	    !of_machine_is_compatible("PowerMac7,3"))
+		return -ENODEV;
+
+	/* Count the number of CPU cores */
+	nr_chips = 0;
+	for (cpu = NULL; (cpu = of_find_node_by_type(cpu, "cpu")) != NULL; )
+		++nr_chips;
+	if (nr_chips > NR_CHIPS)
+		nr_chips = NR_CHIPS;
+
+	pr_info("windfarm: Initializing for desktop G5 with %d chips\n",
+		nr_chips);
+
+	/* Get MPU data for each CPU */
+	for (i = 0; i < nr_chips; i++) {
+		cpu_mpu_data[i] = wf_get_mpu(i);
+		if (!cpu_mpu_data[i]) {
+			pr_err("wf_pm72: Failed to find MPU data for CPU %d\n", i);
+			return -ENXIO;
+		}
+	}
+
+#ifdef MODULE
+	request_module("windfarm_fcu_controls");
+	request_module("windfarm_lm75_sensor");
+	request_module("windfarm_ad7417_sensor");
+	request_module("windfarm_max6690_sensor");
+	request_module("windfarm_cpufreq_clamp");
+#endif /* MODULE */
+
+	platform_driver_register(&wf_pm72_driver);
+	return 0;
+}
+
+static void __exit wf_pm72_exit(void)
+{
+	platform_driver_unregister(&wf_pm72_driver);
+}
+
+module_init(wf_pm72_init);
+module_exit(wf_pm72_exit);
+
+MODULE_AUTHOR("Benjamin Herrenschmidt <benh@kernel.crashing.org>");
+MODULE_DESCRIPTION("Thermal control for AGP PowerMac G5s");
+MODULE_LICENSE("GPL");
+MODULE_ALIAS("platform:windfarm");
diff --git a/drivers/macintosh/windfarm_pm81.c b/drivers/macintosh/windfarm_pm81.c
index fc13d0f2663..990c87606be 100644
--- a/drivers/macintosh/windfarm_pm81.c
+++ b/drivers/macintosh/windfarm_pm81.c
@@ -302,13 +302,13 @@ static void wf_smu_create_sys_fans(void)
 	pid_param.interval = WF_SMU_SYS_FANS_INTERVAL;
 	pid_param.history_len = WF_SMU_SYS_FANS_HISTORY_SIZE;
 	pid_param.itarget = param->itarget;
-	pid_param.min = fan_system->ops->get_min(fan_system);
-	pid_param.max = fan_system->ops->get_max(fan_system);
+	pid_param.min = wf_control_get_min(fan_system);
+	pid_param.max = wf_control_get_max(fan_system);
 	if (fan_hd) {
 		pid_param.min =
-			max(pid_param.min,fan_hd->ops->get_min(fan_hd));
+			max(pid_param.min, wf_control_get_min(fan_hd));
 		pid_param.max =
-			min(pid_param.max,fan_hd->ops->get_max(fan_hd));
+			min(pid_param.max, wf_control_get_max(fan_hd));
 	}
 	wf_pid_init(&wf_smu_sys_fans->pid, &pid_param);
 
@@ -337,7 +337,7 @@ static void wf_smu_sys_fans_tick(struct wf_smu_sys_fans_state *st)
 	}
 	st->ticks = WF_SMU_SYS_FANS_INTERVAL;
 
-	rc = sensor_hd_temp->ops->get_value(sensor_hd_temp, &temp);
+	rc = wf_sensor_get(sensor_hd_temp, &temp);
 	if (rc) {
 		printk(KERN_WARNING "windfarm: HD temp sensor error %d\n",
 		       rc);
@@ -373,7 +373,7 @@ static void wf_smu_sys_fans_tick(struct wf_smu_sys_fans_state *st)
 	st->hd_setpoint = new_setpoint;
  readjust:
 	if (fan_system && wf_smu_failure_state == 0) {
-		rc = fan_system->ops->set_value(fan_system, st->sys_setpoint);
+		rc = wf_control_set(fan_system, st->sys_setpoint);
 		if (rc) {
 			printk(KERN_WARNING "windfarm: Sys fan error %d\n",
 			       rc);
@@ -381,7 +381,7 @@ static void wf_smu_sys_fans_tick(struct wf_smu_sys_fans_state *st)
 		}
 	}
 	if (fan_hd && wf_smu_failure_state == 0) {
-		rc = fan_hd->ops->set_value(fan_hd, st->hd_setpoint);
+		rc = wf_control_set(fan_hd, st->hd_setpoint);
 		if (rc) {
 			printk(KERN_WARNING "windfarm: HD fan error %d\n",
 			       rc);
@@ -447,8 +447,8 @@ static void wf_smu_create_cpu_fans(void)
 	pid_param.ttarget = tmax - tdelta;
 	pid_param.pmaxadj = maxpow - powadj;
 
-	pid_param.min = fan_cpu_main->ops->get_min(fan_cpu_main);
-	pid_param.max = fan_cpu_main->ops->get_max(fan_cpu_main);
+	pid_param.min = wf_control_get_min(fan_cpu_main);
+	pid_param.max = wf_control_get_max(fan_cpu_main);
 
 	wf_cpu_pid_init(&wf_smu_cpu_fans->pid, &pid_param);
 
@@ -481,7 +481,7 @@ static void wf_smu_cpu_fans_tick(struct wf_smu_cpu_fans_state *st)
 	}
 	st->ticks = WF_SMU_CPU_FANS_INTERVAL;
 
-	rc = sensor_cpu_temp->ops->get_value(sensor_cpu_temp, &temp);
+	rc = wf_sensor_get(sensor_cpu_temp, &temp);
 	if (rc) {
 		printk(KERN_WARNING "windfarm: CPU temp sensor error %d\n",
 		       rc);
@@ -489,7 +489,7 @@ static void wf_smu_cpu_fans_tick(struct wf_smu_cpu_fans_state *st)
 		return;
 	}
 
-	rc = sensor_cpu_power->ops->get_value(sensor_cpu_power, &power);
+	rc = wf_sensor_get(sensor_cpu_power, &power);
 	if (rc) {
 		printk(KERN_WARNING "windfarm: CPU power sensor error %d\n",
 		       rc);
@@ -525,8 +525,7 @@ static void wf_smu_cpu_fans_tick(struct wf_smu_cpu_fans_state *st)
 	st->cpu_setpoint = new_setpoint;
  readjust:
 	if (fan_cpu_main && wf_smu_failure_state == 0) {
-		rc = fan_cpu_main->ops->set_value(fan_cpu_main,
-						  st->cpu_setpoint);
+		rc = wf_control_set(fan_cpu_main, st->cpu_setpoint);
 		if (rc) {
 			printk(KERN_WARNING "windfarm: CPU main fan"
 			       " error %d\n", rc);
diff --git a/drivers/macintosh/windfarm_pm91.c b/drivers/macintosh/windfarm_pm91.c
index a9430ed4f36..7653603cb00 100644
--- a/drivers/macintosh/windfarm_pm91.c
+++ b/drivers/macintosh/windfarm_pm91.c
@@ -192,8 +192,8 @@ static void wf_smu_create_cpu_fans(void)
 	pid_param.ttarget = tmax - tdelta;
 	pid_param.pmaxadj = maxpow - powadj;
 
-	pid_param.min = fan_cpu_main->ops->get_min(fan_cpu_main);
-	pid_param.max = fan_cpu_main->ops->get_max(fan_cpu_main);
+	pid_param.min = wf_control_get_min(fan_cpu_main);
+	pid_param.max = wf_control_get_max(fan_cpu_main);
 
 	wf_cpu_pid_init(&wf_smu_cpu_fans->pid, &pid_param);
 
@@ -226,7 +226,7 @@ static void wf_smu_cpu_fans_tick(struct wf_smu_cpu_fans_state *st)
 	}
 	st->ticks = WF_SMU_CPU_FANS_INTERVAL;
 
-	rc = sensor_cpu_temp->ops->get_value(sensor_cpu_temp, &temp);
+	rc = wf_sensor_get(sensor_cpu_temp, &temp);
 	if (rc) {
 		printk(KERN_WARNING "windfarm: CPU temp sensor error %d\n",
 		       rc);
@@ -234,7 +234,7 @@ static void wf_smu_cpu_fans_tick(struct wf_smu_cpu_fans_state *st)
 		return;
 	}
 
-	rc = sensor_cpu_power->ops->get_value(sensor_cpu_power, &power);
+	rc = wf_sensor_get(sensor_cpu_power, &power);
 	if (rc) {
 		printk(KERN_WARNING "windfarm: CPU power sensor error %d\n",
 		       rc);
@@ -261,8 +261,7 @@ static void wf_smu_cpu_fans_tick(struct wf_smu_cpu_fans_state *st)
 	st->cpu_setpoint = new_setpoint;
  readjust:
 	if (fan_cpu_main && wf_smu_failure_state == 0) {
-		rc = fan_cpu_main->ops->set_value(fan_cpu_main,
-						  st->cpu_setpoint);
+		rc = wf_control_set(fan_cpu_main, st->cpu_setpoint);
 		if (rc) {
 			printk(KERN_WARNING "windfarm: CPU main fan"
 			       " error %d\n", rc);
@@ -270,8 +269,7 @@ static void wf_smu_cpu_fans_tick(struct wf_smu_cpu_fans_state *st)
 		}
 	}
 	if (fan_cpu_second && wf_smu_failure_state == 0) {
-		rc = fan_cpu_second->ops->set_value(fan_cpu_second,
-						    st->cpu_setpoint);
+		rc = wf_control_set(fan_cpu_second, st->cpu_setpoint);
 		if (rc) {
 			printk(KERN_WARNING "windfarm: CPU second fan"
 			       " error %d\n", rc);
@@ -279,8 +277,7 @@ static void wf_smu_cpu_fans_tick(struct wf_smu_cpu_fans_state *st)
 		}
 	}
 	if (fan_cpu_third && wf_smu_failure_state == 0) {
-		rc = fan_cpu_main->ops->set_value(fan_cpu_third,
-						  st->cpu_setpoint);
+		rc = wf_control_set(fan_cpu_third, st->cpu_setpoint);
 		if (rc) {
 			printk(KERN_WARNING "windfarm: CPU third fan"
 			       " error %d\n", rc);
@@ -312,8 +309,8 @@ static void wf_smu_create_drive_fans(void)
 
 	/* Fill PID params */
 	param.additive = (fan_hd->type == WF_CONTROL_RPM_FAN);
-	param.min = fan_hd->ops->get_min(fan_hd);
-	param.max = fan_hd->ops->get_max(fan_hd);
+	param.min = wf_control_get_min(fan_hd);
+	param.max = wf_control_get_max(fan_hd);
 	wf_pid_init(&wf_smu_drive_fans->pid, &param);
 
 	DBG("wf: Drive Fan control initialized.\n");
@@ -338,7 +335,7 @@ static void wf_smu_drive_fans_tick(struct wf_smu_drive_fans_state *st)
 	}
 	st->ticks = st->pid.param.interval;
 
-	rc = sensor_hd_temp->ops->get_value(sensor_hd_temp, &temp);
+	rc = wf_sensor_get(sensor_hd_temp, &temp);
 	if (rc) {
 		printk(KERN_WARNING "windfarm: HD temp sensor error %d\n",
 		       rc);
@@ -361,7 +358,7 @@ static void wf_smu_drive_fans_tick(struct wf_smu_drive_fans_state *st)
 	st->setpoint = new_setpoint;
  readjust:
 	if (fan_hd && wf_smu_failure_state == 0) {
-		rc = fan_hd->ops->set_value(fan_hd, st->setpoint);
+		rc = wf_control_set(fan_hd, st->setpoint);
 		if (rc) {
 			printk(KERN_WARNING "windfarm: HD fan error %d\n",
 			       rc);
@@ -393,8 +390,8 @@ static void wf_smu_create_slots_fans(void)
 
 	/* Fill PID params */
 	param.additive = (fan_slots->type == WF_CONTROL_RPM_FAN);
-	param.min = fan_slots->ops->get_min(fan_slots);
-	param.max = fan_slots->ops->get_max(fan_slots);
+	param.min = wf_control_get_min(fan_slots);
+	param.max = wf_control_get_max(fan_slots);
 	wf_pid_init(&wf_smu_slots_fans->pid, &param);
 
 	DBG("wf: Slots Fan control initialized.\n");
@@ -419,7 +416,7 @@ static void wf_smu_slots_fans_tick(struct wf_smu_slots_fans_state *st)
 	}
 	st->ticks = st->pid.param.interval;
 
-	rc = sensor_slots_power->ops->get_value(sensor_slots_power, &power);
+	rc = wf_sensor_get(sensor_slots_power, &power);
 	if (rc) {
 		printk(KERN_WARNING "windfarm: Slots power sensor error %d\n",
 		       rc);
@@ -444,7 +441,7 @@ static void wf_smu_slots_fans_tick(struct wf_smu_slots_fans_state *st)
 	st->setpoint = new_setpoint;
  readjust:
 	if (fan_slots && wf_smu_failure_state == 0) {
-		rc = fan_slots->ops->set_value(fan_slots, st->setpoint);
+		rc = wf_control_set(fan_slots, st->setpoint);
 		if (rc) {
 			printk(KERN_WARNING "windfarm: Slots fan error %d\n",
 			       rc);
diff --git a/drivers/macintosh/windfarm_rm31.c b/drivers/macintosh/windfarm_rm31.c
new file mode 100644
index 00000000000..3eca6d4b52f
--- /dev/null
+++ b/drivers/macintosh/windfarm_rm31.c
@@ -0,0 +1,740 @@
+/*
+ * Windfarm PowerMac thermal control.
+ * Control loops for RackMack3,1 (Xserve G5)
+ *
+ * Copyright (C) 2012 Benjamin Herrenschmidt, IBM Corp.
+ *
+ * Use and redistribute under the terms of the GNU GPL v2.
+ */
+#include <linux/types.h>
+#include <linux/errno.h>
+#include <linux/kernel.h>
+#include <linux/device.h>
+#include <linux/platform_device.h>
+#include <linux/reboot.h>
+#include <asm/prom.h>
+#include <asm/smu.h>
+
+#include "windfarm.h"
+#include "windfarm_pid.h"
+#include "windfarm_mpu.h"
+
+#define VERSION "1.0"
+
+#undef DEBUG
+#undef LOTSA_DEBUG
+
+#ifdef DEBUG
+#define DBG(args...)	printk(args)
+#else
+#define DBG(args...)	do { } while(0)
+#endif
+
+#ifdef LOTSA_DEBUG
+#define DBG_LOTS(args...)	printk(args)
+#else
+#define DBG_LOTS(args...)	do { } while(0)
+#endif
+
+/* define this to force CPU overtemp to 60 degree, useful for testing
+ * the overtemp code
+ */
+#undef HACKED_OVERTEMP
+
+/* We currently only handle 2 chips */
+#define NR_CHIPS	2
+#define NR_CPU_FANS	3 * NR_CHIPS
+
+/* Controls and sensors */
+static struct wf_sensor *sens_cpu_temp[NR_CHIPS];
+static struct wf_sensor *sens_cpu_volts[NR_CHIPS];
+static struct wf_sensor *sens_cpu_amps[NR_CHIPS];
+static struct wf_sensor *backside_temp;
+static struct wf_sensor *slots_temp;
+static struct wf_sensor *dimms_temp;
+
+static struct wf_control *cpu_fans[NR_CHIPS][3];
+static struct wf_control *backside_fan;
+static struct wf_control *slots_fan;
+static struct wf_control *cpufreq_clamp;
+
+/* We keep a temperature history for average calculation of 180s */
+#define CPU_TEMP_HIST_SIZE	180
+
+/* PID loop state */
+static const struct mpu_data *cpu_mpu_data[NR_CHIPS];
+static struct wf_cpu_pid_state cpu_pid[NR_CHIPS];
+static u32 cpu_thist[CPU_TEMP_HIST_SIZE];
+static int cpu_thist_pt;
+static s64 cpu_thist_total;
+static s32 cpu_all_tmax = 100 << 16;
+static struct wf_pid_state backside_pid;
+static int backside_tick;
+static struct wf_pid_state slots_pid;
+static int slots_tick;
+static int slots_speed;
+static struct wf_pid_state dimms_pid;
+static int dimms_output_clamp;
+
+static int nr_chips;
+static bool have_all_controls;
+static bool have_all_sensors;
+static bool started;
+
+static int failure_state;
+#define FAILURE_SENSOR		1
+#define FAILURE_FAN		2
+#define FAILURE_PERM		4
+#define FAILURE_LOW_OVERTEMP	8
+#define FAILURE_HIGH_OVERTEMP	16
+
+/* Overtemp values */
+#define LOW_OVER_AVERAGE	0
+#define LOW_OVER_IMMEDIATE	(10 << 16)
+#define LOW_OVER_CLEAR		((-10) << 16)
+#define HIGH_OVER_IMMEDIATE	(14 << 16)
+#define HIGH_OVER_AVERAGE	(10 << 16)
+#define HIGH_OVER_IMMEDIATE	(14 << 16)
+
+
+static void cpu_max_all_fans(void)
+{
+	int i;
+
+	/* We max all CPU fans in case of a sensor error. We also do the
+	 * cpufreq clamping now, even if it's supposedly done later by the
+	 * generic code anyway, we do it earlier here to react faster
+	 */
+	if (cpufreq_clamp)
+		wf_control_set_max(cpufreq_clamp);
+	for (i = 0; i < nr_chips; i++) {
+		if (cpu_fans[i][0])
+			wf_control_set_max(cpu_fans[i][0]);
+		if (cpu_fans[i][1])
+			wf_control_set_max(cpu_fans[i][1]);
+		if (cpu_fans[i][2])
+			wf_control_set_max(cpu_fans[i][2]);
+	}
+}
+
+static int cpu_check_overtemp(s32 temp)
+{
+	int new_state = 0;
+	s32 t_avg, t_old;
+	static bool first = true;
+
+	/* First check for immediate overtemps */
+	if (temp >= (cpu_all_tmax + LOW_OVER_IMMEDIATE)) {
+		new_state |= FAILURE_LOW_OVERTEMP;
+		if ((failure_state & FAILURE_LOW_OVERTEMP) == 0)
+			printk(KERN_ERR "windfarm: Overtemp due to immediate CPU"
+			       " temperature !\n");
+	}
+	if (temp >= (cpu_all_tmax + HIGH_OVER_IMMEDIATE)) {
+		new_state |= FAILURE_HIGH_OVERTEMP;
+		if ((failure_state & FAILURE_HIGH_OVERTEMP) == 0)
+			printk(KERN_ERR "windfarm: Critical overtemp due to"
+			       " immediate CPU temperature !\n");
+	}
+
+	/*
+	 * The first time around, initialize the array with the first
+	 * temperature reading
+	 */
+	if (first) {
+		int i;
+
+		cpu_thist_total = 0;
+		for (i = 0; i < CPU_TEMP_HIST_SIZE; i++) {
+			cpu_thist[i] = temp;
+			cpu_thist_total += temp;
+		}
+		first = false;
+	}
+
+	/*
+	 * We calculate a history of max temperatures and use that for the
+	 * overtemp management
+	 */
+	t_old = cpu_thist[cpu_thist_pt];
+	cpu_thist[cpu_thist_pt] = temp;
+	cpu_thist_pt = (cpu_thist_pt + 1) % CPU_TEMP_HIST_SIZE;
+	cpu_thist_total -= t_old;
+	cpu_thist_total += temp;
+	t_avg = cpu_thist_total / CPU_TEMP_HIST_SIZE;
+
+	DBG_LOTS("  t_avg = %d.%03d (out: %d.%03d, in: %d.%03d)\n",
+		 FIX32TOPRINT(t_avg), FIX32TOPRINT(t_old), FIX32TOPRINT(temp));
+
+	/* Now check for average overtemps */
+	if (t_avg >= (cpu_all_tmax + LOW_OVER_AVERAGE)) {
+		new_state |= FAILURE_LOW_OVERTEMP;
+		if ((failure_state & FAILURE_LOW_OVERTEMP) == 0)
+			printk(KERN_ERR "windfarm: Overtemp due to average CPU"
+			       " temperature !\n");
+	}
+	if (t_avg >= (cpu_all_tmax + HIGH_OVER_AVERAGE)) {
+		new_state |= FAILURE_HIGH_OVERTEMP;
+		if ((failure_state & FAILURE_HIGH_OVERTEMP) == 0)
+			printk(KERN_ERR "windfarm: Critical overtemp due to"
+			       " average CPU temperature !\n");
+	}
+
+	/* Now handle overtemp conditions. We don't currently use the windfarm
+	 * overtemp handling core as it's not fully suited to the needs of those
+	 * new machine. This will be fixed later.
+	 */
+	if (new_state) {
+		/* High overtemp -> immediate shutdown */
+		if (new_state & FAILURE_HIGH_OVERTEMP)
+			machine_power_off();
+		if ((failure_state & new_state) != new_state)
+			cpu_max_all_fans();
+		failure_state |= new_state;
+	} else if ((failure_state & FAILURE_LOW_OVERTEMP) &&
+		   (temp < (cpu_all_tmax + LOW_OVER_CLEAR))) {
+		printk(KERN_ERR "windfarm: Overtemp condition cleared !\n");
+		failure_state &= ~FAILURE_LOW_OVERTEMP;
+	}
+
+	return failure_state & (FAILURE_LOW_OVERTEMP | FAILURE_HIGH_OVERTEMP);
+}
+
+static int read_one_cpu_vals(int cpu, s32 *temp, s32 *power)
+{
+	s32 dtemp, volts, amps;
+	int rc;
+
+	/* Get diode temperature */
+	rc = wf_sensor_get(sens_cpu_temp[cpu], &dtemp);
+	if (rc) {
+		DBG("  CPU%d: temp reading error !\n", cpu);
+		return -EIO;
+	}
+	DBG_LOTS("  CPU%d: temp   = %d.%03d\n", cpu, FIX32TOPRINT((dtemp)));
+	*temp = dtemp;
+
+	/* Get voltage */
+	rc = wf_sensor_get(sens_cpu_volts[cpu], &volts);
+	if (rc) {
+		DBG("  CPU%d, volts reading error !\n", cpu);
+		return -EIO;
+	}
+	DBG_LOTS("  CPU%d: volts  = %d.%03d\n", cpu, FIX32TOPRINT((volts)));
+
+	/* Get current */
+	rc = wf_sensor_get(sens_cpu_amps[cpu], &amps);
+	if (rc) {
+		DBG("  CPU%d, current reading error !\n", cpu);
+		return -EIO;
+	}
+	DBG_LOTS("  CPU%d: amps   = %d.%03d\n", cpu, FIX32TOPRINT((amps)));
+
+	/* Calculate power */
+
+	/* Scale voltage and current raw sensor values according to fixed scales
+	 * obtained in Darwin and calculate power from I and V
+	 */
+	*power = (((u64)volts) * ((u64)amps)) >> 16;
+
+	DBG_LOTS("  CPU%d: power  = %d.%03d\n", cpu, FIX32TOPRINT((*power)));
+
+	return 0;
+
+}
+
+static void cpu_fans_tick(void)
+{
+	int err, cpu, i;
+	s32 speed, temp, power, t_max = 0;
+
+	DBG_LOTS("* cpu fans_tick_split()\n");
+
+	for (cpu = 0; cpu < nr_chips; ++cpu) {
+		struct wf_cpu_pid_state *sp = &cpu_pid[cpu];
+
+		/* Read current speed */
+		wf_control_get(cpu_fans[cpu][0], &sp->target);
+
+		err = read_one_cpu_vals(cpu, &temp, &power);
+		if (err) {
+			failure_state |= FAILURE_SENSOR;
+			cpu_max_all_fans();
+			return;
+		}
+
+		/* Keep track of highest temp */
+		t_max = max(t_max, temp);
+
+		/* Handle possible overtemps */
+		if (cpu_check_overtemp(t_max))
+			return;
+
+		/* Run PID */
+		wf_cpu_pid_run(sp, power, temp);
+
+		DBG_LOTS("  CPU%d: target = %d RPM\n", cpu, sp->target);
+
+		/* Apply DIMMs clamp */
+		speed = max(sp->target, dimms_output_clamp);
+
+		/* Apply result to all cpu fans */
+		for (i = 0; i < 3; i++) {
+			err = wf_control_set(cpu_fans[cpu][i], speed);
+			if (err) {
+				pr_warning("wf_rm31: Fan %s reports error %d\n",
+					   cpu_fans[cpu][i]->name, err);
+				failure_state |= FAILURE_FAN;
+			}
+		}
+	}
+}
+
+/* Implementation... */
+static int cpu_setup_pid(int cpu)
+{
+	struct wf_cpu_pid_param pid;
+	const struct mpu_data *mpu = cpu_mpu_data[cpu];
+	s32 tmax, ttarget, ptarget;
+	int fmin, fmax, hsize;
+
+	/* Get PID params from the appropriate MPU EEPROM */
+	tmax = mpu->tmax << 16;
+	ttarget = mpu->ttarget << 16;
+	ptarget = ((s32)(mpu->pmaxh - mpu->padjmax)) << 16;
+
+	DBG("wf_72: CPU%d ttarget = %d.%03d, tmax = %d.%03d\n",
+	    cpu, FIX32TOPRINT(ttarget), FIX32TOPRINT(tmax));
+
+	/* We keep a global tmax for overtemp calculations */
+	if (tmax < cpu_all_tmax)
+		cpu_all_tmax = tmax;
+
+	/* Set PID min/max by using the rear fan min/max */
+	fmin = wf_control_get_min(cpu_fans[cpu][0]);
+	fmax = wf_control_get_max(cpu_fans[cpu][0]);
+	DBG("wf_72: CPU%d max RPM range = [%d..%d]\n", cpu, fmin, fmax);
+
+	/* History size */
+	hsize = min_t(int, mpu->tguardband, WF_PID_MAX_HISTORY);
+	DBG("wf_72: CPU%d history size = %d\n", cpu, hsize);
+
+	/* Initialize PID loop */
+	pid.interval	= 1;	/* seconds */
+	pid.history_len = hsize;
+	pid.gd		= mpu->pid_gd;
+	pid.gp		= mpu->pid_gp;
+	pid.gr		= mpu->pid_gr;
+	pid.tmax	= tmax;
+	pid.ttarget	= ttarget;
+	pid.pmaxadj	= ptarget;
+	pid.min		= fmin;
+	pid.max		= fmax;
+
+	wf_cpu_pid_init(&cpu_pid[cpu], &pid);
+	cpu_pid[cpu].target = 4000;
+	
+	return 0;
+}
+
+/* Backside/U3 fan */
+static struct wf_pid_param backside_param = {
+	.interval	= 1,
+	.history_len	= 2,
+	.gd		= 0x00500000,
+	.gp		= 0x0004cccc,
+	.gr		= 0,
+	.itarget	= 70 << 16,
+	.additive	= 0,
+	.min		= 20,
+	.max		= 100,
+};
+
+/* DIMMs temperature (clamp the backside fan) */
+static struct wf_pid_param dimms_param = {
+	.interval	= 1,
+	.history_len	= 20,
+	.gd		= 0,
+	.gp		= 0,
+	.gr		= 0x06553600,
+	.itarget	= 50 << 16,
+	.additive	= 0,
+	.min		= 4000,
+	.max		= 14000,
+};
+
+static void backside_fan_tick(void)
+{
+	s32 temp, dtemp;
+	int speed, dspeed, fan_min;
+	int err;
+
+	if (!backside_fan || !backside_temp || !dimms_temp || !backside_tick)
+		return;
+	if (--backside_tick > 0)
+		return;
+	backside_tick = backside_pid.param.interval;
+
+	DBG_LOTS("* backside fans tick\n");
+
+	/* Update fan speed from actual fans */
+	err = wf_control_get(backside_fan, &speed);
+	if (!err)
+		backside_pid.target = speed;
+
+	err = wf_sensor_get(backside_temp, &temp);
+	if (err) {
+		printk(KERN_WARNING "windfarm: U3 temp sensor error %d\n",
+		       err);
+		failure_state |= FAILURE_SENSOR;
+		wf_control_set_max(backside_fan);
+		return;
+	}
+	speed = wf_pid_run(&backside_pid, temp);
+
+	DBG_LOTS("backside PID temp=%d.%.3d speed=%d\n",
+		 FIX32TOPRINT(temp), speed);
+
+	err = wf_sensor_get(dimms_temp, &dtemp);
+	if (err) {
+		printk(KERN_WARNING "windfarm: DIMMs temp sensor error %d\n",
+		       err);
+		failure_state |= FAILURE_SENSOR;
+		wf_control_set_max(backside_fan);
+		return;
+	}
+	dspeed = wf_pid_run(&dimms_pid, dtemp);
+	dimms_output_clamp = dspeed;
+
+	fan_min = (dspeed * 100) / 14000;
+	fan_min = max(fan_min, backside_param.min);
+	speed = max(speed, fan_min);
+
+	err = wf_control_set(backside_fan, speed);
+	if (err) {
+		printk(KERN_WARNING "windfarm: backside fan error %d\n", err);
+		failure_state |= FAILURE_FAN;
+	}
+}
+
+static void backside_setup_pid(void)
+{
+	/* first time initialize things */
+	s32 fmin = wf_control_get_min(backside_fan);
+	s32 fmax = wf_control_get_max(backside_fan);
+	struct wf_pid_param param;
+
+	param = backside_param;
+	param.min = max(param.min, fmin);
+	param.max = min(param.max, fmax);
+	wf_pid_init(&backside_pid, &param);
+
+	param = dimms_param;
+	wf_pid_init(&dimms_pid, &param);
+
+	backside_tick = 1;
+
+	pr_info("wf_rm31: Backside control loop started.\n");
+}
+
+/* Slots fan */
+static const struct wf_pid_param slots_param = {
+	.interval	= 5,
+	.history_len	= 2,
+	.gd		= 30 << 20,
+	.gp		= 5 << 20,
+	.gr		= 0,
+	.itarget	= 40 << 16,
+	.additive	= 1,
+	.min		= 300,
+	.max		= 4000,
+};
+
+static void slots_fan_tick(void)
+{
+	s32 temp;
+	int speed;
+	int err;
+
+	if (!slots_fan || !slots_temp || !slots_tick)
+		return;
+	if (--slots_tick > 0)
+		return;
+	slots_tick = slots_pid.param.interval;
+
+	DBG_LOTS("* slots fans tick\n");
+
+	err = wf_sensor_get(slots_temp, &temp);
+	if (err) {
+		pr_warning("wf_rm31: slots temp sensor error %d\n", err);
+		failure_state |= FAILURE_SENSOR;
+		wf_control_set_max(slots_fan);
+		return;
+	}
+	speed = wf_pid_run(&slots_pid, temp);
+
+	DBG_LOTS("slots PID temp=%d.%.3d speed=%d\n",
+		 FIX32TOPRINT(temp), speed);
+
+	slots_speed = speed;
+	err = wf_control_set(slots_fan, speed);
+	if (err) {
+		printk(KERN_WARNING "windfarm: slots bay fan error %d\n", err);
+		failure_state |= FAILURE_FAN;
+	}
+}
+
+static void slots_setup_pid(void)
+{
+	/* first time initialize things */
+	s32 fmin = wf_control_get_min(slots_fan);
+	s32 fmax = wf_control_get_max(slots_fan);
+	struct wf_pid_param param = slots_param;
+
+	param.min = max(param.min, fmin);
+	param.max = min(param.max, fmax);
+	wf_pid_init(&slots_pid, &param);
+	slots_tick = 1;
+
+	pr_info("wf_rm31: Slots control loop started.\n");
+}
+
+static void set_fail_state(void)
+{
+	cpu_max_all_fans();
+
+	if (backside_fan)
+		wf_control_set_max(backside_fan);
+	if (slots_fan)
+		wf_control_set_max(slots_fan);
+}
+
+static void rm31_tick(void)
+{
+	int i, last_failure;
+
+	if (!started) {
+		started = 1;
+		printk(KERN_INFO "windfarm: CPUs control loops started.\n");
+		for (i = 0; i < nr_chips; ++i) {
+			if (cpu_setup_pid(i) < 0) {
+				failure_state = FAILURE_PERM;
+				set_fail_state();
+				break;
+			}
+		}
+		DBG_LOTS("cpu_all_tmax=%d.%03d\n", FIX32TOPRINT(cpu_all_tmax));
+
+		backside_setup_pid();
+		slots_setup_pid();
+
+#ifdef HACKED_OVERTEMP
+		cpu_all_tmax = 60 << 16;
+#endif
+	}
+
+	/* Permanent failure, bail out */
+	if (failure_state & FAILURE_PERM)
+		return;
+
+	/*
+	 * Clear all failure bits except low overtemp which will be eventually
+	 * cleared by the control loop itself
+	 */
+	last_failure = failure_state;
+	failure_state &= FAILURE_LOW_OVERTEMP;
+	backside_fan_tick();
+	slots_fan_tick();
+
+	/* We do CPUs last because they can be clamped high by
+	 * DIMM temperature
+	 */
+	cpu_fans_tick();
+
+	DBG_LOTS("  last_failure: 0x%x, failure_state: %x\n",
+		 last_failure, failure_state);
+
+	/* Check for failures. Any failure causes cpufreq clamping */
+	if (failure_state && last_failure == 0 && cpufreq_clamp)
+		wf_control_set_max(cpufreq_clamp);
+	if (failure_state == 0 && last_failure && cpufreq_clamp)
+		wf_control_set_min(cpufreq_clamp);
+
+	/* That's it for now, we might want to deal with other failures
+	 * differently in the future though
+	 */
+}
+
+static void rm31_new_control(struct wf_control *ct)
+{
+	bool all_controls;
+
+	if (!strcmp(ct->name, "cpu-fan-a-0"))
+		cpu_fans[0][0] = ct;
+	else if (!strcmp(ct->name, "cpu-fan-b-0"))
+		cpu_fans[0][1] = ct;
+	else if (!strcmp(ct->name, "cpu-fan-c-0"))
+		cpu_fans[0][2] = ct;
+	else if (!strcmp(ct->name, "cpu-fan-a-1"))
+		cpu_fans[1][0] = ct;
+	else if (!strcmp(ct->name, "cpu-fan-b-1"))
+		cpu_fans[1][1] = ct;
+	else if (!strcmp(ct->name, "cpu-fan-c-1"))
+		cpu_fans[1][2] = ct;
+	else if (!strcmp(ct->name, "backside-fan"))
+		backside_fan = ct;
+	else if (!strcmp(ct->name, "slots-fan"))
+		slots_fan = ct;
+	else if (!strcmp(ct->name, "cpufreq-clamp"))
+		cpufreq_clamp = ct;
+
+	all_controls =
+		cpu_fans[0][0] &&
+		cpu_fans[0][1] &&
+		cpu_fans[0][2] &&
+		backside_fan &&
+		slots_fan;
+	if (nr_chips > 1)
+		all_controls &=
+			cpu_fans[1][0] &&
+			cpu_fans[1][1] &&
+			cpu_fans[1][2];
+	have_all_controls = all_controls;
+}
+
+
+static void rm31_new_sensor(struct wf_sensor *sr)
+{
+	bool all_sensors;
+
+	if (!strcmp(sr->name, "cpu-diode-temp-0"))
+		sens_cpu_temp[0] = sr;
+	else if (!strcmp(sr->name, "cpu-diode-temp-1"))
+		sens_cpu_temp[1] = sr;
+	else if (!strcmp(sr->name, "cpu-voltage-0"))
+		sens_cpu_volts[0] = sr;
+	else if (!strcmp(sr->name, "cpu-voltage-1"))
+		sens_cpu_volts[1] = sr;
+	else if (!strcmp(sr->name, "cpu-current-0"))
+		sens_cpu_amps[0] = sr;
+	else if (!strcmp(sr->name, "cpu-current-1"))
+		sens_cpu_amps[1] = sr;
+	else if (!strcmp(sr->name, "backside-temp"))
+		backside_temp = sr;
+	else if (!strcmp(sr->name, "slots-temp"))
+		slots_temp = sr;
+	else if (!strcmp(sr->name, "dimms-temp"))
+		dimms_temp = sr;
+
+	all_sensors =
+		sens_cpu_temp[0] &&
+		sens_cpu_volts[0] &&
+		sens_cpu_amps[0] &&
+		backside_temp &&
+		slots_temp &&
+		dimms_temp;
+	if (nr_chips > 1)
+		all_sensors &=
+			sens_cpu_temp[1] &&
+			sens_cpu_volts[1] &&
+			sens_cpu_amps[1];
+
+	have_all_sensors = all_sensors;
+}
+
+static int rm31_wf_notify(struct notifier_block *self,
+			  unsigned long event, void *data)
+{
+	switch (event) {
+	case WF_EVENT_NEW_SENSOR:
+		rm31_new_sensor(data);
+		break;
+	case WF_EVENT_NEW_CONTROL:
+		rm31_new_control(data);
+		break;
+	case WF_EVENT_TICK:
+		if (have_all_controls && have_all_sensors)
+			rm31_tick();
+	}
+	return 0;
+}
+
+static struct notifier_block rm31_events = {
+	.notifier_call = rm31_wf_notify,
+};
+
+static int wf_rm31_probe(struct platform_device *dev)
+{
+	wf_register_client(&rm31_events);
+	return 0;
+}
+
+static int __devexit wf_rm31_remove(struct platform_device *dev)
+{
+	wf_unregister_client(&rm31_events);
+
+	/* should release all sensors and controls */
+	return 0;
+}
+
+static struct platform_driver wf_rm31_driver = {
+	.probe	= wf_rm31_probe,
+	.remove	= wf_rm31_remove,
+	.driver	= {
+		.name = "windfarm",
+		.owner	= THIS_MODULE,
+	},
+};
+
+static int __init wf_rm31_init(void)
+{
+	struct device_node *cpu;
+	int i;
+
+	if (!of_machine_is_compatible("RackMac3,1"))
+		return -ENODEV;
+
+	/* Count the number of CPU cores */
+	nr_chips = 0;
+	for (cpu = NULL; (cpu = of_find_node_by_type(cpu, "cpu")) != NULL; )
+		++nr_chips;
+	if (nr_chips > NR_CHIPS)
+		nr_chips = NR_CHIPS;
+
+	pr_info("windfarm: Initializing for desktop G5 with %d chips\n",
+		nr_chips);
+
+	/* Get MPU data for each CPU */
+	for (i = 0; i < nr_chips; i++) {
+		cpu_mpu_data[i] = wf_get_mpu(i);
+		if (!cpu_mpu_data[i]) {
+			pr_err("wf_rm31: Failed to find MPU data for CPU %d\n", i);
+			return -ENXIO;
+		}
+	}
+
+#ifdef MODULE
+	request_module("windfarm_fcu_controls");
+	request_module("windfarm_lm75_sensor");
+	request_module("windfarm_lm87_sensor");
+	request_module("windfarm_ad7417_sensor");
+	request_module("windfarm_max6690_sensor");
+	request_module("windfarm_cpufreq_clamp");
+#endif /* MODULE */
+
+	platform_driver_register(&wf_rm31_driver);
+	return 0;
+}
+
+static void __exit wf_rm31_exit(void)
+{
+	platform_driver_unregister(&wf_rm31_driver);
+}
+
+module_init(wf_rm31_init);
+module_exit(wf_rm31_exit);
+
+MODULE_AUTHOR("Benjamin Herrenschmidt <benh@kernel.crashing.org>");
+MODULE_DESCRIPTION("Thermal control for Xserve G5");
+MODULE_LICENSE("GPL");
+MODULE_ALIAS("platform:windfarm");
diff --git a/drivers/macintosh/windfarm_smu_controls.c b/drivers/macintosh/windfarm_smu_controls.c
index 3c2be5193fd..c155a54e863 100644
--- a/drivers/macintosh/windfarm_smu_controls.c
+++ b/drivers/macintosh/windfarm_smu_controls.c
@@ -172,7 +172,6 @@ static struct smu_fan_control *smu_fan_create(struct device_node *node,
 
 	fct->fan_type = pwm_fan;
 	fct->ctrl.type = pwm_fan ? WF_CONTROL_PWM_FAN : WF_CONTROL_RPM_FAN;
-	sysfs_attr_init(&fct->ctrl.attr.attr);
 
 	/* We use the name & location here the same way we do for SMU sensors,
 	 * see the comment in windfarm_smu_sensors.c. The locations are a bit
diff --git a/drivers/macintosh/windfarm_smu_sat.c b/drivers/macintosh/windfarm_smu_sat.c
index 65a8ff3e1f8..426e810233d 100644
--- a/drivers/macintosh/windfarm_smu_sat.c
+++ b/drivers/macintosh/windfarm_smu_sat.c
@@ -20,7 +20,7 @@
 
 #include "windfarm.h"
 
-#define VERSION "0.2"
+#define VERSION "1.0"
 
 #define DEBUG
 
@@ -34,11 +34,12 @@
 #define MAX_AGE		msecs_to_jiffies(800)
 
 struct wf_sat {
+	struct kref		ref;
 	int			nr;
-	atomic_t		refcnt;
 	struct mutex		mutex;
 	unsigned long		last_read; /* jiffies when cache last updated */
 	u8			cache[16];
+	struct list_head	sensors;
 	struct i2c_client	*i2c;
 	struct device_node	*node;
 };
@@ -46,11 +47,12 @@ struct wf_sat {
 static struct wf_sat *sats[2];
 
 struct wf_sat_sensor {
-	int		index;
-	int		index2;		/* used for power sensors */
-	int		shift;
-	struct wf_sat	*sat;
-	struct wf_sensor sens;
+	struct list_head	link;
+	int			index;
+	int			index2;		/* used for power sensors */
+	int			shift;
+	struct wf_sat		*sat;
+	struct wf_sensor 	sens;
 };
 
 #define wf_to_sat(c)	container_of(c, struct wf_sat_sensor, sens)
@@ -142,7 +144,7 @@ static int wf_sat_read_cache(struct wf_sat *sat)
 	return 0;
 }
 
-static int wf_sat_get(struct wf_sensor *sr, s32 *value)
+static int wf_sat_sensor_get(struct wf_sensor *sr, s32 *value)
 {
 	struct wf_sat_sensor *sens = wf_to_sat(sr);
 	struct wf_sat *sat = sens->sat;
@@ -175,62 +177,34 @@ static int wf_sat_get(struct wf_sensor *sr, s32 *value)
 	return err;
 }
 
-static void wf_sat_release(struct wf_sensor *sr)
+static void wf_sat_release(struct kref *ref)
+{
+	struct wf_sat *sat = container_of(ref, struct wf_sat, ref);
+
+	if (sat->nr >= 0)
+		sats[sat->nr] = NULL;
+	kfree(sat);
+}
+
+static void wf_sat_sensor_release(struct wf_sensor *sr)
 {
 	struct wf_sat_sensor *sens = wf_to_sat(sr);
 	struct wf_sat *sat = sens->sat;
 
-	if (atomic_dec_and_test(&sat->refcnt)) {
-		if (sat->nr >= 0)
-			sats[sat->nr] = NULL;
-		kfree(sat);
-	}
 	kfree(sens);
+	kref_put(&sat->ref, wf_sat_release);
 }
 
 static struct wf_sensor_ops wf_sat_ops = {
-	.get_value	= wf_sat_get,
-	.release	= wf_sat_release,
+	.get_value	= wf_sat_sensor_get,
+	.release	= wf_sat_sensor_release,
 	.owner		= THIS_MODULE,
 };
 
-static struct i2c_driver wf_sat_driver;
-
-static void wf_sat_create(struct i2c_adapter *adapter, struct device_node *dev)
-{
-	struct i2c_board_info info;
-	struct i2c_client *client;
-	const u32 *reg;
-	u8 addr;
-
-	reg = of_get_property(dev, "reg", NULL);
-	if (reg == NULL)
-		return;
-	addr = *reg;
-	DBG(KERN_DEBUG "wf_sat: creating sat at address %x\n", addr);
-
-	memset(&info, 0, sizeof(struct i2c_board_info));
-	info.addr = (addr >> 1) & 0x7f;
-	info.platform_data = dev;
-	strlcpy(info.type, "wf_sat", I2C_NAME_SIZE);
-
-	client = i2c_new_device(adapter, &info);
-	if (client == NULL) {
-		printk(KERN_ERR "windfarm: failed to attach smu-sat to i2c\n");
-		return;
-	}
-
-	/*
-	 * Let i2c-core delete that device on driver removal.
-	 * This is safe because i2c-core holds the core_lock mutex for us.
-	 */
-	list_add_tail(&client->detected, &wf_sat_driver.clients);
-}
-
 static int wf_sat_probe(struct i2c_client *client,
 			const struct i2c_device_id *id)
 {
-	struct device_node *dev = client->dev.platform_data;
+	struct device_node *dev = client->dev.of_node;
 	struct wf_sat *sat;
 	struct wf_sat_sensor *sens;
 	const u32 *reg;
@@ -246,9 +220,10 @@ static int wf_sat_probe(struct i2c_client *client,
 		return -ENOMEM;
 	sat->nr = -1;
 	sat->node = of_node_get(dev);
-	atomic_set(&sat->refcnt, 0);
+	kref_init(&sat->ref);
 	mutex_init(&sat->mutex);
 	sat->i2c = client;
+	INIT_LIST_HEAD(&sat->sensors);
 	i2c_set_clientdata(client, sat);
 
 	vsens[0] = vsens[1] = -1;
@@ -310,14 +285,15 @@ static int wf_sat_probe(struct i2c_client *client,
 		sens->index2 = -1;
 		sens->shift = shift;
 		sens->sat = sat;
-		atomic_inc(&sat->refcnt);
 		sens->sens.ops = &wf_sat_ops;
 		sens->sens.name = (char *) (sens + 1);
-		snprintf(sens->sens.name, 16, "%s-%d", name, cpu);
+		snprintf((char *)sens->sens.name, 16, "%s-%d", name, cpu);
 
-		if (wf_register_sensor(&sens->sens)) {
-			atomic_dec(&sat->refcnt);
+		if (wf_register_sensor(&sens->sens))
 			kfree(sens);
+		else {
+			list_add(&sens->link, &sat->sensors);
+			kref_get(&sat->ref);
 		}
 	}
 
@@ -336,14 +312,15 @@ static int wf_sat_probe(struct i2c_client *client,
 		sens->index2 = isens[core];
 		sens->shift = 0;
 		sens->sat = sat;
-		atomic_inc(&sat->refcnt);
 		sens->sens.ops = &wf_sat_ops;
 		sens->sens.name = (char *) (sens + 1);
-		snprintf(sens->sens.name, 16, "cpu-power-%d", cpu);
+		snprintf((char *)sens->sens.name, 16, "cpu-power-%d", cpu);
 
-		if (wf_register_sensor(&sens->sens)) {
-			atomic_dec(&sat->refcnt);
+		if (wf_register_sensor(&sens->sens))
 			kfree(sens);
+		else {
+			list_add(&sens->link, &sat->sensors);
+			kref_get(&sat->ref);
 		}
 	}
 
@@ -353,42 +330,35 @@ static int wf_sat_probe(struct i2c_client *client,
 	return 0;
 }
 
-static int wf_sat_attach(struct i2c_adapter *adapter)
-{
-	struct device_node *busnode, *dev = NULL;
-	struct pmac_i2c_bus *bus;
-
-	bus = pmac_i2c_adapter_to_bus(adapter);
-	if (bus == NULL)
-		return -ENODEV;
-	busnode = pmac_i2c_get_bus_node(bus);
-
-	while ((dev = of_get_next_child(busnode, dev)) != NULL)
-		if (of_device_is_compatible(dev, "smu-sat"))
-			wf_sat_create(adapter, dev);
-	return 0;
-}
-
 static int wf_sat_remove(struct i2c_client *client)
 {
 	struct wf_sat *sat = i2c_get_clientdata(client);
+	struct wf_sat_sensor *sens;
 
-	/* XXX TODO */
-
+	/* release sensors */
+	while(!list_empty(&sat->sensors)) {
+		sens = list_first_entry(&sat->sensors,
+					struct wf_sat_sensor, link);
+		list_del(&sens->link);
+		wf_unregister_sensor(&sens->sens);
+	}
 	sat->i2c = NULL;
+	i2c_set_clientdata(client, NULL);
+	kref_put(&sat->ref, wf_sat_release);
+
 	return 0;
 }
 
 static const struct i2c_device_id wf_sat_id[] = {
-	{ "wf_sat", 0 },
+	{ "MAC,smu-sat", 0 },
 	{ }
 };
+MODULE_DEVICE_TABLE(i2c, wf_sat_id);
 
 static struct i2c_driver wf_sat_driver = {
 	.driver = {
 		.name		= "wf_smu_sat",
 	},
-	.attach_adapter	= wf_sat_attach,
 	.probe		= wf_sat_probe,
 	.remove		= wf_sat_remove,
 	.id_table	= wf_sat_id,
@@ -399,15 +369,13 @@ static int __init sat_sensors_init(void)
 	return i2c_add_driver(&wf_sat_driver);
 }
 
-#if 0	/* uncomment when module_exit() below is uncommented */
 static void __exit sat_sensors_exit(void)
 {
 	i2c_del_driver(&wf_sat_driver);
 }
-#endif
 
 module_init(sat_sensors_init);
-/*module_exit(sat_sensors_exit); Uncomment when cleanup is implemented */
+module_exit(sat_sensors_exit);
 
 MODULE_AUTHOR("Paul Mackerras <paulus@samba.org>");
 MODULE_DESCRIPTION("SMU satellite sensors for PowerMac thermal control");
diff --git a/drivers/mca/Kconfig b/drivers/mca/Kconfig
deleted file mode 100644
index a7a0220ab4b..00000000000
--- a/drivers/mca/Kconfig
+++ /dev/null
@@ -1,14 +0,0 @@
-config MCA_LEGACY
-	bool "Legacy MCA API Support"
-	depends on MCA
-	help
-	  This compiles in support for the old slot based MCA API.  If you
-	  have an unconverted MCA driver, you will need to say Y here.  It
-	  is safe to say Y anyway.
-
-config MCA_PROC_FS
-	bool "Support for the mca entry in /proc"
-	depends on MCA_LEGACY && PROC_FS
-	help
-	  If you want the old style /proc/mca directory in addition to the
-	  new style sysfs say Y here.
diff --git a/drivers/mca/Makefile b/drivers/mca/Makefile
deleted file mode 100644
index 0794b122520..00000000000
--- a/drivers/mca/Makefile
+++ /dev/null
@@ -1,7 +0,0 @@
-# Makefile for the Linux MCA bus support
-
-obj-y	:= mca-bus.o mca-device.o mca-driver.o
-
-obj-$(CONFIG_MCA_PROC_FS)	+= mca-proc.o
-obj-$(CONFIG_MCA_LEGACY)	+= mca-legacy.o
-
diff --git a/drivers/mca/mca-bus.c b/drivers/mca/mca-bus.c
deleted file mode 100644
index ada5ebbaa25..00000000000
--- a/drivers/mca/mca-bus.c
+++ /dev/null
@@ -1,169 +0,0 @@
-/* -*- mode: c; c-basic-offset: 8 -*- */
-
-/*
- * MCA bus support functions for sysfs.
- *
- * (C) 2002 James Bottomley <James.Bottomley@HansenPartnership.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/kernel.h>
-#include <linux/device.h>
-#include <linux/mca.h>
-#include <linux/module.h>
-#include <linux/init.h>
-#include <linux/slab.h>
-
-/* Very few machines have more than one MCA bus.  However, there are
- * those that do (Voyager 35xx/5xxx), so we do it this way for future
- * expansion.  None that I know have more than 2 */
-static struct mca_bus *mca_root_busses[MAX_MCA_BUSSES];
-
-#define MCA_DEVINFO(i,s) { .pos = i, .name = s }
-
-struct mca_device_info {
-	short pos_id;		/* the 2 byte pos id for this card */
-	char name[50];
-};
-
-static int mca_bus_match (struct device *dev, struct device_driver *drv)
-{
-	struct mca_device *mca_dev = to_mca_device (dev);
-	struct mca_driver *mca_drv = to_mca_driver (drv);
-	const unsigned short *mca_ids = mca_drv->id_table;
-	int i = 0;
-
-	if (mca_ids) {
-		for(i = 0; mca_ids[i]; i++) {
-			if (mca_ids[i] == mca_dev->pos_id) {
-				mca_dev->index = i;
-				return 1;
-			}
-		}
-	}
-	/* If the integrated id is present, treat it as though it were an
-	 * additional id in the id_table (it can't be because by definition,
-	 * integrated id's overflow a short */
-	if (mca_drv->integrated_id && mca_dev->pos_id ==
-	    mca_drv->integrated_id) {
-		mca_dev->index = i;
-		return 1;
-	}
-	return 0;
-}
-
-struct bus_type mca_bus_type = {
-	.name  = "MCA",
-	.match = mca_bus_match,
-};
-EXPORT_SYMBOL (mca_bus_type);
-
-static ssize_t mca_show_pos_id(struct device *dev, struct device_attribute *attr, char *buf)
-{
-	/* four digits, \n and trailing \0 */
-	struct mca_device *mca_dev = to_mca_device(dev);
-	int len;
-
-	if(mca_dev->pos_id < MCA_DUMMY_POS_START)
-		len = sprintf(buf, "%04x\n", mca_dev->pos_id);
-	else
-		len = sprintf(buf, "none\n");
-	return len;
-}
-static ssize_t mca_show_pos(struct device *dev, struct device_attribute *attr, char *buf)
-{
-	/* enough for 8 two byte hex chars plus space and new line */
-	int j, len=0;
-	struct mca_device *mca_dev = to_mca_device(dev);
-
-	for(j=0; j<8; j++)
-		len += sprintf(buf+len, "%02x ", mca_dev->pos[j]);
-	/* change last trailing space to new line */
-	buf[len-1] = '\n';
-	return len;
-}
-
-static DEVICE_ATTR(id, S_IRUGO, mca_show_pos_id, NULL);
-static DEVICE_ATTR(pos, S_IRUGO, mca_show_pos, NULL);
-
-int __init mca_register_device(int bus, struct mca_device *mca_dev)
-{
-	struct mca_bus *mca_bus = mca_root_busses[bus];
-	int rc;
-
-	mca_dev->dev.parent = &mca_bus->dev;
-	mca_dev->dev.bus = &mca_bus_type;
-	dev_set_name(&mca_dev->dev, "%02d:%02X", bus, mca_dev->slot);
-	mca_dev->dma_mask = mca_bus->default_dma_mask;
-	mca_dev->dev.dma_mask = &mca_dev->dma_mask;
-	mca_dev->dev.coherent_dma_mask = mca_dev->dma_mask;
-
-	rc = device_register(&mca_dev->dev);
-	if (rc)
-		goto err_out;
-
-	rc = device_create_file(&mca_dev->dev, &dev_attr_id);
-	if (rc) goto err_out_devreg;
-	rc = device_create_file(&mca_dev->dev, &dev_attr_pos);
-	if (rc) goto err_out_id;
-
-	return 1;
-
-err_out_id:
-	device_remove_file(&mca_dev->dev, &dev_attr_id);
-err_out_devreg:
-	device_unregister(&mca_dev->dev);
-err_out:
-	return 0;
-}
-
-/* */
-struct mca_bus * __devinit mca_attach_bus(int bus)
-{
-	struct mca_bus *mca_bus;
-
-	if (unlikely(mca_root_busses[bus] != NULL)) {
-		/* This should never happen, but just in case */
-		printk(KERN_EMERG "MCA tried to add already existing bus %d\n",
-		       bus);
-		dump_stack();
-		return NULL;
-	}
-
-	mca_bus = kzalloc(sizeof(struct mca_bus), GFP_KERNEL);
-	if (!mca_bus)
-		return NULL;
-
-	dev_set_name(&mca_bus->dev, "mca%d", bus);
-	sprintf(mca_bus->name,"Host %s MCA Bridge", bus ? "Secondary" : "Primary");
-	if (device_register(&mca_bus->dev)) {
-		kfree(mca_bus);
-		return NULL;
-	}
-
-	mca_root_busses[bus] = mca_bus;
-
-	return mca_bus;
-}
-
-int __init mca_system_init (void)
-{
-	return bus_register(&mca_bus_type);
-}
diff --git a/drivers/mca/mca-device.c b/drivers/mca/mca-device.c
deleted file mode 100644
index e7adf89fae4..00000000000
--- a/drivers/mca/mca-device.c
+++ /dev/null
@@ -1,218 +0,0 @@
-/* -*- mode: c; c-basic-offset: 8 -*- */
-
-/*
- * MCA device support functions
- *
- * These functions support the ongoing device access API.
- *
- * (C) 2002 James Bottomley <James.Bottomley@HansenPartnership.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/module.h>
-#include <linux/device.h>
-#include <linux/mca.h>
-#include <linux/string.h>
-
-/**
- *	mca_device_read_stored_pos - read POS register from stored data
- *	@mca_dev: device to read from
- *	@reg:  register to read from
- *
- *	Fetch a POS value that was stored at boot time by the kernel
- *	when it scanned the MCA space. The register value is returned.
- *	Missing or invalid registers report 0.
- */
-unsigned char mca_device_read_stored_pos(struct mca_device *mca_dev, int reg)
-{
-	if(reg < 0 || reg >= 8)
-		return 0;
-
-	return mca_dev->pos[reg];
-}
-EXPORT_SYMBOL(mca_device_read_stored_pos);
-
-/**
- *	mca_device_read_pos - read POS register from card
- *	@mca_dev: device to read from
- *	@reg:  register to read from
- *
- *	Fetch a POS value directly from the hardware to obtain the
- *	current value. This is much slower than
- *	mca_device_read_stored_pos and may not be invoked from
- *	interrupt context. It handles the deep magic required for
- *	onboard devices transparently.
- */
-unsigned char mca_device_read_pos(struct mca_device *mca_dev, int reg)
-{
-	struct mca_bus *mca_bus = to_mca_bus(mca_dev->dev.parent);
-
-	return mca_bus->f.mca_read_pos(mca_dev, reg);
-
-	return 	mca_dev->pos[reg];
-}
-EXPORT_SYMBOL(mca_device_read_pos);
-
-
-/**
- *	mca_device_write_pos - read POS register from card
- *	@mca_dev: device to write pos register to
- *	@reg:  register to write to
- *	@byte: byte to write to the POS registers
- *
- *	Store a POS value directly to the hardware. You should not
- *	normally need to use this function and should have a very good
- *	knowledge of MCA bus before you do so. Doing this wrongly can
- *	damage the hardware.
- *
- *	This function may not be used from interrupt context.
- *
- */
-void mca_device_write_pos(struct mca_device *mca_dev, int reg,
-			  unsigned char byte)
-{
-	struct mca_bus *mca_bus = to_mca_bus(mca_dev->dev.parent);
-
-	mca_bus->f.mca_write_pos(mca_dev, reg, byte);
-}
-EXPORT_SYMBOL(mca_device_write_pos);
-
-/**
- *	mca_device_transform_irq - transform the ADF obtained IRQ
- *	@mca_device: device whose irq needs transforming
- *	@irq: input irq from ADF
- *
- *	MCA Adapter Definition Files (ADF) contain irq, ioport, memory
- *	etc. definitions.  In systems with more than one bus, these need
- *	to be transformed through bus mapping functions to get the real
- *	system global quantities.
- *
- *	This function transforms the interrupt number and returns the
- *	transformed system global interrupt
- */
-int mca_device_transform_irq(struct mca_device *mca_dev, int irq)
-{
-	struct mca_bus *mca_bus = to_mca_bus(mca_dev->dev.parent);
-
-	return mca_bus->f.mca_transform_irq(mca_dev, irq);
-}
-EXPORT_SYMBOL(mca_device_transform_irq);
-
-/**
- *	mca_device_transform_ioport - transform the ADF obtained I/O port
- *	@mca_device: device whose port needs transforming
- *	@ioport: input I/O port from ADF
- *
- *	MCA Adapter Definition Files (ADF) contain irq, ioport, memory
- *	etc. definitions.  In systems with more than one bus, these need
- *	to be transformed through bus mapping functions to get the real
- *	system global quantities.
- *
- *	This function transforms the I/O port number and returns the
- *	transformed system global port number.
- *
- *	This transformation can be assumed to be linear for port ranges.
- */
-int mca_device_transform_ioport(struct mca_device *mca_dev, int port)
-{
-	struct mca_bus *mca_bus = to_mca_bus(mca_dev->dev.parent);
-
-	return mca_bus->f.mca_transform_ioport(mca_dev, port);
-}
-EXPORT_SYMBOL(mca_device_transform_ioport);
-
-/**
- *	mca_device_transform_memory - transform the ADF obtained memory
- *	@mca_device: device whose memory region needs transforming
- *	@mem: memory region start from ADF
- *
- *	MCA Adapter Definition Files (ADF) contain irq, ioport, memory
- *	etc. definitions.  In systems with more than one bus, these need
- *	to be transformed through bus mapping functions to get the real
- *	system global quantities.
- *
- *	This function transforms the memory region start and returns the
- *	transformed system global memory region (physical).
- *
- *	This transformation can be assumed to be linear for region ranges.
- */
-void *mca_device_transform_memory(struct mca_device *mca_dev, void *mem)
-{
-	struct mca_bus *mca_bus = to_mca_bus(mca_dev->dev.parent);
-
-	return mca_bus->f.mca_transform_memory(mca_dev, mem);
-}
-EXPORT_SYMBOL(mca_device_transform_memory);
-
-
-/**
- *	mca_device_claimed - check if claimed by driver
- *	@mca_dev:	device to check
- *
- *	Returns 1 if the slot has been claimed by a driver
- */
-
-int mca_device_claimed(struct mca_device *mca_dev)
-{
-	return mca_dev->driver_loaded;
-}
-EXPORT_SYMBOL(mca_device_claimed);
-
-/**
- *	mca_device_set_claim - set the claim value of the driver
- *	@mca_dev:	device to set value for
- *	@val:		claim value to set (1 claimed, 0 unclaimed)
- */
-void mca_device_set_claim(struct mca_device *mca_dev, int val)
-{
-	mca_dev->driver_loaded = val;
-}
-EXPORT_SYMBOL(mca_device_set_claim);
-
-/**
- *	mca_device_status - get the status of the device
- *	@mca_device:	device to get
- *
- *	returns an enumeration of the device status:
- *
- *	MCA_ADAPTER_NORMAL	adapter is OK.
- *	MCA_ADAPTER_NONE	no adapter at device (should never happen).
- *	MCA_ADAPTER_DISABLED	adapter is disabled.
- *	MCA_ADAPTER_ERROR	adapter cannot be initialised.
- */
-enum MCA_AdapterStatus mca_device_status(struct mca_device *mca_dev)
-{
-	return mca_dev->status;
-}
-EXPORT_SYMBOL(mca_device_status);
-
-/**
- *	mca_device_set_name - set the name of the device
- *	@mca_device:	device to set the name of
- *	@name:		name to set
- */
-void mca_device_set_name(struct mca_device *mca_dev, const char *name)
-{
-	if(!mca_dev)
-		return;
-
-	strlcpy(mca_dev->name, name, sizeof(mca_dev->name));
-}
-EXPORT_SYMBOL(mca_device_set_name);
diff --git a/drivers/mca/mca-driver.c b/drivers/mca/mca-driver.c
deleted file mode 100644
index 32cd39bcc71..00000000000
--- a/drivers/mca/mca-driver.c
+++ /dev/null
@@ -1,63 +0,0 @@
-/* -*- mode: c; c-basic-offset: 8 -*- */
-
-/*
- * MCA driver support functions for sysfs.
- *
- * (C) 2002 James Bottomley <James.Bottomley@HansenPartnership.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/device.h>
-#include <linux/mca.h>
-#include <linux/module.h>
-
-int mca_register_driver(struct mca_driver *mca_drv)
-{
-	int r;
-
-	if (MCA_bus) {
-		mca_drv->driver.bus = &mca_bus_type;
-		if ((r = driver_register(&mca_drv->driver)) < 0)
-			return r;
-		mca_drv->integrated_id = 0;
-	}
-
-	return 0;
-}
-EXPORT_SYMBOL(mca_register_driver);
-
-int mca_register_driver_integrated(struct mca_driver *mca_driver,
-				   int integrated_id)
-{
-	int r = mca_register_driver(mca_driver);
-
-	if (!r)
-		mca_driver->integrated_id = integrated_id;
-
-	return r;
-}
-EXPORT_SYMBOL(mca_register_driver_integrated);
-
-void mca_unregister_driver(struct mca_driver *mca_drv)
-{
-	if (MCA_bus)
-		driver_unregister(&mca_drv->driver);
-}
-EXPORT_SYMBOL(mca_unregister_driver);
diff --git a/drivers/mca/mca-legacy.c b/drivers/mca/mca-legacy.c
deleted file mode 100644
index 494f0c2001f..00000000000
--- a/drivers/mca/mca-legacy.c
+++ /dev/null
@@ -1,329 +0,0 @@
-/* -*- mode: c; c-basic-offset: 8 -*- */
-
-/*
- * MCA bus support functions for legacy (2.4) API.
- *
- * Legacy API means the API that operates in terms of MCA slot number
- *
- * (C) 2002 James Bottomley <James.Bottomley@HansenPartnership.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/module.h>
-#include <linux/device.h>
-#include <linux/mca-legacy.h>
-#include <asm/io.h>
-
-/* NOTE: This structure is stack allocated */
-struct mca_find_adapter_info {
-	int			id;
-	int			slot;
-	struct mca_device	*mca_dev;
-};
-
-/* The purpose of this iterator is to loop over all the devices and
- * find the one with the smallest slot number that's just greater than
- * or equal to the required slot with a matching id */
-static int mca_find_adapter_callback(struct device *dev, void *data)
-{
-	struct mca_find_adapter_info *info = data;
-	struct mca_device *mca_dev = to_mca_device(dev);
-
-	if(mca_dev->pos_id != info->id)
-		return 0;
-
-	if(mca_dev->slot < info->slot)
-		return 0;
-
-	if(!info->mca_dev || info->mca_dev->slot >= mca_dev->slot)
-		info->mca_dev = mca_dev;
-
-	return 0;
-}
-
-/**
- *	mca_find_adapter - scan for adapters
- *	@id:	MCA identification to search for
- *	@start:	starting slot
- *
- *	Search the MCA configuration for adapters matching the 16bit
- *	ID given. The first time it should be called with start as zero
- *	and then further calls made passing the return value of the
- *	previous call until %MCA_NOTFOUND is returned.
- *
- *	Disabled adapters are not reported.
- */
-
-int mca_find_adapter(int id, int start)
-{
-	struct mca_find_adapter_info info;
-
-	if(id == 0xffff)
-		return MCA_NOTFOUND;
-
-	info.slot = start;
-	info.id = id;
-	info.mca_dev = NULL;
-
-	for(;;) {
-		bus_for_each_dev(&mca_bus_type, NULL, &info, mca_find_adapter_callback);
-
-		if(info.mca_dev == NULL)
-			return MCA_NOTFOUND;
-
-		if(info.mca_dev->status != MCA_ADAPTER_DISABLED)
-			break;
-
-		/* OK, found adapter but it was disabled.  Go around
-		 * again, excluding the slot we just found */
-
-		info.slot = info.mca_dev->slot + 1;
-		info.mca_dev = NULL;
-	}
-		
-	return info.mca_dev->slot;
-}
-EXPORT_SYMBOL(mca_find_adapter);
-
-/*--------------------------------------------------------------------*/
-
-/**
- *	mca_find_unused_adapter - scan for unused adapters
- *	@id:	MCA identification to search for
- *	@start:	starting slot
- *
- *	Search the MCA configuration for adapters matching the 16bit
- *	ID given. The first time it should be called with start as zero
- *	and then further calls made passing the return value of the
- *	previous call until %MCA_NOTFOUND is returned.
- *
- *	Adapters that have been claimed by drivers and those that
- *	are disabled are not reported. This function thus allows a driver
- *	to scan for further cards when some may already be driven.
- */
-
-int mca_find_unused_adapter(int id, int start)
-{
-	struct mca_find_adapter_info info = { 0 };
-
-	if (!MCA_bus || id == 0xffff)
-		return MCA_NOTFOUND;
-
-	info.slot = start;
-	info.id = id;
-	info.mca_dev = NULL;
-
-	for(;;) {
-		bus_for_each_dev(&mca_bus_type, NULL, &info, mca_find_adapter_callback);
-
-		if(info.mca_dev == NULL)
-			return MCA_NOTFOUND;
-
-		if(info.mca_dev->status != MCA_ADAPTER_DISABLED
-		   && !info.mca_dev->driver_loaded)
-			break;
-
-		/* OK, found adapter but it was disabled or already in
-		 * use.  Go around again, excluding the slot we just
-		 * found */
-
-		info.slot = info.mca_dev->slot + 1;
-		info.mca_dev = NULL;
-	}
-		
-	return info.mca_dev->slot;
-}
-EXPORT_SYMBOL(mca_find_unused_adapter);
-
-/* NOTE: stack allocated structure */
-struct mca_find_device_by_slot_info {
-	int			slot;
-	struct mca_device 	*mca_dev;
-};
-
-static int mca_find_device_by_slot_callback(struct device *dev, void *data)
-{
-	struct mca_find_device_by_slot_info *info = data;
-	struct mca_device *mca_dev = to_mca_device(dev);
-
-	if(mca_dev->slot == info->slot)
-		info->mca_dev = mca_dev;
-
-	return 0;
-}
-
-struct mca_device *mca_find_device_by_slot(int slot)
-{
-	struct mca_find_device_by_slot_info info;
-
-	info.slot = slot;
-	info.mca_dev = NULL;
-
-	bus_for_each_dev(&mca_bus_type, NULL, &info, mca_find_device_by_slot_callback);
-
-	return info.mca_dev;
-}
-
-/**
- *	mca_read_stored_pos - read POS register from boot data
- *	@slot: slot number to read from
- *	@reg:  register to read from
- *
- *	Fetch a POS value that was stored at boot time by the kernel
- *	when it scanned the MCA space. The register value is returned.
- *	Missing or invalid registers report 0.
- */
-unsigned char mca_read_stored_pos(int slot, int reg)
-{
-	struct mca_device *mca_dev = mca_find_device_by_slot(slot);
-
-	if(!mca_dev)
-		return 0;
-
-	return mca_device_read_stored_pos(mca_dev, reg);
-}
-EXPORT_SYMBOL(mca_read_stored_pos);
-
-
-/**
- *	mca_read_pos - read POS register from card
- *	@slot: slot number to read from
- *	@reg:  register to read from
- *
- *	Fetch a POS value directly from the hardware to obtain the
- *	current value. This is much slower than mca_read_stored_pos and
- *	may not be invoked from interrupt context. It handles the
- *	deep magic required for onboard devices transparently.
- */
-
-unsigned char mca_read_pos(int slot, int reg)
-{
-	struct mca_device *mca_dev = mca_find_device_by_slot(slot);
-
-	if(!mca_dev)
-		return 0;
-
-	return mca_device_read_pos(mca_dev, reg);
-}
-EXPORT_SYMBOL(mca_read_pos);
-
-		
-/**
- *	mca_write_pos - read POS register from card
- *	@slot: slot number to read from
- *	@reg:  register to read from
- *	@byte: byte to write to the POS registers
- *
- *	Store a POS value directly from the hardware. You should not
- *	normally need to use this function and should have a very good
- *	knowledge of MCA bus before you do so. Doing this wrongly can
- *	damage the hardware.
- *
- *	This function may not be used from interrupt context.
- *
- *	Note that this a technically a Bad Thing, as IBM tech stuff says
- *	you should only set POS values through their utilities.
- *	However, some devices such as the 3c523 recommend that you write
- *	back some data to make sure the configuration is consistent.
- *	I'd say that IBM is right, but I like my drivers to work.
- *
- *	This function can't do checks to see if multiple devices end up
- *	with the same resources, so you might see magic smoke if someone
- *	screws up.
- */
-
-void mca_write_pos(int slot, int reg, unsigned char byte)
-{
-	struct mca_device *mca_dev = mca_find_device_by_slot(slot);
-
-	if(!mca_dev)
-		return;
-
-	mca_device_write_pos(mca_dev, reg, byte);
-}
-EXPORT_SYMBOL(mca_write_pos);
-
-/**
- *	mca_set_adapter_name - Set the description of the card
- *	@slot: slot to name
- *	@name: text string for the namen
- *
- *	This function sets the name reported via /proc for this
- *	adapter slot. This is for user information only. Setting a
- *	name deletes any previous name.
- */
-
-void mca_set_adapter_name(int slot, char* name)
-{
-	struct mca_device *mca_dev = mca_find_device_by_slot(slot);
-
-	if(!mca_dev)
-		return;
-
-	mca_device_set_name(mca_dev, name);
-}
-EXPORT_SYMBOL(mca_set_adapter_name);
-
-/**
- *	mca_mark_as_used - claim an MCA device
- *	@slot:	slot to claim
- *	FIXME:  should we make this threadsafe
- *
- *	Claim an MCA slot for a device driver. If the
- *	slot is already taken the function returns 1,
- *	if it is not taken it is claimed and 0 is
- *	returned.
- */
-
-int mca_mark_as_used(int slot)
-{
-	struct mca_device *mca_dev = mca_find_device_by_slot(slot);
-
-	if(!mca_dev)
-		/* FIXME: this is actually a severe error */
-		return 1;
-
-	if(mca_device_claimed(mca_dev))
-		return 1;
-
-	mca_device_set_claim(mca_dev, 1);
-
-	return 0;
-}
-EXPORT_SYMBOL(mca_mark_as_used);
-
-/**
- *	mca_mark_as_unused - release an MCA device
- *	@slot:	slot to claim
- *
- *	Release the slot for other drives to use.
- */
-
-void mca_mark_as_unused(int slot)
-{
-	struct mca_device *mca_dev = mca_find_device_by_slot(slot);
-
-	if(!mca_dev)
-		return;
-
-	mca_device_set_claim(mca_dev, 0);
-}
-EXPORT_SYMBOL(mca_mark_as_unused);
-
diff --git a/drivers/mca/mca-proc.c b/drivers/mca/mca-proc.c
deleted file mode 100644
index 81ea0d377bf..00000000000
--- a/drivers/mca/mca-proc.c
+++ /dev/null
@@ -1,249 +0,0 @@
-/* -*- mode: c; c-basic-offset: 8 -*- */
-
-/*
- * MCA bus support functions for the proc fs.
- *
- * NOTE: this code *requires* the legacy MCA api.
- *
- * Legacy API means the API that operates in terms of MCA slot number
- *
- * (C) 2002 James Bottomley <James.Bottomley@HansenPartnership.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/module.h>
-#include <linux/init.h>
-#include <linux/proc_fs.h>
-#include <linux/mca.h>
-
-static int get_mca_info_helper(struct mca_device *mca_dev, char *page, int len)
-{
-	int j;
-
-	for(j=0; j<8; j++)
-		len += sprintf(page+len, "%02x ",
-			       mca_dev ? mca_dev->pos[j] : 0xff);
-	len += sprintf(page+len, " %s\n", mca_dev ? mca_dev->name : "");
-	return len;
-}
-
-static int get_mca_info(char *page, char **start, off_t off,
-			int count, int *eof, void *data)
-{
-	int i, len = 0;
-
-	if(MCA_bus) {
-		struct mca_device *mca_dev;
-		/* Format POS registers of eight MCA slots */
-
-		for(i=0; i<MCA_MAX_SLOT_NR; i++) {
-			mca_dev = mca_find_device_by_slot(i);
-
-			len += sprintf(page+len, "Slot %d: ", i+1);
-			len = get_mca_info_helper(mca_dev, page, len);
-		}
-
-		/* Format POS registers of integrated video subsystem */
-
-		mca_dev = mca_find_device_by_slot(MCA_INTEGVIDEO);
-		len += sprintf(page+len, "Video : ");
-		len = get_mca_info_helper(mca_dev, page, len);
-
-		/* Format POS registers of integrated SCSI subsystem */
-
-		mca_dev = mca_find_device_by_slot(MCA_INTEGSCSI);
-		len += sprintf(page+len, "SCSI  : ");
-		len = get_mca_info_helper(mca_dev, page, len);
-
-		/* Format POS registers of motherboard */
-
-		mca_dev = mca_find_device_by_slot(MCA_MOTHERBOARD);
-		len += sprintf(page+len, "Planar: ");
-		len = get_mca_info_helper(mca_dev, page, len);
-	} else {
-		/* Leave it empty if MCA not detected - this should *never*
-		 * happen!
-		 */
-	}
-
-	if (len <= off+count) *eof = 1;
-	*start = page + off;
-	len -= off;
-	if (len>count) len = count;
-	if (len<0) len = 0;
-	return len;
-}
-
-/*--------------------------------------------------------------------*/
-
-static int mca_default_procfn(char* buf, struct mca_device *mca_dev)
-{
-	int len = 0, i;
-	int slot = mca_dev->slot;
-
-	/* Print out the basic information */
-
-	if(slot < MCA_MAX_SLOT_NR) {
-		len += sprintf(buf+len, "Slot: %d\n", slot+1);
-	} else if(slot == MCA_INTEGSCSI) {
-		len += sprintf(buf+len, "Integrated SCSI Adapter\n");
-	} else if(slot == MCA_INTEGVIDEO) {
-		len += sprintf(buf+len, "Integrated Video Adapter\n");
-	} else if(slot == MCA_MOTHERBOARD) {
-		len += sprintf(buf+len, "Motherboard\n");
-	}
-	if (mca_dev->name[0]) {
-
-		/* Drivers might register a name without /proc handler... */
-
-		len += sprintf(buf+len, "Adapter Name: %s\n",
-			       mca_dev->name);
-	} else {
-		len += sprintf(buf+len, "Adapter Name: Unknown\n");
-	}
-	len += sprintf(buf+len, "Id: %02x%02x\n",
-		mca_dev->pos[1], mca_dev->pos[0]);
-	len += sprintf(buf+len, "Enabled: %s\nPOS: ",
-		mca_device_status(mca_dev) == MCA_ADAPTER_NORMAL ?
-			"Yes" : "No");
-	for(i=0; i<8; i++) {
-		len += sprintf(buf+len, "%02x ", mca_dev->pos[i]);
-	}
-	len += sprintf(buf+len, "\nDriver Installed: %s",
-		mca_device_claimed(mca_dev) ? "Yes" : "No");
-	buf[len++] = '\n';
-	buf[len] = 0;
-
-	return len;
-} /* mca_default_procfn() */
-
-static int get_mca_machine_info(char* page, char **start, off_t off,
-				 int count, int *eof, void *data)
-{
-	int len = 0;
-
-	len += sprintf(page+len, "Model Id: 0x%x\n", machine_id);
-	len += sprintf(page+len, "Submodel Id: 0x%x\n", machine_submodel_id);
-	len += sprintf(page+len, "BIOS Revision: 0x%x\n", BIOS_revision);
-
-	if (len <= off+count) *eof = 1;
-	*start = page + off;
-	len -= off;
-	if (len>count) len = count;
-	if (len<0) len = 0;
-	return len;
-}
-
-static int mca_read_proc(char *page, char **start, off_t off,
-				 int count, int *eof, void *data)
-{
-	struct mca_device *mca_dev = (struct mca_device *)data;
-	int len = 0;
-
-	/* Get the standard info */
-
-	len = mca_default_procfn(page, mca_dev);
-
-	/* Do any device-specific processing, if there is any */
-
-	if(mca_dev->procfn) {
-		len += mca_dev->procfn(page+len, mca_dev->slot,
-				       mca_dev->proc_dev);
-	}
-	if (len <= off+count) *eof = 1;
-	*start = page + off;
-	len -= off;
-	if (len>count) len = count;
-	if (len<0) len = 0;
-	return len;
-} /* mca_read_proc() */
-
-/*--------------------------------------------------------------------*/
-
-void __init mca_do_proc_init(void)
-{
-	int i;
-	struct proc_dir_entry *proc_mca;
-	struct proc_dir_entry* node = NULL;
-	struct mca_device *mca_dev;
-
-	proc_mca = proc_mkdir("mca", NULL);
-	create_proc_read_entry("pos",0,proc_mca,get_mca_info,NULL);
-	create_proc_read_entry("machine",0,proc_mca,get_mca_machine_info,NULL);
-
-	/* Initialize /proc/mca entries for existing adapters */
-
-	for(i = 0; i < MCA_NUMADAPTERS; i++) {
-		enum MCA_AdapterStatus status;
-		mca_dev = mca_find_device_by_slot(i);
-		if(!mca_dev)
-			continue;
-
-		mca_dev->procfn = NULL;
-
-		if(i < MCA_MAX_SLOT_NR) sprintf(mca_dev->procname,"slot%d", i+1);
-		else if(i == MCA_INTEGVIDEO) sprintf(mca_dev->procname,"video");
-		else if(i == MCA_INTEGSCSI) sprintf(mca_dev->procname,"scsi");
-		else if(i == MCA_MOTHERBOARD) sprintf(mca_dev->procname,"planar");
-
-		status = mca_device_status(mca_dev);
-		if (status != MCA_ADAPTER_NORMAL &&
-		    status != MCA_ADAPTER_DISABLED)
-			continue;
-
-		node = create_proc_read_entry(mca_dev->procname, 0, proc_mca,
-					      mca_read_proc, (void *)mca_dev);
-
-		if(node == NULL) {
-			printk("Failed to allocate memory for MCA proc-entries!");
-			return;
-		}
-	}
-
-} /* mca_do_proc_init() */
-
-/**
- *	mca_set_adapter_procfn - Set the /proc callback
- *	@slot: slot to configure
- *	@procfn: callback function to call for /proc
- *	@dev: device information passed to the callback
- *
- *	This sets up an information callback for /proc/mca/slot?.  The
- *	function is called with the buffer, slot, and device pointer (or
- *	some equally informative context information, or nothing, if you
- *	prefer), and is expected to put useful information into the
- *	buffer.  The adapter name, ID, and POS registers get printed
- *	before this is called though, so don't do it again.
- *
- *	This should be called with a %NULL @procfn when a module
- *	unregisters, thus preventing kernel crashes and other such
- *	nastiness.
- */
-
-void mca_set_adapter_procfn(int slot, MCA_ProcFn procfn, void* proc_dev)
-{
-	struct mca_device *mca_dev = mca_find_device_by_slot(slot);
-
-	if(!mca_dev)
-		return;
-
-	mca_dev->procfn = procfn;
-	mca_dev->proc_dev = proc_dev;
-}
-EXPORT_SYMBOL(mca_set_adapter_procfn);
diff --git a/drivers/md/bitmap.c b/drivers/md/bitmap.c
index 17e2b472e16..15dbe03117e 100644
--- a/drivers/md/bitmap.c
+++ b/drivers/md/bitmap.c
@@ -45,7 +45,7 @@ static inline char *bmname(struct bitmap *bitmap)
  * if we find our page, we increment the page's refcount so that it stays
  * allocated while we're using it
  */
-static int bitmap_checkpage(struct bitmap *bitmap,
+static int bitmap_checkpage(struct bitmap_counts *bitmap,
 			    unsigned long page, int create)
 __releases(bitmap->lock)
 __acquires(bitmap->lock)
@@ -76,8 +76,7 @@ __acquires(bitmap->lock)
 	spin_lock_irq(&bitmap->lock);
 
 	if (mappage == NULL) {
-		pr_debug("%s: bitmap map page allocation failed, hijacking\n",
-			 bmname(bitmap));
+		pr_debug("md/bitmap: map page allocation failed, hijacking\n");
 		/* failed - set the hijacked flag so that we can use the
 		 * pointer as a counter */
 		if (!bitmap->bp[page].map)
@@ -100,7 +99,7 @@ __acquires(bitmap->lock)
 /* if page is completely empty, put it back on the free list, or dealloc it */
 /* if page was hijacked, unmark the flag so it might get alloced next time */
 /* Note: lock should be held when calling this */
-static void bitmap_checkfree(struct bitmap *bitmap, unsigned long page)
+static void bitmap_checkfree(struct bitmap_counts *bitmap, unsigned long page)
 {
 	char *ptr;
 
@@ -130,22 +129,14 @@ static void bitmap_checkfree(struct bitmap *bitmap, unsigned long page)
  */
 
 /* IO operations when bitmap is stored near all superblocks */
-static struct page *read_sb_page(struct mddev *mddev, loff_t offset,
-				 struct page *page,
-				 unsigned long index, int size)
+static int read_sb_page(struct mddev *mddev, loff_t offset,
+			struct page *page,
+			unsigned long index, int size)
 {
 	/* choose a good rdev and read the page from there */
 
 	struct md_rdev *rdev;
 	sector_t target;
-	int did_alloc = 0;
-
-	if (!page) {
-		page = alloc_page(GFP_KERNEL);
-		if (!page)
-			return ERR_PTR(-ENOMEM);
-		did_alloc = 1;
-	}
 
 	rdev_for_each(rdev, mddev) {
 		if (! test_bit(In_sync, &rdev->flags)
@@ -158,15 +149,10 @@ static struct page *read_sb_page(struct mddev *mddev, loff_t offset,
 				 roundup(size, bdev_logical_block_size(rdev->bdev)),
 				 page, READ, true)) {
 			page->index = index;
-			attach_page_buffers(page, NULL); /* so that free_buffer will
-							  * quietly no-op */
-			return page;
+			return 0;
 		}
 	}
-	if (did_alloc)
-		put_page(page);
-	return ERR_PTR(-EIO);
-
+	return -EIO;
 }
 
 static struct md_rdev *next_active_rdev(struct md_rdev *rdev, struct mddev *mddev)
@@ -208,6 +194,7 @@ static int write_sb_page(struct bitmap *bitmap, struct page *page, int wait)
 	struct md_rdev *rdev = NULL;
 	struct block_device *bdev;
 	struct mddev *mddev = bitmap->mddev;
+	struct bitmap_storage *store = &bitmap->storage;
 
 	while ((rdev = next_active_rdev(rdev, mddev)) != NULL) {
 		int size = PAGE_SIZE;
@@ -215,9 +202,13 @@ static int write_sb_page(struct bitmap *bitmap, struct page *page, int wait)
 
 		bdev = (rdev->meta_bdev) ? rdev->meta_bdev : rdev->bdev;
 
-		if (page->index == bitmap->file_pages-1)
-			size = roundup(bitmap->last_page_size,
+		if (page->index == store->file_pages-1) {
+			int last_page_size = store->bytes & (PAGE_SIZE-1);
+			if (last_page_size == 0)
+				last_page_size = PAGE_SIZE;
+			size = roundup(last_page_size,
 				       bdev_logical_block_size(bdev));
+		}
 		/* Just make sure we aren't corrupting data or
 		 * metadata
 		 */
@@ -276,10 +267,10 @@ static void write_page(struct bitmap *bitmap, struct page *page, int wait)
 {
 	struct buffer_head *bh;
 
-	if (bitmap->file == NULL) {
+	if (bitmap->storage.file == NULL) {
 		switch (write_sb_page(bitmap, page, wait)) {
 		case -EINVAL:
-			bitmap->flags |= BITMAP_WRITE_ERROR;
+			set_bit(BITMAP_WRITE_ERROR, &bitmap->flags);
 		}
 	} else {
 
@@ -297,20 +288,16 @@ static void write_page(struct bitmap *bitmap, struct page *page, int wait)
 			wait_event(bitmap->write_wait,
 				   atomic_read(&bitmap->pending_writes)==0);
 	}
-	if (bitmap->flags & BITMAP_WRITE_ERROR)
+	if (test_bit(BITMAP_WRITE_ERROR, &bitmap->flags))
 		bitmap_file_kick(bitmap);
 }
 
 static void end_bitmap_write(struct buffer_head *bh, int uptodate)
 {
 	struct bitmap *bitmap = bh->b_private;
-	unsigned long flags;
 
-	if (!uptodate) {
-		spin_lock_irqsave(&bitmap->lock, flags);
-		bitmap->flags |= BITMAP_WRITE_ERROR;
-		spin_unlock_irqrestore(&bitmap->lock, flags);
-	}
+	if (!uptodate)
+		set_bit(BITMAP_WRITE_ERROR, &bitmap->flags);
 	if (atomic_dec_and_test(&bitmap->pending_writes))
 		wake_up(&bitmap->write_wait);
 }
@@ -325,8 +312,12 @@ __clear_page_buffers(struct page *page)
 }
 static void free_buffers(struct page *page)
 {
-	struct buffer_head *bh = page_buffers(page);
+	struct buffer_head *bh;
 
+	if (!PagePrivate(page))
+		return;
+
+	bh = page_buffers(page);
 	while (bh) {
 		struct buffer_head *next = bh->b_this_page;
 		free_buffer_head(bh);
@@ -343,11 +334,12 @@ static void free_buffers(struct page *page)
  * This usage is similar to how swap files are handled, and allows us
  * to write to a file with no concerns of memory allocation failing.
  */
-static struct page *read_page(struct file *file, unsigned long index,
-			      struct bitmap *bitmap,
-			      unsigned long count)
+static int read_page(struct file *file, unsigned long index,
+		     struct bitmap *bitmap,
+		     unsigned long count,
+		     struct page *page)
 {
-	struct page *page = NULL;
+	int ret = 0;
 	struct inode *inode = file->f_path.dentry->d_inode;
 	struct buffer_head *bh;
 	sector_t block;
@@ -355,16 +347,9 @@ static struct page *read_page(struct file *file, unsigned long index,
 	pr_debug("read bitmap file (%dB @ %llu)\n", (int)PAGE_SIZE,
 		 (unsigned long long)index << PAGE_SHIFT);
 
-	page = alloc_page(GFP_KERNEL);
-	if (!page)
-		page = ERR_PTR(-ENOMEM);
-	if (IS_ERR(page))
-		goto out;
-
 	bh = alloc_page_buffers(page, 1<<inode->i_blkbits, 0);
 	if (!bh) {
-		put_page(page);
-		page = ERR_PTR(-ENOMEM);
+		ret = -ENOMEM;
 		goto out;
 	}
 	attach_page_buffers(page, bh);
@@ -376,8 +361,7 @@ static struct page *read_page(struct file *file, unsigned long index,
 			bh->b_blocknr = bmap(inode, block);
 			if (bh->b_blocknr == 0) {
 				/* Cannot use this file! */
-				free_buffers(page);
-				page = ERR_PTR(-EINVAL);
+				ret = -EINVAL;
 				goto out;
 			}
 			bh->b_bdev = inode->i_sb->s_bdev;
@@ -400,17 +384,15 @@ static struct page *read_page(struct file *file, unsigned long index,
 
 	wait_event(bitmap->write_wait,
 		   atomic_read(&bitmap->pending_writes)==0);
-	if (bitmap->flags & BITMAP_WRITE_ERROR) {
-		free_buffers(page);
-		page = ERR_PTR(-EIO);
-	}
+	if (test_bit(BITMAP_WRITE_ERROR, &bitmap->flags))
+		ret = -EIO;
 out:
-	if (IS_ERR(page))
-		printk(KERN_ALERT "md: bitmap read error: (%dB @ %llu): %ld\n",
+	if (ret)
+		printk(KERN_ALERT "md: bitmap read error: (%dB @ %llu): %d\n",
 			(int)PAGE_SIZE,
 			(unsigned long long)index << PAGE_SHIFT,
-			PTR_ERR(page));
-	return page;
+			ret);
+	return ret;
 }
 
 /*
@@ -426,9 +408,9 @@ void bitmap_update_sb(struct bitmap *bitmap)
 		return;
 	if (bitmap->mddev->bitmap_info.external)
 		return;
-	if (!bitmap->sb_page) /* no superblock */
+	if (!bitmap->storage.sb_page) /* no superblock */
 		return;
-	sb = kmap_atomic(bitmap->sb_page);
+	sb = kmap_atomic(bitmap->storage.sb_page);
 	sb->events = cpu_to_le64(bitmap->mddev->events);
 	if (bitmap->mddev->events < bitmap->events_cleared)
 		/* rocking back to read-only */
@@ -438,8 +420,13 @@ void bitmap_update_sb(struct bitmap *bitmap)
 	/* Just in case these have been changed via sysfs: */
 	sb->daemon_sleep = cpu_to_le32(bitmap->mddev->bitmap_info.daemon_sleep/HZ);
 	sb->write_behind = cpu_to_le32(bitmap->mddev->bitmap_info.max_write_behind);
+	/* This might have been changed by a reshape */
+	sb->sync_size = cpu_to_le64(bitmap->mddev->resync_max_sectors);
+	sb->chunksize = cpu_to_le32(bitmap->mddev->bitmap_info.chunksize);
+	sb->sectors_reserved = cpu_to_le32(bitmap->mddev->
+					   bitmap_info.space);
 	kunmap_atomic(sb);
-	write_page(bitmap, bitmap->sb_page, 1);
+	write_page(bitmap, bitmap->storage.sb_page, 1);
 }
 
 /* print out the bitmap file superblock */
@@ -447,9 +434,9 @@ void bitmap_print_sb(struct bitmap *bitmap)
 {
 	bitmap_super_t *sb;
 
-	if (!bitmap || !bitmap->sb_page)
+	if (!bitmap || !bitmap->storage.sb_page)
 		return;
-	sb = kmap_atomic(bitmap->sb_page);
+	sb = kmap_atomic(bitmap->storage.sb_page);
 	printk(KERN_DEBUG "%s: bitmap file superblock:\n", bmname(bitmap));
 	printk(KERN_DEBUG "         magic: %08x\n", le32_to_cpu(sb->magic));
 	printk(KERN_DEBUG "       version: %d\n", le32_to_cpu(sb->version));
@@ -488,15 +475,15 @@ static int bitmap_new_disk_sb(struct bitmap *bitmap)
 	unsigned long chunksize, daemon_sleep, write_behind;
 	int err = -EINVAL;
 
-	bitmap->sb_page = alloc_page(GFP_KERNEL);
-	if (IS_ERR(bitmap->sb_page)) {
-		err = PTR_ERR(bitmap->sb_page);
-		bitmap->sb_page = NULL;
+	bitmap->storage.sb_page = alloc_page(GFP_KERNEL);
+	if (IS_ERR(bitmap->storage.sb_page)) {
+		err = PTR_ERR(bitmap->storage.sb_page);
+		bitmap->storage.sb_page = NULL;
 		return err;
 	}
-	bitmap->sb_page->index = 0;
+	bitmap->storage.sb_page->index = 0;
 
-	sb = kmap_atomic(bitmap->sb_page);
+	sb = kmap_atomic(bitmap->storage.sb_page);
 
 	sb->magic = cpu_to_le32(BITMAP_MAGIC);
 	sb->version = cpu_to_le32(BITMAP_MAJOR_HI);
@@ -534,8 +521,8 @@ static int bitmap_new_disk_sb(struct bitmap *bitmap)
 
 	memcpy(sb->uuid, bitmap->mddev->uuid, 16);
 
-	bitmap->flags |= BITMAP_STALE;
-	sb->state |= cpu_to_le32(BITMAP_STALE);
+	set_bit(BITMAP_STALE, &bitmap->flags);
+	sb->state = cpu_to_le32(bitmap->flags);
 	bitmap->events_cleared = bitmap->mddev->events;
 	sb->events_cleared = cpu_to_le64(bitmap->mddev->events);
 
@@ -551,31 +538,45 @@ static int bitmap_read_sb(struct bitmap *bitmap)
 	bitmap_super_t *sb;
 	unsigned long chunksize, daemon_sleep, write_behind;
 	unsigned long long events;
+	unsigned long sectors_reserved = 0;
 	int err = -EINVAL;
+	struct page *sb_page;
 
+	if (!bitmap->storage.file && !bitmap->mddev->bitmap_info.offset) {
+		chunksize = 128 * 1024 * 1024;
+		daemon_sleep = 5 * HZ;
+		write_behind = 0;
+		set_bit(BITMAP_STALE, &bitmap->flags);
+		err = 0;
+		goto out_no_sb;
+	}
 	/* page 0 is the superblock, read it... */
-	if (bitmap->file) {
-		loff_t isize = i_size_read(bitmap->file->f_mapping->host);
+	sb_page = alloc_page(GFP_KERNEL);
+	if (!sb_page)
+		return -ENOMEM;
+	bitmap->storage.sb_page = sb_page;
+
+	if (bitmap->storage.file) {
+		loff_t isize = i_size_read(bitmap->storage.file->f_mapping->host);
 		int bytes = isize > PAGE_SIZE ? PAGE_SIZE : isize;
 
-		bitmap->sb_page = read_page(bitmap->file, 0, bitmap, bytes);
+		err = read_page(bitmap->storage.file, 0,
+				bitmap, bytes, sb_page);
 	} else {
-		bitmap->sb_page = read_sb_page(bitmap->mddev,
-					       bitmap->mddev->bitmap_info.offset,
-					       NULL,
-					       0, sizeof(bitmap_super_t));
+		err = read_sb_page(bitmap->mddev,
+				   bitmap->mddev->bitmap_info.offset,
+				   sb_page,
+				   0, sizeof(bitmap_super_t));
 	}
-	if (IS_ERR(bitmap->sb_page)) {
-		err = PTR_ERR(bitmap->sb_page);
-		bitmap->sb_page = NULL;
+	if (err)
 		return err;
-	}
 
-	sb = kmap_atomic(bitmap->sb_page);
+	sb = kmap_atomic(sb_page);
 
 	chunksize = le32_to_cpu(sb->chunksize);
 	daemon_sleep = le32_to_cpu(sb->daemon_sleep) * HZ;
 	write_behind = le32_to_cpu(sb->write_behind);
+	sectors_reserved = le32_to_cpu(sb->sectors_reserved);
 
 	/* verify that the bitmap-specific fields are valid */
 	if (sb->magic != cpu_to_le32(BITMAP_MAGIC))
@@ -618,60 +619,32 @@ static int bitmap_read_sb(struct bitmap *bitmap)
 			       "-- forcing full recovery\n",
 			       bmname(bitmap), events,
 			       (unsigned long long) bitmap->mddev->events);
-			sb->state |= cpu_to_le32(BITMAP_STALE);
+			set_bit(BITMAP_STALE, &bitmap->flags);
 		}
 	}
 
 	/* assign fields using values from superblock */
-	bitmap->mddev->bitmap_info.chunksize = chunksize;
-	bitmap->mddev->bitmap_info.daemon_sleep = daemon_sleep;
-	bitmap->mddev->bitmap_info.max_write_behind = write_behind;
 	bitmap->flags |= le32_to_cpu(sb->state);
 	if (le32_to_cpu(sb->version) == BITMAP_MAJOR_HOSTENDIAN)
-		bitmap->flags |= BITMAP_HOSTENDIAN;
+		set_bit(BITMAP_HOSTENDIAN, &bitmap->flags);
 	bitmap->events_cleared = le64_to_cpu(sb->events_cleared);
-	if (bitmap->flags & BITMAP_STALE)
-		bitmap->events_cleared = bitmap->mddev->events;
 	err = 0;
 out:
 	kunmap_atomic(sb);
+out_no_sb:
+	if (test_bit(BITMAP_STALE, &bitmap->flags))
+		bitmap->events_cleared = bitmap->mddev->events;
+	bitmap->mddev->bitmap_info.chunksize = chunksize;
+	bitmap->mddev->bitmap_info.daemon_sleep = daemon_sleep;
+	bitmap->mddev->bitmap_info.max_write_behind = write_behind;
+	if (bitmap->mddev->bitmap_info.space == 0 ||
+	    bitmap->mddev->bitmap_info.space > sectors_reserved)
+		bitmap->mddev->bitmap_info.space = sectors_reserved;
 	if (err)
 		bitmap_print_sb(bitmap);
 	return err;
 }
 
-enum bitmap_mask_op {
-	MASK_SET,
-	MASK_UNSET
-};
-
-/* record the state of the bitmap in the superblock.  Return the old value */
-static int bitmap_mask_state(struct bitmap *bitmap, enum bitmap_state bits,
-			     enum bitmap_mask_op op)
-{
-	bitmap_super_t *sb;
-	int old;
-
-	if (!bitmap->sb_page) /* can't set the state */
-		return 0;
-	sb = kmap_atomic(bitmap->sb_page);
-	old = le32_to_cpu(sb->state) & bits;
-	switch (op) {
-	case MASK_SET:
-		sb->state |= cpu_to_le32(bits);
-		bitmap->flags |= bits;
-		break;
-	case MASK_UNSET:
-		sb->state &= cpu_to_le32(~bits);
-		bitmap->flags &= ~bits;
-		break;
-	default:
-		BUG();
-	}
-	kunmap_atomic(sb);
-	return old;
-}
-
 /*
  * general bitmap file operations
  */
@@ -683,17 +656,19 @@ static int bitmap_mask_state(struct bitmap *bitmap, enum bitmap_state bits,
  * file a page at a time. There's a superblock at the start of the file.
  */
 /* calculate the index of the page that contains this bit */
-static inline unsigned long file_page_index(struct bitmap *bitmap, unsigned long chunk)
+static inline unsigned long file_page_index(struct bitmap_storage *store,
+					    unsigned long chunk)
 {
-	if (!bitmap->mddev->bitmap_info.external)
+	if (store->sb_page)
 		chunk += sizeof(bitmap_super_t) << 3;
 	return chunk >> PAGE_BIT_SHIFT;
 }
 
 /* calculate the (bit) offset of this bit within a page */
-static inline unsigned long file_page_offset(struct bitmap *bitmap, unsigned long chunk)
+static inline unsigned long file_page_offset(struct bitmap_storage *store,
+					     unsigned long chunk)
 {
-	if (!bitmap->mddev->bitmap_info.external)
+	if (store->sb_page)
 		chunk += sizeof(bitmap_super_t) << 3;
 	return chunk & (PAGE_BITS - 1);
 }
@@ -705,57 +680,86 @@ static inline unsigned long file_page_offset(struct bitmap *bitmap, unsigned lon
  * 1 page (e.g., x86) or less than 1 page -- so the bitmap might start on page
  * 0 or page 1
  */
-static inline struct page *filemap_get_page(struct bitmap *bitmap,
+static inline struct page *filemap_get_page(struct bitmap_storage *store,
 					    unsigned long chunk)
 {
-	if (file_page_index(bitmap, chunk) >= bitmap->file_pages)
+	if (file_page_index(store, chunk) >= store->file_pages)
 		return NULL;
-	return bitmap->filemap[file_page_index(bitmap, chunk)
-			       - file_page_index(bitmap, 0)];
+	return store->filemap[file_page_index(store, chunk)
+			      - file_page_index(store, 0)];
 }
 
-static void bitmap_file_unmap(struct bitmap *bitmap)
+static int bitmap_storage_alloc(struct bitmap_storage *store,
+				unsigned long chunks, int with_super)
+{
+	int pnum;
+	unsigned long num_pages;
+	unsigned long bytes;
+
+	bytes = DIV_ROUND_UP(chunks, 8);
+	if (with_super)
+		bytes += sizeof(bitmap_super_t);
+
+	num_pages = DIV_ROUND_UP(bytes, PAGE_SIZE);
+
+	store->filemap = kmalloc(sizeof(struct page *)
+				 * num_pages, GFP_KERNEL);
+	if (!store->filemap)
+		return -ENOMEM;
+
+	if (with_super && !store->sb_page) {
+		store->sb_page = alloc_page(GFP_KERNEL|__GFP_ZERO);
+		if (store->sb_page == NULL)
+			return -ENOMEM;
+		store->sb_page->index = 0;
+	}
+	pnum = 0;
+	if (store->sb_page) {
+		store->filemap[0] = store->sb_page;
+		pnum = 1;
+	}
+	for ( ; pnum < num_pages; pnum++) {
+		store->filemap[pnum] = alloc_page(GFP_KERNEL|__GFP_ZERO);
+		if (!store->filemap[pnum]) {
+			store->file_pages = pnum;
+			return -ENOMEM;
+		}
+		store->filemap[pnum]->index = pnum;
+	}
+	store->file_pages = pnum;
+
+	/* We need 4 bits per page, rounded up to a multiple
+	 * of sizeof(unsigned long) */
+	store->filemap_attr = kzalloc(
+		roundup(DIV_ROUND_UP(num_pages*4, 8), sizeof(unsigned long)),
+		GFP_KERNEL);
+	if (!store->filemap_attr)
+		return -ENOMEM;
+
+	store->bytes = bytes;
+
+	return 0;
+}
+
+static void bitmap_file_unmap(struct bitmap_storage *store)
 {
 	struct page **map, *sb_page;
-	unsigned long *attr;
 	int pages;
-	unsigned long flags;
+	struct file *file;
 
-	spin_lock_irqsave(&bitmap->lock, flags);
-	map = bitmap->filemap;
-	bitmap->filemap = NULL;
-	attr = bitmap->filemap_attr;
-	bitmap->filemap_attr = NULL;
-	pages = bitmap->file_pages;
-	bitmap->file_pages = 0;
-	sb_page = bitmap->sb_page;
-	bitmap->sb_page = NULL;
-	spin_unlock_irqrestore(&bitmap->lock, flags);
+	file = store->file;
+	map = store->filemap;
+	pages = store->file_pages;
+	sb_page = store->sb_page;
 
 	while (pages--)
 		if (map[pages] != sb_page) /* 0 is sb_page, release it below */
 			free_buffers(map[pages]);
 	kfree(map);
-	kfree(attr);
+	kfree(store->filemap_attr);
 
 	if (sb_page)
 		free_buffers(sb_page);
-}
-
-static void bitmap_file_put(struct bitmap *bitmap)
-{
-	struct file *file;
-	unsigned long flags;
-
-	spin_lock_irqsave(&bitmap->lock, flags);
-	file = bitmap->file;
-	bitmap->file = NULL;
-	spin_unlock_irqrestore(&bitmap->lock, flags);
-
-	if (file)
-		wait_event(bitmap->write_wait,
-			   atomic_read(&bitmap->pending_writes)==0);
-	bitmap_file_unmap(bitmap);
 
 	if (file) {
 		struct inode *inode = file->f_path.dentry->d_inode;
@@ -773,14 +777,14 @@ static void bitmap_file_kick(struct bitmap *bitmap)
 {
 	char *path, *ptr = NULL;
 
-	if (bitmap_mask_state(bitmap, BITMAP_STALE, MASK_SET) == 0) {
+	if (!test_and_set_bit(BITMAP_STALE, &bitmap->flags)) {
 		bitmap_update_sb(bitmap);
 
-		if (bitmap->file) {
+		if (bitmap->storage.file) {
 			path = kmalloc(PAGE_SIZE, GFP_KERNEL);
 			if (path)
-				ptr = d_path(&bitmap->file->f_path, path,
-					     PAGE_SIZE);
+				ptr = d_path(&bitmap->storage.file->f_path,
+					     path, PAGE_SIZE);
 
 			printk(KERN_ALERT
 			      "%s: kicking failed bitmap file %s from array!\n",
@@ -792,10 +796,6 @@ static void bitmap_file_kick(struct bitmap *bitmap)
 			       "%s: disabling internal bitmap due to errors\n",
 			       bmname(bitmap));
 	}
-
-	bitmap_file_put(bitmap);
-
-	return;
 }
 
 enum bitmap_page_attr {
@@ -805,24 +805,30 @@ enum bitmap_page_attr {
 	BITMAP_PAGE_NEEDWRITE = 2, /* there are cleared bits that need to be synced */
 };
 
-static inline void set_page_attr(struct bitmap *bitmap, struct page *page,
-				enum bitmap_page_attr attr)
+static inline void set_page_attr(struct bitmap *bitmap, int pnum,
+				 enum bitmap_page_attr attr)
 {
-	__set_bit((page->index<<2) + attr, bitmap->filemap_attr);
+	set_bit((pnum<<2) + attr, bitmap->storage.filemap_attr);
 }
 
-static inline void clear_page_attr(struct bitmap *bitmap, struct page *page,
-				enum bitmap_page_attr attr)
+static inline void clear_page_attr(struct bitmap *bitmap, int pnum,
+				   enum bitmap_page_attr attr)
 {
-	__clear_bit((page->index<<2) + attr, bitmap->filemap_attr);
+	clear_bit((pnum<<2) + attr, bitmap->storage.filemap_attr);
 }
 
-static inline unsigned long test_page_attr(struct bitmap *bitmap, struct page *page,
-					   enum bitmap_page_attr attr)
+static inline int test_page_attr(struct bitmap *bitmap, int pnum,
+				 enum bitmap_page_attr attr)
 {
-	return test_bit((page->index<<2) + attr, bitmap->filemap_attr);
+	return test_bit((pnum<<2) + attr, bitmap->storage.filemap_attr);
 }
 
+static inline int test_and_clear_page_attr(struct bitmap *bitmap, int pnum,
+					   enum bitmap_page_attr attr)
+{
+	return test_and_clear_bit((pnum<<2) + attr,
+				  bitmap->storage.filemap_attr);
+}
 /*
  * bitmap_file_set_bit -- called before performing a write to the md device
  * to set (and eventually sync) a particular bit in the bitmap file
@@ -835,26 +841,46 @@ static void bitmap_file_set_bit(struct bitmap *bitmap, sector_t block)
 	unsigned long bit;
 	struct page *page;
 	void *kaddr;
-	unsigned long chunk = block >> bitmap->chunkshift;
+	unsigned long chunk = block >> bitmap->counts.chunkshift;
 
-	if (!bitmap->filemap)
-		return;
-
-	page = filemap_get_page(bitmap, chunk);
+	page = filemap_get_page(&bitmap->storage, chunk);
 	if (!page)
 		return;
-	bit = file_page_offset(bitmap, chunk);
+	bit = file_page_offset(&bitmap->storage, chunk);
 
 	/* set the bit */
 	kaddr = kmap_atomic(page);
-	if (bitmap->flags & BITMAP_HOSTENDIAN)
+	if (test_bit(BITMAP_HOSTENDIAN, &bitmap->flags))
 		set_bit(bit, kaddr);
 	else
-		__set_bit_le(bit, kaddr);
+		test_and_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 */
-	set_page_attr(bitmap, page, BITMAP_PAGE_DIRTY);
+	set_page_attr(bitmap, page->index, BITMAP_PAGE_DIRTY);
+}
+
+static void bitmap_file_clear_bit(struct bitmap *bitmap, sector_t block)
+{
+	unsigned long bit;
+	struct page *page;
+	void *paddr;
+	unsigned long chunk = block >> bitmap->counts.chunkshift;
+
+	page = filemap_get_page(&bitmap->storage, chunk);
+	if (!page)
+		return;
+	bit = file_page_offset(&bitmap->storage, chunk);
+	paddr = kmap_atomic(page);
+	if (test_bit(BITMAP_HOSTENDIAN, &bitmap->flags))
+		clear_bit(bit, paddr);
+	else
+		test_and_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);
+		bitmap->allclean = 0;
+	}
 }
 
 /* this gets called when the md device is ready to unplug its underlying
@@ -862,42 +888,37 @@ static void bitmap_file_set_bit(struct bitmap *bitmap, sector_t block)
  * sync the dirty pages of the bitmap file to disk */
 void bitmap_unplug(struct bitmap *bitmap)
 {
-	unsigned long i, flags;
+	unsigned long i;
 	int dirty, need_write;
-	struct page *page;
 	int wait = 0;
 
-	if (!bitmap)
+	if (!bitmap || !bitmap->storage.filemap ||
+	    test_bit(BITMAP_STALE, &bitmap->flags))
 		return;
 
 	/* look at each page to see if there are any set bits that need to be
 	 * flushed out to disk */
-	for (i = 0; i < bitmap->file_pages; i++) {
-		spin_lock_irqsave(&bitmap->lock, flags);
-		if (!bitmap->filemap) {
-			spin_unlock_irqrestore(&bitmap->lock, flags);
+	for (i = 0; i < bitmap->storage.file_pages; i++) {
+		if (!bitmap->storage.filemap)
 			return;
+		dirty = test_and_clear_page_attr(bitmap, i, BITMAP_PAGE_DIRTY);
+		need_write = test_and_clear_page_attr(bitmap, i,
+						      BITMAP_PAGE_NEEDWRITE);
+		if (dirty || need_write) {
+			clear_page_attr(bitmap, i, BITMAP_PAGE_PENDING);
+			write_page(bitmap, bitmap->storage.filemap[i], 0);
 		}
-		page = bitmap->filemap[i];
-		dirty = test_page_attr(bitmap, page, BITMAP_PAGE_DIRTY);
-		need_write = test_page_attr(bitmap, page, BITMAP_PAGE_NEEDWRITE);
-		clear_page_attr(bitmap, page, BITMAP_PAGE_DIRTY);
-		clear_page_attr(bitmap, page, BITMAP_PAGE_NEEDWRITE);
 		if (dirty)
 			wait = 1;
-		spin_unlock_irqrestore(&bitmap->lock, flags);
-
-		if (dirty || need_write)
-			write_page(bitmap, page, 0);
 	}
 	if (wait) { /* if any writes were performed, we need to wait on them */
-		if (bitmap->file)
+		if (bitmap->storage.file)
 			wait_event(bitmap->write_wait,
 				   atomic_read(&bitmap->pending_writes)==0);
 		else
 			md_super_wait(bitmap->mddev);
 	}
-	if (bitmap->flags & BITMAP_WRITE_ERROR)
+	if (test_bit(BITMAP_WRITE_ERROR, &bitmap->flags))
 		bitmap_file_kick(bitmap);
 }
 EXPORT_SYMBOL(bitmap_unplug);
@@ -917,98 +938,77 @@ static void bitmap_set_memory_bits(struct bitmap *bitmap, sector_t offset, int n
 static int bitmap_init_from_disk(struct bitmap *bitmap, sector_t start)
 {
 	unsigned long i, chunks, index, oldindex, bit;
-	struct page *page = NULL, *oldpage = NULL;
-	unsigned long num_pages, bit_cnt = 0;
+	struct page *page = NULL;
+	unsigned long bit_cnt = 0;
 	struct file *file;
-	unsigned long bytes, offset;
+	unsigned long offset;
 	int outofdate;
 	int ret = -ENOSPC;
 	void *paddr;
+	struct bitmap_storage *store = &bitmap->storage;
 
-	chunks = bitmap->chunks;
-	file = bitmap->file;
+	chunks = bitmap->counts.chunks;
+	file = store->file;
 
-	BUG_ON(!file && !bitmap->mddev->bitmap_info.offset);
+	if (!file && !bitmap->mddev->bitmap_info.offset) {
+		/* No permanent bitmap - fill with '1s'. */
+		store->filemap = NULL;
+		store->file_pages = 0;
+		for (i = 0; i < chunks ; i++) {
+			/* if the disk bit is set, set the memory bit */
+			int needed = ((sector_t)(i+1) << (bitmap->counts.chunkshift)
+				      >= start);
+			bitmap_set_memory_bits(bitmap,
+					       (sector_t)i << bitmap->counts.chunkshift,
+					       needed);
+		}
+		return 0;
+	}
 
-	outofdate = bitmap->flags & BITMAP_STALE;
+	outofdate = test_bit(BITMAP_STALE, &bitmap->flags);
 	if (outofdate)
 		printk(KERN_INFO "%s: bitmap file is out of date, doing full "
 			"recovery\n", bmname(bitmap));
 
-	bytes = DIV_ROUND_UP(bitmap->chunks, 8);
-	if (!bitmap->mddev->bitmap_info.external)
-		bytes += sizeof(bitmap_super_t);
-
-	num_pages = DIV_ROUND_UP(bytes, PAGE_SIZE);
-
-	if (file && i_size_read(file->f_mapping->host) < bytes) {
+	if (file && i_size_read(file->f_mapping->host) < store->bytes) {
 		printk(KERN_INFO "%s: bitmap file too short %lu < %lu\n",
-			bmname(bitmap),
-			(unsigned long) i_size_read(file->f_mapping->host),
-			bytes);
+		       bmname(bitmap),
+		       (unsigned long) i_size_read(file->f_mapping->host),
+		       store->bytes);
 		goto err;
 	}
 
-	ret = -ENOMEM;
-
-	bitmap->filemap = kmalloc(sizeof(struct page *) * num_pages, GFP_KERNEL);
-	if (!bitmap->filemap)
-		goto err;
-
-	/* We need 4 bits per page, rounded up to a multiple of sizeof(unsigned long) */
-	bitmap->filemap_attr = kzalloc(
-		roundup(DIV_ROUND_UP(num_pages*4, 8), sizeof(unsigned long)),
-		GFP_KERNEL);
-	if (!bitmap->filemap_attr)
-		goto err;
-
 	oldindex = ~0L;
+	offset = 0;
+	if (!bitmap->mddev->bitmap_info.external)
+		offset = sizeof(bitmap_super_t);
 
 	for (i = 0; i < chunks; i++) {
 		int b;
-		index = file_page_index(bitmap, i);
-		bit = file_page_offset(bitmap, i);
+		index = file_page_index(&bitmap->storage, i);
+		bit = file_page_offset(&bitmap->storage, i);
 		if (index != oldindex) { /* this is a new page, read it in */
 			int count;
 			/* unmap the old page, we're done with it */
-			if (index == num_pages-1)
-				count = bytes - index * PAGE_SIZE;
+			if (index == store->file_pages-1)
+				count = store->bytes - index * PAGE_SIZE;
 			else
 				count = PAGE_SIZE;
-			if (index == 0 && bitmap->sb_page) {
-				/*
-				 * if we're here then the superblock page
-				 * contains some bits (PAGE_SIZE != sizeof sb)
-				 * we've already read it in, so just use it
-				 */
-				page = bitmap->sb_page;
-				offset = sizeof(bitmap_super_t);
-				if (!file)
-					page = read_sb_page(
-						bitmap->mddev,
-						bitmap->mddev->bitmap_info.offset,
-						page,
-						index, count);
-			} else if (file) {
-				page = read_page(file, index, bitmap, count);
-				offset = 0;
-			} else {
-				page = read_sb_page(bitmap->mddev,
-						    bitmap->mddev->bitmap_info.offset,
-						    NULL,
-						    index, count);
-				offset = 0;
-			}
-			if (IS_ERR(page)) { /* read error */
-				ret = PTR_ERR(page);
+			page = store->filemap[index];
+			if (file)
+				ret = read_page(file, index, bitmap,
+						count, page);
+			else
+				ret = read_sb_page(
+					bitmap->mddev,
+					bitmap->mddev->bitmap_info.offset,
+					page,
+					index, count);
+
+			if (ret)
 				goto err;
-			}
 
 			oldindex = index;
-			oldpage = page;
-
-			bitmap->filemap[bitmap->file_pages++] = page;
-			bitmap->last_page_size = count;
 
 			if (outofdate) {
 				/*
@@ -1022,39 +1022,33 @@ static int bitmap_init_from_disk(struct bitmap *bitmap, sector_t start)
 				write_page(bitmap, page, 1);
 
 				ret = -EIO;
-				if (bitmap->flags & BITMAP_WRITE_ERROR)
+				if (test_bit(BITMAP_WRITE_ERROR,
+					     &bitmap->flags))
 					goto err;
 			}
 		}
 		paddr = kmap_atomic(page);
-		if (bitmap->flags & BITMAP_HOSTENDIAN)
+		if (test_bit(BITMAP_HOSTENDIAN, &bitmap->flags))
 			b = test_bit(bit, paddr);
 		else
 			b = test_bit_le(bit, paddr);
 		kunmap_atomic(paddr);
 		if (b) {
 			/* if the disk bit is set, set the memory bit */
-			int needed = ((sector_t)(i+1) << bitmap->chunkshift
+			int needed = ((sector_t)(i+1) << bitmap->counts.chunkshift
 				      >= start);
 			bitmap_set_memory_bits(bitmap,
-					       (sector_t)i << bitmap->chunkshift,
+					       (sector_t)i << bitmap->counts.chunkshift,
 					       needed);
 			bit_cnt++;
 		}
-	}
-
-	/* everything went OK */
-	ret = 0;
-	bitmap_mask_state(bitmap, BITMAP_STALE, MASK_UNSET);
-
-	if (bit_cnt) { /* Kick recovery if any bits were set */
-		set_bit(MD_RECOVERY_NEEDED, &bitmap->mddev->recovery);
-		md_wakeup_thread(bitmap->mddev->thread);
+		offset = 0;
 	}
 
 	printk(KERN_INFO "%s: bitmap initialized from disk: "
-	       "read %lu/%lu pages, set %lu of %lu bits\n",
-	       bmname(bitmap), bitmap->file_pages, num_pages, bit_cnt, chunks);
+	       "read %lu pages, set %lu of %lu bits\n",
+	       bmname(bitmap), store->file_pages,
+	       bit_cnt, chunks);
 
 	return 0;
 
@@ -1071,22 +1065,38 @@ void bitmap_write_all(struct bitmap *bitmap)
 	 */
 	int i;
 
-	spin_lock_irq(&bitmap->lock);
-	for (i = 0; i < bitmap->file_pages; i++)
-		set_page_attr(bitmap, bitmap->filemap[i],
+	if (!bitmap || !bitmap->storage.filemap)
+		return;
+	if (bitmap->storage.file)
+		/* Only one copy, so nothing needed */
+		return;
+
+	for (i = 0; i < bitmap->storage.file_pages; i++)
+		set_page_attr(bitmap, i,
 			      BITMAP_PAGE_NEEDWRITE);
 	bitmap->allclean = 0;
-	spin_unlock_irq(&bitmap->lock);
 }
 
-static void bitmap_count_page(struct bitmap *bitmap, sector_t offset, int inc)
+static void bitmap_count_page(struct bitmap_counts *bitmap,
+			      sector_t offset, int inc)
 {
 	sector_t chunk = offset >> bitmap->chunkshift;
 	unsigned long page = chunk >> PAGE_COUNTER_SHIFT;
 	bitmap->bp[page].count += inc;
 	bitmap_checkfree(bitmap, page);
 }
-static bitmap_counter_t *bitmap_get_counter(struct bitmap *bitmap,
+
+static void bitmap_set_pending(struct bitmap_counts *bitmap, sector_t offset)
+{
+	sector_t chunk = offset >> bitmap->chunkshift;
+	unsigned long page = chunk >> PAGE_COUNTER_SHIFT;
+	struct bitmap_page *bp = &bitmap->bp[page];
+
+	if (!bp->pending)
+		bp->pending = 1;
+}
+
+static bitmap_counter_t *bitmap_get_counter(struct bitmap_counts *bitmap,
 					    sector_t offset, sector_t *blocks,
 					    int create);
 
@@ -1099,10 +1109,9 @@ void bitmap_daemon_work(struct mddev *mddev)
 {
 	struct bitmap *bitmap;
 	unsigned long j;
-	unsigned long flags;
-	struct page *page = NULL, *lastpage = NULL;
+	unsigned long nextpage;
 	sector_t blocks;
-	void *paddr;
+	struct bitmap_counts *counts;
 
 	/* Use a mutex to guard daemon_work against
 	 * bitmap_destroy.
@@ -1124,112 +1133,90 @@ void bitmap_daemon_work(struct mddev *mddev)
 	}
 	bitmap->allclean = 1;
 
-	spin_lock_irqsave(&bitmap->lock, flags);
-	for (j = 0; j < bitmap->chunks; j++) {
+	/* Any file-page which is PENDING now needs to be written.
+	 * So set NEEDWRITE now, then after we make any last-minute changes
+	 * we will write it.
+	 */
+	for (j = 0; j < bitmap->storage.file_pages; j++)
+		if (test_and_clear_page_attr(bitmap, j,
+					     BITMAP_PAGE_PENDING))
+			set_page_attr(bitmap, j,
+				      BITMAP_PAGE_NEEDWRITE);
+
+	if (bitmap->need_sync &&
+	    mddev->bitmap_info.external == 0) {
+		/* Arrange for superblock update as well as
+		 * other changes */
+		bitmap_super_t *sb;
+		bitmap->need_sync = 0;
+		if (bitmap->storage.filemap) {
+			sb = kmap_atomic(bitmap->storage.sb_page);
+			sb->events_cleared =
+				cpu_to_le64(bitmap->events_cleared);
+			kunmap_atomic(sb);
+			set_page_attr(bitmap, 0,
+				      BITMAP_PAGE_NEEDWRITE);
+		}
+	}
+	/* Now look at the bitmap counters and if any are '2' or '1',
+	 * decrement and handle accordingly.
+	 */
+	counts = &bitmap->counts;
+	spin_lock_irq(&counts->lock);
+	nextpage = 0;
+	for (j = 0; j < counts->chunks; j++) {
 		bitmap_counter_t *bmc;
-		if (!bitmap->filemap)
-			/* error or shutdown */
-			break;
+		sector_t  block = (sector_t)j << counts->chunkshift;
 
-		page = filemap_get_page(bitmap, j);
-
-		if (page != lastpage) {
-			/* skip this page unless it's marked as needing cleaning */
-			if (!test_page_attr(bitmap, page, BITMAP_PAGE_PENDING)) {
-				int need_write = test_page_attr(bitmap, page,
-								BITMAP_PAGE_NEEDWRITE);
-				if (need_write)
-					clear_page_attr(bitmap, page, BITMAP_PAGE_NEEDWRITE);
-
-				spin_unlock_irqrestore(&bitmap->lock, flags);
-				if (need_write)
-					write_page(bitmap, page, 0);
-				spin_lock_irqsave(&bitmap->lock, flags);
-				j |= (PAGE_BITS - 1);
+		if (j == nextpage) {
+			nextpage += PAGE_COUNTER_RATIO;
+			if (!counts->bp[j >> PAGE_COUNTER_SHIFT].pending) {
+				j |= PAGE_COUNTER_MASK;
 				continue;
 			}
-
-			/* grab the new page, sync and release the old */
-			if (lastpage != NULL) {
-				if (test_page_attr(bitmap, lastpage,
-						   BITMAP_PAGE_NEEDWRITE)) {
-					clear_page_attr(bitmap, lastpage,
-							BITMAP_PAGE_NEEDWRITE);
-					spin_unlock_irqrestore(&bitmap->lock, flags);
-					write_page(bitmap, lastpage, 0);
-				} else {
-					set_page_attr(bitmap, lastpage,
-						      BITMAP_PAGE_NEEDWRITE);
-					bitmap->allclean = 0;
-					spin_unlock_irqrestore(&bitmap->lock, flags);
-				}
-			} else
-				spin_unlock_irqrestore(&bitmap->lock, flags);
-			lastpage = page;
-
-			/* We are possibly going to clear some bits, so make
-			 * sure that events_cleared is up-to-date.
-			 */
-			if (bitmap->need_sync &&
-			    mddev->bitmap_info.external == 0) {
-				bitmap_super_t *sb;
-				bitmap->need_sync = 0;
-				sb = kmap_atomic(bitmap->sb_page);
-				sb->events_cleared =
-					cpu_to_le64(bitmap->events_cleared);
-				kunmap_atomic(sb);
-				write_page(bitmap, bitmap->sb_page, 1);
-			}
-			spin_lock_irqsave(&bitmap->lock, flags);
-			if (!bitmap->need_sync)
-				clear_page_attr(bitmap, page, BITMAP_PAGE_PENDING);
-			else
-				bitmap->allclean = 0;
+			counts->bp[j >> PAGE_COUNTER_SHIFT].pending = 0;
 		}
-		bmc = bitmap_get_counter(bitmap,
-					 (sector_t)j << bitmap->chunkshift,
+		bmc = bitmap_get_counter(counts,
+					 block,
 					 &blocks, 0);
-		if (!bmc)
+
+		if (!bmc) {
 			j |= PAGE_COUNTER_MASK;
-		else if (*bmc) {
-			if (*bmc == 1 && !bitmap->need_sync) {
-				/* we can clear the bit */
-				*bmc = 0;
-				bitmap_count_page(bitmap,
-						  (sector_t)j << bitmap->chunkshift,
-						  -1);
-
-				/* clear the bit */
-				paddr = kmap_atomic(page);
-				if (bitmap->flags & BITMAP_HOSTENDIAN)
-					clear_bit(file_page_offset(bitmap, j),
-						  paddr);
-				else
-					__clear_bit_le(
-						file_page_offset(bitmap,
-								 j),
-						paddr);
-				kunmap_atomic(paddr);
-			} else if (*bmc <= 2) {
-				*bmc = 1; /* maybe clear the bit next time */
-				set_page_attr(bitmap, page, BITMAP_PAGE_PENDING);
-				bitmap->allclean = 0;
-			}
+			continue;
 		}
-	}
-	spin_unlock_irqrestore(&bitmap->lock, flags);
-
-	/* now sync the final page */
-	if (lastpage != NULL) {
-		spin_lock_irqsave(&bitmap->lock, flags);
-		if (test_page_attr(bitmap, lastpage, BITMAP_PAGE_NEEDWRITE)) {
-			clear_page_attr(bitmap, lastpage, BITMAP_PAGE_NEEDWRITE);
-			spin_unlock_irqrestore(&bitmap->lock, flags);
-			write_page(bitmap, lastpage, 0);
-		} else {
-			set_page_attr(bitmap, lastpage, BITMAP_PAGE_NEEDWRITE);
+		if (*bmc == 1 && !bitmap->need_sync) {
+			/* We can clear the bit */
+			*bmc = 0;
+			bitmap_count_page(counts, block, -1);
+			bitmap_file_clear_bit(bitmap, block);
+		} else if (*bmc && *bmc <= 2) {
+			*bmc = 1;
+			bitmap_set_pending(counts, block);
 			bitmap->allclean = 0;
-			spin_unlock_irqrestore(&bitmap->lock, flags);
+		}
+	}
+	spin_unlock_irq(&counts->lock);
+
+	/* Now start writeout on any page in NEEDWRITE that isn't DIRTY.
+	 * DIRTY pages need to be written by bitmap_unplug so it can wait
+	 * for them.
+	 * If we find any DIRTY page we stop there and let bitmap_unplug
+	 * handle all the rest.  This is important in the case where
+	 * the first blocking holds the superblock and it has been updated.
+	 * We mustn't write any other blocks before the superblock.
+	 */
+	for (j = 0;
+	     j < bitmap->storage.file_pages
+		     && !test_bit(BITMAP_STALE, &bitmap->flags);
+	     j++) {
+
+		if (test_page_attr(bitmap, j,
+				   BITMAP_PAGE_DIRTY))
+			/* bitmap_unplug will handle the rest */
+			break;
+		if (test_and_clear_page_attr(bitmap, j,
+					     BITMAP_PAGE_NEEDWRITE)) {
+			write_page(bitmap, bitmap->storage.filemap[j], 0);
 		}
 	}
 
@@ -1240,7 +1227,7 @@ void bitmap_daemon_work(struct mddev *mddev)
 	mutex_unlock(&mddev->bitmap_info.mutex);
 }
 
-static bitmap_counter_t *bitmap_get_counter(struct bitmap *bitmap,
+static bitmap_counter_t *bitmap_get_counter(struct bitmap_counts *bitmap,
 					    sector_t offset, sector_t *blocks,
 					    int create)
 __releases(bitmap->lock)
@@ -1302,10 +1289,10 @@ int bitmap_startwrite(struct bitmap *bitmap, sector_t offset, unsigned long sect
 		sector_t blocks;
 		bitmap_counter_t *bmc;
 
-		spin_lock_irq(&bitmap->lock);
-		bmc = bitmap_get_counter(bitmap, offset, &blocks, 1);
+		spin_lock_irq(&bitmap->counts.lock);
+		bmc = bitmap_get_counter(&bitmap->counts, offset, &blocks, 1);
 		if (!bmc) {
-			spin_unlock_irq(&bitmap->lock);
+			spin_unlock_irq(&bitmap->counts.lock);
 			return 0;
 		}
 
@@ -1317,7 +1304,7 @@ int bitmap_startwrite(struct bitmap *bitmap, sector_t offset, unsigned long sect
 			 */
 			prepare_to_wait(&bitmap->overflow_wait, &__wait,
 					TASK_UNINTERRUPTIBLE);
-			spin_unlock_irq(&bitmap->lock);
+			spin_unlock_irq(&bitmap->counts.lock);
 			io_schedule();
 			finish_wait(&bitmap->overflow_wait, &__wait);
 			continue;
@@ -1326,7 +1313,7 @@ int bitmap_startwrite(struct bitmap *bitmap, sector_t offset, unsigned long sect
 		switch (*bmc) {
 		case 0:
 			bitmap_file_set_bit(bitmap, offset);
-			bitmap_count_page(bitmap, offset, 1);
+			bitmap_count_page(&bitmap->counts, offset, 1);
 			/* fall through */
 		case 1:
 			*bmc = 2;
@@ -1334,7 +1321,7 @@ int bitmap_startwrite(struct bitmap *bitmap, sector_t offset, unsigned long sect
 
 		(*bmc)++;
 
-		spin_unlock_irq(&bitmap->lock);
+		spin_unlock_irq(&bitmap->counts.lock);
 
 		offset += blocks;
 		if (sectors > blocks)
@@ -1364,10 +1351,10 @@ void bitmap_endwrite(struct bitmap *bitmap, sector_t offset, unsigned long secto
 		unsigned long flags;
 		bitmap_counter_t *bmc;
 
-		spin_lock_irqsave(&bitmap->lock, flags);
-		bmc = bitmap_get_counter(bitmap, offset, &blocks, 0);
+		spin_lock_irqsave(&bitmap->counts.lock, flags);
+		bmc = bitmap_get_counter(&bitmap->counts, offset, &blocks, 0);
 		if (!bmc) {
-			spin_unlock_irqrestore(&bitmap->lock, flags);
+			spin_unlock_irqrestore(&bitmap->counts.lock, flags);
 			return;
 		}
 
@@ -1386,14 +1373,10 @@ void bitmap_endwrite(struct bitmap *bitmap, sector_t offset, unsigned long secto
 
 		(*bmc)--;
 		if (*bmc <= 2) {
-			set_page_attr(bitmap,
-				      filemap_get_page(
-					      bitmap,
-					      offset >> bitmap->chunkshift),
-				      BITMAP_PAGE_PENDING);
+			bitmap_set_pending(&bitmap->counts, offset);
 			bitmap->allclean = 0;
 		}
-		spin_unlock_irqrestore(&bitmap->lock, flags);
+		spin_unlock_irqrestore(&bitmap->counts.lock, flags);
 		offset += blocks;
 		if (sectors > blocks)
 			sectors -= blocks;
@@ -1412,8 +1395,8 @@ static int __bitmap_start_sync(struct bitmap *bitmap, sector_t offset, sector_t
 		*blocks = 1024;
 		return 1; /* always resync if no bitmap */
 	}
-	spin_lock_irq(&bitmap->lock);
-	bmc = bitmap_get_counter(bitmap, offset, blocks, 0);
+	spin_lock_irq(&bitmap->counts.lock);
+	bmc = bitmap_get_counter(&bitmap->counts, offset, blocks, 0);
 	rv = 0;
 	if (bmc) {
 		/* locked */
@@ -1427,7 +1410,7 @@ static int __bitmap_start_sync(struct bitmap *bitmap, sector_t offset, sector_t
 			}
 		}
 	}
-	spin_unlock_irq(&bitmap->lock);
+	spin_unlock_irq(&bitmap->counts.lock);
 	return rv;
 }
 
@@ -1464,8 +1447,8 @@ void bitmap_end_sync(struct bitmap *bitmap, sector_t offset, sector_t *blocks, i
 		*blocks = 1024;
 		return;
 	}
-	spin_lock_irqsave(&bitmap->lock, flags);
-	bmc = bitmap_get_counter(bitmap, offset, blocks, 0);
+	spin_lock_irqsave(&bitmap->counts.lock, flags);
+	bmc = bitmap_get_counter(&bitmap->counts, offset, blocks, 0);
 	if (bmc == NULL)
 		goto unlock;
 	/* locked */
@@ -1476,15 +1459,13 @@ void bitmap_end_sync(struct bitmap *bitmap, sector_t offset, sector_t *blocks, i
 			*bmc |= NEEDED_MASK;
 		else {
 			if (*bmc <= 2) {
-				set_page_attr(bitmap,
-					      filemap_get_page(bitmap, offset >> bitmap->chunkshift),
-					      BITMAP_PAGE_PENDING);
+				bitmap_set_pending(&bitmap->counts, offset);
 				bitmap->allclean = 0;
 			}
 		}
 	}
  unlock:
-	spin_unlock_irqrestore(&bitmap->lock, flags);
+	spin_unlock_irqrestore(&bitmap->counts.lock, flags);
 }
 EXPORT_SYMBOL(bitmap_end_sync);
 
@@ -1524,7 +1505,7 @@ void bitmap_cond_end_sync(struct bitmap *bitmap, sector_t sector)
 
 	bitmap->mddev->curr_resync_completed = sector;
 	set_bit(MD_CHANGE_CLEAN, &bitmap->mddev->flags);
-	sector &= ~((1ULL << bitmap->chunkshift) - 1);
+	sector &= ~((1ULL << bitmap->counts.chunkshift) - 1);
 	s = 0;
 	while (s < sector && s < bitmap->mddev->resync_max_sectors) {
 		bitmap_end_sync(bitmap, s, &blocks, 0);
@@ -1538,27 +1519,25 @@ EXPORT_SYMBOL(bitmap_cond_end_sync);
 static void bitmap_set_memory_bits(struct bitmap *bitmap, sector_t offset, int needed)
 {
 	/* For each chunk covered by any of these sectors, set the
-	 * counter to 1 and set resync_needed.  They should all
+	 * counter to 2 and possibly set resync_needed.  They should all
 	 * be 0 at this point
 	 */
 
 	sector_t secs;
 	bitmap_counter_t *bmc;
-	spin_lock_irq(&bitmap->lock);
-	bmc = bitmap_get_counter(bitmap, offset, &secs, 1);
+	spin_lock_irq(&bitmap->counts.lock);
+	bmc = bitmap_get_counter(&bitmap->counts, offset, &secs, 1);
 	if (!bmc) {
-		spin_unlock_irq(&bitmap->lock);
+		spin_unlock_irq(&bitmap->counts.lock);
 		return;
 	}
 	if (!*bmc) {
-		struct page *page;
 		*bmc = 2 | (needed ? NEEDED_MASK : 0);
-		bitmap_count_page(bitmap, offset, 1);
-		page = filemap_get_page(bitmap, offset >> bitmap->chunkshift);
-		set_page_attr(bitmap, page, BITMAP_PAGE_PENDING);
+		bitmap_count_page(&bitmap->counts, offset, 1);
+		bitmap_set_pending(&bitmap->counts, offset);
 		bitmap->allclean = 0;
 	}
-	spin_unlock_irq(&bitmap->lock);
+	spin_unlock_irq(&bitmap->counts.lock);
 }
 
 /* dirty the memory and file bits for bitmap chunks "s" to "e" */
@@ -1567,11 +1546,9 @@ void bitmap_dirty_bits(struct bitmap *bitmap, unsigned long s, unsigned long e)
 	unsigned long chunk;
 
 	for (chunk = s; chunk <= e; chunk++) {
-		sector_t sec = (sector_t)chunk << bitmap->chunkshift;
+		sector_t sec = (sector_t)chunk << bitmap->counts.chunkshift;
 		bitmap_set_memory_bits(bitmap, sec, 1);
-		spin_lock_irq(&bitmap->lock);
 		bitmap_file_set_bit(bitmap, sec);
-		spin_unlock_irq(&bitmap->lock);
 		if (sec < bitmap->mddev->recovery_cp)
 			/* We are asserting that the array is dirty,
 			 * so move the recovery_cp address back so
@@ -1616,11 +1593,15 @@ static void bitmap_free(struct bitmap *bitmap)
 	if (!bitmap) /* there was no bitmap */
 		return;
 
-	/* release the bitmap file and kill the daemon */
-	bitmap_file_put(bitmap);
+	/* Shouldn't be needed - but just in case.... */
+	wait_event(bitmap->write_wait,
+		   atomic_read(&bitmap->pending_writes) == 0);
+
+	/* release the bitmap file  */
+	bitmap_file_unmap(&bitmap->storage);
 
-	bp = bitmap->bp;
-	pages = bitmap->pages;
+	bp = bitmap->counts.bp;
+	pages = bitmap->counts.pages;
 
 	/* free all allocated memory */
 
@@ -1659,25 +1640,19 @@ int bitmap_create(struct mddev *mddev)
 {
 	struct bitmap *bitmap;
 	sector_t blocks = mddev->resync_max_sectors;
-	unsigned long chunks;
-	unsigned long pages;
 	struct file *file = mddev->bitmap_info.file;
 	int err;
 	struct sysfs_dirent *bm = NULL;
 
 	BUILD_BUG_ON(sizeof(bitmap_super_t) != 256);
 
-	if (!file
-	    && !mddev->bitmap_info.offset) /* bitmap disabled, nothing to do */
-		return 0;
-
 	BUG_ON(file && mddev->bitmap_info.offset);
 
 	bitmap = kzalloc(sizeof(*bitmap), GFP_KERNEL);
 	if (!bitmap)
 		return -ENOMEM;
 
-	spin_lock_init(&bitmap->lock);
+	spin_lock_init(&bitmap->counts.lock);
 	atomic_set(&bitmap->pending_writes, 0);
 	init_waitqueue_head(&bitmap->write_wait);
 	init_waitqueue_head(&bitmap->overflow_wait);
@@ -1693,7 +1668,7 @@ int bitmap_create(struct mddev *mddev)
 	} else
 		bitmap->sysfs_can_clear = NULL;
 
-	bitmap->file = file;
+	bitmap->storage.file = file;
 	if (file) {
 		get_file(file);
 		/* As future accesses to this file will use bmap,
@@ -1724,32 +1699,15 @@ int bitmap_create(struct mddev *mddev)
 		goto error;
 
 	bitmap->daemon_lastrun = jiffies;
-	bitmap->chunkshift = (ffz(~mddev->bitmap_info.chunksize)
-			      - BITMAP_BLOCK_SHIFT);
-
-	chunks = (blocks + (1 << bitmap->chunkshift) - 1) >>
-			bitmap->chunkshift;
-	pages = (chunks + PAGE_COUNTER_RATIO - 1) / PAGE_COUNTER_RATIO;
-
-	BUG_ON(!pages);
-
-	bitmap->chunks = chunks;
-	bitmap->pages = pages;
-	bitmap->missing_pages = pages;
-
-	bitmap->bp = kzalloc(pages * sizeof(*bitmap->bp), GFP_KERNEL);
-
-	err = -ENOMEM;
-	if (!bitmap->bp)
+	err = bitmap_resize(bitmap, blocks, mddev->bitmap_info.chunksize, 1);
+	if (err)
 		goto error;
 
 	printk(KERN_INFO "created bitmap (%lu pages) for device %s\n",
-		pages, bmname(bitmap));
+	       bitmap->counts.pages, bmname(bitmap));
 
 	mddev->bitmap = bitmap;
-
-
-	return (bitmap->flags & BITMAP_WRITE_ERROR) ? -EIO : 0;
+	return test_bit(BITMAP_WRITE_ERROR, &bitmap->flags) ? -EIO : 0;
 
  error:
 	bitmap_free(bitmap);
@@ -1790,13 +1748,17 @@ int bitmap_load(struct mddev *mddev)
 
 	if (err)
 		goto out;
+	clear_bit(BITMAP_STALE, &bitmap->flags);
+
+	/* Kick recovery in case any bits were set */
+	set_bit(MD_RECOVERY_NEEDED, &bitmap->mddev->recovery);
 
 	mddev->thread->timeout = mddev->bitmap_info.daemon_sleep;
 	md_wakeup_thread(mddev->thread);
 
 	bitmap_update_sb(bitmap);
 
-	if (bitmap->flags & BITMAP_WRITE_ERROR)
+	if (test_bit(BITMAP_WRITE_ERROR, &bitmap->flags))
 		err = -EIO;
 out:
 	return err;
@@ -1806,30 +1768,194 @@ EXPORT_SYMBOL_GPL(bitmap_load);
 void bitmap_status(struct seq_file *seq, struct bitmap *bitmap)
 {
 	unsigned long chunk_kb;
-	unsigned long flags;
+	struct bitmap_counts *counts;
 
 	if (!bitmap)
 		return;
 
-	spin_lock_irqsave(&bitmap->lock, flags);
+	counts = &bitmap->counts;
+
 	chunk_kb = bitmap->mddev->bitmap_info.chunksize >> 10;
 	seq_printf(seq, "bitmap: %lu/%lu pages [%luKB], "
 		   "%lu%s chunk",
-		   bitmap->pages - bitmap->missing_pages,
-		   bitmap->pages,
-		   (bitmap->pages - bitmap->missing_pages)
+		   counts->pages - counts->missing_pages,
+		   counts->pages,
+		   (counts->pages - counts->missing_pages)
 		   << (PAGE_SHIFT - 10),
 		   chunk_kb ? chunk_kb : bitmap->mddev->bitmap_info.chunksize,
 		   chunk_kb ? "KB" : "B");
-	if (bitmap->file) {
+	if (bitmap->storage.file) {
 		seq_printf(seq, ", file: ");
-		seq_path(seq, &bitmap->file->f_path, " \t\n");
+		seq_path(seq, &bitmap->storage.file->f_path, " \t\n");
 	}
 
 	seq_printf(seq, "\n");
-	spin_unlock_irqrestore(&bitmap->lock, flags);
 }
 
+int bitmap_resize(struct bitmap *bitmap, sector_t blocks,
+		  int chunksize, int init)
+{
+	/* If chunk_size is 0, choose an appropriate chunk size.
+	 * Then possibly allocate new storage space.
+	 * Then quiesce, copy bits, replace bitmap, and re-start
+	 *
+	 * This function is called both to set up the initial bitmap
+	 * and to resize the bitmap while the array is active.
+	 * If this happens as a result of the array being resized,
+	 * chunksize will be zero, and we need to choose a suitable
+	 * chunksize, otherwise we use what we are given.
+	 */
+	struct bitmap_storage store;
+	struct bitmap_counts old_counts;
+	unsigned long chunks;
+	sector_t block;
+	sector_t old_blocks, new_blocks;
+	int chunkshift;
+	int ret = 0;
+	long pages;
+	struct bitmap_page *new_bp;
+
+	if (chunksize == 0) {
+		/* If there is enough space, leave the chunk size unchanged,
+		 * else increase by factor of two until there is enough space.
+		 */
+		long bytes;
+		long space = bitmap->mddev->bitmap_info.space;
+
+		if (space == 0) {
+			/* We don't know how much space there is, so limit
+			 * to current size - in sectors.
+			 */
+			bytes = DIV_ROUND_UP(bitmap->counts.chunks, 8);
+			if (!bitmap->mddev->bitmap_info.external)
+				bytes += sizeof(bitmap_super_t);
+			space = DIV_ROUND_UP(bytes, 512);
+			bitmap->mddev->bitmap_info.space = space;
+		}
+		chunkshift = bitmap->counts.chunkshift;
+		chunkshift--;
+		do {
+			/* 'chunkshift' is shift from block size to chunk size */
+			chunkshift++;
+			chunks = DIV_ROUND_UP_SECTOR_T(blocks, 1 << chunkshift);
+			bytes = DIV_ROUND_UP(chunks, 8);
+			if (!bitmap->mddev->bitmap_info.external)
+				bytes += sizeof(bitmap_super_t);
+		} while (bytes > (space << 9));
+	} else
+		chunkshift = ffz(~chunksize) - BITMAP_BLOCK_SHIFT;
+
+	chunks = DIV_ROUND_UP_SECTOR_T(blocks, 1 << chunkshift);
+	memset(&store, 0, sizeof(store));
+	if (bitmap->mddev->bitmap_info.offset || bitmap->mddev->bitmap_info.file)
+		ret = bitmap_storage_alloc(&store, chunks,
+					   !bitmap->mddev->bitmap_info.external);
+	if (ret)
+		goto err;
+
+	pages = DIV_ROUND_UP(chunks, PAGE_COUNTER_RATIO);
+
+	new_bp = kzalloc(pages * sizeof(*new_bp), GFP_KERNEL);
+	ret = -ENOMEM;
+	if (!new_bp) {
+		bitmap_file_unmap(&store);
+		goto err;
+	}
+
+	if (!init)
+		bitmap->mddev->pers->quiesce(bitmap->mddev, 1);
+
+	store.file = bitmap->storage.file;
+	bitmap->storage.file = NULL;
+
+	if (store.sb_page && bitmap->storage.sb_page)
+		memcpy(page_address(store.sb_page),
+		       page_address(bitmap->storage.sb_page),
+		       sizeof(bitmap_super_t));
+	bitmap_file_unmap(&bitmap->storage);
+	bitmap->storage = store;
+
+	old_counts = bitmap->counts;
+	bitmap->counts.bp = new_bp;
+	bitmap->counts.pages = pages;
+	bitmap->counts.missing_pages = pages;
+	bitmap->counts.chunkshift = chunkshift;
+	bitmap->counts.chunks = chunks;
+	bitmap->mddev->bitmap_info.chunksize = 1 << (chunkshift +
+						     BITMAP_BLOCK_SHIFT);
+
+	blocks = min(old_counts.chunks << old_counts.chunkshift,
+		     chunks << chunkshift);
+
+	spin_lock_irq(&bitmap->counts.lock);
+	for (block = 0; block < blocks; ) {
+		bitmap_counter_t *bmc_old, *bmc_new;
+		int set;
+
+		bmc_old = bitmap_get_counter(&old_counts, block,
+					     &old_blocks, 0);
+		set = bmc_old && NEEDED(*bmc_old);
+
+		if (set) {
+			bmc_new = bitmap_get_counter(&bitmap->counts, block,
+						     &new_blocks, 1);
+			if (*bmc_new == 0) {
+				/* need to set on-disk bits too. */
+				sector_t end = block + new_blocks;
+				sector_t start = block >> chunkshift;
+				start <<= chunkshift;
+				while (start < end) {
+					bitmap_file_set_bit(bitmap, block);
+					start += 1 << chunkshift;
+				}
+				*bmc_new = 2;
+				bitmap_count_page(&bitmap->counts,
+						  block, 1);
+				bitmap_set_pending(&bitmap->counts,
+						   block);
+			}
+			*bmc_new |= NEEDED_MASK;
+			if (new_blocks < old_blocks)
+				old_blocks = new_blocks;
+		}
+		block += old_blocks;
+	}
+
+	if (!init) {
+		int i;
+		while (block < (chunks << chunkshift)) {
+			bitmap_counter_t *bmc;
+			bmc = bitmap_get_counter(&bitmap->counts, block,
+						 &new_blocks, 1);
+			if (bmc) {
+				/* new space.  It needs to be resynced, so
+				 * we set NEEDED_MASK.
+				 */
+				if (*bmc == 0) {
+					*bmc = NEEDED_MASK | 2;
+					bitmap_count_page(&bitmap->counts,
+							  block, 1);
+					bitmap_set_pending(&bitmap->counts,
+							   block);
+				}
+			}
+			block += new_blocks;
+		}
+		for (i = 0; i < bitmap->storage.file_pages; i++)
+			set_page_attr(bitmap, i, BITMAP_PAGE_DIRTY);
+	}
+	spin_unlock_irq(&bitmap->counts.lock);
+
+	if (!init) {
+		bitmap_unplug(bitmap);
+		bitmap->mddev->pers->quiesce(bitmap->mddev, 0);
+	}
+	ret = 0;
+err:
+	return ret;
+}
+EXPORT_SYMBOL_GPL(bitmap_resize);
+
 static ssize_t
 location_show(struct mddev *mddev, char *page)
 {
@@ -1923,6 +2049,43 @@ location_store(struct mddev *mddev, const char *buf, size_t len)
 static struct md_sysfs_entry bitmap_location =
 __ATTR(location, S_IRUGO|S_IWUSR, location_show, location_store);
 
+/* 'bitmap/space' is the space available at 'location' for the
+ * bitmap.  This allows the kernel to know when it is safe to
+ * resize the bitmap to match a resized array.
+ */
+static ssize_t
+space_show(struct mddev *mddev, char *page)
+{
+	return sprintf(page, "%lu\n", mddev->bitmap_info.space);
+}
+
+static ssize_t
+space_store(struct mddev *mddev, const char *buf, size_t len)
+{
+	unsigned long sectors;
+	int rv;
+
+	rv = kstrtoul(buf, 10, &sectors);
+	if (rv)
+		return rv;
+
+	if (sectors == 0)
+		return -EINVAL;
+
+	if (mddev->bitmap &&
+	    sectors < (mddev->bitmap->storage.bytes + 511) >> 9)
+		return -EFBIG; /* Bitmap is too big for this small space */
+
+	/* could make sure it isn't too big, but that isn't really
+	 * needed - user-space should be careful.
+	 */
+	mddev->bitmap_info.space = sectors;
+	return len;
+}
+
+static struct md_sysfs_entry bitmap_space =
+__ATTR(space, S_IRUGO|S_IWUSR, space_show, space_store);
+
 static ssize_t
 timeout_show(struct mddev *mddev, char *page)
 {
@@ -2098,6 +2261,7 @@ __ATTR(max_backlog_used, S_IRUGO | S_IWUSR,
 
 static struct attribute *md_bitmap_attrs[] = {
 	&bitmap_location.attr,
+	&bitmap_space.attr,
 	&bitmap_timeout.attr,
 	&bitmap_backlog.attr,
 	&bitmap_chunksize.attr,
diff --git a/drivers/md/bitmap.h b/drivers/md/bitmap.h
index b44b0aba2d4..df4aeb6ac6f 100644
--- a/drivers/md/bitmap.h
+++ b/drivers/md/bitmap.h
@@ -111,9 +111,9 @@ typedef __u16 bitmap_counter_t;
 
 /* use these for bitmap->flags and bitmap->sb->state bit-fields */
 enum bitmap_state {
-	BITMAP_STALE  = 0x002,  /* the bitmap file is out of date or had -EIO */
-	BITMAP_WRITE_ERROR = 0x004, /* A write error has occurred */
-	BITMAP_HOSTENDIAN = 0x8000,
+	BITMAP_STALE	   = 1,  /* the bitmap file is out of date or had -EIO */
+	BITMAP_WRITE_ERROR = 2, /* A write error has occurred */
+	BITMAP_HOSTENDIAN  =15,
 };
 
 /* the superblock at the front of the bitmap file -- little endian */
@@ -128,8 +128,10 @@ typedef struct bitmap_super_s {
 	__le32 chunksize;    /* 52  the bitmap chunk size in bytes */
 	__le32 daemon_sleep; /* 56  seconds between disk flushes */
 	__le32 write_behind; /* 60  number of outstanding write-behind writes */
+	__le32 sectors_reserved; /* 64 number of 512-byte sectors that are
+				  * reserved for the bitmap. */
 
-	__u8  pad[256 - 64]; /* set to zero */
+	__u8  pad[256 - 68]; /* set to zero */
 } bitmap_super_t;
 
 /* notes:
@@ -160,35 +162,48 @@ struct bitmap_page {
 	 */
 	unsigned int hijacked:1;
 	/*
+	 * If any counter in this page is '1' or '2' - and so could be
+	 * cleared then that page is marked as 'pending'
+	 */
+	unsigned int pending:1;
+	/*
 	 * count of dirty bits on the page
 	 */
-	unsigned int  count:31;
+	unsigned int  count:30;
 };
 
 /* the main bitmap structure - one per mddev */
 struct bitmap {
-	struct bitmap_page *bp;
-	unsigned long pages; /* total number of pages in the bitmap */
-	unsigned long missing_pages; /* number of pages not yet allocated */
 
-	struct mddev *mddev; /* the md device that the bitmap is for */
+	struct bitmap_counts {
+		spinlock_t lock;
+		struct bitmap_page *bp;
+		unsigned long pages;		/* total number of pages
+						 * in the bitmap */
+		unsigned long missing_pages;	/* number of pages
+						 * not yet allocated */
+		unsigned long chunkshift;	/* chunksize = 2^chunkshift
+						 * (for bitops) */
+		unsigned long chunks;		/* Total number of data
+						 * chunks for the array */
+	} counts;
 
-	/* bitmap chunksize -- how much data does each bit represent? */
-	unsigned long chunkshift; /* chunksize = 2^(chunkshift+9) (for bitops) */
-	unsigned long chunks; /* total number of data chunks for the array */
+	struct mddev *mddev; /* the md device that the bitmap is for */
 
 	__u64	events_cleared;
 	int need_sync;
 
-	/* bitmap spinlock */
-	spinlock_t lock;
-
-	struct file *file; /* backing disk file */
-	struct page *sb_page; /* cached copy of the bitmap file superblock */
-	struct page **filemap; /* list of cache pages for the file */
-	unsigned long *filemap_attr; /* attributes associated w/ filemap pages */
-	unsigned long file_pages; /* number of pages in the file */
-	int last_page_size; /* bytes in the last page */
+	struct bitmap_storage {
+		struct file *file;		/* backing disk file */
+		struct page *sb_page;		/* cached copy of the bitmap
+						 * file superblock */
+		struct page **filemap;		/* list of cache pages for
+						 * the file */
+		unsigned long *filemap_attr;	/* attributes associated
+						 * w/ filemap pages */
+		unsigned long file_pages;	/* number of pages in the file*/
+		unsigned long bytes;		/* total bytes in the bitmap */
+	} storage;
 
 	unsigned long flags;
 
@@ -242,6 +257,9 @@ void bitmap_cond_end_sync(struct bitmap *bitmap, sector_t sector);
 
 void bitmap_unplug(struct bitmap *bitmap);
 void bitmap_daemon_work(struct mddev *mddev);
+
+int bitmap_resize(struct bitmap *bitmap, sector_t blocks,
+		  int chunksize, int init);
 #endif
 
 #endif
diff --git a/drivers/md/dm-log-userspace-transfer.c b/drivers/md/dm-log-userspace-transfer.c
index 1f23e048f07..08d9a207259 100644
--- a/drivers/md/dm-log-userspace-transfer.c
+++ b/drivers/md/dm-log-userspace-transfer.c
@@ -134,7 +134,7 @@ static void cn_ulog_callback(struct cn_msg *msg, struct netlink_skb_parms *nsp)
 {
 	struct dm_ulog_request *tfr = (struct dm_ulog_request *)(msg + 1);
 
-	if (!cap_raised(current_cap(), CAP_SYS_ADMIN))
+	if (!capable(CAP_SYS_ADMIN))
 		return;
 
 	spin_lock(&receiving_list_lock);
diff --git a/drivers/md/dm-mpath.c b/drivers/md/dm-mpath.c
index 922a3385eea..754f38f8a69 100644
--- a/drivers/md/dm-mpath.c
+++ b/drivers/md/dm-mpath.c
@@ -718,8 +718,8 @@ static int parse_hw_handler(struct dm_arg_set *as, struct multipath *m)
 		return 0;
 
 	m->hw_handler_name = kstrdup(dm_shift_arg(as), GFP_KERNEL);
-	request_module("scsi_dh_%s", m->hw_handler_name);
-	if (scsi_dh_handler_exist(m->hw_handler_name) == 0) {
+	if (!try_then_request_module(scsi_dh_handler_exist(m->hw_handler_name),
+				     "scsi_dh_%s", m->hw_handler_name)) {
 		ti->error = "unknown hardware handler type";
 		ret = -EINVAL;
 		goto fail;
diff --git a/drivers/md/dm-raid.c b/drivers/md/dm-raid.c
index 68965e66324..017c34d78d6 100644
--- a/drivers/md/dm-raid.c
+++ b/drivers/md/dm-raid.c
@@ -155,10 +155,7 @@ static void context_free(struct raid_set *rs)
 	for (i = 0; i < rs->md.raid_disks; i++) {
 		if (rs->dev[i].meta_dev)
 			dm_put_device(rs->ti, rs->dev[i].meta_dev);
-		if (rs->dev[i].rdev.sb_page)
-			put_page(rs->dev[i].rdev.sb_page);
-		rs->dev[i].rdev.sb_page = NULL;
-		rs->dev[i].rdev.sb_loaded = 0;
+		md_rdev_clear(&rs->dev[i].rdev);
 		if (rs->dev[i].data_dev)
 			dm_put_device(rs->ti, rs->dev[i].data_dev);
 	}
@@ -606,7 +603,7 @@ static int read_disk_sb(struct md_rdev *rdev, int size)
 	if (!sync_page_io(rdev, 0, size, rdev->sb_page, READ, 1)) {
 		DMERR("Failed to read superblock of device at position %d",
 		      rdev->raid_disk);
-		set_bit(Faulty, &rdev->flags);
+		md_error(rdev->mddev, rdev);
 		return -EINVAL;
 	}
 
@@ -617,16 +614,18 @@ static int read_disk_sb(struct md_rdev *rdev, int size)
 
 static void super_sync(struct mddev *mddev, struct md_rdev *rdev)
 {
-	struct md_rdev *r;
+	int i;
 	uint64_t failed_devices;
 	struct dm_raid_superblock *sb;
+	struct raid_set *rs = container_of(mddev, struct raid_set, md);
 
 	sb = page_address(rdev->sb_page);
 	failed_devices = le64_to_cpu(sb->failed_devices);
 
-	rdev_for_each(r, mddev)
-		if ((r->raid_disk >= 0) && test_bit(Faulty, &r->flags))
-			failed_devices |= (1ULL << r->raid_disk);
+	for (i = 0; i < mddev->raid_disks; i++)
+		if (!rs->dev[i].data_dev ||
+		    test_bit(Faulty, &(rs->dev[i].rdev.flags)))
+			failed_devices |= (1ULL << i);
 
 	memset(sb, 0, sizeof(*sb));
 
@@ -1252,12 +1251,13 @@ static void raid_resume(struct dm_target *ti)
 {
 	struct raid_set *rs = ti->private;
 
+	set_bit(MD_CHANGE_DEVS, &rs->md.flags);
 	if (!rs->bitmap_loaded) {
 		bitmap_load(&rs->md);
 		rs->bitmap_loaded = 1;
-	} else
-		md_wakeup_thread(rs->md.thread);
+	}
 
+	clear_bit(MD_RECOVERY_FROZEN, &rs->md.recovery);
 	mddev_resume(&rs->md);
 }
 
diff --git a/drivers/md/dm-thin.c b/drivers/md/dm-thin.c
index 213ae32a0fc..eb3d138ff55 100644
--- a/drivers/md/dm-thin.c
+++ b/drivers/md/dm-thin.c
@@ -279,8 +279,10 @@ static void __cell_release(struct cell *cell, struct bio_list *inmates)
 
 	hlist_del(&cell->list);
 
-	bio_list_add(inmates, cell->holder);
-	bio_list_merge(inmates, &cell->bios);
+	if (inmates) {
+		bio_list_add(inmates, cell->holder);
+		bio_list_merge(inmates, &cell->bios);
+	}
 
 	mempool_free(cell, prison->cell_pool);
 }
@@ -303,9 +305,10 @@ static void cell_release(struct cell *cell, struct bio_list *bios)
  */
 static void __cell_release_singleton(struct cell *cell, struct bio *bio)
 {
-	hlist_del(&cell->list);
 	BUG_ON(cell->holder != bio);
 	BUG_ON(!bio_list_empty(&cell->bios));
+
+	__cell_release(cell, NULL);
 }
 
 static void cell_release_singleton(struct cell *cell, struct bio *bio)
@@ -1177,6 +1180,7 @@ static void no_space(struct cell *cell)
 static void process_discard(struct thin_c *tc, struct bio *bio)
 {
 	int r;
+	unsigned long flags;
 	struct pool *pool = tc->pool;
 	struct cell *cell, *cell2;
 	struct cell_key key, key2;
@@ -1218,7 +1222,9 @@ static void process_discard(struct thin_c *tc, struct bio *bio)
 			m->bio = bio;
 
 			if (!ds_add_work(&pool->all_io_ds, &m->list)) {
+				spin_lock_irqsave(&pool->lock, flags);
 				list_add(&m->list, &pool->prepared_discards);
+				spin_unlock_irqrestore(&pool->lock, flags);
 				wake_worker(pool);
 			}
 		} else {
@@ -1626,6 +1632,21 @@ static int bind_control_target(struct pool *pool, struct dm_target *ti)
 	pool->low_water_blocks = pt->low_water_blocks;
 	pool->pf = pt->pf;
 
+	/*
+	 * If discard_passdown was enabled verify that the data device
+	 * supports discards.  Disable discard_passdown if not; otherwise
+	 * -EOPNOTSUPP will be returned.
+	 */
+	if (pt->pf.discard_passdown) {
+		struct request_queue *q = bdev_get_queue(pt->data_dev->bdev);
+		if (!q || !blk_queue_discard(q)) {
+			char buf[BDEVNAME_SIZE];
+			DMWARN("Discard unsupported by data device (%s): Disabling discard passdown.",
+			       bdevname(pt->data_dev->bdev, buf));
+			pool->pf.discard_passdown = 0;
+		}
+	}
+
 	return 0;
 }
 
@@ -1982,19 +2003,6 @@ static int pool_ctr(struct dm_target *ti, unsigned argc, char **argv)
 		goto out_flags_changed;
 	}
 
-	/*
-	 * If discard_passdown was enabled verify that the data device
-	 * supports discards.  Disable discard_passdown if not; otherwise
-	 * -EOPNOTSUPP will be returned.
-	 */
-	if (pf.discard_passdown) {
-		struct request_queue *q = bdev_get_queue(data_dev->bdev);
-		if (!q || !blk_queue_discard(q)) {
-			DMWARN("Discard unsupported by data device: Disabling discard passdown.");
-			pf.discard_passdown = 0;
-		}
-	}
-
 	pt->pool = pool;
 	pt->ti = ti;
 	pt->metadata_dev = metadata_dev;
@@ -2379,7 +2387,7 @@ static int pool_status(struct dm_target *ti, status_type_t type,
 		       (unsigned long long)pt->low_water_blocks);
 
 		count = !pool->pf.zero_new_blocks + !pool->pf.discard_enabled +
-			!pool->pf.discard_passdown;
+			!pt->pf.discard_passdown;
 		DMEMIT("%u ", count);
 
 		if (!pool->pf.zero_new_blocks)
@@ -2388,7 +2396,7 @@ static int pool_status(struct dm_target *ti, status_type_t type,
 		if (!pool->pf.discard_enabled)
 			DMEMIT("ignore_discard ");
 
-		if (!pool->pf.discard_passdown)
+		if (!pt->pf.discard_passdown)
 			DMEMIT("no_discard_passdown ");
 
 		break;
@@ -2626,8 +2634,10 @@ static int thin_endio(struct dm_target *ti,
 	if (h->all_io_entry) {
 		INIT_LIST_HEAD(&work);
 		ds_dec(h->all_io_entry, &work);
+		spin_lock_irqsave(&pool->lock, flags);
 		list_for_each_entry_safe(m, tmp, &work, list)
 			list_add(&m->list, &pool->prepared_discards);
+		spin_unlock_irqrestore(&pool->lock, flags);
 	}
 
 	mempool_free(h, pool->endio_hook_pool);
@@ -2759,6 +2769,6 @@ static void dm_thin_exit(void)
 module_init(dm_thin_init);
 module_exit(dm_thin_exit);
 
-MODULE_DESCRIPTION(DM_NAME "device-mapper thin provisioning target");
+MODULE_DESCRIPTION(DM_NAME " thin provisioning target");
 MODULE_AUTHOR("Joe Thornber <dm-devel@redhat.com>");
 MODULE_LICENSE("GPL");
diff --git a/drivers/md/md.c b/drivers/md/md.c
index 477eb2e180c..1c2f9048e1a 100644
--- a/drivers/md/md.c
+++ b/drivers/md/md.c
@@ -391,6 +391,8 @@ void mddev_suspend(struct mddev *mddev)
 	synchronize_rcu();
 	wait_event(mddev->sb_wait, atomic_read(&mddev->active_io) == 0);
 	mddev->pers->quiesce(mddev, 1);
+
+	del_timer_sync(&mddev->safemode_timer);
 }
 EXPORT_SYMBOL_GPL(mddev_suspend);
 
@@ -400,6 +402,7 @@ void mddev_resume(struct mddev *mddev)
 	wake_up(&mddev->sb_wait);
 	mddev->pers->quiesce(mddev, 0);
 
+	set_bit(MD_RECOVERY_NEEDED, &mddev->recovery);
 	md_wakeup_thread(mddev->thread);
 	md_wakeup_thread(mddev->sync_thread); /* possibly kick off a reshape */
 }
@@ -450,7 +453,7 @@ static void submit_flushes(struct work_struct *ws)
 			atomic_inc(&rdev->nr_pending);
 			atomic_inc(&rdev->nr_pending);
 			rcu_read_unlock();
-			bi = bio_alloc_mddev(GFP_KERNEL, 0, mddev);
+			bi = bio_alloc_mddev(GFP_NOIO, 0, mddev);
 			bi->bi_end_io = md_end_flush;
 			bi->bi_private = rdev;
 			bi->bi_bdev = rdev->bdev;
@@ -605,6 +608,7 @@ void mddev_init(struct mddev *mddev)
 	init_waitqueue_head(&mddev->sb_wait);
 	init_waitqueue_head(&mddev->recovery_wait);
 	mddev->reshape_position = MaxSector;
+	mddev->reshape_backwards = 0;
 	mddev->resync_min = 0;
 	mddev->resync_max = MaxSector;
 	mddev->level = LEVEL_NONE;
@@ -800,7 +804,7 @@ static int alloc_disk_sb(struct md_rdev * rdev)
 	return 0;
 }
 
-static void free_disk_sb(struct md_rdev * rdev)
+void md_rdev_clear(struct md_rdev *rdev)
 {
 	if (rdev->sb_page) {
 		put_page(rdev->sb_page);
@@ -813,8 +817,10 @@ static void free_disk_sb(struct md_rdev * rdev)
 		put_page(rdev->bb_page);
 		rdev->bb_page = NULL;
 	}
+	kfree(rdev->badblocks.page);
+	rdev->badblocks.page = NULL;
 }
-
+EXPORT_SYMBOL_GPL(md_rdev_clear);
 
 static void super_written(struct bio *bio, int error)
 {
@@ -885,6 +891,10 @@ int sync_page_io(struct md_rdev *rdev, sector_t sector, int size,
 		rdev->meta_bdev : rdev->bdev;
 	if (metadata_op)
 		bio->bi_sector = sector + rdev->sb_start;
+	else if (rdev->mddev->reshape_position != MaxSector &&
+		 (rdev->mddev->reshape_backwards ==
+		  (sector >= rdev->mddev->reshape_position)))
+		bio->bi_sector = sector + rdev->new_data_offset;
 	else
 		bio->bi_sector = sector + rdev->data_offset;
 	bio_add_page(bio, page, size, 0);
@@ -1032,12 +1042,17 @@ static unsigned int calc_sb_csum(mdp_super_t * sb)
 struct super_type  {
 	char		    *name;
 	struct module	    *owner;
-	int		    (*load_super)(struct md_rdev *rdev, struct md_rdev *refdev,
+	int		    (*load_super)(struct md_rdev *rdev,
+					  struct md_rdev *refdev,
 					  int minor_version);
-	int		    (*validate_super)(struct mddev *mddev, struct md_rdev *rdev);
-	void		    (*sync_super)(struct mddev *mddev, struct md_rdev *rdev);
+	int		    (*validate_super)(struct mddev *mddev,
+					      struct md_rdev *rdev);
+	void		    (*sync_super)(struct mddev *mddev,
+					  struct md_rdev *rdev);
 	unsigned long long  (*rdev_size_change)(struct md_rdev *rdev,
 						sector_t num_sectors);
+	int		    (*allow_new_offset)(struct md_rdev *rdev,
+						unsigned long long new_offset);
 };
 
 /*
@@ -1109,6 +1124,7 @@ static int super_90_load(struct md_rdev *rdev, struct md_rdev *refdev, int minor
 
 	rdev->preferred_minor = sb->md_minor;
 	rdev->data_offset = 0;
+	rdev->new_data_offset = 0;
 	rdev->sb_size = MD_SB_BYTES;
 	rdev->badblocks.shift = -1;
 
@@ -1182,7 +1198,11 @@ static int super_90_validate(struct mddev *mddev, struct md_rdev *rdev)
 		mddev->dev_sectors = ((sector_t)sb->size) * 2;
 		mddev->events = ev1;
 		mddev->bitmap_info.offset = 0;
+		mddev->bitmap_info.space = 0;
+		/* bitmap can use 60 K after the 4K superblocks */
 		mddev->bitmap_info.default_offset = MD_SB_BYTES >> 9;
+		mddev->bitmap_info.default_space = 64*2 - (MD_SB_BYTES >> 9);
+		mddev->reshape_backwards = 0;
 
 		if (mddev->minor_version >= 91) {
 			mddev->reshape_position = sb->reshape_position;
@@ -1190,6 +1210,8 @@ static int super_90_validate(struct mddev *mddev, struct md_rdev *rdev)
 			mddev->new_level = sb->new_level;
 			mddev->new_layout = sb->new_layout;
 			mddev->new_chunk_sectors = sb->new_chunk >> 9;
+			if (mddev->delta_disks < 0)
+				mddev->reshape_backwards = 1;
 		} else {
 			mddev->reshape_position = MaxSector;
 			mddev->delta_disks = 0;
@@ -1216,9 +1238,12 @@ static int super_90_validate(struct mddev *mddev, struct md_rdev *rdev)
 		mddev->max_disks = MD_SB_DISKS;
 
 		if (sb->state & (1<<MD_SB_BITMAP_PRESENT) &&
-		    mddev->bitmap_info.file == NULL)
+		    mddev->bitmap_info.file == NULL) {
 			mddev->bitmap_info.offset =
 				mddev->bitmap_info.default_offset;
+			mddev->bitmap_info.space =
+				mddev->bitmap_info.space;
+		}
 
 	} else if (mddev->pers == NULL) {
 		/* Insist on good event counter while assembling, except
@@ -1432,6 +1457,12 @@ super_90_rdev_size_change(struct md_rdev *rdev, sector_t num_sectors)
 	return num_sectors;
 }
 
+static int
+super_90_allow_new_offset(struct md_rdev *rdev, unsigned long long new_offset)
+{
+	/* non-zero offset changes not possible with v0.90 */
+	return new_offset == 0;
+}
 
 /*
  * version 1 superblock
@@ -1467,6 +1498,7 @@ static int super_1_load(struct md_rdev *rdev, struct md_rdev *refdev, int minor_
 	struct mdp_superblock_1 *sb;
 	int ret;
 	sector_t sb_start;
+	sector_t sectors;
 	char b[BDEVNAME_SIZE], b2[BDEVNAME_SIZE];
 	int bmask;
 
@@ -1521,9 +1553,18 @@ static int super_1_load(struct md_rdev *rdev, struct md_rdev *refdev, int minor_
 		       bdevname(rdev->bdev,b));
 		return -EINVAL;
 	}
+	if (sb->pad0 ||
+	    sb->pad3[0] ||
+	    memcmp(sb->pad3, sb->pad3+1, sizeof(sb->pad3) - sizeof(sb->pad3[1])))
+		/* Some padding is non-zero, might be a new feature */
+		return -EINVAL;
 
 	rdev->preferred_minor = 0xffff;
 	rdev->data_offset = le64_to_cpu(sb->data_offset);
+	rdev->new_data_offset = rdev->data_offset;
+	if ((le32_to_cpu(sb->feature_map) & MD_FEATURE_RESHAPE_ACTIVE) &&
+	    (le32_to_cpu(sb->feature_map) & MD_FEATURE_NEW_OFFSET))
+		rdev->new_data_offset += (s32)le32_to_cpu(sb->new_offset);
 	atomic_set(&rdev->corrected_errors, le32_to_cpu(sb->cnt_corrected_read));
 
 	rdev->sb_size = le32_to_cpu(sb->max_dev) * 2 + 256;
@@ -1534,6 +1575,9 @@ static int super_1_load(struct md_rdev *rdev, struct md_rdev *refdev, int minor_
 	if (minor_version
 	    && rdev->data_offset < sb_start + (rdev->sb_size/512))
 		return -EINVAL;
+	if (minor_version
+	    && rdev->new_data_offset < sb_start + (rdev->sb_size/512))
+		return -EINVAL;
 
 	if (sb->level == cpu_to_le32(LEVEL_MULTIPATH))
 		rdev->desc_nr = -1;
@@ -1605,16 +1649,14 @@ static int super_1_load(struct md_rdev *rdev, struct md_rdev *refdev, int minor_
 		else
 			ret = 0;
 	}
-	if (minor_version)
-		rdev->sectors = (i_size_read(rdev->bdev->bd_inode) >> 9) -
-			le64_to_cpu(sb->data_offset);
-	else
-		rdev->sectors = rdev->sb_start;
-	if (rdev->sectors < le64_to_cpu(sb->data_size))
+	if (minor_version) {
+		sectors = (i_size_read(rdev->bdev->bd_inode) >> 9);
+		sectors -= rdev->data_offset;
+	} else
+		sectors = rdev->sb_start;
+	if (sectors < le64_to_cpu(sb->data_size))
 		return -EINVAL;
 	rdev->sectors = le64_to_cpu(sb->data_size);
-	if (le64_to_cpu(sb->size) > rdev->sectors)
-		return -EINVAL;
 	return ret;
 }
 
@@ -1642,17 +1684,37 @@ static int super_1_validate(struct mddev *mddev, struct md_rdev *rdev)
 		mddev->dev_sectors = le64_to_cpu(sb->size);
 		mddev->events = ev1;
 		mddev->bitmap_info.offset = 0;
+		mddev->bitmap_info.space = 0;
+		/* Default location for bitmap is 1K after superblock
+		 * using 3K - total of 4K
+		 */
 		mddev->bitmap_info.default_offset = 1024 >> 9;
-		
+		mddev->bitmap_info.default_space = (4096-1024) >> 9;
+		mddev->reshape_backwards = 0;
+
 		mddev->recovery_cp = le64_to_cpu(sb->resync_offset);
 		memcpy(mddev->uuid, sb->set_uuid, 16);
 
 		mddev->max_disks =  (4096-256)/2;
 
 		if ((le32_to_cpu(sb->feature_map) & MD_FEATURE_BITMAP_OFFSET) &&
-		    mddev->bitmap_info.file == NULL )
+		    mddev->bitmap_info.file == NULL) {
 			mddev->bitmap_info.offset =
 				(__s32)le32_to_cpu(sb->bitmap_offset);
+			/* Metadata doesn't record how much space is available.
+			 * For 1.0, we assume we can use up to the superblock
+			 * if before, else to 4K beyond superblock.
+			 * For others, assume no change is possible.
+			 */
+			if (mddev->minor_version > 0)
+				mddev->bitmap_info.space = 0;
+			else if (mddev->bitmap_info.offset > 0)
+				mddev->bitmap_info.space =
+					8 - mddev->bitmap_info.offset;
+			else
+				mddev->bitmap_info.space =
+					-mddev->bitmap_info.offset;
+		}
 
 		if ((le32_to_cpu(sb->feature_map) & MD_FEATURE_RESHAPE_ACTIVE)) {
 			mddev->reshape_position = le64_to_cpu(sb->reshape_position);
@@ -1660,6 +1722,11 @@ static int super_1_validate(struct mddev *mddev, struct md_rdev *rdev)
 			mddev->new_level = le32_to_cpu(sb->new_level);
 			mddev->new_layout = le32_to_cpu(sb->new_layout);
 			mddev->new_chunk_sectors = le32_to_cpu(sb->new_chunk);
+			if (mddev->delta_disks < 0 ||
+			    (mddev->delta_disks == 0 &&
+			     (le32_to_cpu(sb->feature_map)
+			      & MD_FEATURE_RESHAPE_BACKWARDS)))
+				mddev->reshape_backwards = 1;
 		} else {
 			mddev->reshape_position = MaxSector;
 			mddev->delta_disks = 0;
@@ -1733,7 +1800,6 @@ static void super_1_sync(struct mddev *mddev, struct md_rdev *rdev)
 	sb->feature_map = 0;
 	sb->pad0 = 0;
 	sb->recovery_offset = cpu_to_le64(0);
-	memset(sb->pad1, 0, sizeof(sb->pad1));
 	memset(sb->pad3, 0, sizeof(sb->pad3));
 
 	sb->utime = cpu_to_le64((__u64)mddev->utime);
@@ -1755,6 +1821,8 @@ static void super_1_sync(struct mddev *mddev, struct md_rdev *rdev)
 		sb->devflags |= WriteMostly1;
 	else
 		sb->devflags &= ~WriteMostly1;
+	sb->data_offset = cpu_to_le64(rdev->data_offset);
+	sb->data_size = cpu_to_le64(rdev->sectors);
 
 	if (mddev->bitmap && mddev->bitmap_info.file == NULL) {
 		sb->bitmap_offset = cpu_to_le32((__u32)mddev->bitmap_info.offset);
@@ -1779,6 +1847,16 @@ static void super_1_sync(struct mddev *mddev, struct md_rdev *rdev)
 		sb->delta_disks = cpu_to_le32(mddev->delta_disks);
 		sb->new_level = cpu_to_le32(mddev->new_level);
 		sb->new_chunk = cpu_to_le32(mddev->new_chunk_sectors);
+		if (mddev->delta_disks == 0 &&
+		    mddev->reshape_backwards)
+			sb->feature_map
+				|= cpu_to_le32(MD_FEATURE_RESHAPE_BACKWARDS);
+		if (rdev->new_data_offset != rdev->data_offset) {
+			sb->feature_map
+				|= cpu_to_le32(MD_FEATURE_NEW_OFFSET);
+			sb->new_offset = cpu_to_le32((__u32)(rdev->new_data_offset
+							     - rdev->data_offset));
+		}
 	}
 
 	if (rdev->badblocks.count == 0)
@@ -1855,6 +1933,8 @@ super_1_rdev_size_change(struct md_rdev *rdev, sector_t num_sectors)
 	sector_t max_sectors;
 	if (num_sectors && num_sectors < rdev->mddev->dev_sectors)
 		return 0; /* component must fit device */
+	if (rdev->data_offset != rdev->new_data_offset)
+		return 0; /* too confusing */
 	if (rdev->sb_start < rdev->data_offset) {
 		/* minor versions 1 and 2; superblock before data */
 		max_sectors = i_size_read(rdev->bdev->bd_inode) >> 9;
@@ -1882,6 +1962,40 @@ super_1_rdev_size_change(struct md_rdev *rdev, sector_t num_sectors)
 		       rdev->sb_page);
 	md_super_wait(rdev->mddev);
 	return num_sectors;
+
+}
+
+static int
+super_1_allow_new_offset(struct md_rdev *rdev,
+			 unsigned long long new_offset)
+{
+	/* All necessary checks on new >= old have been done */
+	struct bitmap *bitmap;
+	if (new_offset >= rdev->data_offset)
+		return 1;
+
+	/* with 1.0 metadata, there is no metadata to tread on
+	 * so we can always move back */
+	if (rdev->mddev->minor_version == 0)
+		return 1;
+
+	/* otherwise we must be sure not to step on
+	 * any metadata, so stay:
+	 * 36K beyond start of superblock
+	 * beyond end of badblocks
+	 * beyond write-intent bitmap
+	 */
+	if (rdev->sb_start + (32+4)*2 > new_offset)
+		return 0;
+	bitmap = rdev->mddev->bitmap;
+	if (bitmap && !rdev->mddev->bitmap_info.file &&
+	    rdev->sb_start + rdev->mddev->bitmap_info.offset +
+	    bitmap->storage.file_pages * (PAGE_SIZE>>9) > new_offset)
+		return 0;
+	if (rdev->badblocks.sector + rdev->badblocks.size > new_offset)
+		return 0;
+
+	return 1;
 }
 
 static struct super_type super_types[] = {
@@ -1892,6 +2006,7 @@ static struct super_type super_types[] = {
 		.validate_super	    = super_90_validate,
 		.sync_super	    = super_90_sync,
 		.rdev_size_change   = super_90_rdev_size_change,
+		.allow_new_offset   = super_90_allow_new_offset,
 	},
 	[1] = {
 		.name	= "md-1",
@@ -1900,6 +2015,7 @@ static struct super_type super_types[] = {
 		.validate_super	    = super_1_validate,
 		.sync_super	    = super_1_sync,
 		.rdev_size_change   = super_1_rdev_size_change,
+		.allow_new_offset   = super_1_allow_new_offset,
 	},
 };
 
@@ -2103,9 +2219,7 @@ static void unbind_rdev_from_array(struct md_rdev * rdev)
 	sysfs_remove_link(&rdev->kobj, "block");
 	sysfs_put(rdev->sysfs_state);
 	rdev->sysfs_state = NULL;
-	kfree(rdev->badblocks.page);
 	rdev->badblocks.count = 0;
-	rdev->badblocks.page = NULL;
 	/* We need to delay this, otherwise we can deadlock when
 	 * writing to 'remove' to "dev/state".  We also need
 	 * to delay it due to rcu usage.
@@ -2156,7 +2270,7 @@ static void export_rdev(struct md_rdev * rdev)
 		bdevname(rdev->bdev,b));
 	if (rdev->mddev)
 		MD_BUG();
-	free_disk_sb(rdev);
+	md_rdev_clear(rdev);
 #ifndef MODULE
 	if (test_bit(AutoDetected, &rdev->flags))
 		md_autodetect_dev(rdev->bdev->bd_dev);
@@ -2807,9 +2921,8 @@ offset_show(struct md_rdev *rdev, char *page)
 static ssize_t
 offset_store(struct md_rdev *rdev, const char *buf, size_t len)
 {
-	char *e;
-	unsigned long long offset = simple_strtoull(buf, &e, 10);
-	if (e==buf || (*e && *e != '\n'))
+	unsigned long long offset;
+	if (strict_strtoull(buf, 10, &offset) < 0)
 		return -EINVAL;
 	if (rdev->mddev->pers && rdev->raid_disk >= 0)
 		return -EBUSY;
@@ -2824,6 +2937,63 @@ offset_store(struct md_rdev *rdev, const char *buf, size_t len)
 static struct rdev_sysfs_entry rdev_offset =
 __ATTR(offset, S_IRUGO|S_IWUSR, offset_show, offset_store);
 
+static ssize_t new_offset_show(struct md_rdev *rdev, char *page)
+{
+	return sprintf(page, "%llu\n",
+		       (unsigned long long)rdev->new_data_offset);
+}
+
+static ssize_t new_offset_store(struct md_rdev *rdev,
+				const char *buf, size_t len)
+{
+	unsigned long long new_offset;
+	struct mddev *mddev = rdev->mddev;
+
+	if (strict_strtoull(buf, 10, &new_offset) < 0)
+		return -EINVAL;
+
+	if (mddev->sync_thread)
+		return -EBUSY;
+	if (new_offset == rdev->data_offset)
+		/* reset is always permitted */
+		;
+	else if (new_offset > rdev->data_offset) {
+		/* must not push array size beyond rdev_sectors */
+		if (new_offset - rdev->data_offset
+		    + mddev->dev_sectors > rdev->sectors)
+				return -E2BIG;
+	}
+	/* Metadata worries about other space details. */
+
+	/* decreasing the offset is inconsistent with a backwards
+	 * reshape.
+	 */
+	if (new_offset < rdev->data_offset &&
+	    mddev->reshape_backwards)
+		return -EINVAL;
+	/* Increasing offset is inconsistent with forwards
+	 * reshape.  reshape_direction should be set to
+	 * 'backwards' first.
+	 */
+	if (new_offset > rdev->data_offset &&
+	    !mddev->reshape_backwards)
+		return -EINVAL;
+
+	if (mddev->pers && mddev->persistent &&
+	    !super_types[mddev->major_version]
+	    .allow_new_offset(rdev, new_offset))
+		return -E2BIG;
+	rdev->new_data_offset = new_offset;
+	if (new_offset > rdev->data_offset)
+		mddev->reshape_backwards = 1;
+	else if (new_offset < rdev->data_offset)
+		mddev->reshape_backwards = 0;
+
+	return len;
+}
+static struct rdev_sysfs_entry rdev_new_offset =
+__ATTR(new_offset, S_IRUGO|S_IWUSR, new_offset_show, new_offset_store);
+
 static ssize_t
 rdev_size_show(struct md_rdev *rdev, char *page)
 {
@@ -2868,6 +3038,8 @@ rdev_size_store(struct md_rdev *rdev, const char *buf, size_t len)
 
 	if (strict_blocks_to_sectors(buf, &sectors) < 0)
 		return -EINVAL;
+	if (rdev->data_offset != rdev->new_data_offset)
+		return -EINVAL; /* too confusing */
 	if (my_mddev->pers && rdev->raid_disk >= 0) {
 		if (my_mddev->persistent) {
 			sectors = super_types[my_mddev->major_version].
@@ -3004,6 +3176,7 @@ static struct attribute *rdev_default_attrs[] = {
 	&rdev_errors.attr,
 	&rdev_slot.attr,
 	&rdev_offset.attr,
+	&rdev_new_offset.attr,
 	&rdev_size.attr,
 	&rdev_recovery_start.attr,
 	&rdev_bad_blocks.attr,
@@ -3078,6 +3251,7 @@ int md_rdev_init(struct md_rdev *rdev)
 	rdev->raid_disk = -1;
 	rdev->flags = 0;
 	rdev->data_offset = 0;
+	rdev->new_data_offset = 0;
 	rdev->sb_events = 0;
 	rdev->last_read_error.tv_sec  = 0;
 	rdev->last_read_error.tv_nsec = 0;
@@ -3176,8 +3350,7 @@ static struct md_rdev *md_import_device(dev_t newdev, int super_format, int supe
 abort_free:
 	if (rdev->bdev)
 		unlock_rdev(rdev);
-	free_disk_sb(rdev);
-	kfree(rdev->badblocks.page);
+	md_rdev_clear(rdev);
 	kfree(rdev);
 	return ERR_PTR(err);
 }
@@ -3417,6 +3590,7 @@ level_store(struct mddev *mddev, const char *buf, size_t len)
 		mddev->new_chunk_sectors = mddev->chunk_sectors;
 		mddev->raid_disks -= mddev->delta_disks;
 		mddev->delta_disks = 0;
+		mddev->reshape_backwards = 0;
 		module_put(pers->owner);
 		printk(KERN_WARNING "md: %s: %s would not accept array\n",
 		       mdname(mddev), clevel);
@@ -3490,6 +3664,7 @@ level_store(struct mddev *mddev, const char *buf, size_t len)
 	mddev->layout = mddev->new_layout;
 	mddev->chunk_sectors = mddev->new_chunk_sectors;
 	mddev->delta_disks = 0;
+	mddev->reshape_backwards = 0;
 	mddev->degraded = 0;
 	if (mddev->pers->sync_request == NULL) {
 		/* this is now an array without redundancy, so
@@ -3499,10 +3674,8 @@ level_store(struct mddev *mddev, const char *buf, size_t len)
 		del_timer_sync(&mddev->safemode_timer);
 	}
 	pers->run(mddev);
-	mddev_resume(mddev);
 	set_bit(MD_CHANGE_DEVS, &mddev->flags);
-	set_bit(MD_RECOVERY_NEEDED, &mddev->recovery);
-	md_wakeup_thread(mddev->thread);
+	mddev_resume(mddev);
 	sysfs_notify(&mddev->kobj, NULL, "level");
 	md_new_event(mddev);
 	return rv;
@@ -3580,9 +3753,20 @@ raid_disks_store(struct mddev *mddev, const char *buf, size_t len)
 	if (mddev->pers)
 		rv = update_raid_disks(mddev, n);
 	else if (mddev->reshape_position != MaxSector) {
+		struct md_rdev *rdev;
 		int olddisks = mddev->raid_disks - mddev->delta_disks;
+
+		rdev_for_each(rdev, mddev) {
+			if (olddisks < n &&
+			    rdev->data_offset < rdev->new_data_offset)
+				return -EINVAL;
+			if (olddisks > n &&
+			    rdev->data_offset > rdev->new_data_offset)
+				return -EINVAL;
+		}
 		mddev->delta_disks = n - olddisks;
 		mddev->raid_disks = n;
+		mddev->reshape_backwards = (mddev->delta_disks < 0);
 	} else
 		mddev->raid_disks = n;
 	return rv ? rv : len;
@@ -4264,7 +4448,8 @@ sync_completed_show(struct mddev *mddev, char *page)
 	if (!test_bit(MD_RECOVERY_RUNNING, &mddev->recovery))
 		return sprintf(page, "none\n");
 
-	if (test_bit(MD_RECOVERY_SYNC, &mddev->recovery))
+	if (test_bit(MD_RECOVERY_SYNC, &mddev->recovery) ||
+	    test_bit(MD_RECOVERY_RESHAPE, &mddev->recovery))
 		max_sectors = mddev->resync_max_sectors;
 	else
 		max_sectors = mddev->dev_sectors;
@@ -4426,6 +4611,7 @@ reshape_position_show(struct mddev *mddev, char *page)
 static ssize_t
 reshape_position_store(struct mddev *mddev, const char *buf, size_t len)
 {
+	struct md_rdev *rdev;
 	char *e;
 	unsigned long long new = simple_strtoull(buf, &e, 10);
 	if (mddev->pers)
@@ -4434,9 +4620,12 @@ reshape_position_store(struct mddev *mddev, const char *buf, size_t len)
 		return -EINVAL;
 	mddev->reshape_position = new;
 	mddev->delta_disks = 0;
+	mddev->reshape_backwards = 0;
 	mddev->new_level = mddev->level;
 	mddev->new_layout = mddev->layout;
 	mddev->new_chunk_sectors = mddev->chunk_sectors;
+	rdev_for_each(rdev, mddev)
+		rdev->new_data_offset = rdev->data_offset;
 	return len;
 }
 
@@ -4445,6 +4634,42 @@ __ATTR(reshape_position, S_IRUGO|S_IWUSR, reshape_position_show,
        reshape_position_store);
 
 static ssize_t
+reshape_direction_show(struct mddev *mddev, char *page)
+{
+	return sprintf(page, "%s\n",
+		       mddev->reshape_backwards ? "backwards" : "forwards");
+}
+
+static ssize_t
+reshape_direction_store(struct mddev *mddev, const char *buf, size_t len)
+{
+	int backwards = 0;
+	if (cmd_match(buf, "forwards"))
+		backwards = 0;
+	else if (cmd_match(buf, "backwards"))
+		backwards = 1;
+	else
+		return -EINVAL;
+	if (mddev->reshape_backwards == backwards)
+		return len;
+
+	/* check if we are allowed to change */
+	if (mddev->delta_disks)
+		return -EBUSY;
+
+	if (mddev->persistent &&
+	    mddev->major_version == 0)
+		return -EINVAL;
+
+	mddev->reshape_backwards = backwards;
+	return len;
+}
+
+static struct md_sysfs_entry md_reshape_direction =
+__ATTR(reshape_direction, S_IRUGO|S_IWUSR, reshape_direction_show,
+       reshape_direction_store);
+
+static ssize_t
 array_size_show(struct mddev *mddev, char *page)
 {
 	if (mddev->external_size)
@@ -4499,6 +4724,7 @@ static struct attribute *md_default_attrs[] = {
 	&md_safe_delay.attr,
 	&md_array_state.attr,
 	&md_reshape_position.attr,
+	&md_reshape_direction.attr,
 	&md_array_size.attr,
 	&max_corr_read_errors.attr,
 	NULL,
@@ -4912,7 +5138,8 @@ int md_run(struct mddev *mddev)
 		err = -EINVAL;
 		mddev->pers->stop(mddev);
 	}
-	if (err == 0 && mddev->pers->sync_request) {
+	if (err == 0 && mddev->pers->sync_request &&
+	    (mddev->bitmap_info.file || mddev->bitmap_info.offset)) {
 		err = bitmap_create(mddev);
 		if (err) {
 			printk(KERN_ERR "%s: failed to create bitmap (%d)\n",
@@ -5062,6 +5289,7 @@ static void md_clean(struct mddev *mddev)
 	mddev->events = 0;
 	mddev->can_decrease_events = 0;
 	mddev->delta_disks = 0;
+	mddev->reshape_backwards = 0;
 	mddev->new_level = LEVEL_NONE;
 	mddev->new_layout = 0;
 	mddev->new_chunk_sectors = 0;
@@ -5077,6 +5305,7 @@ static void md_clean(struct mddev *mddev)
 	mddev->merge_check_needed = 0;
 	mddev->bitmap_info.offset = 0;
 	mddev->bitmap_info.default_offset = 0;
+	mddev->bitmap_info.default_space = 0;
 	mddev->bitmap_info.chunksize = 0;
 	mddev->bitmap_info.daemon_sleep = 0;
 	mddev->bitmap_info.max_write_behind = 0;
@@ -5419,7 +5648,7 @@ static int get_bitmap_file(struct mddev * mddev, void __user * arg)
 		goto out;
 
 	/* bitmap disabled, zero the first byte and copy out */
-	if (!mddev->bitmap || !mddev->bitmap->file) {
+	if (!mddev->bitmap || !mddev->bitmap->storage.file) {
 		file->pathname[0] = '\0';
 		goto copy_out;
 	}
@@ -5428,7 +5657,8 @@ static int get_bitmap_file(struct mddev * mddev, void __user * arg)
 	if (!buf)
 		goto out;
 
-	ptr = d_path(&mddev->bitmap->file->f_path, buf, sizeof(file->pathname));
+	ptr = d_path(&mddev->bitmap->storage.file->f_path,
+		     buf, sizeof(file->pathname));
 	if (IS_ERR(ptr))
 		goto out;
 
@@ -5873,6 +6103,7 @@ static int set_array_info(struct mddev * mddev, mdu_array_info_t *info)
 	set_bit(MD_CHANGE_DEVS, &mddev->flags);
 
 	mddev->bitmap_info.default_offset = MD_SB_BYTES >> 9;
+	mddev->bitmap_info.default_space = 64*2 - (MD_SB_BYTES >> 9);
 	mddev->bitmap_info.offset = 0;
 
 	mddev->reshape_position = MaxSector;
@@ -5886,6 +6117,7 @@ static int set_array_info(struct mddev * mddev, mdu_array_info_t *info)
 	mddev->new_chunk_sectors = mddev->chunk_sectors;
 	mddev->new_layout = mddev->layout;
 	mddev->delta_disks = 0;
+	mddev->reshape_backwards = 0;
 
 	return 0;
 }
@@ -5920,11 +6152,7 @@ static int update_size(struct mddev *mddev, sector_t num_sectors)
 	 */
 	if (mddev->sync_thread)
 		return -EBUSY;
-	if (mddev->bitmap)
-		/* Sorry, cannot grow a bitmap yet, just remove it,
-		 * grow, and re-add.
-		 */
-		return -EBUSY;
+
 	rdev_for_each(rdev, mddev) {
 		sector_t avail = rdev->sectors;
 
@@ -5942,6 +6170,7 @@ static int update_size(struct mddev *mddev, sector_t num_sectors)
 static int update_raid_disks(struct mddev *mddev, int raid_disks)
 {
 	int rv;
+	struct md_rdev *rdev;
 	/* change the number of raid disks */
 	if (mddev->pers->check_reshape == NULL)
 		return -EINVAL;
@@ -5950,11 +6179,27 @@ static int update_raid_disks(struct mddev *mddev, int raid_disks)
 		return -EINVAL;
 	if (mddev->sync_thread || mddev->reshape_position != MaxSector)
 		return -EBUSY;
+
+	rdev_for_each(rdev, mddev) {
+		if (mddev->raid_disks < raid_disks &&
+		    rdev->data_offset < rdev->new_data_offset)
+			return -EINVAL;
+		if (mddev->raid_disks > raid_disks &&
+		    rdev->data_offset > rdev->new_data_offset)
+			return -EINVAL;
+	}
+
 	mddev->delta_disks = raid_disks - mddev->raid_disks;
+	if (mddev->delta_disks < 0)
+		mddev->reshape_backwards = 1;
+	else if (mddev->delta_disks > 0)
+		mddev->reshape_backwards = 0;
 
 	rv = mddev->pers->check_reshape(mddev);
-	if (rv < 0)
+	if (rv < 0) {
 		mddev->delta_disks = 0;
+		mddev->reshape_backwards = 0;
+	}
 	return rv;
 }
 
@@ -6037,6 +6282,8 @@ static int update_array_info(struct mddev *mddev, mdu_array_info_t *info)
 				return -EINVAL;
 			mddev->bitmap_info.offset =
 				mddev->bitmap_info.default_offset;
+			mddev->bitmap_info.space =
+				mddev->bitmap_info.default_space;
 			mddev->pers->quiesce(mddev, 1);
 			rv = bitmap_create(mddev);
 			if (!rv)
@@ -6048,7 +6295,7 @@ static int update_array_info(struct mddev *mddev, mdu_array_info_t *info)
 			/* remove the bitmap */
 			if (!mddev->bitmap)
 				return -ENOENT;
-			if (mddev->bitmap->file)
+			if (mddev->bitmap->storage.file)
 				return -EINVAL;
 			mddev->pers->quiesce(mddev, 1);
 			bitmap_destroy(mddev);
@@ -6371,6 +6618,9 @@ static int md_open(struct block_device *bdev, fmode_t mode)
 	struct mddev *mddev = mddev_find(bdev->bd_dev);
 	int err;
 
+	if (!mddev)
+		return -ENODEV;
+
 	if (mddev->gendisk != bdev->bd_disk) {
 		/* we are racing with mddev_put which is discarding this
 		 * bd_disk.
@@ -6582,7 +6832,8 @@ static void status_resync(struct seq_file *seq, struct mddev * mddev)
 
 	resync = mddev->curr_resync - atomic_read(&mddev->recovery_active);
 
-	if (test_bit(MD_RECOVERY_SYNC, &mddev->recovery))
+	if (test_bit(MD_RECOVERY_SYNC, &mddev->recovery) ||
+	    test_bit(MD_RECOVERY_RESHAPE, &mddev->recovery))
 		max_sectors = mddev->resync_max_sectors;
 	else
 		max_sectors = mddev->dev_sectors;
@@ -7145,7 +7396,7 @@ void md_do_sync(struct mddev *mddev)
 			j = mddev->recovery_cp;
 
 	} else if (test_bit(MD_RECOVERY_RESHAPE, &mddev->recovery))
-		max_sectors = mddev->dev_sectors;
+		max_sectors = mddev->resync_max_sectors;
 	else {
 		/* recovery follows the physical size of devices */
 		max_sectors = mddev->dev_sectors;
@@ -7596,7 +7847,7 @@ void md_check_recovery(struct mddev *mddev)
 			goto unlock;
 
 		if (mddev->pers->sync_request) {
-			if (spares && mddev->bitmap && ! mddev->bitmap->file) {
+			if (spares) {
 				/* We are adding a device or devices to an array
 				 * which has the bitmap stored on all devices.
 				 * So make sure all bitmap pages get written
@@ -7644,6 +7895,20 @@ void md_wait_for_blocked_rdev(struct md_rdev *rdev, struct mddev *mddev)
 }
 EXPORT_SYMBOL(md_wait_for_blocked_rdev);
 
+void md_finish_reshape(struct mddev *mddev)
+{
+	/* called be personality module when reshape completes. */
+	struct md_rdev *rdev;
+
+	rdev_for_each(rdev, mddev) {
+		if (rdev->data_offset > rdev->new_data_offset)
+			rdev->sectors += rdev->data_offset - rdev->new_data_offset;
+		else
+			rdev->sectors -= rdev->new_data_offset - rdev->data_offset;
+		rdev->data_offset = rdev->new_data_offset;
+	}
+}
+EXPORT_SYMBOL(md_finish_reshape);
 
 /* Bad block management.
  * We can record which blocks on each device are 'bad' and so just
@@ -7892,10 +8157,15 @@ static int md_set_badblocks(struct badblocks *bb, sector_t s, int sectors,
 }
 
 int rdev_set_badblocks(struct md_rdev *rdev, sector_t s, int sectors,
-		       int acknowledged)
+		       int is_new)
 {
-	int rv = md_set_badblocks(&rdev->badblocks,
-				  s + rdev->data_offset, sectors, acknowledged);
+	int rv;
+	if (is_new)
+		s += rdev->new_data_offset;
+	else
+		s += rdev->data_offset;
+	rv = md_set_badblocks(&rdev->badblocks,
+			      s, sectors, 0);
 	if (rv) {
 		/* Make sure they get written out promptly */
 		sysfs_notify_dirent_safe(rdev->sysfs_state);
@@ -8001,11 +8271,15 @@ out:
 	return rv;
 }
 
-int rdev_clear_badblocks(struct md_rdev *rdev, sector_t s, int sectors)
+int rdev_clear_badblocks(struct md_rdev *rdev, sector_t s, int sectors,
+			 int is_new)
 {
+	if (is_new)
+		s += rdev->new_data_offset;
+	else
+		s += rdev->data_offset;
 	return md_clear_badblocks(&rdev->badblocks,
-				  s + rdev->data_offset,
-				  sectors);
+				  s, sectors);
 }
 EXPORT_SYMBOL_GPL(rdev_clear_badblocks);
 
diff --git a/drivers/md/md.h b/drivers/md/md.h
index 1c2063ccf48..7b4a3c318ca 100644
--- a/drivers/md/md.h
+++ b/drivers/md/md.h
@@ -55,6 +55,7 @@ struct md_rdev {
 	int		sb_loaded;
 	__u64		sb_events;
 	sector_t	data_offset;	/* start of data in array */
+	sector_t	new_data_offset;/* only relevant while reshaping */
 	sector_t 	sb_start;	/* offset of the super block (in 512byte sectors) */
 	int		sb_size;	/* bytes in the superblock */
 	int		preferred_minor;	/* autorun support */
@@ -193,8 +194,9 @@ static inline int is_badblock(struct md_rdev *rdev, sector_t s, int sectors,
 	return 0;
 }
 extern int rdev_set_badblocks(struct md_rdev *rdev, sector_t s, int sectors,
-			      int acknowledged);
-extern int rdev_clear_badblocks(struct md_rdev *rdev, sector_t s, int sectors);
+			      int is_new);
+extern int rdev_clear_badblocks(struct md_rdev *rdev, sector_t s, int sectors,
+				int is_new);
 extern void md_ack_all_badblocks(struct badblocks *bb);
 
 struct mddev {
@@ -262,6 +264,7 @@ struct mddev {
 	sector_t			reshape_position;
 	int				delta_disks, new_level, new_layout;
 	int				new_chunk_sectors;
+	int				reshape_backwards;
 
 	atomic_t			plug_cnt;	/* If device is expecting
 							 * more bios soon.
@@ -390,10 +393,13 @@ struct mddev {
 						 * For external metadata, offset
 						 * from start of device. 
 						 */
+		unsigned long		space; /* space available at this offset */
 		loff_t			default_offset; /* this is the offset to use when
 							 * hot-adding a bitmap.  It should
 							 * eventually be settable by sysfs.
 							 */
+		unsigned long		default_space; /* space available at
+							* default offset */
 		struct mutex		mutex;
 		unsigned long		chunksize;
 		unsigned long		daemon_sleep; /* how many jiffies between updates? */
@@ -591,6 +597,7 @@ 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);
 extern void md_error(struct mddev *mddev, struct md_rdev *rdev);
+extern void md_finish_reshape(struct mddev *mddev);
 
 extern int mddev_congested(struct mddev *mddev, int bits);
 extern void md_flush_request(struct mddev *mddev, struct bio *bio);
@@ -615,6 +622,7 @@ extern int md_run(struct mddev *mddev);
 extern void md_stop(struct mddev *mddev);
 extern void md_stop_writes(struct mddev *mddev);
 extern int md_rdev_init(struct md_rdev *rdev);
+extern void md_rdev_clear(struct md_rdev *rdev);
 
 extern void mddev_suspend(struct mddev *mddev);
 extern void mddev_resume(struct mddev *mddev);
diff --git a/drivers/md/raid1.c b/drivers/md/raid1.c
index 15dd59b84e9..835de7168cd 100644
--- a/drivers/md/raid1.c
+++ b/drivers/md/raid1.c
@@ -1859,7 +1859,9 @@ static void fix_read_error(struct r1conf *conf, int read_disk,
 
 			rdev = conf->mirrors[d].rdev;
 			if (rdev &&
-			    test_bit(In_sync, &rdev->flags) &&
+			    (test_bit(In_sync, &rdev->flags) ||
+			     (!test_bit(Faulty, &rdev->flags) &&
+			      rdev->recovery_offset >= sect + s)) &&
 			    is_badblock(rdev, sect, s,
 					&first_bad, &bad_sectors) == 0 &&
 			    sync_page_io(rdev, sect, s<<9,
@@ -2024,7 +2026,7 @@ static void handle_sync_write_finished(struct r1conf *conf, struct r1bio *r1_bio
 			continue;
 		if (test_bit(BIO_UPTODATE, &bio->bi_flags) &&
 		    test_bit(R1BIO_MadeGood, &r1_bio->state)) {
-			rdev_clear_badblocks(rdev, r1_bio->sector, s);
+			rdev_clear_badblocks(rdev, r1_bio->sector, s, 0);
 		}
 		if (!test_bit(BIO_UPTODATE, &bio->bi_flags) &&
 		    test_bit(R1BIO_WriteError, &r1_bio->state)) {
@@ -2044,7 +2046,7 @@ static void handle_write_finished(struct r1conf *conf, struct r1bio *r1_bio)
 			struct md_rdev *rdev = conf->mirrors[m].rdev;
 			rdev_clear_badblocks(rdev,
 					     r1_bio->sector,
-					     r1_bio->sectors);
+					     r1_bio->sectors, 0);
 			rdev_dec_pending(rdev, conf->mddev);
 		} else if (r1_bio->bios[m] != NULL) {
 			/* This drive got a write error.  We need to
@@ -2598,7 +2600,8 @@ static struct r1conf *setup_conf(struct mddev *mddev)
 		if (!disk->rdev ||
 		    !test_bit(In_sync, &disk->rdev->flags)) {
 			disk->head_position = 0;
-			if (disk->rdev)
+			if (disk->rdev &&
+			    (disk->rdev->saved_raid_disk < 0))
 				conf->fullsync = 1;
 		} else if (conf->last_used < 0)
 			/*
@@ -2750,9 +2753,16 @@ static int raid1_resize(struct mddev *mddev, sector_t sectors)
 	 * any io in the removed space completes, but it hardly seems
 	 * worth it.
 	 */
-	md_set_array_sectors(mddev, raid1_size(mddev, sectors, 0));
-	if (mddev->array_sectors > raid1_size(mddev, sectors, 0))
+	sector_t newsize = raid1_size(mddev, sectors, 0);
+	if (mddev->external_size &&
+	    mddev->array_sectors > newsize)
 		return -EINVAL;
+	if (mddev->bitmap) {
+		int ret = bitmap_resize(mddev->bitmap, newsize, 0, 0);
+		if (ret)
+			return ret;
+	}
+	md_set_array_sectors(mddev, newsize);
 	set_capacity(mddev->gendisk, mddev->array_sectors);
 	revalidate_disk(mddev->gendisk);
 	if (sectors > mddev->dev_sectors &&
diff --git a/drivers/md/raid10.c b/drivers/md/raid10.c
index c8dbb84d535..987db37cb87 100644
--- a/drivers/md/raid10.c
+++ b/drivers/md/raid10.c
@@ -24,6 +24,7 @@
 #include <linux/module.h>
 #include <linux/seq_file.h>
 #include <linux/ratelimit.h>
+#include <linux/kthread.h>
 #include "md.h"
 #include "raid10.h"
 #include "raid0.h"
@@ -68,6 +69,11 @@ static int max_queued_requests = 1024;
 static void allow_barrier(struct r10conf *conf);
 static void lower_barrier(struct r10conf *conf);
 static int enough(struct r10conf *conf, int ignore);
+static sector_t reshape_request(struct mddev *mddev, sector_t sector_nr,
+				int *skipped);
+static void reshape_request_write(struct mddev *mddev, struct r10bio *r10_bio);
+static void end_reshape_write(struct bio *bio, int error);
+static void end_reshape(struct r10conf *conf);
 
 static void * r10bio_pool_alloc(gfp_t gfp_flags, void *data)
 {
@@ -112,7 +118,8 @@ static void * r10buf_pool_alloc(gfp_t gfp_flags, void *data)
 	if (!r10_bio)
 		return NULL;
 
-	if (test_bit(MD_RECOVERY_SYNC, &conf->mddev->recovery))
+	if (test_bit(MD_RECOVERY_SYNC, &conf->mddev->recovery) ||
+	    test_bit(MD_RECOVERY_RESHAPE, &conf->mddev->recovery))
 		nalloc = conf->copies; /* resync */
 	else
 		nalloc = 2; /* recovery */
@@ -140,9 +147,10 @@ static void * r10buf_pool_alloc(gfp_t gfp_flags, void *data)
 		struct bio *rbio = r10_bio->devs[j].repl_bio;
 		bio = r10_bio->devs[j].bio;
 		for (i = 0; i < RESYNC_PAGES; i++) {
-			if (j == 1 && !test_bit(MD_RECOVERY_SYNC,
-						&conf->mddev->recovery)) {
-				/* we can share bv_page's during recovery */
+			if (j > 0 && !test_bit(MD_RECOVERY_SYNC,
+					       &conf->mddev->recovery)) {
+				/* we can share bv_page's during recovery
+				 * and reshape */
 				struct bio *rbio = r10_bio->devs[0].bio;
 				page = rbio->bi_io_vec[i].bv_page;
 				get_page(page);
@@ -165,10 +173,11 @@ out_free_pages:
 	while (j--)
 		for (i = 0; i < RESYNC_PAGES ; i++)
 			safe_put_page(r10_bio->devs[j].bio->bi_io_vec[i].bv_page);
-	j = -1;
+	j = 0;
 out_free_bio:
-	while (++j < nalloc) {
-		bio_put(r10_bio->devs[j].bio);
+	for ( ; j < nalloc; j++) {
+		if (r10_bio->devs[j].bio)
+			bio_put(r10_bio->devs[j].bio);
 		if (r10_bio->devs[j].repl_bio)
 			bio_put(r10_bio->devs[j].repl_bio);
 	}
@@ -504,79 +513,96 @@ static void raid10_end_write_request(struct bio *bio, int error)
  * sector offset to a virtual address
  */
 
-static void raid10_find_phys(struct r10conf *conf, struct r10bio *r10bio)
+static void __raid10_find_phys(struct geom *geo, struct r10bio *r10bio)
 {
 	int n,f;
 	sector_t sector;
 	sector_t chunk;
 	sector_t stripe;
 	int dev;
-
 	int slot = 0;
 
 	/* now calculate first sector/dev */
-	chunk = r10bio->sector >> conf->chunk_shift;
-	sector = r10bio->sector & conf->chunk_mask;
+	chunk = r10bio->sector >> geo->chunk_shift;
+	sector = r10bio->sector & geo->chunk_mask;
 
-	chunk *= conf->near_copies;
+	chunk *= geo->near_copies;
 	stripe = chunk;
-	dev = sector_div(stripe, conf->raid_disks);
-	if (conf->far_offset)
-		stripe *= conf->far_copies;
+	dev = sector_div(stripe, geo->raid_disks);
+	if (geo->far_offset)
+		stripe *= geo->far_copies;
 
-	sector += stripe << conf->chunk_shift;
+	sector += stripe << geo->chunk_shift;
 
 	/* and calculate all the others */
-	for (n=0; n < conf->near_copies; n++) {
+	for (n = 0; n < geo->near_copies; n++) {
 		int d = dev;
 		sector_t s = sector;
 		r10bio->devs[slot].addr = sector;
 		r10bio->devs[slot].devnum = d;
 		slot++;
 
-		for (f = 1; f < conf->far_copies; f++) {
-			d += conf->near_copies;
-			if (d >= conf->raid_disks)
-				d -= conf->raid_disks;
-			s += conf->stride;
+		for (f = 1; f < geo->far_copies; f++) {
+			d += geo->near_copies;
+			if (d >= geo->raid_disks)
+				d -= geo->raid_disks;
+			s += geo->stride;
 			r10bio->devs[slot].devnum = d;
 			r10bio->devs[slot].addr = s;
 			slot++;
 		}
 		dev++;
-		if (dev >= conf->raid_disks) {
+		if (dev >= geo->raid_disks) {
 			dev = 0;
-			sector += (conf->chunk_mask + 1);
+			sector += (geo->chunk_mask + 1);
 		}
 	}
-	BUG_ON(slot != conf->copies);
+}
+
+static void raid10_find_phys(struct r10conf *conf, struct r10bio *r10bio)
+{
+	struct geom *geo = &conf->geo;
+
+	if (conf->reshape_progress != MaxSector &&
+	    ((r10bio->sector >= conf->reshape_progress) !=
+	     conf->mddev->reshape_backwards)) {
+		set_bit(R10BIO_Previous, &r10bio->state);
+		geo = &conf->prev;
+	} else
+		clear_bit(R10BIO_Previous, &r10bio->state);
+
+	__raid10_find_phys(geo, r10bio);
 }
 
 static sector_t raid10_find_virt(struct r10conf *conf, sector_t sector, int dev)
 {
 	sector_t offset, chunk, vchunk;
+	/* Never use conf->prev as this is only called during resync
+	 * or recovery, so reshape isn't happening
+	 */
+	struct geom *geo = &conf->geo;
 
-	offset = sector & conf->chunk_mask;
-	if (conf->far_offset) {
+	offset = sector & geo->chunk_mask;
+	if (geo->far_offset) {
 		int fc;
-		chunk = sector >> conf->chunk_shift;
-		fc = sector_div(chunk, conf->far_copies);
-		dev -= fc * conf->near_copies;
+		chunk = sector >> geo->chunk_shift;
+		fc = sector_div(chunk, geo->far_copies);
+		dev -= fc * geo->near_copies;
 		if (dev < 0)
-			dev += conf->raid_disks;
+			dev += geo->raid_disks;
 	} else {
-		while (sector >= conf->stride) {
-			sector -= conf->stride;
-			if (dev < conf->near_copies)
-				dev += conf->raid_disks - conf->near_copies;
+		while (sector >= geo->stride) {
+			sector -= geo->stride;
+			if (dev < geo->near_copies)
+				dev += geo->raid_disks - geo->near_copies;
 			else
-				dev -= conf->near_copies;
+				dev -= geo->near_copies;
 		}
-		chunk = sector >> conf->chunk_shift;
+		chunk = sector >> geo->chunk_shift;
 	}
-	vchunk = chunk * conf->raid_disks + dev;
-	sector_div(vchunk, conf->near_copies);
-	return (vchunk << conf->chunk_shift) + offset;
+	vchunk = chunk * geo->raid_disks + dev;
+	sector_div(vchunk, geo->near_copies);
+	return (vchunk << geo->chunk_shift) + offset;
 }
 
 /**
@@ -597,10 +623,17 @@ static int raid10_mergeable_bvec(struct request_queue *q,
 	struct r10conf *conf = mddev->private;
 	sector_t sector = bvm->bi_sector + get_start_sect(bvm->bi_bdev);
 	int max;
-	unsigned int chunk_sectors = mddev->chunk_sectors;
+	unsigned int chunk_sectors;
 	unsigned int bio_sectors = bvm->bi_size >> 9;
+	struct geom *geo = &conf->geo;
+
+	chunk_sectors = (conf->geo.chunk_mask & conf->prev.chunk_mask) + 1;
+	if (conf->reshape_progress != MaxSector &&
+	    ((sector >= conf->reshape_progress) !=
+	     conf->mddev->reshape_backwards))
+		geo = &conf->prev;
 
-	if (conf->near_copies < conf->raid_disks) {
+	if (geo->near_copies < geo->raid_disks) {
 		max = (chunk_sectors - ((sector & (chunk_sectors - 1))
 					+ bio_sectors)) << 9;
 		if (max < 0)
@@ -614,6 +647,12 @@ static int raid10_mergeable_bvec(struct request_queue *q,
 	if (mddev->merge_check_needed) {
 		struct r10bio r10_bio;
 		int s;
+		if (conf->reshape_progress != MaxSector) {
+			/* Cannot give any guidance during reshape */
+			if (max <= biovec->bv_len && bio_sectors == 0)
+				return biovec->bv_len;
+			return 0;
+		}
 		r10_bio.sector = sector;
 		raid10_find_phys(conf, &r10_bio);
 		rcu_read_lock();
@@ -681,6 +720,7 @@ static struct md_rdev *read_balance(struct r10conf *conf,
 	struct md_rdev *rdev, *best_rdev;
 	int do_balance;
 	int best_slot;
+	struct geom *geo = &conf->geo;
 
 	raid10_find_phys(conf, r10_bio);
 	rcu_read_lock();
@@ -761,11 +801,11 @@ retry:
 		 * sequential read speed for 'far copies' arrays.  So only
 		 * keep it for 'near' arrays, and review those later.
 		 */
-		if (conf->near_copies > 1 && !atomic_read(&rdev->nr_pending))
+		if (geo->near_copies > 1 && !atomic_read(&rdev->nr_pending))
 			break;
 
 		/* for far > 1 always use the lowest address */
-		if (conf->far_copies > 1)
+		if (geo->far_copies > 1)
 			new_distance = r10_bio->devs[slot].addr;
 		else
 			new_distance = abs(r10_bio->devs[slot].addr -
@@ -812,7 +852,10 @@ static int raid10_congested(void *data, int bits)
 	if (mddev_congested(mddev, bits))
 		return 1;
 	rcu_read_lock();
-	for (i = 0; i < conf->raid_disks && ret == 0; i++) {
+	for (i = 0;
+	     (i < conf->geo.raid_disks || i < conf->prev.raid_disks)
+		     && ret == 0;
+	     i++) {
 		struct md_rdev *rdev = rcu_dereference(conf->mirrors[i].rdev);
 		if (rdev && !test_bit(Faulty, &rdev->flags)) {
 			struct request_queue *q = bdev_get_queue(rdev->bdev);
@@ -973,13 +1016,24 @@ static void unfreeze_array(struct r10conf *conf)
 	spin_unlock_irq(&conf->resync_lock);
 }
 
+static sector_t choose_data_offset(struct r10bio *r10_bio,
+				   struct md_rdev *rdev)
+{
+	if (!test_bit(MD_RECOVERY_RESHAPE, &rdev->mddev->recovery) ||
+	    test_bit(R10BIO_Previous, &r10_bio->state))
+		return rdev->data_offset;
+	else
+		return rdev->new_data_offset;
+}
+
 static void make_request(struct mddev *mddev, struct bio * bio)
 {
 	struct r10conf *conf = mddev->private;
 	struct r10bio *r10_bio;
 	struct bio *read_bio;
 	int i;
-	int chunk_sects = conf->chunk_mask + 1;
+	sector_t chunk_mask = (conf->geo.chunk_mask & conf->prev.chunk_mask);
+	int chunk_sects = chunk_mask + 1;
 	const int rw = bio_data_dir(bio);
 	const unsigned long do_sync = (bio->bi_rw & REQ_SYNC);
 	const unsigned long do_fua = (bio->bi_rw & REQ_FUA);
@@ -988,6 +1042,7 @@ static void make_request(struct mddev *mddev, struct bio * bio)
 	int plugged;
 	int sectors_handled;
 	int max_sectors;
+	int sectors;
 
 	if (unlikely(bio->bi_rw & REQ_FLUSH)) {
 		md_flush_request(mddev, bio);
@@ -997,9 +1052,10 @@ 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 & conf->chunk_mask) + (bio->bi_size >> 9)
-		      > chunk_sects &&
-		    conf->near_copies < conf->raid_disks)) {
+	if (unlikely((bio->bi_sector & chunk_mask) + (bio->bi_size >> 9)
+		     > 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 ||
@@ -1051,10 +1107,41 @@ static void make_request(struct mddev *mddev, struct bio * bio)
 	 */
 	wait_barrier(conf);
 
+	sectors = bio->bi_size >> 9;
+	while (test_bit(MD_RECOVERY_RESHAPE, &mddev->recovery) &&
+	    bio->bi_sector < conf->reshape_progress &&
+	    bio->bi_sector + sectors > conf->reshape_progress) {
+		/* IO spans the reshape position.  Need to wait for
+		 * reshape to pass
+		 */
+		allow_barrier(conf);
+		wait_event(conf->wait_barrier,
+			   conf->reshape_progress <= bio->bi_sector ||
+			   conf->reshape_progress >= bio->bi_sector + sectors);
+		wait_barrier(conf);
+	}
+	if (test_bit(MD_RECOVERY_RESHAPE, &mddev->recovery) &&
+	    bio_data_dir(bio) == WRITE &&
+	    (mddev->reshape_backwards
+	     ? (bio->bi_sector < conf->reshape_safe &&
+		bio->bi_sector + sectors > conf->reshape_progress)
+	     : (bio->bi_sector + sectors > conf->reshape_safe &&
+		bio->bi_sector < conf->reshape_progress))) {
+		/* Need to update reshape_position in metadata */
+		mddev->reshape_position = conf->reshape_progress;
+		set_bit(MD_CHANGE_DEVS, &mddev->flags);
+		set_bit(MD_CHANGE_PENDING, &mddev->flags);
+		md_wakeup_thread(mddev->thread);
+		wait_event(mddev->sb_wait,
+			   !test_bit(MD_CHANGE_PENDING, &mddev->flags));
+
+		conf->reshape_safe = mddev->reshape_position;
+	}
+
 	r10_bio = mempool_alloc(conf->r10bio_pool, GFP_NOIO);
 
 	r10_bio->master_bio = bio;
-	r10_bio->sectors = bio->bi_size >> 9;
+	r10_bio->sectors = sectors;
 
 	r10_bio->mddev = mddev;
 	r10_bio->sector = bio->bi_sector;
@@ -1093,7 +1180,7 @@ read_again:
 		r10_bio->devs[slot].rdev = rdev;
 
 		read_bio->bi_sector = r10_bio->devs[slot].addr +
-			rdev->data_offset;
+			choose_data_offset(r10_bio, rdev);
 		read_bio->bi_bdev = rdev->bdev;
 		read_bio->bi_end_io = raid10_end_read_request;
 		read_bio->bi_rw = READ | do_sync;
@@ -1297,7 +1384,8 @@ retry_write:
 		r10_bio->devs[i].bio = mbio;
 
 		mbio->bi_sector	= (r10_bio->devs[i].addr+
-				   conf->mirrors[d].rdev->data_offset);
+				   choose_data_offset(r10_bio,
+						      conf->mirrors[d].rdev));
 		mbio->bi_bdev = conf->mirrors[d].rdev->bdev;
 		mbio->bi_end_io	= raid10_end_write_request;
 		mbio->bi_rw = WRITE | do_sync | do_fua;
@@ -1321,8 +1409,10 @@ retry_write:
 		 * so it cannot disappear, so the replacement cannot
 		 * become NULL here
 		 */
-		mbio->bi_sector	= (r10_bio->devs[i].addr+
-				   conf->mirrors[d].replacement->data_offset);
+		mbio->bi_sector	= (r10_bio->devs[i].addr +
+				   choose_data_offset(
+					   r10_bio,
+					   conf->mirrors[d].replacement));
 		mbio->bi_bdev = conf->mirrors[d].replacement->bdev;
 		mbio->bi_end_io	= raid10_end_write_request;
 		mbio->bi_rw = WRITE | do_sync | do_fua;
@@ -1368,19 +1458,19 @@ static void status(struct seq_file *seq, struct mddev *mddev)
 	struct r10conf *conf = mddev->private;
 	int i;
 
-	if (conf->near_copies < conf->raid_disks)
+	if (conf->geo.near_copies < conf->geo.raid_disks)
 		seq_printf(seq, " %dK chunks", mddev->chunk_sectors / 2);
-	if (conf->near_copies > 1)
-		seq_printf(seq, " %d near-copies", conf->near_copies);
-	if (conf->far_copies > 1) {
-		if (conf->far_offset)
-			seq_printf(seq, " %d offset-copies", conf->far_copies);
+	if (conf->geo.near_copies > 1)
+		seq_printf(seq, " %d near-copies", conf->geo.near_copies);
+	if (conf->geo.far_copies > 1) {
+		if (conf->geo.far_offset)
+			seq_printf(seq, " %d offset-copies", conf->geo.far_copies);
 		else
-			seq_printf(seq, " %d far-copies", conf->far_copies);
+			seq_printf(seq, " %d far-copies", conf->geo.far_copies);
 	}
-	seq_printf(seq, " [%d/%d] [", conf->raid_disks,
-					conf->raid_disks - mddev->degraded);
-	for (i = 0; i < conf->raid_disks; i++)
+	seq_printf(seq, " [%d/%d] [", conf->geo.raid_disks,
+					conf->geo.raid_disks - mddev->degraded);
+	for (i = 0; i < conf->geo.raid_disks; i++)
 		seq_printf(seq, "%s",
 			      conf->mirrors[i].rdev &&
 			      test_bit(In_sync, &conf->mirrors[i].rdev->flags) ? "U" : "_");
@@ -1392,7 +1482,7 @@ static void status(struct seq_file *seq, struct mddev *mddev)
  * Don't consider the device numbered 'ignore'
  * as we might be about to remove it.
  */
-static int enough(struct r10conf *conf, int ignore)
+static int _enough(struct r10conf *conf, struct geom *geo, int ignore)
 {
 	int first = 0;
 
@@ -1403,7 +1493,7 @@ static int enough(struct r10conf *conf, int ignore)
 			if (conf->mirrors[first].rdev &&
 			    first != ignore)
 				cnt++;
-			first = (first+1) % conf->raid_disks;
+			first = (first+1) % geo->raid_disks;
 		}
 		if (cnt == 0)
 			return 0;
@@ -1411,6 +1501,12 @@ static int enough(struct r10conf *conf, int ignore)
 	return 1;
 }
 
+static int enough(struct r10conf *conf, int ignore)
+{
+	return _enough(conf, &conf->geo, ignore) &&
+		_enough(conf, &conf->prev, ignore);
+}
+
 static void error(struct mddev *mddev, struct md_rdev *rdev)
 {
 	char b[BDEVNAME_SIZE];
@@ -1445,7 +1541,7 @@ static void error(struct mddev *mddev, struct md_rdev *rdev)
 	       "md/raid10:%s: Disk failure on %s, disabling device.\n"
 	       "md/raid10:%s: Operation continuing on %d devices.\n",
 	       mdname(mddev), bdevname(rdev->bdev, b),
-	       mdname(mddev), conf->raid_disks - mddev->degraded);
+	       mdname(mddev), conf->geo.raid_disks - mddev->degraded);
 }
 
 static void print_conf(struct r10conf *conf)
@@ -1458,10 +1554,10 @@ static void print_conf(struct r10conf *conf)
 		printk(KERN_DEBUG "(!conf)\n");
 		return;
 	}
-	printk(KERN_DEBUG " --- wd:%d rd:%d\n", conf->raid_disks - conf->mddev->degraded,
-		conf->raid_disks);
+	printk(KERN_DEBUG " --- wd:%d rd:%d\n", conf->geo.raid_disks - conf->mddev->degraded,
+		conf->geo.raid_disks);
 
-	for (i = 0; i < conf->raid_disks; i++) {
+	for (i = 0; i < conf->geo.raid_disks; i++) {
 		char b[BDEVNAME_SIZE];
 		tmp = conf->mirrors + i;
 		if (tmp->rdev)
@@ -1493,7 +1589,7 @@ static int raid10_spare_active(struct mddev *mddev)
 	 * Find all non-in_sync disks within the RAID10 configuration
 	 * and mark them in_sync
 	 */
-	for (i = 0; i < conf->raid_disks; i++) {
+	for (i = 0; i < conf->geo.raid_disks; i++) {
 		tmp = conf->mirrors + i;
 		if (tmp->replacement
 		    && tmp->replacement->recovery_offset == MaxSector
@@ -1535,7 +1631,7 @@ static int raid10_add_disk(struct mddev *mddev, struct md_rdev *rdev)
 	int err = -EEXIST;
 	int mirror;
 	int first = 0;
-	int last = conf->raid_disks - 1;
+	int last = conf->geo.raid_disks - 1;
 	struct request_queue *q = bdev_get_queue(rdev->bdev);
 
 	if (mddev->recovery_cp < MaxSector)
@@ -1543,7 +1639,7 @@ static int raid10_add_disk(struct mddev *mddev, struct md_rdev *rdev)
 		 * very different from resync
 		 */
 		return -EBUSY;
-	if (rdev->saved_raid_disk < 0 && !enough(conf, -1))
+	if (rdev->saved_raid_disk < 0 && !_enough(conf, &conf->prev, -1))
 		return -EINVAL;
 
 	if (rdev->raid_disk >= 0)
@@ -1635,6 +1731,7 @@ static int raid10_remove_disk(struct mddev *mddev, struct md_rdev *rdev)
 	if (!test_bit(Faulty, &rdev->flags) &&
 	    mddev->recovery_disabled != p->recovery_disabled &&
 	    (!p->replacement || p->replacement == rdev) &&
+	    number < conf->geo.raid_disks &&
 	    enough(conf, -1)) {
 		err = -EBUSY;
 		goto abort;
@@ -1676,7 +1773,11 @@ static void end_sync_read(struct bio *bio, int error)
 	struct r10conf *conf = r10_bio->mddev->private;
 	int d;
 
-	d = find_bio_disk(conf, r10_bio, bio, NULL, NULL);
+	if (bio == r10_bio->master_bio) {
+		/* this is a reshape read */
+		d = r10_bio->read_slot; /* really the read dev */
+	} else
+		d = find_bio_disk(conf, r10_bio, bio, NULL, NULL);
 
 	if (test_bit(BIO_UPTODATE, &bio->bi_flags))
 		set_bit(R10BIO_Uptodate, &r10_bio->state);
@@ -2218,7 +2319,9 @@ static void fix_read_error(struct r10conf *conf, struct mddev *mddev, struct r10
 				       " (%d sectors at %llu on %s)\n",
 				       mdname(mddev), s,
 				       (unsigned long long)(
-					       sect + rdev->data_offset),
+					       sect +
+					       choose_data_offset(r10_bio,
+								  rdev)),
 				       bdevname(rdev->bdev, b));
 				printk(KERN_NOTICE "md/raid10:%s: %s: failing "
 				       "drive\n",
@@ -2256,7 +2359,8 @@ static void fix_read_error(struct r10conf *conf, struct mddev *mddev, struct r10
 				       " (%d sectors at %llu on %s)\n",
 				       mdname(mddev), s,
 				       (unsigned long long)(
-					       sect + rdev->data_offset),
+					       sect +
+					       choose_data_offset(r10_bio, rdev)),
 				       bdevname(rdev->bdev, b));
 				printk(KERN_NOTICE "md/raid10:%s: %s: failing "
 				       "drive\n",
@@ -2269,7 +2373,8 @@ static void fix_read_error(struct r10conf *conf, struct mddev *mddev, struct r10
 				       " (%d sectors at %llu on %s)\n",
 				       mdname(mddev), s,
 				       (unsigned long long)(
-					       sect + rdev->data_offset),
+					       sect +
+					       choose_data_offset(r10_bio, rdev)),
 				       bdevname(rdev->bdev, b));
 				atomic_add(s, &rdev->corrected_errors);
 			}
@@ -2343,7 +2448,7 @@ static int narrow_write_error(struct r10bio *r10_bio, int i)
 		wbio = bio_clone_mddev(bio, GFP_NOIO, mddev);
 		md_trim_bio(wbio, sector - bio->bi_sector, sectors);
 		wbio->bi_sector = (r10_bio->devs[i].addr+
-				   rdev->data_offset+
+				   choose_data_offset(r10_bio, rdev) +
 				   (sector - r10_bio->sector));
 		wbio->bi_bdev = rdev->bdev;
 		if (submit_bio_wait(WRITE, wbio) == 0)
@@ -2420,7 +2525,7 @@ read_more:
 	r10_bio->devs[slot].bio = bio;
 	r10_bio->devs[slot].rdev = rdev;
 	bio->bi_sector = r10_bio->devs[slot].addr
-		+ rdev->data_offset;
+		+ choose_data_offset(r10_bio, rdev);
 	bio->bi_bdev = rdev->bdev;
 	bio->bi_rw = READ | do_sync;
 	bio->bi_private = r10_bio;
@@ -2480,7 +2585,7 @@ static void handle_write_completed(struct r10conf *conf, struct r10bio *r10_bio)
 				rdev_clear_badblocks(
 					rdev,
 					r10_bio->devs[m].addr,
-					r10_bio->sectors);
+					r10_bio->sectors, 0);
 			} else {
 				if (!rdev_set_badblocks(
 					    rdev,
@@ -2496,7 +2601,7 @@ static void handle_write_completed(struct r10conf *conf, struct r10bio *r10_bio)
 				rdev_clear_badblocks(
 					rdev,
 					r10_bio->devs[m].addr,
-					r10_bio->sectors);
+					r10_bio->sectors, 0);
 			} else {
 				if (!rdev_set_badblocks(
 					    rdev,
@@ -2515,7 +2620,7 @@ static void handle_write_completed(struct r10conf *conf, struct r10bio *r10_bio)
 				rdev_clear_badblocks(
 					rdev,
 					r10_bio->devs[m].addr,
-					r10_bio->sectors);
+					r10_bio->sectors, 0);
 				rdev_dec_pending(rdev, conf->mddev);
 			} else if (bio != NULL &&
 				   !test_bit(BIO_UPTODATE, &bio->bi_flags)) {
@@ -2532,7 +2637,7 @@ static void handle_write_completed(struct r10conf *conf, struct r10bio *r10_bio)
 				rdev_clear_badblocks(
 					rdev,
 					r10_bio->devs[m].addr,
-					r10_bio->sectors);
+					r10_bio->sectors, 0);
 				rdev_dec_pending(rdev, conf->mddev);
 			}
 		}
@@ -2573,6 +2678,8 @@ static void raid10d(struct mddev *mddev)
 		if (test_bit(R10BIO_MadeGood, &r10_bio->state) ||
 		    test_bit(R10BIO_WriteError, &r10_bio->state))
 			handle_write_completed(conf, r10_bio);
+		else if (test_bit(R10BIO_IsReshape, &r10_bio->state))
+			reshape_request_write(mddev, r10_bio);
 		else if (test_bit(R10BIO_IsSync, &r10_bio->state))
 			sync_request_write(mddev, r10_bio);
 		else if (test_bit(R10BIO_IsRecover, &r10_bio->state))
@@ -2603,7 +2710,7 @@ static int init_resync(struct r10conf *conf)
 	buffs = RESYNC_WINDOW / RESYNC_BLOCK_SIZE;
 	BUG_ON(conf->r10buf_pool);
 	conf->have_replacement = 0;
-	for (i = 0; i < conf->raid_disks; i++)
+	for (i = 0; i < conf->geo.raid_disks; i++)
 		if (conf->mirrors[i].replacement)
 			conf->have_replacement = 1;
 	conf->r10buf_pool = mempool_create(buffs, r10buf_pool_alloc, r10buf_pool_free, conf);
@@ -2657,6 +2764,7 @@ static sector_t sync_request(struct mddev *mddev, sector_t sector_nr,
 	sector_t sync_blocks;
 	sector_t sectors_skipped = 0;
 	int chunks_skipped = 0;
+	sector_t chunk_mask = conf->geo.chunk_mask;
 
 	if (!conf->r10buf_pool)
 		if (init_resync(conf))
@@ -2664,7 +2772,8 @@ static sector_t sync_request(struct mddev *mddev, sector_t sector_nr,
 
  skipped:
 	max_sector = mddev->dev_sectors;
-	if (test_bit(MD_RECOVERY_SYNC, &mddev->recovery))
+	if (test_bit(MD_RECOVERY_SYNC, &mddev->recovery) ||
+	    test_bit(MD_RECOVERY_RESHAPE, &mddev->recovery))
 		max_sector = mddev->resync_max_sectors;
 	if (sector_nr >= max_sector) {
 		/* If we aborted, we need to abort the
@@ -2676,11 +2785,16 @@ static sector_t sync_request(struct mddev *mddev, sector_t sector_nr,
 		 * we need to convert that to several
 		 * virtual addresses.
 		 */
+		if (test_bit(MD_RECOVERY_RESHAPE, &mddev->recovery)) {
+			end_reshape(conf);
+			return 0;
+		}
+
 		if (mddev->curr_resync < max_sector) { /* aborted */
 			if (test_bit(MD_RECOVERY_SYNC, &mddev->recovery))
 				bitmap_end_sync(mddev->bitmap, mddev->curr_resync,
 						&sync_blocks, 1);
-			else for (i=0; i<conf->raid_disks; i++) {
+			else for (i = 0; i < conf->geo.raid_disks; i++) {
 				sector_t sect =
 					raid10_find_virt(conf, mddev->curr_resync, i);
 				bitmap_end_sync(mddev->bitmap, sect,
@@ -2694,7 +2808,7 @@ static sector_t sync_request(struct mddev *mddev, sector_t sector_nr,
 				/* Completed a full sync so the replacements
 				 * are now fully recovered.
 				 */
-				for (i = 0; i < conf->raid_disks; i++)
+				for (i = 0; i < conf->geo.raid_disks; i++)
 					if (conf->mirrors[i].replacement)
 						conf->mirrors[i].replacement
 							->recovery_offset
@@ -2707,7 +2821,11 @@ static sector_t sync_request(struct mddev *mddev, sector_t sector_nr,
 		*skipped = 1;
 		return sectors_skipped;
 	}
-	if (chunks_skipped >= conf->raid_disks) {
+
+	if (test_bit(MD_RECOVERY_RESHAPE, &mddev->recovery))
+		return reshape_request(mddev, sector_nr, skipped);
+
+	if (chunks_skipped >= conf->geo.raid_disks) {
 		/* if there has been nothing to do on any drive,
 		 * then there is nothing to do at all..
 		 */
@@ -2721,9 +2839,9 @@ static sector_t sync_request(struct mddev *mddev, sector_t sector_nr,
 	/* make sure whole request will fit in a chunk - if chunks
 	 * are meaningful
 	 */
-	if (conf->near_copies < conf->raid_disks &&
-	    max_sector > (sector_nr | conf->chunk_mask))
-		max_sector = (sector_nr | conf->chunk_mask) + 1;
+	if (conf->geo.near_copies < conf->geo.raid_disks &&
+	    max_sector > (sector_nr | chunk_mask))
+		max_sector = (sector_nr | chunk_mask) + 1;
 	/*
 	 * If there is non-resync activity waiting for us then
 	 * put in a delay to throttle resync.
@@ -2752,7 +2870,7 @@ static sector_t sync_request(struct mddev *mddev, sector_t sector_nr,
 		int j;
 		r10_bio = NULL;
 
-		for (i=0 ; i<conf->raid_disks; i++) {
+		for (i = 0 ; i < conf->geo.raid_disks; i++) {
 			int still_degraded;
 			struct r10bio *rb2;
 			sector_t sect;
@@ -2806,7 +2924,7 @@ static sector_t sync_request(struct mddev *mddev, sector_t sector_nr,
 			/* Need to check if the array will still be
 			 * degraded
 			 */
-			for (j=0; j<conf->raid_disks; j++)
+			for (j = 0; j < conf->geo.raid_disks; j++)
 				if (conf->mirrors[j].rdev == NULL ||
 				    test_bit(Faulty, &conf->mirrors[j].rdev->flags)) {
 					still_degraded = 1;
@@ -2984,9 +3102,9 @@ static sector_t sync_request(struct mddev *mddev, sector_t sector_nr,
 		r10_bio->sector = sector_nr;
 		set_bit(R10BIO_IsSync, &r10_bio->state);
 		raid10_find_phys(conf, r10_bio);
-		r10_bio->sectors = (sector_nr | conf->chunk_mask) - sector_nr +1;
+		r10_bio->sectors = (sector_nr | chunk_mask) - sector_nr + 1;
 
-		for (i=0; i<conf->copies; i++) {
+		for (i = 0; i < conf->copies; i++) {
 			int d = r10_bio->devs[i].devnum;
 			sector_t first_bad, sector;
 			int bad_sectors;
@@ -3152,40 +3270,107 @@ raid10_size(struct mddev *mddev, sector_t sectors, int raid_disks)
 	struct r10conf *conf = mddev->private;
 
 	if (!raid_disks)
-		raid_disks = conf->raid_disks;
+		raid_disks = min(conf->geo.raid_disks,
+				 conf->prev.raid_disks);
 	if (!sectors)
 		sectors = conf->dev_sectors;
 
-	size = sectors >> conf->chunk_shift;
-	sector_div(size, conf->far_copies);
+	size = sectors >> conf->geo.chunk_shift;
+	sector_div(size, conf->geo.far_copies);
 	size = size * raid_disks;
-	sector_div(size, conf->near_copies);
+	sector_div(size, conf->geo.near_copies);
+
+	return size << conf->geo.chunk_shift;
+}
+
+static void calc_sectors(struct r10conf *conf, sector_t size)
+{
+	/* Calculate the number of sectors-per-device that will
+	 * actually be used, and set conf->dev_sectors and
+	 * conf->stride
+	 */
 
-	return size << conf->chunk_shift;
+	size = size >> conf->geo.chunk_shift;
+	sector_div(size, conf->geo.far_copies);
+	size = size * conf->geo.raid_disks;
+	sector_div(size, conf->geo.near_copies);
+	/* 'size' is now the number of chunks in the array */
+	/* calculate "used chunks per device" */
+	size = size * conf->copies;
+
+	/* We need to round up when dividing by raid_disks to
+	 * get the stride size.
+	 */
+	size = DIV_ROUND_UP_SECTOR_T(size, conf->geo.raid_disks);
+
+	conf->dev_sectors = size << conf->geo.chunk_shift;
+
+	if (conf->geo.far_offset)
+		conf->geo.stride = 1 << conf->geo.chunk_shift;
+	else {
+		sector_div(size, conf->geo.far_copies);
+		conf->geo.stride = size << conf->geo.chunk_shift;
+	}
 }
 
+enum geo_type {geo_new, geo_old, geo_start};
+static int setup_geo(struct geom *geo, struct mddev *mddev, enum geo_type new)
+{
+	int nc, fc, fo;
+	int layout, chunk, disks;
+	switch (new) {
+	case geo_old:
+		layout = mddev->layout;
+		chunk = mddev->chunk_sectors;
+		disks = mddev->raid_disks - mddev->delta_disks;
+		break;
+	case geo_new:
+		layout = mddev->new_layout;
+		chunk = mddev->new_chunk_sectors;
+		disks = mddev->raid_disks;
+		break;
+	default: /* avoid 'may be unused' warnings */
+	case geo_start: /* new when starting reshape - raid_disks not
+			 * updated yet. */
+		layout = mddev->new_layout;
+		chunk = mddev->new_chunk_sectors;
+		disks = mddev->raid_disks + mddev->delta_disks;
+		break;
+	}
+	if (layout >> 17)
+		return -1;
+	if (chunk < (PAGE_SIZE >> 9) ||
+	    !is_power_of_2(chunk))
+		return -2;
+	nc = layout & 255;
+	fc = (layout >> 8) & 255;
+	fo = layout & (1<<16);
+	geo->raid_disks = disks;
+	geo->near_copies = nc;
+	geo->far_copies = fc;
+	geo->far_offset = fo;
+	geo->chunk_mask = chunk - 1;
+	geo->chunk_shift = ffz(~chunk);
+	return nc*fc;
+}
 
 static struct r10conf *setup_conf(struct mddev *mddev)
 {
 	struct r10conf *conf = NULL;
-	int nc, fc, fo;
-	sector_t stride, size;
 	int err = -EINVAL;
+	struct geom geo;
+	int copies;
 
-	if (mddev->new_chunk_sectors < (PAGE_SIZE >> 9) ||
-	    !is_power_of_2(mddev->new_chunk_sectors)) {
+	copies = setup_geo(&geo, mddev, geo_new);
+
+	if (copies == -2) {
 		printk(KERN_ERR "md/raid10:%s: chunk size must be "
 		       "at least PAGE_SIZE(%ld) and be a power of 2.\n",
 		       mdname(mddev), PAGE_SIZE);
 		goto out;
 	}
 
-	nc = mddev->new_layout & 255;
-	fc = (mddev->new_layout >> 8) & 255;
-	fo = mddev->new_layout & (1<<16);
-
-	if ((nc*fc) <2 || (nc*fc) > mddev->raid_disks ||
-	    (mddev->new_layout >> 17)) {
+	if (copies < 2 || copies > mddev->raid_disks) {
 		printk(KERN_ERR "md/raid10:%s: unsupported raid10 layout: 0x%8x\n",
 		       mdname(mddev), mddev->new_layout);
 		goto out;
@@ -3196,7 +3381,9 @@ static struct r10conf *setup_conf(struct mddev *mddev)
 	if (!conf)
 		goto out;
 
-	conf->mirrors = kzalloc(sizeof(struct mirror_info)*mddev->raid_disks,
+	/* FIXME calc properly */
+	conf->mirrors = kzalloc(sizeof(struct mirror_info)*(mddev->raid_disks +
+							    max(0,mddev->delta_disks)),
 				GFP_KERNEL);
 	if (!conf->mirrors)
 		goto out;
@@ -3205,43 +3392,29 @@ static struct r10conf *setup_conf(struct mddev *mddev)
 	if (!conf->tmppage)
 		goto out;
 
-
-	conf->raid_disks = mddev->raid_disks;
-	conf->near_copies = nc;
-	conf->far_copies = fc;
-	conf->copies = nc*fc;
-	conf->far_offset = fo;
-	conf->chunk_mask = mddev->new_chunk_sectors - 1;
-	conf->chunk_shift = ffz(~mddev->new_chunk_sectors);
-
+	conf->geo = geo;
+	conf->copies = copies;
 	conf->r10bio_pool = mempool_create(NR_RAID10_BIOS, r10bio_pool_alloc,
 					   r10bio_pool_free, conf);
 	if (!conf->r10bio_pool)
 		goto out;
 
-	size = mddev->dev_sectors >> conf->chunk_shift;
-	sector_div(size, fc);
-	size = size * conf->raid_disks;
-	sector_div(size, nc);
-	/* 'size' is now the number of chunks in the array */
-	/* calculate "used chunks per device" in 'stride' */
-	stride = size * conf->copies;
-
-	/* We need to round up when dividing by raid_disks to
-	 * get the stride size.
-	 */
-	stride += conf->raid_disks - 1;
-	sector_div(stride, conf->raid_disks);
-
-	conf->dev_sectors = stride << conf->chunk_shift;
-
-	if (fo)
-		stride = 1;
-	else
-		sector_div(stride, fc);
-	conf->stride = stride << conf->chunk_shift;
-
-
+	calc_sectors(conf, mddev->dev_sectors);
+	if (mddev->reshape_position == MaxSector) {
+		conf->prev = conf->geo;
+		conf->reshape_progress = MaxSector;
+	} else {
+		if (setup_geo(&conf->prev, mddev, geo_old) != conf->copies) {
+			err = -EINVAL;
+			goto out;
+		}
+		conf->reshape_progress = mddev->reshape_position;
+		if (conf->prev.far_offset)
+			conf->prev.stride = 1 << conf->prev.chunk_shift;
+		else
+			/* far_copies must be 1 */
+			conf->prev.stride = conf->dev_sectors;
+	}
 	spin_lock_init(&conf->device_lock);
 	INIT_LIST_HEAD(&conf->retry_list);
 
@@ -3256,8 +3429,9 @@ static struct r10conf *setup_conf(struct mddev *mddev)
 	return conf;
 
  out:
-	printk(KERN_ERR "md/raid10:%s: couldn't allocate memory.\n",
-	       mdname(mddev));
+	if (err == -ENOMEM)
+		printk(KERN_ERR "md/raid10:%s: couldn't allocate memory.\n",
+		       mdname(mddev));
 	if (conf) {
 		if (conf->r10bio_pool)
 			mempool_destroy(conf->r10bio_pool);
@@ -3275,12 +3449,8 @@ static int run(struct mddev *mddev)
 	struct mirror_info *disk;
 	struct md_rdev *rdev;
 	sector_t size;
-
-	/*
-	 * copy the already verified devices into our private RAID10
-	 * bookkeeping area. [whatever we allocate in run(),
-	 * should be freed in stop()]
-	 */
+	sector_t min_offset_diff = 0;
+	int first = 1;
 
 	if (mddev->private == NULL) {
 		conf = setup_conf(mddev);
@@ -3297,17 +3467,20 @@ static int run(struct mddev *mddev)
 
 	chunk_size = mddev->chunk_sectors << 9;
 	blk_queue_io_min(mddev->queue, chunk_size);
-	if (conf->raid_disks % conf->near_copies)
-		blk_queue_io_opt(mddev->queue, chunk_size * conf->raid_disks);
+	if (conf->geo.raid_disks % conf->geo.near_copies)
+		blk_queue_io_opt(mddev->queue, chunk_size * conf->geo.raid_disks);
 	else
 		blk_queue_io_opt(mddev->queue, chunk_size *
-				 (conf->raid_disks / conf->near_copies));
+				 (conf->geo.raid_disks / conf->geo.near_copies));
 
 	rdev_for_each(rdev, mddev) {
+		long long diff;
 
 		disk_idx = rdev->raid_disk;
-		if (disk_idx >= conf->raid_disks
-		    || disk_idx < 0)
+		if (disk_idx < 0)
+			continue;
+		if (disk_idx >= conf->geo.raid_disks &&
+		    disk_idx >= conf->prev.raid_disks)
 			continue;
 		disk = conf->mirrors + disk_idx;
 
@@ -3320,12 +3493,20 @@ static int run(struct mddev *mddev)
 				goto out_free_conf;
 			disk->rdev = rdev;
 		}
+		diff = (rdev->new_data_offset - rdev->data_offset);
+		if (!mddev->reshape_backwards)
+			diff = -diff;
+		if (diff < 0)
+			diff = 0;
+		if (first || diff < min_offset_diff)
+			min_offset_diff = diff;
 
 		disk_stack_limits(mddev->gendisk, rdev->bdev,
 				  rdev->data_offset << 9);
 
 		disk->head_position = 0;
 	}
+
 	/* need to check that every block has at least one working mirror */
 	if (!enough(conf, -1)) {
 		printk(KERN_ERR "md/raid10:%s: not enough operational mirrors.\n",
@@ -3333,8 +3514,21 @@ static int run(struct mddev *mddev)
 		goto out_free_conf;
 	}
 
+	if (conf->reshape_progress != MaxSector) {
+		/* must ensure that shape change is supported */
+		if (conf->geo.far_copies != 1 &&
+		    conf->geo.far_offset == 0)
+			goto out_free_conf;
+		if (conf->prev.far_copies != 1 &&
+		    conf->geo.far_offset == 0)
+			goto out_free_conf;
+	}
+
 	mddev->degraded = 0;
-	for (i = 0; i < conf->raid_disks; i++) {
+	for (i = 0;
+	     i < conf->geo.raid_disks
+		     || i < conf->prev.raid_disks;
+	     i++) {
 
 		disk = conf->mirrors + i;
 
@@ -3361,8 +3555,8 @@ static int run(struct mddev *mddev)
 		       mdname(mddev));
 	printk(KERN_INFO
 		"md/raid10:%s: active with %d out of %d devices\n",
-		mdname(mddev), conf->raid_disks - mddev->degraded,
-		conf->raid_disks);
+		mdname(mddev), conf->geo.raid_disks - mddev->degraded,
+		conf->geo.raid_disks);
 	/*
 	 * Ok, everything is just fine now
 	 */
@@ -3379,11 +3573,11 @@ static int run(struct mddev *mddev)
 	 * maybe...
 	 */
 	{
-		int stripe = conf->raid_disks *
+		int stripe = conf->geo.raid_disks *
 			((mddev->chunk_sectors << 9) / PAGE_SIZE);
-		stripe /= conf->near_copies;
-		if (mddev->queue->backing_dev_info.ra_pages < 2* stripe)
-			mddev->queue->backing_dev_info.ra_pages = 2* stripe;
+		stripe /= conf->geo.near_copies;
+		if (mddev->queue->backing_dev_info.ra_pages < 2 * stripe)
+			mddev->queue->backing_dev_info.ra_pages = 2 * stripe;
 	}
 
 	blk_queue_merge_bvec(mddev->queue, raid10_mergeable_bvec);
@@ -3391,6 +3585,30 @@ static int run(struct mddev *mddev)
 	if (md_integrity_register(mddev))
 		goto out_free_conf;
 
+	if (conf->reshape_progress != MaxSector) {
+		unsigned long before_length, after_length;
+
+		before_length = ((1 << conf->prev.chunk_shift) *
+				 conf->prev.far_copies);
+		after_length = ((1 << conf->geo.chunk_shift) *
+				conf->geo.far_copies);
+
+		if (max(before_length, after_length) > min_offset_diff) {
+			/* This cannot work */
+			printk("md/raid10: offset difference not enough to continue reshape\n");
+			goto out_free_conf;
+		}
+		conf->offset_diff = min_offset_diff;
+
+		conf->reshape_safe = conf->reshape_progress;
+		clear_bit(MD_RECOVERY_SYNC, &mddev->recovery);
+		clear_bit(MD_RECOVERY_CHECK, &mddev->recovery);
+		set_bit(MD_RECOVERY_RESHAPE, &mddev->recovery);
+		set_bit(MD_RECOVERY_RUNNING, &mddev->recovery);
+		mddev->sync_thread = md_register_thread(md_do_sync, mddev,
+							"reshape");
+	}
+
 	return 0;
 
 out_free_conf:
@@ -3453,14 +3671,23 @@ static int raid10_resize(struct mddev *mddev, sector_t sectors)
 	struct r10conf *conf = mddev->private;
 	sector_t oldsize, size;
 
-	if (conf->far_copies > 1 && !conf->far_offset)
+	if (mddev->reshape_position != MaxSector)
+		return -EBUSY;
+
+	if (conf->geo.far_copies > 1 && !conf->geo.far_offset)
 		return -EINVAL;
 
 	oldsize = raid10_size(mddev, 0, 0);
 	size = raid10_size(mddev, sectors, 0);
-	md_set_array_sectors(mddev, size);
-	if (mddev->array_sectors > size)
+	if (mddev->external_size &&
+	    mddev->array_sectors > size)
 		return -EINVAL;
+	if (mddev->bitmap) {
+		int ret = bitmap_resize(mddev->bitmap, size, 0, 0);
+		if (ret)
+			return ret;
+	}
+	md_set_array_sectors(mddev, size);
 	set_capacity(mddev->gendisk, mddev->array_sectors);
 	revalidate_disk(mddev->gendisk);
 	if (sectors > mddev->dev_sectors &&
@@ -3468,7 +3695,8 @@ static int raid10_resize(struct mddev *mddev, sector_t sectors)
 		mddev->recovery_cp = oldsize;
 		set_bit(MD_RECOVERY_NEEDED, &mddev->recovery);
 	}
-	mddev->dev_sectors = sectors;
+	calc_sectors(conf, sectors);
+	mddev->dev_sectors = conf->dev_sectors;
 	mddev->resync_max_sectors = size;
 	return 0;
 }
@@ -3526,6 +3754,758 @@ static void *raid10_takeover(struct mddev *mddev)
 	return ERR_PTR(-EINVAL);
 }
 
+static int raid10_check_reshape(struct mddev *mddev)
+{
+	/* Called when there is a request to change
+	 * - layout (to ->new_layout)
+	 * - chunk size (to ->new_chunk_sectors)
+	 * - raid_disks (by delta_disks)
+	 * or when trying to restart a reshape that was ongoing.
+	 *
+	 * We need to validate the request and possibly allocate
+	 * space if that might be an issue later.
+	 *
+	 * Currently we reject any reshape of a 'far' mode array,
+	 * allow chunk size to change if new is generally acceptable,
+	 * allow raid_disks to increase, and allow
+	 * a switch between 'near' mode and 'offset' mode.
+	 */
+	struct r10conf *conf = mddev->private;
+	struct geom geo;
+
+	if (conf->geo.far_copies != 1 && !conf->geo.far_offset)
+		return -EINVAL;
+
+	if (setup_geo(&geo, mddev, geo_start) != conf->copies)
+		/* mustn't change number of copies */
+		return -EINVAL;
+	if (geo.far_copies > 1 && !geo.far_offset)
+		/* Cannot switch to 'far' mode */
+		return -EINVAL;
+
+	if (mddev->array_sectors & geo.chunk_mask)
+			/* not factor of array size */
+			return -EINVAL;
+
+	if (!enough(conf, -1))
+		return -EINVAL;
+
+	kfree(conf->mirrors_new);
+	conf->mirrors_new = NULL;
+	if (mddev->delta_disks > 0) {
+		/* allocate new 'mirrors' list */
+		conf->mirrors_new = kzalloc(
+			sizeof(struct mirror_info)
+			*(mddev->raid_disks +
+			  mddev->delta_disks),
+			GFP_KERNEL);
+		if (!conf->mirrors_new)
+			return -ENOMEM;
+	}
+	return 0;
+}
+
+/*
+ * Need to check if array has failed when deciding whether to:
+ *  - start an array
+ *  - remove non-faulty devices
+ *  - add a spare
+ *  - allow a reshape
+ * This determination is simple when no reshape is happening.
+ * However if there is a reshape, we need to carefully check
+ * both the before and after sections.
+ * This is because some failed devices may only affect one
+ * of the two sections, and some non-in_sync devices may
+ * be insync in the section most affected by failed devices.
+ */
+static int calc_degraded(struct r10conf *conf)
+{
+	int degraded, degraded2;
+	int i;
+
+	rcu_read_lock();
+	degraded = 0;
+	/* 'prev' section first */
+	for (i = 0; i < conf->prev.raid_disks; i++) {
+		struct md_rdev *rdev = rcu_dereference(conf->mirrors[i].rdev);
+		if (!rdev || test_bit(Faulty, &rdev->flags))
+			degraded++;
+		else if (!test_bit(In_sync, &rdev->flags))
+			/* When we can reduce the number of devices in
+			 * an array, this might not contribute to
+			 * 'degraded'.  It does now.
+			 */
+			degraded++;
+	}
+	rcu_read_unlock();
+	if (conf->geo.raid_disks == conf->prev.raid_disks)
+		return degraded;
+	rcu_read_lock();
+	degraded2 = 0;
+	for (i = 0; i < conf->geo.raid_disks; i++) {
+		struct md_rdev *rdev = rcu_dereference(conf->mirrors[i].rdev);
+		if (!rdev || test_bit(Faulty, &rdev->flags))
+			degraded2++;
+		else if (!test_bit(In_sync, &rdev->flags)) {
+			/* If reshape is increasing the number of devices,
+			 * this section has already been recovered, so
+			 * it doesn't contribute to degraded.
+			 * else it does.
+			 */
+			if (conf->geo.raid_disks <= conf->prev.raid_disks)
+				degraded2++;
+		}
+	}
+	rcu_read_unlock();
+	if (degraded2 > degraded)
+		return degraded2;
+	return degraded;
+}
+
+static int raid10_start_reshape(struct mddev *mddev)
+{
+	/* A 'reshape' has been requested. This commits
+	 * the various 'new' fields and sets MD_RECOVER_RESHAPE
+	 * This also checks if there are enough spares and adds them
+	 * to the array.
+	 * We currently require enough spares to make the final
+	 * array non-degraded.  We also require that the difference
+	 * between old and new data_offset - on each device - is
+	 * enough that we never risk over-writing.
+	 */
+
+	unsigned long before_length, after_length;
+	sector_t min_offset_diff = 0;
+	int first = 1;
+	struct geom new;
+	struct r10conf *conf = mddev->private;
+	struct md_rdev *rdev;
+	int spares = 0;
+	int ret;
+
+	if (test_bit(MD_RECOVERY_RUNNING, &mddev->recovery))
+		return -EBUSY;
+
+	if (setup_geo(&new, mddev, geo_start) != conf->copies)
+		return -EINVAL;
+
+	before_length = ((1 << conf->prev.chunk_shift) *
+			 conf->prev.far_copies);
+	after_length = ((1 << conf->geo.chunk_shift) *
+			conf->geo.far_copies);
+
+	rdev_for_each(rdev, mddev) {
+		if (!test_bit(In_sync, &rdev->flags)
+		    && !test_bit(Faulty, &rdev->flags))
+			spares++;
+		if (rdev->raid_disk >= 0) {
+			long long diff = (rdev->new_data_offset
+					  - rdev->data_offset);
+			if (!mddev->reshape_backwards)
+				diff = -diff;
+			if (diff < 0)
+				diff = 0;
+			if (first || diff < min_offset_diff)
+				min_offset_diff = diff;
+		}
+	}
+
+	if (max(before_length, after_length) > min_offset_diff)
+		return -EINVAL;
+
+	if (spares < mddev->delta_disks)
+		return -EINVAL;
+
+	conf->offset_diff = min_offset_diff;
+	spin_lock_irq(&conf->device_lock);
+	if (conf->mirrors_new) {
+		memcpy(conf->mirrors_new, conf->mirrors,
+		       sizeof(struct mirror_info)*conf->prev.raid_disks);
+		smp_mb();
+		kfree(conf->mirrors_old); /* FIXME and elsewhere */
+		conf->mirrors_old = conf->mirrors;
+		conf->mirrors = conf->mirrors_new;
+		conf->mirrors_new = NULL;
+	}
+	setup_geo(&conf->geo, mddev, geo_start);
+	smp_mb();
+	if (mddev->reshape_backwards) {
+		sector_t size = raid10_size(mddev, 0, 0);
+		if (size < mddev->array_sectors) {
+			spin_unlock_irq(&conf->device_lock);
+			printk(KERN_ERR "md/raid10:%s: array size must be reduce before number of disks\n",
+			       mdname(mddev));
+			return -EINVAL;
+		}
+		mddev->resync_max_sectors = size;
+		conf->reshape_progress = size;
+	} else
+		conf->reshape_progress = 0;
+	spin_unlock_irq(&conf->device_lock);
+
+	if (mddev->delta_disks && mddev->bitmap) {
+		ret = bitmap_resize(mddev->bitmap,
+				    raid10_size(mddev, 0,
+						conf->geo.raid_disks),
+				    0, 0);
+		if (ret)
+			goto abort;
+	}
+	if (mddev->delta_disks > 0) {
+		rdev_for_each(rdev, mddev)
+			if (rdev->raid_disk < 0 &&
+			    !test_bit(Faulty, &rdev->flags)) {
+				if (raid10_add_disk(mddev, rdev) == 0) {
+					if (rdev->raid_disk >=
+					    conf->prev.raid_disks)
+						set_bit(In_sync, &rdev->flags);
+					else
+						rdev->recovery_offset = 0;
+
+					if (sysfs_link_rdev(mddev, rdev))
+						/* Failure here  is OK */;
+				}
+			} else if (rdev->raid_disk >= conf->prev.raid_disks
+				   && !test_bit(Faulty, &rdev->flags)) {
+				/* This is a spare that was manually added */
+				set_bit(In_sync, &rdev->flags);
+			}
+	}
+	/* When a reshape changes the number of devices,
+	 * ->degraded is measured against the larger of the
+	 * pre and  post numbers.
+	 */
+	spin_lock_irq(&conf->device_lock);
+	mddev->degraded = calc_degraded(conf);
+	spin_unlock_irq(&conf->device_lock);
+	mddev->raid_disks = conf->geo.raid_disks;
+	mddev->reshape_position = conf->reshape_progress;
+	set_bit(MD_CHANGE_DEVS, &mddev->flags);
+
+	clear_bit(MD_RECOVERY_SYNC, &mddev->recovery);
+	clear_bit(MD_RECOVERY_CHECK, &mddev->recovery);
+	set_bit(MD_RECOVERY_RESHAPE, &mddev->recovery);
+	set_bit(MD_RECOVERY_RUNNING, &mddev->recovery);
+
+	mddev->sync_thread = md_register_thread(md_do_sync, mddev,
+						"reshape");
+	if (!mddev->sync_thread) {
+		ret = -EAGAIN;
+		goto abort;
+	}
+	conf->reshape_checkpoint = jiffies;
+	md_wakeup_thread(mddev->sync_thread);
+	md_new_event(mddev);
+	return 0;
+
+abort:
+	mddev->recovery = 0;
+	spin_lock_irq(&conf->device_lock);
+	conf->geo = conf->prev;
+	mddev->raid_disks = conf->geo.raid_disks;
+	rdev_for_each(rdev, mddev)
+		rdev->new_data_offset = rdev->data_offset;
+	smp_wmb();
+	conf->reshape_progress = MaxSector;
+	mddev->reshape_position = MaxSector;
+	spin_unlock_irq(&conf->device_lock);
+	return ret;
+}
+
+/* Calculate the last device-address that could contain
+ * any block from the chunk that includes the array-address 's'
+ * and report the next address.
+ * i.e. the address returned will be chunk-aligned and after
+ * any data that is in the chunk containing 's'.
+ */
+static sector_t last_dev_address(sector_t s, struct geom *geo)
+{
+	s = (s | geo->chunk_mask) + 1;
+	s >>= geo->chunk_shift;
+	s *= geo->near_copies;
+	s = DIV_ROUND_UP_SECTOR_T(s, geo->raid_disks);
+	s *= geo->far_copies;
+	s <<= geo->chunk_shift;
+	return s;
+}
+
+/* Calculate the first device-address that could contain
+ * any block from the chunk that includes the array-address 's'.
+ * This too will be the start of a chunk
+ */
+static sector_t first_dev_address(sector_t s, struct geom *geo)
+{
+	s >>= geo->chunk_shift;
+	s *= geo->near_copies;
+	sector_div(s, geo->raid_disks);
+	s *= geo->far_copies;
+	s <<= geo->chunk_shift;
+	return s;
+}
+
+static sector_t reshape_request(struct mddev *mddev, sector_t sector_nr,
+				int *skipped)
+{
+	/* We simply copy at most one chunk (smallest of old and new)
+	 * at a time, possibly less if that exceeds RESYNC_PAGES,
+	 * or we hit a bad block or something.
+	 * This might mean we pause for normal IO in the middle of
+	 * a chunk, but that is not a problem was mddev->reshape_position
+	 * can record any location.
+	 *
+	 * If we will want to write to a location that isn't
+	 * yet recorded as 'safe' (i.e. in metadata on disk) then
+	 * we need to flush all reshape requests and update the metadata.
+	 *
+	 * When reshaping forwards (e.g. to more devices), we interpret
+	 * 'safe' as the earliest block which might not have been copied
+	 * down yet.  We divide this by previous stripe size and multiply
+	 * by previous stripe length to get lowest device offset that we
+	 * cannot write to yet.
+	 * We interpret 'sector_nr' as an address that we want to write to.
+	 * From this we use last_device_address() to find where we might
+	 * write to, and first_device_address on the  'safe' position.
+	 * If this 'next' write position is after the 'safe' position,
+	 * we must update the metadata to increase the 'safe' position.
+	 *
+	 * When reshaping backwards, we round in the opposite direction
+	 * and perform the reverse test:  next write position must not be
+	 * less than current safe position.
+	 *
+	 * In all this the minimum difference in data offsets
+	 * (conf->offset_diff - always positive) allows a bit of slack,
+	 * so next can be after 'safe', but not by more than offset_disk
+	 *
+	 * We need to prepare all the bios here before we start any IO
+	 * to ensure the size we choose is acceptable to all devices.
+	 * The means one for each copy for write-out and an extra one for
+	 * read-in.
+	 * We store the read-in bio in ->master_bio and the others in
+	 * ->devs[x].bio and ->devs[x].repl_bio.
+	 */
+	struct r10conf *conf = mddev->private;
+	struct r10bio *r10_bio;
+	sector_t next, safe, last;
+	int max_sectors;
+	int nr_sectors;
+	int s;
+	struct md_rdev *rdev;
+	int need_flush = 0;
+	struct bio *blist;
+	struct bio *bio, *read_bio;
+	int sectors_done = 0;
+
+	if (sector_nr == 0) {
+		/* If restarting in the middle, skip the initial sectors */
+		if (mddev->reshape_backwards &&
+		    conf->reshape_progress < raid10_size(mddev, 0, 0)) {
+			sector_nr = (raid10_size(mddev, 0, 0)
+				     - conf->reshape_progress);
+		} else if (!mddev->reshape_backwards &&
+			   conf->reshape_progress > 0)
+			sector_nr = conf->reshape_progress;
+		if (sector_nr) {
+			mddev->curr_resync_completed = sector_nr;
+			sysfs_notify(&mddev->kobj, NULL, "sync_completed");
+			*skipped = 1;
+			return sector_nr;
+		}
+	}
+
+	/* We don't use sector_nr to track where we are up to
+	 * as that doesn't work well for ->reshape_backwards.
+	 * So just use ->reshape_progress.
+	 */
+	if (mddev->reshape_backwards) {
+		/* 'next' is the earliest device address that we might
+		 * write to for this chunk in the new layout
+		 */
+		next = first_dev_address(conf->reshape_progress - 1,
+					 &conf->geo);
+
+		/* 'safe' is the last device address that we might read from
+		 * in the old layout after a restart
+		 */
+		safe = last_dev_address(conf->reshape_safe - 1,
+					&conf->prev);
+
+		if (next + conf->offset_diff < safe)
+			need_flush = 1;
+
+		last = conf->reshape_progress - 1;
+		sector_nr = last & ~(sector_t)(conf->geo.chunk_mask
+					       & conf->prev.chunk_mask);
+		if (sector_nr + RESYNC_BLOCK_SIZE/512 < last)
+			sector_nr = last + 1 - RESYNC_BLOCK_SIZE/512;
+	} else {
+		/* 'next' is after the last device address that we
+		 * might write to for this chunk in the new layout
+		 */
+		next = last_dev_address(conf->reshape_progress, &conf->geo);
+
+		/* 'safe' is the earliest device address that we might
+		 * read from in the old layout after a restart
+		 */
+		safe = first_dev_address(conf->reshape_safe, &conf->prev);
+
+		/* Need to update metadata if 'next' might be beyond 'safe'
+		 * as that would possibly corrupt data
+		 */
+		if (next > safe + conf->offset_diff)
+			need_flush = 1;
+
+		sector_nr = conf->reshape_progress;
+		last  = sector_nr | (conf->geo.chunk_mask
+				     & conf->prev.chunk_mask);
+
+		if (sector_nr + RESYNC_BLOCK_SIZE/512 <= last)
+			last = sector_nr + RESYNC_BLOCK_SIZE/512 - 1;
+	}
+
+	if (need_flush ||
+	    time_after(jiffies, conf->reshape_checkpoint + 10*HZ)) {
+		/* Need to update reshape_position in metadata */
+		wait_barrier(conf);
+		mddev->reshape_position = conf->reshape_progress;
+		if (mddev->reshape_backwards)
+			mddev->curr_resync_completed = raid10_size(mddev, 0, 0)
+				- conf->reshape_progress;
+		else
+			mddev->curr_resync_completed = conf->reshape_progress;
+		conf->reshape_checkpoint = jiffies;
+		set_bit(MD_CHANGE_DEVS, &mddev->flags);
+		md_wakeup_thread(mddev->thread);
+		wait_event(mddev->sb_wait, mddev->flags == 0 ||
+			   kthread_should_stop());
+		conf->reshape_safe = mddev->reshape_position;
+		allow_barrier(conf);
+	}
+
+read_more:
+	/* Now schedule reads for blocks from sector_nr to last */
+	r10_bio = mempool_alloc(conf->r10buf_pool, GFP_NOIO);
+	raise_barrier(conf, sectors_done != 0);
+	atomic_set(&r10_bio->remaining, 0);
+	r10_bio->mddev = mddev;
+	r10_bio->sector = sector_nr;
+	set_bit(R10BIO_IsReshape, &r10_bio->state);
+	r10_bio->sectors = last - sector_nr + 1;
+	rdev = read_balance(conf, r10_bio, &max_sectors);
+	BUG_ON(!test_bit(R10BIO_Previous, &r10_bio->state));
+
+	if (!rdev) {
+		/* Cannot read from here, so need to record bad blocks
+		 * on all the target devices.
+		 */
+		// FIXME
+		set_bit(MD_RECOVERY_INTR, &mddev->recovery);
+		return sectors_done;
+	}
+
+	read_bio = bio_alloc_mddev(GFP_KERNEL, RESYNC_PAGES, mddev);
+
+	read_bio->bi_bdev = rdev->bdev;
+	read_bio->bi_sector = (r10_bio->devs[r10_bio->read_slot].addr
+			       + rdev->data_offset);
+	read_bio->bi_private = r10_bio;
+	read_bio->bi_end_io = end_sync_read;
+	read_bio->bi_rw = READ;
+	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;
+
+	/* Now find the locations in the new layout */
+	__raid10_find_phys(&conf->geo, r10_bio);
+
+	blist = read_bio;
+	read_bio->bi_next = NULL;
+
+	for (s = 0; s < conf->copies*2; s++) {
+		struct bio *b;
+		int d = r10_bio->devs[s/2].devnum;
+		struct md_rdev *rdev2;
+		if (s&1) {
+			rdev2 = conf->mirrors[d].replacement;
+			b = r10_bio->devs[s/2].repl_bio;
+		} else {
+			rdev2 = conf->mirrors[d].rdev;
+			b = r10_bio->devs[s/2].bio;
+		}
+		if (!rdev2 || test_bit(Faulty, &rdev2->flags))
+			continue;
+		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;
+	}
+
+	/* Now add as many pages as possible to all of these bios. */
+
+	nr_sectors = 0;
+	for (s = 0 ; s < max_sectors; s += PAGE_SIZE >> 9) {
+		struct page *page = r10_bio->devs[0].bio->bi_io_vec[s/(PAGE_SIZE>>9)].bv_page;
+		int len = (max_sectors - s) << 9;
+		if (len > PAGE_SIZE)
+			len = PAGE_SIZE;
+		for (bio = blist; bio ; bio = bio->bi_next) {
+			struct bio *bio2;
+			if (bio_add_page(bio, page, len, 0))
+				continue;
+
+			/* Didn't fit, must stop */
+			for (bio2 = blist;
+			     bio2 && bio2 != bio;
+			     bio2 = bio2->bi_next) {
+				/* Remove last page from this bio */
+				bio2->bi_vcnt--;
+				bio2->bi_size -= len;
+				bio2->bi_flags &= ~(1<<BIO_SEG_VALID);
+			}
+			goto bio_full;
+		}
+		sector_nr += len >> 9;
+		nr_sectors += len >> 9;
+	}
+bio_full:
+	r10_bio->sectors = nr_sectors;
+
+	/* Now submit the read */
+	md_sync_acct(read_bio->bi_bdev, r10_bio->sectors);
+	atomic_inc(&r10_bio->remaining);
+	read_bio->bi_next = NULL;
+	generic_make_request(read_bio);
+	sector_nr += nr_sectors;
+	sectors_done += nr_sectors;
+	if (sector_nr <= last)
+		goto read_more;
+
+	/* Now that we have done the whole section we can
+	 * update reshape_progress
+	 */
+	if (mddev->reshape_backwards)
+		conf->reshape_progress -= sectors_done;
+	else
+		conf->reshape_progress += sectors_done;
+
+	return sectors_done;
+}
+
+static void end_reshape_request(struct r10bio *r10_bio);
+static int handle_reshape_read_error(struct mddev *mddev,
+				     struct r10bio *r10_bio);
+static void reshape_request_write(struct mddev *mddev, struct r10bio *r10_bio)
+{
+	/* Reshape read completed.  Hopefully we have a block
+	 * to write out.
+	 * If we got a read error then we do sync 1-page reads from
+	 * elsewhere until we find the data - or give up.
+	 */
+	struct r10conf *conf = mddev->private;
+	int s;
+
+	if (!test_bit(R10BIO_Uptodate, &r10_bio->state))
+		if (handle_reshape_read_error(mddev, r10_bio) < 0) {
+			/* Reshape has been aborted */
+			md_done_sync(mddev, r10_bio->sectors, 0);
+			return;
+		}
+
+	/* We definitely have the data in the pages, schedule the
+	 * writes.
+	 */
+	atomic_set(&r10_bio->remaining, 1);
+	for (s = 0; s < conf->copies*2; s++) {
+		struct bio *b;
+		int d = r10_bio->devs[s/2].devnum;
+		struct md_rdev *rdev;
+		if (s&1) {
+			rdev = conf->mirrors[d].replacement;
+			b = r10_bio->devs[s/2].repl_bio;
+		} else {
+			rdev = conf->mirrors[d].rdev;
+			b = r10_bio->devs[s/2].bio;
+		}
+		if (!rdev || test_bit(Faulty, &rdev->flags))
+			continue;
+		atomic_inc(&rdev->nr_pending);
+		md_sync_acct(b->bi_bdev, r10_bio->sectors);
+		atomic_inc(&r10_bio->remaining);
+		b->bi_next = NULL;
+		generic_make_request(b);
+	}
+	end_reshape_request(r10_bio);
+}
+
+static void end_reshape(struct r10conf *conf)
+{
+	if (test_bit(MD_RECOVERY_INTR, &conf->mddev->recovery))
+		return;
+
+	spin_lock_irq(&conf->device_lock);
+	conf->prev = conf->geo;
+	md_finish_reshape(conf->mddev);
+	smp_wmb();
+	conf->reshape_progress = MaxSector;
+	spin_unlock_irq(&conf->device_lock);
+
+	/* read-ahead size must cover two whole stripes, which is
+	 * 2 * (datadisks) * chunksize where 'n' is the number of raid devices
+	 */
+	if (conf->mddev->queue) {
+		int stripe = conf->geo.raid_disks *
+			((conf->mddev->chunk_sectors << 9) / PAGE_SIZE);
+		stripe /= conf->geo.near_copies;
+		if (conf->mddev->queue->backing_dev_info.ra_pages < 2 * stripe)
+			conf->mddev->queue->backing_dev_info.ra_pages = 2 * stripe;
+	}
+	conf->fullsync = 0;
+}
+
+
+static int handle_reshape_read_error(struct mddev *mddev,
+				     struct r10bio *r10_bio)
+{
+	/* Use sync reads to get the blocks from somewhere else */
+	int sectors = r10_bio->sectors;
+	struct r10bio r10b;
+	struct r10conf *conf = mddev->private;
+	int slot = 0;
+	int idx = 0;
+	struct bio_vec *bvec = r10_bio->master_bio->bi_io_vec;
+
+	r10b.sector = r10_bio->sector;
+	__raid10_find_phys(&conf->prev, &r10b);
+
+	while (sectors) {
+		int s = sectors;
+		int success = 0;
+		int first_slot = slot;
+
+		if (s > (PAGE_SIZE >> 9))
+			s = PAGE_SIZE >> 9;
+
+		while (!success) {
+			int d = r10b.devs[slot].devnum;
+			struct md_rdev *rdev = conf->mirrors[d].rdev;
+			sector_t addr;
+			if (rdev == NULL ||
+			    test_bit(Faulty, &rdev->flags) ||
+			    !test_bit(In_sync, &rdev->flags))
+				goto failed;
+
+			addr = r10b.devs[slot].addr + idx * PAGE_SIZE;
+			success = sync_page_io(rdev,
+					       addr,
+					       s << 9,
+					       bvec[idx].bv_page,
+					       READ, false);
+			if (success)
+				break;
+		failed:
+			slot++;
+			if (slot >= conf->copies)
+				slot = 0;
+			if (slot == first_slot)
+				break;
+		}
+		if (!success) {
+			/* couldn't read this block, must give up */
+			set_bit(MD_RECOVERY_INTR,
+				&mddev->recovery);
+			return -EIO;
+		}
+		sectors -= s;
+		idx++;
+	}
+	return 0;
+}
+
+static void end_reshape_write(struct bio *bio, int error)
+{
+	int uptodate = test_bit(BIO_UPTODATE, &bio->bi_flags);
+	struct r10bio *r10_bio = bio->bi_private;
+	struct mddev *mddev = r10_bio->mddev;
+	struct r10conf *conf = mddev->private;
+	int d;
+	int slot;
+	int repl;
+	struct md_rdev *rdev = NULL;
+
+	d = find_bio_disk(conf, r10_bio, bio, &slot, &repl);
+	if (repl)
+		rdev = conf->mirrors[d].replacement;
+	if (!rdev) {
+		smp_mb();
+		rdev = conf->mirrors[d].rdev;
+	}
+
+	if (!uptodate) {
+		/* FIXME should record badblock */
+		md_error(mddev, rdev);
+	}
+
+	rdev_dec_pending(rdev, mddev);
+	end_reshape_request(r10_bio);
+}
+
+static void end_reshape_request(struct r10bio *r10_bio)
+{
+	if (!atomic_dec_and_test(&r10_bio->remaining))
+		return;
+	md_done_sync(r10_bio->mddev, r10_bio->sectors, 1);
+	bio_put(r10_bio->master_bio);
+	put_buf(r10_bio);
+}
+
+static void raid10_finish_reshape(struct mddev *mddev)
+{
+	struct r10conf *conf = mddev->private;
+
+	if (test_bit(MD_RECOVERY_INTR, &mddev->recovery))
+		return;
+
+	if (mddev->delta_disks > 0) {
+		sector_t size = raid10_size(mddev, 0, 0);
+		md_set_array_sectors(mddev, size);
+		if (mddev->recovery_cp > mddev->resync_max_sectors) {
+			mddev->recovery_cp = mddev->resync_max_sectors;
+			set_bit(MD_RECOVERY_NEEDED, &mddev->recovery);
+		}
+		mddev->resync_max_sectors = size;
+		set_capacity(mddev->gendisk, mddev->array_sectors);
+		revalidate_disk(mddev->gendisk);
+	} else {
+		int d;
+		for (d = conf->geo.raid_disks ;
+		     d < conf->geo.raid_disks - mddev->delta_disks;
+		     d++) {
+			struct md_rdev *rdev = conf->mirrors[d].rdev;
+			if (rdev)
+				clear_bit(In_sync, &rdev->flags);
+			rdev = conf->mirrors[d].replacement;
+			if (rdev)
+				clear_bit(In_sync, &rdev->flags);
+		}
+	}
+	mddev->layout = mddev->new_layout;
+	mddev->chunk_sectors = 1 << conf->geo.chunk_shift;
+	mddev->reshape_position = MaxSector;
+	mddev->delta_disks = 0;
+	mddev->reshape_backwards = 0;
+}
+
 static struct md_personality raid10_personality =
 {
 	.name		= "raid10",
@@ -3544,6 +4524,9 @@ static struct md_personality raid10_personality =
 	.size		= raid10_size,
 	.resize		= raid10_resize,
 	.takeover	= raid10_takeover,
+	.check_reshape	= raid10_check_reshape,
+	.start_reshape	= raid10_start_reshape,
+	.finish_reshape	= raid10_finish_reshape,
 };
 
 static int __init raid_init(void)
diff --git a/drivers/md/raid10.h b/drivers/md/raid10.h
index 7c615613c38..135b1b0a155 100644
--- a/drivers/md/raid10.h
+++ b/drivers/md/raid10.h
@@ -14,32 +14,38 @@ struct mirror_info {
 struct r10conf {
 	struct mddev		*mddev;
 	struct mirror_info	*mirrors;
-	int			raid_disks;
+	struct mirror_info	*mirrors_new, *mirrors_old;
 	spinlock_t		device_lock;
 
 	/* geometry */
-	int			near_copies;  /* number of copies laid out
+	struct geom {
+		int		raid_disks;
+		int		near_copies;  /* number of copies laid out
 					       * raid0 style */
-	int 			far_copies;   /* number of copies laid out
+		int		far_copies;   /* number of copies laid out
 					       * at large strides across drives
 					       */
-	int			far_offset;   /* far_copies are offset by 1
+		int		far_offset;   /* far_copies are offset by 1
 					       * stripe instead of many
 					       */
-	int			copies;	      /* near_copies * far_copies.
-					       * must be <= raid_disks
-					       */
-	sector_t		stride;	      /* distance between far copies.
+		sector_t	stride;	      /* distance between far copies.
 					       * This is size / far_copies unless
 					       * far_offset, in which case it is
 					       * 1 stripe.
 					       */
+		int		chunk_shift; /* shift from chunks to sectors */
+		sector_t	chunk_mask;
+	} prev, geo;
+	int			copies;	      /* near_copies * far_copies.
+					       * must be <= raid_disks
+					       */
 
 	sector_t		dev_sectors;  /* temp copy of
 					       * mddev->dev_sectors */
-
-	int			chunk_shift; /* shift from chunks to sectors */
-	sector_t		chunk_mask;
+	sector_t		reshape_progress;
+	sector_t		reshape_safe;
+	unsigned long		reshape_checkpoint;
+	sector_t		offset_diff;
 
 	struct list_head	retry_list;
 	/* queue pending writes and submit them on unplug */
@@ -136,6 +142,7 @@ enum r10bio_state {
 	R10BIO_Uptodate,
 	R10BIO_IsSync,
 	R10BIO_IsRecover,
+	R10BIO_IsReshape,
 	R10BIO_Degraded,
 /* Set ReadError on bios that experience a read error
  * so that raid10d knows what to do with them.
@@ -146,5 +153,10 @@ enum r10bio_state {
  */
 	R10BIO_MadeGood,
 	R10BIO_WriteError,
+/* During a reshape we might be performing IO on the
+ * 'previous' part of the array, in which case this
+ * flag is set
+ */
+	R10BIO_Previous,
 };
 #endif
diff --git a/drivers/md/raid5.c b/drivers/md/raid5.c
index f351422938e..d26767246d2 100644
--- a/drivers/md/raid5.c
+++ b/drivers/md/raid5.c
@@ -488,6 +488,27 @@ get_active_stripe(struct r5conf *conf, sector_t sector,
 	return sh;
 }
 
+/* Determine if 'data_offset' or 'new_data_offset' should be used
+ * in this stripe_head.
+ */
+static int use_new_offset(struct r5conf *conf, struct stripe_head *sh)
+{
+	sector_t progress = conf->reshape_progress;
+	/* Need a memory barrier to make sure we see the value
+	 * of conf->generation, or ->data_offset that was set before
+	 * reshape_progress was updated.
+	 */
+	smp_rmb();
+	if (progress == MaxSector)
+		return 0;
+	if (sh->generation == conf->generation - 1)
+		return 0;
+	/* We are in a reshape, and this is a new-generation stripe,
+	 * so use new_data_offset.
+	 */
+	return 1;
+}
+
 static void
 raid5_end_read_request(struct bio *bi, int error);
 static void
@@ -518,6 +539,8 @@ static void ops_run_io(struct stripe_head *sh, struct stripe_head_state *s)
 			replace_only = 1;
 		} else
 			continue;
+		if (test_and_clear_bit(R5_SyncIO, &sh->dev[i].flags))
+			rw |= REQ_SYNC;
 
 		bi = &sh->dev[i].req;
 		rbi = &sh->dev[i].rreq; /* For writing to replacement */
@@ -603,7 +626,12 @@ static void ops_run_io(struct stripe_head *sh, struct stripe_head_state *s)
 				__func__, (unsigned long long)sh->sector,
 				bi->bi_rw, i);
 			atomic_inc(&sh->count);
-			bi->bi_sector = sh->sector + rdev->data_offset;
+			if (use_new_offset(conf, sh))
+				bi->bi_sector = (sh->sector
+						 + rdev->new_data_offset);
+			else
+				bi->bi_sector = (sh->sector
+						 + rdev->data_offset);
 			bi->bi_flags = 1 << BIO_UPTODATE;
 			bi->bi_idx = 0;
 			bi->bi_io_vec[0].bv_len = STRIPE_SIZE;
@@ -627,7 +655,12 @@ static void ops_run_io(struct stripe_head *sh, struct stripe_head_state *s)
 				__func__, (unsigned long long)sh->sector,
 				rbi->bi_rw, i);
 			atomic_inc(&sh->count);
-			rbi->bi_sector = sh->sector + rrdev->data_offset;
+			if (use_new_offset(conf, sh))
+				rbi->bi_sector = (sh->sector
+						  + rrdev->new_data_offset);
+			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;
@@ -1114,6 +1147,8 @@ ops_run_biodrain(struct stripe_head *sh, struct dma_async_tx_descriptor *tx)
 				dev->sector + STRIPE_SECTORS) {
 				if (wbi->bi_rw & REQ_FUA)
 					set_bit(R5_WantFUA, &dev->flags);
+				if (wbi->bi_rw & REQ_SYNC)
+					set_bit(R5_SyncIO, &dev->flags);
 				tx = async_copy_data(1, wbi, dev->page,
 					dev->sector, tx);
 				wbi = r5_next_bio(wbi, dev->sector);
@@ -1131,13 +1166,15 @@ static void ops_complete_reconstruct(void *stripe_head_ref)
 	int pd_idx = sh->pd_idx;
 	int qd_idx = sh->qd_idx;
 	int i;
-	bool fua = false;
+	bool fua = false, sync = false;
 
 	pr_debug("%s: stripe %llu\n", __func__,
 		(unsigned long long)sh->sector);
 
-	for (i = disks; i--; )
+	for (i = disks; i--; ) {
 		fua |= test_bit(R5_WantFUA, &sh->dev[i].flags);
+		sync |= test_bit(R5_SyncIO, &sh->dev[i].flags);
+	}
 
 	for (i = disks; i--; ) {
 		struct r5dev *dev = &sh->dev[i];
@@ -1146,6 +1183,8 @@ static void ops_complete_reconstruct(void *stripe_head_ref)
 			set_bit(R5_UPTODATE, &dev->flags);
 			if (fua)
 				set_bit(R5_WantFUA, &dev->flags);
+			if (sync)
+				set_bit(R5_SyncIO, &dev->flags);
 		}
 	}
 
@@ -1648,7 +1687,7 @@ static void raid5_end_read_request(struct bio * bi, int error)
 	int uptodate = test_bit(BIO_UPTODATE, &bi->bi_flags);
 	char b[BDEVNAME_SIZE];
 	struct md_rdev *rdev = NULL;
-
+	sector_t s;
 
 	for (i=0 ; i<disks; i++)
 		if (bi == &sh->dev[i].req)
@@ -1671,6 +1710,10 @@ static void raid5_end_read_request(struct bio * bi, int error)
 	if (!rdev)
 		rdev = conf->disks[i].rdev;
 
+	if (use_new_offset(conf, sh))
+		s = sh->sector + rdev->new_data_offset;
+	else
+		s = sh->sector + rdev->data_offset;
 	if (uptodate) {
 		set_bit(R5_UPTODATE, &sh->dev[i].flags);
 		if (test_bit(R5_ReadError, &sh->dev[i].flags)) {
@@ -1683,8 +1726,7 @@ static void raid5_end_read_request(struct bio * bi, int error)
 				"md/raid:%s: read error corrected"
 				" (%lu sectors at %llu on %s)\n",
 				mdname(conf->mddev), STRIPE_SECTORS,
-				(unsigned long long)(sh->sector
-						     + rdev->data_offset),
+				(unsigned long long)s,
 				bdevname(rdev->bdev, b));
 			atomic_add(STRIPE_SECTORS, &rdev->corrected_errors);
 			clear_bit(R5_ReadError, &sh->dev[i].flags);
@@ -1704,8 +1746,7 @@ static void raid5_end_read_request(struct bio * bi, int error)
 				"md/raid:%s: read error on replacement device "
 				"(sector %llu on %s).\n",
 				mdname(conf->mddev),
-				(unsigned long long)(sh->sector
-						     + rdev->data_offset),
+				(unsigned long long)s,
 				bdn);
 		else if (conf->mddev->degraded >= conf->max_degraded)
 			printk_ratelimited(
@@ -1713,8 +1754,7 @@ static void raid5_end_read_request(struct bio * bi, int error)
 				"md/raid:%s: read error not correctable "
 				"(sector %llu on %s).\n",
 				mdname(conf->mddev),
-				(unsigned long long)(sh->sector
-						     + rdev->data_offset),
+				(unsigned long long)s,
 				bdn);
 		else if (test_bit(R5_ReWrite, &sh->dev[i].flags))
 			/* Oh, no!!! */
@@ -1723,8 +1763,7 @@ static void raid5_end_read_request(struct bio * bi, int error)
 				"md/raid:%s: read error NOT corrected!! "
 				"(sector %llu on %s).\n",
 				mdname(conf->mddev),
-				(unsigned long long)(sh->sector
-						     + rdev->data_offset),
+				(unsigned long long)s,
 				bdn);
 		else if (atomic_read(&rdev->read_errors)
 			 > conf->max_nr_stripes)
@@ -3561,7 +3600,7 @@ finish:
 			if (test_and_clear_bit(R5_MadeGood, &dev->flags)) {
 				rdev = conf->disks[i].rdev;
 				rdev_clear_badblocks(rdev, sh->sector,
-						     STRIPE_SECTORS);
+						     STRIPE_SECTORS, 0);
 				rdev_dec_pending(rdev, conf->mddev);
 			}
 			if (test_and_clear_bit(R5_MadeGoodRepl, &dev->flags)) {
@@ -3570,7 +3609,7 @@ finish:
 					/* rdev have been moved down */
 					rdev = conf->disks[i].rdev;
 				rdev_clear_badblocks(rdev, sh->sector,
-						     STRIPE_SECTORS);
+						     STRIPE_SECTORS, 0);
 				rdev_dec_pending(rdev, conf->mddev);
 			}
 		}
@@ -3842,6 +3881,7 @@ static int chunk_aligned_read(struct mddev *mddev, struct bio * raid_bio)
 		raid_bio->bi_next = (void*)rdev;
 		align_bi->bi_bdev =  rdev->bdev;
 		align_bi->bi_flags &= ~(1 << BIO_SEG_VALID);
+		/* No reshape active, so we can trust rdev->data_offset */
 		align_bi->bi_sector += rdev->data_offset;
 
 		if (!bio_fits_rdev(align_bi) ||
@@ -3953,12 +3993,10 @@ static void make_request(struct mddev *mddev, struct bio * bi)
 	plugged = mddev_check_plugged(mddev);
 	for (;logical_sector < last_sector; logical_sector += STRIPE_SECTORS) {
 		DEFINE_WAIT(w);
-		int disks, data_disks;
 		int previous;
 
 	retry:
 		previous = 0;
-		disks = conf->raid_disks;
 		prepare_to_wait(&conf->wait_for_overlap, &w, TASK_UNINTERRUPTIBLE);
 		if (unlikely(conf->reshape_progress != MaxSector)) {
 			/* spinlock is needed as reshape_progress may be
@@ -3970,13 +4008,12 @@ static void make_request(struct mddev *mddev, struct bio * bi)
 			 * to check again.
 			 */
 			spin_lock_irq(&conf->device_lock);
-			if (mddev->delta_disks < 0
+			if (mddev->reshape_backwards
 			    ? logical_sector < conf->reshape_progress
 			    : logical_sector >= conf->reshape_progress) {
-				disks = conf->previous_raid_disks;
 				previous = 1;
 			} else {
-				if (mddev->delta_disks < 0
+				if (mddev->reshape_backwards
 				    ? logical_sector < conf->reshape_safe
 				    : logical_sector >= conf->reshape_safe) {
 					spin_unlock_irq(&conf->device_lock);
@@ -3986,7 +4023,6 @@ static void make_request(struct mddev *mddev, struct bio * bi)
 			}
 			spin_unlock_irq(&conf->device_lock);
 		}
-		data_disks = disks - conf->max_degraded;
 
 		new_sector = raid5_compute_sector(conf, logical_sector,
 						  previous,
@@ -4009,7 +4045,7 @@ static void make_request(struct mddev *mddev, struct bio * bi)
 				 */
 				int must_retry = 0;
 				spin_lock_irq(&conf->device_lock);
-				if (mddev->delta_disks < 0
+				if (mddev->reshape_backwards
 				    ? logical_sector >= conf->reshape_progress
 				    : logical_sector < conf->reshape_progress)
 					/* mismatch, need to try again */
@@ -4108,11 +4144,11 @@ static sector_t reshape_request(struct mddev *mddev, sector_t sector_nr, int *sk
 
 	if (sector_nr == 0) {
 		/* If restarting in the middle, skip the initial sectors */
-		if (mddev->delta_disks < 0 &&
+		if (mddev->reshape_backwards &&
 		    conf->reshape_progress < raid5_size(mddev, 0, 0)) {
 			sector_nr = raid5_size(mddev, 0, 0)
 				- conf->reshape_progress;
-		} else if (mddev->delta_disks >= 0 &&
+		} else if (!mddev->reshape_backwards &&
 			   conf->reshape_progress > 0)
 			sector_nr = conf->reshape_progress;
 		sector_div(sector_nr, new_data_disks);
@@ -4133,13 +4169,11 @@ static sector_t reshape_request(struct mddev *mddev, sector_t sector_nr, int *sk
 	else
 		reshape_sectors = mddev->chunk_sectors;
 
-	/* we update the metadata when there is more than 3Meg
-	 * in the block range (that is rather arbitrary, should
-	 * probably be time based) or when the data about to be
-	 * copied would over-write the source of the data at
-	 * the front of the range.
-	 * i.e. one new_stripe along from reshape_progress new_maps
-	 * to after where reshape_safe old_maps to
+	/* We update the metadata at least every 10 seconds, or when
+	 * the data about to be copied would over-write the source of
+	 * the data at the front of the range.  i.e. one new_stripe
+	 * along from reshape_progress new_maps to after where
+	 * reshape_safe old_maps to
 	 */
 	writepos = conf->reshape_progress;
 	sector_div(writepos, new_data_disks);
@@ -4147,7 +4181,7 @@ static sector_t reshape_request(struct mddev *mddev, sector_t sector_nr, int *sk
 	sector_div(readpos, data_disks);
 	safepos = conf->reshape_safe;
 	sector_div(safepos, data_disks);
-	if (mddev->delta_disks < 0) {
+	if (mddev->reshape_backwards) {
 		writepos -= min_t(sector_t, reshape_sectors, writepos);
 		readpos += reshape_sectors;
 		safepos += reshape_sectors;
@@ -4157,11 +4191,29 @@ static sector_t reshape_request(struct mddev *mddev, sector_t sector_nr, int *sk
 		safepos -= min_t(sector_t, reshape_sectors, safepos);
 	}
 
+	/* Having calculated the 'writepos' possibly use it
+	 * to set 'stripe_addr' which is where we will write to.
+	 */
+	if (mddev->reshape_backwards) {
+		BUG_ON(conf->reshape_progress == 0);
+		stripe_addr = writepos;
+		BUG_ON((mddev->dev_sectors &
+			~((sector_t)reshape_sectors - 1))
+		       - reshape_sectors - stripe_addr
+		       != sector_nr);
+	} else {
+		BUG_ON(writepos != sector_nr + reshape_sectors);
+		stripe_addr = sector_nr;
+	}
+
 	/* 'writepos' is the most advanced device address we might write.
 	 * 'readpos' is the least advanced device address we might read.
 	 * 'safepos' is the least address recorded in the metadata as having
 	 *     been reshaped.
-	 * If 'readpos' is behind 'writepos', then there is no way that we can
+	 * If there is a min_offset_diff, these are adjusted either by
+	 * increasing the safepos/readpos if diff is negative, or
+	 * increasing writepos if diff is positive.
+	 * If 'readpos' is then behind 'writepos', there is no way that we can
 	 * ensure safety in the face of a crash - that must be done by userspace
 	 * making a backup of the data.  So in that case there is no particular
 	 * rush to update metadata.
@@ -4174,7 +4226,13 @@ static sector_t reshape_request(struct mddev *mddev, sector_t sector_nr, int *sk
 	 * Maybe that number should be configurable, but I'm not sure it is
 	 * worth it.... maybe it could be a multiple of safemode_delay???
 	 */
-	if ((mddev->delta_disks < 0
+	if (conf->min_offset_diff < 0) {
+		safepos += -conf->min_offset_diff;
+		readpos += -conf->min_offset_diff;
+	} else
+		writepos += conf->min_offset_diff;
+
+	if ((mddev->reshape_backwards
 	     ? (safepos > writepos && readpos < writepos)
 	     : (safepos < writepos && readpos > writepos)) ||
 	    time_after(jiffies, conf->reshape_checkpoint + 10*HZ)) {
@@ -4195,17 +4253,6 @@ static sector_t reshape_request(struct mddev *mddev, sector_t sector_nr, int *sk
 		sysfs_notify(&mddev->kobj, NULL, "sync_completed");
 	}
 
-	if (mddev->delta_disks < 0) {
-		BUG_ON(conf->reshape_progress == 0);
-		stripe_addr = writepos;
-		BUG_ON((mddev->dev_sectors &
-			~((sector_t)reshape_sectors - 1))
-		       - reshape_sectors - stripe_addr
-		       != sector_nr);
-	} else {
-		BUG_ON(writepos != sector_nr + reshape_sectors);
-		stripe_addr = sector_nr;
-	}
 	INIT_LIST_HEAD(&stripes);
 	for (i = 0; i < reshape_sectors; i += STRIPE_SECTORS) {
 		int j;
@@ -4239,7 +4286,7 @@ static sector_t reshape_request(struct mddev *mddev, sector_t sector_nr, int *sk
 		list_add(&sh->lru, &stripes);
 	}
 	spin_lock_irq(&conf->device_lock);
-	if (mddev->delta_disks < 0)
+	if (mddev->reshape_backwards)
 		conf->reshape_progress -= reshape_sectors * new_data_disks;
 	else
 		conf->reshape_progress += reshape_sectors * new_data_disks;
@@ -4952,16 +4999,42 @@ static int run(struct mddev *mddev)
 	struct md_rdev *rdev;
 	sector_t reshape_offset = 0;
 	int i;
+	long long min_offset_diff = 0;
+	int first = 1;
 
 	if (mddev->recovery_cp != MaxSector)
 		printk(KERN_NOTICE "md/raid:%s: not clean"
 		       " -- starting background reconstruction\n",
 		       mdname(mddev));
+
+	rdev_for_each(rdev, mddev) {
+		long long diff;
+		if (rdev->raid_disk < 0)
+			continue;
+		diff = (rdev->new_data_offset - rdev->data_offset);
+		if (first) {
+			min_offset_diff = diff;
+			first = 0;
+		} else if (mddev->reshape_backwards &&
+			 diff < min_offset_diff)
+			min_offset_diff = diff;
+		else if (!mddev->reshape_backwards &&
+			 diff > min_offset_diff)
+			min_offset_diff = diff;
+	}
+
 	if (mddev->reshape_position != MaxSector) {
 		/* Check that we can continue the reshape.
-		 * Currently only disks can change, it must
-		 * increase, and we must be past the point where
-		 * a stripe over-writes itself
+		 * Difficulties arise if the stripe we would write to
+		 * next is at or after the stripe we would read from next.
+		 * For a reshape that changes the number of devices, this
+		 * is only possible for a very short time, and mdadm makes
+		 * sure that time appears to have past before assembling
+		 * the array.  So we fail if that time hasn't passed.
+		 * For a reshape that keeps the number of devices the same
+		 * mdadm must be monitoring the reshape can keeping the
+		 * critical areas read-only and backed up.  It will start
+		 * the array in read-only mode, so we check for that.
 		 */
 		sector_t here_new, here_old;
 		int old_disks;
@@ -4993,26 +5066,34 @@ static int run(struct mddev *mddev)
 		/* here_old is the first stripe that we might need to read
 		 * from */
 		if (mddev->delta_disks == 0) {
+			if ((here_new * mddev->new_chunk_sectors !=
+			     here_old * mddev->chunk_sectors)) {
+				printk(KERN_ERR "md/raid:%s: reshape position is"
+				       " confused - aborting\n", mdname(mddev));
+				return -EINVAL;
+			}
 			/* We cannot be sure it is safe to start an in-place
-			 * reshape.  It is only safe if user-space if monitoring
+			 * reshape.  It is only safe if user-space is monitoring
 			 * and taking constant backups.
 			 * mdadm always starts a situation like this in
 			 * readonly mode so it can take control before
 			 * allowing any writes.  So just check for that.
 			 */
-			if ((here_new * mddev->new_chunk_sectors != 
-			     here_old * mddev->chunk_sectors) ||
-			    mddev->ro == 0) {
-				printk(KERN_ERR "md/raid:%s: in-place reshape must be started"
-				       " in read-only mode - aborting\n",
+			if (abs(min_offset_diff) >= mddev->chunk_sectors &&
+			    abs(min_offset_diff) >= mddev->new_chunk_sectors)
+				/* not really in-place - so OK */;
+			else if (mddev->ro == 0) {
+				printk(KERN_ERR "md/raid:%s: in-place reshape "
+				       "must be started in read-only mode "
+				       "- aborting\n",
 				       mdname(mddev));
 				return -EINVAL;
 			}
-		} else if (mddev->delta_disks < 0
-		    ? (here_new * mddev->new_chunk_sectors <=
+		} else if (mddev->reshape_backwards
+		    ? (here_new * mddev->new_chunk_sectors + min_offset_diff <=
 		       here_old * mddev->chunk_sectors)
 		    : (here_new * mddev->new_chunk_sectors >=
-		       here_old * mddev->chunk_sectors)) {
+		       here_old * mddev->chunk_sectors + (-min_offset_diff))) {
 			/* Reading from the same stripe as writing to - bad */
 			printk(KERN_ERR "md/raid:%s: reshape_position too early for "
 			       "auto-recovery - aborting.\n",
@@ -5037,6 +5118,7 @@ static int run(struct mddev *mddev)
 	if (IS_ERR(conf))
 		return PTR_ERR(conf);
 
+	conf->min_offset_diff = min_offset_diff;
 	mddev->thread = conf->thread;
 	conf->thread = NULL;
 	mddev->private = conf;
@@ -5182,9 +5264,12 @@ static int run(struct mddev *mddev)
 		blk_queue_io_opt(mddev->queue, chunk_size *
 				 (conf->raid_disks - conf->max_degraded));
 
-		rdev_for_each(rdev, mddev)
+		rdev_for_each(rdev, mddev) {
 			disk_stack_limits(mddev->gendisk, rdev->bdev,
 					  rdev->data_offset << 9);
+			disk_stack_limits(mddev->gendisk, rdev->bdev,
+					  rdev->new_data_offset << 9);
+		}
 	}
 
 	return 0;
@@ -5418,12 +5503,18 @@ static int raid5_resize(struct mddev *mddev, sector_t sectors)
 	 * any io in the removed space completes, but it hardly seems
 	 * worth it.
 	 */
+	sector_t newsize;
 	sectors &= ~((sector_t)mddev->chunk_sectors - 1);
-	md_set_array_sectors(mddev, raid5_size(mddev, sectors,
-					       mddev->raid_disks));
-	if (mddev->array_sectors >
-	    raid5_size(mddev, sectors, mddev->raid_disks))
+	newsize = raid5_size(mddev, sectors, mddev->raid_disks);
+	if (mddev->external_size &&
+	    mddev->array_sectors > newsize)
 		return -EINVAL;
+	if (mddev->bitmap) {
+		int ret = bitmap_resize(mddev->bitmap, sectors, 0, 0);
+		if (ret)
+			return ret;
+	}
+	md_set_array_sectors(mddev, newsize);
 	set_capacity(mddev->gendisk, mddev->array_sectors);
 	revalidate_disk(mddev->gendisk);
 	if (sectors > mddev->dev_sectors &&
@@ -5468,9 +5559,6 @@ static int check_reshape(struct mddev *mddev)
 	    mddev->new_layout == mddev->layout &&
 	    mddev->new_chunk_sectors == mddev->chunk_sectors)
 		return 0; /* nothing to do */
-	if (mddev->bitmap)
-		/* Cannot grow a bitmap yet */
-		return -EBUSY;
 	if (has_failed(conf))
 		return -EINVAL;
 	if (mddev->delta_disks < 0) {
@@ -5505,10 +5593,14 @@ static int raid5_start_reshape(struct mddev *mddev)
 	if (!check_stripe_cache(mddev))
 		return -ENOSPC;
 
-	rdev_for_each(rdev, mddev)
+	if (has_failed(conf))
+		return -EINVAL;
+
+	rdev_for_each(rdev, mddev) {
 		if (!test_bit(In_sync, &rdev->flags)
 		    && !test_bit(Faulty, &rdev->flags))
 			spares++;
+	}
 
 	if (spares - mddev->degraded < mddev->delta_disks - conf->max_degraded)
 		/* Not enough devices even to make a degraded array
@@ -5535,12 +5627,16 @@ static int raid5_start_reshape(struct mddev *mddev)
 	conf->chunk_sectors = mddev->new_chunk_sectors;
 	conf->prev_algo = conf->algorithm;
 	conf->algorithm = mddev->new_layout;
-	if (mddev->delta_disks < 0)
+	conf->generation++;
+	/* Code that selects data_offset needs to see the generation update
+	 * if reshape_progress has been set - so a memory barrier needed.
+	 */
+	smp_mb();
+	if (mddev->reshape_backwards)
 		conf->reshape_progress = raid5_size(mddev, 0, 0);
 	else
 		conf->reshape_progress = 0;
 	conf->reshape_safe = conf->reshape_progress;
-	conf->generation++;
 	spin_unlock_irq(&conf->device_lock);
 
 	/* Add some new drives, as many as will fit.
@@ -5592,6 +5688,9 @@ static int raid5_start_reshape(struct mddev *mddev)
 		mddev->recovery = 0;
 		spin_lock_irq(&conf->device_lock);
 		mddev->raid_disks = conf->raid_disks = conf->previous_raid_disks;
+		rdev_for_each(rdev, mddev)
+			rdev->new_data_offset = rdev->data_offset;
+		smp_wmb();
 		conf->reshape_progress = MaxSector;
 		mddev->reshape_position = MaxSector;
 		spin_unlock_irq(&conf->device_lock);
@@ -5610,9 +5709,13 @@ static void end_reshape(struct r5conf *conf)
 {
 
 	if (!test_bit(MD_RECOVERY_INTR, &conf->mddev->recovery)) {
+		struct md_rdev *rdev;
 
 		spin_lock_irq(&conf->device_lock);
 		conf->previous_raid_disks = conf->raid_disks;
+		rdev_for_each(rdev, conf->mddev)
+			rdev->data_offset = rdev->new_data_offset;
+		smp_wmb();
 		conf->reshape_progress = MaxSector;
 		spin_unlock_irq(&conf->device_lock);
 		wake_up(&conf->wait_for_overlap);
@@ -5652,17 +5755,18 @@ static void raid5_finish_reshape(struct mddev *mddev)
 			     d < conf->raid_disks - mddev->delta_disks;
 			     d++) {
 				struct md_rdev *rdev = conf->disks[d].rdev;
-				if (rdev &&
-				    raid5_remove_disk(mddev, rdev) == 0) {
-					sysfs_unlink_rdev(mddev, rdev);
-					rdev->raid_disk = -1;
-				}
+				if (rdev)
+					clear_bit(In_sync, &rdev->flags);
+				rdev = conf->disks[d].replacement;
+				if (rdev)
+					clear_bit(In_sync, &rdev->flags);
 			}
 		}
 		mddev->layout = conf->algorithm;
 		mddev->chunk_sectors = conf->chunk_sectors;
 		mddev->reshape_position = MaxSector;
 		mddev->delta_disks = 0;
+		mddev->reshape_backwards = 0;
 	}
 }
 
diff --git a/drivers/md/raid5.h b/drivers/md/raid5.h
index 8d8e13934a4..2164021f3b5 100644
--- a/drivers/md/raid5.h
+++ b/drivers/md/raid5.h
@@ -285,6 +285,7 @@ enum r5dev_flags {
 			 */
 	R5_Wantdrain,	/* dev->towrite needs to be drained */
 	R5_WantFUA,	/* Write should be FUA */
+	R5_SyncIO,	/* The IO is sync */
 	R5_WriteError,	/* got a write error - need to record it */
 	R5_MadeGood,	/* A bad block has been fixed by writing to it */
 	R5_ReadRepl,	/* Will/did read from replacement rather than orig */
@@ -385,6 +386,12 @@ struct r5conf {
 	short			generation; /* increments with every reshape */
 	unsigned long		reshape_checkpoint; /* Time we last updated
 						     * metadata */
+	long long		min_offset_diff; /* minimum difference between
+						  * data_offset and
+						  * new_data_offset across all
+						  * devices.  May be negative,
+						  * but is closest to zero.
+						  */
 
 	struct list_head	handle_list; /* stripes needing handling */
 	struct list_head	hold_list; /* preread ready stripes */
diff --git a/drivers/media/common/saa7146_fops.c b/drivers/media/common/saa7146_fops.c
index 71f8e018e56..7d42c11c868 100644
--- a/drivers/media/common/saa7146_fops.c
+++ b/drivers/media/common/saa7146_fops.c
@@ -198,7 +198,6 @@ static int fops_open(struct file *file)
 	struct saa7146_dev *dev = video_drvdata(file);
 	struct saa7146_fh *fh = NULL;
 	int result = 0;
-
 	enum v4l2_buf_type type;
 
 	DEB_EE("file:%p, dev:%s\n", file, video_device_node_name(vdev));
@@ -227,11 +226,12 @@ static int fops_open(struct file *file)
 		goto out;
 	}
 
-	file->private_data = fh;
+	v4l2_fh_init(&fh->fh, vdev);
+
+	file->private_data = &fh->fh;
 	fh->dev = dev;
-	fh->type = type;
 
-	if( fh->type == V4L2_BUF_TYPE_VBI_CAPTURE) {
+	if (vdev->vfl_type == VFL_TYPE_VBI) {
 		DEB_S("initializing vbi...\n");
 		if (dev->ext_vv_data->capabilities & V4L2_CAP_VBI_CAPTURE)
 			result = saa7146_vbi_uops.open(dev,file);
@@ -252,6 +252,7 @@ static int fops_open(struct file *file)
 	}
 
 	result = 0;
+	v4l2_fh_add(&fh->fh);
 out:
 	if (fh && result != 0) {
 		kfree(fh);
@@ -263,6 +264,7 @@ out:
 
 static int fops_release(struct file *file)
 {
+	struct video_device *vdev = video_devdata(file);
 	struct saa7146_fh  *fh  = file->private_data;
 	struct saa7146_dev *dev = fh->dev;
 
@@ -271,7 +273,7 @@ static int fops_release(struct file *file)
 	if (mutex_lock_interruptible(&saa7146_devices_lock))
 		return -ERESTARTSYS;
 
-	if( fh->type == V4L2_BUF_TYPE_VBI_CAPTURE) {
+	if (vdev->vfl_type == VFL_TYPE_VBI) {
 		if (dev->ext_vv_data->capabilities & V4L2_CAP_VBI_CAPTURE)
 			saa7146_vbi_uops.release(dev,file);
 		if (dev->ext_vv_data->vbi_fops.release)
@@ -280,6 +282,8 @@ static int fops_release(struct file *file)
 		saa7146_video_uops.release(dev,file);
 	}
 
+	v4l2_fh_del(&fh->fh);
+	v4l2_fh_exit(&fh->fh);
 	module_put(dev->ext->module);
 	file->private_data = NULL;
 	kfree(fh);
@@ -291,19 +295,22 @@ static int fops_release(struct file *file)
 
 static int fops_mmap(struct file *file, struct vm_area_struct * vma)
 {
+	struct video_device *vdev = video_devdata(file);
 	struct saa7146_fh *fh = file->private_data;
 	struct videobuf_queue *q;
 
-	switch (fh->type) {
-	case V4L2_BUF_TYPE_VIDEO_CAPTURE: {
+	switch (vdev->vfl_type) {
+	case VFL_TYPE_GRABBER: {
 		DEB_EE("V4L2_BUF_TYPE_VIDEO_CAPTURE: file:%p, vma:%p\n",
 		       file, vma);
 		q = &fh->video_q;
 		break;
 		}
-	case V4L2_BUF_TYPE_VBI_CAPTURE: {
+	case VFL_TYPE_VBI: {
 		DEB_EE("V4L2_BUF_TYPE_VBI_CAPTURE: file:%p, vma:%p\n",
 		       file, vma);
+		if (fh->dev->ext_vv_data->capabilities & V4L2_CAP_SLICED_VBI_OUTPUT)
+			return -ENODEV;
 		q = &fh->vbi_q;
 		break;
 		}
@@ -317,15 +324,19 @@ static int fops_mmap(struct file *file, struct vm_area_struct * vma)
 
 static unsigned int fops_poll(struct file *file, struct poll_table_struct *wait)
 {
+	struct video_device *vdev = video_devdata(file);
 	struct saa7146_fh *fh = file->private_data;
 	struct videobuf_buffer *buf = NULL;
 	struct videobuf_queue *q;
+	unsigned int res = v4l2_ctrl_poll(file, wait);
 
 	DEB_EE("file:%p, poll:%p\n", file, wait);
 
-	if (V4L2_BUF_TYPE_VBI_CAPTURE == fh->type) {
+	if (vdev->vfl_type == VFL_TYPE_VBI) {
+		if (fh->dev->ext_vv_data->capabilities & V4L2_CAP_SLICED_VBI_OUTPUT)
+			return res | POLLOUT | POLLWRNORM;
 		if( 0 == fh->vbi_q.streaming )
-			return videobuf_poll_stream(file, &fh->vbi_q, wait);
+			return res | videobuf_poll_stream(file, &fh->vbi_q, wait);
 		q = &fh->vbi_q;
 	} else {
 		DEB_D("using video queue\n");
@@ -337,31 +348,32 @@ static unsigned int fops_poll(struct file *file, struct poll_table_struct *wait)
 
 	if (!buf) {
 		DEB_D("buf == NULL!\n");
-		return POLLERR;
+		return res | POLLERR;
 	}
 
 	poll_wait(file, &buf->done, wait);
 	if (buf->state == VIDEOBUF_DONE || buf->state == VIDEOBUF_ERROR) {
 		DEB_D("poll succeeded!\n");
-		return POLLIN|POLLRDNORM;
+		return res | POLLIN | POLLRDNORM;
 	}
 
 	DEB_D("nothing to poll for, buf->state:%d\n", buf->state);
-	return 0;
+	return res;
 }
 
 static ssize_t fops_read(struct file *file, char __user *data, size_t count, loff_t *ppos)
 {
+	struct video_device *vdev = video_devdata(file);
 	struct saa7146_fh *fh = file->private_data;
 
-	switch (fh->type) {
-	case V4L2_BUF_TYPE_VIDEO_CAPTURE:
+	switch (vdev->vfl_type) {
+	case VFL_TYPE_GRABBER:
 /*
 		DEB_EE("V4L2_BUF_TYPE_VIDEO_CAPTURE: file:%p, data:%p, count:%lun",
 		       file, data, (unsigned long)count);
 */
 		return saa7146_video_uops.read(file,data,count,ppos);
-	case V4L2_BUF_TYPE_VBI_CAPTURE:
+	case VFL_TYPE_VBI:
 /*
 		DEB_EE("V4L2_BUF_TYPE_VBI_CAPTURE: file:%p, data:%p, count:%lu\n",
 		       file, data, (unsigned long)count);
@@ -377,12 +389,13 @@ static ssize_t fops_read(struct file *file, char __user *data, size_t count, lof
 
 static ssize_t fops_write(struct file *file, const char __user *data, size_t count, loff_t *ppos)
 {
+	struct video_device *vdev = video_devdata(file);
 	struct saa7146_fh *fh = file->private_data;
 
-	switch (fh->type) {
-	case V4L2_BUF_TYPE_VIDEO_CAPTURE:
+	switch (vdev->vfl_type) {
+	case VFL_TYPE_GRABBER:
 		return -EINVAL;
-	case V4L2_BUF_TYPE_VBI_CAPTURE:
+	case VFL_TYPE_VBI:
 		if (fh->dev->ext_vv_data->vbi_fops.write)
 			return fh->dev->ext_vv_data->vbi_fops.write(file, data, count, ppos);
 		else
@@ -429,8 +442,15 @@ static void vv_callback(struct saa7146_dev *dev, unsigned long status)
 	}
 }
 
+static const struct v4l2_ctrl_ops saa7146_ctrl_ops = {
+	.s_ctrl = saa7146_s_ctrl,
+};
+
 int saa7146_vv_init(struct saa7146_dev* dev, struct saa7146_ext_vv *ext_vv)
 {
+	struct v4l2_ctrl_handler *hdl = &dev->ctrl_handler;
+	struct v4l2_pix_format *fmt;
+	struct v4l2_vbi_format *vbi;
 	struct saa7146_vv *vv;
 	int err;
 
@@ -438,12 +458,32 @@ int saa7146_vv_init(struct saa7146_dev* dev, struct saa7146_ext_vv *ext_vv)
 	if (err)
 		return err;
 
+	v4l2_ctrl_handler_init(hdl, 6);
+	v4l2_ctrl_new_std(hdl, &saa7146_ctrl_ops,
+		V4L2_CID_BRIGHTNESS, 0, 255, 1, 128);
+	v4l2_ctrl_new_std(hdl, &saa7146_ctrl_ops,
+		V4L2_CID_CONTRAST, 0, 127, 1, 64);
+	v4l2_ctrl_new_std(hdl, &saa7146_ctrl_ops,
+		V4L2_CID_SATURATION, 0, 127, 1, 64);
+	v4l2_ctrl_new_std(hdl, &saa7146_ctrl_ops,
+		V4L2_CID_VFLIP, 0, 1, 1, 0);
+	v4l2_ctrl_new_std(hdl, &saa7146_ctrl_ops,
+		V4L2_CID_HFLIP, 0, 1, 1, 0);
+	if (hdl->error) {
+		err = hdl->error;
+		v4l2_ctrl_handler_free(hdl);
+		return err;
+	}
+	dev->v4l2_dev.ctrl_handler = hdl;
+
 	vv = kzalloc(sizeof(struct saa7146_vv), GFP_KERNEL);
 	if (vv == NULL) {
 		ERR("out of memory. aborting.\n");
+		v4l2_ctrl_handler_free(hdl);
 		return -ENOMEM;
 	}
-	ext_vv->ops = saa7146_video_ioctl_ops;
+	ext_vv->vid_ops = saa7146_video_ioctl_ops;
+	ext_vv->vbi_ops = saa7146_vbi_ioctl_ops;
 	ext_vv->core_ops = &saa7146_video_ioctl_ops;
 
 	DEB_EE("dev:%p\n", dev);
@@ -463,6 +503,7 @@ int saa7146_vv_init(struct saa7146_dev* dev, struct saa7146_ext_vv *ext_vv)
 	if( NULL == vv->d_clipping.cpu_addr ) {
 		ERR("out of memory. aborting.\n");
 		kfree(vv);
+		v4l2_ctrl_handler_free(hdl);
 		return -1;
 	}
 	memset(vv->d_clipping.cpu_addr, 0x0, SAA7146_CLIPPING_MEM);
@@ -471,6 +512,39 @@ int saa7146_vv_init(struct saa7146_dev* dev, struct saa7146_ext_vv *ext_vv)
 	if (dev->ext_vv_data->capabilities & V4L2_CAP_VBI_CAPTURE)
 		saa7146_vbi_uops.init(dev,vv);
 
+	fmt = &vv->ov_fb.fmt;
+	fmt->width = vv->standard->h_max_out;
+	fmt->height = vv->standard->v_max_out;
+	fmt->pixelformat = V4L2_PIX_FMT_RGB565;
+	fmt->bytesperline = 2 * fmt->width;
+	fmt->sizeimage = fmt->bytesperline * fmt->height;
+	fmt->colorspace = V4L2_COLORSPACE_SRGB;
+
+	fmt = &vv->video_fmt;
+	fmt->width = 384;
+	fmt->height = 288;
+	fmt->pixelformat = V4L2_PIX_FMT_BGR24;
+	fmt->field = V4L2_FIELD_ANY;
+	fmt->colorspace = V4L2_COLORSPACE_SMPTE170M;
+	fmt->bytesperline = 3 * fmt->width;
+	fmt->sizeimage = fmt->bytesperline * fmt->height;
+
+	vbi = &vv->vbi_fmt;
+	vbi->sampling_rate	= 27000000;
+	vbi->offset		= 248; /* todo */
+	vbi->samples_per_line	= 720 * 2;
+	vbi->sample_format	= V4L2_PIX_FMT_GREY;
+
+	/* fixme: this only works for PAL */
+	vbi->start[0] = 5;
+	vbi->count[0] = 16;
+	vbi->start[1] = 312;
+	vbi->count[1] = 16;
+
+	init_timer(&vv->vbi_read_timeout);
+
+	vv->ov_fb.capability = V4L2_FBUF_CAP_LIST_CLIPPING;
+	vv->ov_fb.flags = V4L2_FBUF_FLAG_PRIMARY;
 	dev->vv_data = vv;
 	dev->vv_callback = &vv_callback;
 
@@ -486,6 +560,7 @@ int saa7146_vv_release(struct saa7146_dev* dev)
 
 	v4l2_device_unregister(&dev->v4l2_dev);
 	pci_free_consistent(dev->pci, SAA7146_CLIPPING_MEM, vv->d_clipping.cpu_addr, vv->d_clipping.dma_handle);
+	v4l2_ctrl_handler_free(&dev->ctrl_handler);
 	kfree(vv);
 	dev->vv_data = NULL;
 	dev->vv_callback = NULL;
@@ -509,10 +584,19 @@ int saa7146_register_device(struct video_device **vid, struct saa7146_dev* dev,
 		return -ENOMEM;
 
 	vfd->fops = &video_fops;
-	vfd->ioctl_ops = &dev->ext_vv_data->ops;
+	if (type == VFL_TYPE_GRABBER)
+		vfd->ioctl_ops = &dev->ext_vv_data->vid_ops;
+	else
+		vfd->ioctl_ops = &dev->ext_vv_data->vbi_ops;
 	vfd->release = video_device_release;
+	/* Locking in file operations other than ioctl should be done by
+	   the driver, not the V4L2 core.
+	   This driver needs auditing so that this flag can be removed. */
+	set_bit(V4L2_FL_LOCK_ALL_FOPS, &vfd->flags);
 	vfd->lock = &dev->v4l2_lock;
+	vfd->v4l2_dev = &dev->v4l2_dev;
 	vfd->tvnorms = 0;
+	set_bit(V4L2_FL_USE_FH_PRIO, &vfd->flags);
 	for (i = 0; i < dev->ext_vv_data->num_stds; i++)
 		vfd->tvnorms |= dev->ext_vv_data->stds[i].id;
 	strlcpy(vfd->name, name, sizeof(vfd->name));
diff --git a/drivers/media/common/saa7146_hlp.c b/drivers/media/common/saa7146_hlp.c
index bc1f545c95c..be746d1aee9 100644
--- a/drivers/media/common/saa7146_hlp.c
+++ b/drivers/media/common/saa7146_hlp.c
@@ -343,9 +343,9 @@ static void calculate_clipping_registers_rect(struct saa7146_dev *dev, struct sa
 	struct saa7146_vv *vv = dev->vv_data;
 	__le32 *clipping = vv->d_clipping.cpu_addr;
 
-	int width = fh->ov.win.w.width;
-	int height =  fh->ov.win.w.height;
-	int clipcount = fh->ov.nclips;
+	int width = vv->ov.win.w.width;
+	int height =  vv->ov.win.w.height;
+	int clipcount = vv->ov.nclips;
 
 	u32 line_list[32];
 	u32 pixel_list[32];
@@ -365,10 +365,10 @@ static void calculate_clipping_registers_rect(struct saa7146_dev *dev, struct sa
 	for(i = 0; i < clipcount; i++) {
 		int l = 0, r = 0, t = 0, b = 0;
 
-		x[i] = fh->ov.clips[i].c.left;
-		y[i] = fh->ov.clips[i].c.top;
-		w[i] = fh->ov.clips[i].c.width;
-		h[i] = fh->ov.clips[i].c.height;
+		x[i] = vv->ov.clips[i].c.left;
+		y[i] = vv->ov.clips[i].c.top;
+		w[i] = vv->ov.clips[i].c.width;
+		h[i] = vv->ov.clips[i].c.height;
 
 		if( w[i] < 0) {
 			x[i] += w[i]; w[i] = -w[i];
@@ -485,13 +485,14 @@ static void saa7146_disable_clipping(struct saa7146_dev *dev)
 static void saa7146_set_clipping_rect(struct saa7146_fh *fh)
 {
 	struct saa7146_dev *dev = fh->dev;
-	enum v4l2_field field = fh->ov.win.field;
+	struct saa7146_vv *vv = dev->vv_data;
+	enum v4l2_field field = vv->ov.win.field;
 	struct	saa7146_video_dma vdma2;
 	u32 clip_format;
 	u32 arbtr_ctrl;
 
 	/* check clipcount, disable clipping if clipcount == 0*/
-	if( fh->ov.nclips == 0 ) {
+	if (vv->ov.nclips == 0) {
 		saa7146_disable_clipping(dev);
 		return;
 	}
@@ -651,8 +652,8 @@ int saa7146_enable_overlay(struct saa7146_fh *fh)
 	struct saa7146_dev *dev = fh->dev;
 	struct saa7146_vv *vv = dev->vv_data;
 
-	saa7146_set_window(dev, fh->ov.win.w.width, fh->ov.win.w.height, fh->ov.win.field);
-	saa7146_set_position(dev, fh->ov.win.w.left, fh->ov.win.w.top, fh->ov.win.w.height, fh->ov.win.field, vv->ov_fmt->pixelformat);
+	saa7146_set_window(dev, vv->ov.win.w.width, vv->ov.win.w.height, vv->ov.win.field);
+	saa7146_set_position(dev, vv->ov.win.w.left, vv->ov.win.w.top, vv->ov.win.w.height, vv->ov.win.field, vv->ov_fmt->pixelformat);
 	saa7146_set_output_format(dev, vv->ov_fmt->trans);
 	saa7146_set_clipping_rect(fh);
 
diff --git a/drivers/media/common/saa7146_vbi.c b/drivers/media/common/saa7146_vbi.c
index b2e71834373..1e71e374bbf 100644
--- a/drivers/media/common/saa7146_vbi.c
+++ b/drivers/media/common/saa7146_vbi.c
@@ -211,7 +211,7 @@ static int buffer_activate(struct saa7146_dev *dev,
 	DEB_VBI("dev:%p, buf:%p, next:%p\n", dev, buf, next);
 	saa7146_set_vbi_capture(dev,buf,next);
 
-	mod_timer(&vv->vbi_q.timeout, jiffies+BUFFER_TIMEOUT);
+	mod_timer(&vv->vbi_dmaq.timeout, jiffies+BUFFER_TIMEOUT);
 	return 0;
 }
 
@@ -294,7 +294,7 @@ static void buffer_queue(struct videobuf_queue *q, struct videobuf_buffer *vb)
 	struct saa7146_buf *buf = (struct saa7146_buf *)vb;
 
 	DEB_VBI("vb:%p\n", vb);
-	saa7146_buffer_queue(dev,&vv->vbi_q,buf);
+	saa7146_buffer_queue(dev, &vv->vbi_dmaq, buf);
 }
 
 static void buffer_release(struct videobuf_queue *q, struct videobuf_buffer *vb)
@@ -335,16 +335,15 @@ static void vbi_stop(struct saa7146_fh *fh, struct file *file)
 	/* shut down dma 3 transfers */
 	saa7146_write(dev, MC1, MASK_20);
 
-	if (vv->vbi_q.curr) {
-		saa7146_buffer_finish(dev,&vv->vbi_q,VIDEOBUF_DONE);
-	}
+	if (vv->vbi_dmaq.curr)
+		saa7146_buffer_finish(dev, &vv->vbi_dmaq, VIDEOBUF_DONE);
 
 	videobuf_queue_cancel(&fh->vbi_q);
 
 	vv->vbi_streaming = NULL;
 
-	del_timer(&vv->vbi_q.timeout);
-	del_timer(&fh->vbi_read_timeout);
+	del_timer(&vv->vbi_dmaq.timeout);
+	del_timer(&vv->vbi_read_timeout);
 
 	spin_unlock_irqrestore(&dev->slock, flags);
 }
@@ -364,12 +363,12 @@ static void vbi_init(struct saa7146_dev *dev, struct saa7146_vv *vv)
 {
 	DEB_VBI("dev:%p\n", dev);
 
-	INIT_LIST_HEAD(&vv->vbi_q.queue);
+	INIT_LIST_HEAD(&vv->vbi_dmaq.queue);
 
-	init_timer(&vv->vbi_q.timeout);
-	vv->vbi_q.timeout.function = saa7146_buffer_timeout;
-	vv->vbi_q.timeout.data     = (unsigned long)(&vv->vbi_q);
-	vv->vbi_q.dev              = dev;
+	init_timer(&vv->vbi_dmaq.timeout);
+	vv->vbi_dmaq.timeout.function = saa7146_buffer_timeout;
+	vv->vbi_dmaq.timeout.data     = (unsigned long)(&vv->vbi_dmaq);
+	vv->vbi_dmaq.dev              = dev;
 
 	init_waitqueue_head(&vv->vbi_wq);
 }
@@ -377,6 +376,7 @@ static void vbi_init(struct saa7146_dev *dev, struct saa7146_vv *vv)
 static int vbi_open(struct saa7146_dev *dev, struct file *file)
 {
 	struct saa7146_fh *fh = file->private_data;
+	struct saa7146_vv *vv = fh->dev->vv_data;
 
 	u32 arbtr_ctrl	= saa7146_read(dev, PCI_BT_V1);
 	int ret = 0;
@@ -395,19 +395,6 @@ static int vbi_open(struct saa7146_dev *dev, struct file *file)
 	saa7146_write(dev, PCI_BT_V1, arbtr_ctrl);
 	saa7146_write(dev, MC2, (MASK_04|MASK_20));
 
-	memset(&fh->vbi_fmt,0,sizeof(fh->vbi_fmt));
-
-	fh->vbi_fmt.sampling_rate	= 27000000;
-	fh->vbi_fmt.offset		= 248; /* todo */
-	fh->vbi_fmt.samples_per_line	= vbi_pixel_to_capture;
-	fh->vbi_fmt.sample_format	= V4L2_PIX_FMT_GREY;
-
-	/* fixme: this only works for PAL */
-	fh->vbi_fmt.start[0] = 5;
-	fh->vbi_fmt.count[0] = 16;
-	fh->vbi_fmt.start[1] = 312;
-	fh->vbi_fmt.count[1] = 16;
-
 	videobuf_queue_sg_init(&fh->vbi_q, &vbi_qops,
 			    &dev->pci->dev, &dev->slock,
 			    V4L2_BUF_TYPE_VBI_CAPTURE,
@@ -415,9 +402,8 @@ static int vbi_open(struct saa7146_dev *dev, struct file *file)
 			    sizeof(struct saa7146_buf),
 			    file, &dev->v4l2_lock);
 
-	init_timer(&fh->vbi_read_timeout);
-	fh->vbi_read_timeout.function = vbi_read_timeout;
-	fh->vbi_read_timeout.data = (unsigned long)file;
+	vv->vbi_read_timeout.function = vbi_read_timeout;
+	vv->vbi_read_timeout.data = (unsigned long)file;
 
 	/* initialize the brs */
 	if ( 0 != (SAA7146_USE_PORT_B_FOR_VBI & dev->ext_vv_data->flags)) {
@@ -453,16 +439,16 @@ static void vbi_irq_done(struct saa7146_dev *dev, unsigned long status)
 	struct saa7146_vv *vv = dev->vv_data;
 	spin_lock(&dev->slock);
 
-	if (vv->vbi_q.curr) {
-		DEB_VBI("dev:%p, curr:%p\n", dev, vv->vbi_q.curr);
+	if (vv->vbi_dmaq.curr) {
+		DEB_VBI("dev:%p, curr:%p\n", dev, vv->vbi_dmaq.curr);
 		/* this must be += 2, one count for each field */
 		vv->vbi_fieldcount+=2;
-		vv->vbi_q.curr->vb.field_count = vv->vbi_fieldcount;
-		saa7146_buffer_finish(dev,&vv->vbi_q,VIDEOBUF_DONE);
+		vv->vbi_dmaq.curr->vb.field_count = vv->vbi_fieldcount;
+		saa7146_buffer_finish(dev, &vv->vbi_dmaq, VIDEOBUF_DONE);
 	} else {
 		DEB_VBI("dev:%p\n", dev);
 	}
-	saa7146_buffer_next(dev,&vv->vbi_q,1);
+	saa7146_buffer_next(dev, &vv->vbi_dmaq, 1);
 
 	spin_unlock(&dev->slock);
 }
@@ -488,7 +474,7 @@ static ssize_t vbi_read(struct file *file, char __user *data, size_t count, loff
 		return -EBUSY;
 	}
 
-	mod_timer(&fh->vbi_read_timeout, jiffies+BUFFER_TIMEOUT);
+	mod_timer(&vv->vbi_read_timeout, jiffies+BUFFER_TIMEOUT);
 	ret = videobuf_read_stream(&fh->vbi_q, data, count, ppos, 1,
 				   file->f_flags & O_NONBLOCK);
 /*
diff --git a/drivers/media/common/saa7146_video.c b/drivers/media/common/saa7146_video.c
index ce30533fd97..6d14785d474 100644
--- a/drivers/media/common/saa7146_video.c
+++ b/drivers/media/common/saa7146_video.c
@@ -2,6 +2,8 @@
 
 #include <media/saa7146_vv.h>
 #include <media/v4l2-chip-ident.h>
+#include <media/v4l2-event.h>
+#include <media/v4l2-ctrls.h>
 #include <linux/module.h>
 
 static int max_memory = 32;
@@ -112,8 +114,8 @@ int saa7146_start_preview(struct saa7146_fh *fh)
 
 	DEB_EE("dev:%p, fh:%p\n", dev, fh);
 
-	/* check if we have overlay informations */
-	if( NULL == fh->ov.fh ) {
+	/* check if we have overlay information */
+	if (vv->ov.fh == NULL) {
 		DEB_D("no overlay data available. try S_FMT first.\n");
 		return -EAGAIN;
 	}
@@ -139,19 +141,18 @@ int saa7146_start_preview(struct saa7146_fh *fh)
 		return -EBUSY;
 	}
 
-	fmt.fmt.win = fh->ov.win;
+	fmt.fmt.win = vv->ov.win;
 	err = vidioc_try_fmt_vid_overlay(NULL, fh, &fmt);
 	if (0 != err) {
 		saa7146_res_free(vv->video_fh, RESOURCE_DMA1_HPS|RESOURCE_DMA2_CLP);
 		return -EBUSY;
 	}
-	fh->ov.win = fmt.fmt.win;
-	vv->ov_data = &fh->ov;
+	vv->ov.win = fmt.fmt.win;
 
 	DEB_D("%dx%d+%d+%d %s field=%s\n",
-	      fh->ov.win.w.width, fh->ov.win.w.height,
-	      fh->ov.win.w.left, fh->ov.win.w.top,
-	      vv->ov_fmt->name, v4l2_field_names[fh->ov.win.field]);
+	      vv->ov.win.w.width, vv->ov.win.w.height,
+	      vv->ov.win.w.left, vv->ov.win.w.top,
+	      vv->ov_fmt->name, v4l2_field_names[vv->ov.win.field]);
 
 	if (0 != (ret = saa7146_enable_overlay(fh))) {
 		DEB_D("enabling overlay failed: %d\n", ret);
@@ -202,65 +203,6 @@ int saa7146_stop_preview(struct saa7146_fh *fh)
 EXPORT_SYMBOL_GPL(saa7146_stop_preview);
 
 /********************************************************************************/
-/* device controls */
-
-static struct v4l2_queryctrl controls[] = {
-	{
-		.id		= V4L2_CID_BRIGHTNESS,
-		.name		= "Brightness",
-		.minimum	= 0,
-		.maximum	= 255,
-		.step		= 1,
-		.default_value	= 128,
-		.type		= V4L2_CTRL_TYPE_INTEGER,
-		.flags 		= V4L2_CTRL_FLAG_SLIDER,
-	},{
-		.id		= V4L2_CID_CONTRAST,
-		.name		= "Contrast",
-		.minimum	= 0,
-		.maximum	= 127,
-		.step		= 1,
-		.default_value	= 64,
-		.type		= V4L2_CTRL_TYPE_INTEGER,
-		.flags 		= V4L2_CTRL_FLAG_SLIDER,
-	},{
-		.id		= V4L2_CID_SATURATION,
-		.name		= "Saturation",
-		.minimum	= 0,
-		.maximum	= 127,
-		.step		= 1,
-		.default_value	= 64,
-		.type		= V4L2_CTRL_TYPE_INTEGER,
-		.flags 		= V4L2_CTRL_FLAG_SLIDER,
-	},{
-		.id		= V4L2_CID_VFLIP,
-		.name		= "Vertical Flip",
-		.minimum	= 0,
-		.maximum	= 1,
-		.type		= V4L2_CTRL_TYPE_BOOLEAN,
-	},{
-		.id		= V4L2_CID_HFLIP,
-		.name		= "Horizontal Flip",
-		.minimum	= 0,
-		.maximum	= 1,
-		.type		= V4L2_CTRL_TYPE_BOOLEAN,
-	},
-};
-static int NUM_CONTROLS = sizeof(controls)/sizeof(struct v4l2_queryctrl);
-
-#define V4L2_CID_PRIVATE_LASTP1      (V4L2_CID_PRIVATE_BASE + 0)
-
-static struct v4l2_queryctrl* ctrl_by_id(int id)
-{
-	int i;
-
-	for (i = 0; i < NUM_CONTROLS; i++)
-		if (controls[i].id == id)
-			return controls+i;
-	return NULL;
-}
-
-/********************************************************************************/
 /* common pagetable functions */
 
 static int saa7146_pgtable_build(struct saa7146_dev *dev, struct saa7146_buf *buf)
@@ -413,7 +355,7 @@ static int video_begin(struct saa7146_fh *fh)
 		}
 	}
 
-	fmt = saa7146_format_by_fourcc(dev,fh->video_fmt.pixelformat);
+	fmt = saa7146_format_by_fourcc(dev, vv->video_fmt.pixelformat);
 	/* we need to have a valid format set here */
 	BUG_ON(NULL == fmt);
 
@@ -465,7 +407,7 @@ static int video_end(struct saa7146_fh *fh, struct file *file)
 		return -EBUSY;
 	}
 
-	fmt = saa7146_format_by_fourcc(dev,fh->video_fmt.pixelformat);
+	fmt = saa7146_format_by_fourcc(dev, vv->video_fmt.pixelformat);
 	/* we need to have a valid format set here */
 	BUG_ON(NULL == fmt);
 
@@ -504,18 +446,25 @@ static int video_end(struct saa7146_fh *fh, struct file *file)
 
 static int vidioc_querycap(struct file *file, void *fh, struct v4l2_capability *cap)
 {
+	struct video_device *vdev = video_devdata(file);
 	struct saa7146_dev *dev = ((struct saa7146_fh *)fh)->dev;
 
 	strcpy((char *)cap->driver, "saa7146 v4l2");
 	strlcpy((char *)cap->card, dev->ext->name, sizeof(cap->card));
 	sprintf((char *)cap->bus_info, "PCI:%s", pci_name(dev->pci));
-	cap->version = SAA7146_VERSION_CODE;
-	cap->capabilities =
+	cap->device_caps =
 		V4L2_CAP_VIDEO_CAPTURE |
 		V4L2_CAP_VIDEO_OVERLAY |
 		V4L2_CAP_READWRITE |
 		V4L2_CAP_STREAMING;
-	cap->capabilities |= dev->ext_vv_data->capabilities;
+	cap->device_caps |= dev->ext_vv_data->capabilities;
+	cap->capabilities = cap->device_caps | V4L2_CAP_DEVICE_CAPS;
+	if (vdev->vfl_type == VFL_TYPE_GRABBER)
+		cap->device_caps &=
+			~(V4L2_CAP_VBI_CAPTURE | V4L2_CAP_SLICED_VBI_OUTPUT);
+	else
+		cap->device_caps &=
+			~(V4L2_CAP_VIDEO_CAPTURE | V4L2_CAP_VIDEO_OVERLAY | V4L2_CAP_AUDIO);
 	return 0;
 }
 
@@ -526,6 +475,7 @@ static int vidioc_g_fbuf(struct file *file, void *fh, struct v4l2_framebuffer *f
 
 	*fb = vv->ov_fb;
 	fb->capability = V4L2_FBUF_CAP_LIST_CLIPPING;
+	fb->flags = V4L2_FBUF_FLAG_PRIMARY;
 	return 0;
 }
 
@@ -579,135 +529,58 @@ static int vidioc_enum_fmt_vid_cap(struct file *file, void *fh, struct v4l2_fmtd
 	return 0;
 }
 
-static int vidioc_queryctrl(struct file *file, void *fh, struct v4l2_queryctrl *c)
+int saa7146_s_ctrl(struct v4l2_ctrl *ctrl)
 {
-	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;
-
-	ctrl = ctrl_by_id(c->id);
-	if (ctrl == NULL)
-		return -EINVAL;
-
-	DEB_EE("VIDIOC_QUERYCTRL: id:%d\n", c->id);
-	*c = *ctrl;
-	return 0;
-}
-
-static int vidioc_g_ctrl(struct file *file, void *fh, struct v4l2_control *c)
-{
-	struct saa7146_dev *dev = ((struct saa7146_fh *)fh)->dev;
+	struct saa7146_dev *dev = container_of(ctrl->handler,
+				struct saa7146_dev, ctrl_handler);
 	struct saa7146_vv *vv = dev->vv_data;
-	const struct v4l2_queryctrl *ctrl;
-	u32 value = 0;
+	u32 val;
 
-	ctrl = ctrl_by_id(c->id);
-	if (NULL == ctrl)
-		return -EINVAL;
-	switch (c->id) {
+	switch (ctrl->id) {
 	case V4L2_CID_BRIGHTNESS:
-		value = saa7146_read(dev, BCS_CTRL);
-		c->value = 0xff & (value >> 24);
-		DEB_D("V4L2_CID_BRIGHTNESS: %d\n", c->value);
-		break;
-	case V4L2_CID_CONTRAST:
-		value = saa7146_read(dev, BCS_CTRL);
-		c->value = 0x7f & (value >> 16);
-		DEB_D("V4L2_CID_CONTRAST: %d\n", c->value);
-		break;
-	case V4L2_CID_SATURATION:
-		value = saa7146_read(dev, BCS_CTRL);
-		c->value = 0x7f & (value >> 0);
-		DEB_D("V4L2_CID_SATURATION: %d\n", c->value);
-		break;
-	case V4L2_CID_VFLIP:
-		c->value = vv->vflip;
-		DEB_D("V4L2_CID_VFLIP: %d\n", c->value);
-		break;
-	case V4L2_CID_HFLIP:
-		c->value = vv->hflip;
-		DEB_D("V4L2_CID_HFLIP: %d\n", c->value);
-		break;
-	default:
-		return -EINVAL;
-	}
-	return 0;
-}
-
-static int vidioc_s_ctrl(struct file *file, void *fh, struct v4l2_control *c)
-{
-	struct saa7146_dev *dev = ((struct saa7146_fh *)fh)->dev;
-	struct saa7146_vv *vv = dev->vv_data;
-	const struct v4l2_queryctrl *ctrl;
-
-	ctrl = ctrl_by_id(c->id);
-	if (NULL == ctrl) {
-		DEB_D("unknown control %d\n", c->id);
-		return -EINVAL;
-	}
-
-	switch (ctrl->type) {
-	case V4L2_CTRL_TYPE_BOOLEAN:
-	case V4L2_CTRL_TYPE_MENU:
-	case V4L2_CTRL_TYPE_INTEGER:
-		if (c->value < ctrl->minimum)
-			c->value = ctrl->minimum;
-		if (c->value > ctrl->maximum)
-			c->value = ctrl->maximum;
-		break;
-	default:
-		/* nothing */;
-	}
-
-	switch (c->id) {
-	case V4L2_CID_BRIGHTNESS: {
-		u32 value = saa7146_read(dev, BCS_CTRL);
-		value &= 0x00ffffff;
-		value |= (c->value << 24);
-		saa7146_write(dev, BCS_CTRL, value);
+		val = saa7146_read(dev, BCS_CTRL);
+		val &= 0x00ffffff;
+		val |= (ctrl->val << 24);
+		saa7146_write(dev, BCS_CTRL, val);
 		saa7146_write(dev, MC2, MASK_22 | MASK_06);
 		break;
-	}
-	case V4L2_CID_CONTRAST: {
-		u32 value = saa7146_read(dev, BCS_CTRL);
-		value &= 0xff00ffff;
-		value |= (c->value << 16);
-		saa7146_write(dev, BCS_CTRL, value);
+
+	case V4L2_CID_CONTRAST:
+		val = saa7146_read(dev, BCS_CTRL);
+		val &= 0xff00ffff;
+		val |= (ctrl->val << 16);
+		saa7146_write(dev, BCS_CTRL, val);
 		saa7146_write(dev, MC2, MASK_22 | MASK_06);
 		break;
-	}
-	case V4L2_CID_SATURATION: {
-		u32 value = saa7146_read(dev, BCS_CTRL);
-		value &= 0xffffff00;
-		value |= (c->value << 0);
-		saa7146_write(dev, BCS_CTRL, value);
+
+	case V4L2_CID_SATURATION:
+		val = saa7146_read(dev, BCS_CTRL);
+		val &= 0xffffff00;
+		val |= (ctrl->val << 0);
+		saa7146_write(dev, BCS_CTRL, val);
 		saa7146_write(dev, MC2, MASK_22 | MASK_06);
 		break;
-	}
+
 	case V4L2_CID_HFLIP:
 		/* fixme: we can support changing VFLIP and HFLIP here... */
-		if (IS_CAPTURE_ACTIVE(fh) != 0) {
-			DEB_D("V4L2_CID_HFLIP while active capture\n");
+		if ((vv->video_status & STATUS_CAPTURE))
 			return -EBUSY;
-		}
-		vv->hflip = c->value;
+		vv->hflip = ctrl->val;
 		break;
+
 	case V4L2_CID_VFLIP:
-		if (IS_CAPTURE_ACTIVE(fh) != 0) {
-			DEB_D("V4L2_CID_VFLIP while active capture\n");
+		if ((vv->video_status & STATUS_CAPTURE))
 			return -EBUSY;
-		}
-		vv->vflip = c->value;
+		vv->vflip = ctrl->val;
 		break;
+
 	default:
 		return -EINVAL;
 	}
 
-	if (IS_OVERLAY_ACTIVE(fh) != 0) {
+	if ((vv->video_status & STATUS_OVERLAY) != 0) { /* CHECK: && (vv->video_fh == fh)) */
+		struct saa7146_fh *fh = vv->video_fh;
+
 		saa7146_stop_preview(fh);
 		saa7146_start_preview(fh);
 	}
@@ -720,6 +593,8 @@ static int vidioc_g_parm(struct file *file, void *fh,
 	struct saa7146_dev *dev = ((struct saa7146_fh *)fh)->dev;
 	struct saa7146_vv *vv = dev->vv_data;
 
+	if (parm->type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
+		return -EINVAL;
 	parm->parm.capture.readbuffers = 1;
 	v4l2_video_std_frame_period(vv->standard->id,
 				    &parm->parm.capture.timeperframe);
@@ -728,19 +603,28 @@ static int vidioc_g_parm(struct file *file, void *fh,
 
 static int vidioc_g_fmt_vid_cap(struct file *file, void *fh, struct v4l2_format *f)
 {
-	f->fmt.pix = ((struct saa7146_fh *)fh)->video_fmt;
+	struct saa7146_dev *dev = ((struct saa7146_fh *)fh)->dev;
+	struct saa7146_vv *vv = dev->vv_data;
+
+	f->fmt.pix = vv->video_fmt;
 	return 0;
 }
 
 static int vidioc_g_fmt_vid_overlay(struct file *file, void *fh, struct v4l2_format *f)
 {
-	f->fmt.win = ((struct saa7146_fh *)fh)->ov.win;
+	struct saa7146_dev *dev = ((struct saa7146_fh *)fh)->dev;
+	struct saa7146_vv *vv = dev->vv_data;
+
+	f->fmt.win = vv->ov.win;
 	return 0;
 }
 
 static int vidioc_g_fmt_vbi_cap(struct file *file, void *fh, struct v4l2_format *f)
 {
-	f->fmt.vbi = ((struct saa7146_fh *)fh)->vbi_fmt;
+	struct saa7146_dev *dev = ((struct saa7146_fh *)fh)->dev;
+	struct saa7146_vv *vv = dev->vv_data;
+
+	f->fmt.vbi = vv->vbi_fmt;
 	return 0;
 }
 
@@ -787,6 +671,7 @@ static int vidioc_try_fmt_vid_cap(struct file *file, void *fh, struct v4l2_forma
 	}
 
 	f->fmt.pix.field = field;
+	f->fmt.pix.colorspace = V4L2_COLORSPACE_SMPTE170M;
 	if (f->fmt.pix.width > maxw)
 		f->fmt.pix.width = maxw;
 	if (f->fmt.pix.height > maxh)
@@ -883,9 +768,9 @@ static int vidioc_s_fmt_vid_cap(struct file *file, void *__fh, struct v4l2_forma
 	err = vidioc_try_fmt_vid_cap(file, fh, f);
 	if (0 != err)
 		return err;
-	fh->video_fmt = f->fmt.pix;
+	vv->video_fmt = f->fmt.pix;
 	DEB_EE("set to pixelformat '%4.4s'\n",
-	       (char *)&fh->video_fmt.pixelformat);
+	       (char *)&vv->video_fmt.pixelformat);
 	return 0;
 }
 
@@ -900,17 +785,17 @@ static int vidioc_s_fmt_vid_overlay(struct file *file, void *__fh, struct v4l2_f
 	err = vidioc_try_fmt_vid_overlay(file, fh, f);
 	if (0 != err)
 		return err;
-	fh->ov.win    = f->fmt.win;
-	fh->ov.nclips = f->fmt.win.clipcount;
-	if (fh->ov.nclips > 16)
-		fh->ov.nclips = 16;
-	if (copy_from_user(fh->ov.clips, f->fmt.win.clips,
-				sizeof(struct v4l2_clip) * fh->ov.nclips)) {
+	vv->ov.win    = f->fmt.win;
+	vv->ov.nclips = f->fmt.win.clipcount;
+	if (vv->ov.nclips > 16)
+		vv->ov.nclips = 16;
+	if (copy_from_user(vv->ov.clips, f->fmt.win.clips,
+				sizeof(struct v4l2_clip) * vv->ov.nclips)) {
 		return -EFAULT;
 	}
 
-	/* fh->ov.fh is used to indicate that we have valid overlay informations, too */
-	fh->ov.fh = fh;
+	/* vv->ov.fh is used to indicate that we have valid overlay informations, too */
+	vv->ov.fh = fh;
 
 	/* check if our current overlay is active */
 	if (IS_OVERLAY_ACTIVE(fh) != 0) {
@@ -1111,10 +996,14 @@ static int vidioc_g_chip_ident(struct file *file, void *__fh,
 
 	chip->ident = V4L2_IDENT_NONE;
 	chip->revision = 0;
-	if (chip->match.type == V4L2_CHIP_MATCH_HOST && !chip->match.addr) {
-		chip->ident = V4L2_IDENT_SAA7146;
+	if (chip->match.type == V4L2_CHIP_MATCH_HOST) {
+		if (v4l2_chip_match_host(&chip->match))
+			chip->ident = V4L2_IDENT_SAA7146;
 		return 0;
 	}
+	if (chip->match.type != V4L2_CHIP_MATCH_I2C_DRIVER &&
+	    chip->match.type != V4L2_CHIP_MATCH_I2C_ADDR)
+		return -EINVAL;
 	return v4l2_device_call_until_err(&dev->v4l2_dev, 0,
 			core, g_chip_ident, chip);
 }
@@ -1129,7 +1018,6 @@ const struct v4l2_ioctl_ops saa7146_video_ioctl_ops = {
 	.vidioc_g_fmt_vid_overlay    = vidioc_g_fmt_vid_overlay,
 	.vidioc_try_fmt_vid_overlay  = vidioc_try_fmt_vid_overlay,
 	.vidioc_s_fmt_vid_overlay    = vidioc_s_fmt_vid_overlay,
-	.vidioc_g_fmt_vbi_cap        = vidioc_g_fmt_vbi_cap,
 	.vidioc_g_chip_ident         = vidioc_g_chip_ident,
 
 	.vidioc_overlay 	     = vidioc_overlay,
@@ -1141,12 +1029,29 @@ const struct v4l2_ioctl_ops saa7146_video_ioctl_ops = {
 	.vidioc_dqbuf                = vidioc_dqbuf,
 	.vidioc_g_std                = vidioc_g_std,
 	.vidioc_s_std                = vidioc_s_std,
-	.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_parm 		     = vidioc_g_parm,
+	.vidioc_subscribe_event      = v4l2_ctrl_subscribe_event,
+	.vidioc_unsubscribe_event    = v4l2_event_unsubscribe,
+};
+
+const struct v4l2_ioctl_ops saa7146_vbi_ioctl_ops = {
+	.vidioc_querycap             = vidioc_querycap,
+	.vidioc_g_fmt_vbi_cap        = vidioc_g_fmt_vbi_cap,
+	.vidioc_g_chip_ident         = vidioc_g_chip_ident,
+
+	.vidioc_reqbufs              = vidioc_reqbufs,
+	.vidioc_querybuf             = vidioc_querybuf,
+	.vidioc_qbuf                 = vidioc_qbuf,
+	.vidioc_dqbuf                = vidioc_dqbuf,
+	.vidioc_g_std                = vidioc_g_std,
+	.vidioc_s_std                = vidioc_s_std,
+	.vidioc_streamon             = vidioc_streamon,
+	.vidioc_streamoff            = vidioc_streamoff,
+	.vidioc_g_parm		     = vidioc_g_parm,
+	.vidioc_subscribe_event      = v4l2_ctrl_subscribe_event,
+	.vidioc_unsubscribe_event    = v4l2_event_unsubscribe,
 };
 
 /*********************************************************************************/
@@ -1161,7 +1066,7 @@ static int buffer_activate (struct saa7146_dev *dev,
 	buf->vb.state = VIDEOBUF_ACTIVE;
 	saa7146_set_capture(dev,buf,next);
 
-	mod_timer(&vv->video_q.timeout, jiffies+BUFFER_TIMEOUT);
+	mod_timer(&vv->video_dmaq.timeout, jiffies+BUFFER_TIMEOUT);
 	return 0;
 }
 
@@ -1185,44 +1090,44 @@ static int buffer_prepare(struct videobuf_queue *q,
 	DEB_CAP("vbuf:%p\n", vb);
 
 	/* sanity checks */
-	if (fh->video_fmt.width  < 48 ||
-	    fh->video_fmt.height < 32 ||
-	    fh->video_fmt.width  > vv->standard->h_max_out ||
-	    fh->video_fmt.height > vv->standard->v_max_out) {
+	if (vv->video_fmt.width  < 48 ||
+	    vv->video_fmt.height < 32 ||
+	    vv->video_fmt.width  > vv->standard->h_max_out ||
+	    vv->video_fmt.height > vv->standard->v_max_out) {
 		DEB_D("w (%d) / h (%d) out of bounds\n",
-		      fh->video_fmt.width, fh->video_fmt.height);
+		      vv->video_fmt.width, vv->video_fmt.height);
 		return -EINVAL;
 	}
 
-	size = fh->video_fmt.sizeimage;
+	size = vv->video_fmt.sizeimage;
 	if (0 != buf->vb.baddr && buf->vb.bsize < size) {
 		DEB_D("size mismatch\n");
 		return -EINVAL;
 	}
 
 	DEB_CAP("buffer_prepare [size=%dx%d,bytes=%d,fields=%s]\n",
-		fh->video_fmt.width, fh->video_fmt.height,
-		size, v4l2_field_names[fh->video_fmt.field]);
-	if (buf->vb.width  != fh->video_fmt.width  ||
-	    buf->vb.bytesperline != fh->video_fmt.bytesperline ||
-	    buf->vb.height != fh->video_fmt.height ||
+		vv->video_fmt.width, vv->video_fmt.height,
+		size, v4l2_field_names[vv->video_fmt.field]);
+	if (buf->vb.width  != vv->video_fmt.width  ||
+	    buf->vb.bytesperline != vv->video_fmt.bytesperline ||
+	    buf->vb.height != vv->video_fmt.height ||
 	    buf->vb.size   != size ||
 	    buf->vb.field  != field      ||
-	    buf->vb.field  != fh->video_fmt.field  ||
-	    buf->fmt       != &fh->video_fmt) {
+	    buf->vb.field  != vv->video_fmt.field  ||
+	    buf->fmt       != &vv->video_fmt) {
 		saa7146_dma_free(dev,q,buf);
 	}
 
 	if (VIDEOBUF_NEEDS_INIT == buf->vb.state) {
 		struct saa7146_format *sfmt;
 
-		buf->vb.bytesperline  = fh->video_fmt.bytesperline;
-		buf->vb.width  = fh->video_fmt.width;
-		buf->vb.height = fh->video_fmt.height;
+		buf->vb.bytesperline  = vv->video_fmt.bytesperline;
+		buf->vb.width  = vv->video_fmt.width;
+		buf->vb.height = vv->video_fmt.height;
 		buf->vb.size   = size;
 		buf->vb.field  = field;
-		buf->fmt       = &fh->video_fmt;
-		buf->vb.field  = fh->video_fmt.field;
+		buf->fmt       = &vv->video_fmt;
+		buf->vb.field  = vv->video_fmt.field;
 
 		sfmt = saa7146_format_by_fourcc(dev,buf->fmt->pixelformat);
 
@@ -1258,11 +1163,12 @@ static int buffer_setup(struct videobuf_queue *q, unsigned int *count, unsigned
 {
 	struct file *file = q->priv_data;
 	struct saa7146_fh *fh = file->private_data;
+	struct saa7146_vv *vv = fh->dev->vv_data;
 
 	if (0 == *count || *count > MAX_SAA7146_CAPTURE_BUFFERS)
 		*count = MAX_SAA7146_CAPTURE_BUFFERS;
 
-	*size = fh->video_fmt.sizeimage;
+	*size = vv->video_fmt.sizeimage;
 
 	/* check if we exceed the "max_memory" parameter */
 	if( (*count * *size) > (max_memory*1048576) ) {
@@ -1283,7 +1189,7 @@ static void buffer_queue(struct videobuf_queue *q, struct videobuf_buffer *vb)
 	struct saa7146_buf *buf = (struct saa7146_buf *)vb;
 
 	DEB_CAP("vbuf:%p\n", vb);
-	saa7146_buffer_queue(fh->dev,&vv->video_q,buf);
+	saa7146_buffer_queue(fh->dev, &vv->video_dmaq, buf);
 }
 
 static void buffer_release(struct videobuf_queue *q, struct videobuf_buffer *vb)
@@ -1312,12 +1218,12 @@ static struct videobuf_queue_ops video_qops = {
 
 static void video_init(struct saa7146_dev *dev, struct saa7146_vv *vv)
 {
-	INIT_LIST_HEAD(&vv->video_q.queue);
+	INIT_LIST_HEAD(&vv->video_dmaq.queue);
 
-	init_timer(&vv->video_q.timeout);
-	vv->video_q.timeout.function = saa7146_buffer_timeout;
-	vv->video_q.timeout.data     = (unsigned long)(&vv->video_q);
-	vv->video_q.dev              = dev;
+	init_timer(&vv->video_dmaq.timeout);
+	vv->video_dmaq.timeout.function = saa7146_buffer_timeout;
+	vv->video_dmaq.timeout.data     = (unsigned long)(&vv->video_dmaq);
+	vv->video_dmaq.dev              = dev;
 
 	/* set some default values */
 	vv->standard = &dev->ext_vv_data->stds[0];
@@ -1331,15 +1237,6 @@ static void video_init(struct saa7146_dev *dev, struct saa7146_vv *vv)
 static int video_open(struct saa7146_dev *dev, struct file *file)
 {
 	struct saa7146_fh *fh = file->private_data;
-	struct saa7146_format *sfmt;
-
-	fh->video_fmt.width = 384;
-	fh->video_fmt.height = 288;
-	fh->video_fmt.pixelformat = V4L2_PIX_FMT_BGR24;
-	fh->video_fmt.bytesperline = 0;
-	fh->video_fmt.field = V4L2_FIELD_ANY;
-	sfmt = saa7146_format_by_fourcc(dev,fh->video_fmt.pixelformat);
-	fh->video_fmt.sizeimage = (fh->video_fmt.width * fh->video_fmt.height * sfmt->depth)/8;
 
 	videobuf_queue_sg_init(&fh->video_q, &video_qops,
 			    &dev->pci->dev, &dev->slock,
@@ -1371,7 +1268,7 @@ static void video_close(struct saa7146_dev *dev, struct file *file)
 static void video_irq_done(struct saa7146_dev *dev, unsigned long st)
 {
 	struct saa7146_vv *vv = dev->vv_data;
-	struct saa7146_dmaqueue *q = &vv->video_q;
+	struct saa7146_dmaqueue *q = &vv->video_dmaq;
 
 	spin_lock(&dev->slock);
 	DEB_CAP("called\n");
diff --git a/drivers/media/common/tuners/Kconfig b/drivers/media/common/tuners/Kconfig
index 4a6d5cef396..bbf4945149a 100644
--- a/drivers/media/common/tuners/Kconfig
+++ b/drivers/media/common/tuners/Kconfig
@@ -204,6 +204,27 @@ config MEDIA_TUNER_TDA18218
 	help
 	  NXP TDA18218 silicon tuner driver.
 
+config MEDIA_TUNER_FC0011
+	tristate "Fitipower FC0011 silicon tuner"
+	depends on VIDEO_MEDIA && I2C
+	default m if MEDIA_TUNER_CUSTOMISE
+	help
+	  Fitipower FC0011 silicon tuner driver.
+
+config MEDIA_TUNER_FC0012
+	tristate "Fitipower FC0012 silicon tuner"
+	depends on VIDEO_MEDIA && I2C
+	default m if MEDIA_TUNER_CUSTOMISE
+	help
+	  Fitipower FC0012 silicon tuner driver.
+
+config MEDIA_TUNER_FC0013
+	tristate "Fitipower FC0013 silicon tuner"
+	depends on VIDEO_MEDIA && I2C
+	default m if MEDIA_TUNER_CUSTOMISE
+	help
+	  Fitipower FC0013 silicon tuner driver.
+
 config MEDIA_TUNER_TDA18212
 	tristate "NXP TDA18212 silicon tuner"
 	depends on VIDEO_MEDIA && I2C
@@ -211,4 +232,10 @@ config MEDIA_TUNER_TDA18212
 	help
 	  NXP TDA18212 silicon tuner driver.
 
+config MEDIA_TUNER_TUA9001
+	tristate "Infineon TUA 9001 silicon tuner"
+	depends on VIDEO_MEDIA && I2C
+	default m if MEDIA_TUNER_CUSTOMISE
+	help
+	  Infineon TUA 9001 silicon tuner driver.
 endmenu
diff --git a/drivers/media/common/tuners/Makefile b/drivers/media/common/tuners/Makefile
index f80407eb899..891b80e6080 100644
--- a/drivers/media/common/tuners/Makefile
+++ b/drivers/media/common/tuners/Makefile
@@ -28,6 +28,10 @@ obj-$(CONFIG_MEDIA_TUNER_MC44S803) += mc44s803.o
 obj-$(CONFIG_MEDIA_TUNER_MAX2165) += max2165.o
 obj-$(CONFIG_MEDIA_TUNER_TDA18218) += tda18218.o
 obj-$(CONFIG_MEDIA_TUNER_TDA18212) += tda18212.o
+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
 
 ccflags-y += -I$(srctree)/drivers/media/dvb/dvb-core
 ccflags-y += -I$(srctree)/drivers/media/dvb/frontends
diff --git a/drivers/media/common/tuners/fc0011.c b/drivers/media/common/tuners/fc0011.c
new file mode 100644
index 00000000000..e4882546c28
--- /dev/null
+++ b/drivers/media/common/tuners/fc0011.c
@@ -0,0 +1,524 @@
+/*
+ * Fitipower FC0011 tuner driver
+ *
+ * Copyright (C) 2012 Michael Buesch <m@bues.ch>
+ *
+ * Derived from FC0012 tuner driver:
+ * Copyright (C) 2012 Hans-Frieder Vogt <hfvogt@gmx.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., 675 Mass Ave, Cambridge, MA 02139, USA.
+ */
+
+#include "fc0011.h"
+
+
+/* Tuner registers */
+enum {
+	FC11_REG_0,
+	FC11_REG_FA,		/* FA */
+	FC11_REG_FP,		/* FP */
+	FC11_REG_XINHI,		/* XIN high 8 bit */
+	FC11_REG_XINLO,		/* XIN low 8 bit */
+	FC11_REG_VCO,		/* VCO */
+	FC11_REG_VCOSEL,	/* VCO select */
+	FC11_REG_7,		/* Unknown tuner reg 7 */
+	FC11_REG_8,		/* Unknown tuner reg 8 */
+	FC11_REG_9,
+	FC11_REG_10,		/* Unknown tuner reg 10 */
+	FC11_REG_11,		/* Unknown tuner reg 11 */
+	FC11_REG_12,
+	FC11_REG_RCCAL,		/* RC calibrate */
+	FC11_REG_VCOCAL,	/* VCO calibrate */
+	FC11_REG_15,
+	FC11_REG_16,		/* Unknown tuner reg 16 */
+	FC11_REG_17,
+
+	FC11_NR_REGS,		/* Number of registers */
+};
+
+enum FC11_REG_VCOSEL_bits {
+	FC11_VCOSEL_2		= 0x08, /* VCO select 2 */
+	FC11_VCOSEL_1		= 0x10, /* VCO select 1 */
+	FC11_VCOSEL_CLKOUT	= 0x20, /* Fix clock out */
+	FC11_VCOSEL_BW7M	= 0x40, /* 7MHz bw */
+	FC11_VCOSEL_BW6M	= 0x80, /* 6MHz bw */
+};
+
+enum FC11_REG_RCCAL_bits {
+	FC11_RCCAL_FORCE	= 0x10, /* force */
+};
+
+enum FC11_REG_VCOCAL_bits {
+	FC11_VCOCAL_RUN		= 0,	/* VCO calibration run */
+	FC11_VCOCAL_VALUEMASK	= 0x3F,	/* VCO calibration value mask */
+	FC11_VCOCAL_OK		= 0x40,	/* VCO calibration Ok */
+	FC11_VCOCAL_RESET	= 0x80, /* VCO calibration reset */
+};
+
+
+struct fc0011_priv {
+	struct i2c_adapter *i2c;
+	u8 addr;
+
+	u32 frequency;
+	u32 bandwidth;
+};
+
+
+static int fc0011_writereg(struct fc0011_priv *priv, u8 reg, u8 val)
+{
+	u8 buf[2] = { reg, val };
+	struct i2c_msg msg = { .addr = priv->addr,
+		.flags = 0, .buf = buf, .len = 2 };
+
+	if (i2c_transfer(priv->i2c, &msg, 1) != 1) {
+		dev_err(&priv->i2c->dev,
+			"I2C write reg failed, reg: %02x, val: %02x\n",
+			reg, val);
+		return -EIO;
+	}
+
+	return 0;
+}
+
+static int fc0011_readreg(struct fc0011_priv *priv, u8 reg, u8 *val)
+{
+	u8 dummy;
+	struct i2c_msg msg[2] = {
+		{ .addr = priv->addr,
+		  .flags = 0, .buf = &reg, .len = 1 },
+		{ .addr = priv->addr,
+		  .flags = I2C_M_RD, .buf = val ? : &dummy, .len = 1 },
+	};
+
+	if (i2c_transfer(priv->i2c, msg, 2) != 2) {
+		dev_err(&priv->i2c->dev,
+			"I2C read failed, reg: %02x\n", reg);
+		return -EIO;
+	}
+
+	return 0;
+}
+
+static int fc0011_release(struct dvb_frontend *fe)
+{
+	kfree(fe->tuner_priv);
+	fe->tuner_priv = NULL;
+
+	return 0;
+}
+
+static int fc0011_init(struct dvb_frontend *fe)
+{
+	struct fc0011_priv *priv = fe->tuner_priv;
+	int err;
+
+	if (WARN_ON(!fe->callback))
+		return -EINVAL;
+
+	err = fe->callback(priv->i2c, DVB_FRONTEND_COMPONENT_TUNER,
+			   FC0011_FE_CALLBACK_POWER, priv->addr);
+	if (err) {
+		dev_err(&priv->i2c->dev, "Power-on callback failed\n");
+		return err;
+	}
+	err = fe->callback(priv->i2c, DVB_FRONTEND_COMPONENT_TUNER,
+			   FC0011_FE_CALLBACK_RESET, priv->addr);
+	if (err) {
+		dev_err(&priv->i2c->dev, "Reset callback failed\n");
+		return err;
+	}
+
+	return 0;
+}
+
+/* Initiate VCO calibration */
+static int fc0011_vcocal_trigger(struct fc0011_priv *priv)
+{
+	int err;
+
+	err = fc0011_writereg(priv, FC11_REG_VCOCAL, FC11_VCOCAL_RESET);
+	if (err)
+		return err;
+	err = fc0011_writereg(priv, FC11_REG_VCOCAL, FC11_VCOCAL_RUN);
+	if (err)
+		return err;
+
+	return 0;
+}
+
+/* Read VCO calibration value */
+static int fc0011_vcocal_read(struct fc0011_priv *priv, u8 *value)
+{
+	int err;
+
+	err = fc0011_writereg(priv, FC11_REG_VCOCAL, FC11_VCOCAL_RUN);
+	if (err)
+		return err;
+	usleep_range(10000, 20000);
+	err = fc0011_readreg(priv, FC11_REG_VCOCAL, value);
+	if (err)
+		return err;
+
+	return 0;
+}
+
+static int fc0011_set_params(struct dvb_frontend *fe)
+{
+	struct dtv_frontend_properties *p = &fe->dtv_property_cache;
+	struct fc0011_priv *priv = fe->tuner_priv;
+	int err;
+	unsigned int i, vco_retries;
+	u32 freq = p->frequency / 1000;
+	u32 bandwidth = p->bandwidth_hz / 1000;
+	u32 fvco, xin, xdiv, xdivr;
+	u16 frac;
+	u8 fa, fp, vco_sel, vco_cal;
+	u8 regs[FC11_NR_REGS] = { };
+
+	regs[FC11_REG_7] = 0x0F;
+	regs[FC11_REG_8] = 0x3E;
+	regs[FC11_REG_10] = 0xB8;
+	regs[FC11_REG_11] = 0x80;
+	regs[FC11_REG_RCCAL] = 0x04;
+	err = fc0011_writereg(priv, FC11_REG_7, regs[FC11_REG_7]);
+	err |= fc0011_writereg(priv, FC11_REG_8, regs[FC11_REG_8]);
+	err |= fc0011_writereg(priv, FC11_REG_10, regs[FC11_REG_10]);
+	err |= fc0011_writereg(priv, FC11_REG_11, regs[FC11_REG_11]);
+	err |= fc0011_writereg(priv, FC11_REG_RCCAL, regs[FC11_REG_RCCAL]);
+	if (err)
+		return -EIO;
+
+	/* Set VCO freq and VCO div */
+	if (freq < 54000) {
+		fvco = freq * 64;
+		regs[FC11_REG_VCO] = 0x82;
+	} else if (freq < 108000) {
+		fvco = freq * 32;
+		regs[FC11_REG_VCO] = 0x42;
+	} else if (freq < 216000) {
+		fvco = freq * 16;
+		regs[FC11_REG_VCO] = 0x22;
+	} else if (freq < 432000) {
+		fvco = freq * 8;
+		regs[FC11_REG_VCO] = 0x12;
+	} else {
+		fvco = freq * 4;
+		regs[FC11_REG_VCO] = 0x0A;
+	}
+
+	/* Calc XIN. The PLL reference frequency is 18 MHz. */
+	xdiv = fvco / 18000;
+	frac = fvco - xdiv * 18000;
+	frac = (frac << 15) / 18000;
+	if (frac >= 16384)
+		frac += 32786;
+	if (!frac)
+		xin = 0;
+	else if (frac < 511)
+		xin = 512;
+	else if (frac < 65026)
+		xin = frac;
+	else
+		xin = 65024;
+	regs[FC11_REG_XINHI] = xin >> 8;
+	regs[FC11_REG_XINLO] = xin;
+
+	/* Calc FP and FA */
+	xdivr = xdiv;
+	if (fvco - xdiv * 18000 >= 9000)
+		xdivr += 1; /* round */
+	fp = xdivr / 8;
+	fa = xdivr - fp * 8;
+	if (fa < 2) {
+		fp -= 1;
+		fa += 8;
+	}
+	if (fp > 0x1F) {
+		fp &= 0x1F;
+		fa &= 0xF;
+	}
+	if (fa >= fp) {
+		dev_warn(&priv->i2c->dev,
+			 "fa %02X >= fp %02X, but trying to continue\n",
+			 (unsigned int)(u8)fa, (unsigned int)(u8)fp);
+	}
+	regs[FC11_REG_FA] = fa;
+	regs[FC11_REG_FP] = fp;
+
+	/* Select bandwidth */
+	switch (bandwidth) {
+	case 8000:
+		break;
+	case 7000:
+		regs[FC11_REG_VCOSEL] |= FC11_VCOSEL_BW7M;
+		break;
+	default:
+		dev_warn(&priv->i2c->dev, "Unsupported bandwidth %u kHz. "
+			 "Using 6000 kHz.\n",
+			 bandwidth);
+		bandwidth = 6000;
+		/* fallthrough */
+	case 6000:
+		regs[FC11_REG_VCOSEL] |= FC11_VCOSEL_BW6M;
+		break;
+	}
+
+	/* Pre VCO select */
+	if (fvco < 2320000) {
+		vco_sel = 0;
+		regs[FC11_REG_VCOSEL] &= ~(FC11_VCOSEL_1 | FC11_VCOSEL_2);
+	} else if (fvco < 3080000) {
+		vco_sel = 1;
+		regs[FC11_REG_VCOSEL] &= ~(FC11_VCOSEL_1 | FC11_VCOSEL_2);
+		regs[FC11_REG_VCOSEL] |= FC11_VCOSEL_1;
+	} else {
+		vco_sel = 2;
+		regs[FC11_REG_VCOSEL] &= ~(FC11_VCOSEL_1 | FC11_VCOSEL_2);
+		regs[FC11_REG_VCOSEL] |= FC11_VCOSEL_2;
+	}
+
+	/* Fix for low freqs */
+	if (freq < 45000) {
+		regs[FC11_REG_FA] = 0x6;
+		regs[FC11_REG_FP] = 0x11;
+	}
+
+	/* Clock out fix */
+	regs[FC11_REG_VCOSEL] |= FC11_VCOSEL_CLKOUT;
+
+	/* Write the cached registers */
+	for (i = FC11_REG_FA; i <= FC11_REG_VCOSEL; i++) {
+		err = fc0011_writereg(priv, i, regs[i]);
+		if (err)
+			return err;
+	}
+
+	/* VCO calibration */
+	err = fc0011_vcocal_trigger(priv);
+	if (err)
+		return err;
+	err = fc0011_vcocal_read(priv, &vco_cal);
+	if (err)
+		return err;
+	vco_retries = 0;
+	while (!(vco_cal & FC11_VCOCAL_OK) && vco_retries < 3) {
+		/* Reset the tuner and try again */
+		err = fe->callback(priv->i2c, DVB_FRONTEND_COMPONENT_TUNER,
+				   FC0011_FE_CALLBACK_RESET, priv->addr);
+		if (err) {
+			dev_err(&priv->i2c->dev, "Failed to reset tuner\n");
+			return err;
+		}
+		/* Reinit tuner config */
+		err = 0;
+		for (i = FC11_REG_FA; i <= FC11_REG_VCOSEL; i++)
+			err |= fc0011_writereg(priv, i, regs[i]);
+		err |= fc0011_writereg(priv, FC11_REG_7, regs[FC11_REG_7]);
+		err |= fc0011_writereg(priv, FC11_REG_8, regs[FC11_REG_8]);
+		err |= fc0011_writereg(priv, FC11_REG_10, regs[FC11_REG_10]);
+		err |= fc0011_writereg(priv, FC11_REG_11, regs[FC11_REG_11]);
+		err |= fc0011_writereg(priv, FC11_REG_RCCAL, regs[FC11_REG_RCCAL]);
+		if (err)
+			return -EIO;
+		/* VCO calibration */
+		err = fc0011_vcocal_trigger(priv);
+		if (err)
+			return err;
+		err = fc0011_vcocal_read(priv, &vco_cal);
+		if (err)
+			return err;
+		vco_retries++;
+	}
+	if (!(vco_cal & FC11_VCOCAL_OK)) {
+		dev_err(&priv->i2c->dev,
+			"Failed to read VCO calibration value (got %02X)\n",
+			(unsigned int)vco_cal);
+		return -EIO;
+	}
+	vco_cal &= FC11_VCOCAL_VALUEMASK;
+
+	switch (vco_sel) {
+	case 0:
+		if (vco_cal < 8) {
+			regs[FC11_REG_VCOSEL] &= ~(FC11_VCOSEL_1 | FC11_VCOSEL_2);
+			regs[FC11_REG_VCOSEL] |= FC11_VCOSEL_1;
+			err = fc0011_writereg(priv, FC11_REG_VCOSEL,
+					      regs[FC11_REG_VCOSEL]);
+			if (err)
+				return err;
+			err = fc0011_vcocal_trigger(priv);
+			if (err)
+				return err;
+		} else {
+			regs[FC11_REG_VCOSEL] &= ~(FC11_VCOSEL_1 | FC11_VCOSEL_2);
+			err = fc0011_writereg(priv, FC11_REG_VCOSEL,
+					      regs[FC11_REG_VCOSEL]);
+			if (err)
+				return err;
+		}
+		break;
+	case 1:
+		if (vco_cal < 5) {
+			regs[FC11_REG_VCOSEL] &= ~(FC11_VCOSEL_1 | FC11_VCOSEL_2);
+			regs[FC11_REG_VCOSEL] |= FC11_VCOSEL_2;
+			err = fc0011_writereg(priv, FC11_REG_VCOSEL,
+					      regs[FC11_REG_VCOSEL]);
+			if (err)
+				return err;
+			err = fc0011_vcocal_trigger(priv);
+			if (err)
+				return err;
+		} else if (vco_cal <= 48) {
+			regs[FC11_REG_VCOSEL] &= ~(FC11_VCOSEL_1 | FC11_VCOSEL_2);
+			regs[FC11_REG_VCOSEL] |= FC11_VCOSEL_1;
+			err = fc0011_writereg(priv, FC11_REG_VCOSEL,
+					      regs[FC11_REG_VCOSEL]);
+			if (err)
+				return err;
+		} else {
+			regs[FC11_REG_VCOSEL] &= ~(FC11_VCOSEL_1 | FC11_VCOSEL_2);
+			err = fc0011_writereg(priv, FC11_REG_VCOSEL,
+					      regs[FC11_REG_VCOSEL]);
+			if (err)
+				return err;
+			err = fc0011_vcocal_trigger(priv);
+			if (err)
+				return err;
+		}
+		break;
+	case 2:
+		if (vco_cal > 53) {
+			regs[FC11_REG_VCOSEL] &= ~(FC11_VCOSEL_1 | FC11_VCOSEL_2);
+			regs[FC11_REG_VCOSEL] |= FC11_VCOSEL_1;
+			err = fc0011_writereg(priv, FC11_REG_VCOSEL,
+					      regs[FC11_REG_VCOSEL]);
+			if (err)
+				return err;
+			err = fc0011_vcocal_trigger(priv);
+			if (err)
+				return err;
+		} else {
+			regs[FC11_REG_VCOSEL] &= ~(FC11_VCOSEL_1 | FC11_VCOSEL_2);
+			regs[FC11_REG_VCOSEL] |= FC11_VCOSEL_2;
+			err = fc0011_writereg(priv, FC11_REG_VCOSEL,
+					      regs[FC11_REG_VCOSEL]);
+			if (err)
+				return err;
+		}
+		break;
+	}
+	err = fc0011_vcocal_read(priv, NULL);
+	if (err)
+		return err;
+	usleep_range(10000, 50000);
+
+	err = fc0011_readreg(priv, FC11_REG_RCCAL, &regs[FC11_REG_RCCAL]);
+	if (err)
+		return err;
+	regs[FC11_REG_RCCAL] |= FC11_RCCAL_FORCE;
+	err = fc0011_writereg(priv, FC11_REG_RCCAL, regs[FC11_REG_RCCAL]);
+	if (err)
+		return err;
+	err = fc0011_writereg(priv, FC11_REG_16, 0xB);
+	if (err)
+		return err;
+
+	dev_dbg(&priv->i2c->dev, "Tuned to "
+		"fa=%02X fp=%02X xin=%02X%02X vco=%02X vcosel=%02X "
+		"vcocal=%02X(%u) bw=%u\n",
+		(unsigned int)regs[FC11_REG_FA],
+		(unsigned int)regs[FC11_REG_FP],
+		(unsigned int)regs[FC11_REG_XINHI],
+		(unsigned int)regs[FC11_REG_XINLO],
+		(unsigned int)regs[FC11_REG_VCO],
+		(unsigned int)regs[FC11_REG_VCOSEL],
+		(unsigned int)vco_cal, vco_retries,
+		(unsigned int)bandwidth);
+
+	priv->frequency = p->frequency;
+	priv->bandwidth = p->bandwidth_hz;
+
+	return 0;
+}
+
+static int fc0011_get_frequency(struct dvb_frontend *fe, u32 *frequency)
+{
+	struct fc0011_priv *priv = fe->tuner_priv;
+
+	*frequency = priv->frequency;
+
+	return 0;
+}
+
+static int fc0011_get_if_frequency(struct dvb_frontend *fe, u32 *frequency)
+{
+	*frequency = 0;
+
+	return 0;
+}
+
+static int fc0011_get_bandwidth(struct dvb_frontend *fe, u32 *bandwidth)
+{
+	struct fc0011_priv *priv = fe->tuner_priv;
+
+	*bandwidth = priv->bandwidth;
+
+	return 0;
+}
+
+static const struct dvb_tuner_ops fc0011_tuner_ops = {
+	.info = {
+		.name		= "Fitipower FC0011",
+
+		.frequency_min	= 45000000,
+		.frequency_max	= 1000000000,
+	},
+
+	.release		= fc0011_release,
+	.init			= fc0011_init,
+
+	.set_params		= fc0011_set_params,
+
+	.get_frequency		= fc0011_get_frequency,
+	.get_if_frequency	= fc0011_get_if_frequency,
+	.get_bandwidth		= fc0011_get_bandwidth,
+};
+
+struct dvb_frontend *fc0011_attach(struct dvb_frontend *fe,
+				   struct i2c_adapter *i2c,
+				   const struct fc0011_config *config)
+{
+	struct fc0011_priv *priv;
+
+	priv = kzalloc(sizeof(struct fc0011_priv), GFP_KERNEL);
+	if (!priv)
+		return NULL;
+
+	priv->i2c = i2c;
+	priv->addr = config->i2c_address;
+
+	fe->tuner_priv = priv;
+	fe->ops.tuner_ops = fc0011_tuner_ops;
+
+	dev_info(&priv->i2c->dev, "Fitipower FC0011 tuner attached\n");
+
+	return fe;
+}
+EXPORT_SYMBOL(fc0011_attach);
+
+MODULE_DESCRIPTION("Fitipower FC0011 silicon tuner driver");
+MODULE_AUTHOR("Michael Buesch <m@bues.ch>");
+MODULE_LICENSE("GPL");
diff --git a/drivers/media/common/tuners/fc0011.h b/drivers/media/common/tuners/fc0011.h
new file mode 100644
index 00000000000..0ee581f122d
--- /dev/null
+++ b/drivers/media/common/tuners/fc0011.h
@@ -0,0 +1,41 @@
+#ifndef LINUX_FC0011_H_
+#define LINUX_FC0011_H_
+
+#include "dvb_frontend.h"
+
+
+/** struct fc0011_config - fc0011 hardware config
+ *
+ * @i2c_address: I2C bus address.
+ */
+struct fc0011_config {
+	u8 i2c_address;
+};
+
+/** enum fc0011_fe_callback_commands - Frontend callbacks
+ *
+ * @FC0011_FE_CALLBACK_POWER: Power on tuner hardware.
+ * @FC0011_FE_CALLBACK_RESET: Request a tuner reset.
+ */
+enum fc0011_fe_callback_commands {
+	FC0011_FE_CALLBACK_POWER,
+	FC0011_FE_CALLBACK_RESET,
+};
+
+#if defined(CONFIG_MEDIA_TUNER_FC0011) ||\
+    defined(CONFIG_MEDIA_TUNER_FC0011_MODULE)
+struct dvb_frontend *fc0011_attach(struct dvb_frontend *fe,
+				   struct i2c_adapter *i2c,
+				   const struct fc0011_config *config);
+#else
+static inline
+struct dvb_frontend *fc0011_attach(struct dvb_frontend *fe,
+				   struct i2c_adapter *i2c,
+				   const struct fc0011_config *config)
+{
+	dev_err(&i2c->dev, "fc0011 driver disabled in Kconfig\n");
+	return NULL;
+}
+#endif
+
+#endif /* LINUX_FC0011_H_ */
diff --git a/drivers/media/common/tuners/fc0012-priv.h b/drivers/media/common/tuners/fc0012-priv.h
new file mode 100644
index 00000000000..4577c917e61
--- /dev/null
+++ b/drivers/media/common/tuners/fc0012-priv.h
@@ -0,0 +1,43 @@
+/*
+ * Fitipower FC0012 tuner driver - private includes
+ *
+ * Copyright (C) 2012 Hans-Frieder Vogt <hfvogt@gmx.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., 675 Mass Ave, Cambridge, MA 02139, USA.
+ */
+
+#ifndef _FC0012_PRIV_H_
+#define _FC0012_PRIV_H_
+
+#define LOG_PREFIX "fc0012"
+
+#undef err
+#define err(f, arg...)  printk(KERN_ERR     LOG_PREFIX": " f "\n" , ## arg)
+#undef info
+#define info(f, arg...) printk(KERN_INFO    LOG_PREFIX": " f "\n" , ## arg)
+#undef warn
+#define warn(f, arg...) printk(KERN_WARNING LOG_PREFIX": " f "\n" , ## arg)
+
+struct fc0012_priv {
+	struct i2c_adapter *i2c;
+	u8 addr;
+	u8 dual_master;
+	u8 xtal_freq;
+
+	u32 frequency;
+	u32 bandwidth;
+};
+
+#endif
diff --git a/drivers/media/common/tuners/fc0012.c b/drivers/media/common/tuners/fc0012.c
new file mode 100644
index 00000000000..308135abd54
--- /dev/null
+++ b/drivers/media/common/tuners/fc0012.c
@@ -0,0 +1,467 @@
+/*
+ * Fitipower FC0012 tuner driver
+ *
+ * Copyright (C) 2012 Hans-Frieder Vogt <hfvogt@gmx.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., 675 Mass Ave, Cambridge, MA 02139, USA.
+ */
+
+#include "fc0012.h"
+#include "fc0012-priv.h"
+
+static int fc0012_writereg(struct fc0012_priv *priv, u8 reg, u8 val)
+{
+	u8 buf[2] = {reg, val};
+	struct i2c_msg msg = {
+		.addr = priv->addr, .flags = 0, .buf = buf, .len = 2
+	};
+
+	if (i2c_transfer(priv->i2c, &msg, 1) != 1) {
+		err("I2C write reg failed, reg: %02x, val: %02x", reg, val);
+		return -EREMOTEIO;
+	}
+	return 0;
+}
+
+static int fc0012_readreg(struct fc0012_priv *priv, u8 reg, u8 *val)
+{
+	struct i2c_msg msg[2] = {
+		{ .addr = priv->addr, .flags = 0, .buf = &reg, .len = 1 },
+		{ .addr = priv->addr, .flags = I2C_M_RD, .buf = val, .len = 1 },
+	};
+
+	if (i2c_transfer(priv->i2c, msg, 2) != 2) {
+		err("I2C read reg failed, reg: %02x", reg);
+		return -EREMOTEIO;
+	}
+	return 0;
+}
+
+static int fc0012_release(struct dvb_frontend *fe)
+{
+	kfree(fe->tuner_priv);
+	fe->tuner_priv = NULL;
+	return 0;
+}
+
+static int fc0012_init(struct dvb_frontend *fe)
+{
+	struct fc0012_priv *priv = fe->tuner_priv;
+	int i, ret = 0;
+	unsigned char reg[] = {
+		0x00,	/* dummy reg. 0 */
+		0x05,	/* reg. 0x01 */
+		0x10,	/* reg. 0x02 */
+		0x00,	/* reg. 0x03 */
+		0x00,	/* reg. 0x04 */
+		0x0f,	/* reg. 0x05: may also be 0x0a */
+		0x00,	/* reg. 0x06: divider 2, VCO slow */
+		0x00,	/* reg. 0x07: may also be 0x0f */
+		0xff,	/* reg. 0x08: AGC Clock divide by 256, AGC gain 1/256,
+			   Loop Bw 1/8 */
+		0x6e,	/* reg. 0x09: Disable LoopThrough, Enable LoopThrough: 0x6f */
+		0xb8,	/* reg. 0x0a: Disable LO Test Buffer */
+		0x82,	/* reg. 0x0b: Output Clock is same as clock frequency,
+			   may also be 0x83 */
+		0xfc,	/* reg. 0x0c: depending on AGC Up-Down mode, may need 0xf8 */
+		0x02,	/* reg. 0x0d: AGC Not Forcing & LNA Forcing, 0x02 for DVB-T */
+		0x00,	/* reg. 0x0e */
+		0x00,	/* reg. 0x0f */
+		0x00,	/* reg. 0x10: may also be 0x0d */
+		0x00,	/* reg. 0x11 */
+		0x1f,	/* reg. 0x12: Set to maximum gain */
+		0x08,	/* reg. 0x13: Set to Middle Gain: 0x08,
+			   Low Gain: 0x00, High Gain: 0x10, enable IX2: 0x80 */
+		0x00,	/* reg. 0x14 */
+		0x04,	/* reg. 0x15: Enable LNA COMPS */
+	};
+
+	switch (priv->xtal_freq) {
+	case FC_XTAL_27_MHZ:
+	case FC_XTAL_28_8_MHZ:
+		reg[0x07] |= 0x20;
+		break;
+	case FC_XTAL_36_MHZ:
+	default:
+		break;
+	}
+
+	if (priv->dual_master)
+		reg[0x0c] |= 0x02;
+
+	if (fe->ops.i2c_gate_ctrl)
+		fe->ops.i2c_gate_ctrl(fe, 1); /* open I2C-gate */
+
+	for (i = 1; i < sizeof(reg); i++) {
+		ret = fc0012_writereg(priv, i, reg[i]);
+		if (ret)
+			break;
+	}
+
+	if (fe->ops.i2c_gate_ctrl)
+		fe->ops.i2c_gate_ctrl(fe, 0); /* close I2C-gate */
+
+	if (ret)
+		err("fc0012_writereg failed: %d", ret);
+
+	return ret;
+}
+
+static int fc0012_sleep(struct dvb_frontend *fe)
+{
+	/* nothing to do here */
+	return 0;
+}
+
+static int fc0012_set_params(struct dvb_frontend *fe)
+{
+	struct fc0012_priv *priv = fe->tuner_priv;
+	int i, ret = 0;
+	struct dtv_frontend_properties *p = &fe->dtv_property_cache;
+	u32 freq = p->frequency / 1000;
+	u32 delsys = p->delivery_system;
+	unsigned char reg[7], am, pm, multi, tmp;
+	unsigned long f_vco;
+	unsigned short xtal_freq_khz_2, xin, xdiv;
+	int vco_select = false;
+
+	if (fe->callback) {
+		ret = fe->callback(priv->i2c, DVB_FRONTEND_COMPONENT_TUNER,
+			FC_FE_CALLBACK_VHF_ENABLE, (freq > 300000 ? 0 : 1));
+		if (ret)
+			goto exit;
+	}
+
+	switch (priv->xtal_freq) {
+	case FC_XTAL_27_MHZ:
+		xtal_freq_khz_2 = 27000 / 2;
+		break;
+	case FC_XTAL_36_MHZ:
+		xtal_freq_khz_2 = 36000 / 2;
+		break;
+	case FC_XTAL_28_8_MHZ:
+	default:
+		xtal_freq_khz_2 = 28800 / 2;
+		break;
+	}
+
+	/* select frequency divider and the frequency of VCO */
+	if (freq < 37084) {		/* freq * 96 < 3560000 */
+		multi = 96;
+		reg[5] = 0x82;
+		reg[6] = 0x00;
+	} else if (freq < 55625) {	/* freq * 64 < 3560000 */
+		multi = 64;
+		reg[5] = 0x82;
+		reg[6] = 0x02;
+	} else if (freq < 74167) {	/* freq * 48 < 3560000 */
+		multi = 48;
+		reg[5] = 0x42;
+		reg[6] = 0x00;
+	} else if (freq < 111250) {	/* freq * 32 < 3560000 */
+		multi = 32;
+		reg[5] = 0x42;
+		reg[6] = 0x02;
+	} else if (freq < 148334) {	/* freq * 24 < 3560000 */
+		multi = 24;
+		reg[5] = 0x22;
+		reg[6] = 0x00;
+	} else if (freq < 222500) {	/* freq * 16 < 3560000 */
+		multi = 16;
+		reg[5] = 0x22;
+		reg[6] = 0x02;
+	} else if (freq < 296667) {	/* freq * 12 < 3560000 */
+		multi = 12;
+		reg[5] = 0x12;
+		reg[6] = 0x00;
+	} else if (freq < 445000) {	/* freq * 8 < 3560000 */
+		multi = 8;
+		reg[5] = 0x12;
+		reg[6] = 0x02;
+	} else if (freq < 593334) {	/* freq * 6 < 3560000 */
+		multi = 6;
+		reg[5] = 0x0a;
+		reg[6] = 0x00;
+	} else {
+		multi = 4;
+		reg[5] = 0x0a;
+		reg[6] = 0x02;
+	}
+
+	f_vco = freq * multi;
+
+	if (f_vco >= 3060000) {
+		reg[6] |= 0x08;
+		vco_select = true;
+	}
+
+	if (freq >= 45000) {
+		/* From divided value (XDIV) determined the FA and FP value */
+		xdiv = (unsigned short)(f_vco / xtal_freq_khz_2);
+		if ((f_vco - xdiv * xtal_freq_khz_2) >= (xtal_freq_khz_2 / 2))
+			xdiv++;
+
+		pm = (unsigned char)(xdiv / 8);
+		am = (unsigned char)(xdiv - (8 * pm));
+
+		if (am < 2) {
+			reg[1] = am + 8;
+			reg[2] = pm - 1;
+		} else {
+			reg[1] = am;
+			reg[2] = pm;
+		}
+	} else {
+		/* fix for frequency less than 45 MHz */
+		reg[1] = 0x06;
+		reg[2] = 0x11;
+	}
+
+	/* fix clock out */
+	reg[6] |= 0x20;
+
+	/* From VCO frequency determines the XIN ( fractional part of Delta
+	   Sigma PLL) and divided value (XDIV) */
+	xin = (unsigned short)(f_vco - (f_vco / xtal_freq_khz_2) * xtal_freq_khz_2);
+	xin = (xin << 15) / xtal_freq_khz_2;
+	if (xin >= 16384)
+		xin += 32768;
+
+	reg[3] = xin >> 8;	/* xin with 9 bit resolution */
+	reg[4] = xin & 0xff;
+
+	if (delsys == SYS_DVBT) {
+		reg[6] &= 0x3f;	/* bits 6 and 7 describe the bandwidth */
+		switch (p->bandwidth_hz) {
+		case 6000000:
+			reg[6] |= 0x80;
+			break;
+		case 7000000:
+			reg[6] |= 0x40;
+			break;
+		case 8000000:
+		default:
+			break;
+		}
+	} else {
+		err("%s: modulation type not supported!", __func__);
+		return -EINVAL;
+	}
+
+	/* modified for Realtek demod */
+	reg[5] |= 0x07;
+
+	if (fe->ops.i2c_gate_ctrl)
+		fe->ops.i2c_gate_ctrl(fe, 1); /* open I2C-gate */
+
+	for (i = 1; i <= 6; i++) {
+		ret = fc0012_writereg(priv, i, reg[i]);
+		if (ret)
+			goto exit;
+	}
+
+	/* VCO Calibration */
+	ret = fc0012_writereg(priv, 0x0e, 0x80);
+	if (!ret)
+		ret = fc0012_writereg(priv, 0x0e, 0x00);
+
+	/* VCO Re-Calibration if needed */
+	if (!ret)
+		ret = fc0012_writereg(priv, 0x0e, 0x00);
+
+	if (!ret) {
+		msleep(10);
+		ret = fc0012_readreg(priv, 0x0e, &tmp);
+	}
+	if (ret)
+		goto exit;
+
+	/* vco selection */
+	tmp &= 0x3f;
+
+	if (vco_select) {
+		if (tmp > 0x3c) {
+			reg[6] &= ~0x08;
+			ret = fc0012_writereg(priv, 0x06, reg[6]);
+			if (!ret)
+				ret = fc0012_writereg(priv, 0x0e, 0x80);
+			if (!ret)
+				ret = fc0012_writereg(priv, 0x0e, 0x00);
+		}
+	} else {
+		if (tmp < 0x02) {
+			reg[6] |= 0x08;
+			ret = fc0012_writereg(priv, 0x06, reg[6]);
+			if (!ret)
+				ret = fc0012_writereg(priv, 0x0e, 0x80);
+			if (!ret)
+				ret = fc0012_writereg(priv, 0x0e, 0x00);
+		}
+	}
+
+	priv->frequency = p->frequency;
+	priv->bandwidth = p->bandwidth_hz;
+
+exit:
+	if (fe->ops.i2c_gate_ctrl)
+		fe->ops.i2c_gate_ctrl(fe, 0); /* close I2C-gate */
+	if (ret)
+		warn("%s: failed: %d", __func__, ret);
+	return ret;
+}
+
+static int fc0012_get_frequency(struct dvb_frontend *fe, u32 *frequency)
+{
+	struct fc0012_priv *priv = fe->tuner_priv;
+	*frequency = priv->frequency;
+	return 0;
+}
+
+static int fc0012_get_if_frequency(struct dvb_frontend *fe, u32 *frequency)
+{
+	/* CHECK: always ? */
+	*frequency = 0;
+	return 0;
+}
+
+static int fc0012_get_bandwidth(struct dvb_frontend *fe, u32 *bandwidth)
+{
+	struct fc0012_priv *priv = fe->tuner_priv;
+	*bandwidth = priv->bandwidth;
+	return 0;
+}
+
+#define INPUT_ADC_LEVEL	-8
+
+static int fc0012_get_rf_strength(struct dvb_frontend *fe, u16 *strength)
+{
+	struct fc0012_priv *priv = fe->tuner_priv;
+	int ret;
+	unsigned char tmp;
+	int int_temp, lna_gain, int_lna, tot_agc_gain, power;
+	const int fc0012_lna_gain_table[] = {
+		/* low gain */
+		-63, -58, -99, -73,
+		-63, -65, -54, -60,
+		/* middle gain */
+		 71,  70,  68,  67,
+		 65,  63,  61,  58,
+		/* high gain */
+		197, 191, 188, 186,
+		184, 182, 181, 179,
+	};
+
+	if (fe->ops.i2c_gate_ctrl)
+		fe->ops.i2c_gate_ctrl(fe, 1); /* open I2C-gate */
+
+	ret = fc0012_writereg(priv, 0x12, 0x00);
+	if (ret)
+		goto err;
+
+	ret = fc0012_readreg(priv, 0x12, &tmp);
+	if (ret)
+		goto err;
+	int_temp = tmp;
+
+	ret = fc0012_readreg(priv, 0x13, &tmp);
+	if (ret)
+		goto err;
+	lna_gain = tmp & 0x1f;
+
+	if (fe->ops.i2c_gate_ctrl)
+		fe->ops.i2c_gate_ctrl(fe, 0); /* close I2C-gate */
+
+	if (lna_gain < ARRAY_SIZE(fc0012_lna_gain_table)) {
+		int_lna = fc0012_lna_gain_table[lna_gain];
+		tot_agc_gain = (abs((int_temp >> 5) - 7) - 2 +
+				(int_temp & 0x1f)) * 2;
+		power = INPUT_ADC_LEVEL - tot_agc_gain - int_lna / 10;
+
+		if (power >= 45)
+			*strength = 255;	/* 100% */
+		else if (power < -95)
+			*strength = 0;
+		else
+			*strength = (power + 95) * 255 / 140;
+
+		*strength |= *strength << 8;
+	} else {
+		ret = -1;
+	}
+
+	goto exit;
+
+err:
+	if (fe->ops.i2c_gate_ctrl)
+		fe->ops.i2c_gate_ctrl(fe, 0); /* close I2C-gate */
+exit:
+	if (ret)
+		warn("%s: failed: %d", __func__, ret);
+	return ret;
+}
+
+static const struct dvb_tuner_ops fc0012_tuner_ops = {
+	.info = {
+		.name           = "Fitipower FC0012",
+
+		.frequency_min  = 37000000,	/* estimate */
+		.frequency_max  = 862000000,	/* estimate */
+		.frequency_step = 0,
+	},
+
+	.release	= fc0012_release,
+
+	.init		= fc0012_init,
+	.sleep		= fc0012_sleep,
+
+	.set_params	= fc0012_set_params,
+
+	.get_frequency	= fc0012_get_frequency,
+	.get_if_frequency = fc0012_get_if_frequency,
+	.get_bandwidth	= fc0012_get_bandwidth,
+
+	.get_rf_strength = fc0012_get_rf_strength,
+};
+
+struct dvb_frontend *fc0012_attach(struct dvb_frontend *fe,
+	struct i2c_adapter *i2c, u8 i2c_address, int dual_master,
+	enum fc001x_xtal_freq xtal_freq)
+{
+	struct fc0012_priv *priv = NULL;
+
+	priv = kzalloc(sizeof(struct fc0012_priv), GFP_KERNEL);
+	if (priv == NULL)
+		return NULL;
+
+	priv->i2c = i2c;
+	priv->dual_master = dual_master;
+	priv->addr = i2c_address;
+	priv->xtal_freq = xtal_freq;
+
+	info("Fitipower FC0012 successfully attached.");
+
+	fe->tuner_priv = priv;
+
+	memcpy(&fe->ops.tuner_ops, &fc0012_tuner_ops,
+		sizeof(struct dvb_tuner_ops));
+
+	return fe;
+}
+EXPORT_SYMBOL(fc0012_attach);
+
+MODULE_DESCRIPTION("Fitipower FC0012 silicon tuner driver");
+MODULE_AUTHOR("Hans-Frieder Vogt <hfvogt@gmx.net>");
+MODULE_LICENSE("GPL");
+MODULE_VERSION("0.6");
diff --git a/drivers/media/common/tuners/fc0012.h b/drivers/media/common/tuners/fc0012.h
new file mode 100644
index 00000000000..4dbd5efe884
--- /dev/null
+++ b/drivers/media/common/tuners/fc0012.h
@@ -0,0 +1,44 @@
+/*
+ * Fitipower FC0012 tuner driver - include
+ *
+ * Copyright (C) 2012 Hans-Frieder Vogt <hfvogt@gmx.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., 675 Mass Ave, Cambridge, MA 02139, USA.
+ */
+
+#ifndef _FC0012_H_
+#define _FC0012_H_
+
+#include "dvb_frontend.h"
+#include "fc001x-common.h"
+
+#if defined(CONFIG_MEDIA_TUNER_FC0012) || \
+	(defined(CONFIG_MEDIA_TUNER_FC0012_MODULE) && defined(MODULE))
+extern struct dvb_frontend *fc0012_attach(struct dvb_frontend *fe,
+					struct i2c_adapter *i2c,
+					u8 i2c_address, int dual_master,
+					enum fc001x_xtal_freq xtal_freq);
+#else
+static inline struct dvb_frontend *fc0012_attach(struct dvb_frontend *fe,
+					struct i2c_adapter *i2c,
+					u8 i2c_address, int dual_master,
+					enum fc001x_xtal_freq xtal_freq)
+{
+	printk(KERN_WARNING "%s: driver disabled by Kconfig\n", __func__);
+	return NULL;
+}
+#endif
+
+#endif
diff --git a/drivers/media/common/tuners/fc0013-priv.h b/drivers/media/common/tuners/fc0013-priv.h
new file mode 100644
index 00000000000..bfd49dedea2
--- /dev/null
+++ b/drivers/media/common/tuners/fc0013-priv.h
@@ -0,0 +1,44 @@
+/*
+ * Fitipower FC0013 tuner driver
+ *
+ * Copyright (C) 2012 Hans-Frieder Vogt <hfvogt@gmx.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., 675 Mass Ave, Cambridge, MA 02139, USA.
+ *
+ */
+
+#ifndef _FC0013_PRIV_H_
+#define _FC0013_PRIV_H_
+
+#define LOG_PREFIX "fc0013"
+
+#undef err
+#define err(f, arg...)  printk(KERN_ERR     LOG_PREFIX": " f "\n" , ## arg)
+#undef info
+#define info(f, arg...) printk(KERN_INFO    LOG_PREFIX": " f "\n" , ## arg)
+#undef warn
+#define warn(f, arg...) printk(KERN_WARNING LOG_PREFIX": " f "\n" , ## arg)
+
+struct fc0013_priv {
+	struct i2c_adapter *i2c;
+	u8 addr;
+	u8 dual_master;
+	u8 xtal_freq;
+
+	u32 frequency;
+	u32 bandwidth;
+};
+
+#endif
diff --git a/drivers/media/common/tuners/fc0013.c b/drivers/media/common/tuners/fc0013.c
new file mode 100644
index 00000000000..bd8f0f1e8f3
--- /dev/null
+++ b/drivers/media/common/tuners/fc0013.c
@@ -0,0 +1,634 @@
+/*
+ * Fitipower FC0013 tuner driver
+ *
+ * Copyright (C) 2012 Hans-Frieder Vogt <hfvogt@gmx.net>
+ * partially based on driver code from Fitipower
+ * Copyright (C) 2010 Fitipower Integrated Technology 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 "fc0013.h"
+#include "fc0013-priv.h"
+
+static int fc0013_writereg(struct fc0013_priv *priv, u8 reg, u8 val)
+{
+	u8 buf[2] = {reg, val};
+	struct i2c_msg msg = {
+		.addr = priv->addr, .flags = 0, .buf = buf, .len = 2
+	};
+
+	if (i2c_transfer(priv->i2c, &msg, 1) != 1) {
+		err("I2C write reg failed, reg: %02x, val: %02x", reg, val);
+		return -EREMOTEIO;
+	}
+	return 0;
+}
+
+static int fc0013_readreg(struct fc0013_priv *priv, u8 reg, u8 *val)
+{
+	struct i2c_msg msg[2] = {
+		{ .addr = priv->addr, .flags = 0, .buf = &reg, .len = 1 },
+		{ .addr = priv->addr, .flags = I2C_M_RD, .buf = val, .len = 1 },
+	};
+
+	if (i2c_transfer(priv->i2c, msg, 2) != 2) {
+		err("I2C read reg failed, reg: %02x", reg);
+		return -EREMOTEIO;
+	}
+	return 0;
+}
+
+static int fc0013_release(struct dvb_frontend *fe)
+{
+	kfree(fe->tuner_priv);
+	fe->tuner_priv = NULL;
+	return 0;
+}
+
+static int fc0013_init(struct dvb_frontend *fe)
+{
+	struct fc0013_priv *priv = fe->tuner_priv;
+	int i, ret = 0;
+	unsigned char reg[] = {
+		0x00,	/* reg. 0x00: dummy */
+		0x09,	/* reg. 0x01 */
+		0x16,	/* reg. 0x02 */
+		0x00,	/* reg. 0x03 */
+		0x00,	/* reg. 0x04 */
+		0x17,	/* reg. 0x05 */
+		0x02,	/* reg. 0x06 */
+		0x0a,	/* reg. 0x07: CHECK */
+		0xff,	/* reg. 0x08: AGC Clock divide by 256, AGC gain 1/256,
+			   Loop Bw 1/8 */
+		0x6f,	/* reg. 0x09: enable LoopThrough */
+		0xb8,	/* reg. 0x0a: Disable LO Test Buffer */
+		0x82,	/* reg. 0x0b: CHECK */
+		0xfc,	/* reg. 0x0c: depending on AGC Up-Down mode, may need 0xf8 */
+		0x01,	/* reg. 0x0d: AGC Not Forcing & LNA Forcing, may need 0x02 */
+		0x00,	/* reg. 0x0e */
+		0x00,	/* reg. 0x0f */
+		0x00,	/* reg. 0x10 */
+		0x00,	/* reg. 0x11 */
+		0x00,	/* reg. 0x12 */
+		0x00,	/* reg. 0x13 */
+		0x50,	/* reg. 0x14: DVB-t High Gain, UHF.
+			   Middle Gain: 0x48, Low Gain: 0x40 */
+		0x01,	/* reg. 0x15 */
+	};
+
+	switch (priv->xtal_freq) {
+	case FC_XTAL_27_MHZ:
+	case FC_XTAL_28_8_MHZ:
+		reg[0x07] |= 0x20;
+		break;
+	case FC_XTAL_36_MHZ:
+	default:
+		break;
+	}
+
+	if (priv->dual_master)
+		reg[0x0c] |= 0x02;
+
+	if (fe->ops.i2c_gate_ctrl)
+		fe->ops.i2c_gate_ctrl(fe, 1); /* open I2C-gate */
+
+	for (i = 1; i < sizeof(reg); i++) {
+		ret = fc0013_writereg(priv, i, reg[i]);
+		if (ret)
+			break;
+	}
+
+	if (fe->ops.i2c_gate_ctrl)
+		fe->ops.i2c_gate_ctrl(fe, 0); /* close I2C-gate */
+
+	if (ret)
+		err("fc0013_writereg failed: %d", ret);
+
+	return ret;
+}
+
+static int fc0013_sleep(struct dvb_frontend *fe)
+{
+	/* nothing to do here */
+	return 0;
+}
+
+int fc0013_rc_cal_add(struct dvb_frontend *fe, int rc_val)
+{
+	struct fc0013_priv *priv = fe->tuner_priv;
+	int ret;
+	u8 rc_cal;
+	int val;
+
+	if (fe->ops.i2c_gate_ctrl)
+		fe->ops.i2c_gate_ctrl(fe, 1); /* open I2C-gate */
+
+	/* push rc_cal value, get rc_cal value */
+	ret = fc0013_writereg(priv, 0x10, 0x00);
+	if (ret)
+		goto error_out;
+
+	/* get rc_cal value */
+	ret = fc0013_readreg(priv, 0x10, &rc_cal);
+	if (ret)
+		goto error_out;
+
+	rc_cal &= 0x0f;
+
+	val = (int)rc_cal + rc_val;
+
+	/* forcing rc_cal */
+	ret = fc0013_writereg(priv, 0x0d, 0x11);
+	if (ret)
+		goto error_out;
+
+	/* modify rc_cal value */
+	if (val > 15)
+		ret = fc0013_writereg(priv, 0x10, 0x0f);
+	else if (val < 0)
+		ret = fc0013_writereg(priv, 0x10, 0x00);
+	else
+		ret = fc0013_writereg(priv, 0x10, (u8)val);
+
+error_out:
+	if (fe->ops.i2c_gate_ctrl)
+		fe->ops.i2c_gate_ctrl(fe, 0); /* close I2C-gate */
+
+	return ret;
+}
+EXPORT_SYMBOL(fc0013_rc_cal_add);
+
+int fc0013_rc_cal_reset(struct dvb_frontend *fe)
+{
+	struct fc0013_priv *priv = fe->tuner_priv;
+	int ret;
+
+	if (fe->ops.i2c_gate_ctrl)
+		fe->ops.i2c_gate_ctrl(fe, 1); /* open I2C-gate */
+
+	ret = fc0013_writereg(priv, 0x0d, 0x01);
+	if (!ret)
+		ret = fc0013_writereg(priv, 0x10, 0x00);
+
+	if (fe->ops.i2c_gate_ctrl)
+		fe->ops.i2c_gate_ctrl(fe, 0); /* close I2C-gate */
+
+	return ret;
+}
+EXPORT_SYMBOL(fc0013_rc_cal_reset);
+
+static int fc0013_set_vhf_track(struct fc0013_priv *priv, u32 freq)
+{
+	int ret;
+	u8 tmp;
+
+	ret = fc0013_readreg(priv, 0x1d, &tmp);
+	if (ret)
+		goto error_out;
+	tmp &= 0xe3;
+	if (freq <= 177500) {		/* VHF Track: 7 */
+		ret = fc0013_writereg(priv, 0x1d, tmp | 0x1c);
+	} else if (freq <= 184500) {	/* VHF Track: 6 */
+		ret = fc0013_writereg(priv, 0x1d, tmp | 0x18);
+	} else if (freq <= 191500) {	/* VHF Track: 5 */
+		ret = fc0013_writereg(priv, 0x1d, tmp | 0x14);
+	} else if (freq <= 198500) {	/* VHF Track: 4 */
+		ret = fc0013_writereg(priv, 0x1d, tmp | 0x10);
+	} else if (freq <= 205500) {	/* VHF Track: 3 */
+		ret = fc0013_writereg(priv, 0x1d, tmp | 0x0c);
+	} else if (freq <= 219500) {	/* VHF Track: 2 */
+		ret = fc0013_writereg(priv, 0x1d, tmp | 0x08);
+	} else if (freq < 300000) {	/* VHF Track: 1 */
+		ret = fc0013_writereg(priv, 0x1d, tmp | 0x04);
+	} else {			/* UHF and GPS */
+		ret = fc0013_writereg(priv, 0x1d, tmp | 0x1c);
+	}
+	if (ret)
+		goto error_out;
+error_out:
+	return ret;
+}
+
+static int fc0013_set_params(struct dvb_frontend *fe)
+{
+	struct fc0013_priv *priv = fe->tuner_priv;
+	int i, ret = 0;
+	struct dtv_frontend_properties *p = &fe->dtv_property_cache;
+	u32 freq = p->frequency / 1000;
+	u32 delsys = p->delivery_system;
+	unsigned char reg[7], am, pm, multi, tmp;
+	unsigned long f_vco;
+	unsigned short xtal_freq_khz_2, xin, xdiv;
+	int vco_select = false;
+
+	if (fe->callback) {
+		ret = fe->callback(priv->i2c, DVB_FRONTEND_COMPONENT_TUNER,
+			FC_FE_CALLBACK_VHF_ENABLE, (freq > 300000 ? 0 : 1));
+		if (ret)
+			goto exit;
+	}
+
+	switch (priv->xtal_freq) {
+	case FC_XTAL_27_MHZ:
+		xtal_freq_khz_2 = 27000 / 2;
+		break;
+	case FC_XTAL_36_MHZ:
+		xtal_freq_khz_2 = 36000 / 2;
+		break;
+	case FC_XTAL_28_8_MHZ:
+	default:
+		xtal_freq_khz_2 = 28800 / 2;
+		break;
+	}
+
+	if (fe->ops.i2c_gate_ctrl)
+		fe->ops.i2c_gate_ctrl(fe, 1); /* open I2C-gate */
+
+	/* set VHF track */
+	ret = fc0013_set_vhf_track(priv, freq);
+	if (ret)
+		goto exit;
+
+	if (freq < 300000) {
+		/* enable VHF filter */
+		ret = fc0013_readreg(priv, 0x07, &tmp);
+		if (ret)
+			goto exit;
+		ret = fc0013_writereg(priv, 0x07, tmp | 0x10);
+		if (ret)
+			goto exit;
+
+		/* disable UHF & disable GPS */
+		ret = fc0013_readreg(priv, 0x14, &tmp);
+		if (ret)
+			goto exit;
+		ret = fc0013_writereg(priv, 0x14, tmp & 0x1f);
+		if (ret)
+			goto exit;
+	} else if (freq <= 862000) {
+		/* disable VHF filter */
+		ret = fc0013_readreg(priv, 0x07, &tmp);
+		if (ret)
+			goto exit;
+		ret = fc0013_writereg(priv, 0x07, tmp & 0xef);
+		if (ret)
+			goto exit;
+
+		/* enable UHF & disable GPS */
+		ret = fc0013_readreg(priv, 0x14, &tmp);
+		if (ret)
+			goto exit;
+		ret = fc0013_writereg(priv, 0x14, (tmp & 0x1f) | 0x40);
+		if (ret)
+			goto exit;
+	} else {
+		/* disable VHF filter */
+		ret = fc0013_readreg(priv, 0x07, &tmp);
+		if (ret)
+			goto exit;
+		ret = fc0013_writereg(priv, 0x07, tmp & 0xef);
+		if (ret)
+			goto exit;
+
+		/* disable UHF & enable GPS */
+		ret = fc0013_readreg(priv, 0x14, &tmp);
+		if (ret)
+			goto exit;
+		ret = fc0013_writereg(priv, 0x14, (tmp & 0x1f) | 0x20);
+		if (ret)
+			goto exit;
+	}
+
+	/* select frequency divider and the frequency of VCO */
+	if (freq < 37084) {		/* freq * 96 < 3560000 */
+		multi = 96;
+		reg[5] = 0x82;
+		reg[6] = 0x00;
+	} else if (freq < 55625) {	/* freq * 64 < 3560000 */
+		multi = 64;
+		reg[5] = 0x02;
+		reg[6] = 0x02;
+	} else if (freq < 74167) {	/* freq * 48 < 3560000 */
+		multi = 48;
+		reg[5] = 0x42;
+		reg[6] = 0x00;
+	} else if (freq < 111250) {	/* freq * 32 < 3560000 */
+		multi = 32;
+		reg[5] = 0x82;
+		reg[6] = 0x02;
+	} else if (freq < 148334) {	/* freq * 24 < 3560000 */
+		multi = 24;
+		reg[5] = 0x22;
+		reg[6] = 0x00;
+	} else if (freq < 222500) {	/* freq * 16 < 3560000 */
+		multi = 16;
+		reg[5] = 0x42;
+		reg[6] = 0x02;
+	} else if (freq < 296667) {	/* freq * 12 < 3560000 */
+		multi = 12;
+		reg[5] = 0x12;
+		reg[6] = 0x00;
+	} else if (freq < 445000) {	/* freq * 8 < 3560000 */
+		multi = 8;
+		reg[5] = 0x22;
+		reg[6] = 0x02;
+	} else if (freq < 593334) {	/* freq * 6 < 3560000 */
+		multi = 6;
+		reg[5] = 0x0a;
+		reg[6] = 0x00;
+	} else if (freq < 950000) {	/* freq * 4 < 3800000 */
+		multi = 4;
+		reg[5] = 0x12;
+		reg[6] = 0x02;
+	} else {
+		multi = 2;
+		reg[5] = 0x0a;
+		reg[6] = 0x02;
+	}
+
+	f_vco = freq * multi;
+
+	if (f_vco >= 3060000) {
+		reg[6] |= 0x08;
+		vco_select = true;
+	}
+
+	if (freq >= 45000) {
+		/* From divided value (XDIV) determined the FA and FP value */
+		xdiv = (unsigned short)(f_vco / xtal_freq_khz_2);
+		if ((f_vco - xdiv * xtal_freq_khz_2) >= (xtal_freq_khz_2 / 2))
+			xdiv++;
+
+		pm = (unsigned char)(xdiv / 8);
+		am = (unsigned char)(xdiv - (8 * pm));
+
+		if (am < 2) {
+			reg[1] = am + 8;
+			reg[2] = pm - 1;
+		} else {
+			reg[1] = am;
+			reg[2] = pm;
+		}
+	} else {
+		/* fix for frequency less than 45 MHz */
+		reg[1] = 0x06;
+		reg[2] = 0x11;
+	}
+
+	/* fix clock out */
+	reg[6] |= 0x20;
+
+	/* From VCO frequency determines the XIN ( fractional part of Delta
+	   Sigma PLL) and divided value (XDIV) */
+	xin = (unsigned short)(f_vco - (f_vco / xtal_freq_khz_2) * xtal_freq_khz_2);
+	xin = (xin << 15) / xtal_freq_khz_2;
+	if (xin >= 16384)
+		xin += 32768;
+
+	reg[3] = xin >> 8;
+	reg[4] = xin & 0xff;
+
+	if (delsys == SYS_DVBT) {
+		reg[6] &= 0x3f; /* bits 6 and 7 describe the bandwidth */
+		switch (p->bandwidth_hz) {
+		case 6000000:
+			reg[6] |= 0x80;
+			break;
+		case 7000000:
+			reg[6] |= 0x40;
+			break;
+		case 8000000:
+		default:
+			break;
+		}
+	} else {
+		err("%s: modulation type not supported!", __func__);
+		return -EINVAL;
+	}
+
+	/* modified for Realtek demod */
+	reg[5] |= 0x07;
+
+	for (i = 1; i <= 6; i++) {
+		ret = fc0013_writereg(priv, i, reg[i]);
+		if (ret)
+			goto exit;
+	}
+
+	ret = fc0013_readreg(priv, 0x11, &tmp);
+	if (ret)
+		goto exit;
+	if (multi == 64)
+		ret = fc0013_writereg(priv, 0x11, tmp | 0x04);
+	else
+		ret = fc0013_writereg(priv, 0x11, tmp & 0xfb);
+	if (ret)
+		goto exit;
+
+	/* VCO Calibration */
+	ret = fc0013_writereg(priv, 0x0e, 0x80);
+	if (!ret)
+		ret = fc0013_writereg(priv, 0x0e, 0x00);
+
+	/* VCO Re-Calibration if needed */
+	if (!ret)
+		ret = fc0013_writereg(priv, 0x0e, 0x00);
+
+	if (!ret) {
+		msleep(10);
+		ret = fc0013_readreg(priv, 0x0e, &tmp);
+	}
+	if (ret)
+		goto exit;
+
+	/* vco selection */
+	tmp &= 0x3f;
+
+	if (vco_select) {
+		if (tmp > 0x3c) {
+			reg[6] &= ~0x08;
+			ret = fc0013_writereg(priv, 0x06, reg[6]);
+			if (!ret)
+				ret = fc0013_writereg(priv, 0x0e, 0x80);
+			if (!ret)
+				ret = fc0013_writereg(priv, 0x0e, 0x00);
+		}
+	} else {
+		if (tmp < 0x02) {
+			reg[6] |= 0x08;
+			ret = fc0013_writereg(priv, 0x06, reg[6]);
+			if (!ret)
+				ret = fc0013_writereg(priv, 0x0e, 0x80);
+			if (!ret)
+				ret = fc0013_writereg(priv, 0x0e, 0x00);
+		}
+	}
+
+	priv->frequency = p->frequency;
+	priv->bandwidth = p->bandwidth_hz;
+
+exit:
+	if (fe->ops.i2c_gate_ctrl)
+		fe->ops.i2c_gate_ctrl(fe, 0); /* close I2C-gate */
+	if (ret)
+		warn("%s: failed: %d", __func__, ret);
+	return ret;
+}
+
+static int fc0013_get_frequency(struct dvb_frontend *fe, u32 *frequency)
+{
+	struct fc0013_priv *priv = fe->tuner_priv;
+	*frequency = priv->frequency;
+	return 0;
+}
+
+static int fc0013_get_if_frequency(struct dvb_frontend *fe, u32 *frequency)
+{
+	/* always ? */
+	*frequency = 0;
+	return 0;
+}
+
+static int fc0013_get_bandwidth(struct dvb_frontend *fe, u32 *bandwidth)
+{
+	struct fc0013_priv *priv = fe->tuner_priv;
+	*bandwidth = priv->bandwidth;
+	return 0;
+}
+
+#define INPUT_ADC_LEVEL	-8
+
+static int fc0013_get_rf_strength(struct dvb_frontend *fe, u16 *strength)
+{
+	struct fc0013_priv *priv = fe->tuner_priv;
+	int ret;
+	unsigned char tmp;
+	int int_temp, lna_gain, int_lna, tot_agc_gain, power;
+	const int fc0013_lna_gain_table[] = {
+		/* low gain */
+		-63, -58, -99, -73,
+		-63, -65, -54, -60,
+		/* middle gain */
+		 71,  70,  68,  67,
+		 65,  63,  61,  58,
+		/* high gain */
+		197, 191, 188, 186,
+		184, 182, 181, 179,
+	};
+
+	if (fe->ops.i2c_gate_ctrl)
+		fe->ops.i2c_gate_ctrl(fe, 1); /* open I2C-gate */
+
+	ret = fc0013_writereg(priv, 0x13, 0x00);
+	if (ret)
+		goto err;
+
+	ret = fc0013_readreg(priv, 0x13, &tmp);
+	if (ret)
+		goto err;
+	int_temp = tmp;
+
+	ret = fc0013_readreg(priv, 0x14, &tmp);
+	if (ret)
+		goto err;
+	lna_gain = tmp & 0x1f;
+
+	if (fe->ops.i2c_gate_ctrl)
+		fe->ops.i2c_gate_ctrl(fe, 0); /* close I2C-gate */
+
+	if (lna_gain < ARRAY_SIZE(fc0013_lna_gain_table)) {
+		int_lna = fc0013_lna_gain_table[lna_gain];
+		tot_agc_gain = (abs((int_temp >> 5) - 7) - 2 +
+				(int_temp & 0x1f)) * 2;
+		power = INPUT_ADC_LEVEL - tot_agc_gain - int_lna / 10;
+
+		if (power >= 45)
+			*strength = 255;	/* 100% */
+		else if (power < -95)
+			*strength = 0;
+		else
+			*strength = (power + 95) * 255 / 140;
+
+		*strength |= *strength << 8;
+	} else {
+		ret = -1;
+	}
+
+	goto exit;
+
+err:
+	if (fe->ops.i2c_gate_ctrl)
+		fe->ops.i2c_gate_ctrl(fe, 0); /* close I2C-gate */
+exit:
+	if (ret)
+		warn("%s: failed: %d", __func__, ret);
+	return ret;
+}
+
+static const struct dvb_tuner_ops fc0013_tuner_ops = {
+	.info = {
+		.name		= "Fitipower FC0013",
+
+		.frequency_min	= 37000000,	/* estimate */
+		.frequency_max	= 1680000000,	/* CHECK */
+		.frequency_step	= 0,
+	},
+
+	.release	= fc0013_release,
+
+	.init		= fc0013_init,
+	.sleep		= fc0013_sleep,
+
+	.set_params	= fc0013_set_params,
+
+	.get_frequency	= fc0013_get_frequency,
+	.get_if_frequency = fc0013_get_if_frequency,
+	.get_bandwidth	= fc0013_get_bandwidth,
+
+	.get_rf_strength = fc0013_get_rf_strength,
+};
+
+struct dvb_frontend *fc0013_attach(struct dvb_frontend *fe,
+	struct i2c_adapter *i2c, u8 i2c_address, int dual_master,
+	enum fc001x_xtal_freq xtal_freq)
+{
+	struct fc0013_priv *priv = NULL;
+
+	priv = kzalloc(sizeof(struct fc0013_priv), GFP_KERNEL);
+	if (priv == NULL)
+		return NULL;
+
+	priv->i2c = i2c;
+	priv->dual_master = dual_master;
+	priv->addr = i2c_address;
+	priv->xtal_freq = xtal_freq;
+
+	info("Fitipower FC0013 successfully attached.");
+
+	fe->tuner_priv = priv;
+
+	memcpy(&fe->ops.tuner_ops, &fc0013_tuner_ops,
+		sizeof(struct dvb_tuner_ops));
+
+	return fe;
+}
+EXPORT_SYMBOL(fc0013_attach);
+
+MODULE_DESCRIPTION("Fitipower FC0013 silicon tuner driver");
+MODULE_AUTHOR("Hans-Frieder Vogt <hfvogt@gmx.net>");
+MODULE_LICENSE("GPL");
+MODULE_VERSION("0.2");
diff --git a/drivers/media/common/tuners/fc0013.h b/drivers/media/common/tuners/fc0013.h
new file mode 100644
index 00000000000..594efd64aee
--- /dev/null
+++ b/drivers/media/common/tuners/fc0013.h
@@ -0,0 +1,57 @@
+/*
+ * Fitipower FC0013 tuner driver
+ *
+ * Copyright (C) 2012 Hans-Frieder Vogt <hfvogt@gmx.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., 675 Mass Ave, Cambridge, MA 02139, USA.
+ *
+ */
+
+#ifndef _FC0013_H_
+#define _FC0013_H_
+
+#include "dvb_frontend.h"
+#include "fc001x-common.h"
+
+#if defined(CONFIG_MEDIA_TUNER_FC0013) || \
+	(defined(CONFIG_MEDIA_TUNER_FC0013_MODULE) && defined(MODULE))
+extern struct dvb_frontend *fc0013_attach(struct dvb_frontend *fe,
+					struct i2c_adapter *i2c,
+					u8 i2c_address, int dual_master,
+					enum fc001x_xtal_freq xtal_freq);
+extern int fc0013_rc_cal_add(struct dvb_frontend *fe, int rc_val);
+extern int fc0013_rc_cal_reset(struct dvb_frontend *fe);
+#else
+static inline struct dvb_frontend *fc0013_attach(struct dvb_frontend *fe,
+					struct i2c_adapter *i2c,
+					u8 i2c_address, int dual_master,
+					enum fc001x_xtal_freq xtal_freq)
+{
+	printk(KERN_WARNING "%s: driver disabled by Kconfig\n", __func__);
+	return NULL;
+}
+
+static inline int fc0013_rc_cal_add(struct dvb_frontend *fe, int rc_val)
+{
+	return 0;
+}
+
+static inline int fc0013_rc_cal_reset(struct dvb_frontend *fe)
+{
+	return 0;
+}
+#endif
+
+#endif
diff --git a/drivers/media/common/tuners/fc001x-common.h b/drivers/media/common/tuners/fc001x-common.h
new file mode 100644
index 00000000000..71881815693
--- /dev/null
+++ b/drivers/media/common/tuners/fc001x-common.h
@@ -0,0 +1,39 @@
+/*
+ * Fitipower FC0012 & FC0013 tuner driver - common defines
+ *
+ * Copyright (C) 2012 Hans-Frieder Vogt <hfvogt@gmx.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., 675 Mass Ave, Cambridge, MA 02139, USA.
+ */
+
+#ifndef _FC001X_COMMON_H_
+#define _FC001X_COMMON_H_
+
+enum fc001x_xtal_freq {
+	FC_XTAL_27_MHZ,		/* 27000000 */
+	FC_XTAL_28_8_MHZ,	/* 28800000 */
+	FC_XTAL_36_MHZ,		/* 36000000 */
+};
+
+/*
+ * enum fc001x_fe_callback_commands - Frontend callbacks
+ *
+ * @FC_FE_CALLBACK_VHF_ENABLE: enable VHF or UHF
+ */
+enum fc001x_fe_callback_commands {
+	FC_FE_CALLBACK_VHF_ENABLE,
+};
+
+#endif
diff --git a/drivers/media/common/tuners/tua9001.c b/drivers/media/common/tuners/tua9001.c
new file mode 100644
index 00000000000..de260708467
--- /dev/null
+++ b/drivers/media/common/tuners/tua9001.c
@@ -0,0 +1,215 @@
+/*
+ * Infineon TUA 9001 silicon tuner driver
+ *
+ * Copyright (C) 2009 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 "tua9001.h"
+#include "tua9001_priv.h"
+
+/* write register */
+static int tua9001_wr_reg(struct tua9001_priv *priv, u8 reg, u16 val)
+{
+	int ret;
+	u8 buf[3] = { reg, (val >> 8) & 0xff, (val >> 0) & 0xff };
+	struct i2c_msg msg[1] = {
+		{
+			.addr = priv->cfg->i2c_addr,
+			.flags = 0,
+			.len = sizeof(buf),
+			.buf = buf,
+		}
+	};
+
+	ret = i2c_transfer(priv->i2c, msg, 1);
+	if (ret == 1) {
+		ret = 0;
+	} else {
+		printk(KERN_WARNING "%s: I2C wr failed=%d reg=%02x\n",
+				__func__, ret, reg);
+		ret = -EREMOTEIO;
+	}
+
+	return ret;
+}
+
+static int tua9001_release(struct dvb_frontend *fe)
+{
+	kfree(fe->tuner_priv);
+	fe->tuner_priv = NULL;
+
+	return 0;
+}
+
+static int tua9001_init(struct dvb_frontend *fe)
+{
+	struct tua9001_priv *priv = fe->tuner_priv;
+	int ret = 0;
+	u8 i;
+	struct reg_val data[] = {
+		{ 0x1e, 0x6512 },
+		{ 0x25, 0xb888 },
+		{ 0x39, 0x5460 },
+		{ 0x3b, 0x00c0 },
+		{ 0x3a, 0xf000 },
+		{ 0x08, 0x0000 },
+		{ 0x32, 0x0030 },
+		{ 0x41, 0x703a },
+		{ 0x40, 0x1c78 },
+		{ 0x2c, 0x1c00 },
+		{ 0x36, 0xc013 },
+		{ 0x37, 0x6f18 },
+		{ 0x27, 0x0008 },
+		{ 0x2a, 0x0001 },
+		{ 0x34, 0x0a40 },
+	};
+
+	if (fe->ops.i2c_gate_ctrl)
+		fe->ops.i2c_gate_ctrl(fe, 1); /* open i2c-gate */
+
+	for (i = 0; i < ARRAY_SIZE(data); i++) {
+		ret = tua9001_wr_reg(priv, data[i].reg, data[i].val);
+		if (ret)
+			break;
+	}
+
+	if (fe->ops.i2c_gate_ctrl)
+		fe->ops.i2c_gate_ctrl(fe, 0); /* close i2c-gate */
+
+	if (ret < 0)
+		pr_debug("%s: failed=%d\n", __func__, ret);
+
+	return ret;
+}
+
+static int tua9001_set_params(struct dvb_frontend *fe)
+{
+	struct tua9001_priv *priv = fe->tuner_priv;
+	struct dtv_frontend_properties *c = &fe->dtv_property_cache;
+	int ret, i;
+	u16 val;
+	u32 frequency;
+	struct reg_val data[2];
+
+	pr_debug("%s: delivery_system=%d frequency=%d bandwidth_hz=%d\n",
+			__func__, c->delivery_system, c->frequency,
+			c->bandwidth_hz);
+
+	switch (c->delivery_system) {
+	case SYS_DVBT:
+		switch (c->bandwidth_hz) {
+		case 8000000:
+			val  = 0x0000;
+			break;
+		case 7000000:
+			val  = 0x1000;
+			break;
+		case 6000000:
+			val  = 0x2000;
+			break;
+		case 5000000:
+			val  = 0x3000;
+			break;
+		default:
+			ret = -EINVAL;
+			goto err;
+		}
+		break;
+	default:
+		ret = -EINVAL;
+		goto err;
+	}
+
+	data[0].reg = 0x04;
+	data[0].val = val;
+
+	frequency = (c->frequency - 150000000);
+	frequency /= 100;
+	frequency *= 48;
+	frequency /= 10000;
+
+	data[1].reg = 0x1f;
+	data[1].val = frequency;
+
+	if (fe->ops.i2c_gate_ctrl)
+		fe->ops.i2c_gate_ctrl(fe, 1); /* open i2c-gate */
+
+	for (i = 0; i < ARRAY_SIZE(data); i++) {
+		ret = tua9001_wr_reg(priv, data[i].reg, data[i].val);
+		if (ret < 0)
+			break;
+	}
+
+	if (fe->ops.i2c_gate_ctrl)
+		fe->ops.i2c_gate_ctrl(fe, 0); /* close i2c-gate */
+
+err:
+	if (ret < 0)
+		pr_debug("%s: failed=%d\n", __func__, ret);
+
+	return ret;
+}
+
+static int tua9001_get_if_frequency(struct dvb_frontend *fe, u32 *frequency)
+{
+	*frequency = 0; /* Zero-IF */
+
+	return 0;
+}
+
+static const struct dvb_tuner_ops tua9001_tuner_ops = {
+	.info = {
+		.name           = "Infineon TUA 9001",
+
+		.frequency_min  = 170000000,
+		.frequency_max  = 862000000,
+		.frequency_step = 0,
+	},
+
+	.release = tua9001_release,
+
+	.init = tua9001_init,
+	.set_params = tua9001_set_params,
+
+	.get_if_frequency = tua9001_get_if_frequency,
+};
+
+struct dvb_frontend *tua9001_attach(struct dvb_frontend *fe,
+		struct i2c_adapter *i2c, struct tua9001_config *cfg)
+{
+	struct tua9001_priv *priv = NULL;
+
+	priv = kzalloc(sizeof(struct tua9001_priv), GFP_KERNEL);
+	if (priv == NULL)
+		return NULL;
+
+	priv->cfg = cfg;
+	priv->i2c = i2c;
+
+	printk(KERN_INFO "Infineon TUA 9001 successfully attached.");
+
+	memcpy(&fe->ops.tuner_ops, &tua9001_tuner_ops,
+			sizeof(struct dvb_tuner_ops));
+
+	fe->tuner_priv = priv;
+	return fe;
+}
+EXPORT_SYMBOL(tua9001_attach);
+
+MODULE_DESCRIPTION("Infineon TUA 9001 silicon tuner driver");
+MODULE_AUTHOR("Antti Palosaari <crope@iki.fi>");
+MODULE_LICENSE("GPL");
diff --git a/drivers/media/common/tuners/tua9001.h b/drivers/media/common/tuners/tua9001.h
new file mode 100644
index 00000000000..38d6ae76b1d
--- /dev/null
+++ b/drivers/media/common/tuners/tua9001.h
@@ -0,0 +1,46 @@
+/*
+ * Infineon TUA 9001 silicon tuner driver
+ *
+ * Copyright (C) 2009 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 TUA9001_H
+#define TUA9001_H
+
+#include "dvb_frontend.h"
+
+struct tua9001_config {
+	/*
+	 * I2C address
+	 */
+	u8 i2c_addr;
+};
+
+#if defined(CONFIG_MEDIA_TUNER_TUA9001) || \
+	(defined(CONFIG_MEDIA_TUNER_TUA9001_MODULE) && defined(MODULE))
+extern struct dvb_frontend *tua9001_attach(struct dvb_frontend *fe,
+		struct i2c_adapter *i2c, struct tua9001_config *cfg);
+#else
+static inline struct dvb_frontend *tua9001_attach(struct dvb_frontend *fe,
+		struct i2c_adapter *i2c, struct tua9001_config *cfg)
+{
+	printk(KERN_WARNING "%s: driver disabled by Kconfig\n", __func__);
+	return NULL;
+}
+#endif
+
+#endif
diff --git a/drivers/media/common/tuners/tua9001_priv.h b/drivers/media/common/tuners/tua9001_priv.h
new file mode 100644
index 00000000000..73cc1ce0575
--- /dev/null
+++ b/drivers/media/common/tuners/tua9001_priv.h
@@ -0,0 +1,34 @@
+/*
+ * Infineon TUA 9001 silicon tuner driver
+ *
+ * Copyright (C) 2009 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 TUA9001_PRIV_H
+#define TUA9001_PRIV_H
+
+struct reg_val {
+	u8 reg;
+	u16 val;
+};
+
+struct tua9001_priv {
+	struct tua9001_config *cfg;
+	struct i2c_adapter *i2c;
+};
+
+#endif
diff --git a/drivers/media/common/tuners/xc5000.c b/drivers/media/common/tuners/xc5000.c
index eab2ea42420..dcca42ca57b 100644
--- a/drivers/media/common/tuners/xc5000.c
+++ b/drivers/media/common/tuners/xc5000.c
@@ -54,7 +54,7 @@ struct xc5000_priv {
 	struct list_head hybrid_tuner_instance_list;
 
 	u32 if_khz;
-	u32 xtal_khz;
+	u16 xtal_khz;
 	u32 freq_hz;
 	u32 bandwidth;
 	u8  video_standard;
@@ -631,7 +631,10 @@ static int xc5000_fwupload(struct dvb_frontend *fe)
 		ret = xc_load_i2c_sequence(fe,  fw->data);
 		if (XC_RESULT_SUCCESS == ret)
 			ret = xc_set_xtal(fe);
-		printk(KERN_INFO "xc5000: firmware upload complete...\n");
+		if (XC_RESULT_SUCCESS == ret)
+			printk(KERN_INFO "xc5000: firmware upload complete...\n");
+		else
+			printk(KERN_ERR "xc5000: firmware upload failed...\n");
 	}
 
 out:
diff --git a/drivers/media/common/tuners/xc5000.h b/drivers/media/common/tuners/xc5000.h
index 39a73bf0140..b1a54749462 100644
--- a/drivers/media/common/tuners/xc5000.h
+++ b/drivers/media/common/tuners/xc5000.h
@@ -34,7 +34,7 @@ struct xc5000_config {
 	u8   i2c_address;
 	u32  if_khz;
 	u8   radio_input;
-	u32  xtal_khz;
+	u16  xtal_khz;
 
 	int chip_id;
 };
diff --git a/drivers/media/dvb/bt8xx/dst_ca.c b/drivers/media/dvb/bt8xx/dst_ca.c
index 48e48e8af55..66f52f116b6 100644
--- a/drivers/media/dvb/bt8xx/dst_ca.c
+++ b/drivers/media/dvb/bt8xx/dst_ca.c
@@ -477,7 +477,6 @@ static int dst_check_ca_pmt(struct dst_state *state, struct ca_msg *p_ca_message
 static int ca_send_message(struct dst_state *state, struct ca_msg *p_ca_message, void __user *arg)
 {
 	int i = 0;
-	unsigned int ca_message_header_len;
 
 	u32 command = 0;
 	struct ca_msg *hw_buffer;
@@ -496,7 +495,6 @@ static int ca_send_message(struct dst_state *state, struct ca_msg *p_ca_message,
 
 
 	if (p_ca_message->msg) {
-		ca_message_header_len = p_ca_message->length;	/*	Restore it back when you are done	*/
 		/*	EN50221 tag	*/
 		command = 0;
 
diff --git a/drivers/media/dvb/ddbridge/ddbridge-core.c b/drivers/media/dvb/ddbridge/ddbridge-core.c
index d88c4aa7d24..131b938e9e8 100644
--- a/drivers/media/dvb/ddbridge/ddbridge-core.c
+++ b/drivers/media/dvb/ddbridge/ddbridge-core.c
@@ -31,7 +31,6 @@
 #include <linux/pci.h>
 #include <linux/pci_ids.h>
 #include <linux/timer.h>
-#include <linux/version.h>
 #include <linux/i2c.h>
 #include <linux/swab.h>
 #include <linux/vmalloc.h>
@@ -1696,7 +1695,7 @@ static struct pci_driver ddb_pci_driver = {
 	.name        = "DDBridge",
 	.id_table    = ddb_id_tbl,
 	.probe       = ddb_probe,
-	.remove      = ddb_remove,
+	.remove      = __devexit_p(ddb_remove),
 };
 
 static __init int module_init_ddbridge(void)
diff --git a/drivers/media/dvb/dvb-core/dvb_demux.c b/drivers/media/dvb/dvb-core/dvb_demux.c
index faa3671b649..d82469f842e 100644
--- a/drivers/media/dvb/dvb-core/dvb_demux.c
+++ b/drivers/media/dvb/dvb-core/dvb_demux.c
@@ -568,6 +568,16 @@ void dvb_dmx_swfilter_204(struct dvb_demux *demux, const u8 *buf, size_t count)
 }
 EXPORT_SYMBOL(dvb_dmx_swfilter_204);
 
+void dvb_dmx_swfilter_raw(struct dvb_demux *demux, const u8 *buf, size_t count)
+{
+	spin_lock(&demux->lock);
+
+	demux->feed->cb.ts(buf, count, NULL, 0, &demux->feed->feed.ts, DMX_OK);
+
+	spin_unlock(&demux->lock);
+}
+EXPORT_SYMBOL(dvb_dmx_swfilter_raw);
+
 static struct dvb_demux_filter *dvb_dmx_filter_alloc(struct dvb_demux *demux)
 {
 	int i;
diff --git a/drivers/media/dvb/dvb-core/dvb_demux.h b/drivers/media/dvb/dvb-core/dvb_demux.h
index a7d876fd02d..fa7188a253a 100644
--- a/drivers/media/dvb/dvb-core/dvb_demux.h
+++ b/drivers/media/dvb/dvb-core/dvb_demux.h
@@ -145,5 +145,7 @@ void dvb_dmx_swfilter_packets(struct dvb_demux *dvbdmx, const u8 *buf,
 void dvb_dmx_swfilter(struct dvb_demux *demux, const u8 *buf, size_t count);
 void dvb_dmx_swfilter_204(struct dvb_demux *demux, const u8 *buf,
 			  size_t count);
+void dvb_dmx_swfilter_raw(struct dvb_demux *demux, const u8 *buf,
+			  size_t count);
 
 #endif /* _DVB_DEMUX_H_ */
diff --git a/drivers/media/dvb/dvb-core/dvb_frontend.c b/drivers/media/dvb/dvb-core/dvb_frontend.c
index 0f64d718265..aebcdf221dd 100644
--- a/drivers/media/dvb/dvb-core/dvb_frontend.c
+++ b/drivers/media/dvb/dvb-core/dvb_frontend.c
@@ -182,13 +182,13 @@ static enum dvbv3_emulation_type dvbv3_type(u32 delivery_system)
 	case SYS_DMBTH:
 		return DVBV3_OFDM;
 	case SYS_ATSC:
+	case SYS_ATSCMH:
 	case SYS_DVBC_ANNEX_B:
 		return DVBV3_ATSC;
 	case SYS_UNDEFINED:
 	case SYS_ISDBC:
 	case SYS_DVBH:
 	case SYS_DAB:
-	case SYS_ATSCMH:
 	default:
 		/*
 		 * Doesn't know how to emulate those types and/or
@@ -1030,6 +1030,25 @@ static struct dtv_cmds_h dtv_cmds[DTV_MAX_COMMAND + 1] = {
 	_DTV_CMD(DTV_HIERARCHY, 0, 0),
 
 	_DTV_CMD(DTV_ENUM_DELSYS, 0, 0),
+
+	_DTV_CMD(DTV_ATSCMH_PARADE_ID, 1, 0),
+	_DTV_CMD(DTV_ATSCMH_RS_FRAME_ENSEMBLE, 1, 0),
+
+	_DTV_CMD(DTV_ATSCMH_FIC_VER, 0, 0),
+	_DTV_CMD(DTV_ATSCMH_PARADE_ID, 0, 0),
+	_DTV_CMD(DTV_ATSCMH_NOG, 0, 0),
+	_DTV_CMD(DTV_ATSCMH_TNOG, 0, 0),
+	_DTV_CMD(DTV_ATSCMH_SGN, 0, 0),
+	_DTV_CMD(DTV_ATSCMH_PRC, 0, 0),
+	_DTV_CMD(DTV_ATSCMH_RS_FRAME_MODE, 0, 0),
+	_DTV_CMD(DTV_ATSCMH_RS_FRAME_ENSEMBLE, 0, 0),
+	_DTV_CMD(DTV_ATSCMH_RS_CODE_MODE_PRI, 0, 0),
+	_DTV_CMD(DTV_ATSCMH_RS_CODE_MODE_SEC, 0, 0),
+	_DTV_CMD(DTV_ATSCMH_SCCC_BLOCK_MODE, 0, 0),
+	_DTV_CMD(DTV_ATSCMH_SCCC_CODE_MODE_A, 0, 0),
+	_DTV_CMD(DTV_ATSCMH_SCCC_CODE_MODE_B, 0, 0),
+	_DTV_CMD(DTV_ATSCMH_SCCC_CODE_MODE_C, 0, 0),
+	_DTV_CMD(DTV_ATSCMH_SCCC_CODE_MODE_D, 0, 0),
 };
 
 static void dtv_property_dump(struct dtv_property *tvp)
@@ -1121,6 +1140,8 @@ static int dtv_property_cache_sync(struct dvb_frontend *fe,
 	case DVBV3_ATSC:
 		dprintk("%s() Preparing ATSC req\n", __func__);
 		c->modulation = p->u.vsb.modulation;
+		if (c->delivery_system == SYS_ATSCMH)
+			break;
 		if ((c->modulation == VSB_8) || (c->modulation == VSB_16))
 			c->delivery_system = SYS_ATSC;
 		else
@@ -1367,6 +1388,54 @@ static int dtv_property_process_get(struct dvb_frontend *fe,
 	case DTV_DVBT2_PLP_ID:
 		tvp->u.data = c->dvbt2_plp_id;
 		break;
+
+	/* ATSC-MH */
+	case DTV_ATSCMH_FIC_VER:
+		tvp->u.data = fe->dtv_property_cache.atscmh_fic_ver;
+		break;
+	case DTV_ATSCMH_PARADE_ID:
+		tvp->u.data = fe->dtv_property_cache.atscmh_parade_id;
+		break;
+	case DTV_ATSCMH_NOG:
+		tvp->u.data = fe->dtv_property_cache.atscmh_nog;
+		break;
+	case DTV_ATSCMH_TNOG:
+		tvp->u.data = fe->dtv_property_cache.atscmh_tnog;
+		break;
+	case DTV_ATSCMH_SGN:
+		tvp->u.data = fe->dtv_property_cache.atscmh_sgn;
+		break;
+	case DTV_ATSCMH_PRC:
+		tvp->u.data = fe->dtv_property_cache.atscmh_prc;
+		break;
+	case DTV_ATSCMH_RS_FRAME_MODE:
+		tvp->u.data = fe->dtv_property_cache.atscmh_rs_frame_mode;
+		break;
+	case DTV_ATSCMH_RS_FRAME_ENSEMBLE:
+		tvp->u.data = fe->dtv_property_cache.atscmh_rs_frame_ensemble;
+		break;
+	case DTV_ATSCMH_RS_CODE_MODE_PRI:
+		tvp->u.data = fe->dtv_property_cache.atscmh_rs_code_mode_pri;
+		break;
+	case DTV_ATSCMH_RS_CODE_MODE_SEC:
+		tvp->u.data = fe->dtv_property_cache.atscmh_rs_code_mode_sec;
+		break;
+	case DTV_ATSCMH_SCCC_BLOCK_MODE:
+		tvp->u.data = fe->dtv_property_cache.atscmh_sccc_block_mode;
+		break;
+	case DTV_ATSCMH_SCCC_CODE_MODE_A:
+		tvp->u.data = fe->dtv_property_cache.atscmh_sccc_code_mode_a;
+		break;
+	case DTV_ATSCMH_SCCC_CODE_MODE_B:
+		tvp->u.data = fe->dtv_property_cache.atscmh_sccc_code_mode_b;
+		break;
+	case DTV_ATSCMH_SCCC_CODE_MODE_C:
+		tvp->u.data = fe->dtv_property_cache.atscmh_sccc_code_mode_c;
+		break;
+	case DTV_ATSCMH_SCCC_CODE_MODE_D:
+		tvp->u.data = fe->dtv_property_cache.atscmh_sccc_code_mode_d;
+		break;
+
 	default:
 		return -EINVAL;
 	}
@@ -1708,6 +1777,15 @@ static int dtv_property_process_set(struct dvb_frontend *fe,
 	case DTV_DVBT2_PLP_ID:
 		c->dvbt2_plp_id = tvp->u.data;
 		break;
+
+	/* ATSC-MH */
+	case DTV_ATSCMH_PARADE_ID:
+		fe->dtv_property_cache.atscmh_parade_id = tvp->u.data;
+		break;
+	case DTV_ATSCMH_RS_FRAME_ENSEMBLE:
+		fe->dtv_property_cache.atscmh_rs_frame_ensemble = tvp->u.data;
+		break;
+
 	default:
 		return -EINVAL;
 	}
@@ -1921,6 +1999,10 @@ static int dtv_set_frontend(struct dvb_frontend *fe)
 	} else {
 		/* default values */
 		switch (c->delivery_system) {
+		case SYS_DVBS:
+		case SYS_DVBS2:
+		case SYS_ISDBS:
+		case SYS_TURBO:
 		case SYS_DVBC_ANNEX_A:
 		case SYS_DVBC_ANNEX_C:
 			fepriv->min_delay = HZ / 20;
diff --git a/drivers/media/dvb/dvb-core/dvb_frontend.h b/drivers/media/dvb/dvb-core/dvb_frontend.h
index d63a8215fe0..e929d5697b8 100644
--- a/drivers/media/dvb/dvb-core/dvb_frontend.h
+++ b/drivers/media/dvb/dvb-core/dvb_frontend.h
@@ -372,6 +372,24 @@ struct dtv_frontend_properties {
 
 	/* DVB-T2 specifics */
 	u32                     dvbt2_plp_id;
+
+	/* ATSC-MH specifics */
+	u8			atscmh_fic_ver;
+	u8			atscmh_parade_id;
+	u8			atscmh_nog;
+	u8			atscmh_tnog;
+	u8			atscmh_sgn;
+	u8			atscmh_prc;
+
+	u8			atscmh_rs_frame_mode;
+	u8			atscmh_rs_frame_ensemble;
+	u8			atscmh_rs_code_mode_pri;
+	u8			atscmh_rs_code_mode_sec;
+	u8			atscmh_sccc_block_mode;
+	u8			atscmh_sccc_code_mode_a;
+	u8			atscmh_sccc_code_mode_b;
+	u8			atscmh_sccc_code_mode_c;
+	u8			atscmh_sccc_code_mode_d;
 };
 
 struct dvb_frontend {
diff --git a/drivers/media/dvb/dvb-usb/Kconfig b/drivers/media/dvb/dvb-usb/Kconfig
index 63bf45679f9..a26949336b3 100644
--- a/drivers/media/dvb/dvb-usb/Kconfig
+++ b/drivers/media/dvb/dvb-usb/Kconfig
@@ -409,6 +409,7 @@ config DVB_USB_MXL111SF
 	tristate "MxL111SF DTV USB2.0 support"
 	depends on DVB_USB
 	select DVB_LGDT3305 if !DVB_FE_CUSTOMISE
+	select DVB_LG2160 if !DVB_FE_CUSTOMISE
 	select VIDEO_TVEEPROM
 	help
 	  Say Y here to support the MxL111SF USB2.0 DTV receiver.
@@ -422,3 +423,15 @@ config DVB_USB_RTL28XXU
 	select MEDIA_TUNER_MXL5005S if !MEDIA_TUNER_CUSTOMISE
 	help
 	  Say Y here to support the Realtek RTL28xxU DVB USB receiver.
+
+config DVB_USB_AF9035
+	tristate "Afatech AF9035 DVB-T USB2.0 support"
+	depends on DVB_USB
+	select DVB_AF9033
+	select MEDIA_TUNER_TUA9001 if !MEDIA_TUNER_CUSTOMISE
+	select MEDIA_TUNER_FC0011 if !MEDIA_TUNER_CUSTOMISE
+	select MEDIA_TUNER_MXL5007T if !MEDIA_TUNER_CUSTOMISE
+	select MEDIA_TUNER_TDA18218 if !MEDIA_TUNER_CUSTOMISE
+	help
+	  Say Y here to support the Afatech AF9035 based DVB USB receiver.
+
diff --git a/drivers/media/dvb/dvb-usb/Makefile b/drivers/media/dvb/dvb-usb/Makefile
index b76acb5387e..b667ac39a4e 100644
--- a/drivers/media/dvb/dvb-usb/Makefile
+++ b/drivers/media/dvb/dvb-usb/Makefile
@@ -110,6 +110,9 @@ obj-$(CONFIG_DVB_USB_MXL111SF) += mxl111sf-tuner.o
 dvb-usb-rtl28xxu-objs = rtl28xxu.o
 obj-$(CONFIG_DVB_USB_RTL28XXU) += dvb-usb-rtl28xxu.o
 
+dvb-usb-af9035-objs = af9035.o
+obj-$(CONFIG_DVB_USB_AF9035) += dvb-usb-af9035.o
+
 ccflags-y += -I$(srctree)/drivers/media/dvb/dvb-core
 ccflags-y += -I$(srctree)/drivers/media/dvb/frontends/
 # due to tuner-xc3028
diff --git a/drivers/media/dvb/dvb-usb/af9015.c b/drivers/media/dvb/dvb-usb/af9015.c
index 7e70ea50ef2..677fed79b01 100644
--- a/drivers/media/dvb/dvb-usb/af9015.c
+++ b/drivers/media/dvb/dvb-usb/af9015.c
@@ -244,8 +244,7 @@ static int af9015_i2c_xfer(struct i2c_adapter *adap, struct i2c_msg msg[],
 	u8 uninitialized_var(mbox), addr_len;
 	struct req_t req;
 
-/* TODO: implement bus lock
-
+/*
 The bus lock is needed because there is two tuners both using same I2C-address.
 Due to that the only way to select correct tuner is use demodulator I2C-gate.
 
@@ -789,7 +788,7 @@ static void af9015_set_remote_config(struct usb_device *udev,
 	/* try to load remote based USB ID */
 	if (!props->rc.core.rc_codes)
 		props->rc.core.rc_codes = af9015_rc_setup_match(
-			(vid << 16) + pid, af9015_rc_setup_usbids);
+			(vid << 16) | pid, af9015_rc_setup_usbids);
 
 	/* try to load remote based USB iManufacturer string */
 	if (!props->rc.core.rc_codes && vid == USB_VID_AFATECH) {
@@ -1220,8 +1219,8 @@ static int af9015_af9013_frontend_attach(struct dvb_usb_adapter *adap)
 	}
 
 	/* attach demodulator */
-	adap->fe_adap[0].fe = dvb_attach(af9013_attach, &af9015_af9013_config[adap->id],
-		&adap->dev->i2c_adap);
+	adap->fe_adap[0].fe = dvb_attach(af9013_attach,
+		&af9015_af9013_config[adap->id], &adap->dev->i2c_adap);
 
 	/*
 	 * AF9015 firmware does not like if it gets interrupted by I2C adapter
@@ -1324,14 +1323,15 @@ static int af9015_tuner_attach(struct dvb_usb_adapter *adap)
 	switch (af9015_af9013_config[adap->id].tuner) {
 	case AF9013_TUNER_MT2060:
 	case AF9013_TUNER_MT2060_2:
-		ret = dvb_attach(mt2060_attach, adap->fe_adap[0].fe, &adap->dev->i2c_adap,
-			&af9015_mt2060_config,
+		ret = dvb_attach(mt2060_attach, adap->fe_adap[0].fe,
+			&adap->dev->i2c_adap, &af9015_mt2060_config,
 			af9015_config.mt2060_if1[adap->id])
 			== NULL ? -ENODEV : 0;
 		break;
 	case AF9013_TUNER_QT1010:
 	case AF9013_TUNER_QT1010A:
-		ret = dvb_attach(qt1010_attach, adap->fe_adap[0].fe, &adap->dev->i2c_adap,
+		ret = dvb_attach(qt1010_attach, adap->fe_adap[0].fe,
+			&adap->dev->i2c_adap,
 			&af9015_qt1010_config) == NULL ? -ENODEV : 0;
 		break;
 	case AF9013_TUNER_TDA18271:
@@ -1434,69 +1434,85 @@ enum af9015_usb_table_entry {
 };
 
 static struct usb_device_id af9015_usb_table[] = {
-	[AFATECH_9015] =
-		{USB_DEVICE(USB_VID_AFATECH, USB_PID_AFATECH_AF9015_9015)},
-	[AFATECH_9016] =
-		{USB_DEVICE(USB_VID_AFATECH, USB_PID_AFATECH_AF9015_9016)},
-	[WINFAST_DTV_GOLD] =
-		{USB_DEVICE(USB_VID_LEADTEK, USB_PID_WINFAST_DTV_DONGLE_GOLD)},
-	[PINNACLE_PCTV_71E] =
-		{USB_DEVICE(USB_VID_PINNACLE, USB_PID_PINNACLE_PCTV71E)},
-	[KWORLD_PLUSTV_399U] =
-		{USB_DEVICE(USB_VID_KWORLD_2, USB_PID_KWORLD_399U)},
-	[TINYTWIN] = {USB_DEVICE(USB_VID_VISIONPLUS, USB_PID_TINYTWIN)},
-	[AZUREWAVE_TU700] =
-		{USB_DEVICE(USB_VID_VISIONPLUS, USB_PID_AZUREWAVE_AD_TU700)},
-	[TERRATEC_AF9015] = {USB_DEVICE(USB_VID_TERRATEC,
+	[AFATECH_9015] = {
+		USB_DEVICE(USB_VID_AFATECH, USB_PID_AFATECH_AF9015_9015)},
+	[AFATECH_9016] = {
+		USB_DEVICE(USB_VID_AFATECH, USB_PID_AFATECH_AF9015_9016)},
+	[WINFAST_DTV_GOLD] = {
+		USB_DEVICE(USB_VID_LEADTEK, USB_PID_WINFAST_DTV_DONGLE_GOLD)},
+	[PINNACLE_PCTV_71E] = {
+		USB_DEVICE(USB_VID_PINNACLE, USB_PID_PINNACLE_PCTV71E)},
+	[KWORLD_PLUSTV_399U] = {
+		USB_DEVICE(USB_VID_KWORLD_2, USB_PID_KWORLD_399U)},
+	[TINYTWIN] = {
+		USB_DEVICE(USB_VID_VISIONPLUS, USB_PID_TINYTWIN)},
+	[AZUREWAVE_TU700] = {
+		USB_DEVICE(USB_VID_VISIONPLUS, USB_PID_AZUREWAVE_AD_TU700)},
+	[TERRATEC_AF9015] = {
+		USB_DEVICE(USB_VID_TERRATEC,
 				USB_PID_TERRATEC_CINERGY_T_USB_XE_REV2)},
-	[KWORLD_PLUSTV_PC160] =
-		{USB_DEVICE(USB_VID_KWORLD_2, USB_PID_KWORLD_PC160_2T)},
-	[AVERTV_VOLAR_X] =
-		{USB_DEVICE(USB_VID_AVERMEDIA, USB_PID_AVERMEDIA_VOLAR_X)},
-	[XTENSIONS_380U] =
-		{USB_DEVICE(USB_VID_XTENSIONS, USB_PID_XTENSIONS_XD_380)},
-	[MSI_DIGIVOX_DUO] =
-		{USB_DEVICE(USB_VID_MSI_2, USB_PID_MSI_DIGIVOX_DUO)},
-	[AVERTV_VOLAR_X_REV2] =
-		{USB_DEVICE(USB_VID_AVERMEDIA, USB_PID_AVERMEDIA_VOLAR_X_2)},
-	[TELESTAR_STARSTICK_2] =
-		{USB_DEVICE(USB_VID_TELESTAR,  USB_PID_TELESTAR_STARSTICK_2)},
-	[AVERMEDIA_A309_USB] =
-		{USB_DEVICE(USB_VID_AVERMEDIA, USB_PID_AVERMEDIA_A309)},
-	[MSI_DIGIVOX_MINI_III] =
-		{USB_DEVICE(USB_VID_MSI_2, USB_PID_MSI_DIGI_VOX_MINI_III)},
-	[KWORLD_E396] = {USB_DEVICE(USB_VID_KWORLD_2, USB_PID_KWORLD_395U)},
-	[KWORLD_E39B] = {USB_DEVICE(USB_VID_KWORLD_2, USB_PID_KWORLD_395U_2)},
-	[KWORLD_E395] = {USB_DEVICE(USB_VID_KWORLD_2, USB_PID_KWORLD_395U_3)},
-	[TREKSTOR_DVBT] = {USB_DEVICE(USB_VID_AFATECH, USB_PID_TREKSTOR_DVBT)},
-	[AVERTV_A850] = {USB_DEVICE(USB_VID_AVERMEDIA, USB_PID_AVERMEDIA_A850)},
-	[AVERTV_A805] = {USB_DEVICE(USB_VID_AVERMEDIA, USB_PID_AVERMEDIA_A805)},
-	[CONCEPTRONIC_CTVDIGRCU] =
-		{USB_DEVICE(USB_VID_KWORLD_2, USB_PID_CONCEPTRONIC_CTVDIGRCU)},
-	[KWORLD_MC810] = {USB_DEVICE(USB_VID_KWORLD_2, USB_PID_KWORLD_MC810)},
-	[GENIUS_TVGO_DVB_T03] =
-		{USB_DEVICE(USB_VID_KYE, USB_PID_GENIUS_TVGO_DVB_T03)},
-	[KWORLD_399U_2] = {USB_DEVICE(USB_VID_KWORLD_2, USB_PID_KWORLD_399U_2)},
-	[KWORLD_PC160_T] =
-		{USB_DEVICE(USB_VID_KWORLD_2, USB_PID_KWORLD_PC160_T)},
-	[SVEON_STV20] = {USB_DEVICE(USB_VID_KWORLD_2, USB_PID_SVEON_STV20)},
-	[TINYTWIN_2] = {USB_DEVICE(USB_VID_KWORLD_2, USB_PID_TINYTWIN_2)},
-	[WINFAST_DTV2000DS] =
-		{USB_DEVICE(USB_VID_LEADTEK, USB_PID_WINFAST_DTV2000DS)},
-	[KWORLD_UB383_T] =
-		{USB_DEVICE(USB_VID_KWORLD_2, USB_PID_KWORLD_UB383_T)},
-	[KWORLD_E39A] =
-		{USB_DEVICE(USB_VID_KWORLD_2, USB_PID_KWORLD_395U_4)},
-	[AVERMEDIA_A815M] =
-		{USB_DEVICE(USB_VID_AVERMEDIA, USB_PID_AVERMEDIA_A815M)},
-	[CINERGY_T_STICK_RC] = {USB_DEVICE(USB_VID_TERRATEC,
+	[KWORLD_PLUSTV_PC160] = {
+		USB_DEVICE(USB_VID_KWORLD_2, USB_PID_KWORLD_PC160_2T)},
+	[AVERTV_VOLAR_X] = {
+		USB_DEVICE(USB_VID_AVERMEDIA, USB_PID_AVERMEDIA_VOLAR_X)},
+	[XTENSIONS_380U] = {
+		USB_DEVICE(USB_VID_XTENSIONS, USB_PID_XTENSIONS_XD_380)},
+	[MSI_DIGIVOX_DUO] = {
+		USB_DEVICE(USB_VID_MSI_2, USB_PID_MSI_DIGIVOX_DUO)},
+	[AVERTV_VOLAR_X_REV2] = {
+		USB_DEVICE(USB_VID_AVERMEDIA, USB_PID_AVERMEDIA_VOLAR_X_2)},
+	[TELESTAR_STARSTICK_2] = {
+		USB_DEVICE(USB_VID_TELESTAR,  USB_PID_TELESTAR_STARSTICK_2)},
+	[AVERMEDIA_A309_USB] = {
+		USB_DEVICE(USB_VID_AVERMEDIA, USB_PID_AVERMEDIA_A309)},
+	[MSI_DIGIVOX_MINI_III] = {
+		USB_DEVICE(USB_VID_MSI_2, USB_PID_MSI_DIGI_VOX_MINI_III)},
+	[KWORLD_E396] = {
+		USB_DEVICE(USB_VID_KWORLD_2, USB_PID_KWORLD_395U)},
+	[KWORLD_E39B] = {
+		USB_DEVICE(USB_VID_KWORLD_2, USB_PID_KWORLD_395U_2)},
+	[KWORLD_E395] = {
+		USB_DEVICE(USB_VID_KWORLD_2, USB_PID_KWORLD_395U_3)},
+	[TREKSTOR_DVBT] = {
+		USB_DEVICE(USB_VID_AFATECH, USB_PID_TREKSTOR_DVBT)},
+	[AVERTV_A850] = {
+		USB_DEVICE(USB_VID_AVERMEDIA, USB_PID_AVERMEDIA_A850)},
+	[AVERTV_A805] = {
+		USB_DEVICE(USB_VID_AVERMEDIA, USB_PID_AVERMEDIA_A805)},
+	[CONCEPTRONIC_CTVDIGRCU] = {
+		USB_DEVICE(USB_VID_KWORLD_2, USB_PID_CONCEPTRONIC_CTVDIGRCU)},
+	[KWORLD_MC810] = {
+		USB_DEVICE(USB_VID_KWORLD_2, USB_PID_KWORLD_MC810)},
+	[GENIUS_TVGO_DVB_T03] = {
+		USB_DEVICE(USB_VID_KYE, USB_PID_GENIUS_TVGO_DVB_T03)},
+	[KWORLD_399U_2] = {
+		USB_DEVICE(USB_VID_KWORLD_2, USB_PID_KWORLD_399U_2)},
+	[KWORLD_PC160_T] = {
+		USB_DEVICE(USB_VID_KWORLD_2, USB_PID_KWORLD_PC160_T)},
+	[SVEON_STV20] = {
+		USB_DEVICE(USB_VID_KWORLD_2, USB_PID_SVEON_STV20)},
+	[TINYTWIN_2] = {
+		USB_DEVICE(USB_VID_KWORLD_2, USB_PID_TINYTWIN_2)},
+	[WINFAST_DTV2000DS] = {
+		USB_DEVICE(USB_VID_LEADTEK, USB_PID_WINFAST_DTV2000DS)},
+	[KWORLD_UB383_T] = {
+		USB_DEVICE(USB_VID_KWORLD_2, USB_PID_KWORLD_UB383_T)},
+	[KWORLD_E39A] = {
+		USB_DEVICE(USB_VID_KWORLD_2, USB_PID_KWORLD_395U_4)},
+	[AVERMEDIA_A815M] = {
+		USB_DEVICE(USB_VID_AVERMEDIA, USB_PID_AVERMEDIA_A815M)},
+	[CINERGY_T_STICK_RC] = {
+		USB_DEVICE(USB_VID_TERRATEC,
 				USB_PID_TERRATEC_CINERGY_T_STICK_RC)},
-	[CINERGY_T_DUAL_RC] = {USB_DEVICE(USB_VID_TERRATEC,
+	[CINERGY_T_DUAL_RC] = {
+		USB_DEVICE(USB_VID_TERRATEC,
 				USB_PID_TERRATEC_CINERGY_T_STICK_DUAL_RC)},
-	[AVERTV_A850T] =
-		{USB_DEVICE(USB_VID_AVERMEDIA, USB_PID_AVERMEDIA_A850T)},
-	[TINYTWIN_3] = {USB_DEVICE(USB_VID_GTEK, USB_PID_TINYTWIN_3)},
-	[SVEON_STV22] = {USB_DEVICE(USB_VID_KWORLD_2, USB_PID_SVEON_STV22)},
+	[AVERTV_A850T] = {
+		USB_DEVICE(USB_VID_AVERMEDIA, USB_PID_AVERMEDIA_A850T)},
+	[TINYTWIN_3] = {
+		USB_DEVICE(USB_VID_GTEK, USB_PID_TINYTWIN_3)},
+	[SVEON_STV22] = {
+		USB_DEVICE(USB_VID_KWORLD_2, USB_PID_SVEON_STV22)},
 	{ }
 };
 MODULE_DEVICE_TABLE(usb, af9015_usb_table);
@@ -1516,43 +1532,44 @@ static struct dvb_usb_device_properties af9015_properties[] = {
 		.num_adapters = 2,
 		.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       = af9015_pid_filter,
-				.pid_filter_ctrl  = af9015_pid_filter_ctrl,
-
-				.frontend_attach =
-					af9015_af9013_frontend_attach,
-				.tuner_attach    = af9015_tuner_attach,
-				.stream = {
-					.type = USB_BULK,
-					.count = 6,
-					.endpoint = 0x84,
+				.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       = af9015_pid_filter,
+						.pid_filter_ctrl  = af9015_pid_filter_ctrl,
+
+						.frontend_attach = af9015_af9013_frontend_attach,
+						.tuner_attach    = af9015_tuner_attach,
+						.stream = {
+							.type = USB_BULK,
+							.count = 6,
+							.endpoint = 0x84,
+						},
+					}
 				},
-			}},
 			},
 			{
-			.num_frontends = 1,
-			.fe = {{
-				.frontend_attach =
-					af9015_af9013_frontend_attach,
-				.tuner_attach    = af9015_tuner_attach,
-				.stream = {
-					.type = USB_BULK,
-					.count = 6,
-					.endpoint = 0x85,
-					.u = {
-						.bulk = {
-							.buffersize =
-						TS_USB20_FRAME_SIZE,
-						}
+				.num_frontends = 1,
+				.fe = {
+					{
+						.frontend_attach = af9015_af9013_frontend_attach,
+						.tuner_attach    = af9015_tuner_attach,
+						.stream = {
+							.type = USB_BULK,
+							.count = 6,
+							.endpoint = 0x85,
+							.u = {
+								.bulk = {
+									.buffersize = TS_USB20_FRAME_SIZE,
+								}
+							}
+						},
 					}
 				},
-			}},
 			}
 		},
 
@@ -1575,102 +1592,67 @@ static struct dvb_usb_device_properties af9015_properties[] = {
 				.cold_ids = {
 					&af9015_usb_table[AFATECH_9015],
 					&af9015_usb_table[AFATECH_9016],
-					NULL
 				},
-				.warm_ids = {NULL},
-			},
-			{
+			}, {
 				.name = "Leadtek WinFast DTV Dongle Gold",
 				.cold_ids = {
 					&af9015_usb_table[WINFAST_DTV_GOLD],
-					NULL
 				},
-				.warm_ids = {NULL},
-			},
-			{
+			}, {
 				.name = "Pinnacle PCTV 71e",
 				.cold_ids = {
 					&af9015_usb_table[PINNACLE_PCTV_71E],
-					NULL
 				},
-				.warm_ids = {NULL},
-			},
-			{
+			}, {
 				.name = "KWorld PlusTV Dual DVB-T Stick " \
 					"(DVB-T 399U)",
 				.cold_ids = {
 					&af9015_usb_table[KWORLD_PLUSTV_399U],
 					&af9015_usb_table[KWORLD_399U_2],
-					NULL
 				},
-				.warm_ids = {NULL},
-			},
-			{
+			}, {
 				.name = "DigitalNow TinyTwin DVB-T Receiver",
 				.cold_ids = {
 					&af9015_usb_table[TINYTWIN],
 					&af9015_usb_table[TINYTWIN_2],
 					&af9015_usb_table[TINYTWIN_3],
-					NULL
 				},
-				.warm_ids = {NULL},
-			},
-			{
+			}, {
 				.name = "TwinHan AzureWave AD-TU700(704J)",
 				.cold_ids = {
 					&af9015_usb_table[AZUREWAVE_TU700],
-					NULL
 				},
-				.warm_ids = {NULL},
-			},
-			{
+			}, {
 				.name = "TerraTec Cinergy T USB XE",
 				.cold_ids = {
 					&af9015_usb_table[TERRATEC_AF9015],
-					NULL
 				},
-				.warm_ids = {NULL},
-			},
-			{
+			}, {
 				.name = "KWorld PlusTV Dual DVB-T PCI " \
 					"(DVB-T PC160-2T)",
 				.cold_ids = {
 					&af9015_usb_table[KWORLD_PLUSTV_PC160],
-					NULL
 				},
-				.warm_ids = {NULL},
-			},
-			{
+			}, {
 				.name = "AVerMedia AVerTV DVB-T Volar X",
 				.cold_ids = {
 					&af9015_usb_table[AVERTV_VOLAR_X],
-					NULL
 				},
-				.warm_ids = {NULL},
-			},
-			{
+			}, {
 				.name = "TerraTec Cinergy T Stick RC",
 				.cold_ids = {
 					&af9015_usb_table[CINERGY_T_STICK_RC],
-					NULL
 				},
-				.warm_ids = {NULL},
-			},
-			{
+			}, {
 				.name = "TerraTec Cinergy T Stick Dual RC",
 				.cold_ids = {
 					&af9015_usb_table[CINERGY_T_DUAL_RC],
-					NULL
 				},
-				.warm_ids = {NULL},
-			},
-			{
+			}, {
 				.name = "AverMedia AVerTV Red HD+ (A850T)",
 				.cold_ids = {
 					&af9015_usb_table[AVERTV_A850T],
-					NULL
 				},
-				.warm_ids = {NULL},
 			},
 		}
 	}, {
@@ -1686,43 +1668,44 @@ static struct dvb_usb_device_properties af9015_properties[] = {
 		.num_adapters = 2,
 		.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       = af9015_pid_filter,
-				.pid_filter_ctrl  = af9015_pid_filter_ctrl,
-
-				.frontend_attach =
-					af9015_af9013_frontend_attach,
-				.tuner_attach    = af9015_tuner_attach,
-				.stream = {
-					.type = USB_BULK,
-					.count = 6,
-					.endpoint = 0x84,
+				.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       = af9015_pid_filter,
+						.pid_filter_ctrl  = af9015_pid_filter_ctrl,
+
+						.frontend_attach = af9015_af9013_frontend_attach,
+						.tuner_attach    = af9015_tuner_attach,
+						.stream = {
+							.type = USB_BULK,
+							.count = 6,
+							.endpoint = 0x84,
+						},
+					}
 				},
-			}},
 			},
 			{
-			.num_frontends = 1,
-			.fe = {{
-				.frontend_attach =
-					af9015_af9013_frontend_attach,
-				.tuner_attach    = af9015_tuner_attach,
-				.stream = {
-					.type = USB_BULK,
-					.count = 6,
-					.endpoint = 0x85,
-					.u = {
-						.bulk = {
-							.buffersize =
-						TS_USB20_FRAME_SIZE,
-						}
+				.num_frontends = 1,
+				.fe = {
+					{
+						.frontend_attach = af9015_af9013_frontend_attach,
+						.tuner_attach    = af9015_tuner_attach,
+						.stream = {
+							.type = USB_BULK,
+							.count = 6,
+							.endpoint = 0x85,
+							.u = {
+								.bulk = {
+									.buffersize = TS_USB20_FRAME_SIZE,
+								}
+							}
+						},
 					}
 				},
-			}},
 			}
 		},
 
@@ -1744,51 +1727,33 @@ static struct dvb_usb_device_properties af9015_properties[] = {
 				.name = "Xtensions XD-380",
 				.cold_ids = {
 					&af9015_usb_table[XTENSIONS_380U],
-					NULL
 				},
-				.warm_ids = {NULL},
-			},
-			{
+			}, {
 				.name = "MSI DIGIVOX Duo",
 				.cold_ids = {
 					&af9015_usb_table[MSI_DIGIVOX_DUO],
-					NULL
 				},
-				.warm_ids = {NULL},
-			},
-			{
+			}, {
 				.name = "Fujitsu-Siemens Slim Mobile USB DVB-T",
 				.cold_ids = {
 					&af9015_usb_table[AVERTV_VOLAR_X_REV2],
-					NULL
 				},
-				.warm_ids = {NULL},
-			},
-			{
+			}, {
 				.name = "Telestar Starstick 2",
 				.cold_ids = {
 					&af9015_usb_table[TELESTAR_STARSTICK_2],
-					NULL
 				},
-				.warm_ids = {NULL},
-			},
-			{
+			}, {
 				.name = "AVerMedia A309",
 				.cold_ids = {
 					&af9015_usb_table[AVERMEDIA_A309_USB],
-					NULL
 				},
-				.warm_ids = {NULL},
-			},
-			{
+			}, {
 				.name = "MSI Digi VOX mini III",
 				.cold_ids = {
 					&af9015_usb_table[MSI_DIGIVOX_MINI_III],
-					NULL
 				},
-				.warm_ids = {NULL},
-			},
-			{
+			}, {
 				.name = "KWorld USB DVB-T TV Stick II " \
 					"(VS-DVB-T 395U)",
 				.cold_ids = {
@@ -1796,34 +1761,23 @@ static struct dvb_usb_device_properties af9015_properties[] = {
 					&af9015_usb_table[KWORLD_E39B],
 					&af9015_usb_table[KWORLD_E395],
 					&af9015_usb_table[KWORLD_E39A],
-					NULL
 				},
-				.warm_ids = {NULL},
-			},
-			{
+			}, {
 				.name = "TrekStor DVB-T USB Stick",
 				.cold_ids = {
 					&af9015_usb_table[TREKSTOR_DVBT],
-					NULL
 				},
-				.warm_ids = {NULL},
-			},
-			{
+			}, {
 				.name = "AverMedia AVerTV Volar Black HD " \
 					"(A850)",
 				.cold_ids = {
 					&af9015_usb_table[AVERTV_A850],
-					NULL
 				},
-				.warm_ids = {NULL},
-			},
-			{
+			}, {
 				.name = "Sveon STV22 Dual USB DVB-T Tuner HDTV",
 				.cold_ids = {
 					&af9015_usb_table[SVEON_STV22],
-					NULL
 				},
-				.warm_ids = {NULL},
 			},
 		}
 	}, {
@@ -1839,43 +1793,44 @@ static struct dvb_usb_device_properties af9015_properties[] = {
 		.num_adapters = 2,
 		.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       = af9015_pid_filter,
-				.pid_filter_ctrl  = af9015_pid_filter_ctrl,
-
-				.frontend_attach =
-					af9015_af9013_frontend_attach,
-				.tuner_attach    = af9015_tuner_attach,
-				.stream = {
-					.type = USB_BULK,
-					.count = 6,
-					.endpoint = 0x84,
+				.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       = af9015_pid_filter,
+						.pid_filter_ctrl  = af9015_pid_filter_ctrl,
+
+						.frontend_attach = af9015_af9013_frontend_attach,
+						.tuner_attach    = af9015_tuner_attach,
+						.stream = {
+							.type = USB_BULK,
+							.count = 6,
+							.endpoint = 0x84,
+						},
+					}
 				},
-			}},
 			},
 			{
-			.num_frontends = 1,
-			.fe = {{
-				.frontend_attach =
-					af9015_af9013_frontend_attach,
-				.tuner_attach    = af9015_tuner_attach,
-				.stream = {
-					.type = USB_BULK,
-					.count = 6,
-					.endpoint = 0x85,
-					.u = {
-						.bulk = {
-							.buffersize =
-						TS_USB20_FRAME_SIZE,
-						}
+				.num_frontends = 1,
+				.fe = {
+					{
+						.frontend_attach = af9015_af9013_frontend_attach,
+						.tuner_attach    = af9015_tuner_attach,
+						.stream = {
+							.type = USB_BULK,
+							.count = 6,
+							.endpoint = 0x85,
+							.u = {
+								.bulk = {
+									.buffersize = TS_USB20_FRAME_SIZE,
+								}
+							}
+						},
 					}
 				},
-			}},
 			}
 		},
 
@@ -1897,76 +1852,50 @@ static struct dvb_usb_device_properties af9015_properties[] = {
 				.name = "AverMedia AVerTV Volar GPS 805 (A805)",
 				.cold_ids = {
 					&af9015_usb_table[AVERTV_A805],
-					NULL
 				},
-				.warm_ids = {NULL},
-			},
-			{
+			}, {
 				.name = "Conceptronic USB2.0 DVB-T CTVDIGRCU " \
 					"V3.0",
 				.cold_ids = {
 					&af9015_usb_table[CONCEPTRONIC_CTVDIGRCU],
-					NULL
 				},
-				.warm_ids = {NULL},
-			},
-			{
+			}, {
 				.name = "KWorld Digial MC-810",
 				.cold_ids = {
 					&af9015_usb_table[KWORLD_MC810],
-					NULL
 				},
-				.warm_ids = {NULL},
-			},
-			{
+			}, {
 				.name = "Genius TVGo DVB-T03",
 				.cold_ids = {
 					&af9015_usb_table[GENIUS_TVGO_DVB_T03],
-					NULL
 				},
-				.warm_ids = {NULL},
-			},
-			{
+			}, {
 				.name = "KWorld PlusTV DVB-T PCI Pro Card " \
 					"(DVB-T PC160-T)",
 				.cold_ids = {
 					&af9015_usb_table[KWORLD_PC160_T],
-					NULL
 				},
-				.warm_ids = {NULL},
-			},
-			{
+			}, {
 				.name = "Sveon STV20 Tuner USB DVB-T HDTV",
 				.cold_ids = {
 					&af9015_usb_table[SVEON_STV20],
-					NULL
 				},
-				.warm_ids = {NULL},
-			},
-			{
+			}, {
 				.name = "Leadtek WinFast DTV2000DS",
 				.cold_ids = {
 					&af9015_usb_table[WINFAST_DTV2000DS],
-					NULL
 				},
-				.warm_ids = {NULL},
-			},
-			{
+			}, {
 				.name = "KWorld USB DVB-T Stick Mobile " \
 					"(UB383-T)",
 				.cold_ids = {
 					&af9015_usb_table[KWORLD_UB383_T],
-					NULL
 				},
-				.warm_ids = {NULL},
-			},
-			{
+			}, {
 				.name = "AverMedia AVerTV Volar M (A815Mac)",
 				.cold_ids = {
 					&af9015_usb_table[AVERMEDIA_A815M],
-					NULL
 				},
-				.warm_ids = {NULL},
 			},
 		}
 	},
@@ -2019,5 +1948,5 @@ static struct usb_driver af9015_usb_driver = {
 module_usb_driver(af9015_usb_driver);
 
 MODULE_AUTHOR("Antti Palosaari <crope@iki.fi>");
-MODULE_DESCRIPTION("Driver for Afatech AF9015 DVB-T");
+MODULE_DESCRIPTION("Afatech AF9015 driver");
 MODULE_LICENSE("GPL");
diff --git a/drivers/media/dvb/dvb-usb/af9035.c b/drivers/media/dvb/dvb-usb/af9035.c
new file mode 100644
index 00000000000..e83b39d3993
--- /dev/null
+++ b/drivers/media/dvb/dvb-usb/af9035.c
@@ -0,0 +1,1242 @@
+/*
+ * Afatech AF9035 DVB USB driver
+ *
+ * Copyright (C) 2009 Antti Palosaari <crope@iki.fi>
+ * 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.
+ */
+
+#include "af9035.h"
+
+DVB_DEFINE_MOD_OPT_ADAPTER_NR(adapter_nr);
+static DEFINE_MUTEX(af9035_usb_mutex);
+static struct dvb_usb_device_properties af9035_properties[2];
+static int af9035_properties_count = ARRAY_SIZE(af9035_properties);
+
+static u16 af9035_checksum(const u8 *buf, size_t len)
+{
+	size_t i;
+	u16 checksum = 0;
+
+	for (i = 1; i < len; i++) {
+		if (i % 2)
+			checksum += buf[i] << 8;
+		else
+			checksum += buf[i];
+	}
+	checksum = ~checksum;
+
+	return checksum;
+}
+
+static int af9035_ctrl_msg(struct usb_device *udev, 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
+
+	int ret, msg_len, act_len;
+	u8 buf[BUF_LEN];
+	static u8 seq; /* packet sequence number */
+	u16 checksum, tmp_checksum;
+
+	/* buffer overflow check */
+	if (req->wlen > (BUF_LEN - REQ_HDR_LEN - CHECKSUM_LEN) ||
+		req->rlen > (BUF_LEN - ACK_HDR_LEN - CHECKSUM_LEN)) {
+		pr_debug("%s: too much data wlen=%d rlen=%d\n", __func__,
+				req->wlen, req->rlen);
+		return -EINVAL;
+	}
+
+	if (mutex_lock_interruptible(&af9035_usb_mutex) < 0)
+		return -EAGAIN;
+
+	buf[0] = REQ_HDR_LEN + req->wlen + CHECKSUM_LEN - 1;
+	buf[1] = req->mbox;
+	buf[2] = req->cmd;
+	buf[3] = seq++;
+	if (req->wlen)
+		memcpy(&buf[4], req->wbuf, req->wlen);
+
+	/* calc and add checksum */
+	checksum = af9035_checksum(buf, buf[0] - 1);
+	buf[buf[0] - 1] = (checksum >> 8);
+	buf[buf[0] - 0] = (checksum & 0xff);
+
+	msg_len = REQ_HDR_LEN + req->wlen + CHECKSUM_LEN ;
+
+	/* send req */
+	ret = usb_bulk_msg(udev, usb_sndbulkpipe(udev, 0x02), buf, msg_len,
+			&act_len, USB_TIMEOUT);
+	if (ret < 0)
+		err("bulk message failed=%d (%d/%d)", ret, msg_len, act_len);
+	else
+		if (act_len != msg_len)
+			ret = -EIO; /* all data is not send */
+	if (ret < 0)
+		goto err_mutex_unlock;
+
+	/* no ack for those packets */
+	if (req->cmd == CMD_FW_DL)
+		goto exit_mutex_unlock;
+
+	/* receive ack and data if read req */
+	msg_len = ACK_HDR_LEN + req->rlen + CHECKSUM_LEN;
+	ret = usb_bulk_msg(udev, usb_rcvbulkpipe(udev, 0x81), buf, msg_len,
+			&act_len, USB_TIMEOUT);
+	if (ret < 0) {
+		err("recv bulk message failed=%d", ret);
+		ret = -EIO;
+		goto err_mutex_unlock;
+	}
+
+	if (act_len != msg_len) {
+		err("recv bulk message truncated (%d != %d)", act_len, msg_len);
+		ret = -EIO;
+		goto err_mutex_unlock;
+	}
+
+	/* verify checksum */
+	checksum = af9035_checksum(buf, act_len - 2);
+	tmp_checksum = (buf[act_len - 2] << 8) | buf[act_len - 1];
+	if (tmp_checksum != checksum) {
+		err("%s: command=%02x checksum mismatch (%04x != %04x)",
+		    __func__, req->cmd, tmp_checksum, checksum);
+		ret = -EIO;
+		goto err_mutex_unlock;
+	}
+
+	/* check status */
+	if (buf[2]) {
+		pr_debug("%s: command=%02x failed fw error=%d\n", __func__,
+				req->cmd, buf[2]);
+		ret = -EIO;
+		goto err_mutex_unlock;
+	}
+
+	/* read request, copy returned data to return buf */
+	if (req->rlen)
+		memcpy(req->rbuf, &buf[ACK_HDR_LEN], req->rlen);
+
+err_mutex_unlock:
+exit_mutex_unlock:
+	mutex_unlock(&af9035_usb_mutex);
+
+	return ret;
+}
+
+/* write multiple registers */
+static int af9035_wr_regs(struct dvb_usb_device *d, u32 reg, u8 *val, int len)
+{
+	u8 wbuf[6 + len];
+	u8 mbox = (reg >> 16) & 0xff;
+	struct usb_req req = { CMD_MEM_WR, mbox, sizeof(wbuf), wbuf, 0, NULL };
+
+	wbuf[0] = len;
+	wbuf[1] = 2;
+	wbuf[2] = 0;
+	wbuf[3] = 0;
+	wbuf[4] = (reg >> 8) & 0xff;
+	wbuf[5] = (reg >> 0) & 0xff;
+	memcpy(&wbuf[6], val, len);
+
+	return af9035_ctrl_msg(d->udev, &req);
+}
+
+/* read multiple registers */
+static int af9035_rd_regs(struct dvb_usb_device *d, u32 reg, u8 *val, int len)
+{
+	u8 wbuf[] = { len, 2, 0, 0, (reg >> 8) & 0xff, reg & 0xff };
+	u8 mbox = (reg >> 16) & 0xff;
+	struct usb_req req = { CMD_MEM_RD, mbox, sizeof(wbuf), wbuf, len, val };
+
+	return af9035_ctrl_msg(d->udev, &req);
+}
+
+/* write single register */
+static int af9035_wr_reg(struct dvb_usb_device *d, u32 reg, u8 val)
+{
+	return af9035_wr_regs(d, reg, &val, 1);
+}
+
+/* read single register */
+static int af9035_rd_reg(struct dvb_usb_device *d, u32 reg, u8 *val)
+{
+	return af9035_rd_regs(d, reg, val, 1);
+}
+
+/* write single register with mask */
+static int af9035_wr_reg_mask(struct dvb_usb_device *d, u32 reg, u8 val,
+		u8 mask)
+{
+	int ret;
+	u8 tmp;
+
+	/* no need for read if whole reg is written */
+	if (mask != 0xff) {
+		ret = af9035_rd_regs(d, reg, &tmp, 1);
+		if (ret)
+			return ret;
+
+		val &= mask;
+		tmp &= ~mask;
+		val |= tmp;
+	}
+
+	return af9035_wr_regs(d, reg, &val, 1);
+}
+
+static int af9035_i2c_master_xfer(struct i2c_adapter *adap,
+		struct i2c_msg msg[], int num)
+{
+	struct dvb_usb_device *d = i2c_get_adapdata(adap);
+	struct state *state = d->priv;
+	int ret;
+
+	if (mutex_lock_interruptible(&d->i2c_mutex) < 0)
+		return -EAGAIN;
+
+	/*
+	 * I2C sub header is 5 bytes long. Meaning of those bytes are:
+	 * 0: data len
+	 * 1: I2C addr << 1
+	 * 2: reg addr len
+	 *    byte 3 and 4 can be used as reg addr
+	 * 3: reg addr MSB
+	 *    used when reg addr len is set to 2
+	 * 4: reg addr LSB
+	 *    used when reg addr len is set to 1 or 2
+	 *
+	 * For the simplify we do not use register addr at all.
+	 * NOTE: As a firmware knows tuner type there is very small possibility
+	 * there could be some tuner I2C hacks done by firmware and this may
+	 * lead problems if firmware expects those bytes are used.
+	 */
+	if (num == 2 && !(msg[0].flags & I2C_M_RD) &&
+			(msg[1].flags & I2C_M_RD)) {
+		if (msg[0].len > 40 || msg[1].len > 40) {
+			/* TODO: correct limits > 40 */
+			ret = -EOPNOTSUPP;
+		} else if (msg[0].addr == state->af9033_config[0].i2c_addr) {
+			/* integrated demod */
+			u32 reg = msg[0].buf[0] << 16 | msg[0].buf[1] << 8 |
+					msg[0].buf[2];
+			ret = af9035_rd_regs(d, reg, &msg[1].buf[0],
+					msg[1].len);
+		} else {
+			/* I2C */
+			u8 buf[5 + msg[0].len];
+			struct usb_req req = { CMD_I2C_RD, 0, sizeof(buf),
+					buf, msg[1].len, msg[1].buf };
+			buf[0] = msg[1].len;
+			buf[1] = msg[0].addr << 1;
+			buf[2] = 0x00; /* reg addr len */
+			buf[3] = 0x00; /* reg addr MSB */
+			buf[4] = 0x00; /* reg addr LSB */
+			memcpy(&buf[5], msg[0].buf, msg[0].len);
+			ret = af9035_ctrl_msg(d->udev, &req);
+		}
+	} else if (num == 1 && !(msg[0].flags & I2C_M_RD)) {
+		if (msg[0].len > 40) {
+			/* TODO: correct limits > 40 */
+			ret = -EOPNOTSUPP;
+		} else if (msg[0].addr == state->af9033_config[0].i2c_addr) {
+			/* integrated demod */
+			u32 reg = msg[0].buf[0] << 16 | msg[0].buf[1] << 8 |
+					msg[0].buf[2];
+			ret = af9035_wr_regs(d, reg, &msg[0].buf[3],
+					msg[0].len - 3);
+		} else {
+			/* I2C */
+			u8 buf[5 + msg[0].len];
+			struct usb_req req = { CMD_I2C_WR, 0, sizeof(buf), buf,
+					0, NULL };
+			buf[0] = msg[0].len;
+			buf[1] = msg[0].addr << 1;
+			buf[2] = 0x00; /* reg addr len */
+			buf[3] = 0x00; /* reg addr MSB */
+			buf[4] = 0x00; /* reg addr LSB */
+			memcpy(&buf[5], msg[0].buf, msg[0].len);
+			ret = af9035_ctrl_msg(d->udev, &req);
+		}
+	} else {
+		/*
+		 * We support only two kind of I2C transactions:
+		 * 1) 1 x read + 1 x write
+		 * 2) 1 x write
+		 */
+		ret = -EOPNOTSUPP;
+	}
+
+	mutex_unlock(&d->i2c_mutex);
+
+	if (ret < 0)
+		return ret;
+	else
+		return num;
+}
+
+static u32 af9035_i2c_functionality(struct i2c_adapter *adapter)
+{
+	return I2C_FUNC_I2C;
+}
+
+static struct i2c_algorithm af9035_i2c_algo = {
+	.master_xfer = af9035_i2c_master_xfer,
+	.functionality = af9035_i2c_functionality,
+};
+
+#define AF9035_POLL 250
+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 };
+
+	ret = af9035_ctrl_msg(d->udev, &req);
+	if (ret < 0)
+		goto err;
+
+	if ((b[2] + b[3]) == 0xff) {
+		if ((b[0] + b[1]) == 0xff) {
+			/* NEC */
+			key = b[0] << 8 | b[2];
+		} else {
+			/* ext. NEC */
+			key = b[0] << 16 | b[1] << 8 | b[2];
+		}
+	} else {
+		key = b[0] << 24 | b[1] << 16 | b[2] << 8 | b[3];
+	}
+
+	rc_keydown(d->rc_dev, key, 0);
+
+err:
+	/* ignore errors */
+	return 0;
+}
+
+static int af9035_init(struct dvb_usb_device *d)
+{
+	struct state *state = d->priv;
+	int ret, i;
+	u16 frame_size = 87 * 188 / 4;
+	u8  packet_size = 512 / 4;
+	struct reg_val_mask tab[] = {
+		{ 0x80f99d, 0x01, 0x01 },
+		{ 0x80f9a4, 0x01, 0x01 },
+		{ 0x00dd11, 0x00, 0x20 },
+		{ 0x00dd11, 0x00, 0x40 },
+		{ 0x00dd13, 0x00, 0x20 },
+		{ 0x00dd13, 0x00, 0x40 },
+		{ 0x00dd11, 0x20, 0x20 },
+		{ 0x00dd88, (frame_size >> 0) & 0xff, 0xff},
+		{ 0x00dd89, (frame_size >> 8) & 0xff, 0xff},
+		{ 0x00dd0c, packet_size, 0xff},
+		{ 0x00dd11, state->dual_mode << 6, 0x40 },
+		{ 0x00dd8a, (frame_size >> 0) & 0xff, 0xff},
+		{ 0x00dd8b, (frame_size >> 8) & 0xff, 0xff},
+		{ 0x00dd0d, packet_size, 0xff },
+		{ 0x80f9a3, 0x00, 0x01 },
+		{ 0x80f9cd, 0x00, 0x01 },
+		{ 0x80f99d, 0x00, 0x01 },
+		{ 0x80f9a4, 0x00, 0x01 },
+	};
+
+	pr_debug("%s: USB speed=%d frame_size=%04x packet_size=%02x\n",
+		__func__, d->udev->speed, frame_size, packet_size);
+
+	/* init endpoints */
+	for (i = 0; i < ARRAY_SIZE(tab); i++) {
+		ret = af9035_wr_reg_mask(d, tab[i].reg, tab[i].val,
+				tab[i].mask);
+		if (ret < 0)
+			goto err;
+	}
+
+	return 0;
+
+err:
+	pr_debug("%s: failed=%d\n", __func__, ret);
+
+	return ret;
+}
+
+static int af9035_identify_state(struct usb_device *udev,
+		struct dvb_usb_device_properties *props,
+		struct dvb_usb_device_description **desc,
+		int *cold)
+{
+	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_ctrl_msg(udev, &req);
+	if (ret < 0)
+		goto err;
+
+	pr_debug("%s: reply=%02x %02x %02x %02x\n", __func__,
+		rbuf[0], rbuf[1], rbuf[2], rbuf[3]);
+	if (rbuf[0] || rbuf[1] || rbuf[2] || rbuf[3])
+		*cold = 0;
+	else
+		*cold = 1;
+
+	return 0;
+
+err:
+	pr_debug("%s: failed=%d\n", __func__, ret);
+
+	return ret;
+}
+
+static int af9035_download_firmware(struct usb_device *udev,
+		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;
+	u16 hdr_addr, hdr_data_len, hdr_checksum;
+	#define MAX_DATA 58
+	#define HDR_SIZE 7
+
+	/*
+	 * Thanks to Daniel Glöckner <daniel-gl@gmx.net> about that info!
+	 *
+	 * byte 0: MCS 51 core
+	 *  There are two inside the AF9035 (1=Link and 2=OFDM) with separate
+	 *  address spaces
+	 * byte 1-2: Big endian destination address
+	 * byte 3-4: Big endian number of data bytes following the header
+	 * byte 5-6: Big endian header checksum, apparently ignored by the chip
+	 *  Calculated as ~(h[0]*256+h[1]+h[2]*256+h[3]+h[4]*256)
+	 */
+
+	for (i = fw->size; i > HDR_SIZE;) {
+		hdr_core = fw->data[fw->size - i + 0];
+		hdr_addr = fw->data[fw->size - i + 1] << 8;
+		hdr_addr |= fw->data[fw->size - i + 2] << 0;
+		hdr_data_len = fw->data[fw->size - i + 3] << 8;
+		hdr_data_len |= fw->data[fw->size - i + 4] << 0;
+		hdr_checksum = fw->data[fw->size - i + 5] << 8;
+		hdr_checksum |= fw->data[fw->size - i + 6] << 0;
+
+		pr_debug("%s: core=%d addr=%04x data_len=%d checksum=%04x\n",
+				__func__, hdr_core, hdr_addr, hdr_data_len,
+				hdr_checksum);
+
+		if (((hdr_core != 1) && (hdr_core != 2)) ||
+				(hdr_data_len > i)) {
+			pr_debug("%s: bad firmware\n", __func__);
+			break;
+		}
+
+		/* download begin packet */
+		req.cmd = CMD_FW_DL_BEGIN;
+		ret = af9035_ctrl_msg(udev, &req);
+		if (ret < 0)
+			goto err;
+
+		/* download firmware packet(s) */
+		for (j = HDR_SIZE + hdr_data_len; j > 0; j -= MAX_DATA) {
+			len = j;
+			if (len > MAX_DATA)
+				len = MAX_DATA;
+			req_fw_dl.wlen = len;
+			req_fw_dl.wbuf = (u8 *) &fw->data[fw->size - i +
+					HDR_SIZE + hdr_data_len - j];
+			ret = af9035_ctrl_msg(udev, &req_fw_dl);
+			if (ret < 0)
+				goto err;
+		}
+
+		/* download end packet */
+		req.cmd = CMD_FW_DL_END;
+		ret = af9035_ctrl_msg(udev, &req);
+		if (ret < 0)
+			goto err;
+
+		i -= hdr_data_len + HDR_SIZE;
+
+		pr_debug("%s: data uploaded=%zu\n", __func__, fw->size - i);
+	}
+
+	/* firmware loaded, request boot */
+	req.cmd = CMD_FW_BOOT;
+	ret = af9035_ctrl_msg(udev, &req);
+	if (ret < 0)
+		goto err;
+
+	/* ensure firmware starts */
+	wbuf[0] = 1;
+	ret = af9035_ctrl_msg(udev, &req_fw_ver);
+	if (ret < 0)
+		goto err;
+
+	if (!(rbuf[0] || rbuf[1] || rbuf[2] || rbuf[3])) {
+		info("firmware did not run");
+		ret = -ENODEV;
+		goto err;
+	}
+
+	info("firmware version=%d.%d.%d.%d", rbuf[0], rbuf[1], rbuf[2],
+			rbuf[3]);
+
+	return 0;
+
+err:
+	pr_debug("%s: failed=%d\n", __func__, ret);
+
+	return ret;
+}
+
+static int af9035_download_firmware_it9135(struct usb_device *udev,
+		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
+
+	/*
+	 * There seems to be following firmware header. Meaning of bytes 0-3
+	 * is unknown.
+	 *
+	 * 0: 3
+	 * 1: 0, 1
+	 * 2: 0
+	 * 3: 1, 2, 3
+	 * 4: addr MSB
+	 * 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 &&
+				(fw->data[i + 1] == 0x00 ||
+				fw->data[i + 1] == 0x01) &&
+				fw->data[i + 2] == 0x00)) {
+			req_fw_dl.wlen = i - i_prev;
+			req_fw_dl.wbuf = (u8 *) &fw->data[i_prev];
+			i_prev = i;
+			ret = af9035_ctrl_msg(udev, &req_fw_dl);
+			if (ret < 0)
+				goto err;
+
+			pr_debug("%s: data uploaded=%d\n", __func__, i);
+		}
+	}
+
+	/* firmware loaded, request boot */
+	req.cmd = CMD_FW_BOOT;
+	ret = af9035_ctrl_msg(udev, &req);
+	if (ret < 0)
+		goto err;
+
+	/* ensure firmware starts */
+	wbuf[0] = 1;
+	ret = af9035_ctrl_msg(udev, &req_fw_ver);
+	if (ret < 0)
+		goto err;
+
+	if (!(rbuf[0] || rbuf[1] || rbuf[2] || rbuf[3])) {
+		info("firmware did not run");
+		ret = -ENODEV;
+		goto err;
+	}
+
+	info("firmware version=%d.%d.%d.%d", rbuf[0], rbuf[1], rbuf[2],
+			rbuf[3]);
+
+	return 0;
+
+err:
+	pr_debug("%s: failed=%d\n", __func__, ret);
+
+	return ret;
+}
+
+/* abuse that callback as there is no better one for reading eeprom */
+static int af9035_read_mac_address(struct dvb_usb_device *d, u8 mac[6])
+{
+	struct state *state = d->priv;
+	int ret, i, eeprom_shift = 0;
+	u8 tmp;
+	u16 tmp16;
+
+	/* check if there is dual tuners */
+	ret = af9035_rd_reg(d, EEPROM_DUAL_MODE, &tmp);
+	if (ret < 0)
+		goto err;
+
+	state->dual_mode = tmp;
+	pr_debug("%s: dual mode=%d\n", __func__, state->dual_mode);
+
+	for (i = 0; i < af9035_properties[0].num_adapters; i++) {
+		/* tuner */
+		ret = af9035_rd_reg(d, EEPROM_1_TUNER_ID + eeprom_shift, &tmp);
+		if (ret < 0)
+			goto err;
+
+		state->af9033_config[i].tuner = tmp;
+		pr_debug("%s: [%d]tuner=%02x\n", __func__, i, tmp);
+
+		switch (tmp) {
+		case AF9033_TUNER_TUA9001:
+		case AF9033_TUNER_FC0011:
+		case AF9033_TUNER_MXL5007T:
+		case AF9033_TUNER_TDA18218:
+			state->af9033_config[i].spec_inv = 1;
+			break;
+		default:
+			warn("tuner ID=%02x not supported, please report!",
+				tmp);
+		};
+
+		/* tuner IF frequency */
+		ret = af9035_rd_reg(d, EEPROM_1_IFFREQ_L + eeprom_shift, &tmp);
+		if (ret < 0)
+			goto err;
+
+		tmp16 = tmp;
+
+		ret = af9035_rd_reg(d, EEPROM_1_IFFREQ_H + eeprom_shift, &tmp);
+		if (ret < 0)
+			goto err;
+
+		tmp16 |= tmp << 8;
+
+		pr_debug("%s: [%d]IF=%d\n", __func__, i, tmp16);
+
+		eeprom_shift = 0x10; /* shift for the 2nd tuner params */
+	}
+
+	/* get demod clock */
+	ret = af9035_rd_reg(d, 0x00d800, &tmp);
+	if (ret < 0)
+		goto err;
+
+	tmp = (tmp >> 0) & 0x0f;
+
+	for (i = 0; i < af9035_properties[0].num_adapters; i++)
+		state->af9033_config[i].clock = clock_lut[tmp];
+
+	ret = af9035_rd_reg(d, EEPROM_IR_MODE, &tmp);
+	if (ret < 0)
+		goto err;
+	pr_debug("%s: ir_mode=%02x\n", __func__, tmp);
+
+	/* don't activate rc if in HID mode or if not available */
+	if (tmp == 5) {
+		ret = af9035_rd_reg(d, EEPROM_IR_TYPE, &tmp);
+		if (ret < 0)
+			goto err;
+		pr_debug("%s: ir_type=%02x\n", __func__, tmp);
+
+		switch (tmp) {
+		case 0: /* NEC */
+		default:
+			d->props.rc.core.protocol = RC_TYPE_NEC;
+			d->props.rc.core.allowed_protos = RC_TYPE_NEC;
+			break;
+		case 1: /* RC6 */
+			d->props.rc.core.protocol = RC_TYPE_RC6;
+			d->props.rc.core.allowed_protos = RC_TYPE_RC6;
+			break;
+		}
+		d->props.rc.core.rc_query = af9035_rc_query;
+	}
+
+	return 0;
+
+err:
+	pr_debug("%s: failed=%d\n", __func__, ret);
+
+	return ret;
+}
+
+/* abuse that callback as there is no better one for reading eeprom */
+static int af9035_read_mac_address_it9135(struct dvb_usb_device *d, u8 mac[6])
+{
+	struct state *state = d->priv;
+	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 < af9035_properties[0].num_adapters; i++)
+		state->af9033_config[i].clock = clock_lut_it9135[tmp];
+
+	return 0;
+
+err:
+	pr_debug("%s: failed=%d\n", __func__, ret);
+
+	return ret;
+}
+
+static int af9035_fc0011_tuner_callback(struct dvb_usb_device *d,
+		int cmd, int arg)
+{
+	int ret;
+
+	switch (cmd) {
+	case FC0011_FE_CALLBACK_POWER:
+		/* Tuner enable */
+		ret = af9035_wr_reg_mask(d, 0xd8eb, 1, 1);
+		if (ret < 0)
+			goto err;
+
+		ret = af9035_wr_reg_mask(d, 0xd8ec, 1, 1);
+		if (ret < 0)
+			goto err;
+
+		ret = af9035_wr_reg_mask(d, 0xd8ed, 1, 1);
+		if (ret < 0)
+			goto err;
+
+		/* LED */
+		ret = af9035_wr_reg_mask(d, 0xd8d0, 1, 1);
+		if (ret < 0)
+			goto err;
+
+		ret = af9035_wr_reg_mask(d, 0xd8d1, 1, 1);
+		if (ret < 0)
+			goto err;
+
+		usleep_range(10000, 50000);
+		break;
+	case FC0011_FE_CALLBACK_RESET:
+		ret = af9035_wr_reg(d, 0xd8e9, 1);
+		if (ret < 0)
+			goto err;
+
+		ret = af9035_wr_reg(d, 0xd8e8, 1);
+		if (ret < 0)
+			goto err;
+
+		ret = af9035_wr_reg(d, 0xd8e7, 1);
+		if (ret < 0)
+			goto err;
+
+		usleep_range(10000, 20000);
+
+		ret = af9035_wr_reg(d, 0xd8e7, 0);
+		if (ret < 0)
+			goto err;
+
+		usleep_range(10000, 20000);
+		break;
+	default:
+		ret = -EINVAL;
+		goto err;
+	}
+
+	return 0;
+
+err:
+	pr_debug("%s: failed=%d\n", __func__, ret);
+
+	return ret;
+}
+
+static int af9035_tuner_callback(struct dvb_usb_device *d, int cmd, int arg)
+{
+	struct state *state = d->priv;
+
+	switch (state->af9033_config[0].tuner) {
+	case AF9033_TUNER_FC0011:
+		return af9035_fc0011_tuner_callback(d, cmd, arg);
+	default:
+		break;
+	}
+
+	return -ENODEV;
+}
+
+static int af9035_frontend_callback(void *adapter_priv, int component,
+				    int cmd, int arg)
+{
+	struct i2c_adapter *adap = adapter_priv;
+	struct dvb_usb_device *d = i2c_get_adapdata(adap);
+
+	switch (component) {
+	case DVB_FRONTEND_COMPONENT_TUNER:
+		return af9035_tuner_callback(d, cmd, arg);
+	default:
+		break;
+	}
+
+	return -EINVAL;
+}
+
+static int af9035_frontend_attach(struct dvb_usb_adapter *adap)
+{
+	struct state *state = adap->dev->priv;
+	int ret;
+
+	if (!state->af9033_config[adap->id].tuner) {
+		/* unsupported tuner */
+		ret = -ENODEV;
+		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(adap->dev, 0x00417f,
+				state->af9033_config[1].i2c_addr);
+		if (ret < 0)
+			goto err;
+
+		ret = af9035_wr_reg(adap->dev, 0x00d81a,
+				state->dual_mode);
+		if (ret < 0)
+			goto err;
+	}
+
+	/* attach demodulator */
+	adap->fe_adap[0].fe = dvb_attach(af9033_attach,
+			&state->af9033_config[adap->id], &adap->dev->i2c_adap);
+	if (adap->fe_adap[0].fe == NULL) {
+		ret = -ENODEV;
+		goto err;
+	}
+
+	/* disable I2C-gate */
+	adap->fe_adap[0].fe->ops.i2c_gate_ctrl = NULL;
+	adap->fe_adap[0].fe->callback = af9035_frontend_callback;
+
+	return 0;
+
+err:
+	pr_debug("%s: failed=%d\n", __func__, ret);
+
+	return ret;
+}
+
+static struct tua9001_config af9035_tua9001_config = {
+	.i2c_addr = 0x60,
+};
+
+static const struct fc0011_config af9035_fc0011_config = {
+	.i2c_address = 0x60,
+};
+
+static struct mxl5007t_config af9035_mxl5007t_config = {
+	.xtal_freq_hz = MxL_XTAL_24_MHZ,
+	.if_freq_hz = MxL_IF_4_57_MHZ,
+	.invert_if = 0,
+	.loop_thru_enable = 0,
+	.clk_out_enable = 0,
+	.clk_out_amp = MxL_CLKOUT_AMP_0_94V,
+};
+
+static struct tda18218_config af9035_tda18218_config = {
+	.i2c_address = 0x60,
+	.i2c_wr_max = 21,
+};
+
+static int af9035_tuner_attach(struct dvb_usb_adapter *adap)
+{
+	struct state *state = adap->dev->priv;
+	int ret;
+	struct dvb_frontend *fe;
+
+	switch (state->af9033_config[adap->id].tuner) {
+	case AF9033_TUNER_TUA9001:
+		/* AF9035 gpiot3 = TUA9001 RESETN
+		   AF9035 gpiot2 = TUA9001 RXEN */
+
+		/* configure gpiot2 and gpiot2 as output */
+		ret = af9035_wr_reg_mask(adap->dev, 0x00d8ec, 0x01, 0x01);
+		if (ret < 0)
+			goto err;
+
+		ret = af9035_wr_reg_mask(adap->dev, 0x00d8ed, 0x01, 0x01);
+		if (ret < 0)
+			goto err;
+
+		ret = af9035_wr_reg_mask(adap->dev, 0x00d8e8, 0x01, 0x01);
+		if (ret < 0)
+			goto err;
+
+		ret = af9035_wr_reg_mask(adap->dev, 0x00d8e9, 0x01, 0x01);
+		if (ret < 0)
+			goto err;
+
+		/* reset tuner */
+		ret = af9035_wr_reg_mask(adap->dev, 0x00d8e7, 0x00, 0x01);
+		if (ret < 0)
+			goto err;
+
+		usleep_range(2000, 20000);
+
+		ret = af9035_wr_reg_mask(adap->dev, 0x00d8e7, 0x01, 0x01);
+		if (ret < 0)
+			goto err;
+
+		/* activate tuner RX */
+		/* TODO: use callback for TUA9001 RXEN */
+		ret = af9035_wr_reg_mask(adap->dev, 0x00d8eb, 0x01, 0x01);
+		if (ret < 0)
+			goto err;
+
+		/* attach tuner */
+		fe = dvb_attach(tua9001_attach, adap->fe_adap[0].fe,
+				&adap->dev->i2c_adap, &af9035_tua9001_config);
+		break;
+	case AF9033_TUNER_FC0011:
+		fe = dvb_attach(fc0011_attach, adap->fe_adap[0].fe,
+				&adap->dev->i2c_adap, &af9035_fc0011_config);
+		break;
+	case AF9033_TUNER_MXL5007T:
+		ret = af9035_wr_reg(adap->dev, 0x00d8e0, 1);
+		if (ret < 0)
+			goto err;
+		ret = af9035_wr_reg(adap->dev, 0x00d8e1, 1);
+		if (ret < 0)
+			goto err;
+		ret = af9035_wr_reg(adap->dev, 0x00d8df, 0);
+		if (ret < 0)
+			goto err;
+
+		msleep(30);
+
+		ret = af9035_wr_reg(adap->dev, 0x00d8df, 1);
+		if (ret < 0)
+			goto err;
+
+		msleep(300);
+
+		ret = af9035_wr_reg(adap->dev, 0x00d8c0, 1);
+		if (ret < 0)
+			goto err;
+		ret = af9035_wr_reg(adap->dev, 0x00d8c1, 1);
+		if (ret < 0)
+			goto err;
+		ret = af9035_wr_reg(adap->dev, 0x00d8bf, 0);
+		if (ret < 0)
+			goto err;
+		ret = af9035_wr_reg(adap->dev, 0x00d8b4, 1);
+		if (ret < 0)
+			goto err;
+		ret = af9035_wr_reg(adap->dev, 0x00d8b5, 1);
+		if (ret < 0)
+			goto err;
+		ret = af9035_wr_reg(adap->dev, 0x00d8b3, 1);
+		if (ret < 0)
+			goto err;
+
+		/* attach tuner */
+		fe = dvb_attach(mxl5007t_attach, adap->fe_adap[0].fe,
+				&adap->dev->i2c_adap, 0x60, &af9035_mxl5007t_config);
+		break;
+	case AF9033_TUNER_TDA18218:
+		/* attach tuner */
+		fe = dvb_attach(tda18218_attach, adap->fe_adap[0].fe,
+				&adap->dev->i2c_adap, &af9035_tda18218_config);
+		break;
+	default:
+		fe = NULL;
+	}
+
+	if (fe == NULL) {
+		ret = -ENODEV;
+		goto err;
+	}
+
+	return 0;
+
+err:
+	pr_debug("%s: failed=%d\n", __func__, ret);
+
+	return ret;
+}
+
+enum af9035_id_entry {
+	AF9035_15A4_9035,
+	AF9035_15A4_1000,
+	AF9035_15A4_1001,
+	AF9035_15A4_1002,
+	AF9035_15A4_1003,
+	AF9035_0CCD_0093,
+	AF9035_07CA_A835,
+	AF9035_07CA_B835,
+	AF9035_07CA_1867,
+	AF9035_07CA_A867,
+	AF9035_07CA_0825,
+};
+
+static struct usb_device_id af9035_id[] = {
+	[AF9035_15A4_9035] = {
+		USB_DEVICE(USB_VID_AFATECH, USB_PID_AFATECH_AF9035_9035)},
+	[AF9035_15A4_1000] = {
+		USB_DEVICE(USB_VID_AFATECH, USB_PID_AFATECH_AF9035_1000)},
+	[AF9035_15A4_1001] = {
+		USB_DEVICE(USB_VID_AFATECH, USB_PID_AFATECH_AF9035_1001)},
+	[AF9035_15A4_1002] = {
+		USB_DEVICE(USB_VID_AFATECH, USB_PID_AFATECH_AF9035_1002)},
+	[AF9035_15A4_1003] = {
+		USB_DEVICE(USB_VID_AFATECH, USB_PID_AFATECH_AF9035_1003)},
+	[AF9035_0CCD_0093] = {
+		USB_DEVICE(USB_VID_TERRATEC, USB_PID_TERRATEC_CINERGY_T_STICK)},
+	[AF9035_07CA_A835] = {
+		USB_DEVICE(USB_VID_AVERMEDIA, USB_PID_AVERMEDIA_A835)},
+	[AF9035_07CA_B835] = {
+		USB_DEVICE(USB_VID_AVERMEDIA, USB_PID_AVERMEDIA_B835)},
+	[AF9035_07CA_1867] = {
+		USB_DEVICE(USB_VID_AVERMEDIA, USB_PID_AVERMEDIA_1867)},
+	[AF9035_07CA_A867] = {
+		USB_DEVICE(USB_VID_AVERMEDIA, USB_PID_AVERMEDIA_A867)},
+	[AF9035_07CA_0825] = {
+		USB_DEVICE(USB_VID_AVERMEDIA, USB_PID_AVERMEDIA_TWINSTAR)},
+	{},
+};
+
+MODULE_DEVICE_TABLE(usb, af9035_id);
+
+static struct dvb_usb_device_properties af9035_properties[] = {
+	{
+		.caps = DVB_USB_IS_AN_I2C_ADAPTER,
+
+		.usb_ctrl = DEVICE_SPECIFIC,
+		.download_firmware = af9035_download_firmware,
+		.firmware = "dvb-usb-af9035-02.fw",
+		.no_reconnect = 1,
+
+		.size_of_priv = sizeof(struct state),
+
+		.num_adapters = 1,
+		.adapter = {
+			{
+				.num_frontends = 1,
+				.fe = {
+					{
+						.frontend_attach = af9035_frontend_attach,
+						.tuner_attach = af9035_tuner_attach,
+						.stream = {
+							.type = USB_BULK,
+							.count = 6,
+							.endpoint = 0x84,
+							.u = {
+								.bulk = {
+									.buffersize = (87 * 188),
+								}
+							}
+						}
+					}
+				}
+			}
+		},
+
+		.identify_state = af9035_identify_state,
+		.read_mac_address = af9035_read_mac_address,
+
+		.i2c_algo = &af9035_i2c_algo,
+
+		.rc.core = {
+			.protocol       = RC_TYPE_UNKNOWN,
+			.module_name    = "af9035",
+			.rc_query       = NULL,
+			.rc_interval    = AF9035_POLL,
+			.allowed_protos = RC_TYPE_UNKNOWN,
+			.rc_codes       = RC_MAP_EMPTY,
+		},
+		.num_device_descs = 5,
+		.devices = {
+			{
+				.name = "Afatech AF9035 reference design",
+				.cold_ids = {
+					&af9035_id[AF9035_15A4_9035],
+					&af9035_id[AF9035_15A4_1000],
+					&af9035_id[AF9035_15A4_1001],
+					&af9035_id[AF9035_15A4_1002],
+					&af9035_id[AF9035_15A4_1003],
+				},
+			}, {
+				.name = "TerraTec Cinergy T Stick",
+				.cold_ids = {
+					&af9035_id[AF9035_0CCD_0093],
+				},
+			}, {
+				.name = "AVerMedia AVerTV Volar HD/PRO (A835)",
+				.cold_ids = {
+					&af9035_id[AF9035_07CA_A835],
+					&af9035_id[AF9035_07CA_B835],
+				},
+			}, {
+				.name = "AVerMedia HD Volar (A867)",
+				.cold_ids = {
+					&af9035_id[AF9035_07CA_1867],
+					&af9035_id[AF9035_07CA_A867],
+				},
+			}, {
+				.name = "AVerMedia Twinstar (A825)",
+				.cold_ids = {
+					&af9035_id[AF9035_07CA_0825],
+				},
+			},
+		}
+	},
+	{
+		.caps = DVB_USB_IS_AN_I2C_ADAPTER,
+
+		.usb_ctrl = DEVICE_SPECIFIC,
+		.download_firmware = af9035_download_firmware_it9135,
+		.firmware = "dvb-usb-it9135-01.fw",
+		.no_reconnect = 1,
+
+		.size_of_priv = sizeof(struct state),
+
+		.num_adapters = 1,
+		.adapter = {
+			{
+				.num_frontends = 1,
+				.fe = {
+					{
+						.frontend_attach = af9035_frontend_attach,
+						.tuner_attach = af9035_tuner_attach,
+						.stream = {
+							.type = USB_BULK,
+							.count = 6,
+							.endpoint = 0x84,
+							.u = {
+								.bulk = {
+									.buffersize = (87 * 188),
+								}
+							}
+						}
+					}
+				}
+			}
+		},
+
+		.identify_state = af9035_identify_state,
+		.read_mac_address = af9035_read_mac_address_it9135,
+
+		.i2c_algo = &af9035_i2c_algo,
+
+		.num_device_descs = 0, /* disabled as no support for IT9135 */
+		.devices = {
+			{
+				.name = "ITE Tech. IT9135 reference design",
+			},
+		}
+	},
+};
+
+static int af9035_usb_probe(struct usb_interface *intf,
+			    const struct usb_device_id *id)
+{
+	int ret, i;
+	struct dvb_usb_device *d = NULL;
+	struct usb_device *udev;
+	bool found;
+
+	pr_debug("%s: interface=%d\n", __func__,
+			intf->cur_altsetting->desc.bInterfaceNumber);
+
+	/* interface 0 is used by DVB-T receiver and
+	   interface 1 is for remote controller (HID) */
+	if (intf->cur_altsetting->desc.bInterfaceNumber != 0)
+		return 0;
+
+	/* Dynamic USB ID support. Replaces first device ID with current one. */
+	udev = interface_to_usbdev(intf);
+
+	for (i = 0, found = false; i < ARRAY_SIZE(af9035_id) - 1; i++) {
+		if (af9035_id[i].idVendor ==
+				le16_to_cpu(udev->descriptor.idVendor) &&
+				af9035_id[i].idProduct ==
+				le16_to_cpu(udev->descriptor.idProduct)) {
+			found = true;
+			break;
+		}
+	}
+
+	if (!found) {
+		pr_debug("%s: using dynamic ID %04x:%04x\n", __func__,
+				le16_to_cpu(udev->descriptor.idVendor),
+				le16_to_cpu(udev->descriptor.idProduct));
+		af9035_properties[0].devices[0].cold_ids[0]->idVendor =
+				le16_to_cpu(udev->descriptor.idVendor);
+		af9035_properties[0].devices[0].cold_ids[0]->idProduct =
+				le16_to_cpu(udev->descriptor.idProduct);
+	}
+
+
+	for (i = 0; i < af9035_properties_count; i++) {
+		ret = dvb_usb_device_init(intf, &af9035_properties[i],
+				THIS_MODULE, &d, adapter_nr);
+
+		if (ret == -ENODEV)
+			continue;
+		else
+			break;
+	}
+
+	if (ret < 0)
+		goto err;
+
+	if (d) {
+		ret = af9035_init(d);
+		if (ret < 0)
+			goto err;
+	}
+
+	return 0;
+
+err:
+	pr_debug("%s: failed=%d\n", __func__, ret);
+
+	return ret;
+}
+
+/* usb specific object needed to register this driver with the usb subsystem */
+static struct usb_driver af9035_usb_driver = {
+	.name = "dvb_usb_af9035",
+	.probe = af9035_usb_probe,
+	.disconnect = dvb_usb_device_exit,
+	.id_table = af9035_id,
+};
+
+module_usb_driver(af9035_usb_driver);
+
+MODULE_AUTHOR("Antti Palosaari <crope@iki.fi>");
+MODULE_DESCRIPTION("Afatech AF9035 driver");
+MODULE_LICENSE("GPL");
diff --git a/drivers/media/dvb/dvb-usb/af9035.h b/drivers/media/dvb/dvb-usb/af9035.h
new file mode 100644
index 00000000000..481a1a43dd2
--- /dev/null
+++ b/drivers/media/dvb/dvb-usb/af9035.h
@@ -0,0 +1,113 @@
+/*
+ * Afatech AF9035 DVB USB driver
+ *
+ * Copyright (C) 2009 Antti Palosaari <crope@iki.fi>
+ * 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 AF9035_H
+#define AF9035_H
+
+/* prefix for dvb-usb log writings */
+#define DVB_USB_LOG_PREFIX "af9035"
+
+#include "dvb-usb.h"
+#include "af9033.h"
+#include "tua9001.h"
+#include "fc0011.h"
+#include "mxl5007t.h"
+#include "tda18218.h"
+
+struct reg_val {
+	u32 reg;
+	u8  val;
+};
+
+struct reg_val_mask {
+	u32 reg;
+	u8  val;
+	u8  mask;
+};
+
+struct usb_req {
+	u8  cmd;
+	u8  mbox;
+	u8  wlen;
+	u8  *wbuf;
+	u8  rlen;
+	u8  *rbuf;
+};
+
+struct state {
+	bool dual_mode;
+
+	struct af9033_config af9033_config[2];
+};
+
+u32 clock_lut[] = {
+	20480000, /*      FPGA */
+	16384000, /* 16.38 MHz */
+	20480000, /* 20.48 MHz */
+	36000000, /* 36.00 MHz */
+	30000000, /* 30.00 MHz */
+	26000000, /* 26.00 MHz */
+	28000000, /* 28.00 MHz */
+	32000000, /* 32.00 MHz */
+	34000000, /* 34.00 MHz */
+	24000000, /* 24.00 MHz */
+	22000000, /* 22.00 MHz */
+	12000000, /* 12.00 MHz */
+};
+
+u32 clock_lut_it9135[] = {
+	12000000, /* 12.00 MHz */
+	20480000, /* 20.48 MHz */
+	36000000, /* 36.00 MHz */
+	30000000, /* 30.00 MHz */
+	26000000, /* 26.00 MHz */
+	28000000, /* 28.00 MHz */
+	32000000, /* 32.00 MHz */
+	34000000, /* 34.00 MHz */
+	24000000, /* 24.00 MHz */
+	22000000, /* 22.00 MHz */
+};
+
+/* EEPROM locations */
+#define EEPROM_IR_MODE            0x430d
+#define EEPROM_DUAL_MODE          0x4326
+#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
+
+/* USB commands */
+#define CMD_MEM_RD                  0x00
+#define CMD_MEM_WR                  0x01
+#define CMD_I2C_RD                  0x02
+#define CMD_I2C_WR                  0x03
+#define CMD_IR_GET                  0x18
+#define CMD_FW_DL                   0x21
+#define CMD_FW_QUERYINFO            0x22
+#define CMD_FW_BOOT                 0x23
+#define CMD_FW_DL_BEGIN             0x24
+#define CMD_FW_DL_END               0x25
+#define CMD_FW_SCATTER_WR           0x29
+
+#endif
diff --git a/drivers/media/dvb/dvb-usb/dib0700_core.c b/drivers/media/dvb/dvb-usb/dib0700_core.c
index 02290c60f72..7e9e00fae04 100644
--- a/drivers/media/dvb/dvb-usb/dib0700_core.c
+++ b/drivers/media/dvb/dvb-usb/dib0700_core.c
@@ -32,7 +32,7 @@ int dib0700_get_version(struct dvb_usb_device *d, u32 *hwversion,
 
 	if (mutex_lock_interruptible(&d->usb_mutex) < 0) {
 		err("could not acquire lock");
-		return 0;
+		return -EINTR;
 	}
 
 	ret = usb_control_msg(d->udev, usb_rcvctrlpipe(d->udev, 0),
@@ -118,7 +118,7 @@ int dib0700_set_gpio(struct dvb_usb_device *d, enum dib07x0_gpios gpio, u8 gpio_
 
 	if (mutex_lock_interruptible(&d->usb_mutex) < 0) {
 		err("could not acquire lock");
-		return 0;
+		return -EINTR;
 	}
 
 	st->buf[0] = REQUEST_SET_GPIO;
@@ -139,7 +139,7 @@ static int dib0700_set_usb_xfer_len(struct dvb_usb_device *d, u16 nb_ts_packets)
 	if (st->fw_version >= 0x10201) {
 		if (mutex_lock_interruptible(&d->usb_mutex) < 0) {
 			err("could not acquire lock");
-			return 0;
+			return -EINTR;
 		}
 
 		st->buf[0] = REQUEST_SET_USB_XFER_LEN;
@@ -178,7 +178,7 @@ static int dib0700_i2c_xfer_new(struct i2c_adapter *adap, struct i2c_msg *msg,
 	/* Ensure nobody else hits the i2c bus while we're sending our
 	   sequence of messages, (such as the remote control thread) */
 	if (mutex_lock_interruptible(&d->i2c_mutex) < 0)
-		return -EAGAIN;
+		return -EINTR;
 
 	for (i = 0; i < num; i++) {
 		if (i == 0) {
@@ -228,7 +228,8 @@ static int dib0700_i2c_xfer_new(struct i2c_adapter *adap, struct i2c_msg *msg,
 			/* Write request */
 			if (mutex_lock_interruptible(&d->usb_mutex) < 0) {
 				err("could not acquire lock");
-				return 0;
+				mutex_unlock(&d->i2c_mutex);
+				return -EINTR;
 			}
 			st->buf[0] = REQUEST_NEW_I2C_WRITE;
 			st->buf[1] = msg[i].addr << 1;
@@ -271,10 +272,11 @@ static int dib0700_i2c_xfer_legacy(struct i2c_adapter *adap,
 	int i,len;
 
 	if (mutex_lock_interruptible(&d->i2c_mutex) < 0)
-		return -EAGAIN;
+		return -EINTR;
 	if (mutex_lock_interruptible(&d->usb_mutex) < 0) {
 		err("could not acquire lock");
-		return 0;
+		mutex_unlock(&d->i2c_mutex);
+		return -EINTR;
 	}
 
 	for (i = 0; i < num; i++) {
@@ -369,7 +371,7 @@ static int dib0700_set_clock(struct dvb_usb_device *d, u8 en_pll,
 
 	if (mutex_lock_interruptible(&d->usb_mutex) < 0) {
 		err("could not acquire lock");
-		return 0;
+		return -EINTR;
 	}
 
 	st->buf[0] = REQUEST_SET_CLOCK;
@@ -401,7 +403,7 @@ int dib0700_set_i2c_speed(struct dvb_usb_device *d, u16 scl_kHz)
 
 	if (mutex_lock_interruptible(&d->usb_mutex) < 0) {
 		err("could not acquire lock");
-		return 0;
+		return -EINTR;
 	}
 
 	st->buf[0] = REQUEST_SET_I2C_PARAM;
@@ -561,7 +563,7 @@ int dib0700_streaming_ctrl(struct dvb_usb_adapter *adap, int onoff)
 
 	if (mutex_lock_interruptible(&adap->dev->usb_mutex) < 0) {
 		err("could not acquire lock");
-		return 0;
+		return -EINTR;
 	}
 
 	st->buf[0] = REQUEST_ENABLE_VIDEO;
@@ -611,7 +613,7 @@ int dib0700_change_protocol(struct rc_dev *rc, u64 rc_type)
 
 	if (mutex_lock_interruptible(&d->usb_mutex) < 0) {
 		err("could not acquire lock");
-		return 0;
+		return -EINTR;
 	}
 
 	st->buf[0] = REQUEST_SET_RC;
diff --git a/drivers/media/dvb/dvb-usb/dib0700_devices.c b/drivers/media/dvb/dvb-usb/dib0700_devices.c
index f9e966aa26e..510001da6e8 100644
--- a/drivers/media/dvb/dvb-usb/dib0700_devices.c
+++ b/drivers/media/dvb/dvb-usb/dib0700_devices.c
@@ -3569,6 +3569,7 @@ struct usb_device_id dib0700_usb_id_table[] = {
 	{ 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) },
 	{ 0 }		/* Terminating entry */
 };
 MODULE_DEVICE_TABLE(usb, dib0700_usb_id_table);
@@ -3832,7 +3833,7 @@ struct dvb_usb_device_properties dib0700_devices[] = {
 			},
 		},
 
-		.num_device_descs = 11,
+		.num_device_descs = 12,
 		.devices = {
 			{   "DiBcom STK7070P reference design",
 				{ &dib0700_usb_id_table[15], NULL },
@@ -3878,6 +3879,10 @@ struct dvb_usb_device_properties dib0700_devices[] = {
 				{ &dib0700_usb_id_table[50], NULL },
 				{ NULL },
 			},
+			{   "Elgato EyeTV DTT rev. 2",
+				{ &dib0700_usb_id_table[81], NULL },
+				{ NULL },
+			},
 		},
 
 		.rc.core = {
diff --git a/drivers/media/dvb/dvb-usb/dvb-usb-ids.h b/drivers/media/dvb/dvb-usb/dvb-usb-ids.h
index 397d8f23273..7a6160bf54b 100644
--- a/drivers/media/dvb/dvb-usb/dvb-usb-ids.h
+++ b/drivers/media/dvb/dvb-usb/dvb-usb-ids.h
@@ -76,6 +76,11 @@
 #define USB_PID_AFATECH_AF9005				0x9020
 #define USB_PID_AFATECH_AF9015_9015			0x9015
 #define USB_PID_AFATECH_AF9015_9016			0x9016
+#define USB_PID_AFATECH_AF9035_1000			0x1000
+#define USB_PID_AFATECH_AF9035_1001			0x1001
+#define USB_PID_AFATECH_AF9035_1002			0x1002
+#define USB_PID_AFATECH_AF9035_1003			0x1003
+#define USB_PID_AFATECH_AF9035_9035			0x9035
 #define USB_PID_TREKSTOR_DVBT				0x901b
 #define USB_VID_ALINK_DTU				0xf170
 #define USB_PID_ANSONIC_DVBT_USB			0x6000
@@ -152,6 +157,7 @@
 #define USB_PID_KWORLD_VSTREAM_WARM			0x17df
 #define USB_PID_TERRATEC_CINERGY_T_USB_XE		0x0055
 #define USB_PID_TERRATEC_CINERGY_T_USB_XE_REV2		0x0069
+#define USB_PID_TERRATEC_CINERGY_T_STICK		0x0093
 #define USB_PID_TERRATEC_CINERGY_T_STICK_RC		0x0097
 #define USB_PID_TERRATEC_CINERGY_T_STICK_DUAL_RC	0x0099
 #define USB_PID_TWINHAN_VP7041_COLD			0x3201
@@ -221,6 +227,11 @@
 #define USB_PID_AVERMEDIA_A850T				0x850b
 #define USB_PID_AVERMEDIA_A805				0xa805
 #define USB_PID_AVERMEDIA_A815M				0x815a
+#define USB_PID_AVERMEDIA_A835				0xa835
+#define USB_PID_AVERMEDIA_B835				0xb835
+#define USB_PID_AVERMEDIA_1867				0x1867
+#define USB_PID_AVERMEDIA_A867				0xa867
+#define USB_PID_AVERMEDIA_TWINSTAR			0x0825
 #define USB_PID_TECHNOTREND_CONNECT_S2400               0x3006
 #define USB_PID_TECHNOTREND_CONNECT_CT3650		0x300d
 #define USB_PID_TERRATEC_CINERGY_DT_XS_DIVERSITY	0x005a
@@ -327,6 +338,7 @@
 #define USB_PID_MYGICA_D689				0xd811
 #define USB_PID_ELGATO_EYETV_DIVERSITY			0x0011
 #define USB_PID_ELGATO_EYETV_DTT			0x0021
+#define USB_PID_ELGATO_EYETV_DTT_2			0x003f
 #define USB_PID_ELGATO_EYETV_DTT_Dlx			0x0020
 #define USB_PID_ELGATO_EYETV_SAT			0x002a
 #define USB_PID_DVB_T_USB_STICK_HIGH_SPEED_COLD		0x5000
diff --git a/drivers/media/dvb/dvb-usb/dvb-usb-urb.c b/drivers/media/dvb/dvb-usb/dvb-usb-urb.c
index 53a5c30b51b..5c8f651344f 100644
--- a/drivers/media/dvb/dvb-usb/dvb-usb-urb.c
+++ b/drivers/media/dvb/dvb-usb/dvb-usb-urb.c
@@ -80,6 +80,14 @@ static void dvb_usb_data_complete_204(struct usb_data_stream *stream, u8 *buffer
 		dvb_dmx_swfilter_204(&adap->demux, buffer, length);
 }
 
+static void dvb_usb_data_complete_raw(struct usb_data_stream *stream,
+				      u8 *buffer, size_t length)
+{
+	struct dvb_usb_adapter *adap = stream->user_priv;
+	if (adap->feedcount > 0 && adap->state & DVB_USB_ADAP_STATE_DVB)
+		dvb_dmx_swfilter_raw(&adap->demux, buffer, length);
+}
+
 int dvb_usb_adapter_stream_init(struct dvb_usb_adapter *adap)
 {
 	int i, ret = 0;
@@ -90,6 +98,10 @@ int dvb_usb_adapter_stream_init(struct dvb_usb_adapter *adap)
 			adap->fe_adap[i].stream.complete =
 				dvb_usb_data_complete_204;
 		else
+		if (adap->props.fe[i].caps & DVB_USB_ADAP_RECEIVES_RAW_PAYLOAD)
+			adap->fe_adap[i].stream.complete =
+				dvb_usb_data_complete_raw;
+		else
 		adap->fe_adap[i].stream.complete  = dvb_usb_data_complete;
 		adap->fe_adap[i].stream.user_priv = adap;
 		ret = usb_urb_init(&adap->fe_adap[i].stream,
diff --git a/drivers/media/dvb/dvb-usb/dvb-usb.h b/drivers/media/dvb/dvb-usb/dvb-usb.h
index 6d7d13f9ce6..99f94409efa 100644
--- a/drivers/media/dvb/dvb-usb/dvb-usb.h
+++ b/drivers/media/dvb/dvb-usb/dvb-usb.h
@@ -141,6 +141,7 @@ struct dvb_usb_adapter_fe_properties {
 #define DVB_USB_ADAP_PID_FILTER_CAN_BE_TURNED_OFF 0x02
 #define DVB_USB_ADAP_NEED_PID_FILTERING           0x04
 #define DVB_USB_ADAP_RECEIVES_204_BYTE_TS         0x08
+#define DVB_USB_ADAP_RECEIVES_RAW_PAYLOAD         0x10
 	int caps;
 	int pid_filter_count;
 
@@ -156,7 +157,7 @@ struct dvb_usb_adapter_fe_properties {
 	int size_of_priv;
 };
 
-#define MAX_NO_OF_FE_PER_ADAP 2
+#define MAX_NO_OF_FE_PER_ADAP 3
 struct dvb_usb_adapter_properties {
 	int size_of_priv;
 
diff --git a/drivers/media/dvb/dvb-usb/dw2102.c b/drivers/media/dvb/dvb-usb/dw2102.c
index 451c5a7adfb..9382895b1b8 100644
--- a/drivers/media/dvb/dvb-usb/dw2102.c
+++ b/drivers/media/dvb/dvb-usb/dw2102.c
@@ -148,7 +148,7 @@ static int dw2102_i2c_transfer(struct i2c_adapter *adap, struct i2c_msg msg[],
 		int num)
 {
 	struct dvb_usb_device *d = i2c_get_adapdata(adap);
-	int i = 0, ret = 0;
+	int i = 0;
 	u8 buf6[] = {0x2c, 0x05, 0xc0, 0, 0, 0, 0};
 	u16 value;
 
@@ -162,7 +162,7 @@ static int dw2102_i2c_transfer(struct i2c_adapter *adap, struct i2c_msg msg[],
 		/* read stv0299 register */
 		value = msg[0].buf[0];/* register */
 		for (i = 0; i < msg[1].len; i++) {
-			ret = dw210x_op_rw(d->udev, 0xb5, value + i, 0,
+			dw210x_op_rw(d->udev, 0xb5, value + i, 0,
 					buf6, 2, DW210X_READ_MSG);
 			msg[1].buf[i] = buf6[0];
 		}
@@ -174,7 +174,7 @@ static int dw2102_i2c_transfer(struct i2c_adapter *adap, struct i2c_msg msg[],
 			buf6[0] = 0x2a;
 			buf6[1] = msg[0].buf[0];
 			buf6[2] = msg[0].buf[1];
-			ret = dw210x_op_rw(d->udev, 0xb2, 0, 0,
+			dw210x_op_rw(d->udev, 0xb2, 0, 0,
 					buf6, 3, DW210X_WRITE_MSG);
 			break;
 		case 0x60:
@@ -187,17 +187,17 @@ static int dw2102_i2c_transfer(struct i2c_adapter *adap, struct i2c_msg msg[],
 				buf6[4] = msg[0].buf[1];
 				buf6[5] = msg[0].buf[2];
 				buf6[6] = msg[0].buf[3];
-				ret = dw210x_op_rw(d->udev, 0xb2, 0, 0,
+				dw210x_op_rw(d->udev, 0xb2, 0, 0,
 						buf6, 7, DW210X_WRITE_MSG);
 			} else {
 			/* read from tuner */
-				ret = dw210x_op_rw(d->udev, 0xb5, 0, 0,
+				dw210x_op_rw(d->udev, 0xb5, 0, 0,
 						buf6, 1, DW210X_READ_MSG);
 				msg[0].buf[0] = buf6[0];
 			}
 			break;
 		case (DW2102_RC_QUERY):
-			ret  = dw210x_op_rw(d->udev, 0xb8, 0, 0,
+			dw210x_op_rw(d->udev, 0xb8, 0, 0,
 					buf6, 2, DW210X_READ_MSG);
 			msg[0].buf[0] = buf6[0];
 			msg[0].buf[1] = buf6[1];
@@ -205,7 +205,7 @@ static int dw2102_i2c_transfer(struct i2c_adapter *adap, struct i2c_msg msg[],
 		case (DW2102_VOLTAGE_CTRL):
 			buf6[0] = 0x30;
 			buf6[1] = msg[0].buf[0];
-			ret = dw210x_op_rw(d->udev, 0xb2, 0, 0,
+			dw210x_op_rw(d->udev, 0xb2, 0, 0,
 					buf6, 2, DW210X_WRITE_MSG);
 			break;
 		}
@@ -221,7 +221,6 @@ static int dw2102_serit_i2c_transfer(struct i2c_adapter *adap,
 						struct i2c_msg msg[], int num)
 {
 	struct dvb_usb_device *d = i2c_get_adapdata(adap);
-	int ret = 0;
 	u8 buf6[] = {0, 0, 0, 0, 0, 0, 0};
 
 	if (!d)
@@ -235,10 +234,10 @@ static int dw2102_serit_i2c_transfer(struct i2c_adapter *adap,
 		buf6[0] = msg[0].addr << 1;
 		buf6[1] = msg[0].len;
 		buf6[2] = msg[0].buf[0];
-		ret = dw210x_op_rw(d->udev, 0xc2, 0, 0,
+		dw210x_op_rw(d->udev, 0xc2, 0, 0,
 				buf6, msg[0].len + 2, DW210X_WRITE_MSG);
 		/* read si2109 register */
-		ret = dw210x_op_rw(d->udev, 0xc3, 0xd0, 0,
+		dw210x_op_rw(d->udev, 0xc3, 0xd0, 0,
 				buf6, msg[1].len + 2, DW210X_READ_MSG);
 		memcpy(msg[1].buf, buf6 + 2, msg[1].len);
 
@@ -250,11 +249,11 @@ static int dw2102_serit_i2c_transfer(struct i2c_adapter *adap,
 			buf6[0] = msg[0].addr << 1;
 			buf6[1] = msg[0].len;
 			memcpy(buf6 + 2, msg[0].buf, msg[0].len);
-			ret = dw210x_op_rw(d->udev, 0xc2, 0, 0, buf6,
+			dw210x_op_rw(d->udev, 0xc2, 0, 0, buf6,
 					msg[0].len + 2, DW210X_WRITE_MSG);
 			break;
 		case(DW2102_RC_QUERY):
-			ret  = dw210x_op_rw(d->udev, 0xb8, 0, 0,
+			dw210x_op_rw(d->udev, 0xb8, 0, 0,
 					buf6, 2, DW210X_READ_MSG);
 			msg[0].buf[0] = buf6[0];
 			msg[0].buf[1] = buf6[1];
@@ -262,7 +261,7 @@ static int dw2102_serit_i2c_transfer(struct i2c_adapter *adap,
 		case(DW2102_VOLTAGE_CTRL):
 			buf6[0] = 0x30;
 			buf6[1] = msg[0].buf[0];
-			ret = dw210x_op_rw(d->udev, 0xb2, 0, 0,
+			dw210x_op_rw(d->udev, 0xb2, 0, 0,
 					buf6, 2, DW210X_WRITE_MSG);
 			break;
 		}
@@ -276,7 +275,6 @@ static int dw2102_serit_i2c_transfer(struct i2c_adapter *adap,
 static int dw2102_earda_i2c_transfer(struct i2c_adapter *adap, struct i2c_msg msg[], int num)
 {
 	struct dvb_usb_device *d = i2c_get_adapdata(adap);
-	int ret = 0;
 
 	if (!d)
 		return -ENODEV;
@@ -291,10 +289,10 @@ static int dw2102_earda_i2c_transfer(struct i2c_adapter *adap, struct i2c_msg ms
 		obuf[0] = msg[0].addr << 1;
 		obuf[1] = msg[0].len;
 		obuf[2] = msg[0].buf[0];
-		ret = dw210x_op_rw(d->udev, 0xc2, 0, 0,
+		dw210x_op_rw(d->udev, 0xc2, 0, 0,
 				obuf, msg[0].len + 2, DW210X_WRITE_MSG);
 		/* second read registers */
-		ret = dw210x_op_rw(d->udev, 0xc3, 0xd1 , 0,
+		dw210x_op_rw(d->udev, 0xc3, 0xd1 , 0,
 				ibuf, msg[1].len + 2, DW210X_READ_MSG);
 		memcpy(msg[1].buf, ibuf + 2, msg[1].len);
 
@@ -308,7 +306,7 @@ static int dw2102_earda_i2c_transfer(struct i2c_adapter *adap, struct i2c_msg ms
 			obuf[0] = msg[0].addr << 1;
 			obuf[1] = msg[0].len;
 			memcpy(obuf + 2, msg[0].buf, msg[0].len);
-			ret = dw210x_op_rw(d->udev, 0xc2, 0, 0,
+			dw210x_op_rw(d->udev, 0xc2, 0, 0,
 					obuf, msg[0].len + 2, DW210X_WRITE_MSG);
 			break;
 		}
@@ -318,13 +316,13 @@ static int dw2102_earda_i2c_transfer(struct i2c_adapter *adap, struct i2c_msg ms
 			obuf[0] = msg[0].addr << 1;
 			obuf[1] = msg[0].len;
 			memcpy(obuf + 2, msg[0].buf, msg[0].len);
-			ret = dw210x_op_rw(d->udev, 0xc2, 0, 0,
+			dw210x_op_rw(d->udev, 0xc2, 0, 0,
 					obuf, msg[0].len + 2, DW210X_WRITE_MSG);
 			break;
 		}
 		case(DW2102_RC_QUERY): {
 			u8 ibuf[2];
-			ret  = dw210x_op_rw(d->udev, 0xb8, 0, 0,
+			dw210x_op_rw(d->udev, 0xb8, 0, 0,
 					ibuf, 2, DW210X_READ_MSG);
 			memcpy(msg[0].buf, ibuf , 2);
 			break;
@@ -333,7 +331,7 @@ static int dw2102_earda_i2c_transfer(struct i2c_adapter *adap, struct i2c_msg ms
 			u8 obuf[2];
 			obuf[0] = 0x30;
 			obuf[1] = msg[0].buf[0];
-			ret = dw210x_op_rw(d->udev, 0xb2, 0, 0,
+			dw210x_op_rw(d->udev, 0xb2, 0, 0,
 					obuf, 2, DW210X_WRITE_MSG);
 			break;
 		}
@@ -349,7 +347,6 @@ static int dw2102_earda_i2c_transfer(struct i2c_adapter *adap, struct i2c_msg ms
 static int dw2104_i2c_transfer(struct i2c_adapter *adap, struct i2c_msg msg[], int num)
 {
 	struct dvb_usb_device *d = i2c_get_adapdata(adap);
-	int ret = 0;
 	int len, i, j;
 
 	if (!d)
@@ -361,7 +358,7 @@ static int dw2104_i2c_transfer(struct i2c_adapter *adap, struct i2c_msg msg[], i
 		switch (msg[j].addr) {
 		case(DW2102_RC_QUERY): {
 			u8 ibuf[2];
-			ret  = dw210x_op_rw(d->udev, 0xb8, 0, 0,
+			dw210x_op_rw(d->udev, 0xb8, 0, 0,
 					ibuf, 2, DW210X_READ_MSG);
 			memcpy(msg[j].buf, ibuf , 2);
 			break;
@@ -370,7 +367,7 @@ static int dw2104_i2c_transfer(struct i2c_adapter *adap, struct i2c_msg msg[], i
 			u8 obuf[2];
 			obuf[0] = 0x30;
 			obuf[1] = msg[j].buf[0];
-			ret = dw210x_op_rw(d->udev, 0xb2, 0, 0,
+			dw210x_op_rw(d->udev, 0xb2, 0, 0,
 					obuf, 2, DW210X_WRITE_MSG);
 			break;
 		}
@@ -382,7 +379,7 @@ static int dw2104_i2c_transfer(struct i2c_adapter *adap, struct i2c_msg msg[], i
 			if (msg[j].flags == I2C_M_RD) {
 				/* read registers */
 				u8  ibuf[msg[j].len + 2];
-				ret = dw210x_op_rw(d->udev, 0xc3,
+				dw210x_op_rw(d->udev, 0xc3,
 						(msg[j].addr << 1) + 1, 0,
 						ibuf, msg[j].len + 2,
 						DW210X_READ_MSG);
@@ -402,7 +399,7 @@ static int dw2104_i2c_transfer(struct i2c_adapter *adap, struct i2c_msg msg[], i
 				do {
 					memcpy(obuf + 3, msg[j].buf + i,
 							(len > 16 ? 16 : len));
-					ret = dw210x_op_rw(d->udev, 0xc2, 0, 0,
+					dw210x_op_rw(d->udev, 0xc2, 0, 0,
 						obuf, (len > 16 ? 16 : len) + 3,
 						DW210X_WRITE_MSG);
 					i += 16;
@@ -414,7 +411,7 @@ static int dw2104_i2c_transfer(struct i2c_adapter *adap, struct i2c_msg msg[], i
 				obuf[0] = msg[j].addr << 1;
 				obuf[1] = msg[j].len;
 				memcpy(obuf + 2, msg[j].buf, msg[j].len);
-				ret = dw210x_op_rw(d->udev, 0xc2, 0, 0,
+				dw210x_op_rw(d->udev, 0xc2, 0, 0,
 						obuf, msg[j].len + 2,
 						DW210X_WRITE_MSG);
 			}
@@ -432,7 +429,7 @@ static int dw3101_i2c_transfer(struct i2c_adapter *adap, struct i2c_msg msg[],
 								int num)
 {
 	struct dvb_usb_device *d = i2c_get_adapdata(adap);
-	int ret = 0, i;
+	int i;
 
 	if (!d)
 		return -ENODEV;
@@ -447,10 +444,10 @@ static int dw3101_i2c_transfer(struct i2c_adapter *adap, struct i2c_msg msg[],
 		obuf[0] = msg[0].addr << 1;
 		obuf[1] = msg[0].len;
 		obuf[2] = msg[0].buf[0];
-		ret = dw210x_op_rw(d->udev, 0xc2, 0, 0,
+		dw210x_op_rw(d->udev, 0xc2, 0, 0,
 				obuf, msg[0].len + 2, DW210X_WRITE_MSG);
 		/* second read registers */
-		ret = dw210x_op_rw(d->udev, 0xc3, 0x19 , 0,
+		dw210x_op_rw(d->udev, 0xc3, 0x19 , 0,
 				ibuf, msg[1].len + 2, DW210X_READ_MSG);
 		memcpy(msg[1].buf, ibuf + 2, msg[1].len);
 
@@ -465,13 +462,13 @@ static int dw3101_i2c_transfer(struct i2c_adapter *adap, struct i2c_msg msg[],
 			obuf[0] = msg[0].addr << 1;
 			obuf[1] = msg[0].len;
 			memcpy(obuf + 2, msg[0].buf, msg[0].len);
-			ret = dw210x_op_rw(d->udev, 0xc2, 0, 0,
+			dw210x_op_rw(d->udev, 0xc2, 0, 0,
 					obuf, msg[0].len + 2, DW210X_WRITE_MSG);
 			break;
 		}
 		case(DW2102_RC_QUERY): {
 			u8 ibuf[2];
-			ret  = dw210x_op_rw(d->udev, 0xb8, 0, 0,
+			dw210x_op_rw(d->udev, 0xb8, 0, 0,
 					ibuf, 2, DW210X_READ_MSG);
 			memcpy(msg[0].buf, ibuf , 2);
 			break;
@@ -496,7 +493,6 @@ static int s6x0_i2c_transfer(struct i2c_adapter *adap, struct i2c_msg msg[],
 {
 	struct dvb_usb_device *d = i2c_get_adapdata(adap);
 	struct usb_device *udev;
-	int ret = 0;
 	int len, i, j;
 
 	if (!d)
@@ -509,7 +505,7 @@ static int s6x0_i2c_transfer(struct i2c_adapter *adap, struct i2c_msg msg[],
 		switch (msg[j].addr) {
 		case (DW2102_RC_QUERY): {
 			u8 ibuf[5];
-			ret  = dw210x_op_rw(d->udev, 0xb8, 0, 0,
+			dw210x_op_rw(d->udev, 0xb8, 0, 0,
 					ibuf, 5, DW210X_READ_MSG);
 			memcpy(msg[j].buf, ibuf + 3, 2);
 			break;
@@ -519,11 +515,11 @@ static int s6x0_i2c_transfer(struct i2c_adapter *adap, struct i2c_msg msg[],
 
 			obuf[0] = 1;
 			obuf[1] = msg[j].buf[1];/* off-on */
-			ret = dw210x_op_rw(d->udev, 0x8a, 0, 0,
+			dw210x_op_rw(d->udev, 0x8a, 0, 0,
 					obuf, 2, DW210X_WRITE_MSG);
 			obuf[0] = 3;
 			obuf[1] = msg[j].buf[0];/* 13v-18v */
-			ret = dw210x_op_rw(d->udev, 0x8a, 0, 0,
+			dw210x_op_rw(d->udev, 0x8a, 0, 0,
 					obuf, 2, DW210X_WRITE_MSG);
 			break;
 		}
@@ -532,7 +528,7 @@ static int s6x0_i2c_transfer(struct i2c_adapter *adap, struct i2c_msg msg[],
 
 			obuf[0] = 5;
 			obuf[1] = msg[j].buf[0];
-			ret = dw210x_op_rw(d->udev, 0x8a, 0, 0,
+			dw210x_op_rw(d->udev, 0x8a, 0, 0,
 					obuf, 2, DW210X_WRITE_MSG);
 			break;
 		}
@@ -545,7 +541,7 @@ static int s6x0_i2c_transfer(struct i2c_adapter *adap, struct i2c_msg msg[],
 			if (msg[j].flags == I2C_M_RD) {
 				/* read registers */
 				u8 ibuf[msg[j].len];
-				ret = dw210x_op_rw(d->udev, 0x91, 0, 0,
+				dw210x_op_rw(d->udev, 0x91, 0, 0,
 						ibuf, msg[j].len,
 						DW210X_READ_MSG);
 				memcpy(msg[j].buf, ibuf, msg[j].len);
@@ -563,7 +559,7 @@ static int s6x0_i2c_transfer(struct i2c_adapter *adap, struct i2c_msg msg[],
 				do {
 					memcpy(obuf + 3, msg[j].buf + i,
 							(len > 16 ? 16 : len));
-					ret = dw210x_op_rw(d->udev, 0x80, 0, 0,
+					dw210x_op_rw(d->udev, 0x80, 0, 0,
 						obuf, (len > 16 ? 16 : len) + 3,
 						DW210X_WRITE_MSG);
 					i += 16;
@@ -575,7 +571,7 @@ static int s6x0_i2c_transfer(struct i2c_adapter *adap, struct i2c_msg msg[],
 				obuf[0] = msg[j + 1].len;
 				obuf[1] = (msg[j].addr << 1);
 				memcpy(obuf + 2, msg[j].buf, msg[j].len);
-				ret = dw210x_op_rw(d->udev,
+				dw210x_op_rw(d->udev,
 						udev->descriptor.idProduct ==
 						0x7500 ? 0x92 : 0x90, 0, 0,
 						obuf, msg[j].len + 2,
@@ -587,7 +583,7 @@ static int s6x0_i2c_transfer(struct i2c_adapter *adap, struct i2c_msg msg[],
 				obuf[0] = msg[j].len + 1;
 				obuf[1] = (msg[j].addr << 1);
 				memcpy(obuf + 2, msg[j].buf, msg[j].len);
-				ret = dw210x_op_rw(d->udev, 0x80, 0, 0,
+				dw210x_op_rw(d->udev, 0x80, 0, 0,
 						obuf, msg[j].len + 2,
 						DW210X_WRITE_MSG);
 				break;
diff --git a/drivers/media/dvb/dvb-usb/it913x.c b/drivers/media/dvb/dvb-usb/it913x.c
index 482d249ca7f..6244fe9d1a3 100644
--- a/drivers/media/dvb/dvb-usb/it913x.c
+++ b/drivers/media/dvb/dvb-usb/it913x.c
@@ -81,7 +81,7 @@ static int it913x_bulk_write(struct usb_device *dev,
 	for (i = 0; i < IT913X_RETRY; i++) {
 		ret = usb_bulk_msg(dev, usb_sndbulkpipe(dev, pipe),
 				snd, len , &actual_l, IT913X_SND_TIMEOUT);
-		if (ret == 0 || ret != -EBUSY || ret != -ETIMEDOUT)
+		if (ret != -EBUSY && ret != -ETIMEDOUT)
 			break;
 	}
 
@@ -99,7 +99,7 @@ static int it913x_bulk_read(struct usb_device *dev,
 	for (i = 0; i < IT913X_RETRY; i++) {
 		ret = usb_bulk_msg(dev, usb_rcvbulkpipe(dev, pipe),
 				 rev, len , &actual_l, IT913X_RCV_TIMEOUT);
-		if (ret == 0 || ret != -EBUSY || ret != -ETIMEDOUT)
+		if (ret != -EBUSY && ret != -ETIMEDOUT)
 			break;
 	}
 
diff --git a/drivers/media/dvb/dvb-usb/lmedm04.c b/drivers/media/dvb/dvb-usb/lmedm04.c
index 5dde06d066f..25d1031460f 100644
--- a/drivers/media/dvb/dvb-usb/lmedm04.c
+++ b/drivers/media/dvb/dvb-usb/lmedm04.c
@@ -373,7 +373,7 @@ static int lme2510_pid_filter_ctrl(struct dvb_usb_adapter *adap, int onoff)
 	struct lme2510_state *st = adap->dev->priv;
 	static u8 clear_pid_reg[] = LME_ALL_PIDS;
 	static u8 rbuf[1];
-	int ret;
+	int ret = 0;
 
 	deb_info(1, "PID Clearing Filter");
 
@@ -1205,14 +1205,13 @@ static int lme2510_probe(struct usb_interface *intf,
 		const struct usb_device_id *id)
 {
 	struct usb_device *udev = interface_to_usbdev(intf);
-	int ret = 0;
 
 	usb_reset_configuration(udev);
 
 	usb_set_interface(udev, intf->cur_altsetting->desc.bInterfaceNumber, 1);
 
 	if (udev->speed != USB_SPEED_HIGH) {
-		ret = usb_reset_device(udev);
+		usb_reset_device(udev);
 		info("DEV Failed to connect in HIGH SPEED mode");
 		return -ENODEV;
 	}
diff --git a/drivers/media/dvb/dvb-usb/mxl111sf-tuner.c b/drivers/media/dvb/dvb-usb/mxl111sf-tuner.c
index 72db6eef4b9..74da5bb1ce9 100644
--- a/drivers/media/dvb/dvb-usb/mxl111sf-tuner.c
+++ b/drivers/media/dvb/dvb-usb/mxl111sf-tuner.c
@@ -284,6 +284,7 @@ static int mxl111sf_tuner_set_params(struct dvb_frontend *fe)
 
 	switch (delsys) {
 	case SYS_ATSC:
+	case SYS_ATSCMH:
 		bw = 0; /* ATSC */
 		break;
 	case SYS_DVBC_ANNEX_B:
diff --git a/drivers/media/dvb/dvb-usb/mxl111sf.c b/drivers/media/dvb/dvb-usb/mxl111sf.c
index 81305de2fea..cd842798f5a 100644
--- a/drivers/media/dvb/dvb-usb/mxl111sf.c
+++ b/drivers/media/dvb/dvb-usb/mxl111sf.c
@@ -21,6 +21,7 @@
 #include "mxl111sf-tuner.h"
 
 #include "lgdt3305.h"
+#include "lg2160.h"
 
 int dvb_usb_mxl111sf_debug;
 module_param_named(debug, dvb_usb_mxl111sf_debug, int, 0644);
@@ -31,6 +32,10 @@ int dvb_usb_mxl111sf_isoc;
 module_param_named(isoc, dvb_usb_mxl111sf_isoc, int, 0644);
 MODULE_PARM_DESC(isoc, "enable usb isoc xfer (0=bulk, 1=isoc).");
 
+int dvb_usb_mxl111sf_spi;
+module_param_named(spi, dvb_usb_mxl111sf_spi, int, 0644);
+MODULE_PARM_DESC(spi, "use spi rather than tp for data xfer (0=tp, 1=spi).");
+
 #define ANT_PATH_AUTO 0
 #define ANT_PATH_EXTERNAL 1
 #define ANT_PATH_INTERNAL 2
@@ -340,7 +345,6 @@ static int mxl111sf_ep6_streaming_ctrl(struct dvb_usb_adapter *adap, int onoff)
 	struct mxl111sf_state *state = d->priv;
 	struct mxl111sf_adap_state *adap_state = adap->fe_adap[adap->active_fe].priv;
 	int ret = 0;
-	u8 tmp;
 
 	deb_info("%s(%d)\n", __func__, onoff);
 
@@ -361,6 +365,33 @@ static int mxl111sf_ep6_streaming_ctrl(struct dvb_usb_adapter *adap, int onoff)
 	return ret;
 }
 
+static int mxl111sf_ep5_streaming_ctrl(struct dvb_usb_adapter *adap, int onoff)
+{
+	struct dvb_usb_device *d = adap->dev;
+	struct mxl111sf_state *state = d->priv;
+	int ret = 0;
+
+	deb_info("%s(%d)\n", __func__, onoff);
+
+	if (onoff) {
+		ret = mxl111sf_enable_usb_output(state);
+		mxl_fail(ret);
+
+		ret = mxl111sf_init_i2s_port(state, 200);
+		mxl_fail(ret);
+		ret = mxl111sf_config_i2s(state, 0, 15);
+		mxl_fail(ret);
+	} else {
+		ret = mxl111sf_disable_i2s_port(state);
+		mxl_fail(ret);
+	}
+	if (state->chip_rev > MXL111SF_V6)
+		ret = mxl111sf_config_spi(state, onoff);
+	mxl_fail(ret);
+
+	return ret;
+}
+
 static int mxl111sf_ep4_streaming_ctrl(struct dvb_usb_adapter *adap, int onoff)
 {
 	struct dvb_usb_device *d = adap->dev;
@@ -453,6 +484,255 @@ fail:
 	return ret;
 }
 
+static struct lg2160_config hauppauge_lg2160_config = {
+	.lg_chip            = LG2160,
+	.i2c_addr           = 0x1c >> 1,
+	.deny_i2c_rptr      = 1,
+	.spectral_inversion = 0,
+	.if_khz             = 6000,
+};
+
+static int mxl111sf_lg2160_frontend_attach(struct dvb_usb_adapter *adap)
+{
+	struct dvb_usb_device *d = adap->dev;
+	struct mxl111sf_state *state = d->priv;
+	int fe_id = adap->num_frontends_initialized;
+	struct mxl111sf_adap_state *adap_state = adap->fe_adap[fe_id].priv;
+	int ret;
+
+	deb_adv("%s()\n", __func__);
+
+	/* save a pointer to the dvb_usb_device in device state */
+	state->d = d;
+	adap_state->alt_mode = (dvb_usb_mxl111sf_isoc) ? 2 : 1;
+	state->alt_mode = adap_state->alt_mode;
+
+	if (usb_set_interface(adap->dev->udev, 0, state->alt_mode) < 0)
+		err("set interface failed");
+
+	state->gpio_mode = MXL111SF_GPIO_MOD_MH;
+	adap_state->gpio_mode = state->gpio_mode;
+	adap_state->device_mode = MXL_TUNER_MODE;
+	adap_state->ep6_clockphase = 1;
+
+	ret = mxl1x1sf_soft_reset(state);
+	if (mxl_fail(ret))
+		goto fail;
+	ret = mxl111sf_init_tuner_demod(state);
+	if (mxl_fail(ret))
+		goto fail;
+
+	ret = mxl1x1sf_set_device_mode(state, adap_state->device_mode);
+	if (mxl_fail(ret))
+		goto fail;
+
+	ret = mxl111sf_enable_usb_output(state);
+	if (mxl_fail(ret))
+		goto fail;
+	ret = mxl1x1sf_top_master_ctrl(state, 1);
+	if (mxl_fail(ret))
+		goto fail;
+
+	ret = mxl111sf_init_port_expander(state);
+	if (mxl_fail(ret))
+		goto fail;
+	ret = mxl111sf_gpio_mode_switch(state, state->gpio_mode);
+	if (mxl_fail(ret))
+		goto fail;
+
+	ret = get_chip_info(state);
+	if (mxl_fail(ret))
+		goto fail;
+
+	adap->fe_adap[fe_id].fe = dvb_attach(lg2160_attach,
+			      &hauppauge_lg2160_config,
+			      &adap->dev->i2c_adap);
+	if (adap->fe_adap[fe_id].fe) {
+		adap_state->fe_init = adap->fe_adap[fe_id].fe->ops.init;
+		adap->fe_adap[fe_id].fe->ops.init = mxl111sf_adap_fe_init;
+		adap_state->fe_sleep = adap->fe_adap[fe_id].fe->ops.sleep;
+		adap->fe_adap[fe_id].fe->ops.sleep = mxl111sf_adap_fe_sleep;
+		return 0;
+	}
+	ret = -EIO;
+fail:
+	return ret;
+}
+
+static struct lg2160_config hauppauge_lg2161_1019_config = {
+	.lg_chip            = LG2161_1019,
+	.i2c_addr           = 0x1c >> 1,
+	.deny_i2c_rptr      = 1,
+	.spectral_inversion = 0,
+	.if_khz             = 6000,
+	.output_if          = 2, /* LG2161_OIF_SPI_MAS */
+};
+
+static struct lg2160_config hauppauge_lg2161_1040_config = {
+	.lg_chip            = LG2161_1040,
+	.i2c_addr           = 0x1c >> 1,
+	.deny_i2c_rptr      = 1,
+	.spectral_inversion = 0,
+	.if_khz             = 6000,
+	.output_if          = 4, /* LG2161_OIF_SPI_MAS */
+};
+
+static int mxl111sf_lg2161_frontend_attach(struct dvb_usb_adapter *adap)
+{
+	struct dvb_usb_device *d = adap->dev;
+	struct mxl111sf_state *state = d->priv;
+	int fe_id = adap->num_frontends_initialized;
+	struct mxl111sf_adap_state *adap_state = adap->fe_adap[fe_id].priv;
+	int ret;
+
+	deb_adv("%s()\n", __func__);
+
+	/* save a pointer to the dvb_usb_device in device state */
+	state->d = d;
+	adap_state->alt_mode = (dvb_usb_mxl111sf_isoc) ? 2 : 1;
+	state->alt_mode = adap_state->alt_mode;
+
+	if (usb_set_interface(adap->dev->udev, 0, state->alt_mode) < 0)
+		err("set interface failed");
+
+	state->gpio_mode = MXL111SF_GPIO_MOD_MH;
+	adap_state->gpio_mode = state->gpio_mode;
+	adap_state->device_mode = MXL_TUNER_MODE;
+	adap_state->ep6_clockphase = 1;
+
+	ret = mxl1x1sf_soft_reset(state);
+	if (mxl_fail(ret))
+		goto fail;
+	ret = mxl111sf_init_tuner_demod(state);
+	if (mxl_fail(ret))
+		goto fail;
+
+	ret = mxl1x1sf_set_device_mode(state, adap_state->device_mode);
+	if (mxl_fail(ret))
+		goto fail;
+
+	ret = mxl111sf_enable_usb_output(state);
+	if (mxl_fail(ret))
+		goto fail;
+	ret = mxl1x1sf_top_master_ctrl(state, 1);
+	if (mxl_fail(ret))
+		goto fail;
+
+	ret = mxl111sf_init_port_expander(state);
+	if (mxl_fail(ret))
+		goto fail;
+	ret = mxl111sf_gpio_mode_switch(state, state->gpio_mode);
+	if (mxl_fail(ret))
+		goto fail;
+
+	ret = get_chip_info(state);
+	if (mxl_fail(ret))
+		goto fail;
+
+	adap->fe_adap[fe_id].fe = dvb_attach(lg2160_attach,
+			      (MXL111SF_V8_200 == state->chip_rev) ?
+			      &hauppauge_lg2161_1040_config :
+			      &hauppauge_lg2161_1019_config,
+			      &adap->dev->i2c_adap);
+	if (adap->fe_adap[fe_id].fe) {
+		adap_state->fe_init = adap->fe_adap[fe_id].fe->ops.init;
+		adap->fe_adap[fe_id].fe->ops.init = mxl111sf_adap_fe_init;
+		adap_state->fe_sleep = adap->fe_adap[fe_id].fe->ops.sleep;
+		adap->fe_adap[fe_id].fe->ops.sleep = mxl111sf_adap_fe_sleep;
+		return 0;
+	}
+	ret = -EIO;
+fail:
+	return ret;
+}
+
+static struct lg2160_config hauppauge_lg2161_1019_ep6_config = {
+	.lg_chip            = LG2161_1019,
+	.i2c_addr           = 0x1c >> 1,
+	.deny_i2c_rptr      = 1,
+	.spectral_inversion = 0,
+	.if_khz             = 6000,
+	.output_if          = 1, /* LG2161_OIF_SERIAL_TS */
+};
+
+static struct lg2160_config hauppauge_lg2161_1040_ep6_config = {
+	.lg_chip            = LG2161_1040,
+	.i2c_addr           = 0x1c >> 1,
+	.deny_i2c_rptr      = 1,
+	.spectral_inversion = 0,
+	.if_khz             = 6000,
+	.output_if          = 7, /* LG2161_OIF_SERIAL_TS */
+};
+
+static int mxl111sf_lg2161_ep6_frontend_attach(struct dvb_usb_adapter *adap)
+{
+	struct dvb_usb_device *d = adap->dev;
+	struct mxl111sf_state *state = d->priv;
+	int fe_id = adap->num_frontends_initialized;
+	struct mxl111sf_adap_state *adap_state = adap->fe_adap[fe_id].priv;
+	int ret;
+
+	deb_adv("%s()\n", __func__);
+
+	/* save a pointer to the dvb_usb_device in device state */
+	state->d = d;
+	adap_state->alt_mode = (dvb_usb_mxl111sf_isoc) ? 2 : 1;
+	state->alt_mode = adap_state->alt_mode;
+
+	if (usb_set_interface(adap->dev->udev, 0, state->alt_mode) < 0)
+		err("set interface failed");
+
+	state->gpio_mode = MXL111SF_GPIO_MOD_MH;
+	adap_state->gpio_mode = state->gpio_mode;
+	adap_state->device_mode = MXL_TUNER_MODE;
+	adap_state->ep6_clockphase = 0;
+
+	ret = mxl1x1sf_soft_reset(state);
+	if (mxl_fail(ret))
+		goto fail;
+	ret = mxl111sf_init_tuner_demod(state);
+	if (mxl_fail(ret))
+		goto fail;
+
+	ret = mxl1x1sf_set_device_mode(state, adap_state->device_mode);
+	if (mxl_fail(ret))
+		goto fail;
+
+	ret = mxl111sf_enable_usb_output(state);
+	if (mxl_fail(ret))
+		goto fail;
+	ret = mxl1x1sf_top_master_ctrl(state, 1);
+	if (mxl_fail(ret))
+		goto fail;
+
+	ret = mxl111sf_init_port_expander(state);
+	if (mxl_fail(ret))
+		goto fail;
+	ret = mxl111sf_gpio_mode_switch(state, state->gpio_mode);
+	if (mxl_fail(ret))
+		goto fail;
+
+	ret = get_chip_info(state);
+	if (mxl_fail(ret))
+		goto fail;
+
+	adap->fe_adap[fe_id].fe = dvb_attach(lg2160_attach,
+			      (MXL111SF_V8_200 == state->chip_rev) ?
+			      &hauppauge_lg2161_1040_ep6_config :
+			      &hauppauge_lg2161_1019_ep6_config,
+			      &adap->dev->i2c_adap);
+	if (adap->fe_adap[fe_id].fe) {
+		adap_state->fe_init = adap->fe_adap[fe_id].fe->ops.init;
+		adap->fe_adap[fe_id].fe->ops.init = mxl111sf_adap_fe_init;
+		adap_state->fe_sleep = adap->fe_adap[fe_id].fe->ops.sleep;
+		adap->fe_adap[fe_id].fe->ops.sleep = mxl111sf_adap_fe_sleep;
+		return 0;
+	}
+	ret = -EIO;
+fail:
+	return ret;
+}
+
 static struct mxl111sf_demod_config mxl_demod_config = {
 	.read_reg        = mxl111sf_read_reg,
 	.write_reg       = mxl111sf_write_reg,
@@ -650,6 +930,18 @@ static struct dvb_usb_device_properties mxl111sf_dvbt_bulk_properties;
 static struct dvb_usb_device_properties mxl111sf_dvbt_isoc_properties;
 static struct dvb_usb_device_properties mxl111sf_atsc_bulk_properties;
 static struct dvb_usb_device_properties mxl111sf_atsc_isoc_properties;
+static struct dvb_usb_device_properties mxl111sf_atsc_mh_bulk_properties;
+static struct dvb_usb_device_properties mxl111sf_atsc_mh_isoc_properties;
+static struct dvb_usb_device_properties mxl111sf_mh_bulk_properties;
+static struct dvb_usb_device_properties mxl111sf_mh_isoc_properties;
+static struct dvb_usb_device_properties mxl111sf_mercury_spi_bulk_properties;
+static struct dvb_usb_device_properties mxl111sf_mercury_spi_isoc_properties;
+static struct dvb_usb_device_properties mxl111sf_mercury_tp_bulk_properties;
+static struct dvb_usb_device_properties mxl111sf_mercury_tp_isoc_properties;
+static struct dvb_usb_device_properties mxl111sf_mercury_mh_spi_bulk_properties;
+static struct dvb_usb_device_properties mxl111sf_mercury_mh_spi_isoc_properties;
+static struct dvb_usb_device_properties mxl111sf_mercury_mh_tp_bulk_properties;
+static struct dvb_usb_device_properties mxl111sf_mercury_mh_tp_isoc_properties;
 
 static int mxl111sf_probe(struct usb_interface *intf,
 			  const struct usb_device_id *id)
@@ -664,12 +956,50 @@ static int mxl111sf_probe(struct usb_interface *intf,
 				       THIS_MODULE, &d, adapter_nr) ||
 	      0 == dvb_usb_device_init(intf,
 				       &mxl111sf_atsc_isoc_properties,
+				       THIS_MODULE, &d, adapter_nr) ||
+	      0 == dvb_usb_device_init(intf,
+				       &mxl111sf_atsc_mh_isoc_properties,
+				       THIS_MODULE, &d, adapter_nr) ||
+	      0 == dvb_usb_device_init(intf,
+				       &mxl111sf_mh_isoc_properties,
+				       THIS_MODULE, &d, adapter_nr) ||
+	      ((dvb_usb_mxl111sf_spi) &&
+	       (0 == dvb_usb_device_init(intf,
+					 &mxl111sf_mercury_spi_isoc_properties,
+					 THIS_MODULE, &d, adapter_nr) ||
+		0 == dvb_usb_device_init(intf,
+					 &mxl111sf_mercury_mh_spi_isoc_properties,
+					 THIS_MODULE, &d, adapter_nr))) ||
+	      0 == dvb_usb_device_init(intf,
+				       &mxl111sf_mercury_tp_isoc_properties,
+				       THIS_MODULE, &d, adapter_nr) ||
+	      0 == dvb_usb_device_init(intf,
+				       &mxl111sf_mercury_mh_tp_isoc_properties,
 				       THIS_MODULE, &d, adapter_nr))) ||
 	    0 == dvb_usb_device_init(intf,
 				     &mxl111sf_dvbt_bulk_properties,
 				     THIS_MODULE, &d, adapter_nr) ||
 	    0 == dvb_usb_device_init(intf,
 				     &mxl111sf_atsc_bulk_properties,
+				     THIS_MODULE, &d, adapter_nr) ||
+	    0 == dvb_usb_device_init(intf,
+				     &mxl111sf_atsc_mh_bulk_properties,
+				     THIS_MODULE, &d, adapter_nr) ||
+	    0 == dvb_usb_device_init(intf,
+				     &mxl111sf_mh_bulk_properties,
+				     THIS_MODULE, &d, adapter_nr) ||
+	    ((dvb_usb_mxl111sf_spi) &&
+	     (0 == dvb_usb_device_init(intf,
+				       &mxl111sf_mercury_spi_bulk_properties,
+				       THIS_MODULE, &d, adapter_nr) ||
+	      0 == dvb_usb_device_init(intf,
+				       &mxl111sf_mercury_mh_spi_bulk_properties,
+				       THIS_MODULE, &d, adapter_nr))) ||
+	    0 == dvb_usb_device_init(intf,
+				     &mxl111sf_mercury_tp_bulk_properties,
+				     THIS_MODULE, &d, adapter_nr) ||
+	    0 == dvb_usb_device_init(intf,
+				     &mxl111sf_mercury_mh_tp_bulk_properties,
 				     THIS_MODULE, &d, adapter_nr) || 0) {
 
 		struct mxl111sf_state *state = d->priv;
@@ -787,6 +1117,36 @@ MODULE_DEVICE_TABLE(usb, mxl111sf_table);
 		}					\
 	}
 
+#define MXL111SF_EP5_BULK_STREAMING_CONFIG		\
+	.size_of_priv = sizeof(struct mxl111sf_adap_state), \
+	.streaming_ctrl = mxl111sf_ep5_streaming_ctrl,	\
+	.stream = {					\
+		.type = USB_BULK,			\
+		.count = 5,				\
+		.endpoint = 0x05,			\
+		.u = {					\
+			.bulk = {			\
+				.buffersize = 8192,	\
+			}				\
+		}					\
+	}
+
+#define MXL111SF_EP5_ISOC_STREAMING_CONFIG		\
+	.size_of_priv = sizeof(struct mxl111sf_adap_state), \
+	.streaming_ctrl = mxl111sf_ep5_streaming_ctrl,	\
+	.stream = {					\
+		.type = USB_ISOC,			\
+		.count = 5,				\
+		.endpoint = 0x05,			\
+		.u = {					\
+			.isoc = {			\
+				.framesperurb = 96,	\
+				.framesize = 200,	\
+				.interval = 1,		\
+			}				\
+		}					\
+	}
+
 #define MXL111SF_EP6_BULK_STREAMING_CONFIG		\
 	.size_of_priv = sizeof(struct mxl111sf_adap_state), \
 	.streaming_ctrl = mxl111sf_ep6_streaming_ctrl,	\
@@ -848,7 +1208,7 @@ static struct dvb_usb_device_properties mxl111sf_dvbt_bulk_properties = {
 		} },
 		},
 	},
-	.num_device_descs = 4,
+	.num_device_descs = 3,
 	.devices = {
 		{   "Hauppauge 126xxx DVBT (bulk)",
 			{ NULL },
@@ -866,11 +1226,6 @@ static struct dvb_usb_device_properties mxl111sf_dvbt_bulk_properties = {
 			  &mxl111sf_table[24], &mxl111sf_table[26],
 			  NULL },
 		},
-		{   "Hauppauge 126xxx (tp-bulk)",
-			{ NULL },
-			{ &mxl111sf_table[28], &mxl111sf_table[30],
-			  NULL },
-		},
 	}
 };
 
@@ -890,7 +1245,7 @@ static struct dvb_usb_device_properties mxl111sf_dvbt_isoc_properties = {
 		} },
 		},
 	},
-	.num_device_descs = 4,
+	.num_device_descs = 3,
 	.devices = {
 		{   "Hauppauge 126xxx DVBT (isoc)",
 			{ NULL },
@@ -908,11 +1263,6 @@ static struct dvb_usb_device_properties mxl111sf_dvbt_isoc_properties = {
 			  &mxl111sf_table[24], &mxl111sf_table[26],
 			  NULL },
 		},
-		{   "Hauppauge 126xxx (tp-isoc)",
-			{ NULL },
-			{ &mxl111sf_table[28], &mxl111sf_table[30],
-			  NULL },
-		},
 	}
 };
 
@@ -923,33 +1273,159 @@ static struct dvb_usb_device_properties mxl111sf_atsc_bulk_properties = {
 	.adapter = {
 		{
 		.fe_ioctl_override = mxl111sf_fe_ioctl_override,
-		.num_frontends = 2,
+		.num_frontends = 1,
 		.fe = {{
 			.frontend_attach  = mxl111sf_lgdt3305_frontend_attach,
 			.tuner_attach     = mxl111sf_attach_tuner,
 
 			MXL111SF_EP6_BULK_STREAMING_CONFIG,
+		}},
 		},
+	},
+	.num_device_descs = 2,
+	.devices = {
+		{   "Hauppauge 126xxx ATSC (bulk)",
+			{ NULL },
+			{ &mxl111sf_table[1], &mxl111sf_table[5],
+			  NULL },
+		},
+		{   "Hauppauge 117xxx ATSC (bulk)",
+			{ NULL },
+			{ &mxl111sf_table[12],
+			  NULL },
+		},
+	}
+};
+
+static struct dvb_usb_device_properties mxl111sf_atsc_isoc_properties = {
+	MXL111SF_DEFAULT_DEVICE_PROPERTIES,
+
+	.num_adapters = 1,
+	.adapter = {
 		{
-			.frontend_attach  = mxl111sf_attach_demod,
+		.fe_ioctl_override = mxl111sf_fe_ioctl_override,
+		.num_frontends = 1,
+		.fe = {{
+			.frontend_attach  = mxl111sf_lgdt3305_frontend_attach,
 			.tuner_attach     = mxl111sf_attach_tuner,
 
-			MXL111SF_EP4_BULK_STREAMING_CONFIG,
+			MXL111SF_EP6_ISOC_STREAMING_CONFIG,
 		}},
 		},
 	},
-	.num_device_descs = 6,
+	.num_device_descs = 2,
 	.devices = {
-		{   "Hauppauge 126xxx ATSC (bulk)",
+		{   "Hauppauge 126xxx ATSC (isoc)",
 			{ NULL },
 			{ &mxl111sf_table[1], &mxl111sf_table[5],
 			  NULL },
 		},
-		{   "Hauppauge 117xxx ATSC (bulk)",
+		{   "Hauppauge 117xxx ATSC (isoc)",
 			{ NULL },
 			{ &mxl111sf_table[12],
 			  NULL },
 		},
+	}
+};
+
+static struct dvb_usb_device_properties mxl111sf_mh_bulk_properties = {
+	MXL111SF_DEFAULT_DEVICE_PROPERTIES,
+
+	.num_adapters = 1,
+	.adapter = {
+		{
+		.fe_ioctl_override = mxl111sf_fe_ioctl_override,
+		.num_frontends = 1,
+		.fe = {{
+			.caps = DVB_USB_ADAP_RECEIVES_RAW_PAYLOAD,
+
+			.frontend_attach  = mxl111sf_lg2160_frontend_attach,
+			.tuner_attach     = mxl111sf_attach_tuner,
+
+			MXL111SF_EP5_BULK_STREAMING_CONFIG,
+		}},
+		},
+	},
+	.num_device_descs = 2,
+	.devices = {
+		{   "HCW 126xxx (bulk)",
+			{ NULL },
+			{ &mxl111sf_table[2], &mxl111sf_table[6],
+			  NULL },
+		},
+		{   "HCW 117xxx (bulk)",
+			{ NULL },
+			{ &mxl111sf_table[13],
+			  NULL },
+		},
+	}
+};
+
+static struct dvb_usb_device_properties mxl111sf_mh_isoc_properties = {
+	MXL111SF_DEFAULT_DEVICE_PROPERTIES,
+
+	.num_adapters = 1,
+	.adapter = {
+		{
+		.fe_ioctl_override = mxl111sf_fe_ioctl_override,
+		.num_frontends = 1,
+		.fe = {{
+			.caps = DVB_USB_ADAP_RECEIVES_RAW_PAYLOAD,
+
+			.frontend_attach  = mxl111sf_lg2160_frontend_attach,
+			.tuner_attach     = mxl111sf_attach_tuner,
+
+			MXL111SF_EP5_ISOC_STREAMING_CONFIG,
+		}},
+		},
+	},
+	.num_device_descs = 2,
+	.devices = {
+		{   "HCW 126xxx (isoc)",
+			{ NULL },
+			{ &mxl111sf_table[2], &mxl111sf_table[6],
+			  NULL },
+		},
+		{   "HCW 117xxx (isoc)",
+			{ NULL },
+			{ &mxl111sf_table[13],
+			  NULL },
+		},
+	}
+};
+
+static struct dvb_usb_device_properties mxl111sf_atsc_mh_bulk_properties = {
+	MXL111SF_DEFAULT_DEVICE_PROPERTIES,
+
+	.num_adapters = 1,
+	.adapter = {
+		{
+		.fe_ioctl_override = mxl111sf_fe_ioctl_override,
+		.num_frontends = 3,
+		.fe = {{
+			.frontend_attach  = mxl111sf_lgdt3305_frontend_attach,
+			.tuner_attach     = mxl111sf_attach_tuner,
+
+			MXL111SF_EP6_BULK_STREAMING_CONFIG,
+		},
+		{
+			.frontend_attach  = mxl111sf_attach_demod,
+			.tuner_attach     = mxl111sf_attach_tuner,
+
+			MXL111SF_EP4_BULK_STREAMING_CONFIG,
+		},
+		{
+			.caps = DVB_USB_ADAP_RECEIVES_RAW_PAYLOAD,
+
+			.frontend_attach  = mxl111sf_lg2160_frontend_attach,
+			.tuner_attach     = mxl111sf_attach_tuner,
+
+			MXL111SF_EP5_BULK_STREAMING_CONFIG,
+		}},
+		},
+	},
+	.num_device_descs = 2,
+	.devices = {
 		{   "Hauppauge 126xxx ATSC+ (bulk)",
 			{ NULL },
 			{ &mxl111sf_table[0], &mxl111sf_table[3],
@@ -963,13 +1439,96 @@ static struct dvb_usb_device_properties mxl111sf_atsc_bulk_properties = {
 			  &mxl111sf_table[32], &mxl111sf_table[33],
 			  NULL },
 		},
-		{   "Hauppauge Mercury (tp-bulk)",
+	}
+};
+
+static struct dvb_usb_device_properties mxl111sf_atsc_mh_isoc_properties = {
+	MXL111SF_DEFAULT_DEVICE_PROPERTIES,
+
+	.num_adapters = 1,
+	.adapter = {
+		{
+		.fe_ioctl_override = mxl111sf_fe_ioctl_override,
+		.num_frontends = 3,
+		.fe = {{
+			.frontend_attach  = mxl111sf_lgdt3305_frontend_attach,
+			.tuner_attach     = mxl111sf_attach_tuner,
+
+			MXL111SF_EP6_ISOC_STREAMING_CONFIG,
+		},
+		{
+			.frontend_attach  = mxl111sf_attach_demod,
+			.tuner_attach     = mxl111sf_attach_tuner,
+
+			MXL111SF_EP4_ISOC_STREAMING_CONFIG,
+		},
+		{
+			.caps = DVB_USB_ADAP_RECEIVES_RAW_PAYLOAD,
+
+			.frontend_attach  = mxl111sf_lg2160_frontend_attach,
+			.tuner_attach     = mxl111sf_attach_tuner,
+
+			MXL111SF_EP5_ISOC_STREAMING_CONFIG,
+		}},
+		},
+	},
+	.num_device_descs = 2,
+	.devices = {
+		{   "Hauppauge 126xxx ATSC+ (isoc)",
+			{ NULL },
+			{ &mxl111sf_table[0], &mxl111sf_table[3],
+			  &mxl111sf_table[7], &mxl111sf_table[9],
+			  &mxl111sf_table[10], NULL },
+		},
+		{   "Hauppauge 117xxx ATSC+ (isoc)",
+			{ NULL },
+			{ &mxl111sf_table[11], &mxl111sf_table[14],
+			  &mxl111sf_table[16], &mxl111sf_table[17],
+			  &mxl111sf_table[32], &mxl111sf_table[33],
+			  NULL },
+		},
+	}
+};
+
+static struct dvb_usb_device_properties mxl111sf_mercury_spi_bulk_properties = {
+	MXL111SF_DEFAULT_DEVICE_PROPERTIES,
+
+	.num_adapters = 1,
+	.adapter = {
+		{
+		.fe_ioctl_override = mxl111sf_fe_ioctl_override,
+		.num_frontends = 3,
+		.fe = {{
+			.frontend_attach  = mxl111sf_lgdt3305_frontend_attach,
+			.tuner_attach     = mxl111sf_attach_tuner,
+
+			MXL111SF_EP6_BULK_STREAMING_CONFIG,
+		},
+		{
+			.frontend_attach  = mxl111sf_attach_demod,
+			.tuner_attach     = mxl111sf_attach_tuner,
+
+			MXL111SF_EP4_BULK_STREAMING_CONFIG,
+		},
+		{
+			.caps = DVB_USB_ADAP_RECEIVES_RAW_PAYLOAD,
+
+			.frontend_attach  = mxl111sf_lg2161_frontend_attach,
+			.tuner_attach     = mxl111sf_attach_tuner,
+
+			MXL111SF_EP5_BULK_STREAMING_CONFIG,
+		}},
+		},
+	},
+	.num_device_descs = 2,
+	.devices = {
+		{   "Hauppauge Mercury (spi-bulk)",
 			{ NULL },
 			{ &mxl111sf_table[19], &mxl111sf_table[21],
 			  &mxl111sf_table[23], &mxl111sf_table[25],
-			  &mxl111sf_table[27], NULL },
+			  NULL },
 		},
-		{   "Hauppauge WinTV-Aero-M",
+		{   "Hauppauge WinTV-Aero-M (spi-bulk)",
 			{ NULL },
 			{ &mxl111sf_table[29], &mxl111sf_table[31],
 			  NULL },
@@ -977,14 +1536,14 @@ static struct dvb_usb_device_properties mxl111sf_atsc_bulk_properties = {
 	}
 };
 
-static struct dvb_usb_device_properties mxl111sf_atsc_isoc_properties = {
+static struct dvb_usb_device_properties mxl111sf_mercury_spi_isoc_properties = {
 	MXL111SF_DEFAULT_DEVICE_PROPERTIES,
 
 	.num_adapters = 1,
 	.adapter = {
 		{
 		.fe_ioctl_override = mxl111sf_fe_ioctl_override,
-		.num_frontends = 2,
+		.num_frontends = 3,
 		.fe = {{
 			.frontend_attach  = mxl111sf_lgdt3305_frontend_attach,
 			.tuner_attach     = mxl111sf_attach_tuner,
@@ -996,34 +1555,111 @@ static struct dvb_usb_device_properties mxl111sf_atsc_isoc_properties = {
 			.tuner_attach     = mxl111sf_attach_tuner,
 
 			MXL111SF_EP4_ISOC_STREAMING_CONFIG,
+		},
+		{
+			.caps = DVB_USB_ADAP_RECEIVES_RAW_PAYLOAD,
+
+			.frontend_attach  = mxl111sf_lg2161_frontend_attach,
+			.tuner_attach     = mxl111sf_attach_tuner,
+
+			MXL111SF_EP5_ISOC_STREAMING_CONFIG,
 		}},
 		},
 	},
-	.num_device_descs = 6,
+	.num_device_descs = 2,
 	.devices = {
-		{   "Hauppauge 126xxx ATSC (isoc)",
+		{   "Hauppauge Mercury (spi-isoc)",
 			{ NULL },
-			{ &mxl111sf_table[1], &mxl111sf_table[5],
+			{ &mxl111sf_table[19], &mxl111sf_table[21],
+			  &mxl111sf_table[23], &mxl111sf_table[25],
 			  NULL },
 		},
-		{   "Hauppauge 117xxx ATSC (isoc)",
+		{   "Hauppauge WinTV-Aero-M (spi-isoc)",
 			{ NULL },
-			{ &mxl111sf_table[12],
+			{ &mxl111sf_table[29], &mxl111sf_table[31],
 			  NULL },
 		},
-		{   "Hauppauge 126xxx ATSC+ (isoc)",
+	}
+};
+
+static struct dvb_usb_device_properties mxl111sf_mercury_tp_bulk_properties = {
+	MXL111SF_DEFAULT_DEVICE_PROPERTIES,
+
+	.num_adapters = 1,
+	.adapter = {
+		{
+		.fe_ioctl_override = mxl111sf_fe_ioctl_override,
+		.num_frontends = 3,
+		.fe = {{
+			.frontend_attach  = mxl111sf_lgdt3305_frontend_attach,
+			.tuner_attach     = mxl111sf_attach_tuner,
+
+			MXL111SF_EP6_BULK_STREAMING_CONFIG,
+		},
+		{
+			.frontend_attach  = mxl111sf_attach_demod,
+			.tuner_attach     = mxl111sf_attach_tuner,
+
+			MXL111SF_EP4_BULK_STREAMING_CONFIG,
+		},
+		{
+			.caps = DVB_USB_ADAP_RECEIVES_RAW_PAYLOAD,
+
+			.frontend_attach  = mxl111sf_lg2161_ep6_frontend_attach,
+			.tuner_attach     = mxl111sf_attach_tuner,
+
+			MXL111SF_EP6_BULK_STREAMING_CONFIG,
+		}},
+		},
+	},
+	.num_device_descs = 2,
+	.devices = {
+		{   "Hauppauge Mercury (tp-bulk)",
 			{ NULL },
-			{ &mxl111sf_table[0], &mxl111sf_table[3],
-			  &mxl111sf_table[7], &mxl111sf_table[9],
-			  &mxl111sf_table[10], NULL },
+			{ &mxl111sf_table[19], &mxl111sf_table[21],
+			  &mxl111sf_table[23], &mxl111sf_table[25],
+			  &mxl111sf_table[27], NULL },
 		},
-		{   "Hauppauge 117xxx ATSC+ (isoc)",
+		{   "Hauppauge WinTV-Aero-M",
 			{ NULL },
-			{ &mxl111sf_table[11], &mxl111sf_table[14],
-			  &mxl111sf_table[16], &mxl111sf_table[17],
-			  &mxl111sf_table[32], &mxl111sf_table[33],
+			{ &mxl111sf_table[29], &mxl111sf_table[31],
 			  NULL },
 		},
+	}
+};
+
+static struct dvb_usb_device_properties mxl111sf_mercury_tp_isoc_properties = {
+	MXL111SF_DEFAULT_DEVICE_PROPERTIES,
+
+	.num_adapters = 1,
+	.adapter = {
+		{
+		.fe_ioctl_override = mxl111sf_fe_ioctl_override,
+		.num_frontends = 3,
+		.fe = {{
+			.frontend_attach  = mxl111sf_lgdt3305_frontend_attach,
+			.tuner_attach     = mxl111sf_attach_tuner,
+
+			MXL111SF_EP6_ISOC_STREAMING_CONFIG,
+		},
+		{
+			.frontend_attach  = mxl111sf_attach_demod,
+			.tuner_attach     = mxl111sf_attach_tuner,
+
+			MXL111SF_EP4_ISOC_STREAMING_CONFIG,
+		},
+		{
+			.caps = DVB_USB_ADAP_RECEIVES_RAW_PAYLOAD,
+
+			.frontend_attach  = mxl111sf_lg2161_ep6_frontend_attach,
+			.tuner_attach     = mxl111sf_attach_tuner,
+
+			MXL111SF_EP6_ISOC_STREAMING_CONFIG,
+		}},
+		},
+	},
+	.num_device_descs = 2,
+	.devices = {
 		{   "Hauppauge Mercury (tp-isoc)",
 			{ NULL },
 			{ &mxl111sf_table[19], &mxl111sf_table[21],
@@ -1038,6 +1674,146 @@ static struct dvb_usb_device_properties mxl111sf_atsc_isoc_properties = {
 	}
 };
 
+static
+struct dvb_usb_device_properties mxl111sf_mercury_mh_tp_bulk_properties = {
+	MXL111SF_DEFAULT_DEVICE_PROPERTIES,
+
+	.num_adapters = 1,
+	.adapter = {
+		{
+		.fe_ioctl_override = mxl111sf_fe_ioctl_override,
+		.num_frontends = 2,
+		.fe = {{
+			.frontend_attach  = mxl111sf_attach_demod,
+			.tuner_attach     = mxl111sf_attach_tuner,
+
+			MXL111SF_EP4_BULK_STREAMING_CONFIG,
+		},
+		{
+			.caps = DVB_USB_ADAP_RECEIVES_RAW_PAYLOAD,
+
+			.frontend_attach  = mxl111sf_lg2161_ep6_frontend_attach,
+			.tuner_attach     = mxl111sf_attach_tuner,
+
+			MXL111SF_EP6_BULK_STREAMING_CONFIG,
+		}},
+		},
+	},
+	.num_device_descs = 1,
+	.devices = {
+		{   "Hauppauge 126xxx (tp-bulk)",
+			{ NULL },
+			{ &mxl111sf_table[28], &mxl111sf_table[30],
+			  NULL },
+		},
+	}
+};
+
+static
+struct dvb_usb_device_properties mxl111sf_mercury_mh_tp_isoc_properties = {
+	MXL111SF_DEFAULT_DEVICE_PROPERTIES,
+
+	.num_adapters = 1,
+	.adapter = {
+		{
+		.fe_ioctl_override = mxl111sf_fe_ioctl_override,
+		.num_frontends = 2,
+		.fe = {{
+			.frontend_attach  = mxl111sf_attach_demod,
+			.tuner_attach     = mxl111sf_attach_tuner,
+
+			MXL111SF_EP4_ISOC_STREAMING_CONFIG,
+		},
+		{
+			.caps = DVB_USB_ADAP_RECEIVES_RAW_PAYLOAD,
+
+			.frontend_attach  = mxl111sf_lg2161_ep6_frontend_attach,
+			.tuner_attach     = mxl111sf_attach_tuner,
+
+			MXL111SF_EP6_ISOC_STREAMING_CONFIG,
+		}},
+		},
+	},
+	.num_device_descs = 1,
+	.devices = {
+		{   "Hauppauge 126xxx (tp-isoc)",
+			{ NULL },
+			{ &mxl111sf_table[28], &mxl111sf_table[30],
+			  NULL },
+		},
+	}
+};
+
+static
+struct dvb_usb_device_properties mxl111sf_mercury_mh_spi_bulk_properties = {
+	MXL111SF_DEFAULT_DEVICE_PROPERTIES,
+
+	.num_adapters = 1,
+	.adapter = {
+		{
+		.fe_ioctl_override = mxl111sf_fe_ioctl_override,
+		.num_frontends = 2,
+		.fe = {{
+			.frontend_attach  = mxl111sf_attach_demod,
+			.tuner_attach     = mxl111sf_attach_tuner,
+
+			MXL111SF_EP4_BULK_STREAMING_CONFIG,
+		},
+		{
+			.caps = DVB_USB_ADAP_RECEIVES_RAW_PAYLOAD,
+
+			.frontend_attach  = mxl111sf_lg2161_frontend_attach,
+			.tuner_attach     = mxl111sf_attach_tuner,
+
+			MXL111SF_EP5_BULK_STREAMING_CONFIG,
+		}},
+		},
+	},
+	.num_device_descs = 1,
+	.devices = {
+		{   "Hauppauge 126xxx (spi-bulk)",
+			{ NULL },
+			{ &mxl111sf_table[28], &mxl111sf_table[30],
+			  NULL },
+		},
+	}
+};
+
+static
+struct dvb_usb_device_properties mxl111sf_mercury_mh_spi_isoc_properties = {
+	MXL111SF_DEFAULT_DEVICE_PROPERTIES,
+
+	.num_adapters = 1,
+	.adapter = {
+		{
+		.fe_ioctl_override = mxl111sf_fe_ioctl_override,
+		.num_frontends = 2,
+		.fe = {{
+			.frontend_attach  = mxl111sf_attach_demod,
+			.tuner_attach     = mxl111sf_attach_tuner,
+
+			MXL111SF_EP4_ISOC_STREAMING_CONFIG,
+		},
+		{
+			.caps = DVB_USB_ADAP_RECEIVES_RAW_PAYLOAD,
+
+			.frontend_attach  = mxl111sf_lg2161_frontend_attach,
+			.tuner_attach     = mxl111sf_attach_tuner,
+
+			MXL111SF_EP5_ISOC_STREAMING_CONFIG,
+		}},
+		},
+	},
+	.num_device_descs = 1,
+	.devices = {
+		{   "Hauppauge 126xxx (spi-isoc)",
+			{ NULL },
+			{ &mxl111sf_table[28], &mxl111sf_table[30],
+			  NULL },
+		},
+	}
+};
+
 static struct usb_driver mxl111sf_driver = {
 	.name		= "dvb_usb_mxl111sf",
 	.probe		= mxl111sf_probe,
diff --git a/drivers/media/dvb/dvb-usb/rtl28xxu.c b/drivers/media/dvb/dvb-usb/rtl28xxu.c
index 8f4736a10fc..41e1f5537f4 100644
--- a/drivers/media/dvb/dvb-usb/rtl28xxu.c
+++ b/drivers/media/dvb/dvb-usb/rtl28xxu.c
@@ -322,6 +322,9 @@ static int rtl2831u_frontend_attach(struct dvb_usb_adapter *adap)
 	 * since there is some demod params needed to set according to tuner.
 	 */
 
+	/* demod needs some time to wake up */
+	msleep(20);
+
 	/* open demod I2C gate */
 	ret = rtl28xxu_ctrl_msg(adap->dev, &req_gate);
 	if (ret)
@@ -909,6 +912,8 @@ static int rtl28xxu_probe(struct usb_interface *intf,
 	int ret, i;
 	int properties_count = ARRAY_SIZE(rtl28xxu_properties);
 	struct dvb_usb_device *d;
+	struct usb_device *udev;
+	bool found;
 
 	deb_info("%s: interface=%d\n", __func__,
 		intf->cur_altsetting->desc.bInterfaceNumber);
@@ -916,6 +921,29 @@ static int rtl28xxu_probe(struct usb_interface *intf,
 	if (intf->cur_altsetting->desc.bInterfaceNumber != 0)
 		return 0;
 
+	/* Dynamic USB ID support. Replaces first device ID with current one .*/
+	udev = interface_to_usbdev(intf);
+
+	for (i = 0, found = false; i < ARRAY_SIZE(rtl28xxu_table) - 1; i++) {
+		if (rtl28xxu_table[i].idVendor ==
+				le16_to_cpu(udev->descriptor.idVendor) &&
+				rtl28xxu_table[i].idProduct ==
+				le16_to_cpu(udev->descriptor.idProduct)) {
+			found = true;
+			break;
+		}
+	}
+
+	if (!found) {
+		deb_info("%s: using dynamic ID %04x:%04x\n", __func__,
+				le16_to_cpu(udev->descriptor.idVendor),
+				le16_to_cpu(udev->descriptor.idProduct));
+		rtl28xxu_properties[0].devices[0].warm_ids[0]->idVendor =
+				le16_to_cpu(udev->descriptor.idVendor);
+		rtl28xxu_properties[0].devices[0].warm_ids[0]->idProduct =
+				le16_to_cpu(udev->descriptor.idProduct);
+	}
+
 	for (i = 0; i < properties_count; i++) {
 		ret = dvb_usb_device_init(intf, &rtl28xxu_properties[i],
 				THIS_MODULE, &d, adapter_nr);
diff --git a/drivers/media/dvb/frontends/Kconfig b/drivers/media/dvb/frontends/Kconfig
index 21246707fbf..b98ebb264e2 100644
--- a/drivers/media/dvb/frontends/Kconfig
+++ b/drivers/media/dvb/frontends/Kconfig
@@ -531,6 +531,14 @@ config DVB_LGDT3305
 	  An ATSC 8VSB and QAM64/256 tuner module. Say Y when you want
 	  to support this frontend.
 
+config DVB_LG2160
+	tristate "LG Electronics LG216x based"
+	depends on DVB_CORE && I2C
+	default m if DVB_FE_CUSTOMISE
+	help
+	  An ATSC/MH demodulator module. Say Y when you want
+	  to support this frontend.
+
 config DVB_S5H1409
 	tristate "Samsung S5H1409 based"
 	depends on DVB_CORE && I2C
@@ -540,12 +548,26 @@ config DVB_S5H1409
 	  to support this frontend.
 
 config DVB_AU8522
-	tristate "Auvitek AU8522 based"
-	depends on DVB_CORE && I2C && VIDEO_V4L2
+	depends on I2C
+	tristate
+
+config DVB_AU8522_DTV
+	tristate "Auvitek AU8522 based DTV demod"
+	depends on DVB_CORE && I2C
+	select DVB_AU8522
 	default m if DVB_FE_CUSTOMISE
 	help
-	  An ATSC 8VSB and QAM64/256 tuner module. Say Y when you want
-	  to support this frontend.
+	  An ATSC 8VSB, QAM64/256 & NTSC demodulator module. Say Y when
+	  you want to enable DTV demodulation support for this frontend.
+
+config DVB_AU8522_V4L
+	tristate "Auvitek AU8522 based ATV demod"
+	depends on VIDEO_V4L2 && I2C
+	select DVB_AU8522
+	default m if DVB_FE_CUSTOMISE
+	help
+	  An ATSC 8VSB, QAM64/256 & NTSC demodulator module. Say Y when
+	  you want to enable ATV demodulation support for this frontend.
 
 config DVB_S5H1411
 	tristate "Samsung S5H1411 based"
@@ -713,6 +735,11 @@ config DVB_M88RS2000
 	  A DVB-S tuner module.
 	  Say Y when you want to support this frontend.
 
+config DVB_AF9033
+	tristate "Afatech AF9033 DVB-T demodulator"
+	depends on DVB_CORE && I2C
+	default m if DVB_FE_CUSTOMISE
+
 comment "Tools to develop new frontends"
 
 config DVB_DUMMY_FE
diff --git a/drivers/media/dvb/frontends/Makefile b/drivers/media/dvb/frontends/Makefile
index 86fa808bf58..cd1ac2fd577 100644
--- a/drivers/media/dvb/frontends/Makefile
+++ b/drivers/media/dvb/frontends/Makefile
@@ -7,7 +7,6 @@ ccflags-y += -I$(srctree)/drivers/media/common/tuners/
 
 stb0899-objs = stb0899_drv.o stb0899_algo.o
 stv0900-objs = stv0900_core.o stv0900_sw.o
-au8522-objs = au8522_dig.o au8522_decoder.o
 drxd-objs = drxd_firm.o drxd_hard.o
 cxd2820r-objs = cxd2820r_core.o cxd2820r_c.o cxd2820r_t.o cxd2820r_t2.o
 drxk-objs := drxk_hard.o
@@ -50,6 +49,7 @@ obj-$(CONFIG_DVB_BCM3510) += bcm3510.o
 obj-$(CONFIG_DVB_S5H1420) += s5h1420.o
 obj-$(CONFIG_DVB_LGDT330X) += lgdt330x.o
 obj-$(CONFIG_DVB_LGDT3305) += lgdt3305.o
+obj-$(CONFIG_DVB_LG2160) += lg2160.o
 obj-$(CONFIG_DVB_CX24123) += cx24123.o
 obj-$(CONFIG_DVB_LNBP21) += lnbp21.o
 obj-$(CONFIG_DVB_LNBP22) += lnbp22.o
@@ -63,7 +63,9 @@ obj-$(CONFIG_DVB_TUNER_DIB0090) += dib0090.o
 obj-$(CONFIG_DVB_TUA6100) += tua6100.o
 obj-$(CONFIG_DVB_S5H1409) += s5h1409.o
 obj-$(CONFIG_DVB_TUNER_ITD1000) += itd1000.o
-obj-$(CONFIG_DVB_AU8522) += au8522.o
+obj-$(CONFIG_DVB_AU8522) += au8522_common.o
+obj-$(CONFIG_DVB_AU8522_DTV) += au8522_dig.o
+obj-$(CONFIG_DVB_AU8522_V4L) += au8522_decoder.o
 obj-$(CONFIG_DVB_TDA10048) += tda10048.o
 obj-$(CONFIG_DVB_TUNER_CX24113) += cx24113.o
 obj-$(CONFIG_DVB_S5H1411) += s5h1411.o
@@ -98,4 +100,5 @@ obj-$(CONFIG_DVB_A8293) += a8293.o
 obj-$(CONFIG_DVB_TDA10071) += tda10071.o
 obj-$(CONFIG_DVB_RTL2830) += rtl2830.o
 obj-$(CONFIG_DVB_M88RS2000) += m88rs2000.o
+obj-$(CONFIG_DVB_AF9033) += af9033.o
 
diff --git a/drivers/media/dvb/frontends/af9013.c b/drivers/media/dvb/frontends/af9013.c
index 6bcbcf543b3..5bc570d7784 100644
--- a/drivers/media/dvb/frontends/af9013.c
+++ b/drivers/media/dvb/frontends/af9013.c
@@ -514,7 +514,6 @@ err:
 
 static void af9013_statistics_work(struct work_struct *work)
 {
-	int ret;
 	struct af9013_state *state = container_of(work,
 		struct af9013_state, statistics_work.work);
 	unsigned int next_msec;
@@ -530,27 +529,27 @@ static void af9013_statistics_work(struct work_struct *work)
 	default:
 		state->statistics_step = 0;
 	case 0:
-		ret = af9013_statistics_signal_strength(&state->fe);
+		af9013_statistics_signal_strength(&state->fe);
 		state->statistics_step++;
 		next_msec = 300;
 		break;
 	case 1:
-		ret = af9013_statistics_snr_start(&state->fe);
+		af9013_statistics_snr_start(&state->fe);
 		state->statistics_step++;
 		next_msec = 200;
 		break;
 	case 2:
-		ret = af9013_statistics_ber_unc_start(&state->fe);
+		af9013_statistics_ber_unc_start(&state->fe);
 		state->statistics_step++;
 		next_msec = 1000;
 		break;
 	case 3:
-		ret = af9013_statistics_snr_result(&state->fe);
+		af9013_statistics_snr_result(&state->fe);
 		state->statistics_step++;
 		next_msec = 400;
 		break;
 	case 4:
-		ret = af9013_statistics_ber_unc_result(&state->fe);
+		af9013_statistics_ber_unc_result(&state->fe);
 		state->statistics_step++;
 		next_msec = 100;
 		break;
@@ -558,8 +557,6 @@ static void af9013_statistics_work(struct work_struct *work)
 
 	schedule_delayed_work(&state->statistics_work,
 		msecs_to_jiffies(next_msec));
-
-	return;
 }
 
 static int af9013_get_tune_settings(struct dvb_frontend *fe,
diff --git a/drivers/media/dvb/frontends/af9033.c b/drivers/media/dvb/frontends/af9033.c
new file mode 100644
index 00000000000..a3899828626
--- /dev/null
+++ b/drivers/media/dvb/frontends/af9033.c
@@ -0,0 +1,980 @@
+/*
+ * Afatech AF9033 demodulator driver
+ *
+ * Copyright (C) 2009 Antti Palosaari <crope@iki.fi>
+ * 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.
+ */
+
+#include "af9033_priv.h"
+
+struct af9033_state {
+	struct i2c_adapter *i2c;
+	struct dvb_frontend fe;
+	struct af9033_config cfg;
+
+	u32 bandwidth_hz;
+	bool ts_mode_parallel;
+	bool ts_mode_serial;
+
+	u32 ber;
+	u32 ucb;
+	unsigned long last_stat_check;
+};
+
+/* write multiple registers */
+static int af9033_wr_regs(struct af9033_state *state, u32 reg, const u8 *val,
+		int len)
+{
+	int ret;
+	u8 buf[3 + len];
+	struct i2c_msg msg[1] = {
+		{
+			.addr = state->cfg.i2c_addr,
+			.flags = 0,
+			.len = sizeof(buf),
+			.buf = buf,
+		}
+	};
+
+	buf[0] = (reg >> 16) & 0xff;
+	buf[1] = (reg >>  8) & 0xff;
+	buf[2] = (reg >>  0) & 0xff;
+	memcpy(&buf[3], val, len);
+
+	ret = i2c_transfer(state->i2c, msg, 1);
+	if (ret == 1) {
+		ret = 0;
+	} else {
+		printk(KERN_WARNING "%s: i2c wr failed=%d reg=%06x len=%d\n",
+				__func__, ret, reg, len);
+		ret = -EREMOTEIO;
+	}
+
+	return ret;
+}
+
+/* read multiple registers */
+static int af9033_rd_regs(struct af9033_state *state, u32 reg, u8 *val, int len)
+{
+	int ret;
+	u8 buf[3] = { (reg >> 16) & 0xff, (reg >> 8) & 0xff,
+			(reg >> 0) & 0xff };
+	struct i2c_msg msg[2] = {
+		{
+			.addr = state->cfg.i2c_addr,
+			.flags = 0,
+			.len = sizeof(buf),
+			.buf = buf
+		}, {
+			.addr = state->cfg.i2c_addr,
+			.flags = I2C_M_RD,
+			.len = len,
+			.buf = val
+		}
+	};
+
+	ret = i2c_transfer(state->i2c, msg, 2);
+	if (ret == 2) {
+		ret = 0;
+	} else {
+		printk(KERN_WARNING "%s: i2c rd failed=%d reg=%06x len=%d\n",
+				__func__, ret, reg, len);
+		ret = -EREMOTEIO;
+	}
+
+	return ret;
+}
+
+
+/* write single register */
+static int af9033_wr_reg(struct af9033_state *state, u32 reg, u8 val)
+{
+	return af9033_wr_regs(state, reg, &val, 1);
+}
+
+/* read single register */
+static int af9033_rd_reg(struct af9033_state *state, u32 reg, u8 *val)
+{
+	return af9033_rd_regs(state, reg, val, 1);
+}
+
+/* write single register with mask */
+static int af9033_wr_reg_mask(struct af9033_state *state, u32 reg, u8 val,
+		u8 mask)
+{
+	int ret;
+	u8 tmp;
+
+	/* no need for read if whole reg is written */
+	if (mask != 0xff) {
+		ret = af9033_rd_regs(state, reg, &tmp, 1);
+		if (ret)
+			return ret;
+
+		val &= mask;
+		tmp &= ~mask;
+		val |= tmp;
+	}
+
+	return af9033_wr_regs(state, reg, &val, 1);
+}
+
+/* read single register with mask */
+static int af9033_rd_reg_mask(struct af9033_state *state, u32 reg, u8 *val,
+		u8 mask)
+{
+	int ret, i;
+	u8 tmp;
+
+	ret = af9033_rd_regs(state, reg, &tmp, 1);
+	if (ret)
+		return ret;
+
+	tmp &= mask;
+
+	/* find position of the first bit */
+	for (i = 0; i < 8; i++) {
+		if ((mask >> i) & 0x01)
+			break;
+	}
+	*val = tmp >> i;
+
+	return 0;
+}
+
+static u32 af9033_div(u32 a, u32 b, u32 x)
+{
+	u32 r = 0, c = 0, i;
+
+	pr_debug("%s: a=%d b=%d x=%d\n", __func__, a, b, x);
+
+	if (a > b) {
+		c = a / b;
+		a = a - c * b;
+	}
+
+	for (i = 0; i < x; i++) {
+		if (a >= b) {
+			r += 1;
+			a -= b;
+		}
+		a <<= 1;
+		r <<= 1;
+	}
+	r = (c << (u32)x) + r;
+
+	pr_debug("%s: a=%d b=%d x=%d r=%d r=%x\n", __func__, a, b, x, r, r);
+
+	return r;
+}
+
+static void af9033_release(struct dvb_frontend *fe)
+{
+	struct af9033_state *state = fe->demodulator_priv;
+
+	kfree(state);
+}
+
+static int af9033_init(struct dvb_frontend *fe)
+{
+	struct af9033_state *state = fe->demodulator_priv;
+	int ret, i, len;
+	const struct reg_val *init;
+	u8 buf[4];
+	u32 adc_cw, clock_cw;
+	struct reg_val_mask tab[] = {
+		{ 0x80fb24, 0x00, 0x08 },
+		{ 0x80004c, 0x00, 0xff },
+		{ 0x00f641, state->cfg.tuner, 0xff },
+		{ 0x80f5ca, 0x01, 0x01 },
+		{ 0x80f715, 0x01, 0x01 },
+		{ 0x00f41f, 0x04, 0x04 },
+		{ 0x00f41a, 0x01, 0x01 },
+		{ 0x80f731, 0x00, 0x01 },
+		{ 0x00d91e, 0x00, 0x01 },
+		{ 0x00d919, 0x00, 0x01 },
+		{ 0x80f732, 0x00, 0x01 },
+		{ 0x00d91f, 0x00, 0x01 },
+		{ 0x00d91a, 0x00, 0x01 },
+		{ 0x80f730, 0x00, 0x01 },
+		{ 0x80f778, 0x00, 0xff },
+		{ 0x80f73c, 0x01, 0x01 },
+		{ 0x80f776, 0x00, 0x01 },
+		{ 0x00d8fd, 0x01, 0xff },
+		{ 0x00d830, 0x01, 0xff },
+		{ 0x00d831, 0x00, 0xff },
+		{ 0x00d832, 0x00, 0xff },
+		{ 0x80f985, state->ts_mode_serial, 0x01 },
+		{ 0x80f986, state->ts_mode_parallel, 0x01 },
+		{ 0x00d827, 0x00, 0xff },
+		{ 0x00d829, 0x00, 0xff },
+	};
+
+	/* program clock control */
+	clock_cw = af9033_div(state->cfg.clock, 1000000ul, 19ul);
+	buf[0] = (clock_cw >>  0) & 0xff;
+	buf[1] = (clock_cw >>  8) & 0xff;
+	buf[2] = (clock_cw >> 16) & 0xff;
+	buf[3] = (clock_cw >> 24) & 0xff;
+
+	pr_debug("%s: clock=%d clock_cw=%08x\n", __func__, state->cfg.clock,
+			clock_cw);
+
+	ret = af9033_wr_regs(state, 0x800025, buf, 4);
+	if (ret < 0)
+		goto err;
+
+	/* program ADC control */
+	for (i = 0; i < ARRAY_SIZE(clock_adc_lut); i++) {
+		if (clock_adc_lut[i].clock == state->cfg.clock)
+			break;
+	}
+
+	adc_cw = af9033_div(clock_adc_lut[i].adc, 1000000ul, 19ul);
+	buf[0] = (adc_cw >>  0) & 0xff;
+	buf[1] = (adc_cw >>  8) & 0xff;
+	buf[2] = (adc_cw >> 16) & 0xff;
+
+	pr_debug("%s: adc=%d adc_cw=%06x\n", __func__, clock_adc_lut[i].adc,
+			adc_cw);
+
+	ret = af9033_wr_regs(state, 0x80f1cd, buf, 3);
+	if (ret < 0)
+		goto err;
+
+	/* program register table */
+	for (i = 0; i < ARRAY_SIZE(tab); i++) {
+		ret = af9033_wr_reg_mask(state, tab[i].reg, tab[i].val,
+				tab[i].mask);
+		if (ret < 0)
+			goto err;
+	}
+
+	/* settings for TS interface */
+	if (state->cfg.ts_mode == AF9033_TS_MODE_USB) {
+		ret = af9033_wr_reg_mask(state, 0x80f9a5, 0x00, 0x01);
+		if (ret < 0)
+			goto err;
+
+		ret = af9033_wr_reg_mask(state, 0x80f9b5, 0x01, 0x01);
+		if (ret < 0)
+			goto err;
+	} else {
+		ret = af9033_wr_reg_mask(state, 0x80f990, 0x00, 0x01);
+		if (ret < 0)
+			goto err;
+
+		ret = af9033_wr_reg_mask(state, 0x80f9b5, 0x00, 0x01);
+		if (ret < 0)
+			goto err;
+	}
+
+	/* load OFSM settings */
+	pr_debug("%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;
+	}
+
+	/* load tuner specific settings */
+	pr_debug("%s: load tuner specific settings\n",
+			__func__);
+	switch (state->cfg.tuner) {
+	case AF9033_TUNER_TUA9001:
+		len = ARRAY_SIZE(tuner_init_tua9001);
+		init = tuner_init_tua9001;
+		break;
+	case AF9033_TUNER_FC0011:
+		len = ARRAY_SIZE(tuner_init_fc0011);
+		init = tuner_init_fc0011;
+		break;
+	case AF9033_TUNER_MXL5007T:
+		len = ARRAY_SIZE(tuner_init_mxl5007t);
+		init = tuner_init_mxl5007t;
+		break;
+	case AF9033_TUNER_TDA18218:
+		len = ARRAY_SIZE(tuner_init_tda18218);
+		init = tuner_init_tda18218;
+		break;
+	default:
+		pr_debug("%s: unsupported tuner ID=%d\n", __func__,
+				state->cfg.tuner);
+		ret = -ENODEV;
+		goto err;
+	}
+
+	for (i = 0; i < len; i++) {
+		ret = af9033_wr_reg(state, init[i].reg, init[i].val);
+		if (ret < 0)
+			goto err;
+	}
+
+	state->bandwidth_hz = 0; /* force to program all parameters */
+
+	return 0;
+
+err:
+	pr_debug("%s: failed=%d\n", __func__, ret);
+
+	return ret;
+}
+
+static int af9033_sleep(struct dvb_frontend *fe)
+{
+	struct af9033_state *state = fe->demodulator_priv;
+	int ret, i;
+	u8 tmp;
+
+	ret = af9033_wr_reg(state, 0x80004c, 1);
+	if (ret < 0)
+		goto err;
+
+	ret = af9033_wr_reg(state, 0x800000, 0);
+	if (ret < 0)
+		goto err;
+
+	for (i = 100, tmp = 1; i && tmp; i--) {
+		ret = af9033_rd_reg(state, 0x80004c, &tmp);
+		if (ret < 0)
+			goto err;
+
+		usleep_range(200, 10000);
+	}
+
+	pr_debug("%s: loop=%d\n", __func__, i);
+
+	if (i == 0) {
+		ret = -ETIMEDOUT;
+		goto err;
+	}
+
+	ret = af9033_wr_reg_mask(state, 0x80fb24, 0x08, 0x08);
+	if (ret < 0)
+		goto err;
+
+	/* prevent current leak (?) */
+	if (state->cfg.ts_mode == AF9033_TS_MODE_SERIAL) {
+		/* enable parallel TS */
+		ret = af9033_wr_reg_mask(state, 0x00d917, 0x00, 0x01);
+		if (ret < 0)
+			goto err;
+
+		ret = af9033_wr_reg_mask(state, 0x00d916, 0x01, 0x01);
+		if (ret < 0)
+			goto err;
+	}
+
+	return 0;
+
+err:
+	pr_debug("%s: failed=%d\n", __func__, ret);
+
+	return ret;
+}
+
+static int af9033_get_tune_settings(struct dvb_frontend *fe,
+		struct dvb_frontend_tune_settings *fesettings)
+{
+	fesettings->min_delay_ms = 800;
+	fesettings->step_size = 0;
+	fesettings->max_drift = 0;
+
+	return 0;
+}
+
+static int af9033_set_frontend(struct dvb_frontend *fe)
+{
+	struct af9033_state *state = fe->demodulator_priv;
+	struct dtv_frontend_properties *c = &fe->dtv_property_cache;
+	int ret, i, spec_inv;
+	u8 tmp, buf[3], bandwidth_reg_val;
+	u32 if_frequency, freq_cw, adc_freq;
+
+	pr_debug("%s: frequency=%d bandwidth_hz=%d\n", __func__, c->frequency,
+			c->bandwidth_hz);
+
+	/* check bandwidth */
+	switch (c->bandwidth_hz) {
+	case 6000000:
+		bandwidth_reg_val = 0x00;
+		break;
+	case 7000000:
+		bandwidth_reg_val = 0x01;
+		break;
+	case 8000000:
+		bandwidth_reg_val = 0x02;
+		break;
+	default:
+		pr_debug("%s: invalid bandwidth_hz\n", __func__);
+		ret = -EINVAL;
+		goto err;
+	}
+
+	/* program tuner */
+	if (fe->ops.tuner_ops.set_params)
+		fe->ops.tuner_ops.set_params(fe);
+
+	/* program CFOE coefficients */
+	if (c->bandwidth_hz != state->bandwidth_hz) {
+		for (i = 0; i < ARRAY_SIZE(coeff_lut); i++) {
+			if (coeff_lut[i].clock == state->cfg.clock &&
+				coeff_lut[i].bandwidth_hz == c->bandwidth_hz) {
+				break;
+			}
+		}
+		ret =  af9033_wr_regs(state, 0x800001,
+				coeff_lut[i].val, sizeof(coeff_lut[i].val));
+	}
+
+	/* program frequency control */
+	if (c->bandwidth_hz != state->bandwidth_hz) {
+		spec_inv = state->cfg.spec_inv ? -1 : 1;
+
+		for (i = 0; i < ARRAY_SIZE(clock_adc_lut); i++) {
+			if (clock_adc_lut[i].clock == state->cfg.clock)
+				break;
+		}
+		adc_freq = clock_adc_lut[i].adc;
+
+		/* get used IF frequency */
+		if (fe->ops.tuner_ops.get_if_frequency)
+			fe->ops.tuner_ops.get_if_frequency(fe, &if_frequency);
+		else
+			if_frequency = 0;
+
+		while (if_frequency > (adc_freq / 2))
+			if_frequency -= adc_freq;
+
+		if (if_frequency >= 0)
+			spec_inv *= -1;
+		else
+			if_frequency *= -1;
+
+		freq_cw = af9033_div(if_frequency, adc_freq, 23ul);
+
+		if (spec_inv == -1)
+			freq_cw *= -1;
+
+		/* get adc multiplies */
+		ret = af9033_rd_reg(state, 0x800045, &tmp);
+		if (ret < 0)
+			goto err;
+
+		if (tmp == 1)
+			freq_cw /= 2;
+
+		buf[0] = (freq_cw >>  0) & 0xff;
+		buf[1] = (freq_cw >>  8) & 0xff;
+		buf[2] = (freq_cw >> 16) & 0x7f;
+		ret = af9033_wr_regs(state, 0x800029, buf, 3);
+		if (ret < 0)
+			goto err;
+
+		state->bandwidth_hz = c->bandwidth_hz;
+	}
+
+	ret = af9033_wr_reg_mask(state, 0x80f904, bandwidth_reg_val, 0x03);
+	if (ret < 0)
+		goto err;
+
+	ret = af9033_wr_reg(state, 0x800040, 0x00);
+	if (ret < 0)
+		goto err;
+
+	ret = af9033_wr_reg(state, 0x800047, 0x00);
+	if (ret < 0)
+		goto err;
+
+	ret = af9033_wr_reg_mask(state, 0x80f999, 0x00, 0x01);
+	if (ret < 0)
+		goto err;
+
+	if (c->frequency <= 230000000)
+		tmp = 0x00; /* VHF */
+	else
+		tmp = 0x01; /* UHF */
+
+	ret = af9033_wr_reg(state, 0x80004b, tmp);
+	if (ret < 0)
+		goto err;
+
+	ret = af9033_wr_reg(state, 0x800000, 0x00);
+	if (ret < 0)
+		goto err;
+
+	return 0;
+
+err:
+	pr_debug("%s: failed=%d\n", __func__, ret);
+
+	return ret;
+}
+
+static int af9033_get_frontend(struct dvb_frontend *fe)
+{
+	struct af9033_state *state = fe->demodulator_priv;
+	struct dtv_frontend_properties *c = &fe->dtv_property_cache;
+	int ret;
+	u8 buf[8];
+
+	pr_debug("%s\n", __func__);
+
+	/* read all needed registers */
+	ret = af9033_rd_regs(state, 0x80f900, buf, sizeof(buf));
+	if (ret < 0)
+		goto err;
+
+	switch ((buf[0] >> 0) & 3) {
+	case 0:
+		c->transmission_mode = TRANSMISSION_MODE_2K;
+		break;
+	case 1:
+		c->transmission_mode = TRANSMISSION_MODE_8K;
+		break;
+	}
+
+	switch ((buf[1] >> 0) & 3) {
+	case 0:
+		c->guard_interval = GUARD_INTERVAL_1_32;
+		break;
+	case 1:
+		c->guard_interval = GUARD_INTERVAL_1_16;
+		break;
+	case 2:
+		c->guard_interval = GUARD_INTERVAL_1_8;
+		break;
+	case 3:
+		c->guard_interval = GUARD_INTERVAL_1_4;
+		break;
+	}
+
+	switch ((buf[2] >> 0) & 7) {
+	case 0:
+		c->hierarchy = HIERARCHY_NONE;
+		break;
+	case 1:
+		c->hierarchy = HIERARCHY_1;
+		break;
+	case 2:
+		c->hierarchy = HIERARCHY_2;
+		break;
+	case 3:
+		c->hierarchy = HIERARCHY_4;
+		break;
+	}
+
+	switch ((buf[3] >> 0) & 3) {
+	case 0:
+		c->modulation = QPSK;
+		break;
+	case 1:
+		c->modulation = QAM_16;
+		break;
+	case 2:
+		c->modulation = QAM_64;
+		break;
+	}
+
+	switch ((buf[4] >> 0) & 3) {
+	case 0:
+		c->bandwidth_hz = 6000000;
+		break;
+	case 1:
+		c->bandwidth_hz = 7000000;
+		break;
+	case 2:
+		c->bandwidth_hz = 8000000;
+		break;
+	}
+
+	switch ((buf[6] >> 0) & 7) {
+	case 0:
+		c->code_rate_HP = FEC_1_2;
+		break;
+	case 1:
+		c->code_rate_HP = FEC_2_3;
+		break;
+	case 2:
+		c->code_rate_HP = FEC_3_4;
+		break;
+	case 3:
+		c->code_rate_HP = FEC_5_6;
+		break;
+	case 4:
+		c->code_rate_HP = FEC_7_8;
+		break;
+	case 5:
+		c->code_rate_HP = FEC_NONE;
+		break;
+	}
+
+	switch ((buf[7] >> 0) & 7) {
+	case 0:
+		c->code_rate_LP = FEC_1_2;
+		break;
+	case 1:
+		c->code_rate_LP = FEC_2_3;
+		break;
+	case 2:
+		c->code_rate_LP = FEC_3_4;
+		break;
+	case 3:
+		c->code_rate_LP = FEC_5_6;
+		break;
+	case 4:
+		c->code_rate_LP = FEC_7_8;
+		break;
+	case 5:
+		c->code_rate_LP = FEC_NONE;
+		break;
+	}
+
+	return 0;
+
+err:
+	pr_debug("%s: failed=%d\n", __func__, ret);
+
+	return ret;
+}
+
+static int af9033_read_status(struct dvb_frontend *fe, fe_status_t *status)
+{
+	struct af9033_state *state = fe->demodulator_priv;
+	int ret;
+	u8 tmp;
+
+	*status = 0;
+
+	/* radio channel status, 0=no result, 1=has signal, 2=no signal */
+	ret = af9033_rd_reg(state, 0x800047, &tmp);
+	if (ret < 0)
+		goto err;
+
+	/* has signal */
+	if (tmp == 0x01)
+		*status |= FE_HAS_SIGNAL;
+
+	if (tmp != 0x02) {
+		/* TPS lock */
+		ret = af9033_rd_reg_mask(state, 0x80f5a9, &tmp, 0x01);
+		if (ret < 0)
+			goto err;
+
+		if (tmp)
+			*status |= FE_HAS_SIGNAL | FE_HAS_CARRIER |
+					FE_HAS_VITERBI;
+
+		/* full lock */
+		ret = af9033_rd_reg_mask(state, 0x80f999, &tmp, 0x01);
+		if (ret < 0)
+			goto err;
+
+		if (tmp)
+			*status |= FE_HAS_SIGNAL | FE_HAS_CARRIER |
+					FE_HAS_VITERBI | FE_HAS_SYNC |
+					FE_HAS_LOCK;
+	}
+
+	return 0;
+
+err:
+	pr_debug("%s: failed=%d\n", __func__, ret);
+
+	return ret;
+}
+
+static int af9033_read_snr(struct dvb_frontend *fe, u16 *snr)
+{
+	struct af9033_state *state = fe->demodulator_priv;
+	int ret, i, len;
+	u8 buf[3], tmp;
+	u32 snr_val;
+	const struct val_snr *uninitialized_var(snr_lut);
+
+	/* read value */
+	ret = af9033_rd_regs(state, 0x80002c, buf, 3);
+	if (ret < 0)
+		goto err;
+
+	snr_val = (buf[2] << 16) | (buf[1] << 8) | buf[0];
+
+	/* read current modulation */
+	ret = af9033_rd_reg(state, 0x80f903, &tmp);
+	if (ret < 0)
+		goto err;
+
+	switch ((tmp >> 0) & 3) {
+	case 0:
+		len = ARRAY_SIZE(qpsk_snr_lut);
+		snr_lut = qpsk_snr_lut;
+		break;
+	case 1:
+		len = ARRAY_SIZE(qam16_snr_lut);
+		snr_lut = qam16_snr_lut;
+		break;
+	case 2:
+		len = ARRAY_SIZE(qam64_snr_lut);
+		snr_lut = qam64_snr_lut;
+		break;
+	default:
+		goto err;
+	}
+
+	for (i = 0; i < len; i++) {
+		tmp = snr_lut[i].snr;
+
+		if (snr_val < snr_lut[i].val)
+			break;
+	}
+
+	*snr = tmp * 10; /* dB/10 */
+
+	return 0;
+
+err:
+	pr_debug("%s: failed=%d\n", __func__, ret);
+
+	return ret;
+}
+
+static int af9033_read_signal_strength(struct dvb_frontend *fe, u16 *strength)
+{
+	struct af9033_state *state = fe->demodulator_priv;
+	int ret;
+	u8 strength2;
+
+	/* read signal strength of 0-100 scale */
+	ret = af9033_rd_reg(state, 0x800048, &strength2);
+	if (ret < 0)
+		goto err;
+
+	/* scale value to 0x0000-0xffff */
+	*strength = strength2 * 0xffff / 100;
+
+	return 0;
+
+err:
+	pr_debug("%s: failed=%d\n", __func__, ret);
+
+	return ret;
+}
+
+static int af9033_update_ch_stat(struct af9033_state *state)
+{
+	int ret = 0;
+	u32 err_cnt, bit_cnt;
+	u16 abort_cnt;
+	u8 buf[7];
+
+	/* only update data every half second */
+	if (time_after(jiffies, state->last_stat_check + msecs_to_jiffies(500))) {
+		ret = af9033_rd_regs(state, 0x800032, buf, sizeof(buf));
+		if (ret < 0)
+			goto err;
+		/* in 8 byte packets? */
+		abort_cnt = (buf[1] << 8) + buf[0];
+		/* in bits */
+		err_cnt = (buf[4] << 16) + (buf[3] << 8) + buf[2];
+		/* in 8 byte packets? always(?) 0x2710 = 10000 */
+		bit_cnt = (buf[6] << 8) + buf[5];
+
+		if (bit_cnt < abort_cnt) {
+			abort_cnt = 1000;
+			state->ber = 0xffffffff;
+		} else {
+			/* 8 byte packets, that have not been rejected already */
+			bit_cnt -= (u32)abort_cnt;
+			if (bit_cnt == 0) {
+				state->ber = 0xffffffff;
+			} else {
+				err_cnt -= (u32)abort_cnt * 8 * 8;
+				bit_cnt *= 8 * 8;
+				state->ber = err_cnt * (0xffffffff / bit_cnt);
+			}
+		}
+		state->ucb += abort_cnt;
+		state->last_stat_check = jiffies;
+	}
+
+	return 0;
+err:
+	pr_debug("%s: failed=%d\n", __func__, ret);
+	return ret;
+}
+
+static int af9033_read_ber(struct dvb_frontend *fe, u32 *ber)
+{
+	struct af9033_state *state = fe->demodulator_priv;
+	int ret;
+
+	ret = af9033_update_ch_stat(state);
+	if (ret < 0)
+		return ret;
+
+	*ber = state->ber;
+
+	return 0;
+}
+
+static int af9033_read_ucblocks(struct dvb_frontend *fe, u32 *ucblocks)
+{
+	struct af9033_state *state = fe->demodulator_priv;
+	int ret;
+
+	ret = af9033_update_ch_stat(state);
+	if (ret < 0)
+		return ret;
+
+	*ucblocks = state->ucb;
+
+	return 0;
+}
+
+static int af9033_i2c_gate_ctrl(struct dvb_frontend *fe, int enable)
+{
+	struct af9033_state *state = fe->demodulator_priv;
+	int ret;
+
+	pr_debug("%s: enable=%d\n", __func__, enable);
+
+	ret = af9033_wr_reg_mask(state, 0x00fa04, enable, 0x01);
+	if (ret < 0)
+		goto err;
+
+	return 0;
+
+err:
+	pr_debug("%s: failed=%d\n", __func__, ret);
+
+	return ret;
+}
+
+static struct dvb_frontend_ops af9033_ops;
+
+struct dvb_frontend *af9033_attach(const struct af9033_config *config,
+		struct i2c_adapter *i2c)
+{
+	int ret;
+	struct af9033_state *state;
+	u8 buf[8];
+
+	pr_debug("%s:\n", __func__);
+
+	/* allocate memory for the internal state */
+	state = kzalloc(sizeof(struct af9033_state), GFP_KERNEL);
+	if (state == NULL)
+		goto err;
+
+	/* setup the state */
+	state->i2c = i2c;
+	memcpy(&state->cfg, config, sizeof(struct af9033_config));
+
+	if (state->cfg.clock != 12000000) {
+		printk(KERN_INFO "af9033: unsupported clock=%d, only " \
+				"12000000 Hz is supported currently\n",
+				state->cfg.clock);
+		goto err;
+	}
+
+	/* firmware version */
+	ret = af9033_rd_regs(state, 0x0083e9, &buf[0], 4);
+	if (ret < 0)
+		goto err;
+
+	ret = af9033_rd_regs(state, 0x804191, &buf[4], 4);
+	if (ret < 0)
+		goto err;
+
+	printk(KERN_INFO "af9033: firmware version: LINK=%d.%d.%d.%d " \
+			"OFDM=%d.%d.%d.%d\n", buf[0], buf[1], buf[2], buf[3],
+			buf[4], buf[5], buf[6], buf[7]);
+
+	/* configure internal TS mode */
+	switch (state->cfg.ts_mode) {
+	case AF9033_TS_MODE_PARALLEL:
+		state->ts_mode_parallel = true;
+		break;
+	case AF9033_TS_MODE_SERIAL:
+		state->ts_mode_serial = true;
+		break;
+	case AF9033_TS_MODE_USB:
+		/* usb mode for AF9035 */
+	default:
+		break;
+	}
+
+	/* create dvb_frontend */
+	memcpy(&state->fe.ops, &af9033_ops, sizeof(struct dvb_frontend_ops));
+	state->fe.demodulator_priv = state;
+
+	return &state->fe;
+
+err:
+	kfree(state);
+	return NULL;
+}
+EXPORT_SYMBOL(af9033_attach);
+
+static struct dvb_frontend_ops af9033_ops = {
+	.delsys = { SYS_DVBT },
+	.info = {
+		.name = "Afatech AF9033 (DVB-T)",
+		.frequency_min = 174000000,
+		.frequency_max = 862000000,
+		.frequency_stepsize = 250000,
+		.frequency_tolerance = 0,
+		.caps =	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_HIERARCHY_AUTO |
+			FE_CAN_RECOVER |
+			FE_CAN_MUTE_TS
+	},
+
+	.release = af9033_release,
+
+	.init = af9033_init,
+	.sleep = af9033_sleep,
+
+	.get_tune_settings = af9033_get_tune_settings,
+	.set_frontend = af9033_set_frontend,
+	.get_frontend = af9033_get_frontend,
+
+	.read_status = af9033_read_status,
+	.read_snr = af9033_read_snr,
+	.read_signal_strength = af9033_read_signal_strength,
+	.read_ber = af9033_read_ber,
+	.read_ucblocks = af9033_read_ucblocks,
+
+	.i2c_gate_ctrl = af9033_i2c_gate_ctrl,
+};
+
+MODULE_AUTHOR("Antti Palosaari <crope@iki.fi>");
+MODULE_DESCRIPTION("Afatech AF9033 DVB-T demodulator driver");
+MODULE_LICENSE("GPL");
diff --git a/drivers/media/dvb/frontends/af9033.h b/drivers/media/dvb/frontends/af9033.h
new file mode 100644
index 00000000000..9e302c3f0f7
--- /dev/null
+++ b/drivers/media/dvb/frontends/af9033.h
@@ -0,0 +1,75 @@
+/*
+ * Afatech AF9033 demodulator driver
+ *
+ * Copyright (C) 2009 Antti Palosaari <crope@iki.fi>
+ * 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 AF9033_H
+#define AF9033_H
+
+struct af9033_config {
+	/*
+	 * I2C address
+	 */
+	u8 i2c_addr;
+
+	/*
+	 * clock Hz
+	 * 12000000, 22000000, 24000000, 34000000, 32000000, 28000000, 26000000,
+	 * 30000000, 36000000, 20480000, 16384000
+	 */
+	u32 clock;
+
+	/*
+	 * tuner
+	 */
+#define AF9033_TUNER_TUA9001     0x27 /* Infineon TUA 9001 */
+#define AF9033_TUNER_FC0011      0x28 /* Fitipower FC0011 */
+#define AF9033_TUNER_MXL5007T    0xa0 /* MaxLinear MxL5007T */
+#define AF9033_TUNER_TDA18218    0xa1 /* NXP TDA 18218HN */
+	u8 tuner;
+
+	/*
+	 * TS settings
+	 */
+#define AF9033_TS_MODE_USB       0
+#define AF9033_TS_MODE_PARALLEL  1
+#define AF9033_TS_MODE_SERIAL    2
+	u8 ts_mode:2;
+
+	/*
+	 * input spectrum inversion
+	 */
+	bool spec_inv;
+};
+
+
+#if defined(CONFIG_DVB_AF9033) || \
+	(defined(CONFIG_DVB_AF9033_MODULE) && defined(MODULE))
+extern struct dvb_frontend *af9033_attach(const struct af9033_config *config,
+	struct i2c_adapter *i2c);
+#else
+static inline struct dvb_frontend *af9033_attach(
+	const struct af9033_config *config, struct i2c_adapter *i2c)
+{
+	printk(KERN_WARNING "%s: driver disabled by Kconfig\n", __func__);
+	return NULL;
+}
+#endif
+
+#endif /* AF9033_H */
diff --git a/drivers/media/dvb/frontends/af9033_priv.h b/drivers/media/dvb/frontends/af9033_priv.h
new file mode 100644
index 00000000000..0b783b9ed75
--- /dev/null
+++ b/drivers/media/dvb/frontends/af9033_priv.h
@@ -0,0 +1,470 @@
+/*
+ * Afatech AF9033 demodulator driver
+ *
+ * Copyright (C) 2009 Antti Palosaari <crope@iki.fi>
+ * 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 AF9033_PRIV_H
+#define AF9033_PRIV_H
+
+#include "dvb_frontend.h"
+#include "af9033.h"
+
+struct reg_val {
+	u32 reg;
+	u8  val;
+};
+
+struct reg_val_mask {
+	u32 reg;
+	u8  val;
+	u8  mask;
+};
+
+struct coeff {
+	u32 clock;
+	u32 bandwidth_hz;
+	u8 val[36];
+};
+
+struct clock_adc {
+	u32 clock;
+	u32 adc;
+};
+
+struct val_snr {
+	u32 val;
+	u8 snr;
+};
+
+/* Xtal clock vs. ADC clock lookup table */
+static const struct clock_adc clock_adc_lut[] = {
+	{ 16384000, 20480000 },
+	{ 20480000, 20480000 },
+	{ 36000000, 20250000 },
+	{ 30000000, 20156250 },
+	{ 26000000, 20583333 },
+	{ 28000000, 20416667 },
+	{ 32000000, 20500000 },
+	{ 34000000, 20187500 },
+	{ 24000000, 20500000 },
+	{ 22000000, 20625000 },
+	{ 12000000, 20250000 },
+};
+
+/* pre-calculated coeff lookup table */
+static const struct coeff coeff_lut[] = {
+	/* 12.000 MHz */
+	{ 12000000, 8000000, {
+		0x01, 0xce, 0x55, 0xc9, 0x00, 0xe7, 0x2a, 0xe4, 0x00, 0x73,
+		0x99, 0x0f, 0x00, 0x73, 0x95, 0x72, 0x00, 0x73, 0x91, 0xd5,
+		0x00, 0x39, 0xca, 0xb9, 0x00, 0xe7, 0x2a, 0xe4, 0x00, 0x73,
+		0x95, 0x72, 0x37, 0x02, 0xce, 0x01 }
+	},
+	{ 12000000, 7000000, {
+		0x01, 0x94, 0x8b, 0x10, 0x00, 0xca, 0x45, 0x88, 0x00, 0x65,
+		0x25, 0xed, 0x00, 0x65, 0x22, 0xc4, 0x00, 0x65, 0x1f, 0x9b,
+		0x00, 0x32, 0x91, 0x62, 0x00, 0xca, 0x45, 0x88, 0x00, 0x65,
+		0x22, 0xc4, 0x88, 0x02, 0x95, 0x01 }
+	},
+	{ 12000000, 6000000, {
+		0x01, 0x5a, 0xc0, 0x56, 0x00, 0xad, 0x60, 0x2b, 0x00, 0x56,
+		0xb2, 0xcb, 0x00, 0x56, 0xb0, 0x15, 0x00, 0x56, 0xad, 0x60,
+		0x00, 0x2b, 0x58, 0x0b, 0x00, 0xad, 0x60, 0x2b, 0x00, 0x56,
+		0xb0, 0x15, 0xf4, 0x02, 0x5b, 0x01 }
+	},
+};
+
+/* QPSK SNR lookup table */
+static const struct val_snr qpsk_snr_lut[] = {
+	{ 0x0b4771,  0 },
+	{ 0x0c1aed,  1 },
+	{ 0x0d0d27,  2 },
+	{ 0x0e4d19,  3 },
+	{ 0x0e5da8,  4 },
+	{ 0x107097,  5 },
+	{ 0x116975,  6 },
+	{ 0x1252d9,  7 },
+	{ 0x131fa4,  8 },
+	{ 0x13d5e1,  9 },
+	{ 0x148e53, 10 },
+	{ 0x15358b, 11 },
+	{ 0x15dd29, 12 },
+	{ 0x168112, 13 },
+	{ 0x170b61, 14 },
+	{ 0x17a532, 15 },
+	{ 0x180f94, 16 },
+	{ 0x186ed2, 17 },
+	{ 0x18b271, 18 },
+	{ 0x18e118, 19 },
+	{ 0x18ff4b, 20 },
+	{ 0x190af1, 21 },
+	{ 0x191451, 22 },
+	{ 0xffffff, 23 },
+};
+
+/* QAM16 SNR lookup table */
+static const struct val_snr qam16_snr_lut[] = {
+	{ 0x04f0d5,  0 },
+	{ 0x05387a,  1 },
+	{ 0x0573a4,  2 },
+	{ 0x05a99e,  3 },
+	{ 0x05cc80,  4 },
+	{ 0x05eb62,  5 },
+	{ 0x05fecf,  6 },
+	{ 0x060b80,  7 },
+	{ 0x062501,  8 },
+	{ 0x064865,  9 },
+	{ 0x069604, 10 },
+	{ 0x06f356, 11 },
+	{ 0x07706a, 12 },
+	{ 0x0804d3, 13 },
+	{ 0x089d1a, 14 },
+	{ 0x093e3d, 15 },
+	{ 0x09e35d, 16 },
+	{ 0x0a7c3c, 17 },
+	{ 0x0afaf8, 18 },
+	{ 0x0b719d, 19 },
+	{ 0x0bda6a, 20 },
+	{ 0x0c0c75, 21 },
+	{ 0x0c3f7d, 22 },
+	{ 0x0c5e62, 23 },
+	{ 0x0c6c31, 24 },
+	{ 0x0c7925, 25 },
+	{ 0xffffff, 26 },
+};
+
+/* QAM64 SNR lookup table */
+static const struct val_snr qam64_snr_lut[] = {
+	{ 0x0256d0,  0 },
+	{ 0x027a65,  1 },
+	{ 0x029873,  2 },
+	{ 0x02b7fe,  3 },
+	{ 0x02cf1e,  4 },
+	{ 0x02e234,  5 },
+	{ 0x02f409,  6 },
+	{ 0x030046,  7 },
+	{ 0x030844,  8 },
+	{ 0x030a02,  9 },
+	{ 0x030cde, 10 },
+	{ 0x031031, 11 },
+	{ 0x03144c, 12 },
+	{ 0x0315dd, 13 },
+	{ 0x031920, 14 },
+	{ 0x0322d0, 15 },
+	{ 0x0339fc, 16 },
+	{ 0x0364a1, 17 },
+	{ 0x038bcc, 18 },
+	{ 0x03c7d3, 19 },
+	{ 0x0408cc, 20 },
+	{ 0x043bed, 21 },
+	{ 0x048061, 22 },
+	{ 0x04be95, 23 },
+	{ 0x04fa7d, 24 },
+	{ 0x052405, 25 },
+	{ 0x05570d, 26 },
+	{ 0x059feb, 27 },
+	{ 0x05bf38, 28 },
+	{ 0xffffff, 29 },
+};
+
+static const struct reg_val ofsm_init[] = {
+	{ 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 },
+	{ 0x8000ab, 0x01 },
+	{ 0x8000b0, 0x01 },
+	{ 0x8000c0, 0x05 },
+	{ 0x8000c4, 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 },
+};
+
+/* Infineon TUA 9001 tuner init
+   AF9033_TUNER_TUA9001    = 0x27 */
+static const struct reg_val tuner_init_tua9001[] = {
+	{ 0x800046, 0x27 },
+	{ 0x800057, 0x00 },
+	{ 0x800058, 0x01 },
+	{ 0x80005f, 0x00 },
+	{ 0x800060, 0x00 },
+	{ 0x80006d, 0x00 },
+	{ 0x800071, 0x05 },
+	{ 0x800072, 0x02 },
+	{ 0x800074, 0x01 },
+	{ 0x800075, 0x03 },
+	{ 0x800076, 0x02 },
+	{ 0x800077, 0x00 },
+	{ 0x800078, 0x01 },
+	{ 0x800079, 0x00 },
+	{ 0x80007a, 0x7e },
+	{ 0x80007b, 0x3e },
+	{ 0x800093, 0x00 },
+	{ 0x800094, 0x01 },
+	{ 0x800095, 0x02 },
+	{ 0x800096, 0x01 },
+	{ 0x800098, 0x0a },
+	{ 0x80009b, 0x05 },
+	{ 0x80009c, 0x80 },
+	{ 0x8000b3, 0x00 },
+	{ 0x8000c1, 0x01 },
+	{ 0x8000c2, 0x00 },
+	{ 0x80f007, 0x00 },
+	{ 0x80f01f, 0x82 },
+	{ 0x80f020, 0x00 },
+	{ 0x80f029, 0x82 },
+	{ 0x80f02a, 0x00 },
+	{ 0x80f047, 0x00 },
+	{ 0x80f054, 0x00 },
+	{ 0x80f055, 0x00 },
+	{ 0x80f077, 0x01 },
+	{ 0x80f1e6, 0x00 },
+};
+
+/* Fitipower fc0011 tuner init
+   AF9033_TUNER_FC0011    = 0x28 */
+static const struct reg_val tuner_init_fc0011[] = {
+	{ 0x800046, AF9033_TUNER_FC0011 },
+	{ 0x800057, 0x00 },
+	{ 0x800058, 0x01 },
+	{ 0x80005f, 0x00 },
+	{ 0x800060, 0x00 },
+	{ 0x800068, 0xa5 },
+	{ 0x80006e, 0x01 },
+	{ 0x800071, 0x0A },
+	{ 0x800072, 0x02 },
+	{ 0x800074, 0x01 },
+	{ 0x800079, 0x01 },
+	{ 0x800093, 0x00 },
+	{ 0x800094, 0x00 },
+	{ 0x800095, 0x00 },
+	{ 0x800096, 0x00 },
+	{ 0x80009b, 0x2D },
+	{ 0x80009c, 0x60 },
+	{ 0x80009d, 0x23 },
+	{ 0x8000a4, 0x50 },
+	{ 0x8000ad, 0x50 },
+	{ 0x8000b3, 0x01 },
+	{ 0x8000b7, 0x88 },
+	{ 0x8000b8, 0xa6 },
+	{ 0x8000c3, 0x01 },
+	{ 0x8000c4, 0x01 },
+	{ 0x8000c7, 0x69 },
+	{ 0x80F007, 0x00 },
+	{ 0x80F00A, 0x1B },
+	{ 0x80F00B, 0x1B },
+	{ 0x80F00C, 0x1B },
+	{ 0x80F00D, 0x1B },
+	{ 0x80F00E, 0xFF },
+	{ 0x80F00F, 0x01 },
+	{ 0x80F010, 0x00 },
+	{ 0x80F011, 0x02 },
+	{ 0x80F012, 0xFF },
+	{ 0x80F013, 0x01 },
+	{ 0x80F014, 0x00 },
+	{ 0x80F015, 0x02 },
+	{ 0x80F01B, 0xEF },
+	{ 0x80F01C, 0x01 },
+	{ 0x80F01D, 0x0f },
+	{ 0x80F01E, 0x02 },
+	{ 0x80F01F, 0x6E },
+	{ 0x80F020, 0x00 },
+	{ 0x80F025, 0xDE },
+	{ 0x80F026, 0x00 },
+	{ 0x80F027, 0x0A },
+	{ 0x80F028, 0x03 },
+	{ 0x80F029, 0x6E },
+	{ 0x80F02A, 0x00 },
+	{ 0x80F047, 0x00 },
+	{ 0x80F054, 0x00 },
+	{ 0x80F055, 0x00 },
+	{ 0x80F077, 0x01 },
+	{ 0x80F1E6, 0x00 },
+};
+
+/* MaxLinear MxL5007T tuner init
+   AF9033_TUNER_MXL5007T    = 0xa0 */
+static const struct reg_val tuner_init_mxl5007t[] = {
+	{ 0x800046, 0x1b },
+	{ 0x800057, 0x01 },
+	{ 0x800058, 0x01 },
+	{ 0x80005f, 0x00 },
+	{ 0x800060, 0x00 },
+	{ 0x800068, 0x96 },
+	{ 0x800071, 0x05 },
+	{ 0x800072, 0x02 },
+	{ 0x800074, 0x01 },
+	{ 0x800079, 0x01 },
+	{ 0x800093, 0x00 },
+	{ 0x800094, 0x00 },
+	{ 0x800095, 0x00 },
+	{ 0x800096, 0x00 },
+	{ 0x8000b3, 0x01 },
+	{ 0x8000c1, 0x01 },
+	{ 0x8000c2, 0x00 },
+	{ 0x80f007, 0x00 },
+	{ 0x80f00c, 0x19 },
+	{ 0x80f00d, 0x1a },
+	{ 0x80f012, 0xda },
+	{ 0x80f013, 0x00 },
+	{ 0x80f014, 0x00 },
+	{ 0x80f015, 0x02 },
+	{ 0x80f01f, 0x82 },
+	{ 0x80f020, 0x00 },
+	{ 0x80f029, 0x82 },
+	{ 0x80f02a, 0x00 },
+	{ 0x80f077, 0x02 },
+	{ 0x80f1e6, 0x00 },
+};
+
+/* NXP TDA 18218HN tuner init
+   AF9033_TUNER_TDA18218    = 0xa1 */
+static const struct reg_val tuner_init_tda18218[] = {
+	{0x800046, 0xa1},
+	{0x800057, 0x01},
+	{0x800058, 0x01},
+	{0x80005f, 0x00},
+	{0x800060, 0x00},
+	{0x800071, 0x05},
+	{0x800072, 0x02},
+	{0x800074, 0x01},
+	{0x800079, 0x01},
+	{0x800093, 0x00},
+	{0x800094, 0x00},
+	{0x800095, 0x00},
+	{0x800096, 0x00},
+	{0x8000b3, 0x01},
+	{0x8000c3, 0x01},
+	{0x8000c4, 0x00},
+	{0x80f007, 0x00},
+	{0x80f00c, 0x19},
+	{0x80f00d, 0x1a},
+	{0x80f012, 0xda},
+	{0x80f013, 0x00},
+	{0x80f014, 0x00},
+	{0x80f015, 0x02},
+	{0x80f01f, 0x82},
+	{0x80f020, 0x00},
+	{0x80f029, 0x82},
+	{0x80f02a, 0x00},
+	{0x80f077, 0x02},
+	{0x80f1e6, 0x00},
+};
+
+#endif /* AF9033_PRIV_H */
+
diff --git a/drivers/media/dvb/frontends/au8522_common.c b/drivers/media/dvb/frontends/au8522_common.c
new file mode 100644
index 00000000000..5cfe151ee39
--- /dev/null
+++ b/drivers/media/dvb/frontends/au8522_common.c
@@ -0,0 +1,259 @@
+/*
+    Auvitek AU8522 QAM/8VSB demodulator driver
+
+    Copyright (C) 2008 Steven Toth <stoth@linuxtv.org>
+    Copyright (C) 2008 Devin Heitmueller <dheitmueller@linuxtv.org>
+    Copyright (C) 2005-2008 Auvitek International, Ltd.
+    Copyright (C) 2012 Michael Krufky <mkrufky@linuxtv.org>
+
+    This program is free software; you can redistribute it and/or modify
+    it under the terms of the GNU General Public License as published by
+    the Free Software Foundation; either version 2 of the License, or
+    (at your option) any later version.
+
+    This program is distributed in the hope that it will be useful,
+    but WITHOUT ANY WARRANTY; without even the implied warranty of
+    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+    GNU General Public License for more details.
+
+    You should have received a copy of the GNU General Public License
+    along with this program; if not, write to the Free Software
+    Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+
+*/
+
+#include <linux/i2c.h>
+#include "dvb_frontend.h"
+#include "au8522_priv.h"
+
+MODULE_LICENSE("GPL");
+
+static int debug;
+
+#define dprintk(arg...)\
+  do { if (debug)\
+	 printk(arg);\
+  } while (0)
+
+/* Despite the name "hybrid_tuner", the framework works just as well for
+   hybrid demodulators as well... */
+static LIST_HEAD(hybrid_tuner_instance_list);
+static DEFINE_MUTEX(au8522_list_mutex);
+
+/* 16 bit registers, 8 bit values */
+int au8522_writereg(struct au8522_state *state, u16 reg, u8 data)
+{
+	int ret;
+	u8 buf[] = { (reg >> 8) | 0x80, reg & 0xff, data };
+
+	struct i2c_msg msg = { .addr = state->config->demod_address,
+			       .flags = 0, .buf = buf, .len = 3 };
+
+	ret = i2c_transfer(state->i2c, &msg, 1);
+
+	if (ret != 1)
+		printk("%s: writereg error (reg == 0x%02x, val == 0x%04x, "
+		       "ret == %i)\n", __func__, reg, data, ret);
+
+	return (ret != 1) ? -1 : 0;
+}
+EXPORT_SYMBOL(au8522_writereg);
+
+u8 au8522_readreg(struct au8522_state *state, u16 reg)
+{
+	int ret;
+	u8 b0[] = { (reg >> 8) | 0x40, reg & 0xff };
+	u8 b1[] = { 0 };
+
+	struct i2c_msg msg[] = {
+		{ .addr = state->config->demod_address, .flags = 0,
+		  .buf = b0, .len = 2 },
+		{ .addr = state->config->demod_address, .flags = I2C_M_RD,
+		  .buf = b1, .len = 1 } };
+
+	ret = i2c_transfer(state->i2c, msg, 2);
+
+	if (ret != 2)
+		printk(KERN_ERR "%s: readreg error (ret == %i)\n",
+		       __func__, ret);
+	return b1[0];
+}
+EXPORT_SYMBOL(au8522_readreg);
+
+int au8522_i2c_gate_ctrl(struct dvb_frontend *fe, int enable)
+{
+	struct au8522_state *state = fe->demodulator_priv;
+
+	dprintk("%s(%d)\n", __func__, enable);
+
+	if (state->operational_mode == AU8522_ANALOG_MODE) {
+		/* We're being asked to manage the gate even though we're
+		   not in digital mode.  This can occur if we get switched
+		   over to analog mode before the dvb_frontend kernel thread
+		   has completely shutdown */
+		return 0;
+	}
+
+	if (enable)
+		return au8522_writereg(state, 0x106, 1);
+	else
+		return au8522_writereg(state, 0x106, 0);
+}
+EXPORT_SYMBOL(au8522_i2c_gate_ctrl);
+
+/* Reset the demod hardware and reset all of the configuration registers
+   to a default state. */
+int au8522_get_state(struct au8522_state **state, struct i2c_adapter *i2c,
+		     u8 client_address)
+{
+	int ret;
+
+	mutex_lock(&au8522_list_mutex);
+	ret = hybrid_tuner_request_state(struct au8522_state, (*state),
+					 hybrid_tuner_instance_list,
+					 i2c, client_address, "au8522");
+	mutex_unlock(&au8522_list_mutex);
+
+	return ret;
+}
+EXPORT_SYMBOL(au8522_get_state);
+
+void au8522_release_state(struct au8522_state *state)
+{
+	mutex_lock(&au8522_list_mutex);
+	if (state != NULL)
+		hybrid_tuner_release_state(state);
+	mutex_unlock(&au8522_list_mutex);
+}
+EXPORT_SYMBOL(au8522_release_state);
+
+static int au8522_led_gpio_enable(struct au8522_state *state, int onoff)
+{
+	struct au8522_led_config *led_config = state->config->led_cfg;
+	u8 val;
+
+	/* bail out if we can't control an LED */
+	if (!led_config || !led_config->gpio_output ||
+	    !led_config->gpio_output_enable || !led_config->gpio_output_disable)
+		return 0;
+
+	val = au8522_readreg(state, 0x4000 |
+			     (led_config->gpio_output & ~0xc000));
+	if (onoff) {
+		/* enable GPIO output */
+		val &= ~((led_config->gpio_output_enable >> 8) & 0xff);
+		val |=  (led_config->gpio_output_enable & 0xff);
+	} else {
+		/* disable GPIO output */
+		val &= ~((led_config->gpio_output_disable >> 8) & 0xff);
+		val |=  (led_config->gpio_output_disable & 0xff);
+	}
+	return au8522_writereg(state, 0x8000 |
+			       (led_config->gpio_output & ~0xc000), val);
+}
+
+/* led = 0 | off
+ * led = 1 | signal ok
+ * led = 2 | signal strong
+ * led < 0 | only light led if leds are currently off
+ */
+int au8522_led_ctrl(struct au8522_state *state, int led)
+{
+	struct au8522_led_config *led_config = state->config->led_cfg;
+	int i, ret = 0;
+
+	/* bail out if we can't control an LED */
+	if (!led_config || !led_config->gpio_leds ||
+	    !led_config->num_led_states || !led_config->led_states)
+		return 0;
+
+	if (led < 0) {
+		/* if LED is already lit, then leave it as-is */
+		if (state->led_state)
+			return 0;
+		else
+			led *= -1;
+	}
+
+	/* toggle LED if changing state */
+	if (state->led_state != led) {
+		u8 val;
+
+		dprintk("%s: %d\n", __func__, led);
+
+		au8522_led_gpio_enable(state, 1);
+
+		val = au8522_readreg(state, 0x4000 |
+				     (led_config->gpio_leds & ~0xc000));
+
+		/* start with all leds off */
+		for (i = 0; i < led_config->num_led_states; i++)
+			val &= ~led_config->led_states[i];
+
+		/* set selected LED state */
+		if (led < led_config->num_led_states)
+			val |= led_config->led_states[led];
+		else if (led_config->num_led_states)
+			val |=
+			led_config->led_states[led_config->num_led_states - 1];
+
+		ret = au8522_writereg(state, 0x8000 |
+				      (led_config->gpio_leds & ~0xc000), val);
+		if (ret < 0)
+			return ret;
+
+		state->led_state = led;
+
+		if (led == 0)
+			au8522_led_gpio_enable(state, 0);
+	}
+
+	return 0;
+}
+EXPORT_SYMBOL(au8522_led_ctrl);
+
+int au8522_init(struct dvb_frontend *fe)
+{
+	struct au8522_state *state = fe->demodulator_priv;
+	dprintk("%s()\n", __func__);
+
+	state->operational_mode = AU8522_DIGITAL_MODE;
+
+	/* Clear out any state associated with the digital side of the
+	   chip, so that when it gets powered back up it won't think
+	   that it is already tuned */
+	state->current_frequency = 0;
+
+	au8522_writereg(state, 0xa4, 1 << 5);
+
+	au8522_i2c_gate_ctrl(fe, 1);
+
+	return 0;
+}
+EXPORT_SYMBOL(au8522_init);
+
+int au8522_sleep(struct dvb_frontend *fe)
+{
+	struct au8522_state *state = fe->demodulator_priv;
+	dprintk("%s()\n", __func__);
+
+	/* Only power down if the digital side is currently using the chip */
+	if (state->operational_mode == AU8522_ANALOG_MODE) {
+		/* We're not in one of the expected power modes, which means
+		   that the DVB thread is probably telling us to go to sleep
+		   even though the analog frontend has already started using
+		   the chip.  So ignore the request */
+		return 0;
+	}
+
+	/* turn off led */
+	au8522_led_ctrl(state, 0);
+
+	/* Power down the chip */
+	au8522_writereg(state, 0xa4, 1 << 5);
+
+	state->current_frequency = 0;
+
+	return 0;
+}
+EXPORT_SYMBOL(au8522_sleep);
diff --git a/drivers/media/dvb/frontends/au8522_dig.c b/drivers/media/dvb/frontends/au8522_dig.c
index 25f650934c7..5fc70d6cd04 100644
--- a/drivers/media/dvb/frontends/au8522_dig.c
+++ b/drivers/media/dvb/frontends/au8522_dig.c
@@ -30,74 +30,11 @@
 
 static int debug;
 
-/* Despite the name "hybrid_tuner", the framework works just as well for
-   hybrid demodulators as well... */
-static LIST_HEAD(hybrid_tuner_instance_list);
-static DEFINE_MUTEX(au8522_list_mutex);
-
 #define dprintk(arg...)\
 	do { if (debug)\
 		printk(arg);\
 	} while (0)
 
-/* 16 bit registers, 8 bit values */
-int au8522_writereg(struct au8522_state *state, u16 reg, u8 data)
-{
-	int ret;
-	u8 buf[] = { (reg >> 8) | 0x80, reg & 0xff, data };
-
-	struct i2c_msg msg = { .addr = state->config->demod_address,
-			       .flags = 0, .buf = buf, .len = 3 };
-
-	ret = i2c_transfer(state->i2c, &msg, 1);
-
-	if (ret != 1)
-		printk("%s: writereg error (reg == 0x%02x, val == 0x%04x, "
-		       "ret == %i)\n", __func__, reg, data, ret);
-
-	return (ret != 1) ? -1 : 0;
-}
-
-u8 au8522_readreg(struct au8522_state *state, u16 reg)
-{
-	int ret;
-	u8 b0[] = { (reg >> 8) | 0x40, reg & 0xff };
-	u8 b1[] = { 0 };
-
-	struct i2c_msg msg[] = {
-		{ .addr = state->config->demod_address, .flags = 0,
-		  .buf = b0, .len = 2 },
-		{ .addr = state->config->demod_address, .flags = I2C_M_RD,
-		  .buf = b1, .len = 1 } };
-
-	ret = i2c_transfer(state->i2c, msg, 2);
-
-	if (ret != 2)
-		printk(KERN_ERR "%s: readreg error (ret == %i)\n",
-		       __func__, ret);
-	return b1[0];
-}
-
-static int au8522_i2c_gate_ctrl(struct dvb_frontend *fe, int enable)
-{
-	struct au8522_state *state = fe->demodulator_priv;
-
-	dprintk("%s(%d)\n", __func__, enable);
-
-	if (state->operational_mode == AU8522_ANALOG_MODE) {
-		/* We're being asked to manage the gate even though we're
-		   not in digital mode.  This can occur if we get switched
-		   over to analog mode before the dvb_frontend kernel thread
-		   has completely shutdown */
-		return 0;
-	}
-
-	if (enable)
-		return au8522_writereg(state, 0x106, 1);
-	else
-		return au8522_writereg(state, 0x106, 0);
-}
-
 struct mse2snr_tab {
 	u16 val;
 	u16 data;
@@ -609,136 +546,6 @@ static int au8522_set_frontend(struct dvb_frontend *fe)
 	return 0;
 }
 
-/* Reset the demod hardware and reset all of the configuration registers
-   to a default state. */
-int au8522_init(struct dvb_frontend *fe)
-{
-	struct au8522_state *state = fe->demodulator_priv;
-	dprintk("%s()\n", __func__);
-
-	state->operational_mode = AU8522_DIGITAL_MODE;
-
-	/* Clear out any state associated with the digital side of the
-	   chip, so that when it gets powered back up it won't think
-	   that it is already tuned */
-	state->current_frequency = 0;
-
-	au8522_writereg(state, 0xa4, 1 << 5);
-
-	au8522_i2c_gate_ctrl(fe, 1);
-
-	return 0;
-}
-
-static int au8522_led_gpio_enable(struct au8522_state *state, int onoff)
-{
-	struct au8522_led_config *led_config = state->config->led_cfg;
-	u8 val;
-
-	/* bail out if we can't control an LED */
-	if (!led_config || !led_config->gpio_output ||
-	    !led_config->gpio_output_enable || !led_config->gpio_output_disable)
-		return 0;
-
-	val = au8522_readreg(state, 0x4000 |
-			     (led_config->gpio_output & ~0xc000));
-	if (onoff) {
-		/* enable GPIO output */
-		val &= ~((led_config->gpio_output_enable >> 8) & 0xff);
-		val |=  (led_config->gpio_output_enable & 0xff);
-	} else {
-		/* disable GPIO output */
-		val &= ~((led_config->gpio_output_disable >> 8) & 0xff);
-		val |=  (led_config->gpio_output_disable & 0xff);
-	}
-	return au8522_writereg(state, 0x8000 |
-			       (led_config->gpio_output & ~0xc000), val);
-}
-
-/* led = 0 | off
- * led = 1 | signal ok
- * led = 2 | signal strong
- * led < 0 | only light led if leds are currently off
- */
-static int au8522_led_ctrl(struct au8522_state *state, int led)
-{
-	struct au8522_led_config *led_config = state->config->led_cfg;
-	int i, ret = 0;
-
-	/* bail out if we can't control an LED */
-	if (!led_config || !led_config->gpio_leds ||
-	    !led_config->num_led_states || !led_config->led_states)
-		return 0;
-
-	if (led < 0) {
-		/* if LED is already lit, then leave it as-is */
-		if (state->led_state)
-			return 0;
-		else
-			led *= -1;
-	}
-
-	/* toggle LED if changing state */
-	if (state->led_state != led) {
-		u8 val;
-
-		dprintk("%s: %d\n", __func__, led);
-
-		au8522_led_gpio_enable(state, 1);
-
-		val = au8522_readreg(state, 0x4000 |
-				     (led_config->gpio_leds & ~0xc000));
-
-		/* start with all leds off */
-		for (i = 0; i < led_config->num_led_states; i++)
-			val &= ~led_config->led_states[i];
-
-		/* set selected LED state */
-		if (led < led_config->num_led_states)
-			val |= led_config->led_states[led];
-		else if (led_config->num_led_states)
-			val |=
-			led_config->led_states[led_config->num_led_states - 1];
-
-		ret = au8522_writereg(state, 0x8000 |
-				      (led_config->gpio_leds & ~0xc000), val);
-		if (ret < 0)
-			return ret;
-
-		state->led_state = led;
-
-		if (led == 0)
-			au8522_led_gpio_enable(state, 0);
-	}
-
-	return 0;
-}
-
-int au8522_sleep(struct dvb_frontend *fe)
-{
-	struct au8522_state *state = fe->demodulator_priv;
-	dprintk("%s()\n", __func__);
-
-	/* Only power down if the digital side is currently using the chip */
-	if (state->operational_mode == AU8522_ANALOG_MODE) {
-		/* We're not in one of the expected power modes, which means
-		   that the DVB thread is probably telling us to go to sleep
-		   even though the analog frontend has already started using
-		   the chip.  So ignore the request */
-		return 0;
-	}
-
-	/* turn off led */
-	au8522_led_ctrl(state, 0);
-
-	/* Power down the chip */
-	au8522_writereg(state, 0xa4, 1 << 5);
-
-	state->current_frequency = 0;
-
-	return 0;
-}
-
 static int au8522_read_status(struct dvb_frontend *fe, fe_status_t *status)
 {
 	struct au8522_state *state = fe->demodulator_priv;
@@ -931,28 +738,6 @@ static int au8522_get_tune_settings(struct dvb_frontend *fe,
 
 static struct dvb_frontend_ops au8522_ops;
 
-int au8522_get_state(struct au8522_state **state, struct i2c_adapter *i2c,
-		     u8 client_address)
-{
-	int ret;
-
-	mutex_lock(&au8522_list_mutex);
-	ret = hybrid_tuner_request_state(struct au8522_state, (*state),
-					 hybrid_tuner_instance_list,
-					 i2c, client_address, "au8522");
-	mutex_unlock(&au8522_list_mutex);
-
-	return ret;
-}
-
-void au8522_release_state(struct au8522_state *state)
-{
-	mutex_lock(&au8522_list_mutex);
-	if (state != NULL)
-		hybrid_tuner_release_state(state);
-	mutex_unlock(&au8522_list_mutex);
-}
-
 
 static void au8522_release(struct dvb_frontend *fe)
 {
diff --git a/drivers/media/dvb/frontends/au8522_priv.h b/drivers/media/dvb/frontends/au8522_priv.h
index 751e17d692a..6e4a438732b 100644
--- a/drivers/media/dvb/frontends/au8522_priv.h
+++ b/drivers/media/dvb/frontends/au8522_priv.h
@@ -81,6 +81,8 @@ int au8522_sleep(struct dvb_frontend *fe);
 int au8522_get_state(struct au8522_state **state, struct i2c_adapter *i2c,
 		     u8 client_address);
 void au8522_release_state(struct au8522_state *state);
+int au8522_i2c_gate_ctrl(struct dvb_frontend *fe, int enable);
+int au8522_led_ctrl(struct au8522_state *state, int led);
 
 /* REGISTERS */
 #define AU8522_INPUT_CONTROL_REG081H			0x081
diff --git a/drivers/media/dvb/frontends/cx24110.c b/drivers/media/dvb/frontends/cx24110.c
index 5101f10f2d7..98ecaf0900d 100644
--- a/drivers/media/dvb/frontends/cx24110.c
+++ b/drivers/media/dvb/frontends/cx24110.c
@@ -512,14 +512,13 @@ static int cx24110_read_snr(struct dvb_frontend* fe, u16* snr)
 static int cx24110_read_ucblocks(struct dvb_frontend* fe, u32* ucblocks)
 {
 	struct cx24110_state *state = fe->demodulator_priv;
-	u32 lastbyer;
 
 	if(cx24110_readreg(state,0x10)&0x40) {
 		/* the RS error counter has finished one counting window */
 		cx24110_writereg(state,0x10,0x60); /* select the byer reg */
-		lastbyer=cx24110_readreg(state,0x12)|
-			(cx24110_readreg(state,0x13)<<8)|
-			(cx24110_readreg(state,0x14)<<16);
+		cx24110_readreg(state, 0x12) |
+			(cx24110_readreg(state, 0x13) << 8) |
+			(cx24110_readreg(state, 0x14) << 16);
 		cx24110_writereg(state,0x10,0x70); /* select the bler reg */
 		state->lastbler=cx24110_readreg(state,0x12)|
 			(cx24110_readreg(state,0x13)<<8)|
diff --git a/drivers/media/dvb/frontends/cxd2820r_core.c b/drivers/media/dvb/frontends/cxd2820r_core.c
index 5c7c2aaf9bf..3bba37d74f5 100644
--- a/drivers/media/dvb/frontends/cxd2820r_core.c
+++ b/drivers/media/dvb/frontends/cxd2820r_core.c
@@ -526,12 +526,12 @@ static enum dvbfe_search cxd2820r_search(struct dvb_frontend *fe)
 		if (ret)
 			goto error;
 
-		if (status & FE_HAS_SIGNAL)
+		if (status & FE_HAS_LOCK)
 			break;
 	}
 
 	/* check if we have a valid signal */
-	if (status) {
+	if (status & FE_HAS_LOCK) {
 		priv->last_tune_failed = 0;
 		return DVBFE_ALGO_SEARCH_SUCCESS;
 	} else {
diff --git a/drivers/media/dvb/frontends/dib7000p.c b/drivers/media/dvb/frontends/dib7000p.c
index 5ceadc285b3..3e1eefada0e 100644
--- a/drivers/media/dvb/frontends/dib7000p.c
+++ b/drivers/media/dvb/frontends/dib7000p.c
@@ -2396,11 +2396,6 @@ struct dvb_frontend *dib7000p_attach(struct i2c_adapter *i2c_adap, u8 i2c_addr,
 	   more common) */
 	st->i2c_master.gated_tuner_i2c_adap.dev.parent = i2c_adap->dev.parent;
 
-	/* FIXME: make sure the dev.parent field is initialized, or else
-	   request_firmware() will hit an OOPS (this should be moved somewhere
-	   more common) */
-	st->i2c_master.gated_tuner_i2c_adap.dev.parent = i2c_adap->dev.parent;
-
 	dibx000_init_i2c_master(&st->i2c_master, DIB7000P, st->i2c_adap, st->i2c_addr);
 
 	/* init 7090 tuner adapter */
diff --git a/drivers/media/dvb/frontends/dib9000.c b/drivers/media/dvb/frontends/dib9000.c
index 80848b4c15d..6201c59a78d 100644
--- a/drivers/media/dvb/frontends/dib9000.c
+++ b/drivers/media/dvb/frontends/dib9000.c
@@ -31,13 +31,6 @@ struct i2c_device {
 	u8 *i2c_write_buffer;
 };
 
-/* lock */
-#define DIB_LOCK struct mutex
-#define DibAcquireLock(lock) mutex_lock_interruptible(lock)
-#define DibReleaseLock(lock) mutex_unlock(lock)
-#define DibInitLock(lock) mutex_init(lock)
-#define DibFreeLock(lock)
-
 struct dib9000_pid_ctrl {
 #define DIB9000_PID_FILTER_CTRL 0
 #define DIB9000_PID_FILTER      1
@@ -82,11 +75,11 @@ struct dib9000_state {
 			} fe_mm[18];
 			u8 memcmd;
 
-			DIB_LOCK mbx_if_lock;	/* to protect read/write operations */
-			DIB_LOCK mbx_lock;	/* to protect the whole mailbox handling */
+			struct mutex mbx_if_lock;	/* to protect read/write operations */
+			struct mutex mbx_lock;	/* to protect the whole mailbox handling */
 
-			DIB_LOCK mem_lock;	/* to protect the memory accesses */
-			DIB_LOCK mem_mbx_lock;	/* to protect the memory-based mailbox */
+			struct mutex mem_lock;	/* to protect the memory accesses */
+			struct mutex mem_mbx_lock;	/* to protect the memory-based mailbox */
 
 #define MBX_MAX_WORDS (256 - 200 - 2)
 #define DIB9000_MSG_CACHE_SIZE 2
@@ -108,7 +101,7 @@ struct dib9000_state {
 	struct i2c_msg msg[2];
 	u8 i2c_write_buffer[255];
 	u8 i2c_read_buffer[255];
-	DIB_LOCK demod_lock;
+	struct mutex demod_lock;
 	u8 get_frontend_internal;
 	struct dib9000_pid_ctrl pid_ctrl[10];
 	s8 pid_ctrl_index; /* -1: empty list; -2: do not use the list */
@@ -446,13 +439,13 @@ static int dib9000_risc_mem_read(struct dib9000_state *state, u8 cmd, u8 * b, u1
 	if (!state->platform.risc.fw_is_running)
 		return -EIO;
 
-	if (DibAcquireLock(&state->platform.risc.mem_lock) < 0) {
+	if (mutex_lock_interruptible(&state->platform.risc.mem_lock) < 0) {
 		dprintk("could not get the lock");
 		return -EINTR;
 	}
 	dib9000_risc_mem_setup(state, cmd | 0x80);
 	dib9000_risc_mem_read_chunks(state, b, len);
-	DibReleaseLock(&state->platform.risc.mem_lock);
+	mutex_unlock(&state->platform.risc.mem_lock);
 	return 0;
 }
 
@@ -462,13 +455,13 @@ static int dib9000_risc_mem_write(struct dib9000_state *state, u8 cmd, const u8
 	if (!state->platform.risc.fw_is_running)
 		return -EIO;
 
-	if (DibAcquireLock(&state->platform.risc.mem_lock) < 0) {
+	if (mutex_lock_interruptible(&state->platform.risc.mem_lock) < 0) {
 		dprintk("could not get the lock");
 		return -EINTR;
 	}
 	dib9000_risc_mem_setup(state, cmd);
 	dib9000_risc_mem_write_chunks(state, b, m->size);
-	DibReleaseLock(&state->platform.risc.mem_lock);
+	mutex_unlock(&state->platform.risc.mem_lock);
 	return 0;
 }
 
@@ -537,7 +530,7 @@ static int dib9000_mbx_send_attr(struct dib9000_state *state, u8 id, u16 * data,
 	if (!state->platform.risc.fw_is_running)
 		return -EINVAL;
 
-	if (DibAcquireLock(&state->platform.risc.mbx_if_lock) < 0) {
+	if (mutex_lock_interruptible(&state->platform.risc.mbx_if_lock) < 0) {
 		dprintk("could not get the lock");
 		return -EINTR;
 	}
@@ -584,7 +577,7 @@ static int dib9000_mbx_send_attr(struct dib9000_state *state, u8 id, u16 * data,
 	ret = (u8) dib9000_write_word_attr(state, 1043, 1 << 14, attr);
 
 out:
-	DibReleaseLock(&state->platform.risc.mbx_if_lock);
+	mutex_unlock(&state->platform.risc.mbx_if_lock);
 
 	return ret;
 }
@@ -602,7 +595,7 @@ static u8 dib9000_mbx_read(struct dib9000_state *state, u16 * data, u8 risc_id,
 	if (!state->platform.risc.fw_is_running)
 		return 0;
 
-	if (DibAcquireLock(&state->platform.risc.mbx_if_lock) < 0) {
+	if (mutex_lock_interruptible(&state->platform.risc.mbx_if_lock) < 0) {
 		dprintk("could not get the lock");
 		return 0;
 	}
@@ -643,7 +636,7 @@ static u8 dib9000_mbx_read(struct dib9000_state *state, u16 * data, u8 risc_id,
 	/* Update register nb_mes_in_TX */
 	dib9000_write_word_attr(state, 1028 + mc_base, 1 << 14, attr);
 
-	DibReleaseLock(&state->platform.risc.mbx_if_lock);
+	mutex_unlock(&state->platform.risc.mbx_if_lock);
 
 	return size + 1;
 }
@@ -708,12 +701,11 @@ static u8 dib9000_mbx_count(struct dib9000_state *state, u8 risc_id, u16 attr)
 static int dib9000_mbx_process(struct dib9000_state *state, u16 attr)
 {
 	int ret = 0;
-	u16 tmp;
 
 	if (!state->platform.risc.fw_is_running)
 		return -1;
 
-	if (DibAcquireLock(&state->platform.risc.mbx_lock) < 0) {
+	if (mutex_lock_interruptible(&state->platform.risc.mbx_lock) < 0) {
 		dprintk("could not get the lock");
 		return -1;
 	}
@@ -721,10 +713,10 @@ static int dib9000_mbx_process(struct dib9000_state *state, u16 attr)
 	if (dib9000_mbx_count(state, 1, attr))	/* 1=RiscB */
 		ret = dib9000_mbx_fetch_to_cache(state, attr);
 
-	tmp = dib9000_read_word_attr(state, 1229, attr);	/* Clear the IRQ */
+	dib9000_read_word_attr(state, 1229, attr);	/* Clear the IRQ */
 /*      if (tmp) */
 /*              dprintk( "cleared IRQ: %x", tmp); */
-	DibReleaseLock(&state->platform.risc.mbx_lock);
+	mutex_unlock(&state->platform.risc.mbx_lock);
 
 	return ret;
 }
@@ -1193,7 +1185,7 @@ static int dib9000_fw_get_channel(struct dvb_frontend *fe)
 	struct dibDVBTChannel *ch;
 	int ret = 0;
 
-	if (DibAcquireLock(&state->platform.risc.mem_mbx_lock) < 0) {
+	if (mutex_lock_interruptible(&state->platform.risc.mem_mbx_lock) < 0) {
 		dprintk("could not get the lock");
 		return -EINTR;
 	}
@@ -1323,7 +1315,7 @@ static int dib9000_fw_get_channel(struct dvb_frontend *fe)
 	}
 
 error:
-	DibReleaseLock(&state->platform.risc.mem_mbx_lock);
+	mutex_unlock(&state->platform.risc.mem_mbx_lock);
 	return ret;
 }
 
@@ -1678,7 +1670,7 @@ static int dib9000_fw_component_bus_xfer(struct i2c_adapter *i2c_adap, struct i2
 		p[12] = 0;
 	}
 
-	if (DibAcquireLock(&state->platform.risc.mem_mbx_lock) < 0) {
+	if (mutex_lock_interruptible(&state->platform.risc.mem_mbx_lock) < 0) {
 		dprintk("could not get the lock");
 		return 0;
 	}
@@ -1692,7 +1684,7 @@ static int dib9000_fw_component_bus_xfer(struct i2c_adapter *i2c_adap, struct i2
 
 	/* do the transaction */
 	if (dib9000_fw_memmbx_sync(state, FE_SYNC_COMPONENT_ACCESS) < 0) {
-		DibReleaseLock(&state->platform.risc.mem_mbx_lock);
+		mutex_unlock(&state->platform.risc.mem_mbx_lock);
 		return 0;
 	}
 
@@ -1700,7 +1692,7 @@ static int dib9000_fw_component_bus_xfer(struct i2c_adapter *i2c_adap, struct i2
 	if ((num > 1) && (msg[1].flags & I2C_M_RD))
 		dib9000_risc_mem_read(state, FE_MM_RW_COMPONENT_ACCESS_BUFFER, msg[1].buf, msg[1].len);
 
-	DibReleaseLock(&state->platform.risc.mem_mbx_lock);
+	mutex_unlock(&state->platform.risc.mem_mbx_lock);
 
 	return num;
 }
@@ -1789,7 +1781,7 @@ int dib9000_fw_pid_filter_ctrl(struct dvb_frontend *fe, u8 onoff)
 		return 0;
 	}
 
-	if (DibAcquireLock(&state->demod_lock) < 0) {
+	if (mutex_lock_interruptible(&state->demod_lock) < 0) {
 		dprintk("could not get the lock");
 		return -EINTR;
 	}
@@ -1799,7 +1791,7 @@ int dib9000_fw_pid_filter_ctrl(struct dvb_frontend *fe, u8 onoff)
 
 	dprintk("PID filter enabled %d", onoff);
 	ret = dib9000_write_word(state, 294 + 1, val);
-	DibReleaseLock(&state->demod_lock);
+	mutex_unlock(&state->demod_lock);
 	return ret;
 
 }
@@ -1824,14 +1816,14 @@ int dib9000_fw_pid_filter(struct dvb_frontend *fe, u8 id, u16 pid, u8 onoff)
 		return 0;
 	}
 
-	if (DibAcquireLock(&state->demod_lock) < 0) {
+	if (mutex_lock_interruptible(&state->demod_lock) < 0) {
 		dprintk("could not get the lock");
 		return -EINTR;
 	}
 	dprintk("Index %x, PID %d, OnOff %d", id, pid, onoff);
 	ret = dib9000_write_word(state, 300 + 1 + id,
 			onoff ? (1 << 13) | pid : 0);
-	DibReleaseLock(&state->demod_lock);
+	mutex_unlock(&state->demod_lock);
 	return ret;
 }
 EXPORT_SYMBOL(dib9000_fw_pid_filter);
@@ -1851,11 +1843,6 @@ static void dib9000_release(struct dvb_frontend *demod)
 	for (index_frontend = 1; (index_frontend < MAX_NUMBER_OF_FRONTENDS) && (st->fe[index_frontend] != NULL); index_frontend++)
 		dvb_frontend_detach(st->fe[index_frontend]);
 
-	DibFreeLock(&state->platform.risc.mbx_if_lock);
-	DibFreeLock(&state->platform.risc.mbx_lock);
-	DibFreeLock(&state->platform.risc.mem_lock);
-	DibFreeLock(&state->platform.risc.mem_mbx_lock);
-	DibFreeLock(&state->demod_lock);
 	dibx000_exit_i2c_master(&st->i2c_master);
 
 	i2c_del_adapter(&st->tuner_adap);
@@ -1875,7 +1862,7 @@ static int dib9000_sleep(struct dvb_frontend *fe)
 	u8 index_frontend;
 	int ret = 0;
 
-	if (DibAcquireLock(&state->demod_lock) < 0) {
+	if (mutex_lock_interruptible(&state->demod_lock) < 0) {
 		dprintk("could not get the lock");
 		return -EINTR;
 	}
@@ -1887,7 +1874,7 @@ static int dib9000_sleep(struct dvb_frontend *fe)
 	ret = dib9000_mbx_send(state, OUT_MSG_FE_SLEEP, NULL, 0);
 
 error:
-	DibReleaseLock(&state->demod_lock);
+	mutex_unlock(&state->demod_lock);
 	return ret;
 }
 
@@ -1905,7 +1892,7 @@ static int dib9000_get_frontend(struct dvb_frontend *fe)
 	int ret = 0;
 
 	if (state->get_frontend_internal == 0) {
-		if (DibAcquireLock(&state->demod_lock) < 0) {
+		if (mutex_lock_interruptible(&state->demod_lock) < 0) {
 			dprintk("could not get the lock");
 			return -EINTR;
 		}
@@ -1964,7 +1951,7 @@ static int dib9000_get_frontend(struct dvb_frontend *fe)
 
 return_value:
 	if (state->get_frontend_internal == 0)
-		DibReleaseLock(&state->demod_lock);
+		mutex_unlock(&state->demod_lock);
 	return ret;
 }
 
@@ -2012,7 +1999,7 @@ static int dib9000_set_frontend(struct dvb_frontend *fe)
 	}
 
 	state->pid_ctrl_index = -1; /* postpone the pid filtering cmd */
-	if (DibAcquireLock(&state->demod_lock) < 0) {
+	if (mutex_lock_interruptible(&state->demod_lock) < 0) {
 		dprintk("could not get the lock");
 		return 0;
 	}
@@ -2081,7 +2068,7 @@ static int dib9000_set_frontend(struct dvb_frontend *fe)
 	/* check the tune result */
 	if (exit_condition == 1) {	/* tune failed */
 		dprintk("tune failed");
-		DibReleaseLock(&state->demod_lock);
+		mutex_unlock(&state->demod_lock);
 		/* tune failed; put all the pid filtering cmd to junk */
 		state->pid_ctrl_index = -1;
 		return 0;
@@ -2137,7 +2124,7 @@ static int dib9000_set_frontend(struct dvb_frontend *fe)
 	/* turn off the diversity for the last frontend */
 	dib9000_fw_set_diversity_in(state->fe[index_frontend - 1], 0);
 
-	DibReleaseLock(&state->demod_lock);
+	mutex_unlock(&state->demod_lock);
 	if (state->pid_ctrl_index >= 0) {
 		u8 index_pid_filter_cmd;
 		u8 pid_ctrl_index = state->pid_ctrl_index;
@@ -2175,7 +2162,7 @@ static int dib9000_read_status(struct dvb_frontend *fe, fe_status_t * stat)
 	u8 index_frontend;
 	u16 lock = 0, lock_slave = 0;
 
-	if (DibAcquireLock(&state->demod_lock) < 0) {
+	if (mutex_lock_interruptible(&state->demod_lock) < 0) {
 		dprintk("could not get the lock");
 		return -EINTR;
 	}
@@ -2197,7 +2184,7 @@ static int dib9000_read_status(struct dvb_frontend *fe, fe_status_t * stat)
 	if ((lock & 0x0008) || (lock_slave & 0x0008))
 		*stat |= FE_HAS_LOCK;
 
-	DibReleaseLock(&state->demod_lock);
+	mutex_unlock(&state->demod_lock);
 
 	return 0;
 }
@@ -2208,30 +2195,30 @@ static int dib9000_read_ber(struct dvb_frontend *fe, u32 * ber)
 	u16 *c;
 	int ret = 0;
 
-	if (DibAcquireLock(&state->demod_lock) < 0) {
+	if (mutex_lock_interruptible(&state->demod_lock) < 0) {
 		dprintk("could not get the lock");
 		return -EINTR;
 	}
-	if (DibAcquireLock(&state->platform.risc.mem_mbx_lock) < 0) {
+	if (mutex_lock_interruptible(&state->platform.risc.mem_mbx_lock) < 0) {
 		dprintk("could not get the lock");
 		ret = -EINTR;
 		goto error;
 	}
 	if (dib9000_fw_memmbx_sync(state, FE_SYNC_CHANNEL) < 0) {
-		DibReleaseLock(&state->platform.risc.mem_mbx_lock);
+		mutex_unlock(&state->platform.risc.mem_mbx_lock);
 		ret = -EIO;
 		goto error;
 	}
 	dib9000_risc_mem_read(state, FE_MM_R_FE_MONITOR,
 			state->i2c_read_buffer, 16 * 2);
-	DibReleaseLock(&state->platform.risc.mem_mbx_lock);
+	mutex_unlock(&state->platform.risc.mem_mbx_lock);
 
 	c = (u16 *)state->i2c_read_buffer;
 
 	*ber = c[10] << 16 | c[11];
 
 error:
-	DibReleaseLock(&state->demod_lock);
+	mutex_unlock(&state->demod_lock);
 	return ret;
 }
 
@@ -2243,7 +2230,7 @@ static int dib9000_read_signal_strength(struct dvb_frontend *fe, u16 * strength)
 	u16 val;
 	int ret = 0;
 
-	if (DibAcquireLock(&state->demod_lock) < 0) {
+	if (mutex_lock_interruptible(&state->demod_lock) < 0) {
 		dprintk("could not get the lock");
 		return -EINTR;
 	}
@@ -2256,18 +2243,18 @@ static int dib9000_read_signal_strength(struct dvb_frontend *fe, u16 * strength)
 			*strength += val;
 	}
 
-	if (DibAcquireLock(&state->platform.risc.mem_mbx_lock) < 0) {
+	if (mutex_lock_interruptible(&state->platform.risc.mem_mbx_lock) < 0) {
 		dprintk("could not get the lock");
 		ret = -EINTR;
 		goto error;
 	}
 	if (dib9000_fw_memmbx_sync(state, FE_SYNC_CHANNEL) < 0) {
-		DibReleaseLock(&state->platform.risc.mem_mbx_lock);
+		mutex_unlock(&state->platform.risc.mem_mbx_lock);
 		ret = -EIO;
 		goto error;
 	}
 	dib9000_risc_mem_read(state, FE_MM_R_FE_MONITOR, (u8 *) c, 16 * 2);
-	DibReleaseLock(&state->platform.risc.mem_mbx_lock);
+	mutex_unlock(&state->platform.risc.mem_mbx_lock);
 
 	val = 65535 - c[4];
 	if (val > 65535 - *strength)
@@ -2276,7 +2263,7 @@ static int dib9000_read_signal_strength(struct dvb_frontend *fe, u16 * strength)
 		*strength += val;
 
 error:
-	DibReleaseLock(&state->demod_lock);
+	mutex_unlock(&state->demod_lock);
 	return ret;
 }
 
@@ -2287,16 +2274,16 @@ static u32 dib9000_get_snr(struct dvb_frontend *fe)
 	u32 n, s, exp;
 	u16 val;
 
-	if (DibAcquireLock(&state->platform.risc.mem_mbx_lock) < 0) {
+	if (mutex_lock_interruptible(&state->platform.risc.mem_mbx_lock) < 0) {
 		dprintk("could not get the lock");
 		return 0;
 	}
 	if (dib9000_fw_memmbx_sync(state, FE_SYNC_CHANNEL) < 0) {
-		DibReleaseLock(&state->platform.risc.mem_mbx_lock);
+		mutex_unlock(&state->platform.risc.mem_mbx_lock);
 		return 0;
 	}
 	dib9000_risc_mem_read(state, FE_MM_R_FE_MONITOR, (u8 *) c, 16 * 2);
-	DibReleaseLock(&state->platform.risc.mem_mbx_lock);
+	mutex_unlock(&state->platform.risc.mem_mbx_lock);
 
 	val = c[7];
 	n = (val >> 4) & 0xff;
@@ -2326,7 +2313,7 @@ static int dib9000_read_snr(struct dvb_frontend *fe, u16 * snr)
 	u8 index_frontend;
 	u32 snr_master;
 
-	if (DibAcquireLock(&state->demod_lock) < 0) {
+	if (mutex_lock_interruptible(&state->demod_lock) < 0) {
 		dprintk("could not get the lock");
 		return -EINTR;
 	}
@@ -2340,7 +2327,7 @@ static int dib9000_read_snr(struct dvb_frontend *fe, u16 * snr)
 	} else
 		*snr = 0;
 
-	DibReleaseLock(&state->demod_lock);
+	mutex_unlock(&state->demod_lock);
 
 	return 0;
 }
@@ -2351,27 +2338,27 @@ static int dib9000_read_unc_blocks(struct dvb_frontend *fe, u32 * unc)
 	u16 *c = (u16 *)state->i2c_read_buffer;
 	int ret = 0;
 
-	if (DibAcquireLock(&state->demod_lock) < 0) {
+	if (mutex_lock_interruptible(&state->demod_lock) < 0) {
 		dprintk("could not get the lock");
 		return -EINTR;
 	}
-	if (DibAcquireLock(&state->platform.risc.mem_mbx_lock) < 0) {
+	if (mutex_lock_interruptible(&state->platform.risc.mem_mbx_lock) < 0) {
 		dprintk("could not get the lock");
 		ret = -EINTR;
 		goto error;
 	}
 	if (dib9000_fw_memmbx_sync(state, FE_SYNC_CHANNEL) < 0) {
-		DibReleaseLock(&state->platform.risc.mem_mbx_lock);
+		mutex_unlock(&state->platform.risc.mem_mbx_lock);
 		ret = -EIO;
 		goto error;
 	}
 	dib9000_risc_mem_read(state, FE_MM_R_FE_MONITOR, (u8 *) c, 16 * 2);
-	DibReleaseLock(&state->platform.risc.mem_mbx_lock);
+	mutex_unlock(&state->platform.risc.mem_mbx_lock);
 
 	*unc = c[12];
 
 error:
-	DibReleaseLock(&state->demod_lock);
+	mutex_unlock(&state->demod_lock);
 	return ret;
 }
 
@@ -2514,11 +2501,11 @@ struct dvb_frontend *dib9000_attach(struct i2c_adapter *i2c_adap, u8 i2c_addr, c
 	st->gpio_val = DIB9000_GPIO_DEFAULT_VALUES;
 	st->gpio_pwm_pos = DIB9000_GPIO_DEFAULT_PWM_POS;
 
-	DibInitLock(&st->platform.risc.mbx_if_lock);
-	DibInitLock(&st->platform.risc.mbx_lock);
-	DibInitLock(&st->platform.risc.mem_lock);
-	DibInitLock(&st->platform.risc.mem_mbx_lock);
-	DibInitLock(&st->demod_lock);
+	mutex_init(&st->platform.risc.mbx_if_lock);
+	mutex_init(&st->platform.risc.mbx_lock);
+	mutex_init(&st->platform.risc.mem_lock);
+	mutex_init(&st->platform.risc.mem_mbx_lock);
+	mutex_init(&st->demod_lock);
 	st->get_frontend_internal = 0;
 
 	st->pid_ctrl_index = -2;
diff --git a/drivers/media/dvb/frontends/drxd.h b/drivers/media/dvb/frontends/drxd.h
index 34398738f9b..216c8c3702f 100644
--- a/drivers/media/dvb/frontends/drxd.h
+++ b/drivers/media/dvb/frontends/drxd.h
@@ -51,9 +51,23 @@ struct drxd_config {
 	 s16(*osc_deviation) (void *priv, s16 dev, int flag);
 };
 
+#if defined(CONFIG_DVB_DRXD) || \
+			(defined(CONFIG_DVB_DRXD_MODULE) && defined(MODULE))
 extern
 struct dvb_frontend *drxd_attach(const struct drxd_config *config,
 				 void *priv, struct i2c_adapter *i2c,
 				 struct device *dev);
+#else
+static inline
+struct dvb_frontend *drxd_attach(const struct drxd_config *config,
+				 void *priv, struct i2c_adapter *i2c,
+				 struct device *dev)
+{
+	printk(KERN_INFO "%s: not probed - driver disabled by Kconfig\n",
+	       __func__);
+	return NULL;
+}
+#endif
+
 extern int drxd_config_i2c(struct dvb_frontend *, int);
 #endif
diff --git a/drivers/media/dvb/frontends/drxk_hard.c b/drivers/media/dvb/frontends/drxk_hard.c
index a414b1f2b6a..60b868faeac 100644
--- a/drivers/media/dvb/frontends/drxk_hard.c
+++ b/drivers/media/dvb/frontends/drxk_hard.c
@@ -1380,20 +1380,20 @@ static int DownloadMicrocode(struct drxk_state *state,
 			     const u8 pMCImage[], u32 Length)
 {
 	const u8 *pSrc = pMCImage;
-	u16 Flags;
-	u16 Drain;
 	u32 Address;
 	u16 nBlocks;
 	u16 BlockSize;
-	u16 BlockCRC;
 	u32 offset = 0;
 	u32 i;
 	int status = 0;
 
 	dprintk(1, "\n");
 
-	/* down the drain (we don care about MAGIC_WORD) */
+	/* down the drain (we don't care about MAGIC_WORD) */
+#if 0
+	/* For future reference */
 	Drain = (pSrc[0] << 8) | pSrc[1];
+#endif
 	pSrc += sizeof(u16);
 	offset += sizeof(u16);
 	nBlocks = (pSrc[0] << 8) | pSrc[1];
@@ -1410,11 +1410,17 @@ static int DownloadMicrocode(struct drxk_state *state,
 		pSrc += sizeof(u16);
 		offset += sizeof(u16);
 
+#if 0
+		/* For future reference */
 		Flags = (pSrc[0] << 8) | pSrc[1];
+#endif
 		pSrc += sizeof(u16);
 		offset += sizeof(u16);
 
+#if 0
+		/* For future reference */
 		BlockCRC = (pSrc[0] << 8) | pSrc[1];
+#endif
 		pSrc += sizeof(u16);
 		offset += sizeof(u16);
 
@@ -5829,7 +5835,7 @@ static int WriteGPIO(struct drxk_state *state)
 		}
 		if (state->UIO_mask & 0x0002) { /* UIO-2 */
 			/* write to io pad configuration register - output mode */
-			status = write16(state, SIO_PDR_SMA_TX_CFG__A, state->m_GPIOCfg);
+			status = write16(state, SIO_PDR_SMA_RX_CFG__A, state->m_GPIOCfg);
 			if (status < 0)
 				goto error;
 
@@ -5848,7 +5854,7 @@ static int WriteGPIO(struct drxk_state *state)
 		}
 		if (state->UIO_mask & 0x0004) { /* UIO-3 */
 			/* write to io pad configuration register - output mode */
-			status = write16(state, SIO_PDR_SMA_TX_CFG__A, state->m_GPIOCfg);
+			status = write16(state, SIO_PDR_GPIO_CFG__A, state->m_GPIOCfg);
 			if (status < 0)
 				goto error;
 
diff --git a/drivers/media/dvb/frontends/drxk_map.h b/drivers/media/dvb/frontends/drxk_map.h
index 9b11a832886..23e16c12f23 100644
--- a/drivers/media/dvb/frontends/drxk_map.h
+++ b/drivers/media/dvb/frontends/drxk_map.h
@@ -432,6 +432,7 @@
 #define  SIO_PDR_UIO_OUT_LO__A                                             0x7F0016
 #define  SIO_PDR_OHW_CFG__A                                                0x7F001F
 #define    SIO_PDR_OHW_CFG_FREF_SEL__M                                     0x3
+#define  SIO_PDR_GPIO_CFG__A                                               0x7F0021
 #define  SIO_PDR_MSTRT_CFG__A                                              0x7F0025
 #define  SIO_PDR_MERR_CFG__A                                               0x7F0026
 #define  SIO_PDR_MCLK_CFG__A                                               0x7F0028
@@ -446,4 +447,5 @@
 #define  SIO_PDR_MD5_CFG__A                                                0x7F0030
 #define  SIO_PDR_MD6_CFG__A                                                0x7F0031
 #define  SIO_PDR_MD7_CFG__A                                                0x7F0032
+#define  SIO_PDR_SMA_RX_CFG__A                                             0x7F0037
 #define  SIO_PDR_SMA_TX_CFG__A                                             0x7F0038
diff --git a/drivers/media/dvb/frontends/ds3000.c b/drivers/media/dvb/frontends/ds3000.c
index af65d013db1..4c8ac2657c4 100644
--- a/drivers/media/dvb/frontends/ds3000.c
+++ b/drivers/media/dvb/frontends/ds3000.c
@@ -1114,7 +1114,10 @@ static int ds3000_set_frontend(struct dvb_frontend *fe)
 			ds3000_writereg(state,
 				ds3000_dvbs2_init_tab[i],
 				ds3000_dvbs2_init_tab[i + 1]);
-		ds3000_writereg(state, 0xfe, 0x98);
+		if (c->symbol_rate >= 30000000)
+			ds3000_writereg(state, 0xfe, 0x54);
+		else
+			ds3000_writereg(state, 0xfe, 0x98);
 		break;
 	default:
 		return 1;
diff --git a/drivers/media/dvb/frontends/it913x-fe.c b/drivers/media/dvb/frontends/it913x-fe.c
index 84df03c2917..708cbf19791 100644
--- a/drivers/media/dvb/frontends/it913x-fe.c
+++ b/drivers/media/dvb/frontends/it913x-fe.c
@@ -633,10 +633,9 @@ static int it913x_fe_read_snr(struct dvb_frontend *fe, u16 *snr)
 static int it913x_fe_read_ber(struct dvb_frontend *fe, u32 *ber)
 {
 	struct it913x_fe_state *state = fe->demodulator_priv;
-	int ret;
 	u8 reg[5];
 	/* Read Aborted Packets and Pre-Viterbi error rate 5 bytes */
-	ret = it913x_read_reg(state, RSD_ABORT_PKT_LSB, reg, sizeof(reg));
+	it913x_read_reg(state, RSD_ABORT_PKT_LSB, reg, sizeof(reg));
 	state->ucblocks += (u32)(reg[1] << 8) | reg[0];
 	*ber = (u32)(reg[4] << 16) | (reg[3] << 8) | reg[2];
 	return 0;
@@ -658,10 +657,9 @@ static int it913x_fe_get_frontend(struct dvb_frontend *fe)
 {
 	struct dtv_frontend_properties *p = &fe->dtv_property_cache;
 	struct it913x_fe_state *state = fe->demodulator_priv;
-	int ret;
 	u8 reg[8];
 
-	ret = it913x_read_reg(state, REG_TPSD_TX_MODE, reg, sizeof(reg));
+	it913x_read_reg(state, REG_TPSD_TX_MODE, reg, sizeof(reg));
 
 	if (reg[3] < 3)
 		p->modulation = fe_con[reg[3]];
@@ -691,25 +689,25 @@ static int it913x_fe_set_frontend(struct dvb_frontend *fe)
 {
 	struct dtv_frontend_properties *p = &fe->dtv_property_cache;
 	struct it913x_fe_state *state = fe->demodulator_priv;
-	int ret, i;
+	int i;
 	u8 empty_ch, last_ch;
 
 	state->it913x_status = 0;
 
 	/* Set bw*/
-	ret = it913x_fe_select_bw(state, p->bandwidth_hz,
+	it913x_fe_select_bw(state, p->bandwidth_hz,
 		state->adcFrequency);
 
 	/* Training Mode Off */
-	ret = it913x_write_reg(state, PRO_LINK, TRAINING_MODE, 0x0);
+	it913x_write_reg(state, PRO_LINK, TRAINING_MODE, 0x0);
 
 	/* Clear Empty Channel */
-	ret = it913x_write_reg(state, PRO_DMOD, EMPTY_CHANNEL_STATUS, 0x0);
+	it913x_write_reg(state, PRO_DMOD, EMPTY_CHANNEL_STATUS, 0x0);
 
 	/* Clear bits */
-	ret = it913x_write_reg(state, PRO_DMOD, MP2IF_SYNC_LK, 0x0);
+	it913x_write_reg(state, PRO_DMOD, MP2IF_SYNC_LK, 0x0);
 	/* LED on */
-	ret = it913x_write_reg(state, PRO_LINK, GPIOH3_O, 0x1);
+	it913x_write_reg(state, PRO_LINK, GPIOH3_O, 0x1);
 	/* Select Band*/
 	if ((p->frequency >= 51000000) && (p->frequency <= 230000000))
 		i = 0;
@@ -720,7 +718,7 @@ static int it913x_fe_set_frontend(struct dvb_frontend *fe)
 	else
 		return -EOPNOTSUPP;
 
-	ret = it913x_write_reg(state, PRO_DMOD, FREE_BAND, i);
+	it913x_write_reg(state, PRO_DMOD, FREE_BAND, i);
 
 	deb_info("Frontend Set Tuner Type %02x", state->tuner_type);
 	switch (state->tuner_type) {
@@ -730,7 +728,7 @@ static int it913x_fe_set_frontend(struct dvb_frontend *fe)
 	case IT9135_60:
 	case IT9135_61:
 	case IT9135_62:
-		ret = it9137_set_tuner(state,
+		it9137_set_tuner(state,
 			p->bandwidth_hz, p->frequency);
 		break;
 	default:
@@ -742,9 +740,9 @@ static int it913x_fe_set_frontend(struct dvb_frontend *fe)
 		break;
 	}
 	/* LED off */
-	ret = it913x_write_reg(state, PRO_LINK, GPIOH3_O, 0x0);
+	it913x_write_reg(state, PRO_LINK, GPIOH3_O, 0x0);
 	/* Trigger ofsm */
-	ret = it913x_write_reg(state, PRO_DMOD, TRIGGER_OFSM, 0x0);
+	it913x_write_reg(state, PRO_DMOD, TRIGGER_OFSM, 0x0);
 	last_ch = 2;
 	for (i = 0; i < 40; ++i) {
 		empty_ch = it913x_read_reg_u8(state, EMPTY_CHANNEL_STATUS);
diff --git a/drivers/media/dvb/frontends/lg2160.c b/drivers/media/dvb/frontends/lg2160.c
new file mode 100644
index 00000000000..a3ab1a5b659
--- /dev/null
+++ b/drivers/media/dvb/frontends/lg2160.c
@@ -0,0 +1,1468 @@
+/*
+ *    Support for LG2160 - ATSC/MH
+ *
+ *    Copyright (C) 2010 Michael Krufky <mkrufky@linuxtv.org>
+ *
+ *    This program is free software; you can redistribute it and/or modify
+ *    it under the terms of the GNU General Public License as published by
+ *    the Free Software Foundation; either version 2 of the License, or
+ *    (at your option) any later version.
+ *
+ *    This program is distributed in the hope that it will be useful,
+ *    but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ *    GNU General Public License for more details.
+ *
+ *    You should have received a copy of the GNU General Public License
+ *    along with this program; if not, write to the Free Software
+ *    Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+ *
+ */
+
+#include <linux/jiffies.h>
+#include <linux/dvb/frontend.h>
+#include "lg2160.h"
+
+static int debug;
+module_param(debug, int, 0644);
+MODULE_PARM_DESC(debug, "set debug level (info=1, reg=2 (or-able))");
+
+#define DBG_INFO 1
+#define DBG_REG  2
+
+#define lg_printk(kern, fmt, arg...)					\
+	printk(kern "%s: " fmt, __func__, ##arg)
+
+#define lg_info(fmt, arg...)	printk(KERN_INFO "lg2160: " fmt, ##arg)
+#define lg_warn(fmt, arg...)	lg_printk(KERN_WARNING,       fmt, ##arg)
+#define lg_err(fmt, arg...)	lg_printk(KERN_ERR,           fmt, ##arg)
+#define lg_dbg(fmt, arg...) if (debug & DBG_INFO)			\
+				lg_printk(KERN_DEBUG,         fmt, ##arg)
+#define lg_reg(fmt, arg...) if (debug & DBG_REG)			\
+				lg_printk(KERN_DEBUG,         fmt, ##arg)
+
+#define lg_fail(ret)							\
+({									\
+	int __ret;							\
+	__ret = (ret < 0);						\
+	if (__ret)							\
+		lg_err("error %d on line %d\n",	ret, __LINE__);		\
+	__ret;								\
+})
+
+struct lg216x_state {
+	struct i2c_adapter *i2c_adap;
+	const struct lg2160_config *cfg;
+
+	struct dvb_frontend frontend;
+
+	u32 current_frequency;
+	u8 parade_id;
+	u8 fic_ver;
+	unsigned int last_reset;
+};
+
+/* ------------------------------------------------------------------------ */
+
+static int lg216x_write_reg(struct lg216x_state *state, u16 reg, u8 val)
+{
+	int ret;
+	u8 buf[] = { reg >> 8, reg & 0xff, val };
+	struct i2c_msg msg = {
+		.addr = state->cfg->i2c_addr, .flags = 0,
+		.buf = buf, .len = 3,
+	};
+
+	lg_reg("reg: 0x%04x, val: 0x%02x\n", reg, val);
+
+	ret = i2c_transfer(state->i2c_adap, &msg, 1);
+
+	if (ret != 1) {
+		lg_err("error (addr %02x %02x <- %02x, err = %i)\n",
+		       msg.buf[0], msg.buf[1], msg.buf[2], ret);
+		if (ret < 0)
+			return ret;
+		else
+			return -EREMOTEIO;
+	}
+	return 0;
+}
+
+static int lg216x_read_reg(struct lg216x_state *state, u16 reg, u8 *val)
+{
+	int ret;
+	u8 reg_buf[] = { reg >> 8, reg & 0xff };
+	struct i2c_msg msg[] = {
+		{ .addr = state->cfg->i2c_addr,
+		  .flags = 0, .buf = reg_buf, .len = 2 },
+		{ .addr = state->cfg->i2c_addr,
+		  .flags = I2C_M_RD, .buf = val, .len = 1 },
+	};
+
+	lg_reg("reg: 0x%04x\n", reg);
+
+	ret = i2c_transfer(state->i2c_adap, msg, 2);
+
+	if (ret != 2) {
+		lg_err("error (addr %02x reg %04x error (ret == %i)\n",
+		       state->cfg->i2c_addr, reg, ret);
+		if (ret < 0)
+			return ret;
+		else
+			return -EREMOTEIO;
+	}
+	return 0;
+}
+
+struct lg216x_reg {
+	u16 reg;
+	u8 val;
+};
+
+static int lg216x_write_regs(struct lg216x_state *state,
+			     struct lg216x_reg *regs, int len)
+{
+	int i, ret;
+
+	lg_reg("writing %d registers...\n", len);
+
+	for (i = 0; i < len - 1; i++) {
+		ret = lg216x_write_reg(state, regs[i].reg, regs[i].val);
+		if (lg_fail(ret))
+			return ret;
+	}
+	return 0;
+}
+
+static int lg216x_set_reg_bit(struct lg216x_state *state,
+			      u16 reg, int bit, int onoff)
+{
+	u8 val;
+	int ret;
+
+	lg_reg("reg: 0x%04x, bit: %d, level: %d\n", reg, bit, onoff);
+
+	ret = lg216x_read_reg(state, reg, &val);
+	if (lg_fail(ret))
+		goto fail;
+
+	val &= ~(1 << bit);
+	val |= (onoff & 1) << bit;
+
+	ret = lg216x_write_reg(state, reg, val);
+	lg_fail(ret);
+fail:
+	return ret;
+}
+
+/* ------------------------------------------------------------------------ */
+
+static int lg216x_i2c_gate_ctrl(struct dvb_frontend *fe, int enable)
+{
+	struct lg216x_state *state = fe->demodulator_priv;
+	int ret;
+
+	if (state->cfg->deny_i2c_rptr)
+		return 0;
+
+	lg_dbg("(%d)\n", enable);
+
+	ret = lg216x_set_reg_bit(state, 0x0000, 0, enable ? 0 : 1);
+
+	msleep(1);
+
+	return ret;
+}
+
+static int lg216x_soft_reset(struct lg216x_state *state)
+{
+	int ret;
+
+	lg_dbg("\n");
+
+	ret = lg216x_write_reg(state, 0x0002, 0x00);
+	if (lg_fail(ret))
+		goto fail;
+
+	msleep(20);
+	ret = lg216x_write_reg(state, 0x0002, 0x01);
+	if (lg_fail(ret))
+		goto fail;
+
+	state->last_reset = jiffies_to_msecs(jiffies);
+fail:
+	return ret;
+}
+
+static int lg216x_initialize(struct lg216x_state *state)
+{
+	int ret;
+
+	static struct lg216x_reg lg2160_init[] = {
+#if 0
+		{ .reg = 0x0015, .val = 0xe6 },
+#else
+		{ .reg = 0x0015, .val = 0xf7 },
+		{ .reg = 0x001b, .val = 0x52 },
+		{ .reg = 0x0208, .val = 0x00 },
+		{ .reg = 0x0209, .val = 0x82 },
+		{ .reg = 0x0210, .val = 0xf9 },
+		{ .reg = 0x020a, .val = 0x00 },
+		{ .reg = 0x020b, .val = 0x82 },
+		{ .reg = 0x020d, .val = 0x28 },
+		{ .reg = 0x020f, .val = 0x14 },
+#endif
+	};
+
+	static struct lg216x_reg lg2161_init[] = {
+		{ .reg = 0x0000, .val = 0x41 },
+		{ .reg = 0x0001, .val = 0xfb },
+		{ .reg = 0x0216, .val = 0x00 },
+		{ .reg = 0x0219, .val = 0x00 },
+		{ .reg = 0x021b, .val = 0x55 },
+		{ .reg = 0x0606, .val = 0x0a },
+	};
+
+	switch (state->cfg->lg_chip) {
+	case LG2160:
+		ret = lg216x_write_regs(state,
+					lg2160_init, ARRAY_SIZE(lg2160_init));
+		break;
+	case LG2161:
+		ret = lg216x_write_regs(state,
+					lg2161_init, ARRAY_SIZE(lg2161_init));
+		break;
+	default:
+		ret = -EINVAL;
+		break;
+	}
+	if (lg_fail(ret))
+		goto fail;
+
+	ret = lg216x_soft_reset(state);
+	lg_fail(ret);
+fail:
+	return ret;
+}
+
+/* ------------------------------------------------------------------------ */
+
+static int lg216x_set_if(struct lg216x_state *state)
+{
+	u8 val;
+	int ret;
+
+	lg_dbg("%d KHz\n", state->cfg->if_khz);
+
+	ret = lg216x_read_reg(state, 0x0132, &val);
+	if (lg_fail(ret))
+		goto fail;
+
+	val &= 0xfb;
+	val |= (0 == state->cfg->if_khz) ? 0x04 : 0x00;
+
+	ret = lg216x_write_reg(state, 0x0132, val);
+	lg_fail(ret);
+
+	/* if NOT zero IF, 6 MHz is the default */
+fail:
+	return ret;
+}
+
+/* ------------------------------------------------------------------------ */
+
+static int lg2160_agc_fix(struct lg216x_state *state,
+			  int if_agc_fix, int rf_agc_fix)
+{
+	u8 val;
+	int ret;
+
+	ret = lg216x_read_reg(state, 0x0100, &val);
+	if (lg_fail(ret))
+		goto fail;
+
+	val &= 0xf3;
+	val |= (if_agc_fix) ? 0x08 : 0x00;
+	val |= (rf_agc_fix) ? 0x04 : 0x00;
+
+	ret = lg216x_write_reg(state, 0x0100, val);
+	lg_fail(ret);
+fail:
+	return ret;
+}
+
+#if 0
+static int lg2160_agc_freeze(struct lg216x_state *state,
+			     int if_agc_freeze, int rf_agc_freeze)
+{
+	u8 val;
+	int ret;
+
+	ret = lg216x_read_reg(state, 0x0100, &val);
+	if (lg_fail(ret))
+		goto fail;
+
+	val &= 0xcf;
+	val |= (if_agc_freeze) ? 0x20 : 0x00;
+	val |= (rf_agc_freeze) ? 0x10 : 0x00;
+
+	ret = lg216x_write_reg(state, 0x0100, val);
+	lg_fail(ret);
+fail:
+	return ret;
+}
+#endif
+
+static int lg2160_agc_polarity(struct lg216x_state *state,
+			       int if_agc_polarity, int rf_agc_polarity)
+{
+	u8 val;
+	int ret;
+
+	ret = lg216x_read_reg(state, 0x0100, &val);
+	if (lg_fail(ret))
+		goto fail;
+
+	val &= 0xfc;
+	val |= (if_agc_polarity) ? 0x02 : 0x00;
+	val |= (rf_agc_polarity) ? 0x01 : 0x00;
+
+	ret = lg216x_write_reg(state, 0x0100, val);
+	lg_fail(ret);
+fail:
+	return ret;
+}
+
+static int lg2160_tuner_pwr_save_polarity(struct lg216x_state *state,
+					  int polarity)
+{
+	u8 val;
+	int ret;
+
+	ret = lg216x_read_reg(state, 0x0008, &val);
+	if (lg_fail(ret))
+		goto fail;
+
+	val &= 0xfe;
+	val |= (polarity) ? 0x01 : 0x00;
+
+	ret = lg216x_write_reg(state, 0x0008, val);
+	lg_fail(ret);
+fail:
+	return ret;
+}
+
+static int lg2160_spectrum_polarity(struct lg216x_state *state,
+				    int inverted)
+{
+	u8 val;
+	int ret;
+
+	ret = lg216x_read_reg(state, 0x0132, &val);
+	if (lg_fail(ret))
+		goto fail;
+
+	val &= 0xfd;
+	val |= (inverted) ? 0x02 : 0x00;
+
+	ret = lg216x_write_reg(state, 0x0132, val);
+	lg_fail(ret);
+fail:
+	return lg216x_soft_reset(state);
+}
+
+static int lg2160_tuner_pwr_save(struct lg216x_state *state, int onoff)
+{
+	u8 val;
+	int ret;
+
+	ret = lg216x_read_reg(state, 0x0007, &val);
+	if (lg_fail(ret))
+		goto fail;
+
+	val &= 0xbf;
+	val |= (onoff) ? 0x40 : 0x00;
+
+	ret = lg216x_write_reg(state, 0x0007, val);
+	lg_fail(ret);
+fail:
+	return ret;
+}
+
+static int lg216x_set_parade(struct lg216x_state *state, int id)
+{
+	int ret;
+
+	ret = lg216x_write_reg(state, 0x013e, id & 0x7f);
+	if (lg_fail(ret))
+		goto fail;
+
+	state->parade_id = id & 0x7f;
+fail:
+	return ret;
+}
+
+static int lg216x_set_ensemble(struct lg216x_state *state, int id)
+{
+	int ret;
+	u16 reg;
+	u8 val;
+
+	switch (state->cfg->lg_chip) {
+	case LG2160:
+		reg = 0x0400;
+		break;
+	case LG2161:
+	default:
+		reg = 0x0500;
+		break;
+	}
+
+	ret = lg216x_read_reg(state, reg, &val);
+	if (lg_fail(ret))
+		goto fail;
+
+	val &= 0xfe;
+	val |= (id) ? 0x01 : 0x00;
+
+	ret = lg216x_write_reg(state, reg, val);
+	lg_fail(ret);
+fail:
+	return ret;
+}
+
+static int lg2160_set_spi_clock(struct lg216x_state *state)
+{
+	u8 val;
+	int ret;
+
+	ret = lg216x_read_reg(state, 0x0014, &val);
+	if (lg_fail(ret))
+		goto fail;
+
+	val &= 0xf3;
+	val |= (state->cfg->spi_clock << 2);
+
+	ret = lg216x_write_reg(state, 0x0014, val);
+	lg_fail(ret);
+fail:
+	return ret;
+}
+
+static int lg2161_set_output_interface(struct lg216x_state *state)
+{
+	u8 val;
+	int ret;
+
+	ret = lg216x_read_reg(state, 0x0014, &val);
+	if (lg_fail(ret))
+		goto fail;
+
+	val &= ~0x07;
+	val |= state->cfg->output_if; /* FIXME: needs sanity check */
+
+	ret = lg216x_write_reg(state, 0x0014, val);
+	lg_fail(ret);
+fail:
+	return ret;
+}
+
+static int lg216x_enable_fic(struct lg216x_state *state, int onoff)
+{
+	int ret;
+
+	ret = lg216x_write_reg(state, 0x0017, 0x23);
+	if (lg_fail(ret))
+		goto fail;
+
+	ret = lg216x_write_reg(state, 0x0016, 0xfc);
+	if (lg_fail(ret))
+		goto fail;
+
+	switch (state->cfg->lg_chip) {
+	case LG2160:
+		ret = lg216x_write_reg(state, 0x0016,
+				       0xfc | ((onoff) ? 0x02 : 0x00));
+		break;
+	case LG2161:
+		ret = lg216x_write_reg(state, 0x0016, (onoff) ? 0x10 : 0x00);
+		break;
+	}
+	if (lg_fail(ret))
+		goto fail;
+
+	ret = lg216x_initialize(state);
+	if (lg_fail(ret))
+		goto fail;
+
+	if (onoff) {
+		ret = lg216x_write_reg(state, 0x0017, 0x03);
+		lg_fail(ret);
+	}
+fail:
+	return ret;
+}
+
+/* ------------------------------------------------------------------------ */
+
+static int lg216x_get_fic_version(struct lg216x_state *state, u8 *ficver)
+{
+	u8 val;
+	int ret;
+
+	*ficver = 0xff; /* invalid value */
+
+	ret = lg216x_read_reg(state, 0x0128, &val);
+	if (lg_fail(ret))
+		goto fail;
+
+	*ficver = (val >> 3) & 0x1f;
+fail:
+	return ret;
+}
+
+#if 0
+static int lg2160_get_parade_id(struct lg216x_state *state, u8 *id)
+{
+	u8 val;
+	int ret;
+
+	*id = 0xff; /* invalid value */
+
+	ret = lg216x_read_reg(state, 0x0123, &val);
+	if (lg_fail(ret))
+		goto fail;
+
+	*id = val & 0x7f;
+fail:
+	return ret;
+}
+#endif
+
+static int lg216x_get_nog(struct lg216x_state *state, u8 *nog)
+{
+	u8 val;
+	int ret;
+
+	*nog = 0xff; /* invalid value */
+
+	ret = lg216x_read_reg(state, 0x0124, &val);
+	if (lg_fail(ret))
+		goto fail;
+
+	*nog = ((val >> 4) & 0x07) + 1;
+fail:
+	return ret;
+}
+
+static int lg216x_get_tnog(struct lg216x_state *state, u8 *tnog)
+{
+	u8 val;
+	int ret;
+
+	*tnog = 0xff; /* invalid value */
+
+	ret = lg216x_read_reg(state, 0x0125, &val);
+	if (lg_fail(ret))
+		goto fail;
+
+	*tnog = val & 0x1f;
+fail:
+	return ret;
+}
+
+static int lg216x_get_sgn(struct lg216x_state *state, u8 *sgn)
+{
+	u8 val;
+	int ret;
+
+	*sgn = 0xff; /* invalid value */
+
+	ret = lg216x_read_reg(state, 0x0124, &val);
+	if (lg_fail(ret))
+		goto fail;
+
+	*sgn = val & 0x0f;
+fail:
+	return ret;
+}
+
+static int lg216x_get_prc(struct lg216x_state *state, u8 *prc)
+{
+	u8 val;
+	int ret;
+
+	*prc = 0xff; /* invalid value */
+
+	ret = lg216x_read_reg(state, 0x0125, &val);
+	if (lg_fail(ret))
+		goto fail;
+
+	*prc = ((val >> 5) & 0x07) + 1;
+fail:
+	return ret;
+}
+
+/* ------------------------------------------------------------------------ */
+
+static int lg216x_get_rs_frame_mode(struct lg216x_state *state,
+				    enum atscmh_rs_frame_mode *rs_framemode)
+{
+	u8 val;
+	int ret;
+
+	switch (state->cfg->lg_chip) {
+	case LG2160:
+		ret = lg216x_read_reg(state, 0x0410, &val);
+		break;
+	case LG2161:
+		ret = lg216x_read_reg(state, 0x0513, &val);
+		break;
+	default:
+		ret = -EINVAL;
+	}
+	if (lg_fail(ret))
+		goto fail;
+
+	switch ((val >> 4) & 0x03) {
+#if 1
+	default:
+#endif
+	case 0x00:
+		*rs_framemode = ATSCMH_RSFRAME_PRI_ONLY;
+		break;
+	case 0x01:
+		*rs_framemode = ATSCMH_RSFRAME_PRI_SEC;
+		break;
+#if 0
+	default:
+		*rs_framemode = ATSCMH_RSFRAME_RES;
+		break;
+#endif
+	}
+fail:
+	return ret;
+}
+
+static
+int lg216x_get_rs_frame_ensemble(struct lg216x_state *state,
+				 enum atscmh_rs_frame_ensemble *rs_frame_ens)
+{
+	u8 val;
+	int ret;
+
+	switch (state->cfg->lg_chip) {
+	case LG2160:
+		ret = lg216x_read_reg(state, 0x0400, &val);
+		break;
+	case LG2161:
+		ret = lg216x_read_reg(state, 0x0500, &val);
+		break;
+	default:
+		ret = -EINVAL;
+	}
+	if (lg_fail(ret))
+		goto fail;
+
+	val &= 0x01;
+	*rs_frame_ens = (enum atscmh_rs_frame_ensemble) val;
+fail:
+	return ret;
+}
+
+static int lg216x_get_rs_code_mode(struct lg216x_state *state,
+				   enum atscmh_rs_code_mode *rs_code_pri,
+				   enum atscmh_rs_code_mode *rs_code_sec)
+{
+	u8 val;
+	int ret;
+
+	switch (state->cfg->lg_chip) {
+	case LG2160:
+		ret = lg216x_read_reg(state, 0x0410, &val);
+		break;
+	case LG2161:
+		ret = lg216x_read_reg(state, 0x0513, &val);
+		break;
+	default:
+		ret = -EINVAL;
+	}
+	if (lg_fail(ret))
+		goto fail;
+
+	*rs_code_pri = (enum atscmh_rs_code_mode) ((val >> 2) & 0x03);
+	*rs_code_sec = (enum atscmh_rs_code_mode) (val & 0x03);
+fail:
+	return ret;
+}
+
+static int lg216x_get_sccc_block_mode(struct lg216x_state *state,
+				      enum atscmh_sccc_block_mode *sccc_block)
+{
+	u8 val;
+	int ret;
+
+	switch (state->cfg->lg_chip) {
+	case LG2160:
+		ret = lg216x_read_reg(state, 0x0315, &val);
+		break;
+	case LG2161:
+		ret = lg216x_read_reg(state, 0x0511, &val);
+		break;
+	default:
+		ret = -EINVAL;
+	}
+	if (lg_fail(ret))
+		goto fail;
+
+	switch (val & 0x03) {
+	case 0x00:
+		*sccc_block = ATSCMH_SCCC_BLK_SEP;
+		break;
+	case 0x01:
+		*sccc_block = ATSCMH_SCCC_BLK_COMB;
+		break;
+	default:
+		*sccc_block = ATSCMH_SCCC_BLK_RES;
+		break;
+	}
+fail:
+	return ret;
+}
+
+static int lg216x_get_sccc_code_mode(struct lg216x_state *state,
+				     enum atscmh_sccc_code_mode *mode_a,
+				     enum atscmh_sccc_code_mode *mode_b,
+				     enum atscmh_sccc_code_mode *mode_c,
+				     enum atscmh_sccc_code_mode *mode_d)
+{
+	u8 val;
+	int ret;
+
+	switch (state->cfg->lg_chip) {
+	case LG2160:
+		ret = lg216x_read_reg(state, 0x0316, &val);
+		break;
+	case LG2161:
+		ret = lg216x_read_reg(state, 0x0512, &val);
+		break;
+	default:
+		ret = -EINVAL;
+	}
+	if (lg_fail(ret))
+		goto fail;
+
+	switch ((val >> 6) & 0x03) {
+	case 0x00:
+		*mode_a = ATSCMH_SCCC_CODE_HLF;
+		break;
+	case 0x01:
+		*mode_a = ATSCMH_SCCC_CODE_QTR;
+		break;
+	default:
+		*mode_a = ATSCMH_SCCC_CODE_RES;
+		break;
+	}
+
+	switch ((val >> 4) & 0x03) {
+	case 0x00:
+		*mode_b = ATSCMH_SCCC_CODE_HLF;
+		break;
+	case 0x01:
+		*mode_b = ATSCMH_SCCC_CODE_QTR;
+		break;
+	default:
+		*mode_b = ATSCMH_SCCC_CODE_RES;
+		break;
+	}
+
+	switch ((val >> 2) & 0x03) {
+	case 0x00:
+		*mode_c = ATSCMH_SCCC_CODE_HLF;
+		break;
+	case 0x01:
+		*mode_c = ATSCMH_SCCC_CODE_QTR;
+		break;
+	default:
+		*mode_c = ATSCMH_SCCC_CODE_RES;
+		break;
+	}
+
+	switch (val & 0x03) {
+	case 0x00:
+		*mode_d = ATSCMH_SCCC_CODE_HLF;
+		break;
+	case 0x01:
+		*mode_d = ATSCMH_SCCC_CODE_QTR;
+		break;
+	default:
+		*mode_d = ATSCMH_SCCC_CODE_RES;
+		break;
+	}
+fail:
+	return ret;
+}
+
+/* ------------------------------------------------------------------------ */
+
+#if 0
+static int lg216x_read_fic_err_count(struct lg216x_state *state, u8 *err)
+{
+	u8 fic_err;
+	int ret;
+
+	*err = 0;
+
+	switch (state->cfg->lg_chip) {
+	case LG2160:
+		ret = lg216x_read_reg(state, 0x0012, &fic_err);
+		break;
+	case LG2161:
+		ret = lg216x_read_reg(state, 0x001e, &fic_err);
+		break;
+	}
+	if (lg_fail(ret))
+		goto fail;
+
+	*err = fic_err;
+fail:
+	return ret;
+}
+
+static int lg2160_read_crc_err_count(struct lg216x_state *state, u16 *err)
+{
+	u8 crc_err1, crc_err2;
+	int ret;
+
+	*err = 0;
+
+	ret = lg216x_read_reg(state, 0x0411, &crc_err1);
+	if (lg_fail(ret))
+		goto fail;
+
+	ret = lg216x_read_reg(state, 0x0412, &crc_err2);
+	if (lg_fail(ret))
+		goto fail;
+
+	*err = (u16)(((crc_err2 & 0x0f) << 8) | crc_err1);
+fail:
+	return ret;
+}
+
+static int lg2161_read_crc_err_count(struct lg216x_state *state, u16 *err)
+{
+	u8 crc_err;
+	int ret;
+
+	*err = 0;
+
+	ret = lg216x_read_reg(state, 0x0612, &crc_err);
+	if (lg_fail(ret))
+		goto fail;
+
+	*err = (u16)crc_err;
+fail:
+	return ret;
+}
+
+static int lg216x_read_crc_err_count(struct lg216x_state *state, u16 *err)
+{
+	int ret;
+	switch (state->cfg->lg_chip) {
+	case LG2160:
+		ret = lg2160_read_crc_err_count(state, err);
+		break;
+	case LG2161:
+		ret = lg2161_read_crc_err_count(state, err);
+		break;
+	default:
+		ret = -EINVAL;
+		break;
+	}
+	return ret;
+}
+
+static int lg2160_read_rs_err_count(struct lg216x_state *state, u16 *err)
+{
+	u8 rs_err1, rs_err2;
+	int ret;
+
+	*err = 0;
+
+	ret = lg216x_read_reg(state, 0x0413, &rs_err1);
+	if (lg_fail(ret))
+		goto fail;
+
+	ret = lg216x_read_reg(state, 0x0414, &rs_err2);
+	if (lg_fail(ret))
+		goto fail;
+
+	*err = (u16)(((rs_err2 & 0x0f) << 8) | rs_err1);
+fail:
+	return ret;
+}
+
+static int lg2161_read_rs_err_count(struct lg216x_state *state, u16 *err)
+{
+	u8 rs_err1, rs_err2;
+	int ret;
+
+	*err = 0;
+
+	ret = lg216x_read_reg(state, 0x0613, &rs_err1);
+	if (lg_fail(ret))
+		goto fail;
+
+	ret = lg216x_read_reg(state, 0x0614, &rs_err2);
+	if (lg_fail(ret))
+		goto fail;
+
+	*err = (u16)((rs_err1 << 8) | rs_err2);
+fail:
+	return ret;
+}
+
+static int lg216x_read_rs_err_count(struct lg216x_state *state, u16 *err)
+{
+	int ret;
+	switch (state->cfg->lg_chip) {
+	case LG2160:
+		ret = lg2160_read_rs_err_count(state, err);
+		break;
+	case LG2161:
+		ret = lg2161_read_rs_err_count(state, err);
+		break;
+	default:
+		ret = -EINVAL;
+		break;
+	}
+	return ret;
+}
+#endif
+
+/* ------------------------------------------------------------------------ */
+
+static int lg216x_get_frontend(struct dvb_frontend *fe)
+{
+	struct lg216x_state *state = fe->demodulator_priv;
+	int ret;
+
+	lg_dbg("\n");
+
+	fe->dtv_property_cache.modulation = VSB_8;
+	fe->dtv_property_cache.frequency = state->current_frequency;
+	fe->dtv_property_cache.delivery_system = SYS_ATSCMH;
+
+	ret = lg216x_get_fic_version(state,
+				     &fe->dtv_property_cache.atscmh_fic_ver);
+	if (lg_fail(ret))
+		goto fail;
+	if (state->fic_ver != fe->dtv_property_cache.atscmh_fic_ver) {
+		state->fic_ver = fe->dtv_property_cache.atscmh_fic_ver;
+
+#if 0
+		ret = lg2160_get_parade_id(state,
+				&fe->dtv_property_cache.atscmh_parade_id);
+		if (lg_fail(ret))
+			goto fail;
+/* #else */
+		fe->dtv_property_cache.atscmh_parade_id = state->parade_id;
+#endif
+		ret = lg216x_get_nog(state,
+				     &fe->dtv_property_cache.atscmh_nog);
+		if (lg_fail(ret))
+			goto fail;
+		ret = lg216x_get_tnog(state,
+				      &fe->dtv_property_cache.atscmh_tnog);
+		if (lg_fail(ret))
+			goto fail;
+		ret = lg216x_get_sgn(state,
+				     &fe->dtv_property_cache.atscmh_sgn);
+		if (lg_fail(ret))
+			goto fail;
+		ret = lg216x_get_prc(state,
+				     &fe->dtv_property_cache.atscmh_prc);
+		if (lg_fail(ret))
+			goto fail;
+
+		ret = lg216x_get_rs_frame_mode(state,
+			(enum atscmh_rs_frame_mode *)
+			&fe->dtv_property_cache.atscmh_rs_frame_mode);
+		if (lg_fail(ret))
+			goto fail;
+		ret = lg216x_get_rs_frame_ensemble(state,
+			(enum atscmh_rs_frame_ensemble *)
+			&fe->dtv_property_cache.atscmh_rs_frame_ensemble);
+		if (lg_fail(ret))
+			goto fail;
+		ret = lg216x_get_rs_code_mode(state,
+			(enum atscmh_rs_code_mode *)
+			&fe->dtv_property_cache.atscmh_rs_code_mode_pri,
+			(enum atscmh_rs_code_mode *)
+			&fe->dtv_property_cache.atscmh_rs_code_mode_sec);
+		if (lg_fail(ret))
+			goto fail;
+		ret = lg216x_get_sccc_block_mode(state,
+			(enum atscmh_sccc_block_mode *)
+			&fe->dtv_property_cache.atscmh_sccc_block_mode);
+		if (lg_fail(ret))
+			goto fail;
+		ret = lg216x_get_sccc_code_mode(state,
+			(enum atscmh_sccc_code_mode *)
+			&fe->dtv_property_cache.atscmh_sccc_code_mode_a,
+			(enum atscmh_sccc_code_mode *)
+			&fe->dtv_property_cache.atscmh_sccc_code_mode_b,
+			(enum atscmh_sccc_code_mode *)
+			&fe->dtv_property_cache.atscmh_sccc_code_mode_c,
+			(enum atscmh_sccc_code_mode *)
+			&fe->dtv_property_cache.atscmh_sccc_code_mode_d);
+		if (lg_fail(ret))
+			goto fail;
+	}
+#if 0
+	ret = lg216x_read_fic_err_count(state,
+				(u8 *)&fe->dtv_property_cache.atscmh_fic_err);
+	if (lg_fail(ret))
+		goto fail;
+	ret = lg216x_read_crc_err_count(state,
+				&fe->dtv_property_cache.atscmh_crc_err);
+	if (lg_fail(ret))
+		goto fail;
+	ret = lg216x_read_rs_err_count(state,
+				&fe->dtv_property_cache.atscmh_rs_err);
+	if (lg_fail(ret))
+		goto fail;
+
+	switch (state->cfg->lg_chip) {
+	case LG2160:
+		if (((fe->dtv_property_cache.atscmh_rs_err >= 240) &&
+		     (fe->dtv_property_cache.atscmh_crc_err >= 240)) &&
+		    ((jiffies_to_msecs(jiffies) - state->last_reset) > 6000))
+			ret = lg216x_soft_reset(state);
+		break;
+	case LG2161:
+		/* no fix needed here (as far as we know) */
+		ret = 0;
+		break;
+	}
+	lg_fail(ret);
+#endif
+fail:
+	return ret;
+}
+
+static int lg216x_get_property(struct dvb_frontend *fe,
+			       struct dtv_property *tvp)
+{
+	return (DTV_ATSCMH_FIC_VER == tvp->cmd) ?
+		lg216x_get_frontend(fe) : 0;
+}
+
+
+static int lg2160_set_frontend(struct dvb_frontend *fe)
+{
+	struct lg216x_state *state = fe->demodulator_priv;
+	int ret;
+
+	lg_dbg("(%d)\n", fe->dtv_property_cache.frequency);
+
+	if (fe->ops.tuner_ops.set_params) {
+		ret = fe->ops.tuner_ops.set_params(fe);
+		if (fe->ops.i2c_gate_ctrl)
+			fe->ops.i2c_gate_ctrl(fe, 0);
+		if (lg_fail(ret))
+			goto fail;
+		state->current_frequency = fe->dtv_property_cache.frequency;
+	}
+
+	ret = lg2160_agc_fix(state, 0, 0);
+	if (lg_fail(ret))
+		goto fail;
+	ret = lg2160_agc_polarity(state, 0, 0);
+	if (lg_fail(ret))
+		goto fail;
+	ret = lg2160_tuner_pwr_save_polarity(state, 1);
+	if (lg_fail(ret))
+		goto fail;
+	ret = lg216x_set_if(state);
+	if (lg_fail(ret))
+		goto fail;
+	ret = lg2160_spectrum_polarity(state, state->cfg->spectral_inversion);
+	if (lg_fail(ret))
+		goto fail;
+
+	/* be tuned before this point */
+	ret = lg216x_soft_reset(state);
+	if (lg_fail(ret))
+		goto fail;
+
+	ret = lg2160_tuner_pwr_save(state, 0);
+	if (lg_fail(ret))
+		goto fail;
+
+	switch (state->cfg->lg_chip) {
+	case LG2160:
+		ret = lg2160_set_spi_clock(state);
+		if (lg_fail(ret))
+			goto fail;
+		break;
+	case LG2161:
+		ret = lg2161_set_output_interface(state);
+		if (lg_fail(ret))
+			goto fail;
+		break;
+	}
+
+	ret = lg216x_set_parade(state, fe->dtv_property_cache.atscmh_parade_id);
+	if (lg_fail(ret))
+		goto fail;
+
+	ret = lg216x_set_ensemble(state,
+			fe->dtv_property_cache.atscmh_rs_frame_ensemble);
+	if (lg_fail(ret))
+		goto fail;
+
+	ret = lg216x_initialize(state);
+	if (lg_fail(ret))
+		goto fail;
+
+	ret = lg216x_enable_fic(state, 1);
+	lg_fail(ret);
+
+	lg216x_get_frontend(fe);
+fail:
+	return ret;
+}
+
+/* ------------------------------------------------------------------------ */
+
+static int lg2160_read_lock_status(struct lg216x_state *state,
+				   int *acq_lock, int *sync_lock)
+{
+	u8 val;
+	int ret;
+
+	*acq_lock = 0;
+	*sync_lock = 0;
+
+	ret = lg216x_read_reg(state, 0x011b, &val);
+	if (lg_fail(ret))
+		goto fail;
+
+	*sync_lock = (val & 0x20) ? 0 : 1;
+	*acq_lock  = (val & 0x40) ? 0 : 1;
+fail:
+	return ret;
+}
+
+#ifdef USE_LG2161_LOCK_BITS
+static int lg2161_read_lock_status(struct lg216x_state *state,
+				   int *acq_lock, int *sync_lock)
+{
+	u8 val;
+	int ret;
+
+	*acq_lock = 0;
+	*sync_lock = 0;
+
+	ret = lg216x_read_reg(state, 0x0304, &val);
+	if (lg_fail(ret))
+		goto fail;
+
+	*sync_lock = (val & 0x80) ? 0 : 1;
+
+	ret = lg216x_read_reg(state, 0x011b, &val);
+	if (lg_fail(ret))
+		goto fail;
+
+	*acq_lock  = (val & 0x40) ? 0 : 1;
+fail:
+	return ret;
+}
+#endif
+
+static int lg216x_read_lock_status(struct lg216x_state *state,
+				   int *acq_lock, int *sync_lock)
+{
+#ifdef USE_LG2161_LOCK_BITS
+	int ret;
+	switch (state->cfg->lg_chip) {
+	case LG2160:
+		ret = lg2160_read_lock_status(state, acq_lock, sync_lock);
+		break;
+	case LG2161:
+		ret = lg2161_read_lock_status(state, acq_lock, sync_lock);
+		break;
+	default:
+		ret = -EINVAL;
+		break;
+	}
+	return ret;
+#else
+	return lg2160_read_lock_status(state, acq_lock, sync_lock);
+#endif
+}
+
+static int lg216x_read_status(struct dvb_frontend *fe, fe_status_t *status)
+{
+	struct lg216x_state *state = fe->demodulator_priv;
+	int ret, acq_lock, sync_lock;
+
+	*status = 0;
+
+	ret = lg216x_read_lock_status(state, &acq_lock, &sync_lock);
+	if (lg_fail(ret))
+		goto fail;
+
+	lg_dbg("%s%s\n",
+	       acq_lock  ? "SIGNALEXIST " : "",
+	       sync_lock ? "SYNCLOCK"     : "");
+
+	if (acq_lock)
+		*status |= FE_HAS_SIGNAL;
+	if (sync_lock)
+		*status |= FE_HAS_SYNC;
+
+	if (*status)
+		*status |= FE_HAS_CARRIER | FE_HAS_VITERBI | FE_HAS_LOCK;
+
+fail:
+	return ret;
+}
+
+/* ------------------------------------------------------------------------ */
+
+static int lg2160_read_snr(struct dvb_frontend *fe, u16 *snr)
+{
+	struct lg216x_state *state = fe->demodulator_priv;
+	u8 snr1, snr2;
+	int ret;
+
+	*snr = 0;
+
+	ret = lg216x_read_reg(state, 0x0202, &snr1);
+	if (lg_fail(ret))
+		goto fail;
+
+	ret = lg216x_read_reg(state, 0x0203, &snr2);
+	if (lg_fail(ret))
+		goto fail;
+
+	if ((snr1 == 0xba) || (snr2 == 0xdf))
+		*snr = 0;
+	else
+#if 1
+	*snr =  ((snr1 >> 4) * 100) + ((snr1 & 0x0f) * 10) + (snr2 >> 4);
+#else /* BCD */
+	*snr =  (snr2 | (snr1 << 8));
+#endif
+fail:
+	return ret;
+}
+
+static int lg2161_read_snr(struct dvb_frontend *fe, u16 *snr)
+{
+	struct lg216x_state *state = fe->demodulator_priv;
+	u8 snr1, snr2;
+	int ret;
+
+	*snr = 0;
+
+	ret = lg216x_read_reg(state, 0x0302, &snr1);
+	if (lg_fail(ret))
+		goto fail;
+
+	ret = lg216x_read_reg(state, 0x0303, &snr2);
+	if (lg_fail(ret))
+		goto fail;
+
+	if ((snr1 == 0xba) || (snr2 == 0xfd))
+		*snr = 0;
+	else
+
+	*snr =  ((snr1 >> 4) * 100) + ((snr1 & 0x0f) * 10) + (snr2 & 0x0f);
+fail:
+	return ret;
+}
+
+static int lg216x_read_signal_strength(struct dvb_frontend *fe,
+				       u16 *strength)
+{
+#if 0
+	/* borrowed from lgdt330x.c
+	 *
+	 * Calculate strength from SNR up to 35dB
+	 * Even though the SNR can go higher than 35dB,
+	 * there is some comfort factor in having a range of
+	 * strong signals that can show at 100%
+	 */
+	struct lg216x_state *state = fe->demodulator_priv;
+	u16 snr;
+	int ret;
+#endif
+	*strength = 0;
+#if 0
+	ret = fe->ops.read_snr(fe, &snr);
+	if (lg_fail(ret))
+		goto fail;
+	/* Rather than use the 8.8 value snr, use state->snr which is 8.24 */
+	/* scale the range 0 - 35*2^24 into 0 - 65535 */
+	if (state->snr >= 8960 * 0x10000)
+		*strength = 0xffff;
+	else
+		*strength = state->snr / 8960;
+fail:
+	return ret;
+#else
+	return 0;
+#endif
+}
+
+/* ------------------------------------------------------------------------ */
+
+static int lg216x_read_ucblocks(struct dvb_frontend *fe, u32 *ucblocks)
+{
+#if 0
+	struct lg216x_state *state = fe->demodulator_priv;
+	int ret;
+
+	ret = lg216x_read_rs_err_count(state,
+				       &fe->dtv_property_cache.atscmh_rs_err);
+	if (lg_fail(ret))
+		goto fail;
+
+	*ucblocks = fe->dtv_property_cache.atscmh_rs_err;
+fail:
+#else
+	*ucblocks = 0;
+#endif
+	return 0;
+}
+
+static int lg216x_get_tune_settings(struct dvb_frontend *fe,
+				    struct dvb_frontend_tune_settings
+				    *fe_tune_settings)
+{
+	fe_tune_settings->min_delay_ms = 500;
+	lg_dbg("\n");
+	return 0;
+}
+
+static void lg216x_release(struct dvb_frontend *fe)
+{
+	struct lg216x_state *state = fe->demodulator_priv;
+	lg_dbg("\n");
+	kfree(state);
+}
+
+static struct dvb_frontend_ops lg2160_ops = {
+	.delsys = { SYS_ATSCMH },
+	.info = {
+		.name = "LG Electronics LG2160 ATSC/MH Frontend",
+		.frequency_min      = 54000000,
+		.frequency_max      = 858000000,
+		.frequency_stepsize = 62500,
+	},
+	.i2c_gate_ctrl        = lg216x_i2c_gate_ctrl,
+#if 0
+	.init                 = lg216x_init,
+	.sleep                = lg216x_sleep,
+#endif
+	.get_property         = lg216x_get_property,
+
+	.set_frontend         = lg2160_set_frontend,
+	.get_frontend         = lg216x_get_frontend,
+	.get_tune_settings    = lg216x_get_tune_settings,
+	.read_status          = lg216x_read_status,
+#if 0
+	.read_ber             = lg216x_read_ber,
+#endif
+	.read_signal_strength = lg216x_read_signal_strength,
+	.read_snr             = lg2160_read_snr,
+	.read_ucblocks        = lg216x_read_ucblocks,
+	.release              = lg216x_release,
+};
+
+static struct dvb_frontend_ops lg2161_ops = {
+	.delsys = { SYS_ATSCMH },
+	.info = {
+		.name = "LG Electronics LG2161 ATSC/MH Frontend",
+		.frequency_min      = 54000000,
+		.frequency_max      = 858000000,
+		.frequency_stepsize = 62500,
+	},
+	.i2c_gate_ctrl        = lg216x_i2c_gate_ctrl,
+#if 0
+	.init                 = lg216x_init,
+	.sleep                = lg216x_sleep,
+#endif
+	.get_property         = lg216x_get_property,
+
+	.set_frontend         = lg2160_set_frontend,
+	.get_frontend         = lg216x_get_frontend,
+	.get_tune_settings    = lg216x_get_tune_settings,
+	.read_status          = lg216x_read_status,
+#if 0
+	.read_ber             = lg216x_read_ber,
+#endif
+	.read_signal_strength = lg216x_read_signal_strength,
+	.read_snr             = lg2161_read_snr,
+	.read_ucblocks        = lg216x_read_ucblocks,
+	.release              = lg216x_release,
+};
+
+struct dvb_frontend *lg2160_attach(const struct lg2160_config *config,
+				   struct i2c_adapter *i2c_adap)
+{
+	struct lg216x_state *state = NULL;
+
+	lg_dbg("(%d-%04x)\n",
+	       i2c_adap ? i2c_adapter_id(i2c_adap) : 0,
+	       config ? config->i2c_addr : 0);
+
+	state = kzalloc(sizeof(struct lg216x_state), GFP_KERNEL);
+	if (state == NULL)
+		goto fail;
+
+	state->cfg = config;
+	state->i2c_adap = i2c_adap;
+	state->fic_ver = 0xff;
+	state->parade_id = 0xff;
+
+	switch (config->lg_chip) {
+	default:
+		lg_warn("invalid chip requested, defaulting to LG2160");
+		/* fall-thru */
+	case LG2160:
+		memcpy(&state->frontend.ops, &lg2160_ops,
+		       sizeof(struct dvb_frontend_ops));
+		break;
+	case LG2161:
+		memcpy(&state->frontend.ops, &lg2161_ops,
+		       sizeof(struct dvb_frontend_ops));
+		break;
+	}
+
+	state->frontend.demodulator_priv = state;
+	state->current_frequency = -1;
+	/* parade 1 by default */
+	state->frontend.dtv_property_cache.atscmh_parade_id = 1;
+
+	return &state->frontend;
+fail:
+	lg_warn("unable to detect LG216x hardware\n");
+	kfree(state);
+	return NULL;
+}
+EXPORT_SYMBOL(lg2160_attach);
+
+MODULE_DESCRIPTION("LG Electronics LG216x ATSC/MH Demodulator Driver");
+MODULE_AUTHOR("Michael Krufky <mkrufky@linuxtv.org>");
+MODULE_LICENSE("GPL");
+MODULE_VERSION("0.3");
+
+/*
+ * Local variables:
+ * c-basic-offset: 8
+ * End:
+ */
diff --git a/drivers/media/dvb/frontends/lg2160.h b/drivers/media/dvb/frontends/lg2160.h
new file mode 100644
index 00000000000..9e2c0f41199
--- /dev/null
+++ b/drivers/media/dvb/frontends/lg2160.h
@@ -0,0 +1,84 @@
+/*
+ *    Support for LG2160 - ATSC/MH
+ *
+ *    Copyright (C) 2010 Michael Krufky <mkrufky@linuxtv.org>
+ *
+ *    This program is free software; you can redistribute it and/or modify
+ *    it under the terms of the GNU General Public License as published by
+ *    the Free Software Foundation; either version 2 of the License, or
+ *    (at your option) any later version.
+ *
+ *    This program is distributed in the hope that it will be useful,
+ *    but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ *    GNU General Public License for more details.
+ *
+ *    You should have received a copy of the GNU General Public License
+ *    along with this program; if not, write to the Free Software
+ *    Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+ *
+ */
+
+#ifndef _LG2160_H_
+#define _LG2160_H_
+
+#include <linux/i2c.h>
+#include "dvb_frontend.h"
+
+enum lg_chip_type {
+	LG2160 = 0,
+	LG2161 = 1,
+};
+
+#define LG2161_1019 LG2161
+#define LG2161_1040 LG2161
+
+enum lg2160_spi_clock {
+	LG2160_SPI_3_125_MHZ = 0,
+	LG2160_SPI_6_25_MHZ = 1,
+	LG2160_SPI_12_5_MHZ = 2,
+};
+
+#if 0
+enum lg2161_oif {
+	LG2161_OIF_EBI2_SLA  = 1,
+	LG2161_OIF_SDIO_SLA  = 2,
+	LG2161_OIF_SPI_SLA   = 3,
+	LG2161_OIF_SPI_MAS   = 4,
+	LG2161_OIF_SERIAL_TS = 7,
+};
+#endif
+
+struct lg2160_config {
+	u8 i2c_addr;
+
+	/* user defined IF frequency in KHz */
+	u16 if_khz;
+
+	/* disable i2c repeater - 0:repeater enabled 1:repeater disabled */
+	int deny_i2c_rptr:1;
+
+	/* spectral inversion - 0:disabled 1:enabled */
+	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))
+extern
+struct dvb_frontend *lg2160_attach(const struct lg2160_config *config,
+				     struct i2c_adapter *i2c_adap);
+#else
+static inline
+struct dvb_frontend *lg2160_attach(const struct lg2160_config *config,
+				     struct i2c_adapter *i2c_adap)
+{
+	printk(KERN_WARNING "%s: driver disabled by Kconfig\n", __func__);
+	return NULL;
+}
+#endif /* CONFIG_DVB_LG2160 */
+
+#endif /* _LG2160_H_ */
diff --git a/drivers/media/dvb/frontends/lgs8gxx.c b/drivers/media/dvb/frontends/lgs8gxx.c
index 4de1d3520cd..568363a10a3 100644
--- a/drivers/media/dvb/frontends/lgs8gxx.c
+++ b/drivers/media/dvb/frontends/lgs8gxx.c
@@ -262,7 +262,6 @@ static int lgs8gxx_set_mode_auto(struct lgs8gxx_state *priv)
 
 static int lgs8gxx_set_mode_manual(struct lgs8gxx_state *priv)
 {
-	int ret = 0;
 	u8 t;
 
 	if (priv->config->prod == LGS8GXX_PROD_LGS8G75) {
@@ -296,7 +295,7 @@ static int lgs8gxx_set_mode_manual(struct lgs8gxx_state *priv)
 	if (priv->config->prod == LGS8GXX_PROD_LGS8913)
 		lgs8gxx_write_reg(priv, 0xC1, 0);
 
-	ret = lgs8gxx_read_reg(priv, 0xC5, &t);
+	lgs8gxx_read_reg(priv, 0xC5, &t);
 	t = (t & 0xE0) | 0x06;
 	lgs8gxx_write_reg(priv, 0xC5, t);
 
diff --git a/drivers/media/dvb/frontends/m88rs2000.c b/drivers/media/dvb/frontends/m88rs2000.c
index 045ee5a6f7a..312588e84da 100644
--- a/drivers/media/dvb/frontends/m88rs2000.c
+++ b/drivers/media/dvb/frontends/m88rs2000.c
@@ -416,9 +416,25 @@ static int m88rs2000_tab_set(struct m88rs2000_state *state,
 
 static int m88rs2000_set_voltage(struct dvb_frontend *fe, fe_sec_voltage_t volt)
 {
-	deb_info("%s: %s\n", __func__,
-		volt == SEC_VOLTAGE_13 ? "SEC_VOLTAGE_13" :
-		volt == SEC_VOLTAGE_18 ? "SEC_VOLTAGE_18" : "??");
+	struct m88rs2000_state *state = fe->demodulator_priv;
+	u8 data;
+
+	data = m88rs2000_demod_read(state, 0xb2);
+	data |= 0x03; /* bit0 V/H, bit1 off/on */
+
+	switch (volt) {
+	case SEC_VOLTAGE_18:
+		data &= ~0x03;
+		break;
+	case SEC_VOLTAGE_13:
+		data &= ~0x03;
+		data |= 0x01;
+		break;
+	case SEC_VOLTAGE_OFF:
+		break;
+	}
+
+	m88rs2000_demod_write(state, 0xb2, data);
 
 	return 0;
 }
@@ -654,7 +670,6 @@ static int m88rs2000_set_tuner(struct dvb_frontend *fe, u16 *offset)
 static int m88rs2000_set_fec(struct m88rs2000_state *state,
 		fe_code_rate_t fec)
 {
-	int ret;
 	u16 fec_set;
 	switch (fec) {
 	/* This is not confirmed kept for reference */
@@ -677,7 +692,7 @@ static int m88rs2000_set_fec(struct m88rs2000_state *state,
 	default:
 		fec_set = 0x08;
 	}
-	ret = m88rs2000_demod_write(state, 0x76, fec_set);
+	m88rs2000_demod_write(state, 0x76, fec_set);
 
 	return 0;
 }
@@ -772,13 +787,13 @@ static int m88rs2000_set_frontend(struct dvb_frontend *fe)
 		return -ENODEV;
 
 	for (i = 0; i < 25; i++) {
-		u8 reg = m88rs2000_demod_read(state, 0x8c);
+		reg = m88rs2000_demod_read(state, 0x8c);
 		if ((reg & 0x7) == 0x7) {
 			status = FE_HAS_LOCK;
 			break;
 		}
 		state->no_lock_count++;
-		if (state->no_lock_count > 15) {
+		if (state->no_lock_count == 15) {
 			reg = m88rs2000_demod_read(state, 0x70);
 			reg ^= 0x4;
 			m88rs2000_demod_write(state, 0x70, reg);
diff --git a/drivers/media/dvb/frontends/rtl2830.c b/drivers/media/dvb/frontends/rtl2830.c
index 45196c5b073..93612ebac51 100644
--- a/drivers/media/dvb/frontends/rtl2830.c
+++ b/drivers/media/dvb/frontends/rtl2830.c
@@ -374,6 +374,118 @@ err:
 	return ret;
 }
 
+static int rtl2830_get_frontend(struct dvb_frontend *fe)
+{
+	struct rtl2830_priv *priv = fe->demodulator_priv;
+	struct dtv_frontend_properties *c = &fe->dtv_property_cache;
+	int ret;
+	u8 buf[3];
+
+	if (priv->sleeping)
+		return 0;
+
+	ret = rtl2830_rd_regs(priv, 0x33c, buf, 2);
+	if (ret)
+		goto err;
+
+	ret = rtl2830_rd_reg(priv, 0x351, &buf[2]);
+	if (ret)
+		goto err;
+
+	dbg("%s: TPS=%02x %02x %02x", __func__, buf[0], buf[1], buf[2]);
+
+	switch ((buf[0] >> 2) & 3) {
+	case 0:
+		c->modulation = QPSK;
+		break;
+	case 1:
+		c->modulation = QAM_16;
+		break;
+	case 2:
+		c->modulation = QAM_64;
+		break;
+	}
+
+	switch ((buf[2] >> 2) & 1) {
+	case 0:
+		c->transmission_mode = TRANSMISSION_MODE_2K;
+		break;
+	case 1:
+		c->transmission_mode = TRANSMISSION_MODE_8K;
+	}
+
+	switch ((buf[2] >> 0) & 3) {
+	case 0:
+		c->guard_interval = GUARD_INTERVAL_1_32;
+		break;
+	case 1:
+		c->guard_interval = GUARD_INTERVAL_1_16;
+		break;
+	case 2:
+		c->guard_interval = GUARD_INTERVAL_1_8;
+		break;
+	case 3:
+		c->guard_interval = GUARD_INTERVAL_1_4;
+		break;
+	}
+
+	switch ((buf[0] >> 4) & 7) {
+	case 0:
+		c->hierarchy = HIERARCHY_NONE;
+		break;
+	case 1:
+		c->hierarchy = HIERARCHY_1;
+		break;
+	case 2:
+		c->hierarchy = HIERARCHY_2;
+		break;
+	case 3:
+		c->hierarchy = HIERARCHY_4;
+		break;
+	}
+
+	switch ((buf[1] >> 3) & 7) {
+	case 0:
+		c->code_rate_HP = FEC_1_2;
+		break;
+	case 1:
+		c->code_rate_HP = FEC_2_3;
+		break;
+	case 2:
+		c->code_rate_HP = FEC_3_4;
+		break;
+	case 3:
+		c->code_rate_HP = FEC_5_6;
+		break;
+	case 4:
+		c->code_rate_HP = FEC_7_8;
+		break;
+	}
+
+	switch ((buf[1] >> 0) & 7) {
+	case 0:
+		c->code_rate_LP = FEC_1_2;
+		break;
+	case 1:
+		c->code_rate_LP = FEC_2_3;
+		break;
+	case 2:
+		c->code_rate_LP = FEC_3_4;
+		break;
+	case 3:
+		c->code_rate_LP = FEC_5_6;
+		break;
+	case 4:
+		c->code_rate_LP = FEC_7_8;
+		break;
+	}
+
+	return 0;
+err:
+	dbg("%s: failed=%d", __func__, ret);
+	return ret;
+}
+
 static int rtl2830_read_status(struct dvb_frontend *fe, fe_status_t *status)
 {
 	struct rtl2830_priv *priv = fe->demodulator_priv;
@@ -404,14 +516,72 @@ err:
 
 static int rtl2830_read_snr(struct dvb_frontend *fe, u16 *snr)
 {
-	*snr = 0;
+	struct rtl2830_priv *priv = fe->demodulator_priv;
+	int ret, hierarchy, constellation;
+	u8 buf[2], tmp;
+	u16 tmp16;
+#define CONSTELLATION_NUM 3
+#define HIERARCHY_NUM 4
+	static const u32 snr_constant[CONSTELLATION_NUM][HIERARCHY_NUM] = {
+		{ 70705899, 70705899, 70705899, 70705899 },
+		{ 82433173, 82433173, 87483115, 94445660 },
+		{ 92888734, 92888734, 95487525, 99770748 },
+	};
+
+	if (priv->sleeping)
+		return 0;
+
+	/* reports SNR in resolution of 0.1 dB */
+
+	ret = rtl2830_rd_reg(priv, 0x33c, &tmp);
+	if (ret)
+		goto err;
+
+	constellation = (tmp >> 2) & 0x03; /* [3:2] */
+	if (constellation > CONSTELLATION_NUM - 1)
+		goto err;
+
+	hierarchy = (tmp >> 4) & 0x07; /* [6:4] */
+	if (hierarchy > HIERARCHY_NUM - 1)
+		goto err;
+
+	ret = rtl2830_rd_regs(priv, 0x40c, buf, 2);
+	if (ret)
+		goto err;
+
+	tmp16 = buf[0] << 8 | buf[1];
+
+	if (tmp16)
+		*snr = (snr_constant[constellation][hierarchy] -
+				intlog10(tmp16)) / ((1 << 24) / 100);
+	else
+		*snr = 0;
+
 	return 0;
+err:
+	dbg("%s: failed=%d", __func__, ret);
+	return ret;
 }
 
 static int rtl2830_read_ber(struct dvb_frontend *fe, u32 *ber)
 {
-	*ber = 0;
+	struct rtl2830_priv *priv = fe->demodulator_priv;
+	int ret;
+	u8 buf[2];
+
+	if (priv->sleeping)
+		return 0;
+
+	ret = rtl2830_rd_regs(priv, 0x34e, buf, 2);
+	if (ret)
+		goto err;
+
+	*ber = buf[0] << 8 | buf[1];
+
 	return 0;
+err:
+	dbg("%s: failed=%d", __func__, ret);
+	return ret;
 }
 
 static int rtl2830_read_ucblocks(struct dvb_frontend *fe, u32 *ucblocks)
@@ -422,8 +592,32 @@ static int rtl2830_read_ucblocks(struct dvb_frontend *fe, u32 *ucblocks)
 
 static int rtl2830_read_signal_strength(struct dvb_frontend *fe, u16 *strength)
 {
-	*strength = 0;
+	struct rtl2830_priv *priv = fe->demodulator_priv;
+	int ret;
+	u8 buf[2];
+	u16 if_agc_raw, if_agc;
+
+	if (priv->sleeping)
+		return 0;
+
+	ret = rtl2830_rd_regs(priv, 0x359, buf, 2);
+	if (ret)
+		goto err;
+
+	if_agc_raw = (buf[0] << 8 | buf[1]) & 0x3fff;
+
+	if (if_agc_raw & (1 << 9))
+		if_agc = -(~(if_agc_raw - 1) & 0x1ff);
+	else
+		if_agc = if_agc_raw;
+
+	*strength = (u8) (55 - if_agc / 182);
+	*strength |= *strength << 8;
+
 	return 0;
+err:
+	dbg("%s: failed=%d", __func__, ret);
+	return ret;
 }
 
 static struct dvb_frontend_ops rtl2830_ops;
@@ -549,6 +743,7 @@ static struct dvb_frontend_ops rtl2830_ops = {
 	.get_tune_settings = rtl2830_get_tune_settings,
 
 	.set_frontend = rtl2830_set_frontend,
+	.get_frontend = rtl2830_get_frontend,
 
 	.read_status = rtl2830_read_status,
 	.read_snr = rtl2830_read_snr,
diff --git a/drivers/media/dvb/frontends/rtl2830_priv.h b/drivers/media/dvb/frontends/rtl2830_priv.h
index 4a464761b5b..9b20557ccf6 100644
--- a/drivers/media/dvb/frontends/rtl2830_priv.h
+++ b/drivers/media/dvb/frontends/rtl2830_priv.h
@@ -22,6 +22,7 @@
 #define RTL2830_PRIV_H
 
 #include "dvb_frontend.h"
+#include "dvb_math.h"
 #include "rtl2830.h"
 
 #define LOG_PREFIX "rtl2830"
diff --git a/drivers/media/dvb/frontends/stb0899_drv.c b/drivers/media/dvb/frontends/stb0899_drv.c
index dd08f4ac64a..8b0dc74a329 100644
--- a/drivers/media/dvb/frontends/stb0899_drv.c
+++ b/drivers/media/dvb/frontends/stb0899_drv.c
@@ -637,11 +637,9 @@ static void stb0899_init_calc(struct stb0899_state *state)
 	struct stb0899_internal *internal = &state->internal;
 	int master_clk;
 	u8 agc[2];
-	u8 agc1cn;
 	u32 reg;
 
 	/* Read registers (in burst mode)	*/
-	agc1cn = stb0899_read_reg(state, STB0899_AGC1CN);
 	stb0899_read_regs(state, STB0899_AGC1REF, agc, 2); /* AGC1R and AGC2O	*/
 
 	/* Initial calculations	*/
@@ -823,15 +821,12 @@ static int stb0899_send_diseqc_burst(struct dvb_frontend *fe, fe_sec_mini_cmd_t
 
 static int stb0899_diseqc_init(struct stb0899_state *state)
 {
-	struct dvb_diseqc_master_cmd tx_data;
 /*
 	struct dvb_diseqc_slave_reply rx_data;
 */
-	u8 f22_tx, f22_rx, reg;
+	u8 f22_tx, reg;
 
 	u32 mclk, tx_freq = 22000;/* count = 0, i; */
-	tx_data.msg[0] = 0xe2;
-	tx_data.msg_len = 3;
 	reg = stb0899_read_reg(state, STB0899_DISCNTRL2);
 	STB0899_SETFIELD_VAL(ONECHIP_TRX, reg, 0);
 	stb0899_write_reg(state, STB0899_DISCNTRL2, reg);
@@ -849,7 +844,6 @@ static int stb0899_diseqc_init(struct stb0899_state *state)
 	f22_tx = mclk / (tx_freq * 32);
 	stb0899_write_reg(state, STB0899_DISF22, f22_tx); /* DiSEqC Tx freq	*/
 	state->rx_freq = 20000;
-	f22_rx = mclk / (state->rx_freq * 32);
 
 	return 0;
 }
diff --git a/drivers/media/dvb/frontends/stb6100.c b/drivers/media/dvb/frontends/stb6100.c
index def88abb30b..2e93e65d2cd 100644
--- a/drivers/media/dvb/frontends/stb6100.c
+++ b/drivers/media/dvb/frontends/stb6100.c
@@ -158,7 +158,6 @@ static int stb6100_read_regs(struct stb6100_state *state, u8 regs[])
 static int stb6100_read_reg(struct stb6100_state *state, u8 reg)
 {
 	u8 regs[STB6100_NUMREGS];
-	int rc;
 
 	struct i2c_msg msg = {
 		.addr	= state->config->tuner_address + reg,
@@ -167,7 +166,7 @@ static int stb6100_read_reg(struct stb6100_state *state, u8 reg)
 		.len	= 1
 	};
 
-	rc = i2c_transfer(state->i2c, &msg, 1);
+	i2c_transfer(state->i2c, &msg, 1);
 
 	if (unlikely(reg >= STB6100_NUMREGS)) {
 		dprintk(verbose, FE_ERROR, 1, "Invalid register offset 0x%x", reg);
diff --git a/drivers/media/dvb/frontends/stv0297.c b/drivers/media/dvb/frontends/stv0297.c
index 85c157a1fe5..d40f226160e 100644
--- a/drivers/media/dvb/frontends/stv0297.c
+++ b/drivers/media/dvb/frontends/stv0297.c
@@ -414,7 +414,6 @@ static int stv0297_set_frontend(struct dvb_frontend *fe)
 	int delay;
 	int sweeprate;
 	int carrieroffset;
-	unsigned long starttime;
 	unsigned long timeout;
 	fe_spectral_inversion_t inversion;
 
@@ -543,7 +542,6 @@ static int stv0297_set_frontend(struct dvb_frontend *fe)
 	stv0297_writereg_mask(state, 0x43, 0x10, 0x10);
 
 	/* wait for WGAGC lock */
-	starttime = jiffies;
 	timeout = jiffies + msecs_to_jiffies(2000);
 	while (time_before(jiffies, timeout)) {
 		msleep(10);
diff --git a/drivers/media/dvb/frontends/stv0900_sw.c b/drivers/media/dvb/frontends/stv0900_sw.c
index ba0709b2d43..4af20780fb9 100644
--- a/drivers/media/dvb/frontends/stv0900_sw.c
+++ b/drivers/media/dvb/frontends/stv0900_sw.c
@@ -835,7 +835,6 @@ static void stv0900_track_optimization(struct dvb_frontend *fe)
 		blind_tun_sw = 0,
 		modulation;
 
-	enum fe_stv0900_rolloff rolloff;
 	enum fe_stv0900_modcode foundModcod;
 
 	dprintk("%s\n", __func__);
@@ -940,7 +939,6 @@ static void stv0900_track_optimization(struct dvb_frontend *fe)
 
 	freq1 = stv0900_read_reg(intp, CFR2);
 	freq0 = stv0900_read_reg(intp, CFR1);
-	rolloff = stv0900_get_bits(intp, ROLLOFF_STATUS);
 	if (intp->srch_algo[demod] == STV0900_BLIND_SEARCH) {
 		stv0900_write_reg(intp, SFRSTEP, 0x00);
 		stv0900_write_bits(intp, SCAN_ENABLE, 0);
diff --git a/drivers/media/dvb/frontends/stv090x.c b/drivers/media/dvb/frontends/stv090x.c
index 4aef1877ed4..d79e69f65cb 100644
--- a/drivers/media/dvb/frontends/stv090x.c
+++ b/drivers/media/dvb/frontends/stv090x.c
@@ -2842,7 +2842,6 @@ static int stv090x_optimize_track(struct stv090x_state *state)
 {
 	struct dvb_frontend *fe = &state->frontend;
 
-	enum stv090x_rolloff rolloff;
 	enum stv090x_modcod modcod;
 
 	s32 srate, pilots, aclc, f_1, f_0, i = 0, blind_tune = 0;
@@ -2966,7 +2965,6 @@ static int stv090x_optimize_track(struct stv090x_state *state)
 	f_1 = STV090x_READ_DEMOD(state, CFR2);
 	f_0 = STV090x_READ_DEMOD(state, CFR1);
 	reg = STV090x_READ_DEMOD(state, TMGOBS);
-	rolloff = STV090x_GETFIELD_Px(reg, ROLLOFF_STATUS_FIELD);
 
 	if (state->algo == STV090x_BLIND_SEARCH) {
 		STV090x_WRITE_DEMOD(state, SFRSTEP, 0x00);
diff --git a/drivers/media/dvb/frontends/zl10353.c b/drivers/media/dvb/frontends/zl10353.c
index ac723789137..82946cd517f 100644
--- a/drivers/media/dvb/frontends/zl10353.c
+++ b/drivers/media/dvb/frontends/zl10353.c
@@ -525,7 +525,7 @@ static int zl10353_read_snr(struct dvb_frontend *fe, u16 *snr)
 		zl10353_dump_regs(fe);
 
 	_snr = zl10353_read_register(state, SNR);
-	*snr = (_snr << 8) | _snr;
+	*snr = 10 * _snr / 8;
 
 	return 0;
 }
@@ -559,7 +559,6 @@ static int zl10353_init(struct dvb_frontend *fe)
 {
 	struct zl10353_state *state = fe->demodulator_priv;
 	u8 zl10353_reset_attach[6] = { 0x50, 0x03, 0x64, 0x46, 0x15, 0x0F };
-	int rc = 0;
 
 	if (debug_regs)
 		zl10353_dump_regs(fe);
@@ -573,7 +572,7 @@ static int zl10353_init(struct dvb_frontend *fe)
 	/* Do a "hard" reset if not already done */
 	if (zl10353_read_register(state, 0x50) != zl10353_reset_attach[1] ||
 	    zl10353_read_register(state, 0x51) != zl10353_reset_attach[2]) {
-		rc = zl10353_write(fe, zl10353_reset_attach,
+		zl10353_write(fe, zl10353_reset_attach,
 				   sizeof(zl10353_reset_attach));
 		if (debug_regs)
 			zl10353_dump_regs(fe);
diff --git a/drivers/media/dvb/mantis/hopper_cards.c b/drivers/media/dvb/mantis/hopper_cards.c
index 71622f65c03..cc0251e0107 100644
--- a/drivers/media/dvb/mantis/hopper_cards.c
+++ b/drivers/media/dvb/mantis/hopper_cards.c
@@ -65,7 +65,7 @@ static int devs;
 
 static irqreturn_t hopper_irq_handler(int irq, void *dev_id)
 {
-	u32 stat = 0, mask = 0, lstat = 0;
+	u32 stat = 0, mask = 0;
 	u32 rst_stat = 0, rst_mask = 0;
 
 	struct mantis_pci *mantis;
@@ -80,7 +80,6 @@ static irqreturn_t hopper_irq_handler(int irq, void *dev_id)
 
 	stat = mmread(MANTIS_INT_STAT);
 	mask = mmread(MANTIS_INT_MASK);
-	lstat = stat & ~MANTIS_INT_RISCSTAT;
 	if (!(stat & mask))
 		return IRQ_NONE;
 
diff --git a/drivers/media/dvb/mantis/mantis_cards.c b/drivers/media/dvb/mantis/mantis_cards.c
index c2bb90b3e52..095cf3a994e 100644
--- a/drivers/media/dvb/mantis/mantis_cards.c
+++ b/drivers/media/dvb/mantis/mantis_cards.c
@@ -73,7 +73,7 @@ static char *label[10] = {
 
 static irqreturn_t mantis_irq_handler(int irq, void *dev_id)
 {
-	u32 stat = 0, mask = 0, lstat = 0;
+	u32 stat = 0, mask = 0;
 	u32 rst_stat = 0, rst_mask = 0;
 
 	struct mantis_pci *mantis;
@@ -88,7 +88,6 @@ static irqreturn_t mantis_irq_handler(int irq, void *dev_id)
 
 	stat = mmread(MANTIS_INT_STAT);
 	mask = mmread(MANTIS_INT_MASK);
-	lstat = stat & ~MANTIS_INT_RISCSTAT;
 	if (!(stat & mask))
 		return IRQ_NONE;
 
diff --git a/drivers/media/dvb/mantis/mantis_dma.c b/drivers/media/dvb/mantis/mantis_dma.c
index c61ca7d3dae..566c407175a 100644
--- a/drivers/media/dvb/mantis/mantis_dma.c
+++ b/drivers/media/dvb/mantis/mantis_dma.c
@@ -199,10 +199,6 @@ void mantis_dma_start(struct mantis_pci *mantis)
 
 void mantis_dma_stop(struct mantis_pci *mantis)
 {
-	u32 stat = 0, mask = 0;
-
-	stat = mmread(MANTIS_INT_STAT);
-	mask = mmread(MANTIS_INT_MASK);
 	dprintk(MANTIS_DEBUG, 1, "Mantis Stop DMA engine");
 
 	mmwrite((mmread(MANTIS_GPIF_ADDR) & (~(MANTIS_GPIF_HIFRDWRN))), MANTIS_GPIF_ADDR);
diff --git a/drivers/media/dvb/mantis/mantis_evm.c b/drivers/media/dvb/mantis/mantis_evm.c
index 36f2256ebb0..71ce52875c3 100644
--- a/drivers/media/dvb/mantis/mantis_evm.c
+++ b/drivers/media/dvb/mantis/mantis_evm.c
@@ -41,10 +41,9 @@ static void mantis_hifevm_work(struct work_struct *work)
 	struct mantis_ca *ca = container_of(work, struct mantis_ca, hif_evm_work);
 	struct mantis_pci *mantis = ca->ca_priv;
 
-	u32 gpif_stat, gpif_mask;
+	u32 gpif_stat;
 
 	gpif_stat = mmread(MANTIS_GPIF_STATUS);
-	gpif_mask = mmread(MANTIS_GPIF_IRQCFG);
 
 	if (gpif_stat & MANTIS_GPIF_DETSTAT) {
 		if (gpif_stat & MANTIS_CARD_PLUGIN) {
diff --git a/drivers/media/dvb/ngene/ngene-core.c b/drivers/media/dvb/ngene/ngene-core.c
index f129a9303f8..39857384af1 100644
--- a/drivers/media/dvb/ngene/ngene-core.c
+++ b/drivers/media/dvb/ngene/ngene-core.c
@@ -1409,10 +1409,8 @@ static int ngene_start(struct ngene *dev)
 	if (stat < 0)
 		goto fail;
 
-	if (!stat)
-		return stat;
+	return 0;
 
-	/* otherwise error: fall through */
 fail:
 	ngwritel(0, NGENE_INT_ENABLE);
 	free_irq(dev->pci_dev->irq, dev);
diff --git a/drivers/media/dvb/pluto2/pluto2.c b/drivers/media/dvb/pluto2/pluto2.c
index e1f20c23698..f148b19a206 100644
--- a/drivers/media/dvb/pluto2/pluto2.c
+++ b/drivers/media/dvb/pluto2/pluto2.c
@@ -481,14 +481,6 @@ static int lg_tdtpe001p_tuner_set_params(struct dvb_frontend *fe)
 	if (p->bandwidth_hz == 8000000)
 		buf[3] |= 0x08;
 
-	if (sizeof(buf) == 6) {
-		buf[4] = buf[2];
-		buf[4] &= ~0x1c;
-		buf[4] |=  0x18;
-
-		buf[5] = (0 << 7) | (2 << 4);
-	}
-
 	msg.addr = I2C_ADDR_TUA6034 >> 1;
 	msg.flags = 0;
 	msg.buf = buf;
diff --git a/drivers/media/dvb/siano/smssdio.c b/drivers/media/dvb/siano/smssdio.c
index 91f8c8291e2..d6f3f100699 100644
--- a/drivers/media/dvb/siano/smssdio.c
+++ b/drivers/media/dvb/siano/smssdio.c
@@ -114,7 +114,7 @@ out:
 
 static void smssdio_interrupt(struct sdio_func *func)
 {
-	int ret, isr;
+	int ret;
 
 	struct smssdio_device *smsdev;
 	struct smscore_buffer_t *cb;
@@ -127,7 +127,7 @@ static void smssdio_interrupt(struct sdio_func *func)
 	 * The interrupt register has no defined meaning. It is just
 	 * a way of turning of the level triggered interrupt.
 	 */
-	isr = sdio_readb(func, SMSSDIO_INT, &ret);
+	(void)sdio_readb(func, SMSSDIO_INT, &ret);
 	if (ret) {
 		sms_err("Unable to read interrupt register!\n");
 		return;
diff --git a/drivers/media/dvb/siano/smsusb.c b/drivers/media/dvb/siano/smsusb.c
index b1fe5137df0..63c004a25e0 100644
--- a/drivers/media/dvb/siano/smsusb.c
+++ b/drivers/media/dvb/siano/smsusb.c
@@ -542,6 +542,8 @@ static const struct usb_device_id smsusb_id_table[] __devinitconst = {
 		.driver_info = SMS1XXX_BOARD_HAUPPAUGE_WINDHAM },
 	{ USB_DEVICE(0x2040, 0xc090),
 		.driver_info = SMS1XXX_BOARD_HAUPPAUGE_WINDHAM },
+	{ USB_DEVICE(0x2040, 0xc0a0),
+		.driver_info = SMS1XXX_BOARD_HAUPPAUGE_WINDHAM },
 	{ } /* Terminating entry */
 	};
 
diff --git a/drivers/media/dvb/ttpci/av7110_v4l.c b/drivers/media/dvb/ttpci/av7110_v4l.c
index ee8ee1d481f..1b2d15140a1 100644
--- a/drivers/media/dvb/ttpci/av7110_v4l.c
+++ b/drivers/media/dvb/ttpci/av7110_v4l.c
@@ -107,7 +107,7 @@ static struct v4l2_input inputs[4] = {
 		.index		= 1,
 		.name		= "Television",
 		.type		= V4L2_INPUT_TYPE_TUNER,
-		.audioset	= 2,
+		.audioset	= 1,
 		.tuner		= 0,
 		.std		= V4L2_STD_PAL_BG|V4L2_STD_NTSC_M,
 		.status		= 0,
@@ -494,7 +494,7 @@ static int vidioc_s_input(struct file *file, void *fh, unsigned int input)
 	dprintk(2, "VIDIOC_S_INPUT: %d\n", input);
 
 	if (!av7110->analog_tuner_flags)
-		return 0;
+		return input ? -EINVAL : 0;
 
 	if (input >= 4)
 		return -EINVAL;
@@ -503,19 +503,38 @@ static int vidioc_s_input(struct file *file, void *fh, unsigned int input)
 	return av7110_dvb_c_switch(fh);
 }
 
+static int vidioc_enumaudio(struct file *file, void *fh, struct v4l2_audio *a)
+{
+	dprintk(2, "VIDIOC_G_AUDIO: %d\n", a->index);
+	if (a->index != 0)
+		return -EINVAL;
+	*a = msp3400_v4l2_audio;
+	return 0;
+}
+
 static int vidioc_g_audio(struct file *file, void *fh, struct v4l2_audio *a)
 {
+	struct saa7146_dev *dev = ((struct saa7146_fh *)fh)->dev;
+	struct av7110 *av7110 = (struct av7110 *)dev->ext_priv;
+
 	dprintk(2, "VIDIOC_G_AUDIO: %d\n", a->index);
 	if (a->index != 0)
 		return -EINVAL;
-	memcpy(a, &msp3400_v4l2_audio, sizeof(struct v4l2_audio));
+	if (av7110->current_input >= 2)
+		return -EINVAL;
+	*a = msp3400_v4l2_audio;
 	return 0;
 }
 
 static int vidioc_s_audio(struct file *file, void *fh, struct v4l2_audio *a)
 {
+	struct saa7146_dev *dev = ((struct saa7146_fh *)fh)->dev;
+	struct av7110 *av7110 = (struct av7110 *)dev->ext_priv;
+
 	dprintk(2, "VIDIOC_S_AUDIO: %d\n", a->index);
-	return 0;
+	if (av7110->current_input >= 2)
+		return -EINVAL;
+	return a->index ? -EINVAL : 0;
 }
 
 static int vidioc_g_sliced_vbi_cap(struct file *file, void *fh,
@@ -802,26 +821,39 @@ int av7110_init_v4l(struct av7110 *av7110)
 		ERR("cannot init capture device. skipping\n");
 		return -ENODEV;
 	}
-	vv_data->ops.vidioc_enum_input = vidioc_enum_input;
-	vv_data->ops.vidioc_g_input = vidioc_g_input;
-	vv_data->ops.vidioc_s_input = vidioc_s_input;
-	vv_data->ops.vidioc_g_tuner = vidioc_g_tuner;
-	vv_data->ops.vidioc_s_tuner = vidioc_s_tuner;
-	vv_data->ops.vidioc_g_frequency = vidioc_g_frequency;
-	vv_data->ops.vidioc_s_frequency = vidioc_s_frequency;
-	vv_data->ops.vidioc_g_audio = vidioc_g_audio;
-	vv_data->ops.vidioc_s_audio = vidioc_s_audio;
-	vv_data->ops.vidioc_g_sliced_vbi_cap = vidioc_g_sliced_vbi_cap;
-	vv_data->ops.vidioc_g_fmt_sliced_vbi_out = vidioc_g_fmt_sliced_vbi_out;
-	vv_data->ops.vidioc_s_fmt_sliced_vbi_out = vidioc_s_fmt_sliced_vbi_out;
+	vv_data->vid_ops.vidioc_enum_input = vidioc_enum_input;
+	vv_data->vid_ops.vidioc_g_input = vidioc_g_input;
+	vv_data->vid_ops.vidioc_s_input = vidioc_s_input;
+	vv_data->vid_ops.vidioc_g_tuner = vidioc_g_tuner;
+	vv_data->vid_ops.vidioc_s_tuner = vidioc_s_tuner;
+	vv_data->vid_ops.vidioc_g_frequency = vidioc_g_frequency;
+	vv_data->vid_ops.vidioc_s_frequency = vidioc_s_frequency;
+	vv_data->vid_ops.vidioc_enumaudio = vidioc_enumaudio;
+	vv_data->vid_ops.vidioc_g_audio = vidioc_g_audio;
+	vv_data->vid_ops.vidioc_s_audio = vidioc_s_audio;
+	vv_data->vid_ops.vidioc_g_fmt_vbi_cap = NULL;
+
+	vv_data->vbi_ops.vidioc_g_tuner = vidioc_g_tuner;
+	vv_data->vbi_ops.vidioc_s_tuner = vidioc_s_tuner;
+	vv_data->vbi_ops.vidioc_g_frequency = vidioc_g_frequency;
+	vv_data->vbi_ops.vidioc_s_frequency = vidioc_s_frequency;
+	vv_data->vbi_ops.vidioc_g_fmt_vbi_cap = NULL;
+	vv_data->vbi_ops.vidioc_g_sliced_vbi_cap = vidioc_g_sliced_vbi_cap;
+	vv_data->vbi_ops.vidioc_g_fmt_sliced_vbi_out = vidioc_g_fmt_sliced_vbi_out;
+	vv_data->vbi_ops.vidioc_s_fmt_sliced_vbi_out = vidioc_s_fmt_sliced_vbi_out;
+
+	if (FW_VERSION(av7110->arm_app) < 0x2623)
+		vv_data->capabilities &= ~V4L2_CAP_SLICED_VBI_OUTPUT;
 
 	if (saa7146_register_device(&av7110->v4l_dev, dev, "av7110", VFL_TYPE_GRABBER)) {
 		ERR("cannot register capture device. skipping\n");
 		saa7146_vv_release(dev);
 		return -ENODEV;
 	}
-	if (saa7146_register_device(&av7110->vbi_dev, dev, "av7110", VFL_TYPE_VBI))
-		ERR("cannot register vbi v4l2 device. skipping\n");
+	if (FW_VERSION(av7110->arm_app) >= 0x2623) {
+		if (saa7146_register_device(&av7110->vbi_dev, dev, "av7110", VFL_TYPE_VBI))
+			ERR("cannot register vbi v4l2 device. skipping\n");
+	}
 	return 0;
 }
 
@@ -905,7 +937,7 @@ static int std_callback(struct saa7146_dev* dev, struct saa7146_standard *std)
 static struct saa7146_ext_vv av7110_vv_data_st = {
 	.inputs		= 1,
 	.audios		= 1,
-	.capabilities	= V4L2_CAP_SLICED_VBI_OUTPUT,
+	.capabilities	= V4L2_CAP_SLICED_VBI_OUTPUT | V4L2_CAP_AUDIO,
 	.flags		= 0,
 
 	.stds		= &standard[0],
@@ -920,7 +952,7 @@ static struct saa7146_ext_vv av7110_vv_data_st = {
 static struct saa7146_ext_vv av7110_vv_data_c = {
 	.inputs		= 1,
 	.audios		= 1,
-	.capabilities	= V4L2_CAP_TUNER | V4L2_CAP_SLICED_VBI_OUTPUT,
+	.capabilities	= V4L2_CAP_TUNER | V4L2_CAP_SLICED_VBI_OUTPUT | V4L2_CAP_AUDIO,
 	.flags		= SAA7146_USE_PORT_B_FOR_VBI,
 
 	.stds		= &standard[0],
diff --git a/drivers/media/dvb/ttpci/budget-av.c b/drivers/media/dvb/ttpci/budget-av.c
index 8b32e282bf5..12ddb53c58d 100644
--- a/drivers/media/dvb/ttpci/budget-av.c
+++ b/drivers/media/dvb/ttpci/budget-av.c
@@ -1483,9 +1483,9 @@ static int budget_av_attach(struct saa7146_dev *dev, struct saa7146_pci_extensio
 			ERR("cannot init vv subsystem\n");
 			return err;
 		}
-		vv_data.ops.vidioc_enum_input = vidioc_enum_input;
-		vv_data.ops.vidioc_g_input = vidioc_g_input;
-		vv_data.ops.vidioc_s_input = vidioc_s_input;
+		vv_data.vid_ops.vidioc_enum_input = vidioc_enum_input;
+		vv_data.vid_ops.vidioc_g_input = vidioc_g_input;
+		vv_data.vid_ops.vidioc_s_input = vidioc_s_input;
 
 		if ((err = saa7146_register_device(&budget_av->vd, dev, "knc1", VFL_TYPE_GRABBER))) {
 			/* fixme: proper cleanup here */
diff --git a/drivers/media/media-entity.c b/drivers/media/media-entity.c
index 056138f63c7..e1cd1328340 100644
--- a/drivers/media/media-entity.c
+++ b/drivers/media/media-entity.c
@@ -214,23 +214,76 @@ EXPORT_SYMBOL_GPL(media_entity_graph_walk_next);
  * pipeline pointer must be identical for all nested calls to
  * media_entity_pipeline_start().
  */
-void media_entity_pipeline_start(struct media_entity *entity,
-				 struct media_pipeline *pipe)
+__must_check int media_entity_pipeline_start(struct media_entity *entity,
+					     struct media_pipeline *pipe)
 {
 	struct media_device *mdev = entity->parent;
 	struct media_entity_graph graph;
+	struct media_entity *entity_err = entity;
+	int ret;
 
 	mutex_lock(&mdev->graph_mutex);
 
 	media_entity_graph_walk_start(&graph, entity);
 
 	while ((entity = media_entity_graph_walk_next(&graph))) {
+		unsigned int i;
+
 		entity->stream_count++;
 		WARN_ON(entity->pipe && entity->pipe != pipe);
 		entity->pipe = pipe;
+
+		/* Already streaming --- no need to check. */
+		if (entity->stream_count > 1)
+			continue;
+
+		if (!entity->ops || !entity->ops->link_validate)
+			continue;
+
+		for (i = 0; i < entity->num_links; i++) {
+			struct media_link *link = &entity->links[i];
+
+			/* Is this pad part of an enabled link? */
+			if (!(link->flags & MEDIA_LNK_FL_ENABLED))
+				continue;
+
+			/* Are we the sink or not? */
+			if (link->sink->entity != entity)
+				continue;
+
+			ret = entity->ops->link_validate(link);
+			if (ret < 0 && ret != -ENOIOCTLCMD)
+				goto error;
+		}
 	}
 
 	mutex_unlock(&mdev->graph_mutex);
+
+	return 0;
+
+error:
+	/*
+	 * Link validation on graph failed. We revert what we did and
+	 * return the error.
+	 */
+	media_entity_graph_walk_start(&graph, entity_err);
+
+	while ((entity_err = media_entity_graph_walk_next(&graph))) {
+		entity_err->stream_count--;
+		if (entity_err->stream_count == 0)
+			entity_err->pipe = NULL;
+
+		/*
+		 * We haven't increased stream_count further than this
+		 * so we quit here.
+		 */
+		if (entity_err == entity)
+			break;
+	}
+
+	mutex_unlock(&mdev->graph_mutex);
+
+	return ret;
 }
 EXPORT_SYMBOL_GPL(media_entity_pipeline_start);
 
diff --git a/drivers/media/radio/Kconfig b/drivers/media/radio/Kconfig
index 8db2d7f4b52..c257da13d76 100644
--- a/drivers/media/radio/Kconfig
+++ b/drivers/media/radio/Kconfig
@@ -320,7 +320,7 @@ config RADIO_MIROPCM20
 	  module will be called radio-miropcm20.
 
 config RADIO_SF16FMI
-	tristate "SF16-FMI/SF16-FMP Radio"
+	tristate "SF16-FMI/SF16-FMP/SF16-FMD Radio"
 	depends on ISA && VIDEO_V4L2
 	---help---
 	  Choose Y here if you have one of these FM radio cards.
@@ -329,7 +329,7 @@ config RADIO_SF16FMI
 	  module will be called radio-sf16fmi.
 
 config RADIO_SF16FMR2
-	tristate "SF16FMR2 Radio"
+	tristate "SF16-FMR2/SF16-FMD2 Radio"
 	depends on ISA && VIDEO_V4L2 && SND
 	---help---
 	  Choose Y here if you have one of these FM radio cards.
diff --git a/drivers/media/radio/dsbr100.c b/drivers/media/radio/dsbr100.c
index f36905b6364..63b112b555b 100644
--- a/drivers/media/radio/dsbr100.c
+++ b/drivers/media/radio/dsbr100.c
@@ -1,92 +1,37 @@
 /* A driver for the D-Link DSB-R100 USB radio and Gemtek USB Radio 21.
- The device plugs into both the USB and an analog audio input, so this thing
- only deals with initialisation and frequency setting, the
- audio data has to be handled by a sound driver.
-
- Major issue: I can't find out where the device reports the signal
- strength, and indeed the windows software appearantly just looks
- at the stereo indicator as well.  So, scanning will only find
- stereo stations.  Sad, but I can't help it.
-
- Also, the windows program sends oodles of messages over to the
- device, and I couldn't figure out their meaning.  My suspicion
- is that they don't have any:-)
-
- You might find some interesting stuff about this module at
- http://unimut.fsk.uni-heidelberg.de/unimut/demi/dsbr
-
- Copyright (c) 2000 Markus Demleitner <msdemlei@cl.uni-heidelberg.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.
-
- 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:
-
- Version 0.46:
-	Removed usb_dsbr100_open/close calls and radio->users counter. Also,
-	radio->muted changed to radio->status and suspend/resume calls updated.
-
- Version 0.45:
-	Converted to v4l2_device.
-
- Version 0.44:
-	Add suspend/resume functions, fix unplug of device,
-	a lot of cleanups and fixes by Alexey Klimov <klimov.linux@gmail.com>
-
- Version 0.43:
-	Oliver Neukum: avoided DMA coherency issue
-
- Version 0.42:
-	Converted dsbr100 to use video_ioctl2
-	by Douglas Landgraf <dougsland@gmail.com>
-
- Version 0.41-ac1:
-	Alan Cox: Some cleanups and fixes
-
- Version 0.41:
-	Converted to V4L2 API by Mauro Carvalho Chehab <mchehab@infradead.org>
-
- Version 0.40:
-	Markus: Updates for 2.6.x kernels, code layout changes, name sanitizing
-
- Version 0.30:
-	Markus: Updates for 2.5.x kernel and more ISO compliant source
-
- Version 0.25:
-	PSL and Markus: Cleanup, radio now doesn't stop on device close
-
- Version 0.24:
-	Markus: Hope I got these silly VIDEO_TUNER_LOW issues finally
-	right.  Some minor cleanup, improved standalone compilation
-
- Version 0.23:
-	Markus: Sign extension bug fixed by declaring transfer_buffer unsigned
-
- Version 0.22:
-	Markus: Some (brown bag) cleanup in what VIDIOCSTUNER returns,
-	thanks to Mike Cox for pointing the problem out.
-
- Version 0.21:
-	Markus: Minor cleanup, warnings if something goes wrong, lame attempt
-	to adhere to Documentation/CodingStyle
-
- Version 0.2:
-	Brad Hards <bradh@dynamite.com.au>: Fixes to make it work as non-module
-	Markus: Copyright clarification
-
- Version 0.01: Markus: initial release
-
+ * The device plugs into both the USB and an analog audio input, so this thing
+ * only deals with initialisation and frequency setting, the
+ * audio data has to be handled by a sound driver.
+ *
+ * Major issue: I can't find out where the device reports the signal
+ * strength, and indeed the windows software appearantly just looks
+ * at the stereo indicator as well.  So, scanning will only find
+ * stereo stations.  Sad, but I can't help it.
+ *
+ * Also, the windows program sends oodles of messages over to the
+ * device, and I couldn't figure out their meaning.  My suspicion
+ * is that they don't have any:-)
+ *
+ * You might find some interesting stuff about this module at
+ * http://unimut.fsk.uni-heidelberg.de/unimut/demi/dsbr
+ *
+ * Fully tested with the Keene USB FM Transmitter and the v4l2-compliance tool.
+ *
+ * Copyright (c) 2000 Markus Demleitner <msdemlei@cl.uni-heidelberg.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.
+ *
+ * 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>
@@ -95,17 +40,19 @@
 #include <linux/slab.h>
 #include <linux/input.h>
 #include <linux/videodev2.h>
+#include <linux/usb.h>
 #include <media/v4l2-device.h>
 #include <media/v4l2-ioctl.h>
-#include <linux/usb.h>
+#include <media/v4l2-ctrls.h>
+#include <media/v4l2-event.h>
 
 /*
  * Version Information
  */
-#define DRIVER_VERSION "0.4.7"
-
-#define DRIVER_AUTHOR "Markus Demleitner <msdemlei@tucana.harvard.edu>"
-#define DRIVER_DESC "D-Link DSB-R100 USB FM radio driver"
+MODULE_AUTHOR("Markus Demleitner <msdemlei@tucana.harvard.edu>");
+MODULE_DESCRIPTION("D-Link DSB-R100 USB FM radio driver");
+MODULE_LICENSE("GPL");
+MODULE_VERSION("1.1.0");
 
 #define DSB100_VENDOR 0x04b4
 #define DSB100_PRODUCT 0x1002
@@ -122,19 +69,8 @@ devices, that would be 76 and 91.  */
 #define FREQ_MAX 108.0
 #define FREQ_MUL 16000
 
-/* defines for radio->status */
-#define STARTED	0
-#define STOPPED	1
-
 #define v4l2_dev_to_radio(d) container_of(d, struct dsbr100_device, v4l2_dev)
 
-static int usb_dsbr100_probe(struct usb_interface *intf,
-			     const struct usb_device_id *id);
-static void usb_dsbr100_disconnect(struct usb_interface *intf);
-static int usb_dsbr100_suspend(struct usb_interface *intf,
-						pm_message_t message);
-static int usb_dsbr100_resume(struct usb_interface *intf);
-
 static int radio_nr = -1;
 module_param(radio_nr, int, 0);
 
@@ -143,179 +79,92 @@ struct dsbr100_device {
 	struct usb_device *usbdev;
 	struct video_device videodev;
 	struct v4l2_device v4l2_dev;
+	struct v4l2_ctrl_handler hdl;
 
 	u8 *transfer_buffer;
 	struct mutex v4l2_lock;
 	int curfreq;
-	int stereo;
-	int status;
-};
-
-static struct usb_device_id usb_dsbr100_device_table [] = {
-	{ USB_DEVICE(DSB100_VENDOR, DSB100_PRODUCT) },
-	{ }						/* Terminating entry */
-};
-
-MODULE_DEVICE_TABLE (usb, usb_dsbr100_device_table);
-
-/* USB subsystem interface */
-static struct usb_driver usb_dsbr100_driver = {
-	.name			= "dsbr100",
-	.probe			= usb_dsbr100_probe,
-	.disconnect		= usb_dsbr100_disconnect,
-	.id_table		= usb_dsbr100_device_table,
-	.suspend		= usb_dsbr100_suspend,
-	.resume			= usb_dsbr100_resume,
-	.reset_resume		= usb_dsbr100_resume,
-	.supports_autosuspend	= 0,
+	bool stereo;
+	bool muted;
 };
 
 /* Low-level device interface begins here */
 
-/* switch on radio */
-static int dsbr100_start(struct dsbr100_device *radio)
+/* set a frequency, freq is defined by v4l's TUNER_LOW, i.e. 1/16th kHz */
+static int dsbr100_setfreq(struct dsbr100_device *radio, unsigned freq)
 {
-	int retval;
-	int request;
-
-	retval = usb_control_msg(radio->usbdev,
-		usb_rcvctrlpipe(radio->usbdev, 0),
-		USB_REQ_GET_STATUS,
-		USB_TYPE_VENDOR | USB_RECIP_DEVICE | USB_DIR_IN,
-		0x00, 0xC7, radio->transfer_buffer, 8, 300);
-
-	if (retval < 0) {
-		request = USB_REQ_GET_STATUS;
-		goto usb_control_msg_failed;
+	unsigned f = (freq / 16 * 80) / 1000 + 856;
+	int retval = 0;
+
+	if (!radio->muted) {
+		retval = usb_control_msg(radio->usbdev,
+				usb_rcvctrlpipe(radio->usbdev, 0),
+				DSB100_TUNE,
+				USB_TYPE_VENDOR | USB_RECIP_DEVICE | USB_DIR_IN,
+				(f >> 8) & 0x00ff, f & 0xff,
+				radio->transfer_buffer, 8, 300);
+		if (retval >= 0)
+			mdelay(1);
 	}
 
-	retval = usb_control_msg(radio->usbdev,
-		usb_rcvctrlpipe(radio->usbdev, 0),
-		DSB100_ONOFF,
-		USB_TYPE_VENDOR | USB_RECIP_DEVICE | USB_DIR_IN,
-		0x01, 0x00, radio->transfer_buffer, 8, 300);
-
-	if (retval < 0) {
-		request = DSB100_ONOFF;
-		goto usb_control_msg_failed;
+	if (retval >= 0) {
+		radio->curfreq = freq;
+		return 0;
 	}
-
-	radio->status = STARTED;
-	return (radio->transfer_buffer)[0];
-
-usb_control_msg_failed:
 	dev_err(&radio->usbdev->dev,
 		"%s - usb_control_msg returned %i, request %i\n",
-			__func__, retval, request);
+			__func__, retval, DSB100_TUNE);
 	return retval;
-
 }
 
-/* switch off radio */
-static int dsbr100_stop(struct dsbr100_device *radio)
+/* switch on radio */
+static int dsbr100_start(struct dsbr100_device *radio)
 {
-	int retval;
-	int request;
-
-	retval = usb_control_msg(radio->usbdev,
-		usb_rcvctrlpipe(radio->usbdev, 0),
-		USB_REQ_GET_STATUS,
-		USB_TYPE_VENDOR | USB_RECIP_DEVICE | USB_DIR_IN,
-		0x16, 0x1C, radio->transfer_buffer, 8, 300);
-
-	if (retval < 0) {
-		request = USB_REQ_GET_STATUS;
-		goto usb_control_msg_failed;
-	}
-
-	retval = usb_control_msg(radio->usbdev,
+	int retval = usb_control_msg(radio->usbdev,
 		usb_rcvctrlpipe(radio->usbdev, 0),
 		DSB100_ONOFF,
 		USB_TYPE_VENDOR | USB_RECIP_DEVICE | USB_DIR_IN,
-		0x00, 0x00, radio->transfer_buffer, 8, 300);
-
-	if (retval < 0) {
-		request = DSB100_ONOFF;
-		goto usb_control_msg_failed;
-	}
-
-	radio->status = STOPPED;
-	return (radio->transfer_buffer)[0];
+		0x01, 0x00, radio->transfer_buffer, 8, 300);
 
-usb_control_msg_failed:
+	if (retval >= 0)
+		return dsbr100_setfreq(radio, radio->curfreq);
 	dev_err(&radio->usbdev->dev,
 		"%s - usb_control_msg returned %i, request %i\n",
-			__func__, retval, request);
+			__func__, retval, DSB100_ONOFF);
 	return retval;
 
 }
 
-/* set a frequency, freq is defined by v4l's TUNER_LOW, i.e. 1/16th kHz */
-static int dsbr100_setfreq(struct dsbr100_device *radio)
+/* switch off radio */
+static int dsbr100_stop(struct dsbr100_device *radio)
 {
-	int retval;
-	int request;
-	int freq = (radio->curfreq / 16 * 80) / 1000 + 856;
-
-	retval = usb_control_msg(radio->usbdev,
-		usb_rcvctrlpipe(radio->usbdev, 0),
-		DSB100_TUNE,
-		USB_TYPE_VENDOR | USB_RECIP_DEVICE | USB_DIR_IN,
-		(freq >> 8) & 0x00ff, freq & 0xff,
-		radio->transfer_buffer, 8, 300);
-
-	if (retval < 0) {
-		request = DSB100_TUNE;
-		goto usb_control_msg_failed;
-	}
-
-	retval = usb_control_msg(radio->usbdev,
+	int retval = usb_control_msg(radio->usbdev,
 		usb_rcvctrlpipe(radio->usbdev, 0),
-		USB_REQ_GET_STATUS,
+		DSB100_ONOFF,
 		USB_TYPE_VENDOR | USB_RECIP_DEVICE | USB_DIR_IN,
-		0x96, 0xB7, radio->transfer_buffer, 8, 300);
-
-	if (retval < 0) {
-		request = USB_REQ_GET_STATUS;
-		goto usb_control_msg_failed;
-	}
-
-	retval = usb_control_msg(radio->usbdev,
-		usb_rcvctrlpipe(radio->usbdev, 0),
-		USB_REQ_GET_STATUS,
-		USB_TYPE_VENDOR | USB_RECIP_DEVICE |  USB_DIR_IN,
-		0x00, 0x24, radio->transfer_buffer, 8, 300);
-
-	if (retval < 0) {
-		request = USB_REQ_GET_STATUS;
-		goto usb_control_msg_failed;
-	}
-
-	radio->stereo = !((radio->transfer_buffer)[0] & 0x01);
-	return (radio->transfer_buffer)[0];
+		0x00, 0x00, radio->transfer_buffer, 8, 300);
 
-usb_control_msg_failed:
-	radio->stereo = -1;
+	if (retval >= 0)
+		return 0;
 	dev_err(&radio->usbdev->dev,
 		"%s - usb_control_msg returned %i, request %i\n",
-			__func__, retval, request);
+			__func__, retval, DSB100_ONOFF);
 	return retval;
+
 }
 
 /* return the device status.  This is, in effect, just whether it
 sees a stereo signal or not.  Pity. */
 static void dsbr100_getstat(struct dsbr100_device *radio)
 {
-	int retval;
-
-	retval = usb_control_msg(radio->usbdev,
+	int retval = usb_control_msg(radio->usbdev,
 		usb_rcvctrlpipe(radio->usbdev, 0),
 		USB_REQ_GET_STATUS,
 		USB_TYPE_VENDOR | USB_RECIP_DEVICE | USB_DIR_IN,
-		0x00 , 0x24, radio->transfer_buffer, 8, 300);
+		0x00, 0x24, radio->transfer_buffer, 8, 300);
 
 	if (retval < 0) {
-		radio->stereo = -1;
+		radio->stereo = false;
 		dev_err(&radio->usbdev->dev,
 			"%s - usb_control_msg returned %i, request %i\n",
 				__func__, retval, USB_REQ_GET_STATUS);
@@ -332,7 +181,8 @@ static int vidioc_querycap(struct file *file, void *priv,
 	strlcpy(v->driver, "dsbr100", sizeof(v->driver));
 	strlcpy(v->card, "D-Link R-100 USB FM Radio", sizeof(v->card));
 	usb_make_path(radio->usbdev, v->bus_info, sizeof(v->bus_info));
-	v->capabilities = V4L2_CAP_TUNER;
+	v->device_caps = V4L2_CAP_RADIO | V4L2_CAP_TUNER;
+	v->capabilities = v->device_caps | V4L2_CAP_DEVICE_CAPS;
 	return 0;
 }
 
@@ -349,13 +199,11 @@ static int vidioc_g_tuner(struct file *file, void *priv,
 	v->type = V4L2_TUNER_RADIO;
 	v->rangelow = FREQ_MIN * FREQ_MUL;
 	v->rangehigh = FREQ_MAX * FREQ_MUL;
-	v->rxsubchans = V4L2_TUNER_SUB_MONO | V4L2_TUNER_SUB_STEREO;
-	v->capability = V4L2_TUNER_CAP_LOW;
-	if(radio->stereo)
-		v->audmode = V4L2_TUNER_MODE_STEREO;
-	else
-		v->audmode = V4L2_TUNER_MODE_MONO;
-	v->signal = 0xffff;     /* We can't get the signal strength */
+	v->rxsubchans = radio->stereo ? V4L2_TUNER_SUB_STEREO :
+		V4L2_TUNER_SUB_MONO;
+	v->capability = V4L2_TUNER_CAP_LOW | V4L2_TUNER_CAP_STEREO;
+	v->audmode = V4L2_TUNER_MODE_STEREO;
+	v->signal = radio->stereo ? 0xffff : 0;     /* We can't get the signal strength */
 	return 0;
 }
 
@@ -369,14 +217,12 @@ static int vidioc_s_frequency(struct file *file, void *priv,
 				struct v4l2_frequency *f)
 {
 	struct dsbr100_device *radio = video_drvdata(file);
-	int retval;
 
-	radio->curfreq = f->frequency;
+	if (f->tuner != 0 || f->type != V4L2_TUNER_RADIO)
+		return -EINVAL;
 
-	retval = dsbr100_setfreq(radio);
-	if (retval < 0)
-		dev_warn(&radio->usbdev->dev, "Set frequency failed\n");
-	return 0;
+	return dsbr100_setfreq(radio, clamp_t(unsigned, f->frequency,
+			FREQ_MIN * FREQ_MUL, FREQ_MAX * FREQ_MUL));
 }
 
 static int vidioc_g_frequency(struct file *file, void *priv,
@@ -384,90 +230,26 @@ static int vidioc_g_frequency(struct file *file, void *priv,
 {
 	struct dsbr100_device *radio = video_drvdata(file);
 
+	if (f->tuner)
+		return -EINVAL;
 	f->type = V4L2_TUNER_RADIO;
 	f->frequency = radio->curfreq;
 	return 0;
 }
 
-static int vidioc_queryctrl(struct file *file, void *priv,
-				struct v4l2_queryctrl *qc)
-{
-	switch (qc->id) {
-	case V4L2_CID_AUDIO_MUTE:
-		return v4l2_ctrl_query_fill(qc, 0, 1, 1, 1);
-	}
-
-	return -EINVAL;
-}
-
-static int vidioc_g_ctrl(struct file *file, void *priv,
-				struct v4l2_control *ctrl)
+static int usb_dsbr100_s_ctrl(struct v4l2_ctrl *ctrl)
 {
-	struct dsbr100_device *radio = video_drvdata(file);
+	struct dsbr100_device *radio =
+		container_of(ctrl->handler, struct dsbr100_device, hdl);
 
 	switch (ctrl->id) {
 	case V4L2_CID_AUDIO_MUTE:
-		ctrl->value = radio->status;
-		return 0;
+		radio->muted = ctrl->val;
+		return radio->muted ? dsbr100_stop(radio) : dsbr100_start(radio);
 	}
 	return -EINVAL;
 }
 
-static int vidioc_s_ctrl(struct file *file, void *priv,
-				struct v4l2_control *ctrl)
-{
-	struct dsbr100_device *radio = video_drvdata(file);
-	int retval;
-
-	switch (ctrl->id) {
-	case V4L2_CID_AUDIO_MUTE:
-		if (ctrl->value) {
-			retval = dsbr100_stop(radio);
-			if (retval < 0) {
-				dev_warn(&radio->usbdev->dev,
-					 "Radio did not respond properly\n");
-				return -EBUSY;
-			}
-		} else {
-			retval = dsbr100_start(radio);
-			if (retval < 0) {
-				dev_warn(&radio->usbdev->dev,
-					 "Radio did not respond properly\n");
-				return -EBUSY;
-			}
-		}
-		return 0;
-	}
-	return -EINVAL;
-}
-
-static int vidioc_g_audio(struct file *file, void *priv,
-				struct v4l2_audio *a)
-{
-	if (a->index > 1)
-		return -EINVAL;
-
-	strcpy(a->name, "Radio");
-	a->capability = V4L2_AUDCAP_STEREO;
-	return 0;
-}
-
-static int vidioc_g_input(struct file *filp, void *priv, unsigned int *i)
-{
-	*i = 0;
-	return 0;
-}
-
-static int vidioc_s_input(struct file *filp, void *priv, unsigned int i)
-{
-	return i ? -EINVAL : 0;
-}
-
-static int vidioc_s_audio(struct file *file, void *priv,
-					struct v4l2_audio *a)
-{
-	return a->index ? -EINVAL : 0;
-}
 
 /* USB subsystem interface begins here */
 
@@ -481,8 +263,17 @@ static void usb_dsbr100_disconnect(struct usb_interface *intf)
 {
 	struct dsbr100_device *radio = usb_get_intfdata(intf);
 
-	v4l2_device_get(&radio->v4l2_dev);
 	mutex_lock(&radio->v4l2_lock);
+	/*
+	 * Disconnect is also called on unload, and in that case we need to
+	 * mute the device. This call will silently fail if it is called
+	 * after a physical disconnect.
+	 */
+	usb_control_msg(radio->usbdev,
+		usb_rcvctrlpipe(radio->usbdev, 0),
+		DSB100_ONOFF,
+		USB_TYPE_VENDOR | USB_RECIP_DEVICE | USB_DIR_IN,
+		0x00, 0x00, radio->transfer_buffer, 8, 300);
 	usb_set_intfdata(intf, NULL);
 	video_unregister_device(&radio->videodev);
 	v4l2_device_disconnect(&radio->v4l2_dev);
@@ -495,25 +286,13 @@ static void usb_dsbr100_disconnect(struct usb_interface *intf)
 static int usb_dsbr100_suspend(struct usb_interface *intf, pm_message_t message)
 {
 	struct dsbr100_device *radio = usb_get_intfdata(intf);
-	int retval;
 
 	mutex_lock(&radio->v4l2_lock);
-	if (radio->status == STARTED) {
-		retval = dsbr100_stop(radio);
-		if (retval < 0)
-			dev_warn(&intf->dev, "dsbr100_stop failed\n");
-
-		/* After dsbr100_stop() status set to STOPPED.
-		 * If we want driver to start radio on resume
-		 * we set status equal to STARTED.
-		 * On resume we will check status and run radio if needed.
-		 */
-		radio->status = STARTED;
-	}
+	if (!radio->muted && dsbr100_stop(radio) < 0)
+		dev_warn(&intf->dev, "dsbr100_stop failed\n");
 	mutex_unlock(&radio->v4l2_lock);
 
 	dev_info(&intf->dev, "going into suspend..\n");
-
 	return 0;
 }
 
@@ -521,18 +300,13 @@ static int usb_dsbr100_suspend(struct usb_interface *intf, pm_message_t message)
 static int usb_dsbr100_resume(struct usb_interface *intf)
 {
 	struct dsbr100_device *radio = usb_get_intfdata(intf);
-	int retval;
 
 	mutex_lock(&radio->v4l2_lock);
-	if (radio->status == STARTED) {
-		retval = dsbr100_start(radio);
-		if (retval < 0)
-			dev_warn(&intf->dev, "dsbr100_start failed\n");
-	}
+	if (!radio->muted && dsbr100_start(radio) < 0)
+		dev_warn(&intf->dev, "dsbr100_start failed\n");
 	mutex_unlock(&radio->v4l2_lock);
 
 	dev_info(&intf->dev, "coming out of suspend..\n");
-
 	return 0;
 }
 
@@ -541,15 +315,23 @@ static void usb_dsbr100_release(struct v4l2_device *v4l2_dev)
 {
 	struct dsbr100_device *radio = v4l2_dev_to_radio(v4l2_dev);
 
+	v4l2_ctrl_handler_free(&radio->hdl);
 	v4l2_device_unregister(&radio->v4l2_dev);
 	kfree(radio->transfer_buffer);
 	kfree(radio);
 }
 
+static const struct v4l2_ctrl_ops usb_dsbr100_ctrl_ops = {
+	.s_ctrl = usb_dsbr100_s_ctrl,
+};
+
 /* File system interface */
 static const struct v4l2_file_operations usb_dsbr100_fops = {
 	.owner		= THIS_MODULE,
 	.unlocked_ioctl	= video_ioctl2,
+	.open           = v4l2_fh_open,
+	.release        = v4l2_fh_release,
+	.poll		= v4l2_ctrl_poll,
 };
 
 static const struct v4l2_ioctl_ops usb_dsbr100_ioctl_ops = {
@@ -558,13 +340,9 @@ static const struct v4l2_ioctl_ops usb_dsbr100_ioctl_ops = {
 	.vidioc_s_tuner     = vidioc_s_tuner,
 	.vidioc_g_frequency = vidioc_g_frequency,
 	.vidioc_s_frequency = vidioc_s_frequency,
-	.vidioc_queryctrl   = vidioc_queryctrl,
-	.vidioc_g_ctrl      = vidioc_g_ctrl,
-	.vidioc_s_ctrl      = vidioc_s_ctrl,
-	.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_log_status  = v4l2_ctrl_log_status,
+	.vidioc_subscribe_event = v4l2_ctrl_subscribe_event,
+	.vidioc_unsubscribe_event = v4l2_event_unsubscribe,
 };
 
 /* check if the device is present and register with v4l and usb if it is */
@@ -593,11 +371,17 @@ static int usb_dsbr100_probe(struct usb_interface *intf,
 	retval = v4l2_device_register(&intf->dev, v4l2_dev);
 	if (retval < 0) {
 		v4l2_err(v4l2_dev, "couldn't register v4l2_device\n");
-		kfree(radio->transfer_buffer);
-		kfree(radio);
-		return retval;
+		goto err_reg_dev;
 	}
 
+	v4l2_ctrl_handler_init(&radio->hdl, 1);
+	v4l2_ctrl_new_std(&radio->hdl, &usb_dsbr100_ctrl_ops,
+			  V4L2_CID_AUDIO_MUTE, 0, 1, 1, 1);
+	if (radio->hdl.error) {
+		retval = radio->hdl.error;
+		v4l2_err(v4l2_dev, "couldn't register control\n");
+		goto err_reg_ctrl;
+	}
 	mutex_init(&radio->v4l2_lock);
 	strlcpy(radio->videodev.name, v4l2_dev->name, sizeof(radio->videodev.name));
 	radio->videodev.v4l2_dev = v4l2_dev;
@@ -605,28 +389,46 @@ static int usb_dsbr100_probe(struct usb_interface *intf,
 	radio->videodev.ioctl_ops = &usb_dsbr100_ioctl_ops;
 	radio->videodev.release = video_device_release_empty;
 	radio->videodev.lock = &radio->v4l2_lock;
+	radio->videodev.ctrl_handler = &radio->hdl;
+	set_bit(V4L2_FL_USE_FH_PRIO, &radio->videodev.flags);
 
 	radio->usbdev = interface_to_usbdev(intf);
 	radio->curfreq = FREQ_MIN * FREQ_MUL;
-	radio->status = STOPPED;
+	radio->muted = true;
 
 	video_set_drvdata(&radio->videodev, radio);
+	usb_set_intfdata(intf, radio);
 
 	retval = video_register_device(&radio->videodev, VFL_TYPE_RADIO, radio_nr);
-	if (retval < 0) {
-		v4l2_err(v4l2_dev, "couldn't register video device\n");
-		v4l2_device_unregister(v4l2_dev);
-		kfree(radio->transfer_buffer);
-		kfree(radio);
-		return -EIO;
-	}
-	usb_set_intfdata(intf, radio);
-	return 0;
+	if (retval == 0)
+		return 0;
+	v4l2_err(v4l2_dev, "couldn't register video device\n");
+
+err_reg_ctrl:
+	v4l2_ctrl_handler_free(&radio->hdl);
+	v4l2_device_unregister(v4l2_dev);
+err_reg_dev:
+	kfree(radio->transfer_buffer);
+	kfree(radio);
+	return retval;
 }
 
-module_usb_driver(usb_dsbr100_driver);
+static struct usb_device_id usb_dsbr100_device_table[] = {
+	{ USB_DEVICE(DSB100_VENDOR, DSB100_PRODUCT) },
+	{ }						/* Terminating entry */
+};
 
-MODULE_AUTHOR( DRIVER_AUTHOR );
-MODULE_DESCRIPTION( DRIVER_DESC );
-MODULE_LICENSE("GPL");
-MODULE_VERSION(DRIVER_VERSION);
+MODULE_DEVICE_TABLE(usb, usb_dsbr100_device_table);
+
+/* USB subsystem interface */
+static struct usb_driver usb_dsbr100_driver = {
+	.name			= "dsbr100",
+	.probe			= usb_dsbr100_probe,
+	.disconnect		= usb_dsbr100_disconnect,
+	.id_table		= usb_dsbr100_device_table,
+	.suspend		= usb_dsbr100_suspend,
+	.resume			= usb_dsbr100_resume,
+	.reset_resume		= usb_dsbr100_resume,
+};
+
+module_usb_driver(usb_dsbr100_driver);
diff --git a/drivers/media/radio/radio-gemtek.c b/drivers/media/radio/radio-gemtek.c
index 2e639ce6f25..235c0e34982 100644
--- a/drivers/media/radio/radio-gemtek.c
+++ b/drivers/media/radio/radio-gemtek.c
@@ -29,6 +29,7 @@
 #include <linux/videodev2.h>	/* kernel radio structs		*/
 #include <linux/mutex.h>
 #include <linux/io.h>		/* outb, outb_p			*/
+#include <linux/pnp.h>
 #include <linux/slab.h>
 #include <media/v4l2-ioctl.h>
 #include <media/v4l2-device.h>
@@ -283,6 +284,16 @@ static const struct radio_isa_ops gemtek_ops = {
 
 static const int gemtek_ioports[] = { 0x20c, 0x30c, 0x24c, 0x34c, 0x248, 0x28c };
 
+#ifdef CONFIG_PNP
+static struct pnp_device_id gemtek_pnp_devices[] = {
+	/* AOpen FX-3D/Pro Radio */
+	{.id = "ADS7183", .driver_data = 0},
+	{.id = ""}
+};
+
+MODULE_DEVICE_TABLE(pnp, gemtek_pnp_devices);
+#endif
+
 static struct radio_isa_driver gemtek_driver = {
 	.driver = {
 		.match		= radio_isa_match,
@@ -292,6 +303,14 @@ static struct radio_isa_driver gemtek_driver = {
 			.name	= "radio-gemtek",
 		},
 	},
+#ifdef CONFIG_PNP
+	.pnp_driver = {
+		.name		= "radio-gemtek",
+		.id_table	= gemtek_pnp_devices,
+		.probe		= radio_isa_pnp_probe,
+		.remove		= radio_isa_pnp_remove,
+	},
+#endif
 	.io_params = io,
 	.radio_nr_params = radio_nr,
 	.io_ports = gemtek_ioports,
@@ -305,12 +324,18 @@ static struct radio_isa_driver gemtek_driver = {
 static int __init gemtek_init(void)
 {
 	gemtek_driver.probe = probe;
+#ifdef CONFIG_PNP
+	pnp_register_driver(&gemtek_driver.pnp_driver);
+#endif
 	return isa_register_driver(&gemtek_driver.driver, GEMTEK_MAX);
 }
 
 static void __exit gemtek_exit(void)
 {
 	hardmute = 1;	/* Turn off PLL */
+#ifdef CONFIG_PNP
+	pnp_unregister_driver(&gemtek_driver.pnp_driver);
+#endif
 	isa_unregister_driver(&gemtek_driver.driver);
 }
 
diff --git a/drivers/media/radio/radio-isa.c b/drivers/media/radio/radio-isa.c
index 06f906351fa..3c0067de432 100644
--- a/drivers/media/radio/radio-isa.c
+++ b/drivers/media/radio/radio-isa.c
@@ -150,14 +150,6 @@ static int radio_isa_log_status(struct file *file, void *priv)
 	return 0;
 }
 
-static int radio_isa_subscribe_event(struct v4l2_fh *fh,
-				struct v4l2_event_subscription *sub)
-{
-	if (sub->type == V4L2_EVENT_CTRL)
-		return v4l2_event_subscribe(fh, sub, 0);
-	return -EINVAL;
-}
-
 static const struct v4l2_ctrl_ops radio_isa_ctrl_ops = {
 	.s_ctrl = radio_isa_s_ctrl,
 };
@@ -177,7 +169,7 @@ static const struct v4l2_ioctl_ops radio_isa_ioctl_ops = {
 	.vidioc_g_frequency = radio_isa_g_frequency,
 	.vidioc_s_frequency = radio_isa_s_frequency,
 	.vidioc_log_status  = radio_isa_log_status,
-	.vidioc_subscribe_event   = radio_isa_subscribe_event,
+	.vidioc_subscribe_event   = v4l2_ctrl_subscribe_event,
 	.vidioc_unsubscribe_event = v4l2_event_unsubscribe,
 };
 
@@ -199,56 +191,31 @@ static bool radio_isa_valid_io(const struct radio_isa_driver *drv, int io)
 	return false;
 }
 
-int radio_isa_probe(struct device *pdev, unsigned int dev)
+struct radio_isa_card *radio_isa_alloc(struct radio_isa_driver *drv,
+				struct device *pdev)
 {
-	struct radio_isa_driver *drv = pdev->platform_data;
-	const struct radio_isa_ops *ops = drv->ops;
 	struct v4l2_device *v4l2_dev;
-	struct radio_isa_card *isa;
-	int res;
+	struct radio_isa_card *isa = drv->ops->alloc();
+	if (!isa)
+		return NULL;
 
-	isa = drv->ops->alloc();
-	if (isa == NULL)
-		return -ENOMEM;
 	dev_set_drvdata(pdev, isa);
 	isa->drv = drv;
-	isa->io = drv->io_params[dev];
 	v4l2_dev = &isa->v4l2_dev;
 	strlcpy(v4l2_dev->name, dev_name(pdev), sizeof(v4l2_dev->name));
 
-	if (drv->probe && ops->probe) {
-		int i;
-
-		for (i = 0; i < drv->num_of_io_ports; ++i) {
-			int io = drv->io_ports[i];
-
-			if (request_region(io, drv->region_size, v4l2_dev->name)) {
-				bool found = ops->probe(isa, io);
-
-				release_region(io, drv->region_size);
-				if (found) {
-					isa->io = io;
-					break;
-				}
-			}
-		}
-	}
-
-	if (!radio_isa_valid_io(drv, isa->io)) {
-		int i;
+	return isa;
+}
 
-		if (isa->io < 0)
-			return -ENODEV;
-		v4l2_err(v4l2_dev, "you must set an I/O address with io=0x%03x",
-				drv->io_ports[0]);
-		for (i = 1; i < drv->num_of_io_ports; i++)
-			printk(KERN_CONT "/0x%03x", drv->io_ports[i]);
-		printk(KERN_CONT ".\n");
-		kfree(isa);
-		return -EINVAL;
-	}
+int radio_isa_common_probe(struct radio_isa_card *isa, struct device *pdev,
+				int radio_nr, unsigned region_size)
+{
+	const struct radio_isa_driver *drv = isa->drv;
+	const struct radio_isa_ops *ops = drv->ops;
+	struct v4l2_device *v4l2_dev = &isa->v4l2_dev;
+	int res;
 
-	if (!request_region(isa->io, drv->region_size, v4l2_dev->name)) {
+	if (!request_region(isa->io, region_size, v4l2_dev->name)) {
 		v4l2_err(v4l2_dev, "port 0x%x already in use\n", isa->io);
 		kfree(isa);
 		return -EBUSY;
@@ -299,42 +266,126 @@ int radio_isa_probe(struct device *pdev, unsigned int dev)
 		res = ops->s_stereo(isa, isa->stereo);
 	if (res < 0) {
 		v4l2_err(v4l2_dev, "Could not setup card\n");
-		goto err_node_reg;
+		goto err_hdl;
 	}
-	res = video_register_device(&isa->vdev, VFL_TYPE_RADIO,
-					drv->radio_nr_params[dev]);
+	res = video_register_device(&isa->vdev, VFL_TYPE_RADIO, radio_nr);
+
 	if (res < 0) {
 		v4l2_err(v4l2_dev, "Could not register device node\n");
-		goto err_node_reg;
+		goto err_hdl;
 	}
 
 	v4l2_info(v4l2_dev, "Initialized radio card %s on port 0x%03x\n",
 			drv->card, isa->io);
 	return 0;
 
-err_node_reg:
-	v4l2_ctrl_handler_free(&isa->hdl);
 err_hdl:
-	v4l2_device_unregister(&isa->v4l2_dev);
+	v4l2_ctrl_handler_free(&isa->hdl);
 err_dev_reg:
-	release_region(isa->io, drv->region_size);
+	release_region(isa->io, region_size);
 	kfree(isa);
 	return res;
 }
-EXPORT_SYMBOL_GPL(radio_isa_probe);
 
-int radio_isa_remove(struct device *pdev, unsigned int dev)
+int radio_isa_common_remove(struct radio_isa_card *isa, unsigned region_size)
 {
-	struct radio_isa_card *isa = dev_get_drvdata(pdev);
 	const struct radio_isa_ops *ops = isa->drv->ops;
 
 	ops->s_mute_volume(isa, true, isa->volume ? isa->volume->cur.val : 0);
 	video_unregister_device(&isa->vdev);
 	v4l2_ctrl_handler_free(&isa->hdl);
 	v4l2_device_unregister(&isa->v4l2_dev);
-	release_region(isa->io, isa->drv->region_size);
+	release_region(isa->io, region_size);
 	v4l2_info(&isa->v4l2_dev, "Removed radio card %s\n", isa->drv->card);
 	kfree(isa);
 	return 0;
 }
+
+int radio_isa_probe(struct device *pdev, unsigned int dev)
+{
+	struct radio_isa_driver *drv = pdev->platform_data;
+	const struct radio_isa_ops *ops = drv->ops;
+	struct v4l2_device *v4l2_dev;
+	struct radio_isa_card *isa;
+
+	isa = radio_isa_alloc(drv, pdev);
+	if (!isa)
+		return -ENOMEM;
+	isa->io = drv->io_params[dev];
+	v4l2_dev = &isa->v4l2_dev;
+
+	if (drv->probe && ops->probe) {
+		int i;
+
+		for (i = 0; i < drv->num_of_io_ports; ++i) {
+			int io = drv->io_ports[i];
+
+			if (request_region(io, drv->region_size, v4l2_dev->name)) {
+				bool found = ops->probe(isa, io);
+
+				release_region(io, drv->region_size);
+				if (found) {
+					isa->io = io;
+					break;
+				}
+			}
+		}
+	}
+
+	if (!radio_isa_valid_io(drv, isa->io)) {
+		int i;
+
+		if (isa->io < 0)
+			return -ENODEV;
+		v4l2_err(v4l2_dev, "you must set an I/O address with io=0x%03x",
+				drv->io_ports[0]);
+		for (i = 1; i < drv->num_of_io_ports; i++)
+			printk(KERN_CONT "/0x%03x", drv->io_ports[i]);
+		printk(KERN_CONT ".\n");
+		kfree(isa);
+		return -EINVAL;
+	}
+
+	return radio_isa_common_probe(isa, pdev, drv->radio_nr_params[dev],
+					drv->region_size);
+}
+EXPORT_SYMBOL_GPL(radio_isa_probe);
+
+int radio_isa_remove(struct device *pdev, unsigned int dev)
+{
+	struct radio_isa_card *isa = dev_get_drvdata(pdev);
+
+	return radio_isa_common_remove(isa, isa->drv->region_size);
+}
 EXPORT_SYMBOL_GPL(radio_isa_remove);
+
+#ifdef CONFIG_PNP
+int radio_isa_pnp_probe(struct pnp_dev *dev, const struct pnp_device_id *dev_id)
+{
+	struct pnp_driver *pnp_drv = to_pnp_driver(dev->dev.driver);
+	struct radio_isa_driver *drv = container_of(pnp_drv,
+					struct radio_isa_driver, pnp_driver);
+	struct radio_isa_card *isa;
+
+	if (!pnp_port_valid(dev, 0))
+		return -ENODEV;
+
+	isa = radio_isa_alloc(drv, &dev->dev);
+	if (!isa)
+		return -ENOMEM;
+
+	isa->io = pnp_port_start(dev, 0);
+
+	return radio_isa_common_probe(isa, &dev->dev, drv->radio_nr_params[0],
+					pnp_port_len(dev, 0));
+}
+EXPORT_SYMBOL_GPL(radio_isa_pnp_probe);
+
+void radio_isa_pnp_remove(struct pnp_dev *dev)
+{
+	struct radio_isa_card *isa = dev_get_drvdata(&dev->dev);
+
+	radio_isa_common_remove(isa, pnp_port_len(dev, 0));
+}
+EXPORT_SYMBOL_GPL(radio_isa_pnp_remove);
+#endif
diff --git a/drivers/media/radio/radio-isa.h b/drivers/media/radio/radio-isa.h
index 8a0ea84d86d..ba4c01f1bd0 100644
--- a/drivers/media/radio/radio-isa.h
+++ b/drivers/media/radio/radio-isa.h
@@ -24,6 +24,7 @@
 #define _RADIO_ISA_H_
 
 #include <linux/isa.h>
+#include <linux/pnp.h>
 #include <linux/videodev2.h>
 #include <media/v4l2-device.h>
 #include <media/v4l2-ctrls.h>
@@ -76,6 +77,9 @@ struct radio_isa_ops {
 /* Top level structure needed to instantiate the cards */
 struct radio_isa_driver {
 	struct isa_driver driver;
+#ifdef CONFIG_PNP
+	struct pnp_driver pnp_driver;
+#endif
 	const struct radio_isa_ops *ops;
 	/* The module_param_array with the specified I/O ports */
 	int *io_params;
@@ -101,5 +105,10 @@ struct radio_isa_driver {
 int radio_isa_match(struct device *pdev, unsigned int dev);
 int radio_isa_probe(struct device *pdev, unsigned int dev);
 int radio_isa_remove(struct device *pdev, unsigned int dev);
+#ifdef CONFIG_PNP
+int radio_isa_pnp_probe(struct pnp_dev *dev,
+			const struct pnp_device_id *dev_id);
+void radio_isa_pnp_remove(struct pnp_dev *dev);
+#endif
 
 #endif
diff --git a/drivers/media/radio/radio-keene.c b/drivers/media/radio/radio-keene.c
index 55bd1d2937c..79adf3e654e 100644
--- a/drivers/media/radio/radio-keene.c
+++ b/drivers/media/radio/radio-keene.c
@@ -28,7 +28,6 @@
 #include <media/v4l2-ctrls.h>
 #include <media/v4l2-event.h>
 #include <linux/usb.h>
-#include <linux/version.h>
 #include <linux/mutex.h>
 
 /* driver and module definitions */
@@ -149,7 +148,6 @@ static void usb_keene_disconnect(struct usb_interface *intf)
 {
 	struct keene_device *radio = to_keene_dev(usb_get_intfdata(intf));
 
-	v4l2_device_get(&radio->v4l2_dev);
 	mutex_lock(&radio->lock);
 	usb_set_intfdata(intf, NULL);
 	video_unregister_device(&radio->vdev);
@@ -158,6 +156,23 @@ static void usb_keene_disconnect(struct usb_interface *intf)
 	v4l2_device_put(&radio->v4l2_dev);
 }
 
+static int usb_keene_suspend(struct usb_interface *intf, pm_message_t message)
+{
+	struct keene_device *radio = to_keene_dev(usb_get_intfdata(intf));
+
+	return keene_cmd_main(radio, 0, false);
+}
+
+static int usb_keene_resume(struct usb_interface *intf)
+{
+	struct keene_device *radio = to_keene_dev(usb_get_intfdata(intf));
+
+	mdelay(50);
+	keene_cmd_set(radio);
+	keene_cmd_main(radio, radio->curfreq, true);
+	return 0;
+}
+
 static int vidioc_querycap(struct file *file, void *priv,
 					struct v4l2_capability *v)
 {
@@ -256,18 +271,6 @@ static int keene_s_ctrl(struct v4l2_ctrl *ctrl)
 	return -EINVAL;
 }
 
-static int vidioc_subscribe_event(struct v4l2_fh *fh,
-				struct v4l2_event_subscription *sub)
-{
-	switch (sub->type) {
-	case V4L2_EVENT_CTRL:
-		return v4l2_event_subscribe(fh, sub, 0);
-	default:
-		return -EINVAL;
-	}
-}
-
-
 /* File system interface */
 static const struct v4l2_file_operations usb_keene_fops = {
 	.owner		= THIS_MODULE,
@@ -288,7 +291,7 @@ static const struct v4l2_ioctl_ops usb_keene_ioctl_ops = {
 	.vidioc_g_frequency = vidioc_g_frequency,
 	.vidioc_s_frequency = vidioc_s_frequency,
 	.vidioc_log_status = v4l2_ctrl_log_status,
-	.vidioc_subscribe_event = vidioc_subscribe_event,
+	.vidioc_subscribe_event = v4l2_ctrl_subscribe_event,
 	.vidioc_unsubscribe_event = v4l2_event_unsubscribe,
 };
 
@@ -404,6 +407,9 @@ static struct usb_driver usb_keene_driver = {
 	.probe			= usb_keene_probe,
 	.disconnect		= usb_keene_disconnect,
 	.id_table		= usb_keene_device_table,
+	.suspend		= usb_keene_suspend,
+	.resume			= usb_keene_resume,
+	.reset_resume		= usb_keene_resume,
 };
 
 static int __init keene_init(void)
diff --git a/drivers/media/radio/radio-mr800.c b/drivers/media/radio/radio-mr800.c
index a860a72a58e..94cb6bc690f 100644
--- a/drivers/media/radio/radio-mr800.c
+++ b/drivers/media/radio/radio-mr800.c
@@ -62,6 +62,8 @@
 #include <linux/videodev2.h>
 #include <media/v4l2-device.h>
 #include <media/v4l2-ioctl.h>
+#include <media/v4l2-ctrls.h>
+#include <media/v4l2-event.h>
 #include <linux/usb.h>
 #include <linux/mutex.h>
 
@@ -101,12 +103,17 @@ devices, that would be 76 and 91.  */
  * List isn't full and will be updated with implementation of new functions
  */
 #define AMRADIO_SET_FREQ	0xa4
+#define AMRADIO_GET_READY_FLAG	0xa5
+#define AMRADIO_GET_SIGNAL	0xa7
+#define AMRADIO_GET_FREQ	0xa8
+#define AMRADIO_SET_SEARCH_UP	0xa9
+#define AMRADIO_SET_SEARCH_DOWN	0xaa
 #define AMRADIO_SET_MUTE	0xab
+#define AMRADIO_SET_RIGHT_MUTE	0xac
+#define AMRADIO_SET_LEFT_MUTE	0xad
 #define AMRADIO_SET_MONO	0xae
-
-/* Comfortable defines for amradio_set_mute */
-#define AMRADIO_START		0x00
-#define AMRADIO_STOP		0x01
+#define AMRADIO_SET_SEARCH_LVL	0xb0
+#define AMRADIO_STOP_SEARCH	0xb1
 
 /* Comfortable defines for amradio_set_stereo */
 #define WANT_STEREO		0x00
@@ -117,29 +124,20 @@ static int radio_nr = -1;
 module_param(radio_nr, int, 0);
 MODULE_PARM_DESC(radio_nr, "Radio Nr");
 
-static int usb_amradio_probe(struct usb_interface *intf,
-			     const struct usb_device_id *id);
-static void usb_amradio_disconnect(struct usb_interface *intf);
-static int usb_amradio_open(struct file *file);
-static int usb_amradio_close(struct file *file);
-static int usb_amradio_suspend(struct usb_interface *intf,
-				pm_message_t message);
-static int usb_amradio_resume(struct usb_interface *intf);
-
 /* Data for one (physical) device */
 struct amradio_device {
 	/* reference to USB and video device */
 	struct usb_device *usbdev;
 	struct usb_interface *intf;
-	struct video_device videodev;
+	struct video_device vdev;
 	struct v4l2_device v4l2_dev;
+	struct v4l2_ctrl_handler hdl;
 
-	unsigned char *buffer;
+	u8 *buffer;
 	struct mutex lock;	/* buffer locking */
 	int curfreq;
 	int stereo;
 	int muted;
-	int initialized;
 };
 
 static inline struct amradio_device *to_amradio_dev(struct v4l2_device *v4l2_dev)
@@ -147,29 +145,8 @@ static inline struct amradio_device *to_amradio_dev(struct v4l2_device *v4l2_dev
 	return container_of(v4l2_dev, struct amradio_device, v4l2_dev);
 }
 
-/* USB Device ID List */
-static struct usb_device_id usb_amradio_device_table[] = {
-	{USB_DEVICE_AND_INTERFACE_INFO(USB_AMRADIO_VENDOR, USB_AMRADIO_PRODUCT,
-							USB_CLASS_HID, 0, 0) },
-	{ }						/* Terminating entry */
-};
-
-MODULE_DEVICE_TABLE(usb, usb_amradio_device_table);
-
-/* USB subsystem interface */
-static struct usb_driver usb_amradio_driver = {
-	.name			= MR800_DRIVER_NAME,
-	.probe			= usb_amradio_probe,
-	.disconnect		= usb_amradio_disconnect,
-	.suspend		= usb_amradio_suspend,
-	.resume			= usb_amradio_resume,
-	.reset_resume		= usb_amradio_resume,
-	.id_table		= usb_amradio_device_table,
-	.supports_autosuspend	= 1,
-};
-
-/* switch on/off the radio. Send 8 bytes to device */
-static int amradio_set_mute(struct amradio_device *radio, char argument)
+static int amradio_send_cmd(struct amradio_device *radio, u8 cmd, u8 arg,
+		u8 *extra, u8 extralen, bool reply)
 {
 	int retval;
 	int size;
@@ -177,99 +154,92 @@ static int amradio_set_mute(struct amradio_device *radio, char argument)
 	radio->buffer[0] = 0x00;
 	radio->buffer[1] = 0x55;
 	radio->buffer[2] = 0xaa;
-	radio->buffer[3] = 0x00;
-	radio->buffer[4] = AMRADIO_SET_MUTE;
-	radio->buffer[5] = argument;
+	radio->buffer[3] = extralen;
+	radio->buffer[4] = cmd;
+	radio->buffer[5] = arg;
 	radio->buffer[6] = 0x00;
-	radio->buffer[7] = 0x00;
+	radio->buffer[7] = extra || reply ? 8 : 0;
 
 	retval = usb_bulk_msg(radio->usbdev, usb_sndintpipe(radio->usbdev, 2),
-		(void *) (radio->buffer), BUFFER_LENGTH, &size, USB_TIMEOUT);
+		radio->buffer, BUFFER_LENGTH, &size, USB_TIMEOUT);
 
 	if (retval < 0 || size != BUFFER_LENGTH) {
-		amradio_dev_warn(&radio->videodev.dev, "set mute failed\n");
-		return retval;
+		if (video_is_registered(&radio->vdev))
+			amradio_dev_warn(&radio->vdev.dev,
+					"cmd %02x failed\n", cmd);
+		return retval ? retval : -EIO;
 	}
+	if (!extra && !reply)
+		return 0;
 
-	radio->muted = argument;
+	if (extra) {
+		memcpy(radio->buffer, extra, extralen);
+		memset(radio->buffer + extralen, 0, 8 - extralen);
+		retval = usb_bulk_msg(radio->usbdev, usb_sndintpipe(radio->usbdev, 2),
+			radio->buffer, BUFFER_LENGTH, &size, USB_TIMEOUT);
+	} else {
+		memset(radio->buffer, 0, 8);
+		retval = usb_bulk_msg(radio->usbdev, usb_rcvbulkpipe(radio->usbdev, 0x81),
+			radio->buffer, BUFFER_LENGTH, &size, USB_TIMEOUT);
+	}
+	if (retval == 0 && size == BUFFER_LENGTH)
+		return 0;
+	if (video_is_registered(&radio->vdev) && cmd != AMRADIO_GET_READY_FLAG)
+		amradio_dev_warn(&radio->vdev.dev, "follow-up to cmd %02x failed\n", cmd);
+	return retval ? retval : -EIO;
+}
 
-	return retval;
+/* switch on/off the radio. Send 8 bytes to device */
+static int amradio_set_mute(struct amradio_device *radio, bool mute)
+{
+	int ret = amradio_send_cmd(radio,
+			AMRADIO_SET_MUTE, mute, NULL, 0, false);
+
+	if (!ret)
+		radio->muted = mute;
+	return ret;
 }
 
 /* set a frequency, freq is defined by v4l's TUNER_LOW, i.e. 1/16th kHz */
-static int amradio_setfreq(struct amradio_device *radio, int freq)
+static int amradio_set_freq(struct amradio_device *radio, int freq)
 {
-	int retval;
-	int size;
 	unsigned short freq_send = 0x10 + (freq >> 3) / 25;
-
-	radio->buffer[0] = 0x00;
-	radio->buffer[1] = 0x55;
-	radio->buffer[2] = 0xaa;
-	radio->buffer[3] = 0x03;
-	radio->buffer[4] = AMRADIO_SET_FREQ;
-	radio->buffer[5] = 0x00;
-	radio->buffer[6] = 0x00;
-	radio->buffer[7] = 0x08;
-
-	retval = usb_bulk_msg(radio->usbdev, usb_sndintpipe(radio->usbdev, 2),
-		(void *) (radio->buffer), BUFFER_LENGTH, &size, USB_TIMEOUT);
-
-	if (retval < 0 || size != BUFFER_LENGTH)
-		goto out_err;
+	u8 buf[3];
+	int retval;
 
 	/* frequency is calculated from freq_send and placed in first 2 bytes */
-	radio->buffer[0] = (freq_send >> 8) & 0xff;
-	radio->buffer[1] = freq_send & 0xff;
-	radio->buffer[2] = 0x01;
-	radio->buffer[3] = 0x00;
-	radio->buffer[4] = 0x00;
-	/* 5 and 6 bytes of buffer already = 0x00 */
-	radio->buffer[7] = 0x00;
-
-	retval = usb_bulk_msg(radio->usbdev, usb_sndintpipe(radio->usbdev, 2),
-		(void *) (radio->buffer), BUFFER_LENGTH, &size, USB_TIMEOUT);
-
-	if (retval < 0 || size != BUFFER_LENGTH)
-		goto out_err;
+	buf[0] = (freq_send >> 8) & 0xff;
+	buf[1] = freq_send & 0xff;
+	buf[2] = 0x01;
 
+	retval = amradio_send_cmd(radio, AMRADIO_SET_FREQ, 0, buf, 3, false);
+	if (retval)
+		return retval;
 	radio->curfreq = freq;
-	goto out;
-
-out_err:
-	amradio_dev_warn(&radio->videodev.dev, "set frequency failed\n");
-out:
-	return retval;
+	msleep(40);
+	return 0;
 }
 
-static int amradio_set_stereo(struct amradio_device *radio, char argument)
+static int amradio_set_stereo(struct amradio_device *radio, bool stereo)
 {
-	int retval;
-	int size;
+	int ret = amradio_send_cmd(radio,
+			AMRADIO_SET_MONO, !stereo, NULL, 0, false);
 
-	radio->buffer[0] = 0x00;
-	radio->buffer[1] = 0x55;
-	radio->buffer[2] = 0xaa;
-	radio->buffer[3] = 0x00;
-	radio->buffer[4] = AMRADIO_SET_MONO;
-	radio->buffer[5] = argument;
-	radio->buffer[6] = 0x00;
-	radio->buffer[7] = 0x00;
-
-	retval = usb_bulk_msg(radio->usbdev, usb_sndintpipe(radio->usbdev, 2),
-		(void *) (radio->buffer), BUFFER_LENGTH, &size, USB_TIMEOUT);
-
-	if (retval < 0 || size != BUFFER_LENGTH) {
-		amradio_dev_warn(&radio->videodev.dev, "set stereo failed\n");
-		return retval;
-	}
+	if (!ret)
+		radio->stereo = stereo;
+	return ret;
+}
 
-	if (argument == WANT_STEREO)
-		radio->stereo = 1;
-	else
-		radio->stereo = 0;
+static int amradio_get_stat(struct amradio_device *radio, bool *is_stereo, u32 *signal)
+{
+	int ret = amradio_send_cmd(radio,
+			AMRADIO_GET_SIGNAL, 0, NULL, 0, true);
 
-	return retval;
+	if (ret)
+		return ret;
+	*is_stereo = radio->buffer[2] >> 7;
+	*signal = (radio->buffer[3] & 0xf0) << 8;
+	return 0;
 }
 
 /* Handle unplugging the device.
@@ -282,25 +252,26 @@ static void usb_amradio_disconnect(struct usb_interface *intf)
 	struct amradio_device *radio = to_amradio_dev(usb_get_intfdata(intf));
 
 	mutex_lock(&radio->lock);
-	/* increase the device node's refcount */
-	get_device(&radio->videodev.dev);
+	video_unregister_device(&radio->vdev);
+	amradio_set_mute(radio, true);
+	usb_set_intfdata(intf, NULL);
 	v4l2_device_disconnect(&radio->v4l2_dev);
-	video_unregister_device(&radio->videodev);
 	mutex_unlock(&radio->lock);
-	/* decrease the device node's refcount, allowing it to be released */
-	put_device(&radio->videodev.dev);
+	v4l2_device_put(&radio->v4l2_dev);
 }
 
 /* vidioc_querycap - query device capabilities */
 static int vidioc_querycap(struct file *file, void *priv,
 					struct v4l2_capability *v)
 {
-	struct amradio_device *radio = file->private_data;
+	struct amradio_device *radio = video_drvdata(file);
 
 	strlcpy(v->driver, "radio-mr800", sizeof(v->driver));
 	strlcpy(v->card, "AverMedia MR 800 USB FM Radio", sizeof(v->card));
 	usb_make_path(radio->usbdev, v->bus_info, sizeof(v->bus_info));
-	v->capabilities = V4L2_CAP_TUNER;
+	v->device_caps = V4L2_CAP_RADIO | V4L2_CAP_TUNER |
+					V4L2_CAP_HW_FREQ_SEEK;
+	v->capabilities = v->device_caps | V4L2_CAP_DEVICE_CAPS;
 	return 0;
 }
 
@@ -308,44 +279,34 @@ static int vidioc_querycap(struct file *file, void *priv,
 static int vidioc_g_tuner(struct file *file, void *priv,
 				struct v4l2_tuner *v)
 {
-	struct amradio_device *radio = file->private_data;
+	struct amradio_device *radio = video_drvdata(file);
+	bool is_stereo = false;
 	int retval;
 
 	if (v->index > 0)
 		return -EINVAL;
 
-/* TODO: Add function which look is signal stereo or not
- * 	amradio_getstat(radio);
- */
-
-/* we call amradio_set_stereo to set radio->stereo
- * Honestly, amradio_getstat should cover this in future and
- * amradio_set_stereo shouldn't be here
- */
-	retval = amradio_set_stereo(radio, WANT_STEREO);
+	v->signal = 0;
+	retval = amradio_get_stat(radio, &is_stereo, &v->signal);
+	if (retval)
+		return retval;
 
 	strcpy(v->name, "FM");
 	v->type = V4L2_TUNER_RADIO;
 	v->rangelow = FREQ_MIN * FREQ_MUL;
 	v->rangehigh = FREQ_MAX * FREQ_MUL;
-	v->rxsubchans = V4L2_TUNER_SUB_MONO | V4L2_TUNER_SUB_STEREO;
-	v->capability = V4L2_TUNER_CAP_LOW;
-	if (radio->stereo)
-		v->audmode = V4L2_TUNER_MODE_STEREO;
-	else
-		v->audmode = V4L2_TUNER_MODE_MONO;
-	v->signal = 0xffff;     /* Can't get the signal strength, sad.. */
-	v->afc = 0; /* Don't know what is this */
-
-	return retval;
+	v->capability = V4L2_TUNER_CAP_LOW | V4L2_TUNER_CAP_STEREO;
+	v->rxsubchans = is_stereo ? V4L2_TUNER_SUB_STEREO : V4L2_TUNER_SUB_MONO;
+	v->audmode = radio->stereo ?
+		V4L2_TUNER_MODE_STEREO : V4L2_TUNER_MODE_MONO;
+	return 0;
 }
 
 /* vidioc_s_tuner - set tuner attributes */
 static int vidioc_s_tuner(struct file *file, void *priv,
 				struct v4l2_tuner *v)
 {
-	struct amradio_device *radio = file->private_data;
-	int retval = -EINVAL;
+	struct amradio_device *radio = video_drvdata(file);
 
 	if (v->index > 0)
 		return -EINVAL;
@@ -353,34 +314,31 @@ static int vidioc_s_tuner(struct file *file, void *priv,
 	/* mono/stereo selector */
 	switch (v->audmode) {
 	case V4L2_TUNER_MODE_MONO:
-		retval = amradio_set_stereo(radio, WANT_MONO);
-		break;
-	case V4L2_TUNER_MODE_STEREO:
-		retval = amradio_set_stereo(radio, WANT_STEREO);
-		break;
+		return amradio_set_stereo(radio, WANT_MONO);
+	default:
+		return amradio_set_stereo(radio, WANT_STEREO);
 	}
-
-	return retval;
 }
 
 /* vidioc_s_frequency - set tuner radio frequency */
 static int vidioc_s_frequency(struct file *file, void *priv,
 				struct v4l2_frequency *f)
 {
-	struct amradio_device *radio = file->private_data;
+	struct amradio_device *radio = video_drvdata(file);
 
-	if (f->tuner != 0 || f->type != V4L2_TUNER_RADIO)
+	if (f->tuner != 0)
 		return -EINVAL;
-	return amradio_setfreq(radio, f->frequency);
+	return amradio_set_freq(radio, clamp_t(unsigned, f->frequency,
+				FREQ_MIN * FREQ_MUL, FREQ_MAX * FREQ_MUL));
 }
 
 /* vidioc_g_frequency - get tuner radio frequency */
 static int vidioc_g_frequency(struct file *file, void *priv,
 				struct v4l2_frequency *f)
 {
-	struct amradio_device *radio = file->private_data;
+	struct amradio_device *radio = video_drvdata(file);
 
-	if (f->tuner != 0)
+	if (f->tuner != 0 || f->type != V4L2_TUNER_RADIO)
 		return -EINVAL;
 	f->type = V4L2_TUNER_RADIO;
 	f->frequency = radio->curfreq;
@@ -388,148 +346,101 @@ static int vidioc_g_frequency(struct file *file, void *priv,
 	return 0;
 }
 
-/* vidioc_queryctrl - enumerate control items */
-static int vidioc_queryctrl(struct file *file, void *priv,
-				struct v4l2_queryctrl *qc)
+static int vidioc_s_hw_freq_seek(struct file *file, void *priv,
+		struct v4l2_hw_freq_seek *seek)
 {
-	switch (qc->id) {
-	case V4L2_CID_AUDIO_MUTE:
-		return v4l2_ctrl_query_fill(qc, 0, 1, 1, 1);
-	}
-
-	return -EINVAL;
-}
+	static u8 buf[8] = {
+		0x3d, 0x32, 0x0f, 0x08, 0x3d, 0x32, 0x0f, 0x08
+	};
+	struct amradio_device *radio = video_drvdata(file);
+	unsigned long timeout;
+	int retval;
 
-/* vidioc_g_ctrl - get the value of a control */
-static int vidioc_g_ctrl(struct file *file, void *priv,
-				struct v4l2_control *ctrl)
-{
-	struct amradio_device *radio = file->private_data;
+	if (seek->tuner != 0 || !seek->wrap_around)
+		return -EINVAL;
 
-	switch (ctrl->id) {
-	case V4L2_CID_AUDIO_MUTE:
-		ctrl->value = radio->muted;
-		return 0;
+	retval = amradio_send_cmd(radio,
+			AMRADIO_SET_SEARCH_LVL, 0, buf, 8, false);
+	if (retval)
+		return retval;
+	amradio_set_freq(radio, radio->curfreq);
+	retval = amradio_send_cmd(radio,
+		seek->seek_upward ? AMRADIO_SET_SEARCH_UP : AMRADIO_SET_SEARCH_DOWN,
+		0, NULL, 0, false);
+	if (retval)
+		return retval;
+	timeout = jiffies + msecs_to_jiffies(30000);
+	for (;;) {
+		if (time_after(jiffies, timeout)) {
+			retval = -EAGAIN;
+			break;
+		}
+		if (schedule_timeout_interruptible(msecs_to_jiffies(10))) {
+			retval = -ERESTARTSYS;
+			break;
+		}
+		retval = amradio_send_cmd(radio, AMRADIO_GET_READY_FLAG,
+				0, NULL, 0, true);
+		if (retval)
+			continue;
+		amradio_send_cmd(radio, AMRADIO_GET_FREQ, 0, NULL, 0, true);
+		if (radio->buffer[1] || radio->buffer[2]) {
+			radio->curfreq = (radio->buffer[1] << 8) | radio->buffer[2];
+			radio->curfreq = (radio->curfreq - 0x10) * 200;
+			amradio_send_cmd(radio, AMRADIO_STOP_SEARCH,
+					0, NULL, 0, false);
+			amradio_set_freq(radio, radio->curfreq);
+			retval = 0;
+			break;
+		}
 	}
-
-	return -EINVAL;
+	amradio_send_cmd(radio, AMRADIO_STOP_SEARCH, 0, NULL, 0, false);
+	amradio_set_freq(radio, radio->curfreq);
+	return retval;
 }
 
-/* vidioc_s_ctrl - set the value of a control */
-static int vidioc_s_ctrl(struct file *file, void *priv,
-				struct v4l2_control *ctrl)
+static int usb_amradio_s_ctrl(struct v4l2_ctrl *ctrl)
 {
-	struct amradio_device *radio = file->private_data;
-	int retval = -EINVAL;
+	struct amradio_device *radio =
+		container_of(ctrl->handler, struct amradio_device, hdl);
 
 	switch (ctrl->id) {
 	case V4L2_CID_AUDIO_MUTE:
-		if (ctrl->value)
-			retval = amradio_set_mute(radio, AMRADIO_STOP);
-		else
-			retval = amradio_set_mute(radio, AMRADIO_START);
-
-		break;
+		return amradio_set_mute(radio, ctrl->val);
 	}
 
-	return retval;
-}
-
-/* vidioc_g_audio - get audio attributes */
-static int vidioc_g_audio(struct file *file, void *priv,
-				struct v4l2_audio *a)
-{
-	if (a->index > 1)
-		return -EINVAL;
-
-	strcpy(a->name, "Radio");
-	a->capability = V4L2_AUDCAP_STEREO;
-	return 0;
-}
-
-/* vidioc_s_audio - set audio attributes  */
-static int vidioc_s_audio(struct file *file, void *priv,
-					struct v4l2_audio *a)
-{
-	if (a->index != 0)
-		return -EINVAL;
-	return 0;
-}
-
-/* vidioc_g_input - get input */
-static int vidioc_g_input(struct file *filp, void *priv, unsigned int *i)
-{
-	*i = 0;
-	return 0;
-}
-
-/* vidioc_s_input - set input */
-static int vidioc_s_input(struct file *filp, void *priv, unsigned int i)
-{
-	if (i != 0)
-		return -EINVAL;
-	return 0;
+	return -EINVAL;
 }
 
 static int usb_amradio_init(struct amradio_device *radio)
 {
 	int retval;
 
-	retval = amradio_set_mute(radio, AMRADIO_STOP);
+	retval = amradio_set_mute(radio, true);
 	if (retval)
 		goto out_err;
-
-	retval = amradio_set_stereo(radio, WANT_STEREO);
+	retval = amradio_set_stereo(radio, true);
 	if (retval)
 		goto out_err;
-
-	radio->initialized = 1;
-	goto out;
-
-out_err:
-	amradio_dev_err(&radio->videodev.dev, "initialization failed\n");
-out:
-	return retval;
-}
-
-/* open device - amradio_start() and amradio_setfreq() */
-static int usb_amradio_open(struct file *file)
-{
-	struct amradio_device *radio = video_drvdata(file);
-	int retval;
-
-	file->private_data = radio;
-	retval = usb_autopm_get_interface(radio->intf);
+	retval = amradio_set_freq(radio, radio->curfreq);
 	if (retval)
-		return retval;
+		goto out_err;
+	return 0;
 
-	if (unlikely(!radio->initialized)) {
-		retval = usb_amradio_init(radio);
-		if (retval)
-			usb_autopm_put_interface(radio->intf);
-	}
+out_err:
+	amradio_dev_err(&radio->vdev.dev, "initialization failed\n");
 	return retval;
 }
 
-/*close device */
-static int usb_amradio_close(struct file *file)
-{
-	struct amradio_device *radio = file->private_data;
-
-	if (video_is_registered(&radio->videodev))
-		usb_autopm_put_interface(radio->intf);
-	return 0;
-}
-
 /* Suspend device - stop device. Need to be checked and fixed */
 static int usb_amradio_suspend(struct usb_interface *intf, pm_message_t message)
 {
 	struct amradio_device *radio = to_amradio_dev(usb_get_intfdata(intf));
 
 	mutex_lock(&radio->lock);
-	if (!radio->muted && radio->initialized) {
-		amradio_set_mute(radio, AMRADIO_STOP);
-		radio->muted = 0;
+	if (!radio->muted) {
+		amradio_set_mute(radio, true);
+		radio->muted = false;
 	}
 	mutex_unlock(&radio->lock);
 
@@ -543,31 +454,28 @@ static int usb_amradio_resume(struct usb_interface *intf)
 	struct amradio_device *radio = to_amradio_dev(usb_get_intfdata(intf));
 
 	mutex_lock(&radio->lock);
-	if (unlikely(!radio->initialized))
-		goto unlock;
-
-	if (radio->stereo)
-		amradio_set_stereo(radio, WANT_STEREO);
-	else
-		amradio_set_stereo(radio, WANT_MONO);
-
-	amradio_setfreq(radio, radio->curfreq);
+	amradio_set_stereo(radio, radio->stereo);
+	amradio_set_freq(radio, radio->curfreq);
 
 	if (!radio->muted)
-		amradio_set_mute(radio, AMRADIO_START);
+		amradio_set_mute(radio, false);
 
-unlock:
 	mutex_unlock(&radio->lock);
 
 	dev_info(&intf->dev, "coming out of suspend..\n");
 	return 0;
 }
 
+static const struct v4l2_ctrl_ops usb_amradio_ctrl_ops = {
+	.s_ctrl = usb_amradio_s_ctrl,
+};
+
 /* File system interface */
 static const struct v4l2_file_operations usb_amradio_fops = {
 	.owner		= THIS_MODULE,
-	.open		= usb_amradio_open,
-	.release	= usb_amradio_close,
+	.open		= v4l2_fh_open,
+	.release	= v4l2_fh_release,
+	.poll		= v4l2_ctrl_poll,
 	.unlocked_ioctl	= video_ioctl2,
 };
 
@@ -577,20 +485,19 @@ static const struct v4l2_ioctl_ops usb_amradio_ioctl_ops = {
 	.vidioc_s_tuner     = vidioc_s_tuner,
 	.vidioc_g_frequency = vidioc_g_frequency,
 	.vidioc_s_frequency = vidioc_s_frequency,
-	.vidioc_queryctrl   = vidioc_queryctrl,
-	.vidioc_g_ctrl      = vidioc_g_ctrl,
-	.vidioc_s_ctrl      = vidioc_s_ctrl,
-	.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_s_hw_freq_seek = vidioc_s_hw_freq_seek,
+	.vidioc_log_status  = v4l2_ctrl_log_status,
+	.vidioc_subscribe_event = v4l2_ctrl_subscribe_event,
+	.vidioc_unsubscribe_event = v4l2_event_unsubscribe,
 };
 
-static void usb_amradio_video_device_release(struct video_device *videodev)
+static void usb_amradio_release(struct v4l2_device *v4l2_dev)
 {
-	struct amradio_device *radio = video_get_drvdata(videodev);
+	struct amradio_device *radio = to_amradio_dev(v4l2_dev);
 
 	/* free rest memory */
+	v4l2_ctrl_handler_free(&radio->hdl);
+	v4l2_device_unregister(&radio->v4l2_dev);
 	kfree(radio->buffer);
 	kfree(radio);
 }
@@ -624,23 +531,38 @@ static int usb_amradio_probe(struct usb_interface *intf,
 		goto err_v4l2;
 	}
 
+	v4l2_ctrl_handler_init(&radio->hdl, 1);
+	v4l2_ctrl_new_std(&radio->hdl, &usb_amradio_ctrl_ops,
+			  V4L2_CID_AUDIO_MUTE, 0, 1, 1, 1);
+	if (radio->hdl.error) {
+		retval = radio->hdl.error;
+		dev_err(&intf->dev, "couldn't register control\n");
+		goto err_ctrl;
+	}
 	mutex_init(&radio->lock);
 
-	strlcpy(radio->videodev.name, radio->v4l2_dev.name,
-		sizeof(radio->videodev.name));
-	radio->videodev.v4l2_dev = &radio->v4l2_dev;
-	radio->videodev.fops = &usb_amradio_fops;
-	radio->videodev.ioctl_ops = &usb_amradio_ioctl_ops;
-	radio->videodev.release = usb_amradio_video_device_release;
-	radio->videodev.lock = &radio->lock;
+	radio->v4l2_dev.ctrl_handler = &radio->hdl;
+	radio->v4l2_dev.release = usb_amradio_release;
+	strlcpy(radio->vdev.name, radio->v4l2_dev.name,
+		sizeof(radio->vdev.name));
+	radio->vdev.v4l2_dev = &radio->v4l2_dev;
+	radio->vdev.fops = &usb_amradio_fops;
+	radio->vdev.ioctl_ops = &usb_amradio_ioctl_ops;
+	radio->vdev.release = video_device_release_empty;
+	radio->vdev.lock = &radio->lock;
+	set_bit(V4L2_FL_USE_FH_PRIO, &radio->vdev.flags);
 
 	radio->usbdev = interface_to_usbdev(intf);
 	radio->intf = intf;
+	usb_set_intfdata(intf, &radio->v4l2_dev);
 	radio->curfreq = 95.16 * FREQ_MUL;
 
-	video_set_drvdata(&radio->videodev, radio);
+	video_set_drvdata(&radio->vdev, radio);
+	retval = usb_amradio_init(radio);
+	if (retval)
+		goto err_vdev;
 
-	retval = video_register_device(&radio->videodev, VFL_TYPE_RADIO,
+	retval = video_register_device(&radio->vdev, VFL_TYPE_RADIO,
 					radio_nr);
 	if (retval < 0) {
 		dev_err(&intf->dev, "could not register video device\n");
@@ -650,6 +572,8 @@ static int usb_amradio_probe(struct usb_interface *intf,
 	return 0;
 
 err_vdev:
+	v4l2_ctrl_handler_free(&radio->hdl);
+err_ctrl:
 	v4l2_device_unregister(&radio->v4l2_dev);
 err_v4l2:
 	kfree(radio->buffer);
@@ -659,4 +583,24 @@ err:
 	return retval;
 }
 
+/* USB Device ID List */
+static struct usb_device_id usb_amradio_device_table[] = {
+	{ USB_DEVICE_AND_INTERFACE_INFO(USB_AMRADIO_VENDOR, USB_AMRADIO_PRODUCT,
+							USB_CLASS_HID, 0, 0) },
+	{ }						/* Terminating entry */
+};
+
+MODULE_DEVICE_TABLE(usb, usb_amradio_device_table);
+
+/* USB subsystem interface */
+static struct usb_driver usb_amradio_driver = {
+	.name			= MR800_DRIVER_NAME,
+	.probe			= usb_amradio_probe,
+	.disconnect		= usb_amradio_disconnect,
+	.suspend		= usb_amradio_suspend,
+	.resume			= usb_amradio_resume,
+	.reset_resume		= usb_amradio_resume,
+	.id_table		= usb_amradio_device_table,
+};
+
 module_usb_driver(usb_amradio_driver);
diff --git a/drivers/media/radio/radio-rtrack2.c b/drivers/media/radio/radio-rtrack2.c
index b275c5d0fe9..b1f844c64fd 100644
--- a/drivers/media/radio/radio-rtrack2.c
+++ b/drivers/media/radio/radio-rtrack2.c
@@ -17,6 +17,7 @@
 #include <linux/videodev2.h>	/* kernel radio structs		*/
 #include <linux/mutex.h>
 #include <linux/io.h>		/* outb, outb_p			*/
+#include <linux/slab.h>
 #include <media/v4l2-device.h>
 #include <media/v4l2-ioctl.h>
 #include "radio-isa.h"
diff --git a/drivers/media/radio/radio-sf16fmi.c b/drivers/media/radio/radio-sf16fmi.c
index 22c5743bf9d..a81d723b8c7 100644
--- a/drivers/media/radio/radio-sf16fmi.c
+++ b/drivers/media/radio/radio-sf16fmi.c
@@ -1,4 +1,4 @@
-/* SF16-FMI and SF16-FMP radio driver for Linux radio support
+/* SF16-FMI, SF16-FMP and SF16-FMD radio driver for Linux radio support
  * heavily based on rtrack driver...
  * (c) 1997 M. Kirkwood
  * (c) 1998 Petr Vandrovec, vandrove@vc.cvut.cz
@@ -11,7 +11,7 @@
  *
  *  Frequency control is done digitally -- ie out(port,encodefreq(95.8));
  *  No volume control - only mute/unmute - you have to use line volume
- *  control on SB-part of SF16-FMI/SF16-FMP
+ *  control on SB-part of SF16-FMI/SF16-FMP/SF16-FMD
  *
  * Converted to V4L2 API by Mauro Carvalho Chehab <mchehab@infradead.org>
  */
@@ -29,7 +29,7 @@
 #include <media/v4l2-ioctl.h>
 
 MODULE_AUTHOR("Petr Vandrovec, vandrove@vc.cvut.cz and M. Kirkwood");
-MODULE_DESCRIPTION("A driver for the SF16-FMI and SF16-FMP radio.");
+MODULE_DESCRIPTION("A driver for the SF16-FMI, SF16-FMP and SF16-FMD radio.");
 MODULE_LICENSE("GPL");
 MODULE_VERSION("0.0.3");
 
@@ -37,7 +37,7 @@ static int io = -1;
 static int radio_nr = -1;
 
 module_param(io, int, 0);
-MODULE_PARM_DESC(io, "I/O address of the SF16-FMI or SF16-FMP card (0x284 or 0x384)");
+MODULE_PARM_DESC(io, "I/O address of the SF16-FMI/SF16-FMP/SF16-FMD card (0x284 or 0x384)");
 module_param(radio_nr, int, 0);
 
 struct fmi
@@ -130,7 +130,7 @@ static int vidioc_querycap(struct file *file, void  *priv,
 					struct v4l2_capability *v)
 {
 	strlcpy(v->driver, "radio-sf16fmi", sizeof(v->driver));
-	strlcpy(v->card, "SF16-FMx radio", sizeof(v->card));
+	strlcpy(v->card, "SF16-FMI/FMP/FMD radio", sizeof(v->card));
 	strlcpy(v->bus_info, "ISA", sizeof(v->bus_info));
 	v->capabilities = V4L2_CAP_TUNER | V4L2_CAP_RADIO;
 	return 0;
@@ -277,8 +277,12 @@ static const struct v4l2_ioctl_ops fmi_ioctl_ops = {
 
 /* ladis: this is my card. does any other types exist? */
 static struct isapnp_device_id id_table[] __devinitdata = {
+		/* SF16-FMI */
 	{	ISAPNP_ANY_ID, ISAPNP_ANY_ID,
 		ISAPNP_VENDOR('M','F','R'), ISAPNP_FUNCTION(0xad10), 0},
+		/* SF16-FMD */
+	{	ISAPNP_ANY_ID, ISAPNP_ANY_ID,
+		ISAPNP_VENDOR('M','F','R'), ISAPNP_FUNCTION(0xad12), 0},
 	{	ISAPNP_CARD_END, },
 };
 
diff --git a/drivers/media/radio/radio-sf16fmr2.c b/drivers/media/radio/radio-sf16fmr2.c
index 7c69214334b..52b8011f1b2 100644
--- a/drivers/media/radio/radio-sf16fmr2.c
+++ b/drivers/media/radio/radio-sf16fmr2.c
@@ -1,4 +1,4 @@
-/* SF16-FMR2 radio driver for Linux
+/* SF16-FMR2 and SF16-FMD2 radio driver for Linux
  * Copyright (c) 2011 Ondrej Zary
  *
  * Original driver was (c) 2000-2002 Ziglio Frediano, freddy77@angelfire.com
@@ -13,15 +13,19 @@
 #include <linux/ioport.h>	/* request_region		*/
 #include <linux/io.h>		/* outb, outb_p			*/
 #include <linux/isa.h>
+#include <linux/pnp.h>
 #include <sound/tea575x-tuner.h>
 
 MODULE_AUTHOR("Ondrej Zary");
-MODULE_DESCRIPTION("MediaForte SF16-FMR2 FM radio card driver");
+MODULE_DESCRIPTION("MediaForte SF16-FMR2 and SF16-FMD2 FM radio card driver");
 MODULE_LICENSE("GPL");
 
-static int radio_nr = -1;
-module_param(radio_nr, int, 0444);
-MODULE_PARM_DESC(radio_nr, "Radio device number");
+/* these cards can only use two different ports (0x384 and 0x284) */
+#define FMR2_MAX 2
+
+static int radio_nr[FMR2_MAX] = { [0 ... (FMR2_MAX - 1)] = -1 };
+module_param_array(radio_nr, int, NULL, 0444);
+MODULE_PARM_DESC(radio_nr, "Radio device numbers");
 
 struct fmr2 {
 	int io;
@@ -29,9 +33,15 @@ struct fmr2 {
 	struct snd_tea575x tea;
 	struct v4l2_ctrl *volume;
 	struct v4l2_ctrl *balance;
+	bool is_fmd2;
 };
 
-/* the port is hardwired so no need to support multiple cards */
+static int num_fmr2_cards;
+static struct fmr2 *fmr2_cards[FMR2_MAX];
+static bool isa_registered;
+static bool pnp_registered;
+
+/* the port is hardwired on SF16-FMR2 */
 #define FMR2_PORT	0x384
 
 /* TEA575x tuner pins */
@@ -174,7 +184,8 @@ static int fmr2_tea_ext_init(struct snd_tea575x *tea)
 {
 	struct fmr2 *fmr2 = tea->private_data;
 
-	if (inb(fmr2->io) & FMR2_HASVOL) {
+	/* FMR2 can have volume control, FMD2 can't (uses SB16 mixer) */
+	if (!fmr2->is_fmd2 && inb(fmr2->io) & FMR2_HASVOL) {
 		fmr2->volume = v4l2_ctrl_new_std(&tea->ctrl_handler, &fmr2_ctrl_ops, V4L2_CID_AUDIO_VOLUME, 0, 68, 2, 56);
 		fmr2->balance = v4l2_ctrl_new_std(&tea->ctrl_handler, &fmr2_ctrl_ops, V4L2_CID_AUDIO_BALANCE, -68, 68, 2, 0);
 		if (tea->ctrl_handler.error) {
@@ -186,22 +197,28 @@ static int fmr2_tea_ext_init(struct snd_tea575x *tea)
 	return 0;
 }
 
-static int __devinit fmr2_probe(struct device *pdev, unsigned int dev)
+static struct pnp_device_id fmr2_pnp_ids[] __devinitdata = {
+	{ .id = "MFRad13" }, /* tuner subdevice of SF16-FMD2 */
+	{ .id = "" }
+};
+MODULE_DEVICE_TABLE(pnp, fmr2_pnp_ids);
+
+static int __devinit fmr2_probe(struct fmr2 *fmr2, struct device *pdev, int io)
 {
-	struct fmr2 *fmr2;
-	int err;
+	int err, i;
+	char *card_name = fmr2->is_fmd2 ? "SF16-FMD2" : "SF16-FMR2";
 
-	fmr2 = kzalloc(sizeof(*fmr2), GFP_KERNEL);
-	if (fmr2 == NULL)
-		return -ENOMEM;
+	/* avoid errors if a card was already registered at given port */
+	for (i = 0; i < num_fmr2_cards; i++)
+		if (io == fmr2_cards[i]->io)
+			return -EBUSY;
 
-	strlcpy(fmr2->v4l2_dev.name, dev_name(pdev),
-			sizeof(fmr2->v4l2_dev.name));
-	fmr2->io = FMR2_PORT;
+	strlcpy(fmr2->v4l2_dev.name, "radio-sf16fmr2",
+			sizeof(fmr2->v4l2_dev.name)),
+	fmr2->io = io;
 
 	if (!request_region(fmr2->io, 2, fmr2->v4l2_dev.name)) {
 		printk(KERN_ERR "radio-sf16fmr2: I/O port 0x%x already in use\n", fmr2->io);
-		kfree(fmr2);
 		return -EBUSY;
 	}
 
@@ -210,56 +227,121 @@ static int __devinit fmr2_probe(struct device *pdev, unsigned int dev)
 	if (err < 0) {
 		v4l2_err(&fmr2->v4l2_dev, "Could not register v4l2_device\n");
 		release_region(fmr2->io, 2);
-		kfree(fmr2);
 		return err;
 	}
 	fmr2->tea.v4l2_dev = &fmr2->v4l2_dev;
 	fmr2->tea.private_data = fmr2;
-	fmr2->tea.radio_nr = radio_nr;
+	fmr2->tea.radio_nr = radio_nr[num_fmr2_cards];
 	fmr2->tea.ops = &fmr2_tea_ops;
 	fmr2->tea.ext_init = fmr2_tea_ext_init;
-	strlcpy(fmr2->tea.card, "SF16-FMR2", sizeof(fmr2->tea.card));
-	snprintf(fmr2->tea.bus_info, sizeof(fmr2->tea.bus_info), "ISA:%s",
-			fmr2->v4l2_dev.name);
+	strlcpy(fmr2->tea.card, card_name, sizeof(fmr2->tea.card));
+	snprintf(fmr2->tea.bus_info, sizeof(fmr2->tea.bus_info), "%s:%s",
+			fmr2->is_fmd2 ? "PnP" : "ISA", dev_name(pdev));
 
 	if (snd_tea575x_init(&fmr2->tea)) {
 		printk(KERN_ERR "radio-sf16fmr2: Unable to detect TEA575x tuner\n");
 		release_region(fmr2->io, 2);
-		kfree(fmr2);
 		return -ENODEV;
 	}
 
-	printk(KERN_INFO "radio-sf16fmr2: SF16-FMR2 radio card at 0x%x.\n", fmr2->io);
+	printk(KERN_INFO "radio-sf16fmr2: %s radio card at 0x%x.\n",
+			card_name, fmr2->io);
 	return 0;
 }
 
-static int __exit fmr2_remove(struct device *pdev, unsigned int dev)
+static int __devinit fmr2_isa_match(struct device *pdev, unsigned int ndev)
+{
+	struct fmr2 *fmr2 = kzalloc(sizeof(*fmr2), GFP_KERNEL);
+	if (!fmr2)
+		return 0;
+
+	if (fmr2_probe(fmr2, pdev, FMR2_PORT)) {
+		kfree(fmr2);
+		return 0;
+	}
+	dev_set_drvdata(pdev, fmr2);
+	fmr2_cards[num_fmr2_cards++] = fmr2;
+
+	return 1;
+}
+
+static int __devinit fmr2_pnp_probe(struct pnp_dev *pdev,
+				const struct pnp_device_id *id)
 {
-	struct fmr2 *fmr2 = dev_get_drvdata(pdev);
+	int ret;
+	struct fmr2 *fmr2 = kzalloc(sizeof(*fmr2), GFP_KERNEL);
+	if (!fmr2)
+		return -ENOMEM;
 
+	fmr2->is_fmd2 = true;
+	ret = fmr2_probe(fmr2, &pdev->dev, pnp_port_start(pdev, 0));
+	if (ret) {
+		kfree(fmr2);
+		return ret;
+	}
+	pnp_set_drvdata(pdev, fmr2);
+	fmr2_cards[num_fmr2_cards++] = fmr2;
+
+	return 0;
+}
+
+static void __devexit fmr2_remove(struct fmr2 *fmr2)
+{
 	snd_tea575x_exit(&fmr2->tea);
 	release_region(fmr2->io, 2);
 	v4l2_device_unregister(&fmr2->v4l2_dev);
 	kfree(fmr2);
+}
+
+static int __devexit fmr2_isa_remove(struct device *pdev, unsigned int ndev)
+{
+	fmr2_remove(dev_get_drvdata(pdev));
+	dev_set_drvdata(pdev, NULL);
+
 	return 0;
 }
 
-struct isa_driver fmr2_driver = {
-	.probe		= fmr2_probe,
-	.remove		= fmr2_remove,
+static void __devexit fmr2_pnp_remove(struct pnp_dev *pdev)
+{
+	fmr2_remove(pnp_get_drvdata(pdev));
+	pnp_set_drvdata(pdev, NULL);
+}
+
+struct isa_driver fmr2_isa_driver = {
+	.match		= fmr2_isa_match,
+	.remove		= __devexit_p(fmr2_isa_remove),
 	.driver		= {
 		.name	= "radio-sf16fmr2",
 	},
 };
 
+struct pnp_driver fmr2_pnp_driver = {
+	.name		= "radio-sf16fmr2",
+	.id_table	= fmr2_pnp_ids,
+	.probe		= fmr2_pnp_probe,
+	.remove		= __devexit_p(fmr2_pnp_remove),
+};
+
 static int __init fmr2_init(void)
 {
-	return isa_register_driver(&fmr2_driver, 1);
+	int ret;
+
+	ret = pnp_register_driver(&fmr2_pnp_driver);
+	if (!ret)
+		pnp_registered = true;
+	ret = isa_register_driver(&fmr2_isa_driver, 1);
+	if (!ret)
+		isa_registered = true;
+
+	return (pnp_registered || isa_registered) ? 0 : ret;
 }
 
 static void __exit fmr2_exit(void)
 {
-	isa_unregister_driver(&fmr2_driver);
+	if (pnp_registered)
+		pnp_unregister_driver(&fmr2_pnp_driver);
+	if (isa_registered)
+		isa_unregister_driver(&fmr2_isa_driver);
 }
 
 module_init(fmr2_init);
diff --git a/drivers/media/radio/radio-timb.c b/drivers/media/radio/radio-timb.c
index 5d9a90ac3a1..7052adc0c0b 100644
--- a/drivers/media/radio/radio-timb.c
+++ b/drivers/media/radio/radio-timb.c
@@ -223,7 +223,7 @@ static struct platform_driver timbradio_platform_driver = {
 		.owner	= THIS_MODULE,
 	},
 	.probe		= timbradio_probe,
-	.remove		= timbradio_remove,
+	.remove		= __devexit_p(timbradio_remove),
 };
 
 module_platform_driver(timbradio_platform_driver);
diff --git a/drivers/media/radio/saa7706h.c b/drivers/media/radio/saa7706h.c
index 9474706350f..bb953ef75f6 100644
--- a/drivers/media/radio/saa7706h.c
+++ b/drivers/media/radio/saa7706h.c
@@ -430,7 +430,7 @@ static struct i2c_driver saa7706h_driver = {
 		.name	= DRIVER_NAME,
 	},
 	.probe		= saa7706h_probe,
-	.remove		= saa7706h_remove,
+	.remove		= __devexit_p(saa7706h_remove),
 	.id_table	= saa7706h_id,
 };
 
diff --git a/drivers/media/radio/si470x/radio-si470x-common.c b/drivers/media/radio/si470x/radio-si470x-common.c
index 0e740c98786..969cf494d85 100644
--- a/drivers/media/radio/si470x/radio-si470x-common.c
+++ b/drivers/media/radio/si470x/radio-si470x-common.c
@@ -196,9 +196,9 @@ static int si470x_set_chan(struct si470x_device *radio, unsigned short chan)
 	}
 
 	if ((radio->registers[STATUSRSSI] & STATUSRSSI_STC) == 0)
-		dev_warn(&radio->videodev->dev, "tune does not complete\n");
+		dev_warn(&radio->videodev.dev, "tune does not complete\n");
 	if (timed_out)
-		dev_warn(&radio->videodev->dev,
+		dev_warn(&radio->videodev.dev,
 			"tune timed out after %u ms\n", tune_timeout);
 
 stop:
@@ -262,7 +262,7 @@ static int si470x_get_freq(struct si470x_device *radio, unsigned int *freq)
  */
 int si470x_set_freq(struct si470x_device *radio, unsigned int freq)
 {
-	unsigned int spacing, band_bottom;
+	unsigned int spacing, band_bottom, band_top;
 	unsigned short chan;
 
 	/* Spacing (kHz) */
@@ -278,19 +278,26 @@ int si470x_set_freq(struct si470x_device *radio, unsigned int freq)
 		spacing = 0.050 * FREQ_MUL; break;
 	};
 
-	/* Bottom of Band (MHz) */
+	/* Bottom/Top of Band (MHz) */
 	switch ((radio->registers[SYSCONFIG2] & SYSCONFIG2_BAND) >> 6) {
 	/* 0: 87.5 - 108 MHz (USA, Europe) */
 	case 0:
-		band_bottom = 87.5 * FREQ_MUL; break;
+		band_bottom = 87.5 * FREQ_MUL;
+		band_top = 108 * FREQ_MUL;
+		break;
 	/* 1: 76   - 108 MHz (Japan wide band) */
 	default:
-		band_bottom = 76   * FREQ_MUL; break;
+		band_bottom = 76 * FREQ_MUL;
+		band_top = 108 * FREQ_MUL;
+		break;
 	/* 2: 76   -  90 MHz (Japan) */
 	case 2:
-		band_bottom = 76   * FREQ_MUL; break;
+		band_bottom = 76 * FREQ_MUL;
+		band_top = 90 * FREQ_MUL;
+		break;
 	};
 
+	freq = clamp(freq, band_bottom, band_top);
 	/* Chan = [ Freq (Mhz) - Bottom of Band (MHz) ] / Spacing (kHz) */
 	chan = (freq - band_bottom) / spacing;
 
@@ -320,7 +327,7 @@ static int si470x_set_seek(struct si470x_device *radio,
 		radio->registers[POWERCFG] &= ~POWERCFG_SEEKUP;
 	retval = si470x_set_register(radio, POWERCFG);
 	if (retval < 0)
-		goto done;
+		return retval;
 
 	/* currently I2C driver only uses interrupt way to seek */
 	if (radio->stci_enabled) {
@@ -344,24 +351,19 @@ static int si470x_set_seek(struct si470x_device *radio,
 	}
 
 	if ((radio->registers[STATUSRSSI] & STATUSRSSI_STC) == 0)
-		dev_warn(&radio->videodev->dev, "seek does not complete\n");
+		dev_warn(&radio->videodev.dev, "seek does not complete\n");
 	if (radio->registers[STATUSRSSI] & STATUSRSSI_SF)
-		dev_warn(&radio->videodev->dev,
+		dev_warn(&radio->videodev.dev,
 			"seek failed / band limit reached\n");
-	if (timed_out)
-		dev_warn(&radio->videodev->dev,
-			"seek timed out after %u ms\n", seek_timeout);
 
 stop:
 	/* stop seeking */
 	radio->registers[POWERCFG] &= ~POWERCFG_SEEK;
 	retval = si470x_set_register(radio, POWERCFG);
 
-done:
 	/* try again, if timed out */
-	if ((retval == 0) && timed_out)
-		retval = -EAGAIN;
-
+	if (retval == 0 && timed_out)
+		return -EAGAIN;
 	return retval;
 }
 
@@ -463,7 +465,6 @@ static ssize_t si470x_fops_read(struct file *file, char __user *buf,
 	unsigned int block_count = 0;
 
 	/* switch on rds reception */
-	mutex_lock(&radio->lock);
 	if ((radio->registers[SYSCONFIG1] & SYSCONFIG1_RDS) == 0)
 		si470x_rds_on(radio);
 
@@ -505,7 +506,6 @@ static ssize_t si470x_fops_read(struct file *file, char __user *buf,
 	}
 
 done:
-	mutex_unlock(&radio->lock);
 	return retval;
 }
 
@@ -517,19 +517,19 @@ static unsigned int si470x_fops_poll(struct file *file,
 		struct poll_table_struct *pts)
 {
 	struct si470x_device *radio = video_drvdata(file);
-	int retval = 0;
+	unsigned long req_events = poll_requested_events(pts);
+	int retval = v4l2_ctrl_poll(file, pts);
 
-	/* switch on rds reception */
-
-	mutex_lock(&radio->lock);
-	if ((radio->registers[SYSCONFIG1] & SYSCONFIG1_RDS) == 0)
-		si470x_rds_on(radio);
-	mutex_unlock(&radio->lock);
+	if (req_events & (POLLIN | POLLRDNORM)) {
+		/* switch on rds reception */
+		if ((radio->registers[SYSCONFIG1] & SYSCONFIG1_RDS) == 0)
+			si470x_rds_on(radio);
 
-	poll_wait(file, &radio->read_queue, pts);
+		poll_wait(file, &radio->read_queue, pts);
 
-	if (radio->rd_index != radio->wr_index)
-		retval = POLLIN | POLLRDNORM;
+		if (radio->rd_index != radio->wr_index)
+			retval |= POLLIN | POLLRDNORM;
+	}
 
 	return retval;
 }
@@ -553,134 +553,26 @@ static const struct v4l2_file_operations si470x_fops = {
  * Video4Linux Interface
  **************************************************************************/
 
-/*
- * si470x_vidioc_queryctrl - enumerate control items
- */
-static int si470x_vidioc_queryctrl(struct file *file, void *priv,
-		struct v4l2_queryctrl *qc)
-{
-	struct si470x_device *radio = video_drvdata(file);
-	int retval = -EINVAL;
-
-	/* abort if qc->id is below V4L2_CID_BASE */
-	if (qc->id < V4L2_CID_BASE)
-		goto done;
-
-	/* search video control */
-	switch (qc->id) {
-	case V4L2_CID_AUDIO_VOLUME:
-		return v4l2_ctrl_query_fill(qc, 0, 15, 1, 15);
-	case V4L2_CID_AUDIO_MUTE:
-		return v4l2_ctrl_query_fill(qc, 0, 1, 1, 1);
-	}
-
-	/* disable unsupported base controls */
-	/* to satisfy kradio and such apps */
-	if ((retval == -EINVAL) && (qc->id < V4L2_CID_LASTP1)) {
-		qc->flags = V4L2_CTRL_FLAG_DISABLED;
-		retval = 0;
-	}
 
-done:
-	if (retval < 0)
-		dev_warn(&radio->videodev->dev,
-			"query controls failed with %d\n", retval);
-	return retval;
-}
-
-
-/*
- * si470x_vidioc_g_ctrl - get the value of a control
- */
-static int si470x_vidioc_g_ctrl(struct file *file, void *priv,
-		struct v4l2_control *ctrl)
-{
-	struct si470x_device *radio = video_drvdata(file);
-	int retval = 0;
-
-	mutex_lock(&radio->lock);
-	/* safety checks */
-	retval = si470x_disconnect_check(radio);
-	if (retval)
-		goto done;
-
-	switch (ctrl->id) {
-	case V4L2_CID_AUDIO_VOLUME:
-		ctrl->value = radio->registers[SYSCONFIG2] &
-				SYSCONFIG2_VOLUME;
-		break;
-	case V4L2_CID_AUDIO_MUTE:
-		ctrl->value = ((radio->registers[POWERCFG] &
-				POWERCFG_DMUTE) == 0) ? 1 : 0;
-		break;
-	default:
-		retval = -EINVAL;
-	}
-
-done:
-	if (retval < 0)
-		dev_warn(&radio->videodev->dev,
-			"get control failed with %d\n", retval);
-
-	mutex_unlock(&radio->lock);
-	return retval;
-}
-
-
-/*
- * si470x_vidioc_s_ctrl - set the value of a control
- */
-static int si470x_vidioc_s_ctrl(struct file *file, void *priv,
-		struct v4l2_control *ctrl)
+static int si470x_s_ctrl(struct v4l2_ctrl *ctrl)
 {
-	struct si470x_device *radio = video_drvdata(file);
-	int retval = 0;
-
-	mutex_lock(&radio->lock);
-	/* safety checks */
-	retval = si470x_disconnect_check(radio);
-	if (retval)
-		goto done;
+	struct si470x_device *radio =
+		container_of(ctrl->handler, struct si470x_device, hdl);
 
 	switch (ctrl->id) {
 	case V4L2_CID_AUDIO_VOLUME:
 		radio->registers[SYSCONFIG2] &= ~SYSCONFIG2_VOLUME;
-		radio->registers[SYSCONFIG2] |= ctrl->value;
-		retval = si470x_set_register(radio, SYSCONFIG2);
-		break;
+		radio->registers[SYSCONFIG2] |= ctrl->val;
+		return si470x_set_register(radio, SYSCONFIG2);
 	case V4L2_CID_AUDIO_MUTE:
-		if (ctrl->value == 1)
+		if (ctrl->val)
 			radio->registers[POWERCFG] &= ~POWERCFG_DMUTE;
 		else
 			radio->registers[POWERCFG] |= POWERCFG_DMUTE;
-		retval = si470x_set_register(radio, POWERCFG);
-		break;
+		return si470x_set_register(radio, POWERCFG);
 	default:
-		retval = -EINVAL;
+		return -EINVAL;
 	}
-
-done:
-	if (retval < 0)
-		dev_warn(&radio->videodev->dev,
-			"set control failed with %d\n", retval);
-	mutex_unlock(&radio->lock);
-	return retval;
-}
-
-
-/*
- * si470x_vidioc_g_audio - get audio attributes
- */
-static int si470x_vidioc_g_audio(struct file *file, void *priv,
-		struct v4l2_audio *audio)
-{
-	/* driver constants */
-	audio->index = 0;
-	strcpy(audio->name, "Radio");
-	audio->capability = V4L2_AUDCAP_STEREO;
-	audio->mode = 0;
-
-	return 0;
 }
 
 
@@ -691,22 +583,14 @@ static int si470x_vidioc_g_tuner(struct file *file, void *priv,
 		struct v4l2_tuner *tuner)
 {
 	struct si470x_device *radio = video_drvdata(file);
-	int retval = 0;
-
-	mutex_lock(&radio->lock);
-	/* safety checks */
-	retval = si470x_disconnect_check(radio);
-	if (retval)
-		goto done;
+	int retval;
 
-	if (tuner->index != 0) {
-		retval = -EINVAL;
-		goto done;
-	}
+	if (tuner->index != 0)
+		return -EINVAL;
 
 	retval = si470x_get_register(radio, STATUSRSSI);
 	if (retval < 0)
-		goto done;
+		return retval;
 
 	/* driver constants */
 	strcpy(tuner->name, "FM");
@@ -737,7 +621,7 @@ static int si470x_vidioc_g_tuner(struct file *file, void *priv,
 	if ((radio->registers[STATUSRSSI] & STATUSRSSI_ST) == 0)
 		tuner->rxsubchans = V4L2_TUNER_SUB_MONO;
 	else
-		tuner->rxsubchans = V4L2_TUNER_SUB_MONO | V4L2_TUNER_SUB_STEREO;
+		tuner->rxsubchans = V4L2_TUNER_SUB_STEREO;
 	/* If there is a reliable method of detecting an RDS channel,
 	   then this code should check for that before setting this
 	   RDS subchannel. */
@@ -754,16 +638,13 @@ static int si470x_vidioc_g_tuner(struct file *file, void *priv,
 	tuner->signal = (radio->registers[STATUSRSSI] & STATUSRSSI_RSSI);
 	/* the ideal factor is 0xffff/75 = 873,8 */
 	tuner->signal = (tuner->signal * 873) + (8 * tuner->signal / 10);
+	if (tuner->signal > 0xffff)
+		tuner->signal = 0xffff;
 
 	/* automatic frequency control: -1: freq to low, 1 freq to high */
 	/* AFCRL does only indicate that freq. differs, not if too low/high */
 	tuner->afc = (radio->registers[STATUSRSSI] & STATUSRSSI_AFCRL) ? 1 : 0;
 
-done:
-	if (retval < 0)
-		dev_warn(&radio->videodev->dev,
-			"get tuner failed with %d\n", retval);
-	mutex_unlock(&radio->lock);
 	return retval;
 }
 
@@ -775,16 +656,9 @@ static int si470x_vidioc_s_tuner(struct file *file, void *priv,
 		struct v4l2_tuner *tuner)
 {
 	struct si470x_device *radio = video_drvdata(file);
-	int retval = 0;
-
-	mutex_lock(&radio->lock);
-	/* safety checks */
-	retval = si470x_disconnect_check(radio);
-	if (retval)
-		goto done;
 
 	if (tuner->index != 0)
-		goto done;
+		return -EINVAL;
 
 	/* mono/stereo selector */
 	switch (tuner->audmode) {
@@ -792,20 +666,12 @@ static int si470x_vidioc_s_tuner(struct file *file, void *priv,
 		radio->registers[POWERCFG] |= POWERCFG_MONO;  /* force mono */
 		break;
 	case V4L2_TUNER_MODE_STEREO:
+	default:
 		radio->registers[POWERCFG] &= ~POWERCFG_MONO; /* try stereo */
 		break;
-	default:
-		goto done;
 	}
 
-	retval = si470x_set_register(radio, POWERCFG);
-
-done:
-	if (retval < 0)
-		dev_warn(&radio->videodev->dev,
-			"set tuner failed with %d\n", retval);
-	mutex_unlock(&radio->lock);
-	return retval;
+	return si470x_set_register(radio, POWERCFG);
 }
 
 
@@ -816,28 +682,12 @@ static int si470x_vidioc_g_frequency(struct file *file, void *priv,
 		struct v4l2_frequency *freq)
 {
 	struct si470x_device *radio = video_drvdata(file);
-	int retval = 0;
-
-	/* safety checks */
-	mutex_lock(&radio->lock);
-	retval = si470x_disconnect_check(radio);
-	if (retval)
-		goto done;
 
-	if (freq->tuner != 0) {
-		retval = -EINVAL;
-		goto done;
-	}
+	if (freq->tuner != 0)
+		return -EINVAL;
 
 	freq->type = V4L2_TUNER_RADIO;
-	retval = si470x_get_freq(radio, &freq->frequency);
-
-done:
-	if (retval < 0)
-		dev_warn(&radio->videodev->dev,
-			"get frequency failed with %d\n", retval);
-	mutex_unlock(&radio->lock);
-	return retval;
+	return si470x_get_freq(radio, &freq->frequency);
 }
 
 
@@ -848,27 +698,11 @@ static int si470x_vidioc_s_frequency(struct file *file, void *priv,
 		struct v4l2_frequency *freq)
 {
 	struct si470x_device *radio = video_drvdata(file);
-	int retval = 0;
-
-	mutex_lock(&radio->lock);
-	/* safety checks */
-	retval = si470x_disconnect_check(radio);
-	if (retval)
-		goto done;
 
-	if (freq->tuner != 0) {
-		retval = -EINVAL;
-		goto done;
-	}
+	if (freq->tuner != 0)
+		return -EINVAL;
 
-	retval = si470x_set_freq(radio, freq->frequency);
-
-done:
-	if (retval < 0)
-		dev_warn(&radio->videodev->dev,
-			"set frequency failed with %d\n", retval);
-	mutex_unlock(&radio->lock);
-	return retval;
+	return si470x_set_freq(radio, freq->frequency);
 }
 
 
@@ -879,44 +713,29 @@ static int si470x_vidioc_s_hw_freq_seek(struct file *file, void *priv,
 		struct v4l2_hw_freq_seek *seek)
 {
 	struct si470x_device *radio = video_drvdata(file);
-	int retval = 0;
-
-	mutex_lock(&radio->lock);
-	/* safety checks */
-	retval = si470x_disconnect_check(radio);
-	if (retval)
-		goto done;
-
-	if (seek->tuner != 0) {
-		retval = -EINVAL;
-		goto done;
-	}
 
-	retval = si470x_set_seek(radio, seek->wrap_around, seek->seek_upward);
+	if (seek->tuner != 0)
+		return -EINVAL;
 
-done:
-	if (retval < 0)
-		dev_warn(&radio->videodev->dev,
-			"set hardware frequency seek failed with %d\n", retval);
-	mutex_unlock(&radio->lock);
-	return retval;
+	return si470x_set_seek(radio, seek->wrap_around, seek->seek_upward);
 }
 
+const struct v4l2_ctrl_ops si470x_ctrl_ops = {
+	.s_ctrl = si470x_s_ctrl,
+};
 
 /*
  * si470x_ioctl_ops - video device ioctl operations
  */
 static const struct v4l2_ioctl_ops si470x_ioctl_ops = {
 	.vidioc_querycap	= si470x_vidioc_querycap,
-	.vidioc_queryctrl	= si470x_vidioc_queryctrl,
-	.vidioc_g_ctrl		= si470x_vidioc_g_ctrl,
-	.vidioc_s_ctrl		= si470x_vidioc_s_ctrl,
-	.vidioc_g_audio		= si470x_vidioc_g_audio,
 	.vidioc_g_tuner		= si470x_vidioc_g_tuner,
 	.vidioc_s_tuner		= si470x_vidioc_s_tuner,
 	.vidioc_g_frequency	= si470x_vidioc_g_frequency,
 	.vidioc_s_frequency	= si470x_vidioc_s_frequency,
 	.vidioc_s_hw_freq_seek	= si470x_vidioc_s_hw_freq_seek,
+	.vidioc_subscribe_event = v4l2_ctrl_subscribe_event,
+	.vidioc_unsubscribe_event = v4l2_event_unsubscribe,
 };
 
 
@@ -926,6 +745,6 @@ static const struct v4l2_ioctl_ops si470x_ioctl_ops = {
 struct video_device si470x_viddev_template = {
 	.fops			= &si470x_fops,
 	.name			= DRIVER_NAME,
-	.release		= video_device_release,
+	.release		= video_device_release_empty,
 	.ioctl_ops		= &si470x_ioctl_ops,
 };
diff --git a/drivers/media/radio/si470x/radio-si470x-i2c.c b/drivers/media/radio/si470x/radio-si470x-i2c.c
index 9b546a5523f..a80044c5874 100644
--- a/drivers/media/radio/si470x/radio-si470x-i2c.c
+++ b/drivers/media/radio/si470x/radio-si470x-i2c.c
@@ -162,20 +162,6 @@ static int si470x_get_all_registers(struct si470x_device *radio)
 
 
 /**************************************************************************
- * General Driver Functions - DISCONNECT_CHECK
- **************************************************************************/
-
-/*
- * si470x_disconnect_check - check whether radio disconnects
- */
-int si470x_disconnect_check(struct si470x_device *radio)
-{
-	return 0;
-}
-
-
-
-/**************************************************************************
  * File Operations Interface
  **************************************************************************/
 
@@ -185,12 +171,12 @@ int si470x_disconnect_check(struct si470x_device *radio)
 int si470x_fops_open(struct file *file)
 {
 	struct si470x_device *radio = video_drvdata(file);
-	int retval = 0;
+	int retval = v4l2_fh_open(file);
 
-	mutex_lock(&radio->lock);
-	radio->users++;
+	if (retval)
+		return retval;
 
-	if (radio->users == 1) {
+	if (v4l2_fh_is_singular_file(file)) {
 		/* start radio */
 		retval = si470x_start(radio);
 		if (retval < 0)
@@ -205,7 +191,8 @@ int si470x_fops_open(struct file *file)
 	}
 
 done:
-	mutex_unlock(&radio->lock);
+	if (retval)
+		v4l2_fh_release(file);
 	return retval;
 }
 
@@ -216,21 +203,12 @@ done:
 int si470x_fops_release(struct file *file)
 {
 	struct si470x_device *radio = video_drvdata(file);
-	int retval = 0;
-
-	/* safety check */
-	if (!radio)
-		return -ENODEV;
 
-	mutex_lock(&radio->lock);
-	radio->users--;
-	if (radio->users == 0)
+	if (v4l2_fh_is_singular_file(file))
 		/* stop radio */
-		retval = si470x_stop(radio);
+		si470x_stop(radio);
 
-	mutex_unlock(&radio->lock);
-
-	return retval;
+	return v4l2_fh_release(file);
 }
 
 
@@ -371,32 +349,25 @@ static int __devinit si470x_i2c_probe(struct i2c_client *client,
 		goto err_initial;
 	}
 
-	radio->users = 0;
 	radio->client = client;
 	mutex_init(&radio->lock);
 
-	/* video device allocation and initialization */
-	radio->videodev = video_device_alloc();
-	if (!radio->videodev) {
-		retval = -ENOMEM;
-		goto err_radio;
-	}
-	memcpy(radio->videodev, &si470x_viddev_template,
-			sizeof(si470x_viddev_template));
-	video_set_drvdata(radio->videodev, radio);
+	/* video device initialization */
+	radio->videodev = si470x_viddev_template;
+	video_set_drvdata(&radio->videodev, radio);
 
 	/* power up : need 110ms */
 	radio->registers[POWERCFG] = POWERCFG_ENABLE;
 	if (si470x_set_register(radio, POWERCFG) < 0) {
 		retval = -EIO;
-		goto err_video;
+		goto err_radio;
 	}
 	msleep(110);
 
 	/* get device and chip versions */
 	if (si470x_get_all_registers(radio) < 0) {
 		retval = -EIO;
-		goto err_video;
+		goto err_radio;
 	}
 	dev_info(&client->dev, "DeviceID=0x%4.4hx ChipID=0x%4.4hx\n",
 			radio->registers[DEVICEID], radio->registers[CHIPID]);
@@ -427,7 +398,7 @@ static int __devinit si470x_i2c_probe(struct i2c_client *client,
 	radio->buffer = kmalloc(radio->buf_size, GFP_KERNEL);
 	if (!radio->buffer) {
 		retval = -EIO;
-		goto err_video;
+		goto err_radio;
 	}
 
 	/* rds buffer configuration */
@@ -447,7 +418,7 @@ static int __devinit si470x_i2c_probe(struct i2c_client *client,
 	}
 
 	/* register video device */
-	retval = video_register_device(radio->videodev, VFL_TYPE_RADIO,
+	retval = video_register_device(&radio->videodev, VFL_TYPE_RADIO,
 			radio_nr);
 	if (retval) {
 		dev_warn(&client->dev, "Could not register video device\n");
@@ -460,8 +431,6 @@ err_all:
 	free_irq(client->irq, radio);
 err_rds:
 	kfree(radio->buffer);
-err_video:
-	video_device_release(radio->videodev);
 err_radio:
 	kfree(radio);
 err_initial:
@@ -477,7 +446,7 @@ static __devexit int si470x_i2c_remove(struct i2c_client *client)
 	struct si470x_device *radio = i2c_get_clientdata(client);
 
 	free_irq(client->irq, radio);
-	video_unregister_device(radio->videodev);
+	video_unregister_device(&radio->videodev);
 	kfree(radio);
 
 	return 0;
diff --git a/drivers/media/radio/si470x/radio-si470x-usb.c b/drivers/media/radio/si470x/radio-si470x-usb.c
index b7debb67932..e9f63876129 100644
--- a/drivers/media/radio/si470x/radio-si470x-usb.c
+++ b/drivers/media/radio/si470x/radio-si470x-usb.c
@@ -367,23 +367,6 @@ static int si470x_get_scratch_page_versions(struct si470x_device *radio)
 
 
 /**************************************************************************
- * General Driver Functions - DISCONNECT_CHECK
- **************************************************************************/
-
-/*
- * si470x_disconnect_check - check whether radio disconnects
- */
-int si470x_disconnect_check(struct si470x_device *radio)
-{
-	if (radio->disconnected)
-		return -EIO;
-	else
-		return 0;
-}
-
-
-
-/**************************************************************************
  * RDS Driver Functions
  **************************************************************************/
 
@@ -414,9 +397,6 @@ static void si470x_int_in_callback(struct urb *urb)
 		}
 	}
 
-	/* safety checks */
-	if (radio->disconnected)
-		return;
 	if ((radio->registers[SYSCONFIG1] & SYSCONFIG1_RDS) == 0)
 		goto resubmit;
 
@@ -501,112 +481,30 @@ resubmit:
 }
 
 
-
-/**************************************************************************
- * File Operations Interface
- **************************************************************************/
-
-/*
- * si470x_fops_open - file open
- */
 int si470x_fops_open(struct file *file)
 {
-	struct si470x_device *radio = video_drvdata(file);
-	int retval;
-
-	mutex_lock(&radio->lock);
-	radio->users++;
-
-	retval = usb_autopm_get_interface(radio->intf);
-	if (retval < 0) {
-		radio->users--;
-		retval = -EIO;
-		goto done;
-	}
-
-	if (radio->users == 1) {
-		/* start radio */
-		retval = si470x_start(radio);
-		if (retval < 0) {
-			usb_autopm_put_interface(radio->intf);
-			goto done;
-		}
-
-		/* initialize interrupt urb */
-		usb_fill_int_urb(radio->int_in_urb, radio->usbdev,
-			usb_rcvintpipe(radio->usbdev,
-			radio->int_in_endpoint->bEndpointAddress),
-			radio->int_in_buffer,
-			le16_to_cpu(radio->int_in_endpoint->wMaxPacketSize),
-			si470x_int_in_callback,
-			radio,
-			radio->int_in_endpoint->bInterval);
-
-		radio->int_in_running = 1;
-		mb();
-
-		retval = usb_submit_urb(radio->int_in_urb, GFP_KERNEL);
-		if (retval) {
-			dev_info(&radio->intf->dev,
-				 "submitting int urb failed (%d)\n", retval);
-			radio->int_in_running = 0;
-			usb_autopm_put_interface(radio->intf);
-		}
-	}
-
-done:
-	mutex_unlock(&radio->lock);
-	return retval;
+	return v4l2_fh_open(file);
 }
 
-
-/*
- * si470x_fops_release - file release
- */
 int si470x_fops_release(struct file *file)
 {
-	struct si470x_device *radio = video_drvdata(file);
-	int retval = 0;
-
-	/* safety check */
-	if (!radio) {
-		retval = -ENODEV;
-		goto done;
-	}
-
-	mutex_lock(&radio->lock);
-	radio->users--;
-	if (radio->users == 0) {
-		/* shutdown interrupt handler */
-		if (radio->int_in_running) {
-			radio->int_in_running = 0;
-		if (radio->int_in_urb)
-			usb_kill_urb(radio->int_in_urb);
-		}
-
-		if (radio->disconnected) {
-			video_unregister_device(radio->videodev);
-			kfree(radio->int_in_buffer);
-			kfree(radio->buffer);
-			mutex_unlock(&radio->lock);
-			kfree(radio);
-			goto done;
-		}
+	return v4l2_fh_release(file);
+}
 
-		/* cancel read processes */
-		wake_up_interruptible(&radio->read_queue);
+static void si470x_usb_release(struct v4l2_device *v4l2_dev)
+{
+	struct si470x_device *radio =
+		container_of(v4l2_dev, struct si470x_device, v4l2_dev);
 
-		/* stop radio */
-		retval = si470x_stop(radio);
-		usb_autopm_put_interface(radio->intf);
-	}
-	mutex_unlock(&radio->lock);
-done:
-	return retval;
+	usb_free_urb(radio->int_in_urb);
+	v4l2_ctrl_handler_free(&radio->hdl);
+	v4l2_device_unregister(&radio->v4l2_dev);
+	kfree(radio->int_in_buffer);
+	kfree(radio->buffer);
+	kfree(radio);
 }
 
 
-
 /**************************************************************************
  * Video4Linux Interface
  **************************************************************************/
@@ -623,13 +521,45 @@ int si470x_vidioc_querycap(struct file *file, void *priv,
 	strlcpy(capability->card, DRIVER_CARD, sizeof(capability->card));
 	usb_make_path(radio->usbdev, capability->bus_info,
 			sizeof(capability->bus_info));
-	capability->capabilities = V4L2_CAP_HW_FREQ_SEEK |
+	capability->device_caps = V4L2_CAP_HW_FREQ_SEEK |
 		V4L2_CAP_TUNER | V4L2_CAP_RADIO | V4L2_CAP_RDS_CAPTURE;
-
+	capability->capabilities = capability->device_caps | V4L2_CAP_DEVICE_CAPS;
 	return 0;
 }
 
 
+static int si470x_start_usb(struct si470x_device *radio)
+{
+	int retval;
+
+	/* start radio */
+	retval = si470x_start(radio);
+	if (retval < 0)
+		return retval;
+
+	v4l2_ctrl_handler_setup(&radio->hdl);
+
+	/* initialize interrupt urb */
+	usb_fill_int_urb(radio->int_in_urb, radio->usbdev,
+			usb_rcvintpipe(radio->usbdev,
+				radio->int_in_endpoint->bEndpointAddress),
+			radio->int_in_buffer,
+			le16_to_cpu(radio->int_in_endpoint->wMaxPacketSize),
+			si470x_int_in_callback,
+			radio,
+			radio->int_in_endpoint->bInterval);
+
+	radio->int_in_running = 1;
+	mb();
+
+	retval = usb_submit_urb(radio->int_in_urb, GFP_KERNEL);
+	if (retval) {
+		dev_info(&radio->intf->dev,
+				"submitting int urb failed (%d)\n", retval);
+		radio->int_in_running = 0;
+	}
+	return retval;
+}
 
 /**************************************************************************
  * USB Interface
@@ -653,8 +583,6 @@ static int si470x_usb_driver_probe(struct usb_interface *intf,
 		retval = -ENOMEM;
 		goto err_initial;
 	}
-	radio->users = 0;
-	radio->disconnected = 0;
 	radio->usbdev = interface_to_usbdev(intf);
 	radio->intf = intf;
 	mutex_init(&radio->lock);
@@ -691,20 +619,35 @@ static int si470x_usb_driver_probe(struct usb_interface *intf,
 		goto err_intbuffer;
 	}
 
-	/* video device allocation and initialization */
-	radio->videodev = video_device_alloc();
-	if (!radio->videodev) {
-		retval = -ENOMEM;
+	radio->v4l2_dev.release = si470x_usb_release;
+	retval = v4l2_device_register(&intf->dev, &radio->v4l2_dev);
+	if (retval < 0) {
+		dev_err(&intf->dev, "couldn't register v4l2_device\n");
 		goto err_urb;
 	}
-	memcpy(radio->videodev, &si470x_viddev_template,
-			sizeof(si470x_viddev_template));
-	video_set_drvdata(radio->videodev, radio);
+
+	v4l2_ctrl_handler_init(&radio->hdl, 2);
+	v4l2_ctrl_new_std(&radio->hdl, &si470x_ctrl_ops,
+			  V4L2_CID_AUDIO_MUTE, 0, 1, 1, 1);
+	v4l2_ctrl_new_std(&radio->hdl, &si470x_ctrl_ops,
+			  V4L2_CID_AUDIO_VOLUME, 0, 15, 1, 15);
+	if (radio->hdl.error) {
+		retval = radio->hdl.error;
+		dev_err(&intf->dev, "couldn't register control\n");
+		goto err_dev;
+	}
+	radio->videodev = si470x_viddev_template;
+	radio->videodev.ctrl_handler = &radio->hdl;
+	radio->videodev.lock = &radio->lock;
+	radio->videodev.v4l2_dev = &radio->v4l2_dev;
+	radio->videodev.release = video_device_release_empty;
+	set_bit(V4L2_FL_USE_FH_PRIO, &radio->videodev.flags);
+	video_set_drvdata(&radio->videodev, radio);
 
 	/* get device and chip versions */
 	if (si470x_get_all_registers(radio) < 0) {
 		retval = -EIO;
-		goto err_video;
+		goto err_ctrl;
 	}
 	dev_info(&intf->dev, "DeviceID=0x%4.4hx ChipID=0x%4.4hx\n",
 			radio->registers[DEVICEID], radio->registers[CHIPID]);
@@ -721,7 +664,7 @@ static int si470x_usb_driver_probe(struct usb_interface *intf,
 	/* get software and hardware versions */
 	if (si470x_get_scratch_page_versions(radio) < 0) {
 		retval = -EIO;
-		goto err_video;
+		goto err_ctrl;
 	}
 	dev_info(&intf->dev, "software version %d, hardware version %d\n",
 			radio->software_version, radio->hardware_version);
@@ -764,28 +707,35 @@ static int si470x_usb_driver_probe(struct usb_interface *intf,
 	radio->buffer = kmalloc(radio->buf_size, GFP_KERNEL);
 	if (!radio->buffer) {
 		retval = -EIO;
-		goto err_video;
+		goto err_ctrl;
 	}
 
 	/* rds buffer configuration */
 	radio->wr_index = 0;
 	radio->rd_index = 0;
 	init_waitqueue_head(&radio->read_queue);
+	usb_set_intfdata(intf, radio);
+
+	/* start radio */
+	retval = si470x_start_usb(radio);
+	if (retval < 0)
+		goto err_all;
 
 	/* register video device */
-	retval = video_register_device(radio->videodev, VFL_TYPE_RADIO,
+	retval = video_register_device(&radio->videodev, VFL_TYPE_RADIO,
 			radio_nr);
 	if (retval) {
-		dev_warn(&intf->dev, "Could not register video device\n");
+		dev_err(&intf->dev, "Could not register video device\n");
 		goto err_all;
 	}
-	usb_set_intfdata(intf, radio);
 
 	return 0;
 err_all:
 	kfree(radio->buffer);
-err_video:
-	video_device_release(radio->videodev);
+err_ctrl:
+	v4l2_ctrl_handler_free(&radio->hdl);
+err_dev:
+	v4l2_device_unregister(&radio->v4l2_dev);
 err_urb:
 	usb_free_urb(radio->int_in_urb);
 err_intbuffer:
@@ -803,8 +753,22 @@ err_initial:
 static int si470x_usb_driver_suspend(struct usb_interface *intf,
 		pm_message_t message)
 {
+	struct si470x_device *radio = usb_get_intfdata(intf);
+
 	dev_info(&intf->dev, "suspending now...\n");
 
+	/* shutdown interrupt handler */
+	if (radio->int_in_running) {
+		radio->int_in_running = 0;
+		if (radio->int_in_urb)
+			usb_kill_urb(radio->int_in_urb);
+	}
+
+	/* cancel read processes */
+	wake_up_interruptible(&radio->read_queue);
+
+	/* stop radio */
+	si470x_stop(radio);
 	return 0;
 }
 
@@ -814,9 +778,12 @@ static int si470x_usb_driver_suspend(struct usb_interface *intf,
  */
 static int si470x_usb_driver_resume(struct usb_interface *intf)
 {
+	struct si470x_device *radio = usb_get_intfdata(intf);
+
 	dev_info(&intf->dev, "resuming now...\n");
 
-	return 0;
+	/* start radio */
+	return si470x_start_usb(radio);
 }
 
 
@@ -828,28 +795,22 @@ static void si470x_usb_driver_disconnect(struct usb_interface *intf)
 	struct si470x_device *radio = usb_get_intfdata(intf);
 
 	mutex_lock(&radio->lock);
-	radio->disconnected = 1;
+	v4l2_device_disconnect(&radio->v4l2_dev);
+	video_unregister_device(&radio->videodev);
 	usb_set_intfdata(intf, NULL);
-	if (radio->users == 0) {
-		/* set led to disconnect state */
-		si470x_set_led_state(radio, BLINK_ORANGE_LED);
-
-		/* Free data structures. */
-		usb_free_urb(radio->int_in_urb);
-
-		kfree(radio->int_in_buffer);
-		video_unregister_device(radio->videodev);
-		kfree(radio->buffer);
-		mutex_unlock(&radio->lock);
-		kfree(radio);
-	} else {
-		mutex_unlock(&radio->lock);
-	}
+	mutex_unlock(&radio->lock);
+	v4l2_device_put(&radio->v4l2_dev);
 }
 
 
 /*
  * si470x_usb_driver - usb driver interface
+ *
+ * A note on suspend/resume: this driver had only empty suspend/resume
+ * functions, and when I tried to test suspend/resume it always disconnected
+ * instead of resuming (using my ADS InstantFM stick). So I've decided to
+ * remove these callbacks until someone else with better hardware can
+ * implement and test this.
  */
 static struct usb_driver si470x_usb_driver = {
 	.name			= DRIVER_NAME,
@@ -857,8 +818,8 @@ static struct usb_driver si470x_usb_driver = {
 	.disconnect		= si470x_usb_driver_disconnect,
 	.suspend		= si470x_usb_driver_suspend,
 	.resume			= si470x_usb_driver_resume,
+	.reset_resume		= si470x_usb_driver_resume,
 	.id_table		= si470x_usb_driver_id_table,
-	.supports_autosuspend	= 1,
 };
 
 module_usb_driver(si470x_usb_driver);
diff --git a/drivers/media/radio/si470x/radio-si470x.h b/drivers/media/radio/si470x/radio-si470x.h
index f300a55ed85..4921cab8e0f 100644
--- a/drivers/media/radio/si470x/radio-si470x.h
+++ b/drivers/media/radio/si470x/radio-si470x.h
@@ -36,6 +36,9 @@
 #include <linux/mutex.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 <asm/unaligned.h>
 
 
@@ -141,10 +144,9 @@
  * si470x_device - private data
  */
 struct si470x_device {
-	struct video_device *videodev;
-
-	/* driver management */
-	unsigned int users;
+	struct v4l2_device v4l2_dev;
+	struct video_device videodev;
+	struct v4l2_ctrl_handler hdl;
 
 	/* Silabs internal registers (0..15) */
 	unsigned short registers[RADIO_REGISTER_NUM];
@@ -174,9 +176,6 @@ struct si470x_device {
 	/* scratch page */
 	unsigned char software_version;
 	unsigned char hardware_version;
-
-	/* driver management */
-	unsigned char disconnected;
 #endif
 
 #if defined(CONFIG_I2C_SI470X) || defined(CONFIG_I2C_SI470X_MODULE)
@@ -213,6 +212,7 @@ struct si470x_device {
  * Common Functions
  **************************************************************************/
 extern struct video_device si470x_viddev_template;
+extern const struct v4l2_ctrl_ops si470x_ctrl_ops;
 int si470x_get_register(struct si470x_device *radio, int regnr);
 int si470x_set_register(struct si470x_device *radio, int regnr);
 int si470x_disconnect_check(struct si470x_device *radio);
diff --git a/drivers/media/radio/tef6862.c b/drivers/media/radio/tef6862.c
index 6418c4c9faf..06d47e5cce9 100644
--- a/drivers/media/radio/tef6862.c
+++ b/drivers/media/radio/tef6862.c
@@ -211,7 +211,7 @@ static struct i2c_driver tef6862_driver = {
 		.name	= DRIVER_NAME,
 	},
 	.probe		= tef6862_probe,
-	.remove		= tef6862_remove,
+	.remove		= __devexit_p(tef6862_remove),
 	.id_table	= tef6862_id,
 };
 
diff --git a/drivers/media/radio/wl128x/fmdrv_v4l2.c b/drivers/media/radio/wl128x/fmdrv_v4l2.c
index 077d369a017..080b96a61f1 100644
--- a/drivers/media/radio/wl128x/fmdrv_v4l2.c
+++ b/drivers/media/radio/wl128x/fmdrv_v4l2.c
@@ -518,6 +518,10 @@ int fm_v4l2_init_video_device(struct fmdev *fmdev, int radio_nr)
 	video_set_drvdata(gradio_dev, fmdev);
 
 	gradio_dev->lock = &fmdev->mutex;
+	/* Locking in file operations other than ioctl should be done
+	   by the driver, not the V4L2 core.
+	   This driver needs auditing so that this flag can be removed. */
+	set_bit(V4L2_FL_LOCK_ALL_FOPS, &gradio_dev->flags);
 
 	/* Register with V4L2 subsystem as RADIO device */
 	if (video_register_device(gradio_dev, VFL_TYPE_RADIO, radio_nr)) {
diff --git a/drivers/media/rc/Kconfig b/drivers/media/rc/Kconfig
index a3fbb21350e..f97eeb87045 100644
--- a/drivers/media/rc/Kconfig
+++ b/drivers/media/rc/Kconfig
@@ -69,6 +69,7 @@ config IR_JVC_DECODER
 config IR_SONY_DECODER
 	tristate "Enable IR raw decoder for the Sony protocol"
 	depends on RC_CORE
+	select BITREVERSE
 	default y
 
 	---help---
diff --git a/drivers/media/rc/ati_remote.c b/drivers/media/rc/ati_remote.c
index baf907b3ce7..7be377fc1be 100644
--- a/drivers/media/rc/ati_remote.c
+++ b/drivers/media/rc/ati_remote.c
@@ -1,7 +1,7 @@
 /*
  *  USB ATI Remote support
  *
- *                Copyright (c) 2011 Anssi Hannula <anssi.hannula@iki.fi>
+ *                Copyright (c) 2011, 2012 Anssi Hannula <anssi.hannula@iki.fi>
  *  Version 2.2.0 Copyright (c) 2004 Torrey Hoffman <thoffman@arnor.net>
  *  Version 2.1.1 Copyright (c) 2002 Vladimir Dergachev
  *
@@ -151,13 +151,57 @@ MODULE_PARM_DESC(mouse, "Enable mouse device, default = yes");
 #undef err
 #define err(format, arg...) printk(KERN_ERR format , ## arg)
 
+struct ati_receiver_type {
+	/* either default_keymap or get_default_keymap should be set */
+	const char *default_keymap;
+	const char *(*get_default_keymap)(struct usb_interface *interface);
+};
+
+static const char *get_medion_keymap(struct usb_interface *interface)
+{
+	struct usb_device *udev = interface_to_usbdev(interface);
+
+	/*
+	 * There are many different Medion remotes shipped with a receiver
+	 * with the same usb id, but the receivers have subtle differences
+	 * in the USB descriptors allowing us to detect them.
+	 */
+
+	if (udev->manufacturer && udev->product) {
+		if (udev->actconfig->desc.bmAttributes & USB_CONFIG_ATT_WAKEUP) {
+
+			if (!strcmp(udev->manufacturer, "X10 Wireless Technology Inc")
+			    && !strcmp(udev->product, "USB Receiver"))
+				return RC_MAP_MEDION_X10_DIGITAINER;
+
+			if (!strcmp(udev->manufacturer, "X10 WTI")
+			    && !strcmp(udev->product, "RF receiver"))
+				return RC_MAP_MEDION_X10_OR2X;
+		} else {
+
+			 if (!strcmp(udev->manufacturer, "X10 Wireless Technology Inc")
+			    && !strcmp(udev->product, "USB Receiver"))
+				return RC_MAP_MEDION_X10;
+		}
+	}
+
+	dev_info(&interface->dev,
+		 "Unknown Medion X10 receiver, using default ati_remote Medion keymap\n");
+
+	return RC_MAP_MEDION_X10;
+}
+
+static const struct ati_receiver_type type_ati		= { .default_keymap = RC_MAP_ATI_X10 };
+static const struct ati_receiver_type type_medion	= { .get_default_keymap = get_medion_keymap };
+static const struct ati_receiver_type type_firefly	= { .default_keymap = RC_MAP_SNAPSTREAM_FIREFLY };
+
 static struct usb_device_id ati_remote_table[] = {
-	{ USB_DEVICE(ATI_REMOTE_VENDOR_ID, LOLA_REMOTE_PRODUCT_ID),	.driver_info = (unsigned long)RC_MAP_ATI_X10 },
-	{ USB_DEVICE(ATI_REMOTE_VENDOR_ID, LOLA2_REMOTE_PRODUCT_ID),	.driver_info = (unsigned long)RC_MAP_ATI_X10 },
-	{ USB_DEVICE(ATI_REMOTE_VENDOR_ID, ATI_REMOTE_PRODUCT_ID),	.driver_info = (unsigned long)RC_MAP_ATI_X10 },
-	{ USB_DEVICE(ATI_REMOTE_VENDOR_ID, NVIDIA_REMOTE_PRODUCT_ID),	.driver_info = (unsigned long)RC_MAP_ATI_X10 },
-	{ USB_DEVICE(ATI_REMOTE_VENDOR_ID, MEDION_REMOTE_PRODUCT_ID),	.driver_info = (unsigned long)RC_MAP_MEDION_X10 },
-	{ USB_DEVICE(ATI_REMOTE_VENDOR_ID, FIREFLY_REMOTE_PRODUCT_ID),	.driver_info = (unsigned long)RC_MAP_SNAPSTREAM_FIREFLY },
+	{ USB_DEVICE(ATI_REMOTE_VENDOR_ID, LOLA_REMOTE_PRODUCT_ID),	.driver_info = (unsigned long)&type_ati },
+	{ USB_DEVICE(ATI_REMOTE_VENDOR_ID, LOLA2_REMOTE_PRODUCT_ID),	.driver_info = (unsigned long)&type_ati },
+	{ USB_DEVICE(ATI_REMOTE_VENDOR_ID, ATI_REMOTE_PRODUCT_ID),	.driver_info = (unsigned long)&type_ati },
+	{ USB_DEVICE(ATI_REMOTE_VENDOR_ID, NVIDIA_REMOTE_PRODUCT_ID),	.driver_info = (unsigned long)&type_ati },
+	{ USB_DEVICE(ATI_REMOTE_VENDOR_ID, MEDION_REMOTE_PRODUCT_ID),	.driver_info = (unsigned long)&type_medion },
+	{ USB_DEVICE(ATI_REMOTE_VENDOR_ID, FIREFLY_REMOTE_PRODUCT_ID),	.driver_info = (unsigned long)&type_firefly },
 	{}	/* Terminating entry */
 };
 
@@ -445,6 +489,7 @@ static void ati_remote_input_report(struct urb *urb)
 	int acc;
 	int remote_num;
 	unsigned char scancode;
+	u32 wheel_keycode = KEY_RESERVED;
 	int i;
 
 	/*
@@ -484,26 +529,33 @@ static void ati_remote_input_report(struct urb *urb)
 	 */
 	scancode = data[2] & 0x7f;
 
-	/* Look up event code index in the mouse translation table. */
-	for (i = 0; ati_remote_tbl[i].kind != KIND_END; i++) {
-		if (scancode == ati_remote_tbl[i].data) {
-			index = i;
-			break;
+	dbginfo(&ati_remote->interface->dev,
+		"channel 0x%02x; key data %02x, scancode %02x\n",
+		remote_num, data[2], scancode);
+
+	if (scancode >= 0x70) {
+		/*
+		 * This is either a mouse or scrollwheel event, depending on
+		 * the remote/keymap.
+		 * Get the keycode assigned to scancode 0x78/0x70. If it is
+		 * set, assume this is a scrollwheel up/down event.
+		 */
+		wheel_keycode = rc_g_keycode_from_table(ati_remote->rdev,
+							scancode & 0x78);
+
+		if (wheel_keycode == KEY_RESERVED) {
+			/* scrollwheel was not mapped, assume mouse */
+
+			/* Look up event code index in the mouse translation table. */
+			for (i = 0; ati_remote_tbl[i].kind != KIND_END; i++) {
+				if (scancode == ati_remote_tbl[i].data) {
+					index = i;
+					break;
+				}
+			}
 		}
 	}
 
-	if (index >= 0) {
-		dbginfo(&ati_remote->interface->dev,
-			"channel 0x%02x; mouse data %02x; index %d; keycode %d\n",
-			remote_num, data[2], index, ati_remote_tbl[index].code);
-		if (!dev)
-			return; /* no mouse device */
-	} else
-		dbginfo(&ati_remote->interface->dev,
-			"channel 0x%02x; key data %02x, scancode %02x\n",
-			remote_num, data[2], scancode);
-
-
 	if (index >= 0 && ati_remote_tbl[index].kind == KIND_LITERAL) {
 		input_event(dev, ati_remote_tbl[index].type,
 			ati_remote_tbl[index].code,
@@ -542,15 +594,29 @@ static void ati_remote_input_report(struct urb *urb)
 
 		if (index < 0) {
 			/* Not a mouse event, hand it to rc-core. */
-
-			/*
-			 * We don't use the rc-core repeat handling yet as
-			 * it would cause ghost repeats which would be a
-			 * regression for this driver.
-			 */
-			rc_keydown_notimeout(ati_remote->rdev, scancode,
-					     data[2]);
-			rc_keyup(ati_remote->rdev);
+			int count = 1;
+
+			if (wheel_keycode != KEY_RESERVED) {
+				/*
+				 * This is a scrollwheel event, send the
+				 * scroll up (0x78) / down (0x70) scancode
+				 * repeatedly as many times as indicated by
+				 * rest of the scancode.
+				 */
+				count = (scancode & 0x07) + 1;
+				scancode &= 0x78;
+			}
+
+			while (count--) {
+				/*
+				* We don't use the rc-core repeat handling yet as
+				* it would cause ghost repeats which would be a
+				* regression for this driver.
+				*/
+				rc_keydown_notimeout(ati_remote->rdev, scancode,
+						     data[2]);
+				rc_keyup(ati_remote->rdev);
+			}
 			return;
 		}
 
@@ -766,6 +832,7 @@ static int ati_remote_probe(struct usb_interface *interface, const struct usb_de
 	struct usb_device *udev = interface_to_usbdev(interface);
 	struct usb_host_interface *iface_host = interface->cur_altsetting;
 	struct usb_endpoint_descriptor *endpoint_in, *endpoint_out;
+	struct ati_receiver_type *type = (struct ati_receiver_type *)id->driver_info;
 	struct ati_remote *ati_remote;
 	struct input_dev *input_dev;
 	struct rc_dev *rc_dev;
@@ -827,10 +894,15 @@ static int ati_remote_probe(struct usb_interface *interface, const struct usb_de
 	snprintf(ati_remote->mouse_name, sizeof(ati_remote->mouse_name),
 		 "%s mouse", ati_remote->rc_name);
 
-	if (id->driver_info)
-		rc_dev->map_name = (const char *)id->driver_info;
-	else
-		rc_dev->map_name = RC_MAP_ATI_X10;
+	rc_dev->map_name = RC_MAP_ATI_X10; /* default map */
+
+	/* set default keymap according to receiver model */
+	if (type) {
+		if (type->default_keymap)
+			rc_dev->map_name = type->default_keymap;
+		else if (type->get_default_keymap)
+			rc_dev->map_name = type->get_default_keymap(interface);
+	}
 
 	ati_remote_rc_init(ati_remote);
 	mutex_init(&ati_remote->open_mutex);
diff --git a/drivers/media/rc/ene_ir.c b/drivers/media/rc/ene_ir.c
index 860c112e0fd..bef5296173c 100644
--- a/drivers/media/rc/ene_ir.c
+++ b/drivers/media/rc/ene_ir.c
@@ -1018,22 +1018,6 @@ static int ene_probe(struct pnp_dev *pnp_dev, const struct pnp_device_id *id)
 
 	spin_lock_init(&dev->hw_lock);
 
-	/* claim the resources */
-	error = -EBUSY;
-	dev->hw_io = pnp_port_start(pnp_dev, 0);
-	if (!request_region(dev->hw_io, ENE_IO_SIZE, ENE_DRIVER_NAME)) {
-		dev->hw_io = -1;
-		dev->irq = -1;
-		goto error;
-	}
-
-	dev->irq = pnp_irq(pnp_dev, 0);
-	if (request_irq(dev->irq, ene_isr,
-			IRQF_SHARED, ENE_DRIVER_NAME, (void *)dev)) {
-		dev->irq = -1;
-		goto error;
-	}
-
 	pnp_set_drvdata(pnp_dev, dev);
 	dev->pnp_dev = pnp_dev;
 
@@ -1086,6 +1070,22 @@ static int ene_probe(struct pnp_dev *pnp_dev, const struct pnp_device_id *id)
 	device_set_wakeup_capable(&pnp_dev->dev, true);
 	device_set_wakeup_enable(&pnp_dev->dev, true);
 
+	/* claim the resources */
+	error = -EBUSY;
+	dev->hw_io = pnp_port_start(pnp_dev, 0);
+	if (!request_region(dev->hw_io, ENE_IO_SIZE, ENE_DRIVER_NAME)) {
+		dev->hw_io = -1;
+		dev->irq = -1;
+		goto error;
+	}
+
+	dev->irq = pnp_irq(pnp_dev, 0);
+	if (request_irq(dev->irq, ene_isr,
+			IRQF_SHARED, ENE_DRIVER_NAME, (void *)dev)) {
+		dev->irq = -1;
+		goto error;
+	}
+
 	error = rc_register_device(rdev);
 	if (error < 0)
 		goto error;
diff --git a/drivers/media/rc/fintek-cir.c b/drivers/media/rc/fintek-cir.c
index 392d4be91f8..6aabf7ae3a3 100644
--- a/drivers/media/rc/fintek-cir.c
+++ b/drivers/media/rc/fintek-cir.c
@@ -197,7 +197,7 @@ static int fintek_hw_detect(struct fintek_dev *fintek)
 	/*
 	 * Newer reviews of this chipset uses port 8 instead of 5
 	 */
-	if ((chip != 0x0408) || (chip != 0x0804))
+	if ((chip != 0x0408) && (chip != 0x0804))
 		fintek->logical_dev_cir = LOGICAL_DEV_CIR_REV2;
 	else
 		fintek->logical_dev_cir = LOGICAL_DEV_CIR_REV1;
@@ -514,16 +514,6 @@ static int fintek_probe(struct pnp_dev *pdev, const struct pnp_device_id *dev_id
 
 	spin_lock_init(&fintek->fintek_lock);
 
-	ret = -EBUSY;
-	/* now claim resources */
-	if (!request_region(fintek->cir_addr,
-			    fintek->cir_port_len, FINTEK_DRIVER_NAME))
-		goto failure;
-
-	if (request_irq(fintek->cir_irq, fintek_cir_isr, IRQF_SHARED,
-			FINTEK_DRIVER_NAME, (void *)fintek))
-		goto failure;
-
 	pnp_set_drvdata(pdev, fintek);
 	fintek->pdev = pdev;
 
@@ -558,9 +548,19 @@ static int fintek_probe(struct pnp_dev *pdev, const struct pnp_device_id *dev_id
 	/* rx resolution is hardwired to 50us atm, 1, 25, 100 also possible */
 	rdev->rx_resolution = US_TO_NS(CIR_SAMPLE_PERIOD);
 
+	ret = -EBUSY;
+	/* now claim resources */
+	if (!request_region(fintek->cir_addr,
+			    fintek->cir_port_len, FINTEK_DRIVER_NAME))
+		goto failure;
+
+	if (request_irq(fintek->cir_irq, fintek_cir_isr, IRQF_SHARED,
+			FINTEK_DRIVER_NAME, (void *)fintek))
+		goto failure2;
+
 	ret = rc_register_device(rdev);
 	if (ret)
-		goto failure;
+		goto failure3;
 
 	device_init_wakeup(&pdev->dev, true);
 	fintek->rdev = rdev;
@@ -570,12 +570,11 @@ static int fintek_probe(struct pnp_dev *pdev, const struct pnp_device_id *dev_id
 
 	return 0;
 
+failure3:
+	free_irq(fintek->cir_irq, fintek);
+failure2:
+	release_region(fintek->cir_addr, fintek->cir_port_len);
 failure:
-	if (fintek->cir_irq)
-		free_irq(fintek->cir_irq, fintek);
-	if (fintek->cir_addr)
-		release_region(fintek->cir_addr, fintek->cir_port_len);
-
 	rc_free_device(rdev);
 	kfree(fintek);
 
diff --git a/drivers/media/rc/imon.c b/drivers/media/rc/imon.c
index 7f26fdf2e54..5dd0386604f 100644
--- a/drivers/media/rc/imon.c
+++ b/drivers/media/rc/imon.c
@@ -255,7 +255,7 @@ static struct usb_device_id imon_usb_id_table[] = {
 static struct usb_driver imon_driver = {
 	.name		= MOD_NAME,
 	.probe		= imon_probe,
-	.disconnect	= imon_disconnect,
+	.disconnect	= __devexit_p(imon_disconnect),
 	.suspend	= imon_suspend,
 	.resume		= imon_resume,
 	.id_table	= imon_usb_id_table,
diff --git a/drivers/media/rc/ir-raw.c b/drivers/media/rc/ir-raw.c
index 95e630998aa..a8202512134 100644
--- a/drivers/media/rc/ir-raw.c
+++ b/drivers/media/rc/ir-raw.c
@@ -46,9 +46,9 @@ static int ir_raw_event_thread(void *data)
 	while (!kthread_should_stop()) {
 
 		spin_lock_irq(&raw->lock);
-		retval = kfifo_out(&raw->kfifo, &ev, sizeof(ev));
+		retval = kfifo_len(&raw->kfifo);
 
-		if (!retval) {
+		if (retval < sizeof(ev)) {
 			set_current_state(TASK_INTERRUPTIBLE);
 
 			if (kthread_should_stop())
@@ -59,11 +59,9 @@ static int ir_raw_event_thread(void *data)
 			continue;
 		}
 
+		retval = kfifo_out(&raw->kfifo, &ev, sizeof(ev));
 		spin_unlock_irq(&raw->lock);
 
-
-		BUG_ON(retval != sizeof(ev));
-
 		mutex_lock(&ir_raw_handler_lock);
 		list_for_each_entry(handler, &ir_raw_handler_list, list)
 			handler->decode(raw->dev, ev);
diff --git a/drivers/media/rc/ir-sanyo-decoder.c b/drivers/media/rc/ir-sanyo-decoder.c
index d38fbdd0b25..7e54ec57bcf 100644
--- a/drivers/media/rc/ir-sanyo-decoder.c
+++ b/drivers/media/rc/ir-sanyo-decoder.c
@@ -56,7 +56,7 @@ static int ir_sanyo_decode(struct rc_dev *dev, struct ir_raw_event ev)
 {
 	struct sanyo_dec *data = &dev->raw->sanyo;
 	u32 scancode;
-	u8 address, not_address, command, not_command;
+	u8 address, command, not_command;
 
 	if (!(dev->raw->enabled_protocols & RC_TYPE_SANYO))
 		return 0;
@@ -154,7 +154,7 @@ static int ir_sanyo_decode(struct rc_dev *dev, struct ir_raw_event ev)
 			break;
 
 		address     = bitrev16((data->bits >> 29) & 0x1fff) >> 3;
-		not_address = bitrev16((data->bits >> 16) & 0x1fff) >> 3;
+		/* not_address = bitrev16((data->bits >> 16) & 0x1fff) >> 3; */
 		command	    = bitrev8((data->bits >>  8) & 0xff);
 		not_command = bitrev8((data->bits >>  0) & 0xff);
 
diff --git a/drivers/media/rc/ite-cir.c b/drivers/media/rc/ite-cir.c
index 682009d76cd..36fe5a349b9 100644
--- a/drivers/media/rc/ite-cir.c
+++ b/drivers/media/rc/ite-cir.c
@@ -1515,16 +1515,6 @@ static int ite_probe(struct pnp_dev *pdev, const struct pnp_device_id
 	/* initialize raw event */
 	init_ir_raw_event(&itdev->rawir);
 
-	ret = -EBUSY;
-	/* now claim resources */
-	if (!request_region(itdev->cir_addr,
-				dev_desc->io_region_size, ITE_DRIVER_NAME))
-		goto failure;
-
-	if (request_irq(itdev->cir_irq, ite_cir_isr, IRQF_SHARED,
-			ITE_DRIVER_NAME, (void *)itdev))
-		goto failure;
-
 	/* set driver data into the pnp device */
 	pnp_set_drvdata(pdev, itdev);
 	itdev->pdev = pdev;
@@ -1600,22 +1590,30 @@ static int ite_probe(struct pnp_dev *pdev, const struct pnp_device_id
 	rdev->driver_name = ITE_DRIVER_NAME;
 	rdev->map_name = RC_MAP_RC6_MCE;
 
+	ret = -EBUSY;
+	/* now claim resources */
+	if (!request_region(itdev->cir_addr,
+				dev_desc->io_region_size, ITE_DRIVER_NAME))
+		goto failure;
+
+	if (request_irq(itdev->cir_irq, ite_cir_isr, IRQF_SHARED,
+			ITE_DRIVER_NAME, (void *)itdev))
+		goto failure2;
+
 	ret = rc_register_device(rdev);
 	if (ret)
-		goto failure;
+		goto failure3;
 
 	itdev->rdev = rdev;
 	ite_pr(KERN_NOTICE, "driver has been successfully loaded\n");
 
 	return 0;
 
+failure3:
+	free_irq(itdev->cir_irq, itdev);
+failure2:
+	release_region(itdev->cir_addr, itdev->params.io_region_size);
 failure:
-	if (itdev->cir_irq)
-		free_irq(itdev->cir_irq, itdev);
-
-	if (itdev->cir_addr)
-		release_region(itdev->cir_addr, itdev->params.io_region_size);
-
 	rc_free_device(rdev);
 	kfree(itdev);
 
diff --git a/drivers/media/rc/keymaps/Makefile b/drivers/media/rc/keymaps/Makefile
index 49ce2662f56..ab84d66c67c 100644
--- a/drivers/media/rc/keymaps/Makefile
+++ b/drivers/media/rc/keymaps/Makefile
@@ -3,6 +3,7 @@ obj-$(CONFIG_RC_MAP) += rc-adstech-dvb-t-pci.o \
 			rc-anysee.o \
 			rc-apac-viewcomp.o \
 			rc-asus-pc39.o \
+			rc-asus-ps3-100.o \
 			rc-ati-tv-wonder-hd-600.o \
 			rc-ati-x10.o \
 			rc-avermedia-a16d.o \
@@ -52,6 +53,8 @@ obj-$(CONFIG_RC_MAP) += rc-adstech-dvb-t-pci.o \
 			rc-lme2510.o \
 			rc-manli.o \
 			rc-medion-x10.o \
+			rc-medion-x10-digitainer.o \
+			rc-medion-x10-or2x.o \
 			rc-msi-digivox-ii.o \
 			rc-msi-digivox-iii.o \
 			rc-msi-tvanywhere.o \
diff --git a/drivers/media/rc/keymaps/rc-asus-ps3-100.c b/drivers/media/rc/keymaps/rc-asus-ps3-100.c
new file mode 100644
index 00000000000..ba76609c593
--- /dev/null
+++ b/drivers/media/rc/keymaps/rc-asus-ps3-100.c
@@ -0,0 +1,91 @@
+/* asus-ps3-100.h - Keytable for asus_ps3_100 Remote Controller
+ *
+ * Copyright (c) 2012 by Mauro Carvalho Chehab <mchehab@redhat.com>
+ *
+ * Based on a previous patch from Remi Schwartz <remi.schwartz@gmail.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ */
+
+#include <media/rc-map.h>
+#include <linux/module.h>
+
+static struct rc_map_table asus_ps3_100[] = {
+	{ 0x081c, KEY_HOME },             /* home */
+	{ 0x081e, KEY_TV },               /* tv */
+	{ 0x0803, KEY_TEXT },             /* teletext */
+	{ 0x0829, KEY_POWER },            /* close */
+
+	{ 0x080b, KEY_RED },              /* red */
+	{ 0x080d, KEY_YELLOW },           /* yellow */
+	{ 0x0806, KEY_BLUE },             /* blue */
+	{ 0x0807, KEY_GREEN },            /* green */
+
+	/* Keys 0 to 9 */
+	{ 0x082a, KEY_0 },
+	{ 0x0816, KEY_1 },
+	{ 0x0812, KEY_2 },
+	{ 0x0814, KEY_3 },
+	{ 0x0836, KEY_4 },
+	{ 0x0832, KEY_5 },
+	{ 0x0834, KEY_6 },
+	{ 0x080e, KEY_7 },
+	{ 0x080a, KEY_8 },
+	{ 0x080c, KEY_9 },
+
+	{ 0x0815, KEY_VOLUMEUP },
+	{ 0x0826, KEY_VOLUMEDOWN },
+	{ 0x0835, KEY_CHANNELUP },        /* channel / program + */
+	{ 0x0824, KEY_CHANNELDOWN },      /* channel / program - */
+
+	{ 0x0808, KEY_UP },
+	{ 0x0804, KEY_DOWN },
+	{ 0x0818, KEY_LEFT },
+	{ 0x0810, KEY_RIGHT },
+	{ 0x0825, KEY_ENTER },            /* enter */
+
+	{ 0x0822, KEY_EXIT },             /* back */
+	{ 0x082c, KEY_AB },               /* recall */
+
+	{ 0x0820, KEY_AUDIO },            /* TV audio */
+	{ 0x0837, KEY_SCREEN },           /* snapshot */
+	{ 0x082e, KEY_ZOOM },             /* full screen */
+	{ 0x0802, KEY_MUTE },             /* mute */
+
+	{ 0x0831, KEY_REWIND },           /* backward << */
+	{ 0x0811, KEY_RECORD },           /* recording */
+	{ 0x0809, KEY_STOP },
+	{ 0x0805, KEY_FASTFORWARD },      /* forward >> */
+	{ 0x0821, KEY_PREVIOUS },         /* rew */
+	{ 0x081a, KEY_PAUSE },            /* pause */
+	{ 0x0839, KEY_PLAY },             /* play */
+	{ 0x0819, KEY_NEXT },             /* forward */
+};
+
+static struct rc_map_list asus_ps3_100_map = {
+.map = {
+	.scan    = asus_ps3_100,
+	.size    = ARRAY_SIZE(asus_ps3_100),
+	.rc_type = RC_TYPE_RC5,
+	.name    = RC_MAP_ASUS_PS3_100,
+}
+};
+
+static int __init init_rc_map_asus_ps3_100(void)
+{
+return rc_map_register(&asus_ps3_100_map);
+}
+
+static void __exit exit_rc_map_asus_ps3_100(void)
+{
+rc_map_unregister(&asus_ps3_100_map);
+}
+
+module_init(init_rc_map_asus_ps3_100)
+module_exit(exit_rc_map_asus_ps3_100)
+
+MODULE_LICENSE("GPL");
+MODULE_AUTHOR("Mauro Carvalho Chehab <mchehab@redhat.com>");
diff --git a/drivers/media/rc/keymaps/rc-it913x-v2.c b/drivers/media/rc/keymaps/rc-it913x-v2.c
index 28e376e18b9..bd42a30ec06 100644
--- a/drivers/media/rc/keymaps/rc-it913x-v2.c
+++ b/drivers/media/rc/keymaps/rc-it913x-v2.c
@@ -40,7 +40,7 @@ static struct rc_map_table it913x_v2_rc[] = {
 	/* Type 2 */
 	/* keys stereo, snapshot unassigned */
 	{ 0x866b00, KEY_0 },
-	{ 0x866b1b, KEY_1 },
+	{ 0x866b01, KEY_1 },
 	{ 0x866b02, KEY_2 },
 	{ 0x866b03, KEY_3 },
 	{ 0x866b04, KEY_4 },
diff --git a/drivers/media/rc/keymaps/rc-medion-x10-digitainer.c b/drivers/media/rc/keymaps/rc-medion-x10-digitainer.c
new file mode 100644
index 00000000000..966f9b3c71d
--- /dev/null
+++ b/drivers/media/rc/keymaps/rc-medion-x10-digitainer.c
@@ -0,0 +1,123 @@
+/*
+ * Medion X10 RF remote keytable (Digitainer variant)
+ *
+ * Copyright (C) 2012 Anssi Hannula <anssi.hannula@iki.fi>
+ *
+ * This keymap is for a variant that has a distinctive scrollwheel instead of
+ * up/down buttons (tested with P/N 40009936 / 20018268), reportedly
+ * originally shipped with Medion Digitainer but now sold separately simply as
+ * an "X10" remote.
+ *
+ * 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/module.h>
+#include <media/rc-map.h>
+
+static struct rc_map_table medion_x10_digitainer[] = {
+	{ 0x02, KEY_POWER },
+
+	{ 0x2c, KEY_TV },
+	{ 0x2d, KEY_VIDEO },
+	{ 0x04, KEY_DVD },    /* CD/DVD */
+	{ 0x16, KEY_TEXT },   /* "teletext" icon, i.e. a screen with lines */
+	{ 0x06, KEY_AUDIO },
+	{ 0x2e, KEY_RADIO },
+	{ 0x31, KEY_EPG },    /* a screen with an open book */
+	{ 0x05, KEY_IMAGES }, /* Photo */
+	{ 0x2f, KEY_INFO },
+
+	{ 0x78, KEY_UP },     /* scrollwheel up 1 notch */
+	/* 0x79..0x7f: 2-8 notches, driver repeats 0x78 entry */
+
+	{ 0x70, KEY_DOWN },   /* scrollwheel down 1 notch */
+	/* 0x71..0x77: 2-8 notches, driver repeats 0x70 entry */
+
+	{ 0x19, KEY_MENU },
+	{ 0x1d, KEY_LEFT },
+	{ 0x1e, KEY_OK },     /* scrollwheel press */
+	{ 0x1f, KEY_RIGHT },
+	{ 0x20, KEY_BACK },
+
+	{ 0x09, KEY_VOLUMEUP },
+	{ 0x08, KEY_VOLUMEDOWN },
+	{ 0x00, KEY_MUTE },
+
+	{ 0x1b, KEY_SELECT }, /* also has "U" rotated 90 degrees CCW */
+
+	{ 0x0b, KEY_CHANNELUP },
+	{ 0x0c, KEY_CHANNELDOWN },
+	{ 0x1c, KEY_LAST },
+
+	{ 0x32, KEY_RED },    /* also Audio */
+	{ 0x33, KEY_GREEN },  /* also Subtitle */
+	{ 0x34, KEY_YELLOW }, /* also Angle */
+	{ 0x35, KEY_BLUE },   /* also Title */
+
+	{ 0x28, KEY_STOP },
+	{ 0x29, KEY_PAUSE },
+	{ 0x25, KEY_PLAY },
+	{ 0x21, KEY_PREVIOUS },
+	{ 0x18, KEY_CAMERA },
+	{ 0x23, KEY_NEXT },
+	{ 0x24, KEY_REWIND },
+	{ 0x27, KEY_RECORD },
+	{ 0x26, KEY_FORWARD },
+
+	{ 0x0d, KEY_1 },
+	{ 0x0e, KEY_2 },
+	{ 0x0f, KEY_3 },
+	{ 0x10, KEY_4 },
+	{ 0x11, KEY_5 },
+	{ 0x12, KEY_6 },
+	{ 0x13, KEY_7 },
+	{ 0x14, KEY_8 },
+	{ 0x15, KEY_9 },
+	{ 0x17, KEY_0 },
+
+	/* these do not actually exist on this remote, but these scancodes
+	 * exist on all other Medion X10 remotes and adding them here allows
+	 * such remotes to be adequately usable with this keymap in case
+	 * this keymap is wrongly used with them (which is quite possible as
+	 * there are lots of different Medion X10 remotes): */
+	{ 0x1a, KEY_UP },
+	{ 0x22, KEY_DOWN },
+};
+
+static struct rc_map_list medion_x10_digitainer_map = {
+	.map = {
+		.scan    = medion_x10_digitainer,
+		.size    = ARRAY_SIZE(medion_x10_digitainer),
+		.rc_type = RC_TYPE_OTHER,
+		.name    = RC_MAP_MEDION_X10_DIGITAINER,
+	}
+};
+
+static int __init init_rc_map_medion_x10_digitainer(void)
+{
+	return rc_map_register(&medion_x10_digitainer_map);
+}
+
+static void __exit exit_rc_map_medion_x10_digitainer(void)
+{
+	rc_map_unregister(&medion_x10_digitainer_map);
+}
+
+module_init(init_rc_map_medion_x10_digitainer)
+module_exit(exit_rc_map_medion_x10_digitainer)
+
+MODULE_DESCRIPTION("Medion X10 RF remote keytable (Digitainer variant)");
+MODULE_AUTHOR("Anssi Hannula <anssi.hannula@iki.fi>");
+MODULE_LICENSE("GPL");
diff --git a/drivers/media/rc/keymaps/rc-medion-x10-or2x.c b/drivers/media/rc/keymaps/rc-medion-x10-or2x.c
new file mode 100644
index 00000000000..b077300ecb5
--- /dev/null
+++ b/drivers/media/rc/keymaps/rc-medion-x10-or2x.c
@@ -0,0 +1,108 @@
+/*
+ * Medion X10 OR22/OR24 RF remote keytable
+ *
+ * Copyright (C) 2012 Anssi Hannula <anssi.hannula@iki.fi>
+ *
+ * This keymap is for several Medion X10 remotes that have the Windows MCE
+ * button. This has been tested with a "RF VISTA Remote Control", OR24V,
+ * P/N 20035335, but should work with other variants that have the same
+ * buttons, such as OR22V and OR24E.
+ *
+ * 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/module.h>
+#include <media/rc-map.h>
+
+static struct rc_map_table medion_x10_or2x[] = {
+	{ 0x02, KEY_POWER },
+	{ 0x16, KEY_TEXT },   /* "T" in a box, for teletext */
+
+	{ 0x09, KEY_VOLUMEUP },
+	{ 0x08, KEY_VOLUMEDOWN },
+	{ 0x00, KEY_MUTE },
+	{ 0x0b, KEY_CHANNELUP },
+	{ 0x0c, KEY_CHANNELDOWN },
+
+	{ 0x32, KEY_RED },
+	{ 0x33, KEY_GREEN },
+	{ 0x34, KEY_YELLOW },
+	{ 0x35, KEY_BLUE },
+
+	{ 0x18, KEY_PVR },    /* record symbol inside a tv symbol */
+	{ 0x04, KEY_DVD },    /* disc symbol */
+	{ 0x31, KEY_EPG },    /* a tv schedule symbol */
+	{ 0x1c, KEY_TV },     /* play symbol inside a tv symbol */
+	{ 0x20, KEY_BACK },
+	{ 0x2f, KEY_INFO },
+
+	{ 0x1a, KEY_UP },
+	{ 0x22, KEY_DOWN },
+	{ 0x1d, KEY_LEFT },
+	{ 0x1f, KEY_RIGHT },
+	{ 0x1e, KEY_OK },
+
+	{ 0x1b, KEY_MEDIA },  /* Windows MCE button */
+
+	{ 0x21, KEY_PREVIOUS },
+	{ 0x23, KEY_NEXT },
+	{ 0x24, KEY_REWIND },
+	{ 0x26, KEY_FORWARD },
+	{ 0x25, KEY_PLAY },
+	{ 0x28, KEY_STOP },
+	{ 0x29, KEY_PAUSE },
+	{ 0x27, KEY_RECORD },
+
+	{ 0x0d, KEY_1 },
+	{ 0x0e, KEY_2 },
+	{ 0x0f, KEY_3 },
+	{ 0x10, KEY_4 },
+	{ 0x11, KEY_5 },
+	{ 0x12, KEY_6 },
+	{ 0x13, KEY_7 },
+	{ 0x14, KEY_8 },
+	{ 0x15, KEY_9 },
+	{ 0x17, KEY_0 },
+	{ 0x30, KEY_CLEAR },
+	{ 0x36, KEY_ENTER },
+	{ 0x37, KEY_NUMERIC_STAR },
+	{ 0x38, KEY_NUMERIC_POUND },
+};
+
+static struct rc_map_list medion_x10_or2x_map = {
+	.map = {
+		.scan    = medion_x10_or2x,
+		.size    = ARRAY_SIZE(medion_x10_or2x),
+		.rc_type = RC_TYPE_OTHER,
+		.name    = RC_MAP_MEDION_X10_OR2X,
+	}
+};
+
+static int __init init_rc_map_medion_x10_or2x(void)
+{
+	return rc_map_register(&medion_x10_or2x_map);
+}
+
+static void __exit exit_rc_map_medion_x10_or2x(void)
+{
+	rc_map_unregister(&medion_x10_or2x_map);
+}
+
+module_init(init_rc_map_medion_x10_or2x)
+module_exit(exit_rc_map_medion_x10_or2x)
+
+MODULE_DESCRIPTION("Medion X10 OR22/OR24 RF remote keytable");
+MODULE_AUTHOR("Anssi Hannula <anssi.hannula@iki.fi>");
+MODULE_LICENSE("GPL");
diff --git a/drivers/media/rc/mceusb.c b/drivers/media/rc/mceusb.c
index e150a2e29a4..84e06d3aa69 100644
--- a/drivers/media/rc/mceusb.c
+++ b/drivers/media/rc/mceusb.c
@@ -520,7 +520,7 @@ static void mceusb_dev_printdata(struct mceusb_dev *ir, char *buf,
 {
 	char codes[USB_BUFLEN * 3 + 1];
 	char inout[9];
-	u8 cmd, subcmd, data1, data2, data3, data4, data5;
+	u8 cmd, subcmd, data1, data2, data3, data4;
 	struct device *dev = ir->dev;
 	int i, start, skip = 0;
 	u32 carrier, period;
@@ -553,7 +553,6 @@ static void mceusb_dev_printdata(struct mceusb_dev *ir, char *buf,
 	data2  = buf[start + 3] & 0xff;
 	data3  = buf[start + 4] & 0xff;
 	data4  = buf[start + 5] & 0xff;
-	data5  = buf[start + 6] & 0xff;
 
 	switch (cmd) {
 	case MCE_CMD_NULL:
@@ -1443,7 +1442,7 @@ static int mceusb_dev_resume(struct usb_interface *intf)
 static struct usb_driver mceusb_dev_driver = {
 	.name =		DRIVER_NAME,
 	.probe =	mceusb_dev_probe,
-	.disconnect =	mceusb_dev_disconnect,
+	.disconnect =	__devexit_p(mceusb_dev_disconnect),
 	.suspend =	mceusb_dev_suspend,
 	.resume =	mceusb_dev_resume,
 	.reset_resume =	mceusb_dev_resume,
diff --git a/drivers/media/rc/nuvoton-cir.c b/drivers/media/rc/nuvoton-cir.c
index 144f3f55d76..dc8a7dddccd 100644
--- a/drivers/media/rc/nuvoton-cir.c
+++ b/drivers/media/rc/nuvoton-cir.c
@@ -1021,24 +1021,6 @@ static int nvt_probe(struct pnp_dev *pdev, const struct pnp_device_id *dev_id)
 	spin_lock_init(&nvt->nvt_lock);
 	spin_lock_init(&nvt->tx.lock);
 
-	ret = -EBUSY;
-	/* now claim resources */
-	if (!request_region(nvt->cir_addr,
-			    CIR_IOREG_LENGTH, NVT_DRIVER_NAME))
-		goto failure;
-
-	if (request_irq(nvt->cir_irq, nvt_cir_isr, IRQF_SHARED,
-			NVT_DRIVER_NAME, (void *)nvt))
-		goto failure;
-
-	if (!request_region(nvt->cir_wake_addr,
-			    CIR_IOREG_LENGTH, NVT_DRIVER_NAME))
-		goto failure;
-
-	if (request_irq(nvt->cir_wake_irq, nvt_cir_wake_isr, IRQF_SHARED,
-			NVT_DRIVER_NAME, (void *)nvt))
-		goto failure;
-
 	pnp_set_drvdata(pdev, nvt);
 	nvt->pdev = pdev;
 
@@ -1085,9 +1067,27 @@ static int nvt_probe(struct pnp_dev *pdev, const struct pnp_device_id *dev_id)
 	rdev->tx_resolution = XYZ;
 #endif
 
+	ret = -EBUSY;
+	/* now claim resources */
+	if (!request_region(nvt->cir_addr,
+			    CIR_IOREG_LENGTH, NVT_DRIVER_NAME))
+		goto failure;
+
+	if (request_irq(nvt->cir_irq, nvt_cir_isr, IRQF_SHARED,
+			NVT_DRIVER_NAME, (void *)nvt))
+		goto failure2;
+
+	if (!request_region(nvt->cir_wake_addr,
+			    CIR_IOREG_LENGTH, NVT_DRIVER_NAME))
+		goto failure3;
+
+	if (request_irq(nvt->cir_wake_irq, nvt_cir_wake_isr, IRQF_SHARED,
+			NVT_DRIVER_NAME, (void *)nvt))
+		goto failure4;
+
 	ret = rc_register_device(rdev);
 	if (ret)
-		goto failure;
+		goto failure5;
 
 	device_init_wakeup(&pdev->dev, true);
 	nvt->rdev = rdev;
@@ -1099,17 +1099,15 @@ static int nvt_probe(struct pnp_dev *pdev, const struct pnp_device_id *dev_id)
 
 	return 0;
 
+failure5:
+	free_irq(nvt->cir_wake_irq, nvt);
+failure4:
+	release_region(nvt->cir_wake_addr, CIR_IOREG_LENGTH);
+failure3:
+	free_irq(nvt->cir_irq, nvt);
+failure2:
+	release_region(nvt->cir_addr, CIR_IOREG_LENGTH);
 failure:
-	if (nvt->cir_irq)
-		free_irq(nvt->cir_irq, nvt);
-	if (nvt->cir_addr)
-		release_region(nvt->cir_addr, CIR_IOREG_LENGTH);
-
-	if (nvt->cir_wake_irq)
-		free_irq(nvt->cir_wake_irq, nvt);
-	if (nvt->cir_wake_addr)
-		release_region(nvt->cir_wake_addr, CIR_IOREG_LENGTH);
-
 	rc_free_device(rdev);
 	kfree(nvt);
 
diff --git a/drivers/media/rc/rc-loopback.c b/drivers/media/rc/rc-loopback.c
index efc6a514348..fae1615e0ff 100644
--- a/drivers/media/rc/rc-loopback.c
+++ b/drivers/media/rc/rc-loopback.c
@@ -221,7 +221,6 @@ static int __init loop_init(void)
 	rc->s_idle		= loop_set_idle;
 	rc->s_learning_mode	= loop_set_learning_mode;
 	rc->s_carrier_report	= loop_set_carrier_report;
-	rc->priv		= &loopdev;
 
 	loopdev.txmask		= RXMASK_REGULAR;
 	loopdev.txcarrier	= 36000;
diff --git a/drivers/media/rc/redrat3.c b/drivers/media/rc/redrat3.c
index ad95c67a4db..2878b0ed974 100644
--- a/drivers/media/rc/redrat3.c
+++ b/drivers/media/rc/redrat3.c
@@ -1277,7 +1277,7 @@ static int redrat3_dev_resume(struct usb_interface *intf)
 static struct usb_driver redrat3_dev_driver = {
 	.name		= DRIVER_NAME,
 	.probe		= redrat3_dev_probe,
-	.disconnect	= redrat3_dev_disconnect,
+	.disconnect	= __devexit_p(redrat3_dev_disconnect),
 	.suspend	= redrat3_dev_suspend,
 	.resume		= redrat3_dev_resume,
 	.reset_resume	= redrat3_dev_resume,
diff --git a/drivers/media/rc/winbond-cir.c b/drivers/media/rc/winbond-cir.c
index af526586fa2..342c2c8c1dd 100644
--- a/drivers/media/rc/winbond-cir.c
+++ b/drivers/media/rc/winbond-cir.c
@@ -991,39 +991,10 @@ wbcir_probe(struct pnp_dev *device, const struct pnp_device_id *dev_id)
 		"(w: 0x%lX, e: 0x%lX, s: 0x%lX, i: %u)\n",
 		data->wbase, data->ebase, data->sbase, data->irq);
 
-	if (!request_region(data->wbase, WAKEUP_IOMEM_LEN, DRVNAME)) {
-		dev_err(dev, "Region 0x%lx-0x%lx already in use!\n",
-			data->wbase, data->wbase + WAKEUP_IOMEM_LEN - 1);
-		err = -EBUSY;
-		goto exit_free_data;
-	}
-
-	if (!request_region(data->ebase, EHFUNC_IOMEM_LEN, DRVNAME)) {
-		dev_err(dev, "Region 0x%lx-0x%lx already in use!\n",
-			data->ebase, data->ebase + EHFUNC_IOMEM_LEN - 1);
-		err = -EBUSY;
-		goto exit_release_wbase;
-	}
-
-	if (!request_region(data->sbase, SP_IOMEM_LEN, DRVNAME)) {
-		dev_err(dev, "Region 0x%lx-0x%lx already in use!\n",
-			data->sbase, data->sbase + SP_IOMEM_LEN - 1);
-		err = -EBUSY;
-		goto exit_release_ebase;
-	}
-
-	err = request_irq(data->irq, wbcir_irq_handler,
-			  IRQF_DISABLED, DRVNAME, device);
-	if (err) {
-		dev_err(dev, "Failed to claim IRQ %u\n", data->irq);
-		err = -EBUSY;
-		goto exit_release_sbase;
-	}
-
 	led_trigger_register_simple("cir-tx", &data->txtrigger);
 	if (!data->txtrigger) {
 		err = -ENOMEM;
-		goto exit_free_irq;
+		goto exit_free_data;
 	}
 
 	led_trigger_register_simple("cir-rx", &data->rxtrigger);
@@ -1062,9 +1033,38 @@ wbcir_probe(struct pnp_dev *device, const struct pnp_device_id *dev_id)
 	data->dev->priv = data;
 	data->dev->dev.parent = &device->dev;
 
+	if (!request_region(data->wbase, WAKEUP_IOMEM_LEN, DRVNAME)) {
+		dev_err(dev, "Region 0x%lx-0x%lx already in use!\n",
+			data->wbase, data->wbase + WAKEUP_IOMEM_LEN - 1);
+		err = -EBUSY;
+		goto exit_free_rc;
+	}
+
+	if (!request_region(data->ebase, EHFUNC_IOMEM_LEN, DRVNAME)) {
+		dev_err(dev, "Region 0x%lx-0x%lx already in use!\n",
+			data->ebase, data->ebase + EHFUNC_IOMEM_LEN - 1);
+		err = -EBUSY;
+		goto exit_release_wbase;
+	}
+
+	if (!request_region(data->sbase, SP_IOMEM_LEN, DRVNAME)) {
+		dev_err(dev, "Region 0x%lx-0x%lx already in use!\n",
+			data->sbase, data->sbase + SP_IOMEM_LEN - 1);
+		err = -EBUSY;
+		goto exit_release_ebase;
+	}
+
+	err = request_irq(data->irq, wbcir_irq_handler,
+			  IRQF_DISABLED, DRVNAME, device);
+	if (err) {
+		dev_err(dev, "Failed to claim IRQ %u\n", data->irq);
+		err = -EBUSY;
+		goto exit_release_sbase;
+	}
+
 	err = rc_register_device(data->dev);
 	if (err)
-		goto exit_free_rc;
+		goto exit_free_irq;
 
 	device_init_wakeup(&device->dev, 1);
 
@@ -1072,14 +1072,6 @@ wbcir_probe(struct pnp_dev *device, const struct pnp_device_id *dev_id)
 
 	return 0;
 
-exit_free_rc:
-	rc_free_device(data->dev);
-exit_unregister_led:
-	led_classdev_unregister(&data->led);
-exit_unregister_rxtrigger:
-	led_trigger_unregister_simple(data->rxtrigger);
-exit_unregister_txtrigger:
-	led_trigger_unregister_simple(data->txtrigger);
 exit_free_irq:
 	free_irq(data->irq, device);
 exit_release_sbase:
@@ -1088,6 +1080,14 @@ exit_release_ebase:
 	release_region(data->ebase, EHFUNC_IOMEM_LEN);
 exit_release_wbase:
 	release_region(data->wbase, WAKEUP_IOMEM_LEN);
+exit_free_rc:
+	rc_free_device(data->dev);
+exit_unregister_led:
+	led_classdev_unregister(&data->led);
+exit_unregister_rxtrigger:
+	led_trigger_unregister_simple(data->rxtrigger);
+exit_unregister_txtrigger:
+	led_trigger_unregister_simple(data->txtrigger);
 exit_free_data:
 	kfree(data);
 	pnp_set_drvdata(device, NULL);
diff --git a/drivers/media/video/Kconfig b/drivers/media/video/Kconfig
index ce1e7ba940f..99937c94d7d 100644
--- a/drivers/media/video/Kconfig
+++ b/drivers/media/video/Kconfig
@@ -472,6 +472,9 @@ comment "Camera sensor devices"
 config VIDEO_APTINA_PLL
 	tristate
 
+config VIDEO_SMIAPP_PLL
+	tristate
+
 config VIDEO_OV7670
 	tristate "OmniVision OV7670 sensor support"
 	depends on I2C && VIDEO_V4L2
@@ -556,6 +559,8 @@ config VIDEO_S5K6AA
 	  This is a V4L2 sensor-level driver for Samsung S5K6AA(FX) 1.3M
 	  camera sensor with an embedded SoC image signal processor.
 
+source "drivers/media/video/smiapp/Kconfig"
+
 comment "Flash devices"
 
 config VIDEO_ADP1653
@@ -644,6 +649,8 @@ menuconfig V4L_USB_DRIVERS
 
 if V4L_USB_DRIVERS
 
+source "drivers/media/video/au0828/Kconfig"
+
 source "drivers/media/video/uvc/Kconfig"
 
 source "drivers/media/video/gspca/Kconfig"
@@ -662,8 +669,6 @@ source "drivers/media/video/tm6000/Kconfig"
 
 source "drivers/media/video/usbvision/Kconfig"
 
-source "drivers/media/video/et61x251/Kconfig"
-
 source "drivers/media/video/sn9c102/Kconfig"
 
 source "drivers/media/video/pwc/Kconfig"
@@ -721,8 +726,6 @@ menuconfig V4L_PCI_DRIVERS
 
 if V4L_PCI_DRIVERS
 
-source "drivers/media/video/au0828/Kconfig"
-
 source "drivers/media/video/bt8xx/Kconfig"
 
 source "drivers/media/video/cx18/Kconfig"
@@ -794,6 +797,19 @@ source "drivers/media/video/saa7164/Kconfig"
 
 source "drivers/media/video/zoran/Kconfig"
 
+config STA2X11_VIP
+	tristate "STA2X11 VIP Video For Linux"
+	depends on STA2X11
+	select VIDEO_ADV7180 if VIDEO_HELPER_CHIPS_AUTO
+	select VIDEOBUF_DMA_CONTIG
+	depends on PCI && VIDEO_V4L2 && VIRT_TO_BUS
+	help
+	  Say Y for support for STA2X11 VIP (Video Input Port) capture
+	  device.
+
+	  To compile this driver as a module, choose M here: the
+	  module will be called sta2x11_vip.
+
 endif # V4L_PCI_DRIVERS
 
 #
@@ -1127,19 +1143,6 @@ config VIDEO_MX2
 	  This is a v4l2 driver for the i.MX27 and the i.MX25 Camera Sensor
 	  Interface
 
-config  VIDEO_SAMSUNG_S5P_FIMC
-	tristate "Samsung S5P and EXYNOS4 camera interface driver (EXPERIMENTAL)"
-	depends on VIDEO_V4L2 && I2C && PLAT_S5P && PM_RUNTIME && \
-		VIDEO_V4L2_SUBDEV_API && EXPERIMENTAL
-	select VIDEOBUF2_DMA_CONTIG
-	select V4L2_MEM2MEM_DEV
-	---help---
-	  This is a v4l2 driver for Samsung S5P and EXYNOS4 camera
-	  host interface and video postprocessor.
-
-	  To compile this driver as a module, choose M here: the
-	  module will be called s5p-fimc.
-
 config VIDEO_ATMEL_ISI
 	tristate "ATMEL Image Sensor Interface (ISI) support"
 	depends on VIDEO_DEV && SOC_CAMERA && ARCH_AT91
@@ -1148,16 +1151,7 @@ config VIDEO_ATMEL_ISI
 	  This module makes the ATMEL Image Sensor Interface available
 	  as a v4l2 device.
 
-config VIDEO_S5P_MIPI_CSIS
-	tristate "Samsung S5P and EXYNOS4 MIPI CSI receiver driver"
-	depends on VIDEO_V4L2 && PM_RUNTIME && PLAT_S5P
-	depends on VIDEO_V4L2_SUBDEV_API && REGULATOR
-	---help---
-	  This is a v4l2 driver for Samsung S5P/EXYNOS4 MIPI-CSI receiver.
-
-	  To compile this driver as a module, choose M here: the
-	  module will be called s5p-csis.
-
+source "drivers/media/video/s5p-fimc/Kconfig"
 source "drivers/media/video/s5p-tv/Kconfig"
 
 endif # V4L_PLATFORM_DRIVERS
diff --git a/drivers/media/video/Makefile b/drivers/media/video/Makefile
index a6282a3a6a8..d209de0e0ca 100644
--- a/drivers/media/video/Makefile
+++ b/drivers/media/video/Makefile
@@ -79,9 +79,12 @@ obj-$(CONFIG_VIDEO_SR030PC30)	+= sr030pc30.o
 obj-$(CONFIG_VIDEO_NOON010PC30)	+= noon010pc30.o
 obj-$(CONFIG_VIDEO_M5MOLS)	+= m5mols/
 obj-$(CONFIG_VIDEO_S5K6AA)	+= s5k6aa.o
+obj-$(CONFIG_VIDEO_SMIAPP)	+= smiapp/
 obj-$(CONFIG_VIDEO_ADP1653)	+= adp1653.o
 obj-$(CONFIG_VIDEO_AS3645A)	+= as3645a.o
 
+obj-$(CONFIG_VIDEO_SMIAPP_PLL)	+= smiapp-pll.o
+
 obj-$(CONFIG_SOC_CAMERA_IMX074)		+= imx074.o
 obj-$(CONFIG_SOC_CAMERA_MT9M001)	+= mt9m001.o
 obj-$(CONFIG_SOC_CAMERA_MT9M111)	+= mt9m111.o
@@ -120,6 +123,7 @@ obj-$(CONFIG_VIDEO_TM6000) += tm6000/
 obj-$(CONFIG_VIDEO_MXB) += mxb.o
 obj-$(CONFIG_VIDEO_HEXIUM_ORION) += hexium_orion.o
 obj-$(CONFIG_VIDEO_HEXIUM_GEMINI) += hexium_gemini.o
+obj-$(CONFIG_STA2X11_VIP) += sta2x11_vip.o
 obj-$(CONFIG_VIDEO_TIMBERDALE)	+= timblogiw.o
 
 obj-$(CONFIG_VIDEOBUF_GEN) += videobuf-core.o
@@ -152,7 +156,6 @@ obj-$(CONFIG_USB_ZR364XX)       += zr364xx.o
 obj-$(CONFIG_USB_STKWEBCAM)     += stkwebcam.o
 
 obj-$(CONFIG_USB_SN9C102)       += sn9c102/
-obj-$(CONFIG_USB_ET61X251)      += et61x251/
 obj-$(CONFIG_USB_PWC)           += pwc/
 obj-$(CONFIG_USB_GSPCA)         += gspca/
 
diff --git a/drivers/media/video/adp1653.c b/drivers/media/video/adp1653.c
index 5b045b4a66f..57e87090388 100644
--- a/drivers/media/video/adp1653.c
+++ b/drivers/media/video/adp1653.c
@@ -34,7 +34,6 @@
 #include <linux/module.h>
 #include <linux/i2c.h>
 #include <linux/slab.h>
-#include <linux/version.h>
 #include <media/adp1653.h>
 #include <media/v4l2-device.h>
 
@@ -282,19 +281,19 @@ adp1653_init_device(struct adp1653_flash *flash)
 		return -EIO;
 	}
 
-	mutex_lock(&flash->ctrls.lock);
+	mutex_lock(flash->ctrls.lock);
 	/* Reset faults before reading new ones. */
 	flash->fault = 0;
 	rval = adp1653_get_fault(flash);
-	mutex_unlock(&flash->ctrls.lock);
+	mutex_unlock(flash->ctrls.lock);
 	if (rval > 0) {
 		dev_err(&client->dev, "faults detected: 0x%1.1x\n", rval);
 		return -EIO;
 	}
 
-	mutex_lock(&flash->ctrls.lock);
+	mutex_lock(flash->ctrls.lock);
 	rval = adp1653_update_hw(flash);
-	mutex_unlock(&flash->ctrls.lock);
+	mutex_unlock(flash->ctrls.lock);
 	if (rval) {
 		dev_err(&client->dev,
 			"adp1653_update_hw failed at %s\n", __func__);
diff --git a/drivers/media/video/adv7180.c b/drivers/media/video/adv7180.c
index b8b6c4b0cad..174bffacf11 100644
--- a/drivers/media/video/adv7180.c
+++ b/drivers/media/video/adv7180.c
@@ -48,6 +48,7 @@
 #define ADV7180_INPUT_CONTROL_PAL_COMB_N_PED		0xd0
 #define ADV7180_INPUT_CONTROL_PAL_SECAM			0xe0
 #define ADV7180_INPUT_CONTROL_PAL_SECAM_PED		0xf0
+#define ADV7180_INPUT_CONTROL_INSEL_MASK		0x0f
 
 #define ADV7180_EXTENDED_OUTPUT_CONTROL_REG		0x04
 #define ADV7180_EXTENDED_OUTPUT_CONTROL_NTSCDIS		0xC5
@@ -55,9 +56,29 @@
 #define ADV7180_AUTODETECT_ENABLE_REG			0x07
 #define ADV7180_AUTODETECT_DEFAULT			0x7f
 
+#define ADV7180_CON_REG		0x08	/*Unsigned */
+#define CON_REG_MIN		0
+#define CON_REG_DEF		128
+#define CON_REG_MAX		255
+
+#define ADV7180_BRI_REG		0x0a	/*Signed */
+#define BRI_REG_MIN		-128
+#define BRI_REG_DEF		0
+#define BRI_REG_MAX		127
+
+#define ADV7180_HUE_REG		0x0b	/*Signed, inverted */
+#define HUE_REG_MIN		-127
+#define HUE_REG_DEF		0
+#define HUE_REG_MAX		128
+
 #define ADV7180_ADI_CTRL_REG				0x0e
 #define ADV7180_ADI_CTRL_IRQ_SPACE			0x20
 
+#define ADV7180_PWR_MAN_REG		0x0f
+#define ADV7180_PWR_MAN_ON		0x04
+#define ADV7180_PWR_MAN_OFF		0x24
+#define ADV7180_PWR_MAN_RES		0x80
+
 #define ADV7180_STATUS1_REG				0x10
 #define ADV7180_STATUS1_IN_LOCK		0x01
 #define ADV7180_STATUS1_AUTOD_MASK	0x70
@@ -78,6 +99,12 @@
 #define ADV7180_ICONF1_PSYNC_ONLY	0x10
 #define ADV7180_ICONF1_ACTIVE_TO_CLR	0xC0
 
+#define ADV7180_SD_SAT_CB_REG	0xe3	/*Unsigned */
+#define ADV7180_SD_SAT_CR_REG	0xe4	/*Unsigned */
+#define SAT_REG_MIN		0
+#define SAT_REG_DEF		128
+#define SAT_REG_MAX		255
+
 #define ADV7180_IRQ1_LOCK	0x01
 #define ADV7180_IRQ1_UNLOCK	0x02
 #define ADV7180_ISR1_ADI	0x42
@@ -90,6 +117,9 @@
 #define ADV7180_IMR3_ADI	0x4C
 #define ADV7180_IMR4_ADI	0x50
 
+#define ADV7180_NTSC_V_BIT_END_REG	0xE6
+#define ADV7180_NTSC_V_BIT_END_MANUAL_NVEND	0x4F
+
 struct adv7180_state {
 	struct v4l2_subdev	sd;
 	struct work_struct	work;
@@ -97,6 +127,11 @@ struct adv7180_state {
 	int			irq;
 	v4l2_std_id		curr_norm;
 	bool			autodetect;
+	s8			brightness;
+	s16			hue;
+	u8			contrast;
+	u8			saturation;
+	u8			input;
 };
 
 static v4l2_std_id adv7180_std_to_v4l2(u8 status1)
@@ -155,7 +190,7 @@ static u32 adv7180_status_to_v4l2(u8 status1)
 }
 
 static int __adv7180_status(struct i2c_client *client, u32 *status,
-	v4l2_std_id *std)
+			    v4l2_std_id *std)
 {
 	int status1 = i2c_smbus_read_byte_data(client, ADV7180_STATUS1_REG);
 
@@ -192,6 +227,36 @@ static int adv7180_querystd(struct v4l2_subdev *sd, v4l2_std_id *std)
 	return err;
 }
 
+static int adv7180_s_routing(struct v4l2_subdev *sd, u32 input,
+			     u32 output, u32 config)
+{
+	struct adv7180_state *state = to_state(sd);
+	int ret = mutex_lock_interruptible(&state->mutex);
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+
+	if (ret)
+		return ret;
+
+	/*We cannot discriminate between LQFP and 40-pin LFCSP, so accept
+	 * all inputs and let the card driver take care of validation
+	 */
+	if ((input & ADV7180_INPUT_CONTROL_INSEL_MASK) != input)
+		goto out;
+
+	ret = i2c_smbus_read_byte_data(client, ADV7180_INPUT_CONTROL_REG);
+
+	if (ret < 0)
+		goto out;
+
+	ret &= ~ADV7180_INPUT_CONTROL_INSEL_MASK;
+	ret = i2c_smbus_write_byte_data(client,
+					ADV7180_INPUT_CONTROL_REG, ret | input);
+	state->input = input;
+out:
+	mutex_unlock(&state->mutex);
+	return ret;
+}
+
 static int adv7180_g_input_status(struct v4l2_subdev *sd, u32 *status)
 {
 	struct adv7180_state *state = to_state(sd);
@@ -205,7 +270,7 @@ static int adv7180_g_input_status(struct v4l2_subdev *sd, u32 *status)
 }
 
 static int adv7180_g_chip_ident(struct v4l2_subdev *sd,
-	struct v4l2_dbg_chip_ident *chip)
+				struct v4l2_dbg_chip_ident *chip)
 {
 	struct i2c_client *client = v4l2_get_subdevdata(sd);
 
@@ -222,9 +287,10 @@ static int adv7180_s_std(struct v4l2_subdev *sd, v4l2_std_id std)
 
 	/* all standards -> autodetect */
 	if (std == V4L2_STD_ALL) {
-		ret = i2c_smbus_write_byte_data(client,
-			ADV7180_INPUT_CONTROL_REG,
-			ADV7180_INPUT_CONTROL_AD_PAL_BG_NTSC_J_SECAM);
+		ret =
+		    i2c_smbus_write_byte_data(client, ADV7180_INPUT_CONTROL_REG,
+				ADV7180_INPUT_CONTROL_AD_PAL_BG_NTSC_J_SECAM
+					      | state->input);
 		if (ret < 0)
 			goto out;
 
@@ -236,7 +302,8 @@ static int adv7180_s_std(struct v4l2_subdev *sd, v4l2_std_id std)
 			goto out;
 
 		ret = i2c_smbus_write_byte_data(client,
-			ADV7180_INPUT_CONTROL_REG, ret);
+						ADV7180_INPUT_CONTROL_REG,
+						ret | state->input);
 		if (ret < 0)
 			goto out;
 
@@ -249,14 +316,138 @@ out:
 	return ret;
 }
 
+static int adv7180_queryctrl(struct v4l2_subdev *sd, struct v4l2_queryctrl *qc)
+{
+	switch (qc->id) {
+	case V4L2_CID_BRIGHTNESS:
+		return v4l2_ctrl_query_fill(qc, BRI_REG_MIN, BRI_REG_MAX,
+					    1, BRI_REG_DEF);
+	case V4L2_CID_HUE:
+		return v4l2_ctrl_query_fill(qc, HUE_REG_MIN, HUE_REG_MAX,
+					    1, HUE_REG_DEF);
+	case V4L2_CID_CONTRAST:
+		return v4l2_ctrl_query_fill(qc, CON_REG_MIN, CON_REG_MAX,
+					    1, CON_REG_DEF);
+	case V4L2_CID_SATURATION:
+		return v4l2_ctrl_query_fill(qc, SAT_REG_MIN, SAT_REG_MAX,
+					    1, SAT_REG_DEF);
+	default:
+		break;
+	}
+
+	return -EINVAL;
+}
+
+static int adv7180_g_ctrl(struct v4l2_subdev *sd, struct v4l2_control *ctrl)
+{
+	struct adv7180_state *state = to_state(sd);
+	int ret = mutex_lock_interruptible(&state->mutex);
+	if (ret)
+		return ret;
+
+	switch (ctrl->id) {
+	case V4L2_CID_BRIGHTNESS:
+		ctrl->value = state->brightness;
+		break;
+	case V4L2_CID_HUE:
+		ctrl->value = state->hue;
+		break;
+	case V4L2_CID_CONTRAST:
+		ctrl->value = state->contrast;
+		break;
+	case V4L2_CID_SATURATION:
+		ctrl->value = state->saturation;
+		break;
+	default:
+		ret = -EINVAL;
+	}
+
+	mutex_unlock(&state->mutex);
+	return ret;
+}
+
+static int adv7180_s_ctrl(struct v4l2_subdev *sd, struct v4l2_control *ctrl)
+{
+	struct adv7180_state *state = to_state(sd);
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	int ret = mutex_lock_interruptible(&state->mutex);
+	if (ret)
+		return ret;
+
+	switch (ctrl->id) {
+	case V4L2_CID_BRIGHTNESS:
+		if ((ctrl->value > BRI_REG_MAX)
+		    || (ctrl->value < BRI_REG_MIN)) {
+			ret = -ERANGE;
+			break;
+		}
+		state->brightness = ctrl->value;
+		ret = i2c_smbus_write_byte_data(client,
+						ADV7180_BRI_REG,
+						state->brightness);
+		break;
+	case V4L2_CID_HUE:
+		if ((ctrl->value > HUE_REG_MAX)
+		    || (ctrl->value < HUE_REG_MIN)) {
+			ret = -ERANGE;
+			break;
+		}
+		state->hue = ctrl->value;
+		/*Hue is inverted according to HSL chart */
+		ret = i2c_smbus_write_byte_data(client,
+						ADV7180_HUE_REG, -state->hue);
+		break;
+	case V4L2_CID_CONTRAST:
+		if ((ctrl->value > CON_REG_MAX)
+		    || (ctrl->value < CON_REG_MIN)) {
+			ret = -ERANGE;
+			break;
+		}
+		state->contrast = ctrl->value;
+		ret = i2c_smbus_write_byte_data(client,
+						ADV7180_CON_REG,
+						state->contrast);
+		break;
+	case V4L2_CID_SATURATION:
+		if ((ctrl->value > SAT_REG_MAX)
+		    || (ctrl->value < SAT_REG_MIN)) {
+			ret = -ERANGE;
+			break;
+		}
+		/*
+		 *This could be V4L2_CID_BLUE_BALANCE/V4L2_CID_RED_BALANCE
+		 *Let's not confuse the user, everybody understands saturation
+		 */
+		state->saturation = ctrl->value;
+		ret = i2c_smbus_write_byte_data(client,
+						ADV7180_SD_SAT_CB_REG,
+						state->saturation);
+		if (ret < 0)
+			break;
+		ret = i2c_smbus_write_byte_data(client,
+						ADV7180_SD_SAT_CR_REG,
+						state->saturation);
+		break;
+	default:
+		ret = -EINVAL;
+	}
+
+	mutex_unlock(&state->mutex);
+	return ret;
+}
+
 static const struct v4l2_subdev_video_ops adv7180_video_ops = {
 	.querystd = adv7180_querystd,
 	.g_input_status = adv7180_g_input_status,
+	.s_routing = adv7180_s_routing,
 };
 
 static const struct v4l2_subdev_core_ops adv7180_core_ops = {
 	.g_chip_ident = adv7180_g_chip_ident,
 	.s_std = adv7180_s_std,
+	.queryctrl = adv7180_queryctrl,
+	.g_ctrl = adv7180_g_ctrl,
+	.s_ctrl = adv7180_s_ctrl,
 };
 
 static const struct v4l2_subdev_ops adv7180_ops = {
@@ -267,13 +458,13 @@ static const struct v4l2_subdev_ops adv7180_ops = {
 static void adv7180_work(struct work_struct *work)
 {
 	struct adv7180_state *state = container_of(work, struct adv7180_state,
-		work);
+						   work);
 	struct i2c_client *client = v4l2_get_subdevdata(&state->sd);
 	u8 isr3;
 
 	mutex_lock(&state->mutex);
 	i2c_smbus_write_byte_data(client, ADV7180_ADI_CTRL_REG,
-		ADV7180_ADI_CTRL_IRQ_SPACE);
+				  ADV7180_ADI_CTRL_IRQ_SPACE);
 	isr3 = i2c_smbus_read_byte_data(client, ADV7180_ISR3_ADI);
 	/* clear */
 	i2c_smbus_write_byte_data(client, ADV7180_ICR3_ADI, isr3);
@@ -297,56 +488,51 @@ static irqreturn_t adv7180_irq(int irq, void *devid)
 	return IRQ_HANDLED;
 }
 
-/*
- * Generic i2c probe
- * concerning the addresses: i2c wants 7 bit (without the r/w bit), so '>>1'
- */
-
-static __devinit int adv7180_probe(struct i2c_client *client,
-			const struct i2c_device_id *id)
+static int init_device(struct i2c_client *client, struct adv7180_state *state)
 {
-	struct adv7180_state *state;
-	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))
-		return -EIO;
-
-	v4l_info(client, "chip found @ 0x%02x (%s)\n",
-			client->addr << 1, client->adapter->name);
-
-	state = kzalloc(sizeof(struct adv7180_state), GFP_KERNEL);
-	if (state == NULL) {
-		ret = -ENOMEM;
-		goto err;
-	}
-
-	state->irq = client->irq;
-	INIT_WORK(&state->work, adv7180_work);
-	mutex_init(&state->mutex);
-	state->autodetect = true;
-	sd = &state->sd;
-	v4l2_i2c_subdev_init(sd, client, &adv7180_ops);
-
 	/* Initialize adv7180 */
 	/* Enable autodetection */
-	ret = i2c_smbus_write_byte_data(client, ADV7180_INPUT_CONTROL_REG,
-		ADV7180_INPUT_CONTROL_AD_PAL_BG_NTSC_J_SECAM);
-	if (ret < 0)
-		goto err_unreg_subdev;
+	if (state->autodetect) {
+		ret =
+		    i2c_smbus_write_byte_data(client, ADV7180_INPUT_CONTROL_REG,
+				ADV7180_INPUT_CONTROL_AD_PAL_BG_NTSC_J_SECAM
+					      | state->input);
+		if (ret < 0)
+			return ret;
 
-	ret = i2c_smbus_write_byte_data(client, ADV7180_AUTODETECT_ENABLE_REG,
-		ADV7180_AUTODETECT_DEFAULT);
-	if (ret < 0)
-		goto err_unreg_subdev;
+		ret =
+		    i2c_smbus_write_byte_data(client,
+					      ADV7180_AUTODETECT_ENABLE_REG,
+					      ADV7180_AUTODETECT_DEFAULT);
+		if (ret < 0)
+			return ret;
+	} else {
+		ret = v4l2_std_to_adv7180(state->curr_norm);
+		if (ret < 0)
+			return ret;
 
+		ret =
+		    i2c_smbus_write_byte_data(client, ADV7180_INPUT_CONTROL_REG,
+					      ret | state->input);
+		if (ret < 0)
+			return ret;
+
+	}
 	/* ITU-R BT.656-4 compatible */
 	ret = i2c_smbus_write_byte_data(client,
-		ADV7180_EXTENDED_OUTPUT_CONTROL_REG,
-		ADV7180_EXTENDED_OUTPUT_CONTROL_NTSCDIS);
+			ADV7180_EXTENDED_OUTPUT_CONTROL_REG,
+			ADV7180_EXTENDED_OUTPUT_CONTROL_NTSCDIS);
 	if (ret < 0)
-		goto err_unreg_subdev;
+		return ret;
+
+	/* Manually set V bit end position in NTSC mode */
+	ret = i2c_smbus_write_byte_data(client,
+					ADV7180_NTSC_V_BIT_END_REG,
+					ADV7180_NTSC_V_BIT_END_MANUAL_NVEND);
+	if (ret < 0)
+		return ret;
 
 	/* read current norm */
 	__adv7180_status(client, NULL, &state->curr_norm);
@@ -354,45 +540,109 @@ static __devinit int adv7180_probe(struct i2c_client *client,
 	/* register for interrupts */
 	if (state->irq > 0) {
 		ret = request_irq(state->irq, adv7180_irq, 0, DRIVER_NAME,
-			state);
+				  state);
 		if (ret)
-			goto err_unreg_subdev;
+			return ret;
 
 		ret = i2c_smbus_write_byte_data(client, ADV7180_ADI_CTRL_REG,
-			ADV7180_ADI_CTRL_IRQ_SPACE);
+						ADV7180_ADI_CTRL_IRQ_SPACE);
 		if (ret < 0)
-			goto err_unreg_subdev;
+			return ret;
 
 		/* config the Interrupt pin to be active low */
 		ret = i2c_smbus_write_byte_data(client, ADV7180_ICONF1_ADI,
-			ADV7180_ICONF1_ACTIVE_LOW | ADV7180_ICONF1_PSYNC_ONLY);
+						ADV7180_ICONF1_ACTIVE_LOW |
+						ADV7180_ICONF1_PSYNC_ONLY);
 		if (ret < 0)
-			goto err_unreg_subdev;
+			return ret;
 
 		ret = i2c_smbus_write_byte_data(client, ADV7180_IMR1_ADI, 0);
 		if (ret < 0)
-			goto err_unreg_subdev;
+			return ret;
 
 		ret = i2c_smbus_write_byte_data(client, ADV7180_IMR2_ADI, 0);
 		if (ret < 0)
-			goto err_unreg_subdev;
+			return ret;
 
 		/* enable AD change interrupts interrupts */
 		ret = i2c_smbus_write_byte_data(client, ADV7180_IMR3_ADI,
-			ADV7180_IRQ3_AD_CHANGE);
+						ADV7180_IRQ3_AD_CHANGE);
 		if (ret < 0)
-			goto err_unreg_subdev;
+			return ret;
 
 		ret = i2c_smbus_write_byte_data(client, ADV7180_IMR4_ADI, 0);
 		if (ret < 0)
-			goto err_unreg_subdev;
+			return ret;
 
 		ret = i2c_smbus_write_byte_data(client, ADV7180_ADI_CTRL_REG,
-			0);
+						0);
 		if (ret < 0)
-			goto err_unreg_subdev;
+			return ret;
 	}
 
+	/*Set default value for controls */
+	ret = i2c_smbus_write_byte_data(client, ADV7180_BRI_REG,
+					state->brightness);
+	if (ret < 0)
+		return ret;
+
+	ret = i2c_smbus_write_byte_data(client, ADV7180_HUE_REG, state->hue);
+	if (ret < 0)
+		return ret;
+
+	ret = i2c_smbus_write_byte_data(client, ADV7180_CON_REG,
+					state->contrast);
+	if (ret < 0)
+		return ret;
+
+	ret = i2c_smbus_write_byte_data(client, ADV7180_SD_SAT_CB_REG,
+					state->saturation);
+	if (ret < 0)
+		return ret;
+
+	ret = i2c_smbus_write_byte_data(client, ADV7180_SD_SAT_CR_REG,
+					state->saturation);
+	if (ret < 0)
+		return ret;
+
+	return 0;
+}
+
+static __devinit int adv7180_probe(struct i2c_client *client,
+				   const struct i2c_device_id *id)
+{
+	struct adv7180_state *state;
+	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))
+		return -EIO;
+
+	v4l_info(client, "chip found @ 0x%02x (%s)\n",
+		 client->addr, client->adapter->name);
+
+	state = kzalloc(sizeof(struct adv7180_state), GFP_KERNEL);
+	if (state == NULL) {
+		ret = -ENOMEM;
+		goto err;
+	}
+
+	state->irq = client->irq;
+	INIT_WORK(&state->work, adv7180_work);
+	mutex_init(&state->mutex);
+	state->autodetect = true;
+	state->brightness = BRI_REG_DEF;
+	state->hue = HUE_REG_DEF;
+	state->contrast = CON_REG_DEF;
+	state->saturation = SAT_REG_DEF;
+	state->input = 0;
+	sd = &state->sd;
+	v4l2_i2c_subdev_init(sd, client, &adv7180_ops);
+
+	ret = init_device(client, state);
+	if (0 != ret)
+		goto err_unreg_subdev;
 	return 0;
 
 err_unreg_subdev:
@@ -432,16 +682,49 @@ static const struct i2c_device_id adv7180_id[] = {
 	{},
 };
 
+#ifdef CONFIG_PM
+static int adv7180_suspend(struct i2c_client *client, pm_message_t state)
+{
+	int ret;
+
+	ret = i2c_smbus_write_byte_data(client, ADV7180_PWR_MAN_REG,
+					ADV7180_PWR_MAN_OFF);
+	if (ret < 0)
+		return ret;
+	return 0;
+}
+
+static int adv7180_resume(struct i2c_client *client)
+{
+	struct v4l2_subdev *sd = i2c_get_clientdata(client);
+	struct adv7180_state *state = to_state(sd);
+	int ret;
+
+	ret = i2c_smbus_write_byte_data(client, ADV7180_PWR_MAN_REG,
+					ADV7180_PWR_MAN_ON);
+	if (ret < 0)
+		return ret;
+	ret = init_device(client, state);
+	if (ret < 0)
+		return ret;
+	return 0;
+}
+#endif
+
 MODULE_DEVICE_TABLE(i2c, adv7180_id);
 
 static struct i2c_driver adv7180_driver = {
 	.driver = {
-		.owner	= THIS_MODULE,
-		.name	= DRIVER_NAME,
-	},
-	.probe		= adv7180_probe,
-	.remove		= __devexit_p(adv7180_remove),
-	.id_table	= adv7180_id,
+		   .owner = THIS_MODULE,
+		   .name = DRIVER_NAME,
+		   },
+	.probe = adv7180_probe,
+	.remove = __devexit_p(adv7180_remove),
+#ifdef CONFIG_PM
+	.suspend = adv7180_suspend,
+	.resume = adv7180_resume,
+#endif
+	.id_table = adv7180_id,
 };
 
 module_i2c_driver(adv7180_driver);
diff --git a/drivers/media/video/adv7343.c b/drivers/media/video/adv7343.c
index 119b60401bf..2b5aa676a84 100644
--- a/drivers/media/video/adv7343.c
+++ b/drivers/media/video/adv7343.c
@@ -130,14 +130,12 @@ static int adv7343_setstd(struct v4l2_subdev *sd, v4l2_std_id std)
 {
 	struct adv7343_state *state = to_state(sd);
 	struct adv7343_std_info *std_info;
-	int output_idx, num_std;
+	int num_std;
 	char *fsc_ptr;
 	u8 reg, val;
 	int err = 0;
 	int i = 0;
 
-	output_idx = state->output;
-
 	std_info = (struct adv7343_std_info *)stdinfo;
 	num_std = ARRAY_SIZE(stdinfo);
 
diff --git a/drivers/media/video/aptina-pll.c b/drivers/media/video/aptina-pll.c
index 0bd3813bb59..8153a449846 100644
--- a/drivers/media/video/aptina-pll.c
+++ b/drivers/media/video/aptina-pll.c
@@ -148,9 +148,8 @@ int aptina_pll_calculate(struct device *dev,
 		unsigned int mf_high;
 		unsigned int mf_low;
 
-		mf_low = max(roundup(mf_min, mf_inc),
-			     DIV_ROUND_UP(pll->ext_clock * p1,
-			       limits->int_clock_max * div));
+		mf_low = roundup(max(mf_min, DIV_ROUND_UP(pll->ext_clock * p1,
+					limits->int_clock_max * div)), mf_inc);
 		mf_high = min(mf_max, pll->ext_clock * p1 /
 			      (limits->int_clock_min * div));
 
diff --git a/drivers/media/video/arv.c b/drivers/media/video/arv.c
index b6ed44aebe3..e346d32d08c 100644
--- a/drivers/media/video/arv.c
+++ b/drivers/media/video/arv.c
@@ -31,6 +31,7 @@
 #include <media/v4l2-common.h>
 #include <media/v4l2-device.h>
 #include <media/v4l2-ioctl.h>
+#include <media/v4l2-fh.h>
 #include <linux/mutex.h>
 
 #include <asm/uaccess.h>
@@ -403,7 +404,8 @@ static int ar_querycap(struct file *file, void  *priv,
 	strlcpy(vcap->driver, ar->vdev.name, sizeof(vcap->driver));
 	strlcpy(vcap->card, "Colour AR VGA", sizeof(vcap->card));
 	strlcpy(vcap->bus_info, "Platform", sizeof(vcap->bus_info));
-	vcap->capabilities = V4L2_CAP_VIDEO_CAPTURE | V4L2_CAP_READWRITE;
+	vcap->device_caps = V4L2_CAP_VIDEO_CAPTURE | V4L2_CAP_READWRITE;
+	vcap->capabilities = vcap->device_caps | V4L2_CAP_DEVICE_CAPS;
 	return 0;
 }
 
@@ -709,6 +711,8 @@ static int ar_initialize(struct ar *ar)
 
 static const struct v4l2_file_operations ar_fops = {
 	.owner		= THIS_MODULE,
+	.open		= v4l2_fh_open,
+	.release	= v4l2_fh_release,
 	.read		= ar_read,
 	.unlocked_ioctl	= video_ioctl2,
 };
@@ -769,6 +773,7 @@ static int __init ar_init(void)
 	ar->vdev.fops = &ar_fops;
 	ar->vdev.ioctl_ops = &ar_ioctl_ops;
 	ar->vdev.release = video_device_release_empty;
+	set_bit(V4L2_FL_USE_FH_PRIO, &ar->vdev.flags);
 	video_set_drvdata(&ar->vdev, ar);
 
 	if (vga) {
diff --git a/drivers/media/video/as3645a.c b/drivers/media/video/as3645a.c
index 7a3371f044f..c4b03572dce 100644
--- a/drivers/media/video/as3645a.c
+++ b/drivers/media/video/as3645a.c
@@ -713,7 +713,7 @@ static int as3645a_resume(struct device *dev)
  * The number of LEDs reported in platform data is used to compute default
  * limits. Parameters passed through platform data can override those limits.
  */
-static int as3645a_init_controls(struct as3645a *flash)
+static int __devinit as3645a_init_controls(struct as3645a *flash)
 {
 	const struct as3645a_platform_data *pdata = flash->pdata;
 	struct v4l2_ctrl *ctrl;
@@ -804,8 +804,8 @@ static int as3645a_init_controls(struct as3645a *flash)
 	return flash->ctrls.error;
 }
 
-static int as3645a_probe(struct i2c_client *client,
-			 const struct i2c_device_id *devid)
+static int __devinit as3645a_probe(struct i2c_client *client,
+				   const struct i2c_device_id *devid)
 {
 	struct as3645a *flash;
 	int ret;
@@ -846,7 +846,7 @@ done:
 	return ret;
 }
 
-static int __exit as3645a_remove(struct i2c_client *client)
+static int __devexit as3645a_remove(struct i2c_client *client)
 {
 	struct v4l2_subdev *subdev = i2c_get_clientdata(client);
 	struct as3645a *flash = to_as3645a(subdev);
@@ -877,7 +877,7 @@ static struct i2c_driver as3645a_i2c_driver = {
 		.pm   = &as3645a_pm_ops,
 	},
 	.probe	= as3645a_probe,
-	.remove	= __exit_p(as3645a_remove),
+	.remove	= __devexit_p(as3645a_remove),
 	.id_table = as3645a_id_table,
 };
 
diff --git a/drivers/media/video/atmel-isi.c b/drivers/media/video/atmel-isi.c
index ec3f6a06f9c..6274a91c25c 100644
--- a/drivers/media/video/atmel-isi.c
+++ b/drivers/media/video/atmel-isi.c
@@ -260,7 +260,7 @@ static int queue_setup(struct vb2_queue *vq, const struct v4l2_format *fmt,
 	struct soc_camera_host *ici = to_soc_camera_host(icd->parent);
 	struct atmel_isi *isi = ici->priv;
 	unsigned long size;
-	int ret, bytes_per_line;
+	int ret;
 
 	/* Reset ISI */
 	ret = atmel_isi_wait_status(isi, WAIT_ISI_RESET);
@@ -271,13 +271,7 @@ static int queue_setup(struct vb2_queue *vq, const struct v4l2_format *fmt,
 	/* Disable all interrupts */
 	isi_writel(isi, ISI_INTDIS, ~0UL);
 
-	bytes_per_line = soc_mbus_bytes_per_line(icd->user_width,
-						icd->current_fmt->host_fmt);
-
-	if (bytes_per_line < 0)
-		return bytes_per_line;
-
-	size = bytes_per_line * icd->user_height;
+	size = icd->sizeimage;
 
 	if (!*nbuffers || *nbuffers > MAX_BUFFER_NUM)
 		*nbuffers = MAX_BUFFER_NUM;
@@ -316,13 +310,8 @@ static int buffer_prepare(struct vb2_buffer *vb)
 	struct atmel_isi *isi = ici->priv;
 	unsigned long size;
 	struct isi_dma_desc *desc;
-	int bytes_per_line = soc_mbus_bytes_per_line(icd->user_width,
-						icd->current_fmt->host_fmt);
-
-	if (bytes_per_line < 0)
-		return bytes_per_line;
 
-	size = bytes_per_line * icd->user_height;
+	size = icd->sizeimage;
 
 	if (vb2_plane_size(vb, 0) < size) {
 		dev_err(icd->parent, "%s data will not fit into plane (%lu < %lu)\n",
@@ -638,6 +627,7 @@ static const struct soc_mbus_pixelfmt isi_camera_formats[] = {
 		.bits_per_sample	= 8,
 		.packing		= SOC_MBUS_PACKING_2X8_PADHI,
 		.order			= SOC_MBUS_ORDER_LE,
+		.layout			= SOC_MBUS_LAYOUT_PACKED,
 	},
 };
 
diff --git a/drivers/media/video/au0828/Kconfig b/drivers/media/video/au0828/Kconfig
index 81ba9d9d1b5..23f7fd22f0e 100644
--- a/drivers/media/video/au0828/Kconfig
+++ b/drivers/media/video/au0828/Kconfig
@@ -6,7 +6,8 @@ config VIDEO_AU0828
 	select I2C_ALGOBIT
 	select VIDEO_TVEEPROM
 	select VIDEOBUF_VMALLOC
-	select DVB_AU8522 if !DVB_FE_CUSTOMISE
+	select DVB_AU8522_DTV if !DVB_FE_CUSTOMISE
+	select DVB_AU8522_V4L if !DVB_FE_CUSTOMISE
 	select MEDIA_TUNER_XC5000 if !MEDIA_TUNER_CUSTOMISE
 	select MEDIA_TUNER_MXL5007T if !MEDIA_TUNER_CUSTOMISE
 	select MEDIA_TUNER_TDA18271 if !MEDIA_TUNER_CUSTOMISE
diff --git a/drivers/media/video/au0828/au0828-cards.c b/drivers/media/video/au0828/au0828-cards.c
index 1c6015a04f9..e3fe9a6637f 100644
--- a/drivers/media/video/au0828/au0828-cards.c
+++ b/drivers/media/video/au0828/au0828-cards.c
@@ -325,6 +325,8 @@ struct usb_device_id au0828_usb_id_table[] = {
 		.driver_info = AU0828_BOARD_HAUPPAUGE_HVR950Q_MXL },
 	{ USB_DEVICE(0x2040, 0x7281),
 		.driver_info = AU0828_BOARD_HAUPPAUGE_HVR950Q_MXL },
+	{ USB_DEVICE(0x05e1, 0x0480),
+		.driver_info = AU0828_BOARD_HAUPPAUGE_WOODBURY },
 	{ USB_DEVICE(0x2040, 0x8200),
 		.driver_info = AU0828_BOARD_HAUPPAUGE_WOODBURY },
 	{ USB_DEVICE(0x2040, 0x7260),
diff --git a/drivers/media/video/au0828/au0828-dvb.c b/drivers/media/video/au0828/au0828-dvb.c
index 518216743c9..39ece8e2498 100644
--- a/drivers/media/video/au0828/au0828-dvb.c
+++ b/drivers/media/video/au0828/au0828-dvb.c
@@ -25,6 +25,7 @@
 #include <linux/device.h>
 #include <linux/suspend.h>
 #include <media/v4l2-common.h>
+#include <media/tuner.h>
 
 #include "au0828.h"
 #include "au8522.h"
@@ -79,9 +80,16 @@ static struct au8522_config hauppauge_woodbury_config = {
 	.vsb_if        = AU8522_IF_3_25MHZ,
 };
 
-static struct xc5000_config hauppauge_hvr950q_tunerconfig = {
+static struct xc5000_config hauppauge_xc5000a_config = {
 	.i2c_address      = 0x61,
 	.if_khz           = 6000,
+	.chip_id          = XC5000A,
+};
+
+static struct xc5000_config hauppauge_xc5000c_config = {
+	.i2c_address      = 0x61,
+	.if_khz           = 6000,
+	.chip_id          = XC5000C,
 };
 
 static struct mxl5007t_config mxl5007t_hvr950q_config = {
@@ -383,8 +391,19 @@ int au0828_dvb_register(struct au0828_dev *dev)
 				&hauppauge_hvr950q_config,
 				&dev->i2c_adap);
 		if (dvb->frontend != NULL)
-			dvb_attach(xc5000_attach, dvb->frontend, &dev->i2c_adap,
-				   &hauppauge_hvr950q_tunerconfig);
+			switch (dev->board.tuner_type) {
+			default:
+			case TUNER_XC5000:
+				dvb_attach(xc5000_attach, dvb->frontend,
+					   &dev->i2c_adap,
+					   &hauppauge_xc5000a_config);
+				break;
+			case TUNER_XC5000C:
+				dvb_attach(xc5000_attach, dvb->frontend,
+					   &dev->i2c_adap,
+					   &hauppauge_xc5000c_config);
+				break;
+			}
 		break;
 	case AU0828_BOARD_HAUPPAUGE_HVR950Q_MXL:
 		dvb->frontend = dvb_attach(au8522_attach,
@@ -411,7 +430,7 @@ int au0828_dvb_register(struct au0828_dev *dev)
 		if (dvb->frontend != NULL) {
 			dvb_attach(xc5000_attach, dvb->frontend,
 				&dev->i2c_adap,
-				&hauppauge_hvr950q_tunerconfig);
+				&hauppauge_xc5000a_config);
 		}
 		break;
 	default:
diff --git a/drivers/media/video/au0828/au0828-video.c b/drivers/media/video/au0828/au0828-video.c
index 0b3e481ffe8..ac3dd733ab8 100644
--- a/drivers/media/video/au0828/au0828-video.c
+++ b/drivers/media/video/au0828/au0828-video.c
@@ -120,7 +120,7 @@ static void au0828_irq_callback(struct urb *urb)
 	struct au0828_dmaqueue  *dma_q = urb->context;
 	struct au0828_dev *dev = container_of(dma_q, struct au0828_dev, vidq);
 	unsigned long flags = 0;
-	int rc, i;
+	int i;
 
 	switch (urb->status) {
 	case 0:             /* success */
@@ -138,7 +138,7 @@ static void au0828_irq_callback(struct urb *urb)
 
 	/* Copy data from URB */
 	spin_lock_irqsave(&dev->slock, flags);
-	rc = dev->isoc_ctl.isoc_copy(dev, urb);
+	dev->isoc_ctl.isoc_copy(dev, urb);
 	spin_unlock_irqrestore(&dev->slock, flags);
 
 	/* Reset urb buffers */
@@ -1881,7 +1881,7 @@ int au0828_analog_register(struct au0828_dev *dev,
 	int retval = -ENOMEM;
 	struct usb_host_interface *iface_desc;
 	struct usb_endpoint_descriptor *endpoint;
-	int i;
+	int i, ret;
 
 	dprintk(1, "au0828_analog_register called!\n");
 
@@ -1951,8 +1951,8 @@ int au0828_analog_register(struct au0828_dev *dev,
 	dev->vbi_dev = video_device_alloc();
 	if (NULL == dev->vbi_dev) {
 		dprintk(1, "Can't allocate vbi_device.\n");
-		kfree(dev->vdev);
-		return -ENOMEM;
+		ret = -ENOMEM;
+		goto err_vdev;
 	}
 
 	/* Fill the video capture device struct */
@@ -1971,8 +1971,8 @@ int au0828_analog_register(struct au0828_dev *dev,
 	if (retval != 0) {
 		dprintk(1, "unable to register video device (error = %d).\n",
 			retval);
-		video_device_release(dev->vdev);
-		return -ENODEV;
+		ret = -ENODEV;
+		goto err_vbi_dev;
 	}
 
 	/* Register the vbi device */
@@ -1981,13 +1981,18 @@ int au0828_analog_register(struct au0828_dev *dev,
 	if (retval != 0) {
 		dprintk(1, "unable to register vbi device (error = %d).\n",
 			retval);
-		video_device_release(dev->vbi_dev);
-		video_device_release(dev->vdev);
-		return -ENODEV;
+		ret = -ENODEV;
+		goto err_vbi_dev;
 	}
 
 	dprintk(1, "%s completed!\n", __func__);
 
 	return 0;
+
+err_vbi_dev:
+	video_device_release(dev->vbi_dev);
+err_vdev:
+	video_device_release(dev->vdev);
+	return ret;
 }
 
diff --git a/drivers/media/video/blackfin/bfin_capture.c b/drivers/media/video/blackfin/bfin_capture.c
index 514fcf742f5..0aba45e34f7 100644
--- a/drivers/media/video/blackfin/bfin_capture.c
+++ b/drivers/media/video/blackfin/bfin_capture.c
@@ -942,6 +942,10 @@ static int __devinit bcap_probe(struct platform_device *pdev)
 	INIT_LIST_HEAD(&bcap_dev->dma_queue);
 
 	vfd->lock = &bcap_dev->mutex;
+	/* Locking in file operations other than ioctl should be done
+	   by the driver, not the V4L2 core.
+	   This driver needs auditing so that this flag can be removed. */
+	set_bit(V4L2_FL_LOCK_ALL_FOPS, &vfd->flags);
 
 	/* register video device */
 	ret = video_register_device(bcap_dev->video_dev, VFL_TYPE_GRABBER, -1);
diff --git a/drivers/media/video/bt8xx/bttv-driver.c b/drivers/media/video/bt8xx/bttv-driver.c
index e581b37be78..a9cfb0f4be4 100644
--- a/drivers/media/video/bt8xx/bttv-driver.c
+++ b/drivers/media/video/bt8xx/bttv-driver.c
@@ -663,7 +663,7 @@ static const struct v4l2_queryctrl bttv_ctls[] = {
 		.minimum       = 0,
 		.maximum       = 65535,
 		.step          = 128,
-		.default_value = 32768,
+		.default_value = 27648,
 		.type          = V4L2_CTRL_TYPE_INTEGER,
 	},{
 		.id            = V4L2_CID_SATURATION,
@@ -4394,7 +4394,7 @@ static int __devinit bttv_probe(struct pci_dev *dev,
 	if (!bttv_tvcards[btv->c.type].no_video) {
 		bttv_register_video(btv);
 		bt848_bright(btv,32768);
-		bt848_contrast(btv,32768);
+		bt848_contrast(btv, 27648);
 		bt848_hue(btv,32768);
 		bt848_sat(btv,32768);
 		audio_mute(btv, 1);
diff --git a/drivers/media/video/bw-qcam.c b/drivers/media/video/bw-qcam.c
index f09df9dffaa..2520219f01b 100644
--- a/drivers/media/video/bw-qcam.c
+++ b/drivers/media/video/bw-qcam.c
@@ -77,6 +77,9 @@ OTHER DEALINGS IN THE SOFTWARE.
 #include <media/v4l2-common.h>
 #include <media/v4l2-ioctl.h>
 #include <media/v4l2-device.h>
+#include <media/v4l2-fh.h>
+#include <media/v4l2-ctrls.h>
+#include <media/v4l2-event.h>
 
 /* One from column A... */
 #define QC_NOTSET 0
@@ -103,6 +106,7 @@ OTHER DEALINGS IN THE SOFTWARE.
 struct qcam {
 	struct v4l2_device v4l2_dev;
 	struct video_device vdev;
+	struct v4l2_ctrl_handler hdl;
 	struct pardevice *pdev;
 	struct parport *pport;
 	struct mutex lock;
@@ -646,7 +650,8 @@ static int qcam_querycap(struct file *file, void  *priv,
 	strlcpy(vcap->driver, qcam->v4l2_dev.name, sizeof(vcap->driver));
 	strlcpy(vcap->card, "B&W Quickcam", sizeof(vcap->card));
 	strlcpy(vcap->bus_info, "parport", sizeof(vcap->bus_info));
-	vcap->capabilities = V4L2_CAP_VIDEO_CAPTURE | V4L2_CAP_READWRITE;
+	vcap->device_caps = V4L2_CAP_VIDEO_CAPTURE | V4L2_CAP_READWRITE;
+	vcap->capabilities = vcap->device_caps | V4L2_CAP_DEVICE_CAPS;
 	return 0;
 }
 
@@ -674,72 +679,6 @@ static int qcam_s_input(struct file *file, void *fh, unsigned int inp)
 	return (inp > 0) ? -EINVAL : 0;
 }
 
-static int qcam_queryctrl(struct file *file, void *priv,
-					struct v4l2_queryctrl *qc)
-{
-	switch (qc->id) {
-	case V4L2_CID_BRIGHTNESS:
-		return v4l2_ctrl_query_fill(qc, 0, 255, 1, 180);
-	case V4L2_CID_CONTRAST:
-		return v4l2_ctrl_query_fill(qc, 0, 255, 1, 192);
-	case V4L2_CID_GAMMA:
-		return v4l2_ctrl_query_fill(qc, 0, 255, 1, 105);
-	}
-	return -EINVAL;
-}
-
-static int qcam_g_ctrl(struct file *file, void *priv,
-					struct v4l2_control *ctrl)
-{
-	struct qcam *qcam = video_drvdata(file);
-	int ret = 0;
-
-	switch (ctrl->id) {
-	case V4L2_CID_BRIGHTNESS:
-		ctrl->value = qcam->brightness;
-		break;
-	case V4L2_CID_CONTRAST:
-		ctrl->value = qcam->contrast;
-		break;
-	case V4L2_CID_GAMMA:
-		ctrl->value = qcam->whitebal;
-		break;
-	default:
-		ret = -EINVAL;
-		break;
-	}
-	return ret;
-}
-
-static int qcam_s_ctrl(struct file *file, void *priv,
-					struct v4l2_control *ctrl)
-{
-	struct qcam *qcam = video_drvdata(file);
-	int ret = 0;
-
-	mutex_lock(&qcam->lock);
-	switch (ctrl->id) {
-	case V4L2_CID_BRIGHTNESS:
-		qcam->brightness = ctrl->value;
-		break;
-	case V4L2_CID_CONTRAST:
-		qcam->contrast = ctrl->value;
-		break;
-	case V4L2_CID_GAMMA:
-		qcam->whitebal = ctrl->value;
-		break;
-	default:
-		ret = -EINVAL;
-		break;
-	}
-	if (ret == 0) {
-		qc_setscanmode(qcam);
-		qcam->status |= QC_PARAM_CHANGE;
-	}
-	mutex_unlock(&qcam->lock);
-	return ret;
-}
-
 static int qcam_g_fmt_vid_cap(struct file *file, void *fh, struct v4l2_format *fmt)
 {
 	struct qcam *qcam = video_drvdata(file);
@@ -856,8 +795,40 @@ static ssize_t qcam_read(struct file *file, char __user *buf,
 	return len;
 }
 
+static int qcam_s_ctrl(struct v4l2_ctrl *ctrl)
+{
+	struct qcam *qcam =
+		container_of(ctrl->handler, struct qcam, hdl);
+	int ret = 0;
+
+	mutex_lock(&qcam->lock);
+	switch (ctrl->id) {
+	case V4L2_CID_BRIGHTNESS:
+		qcam->brightness = ctrl->val;
+		break;
+	case V4L2_CID_CONTRAST:
+		qcam->contrast = ctrl->val;
+		break;
+	case V4L2_CID_GAMMA:
+		qcam->whitebal = ctrl->val;
+		break;
+	default:
+		ret = -EINVAL;
+		break;
+	}
+	if (ret == 0) {
+		qc_setscanmode(qcam);
+		qcam->status |= QC_PARAM_CHANGE;
+	}
+	mutex_unlock(&qcam->lock);
+	return ret;
+}
+
 static const struct v4l2_file_operations qcam_fops = {
 	.owner		= THIS_MODULE,
+	.open		= v4l2_fh_open,
+	.release	= v4l2_fh_release,
+	.poll		= v4l2_ctrl_poll,
 	.unlocked_ioctl = video_ioctl2,
 	.read		= qcam_read,
 };
@@ -867,13 +838,17 @@ static const struct v4l2_ioctl_ops qcam_ioctl_ops = {
 	.vidioc_g_input      		    = qcam_g_input,
 	.vidioc_s_input      		    = qcam_s_input,
 	.vidioc_enum_input   		    = qcam_enum_input,
-	.vidioc_queryctrl 		    = qcam_queryctrl,
-	.vidioc_g_ctrl  		    = qcam_g_ctrl,
-	.vidioc_s_ctrl 			    = qcam_s_ctrl,
 	.vidioc_enum_fmt_vid_cap 	    = qcam_enum_fmt_vid_cap,
 	.vidioc_g_fmt_vid_cap 		    = qcam_g_fmt_vid_cap,
 	.vidioc_s_fmt_vid_cap  		    = qcam_s_fmt_vid_cap,
 	.vidioc_try_fmt_vid_cap  	    = qcam_try_fmt_vid_cap,
+	.vidioc_log_status		    = v4l2_ctrl_log_status,
+	.vidioc_subscribe_event		    = v4l2_ctrl_subscribe_event,
+	.vidioc_unsubscribe_event	    = v4l2_event_unsubscribe,
+};
+
+static const struct v4l2_ctrl_ops qcam_ctrl_ops = {
+	.s_ctrl = qcam_s_ctrl,
 };
 
 /* Initialize the QuickCam driver control structure.  This is where
@@ -897,19 +872,35 @@ static struct qcam *qcam_init(struct parport *port)
 		return NULL;
 	}
 
+	v4l2_ctrl_handler_init(&qcam->hdl, 3);
+	v4l2_ctrl_new_std(&qcam->hdl, &qcam_ctrl_ops,
+			  V4L2_CID_BRIGHTNESS, 0, 255, 1, 180);
+	v4l2_ctrl_new_std(&qcam->hdl, &qcam_ctrl_ops,
+			  V4L2_CID_CONTRAST, 0, 255, 1, 192);
+	v4l2_ctrl_new_std(&qcam->hdl, &qcam_ctrl_ops,
+			  V4L2_CID_GAMMA, 0, 255, 1, 105);
+	if (qcam->hdl.error) {
+		v4l2_err(v4l2_dev, "couldn't register controls\n");
+		v4l2_ctrl_handler_free(&qcam->hdl);
+		kfree(qcam);
+		return NULL;
+	}
 	qcam->pport = port;
 	qcam->pdev = parport_register_device(port, "bw-qcam", NULL, NULL,
 			NULL, 0, NULL);
 	if (qcam->pdev == NULL) {
 		v4l2_err(v4l2_dev, "couldn't register for %s.\n", port->name);
+		v4l2_ctrl_handler_free(&qcam->hdl);
 		kfree(qcam);
 		return NULL;
 	}
 
 	strlcpy(qcam->vdev.name, "Connectix QuickCam", sizeof(qcam->vdev.name));
 	qcam->vdev.v4l2_dev = v4l2_dev;
+	qcam->vdev.ctrl_handler = &qcam->hdl;
 	qcam->vdev.fops = &qcam_fops;
 	qcam->vdev.ioctl_ops = &qcam_ioctl_ops;
+	set_bit(V4L2_FL_USE_FH_PRIO, &qcam->vdev.flags);
 	qcam->vdev.release = video_device_release_empty;
 	video_set_drvdata(&qcam->vdev, qcam);
 
@@ -1003,6 +994,7 @@ static int init_bwqcam(struct parport *port)
 static void close_bwqcam(struct qcam *qcam)
 {
 	video_unregister_device(&qcam->vdev);
+	v4l2_ctrl_handler_free(&qcam->hdl);
 	parport_unregister_device(qcam->pdev);
 	kfree(qcam);
 }
diff --git a/drivers/media/video/c-qcam.c b/drivers/media/video/c-qcam.c
index fda32f52554..ec51e1f12e8 100644
--- a/drivers/media/video/c-qcam.c
+++ b/drivers/media/video/c-qcam.c
@@ -40,10 +40,14 @@
 #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>
 
 struct qcam {
 	struct v4l2_device v4l2_dev;
 	struct video_device vdev;
+	struct v4l2_ctrl_handler hdl;
 	struct pardevice *pdev;
 	struct parport *pport;
 	int width, height;
@@ -378,7 +382,7 @@ get_fragment:
 static long qc_capture(struct qcam *qcam, char __user *buf, unsigned long len)
 {
 	struct v4l2_device *v4l2_dev = &qcam->v4l2_dev;
-	unsigned lines, pixelsperline, bitsperxfer;
+	unsigned lines, pixelsperline;
 	unsigned int is_bi_dir = qcam->bidirectional;
 	size_t wantlen, outptr = 0;
 	char tmpbuf[BUFSZ];
@@ -404,7 +408,6 @@ static long qc_capture(struct qcam *qcam, char __user *buf, unsigned long len)
 
 	lines = qcam->height;
 	pixelsperline = qcam->width;
-	bitsperxfer = (is_bi_dir) ? 24 : 8;
 
 	if (is_bi_dir) {
 		/* Turn the port around */
@@ -516,7 +519,8 @@ static int qcam_querycap(struct file *file, void  *priv,
 	strlcpy(vcap->driver, qcam->v4l2_dev.name, sizeof(vcap->driver));
 	strlcpy(vcap->card, "Color Quickcam", sizeof(vcap->card));
 	strlcpy(vcap->bus_info, "parport", sizeof(vcap->bus_info));
-	vcap->capabilities = V4L2_CAP_VIDEO_CAPTURE | V4L2_CAP_READWRITE;
+	vcap->device_caps = V4L2_CAP_VIDEO_CAPTURE | V4L2_CAP_READWRITE;
+	vcap->capabilities = vcap->device_caps | V4L2_CAP_DEVICE_CAPS;
 	return 0;
 }
 
@@ -544,73 +548,6 @@ static int qcam_s_input(struct file *file, void *fh, unsigned int inp)
 	return (inp > 0) ? -EINVAL : 0;
 }
 
-static int qcam_queryctrl(struct file *file, void *priv,
-					struct v4l2_queryctrl *qc)
-{
-	switch (qc->id) {
-	case V4L2_CID_BRIGHTNESS:
-		return v4l2_ctrl_query_fill(qc, 0, 255, 1, 240);
-	case V4L2_CID_CONTRAST:
-		return v4l2_ctrl_query_fill(qc, 0, 255, 1, 192);
-	case V4L2_CID_GAMMA:
-		return v4l2_ctrl_query_fill(qc, 0, 255, 1, 128);
-	}
-	return -EINVAL;
-}
-
-static int qcam_g_ctrl(struct file *file, void *priv,
-					struct v4l2_control *ctrl)
-{
-	struct qcam *qcam = video_drvdata(file);
-	int ret = 0;
-
-	switch (ctrl->id) {
-	case V4L2_CID_BRIGHTNESS:
-		ctrl->value = qcam->brightness;
-		break;
-	case V4L2_CID_CONTRAST:
-		ctrl->value = qcam->contrast;
-		break;
-	case V4L2_CID_GAMMA:
-		ctrl->value = qcam->whitebal;
-		break;
-	default:
-		ret = -EINVAL;
-		break;
-	}
-	return ret;
-}
-
-static int qcam_s_ctrl(struct file *file, void *priv,
-					struct v4l2_control *ctrl)
-{
-	struct qcam *qcam = video_drvdata(file);
-	int ret = 0;
-
-	mutex_lock(&qcam->lock);
-	switch (ctrl->id) {
-	case V4L2_CID_BRIGHTNESS:
-		qcam->brightness = ctrl->value;
-		break;
-	case V4L2_CID_CONTRAST:
-		qcam->contrast = ctrl->value;
-		break;
-	case V4L2_CID_GAMMA:
-		qcam->whitebal = ctrl->value;
-		break;
-	default:
-		ret = -EINVAL;
-		break;
-	}
-	if (ret == 0) {
-		parport_claim_or_block(qcam->pdev);
-		qc_setup(qcam);
-		parport_release(qcam->pdev);
-	}
-	mutex_unlock(&qcam->lock);
-	return ret;
-}
-
 static int qcam_g_fmt_vid_cap(struct file *file, void *fh, struct v4l2_format *fmt)
 {
 	struct qcam *qcam = video_drvdata(file);
@@ -714,8 +651,41 @@ static ssize_t qcam_read(struct file *file, char __user *buf,
 	return len;
 }
 
+static int qcam_s_ctrl(struct v4l2_ctrl *ctrl)
+{
+	struct qcam *qcam =
+		container_of(ctrl->handler, struct qcam, hdl);
+	int ret = 0;
+
+	mutex_lock(&qcam->lock);
+	switch (ctrl->id) {
+	case V4L2_CID_BRIGHTNESS:
+		qcam->brightness = ctrl->val;
+		break;
+	case V4L2_CID_CONTRAST:
+		qcam->contrast = ctrl->val;
+		break;
+	case V4L2_CID_GAMMA:
+		qcam->whitebal = ctrl->val;
+		break;
+	default:
+		ret = -EINVAL;
+		break;
+	}
+	if (ret == 0) {
+		parport_claim_or_block(qcam->pdev);
+		qc_setup(qcam);
+		parport_release(qcam->pdev);
+	}
+	mutex_unlock(&qcam->lock);
+	return ret;
+}
+
 static const struct v4l2_file_operations qcam_fops = {
 	.owner		= THIS_MODULE,
+	.open		= v4l2_fh_open,
+	.release	= v4l2_fh_release,
+	.poll		= v4l2_ctrl_poll,
 	.unlocked_ioctl	= video_ioctl2,
 	.read		= qcam_read,
 };
@@ -725,13 +695,17 @@ static const struct v4l2_ioctl_ops qcam_ioctl_ops = {
 	.vidioc_g_input      		    = qcam_g_input,
 	.vidioc_s_input      		    = qcam_s_input,
 	.vidioc_enum_input   		    = qcam_enum_input,
-	.vidioc_queryctrl 		    = qcam_queryctrl,
-	.vidioc_g_ctrl  		    = qcam_g_ctrl,
-	.vidioc_s_ctrl 			    = qcam_s_ctrl,
-	.vidioc_enum_fmt_vid_cap 	    = qcam_enum_fmt_vid_cap,
+	.vidioc_enum_fmt_vid_cap	    = qcam_enum_fmt_vid_cap,
 	.vidioc_g_fmt_vid_cap 		    = qcam_g_fmt_vid_cap,
 	.vidioc_s_fmt_vid_cap  		    = qcam_s_fmt_vid_cap,
 	.vidioc_try_fmt_vid_cap  	    = qcam_try_fmt_vid_cap,
+	.vidioc_log_status		    = v4l2_ctrl_log_status,
+	.vidioc_subscribe_event		    = v4l2_ctrl_subscribe_event,
+	.vidioc_unsubscribe_event	    = v4l2_event_unsubscribe,
+};
+
+static const struct v4l2_ctrl_ops qcam_ctrl_ops = {
+	.s_ctrl = qcam_s_ctrl,
 };
 
 /* Initialize the QuickCam driver control structure. */
@@ -754,6 +728,20 @@ static struct qcam *qcam_init(struct parport *port)
 		return NULL;
 	}
 
+	v4l2_ctrl_handler_init(&qcam->hdl, 3);
+	v4l2_ctrl_new_std(&qcam->hdl, &qcam_ctrl_ops,
+			  V4L2_CID_BRIGHTNESS, 0, 255, 1, 240);
+	v4l2_ctrl_new_std(&qcam->hdl, &qcam_ctrl_ops,
+			  V4L2_CID_CONTRAST, 0, 255, 1, 192);
+	v4l2_ctrl_new_std(&qcam->hdl, &qcam_ctrl_ops,
+			  V4L2_CID_GAMMA, 0, 255, 1, 128);
+	if (qcam->hdl.error) {
+		v4l2_err(v4l2_dev, "couldn't register controls\n");
+		v4l2_ctrl_handler_free(&qcam->hdl);
+		kfree(qcam);
+		return NULL;
+	}
+
 	qcam->pport = port;
 	qcam->pdev = parport_register_device(port, "c-qcam", NULL, NULL,
 					  NULL, 0, NULL);
@@ -762,6 +750,7 @@ static struct qcam *qcam_init(struct parport *port)
 
 	if (qcam->pdev == NULL) {
 		v4l2_err(v4l2_dev, "couldn't register for %s.\n", port->name);
+		v4l2_ctrl_handler_free(&qcam->hdl);
 		kfree(qcam);
 		return NULL;
 	}
@@ -771,6 +760,8 @@ static struct qcam *qcam_init(struct parport *port)
 	qcam->vdev.fops = &qcam_fops;
 	qcam->vdev.ioctl_ops = &qcam_ioctl_ops;
 	qcam->vdev.release = video_device_release_empty;
+	qcam->vdev.ctrl_handler = &qcam->hdl;
+	set_bit(V4L2_FL_USE_FH_PRIO, &qcam->vdev.flags);
 	video_set_drvdata(&qcam->vdev, qcam);
 
 	mutex_init(&qcam->lock);
@@ -845,6 +836,7 @@ static int init_cqcam(struct parport *port)
 static void close_cqcam(struct qcam *qcam)
 {
 	video_unregister_device(&qcam->vdev);
+	v4l2_ctrl_handler_free(&qcam->hdl);
 	parport_unregister_device(qcam->pdev);
 	kfree(qcam);
 }
diff --git a/drivers/media/video/cpia2/cpia2.h b/drivers/media/video/cpia2/cpia2.h
index ab252188981..cdef677d57e 100644
--- a/drivers/media/video/cpia2/cpia2.h
+++ b/drivers/media/video/cpia2/cpia2.h
@@ -32,11 +32,12 @@
 #define __CPIA2_H__
 
 #include <linux/videodev2.h>
-#include <media/v4l2-common.h>
 #include <linux/usb.h>
 #include <linux/poll.h>
+#include <media/v4l2-common.h>
+#include <media/v4l2-device.h>
+#include <media/v4l2-ctrls.h>
 
-#include "cpia2dev.h"
 #include "cpia2_registers.h"
 
 /* define for verbose debug output */
@@ -65,7 +66,6 @@
 
 /* Flicker Modes */
 #define NEVER_FLICKER   0
-#define ANTI_FLICKER_ON 1
 #define FLICKER_60      60
 #define FLICKER_50      50
 
@@ -148,7 +148,6 @@ enum {
 #define DEFAULT_BRIGHTNESS 0x46
 #define DEFAULT_CONTRAST 0x93
 #define DEFAULT_SATURATION 0x7f
-#define DEFAULT_TARGET_KB 0x30
 
 /* Power state */
 #define HI_POWER_MODE CPIA2_SYSTEM_CONTROL_HIGH_POWER
@@ -287,7 +286,6 @@ struct camera_params {
 	struct {
 		u8 cam_register;
 		u8 flicker_mode_req;	/* 1 if flicker on, else never flicker */
-		int mains_frequency;
 	} flicker_control;
 
 	struct {
@@ -337,7 +335,7 @@ struct camera_params {
 		u8 vc_control;
 		u8 vc_mp_direction;
 		u8 vc_mp_data;
-		u8 target_kb;
+		u8 quality;
 	} vc_params;
 
 	struct {
@@ -366,23 +364,23 @@ struct framebuf {
 	struct framebuf *next;
 };
 
-struct cpia2_fh {
-	enum v4l2_priority prio;
-	u8 mmapped;
-};
-
 struct camera_data {
 	/* locks */
+	struct v4l2_device v4l2_dev;
 	struct mutex v4l2_lock;	/* serialize file operations */
-	struct v4l2_prio_state prio;
+	struct v4l2_ctrl_handler hdl;
+	struct {
+		/* Lights control cluster */
+		struct v4l2_ctrl *top_light;
+		struct v4l2_ctrl *bottom_light;
+	};
+	struct v4l2_ctrl *usb_alt;
 
 	/* camera status */
-	volatile int present;	/* Is the camera still present? */
-	int open_count;		/* # of process that have camera open */
 	int first_image_seen;
-	u8 mains_freq;		/* for flicker control */
 	enum sensors sensor_type;
 	u8 flush;
+	struct v4l2_fh *stream_fh;
 	u8 mmapped;
 	int streaming;		/* 0 = no, 1 = yes */
 	int xfer_mode;		/* XFER_BULK or XFER_ISOC */
@@ -390,7 +388,7 @@ struct camera_data {
 
 	/* v4l */
 	int video_size;			/* VIDEO_SIZE_ */
-	struct video_device *vdev;	/* v4l videodev */
+	struct video_device vdev;	/* v4l videodev */
 	u32 width;
 	u32 height;			/* Its size */
 	__u32 pixelformat;       /* Format fourcc      */
@@ -425,6 +423,7 @@ struct camera_data {
 /* v4l */
 int cpia2_register_camera(struct camera_data *cam);
 void cpia2_unregister_camera(struct camera_data *cam);
+void cpia2_camera_release(struct v4l2_device *v4l2_dev);
 
 /* core */
 int cpia2_reset_camera(struct camera_data *cam);
@@ -443,7 +442,7 @@ int cpia2_send_command(struct camera_data *cam, struct cpia2_command *cmd);
 int cpia2_do_command(struct camera_data *cam,
 		     unsigned int command,
 		     unsigned char direction, unsigned char param);
-struct camera_data *cpia2_init_camera_struct(void);
+struct camera_data *cpia2_init_camera_struct(struct usb_interface *intf);
 int cpia2_init_camera(struct camera_data *cam);
 int cpia2_allocate_buffers(struct camera_data *cam);
 void cpia2_free_buffers(struct camera_data *cam);
@@ -454,7 +453,6 @@ unsigned int cpia2_poll(struct camera_data *cam,
 int cpia2_remap_buffer(struct camera_data *cam, struct vm_area_struct *vma);
 void cpia2_set_property_flip(struct camera_data *cam, int prop_val);
 void cpia2_set_property_mirror(struct camera_data *cam, int prop_val);
-int cpia2_set_target_kb(struct camera_data *cam, unsigned char value);
 int cpia2_set_gpio(struct camera_data *cam, unsigned char setting);
 int cpia2_set_fps(struct camera_data *cam, int framerate);
 
diff --git a/drivers/media/video/cpia2/cpia2_core.c b/drivers/media/video/cpia2/cpia2_core.c
index ee91e295c90..17188e23477 100644
--- a/drivers/media/video/cpia2/cpia2_core.c
+++ b/drivers/media/video/cpia2/cpia2_core.c
@@ -66,7 +66,6 @@ static int config_sensor_410(struct camera_data *cam,
 static int config_sensor_500(struct camera_data *cam,
 			    int reqwidth, int reqheight);
 static int set_all_properties(struct camera_data *cam);
-static void get_color_params(struct camera_data *cam);
 static void wake_system(struct camera_data *cam);
 static void set_lowlight_boost(struct camera_data *cam);
 static void reset_camera_struct(struct camera_data *cam);
@@ -453,15 +452,6 @@ int cpia2_do_command(struct camera_data *cam,
 		cam->params.version.vp_device_hi = cmd.buffer.block_data[0];
 		cam->params.version.vp_device_lo = cmd.buffer.block_data[1];
 		break;
-	case CPIA2_CMD_GET_VP_BRIGHTNESS:
-		cam->params.color_params.brightness = cmd.buffer.block_data[0];
-		break;
-	case CPIA2_CMD_GET_CONTRAST:
-		cam->params.color_params.contrast = cmd.buffer.block_data[0];
-		break;
-	case CPIA2_CMD_GET_VP_SATURATION:
-		cam->params.color_params.saturation = cmd.buffer.block_data[0];
-		break;
 	case CPIA2_CMD_GET_VP_GPIO_DATA:
 		cam->params.vp_params.gpio_data = cmd.buffer.block_data[0];
 		break;
@@ -617,6 +607,7 @@ int cpia2_reset_camera(struct camera_data *cam)
 {
 	u8 tmp_reg;
 	int retval = 0;
+	int target_kb;
 	int i;
 	struct cpia2_command cmd;
 
@@ -800,9 +791,16 @@ int cpia2_reset_camera(struct camera_data *cam)
 	}
 	cpia2_do_command(cam, CPIA2_CMD_SET_VC_CONTROL, TRANSFER_WRITE,tmp_reg);
 
-	/* Set target size (kb) on vc */
+	/* Set target size (kb) on vc
+	   This is a heuristic based on the quality parameter and the raw
+	   framesize in kB divided by 16 (the compression factor when the
+	   quality is 100%) */
+	target_kb = (cam->width * cam->height * 2 / 16384) *
+				cam->params.vc_params.quality / 100;
+	if (target_kb < 1)
+		target_kb = 1;
 	cpia2_do_command(cam, CPIA2_CMD_SET_TARGET_KB,
-			 TRANSFER_WRITE, cam->params.vc_params.target_kb);
+			 TRANSFER_WRITE, target_kb);
 
 	/* Wiggle VC Reset */
 	/***
@@ -1538,23 +1536,17 @@ static int set_all_properties(struct camera_data *cam)
 	 * framerate and user_mode were already set (set_default_user_mode).
 	 **/
 
-	cpia2_set_color_params(cam);
-
 	cpia2_usb_change_streaming_alternate(cam,
 					  cam->params.camera_state.stream_mode);
 
-	cpia2_do_command(cam, CPIA2_CMD_SET_USER_EFFECTS, TRANSFER_WRITE,
-			 cam->params.vp_params.user_effects);
-
-	cpia2_set_flicker_mode(cam,
-			       cam->params.flicker_control.flicker_mode_req);
-
 	cpia2_do_command(cam,
 			 CPIA2_CMD_SET_VC_MP_GPIO_DIRECTION,
 			 TRANSFER_WRITE, cam->params.vp_params.gpio_direction);
 	cpia2_do_command(cam, CPIA2_CMD_SET_VC_MP_GPIO_DATA, TRANSFER_WRITE,
 			 cam->params.vp_params.gpio_data);
 
+	v4l2_ctrl_handler_setup(&cam->hdl);
+
 	wake_system(cam);
 
 	set_lowlight_boost(cam);
@@ -1569,7 +1561,6 @@ static int set_all_properties(struct camera_data *cam)
  *****************************************************************************/
 void cpia2_save_camera_state(struct camera_data *cam)
 {
-	get_color_params(cam);
 	cpia2_do_command(cam, CPIA2_CMD_GET_USER_EFFECTS, TRANSFER_READ, 0);
 	cpia2_do_command(cam, CPIA2_CMD_GET_VC_MP_GPIO_DIRECTION, TRANSFER_READ,
 			 0);
@@ -1577,30 +1568,6 @@ void cpia2_save_camera_state(struct camera_data *cam)
 	/* Don't get framerate or target_kb. Trust the values we already have */
 }
 
-/******************************************************************************
- *
- *  get_color_params
- *
- *****************************************************************************/
-static void get_color_params(struct camera_data *cam)
-{
-	cpia2_do_command(cam, CPIA2_CMD_GET_VP_BRIGHTNESS, TRANSFER_READ, 0);
-	cpia2_do_command(cam, CPIA2_CMD_GET_VP_SATURATION, TRANSFER_READ, 0);
-	cpia2_do_command(cam, CPIA2_CMD_GET_CONTRAST, TRANSFER_READ, 0);
-}
-
-/******************************************************************************
- *
- *  cpia2_set_color_params
- *
- *****************************************************************************/
-void cpia2_set_color_params(struct camera_data *cam)
-{
-	DBG("Setting color params\n");
-	cpia2_set_brightness(cam, cam->params.color_params.brightness);
-	cpia2_set_contrast(cam, cam->params.color_params.contrast);
-	cpia2_set_saturation(cam, cam->params.color_params.saturation);
-}
 
 /******************************************************************************
  *
@@ -1664,15 +1631,9 @@ int cpia2_set_flicker_mode(struct camera_data *cam, int mode)
 
 	switch(mode) {
 	case NEVER_FLICKER:
-		cam->params.flicker_control.flicker_mode_req = mode;
-		break;
 	case FLICKER_60:
-		cam->params.flicker_control.flicker_mode_req = mode;
-		cam->params.flicker_control.mains_frequency = 60;
-		break;
 	case FLICKER_50:
 		cam->params.flicker_control.flicker_mode_req = mode;
-		cam->params.flicker_control.mains_frequency = 50;
 		break;
 	default:
 		err = -EINVAL;
@@ -1701,6 +1662,7 @@ void cpia2_set_property_flip(struct camera_data *cam, int prop_val)
 	{
 		cam_reg &= ~CPIA2_VP_USER_EFFECTS_FLIP;
 	}
+	cam->params.vp_params.user_effects = cam_reg;
 	cpia2_do_command(cam, CPIA2_CMD_SET_USER_EFFECTS, TRANSFER_WRITE,
 			 cam_reg);
 }
@@ -1725,37 +1687,13 @@ void cpia2_set_property_mirror(struct camera_data *cam, int prop_val)
 	{
 		cam_reg &= ~CPIA2_VP_USER_EFFECTS_MIRROR;
 	}
+	cam->params.vp_params.user_effects = cam_reg;
 	cpia2_do_command(cam, CPIA2_CMD_SET_USER_EFFECTS, TRANSFER_WRITE,
 			 cam_reg);
 }
 
 /******************************************************************************
  *
- *  set_target_kb
- *
- *  The new Target KB is set in cam->params.vc_params.target_kb and
- *  activates on reset.
- *****************************************************************************/
-
-int cpia2_set_target_kb(struct camera_data *cam, unsigned char value)
-{
-	DBG("Requested target_kb = %d\n", value);
-	if (value != cam->params.vc_params.target_kb) {
-
-		cpia2_usb_stream_pause(cam);
-
-		/* reset camera for new target_kb */
-		cam->params.vc_params.target_kb = value;
-		cpia2_reset_camera(cam);
-
-		cpia2_usb_stream_resume(cam);
-	}
-
-	return 0;
-}
-
-/******************************************************************************
- *
  *  cpia2_set_gpio
  *
  *****************************************************************************/
@@ -1843,7 +1781,7 @@ void cpia2_set_brightness(struct camera_data *cam, unsigned char value)
 	if (cam->params.pnp_id.device_type == DEVICE_STV_672 && value == 0)
 		value++;
 	DBG("Setting brightness to %d (0x%0x)\n", value, value);
-	cpia2_do_command(cam,CPIA2_CMD_SET_VP_BRIGHTNESS, TRANSFER_WRITE,value);
+	cpia2_do_command(cam, CPIA2_CMD_SET_VP_BRIGHTNESS, TRANSFER_WRITE, value);
 }
 
 /******************************************************************************
@@ -1854,7 +1792,6 @@ void cpia2_set_brightness(struct camera_data *cam, unsigned char value)
 void cpia2_set_contrast(struct camera_data *cam, unsigned char value)
 {
 	DBG("Setting contrast to %d (0x%0x)\n", value, value);
-	cam->params.color_params.contrast = value;
 	cpia2_do_command(cam, CPIA2_CMD_SET_CONTRAST, TRANSFER_WRITE, value);
 }
 
@@ -1866,7 +1803,6 @@ void cpia2_set_contrast(struct camera_data *cam, unsigned char value)
 void cpia2_set_saturation(struct camera_data *cam, unsigned char value)
 {
 	DBG("Setting saturation to %d (0x%0x)\n", value, value);
-	cam->params.color_params.saturation = value;
 	cpia2_do_command(cam,CPIA2_CMD_SET_VP_SATURATION, TRANSFER_WRITE,value);
 }
 
@@ -2168,14 +2104,10 @@ static void reset_camera_struct(struct camera_data *cam)
 	/***
 	 * The following parameter values are the defaults from the register map.
 	 ***/
-	cam->params.color_params.brightness = DEFAULT_BRIGHTNESS;
-	cam->params.color_params.contrast = DEFAULT_CONTRAST;
-	cam->params.color_params.saturation = DEFAULT_SATURATION;
 	cam->params.vp_params.lowlight_boost = 0;
 
 	/* FlickerModes */
 	cam->params.flicker_control.flicker_mode_req = NEVER_FLICKER;
-	cam->params.flicker_control.mains_frequency = 60;
 
 	/* jpeg params */
 	cam->params.compression.jpeg_options = CPIA2_VC_VC_JPEG_OPT_DEFAULT;
@@ -2188,7 +2120,7 @@ static void reset_camera_struct(struct camera_data *cam)
 	cam->params.vp_params.gpio_data = 0;
 
 	/* Target kb params */
-	cam->params.vc_params.target_kb = DEFAULT_TARGET_KB;
+	cam->params.vc_params.quality = 100;
 
 	/***
 	 * Set Sensor FPS as fast as possible.
@@ -2228,7 +2160,7 @@ static void reset_camera_struct(struct camera_data *cam)
  *
  *  Initializes camera struct, does not call reset to fill in defaults.
  *****************************************************************************/
-struct camera_data *cpia2_init_camera_struct(void)
+struct camera_data *cpia2_init_camera_struct(struct usb_interface *intf)
 {
 	struct camera_data *cam;
 
@@ -2239,8 +2171,13 @@ struct camera_data *cpia2_init_camera_struct(void)
 		return NULL;
 	}
 
+	cam->v4l2_dev.release = cpia2_camera_release;
+	if (v4l2_device_register(&intf->dev, &cam->v4l2_dev) < 0) {
+		v4l2_err(&cam->v4l2_dev, "couldn't register v4l2_device\n");
+		kfree(cam);
+		return NULL;
+	}
 
-	cam->present = 1;
 	mutex_init(&cam->v4l2_lock);
 	init_waitqueue_head(&cam->wq_stream);
 
@@ -2373,11 +2310,6 @@ long cpia2_read(struct camera_data *cam,
 		return -EINVAL;
 	}
 
-	if (!cam->present) {
-		LOG("%s: camera removed\n",__func__);
-		return 0;	/* EOF */
-	}
-
 	if (!cam->streaming) {
 		/* Start streaming */
 		cpia2_usb_stream_start(cam,
@@ -2393,12 +2325,12 @@ long cpia2_read(struct camera_data *cam,
 	if (frame->status != FRAME_READY) {
 		mutex_unlock(&cam->v4l2_lock);
 		wait_event_interruptible(cam->wq_stream,
-			       !cam->present ||
+			       !video_is_registered(&cam->vdev) ||
 			       (frame = cam->curbuff)->status == FRAME_READY);
 		mutex_lock(&cam->v4l2_lock);
 		if (signal_pending(current))
 			return -ERESTARTSYS;
-		if (!cam->present)
+		if (!video_is_registered(&cam->vdev))
 			return 0;
 	}
 
@@ -2423,17 +2355,10 @@ long cpia2_read(struct camera_data *cam,
 unsigned int cpia2_poll(struct camera_data *cam, struct file *filp,
 			poll_table *wait)
 {
-	unsigned int status=0;
+	unsigned int status = v4l2_ctrl_poll(filp, wait);
 
-	if (!cam) {
-		ERR("%s: Internal error, camera_data not found!\n",__func__);
-		return POLLERR;
-	}
-
-	if (!cam->present)
-		return POLLHUP;
-
-	if(!cam->streaming) {
+	if ((poll_requested_events(wait) & (POLLIN | POLLRDNORM)) &&
+			!cam->streaming) {
 		/* Start streaming */
 		cpia2_usb_stream_start(cam,
 				       cam->params.camera_state.stream_mode);
@@ -2441,10 +2366,8 @@ unsigned int cpia2_poll(struct camera_data *cam, struct file *filp,
 
 	poll_wait(filp, &cam->wq_stream, wait);
 
-	if(!cam->present)
-		status = POLLHUP;
-	else if(cam->curbuff->status == FRAME_READY)
-		status = POLLIN | POLLRDNORM;
+	if (cam->curbuff->status == FRAME_READY)
+		status |= POLLIN | POLLRDNORM;
 
 	return status;
 }
@@ -2462,12 +2385,9 @@ int cpia2_remap_buffer(struct camera_data *cam, struct vm_area_struct *vma)
 	unsigned long start = (unsigned long) adr;
 	unsigned long page, pos;
 
-	if (!cam)
-		return -ENODEV;
-
 	DBG("mmap offset:%ld size:%ld\n", start_offset, size);
 
-	if (!cam->present)
+	if (!video_is_registered(&cam->vdev))
 		return -ENODEV;
 
 	if (size > cam->frame_size*cam->num_frames  ||
diff --git a/drivers/media/video/cpia2/cpia2_usb.c b/drivers/media/video/cpia2/cpia2_usb.c
index 59c797c1527..95b5d6e7cdc 100644
--- a/drivers/media/video/cpia2/cpia2_usb.c
+++ b/drivers/media/video/cpia2/cpia2_usb.c
@@ -54,6 +54,8 @@ static void cpia2_usb_complete(struct urb *urb);
 static int cpia2_usb_probe(struct usb_interface *intf,
 			   const struct usb_device_id *id);
 static void cpia2_usb_disconnect(struct usb_interface *intf);
+static int cpia2_usb_suspend(struct usb_interface *intf, pm_message_t message);
+static int cpia2_usb_resume(struct usb_interface *intf);
 
 static void free_sbufs(struct camera_data *cam);
 static void add_APPn(struct camera_data *cam);
@@ -74,6 +76,9 @@ static struct usb_driver cpia2_driver = {
 	.name		= "cpia2",
 	.probe		= cpia2_usb_probe,
 	.disconnect	= cpia2_usb_disconnect,
+	.suspend	= cpia2_usb_suspend,
+	.resume		= cpia2_usb_resume,
+	.reset_resume	= cpia2_usb_resume,
 	.id_table	= cpia2_id_table
 };
 
@@ -218,10 +223,9 @@ static void cpia2_usb_complete(struct urb *urb)
 		return;
 	}
 
-	if (!cam->streaming || !cam->present || cam->open_count == 0) {
-		LOG("Will now stop the streaming: streaming = %d, "
-		    "present=%d, open_count=%d\n",
-		    cam->streaming, cam->present, cam->open_count);
+	if (!cam->streaming || !video_is_registered(&cam->vdev)) {
+		LOG("Will now stop the streaming: streaming = %d, present=%d\n",
+		    cam->streaming, video_is_registered(&cam->vdev));
 		return;
 	}
 
@@ -392,7 +396,7 @@ static int configure_transfer_mode(struct camera_data *cam, unsigned int alt)
 	struct cpia2_command cmd;
 	unsigned char reg;
 
-	if(!cam->present)
+	if (!video_is_registered(&cam->vdev))
 		return -ENODEV;
 
 	/***
@@ -752,8 +756,8 @@ int cpia2_usb_stream_pause(struct camera_data *cam)
 {
 	int ret = 0;
 	if(cam->streaming) {
-		ret = set_alternate(cam, USBIF_CMDONLY);
 		free_sbufs(cam);
+		ret = set_alternate(cam, USBIF_CMDONLY);
 	}
 	return ret;
 }
@@ -770,6 +774,10 @@ int cpia2_usb_stream_resume(struct camera_data *cam)
 		cam->first_image_seen = 0;
 		ret = set_alternate(cam, cam->params.camera_state.stream_mode);
 		if(ret == 0) {
+			/* for some reason the user effects need to be set
+			   again when starting streaming. */
+			cpia2_do_command(cam, CPIA2_CMD_SET_USER_EFFECTS, TRANSFER_WRITE,
+					cam->params.vp_params.user_effects);
 			ret = submit_urbs(cam);
 		}
 	}
@@ -784,6 +792,7 @@ int cpia2_usb_stream_resume(struct camera_data *cam)
 int cpia2_usb_stream_stop(struct camera_data *cam)
 {
 	int ret;
+
 	ret = cpia2_usb_stream_pause(cam);
 	cam->streaming = 0;
 	configure_transfer_mode(cam, 0);
@@ -812,7 +821,8 @@ static int cpia2_usb_probe(struct usb_interface *intf,
 	/* If we get to this point, we found a CPiA2 camera */
 	LOG("CPiA2 USB camera found\n");
 
-	if((cam = cpia2_init_camera_struct()) == NULL)
+	cam = cpia2_init_camera_struct(intf);
+	if (cam == NULL)
 		return -ENOMEM;
 
 	cam->dev = udev;
@@ -825,16 +835,9 @@ static int cpia2_usb_probe(struct usb_interface *intf,
 		return ret;
 	}
 
-	if ((ret = cpia2_register_camera(cam)) < 0) {
-		ERR("%s: Failed to register cpia2 camera (ret = %d)\n", __func__, ret);
-		kfree(cam);
-		return ret;
-	}
-
 
 	if((ret = cpia2_init_camera(cam)) < 0) {
 		ERR("%s: failed to initialize cpia2 camera (ret = %d)\n", __func__, ret);
-		cpia2_unregister_camera(cam);
 		kfree(cam);
 		return ret;
 	}
@@ -853,6 +856,13 @@ static int cpia2_usb_probe(struct usb_interface *intf,
 
 	usb_set_intfdata(intf, cam);
 
+	ret = cpia2_register_camera(cam);
+	if (ret < 0) {
+		ERR("%s: Failed to register cpia2 camera (ret = %d)\n", __func__, ret);
+		kfree(cam);
+		return ret;
+	}
+
 	return 0;
 }
 
@@ -865,13 +875,16 @@ static void cpia2_usb_disconnect(struct usb_interface *intf)
 {
 	struct camera_data *cam = usb_get_intfdata(intf);
 	usb_set_intfdata(intf, NULL);
-	cam->present = 0;
 
 	DBG("Stopping stream\n");
 	cpia2_usb_stream_stop(cam);
 
+	mutex_lock(&cam->v4l2_lock);
 	DBG("Unregistering camera\n");
 	cpia2_unregister_camera(cam);
+	v4l2_device_disconnect(&cam->v4l2_dev);
+	mutex_unlock(&cam->v4l2_lock);
+	v4l2_device_put(&cam->v4l2_dev);
 
 	if(cam->buffers) {
 		DBG("Wakeup waiting processes\n");
@@ -884,14 +897,41 @@ static void cpia2_usb_disconnect(struct usb_interface *intf)
 	DBG("Releasing interface\n");
 	usb_driver_release_interface(&cpia2_driver, intf);
 
-	if (cam->open_count == 0) {
-		DBG("Freeing camera structure\n");
-		kfree(cam);
+	LOG("CPiA2 camera disconnected.\n");
+}
+
+static int cpia2_usb_suspend(struct usb_interface *intf, pm_message_t message)
+{
+	struct camera_data *cam = usb_get_intfdata(intf);
+
+	mutex_lock(&cam->v4l2_lock);
+	if (cam->streaming) {
+		cpia2_usb_stream_stop(cam);
+		cam->streaming = 1;
 	}
+	mutex_unlock(&cam->v4l2_lock);
 
-	LOG("CPiA2 camera disconnected.\n");
+	dev_info(&intf->dev, "going into suspend..\n");
+	return 0;
 }
 
+/* Resume device - start device. */
+static int cpia2_usb_resume(struct usb_interface *intf)
+{
+	struct camera_data *cam = usb_get_intfdata(intf);
+
+	mutex_lock(&cam->v4l2_lock);
+	v4l2_ctrl_handler_setup(&cam->hdl);
+	if (cam->streaming) {
+		cam->streaming = 0;
+		cpia2_usb_stream_start(cam,
+				cam->params.camera_state.stream_mode);
+	}
+	mutex_unlock(&cam->v4l2_lock);
+
+	dev_info(&intf->dev, "coming out of suspend..\n");
+	return 0;
+}
 
 /******************************************************************************
  *
diff --git a/drivers/media/video/cpia2/cpia2_v4l.c b/drivers/media/video/cpia2/cpia2_v4l.c
index 077eb1db80a..55e92902a76 100644
--- a/drivers/media/video/cpia2/cpia2_v4l.c
+++ b/drivers/media/video/cpia2/cpia2_v4l.c
@@ -39,15 +39,15 @@
 #include <linux/videodev2.h>
 #include <linux/stringify.h>
 #include <media/v4l2-ioctl.h>
+#include <media/v4l2-event.h>
 
 #include "cpia2.h"
-#include "cpia2dev.h"
 
 static int video_nr = -1;
 module_param(video_nr, int, 0);
-MODULE_PARM_DESC(video_nr,"video device to register (0=/dev/video0, etc)");
+MODULE_PARM_DESC(video_nr, "video device to register (0=/dev/video0, etc)");
 
-static int buffer_size = 68*1024;
+static int buffer_size = 68 * 1024;
 module_param(buffer_size, int, 0);
 MODULE_PARM_DESC(buffer_size, "Size for each frame buffer in bytes (default 68k)");
 
@@ -62,18 +62,10 @@ MODULE_PARM_DESC(alternate, "USB Alternate (" __stringify(USBIF_ISO_1) "-"
 		 __stringify(USBIF_ISO_6) ", default "
 		 __stringify(DEFAULT_ALT) ")");
 
-static int flicker_freq = 60;
-module_param(flicker_freq, int, 0);
-MODULE_PARM_DESC(flicker_freq, "Flicker frequency (" __stringify(50) "or"
-		 __stringify(60) ", default "
-		 __stringify(60) ")");
-
-static int flicker_mode = NEVER_FLICKER;
+static int flicker_mode;
 module_param(flicker_mode, int, 0);
-MODULE_PARM_DESC(flicker_mode,
-		 "Flicker supression (" __stringify(NEVER_FLICKER) "or"
-		 __stringify(ANTI_FLICKER_ON) ", default "
-		 __stringify(NEVER_FLICKER) ")");
+MODULE_PARM_DESC(flicker_mode, "Flicker frequency (0 (disabled), " __stringify(50) " or "
+		 __stringify(60) ", default 0)");
 
 MODULE_AUTHOR("Steve Miller (STMicroelectronics) <steve.miller@st.com>");
 MODULE_DESCRIPTION("V4L-driver for STMicroelectronics CPiA2 based cameras");
@@ -82,153 +74,7 @@ MODULE_LICENSE("GPL");
 MODULE_VERSION(CPIA_VERSION);
 
 #define ABOUT "V4L-Driver for Vision CPiA2 based cameras"
-
-struct control_menu_info {
-	int value;
-	char name[32];
-};
-
-static struct control_menu_info framerate_controls[] =
-{
-	{ CPIA2_VP_FRAMERATE_6_25, "6.25 fps" },
-	{ CPIA2_VP_FRAMERATE_7_5,  "7.5 fps"  },
-	{ CPIA2_VP_FRAMERATE_12_5, "12.5 fps" },
-	{ CPIA2_VP_FRAMERATE_15,   "15 fps"   },
-	{ CPIA2_VP_FRAMERATE_25,   "25 fps"   },
-	{ CPIA2_VP_FRAMERATE_30,   "30 fps"   },
-};
-#define NUM_FRAMERATE_CONTROLS (ARRAY_SIZE(framerate_controls))
-
-static struct control_menu_info flicker_controls[] =
-{
-	{ NEVER_FLICKER, "Off" },
-	{ FLICKER_50,    "50 Hz" },
-	{ FLICKER_60,    "60 Hz"  },
-};
-#define NUM_FLICKER_CONTROLS (ARRAY_SIZE(flicker_controls))
-
-static struct control_menu_info lights_controls[] =
-{
-	{ 0,   "Off" },
-	{ 64,  "Top" },
-	{ 128, "Bottom"  },
-	{ 192, "Both"  },
-};
-#define NUM_LIGHTS_CONTROLS (ARRAY_SIZE(lights_controls))
-#define GPIO_LIGHTS_MASK 192
-
-static struct v4l2_queryctrl controls[] = {
-	{
-		.id            = V4L2_CID_BRIGHTNESS,
-		.type          = V4L2_CTRL_TYPE_INTEGER,
-		.name          = "Brightness",
-		.minimum       = 0,
-		.maximum       = 255,
-		.step          = 1,
-		.default_value = DEFAULT_BRIGHTNESS,
-	},
-	{
-		.id            = V4L2_CID_CONTRAST,
-		.type          = V4L2_CTRL_TYPE_INTEGER,
-		.name          = "Contrast",
-		.minimum       = 0,
-		.maximum       = 255,
-		.step          = 1,
-		.default_value = DEFAULT_CONTRAST,
-	},
-	{
-		.id            = V4L2_CID_SATURATION,
-		.type          = V4L2_CTRL_TYPE_INTEGER,
-		.name          = "Saturation",
-		.minimum       = 0,
-		.maximum       = 255,
-		.step          = 1,
-		.default_value = DEFAULT_SATURATION,
-	},
-	{
-		.id            = V4L2_CID_HFLIP,
-		.type          = V4L2_CTRL_TYPE_BOOLEAN,
-		.name          = "Mirror Horizontally",
-		.minimum       = 0,
-		.maximum       = 1,
-		.step          = 1,
-		.default_value = 0,
-	},
-	{
-		.id            = V4L2_CID_VFLIP,
-		.type          = V4L2_CTRL_TYPE_BOOLEAN,
-		.name          = "Flip Vertically",
-		.minimum       = 0,
-		.maximum       = 1,
-		.step          = 1,
-		.default_value = 0,
-	},
-	{
-		.id            = CPIA2_CID_TARGET_KB,
-		.type          = V4L2_CTRL_TYPE_INTEGER,
-		.name          = "Target KB",
-		.minimum       = 0,
-		.maximum       = 255,
-		.step          = 1,
-		.default_value = DEFAULT_TARGET_KB,
-	},
-	{
-		.id            = CPIA2_CID_GPIO,
-		.type          = V4L2_CTRL_TYPE_INTEGER,
-		.name          = "GPIO",
-		.minimum       = 0,
-		.maximum       = 255,
-		.step          = 1,
-		.default_value = 0,
-	},
-	{
-		.id            = CPIA2_CID_FLICKER_MODE,
-		.type          = V4L2_CTRL_TYPE_MENU,
-		.name          = "Flicker Reduction",
-		.minimum       = 0,
-		.maximum       = NUM_FLICKER_CONTROLS-1,
-		.step          = 1,
-		.default_value = 0,
-	},
-	{
-		.id            = CPIA2_CID_FRAMERATE,
-		.type          = V4L2_CTRL_TYPE_MENU,
-		.name          = "Framerate",
-		.minimum       = 0,
-		.maximum       = NUM_FRAMERATE_CONTROLS-1,
-		.step          = 1,
-		.default_value = NUM_FRAMERATE_CONTROLS-1,
-	},
-	{
-		.id            = CPIA2_CID_USB_ALT,
-		.type          = V4L2_CTRL_TYPE_INTEGER,
-		.name          = "USB Alternate",
-		.minimum       = USBIF_ISO_1,
-		.maximum       = USBIF_ISO_6,
-		.step          = 1,
-		.default_value = DEFAULT_ALT,
-	},
-	{
-		.id            = CPIA2_CID_LIGHTS,
-		.type          = V4L2_CTRL_TYPE_MENU,
-		.name          = "Lights",
-		.minimum       = 0,
-		.maximum       = NUM_LIGHTS_CONTROLS-1,
-		.step          = 1,
-		.default_value = 0,
-	},
-	{
-		.id            = CPIA2_CID_RESET_CAMERA,
-		.type          = V4L2_CTRL_TYPE_BUTTON,
-		.name          = "Reset Camera",
-		.minimum       = 0,
-		.maximum       = 0,
-		.step          = 0,
-		.default_value = 0,
-	},
-};
-#define NUM_CONTROLS (ARRAY_SIZE(controls))
-
+#define CPIA2_CID_USB_ALT (V4L2_CID_USER_BASE | 0xf000)
 
 /******************************************************************************
  *
@@ -238,38 +84,27 @@ static struct v4l2_queryctrl controls[] = {
 static int cpia2_open(struct file *file)
 {
 	struct camera_data *cam = video_drvdata(file);
-	struct cpia2_fh *fh;
-
-	if (!cam) {
-		ERR("Internal error, camera_data not found!\n");
-		return -ENODEV;
-	}
+	int retval = v4l2_fh_open(file);
 
-	if (!cam->present)
-		return -ENODEV;
+	if (retval)
+		return retval;
 
-	if (cam->open_count == 0) {
-		if (cpia2_allocate_buffers(cam))
+	if (v4l2_fh_is_singular_file(file)) {
+		if (cpia2_allocate_buffers(cam)) {
+			v4l2_fh_release(file);
 			return -ENOMEM;
+		}
 
 		/* reset the camera */
-		if (cpia2_reset_camera(cam) < 0)
+		if (cpia2_reset_camera(cam) < 0) {
+			v4l2_fh_release(file);
 			return -EIO;
+		}
 
 		cam->APP_len = 0;
 		cam->COM_len = 0;
 	}
 
-	fh = kmalloc(sizeof(*fh), GFP_KERNEL);
-	if (!fh)
-		return -ENOMEM;
-	file->private_data = fh;
-	fh->prio = V4L2_PRIORITY_UNSET;
-	v4l2_prio_open(&cam->prio, &fh->prio);
-	fh->mmapped = 0;
-
-	++cam->open_count;
-
 	cpia2_dbg_dump_registers(cam);
 	return 0;
 }
@@ -283,37 +118,22 @@ static int cpia2_close(struct file *file)
 {
 	struct video_device *dev = video_devdata(file);
 	struct camera_data *cam = video_get_drvdata(dev);
-	struct cpia2_fh *fh = file->private_data;
 
-	if (cam->present &&
-	    (cam->open_count == 1 || fh->prio == V4L2_PRIORITY_RECORD)) {
+	if (video_is_registered(&cam->vdev) && v4l2_fh_is_singular_file(file)) {
 		cpia2_usb_stream_stop(cam);
 
-		if (cam->open_count == 1) {
-			/* save camera state for later open */
-			cpia2_save_camera_state(cam);
+		/* save camera state for later open */
+		cpia2_save_camera_state(cam);
 
-			cpia2_set_low_power(cam);
-			cpia2_free_buffers(cam);
-		}
+		cpia2_set_low_power(cam);
+		cpia2_free_buffers(cam);
 	}
 
-	if (fh->mmapped)
+	if (cam->stream_fh == file->private_data) {
+		cam->stream_fh = NULL;
 		cam->mmapped = 0;
-	v4l2_prio_close(&cam->prio, fh->prio);
-	file->private_data = NULL;
-	kfree(fh);
-
-	if (--cam->open_count == 0) {
-		cpia2_free_buffers(cam);
-		if (!cam->present) {
-			video_unregister_device(dev);
-			kfree(cam);
-			return 0;
-		}
 	}
-
-	return 0;
+	return v4l2_fh_release(file);
 }
 
 /******************************************************************************
@@ -327,16 +147,9 @@ static ssize_t cpia2_v4l_read(struct file *file, char __user *buf, size_t count,
 	struct camera_data *cam = video_drvdata(file);
 	int noblock = file->f_flags&O_NONBLOCK;
 
-	struct cpia2_fh *fh = file->private_data;
-
 	if(!cam)
 		return -EINVAL;
 
-	/* Priority check */
-	if(fh->prio != V4L2_PRIORITY_RECORD) {
-		return -EBUSY;
-	}
-
 	return cpia2_read(cam, buf, count, noblock);
 }
 
@@ -349,15 +162,6 @@ static ssize_t cpia2_v4l_read(struct file *file, char __user *buf, size_t count,
 static unsigned int cpia2_v4l_poll(struct file *filp, struct poll_table_struct *wait)
 {
 	struct camera_data *cam = video_drvdata(filp);
-	struct cpia2_fh *fh = filp->private_data;
-
-	if(!cam)
-		return POLLERR;
-
-	/* Priority check */
-	if(fh->prio != V4L2_PRIORITY_RECORD) {
-		return POLLERR;
-	}
 
 	return cpia2_poll(cam, filp, wait);
 }
@@ -384,36 +188,13 @@ static int sync(struct camera_data *cam, int frame_nr)
 		mutex_lock(&cam->v4l2_lock);
 		if (signal_pending(current))
 			return -ERESTARTSYS;
-		if(!cam->present)
+		if (!video_is_registered(&cam->vdev))
 			return -ENOTTY;
 	}
 }
 
 /******************************************************************************
  *
- *  ioctl_set_gpio
- *
- *****************************************************************************/
-
-static long cpia2_default(struct file *file, void *fh, bool valid_prio,
-			  int cmd, void *arg)
-{
-	struct camera_data *cam = video_drvdata(file);
-	__u32 gpio_val;
-
-	if (cmd != CPIA2_CID_GPIO)
-		return -EINVAL;
-
-	gpio_val = *(__u32*) arg;
-
-	if (gpio_val &~ 0xFFU)
-		return -EINVAL;
-
-	return cpia2_set_gpio(cam, (unsigned char)gpio_val);
-}
-
-/******************************************************************************
- *
  *  ioctl_querycap
  *
  *  V4L2 device capabilities
@@ -465,9 +246,11 @@ static int cpia2_querycap(struct file *file, void *fh, struct v4l2_capability *v
 	if (usb_make_path(cam->dev, vc->bus_info, sizeof(vc->bus_info)) <0)
 		memset(vc->bus_info,0, sizeof(vc->bus_info));
 
-	vc->capabilities = V4L2_CAP_VIDEO_CAPTURE |
+	vc->device_caps = V4L2_CAP_VIDEO_CAPTURE |
 			   V4L2_CAP_READWRITE |
 			   V4L2_CAP_STREAMING;
+	vc->capabilities = vc->device_caps |
+			   V4L2_CAP_DEVICE_CAPS;
 
 	return 0;
 }
@@ -610,22 +393,12 @@ static int cpia2_s_fmt_vid_cap(struct file *file, void *_fh,
 					struct v4l2_format *f)
 {
 	struct camera_data *cam = video_drvdata(file);
-	struct cpia2_fh *fh = _fh;
 	int err, frame;
 
-	err = v4l2_prio_check(&cam->prio, fh->prio);
-	if (err)
-		return err;
 	err = cpia2_try_fmt_vid_cap(file, _fh, f);
 	if(err != 0)
 		return err;
 
-	/* Ensure that only this process can change the format. */
-	err = v4l2_prio_change(&cam->prio, &fh->prio, V4L2_PRIORITY_RECORD);
-	if(err != 0) {
-		return err;
-	}
-
 	cam->pixelformat = f->fmt.pix.pixelformat;
 
 	/* NOTE: This should be set to 1 for MJPEG, but some apps don't handle
@@ -713,240 +486,126 @@ static int cpia2_cropcap(struct file *file, void *fh, struct v4l2_cropcap *c)
 	return 0;
 }
 
-/******************************************************************************
- *
- *  ioctl_queryctrl
- *
- *  V4L2 query possible control variables
- *
- *****************************************************************************/
+struct framerate_info {
+	int value;
+	struct v4l2_fract period;
+};
 
-static int cpia2_queryctrl(struct file *file, void *fh, struct v4l2_queryctrl *c)
+static const struct framerate_info framerate_controls[] = {
+	{ CPIA2_VP_FRAMERATE_6_25, { 4, 25 } },
+	{ CPIA2_VP_FRAMERATE_7_5,  { 2, 15 } },
+	{ CPIA2_VP_FRAMERATE_12_5, { 2, 25 } },
+	{ CPIA2_VP_FRAMERATE_15,   { 1, 15 } },
+	{ CPIA2_VP_FRAMERATE_25,   { 1, 25 } },
+	{ CPIA2_VP_FRAMERATE_30,   { 1, 30 } },
+};
+
+static int cpia2_g_parm(struct file *file, void *fh, struct v4l2_streamparm *p)
 {
 	struct camera_data *cam = video_drvdata(file);
+	struct v4l2_captureparm *cap = &p->parm.capture;
 	int i;
 
-	for(i=0; i<NUM_CONTROLS; ++i) {
-		if(c->id == controls[i].id) {
-			memcpy(c, controls+i, sizeof(*c));
-			break;
-		}
-	}
-
-	if(i == NUM_CONTROLS)
+	if (p->type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
 		return -EINVAL;
 
-	/* Some devices have additional limitations */
-	switch(c->id) {
-	case V4L2_CID_BRIGHTNESS:
-		/***
-		 * Don't let the register be set to zero - bug in VP4
-		 * flash of full brightness
-		 ***/
-		if (cam->params.pnp_id.device_type == DEVICE_STV_672)
-			c->minimum = 1;
-		break;
-	case V4L2_CID_VFLIP:
-		// VP5 Only
-		if(cam->params.pnp_id.device_type == DEVICE_STV_672)
-			c->flags |= V4L2_CTRL_FLAG_DISABLED;
-		break;
-	case CPIA2_CID_FRAMERATE:
-		if(cam->params.pnp_id.device_type == DEVICE_STV_672 &&
-		   cam->params.version.sensor_flags==CPIA2_VP_SENSOR_FLAGS_500){
-			// Maximum 15fps
-			for(i=0; i<c->maximum; ++i) {
-				if(framerate_controls[i].value ==
-				   CPIA2_VP_FRAMERATE_15) {
-					c->maximum = i;
-					c->default_value = i;
-				}
-			}
+	cap->capability = V4L2_CAP_TIMEPERFRAME;
+	cap->readbuffers = cam->num_frames;
+	for (i = 0; i < ARRAY_SIZE(framerate_controls); i++)
+		if (cam->params.vp_params.frame_rate == framerate_controls[i].value) {
+			cap->timeperframe = framerate_controls[i].period;
+			break;
 		}
-		break;
-	case CPIA2_CID_FLICKER_MODE:
-		// Flicker control only valid for 672.
-		if(cam->params.pnp_id.device_type != DEVICE_STV_672)
-			c->flags |= V4L2_CTRL_FLAG_DISABLED;
-		break;
-	case CPIA2_CID_LIGHTS:
-		// Light control only valid for the QX5 Microscope.
-		if(cam->params.pnp_id.product != 0x151)
-			c->flags |= V4L2_CTRL_FLAG_DISABLED;
-		break;
-	default:
-		break;
-	}
-
 	return 0;
 }
 
-/******************************************************************************
- *
- *  ioctl_querymenu
- *
- *  V4L2 query possible control variables
- *
- *****************************************************************************/
-
-static int cpia2_querymenu(struct file *file, void *fh, struct v4l2_querymenu *m)
+static int cpia2_s_parm(struct file *file, void *fh, struct v4l2_streamparm *p)
 {
 	struct camera_data *cam = video_drvdata(file);
+	struct v4l2_captureparm *cap = &p->parm.capture;
+	struct v4l2_fract tpf = cap->timeperframe;
+	int max = ARRAY_SIZE(framerate_controls) - 1;
+	int ret;
+	int i;
 
-	switch(m->id) {
-	case CPIA2_CID_FLICKER_MODE:
-		if (m->index >= NUM_FLICKER_CONTROLS)
-			return -EINVAL;
+	ret = cpia2_g_parm(file, fh, p);
+	if (ret || !tpf.denominator || !tpf.numerator)
+		return ret;
+
+	/* Maximum 15 fps for this model */
+	if (cam->params.pnp_id.device_type == DEVICE_STV_672 &&
+	    cam->params.version.sensor_flags == CPIA2_VP_SENSOR_FLAGS_500)
+		max -= 2;
+	for (i = 0; i <= max; i++) {
+		struct v4l2_fract f1 = tpf;
+		struct v4l2_fract f2 = framerate_controls[i].period;
+
+		f1.numerator *= f2.denominator;
+		f2.numerator *= f1.denominator;
+		if (f1.numerator >= f2.numerator)
+			break;
+	}
+	if (i > max)
+		i = max;
+	cap->timeperframe = framerate_controls[i].period;
+	return cpia2_set_fps(cam, framerate_controls[i].value);
+}
 
-		strcpy(m->name, flicker_controls[m->index].name);
-		break;
-	case CPIA2_CID_FRAMERATE:
-	    {
-		int maximum = NUM_FRAMERATE_CONTROLS - 1;
-		if(cam->params.pnp_id.device_type == DEVICE_STV_672 &&
-		   cam->params.version.sensor_flags==CPIA2_VP_SENSOR_FLAGS_500){
-			// Maximum 15fps
-			int i;
-			for(i=0; i<maximum; ++i) {
-				if(framerate_controls[i].value ==
-				   CPIA2_VP_FRAMERATE_15)
-					maximum = i;
-			}
-		}
-		if (m->index > maximum)
-			return -EINVAL;
+static const struct {
+	u32 width;
+	u32 height;
+} cpia2_framesizes[] = {
+	{ 640, 480 },
+	{ 352, 288 },
+	{ 320, 240 },
+	{ 288, 216 },
+	{ 256, 192 },
+	{ 224, 168 },
+	{ 192, 144 },
+	{ 176, 144 },
+};
 
-		strcpy(m->name, framerate_controls[m->index].name);
-		break;
-	    }
-	case CPIA2_CID_LIGHTS:
-		if (m->index >= NUM_LIGHTS_CONTROLS)
-			return -EINVAL;
+static int cpia2_enum_framesizes(struct file *file, void *fh,
+					 struct v4l2_frmsizeenum *fsize)
+{
 
-		strcpy(m->name, lights_controls[m->index].name);
-		break;
-	default:
+	if (fsize->pixel_format != V4L2_PIX_FMT_MJPEG &&
+	    fsize->pixel_format != V4L2_PIX_FMT_JPEG)
 		return -EINVAL;
-	}
+	if (fsize->index >= ARRAY_SIZE(cpia2_framesizes))
+		return -EINVAL;
+	fsize->type = V4L2_FRMSIZE_TYPE_DISCRETE;
+	fsize->discrete.width = cpia2_framesizes[fsize->index].width;
+	fsize->discrete.height = cpia2_framesizes[fsize->index].height;
 
 	return 0;
 }
 
-/******************************************************************************
- *
- *  ioctl_g_ctrl
- *
- *  V4L2 get the value of a control variable
- *
- *****************************************************************************/
-
-static int cpia2_g_ctrl(struct file *file, void *fh, struct v4l2_control *c)
+static int cpia2_enum_frameintervals(struct file *file, void *fh,
+					   struct v4l2_frmivalenum *fival)
 {
 	struct camera_data *cam = video_drvdata(file);
+	int max = ARRAY_SIZE(framerate_controls) - 1;
+	int i;
 
-	switch(c->id) {
-	case V4L2_CID_BRIGHTNESS:
-		cpia2_do_command(cam, CPIA2_CMD_GET_VP_BRIGHTNESS,
-				 TRANSFER_READ, 0);
-		c->value = cam->params.color_params.brightness;
-		break;
-	case V4L2_CID_CONTRAST:
-		cpia2_do_command(cam, CPIA2_CMD_GET_CONTRAST,
-				 TRANSFER_READ, 0);
-		c->value = cam->params.color_params.contrast;
-		break;
-	case V4L2_CID_SATURATION:
-		cpia2_do_command(cam, CPIA2_CMD_GET_VP_SATURATION,
-				 TRANSFER_READ, 0);
-		c->value = cam->params.color_params.saturation;
-		break;
-	case V4L2_CID_HFLIP:
-		cpia2_do_command(cam, CPIA2_CMD_GET_USER_EFFECTS,
-				 TRANSFER_READ, 0);
-		c->value = (cam->params.vp_params.user_effects &
-			    CPIA2_VP_USER_EFFECTS_MIRROR) != 0;
-		break;
-	case V4L2_CID_VFLIP:
-		cpia2_do_command(cam, CPIA2_CMD_GET_USER_EFFECTS,
-				 TRANSFER_READ, 0);
-		c->value = (cam->params.vp_params.user_effects &
-			    CPIA2_VP_USER_EFFECTS_FLIP) != 0;
-		break;
-	case CPIA2_CID_TARGET_KB:
-		c->value = cam->params.vc_params.target_kb;
-		break;
-	case CPIA2_CID_GPIO:
-		cpia2_do_command(cam, CPIA2_CMD_GET_VP_GPIO_DATA,
-				 TRANSFER_READ, 0);
-		c->value = cam->params.vp_params.gpio_data;
-		break;
-	case CPIA2_CID_FLICKER_MODE:
-	{
-		int i, mode;
-		cpia2_do_command(cam, CPIA2_CMD_GET_FLICKER_MODES,
-				 TRANSFER_READ, 0);
-		if(cam->params.flicker_control.cam_register &
-		   CPIA2_VP_FLICKER_MODES_NEVER_FLICKER) {
-			mode = NEVER_FLICKER;
-		} else {
-		    if(cam->params.flicker_control.cam_register &
-		       CPIA2_VP_FLICKER_MODES_50HZ) {
-			mode = FLICKER_50;
-		    } else {
-			mode = FLICKER_60;
-		    }
-		}
-		for(i=0; i<NUM_FLICKER_CONTROLS; i++) {
-			if(flicker_controls[i].value == mode) {
-				c->value = i;
-				break;
-			}
-		}
-		if(i == NUM_FLICKER_CONTROLS)
-			return -EINVAL;
-		break;
-	}
-	case CPIA2_CID_FRAMERATE:
-	{
-		int maximum = NUM_FRAMERATE_CONTROLS - 1;
-		int i;
-		for(i=0; i<= maximum; i++) {
-			if(cam->params.vp_params.frame_rate ==
-			   framerate_controls[i].value)
-				break;
-		}
-		if(i > maximum)
-			return -EINVAL;
-		c->value = i;
-		break;
-	}
-	case CPIA2_CID_USB_ALT:
-		c->value = cam->params.camera_state.stream_mode;
-		break;
-	case CPIA2_CID_LIGHTS:
-	{
-		int i;
-		cpia2_do_command(cam, CPIA2_CMD_GET_VP_GPIO_DATA,
-				 TRANSFER_READ, 0);
-		for(i=0; i<NUM_LIGHTS_CONTROLS; i++) {
-			if((cam->params.vp_params.gpio_data&GPIO_LIGHTS_MASK) ==
-			   lights_controls[i].value) {
-				break;
-			}
-		}
-		if(i == NUM_LIGHTS_CONTROLS)
-			return -EINVAL;
-		c->value = i;
-		break;
-	}
-	case CPIA2_CID_RESET_CAMERA:
+	if (fival->pixel_format != V4L2_PIX_FMT_MJPEG &&
+	    fival->pixel_format != V4L2_PIX_FMT_JPEG)
 		return -EINVAL;
-	default:
-		return -EINVAL;
-	}
-
-	DBG("Get control id:%d, value:%d\n", c->id, c->value);
 
+	/* Maximum 15 fps for this model */
+	if (cam->params.pnp_id.device_type == DEVICE_STV_672 &&
+	    cam->params.version.sensor_flags == CPIA2_VP_SENSOR_FLAGS_500)
+		max -= 2;
+	if (fival->index > max)
+		return -EINVAL;
+	for (i = 0; i < ARRAY_SIZE(cpia2_framesizes); i++)
+		if (fival->width == cpia2_framesizes[i].width &&
+		    fival->height == cpia2_framesizes[i].height)
+			break;
+	if (i == ARRAY_SIZE(cpia2_framesizes))
+		return -EINVAL;
+	fival->type = V4L2_FRMIVAL_TYPE_DISCRETE;
+	fival->discrete = framerate_controls[fival->index].period;
 	return 0;
 }
 
@@ -958,72 +617,54 @@ static int cpia2_g_ctrl(struct file *file, void *fh, struct v4l2_control *c)
  *
  *****************************************************************************/
 
-static int cpia2_s_ctrl(struct file *file, void *fh, struct v4l2_control *c)
+static int cpia2_s_ctrl(struct v4l2_ctrl *ctrl)
 {
-	struct camera_data *cam = video_drvdata(file);
-	int i;
-	int retval = 0;
+	struct camera_data *cam =
+		container_of(ctrl->handler, struct camera_data, hdl);
+	static const int flicker_table[] = {
+		NEVER_FLICKER,
+		FLICKER_50,
+		FLICKER_60,
+	};
 
-	DBG("Set control id:%d, value:%d\n", c->id, c->value);
-
-	/* Check that the value is in range */
-	for(i=0; i<NUM_CONTROLS; i++) {
-		if(c->id == controls[i].id) {
-			if(c->value < controls[i].minimum ||
-			   c->value > controls[i].maximum) {
-				return -EINVAL;
-			}
-			break;
-		}
-	}
-	if(i == NUM_CONTROLS)
-		return -EINVAL;
+	DBG("Set control id:%d, value:%d\n", ctrl->id, ctrl->val);
 
-	switch(c->id) {
+	switch (ctrl->id) {
 	case V4L2_CID_BRIGHTNESS:
-		cpia2_set_brightness(cam, c->value);
+		cpia2_set_brightness(cam, ctrl->val);
 		break;
 	case V4L2_CID_CONTRAST:
-		cpia2_set_contrast(cam, c->value);
+		cpia2_set_contrast(cam, ctrl->val);
 		break;
 	case V4L2_CID_SATURATION:
-		cpia2_set_saturation(cam, c->value);
+		cpia2_set_saturation(cam, ctrl->val);
 		break;
 	case V4L2_CID_HFLIP:
-		cpia2_set_property_mirror(cam, c->value);
+		cpia2_set_property_mirror(cam, ctrl->val);
 		break;
 	case V4L2_CID_VFLIP:
-		cpia2_set_property_flip(cam, c->value);
+		cpia2_set_property_flip(cam, ctrl->val);
 		break;
-	case CPIA2_CID_TARGET_KB:
-		retval = cpia2_set_target_kb(cam, c->value);
+	case V4L2_CID_POWER_LINE_FREQUENCY:
+		return cpia2_set_flicker_mode(cam, flicker_table[ctrl->val]);
+	case V4L2_CID_ILLUMINATORS_1:
+		return cpia2_set_gpio(cam, (cam->top_light->val << 6) |
+					   (cam->bottom_light->val << 7));
+	case V4L2_CID_JPEG_ACTIVE_MARKER:
+		cam->params.compression.inhibit_htables =
+			!(ctrl->val & V4L2_JPEG_ACTIVE_MARKER_DHT);
 		break;
-	case CPIA2_CID_GPIO:
-		retval = cpia2_set_gpio(cam, c->value);
-		break;
-	case CPIA2_CID_FLICKER_MODE:
-		retval = cpia2_set_flicker_mode(cam,
-					      flicker_controls[c->value].value);
-		break;
-	case CPIA2_CID_FRAMERATE:
-		retval = cpia2_set_fps(cam, framerate_controls[c->value].value);
+	case V4L2_CID_JPEG_COMPRESSION_QUALITY:
+		cam->params.vc_params.quality = ctrl->val;
 		break;
 	case CPIA2_CID_USB_ALT:
-		retval = cpia2_usb_change_streaming_alternate(cam, c->value);
-		break;
-	case CPIA2_CID_LIGHTS:
-		retval = cpia2_set_gpio(cam, lights_controls[c->value].value);
-		break;
-	case CPIA2_CID_RESET_CAMERA:
-		cpia2_usb_stream_pause(cam);
-		cpia2_reset_camera(cam);
-		cpia2_usb_stream_resume(cam);
+		cam->params.camera_state.stream_mode = ctrl->val;
 		break;
 	default:
-		retval = -EINVAL;
+		return -EINVAL;
 	}
 
-	return retval;
+	return 0;
 }
 
 /******************************************************************************
@@ -1084,6 +725,8 @@ static int cpia2_s_jpegcomp(struct file *file, void *fh, struct v4l2_jpegcompres
 
 	cam->params.compression.inhibit_htables =
 		!(parms->jpeg_markers & V4L2_JPEG_MARKER_DHT);
+	parms->jpeg_markers &= V4L2_JPEG_MARKER_DQT | V4L2_JPEG_MARKER_DRI |
+			       V4L2_JPEG_MARKER_DHT;
 
 	if(parms->APP_len != 0) {
 		if(parms->APP_len > 0 &&
@@ -1270,12 +913,12 @@ static int cpia2_dqbuf(struct file *file, void *fh, struct v4l2_buffer *buf)
 		struct framebuf *cb=cam->curbuff;
 		mutex_unlock(&cam->v4l2_lock);
 		wait_event_interruptible(cam->wq_stream,
-					 !cam->present ||
+					 !video_is_registered(&cam->vdev) ||
 					 (cb=cam->curbuff)->status == FRAME_READY);
 		mutex_lock(&cam->v4l2_lock);
 		if (signal_pending(current))
 			return -ERESTARTSYS;
-		if(!cam->present)
+		if (!video_is_registered(&cam->vdev))
 			return -ENOTTY;
 		frame = cb->num;
 	}
@@ -1299,56 +942,39 @@ static int cpia2_dqbuf(struct file *file, void *fh, struct v4l2_buffer *buf)
 	return 0;
 }
 
-static int cpia2_g_priority(struct file *file, void *_fh, enum v4l2_priority *p)
-{
-	struct cpia2_fh *fh = _fh;
-
-	*p = fh->prio;
-	return 0;
-}
-
-static int cpia2_s_priority(struct file *file, void *_fh, enum v4l2_priority prio)
-{
-	struct camera_data *cam = video_drvdata(file);
-	struct cpia2_fh *fh = _fh;
-
-	if (cam->streaming && prio != fh->prio &&
-			fh->prio == V4L2_PRIORITY_RECORD)
-		/* Can't drop record priority while streaming */
-		return -EBUSY;
-
-	if (prio == V4L2_PRIORITY_RECORD && prio != fh->prio &&
-			v4l2_prio_max(&cam->prio) == V4L2_PRIORITY_RECORD)
-		/* Only one program can record at a time */
-		return -EBUSY;
-	return v4l2_prio_change(&cam->prio, &fh->prio, prio);
-}
-
 static int cpia2_streamon(struct file *file, void *fh, enum v4l2_buf_type type)
 {
 	struct camera_data *cam = video_drvdata(file);
+	int ret = -EINVAL;
 
 	DBG("VIDIOC_STREAMON, streaming=%d\n", cam->streaming);
 	if (!cam->mmapped || type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
 		return -EINVAL;
 
-	if (!cam->streaming)
-		return cpia2_usb_stream_start(cam,
+	if (!cam->streaming) {
+		ret = cpia2_usb_stream_start(cam,
 				cam->params.camera_state.stream_mode);
-	return -EINVAL;
+		if (!ret)
+			v4l2_ctrl_grab(cam->usb_alt, true);
+	}
+	return ret;
 }
 
 static int cpia2_streamoff(struct file *file, void *fh, enum v4l2_buf_type type)
 {
 	struct camera_data *cam = video_drvdata(file);
+	int ret = -EINVAL;
 
 	DBG("VIDIOC_STREAMOFF, streaming=%d\n", cam->streaming);
 	if (!cam->mmapped || type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
 		return -EINVAL;
 
-	if (cam->streaming)
-		return cpia2_usb_stream_stop(cam);
-	return -EINVAL;
+	if (cam->streaming) {
+		ret = cpia2_usb_stream_stop(cam);
+		if (!ret)
+			v4l2_ctrl_grab(cam->usb_alt, false);
+	}
+	return ret;
 }
 
 /******************************************************************************
@@ -1361,16 +987,10 @@ static int cpia2_mmap(struct file *file, struct vm_area_struct *area)
 	struct camera_data *cam = video_drvdata(file);
 	int retval;
 
-	/* Priority check */
-	struct cpia2_fh *fh = file->private_data;
-	if(fh->prio != V4L2_PRIORITY_RECORD) {
-		return -EBUSY;
-	}
-
 	retval = cpia2_remap_buffer(cam, area);
 
 	if(!retval)
-		fh->mmapped = 1;
+		cam->stream_fh = file->private_data;
 	return retval;
 }
 
@@ -1388,15 +1008,13 @@ static void reset_camera_struct_v4l(struct camera_data *cam)
 	cam->frame_size = buffer_size;
 	cam->num_frames = num_buffers;
 
-	/* FlickerModes */
+	/* Flicker modes */
 	cam->params.flicker_control.flicker_mode_req = flicker_mode;
-	cam->params.flicker_control.mains_frequency = flicker_freq;
 
-	/* streamMode */
+	/* stream modes */
 	cam->params.camera_state.stream_mode = alternate;
 
 	cam->pixelformat = V4L2_PIX_FMT_JPEG;
-	v4l2_prio_init(&cam->prio);
 }
 
 static const struct v4l2_ioctl_ops cpia2_ioctl_ops = {
@@ -1408,10 +1026,6 @@ static const struct v4l2_ioctl_ops cpia2_ioctl_ops = {
 	.vidioc_g_fmt_vid_cap		    = cpia2_g_fmt_vid_cap,
 	.vidioc_s_fmt_vid_cap		    = cpia2_s_fmt_vid_cap,
 	.vidioc_try_fmt_vid_cap		    = cpia2_try_fmt_vid_cap,
-	.vidioc_queryctrl		    = cpia2_queryctrl,
-	.vidioc_querymenu		    = cpia2_querymenu,
-	.vidioc_g_ctrl			    = cpia2_g_ctrl,
-	.vidioc_s_ctrl			    = cpia2_s_ctrl,
 	.vidioc_g_jpegcomp		    = cpia2_g_jpegcomp,
 	.vidioc_s_jpegcomp		    = cpia2_s_jpegcomp,
 	.vidioc_cropcap			    = cpia2_cropcap,
@@ -1421,9 +1035,12 @@ static const struct v4l2_ioctl_ops cpia2_ioctl_ops = {
 	.vidioc_dqbuf			    = cpia2_dqbuf,
 	.vidioc_streamon		    = cpia2_streamon,
 	.vidioc_streamoff		    = cpia2_streamoff,
-	.vidioc_g_priority		    = cpia2_g_priority,
-	.vidioc_s_priority		    = cpia2_s_priority,
-	.vidioc_default			    = cpia2_default,
+	.vidioc_s_parm			    = cpia2_s_parm,
+	.vidioc_g_parm			    = cpia2_g_parm,
+	.vidioc_enum_framesizes		    = cpia2_enum_framesizes,
+	.vidioc_enum_frameintervals	    = cpia2_enum_frameintervals,
+	.vidioc_subscribe_event		    = v4l2_ctrl_subscribe_event,
+	.vidioc_unsubscribe_event	    = v4l2_event_unsubscribe,
 };
 
 /***
@@ -1444,7 +1061,21 @@ static struct video_device cpia2_template = {
 	.name =		"CPiA2 Camera",
 	.fops =		&cpia2_fops,
 	.ioctl_ops =	&cpia2_ioctl_ops,
-	.release =	video_device_release,
+	.release =	video_device_release_empty,
+};
+
+void cpia2_camera_release(struct v4l2_device *v4l2_dev)
+{
+	struct camera_data *cam =
+		container_of(v4l2_dev, struct camera_data, v4l2_dev);
+
+	v4l2_ctrl_handler_free(&cam->hdl);
+	v4l2_device_unregister(&cam->v4l2_dev);
+	kfree(cam);
+}
+
+static const struct v4l2_ctrl_ops cpia2_ctrl_ops = {
+	.s_ctrl = cpia2_s_ctrl,
 };
 
 /******************************************************************************
@@ -1454,20 +1085,78 @@ static struct video_device cpia2_template = {
  *****************************************************************************/
 int cpia2_register_camera(struct camera_data *cam)
 {
-	cam->vdev = video_device_alloc();
-	if(!cam->vdev)
-		return -ENOMEM;
+	struct v4l2_ctrl_handler *hdl = &cam->hdl;
+	struct v4l2_ctrl_config cpia2_usb_alt = {
+		.ops = &cpia2_ctrl_ops,
+		.id = CPIA2_CID_USB_ALT,
+		.name = "USB Alternate",
+		.type = V4L2_CTRL_TYPE_INTEGER,
+		.min = USBIF_ISO_1,
+		.max = USBIF_ISO_6,
+		.step = 1,
+	};
+	int ret;
+
+	v4l2_ctrl_handler_init(hdl, 12);
+	v4l2_ctrl_new_std(hdl, &cpia2_ctrl_ops,
+			V4L2_CID_BRIGHTNESS,
+			cam->params.pnp_id.device_type == DEVICE_STV_672 ? 1 : 0,
+			255, 1, DEFAULT_BRIGHTNESS);
+	v4l2_ctrl_new_std(hdl, &cpia2_ctrl_ops,
+			V4L2_CID_CONTRAST, 0, 255, 1, DEFAULT_CONTRAST);
+	v4l2_ctrl_new_std(hdl, &cpia2_ctrl_ops,
+			V4L2_CID_SATURATION, 0, 255, 1, DEFAULT_SATURATION);
+	v4l2_ctrl_new_std(hdl, &cpia2_ctrl_ops,
+			V4L2_CID_HFLIP, 0, 1, 1, 0);
+	v4l2_ctrl_new_std(hdl, &cpia2_ctrl_ops,
+			V4L2_CID_JPEG_ACTIVE_MARKER, 0,
+			V4L2_JPEG_ACTIVE_MARKER_DHT, 0,
+			V4L2_JPEG_ACTIVE_MARKER_DHT);
+	v4l2_ctrl_new_std(hdl, &cpia2_ctrl_ops,
+			V4L2_CID_JPEG_COMPRESSION_QUALITY, 1,
+			100, 1, 100);
+	cpia2_usb_alt.def = alternate;
+	cam->usb_alt = v4l2_ctrl_new_custom(hdl, &cpia2_usb_alt, NULL);
+	/* VP5 Only */
+	if (cam->params.pnp_id.device_type != DEVICE_STV_672)
+		v4l2_ctrl_new_std(hdl, &cpia2_ctrl_ops,
+			V4L2_CID_VFLIP, 0, 1, 1, 0);
+	/* Flicker control only valid for 672 */
+	if (cam->params.pnp_id.device_type == DEVICE_STV_672)
+		v4l2_ctrl_new_std_menu(hdl, &cpia2_ctrl_ops,
+			V4L2_CID_POWER_LINE_FREQUENCY,
+			V4L2_CID_POWER_LINE_FREQUENCY_60HZ, 0, 0);
+	/* Light control only valid for the QX5 Microscope */
+	if (cam->params.pnp_id.product == 0x151) {
+		cam->top_light = v4l2_ctrl_new_std(hdl, &cpia2_ctrl_ops,
+				V4L2_CID_ILLUMINATORS_1, 0, 1, 1, 0);
+		cam->bottom_light = v4l2_ctrl_new_std(hdl, &cpia2_ctrl_ops,
+				V4L2_CID_ILLUMINATORS_2, 0, 1, 1, 0);
+		v4l2_ctrl_cluster(2, &cam->top_light);
+	}
 
-	memcpy(cam->vdev, &cpia2_template, sizeof(cpia2_template));
-	video_set_drvdata(cam->vdev, cam);
-	cam->vdev->lock = &cam->v4l2_lock;
+	if (hdl->error) {
+		ret = hdl->error;
+		v4l2_ctrl_handler_free(hdl);
+		return ret;
+	}
+
+	cam->vdev = cpia2_template;
+	video_set_drvdata(&cam->vdev, cam);
+	cam->vdev.lock = &cam->v4l2_lock;
+	cam->vdev.ctrl_handler = hdl;
+	cam->vdev.v4l2_dev = &cam->v4l2_dev;
+	set_bit(V4L2_FL_USE_FH_PRIO, &cam->vdev.flags);
+	/* Locking in file operations other than ioctl should be done
+	   by the driver, not the V4L2 core.
+	   This driver needs auditing so that this flag can be removed. */
+	set_bit(V4L2_FL_LOCK_ALL_FOPS, &cam->vdev.flags);
 
 	reset_camera_struct_v4l(cam);
 
 	/* register v4l device */
-	if (video_register_device(cam->vdev, VFL_TYPE_GRABBER, video_nr) < 0) {
+	if (video_register_device(&cam->vdev, VFL_TYPE_GRABBER, video_nr) < 0) {
 		ERR("video_register_device failed\n");
-		video_device_release(cam->vdev);
 		return -ENODEV;
 	}
 
@@ -1481,13 +1170,7 @@ int cpia2_register_camera(struct camera_data *cam)
  *****************************************************************************/
 void cpia2_unregister_camera(struct camera_data *cam)
 {
-	if (!cam->open_count) {
-		video_unregister_device(cam->vdev);
-	} else {
-		LOG("%s removed while open, deferring "
-		    "video_unregister_device\n",
-		    video_device_node_name(cam->vdev));
-	}
+	video_unregister_device(&cam->vdev);
 }
 
 /******************************************************************************
@@ -1524,23 +1207,12 @@ static void __init check_parameters(void)
 		LOG("alternate specified is invalid, using %d\n", alternate);
 	}
 
-	if (flicker_mode != NEVER_FLICKER && flicker_mode != ANTI_FLICKER_ON) {
-		flicker_mode = NEVER_FLICKER;
+	if (flicker_mode != 0 && flicker_mode != FLICKER_50 && flicker_mode != FLICKER_60) {
+		flicker_mode = 0;
 		LOG("Flicker mode specified is invalid, using %d\n",
 		    flicker_mode);
 	}
 
-	if (flicker_freq != FLICKER_50 && flicker_freq != FLICKER_60) {
-		flicker_freq = FLICKER_60;
-		LOG("Flicker mode specified is invalid, using %d\n",
-		    flicker_freq);
-	}
-
-	if(video_nr < -1 || video_nr > 64) {
-		video_nr = -1;
-		LOG("invalid video_nr specified, must be -1 to 64\n");
-	}
-
 	DBG("Using %d buffers, each %d bytes, alternate=%d\n",
 	    num_buffers, buffer_size, alternate);
 }
diff --git a/drivers/media/video/cpia2/cpia2dev.h b/drivers/media/video/cpia2/cpia2dev.h
deleted file mode 100644
index f66691fe5a3..00000000000
--- a/drivers/media/video/cpia2/cpia2dev.h
+++ /dev/null
@@ -1,50 +0,0 @@
-/****************************************************************************
- *
- *  Filename: cpia2dev.h
- *
- *  Copyright 2001, STMicrolectronics, Inc.
- *
- *  Contact:  steve.miller@st.com
- *
- *  Description:
- *     This file provides definitions for applications wanting to use the
- *     cpia2 driver beyond the generic v4l capabilities.
- *
- *  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 CPIA2_DEV_HEADER
-#define CPIA2_DEV_HEADER
-
-#include <linux/videodev2.h>
-
-/***
- * The following defines are ioctl numbers based on video4linux private ioctls,
- * which can range from 192 (BASE_VIDIOCPRIVATE) to 255. All of these take int
- * args
- */
-#define CPIA2_IOC_SET_GPIO         _IOW('v', BASE_VIDIOC_PRIVATE + 17, __u32)
-
-/* V4L2 driver specific controls */
-#define CPIA2_CID_TARGET_KB     (V4L2_CID_PRIVATE_BASE+0)
-#define CPIA2_CID_GPIO          (V4L2_CID_PRIVATE_BASE+1)
-#define CPIA2_CID_FLICKER_MODE  (V4L2_CID_PRIVATE_BASE+2)
-#define CPIA2_CID_FRAMERATE     (V4L2_CID_PRIVATE_BASE+3)
-#define CPIA2_CID_USB_ALT       (V4L2_CID_PRIVATE_BASE+4)
-#define CPIA2_CID_LIGHTS        (V4L2_CID_PRIVATE_BASE+5)
-#define CPIA2_CID_RESET_CAMERA  (V4L2_CID_PRIVATE_BASE+6)
-
-#endif
diff --git a/drivers/media/video/cx18/cx18-alsa-main.c b/drivers/media/video/cx18/cx18-alsa-main.c
index e118361c2e7..6d2a98246b6 100644
--- a/drivers/media/video/cx18/cx18-alsa-main.c
+++ b/drivers/media/video/cx18/cx18-alsa-main.c
@@ -285,6 +285,7 @@ static void __exit cx18_alsa_exit(void)
 
 	drv = driver_find("cx18", &pci_bus_type);
 	ret = driver_for_each_device(drv, NULL, NULL, cx18_alsa_exit_callback);
+	(void)ret;	/* suppress compiler warning */
 
 	cx18_ext_init = NULL;
 	printk(KERN_INFO "cx18-alsa: module unload complete\n");
diff --git a/drivers/media/video/cx18/cx18-alsa-pcm.c b/drivers/media/video/cx18/cx18-alsa-pcm.c
index 82d195be919..7a5b84a86bb 100644
--- a/drivers/media/video/cx18/cx18-alsa-pcm.c
+++ b/drivers/media/video/cx18/cx18-alsa-pcm.c
@@ -190,7 +190,7 @@ static int snd_cx18_pcm_capture_open(struct snd_pcm_substream *substream)
 	ret = cx18_start_v4l2_encode_stream(s);
 	snd_cx18_unlock(cxsc);
 
-	return 0;
+	return ret;
 }
 
 static int snd_cx18_pcm_capture_close(struct snd_pcm_substream *substream)
@@ -199,12 +199,11 @@ static int snd_cx18_pcm_capture_close(struct snd_pcm_substream *substream)
 	struct v4l2_device *v4l2_dev = cxsc->v4l2_dev;
 	struct cx18 *cx = to_cx18(v4l2_dev);
 	struct cx18_stream *s;
-	int ret;
 
 	/* Instruct the cx18 to stop sending packets */
 	snd_cx18_lock(cxsc);
 	s = &cx->streams[CX18_ENC_STREAM_TYPE_PCM];
-	ret = cx18_stop_v4l2_encode_stream(s, 0);
+	cx18_stop_v4l2_encode_stream(s, 0);
 	clear_bit(CX18_F_S_STREAMING, &s->s_flags);
 
 	cx18_release_stream(s);
@@ -252,13 +251,10 @@ static int snd_pcm_alloc_vmalloc_buffer(struct snd_pcm_substream *subs,
 static int snd_cx18_pcm_hw_params(struct snd_pcm_substream *substream,
 			 struct snd_pcm_hw_params *params)
 {
-	int ret;
-
 	dprintk("%s called\n", __func__);
 
-	ret = snd_pcm_alloc_vmalloc_buffer(substream,
+	return snd_pcm_alloc_vmalloc_buffer(substream,
 					   params_buffer_bytes(params));
-	return 0;
 }
 
 static int snd_cx18_pcm_hw_free(struct snd_pcm_substream *substream)
diff --git a/drivers/media/video/cx18/cx18-ioctl.c b/drivers/media/video/cx18/cx18-ioctl.c
index be49f68ddf3..35fde4e931f 100644
--- a/drivers/media/video/cx18/cx18-ioctl.c
+++ b/drivers/media/video/cx18/cx18-ioctl.c
@@ -1137,7 +1137,7 @@ static long cx18_default(struct file *file, void *fh, bool valid_prio,
 	}
 
 	default:
-		return -EINVAL;
+		return -ENOTTY;
 	}
 	return 0;
 }
diff --git a/drivers/media/video/cx18/cx18-mailbox.c b/drivers/media/video/cx18/cx18-mailbox.c
index 0c7796e76ac..ed8118390b0 100644
--- a/drivers/media/video/cx18/cx18-mailbox.c
+++ b/drivers/media/video/cx18/cx18-mailbox.c
@@ -595,9 +595,8 @@ void cx18_api_epu_cmd_irq(struct cx18 *cx, int rpu)
 static int cx18_api_call(struct cx18 *cx, u32 cmd, int args, u32 data[])
 {
 	const struct cx18_api_info *info = find_api_info(cmd);
-	u32 state, irq, req, ack, err;
+	u32 irq, req, ack, err;
 	struct cx18_mailbox __iomem *mb;
-	u32 __iomem *xpu_state;
 	wait_queue_head_t *waitq;
 	struct mutex *mb_lock;
 	unsigned long int t0, timeout, ret;
@@ -628,14 +627,12 @@ static int cx18_api_call(struct cx18 *cx, u32 cmd, int args, u32 data[])
 		mb_lock = &cx->epu2apu_mb_lock;
 		irq = IRQ_EPU_TO_APU;
 		mb = &cx->scb->epu2apu_mb;
-		xpu_state = &cx->scb->apu_state;
 		break;
 	case CPU:
 		waitq = &cx->mb_cpu_waitq;
 		mb_lock = &cx->epu2cpu_mb_lock;
 		irq = IRQ_EPU_TO_CPU;
 		mb = &cx->scb->epu2cpu_mb;
-		xpu_state = &cx->scb->cpu_state;
 		break;
 	default:
 		CX18_WARN("Unknown RPU (%d) for API call\n", info->rpu);
@@ -653,7 +650,6 @@ static int cx18_api_call(struct cx18 *cx, u32 cmd, int args, u32 data[])
 	 * by a signal, we may get here and find a busy mailbox.  After waiting,
 	 * mark it "not busy" from our end, if the XPU hasn't ack'ed it still.
 	 */
-	state = cx18_readl(cx, xpu_state);
 	req = cx18_readl(cx, &mb->request);
 	timeout = msecs_to_jiffies(10);
 	ret = wait_event_timeout(*waitq,
diff --git a/drivers/media/video/cx18/cx18-streams.c b/drivers/media/video/cx18/cx18-streams.c
index 638cca156b5..4185bcb80ca 100644
--- a/drivers/media/video/cx18/cx18-streams.c
+++ b/drivers/media/video/cx18/cx18-streams.c
@@ -980,7 +980,6 @@ void cx18_stop_all_captures(struct cx18 *cx)
 int cx18_stop_v4l2_encode_stream(struct cx18_stream *s, int gop_end)
 {
 	struct cx18 *cx = s->cx;
-	unsigned long then;
 
 	if (!cx18_stream_enabled(s))
 		return -EINVAL;
@@ -999,8 +998,6 @@ int cx18_stop_v4l2_encode_stream(struct cx18_stream *s, int gop_end)
 	else
 		cx18_vapi(cx, CX18_CPU_CAPTURE_STOP, 1, s->handle);
 
-	then = jiffies;
-
 	if (s->type == CX18_ENC_STREAM_TYPE_MPG && gop_end) {
 		CX18_INFO("ignoring gop_end: not (yet?) supported by the firmware\n");
 	}
diff --git a/drivers/media/video/cx231xx/cx231xx-417.c b/drivers/media/video/cx231xx/cx231xx-417.c
index d4327dab5a3..ce2f62238a1 100644
--- a/drivers/media/video/cx231xx/cx231xx-417.c
+++ b/drivers/media/video/cx231xx/cx231xx-417.c
@@ -1095,7 +1095,7 @@ static int cx231xx_initialize_codec(struct cx231xx *dev)
 {
 	int version;
 	int retval;
-	u32 i, data[7];
+	u32 i;
 	u32 val = 0;
 
 	dprintk(1, "%s()\n", __func__);
@@ -1154,6 +1154,11 @@ static int cx231xx_initialize_codec(struct cx231xx *dev)
 		CX231xx_CUSTOM_EXTENSION_USR_DATA, 0, 0, 0, 0, 0, 0, 0, 0, 0,
 		0, 0);
 */
+
+#if 0
+	/* TODO */
+	u32 data[7];
+
 	/* Setup to capture VBI */
 	data[0] = 0x0001BD00;
 	data[1] = 1;          /* frames per interrupt */
@@ -1162,7 +1167,7 @@ static int cx231xx_initialize_codec(struct cx231xx *dev)
 	data[4] = 0x206080C0; /* stop codes */
 	data[5] = 6;          /* lines */
 	data[6] = 64;         /* BPL */
-/*
+
 	cx231xx_api_cmd(dev, CX2341X_ENC_SET_VBI_CONFIG, 7, 0, data[0], data[1],
 		data[2], data[3], data[4], data[5], data[6]);
 
@@ -1175,7 +1180,7 @@ static int cx231xx_initialize_codec(struct cx231xx *dev)
 		cx231xx_api_cmd(dev, CX2341X_ENC_SET_VBI_LINE, 5, 0,
 				i | 0x80000000, valid, 0, 0, 0);
 	}
-*/
+#endif
 /*	cx231xx_api_cmd(dev, CX2341X_ENC_MUTE_AUDIO, 1, 0, CX231xx_UNMUTE);
 	msleep(60);
 */
@@ -1792,17 +1797,16 @@ static int vidioc_streamon(struct file *file, void *priv,
 	struct cx231xx_fh  *fh  = file->private_data;
 
 	struct cx231xx *dev = fh->dev;
-	int rc = 0;
 	dprintk(3, "enter vidioc_streamon()\n");
 		cx231xx_set_alt_setting(dev, INDEX_TS1, 0);
-		rc = cx231xx_set_mode(dev, CX231XX_DIGITAL_MODE);
+		cx231xx_set_mode(dev, CX231XX_DIGITAL_MODE);
 		if (dev->USE_ISO)
-			rc = cx231xx_init_isoc(dev, CX231XX_NUM_PACKETS,
+			cx231xx_init_isoc(dev, CX231XX_NUM_PACKETS,
 				       CX231XX_NUM_BUFS,
 				       dev->video_mode.max_pkt_size,
 				       cx231xx_isoc_copy);
 		else {
-			rc = cx231xx_init_bulk(dev, 320,
+			cx231xx_init_bulk(dev, 320,
 				       5,
 				       dev->ts1_mode.max_pkt_size,
 				       cx231xx_bulk_copy);
diff --git a/drivers/media/video/cx231xx/cx231xx-audio.c b/drivers/media/video/cx231xx/cx231xx-audio.c
index a2c2b7d343e..068f78dc5d1 100644
--- a/drivers/media/video/cx231xx/cx231xx-audio.c
+++ b/drivers/media/video/cx231xx/cx231xx-audio.c
@@ -523,21 +523,24 @@ static int snd_cx231xx_pcm_close(struct snd_pcm_substream *substream)
 static int snd_cx231xx_hw_capture_params(struct snd_pcm_substream *substream,
 					 struct snd_pcm_hw_params *hw_params)
 {
-	unsigned int channels, rate, format;
 	int ret;
 
 	dprintk("Setting capture parameters\n");
 
 	ret = snd_pcm_alloc_vmalloc_buffer(substream,
 					   params_buffer_bytes(hw_params));
+#if 0
+	/* TODO: set up cx231xx audio chip to deliver the correct audio format,
+	   current default is 48000hz multiplexed => 96000hz mono
+	   which shouldn't matter since analogue TV only supports mono */
+	unsigned int channels, rate, format;
+
 	format = params_format(hw_params);
 	rate = params_rate(hw_params);
 	channels = params_channels(hw_params);
+#endif
 
-	/* TODO: set up cx231xx audio chip to deliver the correct audio format,
-	   current default is 48000hz multiplexed => 96000hz mono
-	   which shouldn't matter since analogue TV only supports mono */
-	return 0;
+	return ret;
 }
 
 static int snd_cx231xx_hw_capture_free(struct snd_pcm_substream *substream)
@@ -586,7 +589,7 @@ static int snd_cx231xx_capture_trigger(struct snd_pcm_substream *substream,
 				       int cmd)
 {
 	struct cx231xx *dev = snd_pcm_substream_chip(substream);
-	int retval;
+	int retval = 0;
 
 	if (dev->state & DEV_DISCONNECTED)
 		return -ENODEV;
@@ -601,12 +604,13 @@ static int snd_cx231xx_capture_trigger(struct snd_pcm_substream *substream,
 		break;
 	default:
 		retval = -EINVAL;
+		break;
 	}
 	spin_unlock(&dev->adev.slock);
 
 	schedule_work(&dev->wq_trigger);
 
-	return 0;
+	return retval;
 }
 
 static snd_pcm_uframes_t snd_cx231xx_capture_pointer(struct snd_pcm_substream
diff --git a/drivers/media/video/cx231xx/cx231xx-avcore.c b/drivers/media/video/cx231xx/cx231xx-avcore.c
index 53ff26e7abf..b085a3c6dc0 100644
--- a/drivers/media/video/cx231xx/cx231xx-avcore.c
+++ b/drivers/media/video/cx231xx/cx231xx-avcore.c
@@ -934,33 +934,29 @@ int cx231xx_set_decoder_video_input(struct cx231xx *dev,
 void cx231xx_enable656(struct cx231xx *dev)
 {
 	u8 temp = 0;
-	int status;
 	/*enable TS1 data[0:7] as output to export 656*/
 
-	status = vid_blk_write_byte(dev, TS1_PIN_CTL0, 0xFF);
+	vid_blk_write_byte(dev, TS1_PIN_CTL0, 0xFF);
 
 	/*enable TS1 clock as output to export 656*/
 
-	status = vid_blk_read_byte(dev, TS1_PIN_CTL1, &temp);
+	vid_blk_read_byte(dev, TS1_PIN_CTL1, &temp);
 	temp = temp|0x04;
 
-	status = vid_blk_write_byte(dev, TS1_PIN_CTL1, temp);
-
+	vid_blk_write_byte(dev, TS1_PIN_CTL1, temp);
 }
 EXPORT_SYMBOL_GPL(cx231xx_enable656);
 
 void cx231xx_disable656(struct cx231xx *dev)
 {
 	u8 temp = 0;
-	int status;
-
 
-	status = vid_blk_write_byte(dev, TS1_PIN_CTL0, 0x00);
+	vid_blk_write_byte(dev, TS1_PIN_CTL0, 0x00);
 
-	status = vid_blk_read_byte(dev, TS1_PIN_CTL1, &temp);
+	vid_blk_read_byte(dev, TS1_PIN_CTL1, &temp);
 	temp = temp&0xFB;
 
-	status = vid_blk_write_byte(dev, TS1_PIN_CTL1, temp);
+	vid_blk_write_byte(dev, TS1_PIN_CTL1, temp);
 }
 EXPORT_SYMBOL_GPL(cx231xx_disable656);
 
@@ -1320,117 +1316,115 @@ void update_HH_register_after_set_DIF(struct cx231xx *dev)
 
 void cx231xx_dump_HH_reg(struct cx231xx *dev)
 {
-	u8 status = 0;
 	u32 value = 0;
 	u16  i = 0;
 
 	value = 0x45005390;
-	status = vid_blk_write_word(dev, 0x104, value);
+	vid_blk_write_word(dev, 0x104, value);
 
 	for (i = 0x100; i < 0x140; i++) {
-		status = vid_blk_read_word(dev, i, &value);
+		vid_blk_read_word(dev, i, &value);
 		cx231xx_info("reg0x%x=0x%x\n", i, value);
 		i = i+3;
 	}
 
 	for (i = 0x300; i < 0x400; i++) {
-		status = vid_blk_read_word(dev, i, &value);
+		vid_blk_read_word(dev, i, &value);
 		cx231xx_info("reg0x%x=0x%x\n", i, value);
 		i = i+3;
 	}
 
 	for (i = 0x400; i < 0x440; i++) {
-		status = vid_blk_read_word(dev, i,  &value);
+		vid_blk_read_word(dev, i,  &value);
 		cx231xx_info("reg0x%x=0x%x\n", i, value);
 		i = i+3;
 	}
 
-	status = vid_blk_read_word(dev, AFE_CTRL_C2HH_SRC_CTRL, &value);
+	vid_blk_read_word(dev, AFE_CTRL_C2HH_SRC_CTRL, &value);
 	cx231xx_info("AFE_CTRL_C2HH_SRC_CTRL=0x%x\n", value);
 	vid_blk_write_word(dev, AFE_CTRL_C2HH_SRC_CTRL, 0x4485D390);
-	status = vid_blk_read_word(dev, AFE_CTRL_C2HH_SRC_CTRL, &value);
+	vid_blk_read_word(dev, AFE_CTRL_C2HH_SRC_CTRL, &value);
 	cx231xx_info("AFE_CTRL_C2HH_SRC_CTRL=0x%x\n", value);
 }
 
 void cx231xx_dump_SC_reg(struct cx231xx *dev)
 {
 	u8 value[4] = { 0, 0, 0, 0 };
-	int status = 0;
 	cx231xx_info("cx231xx_dump_SC_reg!\n");
 
-	status = cx231xx_read_ctrl_reg(dev, VRT_GET_REGISTER, BOARD_CFG_STAT,
+	cx231xx_read_ctrl_reg(dev, VRT_GET_REGISTER, BOARD_CFG_STAT,
 				 value, 4);
 	cx231xx_info("reg0x%x=0x%x 0x%x 0x%x 0x%x\n", BOARD_CFG_STAT, value[0],
 				 value[1], value[2], value[3]);
-	status = cx231xx_read_ctrl_reg(dev, VRT_GET_REGISTER, TS_MODE_REG,
+	cx231xx_read_ctrl_reg(dev, VRT_GET_REGISTER, TS_MODE_REG,
 				 value, 4);
 	cx231xx_info("reg0x%x=0x%x 0x%x 0x%x 0x%x\n", TS_MODE_REG, value[0],
 				 value[1], value[2], value[3]);
-	status = cx231xx_read_ctrl_reg(dev, VRT_GET_REGISTER, TS1_CFG_REG,
+	cx231xx_read_ctrl_reg(dev, VRT_GET_REGISTER, TS1_CFG_REG,
 				 value, 4);
 	cx231xx_info("reg0x%x=0x%x 0x%x 0x%x 0x%x\n", TS1_CFG_REG, value[0],
 				 value[1], value[2], value[3]);
-	status = cx231xx_read_ctrl_reg(dev, VRT_GET_REGISTER, TS1_LENGTH_REG,
+	cx231xx_read_ctrl_reg(dev, VRT_GET_REGISTER, TS1_LENGTH_REG,
 				 value, 4);
 	cx231xx_info("reg0x%x=0x%x 0x%x 0x%x 0x%x\n", TS1_LENGTH_REG, value[0],
 				 value[1], value[2], value[3]);
 
-	status = cx231xx_read_ctrl_reg(dev, VRT_GET_REGISTER, TS2_CFG_REG,
+	cx231xx_read_ctrl_reg(dev, VRT_GET_REGISTER, TS2_CFG_REG,
 				 value, 4);
 	cx231xx_info("reg0x%x=0x%x 0x%x 0x%x 0x%x\n", TS2_CFG_REG, value[0],
 				 value[1], value[2], value[3]);
-	status = cx231xx_read_ctrl_reg(dev, VRT_GET_REGISTER, TS2_LENGTH_REG,
+	cx231xx_read_ctrl_reg(dev, VRT_GET_REGISTER, TS2_LENGTH_REG,
 				 value, 4);
 	cx231xx_info("reg0x%x=0x%x 0x%x 0x%x 0x%x\n", TS2_LENGTH_REG, value[0],
 				 value[1], value[2], value[3]);
-	status = cx231xx_read_ctrl_reg(dev, VRT_GET_REGISTER, EP_MODE_SET,
+	cx231xx_read_ctrl_reg(dev, VRT_GET_REGISTER, EP_MODE_SET,
 				 value, 4);
 	cx231xx_info("reg0x%x=0x%x 0x%x 0x%x 0x%x\n", EP_MODE_SET, value[0],
 				 value[1], value[2], value[3]);
-	status = cx231xx_read_ctrl_reg(dev, VRT_GET_REGISTER, CIR_PWR_PTN1,
+	cx231xx_read_ctrl_reg(dev, VRT_GET_REGISTER, CIR_PWR_PTN1,
 				 value, 4);
 	cx231xx_info("reg0x%x=0x%x 0x%x 0x%x 0x%x\n", CIR_PWR_PTN1, value[0],
 				 value[1], value[2], value[3]);
 
-	status = cx231xx_read_ctrl_reg(dev, VRT_GET_REGISTER, CIR_PWR_PTN2,
+	cx231xx_read_ctrl_reg(dev, VRT_GET_REGISTER, CIR_PWR_PTN2,
 				 value, 4);
 	cx231xx_info("reg0x%x=0x%x 0x%x 0x%x 0x%x\n", CIR_PWR_PTN2, value[0],
 				 value[1], value[2], value[3]);
-	status = cx231xx_read_ctrl_reg(dev, VRT_GET_REGISTER, CIR_PWR_PTN3,
+	cx231xx_read_ctrl_reg(dev, VRT_GET_REGISTER, CIR_PWR_PTN3,
 				 value, 4);
 	cx231xx_info("reg0x%x=0x%x 0x%x 0x%x 0x%x\n", CIR_PWR_PTN3, value[0],
 				 value[1], value[2], value[3]);
-	status = cx231xx_read_ctrl_reg(dev, VRT_GET_REGISTER, CIR_PWR_MASK0,
+	cx231xx_read_ctrl_reg(dev, VRT_GET_REGISTER, CIR_PWR_MASK0,
 				 value, 4);
 	cx231xx_info("reg0x%x=0x%x 0x%x 0x%x 0x%x\n", CIR_PWR_MASK0, value[0],
 				 value[1], value[2], value[3]);
-	status = cx231xx_read_ctrl_reg(dev, VRT_GET_REGISTER, CIR_PWR_MASK1,
+	cx231xx_read_ctrl_reg(dev, VRT_GET_REGISTER, CIR_PWR_MASK1,
 				 value, 4);
 	cx231xx_info("reg0x%x=0x%x 0x%x 0x%x 0x%x\n", CIR_PWR_MASK1, value[0],
 				 value[1], value[2], value[3]);
 
-	status = cx231xx_read_ctrl_reg(dev, VRT_GET_REGISTER, CIR_PWR_MASK2,
+	cx231xx_read_ctrl_reg(dev, VRT_GET_REGISTER, CIR_PWR_MASK2,
 				 value, 4);
 	cx231xx_info("reg0x%x=0x%x 0x%x 0x%x 0x%x\n", CIR_PWR_MASK2, value[0],
 				 value[1], value[2], value[3]);
-	status = cx231xx_read_ctrl_reg(dev, VRT_GET_REGISTER, CIR_GAIN,
+	cx231xx_read_ctrl_reg(dev, VRT_GET_REGISTER, CIR_GAIN,
 				 value, 4);
 	cx231xx_info("reg0x%x=0x%x 0x%x 0x%x 0x%x\n", CIR_GAIN, value[0],
 				 value[1], value[2], value[3]);
-	status = cx231xx_read_ctrl_reg(dev, VRT_GET_REGISTER, CIR_CAR_REG,
+	cx231xx_read_ctrl_reg(dev, VRT_GET_REGISTER, CIR_CAR_REG,
 				 value, 4);
 	cx231xx_info("reg0x%x=0x%x 0x%x 0x%x 0x%x\n", CIR_CAR_REG, value[0],
 				 value[1], value[2], value[3]);
-	status = cx231xx_read_ctrl_reg(dev, VRT_GET_REGISTER, CIR_OT_CFG1,
+	cx231xx_read_ctrl_reg(dev, VRT_GET_REGISTER, CIR_OT_CFG1,
 				 value, 4);
 	cx231xx_info("reg0x%x=0x%x 0x%x 0x%x 0x%x\n", CIR_OT_CFG1, value[0],
 				 value[1], value[2], value[3]);
 
-	status = cx231xx_read_ctrl_reg(dev, VRT_GET_REGISTER, CIR_OT_CFG2,
+	cx231xx_read_ctrl_reg(dev, VRT_GET_REGISTER, CIR_OT_CFG2,
 				 value, 4);
 	cx231xx_info("reg0x%x=0x%x 0x%x 0x%x 0x%x\n", CIR_OT_CFG2, value[0],
 				 value[1], value[2], value[3]);
-	status = cx231xx_read_ctrl_reg(dev, VRT_GET_REGISTER, PWR_CTL_EN,
+	cx231xx_read_ctrl_reg(dev, VRT_GET_REGISTER, PWR_CTL_EN,
 				 value, 4);
 	cx231xx_info("reg0x%x=0x%x 0x%x 0x%x 0x%x\n", PWR_CTL_EN, value[0],
 				 value[1], value[2], value[3]);
@@ -1441,18 +1435,15 @@ void cx231xx_dump_SC_reg(struct cx231xx *dev)
 void cx231xx_Setup_AFE_for_LowIF(struct cx231xx *dev)
 
 {
-	u8 status = 0;
 	u8 value = 0;
 
-
-
-	status = afe_read_byte(dev, ADC_STATUS2_CH3, &value);
+	afe_read_byte(dev, ADC_STATUS2_CH3, &value);
 	value = (value & 0xFE)|0x01;
-	status = afe_write_byte(dev, ADC_STATUS2_CH3, value);
+	afe_write_byte(dev, ADC_STATUS2_CH3, value);
 
-	status = afe_read_byte(dev, ADC_STATUS2_CH3, &value);
+	afe_read_byte(dev, ADC_STATUS2_CH3, &value);
 	value = (value & 0xFE)|0x00;
-	status = afe_write_byte(dev, ADC_STATUS2_CH3, value);
+	afe_write_byte(dev, ADC_STATUS2_CH3, value);
 
 
 /*
@@ -1464,44 +1455,43 @@ void cx231xx_Setup_AFE_for_LowIF(struct cx231xx *dev)
 		for low-if agc defect
 */
 
-	status = afe_read_byte(dev, ADC_NTF_PRECLMP_EN_CH3, &value);
+	afe_read_byte(dev, ADC_NTF_PRECLMP_EN_CH3, &value);
 	value = (value & 0xFC)|0x00;
-	status = afe_write_byte(dev, ADC_NTF_PRECLMP_EN_CH3, value);
+	afe_write_byte(dev, ADC_NTF_PRECLMP_EN_CH3, value);
 
-	status = afe_read_byte(dev, ADC_INPUT_CH3, &value);
+	afe_read_byte(dev, ADC_INPUT_CH3, &value);
 	value = (value & 0xF9)|0x02;
-	status = afe_write_byte(dev, ADC_INPUT_CH3, value);
+	afe_write_byte(dev, ADC_INPUT_CH3, value);
 
-	status = afe_read_byte(dev, ADC_FB_FRCRST_CH3, &value);
+	afe_read_byte(dev, ADC_FB_FRCRST_CH3, &value);
 	value = (value & 0xFB)|0x04;
-	status = afe_write_byte(dev, ADC_FB_FRCRST_CH3, value);
+	afe_write_byte(dev, ADC_FB_FRCRST_CH3, value);
 
-	status = afe_read_byte(dev, ADC_DCSERVO_DEM_CH3, &value);
+	afe_read_byte(dev, ADC_DCSERVO_DEM_CH3, &value);
 	value = (value & 0xFC)|0x03;
-	status = afe_write_byte(dev, ADC_DCSERVO_DEM_CH3, value);
+	afe_write_byte(dev, ADC_DCSERVO_DEM_CH3, value);
 
-	status = afe_read_byte(dev, ADC_CTRL_DAC1_CH3, &value);
+	afe_read_byte(dev, ADC_CTRL_DAC1_CH3, &value);
 	value = (value & 0xFB)|0x04;
-	status = afe_write_byte(dev, ADC_CTRL_DAC1_CH3, value);
+	afe_write_byte(dev, ADC_CTRL_DAC1_CH3, value);
 
-	status = afe_read_byte(dev, ADC_CTRL_DAC23_CH3, &value);
+	afe_read_byte(dev, ADC_CTRL_DAC23_CH3, &value);
 	value = (value & 0xF8)|0x06;
-	status = afe_write_byte(dev, ADC_CTRL_DAC23_CH3, value);
+	afe_write_byte(dev, ADC_CTRL_DAC23_CH3, value);
 
-	status = afe_read_byte(dev, ADC_CTRL_DAC23_CH3, &value);
+	afe_read_byte(dev, ADC_CTRL_DAC23_CH3, &value);
 	value = (value & 0x8F)|0x40;
-	status = afe_write_byte(dev, ADC_CTRL_DAC23_CH3, value);
+	afe_write_byte(dev, ADC_CTRL_DAC23_CH3, value);
 
-	status = afe_read_byte(dev, ADC_PWRDN_CLAMP_CH3, &value);
+	afe_read_byte(dev, ADC_PWRDN_CLAMP_CH3, &value);
 	value = (value & 0xDF)|0x20;
-	status = afe_write_byte(dev, ADC_PWRDN_CLAMP_CH3, value);
+	afe_write_byte(dev, ADC_PWRDN_CLAMP_CH3, value);
 }
 
 void cx231xx_set_Colibri_For_LowIF(struct cx231xx *dev, u32 if_freq,
 		 u8 spectral_invert, u32 mode)
 {
 	u32 colibri_carrier_offset = 0;
-	u8 status = 0;
 	u32 func_mode = 0x01; /* Device has a DIF if this function is called */
 	u32 standard = 0;
 	u8 value[4] = { 0, 0, 0, 0 };
@@ -1511,15 +1501,15 @@ void cx231xx_set_Colibri_For_LowIF(struct cx231xx *dev, u32 if_freq,
 	value[1] = (u8) 0x6F;
 	value[2] = (u8) 0x6F;
 	value[3] = (u8) 0x6F;
-	status = cx231xx_write_ctrl_reg(dev, VRT_SET_REGISTER,
+	cx231xx_write_ctrl_reg(dev, VRT_SET_REGISTER,
 					PWR_CTL_EN, value, 4);
 
 	/*Set colibri for low IF*/
-	status = cx231xx_afe_set_mode(dev, AFE_MODE_LOW_IF);
+	cx231xx_afe_set_mode(dev, AFE_MODE_LOW_IF);
 
 	/* Set C2HH for low IF operation.*/
 	standard = dev->norm;
-	status = cx231xx_dif_configure_C2HH_for_low_IF(dev, dev->active_mode,
+	cx231xx_dif_configure_C2HH_for_low_IF(dev, dev->active_mode,
 						       func_mode, standard);
 
 	/* Get colibri offsets.*/
@@ -1556,7 +1546,6 @@ void cx231xx_set_DIF_bandpass(struct cx231xx *dev, u32 if_freq,
 		 u8 spectral_invert, u32 mode)
 {
 	unsigned long pll_freq_word;
-	int status = 0;
 	u32 dif_misc_ctrl_value = 0;
 	u64 pll_freq_u64 = 0;
 	u32 i = 0;
@@ -1567,7 +1556,7 @@ void cx231xx_set_DIF_bandpass(struct cx231xx *dev, u32 if_freq,
 
 	if (mode == TUNER_MODE_FM_RADIO) {
 		pll_freq_word = 0x905A1CAC;
-		status = vid_blk_write_word(dev, DIF_PLL_FREQ_WORD,  pll_freq_word);
+		vid_blk_write_word(dev, DIF_PLL_FREQ_WORD,  pll_freq_word);
 
 	} else /*KSPROPERTY_TUNER_MODE_TV*/{
 		/* Calculate the PLL frequency word based on the adjusted if_freq*/
@@ -1576,23 +1565,23 @@ void cx231xx_set_DIF_bandpass(struct cx231xx *dev, u32 if_freq,
 		do_div(pll_freq_u64, 50000000);
 		pll_freq_word = (u32)pll_freq_u64;
 		/*pll_freq_word = 0x3463497;*/
-		status = vid_blk_write_word(dev, DIF_PLL_FREQ_WORD,  pll_freq_word);
+		vid_blk_write_word(dev, DIF_PLL_FREQ_WORD,  pll_freq_word);
 
 	if (spectral_invert) {
 		if_freq -= 400000;
 		/* Enable Spectral Invert*/
-		status = vid_blk_read_word(dev, DIF_MISC_CTRL,
+		vid_blk_read_word(dev, DIF_MISC_CTRL,
 					&dif_misc_ctrl_value);
 		dif_misc_ctrl_value = dif_misc_ctrl_value | 0x00200000;
-		status = vid_blk_write_word(dev, DIF_MISC_CTRL,
+		vid_blk_write_word(dev, DIF_MISC_CTRL,
 					dif_misc_ctrl_value);
 	} else {
 		if_freq += 400000;
 		/* Disable Spectral Invert*/
-		status = vid_blk_read_word(dev, DIF_MISC_CTRL,
+		vid_blk_read_word(dev, DIF_MISC_CTRL,
 					&dif_misc_ctrl_value);
 		dif_misc_ctrl_value = dif_misc_ctrl_value & 0xFFDFFFFF;
-		status = vid_blk_write_word(dev, DIF_MISC_CTRL,
+		vid_blk_write_word(dev, DIF_MISC_CTRL,
 					dif_misc_ctrl_value);
 	}
 
@@ -1606,10 +1595,10 @@ void cx231xx_set_DIF_bandpass(struct cx231xx *dev, u32 if_freq,
 	}
 
 	cx231xx_info("Enter IF=%zd\n",
-			sizeof(Dif_set_array)/sizeof(struct dif_settings));
-	for (i = 0; i < sizeof(Dif_set_array)/sizeof(struct dif_settings); i++) {
+			ARRAY_SIZE(Dif_set_array));
+	for (i = 0; i < ARRAY_SIZE(Dif_set_array); i++) {
 		if (Dif_set_array[i].if_freq == if_freq) {
-			status = vid_blk_write_word(dev,
+			vid_blk_write_word(dev,
 			Dif_set_array[i].register_address, Dif_set_array[i].value);
 		}
 	}
@@ -3090,31 +3079,30 @@ int cx231xx_gpio_i2c_read(struct cx231xx *dev, u8 dev_addr, u8 *buf, u8 len)
  */
 int cx231xx_gpio_i2c_write(struct cx231xx *dev, u8 dev_addr, u8 *buf, u8 len)
 {
-	int status = 0;
 	int i = 0;
 
 	/* get the lock */
 	mutex_lock(&dev->gpio_i2c_lock);
 
 	/* start */
-	status = cx231xx_gpio_i2c_start(dev);
+	cx231xx_gpio_i2c_start(dev);
 
 	/* write dev_addr */
-	status = cx231xx_gpio_i2c_write_byte(dev, dev_addr << 1);
+	cx231xx_gpio_i2c_write_byte(dev, dev_addr << 1);
 
 	/* read Ack */
-	status = cx231xx_gpio_i2c_read_ack(dev);
+	cx231xx_gpio_i2c_read_ack(dev);
 
 	for (i = 0; i < len; i++) {
 		/* Write data */
-		status = cx231xx_gpio_i2c_write_byte(dev, buf[i]);
+		cx231xx_gpio_i2c_write_byte(dev, buf[i]);
 
 		/* read Ack */
-		status = cx231xx_gpio_i2c_read_ack(dev);
+		cx231xx_gpio_i2c_read_ack(dev);
 	}
 
 	/* write End */
-	status = cx231xx_gpio_i2c_end(dev);
+	cx231xx_gpio_i2c_end(dev);
 
 	/* release the lock */
 	mutex_unlock(&dev->gpio_i2c_lock);
diff --git a/drivers/media/video/cx231xx/cx231xx-core.c b/drivers/media/video/cx231xx/cx231xx-core.c
index 08dd930f882..05358d48613 100644
--- a/drivers/media/video/cx231xx/cx231xx-core.c
+++ b/drivers/media/video/cx231xx/cx231xx-core.c
@@ -754,7 +754,7 @@ int cx231xx_set_mode(struct cx231xx *dev, enum cx231xx_mode set_mode)
 		}
 	}
 
-	return 0;
+	return errCode ? -EINVAL : 0;
 }
 EXPORT_SYMBOL_GPL(cx231xx_set_mode);
 
@@ -764,7 +764,7 @@ int cx231xx_ep5_bulkout(struct cx231xx *dev, u8 *firmware, u16 size)
 	int actlen, ret = -ENOMEM;
 	u32 *buffer;
 
-buffer = kzalloc(4096, GFP_KERNEL);
+	buffer = kzalloc(4096, GFP_KERNEL);
 	if (buffer == NULL) {
 		cx231xx_info("out of mem\n");
 		return -ENOMEM;
@@ -772,16 +772,16 @@ buffer = kzalloc(4096, GFP_KERNEL);
 	memcpy(&buffer[0], firmware, 4096);
 
 	ret = usb_bulk_msg(dev->udev, usb_sndbulkpipe(dev->udev, 5),
-				 buffer, 4096, &actlen, 2000);
+			buffer, 4096, &actlen, 2000);
 
 	if (ret)
 		cx231xx_info("bulk message failed: %d (%d/%d)", ret,
-				 size, actlen);
+				size, actlen);
 	else {
 		errCode = actlen != size ? -1 : 0;
 	}
-kfree(buffer);
-	return 0;
+	kfree(buffer);
+	return errCode;
 }
 
 /*****************************************************************
@@ -797,7 +797,7 @@ static void cx231xx_isoc_irq_callback(struct urb *urb)
 	struct cx231xx_video_mode *vmode =
 	    container_of(dma_q, struct cx231xx_video_mode, vidq);
 	struct cx231xx *dev = container_of(vmode, struct cx231xx, video_mode);
-	int rc, i;
+	int i;
 
 	switch (urb->status) {
 	case 0:		/* success */
@@ -814,7 +814,7 @@ static void cx231xx_isoc_irq_callback(struct urb *urb)
 
 	/* Copy data from URB */
 	spin_lock(&dev->video_mode.slock);
-	rc = dev->video_mode.isoc_ctl.isoc_copy(dev, urb);
+	dev->video_mode.isoc_ctl.isoc_copy(dev, urb);
 	spin_unlock(&dev->video_mode.slock);
 
 	/* Reset urb buffers */
@@ -822,7 +822,6 @@ static void cx231xx_isoc_irq_callback(struct urb *urb)
 		urb->iso_frame_desc[i].status = 0;
 		urb->iso_frame_desc[i].actual_length = 0;
 	}
-	urb->status = 0;
 
 	urb->status = usb_submit_urb(urb, GFP_ATOMIC);
 	if (urb->status) {
@@ -843,7 +842,6 @@ static void cx231xx_bulk_irq_callback(struct urb *urb)
 	struct cx231xx_video_mode *vmode =
 	    container_of(dma_q, struct cx231xx_video_mode, vidq);
 	struct cx231xx *dev = container_of(vmode, struct cx231xx, video_mode);
-	int rc;
 
 	switch (urb->status) {
 	case 0:		/* success */
@@ -860,12 +858,10 @@ static void cx231xx_bulk_irq_callback(struct urb *urb)
 
 	/* Copy data from URB */
 	spin_lock(&dev->video_mode.slock);
-	rc = dev->video_mode.bulk_ctl.bulk_copy(dev, urb);
+	dev->video_mode.bulk_ctl.bulk_copy(dev, urb);
 	spin_unlock(&dev->video_mode.slock);
 
 	/* Reset urb buffers */
-	urb->status = 0;
-
 	urb->status = usb_submit_urb(urb, GFP_ATOMIC);
 	if (urb->status) {
 		cx231xx_isocdbg("urb resubmit failed (error=%i)\n",
@@ -1231,42 +1227,40 @@ int cx231xx_init_bulk(struct cx231xx *dev, int max_packets,
 EXPORT_SYMBOL_GPL(cx231xx_init_bulk);
 void cx231xx_stop_TS1(struct cx231xx *dev)
 {
-	int status = 0;
 	u8 val[4] = { 0, 0, 0, 0 };
 
-			val[0] = 0x00;
-			val[1] = 0x03;
-			val[2] = 0x00;
-			val[3] = 0x00;
-			status = cx231xx_write_ctrl_reg(dev, VRT_SET_REGISTER,
-				 TS_MODE_REG, val, 4);
-
-			val[0] = 0x00;
-			val[1] = 0x70;
-			val[2] = 0x04;
-			val[3] = 0x00;
-			status = cx231xx_write_ctrl_reg(dev, VRT_SET_REGISTER,
-				 TS1_CFG_REG, val, 4);
+	val[0] = 0x00;
+	val[1] = 0x03;
+	val[2] = 0x00;
+	val[3] = 0x00;
+	cx231xx_write_ctrl_reg(dev, VRT_SET_REGISTER,
+			TS_MODE_REG, val, 4);
+
+	val[0] = 0x00;
+	val[1] = 0x70;
+	val[2] = 0x04;
+	val[3] = 0x00;
+	cx231xx_write_ctrl_reg(dev, VRT_SET_REGISTER,
+			TS1_CFG_REG, val, 4);
 }
 /* EXPORT_SYMBOL_GPL(cx231xx_stop_TS1); */
 void cx231xx_start_TS1(struct cx231xx *dev)
 {
-	int status = 0;
 	u8 val[4] = { 0, 0, 0, 0 };
 
-			val[0] = 0x03;
-			val[1] = 0x03;
-			val[2] = 0x00;
-			val[3] = 0x00;
-			status = cx231xx_write_ctrl_reg(dev, VRT_SET_REGISTER,
-				 TS_MODE_REG, val, 4);
-
-			val[0] = 0x04;
-			val[1] = 0xA3;
-			val[2] = 0x3B;
-			val[3] = 0x00;
-			status = cx231xx_write_ctrl_reg(dev, VRT_SET_REGISTER,
-				 TS1_CFG_REG, val, 4);
+	val[0] = 0x03;
+	val[1] = 0x03;
+	val[2] = 0x00;
+	val[3] = 0x00;
+	cx231xx_write_ctrl_reg(dev, VRT_SET_REGISTER,
+			TS_MODE_REG, val, 4);
+
+	val[0] = 0x04;
+	val[1] = 0xA3;
+	val[2] = 0x3B;
+	val[3] = 0x00;
+	cx231xx_write_ctrl_reg(dev, VRT_SET_REGISTER,
+			TS1_CFG_REG, val, 4);
 }
 /* EXPORT_SYMBOL_GPL(cx231xx_start_TS1); */
 /*****************************************************************
diff --git a/drivers/media/video/cx231xx/cx231xx-vbi.c b/drivers/media/video/cx231xx/cx231xx-vbi.c
index 8cdee5f78f1..3d15314e1f8 100644
--- a/drivers/media/video/cx231xx/cx231xx-vbi.c
+++ b/drivers/media/video/cx231xx/cx231xx-vbi.c
@@ -83,7 +83,6 @@ static inline void print_err_status(struct cx231xx *dev, int packet, int status)
  */
 static inline int cx231xx_isoc_vbi_copy(struct cx231xx *dev, struct urb *urb)
 {
-	struct cx231xx_buffer *buf;
 	struct cx231xx_dmaqueue *dma_q = urb->context;
 	int rc = 1;
 	unsigned char *p_buffer;
@@ -102,8 +101,6 @@ static inline int cx231xx_isoc_vbi_copy(struct cx231xx *dev, struct urb *urb)
 			return 0;
 	}
 
-	buf = dev->vbi_mode.bulk_ctl.buf;
-
 	/* get buffer pointer and length */
 	p_buffer = urb->transfer_buffer;
 	buffer_size = urb->actual_length;
@@ -310,7 +307,6 @@ static void cx231xx_irq_vbi_callback(struct urb *urb)
 	struct cx231xx_video_mode *vmode =
 	    container_of(dma_q, struct cx231xx_video_mode, vidq);
 	struct cx231xx *dev = container_of(vmode, struct cx231xx, vbi_mode);
-	int rc;
 
 	switch (urb->status) {
 	case 0:		/* success */
@@ -328,7 +324,7 @@ static void cx231xx_irq_vbi_callback(struct urb *urb)
 
 	/* Copy data from URB */
 	spin_lock(&dev->vbi_mode.slock);
-	rc = dev->vbi_mode.bulk_ctl.bulk_copy(dev, urb);
+	dev->vbi_mode.bulk_ctl.bulk_copy(dev, urb);
 	spin_unlock(&dev->vbi_mode.slock);
 
 	/* Reset status */
diff --git a/drivers/media/video/cx231xx/cx231xx-video.c b/drivers/media/video/cx231xx/cx231xx-video.c
index 7f916f0685e..523aa49d6b8 100644
--- a/drivers/media/video/cx231xx/cx231xx-video.c
+++ b/drivers/media/video/cx231xx/cx231xx-video.c
@@ -326,9 +326,7 @@ static inline void get_next_buf(struct cx231xx_dmaqueue *dma_q,
  */
 static inline int cx231xx_isoc_copy(struct cx231xx *dev, struct urb *urb)
 {
-	struct cx231xx_buffer *buf;
 	struct cx231xx_dmaqueue *dma_q = urb->context;
-	unsigned char *outp = NULL;
 	int i, rc = 1;
 	unsigned char *p_buffer;
 	u32 bytes_parsed = 0, buffer_size = 0;
@@ -346,10 +344,6 @@ static inline int cx231xx_isoc_copy(struct cx231xx *dev, struct urb *urb)
 			return 0;
 	}
 
-	buf = dev->video_mode.isoc_ctl.buf;
-	if (buf != NULL)
-		outp = videobuf_to_vmalloc(&buf->vb);
-
 	for (i = 0; i < urb->number_of_packets; i++) {
 		int status = urb->iso_frame_desc[i].status;
 
@@ -429,9 +423,7 @@ static inline int cx231xx_isoc_copy(struct cx231xx *dev, struct urb *urb)
 
 static inline int cx231xx_bulk_copy(struct cx231xx *dev, struct urb *urb)
 {
-	struct cx231xx_buffer *buf;
 	struct cx231xx_dmaqueue *dma_q = urb->context;
-	unsigned char *outp = NULL;
 	int rc = 1;
 	unsigned char *p_buffer;
 	u32 bytes_parsed = 0, buffer_size = 0;
@@ -449,10 +441,6 @@ static inline int cx231xx_bulk_copy(struct cx231xx *dev, struct urb *urb)
 			return 0;
 	}
 
-	buf = dev->video_mode.bulk_ctl.buf;
-	if (buf != NULL)
-		outp = videobuf_to_vmalloc(&buf->vb);
-
 	if (1) {
 
 		/*  get buffer pointer and length */
@@ -701,13 +689,9 @@ void cx231xx_reset_video_buffer(struct cx231xx *dev,
 		buf = dev->video_mode.bulk_ctl.buf;
 
 	if (buf == NULL) {
-		u8 *outp = NULL;
 		/* first try to get the buffer */
 		get_next_buf(dma_q, &buf);
 
-		if (buf)
-			outp = videobuf_to_vmalloc(&buf->vb);
-
 		dma_q->pos = 0;
 		dma_q->field1_done = 0;
 		dma_q->current_field = -1;
@@ -2561,6 +2545,10 @@ static struct video_device *cx231xx_vdev_init(struct cx231xx *dev,
 	vfd->release = video_device_release;
 	vfd->debug = video_debug;
 	vfd->lock = &dev->lock;
+	/* Locking in file operations other than ioctl should be done
+	   by the driver, not the V4L2 core.
+	   This driver needs auditing so that this flag can be removed. */
+	set_bit(V4L2_FL_LOCK_ALL_FOPS, &vfd->flags);
 
 	snprintf(vfd->name, sizeof(vfd->name), "%s %s", dev->name, type_name);
 
diff --git a/drivers/media/video/cx23885/cx23885-cards.c b/drivers/media/video/cx23885/cx23885-cards.c
index 19b5499d262..13739e002a6 100644
--- a/drivers/media/video/cx23885/cx23885-cards.c
+++ b/drivers/media/video/cx23885/cx23885-cards.c
@@ -497,6 +497,10 @@ struct cx23885_board cx23885_boards[] = {
 		.name		= "TerraTec Cinergy T PCIe Dual",
 		.portb		= CX23885_MPEG_DVB,
 		.portc		= CX23885_MPEG_DVB,
+	},
+	[CX23885_BOARD_TEVII_S471] = {
+		.name		= "TeVii S471",
+		.portb		= CX23885_MPEG_DVB,
 	}
 };
 const unsigned int cx23885_bcount = ARRAY_SIZE(cx23885_boards);
@@ -705,6 +709,10 @@ struct cx23885_subid cx23885_subids[] = {
 		.subvendor = 0x153b,
 		.subdevice = 0x117e,
 		.card      = CX23885_BOARD_TERRATEC_CINERGY_T_PCIE_DUAL,
+	}, {
+		.subvendor = 0xd471,
+		.subdevice = 0x9022,
+		.card      = CX23885_BOARD_TEVII_S471,
 	},
 };
 const unsigned int cx23885_idcount = ARRAY_SIZE(cx23885_subids);
@@ -1460,6 +1468,7 @@ void cx23885_card_setup(struct cx23885_dev *dev)
 		ts1->src_sel_val   = CX23885_SRC_SEL_PARALLEL_MPEG_VIDEO;
 		break;
 	case CX23885_BOARD_TEVII_S470:
+	case CX23885_BOARD_TEVII_S471:
 	case CX23885_BOARD_DVBWORLD_2005:
 		ts1->gen_ctrl_val  = 0x5; /* Parallel */
 		ts1->ts_clk_en_val = 0x1; /* Enable TS_CLK */
diff --git a/drivers/media/video/cx23885/cx23885-core.c b/drivers/media/video/cx23885/cx23885-core.c
index 6ad227029a0..697728f0943 100644
--- a/drivers/media/video/cx23885/cx23885-core.c
+++ b/drivers/media/video/cx23885/cx23885-core.c
@@ -1046,6 +1046,13 @@ static int cx23885_dev_setup(struct cx23885_dev *dev)
 	if (cx23885_boards[dev->board].ci_type > 0)
 		cx_clear(RDR_RDRCTL1, 1 << 8);
 
+	switch (dev->board) {
+	case CX23885_BOARD_TEVII_S470:
+	case CX23885_BOARD_TEVII_S471:
+		cx_clear(RDR_RDRCTL1, 1 << 8);
+		break;
+	}
+
 	return 0;
 }
 
diff --git a/drivers/media/video/cx23885/cx23885-dvb.c b/drivers/media/video/cx23885/cx23885-dvb.c
index 6835eb1fc09..a80a92c4745 100644
--- a/drivers/media/video/cx23885/cx23885-dvb.c
+++ b/drivers/media/video/cx23885/cx23885-dvb.c
@@ -1173,6 +1173,13 @@ static int dvb_register(struct cx23885_tsport *port)
 			break;
 		}
 		break;
+	case CX23885_BOARD_TEVII_S471:
+		i2c_bus = &dev->i2c_bus[1];
+
+		fe0->dvb.frontend = dvb_attach(ds3000_attach,
+					&tevii_ds3000_config,
+					&i2c_bus->i2c_adap);
+		break;
 	default:
 		printk(KERN_INFO "%s: The frontend of your DVB/ATSC card "
 			" isn't supported yet\n",
diff --git a/drivers/media/video/cx23885/cx23885.h b/drivers/media/video/cx23885/cx23885.h
index f020f0568df..d884784a1c8 100644
--- a/drivers/media/video/cx23885/cx23885.h
+++ b/drivers/media/video/cx23885/cx23885.h
@@ -89,6 +89,7 @@
 #define CX23885_BOARD_MPX885                   32
 #define CX23885_BOARD_MYGICA_X8507             33
 #define CX23885_BOARD_TERRATEC_CINERGY_T_PCIE_DUAL 34
+#define CX23885_BOARD_TEVII_S471               35
 
 #define GPIO_0 0x00000001
 #define GPIO_1 0x00000002
diff --git a/drivers/media/video/cx23885/cx23888-ir.c b/drivers/media/video/cx23885/cx23888-ir.c
index bb1ce346425..c2bc39c58f8 100644
--- a/drivers/media/video/cx23885/cx23888-ir.c
+++ b/drivers/media/video/cx23885/cx23888-ir.c
@@ -331,9 +331,7 @@ static u64 ns_to_pulse_clocks(u32 ns)
 
 static u16 pulse_clocks_to_clock_divider(u64 count)
 {
-	u32 rem;
-
-	rem = do_div(count, (FIFO_RXTX << 2) | 0x3);
+	do_div(count, (FIFO_RXTX << 2) | 0x3);
 
 	/* net result needs to be rounded down and decremented by 1 */
 	if (count > RXCLK_RCD + 1)
diff --git a/drivers/media/video/cx25821/cx25821-alsa.c b/drivers/media/video/cx25821/cx25821-alsa.c
index 03cfac476b0..1858a45dd08 100644
--- a/drivers/media/video/cx25821/cx25821-alsa.c
+++ b/drivers/media/video/cx25821/cx25821-alsa.c
@@ -290,11 +290,9 @@ static irqreturn_t cx25821_irq(int irq, void *dev_id)
 	u32 status, pci_status;
 	u32 audint_status, audint_mask;
 	int loop, handled = 0;
-	int audint_count = 0;
 
 	audint_status = cx_read(AUD_A_INT_STAT);
 	audint_mask = cx_read(AUD_A_INT_MSK);
-	audint_count = cx_read(AUD_A_GPCNT);
 	status = cx_read(PCI_INT_STAT);
 
 	for (loop = 0; loop < 1; loop++) {
diff --git a/drivers/media/video/cx25821/cx25821-audio-upstream.c b/drivers/media/video/cx25821/cx25821-audio-upstream.c
index 20c7ca3351a..8b2a99975c2 100644
--- a/drivers/media/video/cx25821/cx25821-audio-upstream.c
+++ b/drivers/media/video/cx25821/cx25821-audio-upstream.c
@@ -585,7 +585,7 @@ int cx25821_audio_upstream_irq(struct cx25821_dev *dev, int chan_num,
 static irqreturn_t cx25821_upstream_irq_audio(int irq, void *dev_id)
 {
 	struct cx25821_dev *dev = dev_id;
-	u32 msk_stat, audio_status;
+	u32 audio_status;
 	int handled = 0;
 	struct sram_channel *sram_ch;
 
@@ -594,7 +594,6 @@ static irqreturn_t cx25821_upstream_irq_audio(int irq, void *dev_id)
 
 	sram_ch = dev->channels[dev->_audio_upstream_channel].sram_channels;
 
-	msk_stat = cx_read(sram_ch->int_mstat);
 	audio_status = cx_read(sram_ch->int_stat);
 
 	/* Only deal with our interrupt */
diff --git a/drivers/media/video/cx25821/cx25821-core.c b/drivers/media/video/cx25821/cx25821-core.c
index 7930ca58349..83c1aa6b2e6 100644
--- a/drivers/media/video/cx25821/cx25821-core.c
+++ b/drivers/media/video/cx25821/cx25821-core.c
@@ -379,14 +379,6 @@ static inline int i2c_slave_did_ack(struct i2c_adapter *i2c_adap)
 	return cx_read(bus->reg_stat) & 0x01;
 }
 
-void cx_i2c_read_print(struct cx25821_dev *dev, u32 reg, const char *reg_string)
-{
-	int tmp = 0;
-	u32 value = 0;
-
-	value = cx25821_i2c_read(&dev->i2c_bus[0], reg, &tmp);
-}
-
 static void cx25821_registers_init(struct cx25821_dev *dev)
 {
 	u32 tmp;
@@ -895,7 +887,7 @@ static void cx25821_iounmap(struct cx25821_dev *dev)
 
 static int cx25821_dev_setup(struct cx25821_dev *dev)
 {
-	int io_size = 0, i;
+	int i;
 
 	pr_info("\n***********************************\n");
 	pr_info("cx25821 set up\n");
@@ -960,7 +952,6 @@ static int cx25821_dev_setup(struct cx25821_dev *dev)
 
 	/* PCIe stuff */
 	dev->base_io_addr = pci_resource_start(dev->pci, 0);
-	io_size = pci_resource_len(dev->pci, 0);
 
 	if (!dev->base_io_addr) {
 		CX25821_ERR("No PCI Memory resources, exiting!\n");
@@ -1317,13 +1308,12 @@ void cx25821_free_buffer(struct videobuf_queue *q, struct cx25821_buffer *buf)
 static irqreturn_t cx25821_irq(int irq, void *dev_id)
 {
 	struct cx25821_dev *dev = dev_id;
-	u32 pci_status, pci_mask;
+	u32 pci_status;
 	u32 vid_status;
 	int i, handled = 0;
 	u32 mask[8] = { 1, 2, 4, 8, 16, 32, 64, 128 };
 
 	pci_status = cx_read(PCI_INT_STAT);
-	pci_mask = cx_read(PCI_INT_MSK);
 
 	if (pci_status == 0)
 		goto out;
diff --git a/drivers/media/video/cx25821/cx25821-i2c.c b/drivers/media/video/cx25821/cx25821-i2c.c
index 12d7300fa1e..6311180f430 100644
--- a/drivers/media/video/cx25821/cx25821-i2c.c
+++ b/drivers/media/video/cx25821/cx25821-i2c.c
@@ -361,7 +361,6 @@ void cx25821_av_clk(struct cx25821_dev *dev, int enable)
 int cx25821_i2c_read(struct cx25821_i2c *bus, u16 reg_addr, int *value)
 {
 	struct i2c_client *client = &bus->i2c_client;
-	int retval = 0;
 	int v = 0;
 	u8 addr[2] = { 0, 0 };
 	u8 buf[4] = { 0, 0, 0, 0 };
@@ -385,7 +384,7 @@ int cx25821_i2c_read(struct cx25821_i2c *bus, u16 reg_addr, int *value)
 	msgs[0].addr = 0x44;
 	msgs[1].addr = 0x44;
 
-	retval = i2c_xfer(client->adapter, msgs, 2);
+	i2c_xfer(client->adapter, msgs, 2);
 
 	v = (buf[3] << 24) | (buf[2] << 16) | (buf[1] << 8) | buf[0];
 	*value = v;
diff --git a/drivers/media/video/cx25821/cx25821-medusa-video.c b/drivers/media/video/cx25821/cx25821-medusa-video.c
index 298a68d98c2..313fb20a0b4 100644
--- a/drivers/media/video/cx25821/cx25821-medusa-video.c
+++ b/drivers/media/video/cx25821/cx25821-medusa-video.c
@@ -35,7 +35,6 @@
 static void medusa_enable_bluefield_output(struct cx25821_dev *dev, int channel,
 					   int enable)
 {
-	int ret_val = 1;
 	u32 value = 0;
 	u32 tmp = 0;
 	int out_ctrl = OUT_CTRL1;
@@ -79,13 +78,13 @@ static void medusa_enable_bluefield_output(struct cx25821_dev *dev, int channel,
 	value &= 0xFFFFFF7F;	/* clear BLUE_FIELD_EN */
 	if (enable)
 		value |= 0x00000080;	/* set BLUE_FIELD_EN */
-	ret_val = cx25821_i2c_write(&dev->i2c_bus[0], out_ctrl, value);
+	cx25821_i2c_write(&dev->i2c_bus[0], out_ctrl, value);
 
 	value = cx25821_i2c_read(&dev->i2c_bus[0], out_ctrl_ns, &tmp);
 	value &= 0xFFFFFF7F;
 	if (enable)
 		value |= 0x00000080;	/* set BLUE_FIELD_EN */
-	ret_val = cx25821_i2c_write(&dev->i2c_bus[0], out_ctrl_ns, value);
+	cx25821_i2c_write(&dev->i2c_bus[0], out_ctrl_ns, value);
 }
 
 static int medusa_initialize_ntsc(struct cx25821_dev *dev)
@@ -431,7 +430,6 @@ void medusa_set_resolution(struct cx25821_dev *dev, int width,
 {
 	int decoder = 0;
 	int decoder_count = 0;
-	int ret_val = 0;
 	u32 hscale = 0x0;
 	u32 vscale = 0x0;
 	const int MAX_WIDTH = 720;
@@ -482,9 +480,9 @@ void medusa_set_resolution(struct cx25821_dev *dev, int width,
 
 	for (; decoder < decoder_count; decoder++) {
 		/* write scaling values for each decoder */
-		ret_val = cx25821_i2c_write(&dev->i2c_bus[0],
+		cx25821_i2c_write(&dev->i2c_bus[0],
 				HSCALE_CTRL + (0x200 * decoder), hscale);
-		ret_val = cx25821_i2c_write(&dev->i2c_bus[0],
+		cx25821_i2c_write(&dev->i2c_bus[0],
 				VSCALE_CTRL + (0x200 * decoder), vscale);
 	}
 
@@ -494,7 +492,6 @@ void medusa_set_resolution(struct cx25821_dev *dev, int width,
 static void medusa_set_decoderduration(struct cx25821_dev *dev, int decoder,
 				       int duration)
 {
-	int ret_val = 0;
 	u32 fld_cnt = 0;
 	u32 tmp = 0;
 	u32 disp_cnt_reg = DISP_AB_CNT;
@@ -537,7 +534,7 @@ static void medusa_set_decoderduration(struct cx25821_dev *dev, int decoder,
 		fld_cnt |= ((u32) duration) << 16;
 	}
 
-	ret_val = cx25821_i2c_write(&dev->i2c_bus[0], disp_cnt_reg, fld_cnt);
+	cx25821_i2c_write(&dev->i2c_bus[0], disp_cnt_reg, fld_cnt);
 
 	mutex_unlock(&dev->lock);
 }
diff --git a/drivers/media/video/cx25821/cx25821-video-upstream-ch2.c b/drivers/media/video/cx25821/cx25821-video-upstream-ch2.c
index 5a157cf4a95..c8c94fbf5d8 100644
--- a/drivers/media/video/cx25821/cx25821-video-upstream-ch2.c
+++ b/drivers/media/video/cx25821/cx25821-video-upstream-ch2.c
@@ -587,7 +587,7 @@ int cx25821_video_upstream_irq_ch2(struct cx25821_dev *dev, int chan_num,
 static irqreturn_t cx25821_upstream_irq_ch2(int irq, void *dev_id)
 {
 	struct cx25821_dev *dev = dev_id;
-	u32 msk_stat, vid_status;
+	u32 vid_status;
 	int handled = 0;
 	int channel_num = 0;
 	struct sram_channel *sram_ch;
@@ -598,7 +598,6 @@ static irqreturn_t cx25821_upstream_irq_ch2(int irq, void *dev_id)
 	channel_num = VID_UPSTREAM_SRAM_CHANNEL_J;
 	sram_ch = dev->channels[channel_num].sram_channels;
 
-	msk_stat = cx_read(sram_ch->int_mstat);
 	vid_status = cx_read(sram_ch->int_stat);
 
 	/* Only deal with our interrupt */
diff --git a/drivers/media/video/cx25821/cx25821-video-upstream.c b/drivers/media/video/cx25821/cx25821-video-upstream.c
index 21e7d657f04..52c13e0b649 100644
--- a/drivers/media/video/cx25821/cx25821-video-upstream.c
+++ b/drivers/media/video/cx25821/cx25821-video-upstream.c
@@ -637,7 +637,7 @@ 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;
-	u32 msk_stat, vid_status;
+	u32 vid_status;
 	int handled = 0;
 	int channel_num = 0;
 	struct sram_channel *sram_ch;
@@ -649,7 +649,6 @@ static irqreturn_t cx25821_upstream_irq(int irq, void *dev_id)
 
 	sram_ch = dev->channels[channel_num].sram_channels;
 
-	msk_stat = cx_read(sram_ch->int_mstat);
 	vid_status = cx_read(sram_ch->int_stat);
 
 	/* Only deal with our interrupt */
diff --git a/drivers/media/video/cx25821/cx25821-video.c b/drivers/media/video/cx25821/cx25821-video.c
index ffd8bc79c02..b38d4379cc3 100644
--- a/drivers/media/video/cx25821/cx25821-video.c
+++ b/drivers/media/video/cx25821/cx25821-video.c
@@ -109,25 +109,6 @@ struct cx25821_fmt *cx25821_format_by_fourcc(unsigned int fourcc)
 	return NULL;
 }
 
-void cx25821_dump_video_queue(struct cx25821_dev *dev,
-			      struct cx25821_dmaqueue *q)
-{
-	struct cx25821_buffer *buf;
-	struct list_head *item;
-	dprintk(1, "%s()\n", __func__);
-
-	if (!list_empty(&q->active)) {
-		list_for_each(item, &q->active)
-			buf = list_entry(item, struct cx25821_buffer, vb.queue);
-	}
-
-	if (!list_empty(&q->queued)) {
-		list_for_each(item, &q->queued)
-			buf = list_entry(item, struct cx25821_buffer, vb.queue);
-	}
-
-}
-
 void cx25821_video_wakeup(struct cx25821_dev *dev, struct cx25821_dmaqueue *q,
 			  u32 count)
 {
@@ -557,7 +538,7 @@ int cx25821_buffer_prepare(struct videobuf_queue *q, struct videobuf_buffer *vb,
 	struct cx25821_buffer *buf =
 		container_of(vb, struct cx25821_buffer, vb);
 	int rc, init_buffer = 0;
-	u32 line0_offset, line1_offset;
+	u32 line0_offset;
 	struct videobuf_dmabuf *dma = videobuf_to_dma(&buf->vb);
 	int bpl_local = LINE_SIZE_D1;
 	int channel_opened = fh->channel_id;
@@ -639,7 +620,6 @@ int cx25821_buffer_prepare(struct videobuf_queue *q, struct videobuf_buffer *vb,
 		case V4L2_FIELD_INTERLACED:
 			/* All other formats are top field first */
 			line0_offset = 0;
-			line1_offset = buf->bpl;
 			dprintk(1, "top field first\n");
 
 			cx25821_risc_buffer(dev->pci, &buf->risc,
@@ -1830,7 +1810,6 @@ static long video_ioctl_set(struct file *file, unsigned int cmd,
 	int i = 0;
 	int cif_enable = 0;
 	int cif_width = 0;
-	u32 value = 0;
 
 	data_from_user = (struct downstream_user_struct *)arg;
 
@@ -1914,7 +1893,7 @@ static long video_ioctl_set(struct file *file, unsigned int cmd,
 		cx_write(data_from_user->reg_address, data_from_user->reg_data);
 		break;
 	case MEDUSA_READ:
-		value = cx25821_i2c_read(&dev->i2c_bus[0],
+		cx25821_i2c_read(&dev->i2c_bus[0],
 					 (u16) data_from_user->reg_address,
 					 &data_from_user->reg_data);
 		break;
diff --git a/drivers/media/video/cx25821/cx25821-video.h b/drivers/media/video/cx25821/cx25821-video.h
index d0d9538ca5b..9652a5e35ba 100644
--- a/drivers/media/video/cx25821/cx25821-video.h
+++ b/drivers/media/video/cx25821/cx25821-video.h
@@ -86,8 +86,6 @@ 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_dump_video_queue(struct cx25821_dev *dev,
-				     struct cx25821_dmaqueue *q);
 extern void cx25821_video_wakeup(struct cx25821_dev *dev,
 				 struct cx25821_dmaqueue *q, u32 count);
 
diff --git a/drivers/media/video/cx25840/cx25840-ir.c b/drivers/media/video/cx25840/cx25840-ir.c
index 13c380ebb56..38ce76ed192 100644
--- a/drivers/media/video/cx25840/cx25840-ir.c
+++ b/drivers/media/video/cx25840/cx25840-ir.c
@@ -316,9 +316,7 @@ static u64 ns_to_pulse_clocks(u32 ns)
 
 static u16 pulse_clocks_to_clock_divider(u64 count)
 {
-	u32 rem;
-
-	rem = do_div(count, (FIFO_RXTX << 2) | 0x3);
+	do_div(count, (FIFO_RXTX << 2) | 0x3);
 
 	/* net result needs to be rounded down and decremented by 1 */
 	if (count > RXCLK_RCD + 1)
@@ -860,12 +858,10 @@ static int cx25840_ir_tx_write(struct v4l2_subdev *sd, u8 *buf, size_t count,
 			       ssize_t *num)
 {
 	struct cx25840_ir_state *ir_state = to_ir_state(sd);
-	struct i2c_client *c;
 
 	if (ir_state == NULL)
 		return -ENODEV;
 
-	c = ir_state->c;
 #if 0
 	/*
 	 * FIXME - the code below is an incomplete and untested sketch of what
diff --git a/drivers/media/video/davinci/Kconfig b/drivers/media/video/davinci/Kconfig
index 60a456ebdc7..9337b5605c9 100644
--- a/drivers/media/video/davinci/Kconfig
+++ b/drivers/media/video/davinci/Kconfig
@@ -40,6 +40,7 @@ config VIDEO_VPSS_SYSTEM
 config VIDEO_VPFE_CAPTURE
 	tristate "VPFE 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
diff --git a/drivers/media/video/davinci/vpbe_display.c b/drivers/media/video/davinci/vpbe_display.c
index 1f3b1c72925..e106b72810a 100644
--- a/drivers/media/video/davinci/vpbe_display.c
+++ b/drivers/media/video/davinci/vpbe_display.c
@@ -1618,6 +1618,10 @@ static __devinit int init_vpbe_layer(int i, struct vpbe_display *disp_dev,
 	vbd->ioctl_ops	= &vpbe_ioctl_ops;
 	vbd->minor	= -1;
 	vbd->v4l2_dev   = &disp_dev->vpbe_dev->v4l2_dev;
+	/* Locking in file operations other than ioctl should be done
+	   by the driver, not the V4L2 core.
+	   This driver needs auditing so that this flag can be removed. */
+	set_bit(V4L2_FL_LOCK_ALL_FOPS, &vbd->flags);
 	vbd->lock	= &vpbe_display_layer->opslock;
 
 	if (disp_dev->vpbe_dev->current_timings.timings_type &
diff --git a/drivers/media/video/davinci/vpfe_capture.c b/drivers/media/video/davinci/vpfe_capture.c
index 20cf271a774..49a845fb804 100644
--- a/drivers/media/video/davinci/vpfe_capture.c
+++ b/drivers/media/video/davinci/vpfe_capture.c
@@ -1761,7 +1761,7 @@ static long vpfe_param_handler(struct file *file, void *priv,
 		}
 		break;
 	default:
-		ret = -EINVAL;
+		ret = -ENOTTY;
 	}
 unlock_out:
 	mutex_unlock(&vpfe_dev->lock);
diff --git a/drivers/media/video/davinci/vpif_capture.c b/drivers/media/video/davinci/vpif_capture.c
index 6504e40a31d..96046957bf2 100644
--- a/drivers/media/video/davinci/vpif_capture.c
+++ b/drivers/media/video/davinci/vpif_capture.c
@@ -2228,6 +2228,10 @@ static __init int vpif_probe(struct platform_device *pdev)
 		common = &(ch->common[VPIF_VIDEO_INDEX]);
 		spin_lock_init(&common->irqlock);
 		mutex_init(&common->lock);
+		/* Locking in file operations other than ioctl should be done
+		   by the driver, not the V4L2 core.
+		   This driver needs auditing so that this flag can be removed. */
+		set_bit(V4L2_FL_LOCK_ALL_FOPS, &ch->video_dev->flags);
 		ch->video_dev->lock = &common->lock;
 		/* Initialize prio member of channel object */
 		v4l2_prio_init(&ch->prio);
diff --git a/drivers/media/video/davinci/vpif_display.c b/drivers/media/video/davinci/vpif_display.c
index 7fa34b4fae2..e6488ee7db1 100644
--- a/drivers/media/video/davinci/vpif_display.c
+++ b/drivers/media/video/davinci/vpif_display.c
@@ -1778,6 +1778,10 @@ static __init int vpif_probe(struct platform_device *pdev)
 		v4l2_prio_init(&ch->prio);
 		ch->common[VPIF_VIDEO_INDEX].fmt.type =
 						V4L2_BUF_TYPE_VIDEO_OUTPUT;
+		/* Locking in file operations other than ioctl should be done
+		   by the driver, not the V4L2 core.
+		   This driver needs auditing so that this flag can be removed. */
+		set_bit(V4L2_FL_LOCK_ALL_FOPS, &ch->video_dev->flags);
 		ch->video_dev->lock = &common->lock;
 
 		/* register video device */
diff --git a/drivers/media/video/em28xx/Kconfig b/drivers/media/video/em28xx/Kconfig
index f6f622e123b..928ef0d0429 100644
--- a/drivers/media/video/em28xx/Kconfig
+++ b/drivers/media/video/em28xx/Kconfig
@@ -49,10 +49,10 @@ config VIDEO_EM28XX_DVB
 	  Empiatech em28xx chips.
 
 config VIDEO_EM28XX_RC
-        bool "EM28XX Remote Controller support"
+        tristate "EM28XX Remote Controller support"
         depends on RC_CORE
         depends on VIDEO_EM28XX
         depends on !(RC_CORE=m && VIDEO_EM28XX=y)
-        default y
+        default VIDEO_EM28XX
         ---help---
           Enables Remote Controller support on em28xx driver.
diff --git a/drivers/media/video/em28xx/Makefile b/drivers/media/video/em28xx/Makefile
index 2abdf76c520..c8b338d4be0 100644
--- a/drivers/media/video/em28xx/Makefile
+++ b/drivers/media/video/em28xx/Makefile
@@ -1,16 +1,15 @@
 em28xx-y :=	em28xx-video.o em28xx-i2c.o em28xx-cards.o
 em28xx-y +=	em28xx-core.o  em28xx-vbi.o
 
-em28xx-$(CONFIG_VIDEO_EM28XX_RC) += em28xx-input.o
-
 em28xx-alsa-objs := em28xx-audio.o
+em28xx-rc-objs := em28xx-input.o
 
 obj-$(CONFIG_VIDEO_EM28XX) += em28xx.o
 obj-$(CONFIG_VIDEO_EM28XX_ALSA) += em28xx-alsa.o
 obj-$(CONFIG_VIDEO_EM28XX_DVB) += em28xx-dvb.o
+obj-$(CONFIG_VIDEO_EM28XX_RC) += em28xx-rc.o
 
 ccflags-y += -Idrivers/media/video
 ccflags-y += -Idrivers/media/common/tuners
 ccflags-y += -Idrivers/media/dvb/dvb-core
 ccflags-y += -Idrivers/media/dvb/frontends
-
diff --git a/drivers/media/video/em28xx/em28xx-audio.c b/drivers/media/video/em28xx/em28xx-audio.c
index e2a7b77c39c..d7e2a3dc552 100644
--- a/drivers/media/video/em28xx/em28xx-audio.c
+++ b/drivers/media/video/em28xx/em28xx-audio.c
@@ -343,7 +343,6 @@ static int snd_em28xx_pcm_close(struct snd_pcm_substream *substream)
 static int snd_em28xx_hw_capture_params(struct snd_pcm_substream *substream,
 					struct snd_pcm_hw_params *hw_params)
 {
-	unsigned int channels, rate, format;
 	int ret;
 
 	dprintk("Setting capture parameters\n");
@@ -352,13 +351,17 @@ static int snd_em28xx_hw_capture_params(struct snd_pcm_substream *substream,
 				params_buffer_bytes(hw_params));
 	if (ret < 0)
 		return ret;
+#if 0
+	/* TODO: set up em28xx audio chip to deliver the correct audio format,
+	   current default is 48000hz multiplexed => 96000hz mono
+	   which shouldn't matter since analogue TV only supports mono */
+	unsigned int channels, rate, format;
+
 	format = params_format(hw_params);
 	rate = params_rate(hw_params);
 	channels = params_channels(hw_params);
+#endif
 
-	/* TODO: set up em28xx audio chip to deliver the correct audio format,
-	   current default is 48000hz multiplexed => 96000hz mono
-	   which shouldn't matter since analogue TV only supports mono */
 	return 0;
 }
 
diff --git a/drivers/media/video/em28xx/em28xx-cards.c b/drivers/media/video/em28xx/em28xx-cards.c
index 9fd8cc7dbb2..20a7e24de6f 100644
--- a/drivers/media/video/em28xx/em28xx-cards.c
+++ b/drivers/media/video/em28xx/em28xx-cards.c
@@ -69,6 +69,8 @@ struct em28xx_hash_table {
 	unsigned int  tuner;
 };
 
+static void em28xx_pre_card_setup(struct em28xx *dev);
+
 /*
  *  Reset sequences for analog/digital modes
  */
@@ -2361,7 +2363,7 @@ static int em28xx_hint_sensor(struct em28xx *dev)
 /* Since em28xx_pre_card_setup() requires a proper dev->model,
  * this won't work for boards with generic PCI IDs
  */
-void em28xx_pre_card_setup(struct em28xx *dev)
+static void em28xx_pre_card_setup(struct em28xx *dev)
 {
 	/* Set the initial XCLK and I2C clock values based on the board
 	   definition */
@@ -2661,55 +2663,7 @@ static int em28xx_hint_board(struct em28xx *dev)
 	return -1;
 }
 
-/* ----------------------------------------------------------------------- */
-void em28xx_register_i2c_ir(struct em28xx *dev)
-{
-	/* Leadtek winfast tv USBII deluxe can find a non working IR-device */
-	/* at address 0x18, so if that address is needed for another board in */
-	/* the future, please put it after 0x1f. */
-	struct i2c_board_info info;
-	const unsigned short addr_list[] = {
-		 0x1f, 0x30, 0x47, I2C_CLIENT_END
-	};
-
-	if (disable_ir)
-		return;
-
-	memset(&info, 0, sizeof(struct i2c_board_info));
-	memset(&dev->init_data, 0, sizeof(dev->init_data));
-	strlcpy(info.type, "ir_video", I2C_NAME_SIZE);
-
-	/* detect & configure */
-	switch (dev->model) {
-	case EM2800_BOARD_TERRATEC_CINERGY_200:
-	case EM2820_BOARD_TERRATEC_CINERGY_250:
-		dev->init_data.ir_codes = RC_MAP_EM_TERRATEC;
-		dev->init_data.get_key = em28xx_get_key_terratec;
-		dev->init_data.name = "i2c IR (EM28XX Terratec)";
-		break;
-	case EM2820_BOARD_PINNACLE_USB_2:
-		dev->init_data.ir_codes = RC_MAP_PINNACLE_GREY;
-		dev->init_data.get_key = em28xx_get_key_pinnacle_usb_grey;
-		dev->init_data.name = "i2c IR (EM28XX Pinnacle PCTV)";
-		break;
-	case EM2820_BOARD_HAUPPAUGE_WINTV_USB_2:
-		dev->init_data.ir_codes = RC_MAP_HAUPPAUGE;
-		dev->init_data.get_key = em28xx_get_key_em_haup;
-		dev->init_data.name = "i2c IR (EM2840 Hauppauge)";
-		break;
-	case EM2820_BOARD_LEADTEK_WINFAST_USBII_DELUXE:
-		dev->init_data.ir_codes = RC_MAP_WINFAST_USBII_DELUXE;
-		dev->init_data.get_key = em28xx_get_key_winfast_usbii_deluxe;
-		dev->init_data.name = "i2c IR (EM2820 Winfast TV USBII Deluxe)";
-		break;
-	}
-
-	if (dev->init_data.name)
-		info.platform_data = &dev->init_data;
-	i2c_new_probed_device(&dev->i2c_adap, &info, addr_list, NULL);
-}
-
-void em28xx_card_setup(struct em28xx *dev)
+static void em28xx_card_setup(struct em28xx *dev)
 {
 	/*
 	 * If the device can be a webcam, seek for a sensor.
@@ -2849,13 +2803,6 @@ void em28xx_card_setup(struct em28xx *dev)
 		break;
 	}
 
-#if defined(CONFIG_MODULES) && defined(MODULE)
-	if (dev->board.has_ir_i2c && !disable_ir)
-		request_module("ir-kbd-i2c");
-#endif
-	if (dev->board.has_snapshot_button)
-		em28xx_register_snapshot_button(dev);
-
 	if (dev->board.valid == EM28XX_BOARD_NOT_VALIDATED) {
 		em28xx_errdev("\n\n");
 		em28xx_errdev("The support for this board weren't "
@@ -2929,9 +2876,6 @@ void em28xx_card_setup(struct em28xx *dev)
 	}
 
 	em28xx_tuner_setup(dev);
-
-	if(!disable_ir)
-		em28xx_ir_init(dev);
 }
 
 
@@ -2948,6 +2892,8 @@ static void request_module_async(struct work_struct *work)
 
 	if (dev->board.has_dvb)
 		request_module("em28xx-dvb");
+	if (dev->board.has_ir_i2c && !disable_ir)
+		request_module("em28xx-rc");
 }
 
 static void request_modules(struct em28xx *dev)
@@ -2972,12 +2918,6 @@ static void flush_request_modules(struct em28xx *dev)
 */
 void em28xx_release_resources(struct em28xx *dev)
 {
-	if (dev->sbutton_input_dev)
-		em28xx_deregister_snapshot_button(dev);
-
-	if (dev->ir)
-		em28xx_ir_fini(dev);
-
 	/*FIXME: I2C IR should be disconnected */
 
 	em28xx_release_analog_resources(dev);
@@ -3005,9 +2945,6 @@ static int em28xx_init_dev(struct em28xx *dev, struct usb_device *udev,
 	dev->udev = udev;
 	mutex_init(&dev->ctrl_urb_lock);
 	spin_lock_init(&dev->slock);
-	init_waitqueue_head(&dev->open);
-	init_waitqueue_head(&dev->wait_frame);
-	init_waitqueue_head(&dev->wait_stream);
 
 	dev->em28xx_write_regs = em28xx_write_regs;
 	dev->em28xx_read_reg = em28xx_read_reg;
@@ -3140,9 +3077,7 @@ static int em28xx_init_dev(struct em28xx *dev, struct usb_device *udev,
 
 	/* init video dma queues */
 	INIT_LIST_HEAD(&dev->vidq.active);
-	INIT_LIST_HEAD(&dev->vidq.queued);
 	INIT_LIST_HEAD(&dev->vbiq.active);
-	INIT_LIST_HEAD(&dev->vbiq.queued);
 
 	if (dev->board.has_msp34xx) {
 		/* Send a reset to other chips via gpio */
@@ -3447,8 +3382,6 @@ static void em28xx_usb_disconnect(struct usb_interface *interface)
 	   resources */
 	mutex_lock(&dev->lock);
 
-	wake_up_interruptible_all(&dev->open);
-
 	v4l2_device_disconnect(&dev->v4l2_dev);
 
 	if (dev->users) {
@@ -3460,8 +3393,6 @@ static void em28xx_usb_disconnect(struct usb_interface *interface)
 		dev->state |= DEV_MISCONFIGURED;
 		em28xx_uninit_isoc(dev, dev->mode);
 		dev->state |= DEV_DISCONNECTED;
-		wake_up_interruptible(&dev->wait_frame);
-		wake_up_interruptible(&dev->wait_stream);
 	} else {
 		dev->state |= DEV_DISCONNECTED;
 		em28xx_release_resources(dev);
diff --git a/drivers/media/video/em28xx/em28xx-core.c b/drivers/media/video/em28xx/em28xx-core.c
index 53a9fb91e97..5717bdee8f1 100644
--- a/drivers/media/video/em28xx/em28xx-core.c
+++ b/drivers/media/video/em28xx/em28xx-core.c
@@ -139,6 +139,7 @@ int em28xx_read_reg(struct em28xx *dev, u16 reg)
 {
 	return em28xx_read_reg_req(dev, USB_REQ_GET_STATUS, reg);
 }
+EXPORT_SYMBOL_GPL(em28xx_read_reg);
 
 /*
  * em28xx_write_regs_req()
@@ -205,6 +206,7 @@ int em28xx_write_regs(struct em28xx *dev, u16 reg, char *buf, int len)
 
 	return rc;
 }
+EXPORT_SYMBOL_GPL(em28xx_write_regs);
 
 /* Write a single register */
 int em28xx_write_reg(struct em28xx *dev, u16 reg, u8 val)
@@ -239,6 +241,7 @@ int em28xx_write_reg_bits(struct em28xx *dev, u16 reg, u8 val,
 
 	return em28xx_write_regs(dev, reg, &newval, 1);
 }
+EXPORT_SYMBOL_GPL(em28xx_write_reg_bits);
 
 /*
  * em28xx_is_ac97_ready()
@@ -666,7 +669,6 @@ int em28xx_capture_start(struct em28xx *dev, int start)
 
 	return rc;
 }
-EXPORT_SYMBOL_GPL(em28xx_capture_start);
 
 int em28xx_vbi_supported(struct em28xx *dev)
 {
@@ -975,7 +977,6 @@ void em28xx_uninit_isoc(struct em28xx *dev, enum em28xx_mode mode)
 	else
 		isoc_bufs = &dev->isoc_ctl.analog_bufs;
 
-	dev->isoc_ctl.nfields = -1;
 	for (i = 0; i < isoc_bufs->num_bufs; i++) {
 		urb = isoc_bufs->urb[i];
 		if (urb) {
@@ -1008,6 +1009,31 @@ void em28xx_uninit_isoc(struct em28xx *dev, enum em28xx_mode mode)
 EXPORT_SYMBOL_GPL(em28xx_uninit_isoc);
 
 /*
+ * Stop URBs
+ */
+void em28xx_stop_urbs(struct em28xx *dev)
+{
+	int i;
+	struct urb *urb;
+	struct em28xx_usb_isoc_bufs *isoc_bufs = &dev->isoc_ctl.digital_bufs;
+
+	em28xx_isocdbg("em28xx: called em28xx_stop_urbs\n");
+
+	for (i = 0; i < isoc_bufs->num_bufs; i++) {
+		urb = isoc_bufs->urb[i];
+		if (urb) {
+			if (!irqs_disabled())
+				usb_kill_urb(urb);
+			else
+				usb_unlink_urb(urb);
+		}
+	}
+
+	em28xx_capture_start(dev, 0);
+}
+EXPORT_SYMBOL_GPL(em28xx_stop_urbs);
+
+/*
  * Allocate URBs
  */
 int em28xx_alloc_isoc(struct em28xx *dev, enum em28xx_mode mode,
diff --git a/drivers/media/video/em28xx/em28xx-dvb.c b/drivers/media/video/em28xx/em28xx-dvb.c
index 503a8d5b538..16410ac2009 100644
--- a/drivers/media/video/em28xx/em28xx-dvb.c
+++ b/drivers/media/video/em28xx/em28xx-dvb.c
@@ -183,7 +183,7 @@ static int em28xx_stop_streaming(struct em28xx_dvb *dvb)
 {
 	struct em28xx *dev = dvb->adapter.priv;
 
-	em28xx_capture_start(dev, 0);
+	em28xx_stop_urbs(dev);
 
 	em28xx_set_mode(dev, EM28XX_SUSPEND);
 
@@ -336,6 +336,8 @@ struct drxk_config pctv_520e_drxk = {
 	.single_master = 1,
 	.microcode_name = "dvb-demod-drxk-pctv.fw",
 	.chunk_size = 58,
+	.antenna_dvbt = true, /* disable LNA */
+	.antenna_gpio = (1 << 2), /* disable LNA */
 };
 
 static int drxk_gate_ctrl(struct dvb_frontend *fe, int enable)
@@ -474,8 +476,8 @@ static void terratec_h5_init(struct em28xx *dev)
 static void pctv_520e_init(struct em28xx *dev)
 {
 	/*
-	 * Init TDA8295(?) analog demodulator. Looks like I2C traffic to
-	 * digital demodulator and tuner are routed via TDA8295.
+	 * Init AVF4910B analog decoder. Looks like I2C traffic to
+	 * digital demodulator and tuner are routed via AVF4910B.
 	 */
 	int i;
 	struct {
@@ -542,7 +544,8 @@ static struct cxd2820r_config em28xx_cxd2820r_config = {
 	.i2c_address = (0xd8 >> 1),
 	.ts_mode = CXD2820R_TS_SERIAL,
 
-	/* enable LNA for DVB-T2 and DVB-C */
+	/* enable LNA for DVB-T, DVB-T2 and DVB-C */
+	.gpio_dvbt[0] = CXD2820R_GPIO_E | CXD2820R_GPIO_O | CXD2820R_GPIO_L,
 	.gpio_dvbt2[0] = CXD2820R_GPIO_E | CXD2820R_GPIO_O | CXD2820R_GPIO_L,
 	.gpio_dvbc[0] = CXD2820R_GPIO_E | CXD2820R_GPIO_O | CXD2820R_GPIO_L,
 };
diff --git a/drivers/media/video/em28xx/em28xx-i2c.c b/drivers/media/video/em28xx/em28xx-i2c.c
index a88e169dba2..185db65b766 100644
--- a/drivers/media/video/em28xx/em28xx-i2c.c
+++ b/drivers/media/video/em28xx/em28xx-i2c.c
@@ -553,9 +553,6 @@ int em28xx_i2c_register(struct em28xx *dev)
 	if (i2c_scan)
 		em28xx_do_i2c_scan(dev);
 
-	/* Instantiate the IR receiver device, if present */
-	em28xx_register_i2c_ir(dev);
-
 	return 0;
 }
 
diff --git a/drivers/media/video/em28xx/em28xx-input.c b/drivers/media/video/em28xx/em28xx-input.c
index 2630b265b0e..fce5f7680c9 100644
--- a/drivers/media/video/em28xx/em28xx-input.c
+++ b/drivers/media/video/em28xx/em28xx-input.c
@@ -80,7 +80,7 @@ struct em28xx_IR {
  I2C IR based get keycodes - should be used with ir-kbd-i2c
  **********************************************************/
 
-int em28xx_get_key_terratec(struct IR_i2c *ir, u32 *ir_key, u32 *ir_raw)
+static int em28xx_get_key_terratec(struct IR_i2c *ir, u32 *ir_key, u32 *ir_raw)
 {
 	unsigned char b;
 
@@ -108,7 +108,7 @@ int em28xx_get_key_terratec(struct IR_i2c *ir, u32 *ir_key, u32 *ir_raw)
 	return 1;
 }
 
-int em28xx_get_key_em_haup(struct IR_i2c *ir, u32 *ir_key, u32 *ir_raw)
+static int em28xx_get_key_em_haup(struct IR_i2c *ir, u32 *ir_key, u32 *ir_raw)
 {
 	unsigned char buf[2];
 	u16 code;
@@ -157,7 +157,7 @@ int em28xx_get_key_em_haup(struct IR_i2c *ir, u32 *ir_key, u32 *ir_raw)
 	return 1;
 }
 
-int em28xx_get_key_pinnacle_usb_grey(struct IR_i2c *ir, u32 *ir_key,
+static int em28xx_get_key_pinnacle_usb_grey(struct IR_i2c *ir, u32 *ir_key,
 				     u32 *ir_raw)
 {
 	unsigned char buf[3];
@@ -179,7 +179,8 @@ int em28xx_get_key_pinnacle_usb_grey(struct IR_i2c *ir, u32 *ir_key,
 	return 1;
 }
 
-int em28xx_get_key_winfast_usbii_deluxe(struct IR_i2c *ir, u32 *ir_key, u32 *ir_raw)
+static int em28xx_get_key_winfast_usbii_deluxe(struct IR_i2c *ir, u32 *ir_key,
+					u32 *ir_raw)
 {
 	unsigned char subaddr, keydetect, key;
 
@@ -387,7 +388,138 @@ int em28xx_ir_change_protocol(struct rc_dev *rc_dev, u64 rc_type)
 	return rc;
 }
 
-int em28xx_ir_init(struct em28xx *dev)
+static void em28xx_register_i2c_ir(struct em28xx *dev)
+{
+	/* Leadtek winfast tv USBII deluxe can find a non working IR-device */
+	/* at address 0x18, so if that address is needed for another board in */
+	/* the future, please put it after 0x1f. */
+	struct i2c_board_info info;
+	const unsigned short addr_list[] = {
+		 0x1f, 0x30, 0x47, I2C_CLIENT_END
+	};
+
+	memset(&info, 0, sizeof(struct i2c_board_info));
+	memset(&dev->init_data, 0, sizeof(dev->init_data));
+	strlcpy(info.type, "ir_video", I2C_NAME_SIZE);
+
+	/* detect & configure */
+	switch (dev->model) {
+	case EM2800_BOARD_TERRATEC_CINERGY_200:
+	case EM2820_BOARD_TERRATEC_CINERGY_250:
+		dev->init_data.ir_codes = RC_MAP_EM_TERRATEC;
+		dev->init_data.get_key = em28xx_get_key_terratec;
+		dev->init_data.name = "i2c IR (EM28XX Terratec)";
+		break;
+	case EM2820_BOARD_PINNACLE_USB_2:
+		dev->init_data.ir_codes = RC_MAP_PINNACLE_GREY;
+		dev->init_data.get_key = em28xx_get_key_pinnacle_usb_grey;
+		dev->init_data.name = "i2c IR (EM28XX Pinnacle PCTV)";
+		break;
+	case EM2820_BOARD_HAUPPAUGE_WINTV_USB_2:
+		dev->init_data.ir_codes = RC_MAP_HAUPPAUGE;
+		dev->init_data.get_key = em28xx_get_key_em_haup;
+		dev->init_data.name = "i2c IR (EM2840 Hauppauge)";
+		break;
+	case EM2820_BOARD_LEADTEK_WINFAST_USBII_DELUXE:
+		dev->init_data.ir_codes = RC_MAP_WINFAST_USBII_DELUXE;
+		dev->init_data.get_key = em28xx_get_key_winfast_usbii_deluxe;
+		dev->init_data.name = "i2c IR (EM2820 Winfast TV USBII Deluxe)";
+		break;
+	}
+
+	if (dev->init_data.name)
+		info.platform_data = &dev->init_data;
+	i2c_new_probed_device(&dev->i2c_adap, &info, addr_list, NULL);
+}
+
+/**********************************************************
+ Handle Webcam snapshot button
+ **********************************************************/
+
+static void em28xx_query_sbutton(struct work_struct *work)
+{
+	/* Poll the register and see if the button is depressed */
+	struct em28xx *dev =
+		container_of(work, struct em28xx, sbutton_query_work.work);
+	int ret;
+
+	ret = em28xx_read_reg(dev, EM28XX_R0C_USBSUSP);
+
+	if (ret & EM28XX_R0C_USBSUSP_SNAPSHOT) {
+		u8 cleared;
+		/* Button is depressed, clear the register */
+		cleared = ((u8) ret) & ~EM28XX_R0C_USBSUSP_SNAPSHOT;
+		em28xx_write_regs(dev, EM28XX_R0C_USBSUSP, &cleared, 1);
+
+		/* Not emulate the keypress */
+		input_report_key(dev->sbutton_input_dev, EM28XX_SNAPSHOT_KEY,
+				 1);
+		/* Now unpress the key */
+		input_report_key(dev->sbutton_input_dev, EM28XX_SNAPSHOT_KEY,
+				 0);
+	}
+
+	/* Schedule next poll */
+	schedule_delayed_work(&dev->sbutton_query_work,
+			      msecs_to_jiffies(EM28XX_SBUTTON_QUERY_INTERVAL));
+}
+
+static void em28xx_register_snapshot_button(struct em28xx *dev)
+{
+	struct input_dev *input_dev;
+	int err;
+
+	em28xx_info("Registering snapshot button...\n");
+	input_dev = input_allocate_device();
+	if (!input_dev) {
+		em28xx_errdev("input_allocate_device failed\n");
+		return;
+	}
+
+	usb_make_path(dev->udev, dev->snapshot_button_path,
+		      sizeof(dev->snapshot_button_path));
+	strlcat(dev->snapshot_button_path, "/sbutton",
+		sizeof(dev->snapshot_button_path));
+	INIT_DELAYED_WORK(&dev->sbutton_query_work, em28xx_query_sbutton);
+
+	input_dev->name = "em28xx snapshot button";
+	input_dev->phys = dev->snapshot_button_path;
+	input_dev->evbit[0] = BIT_MASK(EV_KEY) | BIT_MASK(EV_REP);
+	set_bit(EM28XX_SNAPSHOT_KEY, input_dev->keybit);
+	input_dev->keycodesize = 0;
+	input_dev->keycodemax = 0;
+	input_dev->id.bustype = BUS_USB;
+	input_dev->id.vendor = le16_to_cpu(dev->udev->descriptor.idVendor);
+	input_dev->id.product = le16_to_cpu(dev->udev->descriptor.idProduct);
+	input_dev->id.version = 1;
+	input_dev->dev.parent = &dev->udev->dev;
+
+	err = input_register_device(input_dev);
+	if (err) {
+		em28xx_errdev("input_register_device failed\n");
+		input_free_device(input_dev);
+		return;
+	}
+
+	dev->sbutton_input_dev = input_dev;
+	schedule_delayed_work(&dev->sbutton_query_work,
+			      msecs_to_jiffies(EM28XX_SBUTTON_QUERY_INTERVAL));
+	return;
+
+}
+
+static void em28xx_deregister_snapshot_button(struct em28xx *dev)
+{
+	if (dev->sbutton_input_dev != NULL) {
+		em28xx_info("Deregistering snapshot button\n");
+		cancel_delayed_work_sync(&dev->sbutton_query_work);
+		input_unregister_device(dev->sbutton_input_dev);
+		dev->sbutton_input_dev = NULL;
+	}
+	return;
+}
+
+static int em28xx_ir_init(struct em28xx *dev)
 {
 	struct em28xx_IR *ir;
 	struct rc_dev *rc;
@@ -448,6 +580,15 @@ int em28xx_ir_init(struct em28xx *dev)
 	if (err)
 		goto err_out_stop;
 
+	em28xx_register_i2c_ir(dev);
+
+#if defined(CONFIG_MODULES) && defined(MODULE)
+	if (dev->board.has_ir_i2c)
+		request_module("ir-kbd-i2c");
+#endif
+	if (dev->board.has_snapshot_button)
+		em28xx_register_snapshot_button(dev);
+
 	return 0;
 
  err_out_stop:
@@ -458,10 +599,12 @@ int em28xx_ir_init(struct em28xx *dev)
 	return err;
 }
 
-int em28xx_ir_fini(struct em28xx *dev)
+static int em28xx_ir_fini(struct em28xx *dev)
 {
 	struct em28xx_IR *ir = dev->ir;
 
+	em28xx_deregister_snapshot_button(dev);
+
 	/* skip detach on non attached boards */
 	if (!ir)
 		return 0;
@@ -475,89 +618,26 @@ int em28xx_ir_fini(struct em28xx *dev)
 	return 0;
 }
 
-/**********************************************************
- Handle Webcam snapshot button
- **********************************************************/
+static struct em28xx_ops rc_ops = {
+	.id   = EM28XX_RC,
+	.name = "Em28xx Input Extension",
+	.init = em28xx_ir_init,
+	.fini = em28xx_ir_fini,
+};
 
-static void em28xx_query_sbutton(struct work_struct *work)
+static int __init em28xx_rc_register(void)
 {
-	/* Poll the register and see if the button is depressed */
-	struct em28xx *dev =
-		container_of(work, struct em28xx, sbutton_query_work.work);
-	int ret;
-
-	ret = em28xx_read_reg(dev, EM28XX_R0C_USBSUSP);
-
-	if (ret & EM28XX_R0C_USBSUSP_SNAPSHOT) {
-		u8 cleared;
-		/* Button is depressed, clear the register */
-		cleared = ((u8) ret) & ~EM28XX_R0C_USBSUSP_SNAPSHOT;
-		em28xx_write_regs(dev, EM28XX_R0C_USBSUSP, &cleared, 1);
-
-		/* Not emulate the keypress */
-		input_report_key(dev->sbutton_input_dev, EM28XX_SNAPSHOT_KEY,
-				 1);
-		/* Now unpress the key */
-		input_report_key(dev->sbutton_input_dev, EM28XX_SNAPSHOT_KEY,
-				 0);
-	}
-
-	/* Schedule next poll */
-	schedule_delayed_work(&dev->sbutton_query_work,
-			      msecs_to_jiffies(EM28XX_SBUTTON_QUERY_INTERVAL));
+	return em28xx_register_extension(&rc_ops);
 }
 
-void em28xx_register_snapshot_button(struct em28xx *dev)
+static void __exit em28xx_rc_unregister(void)
 {
-	struct input_dev *input_dev;
-	int err;
-
-	em28xx_info("Registering snapshot button...\n");
-	input_dev = input_allocate_device();
-	if (!input_dev) {
-		em28xx_errdev("input_allocate_device failed\n");
-		return;
-	}
-
-	usb_make_path(dev->udev, dev->snapshot_button_path,
-		      sizeof(dev->snapshot_button_path));
-	strlcat(dev->snapshot_button_path, "/sbutton",
-		sizeof(dev->snapshot_button_path));
-	INIT_DELAYED_WORK(&dev->sbutton_query_work, em28xx_query_sbutton);
-
-	input_dev->name = "em28xx snapshot button";
-	input_dev->phys = dev->snapshot_button_path;
-	input_dev->evbit[0] = BIT_MASK(EV_KEY) | BIT_MASK(EV_REP);
-	set_bit(EM28XX_SNAPSHOT_KEY, input_dev->keybit);
-	input_dev->keycodesize = 0;
-	input_dev->keycodemax = 0;
-	input_dev->id.bustype = BUS_USB;
-	input_dev->id.vendor = le16_to_cpu(dev->udev->descriptor.idVendor);
-	input_dev->id.product = le16_to_cpu(dev->udev->descriptor.idProduct);
-	input_dev->id.version = 1;
-	input_dev->dev.parent = &dev->udev->dev;
-
-	err = input_register_device(input_dev);
-	if (err) {
-		em28xx_errdev("input_register_device failed\n");
-		input_free_device(input_dev);
-		return;
-	}
-
-	dev->sbutton_input_dev = input_dev;
-	schedule_delayed_work(&dev->sbutton_query_work,
-			      msecs_to_jiffies(EM28XX_SBUTTON_QUERY_INTERVAL));
-	return;
-
+	em28xx_unregister_extension(&rc_ops);
 }
 
-void em28xx_deregister_snapshot_button(struct em28xx *dev)
-{
-	if (dev->sbutton_input_dev != NULL) {
-		em28xx_info("Deregistering snapshot button\n");
-		cancel_delayed_work_sync(&dev->sbutton_query_work);
-		input_unregister_device(dev->sbutton_input_dev);
-		dev->sbutton_input_dev = NULL;
-	}
-	return;
-}
+MODULE_LICENSE("GPL");
+MODULE_AUTHOR("Mauro Carvalho Chehab <mchehab@redhat.com>");
+MODULE_DESCRIPTION("Em28xx Input driver");
+
+module_init(em28xx_rc_register);
+module_exit(em28xx_rc_unregister);
diff --git a/drivers/media/video/em28xx/em28xx-video.c b/drivers/media/video/em28xx/em28xx-video.c
index 324b695c072..50f5f4fc214 100644
--- a/drivers/media/video/em28xx/em28xx-video.c
+++ b/drivers/media/video/em28xx/em28xx-video.c
@@ -1305,9 +1305,7 @@ static int vidioc_s_input(struct file *file, void *priv, unsigned int i)
 	if (0 == INPUT(i)->type)
 		return -EINVAL;
 
-	dev->ctl_input = i;
-
-	video_mux(dev, dev->ctl_input);
+	video_mux(dev, i);
 	return 0;
 }
 
@@ -2262,6 +2260,7 @@ static int em28xx_v4l2_close(struct file *filp)
 			em28xx_release_resources(dev);
 			kfree(dev->alt_max_pkt_size);
 			kfree(dev);
+			kfree(fh);
 			return 0;
 		}
 
@@ -2286,7 +2285,6 @@ static int em28xx_v4l2_close(struct file *filp)
 	videobuf_mmap_free(&fh->vb_vbiq);
 	kfree(fh);
 	dev->users--;
-	wake_up_interruptible_nr(&dev->open, 1);
 	return 0;
 }
 
@@ -2497,6 +2495,10 @@ static struct video_device *em28xx_vdev_init(struct em28xx *dev,
 	vfd->release	= video_device_release;
 	vfd->debug	= video_debug;
 	vfd->lock	= &dev->lock;
+	/* Locking in file operations other than ioctl should be done
+	   by the driver, not the V4L2 core.
+	   This driver needs auditing so that this flag can be removed. */
+	set_bit(V4L2_FL_LOCK_ALL_FOPS, &vfd->flags);
 
 	snprintf(vfd->name, sizeof(vfd->name), "%s %s",
 		 dev->name, type_name);
@@ -2518,7 +2520,6 @@ int em28xx_register_analog_devices(struct em28xx *dev)
 	dev->norm = em28xx_video_template.current_norm;
 	v4l2_device_call_all(&dev->v4l2_dev, 0, core, s_std, dev->norm);
 	dev->interlaced = EM28XX_INTERLACED_DEFAULT;
-	dev->ctl_input = 0;
 
 	/* Analog specific initialization */
 	dev->format = &format[0];
@@ -2532,7 +2533,7 @@ int em28xx_register_analog_devices(struct em28xx *dev)
 	em28xx_set_video_format(dev, format[0].fourcc,
 				maxw, norm_maxh(dev));
 
-	video_mux(dev, dev->ctl_input);
+	video_mux(dev, 0);
 
 	/* Audio defaults */
 	dev->mute = 1;
diff --git a/drivers/media/video/em28xx/em28xx.h b/drivers/media/video/em28xx/em28xx.h
index 2868b19f8b5..8757523e686 100644
--- a/drivers/media/video/em28xx/em28xx.h
+++ b/drivers/media/video/em28xx/em28xx.h
@@ -226,24 +226,10 @@ struct em28xx_usb_isoc_ctl {
 		/* isoc transfer buffers for digital mode */
 	struct em28xx_usb_isoc_bufs	digital_bufs;
 
-		/* Last buffer command and region */
-	u8				cmd;
-	int				pos, size, pktsize;
-
-		/* Last field: ODD or EVEN? */
-	int				field;
-
-		/* Stores incomplete commands */
-	u32				tmp_buf;
-	int				tmp_buf_len;
-
 		/* Stores already requested buffers */
 	struct em28xx_buffer    	*vid_buf;
 	struct em28xx_buffer    	*vbi_buf;
 
-		/* Stores the number of received fields */
-	int				nfields;
-
 		/* isoc urb callback */
 	int (*isoc_copy) (struct em28xx *dev, struct urb *urb);
 
@@ -264,12 +250,10 @@ struct em28xx_buffer {
 
 	struct list_head frame;
 	int top_field;
-	int receiving;
 };
 
 struct em28xx_dmaqueue {
 	struct list_head       active;
-	struct list_head       queued;
 
 	wait_queue_head_t          wq;
 
@@ -277,13 +261,6 @@ struct em28xx_dmaqueue {
 	int                        pos;
 };
 
-/* io methods */
-enum em28xx_io_method {
-	IO_NONE,
-	IO_READ,
-	IO_MMAP,
-};
-
 /* inputs */
 
 #define MAX_EM28XX_INPUT 4
@@ -467,6 +444,7 @@ enum em28xx_dev_state {
 /* em28xx extensions */
 #define EM28XX_AUDIO   0x10
 #define EM28XX_DVB     0x20
+#define EM28XX_RC      0x30
 
 /* em28xx resource types (used for res_get/res_lock etc */
 #define EM28XX_RESOURCE_VIDEO 0x01
@@ -577,7 +555,6 @@ struct em28xx {
 
 	/* states */
 	enum em28xx_dev_state state;
-	enum em28xx_io_method io;
 
 	/* vbi related state tracking */
 	int capture_type;
@@ -593,7 +570,6 @@ struct em28xx {
 	struct mutex ctrl_urb_lock;	/* protects urb_buf */
 	/* spinlock_t queue_lock; */
 	struct list_head inqueue, outqueue;
-	wait_queue_head_t open, wait_frame, wait_stream;
 	struct video_device *vbi_dev;
 	struct video_device *radio_dev;
 
@@ -695,6 +671,7 @@ int em28xx_init_isoc(struct em28xx *dev, enum em28xx_mode mode,
 		     int max_packets, int num_bufs, int max_pkt_size,
 		     int (*isoc_copy) (struct em28xx *dev, struct urb *urb));
 void em28xx_uninit_isoc(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);
@@ -710,45 +687,12 @@ void em28xx_release_analog_resources(struct em28xx *dev);
 
 /* Provided by em28xx-cards.c */
 extern int em2800_variant_detect(struct usb_device *udev, int model);
-extern void em28xx_pre_card_setup(struct em28xx *dev);
-extern void em28xx_card_setup(struct em28xx *dev);
 extern struct em28xx_board em28xx_boards[];
 extern struct usb_device_id em28xx_id_table[];
 extern const unsigned int em28xx_bcount;
-void em28xx_register_i2c_ir(struct em28xx *dev);
 int em28xx_tuner_callback(void *ptr, int component, int command, int arg);
 void em28xx_release_resources(struct em28xx *dev);
 
-/* Provided by em28xx-input.c */
-
-#ifdef CONFIG_VIDEO_EM28XX_RC
-
-int em28xx_get_key_terratec(struct IR_i2c *ir, u32 *ir_key, u32 *ir_raw);
-int em28xx_get_key_em_haup(struct IR_i2c *ir, u32 *ir_key, u32 *ir_raw);
-int em28xx_get_key_pinnacle_usb_grey(struct IR_i2c *ir, u32 *ir_key,
-				     u32 *ir_raw);
-int em28xx_get_key_winfast_usbii_deluxe(struct IR_i2c *ir, u32 *ir_key,
-				     u32 *ir_raw);
-void em28xx_register_snapshot_button(struct em28xx *dev);
-void em28xx_deregister_snapshot_button(struct em28xx *dev);
-
-int em28xx_ir_init(struct em28xx *dev);
-int em28xx_ir_fini(struct em28xx *dev);
-
-#else
-
-#define em28xx_get_key_terratec			NULL
-#define em28xx_get_key_em_haup			NULL
-#define em28xx_get_key_pinnacle_usb_grey	NULL
-#define em28xx_get_key_winfast_usbii_deluxe	NULL
-
-static inline void em28xx_register_snapshot_button(struct em28xx *dev) {}
-static inline void em28xx_deregister_snapshot_button(struct em28xx *dev) {}
-static inline int em28xx_ir_init(struct em28xx *dev) { return 0; }
-static inline int em28xx_ir_fini(struct em28xx *dev) { return 0; }
-
-#endif
-
 /* Provided by em28xx-vbi.c */
 extern struct videobuf_queue_ops em28xx_vbi_qops;
 
diff --git a/drivers/media/video/et61x251/Kconfig b/drivers/media/video/et61x251/Kconfig
deleted file mode 100644
index 87981b078fe..00000000000
--- a/drivers/media/video/et61x251/Kconfig
+++ /dev/null
@@ -1,18 +0,0 @@
-config USB_ET61X251
-	tristate "USB ET61X[12]51 PC Camera Controller support (DEPRECATED)"
-	depends on VIDEO_V4L2
-	default n
-	---help---
-	  This driver is DEPRECATED please use the gspca zc3xx module
-	  instead.
-
-	  Say Y here if you want support for cameras based on Etoms ET61X151
-	  or ET61X251 PC Camera Controllers.
-
-	  See <file:Documentation/video4linux/et61x251.txt> for more info.
-
-	  This driver uses the Video For Linux API. You must say Y or M to
-	  "Video For Linux" to use this driver.
-
-	  To compile this driver as a module, choose M here: the
-	  module will be called et61x251.
diff --git a/drivers/media/video/et61x251/Makefile b/drivers/media/video/et61x251/Makefile
deleted file mode 100644
index 2ff4db9ec88..00000000000
--- a/drivers/media/video/et61x251/Makefile
+++ /dev/null
@@ -1,4 +0,0 @@
-et61x251-objs   := et61x251_core.o et61x251_tas5130d1b.o
-
-obj-$(CONFIG_USB_ET61X251)      += et61x251.o
-
diff --git a/drivers/media/video/et61x251/et61x251.h b/drivers/media/video/et61x251/et61x251.h
deleted file mode 100644
index 337ded4a638..00000000000
--- a/drivers/media/video/et61x251/et61x251.h
+++ /dev/null
@@ -1,213 +0,0 @@
-/***************************************************************************
- * V4L2 driver for ET61X[12]51 PC Camera Controllers                       *
- *                                                                         *
- * Copyright (C) 2006 by Luca Risolia <luca.risolia@studio.unibo.it>       *
- *                                                                         *
- * 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 _ET61X251_H_
-#define _ET61X251_H_
-
-#include <linux/usb.h>
-#include <linux/videodev2.h>
-#include <media/v4l2-common.h>
-#include <linux/device.h>
-#include <linux/list.h>
-#include <linux/spinlock.h>
-#include <linux/time.h>
-#include <linux/wait.h>
-#include <linux/types.h>
-#include <linux/param.h>
-#include <linux/rwsem.h>
-#include <linux/mutex.h>
-#include <linux/stddef.h>
-#include <linux/string.h>
-#include <linux/kref.h>
-
-#include "et61x251_sensor.h"
-
-/*****************************************************************************/
-
-#define ET61X251_DEBUG
-#define ET61X251_DEBUG_LEVEL         2
-#define ET61X251_MAX_DEVICES         64
-#define ET61X251_PRESERVE_IMGSCALE   0
-#define ET61X251_FORCE_MUNMAP        0
-#define ET61X251_MAX_FRAMES          32
-#define ET61X251_COMPRESSION_QUALITY 0
-#define ET61X251_URBS                2
-#define ET61X251_ISO_PACKETS         7
-#define ET61X251_ALTERNATE_SETTING   13
-#define ET61X251_URB_TIMEOUT         msecs_to_jiffies(2 * ET61X251_ISO_PACKETS)
-#define ET61X251_CTRL_TIMEOUT        100
-#define ET61X251_FRAME_TIMEOUT       2
-
-/*****************************************************************************/
-
-static const struct usb_device_id et61x251_id_table[] = {
-	{ USB_DEVICE(0x102c, 0x6251), },
-	{ }
-};
-
-ET61X251_SENSOR_TABLE
-
-/*****************************************************************************/
-
-enum et61x251_frame_state {
-	F_UNUSED,
-	F_QUEUED,
-	F_GRABBING,
-	F_DONE,
-	F_ERROR,
-};
-
-struct et61x251_frame_t {
-	void* bufmem;
-	struct v4l2_buffer buf;
-	enum et61x251_frame_state state;
-	struct list_head frame;
-	unsigned long vma_use_count;
-};
-
-enum et61x251_dev_state {
-	DEV_INITIALIZED = 0x01,
-	DEV_DISCONNECTED = 0x02,
-	DEV_MISCONFIGURED = 0x04,
-};
-
-enum et61x251_io_method {
-	IO_NONE,
-	IO_READ,
-	IO_MMAP,
-};
-
-enum et61x251_stream_state {
-	STREAM_OFF,
-	STREAM_INTERRUPT,
-	STREAM_ON,
-};
-
-struct et61x251_sysfs_attr {
-	u8 reg, i2c_reg;
-};
-
-struct et61x251_module_param {
-	u8 force_munmap;
-	u16 frame_timeout;
-};
-
-static DEFINE_MUTEX(et61x251_sysfs_lock);
-static DECLARE_RWSEM(et61x251_dev_lock);
-
-struct et61x251_device {
-	struct video_device* v4ldev;
-
-	struct et61x251_sensor sensor;
-
-	struct usb_device* usbdev;
-	struct urb* urb[ET61X251_URBS];
-	void* transfer_buffer[ET61X251_URBS];
-	u8* control_buffer;
-
-	struct et61x251_frame_t *frame_current, frame[ET61X251_MAX_FRAMES];
-	struct list_head inqueue, outqueue;
-	u32 frame_count, nbuffers, nreadbuffers;
-
-	enum et61x251_io_method io;
-	enum et61x251_stream_state stream;
-
-	struct v4l2_jpegcompression compression;
-
-	struct et61x251_sysfs_attr sysfs;
-	struct et61x251_module_param module_param;
-
-	struct kref kref;
-	enum et61x251_dev_state state;
-	u8 users;
-
-	struct completion probe;
-	struct mutex open_mutex, fileop_mutex;
-	spinlock_t queue_lock;
-	wait_queue_head_t wait_open, wait_frame, wait_stream;
-};
-
-/*****************************************************************************/
-
-struct et61x251_device*
-et61x251_match_id(struct et61x251_device* cam, const struct usb_device_id *id)
-{
-	return usb_match_id(usb_ifnum_to_if(cam->usbdev, 0), id) ? cam : NULL;
-}
-
-
-void
-et61x251_attach_sensor(struct et61x251_device* cam,
-		       const struct et61x251_sensor* sensor)
-{
-	memcpy(&cam->sensor, sensor, sizeof(struct et61x251_sensor));
-}
-
-/*****************************************************************************/
-
-#undef DBG
-#undef KDBG
-#ifdef ET61X251_DEBUG
-#define DBG(level, fmt, ...)						\
-do {									\
-	if (debug >= (level)) {						\
-		if ((level) == 1)					\
-			dev_err(&cam->usbdev->dev, fmt "\n",		\
-				##__VA_ARGS__);				\
-		else if ((level) == 2)					\
-			dev_info(&cam->usbdev->dev, fmt "\n",		\
-				 ##__VA_ARGS__);			\
-		else if ((level) >= 3)					\
-			dev_info(&cam->usbdev->dev, "[%s:%s:%d] " fmt "\n", \
-				 __FILE__, __func__, __LINE__,		\
-				 ##__VA_ARGS__);			\
-	}								\
-} while (0)
-#define KDBG(level, fmt, ...)						\
-do {									\
-	if (debug >= (level)) {						\
-		if ((level) == 1 || (level) == 2)			\
-			pr_info(fmt "\n", ##__VA_ARGS__);		\
-		else if ((level) == 3)					\
-			pr_debug("[%s:%s:%d] " fmt "\n",		\
-				 __FILE__,  __func__, __LINE__,		\
-				 ##__VA_ARGS__);			\
-	}								\
-} while (0)
-#define V4LDBG(level, name, cmd)					\
-do {									\
-	if (debug >= (level))						\
-		v4l_print_ioctl(name, cmd);				\
-} while (0)
-#else
-#define DBG(level, fmt, ...) do {;} while(0)
-#define KDBG(level, fmt, ...) do {;} while(0)
-#define V4LDBG(level, name, cmd) do {;} while(0)
-#endif
-
-#undef PDBG
-#define PDBG(fmt, ...)							\
-	dev_info(&cam->usbdev->dev, "[%s:%s:%d] " fmt "\n",		\
-		 __FILE__, __func__, __LINE__, ##__VA_ARGS__)
-
-#undef PDBGG
-#define PDBGG(fmt, args...) do {;} while (0) /* placeholder */
-
-#endif /* _ET61X251_H_ */
diff --git a/drivers/media/video/et61x251/et61x251_core.c b/drivers/media/video/et61x251/et61x251_core.c
deleted file mode 100644
index 5539f09440a..00000000000
--- a/drivers/media/video/et61x251/et61x251_core.c
+++ /dev/null
@@ -1,2683 +0,0 @@
-/***************************************************************************
- * V4L2 driver for ET61X[12]51 PC Camera Controllers                       *
- *                                                                         *
- * Copyright (C) 2006-2007 by Luca Risolia <luca.risolia@studio.unibo.it>  *
- *                                                                         *
- * 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 <linux/version.h>
-#include <linux/module.h>
-#include <linux/init.h>
-#include <linux/kernel.h>
-#include <linux/param.h>
-#include <linux/errno.h>
-#include <linux/slab.h>
-#include <linux/device.h>
-#include <linux/fs.h>
-#include <linux/delay.h>
-#include <linux/compiler.h>
-#include <linux/ioctl.h>
-#include <linux/poll.h>
-#include <linux/stat.h>
-#include <linux/mm.h>
-#include <linux/vmalloc.h>
-#include <linux/page-flags.h>
-#include <media/v4l2-ioctl.h>
-#include <asm/byteorder.h>
-#include <asm/page.h>
-#include <asm/uaccess.h>
-
-#include "et61x251.h"
-
-/*****************************************************************************/
-
-#define ET61X251_MODULE_NAME    "V4L2 driver for ET61X[12]51 "                \
-				"PC Camera Controllers"
-#define ET61X251_MODULE_AUTHOR  "(C) 2006-2007 Luca Risolia"
-#define ET61X251_AUTHOR_EMAIL   "<luca.risolia@studio.unibo.it>"
-#define ET61X251_MODULE_LICENSE "GPL"
-#define ET61X251_MODULE_VERSION "1.1.10"
-
-/*****************************************************************************/
-
-MODULE_DEVICE_TABLE(usb, et61x251_id_table);
-
-MODULE_AUTHOR(ET61X251_MODULE_AUTHOR " " ET61X251_AUTHOR_EMAIL);
-MODULE_DESCRIPTION(ET61X251_MODULE_NAME);
-MODULE_VERSION(ET61X251_MODULE_VERSION);
-MODULE_LICENSE(ET61X251_MODULE_LICENSE);
-
-static short video_nr[] = {[0 ... ET61X251_MAX_DEVICES-1] = -1};
-module_param_array(video_nr, short, NULL, 0444);
-MODULE_PARM_DESC(video_nr,
-		 "\n<-1|n[,...]> Specify V4L2 minor mode number."
-		 "\n -1 = use next available (default)"
-		 "\n  n = use minor number n (integer >= 0)"
-		 "\nYou can specify up to "
-		 __MODULE_STRING(ET61X251_MAX_DEVICES) " cameras this way."
-		 "\nFor example:"
-		 "\nvideo_nr=-1,2,-1 would assign minor number 2 to"
-		 "\nthe second registered camera and use auto for the first"
-		 "\none and for every other camera."
-		 "\n");
-
-static bool force_munmap[] = {[0 ... ET61X251_MAX_DEVICES-1] =
-			      ET61X251_FORCE_MUNMAP};
-module_param_array(force_munmap, bool, NULL, 0444);
-MODULE_PARM_DESC(force_munmap,
-		 "\n<0|1[,...]> Force the application to unmap previously"
-		 "\nmapped buffer memory before calling any VIDIOC_S_CROP or"
-		 "\nVIDIOC_S_FMT ioctl's. Not all the applications support"
-		 "\nthis feature. This parameter is specific for each"
-		 "\ndetected camera."
-		 "\n 0 = do not force memory unmapping"
-		 "\n 1 = force memory unmapping (save memory)"
-		 "\nDefault value is "__MODULE_STRING(ET61X251_FORCE_MUNMAP)"."
-		 "\n");
-
-static unsigned int frame_timeout[] = {[0 ... ET61X251_MAX_DEVICES-1] =
-				       ET61X251_FRAME_TIMEOUT};
-module_param_array(frame_timeout, uint, NULL, 0644);
-MODULE_PARM_DESC(frame_timeout,
-		 "\n<n[,...]> Timeout for a video frame in seconds."
-		 "\nThis parameter is specific for each detected camera."
-		 "\nDefault value is "
-		 __MODULE_STRING(ET61X251_FRAME_TIMEOUT)"."
-		 "\n");
-
-#ifdef ET61X251_DEBUG
-static unsigned short debug = ET61X251_DEBUG_LEVEL;
-module_param(debug, ushort, 0644);
-MODULE_PARM_DESC(debug,
-		 "\n<n> Debugging information level, from 0 to 3:"
-		 "\n0 = none (use carefully)"
-		 "\n1 = critical errors"
-		 "\n2 = significant informations"
-		 "\n3 = more verbose messages"
-		 "\nLevel 3 is useful for testing only, when only "
-		 "one device is used."
-		 "\nDefault value is "__MODULE_STRING(ET61X251_DEBUG_LEVEL)"."
-		 "\n");
-#endif
-
-/*****************************************************************************/
-
-static u32
-et61x251_request_buffers(struct et61x251_device* cam, u32 count,
-			 enum et61x251_io_method io)
-{
-	struct v4l2_pix_format* p = &(cam->sensor.pix_format);
-	struct v4l2_rect* r = &(cam->sensor.cropcap.bounds);
-	const size_t imagesize = cam->module_param.force_munmap ||
-				 io == IO_READ ?
-				 (p->width * p->height * p->priv) / 8 :
-				 (r->width * r->height * p->priv) / 8;
-	void* buff = NULL;
-	u32 i;
-
-	if (count > ET61X251_MAX_FRAMES)
-		count = ET61X251_MAX_FRAMES;
-
-	cam->nbuffers = count;
-	while (cam->nbuffers > 0) {
-		if ((buff = vmalloc_32_user(cam->nbuffers *
-					    PAGE_ALIGN(imagesize))))
-			break;
-		cam->nbuffers--;
-	}
-
-	for (i = 0; i < cam->nbuffers; i++) {
-		cam->frame[i].bufmem = buff + i*PAGE_ALIGN(imagesize);
-		cam->frame[i].buf.index = i;
-		cam->frame[i].buf.m.offset = i*PAGE_ALIGN(imagesize);
-		cam->frame[i].buf.length = imagesize;
-		cam->frame[i].buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
-		cam->frame[i].buf.sequence = 0;
-		cam->frame[i].buf.field = V4L2_FIELD_NONE;
-		cam->frame[i].buf.memory = V4L2_MEMORY_MMAP;
-		cam->frame[i].buf.flags = 0;
-	}
-
-	return cam->nbuffers;
-}
-
-
-static void et61x251_release_buffers(struct et61x251_device* cam)
-{
-	if (cam->nbuffers) {
-		vfree(cam->frame[0].bufmem);
-		cam->nbuffers = 0;
-	}
-	cam->frame_current = NULL;
-}
-
-
-static void et61x251_empty_framequeues(struct et61x251_device* cam)
-{
-	u32 i;
-
-	INIT_LIST_HEAD(&cam->inqueue);
-	INIT_LIST_HEAD(&cam->outqueue);
-
-	for (i = 0; i < ET61X251_MAX_FRAMES; i++) {
-		cam->frame[i].state = F_UNUSED;
-		cam->frame[i].buf.bytesused = 0;
-	}
-}
-
-
-static void et61x251_requeue_outqueue(struct et61x251_device* cam)
-{
-	struct et61x251_frame_t *i;
-
-	list_for_each_entry(i, &cam->outqueue, frame) {
-		i->state = F_QUEUED;
-		list_add(&i->frame, &cam->inqueue);
-	}
-
-	INIT_LIST_HEAD(&cam->outqueue);
-}
-
-
-static void et61x251_queue_unusedframes(struct et61x251_device* cam)
-{
-	unsigned long lock_flags;
-	u32 i;
-
-	for (i = 0; i < cam->nbuffers; i++)
-		if (cam->frame[i].state == F_UNUSED) {
-			cam->frame[i].state = F_QUEUED;
-			spin_lock_irqsave(&cam->queue_lock, lock_flags);
-			list_add_tail(&cam->frame[i].frame, &cam->inqueue);
-			spin_unlock_irqrestore(&cam->queue_lock, lock_flags);
-		}
-}
-
-/*****************************************************************************/
-
-int et61x251_write_reg(struct et61x251_device* cam, u8 value, u16 index)
-{
-	struct usb_device* udev = cam->usbdev;
-	u8* buff = cam->control_buffer;
-	int res;
-
-	*buff = value;
-
-	res = usb_control_msg(udev, usb_sndctrlpipe(udev, 0), 0x00, 0x41,
-			      0, index, buff, 1, ET61X251_CTRL_TIMEOUT);
-	if (res < 0) {
-		DBG(3, "Failed to write a register (value 0x%02X, index "
-		       "0x%02X, error %d)", value, index, res);
-		return -1;
-	}
-
-	return 0;
-}
-
-
-static int et61x251_read_reg(struct et61x251_device* cam, u16 index)
-{
-	struct usb_device* udev = cam->usbdev;
-	u8* buff = cam->control_buffer;
-	int res;
-
-	res = usb_control_msg(udev, usb_rcvctrlpipe(udev, 0), 0x00, 0xc1,
-			      0, index, buff, 1, ET61X251_CTRL_TIMEOUT);
-	if (res < 0)
-		DBG(3, "Failed to read a register (index 0x%02X, error %d)",
-		    index, res);
-
-	return (res >= 0) ? (int)(*buff) : -1;
-}
-
-
-static int
-et61x251_i2c_wait(struct et61x251_device* cam,
-		  const struct et61x251_sensor* sensor)
-{
-	int i, r;
-
-	for (i = 1; i <= 8; i++) {
-		if (sensor->interface == ET61X251_I2C_3WIRES) {
-			r = et61x251_read_reg(cam, 0x8e);
-			if (!(r & 0x02) && (r >= 0))
-				return 0;
-		} else {
-			r = et61x251_read_reg(cam, 0x8b);
-			if (!(r & 0x01) && (r >= 0))
-				return 0;
-		}
-		if (r < 0)
-			return -EIO;
-		udelay(8*8); /* minimum for sensors at 400kHz */
-	}
-
-	return -EBUSY;
-}
-
-
-int
-et61x251_i2c_raw_write(struct et61x251_device* cam, u8 n, u8 data1, u8 data2,
-		       u8 data3, u8 data4, u8 data5, u8 data6, u8 data7,
-		       u8 data8, u8 address)
-{
-	struct usb_device* udev = cam->usbdev;
-	u8* data = cam->control_buffer;
-	int err = 0, res;
-
-	data[0] = data2;
-	data[1] = data3;
-	data[2] = data4;
-	data[3] = data5;
-	data[4] = data6;
-	data[5] = data7;
-	data[6] = data8;
-	res = usb_control_msg(udev, usb_sndctrlpipe(udev, 0), 0x00, 0x41,
-			      0, 0x81, data, n-1, ET61X251_CTRL_TIMEOUT);
-	if (res < 0)
-		err += res;
-
-	data[0] = address;
-	data[1] = cam->sensor.i2c_slave_id;
-	data[2] = cam->sensor.rsta | 0x02 | (n << 4);
-	res = usb_control_msg(udev, usb_sndctrlpipe(udev, 0), 0x00, 0x41,
-			      0, 0x88, data, 3, ET61X251_CTRL_TIMEOUT);
-	if (res < 0)
-		err += res;
-
-	/* Start writing through the serial interface */
-	data[0] = data1;
-	res = usb_control_msg(udev, usb_sndctrlpipe(udev, 0), 0x00, 0x41,
-			      0, 0x80, data, 1, ET61X251_CTRL_TIMEOUT);
-	if (res < 0)
-		err += res;
-
-	err += et61x251_i2c_wait(cam, &cam->sensor);
-
-	if (err)
-		DBG(3, "I2C raw write failed for %s image sensor",
-		    cam->sensor.name);
-
-	PDBGG("I2C raw write: %u bytes, address = 0x%02X, data1 = 0x%02X, "
-	      "data2 = 0x%02X, data3 = 0x%02X, data4 = 0x%02X, data5 = 0x%02X,"
-	      " data6 = 0x%02X, data7 = 0x%02X, data8 = 0x%02X", n, address,
-	      data1, data2, data3, data4, data5, data6, data7, data8);
-
-	return err ? -1 : 0;
-
-}
-
-
-/*****************************************************************************/
-
-static void et61x251_urb_complete(struct urb *urb)
-{
-	struct et61x251_device* cam = urb->context;
-	struct et61x251_frame_t** f;
-	size_t imagesize;
-	u8 i;
-	int err = 0;
-
-	if (urb->status == -ENOENT)
-		return;
-
-	f = &cam->frame_current;
-
-	if (cam->stream == STREAM_INTERRUPT) {
-		cam->stream = STREAM_OFF;
-		if ((*f))
-			(*f)->state = F_QUEUED;
-		DBG(3, "Stream interrupted");
-		wake_up(&cam->wait_stream);
-	}
-
-	if (cam->state & DEV_DISCONNECTED)
-		return;
-
-	if (cam->state & DEV_MISCONFIGURED) {
-		wake_up_interruptible(&cam->wait_frame);
-		return;
-	}
-
-	if (cam->stream == STREAM_OFF || list_empty(&cam->inqueue))
-		goto resubmit_urb;
-
-	if (!(*f))
-		(*f) = list_entry(cam->inqueue.next, struct et61x251_frame_t,
-				  frame);
-
-	imagesize = (cam->sensor.pix_format.width *
-		     cam->sensor.pix_format.height *
-		     cam->sensor.pix_format.priv) / 8;
-
-	for (i = 0; i < urb->number_of_packets; i++) {
-		unsigned int len, status;
-		void *pos;
-		u8* b1, * b2, sof;
-		const u8 VOID_BYTES = 6;
-		size_t imglen;
-
-		len = urb->iso_frame_desc[i].actual_length;
-		status = urb->iso_frame_desc[i].status;
-		pos = urb->iso_frame_desc[i].offset + urb->transfer_buffer;
-
-		if (status) {
-			DBG(3, "Error in isochronous frame");
-			(*f)->state = F_ERROR;
-			continue;
-		}
-
-		b1 = pos++;
-		b2 = pos++;
-		sof = ((*b1 & 0x3f) == 63);
-		imglen = ((*b1 & 0xc0) << 2) | *b2;
-
-		PDBGG("Isochrnous frame: length %u, #%u i, image length %zu",
-		      len, i, imglen);
-
-		if ((*f)->state == F_QUEUED || (*f)->state == F_ERROR)
-start_of_frame:
-			if (sof) {
-				(*f)->state = F_GRABBING;
-				(*f)->buf.bytesused = 0;
-				do_gettimeofday(&(*f)->buf.timestamp);
-				pos += 22;
-				DBG(3, "SOF detected: new video frame");
-			}
-
-		if ((*f)->state == F_GRABBING) {
-			if (sof && (*f)->buf.bytesused) {
-				if (cam->sensor.pix_format.pixelformat ==
-							 V4L2_PIX_FMT_ET61X251)
-					goto end_of_frame;
-				else {
-					DBG(3, "Not expected SOF detected "
-					       "after %lu bytes",
-					   (unsigned long)(*f)->buf.bytesused);
-					(*f)->state = F_ERROR;
-					continue;
-				}
-			}
-
-			if ((*f)->buf.bytesused + imglen > imagesize) {
-				DBG(3, "Video frame size exceeded");
-				(*f)->state = F_ERROR;
-				continue;
-			}
-
-			pos += VOID_BYTES;
-
-			memcpy((*f)->bufmem+(*f)->buf.bytesused, pos, imglen);
-			(*f)->buf.bytesused += imglen;
-
-			if ((*f)->buf.bytesused == imagesize) {
-				u32 b;
-end_of_frame:
-				b = (*f)->buf.bytesused;
-				(*f)->state = F_DONE;
-				(*f)->buf.sequence= ++cam->frame_count;
-				spin_lock(&cam->queue_lock);
-				list_move_tail(&(*f)->frame, &cam->outqueue);
-				if (!list_empty(&cam->inqueue))
-					(*f) = list_entry(cam->inqueue.next,
-						       struct et61x251_frame_t,
-							  frame);
-				else
-					(*f) = NULL;
-				spin_unlock(&cam->queue_lock);
-				DBG(3, "Video frame captured: : %lu bytes",
-				       (unsigned long)(b));
-
-				if (!(*f))
-					goto resubmit_urb;
-
-				if (sof &&
-				    cam->sensor.pix_format.pixelformat ==
-							 V4L2_PIX_FMT_ET61X251)
-					goto start_of_frame;
-			}
-		}
-	}
-
-resubmit_urb:
-	urb->dev = cam->usbdev;
-	err = usb_submit_urb(urb, GFP_ATOMIC);
-	if (err < 0 && err != -EPERM) {
-		cam->state |= DEV_MISCONFIGURED;
-		DBG(1, "usb_submit_urb() failed");
-	}
-
-	wake_up_interruptible(&cam->wait_frame);
-}
-
-
-static int et61x251_start_transfer(struct et61x251_device* cam)
-{
-	struct usb_device *udev = cam->usbdev;
-	struct urb* urb;
-	struct usb_host_interface* altsetting = usb_altnum_to_altsetting(
-						   usb_ifnum_to_if(udev, 0),
-						   ET61X251_ALTERNATE_SETTING);
-	const unsigned int psz = le16_to_cpu(altsetting->
-					     endpoint[0].desc.wMaxPacketSize);
-	s8 i, j;
-	int err = 0;
-
-	for (i = 0; i < ET61X251_URBS; i++) {
-		cam->transfer_buffer[i] = kzalloc(ET61X251_ISO_PACKETS * psz,
-						  GFP_KERNEL);
-		if (!cam->transfer_buffer[i]) {
-			err = -ENOMEM;
-			DBG(1, "Not enough memory");
-			goto free_buffers;
-		}
-	}
-
-	for (i = 0; i < ET61X251_URBS; i++) {
-		urb = usb_alloc_urb(ET61X251_ISO_PACKETS, GFP_KERNEL);
-		cam->urb[i] = urb;
-		if (!urb) {
-			err = -ENOMEM;
-			DBG(1, "usb_alloc_urb() failed");
-			goto free_urbs;
-		}
-		urb->dev = udev;
-		urb->context = cam;
-		urb->pipe = usb_rcvisocpipe(udev, 1);
-		urb->transfer_flags = URB_ISO_ASAP;
-		urb->number_of_packets = ET61X251_ISO_PACKETS;
-		urb->complete = et61x251_urb_complete;
-		urb->transfer_buffer = cam->transfer_buffer[i];
-		urb->transfer_buffer_length = psz * ET61X251_ISO_PACKETS;
-		urb->interval = 1;
-		for (j = 0; j < ET61X251_ISO_PACKETS; j++) {
-			urb->iso_frame_desc[j].offset = psz * j;
-			urb->iso_frame_desc[j].length = psz;
-		}
-	}
-
-	err = et61x251_write_reg(cam, 0x01, 0x03);
-	err = et61x251_write_reg(cam, 0x00, 0x03);
-	err = et61x251_write_reg(cam, 0x08, 0x03);
-	if (err) {
-		err = -EIO;
-		DBG(1, "I/O hardware error");
-		goto free_urbs;
-	}
-
-	err = usb_set_interface(udev, 0, ET61X251_ALTERNATE_SETTING);
-	if (err) {
-		DBG(1, "usb_set_interface() failed");
-		goto free_urbs;
-	}
-
-	cam->frame_current = NULL;
-
-	for (i = 0; i < ET61X251_URBS; i++) {
-		err = usb_submit_urb(cam->urb[i], GFP_KERNEL);
-		if (err) {
-			for (j = i-1; j >= 0; j--)
-				usb_kill_urb(cam->urb[j]);
-			DBG(1, "usb_submit_urb() failed, error %d", err);
-			goto free_urbs;
-		}
-	}
-
-	return 0;
-
-free_urbs:
-	for (i = 0; (i < ET61X251_URBS) && cam->urb[i]; i++)
-		usb_free_urb(cam->urb[i]);
-
-free_buffers:
-	for (i = 0; (i < ET61X251_URBS) && cam->transfer_buffer[i]; i++)
-		kfree(cam->transfer_buffer[i]);
-
-	return err;
-}
-
-
-static int et61x251_stop_transfer(struct et61x251_device* cam)
-{
-	struct usb_device *udev = cam->usbdev;
-	s8 i;
-	int err = 0;
-
-	if (cam->state & DEV_DISCONNECTED)
-		return 0;
-
-	for (i = ET61X251_URBS-1; i >= 0; i--) {
-		usb_kill_urb(cam->urb[i]);
-		usb_free_urb(cam->urb[i]);
-		kfree(cam->transfer_buffer[i]);
-	}
-
-	err = usb_set_interface(udev, 0, 0); /* 0 Mb/s */
-	if (err)
-		DBG(3, "usb_set_interface() failed");
-
-	return err;
-}
-
-
-static int et61x251_stream_interrupt(struct et61x251_device* cam)
-{
-	long timeout;
-
-	cam->stream = STREAM_INTERRUPT;
-	timeout = wait_event_timeout(cam->wait_stream,
-				     (cam->stream == STREAM_OFF) ||
-				     (cam->state & DEV_DISCONNECTED),
-				     ET61X251_URB_TIMEOUT);
-	if (cam->state & DEV_DISCONNECTED)
-		return -ENODEV;
-	else if (cam->stream != STREAM_OFF) {
-		cam->state |= DEV_MISCONFIGURED;
-		DBG(1, "URB timeout reached. The camera is misconfigured. To "
-		       "use it, close and open %s again.",
-		    video_device_node_name(cam->v4ldev));
-		return -EIO;
-	}
-
-	return 0;
-}
-
-/*****************************************************************************/
-
-#ifdef CONFIG_VIDEO_ADV_DEBUG
-
-static int et61x251_i2c_try_read(struct et61x251_device* cam,
-				 const struct et61x251_sensor* sensor,
-				 u8 address)
-{
-	struct usb_device* udev = cam->usbdev;
-	u8* data = cam->control_buffer;
-	int err = 0, res;
-
-	data[0] = address;
-	data[1] = cam->sensor.i2c_slave_id;
-	data[2] = cam->sensor.rsta | 0x10;
-	data[3] = !(et61x251_read_reg(cam, 0x8b) & 0x02);
-	res = usb_control_msg(udev, usb_sndctrlpipe(udev, 0), 0x00, 0x41,
-			      0, 0x88, data, 4, ET61X251_CTRL_TIMEOUT);
-	if (res < 0)
-		err += res;
-
-	err += et61x251_i2c_wait(cam, sensor);
-
-	res = usb_control_msg(udev, usb_rcvctrlpipe(udev, 0), 0x00, 0xc1,
-			      0, 0x80, data, 8, ET61X251_CTRL_TIMEOUT);
-	if (res < 0)
-		err += res;
-
-	if (err)
-		DBG(3, "I2C read failed for %s image sensor", sensor->name);
-
-	PDBGG("I2C read: address 0x%02X, value: 0x%02X", address, data[0]);
-
-	return err ? -1 : (int)data[0];
-}
-
-
-static int et61x251_i2c_try_write(struct et61x251_device* cam,
-				  const struct et61x251_sensor* sensor,
-				  u8 address, u8 value)
-{
-	struct usb_device* udev = cam->usbdev;
-	u8* data = cam->control_buffer;
-	int err = 0, res;
-
-	data[0] = address;
-	data[1] = cam->sensor.i2c_slave_id;
-	data[2] = cam->sensor.rsta | 0x12;
-	res = usb_control_msg(udev, usb_sndctrlpipe(udev, 0), 0x00, 0x41,
-			      0, 0x88, data, 3, ET61X251_CTRL_TIMEOUT);
-	if (res < 0)
-		err += res;
-
-	data[0] = value;
-	res = usb_control_msg(udev, usb_sndctrlpipe(udev, 0), 0x00, 0x41,
-			      0, 0x80, data, 1, ET61X251_CTRL_TIMEOUT);
-	if (res < 0)
-		err += res;
-
-	err += et61x251_i2c_wait(cam, sensor);
-
-	if (err)
-		DBG(3, "I2C write failed for %s image sensor", sensor->name);
-
-	PDBGG("I2C write: address 0x%02X, value: 0x%02X", address, value);
-
-	return err ? -1 : 0;
-}
-
-static int et61x251_i2c_read(struct et61x251_device* cam, u8 address)
-{
-	return et61x251_i2c_try_read(cam, &cam->sensor, address);
-}
-
-static int et61x251_i2c_write(struct et61x251_device* cam,
-			      u8 address, u8 value)
-{
-	return et61x251_i2c_try_write(cam, &cam->sensor, address, value);
-}
-
-static u8 et61x251_strtou8(const char* buff, size_t len, ssize_t* count)
-{
-	char str[5];
-	char* endp;
-	unsigned long val;
-
-	if (len < 4) {
-		strncpy(str, buff, len);
-		str[len] = '\0';
-	} else {
-		strncpy(str, buff, 4);
-		str[4] = '\0';
-	}
-
-	val = simple_strtoul(str, &endp, 0);
-
-	*count = 0;
-	if (val <= 0xff)
-		*count = (ssize_t)(endp - str);
-	if ((*count) && (len == *count+1) && (buff[*count] == '\n'))
-		*count += 1;
-
-	return (u8)val;
-}
-
-/*
-   NOTE 1: being inside one of the following methods implies that the v4l
-	   device exists for sure (see kobjects and reference counters)
-   NOTE 2: buffers are PAGE_SIZE long
-*/
-
-static ssize_t et61x251_show_reg(struct device* cd,
-				 struct device_attribute *attr, char* buf)
-{
-	struct et61x251_device* cam;
-	ssize_t count;
-
-	if (mutex_lock_interruptible(&et61x251_sysfs_lock))
-		return -ERESTARTSYS;
-
-	cam = video_get_drvdata(to_video_device(cd));
-	if (!cam) {
-		mutex_unlock(&et61x251_sysfs_lock);
-		return -ENODEV;
-	}
-
-	count = sprintf(buf, "%u\n", cam->sysfs.reg);
-
-	mutex_unlock(&et61x251_sysfs_lock);
-
-	return count;
-}
-
-
-static ssize_t
-et61x251_store_reg(struct device* cd,
-		   struct device_attribute *attr, const char* buf, size_t len)
-{
-	struct et61x251_device* cam;
-	u8 index;
-	ssize_t count;
-
-	if (mutex_lock_interruptible(&et61x251_sysfs_lock))
-		return -ERESTARTSYS;
-
-	cam = video_get_drvdata(to_video_device(cd));
-	if (!cam) {
-		mutex_unlock(&et61x251_sysfs_lock);
-		return -ENODEV;
-	}
-
-	index = et61x251_strtou8(buf, len, &count);
-	if (index > 0x8e || !count) {
-		mutex_unlock(&et61x251_sysfs_lock);
-		return -EINVAL;
-	}
-
-	cam->sysfs.reg = index;
-
-	DBG(2, "Moved ET61X[12]51 register index to 0x%02X", cam->sysfs.reg);
-	DBG(3, "Written bytes: %zd", count);
-
-	mutex_unlock(&et61x251_sysfs_lock);
-
-	return count;
-}
-
-
-static ssize_t et61x251_show_val(struct device* cd,
-				 struct device_attribute *attr, char* buf)
-{
-	struct et61x251_device* cam;
-	ssize_t count;
-	int val;
-
-	if (mutex_lock_interruptible(&et61x251_sysfs_lock))
-		return -ERESTARTSYS;
-
-	cam = video_get_drvdata(to_video_device(cd));
-	if (!cam) {
-		mutex_unlock(&et61x251_sysfs_lock);
-		return -ENODEV;
-	}
-
-	if ((val = et61x251_read_reg(cam, cam->sysfs.reg)) < 0) {
-		mutex_unlock(&et61x251_sysfs_lock);
-		return -EIO;
-	}
-
-	count = sprintf(buf, "%d\n", val);
-
-	DBG(3, "Read bytes: %zd", count);
-
-	mutex_unlock(&et61x251_sysfs_lock);
-
-	return count;
-}
-
-
-static ssize_t
-et61x251_store_val(struct device* cd, struct device_attribute *attr,
-		   const char* buf, size_t len)
-{
-	struct et61x251_device* cam;
-	u8 value;
-	ssize_t count;
-	int err;
-
-	if (mutex_lock_interruptible(&et61x251_sysfs_lock))
-		return -ERESTARTSYS;
-
-	cam = video_get_drvdata(to_video_device(cd));
-	if (!cam) {
-		mutex_unlock(&et61x251_sysfs_lock);
-		return -ENODEV;
-	}
-
-	value = et61x251_strtou8(buf, len, &count);
-	if (!count) {
-		mutex_unlock(&et61x251_sysfs_lock);
-		return -EINVAL;
-	}
-
-	err = et61x251_write_reg(cam, value, cam->sysfs.reg);
-	if (err) {
-		mutex_unlock(&et61x251_sysfs_lock);
-		return -EIO;
-	}
-
-	DBG(2, "Written ET61X[12]51 reg. 0x%02X, val. 0x%02X",
-	    cam->sysfs.reg, value);
-	DBG(3, "Written bytes: %zd", count);
-
-	mutex_unlock(&et61x251_sysfs_lock);
-
-	return count;
-}
-
-
-static ssize_t et61x251_show_i2c_reg(struct device* cd,
-				     struct device_attribute *attr, char* buf)
-{
-	struct et61x251_device* cam;
-	ssize_t count;
-
-	if (mutex_lock_interruptible(&et61x251_sysfs_lock))
-		return -ERESTARTSYS;
-
-	cam = video_get_drvdata(to_video_device(cd));
-	if (!cam) {
-		mutex_unlock(&et61x251_sysfs_lock);
-		return -ENODEV;
-	}
-
-	count = sprintf(buf, "%u\n", cam->sysfs.i2c_reg);
-
-	DBG(3, "Read bytes: %zd", count);
-
-	mutex_unlock(&et61x251_sysfs_lock);
-
-	return count;
-}
-
-
-static ssize_t
-et61x251_store_i2c_reg(struct device* cd, struct device_attribute *attr,
-		       const char* buf, size_t len)
-{
-	struct et61x251_device* cam;
-	u8 index;
-	ssize_t count;
-
-	if (mutex_lock_interruptible(&et61x251_sysfs_lock))
-		return -ERESTARTSYS;
-
-	cam = video_get_drvdata(to_video_device(cd));
-	if (!cam) {
-		mutex_unlock(&et61x251_sysfs_lock);
-		return -ENODEV;
-	}
-
-	index = et61x251_strtou8(buf, len, &count);
-	if (!count) {
-		mutex_unlock(&et61x251_sysfs_lock);
-		return -EINVAL;
-	}
-
-	cam->sysfs.i2c_reg = index;
-
-	DBG(2, "Moved sensor register index to 0x%02X", cam->sysfs.i2c_reg);
-	DBG(3, "Written bytes: %zd", count);
-
-	mutex_unlock(&et61x251_sysfs_lock);
-
-	return count;
-}
-
-
-static ssize_t et61x251_show_i2c_val(struct device* cd,
-				     struct device_attribute *attr, char* buf)
-{
-	struct et61x251_device* cam;
-	ssize_t count;
-	int val;
-
-	if (mutex_lock_interruptible(&et61x251_sysfs_lock))
-		return -ERESTARTSYS;
-
-	cam = video_get_drvdata(to_video_device(cd));
-	if (!cam) {
-		mutex_unlock(&et61x251_sysfs_lock);
-		return -ENODEV;
-	}
-
-	if (!(cam->sensor.sysfs_ops & ET61X251_I2C_READ)) {
-		mutex_unlock(&et61x251_sysfs_lock);
-		return -ENOSYS;
-	}
-
-	if ((val = et61x251_i2c_read(cam, cam->sysfs.i2c_reg)) < 0) {
-		mutex_unlock(&et61x251_sysfs_lock);
-		return -EIO;
-	}
-
-	count = sprintf(buf, "%d\n", val);
-
-	DBG(3, "Read bytes: %zd", count);
-
-	mutex_unlock(&et61x251_sysfs_lock);
-
-	return count;
-}
-
-
-static ssize_t
-et61x251_store_i2c_val(struct device* cd, struct device_attribute *attr,
-		       const char* buf, size_t len)
-{
-	struct et61x251_device* cam;
-	u8 value;
-	ssize_t count;
-	int err;
-
-	if (mutex_lock_interruptible(&et61x251_sysfs_lock))
-		return -ERESTARTSYS;
-
-	cam = video_get_drvdata(to_video_device(cd));
-	if (!cam) {
-		mutex_unlock(&et61x251_sysfs_lock);
-		return -ENODEV;
-	}
-
-	if (!(cam->sensor.sysfs_ops & ET61X251_I2C_READ)) {
-		mutex_unlock(&et61x251_sysfs_lock);
-		return -ENOSYS;
-	}
-
-	value = et61x251_strtou8(buf, len, &count);
-	if (!count) {
-		mutex_unlock(&et61x251_sysfs_lock);
-		return -EINVAL;
-	}
-
-	err = et61x251_i2c_write(cam, cam->sysfs.i2c_reg, value);
-	if (err) {
-		mutex_unlock(&et61x251_sysfs_lock);
-		return -EIO;
-	}
-
-	DBG(2, "Written sensor reg. 0x%02X, val. 0x%02X",
-	    cam->sysfs.i2c_reg, value);
-	DBG(3, "Written bytes: %zd", count);
-
-	mutex_unlock(&et61x251_sysfs_lock);
-
-	return count;
-}
-
-
-static DEVICE_ATTR(reg, S_IRUGO | S_IWUSR,
-		   et61x251_show_reg, et61x251_store_reg);
-static DEVICE_ATTR(val, S_IRUGO | S_IWUSR,
-		   et61x251_show_val, et61x251_store_val);
-static DEVICE_ATTR(i2c_reg, S_IRUGO | S_IWUSR,
-		   et61x251_show_i2c_reg, et61x251_store_i2c_reg);
-static DEVICE_ATTR(i2c_val, S_IRUGO | S_IWUSR,
-		   et61x251_show_i2c_val, et61x251_store_i2c_val);
-
-
-static int et61x251_create_sysfs(struct et61x251_device* cam)
-{
-	struct device *classdev = &(cam->v4ldev->dev);
-	int err = 0;
-
-	if ((err = device_create_file(classdev, &dev_attr_reg)))
-		goto err_out;
-	if ((err = device_create_file(classdev, &dev_attr_val)))
-		goto err_reg;
-
-	if (cam->sensor.sysfs_ops) {
-		if ((err = device_create_file(classdev, &dev_attr_i2c_reg)))
-			goto err_val;
-		if ((err = device_create_file(classdev, &dev_attr_i2c_val)))
-			goto err_i2c_reg;
-	}
-
-err_i2c_reg:
-	if (cam->sensor.sysfs_ops)
-		device_remove_file(classdev, &dev_attr_i2c_reg);
-err_val:
-	device_remove_file(classdev, &dev_attr_val);
-err_reg:
-	device_remove_file(classdev, &dev_attr_reg);
-err_out:
-	return err;
-}
-#endif /* CONFIG_VIDEO_ADV_DEBUG */
-
-/*****************************************************************************/
-
-static int
-et61x251_set_pix_format(struct et61x251_device* cam,
-			struct v4l2_pix_format* pix)
-{
-	int r, err = 0;
-
-	if ((r = et61x251_read_reg(cam, 0x12)) < 0)
-		err += r;
-	if (pix->pixelformat == V4L2_PIX_FMT_ET61X251)
-		err += et61x251_write_reg(cam, r & 0xfd, 0x12);
-	else
-		err += et61x251_write_reg(cam, r | 0x02, 0x12);
-
-	return err ? -EIO : 0;
-}
-
-
-static int
-et61x251_set_compression(struct et61x251_device* cam,
-			 struct v4l2_jpegcompression* compression)
-{
-	int r, err = 0;
-
-	if ((r = et61x251_read_reg(cam, 0x12)) < 0)
-		err += r;
-	if (compression->quality == 0)
-		err += et61x251_write_reg(cam, r & 0xfb, 0x12);
-	else
-		err += et61x251_write_reg(cam, r | 0x04, 0x12);
-
-	return err ? -EIO : 0;
-}
-
-
-static int et61x251_set_scale(struct et61x251_device* cam, u8 scale)
-{
-	int r = 0, err = 0;
-
-	r = et61x251_read_reg(cam, 0x12);
-	if (r < 0)
-		err += r;
-
-	if (scale == 1)
-		err += et61x251_write_reg(cam, r & ~0x01, 0x12);
-	else if (scale == 2)
-		err += et61x251_write_reg(cam, r | 0x01, 0x12);
-
-	if (err)
-		return -EIO;
-
-	PDBGG("Scaling factor: %u", scale);
-
-	return 0;
-}
-
-
-static int
-et61x251_set_crop(struct et61x251_device* cam, struct v4l2_rect* rect)
-{
-	struct et61x251_sensor* s = &cam->sensor;
-	u16 fmw_sx = (u16)(rect->left - s->cropcap.bounds.left +
-			   s->active_pixel.left),
-	    fmw_sy = (u16)(rect->top - s->cropcap.bounds.top +
-			   s->active_pixel.top),
-	    fmw_length = (u16)(rect->width),
-	    fmw_height = (u16)(rect->height);
-	int err = 0;
-
-	err += et61x251_write_reg(cam, fmw_sx & 0xff, 0x69);
-	err += et61x251_write_reg(cam, fmw_sy & 0xff, 0x6a);
-	err += et61x251_write_reg(cam, fmw_length & 0xff, 0x6b);
-	err += et61x251_write_reg(cam, fmw_height & 0xff, 0x6c);
-	err += et61x251_write_reg(cam, (fmw_sx >> 8) | ((fmw_sy & 0x300) >> 6)
-				       | ((fmw_length & 0x300) >> 4)
-				       | ((fmw_height & 0x300) >> 2), 0x6d);
-	if (err)
-		return -EIO;
-
-	PDBGG("fmw_sx, fmw_sy, fmw_length, fmw_height: %u %u %u %u",
-	      fmw_sx, fmw_sy, fmw_length, fmw_height);
-
-	return 0;
-}
-
-
-static int et61x251_init(struct et61x251_device* cam)
-{
-	struct et61x251_sensor* s = &cam->sensor;
-	struct v4l2_control ctrl;
-	struct v4l2_queryctrl *qctrl;
-	struct v4l2_rect* rect;
-	u8 i = 0;
-	int err = 0;
-
-	if (!(cam->state & DEV_INITIALIZED)) {
-		mutex_init(&cam->open_mutex);
-		init_waitqueue_head(&cam->wait_open);
-		qctrl = s->qctrl;
-		rect = &(s->cropcap.defrect);
-		cam->compression.quality = ET61X251_COMPRESSION_QUALITY;
-	} else { /* use current values */
-		qctrl = s->_qctrl;
-		rect = &(s->_rect);
-	}
-
-	err += et61x251_set_scale(cam, rect->width / s->pix_format.width);
-	err += et61x251_set_crop(cam, rect);
-	if (err)
-		return err;
-
-	if (s->init) {
-		err = s->init(cam);
-		if (err) {
-			DBG(3, "Sensor initialization failed");
-			return err;
-		}
-	}
-
-	err += et61x251_set_compression(cam, &cam->compression);
-	err += et61x251_set_pix_format(cam, &s->pix_format);
-	if (s->set_pix_format)
-		err += s->set_pix_format(cam, &s->pix_format);
-	if (err)
-		return err;
-
-	if (s->pix_format.pixelformat == V4L2_PIX_FMT_ET61X251)
-		DBG(3, "Compressed video format is active, quality %d",
-		    cam->compression.quality);
-	else
-		DBG(3, "Uncompressed video format is active");
-
-	if (s->set_crop)
-		if ((err = s->set_crop(cam, rect))) {
-			DBG(3, "set_crop() failed");
-			return err;
-		}
-
-	if (s->set_ctrl) {
-		for (i = 0; i < ARRAY_SIZE(s->qctrl); i++)
-			if (s->qctrl[i].id != 0 &&
-			    !(s->qctrl[i].flags & V4L2_CTRL_FLAG_DISABLED)) {
-				ctrl.id = s->qctrl[i].id;
-				ctrl.value = qctrl[i].default_value;
-				err = s->set_ctrl(cam, &ctrl);
-				if (err) {
-					DBG(3, "Set %s control failed",
-					    s->qctrl[i].name);
-					return err;
-				}
-				DBG(3, "Image sensor supports '%s' control",
-				    s->qctrl[i].name);
-			}
-	}
-
-	if (!(cam->state & DEV_INITIALIZED)) {
-		mutex_init(&cam->fileop_mutex);
-		spin_lock_init(&cam->queue_lock);
-		init_waitqueue_head(&cam->wait_frame);
-		init_waitqueue_head(&cam->wait_stream);
-		cam->nreadbuffers = 2;
-		memcpy(s->_qctrl, s->qctrl, sizeof(s->qctrl));
-		memcpy(&(s->_rect), &(s->cropcap.defrect),
-		       sizeof(struct v4l2_rect));
-		cam->state |= DEV_INITIALIZED;
-	}
-
-	DBG(2, "Initialization succeeded");
-	return 0;
-}
-
-/*****************************************************************************/
-
-static void et61x251_release_resources(struct kref *kref)
-{
-	struct et61x251_device *cam;
-
-	mutex_lock(&et61x251_sysfs_lock);
-
-	cam = container_of(kref, struct et61x251_device, kref);
-
-	DBG(2, "V4L2 device %s deregistered",
-	    video_device_node_name(cam->v4ldev));
-	video_set_drvdata(cam->v4ldev, NULL);
-	video_unregister_device(cam->v4ldev);
-	usb_put_dev(cam->usbdev);
-	kfree(cam->control_buffer);
-	kfree(cam);
-
-	mutex_unlock(&et61x251_sysfs_lock);
-}
-
-
-static int et61x251_open(struct file *filp)
-{
-	struct et61x251_device* cam;
-	int err = 0;
-
-	if (!down_read_trylock(&et61x251_dev_lock))
-		return -ERESTARTSYS;
-
-	cam = video_drvdata(filp);
-
-	if (wait_for_completion_interruptible(&cam->probe)) {
-		up_read(&et61x251_dev_lock);
-		return -ERESTARTSYS;
-	}
-
-	kref_get(&cam->kref);
-
-	if (mutex_lock_interruptible(&cam->open_mutex)) {
-		kref_put(&cam->kref, et61x251_release_resources);
-		up_read(&et61x251_dev_lock);
-		return -ERESTARTSYS;
-	}
-
-	if (cam->state & DEV_DISCONNECTED) {
-		DBG(1, "Device not present");
-		err = -ENODEV;
-		goto out;
-	}
-
-	if (cam->users) {
-		DBG(2, "Device %s is already in use",
-		       video_device_node_name(cam->v4ldev));
-		DBG(3, "Simultaneous opens are not supported");
-		if ((filp->f_flags & O_NONBLOCK) ||
-		    (filp->f_flags & O_NDELAY)) {
-			err = -EWOULDBLOCK;
-			goto out;
-		}
-		DBG(2, "A blocking open() has been requested. Wait for the "
-		       "device to be released...");
-		up_read(&et61x251_dev_lock);
-		err = wait_event_interruptible_exclusive(cam->wait_open,
-						(cam->state & DEV_DISCONNECTED)
-							 || !cam->users);
-		down_read(&et61x251_dev_lock);
-		if (err)
-			goto out;
-		if (cam->state & DEV_DISCONNECTED) {
-			err = -ENODEV;
-			goto out;
-		}
-	}
-
-	if (cam->state & DEV_MISCONFIGURED) {
-		err = et61x251_init(cam);
-		if (err) {
-			DBG(1, "Initialization failed again. "
-			       "I will retry on next open().");
-			goto out;
-		}
-		cam->state &= ~DEV_MISCONFIGURED;
-	}
-
-	if ((err = et61x251_start_transfer(cam)))
-		goto out;
-
-	filp->private_data = cam;
-	cam->users++;
-	cam->io = IO_NONE;
-	cam->stream = STREAM_OFF;
-	cam->nbuffers = 0;
-	cam->frame_count = 0;
-	et61x251_empty_framequeues(cam);
-
-	DBG(3, "Video device %s is open",
-	    video_device_node_name(cam->v4ldev));
-
-out:
-	mutex_unlock(&cam->open_mutex);
-	if (err)
-		kref_put(&cam->kref, et61x251_release_resources);
-	up_read(&et61x251_dev_lock);
-	return err;
-}
-
-
-static int et61x251_release(struct file *filp)
-{
-	struct et61x251_device* cam;
-
-	down_write(&et61x251_dev_lock);
-
-	cam = video_drvdata(filp);
-
-	et61x251_stop_transfer(cam);
-	et61x251_release_buffers(cam);
-	cam->users--;
-	wake_up_interruptible_nr(&cam->wait_open, 1);
-
-	DBG(3, "Video device %s closed",
-	    video_device_node_name(cam->v4ldev));
-
-	kref_put(&cam->kref, et61x251_release_resources);
-
-	up_write(&et61x251_dev_lock);
-
-	return 0;
-}
-
-
-static ssize_t
-et61x251_read(struct file* filp, char __user * buf,
-	      size_t count, loff_t* f_pos)
-{
-	struct et61x251_device *cam = video_drvdata(filp);
-	struct et61x251_frame_t* f, * i;
-	unsigned long lock_flags;
-	long timeout;
-	int err = 0;
-
-	if (mutex_lock_interruptible(&cam->fileop_mutex))
-		return -ERESTARTSYS;
-
-	if (cam->state & DEV_DISCONNECTED) {
-		DBG(1, "Device not present");
-		mutex_unlock(&cam->fileop_mutex);
-		return -ENODEV;
-	}
-
-	if (cam->state & DEV_MISCONFIGURED) {
-		DBG(1, "The camera is misconfigured. Close and open it "
-		       "again.");
-		mutex_unlock(&cam->fileop_mutex);
-		return -EIO;
-	}
-
-	if (cam->io == IO_MMAP) {
-		DBG(3, "Close and open the device again to choose the read "
-		       "method");
-		mutex_unlock(&cam->fileop_mutex);
-		return -EBUSY;
-	}
-
-	if (cam->io == IO_NONE) {
-		if (!et61x251_request_buffers(cam, cam->nreadbuffers,
-					      IO_READ)) {
-			DBG(1, "read() failed, not enough memory");
-			mutex_unlock(&cam->fileop_mutex);
-			return -ENOMEM;
-		}
-		cam->io = IO_READ;
-		cam->stream = STREAM_ON;
-	}
-
-	if (list_empty(&cam->inqueue)) {
-		if (!list_empty(&cam->outqueue))
-			et61x251_empty_framequeues(cam);
-		et61x251_queue_unusedframes(cam);
-	}
-
-	if (!count) {
-		mutex_unlock(&cam->fileop_mutex);
-		return 0;
-	}
-
-	if (list_empty(&cam->outqueue)) {
-		if (filp->f_flags & O_NONBLOCK) {
-			mutex_unlock(&cam->fileop_mutex);
-			return -EAGAIN;
-		}
-		timeout = wait_event_interruptible_timeout
-			  ( cam->wait_frame,
-			    (!list_empty(&cam->outqueue)) ||
-			    (cam->state & DEV_DISCONNECTED) ||
-			    (cam->state & DEV_MISCONFIGURED),
-			    msecs_to_jiffies(
-				cam->module_param.frame_timeout * 1000
-			    )
-			  );
-		if (timeout < 0) {
-			mutex_unlock(&cam->fileop_mutex);
-			return timeout;
-		}
-		if (cam->state & DEV_DISCONNECTED) {
-			mutex_unlock(&cam->fileop_mutex);
-			return -ENODEV;
-		}
-		if (!timeout || (cam->state & DEV_MISCONFIGURED)) {
-			mutex_unlock(&cam->fileop_mutex);
-			return -EIO;
-		}
-	}
-
-	f = list_entry(cam->outqueue.prev, struct et61x251_frame_t, frame);
-
-	if (count > f->buf.bytesused)
-		count = f->buf.bytesused;
-
-	if (copy_to_user(buf, f->bufmem, count)) {
-		err = -EFAULT;
-		goto exit;
-	}
-	*f_pos += count;
-
-exit:
-	spin_lock_irqsave(&cam->queue_lock, lock_flags);
-	list_for_each_entry(i, &cam->outqueue, frame)
-		i->state = F_UNUSED;
-	INIT_LIST_HEAD(&cam->outqueue);
-	spin_unlock_irqrestore(&cam->queue_lock, lock_flags);
-
-	et61x251_queue_unusedframes(cam);
-
-	PDBGG("Frame #%lu, bytes read: %zu",
-	      (unsigned long)f->buf.index, count);
-
-	mutex_unlock(&cam->fileop_mutex);
-
-	return err ? err : count;
-}
-
-
-static unsigned int et61x251_poll(struct file *filp, poll_table *wait)
-{
-	struct et61x251_device *cam = video_drvdata(filp);
-	struct et61x251_frame_t* f;
-	unsigned long lock_flags;
-	unsigned int mask = 0;
-
-	if (mutex_lock_interruptible(&cam->fileop_mutex))
-		return POLLERR;
-
-	if (cam->state & DEV_DISCONNECTED) {
-		DBG(1, "Device not present");
-		goto error;
-	}
-
-	if (cam->state & DEV_MISCONFIGURED) {
-		DBG(1, "The camera is misconfigured. Close and open it "
-		       "again.");
-		goto error;
-	}
-
-	if (cam->io == IO_NONE) {
-		if (!et61x251_request_buffers(cam, cam->nreadbuffers,
-					      IO_READ)) {
-			DBG(1, "poll() failed, not enough memory");
-			goto error;
-		}
-		cam->io = IO_READ;
-		cam->stream = STREAM_ON;
-	}
-
-	if (cam->io == IO_READ) {
-		spin_lock_irqsave(&cam->queue_lock, lock_flags);
-		list_for_each_entry(f, &cam->outqueue, frame)
-			f->state = F_UNUSED;
-		INIT_LIST_HEAD(&cam->outqueue);
-		spin_unlock_irqrestore(&cam->queue_lock, lock_flags);
-		et61x251_queue_unusedframes(cam);
-	}
-
-	poll_wait(filp, &cam->wait_frame, wait);
-
-	if (!list_empty(&cam->outqueue))
-		mask |= POLLIN | POLLRDNORM;
-
-	mutex_unlock(&cam->fileop_mutex);
-
-	return mask;
-
-error:
-	mutex_unlock(&cam->fileop_mutex);
-	return POLLERR;
-}
-
-
-static void et61x251_vm_open(struct vm_area_struct* vma)
-{
-	struct et61x251_frame_t* f = vma->vm_private_data;
-	f->vma_use_count++;
-}
-
-
-static void et61x251_vm_close(struct vm_area_struct* vma)
-{
-	/* NOTE: buffers are not freed here */
-	struct et61x251_frame_t* f = vma->vm_private_data;
-	f->vma_use_count--;
-}
-
-
-static const struct vm_operations_struct et61x251_vm_ops = {
-	.open = et61x251_vm_open,
-	.close = et61x251_vm_close,
-};
-
-
-static int et61x251_mmap(struct file* filp, struct vm_area_struct *vma)
-{
-	struct et61x251_device *cam = video_drvdata(filp);
-	unsigned long size = vma->vm_end - vma->vm_start,
-		      start = vma->vm_start;
-	void *pos;
-	u32 i;
-
-	if (mutex_lock_interruptible(&cam->fileop_mutex))
-		return -ERESTARTSYS;
-
-	if (cam->state & DEV_DISCONNECTED) {
-		DBG(1, "Device not present");
-		mutex_unlock(&cam->fileop_mutex);
-		return -ENODEV;
-	}
-
-	if (cam->state & DEV_MISCONFIGURED) {
-		DBG(1, "The camera is misconfigured. Close and open it "
-		       "again.");
-		mutex_unlock(&cam->fileop_mutex);
-		return -EIO;
-	}
-
-	if (!(vma->vm_flags & (VM_WRITE | VM_READ))) {
-		mutex_unlock(&cam->fileop_mutex);
-		return -EACCES;
-	}
-
-	if (cam->io != IO_MMAP ||
-	    size != PAGE_ALIGN(cam->frame[0].buf.length)) {
-		mutex_unlock(&cam->fileop_mutex);
-		return -EINVAL;
-	}
-
-	for (i = 0; i < cam->nbuffers; i++) {
-		if ((cam->frame[i].buf.m.offset>>PAGE_SHIFT) == vma->vm_pgoff)
-			break;
-	}
-	if (i == cam->nbuffers) {
-		mutex_unlock(&cam->fileop_mutex);
-		return -EINVAL;
-	}
-
-	vma->vm_flags |= VM_IO;
-	vma->vm_flags |= VM_RESERVED;
-
-	pos = cam->frame[i].bufmem;
-	while (size > 0) { /* size is page-aligned */
-		if (vm_insert_page(vma, start, vmalloc_to_page(pos))) {
-			mutex_unlock(&cam->fileop_mutex);
-			return -EAGAIN;
-		}
-		start += PAGE_SIZE;
-		pos += PAGE_SIZE;
-		size -= PAGE_SIZE;
-	}
-
-	vma->vm_ops = &et61x251_vm_ops;
-	vma->vm_private_data = &cam->frame[i];
-	et61x251_vm_open(vma);
-
-	mutex_unlock(&cam->fileop_mutex);
-
-	return 0;
-}
-
-/*****************************************************************************/
-
-static int
-et61x251_vidioc_querycap(struct et61x251_device* cam, void __user * arg)
-{
-	struct v4l2_capability cap = {
-		.driver = "et61x251",
-		.version = LINUX_VERSION_CODE,
-		.capabilities = V4L2_CAP_VIDEO_CAPTURE | V4L2_CAP_READWRITE |
-				V4L2_CAP_STREAMING,
-	};
-
-	strlcpy(cap.card, cam->v4ldev->name, sizeof(cap.card));
-	if (usb_make_path(cam->usbdev, cap.bus_info, sizeof(cap.bus_info)) < 0)
-		strlcpy(cap.bus_info, dev_name(&cam->usbdev->dev),
-			sizeof(cap.bus_info));
-
-	if (copy_to_user(arg, &cap, sizeof(cap)))
-		return -EFAULT;
-
-	return 0;
-}
-
-
-static int
-et61x251_vidioc_enuminput(struct et61x251_device* cam, void __user * arg)
-{
-	struct v4l2_input i;
-
-	if (copy_from_user(&i, arg, sizeof(i)))
-		return -EFAULT;
-
-	if (i.index)
-		return -EINVAL;
-
-	memset(&i, 0, sizeof(i));
-	strcpy(i.name, "Camera");
-	i.type = V4L2_INPUT_TYPE_CAMERA;
-	i.capabilities = V4L2_IN_CAP_STD;
-
-	if (copy_to_user(arg, &i, sizeof(i)))
-		return -EFAULT;
-
-	return 0;
-}
-
-
-static int
-et61x251_vidioc_g_input(struct et61x251_device* cam, void __user * arg)
-{
-	int index = 0;
-
-	if (copy_to_user(arg, &index, sizeof(index)))
-		return -EFAULT;
-
-	return 0;
-}
-
-
-static int
-et61x251_vidioc_s_input(struct et61x251_device* cam, void __user * arg)
-{
-	int index;
-
-	if (copy_from_user(&index, arg, sizeof(index)))
-		return -EFAULT;
-
-	if (index != 0)
-		return -EINVAL;
-
-	return 0;
-}
-
-
-static int
-et61x251_vidioc_query_ctrl(struct et61x251_device* cam, void __user * arg)
-{
-	struct et61x251_sensor* s = &cam->sensor;
-	struct v4l2_queryctrl qc;
-	u8 i;
-
-	if (copy_from_user(&qc, arg, sizeof(qc)))
-		return -EFAULT;
-
-	for (i = 0; i < ARRAY_SIZE(s->qctrl); i++)
-		if (qc.id && qc.id == s->qctrl[i].id) {
-			memcpy(&qc, &(s->qctrl[i]), sizeof(qc));
-			if (copy_to_user(arg, &qc, sizeof(qc)))
-				return -EFAULT;
-			return 0;
-		}
-
-	return -EINVAL;
-}
-
-
-static int
-et61x251_vidioc_g_ctrl(struct et61x251_device* cam, void __user * arg)
-{
-	struct et61x251_sensor* s = &cam->sensor;
-	struct v4l2_control ctrl;
-	int err = 0;
-	u8 i;
-
-	if (!s->get_ctrl && !s->set_ctrl)
-		return -EINVAL;
-
-	if (copy_from_user(&ctrl, arg, sizeof(ctrl)))
-		return -EFAULT;
-
-	if (!s->get_ctrl) {
-		for (i = 0; i < ARRAY_SIZE(s->qctrl); i++)
-			if (ctrl.id == s->qctrl[i].id) {
-				ctrl.value = s->_qctrl[i].default_value;
-				goto exit;
-			}
-		return -EINVAL;
-	} else
-		err = s->get_ctrl(cam, &ctrl);
-
-exit:
-	if (copy_to_user(arg, &ctrl, sizeof(ctrl)))
-		return -EFAULT;
-
-	return err;
-}
-
-
-static int
-et61x251_vidioc_s_ctrl(struct et61x251_device* cam, void __user * arg)
-{
-	struct et61x251_sensor* s = &cam->sensor;
-	struct v4l2_control ctrl;
-	u8 i;
-	int err = 0;
-
-	if (!s->set_ctrl)
-		return -EINVAL;
-
-	if (copy_from_user(&ctrl, arg, sizeof(ctrl)))
-		return -EFAULT;
-
-	for (i = 0; i < ARRAY_SIZE(s->qctrl); i++) {
-		if (ctrl.id == s->qctrl[i].id) {
-			if (s->qctrl[i].flags & V4L2_CTRL_FLAG_DISABLED)
-				return -EINVAL;
-			if (ctrl.value < s->qctrl[i].minimum ||
-			    ctrl.value > s->qctrl[i].maximum)
-				return -ERANGE;
-			ctrl.value -= ctrl.value % s->qctrl[i].step;
-			break;
-		}
-	}
-	if (i == ARRAY_SIZE(s->qctrl))
-		return -EINVAL;
-	if ((err = s->set_ctrl(cam, &ctrl)))
-		return err;
-
-	s->_qctrl[i].default_value = ctrl.value;
-
-	return 0;
-}
-
-
-static int
-et61x251_vidioc_cropcap(struct et61x251_device* cam, void __user * arg)
-{
-	struct v4l2_cropcap* cc = &(cam->sensor.cropcap);
-
-	cc->type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
-	cc->pixelaspect.numerator = 1;
-	cc->pixelaspect.denominator = 1;
-
-	if (copy_to_user(arg, cc, sizeof(*cc)))
-		return -EFAULT;
-
-	return 0;
-}
-
-
-static int
-et61x251_vidioc_g_crop(struct et61x251_device* cam, void __user * arg)
-{
-	struct et61x251_sensor* s = &cam->sensor;
-	struct v4l2_crop crop = {
-		.type = V4L2_BUF_TYPE_VIDEO_CAPTURE,
-	};
-
-	memcpy(&(crop.c), &(s->_rect), sizeof(struct v4l2_rect));
-
-	if (copy_to_user(arg, &crop, sizeof(crop)))
-		return -EFAULT;
-
-	return 0;
-}
-
-
-static int
-et61x251_vidioc_s_crop(struct et61x251_device* cam, void __user * arg)
-{
-	struct et61x251_sensor* s = &cam->sensor;
-	struct v4l2_crop crop;
-	struct v4l2_rect* rect;
-	struct v4l2_rect* bounds = &(s->cropcap.bounds);
-	struct v4l2_pix_format* pix_format = &(s->pix_format);
-	u8 scale;
-	const enum et61x251_stream_state stream = cam->stream;
-	const u32 nbuffers = cam->nbuffers;
-	u32 i;
-	int err = 0;
-
-	if (copy_from_user(&crop, arg, sizeof(crop)))
-		return -EFAULT;
-
-	rect = &(crop.c);
-
-	if (crop.type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
-		return -EINVAL;
-
-	if (cam->module_param.force_munmap)
-		for (i = 0; i < cam->nbuffers; i++)
-			if (cam->frame[i].vma_use_count) {
-				DBG(3, "VIDIOC_S_CROP failed. "
-				       "Unmap the buffers first.");
-				return -EBUSY;
-			}
-
-	/* Preserve R,G or B origin */
-	rect->left = (s->_rect.left & 1L) ? rect->left | 1L : rect->left & ~1L;
-	rect->top = (s->_rect.top & 1L) ? rect->top | 1L : rect->top & ~1L;
-
-	if (rect->width < 16)
-		rect->width = 16;
-	if (rect->height < 16)
-		rect->height = 16;
-	if (rect->width > bounds->width)
-		rect->width = bounds->width;
-	if (rect->height > bounds->height)
-		rect->height = bounds->height;
-	if (rect->left < bounds->left)
-		rect->left = bounds->left;
-	if (rect->top < bounds->top)
-		rect->top = bounds->top;
-	if (rect->left + rect->width > bounds->left + bounds->width)
-		rect->left = bounds->left+bounds->width - rect->width;
-	if (rect->top + rect->height > bounds->top + bounds->height)
-		rect->top = bounds->top+bounds->height - rect->height;
-
-	rect->width &= ~15L;
-	rect->height &= ~15L;
-
-	if (ET61X251_PRESERVE_IMGSCALE) {
-		/* Calculate the actual scaling factor */
-		u32 a, b;
-		a = rect->width * rect->height;
-		b = pix_format->width * pix_format->height;
-		scale = b ? (u8)((a / b) < 4 ? 1 : 2) : 1;
-	} else
-		scale = 1;
-
-	if (cam->stream == STREAM_ON)
-		if ((err = et61x251_stream_interrupt(cam)))
-			return err;
-
-	if (copy_to_user(arg, &crop, sizeof(crop))) {
-		cam->stream = stream;
-		return -EFAULT;
-	}
-
-	if (cam->module_param.force_munmap || cam->io == IO_READ)
-		et61x251_release_buffers(cam);
-
-	err = et61x251_set_crop(cam, rect);
-	if (s->set_crop)
-		err += s->set_crop(cam, rect);
-	err += et61x251_set_scale(cam, scale);
-
-	if (err) { /* atomic, no rollback in ioctl() */
-		cam->state |= DEV_MISCONFIGURED;
-		DBG(1, "VIDIOC_S_CROP failed because of hardware problems. To "
-		       "use the camera, close and open %s again.",
-		    video_device_node_name(cam->v4ldev));
-		return -EIO;
-	}
-
-	s->pix_format.width = rect->width/scale;
-	s->pix_format.height = rect->height/scale;
-	memcpy(&(s->_rect), rect, sizeof(*rect));
-
-	if ((cam->module_param.force_munmap  || cam->io == IO_READ) &&
-	    nbuffers != et61x251_request_buffers(cam, nbuffers, cam->io)) {
-		cam->state |= DEV_MISCONFIGURED;
-		DBG(1, "VIDIOC_S_CROP failed because of not enough memory. To "
-		       "use the camera, close and open %s again.",
-		    video_device_node_name(cam->v4ldev));
-		return -ENOMEM;
-	}
-
-	if (cam->io == IO_READ)
-		et61x251_empty_framequeues(cam);
-	else if (cam->module_param.force_munmap)
-		et61x251_requeue_outqueue(cam);
-
-	cam->stream = stream;
-
-	return 0;
-}
-
-
-static int
-et61x251_vidioc_enum_framesizes(struct et61x251_device* cam, void __user * arg)
-{
-	struct v4l2_frmsizeenum frmsize;
-
-	if (copy_from_user(&frmsize, arg, sizeof(frmsize)))
-		return -EFAULT;
-
-	if (frmsize.index != 0)
-		return -EINVAL;
-
-	if (frmsize.pixel_format != V4L2_PIX_FMT_ET61X251 &&
-	    frmsize.pixel_format != V4L2_PIX_FMT_SBGGR8)
-		return -EINVAL;
-
-	frmsize.type = V4L2_FRMSIZE_TYPE_STEPWISE;
-	frmsize.stepwise.min_width = frmsize.stepwise.step_width = 16;
-	frmsize.stepwise.min_height = frmsize.stepwise.step_height = 16;
-	frmsize.stepwise.max_width = cam->sensor.cropcap.bounds.width;
-	frmsize.stepwise.max_height = cam->sensor.cropcap.bounds.height;
-	memset(&frmsize.reserved, 0, sizeof(frmsize.reserved));
-
-	if (copy_to_user(arg, &frmsize, sizeof(frmsize)))
-		return -EFAULT;
-
-	return 0;
-}
-
-
-static int
-et61x251_vidioc_enum_fmt(struct et61x251_device* cam, void __user * arg)
-{
-	struct v4l2_fmtdesc fmtd;
-
-	if (copy_from_user(&fmtd, arg, sizeof(fmtd)))
-		return -EFAULT;
-
-	if (fmtd.type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
-		return -EINVAL;
-
-	if (fmtd.index == 0) {
-		strcpy(fmtd.description, "bayer rgb");
-		fmtd.pixelformat = V4L2_PIX_FMT_SBGGR8;
-	} else if (fmtd.index == 1) {
-		strcpy(fmtd.description, "compressed");
-		fmtd.pixelformat = V4L2_PIX_FMT_ET61X251;
-		fmtd.flags = V4L2_FMT_FLAG_COMPRESSED;
-	} else
-		return -EINVAL;
-
-	fmtd.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
-	memset(&fmtd.reserved, 0, sizeof(fmtd.reserved));
-
-	if (copy_to_user(arg, &fmtd, sizeof(fmtd)))
-		return -EFAULT;
-
-	return 0;
-}
-
-
-static int
-et61x251_vidioc_g_fmt(struct et61x251_device* cam, void __user * arg)
-{
-	struct v4l2_format format;
-	struct v4l2_pix_format* pfmt = &(cam->sensor.pix_format);
-
-	if (copy_from_user(&format, arg, sizeof(format)))
-		return -EFAULT;
-
-	if (format.type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
-		return -EINVAL;
-
-	pfmt->colorspace = (pfmt->pixelformat == V4L2_PIX_FMT_ET61X251) ?
-			   0 : V4L2_COLORSPACE_SRGB;
-	pfmt->bytesperline = (pfmt->pixelformat==V4L2_PIX_FMT_ET61X251)
-			     ? 0 : (pfmt->width * pfmt->priv) / 8;
-	pfmt->sizeimage = pfmt->height * ((pfmt->width*pfmt->priv)/8);
-	pfmt->field = V4L2_FIELD_NONE;
-	memcpy(&(format.fmt.pix), pfmt, sizeof(*pfmt));
-
-	if (copy_to_user(arg, &format, sizeof(format)))
-		return -EFAULT;
-
-	return 0;
-}
-
-
-static int
-et61x251_vidioc_try_s_fmt(struct et61x251_device* cam, unsigned int cmd,
-			  void __user * arg)
-{
-	struct et61x251_sensor* s = &cam->sensor;
-	struct v4l2_format format;
-	struct v4l2_pix_format* pix;
-	struct v4l2_pix_format* pfmt = &(s->pix_format);
-	struct v4l2_rect* bounds = &(s->cropcap.bounds);
-	struct v4l2_rect rect;
-	u8 scale;
-	const enum et61x251_stream_state stream = cam->stream;
-	const u32 nbuffers = cam->nbuffers;
-	u32 i;
-	int err = 0;
-
-	if (copy_from_user(&format, arg, sizeof(format)))
-		return -EFAULT;
-
-	pix = &(format.fmt.pix);
-
-	if (format.type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
-		return -EINVAL;
-
-	memcpy(&rect, &(s->_rect), sizeof(rect));
-
-	{ /* calculate the actual scaling factor */
-		u32 a, b;
-		a = rect.width * rect.height;
-		b = pix->width * pix->height;
-		scale = b ? (u8)((a / b) < 4 ? 1 : 2) : 1;
-	}
-
-	rect.width = scale * pix->width;
-	rect.height = scale * pix->height;
-
-	if (rect.width < 16)
-		rect.width = 16;
-	if (rect.height < 16)
-		rect.height = 16;
-	if (rect.width > bounds->left + bounds->width - rect.left)
-		rect.width = bounds->left + bounds->width - rect.left;
-	if (rect.height > bounds->top + bounds->height - rect.top)
-		rect.height = bounds->top + bounds->height - rect.top;
-
-	rect.width &= ~15L;
-	rect.height &= ~15L;
-
-	{ /* adjust the scaling factor */
-		u32 a, b;
-		a = rect.width * rect.height;
-		b = pix->width * pix->height;
-		scale = b ? (u8)((a / b) < 4 ? 1 : 2) : 1;
-	}
-
-	pix->width = rect.width / scale;
-	pix->height = rect.height / scale;
-
-	if (pix->pixelformat != V4L2_PIX_FMT_ET61X251 &&
-	    pix->pixelformat != V4L2_PIX_FMT_SBGGR8)
-		pix->pixelformat = pfmt->pixelformat;
-	pix->priv = pfmt->priv; /* bpp */
-	pix->colorspace = (pix->pixelformat == V4L2_PIX_FMT_ET61X251) ?
-			  0 : V4L2_COLORSPACE_SRGB;
-	pix->colorspace = pfmt->colorspace;
-	pix->bytesperline = (pix->pixelformat == V4L2_PIX_FMT_ET61X251)
-			    ? 0 : (pix->width * pix->priv) / 8;
-	pix->sizeimage = pix->height * ((pix->width * pix->priv) / 8);
-	pix->field = V4L2_FIELD_NONE;
-
-	if (cmd == VIDIOC_TRY_FMT) {
-		if (copy_to_user(arg, &format, sizeof(format)))
-			return -EFAULT;
-		return 0;
-	}
-
-	if (cam->module_param.force_munmap)
-		for (i = 0; i < cam->nbuffers; i++)
-			if (cam->frame[i].vma_use_count) {
-				DBG(3, "VIDIOC_S_FMT failed. "
-				       "Unmap the buffers first.");
-				return -EBUSY;
-			}
-
-	if (cam->stream == STREAM_ON)
-		if ((err = et61x251_stream_interrupt(cam)))
-			return err;
-
-	if (copy_to_user(arg, &format, sizeof(format))) {
-		cam->stream = stream;
-		return -EFAULT;
-	}
-
-	if (cam->module_param.force_munmap || cam->io == IO_READ)
-		et61x251_release_buffers(cam);
-
-	err += et61x251_set_pix_format(cam, pix);
-	err += et61x251_set_crop(cam, &rect);
-	if (s->set_pix_format)
-		err += s->set_pix_format(cam, pix);
-	if (s->set_crop)
-		err += s->set_crop(cam, &rect);
-	err += et61x251_set_scale(cam, scale);
-
-	if (err) { /* atomic, no rollback in ioctl() */
-		cam->state |= DEV_MISCONFIGURED;
-		DBG(1, "VIDIOC_S_FMT failed because of hardware problems. To "
-		       "use the camera, close and open %s again.",
-		    video_device_node_name(cam->v4ldev));
-		return -EIO;
-	}
-
-	memcpy(pfmt, pix, sizeof(*pix));
-	memcpy(&(s->_rect), &rect, sizeof(rect));
-
-	if ((cam->module_param.force_munmap  || cam->io == IO_READ) &&
-	    nbuffers != et61x251_request_buffers(cam, nbuffers, cam->io)) {
-		cam->state |= DEV_MISCONFIGURED;
-		DBG(1, "VIDIOC_S_FMT failed because of not enough memory. To "
-		       "use the camera, close and open %s again.",
-		    video_device_node_name(cam->v4ldev));
-		return -ENOMEM;
-	}
-
-	if (cam->io == IO_READ)
-		et61x251_empty_framequeues(cam);
-	else if (cam->module_param.force_munmap)
-		et61x251_requeue_outqueue(cam);
-
-	cam->stream = stream;
-
-	return 0;
-}
-
-
-static int
-et61x251_vidioc_g_jpegcomp(struct et61x251_device* cam, void __user * arg)
-{
-	if (copy_to_user(arg, &cam->compression,
-			 sizeof(cam->compression)))
-		return -EFAULT;
-
-	return 0;
-}
-
-
-static int
-et61x251_vidioc_s_jpegcomp(struct et61x251_device* cam, void __user * arg)
-{
-	struct v4l2_jpegcompression jc;
-	const enum et61x251_stream_state stream = cam->stream;
-	int err = 0;
-
-	if (copy_from_user(&jc, arg, sizeof(jc)))
-		return -EFAULT;
-
-	if (jc.quality != 0 && jc.quality != 1)
-		return -EINVAL;
-
-	if (cam->stream == STREAM_ON)
-		if ((err = et61x251_stream_interrupt(cam)))
-			return err;
-
-	err += et61x251_set_compression(cam, &jc);
-	if (err) { /* atomic, no rollback in ioctl() */
-		cam->state |= DEV_MISCONFIGURED;
-		DBG(1, "VIDIOC_S_JPEGCOMP failed because of hardware "
-		       "problems. To use the camera, close and open "
-		       "%s again.", video_device_node_name(cam->v4ldev));
-		return -EIO;
-	}
-
-	cam->compression.quality = jc.quality;
-
-	cam->stream = stream;
-
-	return 0;
-}
-
-
-static int
-et61x251_vidioc_reqbufs(struct et61x251_device* cam, void __user * arg)
-{
-	struct v4l2_requestbuffers rb;
-	u32 i;
-	int err;
-
-	if (copy_from_user(&rb, arg, sizeof(rb)))
-		return -EFAULT;
-
-	if (rb.type != V4L2_BUF_TYPE_VIDEO_CAPTURE ||
-	    rb.memory != V4L2_MEMORY_MMAP)
-		return -EINVAL;
-
-	if (cam->io == IO_READ) {
-		DBG(3, "Close and open the device again to choose the mmap "
-		       "I/O method");
-		return -EBUSY;
-	}
-
-	for (i = 0; i < cam->nbuffers; i++)
-		if (cam->frame[i].vma_use_count) {
-			DBG(3, "VIDIOC_REQBUFS failed. "
-			       "Previous buffers are still mapped.");
-			return -EBUSY;
-		}
-
-	if (cam->stream == STREAM_ON)
-		if ((err = et61x251_stream_interrupt(cam)))
-			return err;
-
-	et61x251_empty_framequeues(cam);
-
-	et61x251_release_buffers(cam);
-	if (rb.count)
-		rb.count = et61x251_request_buffers(cam, rb.count, IO_MMAP);
-
-	if (copy_to_user(arg, &rb, sizeof(rb))) {
-		et61x251_release_buffers(cam);
-		cam->io = IO_NONE;
-		return -EFAULT;
-	}
-
-	cam->io = rb.count ? IO_MMAP : IO_NONE;
-
-	return 0;
-}
-
-
-static int
-et61x251_vidioc_querybuf(struct et61x251_device* cam, void __user * arg)
-{
-	struct v4l2_buffer b;
-
-	if (copy_from_user(&b, arg, sizeof(b)))
-		return -EFAULT;
-
-	if (b.type != V4L2_BUF_TYPE_VIDEO_CAPTURE ||
-	    b.index >= cam->nbuffers || cam->io != IO_MMAP)
-		return -EINVAL;
-
-	memcpy(&b, &cam->frame[b.index].buf, sizeof(b));
-
-	if (cam->frame[b.index].vma_use_count)
-		b.flags |= V4L2_BUF_FLAG_MAPPED;
-
-	if (cam->frame[b.index].state == F_DONE)
-		b.flags |= V4L2_BUF_FLAG_DONE;
-	else if (cam->frame[b.index].state != F_UNUSED)
-		b.flags |= V4L2_BUF_FLAG_QUEUED;
-
-	if (copy_to_user(arg, &b, sizeof(b)))
-		return -EFAULT;
-
-	return 0;
-}
-
-
-static int
-et61x251_vidioc_qbuf(struct et61x251_device* cam, void __user * arg)
-{
-	struct v4l2_buffer b;
-	unsigned long lock_flags;
-
-	if (copy_from_user(&b, arg, sizeof(b)))
-		return -EFAULT;
-
-	if (b.type != V4L2_BUF_TYPE_VIDEO_CAPTURE ||
-	    b.index >= cam->nbuffers || cam->io != IO_MMAP)
-		return -EINVAL;
-
-	if (cam->frame[b.index].state != F_UNUSED)
-		return -EINVAL;
-
-	cam->frame[b.index].state = F_QUEUED;
-
-	spin_lock_irqsave(&cam->queue_lock, lock_flags);
-	list_add_tail(&cam->frame[b.index].frame, &cam->inqueue);
-	spin_unlock_irqrestore(&cam->queue_lock, lock_flags);
-
-	PDBGG("Frame #%lu queued", (unsigned long)b.index);
-
-	return 0;
-}
-
-
-static int
-et61x251_vidioc_dqbuf(struct et61x251_device* cam, struct file* filp,
-		      void __user * arg)
-{
-	struct v4l2_buffer b;
-	struct et61x251_frame_t *f;
-	unsigned long lock_flags;
-	long timeout;
-
-	if (copy_from_user(&b, arg, sizeof(b)))
-		return -EFAULT;
-
-	if (b.type != V4L2_BUF_TYPE_VIDEO_CAPTURE || cam->io!= IO_MMAP)
-		return -EINVAL;
-
-	if (list_empty(&cam->outqueue)) {
-		if (cam->stream == STREAM_OFF)
-			return -EINVAL;
-		if (filp->f_flags & O_NONBLOCK)
-			return -EAGAIN;
-		timeout = wait_event_interruptible_timeout
-			  ( cam->wait_frame,
-			    (!list_empty(&cam->outqueue)) ||
-			    (cam->state & DEV_DISCONNECTED) ||
-			    (cam->state & DEV_MISCONFIGURED),
-			    cam->module_param.frame_timeout *
-			    1000 * msecs_to_jiffies(1) );
-		if (timeout < 0)
-			return timeout;
-		if (cam->state & DEV_DISCONNECTED)
-			return -ENODEV;
-		if (!timeout || (cam->state & DEV_MISCONFIGURED))
-			return -EIO;
-	}
-
-	spin_lock_irqsave(&cam->queue_lock, lock_flags);
-	f = list_entry(cam->outqueue.next, struct et61x251_frame_t, frame);
-	list_del(cam->outqueue.next);
-	spin_unlock_irqrestore(&cam->queue_lock, lock_flags);
-
-	f->state = F_UNUSED;
-
-	memcpy(&b, &f->buf, sizeof(b));
-	if (f->vma_use_count)
-		b.flags |= V4L2_BUF_FLAG_MAPPED;
-
-	if (copy_to_user(arg, &b, sizeof(b)))
-		return -EFAULT;
-
-	PDBGG("Frame #%lu dequeued", (unsigned long)f->buf.index);
-
-	return 0;
-}
-
-
-static int
-et61x251_vidioc_streamon(struct et61x251_device* cam, void __user * arg)
-{
-	int type;
-
-	if (copy_from_user(&type, arg, sizeof(type)))
-		return -EFAULT;
-
-	if (type != V4L2_BUF_TYPE_VIDEO_CAPTURE || cam->io != IO_MMAP)
-		return -EINVAL;
-
-	cam->stream = STREAM_ON;
-
-	DBG(3, "Stream on");
-
-	return 0;
-}
-
-
-static int
-et61x251_vidioc_streamoff(struct et61x251_device* cam, void __user * arg)
-{
-	int type, err;
-
-	if (copy_from_user(&type, arg, sizeof(type)))
-		return -EFAULT;
-
-	if (type != V4L2_BUF_TYPE_VIDEO_CAPTURE || cam->io != IO_MMAP)
-		return -EINVAL;
-
-	if (cam->stream == STREAM_ON)
-		if ((err = et61x251_stream_interrupt(cam)))
-			return err;
-
-	et61x251_empty_framequeues(cam);
-
-	DBG(3, "Stream off");
-
-	return 0;
-}
-
-
-static int
-et61x251_vidioc_g_parm(struct et61x251_device* cam, void __user * arg)
-{
-	struct v4l2_streamparm sp;
-
-	if (copy_from_user(&sp, arg, sizeof(sp)))
-		return -EFAULT;
-
-	if (sp.type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
-		return -EINVAL;
-
-	sp.parm.capture.extendedmode = 0;
-	sp.parm.capture.readbuffers = cam->nreadbuffers;
-
-	if (copy_to_user(arg, &sp, sizeof(sp)))
-		return -EFAULT;
-
-	return 0;
-}
-
-
-static int
-et61x251_vidioc_s_parm(struct et61x251_device* cam, void __user * arg)
-{
-	struct v4l2_streamparm sp;
-
-	if (copy_from_user(&sp, arg, sizeof(sp)))
-		return -EFAULT;
-
-	if (sp.type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
-		return -EINVAL;
-
-	sp.parm.capture.extendedmode = 0;
-
-	if (sp.parm.capture.readbuffers == 0)
-		sp.parm.capture.readbuffers = cam->nreadbuffers;
-
-	if (sp.parm.capture.readbuffers > ET61X251_MAX_FRAMES)
-		sp.parm.capture.readbuffers = ET61X251_MAX_FRAMES;
-
-	if (copy_to_user(arg, &sp, sizeof(sp)))
-		return -EFAULT;
-
-	cam->nreadbuffers = sp.parm.capture.readbuffers;
-
-	return 0;
-}
-
-
-static long et61x251_ioctl_v4l2(struct file *filp,
-			       unsigned int cmd, void __user *arg)
-{
-	struct et61x251_device *cam = video_drvdata(filp);
-
-	switch (cmd) {
-
-	case VIDIOC_QUERYCAP:
-		return et61x251_vidioc_querycap(cam, arg);
-
-	case VIDIOC_ENUMINPUT:
-		return et61x251_vidioc_enuminput(cam, arg);
-
-	case VIDIOC_G_INPUT:
-		return et61x251_vidioc_g_input(cam, arg);
-
-	case VIDIOC_S_INPUT:
-		return et61x251_vidioc_s_input(cam, arg);
-
-	case VIDIOC_QUERYCTRL:
-		return et61x251_vidioc_query_ctrl(cam, arg);
-
-	case VIDIOC_G_CTRL:
-		return et61x251_vidioc_g_ctrl(cam, arg);
-
-	case VIDIOC_S_CTRL:
-		return et61x251_vidioc_s_ctrl(cam, arg);
-
-	case VIDIOC_CROPCAP:
-		return et61x251_vidioc_cropcap(cam, arg);
-
-	case VIDIOC_G_CROP:
-		return et61x251_vidioc_g_crop(cam, arg);
-
-	case VIDIOC_S_CROP:
-		return et61x251_vidioc_s_crop(cam, arg);
-
-	case VIDIOC_ENUM_FMT:
-		return et61x251_vidioc_enum_fmt(cam, arg);
-
-	case VIDIOC_G_FMT:
-		return et61x251_vidioc_g_fmt(cam, arg);
-
-	case VIDIOC_TRY_FMT:
-	case VIDIOC_S_FMT:
-		return et61x251_vidioc_try_s_fmt(cam, cmd, arg);
-
-	case VIDIOC_ENUM_FRAMESIZES:
-		return et61x251_vidioc_enum_framesizes(cam, arg);
-
-	case VIDIOC_G_JPEGCOMP:
-		return et61x251_vidioc_g_jpegcomp(cam, arg);
-
-	case VIDIOC_S_JPEGCOMP:
-		return et61x251_vidioc_s_jpegcomp(cam, arg);
-
-	case VIDIOC_REQBUFS:
-		return et61x251_vidioc_reqbufs(cam, arg);
-
-	case VIDIOC_QUERYBUF:
-		return et61x251_vidioc_querybuf(cam, arg);
-
-	case VIDIOC_QBUF:
-		return et61x251_vidioc_qbuf(cam, arg);
-
-	case VIDIOC_DQBUF:
-		return et61x251_vidioc_dqbuf(cam, filp, arg);
-
-	case VIDIOC_STREAMON:
-		return et61x251_vidioc_streamon(cam, arg);
-
-	case VIDIOC_STREAMOFF:
-		return et61x251_vidioc_streamoff(cam, arg);
-
-	case VIDIOC_G_PARM:
-		return et61x251_vidioc_g_parm(cam, arg);
-
-	case VIDIOC_S_PARM:
-		return et61x251_vidioc_s_parm(cam, arg);
-
-	default:
-		return -ENOTTY;
-
-	}
-}
-
-
-static long et61x251_ioctl(struct file *filp,
-			 unsigned int cmd, unsigned long arg)
-{
-	struct et61x251_device *cam = video_drvdata(filp);
-	long err = 0;
-
-	if (mutex_lock_interruptible(&cam->fileop_mutex))
-		return -ERESTARTSYS;
-
-	if (cam->state & DEV_DISCONNECTED) {
-		DBG(1, "Device not present");
-		mutex_unlock(&cam->fileop_mutex);
-		return -ENODEV;
-	}
-
-	if (cam->state & DEV_MISCONFIGURED) {
-		DBG(1, "The camera is misconfigured. Close and open it "
-		       "again.");
-		mutex_unlock(&cam->fileop_mutex);
-		return -EIO;
-	}
-
-	V4LDBG(3, "et61x251", cmd);
-
-	err = et61x251_ioctl_v4l2(filp, cmd, (void __user *)arg);
-
-	mutex_unlock(&cam->fileop_mutex);
-
-	return err;
-}
-
-
-static const struct v4l2_file_operations et61x251_fops = {
-	.owner = THIS_MODULE,
-	.open =    et61x251_open,
-	.release = et61x251_release,
-	.unlocked_ioctl =   et61x251_ioctl,
-	.read =    et61x251_read,
-	.poll =    et61x251_poll,
-	.mmap =    et61x251_mmap,
-};
-
-/*****************************************************************************/
-
-/* It exists a single interface only. We do not need to validate anything. */
-static int
-et61x251_usb_probe(struct usb_interface* intf, const struct usb_device_id* id)
-{
-	struct usb_device *udev = interface_to_usbdev(intf);
-	struct et61x251_device* cam;
-	static unsigned int dev_nr;
-	unsigned int i;
-	int err = 0;
-
-	if (!(cam = kzalloc(sizeof(struct et61x251_device), GFP_KERNEL)))
-		return -ENOMEM;
-
-	cam->usbdev = udev;
-
-	if (!(cam->control_buffer = kzalloc(8, GFP_KERNEL))) {
-		DBG(1, "kmalloc() failed");
-		err = -ENOMEM;
-		goto fail;
-	}
-
-	if (!(cam->v4ldev = video_device_alloc())) {
-		DBG(1, "video_device_alloc() failed");
-		err = -ENOMEM;
-		goto fail;
-	}
-
-	DBG(2, "ET61X[12]51 PC Camera Controller detected "
-	       "(vid/pid 0x%04X:0x%04X)",id->idVendor, id->idProduct);
-
-	for  (i = 0; et61x251_sensor_table[i]; i++) {
-		err = et61x251_sensor_table[i](cam);
-		if (!err)
-			break;
-	}
-
-	if (!err)
-		DBG(2, "%s image sensor detected", cam->sensor.name);
-	else {
-		DBG(1, "No supported image sensor detected");
-		err = -ENODEV;
-		goto fail;
-	}
-
-	if (et61x251_init(cam)) {
-		DBG(1, "Initialization failed. I will retry on open().");
-		cam->state |= DEV_MISCONFIGURED;
-	}
-
-	strcpy(cam->v4ldev->name, "ET61X[12]51 PC Camera");
-	cam->v4ldev->fops = &et61x251_fops;
-	cam->v4ldev->release = video_device_release;
-	cam->v4ldev->parent = &udev->dev;
-	video_set_drvdata(cam->v4ldev, cam);
-
-	init_completion(&cam->probe);
-
-	err = video_register_device(cam->v4ldev, VFL_TYPE_GRABBER,
-				    video_nr[dev_nr]);
-	if (err) {
-		DBG(1, "V4L2 device registration failed");
-		if (err == -ENFILE && video_nr[dev_nr] == -1)
-			DBG(1, "Free /dev/videoX node not found");
-		video_nr[dev_nr] = -1;
-		dev_nr = (dev_nr < ET61X251_MAX_DEVICES-1) ? dev_nr+1 : 0;
-		complete_all(&cam->probe);
-		goto fail;
-	}
-
-	DBG(2, "V4L2 device registered as %s",
-	    video_device_node_name(cam->v4ldev));
-
-	cam->module_param.force_munmap = force_munmap[dev_nr];
-	cam->module_param.frame_timeout = frame_timeout[dev_nr];
-
-	dev_nr = (dev_nr < ET61X251_MAX_DEVICES-1) ? dev_nr+1 : 0;
-
-#ifdef CONFIG_VIDEO_ADV_DEBUG
-	err = et61x251_create_sysfs(cam);
-	if (!err)
-		DBG(2, "Optional device control through 'sysfs' "
-		       "interface ready");
-	else
-		DBG(2, "Failed to create 'sysfs' interface for optional "
-		       "device controlling. Error #%d", err);
-#else
-	DBG(2, "Optional device control through 'sysfs' interface disabled");
-	DBG(3, "Compile the kernel with the 'CONFIG_VIDEO_ADV_DEBUG' "
-	       "configuration option to enable it.");
-#endif
-
-	usb_set_intfdata(intf, cam);
-	kref_init(&cam->kref);
-	usb_get_dev(cam->usbdev);
-
-	complete_all(&cam->probe);
-
-	return 0;
-
-fail:
-	if (cam) {
-		kfree(cam->control_buffer);
-		if (cam->v4ldev)
-			video_device_release(cam->v4ldev);
-		kfree(cam);
-	}
-	return err;
-}
-
-
-static void et61x251_usb_disconnect(struct usb_interface* intf)
-{
-	struct et61x251_device* cam;
-
-	down_write(&et61x251_dev_lock);
-
-	cam = usb_get_intfdata(intf);
-
-	DBG(2, "Disconnecting %s...", cam->v4ldev->name);
-
-	if (cam->users) {
-		DBG(2, "Device %s is open! Deregistration and memory "
-		       "deallocation are deferred.",
-		    video_device_node_name(cam->v4ldev));
-		cam->state |= DEV_MISCONFIGURED;
-		et61x251_stop_transfer(cam);
-		cam->state |= DEV_DISCONNECTED;
-		wake_up_interruptible(&cam->wait_frame);
-		wake_up(&cam->wait_stream);
-	} else
-		cam->state |= DEV_DISCONNECTED;
-
-	wake_up_interruptible_all(&cam->wait_open);
-
-	kref_put(&cam->kref, et61x251_release_resources);
-
-	up_write(&et61x251_dev_lock);
-}
-
-
-static struct usb_driver et61x251_usb_driver = {
-	.name =       "et61x251",
-	.id_table =   et61x251_id_table,
-	.probe =      et61x251_usb_probe,
-	.disconnect = et61x251_usb_disconnect,
-};
-
-module_usb_driver(et61x251_usb_driver);
diff --git a/drivers/media/video/et61x251/et61x251_sensor.h b/drivers/media/video/et61x251/et61x251_sensor.h
deleted file mode 100644
index 71a03148cb0..00000000000
--- a/drivers/media/video/et61x251/et61x251_sensor.h
+++ /dev/null
@@ -1,108 +0,0 @@
-/***************************************************************************
- * API for image sensors connected to ET61X[12]51 PC Camera Controllers    *
- *                                                                         *
- * Copyright (C) 2006-2007 by Luca Risolia <luca.risolia@studio.unibo.it>  *
- *                                                                         *
- * 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 _ET61X251_SENSOR_H_
-#define _ET61X251_SENSOR_H_
-
-#include <linux/usb.h>
-#include <linux/videodev2.h>
-#include <linux/device.h>
-#include <linux/stddef.h>
-#include <linux/errno.h>
-#include <asm/types.h>
-
-struct et61x251_device;
-struct et61x251_sensor;
-
-/*****************************************************************************/
-
-extern int et61x251_probe_tas5130d1b(struct et61x251_device* cam);
-
-#define ET61X251_SENSOR_TABLE                                                 \
-/* Weak detections must go at the end of the list */                          \
-static int (*et61x251_sensor_table[])(struct et61x251_device*) = {            \
-	&et61x251_probe_tas5130d1b,                                           \
-	NULL,                                                                 \
-};
-
-extern struct et61x251_device*
-et61x251_match_id(struct et61x251_device* cam, const struct usb_device_id *id);
-
-extern void
-et61x251_attach_sensor(struct et61x251_device* cam,
-		       const struct et61x251_sensor* sensor);
-
-/*****************************************************************************/
-
-extern int et61x251_write_reg(struct et61x251_device*, u8 value, u16 index);
-extern int et61x251_i2c_raw_write(struct et61x251_device*, u8 n, u8 data1,
-				  u8 data2, u8 data3, u8 data4, u8 data5,
-				  u8 data6, u8 data7, u8 data8, u8 address);
-
-/*****************************************************************************/
-
-enum et61x251_i2c_sysfs_ops {
-	ET61X251_I2C_READ = 0x01,
-	ET61X251_I2C_WRITE = 0x02,
-};
-
-enum et61x251_i2c_interface {
-	ET61X251_I2C_2WIRES,
-	ET61X251_I2C_3WIRES,
-};
-
-/* Repeat start condition when RSTA is high */
-enum et61x251_i2c_rsta {
-	ET61X251_I2C_RSTA_STOP = 0x00, /* stop then start */
-	ET61X251_I2C_RSTA_REPEAT = 0x01, /* repeat start */
-};
-
-#define ET61X251_MAX_CTRLS (V4L2_CID_LASTP1-V4L2_CID_BASE+10)
-
-struct et61x251_sensor {
-	char name[32];
-
-	enum et61x251_i2c_sysfs_ops sysfs_ops;
-
-	enum et61x251_i2c_interface interface;
-	u8 i2c_slave_id;
-	enum et61x251_i2c_rsta rsta;
-	struct v4l2_rect active_pixel; /* left and top define FVSX and FVSY */
-
-	struct v4l2_queryctrl qctrl[ET61X251_MAX_CTRLS];
-	struct v4l2_cropcap cropcap;
-	struct v4l2_pix_format pix_format;
-
-	int (*init)(struct et61x251_device* cam);
-	int (*get_ctrl)(struct et61x251_device* cam,
-			struct v4l2_control* ctrl);
-	int (*set_ctrl)(struct et61x251_device* cam,
-			const struct v4l2_control* ctrl);
-	int (*set_crop)(struct et61x251_device* cam,
-			const struct v4l2_rect* rect);
-	int (*set_pix_format)(struct et61x251_device* cam,
-			      const struct v4l2_pix_format* pix);
-
-	/* Private */
-	struct v4l2_queryctrl _qctrl[ET61X251_MAX_CTRLS];
-	struct v4l2_rect _rect;
-};
-
-#endif /* _ET61X251_SENSOR_H_ */
diff --git a/drivers/media/video/et61x251/et61x251_tas5130d1b.c b/drivers/media/video/et61x251/et61x251_tas5130d1b.c
deleted file mode 100644
index ced2e167935..00000000000
--- a/drivers/media/video/et61x251/et61x251_tas5130d1b.c
+++ /dev/null
@@ -1,143 +0,0 @@
-/***************************************************************************
- * Plug-in for TAS5130D1B image sensor connected to the ET61X[12]51        *
- * PC Camera Controllers                                                   *
- *                                                                         *
- * Copyright (C) 2006-2007 by Luca Risolia <luca.risolia@studio.unibo.it>  *
- *                                                                         *
- * 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 "et61x251_sensor.h"
-
-
-static int tas5130d1b_init(struct et61x251_device* cam)
-{
-	int err = 0;
-
-	err += et61x251_write_reg(cam, 0x14, 0x01);
-	err += et61x251_write_reg(cam, 0x1b, 0x02);
-	err += et61x251_write_reg(cam, 0x02, 0x12);
-	err += et61x251_write_reg(cam, 0x0e, 0x60);
-	err += et61x251_write_reg(cam, 0x80, 0x61);
-	err += et61x251_write_reg(cam, 0xf0, 0x62);
-	err += et61x251_write_reg(cam, 0x03, 0x63);
-	err += et61x251_write_reg(cam, 0x14, 0x64);
-	err += et61x251_write_reg(cam, 0xf4, 0x65);
-	err += et61x251_write_reg(cam, 0x01, 0x66);
-	err += et61x251_write_reg(cam, 0x05, 0x67);
-	err += et61x251_write_reg(cam, 0x8f, 0x68);
-	err += et61x251_write_reg(cam, 0x0f, 0x8d);
-	err += et61x251_write_reg(cam, 0x08, 0x8e);
-
-	return err;
-}
-
-
-static int tas5130d1b_set_ctrl(struct et61x251_device* cam,
-			       const struct v4l2_control* ctrl)
-{
-	int err = 0;
-
-	switch (ctrl->id) {
-	case V4L2_CID_GAIN:
-		err += et61x251_i2c_raw_write(cam, 2, 0x20,
-					      0xf6-ctrl->value, 0, 0, 0,
-					      0, 0, 0, 0);
-		break;
-	case V4L2_CID_EXPOSURE:
-		err += et61x251_i2c_raw_write(cam, 2, 0x40,
-					      0x47-ctrl->value, 0, 0, 0,
-					      0, 0, 0, 0);
-		break;
-	default:
-		return -EINVAL;
-	}
-
-	return err ? -EIO : 0;
-}
-
-
-static const struct et61x251_sensor tas5130d1b = {
-	.name = "TAS5130D1B",
-	.interface = ET61X251_I2C_3WIRES,
-	.rsta = ET61X251_I2C_RSTA_STOP,
-	.active_pixel = {
-		.left = 106,
-		.top = 13,
-	},
-	.init = &tas5130d1b_init,
-	.qctrl = {
-		{
-			.id = V4L2_CID_GAIN,
-			.type = V4L2_CTRL_TYPE_INTEGER,
-			.name = "global gain",
-			.minimum = 0x00,
-			.maximum = 0xf6,
-			.step = 0x02,
-			.default_value = 0x0d,
-			.flags = 0,
-		},
-		{
-			.id = V4L2_CID_EXPOSURE,
-			.type = V4L2_CTRL_TYPE_INTEGER,
-			.name = "exposure",
-			.minimum = 0x00,
-			.maximum = 0x47,
-			.step = 0x01,
-			.default_value = 0x23,
-			.flags = 0,
-		},
-	},
-	.set_ctrl = &tas5130d1b_set_ctrl,
-	.cropcap = {
-		.bounds = {
-			.left = 0,
-			.top = 0,
-			.width = 640,
-			.height = 480,
-		},
-		.defrect = {
-			.left = 0,
-			.top = 0,
-			.width = 640,
-			.height = 480,
-		},
-	},
-	.pix_format = {
-		.width = 640,
-		.height = 480,
-		.pixelformat = V4L2_PIX_FMT_SBGGR8,
-		.priv = 8,
-	},
-};
-
-
-int et61x251_probe_tas5130d1b(struct et61x251_device* cam)
-{
-	const struct usb_device_id tas5130d1b_id_table[] = {
-		{ USB_DEVICE(0x102c, 0x6251), },
-		{ }
-	};
-
-	/* Sensor detection is based on USB pid/vid */
-	if (!et61x251_match_id(cam, tas5130d1b_id_table))
-		return -ENODEV;
-
-	et61x251_attach_sensor(cam, &tas5130d1b);
-
-	return 0;
-}
diff --git a/drivers/media/video/fsl-viu.c b/drivers/media/video/fsl-viu.c
index 27e3e0c0b21..777486f7cad 100644
--- a/drivers/media/video/fsl-viu.c
+++ b/drivers/media/video/fsl-viu.c
@@ -1544,6 +1544,10 @@ static int __devinit viu_of_probe(struct platform_device *op)
 
 	/* initialize locks */
 	mutex_init(&viu_dev->lock);
+	/* Locking in file operations other than ioctl should be done
+	   by the driver, not the V4L2 core.
+	   This driver needs auditing so that this flag can be removed. */
+	set_bit(V4L2_FL_LOCK_ALL_FOPS, &viu_dev->vdev->flags);
 	viu_dev->vdev->lock = &viu_dev->lock;
 	spin_lock_init(&viu_dev->slock);
 
diff --git a/drivers/media/video/gspca/Makefile b/drivers/media/video/gspca/Makefile
index 79ebe46e1ad..c901da0bd65 100644
--- a/drivers/media/video/gspca/Makefile
+++ b/drivers/media/video/gspca/Makefile
@@ -43,7 +43,7 @@ obj-$(CONFIG_USB_GSPCA_VICAM)    += gspca_vicam.o
 obj-$(CONFIG_USB_GSPCA_XIRLINK_CIT) += gspca_xirlink_cit.o
 obj-$(CONFIG_USB_GSPCA_ZC3XX)    += gspca_zc3xx.o
 
-gspca_main-objs     := gspca.o
+gspca_main-objs     := gspca.o autogain_functions.o
 gspca_benq-objs     := benq.o
 gspca_conex-objs    := conex.o
 gspca_cpia1-objs    := cpia1.o
diff --git a/drivers/media/video/gspca/autogain_functions.c b/drivers/media/video/gspca/autogain_functions.c
new file mode 100644
index 00000000000..67db674bb04
--- /dev/null
+++ b/drivers/media/video/gspca/autogain_functions.c
@@ -0,0 +1,178 @@
+/*
+ * Functions for auto gain.
+ *
+ * Copyright (C) 2010-2012 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
+ */
+#include "gspca.h"
+
+/* 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. */
+int gspca_expo_autogain(
+			struct gspca_dev *gspca_dev,
+			int avg_lum,
+			int desired_avg_lum,
+			int deadzone,
+			int gain_knee,
+			int exposure_knee)
+{
+	s32 gain, orig_gain, exposure, orig_exposure;
+	int i, steps, retval = 0;
+
+	if (v4l2_ctrl_g_ctrl(gspca_dev->autogain) == 0)
+	        return 0;
+
+	orig_gain = gain = v4l2_ctrl_g_ctrl(gspca_dev->gain);
+	orig_exposure = exposure = v4l2_ctrl_g_ctrl(gspca_dev->exposure);
+
+	/* 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 > gspca_dev->gain->default_value)
+				gain--;
+			else if (exposure > gspca_dev->exposure->minimum)
+				exposure--;
+			else if (gain > gspca_dev->gain->minimum)
+				gain--;
+			else
+				break;
+		} else {
+			if (gain < gspca_dev->gain->default_value)
+				gain++;
+			else if (exposure < exposure_knee)
+				exposure++;
+			else if (gain < gain_knee)
+				gain++;
+			else if (exposure < gspca_dev->exposure->maximum)
+				exposure++;
+			else if (gain < gspca_dev->gain->maximum)
+				gain++;
+			else
+				break;
+		}
+	}
+
+	if (gain != orig_gain) {
+	        v4l2_ctrl_s_ctrl(gspca_dev->gain, gain);
+		retval = 1;
+	}
+	if (exposure != orig_exposure) {
+	        v4l2_ctrl_s_ctrl(gspca_dev->exposure, exposure);
+		retval = 1;
+	}
+
+	if (retval)
+		PDEBUG(D_FRAM, "autogain: changed gain: %d, expo: %d",
+			gain, exposure);
+	return retval;
+}
+EXPORT_SYMBOL(gspca_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.
+
+   Returns 0 if no changes were made, 1 if the gain and or exposure settings
+   where changed. */
+int gspca_coarse_grained_expo_autogain(
+			struct gspca_dev *gspca_dev,
+			int avg_lum,
+			int desired_avg_lum,
+			int deadzone)
+{
+	s32 gain_low, gain_high, gain, orig_gain, exposure, orig_exposure;
+	int steps, retval = 0;
+
+	if (v4l2_ctrl_g_ctrl(gspca_dev->autogain) == 0)
+	        return 0;
+
+	orig_gain = gain = v4l2_ctrl_g_ctrl(gspca_dev->gain);
+	orig_exposure = exposure = v4l2_ctrl_g_ctrl(gspca_dev->exposure);
+
+	gain_low  = (gspca_dev->gain->maximum - gspca_dev->gain->minimum) /
+		    5 * 2 + gspca_dev->gain->minimum;
+	gain_high = (gspca_dev->gain->maximum - gspca_dev->gain->minimum) /
+		    5 * 4 + gspca_dev->gain->minimum;
+
+	/* 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 < gspca_dev->exposure->maximum) {
+		gain = gain_high;
+		gspca_dev->exp_too_low_cnt++;
+		gspca_dev->exp_too_high_cnt = 0;
+	} else if ((gain + steps) < gain_low &&
+		   exposure > gspca_dev->exposure->minimum) {
+		gain = gain_low;
+		gspca_dev->exp_too_high_cnt++;
+		gspca_dev->exp_too_low_cnt = 0;
+	} else {
+		gain += steps;
+		if (gain > gspca_dev->gain->maximum)
+			gain = gspca_dev->gain->maximum;
+		else if (gain < gspca_dev->gain->minimum)
+			gain = gspca_dev->gain->minimum;
+		gspca_dev->exp_too_high_cnt = 0;
+		gspca_dev->exp_too_low_cnt = 0;
+	}
+
+	if (gspca_dev->exp_too_high_cnt > 3) {
+		exposure--;
+		gspca_dev->exp_too_high_cnt = 0;
+	} else if (gspca_dev->exp_too_low_cnt > 3) {
+		exposure++;
+		gspca_dev->exp_too_low_cnt = 0;
+	}
+
+	if (gain != orig_gain) {
+	        v4l2_ctrl_s_ctrl(gspca_dev->gain, gain);
+		retval = 1;
+	}
+	if (exposure != orig_exposure) {
+	        v4l2_ctrl_s_ctrl(gspca_dev->exposure, exposure);
+		retval = 1;
+	}
+
+	if (retval)
+		PDEBUG(D_FRAM, "autogain: changed gain: %d, expo: %d",
+			gain, exposure);
+	return retval;
+}
+EXPORT_SYMBOL(gspca_coarse_grained_expo_autogain);
diff --git a/drivers/media/video/gspca/autogain_functions.h b/drivers/media/video/gspca/autogain_functions.h
index 46777eee678..d625eafe63e 100644
--- a/drivers/media/video/gspca/autogain_functions.h
+++ b/drivers/media/video/gspca/autogain_functions.h
@@ -18,6 +18,7 @@
  * 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
 
@@ -91,7 +92,9 @@ static inline int auto_gain_n_exposure(
 			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
@@ -103,7 +106,7 @@ static inline int auto_gain_n_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_high_cnt and exp_too_high_cnt int members for use by this function.
+   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. */
@@ -177,3 +180,4 @@ static inline int coarse_grained_expo_autogain(
 			gain, exposure);
 	return retval;
 }
+#endif
diff --git a/drivers/media/video/gspca/conex.c b/drivers/media/video/gspca/conex.c
index ea17b5d94ea..f39fee0fd10 100644
--- a/drivers/media/video/gspca/conex.c
+++ b/drivers/media/video/gspca/conex.c
@@ -306,7 +306,7 @@ static void cx_sensor(struct gspca_dev*gspca_dev)
 
 	reg_w(gspca_dev, 0x0020, reg20, 8);
 	reg_w(gspca_dev, 0x0028, reg28, 8);
-	reg_w(gspca_dev, 0x0010, reg10, 8);
+	reg_w(gspca_dev, 0x0010, reg10, 2);
 	reg_w_val(gspca_dev, 0x0092, 0x03);
 
 	switch (gspca_dev->cam.cam_mode[(int) gspca_dev->curr_mode].priv) {
@@ -326,7 +326,7 @@ static void cx_sensor(struct gspca_dev*gspca_dev)
 	}
 	reg_w(gspca_dev, 0x007b, reg7b, 6);
 	reg_w_val(gspca_dev, 0x00f8, 0x00);
-	reg_w(gspca_dev, 0x0010, reg10, 8);
+	reg_w(gspca_dev, 0x0010, reg10, 2);
 	reg_w_val(gspca_dev, 0x0098, 0x41);
 	for (i = 0; i < 11; i++) {
 		if (i == 3 || i == 5 || i == 8)
diff --git a/drivers/media/video/gspca/finepix.c b/drivers/media/video/gspca/finepix.c
index 0107513cd72..6e26c93b465 100644
--- a/drivers/media/video/gspca/finepix.c
+++ b/drivers/media/video/gspca/finepix.c
@@ -94,7 +94,11 @@ static void dostream(struct work_struct *work)
 
 	/* loop reading a frame */
 again:
-	while (gspca_dev->present && gspca_dev->streaming) {
+	while (gspca_dev->dev && gspca_dev->streaming) {
+#ifdef CONFIG_PM
+		if (gspca_dev->frozen)
+			break;
+#endif
 
 		/* request a frame */
 		mutex_lock(&gspca_dev->usb_lock);
@@ -102,7 +106,11 @@ again:
 		mutex_unlock(&gspca_dev->usb_lock);
 		if (ret < 0)
 			break;
-		if (!gspca_dev->present || !gspca_dev->streaming)
+#ifdef CONFIG_PM
+		if (gspca_dev->frozen)
+			break;
+#endif
+		if (!gspca_dev->dev || !gspca_dev->streaming)
 			break;
 
 		/* the frame comes in parts */
@@ -117,7 +125,11 @@ again:
 				 * error. Just restart. */
 				goto again;
 			}
-			if (!gspca_dev->present || !gspca_dev->streaming)
+#ifdef CONFIG_PM
+			if (gspca_dev->frozen)
+				goto out;
+#endif
+			if (!gspca_dev->dev || !gspca_dev->streaming)
 				goto out;
 			if (len < FPIX_MAX_TRANSFER ||
 				(data[len - 2] == 0xff &&
diff --git a/drivers/media/video/gspca/gl860/gl860.c b/drivers/media/video/gspca/gl860/gl860.c
index c84e26006fc..c549574c1c7 100644
--- a/drivers/media/video/gspca/gl860/gl860.c
+++ b/drivers/media/video/gspca/gl860/gl860.c
@@ -405,6 +405,9 @@ static void sd_stop0(struct gspca_dev *gspca_dev)
 {
 	struct sd *sd = (struct sd *) gspca_dev;
 
+	if (!sd->gspca_dev.present)
+		return;
+
 	return sd->dev_post_unset_alt(gspca_dev);
 }
 
diff --git a/drivers/media/video/gspca/gspca.c b/drivers/media/video/gspca/gspca.c
index ca5a2b139d0..137166d7394 100644
--- a/drivers/media/video/gspca/gspca.c
+++ b/drivers/media/video/gspca/gspca.c
@@ -38,6 +38,9 @@
 #include <linux/uaccess.h>
 #include <linux/ktime.h>
 #include <media/v4l2-ioctl.h>
+#include <media/v4l2-ctrls.h>
+#include <media/v4l2-fh.h>
+#include <media/v4l2-event.h>
 
 #include "gspca.h"
 
@@ -592,16 +595,13 @@ static int gspca_set_alt0(struct gspca_dev *gspca_dev)
 static void gspca_stream_off(struct gspca_dev *gspca_dev)
 {
 	gspca_dev->streaming = 0;
-	if (gspca_dev->present) {
-		if (gspca_dev->sd_desc->stopN)
-			gspca_dev->sd_desc->stopN(gspca_dev);
-		destroy_urbs(gspca_dev);
-		gspca_input_destroy_urb(gspca_dev);
-		gspca_set_alt0(gspca_dev);
-		gspca_input_create_urb(gspca_dev);
-	}
-
-	/* always call stop0 to free the subdriver's resources */
+	gspca_dev->usb_err = 0;
+	if (gspca_dev->sd_desc->stopN)
+		gspca_dev->sd_desc->stopN(gspca_dev);
+	destroy_urbs(gspca_dev);
+	gspca_input_destroy_urb(gspca_dev);
+	gspca_set_alt0(gspca_dev);
+	gspca_input_create_urb(gspca_dev);
 	if (gspca_dev->sd_desc->stop0)
 		gspca_dev->sd_desc->stop0(gspca_dev);
 	PDEBUG(D_STREAM, "stream off OK");
@@ -847,14 +847,6 @@ static int gspca_init_transfer(struct gspca_dev *gspca_dev)
 	struct ep_tb_s ep_tb[MAX_ALT];
 	int n, ret, xfer, alt, alt_idx;
 
-	if (mutex_lock_interruptible(&gspca_dev->usb_lock))
-		return -ERESTARTSYS;
-
-	if (!gspca_dev->present) {
-		ret = -ENODEV;
-		goto unlock;
-	}
-
 	/* reset the streaming variables */
 	gspca_dev->image = NULL;
 	gspca_dev->image_len = 0;
@@ -869,7 +861,7 @@ static int gspca_init_transfer(struct gspca_dev *gspca_dev)
 	if (gspca_dev->sd_desc->isoc_init) {
 		ret = gspca_dev->sd_desc->isoc_init(gspca_dev);
 		if (ret < 0)
-			goto unlock;
+			return ret;
 	}
 	xfer = gspca_dev->cam.bulk ? USB_ENDPOINT_XFER_BULK
 				   : USB_ENDPOINT_XFER_ISOC;
@@ -880,8 +872,7 @@ static int gspca_init_transfer(struct gspca_dev *gspca_dev)
 		ep = alt_xfer(&intf->altsetting[gspca_dev->alt], xfer);
 		if (ep == NULL) {
 			pr_err("bad altsetting %d\n", gspca_dev->alt);
-			ret = -EIO;
-			goto out;
+			return -EIO;
 		}
 		ep_tb[0].alt = gspca_dev->alt;
 		alt_idx = 1;
@@ -892,8 +883,7 @@ static int gspca_init_transfer(struct gspca_dev *gspca_dev)
 		alt_idx = build_isoc_ep_tb(gspca_dev, intf, ep_tb);
 		if (alt_idx <= 0) {
 			pr_err("no transfer endpoint found\n");
-			ret = -EIO;
-			goto unlock;
+			return -EIO;
 		}
 	}
 
@@ -988,8 +978,6 @@ retry:
 	}
 out:
 	gspca_input_create_urb(gspca_dev);
-unlock:
-	mutex_unlock(&gspca_dev->usb_lock);
 	return ret;
 }
 
@@ -1006,6 +994,8 @@ static void gspca_set_default_mode(struct gspca_dev *gspca_dev)
 
 	/* 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;
@@ -1057,77 +1047,50 @@ static int gspca_get_mode(struct gspca_dev *gspca_dev,
 static int vidioc_g_register(struct file *file, void *priv,
 			struct v4l2_dbg_register *reg)
 {
-	int ret;
-	struct gspca_dev *gspca_dev = priv;
+	struct gspca_dev *gspca_dev = video_drvdata(file);
 
 	if (!gspca_dev->sd_desc->get_chip_ident)
-		return -EINVAL;
+		return -ENOTTY;
 
 	if (!gspca_dev->sd_desc->get_register)
-		return -EINVAL;
+		return -ENOTTY;
 
-	if (mutex_lock_interruptible(&gspca_dev->usb_lock))
-		return -ERESTARTSYS;
 	gspca_dev->usb_err = 0;
-	if (gspca_dev->present)
-		ret = gspca_dev->sd_desc->get_register(gspca_dev, reg);
-	else
-		ret = -ENODEV;
-	mutex_unlock(&gspca_dev->usb_lock);
-
-	return ret;
+	return gspca_dev->sd_desc->get_register(gspca_dev, reg);
 }
 
 static int vidioc_s_register(struct file *file, void *priv,
 			struct v4l2_dbg_register *reg)
 {
-	int ret;
-	struct gspca_dev *gspca_dev = priv;
+	struct gspca_dev *gspca_dev = video_drvdata(file);
 
 	if (!gspca_dev->sd_desc->get_chip_ident)
-		return -EINVAL;
+		return -ENOTTY;
 
 	if (!gspca_dev->sd_desc->set_register)
-		return -EINVAL;
+		return -ENOTTY;
 
-	if (mutex_lock_interruptible(&gspca_dev->usb_lock))
-		return -ERESTARTSYS;
 	gspca_dev->usb_err = 0;
-	if (gspca_dev->present)
-		ret = gspca_dev->sd_desc->set_register(gspca_dev, reg);
-	else
-		ret = -ENODEV;
-	mutex_unlock(&gspca_dev->usb_lock);
-
-	return ret;
+	return gspca_dev->sd_desc->set_register(gspca_dev, reg);
 }
 #endif
 
 static int vidioc_g_chip_ident(struct file *file, void *priv,
 			struct v4l2_dbg_chip_ident *chip)
 {
-	int ret;
-	struct gspca_dev *gspca_dev = priv;
+	struct gspca_dev *gspca_dev = video_drvdata(file);
 
 	if (!gspca_dev->sd_desc->get_chip_ident)
-		return -EINVAL;
+		return -ENOTTY;
 
-	if (mutex_lock_interruptible(&gspca_dev->usb_lock))
-		return -ERESTARTSYS;
 	gspca_dev->usb_err = 0;
-	if (gspca_dev->present)
-		ret = gspca_dev->sd_desc->get_chip_ident(gspca_dev, chip);
-	else
-		ret = -ENODEV;
-	mutex_unlock(&gspca_dev->usb_lock);
-
-	return ret;
+	return gspca_dev->sd_desc->get_chip_ident(gspca_dev, chip);
 }
 
 static int vidioc_enum_fmt_vid_cap(struct file *file, void  *priv,
 				struct v4l2_fmtdesc *fmtdesc)
 {
-	struct gspca_dev *gspca_dev = priv;
+	struct gspca_dev *gspca_dev = video_drvdata(file);
 	int i, j, index;
 	__u32 fmt_tb[8];
 
@@ -1169,7 +1132,7 @@ 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 gspca_dev *gspca_dev = priv;
+	struct gspca_dev *gspca_dev = video_drvdata(file);
 	int mode;
 
 	mode = gspca_dev->curr_mode;
@@ -1214,7 +1177,7 @@ static int vidioc_try_fmt_vid_cap(struct file *file,
 			      void *priv,
 			      struct v4l2_format *fmt)
 {
-	struct gspca_dev *gspca_dev = priv;
+	struct gspca_dev *gspca_dev = video_drvdata(file);
 	int ret;
 
 	ret = try_fmt_vid_cap(gspca_dev, fmt);
@@ -1226,7 +1189,7 @@ static int vidioc_try_fmt_vid_cap(struct file *file,
 static int vidioc_s_fmt_vid_cap(struct file *file, void *priv,
 			    struct v4l2_format *fmt)
 {
-	struct gspca_dev *gspca_dev = priv;
+	struct gspca_dev *gspca_dev = video_drvdata(file);
 	int ret;
 
 	if (mutex_lock_interruptible(&gspca_dev->queue_lock))
@@ -1265,7 +1228,7 @@ out:
 static int vidioc_enum_framesizes(struct file *file, void *priv,
 				  struct v4l2_frmsizeenum *fsize)
 {
-	struct gspca_dev *gspca_dev = priv;
+	struct gspca_dev *gspca_dev = video_drvdata(file);
 	int i;
 	__u32 index = 0;
 
@@ -1291,7 +1254,7 @@ static int vidioc_enum_framesizes(struct file *file, void *priv,
 static int vidioc_enum_frameintervals(struct file *filp, void *priv,
 				      struct v4l2_frmivalenum *fival)
 {
-	struct gspca_dev *gspca_dev = priv;
+	struct gspca_dev *gspca_dev = video_drvdata(filp);
 	int mode = wxh_to_mode(gspca_dev, fival->width, fival->height);
 	__u32 i;
 
@@ -1316,31 +1279,30 @@ static int vidioc_enum_frameintervals(struct file *filp, void *priv,
 	return -EINVAL;
 }
 
-static void gspca_release(struct video_device *vfd)
+static void gspca_release(struct v4l2_device *v4l2_device)
 {
-	struct gspca_dev *gspca_dev = container_of(vfd, struct gspca_dev, vdev);
+	struct gspca_dev *gspca_dev =
+		container_of(v4l2_device, struct gspca_dev, v4l2_dev);
 
 	PDEBUG(D_PROBE, "%s released",
 		video_device_node_name(&gspca_dev->vdev));
 
+	v4l2_ctrl_handler_free(gspca_dev->vdev.ctrl_handler);
+	v4l2_device_unregister(&gspca_dev->v4l2_dev);
 	kfree(gspca_dev->usb_buf);
 	kfree(gspca_dev);
 }
 
 static int dev_open(struct file *file)
 {
-	struct gspca_dev *gspca_dev;
+	struct gspca_dev *gspca_dev = video_drvdata(file);
 
 	PDEBUG(D_STREAM, "[%s] open", current->comm);
-	gspca_dev = (struct gspca_dev *) video_devdata(file);
-	if (!gspca_dev->present)
-		return -ENODEV;
 
 	/* protect the subdriver against rmmod */
 	if (!try_module_get(gspca_dev->module))
 		return -ENODEV;
 
-	file->private_data = gspca_dev;
 #ifdef GSPCA_DEBUG
 	/* activate the v4l2 debug */
 	if (gspca_debug & D_V4L2)
@@ -1350,49 +1312,44 @@ static int dev_open(struct file *file)
 		gspca_dev->vdev.debug &= ~(V4L2_DEBUG_IOCTL
 					| V4L2_DEBUG_IOCTL_ARG);
 #endif
-	return 0;
+	return v4l2_fh_open(file);
 }
 
 static int dev_close(struct file *file)
 {
-	struct gspca_dev *gspca_dev = file->private_data;
+	struct gspca_dev *gspca_dev = video_drvdata(file);
 
 	PDEBUG(D_STREAM, "[%s] close", current->comm);
-	if (mutex_lock_interruptible(&gspca_dev->queue_lock))
+
+	/* Needed for gspca_stream_off, always lock before queue_lock! */
+	if (mutex_lock_interruptible(&gspca_dev->usb_lock))
 		return -ERESTARTSYS;
 
+	if (mutex_lock_interruptible(&gspca_dev->queue_lock)) {
+		mutex_unlock(&gspca_dev->usb_lock);
+		return -ERESTARTSYS;
+	}
+
 	/* if the file did the capture, free the streaming resources */
 	if (gspca_dev->capt_file == file) {
-		if (gspca_dev->streaming) {
-			mutex_lock(&gspca_dev->usb_lock);
-			gspca_dev->usb_err = 0;
+		if (gspca_dev->streaming)
 			gspca_stream_off(gspca_dev);
-			mutex_unlock(&gspca_dev->usb_lock);
-		}
 		frame_free(gspca_dev);
 	}
-	file->private_data = NULL;
 	module_put(gspca_dev->module);
 	mutex_unlock(&gspca_dev->queue_lock);
+	mutex_unlock(&gspca_dev->usb_lock);
 
 	PDEBUG(D_STREAM, "close done");
 
-	return 0;
+	return v4l2_fh_release(file);
 }
 
 static int vidioc_querycap(struct file *file, void  *priv,
 			   struct v4l2_capability *cap)
 {
-	struct gspca_dev *gspca_dev = priv;
-	int ret;
+	struct gspca_dev *gspca_dev = video_drvdata(file);
 
-	/* protect the access to the usb device */
-	if (mutex_lock_interruptible(&gspca_dev->usb_lock))
-		return -ERESTARTSYS;
-	if (!gspca_dev->present) {
-		ret = -ENODEV;
-		goto out;
-	}
 	strlcpy((char *) cap->driver, gspca_dev->sd_desc->name,
 			sizeof cap->driver);
 	if (gspca_dev->dev->product != NULL) {
@@ -1406,13 +1363,11 @@ static int vidioc_querycap(struct file *file, void  *priv,
 	}
 	usb_make_path(gspca_dev->dev, (char *) cap->bus_info,
 			sizeof(cap->bus_info));
-	cap->capabilities = V4L2_CAP_VIDEO_CAPTURE
+	cap->device_caps = V4L2_CAP_VIDEO_CAPTURE
 			  | V4L2_CAP_STREAMING
 			  | V4L2_CAP_READWRITE;
-	ret = 0;
-out:
-	mutex_unlock(&gspca_dev->usb_lock);
-	return ret;
+	cap->capabilities = cap->device_caps | V4L2_CAP_DEVICE_CAPS;
+	return 0;
 }
 
 static int get_ctrl(struct gspca_dev *gspca_dev,
@@ -1435,7 +1390,7 @@ static int get_ctrl(struct gspca_dev *gspca_dev,
 static int vidioc_queryctrl(struct file *file, void *priv,
 			   struct v4l2_queryctrl *q_ctrl)
 {
-	struct gspca_dev *gspca_dev = priv;
+	struct gspca_dev *gspca_dev = video_drvdata(file);
 	const struct ctrl *ctrls;
 	struct gspca_ctrl *gspca_ctrl;
 	int i, idx;
@@ -1478,10 +1433,10 @@ static int vidioc_queryctrl(struct file *file, void *priv,
 static int vidioc_s_ctrl(struct file *file, void *priv,
 			 struct v4l2_control *ctrl)
 {
-	struct gspca_dev *gspca_dev = priv;
+	struct gspca_dev *gspca_dev = video_drvdata(file);
 	const struct ctrl *ctrls;
 	struct gspca_ctrl *gspca_ctrl;
-	int idx, ret;
+	int idx;
 
 	idx = get_ctrl(gspca_dev, ctrl->id);
 	if (idx < 0)
@@ -1501,74 +1456,52 @@ static int vidioc_s_ctrl(struct file *file, void *priv,
 			return -ERANGE;
 	}
 	PDEBUG(D_CONF, "set ctrl [%08x] = %d", ctrl->id, ctrl->value);
-	if (mutex_lock_interruptible(&gspca_dev->usb_lock))
-		return -ERESTARTSYS;
-	if (!gspca_dev->present) {
-		ret = -ENODEV;
-		goto out;
-	}
 	gspca_dev->usb_err = 0;
-	if (ctrls->set != NULL) {
-		ret = ctrls->set(gspca_dev, ctrl->value);
-		goto out;
-	}
+	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);
 	}
-	ret = gspca_dev->usb_err;
-out:
-	mutex_unlock(&gspca_dev->usb_lock);
-	return ret;
+	return gspca_dev->usb_err;
 }
 
 static int vidioc_g_ctrl(struct file *file, void *priv,
 			 struct v4l2_control *ctrl)
 {
-	struct gspca_dev *gspca_dev = priv;
+	struct gspca_dev *gspca_dev = video_drvdata(file);
 	const struct ctrl *ctrls;
-	int idx, ret;
+	int idx;
 
 	idx = get_ctrl(gspca_dev, ctrl->id);
 	if (idx < 0)
 		return -EINVAL;
 	ctrls = &gspca_dev->sd_desc->ctrls[idx];
 
-	if (mutex_lock_interruptible(&gspca_dev->usb_lock))
-		return -ERESTARTSYS;
-	if (!gspca_dev->present) {
-		ret = -ENODEV;
-		goto out;
-	}
 	gspca_dev->usb_err = 0;
-	if (ctrls->get != NULL) {
-		ret = ctrls->get(gspca_dev, &ctrl->value);
-		goto out;
-	}
+	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;
-	ret = 0;
-out:
-	mutex_unlock(&gspca_dev->usb_lock);
-	return ret;
+	return 0;
 }
 
 static int vidioc_querymenu(struct file *file, void *priv,
 			    struct v4l2_querymenu *qmenu)
 {
-	struct gspca_dev *gspca_dev = priv;
+	struct gspca_dev *gspca_dev = video_drvdata(file);
 
 	if (!gspca_dev->sd_desc->querymenu)
-		return -EINVAL;
+		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)
 {
-	struct gspca_dev *gspca_dev = priv;
+	struct gspca_dev *gspca_dev = video_drvdata(file);
 
 	if (input->index != 0)
 		return -EINVAL;
@@ -1595,7 +1528,7 @@ static int vidioc_s_input(struct file *file, void *priv, unsigned int i)
 static int vidioc_reqbufs(struct file *file, void *priv,
 			  struct v4l2_requestbuffers *rb)
 {
-	struct gspca_dev *gspca_dev = priv;
+	struct gspca_dev *gspca_dev = video_drvdata(file);
 	int i, ret = 0, streaming;
 
 	i = rb->memory;			/* (avoid compilation warning) */
@@ -1635,10 +1568,7 @@ static int vidioc_reqbufs(struct file *file, void *priv,
 	/* stop streaming */
 	streaming = gspca_dev->streaming;
 	if (streaming) {
-		mutex_lock(&gspca_dev->usb_lock);
-		gspca_dev->usb_err = 0;
 		gspca_stream_off(gspca_dev);
-		mutex_unlock(&gspca_dev->usb_lock);
 
 		/* Don't restart the stream when switching from read
 		 * to mmap mode */
@@ -1666,7 +1596,7 @@ out:
 static int vidioc_querybuf(struct file *file, void *priv,
 			   struct v4l2_buffer *v4l2_buf)
 {
-	struct gspca_dev *gspca_dev = priv;
+	struct gspca_dev *gspca_dev = video_drvdata(file);
 	struct gspca_frame *frame;
 
 	if (v4l2_buf->index < 0
@@ -1681,7 +1611,7 @@ static int vidioc_querybuf(struct file *file, void *priv,
 static int vidioc_streamon(struct file *file, void *priv,
 			   enum v4l2_buf_type buf_type)
 {
-	struct gspca_dev *gspca_dev = priv;
+	struct gspca_dev *gspca_dev = video_drvdata(file);
 	int ret;
 
 	if (buf_type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
@@ -1722,7 +1652,7 @@ out:
 static int vidioc_streamoff(struct file *file, void *priv,
 				enum v4l2_buf_type buf_type)
 {
-	struct gspca_dev *gspca_dev = priv;
+	struct gspca_dev *gspca_dev = video_drvdata(file);
 	int ret;
 
 	if (buf_type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
@@ -1743,13 +1673,7 @@ static int vidioc_streamoff(struct file *file, void *priv,
 	}
 
 	/* stop streaming */
-	if (mutex_lock_interruptible(&gspca_dev->usb_lock)) {
-		ret = -ERESTARTSYS;
-		goto out;
-	}
-	gspca_dev->usb_err = 0;
 	gspca_stream_off(gspca_dev);
-	mutex_unlock(&gspca_dev->usb_lock);
 	/* In case another thread is waiting in dqbuf */
 	wake_up_interruptible(&gspca_dev->wq);
 
@@ -1766,71 +1690,44 @@ out:
 static int vidioc_g_jpegcomp(struct file *file, void *priv,
 			struct v4l2_jpegcompression *jpegcomp)
 {
-	struct gspca_dev *gspca_dev = priv;
-	int ret;
+	struct gspca_dev *gspca_dev = video_drvdata(file);
 
 	if (!gspca_dev->sd_desc->get_jcomp)
-		return -EINVAL;
-	if (mutex_lock_interruptible(&gspca_dev->usb_lock))
-		return -ERESTARTSYS;
+		return -ENOTTY;
 	gspca_dev->usb_err = 0;
-	if (gspca_dev->present)
-		ret = gspca_dev->sd_desc->get_jcomp(gspca_dev, jpegcomp);
-	else
-		ret = -ENODEV;
-	mutex_unlock(&gspca_dev->usb_lock);
-	return ret;
+	return gspca_dev->sd_desc->get_jcomp(gspca_dev, jpegcomp);
 }
 
 static int vidioc_s_jpegcomp(struct file *file, void *priv,
 			struct v4l2_jpegcompression *jpegcomp)
 {
-	struct gspca_dev *gspca_dev = priv;
-	int ret;
+	struct gspca_dev *gspca_dev = video_drvdata(file);
 
 	if (!gspca_dev->sd_desc->set_jcomp)
-		return -EINVAL;
-	if (mutex_lock_interruptible(&gspca_dev->usb_lock))
-		return -ERESTARTSYS;
+		return -ENOTTY;
 	gspca_dev->usb_err = 0;
-	if (gspca_dev->present)
-		ret = gspca_dev->sd_desc->set_jcomp(gspca_dev, jpegcomp);
-	else
-		ret = -ENODEV;
-	mutex_unlock(&gspca_dev->usb_lock);
-	return ret;
+	return gspca_dev->sd_desc->set_jcomp(gspca_dev, jpegcomp);
 }
 
 static int vidioc_g_parm(struct file *filp, void *priv,
 			struct v4l2_streamparm *parm)
 {
-	struct gspca_dev *gspca_dev = priv;
+	struct gspca_dev *gspca_dev = video_drvdata(filp);
 
 	parm->parm.capture.readbuffers = gspca_dev->nbufread;
 
 	if (gspca_dev->sd_desc->get_streamparm) {
-		int ret;
-
-		if (mutex_lock_interruptible(&gspca_dev->usb_lock))
-			return -ERESTARTSYS;
-		if (gspca_dev->present) {
-			gspca_dev->usb_err = 0;
-			gspca_dev->sd_desc->get_streamparm(gspca_dev, parm);
-			ret = gspca_dev->usb_err;
-		} else {
-			ret = -ENODEV;
-		}
-		mutex_unlock(&gspca_dev->usb_lock);
-		return ret;
+		gspca_dev->usb_err = 0;
+		gspca_dev->sd_desc->get_streamparm(gspca_dev, parm);
+		return gspca_dev->usb_err;
 	}
-
 	return 0;
 }
 
 static int vidioc_s_parm(struct file *filp, void *priv,
 			struct v4l2_streamparm *parm)
 {
-	struct gspca_dev *gspca_dev = priv;
+	struct gspca_dev *gspca_dev = video_drvdata(filp);
 	int n;
 
 	n = parm->parm.capture.readbuffers;
@@ -1840,19 +1737,9 @@ static int vidioc_s_parm(struct file *filp, void *priv,
 		gspca_dev->nbufread = n;
 
 	if (gspca_dev->sd_desc->set_streamparm) {
-		int ret;
-
-		if (mutex_lock_interruptible(&gspca_dev->usb_lock))
-			return -ERESTARTSYS;
-		if (gspca_dev->present) {
-			gspca_dev->usb_err = 0;
-			gspca_dev->sd_desc->set_streamparm(gspca_dev, parm);
-			ret = gspca_dev->usb_err;
-		} else {
-			ret = -ENODEV;
-		}
-		mutex_unlock(&gspca_dev->usb_lock);
-		return ret;
+		gspca_dev->usb_err = 0;
+		gspca_dev->sd_desc->set_streamparm(gspca_dev, parm);
+		return gspca_dev->usb_err;
 	}
 
 	return 0;
@@ -1860,7 +1747,7 @@ static int vidioc_s_parm(struct file *filp, void *priv,
 
 static int dev_mmap(struct file *file, struct vm_area_struct *vma)
 {
-	struct gspca_dev *gspca_dev = file->private_data;
+	struct gspca_dev *gspca_dev = video_drvdata(file);
 	struct gspca_frame *frame;
 	struct page *page;
 	unsigned long addr, start, size;
@@ -1872,10 +1759,6 @@ static int dev_mmap(struct file *file, struct vm_area_struct *vma)
 
 	if (mutex_lock_interruptible(&gspca_dev->queue_lock))
 		return -ERESTARTSYS;
-	if (!gspca_dev->present) {
-		ret = -ENODEV;
-		goto out;
-	}
 	if (gspca_dev->capt_file != file) {
 		ret = -EINVAL;
 		goto out;
@@ -1963,7 +1846,7 @@ static int frame_ready(struct gspca_dev *gspca_dev, struct file *file,
 static int vidioc_dqbuf(struct file *file, void *priv,
 			struct v4l2_buffer *v4l2_buf)
 {
-	struct gspca_dev *gspca_dev = priv;
+	struct gspca_dev *gspca_dev = video_drvdata(file);
 	struct gspca_frame *frame;
 	int i, j, ret;
 
@@ -2003,14 +1886,6 @@ static int vidioc_dqbuf(struct file *file, void *priv,
 
 	gspca_dev->fr_o = (i + 1) % GSPCA_MAX_FRAMES;
 
-	if (gspca_dev->sd_desc->dq_callback) {
-		mutex_lock(&gspca_dev->usb_lock);
-		gspca_dev->usb_err = 0;
-		if (gspca_dev->present)
-			gspca_dev->sd_desc->dq_callback(gspca_dev);
-		mutex_unlock(&gspca_dev->usb_lock);
-	}
-
 	frame->v4l2_buf.flags &= ~V4L2_BUF_FLAG_DONE;
 	memcpy(v4l2_buf, &frame->v4l2_buf, sizeof *v4l2_buf);
 	PDEBUG(D_FRAM, "dqbuf %d", j);
@@ -2027,6 +1902,15 @@ static int vidioc_dqbuf(struct file *file, void *priv,
 	}
 out:
 	mutex_unlock(&gspca_dev->queue_lock);
+
+	if (ret == 0 && gspca_dev->sd_desc->dq_callback) {
+		mutex_lock(&gspca_dev->usb_lock);
+		gspca_dev->usb_err = 0;
+		if (gspca_dev->present)
+			gspca_dev->sd_desc->dq_callback(gspca_dev);
+		mutex_unlock(&gspca_dev->usb_lock);
+	}
+
 	return ret;
 }
 
@@ -2039,7 +1923,7 @@ out:
 static int vidioc_qbuf(struct file *file, void *priv,
 			struct v4l2_buffer *v4l2_buf)
 {
-	struct gspca_dev *gspca_dev = priv;
+	struct gspca_dev *gspca_dev = video_drvdata(file);
 	struct gspca_frame *frame;
 	int i, index, ret;
 
@@ -2098,6 +1982,10 @@ static int read_alloc(struct gspca_dev *gspca_dev,
 	int i, ret;
 
 	PDEBUG(D_STREAM, "read alloc");
+
+	if (mutex_lock_interruptible(&gspca_dev->usb_lock))
+		return -ERESTARTSYS;
+
 	if (gspca_dev->nframes == 0) {
 		struct v4l2_requestbuffers rb;
 
@@ -2108,7 +1996,7 @@ static int read_alloc(struct gspca_dev *gspca_dev,
 		ret = vidioc_reqbufs(file, gspca_dev, &rb);
 		if (ret != 0) {
 			PDEBUG(D_STREAM, "read reqbuf err %d", ret);
-			return ret;
+			goto out;
 		}
 		memset(&v4l2_buf, 0, sizeof v4l2_buf);
 		v4l2_buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
@@ -2118,61 +2006,69 @@ static int read_alloc(struct gspca_dev *gspca_dev,
 			ret = vidioc_qbuf(file, gspca_dev, &v4l2_buf);
 			if (ret != 0) {
 				PDEBUG(D_STREAM, "read qbuf err: %d", ret);
-				return ret;
+				goto out;
 			}
 		}
-		gspca_dev->memory = GSPCA_MEMORY_READ;
 	}
 
 	/* start streaming */
 	ret = vidioc_streamon(file, gspca_dev, V4L2_BUF_TYPE_VIDEO_CAPTURE);
 	if (ret != 0)
 		PDEBUG(D_STREAM, "read streamon err %d", ret);
+out:
+	mutex_unlock(&gspca_dev->usb_lock);
 	return ret;
 }
 
 static unsigned int dev_poll(struct file *file, poll_table *wait)
 {
-	struct gspca_dev *gspca_dev = file->private_data;
-	int ret;
+	struct gspca_dev *gspca_dev = video_drvdata(file);
+	unsigned long req_events = poll_requested_events(wait);
+	int ret = 0;
 
 	PDEBUG(D_FRAM, "poll");
 
-	poll_wait(file, &gspca_dev->wq, wait);
+	if (req_events & POLLPRI)
+		ret |= v4l2_ctrl_poll(file, wait);
 
-	/* if reqbufs is not done, the user would use read() */
-	if (gspca_dev->memory == GSPCA_MEMORY_NO) {
-		ret = read_alloc(gspca_dev, file);
-		if (ret != 0)
-			return POLLERR;
-	}
+	if (req_events & (POLLIN | POLLRDNORM)) {
+		/* if reqbufs is not done, the user would use read() */
+		if (gspca_dev->memory == GSPCA_MEMORY_NO) {
+			if (read_alloc(gspca_dev, file) != 0) {
+				ret |= POLLERR;
+				goto out;
+			}
+		}
 
-	if (mutex_lock_interruptible(&gspca_dev->queue_lock) != 0)
-		return POLLERR;
+		poll_wait(file, &gspca_dev->wq, wait);
 
-	/* check if an image has been received */
-	if (gspca_dev->fr_o != atomic_read(&gspca_dev->fr_i))
-		ret = POLLIN | POLLRDNORM;	/* yes */
-	else
-		ret = 0;
-	mutex_unlock(&gspca_dev->queue_lock);
+		/* check if an image has been received */
+		if (mutex_lock_interruptible(&gspca_dev->queue_lock) != 0) {
+			ret |= POLLERR;
+			goto out;
+		}
+		if (gspca_dev->fr_o != atomic_read(&gspca_dev->fr_i))
+			ret |= POLLIN | POLLRDNORM;
+		mutex_unlock(&gspca_dev->queue_lock);
+	}
+
+out:
 	if (!gspca_dev->present)
-		return POLLHUP;
+		ret |= POLLHUP;
+
 	return ret;
 }
 
 static ssize_t dev_read(struct file *file, char __user *data,
 		    size_t count, loff_t *ppos)
 {
-	struct gspca_dev *gspca_dev = file->private_data;
+	struct gspca_dev *gspca_dev = video_drvdata(file);
 	struct gspca_frame *frame;
 	struct v4l2_buffer v4l2_buf;
 	struct timeval timestamp;
 	int n, ret, ret2;
 
 	PDEBUG(D_FRAM, "read (%zd)", count);
-	if (!gspca_dev->present)
-		return -ENODEV;
 	if (gspca_dev->memory == GSPCA_MEMORY_NO) { /* first time ? */
 		ret = read_alloc(gspca_dev, file);
 		if (ret != 0)
@@ -2266,13 +2162,15 @@ static const struct v4l2_ioctl_ops dev_ioctl_ops = {
 	.vidioc_s_register	= vidioc_s_register,
 #endif
 	.vidioc_g_chip_ident	= vidioc_g_chip_ident,
+	.vidioc_subscribe_event = v4l2_ctrl_subscribe_event,
+	.vidioc_unsubscribe_event = v4l2_event_unsubscribe,
 };
 
 static const struct video_device gspca_template = {
 	.name = "gspca main driver",
 	.fops = &dev_fops,
 	.ioctl_ops = &dev_ioctl_ops,
-	.release = gspca_release,
+	.release = video_device_release_empty, /* We use v4l2_dev.release */
 };
 
 /* initialize the controls */
@@ -2344,9 +2242,24 @@ int gspca_dev_probe2(struct usb_interface *intf,
 		}
 	}
 
+	gspca_dev->v4l2_dev.release = gspca_release;
+	ret = v4l2_device_register(&intf->dev, &gspca_dev->v4l2_dev);
+	if (ret)
+		goto out;
 	gspca_dev->sd_desc = sd_desc;
 	gspca_dev->nbufread = 2;
 	gspca_dev->empty_packet = -1;	/* don't check the empty packets */
+	gspca_dev->vdev = gspca_template;
+	gspca_dev->vdev.v4l2_dev = &gspca_dev->v4l2_dev;
+	video_set_drvdata(&gspca_dev->vdev, gspca_dev);
+	set_bit(V4L2_FL_USE_FH_PRIO, &gspca_dev->vdev.flags);
+	gspca_dev->module = module;
+	gspca_dev->present = 1;
+
+	mutex_init(&gspca_dev->usb_lock);
+	gspca_dev->vdev.lock = &gspca_dev->usb_lock;
+	mutex_init(&gspca_dev->queue_lock);
+	init_waitqueue_head(&gspca_dev->wq);
 
 	/* configure the subdriver and initialize the USB device */
 	ret = sd_desc->config(gspca_dev, id);
@@ -2357,21 +2270,26 @@ int gspca_dev_probe2(struct usb_interface *intf,
 	ret = sd_desc->init(gspca_dev);
 	if (ret < 0)
 		goto out;
+	if (sd_desc->init_controls)
+		ret = sd_desc->init_controls(gspca_dev);
+	if (ret < 0)
+		goto out;
 	gspca_set_default_mode(gspca_dev);
 
 	ret = gspca_input_connect(gspca_dev);
 	if (ret)
 		goto out;
 
-	mutex_init(&gspca_dev->usb_lock);
-	mutex_init(&gspca_dev->queue_lock);
-	init_waitqueue_head(&gspca_dev->wq);
+	/*
+	 * Don't take usb_lock for these ioctls. This improves latency if
+	 * usb_lock is taken for a long time, e.g. when changing a control
+	 * value, and a new frame is ready to be dequeued.
+	 */
+	v4l2_disable_ioctl_locking(&gspca_dev->vdev, VIDIOC_DQBUF);
+	v4l2_disable_ioctl_locking(&gspca_dev->vdev, VIDIOC_QBUF);
+	v4l2_disable_ioctl_locking(&gspca_dev->vdev, VIDIOC_QUERYBUF);
 
 	/* init video stuff */
-	memcpy(&gspca_dev->vdev, &gspca_template, sizeof gspca_template);
-	gspca_dev->vdev.parent = &intf->dev;
-	gspca_dev->module = module;
-	gspca_dev->present = 1;
 	ret = video_register_device(&gspca_dev->vdev,
 				  VFL_TYPE_GRABBER,
 				  -1);
@@ -2391,6 +2309,7 @@ out:
 	if (gspca_dev->input_dev)
 		input_unregister_device(gspca_dev->input_dev);
 #endif
+	v4l2_ctrl_handler_free(gspca_dev->vdev.ctrl_handler);
 	kfree(gspca_dev->usb_buf);
 	kfree(gspca_dev);
 	return ret;
@@ -2437,11 +2356,12 @@ void gspca_disconnect(struct usb_interface *intf)
 
 	PDEBUG(D_PROBE, "%s disconnect",
 		video_device_node_name(&gspca_dev->vdev));
+
 	mutex_lock(&gspca_dev->usb_lock);
 
+	usb_set_intfdata(intf, NULL);
+	gspca_dev->dev = NULL;
 	gspca_dev->present = 0;
-	wake_up_interruptible(&gspca_dev->wq);
-
 	destroy_urbs(gspca_dev);
 
 #if defined(CONFIG_INPUT) || defined(CONFIG_INPUT_MODULE)
@@ -2452,18 +2372,19 @@ void gspca_disconnect(struct usb_interface *intf)
 		input_unregister_device(input_dev);
 	}
 #endif
+	/* Free subdriver's streaming resources / stop sd workqueue(s) */
+	if (gspca_dev->sd_desc->stop0 && gspca_dev->streaming)
+		gspca_dev->sd_desc->stop0(gspca_dev);
+	gspca_dev->streaming = 0;
+	wake_up_interruptible(&gspca_dev->wq);
 
-	/* the device is freed at exit of this function */
-	gspca_dev->dev = NULL;
-	mutex_unlock(&gspca_dev->usb_lock);
+	v4l2_device_disconnect(&gspca_dev->v4l2_dev);
+	video_unregister_device(&gspca_dev->vdev);
 
-	usb_set_intfdata(intf, NULL);
+	mutex_unlock(&gspca_dev->usb_lock);
 
-	/* release the device */
 	/* (this will call gspca_release() immediately or on last close) */
-	video_unregister_device(&gspca_dev->vdev);
-
-/*	PDEBUG(D_PROBE, "disconnect complete"); */
+	v4l2_device_put(&gspca_dev->v4l2_dev);
 }
 EXPORT_SYMBOL(gspca_disconnect);
 
@@ -2474,7 +2395,9 @@ int gspca_suspend(struct usb_interface *intf, pm_message_t message)
 
 	if (!gspca_dev->streaming)
 		return 0;
+	mutex_lock(&gspca_dev->usb_lock);
 	gspca_dev->frozen = 1;		/* avoid urb error messages */
+	gspca_dev->usb_err = 0;
 	if (gspca_dev->sd_desc->stopN)
 		gspca_dev->sd_desc->stopN(gspca_dev);
 	destroy_urbs(gspca_dev);
@@ -2482,6 +2405,7 @@ int gspca_suspend(struct usb_interface *intf, pm_message_t message)
 	gspca_set_alt0(gspca_dev);
 	if (gspca_dev->sd_desc->stop0)
 		gspca_dev->sd_desc->stop0(gspca_dev);
+	mutex_unlock(&gspca_dev->usb_lock);
 	return 0;
 }
 EXPORT_SYMBOL(gspca_suspend);
@@ -2489,105 +2413,28 @@ EXPORT_SYMBOL(gspca_suspend);
 int gspca_resume(struct usb_interface *intf)
 {
 	struct gspca_dev *gspca_dev = usb_get_intfdata(intf);
+	int streaming, ret = 0;
 
+	mutex_lock(&gspca_dev->usb_lock);
 	gspca_dev->frozen = 0;
+	gspca_dev->usb_err = 0;
 	gspca_dev->sd_desc->init(gspca_dev);
 	gspca_input_create_urb(gspca_dev);
-	if (gspca_dev->streaming)
-		return gspca_init_transfer(gspca_dev);
-	return 0;
+	/*
+	 * Most subdrivers send all ctrl values on sd_start and thus
+	 * only write to the device registers on s_ctrl when streaming ->
+	 * Clear streaming to avoid setting all ctrls twice.
+	 */
+	streaming = gspca_dev->streaming;
+	gspca_dev->streaming = 0;
+	v4l2_ctrl_handler_setup(gspca_dev->vdev.ctrl_handler);
+	if (streaming)
+		ret = gspca_init_transfer(gspca_dev);
+	mutex_unlock(&gspca_dev->usb_lock);
+	return ret;
 }
 EXPORT_SYMBOL(gspca_resume);
 #endif
-/* -- cam driver utility functions -- */
-
-/* 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. */
-int gspca_auto_gain_n_exposure(struct gspca_dev *gspca_dev, int avg_lum,
-	int desired_avg_lum, int deadzone, int gain_knee, int exposure_knee)
-{
-	int i, steps, gain, orig_gain, exposure, orig_exposure, autogain;
-	const struct ctrl *gain_ctrl = NULL;
-	const struct ctrl *exposure_ctrl = NULL;
-	const struct ctrl *autogain_ctrl = NULL;
-	int retval = 0;
-
-	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 == V4L2_CID_GAIN)
-			gain_ctrl = &gspca_dev->sd_desc->ctrls[i];
-		if (gspca_dev->sd_desc->ctrls[i].qctrl.id == V4L2_CID_EXPOSURE)
-			exposure_ctrl = &gspca_dev->sd_desc->ctrls[i];
-		if (gspca_dev->sd_desc->ctrls[i].qctrl.id == V4L2_CID_AUTOGAIN)
-			autogain_ctrl = &gspca_dev->sd_desc->ctrls[i];
-	}
-	if (!gain_ctrl || !exposure_ctrl || !autogain_ctrl) {
-		PDEBUG(D_ERR, "Error: gspca_auto_gain_n_exposure called "
-			"on cam without (auto)gain/exposure");
-		return 0;
-	}
-
-	if (gain_ctrl->get(gspca_dev, &gain) ||
-			exposure_ctrl->get(gspca_dev, &exposure) ||
-			autogain_ctrl->get(gspca_dev, &autogain) || !autogain)
-		return 0;
-
-	orig_gain = gain;
-	orig_exposure = exposure;
-
-	/* 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 > gain_ctrl->qctrl.default_value)
-				gain--;
-			else if (exposure > exposure_ctrl->qctrl.minimum)
-				exposure--;
-			else if (gain > gain_ctrl->qctrl.minimum)
-				gain--;
-			else
-				break;
-		} else {
-			if (gain < gain_ctrl->qctrl.default_value)
-				gain++;
-			else if (exposure < exposure_knee)
-				exposure++;
-			else if (gain < gain_knee)
-				gain++;
-			else if (exposure < exposure_ctrl->qctrl.maximum)
-				exposure++;
-			else if (gain < gain_ctrl->qctrl.maximum)
-				gain++;
-			else
-				break;
-		}
-	}
-
-	if (gain != orig_gain) {
-		gain_ctrl->set(gspca_dev, gain);
-		retval = 1;
-	}
-	if (exposure != orig_exposure) {
-		exposure_ctrl->set(gspca_dev, exposure);
-		retval = 1;
-	}
-
-	return retval;
-}
-EXPORT_SYMBOL(gspca_auto_gain_n_exposure);
 
 /* -- module insert / remove -- */
 static int __init gspca_init(void)
diff --git a/drivers/media/video/gspca/gspca.h b/drivers/media/video/gspca/gspca.h
index 589009f4496..dc688c7f5e4 100644
--- a/drivers/media/video/gspca/gspca.h
+++ b/drivers/media/video/gspca/gspca.h
@@ -6,6 +6,8 @@
 #include <linux/usb.h>
 #include <linux/videodev2.h>
 #include <media/v4l2-common.h>
+#include <media/v4l2-ctrls.h>
+#include <media/v4l2-device.h>
 #include <linux/mutex.h>
 
 /* compilation option */
@@ -115,6 +117,7 @@ struct sd_desc {
 /* mandatory operations */
 	cam_cf_op config;	/* called on probe */
 	cam_op init;		/* called on probe and resume */
+	cam_op init_controls;	/* called on probe */
 	cam_op start;		/* called on stream on after URBs creation */
 	cam_pkt_op pkt_scan;
 /* optional operations */
@@ -158,8 +161,10 @@ struct gspca_frame {
 struct gspca_dev {
 	struct video_device vdev;	/* !! must be the first item */
 	struct module *module;		/* subdriver handling the device */
+	struct v4l2_device v4l2_dev;
 	struct usb_device *dev;
 	struct file *capt_file;		/* file doing video capture */
+					/* protected by queue_lock */
 #if defined(CONFIG_INPUT) || defined(CONFIG_INPUT_MODULE)
 	struct input_dev *input_dev;
 	char phys[64];			/* physical device path */
@@ -169,6 +174,16 @@ struct gspca_dev {
 	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
+	   the autogain/exposure functions in autogain_functions.c use them */
+	struct {
+		struct v4l2_ctrl *autogain;
+		struct v4l2_ctrl *exposure;
+		struct v4l2_ctrl *gain;
+		int exp_too_low_cnt, exp_too_high_cnt;
+	};
 
 #define USB_BUF_SZ 64
 	__u8 *usb_buf;				/* buffer for USB exchanges */
@@ -189,7 +204,7 @@ struct gspca_dev {
 	u8 fr_o;				/* next frame to dequeue */
 	__u8 last_packet_type;
 	__s8 empty_packet;		/* if (-1) don't check empty packets */
-	__u8 streaming;
+	__u8 streaming;			/* protected by both mutexes (*) */
 
 	__u8 curr_mode;			/* current camera mode */
 	__u32 pixfmt;			/* current mode parameters */
@@ -211,6 +226,10 @@ struct gspca_dev {
 	__u8 iface;			/* USB interface number */
 	__u8 alt;			/* USB alternate setting */
 	u8 audio;			/* presence of audio device */
+
+	/* (*) These variables are proteced by both usb_lock and queue_lock,
+	   that is any code setting them is holding *both*, which means that
+	   any code getting them needs to hold at least one of them */
 };
 
 int gspca_dev_probe(struct usb_interface *intf,
@@ -232,6 +251,9 @@ void gspca_frame_add(struct gspca_dev *gspca_dev,
 int gspca_suspend(struct usb_interface *intf, pm_message_t message);
 int gspca_resume(struct usb_interface *intf);
 #endif
-int gspca_auto_gain_n_exposure(struct gspca_dev *gspca_dev, int avg_lum,
+int gspca_expo_autogain(struct gspca_dev *gspca_dev, int avg_lum,
 	int desired_avg_lum, int deadzone, int gain_knee, int exposure_knee);
+int gspca_coarse_grained_expo_autogain(struct gspca_dev *gspca_dev,
+        int avg_lum, int desired_avg_lum, int deadzone);
+
 #endif /* GSPCAV2_H */
diff --git a/drivers/media/video/gspca/jl2005bcd.c b/drivers/media/video/gspca/jl2005bcd.c
index 53f58ef367c..9c591c7c6f5 100644
--- a/drivers/media/video/gspca/jl2005bcd.c
+++ b/drivers/media/video/gspca/jl2005bcd.c
@@ -335,7 +335,11 @@ static void jl2005c_dostream(struct work_struct *work)
 		goto quit_stream;
 	}
 
-	while (gspca_dev->present && gspca_dev->streaming) {
+	while (gspca_dev->dev && gspca_dev->streaming) {
+#ifdef CONFIG_PM
+		if (gspca_dev->frozen)
+			break;
+#endif
 		/* Check if this is a new frame. If so, start the frame first */
 		if (!header_read) {
 			mutex_lock(&gspca_dev->usb_lock);
@@ -367,7 +371,7 @@ static void jl2005c_dostream(struct work_struct *work)
 					buffer, act_len);
 			header_read = 1;
 		}
-		while (bytes_left > 0 && gspca_dev->present) {
+		while (bytes_left > 0 && gspca_dev->dev) {
 			data_len = bytes_left > JL2005C_MAX_TRANSFER ?
 				JL2005C_MAX_TRANSFER : bytes_left;
 			ret = usb_bulk_msg(gspca_dev->dev,
@@ -390,7 +394,7 @@ static void jl2005c_dostream(struct work_struct *work)
 		}
 	}
 quit_stream:
-	if (gspca_dev->present) {
+	if (gspca_dev->dev) {
 		mutex_lock(&gspca_dev->usb_lock);
 		jl2005c_stop(gspca_dev);
 		mutex_unlock(&gspca_dev->usb_lock);
diff --git a/drivers/media/video/gspca/mars.c b/drivers/media/video/gspca/mars.c
index b0231465afa..ec7b21ee79f 100644
--- a/drivers/media/video/gspca/mars.c
+++ b/drivers/media/video/gspca/mars.c
@@ -30,22 +30,19 @@ MODULE_AUTHOR("Michel Xhaard <mxhaard@users.sourceforge.net>");
 MODULE_DESCRIPTION("GSPCA/Mars USB Camera Driver");
 MODULE_LICENSE("GPL");
 
-/* controls */
-enum e_ctrl {
-	BRIGHTNESS,
-	COLORS,
-	GAMMA,
-	SHARPNESS,
-	ILLUM_TOP,
-	ILLUM_BOT,
-	NCTRLS		/* number of controls */
-};
-
 /* specific webcam descriptor */
 struct sd {
 	struct gspca_dev gspca_dev;	/* !! must be the first item */
 
-	struct gspca_ctrl ctrls[NCTRLS];
+	struct v4l2_ctrl *brightness;
+	struct v4l2_ctrl *saturation;
+	struct v4l2_ctrl *sharpness;
+	struct v4l2_ctrl *gamma;
+	struct { /* illuminator control cluster */
+		struct v4l2_ctrl *illum_top;
+		struct v4l2_ctrl *illum_bottom;
+	};
+	struct v4l2_ctrl *jpegqual;
 
 	u8 quality;
 #define QUALITY_MIN 40
@@ -56,89 +53,10 @@ struct sd {
 };
 
 /* V4L2 controls supported by the driver */
-static void setbrightness(struct gspca_dev *gspca_dev);
-static void setcolors(struct gspca_dev *gspca_dev);
-static void setgamma(struct gspca_dev *gspca_dev);
-static void setsharpness(struct gspca_dev *gspca_dev);
-static int sd_setilluminator1(struct gspca_dev *gspca_dev, __s32 val);
-static int sd_setilluminator2(struct gspca_dev *gspca_dev, __s32 val);
-
-static const struct ctrl sd_ctrls[NCTRLS] = {
-[BRIGHTNESS] = {
-	    {
-		.id      = V4L2_CID_BRIGHTNESS,
-		.type    = V4L2_CTRL_TYPE_INTEGER,
-		.name    = "Brightness",
-		.minimum = 0,
-		.maximum = 30,
-		.step    = 1,
-		.default_value = 15,
-	    },
-	    .set_control = setbrightness
-	},
-[COLORS] = {
-	    {
-		.id      = V4L2_CID_SATURATION,
-		.type    = V4L2_CTRL_TYPE_INTEGER,
-		.name    = "Color",
-		.minimum = 1,
-		.maximum = 255,
-		.step    = 1,
-		.default_value = 200,
-	    },
-	    .set_control = setcolors
-	},
-[GAMMA] = {
-	    {
-		.id      = V4L2_CID_GAMMA,
-		.type    = V4L2_CTRL_TYPE_INTEGER,
-		.name    = "Gamma",
-		.minimum = 0,
-		.maximum = 3,
-		.step    = 1,
-		.default_value = 1,
-	    },
-	    .set_control = setgamma
-	},
-[SHARPNESS] = {
-	    {
-		.id	 = V4L2_CID_SHARPNESS,
-		.type    = V4L2_CTRL_TYPE_INTEGER,
-		.name    = "Sharpness",
-		.minimum = 0,
-		.maximum = 2,
-		.step    = 1,
-		.default_value = 1,
-	    },
-	    .set_control = setsharpness
-	},
-[ILLUM_TOP] = {
-	    {
-		.id	 = V4L2_CID_ILLUMINATORS_1,
-		.type    = V4L2_CTRL_TYPE_BOOLEAN,
-		.name    = "Top illuminator",
-		.minimum = 0,
-		.maximum = 1,
-		.step    = 1,
-		.default_value = 0,
-		.flags = V4L2_CTRL_FLAG_UPDATE,
-	    },
-	    .set = sd_setilluminator1
-	},
-[ILLUM_BOT] = {
-	    {
-		.id	 = V4L2_CID_ILLUMINATORS_2,
-		.type    = V4L2_CTRL_TYPE_BOOLEAN,
-		.name    = "Bottom illuminator",
-		.minimum = 0,
-		.maximum = 1,
-		.step    = 1,
-		.default_value = 0,
-		.flags = V4L2_CTRL_FLAG_UPDATE,
-	    },
-	    .set = sd_setilluminator2
-	},
-};
+static void setbrightness(struct gspca_dev *gspca_dev, s32 val);
+static void setcolors(struct gspca_dev *gspca_dev, s32 val);
+static void setgamma(struct gspca_dev *gspca_dev, s32 val);
+static void setsharpness(struct gspca_dev *gspca_dev, s32 val);
 
 static const struct v4l2_pix_format vga_mode[] = {
 	{320, 240, V4L2_PIX_FMT_JPEG, V4L2_FIELD_NONE,
@@ -198,59 +116,130 @@ static void mi_w(struct gspca_dev *gspca_dev,
 	reg_w(gspca_dev, 4);
 }
 
-static void setbrightness(struct gspca_dev *gspca_dev)
+static void setbrightness(struct gspca_dev *gspca_dev, s32 val)
 {
-	struct sd *sd = (struct sd *) gspca_dev;
-
 	gspca_dev->usb_buf[0] = 0x61;
-	gspca_dev->usb_buf[1] = sd->ctrls[BRIGHTNESS].val;
+	gspca_dev->usb_buf[1] = val;
 	reg_w(gspca_dev, 2);
 }
 
-static void setcolors(struct gspca_dev *gspca_dev)
+static void setcolors(struct gspca_dev *gspca_dev, s32 val)
 {
-	struct sd *sd = (struct sd *) gspca_dev;
-	s16 val;
-
-	val = sd->ctrls[COLORS].val;
 	gspca_dev->usb_buf[0] = 0x5f;
 	gspca_dev->usb_buf[1] = val << 3;
 	gspca_dev->usb_buf[2] = ((val >> 2) & 0xf8) | 0x04;
 	reg_w(gspca_dev, 3);
 }
 
-static void setgamma(struct gspca_dev *gspca_dev)
+static void setgamma(struct gspca_dev *gspca_dev, s32 val)
 {
-	struct sd *sd = (struct sd *) gspca_dev;
-
 	gspca_dev->usb_buf[0] = 0x06;
-	gspca_dev->usb_buf[1] = sd->ctrls[GAMMA].val * 0x40;
+	gspca_dev->usb_buf[1] = val * 0x40;
 	reg_w(gspca_dev, 2);
 }
 
-static void setsharpness(struct gspca_dev *gspca_dev)
+static void setsharpness(struct gspca_dev *gspca_dev, s32 val)
 {
-	struct sd *sd = (struct sd *) gspca_dev;
-
 	gspca_dev->usb_buf[0] = 0x67;
-	gspca_dev->usb_buf[1] = sd->ctrls[SHARPNESS].val * 4 + 3;
+	gspca_dev->usb_buf[1] = val * 4 + 3;
 	reg_w(gspca_dev, 2);
 }
 
-static void setilluminators(struct gspca_dev *gspca_dev)
+static void setilluminators(struct gspca_dev *gspca_dev, bool top, bool bottom)
 {
-	struct sd *sd = (struct sd *) gspca_dev;
-
+	/* both are off if not streaming */
 	gspca_dev->usb_buf[0] = 0x22;
-	if (sd->ctrls[ILLUM_TOP].val)
+	if (top)
 		gspca_dev->usb_buf[1] = 0x76;
-	else if (sd->ctrls[ILLUM_BOT].val)
+	else if (bottom)
 		gspca_dev->usb_buf[1] = 0x7a;
 	else
 		gspca_dev->usb_buf[1] = 0x7e;
 	reg_w(gspca_dev, 2);
 }
 
+static int mars_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;
+
+	gspca_dev->usb_err = 0;
+
+	if (ctrl->id == V4L2_CID_ILLUMINATORS_1) {
+		/* only one can be on at a time */
+		if (ctrl->is_new && ctrl->val)
+			sd->illum_bottom->val = 0;
+		if (sd->illum_bottom->is_new && sd->illum_bottom->val)
+			sd->illum_top->val = 0;
+	}
+
+	if (!gspca_dev->streaming)
+		return 0;
+
+	switch (ctrl->id) {
+	case V4L2_CID_BRIGHTNESS:
+		setbrightness(gspca_dev, ctrl->val);
+		break;
+	case V4L2_CID_SATURATION:
+		setcolors(gspca_dev, ctrl->val);
+		break;
+	case V4L2_CID_GAMMA:
+		setgamma(gspca_dev, ctrl->val);
+		break;
+	case V4L2_CID_ILLUMINATORS_1:
+		setilluminators(gspca_dev, sd->illum_top->val,
+					   sd->illum_bottom->val);
+		break;
+	case V4L2_CID_SHARPNESS:
+		setsharpness(gspca_dev, ctrl->val);
+		break;
+	case V4L2_CID_JPEG_COMPRESSION_QUALITY:
+		jpeg_set_qual(sd->jpeg_hdr, ctrl->val);
+		break;
+	default:
+		return -EINVAL;
+	}
+	return gspca_dev->usb_err;
+}
+
+static const struct v4l2_ctrl_ops mars_ctrl_ops = {
+	.s_ctrl = mars_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, 7);
+	sd->brightness = v4l2_ctrl_new_std(hdl, &mars_ctrl_ops,
+			V4L2_CID_BRIGHTNESS, 0, 30, 1, 15);
+	sd->saturation = v4l2_ctrl_new_std(hdl, &mars_ctrl_ops,
+			V4L2_CID_SATURATION, 0, 255, 1, 200);
+	sd->gamma = v4l2_ctrl_new_std(hdl, &mars_ctrl_ops,
+			V4L2_CID_GAMMA, 0, 3, 1, 1);
+	sd->sharpness = v4l2_ctrl_new_std(hdl, &mars_ctrl_ops,
+			V4L2_CID_SHARPNESS, 0, 2, 1, 1);
+	sd->illum_top = v4l2_ctrl_new_std(hdl, &mars_ctrl_ops,
+			V4L2_CID_ILLUMINATORS_1, 0, 1, 1, 0);
+	sd->illum_top->flags |= V4L2_CTRL_FLAG_UPDATE;
+	sd->illum_bottom = v4l2_ctrl_new_std(hdl, &mars_ctrl_ops,
+			V4L2_CID_ILLUMINATORS_2, 0, 1, 1, 0);
+	sd->illum_bottom->flags |= V4L2_CTRL_FLAG_UPDATE;
+	sd->jpegqual = v4l2_ctrl_new_std(hdl, &mars_ctrl_ops,
+			V4L2_CID_JPEG_COMPRESSION_QUALITY,
+			QUALITY_MIN, QUALITY_MAX, 1, QUALITY_DEF);
+	if (hdl->error) {
+		pr_err("Could not initialize controls\n");
+		return hdl->error;
+	}
+	v4l2_ctrl_cluster(2, &sd->illum_top);
+	return 0;
+}
+
 /* this function is called at probe time */
 static int sd_config(struct gspca_dev *gspca_dev,
 			const struct usb_device_id *id)
@@ -261,7 +250,6 @@ static int sd_config(struct gspca_dev *gspca_dev,
 	cam = &gspca_dev->cam;
 	cam->cam_mode = vga_mode;
 	cam->nmodes = ARRAY_SIZE(vga_mode);
-	cam->ctrls = sd->ctrls;
 	sd->quality = QUALITY_DEF;
 	return 0;
 }
@@ -269,7 +257,6 @@ static int sd_config(struct gspca_dev *gspca_dev,
 /* this function is called at probe and resume time */
 static int sd_init(struct gspca_dev *gspca_dev)
 {
-	gspca_dev->ctrl_inac = (1 << ILLUM_TOP) | (1 << ILLUM_BOT);
 	return 0;
 }
 
@@ -282,7 +269,7 @@ static int sd_start(struct gspca_dev *gspca_dev)
 	/* create the JPEG header */
 	jpeg_define(sd->jpeg_hdr, gspca_dev->height, gspca_dev->width,
 			0x21);		/* JPEG 422 */
-	jpeg_set_qual(sd->jpeg_hdr, sd->quality);
+	jpeg_set_qual(sd->jpeg_hdr, v4l2_ctrl_g_ctrl(sd->jpegqual));
 
 	data = gspca_dev->usb_buf;
 
@@ -301,7 +288,7 @@ static int sd_start(struct gspca_dev *gspca_dev)
 	data[5] = 0x30;		/* reg 4, MI, PAS5101 :
 				 *	0x30 for 24mhz , 0x28 for 12mhz */
 	data[6] = 0x02;		/* reg 5, H start - was 0x04 */
-	data[7] = sd->ctrls[GAMMA].val * 0x40;	/* reg 0x06: gamma */
+	data[7] = v4l2_ctrl_g_ctrl(sd->gamma) * 0x40;	/* reg 0x06: gamma */
 	data[8] = 0x01;		/* reg 7, V start - was 0x03 */
 /*	if (h_size == 320 ) */
 /*		data[9]= 0x56;	 * reg 8, 24MHz, 2:1 scale down */
@@ -333,16 +320,16 @@ static int sd_start(struct gspca_dev *gspca_dev)
 				/* reg 0x5f/0x60 (LE) = saturation */
 				/* h (60): xxxx x100
 				 * l (5f): xxxx x000 */
-	data[2] = sd->ctrls[COLORS].val << 3;
-	data[3] = ((sd->ctrls[COLORS].val >> 2) & 0xf8) | 0x04;
-	data[4] = sd->ctrls[BRIGHTNESS].val; /* reg 0x61 = brightness */
+	data[2] = v4l2_ctrl_g_ctrl(sd->saturation) << 3;
+	data[3] = ((v4l2_ctrl_g_ctrl(sd->saturation) >> 2) & 0xf8) | 0x04;
+	data[4] = v4l2_ctrl_g_ctrl(sd->brightness); /* reg 0x61 = brightness */
 	data[5] = 0x00;
 
 	reg_w(gspca_dev, 6);
 
 	data[0] = 0x67;
 /*jfm: from win trace*/
-	data[1] = sd->ctrls[SHARPNESS].val * 4 + 3;
+	data[1] = v4l2_ctrl_g_ctrl(sd->sharpness) * 4 + 3;
 	data[2] = 0x14;
 	reg_w(gspca_dev, 3);
 
@@ -365,7 +352,9 @@ static int sd_start(struct gspca_dev *gspca_dev)
 	data[1] = 0x4d;		/* ISOC transferring enable... */
 	reg_w(gspca_dev, 2);
 
-	gspca_dev->ctrl_inac = 0; /* activate the illuminator controls */
+	setilluminators(gspca_dev, v4l2_ctrl_g_ctrl(sd->illum_top),
+				   v4l2_ctrl_g_ctrl(sd->illum_bottom));
+
 	return gspca_dev->usb_err;
 }
 
@@ -373,11 +362,9 @@ static void sd_stopN(struct gspca_dev *gspca_dev)
 {
 	struct sd *sd = (struct sd *) gspca_dev;
 
-	gspca_dev->ctrl_inac = (1 << ILLUM_TOP) | (1 << ILLUM_BOT);
-	if (sd->ctrls[ILLUM_TOP].val || sd->ctrls[ILLUM_BOT].val) {
-		sd->ctrls[ILLUM_TOP].val = 0;
-		sd->ctrls[ILLUM_BOT].val = 0;
-		setilluminators(gspca_dev);
+	if (v4l2_ctrl_g_ctrl(sd->illum_top) ||
+	    v4l2_ctrl_g_ctrl(sd->illum_bottom)) {
+		setilluminators(gspca_dev, false, false);
 		msleep(20);
 	}
 
@@ -424,43 +411,16 @@ static void sd_pkt_scan(struct gspca_dev *gspca_dev,
 	gspca_frame_add(gspca_dev, INTER_PACKET, data, len);
 }
 
-static int sd_setilluminator1(struct gspca_dev *gspca_dev, __s32 val)
-{
-	struct sd *sd = (struct sd *) gspca_dev;
-
-	/* only one illuminator may be on */
-	sd->ctrls[ILLUM_TOP].val = val;
-	if (val)
-		sd->ctrls[ILLUM_BOT].val = 0;
-	setilluminators(gspca_dev);
-	return gspca_dev->usb_err;
-}
-
-static int sd_setilluminator2(struct gspca_dev *gspca_dev, __s32 val)
-{
-	struct sd *sd = (struct sd *) gspca_dev;
-
-	/* only one illuminator may be on */
-	sd->ctrls[ILLUM_BOT].val = val;
-	if (val)
-		sd->ctrls[ILLUM_TOP].val = 0;
-	setilluminators(gspca_dev);
-	return gspca_dev->usb_err;
-}
-
 static int sd_set_jcomp(struct gspca_dev *gspca_dev,
 			struct v4l2_jpegcompression *jcomp)
 {
 	struct sd *sd = (struct sd *) gspca_dev;
+	int ret;
 
-	if (jcomp->quality < QUALITY_MIN)
-		sd->quality = QUALITY_MIN;
-	else if (jcomp->quality > QUALITY_MAX)
-		sd->quality = QUALITY_MAX;
-	else
-		sd->quality = jcomp->quality;
-	if (gspca_dev->streaming)
-		jpeg_set_qual(sd->jpeg_hdr, sd->quality);
+	ret = v4l2_ctrl_s_ctrl(sd->jpegqual, jcomp->quality);
+	if (ret)
+		return ret;
+	jcomp->quality = v4l2_ctrl_g_ctrl(sd->jpegqual);
 	return 0;
 }
 
@@ -470,7 +430,7 @@ static int sd_get_jcomp(struct gspca_dev *gspca_dev,
 	struct sd *sd = (struct sd *) gspca_dev;
 
 	memset(jcomp, 0, sizeof *jcomp);
-	jcomp->quality = sd->quality;
+	jcomp->quality = v4l2_ctrl_g_ctrl(sd->jpegqual);
 	jcomp->jpeg_markers = V4L2_JPEG_MARKER_DHT
 			| V4L2_JPEG_MARKER_DQT;
 	return 0;
@@ -479,10 +439,9 @@ static int sd_get_jcomp(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,
 	.pkt_scan = sd_pkt_scan,
@@ -513,6 +472,7 @@ static struct usb_driver sd_driver = {
 #ifdef CONFIG_PM
 	.suspend = gspca_suspend,
 	.resume = gspca_resume,
+	.reset_resume = gspca_resume,
 #endif
 };
 
diff --git a/drivers/media/video/gspca/nw80x.c b/drivers/media/video/gspca/nw80x.c
index 7167cac7359..42e021931e6 100644
--- a/drivers/media/video/gspca/nw80x.c
+++ b/drivers/media/video/gspca/nw80x.c
@@ -2001,6 +2001,8 @@ static int sd_setautogain(struct gspca_dev *gspca_dev, __s32 val)
 	return gspca_dev->usb_err;
 }
 
+#define WANT_REGULAR_AUTOGAIN
+#define WANT_COARSE_EXPO_AUTOGAIN
 #include "autogain_functions.h"
 
 static void do_autogain(struct gspca_dev *gspca_dev)
diff --git a/drivers/media/video/gspca/ov519.c b/drivers/media/video/gspca/ov519.c
index 739e8a2a2d3..183457c5cfd 100644
--- a/drivers/media/video/gspca/ov519.c
+++ b/drivers/media/video/gspca/ov519.c
@@ -2804,7 +2804,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) {
-		PDEBUG(D_ERR, "Error detecting sensor type");
+		pr_err("Error detecting sensor type\n");
 		return;
 	}
 	if ((rc & 3) == 3) {
@@ -2832,12 +2832,12 @@ static void ov7xx0_configure(struct sd *sd)
 		/* try to read product id registers */
 		high = i2c_r(sd, 0x0a);
 		if (high < 0) {
-			PDEBUG(D_ERR, "Error detecting camera chip PID");
+			pr_err("Error detecting camera chip PID\n");
 			return;
 		}
 		low = i2c_r(sd, 0x0b);
 		if (low < 0) {
-			PDEBUG(D_ERR, "Error detecting camera chip VER");
+			pr_err("Error detecting camera chip VER\n");
 			return;
 		}
 		if (high == 0x76) {
@@ -2863,7 +2863,7 @@ static void ov7xx0_configure(struct sd *sd)
 				sd->sensor = SEN_OV7660;
 				break;
 			default:
-				PDEBUG(D_PROBE, "Unknown sensor: 0x76%x", low);
+				pr_err("Unknown sensor: 0x76%02x\n", low);
 				return;
 			}
 		} else {
@@ -2884,7 +2884,7 @@ static void ov6xx0_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");
+		pr_err("Error detecting sensor type\n");
 		return;
 	}
 
diff --git a/drivers/media/video/gspca/ov534.c b/drivers/media/video/gspca/ov534.c
index 04753391de3..b5acb1e4b4e 100644
--- a/drivers/media/video/gspca/ov534.c
+++ b/drivers/media/video/gspca/ov534.c
@@ -34,6 +34,8 @@
 
 #include "gspca.h"
 
+#include <linux/fixp-arith.h>
+
 #define OV534_REG_ADDRESS	0xf1	/* sensor address */
 #define OV534_REG_SUBADDR	0xf2
 #define OV534_REG_WRITE		0xf3
@@ -53,6 +55,8 @@ MODULE_LICENSE("GPL");
 
 /* controls */
 enum e_ctrl {
+	HUE,
+	SATURATION,
 	BRIGHTNESS,
 	CONTRAST,
 	GAIN,
@@ -63,7 +67,6 @@ enum e_ctrl {
 	SHARPNESS,
 	HFLIP,
 	VFLIP,
-	COLORS,
 	LIGHTFREQ,
 	NCTRLS		/* number of controls */
 };
@@ -87,6 +90,8 @@ enum sensors {
 };
 
 /* V4L2 controls supported by the driver */
+static void sethue(struct gspca_dev *gspca_dev);
+static void setsaturation(struct gspca_dev *gspca_dev);
 static void setbrightness(struct gspca_dev *gspca_dev);
 static void setcontrast(struct gspca_dev *gspca_dev);
 static void setgain(struct gspca_dev *gspca_dev);
@@ -96,13 +101,36 @@ static void setawb(struct gspca_dev *gspca_dev);
 static void setaec(struct gspca_dev *gspca_dev);
 static void setsharpness(struct gspca_dev *gspca_dev);
 static void sethvflip(struct gspca_dev *gspca_dev);
-static void setcolors(struct gspca_dev *gspca_dev);
 static void setlightfreq(struct gspca_dev *gspca_dev);
 
 static int sd_start(struct gspca_dev *gspca_dev);
 static void sd_stopN(struct gspca_dev *gspca_dev);
 
 static const struct ctrl sd_ctrls[] = {
+[HUE] = {
+		{
+			.id      = V4L2_CID_HUE,
+			.type    = V4L2_CTRL_TYPE_INTEGER,
+			.name    = "Hue",
+			.minimum = -90,
+			.maximum = 90,
+			.step    = 1,
+			.default_value = 0,
+		},
+		.set_control = sethue
+	},
+[SATURATION] = {
+		{
+			.id      = V4L2_CID_SATURATION,
+			.type    = V4L2_CTRL_TYPE_INTEGER,
+			.name    = "Saturation",
+			.minimum = 0,
+			.maximum = 255,
+			.step    = 1,
+			.default_value = 64,
+		},
+		.set_control = setsaturation
+	},
 [BRIGHTNESS] = {
 		{
 			.id      = V4L2_CID_BRIGHTNESS,
@@ -223,18 +251,6 @@ static const struct ctrl sd_ctrls[] = {
 		},
 		.set_control = sethvflip
 	},
-[COLORS] = {
-		{
-			.id      = V4L2_CID_SATURATION,
-			.type    = V4L2_CTRL_TYPE_INTEGER,
-			.name    = "Saturation",
-			.minimum = 0,
-			.maximum = 6,
-			.step    = 1,
-			.default_value = 3,
-		},
-		.set_control = setcolors
-	},
 [LIGHTFREQ] = {
 		{
 			.id      = V4L2_CID_POWER_LINE_FREQUENCY,
@@ -684,7 +700,7 @@ static const u8 sensor_init_772x[][2] = {
 	{ 0x9c, 0x20 },
 	{ 0x9e, 0x81 },
 
-	{ 0xa6, 0x04 },
+	{ 0xa6, 0x07 },
 	{ 0x7e, 0x0c },
 	{ 0x7f, 0x16 },
 	{ 0x80, 0x2a },
@@ -955,6 +971,74 @@ static void set_frame_rate(struct gspca_dev *gspca_dev)
 	PDEBUG(D_PROBE, "frame_rate: %d", r->fps);
 }
 
+static void sethue(struct gspca_dev *gspca_dev)
+{
+	struct sd *sd = (struct sd *) gspca_dev;
+	int val;
+
+	val = sd->ctrls[HUE].val;
+	if (sd->sensor == SENSOR_OV767x) {
+		/* TBD */
+	} else {
+		s16 huesin;
+		s16 huecos;
+
+		/* fixp_sin and fixp_cos accept only positive values, while
+		 * our val is between -90 and 90
+		 */
+		val += 360;
+
+		/* According to the datasheet the registers expect HUESIN and
+		 * HUECOS to be the result of the trigonometric functions,
+		 * scaled by 0x80.
+		 *
+		 * The 0x100 here represents the maximun absolute value
+		 * returned byt fixp_sin and fixp_cos, so the scaling will
+		 * consider the result like in the interval [-1.0, 1.0].
+		 */
+		huesin = fixp_sin(val) * 0x80 / 0x100;
+		huecos = fixp_cos(val) * 0x80 / 0x100;
+
+		if (huesin < 0) {
+			sccb_reg_write(gspca_dev, 0xab,
+				sccb_reg_read(gspca_dev, 0xab) | 0x2);
+			huesin = -huesin;
+		} else {
+			sccb_reg_write(gspca_dev, 0xab,
+				sccb_reg_read(gspca_dev, 0xab) & ~0x2);
+
+		}
+		sccb_reg_write(gspca_dev, 0xa9, (u8)huecos);
+		sccb_reg_write(gspca_dev, 0xaa, (u8)huesin);
+	}
+}
+
+static void setsaturation(struct gspca_dev *gspca_dev)
+{
+	struct sd *sd = (struct sd *) gspca_dev;
+	int val;
+
+	val = sd->ctrls[SATURATION].val;
+	if (sd->sensor == SENSOR_OV767x) {
+		int i;
+		static u8 color_tb[][6] = {
+			{0x42, 0x42, 0x00, 0x11, 0x30, 0x41},
+			{0x52, 0x52, 0x00, 0x16, 0x3c, 0x52},
+			{0x66, 0x66, 0x00, 0x1b, 0x4b, 0x66},
+			{0x80, 0x80, 0x00, 0x22, 0x5e, 0x80},
+			{0x9a, 0x9a, 0x00, 0x29, 0x71, 0x9a},
+			{0xb8, 0xb8, 0x00, 0x31, 0x87, 0xb8},
+			{0xdd, 0xdd, 0x00, 0x3b, 0xa2, 0xdd},
+		};
+
+		for (i = 0; i < ARRAY_SIZE(color_tb[0]); i++)
+			sccb_reg_write(gspca_dev, 0x4f + i, color_tb[val][i]);
+	} else {
+		sccb_reg_write(gspca_dev, 0xa7, val); /* U saturation */
+		sccb_reg_write(gspca_dev, 0xa8, val); /* V saturation */
+	}
+}
+
 static void setbrightness(struct gspca_dev *gspca_dev)
 {
 	struct sd *sd = (struct sd *) gspca_dev;
@@ -1132,26 +1216,6 @@ static void sethvflip(struct gspca_dev *gspca_dev)
 	}
 }
 
-static void setcolors(struct gspca_dev *gspca_dev)
-{
-	struct sd *sd = (struct sd *) gspca_dev;
-	u8 val;
-	int i;
-	static u8 color_tb[][6] = {
-		{0x42, 0x42, 0x00, 0x11, 0x30, 0x41},
-		{0x52, 0x52, 0x00, 0x16, 0x3c, 0x52},
-		{0x66, 0x66, 0x00, 0x1b, 0x4b, 0x66},
-		{0x80, 0x80, 0x00, 0x22, 0x5e, 0x80},
-		{0x9a, 0x9a, 0x00, 0x29, 0x71, 0x9a},
-		{0xb8, 0xb8, 0x00, 0x31, 0x87, 0xb8},
-		{0xdd, 0xdd, 0x00, 0x3b, 0xa2, 0xdd},
-	};
-
-	val = sd->ctrls[COLORS].val;
-	for (i = 0; i < ARRAY_SIZE(color_tb[0]); i++)
-		sccb_reg_write(gspca_dev, 0x4f + i, color_tb[val][i]);
-}
-
 static void setlightfreq(struct gspca_dev *gspca_dev)
 {
 	struct sd *sd = (struct sd *) gspca_dev;
@@ -1225,9 +1289,13 @@ static int sd_init(struct gspca_dev *gspca_dev)
 
 	if ((sensor_id & 0xfff0) == 0x7670) {
 		sd->sensor = SENSOR_OV767x;
-		gspca_dev->ctrl_dis = (1 << GAIN) |
+		gspca_dev->ctrl_dis = (1 << HUE) |
+					(1 << GAIN) |
 					(1 << AGC) |
 					(1 << SHARPNESS);	/* auto */
+		sd->ctrls[SATURATION].min = 0,
+		sd->ctrls[SATURATION].max = 6,
+		sd->ctrls[SATURATION].def = 3,
 		sd->ctrls[BRIGHTNESS].min = -127;
 		sd->ctrls[BRIGHTNESS].max = 127;
 		sd->ctrls[BRIGHTNESS].def = 0;
@@ -1243,7 +1311,6 @@ static int sd_init(struct gspca_dev *gspca_dev)
 		gspca_dev->cam.nmodes = ARRAY_SIZE(ov767x_mode);
 	} else {
 		sd->sensor = SENSOR_OV772x;
-		gspca_dev->ctrl_dis = (1 << COLORS);
 		gspca_dev->cam.bulk = 1;
 		gspca_dev->cam.bulk_size = 16384;
 		gspca_dev->cam.bulk_nurbs = 2;
@@ -1302,6 +1369,9 @@ static int sd_start(struct gspca_dev *gspca_dev)
 
 	set_frame_rate(gspca_dev);
 
+	if (!(gspca_dev->ctrl_dis & (1 << HUE)))
+		sethue(gspca_dev);
+	setsaturation(gspca_dev);
 	if (!(gspca_dev->ctrl_dis & (1 << AGC)))
 		setagc(gspca_dev);
 	setawb(gspca_dev);
@@ -1314,8 +1384,6 @@ static int sd_start(struct gspca_dev *gspca_dev)
 	if (!(gspca_dev->ctrl_dis & (1 << SHARPNESS)))
 		setsharpness(gspca_dev);
 	sethvflip(gspca_dev);
-	if (!(gspca_dev->ctrl_dis & (1 << COLORS)))
-		setcolors(gspca_dev);
 	setlightfreq(gspca_dev);
 
 	ov534_set_led(gspca_dev, 1);
diff --git a/drivers/media/video/gspca/ov534_9.c b/drivers/media/video/gspca/ov534_9.c
index e6601b88603..b5797308a39 100644
--- a/drivers/media/video/gspca/ov534_9.c
+++ b/drivers/media/video/gspca/ov534_9.c
@@ -1376,7 +1376,7 @@ static int sd_init(struct gspca_dev *gspca_dev)
 				ARRAY_SIZE(ov562x_init_2));
 		reg_w(gspca_dev, 0xe0, 0x00);
 	} else {
-		err("Unknown sensor %04x", sensor_id);
+		pr_err("Unknown sensor %04x", sensor_id);
 		return -EINVAL;
 	}
 
diff --git a/drivers/media/video/gspca/pac207.c b/drivers/media/video/gspca/pac207.c
index 3844c49f269..fa661c6d6d5 100644
--- a/drivers/media/video/gspca/pac207.c
+++ b/drivers/media/video/gspca/pac207.c
@@ -29,6 +29,8 @@
 
 #include <linux/input.h>
 #include "gspca.h"
+/* Include pac common sof detection functions */
+#include "pac_common.h"
 
 MODULE_AUTHOR("Hans de Goede <hdegoede@redhat.com>");
 MODULE_DESCRIPTION("Pixart PAC207");
@@ -39,16 +41,17 @@ MODULE_LICENSE("GPL");
 #define PAC207_BRIGHTNESS_MIN		0
 #define PAC207_BRIGHTNESS_MAX		255
 #define PAC207_BRIGHTNESS_DEFAULT	46
+#define PAC207_BRIGHTNESS_REG		0x08
 
 #define PAC207_EXPOSURE_MIN		3
 #define PAC207_EXPOSURE_MAX		90 /* 1 sec expo time / 1 fps */
 #define PAC207_EXPOSURE_DEFAULT		5 /* power on default: 3 */
-#define PAC207_EXPOSURE_KNEE		9 /* fps: 90 / exposure -> 9: 10 fps */
+#define PAC207_EXPOSURE_REG		0x02
 
 #define PAC207_GAIN_MIN			0
 #define PAC207_GAIN_MAX			31
 #define PAC207_GAIN_DEFAULT		7 /* power on default: 9 */
-#define PAC207_GAIN_KNEE		15
+#define PAC207_GAIN_REG			0x0e
 
 #define PAC207_AUTOGAIN_DEADZONE	30
 
@@ -56,13 +59,9 @@ MODULE_LICENSE("GPL");
 struct sd {
 	struct gspca_dev gspca_dev;		/* !! must be the first item */
 
-	u8 mode;
-
-	u8 brightness;
-	u8 exposure;
-	u8 autogain;
-	u8 gain;
+	struct v4l2_ctrl *brightness;
 
+	u8 mode;
 	u8 sof_read;
 	u8 header_read;
 	u8 autogain_ignore_frames;
@@ -70,80 +69,6 @@ struct sd {
 	atomic_t avg_lum;
 };
 
-/* V4L2 controls supported by the driver */
-static int sd_setbrightness(struct gspca_dev *gspca_dev, __s32 val);
-static int sd_getbrightness(struct gspca_dev *gspca_dev, __s32 *val);
-static int sd_setexposure(struct gspca_dev *gspca_dev, __s32 val);
-static int sd_getexposure(struct gspca_dev *gspca_dev, __s32 *val);
-static int sd_setautogain(struct gspca_dev *gspca_dev, __s32 val);
-static int sd_getautogain(struct gspca_dev *gspca_dev, __s32 *val);
-static int sd_setgain(struct gspca_dev *gspca_dev, __s32 val);
-static int sd_getgain(struct gspca_dev *gspca_dev, __s32 *val);
-
-static const struct ctrl sd_ctrls[] = {
-#define SD_BRIGHTNESS 0
-	{
-	    {
-		.id      = V4L2_CID_BRIGHTNESS,
-		.type    = V4L2_CTRL_TYPE_INTEGER,
-		.name    = "Brightness",
-		.minimum = PAC207_BRIGHTNESS_MIN,
-		.maximum = PAC207_BRIGHTNESS_MAX,
-		.step = 1,
-		.default_value = PAC207_BRIGHTNESS_DEFAULT,
-		.flags = 0,
-	    },
-	    .set = sd_setbrightness,
-	    .get = sd_getbrightness,
-	},
-#define SD_EXPOSURE 1
-	{
-	    {
-		.id = V4L2_CID_EXPOSURE,
-		.type = V4L2_CTRL_TYPE_INTEGER,
-		.name = "Exposure",
-		.minimum = PAC207_EXPOSURE_MIN,
-		.maximum = PAC207_EXPOSURE_MAX,
-		.step = 1,
-		.default_value = PAC207_EXPOSURE_DEFAULT,
-		.flags = 0,
-	    },
-	    .set = sd_setexposure,
-	    .get = sd_getexposure,
-	},
-#define SD_AUTOGAIN 2
-	{
-	    {
-		.id	  = V4L2_CID_AUTOGAIN,
-		.type	= V4L2_CTRL_TYPE_BOOLEAN,
-		.name	= "Auto Gain",
-		.minimum = 0,
-		.maximum = 1,
-		.step	= 1,
-#define AUTOGAIN_DEF 1
-		.default_value = AUTOGAIN_DEF,
-		.flags = 0,
-	    },
-	    .set = sd_setautogain,
-	    .get = sd_getautogain,
-	},
-#define SD_GAIN 3
-	{
-	    {
-		.id = V4L2_CID_GAIN,
-		.type = V4L2_CTRL_TYPE_INTEGER,
-		.name = "Gain",
-		.minimum = PAC207_GAIN_MIN,
-		.maximum = PAC207_GAIN_MAX,
-		.step = 1,
-		.default_value = PAC207_GAIN_DEFAULT,
-		.flags = 0,
-	    },
-	    .set = sd_setgain,
-	    .get = sd_getgain,
-	},
-};
-
 static const struct v4l2_pix_format sif_mode[] = {
 	{176, 144, V4L2_PIX_FMT_PAC207, V4L2_FIELD_NONE,
 		.bytesperline = 176,
@@ -167,39 +92,44 @@ static const __u8 pac207_sensor_init[][8] = {
 	{0x32, 0x00, 0x96, 0x00, 0xa2, 0x02, 0xaf, 0x00},
 };
 
-static int pac207_write_regs(struct gspca_dev *gspca_dev, u16 index,
+static void pac207_write_regs(struct gspca_dev *gspca_dev, u16 index,
 	const u8 *buffer, u16 length)
 {
 	struct usb_device *udev = gspca_dev->dev;
 	int err;
 
+	if (gspca_dev->usb_err < 0)
+		return;
+
 	memcpy(gspca_dev->usb_buf, buffer, length);
 
 	err = usb_control_msg(udev, usb_sndctrlpipe(udev, 0), 0x01,
 			USB_DIR_OUT | USB_TYPE_VENDOR | USB_RECIP_INTERFACE,
 			0x00, index,
 			gspca_dev->usb_buf, length, PAC207_CTRL_TIMEOUT);
-	if (err < 0)
+	if (err < 0) {
 		pr_err("Failed to write registers to index 0x%04X, error %d\n",
 		       index, err);
-
-	return err;
+		gspca_dev->usb_err = err;
+	}
 }
 
-
-static int pac207_write_reg(struct gspca_dev *gspca_dev, u16 index, u16 value)
+static void pac207_write_reg(struct gspca_dev *gspca_dev, u16 index, u16 value)
 {
 	struct usb_device *udev = gspca_dev->dev;
 	int err;
 
+	if (gspca_dev->usb_err < 0)
+		return;
+
 	err = usb_control_msg(udev, usb_sndctrlpipe(udev, 0), 0x00,
 			USB_DIR_OUT | USB_TYPE_VENDOR | USB_RECIP_INTERFACE,
 			value, index, NULL, 0, PAC207_CTRL_TIMEOUT);
-	if (err)
+	if (err) {
 		pr_err("Failed to write a register (index 0x%04X, value 0x%02X, error %d)\n",
 		       index, value, err);
-
-	return err;
+		gspca_dev->usb_err = err;
+	}
 }
 
 static int pac207_read_reg(struct gspca_dev *gspca_dev, u16 index)
@@ -207,6 +137,9 @@ static int pac207_read_reg(struct gspca_dev *gspca_dev, u16 index)
 	struct usb_device *udev = gspca_dev->dev;
 	int res;
 
+	if (gspca_dev->usb_err < 0)
+		return 0;
+
 	res = usb_control_msg(udev, usb_rcvctrlpipe(udev, 0), 0x00,
 			USB_DIR_IN | USB_TYPE_VENDOR | USB_RECIP_INTERFACE,
 			0x00, index,
@@ -214,7 +147,8 @@ static int pac207_read_reg(struct gspca_dev *gspca_dev, u16 index)
 	if (res < 0) {
 		pr_err("Failed to read a register (index 0x%04X, error %d)\n",
 		       index, res);
-		return res;
+		gspca_dev->usb_err = res;
+		return 0;
 	}
 
 	return gspca_dev->usb_buf[0];
@@ -224,7 +158,6 @@ static int pac207_read_reg(struct gspca_dev *gspca_dev, u16 index)
 static int sd_config(struct gspca_dev *gspca_dev,
 			const struct usb_device_id *id)
 {
-	struct sd *sd = (struct sd *) gspca_dev;
 	struct cam *cam;
 	u8 idreg[2];
 
@@ -247,10 +180,6 @@ static int sd_config(struct gspca_dev *gspca_dev,
 	cam = &gspca_dev->cam;
 	cam->cam_mode = sif_mode;
 	cam->nmodes = ARRAY_SIZE(sif_mode);
-	sd->brightness = PAC207_BRIGHTNESS_DEFAULT;
-	sd->exposure = PAC207_EXPOSURE_DEFAULT;
-	sd->gain = PAC207_GAIN_DEFAULT;
-	sd->autogain = AUTOGAIN_DEF;
 
 	return 0;
 }
@@ -264,6 +193,87 @@ static int sd_init(struct gspca_dev *gspca_dev)
 				 * Bit_2=Compression test mode enable */
 	pac207_write_reg(gspca_dev, 0x0f, 0x00); /* Power Control */
 
+	return gspca_dev->usb_err;
+}
+
+static void setcontrol(struct gspca_dev *gspca_dev, u16 reg, u16 val)
+{
+	pac207_write_reg(gspca_dev, reg, val);
+	pac207_write_reg(gspca_dev, 0x13, 0x01);	/* Bit 0, auto clear */
+	pac207_write_reg(gspca_dev, 0x1c, 0x01);	/* not documented */
+}
+
+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;
+
+	gspca_dev->usb_err = 0;
+
+	if (ctrl->id == V4L2_CID_AUTOGAIN && ctrl->is_new && ctrl->val) {
+		/* when switching to autogain set defaults to make sure
+		   we are on a valid point of the autogain gain /
+		   exposure knee graph, and give this change time to
+		   take effect before doing autogain. */
+		gspca_dev->exposure->val    = PAC207_EXPOSURE_DEFAULT;
+		gspca_dev->gain->val        = PAC207_GAIN_DEFAULT;
+		sd->autogain_ignore_frames  = PAC_AUTOGAIN_IGNORE_FRAMES;
+	}
+
+	if (!gspca_dev->streaming)
+		return 0;
+
+	switch (ctrl->id) {
+	case V4L2_CID_BRIGHTNESS:
+		setcontrol(gspca_dev, PAC207_BRIGHTNESS_REG, ctrl->val);
+		break;
+	case V4L2_CID_AUTOGAIN:
+		if (gspca_dev->exposure->is_new || (ctrl->is_new && ctrl->val))
+			setcontrol(gspca_dev, PAC207_EXPOSURE_REG,
+				   gspca_dev->exposure->val);
+		if (gspca_dev->gain->is_new || (ctrl->is_new && ctrl->val))
+			setcontrol(gspca_dev, PAC207_GAIN_REG,
+				   gspca_dev->gain->val);
+		break;
+	default:
+		return -EINVAL;
+	}
+	return gspca_dev->usb_err;
+}
+
+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, 4);
+
+	sd->brightness = v4l2_ctrl_new_std(hdl, &sd_ctrl_ops,
+				V4L2_CID_BRIGHTNESS,
+				PAC207_BRIGHTNESS_MIN, PAC207_BRIGHTNESS_MAX,
+				1, PAC207_BRIGHTNESS_DEFAULT);
+	gspca_dev->autogain = v4l2_ctrl_new_std(hdl, &sd_ctrl_ops,
+				V4L2_CID_AUTOGAIN, 0, 1, 1, 1);
+	gspca_dev->exposure = v4l2_ctrl_new_std(hdl, &sd_ctrl_ops,
+				V4L2_CID_EXPOSURE,
+				PAC207_EXPOSURE_MIN, PAC207_EXPOSURE_MAX,
+				1, PAC207_EXPOSURE_DEFAULT);
+	gspca_dev->gain = v4l2_ctrl_new_std(hdl, &sd_ctrl_ops,
+				V4L2_CID_GAIN,
+				PAC207_GAIN_MIN, PAC207_GAIN_MAX,
+				1, PAC207_GAIN_DEFAULT);
+	if (hdl->error) {
+		pr_err("Could not initialize controls\n");
+		return hdl->error;
+	}
+	v4l2_ctrl_auto_cluster(3, &gspca_dev->autogain, 0, false);
 	return 0;
 }
 
@@ -285,11 +295,13 @@ static int sd_start(struct gspca_dev *gspca_dev)
 	else
 		pac207_write_reg(gspca_dev, 0x4a, 0x30);
 	pac207_write_reg(gspca_dev, 0x4b, 0x00); /* Sram test value */
-	pac207_write_reg(gspca_dev, 0x08, sd->brightness);
+	pac207_write_reg(gspca_dev, 0x08, v4l2_ctrl_g_ctrl(sd->brightness));
 
 	/* PGA global gain (Bit 4-0) */
-	pac207_write_reg(gspca_dev, 0x0e, sd->gain);
-	pac207_write_reg(gspca_dev, 0x02, sd->exposure); /* PXCK = 12MHz /n */
+	pac207_write_reg(gspca_dev, 0x0e,
+		v4l2_ctrl_g_ctrl(gspca_dev->gain));
+	pac207_write_reg(gspca_dev, 0x02,
+		v4l2_ctrl_g_ctrl(gspca_dev->exposure)); /* PXCK = 12MHz /n */
 
 	mode = 0x02; /* Image Format (Bit 0), LED (1), Compr. test mode (2) */
 	if (gspca_dev->width == 176) {	/* 176x144 */
@@ -308,7 +320,7 @@ static int sd_start(struct gspca_dev *gspca_dev)
 	sd->sof_read = 0;
 	sd->autogain_ignore_frames = 0;
 	atomic_set(&sd->avg_lum, -1);
-	return 0;
+	return gspca_dev->usb_err;
 }
 
 static void sd_stopN(struct gspca_dev *gspca_dev)
@@ -318,8 +330,6 @@ static void sd_stopN(struct gspca_dev *gspca_dev)
 	pac207_write_reg(gspca_dev, 0x0f, 0x00); /* Power Control */
 }
 
-/* Include pac common sof detection functions */
-#include "pac_common.h"
 
 static void pac207_do_auto_gain(struct gspca_dev *gspca_dev)
 {
@@ -331,9 +341,8 @@ static void pac207_do_auto_gain(struct gspca_dev *gspca_dev)
 
 	if (sd->autogain_ignore_frames > 0)
 		sd->autogain_ignore_frames--;
-	else if (gspca_auto_gain_n_exposure(gspca_dev, avg_lum,
-			90, PAC207_AUTOGAIN_DEADZONE,
-			PAC207_GAIN_KNEE, PAC207_EXPOSURE_KNEE))
+	else if (gspca_coarse_grained_expo_autogain(gspca_dev, avg_lum,
+			90, PAC207_AUTOGAIN_DEADZONE))
 		sd->autogain_ignore_frames = PAC_AUTOGAIN_IGNORE_FRAMES;
 }
 
@@ -384,118 +393,6 @@ static void sd_pkt_scan(struct gspca_dev *gspca_dev,
 	gspca_frame_add(gspca_dev, INTER_PACKET, data, len);
 }
 
-static void setbrightness(struct gspca_dev *gspca_dev)
-{
-	struct sd *sd = (struct sd *) gspca_dev;
-
-	pac207_write_reg(gspca_dev, 0x08, sd->brightness);
-	pac207_write_reg(gspca_dev, 0x13, 0x01);	/* Bit 0, auto clear */
-	pac207_write_reg(gspca_dev, 0x1c, 0x01);	/* not documented */
-}
-
-static void setexposure(struct gspca_dev *gspca_dev)
-{
-	struct sd *sd = (struct sd *) gspca_dev;
-
-	pac207_write_reg(gspca_dev, 0x02, sd->exposure);
-	pac207_write_reg(gspca_dev, 0x13, 0x01);	/* Bit 0, auto clear */
-	pac207_write_reg(gspca_dev, 0x1c, 0x01);	/* not documented */
-}
-
-static void setgain(struct gspca_dev *gspca_dev)
-{
-	struct sd *sd = (struct sd *) gspca_dev;
-
-	pac207_write_reg(gspca_dev, 0x0e, sd->gain);
-	pac207_write_reg(gspca_dev, 0x13, 0x01);	/* Bit 0, auto clear */
-	pac207_write_reg(gspca_dev, 0x1c, 0x01);	/* not documented */
-}
-
-static int sd_setbrightness(struct gspca_dev *gspca_dev, __s32 val)
-{
-	struct sd *sd = (struct sd *) gspca_dev;
-
-	sd->brightness = val;
-	if (gspca_dev->streaming)
-		setbrightness(gspca_dev);
-	return 0;
-}
-
-static int sd_getbrightness(struct gspca_dev *gspca_dev, __s32 *val)
-{
-	struct sd *sd = (struct sd *) gspca_dev;
-
-	*val = sd->brightness;
-	return 0;
-}
-
-static int sd_setexposure(struct gspca_dev *gspca_dev, __s32 val)
-{
-	struct sd *sd = (struct sd *) gspca_dev;
-
-	sd->exposure = val;
-	if (gspca_dev->streaming)
-		setexposure(gspca_dev);
-	return 0;
-}
-
-static int sd_getexposure(struct gspca_dev *gspca_dev, __s32 *val)
-{
-	struct sd *sd = (struct sd *) gspca_dev;
-
-	*val = sd->exposure;
-	return 0;
-}
-
-static int sd_setgain(struct gspca_dev *gspca_dev, __s32 val)
-{
-	struct sd *sd = (struct sd *) gspca_dev;
-
-	sd->gain = val;
-	if (gspca_dev->streaming)
-		setgain(gspca_dev);
-	return 0;
-}
-
-static int sd_getgain(struct gspca_dev *gspca_dev, __s32 *val)
-{
-	struct sd *sd = (struct sd *) gspca_dev;
-
-	*val = sd->gain;
-	return 0;
-}
-
-static int sd_setautogain(struct gspca_dev *gspca_dev, __s32 val)
-{
-	struct sd *sd = (struct sd *) gspca_dev;
-
-	sd->autogain = val;
-	/* when switching to autogain set defaults to make sure
-	   we are on a valid point of the autogain gain /
-	   exposure knee graph, and give this change time to
-	   take effect before doing autogain. */
-	if (sd->autogain) {
-		sd->exposure = PAC207_EXPOSURE_DEFAULT;
-		sd->gain = PAC207_GAIN_DEFAULT;
-		if (gspca_dev->streaming) {
-			sd->autogain_ignore_frames =
-				PAC_AUTOGAIN_IGNORE_FRAMES;
-			setexposure(gspca_dev);
-			setgain(gspca_dev);
-		}
-	}
-
-	return 0;
-}
-
-static int sd_getautogain(struct gspca_dev *gspca_dev, __s32 *val)
-{
-	struct sd *sd = (struct sd *) gspca_dev;
-
-	*val = sd->autogain;
-	return 0;
-}
-
 #if defined(CONFIG_INPUT) || defined(CONFIG_INPUT_MODULE)
 static int sd_int_pkt_scan(struct gspca_dev *gspca_dev,
 			u8 *data,		/* interrupt packet data */
@@ -518,10 +415,9 @@ 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 = ARRAY_SIZE(sd_ctrls),
 	.config = sd_config,
 	.init = sd_init,
+	.init_controls = sd_init_controls,
 	.start = sd_start,
 	.stopN = sd_stopN,
 	.dq_callback = pac207_do_auto_gain,
diff --git a/drivers/media/video/gspca/pac7302.c b/drivers/media/video/gspca/pac7302.c
index 30662fccb0c..a0369a58c4b 100644
--- a/drivers/media/video/gspca/pac7302.c
+++ b/drivers/media/video/gspca/pac7302.c
@@ -23,43 +23,58 @@
  * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
  */
 
-/* Some documentation about various registers as determined by trial and error.
-
-   Register page 1:
-
-   Address	Description
-   0x78		Global control, bit 6 controls the LED (inverted)
-
-   Register page 3:
-
-   Address	Description
-   0x02		Clock divider 3-63, fps = 90 / val. Must be a multiple of 3 on
-		the 7302, so one of 3, 6, 9, ..., except when between 6 and 12?
-   0x03		Variable framerate ctrl reg2==3: 0 -> ~30 fps, 255 -> ~22fps
-   0x04		Another var framerate ctrl reg2==3, reg3==0: 0 -> ~30 fps,
-		63 -> ~27 fps, the 2 msb's must always be 1 !!
-   0x05		Another var framerate ctrl reg2==3, reg3==0, reg4==0xc0:
-		1 -> ~30 fps, 2 -> ~20 fps
-   0x0e		Exposure bits 0-7, 0-448, 0 = use full frame time
-   0x0f		Exposure bit 8, 0-448, 448 = no exposure at all
-   0x10		Master gain 0-31
-   0x21		Bitfield: 0-1 unused, 2-3 vflip/hflip, 4-5 unknown, 6-7 unused
-
-   The registers are accessed in the following functions:
-
-   Page | Register   | Function
-   -----+------------+---------------------------------------------------
-    0   | 0x0f..0x20 | setcolors()
-    0   | 0xa2..0xab | setbrightcont()
-    0   | 0xc5       | setredbalance()
-    0   | 0xc6       | setwhitebalance()
-    0   | 0xc7       | setbluebalance()
-    0   | 0xdc       | setbrightcont(), setcolors()
-    3   | 0x02       | setexposure()
-    3   | 0x10       | setgain()
-    3   | 0x11       | setcolors(), setgain(), setexposure(), sethvflip()
-    3   | 0x21       | sethvflip()
-*/
+/*
+ * Some documentation about various registers as determined by trial and error.
+ *
+ * Register page 1:
+ *
+ * Address	Description
+ * 0x78		Global control, bit 6 controls the LED (inverted)
+ * 0x80		Compression balance, 2 interesting settings:
+ *		0x0f Default
+ *		0x50 Values >= this switch the camera to a lower compression,
+ *		     using the same table for both luminance and chrominance.
+ *		     This gives a sharper picture. Only usable when running
+ *		     at < 15 fps! Note currently the driver does not use this
+ *		     as the quality gain is small and the generated JPG-s are
+ *		     only understood by v4l-utils >= 0.8.9
+ *
+ * Register page 3:
+ *
+ * Address	Description
+ * 0x02		Clock divider 3-63, fps = 90 / val. Must be a multiple of 3 on
+ *		the 7302, so one of 3, 6, 9, ..., except when between 6 and 12?
+ * 0x03		Variable framerate ctrl reg2==3: 0 -> ~30 fps, 255 -> ~22fps
+ * 0x04		Another var framerate ctrl reg2==3, reg3==0: 0 -> ~30 fps,
+ *		63 -> ~27 fps, the 2 msb's must always be 1 !!
+ * 0x05		Another var framerate ctrl reg2==3, reg3==0, reg4==0xc0:
+ *		1 -> ~30 fps, 2 -> ~20 fps
+ * 0x0e		Exposure bits 0-7, 0-448, 0 = use full frame time
+ * 0x0f		Exposure bit 8, 0-448, 448 = no exposure at all
+ * 0x10		Gain 0-31
+ * 0x12		Another gain 0-31, unlike 0x10 this one seems to start with an
+ *		amplification value of 1 rather then 0 at its lowest setting
+ * 0x21		Bitfield: 0-1 unused, 2-3 vflip/hflip, 4-5 unknown, 6-7 unused
+ * 0x80		Another framerate control, best left at 1, moving it from 1 to
+ *		2 causes the framerate to become 3/4th of what it was, and
+ *		also seems to cause pixel averaging, resulting in an effective
+ *		resolution of 320x240 and thus a much blockier image
+ *
+ * The registers are accessed in the following functions:
+ *
+ * Page | Register   | Function
+ * -----+------------+---------------------------------------------------
+ *  0   | 0x0f..0x20 | setcolors()
+ *  0   | 0xa2..0xab | setbrightcont()
+ *  0   | 0xc5       | setredbalance()
+ *  0   | 0xc6       | setwhitebalance()
+ *  0   | 0xc7       | setbluebalance()
+ *  0   | 0xdc       | setbrightcont(), setcolors()
+ *  3   | 0x02       | setexposure()
+ *  3   | 0x10, 0x12 | setgain()
+ *  3   | 0x11       | setcolors(), setgain(), setexposure(), sethvflip()
+ *  3   | 0x21       | sethvflip()
+ */
 
 #define pr_fmt(fmt) KBUILD_MODNAME ": " fmt
 
@@ -89,7 +104,6 @@ enum e_ctrl {
 	NCTRLS		/* number of controls */
 };
 
-/* specific webcam descriptor for pac7302 */
 struct sd {
 	struct gspca_dev gspca_dev;		/* !! must be the first item */
 
@@ -198,10 +212,10 @@ static const struct ctrl sd_ctrls[] = {
 		.type    = V4L2_CTRL_TYPE_INTEGER,
 		.name    = "Gain",
 		.minimum = 0,
-		.maximum = 255,
+		.maximum = 62,
 		.step    = 1,
-#define GAIN_DEF 127
-#define GAIN_KNEE 255 /* Gain seems to cause little noise on the pac73xx */
+#define GAIN_DEF 15
+#define GAIN_KNEE 46
 		.default_value = GAIN_DEF,
 	    },
 	    .set_control = setgain
@@ -270,7 +284,6 @@ static const struct v4l2_pix_format vga_mode[] = {
 #define LOAD_PAGE3		255
 #define END_OF_SEQUENCE		0
 
-/* pac 7302 */
 static const u8 init_7302[] = {
 /*	index,value */
 	0xff, 0x01,		/* page 1 */
@@ -509,7 +522,6 @@ static int sd_config(struct gspca_dev *gspca_dev,
 	return 0;
 }
 
-/* This function is used by pac7302 only */
 static void setbrightcont(struct gspca_dev *gspca_dev)
 {
 	struct sd *sd = (struct sd *) gspca_dev;
@@ -536,7 +548,6 @@ static void setbrightcont(struct gspca_dev *gspca_dev)
 	reg_w(gspca_dev, 0xdc, 0x01);
 }
 
-/* This function is used by pac7302 only */
 static void setcolors(struct gspca_dev *gspca_dev)
 {
 	struct sd *sd = (struct sd *) gspca_dev;
@@ -590,9 +601,19 @@ static void setbluebalance(struct gspca_dev *gspca_dev)
 static void setgain(struct gspca_dev *gspca_dev)
 {
 	struct sd *sd = (struct sd *) gspca_dev;
+	u8 reg10, reg12;
+
+	if (sd->ctrls[GAIN].val < 32) {
+		reg10 = sd->ctrls[GAIN].val;
+		reg12 = 0;
+	} else {
+		reg10 = 31;
+		reg12 = sd->ctrls[GAIN].val - 31;
+	}
 
 	reg_w(gspca_dev, 0xff, 0x03);			/* page 3 */
-	reg_w(gspca_dev, 0x10, sd->ctrls[GAIN].val >> 3);
+	reg_w(gspca_dev, 0x10, reg10);
+	reg_w(gspca_dev, 0x12, reg12);
 
 	/* load registers to sensor (Bit 0, auto clear) */
 	reg_w(gspca_dev, 0x11, 0x01);
@@ -604,28 +625,36 @@ static void setexposure(struct gspca_dev *gspca_dev)
 	u8 clockdiv;
 	u16 exposure;
 
-	/* register 2 of frame 3 contains the clock divider configuring the
-	   no fps according to the formula: 90 / reg. sd->exposure is the
-	   desired exposure time in 0.5 ms. */
+	/*
+	 * Register 2 of frame 3 contains the clock divider configuring the
+	 * no fps according to the formula: 90 / reg. sd->exposure is the
+	 * desired exposure time in 0.5 ms.
+	 */
 	clockdiv = (90 * sd->ctrls[EXPOSURE].val + 1999) / 2000;
 
-	/* Note clockdiv = 3 also works, but when running at 30 fps, depending
-	   on the scene being recorded, the camera switches to another
-	   quantization table for certain JPEG blocks, and we don't know how
-	   to decompress these blocks. So we cap the framerate at 15 fps */
+	/*
+	 * Note clockdiv = 3 also works, but when running at 30 fps, depending
+	 * on the scene being recorded, the camera switches to another
+	 * quantization table for certain JPEG blocks, and we don't know how
+	 * to decompress these blocks. So we cap the framerate at 15 fps.
+	 */
 	if (clockdiv < 6)
 		clockdiv = 6;
 	else if (clockdiv > 63)
 		clockdiv = 63;
 
-	/* reg2 MUST be a multiple of 3, except when between 6 and 12?
-	   Always round up, otherwise we cannot get the desired frametime
-	   using the partial frame time exposure control */
+	/*
+	 * Register 2 MUST be a multiple of 3, except when between 6 and 12?
+	 * Always round up, otherwise we cannot get the desired frametime
+	 * using the partial frame time exposure control.
+	 */
 	if (clockdiv < 6 || clockdiv > 12)
 		clockdiv = ((clockdiv + 2) / 3) * 3;
 
-	/* frame exposure time in ms = 1000 * clockdiv / 90    ->
-	exposure = (sd->exposure / 2) * 448 / (1000 * clockdiv / 90) */
+	/*
+	 * frame exposure time in ms = 1000 * clockdiv / 90    ->
+	 * exposure = (sd->exposure / 2) * 448 / (1000 * clockdiv / 90)
+	 */
 	exposure = (sd->ctrls[EXPOSURE].val * 45 * 448) / (1000 * clockdiv);
 	/* 0 = use full frametime, 448 = no exposure, reverse it */
 	exposure = 448 - exposure;
@@ -643,10 +672,12 @@ static void setautogain(struct gspca_dev *gspca_dev)
 {
 	struct sd *sd = (struct sd *) gspca_dev;
 
-	/* when switching to autogain set defaults to make sure
-	   we are on a valid point of the autogain gain /
-	   exposure knee graph, and give this change time to
-	   take effect before doing autogain. */
+	/*
+	 * When switching to autogain set defaults to make sure
+	 * we are on a valid point of the autogain gain /
+	 * exposure knee graph, and give this change time to
+	 * take effect before doing autogain.
+	 */
 	if (sd->ctrls[AUTOGAIN].val) {
 		sd->ctrls[EXPOSURE].val = EXPOSURE_DEF;
 		sd->ctrls[GAIN].val = GAIN_DEF;
@@ -700,8 +731,6 @@ static int sd_start(struct gspca_dev *gspca_dev)
 	setautogain(gspca_dev);
 	sethvflip(gspca_dev);
 
-	/* only resolution 640x480 is supported for pac7302 */
-
 	sd->sof_read = 0;
 	atomic_set(&sd->avg_lum, 270 + sd->ctrls[BRIGHTNESS].val);
 
@@ -729,9 +758,7 @@ static void sd_stop0(struct gspca_dev *gspca_dev)
 	reg_w(gspca_dev, 0x78, 0x40);
 }
 
-/* !! coarse_grained_expo_autogain is not used !! */
-#define exp_too_low_cnt flags
-#define exp_too_high_cnt sof_read
+#define WANT_REGULAR_AUTOGAIN
 #include "autogain_functions.h"
 
 static void do_autogain(struct gspca_dev *gspca_dev)
@@ -792,10 +819,12 @@ static void sd_pkt_scan(struct gspca_dev *gspca_dev,
 	if (sof) {
 		int n, lum_offset, footer_length;
 
-		/* 6 bytes after the FF D9 EOF marker a number of lumination
-		   bytes are send corresponding to different parts of the
-		   image, the 14th and 15th byte after the EOF seem to
-		   correspond to the center of the image */
+		/*
+		 * 6 bytes after the FF D9 EOF marker a number of lumination
+		 * bytes are send corresponding to different parts of the
+		 * image, the 14th and 15th byte after the EOF seem to
+		 * correspond to the center of the image.
+		 */
 		lum_offset = 61 + sizeof pac_sof_marker;
 		footer_length = 74;
 
@@ -839,9 +868,10 @@ static int sd_dbg_s_register(struct gspca_dev *gspca_dev,
 	u8 index;
 	u8 value;
 
-	/* reg->reg: bit0..15: reserved for register index (wIndex is 16bit
-			       long on the USB bus)
-	*/
+	/*
+	 * reg->reg: bit0..15: reserved for register index (wIndex is 16bit
+	 *		       long on the USB bus)
+	 */
 	if (reg->match.type == V4L2_CHIP_MATCH_HOST &&
 	    reg->match.addr == 0 &&
 	    (reg->reg < 0x000000ff) &&
@@ -852,9 +882,11 @@ static int sd_dbg_s_register(struct gspca_dev *gspca_dev,
 		index = reg->reg;
 		value = reg->val;
 
-		/* Note that there shall be no access to other page
-		   by any other function between the page swith and
-		   the actual register write */
+		/*
+		 * Note that there shall be no access to other page
+		 * by any other function between the page switch and
+		 * the actual register write.
+		 */
 		reg_w(gspca_dev, 0xff, 0x00);		/* page 0 */
 		reg_w(gspca_dev, index, value);
 
@@ -940,6 +972,7 @@ static const struct usb_device_id device_table[] = {
 	{USB_DEVICE(0x093a, 0x2624), .driver_info = FL_VFLIP},
 	{USB_DEVICE(0x093a, 0x2625)},
 	{USB_DEVICE(0x093a, 0x2626)},
+	{USB_DEVICE(0x093a, 0x2627), .driver_info = FL_VFLIP},
 	{USB_DEVICE(0x093a, 0x2628)},
 	{USB_DEVICE(0x093a, 0x2629), .driver_info = FL_VFLIP},
 	{USB_DEVICE(0x093a, 0x262a)},
diff --git a/drivers/media/video/gspca/pac7311.c b/drivers/media/video/gspca/pac7311.c
index 1ac111176ff..2cb7d95f7be 100644
--- a/drivers/media/video/gspca/pac7311.c
+++ b/drivers/media/video/gspca/pac7311.c
@@ -20,34 +20,42 @@
  */
 
 /* Some documentation about various registers as determined by trial and error.
-   When the register addresses differ between the 7202 and the 7311 the 2
-   different addresses are written as 7302addr/7311addr, when one of the 2
-   addresses is a - sign that register description is not valid for the
-   matching IC.
-
-   Register page 1:
-
-   Address	Description
-   -/0x08	Unknown compressor related, must always be 8 except when not
-		in 640x480 resolution and page 4 reg 2 <= 3 then set it to 9 !
-   -/0x1b	Auto white balance related, bit 0 is AWB enable (inverted)
-		bits 345 seem to toggle per color gains on/off (inverted)
-   0x78		Global control, bit 6 controls the LED (inverted)
-   -/0x80	JPEG compression ratio ? Best not touched
-
-   Register page 3/4:
-
-   Address	Description
-   0x02		Clock divider 2-63, fps =~ 60 / val. Must be a multiple of 3 on
-		the 7302, so one of 3, 6, 9, ..., except when between 6 and 12?
-   -/0x0f	Master gain 1-245, low value = high gain
-   0x10/-	Master gain 0-31
-   -/0x10	Another gain 0-15, limited influence (1-2x gain I guess)
-   0x21		Bitfield: 0-1 unused, 2-3 vflip/hflip, 4-5 unknown, 6-7 unused
-   -/0x27	Seems to toggle various gains on / off, Setting bit 7 seems to
-		completely disable the analog amplification block. Set to 0x68
-		for max gain, 0x14 for minimal gain.
-*/
+ *
+ * Register page 1:
+ *
+ * Address	Description
+ * 0x08		Unknown compressor related, must always be 8 except when not
+ *		in 640x480 resolution and page 4 reg 2 <= 3 then set it to 9 !
+ * 0x1b		Auto white balance related, bit 0 is AWB enable (inverted)
+ *		bits 345 seem to toggle per color gains on/off (inverted)
+ * 0x78		Global control, bit 6 controls the LED (inverted)
+ * 0x80		Compression balance, interesting settings:
+ *		0x01 Use this to allow the camera to switch to higher compr.
+ *		     on the fly. Needed to stay within bandwidth @ 640x480@30
+ *		0x1c From usb captures under Windows for 640x480
+ *		0x2a Values >= this switch the camera to a lower compression,
+ *		     using the same table for both luminance and chrominance.
+ *		     This gives a sharper picture. Usable only at 640x480@ <
+ *		     15 fps or 320x240 / 160x120. Note currently the driver
+ *		     does not use this as the quality gain is small and the
+ *		     generated JPG-s are only understood by v4l-utils >= 0.8.9
+ *		0x3f From usb captures under Windows for 320x240
+ *		0x69 From usb captures under Windows for 160x120
+ *
+ * Register page 4:
+ *
+ * Address	Description
+ * 0x02		Clock divider 2-63, fps =~ 60 / val. Must be a multiple of 3 on
+ *		the 7302, so one of 3, 6, 9, ..., except when between 6 and 12?
+ * 0x0f		Master gain 1-245, low value = high gain
+ * 0x10		Another gain 0-15, limited influence (1-2x gain I guess)
+ * 0x21		Bitfield: 0-1 unused, 2-3 vflip/hflip, 4-5 unknown, 6-7 unused
+ *		Note setting vflip disabled leads to a much lower image quality,
+ *		so we always vflip, and tell userspace to flip it back
+ * 0x27		Seems to toggle various gains on / off, Setting bit 7 seems to
+ *		completely disable the analog amplification block. Set to 0x68
+ *		for max gain, 0x14 for minimal gain.
+ */
 
 #define pr_fmt(fmt) KBUILD_MODNAME ": " fmt
 
@@ -55,21 +63,21 @@
 
 #include <linux/input.h>
 #include "gspca.h"
+/* Include pac common sof detection functions */
+#include "pac_common.h"
+
+#define PAC7311_GAIN_DEFAULT     122
+#define PAC7311_EXPOSURE_DEFAULT   3 /* 20 fps, avoid using high compr. */
 
 MODULE_AUTHOR("Thomas Kaiser thomas@kaiser-linux.li");
 MODULE_DESCRIPTION("Pixart PAC7311");
 MODULE_LICENSE("GPL");
 
-/* specific webcam descriptor for pac7311 */
 struct sd {
 	struct gspca_dev gspca_dev;		/* !! must be the first item */
 
-	unsigned char contrast;
-	unsigned char gain;
-	unsigned char exposure;
-	unsigned char autogain;
-	__u8 hflip;
-	__u8 vflip;
+	struct v4l2_ctrl *contrast;
+	struct v4l2_ctrl *hflip;
 
 	u8 sof_read;
 	u8 autogain_ignore_frames;
@@ -77,114 +85,6 @@ struct sd {
 	atomic_t avg_lum;
 };
 
-/* V4L2 controls supported by the driver */
-static int sd_setcontrast(struct gspca_dev *gspca_dev, __s32 val);
-static int sd_getcontrast(struct gspca_dev *gspca_dev, __s32 *val);
-static int sd_setautogain(struct gspca_dev *gspca_dev, __s32 val);
-static int sd_getautogain(struct gspca_dev *gspca_dev, __s32 *val);
-static int sd_sethflip(struct gspca_dev *gspca_dev, __s32 val);
-static int sd_gethflip(struct gspca_dev *gspca_dev, __s32 *val);
-static int sd_setvflip(struct gspca_dev *gspca_dev, __s32 val);
-static int sd_getvflip(struct gspca_dev *gspca_dev, __s32 *val);
-static int sd_setgain(struct gspca_dev *gspca_dev, __s32 val);
-static int sd_getgain(struct gspca_dev *gspca_dev, __s32 *val);
-static int sd_setexposure(struct gspca_dev *gspca_dev, __s32 val);
-static int sd_getexposure(struct gspca_dev *gspca_dev, __s32 *val);
-
-static const struct ctrl sd_ctrls[] = {
-/* This control is for both the 7302 and the 7311 */
-	{
-	    {
-		.id      = V4L2_CID_CONTRAST,
-		.type    = V4L2_CTRL_TYPE_INTEGER,
-		.name    = "Contrast",
-		.minimum = 0,
-#define CONTRAST_MAX 255
-		.maximum = CONTRAST_MAX,
-		.step    = 1,
-#define CONTRAST_DEF 127
-		.default_value = CONTRAST_DEF,
-	    },
-	    .set = sd_setcontrast,
-	    .get = sd_getcontrast,
-	},
-/* All controls below are for both the 7302 and the 7311 */
-	{
-	    {
-		.id      = V4L2_CID_GAIN,
-		.type    = V4L2_CTRL_TYPE_INTEGER,
-		.name    = "Gain",
-		.minimum = 0,
-#define GAIN_MAX 255
-		.maximum = GAIN_MAX,
-		.step    = 1,
-#define GAIN_DEF 127
-#define GAIN_KNEE 255 /* Gain seems to cause little noise on the pac73xx */
-		.default_value = GAIN_DEF,
-	    },
-	    .set = sd_setgain,
-	    .get = sd_getgain,
-	},
-	{
-	    {
-		.id      = V4L2_CID_EXPOSURE,
-		.type    = V4L2_CTRL_TYPE_INTEGER,
-		.name    = "Exposure",
-		.minimum = 0,
-#define EXPOSURE_MAX 255
-		.maximum = EXPOSURE_MAX,
-		.step    = 1,
-#define EXPOSURE_DEF  16 /*  32 ms / 30 fps */
-#define EXPOSURE_KNEE 50 /* 100 ms / 10 fps */
-		.default_value = EXPOSURE_DEF,
-	    },
-	    .set = sd_setexposure,
-	    .get = sd_getexposure,
-	},
-	{
-	    {
-		.id      = V4L2_CID_AUTOGAIN,
-		.type    = V4L2_CTRL_TYPE_BOOLEAN,
-		.name    = "Auto Gain",
-		.minimum = 0,
-		.maximum = 1,
-		.step    = 1,
-#define AUTOGAIN_DEF 1
-		.default_value = AUTOGAIN_DEF,
-	    },
-	    .set = sd_setautogain,
-	    .get = sd_getautogain,
-	},
-	{
-	    {
-		.id      = V4L2_CID_HFLIP,
-		.type    = V4L2_CTRL_TYPE_BOOLEAN,
-		.name    = "Mirror",
-		.minimum = 0,
-		.maximum = 1,
-		.step    = 1,
-#define HFLIP_DEF 0
-		.default_value = HFLIP_DEF,
-	    },
-	    .set = sd_sethflip,
-	    .get = sd_gethflip,
-	},
-	{
-	    {
-		.id      = V4L2_CID_VFLIP,
-		.type    = V4L2_CTRL_TYPE_BOOLEAN,
-		.name    = "Vflip",
-		.minimum = 0,
-		.maximum = 1,
-		.step    = 1,
-#define VFLIP_DEF 0
-		.default_value = VFLIP_DEF,
-	    },
-	    .set = sd_setvflip,
-	    .get = sd_getvflip,
-	},
-};
-
 static const struct v4l2_pix_format vga_mode[] = {
 	{160, 120, V4L2_PIX_FMT_PJPG, V4L2_FIELD_NONE,
 		.bytesperline = 160,
@@ -206,8 +106,8 @@ static const struct v4l2_pix_format vga_mode[] = {
 #define LOAD_PAGE4		254
 #define END_OF_SEQUENCE		0
 
-/* pac 7311 */
 static const __u8 init_7311[] = {
+	0xff, 0x01,
 	0x78, 0x40,	/* Bit_0=start stream, Bit_6=LED */
 	0x78, 0x40,	/* Bit_0=start stream, Bit_6=LED */
 	0x78, 0x44,	/* Bit_0=start stream, Bit_6=LED */
@@ -387,90 +287,73 @@ static void reg_w_var(struct gspca_dev *gspca_dev,
 static int sd_config(struct gspca_dev *gspca_dev,
 			const struct usb_device_id *id)
 {
-	struct sd *sd = (struct sd *) gspca_dev;
-	struct cam *cam;
+	struct cam *cam = &gspca_dev->cam;
 
-	cam = &gspca_dev->cam;
-
-	PDEBUG(D_CONF, "Find Sensor PAC7311");
 	cam->cam_mode = vga_mode;
 	cam->nmodes = ARRAY_SIZE(vga_mode);
+	cam->input_flags = V4L2_IN_ST_VFLIP;
 
-	sd->contrast = CONTRAST_DEF;
-	sd->gain = GAIN_DEF;
-	sd->exposure = EXPOSURE_DEF;
-	sd->autogain = AUTOGAIN_DEF;
-	sd->hflip = HFLIP_DEF;
-	sd->vflip = VFLIP_DEF;
 	return 0;
 }
 
-/* This function is used by pac7311 only */
-static void setcontrast(struct gspca_dev *gspca_dev)
+static void setcontrast(struct gspca_dev *gspca_dev, s32 val)
 {
-	struct sd *sd = (struct sd *) gspca_dev;
-
 	reg_w(gspca_dev, 0xff, 0x04);
-	reg_w(gspca_dev, 0x10, sd->contrast >> 4);
+	reg_w(gspca_dev, 0x10, val);
 	/* load registers to sensor (Bit 0, auto clear) */
 	reg_w(gspca_dev, 0x11, 0x01);
 }
 
-static void setgain(struct gspca_dev *gspca_dev)
+static void setgain(struct gspca_dev *gspca_dev, s32 val)
 {
-	struct sd *sd = (struct sd *) gspca_dev;
-	int gain = GAIN_MAX - sd->gain;
-
-	if (gain < 1)
-		gain = 1;
-	else if (gain > 245)
-		gain = 245;
 	reg_w(gspca_dev, 0xff, 0x04);			/* page 4 */
 	reg_w(gspca_dev, 0x0e, 0x00);
-	reg_w(gspca_dev, 0x0f, gain);
+	reg_w(gspca_dev, 0x0f, gspca_dev->gain->maximum - val + 1);
 
 	/* load registers to sensor (Bit 0, auto clear) */
 	reg_w(gspca_dev, 0x11, 0x01);
 }
 
-static void setexposure(struct gspca_dev *gspca_dev)
+static void setexposure(struct gspca_dev *gspca_dev, s32 val)
 {
-	struct sd *sd = (struct sd *) gspca_dev;
-	__u8 reg;
-
-	/* register 2 of frame 3/4 contains the clock divider configuring the
-	   no fps according to the formula: 60 / reg. sd->exposure is the
-	   desired exposure time in ms. */
-	reg = 120 * sd->exposure / 1000;
-	if (reg < 2)
-		reg = 2;
-	else if (reg > 63)
-		reg = 63;
-
 	reg_w(gspca_dev, 0xff, 0x04);			/* page 4 */
-	reg_w(gspca_dev, 0x02, reg);
+	reg_w(gspca_dev, 0x02, val);
 
-	/* Page 1 register 8 must always be 0x08 except when not in
-	   640x480 mode and Page3/4 reg 2 <= 3 then it must be 9 */
+	/* load registers to sensor (Bit 0, auto clear) */
+	reg_w(gspca_dev, 0x11, 0x01);
+
+	/*
+	 * Page 1 register 8 must always be 0x08 except when not in
+	 *  640x480 mode and page 4 reg 2 <= 3 then it must be 9
+	 */
 	reg_w(gspca_dev, 0xff, 0x01);
-	if (gspca_dev->cam.cam_mode[(int)gspca_dev->curr_mode].priv &&
-			reg <= 3) {
+	if (gspca_dev->width != 640 && val <= 3)
 		reg_w(gspca_dev, 0x08, 0x09);
-	} else {
+	else
 		reg_w(gspca_dev, 0x08, 0x08);
-	}
+
+	/*
+	 * Page1 register 80 sets the compression balance, normally we
+	 * want / use 0x1c, but for 640x480@30fps we must allow the
+	 * camera to use higher compression or we may run out of
+	 * bandwidth.
+	 */
+	if (gspca_dev->width == 640 && val == 2)
+		reg_w(gspca_dev, 0x80, 0x01);
+	else
+		reg_w(gspca_dev, 0x80, 0x1c);
 
 	/* load registers to sensor (Bit 0, auto clear) */
 	reg_w(gspca_dev, 0x11, 0x01);
 }
 
-static void sethvflip(struct gspca_dev *gspca_dev)
+static void sethvflip(struct gspca_dev *gspca_dev, s32 hflip, s32 vflip)
 {
-	struct sd *sd = (struct sd *) gspca_dev;
 	__u8 data;
 
 	reg_w(gspca_dev, 0xff, 0x04);			/* page 4 */
-	data = (sd->hflip ? 0x04 : 0x00) | (sd->vflip ? 0x08 : 0x00);
+	data = (hflip ? 0x04 : 0x00) |
+	       (vflip ? 0x08 : 0x00);
 	reg_w(gspca_dev, 0x21, data);
 
 	/* load registers to sensor (Bit 0, auto clear) */
@@ -484,6 +367,82 @@ static int sd_init(struct gspca_dev *gspca_dev)
 	return gspca_dev->usb_err;
 }
 
+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;
+
+	gspca_dev->usb_err = 0;
+
+	if (ctrl->id == V4L2_CID_AUTOGAIN && ctrl->is_new && ctrl->val) {
+		/* when switching to autogain set defaults to make sure
+		   we are on a valid point of the autogain gain /
+		   exposure knee graph, and give this change time to
+		   take effect before doing autogain. */
+		gspca_dev->exposure->val    = PAC7311_EXPOSURE_DEFAULT;
+		gspca_dev->gain->val        = PAC7311_GAIN_DEFAULT;
+		sd->autogain_ignore_frames  = PAC_AUTOGAIN_IGNORE_FRAMES;
+	}
+
+	if (!gspca_dev->streaming)
+		return 0;
+
+	switch (ctrl->id) {
+	case V4L2_CID_CONTRAST:
+		setcontrast(gspca_dev, ctrl->val);
+		break;
+	case V4L2_CID_AUTOGAIN:
+		if (gspca_dev->exposure->is_new || (ctrl->is_new && ctrl->val))
+			setexposure(gspca_dev, gspca_dev->exposure->val);
+		if (gspca_dev->gain->is_new || (ctrl->is_new && ctrl->val))
+			setgain(gspca_dev, gspca_dev->gain->val);
+		break;
+	case V4L2_CID_HFLIP:
+		sethvflip(gspca_dev, sd->hflip->val, 1);
+		break;
+	default:
+		return -EINVAL;
+	}
+	return gspca_dev->usb_err;
+}
+
+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, 4);
+
+	sd->contrast = v4l2_ctrl_new_std(hdl, &sd_ctrl_ops,
+					V4L2_CID_CONTRAST, 0, 15, 1, 7);
+	gspca_dev->autogain = v4l2_ctrl_new_std(hdl, &sd_ctrl_ops,
+					V4L2_CID_AUTOGAIN, 0, 1, 1, 1);
+	gspca_dev->exposure = v4l2_ctrl_new_std(hdl, &sd_ctrl_ops,
+					V4L2_CID_EXPOSURE, 2, 63, 1,
+					PAC7311_EXPOSURE_DEFAULT);
+	gspca_dev->gain = v4l2_ctrl_new_std(hdl, &sd_ctrl_ops,
+					V4L2_CID_GAIN, 0, 244, 1,
+					PAC7311_GAIN_DEFAULT);
+	sd->hflip = v4l2_ctrl_new_std(hdl, &sd_ctrl_ops,
+		V4L2_CID_HFLIP, 0, 1, 1, 0);
+
+	if (hdl->error) {
+		pr_err("Could not initialize controls\n");
+		return hdl->error;
+	}
+
+	v4l2_ctrl_auto_cluster(3, &gspca_dev->autogain, 0, false);
+	return 0;
+}
+
+/* -- start the camera -- */
 static int sd_start(struct gspca_dev *gspca_dev)
 {
 	struct sd *sd = (struct sd *) gspca_dev;
@@ -492,19 +451,19 @@ static int sd_start(struct gspca_dev *gspca_dev)
 
 	reg_w_var(gspca_dev, start_7311,
 		page4_7311, sizeof(page4_7311));
-	setcontrast(gspca_dev);
-	setgain(gspca_dev);
-	setexposure(gspca_dev);
-	sethvflip(gspca_dev);
+	setcontrast(gspca_dev, v4l2_ctrl_g_ctrl(sd->contrast));
+	setgain(gspca_dev, v4l2_ctrl_g_ctrl(gspca_dev->gain));
+	setexposure(gspca_dev, v4l2_ctrl_g_ctrl(gspca_dev->exposure));
+	sethvflip(gspca_dev, v4l2_ctrl_g_ctrl(sd->hflip), 1);
 
 	/* set correct resolution */
 	switch (gspca_dev->cam.cam_mode[(int) gspca_dev->curr_mode].priv) {
-	case 2:					/* 160x120 pac7311 */
+	case 2:					/* 160x120 */
 		reg_w(gspca_dev, 0xff, 0x01);
 		reg_w(gspca_dev, 0x17, 0x20);
 		reg_w(gspca_dev, 0x87, 0x10);
 		break;
-	case 1:					/* 320x240 pac7311 */
+	case 1:					/* 320x240 */
 		reg_w(gspca_dev, 0xff, 0x01);
 		reg_w(gspca_dev, 0x17, 0x30);
 		reg_w(gspca_dev, 0x87, 0x11);
@@ -541,14 +500,6 @@ static void sd_stopN(struct gspca_dev *gspca_dev)
 	reg_w(gspca_dev, 0x78, 0x44); /* Bit_0=start stream, Bit_6=LED */
 }
 
-/* called on streamoff with alt 0 and on disconnect for 7311 */
-static void sd_stop0(struct gspca_dev *gspca_dev)
-{
-}
-
-/* Include pac common sof detection functions */
-#include "pac_common.h"
-
 static void do_autogain(struct gspca_dev *gspca_dev)
 {
 	struct sd *sd = (struct sd *) gspca_dev;
@@ -558,13 +509,13 @@ static void do_autogain(struct gspca_dev *gspca_dev)
 	if (avg_lum == -1)
 		return;
 
-	desired_lum = 200;
+	desired_lum = 170;
 	deadzone = 20;
 
 	if (sd->autogain_ignore_frames > 0)
 		sd->autogain_ignore_frames--;
-	else if (gspca_auto_gain_n_exposure(gspca_dev, avg_lum, desired_lum,
-			deadzone, GAIN_KNEE, EXPOSURE_KNEE))
+	else if (gspca_coarse_grained_expo_autogain(gspca_dev, avg_lum,
+						    desired_lum, deadzone))
 		sd->autogain_ignore_frames = PAC_AUTOGAIN_IGNORE_FRAMES;
 }
 
@@ -628,10 +579,12 @@ static void sd_pkt_scan(struct gspca_dev *gspca_dev,
 	if (sof) {
 		int n, lum_offset, footer_length;
 
-		/* 6 bytes after the FF D9 EOF marker a number of lumination
-		   bytes are send corresponding to different parts of the
-		   image, the 14th and 15th byte after the EOF seem to
-		   correspond to the center of the image */
+		/*
+		 * 6 bytes after the FF D9 EOF marker a number of lumination
+		 * bytes are send corresponding to different parts of the
+		 * image, the 14th and 15th byte after the EOF seem to
+		 * correspond to the center of the image.
+		 */
 		lum_offset = 24 + sizeof pac_sof_marker;
 		footer_length = 26;
 
@@ -668,127 +621,6 @@ static void sd_pkt_scan(struct gspca_dev *gspca_dev,
 	gspca_frame_add(gspca_dev, INTER_PACKET, data, len);
 }
 
-static int sd_setcontrast(struct gspca_dev *gspca_dev, __s32 val)
-{
-	struct sd *sd = (struct sd *) gspca_dev;
-
-	sd->contrast = val;
-	if (gspca_dev->streaming)
-		setcontrast(gspca_dev);
-	return gspca_dev->usb_err;
-}
-
-static int sd_getcontrast(struct gspca_dev *gspca_dev, __s32 *val)
-{
-	struct sd *sd = (struct sd *) gspca_dev;
-
-	*val = sd->contrast;
-	return 0;
-}
-
-static int sd_setgain(struct gspca_dev *gspca_dev, __s32 val)
-{
-	struct sd *sd = (struct sd *) gspca_dev;
-
-	sd->gain = val;
-	if (gspca_dev->streaming)
-		setgain(gspca_dev);
-	return gspca_dev->usb_err;
-}
-
-static int sd_getgain(struct gspca_dev *gspca_dev, __s32 *val)
-{
-	struct sd *sd = (struct sd *) gspca_dev;
-
-	*val = sd->gain;
-	return 0;
-}
-
-static int sd_setexposure(struct gspca_dev *gspca_dev, __s32 val)
-{
-	struct sd *sd = (struct sd *) gspca_dev;
-
-	sd->exposure = val;
-	if (gspca_dev->streaming)
-		setexposure(gspca_dev);
-	return gspca_dev->usb_err;
-}
-
-static int sd_getexposure(struct gspca_dev *gspca_dev, __s32 *val)
-{
-	struct sd *sd = (struct sd *) gspca_dev;
-
-	*val = sd->exposure;
-	return 0;
-}
-
-static int sd_setautogain(struct gspca_dev *gspca_dev, __s32 val)
-{
-	struct sd *sd = (struct sd *) gspca_dev;
-
-	sd->autogain = val;
-	/* when switching to autogain set defaults to make sure
-	   we are on a valid point of the autogain gain /
-	   exposure knee graph, and give this change time to
-	   take effect before doing autogain. */
-	if (sd->autogain) {
-		sd->exposure = EXPOSURE_DEF;
-		sd->gain = GAIN_DEF;
-		if (gspca_dev->streaming) {
-			sd->autogain_ignore_frames =
-				PAC_AUTOGAIN_IGNORE_FRAMES;
-			setexposure(gspca_dev);
-			setgain(gspca_dev);
-		}
-	}
-
-	return gspca_dev->usb_err;
-}
-
-static int sd_getautogain(struct gspca_dev *gspca_dev, __s32 *val)
-{
-	struct sd *sd = (struct sd *) gspca_dev;
-
-	*val = sd->autogain;
-	return 0;
-}
-
-static int sd_sethflip(struct gspca_dev *gspca_dev, __s32 val)
-{
-	struct sd *sd = (struct sd *) gspca_dev;
-
-	sd->hflip = val;
-	if (gspca_dev->streaming)
-		sethvflip(gspca_dev);
-	return gspca_dev->usb_err;
-}
-
-static int sd_gethflip(struct gspca_dev *gspca_dev, __s32 *val)
-{
-	struct sd *sd = (struct sd *) gspca_dev;
-
-	*val = sd->hflip;
-	return 0;
-}
-
-static int sd_setvflip(struct gspca_dev *gspca_dev, __s32 val)
-{
-	struct sd *sd = (struct sd *) gspca_dev;
-
-	sd->vflip = val;
-	if (gspca_dev->streaming)
-		sethvflip(gspca_dev);
-	return gspca_dev->usb_err;
-}
-
-static int sd_getvflip(struct gspca_dev *gspca_dev, __s32 *val)
-{
-	struct sd *sd = (struct sd *) gspca_dev;
-
-	*val = sd->vflip;
-	return 0;
-}
-
 #if defined(CONFIG_INPUT) || defined(CONFIG_INPUT_MODULE)
 static int sd_int_pkt_scan(struct gspca_dev *gspca_dev,
 			u8 *data,		/* interrupt packet data */
@@ -820,16 +652,13 @@ static int sd_int_pkt_scan(struct gspca_dev *gspca_dev,
 }
 #endif
 
-/* sub-driver description for pac7311 */
 static const struct sd_desc sd_desc = {
 	.name = MODULE_NAME,
-	.ctrls = sd_ctrls,
-	.nctrls = ARRAY_SIZE(sd_ctrls),
 	.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,
 #if defined(CONFIG_INPUT) || defined(CONFIG_INPUT_MODULE)
diff --git a/drivers/media/video/gspca/sn9c20x.c b/drivers/media/video/gspca/sn9c20x.c
index 7e71aa2d252..ad098202d7f 100644
--- a/drivers/media/video/gspca/sn9c20x.c
+++ b/drivers/media/video/gspca/sn9c20x.c
@@ -59,35 +59,38 @@ MODULE_LICENSE("GPL");
 #define SENSOR_MT9M111	9
 #define SENSOR_MT9M112  10
 #define SENSOR_HV7131R	11
-#define SENSOR_MT9VPRB	20
+#define SENSOR_MT9VPRB	12
 
 /* camera flags */
 #define HAS_NO_BUTTON	0x1
 #define LED_REVERSE	0x2 /* some cameras unset gpio to turn on leds */
 #define FLIP_DETECT	0x4
 
-enum e_ctrl {
-	BRIGHTNESS,
-	CONTRAST,
-	SATURATION,
-	HUE,
-	GAMMA,
-	BLUE,
-	RED,
-	VFLIP,
-	HFLIP,
-	EXPOSURE,
-	GAIN,
-	AUTOGAIN,
-	QUALITY,
-	NCTRLS		/* number of controls */
-};
-
 /* specific webcam descriptor */
 struct sd {
 	struct gspca_dev gspca_dev;
 
-	struct gspca_ctrl ctrls[NCTRLS];
+	struct { /* color control cluster */
+		struct v4l2_ctrl *brightness;
+		struct v4l2_ctrl *contrast;
+		struct v4l2_ctrl *saturation;
+		struct v4l2_ctrl *hue;
+	};
+	struct { /* blue/red balance control cluster */
+		struct v4l2_ctrl *blue;
+		struct v4l2_ctrl *red;
+	};
+	struct { /* h/vflip control cluster */
+		struct v4l2_ctrl *hflip;
+		struct v4l2_ctrl *vflip;
+	};
+	struct v4l2_ctrl *gamma;
+	struct { /* autogain and exposure or gain control cluster */
+		struct v4l2_ctrl *autogain;
+		struct v4l2_ctrl *exposure;
+		struct v4l2_ctrl *gain;
+	};
+	struct v4l2_ctrl *jpegqual;
 
 	struct work_struct work;
 	struct workqueue_struct *work_thread;
@@ -105,6 +108,7 @@ struct sd {
 	u8 exposure_step;
 
 	u8 i2c_addr;
+	u8 i2c_intf;
 	u8 sensor;
 	u8 hstart;
 	u8 vstart;
@@ -166,175 +170,6 @@ static const struct dmi_system_id flip_dmi_table[] = {
 	{}
 };
 
-static void set_cmatrix(struct gspca_dev *gspca_dev);
-static void set_gamma(struct gspca_dev *gspca_dev);
-static void set_redblue(struct gspca_dev *gspca_dev);
-static void set_hvflip(struct gspca_dev *gspca_dev);
-static void set_exposure(struct gspca_dev *gspca_dev);
-static void set_gain(struct gspca_dev *gspca_dev);
-static void set_quality(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 = 0x7f
-	    },
-	    .set_control = set_cmatrix
-	},
-[CONTRAST] = {
-	    {
-		.id      = V4L2_CID_CONTRAST,
-		.type    = V4L2_CTRL_TYPE_INTEGER,
-		.name    = "Contrast",
-		.minimum = 0,
-		.maximum = 0xff,
-		.step    = 1,
-		.default_value = 0x7f
-	    },
-	    .set_control = set_cmatrix
-	},
-[SATURATION] = {
-	    {
-		.id      = V4L2_CID_SATURATION,
-		.type    = V4L2_CTRL_TYPE_INTEGER,
-		.name    = "Saturation",
-		.minimum = 0,
-		.maximum = 0xff,
-		.step    = 1,
-		.default_value = 0x7f
-	    },
-	    .set_control = set_cmatrix
-	},
-[HUE] = {
-	    {
-		.id      = V4L2_CID_HUE,
-		.type    = V4L2_CTRL_TYPE_INTEGER,
-		.name    = "Hue",
-		.minimum = -180,
-		.maximum = 180,
-		.step    = 1,
-		.default_value = 0
-	    },
-	    .set_control = set_cmatrix
-	},
-[GAMMA] = {
-	    {
-		.id      = V4L2_CID_GAMMA,
-		.type    = V4L2_CTRL_TYPE_INTEGER,
-		.name    = "Gamma",
-		.minimum = 0,
-		.maximum = 0xff,
-		.step    = 1,
-		.default_value = 0x10
-	    },
-	    .set_control = set_gamma
-	},
-[BLUE] = {
-	    {
-		.id	 = V4L2_CID_BLUE_BALANCE,
-		.type	 = V4L2_CTRL_TYPE_INTEGER,
-		.name	 = "Blue Balance",
-		.minimum = 0,
-		.maximum = 0x7f,
-		.step	 = 1,
-		.default_value = 0x28
-	    },
-	    .set_control = set_redblue
-	},
-[RED] = {
-	    {
-		.id	 = V4L2_CID_RED_BALANCE,
-		.type	 = V4L2_CTRL_TYPE_INTEGER,
-		.name	 = "Red Balance",
-		.minimum = 0,
-		.maximum = 0x7f,
-		.step	 = 1,
-		.default_value = 0x28
-	    },
-	    .set_control = set_redblue
-	},
-[HFLIP] = {
-	    {
-		.id      = V4L2_CID_HFLIP,
-		.type    = V4L2_CTRL_TYPE_BOOLEAN,
-		.name    = "Horizontal Flip",
-		.minimum = 0,
-		.maximum = 1,
-		.step    = 1,
-		.default_value = 0,
-	    },
-	    .set_control = set_hvflip
-	},
-[VFLIP] = {
-	    {
-		.id      = V4L2_CID_VFLIP,
-		.type    = V4L2_CTRL_TYPE_BOOLEAN,
-		.name    = "Vertical Flip",
-		.minimum = 0,
-		.maximum = 1,
-		.step    = 1,
-		.default_value = 0,
-	    },
-	    .set_control = set_hvflip
-	},
-[EXPOSURE] = {
-	    {
-		.id      = V4L2_CID_EXPOSURE,
-		.type    = V4L2_CTRL_TYPE_INTEGER,
-		.name    = "Exposure",
-		.minimum = 0,
-		.maximum = 0x1780,
-		.step    = 1,
-		.default_value = 0x33,
-	    },
-	    .set_control = set_exposure
-	},
-[GAIN] = {
-	    {
-		.id      = V4L2_CID_GAIN,
-		.type    = V4L2_CTRL_TYPE_INTEGER,
-		.name    = "Gain",
-		.minimum = 0,
-		.maximum = 28,
-		.step    = 1,
-		.default_value = 0,
-	    },
-	    .set_control = set_gain
-	},
-[AUTOGAIN] = {
-	    {
-		.id      = V4L2_CID_AUTOGAIN,
-		.type    = V4L2_CTRL_TYPE_BOOLEAN,
-		.name    = "Auto Exposure",
-		.minimum = 0,
-		.maximum = 1,
-		.step    = 1,
-		.default_value = 1,
-	    },
-	},
-[QUALITY] = {
-	    {
-		.id      = V4L2_CID_JPEG_COMPRESSION_QUALITY,
-		.type    = V4L2_CTRL_TYPE_INTEGER,
-		.name    = "Compression Quality",
-#define QUALITY_MIN 50
-#define QUALITY_MAX 90
-#define QUALITY_DEF 80
-		.minimum = QUALITY_MIN,
-		.maximum = QUALITY_MAX,
-		.step    = 1,
-		.default_value = QUALITY_DEF,
-	    },
-	    .set_control = set_quality
-	},
-};
-
 static const struct v4l2_pix_format vga_mode[] = {
 	{160, 120, V4L2_PIX_FMT_JPEG, V4L2_FIELD_NONE,
 		.bytesperline = 160,
@@ -747,7 +582,7 @@ static const s16 hsv_blue_y[] = {
 	4,   2,   0,  -1,  -3,  -5,  -7,  -9, -11
 };
 
-static u16 i2c_ident[] = {
+static const u16 i2c_ident[] = {
 	V4L2_IDENT_OV9650,
 	V4L2_IDENT_OV9655,
 	V4L2_IDENT_SOI968,
@@ -760,9 +595,10 @@ static u16 i2c_ident[] = {
 	V4L2_IDENT_MT9M111,
 	V4L2_IDENT_MT9M112,
 	V4L2_IDENT_HV7131R,
+[SENSOR_MT9VPRB] = V4L2_IDENT_UNKNOWN,
 };
 
-static u16 bridge_init[][2] = {
+static const u16 bridge_init[][2] = {
 	{0x1000, 0x78}, {0x1001, 0x40}, {0x1002, 0x1c},
 	{0x1020, 0x80}, {0x1061, 0x01}, {0x1067, 0x40},
 	{0x1068, 0x30}, {0x1069, 0x20},	{0x106a, 0x10},
@@ -786,7 +622,7 @@ static u16 bridge_init[][2] = {
 };
 
 /* Gain = (bit[3:0] / 16 + 1) * (bit[4] + 1) * (bit[5] + 1) * (bit[6] + 1) */
-static u8 ov_gain[] = {
+static const u8 ov_gain[] = {
 	0x00 /* 1x */, 0x04 /* 1.25x */, 0x08 /* 1.5x */, 0x0c /* 1.75x */,
 	0x10 /* 2x */, 0x12 /* 2.25x */, 0x14 /* 2.5x */, 0x16 /* 2.75x */,
 	0x18 /* 3x */, 0x1a /* 3.25x */, 0x1c /* 3.5x */, 0x1e /* 3.75x */,
@@ -798,7 +634,7 @@ static u8 ov_gain[] = {
 };
 
 /* Gain = (bit[8] + 1) * (bit[7] + 1) * (bit[6:0] * 0.03125) */
-static u16 micron1_gain[] = {
+static const u16 micron1_gain[] = {
 	/* 1x   1.25x   1.5x    1.75x */
 	0x0020, 0x0028, 0x0030, 0x0038,
 	/* 2x   2.25x   2.5x    2.75x */
@@ -819,7 +655,7 @@ static u16 micron1_gain[] = {
 
 /* mt9m001 sensor uses a different gain formula then other micron sensors */
 /* Gain = (bit[6] + 1) * (bit[5-0] * 0.125) */
-static u16 micron2_gain[] = {
+static const u16 micron2_gain[] = {
 	/* 1x   1.25x   1.5x    1.75x */
 	0x0008, 0x000a, 0x000c, 0x000e,
 	/* 2x   2.25x   2.5x    2.75x */
@@ -839,7 +675,7 @@ static u16 micron2_gain[] = {
 };
 
 /* Gain = .5 + bit[7:0] / 16 */
-static u8 hv7131r_gain[] = {
+static const u8 hv7131r_gain[] = {
 	0x08 /* 1x */, 0x0c /* 1.25x */, 0x10 /* 1.5x */, 0x14 /* 1.75x */,
 	0x18 /* 2x */, 0x1c /* 2.25x */, 0x20 /* 2.5x */, 0x24 /* 2.75x */,
 	0x28 /* 3x */, 0x2c /* 3.25x */, 0x30 /* 3.5x */, 0x34 /* 3.75x */,
@@ -850,7 +686,7 @@ static u8 hv7131r_gain[] = {
 	0x78 /* 8x */
 };
 
-static struct i2c_reg_u8 soi968_init[] = {
+static const struct i2c_reg_u8 soi968_init[] = {
 	{0x0c, 0x00}, {0x0f, 0x1f},
 	{0x11, 0x80}, {0x38, 0x52}, {0x1e, 0x00},
 	{0x33, 0x08}, {0x35, 0x8c}, {0x36, 0x0c},
@@ -864,7 +700,7 @@ static struct i2c_reg_u8 soi968_init[] = {
 	{0x00, 0x00}, {0x01, 0x80}, {0x02, 0x80},
 };
 
-static struct i2c_reg_u8 ov7660_init[] = {
+static const struct i2c_reg_u8 ov7660_init[] = {
 	{0x0e, 0x80}, {0x0d, 0x08}, {0x0f, 0xc3},
 	{0x04, 0xc3}, {0x10, 0x40}, {0x11, 0x40},
 	{0x12, 0x05}, {0x13, 0xba}, {0x14, 0x2a},
@@ -872,11 +708,11 @@ static struct i2c_reg_u8 ov7660_init[] = {
 	   0x10, 0x61 and sd->hstart, vstart = 3, fixes ugly colored borders */
 	{0x17, 0x10}, {0x18, 0x61},
 	{0x37, 0x0f}, {0x38, 0x02}, {0x39, 0x43},
-	{0x3a, 0x00}, {0x69, 0x90}, {0x2d, 0xf6},
-	{0x2e, 0x0b}, {0x01, 0x78}, {0x02, 0x50},
+	{0x3a, 0x00}, {0x69, 0x90}, {0x2d, 0x00},
+	{0x2e, 0x00}, {0x01, 0x78}, {0x02, 0x50},
 };
 
-static struct i2c_reg_u8 ov7670_init[] = {
+static const struct i2c_reg_u8 ov7670_init[] = {
 	{0x11, 0x80}, {0x3a, 0x04}, {0x12, 0x01},
 	{0x32, 0xb6}, {0x03, 0x0a}, {0x0c, 0x00}, {0x3e, 0x00},
 	{0x70, 0x3a}, {0x71, 0x35}, {0x72, 0x11}, {0x73, 0xf0},
@@ -933,7 +769,7 @@ static struct i2c_reg_u8 ov7670_init[] = {
 	{0x93, 0x00},
 };
 
-static struct i2c_reg_u8 ov9650_init[] = {
+static const struct i2c_reg_u8 ov9650_init[] = {
 	{0x00, 0x00}, {0x01, 0x78},
 	{0x02, 0x78}, {0x03, 0x36}, {0x04, 0x03},
 	{0x05, 0x00}, {0x06, 0x00}, {0x08, 0x00},
@@ -963,7 +799,7 @@ static struct i2c_reg_u8 ov9650_init[] = {
 	{0xaa, 0x92}, {0xab, 0x0a},
 };
 
-static struct i2c_reg_u8 ov9655_init[] = {
+static const struct i2c_reg_u8 ov9655_init[] = {
 	{0x0e, 0x61}, {0x11, 0x80}, {0x13, 0xba},
 	{0x14, 0x2e}, {0x16, 0x24}, {0x1e, 0x04}, {0x27, 0x08},
 	{0x28, 0x08}, {0x29, 0x15}, {0x2c, 0x08}, {0x34, 0x3d},
@@ -990,7 +826,7 @@ static struct i2c_reg_u8 ov9655_init[] = {
 	{0x04, 0x03}, {0x00, 0x13},
 };
 
-static struct i2c_reg_u16 mt9v112_init[] = {
+static const struct i2c_reg_u16 mt9v112_init[] = {
 	{0xf0, 0x0000}, {0x0d, 0x0021}, {0x0d, 0x0020},
 	{0x34, 0xc019}, {0x0a, 0x0011}, {0x0b, 0x000b},
 	{0x20, 0x0703}, {0x35, 0x2022}, {0xf0, 0x0001},
@@ -1009,7 +845,7 @@ static struct i2c_reg_u16 mt9v112_init[] = {
 	{0x2c, 0x00ae}, {0x2d, 0x00ae}, {0x2e, 0x00ae},
 };
 
-static struct i2c_reg_u16 mt9v111_init[] = {
+static const struct i2c_reg_u16 mt9v111_init[] = {
 	{0x01, 0x0004}, {0x0d, 0x0001}, {0x0d, 0x0000},
 	{0x01, 0x0001}, {0x05, 0x0004}, {0x2d, 0xe0a0},
 	{0x2e, 0x0c64},	{0x2f, 0x0064}, {0x06, 0x600e},
@@ -1019,7 +855,7 @@ static struct i2c_reg_u16 mt9v111_init[] = {
 	{0x0e, 0x0008}, {0x20, 0x0000}
 };
 
-static struct i2c_reg_u16 mt9v011_init[] = {
+static const struct i2c_reg_u16 mt9v011_init[] = {
 	{0x07, 0x0002},	{0x0d, 0x0001},	{0x0d, 0x0000},
 	{0x01, 0x0008},	{0x02, 0x0016},	{0x03, 0x01e1},
 	{0x04, 0x0281},	{0x05, 0x0083},	{0x06, 0x0006},
@@ -1046,7 +882,7 @@ static struct i2c_reg_u16 mt9v011_init[] = {
 	{0x06, 0x0029},	{0x05, 0x0009},
 };
 
-static struct i2c_reg_u16 mt9m001_init[] = {
+static const struct i2c_reg_u16 mt9m001_init[] = {
 	{0x0d, 0x0001},
 	{0x0d, 0x0000},
 	{0x04, 0x0500},		/* hres = 1280 */
@@ -1062,21 +898,21 @@ static struct i2c_reg_u16 mt9m001_init[] = {
 	{0x35, 0x0057},
 };
 
-static struct i2c_reg_u16 mt9m111_init[] = {
+static const struct i2c_reg_u16 mt9m111_init[] = {
 	{0xf0, 0x0000}, {0x0d, 0x0021}, {0x0d, 0x0008},
 	{0xf0, 0x0001}, {0x3a, 0x4300}, {0x9b, 0x4300},
 	{0x06, 0x708e}, {0xf0, 0x0002}, {0x2e, 0x0a1e},
 	{0xf0, 0x0000},
 };
 
-static struct i2c_reg_u16 mt9m112_init[] = {
+static const struct i2c_reg_u16 mt9m112_init[] = {
 	{0xf0, 0x0000}, {0x0d, 0x0021}, {0x0d, 0x0008},
 	{0xf0, 0x0001}, {0x3a, 0x4300}, {0x9b, 0x4300},
 	{0x06, 0x708e}, {0xf0, 0x0002}, {0x2e, 0x0a1e},
 	{0xf0, 0x0000},
 };
 
-static struct i2c_reg_u8 hv7131r_init[] = {
+static const struct i2c_reg_u8 hv7131r_init[] = {
 	{0x02, 0x08}, {0x02, 0x00}, {0x01, 0x08},
 	{0x02, 0x00}, {0x20, 0x00}, {0x21, 0xd0},
 	{0x22, 0x00}, {0x23, 0x09}, {0x01, 0x08},
@@ -1167,7 +1003,7 @@ static void i2c_w1(struct gspca_dev *gspca_dev, u8 reg, u8 val)
 	 * from the point of view of the bridge, the length
 	 * includes the address
 	 */
-	row[0] = 0x81 | (2 << 4);
+	row[0] = sd->i2c_intf | (2 << 4);
 	row[1] = sd->i2c_addr;
 	row[2] = reg;
 	row[3] = val;
@@ -1180,7 +1016,7 @@ static void i2c_w1(struct gspca_dev *gspca_dev, u8 reg, u8 val)
 }
 
 static void i2c_w1_buf(struct gspca_dev *gspca_dev,
-			struct i2c_reg_u8 *buf, int sz)
+			const struct i2c_reg_u8 *buf, int sz)
 {
 	while (--sz >= 0) {
 		i2c_w1(gspca_dev, buf->reg, buf->val);
@@ -1197,7 +1033,7 @@ static void i2c_w2(struct gspca_dev *gspca_dev, u8 reg, u16 val)
 	 * from the point of view of the bridge, the length
 	 * includes the address
 	 */
-	row[0] = 0x81 | (3 << 4);
+	row[0] = sd->i2c_intf | (3 << 4);
 	row[1] = sd->i2c_addr;
 	row[2] = reg;
 	row[3] = val >> 8;
@@ -1210,7 +1046,7 @@ static void i2c_w2(struct gspca_dev *gspca_dev, u8 reg, u16 val)
 }
 
 static void i2c_w2_buf(struct gspca_dev *gspca_dev,
-			struct i2c_reg_u16 *buf, int sz)
+			const struct i2c_reg_u16 *buf, int sz)
 {
 	while (--sz >= 0) {
 		i2c_w2(gspca_dev, buf->reg, buf->val);
@@ -1223,7 +1059,7 @@ static void i2c_r1(struct gspca_dev *gspca_dev, u8 reg, u8 *val)
 	struct sd *sd = (struct sd *) gspca_dev;
 	u8 row[8];
 
-	row[0] = 0x81 | (1 << 4);
+	row[0] = sd->i2c_intf | (1 << 4);
 	row[1] = sd->i2c_addr;
 	row[2] = reg;
 	row[3] = 0;
@@ -1232,7 +1068,7 @@ static void i2c_r1(struct gspca_dev *gspca_dev, u8 reg, u8 *val)
 	row[6] = 0;
 	row[7] = 0x10;
 	i2c_w(gspca_dev, row);
-	row[0] = 0x81 | (1 << 4) | 0x02;
+	row[0] = sd->i2c_intf | (1 << 4) | 0x02;
 	row[2] = 0;
 	i2c_w(gspca_dev, row);
 	reg_r(gspca_dev, 0x10c2, 5);
@@ -1244,7 +1080,7 @@ static void i2c_r2(struct gspca_dev *gspca_dev, u8 reg, u16 *val)
 	struct sd *sd = (struct sd *) gspca_dev;
 	u8 row[8];
 
-	row[0] = 0x81 | (1 << 4);
+	row[0] = sd->i2c_intf | (1 << 4);
 	row[1] = sd->i2c_addr;
 	row[2] = reg;
 	row[3] = 0;
@@ -1253,7 +1089,7 @@ static void i2c_r2(struct gspca_dev *gspca_dev, u8 reg, u16 *val)
 	row[6] = 0;
 	row[7] = 0x10;
 	i2c_w(gspca_dev, row);
-	row[0] = 0x81 | (2 << 4) | 0x02;
+	row[0] = sd->i2c_intf | (2 << 4) | 0x02;
 	row[2] = 0;
 	i2c_w(gspca_dev, row);
 	reg_r(gspca_dev, 0x10c2, 5);
@@ -1294,8 +1130,6 @@ static void ov9655_init_sensor(struct gspca_dev *gspca_dev)
 	if (gspca_dev->usb_err < 0)
 		pr_err("OV9655 sensor initialization failed\n");
 
-	/* disable hflip and vflip */
-	gspca_dev->ctrl_dis = (1 << HFLIP) | (1 << VFLIP);
 	sd->hstart = 1;
 	sd->vstart = 2;
 }
@@ -1310,9 +1144,6 @@ static void soi968_init_sensor(struct gspca_dev *gspca_dev)
 	if (gspca_dev->usb_err < 0)
 		pr_err("SOI968 sensor initialization failed\n");
 
-	/* disable hflip and vflip */
-	gspca_dev->ctrl_dis = (1 << HFLIP) | (1 << VFLIP)
-				| (1 << EXPOSURE);
 	sd->hstart = 60;
 	sd->vstart = 11;
 }
@@ -1340,8 +1171,6 @@ static void ov7670_init_sensor(struct gspca_dev *gspca_dev)
 	if (gspca_dev->usb_err < 0)
 		pr_err("OV7670 sensor initialization failed\n");
 
-	/* disable hflip and vflip */
-	gspca_dev->ctrl_dis = (1 << HFLIP) | (1 << VFLIP);
 	sd->hstart = 0;
 	sd->vstart = 1;
 }
@@ -1378,9 +1207,6 @@ static void mt9v_init_sensor(struct gspca_dev *gspca_dev)
 			pr_err("MT9V111 sensor initialization failed\n");
 			return;
 		}
-		gspca_dev->ctrl_dis = (1 << EXPOSURE)
-					| (1 << AUTOGAIN)
-					| (1 << GAIN);
 		sd->hstart = 2;
 		sd->vstart = 2;
 		sd->sensor = SENSOR_MT9V111;
@@ -1422,8 +1248,6 @@ static void mt9m112_init_sensor(struct gspca_dev *gspca_dev)
 	if (gspca_dev->usb_err < 0)
 		pr_err("MT9M112 sensor initialization failed\n");
 
-	gspca_dev->ctrl_dis = (1 << EXPOSURE) | (1 << AUTOGAIN)
-				| (1 << GAIN);
 	sd->hstart = 0;
 	sd->vstart = 2;
 }
@@ -1436,8 +1260,6 @@ static void mt9m111_init_sensor(struct gspca_dev *gspca_dev)
 	if (gspca_dev->usb_err < 0)
 		pr_err("MT9M111 sensor initialization failed\n");
 
-	gspca_dev->ctrl_dis = (1 << EXPOSURE) | (1 << AUTOGAIN)
-				| (1 << GAIN);
 	sd->hstart = 0;
 	sd->vstart = 2;
 }
@@ -1470,8 +1292,6 @@ static void mt9m001_init_sensor(struct gspca_dev *gspca_dev)
 	if (gspca_dev->usb_err < 0)
 		pr_err("MT9M001 sensor initialization failed\n");
 
-	/* disable hflip and vflip */
-	gspca_dev->ctrl_dis = (1 << HFLIP) | (1 << VFLIP);
 	sd->hstart = 1;
 	sd->vstart = 1;
 }
@@ -1488,20 +1308,18 @@ static void hv7131r_init_sensor(struct gspca_dev *gspca_dev)
 	sd->vstart = 1;
 }
 
-static void set_cmatrix(struct gspca_dev *gspca_dev)
+static void set_cmatrix(struct gspca_dev *gspca_dev,
+		s32 brightness, s32 contrast, s32 satur, s32 hue)
 {
-	struct sd *sd = (struct sd *) gspca_dev;
-	int satur;
-	s32 hue_coord, hue_index = 180 + sd->ctrls[HUE].val;
+	s32 hue_coord, hue_index = 180 + hue;
 	u8 cmatrix[21];
 
 	memset(cmatrix, 0, sizeof cmatrix);
-	cmatrix[2] = (sd->ctrls[CONTRAST].val * 0x25 / 0x100) + 0x26;
+	cmatrix[2] = (contrast * 0x25 / 0x100) + 0x26;
 	cmatrix[0] = 0x13 + (cmatrix[2] - 0x26) * 0x13 / 0x25;
 	cmatrix[4] = 0x07 + (cmatrix[2] - 0x26) * 0x07 / 0x25;
-	cmatrix[18] = sd->ctrls[BRIGHTNESS].val - 0x80;
+	cmatrix[18] = brightness - 0x80;
 
-	satur = sd->ctrls[SATURATION].val;
 	hue_coord = (hsv_red_x[hue_index] * satur) >> 8;
 	cmatrix[6] = hue_coord;
 	cmatrix[7] = (hue_coord >> 8) & 0x0f;
@@ -1529,11 +1347,10 @@ static void set_cmatrix(struct gspca_dev *gspca_dev)
 	reg_w(gspca_dev, 0x10e1, cmatrix, 21);
 }
 
-static void set_gamma(struct gspca_dev *gspca_dev)
+static void set_gamma(struct gspca_dev *gspca_dev, s32 val)
 {
-	struct sd *sd = (struct sd *) gspca_dev;
 	u8 gamma[17];
-	u8 gval = sd->ctrls[GAMMA].val * 0xb8 / 0x100;
+	u8 gval = val * 0xb8 / 0x100;
 
 	gamma[0] = 0x0a;
 	gamma[1] = 0x13 + (gval * (0xcb - 0x13) / 0xb8);
@@ -1556,26 +1373,21 @@ static void set_gamma(struct gspca_dev *gspca_dev)
 	reg_w(gspca_dev, 0x1190, gamma, 17);
 }
 
-static void set_redblue(struct gspca_dev *gspca_dev)
+static void set_redblue(struct gspca_dev *gspca_dev, s32 blue, s32 red)
 {
-	struct sd *sd = (struct sd *) gspca_dev;
-
-	reg_w1(gspca_dev, 0x118c, sd->ctrls[RED].val);
-	reg_w1(gspca_dev, 0x118f, sd->ctrls[BLUE].val);
+	reg_w1(gspca_dev, 0x118c, red);
+	reg_w1(gspca_dev, 0x118f, blue);
 }
 
-static void set_hvflip(struct gspca_dev *gspca_dev)
+static void set_hvflip(struct gspca_dev *gspca_dev, s32 hflip, s32 vflip)
 {
-	u8 value, tslb, hflip, vflip;
+	u8 value, tslb;
 	u16 value2;
 	struct sd *sd = (struct sd *) gspca_dev;
 
 	if ((sd->flags & FLIP_DETECT) && dmi_check_system(flip_dmi_table)) {
-		hflip = !sd->ctrls[HFLIP].val;
-		vflip = !sd->ctrls[VFLIP].val;
-	} else {
-		hflip = sd->ctrls[HFLIP].val;
-		vflip = sd->ctrls[VFLIP].val;
+		hflip = !hflip;
+		vflip = !vflip;
 	}
 
 	switch (sd->sensor) {
@@ -1638,20 +1450,38 @@ static void set_hvflip(struct gspca_dev *gspca_dev)
 	}
 }
 
-static void set_exposure(struct gspca_dev *gspca_dev)
+static void set_exposure(struct gspca_dev *gspca_dev, s32 expo)
 {
 	struct sd *sd = (struct sd *) gspca_dev;
-	u8 exp[8] = {0x81, sd->i2c_addr, 0x00, 0x00, 0x00, 0x00, 0x00, 0x1e};
-	int expo;
+	u8 exp[8] = {sd->i2c_intf, sd->i2c_addr,
+				0x00, 0x00, 0x00, 0x00, 0x00, 0x10};
+	int expo2;
+
+	if (gspca_dev->streaming)
+		exp[7] = 0x1e;
 
-	expo = sd->ctrls[EXPOSURE].val;
 	switch (sd->sensor) {
 	case SENSOR_OV7660:
 	case SENSOR_OV7670:
 	case SENSOR_OV9655:
 	case SENSOR_OV9650:
+		if (expo > 547)
+			expo2 = 547;
+		else
+			expo2 = expo;
+		exp[0] |= (2 << 4);
+		exp[2] = 0x10;			/* AECH */
+		exp[3] = expo2 >> 2;
+		exp[7] = 0x10;
+		i2c_w(gspca_dev, exp);
+		exp[2] = 0x04;			/* COM1 */
+		exp[3] = expo2 & 0x0003;
+		exp[7] = 0x10;
+		i2c_w(gspca_dev, exp);
+		expo -= expo2;
+		exp[7] = 0x1e;
 		exp[0] |= (3 << 4);
-		exp[2] = 0x2d;
+		exp[2] = 0x2d;			/* ADVFL & ADVFH */
 		exp[3] = expo;
 		exp[4] = expo >> 8;
 		break;
@@ -1676,13 +1506,15 @@ static void set_exposure(struct gspca_dev *gspca_dev)
 	i2c_w(gspca_dev, exp);
 }
 
-static void set_gain(struct gspca_dev *gspca_dev)
+static void set_gain(struct gspca_dev *gspca_dev, s32 g)
 {
 	struct sd *sd = (struct sd *) gspca_dev;
-	u8 gain[8] = {0x81, sd->i2c_addr, 0x00, 0x00, 0x00, 0x00, 0x00, 0x1d};
-	int g;
+	u8 gain[8] = {sd->i2c_intf, sd->i2c_addr,
+				0x00, 0x00, 0x00, 0x00, 0x00, 0x10};
+
+	if (gspca_dev->streaming)
+		gain[7] = 0x15;		/* or 1d ? */
 
-	g = sd->ctrls[GAIN].val;
 	switch (sd->sensor) {
 	case SENSOR_OV7660:
 	case SENSOR_OV7670:
@@ -1721,11 +1553,11 @@ static void set_gain(struct gspca_dev *gspca_dev)
 	i2c_w(gspca_dev, gain);
 }
 
-static void set_quality(struct gspca_dev *gspca_dev)
+static void set_quality(struct gspca_dev *gspca_dev, s32 val)
 {
 	struct sd *sd = (struct sd *) gspca_dev;
 
-	jpeg_set_qual(sd->jpeg_hdr, sd->ctrls[QUALITY].val);
+	jpeg_set_qual(sd->jpeg_hdr, val);
 	reg_w1(gspca_dev, 0x1061, 0x01);	/* stop transfer */
 	reg_w1(gspca_dev, 0x10e0, sd->fmt | 0x20); /* write QTAB */
 	reg_w(gspca_dev, 0x1100, &sd->jpeg_hdr[JPEG_QT0_OFFSET], 64);
@@ -1827,6 +1659,7 @@ static int sd_config(struct gspca_dev *gspca_dev,
 	sd->sensor = id->driver_info >> 8;
 	sd->i2c_addr = id->driver_info;
 	sd->flags = id->driver_info >> 16;
+	sd->i2c_intf = 0x80;			/* i2c 100 Kb/s */
 
 	switch (sd->sensor) {
 	case SENSOR_MT9M112:
@@ -1840,6 +1673,9 @@ static int sd_config(struct gspca_dev *gspca_dev,
 		cam->cam_mode = mono_mode;
 		cam->nmodes = ARRAY_SIZE(mono_mode);
 		break;
+	case SENSOR_HV7131R:
+		sd->i2c_intf = 0x81;			/* i2c 400 Kb/s */
+		/* fall thru */
 	default:
 		cam->cam_mode = vga_mode;
 		cam->nmodes = ARRAY_SIZE(vga_mode);
@@ -1850,13 +1686,133 @@ static int sd_config(struct gspca_dev *gspca_dev,
 	sd->older_step = 0;
 	sd->exposure_step = 16;
 
-	gspca_dev->cam.ctrls = sd->ctrls;
-
 	INIT_WORK(&sd->work, qual_upd);
 
 	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;
+
+	gspca_dev->usb_err = 0;
+
+	if (!gspca_dev->streaming)
+		return 0;
+
+	switch (ctrl->id) {
+	/* color control cluster */
+	case V4L2_CID_BRIGHTNESS:
+		set_cmatrix(gspca_dev, sd->brightness->val,
+			sd->contrast->val, sd->saturation->val, sd->hue->val);
+		break;
+	case V4L2_CID_GAMMA:
+		set_gamma(gspca_dev, ctrl->val);
+		break;
+	/* blue/red balance cluster */
+	case V4L2_CID_BLUE_BALANCE:
+		set_redblue(gspca_dev, sd->blue->val, sd->red->val);
+		break;
+	/* h/vflip cluster */
+	case V4L2_CID_HFLIP:
+		set_hvflip(gspca_dev, sd->hflip->val, sd->vflip->val);
+		break;
+	/* standalone exposure control */
+	case V4L2_CID_EXPOSURE:
+		set_exposure(gspca_dev, ctrl->val);
+		break;
+	/* standalone gain control */
+	case V4L2_CID_GAIN:
+		set_gain(gspca_dev, ctrl->val);
+		break;
+	/* autogain + exposure or gain control cluster */
+	case V4L2_CID_AUTOGAIN:
+		if (sd->sensor == SENSOR_SOI968)
+			set_gain(gspca_dev, sd->gain->val);
+		else
+			set_exposure(gspca_dev, sd->exposure->val);
+		break;
+	case V4L2_CID_JPEG_COMPRESSION_QUALITY:
+		set_quality(gspca_dev, ctrl->val);
+		break;
+	}
+	return gspca_dev->usb_err;
+}
+
+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 v4l2_ctrl_handler *hdl = &gspca_dev->ctrl_handler;
+
+	gspca_dev->vdev.ctrl_handler = hdl;
+	v4l2_ctrl_handler_init(hdl, 13);
+
+	sd->brightness = v4l2_ctrl_new_std(hdl, &sd_ctrl_ops,
+			V4L2_CID_BRIGHTNESS, 0, 255, 1, 127);
+	sd->contrast = v4l2_ctrl_new_std(hdl, &sd_ctrl_ops,
+			V4L2_CID_CONTRAST, 0, 255, 1, 127);
+	sd->saturation = v4l2_ctrl_new_std(hdl, &sd_ctrl_ops,
+			V4L2_CID_SATURATION, 0, 255, 1, 127);
+	sd->hue = v4l2_ctrl_new_std(hdl, &sd_ctrl_ops,
+			V4L2_CID_HUE, -180, 180, 1, 0);
+	v4l2_ctrl_cluster(4, &sd->brightness);
+
+	sd->gamma = v4l2_ctrl_new_std(hdl, &sd_ctrl_ops,
+			V4L2_CID_GAMMA, 0, 255, 1, 0x10);
+
+	sd->blue = v4l2_ctrl_new_std(hdl, &sd_ctrl_ops,
+			V4L2_CID_BLUE_BALANCE, 0, 127, 1, 0x28);
+	sd->red = v4l2_ctrl_new_std(hdl, &sd_ctrl_ops,
+			V4L2_CID_RED_BALANCE, 0, 127, 1, 0x28);
+	v4l2_ctrl_cluster(2, &sd->blue);
+
+	if (sd->sensor != SENSOR_OV9655 && sd->sensor != SENSOR_SOI968 &&
+	    sd->sensor != SENSOR_OV7670 && sd->sensor != SENSOR_MT9M001 &&
+	    sd->sensor != SENSOR_MT9VPRB) {
+		sd->hflip = v4l2_ctrl_new_std(hdl, &sd_ctrl_ops,
+			V4L2_CID_HFLIP, 0, 1, 1, 0);
+		sd->vflip = v4l2_ctrl_new_std(hdl, &sd_ctrl_ops,
+			V4L2_CID_VFLIP, 0, 1, 1, 0);
+		v4l2_ctrl_cluster(2, &sd->hflip);
+	}
+
+	if (sd->sensor != SENSOR_SOI968 && sd->sensor != SENSOR_MT9VPRB &&
+	    sd->sensor != SENSOR_MT9M112 && sd->sensor != SENSOR_MT9M111 &&
+	    sd->sensor != SENSOR_MT9V111)
+		sd->exposure = v4l2_ctrl_new_std(hdl, &sd_ctrl_ops,
+			V4L2_CID_EXPOSURE, 0, 0x1780, 1, 0x33);
+
+	if (sd->sensor != SENSOR_MT9VPRB && sd->sensor != SENSOR_MT9M112 &&
+	    sd->sensor != SENSOR_MT9M111 && sd->sensor != SENSOR_MT9V111) {
+		sd->gain = v4l2_ctrl_new_std(hdl, &sd_ctrl_ops,
+			V4L2_CID_GAIN, 0, 28, 1, 0);
+		sd->autogain = v4l2_ctrl_new_std(hdl, &sd_ctrl_ops,
+			V4L2_CID_AUTOGAIN, 0, 1, 1, 1);
+		if (sd->sensor == SENSOR_SOI968)
+			/* this sensor doesn't have the exposure control and
+			   autogain is clustered with gain instead. This works
+			   because sd->exposure == NULL. */
+			v4l2_ctrl_auto_cluster(3, &sd->autogain, 0, false);
+		else
+			/* Otherwise autogain is clustered with exposure. */
+			v4l2_ctrl_auto_cluster(2, &sd->autogain, 0, false);
+	}
+
+	sd->jpegqual = v4l2_ctrl_new_std(hdl, &sd_ctrl_ops,
+			V4L2_CID_JPEG_COMPRESSION_QUALITY, 50, 90, 1, 80);
+	if (hdl->error) {
+		pr_err("Could not initialize controls\n");
+		return hdl->error;
+	}
+	return 0;
+}
+
 static int sd_init(struct gspca_dev *gspca_dev)
 {
 	struct sd *sd = (struct sd *) gspca_dev;
@@ -1949,7 +1905,6 @@ static int sd_init(struct gspca_dev *gspca_dev)
 		pr_err("Unsupported sensor\n");
 		gspca_dev->usb_err = -ENODEV;
 	}
-
 	return gspca_dev->usb_err;
 }
 
@@ -2025,8 +1980,8 @@ static int sd_isoc_init(struct gspca_dev *gspca_dev)
 
 		if (intf->num_altsetting != 9) {
 			pr_warn("sn9c20x camera with unknown number of alt "
-			        "settings (%d), please report!\n",
-			        intf->num_altsetting);
+				"settings (%d), please report!\n",
+				intf->num_altsetting);
 			gspca_dev->alt = intf->num_altsetting;
 			return 0;
 		}
@@ -2067,7 +2022,7 @@ static int sd_start(struct gspca_dev *gspca_dev)
 
 	jpeg_define(sd->jpeg_hdr, height, width,
 			0x21);
-	jpeg_set_qual(sd->jpeg_hdr, sd->ctrls[QUALITY].val);
+	jpeg_set_qual(sd->jpeg_hdr, v4l2_ctrl_g_ctrl(sd->jpegqual));
 
 	if (mode & MODE_RAW)
 		fmt = 0x2d;
@@ -2104,12 +2059,17 @@ static int sd_start(struct gspca_dev *gspca_dev)
 	reg_w1(gspca_dev, 0x1189, scale);
 	reg_w1(gspca_dev, 0x10e0, fmt);
 
-	set_cmatrix(gspca_dev);
-	set_gamma(gspca_dev);
-	set_redblue(gspca_dev);
-	set_gain(gspca_dev);
-	set_exposure(gspca_dev);
-	set_hvflip(gspca_dev);
+	set_cmatrix(gspca_dev, v4l2_ctrl_g_ctrl(sd->brightness),
+			v4l2_ctrl_g_ctrl(sd->contrast),
+			v4l2_ctrl_g_ctrl(sd->saturation),
+			v4l2_ctrl_g_ctrl(sd->hue));
+	set_gamma(gspca_dev, v4l2_ctrl_g_ctrl(sd->gamma));
+	set_redblue(gspca_dev, v4l2_ctrl_g_ctrl(sd->blue),
+			v4l2_ctrl_g_ctrl(sd->red));
+	set_gain(gspca_dev, v4l2_ctrl_g_ctrl(sd->gain));
+	set_exposure(gspca_dev, v4l2_ctrl_g_ctrl(sd->exposure));
+	set_hvflip(gspca_dev, v4l2_ctrl_g_ctrl(sd->hflip),
+			v4l2_ctrl_g_ctrl(sd->vflip));
 
 	reg_w1(gspca_dev, 0x1007, 0x20);
 	reg_w1(gspca_dev, 0x1061, 0x03);
@@ -2148,6 +2108,9 @@ static void sd_stop0(struct gspca_dev *gspca_dev)
 static void do_autoexposure(struct gspca_dev *gspca_dev, u16 avg_lum)
 {
 	struct sd *sd = (struct sd *) gspca_dev;
+	s32 cur_exp = v4l2_ctrl_g_ctrl(sd->exposure);
+	s32 max = sd->exposure->maximum - sd->exposure_step;
+	s32 min = sd->exposure->minimum + sd->exposure_step;
 	s16 new_exp;
 
 	/*
@@ -2156,16 +2119,15 @@ static void do_autoexposure(struct gspca_dev *gspca_dev, u16 avg_lum)
 	 * and exposure steps
 	 */
 	if (avg_lum < MIN_AVG_LUM) {
-		if (sd->ctrls[EXPOSURE].val > 0x1770)
+		if (cur_exp > max)
 			return;
 
-		new_exp = sd->ctrls[EXPOSURE].val + sd->exposure_step;
-		if (new_exp > 0x1770)
-			new_exp = 0x1770;
-		if (new_exp < 0x10)
-			new_exp = 0x10;
-		sd->ctrls[EXPOSURE].val = new_exp;
-		set_exposure(gspca_dev);
+		new_exp = cur_exp + sd->exposure_step;
+		if (new_exp > max)
+			new_exp = max;
+		if (new_exp < min)
+			new_exp = min;
+		v4l2_ctrl_s_ctrl(sd->exposure, new_exp);
 
 		sd->older_step = sd->old_step;
 		sd->old_step = 1;
@@ -2176,15 +2138,14 @@ static void do_autoexposure(struct gspca_dev *gspca_dev, u16 avg_lum)
 			sd->exposure_step += 2;
 	}
 	if (avg_lum > MAX_AVG_LUM) {
-		if (sd->ctrls[EXPOSURE].val < 0x10)
+		if (cur_exp < min)
 			return;
-		new_exp = sd->ctrls[EXPOSURE].val - sd->exposure_step;
-		if (new_exp > 0x1700)
-			new_exp = 0x1770;
-		if (new_exp < 0x10)
-			new_exp = 0x10;
-		sd->ctrls[EXPOSURE].val = new_exp;
-		set_exposure(gspca_dev);
+		new_exp = cur_exp - sd->exposure_step;
+		if (new_exp > max)
+			new_exp = max;
+		if (new_exp < min)
+			new_exp = min;
+		v4l2_ctrl_s_ctrl(sd->exposure, new_exp);
 		sd->older_step = sd->old_step;
 		sd->old_step = 0;
 
@@ -2198,19 +2159,12 @@ static void do_autoexposure(struct gspca_dev *gspca_dev, u16 avg_lum)
 static void do_autogain(struct gspca_dev *gspca_dev, u16 avg_lum)
 {
 	struct sd *sd = (struct sd *) gspca_dev;
+	s32 cur_gain = v4l2_ctrl_g_ctrl(sd->gain);
 
-	if (avg_lum < MIN_AVG_LUM) {
-		if (sd->ctrls[GAIN].val + 1 <= 28) {
-			sd->ctrls[GAIN].val++;
-			set_gain(gspca_dev);
-		}
-	}
-	if (avg_lum > MAX_AVG_LUM) {
-		if (sd->ctrls[GAIN].val > 0) {
-			sd->ctrls[GAIN].val--;
-			set_gain(gspca_dev);
-		}
-	}
+	if (avg_lum < MIN_AVG_LUM && cur_gain < sd->gain->maximum)
+		v4l2_ctrl_s_ctrl(sd->gain, cur_gain + 1);
+	if (avg_lum > MAX_AVG_LUM && cur_gain > sd->gain->minimum)
+		v4l2_ctrl_s_ctrl(sd->gain, cur_gain - 1);
 }
 
 static void sd_dqcallback(struct gspca_dev *gspca_dev)
@@ -2218,7 +2172,7 @@ static void sd_dqcallback(struct gspca_dev *gspca_dev)
 	struct sd *sd = (struct sd *) gspca_dev;
 	int avg_lum;
 
-	if (!sd->ctrls[AUTOGAIN].val)
+	if (!v4l2_ctrl_g_ctrl(sd->autogain))
 		return;
 
 	avg_lum = atomic_read(&sd->avg_lum);
@@ -2234,10 +2188,11 @@ static void qual_upd(struct work_struct *work)
 {
 	struct sd *sd = container_of(work, struct sd, work);
 	struct gspca_dev *gspca_dev = &sd->gspca_dev;
+	s32 qual = v4l2_ctrl_g_ctrl(sd->jpegqual);
 
 	mutex_lock(&gspca_dev->usb_lock);
-	PDEBUG(D_STREAM, "qual_upd %d%%", sd->ctrls[QUALITY].val);
-	set_quality(gspca_dev);
+	PDEBUG(D_STREAM, "qual_upd %d%%", qual);
+	set_quality(gspca_dev, qual);
 	mutex_unlock(&gspca_dev->usb_lock);
 }
 
@@ -2286,14 +2241,18 @@ static void transfer_check(struct gspca_dev *gspca_dev,
 	if (new_qual != 0) {
 		sd->nchg += new_qual;
 		if (sd->nchg < -6 || sd->nchg >= 12) {
+			/* Note: we are in interrupt context, so we can't
+			   use v4l2_ctrl_g/s_ctrl here. Access the value
+			   directly instead. */
+			s32 curqual = sd->jpegqual->cur.val;
 			sd->nchg = 0;
-			new_qual += sd->ctrls[QUALITY].val;
-			if (new_qual < QUALITY_MIN)
-				new_qual = QUALITY_MIN;
-			else if (new_qual > QUALITY_MAX)
-				new_qual = QUALITY_MAX;
-			if (new_qual != sd->ctrls[QUALITY].val) {
-				sd->ctrls[QUALITY].val = new_qual;
+			new_qual += curqual;
+			if (new_qual < sd->jpegqual->minimum)
+				new_qual = sd->jpegqual->minimum;
+			else if (new_qual > sd->jpegqual->maximum)
+				new_qual = sd->jpegqual->maximum;
+			if (new_qual != curqual) {
+				sd->jpegqual->cur.val = new_qual;
 				queue_work(sd->work_thread, &sd->work);
 			}
 		}
@@ -2309,7 +2268,7 @@ static void sd_pkt_scan(struct gspca_dev *gspca_dev,
 {
 	struct sd *sd = (struct sd *) gspca_dev;
 	int avg_lum, is_jpeg;
-	static u8 frame_header[] =
+	static const u8 frame_header[] =
 		{0xff, 0xff, 0x00, 0xc4, 0xc4, 0x96};
 
 	is_jpeg = (sd->fmt & 0x03) == 0;
@@ -2373,10 +2332,9 @@ static void sd_pkt_scan(struct gspca_dev *gspca_dev,
 /* sub-driver description */
 static const struct sd_desc sd_desc = {
 	.name = KBUILD_MODNAME,
-	.ctrls = sd_ctrls,
-	.nctrls = ARRAY_SIZE(sd_ctrls),
 	.config = sd_config,
 	.init = sd_init,
+	.init_controls = sd_init_controls,
 	.isoc_init = sd_isoc_init,
 	.start = sd_start,
 	.stopN = sd_stopN,
diff --git a/drivers/media/video/gspca/sonixb.c b/drivers/media/video/gspca/sonixb.c
index 6a1148d7fe9..e2bdf8f632f 100644
--- a/drivers/media/video/gspca/sonixb.c
+++ b/drivers/media/video/gspca/sonixb.c
@@ -1000,6 +1000,8 @@ static void setfreq(struct gspca_dev *gspca_dev)
 	}
 }
 
+#define WANT_REGULAR_AUTOGAIN
+#define WANT_COARSE_EXPO_AUTOGAIN
 #include "autogain_functions.h"
 
 static void do_autogain(struct gspca_dev *gspca_dev)
diff --git a/drivers/media/video/gspca/sonixj.c b/drivers/media/video/gspca/sonixj.c
index db8e5084df0..4d1696d1a7f 100644
--- a/drivers/media/video/gspca/sonixj.c
+++ b/drivers/media/video/gspca/sonixj.c
@@ -2800,10 +2800,7 @@ static void sd_stop0(struct gspca_dev *gspca_dev)
 	}
 }
 
-/* !! coarse_grained_expo_autogain is not used !! */
-#define exp_too_low_cnt bridge
-#define exp_too_high_cnt sensor
-
+#define WANT_REGULAR_AUTOGAIN
 #include "autogain_functions.h"
 
 static void do_autogain(struct gspca_dev *gspca_dev)
@@ -2923,6 +2920,10 @@ static void sd_pkt_scan(struct gspca_dev *gspca_dev,
 	 * not the JPEG end of frame ('ff d9').
 	 */
 
+	/* count the packets and their size */
+	sd->npkt++;
+	sd->pktsz += len;
+
 /*fixme: assumption about the following code:
  *	- there can be only one marker in a packet
  */
@@ -2945,10 +2946,6 @@ static void sd_pkt_scan(struct gspca_dev *gspca_dev,
 		data += i;
 	}
 
-	/* count the packets and their size */
-	sd->npkt++;
-	sd->pktsz += len;
-
 	/* search backwards if there is a marker in the packet */
 	for (i = len - 1; --i >= 0; ) {
 		if (data[i] != 0xff) {
diff --git a/drivers/media/video/gspca/sq905.c b/drivers/media/video/gspca/sq905.c
index 2fe3c29bd6b..04f54654a02 100644
--- a/drivers/media/video/gspca/sq905.c
+++ b/drivers/media/video/gspca/sq905.c
@@ -232,7 +232,11 @@ static void sq905_dostream(struct work_struct *work)
 	frame_sz = gspca_dev->cam.cam_mode[gspca_dev->curr_mode].sizeimage
 			+ FRAME_HEADER_LEN;
 
-	while (gspca_dev->present && gspca_dev->streaming) {
+	while (gspca_dev->dev && gspca_dev->streaming) {
+#ifdef CONFIG_PM
+		if (gspca_dev->frozen)
+			break;
+#endif
 		/* request some data and then read it until we have
 		 * a complete frame. */
 		bytes_left = frame_sz;
@@ -242,7 +246,7 @@ static void sq905_dostream(struct work_struct *work)
 		   we must finish reading an entire frame, otherwise the
 		   next time we stream we start reading in the middle of a
 		   frame. */
-		while (bytes_left > 0 && gspca_dev->present) {
+		while (bytes_left > 0 && gspca_dev->dev) {
 			data_len = bytes_left > SQ905_MAX_TRANSFER ?
 				SQ905_MAX_TRANSFER : bytes_left;
 			ret = sq905_read_data(gspca_dev, buffer, data_len, 1);
@@ -274,7 +278,7 @@ static void sq905_dostream(struct work_struct *work)
 				gspca_frame_add(gspca_dev, LAST_PACKET,
 						NULL, 0);
 		}
-		if (gspca_dev->present) {
+		if (gspca_dev->dev) {
 			/* acknowledge the frame */
 			mutex_lock(&gspca_dev->usb_lock);
 			ret = sq905_ack_frame(gspca_dev);
@@ -284,7 +288,7 @@ static void sq905_dostream(struct work_struct *work)
 		}
 	}
 quit_stream:
-	if (gspca_dev->present) {
+	if (gspca_dev->dev) {
 		mutex_lock(&gspca_dev->usb_lock);
 		sq905_command(gspca_dev, SQ905_CLEAR);
 		mutex_unlock(&gspca_dev->usb_lock);
diff --git a/drivers/media/video/gspca/sq905c.c b/drivers/media/video/gspca/sq905c.c
index ae783634712..f34ddb0570c 100644
--- a/drivers/media/video/gspca/sq905c.c
+++ b/drivers/media/video/gspca/sq905c.c
@@ -150,7 +150,11 @@ static void sq905c_dostream(struct work_struct *work)
 		goto quit_stream;
 	}
 
-	while (gspca_dev->present && gspca_dev->streaming) {
+	while (gspca_dev->dev && gspca_dev->streaming) {
+#ifdef CONFIG_PM
+		if (gspca_dev->frozen)
+			break;
+#endif
 		/* Request the header, which tells the size to download */
 		ret = usb_bulk_msg(gspca_dev->dev,
 				usb_rcvbulkpipe(gspca_dev->dev, 0x81),
@@ -169,7 +173,7 @@ static void sq905c_dostream(struct work_struct *work)
 		packet_type = FIRST_PACKET;
 		gspca_frame_add(gspca_dev, packet_type,
 				buffer, FRAME_HEADER_LEN);
-		while (bytes_left > 0 && gspca_dev->present) {
+		while (bytes_left > 0 && gspca_dev->dev) {
 			data_len = bytes_left > SQ905C_MAX_TRANSFER ?
 				SQ905C_MAX_TRANSFER : bytes_left;
 			ret = usb_bulk_msg(gspca_dev->dev,
@@ -191,7 +195,7 @@ static void sq905c_dostream(struct work_struct *work)
 		}
 	}
 quit_stream:
-	if (gspca_dev->present) {
+	if (gspca_dev->dev) {
 		mutex_lock(&gspca_dev->usb_lock);
 		sq905c_command(gspca_dev, SQ905C_CLEAR, 0);
 		mutex_unlock(&gspca_dev->usb_lock);
diff --git a/drivers/media/video/gspca/stv06xx/stv06xx.c b/drivers/media/video/gspca/stv06xx/stv06xx.c
index 91d99b4cc57..999ec776444 100644
--- a/drivers/media/video/gspca/stv06xx/stv06xx.c
+++ b/drivers/media/video/gspca/stv06xx/stv06xx.c
@@ -261,6 +261,17 @@ static int stv06xx_init(struct gspca_dev *gspca_dev)
 	return (err < 0) ? err : 0;
 }
 
+/* this function is called at probe time */
+static int stv06xx_init_controls(struct gspca_dev *gspca_dev)
+{
+	struct sd *sd = (struct sd *) gspca_dev;
+
+	PDEBUG(D_PROBE, "Initializing controls");
+
+	gspca_dev->vdev.ctrl_handler = &gspca_dev->ctrl_handler;
+	return sd->sensor->init_controls(sd);
+}
+
 /* Start the camera */
 static int stv06xx_start(struct gspca_dev *gspca_dev)
 {
@@ -512,6 +523,7 @@ static const struct sd_desc sd_desc = {
 	.name = MODULE_NAME,
 	.config = stv06xx_config,
 	.init = stv06xx_init,
+	.init_controls = stv06xx_init_controls,
 	.start = stv06xx_start,
 	.stopN = stv06xx_stopN,
 	.pkt_scan = stv06xx_pkt_scan,
@@ -530,9 +542,8 @@ static int stv06xx_config(struct gspca_dev *gspca_dev,
 
 	PDEBUG(D_PROBE, "Configuring camera");
 
-	sd->desc = sd_desc;
 	sd->bridge = id->driver_info;
-	gspca_dev->sd_desc = &sd->desc;
+	gspca_dev->sd_desc = &sd_desc;
 
 	if (dump_bridge)
 		stv06xx_dump_bridge(sd);
@@ -594,11 +605,12 @@ static void sd_disconnect(struct usb_interface *intf)
 {
 	struct gspca_dev *gspca_dev = usb_get_intfdata(intf);
 	struct sd *sd = (struct sd *) gspca_dev;
+	void *priv = sd->sensor_priv;
 	PDEBUG(D_PROBE, "Disconnecting the stv06xx device");
 
-	if (sd->sensor->disconnect)
-		sd->sensor->disconnect(sd);
+	sd->sensor = NULL;
 	gspca_disconnect(intf);
+	kfree(priv);
 }
 
 static struct usb_driver sd_driver = {
@@ -609,6 +621,7 @@ static struct usb_driver sd_driver = {
 #ifdef CONFIG_PM
 	.suspend = gspca_suspend,
 	.resume = gspca_resume,
+	.reset_resume = gspca_resume,
 #endif
 };
 
diff --git a/drivers/media/video/gspca/stv06xx/stv06xx.h b/drivers/media/video/gspca/stv06xx/stv06xx.h
index d270a5981af..34957a4ec15 100644
--- a/drivers/media/video/gspca/stv06xx/stv06xx.h
+++ b/drivers/media/video/gspca/stv06xx/stv06xx.h
@@ -89,9 +89,6 @@ struct sd {
 	/* A pointer to the currently connected sensor */
 	const struct stv06xx_sensor *sensor;
 
-	/* A pointer to the sd_desc struct */
-	struct sd_desc desc;
-
 	/* Sensor private data */
 	void *sensor_priv;
 
diff --git a/drivers/media/video/gspca/stv06xx/stv06xx_hdcs.c b/drivers/media/video/gspca/stv06xx/stv06xx_hdcs.c
index a8698b7a756..06fa54c5efb 100644
--- a/drivers/media/video/gspca/stv06xx/stv06xx_hdcs.c
+++ b/drivers/media/video/gspca/stv06xx/stv06xx_hdcs.c
@@ -32,36 +32,6 @@
 
 #include "stv06xx_hdcs.h"
 
-static const struct ctrl hdcs1x00_ctrl[] = {
-	{
-		{
-			.id		= V4L2_CID_EXPOSURE,
-			.type		= V4L2_CTRL_TYPE_INTEGER,
-			.name		= "exposure",
-			.minimum	= 0x00,
-			.maximum	= 0xff,
-			.step		= 0x1,
-			.default_value	= HDCS_DEFAULT_EXPOSURE,
-			.flags		= V4L2_CTRL_FLAG_SLIDER
-		},
-		.set = hdcs_set_exposure,
-		.get = hdcs_get_exposure
-	}, {
-		{
-			.id		= V4L2_CID_GAIN,
-			.type		= V4L2_CTRL_TYPE_INTEGER,
-			.name		= "gain",
-			.minimum	= 0x00,
-			.maximum	= 0xff,
-			.step		= 0x1,
-			.default_value	= HDCS_DEFAULT_GAIN,
-			.flags		= V4L2_CTRL_FLAG_SLIDER
-		},
-		.set = hdcs_set_gain,
-		.get = hdcs_get_gain
-	}
-};
-
 static struct v4l2_pix_format hdcs1x00_mode[] = {
 	{
 		HDCS_1X00_DEF_WIDTH,
@@ -76,36 +46,6 @@ static struct v4l2_pix_format hdcs1x00_mode[] = {
 	}
 };
 
-static const struct ctrl hdcs1020_ctrl[] = {
-	{
-		{
-			.id		= V4L2_CID_EXPOSURE,
-			.type		= V4L2_CTRL_TYPE_INTEGER,
-			.name		= "exposure",
-			.minimum	= 0x00,
-			.maximum	= 0xffff,
-			.step		= 0x1,
-			.default_value	= HDCS_DEFAULT_EXPOSURE,
-			.flags		= V4L2_CTRL_FLAG_SLIDER
-		},
-		.set = hdcs_set_exposure,
-		.get = hdcs_get_exposure
-	}, {
-		{
-			.id		= V4L2_CID_GAIN,
-			.type		= V4L2_CTRL_TYPE_INTEGER,
-			.name		= "gain",
-			.minimum	= 0x00,
-			.maximum	= 0xff,
-			.step		= 0x1,
-			.default_value	= HDCS_DEFAULT_GAIN,
-			.flags		= V4L2_CTRL_FLAG_SLIDER
-		},
-		.set = hdcs_set_gain,
-		.get = hdcs_get_gain
-	}
-};
-
 static struct v4l2_pix_format hdcs1020_mode[] = {
 	{
 		HDCS_1020_DEF_WIDTH,
@@ -150,7 +90,6 @@ struct hdcs {
 	} exp;
 
 	int psmp;
-	u8 exp_cache, gain_cache;
 };
 
 static int hdcs_reg_write_seq(struct sd *sd, u8 reg, u8 *vals, u8 len)
@@ -232,16 +171,6 @@ static int hdcs_reset(struct sd *sd)
 	return err;
 }
 
-static int hdcs_get_exposure(struct gspca_dev *gspca_dev, __s32 *val)
-{
-	struct sd *sd = (struct sd *) gspca_dev;
-	struct hdcs *hdcs = sd->sensor_priv;
-
-	*val = hdcs->exp_cache;
-
-	return 0;
-}
-
 static int hdcs_set_exposure(struct gspca_dev *gspca_dev, __s32 val)
 {
 	struct sd *sd = (struct sd *) gspca_dev;
@@ -260,9 +189,6 @@ static int hdcs_set_exposure(struct gspca_dev *gspca_dev, __s32 val)
 	int cycles, err;
 	u8 exp[14];
 
-	val &= 0xff;
-	hdcs->exp_cache = val;
-
 	cycles = val * HDCS_CLK_FREQ_MHZ * 257;
 
 	ct = hdcs->exp.cto + hdcs->psmp + (HDCS_ADC_START_SIG_DUR + 2);
@@ -336,12 +262,9 @@ static int hdcs_set_exposure(struct gspca_dev *gspca_dev, __s32 val)
 
 static int hdcs_set_gains(struct sd *sd, u8 g)
 {
-	struct hdcs *hdcs = sd->sensor_priv;
 	int err;
 	u8 gains[4];
 
-	hdcs->gain_cache = g;
-
 	/* the voltage gain Av = (1 + 19 * val / 127) * (1 + bit7) */
 	if (g > 127)
 		g = 0x80 | (g / 2);
@@ -352,17 +275,7 @@ static int hdcs_set_gains(struct sd *sd, u8 g)
 	gains[3] = g;
 
 	err = hdcs_reg_write_seq(sd, HDCS_ERECPGA, gains, 4);
-		return err;
-}
-
-static int hdcs_get_gain(struct gspca_dev *gspca_dev, __s32 *val)
-{
-	struct sd *sd = (struct sd *) gspca_dev;
-	struct hdcs *hdcs = sd->sensor_priv;
-
-	*val = hdcs->gain_cache;
-
-	return 0;
+	return err;
 }
 
 static int hdcs_set_gain(struct gspca_dev *gspca_dev, __s32 val)
@@ -420,6 +333,39 @@ static int hdcs_set_size(struct sd *sd,
 	return err;
 }
 
+static int hdcs_s_ctrl(struct v4l2_ctrl *ctrl)
+{
+	struct gspca_dev *gspca_dev =
+		container_of(ctrl->handler, struct gspca_dev, ctrl_handler);
+	int err = -EINVAL;
+
+	switch (ctrl->id) {
+	case V4L2_CID_GAIN:
+		err = hdcs_set_gain(gspca_dev, ctrl->val);
+		break;
+	case V4L2_CID_EXPOSURE:
+		err = hdcs_set_exposure(gspca_dev, ctrl->val);
+		break;
+	}
+	return err;
+}
+
+static const struct v4l2_ctrl_ops hdcs_ctrl_ops = {
+	.s_ctrl = hdcs_s_ctrl,
+};
+
+static int hdcs_init_controls(struct sd *sd)
+{
+	struct v4l2_ctrl_handler *hdl = &sd->gspca_dev.ctrl_handler;
+
+	v4l2_ctrl_handler_init(hdl, 2);
+	v4l2_ctrl_new_std(hdl, &hdcs_ctrl_ops,
+			V4L2_CID_EXPOSURE, 0, 0xff, 1, HDCS_DEFAULT_EXPOSURE);
+	v4l2_ctrl_new_std(hdl, &hdcs_ctrl_ops,
+			V4L2_CID_GAIN, 0, 0xff, 1, HDCS_DEFAULT_GAIN);
+	return hdl->error;
+}
+
 static int hdcs_probe_1x00(struct sd *sd)
 {
 	struct hdcs *hdcs;
@@ -434,8 +380,6 @@ static int hdcs_probe_1x00(struct sd *sd)
 
 	sd->gspca_dev.cam.cam_mode = hdcs1x00_mode;
 	sd->gspca_dev.cam.nmodes = ARRAY_SIZE(hdcs1x00_mode);
-	sd->desc.ctrls = hdcs1x00_ctrl;
-	sd->desc.nctrls = ARRAY_SIZE(hdcs1x00_ctrl);
 
 	hdcs = kmalloc(sizeof(struct hdcs), GFP_KERNEL);
 	if (!hdcs)
@@ -493,8 +437,6 @@ static int hdcs_probe_1020(struct sd *sd)
 
 	sd->gspca_dev.cam.cam_mode = hdcs1020_mode;
 	sd->gspca_dev.cam.nmodes = ARRAY_SIZE(hdcs1020_mode);
-	sd->desc.ctrls = hdcs1020_ctrl;
-	sd->desc.nctrls = ARRAY_SIZE(hdcs1020_ctrl);
 
 	hdcs = kmalloc(sizeof(struct hdcs), GFP_KERNEL);
 	if (!hdcs)
@@ -537,12 +479,6 @@ static int hdcs_stop(struct sd *sd)
 	return hdcs_set_state(sd, HDCS_STATE_SLEEP);
 }
 
-static void hdcs_disconnect(struct sd *sd)
-{
-	PDEBUG(D_PROBE, "Disconnecting the sensor");
-	kfree(sd->sensor_priv);
-}
-
 static int hdcs_init(struct sd *sd)
 {
 	struct hdcs *hdcs = sd->sensor_priv;
@@ -587,16 +523,7 @@ static int hdcs_init(struct sd *sd)
 	if (err < 0)
 		return err;
 
-	err = hdcs_set_gains(sd, HDCS_DEFAULT_GAIN);
-	if (err < 0)
-		return err;
-
-	err = hdcs_set_size(sd, hdcs->array.width, hdcs->array.height);
-	if (err < 0)
-		return err;
-
-	err = hdcs_set_exposure(&sd->gspca_dev, HDCS_DEFAULT_EXPOSURE);
-	return err;
+	return hdcs_set_size(sd, hdcs->array.width, hdcs->array.height);
 }
 
 static int hdcs_dump(struct sd *sd)
diff --git a/drivers/media/video/gspca/stv06xx/stv06xx_hdcs.h b/drivers/media/video/gspca/stv06xx/stv06xx_hdcs.h
index a14a84a5079..1ba9158d010 100644
--- a/drivers/media/video/gspca/stv06xx/stv06xx_hdcs.h
+++ b/drivers/media/video/gspca/stv06xx/stv06xx_hdcs.h
@@ -131,14 +131,12 @@ static int hdcs_probe_1x00(struct sd *sd);
 static int hdcs_probe_1020(struct sd *sd);
 static int hdcs_start(struct sd *sd);
 static int hdcs_init(struct sd *sd);
+static int hdcs_init_controls(struct sd *sd);
 static int hdcs_stop(struct sd *sd);
 static int hdcs_dump(struct sd *sd);
-static void hdcs_disconnect(struct sd *sd);
 
-static int hdcs_get_exposure(struct gspca_dev *gspca_dev, __s32 *val);
 static int hdcs_set_exposure(struct gspca_dev *gspca_dev, __s32 val);
 static int hdcs_set_gain(struct gspca_dev *gspca_dev, __s32 val);
-static int hdcs_get_gain(struct gspca_dev *gspca_dev, __s32 *val);
 
 const struct stv06xx_sensor stv06xx_sensor_hdcs1x00 = {
 	.name = "HP HDCS-1000/1100",
@@ -152,10 +150,10 @@ const struct stv06xx_sensor stv06xx_sensor_hdcs1x00 = {
 	.max_packet_size = { 847 },
 
 	.init = hdcs_init,
+	.init_controls = hdcs_init_controls,
 	.probe = hdcs_probe_1x00,
 	.start = hdcs_start,
 	.stop = hdcs_stop,
-	.disconnect = hdcs_disconnect,
 	.dump = hdcs_dump,
 };
 
@@ -171,6 +169,7 @@ const struct stv06xx_sensor stv06xx_sensor_hdcs1020 = {
 	.max_packet_size = { 847 },
 
 	.init = hdcs_init,
+	.init_controls = hdcs_init_controls,
 	.probe = hdcs_probe_1020,
 	.start = hdcs_start,
 	.stop = hdcs_stop,
diff --git a/drivers/media/video/gspca/stv06xx/stv06xx_pb0100.c b/drivers/media/video/gspca/stv06xx/stv06xx_pb0100.c
index 26f14fc4a13..cdfc3d05ab6 100644
--- a/drivers/media/video/gspca/stv06xx/stv06xx_pb0100.c
+++ b/drivers/media/video/gspca/stv06xx/stv06xx_pb0100.c
@@ -48,105 +48,16 @@
 
 #include "stv06xx_pb0100.h"
 
-static const struct ctrl pb0100_ctrl[] = {
-#define GAIN_IDX 0
-	{
-		{
-			.id		= V4L2_CID_GAIN,
-			.type		= V4L2_CTRL_TYPE_INTEGER,
-			.name		= "Gain",
-			.minimum	= 0,
-			.maximum	= 255,
-			.step		= 1,
-			.default_value  = 128
-		},
-		.set = pb0100_set_gain,
-		.get = pb0100_get_gain
-	},
-#define RED_BALANCE_IDX 1
-	{
-		{
-			.id		= V4L2_CID_RED_BALANCE,
-			.type		= V4L2_CTRL_TYPE_INTEGER,
-			.name		= "Red Balance",
-			.minimum	= -255,
-			.maximum	= 255,
-			.step		= 1,
-			.default_value  = 0
-		},
-		.set = pb0100_set_red_balance,
-		.get = pb0100_get_red_balance
-	},
-#define BLUE_BALANCE_IDX 2
-	{
-		{
-			.id		= V4L2_CID_BLUE_BALANCE,
-			.type		= V4L2_CTRL_TYPE_INTEGER,
-			.name		= "Blue Balance",
-			.minimum	= -255,
-			.maximum	= 255,
-			.step		= 1,
-			.default_value  = 0
-		},
-		.set = pb0100_set_blue_balance,
-		.get = pb0100_get_blue_balance
-	},
-#define EXPOSURE_IDX 3
-	{
-		{
-			.id		= V4L2_CID_EXPOSURE,
-			.type		= V4L2_CTRL_TYPE_INTEGER,
-			.name		= "Exposure",
-			.minimum	= 0,
-			.maximum	= 511,
-			.step		= 1,
-			.default_value  = 12
-		},
-		.set = pb0100_set_exposure,
-		.get = pb0100_get_exposure
-	},
-#define AUTOGAIN_IDX 4
-	{
-		{
-			.id		= V4L2_CID_AUTOGAIN,
-			.type		= V4L2_CTRL_TYPE_BOOLEAN,
-			.name		= "Automatic Gain and Exposure",
-			.minimum	= 0,
-			.maximum	= 1,
-			.step		= 1,
-			.default_value  = 1
-		},
-		.set = pb0100_set_autogain,
-		.get = pb0100_get_autogain
-	},
-#define AUTOGAIN_TARGET_IDX 5
-	{
-		{
-			.id		= V4L2_CTRL_CLASS_USER + 0x1000,
-			.type		= V4L2_CTRL_TYPE_INTEGER,
-			.name		= "Automatic Gain Target",
-			.minimum	= 0,
-			.maximum	= 255,
-			.step		= 1,
-			.default_value  = 128
-		},
-		.set = pb0100_set_autogain_target,
-		.get = pb0100_get_autogain_target
-	},
-#define NATURAL_IDX 6
-	{
-		{
-			.id		= V4L2_CTRL_CLASS_USER + 0x1001,
-			.type		= V4L2_CTRL_TYPE_BOOLEAN,
-			.name		= "Natural Light Source",
-			.minimum	= 0,
-			.maximum	= 1,
-			.step		= 1,
-			.default_value  = 1
-		},
-		.set = pb0100_set_natural,
-		.get = pb0100_get_natural
-	}
+struct pb0100_ctrls {
+	struct { /* one big happy control cluster... */
+		struct v4l2_ctrl *autogain;
+		struct v4l2_ctrl *gain;
+		struct v4l2_ctrl *exposure;
+		struct v4l2_ctrl *red;
+		struct v4l2_ctrl *blue;
+		struct v4l2_ctrl *natural;
+	};
+	struct v4l2_ctrl *target;
 };
 
 static struct v4l2_pix_format pb0100_mode[] = {
@@ -174,38 +85,104 @@ static struct v4l2_pix_format pb0100_mode[] = {
 	}
 };
 
+static int pb0100_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;
+	struct pb0100_ctrls *ctrls = sd->sensor_priv;
+	int err = -EINVAL;
+
+	switch (ctrl->id) {
+	case V4L2_CID_AUTOGAIN:
+		err = pb0100_set_autogain(gspca_dev, ctrl->val);
+		if (err)
+			break;
+		if (ctrl->val)
+			break;
+		err = pb0100_set_gain(gspca_dev, ctrls->gain->val);
+		if (err)
+			break;
+		err = pb0100_set_exposure(gspca_dev, ctrls->exposure->val);
+		break;
+	case V4L2_CTRL_CLASS_USER + 0x1001:
+		err = pb0100_set_autogain_target(gspca_dev, ctrl->val);
+		break;
+	}
+	return err;
+}
+
+static const struct v4l2_ctrl_ops pb0100_ctrl_ops = {
+	.s_ctrl = pb0100_s_ctrl,
+};
+
+static int pb0100_init_controls(struct sd *sd)
+{
+	struct v4l2_ctrl_handler *hdl = &sd->gspca_dev.ctrl_handler;
+	struct pb0100_ctrls *ctrls;
+	static const struct v4l2_ctrl_config autogain_target = {
+		.ops = &pb0100_ctrl_ops,
+		.id = V4L2_CTRL_CLASS_USER + 0x1000,
+		.type = V4L2_CTRL_TYPE_INTEGER,
+		.name = "Automatic Gain Target",
+		.max = 255,
+		.step = 1,
+		.def = 128,
+	};
+	static const struct v4l2_ctrl_config natural_light = {
+		.ops = &pb0100_ctrl_ops,
+		.id = V4L2_CTRL_CLASS_USER + 0x1001,
+		.type = V4L2_CTRL_TYPE_BOOLEAN,
+		.name = "Natural Light Source",
+		.max = 1,
+		.step = 1,
+		.def = 1,
+	};
+
+	ctrls = kzalloc(sizeof(*ctrls), GFP_KERNEL);
+	if (!ctrls)
+		return -ENOMEM;
+
+	v4l2_ctrl_handler_init(hdl, 6);
+	ctrls->autogain = v4l2_ctrl_new_std(hdl, &pb0100_ctrl_ops,
+			V4L2_CID_AUTOGAIN, 0, 1, 1, 1);
+	ctrls->exposure = v4l2_ctrl_new_std(hdl, &pb0100_ctrl_ops,
+			V4L2_CID_EXPOSURE, 0, 511, 1, 12);
+	ctrls->gain = v4l2_ctrl_new_std(hdl, &pb0100_ctrl_ops,
+			V4L2_CID_GAIN, 0, 255, 1, 128);
+	ctrls->red = v4l2_ctrl_new_std(hdl, &pb0100_ctrl_ops,
+			V4L2_CID_RED_BALANCE, -255, 255, 1, 0);
+	ctrls->blue = v4l2_ctrl_new_std(hdl, &pb0100_ctrl_ops,
+			V4L2_CID_BLUE_BALANCE, -255, 255, 1, 0);
+	ctrls->natural = v4l2_ctrl_new_custom(hdl, &natural_light, NULL);
+	ctrls->target = v4l2_ctrl_new_custom(hdl, &autogain_target, NULL);
+	if (hdl->error) {
+		kfree(ctrls);
+		return hdl->error;
+	}
+	sd->sensor_priv = ctrls;
+	v4l2_ctrl_auto_cluster(5, &ctrls->autogain, 0, false);
+	return 0;
+}
+
 static int pb0100_probe(struct sd *sd)
 {
 	u16 sensor;
-	int i, err;
-	s32 *sensor_settings;
+	int err;
 
 	err = stv06xx_read_sensor(sd, PB_IDENT, &sensor);
 
 	if (err < 0)
 		return -ENODEV;
+	if ((sensor >> 8) != 0x64)
+		return -ENODEV;
 
-	if ((sensor >> 8) == 0x64) {
-		sensor_settings = kmalloc(
-				ARRAY_SIZE(pb0100_ctrl) * sizeof(s32),
-				GFP_KERNEL);
-		if (!sensor_settings)
-			return -ENOMEM;
-
-		pr_info("Photobit pb0100 sensor detected\n");
-
-		sd->gspca_dev.cam.cam_mode = pb0100_mode;
-		sd->gspca_dev.cam.nmodes = ARRAY_SIZE(pb0100_mode);
-		sd->desc.ctrls = pb0100_ctrl;
-		sd->desc.nctrls = ARRAY_SIZE(pb0100_ctrl);
-		for (i = 0; i < sd->desc.nctrls; i++)
-			sensor_settings[i] = pb0100_ctrl[i].qctrl.default_value;
-		sd->sensor_priv = sensor_settings;
+	pr_info("Photobit pb0100 sensor detected\n");
 
-		return 0;
-	}
+	sd->gspca_dev.cam.cam_mode = pb0100_mode;
+	sd->gspca_dev.cam.nmodes = ARRAY_SIZE(pb0100_mode);
 
-	return -ENODEV;
+	return 0;
 }
 
 static int pb0100_start(struct sd *sd)
@@ -214,7 +191,6 @@ static int pb0100_start(struct sd *sd)
 	struct usb_host_interface *alt;
 	struct usb_interface *intf;
 	struct cam *cam = &sd->gspca_dev.cam;
-	s32 *sensor_settings = sd->sensor_priv;
 	u32 mode = cam->cam_mode[sd->gspca_dev.curr_mode].priv;
 
 	intf = usb_ifnum_to_if(sd->gspca_dev.dev, sd->gspca_dev.iface);
@@ -255,13 +231,6 @@ static int pb0100_start(struct sd *sd)
 		stv06xx_write_bridge(sd, STV_SCAN_RATE, 0x20);
 	}
 
-	/* set_gain also sets red and blue balance */
-	pb0100_set_gain(&sd->gspca_dev, sensor_settings[GAIN_IDX]);
-	pb0100_set_exposure(&sd->gspca_dev, sensor_settings[EXPOSURE_IDX]);
-	pb0100_set_autogain_target(&sd->gspca_dev,
-				   sensor_settings[AUTOGAIN_TARGET_IDX]);
-	pb0100_set_autogain(&sd->gspca_dev, sensor_settings[AUTOGAIN_IDX]);
-
 	err = stv06xx_write_sensor(sd, PB_CONTROL, BIT(5)|BIT(3)|BIT(1));
 	PDEBUG(D_STREAM, "Started stream, status: %d", err);
 
@@ -285,12 +254,6 @@ out:
 	return (err < 0) ? err : 0;
 }
 
-static void pb0100_disconnect(struct sd *sd)
-{
-	sd->sensor = NULL;
-	kfree(sd->sensor_priv);
-}
-
 /* FIXME: Sort the init commands out and put them into tables,
 	  this is only for getting the camera to work */
 /* FIXME: No error handling for now,
@@ -362,62 +325,32 @@ static int pb0100_dump(struct sd *sd)
 	return 0;
 }
 
-static int pb0100_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];
-
-	return 0;
-}
-
 static int pb0100_set_gain(struct gspca_dev *gspca_dev, __s32 val)
 {
 	int err;
 	struct sd *sd = (struct sd *) gspca_dev;
-	s32 *sensor_settings = sd->sensor_priv;
+	struct pb0100_ctrls *ctrls = sd->sensor_priv;
 
-	if (sensor_settings[AUTOGAIN_IDX])
-		return -EBUSY;
-
-	sensor_settings[GAIN_IDX] = 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);
 
 	if (!err)
-		err = pb0100_set_red_balance(gspca_dev,
-					     sensor_settings[RED_BALANCE_IDX]);
+		err = pb0100_set_red_balance(gspca_dev, ctrls->red->val);
 	if (!err)
-		err = pb0100_set_blue_balance(gspca_dev,
-					    sensor_settings[BLUE_BALANCE_IDX]);
+		err = pb0100_set_blue_balance(gspca_dev, ctrls->blue->val);
 
 	return err;
 }
 
-static int pb0100_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];
-
-	return 0;
-}
-
 static int pb0100_set_red_balance(struct gspca_dev *gspca_dev, __s32 val)
 {
 	int err;
 	struct sd *sd = (struct sd *) gspca_dev;
-	s32 *sensor_settings = sd->sensor_priv;
+	struct pb0100_ctrls *ctrls = sd->sensor_priv;
 
-	if (sensor_settings[AUTOGAIN_IDX])
-		return -EBUSY;
-
-	sensor_settings[RED_BALANCE_IDX] = val;
-	val += sensor_settings[GAIN_IDX];
+	val += ctrls->gain->val;
 	if (val < 0)
 		val = 0;
 	else if (val > 255)
@@ -429,27 +362,13 @@ static int pb0100_set_red_balance(struct gspca_dev *gspca_dev, __s32 val)
 	return err;
 }
 
-static int pb0100_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];
-
-	return 0;
-}
-
 static int pb0100_set_blue_balance(struct gspca_dev *gspca_dev, __s32 val)
 {
 	int err;
 	struct sd *sd = (struct sd *) gspca_dev;
-	s32 *sensor_settings = sd->sensor_priv;
+	struct pb0100_ctrls *ctrls = sd->sensor_priv;
 
-	if (sensor_settings[AUTOGAIN_IDX])
-		return -EBUSY;
-
-	sensor_settings[BLUE_BALANCE_IDX] = val;
-	val += sensor_settings[GAIN_IDX];
+	val += ctrls->gain->val;
 	if (val < 0)
 		val = 0;
 	else if (val > 255)
@@ -461,51 +380,25 @@ static int pb0100_set_blue_balance(struct gspca_dev *gspca_dev, __s32 val)
 	return err;
 }
 
-static int pb0100_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];
-
-	return 0;
-}
-
 static int pb0100_set_exposure(struct gspca_dev *gspca_dev, __s32 val)
 {
-	int err;
 	struct sd *sd = (struct sd *) gspca_dev;
-	s32 *sensor_settings = sd->sensor_priv;
-
-	if (sensor_settings[AUTOGAIN_IDX])
-		return -EBUSY;
+	int err;
 
-	sensor_settings[EXPOSURE_IDX] = val;
 	err = stv06xx_write_sensor(sd, PB_RINTTIME, val);
 	PDEBUG(D_V4L2, "Set exposure to %d, status: %d", val, err);
 
 	return err;
 }
 
-static int pb0100_get_autogain(struct gspca_dev *gspca_dev, __s32 *val)
-{
-	struct sd *sd = (struct sd *) gspca_dev;
-	s32 *sensor_settings = sd->sensor_priv;
-
-	*val = sensor_settings[AUTOGAIN_IDX];
-
-	return 0;
-}
-
 static int pb0100_set_autogain(struct gspca_dev *gspca_dev, __s32 val)
 {
 	int err;
 	struct sd *sd = (struct sd *) gspca_dev;
-	s32 *sensor_settings = sd->sensor_priv;
+	struct pb0100_ctrls *ctrls = sd->sensor_priv;
 
-	sensor_settings[AUTOGAIN_IDX] = val;
-	if (sensor_settings[AUTOGAIN_IDX]) {
-		if (sensor_settings[NATURAL_IDX])
+	if (val) {
+		if (ctrls->natural->val)
 			val = BIT(6)|BIT(4)|BIT(0);
 		else
 			val = BIT(4)|BIT(0);
@@ -514,29 +407,15 @@ static int pb0100_set_autogain(struct gspca_dev *gspca_dev, __s32 val)
 
 	err = stv06xx_write_sensor(sd, PB_EXPGAIN, val);
 	PDEBUG(D_V4L2, "Set autogain to %d (natural: %d), status: %d",
-	       sensor_settings[AUTOGAIN_IDX], sensor_settings[NATURAL_IDX],
-	       err);
+	       val, ctrls->natural->val, err);
 
 	return err;
 }
 
-static int pb0100_get_autogain_target(struct gspca_dev *gspca_dev, __s32 *val)
-{
-	struct sd *sd = (struct sd *) gspca_dev;
-	s32 *sensor_settings = sd->sensor_priv;
-
-	*val = sensor_settings[AUTOGAIN_TARGET_IDX];
-
-	return 0;
-}
-
 static int pb0100_set_autogain_target(struct gspca_dev *gspca_dev, __s32 val)
 {
 	int err, totalpixels, brightpixels, darkpixels;
 	struct sd *sd = (struct sd *) gspca_dev;
-	s32 *sensor_settings = sd->sensor_priv;
-
-	sensor_settings[AUTOGAIN_TARGET_IDX] = val;
 
 	/* Number of pixels counted by the sensor when subsampling the pixels.
 	 * Slightly larger than the real value to avoid oscillation */
@@ -553,23 +432,3 @@ static int pb0100_set_autogain_target(struct gspca_dev *gspca_dev, __s32 val)
 
 	return err;
 }
-
-static int pb0100_get_natural(struct gspca_dev *gspca_dev, __s32 *val)
-{
-	struct sd *sd = (struct sd *) gspca_dev;
-	s32 *sensor_settings = sd->sensor_priv;
-
-	*val = sensor_settings[NATURAL_IDX];
-
-	return 0;
-}
-
-static int pb0100_set_natural(struct gspca_dev *gspca_dev, __s32 val)
-{
-	struct sd *sd = (struct sd *) gspca_dev;
-	s32 *sensor_settings = sd->sensor_priv;
-
-	sensor_settings[NATURAL_IDX] = val;
-
-	return pb0100_set_autogain(gspca_dev, sensor_settings[AUTOGAIN_IDX]);
-}
diff --git a/drivers/media/video/gspca/stv06xx/stv06xx_pb0100.h b/drivers/media/video/gspca/stv06xx/stv06xx_pb0100.h
index 757de246dc7..5071e5353fd 100644
--- a/drivers/media/video/gspca/stv06xx/stv06xx_pb0100.h
+++ b/drivers/media/video/gspca/stv06xx/stv06xx_pb0100.h
@@ -112,25 +112,17 @@
 static int pb0100_probe(struct sd *sd);
 static int pb0100_start(struct sd *sd);
 static int pb0100_init(struct sd *sd);
+static int pb0100_init_controls(struct sd *sd);
 static int pb0100_stop(struct sd *sd);
 static int pb0100_dump(struct sd *sd);
-static void pb0100_disconnect(struct sd *sd);
 
 /* V4L2 controls supported by the driver */
-static int pb0100_get_gain(struct gspca_dev *gspca_dev, __s32 *val);
 static int pb0100_set_gain(struct gspca_dev *gspca_dev, __s32 val);
-static int pb0100_get_red_balance(struct gspca_dev *gspca_dev, __s32 *val);
 static int pb0100_set_red_balance(struct gspca_dev *gspca_dev, __s32 val);
-static int pb0100_get_blue_balance(struct gspca_dev *gspca_dev, __s32 *val);
 static int pb0100_set_blue_balance(struct gspca_dev *gspca_dev, __s32 val);
-static int pb0100_get_exposure(struct gspca_dev *gspca_dev, __s32 *val);
 static int pb0100_set_exposure(struct gspca_dev *gspca_dev, __s32 val);
-static int pb0100_get_autogain(struct gspca_dev *gspca_dev, __s32 *val);
 static int pb0100_set_autogain(struct gspca_dev *gspca_dev, __s32 val);
-static int pb0100_get_autogain_target(struct gspca_dev *gspca_dev, __s32 *val);
 static int pb0100_set_autogain_target(struct gspca_dev *gspca_dev, __s32 val);
-static int pb0100_get_natural(struct gspca_dev *gspca_dev, __s32 *val);
-static int pb0100_set_natural(struct gspca_dev *gspca_dev, __s32 val);
 
 const struct stv06xx_sensor stv06xx_sensor_pb0100 = {
 	.name = "PB-0100",
@@ -142,11 +134,11 @@ const struct stv06xx_sensor stv06xx_sensor_pb0100 = {
 	.max_packet_size = { 847, 923 },
 
 	.init = pb0100_init,
+	.init_controls = pb0100_init_controls,
 	.probe = pb0100_probe,
 	.start = pb0100_start,
 	.stop = pb0100_stop,
 	.dump = pb0100_dump,
-	.disconnect = pb0100_disconnect,
 };
 
 #endif
diff --git a/drivers/media/video/gspca/stv06xx/stv06xx_sensor.h b/drivers/media/video/gspca/stv06xx/stv06xx_sensor.h
index fb229d8ded5..3a498c2495c 100644
--- a/drivers/media/video/gspca/stv06xx/stv06xx_sensor.h
+++ b/drivers/media/video/gspca/stv06xx/stv06xx_sensor.h
@@ -63,8 +63,8 @@ struct stv06xx_sensor {
 	/* Performs a initialization sequence */
 	int (*init)(struct sd *sd);
 
-	/* Executed at device disconnect */
-	void (*disconnect)(struct sd *sd);
+	/* Initializes the controls */
+	int (*init_controls)(struct sd *sd);
 
 	/* Reads a sensor register */
 	int (*read_sensor)(struct sd *sd, const u8 address,
diff --git a/drivers/media/video/gspca/stv06xx/stv06xx_st6422.c b/drivers/media/video/gspca/stv06xx/stv06xx_st6422.c
index 9940e035b3a..8a57990dfe0 100644
--- a/drivers/media/video/gspca/stv06xx/stv06xx_st6422.c
+++ b/drivers/media/video/gspca/stv06xx/stv06xx_st6422.c
@@ -30,20 +30,6 @@
 
 #include "stv06xx_st6422.h"
 
-/* controls */
-enum e_ctrl {
-	BRIGHTNESS,
-	CONTRAST,
-	GAIN,
-	EXPOSURE,
-	NCTRLS		/* number of controls */
-};
-
-/* sensor settings */
-struct st6422_settings {
-	struct gspca_ctrl ctrls[NCTRLS];
-};
-
 static struct v4l2_pix_format st6422_mode[] = {
 	/* Note we actually get 124 lines of data, of which we skip the 4st
 	   4 as they are garbage */
@@ -74,83 +60,70 @@ static struct v4l2_pix_format st6422_mode[] = {
 };
 
 /* V4L2 controls supported by the driver */
-static void st6422_set_brightness(struct gspca_dev *gspca_dev);
-static void st6422_set_contrast(struct gspca_dev *gspca_dev);
-static void st6422_set_gain(struct gspca_dev *gspca_dev);
-static void st6422_set_exposure(struct gspca_dev *gspca_dev);
-
-static const struct ctrl st6422_ctrl[NCTRLS] = {
-[BRIGHTNESS] = {
-		{
-			.id		= V4L2_CID_BRIGHTNESS,
-			.type		= V4L2_CTRL_TYPE_INTEGER,
-			.name		= "Brightness",
-			.minimum	= 0,
-			.maximum	= 31,
-			.step		= 1,
-			.default_value  = 3
-		},
-		.set_control = st6422_set_brightness
-	},
-[CONTRAST] = {
-		{
-			.id		= V4L2_CID_CONTRAST,
-			.type		= V4L2_CTRL_TYPE_INTEGER,
-			.name		= "Contrast",
-			.minimum	= 0,
-			.maximum	= 15,
-			.step		= 1,
-			.default_value  = 11
-		},
-		.set_control = st6422_set_contrast
-	},
-[GAIN] = {
-		{
-			.id		= V4L2_CID_GAIN,
-			.type		= V4L2_CTRL_TYPE_INTEGER,
-			.name		= "Gain",
-			.minimum	= 0,
-			.maximum	= 255,
-			.step		= 1,
-			.default_value  = 64
-		},
-		.set_control = st6422_set_gain
-	},
-[EXPOSURE] = {
-		{
-			.id		= V4L2_CID_EXPOSURE,
-			.type		= V4L2_CTRL_TYPE_INTEGER,
-			.name		= "Exposure",
-			.minimum	= 0,
-#define EXPOSURE_MAX 1023
-			.maximum	= EXPOSURE_MAX,
-			.step		= 1,
-			.default_value  = 256
-		},
-		.set_control = st6422_set_exposure
-	},
+static int setbrightness(struct sd *sd, s32 val);
+static int setcontrast(struct sd *sd, s32 val);
+static int setgain(struct sd *sd, u8 gain);
+static int setexposure(struct sd *sd, s16 expo);
+
+static int st6422_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 = -EINVAL;
+
+	switch (ctrl->id) {
+	case V4L2_CID_BRIGHTNESS:
+		err = setbrightness(sd, ctrl->val);
+		break;
+	case V4L2_CID_CONTRAST:
+		err = setcontrast(sd, ctrl->val);
+		break;
+	case V4L2_CID_GAIN:
+		err = setgain(sd, ctrl->val);
+		break;
+	case V4L2_CID_EXPOSURE:
+		err = setexposure(sd, ctrl->val);
+		break;
+	}
+
+	/* commit settings */
+	if (err >= 0)
+		err = stv06xx_write_bridge(sd, 0x143f, 0x01);
+	sd->gspca_dev.usb_err = err;
+	return err;
+}
+
+static const struct v4l2_ctrl_ops st6422_ctrl_ops = {
+	.s_ctrl = st6422_s_ctrl,
 };
 
-static int st6422_probe(struct sd *sd)
+static int st6422_init_controls(struct sd *sd)
 {
-	struct st6422_settings *sensor_settings;
+	struct v4l2_ctrl_handler *hdl = &sd->gspca_dev.ctrl_handler;
+
+	v4l2_ctrl_handler_init(hdl, 4);
+	v4l2_ctrl_new_std(hdl, &st6422_ctrl_ops,
+			V4L2_CID_BRIGHTNESS, 0, 31, 1, 3);
+	v4l2_ctrl_new_std(hdl, &st6422_ctrl_ops,
+			V4L2_CID_CONTRAST, 0, 15, 1, 11);
+	v4l2_ctrl_new_std(hdl, &st6422_ctrl_ops,
+			V4L2_CID_EXPOSURE, 0, 1023, 1, 256);
+	v4l2_ctrl_new_std(hdl, &st6422_ctrl_ops,
+			V4L2_CID_GAIN, 0, 255, 1, 64);
+
+	return hdl->error;
+}
 
+static int st6422_probe(struct sd *sd)
+{
 	if (sd->bridge != BRIDGE_ST6422)
 		return -ENODEV;
 
 	pr_info("st6422 sensor detected\n");
 
-	sensor_settings = kmalloc(sizeof *sensor_settings, GFP_KERNEL);
-	if (!sensor_settings)
-		return -ENOMEM;
-
 	sd->gspca_dev.cam.cam_mode = st6422_mode;
 	sd->gspca_dev.cam.nmodes = ARRAY_SIZE(st6422_mode);
-	sd->gspca_dev.cam.ctrls = sensor_settings->ctrls;
-	sd->desc.ctrls = st6422_ctrl;
-	sd->desc.nctrls = ARRAY_SIZE(st6422_ctrl);
-	sd->sensor_priv = sensor_settings;
-
 	return 0;
 }
 
@@ -239,38 +212,22 @@ static int st6422_init(struct sd *sd)
 	return err;
 }
 
-static void st6422_disconnect(struct sd *sd)
-{
-	sd->sensor = NULL;
-	kfree(sd->sensor_priv);
-}
-
-static int setbrightness(struct sd *sd)
+static int setbrightness(struct sd *sd, s32 val)
 {
-	struct st6422_settings *sensor_settings = sd->sensor_priv;
-
 	/* val goes from 0 -> 31 */
-	return stv06xx_write_bridge(sd, 0x1432,
-			sensor_settings->ctrls[BRIGHTNESS].val);
+	return stv06xx_write_bridge(sd, 0x1432, val);
 }
 
-static int setcontrast(struct sd *sd)
+static int setcontrast(struct sd *sd, s32 val)
 {
-	struct st6422_settings *sensor_settings = sd->sensor_priv;
-
 	/* Val goes from 0 -> 15 */
-	return stv06xx_write_bridge(sd, 0x143a,
-			sensor_settings->ctrls[CONTRAST].val | 0xf0);
+	return stv06xx_write_bridge(sd, 0x143a, val | 0xf0);
 }
 
-static int setgain(struct sd *sd)
+static int setgain(struct sd *sd, u8 gain)
 {
-	struct st6422_settings *sensor_settings = sd->sensor_priv;
-	u8 gain;
 	int err;
 
-	gain = sensor_settings->ctrls[GAIN].val;
-
 	/* Set red, green, blue, gain */
 	err = stv06xx_write_bridge(sd, 0x0509, gain);
 	if (err < 0)
@@ -292,13 +249,10 @@ static int setgain(struct sd *sd)
 	return stv06xx_write_bridge(sd, 0x050d, 0x01);
 }
 
-static int setexposure(struct sd *sd)
+static int setexposure(struct sd *sd, s16 expo)
 {
-	struct st6422_settings *sensor_settings = sd->sensor_priv;
-	u16 expo;
 	int err;
 
-	expo = sensor_settings->ctrls[EXPOSURE].val;
 	err = stv06xx_write_bridge(sd, 0x143d, expo & 0xff);
 	if (err < 0)
 		return err;
@@ -318,22 +272,6 @@ static int st6422_start(struct sd *sd)
 	if (err < 0)
 		return err;
 
-	err = setbrightness(sd);
-	if (err < 0)
-		return err;
-
-	err = setcontrast(sd);
-	if (err < 0)
-		return err;
-
-	err = setexposure(sd);
-	if (err < 0)
-		return err;
-
-	err = setgain(sd);
-	if (err < 0)
-		return err;
-
 	/* commit settings */
 	err = stv06xx_write_bridge(sd, 0x143f, 0x01);
 	return (err < 0) ? err : 0;
@@ -345,59 +283,3 @@ static int st6422_stop(struct sd *sd)
 
 	return 0;
 }
-
-static void st6422_set_brightness(struct gspca_dev *gspca_dev)
-{
-	int err;
-	struct sd *sd = (struct sd *) gspca_dev;
-
-	err = setbrightness(sd);
-
-	/* commit settings */
-	if (err >= 0)
-		err = stv06xx_write_bridge(sd, 0x143f, 0x01);
-
-	gspca_dev->usb_err = err;
-}
-
-static void st6422_set_contrast(struct gspca_dev *gspca_dev)
-{
-	int err;
-	struct sd *sd = (struct sd *) gspca_dev;
-
-	err = setcontrast(sd);
-
-	/* commit settings */
-	if (err >= 0)
-		err = stv06xx_write_bridge(sd, 0x143f, 0x01);
-
-	gspca_dev->usb_err = err;
-}
-
-static void st6422_set_gain(struct gspca_dev *gspca_dev)
-{
-	int err;
-	struct sd *sd = (struct sd *) gspca_dev;
-
-	err = setgain(sd);
-
-	/* commit settings */
-	if (err >= 0)
-		err = stv06xx_write_bridge(sd, 0x143f, 0x01);
-
-	gspca_dev->usb_err = err;
-}
-
-static void st6422_set_exposure(struct gspca_dev *gspca_dev)
-{
-	int err;
-	struct sd *sd = (struct sd *) gspca_dev;
-
-	err = setexposure(sd);
-
-	/* commit settings */
-	if (err >= 0)
-		err = stv06xx_write_bridge(sd, 0x143f, 0x01);
-
-	gspca_dev->usb_err = err;
-}
diff --git a/drivers/media/video/gspca/stv06xx/stv06xx_st6422.h b/drivers/media/video/gspca/stv06xx/stv06xx_st6422.h
index d7498e06432..8f20fbf30f3 100644
--- a/drivers/media/video/gspca/stv06xx/stv06xx_st6422.h
+++ b/drivers/media/video/gspca/stv06xx/stv06xx_st6422.h
@@ -34,8 +34,8 @@
 static int st6422_probe(struct sd *sd);
 static int st6422_start(struct sd *sd);
 static int st6422_init(struct sd *sd);
+static int st6422_init_controls(struct sd *sd);
 static int st6422_stop(struct sd *sd);
-static void st6422_disconnect(struct sd *sd);
 
 const struct stv06xx_sensor stv06xx_sensor_st6422 = {
 	.name = "ST6422",
@@ -43,10 +43,10 @@ const struct stv06xx_sensor stv06xx_sensor_st6422 = {
 	.min_packet_size = { 300, 847 },
 	.max_packet_size = { 300, 847 },
 	.init = st6422_init,
+	.init_controls = st6422_init_controls,
 	.probe = st6422_probe,
 	.start = st6422_start,
 	.stop = st6422_stop,
-	.disconnect = st6422_disconnect,
 };
 
 #endif
diff --git a/drivers/media/video/gspca/stv06xx/stv06xx_vv6410.c b/drivers/media/video/gspca/stv06xx/stv06xx_vv6410.c
index a5c69d9ebdd..748e1421d6d 100644
--- a/drivers/media/video/gspca/stv06xx/stv06xx_vv6410.c
+++ b/drivers/media/video/gspca/stv06xx/stv06xx_vv6410.c
@@ -44,130 +44,83 @@ static struct v4l2_pix_format vv6410_mode[] = {
 	}
 };
 
-static const struct ctrl vv6410_ctrl[] = {
-#define HFLIP_IDX 0
-	{
-		{
-			.id		= V4L2_CID_HFLIP,
-			.type		= V4L2_CTRL_TYPE_BOOLEAN,
-			.name		= "horizontal flip",
-			.minimum	= 0,
-			.maximum	= 1,
-			.step		= 1,
-			.default_value	= 0
-		},
-		.set = vv6410_set_hflip,
-		.get = vv6410_get_hflip
-	},
-#define VFLIP_IDX 1
-	{
-		{
-			.id		= V4L2_CID_VFLIP,
-			.type		= V4L2_CTRL_TYPE_BOOLEAN,
-			.name		= "vertical flip",
-			.minimum	= 0,
-			.maximum	= 1,
-			.step		= 1,
-			.default_value	= 0
-		},
-		.set = vv6410_set_vflip,
-		.get = vv6410_get_vflip
-	},
-#define GAIN_IDX 2
-	{
-		{
-			.id		= V4L2_CID_GAIN,
-			.type		= V4L2_CTRL_TYPE_INTEGER,
-			.name		= "analog gain",
-			.minimum	= 0,
-			.maximum	= 15,
-			.step		= 1,
-			.default_value  = 10
-		},
-		.set = vv6410_set_analog_gain,
-		.get = vv6410_get_analog_gain
-	},
-#define EXPOSURE_IDX 3
-	{
-		{
-			.id		= V4L2_CID_EXPOSURE,
-			.type		= V4L2_CTRL_TYPE_INTEGER,
-			.name		= "exposure",
-			.minimum	= 0,
-			.maximum	= 32768,
-			.step		= 1,
-			.default_value  = 20000
-		},
-		.set = vv6410_set_exposure,
-		.get = vv6410_get_exposure
+static int vv6410_s_ctrl(struct v4l2_ctrl *ctrl)
+{
+	struct gspca_dev *gspca_dev =
+		container_of(ctrl->handler, struct gspca_dev, ctrl_handler);
+	int err = -EINVAL;
+
+	switch (ctrl->id) {
+	case V4L2_CID_HFLIP:
+		err = vv6410_set_hflip(gspca_dev, ctrl->val);
+		break;
+	case V4L2_CID_VFLIP:
+		err = vv6410_set_vflip(gspca_dev, ctrl->val);
+		break;
+	case V4L2_CID_GAIN:
+		err = vv6410_set_analog_gain(gspca_dev, ctrl->val);
+		break;
+	case V4L2_CID_EXPOSURE:
+		err = vv6410_set_exposure(gspca_dev, ctrl->val);
+		break;
 	}
-	};
+	return err;
+}
+
+static const struct v4l2_ctrl_ops vv6410_ctrl_ops = {
+	.s_ctrl = vv6410_s_ctrl,
+};
 
 static int vv6410_probe(struct sd *sd)
 {
 	u16 data;
-	int err, i;
-	s32 *sensor_settings;
+	int err;
 
 	err = stv06xx_read_sensor(sd, VV6410_DEVICEH, &data);
 	if (err < 0)
 		return -ENODEV;
 
-	if (data == 0x19) {
-		pr_info("vv6410 sensor detected\n");
+	if (data != 0x19)
+		return -ENODEV;
 
-		sensor_settings = kmalloc(ARRAY_SIZE(vv6410_ctrl) * sizeof(s32),
-					  GFP_KERNEL);
-		if (!sensor_settings)
-			return -ENOMEM;
+	pr_info("vv6410 sensor detected\n");
 
-		sd->gspca_dev.cam.cam_mode = vv6410_mode;
-		sd->gspca_dev.cam.nmodes = ARRAY_SIZE(vv6410_mode);
-		sd->desc.ctrls = vv6410_ctrl;
-		sd->desc.nctrls = ARRAY_SIZE(vv6410_ctrl);
+	sd->gspca_dev.cam.cam_mode = vv6410_mode;
+	sd->gspca_dev.cam.nmodes = ARRAY_SIZE(vv6410_mode);
+	return 0;
+}
 
-		for (i = 0; i < sd->desc.nctrls; i++)
-			sensor_settings[i] = vv6410_ctrl[i].qctrl.default_value;
-		sd->sensor_priv = sensor_settings;
-		return 0;
-	}
-	return -ENODEV;
+static int vv6410_init_controls(struct sd *sd)
+{
+	struct v4l2_ctrl_handler *hdl = &sd->gspca_dev.ctrl_handler;
+
+	v4l2_ctrl_handler_init(hdl, 4);
+	v4l2_ctrl_new_std(hdl, &vv6410_ctrl_ops,
+			V4L2_CID_HFLIP, 0, 1, 1, 0);
+	v4l2_ctrl_new_std(hdl, &vv6410_ctrl_ops,
+			V4L2_CID_VFLIP, 0, 1, 1, 0);
+	v4l2_ctrl_new_std(hdl, &vv6410_ctrl_ops,
+			V4L2_CID_EXPOSURE, 0, 32768, 1, 20000);
+	v4l2_ctrl_new_std(hdl, &vv6410_ctrl_ops,
+			V4L2_CID_GAIN, 0, 15, 1, 10);
+	return hdl->error;
 }
 
 static int vv6410_init(struct sd *sd)
 {
 	int err = 0, i;
-	s32 *sensor_settings = sd->sensor_priv;
 
-	for (i = 0; i < ARRAY_SIZE(stv_bridge_init); i++) {
+	for (i = 0; i < ARRAY_SIZE(stv_bridge_init); i++)
 		stv06xx_write_bridge(sd, stv_bridge_init[i].addr, stv_bridge_init[i].data);
-	}
 
 	if (err < 0)
 		return err;
 
 	err = stv06xx_write_sensor_bytes(sd, (u8 *) vv6410_sensor_init,
 					 ARRAY_SIZE(vv6410_sensor_init));
-	if (err < 0)
-		return err;
-
-	err = vv6410_set_exposure(&sd->gspca_dev,
-				   sensor_settings[EXPOSURE_IDX]);
-	if (err < 0)
-		return err;
-
-	err = vv6410_set_analog_gain(&sd->gspca_dev,
-				      sensor_settings[GAIN_IDX]);
-
 	return (err < 0) ? err : 0;
 }
 
-static void vv6410_disconnect(struct sd *sd)
-{
-	sd->sensor = NULL;
-	kfree(sd->sensor_priv);
-}
-
 static int vv6410_start(struct sd *sd)
 {
 	int err;
@@ -233,25 +186,12 @@ static int vv6410_dump(struct sd *sd)
 	return (err < 0) ? err : 0;
 }
 
-static int vv6410_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 vv6410_set_hflip(struct gspca_dev *gspca_dev, __s32 val)
 {
 	int err;
 	u16 i2c_data;
 	struct sd *sd = (struct sd *) gspca_dev;
-	s32 *sensor_settings = sd->sensor_priv;
 
-	sensor_settings[HFLIP_IDX] = val;
 	err = stv06xx_read_sensor(sd, VV6410_DATAFORMAT, &i2c_data);
 	if (err < 0)
 		return err;
@@ -267,25 +207,12 @@ static int vv6410_set_hflip(struct gspca_dev *gspca_dev, __s32 val)
 	return (err < 0) ? err : 0;
 }
 
-static int vv6410_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 vv6410_set_vflip(struct gspca_dev *gspca_dev, __s32 val)
 {
 	int err;
 	u16 i2c_data;
 	struct sd *sd = (struct sd *) gspca_dev;
-	s32 *sensor_settings = sd->sensor_priv;
 
-	sensor_settings[VFLIP_IDX] = val;
 	err = stv06xx_read_sensor(sd, VV6410_DATAFORMAT, &i2c_data);
 	if (err < 0)
 		return err;
@@ -301,52 +228,23 @@ static int vv6410_set_vflip(struct gspca_dev *gspca_dev, __s32 val)
 	return (err < 0) ? err : 0;
 }
 
-static int vv6410_get_analog_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 analog gain %d", *val);
-
-	return 0;
-}
-
 static int vv6410_set_analog_gain(struct gspca_dev *gspca_dev, __s32 val)
 {
 	int err;
 	struct sd *sd = (struct sd *) gspca_dev;
-	s32 *sensor_settings = sd->sensor_priv;
 
-	sensor_settings[GAIN_IDX] = val;
 	PDEBUG(D_V4L2, "Set analog gain to %d", val);
 	err = stv06xx_write_sensor(sd, VV6410_ANALOGGAIN, 0xf0 | (val & 0xf));
 
 	return (err < 0) ? err : 0;
 }
 
-static int vv6410_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 vv6410_set_exposure(struct gspca_dev *gspca_dev, __s32 val)
 {
 	int err;
 	struct sd *sd = (struct sd *) gspca_dev;
-	s32 *sensor_settings = sd->sensor_priv;
 	unsigned int fine, coarse;
 
-	sensor_settings[EXPOSURE_IDX] = val;
-
 	val = (val * val >> 14) + val / 4;
 
 	fine = val % VV6410_CIF_LINELENGTH;
diff --git a/drivers/media/video/gspca/stv06xx/stv06xx_vv6410.h b/drivers/media/video/gspca/stv06xx/stv06xx_vv6410.h
index a25b8873f2e..53e67b40ca0 100644
--- a/drivers/media/video/gspca/stv06xx/stv06xx_vv6410.h
+++ b/drivers/media/video/gspca/stv06xx/stv06xx_vv6410.h
@@ -178,18 +178,14 @@
 static int vv6410_probe(struct sd *sd);
 static int vv6410_start(struct sd *sd);
 static int vv6410_init(struct sd *sd);
+static int vv6410_init_controls(struct sd *sd);
 static int vv6410_stop(struct sd *sd);
 static int vv6410_dump(struct sd *sd);
-static void vv6410_disconnect(struct sd *sd);
 
 /* V4L2 controls supported by the driver */
-static int vv6410_get_hflip(struct gspca_dev *gspca_dev, __s32 *val);
 static int vv6410_set_hflip(struct gspca_dev *gspca_dev, __s32 val);
-static int vv6410_get_vflip(struct gspca_dev *gspca_dev, __s32 *val);
 static int vv6410_set_vflip(struct gspca_dev *gspca_dev, __s32 val);
-static int vv6410_get_analog_gain(struct gspca_dev *gspca_dev, __s32 *val);
 static int vv6410_set_analog_gain(struct gspca_dev *gspca_dev, __s32 val);
-static int vv6410_get_exposure(struct gspca_dev *gspca_dev, __s32 *val);
 static int vv6410_set_exposure(struct gspca_dev *gspca_dev, __s32 val);
 
 const struct stv06xx_sensor stv06xx_sensor_vv6410 = {
@@ -202,11 +198,11 @@ const struct stv06xx_sensor stv06xx_sensor_vv6410 = {
 	.min_packet_size = { 1023 },
 	.max_packet_size = { 1023 },
 	.init = vv6410_init,
+	.init_controls = vv6410_init_controls,
 	.probe = vv6410_probe,
 	.start = vv6410_start,
 	.stop = vv6410_stop,
 	.dump = vv6410_dump,
-	.disconnect = vv6410_disconnect,
 };
 
 /* If NULL, only single value to write, stored in len */
diff --git a/drivers/media/video/gspca/topro.c b/drivers/media/video/gspca/topro.c
index 444d3c5b907..c6326d177a3 100644
--- a/drivers/media/video/gspca/topro.c
+++ b/drivers/media/video/gspca/topro.c
@@ -4675,11 +4675,9 @@ static void sd_pkt_scan(struct gspca_dev *gspca_dev,
 /* -- do autogain -- */
 /* gain setting is done in setexposure() for tp6810 */
 static void setgain(struct gspca_dev *gspca_dev) {}
-/* !! coarse_grained_expo_autogain is not used !! */
-#define exp_too_low_cnt bridge
-#define exp_too_high_cnt sensor
-
+#define WANT_REGULAR_AUTOGAIN
 #include "autogain_functions.h"
+
 static void sd_dq_callback(struct gspca_dev *gspca_dev)
 {
 	struct sd *sd = (struct sd *) gspca_dev;
diff --git a/drivers/media/video/gspca/vicam.c b/drivers/media/video/gspca/vicam.c
index 911152e169d..15a30f7a4b2 100644
--- a/drivers/media/video/gspca/vicam.c
+++ b/drivers/media/video/gspca/vicam.c
@@ -37,9 +37,12 @@
 #include <linux/ihex.h>
 #include "gspca.h"
 
+#define VICAM_FIRMWARE "vicam/firmware.fw"
+
 MODULE_AUTHOR("Hans de Goede <hdegoede@redhat.com>");
 MODULE_DESCRIPTION("GSPCA ViCam USB Camera Driver");
 MODULE_LICENSE("GPL");
+MODULE_FIRMWARE(VICAM_FIRMWARE);
 
 enum e_ctrl {
 	GAIN,
@@ -222,7 +225,11 @@ static void vicam_dostream(struct work_struct *work)
 		goto exit;
 	}
 
-	while (gspca_dev->present && gspca_dev->streaming) {
+	while (gspca_dev->dev && gspca_dev->streaming) {
+#ifdef CONFIG_PM
+		if (gspca_dev->frozen)
+			break;
+#endif
 		ret = vicam_read_frame(gspca_dev, buffer, frame_sz);
 		if (ret < 0)
 			break;
@@ -268,7 +275,7 @@ static int sd_init(struct gspca_dev *gspca_dev)
 	const struct firmware *uninitialized_var(fw);
 	u8 *firmware_buf;
 
-	ret = request_ihex_firmware(&fw, "vicam/firmware.fw",
+	ret = request_ihex_firmware(&fw, VICAM_FIRMWARE,
 				    &gspca_dev->dev->dev);
 	if (ret) {
 		pr_err("Failed to load \"vicam/firmware.fw\": %d\n", ret);
@@ -324,7 +331,7 @@ static void sd_stop0(struct gspca_dev *gspca_dev)
 	dev->work_thread = NULL;
 	mutex_lock(&gspca_dev->usb_lock);
 
-	if (gspca_dev->present)
+	if (gspca_dev->dev)
 		vicam_set_camera_power(gspca_dev, 0);
 }
 
diff --git a/drivers/media/video/gspca/zc3xx.c b/drivers/media/video/gspca/zc3xx.c
index 7d9a4f1be9d..f0bacee33ef 100644
--- a/drivers/media/video/gspca/zc3xx.c
+++ b/drivers/media/video/gspca/zc3xx.c
@@ -32,29 +32,25 @@ MODULE_LICENSE("GPL");
 
 static int force_sensor = -1;
 
-#define REG08_DEF 3		/* default JPEG compression (70%) */
+#define REG08_DEF 3		/* default JPEG compression (75%) */
 #include "zc3xx-reg.h"
 
-/* controls */
-enum e_ctrl {
-	BRIGHTNESS,
-	CONTRAST,
-	EXPOSURE,
-	GAMMA,
-	AUTOGAIN,
-	LIGHTFREQ,
-	SHARPNESS,
-	QUALITY,
-	NCTRLS		/* number of controls */
-};
-
-#define AUTOGAIN_DEF 1
-
 /* specific webcam descriptor */
 struct sd {
 	struct gspca_dev gspca_dev;	/* !! must be the first item */
 
-	struct gspca_ctrl ctrls[NCTRLS];
+	struct { /* gamma/brightness/contrast control cluster */
+		struct v4l2_ctrl *gamma;
+		struct v4l2_ctrl *brightness;
+		struct v4l2_ctrl *contrast;
+	};
+	struct { /* autogain/exposure control cluster */
+		struct v4l2_ctrl *autogain;
+		struct v4l2_ctrl *exposure;
+	};
+	struct v4l2_ctrl *plfreq;
+	struct v4l2_ctrl *sharpness;
+	struct v4l2_ctrl *jpegqual;
 
 	struct work_struct work;
 	struct workqueue_struct *work_thread;
@@ -94,114 +90,6 @@ enum sensors {
 	SENSOR_MAX
 };
 
-/* V4L2 controls supported by the driver */
-static void setcontrast(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 setlightfreq(struct gspca_dev *gspca_dev);
-static void setsharpness(struct gspca_dev *gspca_dev);
-static int sd_setquality(struct gspca_dev *gspca_dev, __s32 val);
-
-static const struct ctrl sd_ctrls[NCTRLS] = {
-[BRIGHTNESS] = {
-	    {
-		.id      = V4L2_CID_BRIGHTNESS,
-		.type    = V4L2_CTRL_TYPE_INTEGER,
-		.name    = "Brightness",
-		.minimum = 0,
-		.maximum = 255,
-		.step    = 1,
-		.default_value = 128,
-	    },
-	    .set_control = setcontrast
-	},
-[CONTRAST] = {
-	    {
-		.id      = V4L2_CID_CONTRAST,
-		.type    = V4L2_CTRL_TYPE_INTEGER,
-		.name    = "Contrast",
-		.minimum = 0,
-		.maximum = 255,
-		.step    = 1,
-		.default_value = 128,
-	    },
-	    .set_control = setcontrast
-	},
-[EXPOSURE] = {
-	    {
-		.id      = V4L2_CID_EXPOSURE,
-		.type    = V4L2_CTRL_TYPE_INTEGER,
-		.name    = "Exposure",
-		.minimum = 0x30d,
-		.maximum	= 0x493e,
-		.step		= 1,
-		.default_value  = 0x927
-	    },
-	    .set_control = setexposure
-	},
-[GAMMA] = {
-	    {
-		.id      = V4L2_CID_GAMMA,
-		.type    = V4L2_CTRL_TYPE_INTEGER,
-		.name    = "Gamma",
-		.minimum = 1,
-		.maximum = 6,
-		.step    = 1,
-		.default_value = 4,
-	    },
-	    .set_control = setcontrast
-	},
-[AUTOGAIN] = {
-	    {
-		.id      = V4L2_CID_AUTOGAIN,
-		.type    = V4L2_CTRL_TYPE_BOOLEAN,
-		.name    = "Auto Gain",
-		.minimum = 0,
-		.maximum = 1,
-		.step    = 1,
-		.default_value = AUTOGAIN_DEF,
-		.flags   = V4L2_CTRL_FLAG_UPDATE
-	    },
-	    .set = sd_setautogain
-	},
-[LIGHTFREQ] = {
-	    {
-		.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 = 0,
-	    },
-	    .set_control = setlightfreq
-	},
-[SHARPNESS] = {
-	    {
-		.id	 = V4L2_CID_SHARPNESS,
-		.type    = V4L2_CTRL_TYPE_INTEGER,
-		.name    = "Sharpness",
-		.minimum = 0,
-		.maximum = 3,
-		.step    = 1,
-		.default_value = 2,
-	    },
-	    .set_control = setsharpness
-	},
-[QUALITY] = {
-	    {
-		.id	 = V4L2_CID_JPEG_COMPRESSION_QUALITY,
-		.type    = V4L2_CTRL_TYPE_INTEGER,
-		.name    = "Compression Quality",
-		.minimum = 40,
-		.maximum = 70,
-		.step    = 1,
-		.default_value = 70	/* updated in sd_init() */
-	    },
-	    .set = sd_setquality
-	},
-};
-
 static const struct v4l2_pix_format vga_mode[] = {
 	{320, 240, V4L2_PIX_FMT_JPEG, V4L2_FIELD_NONE,
 		.bytesperline = 320,
@@ -241,8 +129,11 @@ static const struct v4l2_pix_format sif_mode[] = {
 		.priv = 0},
 };
 
-/* bridge reg08 -> JPEG quality conversion table */
-static u8 jpeg_qual[] = {40, 50, 60, 70, /*80*/};
+/*
+ * Bridge reg08 bits 1-2 -> JPEG quality conversion table. Note the highest
+ * quality setting is not usable as USB 1 does not have enough bandwidth.
+ */
+static u8 jpeg_qual[] = {50, 75, 87, /* 94 */};
 
 /* usb exchanges */
 struct usb_action {
@@ -5818,10 +5709,8 @@ static void setmatrix(struct gspca_dev *gspca_dev)
 		reg_w(gspca_dev, matrix[i], 0x010a + i);
 }
 
-static void setsharpness(struct gspca_dev *gspca_dev)
+static void setsharpness(struct gspca_dev *gspca_dev, s32 val)
 {
-	struct sd *sd = (struct sd *) gspca_dev;
-	int sharpness;
 	static const u8 sharpness_tb[][2] = {
 		{0x02, 0x03},
 		{0x04, 0x07},
@@ -5829,19 +5718,18 @@ static void setsharpness(struct gspca_dev *gspca_dev)
 		{0x10, 0x1e}
 	};
 
-	sharpness = sd->ctrls[SHARPNESS].val;
-	reg_w(gspca_dev, sharpness_tb[sharpness][0], 0x01c6);
+	reg_w(gspca_dev, sharpness_tb[val][0], 0x01c6);
 	reg_r(gspca_dev, 0x01c8);
 	reg_r(gspca_dev, 0x01c9);
 	reg_r(gspca_dev, 0x01ca);
-	reg_w(gspca_dev, sharpness_tb[sharpness][1], 0x01cb);
+	reg_w(gspca_dev, sharpness_tb[val][1], 0x01cb);
 }
 
-static void setcontrast(struct gspca_dev *gspca_dev)
+static void setcontrast(struct gspca_dev *gspca_dev,
+		s32 gamma, s32 brightness, s32 contrast)
 {
-	struct sd *sd = (struct sd *) gspca_dev;
 	const u8 *Tgamma;
-	int g, i, brightness, contrast, adj, gp1, gp2;
+	int g, i, adj, gp1, gp2;
 	u8 gr[16];
 	static const u8 delta_b[16] =		/* delta for brightness */
 		{0x50, 0x38, 0x2d, 0x28, 0x24, 0x21, 0x1e, 0x1d,
@@ -5864,10 +5752,10 @@ static void setcontrast(struct gspca_dev *gspca_dev)
 		 0xe0, 0xeb, 0xf4, 0xff, 0xff, 0xff, 0xff, 0xff},
 	};
 
-	Tgamma = gamma_tb[sd->ctrls[GAMMA].val - 1];
+	Tgamma = gamma_tb[gamma - 1];
 
-	contrast = ((int) sd->ctrls[CONTRAST].val - 128); /* -128 / 127 */
-	brightness = ((int) sd->ctrls[BRIGHTNESS].val - 128); /* -128 / 92 */
+	contrast -= 128; /* -128 / 127 */
+	brightness -= 128; /* -128 / 92 */
 	adj = 0;
 	gp1 = gp2 = 0;
 	for (i = 0; i < 16; i++) {
@@ -5894,25 +5782,15 @@ static void setcontrast(struct gspca_dev *gspca_dev)
 		reg_w(gspca_dev, gr[i], 0x0130 + i);	/* gradient */
 }
 
-static void getexposure(struct gspca_dev *gspca_dev)
+static s32 getexposure(struct gspca_dev *gspca_dev)
 {
-	struct sd *sd = (struct sd *) gspca_dev;
-
-	if (sd->sensor != SENSOR_HV7131R)
-		return;
-	sd->ctrls[EXPOSURE].val = (i2c_read(gspca_dev, 0x25) << 9)
+	return (i2c_read(gspca_dev, 0x25) << 9)
 		| (i2c_read(gspca_dev, 0x26) << 1)
 		| (i2c_read(gspca_dev, 0x27) >> 7);
 }
 
-static void setexposure(struct gspca_dev *gspca_dev)
+static void setexposure(struct gspca_dev *gspca_dev, s32 val)
 {
-	struct sd *sd = (struct sd *) gspca_dev;
-	int val;
-
-	if (sd->sensor != SENSOR_HV7131R)
-		return;
-	val = sd->ctrls[EXPOSURE].val;
 	i2c_write(gspca_dev, 0x25, val >> 9, 0x00);
 	i2c_write(gspca_dev, 0x26, val >> 1, 0x00);
 	i2c_write(gspca_dev, 0x27, val << 7, 0x00);
@@ -5921,20 +5799,8 @@ static void setexposure(struct gspca_dev *gspca_dev)
 static void setquality(struct gspca_dev *gspca_dev)
 {
 	struct sd *sd = (struct sd *) gspca_dev;
-	s8 reg07;
-
-	reg07 = 0;
-	switch (sd->sensor) {
-	case SENSOR_OV7620:
-		reg07 = 0x30;
-		break;
-	case SENSOR_HV7131R:
-	case SENSOR_PAS202B:
-		return;			/* done by work queue */
-	}
+	jpeg_set_qual(sd->jpeg_hdr, jpeg_qual[sd->reg08 >> 1]);
 	reg_w(gspca_dev, sd->reg08, ZC3XX_R008_CLOCKSETTING);
-	if (reg07 != 0)
-		reg_w(gspca_dev, reg07, 0x0007);
 }
 
 /* Matches the sensor's internal frame rate to the lighting frequency.
@@ -5943,7 +5809,7 @@ static void setquality(struct gspca_dev *gspca_dev)
  *	60Hz, for American lighting
  *	0 = No Fliker (for outdoore usage)
  */
-static void setlightfreq(struct gspca_dev *gspca_dev)
+static void setlightfreq(struct gspca_dev *gspca_dev, s32 val)
 {
 	struct sd *sd = (struct sd *) gspca_dev;
 	int i, mode;
@@ -6027,7 +5893,7 @@ static void setlightfreq(struct gspca_dev *gspca_dev)
 		 tas5130c_60HZ, tas5130c_60HZScale},
 	};
 
-	i = sd->ctrls[LIGHTFREQ].val * 2;
+	i = val * 2;
 	mode = gspca_dev->cam.cam_mode[gspca_dev->curr_mode].priv;
 	if (mode)
 		i++;			/* 320x240 */
@@ -6037,14 +5903,14 @@ static void setlightfreq(struct gspca_dev *gspca_dev)
 	usb_exchange(gspca_dev, zc3_freq);
 	switch (sd->sensor) {
 	case SENSOR_GC0305:
-		if (mode				/* if 320x240 */
-		    && sd->ctrls[LIGHTFREQ].val == 1)	/* and 50Hz */
+		if (mode		/* if 320x240 */
+		    && val == 1)	/* and 50Hz */
 			reg_w(gspca_dev, 0x85, 0x018d);
 					/* win: 0x80, 0x018d */
 		break;
 	case SENSOR_OV7620:
-		if (!mode) {				/* if 640x480 */
-			if (sd->ctrls[LIGHTFREQ].val != 0) /* and filter */
+		if (!mode) {		/* if 640x480 */
+			if (val != 0)	/* and filter */
 				reg_w(gspca_dev, 0x40, 0x0002);
 			else
 				reg_w(gspca_dev, 0x44, 0x0002);
@@ -6056,22 +5922,15 @@ static void setlightfreq(struct gspca_dev *gspca_dev)
 	}
 }
 
-static void setautogain(struct gspca_dev *gspca_dev)
+static void setautogain(struct gspca_dev *gspca_dev, s32 val)
 {
-	struct sd *sd = (struct sd *) gspca_dev;
-	u8 autoval;
-
-	if (sd->ctrls[AUTOGAIN].val)
-		autoval = 0x42;
-	else
-		autoval = 0x02;
-	reg_w(gspca_dev, autoval, 0x0180);
+	reg_w(gspca_dev, val ? 0x42 : 0x02, 0x0180);
 }
 
-/* update the transfer parameters */
-/* This function is executed from a work queue. */
-/* The exact use of the bridge registers 07 and 08 is not known.
- * The following algorithm has been adapted from ms-win traces */
+/*
+ * Update the transfer parameters.
+ * This function is executed from a work queue.
+ */
 static void transfer_update(struct work_struct *work)
 {
 	struct sd *sd = container_of(work, struct sd, work);
@@ -6079,96 +5938,55 @@ static void transfer_update(struct work_struct *work)
 	int change, good;
 	u8 reg07, reg11;
 
-	/* synchronize with the main driver and initialize the registers */
-	mutex_lock(&gspca_dev->usb_lock);
-	reg07 = 0;					/* max */
-	reg_w(gspca_dev, reg07, 0x0007);
-	reg_w(gspca_dev, sd->reg08, ZC3XX_R008_CLOCKSETTING);
-	mutex_unlock(&gspca_dev->usb_lock);
+	/* reg07 gets set to 0 by sd_start before starting us */
+	reg07 = 0;
 
 	good = 0;
 	for (;;) {
 		msleep(100);
 
-		/* get the transfer status */
-		/* the bit 0 of the bridge register 11 indicates overflow */
 		mutex_lock(&gspca_dev->usb_lock);
-		if (!gspca_dev->present || !gspca_dev->streaming)
+#ifdef CONFIG_PM
+		if (gspca_dev->frozen)
 			goto err;
+#endif
+		if (!gspca_dev->dev || !gspca_dev->streaming)
+			goto err;
+
+		/* Bit 0 of register 11 indicates FIFO overflow */
+		gspca_dev->usb_err = 0;
 		reg11 = reg_r(gspca_dev, 0x0011);
-		if (gspca_dev->usb_err < 0
-		 || !gspca_dev->present || !gspca_dev->streaming)
+		if (gspca_dev->usb_err)
 			goto err;
 
 		change = reg11 & 0x01;
 		if (change) {				/* overflow */
-			switch (reg07) {
-			case 0:				/* max */
-				reg07 = sd->sensor == SENSOR_HV7131R
-						? 0x30 : 0x32;
-				if (sd->reg08 != 0) {
-					change = 3;
-					sd->reg08--;
-				}
-				break;
-			case 0x32:
-				reg07 -= 4;
-				break;
-			default:
-				reg07 -= 2;
-				break;
-			case 2:
-				change = 0;		/* already min */
-				break;
-			}
 			good = 0;
+
+			if (reg07 == 0) /* Bit Rate Control not enabled? */
+				reg07 = 0x32; /* Allow 98 bytes / unit */
+			else if (reg07 > 2)
+				reg07 -= 2; /* Decrease allowed bytes / unit */
+			else
+				change = 0;
 		} else {				/* no overflow */
-			if (reg07 != 0) {		/* if not max */
-				good++;
-				if (good >= 10) {
-					good = 0;
+			good++;
+			if (good >= 10) {
+				good = 0;
+				if (reg07) { /* BRC enabled? */
 					change = 1;
-					reg07 += 2;
-					switch (reg07) {
-					case 0x30:
-						if (sd->sensor == SENSOR_PAS202B)
-							reg07 += 2;
-						break;
-					case 0x32:
-					case 0x34:
+					if (reg07 < 0x32)
+						reg07 += 2;
+					else
 						reg07 = 0;
-						break;
-					}
-				}
-			} else {			/* reg07 max */
-				if (sd->reg08 < sizeof jpeg_qual - 1) {
-					good++;
-					if (good > 10) {
-						sd->reg08++;
-						change = 2;
-					}
 				}
 			}
 		}
 		if (change) {
-			if (change & 1) {
-				reg_w(gspca_dev, reg07, 0x0007);
-				if (gspca_dev->usb_err < 0
-				 || !gspca_dev->present
-				 || !gspca_dev->streaming)
-					goto err;
-			}
-			if (change & 2) {
-				reg_w(gspca_dev, sd->reg08,
-						ZC3XX_R008_CLOCKSETTING);
-				if (gspca_dev->usb_err < 0
-				 || !gspca_dev->present
-				 || !gspca_dev->streaming)
-					goto err;
-				sd->ctrls[QUALITY].val = jpeg_qual[sd->reg08];
-				jpeg_set_qual(sd->jpeg_hdr,
-						jpeg_qual[sd->reg08]);
-			}
+			gspca_dev->usb_err = 0;
+			reg_w(gspca_dev, reg07, 0x0007);
+			if (gspca_dev->usb_err)
+				goto err;
 		}
 		mutex_unlock(&gspca_dev->usb_lock);
 	}
@@ -6503,7 +6321,6 @@ static int sd_config(struct gspca_dev *gspca_dev,
 	/* define some sensors from the vendor/product */
 	sd->sensor = id->driver_info;
 
-	gspca_dev->cam.ctrls = sd->ctrls;
 	sd->reg08 = REG08_DEF;
 
 	INIT_WORK(&sd->work, transfer_update);
@@ -6511,12 +6328,87 @@ static int sd_config(struct gspca_dev *gspca_dev,
 	return 0;
 }
 
-/* this function is called at probe and resume time */
-static int sd_init(struct gspca_dev *gspca_dev)
+static int zcxx_g_volatile_ctrl(struct v4l2_ctrl *ctrl)
 {
-	struct sd *sd = (struct sd *) gspca_dev;
-	struct cam *cam;
-	int sensor;
+	struct gspca_dev *gspca_dev =
+		container_of(ctrl->handler, struct gspca_dev, ctrl_handler);
+	struct sd *sd = (struct sd *)gspca_dev;
+
+	switch (ctrl->id) {
+	case V4L2_CID_AUTOGAIN:
+		gspca_dev->usb_err = 0;
+		if (ctrl->val && sd->exposure && gspca_dev->streaming)
+			sd->exposure->val = getexposure(gspca_dev);
+		return gspca_dev->usb_err;
+	}
+	return -EINVAL;
+}
+
+static int zcxx_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 i, qual;
+
+	gspca_dev->usb_err = 0;
+
+	if (ctrl->id == V4L2_CID_JPEG_COMPRESSION_QUALITY) {
+		qual = sd->reg08 >> 1;
+
+		for (i = 0; i < ARRAY_SIZE(jpeg_qual); i++) {
+			if (ctrl->val <= jpeg_qual[i])
+				break;
+		}
+		if (i > 0 && i == qual && ctrl->val < jpeg_qual[i])
+			i--;
+
+		/* With high quality settings we need max bandwidth */
+		if (i >= 2 && gspca_dev->streaming &&
+		    !gspca_dev->cam.needs_full_bandwidth)
+			return -EBUSY;
+
+		sd->reg08 = (i << 1) | 1;
+		ctrl->val = jpeg_qual[i];
+	}
+
+	if (!gspca_dev->streaming)
+		return 0;
+
+	switch (ctrl->id) {
+	/* gamma/brightness/contrast cluster */
+	case V4L2_CID_GAMMA:
+		setcontrast(gspca_dev, sd->gamma->val,
+				sd->brightness->val, sd->contrast->val);
+		break;
+	/* autogain/exposure cluster */
+	case V4L2_CID_AUTOGAIN:
+		setautogain(gspca_dev, ctrl->val);
+		if (!gspca_dev->usb_err && !ctrl->val && sd->exposure)
+			setexposure(gspca_dev, sd->exposure->val);
+		break;
+	case V4L2_CID_POWER_LINE_FREQUENCY:
+		setlightfreq(gspca_dev, ctrl->val);
+		break;
+	case V4L2_CID_SHARPNESS:
+		setsharpness(gspca_dev, ctrl->val);
+		break;
+	case V4L2_CID_JPEG_COMPRESSION_QUALITY:
+		setquality(gspca_dev);
+		break;
+	}
+	return gspca_dev->usb_err;
+}
+
+static const struct v4l2_ctrl_ops zcxx_ctrl_ops = {
+	.g_volatile_ctrl = zcxx_g_volatile_ctrl,
+	.s_ctrl = zcxx_s_ctrl,
+};
+
+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;
 	static const u8 gamma[SENSOR_MAX] = {
 		[SENSOR_ADCM2700] =	4,
 		[SENSOR_CS2102] =	4,
@@ -6538,6 +6430,48 @@ static int sd_init(struct gspca_dev *gspca_dev)
 		[SENSOR_PO2030] =	4,
 		[SENSOR_TAS5130C] =	3,
 	};
+
+	gspca_dev->vdev.ctrl_handler = hdl;
+	v4l2_ctrl_handler_init(hdl, 8);
+	sd->brightness = v4l2_ctrl_new_std(hdl, &zcxx_ctrl_ops,
+			V4L2_CID_BRIGHTNESS, 0, 255, 1, 128);
+	sd->contrast = v4l2_ctrl_new_std(hdl, &zcxx_ctrl_ops,
+			V4L2_CID_CONTRAST, 0, 255, 1, 128);
+	sd->gamma = v4l2_ctrl_new_std(hdl, &zcxx_ctrl_ops,
+			V4L2_CID_GAMMA, 1, 6, 1, gamma[sd->sensor]);
+	if (sd->sensor == SENSOR_HV7131R)
+		sd->exposure = v4l2_ctrl_new_std(hdl, &zcxx_ctrl_ops,
+			V4L2_CID_EXPOSURE, 0x30d, 0x493e, 1, 0x927);
+	sd->autogain = v4l2_ctrl_new_std(hdl, &zcxx_ctrl_ops,
+			V4L2_CID_AUTOGAIN, 0, 1, 1, 1);
+	if (sd->sensor != SENSOR_OV7630C)
+		sd->plfreq = v4l2_ctrl_new_std_menu(hdl, &zcxx_ctrl_ops,
+			V4L2_CID_POWER_LINE_FREQUENCY,
+			V4L2_CID_POWER_LINE_FREQUENCY_60HZ, 0,
+			V4L2_CID_POWER_LINE_FREQUENCY_DISABLED);
+	sd->sharpness = v4l2_ctrl_new_std(hdl, &zcxx_ctrl_ops,
+			V4L2_CID_SHARPNESS, 0, 3, 1,
+			sd->sensor == SENSOR_PO2030 ? 0 : 2);
+	sd->jpegqual = v4l2_ctrl_new_std(hdl, &zcxx_ctrl_ops,
+			V4L2_CID_JPEG_COMPRESSION_QUALITY,
+			jpeg_qual[0], jpeg_qual[ARRAY_SIZE(jpeg_qual) - 1], 1,
+			jpeg_qual[REG08_DEF >> 1]);
+	if (hdl->error) {
+		pr_err("Could not initialize controls\n");
+		return hdl->error;
+	}
+	v4l2_ctrl_cluster(3, &sd->gamma);
+	if (sd->sensor == SENSOR_HV7131R)
+		v4l2_ctrl_auto_cluster(2, &sd->autogain, 0, true);
+	return 0;
+}
+
+/* this function is called at probe and resume time */
+static int sd_init(struct gspca_dev *gspca_dev)
+{
+	struct sd *sd = (struct sd *) gspca_dev;
+	struct cam *cam;
+	int sensor;
 	static const u8 mode_tb[SENSOR_MAX] = {
 		[SENSOR_ADCM2700] =	2,
 		[SENSOR_CS2102] =	1,
@@ -6559,27 +6493,6 @@ static int sd_init(struct gspca_dev *gspca_dev)
 		[SENSOR_PO2030] =	1,
 		[SENSOR_TAS5130C] =	1,
 	};
-	static const u8 reg08_tb[SENSOR_MAX] = {
-		[SENSOR_ADCM2700] =	1,
-		[SENSOR_CS2102] =	3,
-		[SENSOR_CS2102K] =	3,
-		[SENSOR_GC0303] =	2,
-		[SENSOR_GC0305] =	3,
-		[SENSOR_HDCS2020] =	1,
-		[SENSOR_HV7131B] =	3,
-		[SENSOR_HV7131R] =	3,
-		[SENSOR_ICM105A] =	3,
-		[SENSOR_MC501CB] =	3,
-		[SENSOR_MT9V111_1] =	3,
-		[SENSOR_MT9V111_3] =	3,
-		[SENSOR_OV7620] =	1,
-		[SENSOR_OV7630C] =	3,
-		[SENSOR_PAS106] =	3,
-		[SENSOR_PAS202B] =	3,
-		[SENSOR_PB0330] =	3,
-		[SENSOR_PO2030] =	2,
-		[SENSOR_TAS5130C] =	3,
-	};
 
 	sensor = zcxx_probeSensor(gspca_dev);
 	if (sensor >= 0)
@@ -6688,7 +6601,6 @@ static int sd_init(struct gspca_dev *gspca_dev)
 		case 0x2030:
 			PDEBUG(D_PROBE, "Find Sensor PO2030");
 			sd->sensor = SENSOR_PO2030;
-			sd->ctrls[SHARPNESS].def = 0;	/* from win traces */
 			break;
 		case 0x7620:
 			PDEBUG(D_PROBE, "Find Sensor OV7620");
@@ -6730,36 +6642,18 @@ static int sd_init(struct gspca_dev *gspca_dev)
 		break;
 	}
 
-	sd->ctrls[GAMMA].def = gamma[sd->sensor];
-	sd->reg08 = reg08_tb[sd->sensor];
-	sd->ctrls[QUALITY].def = jpeg_qual[sd->reg08];
-	sd->ctrls[QUALITY].min = jpeg_qual[0];
-	sd->ctrls[QUALITY].max = jpeg_qual[ARRAY_SIZE(jpeg_qual) - 1];
-
-	switch (sd->sensor) {
-	case SENSOR_HV7131R:
-		gspca_dev->ctrl_dis = (1 << QUALITY);
-		break;
-	case SENSOR_OV7630C:
-		gspca_dev->ctrl_dis = (1 << LIGHTFREQ) | (1 << EXPOSURE);
-		break;
-	case SENSOR_PAS202B:
-		gspca_dev->ctrl_dis = (1 << QUALITY) | (1 << EXPOSURE);
-		break;
-	default:
-		gspca_dev->ctrl_dis = (1 << EXPOSURE);
-		break;
-	}
-#if AUTOGAIN_DEF
-	if (sd->ctrls[AUTOGAIN].val)
-		gspca_dev->ctrl_inac = (1 << EXPOSURE);
-#endif
-
 	/* switch off the led */
 	reg_w(gspca_dev, 0x01, 0x0000);
 	return gspca_dev->usb_err;
 }
 
+static int sd_pre_start(struct gspca_dev *gspca_dev)
+{
+	struct sd *sd = (struct sd *) gspca_dev;
+	gspca_dev->cam.needs_full_bandwidth = (sd->reg08 >= 4) ? 1 : 0;
+	return 0;
+}
+
 static int sd_start(struct gspca_dev *gspca_dev)
 {
 	struct sd *sd = (struct sd *) gspca_dev;
@@ -6864,7 +6758,7 @@ static int sd_start(struct gspca_dev *gspca_dev)
 		reg_w(gspca_dev, 0x03, 0x0008);
 		break;
 	}
-	setsharpness(gspca_dev);
+	setsharpness(gspca_dev, v4l2_ctrl_g_ctrl(sd->sharpness));
 
 	/* set the gamma tables when not set */
 	switch (sd->sensor) {
@@ -6873,7 +6767,9 @@ static int sd_start(struct gspca_dev *gspca_dev)
 	case SENSOR_OV7630C:
 		break;
 	default:
-		setcontrast(gspca_dev);
+		setcontrast(gspca_dev, v4l2_ctrl_g_ctrl(sd->gamma),
+				v4l2_ctrl_g_ctrl(sd->brightness),
+				v4l2_ctrl_g_ctrl(sd->contrast));
 		break;
 	}
 	setmatrix(gspca_dev);			/* one more time? */
@@ -6885,8 +6781,10 @@ static int sd_start(struct gspca_dev *gspca_dev)
 		break;
 	}
 	setquality(gspca_dev);
-	jpeg_set_qual(sd->jpeg_hdr, jpeg_qual[sd->reg08]);
-	setlightfreq(gspca_dev);
+	/* Start with BRC disabled, transfer_update will enable it if needed */
+	reg_w(gspca_dev, 0x00, 0x0007);
+	if (sd->plfreq)
+		setlightfreq(gspca_dev, v4l2_ctrl_g_ctrl(sd->plfreq));
 
 	switch (sd->sensor) {
 	case SENSOR_ADCM2700:
@@ -6897,7 +6795,7 @@ static int sd_start(struct gspca_dev *gspca_dev)
 		reg_w(gspca_dev, 0x40, 0x0117);
 		break;
 	case SENSOR_HV7131R:
-		setexposure(gspca_dev);
+		setexposure(gspca_dev, v4l2_ctrl_g_ctrl(sd->exposure));
 		reg_w(gspca_dev, 0x00, ZC3XX_R1A7_CALCGLOBALMEAN);
 		break;
 	case SENSOR_GC0305:
@@ -6921,21 +6819,16 @@ static int sd_start(struct gspca_dev *gspca_dev)
 		break;
 	}
 
-	setautogain(gspca_dev);
+	setautogain(gspca_dev, v4l2_ctrl_g_ctrl(sd->autogain));
 
-	/* start the transfer update thread if needed */
-	if (gspca_dev->usb_err >= 0) {
-		switch (sd->sensor) {
-		case SENSOR_HV7131R:
-		case SENSOR_PAS202B:
-			sd->work_thread =
-				create_singlethread_workqueue(KBUILD_MODNAME);
-			queue_work(sd->work_thread, &sd->work);
-			break;
-		}
-	}
+	if (gspca_dev->usb_err < 0)
+		return gspca_dev->usb_err;
 
-	return gspca_dev->usb_err;
+	/* Start the transfer parameters update thread */
+	sd->work_thread = create_singlethread_workqueue(KBUILD_MODNAME);
+	queue_work(sd->work_thread, &sd->work);
+
+	return 0;
 }
 
 /* called on streamoff with alt 0 and on disconnect */
@@ -6949,7 +6842,7 @@ static void sd_stop0(struct gspca_dev *gspca_dev)
 		mutex_lock(&gspca_dev->usb_lock);
 		sd->work_thread = NULL;
 	}
-	if (!gspca_dev->present)
+	if (!gspca_dev->dev)
 		return;
 	send_unknown(gspca_dev, sd->sensor);
 }
@@ -6987,72 +6880,17 @@ static void sd_pkt_scan(struct gspca_dev *gspca_dev,
 	gspca_frame_add(gspca_dev, INTER_PACKET, data, len);
 }
 
-static int sd_setautogain(struct gspca_dev *gspca_dev, __s32 val)
-{
-	struct sd *sd = (struct sd *) gspca_dev;
-
-	sd->ctrls[AUTOGAIN].val = val;
-	if (val) {
-		gspca_dev->ctrl_inac |= (1 << EXPOSURE);
-	} else {
-		gspca_dev->ctrl_inac &= ~(1 << EXPOSURE);
-		if (gspca_dev->streaming)
-			getexposure(gspca_dev);
-	}
-	if (gspca_dev->streaming)
-		setautogain(gspca_dev);
-	return gspca_dev->usb_err;
-}
-
-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;
-}
-
-static int sd_setquality(struct gspca_dev *gspca_dev, __s32 val)
-{
-	struct sd *sd = (struct sd *) gspca_dev;
-	int i;
-
-	for (i = 0; i < ARRAY_SIZE(jpeg_qual) - 1; i++) {
-		if (val <= jpeg_qual[i])
-			break;
-	}
-	if (i > 0
-	 && i == sd->reg08
-	 && val < jpeg_qual[sd->reg08])
-		i--;
-	sd->reg08 = i;
-	sd->ctrls[QUALITY].val = jpeg_qual[i];
-	if (gspca_dev->streaming)
-		jpeg_set_qual(sd->jpeg_hdr, sd->ctrls[QUALITY].val);
-	return gspca_dev->usb_err;
-}
-
 static int sd_set_jcomp(struct gspca_dev *gspca_dev,
 			struct v4l2_jpegcompression *jcomp)
 {
 	struct sd *sd = (struct sd *) gspca_dev;
+	int ret;
 
-	sd_setquality(gspca_dev, jcomp->quality);
-	jcomp->quality = sd->ctrls[QUALITY].val;
-	return gspca_dev->usb_err;
+	ret = v4l2_ctrl_s_ctrl(sd->jpegqual, jcomp->quality);
+	if (ret)
+		return ret;
+	jcomp->quality = v4l2_ctrl_g_ctrl(sd->jpegqual);
+	return 0;
 }
 
 static int sd_get_jcomp(struct gspca_dev *gspca_dev,
@@ -7061,7 +6899,7 @@ static int sd_get_jcomp(struct gspca_dev *gspca_dev,
 	struct sd *sd = (struct sd *) gspca_dev;
 
 	memset(jcomp, 0, sizeof *jcomp);
-	jcomp->quality = sd->ctrls[QUALITY].val;
+	jcomp->quality = v4l2_ctrl_g_ctrl(sd->jpegqual);
 	jcomp->jpeg_markers = V4L2_JPEG_MARKER_DHT
 			| V4L2_JPEG_MARKER_DQT;
 	return 0;
@@ -7085,14 +6923,13 @@ static int sd_int_pkt_scan(struct gspca_dev *gspca_dev,
 
 static const struct sd_desc sd_desc = {
 	.name = KBUILD_MODNAME,
-	.ctrls = sd_ctrls,
-	.nctrls = ARRAY_SIZE(sd_ctrls),
 	.config = sd_config,
 	.init = sd_init,
+	.init_controls = sd_init_controls,
+	.isoc_init = sd_pre_start,
 	.start = sd_start,
 	.stop0 = sd_stop0,
 	.pkt_scan = sd_pkt_scan,
-	.querymenu = sd_querymenu,
 	.get_jcomp = sd_get_jcomp,
 	.set_jcomp = sd_set_jcomp,
 #if defined(CONFIG_INPUT) || defined(CONFIG_INPUT_MODULE)
@@ -7176,6 +7013,7 @@ static struct usb_driver sd_driver = {
 #ifdef CONFIG_PM
 	.suspend = gspca_suspend,
 	.resume = gspca_resume,
+	.reset_resume = gspca_resume,
 #endif
 };
 
diff --git a/drivers/media/video/hdpvr/hdpvr-control.c b/drivers/media/video/hdpvr/hdpvr-control.c
index 068df4ba3f5..ae8f229d114 100644
--- a/drivers/media/video/hdpvr/hdpvr-control.c
+++ b/drivers/media/video/hdpvr/hdpvr-control.c
@@ -113,6 +113,8 @@ int get_input_lines_info(struct hdpvr_device *dev)
 			 "get input lines info returned: %d, %s\n", ret,
 			 print_buf);
 	}
+#else
+	(void)ret;	/* suppress compiler warning */
 #endif
 	lines = dev->usbc_buf[1] << 8 | dev->usbc_buf[0];
 	mutex_unlock(&dev->usbc_mutex);
diff --git a/drivers/media/video/hdpvr/hdpvr-core.c b/drivers/media/video/hdpvr/hdpvr-core.c
index 6510110f53d..304f43ef59e 100644
--- a/drivers/media/video/hdpvr/hdpvr-core.c
+++ b/drivers/media/video/hdpvr/hdpvr-core.c
@@ -303,7 +303,7 @@ static int hdpvr_probe(struct usb_interface *interface,
 	/* allocate memory for our device state and initialize it */
 	dev = kzalloc(sizeof(*dev), GFP_KERNEL);
 	if (!dev) {
-		err("Out of memory");
+		dev_err(&interface->dev, "Out of memory\n");
 		goto error;
 	}
 
@@ -311,7 +311,7 @@ static int hdpvr_probe(struct usb_interface *interface,
 
 	/* register v4l2_device early so it can be used for printks */
 	if (v4l2_device_register(&interface->dev, &dev->v4l2_dev)) {
-		err("v4l2_device_register failed");
+		dev_err(&interface->dev, "v4l2_device_register failed\n");
 		goto error;
 	}
 
diff --git a/drivers/media/video/hdpvr/hdpvr-video.c b/drivers/media/video/hdpvr/hdpvr-video.c
index 11ffe9cc178..0e9e156bb2a 100644
--- a/drivers/media/video/hdpvr/hdpvr-video.c
+++ b/drivers/media/video/hdpvr/hdpvr-video.c
@@ -994,7 +994,7 @@ static int hdpvr_try_ctrl(struct v4l2_ext_control *ctrl, int ac3)
 	default:
 		return -EINVAL;
 	}
-	return 0;
+	return ret;
 }
 
 static int vidioc_try_ext_ctrls(struct file *file, void *priv,
diff --git a/drivers/media/video/hexium_gemini.c b/drivers/media/video/hexium_gemini.c
index a62322d5c0d..366434f5647 100644
--- a/drivers/media/video/hexium_gemini.c
+++ b/drivers/media/video/hexium_gemini.c
@@ -40,15 +40,15 @@ static int hexium_num;
 
 #define HEXIUM_INPUTS	9
 static struct v4l2_input hexium_inputs[HEXIUM_INPUTS] = {
-	{ 0, "CVBS 1",	V4L2_INPUT_TYPE_CAMERA,	2, 0, V4L2_STD_PAL_BG|V4L2_STD_NTSC_M, 0, V4L2_IN_CAP_STD },
-	{ 1, "CVBS 2",	V4L2_INPUT_TYPE_CAMERA,	2, 0, V4L2_STD_PAL_BG|V4L2_STD_NTSC_M, 0, V4L2_IN_CAP_STD },
-	{ 2, "CVBS 3",	V4L2_INPUT_TYPE_CAMERA,	2, 0, V4L2_STD_PAL_BG|V4L2_STD_NTSC_M, 0, V4L2_IN_CAP_STD },
-	{ 3, "CVBS 4",	V4L2_INPUT_TYPE_CAMERA,	2, 0, V4L2_STD_PAL_BG|V4L2_STD_NTSC_M, 0, V4L2_IN_CAP_STD },
-	{ 4, "CVBS 5",	V4L2_INPUT_TYPE_CAMERA,	2, 0, V4L2_STD_PAL_BG|V4L2_STD_NTSC_M, 0, V4L2_IN_CAP_STD },
-	{ 5, "CVBS 6",	V4L2_INPUT_TYPE_CAMERA,	2, 0, V4L2_STD_PAL_BG|V4L2_STD_NTSC_M, 0, V4L2_IN_CAP_STD },
-	{ 6, "Y/C 1",	V4L2_INPUT_TYPE_CAMERA,	2, 0, V4L2_STD_PAL_BG|V4L2_STD_NTSC_M, 0, V4L2_IN_CAP_STD },
-	{ 7, "Y/C 2",	V4L2_INPUT_TYPE_CAMERA,	2, 0, V4L2_STD_PAL_BG|V4L2_STD_NTSC_M, 0, V4L2_IN_CAP_STD },
-	{ 8, "Y/C 3",	V4L2_INPUT_TYPE_CAMERA,	2, 0, V4L2_STD_PAL_BG|V4L2_STD_NTSC_M, 0, V4L2_IN_CAP_STD },
+	{ 0, "CVBS 1",	V4L2_INPUT_TYPE_CAMERA,	0, 0, V4L2_STD_ALL, 0, V4L2_IN_CAP_STD },
+	{ 1, "CVBS 2",	V4L2_INPUT_TYPE_CAMERA,	0, 0, V4L2_STD_ALL, 0, V4L2_IN_CAP_STD },
+	{ 2, "CVBS 3",	V4L2_INPUT_TYPE_CAMERA,	0, 0, V4L2_STD_ALL, 0, V4L2_IN_CAP_STD },
+	{ 3, "CVBS 4",	V4L2_INPUT_TYPE_CAMERA,	0, 0, V4L2_STD_ALL, 0, V4L2_IN_CAP_STD },
+	{ 4, "CVBS 5",	V4L2_INPUT_TYPE_CAMERA,	0, 0, V4L2_STD_ALL, 0, V4L2_IN_CAP_STD },
+	{ 5, "CVBS 6",	V4L2_INPUT_TYPE_CAMERA,	0, 0, V4L2_STD_ALL, 0, V4L2_IN_CAP_STD },
+	{ 6, "Y/C 1",	V4L2_INPUT_TYPE_CAMERA,	0, 0, V4L2_STD_ALL, 0, V4L2_IN_CAP_STD },
+	{ 7, "Y/C 2",	V4L2_INPUT_TYPE_CAMERA,	0, 0, V4L2_STD_ALL, 0, V4L2_IN_CAP_STD },
+	{ 8, "Y/C 3",	V4L2_INPUT_TYPE_CAMERA,	0, 0, V4L2_STD_ALL, 0, V4L2_IN_CAP_STD },
 };
 
 #define HEXIUM_AUDIOS	0
@@ -59,11 +59,6 @@ struct hexium_data
 	u8 byte;
 };
 
-#define HEXIUM_CONTROLS	1
-static struct v4l2_queryctrl hexium_controls[] = {
-	{ V4L2_CID_PRIVATE_BASE, V4L2_CTRL_TYPE_BOOLEAN, "B/W", 0, 1, 1, 0, 0 },
-};
-
 #define HEXIUM_GEMINI_V_1_0		1
 #define HEXIUM_GEMINI_DUAL_V_1_0	2
 
@@ -76,7 +71,6 @@ struct hexium
 
 	int 		cur_input;	/* current input */
 	v4l2_std_id 	cur_std;	/* current standard */
-	int		cur_bw;		/* current black/white status */
 };
 
 /* Samsung KS0127B decoder default registers */
@@ -119,18 +113,10 @@ static struct hexium_data hexium_pal[] = {
 	{ 0x01, 0x52 }, { 0x12, 0x64 }, { 0x2D, 0x2C }, { 0x2E, 0x9B }, { -1 , 0xFF }
 };
 
-static struct hexium_data hexium_pal_bw[] = {
-	{ 0x01, 0x52 },	{ 0x12, 0x64 },	{ 0x2D, 0x2C },	{ 0x2E, 0x9B },	{ -1 , 0xFF }
-};
-
 static struct hexium_data hexium_ntsc[] = {
 	{ 0x01, 0x53 }, { 0x12, 0x04 }, { 0x2D, 0x23 }, { 0x2E, 0x81 }, { -1 , 0xFF }
 };
 
-static struct hexium_data hexium_ntsc_bw[] = {
-	{ 0x01, 0x53 }, { 0x12, 0x04 }, { 0x2D, 0x23 }, { 0x2E, 0x81 }, { -1 , 0xFF }
-};
-
 static struct hexium_data hexium_secam[] = {
 	{ 0x01, 0x52 }, { 0x12, 0x64 }, { 0x2D, 0x2C }, { 0x2E, 0x9B }, { -1 , 0xFF }
 };
@@ -264,93 +250,6 @@ static int vidioc_s_input(struct file *file, void *fh, unsigned int input)
 	return 0;
 }
 
-/* the saa7146 provides some controls (brightness, contrast, saturation)
-   which gets registered *after* this function. because of this we have
-   to return with a value != 0 even if the function succeeded.. */
-static int vidioc_queryctrl(struct file *file, void *fh, struct v4l2_queryctrl *qc)
-{
-	struct saa7146_dev *dev = ((struct saa7146_fh *)fh)->dev;
-	int i;
-
-	for (i = HEXIUM_CONTROLS - 1; i >= 0; i--) {
-		if (hexium_controls[i].id == qc->id) {
-			*qc = hexium_controls[i];
-			DEB_D("VIDIOC_QUERYCTRL %d\n", qc->id);
-			return 0;
-		}
-	}
-	return dev->ext_vv_data->core_ops->vidioc_queryctrl(file, fh, qc);
-}
-
-static int vidioc_g_ctrl(struct file *file, void *fh, struct v4l2_control *vc)
-{
-	struct saa7146_dev *dev = ((struct saa7146_fh *)fh)->dev;
-	struct hexium *hexium = (struct hexium *) dev->ext_priv;
-	int i;
-
-	for (i = HEXIUM_CONTROLS - 1; i >= 0; i--) {
-		if (hexium_controls[i].id == vc->id)
-			break;
-	}
-
-	if (i < 0)
-		return dev->ext_vv_data->core_ops->vidioc_g_ctrl(file, fh, vc);
-
-	if (vc->id == V4L2_CID_PRIVATE_BASE) {
-		vc->value = hexium->cur_bw;
-		DEB_D("VIDIOC_G_CTRL BW:%d\n", vc->value);
-		return 0;
-	}
-	return -EINVAL;
-}
-
-static int vidioc_s_ctrl(struct file *file, void *fh, struct v4l2_control *vc)
-{
-	struct saa7146_dev *dev = ((struct saa7146_fh *)fh)->dev;
-	struct hexium *hexium = (struct hexium *) dev->ext_priv;
-	int i = 0;
-
-	for (i = HEXIUM_CONTROLS - 1; i >= 0; i--) {
-		if (hexium_controls[i].id == vc->id)
-			break;
-	}
-
-	if (i < 0)
-		return dev->ext_vv_data->core_ops->vidioc_s_ctrl(file, fh, vc);
-
-	if (vc->id == V4L2_CID_PRIVATE_BASE)
-		hexium->cur_bw = vc->value;
-
-	DEB_D("VIDIOC_S_CTRL BW:%d\n", hexium->cur_bw);
-
-	if (0 == hexium->cur_bw && V4L2_STD_PAL == hexium->cur_std) {
-		hexium_set_standard(hexium, hexium_pal);
-		return 0;
-	}
-	if (0 == hexium->cur_bw && V4L2_STD_NTSC == hexium->cur_std) {
-		hexium_set_standard(hexium, hexium_ntsc);
-		return 0;
-	}
-	if (0 == hexium->cur_bw && V4L2_STD_SECAM == hexium->cur_std) {
-		hexium_set_standard(hexium, hexium_secam);
-		return 0;
-	}
-	if (1 == hexium->cur_bw && V4L2_STD_PAL == hexium->cur_std) {
-		hexium_set_standard(hexium, hexium_pal_bw);
-		return 0;
-	}
-	if (1 == hexium->cur_bw && V4L2_STD_NTSC == hexium->cur_std) {
-		hexium_set_standard(hexium, hexium_ntsc_bw);
-		return 0;
-	}
-	if (1 == hexium->cur_bw && V4L2_STD_SECAM == hexium->cur_std)
-		/* fixme: is there no bw secam mode? */
-		return -EINVAL;
-
-	return -EINVAL;
-}
-
-
 static struct saa7146_ext_vv vv_data;
 
 /* this function only gets called when the probing was successful */
@@ -399,12 +298,10 @@ static int hexium_attach(struct saa7146_dev *dev, struct saa7146_pci_extension_d
 	hexium->cur_input = 0;
 
 	saa7146_vv_init(dev, &vv_data);
-	vv_data.ops.vidioc_queryctrl = vidioc_queryctrl;
-	vv_data.ops.vidioc_g_ctrl = vidioc_g_ctrl;
-	vv_data.ops.vidioc_s_ctrl = vidioc_s_ctrl;
-	vv_data.ops.vidioc_enum_input = vidioc_enum_input;
-	vv_data.ops.vidioc_g_input = vidioc_g_input;
-	vv_data.ops.vidioc_s_input = vidioc_s_input;
+
+	vv_data.vid_ops.vidioc_enum_input = vidioc_enum_input;
+	vv_data.vid_ops.vidioc_g_input = vidioc_g_input;
+	vv_data.vid_ops.vidioc_s_input = vidioc_s_input;
 	ret = saa7146_register_device(&hexium->video_dev, dev, "hexium gemini", VFL_TYPE_GRABBER);
 	if (ret < 0) {
 		pr_err("cannot register capture v4l2 device. skipping.\n");
diff --git a/drivers/media/video/hexium_orion.c b/drivers/media/video/hexium_orion.c
index 23debc967d9..a1eb26d1107 100644
--- a/drivers/media/video/hexium_orion.c
+++ b/drivers/media/video/hexium_orion.c
@@ -41,15 +41,15 @@ static int hexium_num;
 
 #define HEXIUM_INPUTS	9
 static struct v4l2_input hexium_inputs[HEXIUM_INPUTS] = {
-	{ 0, "CVBS 1",	V4L2_INPUT_TYPE_CAMERA,	2, 0, V4L2_STD_PAL_BG|V4L2_STD_NTSC_M, 0, V4L2_IN_CAP_STD },
-	{ 1, "CVBS 2",	V4L2_INPUT_TYPE_CAMERA,	2, 0, V4L2_STD_PAL_BG|V4L2_STD_NTSC_M, 0, V4L2_IN_CAP_STD },
-	{ 2, "CVBS 3",	V4L2_INPUT_TYPE_CAMERA,	2, 0, V4L2_STD_PAL_BG|V4L2_STD_NTSC_M, 0, V4L2_IN_CAP_STD },
-	{ 3, "CVBS 4",	V4L2_INPUT_TYPE_CAMERA,	2, 0, V4L2_STD_PAL_BG|V4L2_STD_NTSC_M, 0, V4L2_IN_CAP_STD },
-	{ 4, "CVBS 5",	V4L2_INPUT_TYPE_CAMERA,	2, 0, V4L2_STD_PAL_BG|V4L2_STD_NTSC_M, 0, V4L2_IN_CAP_STD },
-	{ 5, "CVBS 6",	V4L2_INPUT_TYPE_CAMERA,	2, 0, V4L2_STD_PAL_BG|V4L2_STD_NTSC_M, 0, V4L2_IN_CAP_STD },
-	{ 6, "Y/C 1",	V4L2_INPUT_TYPE_CAMERA,	2, 0, V4L2_STD_PAL_BG|V4L2_STD_NTSC_M, 0, V4L2_IN_CAP_STD },
-	{ 7, "Y/C 2",	V4L2_INPUT_TYPE_CAMERA,	2, 0, V4L2_STD_PAL_BG|V4L2_STD_NTSC_M, 0, V4L2_IN_CAP_STD },
-	{ 8, "Y/C 3",	V4L2_INPUT_TYPE_CAMERA,	2, 0, V4L2_STD_PAL_BG|V4L2_STD_NTSC_M, 0, V4L2_IN_CAP_STD },
+	{ 0, "CVBS 1",	V4L2_INPUT_TYPE_CAMERA,	0, 0, V4L2_STD_ALL, 0, V4L2_IN_CAP_STD },
+	{ 1, "CVBS 2",	V4L2_INPUT_TYPE_CAMERA,	0, 0, V4L2_STD_ALL, 0, V4L2_IN_CAP_STD },
+	{ 2, "CVBS 3",	V4L2_INPUT_TYPE_CAMERA,	0, 0, V4L2_STD_ALL, 0, V4L2_IN_CAP_STD },
+	{ 3, "CVBS 4",	V4L2_INPUT_TYPE_CAMERA,	0, 0, V4L2_STD_ALL, 0, V4L2_IN_CAP_STD },
+	{ 4, "CVBS 5",	V4L2_INPUT_TYPE_CAMERA,	0, 0, V4L2_STD_ALL, 0, V4L2_IN_CAP_STD },
+	{ 5, "CVBS 6",	V4L2_INPUT_TYPE_CAMERA,	0, 0, V4L2_STD_ALL, 0, V4L2_IN_CAP_STD },
+	{ 6, "Y/C 1",	V4L2_INPUT_TYPE_CAMERA,	0, 0, V4L2_STD_ALL, 0, V4L2_IN_CAP_STD },
+	{ 7, "Y/C 2",	V4L2_INPUT_TYPE_CAMERA,	0, 0, V4L2_STD_ALL, 0, V4L2_IN_CAP_STD },
+	{ 8, "Y/C 3",	V4L2_INPUT_TYPE_CAMERA,	0, 0, V4L2_STD_ALL, 0, V4L2_IN_CAP_STD },
 };
 
 #define HEXIUM_AUDIOS	0
@@ -371,9 +371,9 @@ static int hexium_attach(struct saa7146_dev *dev, struct saa7146_pci_extension_d
 	DEB_EE("\n");
 
 	saa7146_vv_init(dev, &vv_data);
-	vv_data.ops.vidioc_enum_input = vidioc_enum_input;
-	vv_data.ops.vidioc_g_input = vidioc_g_input;
-	vv_data.ops.vidioc_s_input = vidioc_s_input;
+	vv_data.vid_ops.vidioc_enum_input = vidioc_enum_input;
+	vv_data.vid_ops.vidioc_g_input = vidioc_g_input;
+	vv_data.vid_ops.vidioc_s_input = vidioc_s_input;
 	if (0 != saa7146_register_device(&hexium->video_dev, dev, "hexium orion", VFL_TYPE_GRABBER)) {
 		pr_err("cannot register capture v4l2 device. skipping.\n");
 		return -1;
diff --git a/drivers/media/video/ivtv/ivtv-driver.c b/drivers/media/video/ivtv/ivtv-driver.c
index 679262ed13b..057929e165a 100644
--- a/drivers/media/video/ivtv/ivtv-driver.c
+++ b/drivers/media/video/ivtv/ivtv-driver.c
@@ -1201,9 +1201,9 @@ static int __devinit ivtv_probe(struct pci_dev *pdev,
 		struct v4l2_ctrl_handler *hdl = itv->v4l2_dev.ctrl_handler;
 
 		itv->ctrl_pts = v4l2_ctrl_new_std(hdl, &ivtv_hdl_out_ops,
-				V4L2_CID_MPEG_VIDEO_DEC_PTS, 0, 0, 1, 0);
+				V4L2_CID_MPEG_VIDEO_DEC_PTS, 0, 0, 0, 0);
 		itv->ctrl_frame = v4l2_ctrl_new_std(hdl, &ivtv_hdl_out_ops,
-				V4L2_CID_MPEG_VIDEO_DEC_FRAME, 0, 0x7fffffff, 1, 0);
+				V4L2_CID_MPEG_VIDEO_DEC_FRAME, 0, 0, 0, 0);
 		/* Note: V4L2_MPEG_AUDIO_DEC_PLAYBACK_AUTO is not supported,
 		   mask that menu item. */
 		itv->ctrl_audio_playback =
diff --git a/drivers/media/video/ivtv/ivtv-fileops.c b/drivers/media/video/ivtv/ivtv-fileops.c
index c9663e885b9..9ff69b5a87e 100644
--- a/drivers/media/video/ivtv/ivtv-fileops.c
+++ b/drivers/media/video/ivtv/ivtv-fileops.c
@@ -746,8 +746,9 @@ unsigned int ivtv_v4l2_dec_poll(struct file *filp, poll_table *wait)
 	return res;
 }
 
-unsigned int ivtv_v4l2_enc_poll(struct file *filp, poll_table * wait)
+unsigned int ivtv_v4l2_enc_poll(struct file *filp, poll_table *wait)
 {
+	unsigned long req_events = poll_requested_events(wait);
 	struct ivtv_open_id *id = fh2id(filp->private_data);
 	struct ivtv *itv = id->itv;
 	struct ivtv_stream *s = &itv->streams[id->type];
@@ -755,7 +756,8 @@ unsigned int ivtv_v4l2_enc_poll(struct file *filp, poll_table * wait)
 	unsigned res = 0;
 
 	/* Start a capture if there is none */
-	if (!eof && !test_bit(IVTV_F_S_STREAMING, &s->s_flags)) {
+	if (!eof && !test_bit(IVTV_F_S_STREAMING, &s->s_flags) &&
+			(req_events & (POLLIN | POLLRDNORM))) {
 		int rc;
 
 		rc = ivtv_start_capture(id);
diff --git a/drivers/media/video/ivtv/ivtv-ioctl.c b/drivers/media/video/ivtv/ivtv-ioctl.c
index 989e556913e..f7d57b3f284 100644
--- a/drivers/media/video/ivtv/ivtv-ioctl.c
+++ b/drivers/media/video/ivtv/ivtv-ioctl.c
@@ -135,7 +135,6 @@ void ivtv_set_osd_alpha(struct ivtv *itv)
 int ivtv_set_speed(struct ivtv *itv, int speed)
 {
 	u32 data[CX2341X_MBOX_MAX_DATA];
-	struct ivtv_stream *s;
 	int single_step = (speed == 1 || speed == -1);
 	DEFINE_WAIT(wait);
 
@@ -145,8 +144,6 @@ int ivtv_set_speed(struct ivtv *itv, int speed)
 	if (speed == itv->speed && !single_step)
 		return 0;
 
-	s = &itv->streams[IVTV_DEC_STREAM_TYPE_MPG];
-
 	if (single_step && (speed < 0) == (itv->speed < 0)) {
 		/* Single step video and no need to change direction */
 		ivtv_vapi(itv, CX2341X_DEC_STEP_VIDEO, 1, 0);
@@ -1468,8 +1465,9 @@ static int ivtv_subscribe_event(struct v4l2_fh *fh, struct v4l2_event_subscripti
 	switch (sub->type) {
 	case V4L2_EVENT_VSYNC:
 	case V4L2_EVENT_EOS:
+		return v4l2_event_subscribe(fh, sub, 0, NULL);
 	case V4L2_EVENT_CTRL:
-		return v4l2_event_subscribe(fh, sub, 0);
+		return v4l2_event_subscribe(fh, sub, 0, &v4l2_ctrl_sub_ev_ops);
 	default:
 		return -EINVAL;
 	}
@@ -1827,7 +1825,7 @@ static long ivtv_default(struct file *file, void *fh, bool valid_prio,
 		return ivtv_decoder_ioctls(file, cmd, (void *)arg);
 
 	default:
-		return -EINVAL;
+		return -ENOTTY;
 	}
 	return 0;
 }
diff --git a/drivers/media/video/ivtv/ivtv-streams.c b/drivers/media/video/ivtv/ivtv-streams.c
index 7ea5ca7f012..6738592aa35 100644
--- a/drivers/media/video/ivtv/ivtv-streams.c
+++ b/drivers/media/video/ivtv/ivtv-streams.c
@@ -228,6 +228,10 @@ static int ivtv_prep_dev(struct ivtv *itv, int type)
 	s->vdev->release = video_device_release;
 	s->vdev->tvnorms = V4L2_STD_ALL;
 	s->vdev->lock = &itv->serialize_lock;
+	/* Locking in file operations other than ioctl should be done
+	   by the driver, not the V4L2 core.
+	   This driver needs auditing so that this flag can be removed. */
+	set_bit(V4L2_FL_LOCK_ALL_FOPS, &s->vdev->flags);
 	set_bit(V4L2_FL_USE_FH_PRIO, &s->vdev->flags);
 	ivtv_set_funcs(s->vdev);
 	return 0;
diff --git a/drivers/media/video/ivtv/ivtvfb.c b/drivers/media/video/ivtv/ivtvfb.c
index e5e7fa9e737..05b94aa8ba3 100644
--- a/drivers/media/video/ivtv/ivtvfb.c
+++ b/drivers/media/video/ivtv/ivtvfb.c
@@ -1293,6 +1293,7 @@ static int __init ivtvfb_init(void)
 
 	drv = driver_find("ivtv", &pci_bus_type);
 	err = driver_for_each_device(drv, NULL, &registered, ivtvfb_callback_init);
+	(void)err;	/* suppress compiler warning */
 	if (!registered) {
 		printk(KERN_ERR "ivtvfb:  no cards found\n");
 		return -ENODEV;
@@ -1309,6 +1310,7 @@ static void ivtvfb_cleanup(void)
 
 	drv = driver_find("ivtv", &pci_bus_type);
 	err = driver_for_each_device(drv, NULL, NULL, ivtvfb_callback_cleanup);
+	(void)err;	/* suppress compiler warning */
 }
 
 module_init(ivtvfb_init);
diff --git a/drivers/media/video/m5mols/m5mols.h b/drivers/media/video/m5mols/m5mols.h
index 4b021e1ee5f..bb589917b65 100644
--- a/drivers/media/video/m5mols/m5mols.h
+++ b/drivers/media/video/m5mols/m5mols.h
@@ -21,11 +21,6 @@
 
 extern int m5mols_debug;
 
-#define to_m5mols(__sd)	container_of(__sd, struct m5mols_info, sd)
-
-#define to_sd(__ctrl) \
-	(&container_of(__ctrl->handler, struct m5mols_info, handle)->sd)
-
 enum m5mols_restype {
 	M5MOLS_RESTYPE_MONITOR,
 	M5MOLS_RESTYPE_CAPTURE,
@@ -163,21 +158,27 @@ struct m5mols_version {
  * @ffmt: current fmt according to resolution type
  * @res_type: current resolution type
  * @irq_waitq: waitqueue for the capture
- * @flags: state variable for the interrupt handler
+ * @irq_done: set to 1 in the interrupt handler
  * @handle: control handler
- * @autoexposure: Auto Exposure control
- * @exposure: Exposure control
- * @autowb: Auto White Balance control
- * @colorfx: Color effect control
- * @saturation:	Saturation control
- * @zoom: Zoom control
+ * @auto_exposure: auto/manual exposure control
+ * @exposure_bias: exposure compensation control
+ * @exposure: manual exposure control
+ * @metering: exposure metering control
+ * @auto_iso: auto/manual ISO sensitivity control
+ * @iso: manual ISO sensitivity control
+ * @auto_wb: auto white balance control
+ * @lock_3a: 3A lock control
+ * @colorfx: color effect control
+ * @saturation: saturation control
+ * @zoom: zoom control
+ * @wdr: wide dynamic range control
+ * @stabilization: image stabilization control
+ * @jpeg_quality: JPEG compression quality control
  * @ver: information of the version
  * @cap: the capture mode attributes
- * @power: current sensor's power status
  * @isp_ready: 1 when the ISP controller has completed booting
+ * @power: current sensor's power status
  * @ctrl_sync: 1 when the control handler state is restored in H/W
- * @lock_ae: true means the Auto Exposure is locked
- * @lock_awb: true means the Aut WhiteBalance is locked
  * @resolution:	register value for current resolution
  * @mode: register value for current operation mode
  * @set_power: optional power callback to the board code
@@ -193,15 +194,27 @@ struct m5mols_info {
 	atomic_t irq_done;
 
 	struct v4l2_ctrl_handler handle;
+	struct {
+		/* exposure/exposure bias/auto exposure cluster */
+		struct v4l2_ctrl *auto_exposure;
+		struct v4l2_ctrl *exposure_bias;
+		struct v4l2_ctrl *exposure;
+		struct v4l2_ctrl *metering;
+	};
+	struct {
+		/* iso/auto iso cluster */
+		struct v4l2_ctrl *auto_iso;
+		struct v4l2_ctrl *iso;
+	};
+	struct v4l2_ctrl *auto_wb;
 
-	/* Autoexposure/exposure control cluster */
-	struct v4l2_ctrl *autoexposure;
-	struct v4l2_ctrl *exposure;
-
-	struct v4l2_ctrl *autowb;
+	struct v4l2_ctrl *lock_3a;
 	struct v4l2_ctrl *colorfx;
 	struct v4l2_ctrl *saturation;
 	struct v4l2_ctrl *zoom;
+	struct v4l2_ctrl *wdr;
+	struct v4l2_ctrl *stabilization;
+	struct v4l2_ctrl *jpeg_quality;
 
 	struct m5mols_version ver;
 	struct m5mols_capture cap;
@@ -210,8 +223,6 @@ struct m5mols_info {
 	unsigned int power:1;
 	unsigned int ctrl_sync:1;
 
-	bool lock_ae;
-	bool lock_awb;
 	u8 resolution;
 	u8 mode;
 
@@ -282,7 +293,7 @@ int m5mols_busy_wait(struct v4l2_subdev *sd, u32 reg, u32 value, u32 mask,
  * The available executing order between each modes are as follows:
  *   PARAMETER <---> MONITOR <---> CAPTURE
  */
-int m5mols_mode(struct m5mols_info *info, u8 mode);
+int m5mols_set_mode(struct m5mols_info *info, u8 mode);
 
 int m5mols_enable_interrupt(struct v4l2_subdev *sd, u8 reg);
 int m5mols_wait_interrupt(struct v4l2_subdev *sd, u8 condition, u32 timeout);
@@ -291,9 +302,33 @@ int m5mols_start_capture(struct m5mols_info *info);
 int m5mols_do_scenemode(struct m5mols_info *info, u8 mode);
 int m5mols_lock_3a(struct m5mols_info *info, bool lock);
 int m5mols_set_ctrl(struct v4l2_ctrl *ctrl);
+int m5mols_init_controls(struct v4l2_subdev *sd);
 
 /* The firmware function */
 int m5mols_update_fw(struct v4l2_subdev *sd,
 		     int (*set_power)(struct m5mols_info *, bool));
 
+static inline struct m5mols_info *to_m5mols(struct v4l2_subdev *subdev)
+{
+	return container_of(subdev, struct m5mols_info, sd);
+}
+
+static inline struct v4l2_subdev *to_sd(struct v4l2_ctrl *ctrl)
+{
+	struct m5mols_info *info = container_of(ctrl->handler,
+						struct m5mols_info, handle);
+	return &info->sd;
+}
+
+static inline void m5mols_set_ctrl_mode(struct v4l2_ctrl *ctrl,
+					unsigned int mode)
+{
+	ctrl->priv = (void *)mode;
+}
+
+static inline unsigned int m5mols_get_ctrl_mode(struct v4l2_ctrl *ctrl)
+{
+	return (unsigned int)ctrl->priv;
+}
+
 #endif	/* M5MOLS_H */
diff --git a/drivers/media/video/m5mols/m5mols_capture.c b/drivers/media/video/m5mols/m5mols_capture.c
index ba25e8e2ba4..cb243bd278c 100644
--- a/drivers/media/video/m5mols/m5mols_capture.c
+++ b/drivers/media/video/m5mols/m5mols_capture.c
@@ -106,7 +106,6 @@ static int m5mols_capture_info(struct m5mols_info *info)
 int m5mols_start_capture(struct m5mols_info *info)
 {
 	struct v4l2_subdev *sd = &info->sd;
-	u8 resolution = info->resolution;
 	int ret;
 
 	/*
@@ -114,22 +113,18 @@ int m5mols_start_capture(struct m5mols_info *info)
 	 * format. The frame capture is initiated during switching from Monitor
 	 * to Capture mode.
 	 */
-	ret = m5mols_mode(info, REG_MONITOR);
+	ret = m5mols_set_mode(info, REG_MONITOR);
 	if (!ret)
 		ret = m5mols_restore_controls(info);
 	if (!ret)
 		ret = m5mols_write(sd, CAPP_YUVOUT_MAIN, REG_JPEG);
 	if (!ret)
-		ret = m5mols_write(sd, CAPP_MAIN_IMAGE_SIZE, resolution);
+		ret = m5mols_write(sd, CAPP_MAIN_IMAGE_SIZE, info->resolution);
 	if (!ret)
-		ret = m5mols_lock_3a(info, true);
-	if (!ret)
-		ret = m5mols_mode(info, REG_CAPTURE);
+		ret = m5mols_set_mode(info, REG_CAPTURE);
 	if (!ret)
 		/* Wait until a frame is captured to ISP internal memory */
 		ret = m5mols_wait_interrupt(sd, REG_INT_CAPTURE, 2000);
-	if (!ret)
-		ret = m5mols_lock_3a(info, false);
 	if (ret)
 		return ret;
 
diff --git a/drivers/media/video/m5mols/m5mols_controls.c b/drivers/media/video/m5mols/m5mols_controls.c
index d135d20d09c..392a028730e 100644
--- a/drivers/media/video/m5mols/m5mols_controls.c
+++ b/drivers/media/video/m5mols/m5mols_controls.c
@@ -139,7 +139,7 @@ int m5mols_do_scenemode(struct m5mols_info *info, u8 mode)
 	if (mode > REG_SCENE_CANDLE)
 		return -EINVAL;
 
-	ret = m5mols_lock_3a(info, false);
+	ret = v4l2_ctrl_s_ctrl(info->lock_3a, 0);
 	if (!ret)
 		ret = m5mols_write(sd, AE_EV_PRESET_MONITOR, mode);
 	if (!ret)
@@ -169,7 +169,7 @@ int m5mols_do_scenemode(struct m5mols_info *info, u8 mode)
 	if (!ret)
 		ret = m5mols_write(sd, AE_ISO, scenemode.iso);
 	if (!ret)
-		ret = m5mols_mode(info, REG_CAPTURE);
+		ret = m5mols_set_mode(info, REG_CAPTURE);
 	if (!ret)
 		ret = m5mols_write(sd, CAPP_WDR_EN, scenemode.wdr);
 	if (!ret)
@@ -181,119 +181,448 @@ int m5mols_do_scenemode(struct m5mols_info *info, u8 mode)
 	if (!ret)
 		ret = m5mols_write(sd, CAPC_MODE, scenemode.capt_mode);
 	if (!ret)
-		ret = m5mols_mode(info, REG_MONITOR);
+		ret = m5mols_set_mode(info, REG_MONITOR);
 
 	return ret;
 }
 
-static int m5mols_lock_ae(struct m5mols_info *info, bool lock)
+static int m5mols_3a_lock(struct m5mols_info *info, struct v4l2_ctrl *ctrl)
 {
+	bool af_lock = ctrl->val & V4L2_LOCK_FOCUS;
 	int ret = 0;
 
-	if (info->lock_ae != lock)
-		ret = m5mols_write(&info->sd, AE_LOCK,
-				lock ? REG_AE_LOCK : REG_AE_UNLOCK);
-	if (!ret)
-		info->lock_ae = lock;
+	if ((ctrl->val ^ ctrl->cur.val) & V4L2_LOCK_EXPOSURE) {
+		bool ae_lock = ctrl->val & V4L2_LOCK_EXPOSURE;
+
+		ret = m5mols_write(&info->sd, AE_LOCK, ae_lock ?
+				   REG_AE_LOCK : REG_AE_UNLOCK);
+		if (ret)
+			return ret;
+	}
+
+	if (((ctrl->val ^ ctrl->cur.val) & V4L2_LOCK_WHITE_BALANCE)
+	    && info->auto_wb->val) {
+		bool awb_lock = ctrl->val & V4L2_LOCK_WHITE_BALANCE;
+
+		ret = m5mols_write(&info->sd, AWB_LOCK, awb_lock ?
+				   REG_AWB_LOCK : REG_AWB_UNLOCK);
+		if (ret)
+			return ret;
+	}
+
+	if (!info->ver.af || !af_lock)
+		return ret;
+
+	if ((ctrl->val ^ ctrl->cur.val) & V4L2_LOCK_FOCUS)
+		ret = m5mols_write(&info->sd, AF_EXECUTE, REG_AF_STOP);
+
+	return ret;
+}
+
+static int m5mols_set_metering_mode(struct m5mols_info *info, int mode)
+{
+	unsigned int metering;
+
+	switch (mode) {
+	case V4L2_EXPOSURE_METERING_CENTER_WEIGHTED:
+		metering = REG_AE_CENTER;
+		break;
+	case V4L2_EXPOSURE_METERING_SPOT:
+		metering = REG_AE_SPOT;
+		break;
+	default:
+		metering = REG_AE_ALL;
+		break;
+	}
+
+	return m5mols_write(&info->sd, AE_MODE, metering);
+}
+
+static int m5mols_set_exposure(struct m5mols_info *info, int exposure)
+{
+	struct v4l2_subdev *sd = &info->sd;
+	int ret = 0;
+
+	if (exposure == V4L2_EXPOSURE_AUTO) {
+		/* Unlock auto exposure */
+		info->lock_3a->val &= ~V4L2_LOCK_EXPOSURE;
+		m5mols_3a_lock(info, info->lock_3a);
+
+		ret = m5mols_set_metering_mode(info, info->metering->val);
+		if (ret < 0)
+			return ret;
+
+		v4l2_dbg(1, m5mols_debug, sd,
+			 "%s: exposure bias: %#x, metering: %#x\n",
+			 __func__, info->exposure_bias->val,
+			 info->metering->val);
+
+		return m5mols_write(sd, AE_INDEX, info->exposure_bias->val);
+	}
+
+	if (exposure == V4L2_EXPOSURE_MANUAL) {
+		ret = m5mols_write(sd, AE_MODE, REG_AE_OFF);
+		if (ret == 0)
+			ret = m5mols_write(sd, AE_MAN_GAIN_MON,
+					   info->exposure->val);
+		if (ret == 0)
+			ret = m5mols_write(sd, AE_MAN_GAIN_CAP,
+					   info->exposure->val);
+
+		v4l2_dbg(1, m5mols_debug, sd, "%s: exposure: %#x\n",
+			 __func__, info->exposure->val);
+	}
+
+	return ret;
+}
+
+static int m5mols_set_white_balance(struct m5mols_info *info, int val)
+{
+	static const unsigned short wb[][2] = {
+		{ V4L2_WHITE_BALANCE_INCANDESCENT,  REG_AWB_INCANDESCENT },
+		{ V4L2_WHITE_BALANCE_FLUORESCENT,   REG_AWB_FLUORESCENT_1 },
+		{ V4L2_WHITE_BALANCE_FLUORESCENT_H, REG_AWB_FLUORESCENT_2 },
+		{ V4L2_WHITE_BALANCE_HORIZON,       REG_AWB_HORIZON },
+		{ V4L2_WHITE_BALANCE_DAYLIGHT,      REG_AWB_DAYLIGHT },
+		{ V4L2_WHITE_BALANCE_FLASH,         REG_AWB_LEDLIGHT },
+		{ V4L2_WHITE_BALANCE_CLOUDY,        REG_AWB_CLOUDY },
+		{ V4L2_WHITE_BALANCE_SHADE,         REG_AWB_SHADE },
+		{ V4L2_WHITE_BALANCE_AUTO,          REG_AWB_AUTO },
+	};
+	int i;
+	struct v4l2_subdev *sd = &info->sd;
+	int ret = -EINVAL;
+
+	for (i = 0; i < ARRAY_SIZE(wb); i++) {
+		int awb;
+		if (wb[i][0] != val)
+			continue;
+
+		v4l2_dbg(1, m5mols_debug, sd,
+			 "Setting white balance to: %#x\n", wb[i][0]);
+
+		awb = wb[i][0] == V4L2_WHITE_BALANCE_AUTO;
+		ret = m5mols_write(sd, AWB_MODE, awb ? REG_AWB_AUTO :
+						 REG_AWB_PRESET);
+		if (ret < 0)
+			return ret;
+
+		if (!awb)
+			ret = m5mols_write(sd, AWB_MANUAL, wb[i][1]);
+	}
 
 	return ret;
 }
 
-static int m5mols_lock_awb(struct m5mols_info *info, bool lock)
+static int m5mols_set_saturation(struct m5mols_info *info, int val)
+{
+	int ret = m5mols_write(&info->sd, MON_CHROMA_LVL, val);
+	if (ret < 0)
+		return ret;
+
+	return m5mols_write(&info->sd, MON_CHROMA_EN, REG_CHROMA_ON);
+}
+
+static int m5mols_set_color_effect(struct m5mols_info *info, int val)
 {
+	unsigned int m_effect = REG_COLOR_EFFECT_OFF;
+	unsigned int p_effect = REG_EFFECT_OFF;
+	unsigned int cfix_r = 0, cfix_b = 0;
+	struct v4l2_subdev *sd = &info->sd;
 	int ret = 0;
 
-	if (info->lock_awb != lock)
-		ret = m5mols_write(&info->sd, AWB_LOCK,
-				lock ? REG_AWB_LOCK : REG_AWB_UNLOCK);
+	switch (val) {
+	case V4L2_COLORFX_BW:
+		m_effect = REG_COLOR_EFFECT_ON;
+		break;
+	case V4L2_COLORFX_NEGATIVE:
+		p_effect = REG_EFFECT_NEGA;
+		break;
+	case V4L2_COLORFX_EMBOSS:
+		p_effect = REG_EFFECT_EMBOSS;
+		break;
+	case V4L2_COLORFX_SEPIA:
+		m_effect = REG_COLOR_EFFECT_ON;
+		cfix_r = REG_CFIXR_SEPIA;
+		cfix_b = REG_CFIXB_SEPIA;
+		break;
+	}
+
+	ret = m5mols_write(sd, PARM_EFFECT, p_effect);
 	if (!ret)
-		info->lock_awb = lock;
+		ret = m5mols_write(sd, MON_EFFECT, m_effect);
+
+	if (ret == 0 && m_effect == REG_COLOR_EFFECT_ON) {
+		ret = m5mols_write(sd, MON_CFIXR, cfix_r);
+		if (!ret)
+			ret = m5mols_write(sd, MON_CFIXB, cfix_b);
+	}
+
+	v4l2_dbg(1, m5mols_debug, sd,
+		 "p_effect: %#x, m_effect: %#x, r: %#x, b: %#x (%d)\n",
+		 p_effect, m_effect, cfix_r, cfix_b, ret);
 
 	return ret;
 }
 
-/* m5mols_lock_3a() - Lock 3A(Auto Exposure, Auto Whitebalance, Auto Focus) */
-int m5mols_lock_3a(struct m5mols_info *info, bool lock)
+static int m5mols_set_iso(struct m5mols_info *info, int auto_iso)
+{
+	u32 iso = auto_iso ? 0 : info->iso->val + 1;
+
+	return m5mols_write(&info->sd, AE_ISO, iso);
+}
+
+static int m5mols_set_wdr(struct m5mols_info *info, int wdr)
 {
 	int ret;
 
-	ret = m5mols_lock_ae(info, lock);
-	if (!ret)
-		ret = m5mols_lock_awb(info, lock);
-	/* Don't need to handle unlocking AF */
-	if (!ret && is_available_af(info) && lock)
-		ret = m5mols_write(&info->sd, AF_EXECUTE, REG_AF_STOP);
+	ret = m5mols_write(&info->sd, MON_TONE_CTL, wdr ? 9 : 5);
+	if (ret < 0)
+		return ret;
+
+	ret = m5mols_set_mode(info, REG_CAPTURE);
+	if (ret < 0)
+		return ret;
+
+	return m5mols_write(&info->sd, CAPP_WDR_EN, wdr);
+}
+
+static int m5mols_set_stabilization(struct m5mols_info *info, int val)
+{
+	struct v4l2_subdev *sd = &info->sd;
+	unsigned int evp = val ? 0xe : 0x0;
+	int ret;
+
+	ret = m5mols_write(sd, AE_EV_PRESET_MONITOR, evp);
+	if (ret < 0)
+		return ret;
+
+	return m5mols_write(sd, AE_EV_PRESET_CAPTURE, evp);
+}
+
+static int m5mols_g_volatile_ctrl(struct v4l2_ctrl *ctrl)
+{
+	struct v4l2_subdev *sd = to_sd(ctrl);
+	struct m5mols_info *info = to_m5mols(sd);
+	int ret = 0;
+	u8 status;
+
+	v4l2_dbg(1, m5mols_debug, sd, "%s: ctrl: %s (%d)\n",
+		 __func__, ctrl->name, info->isp_ready);
+
+	if (!info->isp_ready)
+		return -EBUSY;
+
+	switch (ctrl->id) {
+	case V4L2_CID_ISO_SENSITIVITY_AUTO:
+		ret = m5mols_read_u8(sd, AE_ISO, &status);
+		if (ret == 0)
+			ctrl->val = !status;
+		if (status != REG_ISO_AUTO)
+			info->iso->val = status - 1;
+		break;
+
+	case V4L2_CID_3A_LOCK:
+		ctrl->val &= ~0x7;
+
+		ret = m5mols_read_u8(sd, AE_LOCK, &status);
+		if (ret)
+			return ret;
+		if (status)
+			info->lock_3a->val |= V4L2_LOCK_EXPOSURE;
+
+		ret = m5mols_read_u8(sd, AWB_LOCK, &status);
+		if (ret)
+			return ret;
+		if (status)
+			info->lock_3a->val |= V4L2_LOCK_EXPOSURE;
+
+		ret = m5mols_read_u8(sd, AF_EXECUTE, &status);
+		if (!status)
+			info->lock_3a->val |= V4L2_LOCK_EXPOSURE;
+		break;
+	}
 
 	return ret;
 }
 
-/* m5mols_set_ctrl() - The main s_ctrl function called by m5mols_set_ctrl() */
-int m5mols_set_ctrl(struct v4l2_ctrl *ctrl)
+static int m5mols_s_ctrl(struct v4l2_ctrl *ctrl)
 {
+	unsigned int ctrl_mode = m5mols_get_ctrl_mode(ctrl);
 	struct v4l2_subdev *sd = to_sd(ctrl);
 	struct m5mols_info *info = to_m5mols(sd);
-	int ret;
+	int last_mode = info->mode;
+	int ret = 0;
+
+	/*
+	 * If needed, defer restoring the controls until
+	 * the device is fully initialized.
+	 */
+	if (!info->isp_ready) {
+		info->ctrl_sync = 0;
+		return 0;
+	}
+
+	v4l2_dbg(1, m5mols_debug, sd, "%s: %s, val: %d, priv: %#x\n",
+		 __func__, ctrl->name, ctrl->val, (int)ctrl->priv);
+
+	if (ctrl_mode && ctrl_mode != info->mode) {
+		ret = m5mols_set_mode(info, ctrl_mode);
+		if (ret < 0)
+			return ret;
+	}
 
 	switch (ctrl->id) {
+	case V4L2_CID_3A_LOCK:
+		ret = m5mols_3a_lock(info, ctrl);
+		break;
+
 	case V4L2_CID_ZOOM_ABSOLUTE:
-		return m5mols_write(sd, MON_ZOOM, ctrl->val);
+		ret = m5mols_write(sd, MON_ZOOM, ctrl->val);
+		break;
 
 	case V4L2_CID_EXPOSURE_AUTO:
-		ret = m5mols_lock_ae(info,
-			ctrl->val == V4L2_EXPOSURE_AUTO ? false : true);
-		if (!ret && ctrl->val == V4L2_EXPOSURE_AUTO)
-			ret = m5mols_write(sd, AE_MODE, REG_AE_ALL);
-		if (!ret && ctrl->val == V4L2_EXPOSURE_MANUAL) {
-			int val = info->exposure->val;
-			ret = m5mols_write(sd, AE_MODE, REG_AE_OFF);
-			if (!ret)
-				ret = m5mols_write(sd, AE_MAN_GAIN_MON, val);
-			if (!ret)
-				ret = m5mols_write(sd, AE_MAN_GAIN_CAP, val);
-		}
-		return ret;
+		ret = m5mols_set_exposure(info, ctrl->val);
+		break;
 
-	case V4L2_CID_AUTO_WHITE_BALANCE:
-		ret = m5mols_lock_awb(info, ctrl->val ? false : true);
-		if (!ret)
-			ret = m5mols_write(sd, AWB_MODE, ctrl->val ?
-				REG_AWB_AUTO : REG_AWB_PRESET);
-		return ret;
+	case V4L2_CID_ISO_SENSITIVITY:
+		ret = m5mols_set_iso(info, ctrl->val);
+		break;
+
+	case V4L2_CID_AUTO_N_PRESET_WHITE_BALANCE:
+		ret = m5mols_set_white_balance(info, ctrl->val);
+		break;
 
 	case V4L2_CID_SATURATION:
-		ret = m5mols_write(sd, MON_CHROMA_LVL, ctrl->val);
-		if (!ret)
-			ret = m5mols_write(sd, MON_CHROMA_EN, REG_CHROMA_ON);
-		return ret;
+		ret = m5mols_set_saturation(info, ctrl->val);
+		break;
 
 	case V4L2_CID_COLORFX:
-		/*
-		 * This control uses two kinds of registers: normal & color.
-		 * The normal effect belongs to category 1, while the color
-		 * one belongs to category 2.
-		 *
-		 * The normal effect uses one register: CAT1_EFFECT.
-		 * The color effect uses three registers:
-		 * CAT2_COLOR_EFFECT, CAT2_CFIXR, CAT2_CFIXB.
-		 */
-		ret = m5mols_write(sd, PARM_EFFECT,
-			ctrl->val == V4L2_COLORFX_NEGATIVE ? REG_EFFECT_NEGA :
-			ctrl->val == V4L2_COLORFX_EMBOSS ? REG_EFFECT_EMBOSS :
-			REG_EFFECT_OFF);
-		if (!ret)
-			ret = m5mols_write(sd, MON_EFFECT,
-				ctrl->val == V4L2_COLORFX_SEPIA ?
-				REG_COLOR_EFFECT_ON : REG_COLOR_EFFECT_OFF);
-		if (!ret)
-			ret = m5mols_write(sd, MON_CFIXR,
-				ctrl->val == V4L2_COLORFX_SEPIA ?
-				REG_CFIXR_SEPIA : 0);
-		if (!ret)
-			ret = m5mols_write(sd, MON_CFIXB,
-				ctrl->val == V4L2_COLORFX_SEPIA ?
-				REG_CFIXB_SEPIA : 0);
+		ret = m5mols_set_color_effect(info, ctrl->val);
+		break;
+
+	case V4L2_CID_WIDE_DYNAMIC_RANGE:
+		ret = m5mols_set_wdr(info, ctrl->val);
+		break;
+
+	case V4L2_CID_IMAGE_STABILIZATION:
+		ret = m5mols_set_stabilization(info, ctrl->val);
+		break;
+
+	case V4L2_CID_JPEG_COMPRESSION_QUALITY:
+		ret = m5mols_write(sd, CAPP_JPEG_RATIO, ctrl->val);
+		break;
+	}
+
+	if (ret == 0 && info->mode != last_mode)
+		ret = m5mols_set_mode(info, last_mode);
+
+	return ret;
+}
+
+static const struct v4l2_ctrl_ops m5mols_ctrl_ops = {
+	.g_volatile_ctrl	= m5mols_g_volatile_ctrl,
+	.s_ctrl			= m5mols_s_ctrl,
+};
+
+/* Supported manual ISO values */
+static const s64 iso_qmenu[] = {
+	/* AE_ISO: 0x01...0x07 */
+	50, 100, 200, 400, 800, 1600, 3200
+};
+
+/* Supported Exposure Bias values, -2.0EV...+2.0EV */
+static const s64 ev_bias_qmenu[] = {
+	/* AE_INDEX: 0x00...0x08 */
+	-2000, -1500, -1000, -500, 0, 500, 1000, 1500, 2000
+};
+
+int m5mols_init_controls(struct v4l2_subdev *sd)
+{
+	struct m5mols_info *info = to_m5mols(sd);
+	u16 exposure_max;
+	u16 zoom_step;
+	int ret;
+
+	/* Determine the firmware dependant control range and step values */
+	ret = m5mols_read_u16(sd, AE_MAX_GAIN_MON, &exposure_max);
+	if (ret < 0)
+		return ret;
+
+	zoom_step = is_manufacturer(info, REG_SAMSUNG_OPTICS) ? 31 : 1;
+	v4l2_ctrl_handler_init(&info->handle, 20);
+
+	info->auto_wb = v4l2_ctrl_new_std_menu(&info->handle,
+			&m5mols_ctrl_ops, V4L2_CID_AUTO_N_PRESET_WHITE_BALANCE,
+			9, ~0x3fe, V4L2_WHITE_BALANCE_AUTO);
+
+	/* Exposure control cluster */
+	info->auto_exposure = v4l2_ctrl_new_std_menu(&info->handle,
+			&m5mols_ctrl_ops, V4L2_CID_EXPOSURE_AUTO,
+			1, ~0x03, V4L2_EXPOSURE_AUTO);
+
+	info->exposure = v4l2_ctrl_new_std(&info->handle,
+			&m5mols_ctrl_ops, V4L2_CID_EXPOSURE,
+			0, exposure_max, 1, exposure_max / 2);
+
+	info->exposure_bias = v4l2_ctrl_new_int_menu(&info->handle,
+			&m5mols_ctrl_ops, V4L2_CID_AUTO_EXPOSURE_BIAS,
+			ARRAY_SIZE(ev_bias_qmenu) - 1,
+			ARRAY_SIZE(ev_bias_qmenu)/2 - 1,
+			ev_bias_qmenu);
+
+	info->metering = v4l2_ctrl_new_std_menu(&info->handle,
+			&m5mols_ctrl_ops, V4L2_CID_EXPOSURE_METERING,
+			2, ~0x7, V4L2_EXPOSURE_METERING_AVERAGE);
+
+	/* ISO control cluster */
+	info->auto_iso = v4l2_ctrl_new_std_menu(&info->handle, &m5mols_ctrl_ops,
+			V4L2_CID_ISO_SENSITIVITY_AUTO, 1, ~0x03, 1);
+
+	info->iso = v4l2_ctrl_new_int_menu(&info->handle, &m5mols_ctrl_ops,
+			V4L2_CID_ISO_SENSITIVITY, ARRAY_SIZE(iso_qmenu) - 1,
+			ARRAY_SIZE(iso_qmenu)/2 - 1, iso_qmenu);
+
+	info->saturation = v4l2_ctrl_new_std(&info->handle, &m5mols_ctrl_ops,
+			V4L2_CID_SATURATION, 1, 5, 1, 3);
+
+	info->zoom = v4l2_ctrl_new_std(&info->handle, &m5mols_ctrl_ops,
+			V4L2_CID_ZOOM_ABSOLUTE, 1, 70, zoom_step, 1);
+
+	info->colorfx = v4l2_ctrl_new_std_menu(&info->handle, &m5mols_ctrl_ops,
+			V4L2_CID_COLORFX, 4, 0, V4L2_COLORFX_NONE);
+
+	info->wdr = v4l2_ctrl_new_std(&info->handle, &m5mols_ctrl_ops,
+			V4L2_CID_WIDE_DYNAMIC_RANGE, 0, 1, 1, 0);
+
+	info->stabilization = v4l2_ctrl_new_std(&info->handle, &m5mols_ctrl_ops,
+			V4L2_CID_IMAGE_STABILIZATION, 0, 1, 1, 0);
+
+	info->jpeg_quality = v4l2_ctrl_new_std(&info->handle, &m5mols_ctrl_ops,
+			V4L2_CID_JPEG_COMPRESSION_QUALITY, 1, 100, 1, 80);
+
+	info->lock_3a = v4l2_ctrl_new_std(&info->handle, &m5mols_ctrl_ops,
+			V4L2_CID_3A_LOCK, 0, 0x7, 0, 0);
+
+	if (info->handle.error) {
+		int ret = info->handle.error;
+		v4l2_err(sd, "Failed to initialize controls: %d\n", ret);
+		v4l2_ctrl_handler_free(&info->handle);
 		return ret;
 	}
 
-	return -EINVAL;
+	v4l2_ctrl_auto_cluster(4, &info->auto_exposure, 1, false);
+	info->auto_iso->flags |= V4L2_CTRL_FLAG_VOLATILE |
+				V4L2_CTRL_FLAG_UPDATE;
+	v4l2_ctrl_auto_cluster(2, &info->auto_iso, 0, false);
+
+	info->lock_3a->flags |= V4L2_CTRL_FLAG_VOLATILE;
+
+	m5mols_set_ctrl_mode(info->auto_exposure, REG_PARAMETER);
+	m5mols_set_ctrl_mode(info->auto_wb, REG_PARAMETER);
+	m5mols_set_ctrl_mode(info->colorfx, REG_MONITOR);
+
+	sd->ctrl_handler = &info->handle;
+
+	return 0;
 }
diff --git a/drivers/media/video/m5mols/m5mols_core.c b/drivers/media/video/m5mols/m5mols_core.c
index d718aee01c7..ac7d28b6ddf 100644
--- a/drivers/media/video/m5mols/m5mols_core.c
+++ b/drivers/media/video/m5mols/m5mols_core.c
@@ -362,14 +362,14 @@ static int m5mols_reg_mode(struct v4l2_subdev *sd, u8 mode)
 }
 
 /**
- * m5mols_mode - manage the M-5MOLS's mode
+ * m5mols_set_mode - set the M-5MOLS controller mode
  * @mode: the required operation mode
  *
  * The commands of M-5MOLS are grouped into specific modes. Each functionality
- * can be guaranteed only when the sensor is operating in mode which which
- * a command belongs to.
+ * can be guaranteed only when the sensor is operating in mode which a command
+ * belongs to.
  */
-int m5mols_mode(struct m5mols_info *info, u8 mode)
+int m5mols_set_mode(struct m5mols_info *info, u8 mode)
 {
 	struct v4l2_subdev *sd = &info->sd;
 	int ret = -EINVAL;
@@ -645,13 +645,13 @@ static int m5mols_start_monitor(struct m5mols_info *info)
 	struct v4l2_subdev *sd = &info->sd;
 	int ret;
 
-	ret = m5mols_mode(info, REG_PARAMETER);
+	ret = m5mols_set_mode(info, REG_PARAMETER);
 	if (!ret)
 		ret = m5mols_write(sd, PARM_MON_SIZE, info->resolution);
 	if (!ret)
 		ret = m5mols_write(sd, PARM_MON_FPS, REG_FPS_30);
 	if (!ret)
-		ret = m5mols_mode(info, REG_MONITOR);
+		ret = m5mols_set_mode(info, REG_MONITOR);
 	if (!ret)
 		ret = m5mols_restore_controls(info);
 
@@ -674,42 +674,13 @@ static int m5mols_s_stream(struct v4l2_subdev *sd, int enable)
 		return ret;
 	}
 
-	return m5mols_mode(info, REG_PARAMETER);
+	return m5mols_set_mode(info, REG_PARAMETER);
 }
 
 static const struct v4l2_subdev_video_ops m5mols_video_ops = {
 	.s_stream	= m5mols_s_stream,
 };
 
-static int m5mols_s_ctrl(struct v4l2_ctrl *ctrl)
-{
-	struct v4l2_subdev *sd = to_sd(ctrl);
-	struct m5mols_info *info = to_m5mols(sd);
-	int ispstate = info->mode;
-	int ret;
-
-	/*
-	 * If needed, defer restoring the controls until
-	 * the device is fully initialized.
-	 */
-	if (!info->isp_ready) {
-		info->ctrl_sync = 0;
-		return 0;
-	}
-
-	ret = m5mols_mode(info, REG_PARAMETER);
-	if (ret < 0)
-		return ret;
-	ret = m5mols_set_ctrl(ctrl);
-	if (ret < 0)
-		return ret;
-	return m5mols_mode(info, ispstate);
-}
-
-static const struct v4l2_ctrl_ops m5mols_ctrl_ops = {
-	.s_ctrl	= m5mols_s_ctrl,
-};
-
 static int m5mols_sensor_power(struct m5mols_info *info, bool enable)
 {
 	struct v4l2_subdev *sd = &info->sd;
@@ -802,52 +773,6 @@ static int m5mols_fw_start(struct v4l2_subdev *sd)
 	return ret;
 }
 
-static int m5mols_init_controls(struct m5mols_info *info)
-{
-	struct v4l2_subdev *sd = &info->sd;
-	u16 max_exposure;
-	u16 step_zoom;
-	int ret;
-
-	/* Determine value's range & step of controls for various FW version */
-	ret = m5mols_read_u16(sd, AE_MAX_GAIN_MON, &max_exposure);
-	if (!ret)
-		step_zoom = is_manufacturer(info, REG_SAMSUNG_OPTICS) ? 31 : 1;
-	if (ret)
-		return ret;
-
-	v4l2_ctrl_handler_init(&info->handle, 6);
-	info->autowb = v4l2_ctrl_new_std(&info->handle,
-			&m5mols_ctrl_ops, V4L2_CID_AUTO_WHITE_BALANCE,
-			0, 1, 1, 0);
-	info->saturation = v4l2_ctrl_new_std(&info->handle,
-			&m5mols_ctrl_ops, V4L2_CID_SATURATION,
-			1, 5, 1, 3);
-	info->zoom = v4l2_ctrl_new_std(&info->handle,
-			&m5mols_ctrl_ops, V4L2_CID_ZOOM_ABSOLUTE,
-			1, 70, step_zoom, 1);
-	info->exposure = v4l2_ctrl_new_std(&info->handle,
-			&m5mols_ctrl_ops, V4L2_CID_EXPOSURE,
-			0, max_exposure, 1, (int)max_exposure/2);
-	info->colorfx = v4l2_ctrl_new_std_menu(&info->handle,
-			&m5mols_ctrl_ops, V4L2_CID_COLORFX,
-			4, (1 << V4L2_COLORFX_BW), V4L2_COLORFX_NONE);
-	info->autoexposure = v4l2_ctrl_new_std_menu(&info->handle,
-			&m5mols_ctrl_ops, V4L2_CID_EXPOSURE_AUTO,
-			1, 0, V4L2_EXPOSURE_AUTO);
-
-	sd->ctrl_handler = &info->handle;
-	if (info->handle.error) {
-		v4l2_err(sd, "Failed to initialize controls: %d\n", ret);
-		v4l2_ctrl_handler_free(&info->handle);
-		return info->handle.error;
-	}
-
-	v4l2_ctrl_cluster(2, &info->autoexposure);
-
-	return 0;
-}
-
 /**
  * m5mols_s_power - Main sensor power control function
  *
@@ -868,7 +793,7 @@ static int m5mols_s_power(struct v4l2_subdev *sd, int on)
 	}
 
 	if (is_manufacturer(info, REG_SAMSUNG_TECHWIN)) {
-		ret = m5mols_mode(info, REG_MONITOR);
+		ret = m5mols_set_mode(info, REG_MONITOR);
 		if (!ret)
 			ret = m5mols_write(sd, AF_EXECUTE, REG_AF_STOP);
 		if (!ret)
@@ -1010,7 +935,7 @@ static int __devinit m5mols_probe(struct i2c_client *client,
 
 	ret = m5mols_fw_start(sd);
 	if (!ret)
-		ret = m5mols_init_controls(info);
+		ret = m5mols_init_controls(sd);
 
 	m5mols_sensor_power(info, false);
 	if (!ret)
diff --git a/drivers/media/video/m5mols/m5mols_reg.h b/drivers/media/video/m5mols/m5mols_reg.h
index ae4aced0f9b..14d4be72aef 100644
--- a/drivers/media/video/m5mols/m5mols_reg.h
+++ b/drivers/media/video/m5mols/m5mols_reg.h
@@ -310,6 +310,7 @@
 #define REG_JPEG		0x10
 
 #define CAPP_MAIN_IMAGE_SIZE	I2C_REG(CAT_CAPT_PARM, 0x01, 1)
+#define CAPP_JPEG_RATIO		I2C_REG(CAT_CAPT_PARM, 0x17, 1)
 
 #define CAPP_MCC_MODE		I2C_REG(CAT_CAPT_PARM, 0x1d, 1)
 #define REG_MCC_OFF		0x00
diff --git a/drivers/media/video/marvell-ccic/mcam-core.c b/drivers/media/video/marvell-ccic/mcam-core.c
index 996ac34d9a8..ce2b7b4788d 100644
--- a/drivers/media/video/marvell-ccic/mcam-core.c
+++ b/drivers/media/video/marvell-ccic/mcam-core.c
@@ -1356,7 +1356,6 @@ static int mcam_vidioc_s_fmt_vid_cap(struct file *filp, void *priv,
 			goto out;
 	}
 	mcam_set_config_needed(cam, 1);
-	ret = 0;
 out:
 	mutex_unlock(&cam->s_mutex);
 	return ret;
diff --git a/drivers/media/video/marvell-ccic/mmp-driver.c b/drivers/media/video/marvell-ccic/mmp-driver.c
index d23552323f4..c4c17fe76c0 100644
--- a/drivers/media/video/marvell-ccic/mmp-driver.c
+++ b/drivers/media/video/marvell-ccic/mmp-driver.c
@@ -181,7 +181,6 @@ static int mmpcam_probe(struct platform_device *pdev)
 	INIT_LIST_HEAD(&cam->devlist);
 
 	mcam = &cam->mcam;
-	mcam->platform = MHP_Armada610;
 	mcam->plat_power_up = mmpcam_power_up;
 	mcam->plat_power_down = mmpcam_power_down;
 	mcam->dev = &pdev->dev;
diff --git a/drivers/media/video/mem2mem_testdev.c b/drivers/media/video/mem2mem_testdev.c
index 12897e8a331..d2dec585e61 100644
--- a/drivers/media/video/mem2mem_testdev.c
+++ b/drivers/media/video/mem2mem_testdev.c
@@ -40,7 +40,7 @@ MODULE_VERSION("0.1.1");
 #define MIN_H 32
 #define MAX_W 640
 #define MAX_H 480
-#define DIM_ALIGN_MASK 0x08 /* 8-alignment for dimensions */
+#define DIM_ALIGN_MASK 7 /* 8-byte alignment for line length */
 
 /* Flags that indicate a format can be used for capture/output */
 #define MEM2MEM_CAPTURE	(1 << 0)
@@ -958,6 +958,10 @@ static int m2mtest_probe(struct platform_device *pdev)
 	}
 
 	*vfd = m2mtest_videodev;
+	/* Locking in file operations other than ioctl should be done
+	   by the driver, not the V4L2 core.
+	   This driver needs auditing so that this flag can be removed. */
+	set_bit(V4L2_FL_LOCK_ALL_FOPS, &vfd->flags);
 	vfd->lock = &dev->dev_mutex;
 
 	ret = video_register_device(vfd, VFL_TYPE_GRABBER, 0);
diff --git a/drivers/media/video/meye.c b/drivers/media/video/meye.c
index b09a3c80a15..7bc775219f9 100644
--- a/drivers/media/video/meye.c
+++ b/drivers/media/video/meye.c
@@ -1570,7 +1570,7 @@ static long vidioc_default(struct file *file, void *fh, bool valid_prio,
 		return meyeioc_stilljcapt((int *) arg);
 
 	default:
-		return -EINVAL;
+		return -ENOTTY;
 	}
 
 }
diff --git a/drivers/media/video/msp3400-driver.c b/drivers/media/video/msp3400-driver.c
index 82ce50721de..aeb22be7dcb 100644
--- a/drivers/media/video/msp3400-driver.c
+++ b/drivers/media/video/msp3400-driver.c
@@ -597,19 +597,23 @@ static int msp_log_status(struct v4l2_subdev *sd)
 	return 0;
 }
 
-static int msp_suspend(struct i2c_client *client, pm_message_t state)
+#ifdef CONFIG_PM_SLEEP
+static int msp_suspend(struct device *dev)
 {
+	struct i2c_client *client = to_i2c_client(dev);
 	v4l_dbg(1, msp_debug, client, "suspend\n");
 	msp_reset(client);
 	return 0;
 }
 
-static int msp_resume(struct i2c_client *client)
+static int msp_resume(struct device *dev)
 {
+	struct i2c_client *client = to_i2c_client(dev);
 	v4l_dbg(1, msp_debug, client, "resume\n");
 	msp_wake_thread(client);
 	return 0;
 }
+#endif
 
 /* ----------------------------------------------------------------------- */
 
@@ -863,6 +867,10 @@ static int msp_remove(struct i2c_client *client)
 
 /* ----------------------------------------------------------------------- */
 
+static const struct dev_pm_ops msp3400_pm_ops = {
+	SET_SYSTEM_SLEEP_PM_OPS(msp_suspend, msp_resume)
+};
+
 static const struct i2c_device_id msp_id[] = {
 	{ "msp3400", 0 },
 	{ }
@@ -873,11 +881,10 @@ static struct i2c_driver msp_driver = {
 	.driver = {
 		.owner	= THIS_MODULE,
 		.name	= "msp3400",
+		.pm	= &msp3400_pm_ops,
 	},
 	.probe		= msp_probe,
 	.remove		= msp_remove,
-	.suspend	= msp_suspend,
-	.resume		= msp_resume,
 	.id_table	= msp_id,
 };
 
diff --git a/drivers/media/video/mt9m032.c b/drivers/media/video/mt9m032.c
index 645973c5feb..3c1e626139b 100644
--- a/drivers/media/video/mt9m032.c
+++ b/drivers/media/video/mt9m032.c
@@ -838,9 +838,9 @@ static int mt9m032_remove(struct i2c_client *client)
 	struct v4l2_subdev *subdev = i2c_get_clientdata(client);
 	struct mt9m032 *sensor = to_mt9m032(subdev);
 
-	v4l2_device_unregister_subdev(&sensor->subdev);
+	v4l2_device_unregister_subdev(subdev);
 	v4l2_ctrl_handler_free(&sensor->ctrls);
-	media_entity_cleanup(&sensor->subdev.entity);
+	media_entity_cleanup(&subdev->entity);
 	mutex_destroy(&sensor->lock);
 	kfree(sensor);
 	return 0;
diff --git a/drivers/media/video/mt9p031.c b/drivers/media/video/mt9p031.c
index c81eaf4fbe0..8f061d9ac44 100644
--- a/drivers/media/video/mt9p031.c
+++ b/drivers/media/video/mt9p031.c
@@ -14,6 +14,7 @@
 
 #include <linux/delay.h>
 #include <linux/device.h>
+#include <linux/gpio.h>
 #include <linux/module.h>
 #include <linux/i2c.h>
 #include <linux/log2.h>
@@ -90,7 +91,14 @@
 #define		MT9P031_GLOBAL_GAIN_MAX			1024
 #define		MT9P031_GLOBAL_GAIN_DEF			8
 #define		MT9P031_GLOBAL_GAIN_MULT		(1 << 6)
+#define MT9P031_ROW_BLACK_TARGET			0x49
 #define MT9P031_ROW_BLACK_DEF_OFFSET			0x4b
+#define MT9P031_GREEN1_OFFSET				0x60
+#define MT9P031_GREEN2_OFFSET				0x61
+#define MT9P031_BLACK_LEVEL_CALIBRATION			0x62
+#define		MT9P031_BLC_MANUAL_BLC			(1 << 0)
+#define MT9P031_RED_OFFSET				0x63
+#define MT9P031_BLUE_OFFSET				0x64
 #define MT9P031_TEST_PATTERN				0xa0
 #define		MT9P031_TEST_PATTERN_SHIFT		3
 #define		MT9P031_TEST_PATTERN_ENABLE		(1 << 0)
@@ -99,17 +107,27 @@
 #define MT9P031_TEST_PATTERN_RED			0xa2
 #define MT9P031_TEST_PATTERN_BLUE			0xa3
 
+enum mt9p031_model {
+	MT9P031_MODEL_COLOR,
+	MT9P031_MODEL_MONOCHROME,
+};
+
 struct mt9p031 {
 	struct v4l2_subdev subdev;
 	struct media_pad pad;
 	struct v4l2_rect crop;  /* Sensor window */
 	struct v4l2_mbus_framefmt format;
-	struct v4l2_ctrl_handler ctrls;
 	struct mt9p031_platform_data *pdata;
 	struct mutex power_lock; /* lock to protect power_count */
 	int power_count;
 
+	enum mt9p031_model model;
 	struct aptina_pll pll;
+	int reset;
+
+	struct v4l2_ctrl_handler ctrls;
+	struct v4l2_ctrl *blc_auto;
+	struct v4l2_ctrl *blc_offset;
 
 	/* Registers cache */
 	u16 output_control;
@@ -241,8 +259,8 @@ static inline int mt9p031_pll_disable(struct mt9p031 *mt9p031)
 static int mt9p031_power_on(struct mt9p031 *mt9p031)
 {
 	/* Ensure RESET_BAR is low */
-	if (mt9p031->pdata->reset) {
-		mt9p031->pdata->reset(&mt9p031->subdev, 1);
+	if (mt9p031->reset != -1) {
+		gpio_set_value(mt9p031->reset, 0);
 		usleep_range(1000, 2000);
 	}
 
@@ -252,8 +270,8 @@ static int mt9p031_power_on(struct mt9p031 *mt9p031)
 					 mt9p031->pdata->ext_freq);
 
 	/* Now RESET_BAR must be high */
-	if (mt9p031->pdata->reset) {
-		mt9p031->pdata->reset(&mt9p031->subdev, 0);
+	if (mt9p031->reset != -1) {
+		gpio_set_value(mt9p031->reset, 1);
 		usleep_range(1000, 2000);
 	}
 
@@ -262,8 +280,8 @@ static int mt9p031_power_on(struct mt9p031 *mt9p031)
 
 static void mt9p031_power_off(struct mt9p031 *mt9p031)
 {
-	if (mt9p031->pdata->reset) {
-		mt9p031->pdata->reset(&mt9p031->subdev, 1);
+	if (mt9p031->reset != -1) {
+		gpio_set_value(mt9p031->reset, 0);
 		usleep_range(1000, 2000);
 	}
 
@@ -557,6 +575,10 @@ static int mt9p031_set_crop(struct v4l2_subdev *subdev,
  */
 
 #define V4L2_CID_TEST_PATTERN		(V4L2_CID_USER_BASE | 0x1001)
+#define V4L2_CID_BLC_AUTO		(V4L2_CID_USER_BASE | 0x1002)
+#define V4L2_CID_BLC_TARGET_LEVEL	(V4L2_CID_USER_BASE | 0x1003)
+#define V4L2_CID_BLC_ANALOG_OFFSET	(V4L2_CID_USER_BASE | 0x1004)
+#define V4L2_CID_BLC_DIGITAL_OFFSET	(V4L2_CID_USER_BASE | 0x1005)
 
 static int mt9p031_s_ctrl(struct v4l2_ctrl *ctrl)
 {
@@ -621,11 +643,17 @@ static int mt9p031_s_ctrl(struct v4l2_ctrl *ctrl)
 
 	case V4L2_CID_TEST_PATTERN:
 		if (!ctrl->val) {
-			ret = mt9p031_set_mode2(mt9p031,
-					0, MT9P031_READ_MODE_2_ROW_BLC);
-			if (ret < 0)
-				return ret;
-
+			/* Restore the black level compensation settings. */
+			if (mt9p031->blc_auto->cur.val != 0) {
+				ret = mt9p031_s_ctrl(mt9p031->blc_auto);
+				if (ret < 0)
+					return ret;
+			}
+			if (mt9p031->blc_offset->cur.val != 0) {
+				ret = mt9p031_s_ctrl(mt9p031->blc_offset);
+				if (ret < 0)
+					return ret;
+			}
 			return mt9p031_write(client, MT9P031_TEST_PATTERN,
 					     MT9P031_TEST_PATTERN_DISABLE);
 		}
@@ -640,10 +668,14 @@ static int mt9p031_s_ctrl(struct v4l2_ctrl *ctrl)
 		if (ret < 0)
 			return ret;
 
+		/* Disable digital black level compensation when using a test
+		 * pattern.
+		 */
 		ret = mt9p031_set_mode2(mt9p031, MT9P031_READ_MODE_2_ROW_BLC,
 					0);
 		if (ret < 0)
 			return ret;
+
 		ret = mt9p031_write(client, MT9P031_ROW_BLACK_DEF_OFFSET, 0);
 		if (ret < 0)
 			return ret;
@@ -651,7 +683,40 @@ static int mt9p031_s_ctrl(struct v4l2_ctrl *ctrl)
 		return mt9p031_write(client, MT9P031_TEST_PATTERN,
 				((ctrl->val - 1) << MT9P031_TEST_PATTERN_SHIFT)
 				| MT9P031_TEST_PATTERN_ENABLE);
+
+	case V4L2_CID_BLC_AUTO:
+		ret = mt9p031_set_mode2(mt9p031,
+				ctrl->val ? 0 : MT9P031_READ_MODE_2_ROW_BLC,
+				ctrl->val ? MT9P031_READ_MODE_2_ROW_BLC : 0);
+		if (ret < 0)
+			return ret;
+
+		return mt9p031_write(client, MT9P031_BLACK_LEVEL_CALIBRATION,
+				     ctrl->val ? 0 : MT9P031_BLC_MANUAL_BLC);
+
+	case V4L2_CID_BLC_TARGET_LEVEL:
+		return mt9p031_write(client, MT9P031_ROW_BLACK_TARGET,
+				     ctrl->val);
+
+	case V4L2_CID_BLC_ANALOG_OFFSET:
+		data = ctrl->val & ((1 << 9) - 1);
+
+		ret = mt9p031_write(client, MT9P031_GREEN1_OFFSET, data);
+		if (ret < 0)
+			return ret;
+		ret = mt9p031_write(client, MT9P031_GREEN2_OFFSET, data);
+		if (ret < 0)
+			return ret;
+		ret = mt9p031_write(client, MT9P031_RED_OFFSET, data);
+		if (ret < 0)
+			return ret;
+		return mt9p031_write(client, MT9P031_BLUE_OFFSET, data);
+
+	case V4L2_CID_BLC_DIGITAL_OFFSET:
+		return mt9p031_write(client, MT9P031_ROW_BLACK_DEF_OFFSET,
+				     ctrl->val & ((1 << 12) - 1));
 	}
+
 	return 0;
 }
 
@@ -685,6 +750,46 @@ static const struct v4l2_ctrl_config mt9p031_ctrls[] = {
 		.flags		= 0,
 		.menu_skip_mask	= 0,
 		.qmenu		= mt9p031_test_pattern_menu,
+	}, {
+		.ops		= &mt9p031_ctrl_ops,
+		.id		= V4L2_CID_BLC_AUTO,
+		.type		= V4L2_CTRL_TYPE_BOOLEAN,
+		.name		= "BLC, Auto",
+		.min		= 0,
+		.max		= 1,
+		.step		= 1,
+		.def		= 1,
+		.flags		= 0,
+	}, {
+		.ops		= &mt9p031_ctrl_ops,
+		.id		= V4L2_CID_BLC_TARGET_LEVEL,
+		.type		= V4L2_CTRL_TYPE_INTEGER,
+		.name		= "BLC Target Level",
+		.min		= 0,
+		.max		= 4095,
+		.step		= 1,
+		.def		= 168,
+		.flags		= 0,
+	}, {
+		.ops		= &mt9p031_ctrl_ops,
+		.id		= V4L2_CID_BLC_ANALOG_OFFSET,
+		.type		= V4L2_CTRL_TYPE_INTEGER,
+		.name		= "BLC Analog Offset",
+		.min		= -255,
+		.max		= 255,
+		.step		= 1,
+		.def		= 32,
+		.flags		= 0,
+	}, {
+		.ops		= &mt9p031_ctrl_ops,
+		.id		= V4L2_CID_BLC_DIGITAL_OFFSET,
+		.type		= V4L2_CTRL_TYPE_INTEGER,
+		.name		= "BLC Digital Offset",
+		.min		= -2048,
+		.max		= 2047,
+		.step		= 1,
+		.def		= 40,
+		.flags		= 0,
 	}
 };
 
@@ -764,7 +869,7 @@ static int mt9p031_open(struct v4l2_subdev *subdev, struct v4l2_subdev_fh *fh)
 
 	format = v4l2_subdev_get_try_format(fh, 0);
 
-	if (mt9p031->pdata->version == MT9P031_MONOCHROME_VERSION)
+	if (mt9p031->model == MT9P031_MODEL_MONOCHROME)
 		format->code = V4L2_MBUS_FMT_Y12_1X12;
 	else
 		format->code = V4L2_MBUS_FMT_SGRBG12_1X12;
@@ -842,6 +947,8 @@ static int mt9p031_probe(struct i2c_client *client,
 	mt9p031->pdata = pdata;
 	mt9p031->output_control	= MT9P031_OUTPUT_CONTROL_DEF;
 	mt9p031->mode2 = MT9P031_READ_MODE_2_ROW_BLC;
+	mt9p031->model = did->driver_data;
+	mt9p031->reset = -1;
 
 	v4l2_ctrl_handler_init(&mt9p031->ctrls, ARRAY_SIZE(mt9p031_ctrls) + 4);
 
@@ -862,9 +969,16 @@ static int mt9p031_probe(struct i2c_client *client,
 
 	mt9p031->subdev.ctrl_handler = &mt9p031->ctrls;
 
-	if (mt9p031->ctrls.error)
+	if (mt9p031->ctrls.error) {
 		printk(KERN_INFO "%s: control initialization error %d\n",
 		       __func__, mt9p031->ctrls.error);
+		ret = mt9p031->ctrls.error;
+		goto done;
+	}
+
+	mt9p031->blc_auto = v4l2_ctrl_find(&mt9p031->ctrls, V4L2_CID_BLC_AUTO);
+	mt9p031->blc_offset = v4l2_ctrl_find(&mt9p031->ctrls,
+					     V4L2_CID_BLC_DIGITAL_OFFSET);
 
 	mutex_init(&mt9p031->power_lock);
 	v4l2_i2c_subdev_init(&mt9p031->subdev, client, &mt9p031_subdev_ops);
@@ -882,7 +996,7 @@ static int mt9p031_probe(struct i2c_client *client,
 	mt9p031->crop.left = MT9P031_COLUMN_START_DEF;
 	mt9p031->crop.top = MT9P031_ROW_START_DEF;
 
-	if (mt9p031->pdata->version == MT9P031_MONOCHROME_VERSION)
+	if (mt9p031->model == MT9P031_MODEL_MONOCHROME)
 		mt9p031->format.code = V4L2_MBUS_FMT_Y12_1X12;
 	else
 		mt9p031->format.code = V4L2_MBUS_FMT_SGRBG12_1X12;
@@ -892,10 +1006,22 @@ static int mt9p031_probe(struct i2c_client *client,
 	mt9p031->format.field = V4L2_FIELD_NONE;
 	mt9p031->format.colorspace = V4L2_COLORSPACE_SRGB;
 
+	if (pdata->reset != -1) {
+		ret = gpio_request_one(pdata->reset, GPIOF_OUT_INIT_LOW,
+				       "mt9p031_rst");
+		if (ret < 0)
+			goto done;
+
+		mt9p031->reset = pdata->reset;
+	}
+
 	ret = mt9p031_pll_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);
@@ -912,13 +1038,16 @@ 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;
 }
 
 static const struct i2c_device_id mt9p031_id[] = {
-	{ "mt9p031", 0 },
+	{ "mt9p031", MT9P031_MODEL_COLOR },
+	{ "mt9p031m", MT9P031_MODEL_MONOCHROME },
 	{ }
 };
 MODULE_DEVICE_TABLE(i2c, mt9p031_id);
diff --git a/drivers/media/video/mt9t112.c b/drivers/media/video/mt9t112.c
index 8d1445f1270..e1ae46a7ee9 100644
--- a/drivers/media/video/mt9t112.c
+++ b/drivers/media/video/mt9t112.c
@@ -453,6 +453,7 @@ static int mt9t112_init_pll(const struct i2c_client *client)
 	 * I2C Master Clock Divider
 	 */
 	mt9t112_reg_write(ret, client, 0x0014, 0x3046);
+	mt9t112_reg_write(ret, client, 0x0016, 0x0400); /* JPEG initialization workaround */
 	mt9t112_reg_write(ret, client, 0x0022, 0x0190);
 	mt9t112_reg_write(ret, client, 0x3B84, 0x0212);
 
diff --git a/drivers/media/video/mt9v032.c b/drivers/media/video/mt9v032.c
index 75e253a343c..4ba4884c016 100644
--- a/drivers/media/video/mt9v032.c
+++ b/drivers/media/video/mt9v032.c
@@ -481,7 +481,7 @@ static int mt9v032_s_ctrl(struct v4l2_ctrl *ctrl)
 
 	case V4L2_CID_EXPOSURE_AUTO:
 		return mt9v032_update_aec_agc(mt9v032, MT9V032_AEC_ENABLE,
-					      ctrl->val);
+					      !ctrl->val);
 
 	case V4L2_CID_EXPOSURE:
 		return mt9v032_write(client, MT9V032_TOTAL_SHUTTER_WIDTH,
diff --git a/drivers/media/video/mx1_camera.c b/drivers/media/video/mx1_camera.c
index 055d11ddb03..4296a835029 100644
--- a/drivers/media/video/mx1_camera.c
+++ b/drivers/media/video/mx1_camera.c
@@ -126,13 +126,8 @@ static int mx1_videobuf_setup(struct videobuf_queue *vq, unsigned int *count,
 			      unsigned int *size)
 {
 	struct soc_camera_device *icd = vq->priv_data;
-	int bytes_per_line = soc_mbus_bytes_per_line(icd->user_width,
-						icd->current_fmt->host_fmt);
 
-	if (bytes_per_line < 0)
-		return bytes_per_line;
-
-	*size = bytes_per_line * icd->user_height;
+	*size = icd->sizeimage;
 
 	if (!*count)
 		*count = 32;
@@ -171,11 +166,6 @@ static int mx1_videobuf_prepare(struct videobuf_queue *vq,
 	struct soc_camera_device *icd = vq->priv_data;
 	struct mx1_buffer *buf = container_of(vb, struct mx1_buffer, vb);
 	int ret;
-	int bytes_per_line = soc_mbus_bytes_per_line(icd->user_width,
-						icd->current_fmt->host_fmt);
-
-	if (bytes_per_line < 0)
-		return bytes_per_line;
 
 	dev_dbg(icd->parent, "%s (vb=0x%p) 0x%08lx %d\n", __func__,
 		vb, vb->baddr, vb->bsize);
@@ -202,7 +192,7 @@ static int mx1_videobuf_prepare(struct videobuf_queue *vq,
 		vb->state	= VIDEOBUF_NEEDS_INIT;
 	}
 
-	vb->size = bytes_per_line * vb->height;
+	vb->size = icd->sizeimage;
 	if (0 != vb->baddr && vb->bsize < vb->size) {
 		ret = -EINVAL;
 		goto out;
diff --git a/drivers/media/video/mx2_camera.c b/drivers/media/video/mx2_camera.c
index 18afaeeadb7..ded26b7286f 100644
--- a/drivers/media/video/mx2_camera.c
+++ b/drivers/media/video/mx2_camera.c
@@ -22,6 +22,7 @@
 #include <linux/gcd.h>
 #include <linux/interrupt.h>
 #include <linux/kernel.h>
+#include <linux/math64.h>
 #include <linux/mm.h>
 #include <linux/moduleparam.h>
 #include <linux/time.h>
@@ -344,6 +345,19 @@ static struct mx2_fmt_cfg mx27_emma_prp_table[] = {
 					PRP_INTR_CH2OVF,
 		}
 	},
+	{
+		.in_fmt		= V4L2_MBUS_FMT_UYVY8_2X8,
+		.out_fmt	= V4L2_PIX_FMT_YUV420,
+		.cfg		= {
+			.channel	= 2,
+			.in_fmt		= PRP_CNTL_DATA_IN_YUV422,
+			.out_fmt	= PRP_CNTL_CH2_OUT_YUV420,
+			.src_pixel	= 0x22000888, /* YUV422 (YUYV) */
+			.irq_flags	= PRP_INTR_RDERR | PRP_INTR_CH2WERR |
+					PRP_INTR_CH2FC | PRP_INTR_LBOVF |
+					PRP_INTR_CH2OVF,
+		}
+	},
 };
 
 static struct mx2_fmt_cfg *mx27_emma_prp_get_format(
@@ -525,8 +539,6 @@ static int mx2_videobuf_setup(struct vb2_queue *vq,
 	struct soc_camera_device *icd = soc_camera_from_vb2q(vq);
 	struct soc_camera_host *ici = to_soc_camera_host(icd->parent);
 	struct mx2_camera_dev *pcdev = ici->priv;
-	int bytes_per_line = soc_mbus_bytes_per_line(icd->user_width,
-			icd->current_fmt->host_fmt);
 
 	dev_dbg(icd->parent, "count=%d, size=%d\n", *count, sizes[0]);
 
@@ -534,12 +546,9 @@ static int mx2_videobuf_setup(struct vb2_queue *vq,
 	if (fmt != NULL)
 		return -ENOTTY;
 
-	if (bytes_per_line < 0)
-		return bytes_per_line;
-
 	alloc_ctxs[0] = pcdev->alloc_ctx;
 
-	sizes[0] = bytes_per_line * icd->user_height;
+	sizes[0] = icd->sizeimage;
 
 	if (0 == *count)
 		*count = 32;
@@ -555,16 +564,11 @@ static int mx2_videobuf_setup(struct vb2_queue *vq,
 static int mx2_videobuf_prepare(struct vb2_buffer *vb)
 {
 	struct soc_camera_device *icd = soc_camera_from_vb2q(vb->vb2_queue);
-	int bytes_per_line = soc_mbus_bytes_per_line(icd->user_width,
-			icd->current_fmt->host_fmt);
 	int ret = 0;
 
 	dev_dbg(icd->parent, "%s (vb=0x%p) 0x%p %lu\n", __func__,
 		vb, vb2_plane_vaddr(vb, 0), vb2_get_plane_payload(vb, 0));
 
-	if (bytes_per_line < 0)
-		return bytes_per_line;
-
 #ifdef DEBUG
 	/*
 	 * This can be useful if you want to see if we actually fill
@@ -574,7 +578,7 @@ static int mx2_videobuf_prepare(struct vb2_buffer *vb)
 	       0xaa, vb2_get_plane_payload(vb, 0));
 #endif
 
-	vb2_set_plane_payload(vb, 0, bytes_per_line * icd->user_height);
+	vb2_set_plane_payload(vb, 0, icd->sizeimage);
 	if (vb2_plane_vaddr(vb, 0) &&
 	    vb2_get_plane_payload(vb, 0) > vb2_plane_size(vb, 0)) {
 		ret = -EINVAL;
@@ -980,6 +984,7 @@ static int mx2_camera_set_bus_param(struct soc_camera_device *icd)
 	struct soc_camera_host *ici = to_soc_camera_host(icd->parent);
 	struct mx2_camera_dev *pcdev = ici->priv;
 	struct v4l2_mbus_config cfg = {.type = V4L2_MBUS_PARALLEL,};
+	const struct soc_camera_format_xlate *xlate;
 	unsigned long common_flags;
 	int ret;
 	int bytesperline;
@@ -1024,14 +1029,31 @@ static int mx2_camera_set_bus_param(struct soc_camera_device *icd)
 		return ret;
 	}
 
+	xlate = soc_camera_xlate_by_fourcc(icd, pixfmt);
+	if (!xlate) {
+		dev_warn(icd->parent, "Format %x not found\n", pixfmt);
+		return -EINVAL;
+	}
+
+	if (xlate->code == V4L2_MBUS_FMT_YUYV8_2X8) {
+		csicr1 |= CSICR1_PACK_DIR;
+		csicr1 &= ~CSICR1_SWAP16_EN;
+		dev_dbg(icd->parent, "already yuyv format, don't convert\n");
+	} else if (xlate->code == V4L2_MBUS_FMT_UYVY8_2X8) {
+		csicr1 &= ~CSICR1_PACK_DIR;
+		csicr1 |= CSICR1_SWAP16_EN;
+		dev_dbg(icd->parent, "convert uyvy mbus format into yuyv\n");
+	} else {
+		dev_warn(icd->parent, "mbus format not supported\n");
+		return -EINVAL;
+	}
+
 	if (common_flags & V4L2_MBUS_PCLK_SAMPLE_RISING)
 		csicr1 |= CSICR1_REDGE;
 	if (common_flags & V4L2_MBUS_VSYNC_ACTIVE_HIGH)
 		csicr1 |= CSICR1_SOF_POL;
 	if (common_flags & V4L2_MBUS_HSYNC_ACTIVE_HIGH)
 		csicr1 |= CSICR1_HSYNC_POL;
-	if (pcdev->platform_flags & MX2_CAMERA_SWAP16)
-		csicr1 |= CSICR1_SWAP16_EN;
 	if (pcdev->platform_flags & MX2_CAMERA_EXT_VSYNC)
 		csicr1 |= CSICR1_EXT_VSYNC;
 	if (pcdev->platform_flags & MX2_CAMERA_CCIR)
@@ -1042,8 +1064,6 @@ static int mx2_camera_set_bus_param(struct soc_camera_device *icd)
 		csicr1 |= CSICR1_GCLK_MODE;
 	if (pcdev->platform_flags & MX2_CAMERA_INV_DATA)
 		csicr1 |= CSICR1_INV_DATA;
-	if (pcdev->platform_flags & MX2_CAMERA_PACK_DIR_MSB)
-		csicr1 |= CSICR1_PACK_DIR;
 
 	pcdev->csicr1 = csicr1;
 
@@ -1118,7 +1138,8 @@ static int mx2_camera_get_formats(struct soc_camera_device *icd,
 		return 0;
 	}
 
-	if (code == V4L2_MBUS_FMT_YUYV8_2X8) {
+	if (code == V4L2_MBUS_FMT_YUYV8_2X8 ||
+	    code == V4L2_MBUS_FMT_UYVY8_2X8) {
 		formats++;
 		if (xlate) {
 			/*
@@ -1134,6 +1155,18 @@ static int mx2_camera_get_formats(struct soc_camera_device *icd,
 		}
 	}
 
+	if (code == V4L2_MBUS_FMT_UYVY8_2X8) {
+		formats++;
+		if (xlate) {
+			xlate->host_fmt =
+				soc_mbus_get_fmtdesc(V4L2_MBUS_FMT_YUYV8_2X8);
+			xlate->code	= code;
+			dev_dbg(dev, "Providing host format %s for sensor code %d\n",
+				xlate->host_fmt->name, code);
+			xlate++;
+		}
+	}
+
 	/* Generic pass-trough */
 	formats++;
 	if (xlate) {
@@ -1363,17 +1396,20 @@ static int mx2_camera_try_fmt(struct soc_camera_device *icd,
 				xlate->host_fmt);
 		if (pix->bytesperline < 0)
 			return pix->bytesperline;
-		pix->sizeimage = pix->height * pix->bytesperline;
+		pix->sizeimage = soc_mbus_image_size(xlate->host_fmt,
+						pix->bytesperline, pix->height);
 		/* Check against the CSIRXCNT limit */
 		if (pix->sizeimage > 4 * 0x3ffff) {
 			/* Adjust geometry, preserve aspect ratio */
-			unsigned int new_height = int_sqrt(4 * 0x3ffff *
-					pix->height / pix->bytesperline);
+			unsigned int new_height = int_sqrt(div_u64(0x3ffffULL *
+					4 * pix->height, pix->bytesperline));
 			pix->width = new_height * pix->width / pix->height;
 			pix->height = new_height;
 			pix->bytesperline = soc_mbus_bytes_per_line(pix->width,
 							xlate->host_fmt);
 			BUG_ON(pix->bytesperline < 0);
+			pix->sizeimage = soc_mbus_image_size(xlate->host_fmt,
+						pix->bytesperline, pix->height);
 		}
 	}
 
@@ -1752,6 +1788,8 @@ static int __devinit mx2_camera_probe(struct platform_device *pdev)
 	pcdev->soc_host.priv		= pcdev;
 	pcdev->soc_host.v4l2_dev.dev	= &pdev->dev;
 	pcdev->soc_host.nr		= pdev->id;
+	if (cpu_is_mx25())
+		pcdev->soc_host.capabilities = SOCAM_HOST_CAP_STRIDE;
 
 	pcdev->alloc_ctx = vb2_dma_contig_init_ctx(&pdev->dev);
 	if (IS_ERR(pcdev->alloc_ctx)) {
diff --git a/drivers/media/video/mx2_emmaprp.c b/drivers/media/video/mx2_emmaprp.c
index ba89a7401c8..0bd5815de36 100644
--- a/drivers/media/video/mx2_emmaprp.c
+++ b/drivers/media/video/mx2_emmaprp.c
@@ -755,7 +755,7 @@ static int queue_init(void *priv, struct vb2_queue *src_vq,
 
 	memset(src_vq, 0, sizeof(*src_vq));
 	src_vq->type = V4L2_BUF_TYPE_VIDEO_OUTPUT;
-	src_vq->io_modes = VB2_MMAP;
+	src_vq->io_modes = VB2_MMAP | VB2_USERPTR;
 	src_vq->drv_priv = ctx;
 	src_vq->buf_struct_size = sizeof(struct v4l2_m2m_buffer);
 	src_vq->ops = &emmaprp_qops;
@@ -767,7 +767,7 @@ static int queue_init(void *priv, struct vb2_queue *src_vq,
 
 	memset(dst_vq, 0, sizeof(*dst_vq));
 	dst_vq->type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
-	dst_vq->io_modes = VB2_MMAP;
+	dst_vq->io_modes = VB2_MMAP | VB2_USERPTR;
 	dst_vq->drv_priv = ctx;
 	dst_vq->buf_struct_size = sizeof(struct v4l2_m2m_buffer);
 	dst_vq->ops = &emmaprp_qops;
@@ -904,6 +904,10 @@ static int emmaprp_probe(struct platform_device *pdev)
 	}
 
 	*vfd = emmaprp_videodev;
+	/* Locking in file operations other than ioctl should be done
+	   by the driver, not the V4L2 core.
+	   This driver needs auditing so that this flag can be removed. */
+	set_bit(V4L2_FL_LOCK_ALL_FOPS, &vfd->flags);
 	vfd->lock = &pcdev->dev_mutex;
 
 	video_set_drvdata(vfd, pcdev);
diff --git a/drivers/media/video/mx3_camera.c b/drivers/media/video/mx3_camera.c
index 93c35ef5f0a..f13643d3135 100644
--- a/drivers/media/video/mx3_camera.c
+++ b/drivers/media/video/mx3_camera.c
@@ -199,8 +199,6 @@ static int mx3_videobuf_setup(struct vb2_queue *vq,
 	struct soc_camera_device *icd = soc_camera_from_vb2q(vq);
 	struct soc_camera_host *ici = to_soc_camera_host(icd->parent);
 	struct mx3_camera_dev *mx3_cam = ici->priv;
-	int bytes_per_line;
-	unsigned int height;
 
 	if (!mx3_cam->idmac_channel[0])
 		return -EINVAL;
@@ -208,21 +206,29 @@ static int mx3_videobuf_setup(struct vb2_queue *vq,
 	if (fmt) {
 		const struct soc_camera_format_xlate *xlate = soc_camera_xlate_by_fourcc(icd,
 								fmt->fmt.pix.pixelformat);
+		unsigned int bytes_per_line;
+		int ret;
+
 		if (!xlate)
 			return -EINVAL;
-		bytes_per_line = soc_mbus_bytes_per_line(fmt->fmt.pix.width,
-							 xlate->host_fmt);
-		height = fmt->fmt.pix.height;
+
+		ret = soc_mbus_bytes_per_line(fmt->fmt.pix.width,
+					      xlate->host_fmt);
+		if (ret < 0)
+			return ret;
+
+		bytes_per_line = max_t(u32, fmt->fmt.pix.bytesperline, ret);
+
+		ret = soc_mbus_image_size(xlate->host_fmt, bytes_per_line,
+					  fmt->fmt.pix.height);
+		if (ret < 0)
+			return ret;
+
+		sizes[0] = max_t(u32, fmt->fmt.pix.sizeimage, ret);
 	} else {
 		/* Called from VIDIOC_REQBUFS or in compatibility mode */
-		bytes_per_line = soc_mbus_bytes_per_line(icd->user_width,
-						icd->current_fmt->host_fmt);
-		height = icd->user_height;
+		sizes[0] = icd->sizeimage;
 	}
-	if (bytes_per_line < 0)
-		return bytes_per_line;
-
-	sizes[0] = bytes_per_line * height;
 
 	alloc_ctxs[0] = mx3_cam->alloc_ctx;
 
@@ -267,14 +273,11 @@ static void mx3_videobuf_queue(struct vb2_buffer *vb)
 	struct idmac_channel *ichan = mx3_cam->idmac_channel[0];
 	struct idmac_video_param *video = &ichan->params.video;
 	const struct soc_mbus_pixelfmt *host_fmt = icd->current_fmt->host_fmt;
-	int bytes_per_line = soc_mbus_bytes_per_line(icd->user_width, host_fmt);
 	unsigned long flags;
 	dma_cookie_t cookie;
 	size_t new_size;
 
-	BUG_ON(bytes_per_line <= 0);
-
-	new_size = bytes_per_line * icd->user_height;
+	new_size = icd->sizeimage;
 
 	if (vb2_plane_size(vb, 0) < new_size) {
 		dev_err(icd->parent, "Buffer #%d too small (%lu < %zu)\n",
@@ -314,9 +317,9 @@ static void mx3_videobuf_queue(struct vb2_buffer *vb)
 		 * horizontal parameters in this case are expressed in bytes,
 		 * not in pixels.
 		 */
-		video->out_width	= bytes_per_line;
+		video->out_width	= icd->bytesperline;
 		video->out_height	= icd->user_height;
-		video->out_stride	= bytes_per_line;
+		video->out_stride	= icd->bytesperline;
 	} else {
 		/*
 		 * For IPU known formats the pixel unit will be managed
@@ -508,7 +511,7 @@ static void mx3_camera_activate(struct mx3_camera_dev *mx3_cam,
 	/* ipu_csi_init_interface() */
 	csi_reg_write(mx3_cam, conf, CSI_SENS_CONF);
 
-	clk_enable(mx3_cam->clk);
+	clk_prepare_enable(mx3_cam->clk);
 	rate = clk_round_rate(mx3_cam->clk, mx3_cam->mclk);
 	dev_dbg(icd->parent, "Set SENS_CONF to %x, rate %ld\n", conf, rate);
 	if (rate)
@@ -549,7 +552,7 @@ static void mx3_camera_remove_device(struct soc_camera_device *icd)
 		*ichan = NULL;
 	}
 
-	clk_disable(mx3_cam->clk);
+	clk_disable_unprepare(mx3_cam->clk);
 
 	mx3_cam->icd = NULL;
 
@@ -642,12 +645,14 @@ static const struct soc_mbus_pixelfmt mx3_camera_formats[] = {
 		.bits_per_sample	= 8,
 		.packing		= SOC_MBUS_PACKING_NONE,
 		.order			= SOC_MBUS_ORDER_LE,
+		.layout			= SOC_MBUS_LAYOUT_PACKED,
 	}, {
 		.fourcc			= V4L2_PIX_FMT_GREY,
 		.name			= "Monochrome 8 bit",
 		.bits_per_sample	= 8,
 		.packing		= SOC_MBUS_PACKING_NONE,
 		.order			= SOC_MBUS_ORDER_LE,
+		.layout			= SOC_MBUS_LAYOUT_PACKED,
 	},
 };
 
diff --git a/drivers/media/video/mxb.c b/drivers/media/video/mxb.c
index 2e413174843..b520a45cb3f 100644
--- a/drivers/media/video/mxb.c
+++ b/drivers/media/video/mxb.c
@@ -31,10 +31,11 @@
 #include <media/saa7115.h>
 #include <linux/module.h>
 
-#include "mxb.h"
 #include "tea6415c.h"
 #include "tea6420.h"
 
+#define MXB_AUDIOS	6
+
 #define I2C_SAA7111A  0x24
 #define	I2C_TDA9840   0x42
 #define	I2C_TEA6415C  0x43
@@ -62,10 +63,14 @@ MODULE_PARM_DESC(debug, "Turn on/off device debugging (default:off).");
 enum { TUNER, AUX1, AUX3, AUX3_YC };
 
 static struct v4l2_input mxb_inputs[MXB_INPUTS] = {
-	{ TUNER,	"Tuner",		V4L2_INPUT_TYPE_TUNER,	1, 0, V4L2_STD_PAL_BG|V4L2_STD_NTSC_M, 0, V4L2_IN_CAP_STD },
-	{ AUX1,		"AUX1",			V4L2_INPUT_TYPE_CAMERA,	2, 0, V4L2_STD_PAL_BG|V4L2_STD_NTSC_M, 0, V4L2_IN_CAP_STD },
-	{ AUX3,		"AUX3 Composite",	V4L2_INPUT_TYPE_CAMERA,	4, 0, V4L2_STD_PAL_BG|V4L2_STD_NTSC_M, 0, V4L2_IN_CAP_STD },
-	{ AUX3_YC,	"AUX3 S-Video",		V4L2_INPUT_TYPE_CAMERA,	4, 0, V4L2_STD_PAL_BG|V4L2_STD_NTSC_M, 0, V4L2_IN_CAP_STD },
+	{ TUNER,   "Tuner",          V4L2_INPUT_TYPE_TUNER,  0x3f, 0,
+		V4L2_STD_PAL_BG | V4L2_STD_PAL_I, 0, V4L2_IN_CAP_STD },
+	{ AUX1,	   "AUX1",           V4L2_INPUT_TYPE_CAMERA, 0x3f, 0,
+		V4L2_STD_ALL, 0, V4L2_IN_CAP_STD },
+	{ AUX3,	   "AUX3 Composite", V4L2_INPUT_TYPE_CAMERA, 0x3f, 0,
+		V4L2_STD_ALL, 0, V4L2_IN_CAP_STD },
+	{ AUX3_YC, "AUX3 S-Video",   V4L2_INPUT_TYPE_CAMERA, 0x3f, 0,
+		V4L2_STD_ALL, 0, V4L2_IN_CAP_STD },
 };
 
 /* this array holds the information, which port of the saa7146 each
@@ -90,6 +95,36 @@ struct mxb_routing {
 	u32 output;
 };
 
+/* these are the available audio sources, which can switched
+   to the line- and cd-output individually */
+static struct v4l2_audio mxb_audios[MXB_AUDIOS] = {
+	    {
+		.index	= 0,
+		.name	= "Tuner",
+		.capability = V4L2_AUDCAP_STEREO,
+	} , {
+		.index	= 1,
+		.name	= "AUX1",
+		.capability = V4L2_AUDCAP_STEREO,
+	} , {
+		.index	= 2,
+		.name	= "AUX2",
+		.capability = V4L2_AUDCAP_STEREO,
+	} , {
+		.index	= 3,
+		.name	= "AUX3",
+		.capability = V4L2_AUDCAP_STEREO,
+	} , {
+		.index	= 4,
+		.name	= "Radio (X9)",
+		.capability = V4L2_AUDCAP_STEREO,
+	} , {
+		.index	= 5,
+		.name	= "CD-ROM (X10)",
+		.capability = V4L2_AUDCAP_STEREO,
+	}
+};
+
 /* These are the necessary input-output-pins for bringing one audio source
    (see above) to the CD-output. Note that gain is set to 0 in this table. */
 static struct mxb_routing TEA6420_cd[MXB_AUDIOS + 1][2] = {
@@ -114,11 +149,6 @@ static struct mxb_routing TEA6420_line[MXB_AUDIOS + 1][2] = {
 	{ { 6, 3 }, { 6, 2 } }	/* Mute */
 };
 
-#define MAXCONTROLS	1
-static struct v4l2_queryctrl mxb_controls[] = {
-	{ V4L2_CID_AUDIO_MUTE, V4L2_CTRL_TYPE_BOOLEAN, "Mute", 0, 1, 1, 0, 0 },
-};
-
 struct mxb
 {
 	struct video_device	*video_dev;
@@ -135,6 +165,7 @@ struct mxb
 
 	int	cur_mode;	/* current audio mode (mono, stereo, ...) */
 	int	cur_input;	/* current input */
+	int	cur_audinput;	/* current audio input */
 	int	cur_mute;	/* current mute status */
 	struct v4l2_frequency	cur_freq;	/* current frequency the tuner is tuned to */
 };
@@ -150,16 +181,21 @@ struct mxb
 #define call_all(dev, o, f, args...) \
 	v4l2_device_call_until_err(&dev->v4l2_dev, 0, o, f, ##args)
 
-static inline void tea6420_route_cd(struct mxb *mxb, int idx)
+static void mxb_update_audmode(struct mxb *mxb)
+{
+	struct v4l2_tuner t = {
+		.audmode = mxb->cur_mode,
+	};
+
+	tda9840_call(mxb, tuner, s_tuner, &t);
+}
+
+static inline void tea6420_route(struct mxb *mxb, int idx)
 {
 	v4l2_subdev_call(mxb->tea6420_1, audio, s_routing,
 		TEA6420_cd[idx][0].input, TEA6420_cd[idx][0].output, 0);
 	v4l2_subdev_call(mxb->tea6420_2, audio, s_routing,
 		TEA6420_cd[idx][1].input, TEA6420_cd[idx][1].output, 0);
-}
-
-static inline void tea6420_route_line(struct mxb *mxb, int idx)
-{
 	v4l2_subdev_call(mxb->tea6420_1, audio, s_routing,
 		TEA6420_line[idx][0].input, TEA6420_line[idx][0].output, 0);
 	v4l2_subdev_call(mxb->tea6420_2, audio, s_routing,
@@ -168,16 +204,45 @@ static inline void tea6420_route_line(struct mxb *mxb, int idx)
 
 static struct saa7146_extension extension;
 
+static int mxb_s_ctrl(struct v4l2_ctrl *ctrl)
+{
+	struct saa7146_dev *dev = container_of(ctrl->handler,
+				struct saa7146_dev, ctrl_handler);
+	struct mxb *mxb = dev->ext_priv;
+
+	switch (ctrl->id) {
+	case V4L2_CID_AUDIO_MUTE:
+		mxb->cur_mute = ctrl->val;
+		/* switch the audio-source */
+		tea6420_route(mxb, ctrl->val ? 6 :
+				video_audio_connect[mxb->cur_input]);
+		break;
+	default:
+		return -EINVAL;
+	}
+	return 0;
+}
+
+static const struct v4l2_ctrl_ops mxb_ctrl_ops = {
+	.s_ctrl = mxb_s_ctrl,
+};
+
 static int mxb_probe(struct saa7146_dev *dev)
 {
+	struct v4l2_ctrl_handler *hdl = &dev->ctrl_handler;
 	struct mxb *mxb = NULL;
 
+	v4l2_ctrl_new_std(hdl, &mxb_ctrl_ops,
+			V4L2_CID_AUDIO_MUTE, 0, 1, 1, 1);
+	if (hdl->error)
+		return hdl->error;
 	mxb = kzalloc(sizeof(struct mxb), GFP_KERNEL);
 	if (mxb == NULL) {
 		DEB_D("not enough kernel memory\n");
 		return -ENOMEM;
 	}
 
+
 	snprintf(mxb->i2c_adapter.name, sizeof(mxb->i2c_adapter.name), "mxb%d", mxb_num);
 
 	saa7146_i2c_adapter_prepare(dev, &mxb->i2c_adapter, SAA7146_I2C_BUS_BIT_RATE_480);
@@ -214,6 +279,8 @@ static int mxb_probe(struct saa7146_dev *dev)
 	/* we store the pointer in our private data field */
 	dev->ext_priv = mxb;
 
+	v4l2_ctrl_handler_setup(hdl);
+
 	return 0;
 }
 
@@ -286,6 +353,9 @@ static int mxb_init_done(struct saa7146_dev* dev)
 
 	int i = 0, err = 0;
 
+	/* mute audio on tea6420s */
+	tea6420_route(mxb, 6);
+
 	/* select video mode in saa7111a */
 	saa7111a_call(mxb, core, s_std, std);
 
@@ -306,12 +376,12 @@ static int mxb_init_done(struct saa7146_dev* dev)
 	tuner_call(mxb, tuner, s_frequency, &mxb->cur_freq);
 
 	/* set a default video standard */
+	/* These two gpio calls set the GPIO pins that control the tda9820 */
+	saa7146_write(dev, GPIO_CTRL, 0x00404050);
+	saa7111a_call(mxb, core, s_gpio, 1);
+	saa7111a_call(mxb, core, s_std, std);
 	tuner_call(mxb, core, s_std, std);
 
-	/* mute audio on tea6420s */
-	tea6420_route_line(mxb, 6);
-	tea6420_route_cd(mxb, 6);
-
 	/* switch to tuner-channel on tea6415c */
 	tea6415c_call(mxb, video, s_routing, 3, 17, 0);
 
@@ -320,9 +390,11 @@ static int mxb_init_done(struct saa7146_dev* dev)
 
 	/* the rest for mxb */
 	mxb->cur_input = 0;
+	mxb->cur_audinput = video_audio_connect[mxb->cur_input];
 	mxb->cur_mute = 1;
 
 	mxb->cur_mode = V4L2_TUNER_MODE_STEREO;
+	mxb_update_audmode(mxb);
 
 	/* check if the saa7740 (aka 'sound arena module') is present
 	   on the mxb. if so, we must initialize it. due to lack of
@@ -385,69 +457,6 @@ void mxb_irq_bh(struct saa7146_dev* dev, u32* irq_mask)
 }
 */
 
-static int vidioc_queryctrl(struct file *file, void *fh, struct v4l2_queryctrl *qc)
-{
-	struct saa7146_dev *dev = ((struct saa7146_fh *)fh)->dev;
-	int i;
-
-	for (i = MAXCONTROLS - 1; i >= 0; i--) {
-		if (mxb_controls[i].id == qc->id) {
-			*qc = mxb_controls[i];
-			DEB_D("VIDIOC_QUERYCTRL %d\n", qc->id);
-			return 0;
-		}
-	}
-	return dev->ext_vv_data->core_ops->vidioc_queryctrl(file, fh, qc);
-}
-
-static int vidioc_g_ctrl(struct file *file, void *fh, struct v4l2_control *vc)
-{
-	struct saa7146_dev *dev = ((struct saa7146_fh *)fh)->dev;
-	struct mxb *mxb = (struct mxb *)dev->ext_priv;
-	int i;
-
-	for (i = MAXCONTROLS - 1; i >= 0; i--) {
-		if (mxb_controls[i].id == vc->id)
-			break;
-	}
-
-	if (i < 0)
-		return dev->ext_vv_data->core_ops->vidioc_g_ctrl(file, fh, vc);
-
-	if (vc->id == V4L2_CID_AUDIO_MUTE) {
-		vc->value = mxb->cur_mute;
-		DEB_D("VIDIOC_G_CTRL V4L2_CID_AUDIO_MUTE:%d\n", vc->value);
-		return 0;
-	}
-
-	DEB_EE("VIDIOC_G_CTRL V4L2_CID_AUDIO_MUTE:%d\n", vc->value);
-	return 0;
-}
-
-static int vidioc_s_ctrl(struct file *file, void *fh, struct v4l2_control *vc)
-{
-	struct saa7146_dev *dev = ((struct saa7146_fh *)fh)->dev;
-	struct mxb *mxb = (struct mxb *)dev->ext_priv;
-	int i = 0;
-
-	for (i = MAXCONTROLS - 1; i >= 0; i--) {
-		if (mxb_controls[i].id == vc->id)
-			break;
-	}
-
-	if (i < 0)
-		return dev->ext_vv_data->core_ops->vidioc_s_ctrl(file, fh, vc);
-
-	if (vc->id == V4L2_CID_AUDIO_MUTE) {
-		mxb->cur_mute = vc->value;
-		/* switch the audio-source */
-		tea6420_route_line(mxb, vc->value ? 6 :
-				video_audio_connect[mxb->cur_input]);
-		DEB_EE("VIDIOC_S_CTRL, V4L2_CID_AUDIO_MUTE: %d\n", vc->value);
-	}
-	return 0;
-}
-
 static int vidioc_enum_input(struct file *file, void *fh, struct v4l2_input *i)
 {
 	DEB_EE("VIDIOC_ENUMINPUT %d\n", i->index);
@@ -519,9 +528,12 @@ static int vidioc_s_input(struct file *file, void *fh, unsigned int input)
 	if (saa7111a_call(mxb, video, s_routing, i, SAA7111_FMT_CCIR, 0))
 		pr_err("VIDIOC_S_INPUT: could not address saa7111a\n");
 
+	mxb->cur_audinput = video_audio_connect[input];
 	/* switch the audio-source only if necessary */
 	if (0 == mxb->cur_mute)
-		tea6420_route_line(mxb, video_audio_connect[input]);
+		tea6420_route(mxb, mxb->cur_audinput);
+	if (mxb->cur_audinput == 0)
+		mxb_update_audmode(mxb);
 
 	return 0;
 }
@@ -563,17 +575,20 @@ static int vidioc_s_tuner(struct file *file, void *fh, struct v4l2_tuner *t)
 	return call_all(dev, tuner, s_tuner, t);
 }
 
+static int vidioc_querystd(struct file *file, void *fh, v4l2_std_id *norm)
+{
+	struct saa7146_dev *dev = ((struct saa7146_fh *)fh)->dev;
+
+	return call_all(dev, video, querystd, norm);
+}
+
 static int vidioc_g_frequency(struct file *file, void *fh, struct v4l2_frequency *f)
 {
 	struct saa7146_dev *dev = ((struct saa7146_fh *)fh)->dev;
 	struct mxb *mxb = (struct mxb *)dev->ext_priv;
 
-	if (mxb->cur_input) {
-		DEB_D("VIDIOC_G_FREQ: channel %d does not have a tuner!\n",
-		      mxb->cur_input);
+	if (f->tuner)
 		return -EINVAL;
-	}
-
 	*f = mxb->cur_freq;
 
 	DEB_EE("VIDIOC_G_FREQ: freq:0x%08x\n", mxb->cur_freq.frequency);
@@ -592,17 +607,18 @@ static int vidioc_s_frequency(struct file *file, void *fh, struct v4l2_frequency
 	if (V4L2_TUNER_ANALOG_TV != f->type)
 		return -EINVAL;
 
-	if (mxb->cur_input) {
-		DEB_D("VIDIOC_S_FREQ: channel %d does not have a tuner!\n",
-		      mxb->cur_input);
-		return -EINVAL;
-	}
-
-	mxb->cur_freq = *f;
 	DEB_EE("VIDIOC_S_FREQUENCY: freq:0x%08x\n", mxb->cur_freq.frequency);
 
 	/* tune in desired frequency */
-	tuner_call(mxb, tuner, s_frequency, &mxb->cur_freq);
+	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;
+	if (mxb->cur_audinput == 0)
+		mxb_update_audmode(mxb);
+
+	if (mxb->cur_input)
+		return 0;
 
 	/* hack: changing the frequency should invalidate the vbi-counter (=> alevt) */
 	spin_lock(&dev->slock);
@@ -612,25 +628,40 @@ static int vidioc_s_frequency(struct file *file, void *fh, struct v4l2_frequency
 	return 0;
 }
 
+static int vidioc_enumaudio(struct file *file, void *fh, struct v4l2_audio *a)
+{
+	if (a->index >= MXB_AUDIOS)
+		return -EINVAL;
+	*a = mxb_audios[a->index];
+	return 0;
+}
+
 static int vidioc_g_audio(struct file *file, void *fh, struct v4l2_audio *a)
 {
 	struct saa7146_dev *dev = ((struct saa7146_fh *)fh)->dev;
 	struct mxb *mxb = (struct mxb *)dev->ext_priv;
 
-	if (a->index > MXB_INPUTS) {
-		DEB_D("VIDIOC_G_AUDIO %d out of range\n", a->index);
-		return -EINVAL;
-	}
-
-	DEB_EE("VIDIOC_G_AUDIO %d\n", a->index);
-	memcpy(a, &mxb_audios[video_audio_connect[mxb->cur_input]], sizeof(struct v4l2_audio));
+	DEB_EE("VIDIOC_G_AUDIO\n");
+	*a = mxb_audios[mxb->cur_audinput];
 	return 0;
 }
 
 static int vidioc_s_audio(struct file *file, void *fh, struct v4l2_audio *a)
 {
+	struct saa7146_dev *dev = ((struct saa7146_fh *)fh)->dev;
+	struct mxb *mxb = (struct mxb *)dev->ext_priv;
+
 	DEB_D("VIDIOC_S_AUDIO %d\n", a->index);
-	return 0;
+	if (mxb_inputs[mxb->cur_input].audioset & (1 << a->index)) {
+		if (mxb->cur_audinput != a->index) {
+			mxb->cur_audinput = a->index;
+			tea6420_route(mxb, a->index);
+			if (mxb->cur_audinput == 0)
+				mxb_update_audmode(mxb);
+		}
+		return 0;
+	}
+	return -EINVAL;
 }
 
 #ifdef CONFIG_VIDEO_ADV_DEBUG
@@ -638,60 +669,31 @@ static int vidioc_g_register(struct file *file, void *fh, struct v4l2_dbg_regist
 {
 	struct saa7146_dev *dev = ((struct saa7146_fh *)fh)->dev;
 
-	return call_all(dev, core, g_register, reg);
+	if (!capable(CAP_SYS_ADMIN))
+		return -EPERM;
+	if (v4l2_chip_match_host(&reg->match)) {
+		reg->val = saa7146_read(dev, reg->reg);
+		reg->size = 4;
+		return 0;
+	}
+	call_all(dev, core, g_register, reg);
+	return 0;
 }
 
 static int vidioc_s_register(struct file *file, void *fh, struct v4l2_dbg_register *reg)
 {
 	struct saa7146_dev *dev = ((struct saa7146_fh *)fh)->dev;
 
-	return call_all(dev, core, s_register, reg);
-}
-#endif
-
-static long vidioc_default(struct file *file, void *fh, bool valid_prio,
-							int cmd, void *arg)
-{
-	struct saa7146_dev *dev = ((struct saa7146_fh *)fh)->dev;
-	struct mxb *mxb = (struct mxb *)dev->ext_priv;
-
-	switch (cmd) {
-	case MXB_S_AUDIO_CD:
-	{
-		int i = *(int *)arg;
-
-		if (i < 0 || i >= MXB_AUDIOS) {
-			DEB_D("invalid argument to MXB_S_AUDIO_CD: i:%d\n", i);
-			return -EINVAL;
-		}
-
-		DEB_EE("MXB_S_AUDIO_CD: i:%d\n", i);
-
-		tea6420_route_cd(mxb, i);
-		return 0;
-	}
-	case MXB_S_AUDIO_LINE:
-	{
-		int i = *(int *)arg;
-
-		if (i < 0 || i >= MXB_AUDIOS) {
-			DEB_D("invalid argument to MXB_S_AUDIO_LINE: i:%d\n",
-			      i);
-			return -EINVAL;
-		}
-
-		DEB_EE("MXB_S_AUDIO_LINE: i:%d\n", i);
-		tea6420_route_line(mxb, i);
+	if (!capable(CAP_SYS_ADMIN))
+		return -EPERM;
+	if (v4l2_chip_match_host(&reg->match)) {
+		saa7146_write(dev, reg->reg, reg->val);
+		reg->size = 4;
 		return 0;
 	}
-	default:
-/*
-		DEB2(pr_err("does not handle this ioctl\n"));
-*/
-		return -ENOIOCTLCMD;
-	}
-	return 0;
+	return call_all(dev, core, s_register, reg);
 }
+#endif
 
 static struct saa7146_ext_vv vv_data;
 
@@ -709,23 +711,21 @@ static int mxb_attach(struct saa7146_dev *dev, struct saa7146_pci_extension_data
 	}
 	mxb = (struct mxb *)dev->ext_priv;
 
-	vv_data.ops.vidioc_queryctrl = vidioc_queryctrl;
-	vv_data.ops.vidioc_g_ctrl = vidioc_g_ctrl;
-	vv_data.ops.vidioc_s_ctrl = vidioc_s_ctrl;
-	vv_data.ops.vidioc_enum_input = vidioc_enum_input;
-	vv_data.ops.vidioc_g_input = vidioc_g_input;
-	vv_data.ops.vidioc_s_input = vidioc_s_input;
-	vv_data.ops.vidioc_g_tuner = vidioc_g_tuner;
-	vv_data.ops.vidioc_s_tuner = vidioc_s_tuner;
-	vv_data.ops.vidioc_g_frequency = vidioc_g_frequency;
-	vv_data.ops.vidioc_s_frequency = vidioc_s_frequency;
-	vv_data.ops.vidioc_g_audio = vidioc_g_audio;
-	vv_data.ops.vidioc_s_audio = vidioc_s_audio;
+	vv_data.vid_ops.vidioc_enum_input = vidioc_enum_input;
+	vv_data.vid_ops.vidioc_g_input = vidioc_g_input;
+	vv_data.vid_ops.vidioc_s_input = vidioc_s_input;
+	vv_data.vid_ops.vidioc_querystd = vidioc_querystd;
+	vv_data.vid_ops.vidioc_g_tuner = vidioc_g_tuner;
+	vv_data.vid_ops.vidioc_s_tuner = vidioc_s_tuner;
+	vv_data.vid_ops.vidioc_g_frequency = vidioc_g_frequency;
+	vv_data.vid_ops.vidioc_s_frequency = vidioc_s_frequency;
+	vv_data.vid_ops.vidioc_enumaudio = vidioc_enumaudio;
+	vv_data.vid_ops.vidioc_g_audio = vidioc_g_audio;
+	vv_data.vid_ops.vidioc_s_audio = vidioc_s_audio;
 #ifdef CONFIG_VIDEO_ADV_DEBUG
-	vv_data.ops.vidioc_g_register = vidioc_g_register;
-	vv_data.ops.vidioc_s_register = vidioc_s_register;
+	vv_data.vid_ops.vidioc_g_register = vidioc_g_register;
+	vv_data.vid_ops.vidioc_s_register = vidioc_s_register;
 #endif
-	vv_data.ops.vidioc_default = vidioc_default;
 	if (saa7146_register_device(&mxb->video_dev, dev, "mxb", VFL_TYPE_GRABBER)) {
 		ERR("cannot register capture v4l2 device. skipping.\n");
 		saa7146_vv_release(dev);
@@ -752,6 +752,9 @@ static int mxb_detach(struct saa7146_dev *dev)
 
 	DEB_EE("dev:%p\n", dev);
 
+	/* mute audio on tea6420s */
+	tea6420_route(mxb, 6);
+
 	saa7146_unregister_device(&mxb->video_dev,dev);
 	if (MXB_BOARD_CAN_DO_VBI(dev))
 		saa7146_unregister_device(&mxb->vbi_dev, dev);
@@ -773,20 +776,24 @@ static int std_callback(struct saa7146_dev *dev, struct saa7146_standard *standa
 		v4l2_std_id std = V4L2_STD_PAL_I;
 
 		DEB_D("VIDIOC_S_STD: setting mxb for PAL_I\n");
-		/* set the 7146 gpio register -- I don't know what this does exactly */
+		/* These two gpio calls set the GPIO pins that control the tda9820 */
 		saa7146_write(dev, GPIO_CTRL, 0x00404050);
-		/* unset the 7111 gpio register -- I don't know what this does exactly */
 		saa7111a_call(mxb, core, s_gpio, 0);
-		tuner_call(mxb, core, s_std, std);
+		saa7111a_call(mxb, core, s_std, std);
+		if (mxb->cur_input == 0)
+			tuner_call(mxb, core, s_std, std);
 	} else {
 		v4l2_std_id std = V4L2_STD_PAL_BG;
 
+		if (mxb->cur_input)
+			std = standard->id;
 		DEB_D("VIDIOC_S_STD: setting mxb for PAL/NTSC/SECAM\n");
-		/* set the 7146 gpio register -- I don't know what this does exactly */
+		/* These two gpio calls set the GPIO pins that control the tda9820 */
 		saa7146_write(dev, GPIO_CTRL, 0x00404050);
-		/* set the 7111 gpio register -- I don't know what this does exactly */
 		saa7111a_call(mxb, core, s_gpio, 1);
-		tuner_call(mxb, core, s_std, std);
+		saa7111a_call(mxb, core, s_std, std);
+		if (mxb->cur_input == 0)
+			tuner_call(mxb, core, s_std, std);
 	}
 	return 0;
 }
@@ -836,14 +843,14 @@ MODULE_DEVICE_TABLE(pci, pci_tbl);
 
 static struct saa7146_ext_vv vv_data = {
 	.inputs		= MXB_INPUTS,
-	.capabilities	= V4L2_CAP_TUNER | V4L2_CAP_VBI_CAPTURE,
+	.capabilities	= V4L2_CAP_TUNER | V4L2_CAP_VBI_CAPTURE | V4L2_CAP_AUDIO,
 	.stds		= &standard[0],
 	.num_stds	= sizeof(standard)/sizeof(struct saa7146_standard),
 	.std_callback	= &std_callback,
 };
 
 static struct saa7146_extension extension = {
-	.name		= MXB_IDENTIFIER,
+	.name		= "Multimedia eXtension Board",
 	.flags		= SAA7146_USE_I2C_IRQ,
 
 	.pci_tbl	= &pci_tbl[0],
diff --git a/drivers/media/video/mxb.h b/drivers/media/video/mxb.h
deleted file mode 100644
index 400a57ba62e..00000000000
--- a/drivers/media/video/mxb.h
+++ /dev/null
@@ -1,42 +0,0 @@
-#ifndef __MXB__
-#define __MXB__
-
-#define BASE_VIDIOC_MXB 10
-
-#define MXB_S_AUDIO_CD		_IOW  ('V', BASE_VIDIOC_PRIVATE+BASE_VIDIOC_MXB+0, int)
-#define MXB_S_AUDIO_LINE	_IOW  ('V', BASE_VIDIOC_PRIVATE+BASE_VIDIOC_MXB+1, int)
-
-#define MXB_IDENTIFIER "Multimedia eXtension Board"
-
-#define MXB_AUDIOS	6
-
-/* these are the available audio sources, which can switched
-   to the line- and cd-output individually */
-static struct v4l2_audio mxb_audios[MXB_AUDIOS] = {
-	    {
-		.index	= 0,
-		.name	= "Tuner",
-		.capability = V4L2_AUDCAP_STEREO,
-	} , {
-		.index	= 1,
-		.name	= "AUX1",
-		.capability = V4L2_AUDCAP_STEREO,
-	} , {
-		.index	= 2,
-		.name	= "AUX2",
-		.capability = V4L2_AUDCAP_STEREO,
-	} , {
-		.index	= 3,
-		.name	= "AUX3",
-		.capability = V4L2_AUDCAP_STEREO,
-	} , {
-		.index	= 4,
-		.name	= "Radio (X9)",
-		.capability = V4L2_AUDCAP_STEREO,
-	} , {
-		.index	= 5,
-		.name	= "CD-ROM (X10)",
-		.capability = V4L2_AUDCAP_STEREO,
-	}
-};
-#endif
diff --git a/drivers/media/video/omap1_camera.c b/drivers/media/video/omap1_camera.c
index c20f5ecd679..c7e41145041 100644
--- a/drivers/media/video/omap1_camera.c
+++ b/drivers/media/video/omap1_camera.c
@@ -206,15 +206,10 @@ static int omap1_videobuf_setup(struct videobuf_queue *vq, unsigned int *count,
 		unsigned int *size)
 {
 	struct soc_camera_device *icd = vq->priv_data;
-	int bytes_per_line = soc_mbus_bytes_per_line(icd->user_width,
-			icd->current_fmt->host_fmt);
 	struct soc_camera_host *ici = to_soc_camera_host(icd->parent);
 	struct omap1_cam_dev *pcdev = ici->priv;
 
-	if (bytes_per_line < 0)
-		return bytes_per_line;
-
-	*size = bytes_per_line * icd->user_height;
+	*size = icd->sizeimage;
 
 	if (!*count || *count < OMAP1_CAMERA_MIN_BUF_COUNT(pcdev->vb_mode))
 		*count = OMAP1_CAMERA_MIN_BUF_COUNT(pcdev->vb_mode);
@@ -256,15 +251,10 @@ static int omap1_videobuf_prepare(struct videobuf_queue *vq,
 {
 	struct soc_camera_device *icd = vq->priv_data;
 	struct omap1_cam_buf *buf = container_of(vb, struct omap1_cam_buf, vb);
-	int bytes_per_line = soc_mbus_bytes_per_line(icd->user_width,
-			icd->current_fmt->host_fmt);
 	struct soc_camera_host *ici = to_soc_camera_host(icd->parent);
 	struct omap1_cam_dev *pcdev = ici->priv;
 	int ret;
 
-	if (bytes_per_line < 0)
-		return bytes_per_line;
-
 	WARN_ON(!list_empty(&vb->queue));
 
 	BUG_ON(NULL == icd->current_fmt);
@@ -281,7 +271,7 @@ static int omap1_videobuf_prepare(struct videobuf_queue *vq,
 		vb->state  = VIDEOBUF_NEEDS_INIT;
 	}
 
-	vb->size = bytes_per_line * vb->height;
+	vb->size = icd->sizeimage;
 
 	if (vb->baddr && vb->bsize < vb->size) {
 		ret = -EINVAL;
@@ -999,6 +989,7 @@ static const struct soc_mbus_lookup omap1_cam_formats[] = {
 		.bits_per_sample	= 8,
 		.packing		= SOC_MBUS_PACKING_2X8_PADHI,
 		.order			= SOC_MBUS_ORDER_BE,
+		.layout			= SOC_MBUS_LAYOUT_PACKED,
 	},
 }, {
 	.code = V4L2_MBUS_FMT_VYUY8_2X8,
@@ -1008,6 +999,7 @@ static const struct soc_mbus_lookup omap1_cam_formats[] = {
 		.bits_per_sample	= 8,
 		.packing		= SOC_MBUS_PACKING_2X8_PADHI,
 		.order			= SOC_MBUS_ORDER_BE,
+		.layout			= SOC_MBUS_LAYOUT_PACKED,
 	},
 }, {
 	.code = V4L2_MBUS_FMT_YUYV8_2X8,
@@ -1017,6 +1009,7 @@ static const struct soc_mbus_lookup omap1_cam_formats[] = {
 		.bits_per_sample	= 8,
 		.packing		= SOC_MBUS_PACKING_2X8_PADHI,
 		.order			= SOC_MBUS_ORDER_BE,
+		.layout			= SOC_MBUS_LAYOUT_PACKED,
 	},
 }, {
 	.code = V4L2_MBUS_FMT_YVYU8_2X8,
@@ -1026,6 +1019,7 @@ static const struct soc_mbus_lookup omap1_cam_formats[] = {
 		.bits_per_sample	= 8,
 		.packing		= SOC_MBUS_PACKING_2X8_PADHI,
 		.order			= SOC_MBUS_ORDER_BE,
+		.layout			= SOC_MBUS_LAYOUT_PACKED,
 	},
 }, {
 	.code = V4L2_MBUS_FMT_RGB555_2X8_PADHI_BE,
@@ -1035,6 +1029,7 @@ static const struct soc_mbus_lookup omap1_cam_formats[] = {
 		.bits_per_sample	= 8,
 		.packing		= SOC_MBUS_PACKING_2X8_PADHI,
 		.order			= SOC_MBUS_ORDER_BE,
+		.layout			= SOC_MBUS_LAYOUT_PACKED,
 	},
 }, {
 	.code = V4L2_MBUS_FMT_RGB555_2X8_PADHI_LE,
@@ -1044,6 +1039,7 @@ static const struct soc_mbus_lookup omap1_cam_formats[] = {
 		.bits_per_sample	= 8,
 		.packing		= SOC_MBUS_PACKING_2X8_PADHI,
 		.order			= SOC_MBUS_ORDER_BE,
+		.layout			= SOC_MBUS_LAYOUT_PACKED,
 	},
 }, {
 	.code = V4L2_MBUS_FMT_RGB565_2X8_BE,
@@ -1053,6 +1049,7 @@ static const struct soc_mbus_lookup omap1_cam_formats[] = {
 		.bits_per_sample	= 8,
 		.packing		= SOC_MBUS_PACKING_2X8_PADHI,
 		.order			= SOC_MBUS_ORDER_BE,
+		.layout			= SOC_MBUS_LAYOUT_PACKED,
 	},
 }, {
 	.code = V4L2_MBUS_FMT_RGB565_2X8_LE,
@@ -1062,6 +1059,7 @@ static const struct soc_mbus_lookup omap1_cam_formats[] = {
 		.bits_per_sample	= 8,
 		.packing		= SOC_MBUS_PACKING_2X8_PADHI,
 		.order			= SOC_MBUS_ORDER_BE,
+		.layout			= SOC_MBUS_LAYOUT_PACKED,
 	},
 },
 };
diff --git a/drivers/media/video/omap24xxcam-dma.c b/drivers/media/video/omap24xxcam-dma.c
index 3ea38a8def8..b5ae170de4a 100644
--- a/drivers/media/video/omap24xxcam-dma.c
+++ b/drivers/media/video/omap24xxcam-dma.c
@@ -38,7 +38,7 @@
  */
 
 /* Ack all interrupt on CSR and IRQSTATUS_L0 */
-static void omap24xxcam_dmahw_ack_all(unsigned long base)
+static void omap24xxcam_dmahw_ack_all(void __iomem *base)
 {
 	u32 csr;
 	int i;
@@ -52,7 +52,7 @@ static void omap24xxcam_dmahw_ack_all(unsigned long base)
 }
 
 /* Ack dmach on CSR and IRQSTATUS_L0 */
-static u32 omap24xxcam_dmahw_ack_ch(unsigned long base, int dmach)
+static u32 omap24xxcam_dmahw_ack_ch(void __iomem *base, int dmach)
 {
 	u32 csr;
 
@@ -65,12 +65,12 @@ static u32 omap24xxcam_dmahw_ack_ch(unsigned long base, int dmach)
 	return csr;
 }
 
-static int omap24xxcam_dmahw_running(unsigned long base, int dmach)
+static int omap24xxcam_dmahw_running(void __iomem *base, int dmach)
 {
 	return omap24xxcam_reg_in(base, CAMDMA_CCR(dmach)) & CAMDMA_CCR_ENABLE;
 }
 
-static void omap24xxcam_dmahw_transfer_setup(unsigned long base, int dmach,
+static void omap24xxcam_dmahw_transfer_setup(void __iomem *base, int dmach,
 					     dma_addr_t start, u32 len)
 {
 	omap24xxcam_reg_out(base, CAMDMA_CCR(dmach),
@@ -112,7 +112,7 @@ static void omap24xxcam_dmahw_transfer_setup(unsigned long base, int dmach,
 			    | CAMDMA_CICR_DROP_IE);
 }
 
-static void omap24xxcam_dmahw_transfer_start(unsigned long base, int dmach)
+static void omap24xxcam_dmahw_transfer_start(void __iomem *base, int dmach)
 {
 	omap24xxcam_reg_out(base, CAMDMA_CCR(dmach),
 			    CAMDMA_CCR_SEL_SRC_DST_SYNC
@@ -124,7 +124,7 @@ static void omap24xxcam_dmahw_transfer_start(unsigned long base, int dmach)
 			    | CAMDMA_CCR_SYNCHRO_CAMERA);
 }
 
-static void omap24xxcam_dmahw_transfer_chain(unsigned long base, int dmach,
+static void omap24xxcam_dmahw_transfer_chain(void __iomem *base, int dmach,
 					     int free_dmach)
 {
 	int prev_dmach, ch;
@@ -160,7 +160,7 @@ static void omap24xxcam_dmahw_transfer_chain(unsigned long base, int dmach,
  * controller may not be idle after this routine completes, because
  * the completion routines might start new transfers.
  */
-static void omap24xxcam_dmahw_abort_ch(unsigned long base, int dmach)
+static void omap24xxcam_dmahw_abort_ch(void __iomem *base, int dmach)
 {
 	/* mask all interrupts from this channel */
 	omap24xxcam_reg_out(base, CAMDMA_CICR(dmach), 0);
@@ -171,7 +171,7 @@ static void omap24xxcam_dmahw_abort_ch(unsigned long base, int dmach)
 	omap24xxcam_reg_merge(base, CAMDMA_CCR(dmach), 0, CAMDMA_CCR_ENABLE);
 }
 
-static void omap24xxcam_dmahw_init(unsigned long base)
+static void omap24xxcam_dmahw_init(void __iomem *base)
 {
 	omap24xxcam_reg_out(base, CAMDMA_OCP_SYSCONFIG,
 			    CAMDMA_OCP_SYSCONFIG_MIDLEMODE_FSTANDBY
@@ -362,7 +362,7 @@ void omap24xxcam_dma_hwinit(struct omap24xxcam_dma *dma)
 }
 
 static void omap24xxcam_dma_init(struct omap24xxcam_dma *dma,
-				 unsigned long base)
+				 void __iomem *base)
 {
 	int ch;
 
@@ -577,7 +577,7 @@ void omap24xxcam_sgdma_sync(struct omap24xxcam_sgdma *sgdma)
 }
 
 void omap24xxcam_sgdma_init(struct omap24xxcam_sgdma *sgdma,
-			    unsigned long base,
+			    void __iomem *base,
 			    void (*reset_callback)(unsigned long data),
 			    unsigned long reset_callback_data)
 {
diff --git a/drivers/media/video/omap24xxcam.c b/drivers/media/video/omap24xxcam.c
index 7d386414436..e5015b0d550 100644
--- a/drivers/media/video/omap24xxcam.c
+++ b/drivers/media/video/omap24xxcam.c
@@ -1776,8 +1776,7 @@ static int __devinit omap24xxcam_probe(struct platform_device *pdev)
 	cam->mmio_size = resource_size(mem);
 
 	/* map the region */
-	cam->mmio_base = (unsigned long)
-		ioremap_nocache(cam->mmio_base_phys, cam->mmio_size);
+	cam->mmio_base = ioremap_nocache(cam->mmio_base_phys, cam->mmio_size);
 	if (!cam->mmio_base) {
 		dev_err(cam->dev, "cannot map camera register I/O region\n");
 		goto err;
diff --git a/drivers/media/video/omap24xxcam.h b/drivers/media/video/omap24xxcam.h
index 2ce67f5a48d..d59727afe89 100644
--- a/drivers/media/video/omap24xxcam.h
+++ b/drivers/media/video/omap24xxcam.h
@@ -429,7 +429,7 @@ struct sgdma_state {
 struct omap24xxcam_dma {
 	spinlock_t lock;	/* Lock for the whole structure. */
 
-	unsigned long base;	/* base address for dma controller */
+	void __iomem *base;	/* base address for dma controller */
 
 	/* While dma_stop!=0, an attempt to start a new DMA transfer will
 	 * fail.
@@ -491,7 +491,7 @@ struct omap24xxcam_device {
 
 	/*** hardware resources ***/
 	unsigned int irq;
-	unsigned long mmio_base;
+	void __iomem *mmio_base;
 	unsigned long mmio_base_phys;
 	unsigned long mmio_size;
 
@@ -544,22 +544,22 @@ struct omap24xxcam_fh {
  *
  */
 
-static inline u32 omap24xxcam_reg_in(unsigned long base, u32 offset)
+static inline u32 omap24xxcam_reg_in(u32 __iomem *base, u32 offset)
 {
 	return readl(base + offset);
 }
 
-static inline u32 omap24xxcam_reg_out(unsigned long base, u32 offset,
+static inline u32 omap24xxcam_reg_out(u32 __iomem *base, u32 offset,
 					  u32 val)
 {
 	writel(val, base + offset);
 	return val;
 }
 
-static inline u32 omap24xxcam_reg_merge(unsigned long base, u32 offset,
+static inline u32 omap24xxcam_reg_merge(u32 __iomem *base, u32 offset,
 					    u32 val, u32 mask)
 {
-	u32 addr = base + offset;
+	u32 __iomem *addr = base + offset;
 	u32 new_val = (readl(addr) & ~mask) | (val & mask);
 
 	writel(new_val, addr);
@@ -585,7 +585,7 @@ int omap24xxcam_sgdma_queue(struct omap24xxcam_sgdma *sgdma,
 			    int len, sgdma_callback_t callback, void *arg);
 void omap24xxcam_sgdma_sync(struct omap24xxcam_sgdma *sgdma);
 void omap24xxcam_sgdma_init(struct omap24xxcam_sgdma *sgdma,
-			    unsigned long base,
+			    void __iomem *base,
 			    void (*reset_callback)(unsigned long data),
 			    unsigned long reset_callback_data);
 void omap24xxcam_sgdma_exit(struct omap24xxcam_sgdma *sgdma);
diff --git a/drivers/media/video/omap3isp/isp.c b/drivers/media/video/omap3isp/isp.c
index 12d5f923e1d..1c347633e66 100644
--- a/drivers/media/video/omap3isp/isp.c
+++ b/drivers/media/video/omap3isp/isp.c
@@ -329,19 +329,6 @@ void omap3isp_configure_bridge(struct isp_device *isp,
 	isp_reg_writel(isp, ispctrl_val, OMAP3_ISP_IOMEM_MAIN, ISP_CTRL);
 }
 
-/**
- * isp_set_pixel_clock - Configures the ISP pixel clock
- * @isp: OMAP3 ISP device
- * @pixelclk: Average pixel clock in Hz
- *
- * Set the average pixel clock required by the sensor. The ISP will use the
- * lowest possible memory bandwidth settings compatible with the clock.
- **/
-static void isp_set_pixel_clock(struct isp_device *isp, unsigned int pixelclk)
-{
-	isp->isp_ccdc.vpcfg.pixelclk = pixelclk;
-}
-
 void omap3isp_hist_dma_done(struct isp_device *isp)
 {
 	if (omap3isp_ccdc_busy(&isp->isp_ccdc) ||
@@ -739,6 +726,17 @@ static int isp_pipeline_enable(struct isp_pipeline *pipe,
 	unsigned long flags;
 	int ret;
 
+	/* If the preview engine crashed it might not respond to read/write
+	 * operations on the L4 bus. This would result in a bus fault and a
+	 * kernel oops. Refuse to start streaming in that case. This check must
+	 * be performed before the loop below to avoid starting entities if the
+	 * pipeline won't start anyway (those entities would then likely fail to
+	 * stop, making the problem worse).
+	 */
+	if ((pipe->entities & isp->crashed) &
+	    (1U << isp->isp_prev.subdev.entity.id))
+		return -EIO;
+
 	spin_lock_irqsave(&pipe->lock, flags);
 	pipe->state &= ~(ISP_PIPELINE_IDLE_INPUT | ISP_PIPELINE_IDLE_OUTPUT);
 	spin_unlock_irqrestore(&pipe->lock, flags);
@@ -774,14 +772,6 @@ static int isp_pipeline_enable(struct isp_pipeline *pipe,
 		}
 	}
 
-	/* Frame number propagation. In continuous streaming mode the number
-	 * is incremented in the frame start ISR. In mem-to-mem mode
-	 * singleshot is used and frame start IRQs are not available.
-	 * Thus we have to increment the number here.
-	 */
-	if (pipe->do_propagation && mode == ISP_PIPELINE_STREAM_SINGLESHOT)
-		atomic_inc(&pipe->frame_number);
-
 	return 0;
 }
 
@@ -879,13 +869,15 @@ static int isp_pipeline_disable(struct isp_pipeline *pipe)
 
 		if (ret) {
 			dev_info(isp->dev, "Unable to stop %s\n", subdev->name);
+			/* If the entity failed to stopped, assume it has
+			 * crashed. Mark it as such, the ISP will be reset when
+			 * applications will release it.
+			 */
+			isp->crashed |= 1U << subdev->entity.id;
 			failure = -ETIMEDOUT;
 		}
 	}
 
-	if (failure < 0)
-		isp->needs_reset = true;
-
 	return failure;
 }
 
@@ -1069,6 +1061,7 @@ static int isp_reset(struct isp_device *isp)
 		udelay(1);
 	}
 
+	isp->crashed = 0;
 	return 0;
 }
 
@@ -1495,11 +1488,13 @@ void omap3isp_put(struct isp_device *isp)
 	BUG_ON(isp->ref_count == 0);
 	if (--isp->ref_count == 0) {
 		isp_disable_interrupts(isp);
-		isp_save_ctx(isp);
-		if (isp->needs_reset) {
+		if (isp->domain)
+			isp_save_ctx(isp);
+		/* Reset the ISP if an entity has failed to stop. This is the
+		 * only way to recover from such conditions.
+		 */
+		if (isp->crashed)
 			isp_reset(isp);
-			isp->needs_reset = false;
-		}
 		isp_disable_clocks(isp);
 	}
 	mutex_unlock(&isp->isp_mutex);
@@ -1970,7 +1965,7 @@ error_csiphy:
  *
  * Always returns 0.
  */
-static int isp_remove(struct platform_device *pdev)
+static int __devexit isp_remove(struct platform_device *pdev)
 {
 	struct isp_device *isp = platform_get_drvdata(pdev);
 	int i;
@@ -1981,6 +1976,7 @@ static int isp_remove(struct platform_device *pdev)
 	omap3isp_get(isp);
 	iommu_detach_device(isp->domain, &pdev->dev);
 	iommu_domain_free(isp->domain);
+	isp->domain = NULL;
 	omap3isp_put(isp);
 
 	free_irq(isp->irq_num, isp);
@@ -2050,7 +2046,7 @@ static int isp_map_mem_resource(struct platform_device *pdev,
  *   -EINVAL if couldn't install ISR,
  *   or clk_get return error value.
  */
-static int isp_probe(struct platform_device *pdev)
+static int __devinit isp_probe(struct platform_device *pdev)
 {
 	struct isp_platform_data *pdata = pdev->dev.platform_data;
 	struct isp_device *isp;
@@ -2068,7 +2064,6 @@ static int isp_probe(struct platform_device *pdev)
 
 	isp->autoidle = autoidle;
 	isp->platform_cb.set_xclk = isp_set_xclk;
-	isp->platform_cb.set_pixel_clock = isp_set_pixel_clock;
 
 	mutex_init(&isp->isp_mutex);
 	spin_lock_init(&isp->stat_lock);
@@ -2218,7 +2213,7 @@ MODULE_DEVICE_TABLE(platform, omap3isp_id_table);
 
 static struct platform_driver omap3isp_driver = {
 	.probe = isp_probe,
-	.remove = isp_remove,
+	.remove = __devexit_p(isp_remove),
 	.id_table = omap3isp_id_table,
 	.driver = {
 		.owner = THIS_MODULE,
diff --git a/drivers/media/video/omap3isp/isp.h b/drivers/media/video/omap3isp/isp.h
index d96603eb0d1..fc7af3e32ef 100644
--- a/drivers/media/video/omap3isp/isp.h
+++ b/drivers/media/video/omap3isp/isp.h
@@ -129,7 +129,6 @@ struct isp_platform_callback {
 	int (*csiphy_config)(struct isp_csiphy *phy,
 			     struct isp_csiphy_dphy_cfg *dphy,
 			     struct isp_csiphy_lanes_cfg *lanes);
-	void (*set_pixel_clock)(struct isp_device *isp, unsigned int pixelclk);
 };
 
 /*
@@ -145,6 +144,7 @@ struct isp_platform_callback {
  * @raw_dmamask: Raw DMA mask
  * @stat_lock: Spinlock for handling statistics
  * @isp_mutex: Mutex for serializing requests to ISP.
+ * @crashed: Bitmask of crashed entities (indexed by entity ID)
  * @has_context: Context has been saved at least once and can be restored.
  * @ref_count: Reference count for handling multiple ISP requests.
  * @cam_ick: Pointer to camera interface clock structure.
@@ -184,7 +184,7 @@ struct isp_device {
 	/* ISP Obj */
 	spinlock_t stat_lock;	/* common lock for statistic drivers */
 	struct mutex isp_mutex;	/* For handling ref_count field */
-	bool needs_reset;
+	u32 crashed;
 	int has_context;
 	int ref_count;
 	unsigned int autoidle;
@@ -237,10 +237,6 @@ void omap3isp_configure_bridge(struct isp_device *isp,
 			       const struct isp_parallel_platform_data *pdata,
 			       unsigned int shift);
 
-#define ISP_XCLK_NONE			0
-#define ISP_XCLK_A			1
-#define ISP_XCLK_B			2
-
 struct isp_device *omap3isp_get(struct isp_device *isp);
 void omap3isp_put(struct isp_device *isp);
 
diff --git a/drivers/media/video/omap3isp/ispccdc.c b/drivers/media/video/omap3isp/ispccdc.c
index eaabc27f0fa..7e32331b60f 100644
--- a/drivers/media/video/omap3isp/ispccdc.c
+++ b/drivers/media/video/omap3isp/ispccdc.c
@@ -38,6 +38,9 @@
 #include "ispreg.h"
 #include "ispccdc.h"
 
+#define CCDC_MIN_WIDTH		32
+#define CCDC_MIN_HEIGHT		32
+
 static struct v4l2_mbus_framefmt *
 __ccdc_get_format(struct isp_ccdc_device *ccdc, struct v4l2_subdev_fh *fh,
 		  unsigned int pad, enum v4l2_subdev_format_whence which);
@@ -836,8 +839,8 @@ static void ccdc_config_vp(struct isp_ccdc_device *ccdc)
 
 	if (pipe->input)
 		div = DIV_ROUND_UP(l3_ick, pipe->max_rate);
-	else if (ccdc->vpcfg.pixelclk)
-		div = l3_ick / ccdc->vpcfg.pixelclk;
+	else if (pipe->external_rate)
+		div = l3_ick / pipe->external_rate;
 
 	div = clamp(div, 2U, max_div);
 	fmtcfg_vp |= (div - 2) << ISPCCDC_FMTCFG_VPIF_FRQ_SHIFT;
@@ -1118,6 +1121,7 @@ static void ccdc_configure(struct isp_ccdc_device *ccdc)
 	struct isp_parallel_platform_data *pdata = NULL;
 	struct v4l2_subdev *sensor;
 	struct v4l2_mbus_framefmt *format;
+	const struct v4l2_rect *crop;
 	const struct isp_format_info *fmt_info;
 	struct v4l2_subdev_format fmt_src;
 	unsigned int depth_out;
@@ -1211,14 +1215,14 @@ static void ccdc_configure(struct isp_ccdc_device *ccdc)
 		       OMAP3_ISP_IOMEM_CCDC, ISPCCDC_VDINT);
 
 	/* CCDC_PAD_SOURCE_OF */
-	format = &ccdc->formats[CCDC_PAD_SOURCE_OF];
+	crop = &ccdc->crop;
 
-	isp_reg_writel(isp, (0 << ISPCCDC_HORZ_INFO_SPH_SHIFT) |
-		       ((format->width - 1) << ISPCCDC_HORZ_INFO_NPH_SHIFT),
+	isp_reg_writel(isp, (crop->left << ISPCCDC_HORZ_INFO_SPH_SHIFT) |
+		       ((crop->width - 1) << ISPCCDC_HORZ_INFO_NPH_SHIFT),
 		       OMAP3_ISP_IOMEM_CCDC, ISPCCDC_HORZ_INFO);
-	isp_reg_writel(isp, 0 << ISPCCDC_VERT_START_SLV0_SHIFT,
+	isp_reg_writel(isp, crop->top << ISPCCDC_VERT_START_SLV0_SHIFT,
 		       OMAP3_ISP_IOMEM_CCDC, ISPCCDC_VERT_START);
-	isp_reg_writel(isp, (format->height - 1)
+	isp_reg_writel(isp, (crop->height - 1)
 			<< ISPCCDC_VERT_LINES_NLV_SHIFT,
 		       OMAP3_ISP_IOMEM_CCDC, ISPCCDC_VERT_LINES);
 
@@ -1410,6 +1414,9 @@ static void ccdc_hs_vs_isr(struct isp_ccdc_device *ccdc)
 	struct video_device *vdev = ccdc->subdev.devnode;
 	struct v4l2_event event;
 
+	/* Frame number propagation */
+	atomic_inc(&pipe->frame_number);
+
 	memset(&event, 0, sizeof(event));
 	event.type = V4L2_EVENT_FRAME_SYNC;
 	event.u.frame_sync.frame_sequence = atomic_read(&pipe->frame_number);
@@ -1703,7 +1710,7 @@ static int ccdc_subscribe_event(struct v4l2_subdev *sd, struct v4l2_fh *fh,
 	if (sub->id != 0)
 		return -EINVAL;
 
-	return v4l2_event_subscribe(fh, sub, OMAP3ISP_CCDC_NEVENTS);
+	return v4l2_event_subscribe(fh, sub, OMAP3ISP_CCDC_NEVENTS, NULL);
 }
 
 static int ccdc_unsubscribe_event(struct v4l2_subdev *sd, struct v4l2_fh *fh,
@@ -1790,6 +1797,16 @@ __ccdc_get_format(struct isp_ccdc_device *ccdc, struct v4l2_subdev_fh *fh,
 		return &ccdc->formats[pad];
 }
 
+static struct v4l2_rect *
+__ccdc_get_crop(struct isp_ccdc_device *ccdc, struct v4l2_subdev_fh *fh,
+		enum v4l2_subdev_format_whence which)
+{
+	if (which == V4L2_SUBDEV_FORMAT_TRY)
+		return v4l2_subdev_get_try_crop(fh, CCDC_PAD_SOURCE_OF);
+	else
+		return &ccdc->crop;
+}
+
 /*
  * ccdc_try_format - Try video format on a pad
  * @ccdc: ISP CCDC device
@@ -1806,6 +1823,7 @@ ccdc_try_format(struct isp_ccdc_device *ccdc, struct v4l2_subdev_fh *fh,
 	const struct isp_format_info *info;
 	unsigned int width = fmt->width;
 	unsigned int height = fmt->height;
+	struct v4l2_rect *crop;
 	unsigned int i;
 
 	switch (pad) {
@@ -1831,14 +1849,10 @@ ccdc_try_format(struct isp_ccdc_device *ccdc, struct v4l2_subdev_fh *fh,
 		format = __ccdc_get_format(ccdc, fh, CCDC_PAD_SINK, which);
 		memcpy(fmt, format, sizeof(*fmt));
 
-		/* The data formatter truncates the number of horizontal output
-		 * pixels to a multiple of 16. To avoid clipping data, allow
-		 * callers to request an output size bigger than the input size
-		 * up to the nearest multiple of 16.
-		 */
-		fmt->width = clamp_t(u32, width, 32, fmt->width + 15);
-		fmt->width &= ~15;
-		fmt->height = clamp_t(u32, height, 32, fmt->height);
+		/* Hardcode the output size to the crop rectangle size. */
+		crop = __ccdc_get_crop(ccdc, fh, which);
+		fmt->width = crop->width;
+		fmt->height = crop->height;
 		break;
 
 	case CCDC_PAD_SOURCE_VP:
@@ -1866,6 +1880,49 @@ ccdc_try_format(struct isp_ccdc_device *ccdc, struct v4l2_subdev_fh *fh,
 }
 
 /*
+ * ccdc_try_crop - Validate a crop rectangle
+ * @ccdc: ISP CCDC device
+ * @sink: format on the sink pad
+ * @crop: crop rectangle to be validated
+ */
+static void ccdc_try_crop(struct isp_ccdc_device *ccdc,
+			  const struct v4l2_mbus_framefmt *sink,
+			  struct v4l2_rect *crop)
+{
+	const struct isp_format_info *info;
+	unsigned int max_width;
+
+	/* For Bayer formats, restrict left/top and width/height to even values
+	 * to keep the Bayer pattern.
+	 */
+	info = omap3isp_video_format_info(sink->code);
+	if (info->flavor != V4L2_MBUS_FMT_Y8_1X8) {
+		crop->left &= ~1;
+		crop->top &= ~1;
+	}
+
+	crop->left = clamp_t(u32, crop->left, 0, sink->width - CCDC_MIN_WIDTH);
+	crop->top = clamp_t(u32, crop->top, 0, sink->height - CCDC_MIN_HEIGHT);
+
+	/* The data formatter truncates the number of horizontal output pixels
+	 * to a multiple of 16. To avoid clipping data, allow callers to request
+	 * an output size bigger than the input size up to the nearest multiple
+	 * of 16.
+	 */
+	max_width = (sink->width - crop->left + 15) & ~15;
+	crop->width = clamp_t(u32, crop->width, CCDC_MIN_WIDTH, max_width)
+		    & ~15;
+	crop->height = clamp_t(u32, crop->height, CCDC_MIN_HEIGHT,
+			       sink->height - crop->top);
+
+	/* Odd width/height values don't make sense for Bayer formats. */
+	if (info->flavor != V4L2_MBUS_FMT_Y8_1X8) {
+		crop->width &= ~1;
+		crop->height &= ~1;
+	}
+}
+
+/*
  * ccdc_enum_mbus_code - Handle pixel format enumeration
  * @sd     : pointer to v4l2 subdev structure
  * @fh : V4L2 subdev file handle
@@ -1937,6 +1994,93 @@ static int ccdc_enum_frame_size(struct v4l2_subdev *sd,
 }
 
 /*
+ * ccdc_get_selection - Retrieve a selection rectangle on a pad
+ * @sd: ISP CCDC V4L2 subdevice
+ * @fh: V4L2 subdev file handle
+ * @sel: Selection rectangle
+ *
+ * The only supported rectangles are the crop rectangles on the output formatter
+ * source pad.
+ *
+ * Return 0 on success or a negative error code otherwise.
+ */
+static int ccdc_get_selection(struct v4l2_subdev *sd, struct v4l2_subdev_fh *fh,
+			      struct v4l2_subdev_selection *sel)
+{
+	struct isp_ccdc_device *ccdc = v4l2_get_subdevdata(sd);
+	struct v4l2_mbus_framefmt *format;
+
+	if (sel->pad != CCDC_PAD_SOURCE_OF)
+		return -EINVAL;
+
+	switch (sel->target) {
+	case V4L2_SUBDEV_SEL_TGT_CROP_BOUNDS:
+		sel->r.left = 0;
+		sel->r.top = 0;
+		sel->r.width = INT_MAX;
+		sel->r.height = INT_MAX;
+
+		format = __ccdc_get_format(ccdc, fh, CCDC_PAD_SINK, sel->which);
+		ccdc_try_crop(ccdc, format, &sel->r);
+		break;
+
+	case V4L2_SUBDEV_SEL_TGT_CROP_ACTUAL:
+		sel->r = *__ccdc_get_crop(ccdc, fh, sel->which);
+		break;
+
+	default:
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+/*
+ * ccdc_set_selection - Set a selection rectangle on a pad
+ * @sd: ISP CCDC V4L2 subdevice
+ * @fh: V4L2 subdev file handle
+ * @sel: Selection rectangle
+ *
+ * The only supported rectangle is the actual crop rectangle on the output
+ * formatter source pad.
+ *
+ * Return 0 on success or a negative error code otherwise.
+ */
+static int ccdc_set_selection(struct v4l2_subdev *sd, struct v4l2_subdev_fh *fh,
+			      struct v4l2_subdev_selection *sel)
+{
+	struct isp_ccdc_device *ccdc = v4l2_get_subdevdata(sd);
+	struct v4l2_mbus_framefmt *format;
+
+	if (sel->target != V4L2_SUBDEV_SEL_TGT_CROP_ACTUAL ||
+	    sel->pad != CCDC_PAD_SOURCE_OF)
+		return -EINVAL;
+
+	/* The crop rectangle can't be changed while streaming. */
+	if (ccdc->state != ISP_PIPELINE_STREAM_STOPPED)
+		return -EBUSY;
+
+	/* Modifying the crop rectangle always changes the format on the source
+	 * pad. If the KEEP_CONFIG flag is set, just return the current crop
+	 * rectangle.
+	 */
+	if (sel->flags & V4L2_SUBDEV_SEL_FLAG_KEEP_CONFIG) {
+		sel->r = *__ccdc_get_crop(ccdc, fh, sel->which);
+		return 0;
+	}
+
+	format = __ccdc_get_format(ccdc, fh, CCDC_PAD_SINK, sel->which);
+	ccdc_try_crop(ccdc, format, &sel->r);
+	*__ccdc_get_crop(ccdc, fh, sel->which) = sel->r;
+
+	/* Update the source format. */
+	format = __ccdc_get_format(ccdc, fh, CCDC_PAD_SOURCE_OF, sel->which);
+	ccdc_try_format(ccdc, fh, CCDC_PAD_SOURCE_OF, format, sel->which);
+
+	return 0;
+}
+
+/*
  * ccdc_get_format - Retrieve the video format on a pad
  * @sd : ISP CCDC V4L2 subdevice
  * @fh : V4L2 subdev file handle
@@ -1973,6 +2117,7 @@ static int ccdc_set_format(struct v4l2_subdev *sd, struct v4l2_subdev_fh *fh,
 {
 	struct isp_ccdc_device *ccdc = v4l2_get_subdevdata(sd);
 	struct v4l2_mbus_framefmt *format;
+	struct v4l2_rect *crop;
 
 	format = __ccdc_get_format(ccdc, fh, fmt->pad, fmt->which);
 	if (format == NULL)
@@ -1983,6 +2128,16 @@ static int ccdc_set_format(struct v4l2_subdev *sd, struct v4l2_subdev_fh *fh,
 
 	/* Propagate the format from sink to source */
 	if (fmt->pad == CCDC_PAD_SINK) {
+		/* Reset the crop rectangle. */
+		crop = __ccdc_get_crop(ccdc, fh, fmt->which);
+		crop->left = 0;
+		crop->top = 0;
+		crop->width = fmt->format.width;
+		crop->height = fmt->format.height;
+
+		ccdc_try_crop(ccdc, &fmt->format, crop);
+
+		/* Update the source formats. */
 		format = __ccdc_get_format(ccdc, fh, CCDC_PAD_SOURCE_OF,
 					   fmt->which);
 		*format = fmt->format;
@@ -2000,6 +2155,69 @@ static int ccdc_set_format(struct v4l2_subdev *sd, struct v4l2_subdev_fh *fh,
 }
 
 /*
+ * Decide whether desired output pixel code can be obtained with
+ * the lane shifter by shifting the input pixel code.
+ * @in: input pixelcode to shifter
+ * @out: output pixelcode from shifter
+ * @additional_shift: # of bits the sensor's LSB is offset from CAMEXT[0]
+ *
+ * return true if the combination is possible
+ * return false otherwise
+ */
+static bool ccdc_is_shiftable(enum v4l2_mbus_pixelcode in,
+			      enum v4l2_mbus_pixelcode out,
+			      unsigned int additional_shift)
+{
+	const struct isp_format_info *in_info, *out_info;
+
+	if (in == out)
+		return true;
+
+	in_info = omap3isp_video_format_info(in);
+	out_info = omap3isp_video_format_info(out);
+
+	if ((in_info->flavor == 0) || (out_info->flavor == 0))
+		return false;
+
+	if (in_info->flavor != out_info->flavor)
+		return false;
+
+	return in_info->bpp - out_info->bpp + additional_shift <= 6;
+}
+
+static int ccdc_link_validate(struct v4l2_subdev *sd,
+			      struct media_link *link,
+			      struct v4l2_subdev_format *source_fmt,
+			      struct v4l2_subdev_format *sink_fmt)
+{
+	struct isp_ccdc_device *ccdc = v4l2_get_subdevdata(sd);
+	unsigned long parallel_shift;
+
+	/* Check if the two ends match */
+	if (source_fmt->format.width != sink_fmt->format.width ||
+	    source_fmt->format.height != sink_fmt->format.height)
+		return -EPIPE;
+
+	/* We've got a parallel sensor here. */
+	if (ccdc->input == CCDC_INPUT_PARALLEL) {
+		struct isp_parallel_platform_data *pdata =
+			&((struct isp_v4l2_subdevs_group *)
+			  media_entity_to_v4l2_subdev(link->source->entity)
+			  ->host_priv)->bus.parallel;
+		parallel_shift = pdata->data_lane_shift * 2;
+	} else {
+		parallel_shift = 0;
+	}
+
+	/* Lane shifter may be used to drop bits on CCDC sink pad */
+	if (!ccdc_is_shiftable(source_fmt->format.code,
+			       sink_fmt->format.code, parallel_shift))
+		return -EPIPE;
+
+	return 0;
+}
+
+/*
  * ccdc_init_formats - Initialize formats on all pads
  * @sd: ISP CCDC V4L2 subdevice
  * @fh: V4L2 subdev file handle
@@ -2041,6 +2259,9 @@ static const struct v4l2_subdev_pad_ops ccdc_v4l2_pad_ops = {
 	.enum_frame_size = ccdc_enum_frame_size,
 	.get_fmt = ccdc_get_format,
 	.set_fmt = ccdc_set_format,
+	.get_selection = ccdc_get_selection,
+	.set_selection = ccdc_set_selection,
+	.link_validate = ccdc_link_validate,
 };
 
 /* V4L2 subdev operations */
@@ -2150,6 +2371,7 @@ static int ccdc_link_setup(struct media_entity *entity,
 /* media operations */
 static const struct media_entity_operations ccdc_media_ops = {
 	.link_setup = ccdc_link_setup,
+	.link_validate = v4l2_subdev_link_validate,
 };
 
 void omap3isp_ccdc_unregister_entities(struct isp_ccdc_device *ccdc)
@@ -2276,8 +2498,6 @@ int omap3isp_ccdc_init(struct isp_device *isp)
 	ccdc->clamp.oblen = 0;
 	ccdc->clamp.dcsubval = 0;
 
-	ccdc->vpcfg.pixelclk = 0;
-
 	ccdc->update = OMAP3ISP_CCDC_BLCLAMP;
 	ccdc_apply_controls(ccdc);
 
diff --git a/drivers/media/video/omap3isp/ispccdc.h b/drivers/media/video/omap3isp/ispccdc.h
index 6d0264bab75..890f6b3a68f 100644
--- a/drivers/media/video/omap3isp/ispccdc.h
+++ b/drivers/media/video/omap3isp/ispccdc.h
@@ -80,14 +80,6 @@ struct ispccdc_syncif {
 	u8 bt_r656_en;
 };
 
-/*
- * struct ispccdc_vp - Structure for Video Port parameters
- * @pixelclk: Input pixel clock in Hz
- */
-struct ispccdc_vp {
-	unsigned int pixelclk;
-};
-
 enum ispccdc_lsc_state {
 	LSC_STATE_STOPPED = 0,
 	LSC_STATE_STOPPING = 1,
@@ -147,6 +139,7 @@ struct ispccdc_lsc {
  * @subdev: V4L2 subdevice
  * @pads: Sink and source media entity pads
  * @formats: Active video formats
+ * @crop: Active crop rectangle on the OF source pad
  * @input: Active input
  * @output: Active outputs
  * @video_out: Output video node
@@ -161,7 +154,6 @@ struct ispccdc_lsc {
  * @update: Bitmask of controls to update during the next interrupt
  * @shadow_update: Controls update in progress by userspace
  * @syncif: Interface synchronization configuration
- * @vpcfg: Video port configuration
  * @underrun: A buffer underrun occurred and a new buffer has been queued
  * @state: Streaming state
  * @lock: Serializes shadow_update with interrupt handler
@@ -173,6 +165,7 @@ struct isp_ccdc_device {
 	struct v4l2_subdev subdev;
 	struct media_pad pads[CCDC_PADS_NUM];
 	struct v4l2_mbus_framefmt formats[CCDC_PADS_NUM];
+	struct v4l2_rect crop;
 
 	enum ccdc_input_entity input;
 	unsigned int output;
@@ -190,7 +183,6 @@ struct isp_ccdc_device {
 	unsigned int shadow_update;
 
 	struct ispccdc_syncif syncif;
-	struct ispccdc_vp vpcfg;
 
 	unsigned int underrun:1;
 	enum isp_pipeline_stream_state state;
diff --git a/drivers/media/video/omap3isp/ispccp2.c b/drivers/media/video/omap3isp/ispccp2.c
index 70ddbf35b22..85f0de85f37 100644
--- a/drivers/media/video/omap3isp/ispccp2.c
+++ b/drivers/media/video/omap3isp/ispccp2.c
@@ -161,7 +161,6 @@ static void ccp2_pwr_cfg(struct isp_ccp2_device *ccp2)
 static void ccp2_if_enable(struct isp_ccp2_device *ccp2, u8 enable)
 {
 	struct isp_device *isp = to_isp_device(ccp2);
-	struct isp_pipeline *pipe = to_isp_pipeline(&ccp2->subdev.entity);
 	int i;
 
 	if (enable && ccp2->vdds_csib)
@@ -178,19 +177,6 @@ static void ccp2_if_enable(struct isp_ccp2_device *ccp2, u8 enable)
 			ISPCCP2_CTRL_MODE | ISPCCP2_CTRL_IF_EN,
 			enable ? (ISPCCP2_CTRL_MODE | ISPCCP2_CTRL_IF_EN) : 0);
 
-	/* For frame count propagation */
-	if (pipe->do_propagation) {
-		/* We may want the Frame Start IRQ from LC0 */
-		if (enable)
-			isp_reg_set(isp, OMAP3_ISP_IOMEM_CCP2,
-				    ISPCCP2_LC01_IRQENABLE,
-				    ISPCCP2_LC01_IRQSTATUS_LC0_FS_IRQ);
-		else
-			isp_reg_clr(isp, OMAP3_ISP_IOMEM_CCP2,
-				    ISPCCP2_LC01_IRQENABLE,
-				    ISPCCP2_LC01_IRQSTATUS_LC0_FS_IRQ);
-	}
-
 	if (!enable && ccp2->vdds_csib)
 		regulator_disable(ccp2->vdds_csib);
 }
@@ -350,7 +336,6 @@ static void ccp2_lcx_config(struct isp_ccp2_device *ccp2,
 	      ISPCCP2_LC01_IRQSTATUS_LC0_CRC_IRQ |
 	      ISPCCP2_LC01_IRQSTATUS_LC0_FSP_IRQ |
 	      ISPCCP2_LC01_IRQSTATUS_LC0_FW_IRQ |
-	      ISPCCP2_LC01_IRQSTATUS_LC0_FS_IRQ |
 	      ISPCCP2_LC01_IRQSTATUS_LC0_FSC_IRQ |
 	      ISPCCP2_LC01_IRQSTATUS_LC0_SSC_IRQ;
 
@@ -613,14 +598,6 @@ void omap3isp_ccp2_isr(struct isp_ccp2_device *ccp2)
 	if (omap3isp_module_sync_is_stopping(&ccp2->wait, &ccp2->stopping))
 		return;
 
-	/* Frame number propagation */
-	if (lcx_irqstatus & ISPCCP2_LC01_IRQSTATUS_LC0_FS_IRQ) {
-		struct isp_pipeline *pipe =
-			to_isp_pipeline(&ccp2->subdev.entity);
-		if (pipe->do_propagation)
-			atomic_inc(&pipe->frame_number);
-	}
-
 	/* Handle queued buffers on frame end interrupts */
 	if (lcm_irqstatus & ISPCCP2_LCM_IRQSTATUS_EOF_IRQ)
 		ccp2_isr_buffer(ccp2);
@@ -1021,6 +998,7 @@ static int ccp2_link_setup(struct media_entity *entity,
 /* media operations */
 static const struct media_entity_operations ccp2_media_ops = {
 	.link_setup = ccp2_link_setup,
+	.link_validate = v4l2_subdev_link_validate,
 };
 
 /*
diff --git a/drivers/media/video/omap3isp/ispcsi2.c b/drivers/media/video/omap3isp/ispcsi2.c
index fcb5168996a..a1724362b6d 100644
--- a/drivers/media/video/omap3isp/ispcsi2.c
+++ b/drivers/media/video/omap3isp/ispcsi2.c
@@ -378,21 +378,17 @@ static void csi2_timing_config(struct isp_device *isp,
 static void csi2_irq_ctx_set(struct isp_device *isp,
 			     struct isp_csi2_device *csi2, int enable)
 {
-	u32 reg = ISPCSI2_CTX_IRQSTATUS_FE_IRQ;
 	int i;
 
-	if (csi2->use_fs_irq)
-		reg |= ISPCSI2_CTX_IRQSTATUS_FS_IRQ;
-
 	for (i = 0; i < 8; i++) {
-		isp_reg_writel(isp, reg, csi2->regs1,
+		isp_reg_writel(isp, ISPCSI2_CTX_IRQSTATUS_FE_IRQ, csi2->regs1,
 			       ISPCSI2_CTX_IRQSTATUS(i));
 		if (enable)
 			isp_reg_set(isp, csi2->regs1, ISPCSI2_CTX_IRQENABLE(i),
-				    reg);
+				    ISPCSI2_CTX_IRQSTATUS_FE_IRQ);
 		else
 			isp_reg_clr(isp, csi2->regs1, ISPCSI2_CTX_IRQENABLE(i),
-				    reg);
+				    ISPCSI2_CTX_IRQSTATUS_FE_IRQ);
 	}
 }
 
@@ -690,14 +686,6 @@ static void csi2_isr_ctx(struct isp_csi2_device *csi2,
 	status = isp_reg_readl(isp, csi2->regs1, ISPCSI2_CTX_IRQSTATUS(n));
 	isp_reg_writel(isp, status, csi2->regs1, ISPCSI2_CTX_IRQSTATUS(n));
 
-	/* Propagate frame number */
-	if (status & ISPCSI2_CTX_IRQSTATUS_FS_IRQ) {
-		struct isp_pipeline *pipe =
-				     to_isp_pipeline(&csi2->subdev.entity);
-		if (pipe->do_propagation)
-			atomic_inc(&pipe->frame_number);
-	}
-
 	if (!(status & ISPCSI2_CTX_IRQSTATUS_FE_IRQ))
 		return;
 
@@ -1047,14 +1035,12 @@ static int csi2_set_stream(struct v4l2_subdev *sd, int enable)
 {
 	struct isp_csi2_device *csi2 = v4l2_get_subdevdata(sd);
 	struct isp_device *isp = csi2->isp;
-	struct isp_pipeline *pipe = to_isp_pipeline(&csi2->subdev.entity);
 	struct isp_video *video_out = &csi2->video_out;
 
 	switch (enable) {
 	case ISP_PIPELINE_STREAM_CONTINUOUS:
 		if (omap3isp_csiphy_acquire(csi2->phy) < 0)
 			return -ENODEV;
-		csi2->use_fs_irq = pipe->do_propagation;
 		if (csi2->output & CSI2_OUTPUT_MEMORY)
 			omap3isp_sbl_enable(isp, OMAP3_ISP_SBL_CSI2A_WRITE);
 		csi2_configure(csi2);
@@ -1181,6 +1167,7 @@ static int csi2_link_setup(struct media_entity *entity,
 /* media operations */
 static const struct media_entity_operations csi2_media_ops = {
 	.link_setup = csi2_link_setup,
+	.link_validate = v4l2_subdev_link_validate,
 };
 
 void omap3isp_csi2_unregister_entities(struct isp_csi2_device *csi2)
diff --git a/drivers/media/video/omap3isp/ispcsi2.h b/drivers/media/video/omap3isp/ispcsi2.h
index 885ad79a767..c57729b7e86 100644
--- a/drivers/media/video/omap3isp/ispcsi2.h
+++ b/drivers/media/video/omap3isp/ispcsi2.h
@@ -145,7 +145,6 @@ struct isp_csi2_device {
 	u32 output; /* output to CCDC, memory or both? */
 	bool dpcm_decompress;
 	unsigned int frame_skip;
-	bool use_fs_irq;
 
 	struct isp_csiphy *phy;
 	struct isp_csi2_ctx_cfg contexts[ISP_CSI2_MAX_CTX_NUM + 1];
diff --git a/drivers/media/video/omap3isp/ispcsiphy.c b/drivers/media/video/omap3isp/ispcsiphy.c
index 5be37ce7d0c..348f67ebbbc 100644
--- a/drivers/media/video/omap3isp/ispcsiphy.c
+++ b/drivers/media/video/omap3isp/ispcsiphy.c
@@ -186,7 +186,9 @@ int omap3isp_csiphy_acquire(struct isp_csiphy *phy)
 	if (rval < 0)
 		goto done;
 
-	omap3isp_csi2_reset(phy->csi2);
+	rval = omap3isp_csi2_reset(phy->csi2);
+	if (rval < 0)
+		goto done;
 
 	csiphy_dphy_config(phy);
 	csiphy_lanes_config(phy);
diff --git a/drivers/media/video/omap3isp/ispcsiphy.h b/drivers/media/video/omap3isp/ispcsiphy.h
index 9596dc6830a..e93a661e65d 100644
--- a/drivers/media/video/omap3isp/ispcsiphy.h
+++ b/drivers/media/video/omap3isp/ispcsiphy.h
@@ -27,22 +27,11 @@
 #ifndef OMAP3_ISP_CSI_PHY_H
 #define OMAP3_ISP_CSI_PHY_H
 
+#include <media/omap3isp.h>
+
 struct isp_csi2_device;
 struct regulator;
 
-struct csiphy_lane {
-	u8 pos;
-	u8 pol;
-};
-
-#define ISP_CSIPHY2_NUM_DATA_LANES	2
-#define ISP_CSIPHY1_NUM_DATA_LANES	1
-
-struct isp_csiphy_lanes_cfg {
-	struct csiphy_lane data[ISP_CSIPHY2_NUM_DATA_LANES];
-	struct csiphy_lane clk;
-};
-
 struct isp_csiphy_dphy_cfg {
 	u8 ths_term;
 	u8 ths_settle;
diff --git a/drivers/media/video/omap3isp/isppreview.c b/drivers/media/video/omap3isp/isppreview.c
index 6d0fb2c8c26..8a4935ecc65 100644
--- a/drivers/media/video/omap3isp/isppreview.c
+++ b/drivers/media/video/omap3isp/isppreview.c
@@ -441,23 +441,6 @@ preview_enable_dcor(struct isp_prev_device *prev, u8 enable)
 }
 
 /*
- * preview_enable_cfa - Enable/Disable the CFA Interpolation.
- * @enable: 1 - Enables the CFA.
- */
-static void
-preview_enable_cfa(struct isp_prev_device *prev, u8 enable)
-{
-	struct isp_device *isp = to_isp_device(prev);
-
-	if (enable)
-		isp_reg_set(isp, OMAP3_ISP_IOMEM_PREV, ISPPRV_PCR,
-			    ISPPRV_PCR_CFAEN);
-	else
-		isp_reg_clr(isp, OMAP3_ISP_IOMEM_PREV, ISPPRV_PCR,
-			    ISPPRV_PCR_CFAEN);
-}
-
-/*
  * preview_enable_gammabypass - Enables/Disables the GammaByPass
  * @enable: 1 - Bypasses Gamma - 10bit input is cropped to 8MSB.
  *          0 - Goes through Gamma Correction. input and output is 10bit.
@@ -608,12 +591,12 @@ preview_config_rgb_blending(struct isp_prev_device *prev, const void *rgb2rgb)
 }
 
 /*
- * Configures the RGB-YCbYCr conversion matrix
+ * Configures the color space conversion (RGB toYCbYCr) matrix
  * @prev_csc: Structure containing the RGB to YCbYCr matrix and the
  *            YCbCr offset.
  */
 static void
-preview_config_rgb_to_ycbcr(struct isp_prev_device *prev, const void *prev_csc)
+preview_config_csc(struct isp_prev_device *prev, const void *prev_csc)
 {
 	struct isp_device *isp = to_isp_device(prev);
 	const struct omap3isp_prev_csc *csc = prev_csc;
@@ -649,12 +632,18 @@ preview_config_rgb_to_ycbcr(struct isp_prev_device *prev, const void *prev_csc)
 static void
 preview_update_contrast(struct isp_prev_device *prev, u8 contrast)
 {
-	struct prev_params *params = &prev->params;
+	struct prev_params *params;
+	unsigned long flags;
+
+	spin_lock_irqsave(&prev->params.lock, flags);
+	params = (prev->params.active & OMAP3ISP_PREV_CONTRAST)
+	       ? &prev->params.params[0] : &prev->params.params[1];
 
 	if (params->contrast != (contrast * ISPPRV_CONTRAST_UNITS)) {
 		params->contrast = contrast * ISPPRV_CONTRAST_UNITS;
-		prev->update |= PREV_CONTRAST;
+		params->update |= OMAP3ISP_PREV_CONTRAST;
 	}
+	spin_unlock_irqrestore(&prev->params.lock, flags);
 }
 
 /*
@@ -681,12 +670,18 @@ preview_config_contrast(struct isp_prev_device *prev, const void *params)
 static void
 preview_update_brightness(struct isp_prev_device *prev, u8 brightness)
 {
-	struct prev_params *params = &prev->params;
+	struct prev_params *params;
+	unsigned long flags;
+
+	spin_lock_irqsave(&prev->params.lock, flags);
+	params = (prev->params.active & OMAP3ISP_PREV_BRIGHTNESS)
+	       ? &prev->params.params[0] : &prev->params.params[1];
 
 	if (params->brightness != (brightness * ISPPRV_BRIGHT_UNITS)) {
 		params->brightness = brightness * ISPPRV_BRIGHT_UNITS;
-		prev->update |= PREV_BRIGHTNESS;
+		params->update |= OMAP3ISP_PREV_BRIGHTNESS;
 	}
+	spin_unlock_irqrestore(&prev->params.lock, flags);
 }
 
 /*
@@ -721,159 +716,188 @@ preview_config_yc_range(struct isp_prev_device *prev, const void *yclimit)
 		       OMAP3_ISP_IOMEM_PREV, ISPPRV_SETUP_YC);
 }
 
+static u32
+preview_params_lock(struct isp_prev_device *prev, u32 update, bool shadow)
+{
+	u32 active = prev->params.active;
+
+	if (shadow) {
+		/* Mark all shadow parameters we are going to touch as busy. */
+		prev->params.params[0].busy |= ~active & update;
+		prev->params.params[1].busy |= active & update;
+	} else {
+		/* Mark all active parameters we are going to touch as busy. */
+		update = (prev->params.params[0].update & active)
+		       | (prev->params.params[1].update & ~active);
+
+		prev->params.params[0].busy |= active & update;
+		prev->params.params[1].busy |= ~active & update;
+	}
+
+	return update;
+}
+
+static void
+preview_params_unlock(struct isp_prev_device *prev, u32 update, bool shadow)
+{
+	u32 active = prev->params.active;
+
+	if (shadow) {
+		/* Set the update flag for shadow parameters that have been
+		 * updated and clear the busy flag for all shadow parameters.
+		 */
+		prev->params.params[0].update |= (~active & update);
+		prev->params.params[1].update |= (active & update);
+		prev->params.params[0].busy &= active;
+		prev->params.params[1].busy &= ~active;
+	} else {
+		/* Clear the update flag for active parameters that have been
+		 * applied and the busy flag for all active parameters.
+		 */
+		prev->params.params[0].update &= ~(active & update);
+		prev->params.params[1].update &= ~(~active & update);
+		prev->params.params[0].busy &= ~active;
+		prev->params.params[1].busy &= active;
+	}
+}
+
+static void preview_params_switch(struct isp_prev_device *prev)
+{
+	u32 to_switch;
+
+	/* Switch active parameters with updated shadow parameters when the
+	 * shadow parameter has been updated and neither the active not the
+	 * shadow parameter is busy.
+	 */
+	to_switch = (prev->params.params[0].update & ~prev->params.active)
+		  | (prev->params.params[1].update & prev->params.active);
+	to_switch &= ~(prev->params.params[0].busy |
+		       prev->params.params[1].busy);
+	if (to_switch == 0)
+		return;
+
+	prev->params.active ^= to_switch;
+
+	/* Remove the update flag for the shadow copy of parameters we have
+	 * switched.
+	 */
+	prev->params.params[0].update &= ~(~prev->params.active & to_switch);
+	prev->params.params[1].update &= ~(prev->params.active & to_switch);
+}
+
 /* preview parameters update structure */
 struct preview_update {
-	int cfg_bit;
-	int feature_bit;
 	void (*config)(struct isp_prev_device *, const void *);
 	void (*enable)(struct isp_prev_device *, u8);
+	unsigned int param_offset;
+	unsigned int param_size;
+	unsigned int config_offset;
+	bool skip;
 };
 
-static struct preview_update update_attrs[] = {
-	{OMAP3ISP_PREV_LUMAENH, PREV_LUMA_ENHANCE,
+/* Keep the array indexed by the OMAP3ISP_PREV_* bit number. */
+static const struct preview_update update_attrs[] = {
+	/* OMAP3ISP_PREV_LUMAENH */ {
 		preview_config_luma_enhancement,
-		preview_enable_luma_enhancement},
-	{OMAP3ISP_PREV_INVALAW, PREV_INVERSE_ALAW,
+		preview_enable_luma_enhancement,
+		offsetof(struct prev_params, luma),
+		FIELD_SIZEOF(struct prev_params, luma),
+		offsetof(struct omap3isp_prev_update_config, luma),
+	}, /* OMAP3ISP_PREV_INVALAW */ {
 		NULL,
-		preview_enable_invalaw},
-	{OMAP3ISP_PREV_HRZ_MED, PREV_HORZ_MEDIAN_FILTER,
+		preview_enable_invalaw,
+	}, /* OMAP3ISP_PREV_HRZ_MED */ {
 		preview_config_hmed,
-		preview_enable_hmed},
-	{OMAP3ISP_PREV_CFA, PREV_CFA,
+		preview_enable_hmed,
+		offsetof(struct prev_params, hmed),
+		FIELD_SIZEOF(struct prev_params, hmed),
+		offsetof(struct omap3isp_prev_update_config, hmed),
+	}, /* OMAP3ISP_PREV_CFA */ {
 		preview_config_cfa,
-		preview_enable_cfa},
-	{OMAP3ISP_PREV_CHROMA_SUPP, PREV_CHROMA_SUPPRESS,
+		NULL,
+		offsetof(struct prev_params, cfa),
+		FIELD_SIZEOF(struct prev_params, cfa),
+		offsetof(struct omap3isp_prev_update_config, cfa),
+	}, /* OMAP3ISP_PREV_CHROMA_SUPP */ {
 		preview_config_chroma_suppression,
-		preview_enable_chroma_suppression},
-	{OMAP3ISP_PREV_WB, PREV_WB,
+		preview_enable_chroma_suppression,
+		offsetof(struct prev_params, csup),
+		FIELD_SIZEOF(struct prev_params, csup),
+		offsetof(struct omap3isp_prev_update_config, csup),
+	}, /* OMAP3ISP_PREV_WB */ {
 		preview_config_whitebalance,
-		NULL},
-	{OMAP3ISP_PREV_BLKADJ, PREV_BLKADJ,
+		NULL,
+		offsetof(struct prev_params, wbal),
+		FIELD_SIZEOF(struct prev_params, wbal),
+		offsetof(struct omap3isp_prev_update_config, wbal),
+	}, /* OMAP3ISP_PREV_BLKADJ */ {
 		preview_config_blkadj,
-		NULL},
-	{OMAP3ISP_PREV_RGB2RGB, PREV_RGB2RGB,
+		NULL,
+		offsetof(struct prev_params, blkadj),
+		FIELD_SIZEOF(struct prev_params, blkadj),
+		offsetof(struct omap3isp_prev_update_config, blkadj),
+	}, /* OMAP3ISP_PREV_RGB2RGB */ {
 		preview_config_rgb_blending,
-		NULL},
-	{OMAP3ISP_PREV_COLOR_CONV, PREV_COLOR_CONV,
-		preview_config_rgb_to_ycbcr,
-		NULL},
-	{OMAP3ISP_PREV_YC_LIMIT, PREV_YCLIMITS,
+		NULL,
+		offsetof(struct prev_params, rgb2rgb),
+		FIELD_SIZEOF(struct prev_params, rgb2rgb),
+		offsetof(struct omap3isp_prev_update_config, rgb2rgb),
+	}, /* OMAP3ISP_PREV_COLOR_CONV */ {
+		preview_config_csc,
+		NULL,
+		offsetof(struct prev_params, csc),
+		FIELD_SIZEOF(struct prev_params, csc),
+		offsetof(struct omap3isp_prev_update_config, csc),
+	}, /* OMAP3ISP_PREV_YC_LIMIT */ {
 		preview_config_yc_range,
-		NULL},
-	{OMAP3ISP_PREV_DEFECT_COR, PREV_DEFECT_COR,
+		NULL,
+		offsetof(struct prev_params, yclimit),
+		FIELD_SIZEOF(struct prev_params, yclimit),
+		offsetof(struct omap3isp_prev_update_config, yclimit),
+	}, /* OMAP3ISP_PREV_DEFECT_COR */ {
 		preview_config_dcor,
-		preview_enable_dcor},
-	{OMAP3ISP_PREV_GAMMABYPASS, PREV_GAMMA_BYPASS,
+		preview_enable_dcor,
+		offsetof(struct prev_params, dcor),
+		FIELD_SIZEOF(struct prev_params, dcor),
+		offsetof(struct omap3isp_prev_update_config, dcor),
+	}, /* OMAP3ISP_PREV_GAMMABYPASS */ {
 		NULL,
-		preview_enable_gammabypass},
-	{OMAP3ISP_PREV_DRK_FRM_CAPTURE, PREV_DARK_FRAME_CAPTURE,
+		preview_enable_gammabypass,
+	}, /* OMAP3ISP_PREV_DRK_FRM_CAPTURE */ {
 		NULL,
-		preview_enable_drkframe_capture},
-	{OMAP3ISP_PREV_DRK_FRM_SUBTRACT, PREV_DARK_FRAME_SUBTRACT,
+		preview_enable_drkframe_capture,
+	}, /* OMAP3ISP_PREV_DRK_FRM_SUBTRACT */ {
 		NULL,
-		preview_enable_drkframe},
-	{OMAP3ISP_PREV_LENS_SHADING, PREV_LENS_SHADING,
+		preview_enable_drkframe,
+	}, /* OMAP3ISP_PREV_LENS_SHADING */ {
 		preview_config_drkf_shadcomp,
-		preview_enable_drkframe},
-	{OMAP3ISP_PREV_NF, PREV_NOISE_FILTER,
+		preview_enable_drkframe,
+	}, /* OMAP3ISP_PREV_NF */ {
 		preview_config_noisefilter,
-		preview_enable_noisefilter},
-	{OMAP3ISP_PREV_GAMMA, PREV_GAMMA,
+		preview_enable_noisefilter,
+		offsetof(struct prev_params, nf),
+		FIELD_SIZEOF(struct prev_params, nf),
+		offsetof(struct omap3isp_prev_update_config, nf),
+	}, /* OMAP3ISP_PREV_GAMMA */ {
 		preview_config_gammacorrn,
-		NULL},
-	{-1, PREV_CONTRAST,
+		NULL,
+		offsetof(struct prev_params, gamma),
+		FIELD_SIZEOF(struct prev_params, gamma),
+		offsetof(struct omap3isp_prev_update_config, gamma),
+	}, /* OMAP3ISP_PREV_CONTRAST */ {
 		preview_config_contrast,
-		NULL},
-	{-1, PREV_BRIGHTNESS,
+		NULL,
+		offsetof(struct prev_params, contrast),
+		0, true,
+	}, /* OMAP3ISP_PREV_BRIGHTNESS */ {
 		preview_config_brightness,
-		NULL},
+		NULL,
+		offsetof(struct prev_params, brightness),
+		0, true,
+	},
 };
 
 /*
- * __preview_get_ptrs - helper function which return pointers to members
- *                         of params and config structures.
- * @params - pointer to preview_params structure.
- * @param - return pointer to appropriate structure field.
- * @configs - pointer to update config structure.
- * @config - return pointer to appropriate structure field.
- * @bit - for which feature to return pointers.
- * Return size of corresponding prev_params member
- */
-static u32
-__preview_get_ptrs(struct prev_params *params, void **param,
-		   struct omap3isp_prev_update_config *configs,
-		   void __user **config, u32 bit)
-{
-#define CHKARG(cfgs, cfg, field)				\
-	if (cfgs && cfg) {					\
-		*(cfg) = (cfgs)->field;				\
-	}
-
-	switch (bit) {
-	case PREV_HORZ_MEDIAN_FILTER:
-		*param = &params->hmed;
-		CHKARG(configs, config, hmed)
-		return sizeof(params->hmed);
-	case PREV_NOISE_FILTER:
-		*param = &params->nf;
-		CHKARG(configs, config, nf)
-		return sizeof(params->nf);
-		break;
-	case PREV_CFA:
-		*param = &params->cfa;
-		CHKARG(configs, config, cfa)
-		return sizeof(params->cfa);
-	case PREV_LUMA_ENHANCE:
-		*param = &params->luma;
-		CHKARG(configs, config, luma)
-		return sizeof(params->luma);
-	case PREV_CHROMA_SUPPRESS:
-		*param = &params->csup;
-		CHKARG(configs, config, csup)
-		return sizeof(params->csup);
-	case PREV_DEFECT_COR:
-		*param = &params->dcor;
-		CHKARG(configs, config, dcor)
-		return sizeof(params->dcor);
-	case PREV_BLKADJ:
-		*param = &params->blk_adj;
-		CHKARG(configs, config, blkadj)
-		return sizeof(params->blk_adj);
-	case PREV_YCLIMITS:
-		*param = &params->yclimit;
-		CHKARG(configs, config, yclimit)
-		return sizeof(params->yclimit);
-	case PREV_RGB2RGB:
-		*param = &params->rgb2rgb;
-		CHKARG(configs, config, rgb2rgb)
-		return sizeof(params->rgb2rgb);
-	case PREV_COLOR_CONV:
-		*param = &params->rgb2ycbcr;
-		CHKARG(configs, config, csc)
-		return sizeof(params->rgb2ycbcr);
-	case PREV_WB:
-		*param = &params->wbal;
-		CHKARG(configs, config, wbal)
-		return sizeof(params->wbal);
-	case PREV_GAMMA:
-		*param = &params->gamma;
-		CHKARG(configs, config, gamma)
-		return sizeof(params->gamma);
-	case PREV_CONTRAST:
-		*param = &params->contrast;
-		return 0;
-	case PREV_BRIGHTNESS:
-		*param = &params->brightness;
-		return 0;
-	default:
-		*param = NULL;
-		*config = NULL;
-		break;
-	}
-	return 0;
-}
-
-/*
  * preview_config - Copy and update local structure with userspace preview
  *                  configuration.
  * @prev: ISP preview engine
@@ -885,84 +909,103 @@ __preview_get_ptrs(struct prev_params *params, void **param,
 static int preview_config(struct isp_prev_device *prev,
 			  struct omap3isp_prev_update_config *cfg)
 {
-	struct prev_params *params;
-	struct preview_update *attr;
-	int i, bit, rval = 0;
+	unsigned long flags;
+	unsigned int i;
+	int rval = 0;
+	u32 update;
+	u32 active;
 
-	params = &prev->params;
+	if (cfg->update == 0)
+		return 0;
 
-	if (prev->state != ISP_PIPELINE_STREAM_STOPPED) {
-		unsigned long flags;
+	/* Mark the shadow parameters we're going to update as busy. */
+	spin_lock_irqsave(&prev->params.lock, flags);
+	preview_params_lock(prev, cfg->update, true);
+	active = prev->params.active;
+	spin_unlock_irqrestore(&prev->params.lock, flags);
 
-		spin_lock_irqsave(&prev->lock, flags);
-		prev->shadow_update = 1;
-		spin_unlock_irqrestore(&prev->lock, flags);
-	}
+	update = 0;
 
 	for (i = 0; i < ARRAY_SIZE(update_attrs); i++) {
-		attr = &update_attrs[i];
-		bit = 0;
+		const struct preview_update *attr = &update_attrs[i];
+		struct prev_params *params;
+		unsigned int bit = 1 << i;
 
-		if (!(cfg->update & attr->cfg_bit))
+		if (attr->skip || !(cfg->update & bit))
 			continue;
 
-		bit = cfg->flag & attr->cfg_bit;
-		if (bit) {
-			void *to = NULL, __user *from = NULL;
-			unsigned long sz = 0;
+		params = &prev->params.params[!!(active & bit)];
+
+		if (cfg->flag & bit) {
+			void __user *from = *(void * __user *)
+				((void *)cfg + attr->config_offset);
+			void *to = (void *)params + attr->param_offset;
+			size_t size = attr->param_size;
 
-			sz = __preview_get_ptrs(params, &to, cfg, &from,
-						   bit);
-			if (to && from && sz) {
-				if (copy_from_user(to, from, sz)) {
+			if (to && from && size) {
+				if (copy_from_user(to, from, size)) {
 					rval = -EFAULT;
 					break;
 				}
 			}
-			params->features |= attr->feature_bit;
+			params->features |= bit;
 		} else {
-			params->features &= ~attr->feature_bit;
+			params->features &= ~bit;
 		}
 
-		prev->update |= attr->feature_bit;
+		update |= bit;
 	}
 
-	prev->shadow_update = 0;
+	spin_lock_irqsave(&prev->params.lock, flags);
+	preview_params_unlock(prev, update, true);
+	preview_params_switch(prev);
+	spin_unlock_irqrestore(&prev->params.lock, flags);
+
 	return rval;
 }
 
 /*
  * preview_setup_hw - Setup preview registers and/or internal memory
  * @prev: pointer to preview private structure
+ * @update: Bitmask of parameters to setup
+ * @active: Bitmask of parameters active in set 0
  * Note: can be called from interrupt context
  * Return none
  */
-static void preview_setup_hw(struct isp_prev_device *prev)
+static void preview_setup_hw(struct isp_prev_device *prev, u32 update,
+			     u32 active)
 {
-	struct prev_params *params = &prev->params;
-	struct preview_update *attr;
-	int i, bit;
-	void *param_ptr;
+	unsigned int i;
+	u32 features;
+
+	if (update == 0)
+		return;
+
+	features = (prev->params.params[0].features & active)
+		 | (prev->params.params[1].features & ~active);
 
 	for (i = 0; i < ARRAY_SIZE(update_attrs); i++) {
-		attr = &update_attrs[i];
+		const struct preview_update *attr = &update_attrs[i];
+		struct prev_params *params;
+		unsigned int bit = 1 << i;
+		void *param_ptr;
 
-		if (!(prev->update & attr->feature_bit))
+		if (!(update & bit))
 			continue;
-		bit = params->features & attr->feature_bit;
-		if (bit) {
+
+		params = &prev->params.params[!(active & bit)];
+
+		if (params->features & bit) {
 			if (attr->config) {
-				__preview_get_ptrs(params, &param_ptr, NULL,
-						      NULL, bit);
+				param_ptr = (void *)params + attr->param_offset;
 				attr->config(prev, param_ptr);
 			}
 			if (attr->enable)
 				attr->enable(prev, 1);
-		} else
+		} else {
 			if (attr->enable)
 				attr->enable(prev, 0);
-
-		prev->update &= ~attr->feature_bit;
+		}
 	}
 }
 
@@ -1000,13 +1043,17 @@ preview_config_ycpos(struct isp_prev_device *prev,
 static void preview_config_averager(struct isp_prev_device *prev, u8 average)
 {
 	struct isp_device *isp = to_isp_device(prev);
+	struct prev_params *params;
 	int reg = 0;
 
-	if (prev->params.cfa.format == OMAP3ISP_CFAFMT_BAYER)
+	params = (prev->params.active & OMAP3ISP_PREV_CFA)
+	       ? &prev->params.params[0] : &prev->params.params[1];
+
+	if (params->cfa.format == OMAP3ISP_CFAFMT_BAYER)
 		reg = ISPPRV_AVE_EVENDIST_2 << ISPPRV_AVE_EVENDIST_SHIFT |
 		      ISPPRV_AVE_ODDDIST_2 << ISPPRV_AVE_ODDDIST_SHIFT |
 		      average;
-	else if (prev->params.cfa.format == OMAP3ISP_CFAFMT_RGBFOVEON)
+	else if (params->cfa.format == OMAP3ISP_CFAFMT_RGBFOVEON)
 		reg = ISPPRV_AVE_EVENDIST_3 << ISPPRV_AVE_EVENDIST_SHIFT |
 		      ISPPRV_AVE_ODDDIST_3 << ISPPRV_AVE_ODDDIST_SHIFT |
 		      average;
@@ -1014,6 +1061,27 @@ static void preview_config_averager(struct isp_prev_device *prev, u8 average)
 }
 
 /*
+ * preview_config_input_format - Configure the input format
+ * @prev: The preview engine
+ * @format: Format on the preview engine sink pad
+ *
+ * Enable CFA interpolation for Bayer formats and disable it for greyscale
+ * formats.
+ */
+static void preview_config_input_format(struct isp_prev_device *prev,
+					const struct v4l2_mbus_framefmt *format)
+{
+	struct isp_device *isp = to_isp_device(prev);
+
+	if (format->code != V4L2_MBUS_FMT_Y10_1X10)
+		isp_reg_set(isp, OMAP3_ISP_IOMEM_PREV, ISPPRV_PCR,
+			    ISPPRV_PCR_CFAEN);
+	else
+		isp_reg_clr(isp, OMAP3_ISP_IOMEM_PREV, ISPPRV_PCR,
+			    ISPPRV_PCR_CFAEN);
+}
+
+/*
  * preview_config_input_size - Configure the input frame size
  *
  * The preview engine crops several rows and columns internally depending on
@@ -1024,32 +1092,37 @@ static void preview_config_averager(struct isp_prev_device *prev, u8 average)
  *
  * See the explanation at the PREV_MARGIN_* definitions for more details.
  */
-static void preview_config_input_size(struct isp_prev_device *prev)
+static void preview_config_input_size(struct isp_prev_device *prev, u32 active)
 {
+	const struct v4l2_mbus_framefmt *format = &prev->formats[PREV_PAD_SINK];
 	struct isp_device *isp = to_isp_device(prev);
-	struct prev_params *params = &prev->params;
 	unsigned int sph = prev->crop.left;
 	unsigned int eph = prev->crop.left + prev->crop.width - 1;
 	unsigned int slv = prev->crop.top;
 	unsigned int elv = prev->crop.top + prev->crop.height - 1;
+	u32 features;
 
-	if (params->features & PREV_CFA) {
+	if (format->code == V4L2_MBUS_FMT_Y10_1X10) {
 		sph -= 2;
 		eph += 2;
 		slv -= 2;
 		elv += 2;
 	}
-	if (params->features & (PREV_DEFECT_COR | PREV_NOISE_FILTER)) {
+
+	features = (prev->params.params[0].features & active)
+		 | (prev->params.params[1].features & ~active);
+
+	if (features & (OMAP3ISP_PREV_DEFECT_COR | OMAP3ISP_PREV_NF)) {
 		sph -= 2;
 		eph += 2;
 		slv -= 2;
 		elv += 2;
 	}
-	if (params->features & PREV_HORZ_MEDIAN_FILTER) {
+	if (features & OMAP3ISP_PREV_HRZ_MED) {
 		sph -= 2;
 		eph += 2;
 	}
-	if (params->features & (PREV_CHROMA_SUPPRESS | PREV_LUMA_ENHANCE))
+	if (features & (OMAP3ISP_PREV_CHROMA_SUPP | OMAP3ISP_PREV_LUMAENH))
 		sph -= 2;
 
 	isp_reg_writel(isp, (sph << ISPPRV_HORZ_INFO_SPH_SHIFT) | eph,
@@ -1184,8 +1257,16 @@ int omap3isp_preview_busy(struct isp_prev_device *prev)
  */
 void omap3isp_preview_restore_context(struct isp_device *isp)
 {
-	isp->isp_prev.update = PREV_FEATURES_END - 1;
-	preview_setup_hw(&isp->isp_prev);
+	struct isp_prev_device *prev = &isp->isp_prev;
+	const u32 update = OMAP3ISP_PREV_FEATURES_END - 1;
+
+	prev->params.params[0].update = prev->params.active & update;
+	prev->params.params[1].update = ~prev->params.active & update;
+
+	preview_setup_hw(prev, update, prev->params.active);
+
+	prev->params.params[0].update = 0;
+	prev->params.params[1].update = 0;
 }
 
 /*
@@ -1244,12 +1325,21 @@ static void preview_print_status(struct isp_prev_device *prev)
 /*
  * preview_init_params - init image processing parameters.
  * @prev: pointer to previewer private structure
- * return none
  */
 static void preview_init_params(struct isp_prev_device *prev)
 {
-	struct prev_params *params = &prev->params;
-	int i = 0;
+	struct prev_params *params;
+	unsigned int i;
+
+	spin_lock_init(&prev->params.lock);
+
+	prev->params.active = ~0;
+	prev->params.params[0].busy = 0;
+	prev->params.params[0].update = OMAP3ISP_PREV_FEATURES_END - 1;
+	prev->params.params[1].busy = 0;
+	prev->params.params[1].update = 0;
+
+	params = &prev->params.params[0];
 
 	/* Init values */
 	params->contrast = ISPPRV_CONTRAST_DEF * ISPPRV_CONTRAST_UNITS;
@@ -1277,22 +1367,22 @@ static void preview_init_params(struct isp_prev_device *prev)
 	params->wbal.coef1 = FLR_WBAL_COEF;
 	params->wbal.coef2 = FLR_WBAL_COEF;
 	params->wbal.coef3 = FLR_WBAL_COEF;
-	params->blk_adj.red = FLR_BLKADJ_RED;
-	params->blk_adj.green = FLR_BLKADJ_GREEN;
-	params->blk_adj.blue = FLR_BLKADJ_BLUE;
+	params->blkadj.red = FLR_BLKADJ_RED;
+	params->blkadj.green = FLR_BLKADJ_GREEN;
+	params->blkadj.blue = FLR_BLKADJ_BLUE;
 	params->rgb2rgb = flr_rgb2rgb;
-	params->rgb2ycbcr = flr_prev_csc;
+	params->csc = flr_prev_csc;
 	params->yclimit.minC = ISPPRV_YC_MIN;
 	params->yclimit.maxC = ISPPRV_YC_MAX;
 	params->yclimit.minY = ISPPRV_YC_MIN;
 	params->yclimit.maxY = ISPPRV_YC_MAX;
 
-	params->features = PREV_CFA | PREV_DEFECT_COR | PREV_NOISE_FILTER
-			 | PREV_GAMMA | PREV_BLKADJ | PREV_YCLIMITS
-			 | PREV_RGB2RGB | PREV_COLOR_CONV | PREV_WB
-			 | PREV_BRIGHTNESS | PREV_CONTRAST;
-
-	prev->update = PREV_FEATURES_END - 1;
+	params->features = OMAP3ISP_PREV_CFA | OMAP3ISP_PREV_DEFECT_COR
+			 | OMAP3ISP_PREV_NF | OMAP3ISP_PREV_GAMMA
+			 | OMAP3ISP_PREV_BLKADJ | OMAP3ISP_PREV_YC_LIMIT
+			 | OMAP3ISP_PREV_RGB2RGB | OMAP3ISP_PREV_COLOR_CONV
+			 | OMAP3ISP_PREV_WB | OMAP3ISP_PREV_BRIGHTNESS
+			 | OMAP3ISP_PREV_CONTRAST;
 }
 
 /*
@@ -1321,8 +1411,17 @@ static void preview_configure(struct isp_prev_device *prev)
 {
 	struct isp_device *isp = to_isp_device(prev);
 	struct v4l2_mbus_framefmt *format;
+	unsigned long flags;
+	u32 update;
+	u32 active;
 
-	preview_setup_hw(prev);
+	spin_lock_irqsave(&prev->params.lock, flags);
+	/* Mark all active parameters we are going to touch as busy. */
+	update = preview_params_lock(prev, 0, false);
+	active = prev->params.active;
+	spin_unlock_irqrestore(&prev->params.lock, flags);
+
+	preview_setup_hw(prev, update, active);
 
 	if (prev->output & PREVIEW_OUTPUT_MEMORY)
 		isp_reg_set(isp, OMAP3_ISP_IOMEM_PREV, ISPPRV_PCR,
@@ -1343,7 +1442,8 @@ static void preview_configure(struct isp_prev_device *prev)
 
 	preview_adjust_bandwidth(prev);
 
-	preview_config_input_size(prev);
+	preview_config_input_format(prev, format);
+	preview_config_input_size(prev, active);
 
 	if (prev->input == PREVIEW_INPUT_CCDC)
 		preview_config_inlineoffset(prev, 0);
@@ -1360,6 +1460,10 @@ static void preview_configure(struct isp_prev_device *prev)
 
 	preview_config_averager(prev, 0);
 	preview_config_ycpos(prev, format->code);
+
+	spin_lock_irqsave(&prev->params.lock, flags);
+	preview_params_unlock(prev, update, false);
+	spin_unlock_irqrestore(&prev->params.lock, flags);
 }
 
 /* -----------------------------------------------------------------------------
@@ -1448,25 +1552,30 @@ static void preview_isr_buffer(struct isp_prev_device *prev)
 void omap3isp_preview_isr(struct isp_prev_device *prev)
 {
 	unsigned long flags;
+	u32 update;
+	u32 active;
 
 	if (omap3isp_module_sync_is_stopping(&prev->wait, &prev->stopping))
 		return;
 
-	spin_lock_irqsave(&prev->lock, flags);
-	if (prev->shadow_update)
-		goto done;
+	spin_lock_irqsave(&prev->params.lock, flags);
+	preview_params_switch(prev);
+	update = preview_params_lock(prev, 0, false);
+	active = prev->params.active;
+	spin_unlock_irqrestore(&prev->params.lock, flags);
 
-	preview_setup_hw(prev);
-	preview_config_input_size(prev);
-
-done:
-	spin_unlock_irqrestore(&prev->lock, flags);
+	preview_setup_hw(prev, update, active);
+	preview_config_input_size(prev, active);
 
 	if (prev->input == PREVIEW_INPUT_MEMORY ||
 	    prev->output & PREVIEW_OUTPUT_MEMORY)
 		preview_isr_buffer(prev);
 	else if (prev->state == ISP_PIPELINE_STREAM_CONTINUOUS)
 		preview_enable_oneshot(prev);
+
+	spin_lock_irqsave(&prev->params.lock, flags);
+	preview_params_unlock(prev, update, false);
+	spin_unlock_irqrestore(&prev->params.lock, flags);
 }
 
 /* -----------------------------------------------------------------------------
@@ -1552,7 +1661,6 @@ static int preview_set_stream(struct v4l2_subdev *sd, int enable)
 	struct isp_video *video_out = &prev->video_out;
 	struct isp_device *isp = to_isp_device(prev);
 	struct device *dev = to_device(prev);
-	unsigned long flags;
 
 	if (prev->state == ISP_PIPELINE_STREAM_STOPPED) {
 		if (enable == ISP_PIPELINE_STREAM_STOPPED)
@@ -1589,11 +1697,9 @@ static int preview_set_stream(struct v4l2_subdev *sd, int enable)
 		if (omap3isp_module_sync_idle(&sd->entity, &prev->wait,
 					      &prev->stopping))
 			dev_dbg(dev, "%s: stop timeout.\n", sd->name);
-		spin_lock_irqsave(&prev->lock, flags);
 		omap3isp_sbl_disable(isp, OMAP3_ISP_SBL_PREVIEW_READ);
 		omap3isp_sbl_disable(isp, OMAP3_ISP_SBL_PREVIEW_WRITE);
 		omap3isp_subclk_disable(isp, OMAP3_ISP_SUBCLK_PREVIEW);
-		spin_unlock_irqrestore(&prev->lock, flags);
 		isp_video_dmaqueue_flags_clr(video_out);
 		break;
 	}
@@ -1624,6 +1730,7 @@ __preview_get_crop(struct isp_prev_device *prev, struct v4l2_subdev_fh *fh,
 
 /* previewer format descriptions */
 static const unsigned int preview_input_fmts[] = {
+	V4L2_MBUS_FMT_Y10_1X10,
 	V4L2_MBUS_FMT_SGRBG10_1X10,
 	V4L2_MBUS_FMT_SRGGB10_1X10,
 	V4L2_MBUS_FMT_SBGGR10_1X10,
@@ -1822,55 +1929,89 @@ static int preview_enum_frame_size(struct v4l2_subdev *sd,
 }
 
 /*
- * preview_get_crop - Retrieve the crop rectangle on a pad
+ * preview_get_selection - Retrieve a selection rectangle on a pad
  * @sd: ISP preview V4L2 subdevice
  * @fh: V4L2 subdev file handle
- * @crop: crop rectangle
+ * @sel: Selection rectangle
+ *
+ * The only supported rectangles are the crop rectangles on the sink pad.
  *
  * Return 0 on success or a negative error code otherwise.
  */
-static int preview_get_crop(struct v4l2_subdev *sd, struct v4l2_subdev_fh *fh,
-			    struct v4l2_subdev_crop *crop)
+static int preview_get_selection(struct v4l2_subdev *sd,
+				 struct v4l2_subdev_fh *fh,
+				 struct v4l2_subdev_selection *sel)
 {
 	struct isp_prev_device *prev = v4l2_get_subdevdata(sd);
+	struct v4l2_mbus_framefmt *format;
+
+	if (sel->pad != PREV_PAD_SINK)
+		return -EINVAL;
+
+	switch (sel->target) {
+	case V4L2_SUBDEV_SEL_TGT_CROP_BOUNDS:
+		sel->r.left = 0;
+		sel->r.top = 0;
+		sel->r.width = INT_MAX;
+		sel->r.height = INT_MAX;
+
+		format = __preview_get_format(prev, fh, PREV_PAD_SINK,
+					      sel->which);
+		preview_try_crop(prev, format, &sel->r);
+		break;
+
+	case V4L2_SUBDEV_SEL_TGT_CROP_ACTUAL:
+		sel->r = *__preview_get_crop(prev, fh, sel->which);
+		break;
 
-	/* Cropping is only supported on the sink pad. */
-	if (crop->pad != PREV_PAD_SINK)
+	default:
 		return -EINVAL;
+	}
 
-	crop->rect = *__preview_get_crop(prev, fh, crop->which);
 	return 0;
 }
 
 /*
- * preview_set_crop - Retrieve the crop rectangle on a pad
+ * preview_set_selection - Set a selection rectangle on a pad
  * @sd: ISP preview V4L2 subdevice
  * @fh: V4L2 subdev file handle
- * @crop: crop rectangle
+ * @sel: Selection rectangle
+ *
+ * The only supported rectangle is the actual crop rectangle on the sink pad.
  *
  * Return 0 on success or a negative error code otherwise.
  */
-static int preview_set_crop(struct v4l2_subdev *sd, struct v4l2_subdev_fh *fh,
-			    struct v4l2_subdev_crop *crop)
+static int preview_set_selection(struct v4l2_subdev *sd,
+				 struct v4l2_subdev_fh *fh,
+				 struct v4l2_subdev_selection *sel)
 {
 	struct isp_prev_device *prev = v4l2_get_subdevdata(sd);
 	struct v4l2_mbus_framefmt *format;
 
-	/* Cropping is only supported on the sink pad. */
-	if (crop->pad != PREV_PAD_SINK)
+	if (sel->target != V4L2_SUBDEV_SEL_TGT_CROP_ACTUAL ||
+	    sel->pad != PREV_PAD_SINK)
 		return -EINVAL;
 
 	/* The crop rectangle can't be changed while streaming. */
 	if (prev->state != ISP_PIPELINE_STREAM_STOPPED)
 		return -EBUSY;
 
-	format = __preview_get_format(prev, fh, PREV_PAD_SINK, crop->which);
-	preview_try_crop(prev, format, &crop->rect);
-	*__preview_get_crop(prev, fh, crop->which) = crop->rect;
+	/* Modifying the crop rectangle always changes the format on the source
+	 * pad. If the KEEP_CONFIG flag is set, just return the current crop
+	 * rectangle.
+	 */
+	if (sel->flags & V4L2_SUBDEV_SEL_FLAG_KEEP_CONFIG) {
+		sel->r = *__preview_get_crop(prev, fh, sel->which);
+		return 0;
+	}
+
+	format = __preview_get_format(prev, fh, PREV_PAD_SINK, sel->which);
+	preview_try_crop(prev, format, &sel->r);
+	*__preview_get_crop(prev, fh, sel->which) = sel->r;
 
 	/* Update the source format. */
-	format = __preview_get_format(prev, fh, PREV_PAD_SOURCE, crop->which);
-	preview_try_format(prev, fh, PREV_PAD_SOURCE, format, crop->which);
+	format = __preview_get_format(prev, fh, PREV_PAD_SOURCE, sel->which);
+	preview_try_format(prev, fh, PREV_PAD_SOURCE, format, sel->which);
 
 	return 0;
 }
@@ -1979,8 +2120,8 @@ static const struct v4l2_subdev_pad_ops preview_v4l2_pad_ops = {
 	.enum_frame_size = preview_enum_frame_size,
 	.get_fmt = preview_get_format,
 	.set_fmt = preview_set_format,
-	.get_crop = preview_get_crop,
-	.set_crop = preview_set_crop,
+	.get_selection = preview_get_selection,
+	.set_selection = preview_set_selection,
 };
 
 /* subdev operations */
@@ -2076,6 +2217,7 @@ static int preview_link_setup(struct media_entity *entity,
 /* media operations */
 static const struct media_entity_operations preview_media_ops = {
 	.link_setup = preview_link_setup,
+	.link_validate = v4l2_subdev_link_validate,
 };
 
 void omap3isp_preview_unregister_entities(struct isp_prev_device *prev)
@@ -2201,7 +2343,7 @@ error_video_in:
 }
 
 /*
- * isp_preview_init - Previewer initialization.
+ * omap3isp_preview_init - Previewer initialization.
  * @dev : Pointer to ISP device
  * return -ENOMEM or zero on success
  */
@@ -2209,8 +2351,8 @@ int omap3isp_preview_init(struct isp_device *isp)
 {
 	struct isp_prev_device *prev = &isp->isp_prev;
 
-	spin_lock_init(&prev->lock);
 	init_waitqueue_head(&prev->wait);
+
 	preview_init_params(prev);
 
 	return preview_init_entities(prev);
diff --git a/drivers/media/video/omap3isp/isppreview.h b/drivers/media/video/omap3isp/isppreview.h
index 09686607973..6663ab64e4b 100644
--- a/drivers/media/video/omap3isp/isppreview.h
+++ b/drivers/media/video/omap3isp/isppreview.h
@@ -45,29 +45,10 @@
 #define ISPPRV_CONTRAST_HIGH		0xFF
 #define ISPPRV_CONTRAST_UNITS		0x1
 
-/* Features list */
-#define PREV_LUMA_ENHANCE		OMAP3ISP_PREV_LUMAENH
-#define PREV_INVERSE_ALAW		OMAP3ISP_PREV_INVALAW
-#define PREV_HORZ_MEDIAN_FILTER		OMAP3ISP_PREV_HRZ_MED
-#define PREV_CFA			OMAP3ISP_PREV_CFA
-#define PREV_CHROMA_SUPPRESS		OMAP3ISP_PREV_CHROMA_SUPP
-#define PREV_WB				OMAP3ISP_PREV_WB
-#define PREV_BLKADJ			OMAP3ISP_PREV_BLKADJ
-#define PREV_RGB2RGB			OMAP3ISP_PREV_RGB2RGB
-#define PREV_COLOR_CONV			OMAP3ISP_PREV_COLOR_CONV
-#define PREV_YCLIMITS			OMAP3ISP_PREV_YC_LIMIT
-#define PREV_DEFECT_COR			OMAP3ISP_PREV_DEFECT_COR
-#define PREV_GAMMA_BYPASS		OMAP3ISP_PREV_GAMMABYPASS
-#define PREV_DARK_FRAME_CAPTURE		OMAP3ISP_PREV_DRK_FRM_CAPTURE
-#define PREV_DARK_FRAME_SUBTRACT	OMAP3ISP_PREV_DRK_FRM_SUBTRACT
-#define PREV_LENS_SHADING		OMAP3ISP_PREV_LENS_SHADING
-#define PREV_NOISE_FILTER		OMAP3ISP_PREV_NF
-#define PREV_GAMMA			OMAP3ISP_PREV_GAMMA
-
-#define PREV_CONTRAST			(1 << 17)
-#define PREV_BRIGHTNESS			(1 << 18)
-#define PREV_AVERAGER			(1 << 19)
-#define PREV_FEATURES_END		(1 << 20)
+/* Additional features not listed in linux/omap3isp.h */
+#define OMAP3ISP_PREV_CONTRAST		(1 << 17)
+#define OMAP3ISP_PREV_BRIGHTNESS	(1 << 18)
+#define OMAP3ISP_PREV_FEATURES_END	(1 << 19)
 
 enum preview_input_entity {
 	PREVIEW_INPUT_NONE,
@@ -88,6 +69,8 @@ enum preview_ycpos_mode {
 
 /*
  * struct prev_params - Structure for all configuration
+ * @busy: Bitmask of busy parameters (being updated or used)
+ * @update: Bitmask of the parameters to be updated
  * @features: Set of features enabled.
  * @cfa: CFA coefficients.
  * @csup: Chroma suppression coefficients.
@@ -96,15 +79,17 @@ enum preview_ycpos_mode {
  * @dcor: Noise filter coefficients.
  * @gamma: Gamma coefficients.
  * @wbal: White Balance parameters.
- * @blk_adj: Black adjustment parameters.
+ * @blkadj: Black adjustment parameters.
  * @rgb2rgb: RGB blending parameters.
- * @rgb2ycbcr: RGB to ycbcr parameters.
+ * @csc: Color space conversion (RGB to YCbCr) parameters.
  * @hmed: Horizontal median filter.
  * @yclimit: YC limits parameters.
  * @contrast: Contrast.
  * @brightness: Brightness.
  */
 struct prev_params {
+	u32 busy;
+	u32 update;
 	u32 features;
 	struct omap3isp_prev_cfa cfa;
 	struct omap3isp_prev_csup csup;
@@ -113,35 +98,15 @@ struct prev_params {
 	struct omap3isp_prev_dcor dcor;
 	struct omap3isp_prev_gtables gamma;
 	struct omap3isp_prev_wbal wbal;
-	struct omap3isp_prev_blkadj blk_adj;
+	struct omap3isp_prev_blkadj blkadj;
 	struct omap3isp_prev_rgbtorgb rgb2rgb;
-	struct omap3isp_prev_csc rgb2ycbcr;
+	struct omap3isp_prev_csc csc;
 	struct omap3isp_prev_hmed hmed;
 	struct omap3isp_prev_yclimit yclimit;
 	u8 contrast;
 	u8 brightness;
 };
 
-/*
- * struct isptables_update - Structure for Table Configuration.
- * @update: Specifies which tables should be updated.
- * @flag: Specifies which tables should be enabled.
- * @nf: Pointer to structure for Noise Filter
- * @lsc: Pointer to LSC gain table. (currently not used)
- * @gamma: Pointer to gamma correction tables.
- * @cfa: Pointer to color filter array configuration.
- * @wbal: Pointer to colour and digital gain configuration.
- */
-struct isptables_update {
-	u32 update;
-	u32 flag;
-	struct omap3isp_prev_nf *nf;
-	u32 *lsc;
-	struct omap3isp_prev_gtables *gamma;
-	struct omap3isp_prev_cfa *cfa;
-	struct omap3isp_prev_wbal *wbal;
-};
-
 /* Sink and source previewer pads */
 #define PREV_PAD_SINK			0
 #define PREV_PAD_SOURCE			1
@@ -157,12 +122,11 @@ struct isptables_update {
  * @output: Bitmask of the active output
  * @video_in: Input video entity
  * @video_out: Output video entity
- * @params: Module configuration data
- * @shadow_update: If set, update the hardware configured in the next interrupt
+ * @params.params : Active and shadow parameters sets
+ * @params.active: Bitmask of parameters active in set 0
+ * @params.lock: Parameters lock, protects params.active and params.shadow
  * @underrun: Whether the preview entity has queued buffers on the output
  * @state: Current preview pipeline state
- * @lock: Shadow update lock
- * @update: Bitmask of the parameters to be updated
  *
  * This structure is used to store the OMAP ISP Preview module Information.
  */
@@ -179,13 +143,15 @@ struct isp_prev_device {
 	struct isp_video video_in;
 	struct isp_video video_out;
 
-	struct prev_params params;
-	unsigned int shadow_update:1;
+	struct {
+		struct prev_params params[2];
+		u32 active;
+		spinlock_t lock;
+	} params;
+
 	enum isp_pipeline_stream_state state;
 	wait_queue_head_t wait;
 	atomic_t stopping;
-	spinlock_t lock;
-	u32 update;
 };
 
 struct isp_device;
diff --git a/drivers/media/video/omap3isp/ispqueue.h b/drivers/media/video/omap3isp/ispqueue.h
index 92c5a12157d..908dfd712e8 100644
--- a/drivers/media/video/omap3isp/ispqueue.h
+++ b/drivers/media/video/omap3isp/ispqueue.h
@@ -90,7 +90,7 @@ struct isp_video_buffer {
 	void *vaddr;
 
 	/* For userspace buffers. */
-	unsigned long vm_flags;
+	vm_flags_t vm_flags;
 	unsigned long offset;
 	unsigned int npages;
 	struct page **pages;
diff --git a/drivers/media/video/omap3isp/ispresizer.c b/drivers/media/video/omap3isp/ispresizer.c
index 6958a9e3dc2..14041c9c864 100644
--- a/drivers/media/video/omap3isp/ispresizer.c
+++ b/drivers/media/video/omap3isp/ispresizer.c
@@ -1188,32 +1188,6 @@ static int resizer_set_stream(struct v4l2_subdev *sd, int enable)
 }
 
 /*
- * resizer_g_crop - handle get crop subdev operation
- * @sd : pointer to v4l2 subdev structure
- * @pad : subdev pad
- * @crop : pointer to crop structure
- * @which : active or try format
- * return zero
- */
-static int resizer_g_crop(struct v4l2_subdev *sd, struct v4l2_subdev_fh *fh,
-			  struct v4l2_subdev_crop *crop)
-{
-	struct isp_res_device *res = v4l2_get_subdevdata(sd);
-	struct v4l2_mbus_framefmt *format;
-	struct resizer_ratio ratio;
-
-	/* Only sink pad has crop capability */
-	if (crop->pad != RESZ_PAD_SINK)
-		return -EINVAL;
-
-	format = __resizer_get_format(res, fh, RESZ_PAD_SOURCE, crop->which);
-	crop->rect = *__resizer_get_crop(res, fh, crop->which);
-	resizer_calc_ratios(res, &crop->rect, format, &ratio);
-
-	return 0;
-}
-
-/*
  * resizer_try_crop - mangles crop parameters.
  */
 static void resizer_try_crop(const struct v4l2_mbus_framefmt *sink,
@@ -1223,7 +1197,7 @@ static void resizer_try_crop(const struct v4l2_mbus_framefmt *sink,
 	const unsigned int spv = DEFAULT_PHASE;
 	const unsigned int sph = DEFAULT_PHASE;
 
-	/* Crop rectangle is constrained to the output size so that zoom ratio
+	/* Crop rectangle is constrained by the output size so that zoom ratio
 	 * cannot exceed +/-4.0.
 	 */
 	unsigned int min_width =
@@ -1248,51 +1222,115 @@ static void resizer_try_crop(const struct v4l2_mbus_framefmt *sink,
 }
 
 /*
- * resizer_s_crop - handle set crop subdev operation
- * @sd : pointer to v4l2 subdev structure
- * @pad : subdev pad
- * @crop : pointer to crop structure
- * @which : active or try format
- * return -EINVAL or zero when succeed
+ * resizer_get_selection - Retrieve a selection rectangle on a pad
+ * @sd: ISP resizer V4L2 subdevice
+ * @fh: V4L2 subdev file handle
+ * @sel: Selection rectangle
+ *
+ * The only supported rectangles are the crop rectangles on the sink pad.
+ *
+ * Return 0 on success or a negative error code otherwise.
  */
-static int resizer_s_crop(struct v4l2_subdev *sd, struct v4l2_subdev_fh *fh,
-			  struct v4l2_subdev_crop *crop)
+static int resizer_get_selection(struct v4l2_subdev *sd,
+				 struct v4l2_subdev_fh *fh,
+				 struct v4l2_subdev_selection *sel)
+{
+	struct isp_res_device *res = v4l2_get_subdevdata(sd);
+	struct v4l2_mbus_framefmt *format_source;
+	struct v4l2_mbus_framefmt *format_sink;
+	struct resizer_ratio ratio;
+
+	if (sel->pad != RESZ_PAD_SINK)
+		return -EINVAL;
+
+	format_sink = __resizer_get_format(res, fh, RESZ_PAD_SINK,
+					   sel->which);
+	format_source = __resizer_get_format(res, fh, RESZ_PAD_SOURCE,
+					     sel->which);
+
+	switch (sel->target) {
+	case V4L2_SUBDEV_SEL_TGT_CROP_BOUNDS:
+		sel->r.left = 0;
+		sel->r.top = 0;
+		sel->r.width = INT_MAX;
+		sel->r.height = INT_MAX;
+
+		resizer_try_crop(format_sink, format_source, &sel->r);
+		resizer_calc_ratios(res, &sel->r, format_source, &ratio);
+		break;
+
+	case V4L2_SUBDEV_SEL_TGT_CROP_ACTUAL:
+		sel->r = *__resizer_get_crop(res, fh, sel->which);
+		resizer_calc_ratios(res, &sel->r, format_source, &ratio);
+		break;
+
+	default:
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+/*
+ * resizer_set_selection - Set a selection rectangle on a pad
+ * @sd: ISP resizer V4L2 subdevice
+ * @fh: V4L2 subdev file handle
+ * @sel: Selection rectangle
+ *
+ * The only supported rectangle is the actual crop rectangle on the sink pad.
+ *
+ * FIXME: This function currently behaves as if the KEEP_CONFIG selection flag
+ * was always set.
+ *
+ * Return 0 on success or a negative error code otherwise.
+ */
+static int resizer_set_selection(struct v4l2_subdev *sd,
+				 struct v4l2_subdev_fh *fh,
+				 struct v4l2_subdev_selection *sel)
 {
 	struct isp_res_device *res = v4l2_get_subdevdata(sd);
 	struct isp_device *isp = to_isp_device(res);
 	struct v4l2_mbus_framefmt *format_sink, *format_source;
 	struct resizer_ratio ratio;
 
-	/* Only sink pad has crop capability */
-	if (crop->pad != RESZ_PAD_SINK)
+	if (sel->target != V4L2_SUBDEV_SEL_TGT_CROP_ACTUAL ||
+	    sel->pad != RESZ_PAD_SINK)
 		return -EINVAL;
 
 	format_sink = __resizer_get_format(res, fh, RESZ_PAD_SINK,
-					   crop->which);
+					   sel->which);
 	format_source = __resizer_get_format(res, fh, RESZ_PAD_SOURCE,
-					     crop->which);
+					     sel->which);
 
 	dev_dbg(isp->dev, "%s: L=%d,T=%d,W=%d,H=%d,which=%d\n", __func__,
-		crop->rect.left, crop->rect.top, crop->rect.width,
-		crop->rect.height, crop->which);
+		sel->r.left, sel->r.top, sel->r.width, sel->r.height,
+		sel->which);
 
 	dev_dbg(isp->dev, "%s: input=%dx%d, output=%dx%d\n", __func__,
 		format_sink->width, format_sink->height,
 		format_source->width, format_source->height);
 
-	resizer_try_crop(format_sink, format_source, &crop->rect);
-	*__resizer_get_crop(res, fh, crop->which) = crop->rect;
-	resizer_calc_ratios(res, &crop->rect, format_source, &ratio);
+	/* Clamp the crop rectangle to the bounds, and then mangle it further to
+	 * fulfill the TRM equations. Store the clamped but otherwise unmangled
+	 * rectangle to avoid cropping the input multiple times: when an
+	 * application sets the output format, the current crop rectangle is
+	 * mangled during crop rectangle computation, which would lead to a new,
+	 * smaller input crop rectangle every time the output size is set if we
+	 * stored the mangled rectangle.
+	 */
+	resizer_try_crop(format_sink, format_source, &sel->r);
+	*__resizer_get_crop(res, fh, sel->which) = sel->r;
+	resizer_calc_ratios(res, &sel->r, format_source, &ratio);
 
-	if (crop->which == V4L2_SUBDEV_FORMAT_TRY)
+	if (sel->which == V4L2_SUBDEV_FORMAT_TRY)
 		return 0;
 
 	res->ratio = ratio;
-	res->crop.active = crop->rect;
+	res->crop.active = sel->r;
 
 	/*
-	 * s_crop can be called while streaming is on. In this case
-	 * the crop values will be set in the next IRQ.
+	 * set_selection can be called while streaming is on. In this case the
+	 * crop values will be set in the next IRQ.
 	 */
 	if (res->state != ISP_PIPELINE_STREAM_STOPPED)
 		res->applycrop = 1;
@@ -1530,8 +1568,8 @@ static const struct v4l2_subdev_pad_ops resizer_v4l2_pad_ops = {
 	.enum_frame_size = resizer_enum_frame_size,
 	.get_fmt = resizer_get_format,
 	.set_fmt = resizer_set_format,
-	.get_crop = resizer_g_crop,
-	.set_crop = resizer_s_crop,
+	.get_selection = resizer_get_selection,
+	.set_selection = resizer_set_selection,
 };
 
 /* subdev operations */
@@ -1603,6 +1641,7 @@ static int resizer_link_setup(struct media_entity *entity,
 /* media operations */
 static const struct media_entity_operations resizer_media_ops = {
 	.link_setup = resizer_link_setup,
+	.link_validate = v4l2_subdev_link_validate,
 };
 
 void omap3isp_resizer_unregister_entities(struct isp_res_device *res)
diff --git a/drivers/media/video/omap3isp/ispstat.c b/drivers/media/video/omap3isp/ispstat.c
index 11871ecc6d2..b8640be692f 100644
--- a/drivers/media/video/omap3isp/ispstat.c
+++ b/drivers/media/video/omap3isp/ispstat.c
@@ -1032,7 +1032,7 @@ int omap3isp_stat_subscribe_event(struct v4l2_subdev *subdev,
 	if (sub->type != stat->event_type)
 		return -EINVAL;
 
-	return v4l2_event_subscribe(fh, sub, STAT_NEVENTS);
+	return v4l2_event_subscribe(fh, sub, STAT_NEVENTS, NULL);
 }
 
 int omap3isp_stat_unsubscribe_event(struct v4l2_subdev *subdev,
diff --git a/drivers/media/video/omap3isp/ispvideo.c b/drivers/media/video/omap3isp/ispvideo.c
index b0207005772..b37379d39cd 100644
--- a/drivers/media/video/omap3isp/ispvideo.c
+++ b/drivers/media/video/omap3isp/ispvideo.c
@@ -46,6 +46,10 @@
  * Helper functions
  */
 
+/*
+ * NOTE: When adding new media bus codes, always remember to add
+ * corresponding in-memory formats to the table below!!!
+ */
 static struct isp_format_info formats[] = {
 	{ V4L2_MBUS_FMT_Y8_1X8, V4L2_MBUS_FMT_Y8_1X8,
 	  V4L2_MBUS_FMT_Y8_1X8, V4L2_MBUS_FMT_Y8_1X8,
@@ -68,9 +72,18 @@ static struct isp_format_info formats[] = {
 	{ V4L2_MBUS_FMT_SRGGB8_1X8, V4L2_MBUS_FMT_SRGGB8_1X8,
 	  V4L2_MBUS_FMT_SRGGB8_1X8, V4L2_MBUS_FMT_SRGGB8_1X8,
 	  V4L2_PIX_FMT_SRGGB8, 8, },
+	{ V4L2_MBUS_FMT_SBGGR10_DPCM8_1X8, V4L2_MBUS_FMT_SBGGR10_DPCM8_1X8,
+	  V4L2_MBUS_FMT_SBGGR10_1X10, 0,
+	  V4L2_PIX_FMT_SBGGR10DPCM8, 8, },
+	{ V4L2_MBUS_FMT_SGBRG10_DPCM8_1X8, V4L2_MBUS_FMT_SGBRG10_DPCM8_1X8,
+	  V4L2_MBUS_FMT_SGBRG10_1X10, 0,
+	  V4L2_PIX_FMT_SGBRG10DPCM8, 8, },
 	{ V4L2_MBUS_FMT_SGRBG10_DPCM8_1X8, V4L2_MBUS_FMT_SGRBG10_DPCM8_1X8,
 	  V4L2_MBUS_FMT_SGRBG10_1X10, 0,
 	  V4L2_PIX_FMT_SGRBG10DPCM8, 8, },
+	{ V4L2_MBUS_FMT_SRGGB10_DPCM8_1X8, V4L2_MBUS_FMT_SRGGB10_DPCM8_1X8,
+	  V4L2_MBUS_FMT_SRGGB10_1X10, 0,
+	  V4L2_PIX_FMT_SRGGB10DPCM8, 8, },
 	{ V4L2_MBUS_FMT_SBGGR10_1X10, V4L2_MBUS_FMT_SBGGR10_1X10,
 	  V4L2_MBUS_FMT_SBGGR10_1X10, V4L2_MBUS_FMT_SBGGR8_1X8,
 	  V4L2_PIX_FMT_SBGGR10, 10, },
@@ -117,37 +130,6 @@ omap3isp_video_format_info(enum v4l2_mbus_pixelcode code)
 }
 
 /*
- * Decide whether desired output pixel code can be obtained with
- * the lane shifter by shifting the input pixel code.
- * @in: input pixelcode to shifter
- * @out: output pixelcode from shifter
- * @additional_shift: # of bits the sensor's LSB is offset from CAMEXT[0]
- *
- * return true if the combination is possible
- * return false otherwise
- */
-static bool isp_video_is_shiftable(enum v4l2_mbus_pixelcode in,
-		enum v4l2_mbus_pixelcode out,
-		unsigned int additional_shift)
-{
-	const struct isp_format_info *in_info, *out_info;
-
-	if (in == out)
-		return true;
-
-	in_info = omap3isp_video_format_info(in);
-	out_info = omap3isp_video_format_info(out);
-
-	if ((in_info->flavor == 0) || (out_info->flavor == 0))
-		return false;
-
-	if (in_info->flavor != out_info->flavor)
-		return false;
-
-	return in_info->bpp - out_info->bpp + additional_shift <= 6;
-}
-
-/*
  * isp_video_mbus_to_pix - Convert v4l2_mbus_framefmt to v4l2_pix_format
  * @video: ISP video instance
  * @mbus: v4l2_mbus_framefmt format (input)
@@ -242,8 +224,8 @@ isp_video_remote_subdev(struct isp_video *video, u32 *pad)
 }
 
 /* Return a pointer to the ISP video instance at the far end of the pipeline. */
-static struct isp_video *
-isp_video_far_end(struct isp_video *video)
+static int isp_video_get_graph_data(struct isp_video *video,
+				    struct isp_pipeline *pipe)
 {
 	struct media_entity_graph graph;
 	struct media_entity *entity = &video->video.entity;
@@ -254,21 +236,38 @@ isp_video_far_end(struct isp_video *video)
 	media_entity_graph_walk_start(&graph, entity);
 
 	while ((entity = media_entity_graph_walk_next(&graph))) {
+		struct isp_video *__video;
+
+		pipe->entities |= 1 << entity->id;
+
+		if (far_end != NULL)
+			continue;
+
 		if (entity == &video->video.entity)
 			continue;
 
 		if (media_entity_type(entity) != MEDIA_ENT_T_DEVNODE)
 			continue;
 
-		far_end = to_isp_video(media_entity_to_video_device(entity));
-		if (far_end->type != video->type)
-			break;
-
-		far_end = NULL;
+		__video = to_isp_video(media_entity_to_video_device(entity));
+		if (__video->type != video->type)
+			far_end = __video;
 	}
 
 	mutex_unlock(&mdev->graph_mutex);
-	return far_end;
+
+	if (video->type == V4L2_BUF_TYPE_VIDEO_CAPTURE) {
+		pipe->input = far_end;
+		pipe->output = video;
+	} else {
+		if (far_end == NULL)
+			return -EPIPE;
+
+		pipe->input = video;
+		pipe->output = far_end;
+	}
+
+	return 0;
 }
 
 /*
@@ -285,52 +284,24 @@ isp_video_far_end(struct isp_video *video)
 static int isp_video_validate_pipeline(struct isp_pipeline *pipe)
 {
 	struct isp_device *isp = pipe->output->isp;
-	struct v4l2_subdev_format fmt_source;
-	struct v4l2_subdev_format fmt_sink;
 	struct media_pad *pad;
 	struct v4l2_subdev *subdev;
-	int ret;
-
-	pipe->max_rate = pipe->l3_ick;
 
 	subdev = isp_video_remote_subdev(pipe->output, NULL);
 	if (subdev == NULL)
 		return -EPIPE;
 
 	while (1) {
-		unsigned int shifter_link;
 		/* Retrieve the sink format */
 		pad = &subdev->entity.pads[0];
 		if (!(pad->flags & MEDIA_PAD_FL_SINK))
 			break;
 
-		fmt_sink.pad = pad->index;
-		fmt_sink.which = V4L2_SUBDEV_FORMAT_ACTIVE;
-		ret = v4l2_subdev_call(subdev, pad, get_fmt, NULL, &fmt_sink);
-		if (ret < 0 && ret != -ENOIOCTLCMD)
-			return -EPIPE;
-
 		/* Update the maximum frame rate */
 		if (subdev == &isp->isp_res.subdev)
 			omap3isp_resizer_max_rate(&isp->isp_res,
 						  &pipe->max_rate);
 
-		/* Check ccdc maximum data rate when data comes from sensor
-		 * TODO: Include ccdc rate in pipe->max_rate and compare the
-		 *       total pipe rate with the input data rate from sensor.
-		 */
-		if (subdev == &isp->isp_ccdc.subdev && pipe->input == NULL) {
-			unsigned int rate = UINT_MAX;
-
-			omap3isp_ccdc_max_rate(&isp->isp_ccdc, &rate);
-			if (isp->isp_ccdc.vpcfg.pixelclk > rate)
-				return -ENOSPC;
-		}
-
-		/* If sink pad is on CCDC, the link has the lane shifter
-		 * in the middle of it. */
-		shifter_link = subdev == &isp->isp_ccdc.subdev;
-
 		/* Retrieve the source format. Return an error if no source
 		 * entity can be found, and stop checking the pipeline if the
 		 * source entity isn't a subdev.
@@ -343,32 +314,6 @@ static int isp_video_validate_pipeline(struct isp_pipeline *pipe)
 			break;
 
 		subdev = media_entity_to_v4l2_subdev(pad->entity);
-
-		fmt_source.pad = pad->index;
-		fmt_source.which = V4L2_SUBDEV_FORMAT_ACTIVE;
-		ret = v4l2_subdev_call(subdev, pad, get_fmt, NULL, &fmt_source);
-		if (ret < 0 && ret != -ENOIOCTLCMD)
-			return -EPIPE;
-
-		/* Check if the two ends match */
-		if (fmt_source.format.width != fmt_sink.format.width ||
-		    fmt_source.format.height != fmt_sink.format.height)
-			return -EPIPE;
-
-		if (shifter_link) {
-			unsigned int parallel_shift = 0;
-			if (isp->isp_ccdc.input == CCDC_INPUT_PARALLEL) {
-				struct isp_parallel_platform_data *pdata =
-					&((struct isp_v4l2_subdevs_group *)
-					      subdev->host_priv)->bus.parallel;
-				parallel_shift = pdata->data_lane_shift * 2;
-			}
-			if (!isp_video_is_shiftable(fmt_source.format.code,
-						fmt_sink.format.code,
-						parallel_shift))
-				return -EPIPE;
-		} else if (fmt_source.format.code != fmt_sink.format.code)
-			return -EPIPE;
 	}
 
 	return 0;
@@ -923,6 +868,92 @@ isp_video_dqbuf(struct file *file, void *fh, struct v4l2_buffer *b)
 					  file->f_flags & O_NONBLOCK);
 }
 
+static int isp_video_check_external_subdevs(struct isp_video *video,
+					    struct isp_pipeline *pipe)
+{
+	struct isp_device *isp = video->isp;
+	struct media_entity *ents[] = {
+		&isp->isp_csi2a.subdev.entity,
+		&isp->isp_csi2c.subdev.entity,
+		&isp->isp_ccp2.subdev.entity,
+		&isp->isp_ccdc.subdev.entity
+	};
+	struct media_pad *source_pad;
+	struct media_entity *source = NULL;
+	struct media_entity *sink;
+	struct v4l2_subdev_format fmt;
+	struct v4l2_ext_controls ctrls;
+	struct v4l2_ext_control ctrl;
+	unsigned int i;
+	int ret = 0;
+
+	for (i = 0; i < ARRAY_SIZE(ents); i++) {
+		/* Is the entity part of the pipeline? */
+		if (!(pipe->entities & (1 << ents[i]->id)))
+			continue;
+
+		/* ISP entities have always sink pad == 0. Find source. */
+		source_pad = media_entity_remote_source(&ents[i]->pads[0]);
+		if (source_pad == NULL)
+			continue;
+
+		source = source_pad->entity;
+		sink = ents[i];
+		break;
+	}
+
+	if (!source) {
+		dev_warn(isp->dev, "can't find source, failing now\n");
+		return ret;
+	}
+
+	if (media_entity_type(source) != MEDIA_ENT_T_V4L2_SUBDEV)
+		return 0;
+
+	pipe->external = media_entity_to_v4l2_subdev(source);
+
+	fmt.pad = source_pad->index;
+	fmt.which = V4L2_SUBDEV_FORMAT_ACTIVE;
+	ret = v4l2_subdev_call(media_entity_to_v4l2_subdev(sink),
+			       pad, get_fmt, NULL, &fmt);
+	if (unlikely(ret < 0)) {
+		dev_warn(isp->dev, "get_fmt returned null!\n");
+		return ret;
+	}
+
+	pipe->external_bpp = omap3isp_video_format_info(fmt.format.code)->bpp;
+
+	memset(&ctrls, 0, sizeof(ctrls));
+	memset(&ctrl, 0, sizeof(ctrl));
+
+	ctrl.id = V4L2_CID_PIXEL_RATE;
+
+	ctrls.count = 1;
+	ctrls.controls = &ctrl;
+
+	ret = v4l2_g_ext_ctrls(pipe->external->ctrl_handler, &ctrls);
+	if (ret < 0) {
+		dev_warn(isp->dev, "no pixel rate control in subdev %s\n",
+			 pipe->external->name);
+		return ret;
+	}
+
+	pipe->external_rate = ctrl.value64;
+
+	if (pipe->entities & (1 << isp->isp_ccdc.subdev.entity.id)) {
+		unsigned int rate = UINT_MAX;
+		/*
+		 * Check that maximum allowed CCDC pixel rate isn't
+		 * exceeded by the pixel rate.
+		 */
+		omap3isp_ccdc_max_rate(&isp->isp_ccdc, &rate);
+		if (pipe->external_rate > rate)
+			return -ENOSPC;
+	}
+
+	return 0;
+}
+
 /*
  * Stream management
  *
@@ -961,7 +992,6 @@ isp_video_streamon(struct file *file, void *fh, enum v4l2_buf_type type)
 	struct isp_video *video = video_drvdata(file);
 	enum isp_pipeline_state state;
 	struct isp_pipeline *pipe;
-	struct isp_video *far_end;
 	unsigned long flags;
 	int ret;
 
@@ -980,46 +1010,45 @@ isp_video_streamon(struct file *file, void *fh, enum v4l2_buf_type type)
 	 */
 	pipe = video->video.entity.pipe
 	     ? to_isp_pipeline(&video->video.entity) : &video->pipe;
-	media_entity_pipeline_start(&video->video.entity, &pipe->pipe);
+
+	pipe->entities = 0;
+
+	if (video->isp->pdata->set_constraints)
+		video->isp->pdata->set_constraints(video->isp, true);
+	pipe->l3_ick = clk_get_rate(video->isp->clock[ISP_CLK_L3_ICK]);
+	pipe->max_rate = pipe->l3_ick;
+
+	ret = media_entity_pipeline_start(&video->video.entity, &pipe->pipe);
+	if (ret < 0)
+		goto err_pipeline_start;
 
 	/* Verify that the currently configured format matches the output of
 	 * the connected subdev.
 	 */
 	ret = isp_video_check_format(video, vfh);
 	if (ret < 0)
-		goto error;
+		goto err_check_format;
 
 	video->bpl_padding = ret;
 	video->bpl_value = vfh->format.fmt.pix.bytesperline;
 
-	/* Find the ISP video node connected at the far end of the pipeline and
-	 * update the pipeline.
-	 */
-	far_end = isp_video_far_end(video);
+	ret = isp_video_get_graph_data(video, pipe);
+	if (ret < 0)
+		goto err_check_format;
 
-	if (video->type == V4L2_BUF_TYPE_VIDEO_CAPTURE) {
+	if (video->type == V4L2_BUF_TYPE_VIDEO_CAPTURE)
 		state = ISP_PIPELINE_STREAM_OUTPUT | ISP_PIPELINE_IDLE_OUTPUT;
-		pipe->input = far_end;
-		pipe->output = video;
-	} else {
-		if (far_end == NULL) {
-			ret = -EPIPE;
-			goto error;
-		}
-
+	else
 		state = ISP_PIPELINE_STREAM_INPUT | ISP_PIPELINE_IDLE_INPUT;
-		pipe->input = video;
-		pipe->output = far_end;
-	}
 
-	if (video->isp->pdata->set_constraints)
-		video->isp->pdata->set_constraints(video->isp, true);
-	pipe->l3_ick = clk_get_rate(video->isp->clock[ISP_CLK_L3_ICK]);
+	ret = isp_video_check_external_subdevs(video, pipe);
+	if (ret < 0)
+		goto err_check_format;
 
 	/* Validate the pipeline and update its state. */
 	ret = isp_video_validate_pipeline(pipe);
 	if (ret < 0)
-		goto error;
+		goto err_check_format;
 
 	pipe->error = false;
 
@@ -1041,7 +1070,7 @@ isp_video_streamon(struct file *file, void *fh, enum v4l2_buf_type type)
 
 	ret = omap3isp_video_queue_streamon(&vfh->queue);
 	if (ret < 0)
-		goto error;
+		goto err_check_format;
 
 	/* In sensor-to-memory mode, the stream can be started synchronously
 	 * to the stream on command. In memory-to-memory mode, it will be
@@ -1051,32 +1080,34 @@ isp_video_streamon(struct file *file, void *fh, enum v4l2_buf_type type)
 		ret = omap3isp_pipeline_set_stream(pipe,
 					      ISP_PIPELINE_STREAM_CONTINUOUS);
 		if (ret < 0)
-			goto error;
+			goto err_set_stream;
 		spin_lock_irqsave(&video->queue->irqlock, flags);
 		if (list_empty(&video->dmaqueue))
 			video->dmaqueue_flags |= ISP_VIDEO_DMAQUEUE_UNDERRUN;
 		spin_unlock_irqrestore(&video->queue->irqlock, flags);
 	}
 
-error:
-	if (ret < 0) {
-		omap3isp_video_queue_streamoff(&vfh->queue);
-		if (video->isp->pdata->set_constraints)
-			video->isp->pdata->set_constraints(video->isp, false);
-		media_entity_pipeline_stop(&video->video.entity);
-		/* The DMA queue must be emptied here, otherwise CCDC interrupts
-		 * that will get triggered the next time the CCDC is powered up
-		 * will try to access buffers that might have been freed but
-		 * still present in the DMA queue. This can easily get triggered
-		 * if the above omap3isp_pipeline_set_stream() call fails on a
-		 * system with a free-running sensor.
-		 */
-		INIT_LIST_HEAD(&video->dmaqueue);
-		video->queue = NULL;
-	}
+	video->streaming = 1;
+
+	mutex_unlock(&video->stream_lock);
+	return 0;
 
-	if (!ret)
-		video->streaming = 1;
+err_set_stream:
+	omap3isp_video_queue_streamoff(&vfh->queue);
+err_check_format:
+	media_entity_pipeline_stop(&video->video.entity);
+err_pipeline_start:
+	if (video->isp->pdata->set_constraints)
+		video->isp->pdata->set_constraints(video->isp, false);
+	/* The DMA queue must be emptied here, otherwise CCDC interrupts that
+	 * will get triggered the next time the CCDC is powered up will try to
+	 * access buffers that might have been freed but still present in the
+	 * DMA queue. This can easily get triggered if the above
+	 * omap3isp_pipeline_set_stream() call fails on a system with a
+	 * free-running sensor.
+	 */
+	INIT_LIST_HEAD(&video->dmaqueue);
+	video->queue = NULL;
 
 	mutex_unlock(&video->stream_lock);
 	return ret;
diff --git a/drivers/media/video/omap3isp/ispvideo.h b/drivers/media/video/omap3isp/ispvideo.h
index d91bdb919be..5acc909500e 100644
--- a/drivers/media/video/omap3isp/ispvideo.h
+++ b/drivers/media/video/omap3isp/ispvideo.h
@@ -88,6 +88,7 @@ enum isp_pipeline_state {
 /*
  * struct isp_pipeline - An ISP hardware pipeline
  * @error: A hardware error occurred during capture
+ * @entities: Bitmask of entities in the pipeline (indexed by entity ID)
  */
 struct isp_pipeline {
 	struct media_pipeline pipe;
@@ -96,12 +97,16 @@ struct isp_pipeline {
 	enum isp_pipeline_stream_state stream_state;
 	struct isp_video *input;
 	struct isp_video *output;
+	u32 entities;
 	unsigned long l3_ick;
 	unsigned int max_rate;
 	atomic_t frame_number;
 	bool do_propagation; /* of frame number */
 	bool error;
 	struct v4l2_fract max_timeperframe;
+	struct v4l2_subdev *external;
+	unsigned int external_rate;
+	unsigned int external_bpp;
 };
 
 #define to_isp_pipeline(__e) \
diff --git a/drivers/media/video/ov5642.c b/drivers/media/video/ov5642.c
index 80e07794ac8..0bc93313d37 100644
--- a/drivers/media/video/ov5642.c
+++ b/drivers/media/video/ov5642.c
@@ -1025,8 +1025,6 @@ static int ov5642_probe(struct i2c_client *client,
 	priv->crop_rect.height	= OV5642_DEFAULT_HEIGHT;
 	priv->crop_rect.left	= (OV5642_MAX_WIDTH - OV5642_DEFAULT_WIDTH) / 2;
 	priv->crop_rect.top	= (OV5642_MAX_HEIGHT - OV5642_DEFAULT_HEIGHT) / 2;
-	priv->crop_rect.width	= OV5642_DEFAULT_WIDTH;
-	priv->crop_rect.height	= OV5642_DEFAULT_HEIGHT;
 	priv->total_width = OV5642_DEFAULT_WIDTH + BLANKING_EXTRA_WIDTH;
 	priv->total_height = BLANKING_MIN_HEIGHT;
 
diff --git a/drivers/media/video/pms.c b/drivers/media/video/pms.c
index e753b5e4d2c..af2d9086d7e 100644
--- a/drivers/media/video/pms.c
+++ b/drivers/media/video/pms.c
@@ -30,15 +30,19 @@
 #include <linux/init.h>
 #include <linux/mutex.h>
 #include <linux/uaccess.h>
+#include <linux/isa.h>
 #include <asm/io.h>
 
 #include <linux/videodev2.h>
 #include <media/v4l2-common.h>
 #include <media/v4l2-ioctl.h>
+#include <media/v4l2-ctrls.h>
+#include <media/v4l2-fh.h>
+#include <media/v4l2-event.h>
 #include <media/v4l2-device.h>
 
 MODULE_LICENSE("GPL");
-MODULE_VERSION("0.0.4");
+MODULE_VERSION("0.0.5");
 
 #define MOTOROLA	1
 #define PHILIPS2	2               /* SAA7191 */
@@ -55,11 +59,11 @@ struct i2c_info {
 struct pms {
 	struct v4l2_device v4l2_dev;
 	struct video_device vdev;
+	struct v4l2_ctrl_handler hdl;
 	int height;
 	int width;
 	int depth;
 	int input;
-	s32 brightness, saturation, hue, contrast;
 	struct mutex lock;
 	int i2c_count;
 	struct i2c_info i2cinfo[64];
@@ -72,8 +76,6 @@ struct pms {
 	void __iomem *mem;
 };
 
-static struct pms pms_card;
-
 /*
  *	I/O ports and Shared Memory
  */
@@ -676,8 +678,10 @@ static int pms_querycap(struct file *file, void  *priv,
 
 	strlcpy(vcap->driver, dev->v4l2_dev.name, sizeof(vcap->driver));
 	strlcpy(vcap->card, "Mediavision PMS", sizeof(vcap->card));
-	strlcpy(vcap->bus_info, "ISA", sizeof(vcap->bus_info));
-	vcap->capabilities = V4L2_CAP_VIDEO_CAPTURE | V4L2_CAP_READWRITE;
+	snprintf(vcap->bus_info, sizeof(vcap->bus_info),
+			"ISA:%s", dev->v4l2_dev.name);
+	vcap->device_caps = V4L2_CAP_VIDEO_CAPTURE | V4L2_CAP_READWRITE;
+	vcap->capabilities = vcap->device_caps | V4L2_CAP_DEVICE_CAPS;
 	return 0;
 }
 
@@ -716,11 +720,9 @@ static int pms_s_input(struct file *file, void *fh, unsigned int inp)
 	if (inp > 3)
 		return -EINVAL;
 
-	mutex_lock(&dev->lock);
 	dev->input = inp;
 	pms_videosource(dev, inp & 1);
 	pms_vcrinput(dev, inp >> 1);
-	mutex_unlock(&dev->lock);
 	return 0;
 }
 
@@ -738,7 +740,6 @@ static int pms_s_std(struct file *file, void *fh, v4l2_std_id *std)
 	int ret = 0;
 
 	dev->std = *std;
-	mutex_lock(&dev->lock);
 	if (dev->std & V4L2_STD_NTSC) {
 		pms_framerate(dev, 30);
 		pms_secamcross(dev, 0);
@@ -762,81 +763,31 @@ static int pms_s_std(struct file *file, void *fh, v4l2_std_id *std)
 		pms_format(dev, 0);
 		break;
 	}*/
-	mutex_unlock(&dev->lock);
-	return 0;
-}
-
-static int pms_queryctrl(struct file *file, void *priv,
-					struct v4l2_queryctrl *qc)
-{
-	switch (qc->id) {
-	case V4L2_CID_BRIGHTNESS:
-		return v4l2_ctrl_query_fill(qc, 0, 255, 1, 139);
-	case V4L2_CID_CONTRAST:
-		return v4l2_ctrl_query_fill(qc, 0, 255, 1, 70);
-	case V4L2_CID_SATURATION:
-		return v4l2_ctrl_query_fill(qc, 0, 255, 1, 64);
-	case V4L2_CID_HUE:
-		return v4l2_ctrl_query_fill(qc, 0, 255, 1, 0);
-	}
-	return -EINVAL;
-}
-
-static int pms_g_ctrl(struct file *file, void *priv,
-					struct v4l2_control *ctrl)
-{
-	struct pms *dev = video_drvdata(file);
-	int ret = 0;
-
-	switch (ctrl->id) {
-	case V4L2_CID_BRIGHTNESS:
-		ctrl->value = dev->brightness;
-		break;
-	case V4L2_CID_CONTRAST:
-		ctrl->value = dev->contrast;
-		break;
-	case V4L2_CID_SATURATION:
-		ctrl->value = dev->saturation;
-		break;
-	case V4L2_CID_HUE:
-		ctrl->value = dev->hue;
-		break;
-	default:
-		ret = -EINVAL;
-		break;
-	}
 	return ret;
 }
 
-static int pms_s_ctrl(struct file *file, void *priv,
-					struct v4l2_control *ctrl)
+static int pms_s_ctrl(struct v4l2_ctrl *ctrl)
 {
-	struct pms *dev = video_drvdata(file);
+	struct pms *dev = container_of(ctrl->handler, struct pms, hdl);
 	int ret = 0;
 
-	mutex_lock(&dev->lock);
 	switch (ctrl->id) {
 	case V4L2_CID_BRIGHTNESS:
-		dev->brightness = ctrl->value;
-		pms_brightness(dev, dev->brightness);
+		pms_brightness(dev, ctrl->val);
 		break;
 	case V4L2_CID_CONTRAST:
-		dev->contrast = ctrl->value;
-		pms_contrast(dev, dev->contrast);
+		pms_contrast(dev, ctrl->val);
 		break;
 	case V4L2_CID_SATURATION:
-		dev->saturation = ctrl->value;
-		pms_saturation(dev, dev->saturation);
+		pms_saturation(dev, ctrl->val);
 		break;
 	case V4L2_CID_HUE:
-		dev->hue = ctrl->value;
-		pms_hue(dev, dev->hue);
+		pms_hue(dev, ctrl->val);
 		break;
 	default:
 		ret = -EINVAL;
 		break;
 	}
-	mutex_unlock(&dev->lock);
 	return ret;
 }
 
@@ -884,13 +835,11 @@ static int pms_s_fmt_vid_cap(struct file *file, void *fh, struct v4l2_format *fm
 
 	if (ret)
 		return ret;
-	mutex_lock(&dev->lock);
 	dev->width = pix->width;
 	dev->height = pix->height;
 	dev->depth = (pix->pixelformat == V4L2_PIX_FMT_RGB555) ? 15 : 16;
 	pms_resolution(dev, dev->width, dev->height);
 	/* Ok we figured out what to use from our wide choice */
-	mutex_unlock(&dev->lock);
 	return 0;
 }
 
@@ -901,7 +850,7 @@ static int pms_enum_fmt_vid_cap(struct file *file, void *fh, struct v4l2_fmtdesc
 		  "RGB 5:5:5", V4L2_PIX_FMT_RGB555,
 		  { 0, 0, 0, 0 }
 		},
-		{ 0, 0, 0,
+		{ 1, 0, 0,
 		  "RGB 5:6:5", V4L2_PIX_FMT_RGB565,
 		  { 0, 0, 0, 0 }
 		},
@@ -922,32 +871,43 @@ static ssize_t pms_read(struct file *file, char __user *buf,
 	struct pms *dev = video_drvdata(file);
 	int len;
 
-	mutex_lock(&dev->lock);
 	len = pms_capture(dev, buf, (dev->depth == 15), count);
-	mutex_unlock(&dev->lock);
 	return len;
 }
 
+static unsigned int pms_poll(struct file *file, struct poll_table_struct *wait)
+{
+	struct v4l2_fh *fh = file->private_data;
+	unsigned int res = POLLIN | POLLRDNORM;
+
+	if (v4l2_event_pending(fh))
+		res |= POLLPRI;
+	poll_wait(file, &fh->wait, wait);
+	return res;
+}
+
 static const struct v4l2_file_operations pms_fops = {
 	.owner		= THIS_MODULE,
+	.open           = v4l2_fh_open,
+	.release        = v4l2_fh_release,
+	.poll           = pms_poll,
 	.unlocked_ioctl	= video_ioctl2,
 	.read           = pms_read,
 };
 
 static const struct v4l2_ioctl_ops pms_ioctl_ops = {
-	.vidioc_querycap    		    = pms_querycap,
-	.vidioc_g_input      		    = pms_g_input,
-	.vidioc_s_input      		    = pms_s_input,
-	.vidioc_enum_input   		    = pms_enum_input,
-	.vidioc_g_std 			    = pms_g_std,
-	.vidioc_s_std 			    = pms_s_std,
-	.vidioc_queryctrl 		    = pms_queryctrl,
-	.vidioc_g_ctrl  		    = pms_g_ctrl,
-	.vidioc_s_ctrl 			    = pms_s_ctrl,
-	.vidioc_enum_fmt_vid_cap 	    = pms_enum_fmt_vid_cap,
-	.vidioc_g_fmt_vid_cap 		    = pms_g_fmt_vid_cap,
-	.vidioc_s_fmt_vid_cap  		    = pms_s_fmt_vid_cap,
-	.vidioc_try_fmt_vid_cap  	    = pms_try_fmt_vid_cap,
+	.vidioc_querycap	    = pms_querycap,
+	.vidioc_g_input		    = pms_g_input,
+	.vidioc_s_input		    = pms_s_input,
+	.vidioc_enum_input	    = pms_enum_input,
+	.vidioc_g_std		    = pms_g_std,
+	.vidioc_s_std		    = pms_s_std,
+	.vidioc_enum_fmt_vid_cap    = pms_enum_fmt_vid_cap,
+	.vidioc_g_fmt_vid_cap	    = pms_g_fmt_vid_cap,
+	.vidioc_s_fmt_vid_cap	    = pms_s_fmt_vid_cap,
+	.vidioc_try_fmt_vid_cap     = pms_try_fmt_vid_cap,
+	.vidioc_subscribe_event     = v4l2_ctrl_subscribe_event,
+	.vidioc_unsubscribe_event   = v4l2_event_unsubscribe,
 };
 
 /*
@@ -956,7 +916,6 @@ static const struct v4l2_ioctl_ops pms_ioctl_ops = {
 
 static int init_mediavision(struct pms *dev)
 {
-	int id;
 	int idec, decst;
 	int i;
 	static const unsigned char i2c_defs[] = {
@@ -988,7 +947,6 @@ static int init_mediavision(struct pms *dev)
 	outb(dev->io >> 4, 0x9a01);	/* Set IO port */
 
 
-	id = mvv_read(dev, 3);
 	decst = pms_i2c_stat(dev, 0x43);
 
 	if (decst != -1)
@@ -1068,76 +1026,125 @@ static int enable;
 module_param(enable, int, 0);
 #endif
 
-static int __init pms_init(void)
+static const struct v4l2_ctrl_ops pms_ctrl_ops = {
+	.s_ctrl = pms_s_ctrl,
+};
+
+static int pms_probe(struct device *pdev, unsigned int card)
 {
-	struct pms *dev = &pms_card;
-	struct v4l2_device *v4l2_dev = &dev->v4l2_dev;
+	struct pms *dev;
+	struct v4l2_device *v4l2_dev;
+	struct v4l2_ctrl_handler *hdl;
 	int res;
 
-	strlcpy(v4l2_dev->name, "pms", sizeof(v4l2_dev->name));
-
-	v4l2_info(v4l2_dev, "Mediavision Pro Movie Studio driver 0.03\n");
-
 #ifndef MODULE
 	if (!enable) {
-		v4l2_err(v4l2_dev,
-			"PMS: not enabled, use pms.enable=1 to probe\n");
+		pr_err("PMS: not enabled, use pms.enable=1 to probe\n");
 		return -ENODEV;
 	}
 #endif
 
+	dev = kzalloc(sizeof(*dev), GFP_KERNEL);
+	if (dev == NULL)
+		return -ENOMEM;
+
 	dev->decoder = PHILIPS2;
 	dev->io = io_port;
 	dev->data = io_port + 1;
+	v4l2_dev = &dev->v4l2_dev;
+	hdl = &dev->hdl;
 
-	if (init_mediavision(dev)) {
+	res = v4l2_device_register(pdev, v4l2_dev);
+	if (res < 0) {
+		v4l2_err(v4l2_dev, "Could not register v4l2_device\n");
+		goto free_dev;
+	}
+	v4l2_info(v4l2_dev, "Mediavision Pro Movie Studio driver 0.05\n");
+
+	res = init_mediavision(dev);
+	if (res) {
 		v4l2_err(v4l2_dev, "Board not found.\n");
-		return -ENODEV;
+		goto free_io;
 	}
 
-	res = v4l2_device_register(NULL, v4l2_dev);
-	if (res < 0) {
-		v4l2_err(v4l2_dev, "Could not register v4l2_device\n");
-		return res;
+	v4l2_ctrl_handler_init(hdl, 4);
+	v4l2_ctrl_new_std(hdl, &pms_ctrl_ops,
+			V4L2_CID_BRIGHTNESS, 0, 255, 1, 139);
+	v4l2_ctrl_new_std(hdl, &pms_ctrl_ops,
+			V4L2_CID_CONTRAST, 0, 255, 1, 70);
+	v4l2_ctrl_new_std(hdl, &pms_ctrl_ops,
+			V4L2_CID_SATURATION, 0, 255, 1, 64);
+	v4l2_ctrl_new_std(hdl, &pms_ctrl_ops,
+			V4L2_CID_HUE, 0, 255, 1, 0);
+	if (hdl->error) {
+		res = hdl->error;
+		goto free_hdl;
 	}
 
+	mutex_init(&dev->lock);
 	strlcpy(dev->vdev.name, v4l2_dev->name, sizeof(dev->vdev.name));
 	dev->vdev.v4l2_dev = v4l2_dev;
+	dev->vdev.ctrl_handler = hdl;
 	dev->vdev.fops = &pms_fops;
 	dev->vdev.ioctl_ops = &pms_ioctl_ops;
 	dev->vdev.release = video_device_release_empty;
+	dev->vdev.lock = &dev->lock;
+	dev->vdev.tvnorms = V4L2_STD_NTSC | V4L2_STD_PAL | V4L2_STD_SECAM;
+	set_bit(V4L2_FL_USE_FH_PRIO, &dev->vdev.flags);
 	video_set_drvdata(&dev->vdev, dev);
-	mutex_init(&dev->lock);
 	dev->std = V4L2_STD_NTSC_M;
 	dev->height = 240;
 	dev->width = 320;
-	dev->depth = 15;
-	dev->brightness = 139;
-	dev->contrast = 70;
-	dev->hue = 0;
-	dev->saturation = 64;
+	dev->depth = 16;
 	pms_swsense(dev, 75);
 	pms_resolution(dev, 320, 240);
 	pms_videosource(dev, 0);
 	pms_vcrinput(dev, 0);
-	if (video_register_device(&dev->vdev, VFL_TYPE_GRABBER, video_nr) < 0) {
-		v4l2_device_unregister(&dev->v4l2_dev);
-		release_region(dev->io, 3);
-		release_region(0x9a01, 1);
-		iounmap(dev->mem);
-		return -EINVAL;
-	}
-	return 0;
+	v4l2_ctrl_handler_setup(hdl);
+	res = video_register_device(&dev->vdev, VFL_TYPE_GRABBER, video_nr);
+	if (res >= 0)
+		return 0;
+
+free_hdl:
+	v4l2_ctrl_handler_free(hdl);
+	v4l2_device_unregister(&dev->v4l2_dev);
+free_io:
+	release_region(dev->io, 3);
+	release_region(0x9a01, 1);
+	iounmap(dev->mem);
+free_dev:
+	kfree(dev);
+	return res;
 }
 
-static void __exit pms_exit(void)
+static int pms_remove(struct device *pdev, unsigned int card)
 {
-	struct pms *dev = &pms_card;
+	struct pms *dev = dev_get_drvdata(pdev);
 
 	video_unregister_device(&dev->vdev);
+	v4l2_ctrl_handler_free(&dev->hdl);
 	release_region(dev->io, 3);
 	release_region(0x9a01, 1);
 	iounmap(dev->mem);
+	return 0;
+}
+
+static struct isa_driver pms_driver = {
+	.probe		= pms_probe,
+	.remove		= pms_remove,
+	.driver		= {
+		.name	= "pms",
+	},
+};
+
+static int __init pms_init(void)
+{
+	return isa_register_driver(&pms_driver, 1);
+}
+
+static void __exit pms_exit(void)
+{
+	isa_unregister_driver(&pms_driver);
 }
 
 module_init(pms_init);
diff --git a/drivers/media/video/pvrusb2/pvrusb2-hdw-internal.h b/drivers/media/video/pvrusb2/pvrusb2-hdw-internal.h
index 305e6aaa844..036952f2a3c 100644
--- a/drivers/media/video/pvrusb2/pvrusb2-hdw-internal.h
+++ b/drivers/media/video/pvrusb2/pvrusb2-hdw-internal.h
@@ -317,18 +317,16 @@ struct pvr2_hdw {
 	v4l2_std_id std_mask_eeprom; // Hardware supported selections
 	v4l2_std_id std_mask_avail;  // Which standards we may select from
 	v4l2_std_id std_mask_cur;    // Currently selected standard(s)
-	unsigned int std_enum_cnt;   // # of enumerated standards
 	int std_enum_cur;            // selected standard enumeration value
 	int std_dirty;               // True if std_mask_cur has changed
 	struct pvr2_ctl_info std_info_enum;
 	struct pvr2_ctl_info std_info_avail;
 	struct pvr2_ctl_info std_info_cur;
-	struct v4l2_standard *std_defs;
-	const char **std_enum_names;
+	struct pvr2_ctl_info std_info_detect;
 
 	// Generated string names, one per actual V4L2 standard
 	const char *std_mask_ptrs[32];
-	char std_mask_names[32][10];
+	char std_mask_names[32][16];
 
 	int unit_number;             /* ID for driver instance */
 	unsigned long serial_number; /* ID for hardware itself */
diff --git a/drivers/media/video/pvrusb2/pvrusb2-hdw.c b/drivers/media/video/pvrusb2/pvrusb2-hdw.c
index ebc2c7e3923..fb828ba1dbb 100644
--- a/drivers/media/video/pvrusb2/pvrusb2-hdw.c
+++ b/drivers/media/video/pvrusb2/pvrusb2-hdw.c
@@ -334,8 +334,6 @@ static void pvr2_hdw_state_log_state(struct pvr2_hdw *);
 static int pvr2_hdw_cmd_usbstream(struct pvr2_hdw *hdw,int runFl);
 static int pvr2_hdw_commit_setup(struct pvr2_hdw *hdw);
 static int pvr2_hdw_get_eeprom_addr(struct pvr2_hdw *hdw);
-static void pvr2_hdw_internal_find_stdenum(struct pvr2_hdw *hdw);
-static void pvr2_hdw_internal_set_std_avail(struct pvr2_hdw *hdw);
 static void pvr2_hdw_quiescent_timeout(unsigned long);
 static void pvr2_hdw_decoder_stabilization_timeout(unsigned long);
 static void pvr2_hdw_encoder_wait_timeout(unsigned long);
@@ -346,7 +344,7 @@ static int pvr2_send_request_ex(struct pvr2_hdw *hdw,
 				void *write_data,unsigned int write_len,
 				void *read_data,unsigned int read_len);
 static int pvr2_hdw_check_cropcap(struct pvr2_hdw *hdw);
-
+static v4l2_std_id pvr2_hdw_get_detected_std(struct pvr2_hdw *hdw);
 
 static void trace_stbit(const char *name,int val)
 {
@@ -840,6 +838,12 @@ static int ctrl_hsm_get(struct pvr2_ctrl *cptr,int *vp)
 	return 0;
 }
 
+static int ctrl_stddetect_get(struct pvr2_ctrl *cptr, int *vp)
+{
+	*vp = pvr2_hdw_get_detected_std(cptr->hdw);
+	return 0;
+}
+
 static int ctrl_stdavail_get(struct pvr2_ctrl *cptr,int *vp)
 {
 	*vp = cptr->hdw->std_mask_avail;
@@ -854,8 +858,7 @@ static int ctrl_stdavail_set(struct pvr2_ctrl *cptr,int m,int v)
 	ns = (ns & ~m) | (v & m);
 	if (ns == hdw->std_mask_avail) return 0;
 	hdw->std_mask_avail = ns;
-	pvr2_hdw_internal_set_std_avail(hdw);
-	pvr2_hdw_internal_find_stdenum(hdw);
+	hdw->std_info_cur.def.type_bitmask.valid_bits = hdw->std_mask_avail;
 	return 0;
 }
 
@@ -895,7 +898,6 @@ static int ctrl_stdcur_set(struct pvr2_ctrl *cptr,int m,int v)
 	if (ns == hdw->std_mask_cur) return 0;
 	hdw->std_mask_cur = ns;
 	hdw->std_dirty = !0;
-	pvr2_hdw_internal_find_stdenum(hdw);
 	return 0;
 }
 
@@ -941,40 +943,6 @@ static int ctrl_audio_modes_present_get(struct pvr2_ctrl *cptr,int *vp)
 }
 
 
-static int ctrl_stdenumcur_set(struct pvr2_ctrl *cptr,int m,int v)
-{
-	struct pvr2_hdw *hdw = cptr->hdw;
-	if (v < 0) return -EINVAL;
-	if (v > hdw->std_enum_cnt) return -EINVAL;
-	hdw->std_enum_cur = v;
-	if (!v) return 0;
-	v--;
-	if (hdw->std_mask_cur == hdw->std_defs[v].id) return 0;
-	hdw->std_mask_cur = hdw->std_defs[v].id;
-	hdw->std_dirty = !0;
-	return 0;
-}
-
-
-static int ctrl_stdenumcur_get(struct pvr2_ctrl *cptr,int *vp)
-{
-	*vp = cptr->hdw->std_enum_cur;
-	return 0;
-}
-
-
-static int ctrl_stdenumcur_is_dirty(struct pvr2_ctrl *cptr)
-{
-	return cptr->hdw->std_dirty != 0;
-}
-
-
-static void ctrl_stdenumcur_clear_dirty(struct pvr2_ctrl *cptr)
-{
-	cptr->hdw->std_dirty = 0;
-}
-
-
 #define DEFINT(vmin,vmax) \
 	.type = pvr2_ctl_int, \
 	.def.type_int.min_value = vmin, \
@@ -1293,15 +1261,14 @@ static const struct pvr2_ctl_info control_defs[] = {
 		.sym_to_val = ctrl_std_sym_to_val,
 		.type = pvr2_ctl_bitmask,
 	},{
-		.desc = "Video Standard Name",
-		.name = "video_standard",
-		.internal_id = PVR2_CID_STDENUM,
+		.desc = "Video Standards Detected Mask",
+		.name = "video_standard_mask_detected",
+		.internal_id = PVR2_CID_STDDETECT,
 		.skip_init = !0,
-		.get_value = ctrl_stdenumcur_get,
-		.set_value = ctrl_stdenumcur_set,
-		.is_dirty = ctrl_stdenumcur_is_dirty,
-		.clear_dirty = ctrl_stdenumcur_clear_dirty,
-		.type = pvr2_ctl_enum,
+		.get_value = ctrl_stddetect_get,
+		.val_to_sym = ctrl_std_val_to_sym,
+		.sym_to_val = ctrl_std_sym_to_val,
+		.type = pvr2_ctl_bitmask,
 	}
 };
 
@@ -1936,7 +1903,7 @@ static void pvr2_hdw_setup_std(struct pvr2_hdw *hdw)
 		hdw->std_mask_avail |= std2;
 	}
 
-	pvr2_hdw_internal_set_std_avail(hdw);
+	hdw->std_info_cur.def.type_bitmask.valid_bits = hdw->std_mask_avail;
 
 	if (std1) {
 		bcnt = pvr2_std_id_to_str(buf,sizeof(buf),std1);
@@ -1945,7 +1912,6 @@ static void pvr2_hdw_setup_std(struct pvr2_hdw *hdw)
 			   bcnt,buf);
 		hdw->std_mask_cur = std1;
 		hdw->std_dirty = !0;
-		pvr2_hdw_internal_find_stdenum(hdw);
 		return;
 	}
 	if (std3) {
@@ -1955,7 +1921,6 @@ static void pvr2_hdw_setup_std(struct pvr2_hdw *hdw)
 			   " (determined by device type): %.*s",bcnt,buf);
 		hdw->std_mask_cur = std3;
 		hdw->std_dirty = !0;
-		pvr2_hdw_internal_find_stdenum(hdw);
 		return;
 	}
 
@@ -1975,24 +1940,10 @@ static void pvr2_hdw_setup_std(struct pvr2_hdw *hdw)
 				   bcnt,buf);
 			hdw->std_mask_cur = std_eeprom_maps[idx].std;
 			hdw->std_dirty = !0;
-			pvr2_hdw_internal_find_stdenum(hdw);
 			return;
 		}
 	}
 
-	if (hdw->std_enum_cnt > 1) {
-		// Autoselect the first listed standard
-		hdw->std_enum_cur = 1;
-		hdw->std_mask_cur = hdw->std_defs[hdw->std_enum_cur-1].id;
-		hdw->std_dirty = !0;
-		pvr2_trace(PVR2_TRACE_STD,
-			   "Initial video standard auto-selected to %s",
-			   hdw->std_defs[hdw->std_enum_cur-1].name);
-		return;
-	}
-
-	pvr2_trace(PVR2_TRACE_ERROR_LEGS,
-		   "Unable to select a viable initial video standard");
 }
 
 
@@ -2594,14 +2545,6 @@ struct pvr2_hdw *pvr2_hdw_create(struct usb_interface *intf,
 		cptr->info = ciptr;
 	}
 
-	// Initialize video standard enum dynamic control
-	cptr = pvr2_hdw_get_ctrl_by_id(hdw,PVR2_CID_STDENUM);
-	if (cptr) {
-		memcpy(&hdw->std_info_enum,cptr->info,
-		       sizeof(hdw->std_info_enum));
-		cptr->info = &hdw->std_info_enum;
-
-	}
 	// Initialize control data regarding video standard masks
 	valid_std_mask = pvr2_std_get_usable();
 	for (idx = 0; idx < 32; idx++) {
@@ -2629,7 +2572,17 @@ struct pvr2_hdw *pvr2_hdw_create(struct usb_interface *intf,
 		cptr->info = &hdw->std_info_cur;
 		hdw->std_info_cur.def.type_bitmask.bit_names =
 			hdw->std_mask_ptrs;
-		hdw->std_info_avail.def.type_bitmask.valid_bits =
+		hdw->std_info_cur.def.type_bitmask.valid_bits =
+			valid_std_mask;
+	}
+	cptr = pvr2_hdw_get_ctrl_by_id(hdw,PVR2_CID_STDDETECT);
+	if (cptr) {
+		memcpy(&hdw->std_info_detect,cptr->info,
+		       sizeof(hdw->std_info_detect));
+		cptr->info = &hdw->std_info_detect;
+		hdw->std_info_detect.def.type_bitmask.bit_names =
+			hdw->std_mask_ptrs;
+		hdw->std_info_detect.def.type_bitmask.valid_bits =
 			valid_std_mask;
 	}
 
@@ -2711,8 +2664,6 @@ struct pvr2_hdw *pvr2_hdw_create(struct usb_interface *intf,
 		kfree(hdw->ctl_write_buffer);
 		kfree(hdw->controls);
 		kfree(hdw->mpeg_ctrl_info);
-		kfree(hdw->std_defs);
-		kfree(hdw->std_enum_names);
 		kfree(hdw);
 	}
 	return NULL;
@@ -2788,8 +2739,6 @@ void pvr2_hdw_destroy(struct pvr2_hdw *hdw)
 	} while (0); mutex_unlock(&pvr2_unit_mtx);
 	kfree(hdw->controls);
 	kfree(hdw->mpeg_ctrl_info);
-	kfree(hdw->std_defs);
-	kfree(hdw->std_enum_names);
 	kfree(hdw);
 }
 
@@ -2812,86 +2761,6 @@ void pvr2_hdw_disconnect(struct pvr2_hdw *hdw)
 }
 
 
-// Attempt to autoselect an appropriate value for std_enum_cur given
-// whatever is currently in std_mask_cur
-static void pvr2_hdw_internal_find_stdenum(struct pvr2_hdw *hdw)
-{
-	unsigned int idx;
-	for (idx = 1; idx < hdw->std_enum_cnt; idx++) {
-		if (hdw->std_defs[idx-1].id == hdw->std_mask_cur) {
-			hdw->std_enum_cur = idx;
-			return;
-		}
-	}
-	hdw->std_enum_cur = 0;
-}
-
-
-// Calculate correct set of enumerated standards based on currently known
-// set of available standards bits.
-static void pvr2_hdw_internal_set_std_avail(struct pvr2_hdw *hdw)
-{
-	struct v4l2_standard *newstd;
-	unsigned int std_cnt;
-	unsigned int idx;
-
-	newstd = pvr2_std_create_enum(&std_cnt,hdw->std_mask_avail);
-
-	if (hdw->std_defs) {
-		kfree(hdw->std_defs);
-		hdw->std_defs = NULL;
-	}
-	hdw->std_enum_cnt = 0;
-	if (hdw->std_enum_names) {
-		kfree(hdw->std_enum_names);
-		hdw->std_enum_names = NULL;
-	}
-
-	if (!std_cnt) {
-		pvr2_trace(
-			PVR2_TRACE_ERROR_LEGS,
-			"WARNING: Failed to identify any viable standards");
-	}
-
-	/* Set up the dynamic control for this standard */
-	hdw->std_enum_names = kmalloc(sizeof(char *)*(std_cnt+1),GFP_KERNEL);
-	if (hdw->std_enum_names) {
-		hdw->std_enum_names[0] = "none";
-		for (idx = 0; idx < std_cnt; idx++)
-			hdw->std_enum_names[idx+1] = newstd[idx].name;
-		hdw->std_info_enum.def.type_enum.value_names =
-						hdw->std_enum_names;
-		hdw->std_info_enum.def.type_enum.count = std_cnt+1;
-	} else {
-		pvr2_trace(
-			PVR2_TRACE_ERROR_LEGS,
-			"WARNING: Failed to alloc memory for names");
-		hdw->std_info_enum.def.type_enum.value_names = NULL;
-		hdw->std_info_enum.def.type_enum.count = 0;
-	}
-	hdw->std_defs = newstd;
-	hdw->std_enum_cnt = std_cnt+1;
-	hdw->std_enum_cur = 0;
-	hdw->std_info_cur.def.type_bitmask.valid_bits = hdw->std_mask_avail;
-}
-
-
-int pvr2_hdw_get_stdenum_value(struct pvr2_hdw *hdw,
-			       struct v4l2_standard *std,
-			       unsigned int idx)
-{
-	int ret = -EINVAL;
-	if (!idx) return ret;
-	LOCK_TAKE(hdw->big_lock); do {
-		if (idx >= hdw->std_enum_cnt) break;
-		idx--;
-		memcpy(std,hdw->std_defs+idx,sizeof(*std));
-		ret = 0;
-	} while (0); LOCK_GIVE(hdw->big_lock);
-	return ret;
-}
-
-
 /* Get the number of defined controls */
 unsigned int pvr2_hdw_get_ctrl_count(struct pvr2_hdw *hdw)
 {
@@ -2995,11 +2864,13 @@ static void pvr2_subdev_set_control(struct pvr2_hdw *hdw, int id,
 		pvr2_subdev_set_control(hdw, id, #lab, (hdw)->lab##_val); \
 	}
 
-int pvr2_hdw_get_detected_std(struct pvr2_hdw *hdw, v4l2_std_id *std)
+v4l2_std_id pvr2_hdw_get_detected_std(struct pvr2_hdw *hdw)
 {
+	v4l2_std_id std;
+	std = (v4l2_std_id)hdw->std_mask_avail;
 	v4l2_device_call_all(&hdw->v4l2_dev, 0,
-			     video, querystd, std);
-	return 0;
+			     video, querystd, &std);
+	return std;
 }
 
 /* Execute whatever commands are required to update the state of all the
diff --git a/drivers/media/video/pvrusb2/pvrusb2-hdw.h b/drivers/media/video/pvrusb2/pvrusb2-hdw.h
index 66546580b17..8060fc666ee 100644
--- a/drivers/media/video/pvrusb2/pvrusb2-hdw.h
+++ b/drivers/media/video/pvrusb2/pvrusb2-hdw.h
@@ -28,7 +28,6 @@
 
 /* Private internal control ids, look these up with
    pvr2_hdw_get_ctrl_by_id() - these are NOT visible in V4L */
-#define PVR2_CID_STDENUM 1
 #define PVR2_CID_STDCUR 2
 #define PVR2_CID_STDAVAIL 3
 #define PVR2_CID_INPUT 4
@@ -46,6 +45,7 @@
 #define PVR2_CID_CROPCAPBT 16
 #define PVR2_CID_CROPCAPBW 17
 #define PVR2_CID_CROPCAPBH 18
+#define PVR2_CID_STDDETECT 19
 
 /* Legal values for the INPUT state variable */
 #define PVR2_CVAL_INPUT_TV 0
@@ -210,13 +210,6 @@ int pvr2_hdw_set_stream_type(struct pvr2_hdw *, enum pvr2_config);
 /* Get handle to video output stream */
 struct pvr2_stream *pvr2_hdw_get_video_stream(struct pvr2_hdw *);
 
-/* Emit a video standard struct */
-int pvr2_hdw_get_stdenum_value(struct pvr2_hdw *hdw,struct v4l2_standard *std,
-			       unsigned int idx);
-
-/* Get the detected video standard */
-int pvr2_hdw_get_detected_std(struct pvr2_hdw *hdw, v4l2_std_id *std);
-
 /* Enable / disable retrieval of CPU firmware or prom contents.  This must
    be enabled before pvr2_hdw_cpufw_get() will function.  Note that doing
    this may prevent the device from running (and leaving this mode may
diff --git a/drivers/media/video/pvrusb2/pvrusb2-v4l2.c b/drivers/media/video/pvrusb2/pvrusb2-v4l2.c
index e1111d968a3..7bddfaeeafc 100644
--- a/drivers/media/video/pvrusb2/pvrusb2-v4l2.c
+++ b/drivers/media/video/pvrusb2/pvrusb2-v4l2.c
@@ -107,7 +107,6 @@ static struct v4l2_fmtdesc pvr_fmtdesc [] = {
 		// This should really be V4L2_PIX_FMT_MPEG, but xawtv
 		// breaks when I do that.
 		.pixelformat    = 0, // V4L2_PIX_FMT_MPEG,
-		.reserved       = { 0, 0, 0, 0 }
 	}
 };
 
@@ -145,740 +144,739 @@ static struct v4l2_format pvr_format [] = {
 				.start = { 0, 0 },
 				.count = { 0, 0 },
 				.flags = 0,
-				.reserved = { 0, 0 }
 			}
 		}
 	}
 };
 
 
+
 /*
- * pvr_ioctl()
- *
- * This is part of Video 4 Linux API. The procedure handles ioctl() calls.
- *
+ * This is part of Video 4 Linux API. These procedures handle ioctl() calls.
  */
-static long pvr2_v4l2_do_ioctl(struct file *file, unsigned int cmd, void *arg)
+static int pvr2_querycap(struct file *file, void *priv, struct v4l2_capability *cap)
 {
 	struct pvr2_v4l2_fh *fh = file->private_data;
-	struct pvr2_v4l2 *vp = fh->vhead;
-	struct pvr2_v4l2_dev *pdi = fh->pdi;
 	struct pvr2_hdw *hdw = fh->channel.mc_head->hdw;
-	long ret = -EINVAL;
 
-	if (pvrusb2_debug & PVR2_TRACE_V4LIOCTL) {
-		v4l_print_ioctl(pvr2_hdw_get_driver_name(hdw),cmd);
-	}
+	memcpy(cap, &pvr_capability, sizeof(struct v4l2_capability));
+	strlcpy(cap->bus_info, pvr2_hdw_get_bus_info(hdw),
+			sizeof(cap->bus_info));
+	strlcpy(cap->card, pvr2_hdw_get_desc(hdw), sizeof(cap->card));
+	return 0;
+}
 
-	if (!pvr2_hdw_dev_ok(hdw)) {
-		pvr2_trace(PVR2_TRACE_ERROR_LEGS,
-			   "ioctl failed - bad or no context");
-		return -EFAULT;
-	}
+static int pvr2_g_priority(struct file *file, void *priv, enum v4l2_priority *p)
+{
+	struct pvr2_v4l2_fh *fh = file->private_data;
+	struct pvr2_v4l2 *vp = fh->vhead;
 
-	/* check priority */
-	switch (cmd) {
-	case VIDIOC_S_CTRL:
-	case VIDIOC_S_STD:
-	case VIDIOC_S_INPUT:
-	case VIDIOC_S_TUNER:
-	case VIDIOC_S_FREQUENCY:
-		ret = v4l2_prio_check(&vp->prio, fh->prio);
-		if (ret)
-			return ret;
-	}
+	*p = v4l2_prio_max(&vp->prio);
+	return 0;
+}
 
-	switch (cmd) {
-	case VIDIOC_QUERYCAP:
-	{
-		struct v4l2_capability *cap = arg;
+static int pvr2_s_priority(struct file *file, void *priv, enum v4l2_priority prio)
+{
+	struct pvr2_v4l2_fh *fh = file->private_data;
+	struct pvr2_v4l2 *vp = fh->vhead;
 
-		memcpy(cap, &pvr_capability, sizeof(struct v4l2_capability));
-		strlcpy(cap->bus_info,pvr2_hdw_get_bus_info(hdw),
-			sizeof(cap->bus_info));
-		strlcpy(cap->card,pvr2_hdw_get_desc(hdw),sizeof(cap->card));
+	return v4l2_prio_change(&vp->prio, &fh->prio, prio);
+}
 
-		ret = 0;
-		break;
-	}
+static int pvr2_g_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;
+	int val = 0;
+	int ret;
 
-	case VIDIOC_G_PRIORITY:
-	{
-		enum v4l2_priority *p = arg;
+	ret = pvr2_ctrl_get_value(
+			pvr2_hdw_get_ctrl_by_id(hdw, PVR2_CID_STDCUR), &val);
+	*std = val;
+	return ret;
+}
 
-		*p = v4l2_prio_max(&vp->prio);
-		ret = 0;
-		break;
-	}
+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;
 
-	case VIDIOC_S_PRIORITY:
-	{
-		enum v4l2_priority *prio = arg;
+	return pvr2_ctrl_set_value(
+		pvr2_hdw_get_ctrl_by_id(hdw, PVR2_CID_STDCUR), *std);
+}
 
-		ret = v4l2_prio_change(&vp->prio, &fh->prio, *prio);
-		break;
-	}
+static int pvr2_querystd(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;
+	int val = 0;
+	int ret;
 
-	case VIDIOC_ENUMSTD:
-	{
-		struct v4l2_standard *vs = (struct v4l2_standard *)arg;
-		int idx = vs->index;
-		ret = pvr2_hdw_get_stdenum_value(hdw,vs,idx+1);
-		break;
-	}
+	ret = pvr2_ctrl_get_value(
+		pvr2_hdw_get_ctrl_by_id(hdw, PVR2_CID_STDDETECT), &val);
+	*std = val;
+	return ret;
+}
 
-	case VIDIOC_QUERYSTD:
-	{
-		v4l2_std_id *std = arg;
-		*std = V4L2_STD_ALL;
-		ret = pvr2_hdw_get_detected_std(hdw, std);
-		break;
-	}
+static int pvr2_enum_input(struct file *file, void *priv, struct v4l2_input *vi)
+{
+	struct pvr2_v4l2_fh *fh = file->private_data;
+	struct pvr2_hdw *hdw = fh->channel.mc_head->hdw;
+	struct pvr2_ctrl *cptr;
+	struct v4l2_input tmp;
+	unsigned int cnt;
+	int val;
+	int ret;
 
-	case VIDIOC_G_STD:
-	{
-		int val = 0;
-		ret = pvr2_ctrl_get_value(
-			pvr2_hdw_get_ctrl_by_id(hdw,PVR2_CID_STDCUR),&val);
-		*(v4l2_std_id *)arg = val;
+	cptr = pvr2_hdw_get_ctrl_by_id(hdw, PVR2_CID_INPUT);
+
+	memset(&tmp, 0, sizeof(tmp));
+	tmp.index = vi->index;
+	ret = 0;
+	if (vi->index >= fh->input_cnt)
+		return -EINVAL;
+	val = fh->input_map[vi->index];
+	switch (val) {
+	case PVR2_CVAL_INPUT_TV:
+	case PVR2_CVAL_INPUT_DTV:
+	case PVR2_CVAL_INPUT_RADIO:
+		tmp.type = V4L2_INPUT_TYPE_TUNER;
 		break;
-	}
-
-	case VIDIOC_S_STD:
-	{
-		ret = pvr2_ctrl_set_value(
-			pvr2_hdw_get_ctrl_by_id(hdw,PVR2_CID_STDCUR),
-			*(v4l2_std_id *)arg);
+	case PVR2_CVAL_INPUT_SVIDEO:
+	case PVR2_CVAL_INPUT_COMPOSITE:
+		tmp.type = V4L2_INPUT_TYPE_CAMERA;
 		break;
+	default:
+		return -EINVAL;
 	}
 
-	case VIDIOC_ENUMINPUT:
-	{
-		struct pvr2_ctrl *cptr;
-		struct v4l2_input *vi = (struct v4l2_input *)arg;
-		struct v4l2_input tmp;
-		unsigned int cnt;
-		int val;
+	cnt = 0;
+	pvr2_ctrl_get_valname(cptr, val,
+			tmp.name, sizeof(tmp.name) - 1, &cnt);
+	tmp.name[cnt] = 0;
 
-		cptr = pvr2_hdw_get_ctrl_by_id(hdw,PVR2_CID_INPUT);
+	/* Don't bother with audioset, since this driver currently
+	   always switches the audio whenever the video is
+	   switched. */
 
-		memset(&tmp,0,sizeof(tmp));
-		tmp.index = vi->index;
-		ret = 0;
-		if (vi->index >= fh->input_cnt) {
-			ret = -EINVAL;
-			break;
-		}
-		val = fh->input_map[vi->index];
-		switch (val) {
-		case PVR2_CVAL_INPUT_TV:
-		case PVR2_CVAL_INPUT_DTV:
-		case PVR2_CVAL_INPUT_RADIO:
-			tmp.type = V4L2_INPUT_TYPE_TUNER;
-			break;
-		case PVR2_CVAL_INPUT_SVIDEO:
-		case PVR2_CVAL_INPUT_COMPOSITE:
-			tmp.type = V4L2_INPUT_TYPE_CAMERA;
-			break;
-		default:
-			ret = -EINVAL;
-			break;
-		}
-		if (ret < 0) break;
-
-		cnt = 0;
-		pvr2_ctrl_get_valname(cptr,val,
-				      tmp.name,sizeof(tmp.name)-1,&cnt);
-		tmp.name[cnt] = 0;
-
-		/* Don't bother with audioset, since this driver currently
-		   always switches the audio whenever the video is
-		   switched. */
-
-		/* Handling std is a tougher problem.  It doesn't make
-		   sense in cases where a device might be multi-standard.
-		   We could just copy out the current value for the
-		   standard, but it can change over time.  For now just
-		   leave it zero. */
-
-		memcpy(vi, &tmp, sizeof(tmp));
-
-		ret = 0;
-		break;
-	}
+	/* Handling std is a tougher problem.  It doesn't make
+	   sense in cases where a device might be multi-standard.
+	   We could just copy out the current value for the
+	   standard, but it can change over time.  For now just
+	   leave it zero. */
+	*vi = tmp;
+	return 0;
+}
 
-	case VIDIOC_G_INPUT:
-	{
-		unsigned int idx;
-		struct pvr2_ctrl *cptr;
-		struct v4l2_input *vi = (struct v4l2_input *)arg;
-		int val;
-		cptr = pvr2_hdw_get_ctrl_by_id(hdw,PVR2_CID_INPUT);
-		val = 0;
-		ret = pvr2_ctrl_get_value(cptr,&val);
-		vi->index = 0;
-		for (idx = 0; idx < fh->input_cnt; idx++) {
-			if (fh->input_map[idx] == val) {
-				vi->index = idx;
-				break;
-			}
-		}
-		break;
-	}
+static int pvr2_g_input(struct file *file, void *priv, unsigned int *i)
+{
+	struct pvr2_v4l2_fh *fh = file->private_data;
+	struct pvr2_hdw *hdw = fh->channel.mc_head->hdw;
+	unsigned int idx;
+	struct pvr2_ctrl *cptr;
+	int val;
+	int ret;
 
-	case VIDIOC_S_INPUT:
-	{
-		struct v4l2_input *vi = (struct v4l2_input *)arg;
-		if (vi->index >= fh->input_cnt) {
-			ret = -ERANGE;
+	cptr = pvr2_hdw_get_ctrl_by_id(hdw, PVR2_CID_INPUT);
+	val = 0;
+	ret = pvr2_ctrl_get_value(cptr, &val);
+	*i = 0;
+	for (idx = 0; idx < fh->input_cnt; idx++) {
+		if (fh->input_map[idx] == val) {
+			*i = idx;
 			break;
 		}
-		ret = pvr2_ctrl_set_value(
-			pvr2_hdw_get_ctrl_by_id(hdw,PVR2_CID_INPUT),
-			fh->input_map[vi->index]);
-		break;
 	}
+	return ret;
+}
 
-	case VIDIOC_ENUMAUDIO:
-	{
-		/* pkt: FIXME: We are returning one "fake" input here
-		   which could very well be called "whatever_we_like".
-		   This is for apps that want to see an audio input
-		   just to feel comfortable, as well as to test if
-		   it can do stereo or sth. There is actually no guarantee
-		   that the actual audio input cannot change behind the app's
-		   back, but most applications should not mind that either.
-
-		   Hopefully, mplayer people will work with us on this (this
-		   whole mess is to support mplayer pvr://), or Hans will come
-		   up with a more standard way to say "we have inputs but we
-		   don 't want you to change them independent of video" which
-		   will sort this mess.
-		 */
-		struct v4l2_audio *vin = arg;
-		ret = -EINVAL;
-		if (vin->index > 0) break;
-		strncpy(vin->name, "PVRUSB2 Audio",14);
-		vin->capability = V4L2_AUDCAP_STEREO;
-		ret = 0;
-		break;
-		break;
-	}
+static int pvr2_s_input(struct file *file, void *priv, unsigned int inp)
+{
+	struct pvr2_v4l2_fh *fh = file->private_data;
+	struct pvr2_hdw *hdw = fh->channel.mc_head->hdw;
 
-	case VIDIOC_G_AUDIO:
-	{
-		/* pkt: FIXME: see above comment (VIDIOC_ENUMAUDIO) */
-		struct v4l2_audio *vin = arg;
-		memset(vin,0,sizeof(*vin));
-		vin->index = 0;
-		strncpy(vin->name, "PVRUSB2 Audio",14);
-		vin->capability = V4L2_AUDCAP_STEREO;
-		ret = 0;
-		break;
-	}
+	if (inp >= fh->input_cnt)
+		return -EINVAL;
+	return pvr2_ctrl_set_value(
+			pvr2_hdw_get_ctrl_by_id(hdw, PVR2_CID_INPUT),
+			fh->input_map[inp]);
+}
 
-	case VIDIOC_G_TUNER:
-	{
-		struct v4l2_tuner *vt = (struct v4l2_tuner *)arg;
+static int pvr2_enumaudio(struct file *file, void *priv, struct v4l2_audio *vin)
+{
+	/* pkt: FIXME: We are returning one "fake" input here
+	   which could very well be called "whatever_we_like".
+	   This is for apps that want to see an audio input
+	   just to feel comfortable, as well as to test if
+	   it can do stereo or sth. There is actually no guarantee
+	   that the actual audio input cannot change behind the app's
+	   back, but most applications should not mind that either.
+
+	   Hopefully, mplayer people will work with us on this (this
+	   whole mess is to support mplayer pvr://), or Hans will come
+	   up with a more standard way to say "we have inputs but we
+	   don 't want you to change them independent of video" which
+	   will sort this mess.
+	 */
+
+	if (vin->index > 0)
+		return -EINVAL;
+	strncpy(vin->name, "PVRUSB2 Audio", 14);
+	vin->capability = V4L2_AUDCAP_STEREO;
+	return 0;
+}
 
-		if (vt->index != 0) break; /* Only answer for the 1st tuner */
+static int pvr2_g_audio(struct file *file, void *priv, struct v4l2_audio *vin)
+{
+	/* pkt: FIXME: see above comment (VIDIOC_ENUMAUDIO) */
+	vin->index = 0;
+	strncpy(vin->name, "PVRUSB2 Audio", 14);
+	vin->capability = V4L2_AUDCAP_STEREO;
+	return 0;
+}
 
-		pvr2_hdw_execute_tuner_poll(hdw);
-		ret = pvr2_hdw_get_tuner_status(hdw,vt);
-		break;
-	}
+static int pvr2_s_audio(struct file *file, void *priv, struct v4l2_audio *vout)
+{
+	if (vout->index)
+		return -EINVAL;
+	return 0;
+}
 
-	case VIDIOC_S_TUNER:
-	{
-		struct v4l2_tuner *vt=(struct v4l2_tuner *)arg;
+static int pvr2_g_tuner(struct file *file, void *priv, struct v4l2_tuner *vt)
+{
+	struct pvr2_v4l2_fh *fh = file->private_data;
+	struct pvr2_hdw *hdw = fh->channel.mc_head->hdw;
 
-		if (vt->index != 0)
-			break;
+	if (vt->index != 0)
+		return -EINVAL; /* Only answer for the 1st tuner */
 
-		ret = pvr2_ctrl_set_value(
-			pvr2_hdw_get_ctrl_by_id(hdw,PVR2_CID_AUDIOMODE),
+	pvr2_hdw_execute_tuner_poll(hdw);
+	return pvr2_hdw_get_tuner_status(hdw, vt);
+}
+
+static int pvr2_s_tuner(struct file *file, void *priv, struct v4l2_tuner *vt)
+{
+	struct pvr2_v4l2_fh *fh = file->private_data;
+	struct pvr2_hdw *hdw = fh->channel.mc_head->hdw;
+
+	if (vt->index != 0)
+		return -EINVAL;
+
+	return pvr2_ctrl_set_value(
+			pvr2_hdw_get_ctrl_by_id(hdw, PVR2_CID_AUDIOMODE),
 			vt->audmode);
-		break;
-	}
+}
 
-	case VIDIOC_S_FREQUENCY:
-	{
-		const struct v4l2_frequency *vf = (struct v4l2_frequency *)arg;
-		unsigned long fv;
-		struct v4l2_tuner vt;
-		int cur_input;
-		struct pvr2_ctrl *ctrlp;
-		ret = pvr2_hdw_get_tuner_status(hdw,&vt);
-		if (ret != 0) break;
-		ctrlp = pvr2_hdw_get_ctrl_by_id(hdw,PVR2_CID_INPUT);
-		ret = pvr2_ctrl_get_value(ctrlp,&cur_input);
-		if (ret != 0) break;
-		if (vf->type == V4L2_TUNER_RADIO) {
-			if (cur_input != PVR2_CVAL_INPUT_RADIO) {
-				pvr2_ctrl_set_value(ctrlp,
-						    PVR2_CVAL_INPUT_RADIO);
-			}
-		} else {
-			if (cur_input == PVR2_CVAL_INPUT_RADIO) {
-				pvr2_ctrl_set_value(ctrlp,
-						    PVR2_CVAL_INPUT_TV);
-			}
-		}
-		fv = vf->frequency;
-		if (vt.capability & V4L2_TUNER_CAP_LOW) {
-			fv = (fv * 125) / 2;
-		} else {
-			fv = fv * 62500;
-		}
-		ret = pvr2_ctrl_set_value(
+int pvr2_s_frequency(struct file *file, void *priv, struct v4l2_frequency *vf)
+{
+	struct pvr2_v4l2_fh *fh = file->private_data;
+	struct pvr2_hdw *hdw = fh->channel.mc_head->hdw;
+	unsigned long fv;
+	struct v4l2_tuner vt;
+	int cur_input;
+	struct pvr2_ctrl *ctrlp;
+	int ret;
+
+	ret = pvr2_hdw_get_tuner_status(hdw, &vt);
+	if (ret != 0)
+		return ret;
+	ctrlp = pvr2_hdw_get_ctrl_by_id(hdw, PVR2_CID_INPUT);
+	ret = pvr2_ctrl_get_value(ctrlp, &cur_input);
+	if (ret != 0)
+		return ret;
+	if (vf->type == V4L2_TUNER_RADIO) {
+		if (cur_input != PVR2_CVAL_INPUT_RADIO)
+			pvr2_ctrl_set_value(ctrlp, PVR2_CVAL_INPUT_RADIO);
+	} else {
+		if (cur_input == PVR2_CVAL_INPUT_RADIO)
+			pvr2_ctrl_set_value(ctrlp, PVR2_CVAL_INPUT_TV);
+	}
+	fv = vf->frequency;
+	if (vt.capability & V4L2_TUNER_CAP_LOW)
+		fv = (fv * 125) / 2;
+	else
+		fv = fv * 62500;
+	return pvr2_ctrl_set_value(
 			pvr2_hdw_get_ctrl_by_id(hdw,PVR2_CID_FREQUENCY),fv);
-		break;
-	}
+}
 
-	case VIDIOC_G_FREQUENCY:
-	{
-		struct v4l2_frequency *vf = (struct v4l2_frequency *)arg;
-		int val = 0;
-		int cur_input;
-		struct v4l2_tuner vt;
-		ret = pvr2_hdw_get_tuner_status(hdw,&vt);
-		if (ret != 0) break;
-		ret = pvr2_ctrl_get_value(
-			pvr2_hdw_get_ctrl_by_id(hdw,PVR2_CID_FREQUENCY),
+static int pvr2_g_frequency(struct file *file, void *priv, struct v4l2_frequency *vf)
+{
+	struct pvr2_v4l2_fh *fh = file->private_data;
+	struct pvr2_hdw *hdw = fh->channel.mc_head->hdw;
+	int val = 0;
+	int cur_input;
+	struct v4l2_tuner vt;
+	int ret;
+
+	ret = pvr2_hdw_get_tuner_status(hdw, &vt);
+	if (ret != 0)
+		return ret;
+	ret = pvr2_ctrl_get_value(
+			pvr2_hdw_get_ctrl_by_id(hdw, PVR2_CID_FREQUENCY),
 			&val);
-		if (ret != 0) break;
-		pvr2_ctrl_get_value(
-			pvr2_hdw_get_ctrl_by_id(hdw,PVR2_CID_INPUT),
+	if (ret != 0)
+		return ret;
+	pvr2_ctrl_get_value(
+			pvr2_hdw_get_ctrl_by_id(hdw, PVR2_CID_INPUT),
 			&cur_input);
-		if (cur_input == PVR2_CVAL_INPUT_RADIO) {
-			vf->type = V4L2_TUNER_RADIO;
-		} else {
-			vf->type = V4L2_TUNER_ANALOG_TV;
-		}
-		if (vt.capability & V4L2_TUNER_CAP_LOW) {
-			val = (val * 2) / 125;
-		} else {
-			val /= 62500;
-		}
-		vf->frequency = val;
-		break;
-	}
+	if (cur_input == PVR2_CVAL_INPUT_RADIO)
+		vf->type = V4L2_TUNER_RADIO;
+	else
+		vf->type = V4L2_TUNER_ANALOG_TV;
+	if (vt.capability & V4L2_TUNER_CAP_LOW)
+		val = (val * 2) / 125;
+	else
+		val /= 62500;
+	vf->frequency = val;
+	return 0;
+}
 
-	case VIDIOC_ENUM_FMT:
-	{
-		struct v4l2_fmtdesc *fd = (struct v4l2_fmtdesc *)arg;
+static int pvr2_enum_fmt_vid_cap(struct file *file, void *priv, struct v4l2_fmtdesc *fd)
+{
+	/* Only one format is supported : mpeg.*/
+	if (fd->index != 0)
+		return -EINVAL;
 
-		/* Only one format is supported : mpeg.*/
-		if (fd->index != 0)
-			break;
+	memcpy(fd, pvr_fmtdesc, sizeof(struct v4l2_fmtdesc));
+	return 0;
+}
 
-		memcpy(fd, pvr_fmtdesc, sizeof(struct v4l2_fmtdesc));
-		ret = 0;
-		break;
-	}
+static int pvr2_g_fmt_vid_cap(struct file *file, void *priv, struct v4l2_format *vf)
+{
+	struct pvr2_v4l2_fh *fh = file->private_data;
+	struct pvr2_hdw *hdw = fh->channel.mc_head->hdw;
+	int val;
 
-	case VIDIOC_G_FMT:
-	{
-		struct v4l2_format *vf = (struct v4l2_format *)arg;
-		int val;
-		switch(vf->type) {
-		case V4L2_BUF_TYPE_VIDEO_CAPTURE:
-			memcpy(vf, &pvr_format[PVR_FORMAT_PIX],
-			       sizeof(struct v4l2_format));
-			val = 0;
-			pvr2_ctrl_get_value(
-				pvr2_hdw_get_ctrl_by_id(hdw,PVR2_CID_HRES),
-				&val);
-			vf->fmt.pix.width = val;
-			val = 0;
-			pvr2_ctrl_get_value(
-				pvr2_hdw_get_ctrl_by_id(hdw,PVR2_CID_VRES),
-				&val);
-			vf->fmt.pix.height = val;
-			ret = 0;
-			break;
-		case V4L2_BUF_TYPE_VBI_CAPTURE:
-			// ????? Still need to figure out to do VBI correctly
-			ret = -EINVAL;
-			break;
-		default:
-			ret = -EINVAL;
-			break;
-		}
-		break;
-	}
+	memcpy(vf, &pvr_format[PVR_FORMAT_PIX], sizeof(struct v4l2_format));
+	val = 0;
+	pvr2_ctrl_get_value(
+			pvr2_hdw_get_ctrl_by_id(hdw, PVR2_CID_HRES),
+			&val);
+	vf->fmt.pix.width = val;
+	val = 0;
+	pvr2_ctrl_get_value(
+			pvr2_hdw_get_ctrl_by_id(hdw, PVR2_CID_VRES),
+			&val);
+	vf->fmt.pix.height = val;
+	return 0;
+}
 
-	case VIDIOC_TRY_FMT:
-	case VIDIOC_S_FMT:
-	{
-		struct v4l2_format *vf = (struct v4l2_format *)arg;
-
-		ret = 0;
-		switch(vf->type) {
-		case V4L2_BUF_TYPE_VIDEO_CAPTURE: {
-			int lmin,lmax,ldef;
-			struct pvr2_ctrl *hcp,*vcp;
-			int h = vf->fmt.pix.height;
-			int w = vf->fmt.pix.width;
-			hcp = pvr2_hdw_get_ctrl_by_id(hdw,PVR2_CID_HRES);
-			vcp = pvr2_hdw_get_ctrl_by_id(hdw,PVR2_CID_VRES);
-
-			lmin = pvr2_ctrl_get_min(hcp);
-			lmax = pvr2_ctrl_get_max(hcp);
-			pvr2_ctrl_get_def(hcp, &ldef);
-			if (w == -1) {
-				w = ldef;
-			} else if (w < lmin) {
-				w = lmin;
-			} else if (w > lmax) {
-				w = lmax;
-			}
-			lmin = pvr2_ctrl_get_min(vcp);
-			lmax = pvr2_ctrl_get_max(vcp);
-			pvr2_ctrl_get_def(vcp, &ldef);
-			if (h == -1) {
-				h = ldef;
-			} else if (h < lmin) {
-				h = lmin;
-			} else if (h > lmax) {
-				h = lmax;
-			}
+static int pvr2_try_fmt_vid_cap(struct file *file, void *priv, struct v4l2_format *vf)
+{
+	struct pvr2_v4l2_fh *fh = file->private_data;
+	struct pvr2_hdw *hdw = fh->channel.mc_head->hdw;
+	int lmin, lmax, ldef;
+	struct pvr2_ctrl *hcp, *vcp;
+	int h = vf->fmt.pix.height;
+	int w = vf->fmt.pix.width;
+
+	hcp = pvr2_hdw_get_ctrl_by_id(hdw, PVR2_CID_HRES);
+	vcp = pvr2_hdw_get_ctrl_by_id(hdw, PVR2_CID_VRES);
+
+	lmin = pvr2_ctrl_get_min(hcp);
+	lmax = pvr2_ctrl_get_max(hcp);
+	pvr2_ctrl_get_def(hcp, &ldef);
+	if (w == -1)
+		w = ldef;
+	else if (w < lmin)
+		w = lmin;
+	else if (w > lmax)
+		w = lmax;
+	lmin = pvr2_ctrl_get_min(vcp);
+	lmax = pvr2_ctrl_get_max(vcp);
+	pvr2_ctrl_get_def(vcp, &ldef);
+	if (h == -1)
+		h = ldef;
+	else if (h < lmin)
+		h = lmin;
+	else if (h > lmax)
+		h = lmax;
+
+	memcpy(vf, &pvr_format[PVR_FORMAT_PIX],
+			sizeof(struct v4l2_format));
+	vf->fmt.pix.width = w;
+	vf->fmt.pix.height = h;
+	return 0;
+}
 
-			memcpy(vf, &pvr_format[PVR_FORMAT_PIX],
-			       sizeof(struct v4l2_format));
-			vf->fmt.pix.width = w;
-			vf->fmt.pix.height = h;
+static int pvr2_s_fmt_vid_cap(struct file *file, void *priv, struct v4l2_format *vf)
+{
+	struct pvr2_v4l2_fh *fh = file->private_data;
+	struct pvr2_hdw *hdw = fh->channel.mc_head->hdw;
+	struct pvr2_ctrl *hcp, *vcp;
+	int ret = pvr2_try_fmt_vid_cap(file, fh, vf);
 
-			if (cmd == VIDIOC_S_FMT) {
-				pvr2_ctrl_set_value(hcp,vf->fmt.pix.width);
-				pvr2_ctrl_set_value(vcp,vf->fmt.pix.height);
-			}
-		} break;
-		case V4L2_BUF_TYPE_VBI_CAPTURE:
-			// ????? Still need to figure out to do VBI correctly
-			ret = -EINVAL;
-			break;
-		default:
-			ret = -EINVAL;
-			break;
-		}
-		break;
-	}
+	if (ret)
+		return ret;
+	hcp = pvr2_hdw_get_ctrl_by_id(hdw, PVR2_CID_HRES);
+	vcp = pvr2_hdw_get_ctrl_by_id(hdw, PVR2_CID_VRES);
+	pvr2_ctrl_set_value(hcp, vf->fmt.pix.width);
+	pvr2_ctrl_set_value(vcp, vf->fmt.pix.height);
+	return 0;
+}
 
-	case VIDIOC_STREAMON:
-	{
-		if (!fh->pdi->stream) {
-			/* No stream defined for this node.  This means
-			   that we're not currently allowed to stream from
-			   this node. */
-			ret = -EPERM;
-			break;
-		}
-		ret = pvr2_hdw_set_stream_type(hdw,pdi->config);
-		if (ret < 0) return ret;
-		ret = pvr2_hdw_set_streaming(hdw,!0);
-		break;
+static int pvr2_streamon(struct file *file, void *priv, enum v4l2_buf_type i)
+{
+	struct pvr2_v4l2_fh *fh = file->private_data;
+	struct pvr2_hdw *hdw = fh->channel.mc_head->hdw;
+	struct pvr2_v4l2_dev *pdi = fh->pdi;
+	int ret;
+
+	if (!fh->pdi->stream) {
+		/* No stream defined for this node.  This means
+		   that we're not currently allowed to stream from
+		   this node. */
+		return -EPERM;
 	}
+	ret = pvr2_hdw_set_stream_type(hdw, pdi->config);
+	if (ret < 0)
+		return ret;
+	return pvr2_hdw_set_streaming(hdw, !0);
+}
 
-	case VIDIOC_STREAMOFF:
-	{
-		if (!fh->pdi->stream) {
-			/* No stream defined for this node.  This means
-			   that we're not currently allowed to stream from
-			   this node. */
-			ret = -EPERM;
-			break;
-		}
-		ret = pvr2_hdw_set_streaming(hdw,0);
-		break;
+static int pvr2_streamoff(struct file *file, void *priv, enum v4l2_buf_type i)
+{
+	struct pvr2_v4l2_fh *fh = file->private_data;
+	struct pvr2_hdw *hdw = fh->channel.mc_head->hdw;
+
+	if (!fh->pdi->stream) {
+		/* No stream defined for this node.  This means
+		   that we're not currently allowed to stream from
+		   this node. */
+		return -EPERM;
 	}
+	return pvr2_hdw_set_streaming(hdw, 0);
+}
 
-	case VIDIOC_QUERYCTRL:
-	{
-		struct pvr2_ctrl *cptr;
-		int val;
-		struct v4l2_queryctrl *vc = (struct v4l2_queryctrl *)arg;
-		ret = 0;
-		if (vc->id & V4L2_CTRL_FLAG_NEXT_CTRL) {
-			cptr = pvr2_hdw_get_ctrl_nextv4l(
-				hdw,(vc->id & ~V4L2_CTRL_FLAG_NEXT_CTRL));
-			if (cptr) vc->id = pvr2_ctrl_get_v4lid(cptr);
-		} else {
-			cptr = pvr2_hdw_get_ctrl_v4l(hdw,vc->id);
-		}
-		if (!cptr) {
-			pvr2_trace(PVR2_TRACE_V4LIOCTL,
-				   "QUERYCTRL id=0x%x not implemented here",
-				   vc->id);
-			ret = -EINVAL;
-			break;
-		}
+static int pvr2_queryctrl(struct file *file, void *priv,
+		struct v4l2_queryctrl *vc)
+{
+	struct pvr2_v4l2_fh *fh = file->private_data;
+	struct pvr2_hdw *hdw = fh->channel.mc_head->hdw;
+	struct pvr2_ctrl *cptr;
+	int val;
+	int ret;
 
+	ret = 0;
+	if (vc->id & V4L2_CTRL_FLAG_NEXT_CTRL) {
+		cptr = pvr2_hdw_get_ctrl_nextv4l(
+				hdw, (vc->id & ~V4L2_CTRL_FLAG_NEXT_CTRL));
+		if (cptr)
+			vc->id = pvr2_ctrl_get_v4lid(cptr);
+	} else {
+		cptr = pvr2_hdw_get_ctrl_v4l(hdw, vc->id);
+	}
+	if (!cptr) {
 		pvr2_trace(PVR2_TRACE_V4LIOCTL,
-			   "QUERYCTRL id=0x%x mapping name=%s (%s)",
-			   vc->id,pvr2_ctrl_get_name(cptr),
-			   pvr2_ctrl_get_desc(cptr));
-		strlcpy(vc->name,pvr2_ctrl_get_desc(cptr),sizeof(vc->name));
-		vc->flags = pvr2_ctrl_get_v4lflags(cptr);
-		pvr2_ctrl_get_def(cptr, &val);
-		vc->default_value = val;
-		switch (pvr2_ctrl_get_type(cptr)) {
-		case pvr2_ctl_enum:
-			vc->type = V4L2_CTRL_TYPE_MENU;
-			vc->minimum = 0;
-			vc->maximum = pvr2_ctrl_get_cnt(cptr) - 1;
-			vc->step = 1;
-			break;
-		case pvr2_ctl_bool:
-			vc->type = V4L2_CTRL_TYPE_BOOLEAN;
-			vc->minimum = 0;
-			vc->maximum = 1;
-			vc->step = 1;
-			break;
-		case pvr2_ctl_int:
-			vc->type = V4L2_CTRL_TYPE_INTEGER;
-			vc->minimum = pvr2_ctrl_get_min(cptr);
-			vc->maximum = pvr2_ctrl_get_max(cptr);
-			vc->step = 1;
-			break;
-		default:
-			pvr2_trace(PVR2_TRACE_V4LIOCTL,
-				   "QUERYCTRL id=0x%x name=%s not mappable",
-				   vc->id,pvr2_ctrl_get_name(cptr));
-			ret = -EINVAL;
-			break;
-		}
+				"QUERYCTRL id=0x%x not implemented here",
+				vc->id);
+		return -EINVAL;
+	}
+
+	pvr2_trace(PVR2_TRACE_V4LIOCTL,
+			"QUERYCTRL id=0x%x mapping name=%s (%s)",
+			vc->id, pvr2_ctrl_get_name(cptr),
+			pvr2_ctrl_get_desc(cptr));
+	strlcpy(vc->name, pvr2_ctrl_get_desc(cptr), sizeof(vc->name));
+	vc->flags = pvr2_ctrl_get_v4lflags(cptr);
+	pvr2_ctrl_get_def(cptr, &val);
+	vc->default_value = val;
+	switch (pvr2_ctrl_get_type(cptr)) {
+	case pvr2_ctl_enum:
+		vc->type = V4L2_CTRL_TYPE_MENU;
+		vc->minimum = 0;
+		vc->maximum = pvr2_ctrl_get_cnt(cptr) - 1;
+		vc->step = 1;
 		break;
-	}
-
-	case VIDIOC_QUERYMENU:
-	{
-		struct v4l2_querymenu *vm = (struct v4l2_querymenu *)arg;
-		unsigned int cnt = 0;
-		ret = pvr2_ctrl_get_valname(pvr2_hdw_get_ctrl_v4l(hdw,vm->id),
-					    vm->index,
-					    vm->name,sizeof(vm->name)-1,
-					    &cnt);
-		vm->name[cnt] = 0;
+	case pvr2_ctl_bool:
+		vc->type = V4L2_CTRL_TYPE_BOOLEAN;
+		vc->minimum = 0;
+		vc->maximum = 1;
+		vc->step = 1;
 		break;
-	}
-
-	case VIDIOC_G_CTRL:
-	{
-		struct v4l2_control *vc = (struct v4l2_control *)arg;
-		int val = 0;
-		ret = pvr2_ctrl_get_value(pvr2_hdw_get_ctrl_v4l(hdw,vc->id),
-					  &val);
-		vc->value = val;
+	case pvr2_ctl_int:
+		vc->type = V4L2_CTRL_TYPE_INTEGER;
+		vc->minimum = pvr2_ctrl_get_min(cptr);
+		vc->maximum = pvr2_ctrl_get_max(cptr);
+		vc->step = 1;
 		break;
+	default:
+		pvr2_trace(PVR2_TRACE_V4LIOCTL,
+				"QUERYCTRL id=0x%x name=%s not mappable",
+				vc->id, pvr2_ctrl_get_name(cptr));
+		return -EINVAL;
 	}
+	return 0;
+}
 
-	case VIDIOC_S_CTRL:
-	{
-		struct v4l2_control *vc = (struct v4l2_control *)arg;
-		ret = pvr2_ctrl_set_value(pvr2_hdw_get_ctrl_v4l(hdw,vc->id),
-					  vc->value);
-		break;
-	}
+static int pvr2_querymenu(struct file *file, void *priv, struct v4l2_querymenu *vm)
+{
+	struct pvr2_v4l2_fh *fh = file->private_data;
+	struct pvr2_hdw *hdw = fh->channel.mc_head->hdw;
+	unsigned int cnt = 0;
+	int ret;
 
-	case VIDIOC_G_EXT_CTRLS:
-	{
-		struct v4l2_ext_controls *ctls =
-			(struct v4l2_ext_controls *)arg;
-		struct v4l2_ext_control *ctrl;
-		unsigned int idx;
-		int val;
-		ret = 0;
-		for (idx = 0; idx < ctls->count; idx++) {
-			ctrl = ctls->controls + idx;
-			ret = pvr2_ctrl_get_value(
-				pvr2_hdw_get_ctrl_v4l(hdw,ctrl->id),&val);
-			if (ret) {
-				ctls->error_idx = idx;
-				break;
-			}
-			/* Ensure that if read as a 64 bit value, the user
-			   will still get a hopefully sane value */
-			ctrl->value64 = 0;
-			ctrl->value = val;
+	ret = pvr2_ctrl_get_valname(pvr2_hdw_get_ctrl_v4l(hdw, vm->id),
+			vm->index,
+			vm->name, sizeof(vm->name) - 1,
+			&cnt);
+	vm->name[cnt] = 0;
+	return ret;
+}
+
+static int pvr2_g_ctrl(struct file *file, void *priv, struct v4l2_control *vc)
+{
+	struct pvr2_v4l2_fh *fh = file->private_data;
+	struct pvr2_hdw *hdw = fh->channel.mc_head->hdw;
+	int val = 0;
+	int ret;
+
+	ret = pvr2_ctrl_get_value(pvr2_hdw_get_ctrl_v4l(hdw, vc->id),
+			&val);
+	vc->value = val;
+	return ret;
+}
+
+static int pvr2_s_ctrl(struct file *file, void *priv, struct v4l2_control *vc)
+{
+	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_v4l(hdw, vc->id),
+			vc->value);
+}
+
+static int pvr2_g_ext_ctrls(struct file *file, void *priv,
+					struct v4l2_ext_controls *ctls)
+{
+	struct pvr2_v4l2_fh *fh = file->private_data;
+	struct pvr2_hdw *hdw = fh->channel.mc_head->hdw;
+	struct v4l2_ext_control *ctrl;
+	unsigned int idx;
+	int val;
+	int ret;
+
+	ret = 0;
+	for (idx = 0; idx < ctls->count; idx++) {
+		ctrl = ctls->controls + idx;
+		ret = pvr2_ctrl_get_value(
+				pvr2_hdw_get_ctrl_v4l(hdw, ctrl->id), &val);
+		if (ret) {
+			ctls->error_idx = idx;
+			return ret;
 		}
-		break;
+		/* Ensure that if read as a 64 bit value, the user
+		   will still get a hopefully sane value */
+		ctrl->value64 = 0;
+		ctrl->value = val;
 	}
+	return 0;
+}
 
-	case VIDIOC_S_EXT_CTRLS:
-	{
-		struct v4l2_ext_controls *ctls =
-			(struct v4l2_ext_controls *)arg;
-		struct v4l2_ext_control *ctrl;
-		unsigned int idx;
-		ret = 0;
-		for (idx = 0; idx < ctls->count; idx++) {
-			ctrl = ctls->controls + idx;
-			ret = pvr2_ctrl_set_value(
-				pvr2_hdw_get_ctrl_v4l(hdw,ctrl->id),
+static int pvr2_s_ext_ctrls(struct file *file, void *priv,
+		struct v4l2_ext_controls *ctls)
+{
+	struct pvr2_v4l2_fh *fh = file->private_data;
+	struct pvr2_hdw *hdw = fh->channel.mc_head->hdw;
+	struct v4l2_ext_control *ctrl;
+	unsigned int idx;
+	int ret;
+
+	ret = 0;
+	for (idx = 0; idx < ctls->count; idx++) {
+		ctrl = ctls->controls + idx;
+		ret = pvr2_ctrl_set_value(
+				pvr2_hdw_get_ctrl_v4l(hdw, ctrl->id),
 				ctrl->value);
-			if (ret) {
-				ctls->error_idx = idx;
-				break;
-			}
+		if (ret) {
+			ctls->error_idx = idx;
+			return ret;
 		}
-		break;
 	}
+	return 0;
+}
 
-	case VIDIOC_TRY_EXT_CTRLS:
-	{
-		struct v4l2_ext_controls *ctls =
-			(struct v4l2_ext_controls *)arg;
-		struct v4l2_ext_control *ctrl;
-		struct pvr2_ctrl *pctl;
-		unsigned int idx;
-		/* For the moment just validate that the requested control
-		   actually exists. */
-		ret = 0;
-		for (idx = 0; idx < ctls->count; idx++) {
-			ctrl = ctls->controls + idx;
-			pctl = pvr2_hdw_get_ctrl_v4l(hdw,ctrl->id);
-			if (!pctl) {
-				ret = -EINVAL;
-				ctls->error_idx = idx;
-				break;
-			}
-		}
-		break;
-	}
+static int pvr2_try_ext_ctrls(struct file *file, void *priv,
+		struct v4l2_ext_controls *ctls)
+{
+	struct pvr2_v4l2_fh *fh = file->private_data;
+	struct pvr2_hdw *hdw = fh->channel.mc_head->hdw;
+	struct v4l2_ext_control *ctrl;
+	struct pvr2_ctrl *pctl;
+	unsigned int idx;
+	int ret;
 
-	case VIDIOC_CROPCAP:
-	{
-		struct v4l2_cropcap *cap = (struct v4l2_cropcap *)arg;
-		if (cap->type != V4L2_BUF_TYPE_VIDEO_CAPTURE) {
-			ret = -EINVAL;
-			break;
+	/* For the moment just validate that the requested control
+	   actually exists. */
+	ret = 0;
+	for (idx = 0; idx < ctls->count; idx++) {
+		ctrl = ctls->controls + idx;
+		pctl = pvr2_hdw_get_ctrl_v4l(hdw, ctrl->id);
+		if (!pctl) {
+			ctls->error_idx = idx;
+			return -EINVAL;
 		}
-		ret = pvr2_hdw_get_cropcap(hdw, cap);
-		cap->type = V4L2_BUF_TYPE_VIDEO_CAPTURE; /* paranoia */
-		break;
 	}
-	case VIDIOC_G_CROP:
-	{
-		struct v4l2_crop *crop = (struct v4l2_crop *)arg;
-		int val = 0;
-		if (crop->type != V4L2_BUF_TYPE_VIDEO_CAPTURE) {
-			ret = -EINVAL;
-			break;
-		}
-		ret = pvr2_ctrl_get_value(
+	return 0;
+}
+
+static int pvr2_cropcap(struct file *file, void *priv, struct v4l2_cropcap *cap)
+{
+	struct pvr2_v4l2_fh *fh = file->private_data;
+	struct pvr2_hdw *hdw = fh->channel.mc_head->hdw;
+	int ret;
+
+	if (cap->type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
+		return -EINVAL;
+	ret = pvr2_hdw_get_cropcap(hdw, cap);
+	cap->type = V4L2_BUF_TYPE_VIDEO_CAPTURE; /* paranoia */
+	return ret;
+}
+
+static int pvr2_g_crop(struct file *file, void *priv, struct v4l2_crop *crop)
+{
+	struct pvr2_v4l2_fh *fh = file->private_data;
+	struct pvr2_hdw *hdw = fh->channel.mc_head->hdw;
+	int val = 0;
+	int ret;
+
+	if (crop->type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
+		return -EINVAL;
+	ret = pvr2_ctrl_get_value(
 			pvr2_hdw_get_ctrl_by_id(hdw, PVR2_CID_CROPL), &val);
-		if (ret != 0) {
-			ret = -EINVAL;
-			break;
-		}
-		crop->c.left = val;
-		ret = pvr2_ctrl_get_value(
+	if (ret != 0)
+		return -EINVAL;
+	crop->c.left = val;
+	ret = pvr2_ctrl_get_value(
 			pvr2_hdw_get_ctrl_by_id(hdw, PVR2_CID_CROPT), &val);
-		if (ret != 0) {
-			ret = -EINVAL;
-			break;
-		}
-		crop->c.top = val;
-		ret = pvr2_ctrl_get_value(
+	if (ret != 0)
+		return -EINVAL;
+	crop->c.top = val;
+	ret = pvr2_ctrl_get_value(
 			pvr2_hdw_get_ctrl_by_id(hdw, PVR2_CID_CROPW), &val);
-		if (ret != 0) {
-			ret = -EINVAL;
-			break;
-		}
-		crop->c.width = val;
-		ret = pvr2_ctrl_get_value(
+	if (ret != 0)
+		return -EINVAL;
+	crop->c.width = val;
+	ret = pvr2_ctrl_get_value(
 			pvr2_hdw_get_ctrl_by_id(hdw, PVR2_CID_CROPH), &val);
-		if (ret != 0) {
-			ret = -EINVAL;
-			break;
-		}
-		crop->c.height = val;
-	}
-	case VIDIOC_S_CROP:
-	{
-		struct v4l2_crop *crop = (struct v4l2_crop *)arg;
-		if (crop->type != V4L2_BUF_TYPE_VIDEO_CAPTURE) {
-			ret = -EINVAL;
-			break;
-		}
-		ret = pvr2_ctrl_set_value(
+	if (ret != 0)
+		return -EINVAL;
+	crop->c.height = val;
+	return 0;
+}
+
+static int pvr2_s_crop(struct file *file, void *priv, struct v4l2_crop *crop)
+{
+	struct pvr2_v4l2_fh *fh = file->private_data;
+	struct pvr2_hdw *hdw = fh->channel.mc_head->hdw;
+	struct v4l2_cropcap cap;
+	int ret;
+
+	if (crop->type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
+		return -EINVAL;
+	cap.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+	ret = pvr2_ctrl_set_value(
 			pvr2_hdw_get_ctrl_by_id(hdw, PVR2_CID_CROPL),
 			crop->c.left);
-		if (ret != 0) {
-			ret = -EINVAL;
-			break;
-		}
-		ret = pvr2_ctrl_set_value(
+	if (ret != 0)
+		return -EINVAL;
+	ret = pvr2_ctrl_set_value(
 			pvr2_hdw_get_ctrl_by_id(hdw, PVR2_CID_CROPT),
 			crop->c.top);
-		if (ret != 0) {
-			ret = -EINVAL;
-			break;
-		}
-		ret = pvr2_ctrl_set_value(
+	if (ret != 0)
+		return -EINVAL;
+	ret = pvr2_ctrl_set_value(
 			pvr2_hdw_get_ctrl_by_id(hdw, PVR2_CID_CROPW),
 			crop->c.width);
-		if (ret != 0) {
-			ret = -EINVAL;
-			break;
-		}
-		ret = pvr2_ctrl_set_value(
+	if (ret != 0)
+		return -EINVAL;
+	ret = pvr2_ctrl_set_value(
 			pvr2_hdw_get_ctrl_by_id(hdw, PVR2_CID_CROPH),
 			crop->c.height);
-		if (ret != 0) {
-			ret = -EINVAL;
-			break;
-		}
-	}
-	case VIDIOC_LOG_STATUS:
-	{
-		pvr2_hdw_trigger_module_log(hdw);
-		ret = 0;
-		break;
-	}
+	if (ret != 0)
+		return -EINVAL;
+	return 0;
+}
+
+static int pvr2_log_status(struct file *file, void *priv)
+{
+	struct pvr2_v4l2_fh *fh = file->private_data;
+	struct pvr2_hdw *hdw = fh->channel.mc_head->hdw;
+
+	pvr2_hdw_trigger_module_log(hdw);
+	return 0;
+}
+
 #ifdef CONFIG_VIDEO_ADV_DEBUG
-	case VIDIOC_DBG_S_REGISTER:
-	case VIDIOC_DBG_G_REGISTER:
-	{
-		u64 val;
-		struct v4l2_dbg_register *req = (struct v4l2_dbg_register *)arg;
-		if (cmd == VIDIOC_DBG_S_REGISTER) val = req->val;
-		ret = pvr2_hdw_register_access(
-			hdw, &req->match, req->reg,
-			cmd == VIDIOC_DBG_S_REGISTER, &val);
-		if (cmd == VIDIOC_DBG_G_REGISTER) req->val = val;
-		break;
-	}
-#endif
+static int pvr2_g_register(struct file *file, void *priv, struct v4l2_dbg_register *req)
+{
+	struct pvr2_v4l2_fh *fh = file->private_data;
+	struct pvr2_hdw *hdw = fh->channel.mc_head->hdw;
+	u64 val;
+	int ret;
 
-	default :
-		ret = -ENOTTY;
-		break;
-	}
+	ret = pvr2_hdw_register_access(
+			hdw, &req->match, req->reg,
+			0, &val);
+	req->val = val;
+	return ret;
+}
 
-	pvr2_hdw_commit_ctl(hdw);
+static int pvr2_s_register(struct file *file, void *priv, struct v4l2_dbg_register *req)
+{
+	struct pvr2_v4l2_fh *fh = file->private_data;
+	struct pvr2_hdw *hdw = fh->channel.mc_head->hdw;
+	u64 val;
+	int ret;
 
-	if (ret < 0) {
-		if (pvrusb2_debug & PVR2_TRACE_V4LIOCTL) {
-			pvr2_trace(PVR2_TRACE_V4LIOCTL,
-				   "pvr2_v4l2_do_ioctl failure, ret=%ld", ret);
-		} else {
-			if (pvrusb2_debug & PVR2_TRACE_V4LIOCTL) {
-				pvr2_trace(PVR2_TRACE_V4LIOCTL,
-					   "pvr2_v4l2_do_ioctl failure, ret=%ld"
-					   " command was:", ret);
-				v4l_print_ioctl(pvr2_hdw_get_driver_name(hdw),
-						cmd);
-			}
-		}
-	} else {
-		pvr2_trace(PVR2_TRACE_V4LIOCTL,
-			   "pvr2_v4l2_do_ioctl complete, ret=%ld (0x%lx)",
-			   ret, ret);
-	}
+	val = req->val;
+	ret = pvr2_hdw_register_access(
+			hdw, &req->match, req->reg,
+			1, &val);
 	return ret;
 }
+#endif
+
+static const struct v4l2_ioctl_ops pvr2_ioctl_ops = {
+	.vidioc_querycap		    = pvr2_querycap,
+	.vidioc_g_priority		    = pvr2_g_priority,
+	.vidioc_s_priority		    = pvr2_s_priority,
+	.vidioc_s_audio			    = pvr2_s_audio,
+	.vidioc_g_audio			    = pvr2_g_audio,
+	.vidioc_enumaudio		    = pvr2_enumaudio,
+	.vidioc_enum_input		    = pvr2_enum_input,
+	.vidioc_cropcap			    = pvr2_cropcap,
+	.vidioc_s_crop			    = pvr2_s_crop,
+	.vidioc_g_crop			    = pvr2_g_crop,
+	.vidioc_g_input			    = pvr2_g_input,
+	.vidioc_s_input			    = pvr2_s_input,
+	.vidioc_g_frequency		    = pvr2_g_frequency,
+	.vidioc_s_frequency		    = pvr2_s_frequency,
+	.vidioc_s_tuner			    = pvr2_s_tuner,
+	.vidioc_g_tuner			    = pvr2_g_tuner,
+	.vidioc_g_std			    = pvr2_g_std,
+	.vidioc_s_std			    = pvr2_s_std,
+	.vidioc_querystd		    = pvr2_querystd,
+	.vidioc_log_status		    = pvr2_log_status,
+	.vidioc_enum_fmt_vid_cap	    = pvr2_enum_fmt_vid_cap,
+	.vidioc_g_fmt_vid_cap		    = pvr2_g_fmt_vid_cap,
+	.vidioc_s_fmt_vid_cap		    = pvr2_s_fmt_vid_cap,
+	.vidioc_try_fmt_vid_cap		    = pvr2_try_fmt_vid_cap,
+	.vidioc_streamon		    = pvr2_streamon,
+	.vidioc_streamoff		    = pvr2_streamoff,
+	.vidioc_queryctrl		    = pvr2_queryctrl,
+	.vidioc_querymenu		    = pvr2_querymenu,
+	.vidioc_g_ctrl			    = pvr2_g_ctrl,
+	.vidioc_s_ctrl			    = pvr2_s_ctrl,
+	.vidioc_g_ext_ctrls		    = pvr2_g_ext_ctrls,
+	.vidioc_s_ext_ctrls		    = pvr2_s_ext_ctrls,
+	.vidioc_try_ext_ctrls		    = pvr2_try_ext_ctrls,
+#ifdef CONFIG_VIDEO_ADV_DEBUG
+	.vidioc_g_register		    = pvr2_g_register,
+	.vidioc_s_register		    = pvr2_s_register,
+#endif
+};
 
 static void pvr2_v4l2_dev_destroy(struct pvr2_v4l2_dev *dip)
 {
@@ -961,7 +959,56 @@ static long pvr2_v4l2_ioctl(struct file *file,
 			   unsigned int cmd, unsigned long arg)
 {
 
-	return video_usercopy(file, cmd, arg, pvr2_v4l2_do_ioctl);
+	struct pvr2_v4l2_fh *fh = file->private_data;
+	struct pvr2_v4l2 *vp = fh->vhead;
+	struct pvr2_hdw *hdw = fh->channel.mc_head->hdw;
+	long ret = -EINVAL;
+
+	if (pvrusb2_debug & PVR2_TRACE_V4LIOCTL)
+		v4l_print_ioctl(pvr2_hdw_get_driver_name(hdw), cmd);
+
+	if (!pvr2_hdw_dev_ok(hdw)) {
+		pvr2_trace(PVR2_TRACE_ERROR_LEGS,
+			   "ioctl failed - bad or no context");
+		return -EFAULT;
+	}
+
+	/* check priority */
+	switch (cmd) {
+	case VIDIOC_S_CTRL:
+	case VIDIOC_S_STD:
+	case VIDIOC_S_INPUT:
+	case VIDIOC_S_TUNER:
+	case VIDIOC_S_FREQUENCY:
+		ret = v4l2_prio_check(&vp->prio, fh->prio);
+		if (ret)
+			return ret;
+	}
+
+	ret = video_ioctl2(file, cmd, arg);
+
+	pvr2_hdw_commit_ctl(hdw);
+
+	if (ret < 0) {
+		if (pvrusb2_debug & PVR2_TRACE_V4LIOCTL) {
+			pvr2_trace(PVR2_TRACE_V4LIOCTL,
+				   "pvr2_v4l2_do_ioctl failure, ret=%ld", ret);
+		} else {
+			if (pvrusb2_debug & PVR2_TRACE_V4LIOCTL) {
+				pvr2_trace(PVR2_TRACE_V4LIOCTL,
+					   "pvr2_v4l2_do_ioctl failure, ret=%ld"
+					   " command was:", ret);
+				v4l_print_ioctl(pvr2_hdw_get_driver_name(hdw),
+						cmd);
+			}
+		}
+	} else {
+		pvr2_trace(PVR2_TRACE_V4LIOCTL,
+			   "pvr2_v4l2_do_ioctl complete, ret=%ld (0x%lx)",
+			   ret, ret);
+	}
+	return ret;
+
 }
 
 
@@ -1262,10 +1309,12 @@ static void pvr2_v4l2_dev_init(struct pvr2_v4l2_dev *dip,
 	struct usb_device *usbdev;
 	int mindevnum;
 	int unit_number;
+	struct pvr2_hdw *hdw;
 	int *nr_ptr = NULL;
 	dip->v4lp = vp;
 
-	usbdev = pvr2_hdw_get_dev(vp->channel.mc_head->hdw);
+	hdw = vp->channel.mc_head->hdw;
+	usbdev = pvr2_hdw_get_dev(hdw);
 	dip->v4l_type = v4l_type;
 	switch (v4l_type) {
 	case VFL_TYPE_GRABBER:
@@ -1300,9 +1349,17 @@ static void pvr2_v4l2_dev_init(struct pvr2_v4l2_dev *dip,
 
 	memcpy(&dip->devbase,&vdev_template,sizeof(vdev_template));
 	dip->devbase.release = pvr2_video_device_release;
+	dip->devbase.ioctl_ops = &pvr2_ioctl_ops;
+	{
+		int val;
+		pvr2_ctrl_get_value(
+			pvr2_hdw_get_ctrl_by_id(hdw,
+						PVR2_CID_STDAVAIL), &val);
+		dip->devbase.tvnorms = (v4l2_std_id)val;
+	}
 
 	mindevnum = -1;
-	unit_number = pvr2_hdw_get_unit_number(vp->channel.mc_head->hdw);
+	unit_number = pvr2_hdw_get_unit_number(hdw);
 	if (nr_ptr && (unit_number >= 0) && (unit_number < PVR_NUM)) {
 		mindevnum = nr_ptr[unit_number];
 	}
@@ -1319,7 +1376,7 @@ static void pvr2_v4l2_dev_init(struct pvr2_v4l2_dev *dip,
 	       video_device_node_name(&dip->devbase),
 	       pvr2_config_get_name(dip->config));
 
-	pvr2_hdw_v4l_store_minor_number(vp->channel.mc_head->hdw,
+	pvr2_hdw_v4l_store_minor_number(hdw,
 					dip->minor_type,dip->devbase.minor);
 }
 
diff --git a/drivers/media/video/pwc/pwc-if.c b/drivers/media/video/pwc/pwc-if.c
index 122fbd0081e..ec4e2ef54e6 100644
--- a/drivers/media/video/pwc/pwc-if.c
+++ b/drivers/media/video/pwc/pwc-if.c
@@ -357,6 +357,7 @@ handler_end:
 		PWC_ERROR("Error (%d) re-submitting urb in pwc_isoc_handler.\n", i);
 }
 
+/* Both v4l2_lock and vb_queue_lock should be locked when calling this */
 static int pwc_isoc_init(struct pwc_device *pdev)
 {
 	struct usb_device *udev;
@@ -366,9 +367,6 @@ static int pwc_isoc_init(struct pwc_device *pdev)
 	struct usb_host_interface *idesc = NULL;
 	int compression = 0; /* 0..3 = uncompressed..high */
 
-	if (pdev->iso_init)
-		return 0;
-
 	pdev->vsync = 0;
 	pdev->vlast_packet_size = 0;
 	pdev->fill_buf = NULL;
@@ -418,7 +416,6 @@ retry:
 		urb = usb_alloc_urb(ISO_FRAMES_PER_DESC, GFP_KERNEL);
 		if (urb == NULL) {
 			PWC_ERROR("Failed to allocate urb %d\n", i);
-			pdev->iso_init = 1;
 			pwc_isoc_cleanup(pdev);
 			return -ENOMEM;
 		}
@@ -435,7 +432,6 @@ retry:
 							  &urb->transfer_dma);
 		if (urb->transfer_buffer == NULL) {
 			PWC_ERROR("Failed to allocate urb buffer %d\n", i);
-			pdev->iso_init = 1;
 			pwc_isoc_cleanup(pdev);
 			return -ENOMEM;
 		}
@@ -455,13 +451,11 @@ retry:
 		ret = usb_submit_urb(pdev->urbs[i], GFP_KERNEL);
 		if (ret == -ENOSPC && compression < 3) {
 			compression++;
-			pdev->iso_init = 1;
 			pwc_isoc_cleanup(pdev);
 			goto retry;
 		}
 		if (ret) {
 			PWC_ERROR("isoc_init() submit_urb %d failed with error %d\n", i, ret);
-			pdev->iso_init = 1;
 			pwc_isoc_cleanup(pdev);
 			return ret;
 		}
@@ -469,7 +463,6 @@ retry:
 	}
 
 	/* All is done... */
-	pdev->iso_init = 1;
 	PWC_DEBUG_OPEN("<< pwc_isoc_init()\n");
 	return 0;
 }
@@ -507,21 +500,19 @@ static void pwc_iso_free(struct pwc_device *pdev)
 	}
 }
 
+/* Both v4l2_lock and vb_queue_lock should be locked when calling this */
 static void pwc_isoc_cleanup(struct pwc_device *pdev)
 {
 	PWC_DEBUG_OPEN(">> pwc_isoc_cleanup()\n");
 
-	if (pdev->iso_init == 0)
-		return;
-
 	pwc_iso_stop(pdev);
 	pwc_iso_free(pdev);
 	usb_set_interface(pdev->udev, 0, 0);
 
-	pdev->iso_init = 0;
 	PWC_DEBUG_OPEN("<< pwc_isoc_cleanup()\n");
 }
 
+/* Must be called with vb_queue_lock hold */
 static void pwc_cleanup_queued_bufs(struct pwc_device *pdev)
 {
 	unsigned long flags = 0;
@@ -573,18 +564,13 @@ static const char *pwc_sensor_type_to_string(unsigned int sensor_type)
 
 int pwc_test_n_set_capt_file(struct pwc_device *pdev, struct file *file)
 {
-	int r = 0;
-
-	mutex_lock(&pdev->capt_file_lock);
 	if (pdev->capt_file != NULL &&
-	    pdev->capt_file != file) {
-		r = -EBUSY;
-		goto leave;
-	}
+	    pdev->capt_file != file)
+		return -EBUSY;
+
 	pdev->capt_file = file;
-leave:
-	mutex_unlock(&pdev->capt_file_lock);
-	return r;
+
+	return 0;
 }
 
 static void pwc_video_release(struct v4l2_device *v)
@@ -592,6 +578,7 @@ static void pwc_video_release(struct v4l2_device *v)
 	struct pwc_device *pdev = container_of(v, struct pwc_device, v4l2_dev);
 
 	v4l2_ctrl_handler_free(&pdev->ctrl_handler);
+	v4l2_device_unregister(&pdev->v4l2_dev);
 	kfree(pdev->ctrl_buf);
 	kfree(pdev);
 }
@@ -600,10 +587,25 @@ static int pwc_video_close(struct file *file)
 {
 	struct pwc_device *pdev = video_drvdata(file);
 
+	/*
+	 * If we're still streaming vb2_queue_release will call stream_stop
+	 * so we must take both the v4l2_lock and the vb_queue_lock.
+	 */
+	if (mutex_lock_interruptible(&pdev->v4l2_lock))
+		return -ERESTARTSYS;
+	if (mutex_lock_interruptible(&pdev->vb_queue_lock)) {
+		mutex_unlock(&pdev->v4l2_lock);
+		return -ERESTARTSYS;
+	}
+
 	if (pdev->capt_file == file) {
 		vb2_queue_release(&pdev->vb_queue);
 		pdev->capt_file = NULL;
 	}
+
+	mutex_unlock(&pdev->vb_queue_lock);
+	mutex_unlock(&pdev->v4l2_lock);
+
 	return v4l2_fh_release(file);
 }
 
@@ -611,35 +613,81 @@ static ssize_t pwc_video_read(struct file *file, char __user *buf,
 			      size_t count, loff_t *ppos)
 {
 	struct pwc_device *pdev = video_drvdata(file);
+	int lock_v4l2 = 0;
+	ssize_t ret;
 
-	if (!pdev->udev)
-		return -ENODEV;
+	if (mutex_lock_interruptible(&pdev->vb_queue_lock))
+		return -ERESTARTSYS;
 
-	if (pwc_test_n_set_capt_file(pdev, file))
-		return -EBUSY;
+	ret = pwc_test_n_set_capt_file(pdev, file);
+	if (ret)
+		goto out;
 
-	return vb2_read(&pdev->vb_queue, buf, count, ppos,
-			file->f_flags & O_NONBLOCK);
+	/* stream_start will get called so we must take the v4l2_lock */
+	if (pdev->vb_queue.fileio == NULL)
+		lock_v4l2 = 1;
+
+	/* Use try_lock, since we're taking the locks in the *wrong* order! */
+	if (lock_v4l2 && !mutex_trylock(&pdev->v4l2_lock)) {
+		ret = -ERESTARTSYS;
+		goto out;
+	}
+	ret = vb2_read(&pdev->vb_queue, buf, count, ppos,
+		       file->f_flags & O_NONBLOCK);
+	if (lock_v4l2)
+		mutex_unlock(&pdev->v4l2_lock);
+out:
+	mutex_unlock(&pdev->vb_queue_lock);
+	return ret;
 }
 
 static unsigned int pwc_video_poll(struct file *file, poll_table *wait)
 {
 	struct pwc_device *pdev = video_drvdata(file);
+	struct vb2_queue *q = &pdev->vb_queue;
+	unsigned long req_events = poll_requested_events(wait);
+	unsigned int ret = POLL_ERR;
+	int lock_v4l2 = 0;
 
-	if (!pdev->udev)
+	if (mutex_lock_interruptible(&pdev->vb_queue_lock))
 		return POLL_ERR;
 
-	return vb2_poll(&pdev->vb_queue, file, wait);
+	/* Will this start fileio and thus call start_stream? */
+	if ((req_events & (POLLIN | POLLRDNORM)) &&
+	    q->num_buffers == 0 && !q->streaming && q->fileio == NULL) {
+		if (pwc_test_n_set_capt_file(pdev, file))
+			goto out;
+		lock_v4l2 = 1;
+	}
+
+	/* Use try_lock, since we're taking the locks in the *wrong* order! */
+	if (lock_v4l2 && !mutex_trylock(&pdev->v4l2_lock))
+		goto out;
+	ret = vb2_poll(&pdev->vb_queue, file, wait);
+	if (lock_v4l2)
+		mutex_unlock(&pdev->v4l2_lock);
+
+out:
+	if (!pdev->udev)
+		ret |= POLLHUP;
+	mutex_unlock(&pdev->vb_queue_lock);
+	return ret;
 }
 
 static int pwc_video_mmap(struct file *file, struct vm_area_struct *vma)
 {
 	struct pwc_device *pdev = video_drvdata(file);
+	int ret;
 
-	if (pdev->capt_file != file)
-		return -EBUSY;
+	if (mutex_lock_interruptible(&pdev->vb_queue_lock))
+		return -ERESTARTSYS;
 
-	return vb2_mmap(&pdev->vb_queue, vma);
+	ret = pwc_test_n_set_capt_file(pdev, file);
+	if (ret == 0)
+		ret = vb2_mmap(&pdev->vb_queue, vma);
+
+	mutex_unlock(&pdev->vb_queue_lock);
+	return ret;
 }
 
 /***************************************************************************/
@@ -715,12 +763,14 @@ static void buffer_queue(struct vb2_buffer *vb)
 	struct pwc_frame_buf *buf = container_of(vb, struct pwc_frame_buf, vb);
 	unsigned long flags = 0;
 
-	spin_lock_irqsave(&pdev->queued_bufs_lock, flags);
 	/* Check the device has not disconnected between prep and queuing */
-	if (pdev->udev)
-		list_add_tail(&buf->list, &pdev->queued_bufs);
-	else
+	if (!pdev->udev) {
 		vb2_buffer_done(&buf->vb, VB2_BUF_STATE_ERROR);
+		return;
+	}
+
+	spin_lock_irqsave(&pdev->queued_bufs_lock, flags);
+	list_add_tail(&buf->list, &pdev->queued_bufs);
 	spin_unlock_irqrestore(&pdev->queued_bufs_lock, flags);
 }
 
@@ -729,11 +779,8 @@ static int start_streaming(struct vb2_queue *vq, unsigned int count)
 	struct pwc_device *pdev = vb2_get_drv_priv(vq);
 	int r;
 
-	mutex_lock(&pdev->udevlock);
-	if (!pdev->udev) {
-		r = -ENODEV;
-		goto leave;
-	}
+	if (!pdev->udev)
+		return -ENODEV;
 
 	/* Turn on camera and set LEDS on */
 	pwc_camera_power(pdev, 1);
@@ -747,8 +794,7 @@ static int start_streaming(struct vb2_queue *vq, unsigned int count)
 		/* And cleanup any queued bufs!! */
 		pwc_cleanup_queued_bufs(pdev);
 	}
-leave:
-	mutex_unlock(&pdev->udevlock);
+
 	return r;
 }
 
@@ -756,19 +802,29 @@ static int stop_streaming(struct vb2_queue *vq)
 {
 	struct pwc_device *pdev = vb2_get_drv_priv(vq);
 
-	mutex_lock(&pdev->udevlock);
 	if (pdev->udev) {
 		pwc_set_leds(pdev, 0, 0);
 		pwc_camera_power(pdev, 0);
 		pwc_isoc_cleanup(pdev);
 	}
-	mutex_unlock(&pdev->udevlock);
 
 	pwc_cleanup_queued_bufs(pdev);
 
 	return 0;
 }
 
+static void wait_prepare(struct vb2_queue *vq)
+{
+	struct pwc_device *pdev = vb2_get_drv_priv(vq);
+	mutex_unlock(&pdev->vb_queue_lock);
+}
+
+static void wait_finish(struct vb2_queue *vq)
+{
+	struct pwc_device *pdev = vb2_get_drv_priv(vq);
+	mutex_lock(&pdev->vb_queue_lock);
+}
+
 static struct vb2_ops pwc_vb_queue_ops = {
 	.queue_setup		= queue_setup,
 	.buf_init		= buffer_init,
@@ -778,6 +834,8 @@ static struct vb2_ops pwc_vb_queue_ops = {
 	.buf_queue		= buffer_queue,
 	.start_streaming	= start_streaming,
 	.stop_streaming		= stop_streaming,
+	.wait_prepare		= wait_prepare,
+	.wait_finish		= wait_finish,
 };
 
 /***************************************************************************/
@@ -1057,8 +1115,8 @@ static int usb_pwc_probe(struct usb_interface *intf, const struct usb_device_id
 	pdev->features = features;
 	pwc_construct(pdev); /* set min/max sizes correct */
 
-	mutex_init(&pdev->capt_file_lock);
-	mutex_init(&pdev->udevlock);
+	mutex_init(&pdev->v4l2_lock);
+	mutex_init(&pdev->vb_queue_lock);
 	spin_lock_init(&pdev->queued_bufs_lock);
 	INIT_LIST_HEAD(&pdev->queued_bufs);
 
@@ -1130,6 +1188,16 @@ static int usb_pwc_probe(struct usb_interface *intf, const struct usb_device_id
 
 	pdev->v4l2_dev.ctrl_handler = &pdev->ctrl_handler;
 	pdev->vdev.v4l2_dev = &pdev->v4l2_dev;
+	pdev->vdev.lock = &pdev->v4l2_lock;
+
+	/*
+	 * Don't take v4l2_lock for these ioctls. This improves latency if
+	 * v4l2_lock is taken for a long time, e.g. when changing a control
+	 * value, and a new frame is ready to be dequeued.
+	 */
+	v4l2_disable_ioctl_locking(&pdev->vdev, VIDIOC_DQBUF);
+	v4l2_disable_ioctl_locking(&pdev->vdev, VIDIOC_QBUF);
+	v4l2_disable_ioctl_locking(&pdev->vdev, VIDIOC_QUERYBUF);
 
 	rc = video_register_device(&pdev->vdev, VFL_TYPE_GRABBER, -1);
 	if (rc < 0) {
@@ -1185,16 +1253,20 @@ static void usb_pwc_disconnect(struct usb_interface *intf)
 	struct v4l2_device *v = usb_get_intfdata(intf);
 	struct pwc_device *pdev = container_of(v, struct pwc_device, v4l2_dev);
 
-	mutex_lock(&pdev->udevlock);
+	mutex_lock(&pdev->v4l2_lock);
+
+	mutex_lock(&pdev->vb_queue_lock);
 	/* No need to keep the urbs around after disconnection */
-	pwc_isoc_cleanup(pdev);
+	if (pdev->vb_queue.streaming)
+		pwc_isoc_cleanup(pdev);
 	pdev->udev = NULL;
-	mutex_unlock(&pdev->udevlock);
-
 	pwc_cleanup_queued_bufs(pdev);
+	mutex_unlock(&pdev->vb_queue_lock);
 
+	v4l2_device_disconnect(&pdev->v4l2_dev);
 	video_unregister_device(&pdev->vdev);
-	v4l2_device_unregister(&pdev->v4l2_dev);
+
+	mutex_unlock(&pdev->v4l2_lock);
 
 #ifdef CONFIG_USB_PWC_INPUT_EVDEV
 	if (pdev->button_dev)
@@ -1229,15 +1301,4 @@ MODULE_LICENSE("GPL");
 MODULE_ALIAS("pwcx");
 MODULE_VERSION( PWC_VERSION );
 
-static int __init usb_pwc_init(void)
-{
-	return usb_register(&pwc_driver);
-}
-
-static void __exit usb_pwc_exit(void)
-{
-	usb_deregister(&pwc_driver);
-}
-
-module_init(usb_pwc_init);
-module_exit(usb_pwc_exit);
+module_usb_driver(pwc_driver);
diff --git a/drivers/media/video/pwc/pwc-v4l.c b/drivers/media/video/pwc/pwc-v4l.c
index 2834e3e65b3..c691e29cc36 100644
--- a/drivers/media/video/pwc/pwc-v4l.c
+++ b/drivers/media/video/pwc/pwc-v4l.c
@@ -464,26 +464,24 @@ static int pwc_s_fmt_vid_cap(struct file *file, void *fh, struct v4l2_format *f)
 	struct pwc_device *pdev = video_drvdata(file);
 	int ret, pixelformat, compression = 0;
 
-	if (pwc_test_n_set_capt_file(pdev, file))
-		return -EBUSY;
-
 	ret = pwc_vidioc_try_fmt(pdev, f);
 	if (ret < 0)
 		return ret;
 
-	pixelformat = f->fmt.pix.pixelformat;
+	if (mutex_lock_interruptible(&pdev->vb_queue_lock))
+		return -ERESTARTSYS;
 
-	mutex_lock(&pdev->udevlock);
-	if (!pdev->udev) {
-		ret = -ENODEV;
+	ret = pwc_test_n_set_capt_file(pdev, file);
+	if (ret)
 		goto leave;
-	}
 
-	if (pdev->iso_init) {
+	if (pdev->vb_queue.streaming) {
 		ret = -EBUSY;
 		goto leave;
 	}
 
+	pixelformat = f->fmt.pix.pixelformat;
+
 	PWC_DEBUG_IOCTL("Trying to set format to: width=%d height=%d fps=%d "
 			"format=%c%c%c%c\n",
 			f->fmt.pix.width, f->fmt.pix.height, pdev->vframes,
@@ -499,7 +497,7 @@ static int pwc_s_fmt_vid_cap(struct file *file, void *fh, struct v4l2_format *f)
 
 	pwc_vidioc_fill_fmt(f, pdev->width, pdev->height, pdev->pixfmt);
 leave:
-	mutex_unlock(&pdev->udevlock);
+	mutex_unlock(&pdev->vb_queue_lock);
 	return ret;
 }
 
@@ -507,9 +505,6 @@ static int pwc_querycap(struct file *file, void *fh, struct v4l2_capability *cap
 {
 	struct pwc_device *pdev = video_drvdata(file);
 
-	if (!pdev->udev)
-		return -ENODEV;
-
 	strcpy(cap->driver, PWC_NAME);
 	strlcpy(cap->card, pdev->vdev.name, sizeof(cap->card));
 	usb_make_path(pdev->udev, cap->bus_info, sizeof(cap->bus_info));
@@ -540,15 +535,12 @@ static int pwc_s_input(struct file *file, void *fh, unsigned int i)
 	return i ? -EINVAL : 0;
 }
 
-static int pwc_g_volatile_ctrl_unlocked(struct v4l2_ctrl *ctrl)
+static int pwc_g_volatile_ctrl(struct v4l2_ctrl *ctrl)
 {
 	struct pwc_device *pdev =
 		container_of(ctrl->handler, struct pwc_device, ctrl_handler);
 	int ret = 0;
 
-	if (!pdev->udev)
-		return -ENODEV;
-
 	switch (ctrl->id) {
 	case V4L2_CID_AUTO_WHITE_BALANCE:
 		if (pdev->color_bal_valid &&
@@ -615,18 +607,6 @@ static int pwc_g_volatile_ctrl_unlocked(struct v4l2_ctrl *ctrl)
 	return ret;
 }
 
-static int pwc_g_volatile_ctrl(struct v4l2_ctrl *ctrl)
-{
-	struct pwc_device *pdev =
-		container_of(ctrl->handler, struct pwc_device, ctrl_handler);
-	int ret;
-
-	mutex_lock(&pdev->udevlock);
-	ret = pwc_g_volatile_ctrl_unlocked(ctrl);
-	mutex_unlock(&pdev->udevlock);
-	return ret;
-}
-
 static int pwc_set_awb(struct pwc_device *pdev)
 {
 	int ret;
@@ -648,7 +628,7 @@ static int pwc_set_awb(struct pwc_device *pdev)
 		if (pdev->auto_white_balance->val == awb_indoor ||
 		    pdev->auto_white_balance->val == awb_outdoor ||
 		    pdev->auto_white_balance->val == awb_fl)
-			pwc_g_volatile_ctrl_unlocked(pdev->auto_white_balance);
+			pwc_g_volatile_ctrl(pdev->auto_white_balance);
 	}
 	if (pdev->auto_white_balance->val != awb_manual)
 		return 0;
@@ -812,13 +792,6 @@ static int pwc_s_ctrl(struct v4l2_ctrl *ctrl)
 		container_of(ctrl->handler, struct pwc_device, ctrl_handler);
 	int ret = 0;
 
-	mutex_lock(&pdev->udevlock);
-
-	if (!pdev->udev) {
-		ret = -ENODEV;
-		goto leave;
-	}
-
 	switch (ctrl->id) {
 	case V4L2_CID_BRIGHTNESS:
 		ret = pwc_set_u8_ctrl(pdev, SET_LUM_CTL,
@@ -915,8 +888,6 @@ static int pwc_s_ctrl(struct v4l2_ctrl *ctrl)
 	if (ret)
 		PWC_ERROR("s_ctrl %s error %d\n", ctrl->name, ret);
 
-leave:
-	mutex_unlock(&pdev->udevlock);
 	return ret;
 }
 
@@ -949,11 +920,9 @@ static int pwc_g_fmt_vid_cap(struct file *file, void *fh, struct v4l2_format *f)
 	if (f->type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
 		return -EINVAL;
 
-	mutex_lock(&pdev->udevlock); /* To avoid race with s_fmt */
 	PWC_DEBUG_IOCTL("ioctl(VIDIOC_G_FMT) return size %dx%d\n",
 			pdev->width, pdev->height);
 	pwc_vidioc_fill_fmt(f, pdev->width, pdev->height, pdev->pixfmt);
-	mutex_unlock(&pdev->udevlock);
 	return 0;
 }
 
@@ -968,70 +937,98 @@ static int pwc_reqbufs(struct file *file, void *fh,
 		       struct v4l2_requestbuffers *rb)
 {
 	struct pwc_device *pdev = video_drvdata(file);
+	int ret;
 
-	if (pwc_test_n_set_capt_file(pdev, file))
-		return -EBUSY;
+	if (mutex_lock_interruptible(&pdev->vb_queue_lock))
+		return -ERESTARTSYS;
 
-	return vb2_reqbufs(&pdev->vb_queue, rb);
+	ret = pwc_test_n_set_capt_file(pdev, file);
+	if (ret == 0)
+		ret = vb2_reqbufs(&pdev->vb_queue, rb);
+
+	mutex_unlock(&pdev->vb_queue_lock);
+	return ret;
 }
 
 static int pwc_querybuf(struct file *file, void *fh, struct v4l2_buffer *buf)
 {
 	struct pwc_device *pdev = video_drvdata(file);
+	int ret;
 
-	return vb2_querybuf(&pdev->vb_queue, buf);
+	if (mutex_lock_interruptible(&pdev->vb_queue_lock))
+		return -ERESTARTSYS;
+
+	ret = pwc_test_n_set_capt_file(pdev, file);
+	if (ret == 0)
+		ret = vb2_querybuf(&pdev->vb_queue, buf);
+
+	mutex_unlock(&pdev->vb_queue_lock);
+	return ret;
 }
 
 static int pwc_qbuf(struct file *file, void *fh, struct v4l2_buffer *buf)
 {
 	struct pwc_device *pdev = video_drvdata(file);
+	int ret;
 
-	if (!pdev->udev)
-		return -ENODEV;
+	if (mutex_lock_interruptible(&pdev->vb_queue_lock))
+		return -ERESTARTSYS;
 
-	if (pdev->capt_file != file)
-		return -EBUSY;
+	ret = pwc_test_n_set_capt_file(pdev, file);
+	if (ret == 0)
+		ret = vb2_qbuf(&pdev->vb_queue, buf);
 
-	return vb2_qbuf(&pdev->vb_queue, buf);
+	mutex_unlock(&pdev->vb_queue_lock);
+	return ret;
 }
 
 static int pwc_dqbuf(struct file *file, void *fh, struct v4l2_buffer *buf)
 {
 	struct pwc_device *pdev = video_drvdata(file);
+	int ret;
 
-	if (!pdev->udev)
-		return -ENODEV;
+	if (mutex_lock_interruptible(&pdev->vb_queue_lock))
+		return -ERESTARTSYS;
 
-	if (pdev->capt_file != file)
-		return -EBUSY;
+	ret = pwc_test_n_set_capt_file(pdev, file);
+	if (ret == 0)
+		ret = vb2_dqbuf(&pdev->vb_queue, buf,
+				file->f_flags & O_NONBLOCK);
 
-	return vb2_dqbuf(&pdev->vb_queue, buf, file->f_flags & O_NONBLOCK);
+	mutex_unlock(&pdev->vb_queue_lock);
+	return ret;
 }
 
 static int pwc_streamon(struct file *file, void *fh, enum v4l2_buf_type i)
 {
 	struct pwc_device *pdev = video_drvdata(file);
+	int ret;
 
-	if (!pdev->udev)
-		return -ENODEV;
+	if (mutex_lock_interruptible(&pdev->vb_queue_lock))
+		return -ERESTARTSYS;
 
-	if (pdev->capt_file != file)
-		return -EBUSY;
+	ret = pwc_test_n_set_capt_file(pdev, file);
+	if (ret == 0)
+		ret = vb2_streamon(&pdev->vb_queue, i);
 
-	return vb2_streamon(&pdev->vb_queue, i);
+	mutex_unlock(&pdev->vb_queue_lock);
+	return ret;
 }
 
 static int pwc_streamoff(struct file *file, void *fh, enum v4l2_buf_type i)
 {
 	struct pwc_device *pdev = video_drvdata(file);
+	int ret;
 
-	if (!pdev->udev)
-		return -ENODEV;
+	if (mutex_lock_interruptible(&pdev->vb_queue_lock))
+		return -ERESTARTSYS;
 
-	if (pdev->capt_file != file)
-		return -EBUSY;
+	ret = pwc_test_n_set_capt_file(pdev, file);
+	if (ret == 0)
+		ret = vb2_streamoff(&pdev->vb_queue, i);
 
-	return vb2_streamoff(&pdev->vb_queue, i);
+	mutex_unlock(&pdev->vb_queue_lock);
+	return ret;
 }
 
 static int pwc_enum_framesizes(struct file *file, void *fh,
@@ -1119,19 +1116,17 @@ static int pwc_s_parm(struct file *file, void *fh,
 	    parm->parm.capture.timeperframe.numerator == 0)
 		return -EINVAL;
 
-	if (pwc_test_n_set_capt_file(pdev, file))
-		return -EBUSY;
-
 	fps = parm->parm.capture.timeperframe.denominator /
 	      parm->parm.capture.timeperframe.numerator;
 
-	mutex_lock(&pdev->udevlock);
-	if (!pdev->udev) {
-		ret = -ENODEV;
+	if (mutex_lock_interruptible(&pdev->vb_queue_lock))
+		return -ERESTARTSYS;
+
+	ret = pwc_test_n_set_capt_file(pdev, file);
+	if (ret)
 		goto leave;
-	}
 
-	if (pdev->iso_init) {
+	if (pdev->vb_queue.streaming) {
 		ret = -EBUSY;
 		goto leave;
 	}
@@ -1142,7 +1137,7 @@ static int pwc_s_parm(struct file *file, void *fh,
 	pwc_g_parm(file, fh, parm);
 
 leave:
-	mutex_unlock(&pdev->udevlock);
+	mutex_unlock(&pdev->vb_queue_lock);
 	return ret;
 }
 
@@ -1166,4 +1161,6 @@ const struct v4l2_ioctl_ops pwc_ioctl_ops = {
 	.vidioc_enum_frameintervals	    = pwc_enum_frameintervals,
 	.vidioc_g_parm			    = pwc_g_parm,
 	.vidioc_s_parm			    = pwc_s_parm,
+	.vidioc_subscribe_event		    = v4l2_ctrl_subscribe_event,
+	.vidioc_unsubscribe_event	    = v4l2_event_unsubscribe,
 };
diff --git a/drivers/media/video/pwc/pwc.h b/drivers/media/video/pwc/pwc.h
index e4d4d711dd1..d6b5b216b9d 100644
--- a/drivers/media/video/pwc/pwc.h
+++ b/drivers/media/video/pwc/pwc.h
@@ -221,9 +221,17 @@ struct pwc_device
 	struct video_device vdev;
 	struct v4l2_device v4l2_dev;
 
-	/* Pointer to our usb_device, may be NULL after unplug */
-	struct usb_device *udev;
-	struct mutex udevlock;
+	/* videobuf2 queue and queued buffers list */
+	struct vb2_queue vb_queue;
+	struct list_head queued_bufs;
+	spinlock_t queued_bufs_lock; /* Protects queued_bufs */
+
+	/* Note if taking both locks v4l2_lock must always be locked first! */
+	struct mutex v4l2_lock;      /* Protects everything else */
+	struct mutex vb_queue_lock;  /* Protects vb_queue and capt_file */
+
+	/* Pointer to our usb_device, will be NULL after unplug */
+	struct usb_device *udev; /* Both mutexes most be hold when setting! */
 
 	/* type of cam (645, 646, 675, 680, 690, 720, 730, 740, 750) */
 	int type;
@@ -232,7 +240,6 @@ struct pwc_device
 
 	/*** Video data ***/
 	struct file *capt_file;	/* file doing video capture */
-	struct mutex capt_file_lock;
 	int vendpoint;		/* video isoc endpoint */
 	int vcinterface;	/* video control interface */
 	int valternate;		/* alternate interface needed */
@@ -251,12 +258,6 @@ struct pwc_device
 	unsigned char *ctrl_buf;
 
 	struct urb *urbs[MAX_ISO_BUFS];
-	char iso_init;
-
-	/* videobuf2 queue and queued buffers list */
-	struct vb2_queue vb_queue;
-	struct list_head queued_bufs;
-	spinlock_t queued_bufs_lock;
 
 	/*
 	 * Frame currently being filled, this only gets touched by the
diff --git a/drivers/media/video/pxa_camera.c b/drivers/media/video/pxa_camera.c
index 5a413f4427e..9c21e01f2c2 100644
--- a/drivers/media/video/pxa_camera.c
+++ b/drivers/media/video/pxa_camera.c
@@ -241,15 +241,10 @@ static int pxa_videobuf_setup(struct videobuf_queue *vq, unsigned int *count,
 			      unsigned int *size)
 {
 	struct soc_camera_device *icd = vq->priv_data;
-	int bytes_per_line = soc_mbus_bytes_per_line(icd->user_width,
-						icd->current_fmt->host_fmt);
-
-	if (bytes_per_line < 0)
-		return bytes_per_line;
 
 	dev_dbg(icd->parent, "count=%d, size=%d\n", *count, *size);
 
-	*size = bytes_per_line * icd->user_height;
+	*size = icd->sizeimage;
 
 	if (0 == *count)
 		*count = 32;
@@ -435,11 +430,6 @@ static int pxa_videobuf_prepare(struct videobuf_queue *vq,
 	struct pxa_buffer *buf = container_of(vb, struct pxa_buffer, vb);
 	int ret;
 	int size_y, size_u = 0, size_v = 0;
-	int bytes_per_line = soc_mbus_bytes_per_line(icd->user_width,
-						icd->current_fmt->host_fmt);
-
-	if (bytes_per_line < 0)
-		return bytes_per_line;
 
 	dev_dbg(dev, "%s (vb=0x%p) 0x%08lx %d\n", __func__,
 		vb, vb->baddr, vb->bsize);
@@ -474,7 +464,7 @@ static int pxa_videobuf_prepare(struct videobuf_queue *vq,
 		vb->state	= VIDEOBUF_NEEDS_INIT;
 	}
 
-	vb->size = bytes_per_line * vb->height;
+	vb->size = icd->sizeimage;
 	if (0 != vb->baddr && vb->bsize < vb->size) {
 		ret = -EINVAL;
 		goto out;
@@ -1244,6 +1234,7 @@ static const struct soc_mbus_pixelfmt pxa_camera_formats[] = {
 		.bits_per_sample	= 8,
 		.packing		= SOC_MBUS_PACKING_2X8_PADHI,
 		.order			= SOC_MBUS_ORDER_LE,
+		.layout			= SOC_MBUS_LAYOUT_PLANAR_2Y_U_V,
 	},
 };
 
diff --git a/drivers/media/video/s2255drv.c b/drivers/media/video/s2255drv.c
index 4894cbb1c54..01c2179f052 100644
--- a/drivers/media/video/s2255drv.c
+++ b/drivers/media/video/s2255drv.c
@@ -634,13 +634,11 @@ static void s2255_fillbuff(struct s2255_channel *channel,
 	const char *tmpbuf;
 	char *vbuf = videobuf_to_vmalloc(&buf->vb);
 	unsigned long last_frame;
-	struct s2255_framei *frm;
 
 	if (!vbuf)
 		return;
 	last_frame = channel->last_frame;
 	if (last_frame != -1) {
-		frm = &channel->buffer.frame[last_frame];
 		tmpbuf =
 		    (const char *)channel->buffer.frame[last_frame].lpvbits;
 		switch (buf->fmt->fourcc) {
@@ -987,7 +985,6 @@ static int vidioc_s_fmt_vid_cap(struct file *file, void *priv,
 	struct videobuf_queue *q = &fh->vb_vidq;
 	struct s2255_mode mode;
 	int ret;
-	int norm;
 
 	ret = vidioc_try_fmt_vid_cap(file, fh, f);
 
@@ -1018,7 +1015,6 @@ 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;
-	norm = norm_minw(&channel->vdev);
 	if (channel->width > norm_minw(&channel->vdev)) {
 		if (channel->height > norm_minh(&channel->vdev)) {
 			if (channel->cap_parm.capturemode &
@@ -1826,8 +1822,7 @@ static void s2255_destroy(struct s2255_dev *dev)
 		usb_free_urb(dev->fw_data->fw_urb);
 		dev->fw_data->fw_urb = NULL;
 	}
-	if (dev->fw_data->fw)
-		release_firmware(dev->fw_data->fw);
+	release_firmware(dev->fw_data->fw);
 	kfree(dev->fw_data->pfw_data);
 	kfree(dev->fw_data);
 	/* reset the DSP so firmware can be reloaded next time */
@@ -1949,6 +1944,10 @@ static int s2255_probe_v4l(struct s2255_dev *dev)
 		/* register 4 video devices */
 		channel->vdev = template;
 		channel->vdev.lock = &dev->lock;
+		/* Locking in file operations other than ioctl should be done
+		   by the driver, not the V4L2 core.
+		   This driver needs auditing so that this flag can be removed. */
+		set_bit(V4L2_FL_LOCK_ALL_FOPS, &channel->vdev.flags);
 		channel->vdev.v4l2_dev = &dev->v4l2_dev;
 		video_set_drvdata(&channel->vdev, channel);
 		if (video_nr == -1)
diff --git a/drivers/media/video/s5p-fimc/Kconfig b/drivers/media/video/s5p-fimc/Kconfig
new file mode 100644
index 00000000000..a564f7eeb06
--- /dev/null
+++ b/drivers/media/video/s5p-fimc/Kconfig
@@ -0,0 +1,48 @@
+
+config VIDEO_SAMSUNG_S5P_FIMC
+	bool "Samsung S5P/EXYNOS SoC camera interface driver (experimental)"
+	depends on VIDEO_V4L2 && VIDEO_V4L2_SUBDEV_API && PLAT_S5P && PM_RUNTIME
+	depends on EXPERIMENTAL
+	help
+	  Say Y here to enable camera host interface devices for
+	  Samsung S5P and EXYNOS SoC series.
+
+if VIDEO_SAMSUNG_S5P_FIMC
+
+config VIDEO_S5P_FIMC
+	tristate "S5P/EXYNOS4 FIMC/CAMIF camera interface driver"
+	depends on I2C
+	select VIDEOBUF2_DMA_CONTIG
+	select V4L2_MEM2MEM_DEV
+	help
+	  This is a V4L2 driver for Samsung S5P and EXYNOS4 SoC camera host
+	  interface and video postprocessor (FIMC and FIMC-LITE) devices.
+
+	  To compile this driver as a module, choose M here: the
+	  module will be called s5p-fimc.
+
+config VIDEO_S5P_MIPI_CSIS
+	tristate "S5P/EXYNOS MIPI-CSI2 receiver (MIPI-CSIS) driver"
+	depends on REGULATOR
+	help
+	  This is a V4L2 driver for Samsung S5P and EXYNOS4 SoC MIPI-CSI2
+	  receiver (MIPI-CSIS) devices.
+
+	  To compile this driver as a module, choose M here: the
+	  module will be called s5p-csis.
+
+if ARCH_EXYNOS
+
+config VIDEO_EXYNOS_FIMC_LITE
+	tristate "EXYNOS FIMC-LITE camera interface driver"
+	depends on I2C
+	select VIDEOBUF2_DMA_CONTIG
+	help
+	  This is a V4L2 driver for Samsung EXYNOS4/5 SoC FIMC-LITE camera
+	  host interface.
+
+	  To compile this driver as a module, choose M here: the
+	  module will be called exynos-fimc-lite.
+endif
+
+endif # VIDEO_SAMSUNG_S5P_FIMC
diff --git a/drivers/media/video/s5p-fimc/Makefile b/drivers/media/video/s5p-fimc/Makefile
index 33dec7f890e..46485143e1c 100644
--- a/drivers/media/video/s5p-fimc/Makefile
+++ b/drivers/media/video/s5p-fimc/Makefile
@@ -1,5 +1,7 @@
-s5p-fimc-objs := fimc-core.o fimc-reg.o fimc-capture.o fimc-mdevice.o
+s5p-fimc-objs := fimc-core.o fimc-reg.o fimc-m2m.o fimc-capture.o fimc-mdevice.o
+exynos-fimc-lite-objs += fimc-lite-reg.o fimc-lite.o
 s5p-csis-objs := mipi-csis.o
 
 obj-$(CONFIG_VIDEO_S5P_MIPI_CSIS)	+= s5p-csis.o
-obj-$(CONFIG_VIDEO_SAMSUNG_S5P_FIMC)	+= s5p-fimc.o
+obj-$(CONFIG_VIDEO_EXYNOS_FIMC_LITE)	+= exynos-fimc-lite.o
+obj-$(CONFIG_VIDEO_S5P_FIMC)		+= s5p-fimc.o
diff --git a/drivers/media/video/s5p-fimc/fimc-capture.c b/drivers/media/video/s5p-fimc/fimc-capture.c
index b06efd20832..35457459190 100644
--- a/drivers/media/video/s5p-fimc/fimc-capture.c
+++ b/drivers/media/video/s5p-fimc/fimc-capture.c
@@ -1,8 +1,8 @@
 /*
  * Samsung S5P/EXYNOS4 SoC series camera interface (camera capture) driver
  *
- * Copyright (C) 2010 - 2011 Samsung Electronics Co., Ltd.
- * Author: Sylwester Nawrocki, <s.nawrocki@samsung.com>
+ * Copyright (C) 2010 - 2012 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
@@ -29,20 +29,22 @@
 
 #include "fimc-mdevice.h"
 #include "fimc-core.h"
+#include "fimc-reg.h"
 
-static int fimc_init_capture(struct fimc_dev *fimc)
+static int fimc_capture_hw_init(struct fimc_dev *fimc)
 {
 	struct fimc_ctx *ctx = fimc->vid_cap.ctx;
+	struct fimc_pipeline *p = &fimc->pipeline;
 	struct fimc_sensor_info *sensor;
 	unsigned long flags;
 	int ret = 0;
 
-	if (fimc->pipeline.sensor == NULL || ctx == NULL)
+	if (p->subdevs[IDX_SENSOR] == NULL || ctx == NULL)
 		return -ENXIO;
 	if (ctx->s_frame.fmt == NULL)
 		return -EINVAL;
 
-	sensor = v4l2_get_subdev_hostdata(fimc->pipeline.sensor);
+	sensor = v4l2_get_subdev_hostdata(p->subdevs[IDX_SENSOR]);
 
 	spin_lock_irqsave(&fimc->slock, flags);
 	fimc_prepare_dma_offset(ctx, &ctx->d_frame);
@@ -60,7 +62,7 @@ static int fimc_init_capture(struct fimc_dev *fimc)
 		fimc_hw_set_mainscaler(ctx);
 		fimc_hw_set_target_format(ctx);
 		fimc_hw_set_rotation(ctx);
-		fimc_hw_set_effect(ctx, false);
+		fimc_hw_set_effect(ctx);
 		fimc_hw_set_output_path(ctx);
 		fimc_hw_set_out_dma(ctx);
 		if (fimc->variant->has_alpha)
@@ -71,6 +73,14 @@ static int fimc_init_capture(struct fimc_dev *fimc)
 	return ret;
 }
 
+/*
+ * Reinitialize the driver so it is ready to start the streaming again.
+ * Set fimc->state to indicate stream off and the hardware shut down state.
+ * If not suspending (@suspend is false), return any buffers to videobuf2.
+ * Otherwise put any owned buffers onto the pending buffers queue, so they
+ * can be re-spun when the device is being resumed. Also perform FIMC
+ * software reset and disable streaming on the whole pipeline if required.
+ */
 static int fimc_capture_state_cleanup(struct fimc_dev *fimc, bool suspend)
 {
 	struct fimc_vid_cap *cap = &fimc->vid_cap;
@@ -83,7 +93,9 @@ static int fimc_capture_state_cleanup(struct fimc_dev *fimc, bool suspend)
 
 	fimc->state &= ~(1 << ST_CAPT_RUN | 1 << ST_CAPT_SHUT |
 			 1 << ST_CAPT_STREAM | 1 << ST_CAPT_ISP_STREAM);
-	if (!suspend)
+	if (suspend)
+		fimc->state |= (1 << ST_CAPT_SUSPENDED);
+	else
 		fimc->state &= ~(1 << ST_CAPT_PEND | 1 << ST_CAPT_SUSPENDED);
 
 	/* Release unused buffers */
@@ -99,7 +111,6 @@ static int fimc_capture_state_cleanup(struct fimc_dev *fimc, bool suspend)
 		else
 			vb2_buffer_done(&buf->vb, VB2_BUF_STATE_ERROR);
 	}
-	set_bit(ST_CAPT_SUSPENDED, &fimc->state);
 
 	fimc_hw_reset(fimc);
 	cap->buf_index = 0;
@@ -107,7 +118,7 @@ static int fimc_capture_state_cleanup(struct fimc_dev *fimc, bool suspend)
 	spin_unlock_irqrestore(&fimc->slock, flags);
 
 	if (streaming)
-		return fimc_pipeline_s_stream(fimc, 0);
+		return fimc_pipeline_s_stream(&fimc->pipeline, 0);
 	else
 		return 0;
 }
@@ -138,32 +149,96 @@ static int fimc_stop_capture(struct fimc_dev *fimc, bool suspend)
  * spinlock held. It updates the camera pixel crop, rotation and
  * image flip in H/W.
  */
-int fimc_capture_config_update(struct fimc_ctx *ctx)
+static int fimc_capture_config_update(struct fimc_ctx *ctx)
 {
 	struct fimc_dev *fimc = ctx->fimc_dev;
 	int ret;
 
-	if (!test_bit(ST_CAPT_APPLY_CFG, &fimc->state))
-		return 0;
-
-	spin_lock(&ctx->slock);
 	fimc_hw_set_camera_offset(fimc, &ctx->s_frame);
+
 	ret = fimc_set_scaler_info(ctx);
-	if (ret == 0) {
-		fimc_hw_set_prescaler(ctx);
-		fimc_hw_set_mainscaler(ctx);
-		fimc_hw_set_target_format(ctx);
-		fimc_hw_set_rotation(ctx);
-		fimc_prepare_dma_offset(ctx, &ctx->d_frame);
-		fimc_hw_set_out_dma(ctx);
-		if (fimc->variant->has_alpha)
-			fimc_hw_set_rgb_alpha(ctx);
-		clear_bit(ST_CAPT_APPLY_CFG, &fimc->state);
-	}
-	spin_unlock(&ctx->slock);
+	if (ret)
+		return ret;
+
+	fimc_hw_set_prescaler(ctx);
+	fimc_hw_set_mainscaler(ctx);
+	fimc_hw_set_target_format(ctx);
+	fimc_hw_set_rotation(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)
+		fimc_hw_set_rgb_alpha(ctx);
+
+	clear_bit(ST_CAPT_APPLY_CFG, &fimc->state);
 	return ret;
 }
 
+void fimc_capture_irq_handler(struct fimc_dev *fimc, int deq_buf)
+{
+	struct fimc_vid_cap *cap = &fimc->vid_cap;
+	struct fimc_vid_buffer *v_buf;
+	struct timeval *tv;
+	struct timespec ts;
+
+	if (test_and_clear_bit(ST_CAPT_SHUT, &fimc->state)) {
+		wake_up(&fimc->irq_queue);
+		goto done;
+	}
+
+	if (!list_empty(&cap->active_buf_q) &&
+	    test_bit(ST_CAPT_RUN, &fimc->state) && deq_buf) {
+		ktime_get_real_ts(&ts);
+
+		v_buf = fimc_active_queue_pop(cap);
+
+		tv = &v_buf->vb.v4l2_buf.timestamp;
+		tv->tv_sec = ts.tv_sec;
+		tv->tv_usec = ts.tv_nsec / NSEC_PER_USEC;
+		v_buf->vb.v4l2_buf.sequence = cap->frame_count++;
+
+		vb2_buffer_done(&v_buf->vb, VB2_BUF_STATE_DONE);
+	}
+
+	if (!list_empty(&cap->pending_buf_q)) {
+
+		v_buf = fimc_pending_queue_pop(cap);
+		fimc_hw_set_output_addr(fimc, &v_buf->paddr, cap->buf_index);
+		v_buf->index = cap->buf_index;
+
+		/* Move the buffer to the capture active queue */
+		fimc_active_queue_add(cap, v_buf);
+
+		dbg("next frame: %d, done frame: %d",
+		    fimc_hw_get_frame_index(fimc), v_buf->index);
+
+		if (++cap->buf_index >= FIMC_MAX_OUT_BUFS)
+			cap->buf_index = 0;
+	}
+
+	if (cap->active_buf_cnt == 0) {
+		if (deq_buf)
+			clear_bit(ST_CAPT_RUN, &fimc->state);
+
+		if (++cap->buf_index >= FIMC_MAX_OUT_BUFS)
+			cap->buf_index = 0;
+	} else {
+		set_bit(ST_CAPT_RUN, &fimc->state);
+	}
+
+	if (test_bit(ST_CAPT_APPLY_CFG, &fimc->state))
+		fimc_capture_config_update(cap->ctx);
+done:
+	if (cap->active_buf_cnt == 1) {
+		fimc_deactivate_capture(fimc);
+		clear_bit(ST_CAPT_STREAM, &fimc->state);
+	}
+
+	dbg("frame: %d, active_buf_cnt: %d",
+	    fimc_hw_get_frame_index(fimc), cap->active_buf_cnt);
+}
+
+
 static int start_streaming(struct vb2_queue *q, unsigned int count)
 {
 	struct fimc_ctx *ctx = q->drv_priv;
@@ -174,9 +249,11 @@ static int start_streaming(struct vb2_queue *q, unsigned int count)
 
 	vid_cap->frame_count = 0;
 
-	ret = fimc_init_capture(fimc);
-	if (ret)
-		goto error;
+	ret = fimc_capture_hw_init(fimc);
+	if (ret) {
+		fimc_capture_state_cleanup(fimc, false);
+		return ret;
+	}
 
 	set_bit(ST_CAPT_PEND, &fimc->state);
 
@@ -187,13 +264,10 @@ 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_s_stream(fimc, 1);
+			fimc_pipeline_s_stream(&fimc->pipeline, 1);
 	}
 
 	return 0;
-error:
-	fimc_capture_state_cleanup(fimc, false);
-	return ret;
 }
 
 static int stop_streaming(struct vb2_queue *q)
@@ -214,7 +288,7 @@ int fimc_capture_suspend(struct fimc_dev *fimc)
 	int ret = fimc_stop_capture(fimc, suspend);
 	if (ret)
 		return ret;
-	return fimc_pipeline_shutdown(fimc);
+	return fimc_pipeline_shutdown(&fimc->pipeline);
 }
 
 static void buffer_queue(struct vb2_buffer *vb);
@@ -230,9 +304,9 @@ int fimc_capture_resume(struct fimc_dev *fimc)
 
 	INIT_LIST_HEAD(&fimc->vid_cap.active_buf_q);
 	vid_cap->buf_index = 0;
-	fimc_pipeline_initialize(fimc, &fimc->vid_cap.vfd->entity,
+	fimc_pipeline_initialize(&fimc->pipeline, &vid_cap->vfd->entity,
 				 false);
-	fimc_init_capture(fimc);
+	fimc_capture_hw_init(fimc);
 
 	clear_bit(ST_CAPT_SUSPENDED, &fimc->state);
 
@@ -246,28 +320,37 @@ int fimc_capture_resume(struct fimc_dev *fimc)
 
 }
 
-static unsigned int get_plane_size(struct fimc_frame *fr, unsigned int plane)
-{
-	if (!fr || plane >= fr->fmt->memplanes)
-		return 0;
-	return fr->f_width * fr->f_height * fr->fmt->depth[plane] / 8;
-}
-
-static int queue_setup(struct vb2_queue *vq,  const struct v4l2_format *pfmt,
+static int queue_setup(struct vb2_queue *vq, const struct v4l2_format *pfmt,
 		       unsigned int *num_buffers, unsigned int *num_planes,
 		       unsigned int sizes[], void *allocators[])
 {
+	const struct v4l2_pix_format_mplane *pixm = NULL;
 	struct fimc_ctx *ctx = vq->drv_priv;
-	struct fimc_fmt *fmt = ctx->d_frame.fmt;
+	struct fimc_frame *frame = &ctx->d_frame;
+	struct fimc_fmt *fmt = frame->fmt;
+	unsigned long wh;
 	int i;
 
-	if (!fmt)
+	if (pfmt) {
+		pixm = &pfmt->fmt.pix_mp;
+		fmt = fimc_find_format(&pixm->pixelformat, NULL,
+				       FMT_FLAGS_CAM | FMT_FLAGS_M2M, -1);
+		wh = pixm->width * pixm->height;
+	} else {
+		wh = frame->f_width * frame->f_height;
+	}
+
+	if (fmt == NULL)
 		return -EINVAL;
 
 	*num_planes = fmt->memplanes;
 
 	for (i = 0; i < fmt->memplanes; i++) {
-		sizes[i] = get_plane_size(&ctx->d_frame, i);
+		unsigned int size = (wh * fmt->depth[i]) / 8;
+		if (pixm)
+			sizes[i] = max(size, pixm->plane_fmt[i].sizeimage);
+		else
+			sizes[i] = size;
 		allocators[i] = ctx->fimc_dev->alloc_ctx;
 	}
 
@@ -338,7 +421,7 @@ static void buffer_queue(struct vb2_buffer *vb)
 		spin_unlock_irqrestore(&fimc->slock, flags);
 
 		if (!test_and_set_bit(ST_CAPT_ISP_STREAM, &fimc->state))
-			fimc_pipeline_s_stream(fimc, 1);
+			fimc_pipeline_s_stream(&fimc->pipeline, 1);
 		return;
 	}
 	spin_unlock_irqrestore(&fimc->slock, flags);
@@ -380,15 +463,15 @@ int fimc_capture_ctrls_create(struct fimc_dev *fimc)
 
 	if (WARN_ON(vid_cap->ctx == NULL))
 		return -ENXIO;
-	if (vid_cap->ctx->ctrls_rdy)
+	if (vid_cap->ctx->ctrls.ready)
 		return 0;
 
 	ret = fimc_ctrls_create(vid_cap->ctx);
-	if (ret || vid_cap->user_subdev_api)
+	if (ret || vid_cap->user_subdev_api || !vid_cap->ctx->ctrls.ready)
 		return ret;
 
-	return v4l2_ctrl_add_handler(&vid_cap->ctx->ctrl_handler,
-				    fimc->pipeline.sensor->ctrl_handler);
+	return v4l2_ctrl_add_handler(&vid_cap->ctx->ctrls.handler,
+		    fimc->pipeline.subdevs[IDX_SENSOR]->ctrl_handler);
 }
 
 static int fimc_capture_set_default_format(struct fimc_dev *fimc);
@@ -411,7 +494,7 @@ static int fimc_capture_open(struct file *file)
 	pm_runtime_get_sync(&fimc->pdev->dev);
 
 	if (++fimc->vid_cap.refcnt == 1) {
-		ret = fimc_pipeline_initialize(fimc,
+		ret = fimc_pipeline_initialize(&fimc->pipeline,
 			       &fimc->vid_cap.vfd->entity, true);
 		if (ret < 0) {
 			dev_err(&fimc->pdev->dev,
@@ -439,7 +522,7 @@ static int fimc_capture_close(struct file *file)
 	if (--fimc->vid_cap.refcnt == 0) {
 		clear_bit(ST_CAPT_BUSY, &fimc->state);
 		fimc_stop_capture(fimc, false);
-		fimc_pipeline_shutdown(fimc);
+		fimc_pipeline_shutdown(&fimc->pipeline);
 		clear_bit(ST_CAPT_SUSPENDED, &fimc->state);
 	}
 
@@ -486,7 +569,7 @@ static struct fimc_fmt *fimc_capture_try_format(struct fimc_ctx *ctx,
 {
 	bool rotation = ctx->rotation == 90 || ctx->rotation == 270;
 	struct fimc_dev *fimc = ctx->fimc_dev;
-	struct samsung_fimc_variant *var = fimc->variant;
+	struct fimc_variant *var = fimc->variant;
 	struct fimc_pix_limit *pl = var->pix_limit;
 	struct fimc_frame *dst = &ctx->d_frame;
 	u32 depth, min_w, max_w, min_h, align_h = 3;
@@ -528,8 +611,13 @@ static struct fimc_fmt *fimc_capture_try_format(struct fimc_ctx *ctx,
 	}
 	/* Apply the scaler and the output DMA constraints */
 	max_w = rotation ? pl->out_rot_en_w : pl->out_rot_dis_w;
-	min_w = ctx->state & FIMC_DST_CROP ? dst->width : var->min_out_pixsize;
-	min_h = ctx->state & FIMC_DST_CROP ? dst->height : var->min_out_pixsize;
+	if (ctx->state & FIMC_COMPOSE) {
+		min_w = dst->offs_h + dst->width;
+		min_h = dst->offs_v + dst->height;
+	} else {
+		min_w = var->min_out_pixsize;
+		min_h = var->min_out_pixsize;
+	}
 	if (var->min_vsize_align == 1 && !rotation)
 		align_h = fimc_fmt_is_rgb(ffmt->color) ? 0 : 1;
 
@@ -547,12 +635,13 @@ static struct fimc_fmt *fimc_capture_try_format(struct fimc_ctx *ctx,
 	return ffmt;
 }
 
-static void fimc_capture_try_crop(struct fimc_ctx *ctx, struct v4l2_rect *r,
-				  int pad)
+static void fimc_capture_try_selection(struct fimc_ctx *ctx,
+				       struct v4l2_rect *r,
+				       int target)
 {
 	bool rotate = ctx->rotation == 90 || ctx->rotation == 270;
 	struct fimc_dev *fimc = ctx->fimc_dev;
-	struct samsung_fimc_variant *var = fimc->variant;
+	struct fimc_variant *var = fimc->variant;
 	struct fimc_pix_limit *pl = var->pix_limit;
 	struct fimc_frame *sink = &ctx->s_frame;
 	u32 max_w, max_h, min_w = 0, min_h = 0, min_sz;
@@ -566,7 +655,7 @@ static void fimc_capture_try_crop(struct fimc_ctx *ctx, struct v4l2_rect *r,
 		r->left   = r->top = 0;
 		return;
 	}
-	if (pad == FIMC_SD_PAD_SOURCE) {
+	if (target == V4L2_SEL_TGT_COMPOSE_ACTIVE) {
 		if (ctx->rotation != 90 && ctx->rotation != 270)
 			align_h = 1;
 		max_sc_h = min(SCALER_MAX_HRATIO, 1 << (ffs(sink->width) - 3));
@@ -580,8 +669,7 @@ static void fimc_capture_try_crop(struct fimc_ctx *ctx, struct v4l2_rect *r,
 		max_sc_h = max_sc_v = 1;
 	}
 	/*
-	 * For the crop rectangle at source pad the following constraints
-	 * must be met:
+	 * For the compose rectangle the following constraints must be met:
 	 * - it must fit in the sink pad format rectangle (f_width/f_height);
 	 * - maximum downscaling ratio is 64;
 	 * - maximum crop size depends if the rotator is used or not;
@@ -593,7 +681,8 @@ static void fimc_capture_try_crop(struct fimc_ctx *ctx, struct v4l2_rect *r,
 		      rotate ? pl->out_rot_en_w : pl->out_rot_dis_w,
 		      rotate ? sink->f_height : sink->f_width);
 	max_h = min_t(u32, FIMC_CAMIF_MAX_HEIGHT, sink->f_height);
-	if (pad == FIMC_SD_PAD_SOURCE) {
+
+	if (target == V4L2_SEL_TGT_COMPOSE_ACTIVE) {
 		min_w = min_t(u32, max_w, sink->f_width / max_sc_h);
 		min_h = min_t(u32, max_h, sink->f_height / max_sc_v);
 		if (rotate) {
@@ -604,13 +693,13 @@ static void fimc_capture_try_crop(struct fimc_ctx *ctx, struct v4l2_rect *r,
 	v4l_bound_align_image(&r->width, min_w, max_w, ffs(min_sz) - 1,
 			      &r->height, min_h, max_h, align_h,
 			      align_sz);
-	/* Adjust left/top if cropping rectangle is out of bounds */
+	/* Adjust left/top if crop/compose rectangle is out of bounds */
 	r->left = clamp_t(u32, r->left, 0, sink->f_width - r->width);
 	r->top  = clamp_t(u32, r->top, 0, sink->f_height - r->height);
 	r->left = round_down(r->left, var->hor_offs_align);
 
-	dbg("pad%d: (%d,%d)/%dx%d, sink fmt: %dx%d",
-	    pad, r->left, r->top, r->width, r->height,
+	dbg("target %#x: (%d,%d)/%dx%d, sink fmt: %dx%d",
+	    target, r->left, r->top, r->width, r->height,
 	    sink->f_width, sink->f_height);
 }
 
@@ -660,8 +749,8 @@ static int fimc_pipeline_try_format(struct fimc_ctx *ctx,
 				    bool set)
 {
 	struct fimc_dev *fimc = ctx->fimc_dev;
-	struct v4l2_subdev *sd = fimc->pipeline.sensor;
-	struct v4l2_subdev *csis = fimc->pipeline.csis;
+	struct v4l2_subdev *sd = fimc->pipeline.subdevs[IDX_SENSOR];
+	struct v4l2_subdev *csis = fimc->pipeline.subdevs[IDX_CSIS];
 	struct v4l2_subdev_format sfmt;
 	struct v4l2_mbus_framefmt *mf = &sfmt.format;
 	struct fimc_fmt *ffmt = NULL;
@@ -842,7 +931,7 @@ static int fimc_capture_set_format(struct fimc_dev *fimc, struct v4l2_format *f)
 
 	set_frame_bounds(ff, pix->width, pix->height);
 	/* Reset the composition rectangle if not yet configured */
-	if (!(ctx->state & FIMC_DST_CROP))
+	if (!(ctx->state & FIMC_COMPOSE))
 		set_frame_crop(ff, 0, 0, pix->width, pix->height);
 
 	fimc_capture_mark_jpeg_xfer(ctx, fimc_fmt_is_jpeg(ff->fmt->color));
@@ -869,7 +958,7 @@ static int fimc_cap_enum_input(struct file *file, void *priv,
 			       struct v4l2_input *i)
 {
 	struct fimc_dev *fimc = video_drvdata(file);
-	struct v4l2_subdev *sd = fimc->pipeline.sensor;
+	struct v4l2_subdev *sd = fimc->pipeline.subdevs[IDX_SENSOR];
 
 	if (i->index != 0)
 		return -EINVAL;
@@ -918,7 +1007,7 @@ static int fimc_pipeline_validate(struct fimc_dev *fimc)
 		if (!(pad->flags & MEDIA_PAD_FL_SINK))
 			break;
 		/* Don't call FIMC subdev operation to avoid nested locking */
-		if (sd == fimc->vid_cap.subdev) {
+		if (sd == &fimc->vid_cap.subdev) {
 			struct fimc_frame *ff = &vid_cap->ctx->s_frame;
 			sink_fmt.format.width = ff->f_width;
 			sink_fmt.format.height = ff->f_height;
@@ -961,7 +1050,8 @@ static int fimc_cap_streamon(struct file *file, void *priv,
 	if (fimc_capture_active(fimc))
 		return -EBUSY;
 
-	media_entity_pipeline_start(&p->sensor->entity, p->pipe);
+	media_entity_pipeline_start(&p->subdevs[IDX_SENSOR]->entity,
+				    p->m_pipeline);
 
 	if (fimc->vid_cap.user_subdev_api) {
 		ret = fimc_pipeline_validate(fimc);
@@ -975,7 +1065,7 @@ 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.sensor;
+	struct v4l2_subdev *sd = fimc->pipeline.subdevs[IDX_SENSOR];
 	int ret;
 
 	ret = vb2_streamoff(&fimc->vid_cap.vbq, type);
@@ -1091,29 +1181,18 @@ static int fimc_cap_s_selection(struct file *file, void *fh,
 	struct v4l2_rect rect = s->r;
 	struct fimc_frame *f;
 	unsigned long flags;
-	unsigned int pad;
 
 	if (s->type != V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE)
 		return -EINVAL;
 
-	switch (s->target) {
-	case V4L2_SEL_TGT_COMPOSE_DEFAULT:
-	case V4L2_SEL_TGT_COMPOSE_BOUNDS:
-	case V4L2_SEL_TGT_COMPOSE_ACTIVE:
+	if (s->target == V4L2_SEL_TGT_COMPOSE_ACTIVE)
 		f = &ctx->d_frame;
-		pad = FIMC_SD_PAD_SOURCE;
-		break;
-	case V4L2_SEL_TGT_CROP_BOUNDS:
-	case V4L2_SEL_TGT_CROP_DEFAULT:
-	case V4L2_SEL_TGT_CROP_ACTIVE:
+	else if (s->target == V4L2_SEL_TGT_CROP_ACTIVE)
 		f = &ctx->s_frame;
-		pad = FIMC_SD_PAD_SINK;
-		break;
-	default:
+	else
 		return -EINVAL;
-	}
 
-	fimc_capture_try_crop(ctx, &rect, pad);
+	fimc_capture_try_selection(ctx, &rect, s->target);
 
 	if (s->flags & V4L2_SEL_FLAG_LE &&
 	    !enclosed_rectangle(&rect, &s->r))
@@ -1234,7 +1313,7 @@ void fimc_sensor_notify(struct v4l2_subdev *sd, unsigned int notification,
 					 struct fimc_vid_buffer, list);
 			vb2_set_plane_payload(&buf->vb, 0, *((u32 *)arg));
 		}
-		fimc_capture_irq_handler(fimc, true);
+		fimc_capture_irq_handler(fimc, 1);
 		fimc_deactivate_capture(fimc);
 		spin_unlock_irqrestore(&fimc->slock, irq_flags);
 	}
@@ -1325,77 +1404,122 @@ static int fimc_subdev_set_fmt(struct v4l2_subdev *sd,
 	ff->fmt = ffmt;
 
 	/* Reset the crop rectangle if required. */
-	if (!(fmt->pad == FIMC_SD_PAD_SOURCE && (ctx->state & FIMC_DST_CROP)))
+	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)
-		ctx->state &= ~FIMC_DST_CROP;
+		ctx->state &= ~FIMC_COMPOSE;
 	mutex_unlock(&fimc->lock);
 	return 0;
 }
 
-static int fimc_subdev_get_crop(struct v4l2_subdev *sd,
-				struct v4l2_subdev_fh *fh,
-				struct v4l2_subdev_crop *crop)
+static int fimc_subdev_get_selection(struct v4l2_subdev *sd,
+				     struct v4l2_subdev_fh *fh,
+				     struct v4l2_subdev_selection *sel)
 {
 	struct fimc_dev *fimc = v4l2_get_subdevdata(sd);
 	struct fimc_ctx *ctx = fimc->vid_cap.ctx;
-	struct v4l2_rect *r = &crop->rect;
-	struct fimc_frame *ff;
+	struct fimc_frame *f = &ctx->s_frame;
+	struct v4l2_rect *r = &sel->r;
+	struct v4l2_rect *try_sel;
 
-	if (crop->which == V4L2_SUBDEV_FORMAT_TRY) {
-		crop->rect = *v4l2_subdev_get_try_crop(fh, crop->pad);
+	if (sel->pad != FIMC_SD_PAD_SINK)
+		return -EINVAL;
+
+	mutex_lock(&fimc->lock);
+
+	switch (sel->target) {
+	case V4L2_SUBDEV_SEL_TGT_COMPOSE_BOUNDS:
+		f = &ctx->d_frame;
+	case V4L2_SUBDEV_SEL_TGT_CROP_BOUNDS:
+		r->width = f->o_width;
+		r->height = f->o_height;
+		r->left = 0;
+		r->top = 0;
+		mutex_unlock(&fimc->lock);
 		return 0;
+
+	case V4L2_SUBDEV_SEL_TGT_CROP_ACTUAL:
+		try_sel = v4l2_subdev_get_try_crop(fh, sel->pad);
+		break;
+	case V4L2_SUBDEV_SEL_TGT_COMPOSE_ACTUAL:
+		try_sel = v4l2_subdev_get_try_compose(fh, sel->pad);
+		f = &ctx->d_frame;
+		break;
+	default:
+		mutex_unlock(&fimc->lock);
+		return -EINVAL;
 	}
-	ff = crop->pad == FIMC_SD_PAD_SINK ?
-		&ctx->s_frame : &ctx->d_frame;
 
-	mutex_lock(&fimc->lock);
-	r->left	  = ff->offs_h;
-	r->top	  = ff->offs_v;
-	r->width  = ff->width;
-	r->height = ff->height;
-	mutex_unlock(&fimc->lock);
+	if (sel->which == V4L2_SUBDEV_FORMAT_TRY) {
+		sel->r = *try_sel;
+	} else {
+		r->left = f->offs_h;
+		r->top = f->offs_v;
+		r->width = f->width;
+		r->height = f->height;
+	}
 
-	dbg("ff:%p, pad%d: l:%d, t:%d, %dx%d, f_w: %d, f_h: %d",
-	    ff, crop->pad, r->left, r->top, r->width, r->height,
-	    ff->f_width, ff->f_height);
+	dbg("target %#x: l:%d, t:%d, %dx%d, f_w: %d, f_h: %d",
+	    sel->pad, r->left, r->top, r->width, r->height,
+	    f->f_width, f->f_height);
 
+	mutex_unlock(&fimc->lock);
 	return 0;
 }
 
-static int fimc_subdev_set_crop(struct v4l2_subdev *sd,
-				struct v4l2_subdev_fh *fh,
-				struct v4l2_subdev_crop *crop)
+static int fimc_subdev_set_selection(struct v4l2_subdev *sd,
+				     struct v4l2_subdev_fh *fh,
+				     struct v4l2_subdev_selection *sel)
 {
 	struct fimc_dev *fimc = v4l2_get_subdevdata(sd);
 	struct fimc_ctx *ctx = fimc->vid_cap.ctx;
-	struct v4l2_rect *r = &crop->rect;
-	struct fimc_frame *ff;
+	struct fimc_frame *f = &ctx->s_frame;
+	struct v4l2_rect *r = &sel->r;
+	struct v4l2_rect *try_sel;
 	unsigned long flags;
 
-	dbg("(%d,%d)/%dx%d", r->left, r->top, r->width, r->height);
-
-	ff = crop->pad == FIMC_SD_PAD_SOURCE ?
-		&ctx->d_frame : &ctx->s_frame;
+	if (sel->pad != FIMC_SD_PAD_SINK)
+		return -EINVAL;
 
 	mutex_lock(&fimc->lock);
-	fimc_capture_try_crop(ctx, r, crop->pad);
+	fimc_capture_try_selection(ctx, r, V4L2_SEL_TGT_CROP_ACTIVE);
 
-	if (crop->which == V4L2_SUBDEV_FORMAT_TRY) {
-		mutex_lock(&fimc->lock);
-		*v4l2_subdev_get_try_crop(fh, crop->pad) = *r;
+	switch (sel->target) {
+	case V4L2_SUBDEV_SEL_TGT_COMPOSE_BOUNDS:
+		f = &ctx->d_frame;
+	case V4L2_SUBDEV_SEL_TGT_CROP_BOUNDS:
+		r->width = f->o_width;
+		r->height = f->o_height;
+		r->left = 0;
+		r->top = 0;
+		mutex_unlock(&fimc->lock);
 		return 0;
+
+	case V4L2_SUBDEV_SEL_TGT_CROP_ACTUAL:
+		try_sel = v4l2_subdev_get_try_crop(fh, sel->pad);
+		break;
+	case V4L2_SUBDEV_SEL_TGT_COMPOSE_ACTUAL:
+		try_sel = v4l2_subdev_get_try_compose(fh, sel->pad);
+		f = &ctx->d_frame;
+		break;
+	default:
+		mutex_unlock(&fimc->lock);
+		return -EINVAL;
 	}
-	spin_lock_irqsave(&fimc->slock, flags);
-	set_frame_crop(ff, r->left, r->top, r->width, r->height);
-	if (crop->pad == FIMC_SD_PAD_SOURCE)
-		ctx->state |= FIMC_DST_CROP;
 
-	set_bit(ST_CAPT_APPLY_CFG, &fimc->state);
-	spin_unlock_irqrestore(&fimc->slock, flags);
+	if (sel->which == V4L2_SUBDEV_FORMAT_TRY) {
+		*try_sel = sel->r;
+	} else {
+		spin_lock_irqsave(&fimc->slock, flags);
+		set_frame_crop(f, r->left, r->top, r->width, r->height);
+		set_bit(ST_CAPT_APPLY_CFG, &fimc->state);
+		spin_unlock_irqrestore(&fimc->slock, flags);
+		if (sel->target == V4L2_SUBDEV_SEL_TGT_COMPOSE_ACTUAL)
+			ctx->state |= FIMC_COMPOSE;
+	}
 
-	dbg("pad%d: (%d,%d)/%dx%d", crop->pad, r->left, r->top,
+	dbg("target %#x: (%d,%d)/%dx%d", sel->target, r->left, r->top,
 	    r->width, r->height);
 
 	mutex_unlock(&fimc->lock);
@@ -1404,63 +1528,16 @@ static int fimc_subdev_set_crop(struct v4l2_subdev *sd,
 
 static struct v4l2_subdev_pad_ops fimc_subdev_pad_ops = {
 	.enum_mbus_code = fimc_subdev_enum_mbus_code,
+	.get_selection = fimc_subdev_get_selection,
+	.set_selection = fimc_subdev_set_selection,
 	.get_fmt = fimc_subdev_get_fmt,
 	.set_fmt = fimc_subdev_set_fmt,
-	.get_crop = fimc_subdev_get_crop,
-	.set_crop = fimc_subdev_set_crop,
 };
 
 static struct v4l2_subdev_ops fimc_subdev_ops = {
 	.pad = &fimc_subdev_pad_ops,
 };
 
-static int fimc_create_capture_subdev(struct fimc_dev *fimc,
-				      struct v4l2_device *v4l2_dev)
-{
-	struct v4l2_subdev *sd;
-	int ret;
-
-	sd = kzalloc(sizeof(*sd), GFP_KERNEL);
-	if (!sd)
-		return -ENOMEM;
-
-	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);
-
-	fimc->vid_cap.sd_pads[FIMC_SD_PAD_SINK].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);
-	if (ret)
-		goto me_err;
-	ret = v4l2_device_register_subdev(v4l2_dev, sd);
-	if (ret)
-		goto sd_err;
-
-	fimc->vid_cap.subdev = sd;
-	v4l2_set_subdevdata(sd, fimc);
-	sd->entity.ops = &fimc_sd_media_ops;
-	return 0;
-sd_err:
-	media_entity_cleanup(&sd->entity);
-me_err:
-	kfree(sd);
-	return ret;
-}
-
-static void fimc_destroy_capture_subdev(struct fimc_dev *fimc)
-{
-	struct v4l2_subdev *sd = fimc->vid_cap.subdev;
-
-	if (!sd)
-		return;
-	media_entity_cleanup(&sd->entity);
-	v4l2_device_unregister_subdev(sd);
-	kfree(sd);
-	fimc->vid_cap.subdev = NULL;
-}
-
 /* Set default format at the sensor and host interface */
 static int fimc_capture_set_default_format(struct fimc_dev *fimc)
 {
@@ -1479,7 +1556,7 @@ static int fimc_capture_set_default_format(struct fimc_dev *fimc)
 }
 
 /* fimc->lock must be already initialized */
-int fimc_register_capture_device(struct fimc_dev *fimc,
+static int fimc_register_capture_device(struct fimc_dev *fimc,
 				 struct v4l2_device *v4l2_dev)
 {
 	struct video_device *vfd;
@@ -1493,11 +1570,11 @@ int fimc_register_capture_device(struct fimc_dev *fimc,
 		return -ENOMEM;
 
 	ctx->fimc_dev	 = fimc;
-	ctx->in_path	 = FIMC_CAMERA;
-	ctx->out_path	 = FIMC_DMA;
+	ctx->in_path	 = FIMC_IO_CAMERA;
+	ctx->out_path	 = FIMC_IO_DMA;
 	ctx->state	 = FIMC_CTX_CAP;
 	ctx->s_frame.fmt = fimc_find_format(NULL, NULL, FMT_FLAGS_CAM, 0);
-	ctx->d_frame.fmt = fimc_find_format(NULL, NULL, FMT_FLAGS_CAM, 0);
+	ctx->d_frame.fmt = ctx->s_frame.fmt;
 
 	vfd = video_device_alloc();
 	if (!vfd) {
@@ -1505,8 +1582,7 @@ int fimc_register_capture_device(struct fimc_dev *fimc,
 		goto err_vd_alloc;
 	}
 
-	snprintf(vfd->name, sizeof(vfd->name), "%s.capture",
-		 dev_name(&fimc->pdev->dev));
+	snprintf(vfd->name, sizeof(vfd->name), "fimc.%d.capture", fimc->id);
 
 	vfd->fops	= &fimc_capture_fops;
 	vfd->ioctl_ops	= &fimc_capture_ioctl_ops;
@@ -1514,6 +1590,10 @@ int fimc_register_capture_device(struct fimc_dev *fimc,
 	vfd->minor	= -1;
 	vfd->release	= video_device_release;
 	vfd->lock	= &fimc->lock;
+	/* Locking in file operations other than ioctl should be done
+	   by the driver, not the V4L2 core.
+	   This driver needs auditing so that this flag can be removed. */
+	set_bit(V4L2_FL_LOCK_ALL_FOPS, &vfd->flags);
 	video_set_drvdata(vfd, fimc);
 
 	vid_cap = &fimc->vid_cap;
@@ -1524,7 +1604,6 @@ int fimc_register_capture_device(struct fimc_dev *fimc,
 
 	INIT_LIST_HEAD(&vid_cap->pending_buf_q);
 	INIT_LIST_HEAD(&vid_cap->active_buf_q);
-	spin_lock_init(&ctx->slock);
 	vid_cap->ctx = ctx;
 
 	q = &fimc->vid_cap.vbq;
@@ -1538,18 +1617,22 @@ int fimc_register_capture_device(struct fimc_dev *fimc,
 
 	vb2_queue_init(q);
 
-	fimc->vid_cap.vd_pad.flags = MEDIA_PAD_FL_SINK;
-	ret = media_entity_init(&vfd->entity, 1, &fimc->vid_cap.vd_pad, 0);
+	vid_cap->vd_pad.flags = MEDIA_PAD_FL_SINK;
+	ret = media_entity_init(&vfd->entity, 1, &vid_cap->vd_pad, 0);
 	if (ret)
 		goto err_ent;
-	ret = fimc_create_capture_subdev(fimc, v4l2_dev);
+
+	ret = video_register_device(vfd, VFL_TYPE_GRABBER, -1);
 	if (ret)
-		goto err_sd_reg;
+		goto err_vd;
+
+	v4l2_info(v4l2_dev, "Registered %s as /dev/%s\n",
+		  vfd->name, video_device_node_name(vfd));
 
-	vfd->ctrl_handler = &ctx->ctrl_handler;
+	vfd->ctrl_handler = &ctx->ctrls.handler;
 	return 0;
 
-err_sd_reg:
+err_vd:
 	media_entity_cleanup(&vfd->entity);
 err_ent:
 	video_device_release(vfd);
@@ -1558,17 +1641,73 @@ err_vd_alloc:
 	return ret;
 }
 
-void fimc_unregister_capture_device(struct fimc_dev *fimc)
+static int fimc_capture_subdev_registered(struct v4l2_subdev *sd)
+{
+	struct fimc_dev *fimc = v4l2_get_subdevdata(sd);
+	int ret;
+
+	ret = fimc_register_m2m_device(fimc, sd->v4l2_dev);
+	if (ret)
+		return ret;
+
+	ret = fimc_register_capture_device(fimc, sd->v4l2_dev);
+	if (ret)
+		fimc_unregister_m2m_device(fimc);
+
+	return ret;
+}
+
+static void fimc_capture_subdev_unregistered(struct v4l2_subdev *sd)
 {
-	struct video_device *vfd = fimc->vid_cap.vfd;
+	struct fimc_dev *fimc = v4l2_get_subdevdata(sd);
+
+	if (fimc == NULL)
+		return;
 
-	if (vfd) {
-		media_entity_cleanup(&vfd->entity);
-		/* Can also be called if video device was
-		   not registered */
-		video_unregister_device(vfd);
+	fimc_unregister_m2m_device(fimc);
+
+	if (fimc->vid_cap.vfd) {
+		media_entity_cleanup(&fimc->vid_cap.vfd->entity);
+		video_unregister_device(fimc->vid_cap.vfd);
+		fimc->vid_cap.vfd = NULL;
 	}
-	fimc_destroy_capture_subdev(fimc);
+
 	kfree(fimc->vid_cap.ctx);
 	fimc->vid_cap.ctx = NULL;
 }
+
+static const struct v4l2_subdev_internal_ops fimc_capture_sd_internal_ops = {
+	.registered = fimc_capture_subdev_registered,
+	.unregistered = fimc_capture_subdev_unregistered,
+};
+
+int fimc_initialize_capture_subdev(struct fimc_dev *fimc)
+{
+	struct v4l2_subdev *sd = &fimc->vid_cap.subdev;
+	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);
+
+	fimc->vid_cap.sd_pads[FIMC_SD_PAD_SINK].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);
+	if (ret)
+		return ret;
+
+	sd->entity.ops = &fimc_sd_media_ops;
+	sd->internal_ops = &fimc_capture_sd_internal_ops;
+	v4l2_set_subdevdata(sd, fimc);
+	return 0;
+}
+
+void fimc_unregister_capture_subdev(struct fimc_dev *fimc)
+{
+	struct v4l2_subdev *sd = &fimc->vid_cap.subdev;
+
+	v4l2_device_unregister_subdev(sd);
+	media_entity_cleanup(&sd->entity);
+	v4l2_set_subdevdata(sd, NULL);
+}
diff --git a/drivers/media/video/s5p-fimc/fimc-core.c b/drivers/media/video/s5p-fimc/fimc-core.c
index e184e650022..fedcd561ba2 100644
--- a/drivers/media/video/s5p-fimc/fimc-core.c
+++ b/drivers/media/video/s5p-fimc/fimc-core.c
@@ -1,8 +1,8 @@
 /*
- * Samsung S5P/EXYNOS4 SoC series camera interface (video postprocessor) driver
+ * Samsung S5P/EXYNOS4 SoC series FIMC (CAMIF) driver
  *
- * Copyright (C) 2010-2011 Samsung Electronics Co., Ltd.
- * Contact: Sylwester Nawrocki, <s.nawrocki@samsung.com>
+ * Copyright (C) 2010-2012 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 as published
@@ -28,6 +28,7 @@
 #include <media/videobuf2-dma-contig.h>
 
 #include "fimc-core.h"
+#include "fimc-reg.h"
 #include "fimc-mdevice.h"
 
 static char *fimc_clocks[MAX_FIMC_CLOCKS] = {
@@ -39,7 +40,7 @@ static struct fimc_fmt fimc_formats[] = {
 		.name		= "RGB565",
 		.fourcc		= V4L2_PIX_FMT_RGB565,
 		.depth		= { 16 },
-		.color		= S5P_FIMC_RGB565,
+		.color		= FIMC_FMT_RGB565,
 		.memplanes	= 1,
 		.colplanes	= 1,
 		.flags		= FMT_FLAGS_M2M,
@@ -47,7 +48,7 @@ static struct fimc_fmt fimc_formats[] = {
 		.name		= "BGR666",
 		.fourcc		= V4L2_PIX_FMT_BGR666,
 		.depth		= { 32 },
-		.color		= S5P_FIMC_RGB666,
+		.color		= FIMC_FMT_RGB666,
 		.memplanes	= 1,
 		.colplanes	= 1,
 		.flags		= FMT_FLAGS_M2M,
@@ -55,7 +56,7 @@ static struct fimc_fmt fimc_formats[] = {
 		.name		= "ARGB8888, 32 bpp",
 		.fourcc		= V4L2_PIX_FMT_RGB32,
 		.depth		= { 32 },
-		.color		= S5P_FIMC_RGB888,
+		.color		= FIMC_FMT_RGB888,
 		.memplanes	= 1,
 		.colplanes	= 1,
 		.flags		= FMT_FLAGS_M2M | FMT_HAS_ALPHA,
@@ -63,7 +64,7 @@ static struct fimc_fmt fimc_formats[] = {
 		.name		= "ARGB1555",
 		.fourcc		= V4L2_PIX_FMT_RGB555,
 		.depth		= { 16 },
-		.color		= S5P_FIMC_RGB555,
+		.color		= FIMC_FMT_RGB555,
 		.memplanes	= 1,
 		.colplanes	= 1,
 		.flags		= FMT_FLAGS_M2M_OUT | FMT_HAS_ALPHA,
@@ -71,7 +72,7 @@ static struct fimc_fmt fimc_formats[] = {
 		.name		= "ARGB4444",
 		.fourcc		= V4L2_PIX_FMT_RGB444,
 		.depth		= { 16 },
-		.color		= S5P_FIMC_RGB444,
+		.color		= FIMC_FMT_RGB444,
 		.memplanes	= 1,
 		.colplanes	= 1,
 		.flags		= FMT_FLAGS_M2M_OUT | FMT_HAS_ALPHA,
@@ -79,7 +80,7 @@ static struct fimc_fmt fimc_formats[] = {
 		.name		= "YUV 4:2:2 packed, YCbYCr",
 		.fourcc		= V4L2_PIX_FMT_YUYV,
 		.depth		= { 16 },
-		.color		= S5P_FIMC_YCBYCR422,
+		.color		= FIMC_FMT_YCBYCR422,
 		.memplanes	= 1,
 		.colplanes	= 1,
 		.mbus_code	= V4L2_MBUS_FMT_YUYV8_2X8,
@@ -88,7 +89,7 @@ static struct fimc_fmt fimc_formats[] = {
 		.name		= "YUV 4:2:2 packed, CbYCrY",
 		.fourcc		= V4L2_PIX_FMT_UYVY,
 		.depth		= { 16 },
-		.color		= S5P_FIMC_CBYCRY422,
+		.color		= FIMC_FMT_CBYCRY422,
 		.memplanes	= 1,
 		.colplanes	= 1,
 		.mbus_code	= V4L2_MBUS_FMT_UYVY8_2X8,
@@ -97,7 +98,7 @@ static struct fimc_fmt fimc_formats[] = {
 		.name		= "YUV 4:2:2 packed, CrYCbY",
 		.fourcc		= V4L2_PIX_FMT_VYUY,
 		.depth		= { 16 },
-		.color		= S5P_FIMC_CRYCBY422,
+		.color		= FIMC_FMT_CRYCBY422,
 		.memplanes	= 1,
 		.colplanes	= 1,
 		.mbus_code	= V4L2_MBUS_FMT_VYUY8_2X8,
@@ -106,7 +107,7 @@ static struct fimc_fmt fimc_formats[] = {
 		.name		= "YUV 4:2:2 packed, YCrYCb",
 		.fourcc		= V4L2_PIX_FMT_YVYU,
 		.depth		= { 16 },
-		.color		= S5P_FIMC_YCRYCB422,
+		.color		= FIMC_FMT_YCRYCB422,
 		.memplanes	= 1,
 		.colplanes	= 1,
 		.mbus_code	= V4L2_MBUS_FMT_YVYU8_2X8,
@@ -115,7 +116,7 @@ static struct fimc_fmt fimc_formats[] = {
 		.name		= "YUV 4:2:2 planar, Y/Cb/Cr",
 		.fourcc		= V4L2_PIX_FMT_YUV422P,
 		.depth		= { 12 },
-		.color		= S5P_FIMC_YCBYCR422,
+		.color		= FIMC_FMT_YCBYCR422,
 		.memplanes	= 1,
 		.colplanes	= 3,
 		.flags		= FMT_FLAGS_M2M,
@@ -123,7 +124,7 @@ static struct fimc_fmt fimc_formats[] = {
 		.name		= "YUV 4:2:2 planar, Y/CbCr",
 		.fourcc		= V4L2_PIX_FMT_NV16,
 		.depth		= { 16 },
-		.color		= S5P_FIMC_YCBYCR422,
+		.color		= FIMC_FMT_YCBYCR422,
 		.memplanes	= 1,
 		.colplanes	= 2,
 		.flags		= FMT_FLAGS_M2M,
@@ -131,7 +132,7 @@ static struct fimc_fmt fimc_formats[] = {
 		.name		= "YUV 4:2:2 planar, Y/CrCb",
 		.fourcc		= V4L2_PIX_FMT_NV61,
 		.depth		= { 16 },
-		.color		= S5P_FIMC_YCRYCB422,
+		.color		= FIMC_FMT_YCRYCB422,
 		.memplanes	= 1,
 		.colplanes	= 2,
 		.flags		= FMT_FLAGS_M2M,
@@ -139,7 +140,7 @@ static struct fimc_fmt fimc_formats[] = {
 		.name		= "YUV 4:2:0 planar, YCbCr",
 		.fourcc		= V4L2_PIX_FMT_YUV420,
 		.depth		= { 12 },
-		.color		= S5P_FIMC_YCBCR420,
+		.color		= FIMC_FMT_YCBCR420,
 		.memplanes	= 1,
 		.colplanes	= 3,
 		.flags		= FMT_FLAGS_M2M,
@@ -147,14 +148,14 @@ static struct fimc_fmt fimc_formats[] = {
 		.name		= "YUV 4:2:0 planar, Y/CbCr",
 		.fourcc		= V4L2_PIX_FMT_NV12,
 		.depth		= { 12 },
-		.color		= S5P_FIMC_YCBCR420,
+		.color		= FIMC_FMT_YCBCR420,
 		.memplanes	= 1,
 		.colplanes	= 2,
 		.flags		= FMT_FLAGS_M2M,
 	}, {
 		.name		= "YUV 4:2:0 non-contiguous 2-planar, Y/CbCr",
 		.fourcc		= V4L2_PIX_FMT_NV12M,
-		.color		= S5P_FIMC_YCBCR420,
+		.color		= FIMC_FMT_YCBCR420,
 		.depth		= { 8, 4 },
 		.memplanes	= 2,
 		.colplanes	= 2,
@@ -162,7 +163,7 @@ static struct fimc_fmt fimc_formats[] = {
 	}, {
 		.name		= "YUV 4:2:0 non-contiguous 3-planar, Y/Cb/Cr",
 		.fourcc		= V4L2_PIX_FMT_YUV420M,
-		.color		= S5P_FIMC_YCBCR420,
+		.color		= FIMC_FMT_YCBCR420,
 		.depth		= { 8, 2, 2 },
 		.memplanes	= 3,
 		.colplanes	= 3,
@@ -170,7 +171,7 @@ static struct fimc_fmt fimc_formats[] = {
 	}, {
 		.name		= "YUV 4:2:0 non-contiguous 2-planar, Y/CbCr, tiled",
 		.fourcc		= V4L2_PIX_FMT_NV12MT,
-		.color		= S5P_FIMC_YCBCR420,
+		.color		= FIMC_FMT_YCBCR420,
 		.depth		= { 8, 4 },
 		.memplanes	= 2,
 		.colplanes	= 2,
@@ -178,7 +179,7 @@ static struct fimc_fmt fimc_formats[] = {
 	}, {
 		.name		= "JPEG encoded data",
 		.fourcc		= V4L2_PIX_FMT_JPEG,
-		.color		= S5P_FIMC_JPEG,
+		.color		= FIMC_FMT_JPEG,
 		.depth		= { 8 },
 		.memplanes	= 1,
 		.colplanes	= 1,
@@ -187,12 +188,12 @@ static struct fimc_fmt fimc_formats[] = {
 	},
 };
 
-static unsigned int get_m2m_fmt_flags(unsigned int stream_type)
+struct fimc_fmt *fimc_get_format(unsigned int index)
 {
-	if (stream_type == V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE)
-		return FMT_FLAGS_M2M_IN;
-	else
-		return FMT_FLAGS_M2M_OUT;
+	if (index >= ARRAY_SIZE(fimc_formats))
+		return NULL;
+
+	return &fimc_formats[index];
 }
 
 int fimc_check_scaler_ratio(struct fimc_ctx *ctx, int sw, int sh,
@@ -230,7 +231,7 @@ static int fimc_get_scaler_factor(u32 src, u32 tar, u32 *ratio, u32 *shift)
 
 int fimc_set_scaler_info(struct fimc_ctx *ctx)
 {
-	struct samsung_fimc_variant *variant = ctx->fimc_dev->variant;
+	struct fimc_variant *variant = ctx->fimc_dev->variant;
 	struct device *dev = &ctx->fimc_dev->pdev->dev;
 	struct fimc_scaler *sc = &ctx->scaler;
 	struct fimc_frame *s_frame = &ctx->s_frame;
@@ -293,126 +294,9 @@ int fimc_set_scaler_info(struct fimc_ctx *ctx)
 	return 0;
 }
 
-static void fimc_m2m_job_finish(struct fimc_ctx *ctx, int vb_state)
-{
-	struct vb2_buffer *src_vb, *dst_vb;
-
-	if (!ctx || !ctx->m2m_ctx)
-		return;
-
-	src_vb = v4l2_m2m_src_buf_remove(ctx->m2m_ctx);
-	dst_vb = v4l2_m2m_dst_buf_remove(ctx->m2m_ctx);
-
-	if (src_vb && dst_vb) {
-		v4l2_m2m_buf_done(src_vb, vb_state);
-		v4l2_m2m_buf_done(dst_vb, vb_state);
-		v4l2_m2m_job_finish(ctx->fimc_dev->m2m.m2m_dev,
-				    ctx->m2m_ctx);
-	}
-}
-
-/* Complete the transaction which has been scheduled for execution. */
-static int fimc_m2m_shutdown(struct fimc_ctx *ctx)
-{
-	struct fimc_dev *fimc = ctx->fimc_dev;
-	int ret;
-
-	if (!fimc_m2m_pending(fimc))
-		return 0;
-
-	fimc_ctx_state_lock_set(FIMC_CTX_SHUT, ctx);
-
-	ret = wait_event_timeout(fimc->irq_queue,
-			   !fimc_ctx_state_is_set(FIMC_CTX_SHUT, ctx),
-			   FIMC_SHUTDOWN_TIMEOUT);
-
-	return ret == 0 ? -ETIMEDOUT : ret;
-}
-
-static int start_streaming(struct vb2_queue *q, unsigned int count)
-{
-	struct fimc_ctx *ctx = q->drv_priv;
-	int ret;
-
-	ret = pm_runtime_get_sync(&ctx->fimc_dev->pdev->dev);
-	return ret > 0 ? 0 : ret;
-}
-
-static int stop_streaming(struct vb2_queue *q)
-{
-	struct fimc_ctx *ctx = q->drv_priv;
-	int ret;
-
-	ret = fimc_m2m_shutdown(ctx);
-	if (ret == -ETIMEDOUT)
-		fimc_m2m_job_finish(ctx, VB2_BUF_STATE_ERROR);
-
-	pm_runtime_put(&ctx->fimc_dev->pdev->dev);
-	return 0;
-}
-
-void fimc_capture_irq_handler(struct fimc_dev *fimc, bool final)
-{
-	struct fimc_vid_cap *cap = &fimc->vid_cap;
-	struct fimc_vid_buffer *v_buf;
-	struct timeval *tv;
-	struct timespec ts;
-
-	if (test_and_clear_bit(ST_CAPT_SHUT, &fimc->state)) {
-		wake_up(&fimc->irq_queue);
-		return;
-	}
-
-	if (!list_empty(&cap->active_buf_q) &&
-	    test_bit(ST_CAPT_RUN, &fimc->state) && final) {
-		ktime_get_real_ts(&ts);
-
-		v_buf = fimc_active_queue_pop(cap);
-
-		tv = &v_buf->vb.v4l2_buf.timestamp;
-		tv->tv_sec = ts.tv_sec;
-		tv->tv_usec = ts.tv_nsec / NSEC_PER_USEC;
-		v_buf->vb.v4l2_buf.sequence = cap->frame_count++;
-
-		vb2_buffer_done(&v_buf->vb, VB2_BUF_STATE_DONE);
-	}
-
-	if (!list_empty(&cap->pending_buf_q)) {
-
-		v_buf = fimc_pending_queue_pop(cap);
-		fimc_hw_set_output_addr(fimc, &v_buf->paddr, cap->buf_index);
-		v_buf->index = cap->buf_index;
-
-		/* Move the buffer to the capture active queue */
-		fimc_active_queue_add(cap, v_buf);
-
-		dbg("next frame: %d, done frame: %d",
-		    fimc_hw_get_frame_index(fimc), v_buf->index);
-
-		if (++cap->buf_index >= FIMC_MAX_OUT_BUFS)
-			cap->buf_index = 0;
-	}
-
-	if (cap->active_buf_cnt == 0) {
-		if (final)
-			clear_bit(ST_CAPT_RUN, &fimc->state);
-
-		if (++cap->buf_index >= FIMC_MAX_OUT_BUFS)
-			cap->buf_index = 0;
-	} else {
-		set_bit(ST_CAPT_RUN, &fimc->state);
-	}
-
-	fimc_capture_config_update(cap->ctx);
-
-	dbg("frame: %d, active_buf_cnt: %d",
-	    fimc_hw_get_frame_index(fimc), cap->active_buf_cnt);
-}
-
 static irqreturn_t fimc_irq_handler(int irq, void *priv)
 {
 	struct fimc_dev *fimc = priv;
-	struct fimc_vid_cap *cap = &fimc->vid_cap;
 	struct fimc_ctx *ctx;
 
 	fimc_hw_clear_irq(fimc);
@@ -430,21 +314,16 @@ static irqreturn_t fimc_irq_handler(int irq, void *priv)
 			spin_unlock(&fimc->slock);
 			fimc_m2m_job_finish(ctx, VB2_BUF_STATE_DONE);
 
-			spin_lock(&ctx->slock);
 			if (ctx->state & FIMC_CTX_SHUT) {
 				ctx->state &= ~FIMC_CTX_SHUT;
 				wake_up(&fimc->irq_queue);
 			}
-			spin_unlock(&ctx->slock);
+			return IRQ_HANDLED;
 		}
-		return IRQ_HANDLED;
 	} else if (test_bit(ST_CAPT_PEND, &fimc->state)) {
-		fimc_capture_irq_handler(fimc,
-				 !test_bit(ST_CAPT_JPEG, &fimc->state));
-		if (cap->active_buf_cnt == 1) {
-			fimc_deactivate_capture(fimc);
-			clear_bit(ST_CAPT_STREAM, &fimc->state);
-		}
+		int last_buf = test_bit(ST_CAPT_JPEG, &fimc->state) &&
+				fimc->vid_cap.reqbufs_count == 1;
+		fimc_capture_irq_handler(fimc, !last_buf);
 	}
 out:
 	spin_unlock(&fimc->slock);
@@ -482,7 +361,7 @@ int fimc_prepare_addr(struct fimc_ctx *ctx, struct vb2_buffer *vb,
 		case 3:
 			paddr->cb = (u32)(paddr->y + pix_size);
 			/* decompose Y into Y/Cb/Cr */
-			if (S5P_FIMC_YCBCR420 == frame->fmt->color)
+			if (FIMC_FMT_YCBCR420 == frame->fmt->color)
 				paddr->cr = (u32)(paddr->cb
 						+ (pix_size >> 2));
 			else /* 422 */
@@ -510,40 +389,40 @@ int fimc_prepare_addr(struct fimc_ctx *ctx, struct vb2_buffer *vb,
 void fimc_set_yuv_order(struct fimc_ctx *ctx)
 {
 	/* The one only mode supported in SoC. */
-	ctx->in_order_2p = S5P_FIMC_LSB_CRCB;
-	ctx->out_order_2p = S5P_FIMC_LSB_CRCB;
+	ctx->in_order_2p = FIMC_REG_CIOCTRL_ORDER422_2P_LSB_CRCB;
+	ctx->out_order_2p = FIMC_REG_CIOCTRL_ORDER422_2P_LSB_CRCB;
 
 	/* Set order for 1 plane input formats. */
 	switch (ctx->s_frame.fmt->color) {
-	case S5P_FIMC_YCRYCB422:
-		ctx->in_order_1p = S5P_MSCTRL_ORDER422_CBYCRY;
+	case FIMC_FMT_YCRYCB422:
+		ctx->in_order_1p = FIMC_REG_MSCTRL_ORDER422_CBYCRY;
 		break;
-	case S5P_FIMC_CBYCRY422:
-		ctx->in_order_1p = S5P_MSCTRL_ORDER422_YCRYCB;
+	case FIMC_FMT_CBYCRY422:
+		ctx->in_order_1p = FIMC_REG_MSCTRL_ORDER422_YCRYCB;
 		break;
-	case S5P_FIMC_CRYCBY422:
-		ctx->in_order_1p = S5P_MSCTRL_ORDER422_YCBYCR;
+	case FIMC_FMT_CRYCBY422:
+		ctx->in_order_1p = FIMC_REG_MSCTRL_ORDER422_YCBYCR;
 		break;
-	case S5P_FIMC_YCBYCR422:
+	case FIMC_FMT_YCBYCR422:
 	default:
-		ctx->in_order_1p = S5P_MSCTRL_ORDER422_CRYCBY;
+		ctx->in_order_1p = FIMC_REG_MSCTRL_ORDER422_CRYCBY;
 		break;
 	}
 	dbg("ctx->in_order_1p= %d", ctx->in_order_1p);
 
 	switch (ctx->d_frame.fmt->color) {
-	case S5P_FIMC_YCRYCB422:
-		ctx->out_order_1p = S5P_CIOCTRL_ORDER422_CBYCRY;
+	case FIMC_FMT_YCRYCB422:
+		ctx->out_order_1p = FIMC_REG_CIOCTRL_ORDER422_CBYCRY;
 		break;
-	case S5P_FIMC_CBYCRY422:
-		ctx->out_order_1p = S5P_CIOCTRL_ORDER422_YCRYCB;
+	case FIMC_FMT_CBYCRY422:
+		ctx->out_order_1p = FIMC_REG_CIOCTRL_ORDER422_YCRYCB;
 		break;
-	case S5P_FIMC_CRYCBY422:
-		ctx->out_order_1p = S5P_CIOCTRL_ORDER422_YCBYCR;
+	case FIMC_FMT_CRYCBY422:
+		ctx->out_order_1p = FIMC_REG_CIOCTRL_ORDER422_YCBYCR;
 		break;
-	case S5P_FIMC_YCBYCR422:
+	case FIMC_FMT_YCBYCR422:
 	default:
-		ctx->out_order_1p = S5P_CIOCTRL_ORDER422_CRYCBY;
+		ctx->out_order_1p = FIMC_REG_CIOCTRL_ORDER422_CRYCBY;
 		break;
 	}
 	dbg("ctx->out_order_1p= %d", ctx->out_order_1p);
@@ -551,7 +430,7 @@ void fimc_set_yuv_order(struct fimc_ctx *ctx)
 
 void fimc_prepare_dma_offset(struct fimc_ctx *ctx, struct fimc_frame *f)
 {
-	struct samsung_fimc_variant *variant = ctx->fimc_dev->variant;
+	struct fimc_variant *variant = ctx->fimc_dev->variant;
 	u32 i, depth = 0;
 
 	for (i = 0; i < f->fmt->colplanes; i++)
@@ -574,7 +453,7 @@ void fimc_prepare_dma_offset(struct fimc_ctx *ctx, struct fimc_frame *f)
 			f->dma_offset.cb_h >>= 1;
 			f->dma_offset.cr_h >>= 1;
 		}
-		if (f->fmt->color == S5P_FIMC_YCBCR420) {
+		if (f->fmt->color == FIMC_FMT_YCBCR420) {
 			f->dma_offset.cb_v >>= 1;
 			f->dma_offset.cr_v >>= 1;
 		}
@@ -584,203 +463,58 @@ void fimc_prepare_dma_offset(struct fimc_ctx *ctx, struct fimc_frame *f)
 	    f->fmt->color, f->dma_offset.y_h, f->dma_offset.y_v);
 }
 
-/**
- * fimc_prepare_config - check dimensions, operation and color mode
- *			 and pre-calculate offset and the scaling coefficients.
- *
- * @ctx: hardware context information
- * @flags: flags indicating which parameters to check/update
- *
- * Return: 0 if dimensions are valid or non zero otherwise.
- */
-int fimc_prepare_config(struct fimc_ctx *ctx, u32 flags)
-{
-	struct fimc_frame *s_frame, *d_frame;
-	struct vb2_buffer *vb = NULL;
-	int ret = 0;
-
-	s_frame = &ctx->s_frame;
-	d_frame = &ctx->d_frame;
-
-	if (flags & FIMC_PARAMS) {
-		/* Prepare the DMA offset ratios for scaler. */
-		fimc_prepare_dma_offset(ctx, &ctx->s_frame);
-		fimc_prepare_dma_offset(ctx, &ctx->d_frame);
-
-		if (s_frame->height > (SCALER_MAX_VRATIO * d_frame->height) ||
-		    s_frame->width > (SCALER_MAX_HRATIO * d_frame->width)) {
-			err("out of scaler range");
-			return -EINVAL;
-		}
-		fimc_set_yuv_order(ctx);
-	}
-
-	if (flags & FIMC_SRC_ADDR) {
-		vb = v4l2_m2m_next_src_buf(ctx->m2m_ctx);
-		ret = fimc_prepare_addr(ctx, vb, s_frame, &s_frame->paddr);
-		if (ret)
-			return ret;
-	}
-
-	if (flags & FIMC_DST_ADDR) {
-		vb = v4l2_m2m_next_dst_buf(ctx->m2m_ctx);
-		ret = fimc_prepare_addr(ctx, vb, d_frame, &d_frame->paddr);
-	}
-
-	return ret;
-}
-
-static void fimc_dma_run(void *priv)
+int fimc_set_color_effect(struct fimc_ctx *ctx, enum v4l2_colorfx colorfx)
 {
-	struct fimc_ctx *ctx = priv;
-	struct fimc_dev *fimc;
-	unsigned long flags;
-	u32 ret;
-
-	if (WARN(!ctx, "null hardware context\n"))
-		return;
-
-	fimc = ctx->fimc_dev;
-	spin_lock_irqsave(&fimc->slock, flags);
-	set_bit(ST_M2M_PEND, &fimc->state);
-
-	spin_lock(&ctx->slock);
-	ctx->state |= (FIMC_SRC_ADDR | FIMC_DST_ADDR);
-	ret = fimc_prepare_config(ctx, ctx->state);
-	if (ret)
-		goto dma_unlock;
+	struct fimc_effect *effect = &ctx->effect;
 
-	/* Reconfigure hardware if the context has changed. */
-	if (fimc->m2m.ctx != ctx) {
-		ctx->state |= FIMC_PARAMS;
-		fimc->m2m.ctx = ctx;
-	}
-	fimc_hw_set_input_addr(fimc, &ctx->s_frame.paddr);
-
-	if (ctx->state & FIMC_PARAMS) {
-		fimc_hw_set_input_path(ctx);
-		fimc_hw_set_in_dma(ctx);
-		ret = fimc_set_scaler_info(ctx);
-		if (ret) {
-			spin_unlock(&fimc->slock);
-			goto dma_unlock;
-		}
-		fimc_hw_set_prescaler(ctx);
-		fimc_hw_set_mainscaler(ctx);
-		fimc_hw_set_target_format(ctx);
-		fimc_hw_set_rotation(ctx);
-		fimc_hw_set_effect(ctx, false);
-	}
-
-	fimc_hw_set_output_path(ctx);
-	if (ctx->state & (FIMC_DST_ADDR | FIMC_PARAMS))
-		fimc_hw_set_output_addr(fimc, &ctx->d_frame.paddr, -1);
-
-	if (ctx->state & FIMC_PARAMS) {
-		fimc_hw_set_out_dma(ctx);
-		if (fimc->variant->has_alpha)
-			fimc_hw_set_rgb_alpha(ctx);
-	}
-
-	fimc_activate_capture(ctx);
-
-	ctx->state &= (FIMC_CTX_M2M | FIMC_CTX_CAP |
-		       FIMC_SRC_FMT | FIMC_DST_FMT);
-	fimc_hw_activate_input_dma(fimc, true);
-dma_unlock:
-	spin_unlock(&ctx->slock);
-	spin_unlock_irqrestore(&fimc->slock, flags);
-}
-
-static void fimc_job_abort(void *priv)
-{
-	fimc_m2m_shutdown(priv);
-}
-
-static int fimc_queue_setup(struct vb2_queue *vq, const struct v4l2_format *fmt,
-			    unsigned int *num_buffers, unsigned int *num_planes,
-			    unsigned int sizes[], void *allocators[])
-{
-	struct fimc_ctx *ctx = vb2_get_drv_priv(vq);
-	struct fimc_frame *f;
-	int i;
-
-	f = ctx_get_frame(ctx, vq->type);
-	if (IS_ERR(f))
-		return PTR_ERR(f);
-	/*
-	 * Return number of non-contigous planes (plane buffers)
-	 * depending on the configured color format.
-	 */
-	if (!f->fmt)
+	switch (colorfx) {
+	case V4L2_COLORFX_NONE:
+		effect->type = FIMC_REG_CIIMGEFF_FIN_BYPASS;
+		break;
+	case V4L2_COLORFX_BW:
+		effect->type = FIMC_REG_CIIMGEFF_FIN_ARBITRARY;
+		effect->pat_cb = 128;
+		effect->pat_cr = 128;
+		break;
+	case V4L2_COLORFX_SEPIA:
+		effect->type = FIMC_REG_CIIMGEFF_FIN_ARBITRARY;
+		effect->pat_cb = 115;
+		effect->pat_cr = 145;
+		break;
+	case V4L2_COLORFX_NEGATIVE:
+		effect->type = FIMC_REG_CIIMGEFF_FIN_NEGATIVE;
+		break;
+	case V4L2_COLORFX_EMBOSS:
+		effect->type = FIMC_REG_CIIMGEFF_FIN_EMBOSSING;
+		break;
+	case V4L2_COLORFX_ART_FREEZE:
+		effect->type = FIMC_REG_CIIMGEFF_FIN_ARTFREEZE;
+		break;
+	case V4L2_COLORFX_SILHOUETTE:
+		effect->type = FIMC_REG_CIIMGEFF_FIN_SILHOUETTE;
+		break;
+	case V4L2_COLORFX_SET_CBCR:
+		effect->type = FIMC_REG_CIIMGEFF_FIN_ARBITRARY;
+		effect->pat_cb = ctx->ctrls.colorfx_cbcr->val >> 8;
+		effect->pat_cr = ctx->ctrls.colorfx_cbcr->val & 0xff;
+		break;
+	default:
 		return -EINVAL;
-
-	*num_planes = f->fmt->memplanes;
-	for (i = 0; i < f->fmt->memplanes; i++) {
-		sizes[i] = (f->f_width * f->f_height * f->fmt->depth[i]) / 8;
-		allocators[i] = ctx->fimc_dev->alloc_ctx;
 	}
-	return 0;
-}
-
-static int fimc_buf_prepare(struct vb2_buffer *vb)
-{
-	struct fimc_ctx *ctx = vb2_get_drv_priv(vb->vb2_queue);
-	struct fimc_frame *frame;
-	int i;
-
-	frame = ctx_get_frame(ctx, vb->vb2_queue->type);
-	if (IS_ERR(frame))
-		return PTR_ERR(frame);
-
-	for (i = 0; i < frame->fmt->memplanes; i++)
-		vb2_set_plane_payload(vb, i, frame->payload[i]);
 
 	return 0;
 }
 
-static void fimc_buf_queue(struct vb2_buffer *vb)
-{
-	struct fimc_ctx *ctx = vb2_get_drv_priv(vb->vb2_queue);
-
-	dbg("ctx: %p, ctx->state: 0x%x", ctx, ctx->state);
-
-	if (ctx->m2m_ctx)
-		v4l2_m2m_buf_queue(ctx->m2m_ctx, vb);
-}
-
-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_qops = {
-	.queue_setup	 = fimc_queue_setup,
-	.buf_prepare	 = fimc_buf_prepare,
-	.buf_queue	 = fimc_buf_queue,
-	.wait_prepare	 = fimc_unlock,
-	.wait_finish	 = fimc_lock,
-	.stop_streaming	 = stop_streaming,
-	.start_streaming = start_streaming,
-};
-
 /*
  * V4L2 controls handling
  */
 #define ctrl_to_ctx(__ctrl) \
-	container_of((__ctrl)->handler, struct fimc_ctx, ctrl_handler)
+	container_of((__ctrl)->handler, struct fimc_ctx, ctrls.handler)
 
 static int __fimc_s_ctrl(struct fimc_ctx *ctx, struct v4l2_ctrl *ctrl)
 {
 	struct fimc_dev *fimc = ctx->fimc_dev;
-	struct samsung_fimc_variant *variant = fimc->variant;
+	struct fimc_variant *variant = fimc->variant;
 	unsigned int flags = FIMC_DST_FMT | FIMC_SRC_FMT;
 	int ret = 0;
 
@@ -815,7 +549,14 @@ static int __fimc_s_ctrl(struct fimc_ctx *ctx, struct v4l2_ctrl *ctrl)
 	case V4L2_CID_ALPHA_COMPONENT:
 		ctx->d_frame.alpha = ctrl->val;
 		break;
+
+	case V4L2_CID_COLORFX:
+		ret = fimc_set_color_effect(ctx, ctrl->val);
+		if (ret)
+			return ret;
+		break;
 	}
+
 	ctx->state |= FIMC_PARAMS;
 	set_bit(ST_CAPT_APPLY_CFG, &fimc->state);
 	return 0;
@@ -827,9 +568,9 @@ static int fimc_s_ctrl(struct v4l2_ctrl *ctrl)
 	unsigned long flags;
 	int ret;
 
-	spin_lock_irqsave(&ctx->slock, flags);
+	spin_lock_irqsave(&ctx->fimc_dev->slock, flags);
 	ret = __fimc_s_ctrl(ctx, ctrl);
-	spin_unlock_irqrestore(&ctx->slock, flags);
+	spin_unlock_irqrestore(&ctx->fimc_dev->slock, flags);
 
 	return ret;
 }
@@ -840,71 +581,93 @@ static const struct v4l2_ctrl_ops fimc_ctrl_ops = {
 
 int fimc_ctrls_create(struct fimc_ctx *ctx)
 {
-	struct samsung_fimc_variant *variant = ctx->fimc_dev->variant;
+	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;
 
-	if (ctx->ctrls_rdy)
+	if (ctx->ctrls.ready)
 		return 0;
-	v4l2_ctrl_handler_init(&ctx->ctrl_handler, 4);
 
-	ctx->ctrl_rotate = v4l2_ctrl_new_std(&ctx->ctrl_handler, &fimc_ctrl_ops,
+	v4l2_ctrl_handler_init(handler, 6);
+
+	ctrls->rotate = v4l2_ctrl_new_std(handler, &fimc_ctrl_ops,
 					V4L2_CID_ROTATE, 0, 270, 90, 0);
-	ctx->ctrl_hflip = v4l2_ctrl_new_std(&ctx->ctrl_handler, &fimc_ctrl_ops,
+	ctrls->hflip = v4l2_ctrl_new_std(handler, &fimc_ctrl_ops,
 					V4L2_CID_HFLIP, 0, 1, 1, 0);
-	ctx->ctrl_vflip = v4l2_ctrl_new_std(&ctx->ctrl_handler, &fimc_ctrl_ops,
+	ctrls->vflip = v4l2_ctrl_new_std(handler, &fimc_ctrl_ops,
 					V4L2_CID_VFLIP, 0, 1, 1, 0);
+
 	if (variant->has_alpha)
-		ctx->ctrl_alpha = v4l2_ctrl_new_std(&ctx->ctrl_handler,
-				    &fimc_ctrl_ops, V4L2_CID_ALPHA_COMPONENT,
-				    0, max_alpha, 1, 0);
+		ctrls->alpha = v4l2_ctrl_new_std(handler, &fimc_ctrl_ops,
+					V4L2_CID_ALPHA_COMPONENT,
+					0, max_alpha, 1, 0);
 	else
-		ctx->ctrl_alpha = NULL;
+		ctrls->alpha = NULL;
 
-	ctx->ctrls_rdy = ctx->ctrl_handler.error == 0;
+	ctrls->colorfx = v4l2_ctrl_new_std_menu(handler, &fimc_ctrl_ops,
+				V4L2_CID_COLORFX, V4L2_COLORFX_SET_CBCR,
+				~0x983f, V4L2_COLORFX_NONE);
 
-	return ctx->ctrl_handler.error;
+	ctrls->colorfx_cbcr = v4l2_ctrl_new_std(handler, &fimc_ctrl_ops,
+				V4L2_CID_COLORFX_CBCR, 0, 0xffff, 1, 0);
+
+	ctx->effect.type = FIMC_REG_CIIMGEFF_FIN_BYPASS;
+
+	if (!handler->error) {
+		v4l2_ctrl_cluster(3, &ctrls->colorfx);
+		ctrls->ready = true;
+	}
+
+	return handler->error;
 }
 
 void fimc_ctrls_delete(struct fimc_ctx *ctx)
 {
-	if (ctx->ctrls_rdy) {
-		v4l2_ctrl_handler_free(&ctx->ctrl_handler);
-		ctx->ctrls_rdy = false;
-		ctx->ctrl_alpha = NULL;
+	struct fimc_ctrls *ctrls = &ctx->ctrls;
+
+	if (ctrls->ready) {
+		v4l2_ctrl_handler_free(&ctrls->handler);
+		ctrls->ready = false;
+		ctrls->alpha = NULL;
 	}
 }
 
 void fimc_ctrls_activate(struct fimc_ctx *ctx, bool active)
 {
 	unsigned int has_alpha = ctx->d_frame.fmt->flags & FMT_HAS_ALPHA;
+	struct fimc_ctrls *ctrls = &ctx->ctrls;
 
-	if (!ctx->ctrls_rdy)
+	if (!ctrls->ready)
 		return;
 
-	mutex_lock(&ctx->ctrl_handler.lock);
-	v4l2_ctrl_activate(ctx->ctrl_rotate, active);
-	v4l2_ctrl_activate(ctx->ctrl_hflip, active);
-	v4l2_ctrl_activate(ctx->ctrl_vflip, active);
-	if (ctx->ctrl_alpha)
-		v4l2_ctrl_activate(ctx->ctrl_alpha, active && has_alpha);
+	mutex_lock(&ctrls->handler.lock);
+	v4l2_ctrl_activate(ctrls->rotate, active);
+	v4l2_ctrl_activate(ctrls->hflip, active);
+	v4l2_ctrl_activate(ctrls->vflip, active);
+	v4l2_ctrl_activate(ctrls->colorfx, active);
+	if (ctrls->alpha)
+		v4l2_ctrl_activate(ctrls->alpha, active && has_alpha);
 
 	if (active) {
-		ctx->rotation = ctx->ctrl_rotate->val;
-		ctx->hflip    = ctx->ctrl_hflip->val;
-		ctx->vflip    = ctx->ctrl_vflip->val;
+		fimc_set_color_effect(ctx, ctrls->colorfx->cur.val);
+		ctx->rotation = ctrls->rotate->val;
+		ctx->hflip    = ctrls->hflip->val;
+		ctx->vflip    = ctrls->vflip->val;
 	} else {
+		ctx->effect.type = FIMC_REG_CIIMGEFF_FIN_BYPASS;
 		ctx->rotation = 0;
 		ctx->hflip    = 0;
 		ctx->vflip    = 0;
 	}
-	mutex_unlock(&ctx->ctrl_handler.lock);
+	mutex_unlock(&ctrls->handler.lock);
 }
 
 /* Update maximum value of the alpha color control */
 void fimc_alpha_ctrl_update(struct fimc_ctx *ctx)
 {
 	struct fimc_dev *fimc = ctx->fimc_dev;
-	struct v4l2_ctrl *ctrl = ctx->ctrl_alpha;
+	struct v4l2_ctrl *ctrl = ctx->ctrls.alpha;
 
 	if (ctrl == NULL || !fimc->variant->has_alpha)
 		return;
@@ -918,39 +681,6 @@ void fimc_alpha_ctrl_update(struct fimc_ctx *ctx)
 	v4l2_ctrl_unlock(ctrl);
 }
 
-/*
- * V4L2 ioctl handlers
- */
-static int fimc_m2m_querycap(struct file *file, void *fh,
-			     struct v4l2_capability *cap)
-{
-	struct fimc_ctx *ctx = fh_to_ctx(fh);
-	struct fimc_dev *fimc = ctx->fimc_dev;
-
-	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 | V4L2_CAP_VIDEO_OUTPUT_MPLANE;
-
-	return 0;
-}
-
-static int fimc_m2m_enum_fmt_mplane(struct file *file, void *priv,
-				    struct v4l2_fmtdesc *f)
-{
-	struct fimc_fmt *fmt;
-
-	fmt = fimc_find_format(NULL, NULL, get_m2m_fmt_flags(f->type),
-			       f->index);
-	if (!fmt)
-		return -EINVAL;
-
-	strncpy(f->description, fmt->name, sizeof(f->description) - 1);
-	f->pixelformat = fmt->fourcc;
-	return 0;
-}
-
 int fimc_fill_format(struct fimc_frame *frame, struct v4l2_format *f)
 {
 	struct v4l2_pix_format_mplane *pixm = &f->fmt.pix_mp;
@@ -1029,18 +759,6 @@ void fimc_adjust_mplane_format(struct fimc_fmt *fmt, u32 width, u32 height,
 	}
 }
 
-static int fimc_m2m_g_fmt_mplane(struct file *file, void *fh,
-				 struct v4l2_format *f)
-{
-	struct fimc_ctx *ctx = fh_to_ctx(fh);
-	struct fimc_frame *frame = ctx_get_frame(ctx, f->type);
-
-	if (IS_ERR(frame))
-		return PTR_ERR(frame);
-
-	return fimc_fill_format(frame, f);
-}
-
 /**
  * fimc_find_format - lookup fimc color format by fourcc or media bus format
  * @pixelformat: fourcc to match, ignored if null
@@ -1048,14 +766,14 @@ static int fimc_m2m_g_fmt_mplane(struct file *file, void *fh,
  * @mask: the color flags to match
  * @index: offset in the fimc_formats array, ignored if negative
  */
-struct fimc_fmt *fimc_find_format(u32 *pixelformat, u32 *mbus_code,
+struct fimc_fmt *fimc_find_format(const u32 *pixelformat, const u32 *mbus_code,
 				  unsigned int mask, int index)
 {
 	struct fimc_fmt *fmt, *def_fmt = NULL;
 	unsigned int i;
 	int id = 0;
 
-	if (index >= ARRAY_SIZE(fimc_formats))
+	if (index >= (int)ARRAY_SIZE(fimc_formats))
 		return NULL;
 
 	for (i = 0; i < ARRAY_SIZE(fimc_formats); ++i) {
@@ -1073,535 +791,10 @@ struct fimc_fmt *fimc_find_format(u32 *pixelformat, u32 *mbus_code,
 	return def_fmt;
 }
 
-static int fimc_try_fmt_mplane(struct fimc_ctx *ctx, struct v4l2_format *f)
-{
-	struct fimc_dev *fimc = ctx->fimc_dev;
-	struct samsung_fimc_variant *variant = fimc->variant;
-	struct v4l2_pix_format_mplane *pix = &f->fmt.pix_mp;
-	struct fimc_fmt *fmt;
-	u32 max_w, mod_x, mod_y;
-
-	if (!IS_M2M(f->type))
-		return -EINVAL;
-
-	dbg("w: %d, h: %d", pix->width, pix->height);
-
-	fmt = fimc_find_format(&pix->pixelformat, NULL,
-			       get_m2m_fmt_flags(f->type), 0);
-	if (WARN(fmt == NULL, "Pixel format lookup failed"))
-		return -EINVAL;
-
-	if (pix->field == V4L2_FIELD_ANY)
-		pix->field = V4L2_FIELD_NONE;
-	else if (pix->field != V4L2_FIELD_NONE)
-		return -EINVAL;
-
-	if (f->type == V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE) {
-		max_w = variant->pix_limit->scaler_dis_w;
-		mod_x = ffs(variant->min_inp_pixsize) - 1;
-	} else {
-		max_w = variant->pix_limit->out_rot_dis_w;
-		mod_x = ffs(variant->min_out_pixsize) - 1;
-	}
-
-	if (tiled_fmt(fmt)) {
-		mod_x = 6; /* 64 x 32 pixels tile */
-		mod_y = 5;
-	} else {
-		if (variant->min_vsize_align == 1)
-			mod_y = fimc_fmt_is_rgb(fmt->color) ? 0 : 1;
-		else
-			mod_y = ffs(variant->min_vsize_align) - 1;
-	}
-
-	v4l_bound_align_image(&pix->width, 16, max_w, mod_x,
-		&pix->height, 8, variant->pix_limit->scaler_dis_w, mod_y, 0);
-
-	fimc_adjust_mplane_format(fmt, pix->width, pix->height, &f->fmt.pix_mp);
-	return 0;
-}
-
-static int fimc_m2m_try_fmt_mplane(struct file *file, void *fh,
-				   struct v4l2_format *f)
-{
-	struct fimc_ctx *ctx = fh_to_ctx(fh);
-
-	return fimc_try_fmt_mplane(ctx, f);
-}
-
-static int fimc_m2m_s_fmt_mplane(struct file *file, void *fh,
-				 struct v4l2_format *f)
-{
-	struct fimc_ctx *ctx = fh_to_ctx(fh);
-	struct fimc_dev *fimc = ctx->fimc_dev;
-	struct vb2_queue *vq;
-	struct fimc_frame *frame;
-	struct v4l2_pix_format_mplane *pix;
-	int i, ret = 0;
-
-	ret = fimc_try_fmt_mplane(ctx, f);
-	if (ret)
-		return ret;
-
-	vq = v4l2_m2m_get_vq(ctx->m2m_ctx, f->type);
-
-	if (vb2_is_busy(vq)) {
-		v4l2_err(fimc->m2m.vfd, "queue (%d) busy\n", f->type);
-		return -EBUSY;
-	}
-
-	if (f->type == V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE)
-		frame = &ctx->s_frame;
-	else
-		frame = &ctx->d_frame;
-
-	pix = &f->fmt.pix_mp;
-	frame->fmt = fimc_find_format(&pix->pixelformat, NULL,
-				      get_m2m_fmt_flags(f->type), 0);
-	if (!frame->fmt)
-		return -EINVAL;
-
-	/* Update RGB Alpha control state and value range */
-	fimc_alpha_ctrl_update(ctx);
-
-	for (i = 0; i < frame->fmt->colplanes; i++) {
-		frame->payload[i] =
-			(pix->width * pix->height * frame->fmt->depth[i]) / 8;
-	}
-
-	fimc_fill_frame(frame, f);
-
-	ctx->scaler.enabled = 1;
-
-	if (f->type == V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE)
-		fimc_ctx_state_lock_set(FIMC_PARAMS | FIMC_DST_FMT, ctx);
-	else
-		fimc_ctx_state_lock_set(FIMC_PARAMS | FIMC_SRC_FMT, ctx);
-
-	dbg("f_w: %d, f_h: %d", frame->f_width, frame->f_height);
-
-	return 0;
-}
-
-static int fimc_m2m_reqbufs(struct file *file, void *fh,
-			    struct v4l2_requestbuffers *reqbufs)
-{
-	struct fimc_ctx *ctx = fh_to_ctx(fh);
-
-	return v4l2_m2m_reqbufs(file, ctx->m2m_ctx, reqbufs);
-}
-
-static int fimc_m2m_querybuf(struct file *file, void *fh,
-			     struct v4l2_buffer *buf)
-{
-	struct fimc_ctx *ctx = fh_to_ctx(fh);
-
-	return v4l2_m2m_querybuf(file, ctx->m2m_ctx, buf);
-}
-
-static int fimc_m2m_qbuf(struct file *file, void *fh,
-			 struct v4l2_buffer *buf)
-{
-	struct fimc_ctx *ctx = fh_to_ctx(fh);
-
-	return v4l2_m2m_qbuf(file, ctx->m2m_ctx, buf);
-}
-
-static int fimc_m2m_dqbuf(struct file *file, void *fh,
-			  struct v4l2_buffer *buf)
-{
-	struct fimc_ctx *ctx = fh_to_ctx(fh);
-
-	return v4l2_m2m_dqbuf(file, ctx->m2m_ctx, buf);
-}
-
-static int fimc_m2m_streamon(struct file *file, void *fh,
-			     enum v4l2_buf_type type)
-{
-	struct fimc_ctx *ctx = fh_to_ctx(fh);
-
-	/* The source and target color format need to be set */
-	if (V4L2_TYPE_IS_OUTPUT(type)) {
-		if (!fimc_ctx_state_is_set(FIMC_SRC_FMT, ctx))
-			return -EINVAL;
-	} else if (!fimc_ctx_state_is_set(FIMC_DST_FMT, ctx)) {
-		return -EINVAL;
-	}
-
-	return v4l2_m2m_streamon(file, ctx->m2m_ctx, type);
-}
-
-static int fimc_m2m_streamoff(struct file *file, void *fh,
-			    enum v4l2_buf_type type)
-{
-	struct fimc_ctx *ctx = fh_to_ctx(fh);
-
-	return v4l2_m2m_streamoff(file, ctx->m2m_ctx, type);
-}
-
-static int fimc_m2m_cropcap(struct file *file, void *fh,
-			    struct v4l2_cropcap *cr)
-{
-	struct fimc_ctx *ctx = fh_to_ctx(fh);
-	struct fimc_frame *frame;
-
-	frame = ctx_get_frame(ctx, cr->type);
-	if (IS_ERR(frame))
-		return PTR_ERR(frame);
-
-	cr->bounds.left		= 0;
-	cr->bounds.top		= 0;
-	cr->bounds.width	= frame->o_width;
-	cr->bounds.height	= frame->o_height;
-	cr->defrect		= cr->bounds;
-
-	return 0;
-}
-
-static int fimc_m2m_g_crop(struct file *file, void *fh, struct v4l2_crop *cr)
-{
-	struct fimc_ctx *ctx = fh_to_ctx(fh);
-	struct fimc_frame *frame;
-
-	frame = ctx_get_frame(ctx, cr->type);
-	if (IS_ERR(frame))
-		return PTR_ERR(frame);
-
-	cr->c.left = frame->offs_h;
-	cr->c.top = frame->offs_v;
-	cr->c.width = frame->width;
-	cr->c.height = frame->height;
-
-	return 0;
-}
-
-static int fimc_m2m_try_crop(struct fimc_ctx *ctx, struct v4l2_crop *cr)
-{
-	struct fimc_dev *fimc = ctx->fimc_dev;
-	struct fimc_frame *f;
-	u32 min_size, halign, depth = 0;
-	int i;
-
-	if (cr->c.top < 0 || cr->c.left < 0) {
-		v4l2_err(fimc->m2m.vfd,
-			"doesn't support negative values for top & left\n");
-		return -EINVAL;
-	}
-	if (cr->type == V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE)
-		f = &ctx->d_frame;
-	else if (cr->type == V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE)
-		f = &ctx->s_frame;
-	else
-		return -EINVAL;
-
-	min_size = (f == &ctx->s_frame) ?
-		fimc->variant->min_inp_pixsize : fimc->variant->min_out_pixsize;
-
-	/* Get pixel alignment constraints. */
-	if (fimc->variant->min_vsize_align == 1)
-		halign = fimc_fmt_is_rgb(f->fmt->color) ? 0 : 1;
-	else
-		halign = ffs(fimc->variant->min_vsize_align) - 1;
-
-	for (i = 0; i < f->fmt->colplanes; i++)
-		depth += f->fmt->depth[i];
-
-	v4l_bound_align_image(&cr->c.width, min_size, f->o_width,
-			      ffs(min_size) - 1,
-			      &cr->c.height, min_size, f->o_height,
-			      halign, 64/(ALIGN(depth, 8)));
-
-	/* adjust left/top if cropping rectangle is out of bounds */
-	if (cr->c.left + cr->c.width > f->o_width)
-		cr->c.left = f->o_width - cr->c.width;
-	if (cr->c.top + cr->c.height > f->o_height)
-		cr->c.top = f->o_height - cr->c.height;
-
-	cr->c.left = round_down(cr->c.left, min_size);
-	cr->c.top  = round_down(cr->c.top, fimc->variant->hor_offs_align);
-
-	dbg("l:%d, t:%d, w:%d, h:%d, f_w: %d, f_h: %d",
-	    cr->c.left, cr->c.top, cr->c.width, cr->c.height,
-	    f->f_width, f->f_height);
-
-	return 0;
-}
-
-static int fimc_m2m_s_crop(struct file *file, void *fh, struct v4l2_crop *cr)
-{
-	struct fimc_ctx *ctx = fh_to_ctx(fh);
-	struct fimc_dev *fimc = ctx->fimc_dev;
-	struct fimc_frame *f;
-	int ret;
-
-	ret = fimc_m2m_try_crop(ctx, cr);
-	if (ret)
-		return ret;
-
-	f = (cr->type == V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE) ?
-		&ctx->s_frame : &ctx->d_frame;
-
-	/* Check to see if scaling ratio is within supported range */
-	if (fimc_ctx_state_is_set(FIMC_DST_FMT | FIMC_SRC_FMT, ctx)) {
-		if (cr->type == V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE) {
-			ret = fimc_check_scaler_ratio(ctx, cr->c.width,
-					cr->c.height, ctx->d_frame.width,
-					ctx->d_frame.height, ctx->rotation);
-		} else {
-			ret = fimc_check_scaler_ratio(ctx, ctx->s_frame.width,
-					ctx->s_frame.height, cr->c.width,
-					cr->c.height, ctx->rotation);
-		}
-		if (ret) {
-			v4l2_err(fimc->m2m.vfd, "Out of scaler range\n");
-			return -EINVAL;
-		}
-	}
-
-	f->offs_h = cr->c.left;
-	f->offs_v = cr->c.top;
-	f->width  = cr->c.width;
-	f->height = cr->c.height;
-
-	fimc_ctx_state_lock_set(FIMC_PARAMS, ctx);
-
-	return 0;
-}
-
-static const struct v4l2_ioctl_ops fimc_m2m_ioctl_ops = {
-	.vidioc_querycap		= fimc_m2m_querycap,
-
-	.vidioc_enum_fmt_vid_cap_mplane	= fimc_m2m_enum_fmt_mplane,
-	.vidioc_enum_fmt_vid_out_mplane	= fimc_m2m_enum_fmt_mplane,
-
-	.vidioc_g_fmt_vid_cap_mplane	= fimc_m2m_g_fmt_mplane,
-	.vidioc_g_fmt_vid_out_mplane	= fimc_m2m_g_fmt_mplane,
-
-	.vidioc_try_fmt_vid_cap_mplane	= fimc_m2m_try_fmt_mplane,
-	.vidioc_try_fmt_vid_out_mplane	= fimc_m2m_try_fmt_mplane,
-
-	.vidioc_s_fmt_vid_cap_mplane	= fimc_m2m_s_fmt_mplane,
-	.vidioc_s_fmt_vid_out_mplane	= fimc_m2m_s_fmt_mplane,
-
-	.vidioc_reqbufs			= fimc_m2m_reqbufs,
-	.vidioc_querybuf		= fimc_m2m_querybuf,
-
-	.vidioc_qbuf			= fimc_m2m_qbuf,
-	.vidioc_dqbuf			= fimc_m2m_dqbuf,
-
-	.vidioc_streamon		= fimc_m2m_streamon,
-	.vidioc_streamoff		= fimc_m2m_streamoff,
-
-	.vidioc_g_crop			= fimc_m2m_g_crop,
-	.vidioc_s_crop			= fimc_m2m_s_crop,
-	.vidioc_cropcap			= fimc_m2m_cropcap
-
-};
-
-static int queue_init(void *priv, struct vb2_queue *src_vq,
-		      struct vb2_queue *dst_vq)
-{
-	struct fimc_ctx *ctx = priv;
-	int ret;
-
-	memset(src_vq, 0, sizeof(*src_vq));
-	src_vq->type = V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE;
-	src_vq->io_modes = VB2_MMAP | VB2_USERPTR;
-	src_vq->drv_priv = ctx;
-	src_vq->ops = &fimc_qops;
-	src_vq->mem_ops = &vb2_dma_contig_memops;
-	src_vq->buf_struct_size = sizeof(struct v4l2_m2m_buffer);
-
-	ret = vb2_queue_init(src_vq);
-	if (ret)
-		return ret;
-
-	memset(dst_vq, 0, sizeof(*dst_vq));
-	dst_vq->type = V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE;
-	dst_vq->io_modes = VB2_MMAP | VB2_USERPTR;
-	dst_vq->drv_priv = ctx;
-	dst_vq->ops = &fimc_qops;
-	dst_vq->mem_ops = &vb2_dma_contig_memops;
-	dst_vq->buf_struct_size = sizeof(struct v4l2_m2m_buffer);
-
-	return vb2_queue_init(dst_vq);
-}
-
-static int fimc_m2m_open(struct file *file)
-{
-	struct fimc_dev *fimc = video_drvdata(file);
-	struct fimc_ctx *ctx;
-	int ret;
-
-	dbg("pid: %d, state: 0x%lx, refcnt: %d",
-		task_pid_nr(current), fimc->state, fimc->vid_cap.refcnt);
-
-	/*
-	 * Return if the corresponding video capture node
-	 * is already opened.
-	 */
-	if (fimc->vid_cap.refcnt > 0)
-		return -EBUSY;
-
-	ctx = kzalloc(sizeof *ctx, GFP_KERNEL);
-	if (!ctx)
-		return -ENOMEM;
-	v4l2_fh_init(&ctx->fh, fimc->m2m.vfd);
-	ctx->fimc_dev = fimc;
-
-	/* Default color format */
-	ctx->s_frame.fmt = &fimc_formats[0];
-	ctx->d_frame.fmt = &fimc_formats[0];
-
-	ret = fimc_ctrls_create(ctx);
-	if (ret)
-		goto error_fh;
-
-	/* Use separate control handler per file handle */
-	ctx->fh.ctrl_handler = &ctx->ctrl_handler;
-	file->private_data = &ctx->fh;
-	v4l2_fh_add(&ctx->fh);
-
-	/* Setup the device context for memory-to-memory mode */
-	ctx->state = FIMC_CTX_M2M;
-	ctx->flags = 0;
-	ctx->in_path = FIMC_DMA;
-	ctx->out_path = FIMC_DMA;
-	spin_lock_init(&ctx->slock);
-
-	ctx->m2m_ctx = v4l2_m2m_ctx_init(fimc->m2m.m2m_dev, ctx, queue_init);
-	if (IS_ERR(ctx->m2m_ctx)) {
-		ret = PTR_ERR(ctx->m2m_ctx);
-		goto error_c;
-	}
-
-	if (fimc->m2m.refcnt++ == 0)
-		set_bit(ST_M2M_RUN, &fimc->state);
-	return 0;
-
-error_c:
-	fimc_ctrls_delete(ctx);
-error_fh:
-	v4l2_fh_del(&ctx->fh);
-	v4l2_fh_exit(&ctx->fh);
-	kfree(ctx);
-	return ret;
-}
-
-static int fimc_m2m_release(struct file *file)
-{
-	struct fimc_ctx *ctx = fh_to_ctx(file->private_data);
-	struct fimc_dev *fimc = ctx->fimc_dev;
-
-	dbg("pid: %d, state: 0x%lx, refcnt= %d",
-		task_pid_nr(current), fimc->state, fimc->m2m.refcnt);
-
-	v4l2_m2m_ctx_release(ctx->m2m_ctx);
-	fimc_ctrls_delete(ctx);
-	v4l2_fh_del(&ctx->fh);
-	v4l2_fh_exit(&ctx->fh);
-
-	if (--fimc->m2m.refcnt <= 0)
-		clear_bit(ST_M2M_RUN, &fimc->state);
-	kfree(ctx);
-	return 0;
-}
-
-static unsigned int fimc_m2m_poll(struct file *file,
-				  struct poll_table_struct *wait)
-{
-	struct fimc_ctx *ctx = fh_to_ctx(file->private_data);
-
-	return v4l2_m2m_poll(file, ctx->m2m_ctx, wait);
-}
-
-
-static int fimc_m2m_mmap(struct file *file, struct vm_area_struct *vma)
-{
-	struct fimc_ctx *ctx = fh_to_ctx(file->private_data);
-
-	return v4l2_m2m_mmap(file, ctx->m2m_ctx, vma);
-}
-
-static const struct v4l2_file_operations fimc_m2m_fops = {
-	.owner		= THIS_MODULE,
-	.open		= fimc_m2m_open,
-	.release	= fimc_m2m_release,
-	.poll		= fimc_m2m_poll,
-	.unlocked_ioctl	= video_ioctl2,
-	.mmap		= fimc_m2m_mmap,
-};
-
-static struct v4l2_m2m_ops m2m_ops = {
-	.device_run	= fimc_dma_run,
-	.job_abort	= fimc_job_abort,
-};
-
-int fimc_register_m2m_device(struct fimc_dev *fimc,
-			     struct v4l2_device *v4l2_dev)
-{
-	struct video_device *vfd;
-	struct platform_device *pdev;
-	int ret = 0;
-
-	if (!fimc)
-		return -ENODEV;
-
-	pdev = fimc->pdev;
-	fimc->v4l2_dev = v4l2_dev;
-
-	vfd = video_device_alloc();
-	if (!vfd) {
-		v4l2_err(v4l2_dev, "Failed to allocate video device\n");
-		return -ENOMEM;
-	}
-
-	vfd->fops	= &fimc_m2m_fops;
-	vfd->ioctl_ops	= &fimc_m2m_ioctl_ops;
-	vfd->v4l2_dev	= v4l2_dev;
-	vfd->minor	= -1;
-	vfd->release	= video_device_release;
-	vfd->lock	= &fimc->lock;
-
-	snprintf(vfd->name, sizeof(vfd->name), "%s.m2m", dev_name(&pdev->dev));
-	video_set_drvdata(vfd, fimc);
-
-	fimc->m2m.vfd = vfd;
-	fimc->m2m.m2m_dev = v4l2_m2m_init(&m2m_ops);
-	if (IS_ERR(fimc->m2m.m2m_dev)) {
-		v4l2_err(v4l2_dev, "failed to initialize v4l2-m2m device\n");
-		ret = PTR_ERR(fimc->m2m.m2m_dev);
-		goto err_init;
-	}
-
-	ret = media_entity_init(&vfd->entity, 0, NULL, 0);
-	if (!ret)
-		return 0;
-
-	v4l2_m2m_release(fimc->m2m.m2m_dev);
-err_init:
-	video_device_release(fimc->m2m.vfd);
-	return ret;
-}
-
-void fimc_unregister_m2m_device(struct fimc_dev *fimc)
-{
-	if (!fimc)
-		return;
-
-	if (fimc->m2m.m2m_dev)
-		v4l2_m2m_release(fimc->m2m.m2m_dev);
-	if (fimc->m2m.vfd) {
-		media_entity_cleanup(&fimc->m2m.vfd->entity);
-		/* Can also be called if video device wasn't registered */
-		video_unregister_device(fimc->m2m.vfd);
-	}
-}
-
 static void fimc_clk_put(struct fimc_dev *fimc)
 {
 	int i;
-	for (i = 0; i < fimc->num_clocks; i++) {
+	for (i = 0; i < MAX_FIMC_CLOCKS; i++) {
 		if (IS_ERR_OR_NULL(fimc->clock[i]))
 			continue;
 		clk_unprepare(fimc->clock[i]);
@@ -1614,7 +807,7 @@ static int fimc_clk_get(struct fimc_dev *fimc)
 {
 	int i, ret;
 
-	for (i = 0; i < fimc->num_clocks; i++) {
+	for (i = 0; i < MAX_FIMC_CLOCKS; i++) {
 		fimc->clock[i] = clk_get(&fimc->pdev->dev, fimc_clocks[i]);
 		if (IS_ERR(fimc->clock[i]))
 			goto err;
@@ -1672,15 +865,12 @@ static int fimc_m2m_resume(struct fimc_dev *fimc)
 
 static int fimc_probe(struct platform_device *pdev)
 {
+	struct fimc_drvdata *drv_data = fimc_get_drvdata(pdev);
+	struct s5p_platform_fimc *pdata;
 	struct fimc_dev *fimc;
 	struct resource *res;
-	struct samsung_fimc_driverdata *drv_data;
-	struct s5p_platform_fimc *pdata;
 	int ret = 0;
 
-	drv_data = (struct samsung_fimc_driverdata *)
-		platform_get_device_id(pdev)->driver_data;
-
 	if (pdev->id >= drv_data->num_entities) {
 		dev_err(&pdev->dev, "Invalid platform device id: %d\n",
 			pdev->id);
@@ -1714,28 +904,29 @@ static int fimc_probe(struct platform_device *pdev)
 		dev_err(&pdev->dev, "Failed to get IRQ resource\n");
 		return -ENXIO;
 	}
-	fimc->irq = res->start;
 
-	fimc->num_clocks = MAX_FIMC_CLOCKS;
 	ret = fimc_clk_get(fimc);
 	if (ret)
 		return ret;
 	clk_set_rate(fimc->clock[CLK_BUS], drv_data->lclk_frequency);
 	clk_enable(fimc->clock[CLK_BUS]);
 
-	platform_set_drvdata(pdev, fimc);
-
-	ret = devm_request_irq(&pdev->dev, fimc->irq, fimc_irq_handler,
-			       0, pdev->name, fimc);
+	ret = devm_request_irq(&pdev->dev, res->start, fimc_irq_handler,
+			       0, dev_name(&pdev->dev), fimc);
 	if (ret) {
 		dev_err(&pdev->dev, "failed to install irq (%d)\n", ret);
 		goto err_clk;
 	}
 
+	ret = fimc_initialize_capture_subdev(fimc);
+	if (ret)
+		goto err_clk;
+
+	platform_set_drvdata(pdev, fimc);
 	pm_runtime_enable(&pdev->dev);
 	ret = pm_runtime_get_sync(&pdev->dev);
 	if (ret < 0)
-		goto err_clk;
+		goto err_sd;
 	/* Initialize contiguous memory allocator */
 	fimc->alloc_ctx = vb2_dma_contig_init_ctx(&pdev->dev);
 	if (IS_ERR(fimc->alloc_ctx)) {
@@ -1747,9 +938,10 @@ static int fimc_probe(struct platform_device *pdev)
 
 	pm_runtime_put(&pdev->dev);
 	return 0;
-
 err_pm:
 	pm_runtime_put(&pdev->dev);
+err_sd:
+	fimc_unregister_capture_subdev(fimc);
 err_clk:
 	fimc_clk_put(fimc);
 	return ret;
@@ -1834,6 +1026,7 @@ static int __devexit fimc_remove(struct platform_device *pdev)
 	pm_runtime_disable(&pdev->dev);
 	pm_runtime_set_suspended(&pdev->dev);
 
+	fimc_unregister_capture_subdev(fimc);
 	vb2_dma_contig_cleanup_ctx(fimc->alloc_ctx);
 
 	clk_disable(fimc->clock[CLK_BUS]);
@@ -1879,7 +1072,7 @@ static struct fimc_pix_limit s5p_pix_limit[4] = {
 	},
 };
 
-static struct samsung_fimc_variant fimc0_variant_s5p = {
+static struct fimc_variant fimc0_variant_s5p = {
 	.has_inp_rot	 = 1,
 	.has_out_rot	 = 1,
 	.has_cam_if	 = 1,
@@ -1891,17 +1084,17 @@ static struct samsung_fimc_variant fimc0_variant_s5p = {
 	.pix_limit	 = &s5p_pix_limit[0],
 };
 
-static struct samsung_fimc_variant fimc2_variant_s5p = {
+static struct fimc_variant fimc2_variant_s5p = {
 	.has_cam_if	 = 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[1],
+	.pix_limit	 = &s5p_pix_limit[1],
 };
 
-static struct samsung_fimc_variant fimc0_variant_s5pv210 = {
+static struct fimc_variant fimc0_variant_s5pv210 = {
 	.pix_hoff	 = 1,
 	.has_inp_rot	 = 1,
 	.has_out_rot	 = 1,
@@ -1914,7 +1107,7 @@ static struct samsung_fimc_variant fimc0_variant_s5pv210 = {
 	.pix_limit	 = &s5p_pix_limit[1],
 };
 
-static struct samsung_fimc_variant fimc1_variant_s5pv210 = {
+static struct fimc_variant fimc1_variant_s5pv210 = {
 	.pix_hoff	 = 1,
 	.has_inp_rot	 = 1,
 	.has_out_rot	 = 1,
@@ -1928,7 +1121,7 @@ static struct samsung_fimc_variant fimc1_variant_s5pv210 = {
 	.pix_limit	 = &s5p_pix_limit[2],
 };
 
-static struct samsung_fimc_variant fimc2_variant_s5pv210 = {
+static struct fimc_variant fimc2_variant_s5pv210 = {
 	.has_cam_if	 = 1,
 	.pix_hoff	 = 1,
 	.min_inp_pixsize = 16,
@@ -1939,7 +1132,7 @@ static struct samsung_fimc_variant fimc2_variant_s5pv210 = {
 	.pix_limit	 = &s5p_pix_limit[2],
 };
 
-static struct samsung_fimc_variant fimc0_variant_exynos4 = {
+static struct fimc_variant fimc0_variant_exynos4 = {
 	.pix_hoff	 = 1,
 	.has_inp_rot	 = 1,
 	.has_out_rot	 = 1,
@@ -1955,7 +1148,7 @@ static struct samsung_fimc_variant fimc0_variant_exynos4 = {
 	.pix_limit	 = &s5p_pix_limit[1],
 };
 
-static struct samsung_fimc_variant fimc3_variant_exynos4 = {
+static struct fimc_variant fimc3_variant_exynos4 = {
 	.pix_hoff	 = 1,
 	.has_cam_if	 = 1,
 	.has_cistatus2	 = 1,
@@ -1970,7 +1163,7 @@ static struct samsung_fimc_variant fimc3_variant_exynos4 = {
 };
 
 /* S5PC100 */
-static struct samsung_fimc_driverdata fimc_drvdata_s5p = {
+static struct fimc_drvdata fimc_drvdata_s5p = {
 	.variant = {
 		[0] = &fimc0_variant_s5p,
 		[1] = &fimc0_variant_s5p,
@@ -1981,7 +1174,7 @@ static struct samsung_fimc_driverdata fimc_drvdata_s5p = {
 };
 
 /* S5PV210, S5PC110 */
-static struct samsung_fimc_driverdata fimc_drvdata_s5pv210 = {
+static struct fimc_drvdata fimc_drvdata_s5pv210 = {
 	.variant = {
 		[0] = &fimc0_variant_s5pv210,
 		[1] = &fimc1_variant_s5pv210,
@@ -1991,8 +1184,8 @@ static struct samsung_fimc_driverdata fimc_drvdata_s5pv210 = {
 	.lclk_frequency = 166000000UL,
 };
 
-/* S5PV310, S5PC210 */
-static struct samsung_fimc_driverdata fimc_drvdata_exynos4 = {
+/* EXYNOS4210, S5PV310, S5PC210 */
+static struct fimc_drvdata fimc_drvdata_exynos4 = {
 	.variant = {
 		[0] = &fimc0_variant_exynos4,
 		[1] = &fimc0_variant_exynos4,
@@ -2036,7 +1229,7 @@ static struct platform_driver fimc_driver = {
 
 int __init fimc_register_driver(void)
 {
-	return platform_driver_probe(&fimc_driver, fimc_probe);
+	return platform_driver_register(&fimc_driver);
 }
 
 void __exit fimc_unregister_driver(void)
diff --git a/drivers/media/video/s5p-fimc/fimc-core.h b/drivers/media/video/s5p-fimc/fimc-core.h
index a18291e648e..95b27ae5cf2 100644
--- a/drivers/media/video/s5p-fimc/fimc-core.h
+++ b/drivers/media/video/s5p-fimc/fimc-core.h
@@ -1,5 +1,5 @@
 /*
- * Copyright (C) 2010 - 2011 Samsung Electronics Co., Ltd.
+ * Copyright (C) 2010 - 2012 Samsung Electronics Co., Ltd.
  *
  * This program is free software; you can redistribute it and/or modify
  * it under the terms of the GNU General Public License version 2 as
@@ -17,6 +17,7 @@
 #include <linux/types.h>
 #include <linux/videodev2.h>
 #include <linux/io.h>
+#include <asm/sizes.h>
 
 #include <media/media-entity.h>
 #include <media/videobuf2-core.h>
@@ -26,8 +27,6 @@
 #include <media/v4l2-mediabus.h>
 #include <media/s5p_fimc.h>
 
-#include "regs-fimc.h"
-
 #define err(fmt, args...) \
 	printk(KERN_ERR "%s:%d: " fmt "\n", __func__, __LINE__, ##args)
 
@@ -78,26 +77,31 @@ enum fimc_dev_flags {
 #define fimc_capture_busy(dev) test_bit(ST_CAPT_BUSY, &(dev)->state)
 
 enum fimc_datapath {
-	FIMC_CAMERA,
-	FIMC_DMA,
-	FIMC_LCDFIFO,
-	FIMC_WRITEBACK
+	FIMC_IO_NONE,
+	FIMC_IO_CAMERA,
+	FIMC_IO_DMA,
+	FIMC_IO_LCDFIFO,
+	FIMC_IO_WRITEBACK,
+	FIMC_IO_ISP,
 };
 
 enum fimc_color_fmt {
-	S5P_FIMC_RGB444 = 0x10,
-	S5P_FIMC_RGB555,
-	S5P_FIMC_RGB565,
-	S5P_FIMC_RGB666,
-	S5P_FIMC_RGB888,
-	S5P_FIMC_RGB30_LOCAL,
-	S5P_FIMC_YCBCR420 = 0x20,
-	S5P_FIMC_YCBYCR422,
-	S5P_FIMC_YCRYCB422,
-	S5P_FIMC_CBYCRY422,
-	S5P_FIMC_CRYCBY422,
-	S5P_FIMC_YCBCR444_LOCAL,
-	S5P_FIMC_JPEG = 0x40,
+	FIMC_FMT_RGB444 = 0x10,
+	FIMC_FMT_RGB555,
+	FIMC_FMT_RGB565,
+	FIMC_FMT_RGB666,
+	FIMC_FMT_RGB888,
+	FIMC_FMT_RGB30_LOCAL,
+	FIMC_FMT_YCBCR420 = 0x20,
+	FIMC_FMT_YCBYCR422,
+	FIMC_FMT_YCRYCB422,
+	FIMC_FMT_CBYCRY422,
+	FIMC_FMT_CRYCBY422,
+	FIMC_FMT_YCBCR444_LOCAL,
+	FIMC_FMT_JPEG = 0x40,
+	FIMC_FMT_RAW8 = 0x80,
+	FIMC_FMT_RAW10,
+	FIMC_FMT_RAW12,
 };
 
 #define fimc_fmt_is_rgb(x) (!!((x) & 0x10))
@@ -106,24 +110,11 @@ enum fimc_color_fmt {
 #define IS_M2M(__strt) ((__strt) == V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE || \
 			__strt == V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE)
 
-/* Cb/Cr chrominance components order for 2 plane Y/CbCr 4:2:2 formats. */
-#define	S5P_FIMC_LSB_CRCB	S5P_CIOCTRL_ORDER422_2P_LSB_CRCB
-
-/* The embedded image effect selection */
-#define	S5P_FIMC_EFFECT_ORIGINAL	S5P_CIIMGEFF_FIN_BYPASS
-#define	S5P_FIMC_EFFECT_ARBITRARY	S5P_CIIMGEFF_FIN_ARBITRARY
-#define	S5P_FIMC_EFFECT_NEGATIVE	S5P_CIIMGEFF_FIN_NEGATIVE
-#define	S5P_FIMC_EFFECT_ARTFREEZE	S5P_CIIMGEFF_FIN_ARTFREEZE
-#define	S5P_FIMC_EFFECT_EMBOSSING	S5P_CIIMGEFF_FIN_EMBOSSING
-#define	S5P_FIMC_EFFECT_SIKHOUETTE	S5P_CIIMGEFF_FIN_SILHOUETTE
-
 /* The hardware context state. */
 #define	FIMC_PARAMS		(1 << 0)
-#define	FIMC_SRC_ADDR		(1 << 1)
-#define	FIMC_DST_ADDR		(1 << 2)
 #define	FIMC_SRC_FMT		(1 << 3)
 #define	FIMC_DST_FMT		(1 << 4)
-#define	FIMC_DST_CROP		(1 << 5)
+#define	FIMC_COMPOSE		(1 << 5)
 #define	FIMC_CTX_M2M		(1 << 16)
 #define	FIMC_CTX_CAP		(1 << 17)
 #define	FIMC_CTX_SHUT		(1 << 18)
@@ -333,7 +324,7 @@ struct fimc_vid_cap {
 	struct fimc_ctx			*ctx;
 	struct vb2_alloc_ctx		*alloc_ctx;
 	struct video_device		*vfd;
-	struct v4l2_subdev		*subdev;
+	struct v4l2_subdev		subdev;
 	struct media_pad		vd_pad;
 	struct v4l2_mbus_framefmt	mf;
 	struct media_pad		sd_pads[FIMC_SD_PADS_NUM];
@@ -370,8 +361,7 @@ struct fimc_pix_limit {
 };
 
 /**
- * struct samsung_fimc_variant - camera interface variant information
- *
+ * 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
@@ -386,7 +376,7 @@ struct fimc_pix_limit {
  * @min_vsize_align: minimum vertical pixel size alignment
  * @out_buf_count: the number of buffers in output DMA sequence
  */
-struct samsung_fimc_variant {
+struct fimc_variant {
 	unsigned int	pix_hoff:1;
 	unsigned int	has_inp_rot:1;
 	unsigned int	has_out_rot:1;
@@ -403,23 +393,19 @@ struct samsung_fimc_variant {
 };
 
 /**
- * struct samsung_fimc_driverdata - per device type driver data for init time.
- *
- * @variant: the variant information for this driver.
- * @dev_cnt: number of fimc sub-devices available in SoC
- * @lclk_frequency: fimc bus clock frequency
+ * struct fimc_drvdata - per device type driver data
+ * @variant: variant information for this device
+ * @num_entities: number of fimc instances available in a SoC
+ * @lclk_frequency: local bus clock frequency
  */
-struct samsung_fimc_driverdata {
-	struct samsung_fimc_variant *variant[FIMC_MAX_DEVS];
-	unsigned long	lclk_frequency;
-	int		num_entities;
+struct fimc_drvdata {
+	struct fimc_variant *variant[FIMC_MAX_DEVS];
+	int num_entities;
+	unsigned long lclk_frequency;
 };
 
-struct fimc_pipeline {
-	struct media_pipeline *pipe;
-	struct v4l2_subdev *sensor;
-	struct v4l2_subdev *csis;
-};
+#define fimc_get_drvdata(_pdev) \
+	((struct fimc_drvdata *) platform_get_device_id(_pdev)->driver_data)
 
 struct fimc_ctx;
 
@@ -431,10 +417,8 @@ struct fimc_ctx;
  * @pdata:	pointer to the device platform data
  * @variant:	the IP variant information
  * @id:		FIMC device index (0..FIMC_MAX_DEVS)
- * @num_clocks: the number of clocks managed by this device instance
  * @clock:	clocks required for FIMC operation
  * @regs:	the mapped hardware registers
- * @irq:	FIMC interrupt number
  * @irq_queue:	interrupt handler waitqueue
  * @v4l2_dev:	root v4l2_device
  * @m2m:	memory-to-memory V4L2 device information
@@ -448,12 +432,10 @@ struct fimc_dev {
 	struct mutex			lock;
 	struct platform_device		*pdev;
 	struct s5p_platform_fimc	*pdata;
-	struct samsung_fimc_variant	*variant;
+	struct fimc_variant		*variant;
 	u16				id;
-	u16				num_clocks;
 	struct clk			*clock[MAX_FIMC_CLOCKS];
 	void __iomem			*regs;
-	int				irq;
 	wait_queue_head_t		irq_queue;
 	struct v4l2_device		*v4l2_dev;
 	struct fimc_m2m_device		m2m;
@@ -464,8 +446,31 @@ struct fimc_dev {
 };
 
 /**
+ * struct fimc_ctrls - v4l2 controls structure
+ * @handler: the control handler
+ * @colorfx: image effect control
+ * @colorfx_cbcr: Cb/Cr coefficients control
+ * @rotate: image rotation control
+ * @hflip: horizontal flip control
+ * @vflip: vertical flip control
+ * @alpha: RGB alpha control
+ * @ready: true if @handler is initialized
+ */
+struct fimc_ctrls {
+	struct v4l2_ctrl_handler handler;
+	struct {
+		struct v4l2_ctrl *colorfx;
+		struct v4l2_ctrl *colorfx_cbcr;
+	};
+	struct v4l2_ctrl *rotate;
+	struct v4l2_ctrl *hflip;
+	struct v4l2_ctrl *vflip;
+	struct v4l2_ctrl *alpha;
+	bool ready;
+};
+
+/**
  * fimc_ctx - the device context data
- * @slock:		spinlock protecting this data structure
  * @s_frame:		source frame properties
  * @d_frame:		destination frame properties
  * @out_order_1p:	output 1-plane YCBCR order
@@ -484,15 +489,9 @@ struct fimc_dev {
  * @fimc_dev:		the FIMC device this context applies to
  * @m2m_ctx:		memory-to-memory device context
  * @fh:			v4l2 file handle
- * @ctrl_handler:	v4l2 controls handler
- * @ctrl_rotate		image rotation control
- * @ctrl_hflip		horizontal flip control
- * @ctrl_vflip		vertical flip control
- * @ctrl_alpha		RGB alpha control
- * @ctrls_rdy:		true if the control handler is initialized
+ * @ctrls:		v4l2 controls structure
  */
 struct fimc_ctx {
-	spinlock_t		slock;
 	struct fimc_frame	s_frame;
 	struct fimc_frame	d_frame;
 	u32			out_order_1p;
@@ -511,12 +510,7 @@ struct fimc_ctx {
 	struct fimc_dev		*fimc_dev;
 	struct v4l2_m2m_ctx	*m2m_ctx;
 	struct v4l2_fh		fh;
-	struct v4l2_ctrl_handler ctrl_handler;
-	struct v4l2_ctrl	*ctrl_rotate;
-	struct v4l2_ctrl	*ctrl_hflip;
-	struct v4l2_ctrl	*ctrl_vflip;
-	struct v4l2_ctrl	*ctrl_alpha;
-	bool			ctrls_rdy;
+	struct fimc_ctrls	ctrls;
 };
 
 #define fh_to_ctx(__fh) container_of(__fh, struct fimc_ctx, fh)
@@ -560,13 +554,13 @@ static inline bool fimc_capture_active(struct fimc_dev *fimc)
 	return ret;
 }
 
-static inline void fimc_ctx_state_lock_set(u32 state, struct fimc_ctx *ctx)
+static inline void fimc_ctx_state_set(u32 state, struct fimc_ctx *ctx)
 {
 	unsigned long flags;
 
-	spin_lock_irqsave(&ctx->slock, flags);
+	spin_lock_irqsave(&ctx->fimc_dev->slock, flags);
 	ctx->state |= state;
-	spin_unlock_irqrestore(&ctx->slock, flags);
+	spin_unlock_irqrestore(&ctx->fimc_dev->slock, flags);
 }
 
 static inline bool fimc_ctx_state_is_set(u32 mask, struct fimc_ctx *ctx)
@@ -574,9 +568,9 @@ static inline bool fimc_ctx_state_is_set(u32 mask, struct fimc_ctx *ctx)
 	unsigned long flags;
 	bool ret;
 
-	spin_lock_irqsave(&ctx->slock, flags);
+	spin_lock_irqsave(&ctx->fimc_dev->slock, flags);
 	ret = (ctx->state & mask) == mask;
-	spin_unlock_irqrestore(&ctx->slock, flags);
+	spin_unlock_irqrestore(&ctx->fimc_dev->slock, flags);
 	return ret;
 }
 
@@ -589,61 +583,13 @@ static inline int tiled_fmt(struct fimc_fmt *fmt)
 static inline int fimc_get_alpha_mask(struct fimc_fmt *fmt)
 {
 	switch (fmt->color) {
-	case S5P_FIMC_RGB444:	return 0x0f;
-	case S5P_FIMC_RGB555:	return 0x01;
-	case S5P_FIMC_RGB888:	return 0xff;
+	case FIMC_FMT_RGB444:	return 0x0f;
+	case FIMC_FMT_RGB555:	return 0x01;
+	case FIMC_FMT_RGB888:	return 0xff;
 	default:		return 0;
 	};
 }
 
-static inline void fimc_hw_clear_irq(struct fimc_dev *dev)
-{
-	u32 cfg = readl(dev->regs + S5P_CIGCTRL);
-	cfg |= S5P_CIGCTRL_IRQ_CLR;
-	writel(cfg, dev->regs + S5P_CIGCTRL);
-}
-
-static inline void fimc_hw_enable_scaler(struct fimc_dev *dev, bool on)
-{
-	u32 cfg = readl(dev->regs + S5P_CISCCTRL);
-	if (on)
-		cfg |= S5P_CISCCTRL_SCALERSTART;
-	else
-		cfg &= ~S5P_CISCCTRL_SCALERSTART;
-	writel(cfg, dev->regs + S5P_CISCCTRL);
-}
-
-static inline void fimc_hw_activate_input_dma(struct fimc_dev *dev, bool on)
-{
-	u32 cfg = readl(dev->regs + S5P_MSCTRL);
-	if (on)
-		cfg |= S5P_MSCTRL_ENVID;
-	else
-		cfg &= ~S5P_MSCTRL_ENVID;
-	writel(cfg, dev->regs + S5P_MSCTRL);
-}
-
-static inline void fimc_hw_dis_capture(struct fimc_dev *dev)
-{
-	u32 cfg = readl(dev->regs + S5P_CIIMGCPT);
-	cfg &= ~(S5P_CIIMGCPT_IMGCPTEN | S5P_CIIMGCPT_IMGCPTEN_SC);
-	writel(cfg, dev->regs + S5P_CIIMGCPT);
-}
-
-/**
- * fimc_hw_set_dma_seq - configure output DMA buffer sequence
- * @mask: each bit corresponds to one of 32 output buffer registers set
- *	  1 to include buffer in the sequence, 0 to disable
- *
- * This function mask output DMA ring buffers, i.e. it allows to configure
- * which of the output buffer address registers will be used by the DMA
- * engine.
- */
-static inline void fimc_hw_set_dma_seq(struct fimc_dev *dev, u32 mask)
-{
-	writel(mask, dev->regs + S5P_CIFCNTSEQ);
-}
-
 static inline struct fimc_frame *ctx_get_frame(struct fimc_ctx *ctx,
 					       enum v4l2_buf_type type)
 {
@@ -665,48 +611,6 @@ static inline struct fimc_frame *ctx_get_frame(struct fimc_ctx *ctx,
 	return frame;
 }
 
-/* Return an index to the buffer actually being written. */
-static inline u32 fimc_hw_get_frame_index(struct fimc_dev *dev)
-{
-	u32 reg;
-
-	if (dev->variant->has_cistatus2) {
-		reg = readl(dev->regs + S5P_CISTATUS2) & 0x3F;
-		return reg > 0 ? --reg : reg;
-	} else {
-		reg = readl(dev->regs + S5P_CISTATUS);
-		return (reg & S5P_CISTATUS_FRAMECNT_MASK) >>
-			S5P_CISTATUS_FRAMECNT_SHIFT;
-	}
-}
-
-/* -----------------------------------------------------*/
-/* fimc-reg.c						*/
-void fimc_hw_reset(struct fimc_dev *fimc);
-void fimc_hw_set_rotation(struct fimc_ctx *ctx);
-void fimc_hw_set_target_format(struct fimc_ctx *ctx);
-void fimc_hw_set_out_dma(struct fimc_ctx *ctx);
-void fimc_hw_en_lastirq(struct fimc_dev *fimc, int enable);
-void fimc_hw_en_irq(struct fimc_dev *fimc, int enable);
-void fimc_hw_set_prescaler(struct fimc_ctx *ctx);
-void fimc_hw_set_mainscaler(struct fimc_ctx *ctx);
-void fimc_hw_en_capture(struct fimc_ctx *ctx);
-void fimc_hw_set_effect(struct fimc_ctx *ctx, bool active);
-void fimc_hw_set_rgb_alpha(struct fimc_ctx *ctx);
-void fimc_hw_set_in_dma(struct fimc_ctx *ctx);
-void fimc_hw_set_input_path(struct fimc_ctx *ctx);
-void fimc_hw_set_output_path(struct fimc_ctx *ctx);
-void fimc_hw_set_input_addr(struct fimc_dev *fimc, struct fimc_addr *paddr);
-void fimc_hw_set_output_addr(struct fimc_dev *fimc, struct fimc_addr *paddr,
-			     int index);
-int fimc_hw_set_camera_source(struct fimc_dev *fimc,
-			      struct s5p_fimc_isp_info *cam);
-int fimc_hw_set_camera_offset(struct fimc_dev *fimc, struct fimc_frame *f);
-int fimc_hw_set_camera_polarity(struct fimc_dev *fimc,
-				struct s5p_fimc_isp_info *cam);
-int fimc_hw_set_camera_type(struct fimc_dev *fimc,
-			    struct s5p_fimc_isp_info *cam);
-
 /* -----------------------------------------------------*/
 /* fimc-core.c */
 int fimc_vidioc_enum_fmt_mplane(struct file *file, void *priv,
@@ -718,8 +622,9 @@ void fimc_alpha_ctrl_update(struct fimc_ctx *ctx);
 int fimc_fill_format(struct fimc_frame *frame, struct v4l2_format *f);
 void fimc_adjust_mplane_format(struct fimc_fmt *fmt, u32 width, u32 height,
 			       struct v4l2_pix_format_mplane *pix);
-struct fimc_fmt *fimc_find_format(u32 *pixelformat, u32 *mbus_code,
+struct fimc_fmt *fimc_find_format(const u32 *pixelformat, const u32 *mbus_code,
 				  unsigned int mask, int index);
+struct fimc_fmt *fimc_get_format(unsigned int index);
 
 int fimc_check_scaler_ratio(struct fimc_ctx *ctx, int sw, int sh,
 			    int dw, int dh, int rotation);
@@ -730,7 +635,7 @@ int fimc_prepare_addr(struct fimc_ctx *ctx, struct vb2_buffer *vb,
 void fimc_prepare_dma_offset(struct fimc_ctx *ctx, struct fimc_frame *f);
 void fimc_set_yuv_order(struct fimc_ctx *ctx);
 void fimc_fill_frame(struct fimc_frame *frame, struct v4l2_format *f);
-void fimc_capture_irq_handler(struct fimc_dev *fimc, bool done);
+void fimc_capture_irq_handler(struct fimc_dev *fimc, int deq_buf);
 
 int fimc_register_m2m_device(struct fimc_dev *fimc,
 			     struct v4l2_device *v4l2_dev);
@@ -739,33 +644,18 @@ int fimc_register_driver(void);
 void fimc_unregister_driver(void);
 
 /* -----------------------------------------------------*/
+/* fimc-m2m.c */
+void fimc_m2m_job_finish(struct fimc_ctx *ctx, int vb_state);
+
+/* -----------------------------------------------------*/
 /* fimc-capture.c					*/
-int fimc_register_capture_device(struct fimc_dev *fimc,
-				 struct v4l2_device *v4l2_dev);
-void fimc_unregister_capture_device(struct fimc_dev *fimc);
+int fimc_initialize_capture_subdev(struct fimc_dev *fimc);
+void fimc_unregister_capture_subdev(struct fimc_dev *fimc);
 int fimc_capture_ctrls_create(struct fimc_dev *fimc);
-int fimc_vid_cap_buf_queue(struct fimc_dev *fimc,
-			     struct fimc_vid_buffer *fimc_vb);
 void fimc_sensor_notify(struct v4l2_subdev *sd, unsigned int notification,
 			void *arg);
 int fimc_capture_suspend(struct fimc_dev *fimc);
 int fimc_capture_resume(struct fimc_dev *fimc);
-int fimc_capture_config_update(struct fimc_ctx *ctx);
-
-/* Locking: the caller holds fimc->slock */
-static inline void fimc_activate_capture(struct fimc_ctx *ctx)
-{
-	fimc_hw_enable_scaler(ctx->fimc_dev, ctx->scaler.enabled);
-	fimc_hw_en_capture(ctx);
-}
-
-static inline void fimc_deactivate_capture(struct fimc_dev *fimc)
-{
-	fimc_hw_en_lastirq(fimc, true);
-	fimc_hw_dis_capture(fimc);
-	fimc_hw_enable_scaler(fimc, false);
-	fimc_hw_en_lastirq(fimc, false);
-}
 
 /*
  * Buffer list manipulation functions. Must be called with fimc.slock held.
diff --git a/drivers/media/video/s5p-fimc/fimc-lite-reg.c b/drivers/media/video/s5p-fimc/fimc-lite-reg.c
new file mode 100644
index 00000000000..419adfb7cdf
--- /dev/null
+++ b/drivers/media/video/s5p-fimc/fimc-lite-reg.c
@@ -0,0 +1,300 @@
+/*
+ * Register interface file for EXYNOS FIMC-LITE (camera interface) driver
+ *
+ * Copyright (C) 2012 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 <media/s5p_fimc.h>
+
+#include "fimc-lite-reg.h"
+#include "fimc-lite.h"
+#include "fimc-core.h"
+
+#define FLITE_RESET_TIMEOUT 50 /* in ms */
+
+void flite_hw_reset(struct fimc_lite *dev)
+{
+	unsigned long end = jiffies + msecs_to_jiffies(FLITE_RESET_TIMEOUT);
+	u32 cfg;
+
+	cfg = readl(dev->regs + FLITE_REG_CIGCTRL);
+	cfg |= FLITE_REG_CIGCTRL_SWRST_REQ;
+	writel(cfg, dev->regs + FLITE_REG_CIGCTRL);
+
+	while (time_is_after_jiffies(end)) {
+		cfg = readl(dev->regs + FLITE_REG_CIGCTRL);
+		if (cfg & FLITE_REG_CIGCTRL_SWRST_RDY)
+			break;
+		usleep_range(1000, 5000);
+	}
+
+	cfg |= FLITE_REG_CIGCTRL_SWRST;
+	writel(cfg, dev->regs + FLITE_REG_CIGCTRL);
+}
+
+void flite_hw_clear_pending_irq(struct fimc_lite *dev)
+{
+	u32 cfg = readl(dev->regs + FLITE_REG_CISTATUS);
+	cfg &= ~FLITE_REG_CISTATUS_IRQ_CAM;
+	writel(cfg, dev->regs + FLITE_REG_CISTATUS);
+}
+
+u32 flite_hw_get_interrupt_source(struct fimc_lite *dev)
+{
+	u32 intsrc = readl(dev->regs + FLITE_REG_CISTATUS);
+	return intsrc & FLITE_REG_CISTATUS_IRQ_MASK;
+}
+
+void flite_hw_clear_last_capture_end(struct fimc_lite *dev)
+{
+
+	u32 cfg = readl(dev->regs + FLITE_REG_CISTATUS2);
+	cfg &= ~FLITE_REG_CISTATUS2_LASTCAPEND;
+	writel(cfg, dev->regs + FLITE_REG_CISTATUS2);
+}
+
+void flite_hw_set_interrupt_mask(struct fimc_lite *dev)
+{
+	u32 cfg, intsrc;
+
+	/* Select interrupts to be enabled for each output mode */
+	if (dev->out_path == FIMC_IO_DMA) {
+		intsrc = FLITE_REG_CIGCTRL_IRQ_OVFEN |
+			 FLITE_REG_CIGCTRL_IRQ_LASTEN |
+			 FLITE_REG_CIGCTRL_IRQ_STARTEN;
+	} else {
+		/* An output to the FIMC-IS */
+		intsrc = FLITE_REG_CIGCTRL_IRQ_OVFEN |
+			 FLITE_REG_CIGCTRL_IRQ_LASTEN;
+	}
+
+	cfg = readl(dev->regs + FLITE_REG_CIGCTRL);
+	cfg |= FLITE_REG_CIGCTRL_IRQ_DISABLE_MASK;
+	cfg &= ~intsrc;
+	writel(cfg, dev->regs + FLITE_REG_CIGCTRL);
+}
+
+void flite_hw_capture_start(struct fimc_lite *dev)
+{
+	u32 cfg = readl(dev->regs + FLITE_REG_CIIMGCPT);
+	cfg |= FLITE_REG_CIIMGCPT_IMGCPTEN;
+	writel(cfg, dev->regs + FLITE_REG_CIIMGCPT);
+}
+
+void flite_hw_capture_stop(struct fimc_lite *dev)
+{
+	u32 cfg = readl(dev->regs + FLITE_REG_CIIMGCPT);
+	cfg &= ~FLITE_REG_CIIMGCPT_IMGCPTEN;
+	writel(cfg, dev->regs + FLITE_REG_CIIMGCPT);
+}
+
+/*
+ * Test pattern (color bars) enable/disable. External sensor
+ * pixel clock must be active for the test pattern to work.
+ */
+void flite_hw_set_test_pattern(struct fimc_lite *dev, bool on)
+{
+	u32 cfg = readl(dev->regs + FLITE_REG_CIGCTRL);
+	if (on)
+		cfg |= FLITE_REG_CIGCTRL_TEST_PATTERN_COLORBAR;
+	else
+		cfg &= ~FLITE_REG_CIGCTRL_TEST_PATTERN_COLORBAR;
+	writel(cfg, dev->regs + FLITE_REG_CIGCTRL);
+}
+
+static const u32 src_pixfmt_map[8][3] = {
+	{ V4L2_MBUS_FMT_YUYV8_2X8, FLITE_REG_CISRCSIZE_ORDER422_IN_YCBYCR,
+	  FLITE_REG_CIGCTRL_YUV422_1P },
+	{ V4L2_MBUS_FMT_YVYU8_2X8, FLITE_REG_CISRCSIZE_ORDER422_IN_YCRYCB,
+	  FLITE_REG_CIGCTRL_YUV422_1P },
+	{ V4L2_MBUS_FMT_UYVY8_2X8, FLITE_REG_CISRCSIZE_ORDER422_IN_CBYCRY,
+	  FLITE_REG_CIGCTRL_YUV422_1P },
+	{ V4L2_MBUS_FMT_VYUY8_2X8, FLITE_REG_CISRCSIZE_ORDER422_IN_CRYCBY,
+	  FLITE_REG_CIGCTRL_YUV422_1P },
+	{ V4L2_PIX_FMT_SGRBG8, 0, FLITE_REG_CIGCTRL_RAW8 },
+	{ V4L2_PIX_FMT_SGRBG10, 0, FLITE_REG_CIGCTRL_RAW10 },
+	{ V4L2_PIX_FMT_SGRBG12, 0, FLITE_REG_CIGCTRL_RAW12 },
+	{ V4L2_MBUS_FMT_JPEG_1X8, 0, FLITE_REG_CIGCTRL_USER(1) },
+};
+
+/* 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;
+	unsigned int i = ARRAY_SIZE(src_pixfmt_map);
+	u32 cfg;
+
+	while (i-- >= 0) {
+		if (src_pixfmt_map[i][0] == pixelcode)
+			break;
+	}
+
+	if (i == 0 && src_pixfmt_map[i][0] != pixelcode) {
+		v4l2_err(dev->vfd,
+			 "Unsupported pixel code, falling back to %#08x\n",
+			 src_pixfmt_map[i][0]);
+	}
+
+	cfg = readl(dev->regs + FLITE_REG_CIGCTRL);
+	cfg &= ~FLITE_REG_CIGCTRL_FMT_MASK;
+	cfg |= src_pixfmt_map[i][2];
+	writel(cfg, dev->regs + FLITE_REG_CIGCTRL);
+
+	cfg = readl(dev->regs + FLITE_REG_CISRCSIZE);
+	cfg &= ~(FLITE_REG_CISRCSIZE_ORDER422_MASK |
+		 FLITE_REG_CISRCSIZE_SIZE_CAM_MASK);
+	cfg |= (f->f_width << 16) | f->f_height;
+	cfg |= src_pixfmt_map[i][1];
+	writel(cfg, dev->regs + FLITE_REG_CISRCSIZE);
+}
+
+/* Set the camera host input window offsets (cropping) */
+void flite_hw_set_window_offset(struct fimc_lite *dev, struct flite_frame *f)
+{
+	u32 hoff2, voff2;
+	u32 cfg;
+
+	cfg = readl(dev->regs + FLITE_REG_CIWDOFST);
+	cfg &= ~FLITE_REG_CIWDOFST_OFST_MASK;
+	cfg |= (f->rect.left << 16) | f->rect.top;
+	cfg |= FLITE_REG_CIWDOFST_WINOFSEN;
+	writel(cfg, dev->regs + FLITE_REG_CIWDOFST);
+
+	hoff2 = f->f_width - f->rect.width - f->rect.left;
+	voff2 = f->f_height - f->rect.height - f->rect.top;
+
+	cfg = (hoff2 << 16) | voff2;
+	writel(cfg, dev->regs + FLITE_REG_CIWDOFST2);
+}
+
+/* Select camera port (A, B) */
+static void flite_hw_set_camera_port(struct fimc_lite *dev, int id)
+{
+	u32 cfg = readl(dev->regs + FLITE_REG_CIGENERAL);
+	if (id == 0)
+		cfg &= ~FLITE_REG_CIGENERAL_CAM_B;
+	else
+		cfg |= FLITE_REG_CIGENERAL_CAM_B;
+	writel(cfg, dev->regs + FLITE_REG_CIGENERAL);
+}
+
+/* Select serial or parallel bus, camera port (A,B) and set signals polarity */
+void flite_hw_set_camera_bus(struct fimc_lite *dev,
+			     struct s5p_fimc_isp_info *s_info)
+{
+	u32 cfg = readl(dev->regs + FLITE_REG_CIGCTRL);
+	unsigned int flags = s_info->flags;
+
+	if (s_info->bus_type != FIMC_MIPI_CSI2) {
+		cfg &= ~(FLITE_REG_CIGCTRL_SELCAM_MIPI |
+			 FLITE_REG_CIGCTRL_INVPOLPCLK |
+			 FLITE_REG_CIGCTRL_INVPOLVSYNC |
+			 FLITE_REG_CIGCTRL_INVPOLHREF);
+
+		if (flags & V4L2_MBUS_PCLK_SAMPLE_FALLING)
+			cfg |= FLITE_REG_CIGCTRL_INVPOLPCLK;
+
+		if (flags & V4L2_MBUS_VSYNC_ACTIVE_LOW)
+			cfg |= FLITE_REG_CIGCTRL_INVPOLVSYNC;
+
+		if (flags & V4L2_MBUS_HSYNC_ACTIVE_LOW)
+			cfg |= FLITE_REG_CIGCTRL_INVPOLHREF;
+	} else {
+		cfg |= FLITE_REG_CIGCTRL_SELCAM_MIPI;
+	}
+
+	writel(cfg, dev->regs + FLITE_REG_CIGCTRL);
+
+	flite_hw_set_camera_port(dev, s_info->mux_id);
+}
+
+void flite_hw_set_out_order(struct fimc_lite *dev, struct flite_frame *f)
+{
+	static const u32 pixcode[4][2] = {
+		{ V4L2_MBUS_FMT_YUYV8_2X8, FLITE_REG_CIODMAFMT_YCBYCR },
+		{ V4L2_MBUS_FMT_YVYU8_2X8, FLITE_REG_CIODMAFMT_YCRYCB },
+		{ V4L2_MBUS_FMT_UYVY8_2X8, FLITE_REG_CIODMAFMT_CBYCRY },
+		{ V4L2_MBUS_FMT_VYUY8_2X8, FLITE_REG_CIODMAFMT_CRYCBY },
+	};
+	u32 cfg = readl(dev->regs + FLITE_REG_CIODMAFMT);
+	unsigned int i = ARRAY_SIZE(pixcode);
+
+	while (i-- >= 0)
+		if (pixcode[i][0] == dev->fmt->mbus_code)
+			break;
+	cfg &= ~FLITE_REG_CIODMAFMT_YCBCR_ORDER_MASK;
+	writel(cfg | pixcode[i][1], dev->regs + FLITE_REG_CIODMAFMT);
+}
+
+void flite_hw_set_dma_window(struct fimc_lite *dev, struct flite_frame *f)
+{
+	u32 cfg;
+
+	/* Maximum output pixel size */
+	cfg = readl(dev->regs + FLITE_REG_CIOCAN);
+	cfg &= ~FLITE_REG_CIOCAN_MASK;
+	cfg = (f->f_height << 16) | f->f_width;
+	writel(cfg, dev->regs + FLITE_REG_CIOCAN);
+
+	/* DMA offsets */
+	cfg = readl(dev->regs + FLITE_REG_CIOOFF);
+	cfg &= ~FLITE_REG_CIOOFF_MASK;
+	cfg |= (f->rect.top << 16) | f->rect.left;
+	writel(cfg, dev->regs + FLITE_REG_CIOOFF);
+}
+
+/* Enable/disable output DMA, set output pixel size and offsets (composition) */
+void flite_hw_set_output_dma(struct fimc_lite *dev, struct flite_frame *f,
+			     bool enable)
+{
+	u32 cfg = readl(dev->regs + FLITE_REG_CIGCTRL);
+
+	if (!enable) {
+		cfg |= FLITE_REG_CIGCTRL_ODMA_DISABLE;
+		writel(cfg, dev->regs + FLITE_REG_CIGCTRL);
+		return;
+	}
+
+	cfg &= ~FLITE_REG_CIGCTRL_ODMA_DISABLE;
+	writel(cfg, dev->regs + FLITE_REG_CIGCTRL);
+
+	flite_hw_set_out_order(dev, f);
+	flite_hw_set_dma_window(dev, f);
+}
+
+void flite_hw_dump_regs(struct fimc_lite *dev, const char *label)
+{
+	struct {
+		u32 offset;
+		const char * const name;
+	} registers[] = {
+		{ 0x00, "CISRCSIZE" },
+		{ 0x04, "CIGCTRL" },
+		{ 0x08, "CIIMGCPT" },
+		{ 0x0c, "CICPTSEQ" },
+		{ 0x10, "CIWDOFST" },
+		{ 0x14, "CIWDOFST2" },
+		{ 0x18, "CIODMAFMT" },
+		{ 0x20, "CIOCAN" },
+		{ 0x24, "CIOOFF" },
+		{ 0x30, "CIOSA" },
+		{ 0x40, "CISTATUS" },
+		{ 0x44, "CISTATUS2" },
+		{ 0xf0, "CITHOLD" },
+		{ 0xfc, "CIGENERAL" },
+	};
+	u32 i;
+
+	pr_info("--- %s ---\n", label);
+	for (i = 0; i < ARRAY_SIZE(registers); i++) {
+		u32 cfg = readl(dev->regs + registers[i].offset);
+		pr_info("%s: %s:\t0x%08x\n", __func__, registers[i].name, cfg);
+	}
+}
diff --git a/drivers/media/video/s5p-fimc/fimc-lite-reg.h b/drivers/media/video/s5p-fimc/fimc-lite-reg.h
new file mode 100644
index 00000000000..adb9e9e6f3c
--- /dev/null
+++ b/drivers/media/video/s5p-fimc/fimc-lite-reg.h
@@ -0,0 +1,150 @@
+/*
+ * Copyright (C) 2012 Samsung Electronics Co., Ltd.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ */
+
+#ifndef FIMC_LITE_REG_H_
+#define FIMC_LITE_REG_H_
+
+#include "fimc-lite.h"
+
+/* Camera Source size */
+#define FLITE_REG_CISRCSIZE			0x00
+#define FLITE_REG_CISRCSIZE_ORDER422_IN_YCBYCR	(0 << 14)
+#define FLITE_REG_CISRCSIZE_ORDER422_IN_YCRYCB	(1 << 14)
+#define FLITE_REG_CISRCSIZE_ORDER422_IN_CBYCRY	(2 << 14)
+#define FLITE_REG_CISRCSIZE_ORDER422_IN_CRYCBY	(3 << 14)
+#define FLITE_REG_CISRCSIZE_ORDER422_MASK	(0x3 << 14)
+#define FLITE_REG_CISRCSIZE_SIZE_CAM_MASK	(0x3fff << 16 | 0x3fff)
+
+/* Global control */
+#define FLITE_REG_CIGCTRL			0x04
+#define FLITE_REG_CIGCTRL_YUV422_1P		(0x1e << 24)
+#define FLITE_REG_CIGCTRL_RAW8			(0x2a << 24)
+#define FLITE_REG_CIGCTRL_RAW10			(0x2b << 24)
+#define FLITE_REG_CIGCTRL_RAW12			(0x2c << 24)
+#define FLITE_REG_CIGCTRL_RAW14			(0x2d << 24)
+/* User defined formats. x = 0...15 */
+#define FLITE_REG_CIGCTRL_USER(x)		((0x30 + x - 1) << 24)
+#define FLITE_REG_CIGCTRL_FMT_MASK		(0x3f << 24)
+#define FLITE_REG_CIGCTRL_SHADOWMASK_DISABLE	(1 << 21)
+#define FLITE_REG_CIGCTRL_ODMA_DISABLE		(1 << 20)
+#define FLITE_REG_CIGCTRL_SWRST_REQ		(1 << 19)
+#define FLITE_REG_CIGCTRL_SWRST_RDY		(1 << 18)
+#define FLITE_REG_CIGCTRL_SWRST			(1 << 17)
+#define FLITE_REG_CIGCTRL_TEST_PATTERN_COLORBAR	(1 << 15)
+#define FLITE_REG_CIGCTRL_INVPOLPCLK		(1 << 14)
+#define FLITE_REG_CIGCTRL_INVPOLVSYNC		(1 << 13)
+#define FLITE_REG_CIGCTRL_INVPOLHREF		(1 << 12)
+/* Interrupts mask bits (1 disables an interrupt) */
+#define FLITE_REG_CIGCTRL_IRQ_LASTEN		(1 << 8)
+#define FLITE_REG_CIGCTRL_IRQ_ENDEN		(1 << 7)
+#define FLITE_REG_CIGCTRL_IRQ_STARTEN		(1 << 6)
+#define FLITE_REG_CIGCTRL_IRQ_OVFEN		(1 << 5)
+#define FLITE_REG_CIGCTRL_IRQ_DISABLE_MASK	(0xf << 5)
+#define FLITE_REG_CIGCTRL_SELCAM_MIPI		(1 << 3)
+
+/* Image Capture Enable */
+#define FLITE_REG_CIIMGCPT			0x08
+#define FLITE_REG_CIIMGCPT_IMGCPTEN		(1 << 31)
+#define FLITE_REG_CIIMGCPT_CPT_FREN		(1 << 25)
+#define FLITE_REG_CIIMGCPT_CPT_MOD_FRCNT	(1 << 18)
+#define FLITE_REG_CIIMGCPT_CPT_MOD_FREN		(0 << 18)
+
+/* Capture Sequence */
+#define FLITE_REG_CICPTSEQ			0x0c
+
+/* Camera Window Offset */
+#define FLITE_REG_CIWDOFST			0x10
+#define FLITE_REG_CIWDOFST_WINOFSEN		(1 << 31)
+#define FLITE_REG_CIWDOFST_CLROVIY		(1 << 31)
+#define FLITE_REG_CIWDOFST_CLROVFICB		(1 << 15)
+#define FLITE_REG_CIWDOFST_CLROVFICR		(1 << 14)
+#define FLITE_REG_CIWDOFST_OFST_MASK		((0x1fff << 16) | 0x1fff)
+
+/* Camera Window Offset2 */
+#define FLITE_REG_CIWDOFST2			0x14
+
+/* Camera Output DMA Format */
+#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_YCBCR_ORDER_MASK	(0x3 << 4)
+
+/* Camera Output Canvas */
+#define FLITE_REG_CIOCAN			0x20
+#define FLITE_REG_CIOCAN_MASK			((0x3fff << 16) | 0x3fff)
+
+/* Camera Output DMA Offset */
+#define FLITE_REG_CIOOFF			0x24
+#define FLITE_REG_CIOOFF_MASK			((0x3fff << 16) | 0x3fff)
+
+/* Camera Output DMA Start Address */
+#define FLITE_REG_CIOSA				0x30
+
+/* Camera Status */
+#define FLITE_REG_CISTATUS			0x40
+#define FLITE_REG_CISTATUS_MIPI_VVALID		(1 << 22)
+#define FLITE_REG_CISTATUS_MIPI_HVALID		(1 << 21)
+#define FLITE_REG_CISTATUS_MIPI_DVALID		(1 << 20)
+#define FLITE_REG_CISTATUS_ITU_VSYNC		(1 << 14)
+#define FLITE_REG_CISTATUS_ITU_HREFF		(1 << 13)
+#define FLITE_REG_CISTATUS_OVFIY		(1 << 10)
+#define FLITE_REG_CISTATUS_OVFICB		(1 << 9)
+#define FLITE_REG_CISTATUS_OVFICR		(1 << 8)
+#define FLITE_REG_CISTATUS_IRQ_SRC_OVERFLOW	(1 << 7)
+#define FLITE_REG_CISTATUS_IRQ_SRC_LASTCAPEND	(1 << 6)
+#define FLITE_REG_CISTATUS_IRQ_SRC_FRMSTART	(1 << 5)
+#define FLITE_REG_CISTATUS_IRQ_SRC_FRMEND	(1 << 4)
+#define FLITE_REG_CISTATUS_IRQ_CAM		(1 << 0)
+#define FLITE_REG_CISTATUS_IRQ_MASK		(0xf << 4)
+
+/* Camera Status2 */
+#define FLITE_REG_CISTATUS2			0x44
+#define FLITE_REG_CISTATUS2_LASTCAPEND		(1 << 1)
+#define FLITE_REG_CISTATUS2_FRMEND		(1 << 0)
+
+/* Qos Threshold */
+#define FLITE_REG_CITHOLD			0xf0
+#define FLITE_REG_CITHOLD_W_QOS_EN		(1 << 30)
+
+/* Camera General Purpose */
+#define FLITE_REG_CIGENERAL			0xfc
+/* b0: 1 - camera B, 0 - camera A */
+#define FLITE_REG_CIGENERAL_CAM_B		(1 << 0)
+
+/* ----------------------------------------------------------------------------
+ * Function declarations
+ */
+void flite_hw_reset(struct fimc_lite *dev);
+void flite_hw_clear_pending_irq(struct fimc_lite *dev);
+u32 flite_hw_get_interrupt_source(struct fimc_lite *dev);
+void flite_hw_clear_last_capture_end(struct fimc_lite *dev);
+void flite_hw_set_interrupt_mask(struct fimc_lite *dev);
+void flite_hw_capture_start(struct fimc_lite *dev);
+void flite_hw_capture_stop(struct fimc_lite *dev);
+void flite_hw_set_camera_bus(struct fimc_lite *dev,
+			     struct s5p_fimc_isp_info *s_info);
+void flite_hw_set_camera_polarity(struct fimc_lite *dev,
+				  struct s5p_fimc_isp_info *cam);
+void flite_hw_set_window_offset(struct fimc_lite *dev, struct flite_frame *f);
+void flite_hw_set_source_format(struct fimc_lite *dev, struct flite_frame *f);
+
+void flite_hw_set_output_dma(struct fimc_lite *dev, struct flite_frame *f,
+			     bool enable);
+void flite_hw_set_dma_window(struct fimc_lite *dev, struct flite_frame *f);
+void flite_hw_set_test_pattern(struct fimc_lite *dev, bool on);
+void flite_hw_dump_regs(struct fimc_lite *dev, const char *label);
+
+static inline void flite_hw_set_output_addr(struct fimc_lite *dev, u32 paddr)
+{
+	writel(paddr, dev->regs + FLITE_REG_CIOSA);
+}
+#endif /* FIMC_LITE_REG_H */
diff --git a/drivers/media/video/s5p-fimc/fimc-lite.c b/drivers/media/video/s5p-fimc/fimc-lite.c
new file mode 100644
index 00000000000..400d701aef0
--- /dev/null
+++ b/drivers/media/video/s5p-fimc/fimc-lite.c
@@ -0,0 +1,1576 @@
+/*
+ * Samsung EXYNOS FIMC-LITE (camera host interface) driver
+*
+ * Copyright (C) 2012 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 pr_fmt(fmt) "%s:%d " fmt, __func__, __LINE__
+
+#include <linux/bug.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/types.h>
+#include <linux/platform_device.h>
+#include <linux/pm_runtime.h>
+#include <linux/slab.h>
+#include <linux/videodev2.h>
+
+#include <media/v4l2-device.h>
+#include <media/v4l2-ioctl.h>
+#include <media/v4l2-mem2mem.h>
+#include <media/videobuf2-core.h>
+#include <media/videobuf2-dma-contig.h>
+
+#include "fimc-mdevice.h"
+#include "fimc-core.h"
+#include "fimc-lite-reg.h"
+
+static int debug;
+module_param(debug, int, 0644);
+
+static const struct fimc_fmt fimc_lite_formats[] = {
+	{
+		.name		= "YUV 4:2:2 packed, YCbYCr",
+		.fourcc		= V4L2_PIX_FMT_YUYV,
+		.depth		= { 16 },
+		.color		= FIMC_FMT_YCBYCR422,
+		.memplanes	= 1,
+		.mbus_code	= V4L2_MBUS_FMT_YUYV8_2X8,
+	}, {
+		.name		= "YUV 4:2:2 packed, CbYCrY",
+		.fourcc		= V4L2_PIX_FMT_UYVY,
+		.depth		= { 16 },
+		.color		= FIMC_FMT_CBYCRY422,
+		.memplanes	= 1,
+		.mbus_code	= V4L2_MBUS_FMT_UYVY8_2X8,
+	}, {
+		.name		= "YUV 4:2:2 packed, CrYCbY",
+		.fourcc		= V4L2_PIX_FMT_VYUY,
+		.depth		= { 16 },
+		.color		= FIMC_FMT_CRYCBY422,
+		.memplanes	= 1,
+		.mbus_code	= V4L2_MBUS_FMT_VYUY8_2X8,
+	}, {
+		.name		= "YUV 4:2:2 packed, YCrYCb",
+		.fourcc		= V4L2_PIX_FMT_YVYU,
+		.depth		= { 16 },
+		.color		= FIMC_FMT_YCRYCB422,
+		.memplanes	= 1,
+		.mbus_code	= V4L2_MBUS_FMT_YVYU8_2X8,
+	}, {
+		.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_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
+ * @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 struct fimc_fmt *fmt, *def_fmt = NULL;
+	unsigned int i;
+	int id = 0;
+
+	if (index >= (int)ARRAY_SIZE(fimc_lite_formats))
+		return NULL;
+
+	for (i = 0; i < ARRAY_SIZE(fimc_lite_formats); ++i) {
+		fmt = &fimc_lite_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;
+}
+
+static int fimc_lite_hw_init(struct fimc_lite *fimc)
+{
+	struct fimc_pipeline *pipeline = &fimc->pipeline;
+	struct fimc_sensor_info *sensor;
+	unsigned long flags;
+
+	if (pipeline->subdevs[IDX_SENSOR] == NULL)
+		return -ENXIO;
+
+	if (fimc->fmt == NULL)
+		return -EINVAL;
+
+	sensor = v4l2_get_subdev_hostdata(pipeline->subdevs[IDX_SENSOR]);
+	spin_lock_irqsave(&fimc->slock, flags);
+
+	flite_hw_set_camera_bus(fimc, sensor->pdata);
+	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, true);
+	flite_hw_set_interrupt_mask(fimc);
+	flite_hw_set_test_pattern(fimc, fimc->test_pattern->val);
+
+	if (debug > 0)
+		flite_hw_dump_regs(fimc, __func__);
+
+	spin_unlock_irqrestore(&fimc->slock, flags);
+	return 0;
+}
+
+/*
+ * Reinitialize the driver so it is ready to start the streaming again.
+ * Set fimc->state to indicate stream off and the hardware shut down state.
+ * If not suspending (@suspend is false), return any buffers to videobuf2.
+ * Otherwise put any owned buffers onto the pending buffers queue, so they
+ * can be re-spun when the device is being resumed. Also perform FIMC
+ * software reset and disable streaming on the whole pipeline if required.
+ */
+static int fimc_lite_reinit(struct fimc_lite *fimc, bool suspend)
+{
+	struct flite_buffer *buf;
+	unsigned long flags;
+	bool streaming;
+
+	spin_lock_irqsave(&fimc->slock, flags);
+	streaming = fimc->state & (1 << ST_SENSOR_STREAM);
+
+	fimc->state &= ~(1 << ST_FLITE_RUN | 1 << ST_FLITE_OFF |
+			 1 << ST_FLITE_STREAM | 1 << ST_SENSOR_STREAM);
+	if (suspend)
+		fimc->state |= (1 << ST_FLITE_SUSPENDED);
+	else
+		fimc->state &= ~(1 << ST_FLITE_PENDING |
+				 1 << ST_FLITE_SUSPENDED);
+
+	/* Release unused buffers */
+	while (!suspend && !list_empty(&fimc->pending_buf_q)) {
+		buf = fimc_lite_pending_queue_pop(fimc);
+		vb2_buffer_done(&buf->vb, VB2_BUF_STATE_ERROR);
+	}
+	/* If suspending put unused buffers onto pending queue */
+	while (!list_empty(&fimc->active_buf_q)) {
+		buf = fimc_lite_active_queue_pop(fimc);
+		if (suspend)
+			fimc_lite_pending_queue_add(fimc, buf);
+		else
+			vb2_buffer_done(&buf->vb, VB2_BUF_STATE_ERROR);
+	}
+
+	spin_unlock_irqrestore(&fimc->slock, flags);
+
+	flite_hw_reset(fimc);
+
+	if (!streaming)
+		return 0;
+
+	return fimc_pipeline_s_stream(&fimc->pipeline, 0);
+}
+
+static int fimc_lite_stop_capture(struct fimc_lite *fimc, bool suspend)
+{
+	unsigned long flags;
+
+	if (!fimc_lite_active(fimc))
+		return 0;
+
+	spin_lock_irqsave(&fimc->slock, flags);
+	set_bit(ST_FLITE_OFF, &fimc->state);
+	flite_hw_capture_stop(fimc);
+	spin_unlock_irqrestore(&fimc->slock, flags);
+
+	wait_event_timeout(fimc->irq_queue,
+			   !test_bit(ST_FLITE_OFF, &fimc->state),
+			   (2*HZ/10)); /* 200 ms */
+
+	return fimc_lite_reinit(fimc, suspend);
+}
+
+/* Must be called  with fimc.slock spinlock held. */
+static void fimc_lite_config_update(struct fimc_lite *fimc)
+{
+	flite_hw_set_window_offset(fimc, &fimc->inp_frame);
+	flite_hw_set_dma_window(fimc, &fimc->out_frame);
+	flite_hw_set_test_pattern(fimc, fimc->test_pattern->val);
+	clear_bit(ST_FLITE_CONFIG, &fimc->state);
+}
+
+static irqreturn_t flite_irq_handler(int irq, void *priv)
+{
+	struct fimc_lite *fimc = priv;
+	struct flite_buffer *vbuf;
+	unsigned long flags;
+	struct timeval *tv;
+	struct timespec ts;
+	u32 intsrc;
+
+	spin_lock_irqsave(&fimc->slock, flags);
+
+	intsrc = flite_hw_get_interrupt_source(fimc);
+	flite_hw_clear_pending_irq(fimc);
+
+	if (test_and_clear_bit(ST_FLITE_OFF, &fimc->state)) {
+		wake_up(&fimc->irq_queue);
+		goto done;
+	}
+
+	if (intsrc & FLITE_REG_CISTATUS_IRQ_SRC_OVERFLOW) {
+		clear_bit(ST_FLITE_RUN, &fimc->state);
+		fimc->events.data_overflow++;
+	}
+
+	if (intsrc & FLITE_REG_CISTATUS_IRQ_SRC_LASTCAPEND) {
+		flite_hw_clear_last_capture_end(fimc);
+		clear_bit(ST_FLITE_STREAM, &fimc->state);
+		wake_up(&fimc->irq_queue);
+	}
+
+	if (fimc->out_path != FIMC_IO_DMA)
+		goto done;
+
+	if ((intsrc & FLITE_REG_CISTATUS_IRQ_SRC_FRMSTART) &&
+	    test_bit(ST_FLITE_RUN, &fimc->state) &&
+	    !list_empty(&fimc->active_buf_q) &&
+	    !list_empty(&fimc->pending_buf_q)) {
+		vbuf = fimc_lite_active_queue_pop(fimc);
+		ktime_get_ts(&ts);
+		tv = &vbuf->vb.v4l2_buf.timestamp;
+		tv->tv_sec = ts.tv_sec;
+		tv->tv_usec = ts.tv_nsec / NSEC_PER_USEC;
+		vbuf->vb.v4l2_buf.sequence = fimc->frame_count++;
+		vb2_buffer_done(&vbuf->vb, VB2_BUF_STATE_DONE);
+
+		vbuf = fimc_lite_pending_queue_pop(fimc);
+		flite_hw_set_output_addr(fimc, vbuf->paddr);
+		fimc_lite_active_queue_add(fimc, vbuf);
+	}
+
+	if (test_bit(ST_FLITE_CONFIG, &fimc->state))
+		fimc_lite_config_update(fimc);
+
+	if (list_empty(&fimc->pending_buf_q)) {
+		flite_hw_capture_stop(fimc);
+		clear_bit(ST_FLITE_STREAM, &fimc->state);
+	}
+done:
+	set_bit(ST_FLITE_RUN, &fimc->state);
+	spin_unlock_irqrestore(&fimc->slock, flags);
+	return IRQ_HANDLED;
+}
+
+static int start_streaming(struct vb2_queue *q, unsigned int count)
+{
+	struct fimc_lite *fimc = q->drv_priv;
+	int ret;
+
+	fimc->frame_count = 0;
+
+	ret = fimc_lite_hw_init(fimc);
+	if (ret) {
+		fimc_lite_reinit(fimc, false);
+		return ret;
+	}
+
+	set_bit(ST_FLITE_PENDING, &fimc->state);
+
+	if (!list_empty(&fimc->active_buf_q) &&
+	    !test_and_set_bit(ST_FLITE_STREAM, &fimc->state)) {
+		flite_hw_capture_start(fimc);
+
+		if (!test_and_set_bit(ST_SENSOR_STREAM, &fimc->state))
+			fimc_pipeline_s_stream(&fimc->pipeline, 1);
+	}
+	if (debug > 0)
+		flite_hw_dump_regs(fimc, __func__);
+
+	return 0;
+}
+
+static int stop_streaming(struct vb2_queue *q)
+{
+	struct fimc_lite *fimc = q->drv_priv;
+
+	if (!fimc_lite_active(fimc))
+		return -EINVAL;
+
+	return fimc_lite_stop_capture(fimc, false);
+}
+
+static int queue_setup(struct vb2_queue *vq, const struct v4l2_format *pfmt,
+		       unsigned int *num_buffers, unsigned int *num_planes,
+		       unsigned int sizes[], void *allocators[])
+{
+	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;
+	unsigned long wh;
+	int i;
+
+	if (pfmt) {
+		pixm = &pfmt->fmt.pix_mp;
+		fmt = fimc_lite_find_format(&pixm->pixelformat, NULL, -1);
+		wh = pixm->width * pixm->height;
+	} else {
+		wh = frame->f_width * frame->f_height;
+	}
+
+	if (fmt == NULL)
+		return -EINVAL;
+
+	*num_planes = fmt->memplanes;
+
+	for (i = 0; i < fmt->memplanes; i++) {
+		unsigned int size = (wh * fmt->depth[i]) / 8;
+		if (pixm)
+			sizes[i] = max(size, pixm->plane_fmt[i].sizeimage);
+		else
+			sizes[i] = size;
+		allocators[i] = fimc->alloc_ctx;
+	}
+
+	return 0;
+}
+
+static int buffer_prepare(struct vb2_buffer *vb)
+{
+	struct vb2_queue *vq = vb->vb2_queue;
+	struct fimc_lite *fimc = vq->drv_priv;
+	int i;
+
+	if (fimc->fmt == NULL)
+		return -EINVAL;
+
+	for (i = 0; i < fimc->fmt->memplanes; i++) {
+		unsigned long size = fimc->payload[i];
+
+		if (vb2_plane_size(vb, i) < size) {
+			v4l2_err(fimc->vfd,
+				 "User buffer too small (%ld < %ld)\n",
+				 vb2_plane_size(vb, i), size);
+			return -EINVAL;
+		}
+		vb2_set_plane_payload(vb, i, size);
+	}
+
+	return 0;
+}
+
+static void buffer_queue(struct vb2_buffer *vb)
+{
+	struct flite_buffer *buf
+		= container_of(vb, struct flite_buffer, vb);
+	struct fimc_lite *fimc = vb2_get_drv_priv(vb->vb2_queue);
+	unsigned long flags;
+
+	spin_lock_irqsave(&fimc->slock, flags);
+	buf->paddr = vb2_dma_contig_plane_dma_addr(vb, 0);
+
+	if (!test_bit(ST_FLITE_SUSPENDED, &fimc->state) &&
+	    !test_bit(ST_FLITE_STREAM, &fimc->state) &&
+	    list_empty(&fimc->active_buf_q)) {
+		flite_hw_set_output_addr(fimc, buf->paddr);
+		fimc_lite_active_queue_add(fimc, buf);
+	} else {
+		fimc_lite_pending_queue_add(fimc, buf);
+	}
+
+	if (vb2_is_streaming(&fimc->vb_queue) &&
+	    !list_empty(&fimc->pending_buf_q) &&
+	    !test_and_set_bit(ST_FLITE_STREAM, &fimc->state)) {
+		flite_hw_capture_start(fimc);
+		spin_unlock_irqrestore(&fimc->slock, flags);
+
+		if (!test_and_set_bit(ST_SENSOR_STREAM, &fimc->state))
+			fimc_pipeline_s_stream(&fimc->pipeline, 1);
+		return;
+	}
+	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,
+	.start_streaming = start_streaming,
+	.stop_streaming	 = stop_streaming,
+};
+
+static void fimc_lite_clear_event_counters(struct fimc_lite *fimc)
+{
+	unsigned long flags;
+
+	spin_lock_irqsave(&fimc->slock, flags);
+	memset(&fimc->events, 0, sizeof(fimc->events));
+	spin_unlock_irqrestore(&fimc->slock, flags);
+}
+
+static int fimc_lite_open(struct file *file)
+{
+	struct fimc_lite *fimc = video_drvdata(file);
+	int ret = v4l2_fh_open(file);
+
+	if (ret)
+		return ret;
+
+	set_bit(ST_FLITE_IN_USE, &fimc->state);
+	pm_runtime_get_sync(&fimc->pdev->dev);
+
+	if (++fimc->ref_count != 1 || fimc->out_path != FIMC_IO_DMA)
+		return ret;
+
+	ret = fimc_pipeline_initialize(&fimc->pipeline, &fimc->vfd->entity,
+				       true);
+	if (ret < 0) {
+		v4l2_err(fimc->vfd, "Video pipeline initialization failed\n");
+		pm_runtime_put_sync(&fimc->pdev->dev);
+		fimc->ref_count--;
+		v4l2_fh_release(file);
+		clear_bit(ST_FLITE_IN_USE, &fimc->state);
+	}
+
+	fimc_lite_clear_event_counters(fimc);
+	return ret;
+}
+
+static int fimc_lite_close(struct file *file)
+{
+	struct fimc_lite *fimc = video_drvdata(file);
+
+	if (--fimc->ref_count == 0 && fimc->out_path == FIMC_IO_DMA) {
+		clear_bit(ST_FLITE_IN_USE, &fimc->state);
+		fimc_lite_stop_capture(fimc, false);
+		fimc_pipeline_shutdown(&fimc->pipeline);
+		clear_bit(ST_FLITE_SUSPENDED, &fimc->state);
+	}
+
+	pm_runtime_put(&fimc->pdev->dev);
+
+	if (fimc->ref_count == 0)
+		vb2_queue_release(&fimc->vb_queue);
+
+	return v4l2_fh_release(file);
+}
+
+static unsigned int fimc_lite_poll(struct file *file,
+				   struct poll_table_struct *wait)
+{
+	struct fimc_lite *fimc = video_drvdata(file);
+	return vb2_poll(&fimc->vb_queue, file, wait);
+}
+
+static int fimc_lite_mmap(struct file *file, struct vm_area_struct *vma)
+{
+	struct fimc_lite *fimc = video_drvdata(file);
+	return vb2_mmap(&fimc->vb_queue, vma);
+}
+
+static const struct v4l2_file_operations fimc_lite_fops = {
+	.owner		= THIS_MODULE,
+	.open		= fimc_lite_open,
+	.release	= fimc_lite_close,
+	.poll		= fimc_lite_poll,
+	.unlocked_ioctl	= video_ioctl2,
+	.mmap		= fimc_lite_mmap,
+};
+
+/*
+ * Format and crop negotiation helpers
+ */
+
+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;
+	const struct fimc_fmt *fmt;
+
+	fmt = fimc_lite_find_format(fourcc, code, 0);
+	if (WARN_ON(!fmt))
+		return NULL;
+
+	if (code)
+		*code = fmt->mbus_code;
+	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);
+
+	return fmt;
+}
+
+static void fimc_lite_try_crop(struct fimc_lite *fimc, struct v4l2_rect *r)
+{
+	struct flite_frame *frame = &fimc->inp_frame;
+
+	v4l_bound_align_image(&r->width, 0, frame->f_width, 0,
+			      &r->height, 0, frame->f_height, 0, 0);
+
+	/* 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->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",
+		 r->left, r->top, r->width, r->height,
+		 frame->f_width, frame->f_height);
+}
+
+static void fimc_lite_try_compose(struct fimc_lite *fimc, struct v4l2_rect *r)
+{
+	struct flite_frame *frame = &fimc->out_frame;
+	struct v4l2_rect *crop_rect = &fimc->inp_frame.rect;
+
+	/* Scaling is not supported so we enforce compose rectangle size
+	   same as size of the sink crop rectangle. */
+	r->width = crop_rect->width;
+	r->height = crop_rect->height;
+
+	/* 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->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",
+		 r->left, r->top, r->width, r->height,
+		 frame->f_width, frame->f_height);
+}
+
+/*
+ * Video node ioctl operations
+ */
+static int fimc_vidioc_querycap_capture(struct file *file, void *priv,
+					struct v4l2_capability *cap)
+{
+	strlcpy(cap->driver, FIMC_LITE_DRV_NAME, sizeof(cap->driver));
+	cap->bus_info[0] = 0;
+	cap->card[0] = 0;
+	cap->capabilities = V4L2_CAP_STREAMING;
+	return 0;
+}
+
+static int fimc_lite_enum_fmt_mplane(struct file *file, void *priv,
+				     struct v4l2_fmtdesc *f)
+{
+	const struct fimc_fmt *fmt;
+
+	if (f->index >= ARRAY_SIZE(fimc_lite_formats))
+		return -EINVAL;
+
+	fmt = &fimc_lite_formats[f->index];
+	strlcpy(f->description, fmt->name, sizeof(f->description));
+	f->pixelformat = fmt->fourcc;
+
+	return 0;
+}
+
+static int fimc_lite_g_fmt_mplane(struct file *file, void *fh,
+				  struct v4l2_format *f)
+{
+	struct fimc_lite *fimc = video_drvdata(file);
+	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;
+
+	plane_fmt->bytesperline = (frame->f_width * fmt->depth[0]) / 8;
+	plane_fmt->sizeimage = plane_fmt->bytesperline * frame->f_height;
+
+	pixm->num_planes = fmt->memplanes;
+	pixm->pixelformat = fmt->fourcc;
+	pixm->width = frame->f_width;
+	pixm->height = frame->f_height;
+	pixm->field = V4L2_FIELD_NONE;
+	pixm->colorspace = V4L2_COLORSPACE_JPEG;
+	return 0;
+}
+
+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;
+	const struct fimc_fmt *fmt;
+
+	fmt = fimc_lite_find_format(&pixm->pixelformat, NULL, 0);
+	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);
+
+	if ((bpl == 0 || ((bpl * 8) / fmt->depth[0]) < pixm->width))
+		pixm->plane_fmt[0].bytesperline = (pixm->width *
+						   fmt->depth[0]) / 8;
+
+	if (pixm->plane_fmt[0].sizeimage == 0)
+		pixm->plane_fmt[0].sizeimage = (pixm->width * pixm->height *
+						fmt->depth[0]) / 8;
+	pixm->num_planes = fmt->memplanes;
+	pixm->pixelformat = fmt->fourcc;
+	pixm->colorspace = V4L2_COLORSPACE_JPEG;
+	pixm->field = V4L2_FIELD_NONE;
+	return 0;
+}
+
+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);
+}
+
+static int fimc_lite_s_fmt_mplane(struct file *file, void *priv,
+				  struct v4l2_format *f)
+{
+	struct v4l2_pix_format_mplane *pixm = &f->fmt.pix_mp;
+	struct fimc_lite *fimc = video_drvdata(file);
+	struct flite_frame *frame = &fimc->out_frame;
+	const struct fimc_fmt *fmt = NULL;
+	int ret;
+
+	if (vb2_is_busy(&fimc->vb_queue))
+		return -EBUSY;
+
+	ret = fimc_lite_try_fmt(fimc, &f->fmt.pix_mp, &fmt);
+	if (ret < 0)
+		return ret;
+
+	fimc->fmt = fmt;
+	fimc->payload[0] = max((pixm->width * pixm->height * fmt->depth[0]) / 8,
+			       pixm->plane_fmt[0].sizeimage);
+	frame->f_width = pixm->width;
+	frame->f_height = pixm->height;
+
+	return 0;
+}
+
+static int fimc_pipeline_validate(struct fimc_lite *fimc)
+{
+	struct v4l2_subdev *sd = &fimc->subdev;
+	struct v4l2_subdev_format sink_fmt, src_fmt;
+	struct media_pad *pad;
+	int ret;
+
+	while (1) {
+		/* Retrieve format at the sink pad */
+		pad = &sd->entity.pads[0];
+		if (!(pad->flags & MEDIA_PAD_FL_SINK))
+			break;
+		/* Don't call FIMC subdev operation to avoid nested locking */
+		if (sd == &fimc->subdev) {
+			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;
+		} else {
+			sink_fmt.pad = 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;
+		src_fmt.which = V4L2_SUBDEV_FORMAT_ACTIVE;
+		ret = v4l2_subdev_call(sd, pad, get_fmt, NULL, &src_fmt);
+		if (ret < 0 && ret != -ENOIOCTLCMD)
+			return -EPIPE;
+
+		if (src_fmt.format.width != sink_fmt.format.width ||
+		    src_fmt.format.height != sink_fmt.format.height ||
+		    src_fmt.format.code != sink_fmt.format.code)
+			return -EPIPE;
+	}
+	return 0;
+}
+
+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 fimc_pipeline *p = &fimc->pipeline;
+	int ret;
+
+	if (fimc_lite_active(fimc))
+		return -EBUSY;
+
+	media_entity_pipeline_start(&sensor->entity, p->m_pipeline);
+
+	ret = fimc_pipeline_validate(fimc);
+	if (ret) {
+		media_entity_pipeline_stop(&sensor->entity);
+		return ret;
+	}
+
+	return vb2_streamon(&fimc->vb_queue, type);
+}
+
+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;
+}
+
+static int fimc_lite_reqbufs(struct file *file, void *priv,
+			     struct v4l2_requestbuffers *reqbufs)
+{
+	struct fimc_lite *fimc = video_drvdata(file);
+	int ret;
+
+	reqbufs->count = max_t(u32, FLITE_REQ_BUFS_MIN, reqbufs->count);
+	ret = vb2_reqbufs(&fimc->vb_queue, reqbufs);
+	if (!ret < 0)
+		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)
+{
+	if (a->left < b->left || a->top < b->top)
+		return 0;
+	if (a->left + a->width > b->left + b->width)
+		return 0;
+	if (a->top + a->height > b->top + b->height)
+		return 0;
+
+	return 1;
+}
+
+static int fimc_lite_g_selection(struct file *file, void *fh,
+				 struct v4l2_selection *sel)
+{
+	struct fimc_lite *fimc = video_drvdata(file);
+	struct flite_frame *f = &fimc->out_frame;
+
+	if (sel->type != V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE)
+		return -EINVAL;
+
+	switch (sel->target) {
+	case V4L2_SEL_TGT_COMPOSE_BOUNDS:
+	case V4L2_SEL_TGT_COMPOSE_DEFAULT:
+		sel->r.left = 0;
+		sel->r.top = 0;
+		sel->r.width = f->f_width;
+		sel->r.height = f->f_height;
+		return 0;
+
+	case V4L2_SEL_TGT_COMPOSE_ACTIVE:
+		sel->r = f->rect;
+		return 0;
+	}
+
+	return -EINVAL;
+}
+
+static int fimc_lite_s_selection(struct file *file, void *fh,
+				 struct v4l2_selection *sel)
+{
+	struct fimc_lite *fimc = video_drvdata(file);
+	struct flite_frame *f = &fimc->out_frame;
+	struct v4l2_rect rect = sel->r;
+	unsigned long flags;
+
+	if (sel->type != V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE ||
+	    sel->target != V4L2_SEL_TGT_COMPOSE_ACTIVE)
+		return -EINVAL;
+
+	fimc_lite_try_compose(fimc, &rect);
+
+	if ((sel->flags & V4L2_SEL_FLAG_LE) &&
+	    !enclosed_rectangle(&rect, &sel->r))
+		return -ERANGE;
+
+	if ((sel->flags & V4L2_SEL_FLAG_GE) &&
+	    !enclosed_rectangle(&sel->r, &rect))
+		return -ERANGE;
+
+	sel->r = rect;
+	spin_lock_irqsave(&fimc->slock, flags);
+	f->rect = rect;
+	set_bit(ST_FLITE_CONFIG, &fimc->state);
+	spin_unlock_irqrestore(&fimc->slock, flags);
+
+	return 0;
+}
+
+static const struct v4l2_ioctl_ops fimc_lite_ioctl_ops = {
+	.vidioc_querycap		= fimc_vidioc_querycap_capture,
+	.vidioc_enum_fmt_vid_cap_mplane	= fimc_lite_enum_fmt_mplane,
+	.vidioc_try_fmt_vid_cap_mplane	= fimc_lite_try_fmt_mplane,
+	.vidioc_s_fmt_vid_cap_mplane	= fimc_lite_s_fmt_mplane,
+	.vidioc_g_fmt_vid_cap_mplane	= fimc_lite_g_fmt_mplane,
+	.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_streamon		= fimc_lite_streamon,
+	.vidioc_streamoff		= fimc_lite_streamoff,
+};
+
+/* Capture subdev media entity operations */
+static int fimc_lite_link_setup(struct media_entity *entity,
+				const struct media_pad *local,
+				const struct media_pad *remote, u32 flags)
+{
+	struct v4l2_subdev *sd = media_entity_to_v4l2_subdev(entity);
+	struct fimc_lite *fimc = v4l2_get_subdevdata(sd);
+	unsigned int remote_ent_type = media_entity_type(remote->entity);
+
+	if (WARN_ON(fimc == NULL))
+		return 0;
+
+	v4l2_dbg(1, debug, sd, "%s: %s --> %s, flags: 0x%x. source_id: 0x%x",
+		 __func__, local->entity->name, remote->entity->name,
+		 flags, fimc->source_subdev_grp_id);
+
+	switch (local->index) {
+	case FIMC_SD_PAD_SINK:
+		if (remote_ent_type != MEDIA_ENT_T_V4L2_SUBDEV)
+			return -EINVAL;
+
+		if (flags & MEDIA_LNK_FL_ENABLED) {
+			if (fimc->source_subdev_grp_id != 0)
+				return -EBUSY;
+			fimc->source_subdev_grp_id = sd->grp_id;
+			return 0;
+		}
+
+		fimc->source_subdev_grp_id = 0;
+		break;
+
+	case FIMC_SD_PAD_SOURCE:
+		if (!(flags & MEDIA_LNK_FL_ENABLED)) {
+			fimc->out_path = FIMC_IO_NONE;
+			return 0;
+		}
+		if (remote_ent_type == MEDIA_ENT_T_V4L2_SUBDEV)
+			fimc->out_path = FIMC_IO_ISP;
+		else
+			fimc->out_path = FIMC_IO_DMA;
+		break;
+
+	default:
+		v4l2_err(sd, "Invalid pad index\n");
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+static const struct media_entity_operations fimc_lite_subdev_media_ops = {
+	.link_setup = fimc_lite_link_setup,
+};
+
+static int fimc_lite_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_lite_find_format(NULL, NULL, code->index);
+	if (!fmt)
+		return -EINVAL;
+	code->code = fmt->mbus_code;
+	return 0;
+}
+
+static int fimc_lite_subdev_get_fmt(struct v4l2_subdev *sd,
+				    struct v4l2_subdev_fh *fh,
+				    struct v4l2_subdev_format *fmt)
+{
+	struct fimc_lite *fimc = v4l2_get_subdevdata(sd);
+	struct v4l2_mbus_framefmt *mf = &fmt->format;
+	struct flite_frame *f = &fimc->out_frame;
+
+	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(&fimc->lock);
+	mf->code = fimc->fmt->mbus_code;
+
+	if (fmt->pad == FLITE_SD_PAD_SINK) {
+		/* full camera input frame size */
+		mf->width = f->f_width;
+		mf->height = f->f_height;
+	} else {
+		/* crop size */
+		mf->width = f->rect.width;
+		mf->height = f->rect.height;
+	}
+	mutex_unlock(&fimc->lock);
+	return 0;
+}
+
+static int fimc_lite_subdev_set_fmt(struct v4l2_subdev *sd,
+				    struct v4l2_subdev_fh *fh,
+				    struct v4l2_subdev_format *fmt)
+{
+	struct fimc_lite *fimc = v4l2_get_subdevdata(sd);
+	struct v4l2_mbus_framefmt *mf = &fmt->format;
+	struct flite_frame *sink = &fimc->inp_frame;
+	const struct fimc_fmt *ffmt;
+
+	v4l2_dbg(1, debug, sd, "pad%d: code: 0x%x, %dx%d",
+		 fmt->pad, mf->code, mf->width, mf->height);
+
+	mf->colorspace = V4L2_COLORSPACE_JPEG;
+	mutex_lock(&fimc->lock);
+
+	if ((fimc->out_path == FIMC_IO_ISP && sd->entity.stream_count > 0) ||
+	    (fimc->out_path == FIMC_IO_DMA && vb2_is_busy(&fimc->vb_queue))) {
+		mutex_unlock(&fimc->lock);
+		return -EBUSY;
+	}
+
+	ffmt = fimc_lite_try_format(fimc, &mf->width, &mf->height,
+				    &mf->code, NULL, fmt->pad);
+
+	if (fmt->which == V4L2_SUBDEV_FORMAT_TRY) {
+		mf = v4l2_subdev_get_try_format(fh, fmt->pad);
+		*mf = fmt->format;
+		mutex_unlock(&fimc->lock);
+		return 0;
+	}
+
+	if (fmt->pad == FLITE_SD_PAD_SINK) {
+		sink->f_width = mf->width;
+		sink->f_height = mf->height;
+		fimc->fmt = ffmt;
+		/* Set sink crop rectangle */
+		sink->rect.width = mf->width;
+		sink->rect.height = mf->height;
+		sink->rect.left = 0;
+		sink->rect.top = 0;
+		/* Reset source crop rectangle */
+		fimc->out_frame.rect = sink->rect;
+	} else {
+		/* Allow changing format only on sink pad */
+		mf->code = fimc->fmt->mbus_code;
+		mf->width = sink->rect.width;
+		mf->height = sink->rect.height;
+	}
+
+	mutex_unlock(&fimc->lock);
+	return 0;
+}
+
+static int fimc_lite_subdev_get_selection(struct v4l2_subdev *sd,
+					  struct v4l2_subdev_fh *fh,
+					  struct v4l2_subdev_selection *sel)
+{
+	struct fimc_lite *fimc = v4l2_get_subdevdata(sd);
+	struct flite_frame *f = &fimc->inp_frame;
+
+	if ((sel->target != V4L2_SUBDEV_SEL_TGT_CROP_ACTUAL &&
+	     sel->target != V4L2_SUBDEV_SEL_TGT_CROP_BOUNDS) ||
+	    sel->pad != FLITE_SD_PAD_SINK)
+		return -EINVAL;
+
+	if (sel->which == V4L2_SUBDEV_FORMAT_TRY) {
+		sel->r = *v4l2_subdev_get_try_crop(fh, sel->pad);
+		return 0;
+	}
+
+	mutex_lock(&fimc->lock);
+	if (sel->target == V4L2_SUBDEV_SEL_TGT_CROP_ACTUAL) {
+		sel->r = f->rect;
+	} else {
+		sel->r.left = 0;
+		sel->r.top = 0;
+		sel->r.width = f->f_width;
+		sel->r.height = f->f_height;
+	}
+	mutex_unlock(&fimc->lock);
+
+	v4l2_dbg(1, debug, sd, "%s: (%d,%d) %dx%d, f_w: %d, f_h: %d",
+		 __func__, f->rect.left, f->rect.top, f->rect.width,
+		 f->rect.height, f->f_width, f->f_height);
+
+	return 0;
+}
+
+static int fimc_lite_subdev_set_selection(struct v4l2_subdev *sd,
+					  struct v4l2_subdev_fh *fh,
+					  struct v4l2_subdev_selection *sel)
+{
+	struct fimc_lite *fimc = v4l2_get_subdevdata(sd);
+	struct flite_frame *f = &fimc->inp_frame;
+	int ret = 0;
+
+	if (sel->target != V4L2_SUBDEV_SEL_TGT_CROP_ACTUAL ||
+	    sel->pad != FLITE_SD_PAD_SINK)
+		return -EINVAL;
+
+	mutex_lock(&fimc->lock);
+	fimc_lite_try_crop(fimc, &sel->r);
+
+	if (sel->which == V4L2_SUBDEV_FORMAT_TRY) {
+		*v4l2_subdev_get_try_crop(fh, sel->pad) = sel->r;
+	} else {
+		unsigned long flags;
+		spin_lock_irqsave(&fimc->slock, flags);
+		f->rect = sel->r;
+		/* Same crop rectangle on the source pad */
+		fimc->out_frame.rect = sel->r;
+		set_bit(ST_FLITE_CONFIG, &fimc->state);
+		spin_unlock_irqrestore(&fimc->slock, flags);
+	}
+	mutex_unlock(&fimc->lock);
+
+	v4l2_dbg(1, debug, sd, "%s: (%d,%d) %dx%d, f_w: %d, f_h: %d",
+		 __func__, f->rect.left, f->rect.top, f->rect.width,
+		 f->rect.height, f->f_width, f->f_height);
+
+	return ret;
+}
+
+static int fimc_lite_subdev_s_stream(struct v4l2_subdev *sd, int on)
+{
+	struct fimc_lite *fimc = v4l2_get_subdevdata(sd);
+
+	if (fimc->out_path == FIMC_IO_DMA)
+		return -ENOIOCTLCMD;
+
+	/* TODO: */
+
+	return 0;
+}
+
+static int fimc_lite_subdev_s_power(struct v4l2_subdev *sd, int on)
+{
+	struct fimc_lite *fimc = v4l2_get_subdevdata(sd);
+
+	if (fimc->out_path == FIMC_IO_DMA)
+		return -ENOIOCTLCMD;
+
+	/* TODO: */
+
+	return 0;
+}
+
+static int fimc_lite_log_status(struct v4l2_subdev *sd)
+{
+	struct fimc_lite *fimc = v4l2_get_subdevdata(sd);
+
+	flite_hw_dump_regs(fimc, __func__);
+	return 0;
+}
+
+static int fimc_lite_subdev_registered(struct v4l2_subdev *sd)
+{
+	struct fimc_lite *fimc = v4l2_get_subdevdata(sd);
+	struct vb2_queue *q = &fimc->vb_queue;
+	struct video_device *vfd;
+	int ret;
+
+	fimc->fmt = &fimc_lite_formats[0];
+	fimc->out_path = FIMC_IO_DMA;
+
+	vfd = video_device_alloc();
+	if (!vfd) {
+		v4l2_err(sd->v4l2_dev, "Failed to allocate video device\n");
+		return -ENOMEM;
+	}
+
+	snprintf(vfd->name, sizeof(vfd->name), "fimc-lite.%d.capture",
+		 fimc->index);
+
+	vfd->fops = &fimc_lite_fops;
+	vfd->ioctl_ops = &fimc_lite_ioctl_ops;
+	vfd->v4l2_dev = sd->v4l2_dev;
+	vfd->minor = -1;
+	vfd->release = video_device_release;
+	vfd->lock = &fimc->lock;
+	fimc->vfd = vfd;
+	fimc->ref_count = 0;
+	fimc->reqbufs_count = 0;
+
+	INIT_LIST_HEAD(&fimc->pending_buf_q);
+	INIT_LIST_HEAD(&fimc->active_buf_q);
+
+	memset(q, 0, sizeof(*q));
+	q->type = V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE;
+	q->io_modes = VB2_MMAP | VB2_USERPTR;
+	q->ops = &fimc_lite_qops;
+	q->mem_ops = &vb2_dma_contig_memops;
+	q->buf_struct_size = sizeof(struct flite_buffer);
+	q->drv_priv = fimc;
+
+	vb2_queue_init(q);
+
+	fimc->vd_pad.flags = MEDIA_PAD_FL_SINK;
+	ret = media_entity_init(&vfd->entity, 1, &fimc->vd_pad, 0);
+	if (ret)
+		goto err;
+
+	video_set_drvdata(vfd, fimc);
+
+	ret = video_register_device(vfd, VFL_TYPE_GRABBER, -1);
+	if (ret)
+		goto err_vd;
+
+	v4l2_info(sd->v4l2_dev, "Registered %s as /dev/%s\n",
+		  vfd->name, video_device_node_name(vfd));
+	return 0;
+
+ err_vd:
+	media_entity_cleanup(&vfd->entity);
+ err:
+	video_device_release(vfd);
+	return ret;
+}
+
+static void fimc_lite_subdev_unregistered(struct v4l2_subdev *sd)
+{
+	struct fimc_lite *fimc = v4l2_get_subdevdata(sd);
+
+	if (fimc == NULL)
+		return;
+
+	if (fimc->vfd) {
+		video_unregister_device(fimc->vfd);
+		media_entity_cleanup(&fimc->vfd->entity);
+		fimc->vfd = NULL;
+	}
+}
+
+static const struct v4l2_subdev_internal_ops fimc_lite_subdev_internal_ops = {
+	.registered = fimc_lite_subdev_registered,
+	.unregistered = fimc_lite_subdev_unregistered,
+};
+
+static const struct v4l2_subdev_pad_ops fimc_lite_subdev_pad_ops = {
+	.enum_mbus_code = fimc_lite_subdev_enum_mbus_code,
+	.get_selection = fimc_lite_subdev_get_selection,
+	.set_selection = fimc_lite_subdev_set_selection,
+	.get_fmt = fimc_lite_subdev_get_fmt,
+	.set_fmt = fimc_lite_subdev_set_fmt,
+};
+
+static const struct v4l2_subdev_video_ops fimc_lite_subdev_video_ops = {
+	.s_stream = fimc_lite_subdev_s_stream,
+};
+
+static const struct v4l2_subdev_core_ops fimc_lite_core_ops = {
+	.s_power = fimc_lite_subdev_s_power,
+	.log_status = fimc_lite_log_status,
+};
+
+static struct v4l2_subdev_ops fimc_lite_subdev_ops = {
+	.core = &fimc_lite_core_ops,
+	.video = &fimc_lite_subdev_video_ops,
+	.pad = &fimc_lite_subdev_pad_ops,
+};
+
+static int fimc_lite_s_ctrl(struct v4l2_ctrl *ctrl)
+{
+	struct fimc_lite *fimc = container_of(ctrl->handler, struct fimc_lite,
+					      ctrl_handler);
+	set_bit(ST_FLITE_CONFIG, &fimc->state);
+	return 0;
+}
+
+static const struct v4l2_ctrl_ops fimc_lite_ctrl_ops = {
+	.s_ctrl	= fimc_lite_s_ctrl,
+};
+
+static const struct v4l2_ctrl_config fimc_lite_ctrl = {
+	.ops	= &fimc_lite_ctrl_ops,
+	.id	= V4L2_CTRL_CLASS_USER | 0x1001,
+	.type	= V4L2_CTRL_TYPE_BOOLEAN,
+	.name	= "Test Pattern 640x480",
+};
+
+static int fimc_lite_create_capture_subdev(struct fimc_lite *fimc)
+{
+	struct v4l2_ctrl_handler *handler = &fimc->ctrl_handler;
+	struct v4l2_subdev *sd = &fimc->subdev;
+	int ret;
+
+	v4l2_subdev_init(sd, &fimc_lite_subdev_ops);
+	sd->flags = V4L2_SUBDEV_FL_HAS_DEVNODE;
+	snprintf(sd->name, sizeof(sd->name), "FIMC-LITE.%d", fimc->index);
+
+	fimc->subdev_pads[FIMC_SD_PAD_SINK].flags = MEDIA_PAD_FL_SINK;
+	fimc->subdev_pads[FIMC_SD_PAD_SOURCE].flags = MEDIA_PAD_FL_SOURCE;
+	ret = media_entity_init(&sd->entity, FIMC_SD_PADS_NUM,
+				fimc->subdev_pads, 0);
+	if (ret)
+		return ret;
+
+	v4l2_ctrl_handler_init(handler, 1);
+	fimc->test_pattern = v4l2_ctrl_new_custom(handler, &fimc_lite_ctrl,
+						  NULL);
+	if (handler->error) {
+		media_entity_cleanup(&sd->entity);
+		return handler->error;
+	}
+
+	sd->ctrl_handler = handler;
+	sd->internal_ops = &fimc_lite_subdev_internal_ops;
+	sd->entity.ops = &fimc_lite_subdev_media_ops;
+	v4l2_set_subdevdata(sd, fimc);
+
+	return 0;
+}
+
+static void fimc_lite_unregister_capture_subdev(struct fimc_lite *fimc)
+{
+	struct v4l2_subdev *sd = &fimc->subdev;
+
+	v4l2_device_unregister_subdev(sd);
+	media_entity_cleanup(&sd->entity);
+	v4l2_ctrl_handler_free(&fimc->ctrl_handler);
+	v4l2_set_subdevdata(sd, NULL);
+}
+
+static void fimc_lite_clk_put(struct fimc_lite *fimc)
+{
+	if (IS_ERR_OR_NULL(fimc->clock))
+		return;
+
+	clk_unprepare(fimc->clock);
+	clk_put(fimc->clock);
+	fimc->clock = NULL;
+}
+
+static int fimc_lite_clk_get(struct fimc_lite *fimc)
+{
+	int ret;
+
+	fimc->clock = clk_get(&fimc->pdev->dev, FLITE_CLK_NAME);
+	if (IS_ERR(fimc->clock))
+		return PTR_ERR(fimc->clock);
+
+	ret = clk_prepare(fimc->clock);
+	if (ret < 0) {
+		clk_put(fimc->clock);
+		fimc->clock = NULL;
+	}
+	return ret;
+}
+
+static int __devinit fimc_lite_probe(struct platform_device *pdev)
+{
+	struct flite_drvdata *drv_data = fimc_lite_get_drvdata(pdev);
+	struct fimc_lite *fimc;
+	struct resource *res;
+	int ret;
+
+	fimc = devm_kzalloc(&pdev->dev, sizeof(*fimc), GFP_KERNEL);
+	if (!fimc)
+		return -ENOMEM;
+
+	fimc->index = pdev->id;
+	fimc->variant = drv_data->variant[fimc->index];
+	fimc->pdev = pdev;
+
+	init_waitqueue_head(&fimc->irq_queue);
+	spin_lock_init(&fimc->slock);
+	mutex_init(&fimc->lock);
+
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	fimc->regs = devm_request_and_ioremap(&pdev->dev, res);
+	if (fimc->regs == NULL) {
+		dev_err(&pdev->dev, "Failed to obtain io memory\n");
+		return -ENOENT;
+	}
+
+	res = platform_get_resource(pdev, IORESOURCE_IRQ, 0);
+	if (res == NULL) {
+		dev_err(&pdev->dev, "Failed to get IRQ resource\n");
+		return -ENXIO;
+	}
+
+	ret = fimc_lite_clk_get(fimc);
+	if (ret)
+		return ret;
+
+	ret = devm_request_irq(&pdev->dev, res->start, flite_irq_handler,
+			       0, dev_name(&pdev->dev), fimc);
+	if (ret) {
+		dev_err(&pdev->dev, "Failed to install irq (%d)\n", ret);
+		goto err_clk;
+	}
+
+	/* The video node will be created within the subdev's registered() op */
+	ret = fimc_lite_create_capture_subdev(fimc);
+	if (ret)
+		goto err_clk;
+
+	platform_set_drvdata(pdev, fimc);
+	pm_runtime_enable(&pdev->dev);
+	ret = pm_runtime_get_sync(&pdev->dev);
+	if (ret < 0)
+		goto err_sd;
+
+	fimc->alloc_ctx = vb2_dma_contig_init_ctx(&pdev->dev);
+	if (IS_ERR(fimc->alloc_ctx)) {
+		ret = PTR_ERR(fimc->alloc_ctx);
+		goto err_pm;
+	}
+	pm_runtime_put(&pdev->dev);
+
+	dev_dbg(&pdev->dev, "FIMC-LITE.%d registered successfully\n",
+		fimc->index);
+	return 0;
+err_pm:
+	pm_runtime_put(&pdev->dev);
+err_sd:
+	fimc_lite_unregister_capture_subdev(fimc);
+err_clk:
+	fimc_lite_clk_put(fimc);
+	return ret;
+}
+
+static int fimc_lite_runtime_resume(struct device *dev)
+{
+	struct fimc_lite *fimc = dev_get_drvdata(dev);
+
+	clk_enable(fimc->clock);
+	return 0;
+}
+
+static int fimc_lite_runtime_suspend(struct device *dev)
+{
+	struct fimc_lite *fimc = dev_get_drvdata(dev);
+
+	clk_disable(fimc->clock);
+	return 0;
+}
+
+#ifdef CONFIG_PM_SLEEP
+static int fimc_lite_resume(struct device *dev)
+{
+	struct fimc_lite *fimc = dev_get_drvdata(dev);
+	struct flite_buffer *buf;
+	unsigned long flags;
+	int i;
+
+	spin_lock_irqsave(&fimc->slock, flags);
+	if (!test_and_clear_bit(ST_LPM, &fimc->state) ||
+	    !test_bit(ST_FLITE_IN_USE, &fimc->state)) {
+		spin_unlock_irqrestore(&fimc->slock, flags);
+		return 0;
+	}
+	flite_hw_reset(fimc);
+	spin_unlock_irqrestore(&fimc->slock, flags);
+
+	if (!test_and_clear_bit(ST_FLITE_SUSPENDED, &fimc->state))
+		return 0;
+
+	INIT_LIST_HEAD(&fimc->active_buf_q);
+	fimc_pipeline_initialize(&fimc->pipeline, &fimc->vfd->entity, false);
+	fimc_lite_hw_init(fimc);
+	clear_bit(ST_FLITE_SUSPENDED, &fimc->state);
+
+	for (i = 0; i < fimc->reqbufs_count; i++) {
+		if (list_empty(&fimc->pending_buf_q))
+			break;
+		buf = fimc_lite_pending_queue_pop(fimc);
+		buffer_queue(&buf->vb);
+	}
+	return 0;
+}
+
+static int fimc_lite_suspend(struct device *dev)
+{
+	struct fimc_lite *fimc = dev_get_drvdata(dev);
+	bool suspend = test_bit(ST_FLITE_IN_USE, &fimc->state);
+	int ret;
+
+	if (test_and_set_bit(ST_LPM, &fimc->state))
+		return 0;
+
+	ret = fimc_lite_stop_capture(fimc, suspend);
+	if (ret)
+		return ret;
+
+	return fimc_pipeline_shutdown(&fimc->pipeline);
+}
+#endif /* CONFIG_PM_SLEEP */
+
+static int __devexit fimc_lite_remove(struct platform_device *pdev)
+{
+	struct fimc_lite *fimc = platform_get_drvdata(pdev);
+	struct device *dev = &pdev->dev;
+
+	pm_runtime_disable(dev);
+	pm_runtime_set_suspended(dev);
+	fimc_lite_unregister_capture_subdev(fimc);
+	vb2_dma_contig_cleanup_ctx(fimc->alloc_ctx);
+	fimc_lite_clk_put(fimc);
+
+	dev_info(dev, "Driver unloaded\n");
+	return 0;
+}
+
+static struct flite_variant fimc_lite0_variant_exynos4 = {
+	.max_width		= 8192,
+	.max_height		= 8192,
+	.out_width_align	= 8,
+	.win_hor_offs_align	= 2,
+	.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",
+		.driver_data	= (unsigned long)&fimc_lite_drvdata_exynos4,
+	},
+	{ /* sentinel */ },
+};
+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 struct platform_driver fimc_lite_driver = {
+	.probe		= fimc_lite_probe,
+	.remove		= __devexit_p(fimc_lite_remove),
+	.id_table	= fimc_lite_driver_ids,
+	.driver = {
+		.name		= FIMC_LITE_DRV_NAME,
+		.owner		= THIS_MODULE,
+		.pm		= &fimc_lite_pm_ops,
+	}
+};
+module_platform_driver(fimc_lite_driver);
+MODULE_LICENSE("GPL");
+MODULE_ALIAS("platform:" FIMC_LITE_DRV_NAME);
diff --git a/drivers/media/video/s5p-fimc/fimc-lite.h b/drivers/media/video/s5p-fimc/fimc-lite.h
new file mode 100644
index 00000000000..44424eee81d
--- /dev/null
+++ b/drivers/media/video/s5p-fimc/fimc-lite.h
@@ -0,0 +1,213 @@
+/*
+ * Copyright (C) 2012 Samsung Electronics Co., Ltd.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ */
+
+#ifndef FIMC_LITE_H_
+#define FIMC_LITE_H_
+
+#include <asm/sizes.h>
+#include <linux/io.h>
+#include <linux/irqreturn.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>
+
+#include "fimc-core.h"
+
+#define FIMC_LITE_DRV_NAME	"exynos-fimc-lite"
+#define FLITE_CLK_NAME		"flite"
+#define FIMC_LITE_MAX_DEVS	2
+#define FLITE_REQ_BUFS_MIN	2
+
+/* Bit index definitions for struct fimc_lite::state */
+enum {
+	ST_FLITE_LPM,
+	ST_FLITE_PENDING,
+	ST_FLITE_RUN,
+	ST_FLITE_STREAM,
+	ST_FLITE_SUSPENDED,
+	ST_FLITE_OFF,
+	ST_FLITE_IN_USE,
+	ST_FLITE_CONFIG,
+	ST_SENSOR_STREAM,
+};
+
+#define FLITE_SD_PAD_SINK	0
+#define FLITE_SD_PAD_SOURCE	1
+#define FLITE_SD_PADS_NUM	2
+
+struct flite_variant {
+	unsigned short max_width;
+	unsigned short max_height;
+	unsigned short out_width_align;
+	unsigned short win_hor_offs_align;
+	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)
+
+struct fimc_lite_events {
+	unsigned int data_overflow;
+};
+
+#define FLITE_MAX_PLANES	1
+
+/**
+ * struct flite_frame - source/target frame properties
+ * @f_width: full pixel width
+ * @f_height: full pixel height
+ * @rect: crop/composition rectangle
+ */
+struct flite_frame {
+	u16 f_width;
+	u16 f_height;
+	struct v4l2_rect rect;
+};
+
+/**
+ * struct flite_buffer - video buffer structure
+ * @vb:    vb2 buffer
+ * @list:  list head for the buffers queue
+ * @paddr: precalculated physical address
+ */
+struct flite_buffer {
+	struct vb2_buffer vb;
+	struct list_head list;
+	dma_addr_t paddr;
+};
+
+/**
+ * struct fimc_lite - fimc lite structure
+ * @pdev: pointer to FIMC-LITE platform device
+ * @variant: variant information for this IP
+ * @v4l2_dev: pointer to top the level v4l2_device
+ * @vfd: video device node
+ * @fh: v4l2 file handle
+ * @alloc_ctx: videobuf2 memory allocator context
+ * @subdev: FIMC-LITE subdev
+ * @vd_pad: media (sink) pad for the capture video node
+ * @subdev_pads: the subdev media pads
+ * @ctrl_handler: v4l2 control handler
+ * @test_pattern: test pattern controls
+ * @index: FIMC-LITE platform device index
+ * @pipeline: video capture pipeline data structure
+ * @slock: spinlock protecting this data structure and the hw registers
+ * @lock: mutex serializing video device and the subdev operations
+ * @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
+ * @out_path: output data path (DMA or FIFO)
+ * @source_subdev_grp_id: source subdev group id
+ * @state: driver state flags
+ * @pending_buf_q: pending buffers queue head
+ * @active_buf_q: the queue head of buffers scheduled in hardware
+ * @vb_queue: vb2 buffers queue
+ * @active_buf_count: number of video buffers scheduled in hardware
+ * @frame_count: the captured frames counter
+ * @reqbufs_count: the number of buffers requested with REQBUFS ioctl
+ * @ref_count: driver's private reference counter
+ */
+struct fimc_lite {
+	struct platform_device	*pdev;
+	struct flite_variant	*variant;
+	struct v4l2_device	*v4l2_dev;
+	struct video_device	*vfd;
+	struct v4l2_fh		fh;
+	struct vb2_alloc_ctx	*alloc_ctx;
+	struct v4l2_subdev	subdev;
+	struct media_pad	vd_pad;
+	struct media_pad	subdev_pads[FLITE_SD_PADS_NUM];
+	struct v4l2_ctrl_handler ctrl_handler;
+	struct v4l2_ctrl	*test_pattern;
+	u32			index;
+	struct fimc_pipeline	pipeline;
+
+	struct mutex		lock;
+	spinlock_t		slock;
+
+	struct clk		*clock;
+	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;
+	enum fimc_datapath	out_path;
+	unsigned int		source_subdev_grp_id;
+
+	unsigned long		state;
+	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			ref_count;
+
+	struct fimc_lite_events	events;
+};
+
+static inline bool fimc_lite_active(struct fimc_lite *fimc)
+{
+	unsigned long flags;
+	bool ret;
+
+	spin_lock_irqsave(&fimc->slock, flags);
+	ret = fimc->state & (1 << ST_FLITE_RUN) ||
+		fimc->state & (1 << ST_FLITE_PENDING);
+	spin_unlock_irqrestore(&fimc->slock, flags);
+	return ret;
+}
+
+static inline void fimc_lite_active_queue_add(struct fimc_lite *dev,
+					 struct flite_buffer *buf)
+{
+	list_add_tail(&buf->list, &dev->active_buf_q);
+}
+
+static inline struct flite_buffer *fimc_lite_active_queue_pop(
+					struct fimc_lite *dev)
+{
+	struct flite_buffer *buf = list_entry(dev->active_buf_q.next,
+					      struct flite_buffer, list);
+	list_del(&buf->list);
+	return buf;
+}
+
+static inline void fimc_lite_pending_queue_add(struct fimc_lite *dev,
+					struct flite_buffer *buf)
+{
+	list_add_tail(&buf->list, &dev->pending_buf_q);
+}
+
+static inline struct flite_buffer *fimc_lite_pending_queue_pop(
+					struct fimc_lite *dev)
+{
+	struct flite_buffer *buf = list_entry(dev->pending_buf_q.next,
+					      struct flite_buffer, list);
+	list_del(&buf->list);
+	return buf;
+}
+
+#endif /* FIMC_LITE_H_ */
diff --git a/drivers/media/video/s5p-fimc/fimc-m2m.c b/drivers/media/video/s5p-fimc/fimc-m2m.c
new file mode 100644
index 00000000000..4c58e057096
--- /dev/null
+++ b/drivers/media/video/s5p-fimc/fimc-m2m.c
@@ -0,0 +1,824 @@
+/*
+ * Samsung S5P/EXYNOS4 SoC series FIMC (video postprocessor) driver
+ *
+ * Copyright (C) 2012 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 as published
+ * by the Free Software Foundation, either version 2 of the License,
+ * or (at your option) any later version.
+ */
+
+#include <linux/module.h>
+#include <linux/kernel.h>
+#include <linux/types.h>
+#include <linux/errno.h>
+#include <linux/bug.h>
+#include <linux/interrupt.h>
+#include <linux/device.h>
+#include <linux/platform_device.h>
+#include <linux/pm_runtime.h>
+#include <linux/list.h>
+#include <linux/io.h>
+#include <linux/slab.h>
+#include <linux/clk.h>
+#include <media/v4l2-ioctl.h>
+#include <media/videobuf2-core.h>
+#include <media/videobuf2-dma-contig.h>
+
+#include "fimc-core.h"
+#include "fimc-reg.h"
+#include "fimc-mdevice.h"
+
+
+static unsigned int get_m2m_fmt_flags(unsigned int stream_type)
+{
+	if (stream_type == V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE)
+		return FMT_FLAGS_M2M_IN;
+	else
+		return FMT_FLAGS_M2M_OUT;
+}
+
+void fimc_m2m_job_finish(struct fimc_ctx *ctx, int vb_state)
+{
+	struct vb2_buffer *src_vb, *dst_vb;
+
+	if (!ctx || !ctx->m2m_ctx)
+		return;
+
+	src_vb = v4l2_m2m_src_buf_remove(ctx->m2m_ctx);
+	dst_vb = v4l2_m2m_dst_buf_remove(ctx->m2m_ctx);
+
+	if (src_vb && dst_vb) {
+		v4l2_m2m_buf_done(src_vb, vb_state);
+		v4l2_m2m_buf_done(dst_vb, vb_state);
+		v4l2_m2m_job_finish(ctx->fimc_dev->m2m.m2m_dev,
+				    ctx->m2m_ctx);
+	}
+}
+
+/* Complete the transaction which has been scheduled for execution. */
+static int fimc_m2m_shutdown(struct fimc_ctx *ctx)
+{
+	struct fimc_dev *fimc = ctx->fimc_dev;
+	int ret;
+
+	if (!fimc_m2m_pending(fimc))
+		return 0;
+
+	fimc_ctx_state_set(FIMC_CTX_SHUT, ctx);
+
+	ret = wait_event_timeout(fimc->irq_queue,
+			   !fimc_ctx_state_is_set(FIMC_CTX_SHUT, ctx),
+			   FIMC_SHUTDOWN_TIMEOUT);
+
+	return ret == 0 ? -ETIMEDOUT : ret;
+}
+
+static int start_streaming(struct vb2_queue *q, unsigned int count)
+{
+	struct fimc_ctx *ctx = q->drv_priv;
+	int ret;
+
+	ret = pm_runtime_get_sync(&ctx->fimc_dev->pdev->dev);
+	return ret > 0 ? 0 : ret;
+}
+
+static int stop_streaming(struct vb2_queue *q)
+{
+	struct fimc_ctx *ctx = q->drv_priv;
+	int ret;
+
+	ret = fimc_m2m_shutdown(ctx);
+	if (ret == -ETIMEDOUT)
+		fimc_m2m_job_finish(ctx, VB2_BUF_STATE_ERROR);
+
+	pm_runtime_put(&ctx->fimc_dev->pdev->dev);
+	return 0;
+}
+
+static void fimc_device_run(void *priv)
+{
+	struct vb2_buffer *vb = NULL;
+	struct fimc_ctx *ctx = priv;
+	struct fimc_frame *sf, *df;
+	struct fimc_dev *fimc;
+	unsigned long flags;
+	u32 ret;
+
+	if (WARN(!ctx, "Null context\n"))
+		return;
+
+	fimc = ctx->fimc_dev;
+	spin_lock_irqsave(&fimc->slock, flags);
+
+	set_bit(ST_M2M_PEND, &fimc->state);
+	sf = &ctx->s_frame;
+	df = &ctx->d_frame;
+
+	if (ctx->state & FIMC_PARAMS) {
+		/* Prepare the DMA offsets for scaler */
+		fimc_prepare_dma_offset(ctx, sf);
+		fimc_prepare_dma_offset(ctx, df);
+	}
+
+	vb = v4l2_m2m_next_src_buf(ctx->m2m_ctx);
+	ret = fimc_prepare_addr(ctx, 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);
+	if (ret)
+		goto dma_unlock;
+
+	/* Reconfigure hardware if the context has changed. */
+	if (fimc->m2m.ctx != ctx) {
+		ctx->state |= FIMC_PARAMS;
+		fimc->m2m.ctx = ctx;
+	}
+
+	if (ctx->state & FIMC_PARAMS) {
+		fimc_set_yuv_order(ctx);
+		fimc_hw_set_input_path(ctx);
+		fimc_hw_set_in_dma(ctx);
+		ret = fimc_set_scaler_info(ctx);
+		if (ret)
+			goto dma_unlock;
+		fimc_hw_set_prescaler(ctx);
+		fimc_hw_set_mainscaler(ctx);
+		fimc_hw_set_target_format(ctx);
+		fimc_hw_set_rotation(ctx);
+		fimc_hw_set_effect(ctx);
+		fimc_hw_set_out_dma(ctx);
+		if (fimc->variant->has_alpha)
+			fimc_hw_set_rgb_alpha(ctx);
+		fimc_hw_set_output_path(ctx);
+	}
+	fimc_hw_set_input_addr(fimc, &sf->paddr);
+	fimc_hw_set_output_addr(fimc, &df->paddr, -1);
+
+	fimc_activate_capture(ctx);
+	ctx->state &= (FIMC_CTX_M2M | FIMC_CTX_CAP |
+		       FIMC_SRC_FMT | FIMC_DST_FMT);
+	fimc_hw_activate_input_dma(fimc, true);
+
+dma_unlock:
+	spin_unlock_irqrestore(&fimc->slock, flags);
+}
+
+static void fimc_job_abort(void *priv)
+{
+	fimc_m2m_shutdown(priv);
+}
+
+static int fimc_queue_setup(struct vb2_queue *vq, const struct v4l2_format *fmt,
+			    unsigned int *num_buffers, unsigned int *num_planes,
+			    unsigned int sizes[], void *allocators[])
+{
+	struct fimc_ctx *ctx = vb2_get_drv_priv(vq);
+	struct fimc_frame *f;
+	int i;
+
+	f = ctx_get_frame(ctx, vq->type);
+	if (IS_ERR(f))
+		return PTR_ERR(f);
+	/*
+	 * Return number of non-contigous planes (plane buffers)
+	 * depending on the configured color format.
+	 */
+	if (!f->fmt)
+		return -EINVAL;
+
+	*num_planes = f->fmt->memplanes;
+	for (i = 0; i < f->fmt->memplanes; i++) {
+		sizes[i] = (f->f_width * f->f_height * f->fmt->depth[i]) / 8;
+		allocators[i] = ctx->fimc_dev->alloc_ctx;
+	}
+	return 0;
+}
+
+static int fimc_buf_prepare(struct vb2_buffer *vb)
+{
+	struct fimc_ctx *ctx = vb2_get_drv_priv(vb->vb2_queue);
+	struct fimc_frame *frame;
+	int i;
+
+	frame = ctx_get_frame(ctx, vb->vb2_queue->type);
+	if (IS_ERR(frame))
+		return PTR_ERR(frame);
+
+	for (i = 0; i < frame->fmt->memplanes; i++)
+		vb2_set_plane_payload(vb, i, frame->payload[i]);
+
+	return 0;
+}
+
+static void fimc_buf_queue(struct vb2_buffer *vb)
+{
+	struct fimc_ctx *ctx = vb2_get_drv_priv(vb->vb2_queue);
+
+	dbg("ctx: %p, ctx->state: 0x%x", ctx, ctx->state);
+
+	if (ctx->m2m_ctx)
+		v4l2_m2m_buf_queue(ctx->m2m_ctx, vb);
+}
+
+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_qops = {
+	.queue_setup	 = fimc_queue_setup,
+	.buf_prepare	 = fimc_buf_prepare,
+	.buf_queue	 = fimc_buf_queue,
+	.wait_prepare	 = fimc_unlock,
+	.wait_finish	 = fimc_lock,
+	.stop_streaming	 = stop_streaming,
+	.start_streaming = start_streaming,
+};
+
+/*
+ * V4L2 ioctl handlers
+ */
+static int fimc_m2m_querycap(struct file *file, void *fh,
+			     struct v4l2_capability *cap)
+{
+	struct fimc_ctx *ctx = fh_to_ctx(fh);
+	struct fimc_dev *fimc = ctx->fimc_dev;
+
+	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 | V4L2_CAP_VIDEO_OUTPUT_MPLANE;
+
+	return 0;
+}
+
+static int fimc_m2m_enum_fmt_mplane(struct file *file, void *priv,
+				    struct v4l2_fmtdesc *f)
+{
+	struct fimc_fmt *fmt;
+
+	fmt = fimc_find_format(NULL, NULL, get_m2m_fmt_flags(f->type),
+			       f->index);
+	if (!fmt)
+		return -EINVAL;
+
+	strncpy(f->description, fmt->name, sizeof(f->description) - 1);
+	f->pixelformat = fmt->fourcc;
+	return 0;
+}
+
+static int fimc_m2m_g_fmt_mplane(struct file *file, void *fh,
+				 struct v4l2_format *f)
+{
+	struct fimc_ctx *ctx = fh_to_ctx(fh);
+	struct fimc_frame *frame = ctx_get_frame(ctx, f->type);
+
+	if (IS_ERR(frame))
+		return PTR_ERR(frame);
+
+	return fimc_fill_format(frame, f);
+}
+
+static int fimc_try_fmt_mplane(struct fimc_ctx *ctx, struct v4l2_format *f)
+{
+	struct fimc_dev *fimc = ctx->fimc_dev;
+	struct fimc_variant *variant = fimc->variant;
+	struct v4l2_pix_format_mplane *pix = &f->fmt.pix_mp;
+	struct fimc_fmt *fmt;
+	u32 max_w, mod_x, mod_y;
+
+	if (!IS_M2M(f->type))
+		return -EINVAL;
+
+	dbg("w: %d, h: %d", pix->width, pix->height);
+
+	fmt = fimc_find_format(&pix->pixelformat, NULL,
+			       get_m2m_fmt_flags(f->type), 0);
+	if (WARN(fmt == NULL, "Pixel format lookup failed"))
+		return -EINVAL;
+
+	if (pix->field == V4L2_FIELD_ANY)
+		pix->field = V4L2_FIELD_NONE;
+	else if (pix->field != V4L2_FIELD_NONE)
+		return -EINVAL;
+
+	if (f->type == V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE) {
+		max_w = variant->pix_limit->scaler_dis_w;
+		mod_x = ffs(variant->min_inp_pixsize) - 1;
+	} else {
+		max_w = variant->pix_limit->out_rot_dis_w;
+		mod_x = ffs(variant->min_out_pixsize) - 1;
+	}
+
+	if (tiled_fmt(fmt)) {
+		mod_x = 6; /* 64 x 32 pixels tile */
+		mod_y = 5;
+	} else {
+		if (variant->min_vsize_align == 1)
+			mod_y = fimc_fmt_is_rgb(fmt->color) ? 0 : 1;
+		else
+			mod_y = ffs(variant->min_vsize_align) - 1;
+	}
+
+	v4l_bound_align_image(&pix->width, 16, max_w, mod_x,
+		&pix->height, 8, variant->pix_limit->scaler_dis_w, mod_y, 0);
+
+	fimc_adjust_mplane_format(fmt, pix->width, pix->height, &f->fmt.pix_mp);
+	return 0;
+}
+
+static int fimc_m2m_try_fmt_mplane(struct file *file, void *fh,
+				   struct v4l2_format *f)
+{
+	struct fimc_ctx *ctx = fh_to_ctx(fh);
+
+	return fimc_try_fmt_mplane(ctx, f);
+}
+
+static int fimc_m2m_s_fmt_mplane(struct file *file, void *fh,
+				 struct v4l2_format *f)
+{
+	struct fimc_ctx *ctx = fh_to_ctx(fh);
+	struct fimc_dev *fimc = ctx->fimc_dev;
+	struct vb2_queue *vq;
+	struct fimc_frame *frame;
+	struct v4l2_pix_format_mplane *pix;
+	int i, ret = 0;
+
+	ret = fimc_try_fmt_mplane(ctx, f);
+	if (ret)
+		return ret;
+
+	vq = v4l2_m2m_get_vq(ctx->m2m_ctx, f->type);
+
+	if (vb2_is_busy(vq)) {
+		v4l2_err(fimc->m2m.vfd, "queue (%d) busy\n", f->type);
+		return -EBUSY;
+	}
+
+	if (f->type == V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE)
+		frame = &ctx->s_frame;
+	else
+		frame = &ctx->d_frame;
+
+	pix = &f->fmt.pix_mp;
+	frame->fmt = fimc_find_format(&pix->pixelformat, NULL,
+				      get_m2m_fmt_flags(f->type), 0);
+	if (!frame->fmt)
+		return -EINVAL;
+
+	/* Update RGB Alpha control state and value range */
+	fimc_alpha_ctrl_update(ctx);
+
+	for (i = 0; i < frame->fmt->colplanes; i++) {
+		frame->payload[i] =
+			(pix->width * pix->height * frame->fmt->depth[i]) / 8;
+	}
+
+	fimc_fill_frame(frame, f);
+
+	ctx->scaler.enabled = 1;
+
+	if (f->type == V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE)
+		fimc_ctx_state_set(FIMC_PARAMS | FIMC_DST_FMT, ctx);
+	else
+		fimc_ctx_state_set(FIMC_PARAMS | FIMC_SRC_FMT, ctx);
+
+	dbg("f_w: %d, f_h: %d", frame->f_width, frame->f_height);
+
+	return 0;
+}
+
+static int fimc_m2m_reqbufs(struct file *file, void *fh,
+			    struct v4l2_requestbuffers *reqbufs)
+{
+	struct fimc_ctx *ctx = fh_to_ctx(fh);
+
+	return v4l2_m2m_reqbufs(file, ctx->m2m_ctx, reqbufs);
+}
+
+static int fimc_m2m_querybuf(struct file *file, void *fh,
+			     struct v4l2_buffer *buf)
+{
+	struct fimc_ctx *ctx = fh_to_ctx(fh);
+
+	return v4l2_m2m_querybuf(file, ctx->m2m_ctx, buf);
+}
+
+static int fimc_m2m_qbuf(struct file *file, void *fh,
+			 struct v4l2_buffer *buf)
+{
+	struct fimc_ctx *ctx = fh_to_ctx(fh);
+
+	return v4l2_m2m_qbuf(file, ctx->m2m_ctx, buf);
+}
+
+static int fimc_m2m_dqbuf(struct file *file, void *fh,
+			  struct v4l2_buffer *buf)
+{
+	struct fimc_ctx *ctx = fh_to_ctx(fh);
+
+	return v4l2_m2m_dqbuf(file, ctx->m2m_ctx, buf);
+}
+
+static int fimc_m2m_streamon(struct file *file, void *fh,
+			     enum v4l2_buf_type type)
+{
+	struct fimc_ctx *ctx = fh_to_ctx(fh);
+
+	/* The source and target color format need to be set */
+	if (V4L2_TYPE_IS_OUTPUT(type)) {
+		if (!fimc_ctx_state_is_set(FIMC_SRC_FMT, ctx))
+			return -EINVAL;
+	} else if (!fimc_ctx_state_is_set(FIMC_DST_FMT, ctx)) {
+		return -EINVAL;
+	}
+
+	return v4l2_m2m_streamon(file, ctx->m2m_ctx, type);
+}
+
+static int fimc_m2m_streamoff(struct file *file, void *fh,
+			    enum v4l2_buf_type type)
+{
+	struct fimc_ctx *ctx = fh_to_ctx(fh);
+
+	return v4l2_m2m_streamoff(file, ctx->m2m_ctx, type);
+}
+
+static int fimc_m2m_cropcap(struct file *file, void *fh,
+			    struct v4l2_cropcap *cr)
+{
+	struct fimc_ctx *ctx = fh_to_ctx(fh);
+	struct fimc_frame *frame;
+
+	frame = ctx_get_frame(ctx, cr->type);
+	if (IS_ERR(frame))
+		return PTR_ERR(frame);
+
+	cr->bounds.left = 0;
+	cr->bounds.top = 0;
+	cr->bounds.width = frame->o_width;
+	cr->bounds.height = frame->o_height;
+	cr->defrect = cr->bounds;
+
+	return 0;
+}
+
+static int fimc_m2m_g_crop(struct file *file, void *fh, struct v4l2_crop *cr)
+{
+	struct fimc_ctx *ctx = fh_to_ctx(fh);
+	struct fimc_frame *frame;
+
+	frame = ctx_get_frame(ctx, cr->type);
+	if (IS_ERR(frame))
+		return PTR_ERR(frame);
+
+	cr->c.left = frame->offs_h;
+	cr->c.top = frame->offs_v;
+	cr->c.width = frame->width;
+	cr->c.height = frame->height;
+
+	return 0;
+}
+
+static int fimc_m2m_try_crop(struct fimc_ctx *ctx, struct v4l2_crop *cr)
+{
+	struct fimc_dev *fimc = ctx->fimc_dev;
+	struct fimc_frame *f;
+	u32 min_size, halign, depth = 0;
+	int i;
+
+	if (cr->c.top < 0 || cr->c.left < 0) {
+		v4l2_err(fimc->m2m.vfd,
+			"doesn't support negative values for top & left\n");
+		return -EINVAL;
+	}
+	if (cr->type == V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE)
+		f = &ctx->d_frame;
+	else if (cr->type == V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE)
+		f = &ctx->s_frame;
+	else
+		return -EINVAL;
+
+	min_size = (f == &ctx->s_frame) ?
+		fimc->variant->min_inp_pixsize : fimc->variant->min_out_pixsize;
+
+	/* Get pixel alignment constraints. */
+	if (fimc->variant->min_vsize_align == 1)
+		halign = fimc_fmt_is_rgb(f->fmt->color) ? 0 : 1;
+	else
+		halign = ffs(fimc->variant->min_vsize_align) - 1;
+
+	for (i = 0; i < f->fmt->colplanes; i++)
+		depth += f->fmt->depth[i];
+
+	v4l_bound_align_image(&cr->c.width, min_size, f->o_width,
+			      ffs(min_size) - 1,
+			      &cr->c.height, min_size, f->o_height,
+			      halign, 64/(ALIGN(depth, 8)));
+
+	/* adjust left/top if cropping rectangle is out of bounds */
+	if (cr->c.left + cr->c.width > f->o_width)
+		cr->c.left = f->o_width - cr->c.width;
+	if (cr->c.top + cr->c.height > f->o_height)
+		cr->c.top = f->o_height - cr->c.height;
+
+	cr->c.left = round_down(cr->c.left, min_size);
+	cr->c.top  = round_down(cr->c.top, fimc->variant->hor_offs_align);
+
+	dbg("l:%d, t:%d, w:%d, h:%d, f_w: %d, f_h: %d",
+	    cr->c.left, cr->c.top, cr->c.width, cr->c.height,
+	    f->f_width, f->f_height);
+
+	return 0;
+}
+
+static int fimc_m2m_s_crop(struct file *file, void *fh, struct v4l2_crop *cr)
+{
+	struct fimc_ctx *ctx = fh_to_ctx(fh);
+	struct fimc_dev *fimc = ctx->fimc_dev;
+	struct fimc_frame *f;
+	int ret;
+
+	ret = fimc_m2m_try_crop(ctx, cr);
+	if (ret)
+		return ret;
+
+	f = (cr->type == V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE) ?
+		&ctx->s_frame : &ctx->d_frame;
+
+	/* Check to see if scaling ratio is within supported range */
+	if (fimc_ctx_state_is_set(FIMC_DST_FMT | FIMC_SRC_FMT, ctx)) {
+		if (cr->type == V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE) {
+			ret = fimc_check_scaler_ratio(ctx, cr->c.width,
+					cr->c.height, ctx->d_frame.width,
+					ctx->d_frame.height, ctx->rotation);
+		} else {
+			ret = fimc_check_scaler_ratio(ctx, ctx->s_frame.width,
+					ctx->s_frame.height, cr->c.width,
+					cr->c.height, ctx->rotation);
+		}
+		if (ret) {
+			v4l2_err(fimc->m2m.vfd, "Out of scaler range\n");
+			return -EINVAL;
+		}
+	}
+
+	f->offs_h = cr->c.left;
+	f->offs_v = cr->c.top;
+	f->width  = cr->c.width;
+	f->height = cr->c.height;
+
+	fimc_ctx_state_set(FIMC_PARAMS, ctx);
+
+	return 0;
+}
+
+static const struct v4l2_ioctl_ops fimc_m2m_ioctl_ops = {
+	.vidioc_querycap		= fimc_m2m_querycap,
+	.vidioc_enum_fmt_vid_cap_mplane	= fimc_m2m_enum_fmt_mplane,
+	.vidioc_enum_fmt_vid_out_mplane	= fimc_m2m_enum_fmt_mplane,
+	.vidioc_g_fmt_vid_cap_mplane	= fimc_m2m_g_fmt_mplane,
+	.vidioc_g_fmt_vid_out_mplane	= fimc_m2m_g_fmt_mplane,
+	.vidioc_try_fmt_vid_cap_mplane	= fimc_m2m_try_fmt_mplane,
+	.vidioc_try_fmt_vid_out_mplane	= fimc_m2m_try_fmt_mplane,
+	.vidioc_s_fmt_vid_cap_mplane	= fimc_m2m_s_fmt_mplane,
+	.vidioc_s_fmt_vid_out_mplane	= fimc_m2m_s_fmt_mplane,
+	.vidioc_reqbufs			= fimc_m2m_reqbufs,
+	.vidioc_querybuf		= fimc_m2m_querybuf,
+	.vidioc_qbuf			= fimc_m2m_qbuf,
+	.vidioc_dqbuf			= fimc_m2m_dqbuf,
+	.vidioc_streamon		= fimc_m2m_streamon,
+	.vidioc_streamoff		= fimc_m2m_streamoff,
+	.vidioc_g_crop			= fimc_m2m_g_crop,
+	.vidioc_s_crop			= fimc_m2m_s_crop,
+	.vidioc_cropcap			= fimc_m2m_cropcap
+
+};
+
+static int queue_init(void *priv, struct vb2_queue *src_vq,
+		      struct vb2_queue *dst_vq)
+{
+	struct fimc_ctx *ctx = priv;
+	int ret;
+
+	memset(src_vq, 0, sizeof(*src_vq));
+	src_vq->type = V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE;
+	src_vq->io_modes = VB2_MMAP | VB2_USERPTR;
+	src_vq->drv_priv = ctx;
+	src_vq->ops = &fimc_qops;
+	src_vq->mem_ops = &vb2_dma_contig_memops;
+	src_vq->buf_struct_size = sizeof(struct v4l2_m2m_buffer);
+
+	ret = vb2_queue_init(src_vq);
+	if (ret)
+		return ret;
+
+	memset(dst_vq, 0, sizeof(*dst_vq));
+	dst_vq->type = V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE;
+	dst_vq->io_modes = VB2_MMAP | VB2_USERPTR;
+	dst_vq->drv_priv = ctx;
+	dst_vq->ops = &fimc_qops;
+	dst_vq->mem_ops = &vb2_dma_contig_memops;
+	dst_vq->buf_struct_size = sizeof(struct v4l2_m2m_buffer);
+
+	return vb2_queue_init(dst_vq);
+}
+
+static int fimc_m2m_open(struct file *file)
+{
+	struct fimc_dev *fimc = video_drvdata(file);
+	struct fimc_ctx *ctx;
+	int ret;
+
+	dbg("pid: %d, state: 0x%lx, refcnt: %d",
+	    task_pid_nr(current), fimc->state, fimc->vid_cap.refcnt);
+
+	/*
+	 * Return if the corresponding video capture node
+	 * is already opened.
+	 */
+	if (fimc->vid_cap.refcnt > 0)
+		return -EBUSY;
+
+	ctx = kzalloc(sizeof *ctx, GFP_KERNEL);
+	if (!ctx)
+		return -ENOMEM;
+	v4l2_fh_init(&ctx->fh, fimc->m2m.vfd);
+	ctx->fimc_dev = fimc;
+
+	/* Default color format */
+	ctx->s_frame.fmt = fimc_get_format(0);
+	ctx->d_frame.fmt = fimc_get_format(0);
+
+	ret = fimc_ctrls_create(ctx);
+	if (ret)
+		goto error_fh;
+
+	/* Use separate control handler per file handle */
+	ctx->fh.ctrl_handler = &ctx->ctrls.handler;
+	file->private_data = &ctx->fh;
+	v4l2_fh_add(&ctx->fh);
+
+	/* Setup the device context for memory-to-memory mode */
+	ctx->state = FIMC_CTX_M2M;
+	ctx->flags = 0;
+	ctx->in_path = FIMC_IO_DMA;
+	ctx->out_path = FIMC_IO_DMA;
+
+	ctx->m2m_ctx = v4l2_m2m_ctx_init(fimc->m2m.m2m_dev, ctx, queue_init);
+	if (IS_ERR(ctx->m2m_ctx)) {
+		ret = PTR_ERR(ctx->m2m_ctx);
+		goto error_c;
+	}
+
+	if (fimc->m2m.refcnt++ == 0)
+		set_bit(ST_M2M_RUN, &fimc->state);
+	return 0;
+
+error_c:
+	fimc_ctrls_delete(ctx);
+error_fh:
+	v4l2_fh_del(&ctx->fh);
+	v4l2_fh_exit(&ctx->fh);
+	kfree(ctx);
+	return ret;
+}
+
+static int fimc_m2m_release(struct file *file)
+{
+	struct fimc_ctx *ctx = fh_to_ctx(file->private_data);
+	struct fimc_dev *fimc = ctx->fimc_dev;
+
+	dbg("pid: %d, state: 0x%lx, refcnt= %d",
+		task_pid_nr(current), fimc->state, fimc->m2m.refcnt);
+
+	v4l2_m2m_ctx_release(ctx->m2m_ctx);
+	fimc_ctrls_delete(ctx);
+	v4l2_fh_del(&ctx->fh);
+	v4l2_fh_exit(&ctx->fh);
+
+	if (--fimc->m2m.refcnt <= 0)
+		clear_bit(ST_M2M_RUN, &fimc->state);
+	kfree(ctx);
+	return 0;
+}
+
+static unsigned int fimc_m2m_poll(struct file *file,
+				  struct poll_table_struct *wait)
+{
+	struct fimc_ctx *ctx = fh_to_ctx(file->private_data);
+
+	return v4l2_m2m_poll(file, ctx->m2m_ctx, wait);
+}
+
+
+static int fimc_m2m_mmap(struct file *file, struct vm_area_struct *vma)
+{
+	struct fimc_ctx *ctx = fh_to_ctx(file->private_data);
+
+	return v4l2_m2m_mmap(file, ctx->m2m_ctx, vma);
+}
+
+static const struct v4l2_file_operations fimc_m2m_fops = {
+	.owner		= THIS_MODULE,
+	.open		= fimc_m2m_open,
+	.release	= fimc_m2m_release,
+	.poll		= fimc_m2m_poll,
+	.unlocked_ioctl	= video_ioctl2,
+	.mmap		= fimc_m2m_mmap,
+};
+
+static struct v4l2_m2m_ops m2m_ops = {
+	.device_run	= fimc_device_run,
+	.job_abort	= fimc_job_abort,
+};
+
+int fimc_register_m2m_device(struct fimc_dev *fimc,
+			     struct v4l2_device *v4l2_dev)
+{
+	struct video_device *vfd;
+	struct platform_device *pdev;
+	int ret = 0;
+
+	if (!fimc)
+		return -ENODEV;
+
+	pdev = fimc->pdev;
+	fimc->v4l2_dev = v4l2_dev;
+
+	vfd = video_device_alloc();
+	if (!vfd) {
+		v4l2_err(v4l2_dev, "Failed to allocate video device\n");
+		return -ENOMEM;
+	}
+
+	vfd->fops = &fimc_m2m_fops;
+	vfd->ioctl_ops = &fimc_m2m_ioctl_ops;
+	vfd->v4l2_dev = v4l2_dev;
+	vfd->minor = -1;
+	vfd->release = video_device_release;
+	vfd->lock = &fimc->lock;
+	/* Locking in file operations other than ioctl should be done
+	   by the driver, not the V4L2 core.
+	   This driver needs auditing so that this flag can be removed. */
+	set_bit(V4L2_FL_LOCK_ALL_FOPS, &vfd->flags);
+
+	snprintf(vfd->name, sizeof(vfd->name), "fimc.%d.m2m", fimc->id);
+	video_set_drvdata(vfd, fimc);
+
+	fimc->m2m.vfd = vfd;
+	fimc->m2m.m2m_dev = v4l2_m2m_init(&m2m_ops);
+	if (IS_ERR(fimc->m2m.m2m_dev)) {
+		v4l2_err(v4l2_dev, "failed to initialize v4l2-m2m device\n");
+		ret = PTR_ERR(fimc->m2m.m2m_dev);
+		goto err_init;
+	}
+
+	ret = media_entity_init(&vfd->entity, 0, NULL, 0);
+	if (ret)
+		goto err_me;
+
+	ret = video_register_device(vfd, VFL_TYPE_GRABBER, -1);
+	if (ret)
+		goto err_vd;
+
+	v4l2_info(v4l2_dev, "Registered %s as /dev/%s\n",
+		  vfd->name, video_device_node_name(vfd));
+	return 0;
+
+err_vd:
+	media_entity_cleanup(&vfd->entity);
+err_me:
+	v4l2_m2m_release(fimc->m2m.m2m_dev);
+err_init:
+	video_device_release(fimc->m2m.vfd);
+	return ret;
+}
+
+void fimc_unregister_m2m_device(struct fimc_dev *fimc)
+{
+	if (!fimc)
+		return;
+
+	if (fimc->m2m.m2m_dev)
+		v4l2_m2m_release(fimc->m2m.m2m_dev);
+	if (fimc->m2m.vfd) {
+		media_entity_cleanup(&fimc->m2m.vfd->entity);
+		/* Can also be called if video device wasn't registered */
+		video_unregister_device(fimc->m2m.vfd);
+	}
+}
diff --git a/drivers/media/video/s5p-fimc/fimc-mdevice.c b/drivers/media/video/s5p-fimc/fimc-mdevice.c
index 62ed37e4014..6753c45631b 100644
--- a/drivers/media/video/s5p-fimc/fimc-mdevice.c
+++ b/drivers/media/video/s5p-fimc/fimc-mdevice.c
@@ -25,6 +25,7 @@
 #include <media/media-device.h>
 
 #include "fimc-core.h"
+#include "fimc-lite.h"
 #include "fimc-mdevice.h"
 #include "mipi-csis.h"
 
@@ -37,22 +38,46 @@ static int __fimc_md_set_camclk(struct fimc_md *fmd,
  *
  * Caller holds the graph mutex.
  */
-void fimc_pipeline_prepare(struct fimc_dev *fimc, struct media_entity *me)
+void fimc_pipeline_prepare(struct fimc_pipeline *p, struct media_entity *me)
 {
-	struct media_entity_graph graph;
+	struct media_pad *pad = &me->pads[0];
 	struct v4l2_subdev *sd;
+	int i;
 
-	media_entity_graph_walk_start(&graph, me);
+	for (i = 0; i < IDX_MAX; i++)
+		p->subdevs[i] = NULL;
 
-	while ((me = media_entity_graph_walk_next(&graph))) {
-		if (media_entity_type(me) != MEDIA_ENT_T_V4L2_SUBDEV)
-			continue;
-		sd = media_entity_to_v4l2_subdev(me);
+	while (1) {
+		if (!(pad->flags & MEDIA_PAD_FL_SINK))
+			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);
 
-		if (sd->grp_id == SENSOR_GROUP_ID)
-			fimc->pipeline.sensor = sd;
-		else if (sd->grp_id == CSIS_GROUP_ID)
-			fimc->pipeline.csis = sd;
+		switch (sd->grp_id) {
+		case SENSOR_GROUP_ID:
+			p->subdevs[IDX_SENSOR] = sd;
+			break;
+		case CSIS_GROUP_ID:
+			p->subdevs[IDX_CSIS] = sd;
+			break;
+		case FLITE_GROUP_ID:
+			p->subdevs[IDX_FLITE] = sd;
+			break;
+		case FIMC_GROUP_ID:
+			/* No need to control FIMC subdev through subdev ops */
+			break;
+		default:
+			pr_warn("%s: Unknown subdev grp_id: %#x\n",
+				__func__, sd->grp_id);
+		}
+		/* sink pad */
+		pad = &sd->entity.pads[0];
 	}
 }
 
@@ -85,30 +110,27 @@ static int __subdev_set_power(struct v4l2_subdev *sd, int on)
 /**
  * fimc_pipeline_s_power - change power state of all pipeline subdevs
  * @fimc: fimc device terminating the pipeline
- * @state: 1 to enable power or 0 for power down
+ * @state: true to power on, false to power off
  *
- * Need to be called with the graph mutex held.
+ * Needs to be called with the graph mutex held.
  */
-int fimc_pipeline_s_power(struct fimc_dev *fimc, int state)
+int fimc_pipeline_s_power(struct fimc_pipeline *p, bool state)
 {
-	int ret = 0;
+	unsigned int i;
+	int ret;
 
-	if (fimc->pipeline.sensor == NULL)
+	if (p->subdevs[IDX_SENSOR] == NULL)
 		return -ENXIO;
 
-	if (state) {
-		ret = __subdev_set_power(fimc->pipeline.csis, 1);
-		if (ret && ret != -ENXIO)
+	for (i = 0; i < IDX_MAX; i++) {
+		unsigned int idx = state ? (IDX_MAX - 1) - i : i;
+
+		ret = __subdev_set_power(p->subdevs[idx], state);
+		if (ret < 0 && ret != -ENXIO)
 			return ret;
-		return __subdev_set_power(fimc->pipeline.sensor, 1);
 	}
 
-	ret = __subdev_set_power(fimc->pipeline.sensor, 0);
-	if (ret)
-		return ret;
-	ret = __subdev_set_power(fimc->pipeline.csis, 0);
-
-	return ret == -ENXIO ? 0 : ret;
+	return 0;
 }
 
 /**
@@ -119,32 +141,36 @@ int fimc_pipeline_s_power(struct fimc_dev *fimc, int state)
  *
  * This function must be called with the graph mutex held.
  */
-static int __fimc_pipeline_initialize(struct fimc_dev *fimc,
+static int __fimc_pipeline_initialize(struct fimc_pipeline *p,
 				      struct media_entity *me, bool prep)
 {
 	int ret;
 
 	if (prep)
-		fimc_pipeline_prepare(fimc, me);
-	if (fimc->pipeline.sensor == NULL)
+		fimc_pipeline_prepare(p, me);
+
+	if (p->subdevs[IDX_SENSOR] == NULL)
 		return -EINVAL;
-	ret = fimc_md_set_camclk(fimc->pipeline.sensor, true);
+
+	ret = fimc_md_set_camclk(p->subdevs[IDX_SENSOR], true);
 	if (ret)
 		return ret;
-	return fimc_pipeline_s_power(fimc, 1);
+
+	return fimc_pipeline_s_power(p, 1);
 }
 
-int fimc_pipeline_initialize(struct fimc_dev *fimc, struct media_entity *me,
+int fimc_pipeline_initialize(struct fimc_pipeline *p, struct media_entity *me,
 			     bool prep)
 {
 	int ret;
 
 	mutex_lock(&me->parent->graph_mutex);
-	ret =  __fimc_pipeline_initialize(fimc, me, prep);
+	ret =  __fimc_pipeline_initialize(p, me, prep);
 	mutex_unlock(&me->parent->graph_mutex);
 
 	return ret;
 }
+EXPORT_SYMBOL_GPL(fimc_pipeline_initialize);
 
 /**
  * __fimc_pipeline_shutdown - disable the sensor clock and pipeline power
@@ -154,52 +180,55 @@ int fimc_pipeline_initialize(struct fimc_dev *fimc, struct media_entity *me,
  * sensor clock.
  * Called with the graph mutex held.
  */
-int __fimc_pipeline_shutdown(struct fimc_dev *fimc)
+int __fimc_pipeline_shutdown(struct fimc_pipeline *p)
 {
 	int ret = 0;
 
-	if (fimc->pipeline.sensor) {
-		ret = fimc_pipeline_s_power(fimc, 0);
-		fimc_md_set_camclk(fimc->pipeline.sensor, false);
+	if (p->subdevs[IDX_SENSOR]) {
+		ret = fimc_pipeline_s_power(p, 0);
+		fimc_md_set_camclk(p->subdevs[IDX_SENSOR], false);
 	}
 	return ret == -ENXIO ? 0 : ret;
 }
 
-int fimc_pipeline_shutdown(struct fimc_dev *fimc)
+int fimc_pipeline_shutdown(struct fimc_pipeline *p)
 {
-	struct media_entity *me = &fimc->vid_cap.vfd->entity;
+	struct media_entity *me = &p->subdevs[IDX_SENSOR]->entity;
 	int ret;
 
 	mutex_lock(&me->parent->graph_mutex);
-	ret = __fimc_pipeline_shutdown(fimc);
+	ret = __fimc_pipeline_shutdown(p);
 	mutex_unlock(&me->parent->graph_mutex);
 
 	return ret;
 }
+EXPORT_SYMBOL_GPL(fimc_pipeline_shutdown);
 
 /**
  * fimc_pipeline_s_stream - invoke s_stream on pipeline subdevs
- * @fimc: fimc device terminating the pipeline
+ * @pipeline: video pipeline structure
  * @on: passed as the s_stream call argument
  */
-int fimc_pipeline_s_stream(struct fimc_dev *fimc, int on)
+int fimc_pipeline_s_stream(struct fimc_pipeline *p, bool on)
 {
-	struct fimc_pipeline *p = &fimc->pipeline;
-	int ret = 0;
+	int i, ret;
 
-	if (p->sensor == NULL)
+	if (p->subdevs[IDX_SENSOR] == NULL)
 		return -ENODEV;
 
-	if ((on && p->csis) || !on)
-		ret = v4l2_subdev_call(on ? p->csis : p->sensor,
-				       video, s_stream, on);
-	if (ret < 0 && ret != -ENOIOCTLCMD)
-		return ret;
-	if ((!on && p->csis) || on)
-		ret = v4l2_subdev_call(on ? p->sensor : p->csis,
-				       video, s_stream, on);
-	return ret == -ENOIOCTLCMD ? 0 : ret;
+	for (i = 0; i < IDX_MAX; i++) {
+		unsigned int idx = on ? (IDX_MAX - 1) - i : i;
+
+		ret = v4l2_subdev_call(p->subdevs[idx], video, s_stream, on);
+
+		if (ret < 0 && ret != -ENOIOCTLCMD && ret != -ENODEV)
+			return ret;
+	}
+
+	return 0;
+
 }
+EXPORT_SYMBOL_GPL(fimc_pipeline_s_stream);
 
 /*
  * Sensor subdevice helper functions
@@ -214,14 +243,20 @@ static struct v4l2_subdev *fimc_md_register_sensor(struct fimc_md *fmd,
 		return NULL;
 
 	adapter = i2c_get_adapter(s_info->pdata->i2c_bus_num);
-	if (!adapter)
-		return NULL;
+	if (!adapter) {
+		v4l2_warn(&fmd->v4l2_dev,
+			  "Failed to get I2C adapter %d, deferring probe\n",
+			  s_info->pdata->i2c_bus_num);
+		return ERR_PTR(-EPROBE_DEFER);
+	}
 	sd = v4l2_i2c_new_subdev_board(&fmd->v4l2_dev, adapter,
 				       s_info->pdata->board_info, NULL);
 	if (IS_ERR_OR_NULL(sd)) {
 		i2c_put_adapter(adapter);
-		v4l2_err(&fmd->v4l2_dev, "Failed to acquire subdev\n");
-		return NULL;
+		v4l2_warn(&fmd->v4l2_dev,
+			  "Failed to acquire subdev %s, deferring probe\n",
+			  s_info->pdata->board_info->type);
+		return ERR_PTR(-EPROBE_DEFER);
 	}
 	v4l2_set_subdev_hostdata(sd, s_info);
 	sd->grp_id = SENSOR_GROUP_ID;
@@ -269,13 +304,22 @@ static int fimc_md_register_sensor_entities(struct fimc_md *fmd)
 
 	fmd->num_sensors = num_clients;
 	for (i = 0; i < num_clients; i++) {
+		struct v4l2_subdev *sd;
+
 		fmd->sensor[i].pdata = &pdata->isp_info[i];
 		ret = __fimc_md_set_camclk(fmd, &fmd->sensor[i], true);
 		if (ret)
 			break;
-		fmd->sensor[i].subdev =
-			fimc_md_register_sensor(fmd, &fmd->sensor[i]);
+		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 (ret)
 			break;
 	}
@@ -289,21 +333,50 @@ static int fimc_md_register_sensor_entities(struct fimc_md *fmd)
 static int fimc_register_callback(struct device *dev, void *p)
 {
 	struct fimc_dev *fimc = dev_get_drvdata(dev);
+	struct v4l2_subdev *sd = &fimc->vid_cap.subdev;
 	struct fimc_md *fmd = p;
-	int ret;
+	int ret = 0;
 
 	if (!fimc || !fimc->pdev)
 		return 0;
+
 	if (fimc->pdev->id < 0 || fimc->pdev->id >= FIMC_MAX_DEVS)
 		return 0;
 
 	fmd->fimc[fimc->pdev->id] = fimc;
-	ret = fimc_register_m2m_device(fimc, &fmd->v4l2_dev);
-	if (ret)
-		return ret;
-	ret = fimc_register_capture_device(fimc, &fmd->v4l2_dev);
-	if (!ret)
-		fimc->vid_cap.user_subdev_api = fmd->user_subdev_api;
+	sd->grp_id = FIMC_GROUP_ID;
+
+	ret = v4l2_device_register_subdev(&fmd->v4l2_dev, sd);
+	if (ret) {
+		v4l2_err(&fmd->v4l2_dev, "Failed to register FIMC.%d (%d)\n",
+			 fimc->id, ret);
+	}
+
+	return ret;
+}
+
+static int fimc_lite_register_callback(struct device *dev, void *p)
+{
+	struct fimc_lite *fimc = dev_get_drvdata(dev);
+	struct v4l2_subdev *sd = &fimc->subdev;
+	struct fimc_md *fmd = p;
+	int ret;
+
+	if (fimc == NULL)
+		return 0;
+
+	if (fimc->index >= FIMC_LITE_MAX_DEVS)
+		return 0;
+
+	fmd->fimc_lite[fimc->index] = fimc;
+	sd->grp_id = FLITE_GROUP_ID;
+
+	ret = v4l2_device_register_subdev(&fmd->v4l2_dev, sd);
+	if (ret) {
+		v4l2_err(&fmd->v4l2_dev,
+			 "Failed to register FIMC-LITE.%d (%d)\n",
+			 fimc->index, ret);
+	}
 	return ret;
 }
 
@@ -336,22 +409,56 @@ static int csis_register_callback(struct device *dev, void *p)
  */
 static int fimc_md_register_platform_entities(struct fimc_md *fmd)
 {
+	struct s5p_platform_fimc *pdata = fmd->pdev->dev.platform_data;
 	struct device_driver *driver;
-	int ret;
+	int ret, i;
 
 	driver = driver_find(FIMC_MODULE_NAME, &platform_bus_type);
-	if (!driver)
-		return -ENODEV;
+	if (!driver) {
+		v4l2_warn(&fmd->v4l2_dev,
+			 "%s driver not found, deffering probe\n",
+			 FIMC_MODULE_NAME);
+		return -EPROBE_DEFER;
+	}
+
 	ret = driver_for_each_device(driver, NULL, fmd,
 				     fimc_register_callback);
 	if (ret)
 		return ret;
 
-	driver = driver_find(CSIS_DRIVER_NAME, &platform_bus_type);
-	if (driver)
+	driver = driver_find(FIMC_LITE_DRV_NAME, &platform_bus_type);
+	if (driver && try_module_get(driver->owner)) {
 		ret = driver_for_each_device(driver, NULL, fmd,
-					     csis_register_callback);
-	return ret;
+					     fimc_lite_register_callback);
+		if (ret)
+			return ret;
+		module_put(driver->owner);
+	}
+	/*
+	 * Check if there is any sensor on the MIPI-CSI2 bus and
+	 * if not skip the s5p-csis module loading.
+	 */
+	if (pdata == NULL)
+		return 0;
+	for (i = 0; i < pdata->num_clients; i++) {
+		if (pdata->isp_info[i].bus_type == FIMC_MIPI_CSI2) {
+			ret = 1;
+			break;
+		}
+	}
+	if (!ret)
+		return 0;
+
+	driver = driver_find(CSIS_DRIVER_NAME, &platform_bus_type);
+	if (!driver || !try_module_get(driver->owner)) {
+		v4l2_warn(&fmd->v4l2_dev,
+			 "%s driver not found, deffering probe\n",
+			 CSIS_DRIVER_NAME);
+		return -EPROBE_DEFER;
+	}
+
+	return driver_for_each_device(driver, NULL, fmd,
+				      csis_register_callback);
 }
 
 static void fimc_md_unregister_entities(struct fimc_md *fmd)
@@ -361,14 +468,20 @@ static void fimc_md_unregister_entities(struct fimc_md *fmd)
 	for (i = 0; i < FIMC_MAX_DEVS; i++) {
 		if (fmd->fimc[i] == NULL)
 			continue;
-		fimc_unregister_m2m_device(fmd->fimc[i]);
-		fimc_unregister_capture_device(fmd->fimc[i]);
+		v4l2_device_unregister_subdev(&fmd->fimc[i]->vid_cap.subdev);
 		fmd->fimc[i] = NULL;
 	}
+	for (i = 0; i < FIMC_LITE_MAX_DEVS; i++) {
+		if (fmd->fimc_lite[i] == NULL)
+			continue;
+		v4l2_device_unregister_subdev(&fmd->fimc_lite[i]->subdev);
+		fmd->fimc_lite[i] = NULL;
+	}
 	for (i = 0; i < CSIS_MAX_ENTITIES; i++) {
 		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++) {
@@ -379,35 +492,6 @@ static void fimc_md_unregister_entities(struct fimc_md *fmd)
 	}
 }
 
-static int fimc_md_register_video_nodes(struct fimc_md *fmd)
-{
-	struct video_device *vdev;
-	int i, ret = 0;
-
-	for (i = 0; i < FIMC_MAX_DEVS && !ret; i++) {
-		if (!fmd->fimc[i])
-			continue;
-
-		vdev = fmd->fimc[i]->m2m.vfd;
-		if (vdev) {
-			ret = video_register_device(vdev, VFL_TYPE_GRABBER, -1);
-			if (ret)
-				break;
-			v4l2_info(&fmd->v4l2_dev, "Registered %s as /dev/%s\n",
-				  vdev->name, video_device_node_name(vdev));
-		}
-
-		vdev = fmd->fimc[i]->vid_cap.vfd;
-		if (vdev == NULL)
-			continue;
-		ret = video_register_device(vdev, VFL_TYPE_GRABBER, -1);
-		v4l2_info(&fmd->v4l2_dev, "Registered %s as /dev/%s\n",
-			  vdev->name, video_device_node_name(vdev));
-	}
-
-	return ret;
-}
-
 /**
  * __fimc_md_create_fimc_links - create links to all FIMC entities
  * @fmd: fimc media device
@@ -416,29 +500,29 @@ static int fimc_md_register_video_nodes(struct fimc_md *fmd)
  * @pad: the source entity pad index
  * @fimc_id: index of the fimc device for which link should be enabled
  */
-static int __fimc_md_create_fimc_links(struct fimc_md *fmd,
-				       struct media_entity *source,
-				       struct v4l2_subdev *sensor,
-				       int pad, int fimc_id)
+static int __fimc_md_create_fimc_sink_links(struct fimc_md *fmd,
+					    struct media_entity *source,
+					    struct v4l2_subdev *sensor,
+					    int pad, int fimc_id)
 {
 	struct fimc_sensor_info *s_info;
 	struct media_entity *sink;
-	unsigned int flags;
+	unsigned int flags = 0;
 	int ret, i;
 
 	for (i = 0; i < FIMC_MAX_DEVS; i++) {
 		if (!fmd->fimc[i])
-			break;
+			continue;
 		/*
 		 * Some FIMC variants are not fitted with camera capture
 		 * interface. Skip creating a link from sensor for those.
 		 */
-		if (sensor->grp_id == SENSOR_GROUP_ID &&
-		    !fmd->fimc[i]->variant->has_cam_if)
+		if (!fmd->fimc[i]->variant->has_cam_if)
 			continue;
 
 		flags = (i == fimc_id) ? MEDIA_LNK_FL_ENABLED : 0;
-		sink = &fmd->fimc[i]->vid_cap.subdev->entity;
+
+		sink = &fmd->fimc[i]->vid_cap.subdev.entity;
 		ret = media_entity_create_link(source, pad, sink,
 					      FIMC_SD_PAD_SINK, flags);
 		if (ret)
@@ -453,7 +537,7 @@ static int __fimc_md_create_fimc_links(struct fimc_md *fmd,
 		v4l2_info(&fmd->v4l2_dev, "created link [%s] %c> [%s]",
 			  source->name, flags ? '=' : '-', sink->name);
 
-		if (flags == 0)
+		if (flags == 0 || sensor == NULL)
 			continue;
 		s_info = v4l2_get_subdev_hostdata(sensor);
 		if (!WARN_ON(s_info == NULL)) {
@@ -463,9 +547,55 @@ static int __fimc_md_create_fimc_links(struct fimc_md *fmd,
 			spin_unlock_irqrestore(&fmd->slock, irq_flags);
 		}
 	}
+
+	for (i = 0; i < FIMC_LITE_MAX_DEVS; i++) {
+		if (!fmd->fimc_lite[i])
+			continue;
+
+		flags = (i == fimc_id) ? MEDIA_LNK_FL_ENABLED : 0;
+
+		sink = &fmd->fimc_lite[i]->subdev.entity;
+		ret = media_entity_create_link(source, pad, sink,
+					       FLITE_SD_PAD_SINK, flags);
+		if (ret)
+			return ret;
+
+		/* Notify FIMC-LITE subdev entity */
+		ret = media_entity_call(sink, link_setup, &sink->pads[0],
+					&source->pads[pad], flags);
+		if (ret)
+			break;
+
+		v4l2_info(&fmd->v4l2_dev, "created link [%s] %c> [%s]",
+			  source->name, flags ? '=' : '-', sink->name);
+	}
 	return 0;
 }
 
+/* Create links from FIMC-LITE source pads to other entities */
+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;
+
+	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, FIMC_SD_PAD_SOURCE,
+					       sink, 0, flags);
+		if (ret)
+			break;
+		/* TODO: create links to other entities */
+	}
+
+	return ret;
+}
+
 /**
  * fimc_md_create_links - create default links between registered entities
  *
@@ -522,8 +652,7 @@ static int fimc_md_create_links(struct fimc_md *fmd)
 			v4l2_info(&fmd->v4l2_dev, "created link [%s] => [%s]",
 				  sensor->entity.name, csis->entity.name);
 
-			source = &csis->entity;
-			pad = CSIS_PAD_SOURCE;
+			source = NULL;
 			break;
 
 		case FIMC_ITU_601...FIMC_ITU_656:
@@ -539,15 +668,27 @@ static int fimc_md_create_links(struct fimc_md *fmd)
 		if (source == NULL)
 			continue;
 
-		ret = __fimc_md_create_fimc_links(fmd, source, sensor, pad,
-						  fimc_id++);
+		ret = __fimc_md_create_fimc_sink_links(fmd, source, sensor,
+						       pad, fimc_id++);
 	}
+
+	fimc_id = 0;
+	for (i = 0; i < ARRAY_SIZE(fmd->csis); i++) {
+		if (fmd->csis[i].sd == NULL)
+			continue;
+		source = &fmd->csis[i].sd->entity;
+		pad = CSIS_PAD_SOURCE;
+
+		ret = __fimc_md_create_fimc_sink_links(fmd, source, NULL,
+						       pad, fimc_id++);
+	}
+
 	/* Create immutable links between each FIMC's subdev and video node */
 	flags = MEDIA_LNK_FL_IMMUTABLE | MEDIA_LNK_FL_ENABLED;
 	for (i = 0; i < FIMC_MAX_DEVS; i++) {
 		if (!fmd->fimc[i])
 			continue;
-		source = &fmd->fimc[i]->vid_cap.subdev->entity;
+		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);
@@ -555,7 +696,7 @@ static int fimc_md_create_links(struct fimc_md *fmd)
 			break;
 	}
 
-	return ret;
+	return __fimc_md_create_flite_source_links(fmd);
 }
 
 /*
@@ -663,24 +804,40 @@ int fimc_md_set_camclk(struct v4l2_subdev *sd, bool on)
 static int fimc_md_link_notify(struct media_pad *source,
 			       struct media_pad *sink, u32 flags)
 {
+	struct fimc_lite *fimc_lite = NULL;
+	struct fimc_dev *fimc = NULL;
+	struct fimc_pipeline *pipeline;
 	struct v4l2_subdev *sd;
-	struct fimc_dev *fimc;
 	int ret = 0;
 
 	if (media_entity_type(sink->entity) != MEDIA_ENT_T_V4L2_SUBDEV)
 		return 0;
 
 	sd = media_entity_to_v4l2_subdev(sink->entity);
-	fimc = v4l2_get_subdevdata(sd);
 
-	if (!(flags & MEDIA_LNK_FL_ENABLED)) {
-		ret = __fimc_pipeline_shutdown(fimc);
-		fimc->pipeline.sensor = NULL;
-		fimc->pipeline.csis = NULL;
+	switch (sd->grp_id) {
+	case FLITE_GROUP_ID:
+		fimc_lite = v4l2_get_subdevdata(sd);
+		pipeline = &fimc_lite->pipeline;
+		break;
+	case FIMC_GROUP_ID:
+		fimc = v4l2_get_subdevdata(sd);
+		pipeline = &fimc->pipeline;
+		break;
+	default:
+		return 0;
+	}
 
-		mutex_lock(&fimc->lock);
-		fimc_ctrls_delete(fimc->vid_cap.ctx);
-		mutex_unlock(&fimc->lock);
+	if (!(flags & MEDIA_LNK_FL_ENABLED)) {
+		ret = __fimc_pipeline_shutdown(pipeline);
+		pipeline->subdevs[IDX_SENSOR] = NULL;
+		pipeline->subdevs[IDX_CSIS] = NULL;
+
+		if (fimc) {
+			mutex_lock(&fimc->lock);
+			fimc_ctrls_delete(fimc->vid_cap.ctx);
+			mutex_unlock(&fimc->lock);
+		}
 		return ret;
 	}
 	/*
@@ -688,14 +845,23 @@ static int fimc_md_link_notify(struct media_pad *source,
 	 * pipeline is already in use, i.e. its video node is opened.
 	 * Recreate the controls destroyed during the link deactivation.
 	 */
-	mutex_lock(&fimc->lock);
-	if (fimc->vid_cap.refcnt > 0) {
-		ret = __fimc_pipeline_initialize(fimc, source->entity, true);
+	if (fimc) {
+		mutex_lock(&fimc->lock);
+		if (fimc->vid_cap.refcnt > 0) {
+			ret = __fimc_pipeline_initialize(pipeline,
+							 source->entity, true);
 		if (!ret)
 			ret = fimc_capture_ctrls_create(fimc);
+		}
+		mutex_unlock(&fimc->lock);
+	} else {
+		mutex_lock(&fimc_lite->lock);
+		if (fimc_lite->ref_count > 0) {
+			ret = __fimc_pipeline_initialize(pipeline,
+							 source->entity, true);
+		}
+		mutex_unlock(&fimc_lite->lock);
 	}
-	mutex_unlock(&fimc->lock);
-
 	return ret ? -EPIPE : ret;
 }
 
@@ -744,7 +910,7 @@ 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 __devinit fimc_md_probe(struct platform_device *pdev)
+static int fimc_md_probe(struct platform_device *pdev)
 {
 	struct v4l2_device *v4l2_dev;
 	struct fimc_md *fmd;
@@ -776,42 +942,48 @@ static int __devinit fimc_md_probe(struct platform_device *pdev)
 	ret = media_device_register(&fmd->media_dev);
 	if (ret < 0) {
 		v4l2_err(v4l2_dev, "Failed to register media device: %d\n", ret);
-		goto err2;
+		goto err_md;
 	}
 	ret = fimc_md_get_clocks(fmd);
 	if (ret)
-		goto err3;
+		goto err_clk;
 
 	fmd->user_subdev_api = false;
+
+	/* Protect the media graph while we're registering entities */
+	mutex_lock(&fmd->media_dev.graph_mutex);
+
 	ret = fimc_md_register_platform_entities(fmd);
 	if (ret)
-		goto err3;
+		goto err_unlock;
 
 	if (pdev->dev.platform_data) {
 		ret = fimc_md_register_sensor_entities(fmd);
 		if (ret)
-			goto err3;
+			goto err_unlock;
 	}
 	ret = fimc_md_create_links(fmd);
 	if (ret)
-		goto err3;
+		goto err_unlock;
 	ret = v4l2_device_register_subdev_nodes(&fmd->v4l2_dev);
 	if (ret)
-		goto err3;
-	ret = fimc_md_register_video_nodes(fmd);
-	if (ret)
-		goto err3;
+		goto err_unlock;
 
 	ret = device_create_file(&pdev->dev, &dev_attr_subdev_conf_mode);
-	if (!ret) {
-		platform_set_drvdata(pdev, fmd);
-		return 0;
-	}
-err3:
+	if (ret)
+		goto err_unlock;
+
+	platform_set_drvdata(pdev, fmd);
+	mutex_unlock(&fmd->media_dev.graph_mutex);
+	return 0;
+
+err_unlock:
+	mutex_unlock(&fmd->media_dev.graph_mutex);
+err_clk:
 	media_device_unregister(&fmd->media_dev);
 	fimc_md_put_clocks(fmd);
 	fimc_md_unregister_entities(fmd);
-err2:
+err_md:
 	v4l2_device_unregister(&fmd->v4l2_dev);
 	return ret;
 }
@@ -841,10 +1013,12 @@ static struct platform_driver fimc_md_driver = {
 int __init fimc_md_init(void)
 {
 	int ret;
+
 	request_module("s5p-csis");
 	ret = fimc_register_driver();
 	if (ret)
 		return ret;
+
 	return platform_driver_register(&fimc_md_driver);
 }
 void __exit fimc_md_exit(void)
diff --git a/drivers/media/video/s5p-fimc/fimc-mdevice.h b/drivers/media/video/s5p-fimc/fimc-mdevice.h
index da3780823e7..3b8a3492a17 100644
--- a/drivers/media/video/s5p-fimc/fimc-mdevice.h
+++ b/drivers/media/video/s5p-fimc/fimc-mdevice.h
@@ -1,5 +1,5 @@
 /*
- * Copyright (C) 2011 Samsung Electronics Co., Ltd.
+ * Copyright (C) 2011 - 2012 Samsung Electronics Co., Ltd.
  *
  * This program is free software; you can redistribute it and/or modify
  * it under the terms of the GNU General Public License version 2 as
@@ -18,12 +18,15 @@
 #include <media/v4l2-subdev.h>
 
 #include "fimc-core.h"
+#include "fimc-lite.h"
 #include "mipi-csis.h"
 
-/* Group IDs of sensor, MIPI CSIS and the writeback subdevs. */
+/* Group IDs of sensor, MIPI-CSIS, FIMC-LITE and the writeback subdevs. */
 #define SENSOR_GROUP_ID		(1 << 8)
 #define CSIS_GROUP_ID		(1 << 9)
 #define WRITEBACK_GROUP_ID	(1 << 10)
+#define FIMC_GROUP_ID		(1 << 11)
+#define FLITE_GROUP_ID		(1 << 12)
 
 #define FIMC_MAX_SENSORS	8
 #define FIMC_MAX_CAMCLKS	2
@@ -73,6 +76,7 @@ struct fimc_md {
 	struct fimc_sensor_info sensor[FIMC_MAX_SENSORS];
 	int num_sensors;
 	struct fimc_camclk_info camclk[FIMC_MAX_CAMCLKS];
+	struct fimc_lite *fimc_lite[FIMC_LITE_MAX_DEVS];
 	struct fimc_dev *fimc[FIMC_MAX_DEVS];
 	struct media_device media_dev;
 	struct v4l2_device v4l2_dev;
@@ -108,11 +112,11 @@ static inline void fimc_md_graph_unlock(struct fimc_dev *fimc)
 }
 
 int fimc_md_set_camclk(struct v4l2_subdev *sd, bool on);
-void fimc_pipeline_prepare(struct fimc_dev *fimc, struct media_entity *me);
-int fimc_pipeline_initialize(struct fimc_dev *fimc, struct media_entity *me,
+void fimc_pipeline_prepare(struct fimc_pipeline *p, struct media_entity *me);
+int fimc_pipeline_initialize(struct fimc_pipeline *p, struct media_entity *me,
 			     bool resume);
-int fimc_pipeline_shutdown(struct fimc_dev *fimc);
-int fimc_pipeline_s_power(struct fimc_dev *fimc, int state);
-int fimc_pipeline_s_stream(struct fimc_dev *fimc, int state);
+int fimc_pipeline_shutdown(struct fimc_pipeline *p);
+int fimc_pipeline_s_power(struct fimc_pipeline *p, bool state);
+int fimc_pipeline_s_stream(struct fimc_pipeline *p, bool state);
 
 #endif
diff --git a/drivers/media/video/s5p-fimc/fimc-reg.c b/drivers/media/video/s5p-fimc/fimc-reg.c
index 15466d0529c..1fc4ce8446f 100644
--- a/drivers/media/video/s5p-fimc/fimc-reg.c
+++ b/drivers/media/video/s5p-fimc/fimc-reg.c
@@ -1,9 +1,8 @@
 /*
  * Register interface file for Samsung Camera Interface (FIMC) driver
  *
- * Copyright (c) 2010 Samsung Electronics
- *
- * Sylwester Nawrocki, s.nawrocki@samsung.com
+ * Copyright (C) 2010 - 2012 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
@@ -12,9 +11,9 @@
 
 #include <linux/io.h>
 #include <linux/delay.h>
-#include <mach/map.h>
 #include <media/s5p_fimc.h>
 
+#include "fimc-reg.h"
 #include "fimc-core.h"
 
 
@@ -22,19 +21,19 @@ void fimc_hw_reset(struct fimc_dev *dev)
 {
 	u32 cfg;
 
-	cfg = readl(dev->regs + S5P_CISRCFMT);
-	cfg |= S5P_CISRCFMT_ITU601_8BIT;
-	writel(cfg, dev->regs + S5P_CISRCFMT);
+	cfg = readl(dev->regs + FIMC_REG_CISRCFMT);
+	cfg |= FIMC_REG_CISRCFMT_ITU601_8BIT;
+	writel(cfg, dev->regs + FIMC_REG_CISRCFMT);
 
 	/* Software reset. */
-	cfg = readl(dev->regs + S5P_CIGCTRL);
-	cfg |= (S5P_CIGCTRL_SWRST | S5P_CIGCTRL_IRQ_LEVEL);
-	writel(cfg, dev->regs + S5P_CIGCTRL);
+	cfg = readl(dev->regs + FIMC_REG_CIGCTRL);
+	cfg |= (FIMC_REG_CIGCTRL_SWRST | FIMC_REG_CIGCTRL_IRQ_LEVEL);
+	writel(cfg, dev->regs + FIMC_REG_CIGCTRL);
 	udelay(10);
 
-	cfg = readl(dev->regs + S5P_CIGCTRL);
-	cfg &= ~S5P_CIGCTRL_SWRST;
-	writel(cfg, dev->regs + S5P_CIGCTRL);
+	cfg = readl(dev->regs + FIMC_REG_CIGCTRL);
+	cfg &= ~FIMC_REG_CIGCTRL_SWRST;
+	writel(cfg, dev->regs + FIMC_REG_CIGCTRL);
 
 	if (dev->variant->out_buf_count > 4)
 		fimc_hw_set_dma_seq(dev, 0xF);
@@ -42,32 +41,32 @@ void fimc_hw_reset(struct fimc_dev *dev)
 
 static u32 fimc_hw_get_in_flip(struct fimc_ctx *ctx)
 {
-	u32 flip = S5P_MSCTRL_FLIP_NORMAL;
+	u32 flip = FIMC_REG_MSCTRL_FLIP_NORMAL;
 
 	if (ctx->hflip)
-		flip = S5P_MSCTRL_FLIP_X_MIRROR;
+		flip = FIMC_REG_MSCTRL_FLIP_X_MIRROR;
 	if (ctx->vflip)
-		flip = S5P_MSCTRL_FLIP_Y_MIRROR;
+		flip = FIMC_REG_MSCTRL_FLIP_Y_MIRROR;
 
 	if (ctx->rotation <= 90)
 		return flip;
 
-	return (flip ^ S5P_MSCTRL_FLIP_180) & S5P_MSCTRL_FLIP_180;
+	return (flip ^ FIMC_REG_MSCTRL_FLIP_180) & FIMC_REG_MSCTRL_FLIP_180;
 }
 
 static u32 fimc_hw_get_target_flip(struct fimc_ctx *ctx)
 {
-	u32 flip = S5P_CITRGFMT_FLIP_NORMAL;
+	u32 flip = FIMC_REG_CITRGFMT_FLIP_NORMAL;
 
 	if (ctx->hflip)
-		flip |= S5P_CITRGFMT_FLIP_X_MIRROR;
+		flip |= FIMC_REG_CITRGFMT_FLIP_X_MIRROR;
 	if (ctx->vflip)
-		flip |= S5P_CITRGFMT_FLIP_Y_MIRROR;
+		flip |= FIMC_REG_CITRGFMT_FLIP_Y_MIRROR;
 
 	if (ctx->rotation <= 90)
 		return flip;
 
-	return (flip ^ S5P_CITRGFMT_FLIP_180) & S5P_CITRGFMT_FLIP_180;
+	return (flip ^ FIMC_REG_CITRGFMT_FLIP_180) & FIMC_REG_CITRGFMT_FLIP_180;
 }
 
 void fimc_hw_set_rotation(struct fimc_ctx *ctx)
@@ -75,9 +74,9 @@ void fimc_hw_set_rotation(struct fimc_ctx *ctx)
 	u32 cfg, flip;
 	struct fimc_dev *dev = ctx->fimc_dev;
 
-	cfg = readl(dev->regs + S5P_CITRGFMT);
-	cfg &= ~(S5P_CITRGFMT_INROT90 | S5P_CITRGFMT_OUTROT90 |
-		 S5P_CITRGFMT_FLIP_180);
+	cfg = readl(dev->regs + FIMC_REG_CITRGFMT);
+	cfg &= ~(FIMC_REG_CITRGFMT_INROT90 | FIMC_REG_CITRGFMT_OUTROT90 |
+		 FIMC_REG_CITRGFMT_FLIP_180);
 
 	/*
 	 * The input and output rotator cannot work simultaneously.
@@ -85,21 +84,21 @@ void fimc_hw_set_rotation(struct fimc_ctx *ctx)
 	 * in direct fifo output mode.
 	 */
 	if (ctx->rotation == 90 || ctx->rotation == 270) {
-		if (ctx->out_path == FIMC_LCDFIFO)
-			cfg |= S5P_CITRGFMT_INROT90;
+		if (ctx->out_path == FIMC_IO_LCDFIFO)
+			cfg |= FIMC_REG_CITRGFMT_INROT90;
 		else
-			cfg |= S5P_CITRGFMT_OUTROT90;
+			cfg |= FIMC_REG_CITRGFMT_OUTROT90;
 	}
 
-	if (ctx->out_path == FIMC_DMA) {
+	if (ctx->out_path == FIMC_IO_DMA) {
 		cfg |= fimc_hw_get_target_flip(ctx);
-		writel(cfg, dev->regs + S5P_CITRGFMT);
+		writel(cfg, dev->regs + FIMC_REG_CITRGFMT);
 	} else {
 		/* LCD FIFO path */
-		flip = readl(dev->regs + S5P_MSCTRL);
-		flip &= ~S5P_MSCTRL_FLIP_MASK;
+		flip = readl(dev->regs + FIMC_REG_MSCTRL);
+		flip &= ~FIMC_REG_MSCTRL_FLIP_MASK;
 		flip |= fimc_hw_get_in_flip(ctx);
-		writel(flip, dev->regs + S5P_MSCTRL);
+		writel(flip, dev->regs + FIMC_REG_MSCTRL);
 	}
 }
 
@@ -110,43 +109,40 @@ void fimc_hw_set_target_format(struct fimc_ctx *ctx)
 	struct fimc_frame *frame = &ctx->d_frame;
 
 	dbg("w= %d, h= %d color: %d", frame->width,
-		frame->height, frame->fmt->color);
+	    frame->height, frame->fmt->color);
 
-	cfg = readl(dev->regs + S5P_CITRGFMT);
-	cfg &= ~(S5P_CITRGFMT_FMT_MASK | S5P_CITRGFMT_HSIZE_MASK |
-		  S5P_CITRGFMT_VSIZE_MASK);
+	cfg = readl(dev->regs + FIMC_REG_CITRGFMT);
+	cfg &= ~(FIMC_REG_CITRGFMT_FMT_MASK | FIMC_REG_CITRGFMT_HSIZE_MASK |
+		 FIMC_REG_CITRGFMT_VSIZE_MASK);
 
 	switch (frame->fmt->color) {
-	case S5P_FIMC_RGB444...S5P_FIMC_RGB888:
-		cfg |= S5P_CITRGFMT_RGB;
+	case FIMC_FMT_RGB444...FIMC_FMT_RGB888:
+		cfg |= FIMC_REG_CITRGFMT_RGB;
 		break;
-	case S5P_FIMC_YCBCR420:
-		cfg |= S5P_CITRGFMT_YCBCR420;
+	case FIMC_FMT_YCBCR420:
+		cfg |= FIMC_REG_CITRGFMT_YCBCR420;
 		break;
-	case S5P_FIMC_YCBYCR422...S5P_FIMC_CRYCBY422:
+	case FIMC_FMT_YCBYCR422...FIMC_FMT_CRYCBY422:
 		if (frame->fmt->colplanes == 1)
-			cfg |= S5P_CITRGFMT_YCBCR422_1P;
+			cfg |= FIMC_REG_CITRGFMT_YCBCR422_1P;
 		else
-			cfg |= S5P_CITRGFMT_YCBCR422;
+			cfg |= FIMC_REG_CITRGFMT_YCBCR422;
 		break;
 	default:
 		break;
 	}
 
-	if (ctx->rotation == 90 || ctx->rotation == 270) {
-		cfg |= S5P_CITRGFMT_HSIZE(frame->height);
-		cfg |= S5P_CITRGFMT_VSIZE(frame->width);
-	} else {
-
-		cfg |= S5P_CITRGFMT_HSIZE(frame->width);
-		cfg |= S5P_CITRGFMT_VSIZE(frame->height);
-	}
+	if (ctx->rotation == 90 || ctx->rotation == 270)
+		cfg |= (frame->height << 16) | frame->width;
+	else
+		cfg |= (frame->width << 16) | frame->height;
 
-	writel(cfg, dev->regs + S5P_CITRGFMT);
+	writel(cfg, dev->regs + FIMC_REG_CITRGFMT);
 
-	cfg = readl(dev->regs + S5P_CITAREA) & ~S5P_CITAREA_MASK;
+	cfg = readl(dev->regs + FIMC_REG_CITAREA);
+	cfg &= ~FIMC_REG_CITAREA_MASK;
 	cfg |= (frame->width * frame->height);
-	writel(cfg, dev->regs + S5P_CITAREA);
+	writel(cfg, dev->regs + FIMC_REG_CITAREA);
 }
 
 static void fimc_hw_set_out_dma_size(struct fimc_ctx *ctx)
@@ -155,87 +151,82 @@ static void fimc_hw_set_out_dma_size(struct fimc_ctx *ctx)
 	struct fimc_frame *frame = &ctx->d_frame;
 	u32 cfg;
 
-	cfg = S5P_ORIG_SIZE_HOR(frame->f_width);
-	cfg |= S5P_ORIG_SIZE_VER(frame->f_height);
-	writel(cfg, dev->regs + S5P_ORGOSIZE);
+	cfg = (frame->f_height << 16) | frame->f_width;
+	writel(cfg, dev->regs + FIMC_REG_ORGOSIZE);
 
 	/* Select color space conversion equation (HD/SD size).*/
-	cfg = readl(dev->regs + S5P_CIGCTRL);
+	cfg = readl(dev->regs + FIMC_REG_CIGCTRL);
 	if (frame->f_width >= 1280) /* HD */
-		cfg |= S5P_CIGCTRL_CSC_ITU601_709;
+		cfg |= FIMC_REG_CIGCTRL_CSC_ITU601_709;
 	else	/* SD */
-		cfg &= ~S5P_CIGCTRL_CSC_ITU601_709;
-	writel(cfg, dev->regs + S5P_CIGCTRL);
+		cfg &= ~FIMC_REG_CIGCTRL_CSC_ITU601_709;
+	writel(cfg, dev->regs + FIMC_REG_CIGCTRL);
 
 }
 
 void fimc_hw_set_out_dma(struct fimc_ctx *ctx)
 {
-	u32 cfg;
 	struct fimc_dev *dev = ctx->fimc_dev;
 	struct fimc_frame *frame = &ctx->d_frame;
 	struct fimc_dma_offset *offset = &frame->dma_offset;
 	struct fimc_fmt *fmt = frame->fmt;
+	u32 cfg;
 
 	/* Set the input dma offsets. */
-	cfg = 0;
-	cfg |= S5P_CIO_OFFS_HOR(offset->y_h);
-	cfg |= S5P_CIO_OFFS_VER(offset->y_v);
-	writel(cfg, dev->regs + S5P_CIOYOFF);
+	cfg = (offset->y_v << 16) | offset->y_h;
+	writel(cfg, dev->regs + FIMC_REG_CIOYOFF);
 
-	cfg = 0;
-	cfg |= S5P_CIO_OFFS_HOR(offset->cb_h);
-	cfg |= S5P_CIO_OFFS_VER(offset->cb_v);
-	writel(cfg, dev->regs + S5P_CIOCBOFF);
+	cfg = (offset->cb_v << 16) | offset->cb_h;
+	writel(cfg, dev->regs + FIMC_REG_CIOCBOFF);
 
-	cfg = 0;
-	cfg |= S5P_CIO_OFFS_HOR(offset->cr_h);
-	cfg |= S5P_CIO_OFFS_VER(offset->cr_v);
-	writel(cfg, dev->regs + S5P_CIOCROFF);
+	cfg = (offset->cr_v << 16) | offset->cr_h;
+	writel(cfg, dev->regs + FIMC_REG_CIOCROFF);
 
 	fimc_hw_set_out_dma_size(ctx);
 
 	/* Configure chroma components order. */
-	cfg = readl(dev->regs + S5P_CIOCTRL);
+	cfg = readl(dev->regs + FIMC_REG_CIOCTRL);
 
-	cfg &= ~(S5P_CIOCTRL_ORDER2P_MASK | S5P_CIOCTRL_ORDER422_MASK |
-		 S5P_CIOCTRL_YCBCR_PLANE_MASK | S5P_CIOCTRL_RGB16FMT_MASK);
+	cfg &= ~(FIMC_REG_CIOCTRL_ORDER2P_MASK |
+		 FIMC_REG_CIOCTRL_ORDER422_MASK |
+		 FIMC_REG_CIOCTRL_YCBCR_PLANE_MASK |
+		 FIMC_REG_CIOCTRL_RGB16FMT_MASK);
 
 	if (fmt->colplanes == 1)
 		cfg |= ctx->out_order_1p;
 	else if (fmt->colplanes == 2)
-		cfg |= ctx->out_order_2p | S5P_CIOCTRL_YCBCR_2PLANE;
+		cfg |= ctx->out_order_2p | FIMC_REG_CIOCTRL_YCBCR_2PLANE;
 	else if (fmt->colplanes == 3)
-		cfg |= S5P_CIOCTRL_YCBCR_3PLANE;
+		cfg |= FIMC_REG_CIOCTRL_YCBCR_3PLANE;
 
-	if (fmt->color == S5P_FIMC_RGB565)
-		cfg |= S5P_CIOCTRL_RGB565;
-	else if (fmt->color == S5P_FIMC_RGB555)
-		cfg |= S5P_CIOCTRL_ARGB1555;
-	else if (fmt->color == S5P_FIMC_RGB444)
-		cfg |= S5P_CIOCTRL_ARGB4444;
+	if (fmt->color == FIMC_FMT_RGB565)
+		cfg |= FIMC_REG_CIOCTRL_RGB565;
+	else if (fmt->color == FIMC_FMT_RGB555)
+		cfg |= FIMC_REG_CIOCTRL_ARGB1555;
+	else if (fmt->color == FIMC_FMT_RGB444)
+		cfg |= FIMC_REG_CIOCTRL_ARGB4444;
 
-	writel(cfg, dev->regs + S5P_CIOCTRL);
+	writel(cfg, dev->regs + FIMC_REG_CIOCTRL);
 }
 
 static void fimc_hw_en_autoload(struct fimc_dev *dev, int enable)
 {
-	u32 cfg = readl(dev->regs + S5P_ORGISIZE);
+	u32 cfg = readl(dev->regs + FIMC_REG_ORGISIZE);
 	if (enable)
-		cfg |= S5P_CIREAL_ISIZE_AUTOLOAD_EN;
+		cfg |= FIMC_REG_CIREAL_ISIZE_AUTOLOAD_EN;
 	else
-		cfg &= ~S5P_CIREAL_ISIZE_AUTOLOAD_EN;
-	writel(cfg, dev->regs + S5P_ORGISIZE);
+		cfg &= ~FIMC_REG_CIREAL_ISIZE_AUTOLOAD_EN;
+	writel(cfg, dev->regs + FIMC_REG_ORGISIZE);
 }
 
 void fimc_hw_en_lastirq(struct fimc_dev *dev, int enable)
 {
-	u32 cfg = readl(dev->regs + S5P_CIOCTRL);
+	u32 cfg = readl(dev->regs + FIMC_REG_CIOCTRL);
 	if (enable)
-		cfg |= S5P_CIOCTRL_LASTIRQ_ENABLE;
+		cfg |= FIMC_REG_CIOCTRL_LASTIRQ_ENABLE;
 	else
-		cfg &= ~S5P_CIOCTRL_LASTIRQ_ENABLE;
-	writel(cfg, dev->regs + S5P_CIOCTRL);
+		cfg &= ~FIMC_REG_CIOCTRL_LASTIRQ_ENABLE;
+	writel(cfg, dev->regs + FIMC_REG_CIOCTRL);
 }
 
 void fimc_hw_set_prescaler(struct fimc_ctx *ctx)
@@ -245,15 +236,13 @@ void fimc_hw_set_prescaler(struct fimc_ctx *ctx)
 	u32 cfg, shfactor;
 
 	shfactor = 10 - (sc->hfactor + sc->vfactor);
+	cfg = shfactor << 28;
 
-	cfg = S5P_CISCPRERATIO_SHFACTOR(shfactor);
-	cfg |= S5P_CISCPRERATIO_HOR(sc->pre_hratio);
-	cfg |= S5P_CISCPRERATIO_VER(sc->pre_vratio);
-	writel(cfg, dev->regs + S5P_CISCPRERATIO);
+	cfg |= (sc->pre_hratio << 16) | sc->pre_vratio;
+	writel(cfg, dev->regs + FIMC_REG_CISCPRERATIO);
 
-	cfg = S5P_CISCPREDST_WIDTH(sc->pre_dst_width);
-	cfg |= S5P_CISCPREDST_HEIGHT(sc->pre_dst_height);
-	writel(cfg, dev->regs + S5P_CISCPREDST);
+	cfg = (sc->pre_dst_width << 16) | sc->pre_dst_height;
+	writel(cfg, dev->regs + FIMC_REG_CISCPREDST);
 }
 
 static void fimc_hw_set_scaler(struct fimc_ctx *ctx)
@@ -263,93 +252,95 @@ static void fimc_hw_set_scaler(struct fimc_ctx *ctx)
 	struct fimc_frame *src_frame = &ctx->s_frame;
 	struct fimc_frame *dst_frame = &ctx->d_frame;
 
-	u32 cfg = readl(dev->regs + S5P_CISCCTRL);
+	u32 cfg = readl(dev->regs + FIMC_REG_CISCCTRL);
 
-	cfg &= ~(S5P_CISCCTRL_CSCR2Y_WIDE | S5P_CISCCTRL_CSCY2R_WIDE |
-		 S5P_CISCCTRL_SCALEUP_H | S5P_CISCCTRL_SCALEUP_V |
-		 S5P_CISCCTRL_SCALERBYPASS | S5P_CISCCTRL_ONE2ONE |
-		 S5P_CISCCTRL_INRGB_FMT_MASK | S5P_CISCCTRL_OUTRGB_FMT_MASK |
-		 S5P_CISCCTRL_INTERLACE | S5P_CISCCTRL_RGB_EXT);
+	cfg &= ~(FIMC_REG_CISCCTRL_CSCR2Y_WIDE | FIMC_REG_CISCCTRL_CSCY2R_WIDE |
+		 FIMC_REG_CISCCTRL_SCALEUP_H | FIMC_REG_CISCCTRL_SCALEUP_V |
+		 FIMC_REG_CISCCTRL_SCALERBYPASS | FIMC_REG_CISCCTRL_ONE2ONE |
+		 FIMC_REG_CISCCTRL_INRGB_FMT_MASK | FIMC_REG_CISCCTRL_OUTRGB_FMT_MASK |
+		 FIMC_REG_CISCCTRL_INTERLACE | FIMC_REG_CISCCTRL_RGB_EXT);
 
 	if (!(ctx->flags & FIMC_COLOR_RANGE_NARROW))
-		cfg |= (S5P_CISCCTRL_CSCR2Y_WIDE | S5P_CISCCTRL_CSCY2R_WIDE);
+		cfg |= (FIMC_REG_CISCCTRL_CSCR2Y_WIDE |
+			FIMC_REG_CISCCTRL_CSCY2R_WIDE);
 
 	if (!sc->enabled)
-		cfg |= S5P_CISCCTRL_SCALERBYPASS;
+		cfg |= FIMC_REG_CISCCTRL_SCALERBYPASS;
 
 	if (sc->scaleup_h)
-		cfg |= S5P_CISCCTRL_SCALEUP_H;
+		cfg |= FIMC_REG_CISCCTRL_SCALEUP_H;
 
 	if (sc->scaleup_v)
-		cfg |= S5P_CISCCTRL_SCALEUP_V;
+		cfg |= FIMC_REG_CISCCTRL_SCALEUP_V;
 
 	if (sc->copy_mode)
-		cfg |= S5P_CISCCTRL_ONE2ONE;
+		cfg |= FIMC_REG_CISCCTRL_ONE2ONE;
 
-	if (ctx->in_path == FIMC_DMA) {
+	if (ctx->in_path == FIMC_IO_DMA) {
 		switch (src_frame->fmt->color) {
-		case S5P_FIMC_RGB565:
-			cfg |= S5P_CISCCTRL_INRGB_FMT_RGB565;
+		case FIMC_FMT_RGB565:
+			cfg |= FIMC_REG_CISCCTRL_INRGB_FMT_RGB565;
 			break;
-		case S5P_FIMC_RGB666:
-			cfg |= S5P_CISCCTRL_INRGB_FMT_RGB666;
+		case FIMC_FMT_RGB666:
+			cfg |= FIMC_REG_CISCCTRL_INRGB_FMT_RGB666;
 			break;
-		case S5P_FIMC_RGB888:
-			cfg |= S5P_CISCCTRL_INRGB_FMT_RGB888;
+		case FIMC_FMT_RGB888:
+			cfg |= FIMC_REG_CISCCTRL_INRGB_FMT_RGB888;
 			break;
 		}
 	}
 
-	if (ctx->out_path == FIMC_DMA) {
+	if (ctx->out_path == FIMC_IO_DMA) {
 		u32 color = dst_frame->fmt->color;
 
-		if (color >= S5P_FIMC_RGB444 && color <= S5P_FIMC_RGB565)
-			cfg |= S5P_CISCCTRL_OUTRGB_FMT_RGB565;
-		else if (color == S5P_FIMC_RGB666)
-			cfg |= S5P_CISCCTRL_OUTRGB_FMT_RGB666;
-		else if (color == S5P_FIMC_RGB888)
-			cfg |= S5P_CISCCTRL_OUTRGB_FMT_RGB888;
+		if (color >= FIMC_FMT_RGB444 && color <= FIMC_FMT_RGB565)
+			cfg |= FIMC_REG_CISCCTRL_OUTRGB_FMT_RGB565;
+		else if (color == FIMC_FMT_RGB666)
+			cfg |= FIMC_REG_CISCCTRL_OUTRGB_FMT_RGB666;
+		else if (color == FIMC_FMT_RGB888)
+			cfg |= FIMC_REG_CISCCTRL_OUTRGB_FMT_RGB888;
 	} else {
-		cfg |= S5P_CISCCTRL_OUTRGB_FMT_RGB888;
+		cfg |= FIMC_REG_CISCCTRL_OUTRGB_FMT_RGB888;
 
 		if (ctx->flags & FIMC_SCAN_MODE_INTERLACED)
-			cfg |= S5P_CISCCTRL_INTERLACE;
+			cfg |= FIMC_REG_CISCCTRL_INTERLACE;
 	}
 
-	writel(cfg, dev->regs + S5P_CISCCTRL);
+	writel(cfg, dev->regs + FIMC_REG_CISCCTRL);
 }
 
 void fimc_hw_set_mainscaler(struct fimc_ctx *ctx)
 {
 	struct fimc_dev *dev = ctx->fimc_dev;
-	struct samsung_fimc_variant *variant = dev->variant;
+	struct fimc_variant *variant = dev->variant;
 	struct fimc_scaler *sc = &ctx->scaler;
 	u32 cfg;
 
 	dbg("main_hratio= 0x%X  main_vratio= 0x%X",
-		sc->main_hratio, sc->main_vratio);
+	    sc->main_hratio, sc->main_vratio);
 
 	fimc_hw_set_scaler(ctx);
 
-	cfg = readl(dev->regs + S5P_CISCCTRL);
-	cfg &= ~(S5P_CISCCTRL_MHRATIO_MASK | S5P_CISCCTRL_MVRATIO_MASK);
+	cfg = readl(dev->regs + FIMC_REG_CISCCTRL);
+	cfg &= ~(FIMC_REG_CISCCTRL_MHRATIO_MASK |
+		 FIMC_REG_CISCCTRL_MVRATIO_MASK);
 
 	if (variant->has_mainscaler_ext) {
-		cfg |= S5P_CISCCTRL_MHRATIO_EXT(sc->main_hratio);
-		cfg |= S5P_CISCCTRL_MVRATIO_EXT(sc->main_vratio);
-		writel(cfg, dev->regs + S5P_CISCCTRL);
+		cfg |= FIMC_REG_CISCCTRL_MHRATIO_EXT(sc->main_hratio);
+		cfg |= FIMC_REG_CISCCTRL_MVRATIO_EXT(sc->main_vratio);
+		writel(cfg, dev->regs + FIMC_REG_CISCCTRL);
 
-		cfg = readl(dev->regs + S5P_CIEXTEN);
+		cfg = readl(dev->regs + FIMC_REG_CIEXTEN);
 
-		cfg &= ~(S5P_CIEXTEN_MVRATIO_EXT_MASK |
-			 S5P_CIEXTEN_MHRATIO_EXT_MASK);
-		cfg |= S5P_CIEXTEN_MHRATIO_EXT(sc->main_hratio);
-		cfg |= S5P_CIEXTEN_MVRATIO_EXT(sc->main_vratio);
-		writel(cfg, dev->regs + S5P_CIEXTEN);
+		cfg &= ~(FIMC_REG_CIEXTEN_MVRATIO_EXT_MASK |
+			 FIMC_REG_CIEXTEN_MHRATIO_EXT_MASK);
+		cfg |= FIMC_REG_CIEXTEN_MHRATIO_EXT(sc->main_hratio);
+		cfg |= FIMC_REG_CIEXTEN_MVRATIO_EXT(sc->main_vratio);
+		writel(cfg, dev->regs + FIMC_REG_CIEXTEN);
 	} else {
-		cfg |= S5P_CISCCTRL_MHRATIO(sc->main_hratio);
-		cfg |= S5P_CISCCTRL_MVRATIO(sc->main_vratio);
-		writel(cfg, dev->regs + S5P_CISCCTRL);
+		cfg |= FIMC_REG_CISCCTRL_MHRATIO(sc->main_hratio);
+		cfg |= FIMC_REG_CISCCTRL_MVRATIO(sc->main_vratio);
+		writel(cfg, dev->regs + FIMC_REG_CISCCTRL);
 	}
 }
 
@@ -357,40 +348,41 @@ void fimc_hw_en_capture(struct fimc_ctx *ctx)
 {
 	struct fimc_dev *dev = ctx->fimc_dev;
 
-	u32 cfg = readl(dev->regs + S5P_CIIMGCPT);
+	u32 cfg = readl(dev->regs + FIMC_REG_CIIMGCPT);
 
-	if (ctx->out_path == FIMC_DMA) {
+	if (ctx->out_path == FIMC_IO_DMA) {
 		/* one shot mode */
-		cfg |= S5P_CIIMGCPT_CPT_FREN_ENABLE | S5P_CIIMGCPT_IMGCPTEN;
+		cfg |= FIMC_REG_CIIMGCPT_CPT_FREN_ENABLE |
+			FIMC_REG_CIIMGCPT_IMGCPTEN;
 	} else {
 		/* Continuous frame capture mode (freerun). */
-		cfg &= ~(S5P_CIIMGCPT_CPT_FREN_ENABLE |
-			 S5P_CIIMGCPT_CPT_FRMOD_CNT);
-		cfg |= S5P_CIIMGCPT_IMGCPTEN;
+		cfg &= ~(FIMC_REG_CIIMGCPT_CPT_FREN_ENABLE |
+			 FIMC_REG_CIIMGCPT_CPT_FRMOD_CNT);
+		cfg |= FIMC_REG_CIIMGCPT_IMGCPTEN;
 	}
 
 	if (ctx->scaler.enabled)
-		cfg |= S5P_CIIMGCPT_IMGCPTEN_SC;
+		cfg |= FIMC_REG_CIIMGCPT_IMGCPTEN_SC;
 
-	writel(cfg | S5P_CIIMGCPT_IMGCPTEN, dev->regs + S5P_CIIMGCPT);
+	cfg |= FIMC_REG_CIIMGCPT_IMGCPTEN;
+	writel(cfg, dev->regs + FIMC_REG_CIIMGCPT);
 }
 
-void fimc_hw_set_effect(struct fimc_ctx *ctx, bool active)
+void fimc_hw_set_effect(struct fimc_ctx *ctx)
 {
 	struct fimc_dev *dev = ctx->fimc_dev;
 	struct fimc_effect *effect = &ctx->effect;
 	u32 cfg = 0;
 
-	if (active) {
-		cfg |= S5P_CIIMGEFF_IE_SC_AFTER | S5P_CIIMGEFF_IE_ENABLE;
+	if (effect->type != FIMC_REG_CIIMGEFF_FIN_BYPASS) {
+		cfg |= FIMC_REG_CIIMGEFF_IE_SC_AFTER |
+			FIMC_REG_CIIMGEFF_IE_ENABLE;
 		cfg |= effect->type;
-		if (effect->type == S5P_FIMC_EFFECT_ARBITRARY) {
-			cfg |= S5P_CIIMGEFF_PAT_CB(effect->pat_cb);
-			cfg |= S5P_CIIMGEFF_PAT_CR(effect->pat_cr);
-		}
+		if (effect->type == FIMC_REG_CIIMGEFF_FIN_ARBITRARY)
+			cfg |= (effect->pat_cb << 13) | effect->pat_cr;
 	}
 
-	writel(cfg, dev->regs + S5P_CIIMGEFF);
+	writel(cfg, dev->regs + FIMC_REG_CIIMGEFF);
 }
 
 void fimc_hw_set_rgb_alpha(struct fimc_ctx *ctx)
@@ -402,10 +394,10 @@ void fimc_hw_set_rgb_alpha(struct fimc_ctx *ctx)
 	if (!(frame->fmt->flags & FMT_HAS_ALPHA))
 		return;
 
-	cfg = readl(dev->regs + S5P_CIOCTRL);
-	cfg &= ~S5P_CIOCTRL_ALPHA_OUT_MASK;
+	cfg = readl(dev->regs + FIMC_REG_CIOCTRL);
+	cfg &= ~FIMC_REG_CIOCTRL_ALPHA_OUT_MASK;
 	cfg |= (frame->alpha << 4);
-	writel(cfg, dev->regs + S5P_CIOCTRL);
+	writel(cfg, dev->regs + FIMC_REG_CIOCTRL);
 }
 
 static void fimc_hw_set_in_dma_size(struct fimc_ctx *ctx)
@@ -415,16 +407,14 @@ static void fimc_hw_set_in_dma_size(struct fimc_ctx *ctx)
 	u32 cfg_o = 0;
 	u32 cfg_r = 0;
 
-	if (FIMC_LCDFIFO == ctx->out_path)
-		cfg_r |= S5P_CIREAL_ISIZE_AUTOLOAD_EN;
+	if (FIMC_IO_LCDFIFO == ctx->out_path)
+		cfg_r |= FIMC_REG_CIREAL_ISIZE_AUTOLOAD_EN;
 
-	cfg_o |= S5P_ORIG_SIZE_HOR(frame->f_width);
-	cfg_o |= S5P_ORIG_SIZE_VER(frame->f_height);
-	cfg_r |= S5P_CIREAL_ISIZE_WIDTH(frame->width);
-	cfg_r |= S5P_CIREAL_ISIZE_HEIGHT(frame->height);
+	cfg_o |= (frame->f_height << 16) | frame->f_width;
+	cfg_r |= (frame->height << 16) | frame->width;
 
-	writel(cfg_o, dev->regs + S5P_ORGISIZE);
-	writel(cfg_r, dev->regs + S5P_CIREAL_ISIZE);
+	writel(cfg_o, dev->regs + FIMC_REG_ORGISIZE);
+	writel(cfg_r, dev->regs + FIMC_REG_CIREAL_ISIZE);
 }
 
 void fimc_hw_set_in_dma(struct fimc_ctx *ctx)
@@ -435,80 +425,77 @@ void fimc_hw_set_in_dma(struct fimc_ctx *ctx)
 	u32 cfg;
 
 	/* Set the pixel offsets. */
-	cfg = S5P_CIO_OFFS_HOR(offset->y_h);
-	cfg |= S5P_CIO_OFFS_VER(offset->y_v);
-	writel(cfg, dev->regs + S5P_CIIYOFF);
+	cfg = (offset->y_v << 16) | offset->y_h;
+	writel(cfg, dev->regs + FIMC_REG_CIIYOFF);
 
-	cfg = S5P_CIO_OFFS_HOR(offset->cb_h);
-	cfg |= S5P_CIO_OFFS_VER(offset->cb_v);
-	writel(cfg, dev->regs + S5P_CIICBOFF);
+	cfg = (offset->cb_v << 16) | offset->cb_h;
+	writel(cfg, dev->regs + FIMC_REG_CIICBOFF);
 
-	cfg = S5P_CIO_OFFS_HOR(offset->cr_h);
-	cfg |= S5P_CIO_OFFS_VER(offset->cr_v);
-	writel(cfg, dev->regs + S5P_CIICROFF);
+	cfg = (offset->cr_v << 16) | offset->cr_h;
+	writel(cfg, dev->regs + FIMC_REG_CIICROFF);
 
 	/* Input original and real size. */
 	fimc_hw_set_in_dma_size(ctx);
 
 	/* Use DMA autoload only in FIFO mode. */
-	fimc_hw_en_autoload(dev, ctx->out_path == FIMC_LCDFIFO);
+	fimc_hw_en_autoload(dev, ctx->out_path == FIMC_IO_LCDFIFO);
 
 	/* Set the input DMA to process single frame only. */
-	cfg = readl(dev->regs + S5P_MSCTRL);
-	cfg &= ~(S5P_MSCTRL_INFORMAT_MASK
-		| S5P_MSCTRL_IN_BURST_COUNT_MASK
-		| S5P_MSCTRL_INPUT_MASK
-		| S5P_MSCTRL_C_INT_IN_MASK
-		| S5P_MSCTRL_2P_IN_ORDER_MASK);
+	cfg = readl(dev->regs + FIMC_REG_MSCTRL);
+	cfg &= ~(FIMC_REG_MSCTRL_INFORMAT_MASK
+		 | 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);
 
-	cfg |= (S5P_MSCTRL_IN_BURST_COUNT(4)
-		| S5P_MSCTRL_INPUT_MEMORY
-		| S5P_MSCTRL_FIFO_CTRL_FULL);
+	cfg |= (FIMC_REG_MSCTRL_IN_BURST_COUNT(4)
+		| FIMC_REG_MSCTRL_INPUT_MEMORY
+		| FIMC_REG_MSCTRL_FIFO_CTRL_FULL);
 
 	switch (frame->fmt->color) {
-	case S5P_FIMC_RGB565...S5P_FIMC_RGB888:
-		cfg |= S5P_MSCTRL_INFORMAT_RGB;
+	case FIMC_FMT_RGB565...FIMC_FMT_RGB888:
+		cfg |= FIMC_REG_MSCTRL_INFORMAT_RGB;
 		break;
-	case S5P_FIMC_YCBCR420:
-		cfg |= S5P_MSCTRL_INFORMAT_YCBCR420;
+	case FIMC_FMT_YCBCR420:
+		cfg |= FIMC_REG_MSCTRL_INFORMAT_YCBCR420;
 
 		if (frame->fmt->colplanes == 2)
-			cfg |= ctx->in_order_2p | S5P_MSCTRL_C_INT_IN_2PLANE;
+			cfg |= ctx->in_order_2p | FIMC_REG_MSCTRL_C_INT_IN_2PLANE;
 		else
-			cfg |= S5P_MSCTRL_C_INT_IN_3PLANE;
+			cfg |= FIMC_REG_MSCTRL_C_INT_IN_3PLANE;
 
 		break;
-	case S5P_FIMC_YCBYCR422...S5P_FIMC_CRYCBY422:
+	case FIMC_FMT_YCBYCR422...FIMC_FMT_CRYCBY422:
 		if (frame->fmt->colplanes == 1) {
 			cfg |= ctx->in_order_1p
-				| S5P_MSCTRL_INFORMAT_YCBCR422_1P;
+				| FIMC_REG_MSCTRL_INFORMAT_YCBCR422_1P;
 		} else {
-			cfg |= S5P_MSCTRL_INFORMAT_YCBCR422;
+			cfg |= FIMC_REG_MSCTRL_INFORMAT_YCBCR422;
 
 			if (frame->fmt->colplanes == 2)
 				cfg |= ctx->in_order_2p
-					| S5P_MSCTRL_C_INT_IN_2PLANE;
+					| FIMC_REG_MSCTRL_C_INT_IN_2PLANE;
 			else
-				cfg |= S5P_MSCTRL_C_INT_IN_3PLANE;
+				cfg |= FIMC_REG_MSCTRL_C_INT_IN_3PLANE;
 		}
 		break;
 	default:
 		break;
 	}
 
-	writel(cfg, dev->regs + S5P_MSCTRL);
+	writel(cfg, dev->regs + FIMC_REG_MSCTRL);
 
 	/* Input/output DMA linear/tiled mode. */
-	cfg = readl(dev->regs + S5P_CIDMAPARAM);
-	cfg &= ~S5P_CIDMAPARAM_TILE_MASK;
+	cfg = readl(dev->regs + FIMC_REG_CIDMAPARAM);
+	cfg &= ~FIMC_REG_CIDMAPARAM_TILE_MASK;
 
 	if (tiled_fmt(ctx->s_frame.fmt))
-		cfg |= S5P_CIDMAPARAM_R_64X32;
+		cfg |= FIMC_REG_CIDMAPARAM_R_64X32;
 
 	if (tiled_fmt(ctx->d_frame.fmt))
-		cfg |= S5P_CIDMAPARAM_W_64X32;
+		cfg |= FIMC_REG_CIDMAPARAM_W_64X32;
 
-	writel(cfg, dev->regs + S5P_CIDMAPARAM);
+	writel(cfg, dev->regs + FIMC_REG_CIDMAPARAM);
 }
 
 
@@ -516,40 +503,40 @@ void fimc_hw_set_input_path(struct fimc_ctx *ctx)
 {
 	struct fimc_dev *dev = ctx->fimc_dev;
 
-	u32 cfg = readl(dev->regs + S5P_MSCTRL);
-	cfg &= ~S5P_MSCTRL_INPUT_MASK;
+	u32 cfg = readl(dev->regs + FIMC_REG_MSCTRL);
+	cfg &= ~FIMC_REG_MSCTRL_INPUT_MASK;
 
-	if (ctx->in_path == FIMC_DMA)
-		cfg |= S5P_MSCTRL_INPUT_MEMORY;
+	if (ctx->in_path == FIMC_IO_DMA)
+		cfg |= FIMC_REG_MSCTRL_INPUT_MEMORY;
 	else
-		cfg |= S5P_MSCTRL_INPUT_EXTCAM;
+		cfg |= FIMC_REG_MSCTRL_INPUT_EXTCAM;
 
-	writel(cfg, dev->regs + S5P_MSCTRL);
+	writel(cfg, dev->regs + FIMC_REG_MSCTRL);
 }
 
 void fimc_hw_set_output_path(struct fimc_ctx *ctx)
 {
 	struct fimc_dev *dev = ctx->fimc_dev;
 
-	u32 cfg = readl(dev->regs + S5P_CISCCTRL);
-	cfg &= ~S5P_CISCCTRL_LCDPATHEN_FIFO;
-	if (ctx->out_path == FIMC_LCDFIFO)
-		cfg |= S5P_CISCCTRL_LCDPATHEN_FIFO;
-	writel(cfg, dev->regs + S5P_CISCCTRL);
+	u32 cfg = readl(dev->regs + FIMC_REG_CISCCTRL);
+	cfg &= ~FIMC_REG_CISCCTRL_LCDPATHEN_FIFO;
+	if (ctx->out_path == FIMC_IO_LCDFIFO)
+		cfg |= FIMC_REG_CISCCTRL_LCDPATHEN_FIFO;
+	writel(cfg, dev->regs + FIMC_REG_CISCCTRL);
 }
 
 void fimc_hw_set_input_addr(struct fimc_dev *dev, struct fimc_addr *paddr)
 {
-	u32 cfg = readl(dev->regs + S5P_CIREAL_ISIZE);
-	cfg |= S5P_CIREAL_ISIZE_ADDR_CH_DIS;
-	writel(cfg, dev->regs + S5P_CIREAL_ISIZE);
+	u32 cfg = readl(dev->regs + FIMC_REG_CIREAL_ISIZE);
+	cfg |= FIMC_REG_CIREAL_ISIZE_ADDR_CH_DIS;
+	writel(cfg, dev->regs + FIMC_REG_CIREAL_ISIZE);
 
-	writel(paddr->y, dev->regs + S5P_CIIYSA(0));
-	writel(paddr->cb, dev->regs + S5P_CIICBSA(0));
-	writel(paddr->cr, dev->regs + S5P_CIICRSA(0));
+	writel(paddr->y, dev->regs + FIMC_REG_CIIYSA(0));
+	writel(paddr->cb, dev->regs + FIMC_REG_CIICBSA(0));
+	writel(paddr->cr, dev->regs + FIMC_REG_CIICRSA(0));
 
-	cfg &= ~S5P_CIREAL_ISIZE_ADDR_CH_DIS;
-	writel(cfg, dev->regs + S5P_CIREAL_ISIZE);
+	cfg &= ~FIMC_REG_CIREAL_ISIZE_ADDR_CH_DIS;
+	writel(cfg, dev->regs + FIMC_REG_CIREAL_ISIZE);
 }
 
 void fimc_hw_set_output_addr(struct fimc_dev *dev,
@@ -557,9 +544,9 @@ void fimc_hw_set_output_addr(struct fimc_dev *dev,
 {
 	int i = (index == -1) ? 0 : index;
 	do {
-		writel(paddr->y, dev->regs + S5P_CIOYSA(i));
-		writel(paddr->cb, dev->regs + S5P_CIOCBSA(i));
-		writel(paddr->cr, dev->regs + S5P_CIOCRSA(i));
+		writel(paddr->y, dev->regs + FIMC_REG_CIOYSA(i));
+		writel(paddr->cb, dev->regs + FIMC_REG_CIOCBSA(i));
+		writel(paddr->cr, dev->regs + FIMC_REG_CIOCRSA(i));
 		dbg("dst_buf[%d]: 0x%X, cb: 0x%X, cr: 0x%X",
 		    i, paddr->y, paddr->cb, paddr->cr);
 	} while (index == -1 && ++i < FIMC_MAX_OUT_BUFS);
@@ -568,32 +555,45 @@ void fimc_hw_set_output_addr(struct fimc_dev *dev,
 int fimc_hw_set_camera_polarity(struct fimc_dev *fimc,
 				struct s5p_fimc_isp_info *cam)
 {
-	u32 cfg = readl(fimc->regs + S5P_CIGCTRL);
+	u32 cfg = readl(fimc->regs + FIMC_REG_CIGCTRL);
 
-	cfg &= ~(S5P_CIGCTRL_INVPOLPCLK | S5P_CIGCTRL_INVPOLVSYNC |
-		 S5P_CIGCTRL_INVPOLHREF | S5P_CIGCTRL_INVPOLHSYNC |
-		 S5P_CIGCTRL_INVPOLFIELD);
+	cfg &= ~(FIMC_REG_CIGCTRL_INVPOLPCLK | FIMC_REG_CIGCTRL_INVPOLVSYNC |
+		 FIMC_REG_CIGCTRL_INVPOLHREF | FIMC_REG_CIGCTRL_INVPOLHSYNC |
+		 FIMC_REG_CIGCTRL_INVPOLFIELD);
 
 	if (cam->flags & V4L2_MBUS_PCLK_SAMPLE_FALLING)
-		cfg |= S5P_CIGCTRL_INVPOLPCLK;
+		cfg |= FIMC_REG_CIGCTRL_INVPOLPCLK;
 
 	if (cam->flags & V4L2_MBUS_VSYNC_ACTIVE_LOW)
-		cfg |= S5P_CIGCTRL_INVPOLVSYNC;
+		cfg |= FIMC_REG_CIGCTRL_INVPOLVSYNC;
 
 	if (cam->flags & V4L2_MBUS_HSYNC_ACTIVE_LOW)
-		cfg |= S5P_CIGCTRL_INVPOLHREF;
+		cfg |= FIMC_REG_CIGCTRL_INVPOLHREF;
 
 	if (cam->flags & V4L2_MBUS_HSYNC_ACTIVE_LOW)
-		cfg |= S5P_CIGCTRL_INVPOLHSYNC;
+		cfg |= FIMC_REG_CIGCTRL_INVPOLHSYNC;
 
 	if (cam->flags & V4L2_MBUS_FIELD_EVEN_LOW)
-		cfg |= S5P_CIGCTRL_INVPOLFIELD;
+		cfg |= FIMC_REG_CIGCTRL_INVPOLFIELD;
 
-	writel(cfg, fimc->regs + S5P_CIGCTRL);
+	writel(cfg, fimc->regs + FIMC_REG_CIGCTRL);
 
 	return 0;
 }
 
+struct mbus_pixfmt_desc {
+	u32 pixelcode;
+	u32 cisrcfmt;
+	u16 bus_width;
+};
+
+static const struct mbus_pixfmt_desc pix_desc[] = {
+	{ V4L2_MBUS_FMT_YUYV8_2X8, FIMC_REG_CISRCFMT_ORDER422_YCBYCR, 8 },
+	{ V4L2_MBUS_FMT_YVYU8_2X8, FIMC_REG_CISRCFMT_ORDER422_YCRYCB, 8 },
+	{ V4L2_MBUS_FMT_VYUY8_2X8, FIMC_REG_CISRCFMT_ORDER422_CRYCBY, 8 },
+	{ V4L2_MBUS_FMT_UYVY8_2X8, FIMC_REG_CISRCFMT_ORDER422_CBYCRY, 8 },
+};
+
 int fimc_hw_set_camera_source(struct fimc_dev *fimc,
 			      struct s5p_fimc_isp_info *cam)
 {
@@ -602,18 +602,6 @@ int fimc_hw_set_camera_source(struct fimc_dev *fimc,
 	u32 bus_width;
 	int i;
 
-	static const struct {
-		u32 pixelcode;
-		u32 cisrcfmt;
-		u16 bus_width;
-	} pix_desc[] = {
-		{ V4L2_MBUS_FMT_YUYV8_2X8, S5P_CISRCFMT_ORDER422_YCBYCR, 8 },
-		{ V4L2_MBUS_FMT_YVYU8_2X8, S5P_CISRCFMT_ORDER422_YCRYCB, 8 },
-		{ V4L2_MBUS_FMT_VYUY8_2X8, S5P_CISRCFMT_ORDER422_CRYCBY, 8 },
-		{ V4L2_MBUS_FMT_UYVY8_2X8, S5P_CISRCFMT_ORDER422_CBYCRY, 8 },
-		/* TODO: Add pixel codes for 16-bit bus width */
-	};
-
 	if (cam->bus_type == FIMC_ITU_601 || cam->bus_type == FIMC_ITU_656) {
 		for (i = 0; i < ARRAY_SIZE(pix_desc); i++) {
 			if (fimc->vid_cap.mf.code == pix_desc[i].pixelcode) {
@@ -632,41 +620,37 @@ int fimc_hw_set_camera_source(struct fimc_dev *fimc,
 
 		if (cam->bus_type == FIMC_ITU_601) {
 			if (bus_width == 8)
-				cfg |= S5P_CISRCFMT_ITU601_8BIT;
+				cfg |= FIMC_REG_CISRCFMT_ITU601_8BIT;
 			else if (bus_width == 16)
-				cfg |= S5P_CISRCFMT_ITU601_16BIT;
+				cfg |= FIMC_REG_CISRCFMT_ITU601_16BIT;
 		} /* else defaults to ITU-R BT.656 8-bit */
 	} else if (cam->bus_type == FIMC_MIPI_CSI2) {
 		if (fimc_fmt_is_jpeg(f->fmt->color))
-			cfg |= S5P_CISRCFMT_ITU601_8BIT;
+			cfg |= FIMC_REG_CISRCFMT_ITU601_8BIT;
 	}
 
-	cfg |= S5P_CISRCFMT_HSIZE(f->o_width) | S5P_CISRCFMT_VSIZE(f->o_height);
-	writel(cfg, fimc->regs + S5P_CISRCFMT);
+	cfg |= (f->o_width << 16) | f->o_height;
+	writel(cfg, fimc->regs + FIMC_REG_CISRCFMT);
 	return 0;
 }
 
-
-int fimc_hw_set_camera_offset(struct fimc_dev *fimc, struct fimc_frame *f)
+void fimc_hw_set_camera_offset(struct fimc_dev *fimc, struct fimc_frame *f)
 {
 	u32 hoff2, voff2;
 
-	u32 cfg = readl(fimc->regs + S5P_CIWDOFST);
+	u32 cfg = readl(fimc->regs + FIMC_REG_CIWDOFST);
 
-	cfg &= ~(S5P_CIWDOFST_HOROFF_MASK | S5P_CIWDOFST_VEROFF_MASK);
-	cfg |=  S5P_CIWDOFST_OFF_EN |
-		S5P_CIWDOFST_HOROFF(f->offs_h) |
-		S5P_CIWDOFST_VEROFF(f->offs_v);
+	cfg &= ~(FIMC_REG_CIWDOFST_HOROFF_MASK | FIMC_REG_CIWDOFST_VEROFF_MASK);
+	cfg |=  FIMC_REG_CIWDOFST_OFF_EN |
+		(f->offs_h << 16) | f->offs_v;
 
-	writel(cfg, fimc->regs + S5P_CIWDOFST);
+	writel(cfg, fimc->regs + FIMC_REG_CIWDOFST);
 
 	/* See CIWDOFSTn register description in the datasheet for details. */
 	hoff2 = f->o_width - f->width - f->offs_h;
 	voff2 = f->o_height - f->height - f->offs_v;
-	cfg = S5P_CIWDOFST2_HOROFF(hoff2) | S5P_CIWDOFST2_VEROFF(voff2);
-
-	writel(cfg, fimc->regs + S5P_CIWDOFST2);
-	return 0;
+	cfg = (hoff2 << 16) | voff2;
+	writel(cfg, fimc->regs + FIMC_REG_CIWDOFST2);
 }
 
 int fimc_hw_set_camera_type(struct fimc_dev *fimc,
@@ -674,28 +658,29 @@ int fimc_hw_set_camera_type(struct fimc_dev *fimc,
 {
 	u32 cfg, tmp;
 	struct fimc_vid_cap *vid_cap = &fimc->vid_cap;
+	u32 csis_data_alignment = 32;
 
-	cfg = readl(fimc->regs + S5P_CIGCTRL);
+	cfg = readl(fimc->regs + FIMC_REG_CIGCTRL);
 
 	/* Select ITU B interface, disable Writeback path and test pattern. */
-	cfg &= ~(S5P_CIGCTRL_TESTPAT_MASK | S5P_CIGCTRL_SELCAM_ITU_A |
-		S5P_CIGCTRL_SELCAM_MIPI | S5P_CIGCTRL_CAMIF_SELWB |
-		S5P_CIGCTRL_SELCAM_MIPI_A | S5P_CIGCTRL_CAM_JPEG);
+	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);
 
 	if (cam->bus_type == FIMC_MIPI_CSI2) {
-		cfg |= S5P_CIGCTRL_SELCAM_MIPI;
+		cfg |= FIMC_REG_CIGCTRL_SELCAM_MIPI;
 
 		if (cam->mux_id == 0)
-			cfg |= S5P_CIGCTRL_SELCAM_MIPI_A;
+			cfg |= FIMC_REG_CIGCTRL_SELCAM_MIPI_A;
 
 		/* TODO: add remaining supported formats. */
 		switch (vid_cap->mf.code) {
 		case V4L2_MBUS_FMT_VYUY8_2X8:
-			tmp = S5P_CSIIMGFMT_YCBCR422_8BIT;
+			tmp = FIMC_REG_CSIIMGFMT_YCBCR422_8BIT;
 			break;
 		case V4L2_MBUS_FMT_JPEG_1X8:
-			tmp = S5P_CSIIMGFMT_USER(1);
-			cfg |= S5P_CIGCTRL_CAM_JPEG;
+			tmp = FIMC_REG_CSIIMGFMT_USER(1);
+			cfg |= FIMC_REG_CIGCTRL_CAM_JPEG;
 			break;
 		default:
 			v4l2_err(fimc->vid_cap.vfd,
@@ -703,21 +688,86 @@ int fimc_hw_set_camera_type(struct fimc_dev *fimc,
 				 vid_cap->mf.code);
 			return -EINVAL;
 		}
-		tmp |= (cam->csi_data_align == 32) << 8;
+		tmp |= (csis_data_alignment == 32) << 8;
 
-		writel(tmp, fimc->regs + S5P_CSIIMGFMT);
+		writel(tmp, fimc->regs + FIMC_REG_CSIIMGFMT);
 
 	} else if (cam->bus_type == FIMC_ITU_601 ||
 		   cam->bus_type == FIMC_ITU_656) {
 		if (cam->mux_id == 0) /* ITU-A, ITU-B: 0, 1 */
-			cfg |= S5P_CIGCTRL_SELCAM_ITU_A;
+			cfg |= FIMC_REG_CIGCTRL_SELCAM_ITU_A;
 	} else if (cam->bus_type == FIMC_LCD_WB) {
-		cfg |= S5P_CIGCTRL_CAMIF_SELWB;
+		cfg |= FIMC_REG_CIGCTRL_CAMIF_SELWB;
 	} else {
 		err("invalid camera bus type selected\n");
 		return -EINVAL;
 	}
-	writel(cfg, fimc->regs + S5P_CIGCTRL);
+	writel(cfg, fimc->regs + FIMC_REG_CIGCTRL);
 
 	return 0;
 }
+
+void fimc_hw_clear_irq(struct fimc_dev *dev)
+{
+	u32 cfg = readl(dev->regs + FIMC_REG_CIGCTRL);
+	cfg |= FIMC_REG_CIGCTRL_IRQ_CLR;
+	writel(cfg, dev->regs + FIMC_REG_CIGCTRL);
+}
+
+void fimc_hw_enable_scaler(struct fimc_dev *dev, bool on)
+{
+	u32 cfg = readl(dev->regs + FIMC_REG_CISCCTRL);
+	if (on)
+		cfg |= FIMC_REG_CISCCTRL_SCALERSTART;
+	else
+		cfg &= ~FIMC_REG_CISCCTRL_SCALERSTART;
+	writel(cfg, dev->regs + FIMC_REG_CISCCTRL);
+}
+
+void fimc_hw_activate_input_dma(struct fimc_dev *dev, bool on)
+{
+	u32 cfg = readl(dev->regs + FIMC_REG_MSCTRL);
+	if (on)
+		cfg |= FIMC_REG_MSCTRL_ENVID;
+	else
+		cfg &= ~FIMC_REG_MSCTRL_ENVID;
+	writel(cfg, dev->regs + FIMC_REG_MSCTRL);
+}
+
+void fimc_hw_dis_capture(struct fimc_dev *dev)
+{
+	u32 cfg = readl(dev->regs + FIMC_REG_CIIMGCPT);
+	cfg &= ~(FIMC_REG_CIIMGCPT_IMGCPTEN | FIMC_REG_CIIMGCPT_IMGCPTEN_SC);
+	writel(cfg, dev->regs + FIMC_REG_CIIMGCPT);
+}
+
+/* Return an index to the buffer actually being written. */
+u32 fimc_hw_get_frame_index(struct fimc_dev *dev)
+{
+	u32 reg;
+
+	if (dev->variant->has_cistatus2) {
+		reg = readl(dev->regs + FIMC_REG_CISTATUS2) & 0x3F;
+		return reg > 0 ? --reg : reg;
+	}
+
+	reg = readl(dev->regs + FIMC_REG_CISTATUS);
+
+	return (reg & FIMC_REG_CISTATUS_FRAMECNT_MASK) >>
+		FIMC_REG_CISTATUS_FRAMECNT_SHIFT;
+}
+
+/* Locking: the caller holds fimc->slock */
+void fimc_activate_capture(struct fimc_ctx *ctx)
+{
+	fimc_hw_enable_scaler(ctx->fimc_dev, ctx->scaler.enabled);
+	fimc_hw_en_capture(ctx);
+}
+
+void fimc_deactivate_capture(struct fimc_dev *fimc)
+{
+	fimc_hw_en_lastirq(fimc, true);
+	fimc_hw_dis_capture(fimc);
+	fimc_hw_enable_scaler(fimc, false);
+	fimc_hw_en_lastirq(fimc, false);
+}
diff --git a/drivers/media/video/s5p-fimc/fimc-reg.h b/drivers/media/video/s5p-fimc/fimc-reg.h
new file mode 100644
index 00000000000..579ac8ac03d
--- /dev/null
+++ b/drivers/media/video/s5p-fimc/fimc-reg.h
@@ -0,0 +1,326 @@
+/*
+ * Samsung camera host interface (FIMC) registers definition
+ *
+ * Copyright (C) 2010 - 2012 Samsung Electronics Co., Ltd.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ */
+
+#ifndef FIMC_REG_H_
+#define FIMC_REG_H_
+
+#include "fimc-core.h"
+
+/* Input source format */
+#define FIMC_REG_CISRCFMT			0x00
+#define FIMC_REG_CISRCFMT_ITU601_8BIT		(1 << 31)
+#define FIMC_REG_CISRCFMT_ITU601_16BIT		(1 << 29)
+#define FIMC_REG_CISRCFMT_ORDER422_YCBYCR	(0 << 14)
+#define FIMC_REG_CISRCFMT_ORDER422_YCRYCB	(1 << 14)
+#define FIMC_REG_CISRCFMT_ORDER422_CBYCRY	(2 << 14)
+#define FIMC_REG_CISRCFMT_ORDER422_CRYCBY	(3 << 14)
+
+/* Window offset */
+#define FIMC_REG_CIWDOFST			0x04
+#define FIMC_REG_CIWDOFST_OFF_EN		(1 << 31)
+#define FIMC_REG_CIWDOFST_CLROVFIY		(1 << 30)
+#define FIMC_REG_CIWDOFST_CLROVRLB		(1 << 29)
+#define FIMC_REG_CIWDOFST_HOROFF_MASK		(0x7ff << 16)
+#define FIMC_REG_CIWDOFST_CLROVFICB		(1 << 15)
+#define FIMC_REG_CIWDOFST_CLROVFICR		(1 << 14)
+#define FIMC_REG_CIWDOFST_VEROFF_MASK		(0xfff << 0)
+
+/* Global control */
+#define FIMC_REG_CIGCTRL			0x08
+#define FIMC_REG_CIGCTRL_SWRST			(1 << 31)
+#define FIMC_REG_CIGCTRL_CAMRST_A		(1 << 30)
+#define FIMC_REG_CIGCTRL_SELCAM_ITU_A		(1 << 29)
+#define FIMC_REG_CIGCTRL_TESTPAT_NORMAL		(0 << 27)
+#define FIMC_REG_CIGCTRL_TESTPAT_COLOR_BAR	(1 << 27)
+#define FIMC_REG_CIGCTRL_TESTPAT_HOR_INC	(2 << 27)
+#define FIMC_REG_CIGCTRL_TESTPAT_VER_INC	(3 << 27)
+#define FIMC_REG_CIGCTRL_TESTPAT_MASK		(3 << 27)
+#define FIMC_REG_CIGCTRL_TESTPAT_SHIFT		27
+#define FIMC_REG_CIGCTRL_INVPOLPCLK		(1 << 26)
+#define FIMC_REG_CIGCTRL_INVPOLVSYNC		(1 << 25)
+#define FIMC_REG_CIGCTRL_INVPOLHREF		(1 << 24)
+#define FIMC_REG_CIGCTRL_IRQ_OVFEN		(1 << 22)
+#define FIMC_REG_CIGCTRL_HREF_MASK		(1 << 21)
+#define FIMC_REG_CIGCTRL_IRQ_LEVEL		(1 << 20)
+#define FIMC_REG_CIGCTRL_IRQ_CLR		(1 << 19)
+#define FIMC_REG_CIGCTRL_IRQ_ENABLE		(1 << 16)
+#define FIMC_REG_CIGCTRL_SHDW_DISABLE		(1 << 12)
+#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)
+/* 0 - ITU601; 1 - ITU709 */
+#define FIMC_REG_CIGCTRL_CSC_ITU601_709		(1 << 5)
+#define FIMC_REG_CIGCTRL_INVPOLHSYNC		(1 << 4)
+#define FIMC_REG_CIGCTRL_SELCAM_MIPI		(1 << 3)
+#define FIMC_REG_CIGCTRL_INVPOLFIELD		(1 << 1)
+#define FIMC_REG_CIGCTRL_INTERLACE		(1 << 0)
+
+/* Window offset 2 */
+#define FIMC_REG_CIWDOFST2			0x14
+#define FIMC_REG_CIWDOFST2_HOROFF_MASK		(0xfff << 16)
+#define FIMC_REG_CIWDOFST2_VEROFF_MASK		(0xfff << 0)
+
+/* Output DMA Y/Cb/Cr plane start addresses */
+#define FIMC_REG_CIOYSA(n)			(0x18 + (n) * 4)
+#define FIMC_REG_CIOCBSA(n)			(0x28 + (n) * 4)
+#define FIMC_REG_CIOCRSA(n)			(0x38 + (n) * 4)
+
+/* Target image format */
+#define FIMC_REG_CITRGFMT			0x48
+#define FIMC_REG_CITRGFMT_INROT90		(1 << 31)
+#define FIMC_REG_CITRGFMT_YCBCR420		(0 << 29)
+#define FIMC_REG_CITRGFMT_YCBCR422		(1 << 29)
+#define FIMC_REG_CITRGFMT_YCBCR422_1P		(2 << 29)
+#define FIMC_REG_CITRGFMT_RGB			(3 << 29)
+#define FIMC_REG_CITRGFMT_FMT_MASK		(3 << 29)
+#define FIMC_REG_CITRGFMT_HSIZE_MASK		(0xfff << 16)
+#define FIMC_REG_CITRGFMT_FLIP_SHIFT		14
+#define FIMC_REG_CITRGFMT_FLIP_NORMAL		(0 << 14)
+#define FIMC_REG_CITRGFMT_FLIP_X_MIRROR		(1 << 14)
+#define FIMC_REG_CITRGFMT_FLIP_Y_MIRROR		(2 << 14)
+#define FIMC_REG_CITRGFMT_FLIP_180		(3 << 14)
+#define FIMC_REG_CITRGFMT_FLIP_MASK		(3 << 14)
+#define FIMC_REG_CITRGFMT_OUTROT90		(1 << 13)
+#define FIMC_REG_CITRGFMT_VSIZE_MASK		(0xfff << 0)
+
+/* 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_LASTIRQ_ENABLE		(1 << 2)
+#define FIMC_REG_CIOCTRL_YCBCR_3PLANE		(0 << 3)
+#define FIMC_REG_CIOCTRL_YCBCR_2PLANE		(1 << 3)
+#define FIMC_REG_CIOCTRL_YCBCR_PLANE_MASK	(1 << 3)
+#define FIMC_REG_CIOCTRL_ALPHA_OUT_MASK		(0xff << 4)
+#define FIMC_REG_CIOCTRL_RGB16FMT_MASK		(3 << 16)
+#define FIMC_REG_CIOCTRL_RGB565			(0 << 16)
+#define FIMC_REG_CIOCTRL_ARGB1555		(1 << 16)
+#define FIMC_REG_CIOCTRL_ARGB4444		(2 << 16)
+#define FIMC_REG_CIOCTRL_ORDER2P_SHIFT		24
+#define FIMC_REG_CIOCTRL_ORDER2P_MASK		(3 << 24)
+#define FIMC_REG_CIOCTRL_ORDER422_2P_LSB_CRCB	(0 << 24)
+
+/* Pre-scaler control 1 */
+#define FIMC_REG_CISCPRERATIO			0x50
+
+#define FIMC_REG_CISCPREDST			0x54
+
+/* Main scaler control */
+#define FIMC_REG_CISCCTRL			0x58
+#define FIMC_REG_CISCCTRL_SCALERBYPASS		(1 << 31)
+#define FIMC_REG_CISCCTRL_SCALEUP_H		(1 << 30)
+#define FIMC_REG_CISCCTRL_SCALEUP_V		(1 << 29)
+#define FIMC_REG_CISCCTRL_CSCR2Y_WIDE		(1 << 28)
+#define FIMC_REG_CISCCTRL_CSCY2R_WIDE		(1 << 27)
+#define FIMC_REG_CISCCTRL_LCDPATHEN_FIFO	(1 << 26)
+#define FIMC_REG_CISCCTRL_INTERLACE		(1 << 25)
+#define FIMC_REG_CISCCTRL_SCALERSTART		(1 << 15)
+#define FIMC_REG_CISCCTRL_INRGB_FMT_RGB565	(0 << 13)
+#define FIMC_REG_CISCCTRL_INRGB_FMT_RGB666	(1 << 13)
+#define FIMC_REG_CISCCTRL_INRGB_FMT_RGB888	(2 << 13)
+#define FIMC_REG_CISCCTRL_INRGB_FMT_MASK	(3 << 13)
+#define FIMC_REG_CISCCTRL_OUTRGB_FMT_RGB565	(0 << 11)
+#define FIMC_REG_CISCCTRL_OUTRGB_FMT_RGB666	(1 << 11)
+#define FIMC_REG_CISCCTRL_OUTRGB_FMT_RGB888	(2 << 11)
+#define FIMC_REG_CISCCTRL_OUTRGB_FMT_MASK	(3 << 11)
+#define FIMC_REG_CISCCTRL_RGB_EXT		(1 << 10)
+#define FIMC_REG_CISCCTRL_ONE2ONE		(1 << 9)
+#define FIMC_REG_CISCCTRL_MHRATIO(x)		((x) << 16)
+#define FIMC_REG_CISCCTRL_MVRATIO(x)		((x) << 0)
+#define FIMC_REG_CISCCTRL_MHRATIO_MASK		(0x1ff << 16)
+#define FIMC_REG_CISCCTRL_MVRATIO_MASK		(0x1ff << 0)
+#define FIMC_REG_CISCCTRL_MHRATIO_EXT(x)	(((x) >> 6) << 16)
+#define FIMC_REG_CISCCTRL_MVRATIO_EXT(x)	(((x) >> 6) << 0)
+
+/* Target area */
+#define FIMC_REG_CITAREA			0x5c
+#define FIMC_REG_CITAREA_MASK			0x0fffffff
+
+/* General status */
+#define FIMC_REG_CISTATUS			0x64
+#define FIMC_REG_CISTATUS_OVFIY			(1 << 31)
+#define FIMC_REG_CISTATUS_OVFICB		(1 << 30)
+#define FIMC_REG_CISTATUS_OVFICR		(1 << 29)
+#define FIMC_REG_CISTATUS_VSYNC			(1 << 28)
+#define FIMC_REG_CISTATUS_FRAMECNT_MASK		(3 << 26)
+#define FIMC_REG_CISTATUS_FRAMECNT_SHIFT	26
+#define FIMC_REG_CISTATUS_WINOFF_EN		(1 << 25)
+#define FIMC_REG_CISTATUS_IMGCPT_EN		(1 << 22)
+#define FIMC_REG_CISTATUS_IMGCPT_SCEN		(1 << 21)
+#define FIMC_REG_CISTATUS_VSYNC_A		(1 << 20)
+#define FIMC_REG_CISTATUS_VSYNC_B		(1 << 19)
+#define FIMC_REG_CISTATUS_OVRLB			(1 << 18)
+#define FIMC_REG_CISTATUS_FRAME_END		(1 << 17)
+#define FIMC_REG_CISTATUS_LASTCAPT_END		(1 << 16)
+#define FIMC_REG_CISTATUS_VVALID_A		(1 << 15)
+#define FIMC_REG_CISTATUS_VVALID_B		(1 << 14)
+
+/* Indexes to the last and the currently processed buffer. */
+#define FIMC_REG_CISTATUS2			0x68
+
+/* Image capture control */
+#define FIMC_REG_CIIMGCPT			0xc0
+#define FIMC_REG_CIIMGCPT_IMGCPTEN		(1 << 31)
+#define FIMC_REG_CIIMGCPT_IMGCPTEN_SC		(1 << 30)
+#define FIMC_REG_CIIMGCPT_CPT_FREN_ENABLE	(1 << 25)
+#define FIMC_REG_CIIMGCPT_CPT_FRMOD_CNT		(1 << 18)
+
+/* Frame capture sequence */
+#define FIMC_REG_CICPTSEQ			0xc4
+
+/* Image effect */
+#define FIMC_REG_CIIMGEFF			0xd0
+#define FIMC_REG_CIIMGEFF_IE_ENABLE		(1 << 30)
+#define FIMC_REG_CIIMGEFF_IE_SC_BEFORE		(0 << 29)
+#define FIMC_REG_CIIMGEFF_IE_SC_AFTER		(1 << 29)
+#define FIMC_REG_CIIMGEFF_FIN_BYPASS		(0 << 26)
+#define FIMC_REG_CIIMGEFF_FIN_ARBITRARY		(1 << 26)
+#define FIMC_REG_CIIMGEFF_FIN_NEGATIVE		(2 << 26)
+#define FIMC_REG_CIIMGEFF_FIN_ARTFREEZE		(3 << 26)
+#define FIMC_REG_CIIMGEFF_FIN_EMBOSSING		(4 << 26)
+#define FIMC_REG_CIIMGEFF_FIN_SILHOUETTE	(5 << 26)
+#define FIMC_REG_CIIMGEFF_FIN_MASK		(7 << 26)
+#define FIMC_REG_CIIMGEFF_PAT_CBCR_MASK		((0xff << 13) | 0xff)
+
+/* Input DMA Y/Cb/Cr plane start address 0/1 */
+#define FIMC_REG_CIIYSA(n)			(0xd4 + (n) * 0x70)
+#define FIMC_REG_CIICBSA(n)			(0xd8 + (n) * 0x70)
+#define FIMC_REG_CIICRSA(n)			(0xdc + (n) * 0x70)
+
+/* Real input DMA image size */
+#define FIMC_REG_CIREAL_ISIZE			0xf8
+#define FIMC_REG_CIREAL_ISIZE_AUTOLOAD_EN	(1 << 31)
+#define FIMC_REG_CIREAL_ISIZE_ADDR_CH_DIS	(1 << 30)
+
+/* Input DMA control */
+#define FIMC_REG_MSCTRL				0xfc
+#define FIMC_REG_MSCTRL_IN_BURST_COUNT_MASK	(0xf << 24)
+#define FIMC_REG_MSCTRL_2P_IN_ORDER_MASK	(3 << 16)
+#define FIMC_REG_MSCTRL_2P_IN_ORDER_SHIFT	16
+#define FIMC_REG_MSCTRL_C_INT_IN_3PLANE		(0 << 15)
+#define FIMC_REG_MSCTRL_C_INT_IN_2PLANE		(1 << 15)
+#define FIMC_REG_MSCTRL_C_INT_IN_MASK		(1 << 15)
+#define FIMC_REG_MSCTRL_FLIP_SHIFT		13
+#define FIMC_REG_MSCTRL_FLIP_MASK		(3 << 13)
+#define FIMC_REG_MSCTRL_FLIP_NORMAL		(0 << 13)
+#define FIMC_REG_MSCTRL_FLIP_X_MIRROR		(1 << 13)
+#define FIMC_REG_MSCTRL_FLIP_Y_MIRROR		(2 << 13)
+#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_MASK		(3 << 4)
+#define FIMC_REG_MSCTRL_INPUT_EXTCAM		(0 << 3)
+#define FIMC_REG_MSCTRL_INPUT_MEMORY		(1 << 3)
+#define FIMC_REG_MSCTRL_INPUT_MASK		(1 << 3)
+#define FIMC_REG_MSCTRL_INFORMAT_YCBCR420	(0 << 1)
+#define FIMC_REG_MSCTRL_INFORMAT_YCBCR422	(1 << 1)
+#define FIMC_REG_MSCTRL_INFORMAT_YCBCR422_1P	(2 << 1)
+#define FIMC_REG_MSCTRL_INFORMAT_RGB		(3 << 1)
+#define FIMC_REG_MSCTRL_INFORMAT_MASK		(3 << 1)
+#define FIMC_REG_MSCTRL_ENVID			(1 << 0)
+#define FIMC_REG_MSCTRL_IN_BURST_COUNT(x)	((x) << 24)
+
+/* Output DMA Y/Cb/Cr offset */
+#define FIMC_REG_CIOYOFF			0x168
+#define FIMC_REG_CIOCBOFF			0x16c
+#define FIMC_REG_CIOCROFF			0x170
+
+/* Input DMA Y/Cb/Cr offset */
+#define FIMC_REG_CIIYOFF			0x174
+#define FIMC_REG_CIICBOFF			0x178
+#define FIMC_REG_CIICROFF			0x17c
+
+/* Input DMA original image size */
+#define FIMC_REG_ORGISIZE			0x180
+
+/* Output DMA original image size */
+#define FIMC_REG_ORGOSIZE			0x184
+
+/* Real output DMA image size (extension register) */
+#define FIMC_REG_CIEXTEN			0x188
+#define FIMC_REG_CIEXTEN_MHRATIO_EXT(x)		(((x) & 0x3f) << 10)
+#define FIMC_REG_CIEXTEN_MVRATIO_EXT(x)		((x) & 0x3f)
+#define FIMC_REG_CIEXTEN_MHRATIO_EXT_MASK	(0x3f << 10)
+#define FIMC_REG_CIEXTEN_MVRATIO_EXT_MASK	0x3f
+
+#define FIMC_REG_CIDMAPARAM			0x18c
+#define FIMC_REG_CIDMAPARAM_R_LINEAR		(0 << 29)
+#define FIMC_REG_CIDMAPARAM_R_64X32		(3 << 29)
+#define FIMC_REG_CIDMAPARAM_W_LINEAR		(0 << 13)
+#define FIMC_REG_CIDMAPARAM_W_64X32		(3 << 13)
+#define FIMC_REG_CIDMAPARAM_TILE_MASK		((3 << 29) | (3 << 13))
+
+/* MIPI CSI image format */
+#define FIMC_REG_CSIIMGFMT			0x194
+#define FIMC_REG_CSIIMGFMT_YCBCR422_8BIT	0x1e
+#define FIMC_REG_CSIIMGFMT_RAW8			0x2a
+#define FIMC_REG_CSIIMGFMT_RAW10		0x2b
+#define FIMC_REG_CSIIMGFMT_RAW12		0x2c
+/* User defined formats. x = 0...16. */
+#define FIMC_REG_CSIIMGFMT_USER(x)		(0x30 + x - 1)
+
+/* Output frame buffer sequence mask */
+#define FIMC_REG_CIFCNTSEQ			0x1fc
+
+/*
+ * Function declarations
+ */
+void fimc_hw_reset(struct fimc_dev *fimc);
+void fimc_hw_set_rotation(struct fimc_ctx *ctx);
+void fimc_hw_set_target_format(struct fimc_ctx *ctx);
+void fimc_hw_set_out_dma(struct fimc_ctx *ctx);
+void fimc_hw_en_lastirq(struct fimc_dev *fimc, int enable);
+void fimc_hw_en_irq(struct fimc_dev *fimc, int enable);
+void fimc_hw_set_prescaler(struct fimc_ctx *ctx);
+void fimc_hw_set_mainscaler(struct fimc_ctx *ctx);
+void fimc_hw_en_capture(struct fimc_ctx *ctx);
+void fimc_hw_set_effect(struct fimc_ctx *ctx);
+void fimc_hw_set_rgb_alpha(struct fimc_ctx *ctx);
+void fimc_hw_set_in_dma(struct fimc_ctx *ctx);
+void fimc_hw_set_input_path(struct fimc_ctx *ctx);
+void fimc_hw_set_output_path(struct fimc_ctx *ctx);
+void fimc_hw_set_input_addr(struct fimc_dev *fimc, struct fimc_addr *paddr);
+void fimc_hw_set_output_addr(struct fimc_dev *fimc, struct fimc_addr *paddr,
+			     int index);
+int fimc_hw_set_camera_source(struct fimc_dev *fimc,
+			      struct s5p_fimc_isp_info *cam);
+void fimc_hw_set_camera_offset(struct fimc_dev *fimc, struct fimc_frame *f);
+int fimc_hw_set_camera_polarity(struct fimc_dev *fimc,
+				struct s5p_fimc_isp_info *cam);
+int fimc_hw_set_camera_type(struct fimc_dev *fimc,
+			    struct s5p_fimc_isp_info *cam);
+void fimc_hw_clear_irq(struct fimc_dev *dev);
+void fimc_hw_enable_scaler(struct fimc_dev *dev, bool on);
+void fimc_hw_activate_input_dma(struct fimc_dev *dev, bool on);
+void fimc_hw_dis_capture(struct fimc_dev *dev);
+u32 fimc_hw_get_frame_index(struct fimc_dev *dev);
+void fimc_activate_capture(struct fimc_ctx *ctx);
+void fimc_deactivate_capture(struct fimc_dev *fimc);
+
+/**
+ * fimc_hw_set_dma_seq - configure output DMA buffer sequence
+ * @mask: bitmask for the DMA output buffer registers, set to 0 to skip buffer
+ * This function masks output DMA ring buffers, it allows to select which of
+ * the 32 available output buffer address registers will be used by the DMA
+ * engine.
+ */
+static inline void fimc_hw_set_dma_seq(struct fimc_dev *dev, u32 mask)
+{
+	writel(mask, dev->regs + FIMC_REG_CIFCNTSEQ);
+}
+
+#endif /* FIMC_REG_H_ */
diff --git a/drivers/media/video/s5p-fimc/mipi-csis.c b/drivers/media/video/s5p-fimc/mipi-csis.c
index f44f690397f..2f73d9e3d0b 100644
--- a/drivers/media/video/s5p-fimc/mipi-csis.c
+++ b/drivers/media/video/s5p-fimc/mipi-csis.c
@@ -127,20 +127,24 @@ struct csis_state {
  *                       multiple of 2^pix_width_alignment
  * @code: corresponding media bus code
  * @fmt_reg: S5PCSIS_CONFIG register value
+ * @data_alignment: MIPI-CSI data alignment in bits
  */
 struct csis_pix_format {
 	unsigned int pix_width_alignment;
 	enum v4l2_mbus_pixelcode code;
 	u32 fmt_reg;
+	u8 data_alignment;
 };
 
 static const struct csis_pix_format s5pcsis_formats[] = {
 	{
 		.code = V4L2_MBUS_FMT_VYUY8_2X8,
 		.fmt_reg = S5PCSIS_CFG_FMT_YCBCR422_8BIT,
+		.data_alignment = 32,
 	}, {
 		.code = V4L2_MBUS_FMT_JPEG_1X8,
 		.fmt_reg = S5PCSIS_CFG_FMT_USER(1),
+		.data_alignment = 32,
 	},
 };
 
@@ -239,7 +243,7 @@ static void s5pcsis_set_params(struct csis_state *state)
 	s5pcsis_set_hsync_settle(state, pdata->hs_settle);
 
 	val = s5pcsis_read(state, S5PCSIS_CTRL);
-	if (pdata->alignment == 32)
+	if (state->csis_fmt->data_alignment == 32)
 		val |= S5PCSIS_CTRL_ALIGN_32BIT;
 	else /* 24-bits */
 		val &= ~S5PCSIS_CTRL_ALIGN_32BIT;
@@ -711,19 +715,8 @@ static struct platform_driver s5pcsis_driver = {
 	},
 };
 
-static int __init s5pcsis_init(void)
-{
-	return platform_driver_probe(&s5pcsis_driver, s5pcsis_probe);
-}
-
-static void __exit s5pcsis_exit(void)
-{
-	platform_driver_unregister(&s5pcsis_driver);
-}
-
-module_init(s5pcsis_init);
-module_exit(s5pcsis_exit);
+module_platform_driver(s5pcsis_driver);
 
 MODULE_AUTHOR("Sylwester Nawrocki <s.nawrocki@samsung.com>");
-MODULE_DESCRIPTION("S5P/EXYNOS4 MIPI CSI receiver driver");
+MODULE_DESCRIPTION("Samsung S5P/EXYNOS SoC MIPI-CSI2 receiver driver");
 MODULE_LICENSE("GPL");
diff --git a/drivers/media/video/s5p-fimc/regs-fimc.h b/drivers/media/video/s5p-fimc/regs-fimc.h
deleted file mode 100644
index c7a5bc51d57..00000000000
--- a/drivers/media/video/s5p-fimc/regs-fimc.h
+++ /dev/null
@@ -1,301 +0,0 @@
-/*
- * Register definition file for Samsung Camera Interface (FIMC) driver
- *
- * Copyright (c) 2010 Samsung Electronics
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License version 2 as
- * published by the Free Software Foundation.
- */
-
-#ifndef REGS_FIMC_H_
-#define REGS_FIMC_H_
-
-/* Input source format */
-#define S5P_CISRCFMT			0x00
-#define S5P_CISRCFMT_ITU601_8BIT	(1 << 31)
-#define S5P_CISRCFMT_ITU601_16BIT	(1 << 29)
-#define S5P_CISRCFMT_ORDER422_YCBYCR	(0 << 14)
-#define S5P_CISRCFMT_ORDER422_YCRYCB	(1 << 14)
-#define S5P_CISRCFMT_ORDER422_CBYCRY	(2 << 14)
-#define S5P_CISRCFMT_ORDER422_CRYCBY	(3 << 14)
-#define S5P_CISRCFMT_HSIZE(x)		((x) << 16)
-#define S5P_CISRCFMT_VSIZE(x)		((x) << 0)
-
-/* Window offset */
-#define S5P_CIWDOFST			0x04
-#define S5P_CIWDOFST_OFF_EN		(1 << 31)
-#define S5P_CIWDOFST_CLROVFIY		(1 << 30)
-#define S5P_CIWDOFST_CLROVRLB		(1 << 29)
-#define S5P_CIWDOFST_HOROFF_MASK	(0x7ff << 16)
-#define S5P_CIWDOFST_CLROVFICB		(1 << 15)
-#define S5P_CIWDOFST_CLROVFICR		(1 << 14)
-#define S5P_CIWDOFST_HOROFF(x)		((x) << 16)
-#define S5P_CIWDOFST_VEROFF(x)		((x) << 0)
-#define S5P_CIWDOFST_VEROFF_MASK	(0xfff << 0)
-
-/* Global control */
-#define S5P_CIGCTRL			0x08
-#define S5P_CIGCTRL_SWRST		(1 << 31)
-#define S5P_CIGCTRL_CAMRST_A		(1 << 30)
-#define S5P_CIGCTRL_SELCAM_ITU_A	(1 << 29)
-#define S5P_CIGCTRL_TESTPAT_NORMAL	(0 << 27)
-#define S5P_CIGCTRL_TESTPAT_COLOR_BAR	(1 << 27)
-#define S5P_CIGCTRL_TESTPAT_HOR_INC	(2 << 27)
-#define S5P_CIGCTRL_TESTPAT_VER_INC	(3 << 27)
-#define S5P_CIGCTRL_TESTPAT_MASK	(3 << 27)
-#define S5P_CIGCTRL_TESTPAT_SHIFT	(27)
-#define S5P_CIGCTRL_INVPOLPCLK		(1 << 26)
-#define S5P_CIGCTRL_INVPOLVSYNC		(1 << 25)
-#define S5P_CIGCTRL_INVPOLHREF		(1 << 24)
-#define S5P_CIGCTRL_IRQ_OVFEN		(1 << 22)
-#define S5P_CIGCTRL_HREF_MASK		(1 << 21)
-#define S5P_CIGCTRL_IRQ_LEVEL		(1 << 20)
-#define S5P_CIGCTRL_IRQ_CLR		(1 << 19)
-#define S5P_CIGCTRL_IRQ_ENABLE		(1 << 16)
-#define S5P_CIGCTRL_SHDW_DISABLE	(1 << 12)
-#define S5P_CIGCTRL_CAM_JPEG		(1 << 8)
-#define S5P_CIGCTRL_SELCAM_MIPI_A	(1 << 7)
-#define S5P_CIGCTRL_CAMIF_SELWB		(1 << 6)
-/* 0 - ITU601; 1 - ITU709 */
-#define S5P_CIGCTRL_CSC_ITU601_709	(1 << 5)
-#define S5P_CIGCTRL_INVPOLHSYNC		(1 << 4)
-#define S5P_CIGCTRL_SELCAM_MIPI		(1 << 3)
-#define S5P_CIGCTRL_INVPOLFIELD		(1 << 1)
-#define S5P_CIGCTRL_INTERLACE		(1 << 0)
-
-/* Window offset 2 */
-#define S5P_CIWDOFST2			0x14
-#define S5P_CIWDOFST2_HOROFF_MASK	(0xfff << 16)
-#define S5P_CIWDOFST2_VEROFF_MASK	(0xfff << 0)
-#define S5P_CIWDOFST2_HOROFF(x)		((x) << 16)
-#define S5P_CIWDOFST2_VEROFF(x)		((x) << 0)
-
-/* Output DMA Y/Cb/Cr plane start addresses */
-#define S5P_CIOYSA(n)			(0x18 + (n) * 4)
-#define S5P_CIOCBSA(n)			(0x28 + (n) * 4)
-#define S5P_CIOCRSA(n)			(0x38 + (n) * 4)
-
-/* Target image format */
-#define S5P_CITRGFMT			0x48
-#define S5P_CITRGFMT_INROT90		(1 << 31)
-#define S5P_CITRGFMT_YCBCR420		(0 << 29)
-#define S5P_CITRGFMT_YCBCR422		(1 << 29)
-#define S5P_CITRGFMT_YCBCR422_1P	(2 << 29)
-#define S5P_CITRGFMT_RGB		(3 << 29)
-#define S5P_CITRGFMT_FMT_MASK		(3 << 29)
-#define S5P_CITRGFMT_HSIZE_MASK		(0xfff << 16)
-#define S5P_CITRGFMT_FLIP_SHIFT		(14)
-#define S5P_CITRGFMT_FLIP_NORMAL	(0 << 14)
-#define S5P_CITRGFMT_FLIP_X_MIRROR	(1 << 14)
-#define S5P_CITRGFMT_FLIP_Y_MIRROR	(2 << 14)
-#define S5P_CITRGFMT_FLIP_180		(3 << 14)
-#define S5P_CITRGFMT_FLIP_MASK		(3 << 14)
-#define S5P_CITRGFMT_OUTROT90		(1 << 13)
-#define S5P_CITRGFMT_VSIZE_MASK		(0xfff << 0)
-#define S5P_CITRGFMT_HSIZE(x)		((x) << 16)
-#define S5P_CITRGFMT_VSIZE(x)		((x) << 0)
-
-/* Output DMA control */
-#define S5P_CIOCTRL			0x4c
-#define S5P_CIOCTRL_ORDER422_MASK	(3 << 0)
-#define S5P_CIOCTRL_ORDER422_CRYCBY	(0 << 0)
-#define S5P_CIOCTRL_ORDER422_CBYCRY	(1 << 0)
-#define S5P_CIOCTRL_ORDER422_YCRYCB	(2 << 0)
-#define S5P_CIOCTRL_ORDER422_YCBYCR	(3 << 0)
-#define S5P_CIOCTRL_LASTIRQ_ENABLE	(1 << 2)
-#define S5P_CIOCTRL_YCBCR_3PLANE	(0 << 3)
-#define S5P_CIOCTRL_YCBCR_2PLANE	(1 << 3)
-#define S5P_CIOCTRL_YCBCR_PLANE_MASK	(1 << 3)
-#define S5P_CIOCTRL_ALPHA_OUT_MASK	(0xff << 4)
-#define S5P_CIOCTRL_RGB16FMT_MASK	(3 << 16)
-#define S5P_CIOCTRL_RGB565		(0 << 16)
-#define S5P_CIOCTRL_ARGB1555		(1 << 16)
-#define S5P_CIOCTRL_ARGB4444		(2 << 16)
-#define S5P_CIOCTRL_ORDER2P_SHIFT	(24)
-#define S5P_CIOCTRL_ORDER2P_MASK	(3 << 24)
-#define S5P_CIOCTRL_ORDER422_2P_LSB_CRCB (0 << 24)
-
-/* Pre-scaler control 1 */
-#define S5P_CISCPRERATIO		0x50
-#define S5P_CISCPRERATIO_SHFACTOR(x)	((x) << 28)
-#define S5P_CISCPRERATIO_HOR(x)		((x) << 16)
-#define S5P_CISCPRERATIO_VER(x)		((x) << 0)
-
-#define S5P_CISCPREDST			0x54
-#define S5P_CISCPREDST_WIDTH(x)		((x) << 16)
-#define S5P_CISCPREDST_HEIGHT(x)	((x) << 0)
-
-/* Main scaler control */
-#define S5P_CISCCTRL			0x58
-#define S5P_CISCCTRL_SCALERBYPASS	(1 << 31)
-#define S5P_CISCCTRL_SCALEUP_H		(1 << 30)
-#define S5P_CISCCTRL_SCALEUP_V		(1 << 29)
-#define S5P_CISCCTRL_CSCR2Y_WIDE	(1 << 28)
-#define S5P_CISCCTRL_CSCY2R_WIDE	(1 << 27)
-#define S5P_CISCCTRL_LCDPATHEN_FIFO	(1 << 26)
-#define S5P_CISCCTRL_INTERLACE		(1 << 25)
-#define S5P_CISCCTRL_SCALERSTART	(1 << 15)
-#define S5P_CISCCTRL_INRGB_FMT_RGB565	(0 << 13)
-#define S5P_CISCCTRL_INRGB_FMT_RGB666	(1 << 13)
-#define S5P_CISCCTRL_INRGB_FMT_RGB888	(2 << 13)
-#define S5P_CISCCTRL_INRGB_FMT_MASK	(3 << 13)
-#define S5P_CISCCTRL_OUTRGB_FMT_RGB565	(0 << 11)
-#define S5P_CISCCTRL_OUTRGB_FMT_RGB666	(1 << 11)
-#define S5P_CISCCTRL_OUTRGB_FMT_RGB888	(2 << 11)
-#define S5P_CISCCTRL_OUTRGB_FMT_MASK	(3 << 11)
-#define S5P_CISCCTRL_RGB_EXT		(1 << 10)
-#define S5P_CISCCTRL_ONE2ONE		(1 << 9)
-#define S5P_CISCCTRL_MHRATIO(x)		((x) << 16)
-#define S5P_CISCCTRL_MVRATIO(x)		((x) << 0)
-#define S5P_CISCCTRL_MHRATIO_MASK	(0x1ff << 16)
-#define S5P_CISCCTRL_MVRATIO_MASK	(0x1ff << 0)
-#define S5P_CISCCTRL_MHRATIO_EXT(x)	(((x) >> 6) << 16)
-#define S5P_CISCCTRL_MVRATIO_EXT(x)	(((x) >> 6) << 0)
-
-/* Target area */
-#define S5P_CITAREA			0x5c
-#define S5P_CITAREA_MASK		0x0fffffff
-
-/* General status */
-#define S5P_CISTATUS			0x64
-#define S5P_CISTATUS_OVFIY		(1 << 31)
-#define S5P_CISTATUS_OVFICB		(1 << 30)
-#define S5P_CISTATUS_OVFICR		(1 << 29)
-#define S5P_CISTATUS_VSYNC		(1 << 28)
-#define S5P_CISTATUS_FRAMECNT_MASK	(3 << 26)
-#define S5P_CISTATUS_FRAMECNT_SHIFT	26
-#define S5P_CISTATUS_WINOFF_EN		(1 << 25)
-#define S5P_CISTATUS_IMGCPT_EN		(1 << 22)
-#define S5P_CISTATUS_IMGCPT_SCEN	(1 << 21)
-#define S5P_CISTATUS_VSYNC_A		(1 << 20)
-#define S5P_CISTATUS_VSYNC_B		(1 << 19)
-#define S5P_CISTATUS_OVRLB		(1 << 18)
-#define S5P_CISTATUS_FRAME_END		(1 << 17)
-#define S5P_CISTATUS_LASTCAPT_END	(1 << 16)
-#define S5P_CISTATUS_VVALID_A		(1 << 15)
-#define S5P_CISTATUS_VVALID_B		(1 << 14)
-
-/* Indexes to the last and the currently processed buffer. */
-#define S5P_CISTATUS2			0x68
-
-/* Image capture control */
-#define S5P_CIIMGCPT			0xc0
-#define S5P_CIIMGCPT_IMGCPTEN		(1 << 31)
-#define S5P_CIIMGCPT_IMGCPTEN_SC	(1 << 30)
-#define S5P_CIIMGCPT_CPT_FREN_ENABLE	(1 << 25)
-#define S5P_CIIMGCPT_CPT_FRMOD_CNT	(1 << 18)
-
-/* Frame capture sequence */
-#define S5P_CICPTSEQ			0xc4
-
-/* Image effect */
-#define S5P_CIIMGEFF			0xd0
-#define S5P_CIIMGEFF_IE_ENABLE		(1 << 30)
-#define S5P_CIIMGEFF_IE_SC_BEFORE	(0 << 29)
-#define S5P_CIIMGEFF_IE_SC_AFTER	(1 << 29)
-#define S5P_CIIMGEFF_FIN_BYPASS		(0 << 26)
-#define S5P_CIIMGEFF_FIN_ARBITRARY	(1 << 26)
-#define S5P_CIIMGEFF_FIN_NEGATIVE	(2 << 26)
-#define S5P_CIIMGEFF_FIN_ARTFREEZE	(3 << 26)
-#define S5P_CIIMGEFF_FIN_EMBOSSING	(4 << 26)
-#define S5P_CIIMGEFF_FIN_SILHOUETTE	(5 << 26)
-#define S5P_CIIMGEFF_FIN_MASK		(7 << 26)
-#define S5P_CIIMGEFF_PAT_CBCR_MASK	((0xff < 13) | (0xff < 0))
-#define S5P_CIIMGEFF_PAT_CB(x)		((x) << 13)
-#define S5P_CIIMGEFF_PAT_CR(x)		((x) << 0)
-
-/* Input DMA Y/Cb/Cr plane start address 0/1 */
-#define S5P_CIIYSA(n)			(0xd4 + (n) * 0x70)
-#define S5P_CIICBSA(n)			(0xd8 + (n) * 0x70)
-#define S5P_CIICRSA(n)			(0xdc + (n) * 0x70)
-
-/* Real input DMA image size */
-#define S5P_CIREAL_ISIZE		0xf8
-#define S5P_CIREAL_ISIZE_AUTOLOAD_EN	(1 << 31)
-#define S5P_CIREAL_ISIZE_ADDR_CH_DIS	(1 << 30)
-#define S5P_CIREAL_ISIZE_HEIGHT(x)	((x) << 16)
-#define S5P_CIREAL_ISIZE_WIDTH(x)	((x) << 0)
-
-
-/* Input DMA control */
-#define S5P_MSCTRL			0xfc
-#define S5P_MSCTRL_IN_BURST_COUNT_MASK	(0xF << 24)
-#define S5P_MSCTRL_2P_IN_ORDER_MASK	(3 << 16)
-#define S5P_MSCTRL_2P_IN_ORDER_SHIFT	16
-#define S5P_MSCTRL_C_INT_IN_3PLANE	(0 << 15)
-#define S5P_MSCTRL_C_INT_IN_2PLANE	(1 << 15)
-#define S5P_MSCTRL_C_INT_IN_MASK	(1 << 15)
-#define S5P_MSCTRL_FLIP_SHIFT		13
-#define S5P_MSCTRL_FLIP_MASK		(3 << 13)
-#define S5P_MSCTRL_FLIP_NORMAL		(0 << 13)
-#define S5P_MSCTRL_FLIP_X_MIRROR	(1 << 13)
-#define S5P_MSCTRL_FLIP_Y_MIRROR	(2 << 13)
-#define S5P_MSCTRL_FLIP_180		(3 << 13)
-#define S5P_MSCTRL_FIFO_CTRL_FULL	(1 << 12)
-#define S5P_MSCTRL_ORDER422_SHIFT	4
-#define S5P_MSCTRL_ORDER422_YCBYCR	(0 << 4)
-#define S5P_MSCTRL_ORDER422_CBYCRY	(1 << 4)
-#define S5P_MSCTRL_ORDER422_YCRYCB	(2 << 4)
-#define S5P_MSCTRL_ORDER422_CRYCBY	(3 << 4)
-#define S5P_MSCTRL_ORDER422_MASK	(3 << 4)
-#define S5P_MSCTRL_INPUT_EXTCAM		(0 << 3)
-#define S5P_MSCTRL_INPUT_MEMORY		(1 << 3)
-#define S5P_MSCTRL_INPUT_MASK		(1 << 3)
-#define S5P_MSCTRL_INFORMAT_YCBCR420	(0 << 1)
-#define S5P_MSCTRL_INFORMAT_YCBCR422	(1 << 1)
-#define S5P_MSCTRL_INFORMAT_YCBCR422_1P	(2 << 1)
-#define S5P_MSCTRL_INFORMAT_RGB		(3 << 1)
-#define S5P_MSCTRL_INFORMAT_MASK	(3 << 1)
-#define S5P_MSCTRL_ENVID		(1 << 0)
-#define S5P_MSCTRL_IN_BURST_COUNT(x)	((x) << 24)
-
-/* Output DMA Y/Cb/Cr offset */
-#define S5P_CIOYOFF			0x168
-#define S5P_CIOCBOFF			0x16c
-#define S5P_CIOCROFF			0x170
-
-/* Input DMA Y/Cb/Cr offset */
-#define S5P_CIIYOFF			0x174
-#define S5P_CIICBOFF			0x178
-#define S5P_CIICROFF			0x17c
-
-#define S5P_CIO_OFFS_VER(x)		((x) << 16)
-#define S5P_CIO_OFFS_HOR(x)		((x) << 0)
-
-/* Input DMA original image size */
-#define S5P_ORGISIZE			0x180
-
-/* Output DMA original image size */
-#define S5P_ORGOSIZE			0x184
-
-#define S5P_ORIG_SIZE_VER(x)		((x) << 16)
-#define S5P_ORIG_SIZE_HOR(x)		((x) << 0)
-
-/* Real output DMA image size (extension register) */
-#define S5P_CIEXTEN			0x188
-#define S5P_CIEXTEN_MHRATIO_EXT(x)	(((x) & 0x3f) << 10)
-#define S5P_CIEXTEN_MVRATIO_EXT(x)	((x) & 0x3f)
-#define S5P_CIEXTEN_MHRATIO_EXT_MASK	(0x3f << 10)
-#define S5P_CIEXTEN_MVRATIO_EXT_MASK	0x3f
-
-#define S5P_CIDMAPARAM			0x18c
-#define S5P_CIDMAPARAM_R_LINEAR		(0 << 29)
-#define S5P_CIDMAPARAM_R_64X32		(3 << 29)
-#define S5P_CIDMAPARAM_W_LINEAR		(0 << 13)
-#define S5P_CIDMAPARAM_W_64X32		(3 << 13)
-#define S5P_CIDMAPARAM_TILE_MASK	((3 << 29) | (3 << 13))
-
-/* MIPI CSI image format */
-#define S5P_CSIIMGFMT			0x194
-#define S5P_CSIIMGFMT_YCBCR422_8BIT	0x1e
-#define S5P_CSIIMGFMT_RAW8		0x2a
-#define S5P_CSIIMGFMT_RAW10		0x2b
-#define S5P_CSIIMGFMT_RAW12		0x2c
-/* User defined formats. x = 0...16. */
-#define S5P_CSIIMGFMT_USER(x)		(0x30 + x - 1)
-
-/* Output frame buffer sequence mask */
-#define S5P_CIFCNTSEQ			0x1FC
-
-#endif /* REGS_FIMC_H_ */
diff --git a/drivers/media/video/s5p-g2d/g2d.c b/drivers/media/video/s5p-g2d/g2d.c
index 789de74014e..7c98ee7377e 100644
--- a/drivers/media/video/s5p-g2d/g2d.c
+++ b/drivers/media/video/s5p-g2d/g2d.c
@@ -65,7 +65,7 @@ static struct g2d_fmt formats[] = {
 };
 #define NUM_FORMATS ARRAY_SIZE(formats)
 
-struct g2d_frame def_frame = {
+static struct g2d_frame def_frame = {
 	.width		= DEFAULT_WIDTH,
 	.height		= DEFAULT_HEIGHT,
 	.c_width	= DEFAULT_WIDTH,
@@ -77,7 +77,7 @@ struct g2d_frame def_frame = {
 	.bottom		= DEFAULT_HEIGHT,
 };
 
-struct g2d_fmt *find_fmt(struct v4l2_format *f)
+static struct g2d_fmt *find_fmt(struct v4l2_format *f)
 {
 	unsigned int i;
 	for (i = 0; i < NUM_FORMATS; i++) {
@@ -202,7 +202,7 @@ static const struct v4l2_ctrl_ops g2d_ctrl_ops = {
 	.s_ctrl		= g2d_s_ctrl,
 };
 
-int g2d_setup_ctrls(struct g2d_ctx *ctx)
+static int g2d_setup_ctrls(struct g2d_ctx *ctx)
 {
 	struct g2d_dev *dev = ctx->dev;
 
@@ -546,11 +546,11 @@ static void job_abort(void *prv)
 	struct g2d_dev *dev = ctx->dev;
 	int ret;
 
-	if (dev->curr == 0) /* No job currently running */
+	if (dev->curr == NULL) /* No job currently running */
 		return;
 
 	ret = wait_event_timeout(dev->irq_queue,
-		dev->curr == 0,
+		dev->curr == NULL,
 		msecs_to_jiffies(G2D_TIMEOUT));
 }
 
@@ -599,19 +599,19 @@ static irqreturn_t g2d_isr(int irq, void *prv)
 	g2d_clear_int(dev);
 	clk_disable(dev->gate);
 
-	BUG_ON(ctx == 0);
+	BUG_ON(ctx == NULL);
 
 	src = v4l2_m2m_src_buf_remove(ctx->m2m_ctx);
 	dst = v4l2_m2m_dst_buf_remove(ctx->m2m_ctx);
 
-	BUG_ON(src == 0);
-	BUG_ON(dst == 0);
+	BUG_ON(src == NULL);
+	BUG_ON(dst == NULL);
 
 	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);
 
-	dev->curr = 0;
+	dev->curr = NULL;
 	wake_up(&dev->irq_queue);
 	return IRQ_HANDLED;
 }
@@ -674,42 +674,27 @@ static int g2d_probe(struct platform_device *pdev)
 	struct resource *res;
 	int ret = 0;
 
-	dev = kzalloc(sizeof(*dev), GFP_KERNEL);
+	dev = devm_kzalloc(&pdev->dev, sizeof(*dev), GFP_KERNEL);
 	if (!dev)
 		return -ENOMEM;
+
 	spin_lock_init(&dev->ctrl_lock);
 	mutex_init(&dev->mutex);
 	atomic_set(&dev->num_inst, 0);
 	init_waitqueue_head(&dev->irq_queue);
 
 	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
-	if (!res) {
-		dev_err(&pdev->dev, "failed to find registers\n");
-		ret = -ENOENT;
-		goto free_dev;
-	}
 
-	dev->res_regs = request_mem_region(res->start, resource_size(res),
-						dev_name(&pdev->dev));
-
-	if (!dev->res_regs) {
-		dev_err(&pdev->dev, "failed to obtain register region\n");
-		ret = -ENOENT;
-		goto free_dev;
-	}
-
-	dev->regs = ioremap(res->start, resource_size(res));
-	if (!dev->regs) {
-		dev_err(&pdev->dev, "failed to map registers\n");
-		ret = -ENOENT;
-		goto rel_res_regs;
+	dev->regs = devm_request_and_ioremap(&pdev->dev, res);
+	if (dev->regs == NULL) {
+			dev_err(&pdev->dev, "Failed to obtain io memory\n");
+			return -ENOENT;
 	}
 
 	dev->clk = clk_get(&pdev->dev, "sclk_fimg2d");
 	if (IS_ERR_OR_NULL(dev->clk)) {
 		dev_err(&pdev->dev, "failed to get g2d clock\n");
-		ret = -ENXIO;
-		goto unmap_regs;
+		return -ENXIO;
 	}
 
 	ret = clk_prepare(dev->clk);
@@ -740,7 +725,8 @@ static int g2d_probe(struct platform_device *pdev)
 
 	dev->irq = res->start;
 
-	ret = request_irq(dev->irq, g2d_isr, 0, pdev->name, dev);
+	ret = devm_request_irq(&pdev->dev, dev->irq, g2d_isr,
+						0, pdev->name, dev);
 	if (ret) {
 		dev_err(&pdev->dev, "failed to install IRQ\n");
 		goto put_clk_gate;
@@ -749,7 +735,7 @@ static int g2d_probe(struct platform_device *pdev)
 	dev->alloc_ctx = vb2_dma_contig_init_ctx(&pdev->dev);
 	if (IS_ERR(dev->alloc_ctx)) {
 		ret = PTR_ERR(dev->alloc_ctx);
-		goto rel_irq;
+		goto unprep_clk_gate;
 	}
 
 	ret = v4l2_device_register(&pdev->dev, &dev->v4l2_dev);
@@ -762,6 +748,10 @@ static int g2d_probe(struct platform_device *pdev)
 		goto unreg_v4l2_dev;
 	}
 	*vfd = g2d_videodev;
+	/* Locking in file operations other than ioctl should be done
+	   by the driver, not the V4L2 core.
+	   This driver needs auditing so that this flag can be removed. */
+	set_bit(V4L2_FL_LOCK_ALL_FOPS, &vfd->flags);
 	vfd->lock = &dev->mutex;
 	ret = video_register_device(vfd, VFL_TYPE_GRABBER, 0);
 	if (ret) {
@@ -793,8 +783,6 @@ unreg_v4l2_dev:
 	v4l2_device_unregister(&dev->v4l2_dev);
 alloc_ctx_cleanup:
 	vb2_dma_contig_cleanup_ctx(dev->alloc_ctx);
-rel_irq:
-	free_irq(dev->irq, dev);
 unprep_clk_gate:
 	clk_unprepare(dev->gate);
 put_clk_gate:
@@ -803,12 +791,7 @@ unprep_clk:
 	clk_unprepare(dev->clk);
 put_clk:
 	clk_put(dev->clk);
-unmap_regs:
-	iounmap(dev->regs);
-rel_res_regs:
-	release_resource(dev->res_regs);
-free_dev:
-	kfree(dev);
+
 	return ret;
 }
 
@@ -821,14 +804,10 @@ static int g2d_remove(struct platform_device *pdev)
 	video_unregister_device(dev->vfd);
 	v4l2_device_unregister(&dev->v4l2_dev);
 	vb2_dma_contig_cleanup_ctx(dev->alloc_ctx);
-	free_irq(dev->irq, dev);
 	clk_unprepare(dev->gate);
 	clk_put(dev->gate);
 	clk_unprepare(dev->clk);
 	clk_put(dev->clk);
-	iounmap(dev->regs);
-	release_resource(dev->res_regs);
-	kfree(dev);
 	return 0;
 }
 
diff --git a/drivers/media/video/s5p-g2d/g2d.h b/drivers/media/video/s5p-g2d/g2d.h
index 1b82065aeae..6b765b0216c 100644
--- a/drivers/media/video/s5p-g2d/g2d.h
+++ b/drivers/media/video/s5p-g2d/g2d.h
@@ -23,7 +23,6 @@ struct g2d_dev {
 	spinlock_t		ctrl_lock;
 	atomic_t		num_inst;
 	struct vb2_alloc_ctx	*alloc_ctx;
-	struct resource		*res_regs;
 	void __iomem		*regs;
 	struct clk		*clk;
 	struct clk		*gate;
diff --git a/drivers/media/video/s5p-jpeg/jpeg-core.c b/drivers/media/video/s5p-jpeg/jpeg-core.c
index 5a49c307f9c..28b5225d94f 100644
--- a/drivers/media/video/s5p-jpeg/jpeg-core.c
+++ b/drivers/media/video/s5p-jpeg/jpeg-core.c
@@ -813,7 +813,7 @@ static int s5p_jpeg_streamoff(struct file *file, void *priv,
 	return v4l2_m2m_streamoff(file, ctx->m2m_ctx, type);
 }
 
-int s5p_jpeg_g_selection(struct file *file, void *priv,
+static int s5p_jpeg_g_selection(struct file *file, void *priv,
 			 struct v4l2_selection *s)
 {
 	struct s5p_jpeg_ctx *ctx = fh_to_ctx(priv);
@@ -1290,7 +1290,7 @@ static int s5p_jpeg_probe(struct platform_device *pdev)
 	int ret;
 
 	/* JPEG IP abstraction struct */
-	jpeg = kzalloc(sizeof(struct s5p_jpeg), GFP_KERNEL);
+	jpeg = devm_kzalloc(&pdev->dev, sizeof(struct s5p_jpeg), GFP_KERNEL);
 	if (!jpeg)
 		return -ENOMEM;
 
@@ -1300,43 +1300,25 @@ static int s5p_jpeg_probe(struct platform_device *pdev)
 
 	/* memory-mapped registers */
 	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
-	if (!res) {
-		dev_err(&pdev->dev, "cannot find IO resource\n");
-		ret = -ENOENT;
-		goto jpeg_alloc_rollback;
-	}
-
-	jpeg->ioarea = request_mem_region(res->start, resource_size(res),
-					  pdev->name);
-	if (!jpeg->ioarea) {
-		dev_err(&pdev->dev, "cannot request IO\n");
-		ret = -ENXIO;
-		goto jpeg_alloc_rollback;
-	}
 
-	jpeg->regs = ioremap(res->start, resource_size(res));
-	if (!jpeg->regs) {
-		dev_err(&pdev->dev, "cannot map IO\n");
-		ret = -ENXIO;
-		goto mem_region_rollback;
+	jpeg->regs = devm_request_and_ioremap(&pdev->dev, res);
+	if (jpeg->regs == NULL) {
+		dev_err(&pdev->dev, "Failed to obtain io memory\n");
+		return -ENOENT;
 	}
 
-	dev_dbg(&pdev->dev, "registers %p (%p, %p)\n",
-		jpeg->regs, jpeg->ioarea, res);
-
 	/* interrupt service routine registration */
 	jpeg->irq = ret = platform_get_irq(pdev, 0);
 	if (ret < 0) {
 		dev_err(&pdev->dev, "cannot find IRQ\n");
-		goto ioremap_rollback;
+		return ret;
 	}
 
-	ret = request_irq(jpeg->irq, s5p_jpeg_irq, 0,
-			  dev_name(&pdev->dev), jpeg);
-
+	ret = devm_request_irq(&pdev->dev, jpeg->irq, s5p_jpeg_irq, 0,
+			dev_name(&pdev->dev), jpeg);
 	if (ret) {
 		dev_err(&pdev->dev, "cannot claim IRQ %d\n", jpeg->irq);
-		goto ioremap_rollback;
+		return ret;
 	}
 
 	/* clocks */
@@ -1344,7 +1326,7 @@ static int s5p_jpeg_probe(struct platform_device *pdev)
 	if (IS_ERR(jpeg->clk)) {
 		dev_err(&pdev->dev, "cannot get clock\n");
 		ret = PTR_ERR(jpeg->clk);
-		goto request_irq_rollback;
+		return ret;
 	}
 	dev_dbg(&pdev->dev, "clock source %p\n", jpeg->clk);
 	clk_enable(jpeg->clk);
@@ -1386,6 +1368,10 @@ static int s5p_jpeg_probe(struct platform_device *pdev)
 	jpeg->vfd_encoder->release	= video_device_release;
 	jpeg->vfd_encoder->lock		= &jpeg->lock;
 	jpeg->vfd_encoder->v4l2_dev	= &jpeg->v4l2_dev;
+	/* Locking in file operations other than ioctl should be done
+	   by the driver, not the V4L2 core.
+	   This driver needs auditing so that this flag can be removed. */
+	set_bit(V4L2_FL_LOCK_ALL_FOPS, &jpeg->vfd_encoder->flags);
 
 	ret = video_register_device(jpeg->vfd_encoder, VFL_TYPE_GRABBER, -1);
 	if (ret) {
@@ -1413,6 +1399,10 @@ static int s5p_jpeg_probe(struct platform_device *pdev)
 	jpeg->vfd_decoder->release	= video_device_release;
 	jpeg->vfd_decoder->lock		= &jpeg->lock;
 	jpeg->vfd_decoder->v4l2_dev	= &jpeg->v4l2_dev;
+	/* Locking in file operations other than ioctl should be done by the driver,
+	   not the V4L2 core.
+	   This driver needs auditing so that this flag can be removed. */
+	set_bit(V4L2_FL_LOCK_ALL_FOPS, &jpeg->vfd_decoder->flags);
 
 	ret = video_register_device(jpeg->vfd_decoder, VFL_TYPE_GRABBER, -1);
 	if (ret) {
@@ -1456,18 +1446,6 @@ clk_get_rollback:
 	clk_disable(jpeg->clk);
 	clk_put(jpeg->clk);
 
-request_irq_rollback:
-	free_irq(jpeg->irq, jpeg);
-
-ioremap_rollback:
-	iounmap(jpeg->regs);
-
-mem_region_rollback:
-	release_resource(jpeg->ioarea);
-	release_mem_region(jpeg->ioarea->start, resource_size(jpeg->ioarea));
-
-jpeg_alloc_rollback:
-	kfree(jpeg);
 	return ret;
 }
 
@@ -1488,14 +1466,6 @@ static int s5p_jpeg_remove(struct platform_device *pdev)
 	clk_disable(jpeg->clk);
 	clk_put(jpeg->clk);
 
-	free_irq(jpeg->irq, jpeg);
-
-	iounmap(jpeg->regs);
-
-	release_resource(jpeg->ioarea);
-	release_mem_region(jpeg->ioarea->start, resource_size(jpeg->ioarea));
-	kfree(jpeg);
-
 	return 0;
 }
 
diff --git a/drivers/media/video/s5p-jpeg/jpeg-core.h b/drivers/media/video/s5p-jpeg/jpeg-core.h
index 38d7367f7a6..9d0cd2b76f6 100644
--- a/drivers/media/video/s5p-jpeg/jpeg-core.h
+++ b/drivers/media/video/s5p-jpeg/jpeg-core.h
@@ -54,7 +54,6 @@
  * @vfd_encoder:	video device node for encoder mem2mem mode
  * @vfd_decoder:	video device node for decoder mem2mem mode
  * @m2m_dev:		v4l2 mem2mem device data
- * @ioarea:		JPEG IP memory region
  * @regs:		JPEG IP registers mapping
  * @irq:		JPEG IP irq
  * @clk:		JPEG IP clock
@@ -70,7 +69,6 @@ struct s5p_jpeg {
 	struct video_device	*vfd_decoder;
 	struct v4l2_m2m_dev	*m2m_dev;
 
-	struct resource		*ioarea;
 	void __iomem		*regs;
 	unsigned int		irq;
 	struct clk		*clk;
diff --git a/drivers/media/video/s5p-mfc/s5p_mfc.c b/drivers/media/video/s5p-mfc/s5p_mfc.c
index 83fe461af26..9bb68e7b5ae 100644
--- a/drivers/media/video/s5p-mfc/s5p_mfc.c
+++ b/drivers/media/video/s5p-mfc/s5p_mfc.c
@@ -70,7 +70,7 @@ static void wake_up_dev(struct s5p_mfc_dev *dev, unsigned int reason,
 	wake_up(&dev->queue);
 }
 
-void s5p_mfc_watchdog(unsigned long arg)
+static void s5p_mfc_watchdog(unsigned long arg)
 {
 	struct s5p_mfc_dev *dev = (struct s5p_mfc_dev *)arg;
 
@@ -373,7 +373,7 @@ static void s5p_mfc_handle_error(struct s5p_mfc_ctx *ctx,
 
 	/* If no context is available then all necessary
 	 * processing has been done. */
-	if (ctx == 0)
+	if (ctx == NULL)
 		return;
 
 	dev = ctx->dev;
@@ -429,7 +429,7 @@ static void s5p_mfc_handle_seq_done(struct s5p_mfc_ctx *ctx,
 	struct s5p_mfc_dev *dev;
 	unsigned int guard_width, guard_height;
 
-	if (ctx == 0)
+	if (ctx == NULL)
 		return;
 	dev = ctx->dev;
 	if (ctx->c_ops->post_seq_start) {
@@ -496,7 +496,7 @@ static void s5p_mfc_handle_init_buffers(struct s5p_mfc_ctx *ctx,
 	struct s5p_mfc_dev *dev;
 	unsigned long flags;
 
-	if (ctx == 0)
+	if (ctx == NULL)
 		return;
 	dev = ctx->dev;
 	s5p_mfc_clear_int_flags(dev);
@@ -772,7 +772,7 @@ err_queue_init:
 err_init_hw:
 	s5p_mfc_release_firmware(dev);
 err_alloc_fw:
-	dev->ctx[ctx->num] = 0;
+	dev->ctx[ctx->num] = NULL;
 	del_timer_sync(&dev->watchdog_timer);
 	s5p_mfc_clock_off();
 err_pwr_enable:
@@ -849,7 +849,7 @@ static int s5p_mfc_release(struct file *file)
 	}
 	mfc_debug(2, "Shutting down clock\n");
 	s5p_mfc_clock_off();
-	dev->ctx[ctx->num] = 0;
+	dev->ctx[ctx->num] = NULL;
 	s5p_mfc_dec_ctrls_delete(ctx);
 	v4l2_fh_del(&ctx->fh);
 	v4l2_fh_exit(&ctx->fh);
@@ -948,7 +948,7 @@ static int s5p_mfc_probe(struct platform_device *pdev)
 	int ret;
 
 	pr_debug("%s++\n", __func__);
-	dev = kzalloc(sizeof *dev, GFP_KERNEL);
+	dev = devm_kzalloc(&pdev->dev, sizeof *dev, GFP_KERNEL);
 	if (!dev) {
 		dev_err(&pdev->dev, "Not enough memory for MFC device\n");
 		return -ENOMEM;
@@ -959,49 +959,35 @@ static int s5p_mfc_probe(struct platform_device *pdev)
 	dev->plat_dev = pdev;
 	if (!dev->plat_dev) {
 		dev_err(&pdev->dev, "No platform data specified\n");
-		ret = -ENODEV;
-		goto err_dev;
+		return -ENODEV;
 	}
 
 	ret = s5p_mfc_init_pm(dev);
 	if (ret < 0) {
 		dev_err(&pdev->dev, "failed to get mfc clock source\n");
-		goto err_clk;
+		return ret;
 	}
 
 	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
-	if (res == NULL) {
-		dev_err(&pdev->dev, "failed to get memory region resource\n");
-		ret = -ENOENT;
-		goto err_res;
-	}
 
-	dev->mfc_mem = request_mem_region(res->start, resource_size(res),
-					  pdev->name);
-	if (dev->mfc_mem == NULL) {
-		dev_err(&pdev->dev, "failed to get memory region\n");
-		ret = -ENOENT;
-		goto err_mem_reg;
-	}
-	dev->regs_base = ioremap(dev->mfc_mem->start, resource_size(dev->mfc_mem));
+	dev->regs_base = devm_request_and_ioremap(&pdev->dev, res);
 	if (dev->regs_base == NULL) {
-		dev_err(&pdev->dev, "failed to ioremap address region\n");
-		ret = -ENOENT;
-		goto err_ioremap;
+		dev_err(&pdev->dev, "Failed to obtain io memory\n");
+		return -ENOENT;
 	}
 
 	res = platform_get_resource(pdev, IORESOURCE_IRQ, 0);
 	if (res == NULL) {
 		dev_err(&pdev->dev, "failed to get irq resource\n");
 		ret = -ENOENT;
-		goto err_get_res;
+		goto err_res;
 	}
 	dev->irq = res->start;
-	ret = request_irq(dev->irq, s5p_mfc_irq, IRQF_DISABLED, pdev->name,
-									dev);
+	ret = devm_request_irq(&pdev->dev, dev->irq, s5p_mfc_irq,
+					IRQF_DISABLED, pdev->name, dev);
 	if (ret) {
 		dev_err(&pdev->dev, "Failed to install irq (%d)\n", ret);
-		goto err_req_irq;
+		goto err_res;
 	}
 
 	dev->mem_dev_l = device_find_child(&dev->plat_dev->dev, "s5p-mfc-l",
@@ -1009,20 +995,20 @@ static int s5p_mfc_probe(struct platform_device *pdev)
 	if (!dev->mem_dev_l) {
 		mfc_err("Mem child (L) device get failed\n");
 		ret = -ENODEV;
-		goto err_find_child;
+		goto err_res;
 	}
 	dev->mem_dev_r = device_find_child(&dev->plat_dev->dev, "s5p-mfc-r",
 					   match_child);
 	if (!dev->mem_dev_r) {
 		mfc_err("Mem child (R) device get failed\n");
 		ret = -ENODEV;
-		goto err_find_child;
+		goto err_res;
 	}
 
 	dev->alloc_ctx[0] = vb2_dma_contig_init_ctx(dev->mem_dev_l);
 	if (IS_ERR_OR_NULL(dev->alloc_ctx[0])) {
 		ret = PTR_ERR(dev->alloc_ctx[0]);
-		goto err_mem_init_ctx_0;
+		goto err_res;
 	}
 	dev->alloc_ctx[1] = vb2_dma_contig_init_ctx(dev->mem_dev_r);
 	if (IS_ERR_OR_NULL(dev->alloc_ctx[1])) {
@@ -1048,6 +1034,10 @@ static int s5p_mfc_probe(struct platform_device *pdev)
 	vfd->ioctl_ops	= get_dec_v4l2_ioctl_ops();
 	vfd->release	= video_device_release,
 	vfd->lock	= &dev->mfc_mutex;
+	/* Locking in file operations other than ioctl should be done
+	   by the driver, not the V4L2 core.
+	   This driver needs auditing so that this flag can be removed. */
+	set_bit(V4L2_FL_LOCK_ALL_FOPS, &vfd->flags);
 	vfd->v4l2_dev	= &dev->v4l2_dev;
 	snprintf(vfd->name, sizeof(vfd->name), "%s", S5P_MFC_DEC_NAME);
 	dev->vfd_dec	= vfd;
@@ -1072,6 +1062,8 @@ static int s5p_mfc_probe(struct platform_device *pdev)
 	vfd->ioctl_ops	= get_enc_v4l2_ioctl_ops();
 	vfd->release	= video_device_release,
 	vfd->lock	= &dev->mfc_mutex;
+	/* This should not be necessary */
+	set_bit(V4L2_FL_LOCK_ALL_FOPS, &vfd->flags);
 	vfd->v4l2_dev	= &dev->v4l2_dev;
 	snprintf(vfd->name, sizeof(vfd->name), "%s", S5P_MFC_ENC_NAME);
 	dev->vfd_enc	= vfd;
@@ -1110,22 +1102,9 @@ err_v4l2_dev_reg:
 	vb2_dma_contig_cleanup_ctx(dev->alloc_ctx[1]);
 err_mem_init_ctx_1:
 	vb2_dma_contig_cleanup_ctx(dev->alloc_ctx[0]);
-err_mem_init_ctx_0:
-err_find_child:
-	free_irq(dev->irq, dev);
-err_req_irq:
-err_get_res:
-	iounmap(dev->regs_base);
-	dev->regs_base = NULL;
-err_ioremap:
-	release_resource(dev->mfc_mem);
-	kfree(dev->mfc_mem);
-err_mem_reg:
 err_res:
 	s5p_mfc_final_pm(dev);
-err_clk:
-err_dev:
-	kfree(dev);
+
 	pr_debug("%s-- with error\n", __func__);
 	return ret;
 
@@ -1148,15 +1127,7 @@ static int __devexit s5p_mfc_remove(struct platform_device *pdev)
 	vb2_dma_contig_cleanup_ctx(dev->alloc_ctx[0]);
 	vb2_dma_contig_cleanup_ctx(dev->alloc_ctx[1]);
 
-	free_irq(dev->irq, dev);
-	iounmap(dev->regs_base);
-	if (dev->mfc_mem) {
-		release_resource(dev->mfc_mem);
-		kfree(dev->mfc_mem);
-		dev->mfc_mem = NULL;
-	}
 	s5p_mfc_final_pm(dev);
-	kfree(dev);
 	return 0;
 }
 
diff --git a/drivers/media/video/s5p-mfc/s5p_mfc_common.h b/drivers/media/video/s5p-mfc/s5p_mfc_common.h
index 91146fa622e..bd5706a6bad 100644
--- a/drivers/media/video/s5p-mfc/s5p_mfc_common.h
+++ b/drivers/media/video/s5p-mfc/s5p_mfc_common.h
@@ -185,7 +185,6 @@ struct s5p_mfc_pm {
  * @mem_dev_r:		child device of the right memory bank (1)
  * @regs_base:		base address of the MFC hw registers
  * @irq:		irq resource
- * @mfc_mem:		MFC registers memory resource
  * @dec_ctrl_handler:	control framework handler for decoding
  * @enc_ctrl_handler:	control framework handler for encoding
  * @pm:			power management control
@@ -221,7 +220,6 @@ struct s5p_mfc_dev {
 	struct device		*mem_dev_r;
 	void __iomem		*regs_base;
 	int			irq;
-	struct resource		*mfc_mem;
 	struct v4l2_ctrl_handler dec_ctrl_handler;
 	struct v4l2_ctrl_handler enc_ctrl_handler;
 	struct s5p_mfc_pm	pm;
diff --git a/drivers/media/video/s5p-mfc/s5p_mfc_ctrl.c b/drivers/media/video/s5p-mfc/s5p_mfc_ctrl.c
index f2481a85e0a..08a5cfeaa59 100644
--- a/drivers/media/video/s5p-mfc/s5p_mfc_ctrl.c
+++ b/drivers/media/video/s5p-mfc/s5p_mfc_ctrl.c
@@ -52,7 +52,7 @@ int s5p_mfc_alloc_and_load_firmware(struct s5p_mfc_dev *dev)
 	s5p_mfc_bitproc_buf = vb2_dma_contig_memops.alloc(
 		dev->alloc_ctx[MFC_BANK1_ALLOC_CTX], dev->fw_size);
 	if (IS_ERR(s5p_mfc_bitproc_buf)) {
-		s5p_mfc_bitproc_buf = 0;
+		s5p_mfc_bitproc_buf = NULL;
 		mfc_err("Allocating bitprocessor buffer failed\n");
 		release_firmware(fw_blob);
 		return -ENOMEM;
@@ -63,7 +63,7 @@ int s5p_mfc_alloc_and_load_firmware(struct s5p_mfc_dev *dev)
 		mfc_err("The base memory for bank 1 is not aligned to 128KB\n");
 		vb2_dma_contig_memops.put(s5p_mfc_bitproc_buf);
 		s5p_mfc_bitproc_phys = 0;
-		s5p_mfc_bitproc_buf = 0;
+		s5p_mfc_bitproc_buf = NULL;
 		release_firmware(fw_blob);
 		return -EIO;
 	}
@@ -72,7 +72,7 @@ int s5p_mfc_alloc_and_load_firmware(struct s5p_mfc_dev *dev)
 		mfc_err("Bitprocessor memory remap failed\n");
 		vb2_dma_contig_memops.put(s5p_mfc_bitproc_buf);
 		s5p_mfc_bitproc_phys = 0;
-		s5p_mfc_bitproc_buf = 0;
+		s5p_mfc_bitproc_buf = NULL;
 		release_firmware(fw_blob);
 		return -EIO;
 	}
@@ -82,7 +82,7 @@ int s5p_mfc_alloc_and_load_firmware(struct s5p_mfc_dev *dev)
 	if (IS_ERR(b_base)) {
 		vb2_dma_contig_memops.put(s5p_mfc_bitproc_buf);
 		s5p_mfc_bitproc_phys = 0;
-		s5p_mfc_bitproc_buf = 0;
+		s5p_mfc_bitproc_buf = NULL;
 		mfc_err("Allocating bank2 base failed\n");
 	release_firmware(fw_blob);
 		return -ENOMEM;
@@ -94,7 +94,7 @@ int s5p_mfc_alloc_and_load_firmware(struct s5p_mfc_dev *dev)
 		mfc_err("The base memory for bank 2 is not aligned to 128KB\n");
 		vb2_dma_contig_memops.put(s5p_mfc_bitproc_buf);
 		s5p_mfc_bitproc_phys = 0;
-		s5p_mfc_bitproc_buf = 0;
+		s5p_mfc_bitproc_buf = NULL;
 		release_firmware(fw_blob);
 		return -EIO;
 	}
@@ -126,7 +126,7 @@ int s5p_mfc_reload_firmware(struct s5p_mfc_dev *dev)
 		release_firmware(fw_blob);
 		return -ENOMEM;
 	}
-	if (s5p_mfc_bitproc_buf == 0 || s5p_mfc_bitproc_phys == 0) {
+	if (s5p_mfc_bitproc_buf == NULL || s5p_mfc_bitproc_phys == 0) {
 		mfc_err("MFC firmware is not allocated or was not mapped correctly\n");
 		release_firmware(fw_blob);
 		return -EINVAL;
@@ -146,9 +146,9 @@ int s5p_mfc_release_firmware(struct s5p_mfc_dev *dev)
 	if (!s5p_mfc_bitproc_buf)
 		return -EINVAL;
 	vb2_dma_contig_memops.put(s5p_mfc_bitproc_buf);
-	s5p_mfc_bitproc_virt =  0;
+	s5p_mfc_bitproc_virt = NULL;
 	s5p_mfc_bitproc_phys = 0;
-	s5p_mfc_bitproc_buf = 0;
+	s5p_mfc_bitproc_buf = NULL;
 	return 0;
 }
 
diff --git a/drivers/media/video/s5p-mfc/s5p_mfc_enc.c b/drivers/media/video/s5p-mfc/s5p_mfc_enc.c
index dff9dc79879..acedb2004be 100644
--- a/drivers/media/video/s5p-mfc/s5p_mfc_enc.c
+++ b/drivers/media/video/s5p-mfc/s5p_mfc_enc.c
@@ -1436,7 +1436,8 @@ static const struct v4l2_ctrl_ops s5p_mfc_enc_ctrl_ops = {
 	.s_ctrl = s5p_mfc_enc_s_ctrl,
 };
 
-int vidioc_s_parm(struct file *file, void *priv, struct v4l2_streamparm *a)
+static int vidioc_s_parm(struct file *file, void *priv,
+			 struct v4l2_streamparm *a)
 {
 	struct s5p_mfc_ctx *ctx = fh_to_ctx(priv);
 
@@ -1452,7 +1453,8 @@ int vidioc_s_parm(struct file *file, void *priv, struct v4l2_streamparm *a)
 	return 0;
 }
 
-int vidioc_g_parm(struct file *file, void *priv, struct v4l2_streamparm *a)
+static int vidioc_g_parm(struct file *file, void *priv,
+			 struct v4l2_streamparm *a)
 {
 	struct s5p_mfc_ctx *ctx = fh_to_ctx(priv);
 
diff --git a/drivers/media/video/s5p-mfc/s5p_mfc_opr.c b/drivers/media/video/s5p-mfc/s5p_mfc_opr.c
index e08b21c50eb..e6217cbfa4a 100644
--- a/drivers/media/video/s5p-mfc/s5p_mfc_opr.c
+++ b/drivers/media/video/s5p-mfc/s5p_mfc_opr.c
@@ -43,7 +43,7 @@ int s5p_mfc_alloc_dec_temp_buffers(struct s5p_mfc_ctx *ctx)
 	ctx->desc_buf = vb2_dma_contig_memops.alloc(
 			dev->alloc_ctx[MFC_BANK1_ALLOC_CTX], DESC_BUF_SIZE);
 	if (IS_ERR_VALUE((int)ctx->desc_buf)) {
-		ctx->desc_buf = 0;
+		ctx->desc_buf = NULL;
 		mfc_err("Allocating DESC buffer failed\n");
 		return -ENOMEM;
 	}
@@ -54,7 +54,7 @@ int s5p_mfc_alloc_dec_temp_buffers(struct s5p_mfc_ctx *ctx)
 	if (desc_virt == NULL) {
 		vb2_dma_contig_memops.put(ctx->desc_buf);
 		ctx->desc_phys = 0;
-		ctx->desc_buf = 0;
+		ctx->desc_buf = NULL;
 		mfc_err("Remapping DESC buffer failed\n");
 		return -ENOMEM;
 	}
@@ -69,7 +69,7 @@ void s5p_mfc_release_dec_desc_buffer(struct s5p_mfc_ctx *ctx)
 	if (ctx->desc_phys) {
 		vb2_dma_contig_memops.put(ctx->desc_buf);
 		ctx->desc_phys = 0;
-		ctx->desc_buf = 0;
+		ctx->desc_buf = NULL;
 	}
 }
 
@@ -186,7 +186,7 @@ int s5p_mfc_alloc_codec_buffers(struct s5p_mfc_ctx *ctx)
 		ctx->bank1_buf = vb2_dma_contig_memops.alloc(
 		dev->alloc_ctx[MFC_BANK1_ALLOC_CTX], ctx->bank1_size);
 		if (IS_ERR(ctx->bank1_buf)) {
-			ctx->bank1_buf = 0;
+			ctx->bank1_buf = NULL;
 			printk(KERN_ERR
 			       "Buf alloc for decoding failed (port A)\n");
 			return -ENOMEM;
@@ -200,7 +200,7 @@ int s5p_mfc_alloc_codec_buffers(struct s5p_mfc_ctx *ctx)
 		ctx->bank2_buf = vb2_dma_contig_memops.alloc(
 		dev->alloc_ctx[MFC_BANK2_ALLOC_CTX], ctx->bank2_size);
 		if (IS_ERR(ctx->bank2_buf)) {
-			ctx->bank2_buf = 0;
+			ctx->bank2_buf = NULL;
 			mfc_err("Buf alloc for decoding failed (port B)\n");
 			return -ENOMEM;
 		}
@@ -216,13 +216,13 @@ void s5p_mfc_release_codec_buffers(struct s5p_mfc_ctx *ctx)
 {
 	if (ctx->bank1_buf) {
 		vb2_dma_contig_memops.put(ctx->bank1_buf);
-		ctx->bank1_buf = 0;
+		ctx->bank1_buf = NULL;
 		ctx->bank1_phys = 0;
 		ctx->bank1_size = 0;
 	}
 	if (ctx->bank2_buf) {
 		vb2_dma_contig_memops.put(ctx->bank2_buf);
-		ctx->bank2_buf = 0;
+		ctx->bank2_buf = NULL;
 		ctx->bank2_phys = 0;
 		ctx->bank2_size = 0;
 	}
@@ -244,7 +244,7 @@ int s5p_mfc_alloc_instance_buffer(struct s5p_mfc_ctx *ctx)
 	if (IS_ERR(ctx->ctx_buf)) {
 		mfc_err("Allocating context buffer failed\n");
 		ctx->ctx_phys = 0;
-		ctx->ctx_buf = 0;
+		ctx->ctx_buf = NULL;
 		return -ENOMEM;
 	}
 	ctx->ctx_phys = s5p_mfc_mem_cookie(
@@ -256,7 +256,7 @@ int s5p_mfc_alloc_instance_buffer(struct s5p_mfc_ctx *ctx)
 		mfc_err("Remapping instance buffer failed\n");
 		vb2_dma_contig_memops.put(ctx->ctx_buf);
 		ctx->ctx_phys = 0;
-		ctx->ctx_buf = 0;
+		ctx->ctx_buf = NULL;
 		return -ENOMEM;
 	}
 	/* Zero content of the allocated memory */
@@ -265,7 +265,7 @@ int s5p_mfc_alloc_instance_buffer(struct s5p_mfc_ctx *ctx)
 	if (s5p_mfc_init_shm(ctx) < 0) {
 		vb2_dma_contig_memops.put(ctx->ctx_buf);
 		ctx->ctx_phys = 0;
-		ctx->ctx_buf = 0;
+		ctx->ctx_buf = NULL;
 		return -ENOMEM;
 	}
 	return 0;
@@ -277,12 +277,12 @@ void s5p_mfc_release_instance_buffer(struct s5p_mfc_ctx *ctx)
 	if (ctx->ctx_buf) {
 		vb2_dma_contig_memops.put(ctx->ctx_buf);
 		ctx->ctx_phys = 0;
-		ctx->ctx_buf = 0;
+		ctx->ctx_buf = NULL;
 	}
 	if (ctx->shm_alloc) {
 		vb2_dma_contig_memops.put(ctx->shm_alloc);
-		ctx->shm_alloc = 0;
-		ctx->shm = 0;
+		ctx->shm_alloc = NULL;
+		ctx->shm = NULL;
 	}
 }
 
@@ -296,7 +296,7 @@ void s5p_mfc_set_dec_desc_buffer(struct s5p_mfc_ctx *ctx)
 }
 
 /* Set registers for shared buffer */
-void s5p_mfc_set_shared_buffer(struct s5p_mfc_ctx *ctx)
+static void s5p_mfc_set_shared_buffer(struct s5p_mfc_ctx *ctx)
 {
 	struct s5p_mfc_dev *dev = ctx->dev;
 	mfc_write(dev, ctx->shm_ofs, S5P_FIMV_SI_CH0_HOST_WR_ADR);
diff --git a/drivers/media/video/s5p-tv/hdmi_drv.c b/drivers/media/video/s5p-tv/hdmi_drv.c
index 4865d25a0e5..20cb6eef297 100644
--- a/drivers/media/video/s5p-tv/hdmi_drv.c
+++ b/drivers/media/video/s5p-tv/hdmi_drv.c
@@ -42,7 +42,23 @@ MODULE_DESCRIPTION("Samsung HDMI");
 MODULE_LICENSE("GPL");
 
 /* default preset configured on probe */
-#define HDMI_DEFAULT_PRESET V4L2_DV_1080P60
+#define HDMI_DEFAULT_PRESET V4L2_DV_480P59_94
+
+struct hdmi_pulse {
+	u32 beg;
+	u32 end;
+};
+
+struct hdmi_timings {
+	struct hdmi_pulse hact;
+	u32 hsyn_pol; /* 0 - high, 1 - low */
+	struct hdmi_pulse hsyn;
+	u32 interlaced;
+	struct hdmi_pulse vact[2];
+	u32 vsyn_pol; /* 0 - high, 1 - low */
+	u32 vsyn_off;
+	struct hdmi_pulse vsyn[2];
+};
 
 struct hdmi_resources {
 	struct clk *hdmi;
@@ -70,64 +86,15 @@ struct hdmi_device {
 	/** subdev of MHL interface */
 	struct v4l2_subdev *mhl_sd;
 	/** configuration of current graphic mode */
-	const struct hdmi_preset_conf *cur_conf;
+	const struct hdmi_timings *cur_conf;
+	/** flag indicating that timings are dirty */
+	int cur_conf_dirty;
 	/** current preset */
 	u32 cur_preset;
 	/** other resources */
 	struct hdmi_resources res;
 };
 
-struct hdmi_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_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_preset_conf {
-	struct hdmi_core_regs core;
-	struct hdmi_tg_regs tg;
-	struct v4l2_mbus_framefmt mbus_fmt;
-};
-
 static struct platform_device_id hdmi_driver_types[] = {
 	{
 		.name		= "s5pv210-hdmi",
@@ -165,6 +132,21 @@ void hdmi_writeb(struct hdmi_device *hdev, u32 reg_id, u8 value)
 	writeb(value, hdev->regs + reg_id);
 }
 
+static inline
+void hdmi_writebn(struct hdmi_device *hdev, u32 reg_id, int n, u32 value)
+{
+	switch (n) {
+	default:
+		writeb(value >> 24, hdev->regs + reg_id + 12);
+	case 3:
+		writeb(value >> 16, hdev->regs + reg_id + 8);
+	case 2:
+		writeb(value >>  8, hdev->regs + reg_id + 4);
+	case 1:
+		writeb(value >>  0, hdev->regs + reg_id + 0);
+	}
+}
+
 static inline u32 hdmi_read(struct hdmi_device *hdev, u32 reg_id)
 {
 	return readl(hdev->regs + reg_id);
@@ -211,77 +193,72 @@ static void hdmi_reg_init(struct hdmi_device *hdev)
 }
 
 static void hdmi_timing_apply(struct hdmi_device *hdev,
-	const struct hdmi_preset_conf *conf)
+	const struct hdmi_timings *t)
 {
-	const struct hdmi_core_regs *core = &conf->core;
-	const struct hdmi_tg_regs *tg = &conf->tg;
-
 	/* setting core registers */
-	hdmi_writeb(hdev, HDMI_H_BLANK_0, core->h_blank[0]);
-	hdmi_writeb(hdev, HDMI_H_BLANK_1, core->h_blank[1]);
-	hdmi_writeb(hdev, HDMI_V_BLANK_0, core->v_blank[0]);
-	hdmi_writeb(hdev, HDMI_V_BLANK_1, core->v_blank[1]);
-	hdmi_writeb(hdev, HDMI_V_BLANK_2, core->v_blank[2]);
-	hdmi_writeb(hdev, HDMI_H_V_LINE_0, core->h_v_line[0]);
-	hdmi_writeb(hdev, HDMI_H_V_LINE_1, core->h_v_line[1]);
-	hdmi_writeb(hdev, HDMI_H_V_LINE_2, core->h_v_line[2]);
-	hdmi_writeb(hdev, HDMI_VSYNC_POL, core->vsync_pol[0]);
-	hdmi_writeb(hdev, HDMI_INT_PRO_MODE, core->int_pro_mode[0]);
-	hdmi_writeb(hdev, HDMI_V_BLANK_F_0, core->v_blank_f[0]);
-	hdmi_writeb(hdev, HDMI_V_BLANK_F_1, core->v_blank_f[1]);
-	hdmi_writeb(hdev, HDMI_V_BLANK_F_2, core->v_blank_f[2]);
-	hdmi_writeb(hdev, HDMI_H_SYNC_GEN_0, core->h_sync_gen[0]);
-	hdmi_writeb(hdev, HDMI_H_SYNC_GEN_1, core->h_sync_gen[1]);
-	hdmi_writeb(hdev, HDMI_H_SYNC_GEN_2, core->h_sync_gen[2]);
-	hdmi_writeb(hdev, HDMI_V_SYNC_GEN_1_0, core->v_sync_gen1[0]);
-	hdmi_writeb(hdev, HDMI_V_SYNC_GEN_1_1, core->v_sync_gen1[1]);
-	hdmi_writeb(hdev, HDMI_V_SYNC_GEN_1_2, core->v_sync_gen1[2]);
-	hdmi_writeb(hdev, HDMI_V_SYNC_GEN_2_0, core->v_sync_gen2[0]);
-	hdmi_writeb(hdev, HDMI_V_SYNC_GEN_2_1, core->v_sync_gen2[1]);
-	hdmi_writeb(hdev, HDMI_V_SYNC_GEN_2_2, core->v_sync_gen2[2]);
-	hdmi_writeb(hdev, HDMI_V_SYNC_GEN_3_0, core->v_sync_gen3[0]);
-	hdmi_writeb(hdev, HDMI_V_SYNC_GEN_3_1, core->v_sync_gen3[1]);
-	hdmi_writeb(hdev, HDMI_V_SYNC_GEN_3_2, core->v_sync_gen3[2]);
+	hdmi_writebn(hdev, HDMI_H_BLANK_0, 2, t->hact.beg);
+	hdmi_writebn(hdev, HDMI_H_SYNC_GEN_0, 3,
+		(t->hsyn_pol << 20) | (t->hsyn.end << 10) | t->hsyn.beg);
+	hdmi_writeb(hdev, HDMI_VSYNC_POL, t->vsyn_pol);
+	hdmi_writebn(hdev, HDMI_V_BLANK_0, 3,
+		(t->vact[0].beg << 11) | t->vact[0].end);
+	hdmi_writebn(hdev, HDMI_V_SYNC_GEN_1_0, 3,
+		(t->vsyn[0].beg << 12) | t->vsyn[0].end);
+	if (t->interlaced) {
+		u32 vsyn_trans = t->hsyn.beg + t->vsyn_off;
+
+		hdmi_writeb(hdev, HDMI_INT_PRO_MODE, 1);
+		hdmi_writebn(hdev, HDMI_H_V_LINE_0, 3,
+			(t->hact.end << 12) | t->vact[1].end);
+		hdmi_writebn(hdev, HDMI_V_BLANK_F_0, 3,
+			(t->vact[1].end << 11) | t->vact[1].beg);
+		hdmi_writebn(hdev, HDMI_V_SYNC_GEN_2_0, 3,
+			(t->vsyn[1].beg << 12) | t->vsyn[1].end);
+		hdmi_writebn(hdev, HDMI_V_SYNC_GEN_3_0, 3,
+			(vsyn_trans << 12) | vsyn_trans);
+	} else {
+		hdmi_writeb(hdev, HDMI_INT_PRO_MODE, 0);
+		hdmi_writebn(hdev, HDMI_H_V_LINE_0, 3,
+			(t->hact.end << 12) | t->vact[0].end);
+	}
+
 	/* Timing generator registers */
-	hdmi_writeb(hdev, HDMI_TG_H_FSZ_L, tg->h_fsz_l);
-	hdmi_writeb(hdev, HDMI_TG_H_FSZ_H, tg->h_fsz_h);
-	hdmi_writeb(hdev, HDMI_TG_HACT_ST_L, tg->hact_st_l);
-	hdmi_writeb(hdev, HDMI_TG_HACT_ST_H, tg->hact_st_h);
-	hdmi_writeb(hdev, HDMI_TG_HACT_SZ_L, tg->hact_sz_l);
-	hdmi_writeb(hdev, HDMI_TG_HACT_SZ_H, tg->hact_sz_h);
-	hdmi_writeb(hdev, HDMI_TG_V_FSZ_L, tg->v_fsz_l);
-	hdmi_writeb(hdev, HDMI_TG_V_FSZ_H, tg->v_fsz_h);
-	hdmi_writeb(hdev, HDMI_TG_VSYNC_L, tg->vsync_l);
-	hdmi_writeb(hdev, HDMI_TG_VSYNC_H, tg->vsync_h);
-	hdmi_writeb(hdev, HDMI_TG_VSYNC2_L, tg->vsync2_l);
-	hdmi_writeb(hdev, HDMI_TG_VSYNC2_H, tg->vsync2_h);
-	hdmi_writeb(hdev, HDMI_TG_VACT_ST_L, tg->vact_st_l);
-	hdmi_writeb(hdev, HDMI_TG_VACT_ST_H, tg->vact_st_h);
-	hdmi_writeb(hdev, HDMI_TG_VACT_SZ_L, tg->vact_sz_l);
-	hdmi_writeb(hdev, HDMI_TG_VACT_SZ_H, tg->vact_sz_h);
-	hdmi_writeb(hdev, HDMI_TG_FIELD_CHG_L, tg->field_chg_l);
-	hdmi_writeb(hdev, HDMI_TG_FIELD_CHG_H, tg->field_chg_h);
-	hdmi_writeb(hdev, HDMI_TG_VACT_ST2_L, tg->vact_st2_l);
-	hdmi_writeb(hdev, HDMI_TG_VACT_ST2_H, tg->vact_st2_h);
-	hdmi_writeb(hdev, HDMI_TG_VSYNC_TOP_HDMI_L, tg->vsync_top_hdmi_l);
-	hdmi_writeb(hdev, HDMI_TG_VSYNC_TOP_HDMI_H, tg->vsync_top_hdmi_h);
-	hdmi_writeb(hdev, HDMI_TG_VSYNC_BOT_HDMI_L, tg->vsync_bot_hdmi_l);
-	hdmi_writeb(hdev, HDMI_TG_VSYNC_BOT_HDMI_H, tg->vsync_bot_hdmi_h);
-	hdmi_writeb(hdev, HDMI_TG_FIELD_TOP_HDMI_L, tg->field_top_hdmi_l);
-	hdmi_writeb(hdev, HDMI_TG_FIELD_TOP_HDMI_H, tg->field_top_hdmi_h);
-	hdmi_writeb(hdev, HDMI_TG_FIELD_BOT_HDMI_L, tg->field_bot_hdmi_l);
-	hdmi_writeb(hdev, HDMI_TG_FIELD_BOT_HDMI_H, tg->field_bot_hdmi_h);
+	hdmi_writebn(hdev, HDMI_TG_H_FSZ_L, 2, t->hact.end);
+	hdmi_writebn(hdev, HDMI_TG_HACT_ST_L, 2, t->hact.beg);
+	hdmi_writebn(hdev, HDMI_TG_HACT_SZ_L, 2, t->hact.end - t->hact.beg);
+	hdmi_writebn(hdev, HDMI_TG_VSYNC_L, 2, t->vsyn[0].beg);
+	hdmi_writebn(hdev, HDMI_TG_VACT_ST_L, 2, t->vact[0].beg);
+	hdmi_writebn(hdev, HDMI_TG_VACT_SZ_L, 2,
+		t->vact[0].end - t->vact[0].beg);
+	hdmi_writebn(hdev, HDMI_TG_VSYNC_TOP_HDMI_L, 2, t->vsyn[0].beg);
+	hdmi_writebn(hdev, HDMI_TG_FIELD_TOP_HDMI_L, 2, t->vsyn[0].beg);
+	if (t->interlaced) {
+		hdmi_write_mask(hdev, HDMI_TG_CMD, ~0, HDMI_TG_FIELD_EN);
+		hdmi_writebn(hdev, HDMI_TG_V_FSZ_L, 2, t->vact[1].end);
+		hdmi_writebn(hdev, HDMI_TG_VSYNC2_L, 2, t->vsyn[1].beg);
+		hdmi_writebn(hdev, HDMI_TG_FIELD_CHG_L, 2, t->vact[0].end);
+		hdmi_writebn(hdev, HDMI_TG_VACT_ST2_L, 2, t->vact[1].beg);
+		hdmi_writebn(hdev, HDMI_TG_VSYNC_BOT_HDMI_L, 2, t->vsyn[1].beg);
+		hdmi_writebn(hdev, HDMI_TG_FIELD_BOT_HDMI_L, 2, t->vsyn[1].beg);
+	} else {
+		hdmi_write_mask(hdev, HDMI_TG_CMD, 0, HDMI_TG_FIELD_EN);
+		hdmi_writebn(hdev, HDMI_TG_V_FSZ_L, 2, t->vact[0].end);
+	}
 }
 
 static int hdmi_conf_apply(struct hdmi_device *hdmi_dev)
 {
 	struct device *dev = hdmi_dev->dev;
-	const struct hdmi_preset_conf *conf = hdmi_dev->cur_conf;
+	const struct hdmi_timings *conf = hdmi_dev->cur_conf;
 	struct v4l2_dv_preset preset;
 	int ret;
 
 	dev_dbg(dev, "%s\n", __func__);
 
+	/* skip if conf is already synchronized with HW */
+	if (!hdmi_dev->cur_conf_dirty)
+		return 0;
+
 	/* reset hdmiphy */
 	hdmi_write_mask(hdmi_dev, HDMI_PHY_RSTOUT, ~0, HDMI_PHY_SW_RSTOUT);
 	mdelay(10);
@@ -307,6 +284,8 @@ static int hdmi_conf_apply(struct hdmi_device *hdmi_dev)
 	/* setting core registers */
 	hdmi_timing_apply(hdmi_dev, conf);
 
+	hdmi_dev->cur_conf_dirty = 0;
+
 	return 0;
 }
 
@@ -398,156 +377,126 @@ static void hdmi_dumpregs(struct hdmi_device *hdev, char *prefix)
 #undef DUMPREG
 }
 
-static const struct hdmi_preset_conf hdmi_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 */
-	},
-	.mbus_fmt = {
-		.width = 720,
-		.height = 480,
-		.code = V4L2_MBUS_FMT_FIXED, /* means RGB888 */
-		.field = V4L2_FIELD_NONE,
-		.colorspace = V4L2_COLORSPACE_SRGB,
-	},
+static const struct hdmi_timings hdmi_timings_480p = {
+	.hact = { .beg = 138, .end = 858 },
+	.hsyn_pol = 1,
+	.hsyn = { .beg = 16, .end = 16 + 62 },
+	.interlaced = 0,
+	.vact[0] = { .beg = 42 + 3, .end = 522 + 3 },
+	.vsyn_pol = 1,
+	.vsyn[0] = { .beg = 6 + 3, .end = 12 + 3},
 };
 
-static const struct hdmi_preset_conf hdmi_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},
-		/* other don't care */
-	},
-	.tg = {
-		0x00, /* cmd */
-		0x72, 0x06, /* h_fsz */
-		0x72, 0x01, 0x00, 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, 0x33, 0x02, /* vsync top/bot */
-		0x01, 0x00, 0x33, 0x02, /* field top/bot */
-	},
-	.mbus_fmt = {
-		.width = 1280,
-		.height = 720,
-		.code = V4L2_MBUS_FMT_FIXED, /* means RGB888 */
-		.field = V4L2_FIELD_NONE,
-		.colorspace = V4L2_COLORSPACE_SRGB,
-	},
+static const struct hdmi_timings hdmi_timings_576p50 = {
+	.hact = { .beg = 144, .end = 864 },
+	.hsyn_pol = 1,
+	.hsyn = { .beg = 12, .end = 12 + 64 },
+	.interlaced = 0,
+	.vact[0] = { .beg = 44 + 5, .end = 620 + 5 },
+	.vsyn_pol = 1,
+	.vsyn[0] = { .beg = 0 + 5, .end = 5 + 5},
 };
 
-static const struct hdmi_preset_conf hdmi_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},
-		/* other don't care */
-	},
-	.tg = {
-		0x00, /* cmd */
-		0x98, 0x08, /* h_fsz */
-		0x18, 0x01, 0x80, 0x07, /* hact */
-		0x65, 0x04, /* v_fsz */
-		0x01, 0x00, 0x33, 0x02, /* vsync */
-		0x2d, 0x00, 0x38, 0x04, /* vact */
-		0x33, 0x02, /* field_chg */
-		0x49, 0x02, /* vact_st2 */
-		0x01, 0x00, 0x33, 0x02, /* vsync top/bot */
-		0x01, 0x00, 0x33, 0x02, /* field top/bot */
-	},
-	.mbus_fmt = {
-		.width = 1920,
-		.height = 1080,
-		.code = V4L2_MBUS_FMT_FIXED, /* means RGB888 */
-		.field = V4L2_FIELD_NONE,
-		.colorspace = V4L2_COLORSPACE_SRGB,
-	},
+static const struct hdmi_timings hdmi_timings_720p60 = {
+	.hact = { .beg = 370, .end = 1650 },
+	.hsyn_pol = 0,
+	.hsyn = { .beg = 110, .end = 110 + 40 },
+	.interlaced = 0,
+	.vact[0] = { .beg = 25 + 5, .end = 745 + 5 },
+	.vsyn_pol = 0,
+	.vsyn[0] = { .beg = 0 + 5, .end = 5 + 5},
 };
 
-static const struct hdmi_preset_conf hdmi_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},
-		/* other don't care */
-	},
-	.tg = {
-		0x00, /* cmd */
-		0x98, 0x08, /* h_fsz */
-		0x18, 0x01, 0x80, 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 */
-	},
-	.mbus_fmt = {
-		.width = 1920,
-		.height = 1080,
-		.code = V4L2_MBUS_FMT_FIXED, /* means RGB888 */
-		.field = V4L2_FIELD_NONE,
-		.colorspace = V4L2_COLORSPACE_SRGB,
-	},
+static const struct hdmi_timings hdmi_timings_720p50 = {
+	.hact = { .beg = 700, .end = 1980 },
+	.hsyn_pol = 0,
+	.hsyn = { .beg = 440, .end = 440 + 40 },
+	.interlaced = 0,
+	.vact[0] = { .beg = 25 + 5, .end = 745 + 5 },
+	.vsyn_pol = 0,
+	.vsyn[0] = { .beg = 0 + 5, .end = 5 + 5},
+};
+
+static const struct hdmi_timings hdmi_timings_1080p24 = {
+	.hact = { .beg = 830, .end = 2750 },
+	.hsyn_pol = 0,
+	.hsyn = { .beg = 638, .end = 638 + 44 },
+	.interlaced = 0,
+	.vact[0] = { .beg = 41 + 4, .end = 1121 + 4 },
+	.vsyn_pol = 0,
+	.vsyn[0] = { .beg = 0 + 4, .end = 5 + 4},
+};
+
+static const struct hdmi_timings hdmi_timings_1080p60 = {
+	.hact = { .beg = 280, .end = 2200 },
+	.hsyn_pol = 0,
+	.hsyn = { .beg = 88, .end = 88 + 44 },
+	.interlaced = 0,
+	.vact[0] = { .beg = 41 + 4, .end = 1121 + 4 },
+	.vsyn_pol = 0,
+	.vsyn[0] = { .beg = 0 + 4, .end = 5 + 4},
+};
+
+static const struct hdmi_timings hdmi_timings_1080i60 = {
+	.hact = { .beg = 280, .end = 2200 },
+	.hsyn_pol = 0,
+	.hsyn = { .beg = 88, .end = 88 + 44 },
+	.interlaced = 1,
+	.vact[0] = { .beg = 20 + 2, .end = 560 + 2 },
+	.vact[1] = { .beg = 583 + 2, .end = 1123 + 2 },
+	.vsyn_pol = 0,
+	.vsyn_off = 1100,
+	.vsyn[0] = { .beg = 0 + 2, .end = 5 + 2},
+	.vsyn[1] = { .beg = 562 + 2, .end = 567 + 2},
+};
+
+static const struct hdmi_timings hdmi_timings_1080i50 = {
+	.hact = { .beg = 720, .end = 2640 },
+	.hsyn_pol = 0,
+	.hsyn = { .beg = 528, .end = 528 + 44 },
+	.interlaced = 1,
+	.vact[0] = { .beg = 20 + 2, .end = 560 + 2 },
+	.vact[1] = { .beg = 583 + 2, .end = 1123 + 2 },
+	.vsyn_pol = 0,
+	.vsyn_off = 1320,
+	.vsyn[0] = { .beg = 0 + 2, .end = 5 + 2},
+	.vsyn[1] = { .beg = 562 + 2, .end = 567 + 2},
+};
+
+static const struct hdmi_timings hdmi_timings_1080p50 = {
+	.hact = { .beg = 720, .end = 2640 },
+	.hsyn_pol = 0,
+	.hsyn = { .beg = 528, .end = 528 + 44 },
+	.interlaced = 0,
+	.vact[0] = { .beg = 41 + 4, .end = 1121 + 4 },
+	.vsyn_pol = 0,
+	.vsyn[0] = { .beg = 0 + 4, .end = 5 + 4},
 };
 
 static const struct {
 	u32 preset;
-	const struct hdmi_preset_conf *conf;
-} hdmi_conf[] = {
-	{ V4L2_DV_480P59_94, &hdmi_conf_480p },
-	{ V4L2_DV_720P59_94, &hdmi_conf_720p60 },
-	{ V4L2_DV_1080P50, &hdmi_conf_1080p50 },
-	{ V4L2_DV_1080P30, &hdmi_conf_1080p60 },
-	{ V4L2_DV_1080P60, &hdmi_conf_1080p60 },
+	const struct hdmi_timings *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 },
 };
 
-static const struct hdmi_preset_conf *hdmi_preset2conf(u32 preset)
+static const struct hdmi_timings *hdmi_preset2timings(u32 preset)
 {
 	int i;
 
-	for (i = 0; i < ARRAY_SIZE(hdmi_conf); ++i)
-		if (hdmi_conf[i].preset == preset)
-			return  hdmi_conf[i].conf;
+	for (i = 0; i < ARRAY_SIZE(hdmi_timings); ++i)
+		if (hdmi_timings[i].preset == preset)
+			return  hdmi_timings[i].timings;
 	return NULL;
 }
 
@@ -559,6 +508,10 @@ static int hdmi_streamon(struct hdmi_device *hdev)
 
 	dev_dbg(dev, "%s\n", __func__);
 
+	ret = hdmi_conf_apply(hdev);
+	if (ret)
+		return ret;
+
 	ret = v4l2_subdev_call(hdev->phy_sd, video, s_stream, 1);
 	if (ret)
 		return ret;
@@ -671,14 +624,15 @@ static int hdmi_s_dv_preset(struct v4l2_subdev *sd,
 {
 	struct hdmi_device *hdev = sd_to_hdmi_dev(sd);
 	struct device *dev = hdev->dev;
-	const struct hdmi_preset_conf *conf;
+	const struct hdmi_timings *conf;
 
-	conf = hdmi_preset2conf(preset->preset);
+	conf = hdmi_preset2timings(preset->preset);
 	if (conf == NULL) {
 		dev_err(dev, "preset (%u) not supported\n", preset->preset);
 		return -EINVAL;
 	}
 	hdev->cur_conf = conf;
+	hdev->cur_conf_dirty = 1;
 	hdev->cur_preset = preset->preset;
 	return 0;
 }
@@ -695,21 +649,32 @@ static int hdmi_g_mbus_fmt(struct v4l2_subdev *sd,
 	  struct v4l2_mbus_framefmt *fmt)
 {
 	struct hdmi_device *hdev = sd_to_hdmi_dev(sd);
-	struct device *dev = hdev->dev;
+	const struct hdmi_timings *t = hdev->cur_conf;
 
-	dev_dbg(dev, "%s\n", __func__);
+	dev_dbg(hdev->dev, "%s\n", __func__);
 	if (!hdev->cur_conf)
 		return -EINVAL;
-	*fmt = hdev->cur_conf->mbus_fmt;
+	memset(fmt, 0, sizeof *fmt);
+	fmt->width = t->hact.end - t->hact.beg;
+	fmt->height = t->vact[0].end - t->vact[0].beg;
+	fmt->code = V4L2_MBUS_FMT_FIXED; /* means RGB888 */
+	fmt->colorspace = V4L2_COLORSPACE_SRGB;
+	if (t->interlaced) {
+		fmt->field = V4L2_FIELD_INTERLACED;
+		fmt->height *= 2;
+	} else {
+		fmt->field = V4L2_FIELD_NONE;
+	}
 	return 0;
 }
 
 static int hdmi_enum_dv_presets(struct v4l2_subdev *sd,
 	struct v4l2_dv_enum_preset *preset)
 {
-	if (preset->index >= ARRAY_SIZE(hdmi_conf))
+	if (preset->index >= ARRAY_SIZE(hdmi_timings))
 		return -EINVAL;
-	return v4l_fill_dv_preset_info(hdmi_conf[preset->index].preset, preset);
+	return v4l_fill_dv_preset_info(hdmi_timings[preset->index].preset,
+		preset);
 }
 
 static const struct v4l2_subdev_core_ops hdmi_sd_core_ops = {
@@ -737,6 +702,8 @@ static int hdmi_runtime_suspend(struct device *dev)
 	dev_dbg(dev, "%s\n", __func__);
 	v4l2_subdev_call(hdev->mhl_sd, core, s_power, 0);
 	hdmi_resource_poweroff(&hdev->res);
+	/* flag that device context is lost */
+	hdev->cur_conf_dirty = 1;
 	return 0;
 }
 
@@ -750,10 +717,6 @@ static int hdmi_runtime_resume(struct device *dev)
 
 	hdmi_resource_poweron(&hdev->res);
 
-	ret = hdmi_conf_apply(hdev);
-	if (ret)
-		goto fail;
-
 	/* starting MHL */
 	ret = v4l2_subdev_call(hdev->mhl_sd, core, s_power, 1);
 	if (hdev->mhl_sd && ret)
@@ -993,7 +956,8 @@ static int __devinit hdmi_probe(struct platform_device *pdev)
 	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_preset2conf(hdmi_dev->cur_preset);
+	hdmi_dev->cur_conf = hdmi_preset2timings(hdmi_dev->cur_preset);
+	hdmi_dev->cur_conf_dirty = 1;
 
 	/* storing subdev for call that have only access to struct device */
 	dev_set_drvdata(dev, sd);
diff --git a/drivers/media/video/s5p-tv/hdmiphy_drv.c b/drivers/media/video/s5p-tv/hdmiphy_drv.c
index 0afef77747e..f67b3863180 100644
--- a/drivers/media/video/s5p-tv/hdmiphy_drv.c
+++ b/drivers/media/video/s5p-tv/hdmiphy_drv.c
@@ -26,53 +26,188 @@ MODULE_DESCRIPTION("Samsung HDMI Physical interface driver");
 MODULE_LICENSE("GPL");
 
 struct hdmiphy_conf {
-	u32 preset;
+	unsigned long pixclk;
 	const u8 *data;
 };
 
-static const u8 hdmiphy_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,
+struct hdmiphy_ctx {
+	struct v4l2_subdev sd;
+	const struct hdmiphy_conf *conf_tab;
 };
 
-static const u8 hdmiphy_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 struct hdmiphy_conf hdmiphy_conf_s5pv210[] = {
+	{ .pixclk = 27000000, .data = (u8 [32]) {
+		0x01, 0x05, 0x00, 0xD8, 0x10, 0x1C, 0x30, 0x40,
+		0x6B, 0x10, 0x02, 0x52, 0xDF, 0xF2, 0x54, 0x87,
+		0x84, 0x00, 0x30, 0x38, 0x00, 0x08, 0x10, 0xE0,
+		0x22, 0x40, 0xE3, 0x26, 0x00, 0x00, 0x00, 0x00, }
+	},
+	{ .pixclk = 27027000, .data = (u8 [32]) {
+		0x01, 0x05, 0x00, 0xD4, 0x10, 0x9C, 0x09, 0x64,
+		0x6B, 0x10, 0x02, 0x52, 0xDF, 0xF2, 0x54, 0x87,
+		0x84, 0x00, 0x30, 0x38, 0x00, 0x08, 0x10, 0xE0,
+		0x22, 0x40, 0xE2, 0x26, 0x00, 0x00, 0x00, 0x00, }
+	},
+	{ .pixclk = 74176000, .data = (u8 [32]) {
+		0x01, 0x05, 0x00, 0xD8, 0x10, 0x9C, 0xEF, 0x5B,
+		0x6D, 0x10, 0x01, 0x52, 0xEF, 0xF3, 0x54, 0xB9,
+		0x84, 0x00, 0x30, 0x38, 0x00, 0x08, 0x10, 0xE0,
+		0x22, 0x40, 0xA5, 0x26, 0x01, 0x00, 0x00, 0x00, }
+	},
+	{ .pixclk = 74250000, .data = (u8 [32]) {
+		0x01, 0x05, 0x00, 0xD8, 0x10, 0x9C, 0xF8, 0x40,
+		0x6A, 0x10, 0x01, 0x52, 0xFF, 0xF1, 0x54, 0xBA,
+		0x84, 0x00, 0x10, 0x38, 0x00, 0x08, 0x10, 0xE0,
+		0x22, 0x40, 0xA4, 0x26, 0x01, 0x00, 0x00, 0x00, }
+	},
+	{ /* end marker */ }
 };
 
-static const u8 hdmiphy_conf74_25[32] = {
-	0x01, 0x05, 0x00, 0xd8, 0x10, 0x9c, 0xf8, 0x40,
-	0x6a, 0x10, 0x01, 0x51, 0xff, 0xf1, 0x54, 0xba,
-	0x84, 0x00, 0x30, 0x38, 0x00, 0x08, 0x10, 0xe0,
-	0x22, 0x40, 0xa4, 0x26, 0x01, 0x00, 0x00, 0x00,
+static const struct hdmiphy_conf hdmiphy_conf_exynos4210[] = {
+	{ .pixclk = 27000000, .data = (u8 [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, }
+	},
+	{ .pixclk = 27027000, .data = (u8 [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, 0xE2, 0x26, 0x00, 0x00, 0x00, 0x00, }
+	},
+	{ .pixclk = 74176000, .data = (u8 [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, }
+	},
+	{ .pixclk = 74250000, .data = (u8 [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, }
+	},
+	{ .pixclk = 148352000, .data = (u8 [32]) {
+		0x01, 0x05, 0x00, 0xD8, 0x10, 0x9C, 0xEF, 0x5B,
+		0x6D, 0x18, 0x00, 0x51, 0xEF, 0xF3, 0x54, 0xB9,
+		0x84, 0x00, 0x30, 0x38, 0x00, 0x08, 0x10, 0xE0,
+		0x11, 0x40, 0xA5, 0x26, 0x02, 0x00, 0x00, 0x00, }
+	},
+	{ .pixclk = 148500000, .data = (u8 [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,
+		0x11, 0x40, 0xA4, 0x26, 0x02, 0x00, 0x00, 0x00, }
+	},
+	{ /* end marker */ }
 };
 
-static const u8 hdmiphy_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,
+static const struct hdmiphy_conf hdmiphy_conf_exynos4212[] = {
+	{ .pixclk = 27000000, .data = (u8 [32]) {
+		0x01, 0x11, 0x2D, 0x75, 0x00, 0x01, 0x00, 0x08,
+		0x82, 0x00, 0x0E, 0xD9, 0x45, 0xA0, 0x34, 0xC0,
+		0x0B, 0x80, 0x12, 0x87, 0x08, 0x24, 0x24, 0x71,
+		0x54, 0xE3, 0x24, 0x00, 0x00, 0x00, 0x01, 0x00, }
+	},
+	{ .pixclk = 27027000, .data = (u8 [32]) {
+		0x01, 0x91, 0x2D, 0x72, 0x00, 0x64, 0x12, 0x08,
+		0x43, 0x20, 0x0E, 0xD9, 0x45, 0xA0, 0x34, 0xC0,
+		0x0B, 0x80, 0x12, 0x87, 0x08, 0x24, 0x24, 0x71,
+		0x54, 0xE2, 0x24, 0x00, 0x00, 0x00, 0x01, 0x00, }
+	},
+	{ .pixclk = 74176000, .data = (u8 [32]) {
+		0x01, 0x91, 0x3E, 0x35, 0x00, 0x5B, 0xDE, 0x08,
+		0x82, 0x20, 0x73, 0xD9, 0x45, 0xA0, 0x34, 0xC0,
+		0x0B, 0x80, 0x12, 0x87, 0x08, 0x24, 0x24, 0x52,
+		0x54, 0xA5, 0x24, 0x01, 0x00, 0x00, 0x01, 0x00, }
+	},
+	{ .pixclk = 74250000, .data = (u8 [32]) {
+		0x01, 0x91, 0x3E, 0x35, 0x00, 0x40, 0xF0, 0x08,
+		0x82, 0x20, 0x73, 0xD9, 0x45, 0xA0, 0x34, 0xC0,
+		0x0B, 0x80, 0x12, 0x87, 0x08, 0x24, 0x24, 0x52,
+		0x54, 0xA4, 0x24, 0x01, 0x00, 0x00, 0x01, 0x00, }
+	},
+	{ .pixclk = 148500000, .data = (u8 [32]) {
+		0x01, 0x91, 0x3E, 0x15, 0x00, 0x40, 0xF0, 0x08,
+		0x82, 0x20, 0x73, 0xD9, 0x45, 0xA0, 0x34, 0xC0,
+		0x0B, 0x80, 0x12, 0x87, 0x08, 0x24, 0x24, 0xA4,
+		0x54, 0x4A, 0x25, 0x03, 0x00, 0x00, 0x01, 0x00, }
+	},
+	{ /* end marker */ }
 };
 
-static const struct hdmiphy_conf hdmiphy_conf[] = {
-	{ V4L2_DV_480P59_94, hdmiphy_conf27 },
-	{ V4L2_DV_1080P30, hdmiphy_conf74_175 },
-	{ V4L2_DV_720P59_94, hdmiphy_conf74_175 },
-	{ V4L2_DV_720P60, hdmiphy_conf74_25 },
-	{ V4L2_DV_1080P50, hdmiphy_conf148_5 },
-	{ V4L2_DV_1080P60, hdmiphy_conf148_5 },
+static const struct hdmiphy_conf hdmiphy_conf_exynos4412[] = {
+	{ .pixclk = 27000000, .data = (u8 [32]) {
+		0x01, 0x11, 0x2D, 0x75, 0x40, 0x01, 0x00, 0x08,
+		0x82, 0x00, 0x0E, 0xD9, 0x45, 0xA0, 0xAC, 0x80,
+		0x08, 0x80, 0x11, 0x84, 0x02, 0x22, 0x44, 0x86,
+		0x54, 0xE4, 0x24, 0x00, 0x00, 0x00, 0x01, 0x00, }
+	},
+	{ .pixclk = 27027000, .data = (u8 [32]) {
+		0x01, 0x91, 0x2D, 0x72, 0x40, 0x64, 0x12, 0x08,
+		0x43, 0x20, 0x0E, 0xD9, 0x45, 0xA0, 0xAC, 0x80,
+		0x08, 0x80, 0x11, 0x84, 0x02, 0x22, 0x44, 0x86,
+		0x54, 0xE3, 0x24, 0x00, 0x00, 0x00, 0x01, 0x00, }
+	},
+	{ .pixclk = 74176000, .data = (u8 [32]) {
+		0x01, 0x91, 0x1F, 0x10, 0x40, 0x5B, 0xEF, 0x08,
+		0x81, 0x20, 0xB9, 0xD8, 0x45, 0xA0, 0xAC, 0x80,
+		0x08, 0x80, 0x11, 0x84, 0x02, 0x22, 0x44, 0x86,
+		0x54, 0xA6, 0x24, 0x01, 0x00, 0x00, 0x01, 0x00, }
+	},
+	{ .pixclk = 74250000, .data = (u8 [32]) {
+		0x01, 0x91, 0x1F, 0x10, 0x40, 0x40, 0xF8, 0x08,
+		0x81, 0x20, 0xBA, 0xD8, 0x45, 0xA0, 0xAC, 0x80,
+		0x08, 0x80, 0x11, 0x84, 0x02, 0x22, 0x44, 0x86,
+		0x54, 0xA5, 0x24, 0x01, 0x00, 0x00, 0x01, 0x00, }
+	},
+	{ .pixclk = 148500000, .data = (u8 [32]) {
+		0x01, 0x91, 0x1F, 0x00, 0x40, 0x40, 0xF8, 0x08,
+		0x81, 0x20, 0xBA, 0xD8, 0x45, 0xA0, 0xAC, 0x80,
+		0x08, 0x80, 0x11, 0x84, 0x02, 0x22, 0x44, 0x86,
+		0x54, 0x4B, 0x25, 0x03, 0x00, 0x00, 0x01, 0x00, }
+	},
+	{ /* end marker */ }
 };
 
-const u8 *hdmiphy_preset2conf(u32 preset)
+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 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)
 {
-	int i;
-	for (i = 0; i < ARRAY_SIZE(hdmiphy_conf); ++i)
-		if (hdmiphy_conf[i].preset == preset)
-			return hdmiphy_conf[i].data;
+	unsigned long pixclk;
+
+	pixclk = hdmiphy_preset_to_pixclk(preset);
+	if (!pixclk)
+		return NULL;
+
+	for (; conf->pixclk; ++conf)
+		if (conf->pixclk == pixclk)
+			return conf->data;
 	return NULL;
 }
 
@@ -88,11 +223,12 @@ static int hdmiphy_s_dv_preset(struct v4l2_subdev *sd,
 	const u8 *data;
 	u8 buffer[32];
 	int ret;
+	struct hdmiphy_ctx *ctx = sd_to_ctx(sd);
 	struct i2c_client *client = v4l2_get_subdevdata(sd);
 	struct device *dev = &client->dev;
 
 	dev_info(dev, "s_dv_preset(preset = %d)\n", preset->preset);
-	data = hdmiphy_preset2conf(preset->preset);
+	data = hdmiphy_find_conf(preset->preset, ctx->conf_tab);
 	if (!data) {
 		dev_err(dev, "format not supported\n");
 		return -EINVAL;
@@ -146,21 +282,36 @@ static const struct v4l2_subdev_ops hdmiphy_ops = {
 static int __devinit hdmiphy_probe(struct i2c_client *client,
 	const struct i2c_device_id *id)
 {
-	static struct v4l2_subdev sd;
+	struct hdmiphy_ctx *ctx;
+
+	ctx = kzalloc(sizeof *ctx, GFP_KERNEL);
+	if (!ctx)
+		return -ENOMEM;
+
+	ctx->conf_tab = (struct hdmiphy_conf *)id->driver_data;
+	v4l2_i2c_subdev_init(&ctx->sd, client, &hdmiphy_ops);
 
-	v4l2_i2c_subdev_init(&sd, client, &hdmiphy_ops);
 	dev_info(&client->dev, "probe successful\n");
 	return 0;
 }
 
 static int __devexit hdmiphy_remove(struct i2c_client *client)
 {
+	struct v4l2_subdev *sd = i2c_get_clientdata(client);
+	struct hdmiphy_ctx *ctx = sd_to_ctx(sd);
+
+	kfree(ctx);
 	dev_info(&client->dev, "remove successful\n");
+
 	return 0;
 }
 
 static const struct i2c_device_id hdmiphy_id[] = {
-	{ "hdmiphy", 0 },
+	{ "hdmiphy", (unsigned long)hdmiphy_conf_exynos4210 },
+	{ "hdmiphy-s5pv210", (unsigned long)hdmiphy_conf_s5pv210 },
+	{ "hdmiphy-exynos4210", (unsigned long)hdmiphy_conf_exynos4210 },
+	{ "hdmiphy-exynos4212", (unsigned long)hdmiphy_conf_exynos4212 },
+	{ "hdmiphy-exynos4412", (unsigned long)hdmiphy_conf_exynos4412 },
 	{ },
 };
 MODULE_DEVICE_TABLE(i2c, hdmiphy_id);
diff --git a/drivers/media/video/s5p-tv/mixer.h b/drivers/media/video/s5p-tv/mixer.h
index 1597078c4a5..ddb422e2355 100644
--- a/drivers/media/video/s5p-tv/mixer.h
+++ b/drivers/media/video/s5p-tv/mixer.h
@@ -226,6 +226,7 @@ struct mxr_resources {
 /* event flags used  */
 enum mxr_devide_flags {
 	MXR_EVENT_VSYNC = 0,
+	MXR_EVENT_TOP = 1,
 };
 
 /** drivers instance */
@@ -293,7 +294,7 @@ int __devinit mxr_acquire_video(struct mxr_device *mdev,
 	struct mxr_output_conf *output_cont, int output_count);
 
 /** releasing common video resources */
-void __devexit mxr_release_video(struct mxr_device *mdev);
+void mxr_release_video(struct mxr_device *mdev);
 
 struct mxr_layer *mxr_graph_layer_create(struct mxr_device *mdev, int idx);
 struct mxr_layer *mxr_vp_layer_create(struct mxr_device *mdev, int idx);
diff --git a/drivers/media/video/s5p-tv/mixer_drv.c b/drivers/media/video/s5p-tv/mixer_drv.c
index a2c0c25ad13..edca0659288 100644
--- a/drivers/media/video/s5p-tv/mixer_drv.c
+++ b/drivers/media/video/s5p-tv/mixer_drv.c
@@ -461,7 +461,7 @@ static struct platform_driver mxr_driver __refdata = {
 static int __init mxr_init(void)
 {
 	int i, ret;
-	static const char banner[] __initdata = KERN_INFO
+	static const char banner[] __initconst = KERN_INFO
 		"Samsung TV Mixer driver, "
 		"(c) 2010-2011 Samsung Electronics Co., Ltd.\n";
 	printk(banner);
diff --git a/drivers/media/video/s5p-tv/mixer_reg.c b/drivers/media/video/s5p-tv/mixer_reg.c
index 4800a3cbb29..3b1670a045f 100644
--- a/drivers/media/video/s5p-tv/mixer_reg.c
+++ b/drivers/media/video/s5p-tv/mixer_reg.c
@@ -296,21 +296,25 @@ irqreturn_t mxr_irq_handler(int irq, void *dev_data)
 	/* wake up process waiting for VSYNC */
 	if (val & MXR_INT_STATUS_VSYNC) {
 		set_bit(MXR_EVENT_VSYNC, &mdev->event_flags);
+		/* toggle TOP field event if working in interlaced mode */
+		if (~mxr_read(mdev, MXR_CFG) & MXR_CFG_SCAN_PROGRASSIVE)
+			change_bit(MXR_EVENT_TOP, &mdev->event_flags);
 		wake_up(&mdev->event_queue);
-	}
-
-	/* clear interrupts */
-	if (~val & MXR_INT_EN_VSYNC) {
 		/* vsync interrupt use different bit for read and clear */
-		val &= ~MXR_INT_EN_VSYNC;
+		val &= ~MXR_INT_STATUS_VSYNC;
 		val |= MXR_INT_CLEAR_VSYNC;
 	}
+
+	/* clear interrupts */
 	mxr_write(mdev, MXR_INT_STATUS, val);
 
 	spin_unlock(&mdev->reg_slock);
 	/* leave on non-vsync event */
 	if (~val & MXR_INT_CLEAR_VSYNC)
 		return IRQ_HANDLED;
+	/* skip layer update on bottom field */
+	if (!test_bit(MXR_EVENT_TOP, &mdev->event_flags))
+		return IRQ_HANDLED;
 	for (i = 0; i < MXR_MAX_LAYERS; ++i)
 		mxr_irq_layer_handle(mdev->layer[i]);
 	return IRQ_HANDLED;
@@ -333,6 +337,7 @@ void mxr_reg_streamon(struct mxr_device *mdev)
 
 	/* start MIXER */
 	mxr_write_mask(mdev, MXR_STATUS, ~0, MXR_STATUS_REG_RUN);
+	set_bit(MXR_EVENT_TOP, &mdev->event_flags);
 
 	spin_unlock_irqrestore(&mdev->reg_slock, flags);
 }
diff --git a/drivers/media/video/s5p-tv/mixer_video.c b/drivers/media/video/s5p-tv/mixer_video.c
index f7ca5cc143c..33fde2a763e 100644
--- a/drivers/media/video/s5p-tv/mixer_video.c
+++ b/drivers/media/video/s5p-tv/mixer_video.c
@@ -140,7 +140,7 @@ fail:
 	return ret;
 }
 
-void __devexit mxr_release_video(struct mxr_device *mdev)
+void mxr_release_video(struct mxr_device *mdev)
 {
 	int i;
 
@@ -853,8 +853,8 @@ static int queue_setup(struct vb2_queue *vq, const struct v4l2_format *pfmt,
 	*nplanes = fmt->num_subframes;
 	for (i = 0; i < fmt->num_subframes; ++i) {
 		alloc_ctxs[i] = layer->mdev->alloc_ctx;
-		sizes[i] = PAGE_ALIGN(planes[i].sizeimage);
-		mxr_dbg(mdev, "size[%d] = %08lx\n", i, sizes[i]);
+		sizes[i] = planes[i].sizeimage;
+		mxr_dbg(mdev, "size[%d] = %08x\n", i, sizes[i]);
 	}
 
 	if (*nbuffers == 0)
@@ -1069,6 +1069,10 @@ struct mxr_layer *mxr_base_layer_create(struct mxr_device *mdev,
 	set_bit(V4L2_FL_USE_FH_PRIO, &layer->vfd.flags);
 
 	video_set_drvdata(&layer->vfd, layer);
+	/* Locking in file operations other than ioctl should be done
+	   by the driver, not the V4L2 core.
+	   This driver needs auditing so that this flag can be removed. */
+	set_bit(V4L2_FL_LOCK_ALL_FOPS, &layer->vfd.flags);
 	layer->vfd.lock = &layer->mutex;
 	layer->vfd.v4l2_dev = &mdev->v4l2_dev;
 
diff --git a/drivers/media/video/s5p-tv/regs-hdmi.h b/drivers/media/video/s5p-tv/regs-hdmi.h
index 33247d13e4c..a889d1f57f2 100644
--- a/drivers/media/video/s5p-tv/regs-hdmi.h
+++ b/drivers/media/video/s5p-tv/regs-hdmi.h
@@ -140,6 +140,7 @@
 #define HDMI_MODE_MASK			(3 << 0)
 
 /* HDMI_TG_CMD */
+#define HDMI_TG_FIELD_EN		(1 << 1)
 #define HDMI_TG_EN			(1 << 0)
 
 #endif /* SAMSUNG_REGS_HDMI_H */
diff --git a/drivers/media/video/saa7134/saa7134-cards.c b/drivers/media/video/saa7134/saa7134-cards.c
index 53aae5968ff..bc08f1dbc29 100644
--- a/drivers/media/video/saa7134/saa7134-cards.c
+++ b/drivers/media/video/saa7134/saa7134-cards.c
@@ -5080,6 +5080,36 @@ struct saa7134_board saa7134_boards[] = {
 			.gpio = 0x0200000,
 		},
 	},
+	[SAA7134_BOARD_ASUSTeK_PS3_100] = {
+		.name           = "Asus My Cinema PS3-100",
+		.audio_clock    = 0x00187de7,
+		.tuner_type     = TUNER_PHILIPS_TDA8290,
+		.radio_type     = UNSET,
+		.tuner_addr     = ADDR_UNSET,
+		.radio_addr     = ADDR_UNSET,
+		.tuner_config   = 2,
+		.gpiomask       = 1 << 21,
+		.mpeg           = SAA7134_MPEG_DVB,
+		.inputs         = {{
+			.name = name_tv,
+			.vmux = 1,
+			.amux = TV,
+			.tv   = 1,
+		}, {
+			.name = name_comp,
+			.vmux = 0,
+			.amux = LINE2,
+		}, {
+			.name = name_svideo,
+			.vmux = 8,
+			.amux = LINE2,
+		} },
+		.radio = {
+			.name = name_radio,
+			.amux = TV,
+			.gpio = 0x0200000,
+		},
+	},
 	[SAA7134_BOARD_REAL_ANGEL_220] = {
 		.name           = "Zogis Real Angel 220",
 		.audio_clock    = 0x00187de7,
@@ -6877,6 +6907,12 @@ struct pci_device_id saa7134_pci_tbl[] = {
 		.driver_data  = SAA7134_BOARD_ASUSTeK_TIGER_3IN1,
 	}, {
 		.vendor       = PCI_VENDOR_ID_PHILIPS,
+		.device       = PCI_DEVICE_ID_PHILIPS_SAA7133,
+		.subvendor    = 0x1043,
+		.subdevice    = 0x48cd,
+		.driver_data  = SAA7134_BOARD_ASUSTeK_PS3_100,
+	}, {
+		.vendor       = PCI_VENDOR_ID_PHILIPS,
 		.device       = PCI_DEVICE_ID_PHILIPS_SAA7134,
 		.subvendor    = 0x17de,
 		.subdevice    = 0x7128,
@@ -7347,6 +7383,7 @@ int saa7134_board_init1(struct saa7134_dev *dev)
 	case SAA7134_BOARD_KWORLD_TERMINATOR:
 	case SAA7134_BOARD_SEDNA_PC_TV_CARDBUS:
 	case SAA7134_BOARD_FLYDVBT_LR301:
+	case SAA7134_BOARD_ASUSTeK_PS3_100:
 	case SAA7134_BOARD_ASUSTeK_P7131_DUAL:
 	case SAA7134_BOARD_ASUSTeK_P7131_HYBRID_LNA:
 	case SAA7134_BOARD_ASUSTeK_P7131_ANALOG:
@@ -7811,6 +7848,14 @@ int saa7134_board_init2(struct saa7134_dev *dev)
 		i2c_transfer(&dev->i2c_adap, &msg, 1);
 		break;
 	}
+	case SAA7134_BOARD_ASUSTeK_PS3_100:
+	{
+		u8 data[] = { 0x3c, 0x33, 0x60};
+		struct i2c_msg msg = {.addr = 0x0b, .flags = 0, .buf = data,
+						       .len = sizeof(data)};
+		i2c_transfer(&dev->i2c_adap, &msg, 1);
+		break;
+	}
 	case SAA7134_BOARD_FLYDVB_TRIO:
 	{
 		u8 temp = 0;
diff --git a/drivers/media/video/saa7134/saa7134-dvb.c b/drivers/media/video/saa7134/saa7134-dvb.c
index aaa5c97a721..5dfd826d734 100644
--- a/drivers/media/video/saa7134/saa7134-dvb.c
+++ b/drivers/media/video/saa7134/saa7134-dvb.c
@@ -881,6 +881,20 @@ static struct tda1004x_config asus_tiger_3in1_config = {
 	.request_firmware = philips_tda1004x_request_firmware
 };
 
+static struct tda1004x_config asus_ps3_100_config = {
+	.demod_address = 0x0b,
+	.invert        = 1,
+	.invert_oclk   = 0,
+	.xtal_freq     = TDA10046_XTAL_16M,
+	.agc_config    = TDA10046_AGC_TDA827X,
+	.gpio_config   = TDA10046_GP11_I,
+	.if_freq       = TDA10046_FREQ_045,
+	.i2c_gate      = 0x4b,
+	.tuner_address = 0x61,
+	.antenna_switch = 1,
+	.request_firmware = philips_tda1004x_request_firmware
+};
+
 /* ------------------------------------------------------------------
  * special case: this card uses saa713x GPIO22 for the mode switch
  */
@@ -1647,6 +1661,31 @@ static int dvb_init(struct saa7134_dev *dev)
 						&dev->i2c_adap, 0, 0) == NULL) {
 					wprintk("%s: Asus Tiger 3in1, no lnbp21"
 						" found!\n", __func__);
+				       goto dettach_frontend;
+			       }
+		       }
+	       }
+	       break;
+	case SAA7134_BOARD_ASUSTeK_PS3_100:
+		if (!use_frontend) {     /* terrestrial */
+			if (configure_tda827x_fe(dev, &asus_ps3_100_config,
+						 &tda827x_cfg_2) < 0)
+				goto dettach_frontend;
+	       } else {                /* satellite */
+			fe0->dvb.frontend = dvb_attach(tda10086_attach,
+						       &flydvbs, &dev->i2c_adap);
+			if (fe0->dvb.frontend) {
+				if (dvb_attach(tda826x_attach,
+					       fe0->dvb.frontend, 0x60,
+					       &dev->i2c_adap, 0) == NULL) {
+					wprintk("%s: Asus My Cinema PS3-100, no "
+						"tda826x found!\n", __func__);
+					goto dettach_frontend;
+				}
+				if (dvb_attach(lnbp21_attach, fe0->dvb.frontend,
+					       &dev->i2c_adap, 0, 0) == NULL) {
+					wprintk("%s: Asus My Cinema PS3-100, no lnbp21"
+						" found!\n", __func__);
 					goto dettach_frontend;
 				}
 			}
diff --git a/drivers/media/video/saa7134/saa7134-input.c b/drivers/media/video/saa7134/saa7134-input.c
index 48d2878699b..05c6e217d8a 100644
--- a/drivers/media/video/saa7134/saa7134-input.c
+++ b/drivers/media/video/saa7134/saa7134-input.c
@@ -753,6 +753,13 @@ int saa7134_input_init1(struct saa7134_dev *dev)
 		mask_keycode = 0xffff;
 		raw_decode   = true;
 		break;
+	case SAA7134_BOARD_ASUSTeK_PS3_100:
+		ir_codes     = RC_MAP_ASUS_PS3_100;
+		mask_keydown = 0x0040000;
+		mask_keyup   = 0x0040000;
+		mask_keycode = 0xffff;
+		raw_decode   = true;
+		break;
 	case SAA7134_BOARD_ENCORE_ENLTV:
 	case SAA7134_BOARD_ENCORE_ENLTV_FM:
 		ir_codes     = RC_MAP_ENCORE_ENLTV;
diff --git a/drivers/media/video/saa7134/saa7134-video.c b/drivers/media/video/saa7134/saa7134-video.c
index 417034eb6ad..6de10b1e725 100644
--- a/drivers/media/video/saa7134/saa7134-video.c
+++ b/drivers/media/video/saa7134/saa7134-video.c
@@ -2036,7 +2036,7 @@ static int saa7134_s_tuner(struct file *file, void *priv,
 	mode = dev->thread.mode;
 	if (UNSET == mode) {
 		rx   = saa7134_tvaudio_getstereo(dev);
-		mode = saa7134_tvaudio_rx2mode(t->rxsubchans);
+		mode = saa7134_tvaudio_rx2mode(rx);
 	}
 	if (mode != t->audmode)
 		dev->thread.mode = t->audmode;
diff --git a/drivers/media/video/saa7134/saa7134.h b/drivers/media/video/saa7134/saa7134.h
index f625060e6a0..89c8333736a 100644
--- a/drivers/media/video/saa7134/saa7134.h
+++ b/drivers/media/video/saa7134/saa7134.h
@@ -332,6 +332,7 @@ struct saa7134_card_ir {
 #define SAA7134_BOARD_BEHOLD_503FM          187
 #define SAA7134_BOARD_SENSORAY811_911       188
 #define SAA7134_BOARD_KWORLD_PC150U         189
+#define SAA7134_BOARD_ASUSTeK_PS3_100      190
 
 #define SAA7134_MAXBOARDS 32
 #define SAA7134_INPUT_MAX 8
diff --git a/drivers/media/video/saa7164/saa7164-vbi.c b/drivers/media/video/saa7164/saa7164-vbi.c
index 273cf807401..d8e6c8f1407 100644
--- a/drivers/media/video/saa7164/saa7164-vbi.c
+++ b/drivers/media/video/saa7164/saa7164-vbi.c
@@ -952,7 +952,7 @@ static int saa7164_vbi_start_streaming(struct saa7164_port *port)
 
 		/* Stop the hardware, regardless */
 		result = saa7164_vbi_stop_port(port);
-		if ((result != SAA_OK) && (result != SAA_ERR_ALREADY_STOPPED)) {
+		if (result != SAA_OK) {
 			printk(KERN_ERR "%s() pause/forced stop transition "
 				"failed, res = 0x%x\n", __func__, result);
 		}
@@ -971,7 +971,7 @@ static int saa7164_vbi_start_streaming(struct saa7164_port *port)
 		/* Stop the hardware, regardless */
 		result = saa7164_vbi_acquire_port(port);
 		result = saa7164_vbi_stop_port(port);
-		if ((result != SAA_OK) && (result != SAA_ERR_ALREADY_STOPPED)) {
+		if (result != SAA_OK) {
 			printk(KERN_ERR "%s() run/forced stop transition "
 				"failed, res = 0x%x\n", __func__, result);
 		}
diff --git a/drivers/media/video/saa7164/saa7164.h b/drivers/media/video/saa7164/saa7164.h
index 742b34103b5..8d120e3baf7 100644
--- a/drivers/media/video/saa7164/saa7164.h
+++ b/drivers/media/video/saa7164/saa7164.h
@@ -611,11 +611,6 @@ extern unsigned int saa_debug;
 		printk(KERN_WARNING "%s: " fmt, dev->name, ## arg);\
 	} while (0)
 
-#define log_err(fmt, arg...)\
-	do { \
-		printk(KERN_ERROR "%s: " fmt, dev->name, ## arg);\
-	} while (0)
-
 #define saa7164_readl(reg) readl(dev->lmmio + ((reg) >> 2))
 #define saa7164_writel(reg, value) writel((value), dev->lmmio + ((reg) >> 2))
 
diff --git a/drivers/media/video/sh_mobile_ceu_camera.c b/drivers/media/video/sh_mobile_ceu_camera.c
index 424dfacd263..0baaf94db7e 100644
--- a/drivers/media/video/sh_mobile_ceu_camera.c
+++ b/drivers/media/video/sh_mobile_ceu_camera.c
@@ -210,27 +210,33 @@ static int sh_mobile_ceu_videobuf_setup(struct vb2_queue *vq,
 	struct soc_camera_device *icd = container_of(vq, struct soc_camera_device, vb2_vidq);
 	struct soc_camera_host *ici = to_soc_camera_host(icd->parent);
 	struct sh_mobile_ceu_dev *pcdev = ici->priv;
-	int bytes_per_line;
-	unsigned int height;
 
 	if (fmt) {
 		const struct soc_camera_format_xlate *xlate = soc_camera_xlate_by_fourcc(icd,
 								fmt->fmt.pix.pixelformat);
+		unsigned int bytes_per_line;
+		int ret;
+
 		if (!xlate)
 			return -EINVAL;
-		bytes_per_line = soc_mbus_bytes_per_line(fmt->fmt.pix.width,
-							 xlate->host_fmt);
-		height = fmt->fmt.pix.height;
+
+		ret = soc_mbus_bytes_per_line(fmt->fmt.pix.width,
+					      xlate->host_fmt);
+		if (ret < 0)
+			return ret;
+
+		bytes_per_line = max_t(u32, fmt->fmt.pix.bytesperline, ret);
+
+		ret = soc_mbus_image_size(xlate->host_fmt, bytes_per_line,
+					  fmt->fmt.pix.height);
+		if (ret < 0)
+			return ret;
+
+		sizes[0] = max_t(u32, fmt->fmt.pix.sizeimage, ret);
 	} else {
 		/* Called from VIDIOC_REQBUFS or in compatibility mode */
-		bytes_per_line = soc_mbus_bytes_per_line(icd->user_width,
-						icd->current_fmt->host_fmt);
-		height = icd->user_height;
+		sizes[0] = icd->sizeimage;
 	}
-	if (bytes_per_line < 0)
-		return bytes_per_line;
-
-	sizes[0] = bytes_per_line * height;
 
 	alloc_ctxs[0] = pcdev->alloc_ctx;
 
@@ -336,21 +342,15 @@ static int sh_mobile_ceu_capture(struct sh_mobile_ceu_dev *pcdev)
 
 	ceu_write(pcdev, top1, phys_addr_top);
 	if (V4L2_FIELD_NONE != pcdev->field) {
-		if (planar)
-			phys_addr_bottom = phys_addr_top + icd->user_width;
-		else
-			phys_addr_bottom = phys_addr_top +
-				soc_mbus_bytes_per_line(icd->user_width,
-							icd->current_fmt->host_fmt);
+		phys_addr_bottom = phys_addr_top + icd->bytesperline;
 		ceu_write(pcdev, bottom1, phys_addr_bottom);
 	}
 
 	if (planar) {
-		phys_addr_top += icd->user_width *
-			icd->user_height;
+		phys_addr_top += icd->bytesperline * icd->user_height;
 		ceu_write(pcdev, top2, phys_addr_top);
 		if (V4L2_FIELD_NONE != pcdev->field) {
-			phys_addr_bottom = phys_addr_top + icd->user_width;
+			phys_addr_bottom = phys_addr_top + icd->bytesperline;
 			ceu_write(pcdev, bottom2, phys_addr_bottom);
 		}
 	}
@@ -377,13 +377,8 @@ static void sh_mobile_ceu_videobuf_queue(struct vb2_buffer *vb)
 	struct sh_mobile_ceu_dev *pcdev = ici->priv;
 	struct sh_mobile_ceu_buffer *buf = to_ceu_vb(vb);
 	unsigned long size;
-	int bytes_per_line = soc_mbus_bytes_per_line(icd->user_width,
-						icd->current_fmt->host_fmt);
 
-	if (bytes_per_line < 0)
-		goto error;
-
-	size = icd->user_height * bytes_per_line;
+	size = icd->sizeimage;
 
 	if (vb2_plane_size(vb, 0) < size) {
 		dev_err(icd->parent, "Buffer #%d too small (%lu < %lu)\n",
@@ -682,10 +677,7 @@ static void sh_mobile_ceu_set_rect(struct soc_camera_device *icd)
 			in_width *= 2;
 			left_offset *= 2;
 		}
-		cdwdr_width = width;
 	} else {
-		int bytes_per_line = soc_mbus_bytes_per_line(width,
-						icd->current_fmt->host_fmt);
 		unsigned int w_factor;
 
 		switch (icd->current_fmt->host_fmt->packing) {
@@ -698,13 +690,10 @@ static void sh_mobile_ceu_set_rect(struct soc_camera_device *icd)
 
 		in_width = cam->width * w_factor;
 		left_offset *= w_factor;
-
-		if (bytes_per_line < 0)
-			cdwdr_width = width;
-		else
-			cdwdr_width = bytes_per_line;
 	}
 
+	cdwdr_width = icd->bytesperline;
+
 	height = icd->user_height;
 	in_height = cam->height;
 	if (V4L2_FIELD_NONE != pcdev->field) {
@@ -881,11 +870,13 @@ static int sh_mobile_ceu_set_bus_param(struct soc_camera_device *icd)
 
 	value |= common_flags & V4L2_MBUS_VSYNC_ACTIVE_LOW ? 1 << 1 : 0;
 	value |= common_flags & V4L2_MBUS_HSYNC_ACTIVE_LOW ? 1 << 0 : 0;
-	value |= pcdev->is_16bit ? 1 << 12 : 0;
 
-	/* CSI2 mode */
-	if (pcdev->pdata->csi2)
+	if (pcdev->pdata->csi2) /* CSI2 mode */
 		value |= 3 << 12;
+	else if (pcdev->is_16bit)
+		value |= 1 << 12;
+	else if (pcdev->pdata->flags & SH_CEU_FLAG_LOWER_8BIT)
+		value |= 2 << 12;
 
 	ceu_write(pcdev, CAMCR, value);
 
@@ -964,24 +955,28 @@ static const struct soc_mbus_pixelfmt sh_mobile_ceu_formats[] = {
 		.bits_per_sample	= 8,
 		.packing		= SOC_MBUS_PACKING_1_5X8,
 		.order			= SOC_MBUS_ORDER_LE,
+		.layout			= SOC_MBUS_LAYOUT_PLANAR_2Y_C,
 	}, {
 		.fourcc			= V4L2_PIX_FMT_NV21,
 		.name			= "NV21",
 		.bits_per_sample	= 8,
 		.packing		= SOC_MBUS_PACKING_1_5X8,
 		.order			= SOC_MBUS_ORDER_LE,
+		.layout			= SOC_MBUS_LAYOUT_PLANAR_2Y_C,
 	}, {
 		.fourcc			= V4L2_PIX_FMT_NV16,
 		.name			= "NV16",
 		.bits_per_sample	= 8,
 		.packing		= SOC_MBUS_PACKING_2X8_PADHI,
 		.order			= SOC_MBUS_ORDER_LE,
+		.layout			= SOC_MBUS_LAYOUT_PLANAR_Y_C,
 	}, {
 		.fourcc			= V4L2_PIX_FMT_NV61,
 		.name			= "NV61",
 		.bits_per_sample	= 8,
 		.packing		= SOC_MBUS_PACKING_2X8_PADHI,
 		.order			= SOC_MBUS_ORDER_LE,
+		.layout			= SOC_MBUS_LAYOUT_PLANAR_Y_C,
 	},
 };
 
@@ -1845,6 +1840,8 @@ static int sh_mobile_ceu_set_fmt(struct soc_camera_device *icd,
 	return 0;
 }
 
+#define CEU_CHDW_MAX	8188U	/* Maximum line stride */
+
 static int sh_mobile_ceu_try_fmt(struct soc_camera_device *icd,
 				 struct v4l2_format *f)
 {
@@ -1863,8 +1860,12 @@ static int sh_mobile_ceu_try_fmt(struct soc_camera_device *icd,
 
 	xlate = soc_camera_xlate_by_fourcc(icd, pixfmt);
 	if (!xlate) {
-		dev_warn(icd->parent, "Format %x not found\n", pixfmt);
-		return -EINVAL;
+		xlate = icd->current_fmt;
+		dev_dbg(icd->parent, "Format %x not found, keeping %x\n",
+			pixfmt, xlate->host_fmt->fourcc);
+		pixfmt = xlate->host_fmt->fourcc;
+		pix->pixelformat = pixfmt;
+		pix->colorspace = icd->colorspace;
 	}
 
 	/* FIXME: calculate using depth and bus width */
@@ -1923,10 +1924,20 @@ static int sh_mobile_ceu_try_fmt(struct soc_camera_device *icd,
 			pix->width = width;
 		if (mf.height > height)
 			pix->height = height;
+
+		pix->bytesperline = max(pix->bytesperline, pix->width);
+		pix->bytesperline = min(pix->bytesperline, CEU_CHDW_MAX);
+		pix->bytesperline &= ~3;
+		break;
+
+	default:
+		/* Configurable stride isn't supported in pass-through mode. */
+		pix->bytesperline  = 0;
 	}
 
 	pix->width	&= ~3;
 	pix->height	&= ~3;
+	pix->sizeimage	= 0;
 
 	dev_geo(icd->parent, "%s(): return %d, fmt 0x%x, %ux%u\n",
 		__func__, ret, pix->pixelformat, pix->width, pix->height);
@@ -2145,6 +2156,7 @@ static int __devinit sh_mobile_ceu_probe(struct platform_device *pdev)
 	pcdev->ici.nr = pdev->id;
 	pcdev->ici.drv_name = dev_name(&pdev->dev);
 	pcdev->ici.ops = &sh_mobile_ceu_host_ops;
+	pcdev->ici.capabilities = SOCAM_HOST_CAP_STRIDE;
 
 	pcdev->alloc_ctx = vb2_dma_contig_init_ctx(&pdev->dev);
 	if (IS_ERR(pcdev->alloc_ctx)) {
diff --git a/drivers/media/video/sh_vou.c b/drivers/media/video/sh_vou.c
index 9644bd861ab..8fd1874382c 100644
--- a/drivers/media/video/sh_vou.c
+++ b/drivers/media/video/sh_vou.c
@@ -1390,6 +1390,10 @@ static int __devinit sh_vou_probe(struct platform_device *pdev)
 	vdev->v4l2_dev = &vou_dev->v4l2_dev;
 	vdev->release = video_device_release;
 	vdev->lock = &vou_dev->fop_lock;
+	/* Locking in file operations other than ioctl should be done
+	   by the driver, not the V4L2 core.
+	   This driver needs auditing so that this flag can be removed. */
+	set_bit(V4L2_FL_LOCK_ALL_FOPS, &vdev->flags);
 
 	vou_dev->vdev = vdev;
 	video_set_drvdata(vdev, vou_dev);
diff --git a/drivers/media/video/smiapp-pll.c b/drivers/media/video/smiapp-pll.c
new file mode 100644
index 00000000000..a2e41a21dc6
--- /dev/null
+++ b/drivers/media/video/smiapp-pll.c
@@ -0,0 +1,418 @@
+/*
+ * drivers/media/video/smiapp-pll.c
+ *
+ * Generic driver for SMIA/SMIA++ compliant camera modules
+ *
+ * Copyright (C) 2011--2012 Nokia Corporation
+ * Contact: Sakari Ailus <sakari.ailus@maxwell.research.nokia.com>
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * version 2 as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+ * General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA
+ * 02110-1301 USA
+ *
+ */
+
+#include <linux/gcd.h>
+#include <linux/lcm.h>
+#include <linux/module.h>
+
+#include "smiapp-pll.h"
+
+/* Return an even number or one. */
+static inline uint32_t clk_div_even(uint32_t a)
+{
+	return max_t(uint32_t, 1, a & ~1);
+}
+
+/* Return an even number or one. */
+static inline uint32_t clk_div_even_up(uint32_t a)
+{
+	if (a == 1)
+		return 1;
+	return (a + 1) & ~1;
+}
+
+static inline uint32_t is_one_or_even(uint32_t a)
+{
+	if (a == 1)
+		return 1;
+	if (a & 1)
+		return 0;
+
+	return 1;
+}
+
+static int bounds_check(struct device *dev, uint32_t val,
+			uint32_t min, uint32_t max, char *str)
+{
+	if (val >= min && val <= max)
+		return 0;
+
+	dev_warn(dev, "%s out of bounds: %d (%d--%d)\n", str, val, min, max);
+
+	return -EINVAL;
+}
+
+static void print_pll(struct device *dev, struct smiapp_pll *pll)
+{
+	dev_dbg(dev, "pre_pll_clk_div\t%d\n",  pll->pre_pll_clk_div);
+	dev_dbg(dev, "pll_multiplier \t%d\n",  pll->pll_multiplier);
+	if (pll->flags != SMIAPP_PLL_FLAG_NO_OP_CLOCKS) {
+		dev_dbg(dev, "op_sys_clk_div \t%d\n", pll->op_sys_clk_div);
+		dev_dbg(dev, "op_pix_clk_div \t%d\n", pll->op_pix_clk_div);
+	}
+	dev_dbg(dev, "vt_sys_clk_div \t%d\n",  pll->vt_sys_clk_div);
+	dev_dbg(dev, "vt_pix_clk_div \t%d\n",  pll->vt_pix_clk_div);
+
+	dev_dbg(dev, "ext_clk_freq_hz \t%d\n", pll->ext_clk_freq_hz);
+	dev_dbg(dev, "pll_ip_clk_freq_hz \t%d\n", pll->pll_ip_clk_freq_hz);
+	dev_dbg(dev, "pll_op_clk_freq_hz \t%d\n", pll->pll_op_clk_freq_hz);
+	if (pll->flags & SMIAPP_PLL_FLAG_NO_OP_CLOCKS) {
+		dev_dbg(dev, "op_sys_clk_freq_hz \t%d\n",
+			pll->op_sys_clk_freq_hz);
+		dev_dbg(dev, "op_pix_clk_freq_hz \t%d\n",
+			pll->op_pix_clk_freq_hz);
+	}
+	dev_dbg(dev, "vt_sys_clk_freq_hz \t%d\n", pll->vt_sys_clk_freq_hz);
+	dev_dbg(dev, "vt_pix_clk_freq_hz \t%d\n", pll->vt_pix_clk_freq_hz);
+}
+
+int smiapp_pll_calculate(struct device *dev, struct smiapp_pll_limits *limits,
+			 struct smiapp_pll *pll)
+{
+	uint32_t sys_div;
+	uint32_t best_pix_div = INT_MAX >> 1;
+	uint32_t vt_op_binning_div;
+	uint32_t lane_op_clock_ratio;
+	uint32_t mul, div;
+	uint32_t more_mul_min, more_mul_max;
+	uint32_t more_mul_factor;
+	uint32_t min_vt_div, max_vt_div, vt_div;
+	uint32_t min_sys_div, max_sys_div;
+	unsigned int i;
+	int rval;
+
+	if (pll->flags & SMIAPP_PLL_FLAG_OP_PIX_CLOCK_PER_LANE)
+		lane_op_clock_ratio = pll->lanes;
+	else
+		lane_op_clock_ratio = 1;
+	dev_dbg(dev, "lane_op_clock_ratio: %d\n", lane_op_clock_ratio);
+
+	dev_dbg(dev, "binning: %dx%d\n", pll->binning_horizontal,
+		pll->binning_vertical);
+
+	/* CSI transfers 2 bits per clock per lane; thus times 2 */
+	pll->pll_op_clk_freq_hz = pll->link_freq * 2
+		* (pll->lanes / lane_op_clock_ratio);
+
+	/* Figure out limits for pre-pll divider based on extclk */
+	dev_dbg(dev, "min / max pre_pll_clk_div: %d / %d\n",
+		limits->min_pre_pll_clk_div, limits->max_pre_pll_clk_div);
+	limits->max_pre_pll_clk_div =
+		min_t(uint16_t, limits->max_pre_pll_clk_div,
+		      clk_div_even(pll->ext_clk_freq_hz /
+				   limits->min_pll_ip_freq_hz));
+	limits->min_pre_pll_clk_div =
+		max_t(uint16_t, limits->min_pre_pll_clk_div,
+		      clk_div_even_up(
+			      DIV_ROUND_UP(pll->ext_clk_freq_hz,
+					   limits->max_pll_ip_freq_hz)));
+	dev_dbg(dev, "pre-pll check: min / max pre_pll_clk_div: %d / %d\n",
+		limits->min_pre_pll_clk_div, limits->max_pre_pll_clk_div);
+
+	i = gcd(pll->pll_op_clk_freq_hz, pll->ext_clk_freq_hz);
+	mul = div_u64(pll->pll_op_clk_freq_hz, i);
+	div = pll->ext_clk_freq_hz / i;
+	dev_dbg(dev, "mul %d / div %d\n", mul, div);
+
+	limits->min_pre_pll_clk_div =
+		max_t(uint16_t, limits->min_pre_pll_clk_div,
+		      clk_div_even_up(
+			      DIV_ROUND_UP(mul * pll->ext_clk_freq_hz,
+					   limits->max_pll_op_freq_hz)));
+	dev_dbg(dev, "pll_op check: min / max pre_pll_clk_div: %d / %d\n",
+		limits->min_pre_pll_clk_div, limits->max_pre_pll_clk_div);
+
+	if (limits->min_pre_pll_clk_div > limits->max_pre_pll_clk_div) {
+		dev_err(dev, "unable to compute pre_pll divisor\n");
+		return -EINVAL;
+	}
+
+	pll->pre_pll_clk_div = limits->min_pre_pll_clk_div;
+
+	/*
+	 * Get pre_pll_clk_div so that our pll_op_clk_freq_hz won't be
+	 * too high.
+	 */
+	dev_dbg(dev, "pre_pll_clk_div %d\n", pll->pre_pll_clk_div);
+
+	/* Don't go above max pll multiplier. */
+	more_mul_max = limits->max_pll_multiplier / mul;
+	dev_dbg(dev, "more_mul_max: max_pll_multiplier check: %d\n",
+		more_mul_max);
+	/* Don't go above max pll op frequency. */
+	more_mul_max =
+		min_t(int,
+		      more_mul_max,
+		      limits->max_pll_op_freq_hz
+		      / (pll->ext_clk_freq_hz / pll->pre_pll_clk_div * mul));
+	dev_dbg(dev, "more_mul_max: max_pll_op_freq_hz check: %d\n",
+		more_mul_max);
+	/* Don't go above the division capability of op sys clock divider. */
+	more_mul_max = min(more_mul_max,
+			   limits->max_op_sys_clk_div * pll->pre_pll_clk_div
+			   / div);
+	dev_dbg(dev, "more_mul_max: max_op_sys_clk_div check: %d\n",
+		more_mul_max);
+	/* Ensure we won't go above min_pll_multiplier. */
+	more_mul_max = min(more_mul_max,
+			   DIV_ROUND_UP(limits->max_pll_multiplier, mul));
+	dev_dbg(dev, "more_mul_max: min_pll_multiplier check: %d\n",
+		more_mul_max);
+
+	/* Ensure we won't go below min_pll_op_freq_hz. */
+	more_mul_min = DIV_ROUND_UP(limits->min_pll_op_freq_hz,
+				    pll->ext_clk_freq_hz / pll->pre_pll_clk_div
+				    * mul);
+	dev_dbg(dev, "more_mul_min: min_pll_op_freq_hz check: %d\n",
+		more_mul_min);
+	/* Ensure we won't go below min_pll_multiplier. */
+	more_mul_min = max(more_mul_min,
+			   DIV_ROUND_UP(limits->min_pll_multiplier, mul));
+	dev_dbg(dev, "more_mul_min: min_pll_multiplier check: %d\n",
+		more_mul_min);
+
+	if (more_mul_min > more_mul_max) {
+		dev_warn(dev,
+			 "unable to compute more_mul_min and more_mul_max");
+		return -EINVAL;
+	}
+
+	more_mul_factor = lcm(div, pll->pre_pll_clk_div) / div;
+	dev_dbg(dev, "more_mul_factor: %d\n", more_mul_factor);
+	more_mul_factor = lcm(more_mul_factor, limits->min_op_sys_clk_div);
+	dev_dbg(dev, "more_mul_factor: min_op_sys_clk_div: %d\n",
+		more_mul_factor);
+	i = roundup(more_mul_min, more_mul_factor);
+	if (!is_one_or_even(i))
+		i <<= 1;
+
+	dev_dbg(dev, "final more_mul: %d\n", i);
+	if (i > more_mul_max) {
+		dev_warn(dev, "final more_mul is bad, max %d", more_mul_max);
+		return -EINVAL;
+	}
+
+	pll->pll_multiplier = mul * i;
+	pll->op_sys_clk_div = div * i / pll->pre_pll_clk_div;
+	dev_dbg(dev, "op_sys_clk_div: %d\n", pll->op_sys_clk_div);
+
+	pll->pll_ip_clk_freq_hz = pll->ext_clk_freq_hz
+		/ pll->pre_pll_clk_div;
+
+	pll->pll_op_clk_freq_hz = pll->pll_ip_clk_freq_hz
+		* pll->pll_multiplier;
+
+	/* Derive pll_op_clk_freq_hz. */
+	pll->op_sys_clk_freq_hz =
+		pll->pll_op_clk_freq_hz / pll->op_sys_clk_div;
+
+	pll->op_pix_clk_div = pll->bits_per_pixel;
+	dev_dbg(dev, "op_pix_clk_div: %d\n", pll->op_pix_clk_div);
+
+	pll->op_pix_clk_freq_hz =
+		pll->op_sys_clk_freq_hz / pll->op_pix_clk_div;
+
+	/*
+	 * Some sensors perform analogue binning and some do this
+	 * digitally. The ones doing this digitally can be roughly be
+	 * found out using this formula. The ones doing this digitally
+	 * should run at higher clock rate, so smaller divisor is used
+	 * on video timing side.
+	 */
+	if (limits->min_line_length_pck_bin > limits->min_line_length_pck
+	    / pll->binning_horizontal)
+		vt_op_binning_div = pll->binning_horizontal;
+	else
+		vt_op_binning_div = 1;
+	dev_dbg(dev, "vt_op_binning_div: %d\n", vt_op_binning_div);
+
+	/*
+	 * Profile 2 supports vt_pix_clk_div E [4, 10]
+	 *
+	 * Horizontal binning can be used as a base for difference in
+	 * divisors. One must make sure that horizontal blanking is
+	 * enough to accommodate the CSI-2 sync codes.
+	 *
+	 * Take scaling factor into account as well.
+	 *
+	 * Find absolute limits for the factor of vt divider.
+	 */
+	dev_dbg(dev, "scale_m: %d\n", pll->scale_m);
+	min_vt_div = DIV_ROUND_UP(pll->op_pix_clk_div * pll->op_sys_clk_div
+				  * pll->scale_n,
+				  lane_op_clock_ratio * vt_op_binning_div
+				  * pll->scale_m);
+
+	/* Find smallest and biggest allowed vt divisor. */
+	dev_dbg(dev, "min_vt_div: %d\n", min_vt_div);
+	min_vt_div = max(min_vt_div,
+			 DIV_ROUND_UP(pll->pll_op_clk_freq_hz,
+				      limits->max_vt_pix_clk_freq_hz));
+	dev_dbg(dev, "min_vt_div: max_vt_pix_clk_freq_hz: %d\n",
+		min_vt_div);
+	min_vt_div = max_t(uint32_t, min_vt_div,
+			   limits->min_vt_pix_clk_div
+			   * limits->min_vt_sys_clk_div);
+	dev_dbg(dev, "min_vt_div: min_vt_clk_div: %d\n", min_vt_div);
+
+	max_vt_div = limits->max_vt_sys_clk_div * limits->max_vt_pix_clk_div;
+	dev_dbg(dev, "max_vt_div: %d\n", max_vt_div);
+	max_vt_div = min(max_vt_div,
+			 DIV_ROUND_UP(pll->pll_op_clk_freq_hz,
+				      limits->min_vt_pix_clk_freq_hz));
+	dev_dbg(dev, "max_vt_div: min_vt_pix_clk_freq_hz: %d\n",
+		max_vt_div);
+
+	/*
+	 * Find limitsits for sys_clk_div. Not all values are possible
+	 * with all values of pix_clk_div.
+	 */
+	min_sys_div = limits->min_vt_sys_clk_div;
+	dev_dbg(dev, "min_sys_div: %d\n", min_sys_div);
+	min_sys_div = max(min_sys_div,
+			  DIV_ROUND_UP(min_vt_div,
+				       limits->max_vt_pix_clk_div));
+	dev_dbg(dev, "min_sys_div: max_vt_pix_clk_div: %d\n", min_sys_div);
+	min_sys_div = max(min_sys_div,
+			  pll->pll_op_clk_freq_hz
+			  / limits->max_vt_sys_clk_freq_hz);
+	dev_dbg(dev, "min_sys_div: max_pll_op_clk_freq_hz: %d\n", min_sys_div);
+	min_sys_div = clk_div_even_up(min_sys_div);
+	dev_dbg(dev, "min_sys_div: one or even: %d\n", min_sys_div);
+
+	max_sys_div = limits->max_vt_sys_clk_div;
+	dev_dbg(dev, "max_sys_div: %d\n", max_sys_div);
+	max_sys_div = min(max_sys_div,
+			  DIV_ROUND_UP(max_vt_div,
+				       limits->min_vt_pix_clk_div));
+	dev_dbg(dev, "max_sys_div: min_vt_pix_clk_div: %d\n", max_sys_div);
+	max_sys_div = min(max_sys_div,
+			  DIV_ROUND_UP(pll->pll_op_clk_freq_hz,
+				       limits->min_vt_pix_clk_freq_hz));
+	dev_dbg(dev, "max_sys_div: min_vt_pix_clk_freq_hz: %d\n", max_sys_div);
+
+	/*
+	 * Find pix_div such that a legal pix_div * sys_div results
+	 * into a value which is not smaller than div, the desired
+	 * divisor.
+	 */
+	for (vt_div = min_vt_div; vt_div <= max_vt_div;
+	     vt_div += 2 - (vt_div & 1)) {
+		for (sys_div = min_sys_div;
+		     sys_div <= max_sys_div;
+		     sys_div += 2 - (sys_div & 1)) {
+			int pix_div = DIV_ROUND_UP(vt_div, sys_div);
+
+			if (pix_div < limits->min_vt_pix_clk_div
+			    || pix_div > limits->max_vt_pix_clk_div) {
+				dev_dbg(dev,
+					"pix_div %d too small or too big (%d--%d)\n",
+					pix_div,
+					limits->min_vt_pix_clk_div,
+					limits->max_vt_pix_clk_div);
+				continue;
+			}
+
+			/* Check if this one is better. */
+			if (pix_div * sys_div
+			    <= roundup(min_vt_div, best_pix_div))
+				best_pix_div = pix_div;
+		}
+		if (best_pix_div < INT_MAX >> 1)
+			break;
+	}
+
+	pll->vt_sys_clk_div = DIV_ROUND_UP(min_vt_div, best_pix_div);
+	pll->vt_pix_clk_div = best_pix_div;
+
+	pll->vt_sys_clk_freq_hz =
+		pll->pll_op_clk_freq_hz / pll->vt_sys_clk_div;
+	pll->vt_pix_clk_freq_hz =
+		pll->vt_sys_clk_freq_hz / pll->vt_pix_clk_div;
+
+	pll->pixel_rate_csi =
+		pll->op_pix_clk_freq_hz * lane_op_clock_ratio;
+
+	print_pll(dev, pll);
+
+	rval = bounds_check(dev, pll->pre_pll_clk_div,
+			    limits->min_pre_pll_clk_div,
+			    limits->max_pre_pll_clk_div, "pre_pll_clk_div");
+	if (!rval)
+		rval = bounds_check(
+			dev, pll->pll_ip_clk_freq_hz,
+			limits->min_pll_ip_freq_hz, limits->max_pll_ip_freq_hz,
+			"pll_ip_clk_freq_hz");
+	if (!rval)
+		rval = bounds_check(
+			dev, pll->pll_multiplier,
+			limits->min_pll_multiplier, limits->max_pll_multiplier,
+			"pll_multiplier");
+	if (!rval)
+		rval = bounds_check(
+			dev, pll->pll_op_clk_freq_hz,
+			limits->min_pll_op_freq_hz, limits->max_pll_op_freq_hz,
+			"pll_op_clk_freq_hz");
+	if (!rval)
+		rval = bounds_check(
+			dev, pll->op_sys_clk_div,
+			limits->min_op_sys_clk_div, limits->max_op_sys_clk_div,
+			"op_sys_clk_div");
+	if (!rval)
+		rval = bounds_check(
+			dev, pll->op_pix_clk_div,
+			limits->min_op_pix_clk_div, limits->max_op_pix_clk_div,
+			"op_pix_clk_div");
+	if (!rval)
+		rval = bounds_check(
+			dev, pll->op_sys_clk_freq_hz,
+			limits->min_op_sys_clk_freq_hz,
+			limits->max_op_sys_clk_freq_hz,
+			"op_sys_clk_freq_hz");
+	if (!rval)
+		rval = bounds_check(
+			dev, pll->op_pix_clk_freq_hz,
+			limits->min_op_pix_clk_freq_hz,
+			limits->max_op_pix_clk_freq_hz,
+			"op_pix_clk_freq_hz");
+	if (!rval)
+		rval = bounds_check(
+			dev, pll->vt_sys_clk_freq_hz,
+			limits->min_vt_sys_clk_freq_hz,
+			limits->max_vt_sys_clk_freq_hz,
+			"vt_sys_clk_freq_hz");
+	if (!rval)
+		rval = bounds_check(
+			dev, pll->vt_pix_clk_freq_hz,
+			limits->min_vt_pix_clk_freq_hz,
+			limits->max_vt_pix_clk_freq_hz,
+			"vt_pix_clk_freq_hz");
+
+	return rval;
+}
+EXPORT_SYMBOL_GPL(smiapp_pll_calculate);
+
+MODULE_AUTHOR("Sakari Ailus <sakari.ailus@maxwell.research.nokia.com>");
+MODULE_DESCRIPTION("Generic SMIA/SMIA++ PLL calculator");
+MODULE_LICENSE("GPL");
diff --git a/drivers/media/video/smiapp-pll.h b/drivers/media/video/smiapp-pll.h
new file mode 100644
index 00000000000..9eab63f23af
--- /dev/null
+++ b/drivers/media/video/smiapp-pll.h
@@ -0,0 +1,103 @@
+/*
+ * drivers/media/video/smiapp-pll.h
+ *
+ * Generic driver for SMIA/SMIA++ compliant camera modules
+ *
+ * Copyright (C) 2012 Nokia Corporation
+ * Contact: Sakari Ailus <sakari.ailus@maxwell.research.nokia.com>
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * version 2 as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+ * General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA
+ * 02110-1301 USA
+ *
+ */
+
+#ifndef SMIAPP_PLL_H
+#define SMIAPP_PLL_H
+
+#include <linux/device.h>
+
+struct smiapp_pll {
+	uint8_t lanes;
+	uint8_t binning_horizontal;
+	uint8_t binning_vertical;
+	uint8_t scale_m;
+	uint8_t scale_n;
+	uint8_t bits_per_pixel;
+	uint16_t flags;
+	uint32_t link_freq;
+
+	uint16_t pre_pll_clk_div;
+	uint16_t pll_multiplier;
+	uint16_t op_sys_clk_div;
+	uint16_t op_pix_clk_div;
+	uint16_t vt_sys_clk_div;
+	uint16_t vt_pix_clk_div;
+
+	uint32_t ext_clk_freq_hz;
+	uint32_t pll_ip_clk_freq_hz;
+	uint32_t pll_op_clk_freq_hz;
+	uint32_t op_sys_clk_freq_hz;
+	uint32_t op_pix_clk_freq_hz;
+	uint32_t vt_sys_clk_freq_hz;
+	uint32_t vt_pix_clk_freq_hz;
+
+	uint32_t pixel_rate_csi;
+};
+
+struct smiapp_pll_limits {
+	/* Strict PLL limits */
+	uint32_t min_ext_clk_freq_hz;
+	uint32_t max_ext_clk_freq_hz;
+	uint16_t min_pre_pll_clk_div;
+	uint16_t max_pre_pll_clk_div;
+	uint32_t min_pll_ip_freq_hz;
+	uint32_t max_pll_ip_freq_hz;
+	uint16_t min_pll_multiplier;
+	uint16_t max_pll_multiplier;
+	uint32_t min_pll_op_freq_hz;
+	uint32_t max_pll_op_freq_hz;
+
+	uint16_t min_vt_sys_clk_div;
+	uint16_t max_vt_sys_clk_div;
+	uint32_t min_vt_sys_clk_freq_hz;
+	uint32_t max_vt_sys_clk_freq_hz;
+	uint16_t min_vt_pix_clk_div;
+	uint16_t max_vt_pix_clk_div;
+	uint32_t min_vt_pix_clk_freq_hz;
+	uint32_t max_vt_pix_clk_freq_hz;
+
+	uint16_t min_op_sys_clk_div;
+	uint16_t max_op_sys_clk_div;
+	uint32_t min_op_sys_clk_freq_hz;
+	uint32_t max_op_sys_clk_freq_hz;
+	uint16_t min_op_pix_clk_div;
+	uint16_t max_op_pix_clk_div;
+	uint32_t min_op_pix_clk_freq_hz;
+	uint32_t max_op_pix_clk_freq_hz;
+
+	/* Other relevant limits */
+	uint32_t min_line_length_pck_bin;
+	uint32_t min_line_length_pck;
+};
+
+/* op pix clock is for all lanes in total normally */
+#define SMIAPP_PLL_FLAG_OP_PIX_CLOCK_PER_LANE			(1 << 0)
+#define SMIAPP_PLL_FLAG_NO_OP_CLOCKS				(1 << 1)
+
+struct device;
+
+int smiapp_pll_calculate(struct device *dev, struct smiapp_pll_limits *limits,
+			 struct smiapp_pll *pll);
+
+#endif /* SMIAPP_PLL_H */
diff --git a/drivers/media/video/smiapp/Kconfig b/drivers/media/video/smiapp/Kconfig
new file mode 100644
index 00000000000..f7b35ff443b
--- /dev/null
+++ b/drivers/media/video/smiapp/Kconfig
@@ -0,0 +1,6 @@
+config VIDEO_SMIAPP
+	tristate "SMIA++/SMIA sensor support"
+	depends on I2C && VIDEO_V4L2 && VIDEO_V4L2_SUBDEV_API
+	select VIDEO_SMIAPP_PLL
+	---help---
+	  This is a generic driver for SMIA++/SMIA camera modules.
diff --git a/drivers/media/video/smiapp/Makefile b/drivers/media/video/smiapp/Makefile
new file mode 100644
index 00000000000..36b0cfa2c54
--- /dev/null
+++ b/drivers/media/video/smiapp/Makefile
@@ -0,0 +1,5 @@
+smiapp-objs			+= smiapp-core.o smiapp-regs.o \
+				   smiapp-quirk.o smiapp-limits.o
+obj-$(CONFIG_VIDEO_SMIAPP)	+= smiapp.o
+
+ccflags-y += -Idrivers/media/video
diff --git a/drivers/media/video/smiapp/smiapp-core.c b/drivers/media/video/smiapp/smiapp-core.c
new file mode 100644
index 00000000000..f518026cb67
--- /dev/null
+++ b/drivers/media/video/smiapp/smiapp-core.c
@@ -0,0 +1,2894 @@
+/*
+ * drivers/media/video/smiapp/smiapp-core.c
+ *
+ * Generic driver for SMIA/SMIA++ compliant camera modules
+ *
+ * Copyright (C) 2010--2012 Nokia Corporation
+ * Contact: Sakari Ailus <sakari.ailus@maxwell.research.nokia.com>
+ *
+ * Based on smiapp driver by Vimarsh Zutshi
+ * Based on jt8ev1.c by Vimarsh Zutshi
+ * Based on smia-sensor.c by Tuukka Toivonen <tuukkat76@gmail.com>
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * version 2 as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+ * General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA
+ * 02110-1301 USA
+ *
+ */
+
+#include <linux/clk.h>
+#include <linux/delay.h>
+#include <linux/device.h>
+#include <linux/gpio.h>
+#include <linux/module.h>
+#include <linux/regulator/consumer.h>
+#include <linux/v4l2-mediabus.h>
+#include <media/v4l2-device.h>
+
+#include "smiapp.h"
+
+#define SMIAPP_ALIGN_DIM(dim, flags)		\
+	((flags) & V4L2_SUBDEV_SEL_FLAG_SIZE_GE	\
+	 ? ALIGN((dim), 2)			\
+	 : (dim) & ~1)
+
+/*
+ * smiapp_module_idents - supported camera modules
+ */
+static const struct smiapp_module_ident smiapp_module_idents[] = {
+	SMIAPP_IDENT_L(0x01, 0x022b, -1, "vs6555"),
+	SMIAPP_IDENT_L(0x01, 0x022e, -1, "vw6558"),
+	SMIAPP_IDENT_L(0x07, 0x7698, -1, "ovm7698"),
+	SMIAPP_IDENT_L(0x0b, 0x4242, -1, "smiapp-003"),
+	SMIAPP_IDENT_L(0x0c, 0x208a, -1, "tcm8330md"),
+	SMIAPP_IDENT_LQ(0x0c, 0x2134, -1, "tcm8500md", &smiapp_tcm8500md_quirk),
+	SMIAPP_IDENT_L(0x0c, 0x213e, -1, "et8en2"),
+	SMIAPP_IDENT_L(0x0c, 0x2184, -1, "tcm8580md"),
+	SMIAPP_IDENT_LQ(0x0c, 0x560f, -1, "jt8ew9", &smiapp_jt8ew9_quirk),
+	SMIAPP_IDENT_LQ(0x10, 0x4141, -1, "jt8ev1", &smiapp_jt8ev1_quirk),
+	SMIAPP_IDENT_LQ(0x10, 0x4241, -1, "imx125es", &smiapp_imx125es_quirk),
+};
+
+/*
+ *
+ * Dynamic Capability Identification
+ *
+ */
+
+static int smiapp_read_frame_fmt(struct smiapp_sensor *sensor)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(&sensor->src->sd);
+	u32 fmt_model_type, fmt_model_subtype, ncol_desc, nrow_desc;
+	unsigned int i;
+	int rval;
+	int line_count = 0;
+	int embedded_start = -1, embedded_end = -1;
+	int image_start = 0;
+
+	rval = smiapp_read(sensor, SMIAPP_REG_U8_FRAME_FORMAT_MODEL_TYPE,
+			   &fmt_model_type);
+	if (rval)
+		return rval;
+
+	rval = smiapp_read(sensor, SMIAPP_REG_U8_FRAME_FORMAT_MODEL_SUBTYPE,
+			   &fmt_model_subtype);
+	if (rval)
+		return rval;
+
+	ncol_desc = (fmt_model_subtype
+		     & SMIAPP_FRAME_FORMAT_MODEL_SUBTYPE_NCOLS_MASK)
+		>> SMIAPP_FRAME_FORMAT_MODEL_SUBTYPE_NCOLS_SHIFT;
+	nrow_desc = fmt_model_subtype
+		& SMIAPP_FRAME_FORMAT_MODEL_SUBTYPE_NROWS_MASK;
+
+	dev_dbg(&client->dev, "format_model_type %s\n",
+		fmt_model_type == SMIAPP_FRAME_FORMAT_MODEL_TYPE_2BYTE
+		? "2 byte" :
+		fmt_model_type == SMIAPP_FRAME_FORMAT_MODEL_TYPE_4BYTE
+		? "4 byte" : "is simply bad");
+
+	for (i = 0; i < ncol_desc + nrow_desc; i++) {
+		u32 desc;
+		u32 pixelcode;
+		u32 pixels;
+		char *which;
+		char *what;
+
+		if (fmt_model_type == SMIAPP_FRAME_FORMAT_MODEL_TYPE_2BYTE) {
+			rval = smiapp_read(
+				sensor,
+				SMIAPP_REG_U16_FRAME_FORMAT_DESCRIPTOR_2(i),
+				&desc);
+			if (rval)
+				return rval;
+
+			pixelcode =
+				(desc
+				 & SMIAPP_FRAME_FORMAT_DESC_2_PIXELCODE_MASK)
+				>> SMIAPP_FRAME_FORMAT_DESC_2_PIXELCODE_SHIFT;
+			pixels = desc & SMIAPP_FRAME_FORMAT_DESC_2_PIXELS_MASK;
+		} else if (fmt_model_type
+			   == SMIAPP_FRAME_FORMAT_MODEL_TYPE_4BYTE) {
+			rval = smiapp_read(
+				sensor,
+				SMIAPP_REG_U32_FRAME_FORMAT_DESCRIPTOR_4(i),
+				&desc);
+			if (rval)
+				return rval;
+
+			pixelcode =
+				(desc
+				 & SMIAPP_FRAME_FORMAT_DESC_4_PIXELCODE_MASK)
+				>> SMIAPP_FRAME_FORMAT_DESC_4_PIXELCODE_SHIFT;
+			pixels = desc & SMIAPP_FRAME_FORMAT_DESC_4_PIXELS_MASK;
+		} else {
+			dev_dbg(&client->dev,
+				"invalid frame format model type %d\n",
+				fmt_model_type);
+			return -EINVAL;
+		}
+
+		if (i < ncol_desc)
+			which = "columns";
+		else
+			which = "rows";
+
+		switch (pixelcode) {
+		case SMIAPP_FRAME_FORMAT_DESC_PIXELCODE_EMBEDDED:
+			what = "embedded";
+			break;
+		case SMIAPP_FRAME_FORMAT_DESC_PIXELCODE_DUMMY:
+			what = "dummy";
+			break;
+		case SMIAPP_FRAME_FORMAT_DESC_PIXELCODE_BLACK:
+			what = "black";
+			break;
+		case SMIAPP_FRAME_FORMAT_DESC_PIXELCODE_DARK:
+			what = "dark";
+			break;
+		case SMIAPP_FRAME_FORMAT_DESC_PIXELCODE_VISIBLE:
+			what = "visible";
+			break;
+		default:
+			what = "invalid";
+			dev_dbg(&client->dev, "pixelcode %d\n", pixelcode);
+			break;
+		}
+
+		dev_dbg(&client->dev, "%s pixels: %d %s\n",
+			what, pixels, which);
+
+		if (i < ncol_desc)
+			continue;
+
+		/* Handle row descriptors */
+		if (pixelcode
+		    == SMIAPP_FRAME_FORMAT_DESC_PIXELCODE_EMBEDDED) {
+			embedded_start = line_count;
+		} else {
+			if (pixelcode == SMIAPP_FRAME_FORMAT_DESC_PIXELCODE_VISIBLE
+			    || pixels >= sensor->limits[SMIAPP_LIMIT_MIN_FRAME_LENGTH_LINES] / 2)
+				image_start = line_count;
+			if (embedded_start != -1 && embedded_end == -1)
+				embedded_end = line_count;
+		}
+		line_count += pixels;
+	}
+
+	if (embedded_start == -1 || embedded_end == -1) {
+		embedded_start = 0;
+		embedded_end = 0;
+	}
+
+	dev_dbg(&client->dev, "embedded data from lines %d to %d\n",
+		embedded_start, embedded_end);
+	dev_dbg(&client->dev, "image data starts at line %d\n", image_start);
+
+	return 0;
+}
+
+static int smiapp_pll_configure(struct smiapp_sensor *sensor)
+{
+	struct smiapp_pll *pll = &sensor->pll;
+	int rval;
+
+	rval = smiapp_write(
+		sensor, SMIAPP_REG_U16_VT_PIX_CLK_DIV, pll->vt_pix_clk_div);
+	if (rval < 0)
+		return rval;
+
+	rval = smiapp_write(
+		sensor, SMIAPP_REG_U16_VT_SYS_CLK_DIV, pll->vt_sys_clk_div);
+	if (rval < 0)
+		return rval;
+
+	rval = smiapp_write(
+		sensor, SMIAPP_REG_U16_PRE_PLL_CLK_DIV, pll->pre_pll_clk_div);
+	if (rval < 0)
+		return rval;
+
+	rval = smiapp_write(
+		sensor, SMIAPP_REG_U16_PLL_MULTIPLIER, pll->pll_multiplier);
+	if (rval < 0)
+		return rval;
+
+	/* Lane op clock ratio does not apply here. */
+	rval = smiapp_write(
+		sensor, SMIAPP_REG_U32_REQUESTED_LINK_BIT_RATE_MBPS,
+		DIV_ROUND_UP(pll->op_sys_clk_freq_hz, 1000000 / 256 / 256));
+	if (rval < 0 || sensor->minfo.smiapp_profile == SMIAPP_PROFILE_0)
+		return rval;
+
+	rval = smiapp_write(
+		sensor, SMIAPP_REG_U16_OP_PIX_CLK_DIV, pll->op_pix_clk_div);
+	if (rval < 0)
+		return rval;
+
+	return smiapp_write(
+		sensor, SMIAPP_REG_U16_OP_SYS_CLK_DIV, pll->op_sys_clk_div);
+}
+
+static int smiapp_pll_update(struct smiapp_sensor *sensor)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(&sensor->src->sd);
+	struct smiapp_pll_limits lim = {
+		.min_pre_pll_clk_div = sensor->limits[SMIAPP_LIMIT_MIN_PRE_PLL_CLK_DIV],
+		.max_pre_pll_clk_div = sensor->limits[SMIAPP_LIMIT_MAX_PRE_PLL_CLK_DIV],
+		.min_pll_ip_freq_hz = sensor->limits[SMIAPP_LIMIT_MIN_PLL_IP_FREQ_HZ],
+		.max_pll_ip_freq_hz = sensor->limits[SMIAPP_LIMIT_MAX_PLL_IP_FREQ_HZ],
+		.min_pll_multiplier = sensor->limits[SMIAPP_LIMIT_MIN_PLL_MULTIPLIER],
+		.max_pll_multiplier = sensor->limits[SMIAPP_LIMIT_MAX_PLL_MULTIPLIER],
+		.min_pll_op_freq_hz = sensor->limits[SMIAPP_LIMIT_MIN_PLL_OP_FREQ_HZ],
+		.max_pll_op_freq_hz = sensor->limits[SMIAPP_LIMIT_MAX_PLL_OP_FREQ_HZ],
+
+		.min_op_sys_clk_div = sensor->limits[SMIAPP_LIMIT_MIN_OP_SYS_CLK_DIV],
+		.max_op_sys_clk_div = sensor->limits[SMIAPP_LIMIT_MAX_OP_SYS_CLK_DIV],
+		.min_op_pix_clk_div = sensor->limits[SMIAPP_LIMIT_MIN_OP_PIX_CLK_DIV],
+		.max_op_pix_clk_div = sensor->limits[SMIAPP_LIMIT_MAX_OP_PIX_CLK_DIV],
+		.min_op_sys_clk_freq_hz = sensor->limits[SMIAPP_LIMIT_MIN_OP_SYS_CLK_FREQ_HZ],
+		.max_op_sys_clk_freq_hz = sensor->limits[SMIAPP_LIMIT_MAX_OP_SYS_CLK_FREQ_HZ],
+		.min_op_pix_clk_freq_hz = sensor->limits[SMIAPP_LIMIT_MIN_OP_PIX_CLK_FREQ_HZ],
+		.max_op_pix_clk_freq_hz = sensor->limits[SMIAPP_LIMIT_MAX_OP_PIX_CLK_FREQ_HZ],
+
+		.min_vt_sys_clk_div = sensor->limits[SMIAPP_LIMIT_MIN_VT_SYS_CLK_DIV],
+		.max_vt_sys_clk_div = sensor->limits[SMIAPP_LIMIT_MAX_VT_SYS_CLK_DIV],
+		.min_vt_pix_clk_div = sensor->limits[SMIAPP_LIMIT_MIN_VT_PIX_CLK_DIV],
+		.max_vt_pix_clk_div = sensor->limits[SMIAPP_LIMIT_MAX_VT_PIX_CLK_DIV],
+		.min_vt_sys_clk_freq_hz = sensor->limits[SMIAPP_LIMIT_MIN_VT_SYS_CLK_FREQ_HZ],
+		.max_vt_sys_clk_freq_hz = sensor->limits[SMIAPP_LIMIT_MAX_VT_SYS_CLK_FREQ_HZ],
+		.min_vt_pix_clk_freq_hz = sensor->limits[SMIAPP_LIMIT_MIN_VT_PIX_CLK_FREQ_HZ],
+		.max_vt_pix_clk_freq_hz = sensor->limits[SMIAPP_LIMIT_MAX_VT_PIX_CLK_FREQ_HZ],
+
+		.min_line_length_pck_bin = sensor->limits[SMIAPP_LIMIT_MIN_LINE_LENGTH_PCK_BIN],
+		.min_line_length_pck = sensor->limits[SMIAPP_LIMIT_MIN_LINE_LENGTH_PCK],
+	};
+	struct smiapp_pll *pll = &sensor->pll;
+	int rval;
+
+	memset(&sensor->pll, 0, sizeof(sensor->pll));
+
+	pll->lanes = sensor->platform_data->lanes;
+	pll->ext_clk_freq_hz = sensor->platform_data->ext_clk;
+
+	if (sensor->minfo.smiapp_profile == SMIAPP_PROFILE_0) {
+		/*
+		 * Fill in operational clock divisors limits from the
+		 * video timing ones. On profile 0 sensors the
+		 * requirements regarding them are essentially the
+		 * same as on VT ones.
+		 */
+		lim.min_op_sys_clk_div = lim.min_vt_sys_clk_div;
+		lim.max_op_sys_clk_div = lim.max_vt_sys_clk_div;
+		lim.min_op_pix_clk_div = lim.min_vt_pix_clk_div;
+		lim.max_op_pix_clk_div = lim.max_vt_pix_clk_div;
+		lim.min_op_sys_clk_freq_hz = lim.min_vt_sys_clk_freq_hz;
+		lim.max_op_sys_clk_freq_hz = lim.max_vt_sys_clk_freq_hz;
+		lim.min_op_pix_clk_freq_hz = lim.min_vt_pix_clk_freq_hz;
+		lim.max_op_pix_clk_freq_hz = lim.max_vt_pix_clk_freq_hz;
+		/* Profile 0 sensors have no separate OP clock branch. */
+		pll->flags |= SMIAPP_PLL_FLAG_NO_OP_CLOCKS;
+	}
+
+	if (smiapp_needs_quirk(sensor,
+			       SMIAPP_QUIRK_FLAG_OP_PIX_CLOCK_PER_LANE))
+		pll->flags |= SMIAPP_PLL_FLAG_OP_PIX_CLOCK_PER_LANE;
+
+	pll->binning_horizontal = sensor->binning_horizontal;
+	pll->binning_vertical = sensor->binning_vertical;
+	pll->link_freq =
+		sensor->link_freq->qmenu_int[sensor->link_freq->val];
+	pll->scale_m = sensor->scale_m;
+	pll->scale_n = sensor->limits[SMIAPP_LIMIT_SCALER_N_MIN];
+	pll->bits_per_pixel = sensor->csi_format->compressed;
+
+	rval = smiapp_pll_calculate(&client->dev, &lim, pll);
+	if (rval < 0)
+		return rval;
+
+	sensor->pixel_rate_parray->cur.val64 = pll->vt_pix_clk_freq_hz;
+	sensor->pixel_rate_csi->cur.val64 = pll->pixel_rate_csi;
+
+	return 0;
+}
+
+
+/*
+ *
+ * V4L2 Controls handling
+ *
+ */
+
+static void __smiapp_update_exposure_limits(struct smiapp_sensor *sensor)
+{
+	struct v4l2_ctrl *ctrl = sensor->exposure;
+	int max;
+
+	max = sensor->pixel_array->crop[SMIAPP_PA_PAD_SRC].height
+		+ sensor->vblank->val
+		- sensor->limits[SMIAPP_LIMIT_COARSE_INTEGRATION_TIME_MAX_MARGIN];
+
+	ctrl->maximum = max;
+	if (ctrl->default_value > max)
+		ctrl->default_value = max;
+	if (ctrl->val > max)
+		ctrl->val = max;
+	if (ctrl->cur.val > max)
+		ctrl->cur.val = max;
+}
+
+/*
+ * Order matters.
+ *
+ * 1. Bits-per-pixel, descending.
+ * 2. Bits-per-pixel compressed, descending.
+ * 3. Pixel order, same as in pixel_order_str. Formats for all four pixel
+ *    orders must be defined.
+ */
+static const struct smiapp_csi_data_format smiapp_csi_data_formats[] = {
+	{ V4L2_MBUS_FMT_SGRBG12_1X12, 12, 12, SMIAPP_PIXEL_ORDER_GRBG, },
+	{ V4L2_MBUS_FMT_SRGGB12_1X12, 12, 12, SMIAPP_PIXEL_ORDER_RGGB, },
+	{ V4L2_MBUS_FMT_SBGGR12_1X12, 12, 12, SMIAPP_PIXEL_ORDER_BGGR, },
+	{ V4L2_MBUS_FMT_SGBRG12_1X12, 12, 12, SMIAPP_PIXEL_ORDER_GBRG, },
+	{ V4L2_MBUS_FMT_SGRBG10_1X10, 10, 10, SMIAPP_PIXEL_ORDER_GRBG, },
+	{ V4L2_MBUS_FMT_SRGGB10_1X10, 10, 10, SMIAPP_PIXEL_ORDER_RGGB, },
+	{ V4L2_MBUS_FMT_SBGGR10_1X10, 10, 10, SMIAPP_PIXEL_ORDER_BGGR, },
+	{ V4L2_MBUS_FMT_SGBRG10_1X10, 10, 10, SMIAPP_PIXEL_ORDER_GBRG, },
+	{ V4L2_MBUS_FMT_SGRBG10_DPCM8_1X8, 10, 8, SMIAPP_PIXEL_ORDER_GRBG, },
+	{ V4L2_MBUS_FMT_SRGGB10_DPCM8_1X8, 10, 8, SMIAPP_PIXEL_ORDER_RGGB, },
+	{ V4L2_MBUS_FMT_SBGGR10_DPCM8_1X8, 10, 8, SMIAPP_PIXEL_ORDER_BGGR, },
+	{ V4L2_MBUS_FMT_SGBRG10_DPCM8_1X8, 10, 8, SMIAPP_PIXEL_ORDER_GBRG, },
+	{ V4L2_MBUS_FMT_SGRBG8_1X8, 8, 8, SMIAPP_PIXEL_ORDER_GRBG, },
+	{ V4L2_MBUS_FMT_SRGGB8_1X8, 8, 8, SMIAPP_PIXEL_ORDER_RGGB, },
+	{ V4L2_MBUS_FMT_SBGGR8_1X8, 8, 8, SMIAPP_PIXEL_ORDER_BGGR, },
+	{ V4L2_MBUS_FMT_SGBRG8_1X8, 8, 8, SMIAPP_PIXEL_ORDER_GBRG, },
+};
+
+const char *pixel_order_str[] = { "GRBG", "RGGB", "BGGR", "GBRG" };
+
+#define to_csi_format_idx(fmt) (((unsigned long)(fmt)			\
+				 - (unsigned long)smiapp_csi_data_formats) \
+				/ sizeof(*smiapp_csi_data_formats))
+
+static u32 smiapp_pixel_order(struct smiapp_sensor *sensor)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(&sensor->src->sd);
+	int flip = 0;
+
+	if (sensor->hflip) {
+		if (sensor->hflip->val)
+			flip |= SMIAPP_IMAGE_ORIENTATION_HFLIP;
+
+		if (sensor->vflip->val)
+			flip |= SMIAPP_IMAGE_ORIENTATION_VFLIP;
+	}
+
+	flip ^= sensor->hvflip_inv_mask;
+
+	dev_dbg(&client->dev, "flip %d\n", flip);
+	return sensor->default_pixel_order ^ flip;
+}
+
+static void smiapp_update_mbus_formats(struct smiapp_sensor *sensor)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(&sensor->src->sd);
+	unsigned int csi_format_idx =
+		to_csi_format_idx(sensor->csi_format) & ~3;
+	unsigned int internal_csi_format_idx =
+		to_csi_format_idx(sensor->internal_csi_format) & ~3;
+	unsigned int pixel_order = smiapp_pixel_order(sensor);
+
+	sensor->mbus_frame_fmts =
+		sensor->default_mbus_frame_fmts << pixel_order;
+	sensor->csi_format =
+		&smiapp_csi_data_formats[csi_format_idx + pixel_order];
+	sensor->internal_csi_format =
+		&smiapp_csi_data_formats[internal_csi_format_idx
+					 + pixel_order];
+
+	BUG_ON(max(internal_csi_format_idx, csi_format_idx) + pixel_order
+	       >= ARRAY_SIZE(smiapp_csi_data_formats));
+	BUG_ON(min(internal_csi_format_idx, csi_format_idx) < 0);
+
+	dev_dbg(&client->dev, "new pixel order %s\n",
+		pixel_order_str[pixel_order]);
+}
+
+static int smiapp_set_ctrl(struct v4l2_ctrl *ctrl)
+{
+	struct smiapp_sensor *sensor =
+		container_of(ctrl->handler, struct smiapp_subdev, ctrl_handler)
+			->sensor;
+	u32 orient = 0;
+	int exposure;
+	int rval;
+
+	switch (ctrl->id) {
+	case V4L2_CID_ANALOGUE_GAIN:
+		return smiapp_write(
+			sensor,
+			SMIAPP_REG_U16_ANALOGUE_GAIN_CODE_GLOBAL, ctrl->val);
+
+	case V4L2_CID_EXPOSURE:
+		return smiapp_write(
+			sensor,
+			SMIAPP_REG_U16_COARSE_INTEGRATION_TIME, ctrl->val);
+
+	case V4L2_CID_HFLIP:
+	case V4L2_CID_VFLIP:
+		if (sensor->streaming)
+			return -EBUSY;
+
+		if (sensor->hflip->val)
+			orient |= SMIAPP_IMAGE_ORIENTATION_HFLIP;
+
+		if (sensor->vflip->val)
+			orient |= SMIAPP_IMAGE_ORIENTATION_VFLIP;
+
+		orient ^= sensor->hvflip_inv_mask;
+		rval = smiapp_write(sensor,
+				    SMIAPP_REG_U8_IMAGE_ORIENTATION,
+				    orient);
+		if (rval < 0)
+			return rval;
+
+		smiapp_update_mbus_formats(sensor);
+
+		return 0;
+
+	case V4L2_CID_VBLANK:
+		exposure = sensor->exposure->val;
+
+		__smiapp_update_exposure_limits(sensor);
+
+		if (exposure > sensor->exposure->maximum) {
+			sensor->exposure->val =
+				sensor->exposure->maximum;
+			rval = smiapp_set_ctrl(
+				sensor->exposure);
+			if (rval < 0)
+				return rval;
+		}
+
+		return smiapp_write(
+			sensor, SMIAPP_REG_U16_FRAME_LENGTH_LINES,
+			sensor->pixel_array->crop[SMIAPP_PA_PAD_SRC].height
+			+ ctrl->val);
+
+	case V4L2_CID_HBLANK:
+		return smiapp_write(
+			sensor, SMIAPP_REG_U16_LINE_LENGTH_PCK,
+			sensor->pixel_array->crop[SMIAPP_PA_PAD_SRC].width
+			+ ctrl->val);
+
+	case V4L2_CID_LINK_FREQ:
+		if (sensor->streaming)
+			return -EBUSY;
+
+		return smiapp_pll_update(sensor);
+
+	default:
+		return -EINVAL;
+	}
+}
+
+static const struct v4l2_ctrl_ops smiapp_ctrl_ops = {
+	.s_ctrl = smiapp_set_ctrl,
+};
+
+static int smiapp_init_controls(struct smiapp_sensor *sensor)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(&sensor->src->sd);
+	unsigned int max;
+	int rval;
+
+	rval = v4l2_ctrl_handler_init(&sensor->pixel_array->ctrl_handler, 7);
+	if (rval)
+		return rval;
+	sensor->pixel_array->ctrl_handler.lock = &sensor->mutex;
+
+	sensor->analog_gain = v4l2_ctrl_new_std(
+		&sensor->pixel_array->ctrl_handler, &smiapp_ctrl_ops,
+		V4L2_CID_ANALOGUE_GAIN,
+		sensor->limits[SMIAPP_LIMIT_ANALOGUE_GAIN_CODE_MIN],
+		sensor->limits[SMIAPP_LIMIT_ANALOGUE_GAIN_CODE_MAX],
+		max(sensor->limits[SMIAPP_LIMIT_ANALOGUE_GAIN_CODE_STEP], 1U),
+		sensor->limits[SMIAPP_LIMIT_ANALOGUE_GAIN_CODE_MIN]);
+
+	/* Exposure limits will be updated soon, use just something here. */
+	sensor->exposure = v4l2_ctrl_new_std(
+		&sensor->pixel_array->ctrl_handler, &smiapp_ctrl_ops,
+		V4L2_CID_EXPOSURE, 0, 0, 1, 0);
+
+	sensor->hflip = v4l2_ctrl_new_std(
+		&sensor->pixel_array->ctrl_handler, &smiapp_ctrl_ops,
+		V4L2_CID_HFLIP, 0, 1, 1, 0);
+	sensor->vflip = v4l2_ctrl_new_std(
+		&sensor->pixel_array->ctrl_handler, &smiapp_ctrl_ops,
+		V4L2_CID_VFLIP, 0, 1, 1, 0);
+
+	sensor->vblank = v4l2_ctrl_new_std(
+		&sensor->pixel_array->ctrl_handler, &smiapp_ctrl_ops,
+		V4L2_CID_VBLANK, 0, 1, 1, 0);
+
+	if (sensor->vblank)
+		sensor->vblank->flags |= V4L2_CTRL_FLAG_UPDATE;
+
+	sensor->hblank = v4l2_ctrl_new_std(
+		&sensor->pixel_array->ctrl_handler, &smiapp_ctrl_ops,
+		V4L2_CID_HBLANK, 0, 1, 1, 0);
+
+	if (sensor->hblank)
+		sensor->hblank->flags |= V4L2_CTRL_FLAG_UPDATE;
+
+	sensor->pixel_rate_parray = v4l2_ctrl_new_std(
+		&sensor->pixel_array->ctrl_handler, &smiapp_ctrl_ops,
+		V4L2_CID_PIXEL_RATE, 0, 0, 1, 0);
+
+	if (sensor->pixel_array->ctrl_handler.error) {
+		dev_err(&client->dev,
+			"pixel array controls initialization failed (%d)\n",
+			sensor->pixel_array->ctrl_handler.error);
+		rval = sensor->pixel_array->ctrl_handler.error;
+		goto error;
+	}
+
+	sensor->pixel_array->sd.ctrl_handler =
+		&sensor->pixel_array->ctrl_handler;
+
+	v4l2_ctrl_cluster(2, &sensor->hflip);
+
+	rval = v4l2_ctrl_handler_init(&sensor->src->ctrl_handler, 0);
+	if (rval)
+		goto error;
+	sensor->src->ctrl_handler.lock = &sensor->mutex;
+
+	for (max = 0; sensor->platform_data->op_sys_clock[max + 1]; max++);
+
+	sensor->link_freq = v4l2_ctrl_new_int_menu(
+		&sensor->src->ctrl_handler, &smiapp_ctrl_ops,
+		V4L2_CID_LINK_FREQ, max, 0,
+		sensor->platform_data->op_sys_clock);
+
+	sensor->pixel_rate_csi = v4l2_ctrl_new_std(
+		&sensor->src->ctrl_handler, &smiapp_ctrl_ops,
+		V4L2_CID_PIXEL_RATE, 0, 0, 1, 0);
+
+	if (sensor->src->ctrl_handler.error) {
+		dev_err(&client->dev,
+			"src controls initialization failed (%d)\n",
+			sensor->src->ctrl_handler.error);
+		rval = sensor->src->ctrl_handler.error;
+		goto error;
+	}
+
+	sensor->src->sd.ctrl_handler =
+		&sensor->src->ctrl_handler;
+
+	return 0;
+
+error:
+	v4l2_ctrl_handler_free(&sensor->pixel_array->ctrl_handler);
+	v4l2_ctrl_handler_free(&sensor->src->ctrl_handler);
+
+	return rval;
+}
+
+static void smiapp_free_controls(struct smiapp_sensor *sensor)
+{
+	unsigned int i;
+
+	for (i = 0; i < sensor->ssds_used; i++)
+		v4l2_ctrl_handler_free(&sensor->ssds[i].ctrl_handler);
+}
+
+static int smiapp_get_limits(struct smiapp_sensor *sensor, int const *limit,
+			     unsigned int n)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(&sensor->src->sd);
+	unsigned int i;
+	u32 val;
+	int rval;
+
+	for (i = 0; i < n; i++) {
+		rval = smiapp_read(
+			sensor, smiapp_reg_limits[limit[i]].addr, &val);
+		if (rval)
+			return rval;
+		sensor->limits[limit[i]] = val;
+		dev_dbg(&client->dev, "0x%8.8x \"%s\" = %d, 0x%x\n",
+			smiapp_reg_limits[limit[i]].addr,
+			smiapp_reg_limits[limit[i]].what, val, val);
+	}
+
+	return 0;
+}
+
+static int smiapp_get_all_limits(struct smiapp_sensor *sensor)
+{
+	unsigned int i;
+	int rval;
+
+	for (i = 0; i < SMIAPP_LIMIT_LAST; i++) {
+		rval = smiapp_get_limits(sensor, &i, 1);
+		if (rval < 0)
+			return rval;
+	}
+
+	if (sensor->limits[SMIAPP_LIMIT_SCALER_N_MIN] == 0)
+		smiapp_replace_limit(sensor, SMIAPP_LIMIT_SCALER_N_MIN, 16);
+
+	return 0;
+}
+
+static int smiapp_get_limits_binning(struct smiapp_sensor *sensor)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(&sensor->src->sd);
+	static u32 const limits[] = {
+		SMIAPP_LIMIT_MIN_FRAME_LENGTH_LINES_BIN,
+		SMIAPP_LIMIT_MAX_FRAME_LENGTH_LINES_BIN,
+		SMIAPP_LIMIT_MIN_LINE_LENGTH_PCK_BIN,
+		SMIAPP_LIMIT_MAX_LINE_LENGTH_PCK_BIN,
+		SMIAPP_LIMIT_MIN_LINE_BLANKING_PCK_BIN,
+		SMIAPP_LIMIT_FINE_INTEGRATION_TIME_MIN_BIN,
+		SMIAPP_LIMIT_FINE_INTEGRATION_TIME_MAX_MARGIN_BIN,
+	};
+	static u32 const limits_replace[] = {
+		SMIAPP_LIMIT_MIN_FRAME_LENGTH_LINES,
+		SMIAPP_LIMIT_MAX_FRAME_LENGTH_LINES,
+		SMIAPP_LIMIT_MIN_LINE_LENGTH_PCK,
+		SMIAPP_LIMIT_MAX_LINE_LENGTH_PCK,
+		SMIAPP_LIMIT_MIN_LINE_BLANKING_PCK,
+		SMIAPP_LIMIT_FINE_INTEGRATION_TIME_MIN,
+		SMIAPP_LIMIT_FINE_INTEGRATION_TIME_MAX_MARGIN,
+	};
+	unsigned int i;
+	int rval;
+
+	if (sensor->limits[SMIAPP_LIMIT_BINNING_CAPABILITY] ==
+	    SMIAPP_BINNING_CAPABILITY_NO) {
+		for (i = 0; i < ARRAY_SIZE(limits); i++)
+			sensor->limits[limits[i]] =
+				sensor->limits[limits_replace[i]];
+
+		return 0;
+	}
+
+	rval = smiapp_get_limits(sensor, limits, ARRAY_SIZE(limits));
+	if (rval < 0)
+		return rval;
+
+	/*
+	 * Sanity check whether the binning limits are valid. If not,
+	 * use the non-binning ones.
+	 */
+	if (sensor->limits[SMIAPP_LIMIT_MIN_FRAME_LENGTH_LINES_BIN]
+	    && sensor->limits[SMIAPP_LIMIT_MIN_LINE_LENGTH_PCK_BIN]
+	    && sensor->limits[SMIAPP_LIMIT_MIN_LINE_BLANKING_PCK_BIN])
+		return 0;
+
+	for (i = 0; i < ARRAY_SIZE(limits); i++) {
+		dev_dbg(&client->dev,
+			"replace limit 0x%8.8x \"%s\" = %d, 0x%x\n",
+			smiapp_reg_limits[limits[i]].addr,
+			smiapp_reg_limits[limits[i]].what,
+			sensor->limits[limits_replace[i]],
+			sensor->limits[limits_replace[i]]);
+		sensor->limits[limits[i]] =
+			sensor->limits[limits_replace[i]];
+	}
+
+	return 0;
+}
+
+static int smiapp_get_mbus_formats(struct smiapp_sensor *sensor)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(&sensor->src->sd);
+	unsigned int type, n;
+	unsigned int i, pixel_order;
+	int rval;
+
+	rval = smiapp_read(
+		sensor, SMIAPP_REG_U8_DATA_FORMAT_MODEL_TYPE, &type);
+	if (rval)
+		return rval;
+
+	dev_dbg(&client->dev, "data_format_model_type %d\n", type);
+
+	rval = smiapp_read(sensor, SMIAPP_REG_U8_PIXEL_ORDER,
+			   &pixel_order);
+	if (rval)
+		return rval;
+
+	if (pixel_order >= ARRAY_SIZE(pixel_order_str)) {
+		dev_dbg(&client->dev, "bad pixel order %d\n", pixel_order);
+		return -EINVAL;
+	}
+
+	dev_dbg(&client->dev, "pixel order %d (%s)\n", pixel_order,
+		pixel_order_str[pixel_order]);
+
+	switch (type) {
+	case SMIAPP_DATA_FORMAT_MODEL_TYPE_NORMAL:
+		n = SMIAPP_DATA_FORMAT_MODEL_TYPE_NORMAL_N;
+		break;
+	case SMIAPP_DATA_FORMAT_MODEL_TYPE_EXTENDED:
+		n = SMIAPP_DATA_FORMAT_MODEL_TYPE_EXTENDED_N;
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	sensor->default_pixel_order = pixel_order;
+	sensor->mbus_frame_fmts = 0;
+
+	for (i = 0; i < n; i++) {
+		unsigned int fmt, j;
+
+		rval = smiapp_read(
+			sensor,
+			SMIAPP_REG_U16_DATA_FORMAT_DESCRIPTOR(i), &fmt);
+		if (rval)
+			return rval;
+
+		dev_dbg(&client->dev, "bpp %d, compressed %d\n",
+			fmt >> 8, (u8)fmt);
+
+		for (j = 0; j < ARRAY_SIZE(smiapp_csi_data_formats); j++) {
+			const struct smiapp_csi_data_format *f =
+				&smiapp_csi_data_formats[j];
+
+			if (f->pixel_order != SMIAPP_PIXEL_ORDER_GRBG)
+				continue;
+
+			if (f->width != fmt >> 8 || f->compressed != (u8)fmt)
+				continue;
+
+			dev_dbg(&client->dev, "jolly good! %d\n", j);
+
+			sensor->default_mbus_frame_fmts |= 1 << j;
+			if (!sensor->csi_format) {
+				sensor->csi_format = f;
+				sensor->internal_csi_format = f;
+			}
+		}
+	}
+
+	if (!sensor->csi_format) {
+		dev_err(&client->dev, "no supported mbus code found\n");
+		return -EINVAL;
+	}
+
+	smiapp_update_mbus_formats(sensor);
+
+	return 0;
+}
+
+static void smiapp_update_blanking(struct smiapp_sensor *sensor)
+{
+	struct v4l2_ctrl *vblank = sensor->vblank;
+	struct v4l2_ctrl *hblank = sensor->hblank;
+
+	vblank->minimum =
+		max_t(int,
+		      sensor->limits[SMIAPP_LIMIT_MIN_FRAME_BLANKING_LINES],
+		      sensor->limits[SMIAPP_LIMIT_MIN_FRAME_LENGTH_LINES_BIN] -
+		      sensor->pixel_array->crop[SMIAPP_PA_PAD_SRC].height);
+	vblank->maximum =
+		sensor->limits[SMIAPP_LIMIT_MAX_FRAME_LENGTH_LINES_BIN] -
+		sensor->pixel_array->crop[SMIAPP_PA_PAD_SRC].height;
+
+	vblank->val = clamp_t(int, vblank->val,
+			      vblank->minimum, vblank->maximum);
+	vblank->default_value = vblank->minimum;
+	vblank->val = vblank->val;
+	vblank->cur.val = vblank->val;
+
+	hblank->minimum =
+		max_t(int,
+		      sensor->limits[SMIAPP_LIMIT_MIN_LINE_LENGTH_PCK_BIN] -
+		      sensor->pixel_array->crop[SMIAPP_PA_PAD_SRC].width,
+		      sensor->limits[SMIAPP_LIMIT_MIN_LINE_BLANKING_PCK_BIN]);
+	hblank->maximum =
+		sensor->limits[SMIAPP_LIMIT_MAX_LINE_LENGTH_PCK_BIN] -
+		sensor->pixel_array->crop[SMIAPP_PA_PAD_SRC].width;
+
+	hblank->val = clamp_t(int, hblank->val,
+			      hblank->minimum, hblank->maximum);
+	hblank->default_value = hblank->minimum;
+	hblank->val = hblank->val;
+	hblank->cur.val = hblank->val;
+
+	__smiapp_update_exposure_limits(sensor);
+}
+
+static int smiapp_update_mode(struct smiapp_sensor *sensor)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(&sensor->src->sd);
+	unsigned int binning_mode;
+	int rval;
+
+	dev_dbg(&client->dev, "frame size: %dx%d\n",
+		sensor->src->crop[SMIAPP_PAD_SRC].width,
+		sensor->src->crop[SMIAPP_PAD_SRC].height);
+	dev_dbg(&client->dev, "csi format width: %d\n",
+		sensor->csi_format->width);
+
+	/* Binning has to be set up here; it affects limits */
+	if (sensor->binning_horizontal == 1 &&
+	    sensor->binning_vertical == 1) {
+		binning_mode = 0;
+	} else {
+		u8 binning_type =
+			(sensor->binning_horizontal << 4)
+			| sensor->binning_vertical;
+
+		rval = smiapp_write(
+			sensor, SMIAPP_REG_U8_BINNING_TYPE, binning_type);
+		if (rval < 0)
+			return rval;
+
+		binning_mode = 1;
+	}
+	rval = smiapp_write(sensor, SMIAPP_REG_U8_BINNING_MODE, binning_mode);
+	if (rval < 0)
+		return rval;
+
+	/* Get updated limits due to binning */
+	rval = smiapp_get_limits_binning(sensor);
+	if (rval < 0)
+		return rval;
+
+	rval = smiapp_pll_update(sensor);
+	if (rval < 0)
+		return rval;
+
+	/* Output from pixel array, including blanking */
+	smiapp_update_blanking(sensor);
+
+	dev_dbg(&client->dev, "vblank\t\t%d\n", sensor->vblank->val);
+	dev_dbg(&client->dev, "hblank\t\t%d\n", sensor->hblank->val);
+
+	dev_dbg(&client->dev, "real timeperframe\t100/%d\n",
+		sensor->pll.vt_pix_clk_freq_hz /
+		((sensor->pixel_array->crop[SMIAPP_PA_PAD_SRC].width
+		  + sensor->hblank->val) *
+		 (sensor->pixel_array->crop[SMIAPP_PA_PAD_SRC].height
+		  + sensor->vblank->val) / 100));
+
+	return 0;
+}
+
+/*
+ *
+ * SMIA++ NVM handling
+ *
+ */
+static int smiapp_read_nvm(struct smiapp_sensor *sensor,
+			   unsigned char *nvm)
+{
+	u32 i, s, p, np, v;
+	int rval = 0, rval2;
+
+	np = sensor->nvm_size / SMIAPP_NVM_PAGE_SIZE;
+	for (p = 0; p < np; p++) {
+		rval = smiapp_write(
+			sensor,
+			SMIAPP_REG_U8_DATA_TRANSFER_IF_1_PAGE_SELECT, p);
+		if (rval)
+			goto out;
+
+		rval = smiapp_write(sensor,
+				    SMIAPP_REG_U8_DATA_TRANSFER_IF_1_CTRL,
+				    SMIAPP_DATA_TRANSFER_IF_1_CTRL_EN |
+				    SMIAPP_DATA_TRANSFER_IF_1_CTRL_RD_EN);
+		if (rval)
+			goto out;
+
+		for (i = 0; i < 1000; i++) {
+			rval = smiapp_read(
+				sensor,
+				SMIAPP_REG_U8_DATA_TRANSFER_IF_1_STATUS, &s);
+
+			if (rval)
+				goto out;
+
+			if (s & SMIAPP_DATA_TRANSFER_IF_1_STATUS_RD_READY)
+				break;
+
+			if (--i == 0) {
+				rval = -ETIMEDOUT;
+				goto out;
+			}
+
+		}
+
+		for (i = 0; i < SMIAPP_NVM_PAGE_SIZE; i++) {
+			rval = smiapp_read(
+				sensor,
+				SMIAPP_REG_U8_DATA_TRANSFER_IF_1_DATA_0 + i,
+				&v);
+			if (rval)
+				goto out;
+
+			*nvm++ = v;
+		}
+	}
+
+out:
+	rval2 = smiapp_write(sensor, SMIAPP_REG_U8_DATA_TRANSFER_IF_1_CTRL, 0);
+	if (rval < 0)
+		return rval;
+	else
+		return rval2;
+}
+
+/*
+ *
+ * SMIA++ CCI address control
+ *
+ */
+static int smiapp_change_cci_addr(struct smiapp_sensor *sensor)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(&sensor->src->sd);
+	int rval;
+	u32 val;
+
+	client->addr = sensor->platform_data->i2c_addr_dfl;
+
+	rval = smiapp_write(sensor,
+			    SMIAPP_REG_U8_CCI_ADDRESS_CONTROL,
+			    sensor->platform_data->i2c_addr_alt << 1);
+	if (rval)
+		return rval;
+
+	client->addr = sensor->platform_data->i2c_addr_alt;
+
+	/* verify addr change went ok */
+	rval = smiapp_read(sensor, SMIAPP_REG_U8_CCI_ADDRESS_CONTROL, &val);
+	if (rval)
+		return rval;
+
+	if (val != sensor->platform_data->i2c_addr_alt << 1)
+		return -ENODEV;
+
+	return 0;
+}
+
+/*
+ *
+ * SMIA++ Mode Control
+ *
+ */
+static int smiapp_setup_flash_strobe(struct smiapp_sensor *sensor)
+{
+	struct smiapp_flash_strobe_parms *strobe_setup;
+	unsigned int ext_freq = sensor->platform_data->ext_clk;
+	u32 tmp;
+	u32 strobe_adjustment;
+	u32 strobe_width_high_rs;
+	int rval;
+
+	strobe_setup = sensor->platform_data->strobe_setup;
+
+	/*
+	 * How to calculate registers related to strobe length. Please
+	 * do not change, or if you do at least know what you're
+	 * doing. :-)
+	 *
+	 * Sakari Ailus <sakari.ailus@maxwell.research.nokia.com> 2010-10-25
+	 *
+	 * flash_strobe_length [us] / 10^6 = (tFlash_strobe_width_ctrl
+	 *	/ EXTCLK freq [Hz]) * flash_strobe_adjustment
+	 *
+	 * tFlash_strobe_width_ctrl E N, [1 - 0xffff]
+	 * flash_strobe_adjustment E N, [1 - 0xff]
+	 *
+	 * The formula above is written as below to keep it on one
+	 * line:
+	 *
+	 * l / 10^6 = w / e * a
+	 *
+	 * Let's mark w * a by x:
+	 *
+	 * x = w * a
+	 *
+	 * Thus, we get:
+	 *
+	 * x = l * e / 10^6
+	 *
+	 * The strobe width must be at least as long as requested,
+	 * thus rounding upwards is needed.
+	 *
+	 * x = (l * e + 10^6 - 1) / 10^6
+	 * -----------------------------
+	 *
+	 * Maximum possible accuracy is wanted at all times. Thus keep
+	 * a as small as possible.
+	 *
+	 * Calculate a, assuming maximum w, with rounding upwards:
+	 *
+	 * a = (x + (2^16 - 1) - 1) / (2^16 - 1)
+	 * -------------------------------------
+	 *
+	 * Thus, we also get w, with that a, with rounding upwards:
+	 *
+	 * w = (x + a - 1) / a
+	 * -------------------
+	 *
+	 * To get limits:
+	 *
+	 * x E [1, (2^16 - 1) * (2^8 - 1)]
+	 *
+	 * Substituting maximum x to the original formula (with rounding),
+	 * the maximum l is thus
+	 *
+	 * (2^16 - 1) * (2^8 - 1) * 10^6 = l * e + 10^6 - 1
+	 *
+	 * l = (10^6 * (2^16 - 1) * (2^8 - 1) - 10^6 + 1) / e
+	 * --------------------------------------------------
+	 *
+	 * flash_strobe_length must be clamped between 1 and
+	 * (10^6 * (2^16 - 1) * (2^8 - 1) - 10^6 + 1) / EXTCLK freq.
+	 *
+	 * Then,
+	 *
+	 * flash_strobe_adjustment = ((flash_strobe_length *
+	 *	EXTCLK freq + 10^6 - 1) / 10^6 + (2^16 - 1) - 1) / (2^16 - 1)
+	 *
+	 * tFlash_strobe_width_ctrl = ((flash_strobe_length *
+	 *	EXTCLK freq + 10^6 - 1) / 10^6 +
+	 *	flash_strobe_adjustment - 1) / flash_strobe_adjustment
+	 */
+	tmp = div_u64(1000000ULL * ((1 << 16) - 1) * ((1 << 8) - 1) -
+		      1000000 + 1, ext_freq);
+	strobe_setup->strobe_width_high_us =
+		clamp_t(u32, strobe_setup->strobe_width_high_us, 1, tmp);
+
+	tmp = div_u64(((u64)strobe_setup->strobe_width_high_us * (u64)ext_freq +
+			1000000 - 1), 1000000ULL);
+	strobe_adjustment = (tmp + (1 << 16) - 1 - 1) / ((1 << 16) - 1);
+	strobe_width_high_rs = (tmp + strobe_adjustment - 1) /
+				strobe_adjustment;
+
+	rval = smiapp_write(sensor, SMIAPP_REG_U8_FLASH_MODE_RS,
+			    strobe_setup->mode);
+	if (rval < 0)
+		goto out;
+
+	rval = smiapp_write(sensor, SMIAPP_REG_U8_FLASH_STROBE_ADJUSTMENT,
+			    strobe_adjustment);
+	if (rval < 0)
+		goto out;
+
+	rval = smiapp_write(
+		sensor, SMIAPP_REG_U16_TFLASH_STROBE_WIDTH_HIGH_RS_CTRL,
+		strobe_width_high_rs);
+	if (rval < 0)
+		goto out;
+
+	rval = smiapp_write(sensor, SMIAPP_REG_U16_TFLASH_STROBE_DELAY_RS_CTRL,
+			    strobe_setup->strobe_delay);
+	if (rval < 0)
+		goto out;
+
+	rval = smiapp_write(sensor, SMIAPP_REG_U16_FLASH_STROBE_START_POINT,
+			    strobe_setup->stobe_start_point);
+	if (rval < 0)
+		goto out;
+
+	rval = smiapp_write(sensor, SMIAPP_REG_U8_FLASH_TRIGGER_RS,
+			    strobe_setup->trigger);
+
+out:
+	sensor->platform_data->strobe_setup->trigger = 0;
+
+	return rval;
+}
+
+/* -----------------------------------------------------------------------------
+ * Power management
+ */
+
+static int smiapp_power_on(struct smiapp_sensor *sensor)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(&sensor->src->sd);
+	unsigned int sleep;
+	int rval;
+
+	rval = regulator_enable(sensor->vana);
+	if (rval) {
+		dev_err(&client->dev, "failed to enable vana regulator\n");
+		return rval;
+	}
+	usleep_range(1000, 1000);
+
+	if (sensor->platform_data->set_xclk)
+		rval = sensor->platform_data->set_xclk(
+			&sensor->src->sd, sensor->platform_data->ext_clk);
+	else
+		rval = clk_enable(sensor->ext_clk);
+	if (rval < 0) {
+		dev_dbg(&client->dev, "failed to set xclk\n");
+		goto out_xclk_fail;
+	}
+	usleep_range(1000, 1000);
+
+	if (sensor->platform_data->xshutdown != SMIAPP_NO_XSHUTDOWN)
+		gpio_set_value(sensor->platform_data->xshutdown, 1);
+
+	sleep = SMIAPP_RESET_DELAY(sensor->platform_data->ext_clk);
+	usleep_range(sleep, sleep);
+
+	/*
+	 * Failures to respond to the address change command have been noticed.
+	 * Those failures seem to be caused by the sensor requiring a longer
+	 * boot time than advertised. An additional 10ms delay seems to work
+	 * around the issue, but the SMIA++ I2C write retry hack makes the delay
+	 * unnecessary. The failures need to be investigated to find a proper
+	 * fix, and a delay will likely need to be added here if the I2C write
+	 * retry hack is reverted before the root cause of the boot time issue
+	 * is found.
+	 */
+
+	if (sensor->platform_data->i2c_addr_alt) {
+		rval = smiapp_change_cci_addr(sensor);
+		if (rval) {
+			dev_err(&client->dev, "cci address change error\n");
+			goto out_cci_addr_fail;
+		}
+	}
+
+	rval = smiapp_write(sensor, SMIAPP_REG_U8_SOFTWARE_RESET,
+			    SMIAPP_SOFTWARE_RESET);
+	if (rval < 0) {
+		dev_err(&client->dev, "software reset failed\n");
+		goto out_cci_addr_fail;
+	}
+
+	if (sensor->platform_data->i2c_addr_alt) {
+		rval = smiapp_change_cci_addr(sensor);
+		if (rval) {
+			dev_err(&client->dev, "cci address change error\n");
+			goto out_cci_addr_fail;
+		}
+	}
+
+	rval = smiapp_write(sensor, SMIAPP_REG_U16_COMPRESSION_MODE,
+			    SMIAPP_COMPRESSION_MODE_SIMPLE_PREDICTOR);
+	if (rval) {
+		dev_err(&client->dev, "compression mode set failed\n");
+		goto out_cci_addr_fail;
+	}
+
+	rval = smiapp_write(
+		sensor, SMIAPP_REG_U16_EXTCLK_FREQUENCY_MHZ,
+		sensor->platform_data->ext_clk / (1000000 / (1 << 8)));
+	if (rval) {
+		dev_err(&client->dev, "extclk frequency set failed\n");
+		goto out_cci_addr_fail;
+	}
+
+	rval = smiapp_write(sensor, SMIAPP_REG_U8_CSI_LANE_MODE,
+			    sensor->platform_data->lanes - 1);
+	if (rval) {
+		dev_err(&client->dev, "csi lane mode set failed\n");
+		goto out_cci_addr_fail;
+	}
+
+	rval = smiapp_write(sensor, SMIAPP_REG_U8_FAST_STANDBY_CTRL,
+			    SMIAPP_FAST_STANDBY_CTRL_IMMEDIATE);
+	if (rval) {
+		dev_err(&client->dev, "fast standby set failed\n");
+		goto out_cci_addr_fail;
+	}
+
+	rval = smiapp_write(sensor, SMIAPP_REG_U8_CSI_SIGNALLING_MODE,
+			    sensor->platform_data->csi_signalling_mode);
+	if (rval) {
+		dev_err(&client->dev, "csi signalling mode set failed\n");
+		goto out_cci_addr_fail;
+	}
+
+	/* DPHY control done by sensor based on requested link rate */
+	rval = smiapp_write(sensor, SMIAPP_REG_U8_DPHY_CTRL,
+			    SMIAPP_DPHY_CTRL_UI);
+	if (rval < 0)
+		return rval;
+
+	rval = smiapp_call_quirk(sensor, post_poweron);
+	if (rval) {
+		dev_err(&client->dev, "post_poweron quirks failed\n");
+		goto out_cci_addr_fail;
+	}
+
+	/* Are we still initialising...? If yes, return here. */
+	if (!sensor->pixel_array)
+		return 0;
+
+	rval = v4l2_ctrl_handler_setup(
+		&sensor->pixel_array->ctrl_handler);
+	if (rval)
+		goto out_cci_addr_fail;
+
+	rval = v4l2_ctrl_handler_setup(&sensor->src->ctrl_handler);
+	if (rval)
+		goto out_cci_addr_fail;
+
+	mutex_lock(&sensor->mutex);
+	rval = smiapp_update_mode(sensor);
+	mutex_unlock(&sensor->mutex);
+	if (rval < 0)
+		goto out_cci_addr_fail;
+
+	return 0;
+
+out_cci_addr_fail:
+	if (sensor->platform_data->xshutdown != SMIAPP_NO_XSHUTDOWN)
+		gpio_set_value(sensor->platform_data->xshutdown, 0);
+	if (sensor->platform_data->set_xclk)
+		sensor->platform_data->set_xclk(&sensor->src->sd, 0);
+	else
+		clk_disable(sensor->ext_clk);
+
+out_xclk_fail:
+	regulator_disable(sensor->vana);
+	return rval;
+}
+
+static void smiapp_power_off(struct smiapp_sensor *sensor)
+{
+	/*
+	 * Currently power/clock to lens are enable/disabled separately
+	 * but they are essentially the same signals. So if the sensor is
+	 * powered off while the lens is powered on the sensor does not
+	 * really see a power off and next time the cci address change
+	 * will fail. So do a soft reset explicitly here.
+	 */
+	if (sensor->platform_data->i2c_addr_alt)
+		smiapp_write(sensor,
+			     SMIAPP_REG_U8_SOFTWARE_RESET,
+			     SMIAPP_SOFTWARE_RESET);
+
+	if (sensor->platform_data->xshutdown != SMIAPP_NO_XSHUTDOWN)
+		gpio_set_value(sensor->platform_data->xshutdown, 0);
+	if (sensor->platform_data->set_xclk)
+		sensor->platform_data->set_xclk(&sensor->src->sd, 0);
+	else
+		clk_disable(sensor->ext_clk);
+	usleep_range(5000, 5000);
+	regulator_disable(sensor->vana);
+	sensor->streaming = 0;
+}
+
+static int smiapp_set_power(struct v4l2_subdev *subdev, int on)
+{
+	struct smiapp_sensor *sensor = to_smiapp_sensor(subdev);
+	int ret = 0;
+
+	mutex_lock(&sensor->power_mutex);
+
+	/*
+	 * If the power count is modified from 0 to != 0 or from != 0
+	 * to 0, update the power state.
+	 */
+	if (!sensor->power_count == !on)
+		goto out;
+
+	if (on) {
+		/* Power on and perform initialisation. */
+		ret = smiapp_power_on(sensor);
+		if (ret < 0)
+			goto out;
+	} else {
+		smiapp_power_off(sensor);
+	}
+
+	/* Update the power count. */
+	sensor->power_count += on ? 1 : -1;
+	WARN_ON(sensor->power_count < 0);
+
+out:
+	mutex_unlock(&sensor->power_mutex);
+	return ret;
+}
+
+/* -----------------------------------------------------------------------------
+ * Video stream management
+ */
+
+static int smiapp_start_streaming(struct smiapp_sensor *sensor)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(&sensor->src->sd);
+	int rval;
+
+	mutex_lock(&sensor->mutex);
+
+	rval = smiapp_write(sensor, SMIAPP_REG_U16_CSI_DATA_FORMAT,
+			    (sensor->csi_format->width << 8) |
+			    sensor->csi_format->compressed);
+	if (rval)
+		goto out;
+
+	rval = smiapp_pll_configure(sensor);
+	if (rval)
+		goto out;
+
+	/* Analog crop start coordinates */
+	rval = smiapp_write(sensor, SMIAPP_REG_U16_X_ADDR_START,
+			    sensor->pixel_array->crop[SMIAPP_PA_PAD_SRC].left);
+	if (rval < 0)
+		goto out;
+
+	rval = smiapp_write(sensor, SMIAPP_REG_U16_Y_ADDR_START,
+			    sensor->pixel_array->crop[SMIAPP_PA_PAD_SRC].top);
+	if (rval < 0)
+		goto out;
+
+	/* Analog crop end coordinates */
+	rval = smiapp_write(
+		sensor, SMIAPP_REG_U16_X_ADDR_END,
+		sensor->pixel_array->crop[SMIAPP_PA_PAD_SRC].left
+		+ sensor->pixel_array->crop[SMIAPP_PA_PAD_SRC].width - 1);
+	if (rval < 0)
+		goto out;
+
+	rval = smiapp_write(
+		sensor, SMIAPP_REG_U16_Y_ADDR_END,
+		sensor->pixel_array->crop[SMIAPP_PA_PAD_SRC].top
+		+ sensor->pixel_array->crop[SMIAPP_PA_PAD_SRC].height - 1);
+	if (rval < 0)
+		goto out;
+
+	/*
+	 * Output from pixel array, including blanking, is set using
+	 * controls below. No need to set here.
+	 */
+
+	/* Digital crop */
+	if (sensor->limits[SMIAPP_LIMIT_DIGITAL_CROP_CAPABILITY]
+	    == SMIAPP_DIGITAL_CROP_CAPABILITY_INPUT_CROP) {
+		rval = smiapp_write(
+			sensor, SMIAPP_REG_U16_DIGITAL_CROP_X_OFFSET,
+			sensor->scaler->crop[SMIAPP_PAD_SINK].left);
+		if (rval < 0)
+			goto out;
+
+		rval = smiapp_write(
+			sensor, SMIAPP_REG_U16_DIGITAL_CROP_Y_OFFSET,
+			sensor->scaler->crop[SMIAPP_PAD_SINK].top);
+		if (rval < 0)
+			goto out;
+
+		rval = smiapp_write(
+			sensor, SMIAPP_REG_U16_DIGITAL_CROP_IMAGE_WIDTH,
+			sensor->scaler->crop[SMIAPP_PAD_SINK].width);
+		if (rval < 0)
+			goto out;
+
+		rval = smiapp_write(
+			sensor, SMIAPP_REG_U16_DIGITAL_CROP_IMAGE_HEIGHT,
+			sensor->scaler->crop[SMIAPP_PAD_SINK].height);
+		if (rval < 0)
+			goto out;
+	}
+
+	/* Scaling */
+	if (sensor->limits[SMIAPP_LIMIT_SCALING_CAPABILITY]
+	    != SMIAPP_SCALING_CAPABILITY_NONE) {
+		rval = smiapp_write(sensor, SMIAPP_REG_U16_SCALING_MODE,
+				    sensor->scaling_mode);
+		if (rval < 0)
+			goto out;
+
+		rval = smiapp_write(sensor, SMIAPP_REG_U16_SCALE_M,
+				    sensor->scale_m);
+		if (rval < 0)
+			goto out;
+	}
+
+	/* Output size from sensor */
+	rval = smiapp_write(sensor, SMIAPP_REG_U16_X_OUTPUT_SIZE,
+			    sensor->src->crop[SMIAPP_PAD_SRC].width);
+	if (rval < 0)
+		goto out;
+	rval = smiapp_write(sensor, SMIAPP_REG_U16_Y_OUTPUT_SIZE,
+			    sensor->src->crop[SMIAPP_PAD_SRC].height);
+	if (rval < 0)
+		goto out;
+
+	if ((sensor->flash_capability &
+	     (SMIAPP_FLASH_MODE_CAPABILITY_SINGLE_STROBE |
+	      SMIAPP_FLASH_MODE_CAPABILITY_MULTIPLE_STROBE)) &&
+	    sensor->platform_data->strobe_setup != NULL &&
+	    sensor->platform_data->strobe_setup->trigger != 0) {
+		rval = smiapp_setup_flash_strobe(sensor);
+		if (rval)
+			goto out;
+	}
+
+	rval = smiapp_call_quirk(sensor, pre_streamon);
+	if (rval) {
+		dev_err(&client->dev, "pre_streamon quirks failed\n");
+		goto out;
+	}
+
+	rval = smiapp_write(sensor, SMIAPP_REG_U8_MODE_SELECT,
+			    SMIAPP_MODE_SELECT_STREAMING);
+
+out:
+	mutex_unlock(&sensor->mutex);
+
+	return rval;
+}
+
+static int smiapp_stop_streaming(struct smiapp_sensor *sensor)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(&sensor->src->sd);
+	int rval;
+
+	mutex_lock(&sensor->mutex);
+	rval = smiapp_write(sensor, SMIAPP_REG_U8_MODE_SELECT,
+			    SMIAPP_MODE_SELECT_SOFTWARE_STANDBY);
+	if (rval)
+		goto out;
+
+	rval = smiapp_call_quirk(sensor, post_streamoff);
+	if (rval)
+		dev_err(&client->dev, "post_streamoff quirks failed\n");
+
+out:
+	mutex_unlock(&sensor->mutex);
+	return rval;
+}
+
+/* -----------------------------------------------------------------------------
+ * V4L2 subdev video operations
+ */
+
+static int smiapp_set_stream(struct v4l2_subdev *subdev, int enable)
+{
+	struct smiapp_sensor *sensor = to_smiapp_sensor(subdev);
+	int rval;
+
+	if (sensor->streaming == enable)
+		return 0;
+
+	if (enable) {
+		sensor->streaming = 1;
+		rval = smiapp_start_streaming(sensor);
+		if (rval < 0)
+			sensor->streaming = 0;
+	} else {
+		rval = smiapp_stop_streaming(sensor);
+		sensor->streaming = 0;
+	}
+
+	return rval;
+}
+
+static int smiapp_enum_mbus_code(struct v4l2_subdev *subdev,
+				 struct v4l2_subdev_fh *fh,
+				 struct v4l2_subdev_mbus_code_enum *code)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(subdev);
+	struct smiapp_sensor *sensor = to_smiapp_sensor(subdev);
+	unsigned int i;
+	int idx = -1;
+	int rval = -EINVAL;
+
+	mutex_lock(&sensor->mutex);
+
+	dev_err(&client->dev, "subdev %s, pad %d, index %d\n",
+		subdev->name, code->pad, code->index);
+
+	if (subdev != &sensor->src->sd || code->pad != SMIAPP_PAD_SRC) {
+		if (code->index)
+			goto out;
+
+		code->code = sensor->internal_csi_format->code;
+		rval = 0;
+		goto out;
+	}
+
+	for (i = 0; i < ARRAY_SIZE(smiapp_csi_data_formats); i++) {
+		if (sensor->mbus_frame_fmts & (1 << i))
+			idx++;
+
+		if (idx == code->index) {
+			code->code = smiapp_csi_data_formats[i].code;
+			dev_err(&client->dev, "found index %d, i %d, code %x\n",
+				code->index, i, code->code);
+			rval = 0;
+			break;
+		}
+	}
+
+out:
+	mutex_unlock(&sensor->mutex);
+
+	return rval;
+}
+
+static u32 __smiapp_get_mbus_code(struct v4l2_subdev *subdev,
+				  unsigned int pad)
+{
+	struct smiapp_sensor *sensor = to_smiapp_sensor(subdev);
+
+	if (subdev == &sensor->src->sd && pad == SMIAPP_PAD_SRC)
+		return sensor->csi_format->code;
+	else
+		return sensor->internal_csi_format->code;
+}
+
+static int __smiapp_get_format(struct v4l2_subdev *subdev,
+			       struct v4l2_subdev_fh *fh,
+			       struct v4l2_subdev_format *fmt)
+{
+	struct smiapp_subdev *ssd = to_smiapp_subdev(subdev);
+
+	if (fmt->which == V4L2_SUBDEV_FORMAT_TRY) {
+		fmt->format = *v4l2_subdev_get_try_format(fh, fmt->pad);
+	} else {
+		struct v4l2_rect *r;
+
+		if (fmt->pad == ssd->source_pad)
+			r = &ssd->crop[ssd->source_pad];
+		else
+			r = &ssd->sink_fmt;
+
+		fmt->format.code = __smiapp_get_mbus_code(subdev, fmt->pad);
+		fmt->format.width = r->width;
+		fmt->format.height = r->height;
+	}
+
+	return 0;
+}
+
+static int smiapp_get_format(struct v4l2_subdev *subdev,
+			     struct v4l2_subdev_fh *fh,
+			     struct v4l2_subdev_format *fmt)
+{
+	struct smiapp_sensor *sensor = to_smiapp_sensor(subdev);
+	int rval;
+
+	mutex_lock(&sensor->mutex);
+	rval = __smiapp_get_format(subdev, fh, fmt);
+	mutex_unlock(&sensor->mutex);
+
+	return rval;
+}
+
+static void smiapp_get_crop_compose(struct v4l2_subdev *subdev,
+				    struct v4l2_subdev_fh *fh,
+				    struct v4l2_rect **crops,
+				    struct v4l2_rect **comps, int which)
+{
+	struct smiapp_subdev *ssd = to_smiapp_subdev(subdev);
+	unsigned int i;
+
+	if (which == V4L2_SUBDEV_FORMAT_ACTIVE) {
+		if (crops)
+			for (i = 0; i < subdev->entity.num_pads; i++)
+				crops[i] = &ssd->crop[i];
+		if (comps)
+			*comps = &ssd->compose;
+	} else {
+		if (crops) {
+			for (i = 0; i < subdev->entity.num_pads; i++) {
+				crops[i] = v4l2_subdev_get_try_crop(fh, i);
+				BUG_ON(!crops[i]);
+			}
+		}
+		if (comps) {
+			*comps = v4l2_subdev_get_try_compose(fh,
+							     SMIAPP_PAD_SINK);
+			BUG_ON(!*comps);
+		}
+	}
+}
+
+/* Changes require propagation only on sink pad. */
+static void smiapp_propagate(struct v4l2_subdev *subdev,
+			     struct v4l2_subdev_fh *fh, int which,
+			     int target)
+{
+	struct smiapp_sensor *sensor = to_smiapp_sensor(subdev);
+	struct smiapp_subdev *ssd = to_smiapp_subdev(subdev);
+	struct v4l2_rect *comp, *crops[SMIAPP_PADS];
+
+	smiapp_get_crop_compose(subdev, fh, crops, &comp, which);
+
+	switch (target) {
+	case V4L2_SUBDEV_SEL_TGT_CROP_ACTUAL:
+		comp->width = crops[SMIAPP_PAD_SINK]->width;
+		comp->height = crops[SMIAPP_PAD_SINK]->height;
+		if (which == V4L2_SUBDEV_FORMAT_ACTIVE) {
+			if (ssd == sensor->scaler) {
+				sensor->scale_m =
+					sensor->limits[
+						SMIAPP_LIMIT_SCALER_N_MIN];
+				sensor->scaling_mode =
+					SMIAPP_SCALING_MODE_NONE;
+			} else if (ssd == sensor->binner) {
+				sensor->binning_horizontal = 1;
+				sensor->binning_vertical = 1;
+			}
+		}
+		/* Fall through */
+	case V4L2_SUBDEV_SEL_TGT_COMPOSE_ACTUAL:
+		*crops[SMIAPP_PAD_SRC] = *comp;
+		break;
+	default:
+		BUG();
+	}
+}
+
+static const struct smiapp_csi_data_format
+*smiapp_validate_csi_data_format(struct smiapp_sensor *sensor, u32 code)
+{
+	const struct smiapp_csi_data_format *csi_format = sensor->csi_format;
+	unsigned int i;
+
+	for (i = 0; i < ARRAY_SIZE(smiapp_csi_data_formats); i++) {
+		if (sensor->mbus_frame_fmts & (1 << i)
+		    && smiapp_csi_data_formats[i].code == code)
+			return &smiapp_csi_data_formats[i];
+	}
+
+	return csi_format;
+}
+
+static int smiapp_set_format(struct v4l2_subdev *subdev,
+			     struct v4l2_subdev_fh *fh,
+			     struct v4l2_subdev_format *fmt)
+{
+	struct smiapp_sensor *sensor = to_smiapp_sensor(subdev);
+	struct smiapp_subdev *ssd = to_smiapp_subdev(subdev);
+	struct v4l2_rect *crops[SMIAPP_PADS];
+
+	mutex_lock(&sensor->mutex);
+
+	/*
+	 * Media bus code is changeable on src subdev's source pad. On
+	 * other source pads we just get format here.
+	 */
+	if (fmt->pad == ssd->source_pad) {
+		u32 code = fmt->format.code;
+		int rval = __smiapp_get_format(subdev, fh, fmt);
+
+		if (!rval && subdev == &sensor->src->sd) {
+			const struct smiapp_csi_data_format *csi_format =
+				smiapp_validate_csi_data_format(sensor, code);
+			if (fmt->which == V4L2_SUBDEV_FORMAT_ACTIVE)
+				sensor->csi_format = csi_format;
+			fmt->format.code = csi_format->code;
+		}
+
+		mutex_unlock(&sensor->mutex);
+		return rval;
+	}
+
+	/* Sink pad. Width and height are changeable here. */
+	fmt->format.code = __smiapp_get_mbus_code(subdev, fmt->pad);
+	fmt->format.width &= ~1;
+	fmt->format.height &= ~1;
+
+	fmt->format.width =
+		clamp(fmt->format.width,
+		      sensor->limits[SMIAPP_LIMIT_MIN_X_OUTPUT_SIZE],
+		      sensor->limits[SMIAPP_LIMIT_MAX_X_OUTPUT_SIZE]);
+	fmt->format.height =
+		clamp(fmt->format.height,
+		      sensor->limits[SMIAPP_LIMIT_MIN_Y_OUTPUT_SIZE],
+		      sensor->limits[SMIAPP_LIMIT_MAX_Y_OUTPUT_SIZE]);
+
+	smiapp_get_crop_compose(subdev, fh, crops, NULL, fmt->which);
+
+	crops[ssd->sink_pad]->left = 0;
+	crops[ssd->sink_pad]->top = 0;
+	crops[ssd->sink_pad]->width = fmt->format.width;
+	crops[ssd->sink_pad]->height = fmt->format.height;
+	if (fmt->which == V4L2_SUBDEV_FORMAT_ACTIVE)
+		ssd->sink_fmt = *crops[ssd->sink_pad];
+	smiapp_propagate(subdev, fh, fmt->which,
+			 V4L2_SUBDEV_SEL_TGT_CROP_ACTUAL);
+
+	mutex_unlock(&sensor->mutex);
+
+	return 0;
+}
+
+/*
+ * Calculate goodness of scaled image size compared to expected image
+ * size and flags provided.
+ */
+#define SCALING_GOODNESS		100000
+#define SCALING_GOODNESS_EXTREME	100000000
+static int scaling_goodness(struct v4l2_subdev *subdev, int w, int ask_w,
+			    int h, int ask_h, u32 flags)
+{
+	struct smiapp_sensor *sensor = to_smiapp_sensor(subdev);
+	struct i2c_client *client = v4l2_get_subdevdata(subdev);
+	int val = 0;
+
+	w &= ~1;
+	ask_w &= ~1;
+	h &= ~1;
+	ask_h &= ~1;
+
+	if (flags & V4L2_SUBDEV_SEL_FLAG_SIZE_GE) {
+		if (w < ask_w)
+			val -= SCALING_GOODNESS;
+		if (h < ask_h)
+			val -= SCALING_GOODNESS;
+	}
+
+	if (flags & V4L2_SUBDEV_SEL_FLAG_SIZE_LE) {
+		if (w > ask_w)
+			val -= SCALING_GOODNESS;
+		if (h > ask_h)
+			val -= SCALING_GOODNESS;
+	}
+
+	val -= abs(w - ask_w);
+	val -= abs(h - ask_h);
+
+	if (w < sensor->limits[SMIAPP_LIMIT_MIN_X_OUTPUT_SIZE])
+		val -= SCALING_GOODNESS_EXTREME;
+
+	dev_dbg(&client->dev, "w %d ask_w %d h %d ask_h %d goodness %d\n",
+		w, ask_h, h, ask_h, val);
+
+	return val;
+}
+
+static void smiapp_set_compose_binner(struct v4l2_subdev *subdev,
+				      struct v4l2_subdev_fh *fh,
+				      struct v4l2_subdev_selection *sel,
+				      struct v4l2_rect **crops,
+				      struct v4l2_rect *comp)
+{
+	struct smiapp_sensor *sensor = to_smiapp_sensor(subdev);
+	unsigned int i;
+	unsigned int binh = 1, binv = 1;
+	unsigned int best = scaling_goodness(
+		subdev,
+		crops[SMIAPP_PAD_SINK]->width, sel->r.width,
+		crops[SMIAPP_PAD_SINK]->height, sel->r.height, sel->flags);
+
+	for (i = 0; i < sensor->nbinning_subtypes; i++) {
+		int this = scaling_goodness(
+			subdev,
+			crops[SMIAPP_PAD_SINK]->width
+			/ sensor->binning_subtypes[i].horizontal,
+			sel->r.width,
+			crops[SMIAPP_PAD_SINK]->height
+			/ sensor->binning_subtypes[i].vertical,
+			sel->r.height, sel->flags);
+
+		if (this > best) {
+			binh = sensor->binning_subtypes[i].horizontal;
+			binv = sensor->binning_subtypes[i].vertical;
+			best = this;
+		}
+	}
+	if (sel->which == V4L2_SUBDEV_FORMAT_ACTIVE) {
+		sensor->binning_vertical = binv;
+		sensor->binning_horizontal = binh;
+	}
+
+	sel->r.width = (crops[SMIAPP_PAD_SINK]->width / binh) & ~1;
+	sel->r.height = (crops[SMIAPP_PAD_SINK]->height / binv) & ~1;
+}
+
+/*
+ * Calculate best scaling ratio and mode for given output resolution.
+ *
+ * Try all of these: horizontal ratio, vertical ratio and smallest
+ * size possible (horizontally).
+ *
+ * Also try whether horizontal scaler or full scaler gives a better
+ * result.
+ */
+static void smiapp_set_compose_scaler(struct v4l2_subdev *subdev,
+				      struct v4l2_subdev_fh *fh,
+				      struct v4l2_subdev_selection *sel,
+				      struct v4l2_rect **crops,
+				      struct v4l2_rect *comp)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(subdev);
+	struct smiapp_sensor *sensor = to_smiapp_sensor(subdev);
+	u32 min, max, a, b, max_m;
+	u32 scale_m = sensor->limits[SMIAPP_LIMIT_SCALER_N_MIN];
+	int mode = SMIAPP_SCALING_MODE_HORIZONTAL;
+	u32 try[4];
+	u32 ntry = 0;
+	unsigned int i;
+	int best = INT_MIN;
+
+	sel->r.width = min_t(unsigned int, sel->r.width,
+			     crops[SMIAPP_PAD_SINK]->width);
+	sel->r.height = min_t(unsigned int, sel->r.height,
+			      crops[SMIAPP_PAD_SINK]->height);
+
+	a = crops[SMIAPP_PAD_SINK]->width
+		* sensor->limits[SMIAPP_LIMIT_SCALER_N_MIN] / sel->r.width;
+	b = crops[SMIAPP_PAD_SINK]->height
+		* sensor->limits[SMIAPP_LIMIT_SCALER_N_MIN] / sel->r.height;
+	max_m = crops[SMIAPP_PAD_SINK]->width
+		* sensor->limits[SMIAPP_LIMIT_SCALER_N_MIN]
+		/ sensor->limits[SMIAPP_LIMIT_MIN_X_OUTPUT_SIZE];
+
+	a = min(sensor->limits[SMIAPP_LIMIT_SCALER_M_MAX],
+		max(a, sensor->limits[SMIAPP_LIMIT_SCALER_M_MIN]));
+	b = min(sensor->limits[SMIAPP_LIMIT_SCALER_M_MAX],
+		max(b, sensor->limits[SMIAPP_LIMIT_SCALER_M_MIN]));
+	max_m = min(sensor->limits[SMIAPP_LIMIT_SCALER_M_MAX],
+		    max(max_m, sensor->limits[SMIAPP_LIMIT_SCALER_M_MIN]));
+
+	dev_dbg(&client->dev, "scaling: a %d b %d max_m %d\n", a, b, max_m);
+
+	min = min(max_m, min(a, b));
+	max = min(max_m, max(a, b));
+
+	try[ntry] = min;
+	ntry++;
+	if (min != max) {
+		try[ntry] = max;
+		ntry++;
+	}
+	if (max != max_m) {
+		try[ntry] = min + 1;
+		ntry++;
+		if (min != max) {
+			try[ntry] = max + 1;
+			ntry++;
+		}
+	}
+
+	for (i = 0; i < ntry; i++) {
+		int this = scaling_goodness(
+			subdev,
+			crops[SMIAPP_PAD_SINK]->width
+			/ try[i]
+			* sensor->limits[SMIAPP_LIMIT_SCALER_N_MIN],
+			sel->r.width,
+			crops[SMIAPP_PAD_SINK]->height,
+			sel->r.height,
+			sel->flags);
+
+		dev_dbg(&client->dev, "trying factor %d (%d)\n", try[i], i);
+
+		if (this > best) {
+			scale_m = try[i];
+			mode = SMIAPP_SCALING_MODE_HORIZONTAL;
+			best = this;
+		}
+
+		if (sensor->limits[SMIAPP_LIMIT_SCALING_CAPABILITY]
+		    == SMIAPP_SCALING_CAPABILITY_HORIZONTAL)
+			continue;
+
+		this = scaling_goodness(
+			subdev, crops[SMIAPP_PAD_SINK]->width
+			/ try[i]
+			* sensor->limits[SMIAPP_LIMIT_SCALER_N_MIN],
+			sel->r.width,
+			crops[SMIAPP_PAD_SINK]->height
+			/ try[i]
+			* sensor->limits[SMIAPP_LIMIT_SCALER_N_MIN],
+			sel->r.height,
+			sel->flags);
+
+		if (this > best) {
+			scale_m = try[i];
+			mode = SMIAPP_SCALING_MODE_BOTH;
+			best = this;
+		}
+	}
+
+	sel->r.width =
+		(crops[SMIAPP_PAD_SINK]->width
+		 / scale_m
+		 * sensor->limits[SMIAPP_LIMIT_SCALER_N_MIN]) & ~1;
+	if (mode == SMIAPP_SCALING_MODE_BOTH)
+		sel->r.height =
+			(crops[SMIAPP_PAD_SINK]->height
+			 / scale_m
+			 * sensor->limits[SMIAPP_LIMIT_SCALER_N_MIN])
+			& ~1;
+	else
+		sel->r.height = crops[SMIAPP_PAD_SINK]->height;
+
+	if (sel->which == V4L2_SUBDEV_FORMAT_ACTIVE) {
+		sensor->scale_m = scale_m;
+		sensor->scaling_mode = mode;
+	}
+}
+/* We're only called on source pads. This function sets scaling. */
+static int smiapp_set_compose(struct v4l2_subdev *subdev,
+			      struct v4l2_subdev_fh *fh,
+			      struct v4l2_subdev_selection *sel)
+{
+	struct smiapp_sensor *sensor = to_smiapp_sensor(subdev);
+	struct smiapp_subdev *ssd = to_smiapp_subdev(subdev);
+	struct v4l2_rect *comp, *crops[SMIAPP_PADS];
+
+	smiapp_get_crop_compose(subdev, fh, crops, &comp, sel->which);
+
+	sel->r.top = 0;
+	sel->r.left = 0;
+
+	if (ssd == sensor->binner)
+		smiapp_set_compose_binner(subdev, fh, sel, crops, comp);
+	else
+		smiapp_set_compose_scaler(subdev, fh, sel, crops, comp);
+
+	*comp = sel->r;
+	smiapp_propagate(subdev, fh, sel->which,
+			 V4L2_SUBDEV_SEL_TGT_COMPOSE_ACTUAL);
+
+	if (sel->which == V4L2_SUBDEV_FORMAT_ACTIVE)
+		return smiapp_update_mode(sensor);
+
+	return 0;
+}
+
+static int __smiapp_sel_supported(struct v4l2_subdev *subdev,
+				  struct v4l2_subdev_selection *sel)
+{
+	struct smiapp_sensor *sensor = to_smiapp_sensor(subdev);
+	struct smiapp_subdev *ssd = to_smiapp_subdev(subdev);
+
+	/* We only implement crop in three places. */
+	switch (sel->target) {
+	case V4L2_SUBDEV_SEL_TGT_CROP_ACTUAL:
+	case V4L2_SUBDEV_SEL_TGT_CROP_BOUNDS:
+		if (ssd == sensor->pixel_array
+		    && sel->pad == SMIAPP_PA_PAD_SRC)
+			return 0;
+		if (ssd == sensor->src
+		    && sel->pad == SMIAPP_PAD_SRC)
+			return 0;
+		if (ssd == sensor->scaler
+		    && sel->pad == SMIAPP_PAD_SINK
+		    && sensor->limits[SMIAPP_LIMIT_DIGITAL_CROP_CAPABILITY]
+		    == SMIAPP_DIGITAL_CROP_CAPABILITY_INPUT_CROP)
+			return 0;
+		return -EINVAL;
+	case V4L2_SUBDEV_SEL_TGT_COMPOSE_ACTUAL:
+	case V4L2_SUBDEV_SEL_TGT_COMPOSE_BOUNDS:
+		if (sel->pad == ssd->source_pad)
+			return -EINVAL;
+		if (ssd == sensor->binner)
+			return 0;
+		if (ssd == sensor->scaler
+		    && sensor->limits[SMIAPP_LIMIT_SCALING_CAPABILITY]
+		    != SMIAPP_SCALING_CAPABILITY_NONE)
+			return 0;
+		/* Fall through */
+	default:
+		return -EINVAL;
+	}
+}
+
+static int smiapp_set_crop(struct v4l2_subdev *subdev,
+			   struct v4l2_subdev_fh *fh,
+			   struct v4l2_subdev_selection *sel)
+{
+	struct smiapp_sensor *sensor = to_smiapp_sensor(subdev);
+	struct smiapp_subdev *ssd = to_smiapp_subdev(subdev);
+	struct v4l2_rect *src_size, *crops[SMIAPP_PADS];
+	struct v4l2_rect _r;
+
+	smiapp_get_crop_compose(subdev, fh, crops, NULL, sel->which);
+
+	if (sel->which == V4L2_SUBDEV_FORMAT_ACTIVE) {
+		if (sel->pad == ssd->sink_pad)
+			src_size = &ssd->sink_fmt;
+		else
+			src_size = &ssd->compose;
+	} else {
+		if (sel->pad == ssd->sink_pad) {
+			_r.left = 0;
+			_r.top = 0;
+			_r.width = v4l2_subdev_get_try_format(fh, sel->pad)
+				->width;
+			_r.height = v4l2_subdev_get_try_format(fh, sel->pad)
+				->height;
+			src_size = &_r;
+		} else {
+			src_size =
+				v4l2_subdev_get_try_compose(
+					fh, ssd->sink_pad);
+		}
+	}
+
+	if (ssd == sensor->src && sel->pad == SMIAPP_PAD_SRC) {
+		sel->r.left = 0;
+		sel->r.top = 0;
+	}
+
+	sel->r.width = min(sel->r.width, src_size->width);
+	sel->r.height = min(sel->r.height, src_size->height);
+
+	sel->r.left = min(sel->r.left, src_size->width - sel->r.width);
+	sel->r.top = min(sel->r.top, src_size->height - sel->r.height);
+
+	*crops[sel->pad] = sel->r;
+
+	if (ssd != sensor->pixel_array && sel->pad == SMIAPP_PAD_SINK)
+		smiapp_propagate(subdev, fh, sel->which,
+				 V4L2_SUBDEV_SEL_TGT_CROP_ACTUAL);
+
+	return 0;
+}
+
+static int __smiapp_get_selection(struct v4l2_subdev *subdev,
+				  struct v4l2_subdev_fh *fh,
+				  struct v4l2_subdev_selection *sel)
+{
+	struct smiapp_sensor *sensor = to_smiapp_sensor(subdev);
+	struct smiapp_subdev *ssd = to_smiapp_subdev(subdev);
+	struct v4l2_rect *comp, *crops[SMIAPP_PADS];
+	struct v4l2_rect sink_fmt;
+	int ret;
+
+	ret = __smiapp_sel_supported(subdev, sel);
+	if (ret)
+		return ret;
+
+	smiapp_get_crop_compose(subdev, fh, crops, &comp, sel->which);
+
+	if (sel->which == V4L2_SUBDEV_FORMAT_ACTIVE) {
+		sink_fmt = ssd->sink_fmt;
+	} else {
+		struct v4l2_mbus_framefmt *fmt =
+			v4l2_subdev_get_try_format(fh, ssd->sink_pad);
+
+		sink_fmt.left = 0;
+		sink_fmt.top = 0;
+		sink_fmt.width = fmt->width;
+		sink_fmt.height = fmt->height;
+	}
+
+	switch (sel->target) {
+	case V4L2_SUBDEV_SEL_TGT_CROP_BOUNDS:
+		if (ssd == sensor->pixel_array) {
+			sel->r.width =
+				sensor->limits[SMIAPP_LIMIT_X_ADDR_MAX] + 1;
+			sel->r.height =
+				sensor->limits[SMIAPP_LIMIT_Y_ADDR_MAX] + 1;
+		} else if (sel->pad == ssd->sink_pad) {
+			sel->r = sink_fmt;
+		} else {
+			sel->r = *comp;
+		}
+		break;
+	case V4L2_SUBDEV_SEL_TGT_CROP_ACTUAL:
+	case V4L2_SUBDEV_SEL_TGT_COMPOSE_BOUNDS:
+		sel->r = *crops[sel->pad];
+		break;
+	case V4L2_SUBDEV_SEL_TGT_COMPOSE_ACTUAL:
+		sel->r = *comp;
+		break;
+	}
+
+	return 0;
+}
+
+static int smiapp_get_selection(struct v4l2_subdev *subdev,
+				struct v4l2_subdev_fh *fh,
+				struct v4l2_subdev_selection *sel)
+{
+	struct smiapp_sensor *sensor = to_smiapp_sensor(subdev);
+	int rval;
+
+	mutex_lock(&sensor->mutex);
+	rval = __smiapp_get_selection(subdev, fh, sel);
+	mutex_unlock(&sensor->mutex);
+
+	return rval;
+}
+static int smiapp_set_selection(struct v4l2_subdev *subdev,
+				struct v4l2_subdev_fh *fh,
+				struct v4l2_subdev_selection *sel)
+{
+	struct smiapp_sensor *sensor = to_smiapp_sensor(subdev);
+	int ret;
+
+	ret = __smiapp_sel_supported(subdev, sel);
+	if (ret)
+		return ret;
+
+	mutex_lock(&sensor->mutex);
+
+	sel->r.left = max(0, sel->r.left & ~1);
+	sel->r.top = max(0, sel->r.top & ~1);
+	sel->r.width = max(0, SMIAPP_ALIGN_DIM(sel->r.width, sel->flags));
+	sel->r.height = max(0, SMIAPP_ALIGN_DIM(sel->r.height, sel->flags));
+
+	sel->r.width = max_t(unsigned int,
+			     sensor->limits[SMIAPP_LIMIT_MIN_X_OUTPUT_SIZE],
+			     sel->r.width);
+	sel->r.height = max_t(unsigned int,
+			      sensor->limits[SMIAPP_LIMIT_MIN_Y_OUTPUT_SIZE],
+			      sel->r.height);
+
+	switch (sel->target) {
+	case V4L2_SUBDEV_SEL_TGT_CROP_ACTUAL:
+		ret = smiapp_set_crop(subdev, fh, sel);
+		break;
+	case V4L2_SUBDEV_SEL_TGT_COMPOSE_ACTUAL:
+		ret = smiapp_set_compose(subdev, fh, sel);
+		break;
+	default:
+		BUG();
+	}
+
+	mutex_unlock(&sensor->mutex);
+	return ret;
+}
+
+static int smiapp_get_skip_frames(struct v4l2_subdev *subdev, u32 *frames)
+{
+	struct smiapp_sensor *sensor = to_smiapp_sensor(subdev);
+
+	*frames = sensor->frame_skip;
+	return 0;
+}
+
+/* -----------------------------------------------------------------------------
+ * sysfs attributes
+ */
+
+static ssize_t
+smiapp_sysfs_nvm_read(struct device *dev, struct device_attribute *attr,
+		      char *buf)
+{
+	struct v4l2_subdev *subdev = i2c_get_clientdata(to_i2c_client(dev));
+	struct i2c_client *client = v4l2_get_subdevdata(subdev);
+	struct smiapp_sensor *sensor = to_smiapp_sensor(subdev);
+	unsigned int nbytes;
+
+	if (!sensor->dev_init_done)
+		return -EBUSY;
+
+	if (!sensor->nvm_size) {
+		/* NVM not read yet - read it now */
+		sensor->nvm_size = sensor->platform_data->nvm_size;
+		if (smiapp_set_power(subdev, 1) < 0)
+			return -ENODEV;
+		if (smiapp_read_nvm(sensor, sensor->nvm)) {
+			dev_err(&client->dev, "nvm read failed\n");
+			return -ENODEV;
+		}
+		smiapp_set_power(subdev, 0);
+	}
+	/*
+	 * NVM is still way below a PAGE_SIZE, so we can safely
+	 * assume this for now.
+	 */
+	nbytes = min_t(unsigned int, sensor->nvm_size, PAGE_SIZE);
+	memcpy(buf, sensor->nvm, nbytes);
+
+	return nbytes;
+}
+static DEVICE_ATTR(nvm, S_IRUGO, smiapp_sysfs_nvm_read, NULL);
+
+/* -----------------------------------------------------------------------------
+ * V4L2 subdev core operations
+ */
+
+static int smiapp_identify_module(struct v4l2_subdev *subdev)
+{
+	struct smiapp_sensor *sensor = to_smiapp_sensor(subdev);
+	struct i2c_client *client = v4l2_get_subdevdata(subdev);
+	struct smiapp_module_info *minfo = &sensor->minfo;
+	unsigned int i;
+	int rval = 0;
+
+	minfo->name = SMIAPP_NAME;
+
+	/* Module info */
+	rval = smiapp_read_8only(sensor, SMIAPP_REG_U8_MANUFACTURER_ID,
+				 &minfo->manufacturer_id);
+	if (!rval)
+		rval = smiapp_read_8only(sensor, SMIAPP_REG_U16_MODEL_ID,
+					 &minfo->model_id);
+	if (!rval)
+		rval = smiapp_read_8only(sensor,
+					 SMIAPP_REG_U8_REVISION_NUMBER_MAJOR,
+					 &minfo->revision_number_major);
+	if (!rval)
+		rval = smiapp_read_8only(sensor,
+					 SMIAPP_REG_U8_REVISION_NUMBER_MINOR,
+					 &minfo->revision_number_minor);
+	if (!rval)
+		rval = smiapp_read_8only(sensor,
+					 SMIAPP_REG_U8_MODULE_DATE_YEAR,
+					 &minfo->module_year);
+	if (!rval)
+		rval = smiapp_read_8only(sensor,
+					 SMIAPP_REG_U8_MODULE_DATE_MONTH,
+					 &minfo->module_month);
+	if (!rval)
+		rval = smiapp_read_8only(sensor, SMIAPP_REG_U8_MODULE_DATE_DAY,
+					 &minfo->module_day);
+
+	/* Sensor info */
+	if (!rval)
+		rval = smiapp_read_8only(sensor,
+					 SMIAPP_REG_U8_SENSOR_MANUFACTURER_ID,
+					 &minfo->sensor_manufacturer_id);
+	if (!rval)
+		rval = smiapp_read_8only(sensor,
+					 SMIAPP_REG_U16_SENSOR_MODEL_ID,
+					 &minfo->sensor_model_id);
+	if (!rval)
+		rval = smiapp_read_8only(sensor,
+					 SMIAPP_REG_U8_SENSOR_REVISION_NUMBER,
+					 &minfo->sensor_revision_number);
+	if (!rval)
+		rval = smiapp_read_8only(sensor,
+					 SMIAPP_REG_U8_SENSOR_FIRMWARE_VERSION,
+					 &minfo->sensor_firmware_version);
+
+	/* SMIA */
+	if (!rval)
+		rval = smiapp_read_8only(sensor, SMIAPP_REG_U8_SMIA_VERSION,
+					 &minfo->smia_version);
+	if (!rval)
+		rval = smiapp_read_8only(sensor, SMIAPP_REG_U8_SMIAPP_VERSION,
+					 &minfo->smiapp_version);
+
+	if (rval) {
+		dev_err(&client->dev, "sensor detection failed\n");
+		return -ENODEV;
+	}
+
+	dev_dbg(&client->dev, "module 0x%2.2x-0x%4.4x\n",
+		minfo->manufacturer_id, minfo->model_id);
+
+	dev_dbg(&client->dev,
+		"module revision 0x%2.2x-0x%2.2x date %2.2d-%2.2d-%2.2d\n",
+		minfo->revision_number_major, minfo->revision_number_minor,
+		minfo->module_year, minfo->module_month, minfo->module_day);
+
+	dev_dbg(&client->dev, "sensor 0x%2.2x-0x%4.4x\n",
+		minfo->sensor_manufacturer_id, minfo->sensor_model_id);
+
+	dev_dbg(&client->dev,
+		"sensor revision 0x%2.2x firmware version 0x%2.2x\n",
+		minfo->sensor_revision_number, minfo->sensor_firmware_version);
+
+	dev_dbg(&client->dev, "smia version %2.2d smiapp version %2.2d\n",
+		minfo->smia_version, minfo->smiapp_version);
+
+	/*
+	 * Some modules have bad data in the lvalues below. Hope the
+	 * rvalues have better stuff. The lvalues are module
+	 * parameters whereas the rvalues are sensor parameters.
+	 */
+	if (!minfo->manufacturer_id && !minfo->model_id) {
+		minfo->manufacturer_id = minfo->sensor_manufacturer_id;
+		minfo->model_id = minfo->sensor_model_id;
+		minfo->revision_number_major = minfo->sensor_revision_number;
+	}
+
+	for (i = 0; i < ARRAY_SIZE(smiapp_module_idents); i++) {
+		if (smiapp_module_idents[i].manufacturer_id
+		    != minfo->manufacturer_id)
+			continue;
+		if (smiapp_module_idents[i].model_id != minfo->model_id)
+			continue;
+		if (smiapp_module_idents[i].flags
+		    & SMIAPP_MODULE_IDENT_FLAG_REV_LE) {
+			if (smiapp_module_idents[i].revision_number_major
+			    < minfo->revision_number_major)
+				continue;
+		} else {
+			if (smiapp_module_idents[i].revision_number_major
+			    != minfo->revision_number_major)
+				continue;
+		}
+
+		minfo->name = smiapp_module_idents[i].name;
+		minfo->quirk = smiapp_module_idents[i].quirk;
+		break;
+	}
+
+	if (i >= ARRAY_SIZE(smiapp_module_idents))
+		dev_warn(&client->dev,
+			 "no quirks for this module; let's hope it's fully compliant\n");
+
+	dev_dbg(&client->dev, "the sensor is called %s, ident %2.2x%4.4x%2.2x\n",
+		minfo->name, minfo->manufacturer_id, minfo->model_id,
+		minfo->revision_number_major);
+
+	strlcpy(subdev->name, sensor->minfo.name, sizeof(subdev->name));
+
+	return 0;
+}
+
+static const struct v4l2_subdev_ops smiapp_ops;
+static const struct v4l2_subdev_internal_ops smiapp_internal_ops;
+static const struct media_entity_operations smiapp_entity_ops;
+
+static int smiapp_registered(struct v4l2_subdev *subdev)
+{
+	struct smiapp_sensor *sensor = to_smiapp_sensor(subdev);
+	struct i2c_client *client = v4l2_get_subdevdata(subdev);
+	struct smiapp_subdev *last = NULL;
+	u32 tmp;
+	unsigned int i;
+	int rval;
+
+	sensor->vana = regulator_get(&client->dev, "VANA");
+	if (IS_ERR(sensor->vana)) {
+		dev_err(&client->dev, "could not get regulator for vana\n");
+		return -ENODEV;
+	}
+
+	if (!sensor->platform_data->set_xclk) {
+		sensor->ext_clk = clk_get(&client->dev,
+					  sensor->platform_data->ext_clk_name);
+		if (IS_ERR(sensor->ext_clk)) {
+			dev_err(&client->dev, "could not get clock %s\n",
+				sensor->platform_data->ext_clk_name);
+			rval = -ENODEV;
+			goto out_clk_get;
+		}
+
+		rval = clk_set_rate(sensor->ext_clk,
+				    sensor->platform_data->ext_clk);
+		if (rval < 0) {
+			dev_err(&client->dev,
+				"unable to set clock %s freq to %u\n",
+				sensor->platform_data->ext_clk_name,
+				sensor->platform_data->ext_clk);
+			rval = -ENODEV;
+			goto out_clk_set_rate;
+		}
+	}
+
+	if (sensor->platform_data->xshutdown != SMIAPP_NO_XSHUTDOWN) {
+		if (gpio_request_one(sensor->platform_data->xshutdown, 0,
+				     "SMIA++ xshutdown") != 0) {
+			dev_err(&client->dev,
+				"unable to acquire reset gpio %d\n",
+				sensor->platform_data->xshutdown);
+			rval = -ENODEV;
+			goto out_clk_set_rate;
+		}
+	}
+
+	rval = smiapp_power_on(sensor);
+	if (rval) {
+		rval = -ENODEV;
+		goto out_smiapp_power_on;
+	}
+
+	rval = smiapp_identify_module(subdev);
+	if (rval) {
+		rval = -ENODEV;
+		goto out_power_off;
+	}
+
+	rval = smiapp_get_all_limits(sensor);
+	if (rval) {
+		rval = -ENODEV;
+		goto out_power_off;
+	}
+
+	/*
+	 * Handle Sensor Module orientation on the board.
+	 *
+	 * The application of H-FLIP and V-FLIP on the sensor is modified by
+	 * the sensor orientation on the board.
+	 *
+	 * For SMIAPP_BOARD_SENSOR_ORIENT_180 the default behaviour is to set
+	 * both H-FLIP and V-FLIP for normal operation which also implies
+	 * that a set/unset operation for user space HFLIP and VFLIP v4l2
+	 * controls will need to be internally inverted.
+	 *
+	 * Rotation also changes the bayer pattern.
+	 */
+	if (sensor->platform_data->module_board_orient ==
+	    SMIAPP_MODULE_BOARD_ORIENT_180)
+		sensor->hvflip_inv_mask = SMIAPP_IMAGE_ORIENTATION_HFLIP |
+					  SMIAPP_IMAGE_ORIENTATION_VFLIP;
+
+	rval = smiapp_get_mbus_formats(sensor);
+	if (rval) {
+		rval = -ENODEV;
+		goto out_power_off;
+	}
+
+	if (sensor->limits[SMIAPP_LIMIT_BINNING_CAPABILITY]) {
+		u32 val;
+
+		rval = smiapp_read(sensor,
+				   SMIAPP_REG_U8_BINNING_SUBTYPES, &val);
+		if (rval < 0) {
+			rval = -ENODEV;
+			goto out_power_off;
+		}
+		sensor->nbinning_subtypes = min_t(u8, val,
+						  SMIAPP_BINNING_SUBTYPES);
+
+		for (i = 0; i < sensor->nbinning_subtypes; i++) {
+			rval = smiapp_read(
+				sensor, SMIAPP_REG_U8_BINNING_TYPE_n(i), &val);
+			if (rval < 0) {
+				rval = -ENODEV;
+				goto out_power_off;
+			}
+			sensor->binning_subtypes[i] =
+				*(struct smiapp_binning_subtype *)&val;
+
+			dev_dbg(&client->dev, "binning %xx%x\n",
+				sensor->binning_subtypes[i].horizontal,
+				sensor->binning_subtypes[i].vertical);
+		}
+	}
+	sensor->binning_horizontal = 1;
+	sensor->binning_vertical = 1;
+
+	/* SMIA++ NVM initialization - it will be read from the sensor
+	 * when it is first requested by userspace.
+	 */
+	if (sensor->minfo.smiapp_version && sensor->platform_data->nvm_size) {
+		sensor->nvm = kzalloc(sensor->platform_data->nvm_size,
+				      GFP_KERNEL);
+		if (sensor->nvm == NULL) {
+			dev_err(&client->dev, "nvm buf allocation failed\n");
+			rval = -ENOMEM;
+			goto out_power_off;
+		}
+
+		if (device_create_file(&client->dev, &dev_attr_nvm) != 0) {
+			dev_err(&client->dev, "sysfs nvm entry failed\n");
+			rval = -EBUSY;
+			goto out_power_off;
+		}
+	}
+
+	rval = smiapp_call_quirk(sensor, limits);
+	if (rval) {
+		dev_err(&client->dev, "limits quirks failed\n");
+		goto out_nvm_release;
+	}
+
+	/* We consider this as profile 0 sensor if any of these are zero. */
+	if (!sensor->limits[SMIAPP_LIMIT_MIN_OP_SYS_CLK_DIV] ||
+	    !sensor->limits[SMIAPP_LIMIT_MAX_OP_SYS_CLK_DIV] ||
+	    !sensor->limits[SMIAPP_LIMIT_MIN_OP_PIX_CLK_DIV] ||
+	    !sensor->limits[SMIAPP_LIMIT_MAX_OP_PIX_CLK_DIV]) {
+		sensor->minfo.smiapp_profile = SMIAPP_PROFILE_0;
+	} else if (sensor->limits[SMIAPP_LIMIT_SCALING_CAPABILITY]
+		   != SMIAPP_SCALING_CAPABILITY_NONE) {
+		if (sensor->limits[SMIAPP_LIMIT_SCALING_CAPABILITY]
+		    == SMIAPP_SCALING_CAPABILITY_HORIZONTAL)
+			sensor->minfo.smiapp_profile = SMIAPP_PROFILE_1;
+		else
+			sensor->minfo.smiapp_profile = SMIAPP_PROFILE_2;
+		sensor->scaler = &sensor->ssds[sensor->ssds_used];
+		sensor->ssds_used++;
+	} else if (sensor->limits[SMIAPP_LIMIT_DIGITAL_CROP_CAPABILITY]
+		   == SMIAPP_DIGITAL_CROP_CAPABILITY_INPUT_CROP) {
+		sensor->scaler = &sensor->ssds[sensor->ssds_used];
+		sensor->ssds_used++;
+	}
+	sensor->binner = &sensor->ssds[sensor->ssds_used];
+	sensor->ssds_used++;
+	sensor->pixel_array = &sensor->ssds[sensor->ssds_used];
+	sensor->ssds_used++;
+
+	sensor->scale_m = sensor->limits[SMIAPP_LIMIT_SCALER_N_MIN];
+
+	for (i = 0; i < SMIAPP_SUBDEVS; i++) {
+		struct {
+			struct smiapp_subdev *ssd;
+			char *name;
+		} const __this[] = {
+			{ sensor->scaler, "scaler", },
+			{ sensor->binner, "binner", },
+			{ sensor->pixel_array, "pixel array", },
+		}, *_this = &__this[i];
+		struct smiapp_subdev *this = _this->ssd;
+
+		if (!this)
+			continue;
+
+		if (this != sensor->src)
+			v4l2_subdev_init(&this->sd, &smiapp_ops);
+
+		this->sensor = sensor;
+
+		if (this == sensor->pixel_array) {
+			this->npads = 1;
+		} else {
+			this->npads = 2;
+			this->source_pad = 1;
+		}
+
+		snprintf(this->sd.name,
+			 sizeof(this->sd.name), "%s %s",
+			 sensor->minfo.name, _this->name);
+
+		this->sink_fmt.width =
+			sensor->limits[SMIAPP_LIMIT_X_ADDR_MAX] + 1;
+		this->sink_fmt.height =
+			sensor->limits[SMIAPP_LIMIT_Y_ADDR_MAX] + 1;
+		this->compose.width = this->sink_fmt.width;
+		this->compose.height = this->sink_fmt.height;
+		this->crop[this->source_pad] = this->compose;
+		this->pads[this->source_pad].flags = MEDIA_PAD_FL_SOURCE;
+		if (this != sensor->pixel_array) {
+			this->crop[this->sink_pad] = this->compose;
+			this->pads[this->sink_pad].flags = MEDIA_PAD_FL_SINK;
+		}
+
+		this->sd.entity.ops = &smiapp_entity_ops;
+
+		if (last == NULL) {
+			last = this;
+			continue;
+		}
+
+		this->sd.flags |= V4L2_SUBDEV_FL_HAS_DEVNODE;
+		this->sd.internal_ops = &smiapp_internal_ops;
+		this->sd.owner = NULL;
+		v4l2_set_subdevdata(&this->sd, client);
+
+		rval = media_entity_init(&this->sd.entity,
+					 this->npads, this->pads, 0);
+		if (rval) {
+			dev_err(&client->dev,
+				"media_entity_init failed\n");
+			goto out_nvm_release;
+		}
+
+		rval = media_entity_create_link(&this->sd.entity,
+						this->source_pad,
+						&last->sd.entity,
+						last->sink_pad,
+						MEDIA_LNK_FL_ENABLED |
+						MEDIA_LNK_FL_IMMUTABLE);
+		if (rval) {
+			dev_err(&client->dev,
+				"media_entity_create_link failed\n");
+			goto out_nvm_release;
+		}
+
+		rval = v4l2_device_register_subdev(sensor->src->sd.v4l2_dev,
+						   &this->sd);
+		if (rval) {
+			dev_err(&client->dev,
+				"v4l2_device_register_subdev failed\n");
+			goto out_nvm_release;
+		}
+
+		last = this;
+	}
+
+	dev_dbg(&client->dev, "profile %d\n", sensor->minfo.smiapp_profile);
+
+	sensor->pixel_array->sd.entity.type = MEDIA_ENT_T_V4L2_SUBDEV_SENSOR;
+
+	/* final steps */
+	smiapp_read_frame_fmt(sensor);
+	rval = smiapp_init_controls(sensor);
+	if (rval < 0)
+		goto out_nvm_release;
+
+	rval = smiapp_update_mode(sensor);
+	if (rval) {
+		dev_err(&client->dev, "update mode failed\n");
+		goto out_nvm_release;
+	}
+
+	sensor->streaming = false;
+	sensor->dev_init_done = true;
+
+	/* check flash capability */
+	rval = smiapp_read(sensor, SMIAPP_REG_U8_FLASH_MODE_CAPABILITY, &tmp);
+	sensor->flash_capability = tmp;
+	if (rval)
+		goto out_nvm_release;
+
+	smiapp_power_off(sensor);
+
+	return 0;
+
+out_nvm_release:
+	device_remove_file(&client->dev, &dev_attr_nvm);
+
+out_power_off:
+	kfree(sensor->nvm);
+	sensor->nvm = NULL;
+	smiapp_power_off(sensor);
+
+out_smiapp_power_on:
+	if (sensor->platform_data->xshutdown != SMIAPP_NO_XSHUTDOWN)
+		gpio_free(sensor->platform_data->xshutdown);
+
+out_clk_set_rate:
+	clk_put(sensor->ext_clk);
+	sensor->ext_clk = NULL;
+
+out_clk_get:
+	regulator_put(sensor->vana);
+	sensor->vana = NULL;
+	return rval;
+}
+
+static int smiapp_open(struct v4l2_subdev *sd, struct v4l2_subdev_fh *fh)
+{
+	struct smiapp_subdev *ssd = to_smiapp_subdev(sd);
+	struct smiapp_sensor *sensor = ssd->sensor;
+	u32 mbus_code =
+		smiapp_csi_data_formats[smiapp_pixel_order(sensor)].code;
+	unsigned int i;
+
+	mutex_lock(&sensor->mutex);
+
+	for (i = 0; i < ssd->npads; i++) {
+		struct v4l2_mbus_framefmt *try_fmt =
+			v4l2_subdev_get_try_format(fh, i);
+		struct v4l2_rect *try_crop = v4l2_subdev_get_try_crop(fh, i);
+		struct v4l2_rect *try_comp;
+
+		try_fmt->width = sensor->limits[SMIAPP_LIMIT_X_ADDR_MAX] + 1;
+		try_fmt->height = sensor->limits[SMIAPP_LIMIT_Y_ADDR_MAX] + 1;
+		try_fmt->code = mbus_code;
+
+		try_crop->top = 0;
+		try_crop->left = 0;
+		try_crop->width = try_fmt->width;
+		try_crop->height = try_fmt->height;
+
+		if (ssd != sensor->pixel_array)
+			continue;
+
+		try_comp = v4l2_subdev_get_try_compose(fh, i);
+		*try_comp = *try_crop;
+	}
+
+	mutex_unlock(&sensor->mutex);
+
+	return smiapp_set_power(sd, 1);
+}
+
+static int smiapp_close(struct v4l2_subdev *sd, struct v4l2_subdev_fh *fh)
+{
+	return smiapp_set_power(sd, 0);
+}
+
+static const struct v4l2_subdev_video_ops smiapp_video_ops = {
+	.s_stream = smiapp_set_stream,
+};
+
+static const struct v4l2_subdev_core_ops smiapp_core_ops = {
+	.s_power = smiapp_set_power,
+};
+
+static const struct v4l2_subdev_pad_ops smiapp_pad_ops = {
+	.enum_mbus_code = smiapp_enum_mbus_code,
+	.get_fmt = smiapp_get_format,
+	.set_fmt = smiapp_set_format,
+	.get_selection = smiapp_get_selection,
+	.set_selection = smiapp_set_selection,
+};
+
+static const struct v4l2_subdev_sensor_ops smiapp_sensor_ops = {
+	.g_skip_frames = smiapp_get_skip_frames,
+};
+
+static const struct v4l2_subdev_ops smiapp_ops = {
+	.core = &smiapp_core_ops,
+	.video = &smiapp_video_ops,
+	.pad = &smiapp_pad_ops,
+	.sensor = &smiapp_sensor_ops,
+};
+
+static const struct media_entity_operations smiapp_entity_ops = {
+	.link_validate = v4l2_subdev_link_validate,
+};
+
+static const struct v4l2_subdev_internal_ops smiapp_internal_src_ops = {
+	.registered = smiapp_registered,
+	.open = smiapp_open,
+	.close = smiapp_close,
+};
+
+static const struct v4l2_subdev_internal_ops smiapp_internal_ops = {
+	.open = smiapp_open,
+	.close = smiapp_close,
+};
+
+/* -----------------------------------------------------------------------------
+ * I2C Driver
+ */
+
+#ifdef CONFIG_PM
+
+static int smiapp_suspend(struct device *dev)
+{
+	struct i2c_client *client = to_i2c_client(dev);
+	struct v4l2_subdev *subdev = i2c_get_clientdata(client);
+	struct smiapp_sensor *sensor = to_smiapp_sensor(subdev);
+	bool streaming;
+
+	BUG_ON(mutex_is_locked(&sensor->mutex));
+
+	if (sensor->power_count == 0)
+		return 0;
+
+	if (sensor->streaming)
+		smiapp_stop_streaming(sensor);
+
+	streaming = sensor->streaming;
+
+	smiapp_power_off(sensor);
+
+	/* save state for resume */
+	sensor->streaming = streaming;
+
+	return 0;
+}
+
+static int smiapp_resume(struct device *dev)
+{
+	struct i2c_client *client = to_i2c_client(dev);
+	struct v4l2_subdev *subdev = i2c_get_clientdata(client);
+	struct smiapp_sensor *sensor = to_smiapp_sensor(subdev);
+	int rval;
+
+	if (sensor->power_count == 0)
+		return 0;
+
+	rval = smiapp_power_on(sensor);
+	if (rval)
+		return rval;
+
+	if (sensor->streaming)
+		rval = smiapp_start_streaming(sensor);
+
+	return rval;
+}
+
+#else
+
+#define smiapp_suspend	NULL
+#define smiapp_resume	NULL
+
+#endif /* CONFIG_PM */
+
+static int smiapp_probe(struct i2c_client *client,
+			const struct i2c_device_id *devid)
+{
+	struct smiapp_sensor *sensor;
+	int rval;
+
+	if (client->dev.platform_data == NULL)
+		return -ENODEV;
+
+	sensor = kzalloc(sizeof(*sensor), GFP_KERNEL);
+	if (sensor == NULL)
+		return -ENOMEM;
+
+	sensor->platform_data = client->dev.platform_data;
+	mutex_init(&sensor->mutex);
+	mutex_init(&sensor->power_mutex);
+	sensor->src = &sensor->ssds[sensor->ssds_used];
+
+	v4l2_i2c_subdev_init(&sensor->src->sd, client, &smiapp_ops);
+	sensor->src->sd.internal_ops = &smiapp_internal_src_ops;
+	sensor->src->sd.flags |= V4L2_SUBDEV_FL_HAS_DEVNODE;
+	sensor->src->sensor = sensor;
+
+	sensor->src->pads[0].flags = MEDIA_PAD_FL_SOURCE;
+	rval = media_entity_init(&sensor->src->sd.entity, 2,
+				 sensor->src->pads, 0);
+	if (rval < 0)
+		kfree(sensor);
+
+	return rval;
+}
+
+static int __exit smiapp_remove(struct i2c_client *client)
+{
+	struct v4l2_subdev *subdev = i2c_get_clientdata(client);
+	struct smiapp_sensor *sensor = to_smiapp_sensor(subdev);
+	unsigned int i;
+
+	if (sensor->power_count) {
+		if (sensor->platform_data->xshutdown != SMIAPP_NO_XSHUTDOWN)
+			gpio_set_value(sensor->platform_data->xshutdown, 0);
+		if (sensor->platform_data->set_xclk)
+			sensor->platform_data->set_xclk(&sensor->src->sd, 0);
+		else
+			clk_disable(sensor->ext_clk);
+		sensor->power_count = 0;
+	}
+
+	if (sensor->nvm) {
+		device_remove_file(&client->dev, &dev_attr_nvm);
+		kfree(sensor->nvm);
+	}
+
+	for (i = 0; i < sensor->ssds_used; i++) {
+		media_entity_cleanup(&sensor->ssds[i].sd.entity);
+		v4l2_device_unregister_subdev(&sensor->ssds[i].sd);
+	}
+	smiapp_free_controls(sensor);
+	if (sensor->platform_data->xshutdown != SMIAPP_NO_XSHUTDOWN)
+		gpio_free(sensor->platform_data->xshutdown);
+	if (sensor->ext_clk)
+		clk_put(sensor->ext_clk);
+	if (sensor->vana)
+		regulator_put(sensor->vana);
+
+	kfree(sensor);
+
+	return 0;
+}
+
+static const struct i2c_device_id smiapp_id_table[] = {
+	{ SMIAPP_NAME, 0 },
+	{ },
+};
+MODULE_DEVICE_TABLE(i2c, smiapp_id_table);
+
+static const struct dev_pm_ops smiapp_pm_ops = {
+	.suspend	= smiapp_suspend,
+	.resume		= smiapp_resume,
+};
+
+static struct i2c_driver smiapp_i2c_driver = {
+	.driver	= {
+		.name = SMIAPP_NAME,
+		.pm = &smiapp_pm_ops,
+	},
+	.probe	= smiapp_probe,
+	.remove	= __exit_p(smiapp_remove),
+	.id_table = smiapp_id_table,
+};
+
+module_i2c_driver(smiapp_i2c_driver);
+
+MODULE_AUTHOR("Sakari Ailus <sakari.ailus@maxwell.research.nokia.com>");
+MODULE_DESCRIPTION("Generic SMIA/SMIA++ camera module driver");
+MODULE_LICENSE("GPL");
diff --git a/drivers/media/video/smiapp/smiapp-limits.c b/drivers/media/video/smiapp/smiapp-limits.c
new file mode 100644
index 00000000000..0800e095724
--- /dev/null
+++ b/drivers/media/video/smiapp/smiapp-limits.c
@@ -0,0 +1,132 @@
+/*
+ * drivers/media/video/smiapp/smiapp-limits.c
+ *
+ * Generic driver for SMIA/SMIA++ compliant camera modules
+ *
+ * Copyright (C) 2011--2012 Nokia Corporation
+ * Contact: Sakari Ailus <sakari.ailus@maxwell.research.nokia.com>
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * version 2 as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+ * General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA
+ * 02110-1301 USA
+ *
+ */
+
+#include "smiapp.h"
+
+struct smiapp_reg_limits smiapp_reg_limits[] = {
+	{ SMIAPP_REG_U16_ANALOGUE_GAIN_CAPABILITY, "analogue_gain_capability" }, /* 0 */
+	{ SMIAPP_REG_U16_ANALOGUE_GAIN_CODE_MIN, "analogue_gain_code_min" },
+	{ SMIAPP_REG_U16_ANALOGUE_GAIN_CODE_MAX, "analogue_gain_code_max" },
+	{ SMIAPP_REG_U8_THS_ZERO_MIN, "ths_zero_min" },
+	{ SMIAPP_REG_U8_TCLK_TRAIL_MIN, "tclk_trail_min" },
+	{ SMIAPP_REG_U16_INTEGRATION_TIME_CAPABILITY, "integration_time_capability" }, /* 5 */
+	{ SMIAPP_REG_U16_COARSE_INTEGRATION_TIME_MIN, "coarse_integration_time_min" },
+	{ SMIAPP_REG_U16_COARSE_INTEGRATION_TIME_MAX_MARGIN, "coarse_integration_time_max_margin" },
+	{ SMIAPP_REG_U16_FINE_INTEGRATION_TIME_MIN, "fine_integration_time_min" },
+	{ SMIAPP_REG_U16_FINE_INTEGRATION_TIME_MAX_MARGIN, "fine_integration_time_max_margin" },
+	{ SMIAPP_REG_U16_DIGITAL_GAIN_CAPABILITY, "digital_gain_capability" }, /* 10 */
+	{ SMIAPP_REG_U16_DIGITAL_GAIN_MIN, "digital_gain_min" },
+	{ SMIAPP_REG_U16_DIGITAL_GAIN_MAX, "digital_gain_max" },
+	{ SMIAPP_REG_F32_MIN_EXT_CLK_FREQ_HZ, "min_ext_clk_freq_hz" },
+	{ SMIAPP_REG_F32_MAX_EXT_CLK_FREQ_HZ, "max_ext_clk_freq_hz" },
+	{ SMIAPP_REG_U16_MIN_PRE_PLL_CLK_DIV, "min_pre_pll_clk_div" }, /* 15 */
+	{ SMIAPP_REG_U16_MAX_PRE_PLL_CLK_DIV, "max_pre_pll_clk_div" },
+	{ SMIAPP_REG_F32_MIN_PLL_IP_FREQ_HZ, "min_pll_ip_freq_hz" },
+	{ SMIAPP_REG_F32_MAX_PLL_IP_FREQ_HZ, "max_pll_ip_freq_hz" },
+	{ SMIAPP_REG_U16_MIN_PLL_MULTIPLIER, "min_pll_multiplier" },
+	{ SMIAPP_REG_U16_MAX_PLL_MULTIPLIER, "max_pll_multiplier" }, /* 20 */
+	{ SMIAPP_REG_F32_MIN_PLL_OP_FREQ_HZ, "min_pll_op_freq_hz" },
+	{ SMIAPP_REG_F32_MAX_PLL_OP_FREQ_HZ, "max_pll_op_freq_hz" },
+	{ SMIAPP_REG_U16_MIN_VT_SYS_CLK_DIV, "min_vt_sys_clk_div" },
+	{ SMIAPP_REG_U16_MAX_VT_SYS_CLK_DIV, "max_vt_sys_clk_div" },
+	{ SMIAPP_REG_F32_MIN_VT_SYS_CLK_FREQ_HZ, "min_vt_sys_clk_freq_hz" }, /* 25 */
+	{ SMIAPP_REG_F32_MAX_VT_SYS_CLK_FREQ_HZ, "max_vt_sys_clk_freq_hz" },
+	{ SMIAPP_REG_F32_MIN_VT_PIX_CLK_FREQ_HZ, "min_vt_pix_clk_freq_hz" },
+	{ SMIAPP_REG_F32_MAX_VT_PIX_CLK_FREQ_HZ, "max_vt_pix_clk_freq_hz" },
+	{ SMIAPP_REG_U16_MIN_VT_PIX_CLK_DIV, "min_vt_pix_clk_div" },
+	{ SMIAPP_REG_U16_MAX_VT_PIX_CLK_DIV, "max_vt_pix_clk_div" }, /* 30 */
+	{ SMIAPP_REG_U16_MIN_FRAME_LENGTH_LINES, "min_frame_length_lines" },
+	{ SMIAPP_REG_U16_MAX_FRAME_LENGTH_LINES, "max_frame_length_lines" },
+	{ SMIAPP_REG_U16_MIN_LINE_LENGTH_PCK, "min_line_length_pck" },
+	{ SMIAPP_REG_U16_MAX_LINE_LENGTH_PCK, "max_line_length_pck" },
+	{ SMIAPP_REG_U16_MIN_LINE_BLANKING_PCK, "min_line_blanking_pck" }, /* 35 */
+	{ SMIAPP_REG_U16_MIN_FRAME_BLANKING_LINES, "min_frame_blanking_lines" },
+	{ SMIAPP_REG_U8_MIN_LINE_LENGTH_PCK_STEP_SIZE, "min_line_length_pck_step_size" },
+	{ SMIAPP_REG_U16_MIN_OP_SYS_CLK_DIV, "min_op_sys_clk_div" },
+	{ SMIAPP_REG_U16_MAX_OP_SYS_CLK_DIV, "max_op_sys_clk_div" },
+	{ SMIAPP_REG_F32_MIN_OP_SYS_CLK_FREQ_HZ, "min_op_sys_clk_freq_hz" }, /* 40 */
+	{ SMIAPP_REG_F32_MAX_OP_SYS_CLK_FREQ_HZ, "max_op_sys_clk_freq_hz" },
+	{ SMIAPP_REG_U16_MIN_OP_PIX_CLK_DIV, "min_op_pix_clk_div" },
+	{ SMIAPP_REG_U16_MAX_OP_PIX_CLK_DIV, "max_op_pix_clk_div" },
+	{ SMIAPP_REG_F32_MIN_OP_PIX_CLK_FREQ_HZ, "min_op_pix_clk_freq_hz" },
+	{ SMIAPP_REG_F32_MAX_OP_PIX_CLK_FREQ_HZ, "max_op_pix_clk_freq_hz" }, /* 45 */
+	{ SMIAPP_REG_U16_X_ADDR_MIN, "x_addr_min" },
+	{ SMIAPP_REG_U16_Y_ADDR_MIN, "y_addr_min" },
+	{ SMIAPP_REG_U16_X_ADDR_MAX, "x_addr_max" },
+	{ SMIAPP_REG_U16_Y_ADDR_MAX, "y_addr_max" },
+	{ SMIAPP_REG_U16_MIN_X_OUTPUT_SIZE, "min_x_output_size" }, /* 50 */
+	{ SMIAPP_REG_U16_MIN_Y_OUTPUT_SIZE, "min_y_output_size" },
+	{ SMIAPP_REG_U16_MAX_X_OUTPUT_SIZE, "max_x_output_size" },
+	{ SMIAPP_REG_U16_MAX_Y_OUTPUT_SIZE, "max_y_output_size" },
+	{ SMIAPP_REG_U16_MIN_EVEN_INC, "min_even_inc" },
+	{ SMIAPP_REG_U16_MAX_EVEN_INC, "max_even_inc" }, /* 55 */
+	{ SMIAPP_REG_U16_MIN_ODD_INC, "min_odd_inc" },
+	{ SMIAPP_REG_U16_MAX_ODD_INC, "max_odd_inc" },
+	{ SMIAPP_REG_U16_SCALING_CAPABILITY, "scaling_capability" },
+	{ SMIAPP_REG_U16_SCALER_M_MIN, "scaler_m_min" },
+	{ SMIAPP_REG_U16_SCALER_M_MAX, "scaler_m_max" }, /* 60 */
+	{ SMIAPP_REG_U16_SCALER_N_MIN, "scaler_n_min" },
+	{ SMIAPP_REG_U16_SCALER_N_MAX, "scaler_n_max" },
+	{ SMIAPP_REG_U16_SPATIAL_SAMPLING_CAPABILITY, "spatial_sampling_capability" },
+	{ SMIAPP_REG_U8_DIGITAL_CROP_CAPABILITY, "digital_crop_capability" },
+	{ SMIAPP_REG_U16_COMPRESSION_CAPABILITY, "compression_capability" }, /* 65 */
+	{ SMIAPP_REG_U8_FIFO_SUPPORT_CAPABILITY, "fifo_support_capability" },
+	{ SMIAPP_REG_U8_DPHY_CTRL_CAPABILITY, "dphy_ctrl_capability" },
+	{ SMIAPP_REG_U8_CSI_LANE_MODE_CAPABILITY, "csi_lane_mode_capability" },
+	{ SMIAPP_REG_U8_CSI_SIGNALLING_MODE_CAPABILITY, "csi_signalling_mode_capability" },
+	{ SMIAPP_REG_U8_FAST_STANDBY_CAPABILITY, "fast_standby_capability" }, /* 70 */
+	{ SMIAPP_REG_U8_CCI_ADDRESS_CONTROL_CAPABILITY, "cci_address_control_capability" },
+	{ SMIAPP_REG_U32_MAX_PER_LANE_BITRATE_1_LANE_MODE_MBPS, "max_per_lane_bitrate_1_lane_mode_mbps" },
+	{ SMIAPP_REG_U32_MAX_PER_LANE_BITRATE_2_LANE_MODE_MBPS, "max_per_lane_bitrate_2_lane_mode_mbps" },
+	{ SMIAPP_REG_U32_MAX_PER_LANE_BITRATE_3_LANE_MODE_MBPS, "max_per_lane_bitrate_3_lane_mode_mbps" },
+	{ SMIAPP_REG_U32_MAX_PER_LANE_BITRATE_4_LANE_MODE_MBPS, "max_per_lane_bitrate_4_lane_mode_mbps" }, /* 75 */
+	{ SMIAPP_REG_U8_TEMP_SENSOR_CAPABILITY, "temp_sensor_capability" },
+	{ SMIAPP_REG_U16_MIN_FRAME_LENGTH_LINES_BIN, "min_frame_length_lines_bin" },
+	{ SMIAPP_REG_U16_MAX_FRAME_LENGTH_LINES_BIN, "max_frame_length_lines_bin" },
+	{ SMIAPP_REG_U16_MIN_LINE_LENGTH_PCK_BIN, "min_line_length_pck_bin" },
+	{ SMIAPP_REG_U16_MAX_LINE_LENGTH_PCK_BIN, "max_line_length_pck_bin" }, /* 80 */
+	{ SMIAPP_REG_U16_MIN_LINE_BLANKING_PCK_BIN, "min_line_blanking_pck_bin" },
+	{ SMIAPP_REG_U16_FINE_INTEGRATION_TIME_MIN_BIN, "fine_integration_time_min_bin" },
+	{ SMIAPP_REG_U16_FINE_INTEGRATION_TIME_MAX_MARGIN_BIN, "fine_integration_time_max_margin_bin" },
+	{ SMIAPP_REG_U8_BINNING_CAPABILITY, "binning_capability" },
+	{ SMIAPP_REG_U8_BINNING_WEIGHTING_CAPABILITY, "binning_weighting_capability" }, /* 85 */
+	{ SMIAPP_REG_U8_DATA_TRANSFER_IF_CAPABILITY, "data_transfer_if_capability" },
+	{ SMIAPP_REG_U8_SHADING_CORRECTION_CAPABILITY, "shading_correction_capability" },
+	{ SMIAPP_REG_U8_GREEN_IMBALANCE_CAPABILITY, "green_imbalance_capability" },
+	{ SMIAPP_REG_U8_BLACK_LEVEL_CAPABILITY, "black_level_capability" },
+	{ SMIAPP_REG_U8_MODULE_SPECIFIC_CORRECTION_CAPABILITY, "module_specific_correction_capability" }, /* 90 */
+	{ SMIAPP_REG_U16_DEFECT_CORRECTION_CAPABILITY, "defect_correction_capability" },
+	{ SMIAPP_REG_U16_DEFECT_CORRECTION_CAPABILITY_2, "defect_correction_capability_2" },
+	{ SMIAPP_REG_U8_EDOF_CAPABILITY, "edof_capability" },
+	{ SMIAPP_REG_U8_COLOUR_FEEDBACK_CAPABILITY, "colour_feedback_capability" },
+	{ SMIAPP_REG_U8_ESTIMATION_MODE_CAPABILITY, "estimation_mode_capability" }, /* 95 */
+	{ SMIAPP_REG_U8_ESTIMATION_ZONE_CAPABILITY, "estimation_zone_capability" },
+	{ SMIAPP_REG_U16_CAPABILITY_TRDY_MIN, "capability_trdy_min" },
+	{ SMIAPP_REG_U8_FLASH_MODE_CAPABILITY, "flash_mode_capability" },
+	{ SMIAPP_REG_U8_ACTUATOR_CAPABILITY, "actuator_capability" },
+	{ SMIAPP_REG_U8_BRACKETING_LUT_CAPABILITY_1, "bracketing_lut_capability_1" }, /* 100 */
+	{ SMIAPP_REG_U8_BRACKETING_LUT_CAPABILITY_2, "bracketing_lut_capability_2" },
+	{ SMIAPP_REG_U16_ANALOGUE_GAIN_CODE_STEP, "analogue_gain_code_step" },
+	{ 0, NULL },
+};
diff --git a/drivers/media/video/smiapp/smiapp-limits.h b/drivers/media/video/smiapp/smiapp-limits.h
new file mode 100644
index 00000000000..7f4836bb78d
--- /dev/null
+++ b/drivers/media/video/smiapp/smiapp-limits.h
@@ -0,0 +1,128 @@
+/*
+ * drivers/media/video/smiapp/smiapp-limits.h
+ *
+ * Generic driver for SMIA/SMIA++ compliant camera modules
+ *
+ * Copyright (C) 2011--2012 Nokia Corporation
+ * Contact: Sakari Ailus <sakari.ailus@maxwell.research.nokia.com>
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * version 2 as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+ * General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA
+ * 02110-1301 USA
+ *
+ */
+
+#define SMIAPP_LIMIT_ANALOGUE_GAIN_CAPABILITY			0
+#define SMIAPP_LIMIT_ANALOGUE_GAIN_CODE_MIN			1
+#define SMIAPP_LIMIT_ANALOGUE_GAIN_CODE_MAX			2
+#define SMIAPP_LIMIT_THS_ZERO_MIN				3
+#define SMIAPP_LIMIT_TCLK_TRAIL_MIN				4
+#define SMIAPP_LIMIT_INTEGRATION_TIME_CAPABILITY		5
+#define SMIAPP_LIMIT_COARSE_INTEGRATION_TIME_MIN		6
+#define SMIAPP_LIMIT_COARSE_INTEGRATION_TIME_MAX_MARGIN		7
+#define SMIAPP_LIMIT_FINE_INTEGRATION_TIME_MIN			8
+#define SMIAPP_LIMIT_FINE_INTEGRATION_TIME_MAX_MARGIN		9
+#define SMIAPP_LIMIT_DIGITAL_GAIN_CAPABILITY			10
+#define SMIAPP_LIMIT_DIGITAL_GAIN_MIN				11
+#define SMIAPP_LIMIT_DIGITAL_GAIN_MAX				12
+#define SMIAPP_LIMIT_MIN_EXT_CLK_FREQ_HZ			13
+#define SMIAPP_LIMIT_MAX_EXT_CLK_FREQ_HZ			14
+#define SMIAPP_LIMIT_MIN_PRE_PLL_CLK_DIV			15
+#define SMIAPP_LIMIT_MAX_PRE_PLL_CLK_DIV			16
+#define SMIAPP_LIMIT_MIN_PLL_IP_FREQ_HZ				17
+#define SMIAPP_LIMIT_MAX_PLL_IP_FREQ_HZ				18
+#define SMIAPP_LIMIT_MIN_PLL_MULTIPLIER				19
+#define SMIAPP_LIMIT_MAX_PLL_MULTIPLIER				20
+#define SMIAPP_LIMIT_MIN_PLL_OP_FREQ_HZ				21
+#define SMIAPP_LIMIT_MAX_PLL_OP_FREQ_HZ				22
+#define SMIAPP_LIMIT_MIN_VT_SYS_CLK_DIV				23
+#define SMIAPP_LIMIT_MAX_VT_SYS_CLK_DIV				24
+#define SMIAPP_LIMIT_MIN_VT_SYS_CLK_FREQ_HZ			25
+#define SMIAPP_LIMIT_MAX_VT_SYS_CLK_FREQ_HZ			26
+#define SMIAPP_LIMIT_MIN_VT_PIX_CLK_FREQ_HZ			27
+#define SMIAPP_LIMIT_MAX_VT_PIX_CLK_FREQ_HZ			28
+#define SMIAPP_LIMIT_MIN_VT_PIX_CLK_DIV				29
+#define SMIAPP_LIMIT_MAX_VT_PIX_CLK_DIV				30
+#define SMIAPP_LIMIT_MIN_FRAME_LENGTH_LINES			31
+#define SMIAPP_LIMIT_MAX_FRAME_LENGTH_LINES			32
+#define SMIAPP_LIMIT_MIN_LINE_LENGTH_PCK			33
+#define SMIAPP_LIMIT_MAX_LINE_LENGTH_PCK			34
+#define SMIAPP_LIMIT_MIN_LINE_BLANKING_PCK			35
+#define SMIAPP_LIMIT_MIN_FRAME_BLANKING_LINES			36
+#define SMIAPP_LIMIT_MIN_LINE_LENGTH_PCK_STEP_SIZE		37
+#define SMIAPP_LIMIT_MIN_OP_SYS_CLK_DIV				38
+#define SMIAPP_LIMIT_MAX_OP_SYS_CLK_DIV				39
+#define SMIAPP_LIMIT_MIN_OP_SYS_CLK_FREQ_HZ			40
+#define SMIAPP_LIMIT_MAX_OP_SYS_CLK_FREQ_HZ			41
+#define SMIAPP_LIMIT_MIN_OP_PIX_CLK_DIV				42
+#define SMIAPP_LIMIT_MAX_OP_PIX_CLK_DIV				43
+#define SMIAPP_LIMIT_MIN_OP_PIX_CLK_FREQ_HZ			44
+#define SMIAPP_LIMIT_MAX_OP_PIX_CLK_FREQ_HZ			45
+#define SMIAPP_LIMIT_X_ADDR_MIN					46
+#define SMIAPP_LIMIT_Y_ADDR_MIN					47
+#define SMIAPP_LIMIT_X_ADDR_MAX					48
+#define SMIAPP_LIMIT_Y_ADDR_MAX					49
+#define SMIAPP_LIMIT_MIN_X_OUTPUT_SIZE				50
+#define SMIAPP_LIMIT_MIN_Y_OUTPUT_SIZE				51
+#define SMIAPP_LIMIT_MAX_X_OUTPUT_SIZE				52
+#define SMIAPP_LIMIT_MAX_Y_OUTPUT_SIZE				53
+#define SMIAPP_LIMIT_MIN_EVEN_INC				54
+#define SMIAPP_LIMIT_MAX_EVEN_INC				55
+#define SMIAPP_LIMIT_MIN_ODD_INC				56
+#define SMIAPP_LIMIT_MAX_ODD_INC				57
+#define SMIAPP_LIMIT_SCALING_CAPABILITY				58
+#define SMIAPP_LIMIT_SCALER_M_MIN				59
+#define SMIAPP_LIMIT_SCALER_M_MAX				60
+#define SMIAPP_LIMIT_SCALER_N_MIN				61
+#define SMIAPP_LIMIT_SCALER_N_MAX				62
+#define SMIAPP_LIMIT_SPATIAL_SAMPLING_CAPABILITY		63
+#define SMIAPP_LIMIT_DIGITAL_CROP_CAPABILITY			64
+#define SMIAPP_LIMIT_COMPRESSION_CAPABILITY			65
+#define SMIAPP_LIMIT_FIFO_SUPPORT_CAPABILITY			66
+#define SMIAPP_LIMIT_DPHY_CTRL_CAPABILITY			67
+#define SMIAPP_LIMIT_CSI_LANE_MODE_CAPABILITY			68
+#define SMIAPP_LIMIT_CSI_SIGNALLING_MODE_CAPABILITY		69
+#define SMIAPP_LIMIT_FAST_STANDBY_CAPABILITY			70
+#define SMIAPP_LIMIT_CCI_ADDRESS_CONTROL_CAPABILITY		71
+#define SMIAPP_LIMIT_MAX_PER_LANE_BITRATE_1_LANE_MODE_MBPS	72
+#define SMIAPP_LIMIT_MAX_PER_LANE_BITRATE_2_LANE_MODE_MBPS	73
+#define SMIAPP_LIMIT_MAX_PER_LANE_BITRATE_3_LANE_MODE_MBPS	74
+#define SMIAPP_LIMIT_MAX_PER_LANE_BITRATE_4_LANE_MODE_MBPS	75
+#define SMIAPP_LIMIT_TEMP_SENSOR_CAPABILITY			76
+#define SMIAPP_LIMIT_MIN_FRAME_LENGTH_LINES_BIN			77
+#define SMIAPP_LIMIT_MAX_FRAME_LENGTH_LINES_BIN			78
+#define SMIAPP_LIMIT_MIN_LINE_LENGTH_PCK_BIN			79
+#define SMIAPP_LIMIT_MAX_LINE_LENGTH_PCK_BIN			80
+#define SMIAPP_LIMIT_MIN_LINE_BLANKING_PCK_BIN			81
+#define SMIAPP_LIMIT_FINE_INTEGRATION_TIME_MIN_BIN		82
+#define SMIAPP_LIMIT_FINE_INTEGRATION_TIME_MAX_MARGIN_BIN	83
+#define SMIAPP_LIMIT_BINNING_CAPABILITY				84
+#define SMIAPP_LIMIT_BINNING_WEIGHTING_CAPABILITY		85
+#define SMIAPP_LIMIT_DATA_TRANSFER_IF_CAPABILITY		86
+#define SMIAPP_LIMIT_SHADING_CORRECTION_CAPABILITY		87
+#define SMIAPP_LIMIT_GREEN_IMBALANCE_CAPABILITY			88
+#define SMIAPP_LIMIT_BLACK_LEVEL_CAPABILITY			89
+#define SMIAPP_LIMIT_MODULE_SPECIFIC_CORRECTION_CAPABILITY	90
+#define SMIAPP_LIMIT_DEFECT_CORRECTION_CAPABILITY		91
+#define SMIAPP_LIMIT_DEFECT_CORRECTION_CAPABILITY_2		92
+#define SMIAPP_LIMIT_EDOF_CAPABILITY				93
+#define SMIAPP_LIMIT_COLOUR_FEEDBACK_CAPABILITY			94
+#define SMIAPP_LIMIT_ESTIMATION_MODE_CAPABILITY			95
+#define SMIAPP_LIMIT_ESTIMATION_ZONE_CAPABILITY			96
+#define SMIAPP_LIMIT_CAPABILITY_TRDY_MIN			97
+#define SMIAPP_LIMIT_FLASH_MODE_CAPABILITY			98
+#define SMIAPP_LIMIT_ACTUATOR_CAPABILITY			99
+#define SMIAPP_LIMIT_BRACKETING_LUT_CAPABILITY_1		100
+#define SMIAPP_LIMIT_BRACKETING_LUT_CAPABILITY_2		101
+#define SMIAPP_LIMIT_ANALOGUE_GAIN_CODE_STEP			102
+#define SMIAPP_LIMIT_LAST					103
diff --git a/drivers/media/video/smiapp/smiapp-quirk.c b/drivers/media/video/smiapp/smiapp-quirk.c
new file mode 100644
index 00000000000..55e87950dce
--- /dev/null
+++ b/drivers/media/video/smiapp/smiapp-quirk.c
@@ -0,0 +1,306 @@
+/*
+ * drivers/media/video/smiapp/smiapp-quirk.c
+ *
+ * Generic driver for SMIA/SMIA++ compliant camera modules
+ *
+ * Copyright (C) 2011--2012 Nokia Corporation
+ * Contact: Sakari Ailus <sakari.ailus@maxwell.research.nokia.com>
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * version 2 as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+ * General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA
+ * 02110-1301 USA
+ *
+ */
+
+#include <linux/delay.h>
+
+#include "smiapp.h"
+
+static int smiapp_write_8(struct smiapp_sensor *sensor, u16 reg, u8 val)
+{
+	return smiapp_write(sensor, (SMIA_REG_8BIT << 16) | reg, val);
+}
+
+static int smiapp_write_8s(struct smiapp_sensor *sensor,
+			   struct smiapp_reg_8 *regs, int len)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(&sensor->src->sd);
+	int rval;
+
+	for (; len > 0; len--, regs++) {
+		rval = smiapp_write_8(sensor, regs->reg, regs->val);
+		if (rval < 0) {
+			dev_err(&client->dev,
+				"error %d writing reg 0x%4.4x, val 0x%2.2x",
+				rval, regs->reg, regs->val);
+			return rval;
+		}
+	}
+
+	return 0;
+}
+
+void smiapp_replace_limit(struct smiapp_sensor *sensor,
+			  u32 limit, u32 val)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(&sensor->src->sd);
+
+	dev_dbg(&client->dev, "quirk: 0x%8.8x \"%s\" = %d, 0x%x\n",
+		smiapp_reg_limits[limit].addr,
+		smiapp_reg_limits[limit].what, val, val);
+	sensor->limits[limit] = val;
+}
+
+int smiapp_replace_limit_at(struct smiapp_sensor *sensor,
+			    u32 reg, u32 val)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(&sensor->src->sd);
+	int i;
+
+	for (i = 0; smiapp_reg_limits[i].addr; i++) {
+		if ((smiapp_reg_limits[i].addr & 0xffff) != reg)
+			continue;
+
+		smiapp_replace_limit(sensor, i, val);
+
+		return 0;
+	}
+
+	dev_dbg(&client->dev, "quirk: bad register 0x%4.4x\n", reg);
+
+	return -EINVAL;
+}
+
+bool smiapp_quirk_reg(struct smiapp_sensor *sensor,
+		      u32 reg, u32 *val)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(&sensor->src->sd);
+	const struct smia_reg *sreg;
+
+	if (!sensor->minfo.quirk)
+		return false;
+
+	sreg = sensor->minfo.quirk->regs;
+
+	if (!sreg)
+		return false;
+
+	while (sreg->type) {
+		u16 type = reg >> 16;
+		u16 reg16 = reg;
+
+		if (sreg->type != type || sreg->reg != reg16) {
+			sreg++;
+			continue;
+		}
+
+		switch ((u8)type) {
+		case SMIA_REG_8BIT:
+			dev_dbg(&client->dev, "quirk: 0x%8.8x: 0x%2.2x\n",
+				reg, sreg->val);
+			break;
+		case SMIA_REG_16BIT:
+			dev_dbg(&client->dev, "quirk: 0x%8.8x: 0x%4.4x\n",
+				reg, sreg->val);
+			break;
+		case SMIA_REG_32BIT:
+			dev_dbg(&client->dev, "quirk: 0x%8.8x: 0x%8.8x\n",
+				reg, sreg->val);
+			break;
+		}
+
+		*val = sreg->val;
+
+		return true;
+	}
+
+	return false;
+}
+
+static int jt8ew9_limits(struct smiapp_sensor *sensor)
+{
+	if (sensor->minfo.revision_number_major < 0x03)
+		sensor->frame_skip = 1;
+
+	/* Below 24 gain doesn't have effect at all, */
+	/* but ~59 is needed for full dynamic range */
+	smiapp_replace_limit(sensor, SMIAPP_LIMIT_ANALOGUE_GAIN_CODE_MIN, 59);
+	smiapp_replace_limit(
+		sensor, SMIAPP_LIMIT_ANALOGUE_GAIN_CODE_MAX, 6000);
+
+	return 0;
+}
+
+static int jt8ew9_post_poweron(struct smiapp_sensor *sensor)
+{
+	struct smiapp_reg_8 regs[] = {
+		{ 0x30a3, 0xd8 }, /* Output port control : LVDS ports only */
+		{ 0x30ae, 0x00 }, /* 0x0307 pll_multiplier maximum value on PLL input 9.6MHz ( 19.2MHz is divided on pre_pll_div) */
+		{ 0x30af, 0xd0 }, /* 0x0307 pll_multiplier maximum value on PLL input 9.6MHz ( 19.2MHz is divided on pre_pll_div) */
+		{ 0x322d, 0x04 }, /* Adjusting Processing Image Size to Scaler Toshiba Recommendation Setting */
+		{ 0x3255, 0x0f }, /* Horizontal Noise Reduction Control Toshiba Recommendation Setting */
+		{ 0x3256, 0x15 }, /* Horizontal Noise Reduction Control Toshiba Recommendation Setting */
+		{ 0x3258, 0x70 }, /* Analog Gain Control Toshiba Recommendation Setting */
+		{ 0x3259, 0x70 }, /* Analog Gain Control Toshiba Recommendation Setting */
+		{ 0x325f, 0x7c }, /* Analog Gain Control Toshiba Recommendation Setting */
+		{ 0x3302, 0x06 }, /* Pixel Reference Voltage Control Toshiba Recommendation Setting */
+		{ 0x3304, 0x00 }, /* Pixel Reference Voltage Control Toshiba Recommendation Setting */
+		{ 0x3307, 0x22 }, /* Pixel Reference Voltage Control Toshiba Recommendation Setting */
+		{ 0x3308, 0x8d }, /* Pixel Reference Voltage Control Toshiba Recommendation Setting */
+		{ 0x331e, 0x0f }, /* Black Hole Sun Correction Control Toshiba Recommendation Setting */
+		{ 0x3320, 0x30 }, /* Black Hole Sun Correction Control Toshiba Recommendation Setting */
+		{ 0x3321, 0x11 }, /* Black Hole Sun Correction Control Toshiba Recommendation Setting */
+		{ 0x3322, 0x98 }, /* Black Hole Sun Correction Control Toshiba Recommendation Setting */
+		{ 0x3323, 0x64 }, /* Black Hole Sun Correction Control Toshiba Recommendation Setting */
+		{ 0x3325, 0x83 }, /* Read Out Timing Control Toshiba Recommendation Setting */
+		{ 0x3330, 0x18 }, /* Read Out Timing Control Toshiba Recommendation Setting */
+		{ 0x333c, 0x01 }, /* Read Out Timing Control Toshiba Recommendation Setting */
+		{ 0x3345, 0x2f }, /* Black Hole Sun Correction Control Toshiba Recommendation Setting */
+		{ 0x33de, 0x38 }, /* Horizontal Noise Reduction Control Toshiba Recommendation Setting */
+		/* Taken from v03. No idea what the rest are. */
+		{ 0x32e0, 0x05 },
+		{ 0x32e1, 0x05 },
+		{ 0x32e2, 0x04 },
+		{ 0x32e5, 0x04 },
+		{ 0x32e6, 0x04 },
+
+	};
+
+	return smiapp_write_8s(sensor, regs, ARRAY_SIZE(regs));
+}
+
+const struct smiapp_quirk smiapp_jt8ew9_quirk = {
+	.limits = jt8ew9_limits,
+	.post_poweron = jt8ew9_post_poweron,
+};
+
+static int imx125es_post_poweron(struct smiapp_sensor *sensor)
+{
+	/* Taken from v02. No idea what the other two are. */
+	struct smiapp_reg_8 regs[] = {
+		/*
+		 * 0x3302: clk during frame blanking:
+		 * 0x00 - HS mode, 0x01 - LP11
+		 */
+		{ 0x3302, 0x01 },
+		{ 0x302d, 0x00 },
+		{ 0x3b08, 0x8c },
+	};
+
+	return smiapp_write_8s(sensor, regs, ARRAY_SIZE(regs));
+}
+
+const struct smiapp_quirk smiapp_imx125es_quirk = {
+	.post_poweron = imx125es_post_poweron,
+};
+
+static int jt8ev1_limits(struct smiapp_sensor *sensor)
+{
+	smiapp_replace_limit(sensor, SMIAPP_LIMIT_X_ADDR_MAX, 4271);
+	smiapp_replace_limit(sensor,
+			     SMIAPP_LIMIT_MIN_LINE_BLANKING_PCK_BIN, 184);
+
+	return 0;
+}
+
+static int jt8ev1_post_poweron(struct smiapp_sensor *sensor)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(&sensor->src->sd);
+	int rval;
+
+	struct smiapp_reg_8 regs[] = {
+		{ 0x3031, 0xcd }, /* For digital binning (EQ_MONI) */
+		{ 0x30a3, 0xd0 }, /* FLASH STROBE enable */
+		{ 0x3237, 0x00 }, /* For control of pulse timing for ADC */
+		{ 0x3238, 0x43 },
+		{ 0x3301, 0x06 }, /* For analog bias for sensor */
+		{ 0x3302, 0x06 },
+		{ 0x3304, 0x00 },
+		{ 0x3305, 0x88 },
+		{ 0x332a, 0x14 },
+		{ 0x332c, 0x6b },
+		{ 0x3336, 0x01 },
+		{ 0x333f, 0x1f },
+		{ 0x3355, 0x00 },
+		{ 0x3356, 0x20 },
+		{ 0x33bf, 0x20 }, /* Adjust the FBC speed */
+		{ 0x33c9, 0x20 },
+		{ 0x33ce, 0x30 }, /* Adjust the parameter for logic function */
+		{ 0x33cf, 0xec }, /* For Black sun */
+		{ 0x3328, 0x80 }, /* Ugh. No idea what's this. */
+	};
+
+	struct smiapp_reg_8 regs_96[] = {
+		{ 0x30ae, 0x00 }, /* For control of ADC clock */
+		{ 0x30af, 0xd0 },
+		{ 0x30b0, 0x01 },
+	};
+
+	rval = smiapp_write_8s(sensor, regs, ARRAY_SIZE(regs));
+	if (rval < 0)
+		return rval;
+
+	switch (sensor->platform_data->ext_clk) {
+	case 9600000:
+		return smiapp_write_8s(sensor, regs_96,
+				       ARRAY_SIZE(regs_96));
+	default:
+		dev_warn(&client->dev, "no MSRs for %d Hz ext_clk\n",
+			 sensor->platform_data->ext_clk);
+		return 0;
+	}
+}
+
+static int jt8ev1_pre_streamon(struct smiapp_sensor *sensor)
+{
+	return smiapp_write_8(sensor, 0x3328, 0x00);
+}
+
+static int jt8ev1_post_streamoff(struct smiapp_sensor *sensor)
+{
+	int rval;
+
+	/* Workaround: allows fast standby to work properly */
+	rval = smiapp_write_8(sensor, 0x3205, 0x04);
+	if (rval < 0)
+		return rval;
+
+	/* Wait for 1 ms + one line => 2 ms is likely enough */
+	usleep_range(2000, 2000);
+
+	/* Restore it */
+	rval = smiapp_write_8(sensor, 0x3205, 0x00);
+	if (rval < 0)
+		return rval;
+
+	return smiapp_write_8(sensor, 0x3328, 0x80);
+}
+
+const struct smiapp_quirk smiapp_jt8ev1_quirk = {
+	.limits = jt8ev1_limits,
+	.post_poweron = jt8ev1_post_poweron,
+	.pre_streamon = jt8ev1_pre_streamon,
+	.post_streamoff = jt8ev1_post_streamoff,
+	.flags = SMIAPP_QUIRK_FLAG_OP_PIX_CLOCK_PER_LANE,
+};
+
+static int tcm8500md_limits(struct smiapp_sensor *sensor)
+{
+	smiapp_replace_limit(sensor, SMIAPP_LIMIT_MIN_PLL_IP_FREQ_HZ, 2700000);
+
+	return 0;
+}
+
+const struct smiapp_quirk smiapp_tcm8500md_quirk = {
+	.limits = tcm8500md_limits,
+};
diff --git a/drivers/media/video/smiapp/smiapp-quirk.h b/drivers/media/video/smiapp/smiapp-quirk.h
new file mode 100644
index 00000000000..f4dcaabaefe
--- /dev/null
+++ b/drivers/media/video/smiapp/smiapp-quirk.h
@@ -0,0 +1,83 @@
+/*
+ * drivers/media/video/smiapp/smiapp-quirk.h
+ *
+ * Generic driver for SMIA/SMIA++ compliant camera modules
+ *
+ * Copyright (C) 2011--2012 Nokia Corporation
+ * Contact: Sakari Ailus <sakari.ailus@maxwell.research.nokia.com>
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * version 2 as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+ * General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA
+ * 02110-1301 USA
+ *
+ */
+
+#ifndef __SMIAPP_QUIRK__
+#define __SMIAPP_QUIRK__
+
+struct smiapp_sensor;
+
+/**
+ * struct smiapp_quirk - quirks for sensors that deviate from SMIA++ standard
+ *
+ * @limits: Replace sensor->limits with values which can't be read from
+ *	    sensor registers. Called the first time the sensor is powered up.
+ * @post_poweron: Called always after the sensor has been fully powered on.
+ * @pre_streamon: Called just before streaming is enabled.
+ * @post_streamon: Called right after stopping streaming.
+ */
+struct smiapp_quirk {
+	int (*limits)(struct smiapp_sensor *sensor);
+	int (*post_poweron)(struct smiapp_sensor *sensor);
+	int (*pre_streamon)(struct smiapp_sensor *sensor);
+	int (*post_streamoff)(struct smiapp_sensor *sensor);
+	const struct smia_reg *regs;
+	unsigned long flags;
+};
+
+/* op pix clock is for all lanes in total normally */
+#define SMIAPP_QUIRK_FLAG_OP_PIX_CLOCK_PER_LANE			(1 << 0)
+#define SMIAPP_QUIRK_FLAG_8BIT_READ_ONLY			(1 << 1)
+
+struct smiapp_reg_8 {
+	u16 reg;
+	u8 val;
+};
+
+void smiapp_replace_limit(struct smiapp_sensor *sensor,
+			  u32 limit, u32 val);
+bool smiapp_quirk_reg(struct smiapp_sensor *sensor,
+		      u32 reg, u32 *val);
+
+#define SMIAPP_MK_QUIRK_REG(_reg, _val) \
+	{				\
+		.type = (_reg >> 16),	\
+		.reg = (u16)_reg,	\
+		.val = _val,		\
+	}
+
+#define smiapp_call_quirk(_sensor, _quirk, ...)				\
+	(_sensor->minfo.quirk &&					\
+	 _sensor->minfo.quirk->_quirk ?					\
+	 _sensor->minfo.quirk->_quirk(_sensor, ##__VA_ARGS__) : 0)
+
+#define smiapp_needs_quirk(_sensor, _quirk)		\
+	(_sensor->minfo.quirk ?				\
+	 _sensor->minfo.quirk->flags & _quirk : 0)
+
+extern const struct smiapp_quirk smiapp_jt8ev1_quirk;
+extern const struct smiapp_quirk smiapp_imx125es_quirk;
+extern const struct smiapp_quirk smiapp_jt8ew9_quirk;
+extern const struct smiapp_quirk smiapp_tcm8500md_quirk;
+
+#endif /* __SMIAPP_QUIRK__ */
diff --git a/drivers/media/video/smiapp/smiapp-reg-defs.h b/drivers/media/video/smiapp/smiapp-reg-defs.h
new file mode 100644
index 00000000000..a089eb8161e
--- /dev/null
+++ b/drivers/media/video/smiapp/smiapp-reg-defs.h
@@ -0,0 +1,503 @@
+/*
+ * drivers/media/video/smiapp/smiapp-reg-defs.h
+ *
+ * Generic driver for SMIA/SMIA++ compliant camera modules
+ *
+ * Copyright (C) 2011--2012 Nokia Corporation
+ * Contact: Sakari Ailus <sakari.ailus@maxwell.research.nokia.com>
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * version 2 as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+ * General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA
+ * 02110-1301 USA
+ *
+ */
+#define SMIAPP_REG_MK_U8(r) ((SMIA_REG_8BIT << 16) | (r))
+#define SMIAPP_REG_MK_U16(r) ((SMIA_REG_16BIT << 16) | (r))
+#define SMIAPP_REG_MK_U32(r) ((SMIA_REG_32BIT << 16) | (r))
+
+#define SMIAPP_REG_MK_F32(r) (SMIA_REG_FLAG_FLOAT | (SMIA_REG_32BIT << 16) | (r))
+
+#define SMIAPP_REG_U16_MODEL_ID					SMIAPP_REG_MK_U16(0x0000)
+#define SMIAPP_REG_U8_REVISION_NUMBER_MAJOR			SMIAPP_REG_MK_U8(0x0002)
+#define SMIAPP_REG_U8_MANUFACTURER_ID				SMIAPP_REG_MK_U8(0x0003)
+#define SMIAPP_REG_U8_SMIA_VERSION				SMIAPP_REG_MK_U8(0x0004)
+#define SMIAPP_REG_U8_FRAME_COUNT				SMIAPP_REG_MK_U8(0x0005)
+#define SMIAPP_REG_U8_PIXEL_ORDER				SMIAPP_REG_MK_U8(0x0006)
+#define SMIAPP_REG_U16_DATA_PEDESTAL				SMIAPP_REG_MK_U16(0x0008)
+#define SMIAPP_REG_U8_PIXEL_DEPTH				SMIAPP_REG_MK_U8(0x000c)
+#define SMIAPP_REG_U8_REVISION_NUMBER_MINOR			SMIAPP_REG_MK_U8(0x0010)
+#define SMIAPP_REG_U8_SMIAPP_VERSION				SMIAPP_REG_MK_U8(0x0011)
+#define SMIAPP_REG_U8_MODULE_DATE_YEAR				SMIAPP_REG_MK_U8(0x0012)
+#define SMIAPP_REG_U8_MODULE_DATE_MONTH				SMIAPP_REG_MK_U8(0x0013)
+#define SMIAPP_REG_U8_MODULE_DATE_DAY				SMIAPP_REG_MK_U8(0x0014)
+#define SMIAPP_REG_U8_MODULE_DATE_PHASE				SMIAPP_REG_MK_U8(0x0015)
+#define SMIAPP_REG_U16_SENSOR_MODEL_ID				SMIAPP_REG_MK_U16(0x0016)
+#define SMIAPP_REG_U8_SENSOR_REVISION_NUMBER			SMIAPP_REG_MK_U8(0x0018)
+#define SMIAPP_REG_U8_SENSOR_MANUFACTURER_ID			SMIAPP_REG_MK_U8(0x0019)
+#define SMIAPP_REG_U8_SENSOR_FIRMWARE_VERSION			SMIAPP_REG_MK_U8(0x001a)
+#define SMIAPP_REG_U32_SERIAL_NUMBER				SMIAPP_REG_MK_U32(0x001c)
+#define SMIAPP_REG_U8_FRAME_FORMAT_MODEL_TYPE			SMIAPP_REG_MK_U8(0x0040)
+#define SMIAPP_REG_U8_FRAME_FORMAT_MODEL_SUBTYPE		SMIAPP_REG_MK_U8(0x0041)
+#define SMIAPP_REG_U16_FRAME_FORMAT_DESCRIPTOR_2(n)		SMIAPP_REG_MK_U16(0x0042 + ((n) << 1)) /* 0 <= n <= 14 */
+#define SMIAPP_REG_U32_FRAME_FORMAT_DESCRIPTOR_4(n)		SMIAPP_REG_MK_U32(0x0060 + ((n) << 2)) /* 0 <= n <= 7 */
+#define SMIAPP_REG_U16_ANALOGUE_GAIN_CAPABILITY			SMIAPP_REG_MK_U16(0x0080)
+#define SMIAPP_REG_U16_ANALOGUE_GAIN_CODE_MIN			SMIAPP_REG_MK_U16(0x0084)
+#define SMIAPP_REG_U16_ANALOGUE_GAIN_CODE_MAX			SMIAPP_REG_MK_U16(0x0086)
+#define SMIAPP_REG_U16_ANALOGUE_GAIN_CODE_STEP			SMIAPP_REG_MK_U16(0x0088)
+#define SMIAPP_REG_U16_ANALOGUE_GAIN_TYPE			SMIAPP_REG_MK_U16(0x008a)
+#define SMIAPP_REG_U16_ANALOGUE_GAIN_M0				SMIAPP_REG_MK_U16(0x008c)
+#define SMIAPP_REG_U16_ANALOGUE_GAIN_C0				SMIAPP_REG_MK_U16(0x008e)
+#define SMIAPP_REG_U16_ANALOGUE_GAIN_M1				SMIAPP_REG_MK_U16(0x0090)
+#define SMIAPP_REG_U16_ANALOGUE_GAIN_C1				SMIAPP_REG_MK_U16(0x0092)
+#define SMIAPP_REG_U8_DATA_FORMAT_MODEL_TYPE			SMIAPP_REG_MK_U8(0x00c0)
+#define SMIAPP_REG_U8_DATA_FORMAT_MODEL_SUBTYPE			SMIAPP_REG_MK_U8(0x00c1)
+#define SMIAPP_REG_U16_DATA_FORMAT_DESCRIPTOR(n)		SMIAPP_REG_MK_U16(0x00c2 + ((n) << 1))
+#define SMIAPP_REG_U8_MODE_SELECT				SMIAPP_REG_MK_U8(0x0100)
+#define SMIAPP_REG_U8_IMAGE_ORIENTATION				SMIAPP_REG_MK_U8(0x0101)
+#define SMIAPP_REG_U8_SOFTWARE_RESET				SMIAPP_REG_MK_U8(0x0103)
+#define SMIAPP_REG_U8_GROUPED_PARAMETER_HOLD			SMIAPP_REG_MK_U8(0x0104)
+#define SMIAPP_REG_U8_MASK_CORRUPTED_FRAMES			SMIAPP_REG_MK_U8(0x0105)
+#define SMIAPP_REG_U8_FAST_STANDBY_CTRL				SMIAPP_REG_MK_U8(0x0106)
+#define SMIAPP_REG_U8_CCI_ADDRESS_CONTROL			SMIAPP_REG_MK_U8(0x0107)
+#define SMIAPP_REG_U8_2ND_CCI_IF_CONTROL			SMIAPP_REG_MK_U8(0x0108)
+#define SMIAPP_REG_U8_2ND_CCI_ADDRESS_CONTROL			SMIAPP_REG_MK_U8(0x0109)
+#define SMIAPP_REG_U8_CSI_CHANNEL_IDENTIFIER			SMIAPP_REG_MK_U8(0x0110)
+#define SMIAPP_REG_U8_CSI_SIGNALLING_MODE			SMIAPP_REG_MK_U8(0x0111)
+#define SMIAPP_REG_U16_CSI_DATA_FORMAT				SMIAPP_REG_MK_U16(0x0112)
+#define SMIAPP_REG_U8_CSI_LANE_MODE				SMIAPP_REG_MK_U8(0x0114)
+#define SMIAPP_REG_U8_CSI2_10_TO_8_DT				SMIAPP_REG_MK_U8(0x0115)
+#define SMIAPP_REG_U8_CSI2_10_TO_7_DT				SMIAPP_REG_MK_U8(0x0116)
+#define SMIAPP_REG_U8_CSI2_10_TO_6_DT				SMIAPP_REG_MK_U8(0x0117)
+#define SMIAPP_REG_U8_CSI2_12_TO_8_DT				SMIAPP_REG_MK_U8(0x0118)
+#define SMIAPP_REG_U8_CSI2_12_TO_7_DT				SMIAPP_REG_MK_U8(0x0119)
+#define SMIAPP_REG_U8_CSI2_12_TO_6_DT				SMIAPP_REG_MK_U8(0x011a)
+#define SMIAPP_REG_U8_CSI2_14_TO_10_DT				SMIAPP_REG_MK_U8(0x011b)
+#define SMIAPP_REG_U8_CSI2_14_TO_8_DT				SMIAPP_REG_MK_U8(0x011c)
+#define SMIAPP_REG_U8_CSI2_16_TO_10_DT				SMIAPP_REG_MK_U8(0x011d)
+#define SMIAPP_REG_U8_CSI2_16_TO_8_DT				SMIAPP_REG_MK_U8(0x011e)
+#define SMIAPP_REG_U8_GAIN_MODE					SMIAPP_REG_MK_U8(0x0120)
+#define SMIAPP_REG_U16_VANA_VOLTAGE				SMIAPP_REG_MK_U16(0x0130)
+#define SMIAPP_REG_U16_VDIG_VOLTAGE				SMIAPP_REG_MK_U16(0x0132)
+#define SMIAPP_REG_U16_VIO_VOLTAGE				SMIAPP_REG_MK_U16(0x0134)
+#define SMIAPP_REG_U16_EXTCLK_FREQUENCY_MHZ			SMIAPP_REG_MK_U16(0x0136)
+#define SMIAPP_REG_U8_TEMP_SENSOR_CONTROL			SMIAPP_REG_MK_U8(0x0138)
+#define SMIAPP_REG_U8_TEMP_SENSOR_MODE				SMIAPP_REG_MK_U8(0x0139)
+#define SMIAPP_REG_U8_TEMP_SENSOR_OUTPUT			SMIAPP_REG_MK_U8(0x013a)
+#define SMIAPP_REG_U16_FINE_INTEGRATION_TIME			SMIAPP_REG_MK_U16(0x0200)
+#define SMIAPP_REG_U16_COARSE_INTEGRATION_TIME			SMIAPP_REG_MK_U16(0x0202)
+#define SMIAPP_REG_U16_ANALOGUE_GAIN_CODE_GLOBAL		SMIAPP_REG_MK_U16(0x0204)
+#define SMIAPP_REG_U16_ANALOGUE_GAIN_CODE_GREENR		SMIAPP_REG_MK_U16(0x0206)
+#define SMIAPP_REG_U16_ANALOGUE_GAIN_CODE_RED			SMIAPP_REG_MK_U16(0x0208)
+#define SMIAPP_REG_U16_ANALOGUE_GAIN_CODE_BLUE			SMIAPP_REG_MK_U16(0x020a)
+#define SMIAPP_REG_U16_ANALOGUE_GAIN_CODE_GREENB		SMIAPP_REG_MK_U16(0x020c)
+#define SMIAPP_REG_U16_DIGITAL_GAIN_GREENR			SMIAPP_REG_MK_U16(0x020e)
+#define SMIAPP_REG_U16_DIGITAL_GAIN_RED				SMIAPP_REG_MK_U16(0x0210)
+#define SMIAPP_REG_U16_DIGITAL_GAIN_BLUE			SMIAPP_REG_MK_U16(0x0212)
+#define SMIAPP_REG_U16_DIGITAL_GAIN_GREENB			SMIAPP_REG_MK_U16(0x0214)
+#define SMIAPP_REG_U16_VT_PIX_CLK_DIV				SMIAPP_REG_MK_U16(0x0300)
+#define SMIAPP_REG_U16_VT_SYS_CLK_DIV				SMIAPP_REG_MK_U16(0x0302)
+#define SMIAPP_REG_U16_PRE_PLL_CLK_DIV				SMIAPP_REG_MK_U16(0x0304)
+#define SMIAPP_REG_U16_PLL_MULTIPLIER				SMIAPP_REG_MK_U16(0x0306)
+#define SMIAPP_REG_U16_OP_PIX_CLK_DIV				SMIAPP_REG_MK_U16(0x0308)
+#define SMIAPP_REG_U16_OP_SYS_CLK_DIV				SMIAPP_REG_MK_U16(0x030a)
+#define SMIAPP_REG_U16_FRAME_LENGTH_LINES			SMIAPP_REG_MK_U16(0x0340)
+#define SMIAPP_REG_U16_LINE_LENGTH_PCK				SMIAPP_REG_MK_U16(0x0342)
+#define SMIAPP_REG_U16_X_ADDR_START				SMIAPP_REG_MK_U16(0x0344)
+#define SMIAPP_REG_U16_Y_ADDR_START				SMIAPP_REG_MK_U16(0x0346)
+#define SMIAPP_REG_U16_X_ADDR_END				SMIAPP_REG_MK_U16(0x0348)
+#define SMIAPP_REG_U16_Y_ADDR_END				SMIAPP_REG_MK_U16(0x034a)
+#define SMIAPP_REG_U16_X_OUTPUT_SIZE				SMIAPP_REG_MK_U16(0x034c)
+#define SMIAPP_REG_U16_Y_OUTPUT_SIZE				SMIAPP_REG_MK_U16(0x034e)
+#define SMIAPP_REG_U16_X_EVEN_INC				SMIAPP_REG_MK_U16(0x0380)
+#define SMIAPP_REG_U16_X_ODD_INC				SMIAPP_REG_MK_U16(0x0382)
+#define SMIAPP_REG_U16_Y_EVEN_INC				SMIAPP_REG_MK_U16(0x0384)
+#define SMIAPP_REG_U16_Y_ODD_INC				SMIAPP_REG_MK_U16(0x0386)
+#define SMIAPP_REG_U16_SCALING_MODE				SMIAPP_REG_MK_U16(0x0400)
+#define SMIAPP_REG_U16_SPATIAL_SAMPLING				SMIAPP_REG_MK_U16(0x0402)
+#define SMIAPP_REG_U16_SCALE_M					SMIAPP_REG_MK_U16(0x0404)
+#define SMIAPP_REG_U16_SCALE_N					SMIAPP_REG_MK_U16(0x0406)
+#define SMIAPP_REG_U16_DIGITAL_CROP_X_OFFSET			SMIAPP_REG_MK_U16(0x0408)
+#define SMIAPP_REG_U16_DIGITAL_CROP_Y_OFFSET			SMIAPP_REG_MK_U16(0x040a)
+#define SMIAPP_REG_U16_DIGITAL_CROP_IMAGE_WIDTH			SMIAPP_REG_MK_U16(0x040c)
+#define SMIAPP_REG_U16_DIGITAL_CROP_IMAGE_HEIGHT		SMIAPP_REG_MK_U16(0x040e)
+#define SMIAPP_REG_U16_COMPRESSION_MODE				SMIAPP_REG_MK_U16(0x0500)
+#define SMIAPP_REG_U16_TEST_PATTERN_MODE			SMIAPP_REG_MK_U16(0x0600)
+#define SMIAPP_REG_U16_TEST_DATA_RED				SMIAPP_REG_MK_U16(0x0602)
+#define SMIAPP_REG_U16_TEST_DATA_GREENR				SMIAPP_REG_MK_U16(0x0604)
+#define SMIAPP_REG_U16_TEST_DATA_BLUE				SMIAPP_REG_MK_U16(0x0606)
+#define SMIAPP_REG_U16_TEST_DATA_GREENB				SMIAPP_REG_MK_U16(0x0608)
+#define SMIAPP_REG_U16_HORIZONTAL_CURSOR_WIDTH			SMIAPP_REG_MK_U16(0x060a)
+#define SMIAPP_REG_U16_HORIZONTAL_CURSOR_POSITION		SMIAPP_REG_MK_U16(0x060c)
+#define SMIAPP_REG_U16_VERTICAL_CURSOR_WIDTH			SMIAPP_REG_MK_U16(0x060e)
+#define SMIAPP_REG_U16_VERTICAL_CURSOR_POSITION			SMIAPP_REG_MK_U16(0x0610)
+#define SMIAPP_REG_U16_FIFO_WATER_MARK_PIXELS			SMIAPP_REG_MK_U16(0x0700)
+#define SMIAPP_REG_U8_TCLK_POST					SMIAPP_REG_MK_U8(0x0800)
+#define SMIAPP_REG_U8_THS_PREPARE				SMIAPP_REG_MK_U8(0x0801)
+#define SMIAPP_REG_U8_THS_ZERO_MIN				SMIAPP_REG_MK_U8(0x0802)
+#define SMIAPP_REG_U8_THS_TRAIL					SMIAPP_REG_MK_U8(0x0803)
+#define SMIAPP_REG_U8_TCLK_TRAIL_MIN				SMIAPP_REG_MK_U8(0x0804)
+#define SMIAPP_REG_U8_TCLK_PREPARE				SMIAPP_REG_MK_U8(0x0805)
+#define SMIAPP_REG_U8_TCLK_ZERO					SMIAPP_REG_MK_U8(0x0806)
+#define SMIAPP_REG_U8_TLPX					SMIAPP_REG_MK_U8(0x0807)
+#define SMIAPP_REG_U8_DPHY_CTRL					SMIAPP_REG_MK_U8(0x0808)
+#define SMIAPP_REG_U32_REQUESTED_LINK_BIT_RATE_MBPS		SMIAPP_REG_MK_U32(0x0820)
+#define SMIAPP_REG_U8_BINNING_MODE				SMIAPP_REG_MK_U8(0x0900)
+#define SMIAPP_REG_U8_BINNING_TYPE				SMIAPP_REG_MK_U8(0x0901)
+#define SMIAPP_REG_U8_BINNING_WEIGHTING				SMIAPP_REG_MK_U8(0x0902)
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_1_CTRL			SMIAPP_REG_MK_U8(0x0a00)
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_1_STATUS			SMIAPP_REG_MK_U8(0x0a01)
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_1_PAGE_SELECT		SMIAPP_REG_MK_U8(0x0a02)
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_1_DATA_0			SMIAPP_REG_MK_U8(0x0a04)
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_1_DATA_1			SMIAPP_REG_MK_U8(0x0a05)
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_1_DATA_2			SMIAPP_REG_MK_U8(0x0a06)
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_1_DATA_3			SMIAPP_REG_MK_U8(0x0a07)
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_1_DATA_4			SMIAPP_REG_MK_U8(0x0a08)
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_1_DATA_5			SMIAPP_REG_MK_U8(0x0a09)
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_1_DATA_12		SMIAPP_REG_MK_U8(0x0a10)
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_1_DATA_13		SMIAPP_REG_MK_U8(0x0a11)
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_1_DATA_14		SMIAPP_REG_MK_U8(0x0a12)
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_1_DATA_15		SMIAPP_REG_MK_U8(0x0a13)
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_1_DATA_16		SMIAPP_REG_MK_U8(0x0a14)
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_1_DATA_17		SMIAPP_REG_MK_U8(0x0a15)
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_1_DATA_18		SMIAPP_REG_MK_U8(0x0a16)
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_1_DATA_19		SMIAPP_REG_MK_U8(0x0a17)
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_1_DATA_20		SMIAPP_REG_MK_U8(0x0a18)
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_1_DATA_21		SMIAPP_REG_MK_U8(0x0a19)
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_1_DATA_22		SMIAPP_REG_MK_U8(0x0a1a)
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_1_DATA_23		SMIAPP_REG_MK_U8(0x0a1b)
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_1_DATA_24		SMIAPP_REG_MK_U8(0x0a1c)
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_1_DATA_25		SMIAPP_REG_MK_U8(0x0a1d)
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_1_DATA_26		SMIAPP_REG_MK_U8(0x0a1e)
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_1_DATA_27		SMIAPP_REG_MK_U8(0x0a1f)
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_1_DATA_28		SMIAPP_REG_MK_U8(0x0a20)
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_1_DATA_29		SMIAPP_REG_MK_U8(0x0a21)
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_1_DATA_30		SMIAPP_REG_MK_U8(0x0a22)
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_1_DATA_31		SMIAPP_REG_MK_U8(0x0a23)
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_1_DATA_32		SMIAPP_REG_MK_U8(0x0a24)
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_1_DATA_33		SMIAPP_REG_MK_U8(0x0a25)
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_1_DATA_34		SMIAPP_REG_MK_U8(0x0a26)
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_1_DATA_35		SMIAPP_REG_MK_U8(0x0a27)
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_1_DATA_36		SMIAPP_REG_MK_U8(0x0a28)
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_1_DATA_37		SMIAPP_REG_MK_U8(0x0a29)
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_1_DATA_38		SMIAPP_REG_MK_U8(0x0a2a)
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_1_DATA_39		SMIAPP_REG_MK_U8(0x0a2b)
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_1_DATA_40		SMIAPP_REG_MK_U8(0x0a2c)
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_1_DATA_41		SMIAPP_REG_MK_U8(0x0a2d)
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_1_DATA_42		SMIAPP_REG_MK_U8(0x0a2e)
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_1_DATA_43		SMIAPP_REG_MK_U8(0x0a2f)
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_1_DATA_44		SMIAPP_REG_MK_U8(0x0a30)
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_1_DATA_45		SMIAPP_REG_MK_U8(0x0a31)
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_1_DATA_46		SMIAPP_REG_MK_U8(0x0a32)
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_1_DATA_47		SMIAPP_REG_MK_U8(0x0a33)
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_1_DATA_48		SMIAPP_REG_MK_U8(0x0a34)
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_1_DATA_49		SMIAPP_REG_MK_U8(0x0a35)
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_1_DATA_50		SMIAPP_REG_MK_U8(0x0a36)
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_1_DATA_51		SMIAPP_REG_MK_U8(0x0a37)
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_1_DATA_52		SMIAPP_REG_MK_U8(0x0a38)
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_1_DATA_53		SMIAPP_REG_MK_U8(0x0a39)
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_1_DATA_54		SMIAPP_REG_MK_U8(0x0a3a)
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_1_DATA_55		SMIAPP_REG_MK_U8(0x0a3b)
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_1_DATA_56		SMIAPP_REG_MK_U8(0x0a3c)
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_1_DATA_57		SMIAPP_REG_MK_U8(0x0a3d)
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_1_DATA_58		SMIAPP_REG_MK_U8(0x0a3e)
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_1_DATA_59		SMIAPP_REG_MK_U8(0x0a3f)
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_1_DATA_60		SMIAPP_REG_MK_U8(0x0a40)
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_1_DATA_61		SMIAPP_REG_MK_U8(0x0a41)
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_1_DATA_62		SMIAPP_REG_MK_U8(0x0a42)
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_1_DATA_63		SMIAPP_REG_MK_U8(0x0a43)
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_2_CTRL			SMIAPP_REG_MK_U8(0x0a44)
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_2_STATUS			SMIAPP_REG_MK_U8(0x0a45)
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_2_PAGE_SELECT		SMIAPP_REG_MK_U8(0x0a46)
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_2_DATA_0			SMIAPP_REG_MK_U8(0x0a48)
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_2_DATA_1			SMIAPP_REG_MK_U8(0x0a49)
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_2_DATA_2			SMIAPP_REG_MK_U8(0x0a4a)
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_2_DATA_3			SMIAPP_REG_MK_U8(0x0a4b)
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_2_DATA_4			SMIAPP_REG_MK_U8(0x0a4c)
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_2_DATA_5			SMIAPP_REG_MK_U8(0x0a4d)
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_2_DATA_6			SMIAPP_REG_MK_U8(0x0a4e)
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_2_DATA_7			SMIAPP_REG_MK_U8(0x0a4f)
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_2_DATA_8			SMIAPP_REG_MK_U8(0x0a50)
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_2_DATA_9			SMIAPP_REG_MK_U8(0x0a51)
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_2_DATA_10		SMIAPP_REG_MK_U8(0x0a52)
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_2_DATA_11		SMIAPP_REG_MK_U8(0x0a53)
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_2_DATA_12		SMIAPP_REG_MK_U8(0x0a54)
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_2_DATA_13		SMIAPP_REG_MK_U8(0x0a55)
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_2_DATA_14		SMIAPP_REG_MK_U8(0x0a56)
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_2_DATA_15		SMIAPP_REG_MK_U8(0x0a57)
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_2_DATA_16		SMIAPP_REG_MK_U8(0x0a58)
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_2_DATA_17		SMIAPP_REG_MK_U8(0x0a59)
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_2_DATA_18		SMIAPP_REG_MK_U8(0x0a5a)
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_2_DATA_19		SMIAPP_REG_MK_U8(0x0a5b)
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_2_DATA_20		SMIAPP_REG_MK_U8(0x0a5c)
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_2_DATA_21		SMIAPP_REG_MK_U8(0x0a5d)
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_2_DATA_22		SMIAPP_REG_MK_U8(0x0a5e)
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_2_DATA_23		SMIAPP_REG_MK_U8(0x0a5f)
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_2_DATA_24		SMIAPP_REG_MK_U8(0x0a60)
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_2_DATA_25		SMIAPP_REG_MK_U8(0x0a61)
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_2_DATA_26		SMIAPP_REG_MK_U8(0x0a62)
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_2_DATA_27		SMIAPP_REG_MK_U8(0x0a63)
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_2_DATA_28		SMIAPP_REG_MK_U8(0x0a64)
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_2_DATA_29		SMIAPP_REG_MK_U8(0x0a65)
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_2_DATA_30		SMIAPP_REG_MK_U8(0x0a66)
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_2_DATA_31		SMIAPP_REG_MK_U8(0x0a67)
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_2_DATA_32		SMIAPP_REG_MK_U8(0x0a68)
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_2_DATA_33		SMIAPP_REG_MK_U8(0x0a69)
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_2_DATA_34		SMIAPP_REG_MK_U8(0x0a6a)
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_2_DATA_35		SMIAPP_REG_MK_U8(0x0a6b)
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_2_DATA_36		SMIAPP_REG_MK_U8(0x0a6c)
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_2_DATA_37		SMIAPP_REG_MK_U8(0x0a6d)
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_2_DATA_38		SMIAPP_REG_MK_U8(0x0a6e)
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_2_DATA_39		SMIAPP_REG_MK_U8(0x0a6f)
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_2_DATA_40		SMIAPP_REG_MK_U8(0x0a70)
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_2_DATA_41		SMIAPP_REG_MK_U8(0x0a71)
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_2_DATA_42		SMIAPP_REG_MK_U8(0x0a72)
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_2_DATA_43		SMIAPP_REG_MK_U8(0x0a73)
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_2_DATA_44		SMIAPP_REG_MK_U8(0x0a74)
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_2_DATA_45		SMIAPP_REG_MK_U8(0x0a75)
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_2_DATA_46		SMIAPP_REG_MK_U8(0x0a76)
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_2_DATA_47		SMIAPP_REG_MK_U8(0x0a77)
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_2_DATA_48		SMIAPP_REG_MK_U8(0x0a78)
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_2_DATA_49		SMIAPP_REG_MK_U8(0x0a79)
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_2_DATA_50		SMIAPP_REG_MK_U8(0x0a7a)
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_2_DATA_51		SMIAPP_REG_MK_U8(0x0a7b)
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_2_DATA_52		SMIAPP_REG_MK_U8(0x0a7c)
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_2_DATA_53		SMIAPP_REG_MK_U8(0x0a7d)
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_2_DATA_54		SMIAPP_REG_MK_U8(0x0a7e)
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_2_DATA_55		SMIAPP_REG_MK_U8(0x0a7f)
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_2_DATA_56		SMIAPP_REG_MK_U8(0x0a80)
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_2_DATA_57		SMIAPP_REG_MK_U8(0x0a81)
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_2_DATA_58		SMIAPP_REG_MK_U8(0x0a82)
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_2_DATA_59		SMIAPP_REG_MK_U8(0x0a83)
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_2_DATA_60		SMIAPP_REG_MK_U8(0x0a84)
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_2_DATA_61		SMIAPP_REG_MK_U8(0x0a85)
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_2_DATA_62		SMIAPP_REG_MK_U8(0x0a86)
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_2_DATA_63		SMIAPP_REG_MK_U8(0x0a87)
+#define SMIAPP_REG_U8_SHADING_CORRECTION_ENABLE			SMIAPP_REG_MK_U8(0x0b00)
+#define SMIAPP_REG_U8_LUMINANCE_CORRECTION_LEVEL		SMIAPP_REG_MK_U8(0x0b01)
+#define SMIAPP_REG_U8_GREEN_IMBALANCE_FILTER_ENABLE		SMIAPP_REG_MK_U8(0x0b02)
+#define SMIAPP_REG_U8_GREEN_IMBALANCE_FILTER_WEIGHT		SMIAPP_REG_MK_U8(0x0b03)
+#define SMIAPP_REG_U8_BLACK_LEVEL_CORRECTION_ENABLE		SMIAPP_REG_MK_U8(0x0b04)
+#define SMIAPP_REG_U8_MAPPED_COUPLET_CORRECT_ENABLE		SMIAPP_REG_MK_U8(0x0b05)
+#define SMIAPP_REG_U8_SINGLE_DEFECT_CORRECT_ENABLE		SMIAPP_REG_MK_U8(0x0b06)
+#define SMIAPP_REG_U8_SINGLE_DEFECT_CORRECT_WEIGHT		SMIAPP_REG_MK_U8(0x0b07)
+#define SMIAPP_REG_U8_DYNAMIC_COUPLET_CORRECT_ENABLE		SMIAPP_REG_MK_U8(0x0b08)
+#define SMIAPP_REG_U8_DYNAMIC_COUPLET_CORRECT_WEIGHT		SMIAPP_REG_MK_U8(0x0b09)
+#define SMIAPP_REG_U8_COMBINED_DEFECT_CORRECT_ENABLE		SMIAPP_REG_MK_U8(0x0b0a)
+#define SMIAPP_REG_U8_COMBINED_DEFECT_CORRECT_WEIGHT		SMIAPP_REG_MK_U8(0x0b0b)
+#define SMIAPP_REG_U8_MODULE_SPECIFIC_CORRECTION_ENABLE		SMIAPP_REG_MK_U8(0x0b0c)
+#define SMIAPP_REG_U8_MODULE_SPECIFIC_CORRECTION_WEIGHT		SMIAPP_REG_MK_U8(0x0b0d)
+#define SMIAPP_REG_U8_MAPPED_LINE_DEFECT_CORRECT_ENABLE		SMIAPP_REG_MK_U8(0x0b0e)
+#define SMIAPP_REG_U8_MAPPED_LINE_DEFECT_CORRECT_ADJUST		SMIAPP_REG_MK_U8(0x0b0f)
+#define SMIAPP_REG_U8_MAPPED_COUPLET_CORRECT_ADJUST		SMIAPP_REG_MK_U8(0x0b10)
+#define SMIAPP_REG_U8_MAPPED_TRIPLET_DEFECT_CORRECT_ENABLE	SMIAPP_REG_MK_U8(0x0b11)
+#define SMIAPP_REG_U8_MAPPED_TRIPLET_DEFECT_CORRECT_ADJUST	SMIAPP_REG_MK_U8(0x0b12)
+#define SMIAPP_REG_U8_DYNAMIC_TRIPLET_DEFECT_CORRECT_ENABLE	SMIAPP_REG_MK_U8(0x0b13)
+#define SMIAPP_REG_U8_DYNAMIC_TRIPLET_DEFECT_CORRECT_ADJUST	SMIAPP_REG_MK_U8(0x0b14)
+#define SMIAPP_REG_U8_DYNAMIC_LINE_DEFECT_CORRECT_ENABLE	SMIAPP_REG_MK_U8(0x0b15)
+#define SMIAPP_REG_U8_DYNAMIC_LINE_DEFECT_CORRECT_ADJUST	SMIAPP_REG_MK_U8(0x0b16)
+#define SMIAPP_REG_U8_EDOF_MODE					SMIAPP_REG_MK_U8(0x0b80)
+#define SMIAPP_REG_U8_SHARPNESS					SMIAPP_REG_MK_U8(0x0b83)
+#define SMIAPP_REG_U8_DENOISING					SMIAPP_REG_MK_U8(0x0b84)
+#define SMIAPP_REG_U8_MODULE_SPECIFIC				SMIAPP_REG_MK_U8(0x0b85)
+#define SMIAPP_REG_U16_DEPTH_OF_FIELD				SMIAPP_REG_MK_U16(0x0b86)
+#define SMIAPP_REG_U16_FOCUS_DISTANCE				SMIAPP_REG_MK_U16(0x0b88)
+#define SMIAPP_REG_U8_ESTIMATION_MODE_CTRL			SMIAPP_REG_MK_U8(0x0b8a)
+#define SMIAPP_REG_U16_COLOUR_TEMPERATURE			SMIAPP_REG_MK_U16(0x0b8c)
+#define SMIAPP_REG_U16_ABSOLUTE_GAIN_GREENR			SMIAPP_REG_MK_U16(0x0b8e)
+#define SMIAPP_REG_U16_ABSOLUTE_GAIN_RED			SMIAPP_REG_MK_U16(0x0b90)
+#define SMIAPP_REG_U16_ABSOLUTE_GAIN_BLUE			SMIAPP_REG_MK_U16(0x0b92)
+#define SMIAPP_REG_U16_ABSOLUTE_GAIN_GREENB			SMIAPP_REG_MK_U16(0x0b94)
+#define SMIAPP_REG_U8_ESTIMATION_ZONE_MODE			SMIAPP_REG_MK_U8(0x0bc0)
+#define SMIAPP_REG_U16_FIXED_ZONE_WEIGHTING			SMIAPP_REG_MK_U16(0x0bc2)
+#define SMIAPP_REG_U16_CUSTOM_ZONE_X_START			SMIAPP_REG_MK_U16(0x0bc4)
+#define SMIAPP_REG_U16_CUSTOM_ZONE_Y_START			SMIAPP_REG_MK_U16(0x0bc6)
+#define SMIAPP_REG_U16_CUSTOM_ZONE_WIDTH			SMIAPP_REG_MK_U16(0x0bc8)
+#define SMIAPP_REG_U16_CUSTOM_ZONE_HEIGHT			SMIAPP_REG_MK_U16(0x0bca)
+#define SMIAPP_REG_U8_GLOBAL_RESET_CTRL1			SMIAPP_REG_MK_U8(0x0c00)
+#define SMIAPP_REG_U8_GLOBAL_RESET_CTRL2			SMIAPP_REG_MK_U8(0x0c01)
+#define SMIAPP_REG_U8_GLOBAL_RESET_MODE_CONFIG_1		SMIAPP_REG_MK_U8(0x0c02)
+#define SMIAPP_REG_U8_GLOBAL_RESET_MODE_CONFIG_2		SMIAPP_REG_MK_U8(0x0c03)
+#define SMIAPP_REG_U16_TRDY_CTRL				SMIAPP_REG_MK_U16(0x0c04)
+#define SMIAPP_REG_U16_TRDOUT_CTRL				SMIAPP_REG_MK_U16(0x0c06)
+#define SMIAPP_REG_U16_TSHUTTER_STROBE_DELAY_CTRL		SMIAPP_REG_MK_U16(0x0c08)
+#define SMIAPP_REG_U16_TSHUTTER_STROBE_WIDTH_CTRL		SMIAPP_REG_MK_U16(0x0c0a)
+#define SMIAPP_REG_U16_TFLASH_STROBE_DELAY_CTRL			SMIAPP_REG_MK_U16(0x0c0c)
+#define SMIAPP_REG_U16_TFLASH_STROBE_WIDTH_HIGH_CTRL		SMIAPP_REG_MK_U16(0x0c0e)
+#define SMIAPP_REG_U16_TGRST_INTERVAL_CTRL			SMIAPP_REG_MK_U16(0x0c10)
+#define SMIAPP_REG_U8_FLASH_STROBE_ADJUSTMENT			SMIAPP_REG_MK_U8(0x0c12)
+#define SMIAPP_REG_U16_FLASH_STROBE_START_POINT			SMIAPP_REG_MK_U16(0x0c14)
+#define SMIAPP_REG_U16_TFLASH_STROBE_DELAY_RS_CTRL		SMIAPP_REG_MK_U16(0x0c16)
+#define SMIAPP_REG_U16_TFLASH_STROBE_WIDTH_HIGH_RS_CTRL		SMIAPP_REG_MK_U16(0x0c18)
+#define SMIAPP_REG_U8_FLASH_MODE_RS				SMIAPP_REG_MK_U8(0x0c1a)
+#define SMIAPP_REG_U8_FLASH_TRIGGER_RS				SMIAPP_REG_MK_U8(0x0c1b)
+#define SMIAPP_REG_U8_FLASH_STATUS				SMIAPP_REG_MK_U8(0x0c1c)
+#define SMIAPP_REG_U8_SA_STROBE_MODE				SMIAPP_REG_MK_U8(0x0c1d)
+#define SMIAPP_REG_U16_SA_STROBE_START_POINT			SMIAPP_REG_MK_U16(0x0c1e)
+#define SMIAPP_REG_U16_TSA_STROBE_DELAY_CTRL			SMIAPP_REG_MK_U16(0x0c20)
+#define SMIAPP_REG_U16_TSA_STROBE_WIDTH_CTRL			SMIAPP_REG_MK_U16(0x0c22)
+#define SMIAPP_REG_U8_SA_STROBE_TRIGGER				SMIAPP_REG_MK_U8(0x0c24)
+#define SMIAPP_REG_U8_SPECIAL_ACTUATOR_STATUS			SMIAPP_REG_MK_U8(0x0c25)
+#define SMIAPP_REG_U16_TFLASH_STROBE_WIDTH2_HIGH_RS_CTRL	SMIAPP_REG_MK_U16(0x0c26)
+#define SMIAPP_REG_U16_TFLASH_STROBE_WIDTH_LOW_RS_CTRL		SMIAPP_REG_MK_U16(0x0c28)
+#define SMIAPP_REG_U8_TFLASH_STROBE_COUNT_RS_CTRL		SMIAPP_REG_MK_U8(0x0c2a)
+#define SMIAPP_REG_U8_TFLASH_STROBE_COUNT_CTRL			SMIAPP_REG_MK_U8(0x0c2b)
+#define SMIAPP_REG_U16_TFLASH_STROBE_WIDTH2_HIGH_CTRL		SMIAPP_REG_MK_U16(0x0c2c)
+#define SMIAPP_REG_U16_TFLASH_STROBE_WIDTH_LOW_CTRL		SMIAPP_REG_MK_U16(0x0c2e)
+#define SMIAPP_REG_U8_LOW_LEVEL_CTRL				SMIAPP_REG_MK_U8(0x0c80)
+#define SMIAPP_REG_U16_MAIN_TRIGGER_REF_POINT			SMIAPP_REG_MK_U16(0x0c82)
+#define SMIAPP_REG_U16_MAIN_TRIGGER_T3				SMIAPP_REG_MK_U16(0x0c84)
+#define SMIAPP_REG_U8_MAIN_TRIGGER_COUNT			SMIAPP_REG_MK_U8(0x0c86)
+#define SMIAPP_REG_U16_PHASE1_TRIGGER_T3			SMIAPP_REG_MK_U16(0x0c88)
+#define SMIAPP_REG_U8_PHASE1_TRIGGER_COUNT			SMIAPP_REG_MK_U8(0x0c8a)
+#define SMIAPP_REG_U16_PHASE2_TRIGGER_T3			SMIAPP_REG_MK_U16(0x0c8c)
+#define SMIAPP_REG_U8_PHASE2_TRIGGER_COUNT			SMIAPP_REG_MK_U8(0x0c8e)
+#define SMIAPP_REG_U8_MECH_SHUTTER_CTRL				SMIAPP_REG_MK_U8(0x0d00)
+#define SMIAPP_REG_U8_OPERATION_MODE				SMIAPP_REG_MK_U8(0x0d01)
+#define SMIAPP_REG_U8_ACT_STATE1				SMIAPP_REG_MK_U8(0x0d02)
+#define SMIAPP_REG_U8_ACT_STATE2				SMIAPP_REG_MK_U8(0x0d03)
+#define SMIAPP_REG_U16_FOCUS_CHANGE				SMIAPP_REG_MK_U16(0x0d80)
+#define SMIAPP_REG_U16_FOCUS_CHANGE_CONTROL			SMIAPP_REG_MK_U16(0x0d82)
+#define SMIAPP_REG_U16_FOCUS_CHANGE_NUMBER_PHASE1		SMIAPP_REG_MK_U16(0x0d84)
+#define SMIAPP_REG_U16_FOCUS_CHANGE_NUMBER_PHASE2		SMIAPP_REG_MK_U16(0x0d86)
+#define SMIAPP_REG_U8_STROBE_COUNT_PHASE1			SMIAPP_REG_MK_U8(0x0d88)
+#define SMIAPP_REG_U8_STROBE_COUNT_PHASE2			SMIAPP_REG_MK_U8(0x0d89)
+#define SMIAPP_REG_U8_POSITION					SMIAPP_REG_MK_U8(0x0d8a)
+#define SMIAPP_REG_U8_BRACKETING_LUT_CONTROL			SMIAPP_REG_MK_U8(0x0e00)
+#define SMIAPP_REG_U8_BRACKETING_LUT_MODE			SMIAPP_REG_MK_U8(0x0e01)
+#define SMIAPP_REG_U8_BRACKETING_LUT_ENTRY_CONTROL		SMIAPP_REG_MK_U8(0x0e02)
+#define SMIAPP_REG_U8_LUT_PARAMETERS_START			SMIAPP_REG_MK_U8(0x0e10)
+#define SMIAPP_REG_U8_LUT_PARAMETERS_END			SMIAPP_REG_MK_U8(0x0eff)
+#define SMIAPP_REG_U16_INTEGRATION_TIME_CAPABILITY		SMIAPP_REG_MK_U16(0x1000)
+#define SMIAPP_REG_U16_COARSE_INTEGRATION_TIME_MIN		SMIAPP_REG_MK_U16(0x1004)
+#define SMIAPP_REG_U16_COARSE_INTEGRATION_TIME_MAX_MARGIN	SMIAPP_REG_MK_U16(0x1006)
+#define SMIAPP_REG_U16_FINE_INTEGRATION_TIME_MIN		SMIAPP_REG_MK_U16(0x1008)
+#define SMIAPP_REG_U16_FINE_INTEGRATION_TIME_MAX_MARGIN		SMIAPP_REG_MK_U16(0x100a)
+#define SMIAPP_REG_U16_DIGITAL_GAIN_CAPABILITY			SMIAPP_REG_MK_U16(0x1080)
+#define SMIAPP_REG_U16_DIGITAL_GAIN_MIN				SMIAPP_REG_MK_U16(0x1084)
+#define SMIAPP_REG_U16_DIGITAL_GAIN_MAX				SMIAPP_REG_MK_U16(0x1086)
+#define SMIAPP_REG_U16_DIGITAL_GAIN_STEP_SIZE			SMIAPP_REG_MK_U16(0x1088)
+#define SMIAPP_REG_F32_MIN_EXT_CLK_FREQ_HZ			SMIAPP_REG_MK_F32(0x1100)
+#define SMIAPP_REG_F32_MAX_EXT_CLK_FREQ_HZ			SMIAPP_REG_MK_F32(0x1104)
+#define SMIAPP_REG_U16_MIN_PRE_PLL_CLK_DIV			SMIAPP_REG_MK_U16(0x1108)
+#define SMIAPP_REG_U16_MAX_PRE_PLL_CLK_DIV			SMIAPP_REG_MK_U16(0x110a)
+#define SMIAPP_REG_F32_MIN_PLL_IP_FREQ_HZ			SMIAPP_REG_MK_F32(0x110c)
+#define SMIAPP_REG_F32_MAX_PLL_IP_FREQ_HZ			SMIAPP_REG_MK_F32(0x1110)
+#define SMIAPP_REG_U16_MIN_PLL_MULTIPLIER			SMIAPP_REG_MK_U16(0x1114)
+#define SMIAPP_REG_U16_MAX_PLL_MULTIPLIER			SMIAPP_REG_MK_U16(0x1116)
+#define SMIAPP_REG_F32_MIN_PLL_OP_FREQ_HZ			SMIAPP_REG_MK_F32(0x1118)
+#define SMIAPP_REG_F32_MAX_PLL_OP_FREQ_HZ			SMIAPP_REG_MK_F32(0x111c)
+#define SMIAPP_REG_U16_MIN_VT_SYS_CLK_DIV			SMIAPP_REG_MK_U16(0x1120)
+#define SMIAPP_REG_U16_MAX_VT_SYS_CLK_DIV			SMIAPP_REG_MK_U16(0x1122)
+#define SMIAPP_REG_F32_MIN_VT_SYS_CLK_FREQ_HZ			SMIAPP_REG_MK_F32(0x1124)
+#define SMIAPP_REG_F32_MAX_VT_SYS_CLK_FREQ_HZ			SMIAPP_REG_MK_F32(0x1128)
+#define SMIAPP_REG_F32_MIN_VT_PIX_CLK_FREQ_HZ			SMIAPP_REG_MK_F32(0x112c)
+#define SMIAPP_REG_F32_MAX_VT_PIX_CLK_FREQ_HZ			SMIAPP_REG_MK_F32(0x1130)
+#define SMIAPP_REG_U16_MIN_VT_PIX_CLK_DIV			SMIAPP_REG_MK_U16(0x1134)
+#define SMIAPP_REG_U16_MAX_VT_PIX_CLK_DIV			SMIAPP_REG_MK_U16(0x1136)
+#define SMIAPP_REG_U16_MIN_FRAME_LENGTH_LINES			SMIAPP_REG_MK_U16(0x1140)
+#define SMIAPP_REG_U16_MAX_FRAME_LENGTH_LINES			SMIAPP_REG_MK_U16(0x1142)
+#define SMIAPP_REG_U16_MIN_LINE_LENGTH_PCK			SMIAPP_REG_MK_U16(0x1144)
+#define SMIAPP_REG_U16_MAX_LINE_LENGTH_PCK			SMIAPP_REG_MK_U16(0x1146)
+#define SMIAPP_REG_U16_MIN_LINE_BLANKING_PCK			SMIAPP_REG_MK_U16(0x1148)
+#define SMIAPP_REG_U16_MIN_FRAME_BLANKING_LINES			SMIAPP_REG_MK_U16(0x114a)
+#define SMIAPP_REG_U8_MIN_LINE_LENGTH_PCK_STEP_SIZE		SMIAPP_REG_MK_U8(0x114c)
+#define SMIAPP_REG_U16_MIN_OP_SYS_CLK_DIV			SMIAPP_REG_MK_U16(0x1160)
+#define SMIAPP_REG_U16_MAX_OP_SYS_CLK_DIV			SMIAPP_REG_MK_U16(0x1162)
+#define SMIAPP_REG_F32_MIN_OP_SYS_CLK_FREQ_HZ			SMIAPP_REG_MK_F32(0x1164)
+#define SMIAPP_REG_F32_MAX_OP_SYS_CLK_FREQ_HZ			SMIAPP_REG_MK_F32(0x1168)
+#define SMIAPP_REG_U16_MIN_OP_PIX_CLK_DIV			SMIAPP_REG_MK_U16(0x116c)
+#define SMIAPP_REG_U16_MAX_OP_PIX_CLK_DIV			SMIAPP_REG_MK_U16(0x116e)
+#define SMIAPP_REG_F32_MIN_OP_PIX_CLK_FREQ_HZ			SMIAPP_REG_MK_F32(0x1170)
+#define SMIAPP_REG_F32_MAX_OP_PIX_CLK_FREQ_HZ			SMIAPP_REG_MK_F32(0x1174)
+#define SMIAPP_REG_U16_X_ADDR_MIN				SMIAPP_REG_MK_U16(0x1180)
+#define SMIAPP_REG_U16_Y_ADDR_MIN				SMIAPP_REG_MK_U16(0x1182)
+#define SMIAPP_REG_U16_X_ADDR_MAX				SMIAPP_REG_MK_U16(0x1184)
+#define SMIAPP_REG_U16_Y_ADDR_MAX				SMIAPP_REG_MK_U16(0x1186)
+#define SMIAPP_REG_U16_MIN_X_OUTPUT_SIZE			SMIAPP_REG_MK_U16(0x1188)
+#define SMIAPP_REG_U16_MIN_Y_OUTPUT_SIZE			SMIAPP_REG_MK_U16(0x118a)
+#define SMIAPP_REG_U16_MAX_X_OUTPUT_SIZE			SMIAPP_REG_MK_U16(0x118c)
+#define SMIAPP_REG_U16_MAX_Y_OUTPUT_SIZE			SMIAPP_REG_MK_U16(0x118e)
+#define SMIAPP_REG_U16_MIN_EVEN_INC				SMIAPP_REG_MK_U16(0x11c0)
+#define SMIAPP_REG_U16_MAX_EVEN_INC				SMIAPP_REG_MK_U16(0x11c2)
+#define SMIAPP_REG_U16_MIN_ODD_INC				SMIAPP_REG_MK_U16(0x11c4)
+#define SMIAPP_REG_U16_MAX_ODD_INC				SMIAPP_REG_MK_U16(0x11c6)
+#define SMIAPP_REG_U16_SCALING_CAPABILITY			SMIAPP_REG_MK_U16(0x1200)
+#define SMIAPP_REG_U16_SCALER_M_MIN				SMIAPP_REG_MK_U16(0x1204)
+#define SMIAPP_REG_U16_SCALER_M_MAX				SMIAPP_REG_MK_U16(0x1206)
+#define SMIAPP_REG_U16_SCALER_N_MIN				SMIAPP_REG_MK_U16(0x1208)
+#define SMIAPP_REG_U16_SCALER_N_MAX				SMIAPP_REG_MK_U16(0x120a)
+#define SMIAPP_REG_U16_SPATIAL_SAMPLING_CAPABILITY		SMIAPP_REG_MK_U16(0x120c)
+#define SMIAPP_REG_U8_DIGITAL_CROP_CAPABILITY			SMIAPP_REG_MK_U8(0x120e)
+#define SMIAPP_REG_U16_COMPRESSION_CAPABILITY			SMIAPP_REG_MK_U16(0x1300)
+#define SMIAPP_REG_U16_MATRIX_ELEMENT_REDINRED			SMIAPP_REG_MK_U16(0x1400)
+#define SMIAPP_REG_U16_MATRIX_ELEMENT_GREENINRED		SMIAPP_REG_MK_U16(0x1402)
+#define SMIAPP_REG_U16_MATRIX_ELEMENT_BLUEINRED			SMIAPP_REG_MK_U16(0x1404)
+#define SMIAPP_REG_U16_MATRIX_ELEMENT_REDINGREEN		SMIAPP_REG_MK_U16(0x1406)
+#define SMIAPP_REG_U16_MATRIX_ELEMENT_GREENINGREEN		SMIAPP_REG_MK_U16(0x1408)
+#define SMIAPP_REG_U16_MATRIX_ELEMENT_BLUEINGREEN		SMIAPP_REG_MK_U16(0x140a)
+#define SMIAPP_REG_U16_MATRIX_ELEMENT_REDINBLUE			SMIAPP_REG_MK_U16(0x140c)
+#define SMIAPP_REG_U16_MATRIX_ELEMENT_GREENINBLUE		SMIAPP_REG_MK_U16(0x140e)
+#define SMIAPP_REG_U16_MATRIX_ELEMENT_BLUEINBLUE		SMIAPP_REG_MK_U16(0x1410)
+#define SMIAPP_REG_U16_FIFO_SIZE_PIXELS				SMIAPP_REG_MK_U16(0x1500)
+#define SMIAPP_REG_U8_FIFO_SUPPORT_CAPABILITY			SMIAPP_REG_MK_U8(0x1502)
+#define SMIAPP_REG_U8_DPHY_CTRL_CAPABILITY			SMIAPP_REG_MK_U8(0x1600)
+#define SMIAPP_REG_U8_CSI_LANE_MODE_CAPABILITY			SMIAPP_REG_MK_U8(0x1601)
+#define SMIAPP_REG_U8_CSI_SIGNALLING_MODE_CAPABILITY		SMIAPP_REG_MK_U8(0x1602)
+#define SMIAPP_REG_U8_FAST_STANDBY_CAPABILITY			SMIAPP_REG_MK_U8(0x1603)
+#define SMIAPP_REG_U8_CCI_ADDRESS_CONTROL_CAPABILITY		SMIAPP_REG_MK_U8(0x1604)
+#define SMIAPP_REG_U32_MAX_PER_LANE_BITRATE_1_LANE_MODE_MBPS	SMIAPP_REG_MK_U32(0x1608)
+#define SMIAPP_REG_U32_MAX_PER_LANE_BITRATE_2_LANE_MODE_MBPS	SMIAPP_REG_MK_U32(0x160c)
+#define SMIAPP_REG_U32_MAX_PER_LANE_BITRATE_3_LANE_MODE_MBPS	SMIAPP_REG_MK_U32(0x1610)
+#define SMIAPP_REG_U32_MAX_PER_LANE_BITRATE_4_LANE_MODE_MBPS	SMIAPP_REG_MK_U32(0x1614)
+#define SMIAPP_REG_U8_TEMP_SENSOR_CAPABILITY			SMIAPP_REG_MK_U8(0x1618)
+#define SMIAPP_REG_U16_MIN_FRAME_LENGTH_LINES_BIN		SMIAPP_REG_MK_U16(0x1700)
+#define SMIAPP_REG_U16_MAX_FRAME_LENGTH_LINES_BIN		SMIAPP_REG_MK_U16(0x1702)
+#define SMIAPP_REG_U16_MIN_LINE_LENGTH_PCK_BIN			SMIAPP_REG_MK_U16(0x1704)
+#define SMIAPP_REG_U16_MAX_LINE_LENGTH_PCK_BIN			SMIAPP_REG_MK_U16(0x1706)
+#define SMIAPP_REG_U16_MIN_LINE_BLANKING_PCK_BIN		SMIAPP_REG_MK_U16(0x1708)
+#define SMIAPP_REG_U16_FINE_INTEGRATION_TIME_MIN_BIN		SMIAPP_REG_MK_U16(0x170a)
+#define SMIAPP_REG_U16_FINE_INTEGRATION_TIME_MAX_MARGIN_BIN	SMIAPP_REG_MK_U16(0x170c)
+#define SMIAPP_REG_U8_BINNING_CAPABILITY			SMIAPP_REG_MK_U8(0x1710)
+#define SMIAPP_REG_U8_BINNING_WEIGHTING_CAPABILITY		SMIAPP_REG_MK_U8(0x1711)
+#define SMIAPP_REG_U8_BINNING_SUBTYPES				SMIAPP_REG_MK_U8(0x1712)
+#define SMIAPP_REG_U8_BINNING_TYPE_n(n)				SMIAPP_REG_MK_U8(0x1713 + (n)) /* 1 <= n <= 237 */
+#define SMIAPP_REG_U8_DATA_TRANSFER_IF_CAPABILITY		SMIAPP_REG_MK_U8(0x1800)
+#define SMIAPP_REG_U8_SHADING_CORRECTION_CAPABILITY		SMIAPP_REG_MK_U8(0x1900)
+#define SMIAPP_REG_U8_GREEN_IMBALANCE_CAPABILITY		SMIAPP_REG_MK_U8(0x1901)
+#define SMIAPP_REG_U8_BLACK_LEVEL_CAPABILITY			SMIAPP_REG_MK_U8(0x1902)
+#define SMIAPP_REG_U8_MODULE_SPECIFIC_CORRECTION_CAPABILITY	SMIAPP_REG_MK_U8(0x1903)
+#define SMIAPP_REG_U16_DEFECT_CORRECTION_CAPABILITY		SMIAPP_REG_MK_U16(0x1904)
+#define SMIAPP_REG_U16_DEFECT_CORRECTION_CAPABILITY_2		SMIAPP_REG_MK_U16(0x1906)
+#define SMIAPP_REG_U8_EDOF_CAPABILITY				SMIAPP_REG_MK_U8(0x1980)
+#define SMIAPP_REG_U8_ESTIMATION_FRAMES				SMIAPP_REG_MK_U8(0x1981)
+#define SMIAPP_REG_U8_SUPPORTS_SHARPNESS_ADJ			SMIAPP_REG_MK_U8(0x1982)
+#define SMIAPP_REG_U8_SUPPORTS_DENOISING_ADJ			SMIAPP_REG_MK_U8(0x1983)
+#define SMIAPP_REG_U8_SUPPORTS_MODULE_SPECIFIC_ADJ		SMIAPP_REG_MK_U8(0x1984)
+#define SMIAPP_REG_U8_SUPPORTS_DEPTH_OF_FIELD_ADJ		SMIAPP_REG_MK_U8(0x1985)
+#define SMIAPP_REG_U8_SUPPORTS_FOCUS_DISTANCE_ADJ		SMIAPP_REG_MK_U8(0x1986)
+#define SMIAPP_REG_U8_COLOUR_FEEDBACK_CAPABILITY		SMIAPP_REG_MK_U8(0x1987)
+#define SMIAPP_REG_U8_EDOF_SUPPORT_AB_NXM			SMIAPP_REG_MK_U8(0x1988)
+#define SMIAPP_REG_U8_ESTIMATION_MODE_CAPABILITY		SMIAPP_REG_MK_U8(0x19c0)
+#define SMIAPP_REG_U8_ESTIMATION_ZONE_CAPABILITY		SMIAPP_REG_MK_U8(0x19c1)
+#define SMIAPP_REG_U16_EST_DEPTH_OF_FIELD			SMIAPP_REG_MK_U16(0x19c2)
+#define SMIAPP_REG_U16_EST_FOCUS_DISTANCE			SMIAPP_REG_MK_U16(0x19c4)
+#define SMIAPP_REG_U16_CAPABILITY_TRDY_MIN			SMIAPP_REG_MK_U16(0x1a00)
+#define SMIAPP_REG_U8_FLASH_MODE_CAPABILITY			SMIAPP_REG_MK_U8(0x1a02)
+#define SMIAPP_REG_U16_MECH_SHUT_AND_ACT_START_ADDR		SMIAPP_REG_MK_U16(0x1b02)
+#define SMIAPP_REG_U8_ACTUATOR_CAPABILITY			SMIAPP_REG_MK_U8(0x1b04)
+#define SMIAPP_REG_U16_ACTUATOR_TYPE				SMIAPP_REG_MK_U16(0x1b40)
+#define SMIAPP_REG_U8_AF_DEVICE_ADDRESS				SMIAPP_REG_MK_U8(0x1b42)
+#define SMIAPP_REG_U16_FOCUS_CHANGE_ADDRESS			SMIAPP_REG_MK_U16(0x1b44)
+#define SMIAPP_REG_U8_BRACKETING_LUT_CAPABILITY_1		SMIAPP_REG_MK_U8(0x1c00)
+#define SMIAPP_REG_U8_BRACKETING_LUT_CAPABILITY_2		SMIAPP_REG_MK_U8(0x1c01)
+#define SMIAPP_REG_U8_BRACKETING_LUT_SIZE			SMIAPP_REG_MK_U8(0x1c02)
diff --git a/drivers/media/video/smiapp/smiapp-reg.h b/drivers/media/video/smiapp/smiapp-reg.h
new file mode 100644
index 00000000000..d0167aa1753
--- /dev/null
+++ b/drivers/media/video/smiapp/smiapp-reg.h
@@ -0,0 +1,122 @@
+/*
+ * drivers/media/video/smiapp/smiapp-reg.h
+ *
+ * Generic driver for SMIA/SMIA++ compliant camera modules
+ *
+ * Copyright (C) 2011--2012 Nokia Corporation
+ * Contact: Sakari Ailus <sakari.ailus@maxwell.research.nokia.com>
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * version 2 as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+ * General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA
+ * 02110-1301 USA
+ *
+ */
+
+#ifndef __SMIAPP_REG_H_
+#define __SMIAPP_REG_H_
+
+#include "smiapp-reg-defs.h"
+
+/* Bits for above register */
+#define SMIAPP_IMAGE_ORIENTATION_HFLIP		(1 << 0)
+#define SMIAPP_IMAGE_ORIENTATION_VFLIP		(1 << 1)
+
+#define SMIAPP_DATA_TRANSFER_IF_1_CTRL_EN		(1 << 0)
+#define SMIAPP_DATA_TRANSFER_IF_1_CTRL_RD_EN		(0 << 1)
+#define SMIAPP_DATA_TRANSFER_IF_1_CTRL_WR_EN		(1 << 1)
+#define SMIAPP_DATA_TRANSFER_IF_1_CTRL_ERR_CLEAR	(1 << 2)
+#define SMIAPP_DATA_TRANSFER_IF_1_STATUS_RD_READY	(1 << 0)
+#define SMIAPP_DATA_TRANSFER_IF_1_STATUS_WR_READY	(1 << 1)
+#define SMIAPP_DATA_TRANSFER_IF_1_STATUS_EDATA		(1 << 2)
+#define SMIAPP_DATA_TRANSFER_IF_1_STATUS_EUSAGE		(1 << 3)
+
+#define SMIAPP_SOFTWARE_RESET				(1 << 0)
+
+#define SMIAPP_FLASH_MODE_CAPABILITY_SINGLE_STROBE	(1 << 0)
+#define SMIAPP_FLASH_MODE_CAPABILITY_MULTIPLE_STROBE	(1 << 1)
+
+#define SMIAPP_DPHY_CTRL_AUTOMATIC			0
+/* DPHY control based on REQUESTED_LINK_BIT_RATE_MBPS */
+#define SMIAPP_DPHY_CTRL_UI				1
+#define SMIAPP_DPHY_CTRL_REGISTER			2
+
+#define SMIAPP_COMPRESSION_MODE_SIMPLE_PREDICTOR	1
+#define SMIAPP_COMPRESSION_MODE_ADVANCED_PREDICTOR	2
+
+#define SMIAPP_MODE_SELECT_SOFTWARE_STANDBY		0
+#define SMIAPP_MODE_SELECT_STREAMING			1
+
+#define SMIAPP_SCALING_MODE_NONE			0
+#define SMIAPP_SCALING_MODE_HORIZONTAL			1
+#define SMIAPP_SCALING_MODE_BOTH			2
+
+#define SMIAPP_SCALING_CAPABILITY_NONE			0
+#define SMIAPP_SCALING_CAPABILITY_HORIZONTAL		1
+#define SMIAPP_SCALING_CAPABILITY_BOTH			2 /* horizontal/both */
+
+/* digital crop right before scaler */
+#define SMIAPP_DIGITAL_CROP_CAPABILITY_NONE		0
+#define SMIAPP_DIGITAL_CROP_CAPABILITY_INPUT_CROP	1
+
+#define SMIAPP_BINNING_CAPABILITY_NO			0
+#define SMIAPP_BINNING_CAPABILITY_YES			1
+
+/* Maximum number of binning subtypes */
+#define SMIAPP_BINNING_SUBTYPES				253
+
+#define SMIAPP_PIXEL_ORDER_GRBG				0
+#define SMIAPP_PIXEL_ORDER_RGGB				1
+#define SMIAPP_PIXEL_ORDER_BGGR				2
+#define SMIAPP_PIXEL_ORDER_GBRG				3
+
+#define SMIAPP_DATA_FORMAT_MODEL_TYPE_NORMAL		1
+#define SMIAPP_DATA_FORMAT_MODEL_TYPE_EXTENDED		2
+#define SMIAPP_DATA_FORMAT_MODEL_TYPE_NORMAL_N		8
+#define SMIAPP_DATA_FORMAT_MODEL_TYPE_EXTENDED_N	16
+
+#define SMIAPP_FRAME_FORMAT_MODEL_TYPE_2BYTE		0x01
+#define SMIAPP_FRAME_FORMAT_MODEL_TYPE_4BYTE		0x02
+#define SMIAPP_FRAME_FORMAT_MODEL_SUBTYPE_NROWS_MASK	0x0f
+#define SMIAPP_FRAME_FORMAT_MODEL_SUBTYPE_NCOLS_MASK	0xf0
+#define SMIAPP_FRAME_FORMAT_MODEL_SUBTYPE_NCOLS_SHIFT	4
+
+#define SMIAPP_FRAME_FORMAT_DESC_2_PIXELCODE_MASK	0xf000
+#define SMIAPP_FRAME_FORMAT_DESC_2_PIXELCODE_SHIFT	12
+#define SMIAPP_FRAME_FORMAT_DESC_2_PIXELS_MASK		0x0fff
+
+#define SMIAPP_FRAME_FORMAT_DESC_4_PIXELCODE_MASK	0xf0000000
+#define SMIAPP_FRAME_FORMAT_DESC_4_PIXELCODE_SHIFT	28
+#define SMIAPP_FRAME_FORMAT_DESC_4_PIXELS_MASK		0x0000ffff
+
+#define SMIAPP_FRAME_FORMAT_DESC_PIXELCODE_EMBEDDED	1
+#define SMIAPP_FRAME_FORMAT_DESC_PIXELCODE_DUMMY	2
+#define SMIAPP_FRAME_FORMAT_DESC_PIXELCODE_BLACK	3
+#define SMIAPP_FRAME_FORMAT_DESC_PIXELCODE_DARK		4
+#define SMIAPP_FRAME_FORMAT_DESC_PIXELCODE_VISIBLE	5
+
+#define SMIAPP_FAST_STANDBY_CTRL_COMPLETE_FRAMES	0
+#define SMIAPP_FAST_STANDBY_CTRL_IMMEDIATE		1
+
+/* Scaling N factor */
+#define SMIAPP_SCALE_N					16
+
+/* Image statistics registers */
+/* Registers 0x2000 to 0x2fff are reserved for future
+ * use for statistics features.
+ */
+
+/* Manufacturer Specific Registers: 0x3000 to 0x3fff
+ * The manufacturer specifies these as a black box.
+ */
+
+#endif /* __SMIAPP_REG_H_ */
diff --git a/drivers/media/video/smiapp/smiapp-regs.c b/drivers/media/video/smiapp/smiapp-regs.c
new file mode 100644
index 00000000000..b1812b17a40
--- /dev/null
+++ b/drivers/media/video/smiapp/smiapp-regs.c
@@ -0,0 +1,273 @@
+/*
+ * drivers/media/video/smiapp/smiapp-regs.c
+ *
+ * Generic driver for SMIA/SMIA++ compliant camera modules
+ *
+ * Copyright (C) 2011--2012 Nokia Corporation
+ * Contact: Sakari Ailus <sakari.ailus@maxwell.research.nokia.com>
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * version 2 as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+ * General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA
+ * 02110-1301 USA
+ *
+ */
+
+#include <linux/delay.h>
+#include <linux/i2c.h>
+
+#include "smiapp.h"
+#include "smiapp-regs.h"
+
+static uint32_t float_to_u32_mul_1000000(struct i2c_client *client,
+					 uint32_t phloat)
+{
+	int32_t exp;
+	uint64_t man;
+
+	if (phloat >= 0x80000000) {
+		dev_err(&client->dev, "this is a negative number\n");
+		return 0;
+	}
+
+	if (phloat == 0x7f800000)
+		return ~0; /* Inf. */
+
+	if ((phloat & 0x7f800000) == 0x7f800000) {
+		dev_err(&client->dev, "NaN or other special number\n");
+		return 0;
+	}
+
+	/* Valid cases begin here */
+	if (phloat == 0)
+		return 0; /* Valid zero */
+
+	if (phloat > 0x4f800000)
+		return ~0; /* larger than 4294967295 */
+
+	/*
+	 * Unbias exponent (note how phloat is now guaranteed to
+	 * have 0 in the high bit)
+	 */
+	exp = ((int32_t)phloat >> 23) - 127;
+
+	/* Extract mantissa, add missing '1' bit and it's in MHz */
+	man = ((phloat & 0x7fffff) | 0x800000) * 1000000ULL;
+
+	if (exp < 0)
+		man >>= -exp;
+	else
+		man <<= exp;
+
+	man >>= 23; /* Remove mantissa bias */
+
+	return man & 0xffffffff;
+}
+
+
+/*
+ * Read a 8/16/32-bit i2c register.  The value is returned in 'val'.
+ * Returns zero if successful, or non-zero otherwise.
+ */
+static int ____smiapp_read(struct smiapp_sensor *sensor, u16 reg,
+			   u16 len, u32 *val)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(&sensor->src->sd);
+	struct i2c_msg msg;
+	unsigned char data[4];
+	u16 offset = reg;
+	int r;
+
+	msg.addr = client->addr;
+	msg.flags = 0;
+	msg.len = 2;
+	msg.buf = data;
+
+	/* high byte goes out first */
+	data[0] = (u8) (offset >> 8);
+	data[1] = (u8) offset;
+	r = i2c_transfer(client->adapter, &msg, 1);
+	if (r != 1) {
+		if (r >= 0)
+			r = -EBUSY;
+		goto err;
+	}
+
+	msg.len = len;
+	msg.flags = I2C_M_RD;
+	r = i2c_transfer(client->adapter, &msg, 1);
+	if (r != 1) {
+		if (r >= 0)
+			r = -EBUSY;
+		goto err;
+	}
+
+	*val = 0;
+	/* high byte comes first */
+	switch (len) {
+	case SMIA_REG_32BIT:
+		*val = (data[0] << 24) + (data[1] << 16) + (data[2] << 8) +
+			data[3];
+		break;
+	case SMIA_REG_16BIT:
+		*val = (data[0] << 8) + data[1];
+		break;
+	case SMIA_REG_8BIT:
+		*val = data[0];
+		break;
+	default:
+		BUG();
+	}
+
+	return 0;
+
+err:
+	dev_err(&client->dev, "read from offset 0x%x error %d\n", offset, r);
+
+	return r;
+}
+
+/* Read a register using 8-bit access only. */
+static int ____smiapp_read_8only(struct smiapp_sensor *sensor, u16 reg,
+				 u16 len, u32 *val)
+{
+	unsigned int i;
+	int rval;
+
+	*val = 0;
+
+	for (i = 0; i < len; i++) {
+		u32 val8;
+
+		rval = ____smiapp_read(sensor, reg + i, 1, &val8);
+		if (rval < 0)
+			return rval;
+		*val |= val8 << ((len - i - 1) << 3);
+	}
+
+	return 0;
+}
+
+/*
+ * Read a 8/16/32-bit i2c register.  The value is returned in 'val'.
+ * Returns zero if successful, or non-zero otherwise.
+ */
+static int __smiapp_read(struct smiapp_sensor *sensor, u32 reg, u32 *val,
+			 bool only8)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(&sensor->src->sd);
+	unsigned int len = (u8)(reg >> 16);
+	int rval;
+
+	if (len != SMIA_REG_8BIT && len != SMIA_REG_16BIT
+	    && len != SMIA_REG_32BIT)
+		return -EINVAL;
+
+	if (smiapp_quirk_reg(sensor, reg, val))
+		goto found_quirk;
+
+	if (len == SMIA_REG_8BIT && !only8)
+		rval = ____smiapp_read(sensor, (u16)reg, len, val);
+	else
+		rval = ____smiapp_read_8only(sensor, (u16)reg, len, val);
+	if (rval < 0)
+		return rval;
+
+found_quirk:
+	if (reg & SMIA_REG_FLAG_FLOAT)
+		*val = float_to_u32_mul_1000000(client, *val);
+
+	return 0;
+}
+
+int smiapp_read(struct smiapp_sensor *sensor, u32 reg, u32 *val)
+{
+	return __smiapp_read(
+		sensor, reg, val,
+		smiapp_needs_quirk(sensor,
+				   SMIAPP_QUIRK_FLAG_8BIT_READ_ONLY));
+}
+
+int smiapp_read_8only(struct smiapp_sensor *sensor, u32 reg, u32 *val)
+{
+	return __smiapp_read(sensor, reg, val, true);
+}
+
+/*
+ * Write to a 8/16-bit register.
+ * Returns zero if successful, or non-zero otherwise.
+ */
+int smiapp_write(struct smiapp_sensor *sensor, u32 reg, u32 val)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(&sensor->src->sd);
+	struct i2c_msg msg;
+	unsigned char data[6];
+	unsigned int retries;
+	unsigned int flags = reg >> 24;
+	unsigned int len = (u8)(reg >> 16);
+	u16 offset = reg;
+	int r;
+
+	if ((len != SMIA_REG_8BIT && len != SMIA_REG_16BIT &&
+	     len != SMIA_REG_32BIT) || flags)
+		return -EINVAL;
+
+	msg.addr = client->addr;
+	msg.flags = 0; /* Write */
+	msg.len = 2 + len;
+	msg.buf = data;
+
+	/* high byte goes out first */
+	data[0] = (u8) (reg >> 8);
+	data[1] = (u8) (reg & 0xff);
+
+	switch (len) {
+	case SMIA_REG_8BIT:
+		data[2] = val;
+		break;
+	case SMIA_REG_16BIT:
+		data[2] = val >> 8;
+		data[3] = val;
+		break;
+	case SMIA_REG_32BIT:
+		data[2] = val >> 24;
+		data[3] = val >> 16;
+		data[4] = val >> 8;
+		data[5] = val;
+		break;
+	default:
+		BUG();
+	}
+
+	for (retries = 0; retries < 5; retries++) {
+		/*
+		 * Due to unknown reason sensor stops responding. This
+		 * loop is a temporaty solution until the root cause
+		 * is found.
+		 */
+		r = i2c_transfer(client->adapter, &msg, 1);
+		if (r == 1) {
+			if (retries)
+				dev_err(&client->dev,
+					"sensor i2c stall encountered. "
+					"retries: %d\n", retries);
+			return 0;
+		}
+
+		usleep_range(2000, 2000);
+	}
+
+	dev_err(&client->dev,
+		"wrote 0x%x to offset 0x%x error %d\n", val, offset, r);
+
+	return r;
+}
diff --git a/drivers/media/video/smiapp/smiapp-regs.h b/drivers/media/video/smiapp/smiapp-regs.h
new file mode 100644
index 00000000000..7f9013b4797
--- /dev/null
+++ b/drivers/media/video/smiapp/smiapp-regs.h
@@ -0,0 +1,49 @@
+/*
+ * include/media/smiapp/smiapp-regs.h
+ *
+ * Generic driver for SMIA/SMIA++ compliant camera modules
+ *
+ * Copyright (C) 2011--2012 Nokia Corporation
+ * Contact: Sakari Ailus <sakari.ailus@maxwell.research.nokia.com>
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * version 2 as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+ * General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA
+ * 02110-1301 USA
+ *
+ */
+
+#ifndef SMIAPP_REGS_H
+#define SMIAPP_REGS_H
+
+#include <linux/i2c.h>
+#include <linux/types.h>
+
+/* Use upper 8 bits of the type field for flags */
+#define SMIA_REG_FLAG_FLOAT		(1 << 24)
+
+#define SMIA_REG_8BIT			1
+#define SMIA_REG_16BIT			2
+#define SMIA_REG_32BIT			4
+struct smia_reg {
+	u16 type;
+	u16 reg;			/* 16-bit offset */
+	u32 val;			/* 8/16/32-bit value */
+};
+
+struct smiapp_sensor;
+
+int smiapp_read(struct smiapp_sensor *sensor, u32 reg, u32 *val);
+int smiapp_read_8only(struct smiapp_sensor *sensor, u32 reg, u32 *val);
+int smiapp_write(struct smiapp_sensor *sensor, u32 reg, u32 val);
+
+#endif
diff --git a/drivers/media/video/smiapp/smiapp.h b/drivers/media/video/smiapp/smiapp.h
new file mode 100644
index 00000000000..587f7f11238
--- /dev/null
+++ b/drivers/media/video/smiapp/smiapp.h
@@ -0,0 +1,252 @@
+/*
+ * drivers/media/video/smiapp/smiapp.h
+ *
+ * Generic driver for SMIA/SMIA++ compliant camera modules
+ *
+ * Copyright (C) 2010--2012 Nokia Corporation
+ * Contact: Sakari Ailus <sakari.ailus@maxwell.research.nokia.com>
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * version 2 as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+ * General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA
+ * 02110-1301 USA
+ *
+ */
+
+#ifndef __SMIAPP_PRIV_H_
+#define __SMIAPP_PRIV_H_
+
+#include <linux/mutex.h>
+#include <media/v4l2-ctrls.h>
+#include <media/v4l2-subdev.h>
+#include <media/smiapp.h>
+
+#include "smiapp-pll.h"
+#include "smiapp-reg.h"
+#include "smiapp-regs.h"
+#include "smiapp-quirk.h"
+
+/*
+ * Standard SMIA++ constants
+ */
+#define SMIA_VERSION_1			10
+#define SMIAPP_VERSION_0_8		8 /* Draft 0.8 */
+#define SMIAPP_VERSION_0_9		9 /* Draft 0.9 */
+#define SMIAPP_VERSION_1		10
+
+#define SMIAPP_PROFILE_0		0
+#define SMIAPP_PROFILE_1		1
+#define SMIAPP_PROFILE_2		2
+
+#define SMIAPP_NVM_PAGE_SIZE		64	/* bytes */
+
+#define SMIAPP_RESET_DELAY_CLOCKS	2400
+#define SMIAPP_RESET_DELAY(clk)				\
+	(1000 +	(SMIAPP_RESET_DELAY_CLOCKS * 1000	\
+		 + (clk) / 1000 - 1) / ((clk) / 1000))
+
+#include "smiapp-limits.h"
+
+struct smiapp_quirk;
+
+#define SMIAPP_MODULE_IDENT_FLAG_REV_LE		(1 << 0)
+
+struct smiapp_module_ident {
+	u8 manufacturer_id;
+	u16 model_id;
+	u8 revision_number_major;
+
+	u8 flags;
+
+	char *name;
+	const struct smiapp_quirk *quirk;
+};
+
+struct smiapp_module_info {
+	u32 manufacturer_id;
+	u32 model_id;
+	u32 revision_number_major;
+	u32 revision_number_minor;
+
+	u32 module_year;
+	u32 module_month;
+	u32 module_day;
+
+	u32 sensor_manufacturer_id;
+	u32 sensor_model_id;
+	u32 sensor_revision_number;
+	u32 sensor_firmware_version;
+
+	u32 smia_version;
+	u32 smiapp_version;
+
+	u32 smiapp_profile;
+
+	char *name;
+	const struct smiapp_quirk *quirk;
+};
+
+#define SMIAPP_IDENT_FQ(manufacturer, model, rev, fl, _name, _quirk)	\
+	{ .manufacturer_id = manufacturer,				\
+	  .model_id = model,						\
+	  .revision_number_major = rev,					\
+	  .flags = fl,							\
+	  .name = _name,						\
+	  .quirk = _quirk, }
+
+#define SMIAPP_IDENT_LQ(manufacturer, model, rev, _name, _quirk)	\
+	{ .manufacturer_id = manufacturer,				\
+	  .model_id = model,						\
+	  .revision_number_major = rev,					\
+	  .flags = SMIAPP_MODULE_IDENT_FLAG_REV_LE,			\
+	  .name = _name,						\
+	  .quirk = _quirk, }
+
+#define SMIAPP_IDENT_L(manufacturer, model, rev, _name)			\
+	{ .manufacturer_id = manufacturer,				\
+	  .model_id = model,						\
+	  .revision_number_major = rev,					\
+	  .flags = SMIAPP_MODULE_IDENT_FLAG_REV_LE,			\
+	  .name = _name, }
+
+#define SMIAPP_IDENT_Q(manufacturer, model, rev, _name, _quirk)		\
+	{ .manufacturer_id = manufacturer,				\
+	  .model_id = model,						\
+	  .revision_number_major = rev,					\
+	  .flags = 0,							\
+	  .name = _name,						\
+	  .quirk = _quirk, }
+
+#define SMIAPP_IDENT(manufacturer, model, rev, _name)			\
+	{ .manufacturer_id = manufacturer,				\
+	  .model_id = model,						\
+	  .revision_number_major = rev,					\
+	  .flags = 0,							\
+	  .name = _name, }
+
+struct smiapp_reg_limits {
+	u32 addr;
+	char *what;
+};
+
+extern struct smiapp_reg_limits smiapp_reg_limits[];
+
+struct smiapp_csi_data_format {
+	u32 code;
+	u8 width;
+	u8 compressed;
+	u8 pixel_order;
+};
+
+#define SMIAPP_SUBDEVS			3
+
+#define SMIAPP_PA_PAD_SRC		0
+#define SMIAPP_PAD_SINK			0
+#define SMIAPP_PAD_SRC			1
+#define SMIAPP_PADS			2
+
+struct smiapp_binning_subtype {
+	u8 horizontal:4;
+	u8 vertical:4;
+} __packed;
+
+struct smiapp_subdev {
+	struct v4l2_subdev sd;
+	struct media_pad pads[2];
+	struct v4l2_rect sink_fmt;
+	struct v4l2_rect crop[2];
+	struct v4l2_rect compose; /* compose on sink */
+	unsigned short sink_pad;
+	unsigned short source_pad;
+	int npads;
+	struct smiapp_sensor *sensor;
+	struct v4l2_ctrl_handler ctrl_handler;
+};
+
+/*
+ * struct smiapp_sensor - Main device structure
+ */
+struct smiapp_sensor {
+	/*
+	 * "mutex" is used to serialise access to all fields here
+	 * except v4l2_ctrls at the end of the struct. "mutex" is also
+	 * used to serialise access to file handle specific
+	 * information. The exception to this rule is the power_mutex
+	 * below.
+	 */
+	struct mutex mutex;
+	/*
+	 * power_mutex is used to serialise power management related
+	 * activities. Acquiring "mutex" at that time isn't necessary
+	 * since there are no other users anyway.
+	 */
+	struct mutex power_mutex;
+	struct smiapp_subdev ssds[SMIAPP_SUBDEVS];
+	u32 ssds_used;
+	struct smiapp_subdev *src;
+	struct smiapp_subdev *binner;
+	struct smiapp_subdev *scaler;
+	struct smiapp_subdev *pixel_array;
+	struct smiapp_platform_data *platform_data;
+	struct regulator *vana;
+	struct clk *ext_clk;
+	u32 limits[SMIAPP_LIMIT_LAST];
+	u8 nbinning_subtypes;
+	struct smiapp_binning_subtype binning_subtypes[SMIAPP_BINNING_SUBTYPES];
+	u32 mbus_frame_fmts;
+	const struct smiapp_csi_data_format *csi_format;
+	const struct smiapp_csi_data_format *internal_csi_format;
+	u32 default_mbus_frame_fmts;
+	int default_pixel_order;
+
+	u8 binning_horizontal;
+	u8 binning_vertical;
+
+	u8 scale_m;
+	u8 scaling_mode;
+
+	u8 hvflip_inv_mask; /* H/VFLIP inversion due to sensor orientation */
+	u8 flash_capability;
+	u8 frame_skip;
+
+	int power_count;
+
+	bool streaming;
+	bool dev_init_done;
+
+	u8 *nvm;		/* nvm memory buffer */
+	unsigned int nvm_size;	/* bytes */
+
+	struct smiapp_module_info minfo;
+
+	struct smiapp_pll pll;
+
+	/* Pixel array controls */
+	struct v4l2_ctrl *analog_gain;
+	struct v4l2_ctrl *exposure;
+	struct v4l2_ctrl *hflip;
+	struct v4l2_ctrl *vflip;
+	struct v4l2_ctrl *vblank;
+	struct v4l2_ctrl *hblank;
+	struct v4l2_ctrl *pixel_rate_parray;
+	/* src controls */
+	struct v4l2_ctrl *link_freq;
+	struct v4l2_ctrl *pixel_rate_csi;
+};
+
+#define to_smiapp_subdev(_sd)				\
+	container_of(_sd, struct smiapp_subdev, sd)
+
+#define to_smiapp_sensor(_sd)	\
+	(to_smiapp_subdev(_sd)->sensor)
+
+#endif /* __SMIAPP_PRIV_H_ */
diff --git a/drivers/media/video/sn9c102/sn9c102_core.c b/drivers/media/video/sn9c102/sn9c102_core.c
index c2882fa5be8..19ea780b16f 100644
--- a/drivers/media/video/sn9c102/sn9c102_core.c
+++ b/drivers/media/video/sn9c102/sn9c102_core.c
@@ -995,10 +995,8 @@ static int sn9c102_stop_transfer(struct sn9c102_device* cam)
 
 static int sn9c102_stream_interrupt(struct sn9c102_device* cam)
 {
-	long timeout;
-
 	cam->stream = STREAM_INTERRUPT;
-	timeout = wait_event_timeout(cam->wait_stream,
+	wait_event_timeout(cam->wait_stream,
 				     (cam->stream == STREAM_OFF) ||
 				     (cam->state & DEV_DISCONNECTED),
 				     SN9C102_URB_TIMEOUT);
diff --git a/drivers/media/video/soc_camera.c b/drivers/media/video/soc_camera.c
index eb25756a07a..0421bf9453b 100644
--- a/drivers/media/video/soc_camera.c
+++ b/drivers/media/video/soc_camera.c
@@ -164,35 +164,38 @@ static int soc_camera_try_fmt(struct soc_camera_device *icd,
 			      struct v4l2_format *f)
 {
 	struct soc_camera_host *ici = to_soc_camera_host(icd->parent);
+	const struct soc_camera_format_xlate *xlate;
 	struct v4l2_pix_format *pix = &f->fmt.pix;
 	int ret;
 
 	dev_dbg(icd->pdev, "TRY_FMT(%c%c%c%c, %ux%u)\n",
 		pixfmtstr(pix->pixelformat), pix->width, pix->height);
 
-	pix->bytesperline = 0;
-	pix->sizeimage = 0;
+	if (!(ici->capabilities & SOCAM_HOST_CAP_STRIDE)) {
+		pix->bytesperline = 0;
+		pix->sizeimage = 0;
+	}
 
 	ret = ici->ops->try_fmt(icd, f);
 	if (ret < 0)
 		return ret;
 
-	if (!pix->sizeimage) {
-		if (!pix->bytesperline) {
-			const struct soc_camera_format_xlate *xlate;
+	xlate = soc_camera_xlate_by_fourcc(icd, pix->pixelformat);
+	if (!xlate)
+		return -EINVAL;
 
-			xlate = soc_camera_xlate_by_fourcc(icd, pix->pixelformat);
-			if (!xlate)
-				return -EINVAL;
+	ret = soc_mbus_bytes_per_line(pix->width, xlate->host_fmt);
+	if (ret < 0)
+		return ret;
 
-			ret = soc_mbus_bytes_per_line(pix->width,
-						      xlate->host_fmt);
-			if (ret > 0)
-				pix->bytesperline = ret;
-		}
-		if (pix->bytesperline)
-			pix->sizeimage = pix->bytesperline * pix->height;
-	}
+	pix->bytesperline = max_t(u32, pix->bytesperline, ret);
+
+	ret = soc_mbus_image_size(xlate->host_fmt, pix->bytesperline,
+				  pix->height);
+	if (ret < 0)
+		return ret;
+
+	pix->sizeimage = max_t(u32, pix->sizeimage, ret);
 
 	return 0;
 }
@@ -257,13 +260,13 @@ static int soc_camera_g_std(struct file *file, void *priv, v4l2_std_id *a)
 	return v4l2_subdev_call(sd, core, g_std, a);
 }
 
-static int soc_camera_enum_fsizes(struct file *file, void *fh,
+static int soc_camera_enum_framesizes(struct file *file, void *fh,
 					 struct v4l2_frmsizeenum *fsize)
 {
 	struct soc_camera_device *icd = file->private_data;
 	struct soc_camera_host *ici = to_soc_camera_host(icd->parent);
 
-	return ici->ops->enum_fsizes(icd, fsize);
+	return ici->ops->enum_framesizes(icd, fsize);
 }
 
 static int soc_camera_reqbufs(struct file *file, void *priv,
@@ -530,7 +533,10 @@ static int soc_camera_open(struct file *file)
 		if (icl->reset)
 			icl->reset(icd->pdev);
 
+		/* Don't mess with the host during probe */
+		mutex_lock(&ici->host_lock);
 		ret = ici->ops->add(icd);
+		mutex_unlock(&ici->host_lock);
 		if (ret < 0) {
 			dev_err(icd->pdev, "Couldn't activate the camera: %d\n", ret);
 			goto eiciadd;
@@ -956,7 +962,7 @@ static void scan_add_host(struct soc_camera_host *ici)
 {
 	struct soc_camera_device *icd;
 
-	mutex_lock(&list_lock);
+	mutex_lock(&ici->host_lock);
 
 	list_for_each_entry(icd, &devices, list) {
 		if (icd->iface == ici->nr) {
@@ -967,7 +973,7 @@ static void scan_add_host(struct soc_camera_host *ici)
 		}
 	}
 
-	mutex_unlock(&list_lock);
+	mutex_unlock(&ici->host_lock);
 }
 
 #ifdef CONFIG_I2C_BOARDINFO
@@ -1241,8 +1247,8 @@ static int default_s_parm(struct soc_camera_device *icd,
 	return v4l2_subdev_call(sd, video, s_parm, parm);
 }
 
-static int default_enum_fsizes(struct soc_camera_device *icd,
-			  struct v4l2_frmsizeenum *fsize)
+static int default_enum_framesizes(struct soc_camera_device *icd,
+				   struct v4l2_frmsizeenum *fsize)
 {
 	int ret;
 	struct v4l2_subdev *sd = soc_camera_to_subdev(icd);
@@ -1256,7 +1262,7 @@ static int default_enum_fsizes(struct soc_camera_device *icd,
 	/* map xlate-code to pixel_format, sensor only handle xlate-code*/
 	fsize_mbus.pixel_format = xlate->code;
 
-	ret = v4l2_subdev_call(sd, video, enum_mbus_fsizes, &fsize_mbus);
+	ret = v4l2_subdev_call(sd, video, enum_framesizes, &fsize_mbus);
 	if (ret < 0)
 		return ret;
 
@@ -1295,8 +1301,8 @@ int soc_camera_host_register(struct soc_camera_host *ici)
 		ici->ops->set_parm = default_s_parm;
 	if (!ici->ops->get_parm)
 		ici->ops->get_parm = default_g_parm;
-	if (!ici->ops->enum_fsizes)
-		ici->ops->enum_fsizes = default_enum_fsizes;
+	if (!ici->ops->enum_framesizes)
+		ici->ops->enum_framesizes = default_enum_framesizes;
 
 	mutex_lock(&list_lock);
 	list_for_each_entry(ix, &hosts, list) {
@@ -1313,6 +1319,7 @@ int soc_camera_host_register(struct soc_camera_host *ici)
 	list_add_tail(&ici->list, &hosts);
 	mutex_unlock(&list_lock);
 
+	mutex_init(&ici->host_lock);
 	scan_add_host(ici);
 
 	return 0;
@@ -1386,7 +1393,7 @@ static const struct v4l2_ioctl_ops soc_camera_ioctl_ops = {
 	.vidioc_s_input		 = soc_camera_s_input,
 	.vidioc_s_std		 = soc_camera_s_std,
 	.vidioc_g_std		 = soc_camera_g_std,
-	.vidioc_enum_framesizes  = soc_camera_enum_fsizes,
+	.vidioc_enum_framesizes  = soc_camera_enum_framesizes,
 	.vidioc_reqbufs		 = soc_camera_reqbufs,
 	.vidioc_querybuf	 = soc_camera_querybuf,
 	.vidioc_qbuf		 = soc_camera_qbuf,
@@ -1425,6 +1432,10 @@ static int video_dev_create(struct soc_camera_device *icd)
 	vdev->tvnorms		= V4L2_STD_UNKNOWN;
 	vdev->ctrl_handler	= &icd->ctrl_handler;
 	vdev->lock		= &icd->video_lock;
+	/* Locking in file operations other than ioctl should be done
+	   by the driver, not the V4L2 core.
+	   This driver needs auditing so that this flag can be removed. */
+	set_bit(V4L2_FL_LOCK_ALL_FOPS, &vdev->flags);
 
 	icd->vdev = vdev;
 
diff --git a/drivers/media/video/soc_mediabus.c b/drivers/media/video/soc_mediabus.c
index cf7f2194ded..89dce097a82 100644
--- a/drivers/media/video/soc_mediabus.c
+++ b/drivers/media/video/soc_mediabus.c
@@ -24,6 +24,7 @@ static const struct soc_mbus_lookup mbus_fmt[] = {
 		.bits_per_sample	= 8,
 		.packing		= SOC_MBUS_PACKING_2X8_PADHI,
 		.order			= SOC_MBUS_ORDER_LE,
+		.layout			= SOC_MBUS_LAYOUT_PACKED,
 	},
 }, {
 	.code = V4L2_MBUS_FMT_YVYU8_2X8,
@@ -33,6 +34,7 @@ static const struct soc_mbus_lookup mbus_fmt[] = {
 		.bits_per_sample	= 8,
 		.packing		= SOC_MBUS_PACKING_2X8_PADHI,
 		.order			= SOC_MBUS_ORDER_LE,
+		.layout			= SOC_MBUS_LAYOUT_PACKED,
 	},
 }, {
 	.code = V4L2_MBUS_FMT_UYVY8_2X8,
@@ -42,6 +44,7 @@ static const struct soc_mbus_lookup mbus_fmt[] = {
 		.bits_per_sample	= 8,
 		.packing		= SOC_MBUS_PACKING_2X8_PADHI,
 		.order			= SOC_MBUS_ORDER_LE,
+		.layout			= SOC_MBUS_LAYOUT_PACKED,
 	},
 }, {
 	.code = V4L2_MBUS_FMT_VYUY8_2X8,
@@ -51,6 +54,7 @@ static const struct soc_mbus_lookup mbus_fmt[] = {
 		.bits_per_sample	= 8,
 		.packing		= SOC_MBUS_PACKING_2X8_PADHI,
 		.order			= SOC_MBUS_ORDER_LE,
+		.layout			= SOC_MBUS_LAYOUT_PACKED,
 	},
 }, {
 	.code = V4L2_MBUS_FMT_RGB555_2X8_PADHI_LE,
@@ -60,6 +64,7 @@ static const struct soc_mbus_lookup mbus_fmt[] = {
 		.bits_per_sample	= 8,
 		.packing		= SOC_MBUS_PACKING_2X8_PADHI,
 		.order			= SOC_MBUS_ORDER_LE,
+		.layout			= SOC_MBUS_LAYOUT_PACKED,
 	},
 }, {
 	.code = V4L2_MBUS_FMT_RGB555_2X8_PADHI_BE,
@@ -69,6 +74,7 @@ static const struct soc_mbus_lookup mbus_fmt[] = {
 		.bits_per_sample	= 8,
 		.packing		= SOC_MBUS_PACKING_2X8_PADHI,
 		.order			= SOC_MBUS_ORDER_LE,
+		.layout			= SOC_MBUS_LAYOUT_PACKED,
 	},
 }, {
 	.code = V4L2_MBUS_FMT_RGB565_2X8_LE,
@@ -78,6 +84,7 @@ static const struct soc_mbus_lookup mbus_fmt[] = {
 		.bits_per_sample	= 8,
 		.packing		= SOC_MBUS_PACKING_2X8_PADHI,
 		.order			= SOC_MBUS_ORDER_LE,
+		.layout			= SOC_MBUS_LAYOUT_PACKED,
 	},
 }, {
 	.code = V4L2_MBUS_FMT_RGB565_2X8_BE,
@@ -87,6 +94,7 @@ static const struct soc_mbus_lookup mbus_fmt[] = {
 		.bits_per_sample	= 8,
 		.packing		= SOC_MBUS_PACKING_2X8_PADHI,
 		.order			= SOC_MBUS_ORDER_LE,
+		.layout			= SOC_MBUS_LAYOUT_PACKED,
 	},
 }, {
 	.code = V4L2_MBUS_FMT_SBGGR8_1X8,
@@ -96,6 +104,7 @@ static const struct soc_mbus_lookup mbus_fmt[] = {
 		.bits_per_sample	= 8,
 		.packing		= SOC_MBUS_PACKING_NONE,
 		.order			= SOC_MBUS_ORDER_LE,
+		.layout			= SOC_MBUS_LAYOUT_PACKED,
 	},
 }, {
 	.code = V4L2_MBUS_FMT_SBGGR10_1X10,
@@ -105,6 +114,7 @@ static const struct soc_mbus_lookup mbus_fmt[] = {
 		.bits_per_sample	= 10,
 		.packing		= SOC_MBUS_PACKING_EXTEND16,
 		.order			= SOC_MBUS_ORDER_LE,
+		.layout			= SOC_MBUS_LAYOUT_PACKED,
 	},
 }, {
 	.code = V4L2_MBUS_FMT_Y8_1X8,
@@ -114,6 +124,7 @@ static const struct soc_mbus_lookup mbus_fmt[] = {
 		.bits_per_sample	= 8,
 		.packing		= SOC_MBUS_PACKING_NONE,
 		.order			= SOC_MBUS_ORDER_LE,
+		.layout			= SOC_MBUS_LAYOUT_PACKED,
 	},
 }, {
 	.code = V4L2_MBUS_FMT_Y10_1X10,
@@ -123,6 +134,7 @@ static const struct soc_mbus_lookup mbus_fmt[] = {
 		.bits_per_sample	= 10,
 		.packing		= SOC_MBUS_PACKING_EXTEND16,
 		.order			= SOC_MBUS_ORDER_LE,
+		.layout			= SOC_MBUS_LAYOUT_PACKED,
 	},
 }, {
 	.code = V4L2_MBUS_FMT_SBGGR10_2X8_PADHI_LE,
@@ -132,6 +144,7 @@ static const struct soc_mbus_lookup mbus_fmt[] = {
 		.bits_per_sample	= 8,
 		.packing		= SOC_MBUS_PACKING_2X8_PADHI,
 		.order			= SOC_MBUS_ORDER_LE,
+		.layout			= SOC_MBUS_LAYOUT_PACKED,
 	},
 }, {
 	.code = V4L2_MBUS_FMT_SBGGR10_2X8_PADLO_LE,
@@ -141,6 +154,7 @@ static const struct soc_mbus_lookup mbus_fmt[] = {
 		.bits_per_sample	= 8,
 		.packing		= SOC_MBUS_PACKING_2X8_PADLO,
 		.order			= SOC_MBUS_ORDER_LE,
+		.layout			= SOC_MBUS_LAYOUT_PACKED,
 	},
 }, {
 	.code = V4L2_MBUS_FMT_SBGGR10_2X8_PADHI_BE,
@@ -150,6 +164,7 @@ static const struct soc_mbus_lookup mbus_fmt[] = {
 		.bits_per_sample	= 8,
 		.packing		= SOC_MBUS_PACKING_2X8_PADHI,
 		.order			= SOC_MBUS_ORDER_BE,
+		.layout			= SOC_MBUS_LAYOUT_PACKED,
 	},
 }, {
 	.code = V4L2_MBUS_FMT_SBGGR10_2X8_PADLO_BE,
@@ -159,6 +174,7 @@ static const struct soc_mbus_lookup mbus_fmt[] = {
 		.bits_per_sample	= 8,
 		.packing		= SOC_MBUS_PACKING_2X8_PADLO,
 		.order			= SOC_MBUS_ORDER_BE,
+		.layout			= SOC_MBUS_LAYOUT_PACKED,
 	},
 }, {
 	.code = V4L2_MBUS_FMT_JPEG_1X8,
@@ -168,6 +184,7 @@ static const struct soc_mbus_lookup mbus_fmt[] = {
 		.bits_per_sample        = 8,
 		.packing                = SOC_MBUS_PACKING_VARIABLE,
 		.order                  = SOC_MBUS_ORDER_LE,
+		.layout			= SOC_MBUS_LAYOUT_PACKED,
 	},
 }, {
 	.code = V4L2_MBUS_FMT_RGB444_2X8_PADHI_BE,
@@ -177,6 +194,7 @@ static const struct soc_mbus_lookup mbus_fmt[] = {
 		.bits_per_sample	= 8,
 		.packing		= SOC_MBUS_PACKING_2X8_PADHI,
 		.order			= SOC_MBUS_ORDER_BE,
+		.layout			= SOC_MBUS_LAYOUT_PACKED,
 	},
 }, {
 	.code = V4L2_MBUS_FMT_YUYV8_1_5X8,
@@ -186,6 +204,7 @@ static const struct soc_mbus_lookup mbus_fmt[] = {
 		.bits_per_sample	= 8,
 		.packing		= SOC_MBUS_PACKING_1_5X8,
 		.order			= SOC_MBUS_ORDER_LE,
+		.layout			= SOC_MBUS_LAYOUT_PACKED,
 	},
 }, {
 	.code = V4L2_MBUS_FMT_YVYU8_1_5X8,
@@ -195,6 +214,7 @@ static const struct soc_mbus_lookup mbus_fmt[] = {
 		.bits_per_sample	= 8,
 		.packing		= SOC_MBUS_PACKING_1_5X8,
 		.order			= SOC_MBUS_ORDER_LE,
+		.layout			= SOC_MBUS_LAYOUT_PACKED,
 	},
 }, {
 	.code = V4L2_MBUS_FMT_UYVY8_1X16,
@@ -204,6 +224,7 @@ static const struct soc_mbus_lookup mbus_fmt[] = {
 		.bits_per_sample	= 16,
 		.packing		= SOC_MBUS_PACKING_EXTEND16,
 		.order			= SOC_MBUS_ORDER_LE,
+		.layout			= SOC_MBUS_LAYOUT_PACKED,
 	},
 }, {
 	.code = V4L2_MBUS_FMT_VYUY8_1X16,
@@ -213,6 +234,7 @@ static const struct soc_mbus_lookup mbus_fmt[] = {
 		.bits_per_sample	= 16,
 		.packing		= SOC_MBUS_PACKING_EXTEND16,
 		.order			= SOC_MBUS_ORDER_LE,
+		.layout			= SOC_MBUS_LAYOUT_PACKED,
 	},
 }, {
 	.code = V4L2_MBUS_FMT_YUYV8_1X16,
@@ -222,6 +244,7 @@ static const struct soc_mbus_lookup mbus_fmt[] = {
 		.bits_per_sample	= 16,
 		.packing		= SOC_MBUS_PACKING_EXTEND16,
 		.order			= SOC_MBUS_ORDER_LE,
+		.layout			= SOC_MBUS_LAYOUT_PACKED,
 	},
 }, {
 	.code = V4L2_MBUS_FMT_YVYU8_1X16,
@@ -231,6 +254,7 @@ static const struct soc_mbus_lookup mbus_fmt[] = {
 		.bits_per_sample	= 16,
 		.packing		= SOC_MBUS_PACKING_EXTEND16,
 		.order			= SOC_MBUS_ORDER_LE,
+		.layout			= SOC_MBUS_LAYOUT_PACKED,
 	},
 }, {
 	.code = V4L2_MBUS_FMT_SGRBG8_1X8,
@@ -240,6 +264,7 @@ static const struct soc_mbus_lookup mbus_fmt[] = {
 		.bits_per_sample	= 8,
 		.packing		= SOC_MBUS_PACKING_NONE,
 		.order			= SOC_MBUS_ORDER_LE,
+		.layout			= SOC_MBUS_LAYOUT_PACKED,
 	},
 }, {
 	.code = V4L2_MBUS_FMT_SGRBG10_DPCM8_1X8,
@@ -249,6 +274,7 @@ static const struct soc_mbus_lookup mbus_fmt[] = {
 		.bits_per_sample	= 8,
 		.packing		= SOC_MBUS_PACKING_NONE,
 		.order			= SOC_MBUS_ORDER_LE,
+		.layout			= SOC_MBUS_LAYOUT_PACKED,
 	},
 }, {
 	.code = V4L2_MBUS_FMT_SGBRG10_1X10,
@@ -258,6 +284,7 @@ static const struct soc_mbus_lookup mbus_fmt[] = {
 		.bits_per_sample	= 10,
 		.packing		= SOC_MBUS_PACKING_EXTEND16,
 		.order			= SOC_MBUS_ORDER_LE,
+		.layout			= SOC_MBUS_LAYOUT_PACKED,
 	},
 }, {
 	.code = V4L2_MBUS_FMT_SGRBG10_1X10,
@@ -267,6 +294,7 @@ static const struct soc_mbus_lookup mbus_fmt[] = {
 		.bits_per_sample	= 10,
 		.packing		= SOC_MBUS_PACKING_EXTEND16,
 		.order			= SOC_MBUS_ORDER_LE,
+		.layout			= SOC_MBUS_LAYOUT_PACKED,
 	},
 }, {
 	.code = V4L2_MBUS_FMT_SRGGB10_1X10,
@@ -276,6 +304,7 @@ static const struct soc_mbus_lookup mbus_fmt[] = {
 		.bits_per_sample	= 10,
 		.packing		= SOC_MBUS_PACKING_EXTEND16,
 		.order			= SOC_MBUS_ORDER_LE,
+		.layout			= SOC_MBUS_LAYOUT_PACKED,
 	},
 }, {
 	.code = V4L2_MBUS_FMT_SBGGR12_1X12,
@@ -285,6 +314,7 @@ static const struct soc_mbus_lookup mbus_fmt[] = {
 		.bits_per_sample	= 12,
 		.packing		= SOC_MBUS_PACKING_EXTEND16,
 		.order			= SOC_MBUS_ORDER_LE,
+		.layout			= SOC_MBUS_LAYOUT_PACKED,
 	},
 }, {
 	.code = V4L2_MBUS_FMT_SGBRG12_1X12,
@@ -294,6 +324,7 @@ static const struct soc_mbus_lookup mbus_fmt[] = {
 		.bits_per_sample	= 12,
 		.packing		= SOC_MBUS_PACKING_EXTEND16,
 		.order			= SOC_MBUS_ORDER_LE,
+		.layout			= SOC_MBUS_LAYOUT_PACKED,
 	},
 }, {
 	.code = V4L2_MBUS_FMT_SGRBG12_1X12,
@@ -303,6 +334,7 @@ static const struct soc_mbus_lookup mbus_fmt[] = {
 		.bits_per_sample	= 12,
 		.packing		= SOC_MBUS_PACKING_EXTEND16,
 		.order			= SOC_MBUS_ORDER_LE,
+		.layout			= SOC_MBUS_LAYOUT_PACKED,
 	},
 }, {
 	.code = V4L2_MBUS_FMT_SRGGB12_1X12,
@@ -312,6 +344,7 @@ static const struct soc_mbus_lookup mbus_fmt[] = {
 		.bits_per_sample	= 12,
 		.packing		= SOC_MBUS_PACKING_EXTEND16,
 		.order			= SOC_MBUS_ORDER_LE,
+		.layout			= SOC_MBUS_LAYOUT_PACKED,
 	},
 },
 };
@@ -345,6 +378,9 @@ EXPORT_SYMBOL(soc_mbus_samples_per_pixel);
 
 s32 soc_mbus_bytes_per_line(u32 width, const struct soc_mbus_pixelfmt *mf)
 {
+	if (mf->layout != SOC_MBUS_LAYOUT_PACKED)
+		return width * mf->bits_per_sample / 8;
+
 	switch (mf->packing) {
 	case SOC_MBUS_PACKING_NONE:
 		return width * mf->bits_per_sample / 8;
@@ -361,6 +397,24 @@ s32 soc_mbus_bytes_per_line(u32 width, const struct soc_mbus_pixelfmt *mf)
 }
 EXPORT_SYMBOL(soc_mbus_bytes_per_line);
 
+s32 soc_mbus_image_size(const struct soc_mbus_pixelfmt *mf,
+			u32 bytes_per_line, u32 height)
+{
+	if (mf->layout == SOC_MBUS_LAYOUT_PACKED)
+		return bytes_per_line * height;
+
+	switch (mf->packing) {
+	case SOC_MBUS_PACKING_2X8_PADHI:
+	case SOC_MBUS_PACKING_2X8_PADLO:
+		return bytes_per_line * height * 2;
+	case SOC_MBUS_PACKING_1_5X8:
+		return bytes_per_line * height * 3 / 2;
+	default:
+		return -EINVAL;
+	}
+}
+EXPORT_SYMBOL(soc_mbus_image_size);
+
 const struct soc_mbus_pixelfmt *soc_mbus_find_fmtdesc(
 	enum v4l2_mbus_pixelcode code,
 	const struct soc_mbus_lookup *lookup,
diff --git a/drivers/media/video/sta2x11_vip.c b/drivers/media/video/sta2x11_vip.c
new file mode 100644
index 00000000000..4c10205264d
--- /dev/null
+++ b/drivers/media/video/sta2x11_vip.c
@@ -0,0 +1,1550 @@
+/*
+ * This is the driver for the STA2x11 Video Input Port.
+ *
+ * Copyright (C) 2010       WindRiver Systems, 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,
+ * 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: Andreas Kies <andreas.kies@windriver.com>
+ *		Vlad Lungu <vlad.lungu@windriver.com>
+ *
+ */
+
+#include <linux/types.h>
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/init.h>
+#include <linux/vmalloc.h>
+
+#include <linux/videodev2.h>
+
+#include <linux/kmod.h>
+
+#include <linux/pci.h>
+#include <linux/interrupt.h>
+#include <linux/mutex.h>
+#include <linux/io.h>
+#include <linux/gpio.h>
+#include <linux/i2c.h>
+#include <linux/delay.h>
+#include <media/v4l2-common.h>
+#include <media/v4l2-device.h>
+#include <media/v4l2-ioctl.h>
+#include <media/videobuf-dma-contig.h>
+
+#include "sta2x11_vip.h"
+
+#define DRV_NAME "sta2x11_vip"
+#define DRV_VERSION "1.3"
+
+#ifndef PCI_DEVICE_ID_STMICRO_VIP
+#define PCI_DEVICE_ID_STMICRO_VIP 0xCC0D
+#endif
+
+#define MAX_FRAMES 4
+
+/*Register offsets*/
+#define DVP_CTL		0x00
+#define DVP_TFO		0x04
+#define DVP_TFS		0x08
+#define DVP_BFO		0x0C
+#define DVP_BFS		0x10
+#define DVP_VTP         0x14
+#define DVP_VBP         0x18
+#define DVP_VMP		0x1C
+#define DVP_ITM		0x98
+#define DVP_ITS		0x9C
+#define DVP_STA		0xA0
+#define DVP_HLFLN	0xA8
+#define DVP_RGB		0xC0
+#define DVP_PKZ		0xF0
+
+/*Register fields*/
+#define DVP_CTL_ENA	0x00000001
+#define DVP_CTL_RST	0x80000000
+#define DVP_CTL_DIS	(~0x00040001)
+
+#define DVP_IT_VSB	0x00000008
+#define DVP_IT_VST	0x00000010
+#define DVP_IT_FIFO	0x00000020
+
+#define DVP_HLFLN_SD	0x00000001
+
+#define REG_WRITE(vip, reg, value) iowrite32((value), (vip->iomem)+(reg))
+#define REG_READ(vip, reg) ioread32((vip->iomem)+(reg))
+
+#define SAVE_COUNT 8
+#define AUX_COUNT 3
+#define IRQ_COUNT 1
+
+/**
+ * struct sta2x11_vip - All internal data for one instance of device
+ * @v4l2_dev: device registered in v4l layer
+ * @video_dev: properties of our device
+ * @pdev: PCI device
+ * @adapter: contains I2C adapter information
+ * @register_save_area: All relevant register are saved here during suspend
+ * @decoder: contains information about video DAC
+ * @format: pixel format, fixed UYVY
+ * @std: video standard (e.g. PAL/NTSC)
+ * @input: input line for video signal ( 0 or 1 )
+ * @users: Number of open of device ( max. 1 )
+ * @disabled: Device is in power down state
+ * @mutex: ensures exclusive opening of device
+ * @slock: for excluse acces of registers
+ * @vb_vidq: queue maintained by videobuf layer
+ * @capture: linked list of capture buffer
+ * @active: struct videobuf_buffer currently beingg filled
+ * @started: device is ready to capture frame
+ * @closing: device will be shut down
+ * @tcount: Number of top frames
+ * @bcount: Number of bottom frames
+ * @overflow: Number of FIFO overflows
+ * @mem_spare: small buffer of unused frame
+ * @dma_spare: dma addres of mem_spare
+ * @iomem: hardware base address
+ * @config: I2C and gpio config from platform
+ *
+ * All non-local data is accessed via this structure.
+ */
+
+struct sta2x11_vip {
+	struct v4l2_device v4l2_dev;
+	struct video_device *video_dev;
+	struct pci_dev *pdev;
+	struct i2c_adapter *adapter;
+	unsigned int register_save_area[IRQ_COUNT + SAVE_COUNT + AUX_COUNT];
+	struct v4l2_subdev *decoder;
+	struct v4l2_pix_format format;
+	v4l2_std_id std;
+	unsigned int input;
+	int users;
+	int disabled;
+	struct mutex mutex;	/* exclusive access during open */
+	spinlock_t slock;	/* spin lock for hardware and queue access */
+	struct videobuf_queue vb_vidq;
+	struct list_head capture;
+	struct videobuf_buffer *active;
+	int started, closing, tcount, bcount;
+	int overflow;
+	void *mem_spare;
+	dma_addr_t dma_spare;
+	void *iomem;
+	struct vip_config *config;
+};
+
+static const unsigned int registers_to_save[AUX_COUNT] = {
+	DVP_HLFLN, DVP_RGB, DVP_PKZ
+};
+
+static struct v4l2_pix_format formats_50[] = {
+	{			/*PAL interlaced */
+	 .width = 720,
+	 .height = 576,
+	 .pixelformat = V4L2_PIX_FMT_UYVY,
+	 .field = V4L2_FIELD_INTERLACED,
+	 .bytesperline = 720 * 2,
+	 .sizeimage = 720 * 2 * 576,
+	 .colorspace = V4L2_COLORSPACE_SMPTE170M},
+	{			/*PAL top */
+	 .width = 720,
+	 .height = 288,
+	 .pixelformat = V4L2_PIX_FMT_UYVY,
+	 .field = V4L2_FIELD_TOP,
+	 .bytesperline = 720 * 2,
+	 .sizeimage = 720 * 2 * 288,
+	 .colorspace = V4L2_COLORSPACE_SMPTE170M},
+	{			/*PAL bottom */
+	 .width = 720,
+	 .height = 288,
+	 .pixelformat = V4L2_PIX_FMT_UYVY,
+	 .field = V4L2_FIELD_BOTTOM,
+	 .bytesperline = 720 * 2,
+	 .sizeimage = 720 * 2 * 288,
+	 .colorspace = V4L2_COLORSPACE_SMPTE170M},
+
+};
+
+static struct v4l2_pix_format formats_60[] = {
+	{			/*NTSC interlaced */
+	 .width = 720,
+	 .height = 480,
+	 .pixelformat = V4L2_PIX_FMT_UYVY,
+	 .field = V4L2_FIELD_INTERLACED,
+	 .bytesperline = 720 * 2,
+	 .sizeimage = 720 * 2 * 480,
+	 .colorspace = V4L2_COLORSPACE_SMPTE170M},
+	{			/*NTSC top */
+	 .width = 720,
+	 .height = 240,
+	 .pixelformat = V4L2_PIX_FMT_UYVY,
+	 .field = V4L2_FIELD_TOP,
+	 .bytesperline = 720 * 2,
+	 .sizeimage = 720 * 2 * 240,
+	 .colorspace = V4L2_COLORSPACE_SMPTE170M},
+	{			/*NTSC bottom */
+	 .width = 720,
+	 .height = 240,
+	 .pixelformat = V4L2_PIX_FMT_UYVY,
+	 .field = V4L2_FIELD_BOTTOM,
+	 .bytesperline = 720 * 2,
+	 .sizeimage = 720 * 2 * 240,
+	 .colorspace = V4L2_COLORSPACE_SMPTE170M},
+};
+
+/**
+ * buf_setup - Get size and number of video buffer
+ * @vq: queue in videobuf
+ * @count: Number of buffers (1..MAX_FRAMES).
+ *		0 use default value.
+ * @size:  size of buffer in bytes
+ *
+ * returns size and number of buffers
+ * a preset value of 0 returns the default number.
+ * return value: 0, always succesfull.
+ */
+static int buf_setup(struct videobuf_queue *vq, unsigned int *count,
+		     unsigned int *size)
+{
+	struct sta2x11_vip *vip = vq->priv_data;
+
+	*size = vip->format.width * vip->format.height * 2;
+	if (0 == *count || MAX_FRAMES < *count)
+		*count = MAX_FRAMES;
+	return 0;
+};
+
+/**
+ * buf_prepare - prepare buffer for usage
+ * @vq: queue in videobuf layer
+ * @vb: buffer to be prepared
+ * @field: type of video data (interlaced/non-interlaced)
+ *
+ * Allocate or realloc buffer
+ * return value: 0, successful.
+ *
+ * -EINVAL, supplied buffer is too small.
+ *
+ *  other, buffer could not be locked.
+ */
+static int buf_prepare(struct videobuf_queue *vq, struct videobuf_buffer *vb,
+		       enum v4l2_field field)
+{
+	struct sta2x11_vip *vip = vq->priv_data;
+	int ret;
+
+	vb->size = vip->format.width * vip->format.height * 2;
+	if ((0 != vb->baddr) && (vb->bsize < vb->size))
+		return -EINVAL;
+	vb->width = vip->format.width;
+	vb->height = vip->format.height;
+	vb->field = field;
+
+	if (VIDEOBUF_NEEDS_INIT == vb->state) {
+		ret = videobuf_iolock(vq, vb, NULL);
+		if (ret)
+			goto fail;
+	}
+	vb->state = VIDEOBUF_PREPARED;
+	return 0;
+fail:
+	videobuf_dma_contig_free(vq, vb);
+	vb->state = VIDEOBUF_NEEDS_INIT;
+	return ret;
+}
+
+/**
+ * buf_queu - queue buffer for filling
+ * @vq: queue in videobuf layer
+ * @vb: buffer to be queued
+ *
+ * if capturing is already running, the buffer will be queued. Otherwise
+ * capture is started and the buffer is used directly.
+ */
+static void buf_queue(struct videobuf_queue *vq, struct videobuf_buffer *vb)
+{
+	struct sta2x11_vip *vip = vq->priv_data;
+	u32 dma;
+
+	vb->state = VIDEOBUF_QUEUED;
+
+	if (vip->active) {
+		list_add_tail(&vb->queue, &vip->capture);
+		return;
+	}
+
+	vip->started = 1;
+	vip->tcount = 0;
+	vip->bcount = 0;
+	vip->active = vb;
+	vb->state = VIDEOBUF_ACTIVE;
+
+	dma = videobuf_to_dma_contig(vb);
+
+	REG_WRITE(vip, DVP_TFO, (0 << 16) | (0));
+	/* despite of interlace mode, upper and lower frames start at zero */
+	REG_WRITE(vip, DVP_BFO, (0 << 16) | (0));
+
+	switch (vip->format.field) {
+	case V4L2_FIELD_INTERLACED:
+		REG_WRITE(vip, DVP_TFS,
+			  ((vip->format.height / 2 - 1) << 16) |
+			  (2 * vip->format.width - 1));
+		REG_WRITE(vip, DVP_BFS, ((vip->format.height / 2 - 1) << 16) |
+			  (2 * vip->format.width - 1));
+		REG_WRITE(vip, DVP_VTP, dma);
+		REG_WRITE(vip, DVP_VBP, dma + vip->format.width * 2);
+		REG_WRITE(vip, DVP_VMP, 4 * vip->format.width);
+		break;
+	case V4L2_FIELD_TOP:
+		REG_WRITE(vip, DVP_TFS,
+			  ((vip->format.height - 1) << 16) |
+			  (2 * vip->format.width - 1));
+		REG_WRITE(vip, DVP_BFS, ((0) << 16) |
+			  (2 * vip->format.width - 1));
+		REG_WRITE(vip, DVP_VTP, dma);
+		REG_WRITE(vip, DVP_VBP, dma);
+		REG_WRITE(vip, DVP_VMP, 2 * vip->format.width);
+		break;
+	case V4L2_FIELD_BOTTOM:
+		REG_WRITE(vip, DVP_TFS, ((0) << 16) |
+			  (2 * vip->format.width - 1));
+		REG_WRITE(vip, DVP_BFS,
+			  ((vip->format.height) << 16) |
+			  (2 * vip->format.width - 1));
+		REG_WRITE(vip, DVP_VTP, dma);
+		REG_WRITE(vip, DVP_VBP, dma);
+		REG_WRITE(vip, DVP_VMP, 2 * vip->format.width);
+		break;
+
+	default:
+		pr_warning("VIP: unknown field format\n");
+		return;
+	}
+
+	REG_WRITE(vip, DVP_CTL, DVP_CTL_ENA);
+}
+
+/**
+ * buff_release - release buffer
+ * @vq: queue in videobuf layer
+ * @vb: buffer to be released
+ *
+ * release buffer in videobuf layer
+ */
+static void buf_release(struct videobuf_queue *vq, struct videobuf_buffer *vb)
+{
+
+	videobuf_dma_contig_free(vq, vb);
+	vb->state = VIDEOBUF_NEEDS_INIT;
+}
+
+static struct videobuf_queue_ops vip_qops = {
+	.buf_setup = buf_setup,
+	.buf_prepare = buf_prepare,
+	.buf_queue = buf_queue,
+	.buf_release = buf_release,
+};
+
+/**
+ * vip_open - open video device
+ * @file: descriptor of device
+ *
+ * open device, make sure it is only opened once.
+ * return value: 0, no error.
+ *
+ * -EBUSY, device is already opened
+ *
+ * -ENOMEM, no memory for auxiliary DMA buffer
+ */
+static int vip_open(struct file *file)
+{
+	struct video_device *dev = video_devdata(file);
+	struct sta2x11_vip *vip = video_get_drvdata(dev);
+
+	mutex_lock(&vip->mutex);
+	vip->users++;
+
+	if (vip->users > 1) {
+		vip->users--;
+		mutex_unlock(&vip->mutex);
+		return -EBUSY;
+	}
+
+	file->private_data = dev;
+	vip->overflow = 0;
+	vip->started = 0;
+	vip->closing = 0;
+	vip->active = NULL;
+
+	INIT_LIST_HEAD(&vip->capture);
+	vip->mem_spare = dma_alloc_coherent(&vip->pdev->dev, 64,
+					    &vip->dma_spare, GFP_KERNEL);
+	if (!vip->mem_spare) {
+		vip->users--;
+		mutex_unlock(&vip->mutex);
+		return -ENOMEM;
+	}
+
+	mutex_unlock(&vip->mutex);
+	videobuf_queue_dma_contig_init_cached(&vip->vb_vidq,
+					      &vip_qops,
+					      &vip->pdev->dev,
+					      &vip->slock,
+					      V4L2_BUF_TYPE_VIDEO_CAPTURE,
+					      V4L2_FIELD_INTERLACED,
+					      sizeof(struct videobuf_buffer),
+					      vip, NULL);
+	REG_READ(vip, DVP_ITS);
+	REG_WRITE(vip, DVP_HLFLN, DVP_HLFLN_SD);
+	REG_WRITE(vip, DVP_ITM, DVP_IT_VSB | DVP_IT_VST);
+	REG_WRITE(vip, DVP_CTL, DVP_CTL_RST);
+	REG_WRITE(vip, DVP_CTL, 0);
+	REG_READ(vip, DVP_ITS);
+	return 0;
+}
+
+/**
+ * vip_close - close video device
+ * @file: descriptor of device
+ *
+ * close video device, wait until all pending operations are finished
+ * ( maximum FRAME_MAX buffers pending )
+ * Turn off interrupts.
+ *
+ * return value: 0, always succesful.
+ */
+static int vip_close(struct file *file)
+{
+	struct video_device *dev = video_devdata(file);
+	struct sta2x11_vip *vip = video_get_drvdata(dev);
+
+	vip->closing = 1;
+	if (vip->active)
+		videobuf_waiton(&vip->vb_vidq, vip->active, 0, 0);
+	spin_lock_irq(&vip->slock);
+
+	REG_WRITE(vip, DVP_ITM, 0);
+	REG_WRITE(vip, DVP_CTL, DVP_CTL_RST);
+	REG_WRITE(vip, DVP_CTL, 0);
+	REG_READ(vip, DVP_ITS);
+
+	vip->started = 0;
+	vip->active = NULL;
+
+	spin_unlock_irq(&vip->slock);
+
+	videobuf_stop(&vip->vb_vidq);
+	videobuf_mmap_free(&vip->vb_vidq);
+
+	dma_free_coherent(&vip->pdev->dev, 64, vip->mem_spare, vip->dma_spare);
+	file->private_data = NULL;
+	mutex_lock(&vip->mutex);
+	vip->users--;
+	mutex_unlock(&vip->mutex);
+	return 0;
+}
+
+/**
+ * vip_read - read from video input
+ * @file: descriptor of device
+ * @data: user buffer
+ * @count: number of bytes to be read
+ * @ppos: position within stream
+ *
+ * read video data from video device.
+ * handling is done in generic videobuf layer
+ * return value: provided by videobuf layer
+ */
+static ssize_t vip_read(struct file *file, char __user *data,
+			size_t count, loff_t *ppos)
+{
+	struct video_device *dev = file->private_data;
+	struct sta2x11_vip *vip = video_get_drvdata(dev);
+
+	return videobuf_read_stream(&vip->vb_vidq, data, count, ppos, 0,
+				    file->f_flags & O_NONBLOCK);
+}
+
+/**
+ * vip_mmap - map user buffer
+ * @file: descriptor of device
+ * @vma: user buffer
+ *
+ * map user space buffer into kernel mode, including DMA address.
+ * handling is done in generic videobuf layer.
+ * return value: provided by videobuf layer
+ */
+static int vip_mmap(struct file *file, struct vm_area_struct *vma)
+{
+	struct video_device *dev = file->private_data;
+	struct sta2x11_vip *vip = video_get_drvdata(dev);
+
+	return videobuf_mmap_mapper(&vip->vb_vidq, vma);
+}
+
+/**
+ * vip_poll - poll for event
+ * @file: descriptor of device
+ * @wait: contains events to be waited for
+ *
+ * wait for event related to video device.
+ * handling is done in generic videobuf layer.
+ * return value: provided by videobuf layer
+ */
+static unsigned int vip_poll(struct file *file, struct poll_table_struct *wait)
+{
+	struct video_device *dev = file->private_data;
+	struct sta2x11_vip *vip = video_get_drvdata(dev);
+
+	return videobuf_poll_stream(file, &vip->vb_vidq, wait);
+}
+
+/**
+ * vidioc_querycap - return capabilities of device
+ * @file: descriptor of device (not used)
+ * @priv: points to current videodevice
+ * @cap: contains return values
+ *
+ * the capabilities of the device are returned
+ *
+ * return value: 0, no error.
+ */
+static int vidioc_querycap(struct file *file, void *priv,
+			   struct v4l2_capability *cap)
+{
+	struct video_device *dev = priv;
+	struct sta2x11_vip *vip = video_get_drvdata(dev);
+
+	memset(cap, 0, sizeof(struct v4l2_capability));
+	strcpy(cap->driver, DRV_NAME);
+	strcpy(cap->card, DRV_NAME);
+	cap->version = 0;
+	snprintf(cap->bus_info, sizeof(cap->bus_info), "PCI:%s",
+		 pci_name(vip->pdev));
+	cap->capabilities = V4L2_CAP_VIDEO_CAPTURE | V4L2_CAP_READWRITE |
+	    V4L2_CAP_STREAMING;
+
+	return 0;
+}
+
+/**
+ * vidioc_s_std - set video standard
+ * @file: descriptor of device (not used)
+ * @priv: points to current videodevice
+ * @std: contains standard to be set
+ *
+ * the video standard is set
+ *
+ * return value: 0, no error.
+ *
+ * -EIO, no input signal detected
+ *
+ * other, returned from video DAC.
+ */
+static int vidioc_s_std(struct file *file, void *priv, v4l2_std_id *std)
+{
+	struct video_device *dev = priv;
+	struct sta2x11_vip *vip = video_get_drvdata(dev);
+	v4l2_std_id oldstd = vip->std, newstd;
+	int status;
+
+	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))
+				vip->format = formats_60[0];
+			else
+				vip->format = formats_50[0];
+		}
+		return 0;
+	}
+
+	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);
+}
+
+/**
+ * vidioc_g_std - get video standard
+ * @file: descriptor of device (not used)
+ * @priv: points to current videodevice
+ * @std: contains return values
+ *
+ * the current video standard is returned
+ *
+ * return value: 0, no error.
+ */
+static int vidioc_g_std(struct file *file, void *priv, v4l2_std_id *std)
+{
+	struct video_device *dev = priv;
+	struct sta2x11_vip *vip = video_get_drvdata(dev);
+
+	*std = vip->std;
+	return 0;
+}
+
+/**
+ * vidioc_querystd - get possible video standards
+ * @file: descriptor of device (not used)
+ * @priv: points to current videodevice
+ * @std: contains return values
+ *
+ * all possible video standards are returned
+ *
+ * return value: delivered by video DAC routine.
+ */
+static int vidioc_querystd(struct file *file, void *priv, v4l2_std_id *std)
+{
+	struct video_device *dev = priv;
+	struct sta2x11_vip *vip = video_get_drvdata(dev);
+
+	return v4l2_subdev_call(vip->decoder, video, querystd, std);
+
+}
+
+/**
+ * vidioc_queryctl - get possible control settings
+ * @file: descriptor of device (not used)
+ * @priv: points to current videodevice
+ * @ctrl: contains return values
+ *
+ * return possible values for a control
+ * return value: delivered by video DAC routine.
+ */
+static int vidioc_queryctrl(struct file *file, void *priv,
+			    struct v4l2_queryctrl *ctrl)
+{
+	struct video_device *dev = priv;
+	struct sta2x11_vip *vip = video_get_drvdata(dev);
+
+	return v4l2_subdev_call(vip->decoder, core, queryctrl, ctrl);
+}
+
+/**
+ * vidioc_g_ctl - get control value
+ * @file: descriptor of device (not used)
+ * @priv: points to current videodevice
+ * @ctrl: contains return values
+ *
+ * return setting for a control value
+ * return value: delivered by video DAC routine.
+ */
+static int vidioc_g_ctrl(struct file *file, void *priv,
+			 struct v4l2_control *ctrl)
+{
+	struct video_device *dev = priv;
+	struct sta2x11_vip *vip = video_get_drvdata(dev);
+
+	return v4l2_subdev_call(vip->decoder, core, g_ctrl, ctrl);
+}
+
+/**
+ * vidioc_s_ctl - set control value
+ * @file: descriptor of device (not used)
+ * @priv: points to current videodevice
+ * @ctrl: contains value to be set
+ *
+ * set value for a specific control
+ * return value: delivered by video DAC routine.
+ */
+static int vidioc_s_ctrl(struct file *file, void *priv,
+			 struct v4l2_control *ctrl)
+{
+	struct video_device *dev = priv;
+	struct sta2x11_vip *vip = video_get_drvdata(dev);
+
+	return v4l2_subdev_call(vip->decoder, core, s_ctrl, ctrl);
+}
+
+/**
+ * vidioc_enum_input - return name of input line
+ * @file: descriptor of device (not used)
+ * @priv: points to current videodevice
+ * @inp: contains return values
+ *
+ * the user friendly name of the input line is returned
+ *
+ * return value: 0, no error.
+ *
+ * -EINVAL, input line number out of range
+ */
+static int vidioc_enum_input(struct file *file, void *priv,
+			     struct v4l2_input *inp)
+{
+	if (inp->index > 1)
+		return -EINVAL;
+
+	inp->type = V4L2_INPUT_TYPE_CAMERA;
+	inp->std = V4L2_STD_ALL;
+	sprintf(inp->name, "Camera %u", inp->index);
+
+	return 0;
+}
+
+/**
+ * vidioc_s_input - set input line
+ * @file: descriptor of device ( not used)
+ * @priv: points to current videodevice
+ * @i: new input line number
+ *
+ * the current active input line is set
+ *
+ * return value: 0, no error.
+ *
+ * -EINVAL, line number out of range
+ */
+static int vidioc_s_input(struct file *file, void *priv, unsigned int i)
+{
+	struct video_device *dev = priv;
+	struct sta2x11_vip *vip = video_get_drvdata(dev);
+	int ret;
+
+	if (i > 1)
+		return -EINVAL;
+	ret = v4l2_subdev_call(vip->decoder, video, s_routing, i, 0, 0);
+
+	if (!ret)
+		vip->input = i;
+
+	return 0;
+}
+
+/**
+ * vidioc_g_input - return input line
+ * @file: descriptor of device ( not used)
+ * @priv: points to current videodevice
+ * @i: returned input line number
+ *
+ * the current active input line is returned
+ *
+ * return value: always 0.
+ */
+static int vidioc_g_input(struct file *file, void *priv, unsigned int *i)
+{
+	struct video_device *dev = priv;
+	struct sta2x11_vip *vip = video_get_drvdata(dev);
+
+	*i = vip->input;
+	return 0;
+}
+
+/**
+ * vidioc_enum_fmt_vid_cap - return video capture format
+ * @file: descriptor of device ( not used)
+ * @priv: points to current videodevice
+ * @f: returned format information
+ *
+ * returns name and format of video capture
+ * Only UYVY is supported by hardware.
+ *
+ * return value: always 0.
+ */
+static int vidioc_enum_fmt_vid_cap(struct file *file, void *priv,
+				   struct v4l2_fmtdesc *f)
+{
+
+	if (f->index != 0)
+		return -EINVAL;
+
+	strcpy(f->description, "4:2:2, packed, UYVY");
+	f->pixelformat = V4L2_PIX_FMT_UYVY;
+	f->flags = 0;
+	return 0;
+}
+
+/**
+ * vidioc_try_fmt_vid_cap - set video capture format
+ * @file: descriptor of device ( not used)
+ * @priv: points to current videodevice
+ * @f: new format
+ *
+ * new video format is set which includes width and
+ * field type. width is fixed to 720, no scaling.
+ * Only UYVY is supported by this hardware.
+ * the minimum height is 200, the maximum is 576 (PAL)
+ *
+ * return value: 0, no error
+ *
+ * -EINVAL, pixel or field format not supported
+ *
+ */
+static int vidioc_try_fmt_vid_cap(struct file *file, void *priv,
+				  struct v4l2_format *f)
+{
+	struct video_device *dev = priv;
+	struct sta2x11_vip *vip = video_get_drvdata(dev);
+	int interlace_lim;
+
+	if (V4L2_PIX_FMT_UYVY != f->fmt.pix.pixelformat)
+		return -EINVAL;
+
+	if (V4L2_STD_525_60 & vip->std)
+		interlace_lim = 240;
+	else
+		interlace_lim = 288;
+
+	switch (f->fmt.pix.field) {
+	case V4L2_FIELD_ANY:
+		if (interlace_lim < f->fmt.pix.height)
+			f->fmt.pix.field = V4L2_FIELD_INTERLACED;
+		else
+			f->fmt.pix.field = V4L2_FIELD_BOTTOM;
+		break;
+	case V4L2_FIELD_TOP:
+	case V4L2_FIELD_BOTTOM:
+		if (interlace_lim < f->fmt.pix.height)
+			f->fmt.pix.height = interlace_lim;
+		break;
+	case V4L2_FIELD_INTERLACED:
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	f->fmt.pix.height &= ~1;
+	if (2 * interlace_lim < f->fmt.pix.height)
+		f->fmt.pix.height = 2 * interlace_lim;
+	if (200 > f->fmt.pix.height)
+		f->fmt.pix.height = 200;
+	f->fmt.pix.width = 720;
+	f->fmt.pix.bytesperline = f->fmt.pix.width * 2;
+	f->fmt.pix.sizeimage = f->fmt.pix.width * 2 * f->fmt.pix.height;
+	f->fmt.pix.colorspace = V4L2_COLORSPACE_SMPTE170M;
+	f->fmt.pix.priv = 0;
+	return 0;
+}
+
+/**
+ * vidioc_s_fmt_vid_cap - set current video format parameters
+ * @file: descriptor of device ( not used)
+ * @priv: points to current videodevice
+ * @f: returned format information
+ *
+ * set new capture format
+ * return value: 0, no error
+ *
+ * other, delivered by video DAC routine.
+ */
+static int vidioc_s_fmt_vid_cap(struct file *file, void *priv,
+				struct v4l2_format *f)
+{
+	struct video_device *dev = priv;
+	struct sta2x11_vip *vip = video_get_drvdata(dev);
+	int ret;
+
+	ret = vidioc_try_fmt_vid_cap(file, priv, f);
+	if (ret)
+		return ret;
+
+	memcpy(&vip->format, &f->fmt.pix, sizeof(struct v4l2_pix_format));
+	return 0;
+}
+
+/**
+ * vidioc_g_fmt_vid_cap - get current video format parameters
+ * @file: descriptor of device ( not used)
+ * @priv: points to current videodevice
+ * @f: contains format information
+ *
+ * returns current video format parameters
+ *
+ * return value: 0, always successful
+ */
+static int vidioc_g_fmt_vid_cap(struct file *file, void *priv,
+				struct v4l2_format *f)
+{
+	struct video_device *dev = priv;
+	struct sta2x11_vip *vip = video_get_drvdata(dev);
+
+	memcpy(&f->fmt.pix, &vip->format, sizeof(struct v4l2_pix_format));
+	return 0;
+}
+
+/**
+ * vidioc_reqfs - request buffer
+ * @file: descriptor of device ( not used)
+ * @priv: points to current videodevice
+ * @p: video buffer
+ *
+ * Handling is done in generic videobuf layer.
+ */
+static int vidioc_reqbufs(struct file *file, void *priv,
+			  struct v4l2_requestbuffers *p)
+{
+	struct video_device *dev = priv;
+	struct sta2x11_vip *vip = video_get_drvdata(dev);
+
+	return videobuf_reqbufs(&vip->vb_vidq, p);
+}
+
+/**
+ * vidioc_querybuf - query buffer
+ * @file: descriptor of device ( not used)
+ * @priv: points to current videodevice
+ * @p: video buffer
+ *
+ * query buffer state.
+ * Handling is done in generic videobuf layer.
+ */
+static int vidioc_querybuf(struct file *file, void *priv, struct v4l2_buffer *p)
+{
+	struct video_device *dev = priv;
+	struct sta2x11_vip *vip = video_get_drvdata(dev);
+
+	return videobuf_querybuf(&vip->vb_vidq, p);
+}
+
+/**
+ * vidioc_qbuf - queue a buffer
+ * @file: descriptor of device ( not used)
+ * @priv: points to current videodevice
+ * @p: video buffer
+ *
+ * Handling is done in generic videobuf layer.
+ */
+static int vidioc_qbuf(struct file *file, void *priv, struct v4l2_buffer *p)
+{
+	struct video_device *dev = priv;
+	struct sta2x11_vip *vip = video_get_drvdata(dev);
+
+	return videobuf_qbuf(&vip->vb_vidq, p);
+}
+
+/**
+ * vidioc_dqbuf - dequeue a buffer
+ * @file: descriptor of device ( not used)
+ * @priv: points to current videodevice
+ * @p: video buffer
+ *
+ * Handling is done in generic videobuf layer.
+ */
+static int vidioc_dqbuf(struct file *file, void *priv, struct v4l2_buffer *p)
+{
+	struct video_device *dev = priv;
+	struct sta2x11_vip *vip = video_get_drvdata(dev);
+
+	return videobuf_dqbuf(&vip->vb_vidq, p, file->f_flags & O_NONBLOCK);
+}
+
+/**
+ * vidioc_streamon - turn on streaming
+ * @file: descriptor of device ( not used)
+ * @priv: points to current videodevice
+ * @type: type of capture
+ *
+ * turn on streaming.
+ * Handling is done in generic videobuf layer.
+ */
+static int vidioc_streamon(struct file *file, void *priv,
+			   enum v4l2_buf_type type)
+{
+	struct video_device *dev = priv;
+	struct sta2x11_vip *vip = video_get_drvdata(dev);
+
+	return videobuf_streamon(&vip->vb_vidq);
+}
+
+/**
+ * vidioc_streamoff - turn off streaming
+ * @file: descriptor of device ( not used)
+ * @priv: points to current videodevice
+ * @type: type of capture
+ *
+ * turn off streaming.
+ * Handling is done in generic videobuf layer.
+ */
+static int vidioc_streamoff(struct file *file, void *priv,
+			    enum v4l2_buf_type type)
+{
+	struct video_device *dev = priv;
+	struct sta2x11_vip *vip = video_get_drvdata(dev);
+
+	return videobuf_streamoff(&vip->vb_vidq);
+}
+
+static const struct v4l2_file_operations vip_fops = {
+	.owner = THIS_MODULE,
+	.open = vip_open,
+	.release = vip_close,
+	.ioctl = video_ioctl2,
+	.read = vip_read,
+	.mmap = vip_mmap,
+	.poll = vip_poll
+};
+
+static const struct v4l2_ioctl_ops vip_ioctl_ops = {
+	.vidioc_querycap = vidioc_querycap,
+	.vidioc_s_std = vidioc_s_std,
+	.vidioc_g_std = vidioc_g_std,
+	.vidioc_querystd = vidioc_querystd,
+	.vidioc_queryctrl = vidioc_queryctrl,
+	.vidioc_g_ctrl = vidioc_g_ctrl,
+	.vidioc_s_ctrl = vidioc_s_ctrl,
+	.vidioc_enum_input = vidioc_enum_input,
+	.vidioc_try_fmt_vid_cap = vidioc_try_fmt_vid_cap,
+	.vidioc_s_input = vidioc_s_input,
+	.vidioc_g_input = vidioc_g_input,
+	.vidioc_enum_fmt_vid_cap = vidioc_enum_fmt_vid_cap,
+	.vidioc_s_fmt_vid_cap = vidioc_s_fmt_vid_cap,
+	.vidioc_g_fmt_vid_cap = vidioc_g_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,
+};
+
+static struct video_device video_dev_template = {
+	.name = DRV_NAME,
+	.release = video_device_release,
+	.fops = &vip_fops,
+	.ioctl_ops = &vip_ioctl_ops,
+	.tvnorms = V4L2_STD_ALL,
+};
+
+/**
+ * vip_irq - interrupt routine
+ * @irq: Number of interrupt ( not used, correct number is assumed )
+ * @vip: local data structure containing all information
+ *
+ * check for both frame interrupts set ( top and bottom ).
+ * check FIFO overflow, but limit number of log messages after open.
+ * signal a complete buffer if done.
+ * dequeue a new buffer if available.
+ * disable VIP if no buffer available.
+ *
+ * return value: IRQ_NONE, interrupt was not generated by VIP
+ *
+ * IRQ_HANDLED, interrupt done.
+ */
+static irqreturn_t vip_irq(int irq, struct sta2x11_vip *vip)
+{
+	u32 status, dma;
+	unsigned long flags;
+	struct videobuf_buffer *vb;
+
+	status = REG_READ(vip, DVP_ITS);
+
+	if (!status) {
+		pr_debug("VIP: irq ignored\n");
+		return IRQ_NONE;
+	}
+
+	if (!vip->started)
+		return IRQ_HANDLED;
+
+	if (status & DVP_IT_VSB)
+		vip->bcount++;
+
+	if (status & DVP_IT_VST)
+		vip->tcount++;
+
+	if ((DVP_IT_VSB | DVP_IT_VST) == (status & (DVP_IT_VST | DVP_IT_VSB))) {
+		/* this is bad, we are too slow, hope the condition is gone
+		 * on the next frame */
+		pr_info("VIP: both irqs\n");
+		return IRQ_HANDLED;
+	}
+
+	if (status & DVP_IT_FIFO) {
+		if (5 > vip->overflow++)
+			pr_info("VIP: fifo overflow\n");
+	}
+
+	if (2 > vip->tcount)
+		return IRQ_HANDLED;
+
+	if (status & DVP_IT_VSB)
+		return IRQ_HANDLED;
+
+	spin_lock_irqsave(&vip->slock, flags);
+
+	REG_WRITE(vip, DVP_CTL, REG_READ(vip, DVP_CTL) & ~DVP_CTL_ENA);
+	if (vip->active) {
+		do_gettimeofday(&vip->active->ts);
+		vip->active->field_count++;
+		vip->active->state = VIDEOBUF_DONE;
+		wake_up(&vip->active->done);
+		vip->active = NULL;
+	}
+	if (!vip->closing) {
+		if (list_empty(&vip->capture))
+			goto done;
+
+		vb = list_first_entry(&vip->capture, struct videobuf_buffer,
+				      queue);
+		if (NULL == vb) {
+			pr_info("VIP: no buffer\n");
+			goto done;
+		}
+		vb->state = VIDEOBUF_ACTIVE;
+		list_del(&vb->queue);
+		vip->active = vb;
+		dma = videobuf_to_dma_contig(vb);
+		switch (vip->format.field) {
+		case V4L2_FIELD_INTERLACED:
+			REG_WRITE(vip, DVP_VTP, dma);
+			REG_WRITE(vip, DVP_VBP, dma + vip->format.width * 2);
+			break;
+		case V4L2_FIELD_TOP:
+		case V4L2_FIELD_BOTTOM:
+			REG_WRITE(vip, DVP_VTP, dma);
+			REG_WRITE(vip, DVP_VBP, dma);
+			break;
+		default:
+			pr_warning("VIP: unknown field format\n");
+			goto done;
+			break;
+		}
+		REG_WRITE(vip, DVP_CTL, REG_READ(vip, DVP_CTL) | DVP_CTL_ENA);
+	}
+done:
+	spin_unlock_irqrestore(&vip->slock, flags);
+	return IRQ_HANDLED;
+}
+
+/**
+ * vip_gpio_reserve - reserve gpio pin
+ * @dev: device
+ * @pin: GPIO pin number
+ * @dir: direction, input or output
+ * @name: GPIO pin name
+ *
+ */
+static int vip_gpio_reserve(struct device *dev, int pin, int dir,
+			    const char *name)
+{
+	int ret;
+
+	if (pin == -1)
+		return 0;
+
+	ret = gpio_request(pin, name);
+	if (ret) {
+		dev_err(dev, "Failed to allocate pin %d (%s)\n", pin, name);
+		return ret;
+	}
+
+	ret = gpio_direction_output(pin, dir);
+	if (ret) {
+		dev_err(dev, "Failed to set direction for pin %d (%s)\n",
+			pin, name);
+		gpio_free(pin);
+		return ret;
+	}
+
+	ret = gpio_export(pin, false);
+	if (ret) {
+		dev_err(dev, "Failed to export pin %d (%s)\n", pin, name);
+		gpio_free(pin);
+		return ret;
+	}
+
+	return 0;
+}
+
+/**
+ * vip_gpio_release - release gpio pin
+ * @dev: device
+ * @pin: GPIO pin number
+ * @name: GPIO pin name
+ *
+ */
+static void vip_gpio_release(struct device *dev, int pin, const char *name)
+{
+	if (pin != -1) {
+		dev_dbg(dev, "releasing pin %d (%s)\n",	pin, name);
+		gpio_unexport(pin);
+		gpio_free(pin);
+	}
+}
+
+/**
+ * sta2x11_vip_init_one - init one instance of video device
+ * @pdev: PCI device
+ * @ent: (not used)
+ *
+ * allocate reset pins for DAC.
+ * Reset video DAC, this is done via reset line.
+ * allocate memory for managing device
+ * request interrupt
+ * map IO region
+ * register device
+ * find and initialize video DAC
+ *
+ * return value: 0, no error
+ *
+ * -ENOMEM, no memory
+ *
+ * -ENODEV, device could not be detected or registered
+ */
+static int __devinit sta2x11_vip_init_one(struct pci_dev *pdev,
+					  const struct pci_device_id *ent)
+{
+	int ret;
+	struct sta2x11_vip *vip;
+	struct vip_config *config;
+
+	ret = pci_enable_device(pdev);
+	if (ret)
+		return ret;
+
+	config = dev_get_platdata(&pdev->dev);
+	if (!config) {
+		dev_info(&pdev->dev, "VIP slot disabled\n");
+		ret = -EINVAL;
+		goto disable;
+	}
+
+	ret = vip_gpio_reserve(&pdev->dev, config->pwr_pin, 0,
+			       config->pwr_name);
+	if (ret)
+		goto disable;
+
+	if (config->reset_pin >= 0) {
+		ret = vip_gpio_reserve(&pdev->dev, config->reset_pin, 0,
+				       config->reset_name);
+		if (ret) {
+			vip_gpio_release(&pdev->dev, config->pwr_pin,
+					 config->pwr_name);
+			goto disable;
+		}
+	}
+
+	if (config->pwr_pin != -1) {
+		/* Datasheet says 5ms between PWR and RST */
+		usleep_range(5000, 25000);
+		ret = gpio_direction_output(config->pwr_pin, 1);
+	}
+
+	if (config->reset_pin != -1) {
+		/* Datasheet says 5ms between PWR and RST */
+		usleep_range(5000, 25000);
+		ret = gpio_direction_output(config->reset_pin, 1);
+	}
+	usleep_range(5000, 25000);
+
+	vip = kzalloc(sizeof(struct sta2x11_vip), GFP_KERNEL);
+	if (!vip) {
+		ret = -ENOMEM;
+		goto release_gpios;
+	}
+
+	vip->pdev = pdev;
+	vip->std = V4L2_STD_PAL;
+	vip->format = formats_50[0];
+	vip->config = config;
+
+	if (v4l2_device_register(&pdev->dev, &vip->v4l2_dev))
+		goto free_mem;
+
+	dev_dbg(&pdev->dev, "BAR #0 at 0x%lx 0x%lx irq %d\n",
+		(unsigned long)pci_resource_start(pdev, 0),
+		(unsigned long)pci_resource_len(pdev, 0), pdev->irq);
+
+	pci_set_master(pdev);
+
+	ret = pci_request_regions(pdev, DRV_NAME);
+	if (ret)
+		goto unreg;
+
+	vip->iomem = pci_iomap(pdev, 0, 0x100);
+	if (!vip->iomem) {
+		ret = -ENOMEM; /* FIXME */
+		goto release;
+	}
+
+	pci_enable_msi(pdev);
+
+	INIT_LIST_HEAD(&vip->capture);
+	spin_lock_init(&vip->slock);
+	mutex_init(&vip->mutex);
+	vip->started = 0;
+	vip->disabled = 0;
+
+	ret = request_irq(pdev->irq,
+			  (irq_handler_t) vip_irq,
+			  IRQF_SHARED, DRV_NAME, vip);
+	if (ret) {
+		dev_err(&pdev->dev, "request_irq failed\n");
+		ret = -ENODEV;
+		goto unmap;
+	}
+
+	vip->video_dev = video_device_alloc();
+	if (!vip->video_dev) {
+		ret = -ENOMEM;
+		goto release_irq;
+	}
+
+	*(vip->video_dev) = video_dev_template;
+	video_set_drvdata(vip->video_dev, vip);
+
+	ret = video_register_device(vip->video_dev, VFL_TYPE_GRABBER, -1);
+	if (ret)
+		goto vrelease;
+
+	vip->adapter = i2c_get_adapter(vip->config->i2c_id);
+	if (!vip->adapter) {
+		ret = -ENODEV;
+		dev_err(&pdev->dev, "no I2C adapter found\n");
+		goto vunreg;
+	}
+
+	vip->decoder = v4l2_i2c_new_subdev(&vip->v4l2_dev, vip->adapter,
+					   "adv7180", vip->config->i2c_addr,
+					   NULL);
+	if (!vip->decoder) {
+		ret = -ENODEV;
+		dev_err(&pdev->dev, "no decoder found\n");
+		goto vunreg;
+	}
+
+	i2c_put_adapter(vip->adapter);
+
+	v4l2_subdev_call(vip->decoder, core, init, 0);
+
+	pr_info("STA2X11 Video Input Port (VIP) loaded\n");
+	return 0;
+
+vunreg:
+	video_set_drvdata(vip->video_dev, NULL);
+vrelease:
+	if (video_is_registered(vip->video_dev))
+		video_unregister_device(vip->video_dev);
+	else
+		video_device_release(vip->video_dev);
+release_irq:
+	free_irq(pdev->irq, vip);
+	pci_disable_msi(pdev);
+unmap:
+	pci_iounmap(pdev, vip->iomem);
+	mutex_destroy(&vip->mutex);
+release:
+	pci_release_regions(pdev);
+unreg:
+	v4l2_device_unregister(&vip->v4l2_dev);
+free_mem:
+	kfree(vip);
+release_gpios:
+	vip_gpio_release(&pdev->dev, config->reset_pin, config->reset_name);
+	vip_gpio_release(&pdev->dev, config->pwr_pin, config->pwr_name);
+disable:
+	/*
+	 * do not call pci_disable_device on sta2x11 because it break all
+	 * other Bus masters on this EP
+	 */
+	return ret;
+}
+
+/**
+ * sta2x11_vip_remove_one - release device
+ * @pdev: PCI device
+ *
+ * Undo everything done in .._init_one
+ *
+ * unregister video device
+ * free interrupt
+ * unmap ioadresses
+ * free memory
+ * free GPIO pins
+ */
+static void __devexit sta2x11_vip_remove_one(struct pci_dev *pdev)
+{
+	struct v4l2_device *v4l2_dev = pci_get_drvdata(pdev);
+	struct sta2x11_vip *vip =
+	    container_of(v4l2_dev, struct sta2x11_vip, v4l2_dev);
+
+	video_set_drvdata(vip->video_dev, NULL);
+	video_unregister_device(vip->video_dev);
+	/*do not call video_device_release() here, is already done */
+	free_irq(pdev->irq, vip);
+	pci_disable_msi(pdev);
+	pci_iounmap(pdev, vip->iomem);
+	pci_release_regions(pdev);
+
+	v4l2_device_unregister(&vip->v4l2_dev);
+	mutex_destroy(&vip->mutex);
+
+	vip_gpio_release(&pdev->dev, vip->config->pwr_pin,
+			 vip->config->pwr_name);
+	vip_gpio_release(&pdev->dev, vip->config->reset_pin,
+			 vip->config->reset_name);
+
+	kfree(vip);
+	/*
+	 * do not call pci_disable_device on sta2x11 because it break all
+	 * other Bus masters on this EP
+	 */
+}
+
+#ifdef CONFIG_PM
+
+/**
+ * sta2x11_vip_suspend - set device into power save mode
+ * @pdev: PCI device
+ * @state: new state of device
+ *
+ * all relevant registers are saved and an attempt to set a new state is made.
+ *
+ * return value: 0 always indicate success,
+ * even if device could not be disabled. (workaround for hardware problem)
+ *
+ * reurn value : 0, always succesful, even if hardware does not not support
+ * power down mode.
+ */
+static int sta2x11_vip_suspend(struct pci_dev *pdev, pm_message_t state)
+{
+	struct v4l2_device *v4l2_dev = pci_get_drvdata(pdev);
+	struct sta2x11_vip *vip =
+	    container_of(v4l2_dev, struct sta2x11_vip, v4l2_dev);
+	unsigned long flags;
+	int i;
+
+	spin_lock_irqsave(&vip->slock, flags);
+	vip->register_save_area[0] = REG_READ(vip, DVP_CTL);
+	REG_WRITE(vip, DVP_CTL, vip->register_save_area[0] & DVP_CTL_DIS);
+	vip->register_save_area[SAVE_COUNT] = REG_READ(vip, DVP_ITM);
+	REG_WRITE(vip, DVP_ITM, 0);
+	for (i = 1; i < SAVE_COUNT; i++)
+		vip->register_save_area[i] = REG_READ(vip, 4 * i);
+	for (i = 0; i < AUX_COUNT; i++)
+		vip->register_save_area[SAVE_COUNT + IRQ_COUNT + i] =
+		    REG_READ(vip, registers_to_save[i]);
+	spin_unlock_irqrestore(&vip->slock, flags);
+	/* save pci state */
+	pci_save_state(pdev);
+	if (pci_set_power_state(pdev, pci_choose_state(pdev, state))) {
+		/*
+		 * do not call pci_disable_device on sta2x11 because it
+		 * break all other Bus masters on this EP
+		 */
+		vip->disabled = 1;
+	}
+
+	pr_info("VIP: suspend\n");
+	return 0;
+}
+
+/**
+ * sta2x11_vip_resume - resume device operation
+ * @pdev : PCI device
+ *
+ * re-enable device, set PCI state to powered and restore registers.
+ * resume normal device operation afterwards.
+ *
+ * return value: 0, no error.
+ *
+ * other, could not set device to power on state.
+ */
+static int sta2x11_vip_resume(struct pci_dev *pdev)
+{
+	struct v4l2_device *v4l2_dev = pci_get_drvdata(pdev);
+	struct sta2x11_vip *vip =
+	    container_of(v4l2_dev, struct sta2x11_vip, v4l2_dev);
+	unsigned long flags;
+	int ret, i;
+
+	pr_info("VIP: resume\n");
+	/* restore pci state */
+	if (vip->disabled) {
+		ret = pci_enable_device(pdev);
+		if (ret) {
+			pr_warning("VIP: Can't enable device.\n");
+			return ret;
+		}
+		vip->disabled = 0;
+	}
+	ret = pci_set_power_state(pdev, PCI_D0);
+	if (ret) {
+		/*
+		 * do not call pci_disable_device on sta2x11 because it
+		 * break all other Bus masters on this EP
+		 */
+		pr_warning("VIP: Can't enable device.\n");
+		vip->disabled = 1;
+		return ret;
+	}
+
+	pci_restore_state(pdev);
+
+	spin_lock_irqsave(&vip->slock, flags);
+	for (i = 1; i < SAVE_COUNT; i++)
+		REG_WRITE(vip, 4 * i, vip->register_save_area[i]);
+	for (i = 0; i < AUX_COUNT; i++)
+		REG_WRITE(vip, registers_to_save[i],
+			  vip->register_save_area[SAVE_COUNT + IRQ_COUNT + i]);
+	REG_WRITE(vip, DVP_CTL, vip->register_save_area[0]);
+	REG_WRITE(vip, DVP_ITM, vip->register_save_area[SAVE_COUNT]);
+	spin_unlock_irqrestore(&vip->slock, flags);
+	return 0;
+}
+
+#endif
+
+static DEFINE_PCI_DEVICE_TABLE(sta2x11_vip_pci_tbl) = {
+	{PCI_DEVICE(PCI_VENDOR_ID_STMICRO, PCI_DEVICE_ID_STMICRO_VIP)},
+	{0,}
+};
+
+static struct pci_driver sta2x11_vip_driver = {
+	.name = DRV_NAME,
+	.probe = sta2x11_vip_init_one,
+	.remove = __devexit_p(sta2x11_vip_remove_one),
+	.id_table = sta2x11_vip_pci_tbl,
+#ifdef CONFIG_PM
+	.suspend = sta2x11_vip_suspend,
+	.resume = sta2x11_vip_resume,
+#endif
+};
+
+static int __init sta2x11_vip_init_module(void)
+{
+	return pci_register_driver(&sta2x11_vip_driver);
+}
+
+static void __exit sta2x11_vip_exit_module(void)
+{
+	pci_unregister_driver(&sta2x11_vip_driver);
+}
+
+#ifdef MODULE
+module_init(sta2x11_vip_init_module);
+module_exit(sta2x11_vip_exit_module);
+#else
+late_initcall_sync(sta2x11_vip_init_module);
+#endif
+
+MODULE_DESCRIPTION("STA2X11 Video Input Port driver");
+MODULE_AUTHOR("Wind River");
+MODULE_LICENSE("GPL v2");
+MODULE_SUPPORTED_DEVICE("sta2x11 video input");
+MODULE_VERSION(DRV_VERSION);
+MODULE_DEVICE_TABLE(pci, sta2x11_vip_pci_tbl);
diff --git a/drivers/media/video/sta2x11_vip.h b/drivers/media/video/sta2x11_vip.h
new file mode 100644
index 00000000000..4f81a13666e
--- /dev/null
+++ b/drivers/media/video/sta2x11_vip.h
@@ -0,0 +1,40 @@
+/*
+ * Copyright (c) 2011 Wind River Systems, 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
+ *
+ * Author:  Anders Wallin <anders.wallin@windriver.com>
+ *
+ */
+
+#ifndef __STA2X11_VIP_H
+#define __STA2X11_VIP_H
+
+/**
+ * struct vip_config - video input configuration data
+ * @pwr_name: ADV powerdown name
+ * @pwr_pin: ADV powerdown pin
+ * @reset_name: ADV reset name
+ * @reset_pin: ADV reset pin
+ */
+struct vip_config {
+	const char *pwr_name;
+	int pwr_pin;
+	const char *reset_name;
+	int reset_pin;
+	int i2c_id;
+	int i2c_addr;
+};
+
+#endif /* __STA2X11_VIP_H */
diff --git a/drivers/media/video/stk-webcam.c b/drivers/media/video/stk-webcam.c
index d427f8436c7..86a0fc56c33 100644
--- a/drivers/media/video/stk-webcam.c
+++ b/drivers/media/video/stk-webcam.c
@@ -38,13 +38,13 @@
 #include "stk-webcam.h"
 
 
-static bool hflip = 1;
+static bool hflip;
 module_param(hflip, bool, 0444);
-MODULE_PARM_DESC(hflip, "Horizontal image flip (mirror). Defaults to 1");
+MODULE_PARM_DESC(hflip, "Horizontal image flip (mirror). Defaults to 0");
 
-static bool vflip = 1;
+static bool vflip;
 module_param(vflip, bool, 0444);
-MODULE_PARM_DESC(vflip, "Vertical image flip. Defaults to 1");
+MODULE_PARM_DESC(vflip, "Vertical image flip. Defaults to 0");
 
 static int debug;
 module_param(debug, int, 0444);
diff --git a/drivers/media/video/tda9840.c b/drivers/media/video/tda9840.c
index 465d7086bab..3d7ddd93282 100644
--- a/drivers/media/video/tda9840.c
+++ b/drivers/media/video/tda9840.c
@@ -66,29 +66,53 @@ static void tda9840_write(struct v4l2_subdev *sd, u8 reg, u8 val)
 				val, reg);
 }
 
+static int tda9840_status(struct v4l2_subdev *sd)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	u8 byte;
+
+	if (1 != i2c_master_recv(client, &byte, 1)) {
+		v4l2_dbg(1, debug, sd,
+			"i2c_master_recv() failed\n");
+		return -EIO;
+	}
+
+	if (byte & 0x80) {
+		v4l2_dbg(1, debug, sd,
+			"TDA9840_DETECT: register contents invalid\n");
+		return -EINVAL;
+	}
+
+	v4l2_dbg(1, debug, sd, "TDA9840_DETECT: byte: 0x%02x\n", byte);
+	return byte & 0x60;
+}
+
 static int tda9840_s_tuner(struct v4l2_subdev *sd, struct v4l2_tuner *t)
 {
+	int stat = tda9840_status(sd);
 	int byte;
 
 	if (t->index)
 		return -EINVAL;
 
-	switch (t->audmode) {
-	case V4L2_TUNER_MODE_STEREO:
-		byte = TDA9840_SET_STEREO;
-		break;
-	case V4L2_TUNER_MODE_LANG1_LANG2:
-		byte = TDA9840_SET_BOTH;
-		break;
-	case V4L2_TUNER_MODE_LANG1:
-		byte = TDA9840_SET_LANG1;
-		break;
-	case V4L2_TUNER_MODE_LANG2:
-		byte = TDA9840_SET_LANG2;
-		break;
-	default:
+	stat = stat < 0 ? 0 : stat;
+	if (stat == 0 || stat == 0x60) /* mono input */
 		byte = TDA9840_SET_MONO;
-		break;
+	else if (stat == 0x40) /* stereo input */
+		byte = (t->audmode == V4L2_TUNER_MODE_MONO) ?
+			TDA9840_SET_MONO : TDA9840_SET_STEREO;
+	else { /* bilingual */
+		switch (t->audmode) {
+		case V4L2_TUNER_MODE_LANG1_LANG2:
+			byte = TDA9840_SET_BOTH;
+			break;
+		case V4L2_TUNER_MODE_LANG2:
+			byte = TDA9840_SET_LANG2;
+			break;
+		default:
+			byte = TDA9840_SET_LANG1;
+			break;
+		}
 	}
 	v4l2_dbg(1, debug, sd, "TDA9840_SWITCH: 0x%02x\n", byte);
 	tda9840_write(sd, SWITCH, byte);
@@ -97,25 +121,14 @@ static int tda9840_s_tuner(struct v4l2_subdev *sd, struct v4l2_tuner *t)
 
 static int tda9840_g_tuner(struct v4l2_subdev *sd, struct v4l2_tuner *t)
 {
-	struct i2c_client *client = v4l2_get_subdevdata(sd);
-	u8 byte;
-
-	t->rxsubchans = V4L2_TUNER_SUB_MONO;
-	if (1 != i2c_master_recv(client, &byte, 1)) {
-		v4l2_dbg(1, debug, sd,
-			"i2c_master_recv() failed\n");
-		return -EIO;
-	}
+	int stat = tda9840_status(sd);
 
-	if (byte & 0x80) {
-		v4l2_dbg(1, debug, sd,
-			"TDA9840_DETECT: register contents invalid\n");
-		return -EINVAL;
-	}
+	if (stat < 0)
+		return stat;
 
-	v4l2_dbg(1, debug, sd, "TDA9840_DETECT: byte: 0x%02x\n", byte);
+	t->rxsubchans = V4L2_TUNER_SUB_MONO;
 
-	switch (byte & 0x60) {
+	switch (stat & 0x60) {
 	case 0x00:
 		t->rxsubchans = V4L2_TUNER_SUB_MONO;
 		break;
diff --git a/drivers/media/video/tlg2300/pd-video.c b/drivers/media/video/tlg2300/pd-video.c
index a794ae62aeb..bfbf9e56b0a 100644
--- a/drivers/media/video/tlg2300/pd-video.c
+++ b/drivers/media/video/tlg2300/pd-video.c
@@ -150,7 +150,6 @@ static int vidioc_querycap(struct file *file, void *fh,
 	strcpy(cap->driver, "tele-video");
 	strcpy(cap->card, "Telegent Poseidon");
 	usb_make_path(p->udev, cap->bus_info, sizeof(cap->bus_info));
-	cap->version = KERNEL_VERSION(0, 0, 1);
 	cap->capabilities = V4L2_CAP_VIDEO_CAPTURE | V4L2_CAP_TUNER |
 				V4L2_CAP_AUDIO | V4L2_CAP_STREAMING |
 				V4L2_CAP_READWRITE | V4L2_CAP_VBI_CAPTURE;
diff --git a/drivers/media/video/tm6000/tm6000-input.c b/drivers/media/video/tm6000/tm6000-input.c
index 859eb90e4d5..e80b7e19047 100644
--- a/drivers/media/video/tm6000/tm6000-input.c
+++ b/drivers/media/video/tm6000/tm6000-input.c
@@ -168,7 +168,6 @@ static void tm6000_ir_urb_received(struct urb *urb)
 	struct tm6000_IR *ir = dev->ir;
 	struct tm6000_ir_poll_result poll_result;
 	char *buf;
-	int rc;
 
 	dprintk(2, "%s\n",__func__);
 	if (urb->status < 0 || urb->actual_length <= 0) {
@@ -192,7 +191,7 @@ static void tm6000_ir_urb_received(struct urb *urb)
 	dprintk(1, "%s, scancode: 0x%04x\n",__func__, poll_result.rc_data);
 	rc_keydown(ir->rc, poll_result.rc_data, 0);
 
-	rc = usb_submit_urb(urb, GFP_ATOMIC);
+	usb_submit_urb(urb, GFP_ATOMIC);
 	/*
 	 * Flash the led. We can't do it here, as it is running on IRQ context.
 	 * So, use the scheduler to do it, in a few ms.
diff --git a/drivers/media/video/tm6000/tm6000-stds.c b/drivers/media/video/tm6000/tm6000-stds.c
index 9dc0831d813..5e28d6a2412 100644
--- a/drivers/media/video/tm6000/tm6000-stds.c
+++ b/drivers/media/video/tm6000/tm6000-stds.c
@@ -338,7 +338,6 @@ static int tm6000_set_audio_std(struct tm6000_core *dev)
 	uint8_t areg_02 = 0x04; /* GC1 Fixed gain 0dB */
 	uint8_t areg_05 = 0x01; /* Auto 4.5 = M Japan, Auto 6.5 = DK */
 	uint8_t areg_06 = 0x02; /* Auto de-emphasis, mannual channel mode */
-	uint8_t nicam_flag = 0; /* No NICAM */
 
 	if (dev->radio) {
 		tm6000_set_reg(dev, TM6010_REQ08_R01_A_INIT, 0x00);
@@ -398,7 +397,6 @@ static int tm6000_set_audio_std(struct tm6000_core *dev)
 		} else {
 			areg_05 = 0x07;
 		}
-		nicam_flag = 1;
 		break;
 	/* other */
 	case 3:
diff --git a/drivers/media/video/tm6000/tm6000-video.c b/drivers/media/video/tm6000/tm6000-video.c
index bc13db736e2..f7034df94e0 100644
--- a/drivers/media/video/tm6000/tm6000-video.c
+++ b/drivers/media/video/tm6000/tm6000-video.c
@@ -169,7 +169,6 @@ static inline void get_next_buf(struct tm6000_dmaqueue *dma_q,
 			       struct tm6000_buffer   **buf)
 {
 	struct tm6000_core *dev = container_of(dma_q, struct tm6000_core, vidq);
-	char *outp;
 
 	if (list_empty(&dma_q->active)) {
 		dprintk(dev, V4L2_DEBUG_QUEUE, "No active queue to serve\n");
@@ -179,11 +178,6 @@ static inline void get_next_buf(struct tm6000_dmaqueue *dma_q,
 
 	*buf = list_entry(dma_q->active.next,
 			struct tm6000_buffer, vb.queue);
-
-	/* Cleans up buffer - Useful for testing for frame/URB loss */
-	outp = videobuf_to_vmalloc(&(*buf)->vb);
-
-	return;
 }
 
 /*
@@ -211,7 +205,7 @@ static int copy_streams(u8 *data, unsigned long len,
 {
 	struct tm6000_dmaqueue  *dma_q = urb->context;
 	struct tm6000_core *dev = container_of(dma_q, struct tm6000_core, vidq);
-	u8 *ptr = data, *endp = data+len, c;
+	u8 *ptr = data, *endp = data+len;
 	unsigned long header = 0;
 	int rc = 0;
 	unsigned int cmd, cpysize, pktsize, size, field, block, line, pos = 0;
@@ -264,7 +258,6 @@ static int copy_streams(u8 *data, unsigned long len,
 			}
 
 			/* split the header fields */
-			c = (header >> 24) & 0xff;
 			size = ((header & 0x7e) << 1);
 			if (size > 0)
 				size -= 4;
@@ -889,7 +882,6 @@ static int vidioc_querycap(struct file *file, void  *priv,
 
 	strlcpy(cap->driver, "tm6000", sizeof(cap->driver));
 	strlcpy(cap->card, "Trident TVMaster TM5600/6000/6010", sizeof(cap->card));
-	cap->version = TM6000_VERSION;
 	cap->capabilities =	V4L2_CAP_VIDEO_CAPTURE |
 				V4L2_CAP_STREAMING     |
 				V4L2_CAP_AUDIO         |
@@ -1732,6 +1724,10 @@ static struct video_device *vdev_init(struct tm6000_core *dev,
 	vfd->release = video_device_release;
 	vfd->debug = tm6000_debug;
 	vfd->lock = &dev->lock;
+	/* Locking in file operations other than ioctl should be done
+	   by the driver, not the V4L2 core.
+	   This driver needs auditing so that this flag can be removed. */
+	set_bit(V4L2_FL_LOCK_ALL_FOPS, &vfd->flags);
 
 	snprintf(vfd->name, sizeof(vfd->name), "%s %s", dev->name, type_name);
 
diff --git a/drivers/media/video/tm6000/tm6000.h b/drivers/media/video/tm6000/tm6000.h
index 27ba659cfa8..6df418658c9 100644
--- a/drivers/media/video/tm6000/tm6000.h
+++ b/drivers/media/video/tm6000/tm6000.h
@@ -33,8 +33,6 @@
 #include "dvb_frontend.h"
 #include "dmxdev.h"
 
-#define TM6000_VERSION KERNEL_VERSION(0, 0, 2)
-
 /* Inputs */
 enum tm6000_itype {
 	TM6000_INPUT_TV	= 1,
diff --git a/drivers/media/video/tuner-core.c b/drivers/media/video/tuner-core.c
index a5c6397ad59..3e050e12153 100644
--- a/drivers/media/video/tuner-core.c
+++ b/drivers/media/video/tuner-core.c
@@ -1241,8 +1241,10 @@ static int tuner_log_status(struct v4l2_subdev *sd)
 	return 0;
 }
 
-static int tuner_suspend(struct i2c_client *c, pm_message_t state)
+#ifdef CONFIG_PM_SLEEP
+static int tuner_suspend(struct device *dev)
 {
+	struct i2c_client *c = to_i2c_client(dev);
 	struct tuner *t = to_tuner(i2c_get_clientdata(c));
 	struct analog_demod_ops *analog_ops = &t->fe.ops.analog_ops;
 
@@ -1254,8 +1256,9 @@ static int tuner_suspend(struct i2c_client *c, pm_message_t state)
 	return 0;
 }
 
-static int tuner_resume(struct i2c_client *c)
+static int tuner_resume(struct device *dev)
 {
+	struct i2c_client *c = to_i2c_client(dev);
 	struct tuner *t = to_tuner(i2c_get_clientdata(c));
 
 	tuner_dbg("resume\n");
@@ -1266,6 +1269,7 @@ static int tuner_resume(struct i2c_client *c)
 
 	return 0;
 }
+#endif
 
 static int tuner_command(struct i2c_client *client, unsigned cmd, void *arg)
 {
@@ -1310,6 +1314,10 @@ static const struct v4l2_subdev_ops tuner_ops = {
  * I2C structs and module init functions
  */
 
+static const struct dev_pm_ops tuner_pm_ops = {
+	SET_SYSTEM_SLEEP_PM_OPS(tuner_suspend, tuner_resume)
+};
+
 static const struct i2c_device_id tuner_id[] = {
 	{ "tuner", }, /* autodetect */
 	{ }
@@ -1320,12 +1328,11 @@ static struct i2c_driver tuner_driver = {
 	.driver = {
 		.owner	= THIS_MODULE,
 		.name	= "tuner",
+		.pm	= &tuner_pm_ops,
 	},
 	.probe		= tuner_probe,
 	.remove		= tuner_remove,
 	.command	= tuner_command,
-	.suspend	= tuner_suspend,
-	.resume		= tuner_resume,
 	.id_table	= tuner_id,
 };
 
diff --git a/drivers/media/video/tvp5150.c b/drivers/media/video/tvp5150.c
index 1326e11cf4a..b7867427e5c 100644
--- a/drivers/media/video/tvp5150.c
+++ b/drivers/media/video/tvp5150.c
@@ -822,7 +822,7 @@ static int tvp5150_enum_mbus_fmt(struct v4l2_subdev *sd, unsigned index,
 	if (index)
 		return -EINVAL;
 
-	*code = V4L2_MBUS_FMT_YUYV8_2X8;
+	*code = V4L2_MBUS_FMT_UYVY8_2X8;
 	return 0;
 }
 
@@ -830,23 +830,16 @@ static int tvp5150_mbus_fmt(struct v4l2_subdev *sd,
 			    struct v4l2_mbus_framefmt *f)
 {
 	struct tvp5150 *decoder = to_tvp5150(sd);
-	v4l2_std_id std;
 
 	if (f == NULL)
 		return -EINVAL;
 
 	tvp5150_reset(sd, 0);
 
-	/* Calculate height and width based on current standard */
-	if (decoder->norm == V4L2_STD_ALL)
-		std = tvp5150_read_std(sd);
-	else
-		std = decoder->norm;
-
 	f->width = decoder->rect.width;
 	f->height = decoder->rect.height;
 
-	f->code = V4L2_MBUS_FMT_YUYV8_2X8;
+	f->code = V4L2_MBUS_FMT_UYVY8_2X8;
 	f->field = V4L2_FIELD_SEQ_TB;
 	f->colorspace = V4L2_COLORSPACE_SMPTE170M;
 
diff --git a/drivers/media/video/tvp7002.c b/drivers/media/video/tvp7002.c
index d7676d85c4d..fb6a5b57eb8 100644
--- a/drivers/media/video/tvp7002.c
+++ b/drivers/media/video/tvp7002.c
@@ -29,6 +29,7 @@
 #include <linux/slab.h>
 #include <linux/videodev2.h>
 #include <linux/module.h>
+#include <linux/v4l2-dv-timings.h>
 #include <media/tvp7002.h>
 #include <media/v4l2-device.h>
 #include <media/v4l2-chip-ident.h>
@@ -328,6 +329,7 @@ static const struct i2c_reg_value tvp7002_parms_720P50[] = {
 /* Preset definition for handling device operation */
 struct tvp7002_preset_definition {
 	u32 preset;
+	struct v4l2_dv_timings timings;
 	const struct i2c_reg_value *p_settings;
 	enum v4l2_colorspace color_space;
 	enum v4l2_field scanmode;
@@ -341,6 +343,7 @@ struct tvp7002_preset_definition {
 static const struct tvp7002_preset_definition tvp7002_presets[] = {
 	{
 		V4L2_DV_720P60,
+		V4L2_DV_BT_CEA_1280X720P60,
 		tvp7002_parms_720P60,
 		V4L2_COLORSPACE_REC709,
 		V4L2_FIELD_NONE,
@@ -351,6 +354,7 @@ static const struct tvp7002_preset_definition tvp7002_presets[] = {
 	},
 	{
 		V4L2_DV_1080I60,
+		V4L2_DV_BT_CEA_1920X1080I60,
 		tvp7002_parms_1080I60,
 		V4L2_COLORSPACE_REC709,
 		V4L2_FIELD_INTERLACED,
@@ -361,6 +365,7 @@ static const struct tvp7002_preset_definition tvp7002_presets[] = {
 	},
 	{
 		V4L2_DV_1080I50,
+		V4L2_DV_BT_CEA_1920X1080I50,
 		tvp7002_parms_1080I50,
 		V4L2_COLORSPACE_REC709,
 		V4L2_FIELD_INTERLACED,
@@ -371,6 +376,7 @@ static const struct tvp7002_preset_definition tvp7002_presets[] = {
 	},
 	{
 		V4L2_DV_720P50,
+		V4L2_DV_BT_CEA_1280X720P50,
 		tvp7002_parms_720P50,
 		V4L2_COLORSPACE_REC709,
 		V4L2_FIELD_NONE,
@@ -381,6 +387,7 @@ static const struct tvp7002_preset_definition tvp7002_presets[] = {
 	},
 	{
 		V4L2_DV_1080P60,
+		V4L2_DV_BT_CEA_1920X1080P60,
 		tvp7002_parms_1080P60,
 		V4L2_COLORSPACE_REC709,
 		V4L2_FIELD_NONE,
@@ -391,6 +398,7 @@ static const struct tvp7002_preset_definition tvp7002_presets[] = {
 	},
 	{
 		V4L2_DV_480P59_94,
+		V4L2_DV_BT_CEA_720X480P59_94,
 		tvp7002_parms_480P,
 		V4L2_COLORSPACE_SMPTE170M,
 		V4L2_FIELD_NONE,
@@ -401,6 +409,7 @@ static const struct tvp7002_preset_definition tvp7002_presets[] = {
 	},
 	{
 		V4L2_DV_576P50,
+		V4L2_DV_BT_CEA_720X576P50,
 		tvp7002_parms_576P,
 		V4L2_COLORSPACE_SMPTE170M,
 		V4L2_FIELD_NONE,
@@ -605,6 +614,35 @@ static int tvp7002_s_dv_preset(struct v4l2_subdev *sd,
 	return -EINVAL;
 }
 
+static int tvp7002_s_dv_timings(struct v4l2_subdev *sd,
+					struct v4l2_dv_timings *dv_timings)
+{
+	struct tvp7002 *device = to_tvp7002(sd);
+	const struct v4l2_bt_timings *bt = &dv_timings->bt;
+	int i;
+
+	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;
+
+		if (!memcmp(bt, t, &bt->standards - &bt->width)) {
+			device->current_preset = &tvp7002_presets[i];
+			return tvp7002_write_inittab(sd, tvp7002_presets[i].p_settings);
+		}
+	}
+	return -EINVAL;
+}
+
+static int tvp7002_g_dv_timings(struct v4l2_subdev *sd,
+					struct v4l2_dv_timings *dv_timings)
+{
+	struct tvp7002 *device = to_tvp7002(sd);
+
+	*dv_timings = device->current_preset->timings;
+	return 0;
+}
+
 /*
  * tvp7002_s_ctrl() - Set a control
  * @ctrl: ptr to v4l2_ctrl struct
@@ -666,11 +704,9 @@ static int tvp7002_mbus_fmt(struct v4l2_subdev *sd, struct v4l2_mbus_framefmt *f
  * Returns the current DV preset by TVP7002. If no active input is
  * detected, returns -EINVAL
  */
-static int tvp7002_query_dv_preset(struct v4l2_subdev *sd,
-						struct v4l2_dv_preset *qpreset)
+static int tvp7002_query_dv(struct v4l2_subdev *sd, int *index)
 {
 	const struct tvp7002_preset_definition *presets = tvp7002_presets;
-	struct tvp7002 *device;
 	u8 progressive;
 	u32 lpfr;
 	u32 cpln;
@@ -679,12 +715,9 @@ static int tvp7002_query_dv_preset(struct v4l2_subdev *sd,
 	u8 lpf_msb;
 	u8 cpl_lsb;
 	u8 cpl_msb;
-	int index;
-
-	/* Return invalid preset if no active input is detected */
-	qpreset->preset = V4L2_DV_INVALID;
 
-	device = to_tvp7002(sd);
+	/* Return invalid index if no active input is detected */
+	*index = NUM_PRESETS;
 
 	/* Read standards from device registers */
 	tvp7002_read_err(sd, TVP7002_L_FRAME_STAT_LSBS, &lpf_lsb, &error);
@@ -705,8 +738,8 @@ static int tvp7002_query_dv_preset(struct v4l2_subdev *sd,
 	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 &&
+	for (*index = 0; *index < NUM_PRESETS; (*index)++, presets++)
+		if (lpfr == presets->lines_per_frame &&
 			progressive == presets->progressive) {
 			if (presets->cpl_min == 0xffff)
 				break;
@@ -714,17 +747,42 @@ static int tvp7002_query_dv_preset(struct v4l2_subdev *sd,
 				break;
 		}
 
-	if (index == NUM_PRESETS) {
+	if (*index == NUM_PRESETS) {
 		v4l2_dbg(1, debug, sd, "detection failed: lpf = %x, cpl = %x\n",
 								lpfr, cpln);
-		return 0;
+		return -ENOLINK;
 	}
 
-	/* Set values in found preset */
-	qpreset->preset = presets->preset;
-
 	/* Update lines per frame and clocks per line info */
-	v4l2_dbg(1, debug, sd, "detected preset: %d\n", presets->preset);
+	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;
+	return 0;
+}
+
+static int tvp7002_query_dv_timings(struct v4l2_subdev *sd,
+					struct v4l2_dv_timings *timings)
+{
+	int index;
+	int err = tvp7002_query_dv(sd, &index);
+
+	if (err)
+		return err;
+	*timings = tvp7002_presets[index].timings;
 	return 0;
 }
 
@@ -894,6 +952,17 @@ static int tvp7002_enum_dv_presets(struct v4l2_subdev *sd,
 	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)
+		return -EINVAL;
+
+	timings->timings = tvp7002_presets[timings->index].timings;
+	return 0;
+}
+
 static const struct v4l2_ctrl_ops tvp7002_ctrl_ops = {
 	.s_ctrl = tvp7002_s_ctrl,
 };
@@ -920,6 +989,10 @@ 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,
+	.query_dv_timings = tvp7002_query_dv_timings,
 	.s_stream = tvp7002_s_stream,
 	.g_mbus_fmt = tvp7002_mbus_fmt,
 	.try_mbus_fmt = tvp7002_mbus_fmt,
diff --git a/drivers/media/video/usbvision/usbvision-core.c b/drivers/media/video/usbvision/usbvision-core.c
index f344411a457..c9b2042f8bd 100644
--- a/drivers/media/video/usbvision/usbvision-core.c
+++ b/drivers/media/video/usbvision/usbvision-core.c
@@ -601,13 +601,12 @@ static int usbvision_decompress(struct usb_usbvision *usbvision, unsigned char *
 								unsigned char *decompressed, int *start_pos,
 								int *block_typestart_pos, int len)
 {
-	int rest_pixel, idx, max_pos, pos, extra_pos, block_len, block_type_pos, block_type_len;
+	int rest_pixel, idx, pos, extra_pos, block_len, block_type_pos, block_type_len;
 	unsigned char block_byte, block_code, block_type, block_type_byte, integrator;
 
 	integrator = 0;
 	pos = *start_pos;
 	block_type_pos = *block_typestart_pos;
-	max_pos = 396; /* pos + len; */
 	extra_pos = pos;
 	block_len = 0;
 	block_byte = 0;
@@ -702,7 +701,7 @@ static enum parse_state usbvision_parse_compress(struct usb_usbvision *usbvision
 	unsigned char strip_data[USBVISION_STRIP_LEN_MAX];
 	unsigned char strip_header[USBVISION_STRIP_HEADER_LEN];
 	int idx, idx_end, strip_len, strip_ptr, startblock_pos, block_pos, block_type_pos;
-	int clipmask_index, bytes_per_pixel, rc;
+	int clipmask_index;
 	int image_size;
 	unsigned char rv, gv, bv;
 	static unsigned char *Y, *U, *V;
@@ -769,7 +768,6 @@ static enum parse_state usbvision_parse_compress(struct usb_usbvision *usbvision
 		return parse_state_next_frame;
 	}
 
-	bytes_per_pixel = frame->v4l2_format.bytes_per_pixel;
 	clipmask_index = frame->curline * MAX_FRAME_WIDTH;
 
 	scratch_get(usbvision, strip_data, strip_len);
@@ -781,14 +779,14 @@ static enum parse_state usbvision_parse_compress(struct usb_usbvision *usbvision
 
 	usbvision->block_pos = block_pos;
 
-	rc = usbvision_decompress(usbvision, strip_data, Y, &block_pos, &block_type_pos, idx_end);
+	usbvision_decompress(usbvision, strip_data, Y, &block_pos, &block_type_pos, idx_end);
 	if (strip_len > usbvision->max_strip_len)
 		usbvision->max_strip_len = strip_len;
 
 	if (frame->curline % 2)
-		rc = usbvision_decompress(usbvision, strip_data, V, &block_pos, &block_type_pos, idx_end / 2);
+		usbvision_decompress(usbvision, strip_data, V, &block_pos, &block_type_pos, idx_end / 2);
 	else
-		rc = usbvision_decompress(usbvision, strip_data, U, &block_pos, &block_type_pos, idx_end / 2);
+		usbvision_decompress(usbvision, strip_data, U, &block_pos, &block_type_pos, idx_end / 2);
 
 	if (block_pos > usbvision->comprblock_pos)
 		usbvision->comprblock_pos = block_pos;
diff --git a/drivers/media/video/usbvision/usbvision-video.c b/drivers/media/video/usbvision/usbvision-video.c
index 5a74f5e07d7..9bd8f084f34 100644
--- a/drivers/media/video/usbvision/usbvision-video.c
+++ b/drivers/media/video/usbvision/usbvision-video.c
@@ -1296,6 +1296,10 @@ static struct video_device *usbvision_vdev_init(struct usb_usbvision *usbvision,
 	if (NULL == vdev)
 		return NULL;
 	*vdev = *vdev_template;
+	/* Locking in file operations other than ioctl should be done
+	   by the driver, not the V4L2 core.
+	   This driver needs auditing so that this flag can be removed. */
+	set_bit(V4L2_FL_LOCK_ALL_FOPS, &vdev->flags);
 	vdev->lock = &usbvision->v4l2_lock;
 	vdev->v4l2_dev = &usbvision->v4l2_dev;
 	snprintf(vdev->name, sizeof(vdev->name), "%s", name);
diff --git a/drivers/media/video/uvc/uvc_ctrl.c b/drivers/media/video/uvc/uvc_ctrl.c
index 0efd3b10b35..af26bbe6f76 100644
--- a/drivers/media/video/uvc/uvc_ctrl.c
+++ b/drivers/media/video/uvc/uvc_ctrl.c
@@ -21,6 +21,7 @@
 #include <linux/vmalloc.h>
 #include <linux/wait.h>
 #include <linux/atomic.h>
+#include <media/v4l2-ctrls.h>
 
 #include "uvcvideo.h"
 
@@ -420,6 +421,8 @@ static struct uvc_control_mapping uvc_ctrl_mappings[] = {
 		.offset		= 0,
 		.v4l2_type	= V4L2_CTRL_TYPE_INTEGER,
 		.data_type	= UVC_CTRL_DATA_TYPE_SIGNED,
+		.master_id	= V4L2_CID_HUE_AUTO,
+		.master_manual	= 0,
 	},
 	{
 		.id		= V4L2_CID_SATURATION,
@@ -492,6 +495,7 @@ static struct uvc_control_mapping uvc_ctrl_mappings[] = {
 		.offset		= 0,
 		.v4l2_type	= V4L2_CTRL_TYPE_BOOLEAN,
 		.data_type	= UVC_CTRL_DATA_TYPE_BOOLEAN,
+		.slave_ids	= { V4L2_CID_HUE, },
 	},
 	{
 		.id		= V4L2_CID_EXPOSURE_AUTO,
@@ -504,6 +508,7 @@ static struct uvc_control_mapping uvc_ctrl_mappings[] = {
 		.data_type	= UVC_CTRL_DATA_TYPE_BITMASK,
 		.menu_info	= exposure_auto_controls,
 		.menu_count	= ARRAY_SIZE(exposure_auto_controls),
+		.slave_ids	= { V4L2_CID_EXPOSURE_ABSOLUTE, },
 	},
 	{
 		.id		= V4L2_CID_EXPOSURE_AUTO_PRIORITY,
@@ -524,6 +529,8 @@ static struct uvc_control_mapping uvc_ctrl_mappings[] = {
 		.offset		= 0,
 		.v4l2_type	= V4L2_CTRL_TYPE_INTEGER,
 		.data_type	= UVC_CTRL_DATA_TYPE_UNSIGNED,
+		.master_id	= V4L2_CID_EXPOSURE_AUTO,
+		.master_manual	= V4L2_EXPOSURE_MANUAL,
 	},
 	{
 		.id		= V4L2_CID_AUTO_WHITE_BALANCE,
@@ -534,6 +541,7 @@ static struct uvc_control_mapping uvc_ctrl_mappings[] = {
 		.offset		= 0,
 		.v4l2_type	= V4L2_CTRL_TYPE_BOOLEAN,
 		.data_type	= UVC_CTRL_DATA_TYPE_BOOLEAN,
+		.slave_ids	= { V4L2_CID_WHITE_BALANCE_TEMPERATURE, },
 	},
 	{
 		.id		= V4L2_CID_WHITE_BALANCE_TEMPERATURE,
@@ -544,6 +552,8 @@ static struct uvc_control_mapping uvc_ctrl_mappings[] = {
 		.offset		= 0,
 		.v4l2_type	= V4L2_CTRL_TYPE_INTEGER,
 		.data_type	= UVC_CTRL_DATA_TYPE_UNSIGNED,
+		.master_id	= V4L2_CID_AUTO_WHITE_BALANCE,
+		.master_manual	= 0,
 	},
 	{
 		.id		= V4L2_CID_AUTO_WHITE_BALANCE,
@@ -554,6 +564,8 @@ static struct uvc_control_mapping uvc_ctrl_mappings[] = {
 		.offset		= 0,
 		.v4l2_type	= V4L2_CTRL_TYPE_BOOLEAN,
 		.data_type	= UVC_CTRL_DATA_TYPE_BOOLEAN,
+		.slave_ids	= { V4L2_CID_BLUE_BALANCE,
+				    V4L2_CID_RED_BALANCE },
 	},
 	{
 		.id		= V4L2_CID_BLUE_BALANCE,
@@ -564,6 +576,8 @@ static struct uvc_control_mapping uvc_ctrl_mappings[] = {
 		.offset		= 0,
 		.v4l2_type	= V4L2_CTRL_TYPE_INTEGER,
 		.data_type	= UVC_CTRL_DATA_TYPE_SIGNED,
+		.master_id	= V4L2_CID_AUTO_WHITE_BALANCE,
+		.master_manual	= 0,
 	},
 	{
 		.id		= V4L2_CID_RED_BALANCE,
@@ -574,6 +588,8 @@ static struct uvc_control_mapping uvc_ctrl_mappings[] = {
 		.offset		= 16,
 		.v4l2_type	= V4L2_CTRL_TYPE_INTEGER,
 		.data_type	= UVC_CTRL_DATA_TYPE_SIGNED,
+		.master_id	= V4L2_CID_AUTO_WHITE_BALANCE,
+		.master_manual	= 0,
 	},
 	{
 		.id		= V4L2_CID_FOCUS_ABSOLUTE,
@@ -584,6 +600,8 @@ static struct uvc_control_mapping uvc_ctrl_mappings[] = {
 		.offset		= 0,
 		.v4l2_type	= V4L2_CTRL_TYPE_INTEGER,
 		.data_type	= UVC_CTRL_DATA_TYPE_UNSIGNED,
+		.master_id	= V4L2_CID_FOCUS_AUTO,
+		.master_manual	= 0,
 	},
 	{
 		.id		= V4L2_CID_FOCUS_AUTO,
@@ -594,6 +612,7 @@ static struct uvc_control_mapping uvc_ctrl_mappings[] = {
 		.offset		= 0,
 		.v4l2_type	= V4L2_CTRL_TYPE_BOOLEAN,
 		.data_type	= UVC_CTRL_DATA_TYPE_BOOLEAN,
+		.slave_ids	= { V4L2_CID_FOCUS_ABSOLUTE, },
 	},
 	{
 		.id		= V4L2_CID_IRIS_ABSOLUTE,
@@ -899,25 +918,54 @@ static int uvc_ctrl_populate_cache(struct uvc_video_chain *chain,
 	return 0;
 }
 
-int uvc_query_v4l2_ctrl(struct uvc_video_chain *chain,
-	struct v4l2_queryctrl *v4l2_ctrl)
+static int __uvc_ctrl_get(struct uvc_video_chain *chain,
+	struct uvc_control *ctrl, struct uvc_control_mapping *mapping,
+	s32 *value)
 {
-	struct uvc_control *ctrl;
-	struct uvc_control_mapping *mapping;
 	struct uvc_menu_info *menu;
 	unsigned int i;
 	int ret;
 
-	ret = mutex_lock_interruptible(&chain->ctrl_mutex);
-	if (ret < 0)
-		return -ERESTARTSYS;
+	if ((ctrl->info.flags & UVC_CTRL_FLAG_GET_CUR) == 0)
+		return -EINVAL;
 
-	ctrl = uvc_find_control(chain, v4l2_ctrl->id, &mapping);
-	if (ctrl == NULL) {
-		ret = -EINVAL;
-		goto done;
+	if (!ctrl->loaded) {
+		ret = uvc_query_ctrl(chain->dev, UVC_GET_CUR, ctrl->entity->id,
+				chain->dev->intfnum, ctrl->info.selector,
+				uvc_ctrl_data(ctrl, UVC_CTRL_DATA_CURRENT),
+				ctrl->info.size);
+		if (ret < 0)
+			return ret;
+
+		ctrl->loaded = 1;
 	}
 
+	*value = mapping->get(mapping, UVC_GET_CUR,
+		uvc_ctrl_data(ctrl, UVC_CTRL_DATA_CURRENT));
+
+	if (mapping->v4l2_type == V4L2_CTRL_TYPE_MENU) {
+		menu = mapping->menu_info;
+		for (i = 0; i < mapping->menu_count; ++i, ++menu) {
+			if (menu->value == *value) {
+				*value = i;
+				break;
+			}
+		}
+	}
+
+	return 0;
+}
+
+static int __uvc_query_v4l2_ctrl(struct uvc_video_chain *chain,
+	struct uvc_control *ctrl,
+	struct uvc_control_mapping *mapping,
+	struct v4l2_queryctrl *v4l2_ctrl)
+{
+	struct uvc_control_mapping *master_map = NULL;
+	struct uvc_control *master_ctrl = NULL;
+	struct uvc_menu_info *menu;
+	unsigned int i;
+
 	memset(v4l2_ctrl, 0, sizeof *v4l2_ctrl);
 	v4l2_ctrl->id = mapping->id;
 	v4l2_ctrl->type = mapping->v4l2_type;
@@ -929,10 +977,23 @@ int uvc_query_v4l2_ctrl(struct uvc_video_chain *chain,
 	if (!(ctrl->info.flags & UVC_CTRL_FLAG_SET_CUR))
 		v4l2_ctrl->flags |= V4L2_CTRL_FLAG_READ_ONLY;
 
+	if (mapping->master_id)
+		__uvc_find_control(ctrl->entity, mapping->master_id,
+				   &master_map, &master_ctrl, 0);
+	if (master_ctrl && (master_ctrl->info.flags & UVC_CTRL_FLAG_GET_CUR)) {
+		s32 val;
+		int ret = __uvc_ctrl_get(chain, master_ctrl, master_map, &val);
+		if (ret < 0)
+			return ret;
+
+		if (val != mapping->master_manual)
+				v4l2_ctrl->flags |= V4L2_CTRL_FLAG_INACTIVE;
+	}
+
 	if (!ctrl->cached) {
-		ret = uvc_ctrl_populate_cache(chain, ctrl);
+		int ret = uvc_ctrl_populate_cache(chain, ctrl);
 		if (ret < 0)
-			goto done;
+			return ret;
 	}
 
 	if (ctrl->info.flags & UVC_CTRL_FLAG_GET_DEF) {
@@ -954,19 +1015,19 @@ int uvc_query_v4l2_ctrl(struct uvc_video_chain *chain,
 			}
 		}
 
-		goto done;
+		return 0;
 
 	case V4L2_CTRL_TYPE_BOOLEAN:
 		v4l2_ctrl->minimum = 0;
 		v4l2_ctrl->maximum = 1;
 		v4l2_ctrl->step = 1;
-		goto done;
+		return 0;
 
 	case V4L2_CTRL_TYPE_BUTTON:
 		v4l2_ctrl->minimum = 0;
 		v4l2_ctrl->maximum = 0;
 		v4l2_ctrl->step = 0;
-		goto done;
+		return 0;
 
 	default:
 		break;
@@ -984,6 +1045,27 @@ int uvc_query_v4l2_ctrl(struct uvc_video_chain *chain,
 		v4l2_ctrl->step = mapping->get(mapping, UVC_GET_RES,
 				  uvc_ctrl_data(ctrl, UVC_CTRL_DATA_RES));
 
+	return 0;
+}
+
+int uvc_query_v4l2_ctrl(struct uvc_video_chain *chain,
+	struct v4l2_queryctrl *v4l2_ctrl)
+{
+	struct uvc_control *ctrl;
+	struct uvc_control_mapping *mapping;
+	int ret;
+
+	ret = mutex_lock_interruptible(&chain->ctrl_mutex);
+	if (ret < 0)
+		return -ERESTARTSYS;
+
+	ctrl = uvc_find_control(chain, v4l2_ctrl->id, &mapping);
+	if (ctrl == NULL) {
+		ret = -EINVAL;
+		goto done;
+	}
+
+	ret = __uvc_query_v4l2_ctrl(chain, ctrl, mapping, v4l2_ctrl);
 done:
 	mutex_unlock(&chain->ctrl_mutex);
 	return ret;
@@ -1054,6 +1136,174 @@ done:
 	return ret;
 }
 
+/* --------------------------------------------------------------------------
+ * Ctrl event handling
+ */
+
+static void uvc_ctrl_fill_event(struct uvc_video_chain *chain,
+	struct v4l2_event *ev,
+	struct uvc_control *ctrl,
+	struct uvc_control_mapping *mapping,
+	s32 value, u32 changes)
+{
+	struct v4l2_queryctrl v4l2_ctrl;
+
+	__uvc_query_v4l2_ctrl(chain, ctrl, mapping, &v4l2_ctrl);
+
+	memset(ev->reserved, 0, sizeof(ev->reserved));
+	ev->type = V4L2_EVENT_CTRL;
+	ev->id = v4l2_ctrl.id;
+	ev->u.ctrl.value = value;
+	ev->u.ctrl.changes = changes;
+	ev->u.ctrl.type = v4l2_ctrl.type;
+	ev->u.ctrl.flags = v4l2_ctrl.flags;
+	ev->u.ctrl.minimum = v4l2_ctrl.minimum;
+	ev->u.ctrl.maximum = v4l2_ctrl.maximum;
+	ev->u.ctrl.step = v4l2_ctrl.step;
+	ev->u.ctrl.default_value = v4l2_ctrl.default_value;
+}
+
+static void uvc_ctrl_send_event(struct uvc_fh *handle,
+	struct uvc_control *ctrl, struct uvc_control_mapping *mapping,
+	s32 value, u32 changes)
+{
+	struct v4l2_subscribed_event *sev;
+	struct v4l2_event ev;
+
+	if (list_empty(&mapping->ev_subs))
+		return;
+
+	uvc_ctrl_fill_event(handle->chain, &ev, ctrl, mapping, value, changes);
+
+	list_for_each_entry(sev, &mapping->ev_subs, node) {
+		if (sev->fh && (sev->fh != &handle->vfh ||
+		    (sev->flags & V4L2_EVENT_SUB_FL_ALLOW_FEEDBACK) ||
+		    (changes & V4L2_EVENT_CTRL_CH_FLAGS)))
+			v4l2_event_queue_fh(sev->fh, &ev);
+	}
+}
+
+static void uvc_ctrl_send_slave_event(struct uvc_fh *handle,
+	struct uvc_control *master, u32 slave_id,
+	const struct v4l2_ext_control *xctrls, unsigned int xctrls_count)
+{
+	struct uvc_control_mapping *mapping = NULL;
+	struct uvc_control *ctrl = NULL;
+	u32 changes = V4L2_EVENT_CTRL_CH_FLAGS;
+	unsigned int i;
+	s32 val = 0;
+
+	/*
+	 * We can skip sending an event for the slave if the slave
+	 * is being modified in the same transaction.
+	 */
+	for (i = 0; i < xctrls_count; i++) {
+		if (xctrls[i].id == slave_id)
+			return;
+	}
+
+	__uvc_find_control(master->entity, slave_id, &mapping, &ctrl, 0);
+	if (ctrl == NULL)
+		return;
+
+	if (__uvc_ctrl_get(handle->chain, ctrl, mapping, &val) == 0)
+		changes |= V4L2_EVENT_CTRL_CH_VALUE;
+
+	uvc_ctrl_send_event(handle, ctrl, mapping, val, changes);
+}
+
+static void uvc_ctrl_send_events(struct uvc_fh *handle,
+	const struct v4l2_ext_control *xctrls, unsigned int xctrls_count)
+{
+	struct uvc_control_mapping *mapping;
+	struct uvc_control *ctrl;
+	u32 changes = V4L2_EVENT_CTRL_CH_VALUE;
+	unsigned int i;
+	unsigned int j;
+
+	for (i = 0; i < xctrls_count; ++i) {
+		ctrl = uvc_find_control(handle->chain, xctrls[i].id, &mapping);
+
+		for (j = 0; j < ARRAY_SIZE(mapping->slave_ids); ++j) {
+			if (!mapping->slave_ids[j])
+				break;
+			uvc_ctrl_send_slave_event(handle, ctrl,
+						  mapping->slave_ids[j],
+						  xctrls, xctrls_count);
+		}
+
+		/*
+		 * If the master is being modified in the same transaction
+		 * flags may change too.
+		 */
+		if (mapping->master_id) {
+			for (j = 0; j < xctrls_count; j++) {
+				if (xctrls[j].id == mapping->master_id) {
+					changes |= V4L2_EVENT_CTRL_CH_FLAGS;
+					break;
+				}
+			}
+		}
+
+		uvc_ctrl_send_event(handle, ctrl, mapping, xctrls[i].value,
+				    changes);
+	}
+}
+
+static int uvc_ctrl_add_event(struct v4l2_subscribed_event *sev, unsigned elems)
+{
+	struct uvc_fh *handle = container_of(sev->fh, struct uvc_fh, vfh);
+	struct uvc_control_mapping *mapping;
+	struct uvc_control *ctrl;
+	int ret;
+
+	ret = mutex_lock_interruptible(&handle->chain->ctrl_mutex);
+	if (ret < 0)
+		return -ERESTARTSYS;
+
+	ctrl = uvc_find_control(handle->chain, sev->id, &mapping);
+	if (ctrl == NULL) {
+		ret = -EINVAL;
+		goto done;
+	}
+
+	list_add_tail(&sev->node, &mapping->ev_subs);
+	if (sev->flags & V4L2_EVENT_SUB_FL_SEND_INITIAL) {
+		struct v4l2_event ev;
+		u32 changes = V4L2_EVENT_CTRL_CH_FLAGS;
+		s32 val = 0;
+
+		if (__uvc_ctrl_get(handle->chain, ctrl, mapping, &val) == 0)
+			changes |= V4L2_EVENT_CTRL_CH_VALUE;
+
+		uvc_ctrl_fill_event(handle->chain, &ev, ctrl, mapping, val,
+				    changes);
+		/* Mark the queue as active, allowing this initial
+		   event to be accepted. */
+		sev->elems = elems;
+		v4l2_event_queue_fh(sev->fh, &ev);
+	}
+
+done:
+	mutex_unlock(&handle->chain->ctrl_mutex);
+	return ret;
+}
+
+static void uvc_ctrl_del_event(struct v4l2_subscribed_event *sev)
+{
+	struct uvc_fh *handle = container_of(sev->fh, struct uvc_fh, vfh);
+
+	mutex_lock(&handle->chain->ctrl_mutex);
+	list_del(&sev->node);
+	mutex_unlock(&handle->chain->ctrl_mutex);
+}
+
+const struct v4l2_subscribed_event_ops uvc_ctrl_sub_ev_ops = {
+	.add = uvc_ctrl_add_event,
+	.del = uvc_ctrl_del_event,
+	.replace = v4l2_ctrl_replace,
+	.merge = v4l2_ctrl_merge,
+};
 
 /* --------------------------------------------------------------------------
  * Control transactions
@@ -1101,9 +1351,12 @@ static int uvc_ctrl_commit_entity(struct uvc_device *dev,
 
 		/* Reset the loaded flag for auto-update controls that were
 		 * marked as loaded in uvc_ctrl_get/uvc_ctrl_set to prevent
-		 * uvc_ctrl_get from using the cached value.
+		 * uvc_ctrl_get from using the cached value, and for write-only
+		 * controls to prevent uvc_ctrl_set from setting bits not
+		 * explicitly set by the user.
 		 */
-		if (ctrl->info.flags & UVC_CTRL_FLAG_AUTO_UPDATE)
+		if (ctrl->info.flags & UVC_CTRL_FLAG_AUTO_UPDATE ||
+		    !(ctrl->info.flags & UVC_CTRL_FLAG_GET_CUR))
 			ctrl->loaded = 0;
 
 		if (!ctrl->dirty)
@@ -1131,8 +1384,11 @@ static int uvc_ctrl_commit_entity(struct uvc_device *dev,
 	return 0;
 }
 
-int __uvc_ctrl_commit(struct uvc_video_chain *chain, int rollback)
+int __uvc_ctrl_commit(struct uvc_fh *handle, int rollback,
+		      const struct v4l2_ext_control *xctrls,
+		      unsigned int xctrls_count)
 {
+	struct uvc_video_chain *chain = handle->chain;
 	struct uvc_entity *entity;
 	int ret = 0;
 
@@ -1143,6 +1399,8 @@ int __uvc_ctrl_commit(struct uvc_video_chain *chain, int rollback)
 			goto done;
 	}
 
+	if (!rollback)
+		uvc_ctrl_send_events(handle, xctrls, xctrls_count);
 done:
 	mutex_unlock(&chain->ctrl_mutex);
 	return ret;
@@ -1153,39 +1411,12 @@ int uvc_ctrl_get(struct uvc_video_chain *chain,
 {
 	struct uvc_control *ctrl;
 	struct uvc_control_mapping *mapping;
-	struct uvc_menu_info *menu;
-	unsigned int i;
-	int ret;
 
 	ctrl = uvc_find_control(chain, xctrl->id, &mapping);
-	if (ctrl == NULL || (ctrl->info.flags & UVC_CTRL_FLAG_GET_CUR) == 0)
+	if (ctrl == NULL)
 		return -EINVAL;
 
-	if (!ctrl->loaded) {
-		ret = uvc_query_ctrl(chain->dev, UVC_GET_CUR, ctrl->entity->id,
-				chain->dev->intfnum, ctrl->info.selector,
-				uvc_ctrl_data(ctrl, UVC_CTRL_DATA_CURRENT),
-				ctrl->info.size);
-		if (ret < 0)
-			return ret;
-
-		ctrl->loaded = 1;
-	}
-
-	xctrl->value = mapping->get(mapping, UVC_GET_CUR,
-		uvc_ctrl_data(ctrl, UVC_CTRL_DATA_CURRENT));
-
-	if (mapping->v4l2_type == V4L2_CTRL_TYPE_MENU) {
-		menu = mapping->menu_info;
-		for (i = 0; i < mapping->menu_count; ++i, ++menu) {
-			if (menu->value == xctrl->value) {
-				xctrl->value = i;
-				break;
-			}
-		}
-	}
-
-	return 0;
+	return __uvc_ctrl_get(chain, ctrl, mapping, &xctrl->value);
 }
 
 int uvc_ctrl_set(struct uvc_video_chain *chain,
@@ -1641,6 +1872,8 @@ static int __uvc_ctrl_add_mapping(struct uvc_device *dev,
 	if (map == NULL)
 		return -ENOMEM;
 
+	INIT_LIST_HEAD(&map->ev_subs);
+
 	size = sizeof(*mapping->menu_info) * mapping->menu_count;
 	map->menu_info = kmemdup(mapping->menu_info, size, GFP_KERNEL);
 	if (map->menu_info == NULL) {
@@ -1653,7 +1886,6 @@ static int __uvc_ctrl_add_mapping(struct uvc_device *dev,
 	if (map->set == NULL)
 		map->set = uvc_set_le_value;
 
-	map->ctrl = &ctrl->info;
 	list_add_tail(&map->list, &ctrl->info.mappings);
 	uvc_trace(UVC_TRACE_CONTROL,
 		"Adding mapping '%s' to control %pUl/%u.\n",
diff --git a/drivers/media/video/uvc/uvc_queue.c b/drivers/media/video/uvc/uvc_queue.c
index 8f54e24e3f3..9288fbd5001 100644
--- a/drivers/media/video/uvc/uvc_queue.c
+++ b/drivers/media/video/uvc/uvc_queue.c
@@ -207,6 +207,19 @@ int uvc_queue_mmap(struct uvc_video_queue *queue, struct vm_area_struct *vma)
 	return ret;
 }
 
+#ifndef CONFIG_MMU
+unsigned long uvc_queue_get_unmapped_area(struct uvc_video_queue *queue,
+		unsigned long pgoff)
+{
+	unsigned long ret;
+
+	mutex_lock(&queue->mutex);
+	ret = vb2_get_unmapped_area(&queue->queue, 0, 0, pgoff, 0);
+	mutex_unlock(&queue->mutex);
+	return ret;
+}
+#endif
+
 unsigned int uvc_queue_poll(struct uvc_video_queue *queue, struct file *file,
 			    poll_table *wait)
 {
@@ -237,36 +250,6 @@ int uvc_queue_allocated(struct uvc_video_queue *queue)
 	return allocated;
 }
 
-#ifndef CONFIG_MMU
-/*
- * Get unmapped area.
- *
- * NO-MMU arch need this function to make mmap() work correctly.
- */
-unsigned long uvc_queue_get_unmapped_area(struct uvc_video_queue *queue,
-		unsigned long pgoff)
-{
-	struct uvc_buffer *buffer;
-	unsigned int i;
-	unsigned long ret;
-
-	mutex_lock(&queue->mutex);
-	for (i = 0; i < queue->count; ++i) {
-		buffer = &queue->buffer[i];
-		if ((buffer->buf.m.offset >> PAGE_SHIFT) == pgoff)
-			break;
-	}
-	if (i == queue->count) {
-		ret = -EINVAL;
-		goto done;
-	}
-	ret = (unsigned long)buf->mem;
-done:
-	mutex_unlock(&queue->mutex);
-	return ret;
-}
-#endif
-
 /*
  * Enable or disable the video buffers queue.
  *
diff --git a/drivers/media/video/uvc/uvc_v4l2.c b/drivers/media/video/uvc/uvc_v4l2.c
index ff2cdddf9bc..759bef8897e 100644
--- a/drivers/media/video/uvc/uvc_v4l2.c
+++ b/drivers/media/video/uvc/uvc_v4l2.c
@@ -25,6 +25,8 @@
 #include <linux/atomic.h>
 
 #include <media/v4l2-common.h>
+#include <media/v4l2-ctrls.h>
+#include <media/v4l2-event.h>
 #include <media/v4l2-ioctl.h>
 
 #include "uvcvideo.h"
@@ -505,6 +507,8 @@ static int uvc_v4l2_open(struct file *file)
 		}
 	}
 
+	v4l2_fh_init(&handle->vfh, stream->vdev);
+	v4l2_fh_add(&handle->vfh);
 	handle->chain = stream->chain;
 	handle->stream = stream;
 	handle->state = UVC_HANDLE_PASSIVE;
@@ -528,6 +532,8 @@ static int uvc_v4l2_release(struct file *file)
 
 	/* Release the file handle. */
 	uvc_dismiss_privileges(handle);
+	v4l2_fh_del(&handle->vfh);
+	v4l2_fh_exit(&handle->vfh);
 	kfree(handle);
 	file->private_data = NULL;
 
@@ -584,7 +590,7 @@ static long uvc_v4l2_do_ioctl(struct file *file, unsigned int cmd, void *arg)
 			return ret;
 
 		ret = uvc_ctrl_get(chain, &xctrl);
-		uvc_ctrl_rollback(chain);
+		uvc_ctrl_rollback(handle);
 		if (ret >= 0)
 			ctrl->value = xctrl.value;
 		break;
@@ -605,10 +611,10 @@ static long uvc_v4l2_do_ioctl(struct file *file, unsigned int cmd, void *arg)
 
 		ret = uvc_ctrl_set(chain, &xctrl);
 		if (ret < 0) {
-			uvc_ctrl_rollback(chain);
+			uvc_ctrl_rollback(handle);
 			return ret;
 		}
-		ret = uvc_ctrl_commit(chain);
+		ret = uvc_ctrl_commit(handle, &xctrl, 1);
 		if (ret == 0)
 			ctrl->value = xctrl.value;
 		break;
@@ -630,13 +636,13 @@ static long uvc_v4l2_do_ioctl(struct file *file, unsigned int cmd, void *arg)
 		for (i = 0; i < ctrls->count; ++ctrl, ++i) {
 			ret = uvc_ctrl_get(chain, ctrl);
 			if (ret < 0) {
-				uvc_ctrl_rollback(chain);
+				uvc_ctrl_rollback(handle);
 				ctrls->error_idx = i;
 				return ret;
 			}
 		}
 		ctrls->error_idx = 0;
-		ret = uvc_ctrl_rollback(chain);
+		ret = uvc_ctrl_rollback(handle);
 		break;
 	}
 
@@ -654,7 +660,7 @@ static long uvc_v4l2_do_ioctl(struct file *file, unsigned int cmd, void *arg)
 		for (i = 0; i < ctrls->count; ++ctrl, ++i) {
 			ret = uvc_ctrl_set(chain, ctrl);
 			if (ret < 0) {
-				uvc_ctrl_rollback(chain);
+				uvc_ctrl_rollback(handle);
 				ctrls->error_idx = i;
 				return ret;
 			}
@@ -663,9 +669,10 @@ static long uvc_v4l2_do_ioctl(struct file *file, unsigned int cmd, void *arg)
 		ctrls->error_idx = 0;
 
 		if (cmd == VIDIOC_S_EXT_CTRLS)
-			ret = uvc_ctrl_commit(chain);
+			ret = uvc_ctrl_commit(handle,
+					      ctrls->controls, ctrls->count);
 		else
-			ret = uvc_ctrl_rollback(chain);
+			ret = uvc_ctrl_rollback(handle);
 		break;
 	}
 
@@ -687,7 +694,7 @@ static long uvc_v4l2_do_ioctl(struct file *file, unsigned int cmd, void *arg)
 					break;
 			}
 			pin = iterm->id;
-		} else if (pin < selector->bNrInPins) {
+		} else if (index < selector->bNrInPins) {
 			pin = selector->baSourceID[index];
 			list_for_each_entry(iterm, &chain->entities, chain) {
 				if (!UVC_ENTITY_IS_ITERM(iterm))
@@ -990,6 +997,26 @@ static long uvc_v4l2_do_ioctl(struct file *file, unsigned int cmd, void *arg)
 		return uvc_video_enable(stream, 0);
 	}
 
+	case VIDIOC_SUBSCRIBE_EVENT:
+	{
+		struct v4l2_event_subscription *sub = arg;
+
+		switch (sub->type) {
+		case V4L2_EVENT_CTRL:
+			return v4l2_event_subscribe(&handle->vfh, sub, 0,
+						    &uvc_ctrl_sub_ev_ops);
+		default:
+			return -EINVAL;
+		}
+	}
+
+	case VIDIOC_UNSUBSCRIBE_EVENT:
+		return v4l2_event_unsubscribe(&handle->vfh, arg);
+
+	case VIDIOC_DQEVENT:
+		return v4l2_event_dequeue(&handle->vfh, arg,
+					  file->f_flags & O_NONBLOCK);
+
 	/* Analog video standards make no sense for digital cameras. */
 	case VIDIOC_ENUMSTD:
 	case VIDIOC_QUERYSTD:
@@ -1097,7 +1124,8 @@ static int uvc_v4l2_put_xu_mapping(const struct uvc_xu_control_mapping *kp,
 	    __put_user(kp->menu_count, &up->menu_count))
 		return -EFAULT;
 
-	__clear_user(up->reserved, sizeof(up->reserved));
+	if (__clear_user(up->reserved, sizeof(up->reserved)))
+		return -EFAULT;
 
 	if (kp->menu_count == 0)
 		return 0;
@@ -1105,8 +1133,6 @@ static int uvc_v4l2_put_xu_mapping(const struct uvc_xu_control_mapping *kp,
 	if (get_user(p, &up->menu_info))
 		return -EFAULT;
 	umenus = compat_ptr(p);
-	if (!access_ok(VERIFY_WRITE, umenus, kp->menu_count * sizeof(*umenus)))
-		return -EFAULT;
 
 	if (copy_in_user(umenus, kmenus, kp->menu_count * sizeof(*umenus)))
 		return -EFAULT;
diff --git a/drivers/media/video/uvc/uvcvideo.h b/drivers/media/video/uvc/uvcvideo.h
index 67f88d85bb1..7c3d082505b 100644
--- a/drivers/media/video/uvc/uvcvideo.h
+++ b/drivers/media/video/uvc/uvcvideo.h
@@ -13,6 +13,8 @@
 #include <linux/videodev2.h>
 #include <media/media-device.h>
 #include <media/v4l2-device.h>
+#include <media/v4l2-event.h>
+#include <media/v4l2-fh.h>
 #include <media/videobuf2-core.h>
 
 /* --------------------------------------------------------------------------
@@ -153,8 +155,7 @@ struct uvc_control_info {
 
 struct uvc_control_mapping {
 	struct list_head list;
-
-	struct uvc_control_info *ctrl;
+	struct list_head ev_subs;
 
 	__u32 id;
 	__u8 name[32];
@@ -169,6 +170,10 @@ struct uvc_control_mapping {
 	struct uvc_menu_info *menu_info;
 	__u32 menu_count;
 
+	__u32 master_id;
+	__s32 master_manual;
+	__u32 slave_ids[2];
+
 	__s32 (*get) (struct uvc_control_mapping *mapping, __u8 query,
 		      const __u8 *data);
 	void (*set) (struct uvc_control_mapping *mapping, __s32 value,
@@ -524,6 +529,7 @@ enum uvc_handle_state {
 };
 
 struct uvc_fh {
+	struct v4l2_fh vfh;
 	struct uvc_video_chain *chain;
 	struct uvc_streaming *stream;
 	enum uvc_handle_state state;
@@ -643,6 +649,8 @@ extern int uvc_status_suspend(struct uvc_device *dev);
 extern int uvc_status_resume(struct uvc_device *dev);
 
 /* Controls */
+extern const struct v4l2_subscribed_event_ops uvc_ctrl_sub_ev_ops;
+
 extern int uvc_query_v4l2_ctrl(struct uvc_video_chain *chain,
 		struct v4l2_queryctrl *v4l2_ctrl);
 extern int uvc_query_v4l2_menu(struct uvc_video_chain *chain,
@@ -655,14 +663,18 @@ extern void uvc_ctrl_cleanup_device(struct uvc_device *dev);
 extern int uvc_ctrl_resume_device(struct uvc_device *dev);
 
 extern int uvc_ctrl_begin(struct uvc_video_chain *chain);
-extern int __uvc_ctrl_commit(struct uvc_video_chain *chain, int rollback);
-static inline int uvc_ctrl_commit(struct uvc_video_chain *chain)
+extern int __uvc_ctrl_commit(struct uvc_fh *handle, int rollback,
+			const struct v4l2_ext_control *xctrls,
+			unsigned int xctrls_count);
+static inline int uvc_ctrl_commit(struct uvc_fh *handle,
+			const struct v4l2_ext_control *xctrls,
+			unsigned int xctrls_count)
 {
-	return __uvc_ctrl_commit(chain, 0);
+	return __uvc_ctrl_commit(handle, 0, xctrls, xctrls_count);
 }
-static inline int uvc_ctrl_rollback(struct uvc_video_chain *chain)
+static inline int uvc_ctrl_rollback(struct uvc_fh *handle)
 {
-	return __uvc_ctrl_commit(chain, 1);
+	return __uvc_ctrl_commit(handle, 1, NULL, 0);
 }
 
 extern int uvc_ctrl_get(struct uvc_video_chain *chain,
diff --git a/drivers/media/video/v4l2-compat-ioctl32.c b/drivers/media/video/v4l2-compat-ioctl32.c
index 2829d256e4b..5327ad3a639 100644
--- a/drivers/media/video/v4l2-compat-ioctl32.c
+++ b/drivers/media/video/v4l2-compat-ioctl32.c
@@ -37,7 +37,7 @@ struct v4l2_clip32 {
 
 struct v4l2_window32 {
 	struct v4l2_rect        w;
-	enum v4l2_field  	field;
+	__u32		  	field;	/* enum v4l2_field */
 	__u32			chromakey;
 	compat_caddr_t		clips; /* actually struct v4l2_clip32 * */
 	__u32			clipcount;
@@ -147,7 +147,7 @@ static inline int put_v4l2_sliced_vbi_format(struct v4l2_sliced_vbi_format *kp,
 }
 
 struct v4l2_format32 {
-	enum v4l2_buf_type type;
+	__u32	type;	/* enum v4l2_buf_type */
 	union {
 		struct v4l2_pix_format	pix;
 		struct v4l2_pix_format_mplane	pix_mp;
@@ -170,7 +170,7 @@ struct v4l2_format32 {
 struct v4l2_create_buffers32 {
 	__u32			index;
 	__u32			count;
-	enum v4l2_memory        memory;
+	__u32			memory;	/* enum v4l2_memory */
 	struct v4l2_format32	format;
 	__u32			reserved[8];
 };
@@ -311,16 +311,16 @@ struct v4l2_plane32 {
 
 struct v4l2_buffer32 {
 	__u32			index;
-	enum v4l2_buf_type      type;
+	__u32			type;	/* enum v4l2_buf_type */
 	__u32			bytesused;
 	__u32			flags;
-	enum v4l2_field		field;
+	__u32			field;	/* enum v4l2_field */
 	struct compat_timeval	timestamp;
 	struct v4l2_timecode	timecode;
 	__u32			sequence;
 
 	/* memory location */
-	enum v4l2_memory        memory;
+	__u32			memory;	/* enum v4l2_memory */
 	union {
 		__u32           offset;
 		compat_long_t   userptr;
@@ -1023,6 +1023,9 @@ long v4l2_compat_ioctl32(struct file *file, unsigned int cmd, unsigned long arg)
 	case VIDIOC_UNSUBSCRIBE_EVENT:
 	case VIDIOC_CREATE_BUFS32:
 	case VIDIOC_PREPARE_BUF32:
+	case VIDIOC_ENUM_DV_TIMINGS:
+	case VIDIOC_QUERY_DV_TIMINGS:
+	case VIDIOC_DV_TIMINGS_CAP:
 		ret = do_video_ioctl(file, cmd, arg);
 		break;
 
diff --git a/drivers/media/video/v4l2-ctrls.c b/drivers/media/video/v4l2-ctrls.c
index 18015c0a8d3..9abd9abd450 100644
--- a/drivers/media/video/v4l2-ctrls.c
+++ b/drivers/media/video/v4l2-ctrls.c
@@ -230,6 +230,19 @@ const char * const *v4l2_ctrl_get_menu(u32 id)
 		"Aperture Priority Mode",
 		NULL
 	};
+	static const char * const camera_exposure_metering[] = {
+		"Average",
+		"Center Weighted",
+		"Spot",
+		NULL
+	};
+	static const char * const camera_auto_focus_range[] = {
+		"Auto",
+		"Normal",
+		"Macro",
+		"Infinity",
+		NULL
+	};
 	static const char * const colorfx[] = {
 		"None",
 		"Black & White",
@@ -241,6 +254,47 @@ const char * const *v4l2_ctrl_get_menu(u32 id)
 		"Grass Green",
 		"Skin Whiten",
 		"Vivid",
+		"Aqua",
+		"Art Freeze",
+		"Silhouette",
+		"Solarization",
+		"Antique",
+		"Set Cb/Cr",
+		NULL
+	};
+	static const char * const auto_n_preset_white_balance[] = {
+		"Manual",
+		"Auto",
+		"Incandescent",
+		"Fluorescent",
+		"Fluorescent H",
+		"Horizon",
+		"Daylight",
+		"Flash",
+		"Cloudy",
+		"Shade",
+		NULL,
+	};
+	static const char * const camera_iso_sensitivity_auto[] = {
+		"Manual",
+		"Auto",
+		NULL
+	};
+	static const char * const scene_mode[] = {
+		"None",
+		"Backlight",
+		"Beach/Snow",
+		"Candle Light",
+		"Dusk/Dawn",
+		"Fall Colors",
+		"Fireworks",
+		"Landscape",
+		"Night",
+		"Party/Indoor",
+		"Portrait",
+		"Sports",
+		"Sunset",
+		"Text",
 		NULL
 	};
 	static const char * const tune_preemphasis[] = {
@@ -410,8 +464,18 @@ const char * const *v4l2_ctrl_get_menu(u32 id)
 		return camera_power_line_frequency;
 	case V4L2_CID_EXPOSURE_AUTO:
 		return camera_exposure_auto;
+	case V4L2_CID_EXPOSURE_METERING:
+		return camera_exposure_metering;
+	case V4L2_CID_AUTO_FOCUS_RANGE:
+		return camera_auto_focus_range;
 	case V4L2_CID_COLORFX:
 		return colorfx;
+	case V4L2_CID_AUTO_N_PRESET_WHITE_BALANCE:
+		return auto_n_preset_white_balance;
+	case V4L2_CID_ISO_SENSITIVITY_AUTO:
+		return camera_iso_sensitivity_auto;
+	case V4L2_CID_SCENE_MODE:
+		return scene_mode;
 	case V4L2_CID_TUNE_PREEMPHASIS:
 		return tune_preemphasis;
 	case V4L2_CID_FLASH_LED_MODE:
@@ -493,6 +557,7 @@ const char *v4l2_ctrl_get_name(u32 id)
 	case V4L2_CID_MIN_BUFFERS_FOR_CAPTURE:	return "Min Number of Capture Buffers";
 	case V4L2_CID_MIN_BUFFERS_FOR_OUTPUT:	return "Min Number of Output Buffers";
 	case V4L2_CID_ALPHA_COMPONENT:		return "Alpha Component";
+	case V4L2_CID_COLORFX_CBCR:		return "Color Effects, CbCr";
 
 	/* MPEG controls */
 	/* Keep the order of the 'case's the same as in videodev2.h! */
@@ -590,13 +655,26 @@ const char *v4l2_ctrl_get_name(u32 id)
 	case V4L2_CID_TILT_ABSOLUTE:		return "Tilt, Absolute";
 	case V4L2_CID_FOCUS_ABSOLUTE:		return "Focus, Absolute";
 	case V4L2_CID_FOCUS_RELATIVE:		return "Focus, Relative";
-	case V4L2_CID_FOCUS_AUTO:		return "Focus, Automatic";
+	case V4L2_CID_FOCUS_AUTO:		return "Focus, Automatic Continuous";
 	case V4L2_CID_ZOOM_ABSOLUTE:		return "Zoom, Absolute";
 	case V4L2_CID_ZOOM_RELATIVE:		return "Zoom, Relative";
 	case V4L2_CID_ZOOM_CONTINUOUS:		return "Zoom, Continuous";
 	case V4L2_CID_PRIVACY:			return "Privacy";
 	case V4L2_CID_IRIS_ABSOLUTE:		return "Iris, Absolute";
 	case V4L2_CID_IRIS_RELATIVE:		return "Iris, Relative";
+	case V4L2_CID_AUTO_EXPOSURE_BIAS:	return "Auto Exposure, Bias";
+	case V4L2_CID_AUTO_N_PRESET_WHITE_BALANCE: return "White Balance, Auto & Preset";
+	case V4L2_CID_WIDE_DYNAMIC_RANGE:	return "Wide Dynamic Range";
+	case V4L2_CID_IMAGE_STABILIZATION:	return "Image Stabilization";
+	case V4L2_CID_ISO_SENSITIVITY:		return "ISO Sensitivity";
+	case V4L2_CID_ISO_SENSITIVITY_AUTO:	return "ISO Sensitivity, Auto";
+	case V4L2_CID_EXPOSURE_METERING:	return "Exposure, Metering Mode";
+	case V4L2_CID_SCENE_MODE:		return "Scene Mode";
+	case V4L2_CID_3A_LOCK:			return "3A Lock";
+	case V4L2_CID_AUTO_FOCUS_START:		return "Auto Focus, Start";
+	case V4L2_CID_AUTO_FOCUS_STOP:		return "Auto Focus, Stop";
+	case V4L2_CID_AUTO_FOCUS_STATUS:	return "Auto Focus, Status";
+	case V4L2_CID_AUTO_FOCUS_RANGE:		return "Auto Focus, Range";
 
 	/* FM Radio Modulator control */
 	/* Keep the order of the 'case's the same as in videodev2.h! */
@@ -644,6 +722,17 @@ const char *v4l2_ctrl_get_name(u32 id)
 	case V4L2_CID_JPEG_COMPRESSION_QUALITY:	return "Compression Quality";
 	case V4L2_CID_JPEG_ACTIVE_MARKER:	return "Active Markers";
 
+	/* Image source controls */
+	case V4L2_CID_IMAGE_SOURCE_CLASS:	return "Image Source Controls";
+	case V4L2_CID_VBLANK:			return "Vertical Blanking";
+	case V4L2_CID_HBLANK:			return "Horizontal Blanking";
+	case V4L2_CID_ANALOGUE_GAIN:		return "Analogue Gain";
+
+	/* Image processing controls */
+	case V4L2_CID_IMAGE_PROC_CLASS:		return "Image Processing Controls";
+	case V4L2_CID_LINK_FREQ:		return "Link Frequency";
+	case V4L2_CID_PIXEL_RATE:		return "Pixel Rate";
+
 	default:
 		return NULL;
 	}
@@ -688,6 +777,8 @@ 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_WIDE_DYNAMIC_RANGE:
+	case V4L2_CID_IMAGE_STABILIZATION:
 		*type = V4L2_CTRL_TYPE_BOOLEAN;
 		*min = 0;
 		*max = *step = 1;
@@ -696,6 +787,8 @@ void v4l2_ctrl_fill(u32 id, const char **name, enum v4l2_ctrl_type *type,
 	case V4L2_CID_TILT_RESET:
 	case V4L2_CID_FLASH_STROBE:
 	case V4L2_CID_FLASH_STROBE_STOP:
+	case V4L2_CID_AUTO_FOCUS_START:
+	case V4L2_CID_AUTO_FOCUS_STOP:
 		*type = V4L2_CTRL_TYPE_BUTTON;
 		*flags |= V4L2_CTRL_FLAG_WRITE_ONLY;
 		*min = *max = *step = *def = 0;
@@ -719,7 +812,9 @@ void v4l2_ctrl_fill(u32 id, const char **name, enum v4l2_ctrl_type *type,
 	case V4L2_CID_MPEG_STREAM_TYPE:
 	case V4L2_CID_MPEG_STREAM_VBI_FMT:
 	case V4L2_CID_EXPOSURE_AUTO:
+	case V4L2_CID_AUTO_FOCUS_RANGE:
 	case V4L2_CID_COLORFX:
+	case V4L2_CID_AUTO_N_PRESET_WHITE_BALANCE:
 	case V4L2_CID_TUNE_PREEMPHASIS:
 	case V4L2_CID_FLASH_LED_MODE:
 	case V4L2_CID_FLASH_STROBE_SOURCE:
@@ -733,18 +828,30 @@ void v4l2_ctrl_fill(u32 id, const char **name, enum v4l2_ctrl_type *type,
 	case V4L2_CID_MPEG_VIDEO_MPEG4_LEVEL:
 	case V4L2_CID_MPEG_VIDEO_MPEG4_PROFILE:
 	case V4L2_CID_JPEG_CHROMA_SUBSAMPLING:
+	case V4L2_CID_ISO_SENSITIVITY_AUTO:
+	case V4L2_CID_EXPOSURE_METERING:
+	case V4L2_CID_SCENE_MODE:
 		*type = V4L2_CTRL_TYPE_MENU;
 		break;
+	case V4L2_CID_LINK_FREQ:
+		*type = V4L2_CTRL_TYPE_INTEGER_MENU;
+		break;
 	case V4L2_CID_RDS_TX_PS_NAME:
 	case V4L2_CID_RDS_TX_RADIO_TEXT:
 		*type = V4L2_CTRL_TYPE_STRING;
 		break;
+	case V4L2_CID_ISO_SENSITIVITY:
+	case V4L2_CID_AUTO_EXPOSURE_BIAS:
+		*type = V4L2_CTRL_TYPE_INTEGER_MENU;
+		break;
 	case V4L2_CID_USER_CLASS:
 	case V4L2_CID_CAMERA_CLASS:
 	case V4L2_CID_MPEG_CLASS:
 	case V4L2_CID_FM_TX_CLASS:
 	case V4L2_CID_FLASH_CLASS:
 	case V4L2_CID_JPEG_CLASS:
+	case V4L2_CID_IMAGE_SOURCE_CLASS:
+	case V4L2_CID_IMAGE_PROC_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;
@@ -759,6 +866,8 @@ void v4l2_ctrl_fill(u32 id, const char **name, enum v4l2_ctrl_type *type,
 		break;
 	case V4L2_CID_FLASH_FAULT:
 	case V4L2_CID_JPEG_ACTIVE_MARKER:
+	case V4L2_CID_3A_LOCK:
+	case V4L2_CID_AUTO_FOCUS_STATUS:
 		*type = V4L2_CTRL_TYPE_BITMASK;
 		break;
 	case V4L2_CID_MIN_BUFFERS_FOR_CAPTURE:
@@ -768,8 +877,12 @@ void v4l2_ctrl_fill(u32 id, const char **name, enum v4l2_ctrl_type *type,
 		break;
 	case V4L2_CID_MPEG_VIDEO_DEC_FRAME:
 	case V4L2_CID_MPEG_VIDEO_DEC_PTS:
+		*flags |= V4L2_CTRL_FLAG_VOLATILE;
+		/* Fall through */
+	case V4L2_CID_PIXEL_RATE:
 		*type = V4L2_CTRL_TYPE_INTEGER64;
-		*flags |= V4L2_CTRL_FLAG_READ_ONLY | V4L2_CTRL_FLAG_VOLATILE;
+		*flags |= V4L2_CTRL_FLAG_READ_ONLY;
+		*min = *max = *step = *def = 0;
 		break;
 	default:
 		*type = V4L2_CTRL_TYPE_INTEGER;
@@ -817,6 +930,7 @@ void v4l2_ctrl_fill(u32 id, const char **name, enum v4l2_ctrl_type *type,
 		*flags |= V4L2_CTRL_FLAG_WRITE_ONLY;
 		break;
 	case V4L2_CID_FLASH_STROBE_STATUS:
+	case V4L2_CID_AUTO_FOCUS_STATUS:
 	case V4L2_CID_FLASH_READY:
 		*flags |= V4L2_CTRL_FLAG_READ_ONLY;
 		break;
@@ -852,7 +966,8 @@ static void fill_event(struct v4l2_event *ev, struct v4l2_ctrl *ctrl, u32 change
 		ev->u.ctrl.value64 = ctrl->cur.val64;
 	ev->u.ctrl.minimum = ctrl->minimum;
 	ev->u.ctrl.maximum = ctrl->maximum;
-	if (ctrl->type == V4L2_CTRL_TYPE_MENU)
+	if (ctrl->type == V4L2_CTRL_TYPE_MENU
+	    || ctrl->type == V4L2_CTRL_TYPE_INTEGER_MENU)
 		ev->u.ctrl.step = 1;
 	else
 		ev->u.ctrl.step = ctrl->step;
@@ -1083,10 +1198,13 @@ static int validate_new_int(const struct v4l2_ctrl *ctrl, s32 *pval)
 		return 0;
 
 	case V4L2_CTRL_TYPE_MENU:
+	case V4L2_CTRL_TYPE_INTEGER_MENU:
 		if (val < ctrl->minimum || val > ctrl->maximum)
 			return -ERANGE;
-		if (ctrl->qmenu[val][0] == '\0' ||
-		    (ctrl->menu_skip_mask & (1 << val)))
+		if (ctrl->menu_skip_mask & (1 << val))
+			return -EINVAL;
+		if (ctrl->type == V4L2_CTRL_TYPE_MENU &&
+		    ctrl->qmenu[val][0] == '\0')
 			return -EINVAL;
 		return 0;
 
@@ -1114,6 +1232,7 @@ static int validate_new(const struct v4l2_ctrl *ctrl, struct v4l2_ext_control *c
 	case V4L2_CTRL_TYPE_INTEGER:
 	case V4L2_CTRL_TYPE_BOOLEAN:
 	case V4L2_CTRL_TYPE_MENU:
+	case V4L2_CTRL_TYPE_INTEGER_MENU:
 	case V4L2_CTRL_TYPE_BITMASK:
 	case V4L2_CTRL_TYPE_BUTTON:
 	case V4L2_CTRL_TYPE_CTRL_CLASS:
@@ -1152,7 +1271,8 @@ static inline int handler_set_err(struct v4l2_ctrl_handler *hdl, int err)
 int v4l2_ctrl_handler_init(struct v4l2_ctrl_handler *hdl,
 			   unsigned nr_of_controls_hint)
 {
-	mutex_init(&hdl->lock);
+	hdl->lock = &hdl->_lock;
+	mutex_init(hdl->lock);
 	INIT_LIST_HEAD(&hdl->ctrls);
 	INIT_LIST_HEAD(&hdl->ctrl_refs);
 	hdl->nr_of_buckets = 1 + nr_of_controls_hint / 8;
@@ -1173,7 +1293,7 @@ void v4l2_ctrl_handler_free(struct v4l2_ctrl_handler *hdl)
 	if (hdl == NULL || hdl->buckets == NULL)
 		return;
 
-	mutex_lock(&hdl->lock);
+	mutex_lock(hdl->lock);
 	/* Free all nodes */
 	list_for_each_entry_safe(ref, next_ref, &hdl->ctrl_refs, node) {
 		list_del(&ref->node);
@@ -1190,7 +1310,7 @@ void v4l2_ctrl_handler_free(struct v4l2_ctrl_handler *hdl)
 	hdl->buckets = NULL;
 	hdl->cached = NULL;
 	hdl->error = 0;
-	mutex_unlock(&hdl->lock);
+	mutex_unlock(hdl->lock);
 }
 EXPORT_SYMBOL(v4l2_ctrl_handler_free);
 
@@ -1255,9 +1375,9 @@ static struct v4l2_ctrl_ref *find_ref_lock(
 	struct v4l2_ctrl_ref *ref = NULL;
 
 	if (hdl) {
-		mutex_lock(&hdl->lock);
+		mutex_lock(hdl->lock);
 		ref = find_ref(hdl, id);
-		mutex_unlock(&hdl->lock);
+		mutex_unlock(hdl->lock);
 	}
 	return ref;
 }
@@ -1304,7 +1424,7 @@ static int handler_new_ref(struct v4l2_ctrl_handler *hdl,
 
 	INIT_LIST_HEAD(&new_ref->node);
 
-	mutex_lock(&hdl->lock);
+	mutex_lock(hdl->lock);
 
 	/* Add immediately at the end of the list if the list is empty, or if
 	   the last element in the list has a lower ID.
@@ -1334,7 +1454,7 @@ insert_in_hash:
 	hdl->buckets[bucket] = new_ref;
 
 unlock:
-	mutex_unlock(&hdl->lock);
+	mutex_unlock(hdl->lock);
 	return 0;
 }
 
@@ -1343,7 +1463,8 @@ static struct v4l2_ctrl *v4l2_ctrl_new(struct v4l2_ctrl_handler *hdl,
 			const struct v4l2_ctrl_ops *ops,
 			u32 id, const char *name, enum v4l2_ctrl_type type,
 			s32 min, s32 max, u32 step, s32 def,
-			u32 flags, const char * const *qmenu, void *priv)
+			u32 flags, const char * const *qmenu,
+			const s64 *qmenu_int, void *priv)
 {
 	struct v4l2_ctrl *ctrl;
 	unsigned sz_extra = 0;
@@ -1356,6 +1477,7 @@ static struct v4l2_ctrl *v4l2_ctrl_new(struct v4l2_ctrl_handler *hdl,
 	    (type == V4L2_CTRL_TYPE_INTEGER && step == 0) ||
 	    (type == V4L2_CTRL_TYPE_BITMASK && max == 0) ||
 	    (type == V4L2_CTRL_TYPE_MENU && qmenu == NULL) ||
+	    (type == V4L2_CTRL_TYPE_INTEGER_MENU && qmenu_int == NULL) ||
 	    (type == V4L2_CTRL_TYPE_STRING && max == 0)) {
 		handler_set_err(hdl, -ERANGE);
 		return NULL;
@@ -1366,6 +1488,7 @@ static struct v4l2_ctrl *v4l2_ctrl_new(struct v4l2_ctrl_handler *hdl,
 	}
 	if ((type == V4L2_CTRL_TYPE_INTEGER ||
 	     type == V4L2_CTRL_TYPE_MENU ||
+	     type == V4L2_CTRL_TYPE_INTEGER_MENU ||
 	     type == V4L2_CTRL_TYPE_BOOLEAN) &&
 	    (def < min || def > max)) {
 		handler_set_err(hdl, -ERANGE);
@@ -1400,7 +1523,10 @@ static struct v4l2_ctrl *v4l2_ctrl_new(struct v4l2_ctrl_handler *hdl,
 	ctrl->minimum = min;
 	ctrl->maximum = max;
 	ctrl->step = step;
-	ctrl->qmenu = qmenu;
+	if (type == V4L2_CTRL_TYPE_MENU)
+		ctrl->qmenu = qmenu;
+	else if (type == V4L2_CTRL_TYPE_INTEGER_MENU)
+		ctrl->qmenu_int = qmenu_int;
 	ctrl->priv = priv;
 	ctrl->cur.val = ctrl->val = ctrl->default_value = def;
 
@@ -1414,9 +1540,9 @@ static struct v4l2_ctrl *v4l2_ctrl_new(struct v4l2_ctrl_handler *hdl,
 		kfree(ctrl);
 		return NULL;
 	}
-	mutex_lock(&hdl->lock);
+	mutex_lock(hdl->lock);
 	list_add_tail(&ctrl->node, &hdl->ctrls);
-	mutex_unlock(&hdl->lock);
+	mutex_unlock(hdl->lock);
 	return ctrl;
 }
 
@@ -1427,6 +1553,7 @@ struct v4l2_ctrl *v4l2_ctrl_new_custom(struct v4l2_ctrl_handler *hdl,
 	struct v4l2_ctrl *ctrl;
 	const char *name = cfg->name;
 	const char * const *qmenu = cfg->qmenu;
+	const s64 *qmenu_int = cfg->qmenu_int;
 	enum v4l2_ctrl_type type = cfg->type;
 	u32 flags = cfg->flags;
 	s32 min = cfg->min;
@@ -1438,18 +1565,24 @@ struct v4l2_ctrl *v4l2_ctrl_new_custom(struct v4l2_ctrl_handler *hdl,
 		v4l2_ctrl_fill(cfg->id, &name, &type, &min, &max, &step,
 								&def, &flags);
 
-	is_menu = (cfg->type == V4L2_CTRL_TYPE_MENU);
+	is_menu = (cfg->type == V4L2_CTRL_TYPE_MENU ||
+		   cfg->type == V4L2_CTRL_TYPE_INTEGER_MENU);
 	if (is_menu)
 		WARN_ON(step);
 	else
 		WARN_ON(cfg->menu_skip_mask);
-	if (is_menu && qmenu == NULL)
+	if (cfg->type == V4L2_CTRL_TYPE_MENU && qmenu == NULL)
 		qmenu = v4l2_ctrl_get_menu(cfg->id);
+	else if (cfg->type == V4L2_CTRL_TYPE_INTEGER_MENU &&
+		 qmenu_int == NULL) {
+		handler_set_err(hdl, -EINVAL);
+		return NULL;
+	}
 
 	ctrl = v4l2_ctrl_new(hdl, cfg->ops, cfg->id, name,
 			type, min, max,
 			is_menu ? cfg->menu_skip_mask : step,
-			def, flags, qmenu, priv);
+			def, flags, qmenu, qmenu_int, priv);
 	if (ctrl)
 		ctrl->is_private = cfg->is_private;
 	return ctrl;
@@ -1466,12 +1599,13 @@ struct v4l2_ctrl *v4l2_ctrl_new_std(struct v4l2_ctrl_handler *hdl,
 	u32 flags;
 
 	v4l2_ctrl_fill(id, &name, &type, &min, &max, &step, &def, &flags);
-	if (type == V4L2_CTRL_TYPE_MENU) {
+	if (type == V4L2_CTRL_TYPE_MENU
+	    || type == V4L2_CTRL_TYPE_INTEGER_MENU) {
 		handler_set_err(hdl, -EINVAL);
 		return NULL;
 	}
 	return v4l2_ctrl_new(hdl, ops, id, name, type,
-				    min, max, step, def, flags, NULL, NULL);
+			     min, max, step, def, flags, NULL, NULL, NULL);
 }
 EXPORT_SYMBOL(v4l2_ctrl_new_std);
 
@@ -1493,10 +1627,31 @@ struct v4l2_ctrl *v4l2_ctrl_new_std_menu(struct v4l2_ctrl_handler *hdl,
 		return NULL;
 	}
 	return v4l2_ctrl_new(hdl, ops, id, name, type,
-				    0, max, mask, def, flags, qmenu, NULL);
+			     0, max, mask, def, flags, qmenu, NULL, NULL);
 }
 EXPORT_SYMBOL(v4l2_ctrl_new_std_menu);
 
+/* Helper function for standard integer menu controls */
+struct v4l2_ctrl *v4l2_ctrl_new_int_menu(struct v4l2_ctrl_handler *hdl,
+			const struct v4l2_ctrl_ops *ops,
+			u32 id, s32 max, s32 def, const s64 *qmenu_int)
+{
+	const char *name;
+	enum v4l2_ctrl_type type;
+	s32 min;
+	s32 step;
+	u32 flags;
+
+	v4l2_ctrl_fill(id, &name, &type, &min, &max, &step, &def, &flags);
+	if (type != V4L2_CTRL_TYPE_INTEGER_MENU) {
+		handler_set_err(hdl, -EINVAL);
+		return NULL;
+	}
+	return v4l2_ctrl_new(hdl, ops, id, name, type,
+			     0, max, 0, def, flags, NULL, qmenu_int, NULL);
+}
+EXPORT_SYMBOL(v4l2_ctrl_new_int_menu);
+
 /* Add a control from another handler to this handler */
 struct v4l2_ctrl *v4l2_ctrl_add_ctrl(struct v4l2_ctrl_handler *hdl,
 					  struct v4l2_ctrl *ctrl)
@@ -1525,7 +1680,7 @@ int v4l2_ctrl_add_handler(struct v4l2_ctrl_handler *hdl,
 		return 0;
 	if (hdl->error)
 		return hdl->error;
-	mutex_lock(&add->lock);
+	mutex_lock(add->lock);
 	list_for_each_entry(ref, &add->ctrl_refs, node) {
 		struct v4l2_ctrl *ctrl = ref->ctrl;
 
@@ -1539,7 +1694,7 @@ int v4l2_ctrl_add_handler(struct v4l2_ctrl_handler *hdl,
 		if (ret)
 			break;
 	}
-	mutex_unlock(&add->lock);
+	mutex_unlock(add->lock);
 	return ret;
 }
 EXPORT_SYMBOL(v4l2_ctrl_add_handler);
@@ -1659,6 +1814,9 @@ static void log_ctrl(const struct v4l2_ctrl *ctrl,
 	case V4L2_CTRL_TYPE_MENU:
 		printk(KERN_CONT "%s", ctrl->qmenu[ctrl->cur.val]);
 		break;
+	case V4L2_CTRL_TYPE_INTEGER_MENU:
+		printk(KERN_CONT "%lld", ctrl->qmenu_int[ctrl->cur.val]);
+		break;
 	case V4L2_CTRL_TYPE_BITMASK:
 		printk(KERN_CONT "0x%08x", ctrl->cur.val);
 		break;
@@ -1700,11 +1858,11 @@ void v4l2_ctrl_handler_log_status(struct v4l2_ctrl_handler *hdl,
 	len = strlen(prefix);
 	if (len && prefix[len - 1] != ' ')
 		colon = ": ";
-	mutex_lock(&hdl->lock);
+	mutex_lock(hdl->lock);
 	list_for_each_entry(ctrl, &hdl->ctrls, node)
 		if (!(ctrl->flags & V4L2_CTRL_FLAG_DISABLED))
 			log_ctrl(ctrl, prefix, colon);
-	mutex_unlock(&hdl->lock);
+	mutex_unlock(hdl->lock);
 }
 EXPORT_SYMBOL(v4l2_ctrl_handler_log_status);
 
@@ -1716,7 +1874,7 @@ int v4l2_ctrl_handler_setup(struct v4l2_ctrl_handler *hdl)
 
 	if (hdl == NULL)
 		return 0;
-	mutex_lock(&hdl->lock);
+	mutex_lock(hdl->lock);
 	list_for_each_entry(ctrl, &hdl->ctrls, node)
 		ctrl->done = false;
 
@@ -1741,7 +1899,7 @@ int v4l2_ctrl_handler_setup(struct v4l2_ctrl_handler *hdl)
 		if (ret)
 			break;
 	}
-	mutex_unlock(&hdl->lock);
+	mutex_unlock(hdl->lock);
 	return ret;
 }
 EXPORT_SYMBOL(v4l2_ctrl_handler_setup);
@@ -1756,7 +1914,7 @@ int v4l2_queryctrl(struct v4l2_ctrl_handler *hdl, struct v4l2_queryctrl *qc)
 	if (hdl == NULL)
 		return -EINVAL;
 
-	mutex_lock(&hdl->lock);
+	mutex_lock(hdl->lock);
 
 	/* Try to find it */
 	ref = find_ref(hdl, id);
@@ -1781,7 +1939,7 @@ int v4l2_queryctrl(struct v4l2_ctrl_handler *hdl, struct v4l2_queryctrl *qc)
 					break;
 		}
 	}
-	mutex_unlock(&hdl->lock);
+	mutex_unlock(hdl->lock);
 	if (!ref)
 		return -EINVAL;
 
@@ -1795,7 +1953,8 @@ int v4l2_queryctrl(struct v4l2_ctrl_handler *hdl, struct v4l2_queryctrl *qc)
 	qc->minimum = ctrl->minimum;
 	qc->maximum = ctrl->maximum;
 	qc->default_value = ctrl->default_value;
-	if (ctrl->type == V4L2_CTRL_TYPE_MENU)
+	if (ctrl->type == V4L2_CTRL_TYPE_MENU
+	    || ctrl->type == V4L2_CTRL_TYPE_INTEGER_MENU)
 		qc->step = 1;
 	else
 		qc->step = ctrl->step;
@@ -1825,16 +1984,33 @@ int v4l2_querymenu(struct v4l2_ctrl_handler *hdl, struct v4l2_querymenu *qm)
 
 	qm->reserved = 0;
 	/* Sanity checks */
-	if (ctrl->qmenu == NULL ||
-	    i < ctrl->minimum || i > ctrl->maximum)
+	switch (ctrl->type) {
+	case V4L2_CTRL_TYPE_MENU:
+		if (ctrl->qmenu == NULL)
+			return -EINVAL;
+		break;
+	case V4L2_CTRL_TYPE_INTEGER_MENU:
+		if (ctrl->qmenu_int == NULL)
+			return -EINVAL;
+		break;
+	default:
 		return -EINVAL;
+	}
+
+	if (i < ctrl->minimum || i > ctrl->maximum)
+		return -EINVAL;
+
 	/* Use mask to see if this menu item should be skipped */
 	if (ctrl->menu_skip_mask & (1 << i))
 		return -EINVAL;
 	/* Empty menu items should also be skipped */
-	if (ctrl->qmenu[i] == NULL || ctrl->qmenu[i][0] == '\0')
-		return -EINVAL;
-	strlcpy(qm->name, ctrl->qmenu[i], sizeof(qm->name));
+	if (ctrl->type == V4L2_CTRL_TYPE_MENU) {
+		if (ctrl->qmenu[i] == NULL || ctrl->qmenu[i][0] == '\0')
+			return -EINVAL;
+		strlcpy(qm->name, ctrl->qmenu[i], sizeof(qm->name));
+	} else {
+		qm->value = ctrl->qmenu_int[i];
+	}
 	return 0;
 }
 EXPORT_SYMBOL(v4l2_querymenu);
@@ -1940,7 +2116,7 @@ static int prepare_ext_ctrls(struct v4l2_ctrl_handler *hdl,
 	   belong to the same cluster. */
 
 	/* This has to be done with the handler lock taken. */
-	mutex_lock(&hdl->lock);
+	mutex_lock(hdl->lock);
 
 	/* First zero the helper field in the master control references */
 	for (i = 0; i < cs->count; i++)
@@ -1962,7 +2138,7 @@ static int prepare_ext_ctrls(struct v4l2_ctrl_handler *hdl,
 		/* Point the mref helper to the current helper struct. */
 		mref->helper = h;
 	}
-	mutex_unlock(&hdl->lock);
+	mutex_unlock(hdl->lock);
 	return 0;
 }
 
@@ -1996,7 +2172,8 @@ int v4l2_g_ext_ctrls(struct v4l2_ctrl_handler *hdl, struct v4l2_ext_controls *cs
 		return class_check(hdl, cs->ctrl_class);
 
 	if (cs->count > ARRAY_SIZE(helper)) {
-		helpers = kmalloc(sizeof(helper[0]) * cs->count, GFP_KERNEL);
+		helpers = kmalloc_array(cs->count, sizeof(helper[0]),
+					GFP_KERNEL);
 		if (helpers == NULL)
 			return -ENOMEM;
 	}
@@ -2218,7 +2395,8 @@ static int try_set_ext_ctrls(struct v4l2_fh *fh, struct v4l2_ctrl_handler *hdl,
 		return class_check(hdl, cs->ctrl_class);
 
 	if (cs->count > ARRAY_SIZE(helper)) {
-		helpers = kmalloc(sizeof(helper[0]) * cs->count, GFP_KERNEL);
+		helpers = kmalloc_array(cs->count, sizeof(helper[0]),
+					GFP_KERNEL);
 		if (!helpers)
 			return -ENOMEM;
 	}
@@ -2381,9 +2559,13 @@ int v4l2_ctrl_s_ctrl(struct v4l2_ctrl *ctrl, s32 val)
 }
 EXPORT_SYMBOL(v4l2_ctrl_s_ctrl);
 
-void v4l2_ctrl_add_event(struct v4l2_ctrl *ctrl,
-				struct v4l2_subscribed_event *sev)
+static int v4l2_ctrl_add_event(struct v4l2_subscribed_event *sev, unsigned elems)
 {
+	struct v4l2_ctrl *ctrl = v4l2_ctrl_find(sev->fh->ctrl_handler, sev->id);
+
+	if (ctrl == NULL)
+		return -EINVAL;
+
 	v4l2_ctrl_lock(ctrl);
 	list_add_tail(&sev->node, &ctrl->ev_subs);
 	if (ctrl->type != V4L2_CTRL_TYPE_CTRL_CLASS &&
@@ -2394,20 +2576,46 @@ void v4l2_ctrl_add_event(struct v4l2_ctrl *ctrl,
 		if (!(ctrl->flags & V4L2_CTRL_FLAG_WRITE_ONLY))
 			changes |= V4L2_EVENT_CTRL_CH_VALUE;
 		fill_event(&ev, ctrl, changes);
+		/* Mark the queue as active, allowing this initial
+		   event to be accepted. */
+		sev->elems = elems;
 		v4l2_event_queue_fh(sev->fh, &ev);
 	}
 	v4l2_ctrl_unlock(ctrl);
+	return 0;
 }
-EXPORT_SYMBOL(v4l2_ctrl_add_event);
 
-void v4l2_ctrl_del_event(struct v4l2_ctrl *ctrl,
-				struct v4l2_subscribed_event *sev)
+static void v4l2_ctrl_del_event(struct v4l2_subscribed_event *sev)
 {
+	struct v4l2_ctrl *ctrl = v4l2_ctrl_find(sev->fh->ctrl_handler, sev->id);
+
 	v4l2_ctrl_lock(ctrl);
 	list_del(&sev->node);
 	v4l2_ctrl_unlock(ctrl);
 }
-EXPORT_SYMBOL(v4l2_ctrl_del_event);
+
+void v4l2_ctrl_replace(struct v4l2_event *old, const struct v4l2_event *new)
+{
+	u32 old_changes = old->u.ctrl.changes;
+
+	old->u.ctrl = new->u.ctrl;
+	old->u.ctrl.changes |= old_changes;
+}
+EXPORT_SYMBOL(v4l2_ctrl_replace);
+
+void v4l2_ctrl_merge(const struct v4l2_event *old, struct v4l2_event *new)
+{
+	new->u.ctrl.changes |= old->u.ctrl.changes;
+}
+EXPORT_SYMBOL(v4l2_ctrl_merge);
+
+const struct v4l2_subscribed_event_ops v4l2_ctrl_sub_ev_ops = {
+	.add = v4l2_ctrl_add_event,
+	.del = v4l2_ctrl_del_event,
+	.replace = v4l2_ctrl_replace,
+	.merge = v4l2_ctrl_merge,
+};
+EXPORT_SYMBOL(v4l2_ctrl_sub_ev_ops);
 
 int v4l2_ctrl_log_status(struct file *file, void *fh)
 {
@@ -2425,7 +2633,7 @@ int v4l2_ctrl_subscribe_event(struct v4l2_fh *fh,
 				struct v4l2_event_subscription *sub)
 {
 	if (sub->type == V4L2_EVENT_CTRL)
-		return v4l2_event_subscribe(fh, sub, 0);
+		return v4l2_event_subscribe(fh, sub, 0, &v4l2_ctrl_sub_ev_ops);
 	return -EINVAL;
 }
 EXPORT_SYMBOL(v4l2_ctrl_subscribe_event);
diff --git a/drivers/media/video/v4l2-dev.c b/drivers/media/video/v4l2-dev.c
index 70bec548d90..5ccbd4629f9 100644
--- a/drivers/media/video/v4l2-dev.c
+++ b/drivers/media/video/v4l2-dev.c
@@ -274,11 +274,12 @@ static ssize_t v4l2_read(struct file *filp, char __user *buf,
 
 	if (!vdev->fops->read)
 		return -EINVAL;
-	if (vdev->lock && mutex_lock_interruptible(vdev->lock))
+	if (test_bit(V4L2_FL_LOCK_ALL_FOPS, &vdev->flags) &&
+	    mutex_lock_interruptible(vdev->lock))
 		return -ERESTARTSYS;
 	if (video_is_registered(vdev))
 		ret = vdev->fops->read(filp, buf, sz, off);
-	if (vdev->lock)
+	if (test_bit(V4L2_FL_LOCK_ALL_FOPS, &vdev->flags))
 		mutex_unlock(vdev->lock);
 	return ret;
 }
@@ -291,11 +292,12 @@ static ssize_t v4l2_write(struct file *filp, const char __user *buf,
 
 	if (!vdev->fops->write)
 		return -EINVAL;
-	if (vdev->lock && mutex_lock_interruptible(vdev->lock))
+	if (test_bit(V4L2_FL_LOCK_ALL_FOPS, &vdev->flags) &&
+	    mutex_lock_interruptible(vdev->lock))
 		return -ERESTARTSYS;
 	if (video_is_registered(vdev))
 		ret = vdev->fops->write(filp, buf, sz, off);
-	if (vdev->lock)
+	if (test_bit(V4L2_FL_LOCK_ALL_FOPS, &vdev->flags))
 		mutex_unlock(vdev->lock);
 	return ret;
 }
@@ -307,11 +309,11 @@ static unsigned int v4l2_poll(struct file *filp, struct poll_table_struct *poll)
 
 	if (!vdev->fops->poll)
 		return DEFAULT_POLLMASK;
-	if (vdev->lock)
+	if (test_bit(V4L2_FL_LOCK_ALL_FOPS, &vdev->flags))
 		mutex_lock(vdev->lock);
 	if (video_is_registered(vdev))
 		ret = vdev->fops->poll(filp, poll);
-	if (vdev->lock)
+	if (test_bit(V4L2_FL_LOCK_ALL_FOPS, &vdev->flags))
 		mutex_unlock(vdev->lock);
 	return ret;
 }
@@ -322,11 +324,19 @@ static long v4l2_ioctl(struct file *filp, unsigned int cmd, unsigned long arg)
 	int ret = -ENODEV;
 
 	if (vdev->fops->unlocked_ioctl) {
-		if (vdev->lock && mutex_lock_interruptible(vdev->lock))
-			return -ERESTARTSYS;
+		bool locked = false;
+
+		if (vdev->lock) {
+			/* always lock unless the cmd is marked as "don't use lock" */
+			locked = !v4l2_is_known_ioctl(cmd) ||
+				 !test_bit(_IOC_NR(cmd), vdev->disable_locking);
+
+			if (locked && mutex_lock_interruptible(vdev->lock))
+				return -ERESTARTSYS;
+		}
 		if (video_is_registered(vdev))
 			ret = vdev->fops->unlocked_ioctl(filp, cmd, arg);
-		if (vdev->lock)
+		if (locked)
 			mutex_unlock(vdev->lock);
 	} else if (vdev->fops->ioctl) {
 		/* This code path is a replacement for the BKL. It is a major
@@ -391,11 +401,12 @@ static int v4l2_mmap(struct file *filp, struct vm_area_struct *vm)
 
 	if (!vdev->fops->mmap)
 		return ret;
-	if (vdev->lock && mutex_lock_interruptible(vdev->lock))
+	if (test_bit(V4L2_FL_LOCK_ALL_FOPS, &vdev->flags) &&
+	    mutex_lock_interruptible(vdev->lock))
 		return -ERESTARTSYS;
 	if (video_is_registered(vdev))
 		ret = vdev->fops->mmap(filp, vm);
-	if (vdev->lock)
+	if (test_bit(V4L2_FL_LOCK_ALL_FOPS, &vdev->flags))
 		mutex_unlock(vdev->lock);
 	return ret;
 }
@@ -418,7 +429,8 @@ static int v4l2_open(struct inode *inode, struct file *filp)
 	video_get(vdev);
 	mutex_unlock(&videodev_lock);
 	if (vdev->fops->open) {
-		if (vdev->lock && mutex_lock_interruptible(vdev->lock)) {
+		if (test_bit(V4L2_FL_LOCK_ALL_FOPS, &vdev->flags) &&
+		    mutex_lock_interruptible(vdev->lock)) {
 			ret = -ERESTARTSYS;
 			goto err;
 		}
@@ -426,7 +438,7 @@ static int v4l2_open(struct inode *inode, struct file *filp)
 			ret = vdev->fops->open(filp);
 		else
 			ret = -ENODEV;
-		if (vdev->lock)
+		if (test_bit(V4L2_FL_LOCK_ALL_FOPS, &vdev->flags))
 			mutex_unlock(vdev->lock);
 	}
 
@@ -444,10 +456,10 @@ static int v4l2_release(struct inode *inode, struct file *filp)
 	int ret = 0;
 
 	if (vdev->fops->release) {
-		if (vdev->lock)
+		if (test_bit(V4L2_FL_LOCK_ALL_FOPS, &vdev->flags))
 			mutex_lock(vdev->lock);
 		vdev->fops->release(filp);
-		if (vdev->lock)
+		if (test_bit(V4L2_FL_LOCK_ALL_FOPS, &vdev->flags))
 			mutex_unlock(vdev->lock);
 	}
 	/* decrease the refcount unconditionally since the release()
@@ -508,6 +520,175 @@ static int get_index(struct video_device *vdev)
 	return find_first_zero_bit(used, VIDEO_NUM_DEVICES);
 }
 
+#define SET_VALID_IOCTL(ops, cmd, op)			\
+	if (ops->op)					\
+		set_bit(_IOC_NR(cmd), valid_ioctls)
+
+/* This determines which ioctls are actually implemented in the driver.
+   It's a one-time thing which simplifies video_ioctl2 as it can just do
+   a bit test.
+
+   Note that drivers can override this by setting bits to 1 in
+   vdev->valid_ioctls. If an ioctl is marked as 1 when this function is
+   called, then that ioctl will actually be marked as unimplemented.
+
+   It does that by first setting up the local valid_ioctls bitmap, and
+   at the end do a:
+
+   vdev->valid_ioctls = valid_ioctls & ~(vdev->valid_ioctls)
+ */
+static void determine_valid_ioctls(struct video_device *vdev)
+{
+	DECLARE_BITMAP(valid_ioctls, BASE_VIDIOC_PRIVATE);
+	const struct v4l2_ioctl_ops *ops = vdev->ioctl_ops;
+
+	bitmap_zero(valid_ioctls, BASE_VIDIOC_PRIVATE);
+
+	SET_VALID_IOCTL(ops, VIDIOC_QUERYCAP, vidioc_querycap);
+	if (ops->vidioc_g_priority ||
+			test_bit(V4L2_FL_USE_FH_PRIO, &vdev->flags))
+		set_bit(_IOC_NR(VIDIOC_G_PRIORITY), valid_ioctls);
+	if (ops->vidioc_s_priority ||
+			test_bit(V4L2_FL_USE_FH_PRIO, &vdev->flags))
+		set_bit(_IOC_NR(VIDIOC_S_PRIORITY), valid_ioctls);
+	if (ops->vidioc_enum_fmt_vid_cap ||
+	    ops->vidioc_enum_fmt_vid_out ||
+	    ops->vidioc_enum_fmt_vid_cap_mplane ||
+	    ops->vidioc_enum_fmt_vid_out_mplane ||
+	    ops->vidioc_enum_fmt_vid_overlay ||
+	    ops->vidioc_enum_fmt_type_private)
+		set_bit(_IOC_NR(VIDIOC_ENUM_FMT), valid_ioctls);
+	if (ops->vidioc_g_fmt_vid_cap ||
+	    ops->vidioc_g_fmt_vid_out ||
+	    ops->vidioc_g_fmt_vid_cap_mplane ||
+	    ops->vidioc_g_fmt_vid_out_mplane ||
+	    ops->vidioc_g_fmt_vid_overlay ||
+	    ops->vidioc_g_fmt_vbi_cap ||
+	    ops->vidioc_g_fmt_vid_out_overlay ||
+	    ops->vidioc_g_fmt_vbi_out ||
+	    ops->vidioc_g_fmt_sliced_vbi_cap ||
+	    ops->vidioc_g_fmt_sliced_vbi_out ||
+	    ops->vidioc_g_fmt_type_private)
+		set_bit(_IOC_NR(VIDIOC_G_FMT), valid_ioctls);
+	if (ops->vidioc_s_fmt_vid_cap ||
+	    ops->vidioc_s_fmt_vid_out ||
+	    ops->vidioc_s_fmt_vid_cap_mplane ||
+	    ops->vidioc_s_fmt_vid_out_mplane ||
+	    ops->vidioc_s_fmt_vid_overlay ||
+	    ops->vidioc_s_fmt_vbi_cap ||
+	    ops->vidioc_s_fmt_vid_out_overlay ||
+	    ops->vidioc_s_fmt_vbi_out ||
+	    ops->vidioc_s_fmt_sliced_vbi_cap ||
+	    ops->vidioc_s_fmt_sliced_vbi_out ||
+	    ops->vidioc_s_fmt_type_private)
+		set_bit(_IOC_NR(VIDIOC_S_FMT), valid_ioctls);
+	if (ops->vidioc_try_fmt_vid_cap ||
+	    ops->vidioc_try_fmt_vid_out ||
+	    ops->vidioc_try_fmt_vid_cap_mplane ||
+	    ops->vidioc_try_fmt_vid_out_mplane ||
+	    ops->vidioc_try_fmt_vid_overlay ||
+	    ops->vidioc_try_fmt_vbi_cap ||
+	    ops->vidioc_try_fmt_vid_out_overlay ||
+	    ops->vidioc_try_fmt_vbi_out ||
+	    ops->vidioc_try_fmt_sliced_vbi_cap ||
+	    ops->vidioc_try_fmt_sliced_vbi_out ||
+	    ops->vidioc_try_fmt_type_private)
+		set_bit(_IOC_NR(VIDIOC_TRY_FMT), valid_ioctls);
+	SET_VALID_IOCTL(ops, VIDIOC_REQBUFS, vidioc_reqbufs);
+	SET_VALID_IOCTL(ops, VIDIOC_QUERYBUF, vidioc_querybuf);
+	SET_VALID_IOCTL(ops, VIDIOC_QBUF, vidioc_qbuf);
+	SET_VALID_IOCTL(ops, VIDIOC_DQBUF, vidioc_dqbuf);
+	SET_VALID_IOCTL(ops, VIDIOC_OVERLAY, vidioc_overlay);
+	SET_VALID_IOCTL(ops, VIDIOC_G_FBUF, vidioc_g_fbuf);
+	SET_VALID_IOCTL(ops, VIDIOC_S_FBUF, vidioc_s_fbuf);
+	SET_VALID_IOCTL(ops, VIDIOC_STREAMON, vidioc_streamon);
+	SET_VALID_IOCTL(ops, VIDIOC_STREAMOFF, vidioc_streamoff);
+	if (vdev->tvnorms)
+		set_bit(_IOC_NR(VIDIOC_ENUMSTD), valid_ioctls);
+	if (ops->vidioc_g_std || vdev->current_norm)
+		set_bit(_IOC_NR(VIDIOC_G_STD), valid_ioctls);
+	SET_VALID_IOCTL(ops, VIDIOC_S_STD, vidioc_s_std);
+	SET_VALID_IOCTL(ops, VIDIOC_QUERYSTD, vidioc_querystd);
+	SET_VALID_IOCTL(ops, VIDIOC_ENUMINPUT, vidioc_enum_input);
+	SET_VALID_IOCTL(ops, VIDIOC_G_INPUT, vidioc_g_input);
+	SET_VALID_IOCTL(ops, VIDIOC_S_INPUT, vidioc_s_input);
+	SET_VALID_IOCTL(ops, VIDIOC_ENUMOUTPUT, vidioc_enum_output);
+	SET_VALID_IOCTL(ops, VIDIOC_G_OUTPUT, vidioc_g_output);
+	SET_VALID_IOCTL(ops, VIDIOC_S_OUTPUT, vidioc_s_output);
+	/* Note: the control handler can also be passed through the filehandle,
+	   and that can't be tested here. If the bit for these control ioctls
+	   is set, then the ioctl is valid. But if it is 0, then it can still
+	   be valid if the filehandle passed the control handler. */
+	if (vdev->ctrl_handler || ops->vidioc_queryctrl)
+		set_bit(_IOC_NR(VIDIOC_QUERYCTRL), valid_ioctls);
+	if (vdev->ctrl_handler || ops->vidioc_g_ctrl || ops->vidioc_g_ext_ctrls)
+		set_bit(_IOC_NR(VIDIOC_G_CTRL), valid_ioctls);
+	if (vdev->ctrl_handler || ops->vidioc_s_ctrl || ops->vidioc_s_ext_ctrls)
+		set_bit(_IOC_NR(VIDIOC_S_CTRL), valid_ioctls);
+	if (vdev->ctrl_handler || ops->vidioc_g_ext_ctrls)
+		set_bit(_IOC_NR(VIDIOC_G_EXT_CTRLS), valid_ioctls);
+	if (vdev->ctrl_handler || ops->vidioc_s_ext_ctrls)
+		set_bit(_IOC_NR(VIDIOC_S_EXT_CTRLS), valid_ioctls);
+	if (vdev->ctrl_handler || ops->vidioc_try_ext_ctrls)
+		set_bit(_IOC_NR(VIDIOC_TRY_EXT_CTRLS), valid_ioctls);
+	if (vdev->ctrl_handler || ops->vidioc_querymenu)
+		set_bit(_IOC_NR(VIDIOC_QUERYMENU), valid_ioctls);
+	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_ENUMAUDOUT, vidioc_enumaudout);
+	SET_VALID_IOCTL(ops, VIDIOC_G_AUDOUT, vidioc_g_audout);
+	SET_VALID_IOCTL(ops, VIDIOC_S_AUDOUT, vidioc_s_audout);
+	SET_VALID_IOCTL(ops, VIDIOC_G_MODULATOR, vidioc_g_modulator);
+	SET_VALID_IOCTL(ops, VIDIOC_S_MODULATOR, vidioc_s_modulator);
+	if (ops->vidioc_g_crop || ops->vidioc_g_selection)
+		set_bit(_IOC_NR(VIDIOC_G_CROP), valid_ioctls);
+	if (ops->vidioc_s_crop || ops->vidioc_s_selection)
+		set_bit(_IOC_NR(VIDIOC_S_CROP), valid_ioctls);
+	SET_VALID_IOCTL(ops, VIDIOC_G_SELECTION, vidioc_g_selection);
+	SET_VALID_IOCTL(ops, VIDIOC_S_SELECTION, vidioc_s_selection);
+	if (ops->vidioc_cropcap || ops->vidioc_g_selection)
+		set_bit(_IOC_NR(VIDIOC_CROPCAP), valid_ioctls);
+	SET_VALID_IOCTL(ops, VIDIOC_G_JPEGCOMP, vidioc_g_jpegcomp);
+	SET_VALID_IOCTL(ops, VIDIOC_S_JPEGCOMP, vidioc_s_jpegcomp);
+	SET_VALID_IOCTL(ops, VIDIOC_G_ENC_INDEX, vidioc_g_enc_index);
+	SET_VALID_IOCTL(ops, VIDIOC_ENCODER_CMD, vidioc_encoder_cmd);
+	SET_VALID_IOCTL(ops, VIDIOC_TRY_ENCODER_CMD, vidioc_try_encoder_cmd);
+	SET_VALID_IOCTL(ops, VIDIOC_DECODER_CMD, vidioc_decoder_cmd);
+	SET_VALID_IOCTL(ops, VIDIOC_TRY_DECODER_CMD, vidioc_try_decoder_cmd);
+	if (ops->vidioc_g_parm || 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_G_TUNER, vidioc_g_tuner);
+	SET_VALID_IOCTL(ops, VIDIOC_S_TUNER, vidioc_s_tuner);
+	SET_VALID_IOCTL(ops, VIDIOC_G_FREQUENCY, vidioc_g_frequency);
+	SET_VALID_IOCTL(ops, VIDIOC_S_FREQUENCY, vidioc_s_frequency);
+	SET_VALID_IOCTL(ops, VIDIOC_G_SLICED_VBI_CAP, vidioc_g_sliced_vbi_cap);
+	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);
+#endif
+	SET_VALID_IOCTL(ops, VIDIOC_DBG_G_CHIP_IDENT, vidioc_g_chip_ident);
+	SET_VALID_IOCTL(ops, VIDIOC_S_HW_FREQ_SEEK, vidioc_s_hw_freq_seek);
+	SET_VALID_IOCTL(ops, VIDIOC_ENUM_FRAMESIZES, vidioc_enum_framesizes);
+	SET_VALID_IOCTL(ops, VIDIOC_ENUM_FRAMEINTERVALS, vidioc_enum_frameintervals);
+	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_QUERY_DV_PRESET, vidioc_query_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);
+	/* yes, really vidioc_subscribe_event */
+	SET_VALID_IOCTL(ops, VIDIOC_DQEVENT, vidioc_subscribe_event);
+	SET_VALID_IOCTL(ops, VIDIOC_SUBSCRIBE_EVENT, vidioc_subscribe_event);
+	SET_VALID_IOCTL(ops, VIDIOC_UNSUBSCRIBE_EVENT, vidioc_unsubscribe_event);
+	SET_VALID_IOCTL(ops, VIDIOC_CREATE_BUFS, vidioc_create_bufs);
+	SET_VALID_IOCTL(ops, VIDIOC_PREPARE_BUF, vidioc_prepare_buf);
+	bitmap_andnot(vdev->valid_ioctls, valid_ioctls, vdev->valid_ioctls,
+			BASE_VIDIOC_PRIVATE);
+}
+
 /**
  *	__video_register_device - register video4linux devices
  *	@vdev: video device structure we want to register
@@ -654,6 +835,13 @@ int __video_register_device(struct video_device *vdev, int type, int nr,
 	WARN_ON(video_device[vdev->minor] != NULL);
 	vdev->index = get_index(vdev);
 	mutex_unlock(&videodev_lock);
+	/* if no lock was passed, then make sure the LOCK_ALL_FOPS bit is
+	   clear and warn if it wasn't. */
+	if (vdev->lock == NULL)
+		WARN_ON(test_and_clear_bit(V4L2_FL_LOCK_ALL_FOPS, &vdev->flags));
+
+	if (vdev->ioctl_ops)
+		determine_valid_ioctls(vdev);
 
 	/* Part 3: Initialize the character device */
 	vdev->cdev = cdev_alloc();
diff --git a/drivers/media/video/v4l2-event.c b/drivers/media/video/v4l2-event.c
index c26ad963714..ef2a33c9404 100644
--- a/drivers/media/video/v4l2-event.c
+++ b/drivers/media/video/v4l2-event.c
@@ -25,7 +25,6 @@
 #include <media/v4l2-dev.h>
 #include <media/v4l2-fh.h>
 #include <media/v4l2-event.h>
-#include <media/v4l2-ctrls.h>
 
 #include <linux/sched.h>
 #include <linux/slab.h>
@@ -120,6 +119,14 @@ static void __v4l2_event_queue_fh(struct v4l2_fh *fh, const struct v4l2_event *e
 	if (sev == NULL)
 		return;
 
+	/*
+	 * If the event has been added to the fh->subscribed list, but its
+	 * add op has not completed yet elems will be 0, treat this as
+	 * not being subscribed.
+	 */
+	if (!sev->elems)
+		return;
+
 	/* Increase event sequence number on fh. */
 	fh->sequence++;
 
@@ -132,14 +139,14 @@ static void __v4l2_event_queue_fh(struct v4l2_fh *fh, const struct v4l2_event *e
 		sev->first = sev_pos(sev, 1);
 		fh->navailable--;
 		if (sev->elems == 1) {
-			if (sev->replace) {
-				sev->replace(&kev->event, ev);
+			if (sev->ops && sev->ops->replace) {
+				sev->ops->replace(&kev->event, ev);
 				copy_payload = false;
 			}
-		} else if (sev->merge) {
+		} else if (sev->ops && sev->ops->merge) {
 			struct v4l2_kevent *second_oldest =
 				sev->events + sev_pos(sev, 0);
-			sev->merge(&kev->event, &second_oldest->event);
+			sev->ops->merge(&kev->event, &second_oldest->event);
 		}
 	}
 
@@ -195,24 +202,11 @@ int v4l2_event_pending(struct v4l2_fh *fh)
 }
 EXPORT_SYMBOL_GPL(v4l2_event_pending);
 
-static void ctrls_replace(struct v4l2_event *old, const struct v4l2_event *new)
-{
-	u32 old_changes = old->u.ctrl.changes;
-
-	old->u.ctrl = new->u.ctrl;
-	old->u.ctrl.changes |= old_changes;
-}
-
-static void ctrls_merge(const struct v4l2_event *old, struct v4l2_event *new)
-{
-	new->u.ctrl.changes |= old->u.ctrl.changes;
-}
-
 int v4l2_event_subscribe(struct v4l2_fh *fh,
-			 struct v4l2_event_subscription *sub, unsigned elems)
+			 struct v4l2_event_subscription *sub, unsigned elems,
+			 const struct v4l2_subscribed_event_ops *ops)
 {
 	struct v4l2_subscribed_event *sev, *found_ev;
-	struct v4l2_ctrl *ctrl = NULL;
 	unsigned long flags;
 	unsigned i;
 
@@ -221,11 +215,6 @@ int v4l2_event_subscribe(struct v4l2_fh *fh,
 
 	if (elems < 1)
 		elems = 1;
-	if (sub->type == V4L2_EVENT_CTRL) {
-		ctrl = v4l2_ctrl_find(fh->ctrl_handler, sub->id);
-		if (ctrl == NULL)
-			return -EINVAL;
-	}
 
 	sev = kzalloc(sizeof(*sev) + sizeof(struct v4l2_kevent) * elems, GFP_KERNEL);
 	if (!sev)
@@ -236,11 +225,7 @@ int v4l2_event_subscribe(struct v4l2_fh *fh,
 	sev->id = sub->id;
 	sev->flags = sub->flags;
 	sev->fh = fh;
-	sev->elems = elems;
-	if (ctrl) {
-		sev->replace = ctrls_replace;
-		sev->merge = ctrls_merge;
-	}
+	sev->ops = ops;
 
 	spin_lock_irqsave(&fh->vdev->fh_lock, flags);
 	found_ev = v4l2_event_subscribed(fh, sub->type, sub->id);
@@ -248,11 +233,22 @@ int v4l2_event_subscribe(struct v4l2_fh *fh,
 		list_add(&sev->list, &fh->subscribed);
 	spin_unlock_irqrestore(&fh->vdev->fh_lock, flags);
 
-	/* v4l2_ctrl_add_event uses a mutex, so do this outside the spin lock */
-	if (found_ev)
+	if (found_ev) {
 		kfree(sev);
-	else if (ctrl)
-		v4l2_ctrl_add_event(ctrl, sev);
+		return 0; /* Already listening */
+	}
+
+	if (sev->ops && sev->ops->add) {
+		int ret = sev->ops->add(sev, elems);
+		if (ret) {
+			sev->ops = NULL;
+			v4l2_event_unsubscribe(fh, sub);
+			return ret;
+		}
+	}
+
+	/* Mark as ready for use */
+	sev->elems = elems;
 
 	return 0;
 }
@@ -306,12 +302,9 @@ int v4l2_event_unsubscribe(struct v4l2_fh *fh,
 	}
 
 	spin_unlock_irqrestore(&fh->vdev->fh_lock, flags);
-	if (sev && sev->type == V4L2_EVENT_CTRL) {
-		struct v4l2_ctrl *ctrl = v4l2_ctrl_find(fh->ctrl_handler, sev->id);
 
-		if (ctrl)
-			v4l2_ctrl_del_event(ctrl, sev);
-	}
+	if (sev && sev->ops && sev->ops->del)
+		sev->ops->del(sev);
 
 	kfree(sev);
 
diff --git a/drivers/media/video/v4l2-ioctl.c b/drivers/media/video/v4l2-ioctl.c
index 5b2ec1fd2d0..91be4e871f4 100644
--- a/drivers/media/video/v4l2-ioctl.c
+++ b/drivers/media/video/v4l2-ioctl.c
@@ -55,19 +55,6 @@
 	memset((u8 *)(p) + offsetof(typeof(*(p)), field) + sizeof((p)->field), \
 	0, sizeof(*(p)) - offsetof(typeof(*(p)), field) - sizeof((p)->field))
 
-#define have_fmt_ops(foo) (						\
-	ops->vidioc_##foo##_fmt_vid_cap ||				\
-	ops->vidioc_##foo##_fmt_vid_out ||				\
-	ops->vidioc_##foo##_fmt_vid_cap_mplane ||			\
-	ops->vidioc_##foo##_fmt_vid_out_mplane ||			\
-	ops->vidioc_##foo##_fmt_vid_overlay ||				\
-	ops->vidioc_##foo##_fmt_vbi_cap ||				\
-	ops->vidioc_##foo##_fmt_vid_out_overlay ||			\
-	ops->vidioc_##foo##_fmt_vbi_out ||				\
-	ops->vidioc_##foo##_fmt_sliced_vbi_cap ||			\
-	ops->vidioc_##foo##_fmt_sliced_vbi_out ||			\
-	ops->vidioc_##foo##_fmt_type_private)
-
 struct std_descr {
 	v4l2_std_id std;
 	const char *descr;
@@ -195,93 +182,118 @@ static const char *v4l2_memory_names[] = {
 
 /* ------------------------------------------------------------------ */
 /* debug help functions                                               */
-static const char *v4l2_ioctls[] = {
-	[_IOC_NR(VIDIOC_QUERYCAP)]         = "VIDIOC_QUERYCAP",
-	[_IOC_NR(VIDIOC_RESERVED)]         = "VIDIOC_RESERVED",
-	[_IOC_NR(VIDIOC_ENUM_FMT)]         = "VIDIOC_ENUM_FMT",
-	[_IOC_NR(VIDIOC_G_FMT)]            = "VIDIOC_G_FMT",
-	[_IOC_NR(VIDIOC_S_FMT)]            = "VIDIOC_S_FMT",
-	[_IOC_NR(VIDIOC_REQBUFS)]          = "VIDIOC_REQBUFS",
-	[_IOC_NR(VIDIOC_QUERYBUF)]         = "VIDIOC_QUERYBUF",
-	[_IOC_NR(VIDIOC_G_FBUF)]           = "VIDIOC_G_FBUF",
-	[_IOC_NR(VIDIOC_S_FBUF)]           = "VIDIOC_S_FBUF",
-	[_IOC_NR(VIDIOC_OVERLAY)]          = "VIDIOC_OVERLAY",
-	[_IOC_NR(VIDIOC_QBUF)]             = "VIDIOC_QBUF",
-	[_IOC_NR(VIDIOC_DQBUF)]            = "VIDIOC_DQBUF",
-	[_IOC_NR(VIDIOC_STREAMON)]         = "VIDIOC_STREAMON",
-	[_IOC_NR(VIDIOC_STREAMOFF)]        = "VIDIOC_STREAMOFF",
-	[_IOC_NR(VIDIOC_G_PARM)]           = "VIDIOC_G_PARM",
-	[_IOC_NR(VIDIOC_S_PARM)]           = "VIDIOC_S_PARM",
-	[_IOC_NR(VIDIOC_G_STD)]            = "VIDIOC_G_STD",
-	[_IOC_NR(VIDIOC_S_STD)]            = "VIDIOC_S_STD",
-	[_IOC_NR(VIDIOC_ENUMSTD)]          = "VIDIOC_ENUMSTD",
-	[_IOC_NR(VIDIOC_ENUMINPUT)]        = "VIDIOC_ENUMINPUT",
-	[_IOC_NR(VIDIOC_G_CTRL)]           = "VIDIOC_G_CTRL",
-	[_IOC_NR(VIDIOC_S_CTRL)]           = "VIDIOC_S_CTRL",
-	[_IOC_NR(VIDIOC_G_TUNER)]          = "VIDIOC_G_TUNER",
-	[_IOC_NR(VIDIOC_S_TUNER)]          = "VIDIOC_S_TUNER",
-	[_IOC_NR(VIDIOC_G_AUDIO)]          = "VIDIOC_G_AUDIO",
-	[_IOC_NR(VIDIOC_S_AUDIO)]          = "VIDIOC_S_AUDIO",
-	[_IOC_NR(VIDIOC_QUERYCTRL)]        = "VIDIOC_QUERYCTRL",
-	[_IOC_NR(VIDIOC_QUERYMENU)]        = "VIDIOC_QUERYMENU",
-	[_IOC_NR(VIDIOC_G_INPUT)]          = "VIDIOC_G_INPUT",
-	[_IOC_NR(VIDIOC_S_INPUT)]          = "VIDIOC_S_INPUT",
-	[_IOC_NR(VIDIOC_G_OUTPUT)]         = "VIDIOC_G_OUTPUT",
-	[_IOC_NR(VIDIOC_S_OUTPUT)]         = "VIDIOC_S_OUTPUT",
-	[_IOC_NR(VIDIOC_ENUMOUTPUT)]       = "VIDIOC_ENUMOUTPUT",
-	[_IOC_NR(VIDIOC_G_AUDOUT)]         = "VIDIOC_G_AUDOUT",
-	[_IOC_NR(VIDIOC_S_AUDOUT)]         = "VIDIOC_S_AUDOUT",
-	[_IOC_NR(VIDIOC_G_MODULATOR)]      = "VIDIOC_G_MODULATOR",
-	[_IOC_NR(VIDIOC_S_MODULATOR)]      = "VIDIOC_S_MODULATOR",
-	[_IOC_NR(VIDIOC_G_FREQUENCY)]      = "VIDIOC_G_FREQUENCY",
-	[_IOC_NR(VIDIOC_S_FREQUENCY)]      = "VIDIOC_S_FREQUENCY",
-	[_IOC_NR(VIDIOC_CROPCAP)]          = "VIDIOC_CROPCAP",
-	[_IOC_NR(VIDIOC_G_CROP)]           = "VIDIOC_G_CROP",
-	[_IOC_NR(VIDIOC_S_CROP)]           = "VIDIOC_S_CROP",
-	[_IOC_NR(VIDIOC_G_SELECTION)]      = "VIDIOC_G_SELECTION",
-	[_IOC_NR(VIDIOC_S_SELECTION)]      = "VIDIOC_S_SELECTION",
-	[_IOC_NR(VIDIOC_G_JPEGCOMP)]       = "VIDIOC_G_JPEGCOMP",
-	[_IOC_NR(VIDIOC_S_JPEGCOMP)]       = "VIDIOC_S_JPEGCOMP",
-	[_IOC_NR(VIDIOC_QUERYSTD)]         = "VIDIOC_QUERYSTD",
-	[_IOC_NR(VIDIOC_TRY_FMT)]          = "VIDIOC_TRY_FMT",
-	[_IOC_NR(VIDIOC_ENUMAUDIO)]        = "VIDIOC_ENUMAUDIO",
-	[_IOC_NR(VIDIOC_ENUMAUDOUT)]       = "VIDIOC_ENUMAUDOUT",
-	[_IOC_NR(VIDIOC_G_PRIORITY)]       = "VIDIOC_G_PRIORITY",
-	[_IOC_NR(VIDIOC_S_PRIORITY)]       = "VIDIOC_S_PRIORITY",
-	[_IOC_NR(VIDIOC_G_SLICED_VBI_CAP)] = "VIDIOC_G_SLICED_VBI_CAP",
-	[_IOC_NR(VIDIOC_LOG_STATUS)]       = "VIDIOC_LOG_STATUS",
-	[_IOC_NR(VIDIOC_G_EXT_CTRLS)]      = "VIDIOC_G_EXT_CTRLS",
-	[_IOC_NR(VIDIOC_S_EXT_CTRLS)]      = "VIDIOC_S_EXT_CTRLS",
-	[_IOC_NR(VIDIOC_TRY_EXT_CTRLS)]    = "VIDIOC_TRY_EXT_CTRLS",
-#if 1
-	[_IOC_NR(VIDIOC_ENUM_FRAMESIZES)]  = "VIDIOC_ENUM_FRAMESIZES",
-	[_IOC_NR(VIDIOC_ENUM_FRAMEINTERVALS)] = "VIDIOC_ENUM_FRAMEINTERVALS",
-	[_IOC_NR(VIDIOC_G_ENC_INDEX)] 	   = "VIDIOC_G_ENC_INDEX",
-	[_IOC_NR(VIDIOC_ENCODER_CMD)] 	   = "VIDIOC_ENCODER_CMD",
-	[_IOC_NR(VIDIOC_TRY_ENCODER_CMD)]  = "VIDIOC_TRY_ENCODER_CMD",
-
-	[_IOC_NR(VIDIOC_DECODER_CMD)]	   = "VIDIOC_DECODER_CMD",
-	[_IOC_NR(VIDIOC_TRY_DECODER_CMD)]  = "VIDIOC_TRY_DECODER_CMD",
-	[_IOC_NR(VIDIOC_DBG_S_REGISTER)]   = "VIDIOC_DBG_S_REGISTER",
-	[_IOC_NR(VIDIOC_DBG_G_REGISTER)]   = "VIDIOC_DBG_G_REGISTER",
-
-	[_IOC_NR(VIDIOC_DBG_G_CHIP_IDENT)] = "VIDIOC_DBG_G_CHIP_IDENT",
-	[_IOC_NR(VIDIOC_S_HW_FREQ_SEEK)]   = "VIDIOC_S_HW_FREQ_SEEK",
+
+struct v4l2_ioctl_info {
+	unsigned int ioctl;
+	u16 flags;
+	const char * const name;
+};
+
+/* This control needs a priority check */
+#define INFO_FL_PRIO	(1 << 0)
+/* This control can be valid if the filehandle passes a control handler. */
+#define INFO_FL_CTRL	(1 << 1)
+
+#define IOCTL_INFO(_ioctl, _flags) [_IOC_NR(_ioctl)] = {	\
+	.ioctl = _ioctl,					\
+	.flags = _flags,					\
+	.name = #_ioctl,					\
+}
+
+static struct v4l2_ioctl_info v4l2_ioctls[] = {
+	IOCTL_INFO(VIDIOC_QUERYCAP, 0),
+	IOCTL_INFO(VIDIOC_ENUM_FMT, 0),
+	IOCTL_INFO(VIDIOC_G_FMT, 0),
+	IOCTL_INFO(VIDIOC_S_FMT, INFO_FL_PRIO),
+	IOCTL_INFO(VIDIOC_REQBUFS, INFO_FL_PRIO),
+	IOCTL_INFO(VIDIOC_QUERYBUF, 0),
+	IOCTL_INFO(VIDIOC_G_FBUF, 0),
+	IOCTL_INFO(VIDIOC_S_FBUF, INFO_FL_PRIO),
+	IOCTL_INFO(VIDIOC_OVERLAY, INFO_FL_PRIO),
+	IOCTL_INFO(VIDIOC_QBUF, 0),
+	IOCTL_INFO(VIDIOC_DQBUF, 0),
+	IOCTL_INFO(VIDIOC_STREAMON, INFO_FL_PRIO),
+	IOCTL_INFO(VIDIOC_STREAMOFF, INFO_FL_PRIO),
+	IOCTL_INFO(VIDIOC_G_PARM, 0),
+	IOCTL_INFO(VIDIOC_S_PARM, INFO_FL_PRIO),
+	IOCTL_INFO(VIDIOC_G_STD, 0),
+	IOCTL_INFO(VIDIOC_S_STD, INFO_FL_PRIO),
+	IOCTL_INFO(VIDIOC_ENUMSTD, 0),
+	IOCTL_INFO(VIDIOC_ENUMINPUT, 0),
+	IOCTL_INFO(VIDIOC_G_CTRL, INFO_FL_CTRL),
+	IOCTL_INFO(VIDIOC_S_CTRL, INFO_FL_PRIO | INFO_FL_CTRL),
+	IOCTL_INFO(VIDIOC_G_TUNER, 0),
+	IOCTL_INFO(VIDIOC_S_TUNER, INFO_FL_PRIO),
+	IOCTL_INFO(VIDIOC_G_AUDIO, 0),
+	IOCTL_INFO(VIDIOC_S_AUDIO, INFO_FL_PRIO),
+	IOCTL_INFO(VIDIOC_QUERYCTRL, INFO_FL_CTRL),
+	IOCTL_INFO(VIDIOC_QUERYMENU, INFO_FL_CTRL),
+	IOCTL_INFO(VIDIOC_G_INPUT, 0),
+	IOCTL_INFO(VIDIOC_S_INPUT, INFO_FL_PRIO),
+	IOCTL_INFO(VIDIOC_G_OUTPUT, 0),
+	IOCTL_INFO(VIDIOC_S_OUTPUT, INFO_FL_PRIO),
+	IOCTL_INFO(VIDIOC_ENUMOUTPUT, 0),
+	IOCTL_INFO(VIDIOC_G_AUDOUT, 0),
+	IOCTL_INFO(VIDIOC_S_AUDOUT, INFO_FL_PRIO),
+	IOCTL_INFO(VIDIOC_G_MODULATOR, 0),
+	IOCTL_INFO(VIDIOC_S_MODULATOR, INFO_FL_PRIO),
+	IOCTL_INFO(VIDIOC_G_FREQUENCY, 0),
+	IOCTL_INFO(VIDIOC_S_FREQUENCY, INFO_FL_PRIO),
+	IOCTL_INFO(VIDIOC_CROPCAP, 0),
+	IOCTL_INFO(VIDIOC_G_CROP, 0),
+	IOCTL_INFO(VIDIOC_S_CROP, INFO_FL_PRIO),
+	IOCTL_INFO(VIDIOC_G_SELECTION, 0),
+	IOCTL_INFO(VIDIOC_S_SELECTION, INFO_FL_PRIO),
+	IOCTL_INFO(VIDIOC_G_JPEGCOMP, 0),
+	IOCTL_INFO(VIDIOC_S_JPEGCOMP, INFO_FL_PRIO),
+	IOCTL_INFO(VIDIOC_QUERYSTD, 0),
+	IOCTL_INFO(VIDIOC_TRY_FMT, 0),
+	IOCTL_INFO(VIDIOC_ENUMAUDIO, 0),
+	IOCTL_INFO(VIDIOC_ENUMAUDOUT, 0),
+	IOCTL_INFO(VIDIOC_G_PRIORITY, 0),
+	IOCTL_INFO(VIDIOC_S_PRIORITY, INFO_FL_PRIO),
+	IOCTL_INFO(VIDIOC_G_SLICED_VBI_CAP, 0),
+	IOCTL_INFO(VIDIOC_LOG_STATUS, 0),
+	IOCTL_INFO(VIDIOC_G_EXT_CTRLS, INFO_FL_CTRL),
+	IOCTL_INFO(VIDIOC_S_EXT_CTRLS, INFO_FL_PRIO | INFO_FL_CTRL),
+	IOCTL_INFO(VIDIOC_TRY_EXT_CTRLS, 0),
+	IOCTL_INFO(VIDIOC_ENUM_FRAMESIZES, 0),
+	IOCTL_INFO(VIDIOC_ENUM_FRAMEINTERVALS, 0),
+	IOCTL_INFO(VIDIOC_G_ENC_INDEX, 0),
+	IOCTL_INFO(VIDIOC_ENCODER_CMD, INFO_FL_PRIO),
+	IOCTL_INFO(VIDIOC_TRY_ENCODER_CMD, 0),
+	IOCTL_INFO(VIDIOC_DECODER_CMD, INFO_FL_PRIO),
+	IOCTL_INFO(VIDIOC_TRY_DECODER_CMD, 0),
+#ifdef CONFIG_VIDEO_ADV_DEBUG
+	IOCTL_INFO(VIDIOC_DBG_S_REGISTER, 0),
+	IOCTL_INFO(VIDIOC_DBG_G_REGISTER, 0),
 #endif
-	[_IOC_NR(VIDIOC_ENUM_DV_PRESETS)]  = "VIDIOC_ENUM_DV_PRESETS",
-	[_IOC_NR(VIDIOC_S_DV_PRESET)]	   = "VIDIOC_S_DV_PRESET",
-	[_IOC_NR(VIDIOC_G_DV_PRESET)]	   = "VIDIOC_G_DV_PRESET",
-	[_IOC_NR(VIDIOC_QUERY_DV_PRESET)]  = "VIDIOC_QUERY_DV_PRESET",
-	[_IOC_NR(VIDIOC_S_DV_TIMINGS)]     = "VIDIOC_S_DV_TIMINGS",
-	[_IOC_NR(VIDIOC_G_DV_TIMINGS)]     = "VIDIOC_G_DV_TIMINGS",
-	[_IOC_NR(VIDIOC_DQEVENT)]	   = "VIDIOC_DQEVENT",
-	[_IOC_NR(VIDIOC_SUBSCRIBE_EVENT)]  = "VIDIOC_SUBSCRIBE_EVENT",
-	[_IOC_NR(VIDIOC_UNSUBSCRIBE_EVENT)] = "VIDIOC_UNSUBSCRIBE_EVENT",
-	[_IOC_NR(VIDIOC_CREATE_BUFS)]      = "VIDIOC_CREATE_BUFS",
-	[_IOC_NR(VIDIOC_PREPARE_BUF)]      = "VIDIOC_PREPARE_BUF",
+	IOCTL_INFO(VIDIOC_DBG_G_CHIP_IDENT, 0),
+	IOCTL_INFO(VIDIOC_S_HW_FREQ_SEEK, INFO_FL_PRIO),
+	IOCTL_INFO(VIDIOC_ENUM_DV_PRESETS, 0),
+	IOCTL_INFO(VIDIOC_S_DV_PRESET, INFO_FL_PRIO),
+	IOCTL_INFO(VIDIOC_G_DV_PRESET, 0),
+	IOCTL_INFO(VIDIOC_QUERY_DV_PRESET, 0),
+	IOCTL_INFO(VIDIOC_S_DV_TIMINGS, INFO_FL_PRIO),
+	IOCTL_INFO(VIDIOC_G_DV_TIMINGS, 0),
+	IOCTL_INFO(VIDIOC_DQEVENT, 0),
+	IOCTL_INFO(VIDIOC_SUBSCRIBE_EVENT, 0),
+	IOCTL_INFO(VIDIOC_UNSUBSCRIBE_EVENT, 0),
+	IOCTL_INFO(VIDIOC_CREATE_BUFS, INFO_FL_PRIO),
+	IOCTL_INFO(VIDIOC_PREPARE_BUF, 0),
+	IOCTL_INFO(VIDIOC_ENUM_DV_TIMINGS, 0),
+	IOCTL_INFO(VIDIOC_QUERY_DV_TIMINGS, 0),
+	IOCTL_INFO(VIDIOC_DV_TIMINGS_CAP, 0),
 };
 #define V4L2_IOCTLS ARRAY_SIZE(v4l2_ioctls)
 
+bool v4l2_is_known_ioctl(unsigned int cmd)
+{
+	if (_IOC_NR(cmd) >= V4L2_IOCTLS)
+		return false;
+	return v4l2_ioctls[_IOC_NR(cmd)].ioctl == cmd;
+}
+
 /* Common ioctl debug function. This function can be used by
    external ioctl messages as well as internal V4L ioctl */
 void v4l_printk_ioctl(unsigned int cmd)
@@ -297,7 +309,7 @@ void v4l_printk_ioctl(unsigned int cmd)
 			type = "v4l2";
 			break;
 		}
-		printk("%s", v4l2_ioctls[_IOC_NR(cmd)]);
+		printk("%s", v4l2_ioctls[_IOC_NR(cmd)].name);
 		return;
 	default:
 		type = "unknown";
@@ -359,6 +371,34 @@ static inline void dbgrect(struct video_device *vfd, char *s,
 						r->width, r->height);
 };
 
+static void dbgtimings(struct video_device *vfd,
+			const struct v4l2_dv_timings *p)
+{
+	switch (p->type) {
+	case V4L2_DV_BT_656_1120:
+		dbgarg2("bt-656/1120:interlaced=%d,"
+				" pixelclock=%lld,"
+				" width=%d, height=%d, polarities=%x,"
+				" hfrontporch=%d, hsync=%d,"
+				" hbackporch=%d, vfrontporch=%d,"
+				" vsync=%d, vbackporch=%d,"
+				" il_vfrontporch=%d, il_vsync=%d,"
+				" il_vbackporch=%d, standards=%x, flags=%x\n",
+				p->bt.interlaced, p->bt.pixelclock,
+				p->bt.width, p->bt.height,
+				p->bt.polarities, p->bt.hfrontporch,
+				p->bt.hsync, p->bt.hbackporch,
+				p->bt.vfrontporch, p->bt.vsync,
+				p->bt.vbackporch, p->bt.il_vfrontporch,
+				p->bt.il_vsync, p->bt.il_vbackporch,
+				p->bt.standards, p->bt.flags);
+		break;
+	default:
+		dbgarg2("Unknown type %d!\n", p->type);
+		break;
+	}
+}
+
 static inline void v4l_print_pix_fmt(struct video_device *vfd,
 						struct v4l2_pix_format *fmt)
 {
@@ -504,7 +544,6 @@ static long __video_do_ioctl(struct file *file,
 	void *fh = file->private_data;
 	struct v4l2_fh *vfh = NULL;
 	int use_fh_prio = 0;
-	long ret_prio = 0;
 	long ret = -ENOTTY;
 
 	if (ops == NULL) {
@@ -513,19 +552,30 @@ static long __video_do_ioctl(struct file *file,
 		return ret;
 	}
 
-	if ((vfd->debug & V4L2_DEBUG_IOCTL) &&
-				!(vfd->debug & V4L2_DEBUG_IOCTL_ARG)) {
-		v4l_print_ioctl(vfd->name, cmd);
-		printk(KERN_CONT "\n");
-	}
-
 	if (test_bit(V4L2_FL_USES_V4L2_FH, &vfd->flags)) {
 		vfh = file->private_data;
 		use_fh_prio = test_bit(V4L2_FL_USE_FH_PRIO, &vfd->flags);
 	}
 
-	if (use_fh_prio)
-		ret_prio = v4l2_prio_check(vfd->prio, vfh->prio);
+	if (v4l2_is_known_ioctl(cmd)) {
+		struct v4l2_ioctl_info *info = &v4l2_ioctls[_IOC_NR(cmd)];
+
+	        if (!test_bit(_IOC_NR(cmd), vfd->valid_ioctls) &&
+		    !((info->flags & INFO_FL_CTRL) && vfh && vfh->ctrl_handler))
+			return -ENOTTY;
+
+		if (use_fh_prio && (info->flags & INFO_FL_PRIO)) {
+			ret = v4l2_prio_check(vfd->prio, vfh->prio);
+			if (ret)
+				return ret;
+		}
+	}
+
+	if ((vfd->debug & V4L2_DEBUG_IOCTL) &&
+				!(vfd->debug & V4L2_DEBUG_IOCTL_ARG)) {
+		v4l_print_ioctl(vfd->name, cmd);
+		printk(KERN_CONT "\n");
+	}
 
 	switch (cmd) {
 
@@ -534,9 +584,6 @@ static long __video_do_ioctl(struct file *file,
 	{
 		struct v4l2_capability *cap = (struct v4l2_capability *)arg;
 
-		if (!ops->vidioc_querycap)
-			break;
-
 		cap->version = LINUX_VERSION_CODE;
 		ret = ops->vidioc_querycap(file, fh, cap);
 		if (!ret)
@@ -570,14 +617,11 @@ static long __video_do_ioctl(struct file *file,
 	{
 		enum v4l2_priority *p = arg;
 
-		if (!ops->vidioc_s_priority && !use_fh_prio)
-			break;
 		dbgarg(cmd, "setting priority to %d\n", *p);
 		if (ops->vidioc_s_priority)
 			ret = ops->vidioc_s_priority(file, fh, *p);
 		else
-			ret = ret_prio ? ret_prio :
-				v4l2_prio_change(&vfd->v4l2_dev->prio,
+			ret = v4l2_prio_change(&vfd->v4l2_dev->prio,
 							&vfh->prio, *p);
 		break;
 	}
@@ -587,6 +631,7 @@ static long __video_do_ioctl(struct file *file,
 	{
 		struct v4l2_fmtdesc *f = arg;
 
+		ret = -EINVAL;
 		switch (f->type) {
 		case V4L2_BUF_TYPE_VIDEO_CAPTURE:
 			if (likely(ops->vidioc_enum_fmt_vid_cap))
@@ -619,7 +664,7 @@ static long __video_do_ioctl(struct file *file,
 		default:
 			break;
 		}
-		if (likely (!ret))
+		if (likely(!ret))
 			dbgarg(cmd, "index=%d, type=%d, flags=%d, "
 				"pixelformat=%c%c%c%c, description='%s'\n",
 				f->index, f->type, f->flags,
@@ -628,14 +673,6 @@ static long __video_do_ioctl(struct file *file,
 				(f->pixelformat >> 16) & 0xff,
 				(f->pixelformat >> 24) & 0xff,
 				f->description);
-		else if (ret == -ENOTTY &&
-			 (ops->vidioc_enum_fmt_vid_cap ||
-			  ops->vidioc_enum_fmt_vid_out ||
-			  ops->vidioc_enum_fmt_vid_cap_mplane ||
-			  ops->vidioc_enum_fmt_vid_out_mplane ||
-			  ops->vidioc_enum_fmt_vid_overlay ||
-			  ops->vidioc_enum_fmt_type_private))
-			ret = -EINVAL;
 		break;
 	}
 	case VIDIOC_G_FMT:
@@ -645,6 +682,7 @@ static long __video_do_ioctl(struct file *file,
 		/* FIXME: Should be one dump per type */
 		dbgarg(cmd, "type=%s\n", prt_names(f->type, v4l2_type_names));
 
+		ret = -EINVAL;
 		switch (f->type) {
 		case V4L2_BUF_TYPE_VIDEO_CAPTURE:
 			if (ops->vidioc_g_fmt_vid_cap)
@@ -706,21 +744,12 @@ static long __video_do_ioctl(struct file *file,
 								fh, f);
 			break;
 		}
-		if (unlikely(ret == -ENOTTY && have_fmt_ops(g)))
-			ret = -EINVAL;
-
 		break;
 	}
 	case VIDIOC_S_FMT:
 	{
 		struct v4l2_format *f = (struct v4l2_format *)arg;
 
-		if (!have_fmt_ops(s))
-			break;
-		if (ret_prio) {
-			ret = ret_prio;
-			break;
-		}
 		ret = -EINVAL;
 
 		/* FIXME: Should be one dump per type */
@@ -804,6 +833,7 @@ static long __video_do_ioctl(struct file *file,
 		/* FIXME: Should be one dump per type */
 		dbgarg(cmd, "type=%s\n", prt_names(f->type,
 						v4l2_type_names));
+		ret = -EINVAL;
 		switch (f->type) {
 		case V4L2_BUF_TYPE_VIDEO_CAPTURE:
 			CLEAR_AFTER_FIELD(f, fmt.pix);
@@ -876,8 +906,6 @@ static long __video_do_ioctl(struct file *file,
 								fh, f);
 			break;
 		}
-		if (unlikely(ret == -ENOTTY && have_fmt_ops(try)))
-			ret = -EINVAL;
 		break;
 	}
 	/* FIXME: Those buf reqs could be handled here,
@@ -888,12 +916,6 @@ static long __video_do_ioctl(struct file *file,
 	{
 		struct v4l2_requestbuffers *p = arg;
 
-		if (!ops->vidioc_reqbufs)
-			break;
-		if (ret_prio) {
-			ret = ret_prio;
-			break;
-		}
 		ret = check_fmt(ops, p->type);
 		if (ret)
 			break;
@@ -912,8 +934,6 @@ static long __video_do_ioctl(struct file *file,
 	{
 		struct v4l2_buffer *p = arg;
 
-		if (!ops->vidioc_querybuf)
-			break;
 		ret = check_fmt(ops, p->type);
 		if (ret)
 			break;
@@ -927,8 +947,6 @@ static long __video_do_ioctl(struct file *file,
 	{
 		struct v4l2_buffer *p = arg;
 
-		if (!ops->vidioc_qbuf)
-			break;
 		ret = check_fmt(ops, p->type);
 		if (ret)
 			break;
@@ -942,8 +960,6 @@ static long __video_do_ioctl(struct file *file,
 	{
 		struct v4l2_buffer *p = arg;
 
-		if (!ops->vidioc_dqbuf)
-			break;
 		ret = check_fmt(ops, p->type);
 		if (ret)
 			break;
@@ -957,12 +973,6 @@ static long __video_do_ioctl(struct file *file,
 	{
 		int *i = arg;
 
-		if (!ops->vidioc_overlay)
-			break;
-		if (ret_prio) {
-			ret = ret_prio;
-			break;
-		}
 		dbgarg(cmd, "value=%d\n", *i);
 		ret = ops->vidioc_overlay(file, fh, *i);
 		break;
@@ -971,8 +981,6 @@ static long __video_do_ioctl(struct file *file,
 	{
 		struct v4l2_framebuffer *p = arg;
 
-		if (!ops->vidioc_g_fbuf)
-			break;
 		ret = ops->vidioc_g_fbuf(file, fh, arg);
 		if (!ret) {
 			dbgarg(cmd, "capability=0x%x, flags=%d, base=0x%08lx\n",
@@ -986,12 +994,6 @@ static long __video_do_ioctl(struct file *file,
 	{
 		struct v4l2_framebuffer *p = arg;
 
-		if (!ops->vidioc_s_fbuf)
-			break;
-		if (ret_prio) {
-			ret = ret_prio;
-			break;
-		}
 		dbgarg(cmd, "capability=0x%x, flags=%d, base=0x%08lx\n",
 			p->capability, p->flags, (unsigned long)p->base);
 		v4l_print_pix_fmt(vfd, &p->fmt);
@@ -1002,12 +1004,6 @@ static long __video_do_ioctl(struct file *file,
 	{
 		enum v4l2_buf_type i = *(int *)arg;
 
-		if (!ops->vidioc_streamon)
-			break;
-		if (ret_prio) {
-			ret = ret_prio;
-			break;
-		}
 		dbgarg(cmd, "type=%s\n", prt_names(i, v4l2_type_names));
 		ret = ops->vidioc_streamon(file, fh, i);
 		break;
@@ -1016,12 +1012,6 @@ static long __video_do_ioctl(struct file *file,
 	{
 		enum v4l2_buf_type i = *(int *)arg;
 
-		if (!ops->vidioc_streamoff)
-			break;
-		if (ret_prio) {
-			ret = ret_prio;
-			break;
-		}
 		dbgarg(cmd, "type=%s\n", prt_names(i, v4l2_type_names));
 		ret = ops->vidioc_streamoff(file, fh, i);
 		break;
@@ -1091,13 +1081,6 @@ static long __video_do_ioctl(struct file *file,
 
 		dbgarg(cmd, "std=%08Lx\n", (long long unsigned)*id);
 
-		if (!ops->vidioc_s_std)
-			break;
-
-		if (ret_prio) {
-			ret = ret_prio;
-			break;
-		}
 		ret = -EINVAL;
 		norm = (*id) & vfd->tvnorms;
 		if (vfd->tvnorms && !norm)	/* Check if std is supported */
@@ -1115,8 +1098,6 @@ static long __video_do_ioctl(struct file *file,
 	{
 		v4l2_std_id *p = arg;
 
-		if (!ops->vidioc_querystd)
-			break;
 		/*
 		 * If nothing detected, it should return all supported
 		 * Drivers just need to mask the std argument, in order
@@ -1150,9 +1131,6 @@ static long __video_do_ioctl(struct file *file,
 		if (ops->vidioc_s_dv_timings)
 			p->capabilities |= V4L2_IN_CAP_CUSTOM_TIMINGS;
 
-		if (!ops->vidioc_enum_input)
-			break;
-
 		ret = ops->vidioc_enum_input(file, fh, p);
 		if (!ret)
 			dbgarg(cmd, "index=%d, name=%s, type=%d, "
@@ -1168,8 +1146,6 @@ static long __video_do_ioctl(struct file *file,
 	{
 		unsigned int *i = arg;
 
-		if (!ops->vidioc_g_input)
-			break;
 		ret = ops->vidioc_g_input(file, fh, i);
 		if (!ret)
 			dbgarg(cmd, "value=%d\n", *i);
@@ -1179,12 +1155,6 @@ static long __video_do_ioctl(struct file *file,
 	{
 		unsigned int *i = arg;
 
-		if (!ops->vidioc_s_input)
-			break;
-		if (ret_prio) {
-			ret = ret_prio;
-			break;
-		}
 		dbgarg(cmd, "value=%d\n", *i);
 		ret = ops->vidioc_s_input(file, fh, *i);
 		break;
@@ -1195,9 +1165,6 @@ static long __video_do_ioctl(struct file *file,
 	{
 		struct v4l2_output *p = arg;
 
-		if (!ops->vidioc_enum_output)
-			break;
-
 		/*
 		 * We set the flags for CAP_PRESETS, CAP_CUSTOM_TIMINGS &
 		 * CAP_STD here based on ioctl handler provided by the
@@ -1224,8 +1191,6 @@ static long __video_do_ioctl(struct file *file,
 	{
 		unsigned int *i = arg;
 
-		if (!ops->vidioc_g_output)
-			break;
 		ret = ops->vidioc_g_output(file, fh, i);
 		if (!ret)
 			dbgarg(cmd, "value=%d\n", *i);
@@ -1235,12 +1200,6 @@ static long __video_do_ioctl(struct file *file,
 	{
 		unsigned int *i = arg;
 
-		if (!ops->vidioc_s_output)
-			break;
-		if (ret_prio) {
-			ret = ret_prio;
-			break;
-		}
 		dbgarg(cmd, "value=%d\n", *i);
 		ret = ops->vidioc_s_output(file, fh, *i);
 		break;
@@ -1310,10 +1269,6 @@ static long __video_do_ioctl(struct file *file,
 		if (!(vfh && vfh->ctrl_handler) && !vfd->ctrl_handler &&
 			!ops->vidioc_s_ctrl && !ops->vidioc_s_ext_ctrls)
 			break;
-		if (ret_prio) {
-			ret = ret_prio;
-			break;
-		}
 
 		dbgarg(cmd, "id=0x%x, value=%d\n", p->id, p->value);
 
@@ -1369,10 +1324,6 @@ static long __video_do_ioctl(struct file *file,
 		if (!(vfh && vfh->ctrl_handler) && !vfd->ctrl_handler &&
 				!ops->vidioc_s_ext_ctrls)
 			break;
-		if (ret_prio) {
-			ret = ret_prio;
-			break;
-		}
 		v4l_print_ext_ctrls(cmd, vfd, p, 1);
 		if (vfh && vfh->ctrl_handler)
 			ret = v4l2_s_ext_ctrls(vfh, vfh->ctrl_handler, p);
@@ -1428,8 +1379,6 @@ static long __video_do_ioctl(struct file *file,
 	{
 		struct v4l2_audio *p = arg;
 
-		if (!ops->vidioc_enumaudio)
-			break;
 		ret = ops->vidioc_enumaudio(file, fh, p);
 		if (!ret)
 			dbgarg(cmd, "index=%d, name=%s, capability=0x%x, "
@@ -1443,9 +1392,6 @@ static long __video_do_ioctl(struct file *file,
 	{
 		struct v4l2_audio *p = arg;
 
-		if (!ops->vidioc_g_audio)
-			break;
-
 		ret = ops->vidioc_g_audio(file, fh, p);
 		if (!ret)
 			dbgarg(cmd, "index=%d, name=%s, capability=0x%x, "
@@ -1459,12 +1405,6 @@ static long __video_do_ioctl(struct file *file,
 	{
 		struct v4l2_audio *p = arg;
 
-		if (!ops->vidioc_s_audio)
-			break;
-		if (ret_prio) {
-			ret = ret_prio;
-			break;
-		}
 		dbgarg(cmd, "index=%d, name=%s, capability=0x%x, "
 					"mode=0x%x\n", p->index, p->name,
 					p->capability, p->mode);
@@ -1475,8 +1415,6 @@ static long __video_do_ioctl(struct file *file,
 	{
 		struct v4l2_audioout *p = arg;
 
-		if (!ops->vidioc_enumaudout)
-			break;
 		dbgarg(cmd, "Enum for index=%d\n", p->index);
 		ret = ops->vidioc_enumaudout(file, fh, p);
 		if (!ret)
@@ -1489,9 +1427,6 @@ static long __video_do_ioctl(struct file *file,
 	{
 		struct v4l2_audioout *p = arg;
 
-		if (!ops->vidioc_g_audout)
-			break;
-
 		ret = ops->vidioc_g_audout(file, fh, p);
 		if (!ret)
 			dbgarg2("index=%d, name=%s, capability=%d, "
@@ -1503,12 +1438,6 @@ static long __video_do_ioctl(struct file *file,
 	{
 		struct v4l2_audioout *p = arg;
 
-		if (!ops->vidioc_s_audout)
-			break;
-		if (ret_prio) {
-			ret = ret_prio;
-			break;
-		}
 		dbgarg(cmd, "index=%d, name=%s, capability=%d, "
 					"mode=%d\n", p->index, p->name,
 					p->capability, p->mode);
@@ -1520,8 +1449,6 @@ static long __video_do_ioctl(struct file *file,
 	{
 		struct v4l2_modulator *p = arg;
 
-		if (!ops->vidioc_g_modulator)
-			break;
 		ret = ops->vidioc_g_modulator(file, fh, p);
 		if (!ret)
 			dbgarg(cmd, "index=%d, name=%s, "
@@ -1536,12 +1463,6 @@ static long __video_do_ioctl(struct file *file,
 	{
 		struct v4l2_modulator *p = arg;
 
-		if (!ops->vidioc_s_modulator)
-			break;
-		if (ret_prio) {
-			ret = ret_prio;
-			break;
-		}
 		dbgarg(cmd, "index=%d, name=%s, capability=%d, "
 				"rangelow=%d, rangehigh=%d, txsubchans=%d\n",
 				p->index, p->name, p->capability, p->rangelow,
@@ -1553,9 +1474,6 @@ static long __video_do_ioctl(struct file *file,
 	{
 		struct v4l2_crop *p = arg;
 
-		if (!ops->vidioc_g_crop && !ops->vidioc_g_selection)
-			break;
-
 		dbgarg(cmd, "type=%s\n", prt_names(p->type, v4l2_type_names));
 
 		if (ops->vidioc_g_crop) {
@@ -1587,13 +1505,6 @@ static long __video_do_ioctl(struct file *file,
 	{
 		struct v4l2_crop *p = arg;
 
-		if (!ops->vidioc_s_crop && !ops->vidioc_s_selection)
-			break;
-
-		if (ret_prio) {
-			ret = ret_prio;
-			break;
-		}
 		dbgarg(cmd, "type=%s\n", prt_names(p->type, v4l2_type_names));
 		dbgrect(vfd, "", &p->c);
 
@@ -1620,9 +1531,6 @@ static long __video_do_ioctl(struct file *file,
 	{
 		struct v4l2_selection *p = arg;
 
-		if (!ops->vidioc_g_selection)
-			break;
-
 		dbgarg(cmd, "type=%s\n", prt_names(p->type, v4l2_type_names));
 
 		ret = ops->vidioc_g_selection(file, fh, p);
@@ -1634,13 +1542,6 @@ static long __video_do_ioctl(struct file *file,
 	{
 		struct v4l2_selection *p = arg;
 
-		if (!ops->vidioc_s_selection)
-			break;
-
-		if (ret_prio) {
-			ret = ret_prio;
-			break;
-		}
 
 		dbgarg(cmd, "type=%s\n", prt_names(p->type, v4l2_type_names));
 		dbgrect(vfd, "", &p->r);
@@ -1653,9 +1554,6 @@ static long __video_do_ioctl(struct file *file,
 		struct v4l2_cropcap *p = arg;
 
 		/*FIXME: Should also show v4l2_fract pixelaspect */
-		if (!ops->vidioc_cropcap && !ops->vidioc_g_selection)
-			break;
-
 		dbgarg(cmd, "type=%s\n", prt_names(p->type, v4l2_type_names));
 		if (ops->vidioc_cropcap) {
 			ret = ops->vidioc_cropcap(file, fh, p);
@@ -1699,9 +1597,6 @@ static long __video_do_ioctl(struct file *file,
 	{
 		struct v4l2_jpegcompression *p = arg;
 
-		if (!ops->vidioc_g_jpegcomp)
-			break;
-
 		ret = ops->vidioc_g_jpegcomp(file, fh, p);
 		if (!ret)
 			dbgarg(cmd, "quality=%d, APPn=%d, "
@@ -1715,12 +1610,6 @@ static long __video_do_ioctl(struct file *file,
 	{
 		struct v4l2_jpegcompression *p = arg;
 
-		if (!ops->vidioc_g_jpegcomp)
-			break;
-		if (ret_prio) {
-			ret = ret_prio;
-			break;
-		}
 		dbgarg(cmd, "quality=%d, APPn=%d, APP_len=%d, "
 					"COM_len=%d, jpeg_markers=%d\n",
 					p->quality, p->APPn, p->APP_len,
@@ -1732,8 +1621,6 @@ static long __video_do_ioctl(struct file *file,
 	{
 		struct v4l2_enc_idx *p = arg;
 
-		if (!ops->vidioc_g_enc_index)
-			break;
 		ret = ops->vidioc_g_enc_index(file, fh, p);
 		if (!ret)
 			dbgarg(cmd, "entries=%d, entries_cap=%d\n",
@@ -1744,12 +1631,6 @@ static long __video_do_ioctl(struct file *file,
 	{
 		struct v4l2_encoder_cmd *p = arg;
 
-		if (!ops->vidioc_encoder_cmd)
-			break;
-		if (ret_prio) {
-			ret = ret_prio;
-			break;
-		}
 		ret = ops->vidioc_encoder_cmd(file, fh, p);
 		if (!ret)
 			dbgarg(cmd, "cmd=%d, flags=%x\n", p->cmd, p->flags);
@@ -1759,8 +1640,6 @@ static long __video_do_ioctl(struct file *file,
 	{
 		struct v4l2_encoder_cmd *p = arg;
 
-		if (!ops->vidioc_try_encoder_cmd)
-			break;
 		ret = ops->vidioc_try_encoder_cmd(file, fh, p);
 		if (!ret)
 			dbgarg(cmd, "cmd=%d, flags=%x\n", p->cmd, p->flags);
@@ -1770,12 +1649,6 @@ static long __video_do_ioctl(struct file *file,
 	{
 		struct v4l2_decoder_cmd *p = arg;
 
-		if (!ops->vidioc_decoder_cmd)
-			break;
-		if (ret_prio) {
-			ret = ret_prio;
-			break;
-		}
 		ret = ops->vidioc_decoder_cmd(file, fh, p);
 		if (!ret)
 			dbgarg(cmd, "cmd=%d, flags=%x\n", p->cmd, p->flags);
@@ -1785,8 +1658,6 @@ static long __video_do_ioctl(struct file *file,
 	{
 		struct v4l2_decoder_cmd *p = arg;
 
-		if (!ops->vidioc_try_decoder_cmd)
-			break;
 		ret = ops->vidioc_try_decoder_cmd(file, fh, p);
 		if (!ret)
 			dbgarg(cmd, "cmd=%d, flags=%x\n", p->cmd, p->flags);
@@ -1796,8 +1667,6 @@ static long __video_do_ioctl(struct file *file,
 	{
 		struct v4l2_streamparm *p = arg;
 
-		if (!ops->vidioc_g_parm && !vfd->current_norm)
-			break;
 		if (ops->vidioc_g_parm) {
 			ret = check_fmt(ops, p->type);
 			if (ret)
@@ -1825,12 +1694,6 @@ static long __video_do_ioctl(struct file *file,
 	{
 		struct v4l2_streamparm *p = arg;
 
-		if (!ops->vidioc_s_parm)
-			break;
-		if (ret_prio) {
-			ret = ret_prio;
-			break;
-		}
 		ret = check_fmt(ops, p->type);
 		if (ret)
 			break;
@@ -1843,9 +1706,6 @@ static long __video_do_ioctl(struct file *file,
 	{
 		struct v4l2_tuner *p = arg;
 
-		if (!ops->vidioc_g_tuner)
-			break;
-
 		p->type = (vfd->vfl_type == VFL_TYPE_RADIO) ?
 			V4L2_TUNER_RADIO : V4L2_TUNER_ANALOG_TV;
 		ret = ops->vidioc_g_tuner(file, fh, p);
@@ -1864,12 +1724,6 @@ static long __video_do_ioctl(struct file *file,
 	{
 		struct v4l2_tuner *p = arg;
 
-		if (!ops->vidioc_s_tuner)
-			break;
-		if (ret_prio) {
-			ret = ret_prio;
-			break;
-		}
 		p->type = (vfd->vfl_type == VFL_TYPE_RADIO) ?
 			V4L2_TUNER_RADIO : V4L2_TUNER_ANALOG_TV;
 		dbgarg(cmd, "index=%d, name=%s, type=%d, "
@@ -1887,9 +1741,6 @@ static long __video_do_ioctl(struct file *file,
 	{
 		struct v4l2_frequency *p = arg;
 
-		if (!ops->vidioc_g_frequency)
-			break;
-
 		p->type = (vfd->vfl_type == VFL_TYPE_RADIO) ?
 			V4L2_TUNER_RADIO : V4L2_TUNER_ANALOG_TV;
 		ret = ops->vidioc_g_frequency(file, fh, p);
@@ -1903,12 +1754,6 @@ static long __video_do_ioctl(struct file *file,
 		struct v4l2_frequency *p = arg;
 		enum v4l2_tuner_type type;
 
-		if (!ops->vidioc_s_frequency)
-			break;
-		if (ret_prio) {
-			ret = ret_prio;
-			break;
-		}
 		type = (vfd->vfl_type == VFL_TYPE_RADIO) ?
 			V4L2_TUNER_RADIO : V4L2_TUNER_ANALOG_TV;
 		dbgarg(cmd, "tuner=%d, type=%d, frequency=%d\n",
@@ -1923,9 +1768,6 @@ static long __video_do_ioctl(struct file *file,
 	{
 		struct v4l2_sliced_vbi_cap *p = arg;
 
-		if (!ops->vidioc_g_sliced_vbi_cap)
-			break;
-
 		/* Clear up to type, everything after type is zerod already */
 		memset(p, 0, offsetof(struct v4l2_sliced_vbi_cap, type));
 
@@ -1937,8 +1779,6 @@ static long __video_do_ioctl(struct file *file,
 	}
 	case VIDIOC_LOG_STATUS:
 	{
-		if (!ops->vidioc_log_status)
-			break;
 		if (vfd->v4l2_dev)
 			pr_info("%s: =================  START STATUS  =================\n",
 				vfd->v4l2_dev->name);
@@ -1948,38 +1788,34 @@ static long __video_do_ioctl(struct file *file,
 				vfd->v4l2_dev->name);
 		break;
 	}
-#ifdef CONFIG_VIDEO_ADV_DEBUG
 	case VIDIOC_DBG_G_REGISTER:
 	{
+#ifdef CONFIG_VIDEO_ADV_DEBUG
 		struct v4l2_dbg_register *p = arg;
 
-		if (ops->vidioc_g_register) {
-			if (!capable(CAP_SYS_ADMIN))
-				ret = -EPERM;
-			else
-				ret = ops->vidioc_g_register(file, fh, p);
-		}
+		if (!capable(CAP_SYS_ADMIN))
+			ret = -EPERM;
+		else
+			ret = ops->vidioc_g_register(file, fh, p);
+#endif
 		break;
 	}
 	case VIDIOC_DBG_S_REGISTER:
 	{
+#ifdef CONFIG_VIDEO_ADV_DEBUG
 		struct v4l2_dbg_register *p = arg;
 
-		if (ops->vidioc_s_register) {
-			if (!capable(CAP_SYS_ADMIN))
-				ret = -EPERM;
-			else
-				ret = ops->vidioc_s_register(file, fh, p);
-		}
+		if (!capable(CAP_SYS_ADMIN))
+			ret = -EPERM;
+		else
+			ret = ops->vidioc_s_register(file, fh, p);
+#endif
 		break;
 	}
-#endif
 	case VIDIOC_DBG_G_CHIP_IDENT:
 	{
 		struct v4l2_dbg_chip_ident *p = arg;
 
-		if (!ops->vidioc_g_chip_ident)
-			break;
 		p->ident = V4L2_IDENT_NONE;
 		p->revision = 0;
 		ret = ops->vidioc_g_chip_ident(file, fh, p);
@@ -1992,12 +1828,6 @@ static long __video_do_ioctl(struct file *file,
 		struct v4l2_hw_freq_seek *p = arg;
 		enum v4l2_tuner_type type;
 
-		if (!ops->vidioc_s_hw_freq_seek)
-			break;
-		if (ret_prio) {
-			ret = ret_prio;
-			break;
-		}
 		type = (vfd->vfl_type == VFL_TYPE_RADIO) ?
 			V4L2_TUNER_RADIO : V4L2_TUNER_ANALOG_TV;
 		dbgarg(cmd,
@@ -2013,9 +1843,6 @@ static long __video_do_ioctl(struct file *file,
 	{
 		struct v4l2_frmsizeenum *p = arg;
 
-		if (!ops->vidioc_enum_framesizes)
-			break;
-
 		ret = ops->vidioc_enum_framesizes(file, fh, p);
 		dbgarg(cmd,
 			"index=%d, pixelformat=%c%c%c%c, type=%d ",
@@ -2049,9 +1876,6 @@ static long __video_do_ioctl(struct file *file,
 	{
 		struct v4l2_frmivalenum *p = arg;
 
-		if (!ops->vidioc_enum_frameintervals)
-			break;
-
 		ret = ops->vidioc_enum_frameintervals(file, fh, p);
 		dbgarg(cmd,
 			"index=%d, pixelformat=%d, width=%d, height=%d, type=%d ",
@@ -2084,9 +1908,6 @@ static long __video_do_ioctl(struct file *file,
 	{
 		struct v4l2_dv_enum_preset *p = arg;
 
-		if (!ops->vidioc_enum_dv_presets)
-			break;
-
 		ret = ops->vidioc_enum_dv_presets(file, fh, p);
 		if (!ret)
 			dbgarg(cmd,
@@ -2100,13 +1921,6 @@ static long __video_do_ioctl(struct file *file,
 	{
 		struct v4l2_dv_preset *p = arg;
 
-		if (!ops->vidioc_s_dv_preset)
-			break;
-		if (ret_prio) {
-			ret = ret_prio;
-			break;
-		}
-
 		dbgarg(cmd, "preset=%d\n", p->preset);
 		ret = ops->vidioc_s_dv_preset(file, fh, p);
 		break;
@@ -2115,9 +1929,6 @@ static long __video_do_ioctl(struct file *file,
 	{
 		struct v4l2_dv_preset *p = arg;
 
-		if (!ops->vidioc_g_dv_preset)
-			break;
-
 		ret = ops->vidioc_g_dv_preset(file, fh, p);
 		if (!ret)
 			dbgarg(cmd, "preset=%d\n", p->preset);
@@ -2127,9 +1938,6 @@ static long __video_do_ioctl(struct file *file,
 	{
 		struct v4l2_dv_preset *p = arg;
 
-		if (!ops->vidioc_query_dv_preset)
-			break;
-
 		ret = ops->vidioc_query_dv_preset(file, fh, p);
 		if (!ret)
 			dbgarg(cmd, "preset=%d\n", p->preset);
@@ -2139,32 +1947,13 @@ static long __video_do_ioctl(struct file *file,
 	{
 		struct v4l2_dv_timings *p = arg;
 
-		if (!ops->vidioc_s_dv_timings)
-			break;
-		if (ret_prio) {
-			ret = ret_prio;
-			break;
-		}
-
+		dbgtimings(vfd, p);
 		switch (p->type) {
 		case V4L2_DV_BT_656_1120:
-			dbgarg2("bt-656/1120:interlaced=%d, pixelclock=%lld,"
-				" width=%d, height=%d, polarities=%x,"
-				" hfrontporch=%d, hsync=%d, hbackporch=%d,"
-				" vfrontporch=%d, vsync=%d, vbackporch=%d,"
-				" il_vfrontporch=%d, il_vsync=%d,"
-				" il_vbackporch=%d\n",
-				p->bt.interlaced, p->bt.pixelclock,
-				p->bt.width, p->bt.height, p->bt.polarities,
-				p->bt.hfrontporch, p->bt.hsync,
-				p->bt.hbackporch, p->bt.vfrontporch,
-				p->bt.vsync, p->bt.vbackporch,
-				p->bt.il_vfrontporch, p->bt.il_vsync,
-				p->bt.il_vbackporch);
 			ret = ops->vidioc_s_dv_timings(file, fh, p);
 			break;
 		default:
-			dbgarg2("Unknown type %d!\n", p->type);
+			ret = -EINVAL;
 			break;
 		}
 		break;
@@ -2173,43 +1962,68 @@ static long __video_do_ioctl(struct file *file,
 	{
 		struct v4l2_dv_timings *p = arg;
 
-		if (!ops->vidioc_g_dv_timings)
+		ret = ops->vidioc_g_dv_timings(file, fh, p);
+		if (!ret)
+			dbgtimings(vfd, p);
+		break;
+	}
+	case VIDIOC_ENUM_DV_TIMINGS:
+	{
+		struct v4l2_enum_dv_timings *p = arg;
+
+		if (!ops->vidioc_enum_dv_timings)
 			break;
 
-		ret = ops->vidioc_g_dv_timings(file, fh, p);
+		ret = ops->vidioc_enum_dv_timings(file, fh, p);
 		if (!ret) {
-			switch (p->type) {
-			case V4L2_DV_BT_656_1120:
-				dbgarg2("bt-656/1120:interlaced=%d,"
-					" pixelclock=%lld,"
-					" width=%d, height=%d, polarities=%x,"
-					" hfrontporch=%d, hsync=%d,"
-					" hbackporch=%d, vfrontporch=%d,"
-					" vsync=%d, vbackporch=%d,"
-					" il_vfrontporch=%d, il_vsync=%d,"
-					" il_vbackporch=%d\n",
-					p->bt.interlaced, p->bt.pixelclock,
-					p->bt.width, p->bt.height,
-					p->bt.polarities, p->bt.hfrontporch,
-					p->bt.hsync, p->bt.hbackporch,
-					p->bt.vfrontporch, p->bt.vsync,
-					p->bt.vbackporch, p->bt.il_vfrontporch,
-					p->bt.il_vsync, p->bt.il_vbackporch);
-				break;
-			default:
-				dbgarg2("Unknown type %d!\n", p->type);
-				break;
-			}
+			dbgarg(cmd, "index=%d: ", p->index);
+			dbgtimings(vfd, &p->timings);
 		}
 		break;
 	}
-	case VIDIOC_DQEVENT:
+	case VIDIOC_QUERY_DV_TIMINGS:
 	{
-		struct v4l2_event *ev = arg;
+		struct v4l2_dv_timings *p = arg;
+
+		if (!ops->vidioc_query_dv_timings)
+			break;
 
-		if (!ops->vidioc_subscribe_event)
+		ret = ops->vidioc_query_dv_timings(file, fh, p);
+		if (!ret)
+			dbgtimings(vfd, p);
+		break;
+	}
+	case VIDIOC_DV_TIMINGS_CAP:
+	{
+		struct v4l2_dv_timings_cap *p = arg;
+
+		if (!ops->vidioc_dv_timings_cap)
 			break;
 
+		ret = ops->vidioc_dv_timings_cap(file, fh, p);
+		if (ret)
+			break;
+		switch (p->type) {
+		case V4L2_DV_BT_656_1120:
+			dbgarg(cmd,
+			       "type=%d, width=%u-%u, height=%u-%u, "
+			       "pixelclock=%llu-%llu, standards=%x, capabilities=%x ",
+			       p->type,
+			       p->bt.min_width, p->bt.max_width,
+			       p->bt.min_height, p->bt.max_height,
+			       p->bt.min_pixelclock, p->bt.max_pixelclock,
+			       p->bt.standards, p->bt.capabilities);
+			break;
+		default:
+			dbgarg(cmd, "unknown type ");
+			break;
+		}
+		break;
+	}
+	case VIDIOC_DQEVENT:
+	{
+		struct v4l2_event *ev = arg;
+
 		ret = v4l2_event_dequeue(fh, ev, file->f_flags & O_NONBLOCK);
 		if (ret < 0) {
 			dbgarg(cmd, "no pending events?");
@@ -2226,9 +2040,6 @@ static long __video_do_ioctl(struct file *file,
 	{
 		struct v4l2_event_subscription *sub = arg;
 
-		if (!ops->vidioc_subscribe_event)
-			break;
-
 		ret = ops->vidioc_subscribe_event(fh, sub);
 		if (ret < 0) {
 			dbgarg(cmd, "failed, ret=%ld", ret);
@@ -2241,9 +2052,6 @@ static long __video_do_ioctl(struct file *file,
 	{
 		struct v4l2_event_subscription *sub = arg;
 
-		if (!ops->vidioc_unsubscribe_event)
-			break;
-
 		ret = ops->vidioc_unsubscribe_event(fh, sub);
 		if (ret < 0) {
 			dbgarg(cmd, "failed, ret=%ld", ret);
@@ -2256,12 +2064,6 @@ static long __video_do_ioctl(struct file *file,
 	{
 		struct v4l2_create_buffers *create = arg;
 
-		if (!ops->vidioc_create_bufs)
-			break;
-		if (ret_prio) {
-			ret = ret_prio;
-			break;
-		}
 		ret = check_fmt(ops, create->format.type);
 		if (ret)
 			break;
@@ -2275,8 +2077,6 @@ static long __video_do_ioctl(struct file *file,
 	{
 		struct v4l2_buffer *b = arg;
 
-		if (!ops->vidioc_prepare_buf)
-			break;
 		ret = check_fmt(ops, b->type);
 		if (ret)
 			break;
@@ -2289,7 +2089,9 @@ static long __video_do_ioctl(struct file *file,
 	default:
 		if (!ops->vidioc_default)
 			break;
-		ret = ops->vidioc_default(file, fh, ret_prio >= 0, cmd, arg);
+		ret = ops->vidioc_default(file, fh, use_fh_prio ?
+				v4l2_prio_check(vfd->prio, vfh->prio) >= 0 : 0,
+				cmd, arg);
 		break;
 	} /* switch */
 
@@ -2463,7 +2265,9 @@ video_usercopy(struct file *file, unsigned int cmd, unsigned long arg,
 			err = -EFAULT;
 		goto out_array_args;
 	}
-	if (err < 0)
+	/* VIDIOC_QUERY_DV_TIMINGS can return an error, but still have valid
+	   results that must be returned. */
+	if (err < 0 && cmd != VIDIOC_QUERY_DV_TIMINGS)
 		goto out;
 
 out_array_args:
diff --git a/drivers/media/video/v4l2-subdev.c b/drivers/media/video/v4l2-subdev.c
index 6fe88e965a8..db6e859b93d 100644
--- a/drivers/media/video/v4l2-subdev.c
+++ b/drivers/media/video/v4l2-subdev.c
@@ -35,14 +35,9 @@
 static int subdev_fh_init(struct v4l2_subdev_fh *fh, struct v4l2_subdev *sd)
 {
 #if defined(CONFIG_VIDEO_V4L2_SUBDEV_API)
-	/* Allocate try format and crop in the same memory block */
-	fh->try_fmt = kzalloc((sizeof(*fh->try_fmt) + sizeof(*fh->try_crop))
-			      * sd->entity.num_pads, GFP_KERNEL);
-	if (fh->try_fmt == NULL)
+	fh->pad = kzalloc(sizeof(*fh->pad) * sd->entity.num_pads, GFP_KERNEL);
+	if (fh->pad == NULL)
 		return -ENOMEM;
-
-	fh->try_crop = (struct v4l2_rect *)
-		(fh->try_fmt + sd->entity.num_pads);
 #endif
 	return 0;
 }
@@ -50,9 +45,8 @@ static int subdev_fh_init(struct v4l2_subdev_fh *fh, struct v4l2_subdev *sd)
 static void subdev_fh_free(struct v4l2_subdev_fh *fh)
 {
 #if defined(CONFIG_VIDEO_V4L2_SUBDEV_API)
-	kfree(fh->try_fmt);
-	fh->try_fmt = NULL;
-	fh->try_crop = NULL;
+	kfree(fh->pad);
+	fh->pad = NULL;
 #endif
 }
 
@@ -234,6 +228,8 @@ static long subdev_do_ioctl(struct file *file, unsigned int cmd, void *arg)
 
 	case VIDIOC_SUBDEV_G_CROP: {
 		struct v4l2_subdev_crop *crop = arg;
+		struct v4l2_subdev_selection sel;
+		int rval;
 
 		if (crop->which != V4L2_SUBDEV_FORMAT_TRY &&
 		    crop->which != V4L2_SUBDEV_FORMAT_ACTIVE)
@@ -242,11 +238,27 @@ static long subdev_do_ioctl(struct file *file, unsigned int cmd, void *arg)
 		if (crop->pad >= sd->entity.num_pads)
 			return -EINVAL;
 
-		return v4l2_subdev_call(sd, pad, get_crop, subdev_fh, crop);
+		rval = v4l2_subdev_call(sd, pad, get_crop, subdev_fh, crop);
+		if (rval != -ENOIOCTLCMD)
+			return rval;
+
+		memset(&sel, 0, sizeof(sel));
+		sel.which = crop->which;
+		sel.pad = crop->pad;
+		sel.target = V4L2_SUBDEV_SEL_TGT_CROP_ACTUAL;
+
+		rval = v4l2_subdev_call(
+			sd, pad, get_selection, subdev_fh, &sel);
+
+		crop->rect = sel.r;
+
+		return rval;
 	}
 
 	case VIDIOC_SUBDEV_S_CROP: {
 		struct v4l2_subdev_crop *crop = arg;
+		struct v4l2_subdev_selection sel;
+		int rval;
 
 		if (crop->which != V4L2_SUBDEV_FORMAT_TRY &&
 		    crop->which != V4L2_SUBDEV_FORMAT_ACTIVE)
@@ -255,7 +267,22 @@ static long subdev_do_ioctl(struct file *file, unsigned int cmd, void *arg)
 		if (crop->pad >= sd->entity.num_pads)
 			return -EINVAL;
 
-		return v4l2_subdev_call(sd, pad, set_crop, subdev_fh, crop);
+		rval = v4l2_subdev_call(sd, pad, set_crop, subdev_fh, crop);
+		if (rval != -ENOIOCTLCMD)
+			return rval;
+
+		memset(&sel, 0, sizeof(sel));
+		sel.which = crop->which;
+		sel.pad = crop->pad;
+		sel.target = V4L2_SUBDEV_SEL_TGT_CROP_ACTUAL;
+		sel.r = crop->rect;
+
+		rval = v4l2_subdev_call(
+			sd, pad, set_selection, subdev_fh, &sel);
+
+		crop->rect = sel.r;
+
+		return rval;
 	}
 
 	case VIDIOC_SUBDEV_ENUM_MBUS_CODE: {
@@ -293,6 +320,34 @@ static long subdev_do_ioctl(struct file *file, unsigned int cmd, void *arg)
 		return v4l2_subdev_call(sd, pad, enum_frame_interval, subdev_fh,
 					fie);
 	}
+
+	case VIDIOC_SUBDEV_G_SELECTION: {
+		struct v4l2_subdev_selection *sel = arg;
+
+		if (sel->which != V4L2_SUBDEV_FORMAT_TRY &&
+		    sel->which != V4L2_SUBDEV_FORMAT_ACTIVE)
+			return -EINVAL;
+
+		if (sel->pad >= sd->entity.num_pads)
+			return -EINVAL;
+
+		return v4l2_subdev_call(
+			sd, pad, get_selection, subdev_fh, sel);
+	}
+
+	case VIDIOC_SUBDEV_S_SELECTION: {
+		struct v4l2_subdev_selection *sel = arg;
+
+		if (sel->which != V4L2_SUBDEV_FORMAT_TRY &&
+		    sel->which != V4L2_SUBDEV_FORMAT_ACTIVE)
+			return -EINVAL;
+
+		if (sel->pad >= sd->entity.num_pads)
+			return -EINVAL;
+
+		return v4l2_subdev_call(
+			sd, pad, set_selection, subdev_fh, sel);
+	}
 #endif
 	default:
 		return v4l2_subdev_call(sd, core, ioctl, cmd, arg);
@@ -332,6 +387,70 @@ const struct v4l2_file_operations v4l2_subdev_fops = {
 	.poll = subdev_poll,
 };
 
+#ifdef CONFIG_MEDIA_CONTROLLER
+int v4l2_subdev_link_validate_default(struct v4l2_subdev *sd,
+				      struct media_link *link,
+				      struct v4l2_subdev_format *source_fmt,
+				      struct v4l2_subdev_format *sink_fmt)
+{
+	if (source_fmt->format.width != sink_fmt->format.width
+	    || source_fmt->format.height != sink_fmt->format.height
+	    || source_fmt->format.code != sink_fmt->format.code)
+		return -EINVAL;
+
+	return 0;
+}
+EXPORT_SYMBOL_GPL(v4l2_subdev_link_validate_default);
+
+static int
+v4l2_subdev_link_validate_get_format(struct media_pad *pad,
+				     struct v4l2_subdev_format *fmt)
+{
+	switch (media_entity_type(pad->entity)) {
+	case MEDIA_ENT_T_V4L2_SUBDEV:
+		fmt->which = V4L2_SUBDEV_FORMAT_ACTIVE;
+		fmt->pad = pad->index;
+		return v4l2_subdev_call(media_entity_to_v4l2_subdev(
+						pad->entity),
+					pad, get_fmt, NULL, fmt);
+	default:
+		WARN(1, "Driver bug! Wrong media entity type %d, entity %s\n",
+		     media_entity_type(pad->entity), pad->entity->name);
+		/* Fall through */
+	case MEDIA_ENT_T_DEVNODE_V4L:
+		return -EINVAL;
+	}
+}
+
+int v4l2_subdev_link_validate(struct media_link *link)
+{
+	struct v4l2_subdev *sink;
+	struct v4l2_subdev_format sink_fmt, source_fmt;
+	int rval;
+
+	rval = v4l2_subdev_link_validate_get_format(
+		link->source, &source_fmt);
+	if (rval < 0)
+		return 0;
+
+	rval = v4l2_subdev_link_validate_get_format(
+		link->sink, &sink_fmt);
+	if (rval < 0)
+		return 0;
+
+	sink = media_entity_to_v4l2_subdev(link->sink->entity);
+
+	rval = v4l2_subdev_call(sink, pad, link_validate, link,
+				&source_fmt, &sink_fmt);
+	if (rval != -ENOIOCTLCMD)
+		return rval;
+
+	return v4l2_subdev_link_validate_default(
+		sink, link, &source_fmt, &sink_fmt);
+}
+EXPORT_SYMBOL_GPL(v4l2_subdev_link_validate);
+#endif /* CONFIG_MEDIA_CONTROLLER */
+
 void v4l2_subdev_init(struct v4l2_subdev *sd, const struct v4l2_subdev_ops *ops)
 {
 	INIT_LIST_HEAD(&sd->list);
diff --git a/drivers/media/video/via-camera.c b/drivers/media/video/via-camera.c
index 20f7237b824..308e150a39b 100644
--- a/drivers/media/video/via-camera.c
+++ b/drivers/media/video/via-camera.c
@@ -18,6 +18,7 @@
 #include <media/v4l2-device.h>
 #include <media/v4l2-ioctl.h>
 #include <media/v4l2-chip-ident.h>
+#include <media/ov7670.h>
 #include <media/videobuf-dma-sg.h>
 #include <linux/delay.h>
 #include <linux/dma-mapping.h>
@@ -1347,11 +1348,21 @@ static __devinit bool viacam_serial_is_enabled(void)
 	return false;
 }
 
+static struct ov7670_config sensor_cfg = {
+	/* The XO-1.5 (only known user) clocks the camera at 90MHz. */
+	.clock_speed = 90,
+};
+
 static __devinit int viacam_probe(struct platform_device *pdev)
 {
 	int ret;
 	struct i2c_adapter *sensor_adapter;
 	struct viafb_dev *viadev = pdev->dev.platform_data;
+	struct i2c_board_info ov7670_info = {
+		.type = "ov7670",
+		.addr = 0x42 >> 1,
+		.platform_data = &sensor_cfg,
+	};
 
 	/*
 	 * Note that there are actually two capture channels on
@@ -1433,8 +1444,8 @@ static __devinit int viacam_probe(struct platform_device *pdev)
 	 * is OLPC-specific.  0x42 assumption is ov7670-specific.
 	 */
 	sensor_adapter = viafb_find_i2c_adapter(VIA_PORT_31);
-	cam->sensor = v4l2_i2c_new_subdev(&cam->v4l2_dev, sensor_adapter,
-			"ov7670", 0x42 >> 1, NULL);
+	cam->sensor = v4l2_i2c_new_subdev_board(&cam->v4l2_dev, sensor_adapter,
+			&ov7670_info, NULL);
 	if (cam->sensor == NULL) {
 		dev_err(&pdev->dev, "Unable to find the sensor!\n");
 		ret = -ENODEV;
diff --git a/drivers/media/video/videobuf-core.c b/drivers/media/video/videobuf-core.c
index de4fa4eb884..ffdf59cfe40 100644
--- a/drivers/media/video/videobuf-core.c
+++ b/drivers/media/video/videobuf-core.c
@@ -1129,6 +1129,7 @@ unsigned int videobuf_poll_stream(struct file *file,
 				  struct videobuf_queue *q,
 				  poll_table *wait)
 {
+	unsigned long req_events = poll_requested_events(wait);
 	struct videobuf_buffer *buf = NULL;
 	unsigned int rc = 0;
 
@@ -1137,7 +1138,7 @@ unsigned int videobuf_poll_stream(struct file *file,
 		if (!list_empty(&q->stream))
 			buf = list_entry(q->stream.next,
 					 struct videobuf_buffer, stream);
-	} else {
+	} else if (req_events & (POLLIN | POLLRDNORM)) {
 		if (!q->reading)
 			__videobuf_read_start(q);
 		if (!q->reading) {
diff --git a/drivers/media/video/videobuf-dma-contig.c b/drivers/media/video/videobuf-dma-contig.c
index c9691115f2d..b6b5cc1a43c 100644
--- a/drivers/media/video/videobuf-dma-contig.c
+++ b/drivers/media/video/videobuf-dma-contig.c
@@ -27,6 +27,7 @@ struct videobuf_dma_contig_memory {
 	u32 magic;
 	void *vaddr;
 	dma_addr_t dma_handle;
+	bool cached;
 	unsigned long size;
 };
 
@@ -37,8 +38,58 @@ struct videobuf_dma_contig_memory {
 		BUG();							    \
 	}
 
-static void
-videobuf_vm_open(struct vm_area_struct *vma)
+static int __videobuf_dc_alloc(struct device *dev,
+			       struct videobuf_dma_contig_memory *mem,
+			       unsigned long size, unsigned long 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 = 0;
+				return err;
+			}
+		}
+	} else
+		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);
+		return -ENOMEM;
+	}
+
+	dev_dbg(dev, "dma mapped data is at %p (%ld)\n", mem->vaddr, mem->size);
+
+	return 0;
+}
+
+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);
+
+	mem->vaddr = NULL;
+}
+
+static void videobuf_vm_open(struct vm_area_struct *vma)
 {
 	struct videobuf_mapping *map = vma->vm_private_data;
 
@@ -91,12 +142,11 @@ static void videobuf_vm_close(struct vm_area_struct *vma)
 				dev_dbg(q->dev, "buf[%d] freeing %p\n",
 					i, mem->vaddr);
 
-				dma_free_coherent(q->dev, mem->size,
-						  mem->vaddr, mem->dma_handle);
+				__videobuf_dc_free(q->dev, mem);
 				mem->vaddr = NULL;
 			}
 
-			q->bufs[i]->map   = NULL;
+			q->bufs[i]->map = NULL;
 			q->bufs[i]->baddr = 0;
 		}
 
@@ -107,8 +157,8 @@ static void videobuf_vm_close(struct vm_area_struct *vma)
 }
 
 static const struct vm_operations_struct videobuf_vm_ops = {
-	.open     = videobuf_vm_open,
-	.close    = videobuf_vm_close,
+	.open	= videobuf_vm_open,
+	.close	= videobuf_vm_close,
 };
 
 /**
@@ -178,26 +228,38 @@ static int videobuf_dma_contig_user_get(struct videobuf_dma_contig_memory *mem,
 		pages_done++;
 	}
 
- out_up:
+out_up:
 	up_read(&current->mm->mmap_sem);
 
 	return ret;
 }
 
-static struct videobuf_buffer *__videobuf_alloc_vb(size_t size)
+static struct videobuf_buffer *__videobuf_alloc_vb(size_t size, bool cached)
 {
 	struct videobuf_dma_contig_memory *mem;
 	struct videobuf_buffer *vb;
 
 	vb = kzalloc(size + sizeof(*mem), GFP_KERNEL);
 	if (vb) {
-		mem = vb->priv = ((char *)vb) + size;
+		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;
@@ -235,28 +297,32 @@ static int __videobuf_iolock(struct videobuf_queue *q,
 			return videobuf_dma_contig_user_get(mem, vb);
 
 		/* allocate memory for the read() method */
-		mem->size = PAGE_ALIGN(vb->size);
-		mem->vaddr = dma_alloc_coherent(q->dev, mem->size,
-						&mem->dma_handle, GFP_KERNEL);
-		if (!mem->vaddr) {
-			dev_err(q->dev, "dma_alloc_coherent %ld failed\n",
-					 mem->size);
+		if (__videobuf_dc_alloc(q->dev, mem, PAGE_ALIGN(vb->size),
+					GFP_KERNEL))
 			return -ENOMEM;
-		}
-
-		dev_dbg(q->dev, "dma_alloc_coherent data is at %p (%ld)\n",
-			mem->vaddr, mem->size);
 		break;
 	case V4L2_MEMORY_OVERLAY:
 	default:
-		dev_dbg(q->dev, "%s memory method OVERLAY/unknown\n",
-			__func__);
+		dev_dbg(q->dev, "%s memory method OVERLAY/unknown\n", __func__);
 		return -EINVAL;
 	}
 
 	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)
@@ -265,6 +331,8 @@ 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__);
 
@@ -282,41 +350,50 @@ static int __videobuf_mmap_mapper(struct videobuf_queue *q,
 	BUG_ON(!mem);
 	MAGIC_CHECK(mem->magic, MAGIC_DC_MEM);
 
-	mem->size = PAGE_ALIGN(buf->bsize);
-	mem->vaddr = dma_alloc_coherent(q->dev, mem->size,
-					&mem->dma_handle, GFP_KERNEL);
-	if (!mem->vaddr) {
-		dev_err(q->dev, "dma_alloc_coherent size %ld failed\n",
-			mem->size);
+	if (__videobuf_dc_alloc(q->dev, mem, PAGE_ALIGN(buf->bsize),
+				GFP_KERNEL | __GFP_COMP))
 		goto error;
-	}
-	dev_dbg(q->dev, "dma_alloc_coherent data is at addr %p (size %ld)\n",
-		mem->vaddr, mem->size);
 
 	/* Try to remap memory */
 
 	size = vma->vm_end - vma->vm_start;
 	size = (size < mem->size) ? size : mem->size;
 
-	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;
+	if (!mem->cached)
+		vma->vm_page_prot = pgprot_noncached(vma->vm_page_prot);
+
+	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;
 	}
 
-	vma->vm_ops          = &videobuf_vm_ops;
-	vma->vm_flags       |= VM_DONTEXPAND;
+	vma->vm_ops = &videobuf_vm_ops;
+	vma->vm_flags |= VM_DONTEXPAND;
 	vma->vm_private_data = map;
 
 	dev_dbg(q->dev, "mmap %p: q=%p %08lx-%08lx (%lx) pgoff %08lx buf %d\n",
 		map, q, vma->vm_start, vma->vm_end,
-		(long int)buf->bsize,
-		vma->vm_pgoff, buf->i);
+		(long int)buf->bsize, vma->vm_pgoff, buf->i);
 
 	videobuf_vm_open(vma);
 
@@ -328,12 +405,20 @@ error:
 }
 
 static struct videobuf_qtype_ops qops = {
-	.magic        = MAGIC_QTYPE_OPS,
+	.magic		= MAGIC_QTYPE_OPS,
+	.alloc_vb	= __videobuf_alloc_uncached,
+	.iolock		= __videobuf_iolock,
+	.mmap_mapper	= __videobuf_mmap_mapper,
+	.vaddr		= __videobuf_to_vaddr,
+};
 
-	.alloc_vb     = __videobuf_alloc_vb,
-	.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,
+	.iolock		= __videobuf_iolock,
+	.sync		= __videobuf_sync,
+	.mmap_mapper	= __videobuf_mmap_mapper,
+	.vaddr		= __videobuf_to_vaddr,
 };
 
 void videobuf_queue_dma_contig_init(struct videobuf_queue *q,
@@ -351,6 +436,20 @@ 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;
@@ -389,7 +488,7 @@ void videobuf_dma_contig_free(struct videobuf_queue *q,
 
 	/* read() method */
 	if (mem->vaddr) {
-		dma_free_coherent(q->dev, mem->size, mem->vaddr, mem->dma_handle);
+		__videobuf_dc_free(q->dev, mem);
 		mem->vaddr = NULL;
 	}
 }
diff --git a/drivers/media/video/videobuf-dvb.c b/drivers/media/video/videobuf-dvb.c
index 59cb54aa294..94d83a41381 100644
--- a/drivers/media/video/videobuf-dvb.c
+++ b/drivers/media/video/videobuf-dvb.c
@@ -45,7 +45,6 @@ static int videobuf_dvb_thread(void *data)
 	struct videobuf_dvb *dvb = data;
 	struct videobuf_buffer *buf;
 	unsigned long flags;
-	int err;
 	void *outp;
 
 	dprintk("dvb thread started\n");
@@ -57,7 +56,7 @@ static int videobuf_dvb_thread(void *data)
 		buf = list_entry(dvb->dvbq.stream.next,
 				 struct videobuf_buffer, stream);
 		list_del(&buf->stream);
-		err = videobuf_waiton(&dvb->dvbq, buf, 0, 1);
+		videobuf_waiton(&dvb->dvbq, buf, 0, 1);
 
 		/* no more feeds left or stop_feed() asked us to quit */
 		if (0 == dvb->nfeeds)
diff --git a/drivers/media/video/videobuf2-core.c b/drivers/media/video/videobuf2-core.c
index 2e8f1df775b..9d4e9edbd2e 100644
--- a/drivers/media/video/videobuf2-core.c
+++ b/drivers/media/video/videobuf2-core.c
@@ -19,6 +19,9 @@
 #include <linux/slab.h>
 #include <linux/sched.h>
 
+#include <media/v4l2-dev.h>
+#include <media/v4l2-fh.h>
+#include <media/v4l2-event.h>
 #include <media/videobuf2-core.h>
 
 static int debug;
@@ -1642,32 +1645,46 @@ static int __vb2_cleanup_fileio(struct vb2_queue *q);
  * For OUTPUT queues, if a buffer is ready to be dequeued, the file descriptor
  * will be reported as available for writing.
  *
+ * If the driver uses struct v4l2_fh, then vb2_poll() will also check for any
+ * pending events.
+ *
  * The return values from this function are intended to be directly returned
  * from poll handler in driver.
  */
 unsigned int vb2_poll(struct vb2_queue *q, struct file *file, poll_table *wait)
 {
-	unsigned long flags;
-	unsigned int ret;
+	struct video_device *vfd = video_devdata(file);
+	unsigned long req_events = poll_requested_events(wait);
 	struct vb2_buffer *vb = NULL;
+	unsigned int res = 0;
+	unsigned long flags;
+
+	if (test_bit(V4L2_FL_USES_V4L2_FH, &vfd->flags)) {
+		struct v4l2_fh *fh = file->private_data;
+
+		if (v4l2_event_pending(fh))
+			res = POLLPRI;
+		else if (req_events & POLLPRI)
+			poll_wait(file, &fh->wait, wait);
+	}
 
 	/*
 	 * Start file I/O emulator only if streaming API has not been used yet.
 	 */
 	if (q->num_buffers == 0 && q->fileio == NULL) {
-		if (!V4L2_TYPE_IS_OUTPUT(q->type) && (q->io_modes & VB2_READ)) {
-			ret = __vb2_init_fileio(q, 1);
-			if (ret)
-				return POLLERR;
+		if (!V4L2_TYPE_IS_OUTPUT(q->type) && (q->io_modes & VB2_READ) &&
+				(req_events & (POLLIN | POLLRDNORM))) {
+			if (__vb2_init_fileio(q, 1))
+				return res | POLLERR;
 		}
-		if (V4L2_TYPE_IS_OUTPUT(q->type) && (q->io_modes & VB2_WRITE)) {
-			ret = __vb2_init_fileio(q, 0);
-			if (ret)
-				return POLLERR;
+		if (V4L2_TYPE_IS_OUTPUT(q->type) && (q->io_modes & VB2_WRITE) &&
+				(req_events & (POLLOUT | POLLWRNORM))) {
+			if (__vb2_init_fileio(q, 0))
+				return res | POLLERR;
 			/*
 			 * Write to OUTPUT queue can be done immediately.
 			 */
-			return POLLOUT | POLLWRNORM;
+			return res | POLLOUT | POLLWRNORM;
 		}
 	}
 
@@ -1675,7 +1692,7 @@ unsigned int vb2_poll(struct vb2_queue *q, struct file *file, poll_table *wait)
 	 * There is nothing to wait for if no buffers have already been queued.
 	 */
 	if (list_empty(&q->queued_list))
-		return POLLERR;
+		return res | POLLERR;
 
 	poll_wait(file, &q->done_wq, wait);
 
@@ -1690,10 +1707,11 @@ unsigned int vb2_poll(struct vb2_queue *q, struct file *file, poll_table *wait)
 
 	if (vb && (vb->state == VB2_BUF_STATE_DONE
 			|| vb->state == VB2_BUF_STATE_ERROR)) {
-		return (V4L2_TYPE_IS_OUTPUT(q->type)) ? POLLOUT | POLLWRNORM :
-			POLLIN | POLLRDNORM;
+		return (V4L2_TYPE_IS_OUTPUT(q->type)) ?
+				res | POLLOUT | POLLWRNORM :
+				res | POLLIN | POLLRDNORM;
 	}
-	return 0;
+	return res;
 }
 EXPORT_SYMBOL_GPL(vb2_poll);
 
@@ -1839,7 +1857,6 @@ static int __vb2_init_fileio(struct vb2_queue *q, int read)
 	 * (multiplane buffers are not supported).
 	 */
 	if (q->bufs[0]->num_planes != 1) {
-		fileio->req.count = 0;
 		ret = -EBUSY;
 		goto err_reqbufs;
 	}
@@ -1886,6 +1903,7 @@ static int __vb2_init_fileio(struct vb2_queue *q, int read)
 	return ret;
 
 err_reqbufs:
+	fileio->req.count = 0;
 	vb2_reqbufs(q, &fileio->req);
 
 err_kfree:
diff --git a/drivers/media/video/videobuf2-dma-contig.c b/drivers/media/video/videobuf2-dma-contig.c
index f17ad98fcc5..4b7132660a9 100644
--- a/drivers/media/video/videobuf2-dma-contig.c
+++ b/drivers/media/video/videobuf2-dma-contig.c
@@ -15,6 +15,7 @@
 #include <linux/dma-mapping.h>
 
 #include <media/videobuf2-core.h>
+#include <media/videobuf2-dma-contig.h>
 #include <media/videobuf2-memops.h>
 
 struct vb2_dc_conf {
@@ -85,7 +86,7 @@ static void *vb2_dma_contig_vaddr(void *buf_priv)
 {
 	struct vb2_dc_buf *buf = buf_priv;
 	if (!buf)
-		return 0;
+		return NULL;
 
 	return buf->vaddr;
 }
diff --git a/drivers/media/video/videobuf2-memops.c b/drivers/media/video/videobuf2-memops.c
index c41cb60245d..504cd4cbe29 100644
--- a/drivers/media/video/videobuf2-memops.c
+++ b/drivers/media/video/videobuf2-memops.c
@@ -55,6 +55,7 @@ struct vm_area_struct *vb2_get_vma(struct vm_area_struct *vma)
 
 	return vma_copy;
 }
+EXPORT_SYMBOL_GPL(vb2_get_vma);
 
 /**
  * vb2_put_userptr() - release a userspace virtual memory area
diff --git a/drivers/media/video/vivi.c b/drivers/media/video/vivi.c
index 5e8b0710105..0960d7f0d39 100644
--- a/drivers/media/video/vivi.c
+++ b/drivers/media/video/vivi.c
@@ -95,6 +95,16 @@ static struct vivi_fmt formats[] = {
 		.depth    = 16,
 	},
 	{
+		.name     = "4:2:2, packed, YVYU",
+		.fourcc   = V4L2_PIX_FMT_YVYU,
+		.depth    = 16,
+	},
+	{
+		.name     = "4:2:2, packed, VYUY",
+		.fourcc   = V4L2_PIX_FMT_VYUY,
+		.depth    = 16,
+	},
+	{
 		.name     = "RGB565 (LE)",
 		.fourcc   = V4L2_PIX_FMT_RGB565, /* gggbbbbb rrrrrggg */
 		.depth    = 16,
@@ -114,6 +124,26 @@ static struct vivi_fmt formats[] = {
 		.fourcc   = V4L2_PIX_FMT_RGB555X, /* arrrrrgg gggbbbbb */
 		.depth    = 16,
 	},
+	{
+		.name     = "RGB24 (LE)",
+		.fourcc   = V4L2_PIX_FMT_RGB24, /* rgb */
+		.depth    = 24,
+	},
+	{
+		.name     = "RGB24 (BE)",
+		.fourcc   = V4L2_PIX_FMT_BGR24, /* bgr */
+		.depth    = 24,
+	},
+	{
+		.name     = "RGB32 (LE)",
+		.fourcc   = V4L2_PIX_FMT_RGB32, /* argb */
+		.depth    = 32,
+	},
+	{
+		.name     = "RGB32 (BE)",
+		.fourcc   = V4L2_PIX_FMT_BGR32, /* bgra */
+		.depth    = 32,
+	},
 };
 
 static struct vivi_fmt *get_format(struct v4l2_format *f)
@@ -170,6 +200,7 @@ struct vivi_dev {
 		struct v4l2_ctrl	   *gain;
 	};
 	struct v4l2_ctrl	   *volume;
+	struct v4l2_ctrl	   *alpha;
 	struct v4l2_ctrl	   *button;
 	struct v4l2_ctrl	   *boolean;
 	struct v4l2_ctrl	   *int32;
@@ -177,6 +208,7 @@ struct vivi_dev {
 	struct v4l2_ctrl	   *menu;
 	struct v4l2_ctrl	   *string;
 	struct v4l2_ctrl	   *bitmask;
+	struct v4l2_ctrl	   *int_menu;
 
 	spinlock_t                 slock;
 	struct mutex		   mutex;
@@ -203,8 +235,10 @@ struct vivi_dev {
 	enum v4l2_field		   field;
 	unsigned int		   field_count;
 
-	u8 			   bars[9][3];
-	u8 			   line[MAX_WIDTH * 4];
+	u8			   bars[9][3];
+	u8			   line[MAX_WIDTH * 8];
+	unsigned int		   pixelsize;
+	u8			   alpha_component;
 };
 
 /* ------------------------------------------------------------------
@@ -283,6 +317,8 @@ static void precalculate_bars(struct vivi_dev *dev)
 		switch (dev->fmt->fourcc) {
 		case V4L2_PIX_FMT_YUYV:
 		case V4L2_PIX_FMT_UYVY:
+		case V4L2_PIX_FMT_YVYU:
+		case V4L2_PIX_FMT_VYUY:
 			is_yuv = 1;
 			break;
 		case V4L2_PIX_FMT_RGB565:
@@ -297,6 +333,11 @@ static void precalculate_bars(struct vivi_dev *dev)
 			g >>= 3;
 			b >>= 3;
 			break;
+		case V4L2_PIX_FMT_RGB24:
+		case V4L2_PIX_FMT_BGR24:
+		case V4L2_PIX_FMT_RGB32:
+		case V4L2_PIX_FMT_BGR32:
+			break;
 		}
 
 		if (is_yuv) {
@@ -316,9 +357,11 @@ static void precalculate_bars(struct vivi_dev *dev)
 #define TSTAMP_INPUT_X	10
 #define TSTAMP_MIN_X	(54 + TSTAMP_INPUT_X)
 
-static void gen_twopix(struct vivi_dev *dev, u8 *buf, int colorpos)
+/* 'odd' is true for pixels 1, 3, 5, etc. and false for pixels 0, 2, 4, etc. */
+static void gen_twopix(struct vivi_dev *dev, u8 *buf, int colorpos, bool odd)
 {
 	u8 r_y, g_u, b_v;
+	u8 alpha = dev->alpha_component;
 	int color;
 	u8 *p;
 
@@ -326,46 +369,56 @@ static void gen_twopix(struct vivi_dev *dev, u8 *buf, int colorpos)
 	g_u = dev->bars[colorpos][1]; /* G or precalculated U */
 	b_v = dev->bars[colorpos][2]; /* B or precalculated V */
 
-	for (color = 0; color < 4; color++) {
+	for (color = 0; color < dev->pixelsize; color++) {
 		p = buf + color;
 
 		switch (dev->fmt->fourcc) {
 		case V4L2_PIX_FMT_YUYV:
 			switch (color) {
 			case 0:
-			case 2:
 				*p = r_y;
 				break;
 			case 1:
-				*p = g_u;
-				break;
-			case 3:
-				*p = b_v;
+				*p = odd ? b_v : g_u;
 				break;
 			}
 			break;
 		case V4L2_PIX_FMT_UYVY:
 			switch (color) {
+			case 0:
+				*p = odd ? b_v : g_u;
+				break;
 			case 1:
-			case 3:
 				*p = r_y;
 				break;
+			}
+			break;
+		case V4L2_PIX_FMT_YVYU:
+			switch (color) {
 			case 0:
-				*p = g_u;
+				*p = r_y;
 				break;
-			case 2:
-				*p = b_v;
+			case 1:
+				*p = odd ? g_u : b_v;
+				break;
+			}
+			break;
+		case V4L2_PIX_FMT_VYUY:
+			switch (color) {
+			case 0:
+				*p = odd ? g_u : b_v;
+				break;
+			case 1:
+				*p = r_y;
 				break;
 			}
 			break;
 		case V4L2_PIX_FMT_RGB565:
 			switch (color) {
 			case 0:
-			case 2:
 				*p = (g_u << 5) | b_v;
 				break;
 			case 1:
-			case 3:
 				*p = (r_y << 3) | (g_u >> 3);
 				break;
 			}
@@ -373,11 +426,9 @@ static void gen_twopix(struct vivi_dev *dev, u8 *buf, int colorpos)
 		case V4L2_PIX_FMT_RGB565X:
 			switch (color) {
 			case 0:
-			case 2:
 				*p = (r_y << 3) | (g_u >> 3);
 				break;
 			case 1:
-			case 3:
 				*p = (g_u << 5) | b_v;
 				break;
 			}
@@ -385,24 +436,78 @@ static void gen_twopix(struct vivi_dev *dev, u8 *buf, int colorpos)
 		case V4L2_PIX_FMT_RGB555:
 			switch (color) {
 			case 0:
-			case 2:
 				*p = (g_u << 5) | b_v;
 				break;
 			case 1:
-			case 3:
-				*p = (r_y << 2) | (g_u >> 3);
+				*p = (alpha & 0x80) | (r_y << 2) | (g_u >> 3);
 				break;
 			}
 			break;
 		case V4L2_PIX_FMT_RGB555X:
 			switch (color) {
 			case 0:
+				*p = (alpha & 0x80) | (r_y << 2) | (g_u >> 3);
+				break;
+			case 1:
+				*p = (g_u << 5) | b_v;
+				break;
+			}
+			break;
+		case V4L2_PIX_FMT_RGB24:
+			switch (color) {
+			case 0:
+				*p = r_y;
+				break;
+			case 1:
+				*p = g_u;
+				break;
 			case 2:
-				*p = (r_y << 2) | (g_u >> 3);
+				*p = b_v;
+				break;
+			}
+			break;
+		case V4L2_PIX_FMT_BGR24:
+			switch (color) {
+			case 0:
+				*p = b_v;
 				break;
 			case 1:
+				*p = g_u;
+				break;
+			case 2:
+				*p = r_y;
+				break;
+			}
+			break;
+		case V4L2_PIX_FMT_RGB32:
+			switch (color) {
+			case 0:
+				*p = alpha;
+				break;
+			case 1:
+				*p = r_y;
+				break;
+			case 2:
+				*p = g_u;
+				break;
 			case 3:
-				*p = (g_u << 5) | b_v;
+				*p = b_v;
+				break;
+			}
+			break;
+		case V4L2_PIX_FMT_BGR32:
+			switch (color) {
+			case 0:
+				*p = b_v;
+				break;
+			case 1:
+				*p = g_u;
+				break;
+			case 2:
+				*p = r_y;
+				break;
+			case 3:
+				*p = alpha;
 				break;
 			}
 			break;
@@ -414,10 +519,10 @@ static void precalculate_line(struct vivi_dev *dev)
 {
 	int w;
 
-	for (w = 0; w < dev->width * 2; w += 2) {
-		int colorpos = (w / (dev->width / 8) % 8);
+	for (w = 0; w < dev->width * 2; w++) {
+		int colorpos = w / (dev->width / 8) % 8;
 
-		gen_twopix(dev, dev->line + w * 2, colorpos);
+		gen_twopix(dev, dev->line + w * dev->pixelsize, colorpos, w & 1);
 	}
 }
 
@@ -433,7 +538,7 @@ static void gen_text(struct vivi_dev *dev, char *basep,
 	/* Print stream time */
 	for (line = y; line < y + 16; line++) {
 		int j = 0;
-		char *pos = basep + line * dev->width * 2 + x * 2;
+		char *pos = basep + line * dev->width * dev->pixelsize + x * dev->pixelsize;
 		char *s;
 
 		for (s = text; *s; s++) {
@@ -443,9 +548,9 @@ static void gen_text(struct vivi_dev *dev, char *basep,
 			for (i = 0; i < 7; i++, j++) {
 				/* Draw white font on black background */
 				if (chr & (1 << (7 - i)))
-					gen_twopix(dev, pos + j * 2, WHITE);
+					gen_twopix(dev, pos + j * dev->pixelsize, WHITE, (x+y) & 1);
 				else
-					gen_twopix(dev, pos + j * 2, TEXT_BLACK);
+					gen_twopix(dev, pos + j * dev->pixelsize, TEXT_BLACK, (x+y) & 1);
 			}
 		}
 	}
@@ -466,7 +571,9 @@ static void vivi_fillbuff(struct vivi_dev *dev, struct vivi_buffer *buf)
 		return;
 
 	for (h = 0; h < hmax; h++)
-		memcpy(vbuf + h * wmax * 2, dev->line + (dev->mv_count % wmax) * 2, wmax * 2);
+		memcpy(vbuf + h * wmax * dev->pixelsize,
+		       dev->line + (dev->mv_count % wmax) * dev->pixelsize,
+		       wmax * dev->pixelsize);
 
 	/* Updates stream time */
 
@@ -484,15 +591,16 @@ static void vivi_fillbuff(struct vivi_dev *dev, struct vivi_buffer *buf)
 	gen_text(dev, vbuf, line++ * 16, 16, str);
 
 	gain = v4l2_ctrl_g_ctrl(dev->gain);
-	mutex_lock(&dev->ctrl_handler.lock);
+	mutex_lock(dev->ctrl_handler.lock);
 	snprintf(str, sizeof(str), " brightness %3d, contrast %3d, saturation %3d, hue %d ",
 			dev->brightness->cur.val,
 			dev->contrast->cur.val,
 			dev->saturation->cur.val,
 			dev->hue->cur.val);
 	gen_text(dev, vbuf, line++ * 16, 16, str);
-	snprintf(str, sizeof(str), " autogain %d, gain %3d, volume %3d ",
-			dev->autogain->cur.val, gain, dev->volume->cur.val);
+	snprintf(str, sizeof(str), " autogain %d, gain %3d, volume %3d, alpha 0x%02x ",
+			dev->autogain->cur.val, gain, dev->volume->cur.val,
+			dev->alpha->cur.val);
 	gen_text(dev, vbuf, line++ * 16, 16, str);
 	snprintf(str, sizeof(str), " int32 %d, int64 %lld, bitmask %08x ",
 			dev->int32->cur.val,
@@ -503,8 +611,12 @@ static void vivi_fillbuff(struct vivi_dev *dev, struct vivi_buffer *buf)
 			dev->boolean->cur.val,
 			dev->menu->qmenu[dev->menu->cur.val],
 			dev->string->cur.string);
-	mutex_unlock(&dev->ctrl_handler.lock);
 	gen_text(dev, vbuf, line++ * 16, 16, str);
+	snprintf(str, sizeof(str), " integer_menu %lld, value %d ",
+			dev->int_menu->qmenu_int[dev->int_menu->cur.val],
+			dev->int_menu->cur.val);
+	gen_text(dev, vbuf, line++ * 16, 16, str);
+	mutex_unlock(dev->ctrl_handler.lock);
 	if (dev->button_pressed) {
 		dev->button_pressed--;
 		snprintf(str, sizeof(str), " button pressed!");
@@ -657,7 +769,7 @@ static int queue_setup(struct vb2_queue *vq, const struct v4l2_format *fmt,
 	struct vivi_dev *dev = vb2_get_drv_priv(vq);
 	unsigned long size;
 
-	size = dev->width * dev->height * 2;
+	size = dev->width * dev->height * dev->pixelsize;
 
 	if (0 == *nbuffers)
 		*nbuffers = 32;
@@ -721,7 +833,7 @@ static int buffer_prepare(struct vb2_buffer *vb)
 	    dev->height < 32 || dev->height > MAX_HEIGHT)
 		return -EINVAL;
 
-	size = dev->width * dev->height * 2;
+	size = dev->width * dev->height * dev->pixelsize;
 	if (vb2_plane_size(vb, 0) < size) {
 		dprintk(dev, 1, "%s data will not fit into plane (%lu < %lu)\n",
 				__func__, vb2_plane_size(vb, 0), size);
@@ -915,6 +1027,7 @@ static int vidioc_s_fmt_vid_cap(struct file *file, void *priv,
 	}
 
 	dev->fmt = get_format(f);
+	dev->pixelsize = dev->fmt->depth / 8;
 	dev->width = f->fmt.pix.width;
 	dev->height = f->fmt.pix.height;
 	dev->field = f->fmt.pix.field;
@@ -1016,8 +1129,15 @@ static int vivi_s_ctrl(struct v4l2_ctrl *ctrl)
 {
 	struct vivi_dev *dev = container_of(ctrl->handler, struct vivi_dev, ctrl_handler);
 
-	if (ctrl == dev->button)
-		dev->button_pressed = 30;
+	switch (ctrl->id) {
+	case V4L2_CID_ALPHA_COMPONENT:
+		dev->alpha_component = ctrl->val;
+		break;
+	default:
+		if (ctrl == dev->button)
+			dev->button_pressed = 30;
+		break;
+	}
 	return 0;
 }
 
@@ -1039,17 +1159,10 @@ static unsigned int
 vivi_poll(struct file *file, struct poll_table_struct *wait)
 {
 	struct vivi_dev *dev = video_drvdata(file);
-	struct v4l2_fh *fh = file->private_data;
 	struct vb2_queue *q = &dev->vb_vidq;
-	unsigned int res;
 
 	dprintk(dev, 1, "%s\n", __func__);
-	res = vb2_poll(q, file, wait);
-	if (v4l2_event_pending(fh))
-		res |= POLLPRI;
-	else
-		poll_wait(file, &fh->wait, wait);
-	return res;
+	return vb2_poll(q, file, wait);
 }
 
 static int vivi_close(struct file *file)
@@ -1165,6 +1278,22 @@ static const struct v4l2_ctrl_config vivi_ctrl_bitmask = {
 	.step = 0,
 };
 
+static const s64 vivi_ctrl_int_menu_values[] = {
+	1, 1, 2, 3, 5, 8, 13, 21, 42,
+};
+
+static const struct v4l2_ctrl_config vivi_ctrl_int_menu = {
+	.ops = &vivi_ctrl_ops,
+	.id = VIVI_CID_CUSTOM_BASE + 7,
+	.name = "Integer menu",
+	.type = V4L2_CTRL_TYPE_INTEGER_MENU,
+	.min = 1,
+	.max = 8,
+	.def = 4,
+	.menu_skip_mask = 0x02,
+	.qmenu_int = vivi_ctrl_int_menu_values,
+};
+
 static const struct v4l2_file_operations vivi_fops = {
 	.owner		= THIS_MODULE,
 	.open           = v4l2_fh_open,
@@ -1252,6 +1381,7 @@ static int __init vivi_create_instance(int inst)
 	dev->fmt = &formats[0];
 	dev->width = 640;
 	dev->height = 480;
+	dev->pixelsize = dev->fmt->depth / 8;
 	hdl = &dev->ctrl_handler;
 	v4l2_ctrl_handler_init(hdl, 11);
 	dev->volume = v4l2_ctrl_new_std(hdl, &vivi_ctrl_ops,
@@ -1268,6 +1398,8 @@ static int __init vivi_create_instance(int inst)
 			V4L2_CID_AUTOGAIN, 0, 1, 1, 1);
 	dev->gain = v4l2_ctrl_new_std(hdl, &vivi_ctrl_ops,
 			V4L2_CID_GAIN, 0, 255, 1, 100);
+	dev->alpha = v4l2_ctrl_new_std(hdl, &vivi_ctrl_ops,
+			V4L2_CID_ALPHA_COMPONENT, 0, 255, 1, 0);
 	dev->button = v4l2_ctrl_new_custom(hdl, &vivi_ctrl_button, NULL);
 	dev->int32 = v4l2_ctrl_new_custom(hdl, &vivi_ctrl_int32, NULL);
 	dev->int64 = v4l2_ctrl_new_custom(hdl, &vivi_ctrl_int64, NULL);
@@ -1275,6 +1407,7 @@ static int __init vivi_create_instance(int inst)
 	dev->menu = v4l2_ctrl_new_custom(hdl, &vivi_ctrl_menu, NULL);
 	dev->string = v4l2_ctrl_new_custom(hdl, &vivi_ctrl_string, NULL);
 	dev->bitmask = v4l2_ctrl_new_custom(hdl, &vivi_ctrl_bitmask, NULL);
+	dev->int_menu = v4l2_ctrl_new_custom(hdl, &vivi_ctrl_int_menu, NULL);
 	if (hdl->error) {
 		ret = hdl->error;
 		goto unreg_dev;
diff --git a/drivers/media/video/w9966.c b/drivers/media/video/w9966.c
index 7fd7ac567e1..db2a6003a1c 100644
--- a/drivers/media/video/w9966.c
+++ b/drivers/media/video/w9966.c
@@ -62,6 +62,9 @@
 #include <media/v4l2-common.h>
 #include <media/v4l2-ioctl.h>
 #include <media/v4l2-device.h>
+#include <media/v4l2-fh.h>
+#include <media/v4l2-ctrls.h>
+#include <media/v4l2-event.h>
 #include <linux/parport.h>
 
 /*#define DEBUG*/				/* Undef me for production */
@@ -104,6 +107,7 @@
 
 struct w9966 {
 	struct v4l2_device v4l2_dev;
+	struct v4l2_ctrl_handler hdl;
 	unsigned char dev_state;
 	unsigned char i2c_state;
 	unsigned short ppmode;
@@ -567,7 +571,8 @@ static int cam_querycap(struct file *file, void  *priv,
 	strlcpy(vcap->driver, cam->v4l2_dev.name, sizeof(vcap->driver));
 	strlcpy(vcap->card, W9966_DRIVERNAME, sizeof(vcap->card));
 	strlcpy(vcap->bus_info, "parport", sizeof(vcap->bus_info));
-	vcap->capabilities = V4L2_CAP_VIDEO_CAPTURE | V4L2_CAP_READWRITE;
+	vcap->device_caps = V4L2_CAP_VIDEO_CAPTURE | V4L2_CAP_READWRITE;
+	vcap->capabilities = vcap->device_caps | V4L2_CAP_DEVICE_CAPS;
 	return 0;
 }
 
@@ -595,67 +600,25 @@ static int cam_s_input(struct file *file, void *fh, unsigned int inp)
 	return (inp > 0) ? -EINVAL : 0;
 }
 
-static int cam_queryctrl(struct file *file, void *priv,
-					struct v4l2_queryctrl *qc)
+static int cam_s_ctrl(struct v4l2_ctrl *ctrl)
 {
-	switch (qc->id) {
-	case V4L2_CID_BRIGHTNESS:
-		return v4l2_ctrl_query_fill(qc, 0, 255, 1, 128);
-	case V4L2_CID_CONTRAST:
-		return v4l2_ctrl_query_fill(qc, -64, 64, 1, 64);
-	case V4L2_CID_SATURATION:
-		return v4l2_ctrl_query_fill(qc, -64, 64, 1, 64);
-	case V4L2_CID_HUE:
-		return v4l2_ctrl_query_fill(qc, -128, 127, 1, 0);
-	}
-	return -EINVAL;
-}
-
-static int cam_g_ctrl(struct file *file, void *priv,
-					struct v4l2_control *ctrl)
-{
-	struct w9966 *cam = video_drvdata(file);
-	int ret = 0;
-
-	switch (ctrl->id) {
-	case V4L2_CID_BRIGHTNESS:
-		ctrl->value = cam->brightness;
-		break;
-	case V4L2_CID_CONTRAST:
-		ctrl->value = cam->contrast;
-		break;
-	case V4L2_CID_SATURATION:
-		ctrl->value = cam->color;
-		break;
-	case V4L2_CID_HUE:
-		ctrl->value = cam->hue;
-		break;
-	default:
-		ret = -EINVAL;
-		break;
-	}
-	return ret;
-}
-
-static int cam_s_ctrl(struct file *file, void *priv,
-					struct v4l2_control *ctrl)
-{
-	struct w9966 *cam = video_drvdata(file);
+	struct w9966 *cam =
+		container_of(ctrl->handler, struct w9966, hdl);
 	int ret = 0;
 
 	mutex_lock(&cam->lock);
 	switch (ctrl->id) {
 	case V4L2_CID_BRIGHTNESS:
-		cam->brightness = ctrl->value;
+		cam->brightness = ctrl->val;
 		break;
 	case V4L2_CID_CONTRAST:
-		cam->contrast = ctrl->value;
+		cam->contrast = ctrl->val;
 		break;
 	case V4L2_CID_SATURATION:
-		cam->color = ctrl->value;
+		cam->color = ctrl->val;
 		break;
 	case V4L2_CID_HUE:
-		cam->hue = ctrl->value;
+		cam->hue = ctrl->val;
 		break;
 	default:
 		ret = -EINVAL;
@@ -813,6 +776,9 @@ out:
 
 static const struct v4l2_file_operations w9966_fops = {
 	.owner		= THIS_MODULE,
+	.open		= v4l2_fh_open,
+	.release	= v4l2_fh_release,
+	.poll		= v4l2_ctrl_poll,
 	.unlocked_ioctl = video_ioctl2,
 	.read           = w9966_v4l_read,
 };
@@ -822,13 +788,17 @@ static const struct v4l2_ioctl_ops w9966_ioctl_ops = {
 	.vidioc_g_input      		    = cam_g_input,
 	.vidioc_s_input      		    = cam_s_input,
 	.vidioc_enum_input   		    = cam_enum_input,
-	.vidioc_queryctrl 		    = cam_queryctrl,
-	.vidioc_g_ctrl  		    = cam_g_ctrl,
-	.vidioc_s_ctrl 			    = cam_s_ctrl,
 	.vidioc_enum_fmt_vid_cap 	    = cam_enum_fmt_vid_cap,
 	.vidioc_g_fmt_vid_cap 		    = cam_g_fmt_vid_cap,
 	.vidioc_s_fmt_vid_cap  		    = cam_s_fmt_vid_cap,
 	.vidioc_try_fmt_vid_cap  	    = cam_try_fmt_vid_cap,
+	.vidioc_log_status		    = v4l2_ctrl_log_status,
+	.vidioc_subscribe_event		    = v4l2_ctrl_subscribe_event,
+	.vidioc_unsubscribe_event	    = v4l2_event_unsubscribe,
+};
+
+static const struct v4l2_ctrl_ops cam_ctrl_ops = {
+	.s_ctrl = cam_s_ctrl,
 };
 
 
@@ -849,6 +819,20 @@ static int w9966_init(struct w9966 *cam, struct parport *port)
 		v4l2_err(v4l2_dev, "Could not register v4l2_device\n");
 		return -1;
 	}
+
+	v4l2_ctrl_handler_init(&cam->hdl, 4);
+	v4l2_ctrl_new_std(&cam->hdl, &cam_ctrl_ops,
+			  V4L2_CID_BRIGHTNESS, 0, 255, 1, 128);
+	v4l2_ctrl_new_std(&cam->hdl, &cam_ctrl_ops,
+			  V4L2_CID_CONTRAST, -64, 64, 1, 64);
+	v4l2_ctrl_new_std(&cam->hdl, &cam_ctrl_ops,
+			  V4L2_CID_SATURATION, -64, 64, 1, 64);
+	v4l2_ctrl_new_std(&cam->hdl, &cam_ctrl_ops,
+			  V4L2_CID_HUE, -128, 127, 1, 0);
+	if (cam->hdl.error) {
+		v4l2_err(v4l2_dev, "couldn't register controls\n");
+		return -1;
+	}
 	cam->pport = port;
 	cam->brightness = 128;
 	cam->contrast = 64;
@@ -898,6 +882,8 @@ static int w9966_init(struct w9966 *cam, struct parport *port)
 	cam->vdev.fops = &w9966_fops;
 	cam->vdev.ioctl_ops = &w9966_ioctl_ops;
 	cam->vdev.release = video_device_release_empty;
+	cam->vdev.ctrl_handler = &cam->hdl;
+	set_bit(V4L2_FL_USE_FH_PRIO, &cam->vdev.flags);
 	video_set_drvdata(&cam->vdev, cam);
 
 	mutex_init(&cam->lock);
@@ -923,6 +909,8 @@ static void w9966_term(struct w9966 *cam)
 		w9966_set_state(cam, W9966_STATE_VDEV, 0);
 	}
 
+	v4l2_ctrl_handler_free(&cam->hdl);
+
 	/* Terminate from IEEE1284 mode and release pdev block */
 	if (w9966_get_state(cam, W9966_STATE_PDEV, W9966_STATE_PDEV)) {
 		w9966_pdev_claim(cam);
diff --git a/drivers/media/video/zoran/zoran_device.c b/drivers/media/video/zoran/zoran_device.c
index e86173bd132..a4cd504b8ee 100644
--- a/drivers/media/video/zoran/zoran_device.c
+++ b/drivers/media/video/zoran/zoran_device.c
@@ -542,11 +542,9 @@ void write_overlay_mask(struct zoran_fh *fh, struct v4l2_clip *vp, int count)
 	u32 *mask;
 	int x, y, width, height;
 	unsigned i, j, k;
-	u32 reg;
 
 	/* fill mask with one bits */
 	memset(fh->overlay_mask, ~0, mask_line_size * 4 * BUZ_MAX_HEIGHT);
-	reg = 0;
 
 	for (i = 0; i < count; ++i) {
 		/* pick up local copy of clip */
diff --git a/drivers/media/video/zoran/zoran_driver.c b/drivers/media/video/zoran/zoran_driver.c
index 4c09ab781ec..c5731093181 100644
--- a/drivers/media/video/zoran/zoran_driver.c
+++ b/drivers/media/video/zoran/zoran_driver.c
@@ -1131,8 +1131,14 @@ static int setup_fbuffer(struct zoran_fh *fh,
 }
 
 
-static int setup_window(struct zoran_fh *fh, int x, int y, int width, int height,
-	struct v4l2_clip __user *clips, int clipcount, void __user *bitmap)
+static int setup_window(struct zoran_fh *fh,
+			int x,
+			int y,
+			int width,
+			int height,
+			struct v4l2_clip __user *clips,
+			unsigned int clipcount,
+			void __user *bitmap)
 {
 	struct zoran *zr = fh->zr;
 	struct v4l2_clip *vcp = NULL;
@@ -1155,6 +1161,14 @@ static int setup_window(struct zoran_fh *fh, int x, int y, int width, int height
 		return -EINVAL;
 	}
 
+	if (clipcount > 2048) {
+		dprintk(1,
+			KERN_ERR
+			"%s: %s - invalid clipcount\n",
+			 ZR_DEVNAME(zr), __func__);
+		return -EINVAL;
+	}
+
 	/*
 	 * The video front end needs 4-byte alinged line sizes, we correct that
 	 * silently here if necessary
@@ -1218,7 +1232,7 @@ static int setup_window(struct zoran_fh *fh, int x, int y, int width, int height
 				   (width * height + 7) / 8)) {
 			return -EFAULT;
 		}
-	} else if (clipcount > 0) {
+	} else if (clipcount) {
 		/* write our own bitmap from the clips */
 		vcp = vmalloc(sizeof(struct v4l2_clip) * (clipcount + 4));
 		if (vcp == NULL) {
diff --git a/drivers/media/video/zr364xx.c b/drivers/media/video/zr364xx.c
index cd2e39fc4bf..e44cb330bbc 100644
--- a/drivers/media/video/zr364xx.c
+++ b/drivers/media/video/zr364xx.c
@@ -507,14 +507,12 @@ static void zr364xx_fillbuff(struct zr364xx_camera *cam,
 	const char *tmpbuf;
 	char *vbuf = videobuf_to_vmalloc(&buf->vb);
 	unsigned long last_frame;
-	struct zr364xx_framei *frm;
 
 	if (!vbuf)
 		return;
 
 	last_frame = cam->last_frame;
 	if (last_frame != -1) {
-		frm = &cam->buffer.frame[last_frame];
 		tmpbuf = (const char *)cam->buffer.frame[last_frame].lpvbits;
 		switch (buf->fmt->fourcc) {
 		case V4L2_PIX_FMT_JPEG:
diff --git a/drivers/memory/Kconfig b/drivers/memory/Kconfig
new file mode 100644
index 00000000000..067f31174a0
--- /dev/null
+++ b/drivers/memory/Kconfig
@@ -0,0 +1,43 @@
+#
+# Memory devices
+#
+
+menuconfig MEMORY
+	bool "Memory Controller drivers"
+
+if MEMORY
+
+config TI_EMIF
+	tristate "Texas Instruments EMIF driver"
+	depends on ARCH_OMAP2PLUS
+	select DDR
+	help
+	  This driver is for the EMIF module available in Texas Instruments
+	  SoCs. EMIF is an SDRAM controller that, based on its revision,
+	  supports one or more of DDR2, DDR3, and LPDDR2 SDRAM protocols.
+	  This driver takes care of only LPDDR2 memories presently. The
+	  functions of the driver includes re-configuring AC timing
+	  parameters and other settings during frequency, voltage and
+	  temperature changes
+
+config TEGRA20_MC
+	bool "Tegra20 Memory Controller(MC) driver"
+	default y
+	depends on ARCH_TEGRA_2x_SOC
+	help
+	  This driver is for the Memory Controller(MC) module available
+	  in Tegra20 SoCs, mainly for a address translation fault
+	  analysis, especially for IOMMU/GART(Graphics Address
+	  Relocation Table) module.
+
+config TEGRA30_MC
+	bool "Tegra30 Memory Controller(MC) driver"
+	default y
+	depends on ARCH_TEGRA_3x_SOC
+	help
+	  This driver is for the Memory Controller(MC) module available
+	  in Tegra30 SoCs, mainly for a address translation fault
+	  analysis, especially for IOMMU/SMMU(System Memory Management
+	  Unit) module.
+
+endif
diff --git a/drivers/memory/Makefile b/drivers/memory/Makefile
new file mode 100644
index 00000000000..42b3ce9d80f
--- /dev/null
+++ b/drivers/memory/Makefile
@@ -0,0 +1,7 @@
+#
+# Makefile for memory devices
+#
+
+obj-$(CONFIG_TI_EMIF)		+= emif.o
+obj-$(CONFIG_TEGRA20_MC)	+= tegra20-mc.o
+obj-$(CONFIG_TEGRA30_MC)	+= tegra30-mc.o
diff --git a/drivers/memory/emif.c b/drivers/memory/emif.c
new file mode 100644
index 00000000000..33a4396b24c
--- /dev/null
+++ b/drivers/memory/emif.c
@@ -0,0 +1,1670 @@
+/*
+ * EMIF driver
+ *
+ * Copyright (C) 2012 Texas Instruments, Inc.
+ *
+ * Aneesh V <aneesh@ti.com>
+ * Santosh Shilimkar <santosh.shilimkar@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/kernel.h>
+#include <linux/reboot.h>
+#include <linux/platform_data/emif_plat.h>
+#include <linux/io.h>
+#include <linux/device.h>
+#include <linux/platform_device.h>
+#include <linux/interrupt.h>
+#include <linux/slab.h>
+#include <linux/debugfs.h>
+#include <linux/seq_file.h>
+#include <linux/module.h>
+#include <linux/list.h>
+#include <linux/spinlock.h>
+#include <memory/jedec_ddr.h>
+#include "emif.h"
+
+/**
+ * struct emif_data - Per device static data for driver's use
+ * @duplicate:			Whether the DDR devices attached to this EMIF
+ *				instance are exactly same as that on EMIF1. In
+ *				this case we can save some memory and processing
+ * @temperature_level:		Maximum temperature of LPDDR2 devices attached
+ *				to this EMIF - read from MR4 register. If there
+ *				are two devices attached to this EMIF, this
+ *				value is the maximum of the two temperature
+ *				levels.
+ * @node:			node in the device list
+ * @base:			base address of memory-mapped IO registers.
+ * @dev:			device pointer.
+ * @addressing			table with addressing information from the spec
+ * @regs_cache:			An array of 'struct emif_regs' that stores
+ *				calculated register values for different
+ *				frequencies, to avoid re-calculating them on
+ *				each DVFS transition.
+ * @curr_regs:			The set of register values used in the last
+ *				frequency change (i.e. corresponding to the
+ *				frequency in effect at the moment)
+ * @plat_data:			Pointer to saved platform data.
+ * @debugfs_root:		dentry to the root folder for EMIF in debugfs
+ */
+struct emif_data {
+	u8				duplicate;
+	u8				temperature_level;
+	u8				lpmode;
+	struct list_head		node;
+	unsigned long			irq_state;
+	void __iomem			*base;
+	struct device			*dev;
+	const struct lpddr2_addressing	*addressing;
+	struct emif_regs		*regs_cache[EMIF_MAX_NUM_FREQUENCIES];
+	struct emif_regs		*curr_regs;
+	struct emif_platform_data	*plat_data;
+	struct dentry			*debugfs_root;
+};
+
+static struct emif_data *emif1;
+static spinlock_t	emif_lock;
+static unsigned long	irq_state;
+static u32		t_ck; /* DDR clock period in ps */
+static LIST_HEAD(device_list);
+
+static void do_emif_regdump_show(struct seq_file *s, struct emif_data *emif,
+	struct emif_regs *regs)
+{
+	u32 type = emif->plat_data->device_info->type;
+	u32 ip_rev = emif->plat_data->ip_rev;
+
+	seq_printf(s, "EMIF register cache dump for %dMHz\n",
+		regs->freq/1000000);
+
+	seq_printf(s, "ref_ctrl_shdw\t: 0x%08x\n", regs->ref_ctrl_shdw);
+	seq_printf(s, "sdram_tim1_shdw\t: 0x%08x\n", regs->sdram_tim1_shdw);
+	seq_printf(s, "sdram_tim2_shdw\t: 0x%08x\n", regs->sdram_tim2_shdw);
+	seq_printf(s, "sdram_tim3_shdw\t: 0x%08x\n", regs->sdram_tim3_shdw);
+
+	if (ip_rev == EMIF_4D) {
+		seq_printf(s, "read_idle_ctrl_shdw_normal\t: 0x%08x\n",
+			regs->read_idle_ctrl_shdw_normal);
+		seq_printf(s, "read_idle_ctrl_shdw_volt_ramp\t: 0x%08x\n",
+			regs->read_idle_ctrl_shdw_volt_ramp);
+	} else if (ip_rev == EMIF_4D5) {
+		seq_printf(s, "dll_calib_ctrl_shdw_normal\t: 0x%08x\n",
+			regs->dll_calib_ctrl_shdw_normal);
+		seq_printf(s, "dll_calib_ctrl_shdw_volt_ramp\t: 0x%08x\n",
+			regs->dll_calib_ctrl_shdw_volt_ramp);
+	}
+
+	if (type == DDR_TYPE_LPDDR2_S2 || type == DDR_TYPE_LPDDR2_S4) {
+		seq_printf(s, "ref_ctrl_shdw_derated\t: 0x%08x\n",
+			regs->ref_ctrl_shdw_derated);
+		seq_printf(s, "sdram_tim1_shdw_derated\t: 0x%08x\n",
+			regs->sdram_tim1_shdw_derated);
+		seq_printf(s, "sdram_tim3_shdw_derated\t: 0x%08x\n",
+			regs->sdram_tim3_shdw_derated);
+	}
+}
+
+static int emif_regdump_show(struct seq_file *s, void *unused)
+{
+	struct emif_data	*emif	= s->private;
+	struct emif_regs	**regs_cache;
+	int			i;
+
+	if (emif->duplicate)
+		regs_cache = emif1->regs_cache;
+	else
+		regs_cache = emif->regs_cache;
+
+	for (i = 0; i < EMIF_MAX_NUM_FREQUENCIES && regs_cache[i]; i++) {
+		do_emif_regdump_show(s, emif, regs_cache[i]);
+		seq_printf(s, "\n");
+	}
+
+	return 0;
+}
+
+static int emif_regdump_open(struct inode *inode, struct file *file)
+{
+	return single_open(file, emif_regdump_show, inode->i_private);
+}
+
+static const struct file_operations emif_regdump_fops = {
+	.open			= emif_regdump_open,
+	.read			= seq_read,
+	.release		= single_release,
+};
+
+static int emif_mr4_show(struct seq_file *s, void *unused)
+{
+	struct emif_data *emif = s->private;
+
+	seq_printf(s, "MR4=%d\n", emif->temperature_level);
+	return 0;
+}
+
+static int emif_mr4_open(struct inode *inode, struct file *file)
+{
+	return single_open(file, emif_mr4_show, inode->i_private);
+}
+
+static const struct file_operations emif_mr4_fops = {
+	.open			= emif_mr4_open,
+	.read			= seq_read,
+	.release		= single_release,
+};
+
+static int __init_or_module emif_debugfs_init(struct emif_data *emif)
+{
+	struct dentry	*dentry;
+	int		ret;
+
+	dentry = debugfs_create_dir(dev_name(emif->dev), NULL);
+	if (IS_ERR(dentry)) {
+		ret = PTR_ERR(dentry);
+		goto err0;
+	}
+	emif->debugfs_root = dentry;
+
+	dentry = debugfs_create_file("regcache_dump", S_IRUGO,
+			emif->debugfs_root, emif, &emif_regdump_fops);
+	if (IS_ERR(dentry)) {
+		ret = PTR_ERR(dentry);
+		goto err1;
+	}
+
+	dentry = debugfs_create_file("mr4", S_IRUGO,
+			emif->debugfs_root, emif, &emif_mr4_fops);
+	if (IS_ERR(dentry)) {
+		ret = PTR_ERR(dentry);
+		goto err1;
+	}
+
+	return 0;
+err1:
+	debugfs_remove_recursive(emif->debugfs_root);
+err0:
+	return ret;
+}
+
+static void __exit emif_debugfs_exit(struct emif_data *emif)
+{
+	debugfs_remove_recursive(emif->debugfs_root);
+	emif->debugfs_root = NULL;
+}
+
+/*
+ * Calculate the period of DDR clock from frequency value
+ */
+static void set_ddr_clk_period(u32 freq)
+{
+	/* Divide 10^12 by frequency to get period in ps */
+	t_ck = (u32)DIV_ROUND_UP_ULL(1000000000000ull, freq);
+}
+
+/*
+ * Get bus width used by EMIF. Note that this may be different from the
+ * bus width of the DDR devices used. For instance two 16-bit DDR devices
+ * may be connected to a given CS of EMIF. In this case bus width as far
+ * as EMIF is concerned is 32, where as the DDR bus width is 16 bits.
+ */
+static u32 get_emif_bus_width(struct emif_data *emif)
+{
+	u32		width;
+	void __iomem	*base = emif->base;
+
+	width = (readl(base + EMIF_SDRAM_CONFIG) & NARROW_MODE_MASK)
+			>> NARROW_MODE_SHIFT;
+	width = width == 0 ? 32 : 16;
+
+	return width;
+}
+
+/*
+ * Get the CL from SDRAM_CONFIG register
+ */
+static u32 get_cl(struct emif_data *emif)
+{
+	u32		cl;
+	void __iomem	*base = emif->base;
+
+	cl = (readl(base + EMIF_SDRAM_CONFIG) & CL_MASK) >> CL_SHIFT;
+
+	return cl;
+}
+
+static void set_lpmode(struct emif_data *emif, u8 lpmode)
+{
+	u32 temp;
+	void __iomem *base = emif->base;
+
+	temp = readl(base + EMIF_POWER_MANAGEMENT_CONTROL);
+	temp &= ~LP_MODE_MASK;
+	temp |= (lpmode << LP_MODE_SHIFT);
+	writel(temp, base + EMIF_POWER_MANAGEMENT_CONTROL);
+}
+
+static void do_freq_update(void)
+{
+	struct emif_data *emif;
+
+	/*
+	 * Workaround for errata i728: Disable LPMODE during FREQ_UPDATE
+	 *
+	 * i728 DESCRIPTION:
+	 * The EMIF automatically puts the SDRAM into self-refresh mode
+	 * after the EMIF has not performed accesses during
+	 * EMIF_PWR_MGMT_CTRL[7:4] REG_SR_TIM number of DDR clock cycles
+	 * and the EMIF_PWR_MGMT_CTRL[10:8] REG_LP_MODE bit field is set
+	 * to 0x2. If during a small window the following three events
+	 * occur:
+	 * - The SR_TIMING counter expires
+	 * - And frequency change is requested
+	 * - And OCP access is requested
+	 * Then it causes instable clock on the DDR interface.
+	 *
+	 * WORKAROUND
+	 * To avoid the occurrence of the three events, the workaround
+	 * is to disable the self-refresh when requesting a frequency
+	 * change. Before requesting a frequency change the software must
+	 * program EMIF_PWR_MGMT_CTRL[10:8] REG_LP_MODE to 0x0. When the
+	 * frequency change has been done, the software can reprogram
+	 * EMIF_PWR_MGMT_CTRL[10:8] REG_LP_MODE to 0x2
+	 */
+	list_for_each_entry(emif, &device_list, node) {
+		if (emif->lpmode == EMIF_LP_MODE_SELF_REFRESH)
+			set_lpmode(emif, EMIF_LP_MODE_DISABLE);
+	}
+
+	/*
+	 * TODO: Do FREQ_UPDATE here when an API
+	 * is available for this as part of the new
+	 * clock framework
+	 */
+
+	list_for_each_entry(emif, &device_list, node) {
+		if (emif->lpmode == EMIF_LP_MODE_SELF_REFRESH)
+			set_lpmode(emif, EMIF_LP_MODE_SELF_REFRESH);
+	}
+}
+
+/* Find addressing table entry based on the device's type and density */
+static const struct lpddr2_addressing *get_addressing_table(
+	const struct ddr_device_info *device_info)
+{
+	u32		index, type, density;
+
+	type = device_info->type;
+	density = device_info->density;
+
+	switch (type) {
+	case DDR_TYPE_LPDDR2_S4:
+		index = density - 1;
+		break;
+	case DDR_TYPE_LPDDR2_S2:
+		switch (density) {
+		case DDR_DENSITY_1Gb:
+		case DDR_DENSITY_2Gb:
+			index = density + 3;
+			break;
+		default:
+			index = density - 1;
+		}
+		break;
+	default:
+		return NULL;
+	}
+
+	return &lpddr2_jedec_addressing_table[index];
+}
+
+/*
+ * Find the the right timing table from the array of timing
+ * tables of the device using DDR clock frequency
+ */
+static const struct lpddr2_timings *get_timings_table(struct emif_data *emif,
+		u32 freq)
+{
+	u32				i, min, max, freq_nearest;
+	const struct lpddr2_timings	*timings = NULL;
+	const struct lpddr2_timings	*timings_arr = emif->plat_data->timings;
+	struct				device *dev = emif->dev;
+
+	/* Start with a very high frequency - 1GHz */
+	freq_nearest = 1000000000;
+
+	/*
+	 * Find the timings table such that:
+	 *  1. the frequency range covers the required frequency(safe) AND
+	 *  2. the max_freq is closest to the required frequency(optimal)
+	 */
+	for (i = 0; i < emif->plat_data->timings_arr_size; i++) {
+		max = timings_arr[i].max_freq;
+		min = timings_arr[i].min_freq;
+		if ((freq >= min) && (freq <= max) && (max < freq_nearest)) {
+			freq_nearest = max;
+			timings = &timings_arr[i];
+		}
+	}
+
+	if (!timings)
+		dev_err(dev, "%s: couldn't find timings for - %dHz\n",
+			__func__, freq);
+
+	dev_dbg(dev, "%s: timings table: freq %d, speed bin freq %d\n",
+		__func__, freq, freq_nearest);
+
+	return timings;
+}
+
+static u32 get_sdram_ref_ctrl_shdw(u32 freq,
+		const struct lpddr2_addressing *addressing)
+{
+	u32 ref_ctrl_shdw = 0, val = 0, freq_khz, t_refi;
+
+	/* Scale down frequency and t_refi to avoid overflow */
+	freq_khz = freq / 1000;
+	t_refi = addressing->tREFI_ns / 100;
+
+	/*
+	 * refresh rate to be set is 'tREFI(in us) * freq in MHz
+	 * division by 10000 to account for change in units
+	 */
+	val = t_refi * freq_khz / 10000;
+	ref_ctrl_shdw |= val << REFRESH_RATE_SHIFT;
+
+	return ref_ctrl_shdw;
+}
+
+static u32 get_sdram_tim_1_shdw(const struct lpddr2_timings *timings,
+		const struct lpddr2_min_tck *min_tck,
+		const struct lpddr2_addressing *addressing)
+{
+	u32 tim1 = 0, val = 0;
+
+	val = max(min_tck->tWTR, DIV_ROUND_UP(timings->tWTR, t_ck)) - 1;
+	tim1 |= val << T_WTR_SHIFT;
+
+	if (addressing->num_banks == B8)
+		val = DIV_ROUND_UP(timings->tFAW, t_ck*4);
+	else
+		val = max(min_tck->tRRD, DIV_ROUND_UP(timings->tRRD, t_ck));
+	tim1 |= (val - 1) << T_RRD_SHIFT;
+
+	val = DIV_ROUND_UP(timings->tRAS_min + timings->tRPab, t_ck) - 1;
+	tim1 |= val << T_RC_SHIFT;
+
+	val = max(min_tck->tRASmin, DIV_ROUND_UP(timings->tRAS_min, t_ck));
+	tim1 |= (val - 1) << T_RAS_SHIFT;
+
+	val = max(min_tck->tWR, DIV_ROUND_UP(timings->tWR, t_ck)) - 1;
+	tim1 |= val << T_WR_SHIFT;
+
+	val = max(min_tck->tRCD, DIV_ROUND_UP(timings->tRCD, t_ck)) - 1;
+	tim1 |= val << T_RCD_SHIFT;
+
+	val = max(min_tck->tRPab, DIV_ROUND_UP(timings->tRPab, t_ck)) - 1;
+	tim1 |= val << T_RP_SHIFT;
+
+	return tim1;
+}
+
+static u32 get_sdram_tim_1_shdw_derated(const struct lpddr2_timings *timings,
+		const struct lpddr2_min_tck *min_tck,
+		const struct lpddr2_addressing *addressing)
+{
+	u32 tim1 = 0, val = 0;
+
+	val = max(min_tck->tWTR, DIV_ROUND_UP(timings->tWTR, t_ck)) - 1;
+	tim1 = val << T_WTR_SHIFT;
+
+	/*
+	 * tFAW is approximately 4 times tRRD. So add 1875*4 = 7500ps
+	 * to tFAW for de-rating
+	 */
+	if (addressing->num_banks == B8) {
+		val = DIV_ROUND_UP(timings->tFAW + 7500, 4 * t_ck) - 1;
+	} else {
+		val = DIV_ROUND_UP(timings->tRRD + 1875, t_ck);
+		val = max(min_tck->tRRD, val) - 1;
+	}
+	tim1 |= val << T_RRD_SHIFT;
+
+	val = DIV_ROUND_UP(timings->tRAS_min + timings->tRPab + 1875, t_ck);
+	tim1 |= (val - 1) << T_RC_SHIFT;
+
+	val = DIV_ROUND_UP(timings->tRAS_min + 1875, t_ck);
+	val = max(min_tck->tRASmin, val) - 1;
+	tim1 |= val << T_RAS_SHIFT;
+
+	val = max(min_tck->tWR, DIV_ROUND_UP(timings->tWR, t_ck)) - 1;
+	tim1 |= val << T_WR_SHIFT;
+
+	val = max(min_tck->tRCD, DIV_ROUND_UP(timings->tRCD + 1875, t_ck));
+	tim1 |= (val - 1) << T_RCD_SHIFT;
+
+	val = max(min_tck->tRPab, DIV_ROUND_UP(timings->tRPab + 1875, t_ck));
+	tim1 |= (val - 1) << T_RP_SHIFT;
+
+	return tim1;
+}
+
+static u32 get_sdram_tim_2_shdw(const struct lpddr2_timings *timings,
+		const struct lpddr2_min_tck *min_tck,
+		const struct lpddr2_addressing *addressing,
+		u32 type)
+{
+	u32 tim2 = 0, val = 0;
+
+	val = min_tck->tCKE - 1;
+	tim2 |= val << T_CKE_SHIFT;
+
+	val = max(min_tck->tRTP, DIV_ROUND_UP(timings->tRTP, t_ck)) - 1;
+	tim2 |= val << T_RTP_SHIFT;
+
+	/* tXSNR = tRFCab_ps + 10 ns(tRFCab_ps for LPDDR2). */
+	val = DIV_ROUND_UP(addressing->tRFCab_ps + 10000, t_ck) - 1;
+	tim2 |= val << T_XSNR_SHIFT;
+
+	/* XSRD same as XSNR for LPDDR2 */
+	tim2 |= val << T_XSRD_SHIFT;
+
+	val = max(min_tck->tXP, DIV_ROUND_UP(timings->tXP, t_ck)) - 1;
+	tim2 |= val << T_XP_SHIFT;
+
+	return tim2;
+}
+
+static u32 get_sdram_tim_3_shdw(const struct lpddr2_timings *timings,
+		const struct lpddr2_min_tck *min_tck,
+		const struct lpddr2_addressing *addressing,
+		u32 type, u32 ip_rev, u32 derated)
+{
+	u32 tim3 = 0, val = 0, t_dqsck;
+
+	val = timings->tRAS_max_ns / addressing->tREFI_ns - 1;
+	val = val > 0xF ? 0xF : val;
+	tim3 |= val << T_RAS_MAX_SHIFT;
+
+	val = DIV_ROUND_UP(addressing->tRFCab_ps, t_ck) - 1;
+	tim3 |= val << T_RFC_SHIFT;
+
+	t_dqsck = (derated == EMIF_DERATED_TIMINGS) ?
+		timings->tDQSCK_max_derated : timings->tDQSCK_max;
+	if (ip_rev == EMIF_4D5)
+		val = DIV_ROUND_UP(t_dqsck + 1000, t_ck) - 1;
+	else
+		val = DIV_ROUND_UP(t_dqsck, t_ck) - 1;
+
+	tim3 |= val << T_TDQSCKMAX_SHIFT;
+
+	val = DIV_ROUND_UP(timings->tZQCS, t_ck) - 1;
+	tim3 |= val << ZQ_ZQCS_SHIFT;
+
+	val = DIV_ROUND_UP(timings->tCKESR, t_ck);
+	val = max(min_tck->tCKESR, val) - 1;
+	tim3 |= val << T_CKESR_SHIFT;
+
+	if (ip_rev == EMIF_4D5) {
+		tim3 |= (EMIF_T_CSTA - 1) << T_CSTA_SHIFT;
+
+		val = DIV_ROUND_UP(EMIF_T_PDLL_UL, 128) - 1;
+		tim3 |= val << T_PDLL_UL_SHIFT;
+	}
+
+	return tim3;
+}
+
+static u32 get_zq_config_reg(const struct lpddr2_addressing *addressing,
+		bool cs1_used, bool cal_resistors_per_cs)
+{
+	u32 zq = 0, val = 0;
+
+	val = EMIF_ZQCS_INTERVAL_US * 1000 / addressing->tREFI_ns;
+	zq |= val << ZQ_REFINTERVAL_SHIFT;
+
+	val = DIV_ROUND_UP(T_ZQCL_DEFAULT_NS, T_ZQCS_DEFAULT_NS) - 1;
+	zq |= val << ZQ_ZQCL_MULT_SHIFT;
+
+	val = DIV_ROUND_UP(T_ZQINIT_DEFAULT_NS, T_ZQCL_DEFAULT_NS) - 1;
+	zq |= val << ZQ_ZQINIT_MULT_SHIFT;
+
+	zq |= ZQ_SFEXITEN_ENABLE << ZQ_SFEXITEN_SHIFT;
+
+	if (cal_resistors_per_cs)
+		zq |= ZQ_DUALCALEN_ENABLE << ZQ_DUALCALEN_SHIFT;
+	else
+		zq |= ZQ_DUALCALEN_DISABLE << ZQ_DUALCALEN_SHIFT;
+
+	zq |= ZQ_CS0EN_MASK; /* CS0 is used for sure */
+
+	val = cs1_used ? 1 : 0;
+	zq |= val << ZQ_CS1EN_SHIFT;
+
+	return zq;
+}
+
+static u32 get_temp_alert_config(const struct lpddr2_addressing *addressing,
+		const struct emif_custom_configs *custom_configs, bool cs1_used,
+		u32 sdram_io_width, u32 emif_bus_width)
+{
+	u32 alert = 0, interval, devcnt;
+
+	if (custom_configs && (custom_configs->mask &
+				EMIF_CUSTOM_CONFIG_TEMP_ALERT_POLL_INTERVAL))
+		interval = custom_configs->temp_alert_poll_interval_ms;
+	else
+		interval = TEMP_ALERT_POLL_INTERVAL_DEFAULT_MS;
+
+	interval *= 1000000;			/* Convert to ns */
+	interval /= addressing->tREFI_ns;	/* Convert to refresh cycles */
+	alert |= (interval << TA_REFINTERVAL_SHIFT);
+
+	/*
+	 * sdram_io_width is in 'log2(x) - 1' form. Convert emif_bus_width
+	 * also to this form and subtract to get TA_DEVCNT, which is
+	 * in log2(x) form.
+	 */
+	emif_bus_width = __fls(emif_bus_width) - 1;
+	devcnt = emif_bus_width - sdram_io_width;
+	alert |= devcnt << TA_DEVCNT_SHIFT;
+
+	/* DEVWDT is in 'log2(x) - 3' form */
+	alert |= (sdram_io_width - 2) << TA_DEVWDT_SHIFT;
+
+	alert |= 1 << TA_SFEXITEN_SHIFT;
+	alert |= 1 << TA_CS0EN_SHIFT;
+	alert |= (cs1_used ? 1 : 0) << TA_CS1EN_SHIFT;
+
+	return alert;
+}
+
+static u32 get_read_idle_ctrl_shdw(u8 volt_ramp)
+{
+	u32 idle = 0, val = 0;
+
+	/*
+	 * Maximum value in normal conditions and increased frequency
+	 * when voltage is ramping
+	 */
+	if (volt_ramp)
+		val = READ_IDLE_INTERVAL_DVFS / t_ck / 64 - 1;
+	else
+		val = 0x1FF;
+
+	/*
+	 * READ_IDLE_CTRL register in EMIF4D has same offset and fields
+	 * as DLL_CALIB_CTRL in EMIF4D5, so use the same shifts
+	 */
+	idle |= val << DLL_CALIB_INTERVAL_SHIFT;
+	idle |= EMIF_READ_IDLE_LEN_VAL << ACK_WAIT_SHIFT;
+
+	return idle;
+}
+
+static u32 get_dll_calib_ctrl_shdw(u8 volt_ramp)
+{
+	u32 calib = 0, val = 0;
+
+	if (volt_ramp == DDR_VOLTAGE_RAMPING)
+		val = DLL_CALIB_INTERVAL_DVFS / t_ck / 16 - 1;
+	else
+		val = 0; /* Disabled when voltage is stable */
+
+	calib |= val << DLL_CALIB_INTERVAL_SHIFT;
+	calib |= DLL_CALIB_ACK_WAIT_VAL << ACK_WAIT_SHIFT;
+
+	return calib;
+}
+
+static u32 get_ddr_phy_ctrl_1_attilaphy_4d(const struct lpddr2_timings *timings,
+	u32 freq, u8 RL)
+{
+	u32 phy = EMIF_DDR_PHY_CTRL_1_BASE_VAL_ATTILAPHY, val = 0;
+
+	val = RL + DIV_ROUND_UP(timings->tDQSCK_max, t_ck) - 1;
+	phy |= val << READ_LATENCY_SHIFT_4D;
+
+	if (freq <= 100000000)
+		val = EMIF_DLL_SLAVE_DLY_CTRL_100_MHZ_AND_LESS_ATTILAPHY;
+	else if (freq <= 200000000)
+		val = EMIF_DLL_SLAVE_DLY_CTRL_200_MHZ_ATTILAPHY;
+	else
+		val = EMIF_DLL_SLAVE_DLY_CTRL_400_MHZ_ATTILAPHY;
+
+	phy |= val << DLL_SLAVE_DLY_CTRL_SHIFT_4D;
+
+	return phy;
+}
+
+static u32 get_phy_ctrl_1_intelliphy_4d5(u32 freq, u8 cl)
+{
+	u32 phy = EMIF_DDR_PHY_CTRL_1_BASE_VAL_INTELLIPHY, half_delay;
+
+	/*
+	 * DLL operates at 266 MHz. If DDR frequency is near 266 MHz,
+	 * half-delay is not needed else set half-delay
+	 */
+	if (freq >= 265000000 && freq < 267000000)
+		half_delay = 0;
+	else
+		half_delay = 1;
+
+	phy |= half_delay << DLL_HALF_DELAY_SHIFT_4D5;
+	phy |= ((cl + DIV_ROUND_UP(EMIF_PHY_TOTAL_READ_LATENCY_INTELLIPHY_PS,
+			t_ck) - 1) << READ_LATENCY_SHIFT_4D5);
+
+	return phy;
+}
+
+static u32 get_ext_phy_ctrl_2_intelliphy_4d5(void)
+{
+	u32 fifo_we_slave_ratio;
+
+	fifo_we_slave_ratio =  DIV_ROUND_CLOSEST(
+		EMIF_INTELLI_PHY_DQS_GATE_OPENING_DELAY_PS * 256 , t_ck);
+
+	return fifo_we_slave_ratio | fifo_we_slave_ratio << 11 |
+		fifo_we_slave_ratio << 22;
+}
+
+static u32 get_ext_phy_ctrl_3_intelliphy_4d5(void)
+{
+	u32 fifo_we_slave_ratio;
+
+	fifo_we_slave_ratio =  DIV_ROUND_CLOSEST(
+		EMIF_INTELLI_PHY_DQS_GATE_OPENING_DELAY_PS * 256 , t_ck);
+
+	return fifo_we_slave_ratio >> 10 | fifo_we_slave_ratio << 1 |
+		fifo_we_slave_ratio << 12 | fifo_we_slave_ratio << 23;
+}
+
+static u32 get_ext_phy_ctrl_4_intelliphy_4d5(void)
+{
+	u32 fifo_we_slave_ratio;
+
+	fifo_we_slave_ratio =  DIV_ROUND_CLOSEST(
+		EMIF_INTELLI_PHY_DQS_GATE_OPENING_DELAY_PS * 256 , t_ck);
+
+	return fifo_we_slave_ratio >> 9 | fifo_we_slave_ratio << 2 |
+		fifo_we_slave_ratio << 13;
+}
+
+static u32 get_pwr_mgmt_ctrl(u32 freq, struct emif_data *emif, u32 ip_rev)
+{
+	u32 pwr_mgmt_ctrl	= 0, timeout;
+	u32 lpmode		= EMIF_LP_MODE_SELF_REFRESH;
+	u32 timeout_perf	= EMIF_LP_MODE_TIMEOUT_PERFORMANCE;
+	u32 timeout_pwr		= EMIF_LP_MODE_TIMEOUT_POWER;
+	u32 freq_threshold	= EMIF_LP_MODE_FREQ_THRESHOLD;
+
+	struct emif_custom_configs *cust_cfgs = emif->plat_data->custom_configs;
+
+	if (cust_cfgs && (cust_cfgs->mask & EMIF_CUSTOM_CONFIG_LPMODE)) {
+		lpmode		= cust_cfgs->lpmode;
+		timeout_perf	= cust_cfgs->lpmode_timeout_performance;
+		timeout_pwr	= cust_cfgs->lpmode_timeout_power;
+		freq_threshold  = cust_cfgs->lpmode_freq_threshold;
+	}
+
+	/* Timeout based on DDR frequency */
+	timeout = freq >= freq_threshold ? timeout_perf : timeout_pwr;
+
+	/* The value to be set in register is "log2(timeout) - 3" */
+	if (timeout < 16) {
+		timeout = 0;
+	} else {
+		timeout = __fls(timeout) - 3;
+		if (timeout & (timeout - 1))
+			timeout++;
+	}
+
+	switch (lpmode) {
+	case EMIF_LP_MODE_CLOCK_STOP:
+		pwr_mgmt_ctrl = (timeout << CS_TIM_SHIFT) |
+					SR_TIM_MASK | PD_TIM_MASK;
+		break;
+	case EMIF_LP_MODE_SELF_REFRESH:
+		/* Workaround for errata i735 */
+		if (timeout < 6)
+			timeout = 6;
+
+		pwr_mgmt_ctrl = (timeout << SR_TIM_SHIFT) |
+					CS_TIM_MASK | PD_TIM_MASK;
+		break;
+	case EMIF_LP_MODE_PWR_DN:
+		pwr_mgmt_ctrl = (timeout << PD_TIM_SHIFT) |
+					CS_TIM_MASK | SR_TIM_MASK;
+		break;
+	case EMIF_LP_MODE_DISABLE:
+	default:
+		pwr_mgmt_ctrl = CS_TIM_MASK |
+					PD_TIM_MASK | SR_TIM_MASK;
+	}
+
+	/* No CS_TIM in EMIF_4D5 */
+	if (ip_rev == EMIF_4D5)
+		pwr_mgmt_ctrl &= ~CS_TIM_MASK;
+
+	pwr_mgmt_ctrl |= lpmode << LP_MODE_SHIFT;
+
+	return pwr_mgmt_ctrl;
+}
+
+/*
+ * Get the temperature level of the EMIF instance:
+ * Reads the MR4 register of attached SDRAM parts to find out the temperature
+ * level. If there are two parts attached(one on each CS), then the temperature
+ * level for the EMIF instance is the higher of the two temperatures.
+ */
+static void get_temperature_level(struct emif_data *emif)
+{
+	u32		temp, temperature_level;
+	void __iomem	*base;
+
+	base = emif->base;
+
+	/* Read mode register 4 */
+	writel(DDR_MR4, base + EMIF_LPDDR2_MODE_REG_CONFIG);
+	temperature_level = readl(base + EMIF_LPDDR2_MODE_REG_DATA);
+	temperature_level = (temperature_level & MR4_SDRAM_REF_RATE_MASK) >>
+				MR4_SDRAM_REF_RATE_SHIFT;
+
+	if (emif->plat_data->device_info->cs1_used) {
+		writel(DDR_MR4 | CS_MASK, base + EMIF_LPDDR2_MODE_REG_CONFIG);
+		temp = readl(base + EMIF_LPDDR2_MODE_REG_DATA);
+		temp = (temp & MR4_SDRAM_REF_RATE_MASK)
+				>> MR4_SDRAM_REF_RATE_SHIFT;
+		temperature_level = max(temp, temperature_level);
+	}
+
+	/* treat everything less than nominal(3) in MR4 as nominal */
+	if (unlikely(temperature_level < SDRAM_TEMP_NOMINAL))
+		temperature_level = SDRAM_TEMP_NOMINAL;
+
+	/* if we get reserved value in MR4 persist with the existing value */
+	if (likely(temperature_level != SDRAM_TEMP_RESERVED_4))
+		emif->temperature_level = temperature_level;
+}
+
+/*
+ * Program EMIF shadow registers that are not dependent on temperature
+ * or voltage
+ */
+static void setup_registers(struct emif_data *emif, struct emif_regs *regs)
+{
+	void __iomem	*base = emif->base;
+
+	writel(regs->sdram_tim2_shdw, base + EMIF_SDRAM_TIMING_2_SHDW);
+	writel(regs->phy_ctrl_1_shdw, base + EMIF_DDR_PHY_CTRL_1_SHDW);
+
+	/* Settings specific for EMIF4D5 */
+	if (emif->plat_data->ip_rev != EMIF_4D5)
+		return;
+	writel(regs->ext_phy_ctrl_2_shdw, base + EMIF_EXT_PHY_CTRL_2_SHDW);
+	writel(regs->ext_phy_ctrl_3_shdw, base + EMIF_EXT_PHY_CTRL_3_SHDW);
+	writel(regs->ext_phy_ctrl_4_shdw, base + EMIF_EXT_PHY_CTRL_4_SHDW);
+}
+
+/*
+ * When voltage ramps dll calibration and forced read idle should
+ * happen more often
+ */
+static void setup_volt_sensitive_regs(struct emif_data *emif,
+		struct emif_regs *regs, u32 volt_state)
+{
+	u32		calib_ctrl;
+	void __iomem	*base = emif->base;
+
+	/*
+	 * EMIF_READ_IDLE_CTRL in EMIF4D refers to the same register as
+	 * EMIF_DLL_CALIB_CTRL in EMIF4D5 and dll_calib_ctrl_shadow_*
+	 * is an alias of the respective read_idle_ctrl_shdw_* (members of
+	 * a union). So, the below code takes care of both cases
+	 */
+	if (volt_state == DDR_VOLTAGE_RAMPING)
+		calib_ctrl = regs->dll_calib_ctrl_shdw_volt_ramp;
+	else
+		calib_ctrl = regs->dll_calib_ctrl_shdw_normal;
+
+	writel(calib_ctrl, base + EMIF_DLL_CALIB_CTRL_SHDW);
+}
+
+/*
+ * setup_temperature_sensitive_regs() - set the timings for temperature
+ * sensitive registers. This happens once at initialisation time based
+ * on the temperature at boot time and subsequently based on the temperature
+ * alert interrupt. Temperature alert can happen when the temperature
+ * increases or drops. So this function can have the effect of either
+ * derating the timings or going back to nominal values.
+ */
+static void setup_temperature_sensitive_regs(struct emif_data *emif,
+		struct emif_regs *regs)
+{
+	u32		tim1, tim3, ref_ctrl, type;
+	void __iomem	*base = emif->base;
+	u32		temperature;
+
+	type = emif->plat_data->device_info->type;
+
+	tim1 = regs->sdram_tim1_shdw;
+	tim3 = regs->sdram_tim3_shdw;
+	ref_ctrl = regs->ref_ctrl_shdw;
+
+	/* No de-rating for non-lpddr2 devices */
+	if (type != DDR_TYPE_LPDDR2_S2 && type != DDR_TYPE_LPDDR2_S4)
+		goto out;
+
+	temperature = emif->temperature_level;
+	if (temperature == SDRAM_TEMP_HIGH_DERATE_REFRESH) {
+		ref_ctrl = regs->ref_ctrl_shdw_derated;
+	} else if (temperature == SDRAM_TEMP_HIGH_DERATE_REFRESH_AND_TIMINGS) {
+		tim1 = regs->sdram_tim1_shdw_derated;
+		tim3 = regs->sdram_tim3_shdw_derated;
+		ref_ctrl = regs->ref_ctrl_shdw_derated;
+	}
+
+out:
+	writel(tim1, base + EMIF_SDRAM_TIMING_1_SHDW);
+	writel(tim3, base + EMIF_SDRAM_TIMING_3_SHDW);
+	writel(ref_ctrl, base + EMIF_SDRAM_REFRESH_CTRL_SHDW);
+}
+
+static irqreturn_t handle_temp_alert(void __iomem *base, struct emif_data *emif)
+{
+	u32		old_temp_level;
+	irqreturn_t	ret = IRQ_HANDLED;
+
+	spin_lock_irqsave(&emif_lock, irq_state);
+	old_temp_level = emif->temperature_level;
+	get_temperature_level(emif);
+
+	if (unlikely(emif->temperature_level == old_temp_level)) {
+		goto out;
+	} else if (!emif->curr_regs) {
+		dev_err(emif->dev, "temperature alert before registers are calculated, not de-rating timings\n");
+		goto out;
+	}
+
+	if (emif->temperature_level < old_temp_level ||
+		emif->temperature_level == SDRAM_TEMP_VERY_HIGH_SHUTDOWN) {
+		/*
+		 * Temperature coming down - defer handling to thread OR
+		 * Temperature far too high - do kernel_power_off() from
+		 * thread context
+		 */
+		ret = IRQ_WAKE_THREAD;
+	} else {
+		/* Temperature is going up - handle immediately */
+		setup_temperature_sensitive_regs(emif, emif->curr_regs);
+		do_freq_update();
+	}
+
+out:
+	spin_unlock_irqrestore(&emif_lock, irq_state);
+	return ret;
+}
+
+static irqreturn_t emif_interrupt_handler(int irq, void *dev_id)
+{
+	u32			interrupts;
+	struct emif_data	*emif = dev_id;
+	void __iomem		*base = emif->base;
+	struct device		*dev = emif->dev;
+	irqreturn_t		ret = IRQ_HANDLED;
+
+	/* Save the status and clear it */
+	interrupts = readl(base + EMIF_SYSTEM_OCP_INTERRUPT_STATUS);
+	writel(interrupts, base + EMIF_SYSTEM_OCP_INTERRUPT_STATUS);
+
+	/*
+	 * Handle temperature alert
+	 * Temperature alert should be same for all ports
+	 * So, it's enough to process it only for one of the ports
+	 */
+	if (interrupts & TA_SYS_MASK)
+		ret = handle_temp_alert(base, emif);
+
+	if (interrupts & ERR_SYS_MASK)
+		dev_err(dev, "Access error from SYS port - %x\n", interrupts);
+
+	if (emif->plat_data->hw_caps & EMIF_HW_CAPS_LL_INTERFACE) {
+		/* Save the status and clear it */
+		interrupts = readl(base + EMIF_LL_OCP_INTERRUPT_STATUS);
+		writel(interrupts, base + EMIF_LL_OCP_INTERRUPT_STATUS);
+
+		if (interrupts & ERR_LL_MASK)
+			dev_err(dev, "Access error from LL port - %x\n",
+				interrupts);
+	}
+
+	return ret;
+}
+
+static irqreturn_t emif_threaded_isr(int irq, void *dev_id)
+{
+	struct emif_data	*emif = dev_id;
+
+	if (emif->temperature_level == SDRAM_TEMP_VERY_HIGH_SHUTDOWN) {
+		dev_emerg(emif->dev, "SDRAM temperature exceeds operating limit.. Needs shut down!!!\n");
+		kernel_power_off();
+		return IRQ_HANDLED;
+	}
+
+	spin_lock_irqsave(&emif_lock, irq_state);
+
+	if (emif->curr_regs) {
+		setup_temperature_sensitive_regs(emif, emif->curr_regs);
+		do_freq_update();
+	} else {
+		dev_err(emif->dev, "temperature alert before registers are calculated, not de-rating timings\n");
+	}
+
+	spin_unlock_irqrestore(&emif_lock, irq_state);
+
+	return IRQ_HANDLED;
+}
+
+static void clear_all_interrupts(struct emif_data *emif)
+{
+	void __iomem	*base = emif->base;
+
+	writel(readl(base + EMIF_SYSTEM_OCP_INTERRUPT_STATUS),
+		base + EMIF_SYSTEM_OCP_INTERRUPT_STATUS);
+	if (emif->plat_data->hw_caps & EMIF_HW_CAPS_LL_INTERFACE)
+		writel(readl(base + EMIF_LL_OCP_INTERRUPT_STATUS),
+			base + EMIF_LL_OCP_INTERRUPT_STATUS);
+}
+
+static void disable_and_clear_all_interrupts(struct emif_data *emif)
+{
+	void __iomem		*base = emif->base;
+
+	/* Disable all interrupts */
+	writel(readl(base + EMIF_SYSTEM_OCP_INTERRUPT_ENABLE_SET),
+		base + EMIF_SYSTEM_OCP_INTERRUPT_ENABLE_CLEAR);
+	if (emif->plat_data->hw_caps & EMIF_HW_CAPS_LL_INTERFACE)
+		writel(readl(base + EMIF_LL_OCP_INTERRUPT_ENABLE_SET),
+			base + EMIF_LL_OCP_INTERRUPT_ENABLE_CLEAR);
+
+	/* Clear all interrupts */
+	clear_all_interrupts(emif);
+}
+
+static int __init_or_module setup_interrupts(struct emif_data *emif, u32 irq)
+{
+	u32		interrupts, type;
+	void __iomem	*base = emif->base;
+
+	type = emif->plat_data->device_info->type;
+
+	clear_all_interrupts(emif);
+
+	/* Enable interrupts for SYS interface */
+	interrupts = EN_ERR_SYS_MASK;
+	if (type == DDR_TYPE_LPDDR2_S2 || type == DDR_TYPE_LPDDR2_S4)
+		interrupts |= EN_TA_SYS_MASK;
+	writel(interrupts, base + EMIF_SYSTEM_OCP_INTERRUPT_ENABLE_SET);
+
+	/* Enable interrupts for LL interface */
+	if (emif->plat_data->hw_caps & EMIF_HW_CAPS_LL_INTERFACE) {
+		/* TA need not be enabled for LL */
+		interrupts = EN_ERR_LL_MASK;
+		writel(interrupts, base + EMIF_LL_OCP_INTERRUPT_ENABLE_SET);
+	}
+
+	/* setup IRQ handlers */
+	return devm_request_threaded_irq(emif->dev, irq,
+				    emif_interrupt_handler,
+				    emif_threaded_isr,
+				    0, dev_name(emif->dev),
+				    emif);
+
+}
+
+static void __init_or_module emif_onetime_settings(struct emif_data *emif)
+{
+	u32				pwr_mgmt_ctrl, zq, temp_alert_cfg;
+	void __iomem			*base = emif->base;
+	const struct lpddr2_addressing	*addressing;
+	const struct ddr_device_info	*device_info;
+
+	device_info = emif->plat_data->device_info;
+	addressing = get_addressing_table(device_info);
+
+	/*
+	 * Init power management settings
+	 * We don't know the frequency yet. Use a high frequency
+	 * value for a conservative timeout setting
+	 */
+	pwr_mgmt_ctrl = get_pwr_mgmt_ctrl(1000000000, emif,
+			emif->plat_data->ip_rev);
+	emif->lpmode = (pwr_mgmt_ctrl & LP_MODE_MASK) >> LP_MODE_SHIFT;
+	writel(pwr_mgmt_ctrl, base + EMIF_POWER_MANAGEMENT_CONTROL);
+
+	/* Init ZQ calibration settings */
+	zq = get_zq_config_reg(addressing, device_info->cs1_used,
+		device_info->cal_resistors_per_cs);
+	writel(zq, base + EMIF_SDRAM_OUTPUT_IMPEDANCE_CALIBRATION_CONFIG);
+
+	/* Check temperature level temperature level*/
+	get_temperature_level(emif);
+	if (emif->temperature_level == SDRAM_TEMP_VERY_HIGH_SHUTDOWN)
+		dev_emerg(emif->dev, "SDRAM temperature exceeds operating limit.. Needs shut down!!!\n");
+
+	/* Init temperature polling */
+	temp_alert_cfg = get_temp_alert_config(addressing,
+		emif->plat_data->custom_configs, device_info->cs1_used,
+		device_info->io_width, get_emif_bus_width(emif));
+	writel(temp_alert_cfg, base + EMIF_TEMPERATURE_ALERT_CONFIG);
+
+	/*
+	 * Program external PHY control registers that are not frequency
+	 * dependent
+	 */
+	if (emif->plat_data->phy_type != EMIF_PHY_TYPE_INTELLIPHY)
+		return;
+	writel(EMIF_EXT_PHY_CTRL_1_VAL, base + EMIF_EXT_PHY_CTRL_1_SHDW);
+	writel(EMIF_EXT_PHY_CTRL_5_VAL, base + EMIF_EXT_PHY_CTRL_5_SHDW);
+	writel(EMIF_EXT_PHY_CTRL_6_VAL, base + EMIF_EXT_PHY_CTRL_6_SHDW);
+	writel(EMIF_EXT_PHY_CTRL_7_VAL, base + EMIF_EXT_PHY_CTRL_7_SHDW);
+	writel(EMIF_EXT_PHY_CTRL_8_VAL, base + EMIF_EXT_PHY_CTRL_8_SHDW);
+	writel(EMIF_EXT_PHY_CTRL_9_VAL, base + EMIF_EXT_PHY_CTRL_9_SHDW);
+	writel(EMIF_EXT_PHY_CTRL_10_VAL, base + EMIF_EXT_PHY_CTRL_10_SHDW);
+	writel(EMIF_EXT_PHY_CTRL_11_VAL, base + EMIF_EXT_PHY_CTRL_11_SHDW);
+	writel(EMIF_EXT_PHY_CTRL_12_VAL, base + EMIF_EXT_PHY_CTRL_12_SHDW);
+	writel(EMIF_EXT_PHY_CTRL_13_VAL, base + EMIF_EXT_PHY_CTRL_13_SHDW);
+	writel(EMIF_EXT_PHY_CTRL_14_VAL, base + EMIF_EXT_PHY_CTRL_14_SHDW);
+	writel(EMIF_EXT_PHY_CTRL_15_VAL, base + EMIF_EXT_PHY_CTRL_15_SHDW);
+	writel(EMIF_EXT_PHY_CTRL_16_VAL, base + EMIF_EXT_PHY_CTRL_16_SHDW);
+	writel(EMIF_EXT_PHY_CTRL_17_VAL, base + EMIF_EXT_PHY_CTRL_17_SHDW);
+	writel(EMIF_EXT_PHY_CTRL_18_VAL, base + EMIF_EXT_PHY_CTRL_18_SHDW);
+	writel(EMIF_EXT_PHY_CTRL_19_VAL, base + EMIF_EXT_PHY_CTRL_19_SHDW);
+	writel(EMIF_EXT_PHY_CTRL_20_VAL, base + EMIF_EXT_PHY_CTRL_20_SHDW);
+	writel(EMIF_EXT_PHY_CTRL_21_VAL, base + EMIF_EXT_PHY_CTRL_21_SHDW);
+	writel(EMIF_EXT_PHY_CTRL_22_VAL, base + EMIF_EXT_PHY_CTRL_22_SHDW);
+	writel(EMIF_EXT_PHY_CTRL_23_VAL, base + EMIF_EXT_PHY_CTRL_23_SHDW);
+	writel(EMIF_EXT_PHY_CTRL_24_VAL, base + EMIF_EXT_PHY_CTRL_24_SHDW);
+}
+
+static void get_default_timings(struct emif_data *emif)
+{
+	struct emif_platform_data *pd = emif->plat_data;
+
+	pd->timings		= lpddr2_jedec_timings;
+	pd->timings_arr_size	= ARRAY_SIZE(lpddr2_jedec_timings);
+
+	dev_warn(emif->dev, "%s: using default timings\n", __func__);
+}
+
+static int is_dev_data_valid(u32 type, u32 density, u32 io_width, u32 phy_type,
+		u32 ip_rev, struct device *dev)
+{
+	int valid;
+
+	valid = (type == DDR_TYPE_LPDDR2_S4 ||
+			type == DDR_TYPE_LPDDR2_S2)
+		&& (density >= DDR_DENSITY_64Mb
+			&& density <= DDR_DENSITY_8Gb)
+		&& (io_width >= DDR_IO_WIDTH_8
+			&& io_width <= DDR_IO_WIDTH_32);
+
+	/* Combinations of EMIF and PHY revisions that we support today */
+	switch (ip_rev) {
+	case EMIF_4D:
+		valid = valid && (phy_type == EMIF_PHY_TYPE_ATTILAPHY);
+		break;
+	case EMIF_4D5:
+		valid = valid && (phy_type == EMIF_PHY_TYPE_INTELLIPHY);
+		break;
+	default:
+		valid = 0;
+	}
+
+	if (!valid)
+		dev_err(dev, "%s: invalid DDR details\n", __func__);
+	return valid;
+}
+
+static int is_custom_config_valid(struct emif_custom_configs *cust_cfgs,
+		struct device *dev)
+{
+	int valid = 1;
+
+	if ((cust_cfgs->mask & EMIF_CUSTOM_CONFIG_LPMODE) &&
+		(cust_cfgs->lpmode != EMIF_LP_MODE_DISABLE))
+		valid = cust_cfgs->lpmode_freq_threshold &&
+			cust_cfgs->lpmode_timeout_performance &&
+			cust_cfgs->lpmode_timeout_power;
+
+	if (cust_cfgs->mask & EMIF_CUSTOM_CONFIG_TEMP_ALERT_POLL_INTERVAL)
+		valid = valid && cust_cfgs->temp_alert_poll_interval_ms;
+
+	if (!valid)
+		dev_warn(dev, "%s: invalid custom configs\n", __func__);
+
+	return valid;
+}
+
+static struct emif_data *__init_or_module get_device_details(
+		struct platform_device *pdev)
+{
+	u32				size;
+	struct emif_data		*emif = NULL;
+	struct ddr_device_info		*dev_info;
+	struct emif_custom_configs	*cust_cfgs;
+	struct emif_platform_data	*pd;
+	struct device			*dev;
+	void				*temp;
+
+	pd = pdev->dev.platform_data;
+	dev = &pdev->dev;
+
+	if (!(pd && pd->device_info && is_dev_data_valid(pd->device_info->type,
+			pd->device_info->density, pd->device_info->io_width,
+			pd->phy_type, pd->ip_rev, dev))) {
+		dev_err(dev, "%s: invalid device data\n", __func__);
+		goto error;
+	}
+
+	emif	= devm_kzalloc(dev, sizeof(*emif), GFP_KERNEL);
+	temp	= devm_kzalloc(dev, sizeof(*pd), GFP_KERNEL);
+	dev_info = devm_kzalloc(dev, sizeof(*dev_info), GFP_KERNEL);
+
+	if (!emif || !pd || !dev_info) {
+		dev_err(dev, "%s:%d: allocation error\n", __func__, __LINE__);
+		goto error;
+	}
+
+	memcpy(temp, pd, sizeof(*pd));
+	pd = temp;
+	memcpy(dev_info, pd->device_info, sizeof(*dev_info));
+
+	pd->device_info		= dev_info;
+	emif->plat_data		= pd;
+	emif->dev		= dev;
+	emif->temperature_level	= SDRAM_TEMP_NOMINAL;
+
+	/*
+	 * For EMIF instances other than EMIF1 see if the devices connected
+	 * are exactly same as on EMIF1(which is typically the case). If so,
+	 * mark it as a duplicate of EMIF1 and skip copying timings data.
+	 * This will save some memory and some computation later.
+	 */
+	emif->duplicate = emif1 && (memcmp(dev_info,
+		emif1->plat_data->device_info,
+		sizeof(struct ddr_device_info)) == 0);
+
+	if (emif->duplicate) {
+		pd->timings = NULL;
+		pd->min_tck = NULL;
+		goto out;
+	} else if (emif1) {
+		dev_warn(emif->dev, "%s: Non-symmetric DDR geometry\n",
+			__func__);
+	}
+
+	/*
+	 * Copy custom configs - ignore allocation error, if any, as
+	 * custom_configs is not very critical
+	 */
+	cust_cfgs = pd->custom_configs;
+	if (cust_cfgs && is_custom_config_valid(cust_cfgs, dev)) {
+		temp = devm_kzalloc(dev, sizeof(*cust_cfgs), GFP_KERNEL);
+		if (temp)
+			memcpy(temp, cust_cfgs, sizeof(*cust_cfgs));
+		else
+			dev_warn(dev, "%s:%d: allocation error\n", __func__,
+				__LINE__);
+		pd->custom_configs = temp;
+	}
+
+	/*
+	 * Copy timings and min-tck values from platform data. If it is not
+	 * available or if memory allocation fails, use JEDEC defaults
+	 */
+	size = sizeof(struct lpddr2_timings) * pd->timings_arr_size;
+	if (pd->timings) {
+		temp = devm_kzalloc(dev, size, GFP_KERNEL);
+		if (temp) {
+			memcpy(temp, pd->timings, sizeof(*pd->timings));
+			pd->timings = temp;
+		} else {
+			dev_warn(dev, "%s:%d: allocation error\n", __func__,
+				__LINE__);
+			get_default_timings(emif);
+		}
+	} else {
+		get_default_timings(emif);
+	}
+
+	if (pd->min_tck) {
+		temp = devm_kzalloc(dev, sizeof(*pd->min_tck), GFP_KERNEL);
+		if (temp) {
+			memcpy(temp, pd->min_tck, sizeof(*pd->min_tck));
+			pd->min_tck = temp;
+		} else {
+			dev_warn(dev, "%s:%d: allocation error\n", __func__,
+				__LINE__);
+			pd->min_tck = &lpddr2_jedec_min_tck;
+		}
+	} else {
+		pd->min_tck = &lpddr2_jedec_min_tck;
+	}
+
+out:
+	return emif;
+
+error:
+	return NULL;
+}
+
+static int __init_or_module emif_probe(struct platform_device *pdev)
+{
+	struct emif_data	*emif;
+	struct resource		*res;
+	int			irq;
+
+	emif = get_device_details(pdev);
+	if (!emif) {
+		pr_err("%s: error getting device data\n", __func__);
+		goto error;
+	}
+
+	list_add(&emif->node, &device_list);
+	emif->addressing = get_addressing_table(emif->plat_data->device_info);
+
+	/* Save pointers to each other in emif and device structures */
+	emif->dev = &pdev->dev;
+	platform_set_drvdata(pdev, emif);
+
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	if (!res) {
+		dev_err(emif->dev, "%s: error getting memory resource\n",
+			__func__);
+		goto error;
+	}
+
+	emif->base = devm_request_and_ioremap(emif->dev, res);
+	if (!emif->base) {
+		dev_err(emif->dev, "%s: devm_request_and_ioremap() failed\n",
+			__func__);
+		goto error;
+	}
+
+	irq = platform_get_irq(pdev, 0);
+	if (irq < 0) {
+		dev_err(emif->dev, "%s: error getting IRQ resource - %d\n",
+			__func__, irq);
+		goto error;
+	}
+
+	emif_onetime_settings(emif);
+	emif_debugfs_init(emif);
+	disable_and_clear_all_interrupts(emif);
+	setup_interrupts(emif, irq);
+
+	/* One-time actions taken on probing the first device */
+	if (!emif1) {
+		emif1 = emif;
+		spin_lock_init(&emif_lock);
+
+		/*
+		 * TODO: register notifiers for frequency and voltage
+		 * change here once the respective frameworks are
+		 * available
+		 */
+	}
+
+	dev_info(&pdev->dev, "%s: device configured with addr = %p and IRQ%d\n",
+		__func__, emif->base, irq);
+
+	return 0;
+error:
+	return -ENODEV;
+}
+
+static int __exit emif_remove(struct platform_device *pdev)
+{
+	struct emif_data *emif = platform_get_drvdata(pdev);
+
+	emif_debugfs_exit(emif);
+
+	return 0;
+}
+
+static void emif_shutdown(struct platform_device *pdev)
+{
+	struct emif_data	*emif = platform_get_drvdata(pdev);
+
+	disable_and_clear_all_interrupts(emif);
+}
+
+static int get_emif_reg_values(struct emif_data *emif, u32 freq,
+		struct emif_regs *regs)
+{
+	u32				cs1_used, ip_rev, phy_type;
+	u32				cl, type;
+	const struct lpddr2_timings	*timings;
+	const struct lpddr2_min_tck	*min_tck;
+	const struct ddr_device_info	*device_info;
+	const struct lpddr2_addressing	*addressing;
+	struct emif_data		*emif_for_calc;
+	struct device			*dev;
+	const struct emif_custom_configs *custom_configs;
+
+	dev = emif->dev;
+	/*
+	 * If the devices on this EMIF instance is duplicate of EMIF1,
+	 * use EMIF1 details for the calculation
+	 */
+	emif_for_calc	= emif->duplicate ? emif1 : emif;
+	timings		= get_timings_table(emif_for_calc, freq);
+	addressing	= emif_for_calc->addressing;
+	if (!timings || !addressing) {
+		dev_err(dev, "%s: not enough data available for %dHz",
+			__func__, freq);
+		return -1;
+	}
+
+	device_info	= emif_for_calc->plat_data->device_info;
+	type		= device_info->type;
+	cs1_used	= device_info->cs1_used;
+	ip_rev		= emif_for_calc->plat_data->ip_rev;
+	phy_type	= emif_for_calc->plat_data->phy_type;
+
+	min_tck		= emif_for_calc->plat_data->min_tck;
+	custom_configs	= emif_for_calc->plat_data->custom_configs;
+
+	set_ddr_clk_period(freq);
+
+	regs->ref_ctrl_shdw = get_sdram_ref_ctrl_shdw(freq, addressing);
+	regs->sdram_tim1_shdw = get_sdram_tim_1_shdw(timings, min_tck,
+			addressing);
+	regs->sdram_tim2_shdw = get_sdram_tim_2_shdw(timings, min_tck,
+			addressing, type);
+	regs->sdram_tim3_shdw = get_sdram_tim_3_shdw(timings, min_tck,
+		addressing, type, ip_rev, EMIF_NORMAL_TIMINGS);
+
+	cl = get_cl(emif);
+
+	if (phy_type == EMIF_PHY_TYPE_ATTILAPHY && ip_rev == EMIF_4D) {
+		regs->phy_ctrl_1_shdw = get_ddr_phy_ctrl_1_attilaphy_4d(
+			timings, freq, cl);
+	} else if (phy_type == EMIF_PHY_TYPE_INTELLIPHY && ip_rev == EMIF_4D5) {
+		regs->phy_ctrl_1_shdw = get_phy_ctrl_1_intelliphy_4d5(freq, cl);
+		regs->ext_phy_ctrl_2_shdw = get_ext_phy_ctrl_2_intelliphy_4d5();
+		regs->ext_phy_ctrl_3_shdw = get_ext_phy_ctrl_3_intelliphy_4d5();
+		regs->ext_phy_ctrl_4_shdw = get_ext_phy_ctrl_4_intelliphy_4d5();
+	} else {
+		return -1;
+	}
+
+	/* Only timeout values in pwr_mgmt_ctrl_shdw register */
+	regs->pwr_mgmt_ctrl_shdw =
+		get_pwr_mgmt_ctrl(freq, emif_for_calc, ip_rev) &
+		(CS_TIM_MASK | SR_TIM_MASK | PD_TIM_MASK);
+
+	if (ip_rev & EMIF_4D) {
+		regs->read_idle_ctrl_shdw_normal =
+			get_read_idle_ctrl_shdw(DDR_VOLTAGE_STABLE);
+
+		regs->read_idle_ctrl_shdw_volt_ramp =
+			get_read_idle_ctrl_shdw(DDR_VOLTAGE_RAMPING);
+	} else if (ip_rev & EMIF_4D5) {
+		regs->dll_calib_ctrl_shdw_normal =
+			get_dll_calib_ctrl_shdw(DDR_VOLTAGE_STABLE);
+
+		regs->dll_calib_ctrl_shdw_volt_ramp =
+			get_dll_calib_ctrl_shdw(DDR_VOLTAGE_RAMPING);
+	}
+
+	if (type == DDR_TYPE_LPDDR2_S2 || type == DDR_TYPE_LPDDR2_S4) {
+		regs->ref_ctrl_shdw_derated = get_sdram_ref_ctrl_shdw(freq / 4,
+			addressing);
+
+		regs->sdram_tim1_shdw_derated =
+			get_sdram_tim_1_shdw_derated(timings, min_tck,
+				addressing);
+
+		regs->sdram_tim3_shdw_derated = get_sdram_tim_3_shdw(timings,
+			min_tck, addressing, type, ip_rev,
+			EMIF_DERATED_TIMINGS);
+	}
+
+	regs->freq = freq;
+
+	return 0;
+}
+
+/*
+ * get_regs() - gets the cached emif_regs structure for a given EMIF instance
+ * given frequency(freq):
+ *
+ * As an optimisation, every EMIF instance other than EMIF1 shares the
+ * register cache with EMIF1 if the devices connected on this instance
+ * are same as that on EMIF1(indicated by the duplicate flag)
+ *
+ * If we do not have an entry corresponding to the frequency given, we
+ * allocate a new entry and calculate the values
+ *
+ * Upon finding the right reg dump, save it in curr_regs. It can be
+ * directly used for thermal de-rating and voltage ramping changes.
+ */
+static struct emif_regs *get_regs(struct emif_data *emif, u32 freq)
+{
+	int			i;
+	struct emif_regs	**regs_cache;
+	struct emif_regs	*regs = NULL;
+	struct device		*dev;
+
+	dev = emif->dev;
+	if (emif->curr_regs && emif->curr_regs->freq == freq) {
+		dev_dbg(dev, "%s: using curr_regs - %u Hz", __func__, freq);
+		return emif->curr_regs;
+	}
+
+	if (emif->duplicate)
+		regs_cache = emif1->regs_cache;
+	else
+		regs_cache = emif->regs_cache;
+
+	for (i = 0; i < EMIF_MAX_NUM_FREQUENCIES && regs_cache[i]; i++) {
+		if (regs_cache[i]->freq == freq) {
+			regs = regs_cache[i];
+			dev_dbg(dev,
+				"%s: reg dump found in reg cache for %u Hz\n",
+				__func__, freq);
+			break;
+		}
+	}
+
+	/*
+	 * If we don't have an entry for this frequency in the cache create one
+	 * and calculate the values
+	 */
+	if (!regs) {
+		regs = devm_kzalloc(emif->dev, sizeof(*regs), GFP_ATOMIC);
+		if (!regs)
+			return NULL;
+
+		if (get_emif_reg_values(emif, freq, regs)) {
+			devm_kfree(emif->dev, regs);
+			return NULL;
+		}
+
+		/*
+		 * Now look for an un-used entry in the cache and save the
+		 * newly created struct. If there are no free entries
+		 * over-write the last entry
+		 */
+		for (i = 0; i < EMIF_MAX_NUM_FREQUENCIES && regs_cache[i]; i++)
+			;
+
+		if (i >= EMIF_MAX_NUM_FREQUENCIES) {
+			dev_warn(dev, "%s: regs_cache full - reusing a slot!!\n",
+				__func__);
+			i = EMIF_MAX_NUM_FREQUENCIES - 1;
+			devm_kfree(emif->dev, regs_cache[i]);
+		}
+		regs_cache[i] = regs;
+	}
+
+	return regs;
+}
+
+static void do_volt_notify_handling(struct emif_data *emif, u32 volt_state)
+{
+	dev_dbg(emif->dev, "%s: voltage notification : %d", __func__,
+		volt_state);
+
+	if (!emif->curr_regs) {
+		dev_err(emif->dev,
+			"%s: volt-notify before registers are ready: %d\n",
+			__func__, volt_state);
+		return;
+	}
+
+	setup_volt_sensitive_regs(emif, emif->curr_regs, volt_state);
+}
+
+/*
+ * TODO: voltage notify handling should be hooked up to
+ * regulator framework as soon as the necessary support
+ * is available in mainline kernel. This function is un-used
+ * right now.
+ */
+static void __attribute__((unused)) volt_notify_handling(u32 volt_state)
+{
+	struct emif_data *emif;
+
+	spin_lock_irqsave(&emif_lock, irq_state);
+
+	list_for_each_entry(emif, &device_list, node)
+		do_volt_notify_handling(emif, volt_state);
+	do_freq_update();
+
+	spin_unlock_irqrestore(&emif_lock, irq_state);
+}
+
+static void do_freq_pre_notify_handling(struct emif_data *emif, u32 new_freq)
+{
+	struct emif_regs *regs;
+
+	regs = get_regs(emif, new_freq);
+	if (!regs)
+		return;
+
+	emif->curr_regs = regs;
+
+	/*
+	 * Update the shadow registers:
+	 * Temperature and voltage-ramp sensitive settings are also configured
+	 * in terms of DDR cycles. So, we need to update them too when there
+	 * is a freq change
+	 */
+	dev_dbg(emif->dev, "%s: setting up shadow registers for %uHz",
+		__func__, new_freq);
+	setup_registers(emif, regs);
+	setup_temperature_sensitive_regs(emif, regs);
+	setup_volt_sensitive_regs(emif, regs, DDR_VOLTAGE_STABLE);
+
+	/*
+	 * Part of workaround for errata i728. See do_freq_update()
+	 * for more details
+	 */
+	if (emif->lpmode == EMIF_LP_MODE_SELF_REFRESH)
+		set_lpmode(emif, EMIF_LP_MODE_DISABLE);
+}
+
+/*
+ * TODO: frequency notify handling should be hooked up to
+ * clock framework as soon as the necessary support is
+ * available in mainline kernel. This function is un-used
+ * right now.
+ */
+static void __attribute__((unused)) freq_pre_notify_handling(u32 new_freq)
+{
+	struct emif_data *emif;
+
+	/*
+	 * NOTE: we are taking the spin-lock here and releases it
+	 * only in post-notifier. This doesn't look good and
+	 * Sparse complains about it, but this seems to be
+	 * un-avoidable. We need to lock a sequence of events
+	 * that is split between EMIF and clock framework.
+	 *
+	 * 1. EMIF driver updates EMIF timings in shadow registers in the
+	 *    frequency pre-notify callback from clock framework
+	 * 2. clock framework sets up the registers for the new frequency
+	 * 3. clock framework initiates a hw-sequence that updates
+	 *    the frequency EMIF timings synchronously.
+	 *
+	 * All these 3 steps should be performed as an atomic operation
+	 * vis-a-vis similar sequence in the EMIF interrupt handler
+	 * for temperature events. Otherwise, there could be race
+	 * conditions that could result in incorrect EMIF timings for
+	 * a given frequency
+	 */
+	spin_lock_irqsave(&emif_lock, irq_state);
+
+	list_for_each_entry(emif, &device_list, node)
+		do_freq_pre_notify_handling(emif, new_freq);
+}
+
+static void do_freq_post_notify_handling(struct emif_data *emif)
+{
+	/*
+	 * Part of workaround for errata i728. See do_freq_update()
+	 * for more details
+	 */
+	if (emif->lpmode == EMIF_LP_MODE_SELF_REFRESH)
+		set_lpmode(emif, EMIF_LP_MODE_SELF_REFRESH);
+}
+
+/*
+ * TODO: frequency notify handling should be hooked up to
+ * clock framework as soon as the necessary support is
+ * available in mainline kernel. This function is un-used
+ * right now.
+ */
+static void __attribute__((unused)) freq_post_notify_handling(void)
+{
+	struct emif_data *emif;
+
+	list_for_each_entry(emif, &device_list, node)
+		do_freq_post_notify_handling(emif);
+
+	/*
+	 * Lock is done in pre-notify handler. See freq_pre_notify_handling()
+	 * for more details
+	 */
+	spin_unlock_irqrestore(&emif_lock, irq_state);
+}
+
+static struct platform_driver emif_driver = {
+	.remove		= __exit_p(emif_remove),
+	.shutdown	= emif_shutdown,
+	.driver = {
+		.name = "emif",
+	},
+};
+
+static int __init_or_module emif_register(void)
+{
+	return platform_driver_probe(&emif_driver, emif_probe);
+}
+
+static void __exit emif_unregister(void)
+{
+	platform_driver_unregister(&emif_driver);
+}
+
+module_init(emif_register);
+module_exit(emif_unregister);
+MODULE_DESCRIPTION("TI EMIF SDRAM Controller Driver");
+MODULE_LICENSE("GPL");
+MODULE_ALIAS("platform:emif");
+MODULE_AUTHOR("Texas Instruments Inc");
diff --git a/drivers/memory/emif.h b/drivers/memory/emif.h
new file mode 100644
index 00000000000..bfe08bae961
--- /dev/null
+++ b/drivers/memory/emif.h
@@ -0,0 +1,589 @@
+/*
+ * Defines for the EMIF driver
+ *
+ * Copyright (C) 2012 Texas Instruments, Inc.
+ *
+ * Benoit Cousson (b-cousson@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.
+ */
+#ifndef __EMIF_H
+#define __EMIF_H
+
+/*
+ * Maximum number of different frequencies supported by EMIF driver
+ * Determines the number of entries in the pointer array for register
+ * cache
+ */
+#define EMIF_MAX_NUM_FREQUENCIES			6
+
+/* State of the core voltage */
+#define DDR_VOLTAGE_STABLE				0
+#define DDR_VOLTAGE_RAMPING				1
+
+/* Defines for timing De-rating */
+#define EMIF_NORMAL_TIMINGS				0
+#define EMIF_DERATED_TIMINGS				1
+
+/* Length of the forced read idle period in terms of cycles */
+#define EMIF_READ_IDLE_LEN_VAL				5
+
+/*
+ * forced read idle interval to be used when voltage
+ * is changed as part of DVFS/DPS - 1ms
+ */
+#define READ_IDLE_INTERVAL_DVFS				(1*1000000)
+
+/*
+ * Forced read idle interval to be used when voltage is stable
+ * 50us - or maximum value will do
+ */
+#define READ_IDLE_INTERVAL_NORMAL			(50*1000000)
+
+/* DLL calibration interval when voltage is NOT stable - 1us */
+#define DLL_CALIB_INTERVAL_DVFS				(1*1000000)
+
+#define DLL_CALIB_ACK_WAIT_VAL				5
+
+/* Interval between ZQCS commands - hw team recommended value */
+#define EMIF_ZQCS_INTERVAL_US				(50*1000)
+/* Enable ZQ Calibration on exiting Self-refresh */
+#define ZQ_SFEXITEN_ENABLE				1
+/*
+ * ZQ Calibration simultaneously on both chip-selects:
+ * Needs one calibration resistor per CS
+ */
+#define	ZQ_DUALCALEN_DISABLE				0
+#define	ZQ_DUALCALEN_ENABLE				1
+
+#define T_ZQCS_DEFAULT_NS				90
+#define T_ZQCL_DEFAULT_NS				360
+#define T_ZQINIT_DEFAULT_NS				1000
+
+/* DPD_EN */
+#define DPD_DISABLE					0
+#define DPD_ENABLE					1
+
+/*
+ * Default values for the low-power entry to be used if not provided by user.
+ * OMAP4/5 has a hw bug(i735) due to which this value can not be less than 512
+ * Timeout values are in DDR clock 'cycles' and frequency threshold in Hz
+ */
+#define EMIF_LP_MODE_TIMEOUT_PERFORMANCE		2048
+#define EMIF_LP_MODE_TIMEOUT_POWER			512
+#define EMIF_LP_MODE_FREQ_THRESHOLD			400000000
+
+/* DDR_PHY_CTRL_1 values for EMIF4D - ATTILA PHY combination */
+#define EMIF_DDR_PHY_CTRL_1_BASE_VAL_ATTILAPHY		0x049FF000
+#define EMIF_DLL_SLAVE_DLY_CTRL_400_MHZ_ATTILAPHY	0x41
+#define EMIF_DLL_SLAVE_DLY_CTRL_200_MHZ_ATTILAPHY	0x80
+#define EMIF_DLL_SLAVE_DLY_CTRL_100_MHZ_AND_LESS_ATTILAPHY 0xFF
+
+/* DDR_PHY_CTRL_1 values for EMIF4D5 INTELLIPHY combination */
+#define EMIF_DDR_PHY_CTRL_1_BASE_VAL_INTELLIPHY		0x0E084200
+#define EMIF_PHY_TOTAL_READ_LATENCY_INTELLIPHY_PS	10000
+
+/* TEMP_ALERT_CONFIG - corresponding to temp gradient 5 C/s */
+#define TEMP_ALERT_POLL_INTERVAL_DEFAULT_MS		360
+
+#define EMIF_T_CSTA					3
+#define EMIF_T_PDLL_UL					128
+
+/* External PHY control registers magic values */
+#define EMIF_EXT_PHY_CTRL_1_VAL				0x04020080
+#define EMIF_EXT_PHY_CTRL_5_VAL				0x04010040
+#define EMIF_EXT_PHY_CTRL_6_VAL				0x01004010
+#define EMIF_EXT_PHY_CTRL_7_VAL				0x00001004
+#define EMIF_EXT_PHY_CTRL_8_VAL				0x04010040
+#define EMIF_EXT_PHY_CTRL_9_VAL				0x01004010
+#define EMIF_EXT_PHY_CTRL_10_VAL			0x00001004
+#define EMIF_EXT_PHY_CTRL_11_VAL			0x00000000
+#define EMIF_EXT_PHY_CTRL_12_VAL			0x00000000
+#define EMIF_EXT_PHY_CTRL_13_VAL			0x00000000
+#define EMIF_EXT_PHY_CTRL_14_VAL			0x80080080
+#define EMIF_EXT_PHY_CTRL_15_VAL			0x00800800
+#define EMIF_EXT_PHY_CTRL_16_VAL			0x08102040
+#define EMIF_EXT_PHY_CTRL_17_VAL			0x00000001
+#define EMIF_EXT_PHY_CTRL_18_VAL			0x540A8150
+#define EMIF_EXT_PHY_CTRL_19_VAL			0xA81502A0
+#define EMIF_EXT_PHY_CTRL_20_VAL			0x002A0540
+#define EMIF_EXT_PHY_CTRL_21_VAL			0x00000000
+#define EMIF_EXT_PHY_CTRL_22_VAL			0x00000000
+#define EMIF_EXT_PHY_CTRL_23_VAL			0x00000000
+#define EMIF_EXT_PHY_CTRL_24_VAL			0x00000077
+
+#define EMIF_INTELLI_PHY_DQS_GATE_OPENING_DELAY_PS	1200
+
+/* Registers offset */
+#define EMIF_MODULE_ID_AND_REVISION			0x0000
+#define EMIF_STATUS					0x0004
+#define EMIF_SDRAM_CONFIG				0x0008
+#define EMIF_SDRAM_CONFIG_2				0x000c
+#define EMIF_SDRAM_REFRESH_CONTROL			0x0010
+#define EMIF_SDRAM_REFRESH_CTRL_SHDW			0x0014
+#define EMIF_SDRAM_TIMING_1				0x0018
+#define EMIF_SDRAM_TIMING_1_SHDW			0x001c
+#define EMIF_SDRAM_TIMING_2				0x0020
+#define EMIF_SDRAM_TIMING_2_SHDW			0x0024
+#define EMIF_SDRAM_TIMING_3				0x0028
+#define EMIF_SDRAM_TIMING_3_SHDW			0x002c
+#define EMIF_LPDDR2_NVM_TIMING				0x0030
+#define EMIF_LPDDR2_NVM_TIMING_SHDW			0x0034
+#define EMIF_POWER_MANAGEMENT_CONTROL			0x0038
+#define EMIF_POWER_MANAGEMENT_CTRL_SHDW			0x003c
+#define EMIF_LPDDR2_MODE_REG_DATA			0x0040
+#define EMIF_LPDDR2_MODE_REG_CONFIG			0x0050
+#define EMIF_OCP_CONFIG					0x0054
+#define EMIF_OCP_CONFIG_VALUE_1				0x0058
+#define EMIF_OCP_CONFIG_VALUE_2				0x005c
+#define EMIF_IODFT_TEST_LOGIC_GLOBAL_CONTROL		0x0060
+#define EMIF_IODFT_TEST_LOGIC_CTRL_MISR_RESULT		0x0064
+#define EMIF_IODFT_TEST_LOGIC_ADDRESS_MISR_RESULT	0x0068
+#define EMIF_IODFT_TEST_LOGIC_DATA_MISR_RESULT_1	0x006c
+#define EMIF_IODFT_TEST_LOGIC_DATA_MISR_RESULT_2	0x0070
+#define EMIF_IODFT_TEST_LOGIC_DATA_MISR_RESULT_3	0x0074
+#define EMIF_PERFORMANCE_COUNTER_1			0x0080
+#define EMIF_PERFORMANCE_COUNTER_2			0x0084
+#define EMIF_PERFORMANCE_COUNTER_CONFIG			0x0088
+#define EMIF_PERFORMANCE_COUNTER_MASTER_REGION_SELECT	0x008c
+#define EMIF_PERFORMANCE_COUNTER_TIME			0x0090
+#define EMIF_MISC_REG					0x0094
+#define EMIF_DLL_CALIB_CTRL				0x0098
+#define EMIF_DLL_CALIB_CTRL_SHDW			0x009c
+#define EMIF_END_OF_INTERRUPT				0x00a0
+#define EMIF_SYSTEM_OCP_INTERRUPT_RAW_STATUS		0x00a4
+#define EMIF_LL_OCP_INTERRUPT_RAW_STATUS		0x00a8
+#define EMIF_SYSTEM_OCP_INTERRUPT_STATUS		0x00ac
+#define EMIF_LL_OCP_INTERRUPT_STATUS			0x00b0
+#define EMIF_SYSTEM_OCP_INTERRUPT_ENABLE_SET		0x00b4
+#define EMIF_LL_OCP_INTERRUPT_ENABLE_SET		0x00b8
+#define EMIF_SYSTEM_OCP_INTERRUPT_ENABLE_CLEAR		0x00bc
+#define EMIF_LL_OCP_INTERRUPT_ENABLE_CLEAR		0x00c0
+#define EMIF_SDRAM_OUTPUT_IMPEDANCE_CALIBRATION_CONFIG	0x00c8
+#define EMIF_TEMPERATURE_ALERT_CONFIG			0x00cc
+#define EMIF_OCP_ERROR_LOG				0x00d0
+#define EMIF_READ_WRITE_LEVELING_RAMP_WINDOW		0x00d4
+#define EMIF_READ_WRITE_LEVELING_RAMP_CONTROL		0x00d8
+#define EMIF_READ_WRITE_LEVELING_CONTROL		0x00dc
+#define EMIF_DDR_PHY_CTRL_1				0x00e4
+#define EMIF_DDR_PHY_CTRL_1_SHDW			0x00e8
+#define EMIF_DDR_PHY_CTRL_2				0x00ec
+#define EMIF_PRIORITY_TO_CLASS_OF_SERVICE_MAPPING	0x0100
+#define EMIF_CONNECTION_ID_TO_CLASS_OF_SERVICE_1_MAPPING 0x0104
+#define EMIF_CONNECTION_ID_TO_CLASS_OF_SERVICE_2_MAPPING 0x0108
+#define EMIF_READ_WRITE_EXECUTION_THRESHOLD		0x0120
+#define EMIF_COS_CONFIG					0x0124
+#define EMIF_PHY_STATUS_1				0x0140
+#define EMIF_PHY_STATUS_2				0x0144
+#define EMIF_PHY_STATUS_3				0x0148
+#define EMIF_PHY_STATUS_4				0x014c
+#define EMIF_PHY_STATUS_5				0x0150
+#define EMIF_PHY_STATUS_6				0x0154
+#define EMIF_PHY_STATUS_7				0x0158
+#define EMIF_PHY_STATUS_8				0x015c
+#define EMIF_PHY_STATUS_9				0x0160
+#define EMIF_PHY_STATUS_10				0x0164
+#define EMIF_PHY_STATUS_11				0x0168
+#define EMIF_PHY_STATUS_12				0x016c
+#define EMIF_PHY_STATUS_13				0x0170
+#define EMIF_PHY_STATUS_14				0x0174
+#define EMIF_PHY_STATUS_15				0x0178
+#define EMIF_PHY_STATUS_16				0x017c
+#define EMIF_PHY_STATUS_17				0x0180
+#define EMIF_PHY_STATUS_18				0x0184
+#define EMIF_PHY_STATUS_19				0x0188
+#define EMIF_PHY_STATUS_20				0x018c
+#define EMIF_PHY_STATUS_21				0x0190
+#define EMIF_EXT_PHY_CTRL_1				0x0200
+#define EMIF_EXT_PHY_CTRL_1_SHDW			0x0204
+#define EMIF_EXT_PHY_CTRL_2				0x0208
+#define EMIF_EXT_PHY_CTRL_2_SHDW			0x020c
+#define EMIF_EXT_PHY_CTRL_3				0x0210
+#define EMIF_EXT_PHY_CTRL_3_SHDW			0x0214
+#define EMIF_EXT_PHY_CTRL_4				0x0218
+#define EMIF_EXT_PHY_CTRL_4_SHDW			0x021c
+#define EMIF_EXT_PHY_CTRL_5				0x0220
+#define EMIF_EXT_PHY_CTRL_5_SHDW			0x0224
+#define EMIF_EXT_PHY_CTRL_6				0x0228
+#define EMIF_EXT_PHY_CTRL_6_SHDW			0x022c
+#define EMIF_EXT_PHY_CTRL_7				0x0230
+#define EMIF_EXT_PHY_CTRL_7_SHDW			0x0234
+#define EMIF_EXT_PHY_CTRL_8				0x0238
+#define EMIF_EXT_PHY_CTRL_8_SHDW			0x023c
+#define EMIF_EXT_PHY_CTRL_9				0x0240
+#define EMIF_EXT_PHY_CTRL_9_SHDW			0x0244
+#define EMIF_EXT_PHY_CTRL_10				0x0248
+#define EMIF_EXT_PHY_CTRL_10_SHDW			0x024c
+#define EMIF_EXT_PHY_CTRL_11				0x0250
+#define EMIF_EXT_PHY_CTRL_11_SHDW			0x0254
+#define EMIF_EXT_PHY_CTRL_12				0x0258
+#define EMIF_EXT_PHY_CTRL_12_SHDW			0x025c
+#define EMIF_EXT_PHY_CTRL_13				0x0260
+#define EMIF_EXT_PHY_CTRL_13_SHDW			0x0264
+#define EMIF_EXT_PHY_CTRL_14				0x0268
+#define EMIF_EXT_PHY_CTRL_14_SHDW			0x026c
+#define EMIF_EXT_PHY_CTRL_15				0x0270
+#define EMIF_EXT_PHY_CTRL_15_SHDW			0x0274
+#define EMIF_EXT_PHY_CTRL_16				0x0278
+#define EMIF_EXT_PHY_CTRL_16_SHDW			0x027c
+#define EMIF_EXT_PHY_CTRL_17				0x0280
+#define EMIF_EXT_PHY_CTRL_17_SHDW			0x0284
+#define EMIF_EXT_PHY_CTRL_18				0x0288
+#define EMIF_EXT_PHY_CTRL_18_SHDW			0x028c
+#define EMIF_EXT_PHY_CTRL_19				0x0290
+#define EMIF_EXT_PHY_CTRL_19_SHDW			0x0294
+#define EMIF_EXT_PHY_CTRL_20				0x0298
+#define EMIF_EXT_PHY_CTRL_20_SHDW			0x029c
+#define EMIF_EXT_PHY_CTRL_21				0x02a0
+#define EMIF_EXT_PHY_CTRL_21_SHDW			0x02a4
+#define EMIF_EXT_PHY_CTRL_22				0x02a8
+#define EMIF_EXT_PHY_CTRL_22_SHDW			0x02ac
+#define EMIF_EXT_PHY_CTRL_23				0x02b0
+#define EMIF_EXT_PHY_CTRL_23_SHDW			0x02b4
+#define EMIF_EXT_PHY_CTRL_24				0x02b8
+#define EMIF_EXT_PHY_CTRL_24_SHDW			0x02bc
+#define EMIF_EXT_PHY_CTRL_25				0x02c0
+#define EMIF_EXT_PHY_CTRL_25_SHDW			0x02c4
+#define EMIF_EXT_PHY_CTRL_26				0x02c8
+#define EMIF_EXT_PHY_CTRL_26_SHDW			0x02cc
+#define EMIF_EXT_PHY_CTRL_27				0x02d0
+#define EMIF_EXT_PHY_CTRL_27_SHDW			0x02d4
+#define EMIF_EXT_PHY_CTRL_28				0x02d8
+#define EMIF_EXT_PHY_CTRL_28_SHDW			0x02dc
+#define EMIF_EXT_PHY_CTRL_29				0x02e0
+#define EMIF_EXT_PHY_CTRL_29_SHDW			0x02e4
+#define EMIF_EXT_PHY_CTRL_30				0x02e8
+#define EMIF_EXT_PHY_CTRL_30_SHDW			0x02ec
+
+/* Registers shifts and masks */
+
+/* EMIF_MODULE_ID_AND_REVISION */
+#define SCHEME_SHIFT					30
+#define SCHEME_MASK					(0x3 << 30)
+#define MODULE_ID_SHIFT					16
+#define MODULE_ID_MASK					(0xfff << 16)
+#define RTL_VERSION_SHIFT				11
+#define RTL_VERSION_MASK				(0x1f << 11)
+#define MAJOR_REVISION_SHIFT				8
+#define MAJOR_REVISION_MASK				(0x7 << 8)
+#define MINOR_REVISION_SHIFT				0
+#define MINOR_REVISION_MASK				(0x3f << 0)
+
+/* STATUS */
+#define BE_SHIFT					31
+#define BE_MASK						(1 << 31)
+#define DUAL_CLK_MODE_SHIFT				30
+#define DUAL_CLK_MODE_MASK				(1 << 30)
+#define FAST_INIT_SHIFT					29
+#define FAST_INIT_MASK					(1 << 29)
+#define RDLVLGATETO_SHIFT				6
+#define RDLVLGATETO_MASK				(1 << 6)
+#define RDLVLTO_SHIFT					5
+#define RDLVLTO_MASK					(1 << 5)
+#define WRLVLTO_SHIFT					4
+#define WRLVLTO_MASK					(1 << 4)
+#define PHY_DLL_READY_SHIFT				2
+#define PHY_DLL_READY_MASK				(1 << 2)
+
+/* SDRAM_CONFIG */
+#define SDRAM_TYPE_SHIFT				29
+#define SDRAM_TYPE_MASK					(0x7 << 29)
+#define IBANK_POS_SHIFT					27
+#define IBANK_POS_MASK					(0x3 << 27)
+#define DDR_TERM_SHIFT					24
+#define DDR_TERM_MASK					(0x7 << 24)
+#define DDR2_DDQS_SHIFT					23
+#define DDR2_DDQS_MASK					(1 << 23)
+#define DYN_ODT_SHIFT					21
+#define DYN_ODT_MASK					(0x3 << 21)
+#define DDR_DISABLE_DLL_SHIFT				20
+#define DDR_DISABLE_DLL_MASK				(1 << 20)
+#define SDRAM_DRIVE_SHIFT				18
+#define SDRAM_DRIVE_MASK				(0x3 << 18)
+#define CWL_SHIFT					16
+#define CWL_MASK					(0x3 << 16)
+#define NARROW_MODE_SHIFT				14
+#define NARROW_MODE_MASK				(0x3 << 14)
+#define CL_SHIFT					10
+#define CL_MASK						(0xf << 10)
+#define ROWSIZE_SHIFT					7
+#define ROWSIZE_MASK					(0x7 << 7)
+#define IBANK_SHIFT					4
+#define IBANK_MASK					(0x7 << 4)
+#define EBANK_SHIFT					3
+#define EBANK_MASK					(1 << 3)
+#define PAGESIZE_SHIFT					0
+#define PAGESIZE_MASK					(0x7 << 0)
+
+/* SDRAM_CONFIG_2 */
+#define CS1NVMEN_SHIFT					30
+#define CS1NVMEN_MASK					(1 << 30)
+#define EBANK_POS_SHIFT					27
+#define EBANK_POS_MASK					(1 << 27)
+#define RDBNUM_SHIFT					4
+#define RDBNUM_MASK					(0x3 << 4)
+#define RDBSIZE_SHIFT					0
+#define RDBSIZE_MASK					(0x7 << 0)
+
+/* SDRAM_REFRESH_CONTROL */
+#define INITREF_DIS_SHIFT				31
+#define INITREF_DIS_MASK				(1 << 31)
+#define SRT_SHIFT					29
+#define SRT_MASK					(1 << 29)
+#define ASR_SHIFT					28
+#define ASR_MASK					(1 << 28)
+#define PASR_SHIFT					24
+#define PASR_MASK					(0x7 << 24)
+#define REFRESH_RATE_SHIFT				0
+#define REFRESH_RATE_MASK				(0xffff << 0)
+
+/* SDRAM_TIMING_1 */
+#define T_RTW_SHIFT					29
+#define T_RTW_MASK					(0x7 << 29)
+#define T_RP_SHIFT					25
+#define T_RP_MASK					(0xf << 25)
+#define T_RCD_SHIFT					21
+#define T_RCD_MASK					(0xf << 21)
+#define T_WR_SHIFT					17
+#define T_WR_MASK					(0xf << 17)
+#define T_RAS_SHIFT					12
+#define T_RAS_MASK					(0x1f << 12)
+#define T_RC_SHIFT					6
+#define T_RC_MASK					(0x3f << 6)
+#define T_RRD_SHIFT					3
+#define T_RRD_MASK					(0x7 << 3)
+#define T_WTR_SHIFT					0
+#define T_WTR_MASK					(0x7 << 0)
+
+/* SDRAM_TIMING_2 */
+#define T_XP_SHIFT					28
+#define T_XP_MASK					(0x7 << 28)
+#define T_ODT_SHIFT					25
+#define T_ODT_MASK					(0x7 << 25)
+#define T_XSNR_SHIFT					16
+#define T_XSNR_MASK					(0x1ff << 16)
+#define T_XSRD_SHIFT					6
+#define T_XSRD_MASK					(0x3ff << 6)
+#define T_RTP_SHIFT					3
+#define T_RTP_MASK					(0x7 << 3)
+#define T_CKE_SHIFT					0
+#define T_CKE_MASK					(0x7 << 0)
+
+/* SDRAM_TIMING_3 */
+#define T_PDLL_UL_SHIFT					28
+#define T_PDLL_UL_MASK					(0xf << 28)
+#define T_CSTA_SHIFT					24
+#define T_CSTA_MASK					(0xf << 24)
+#define T_CKESR_SHIFT					21
+#define T_CKESR_MASK					(0x7 << 21)
+#define ZQ_ZQCS_SHIFT					15
+#define ZQ_ZQCS_MASK					(0x3f << 15)
+#define T_TDQSCKMAX_SHIFT				13
+#define T_TDQSCKMAX_MASK				(0x3 << 13)
+#define T_RFC_SHIFT					4
+#define T_RFC_MASK					(0x1ff << 4)
+#define T_RAS_MAX_SHIFT					0
+#define T_RAS_MAX_MASK					(0xf << 0)
+
+/* POWER_MANAGEMENT_CONTROL */
+#define PD_TIM_SHIFT					12
+#define PD_TIM_MASK					(0xf << 12)
+#define DPD_EN_SHIFT					11
+#define DPD_EN_MASK					(1 << 11)
+#define LP_MODE_SHIFT					8
+#define LP_MODE_MASK					(0x7 << 8)
+#define SR_TIM_SHIFT					4
+#define SR_TIM_MASK					(0xf << 4)
+#define CS_TIM_SHIFT					0
+#define CS_TIM_MASK					(0xf << 0)
+
+/* LPDDR2_MODE_REG_DATA */
+#define VALUE_0_SHIFT					0
+#define VALUE_0_MASK					(0x7f << 0)
+
+/* LPDDR2_MODE_REG_CONFIG */
+#define CS_SHIFT					31
+#define CS_MASK						(1 << 31)
+#define REFRESH_EN_SHIFT				30
+#define REFRESH_EN_MASK					(1 << 30)
+#define ADDRESS_SHIFT					0
+#define ADDRESS_MASK					(0xff << 0)
+
+/* OCP_CONFIG */
+#define SYS_THRESH_MAX_SHIFT				24
+#define SYS_THRESH_MAX_MASK				(0xf << 24)
+#define MPU_THRESH_MAX_SHIFT				20
+#define MPU_THRESH_MAX_MASK				(0xf << 20)
+#define LL_THRESH_MAX_SHIFT				16
+#define LL_THRESH_MAX_MASK				(0xf << 16)
+
+/* PERFORMANCE_COUNTER_1 */
+#define COUNTER1_SHIFT					0
+#define COUNTER1_MASK					(0xffffffff << 0)
+
+/* PERFORMANCE_COUNTER_2 */
+#define COUNTER2_SHIFT					0
+#define COUNTER2_MASK					(0xffffffff << 0)
+
+/* PERFORMANCE_COUNTER_CONFIG */
+#define CNTR2_MCONNID_EN_SHIFT				31
+#define CNTR2_MCONNID_EN_MASK				(1 << 31)
+#define CNTR2_REGION_EN_SHIFT				30
+#define CNTR2_REGION_EN_MASK				(1 << 30)
+#define CNTR2_CFG_SHIFT					16
+#define CNTR2_CFG_MASK					(0xf << 16)
+#define CNTR1_MCONNID_EN_SHIFT				15
+#define CNTR1_MCONNID_EN_MASK				(1 << 15)
+#define CNTR1_REGION_EN_SHIFT				14
+#define CNTR1_REGION_EN_MASK				(1 << 14)
+#define CNTR1_CFG_SHIFT					0
+#define CNTR1_CFG_MASK					(0xf << 0)
+
+/* PERFORMANCE_COUNTER_MASTER_REGION_SELECT */
+#define MCONNID2_SHIFT					24
+#define MCONNID2_MASK					(0xff << 24)
+#define REGION_SEL2_SHIFT				16
+#define REGION_SEL2_MASK				(0x3 << 16)
+#define MCONNID1_SHIFT					8
+#define MCONNID1_MASK					(0xff << 8)
+#define REGION_SEL1_SHIFT				0
+#define REGION_SEL1_MASK				(0x3 << 0)
+
+/* PERFORMANCE_COUNTER_TIME */
+#define TOTAL_TIME_SHIFT				0
+#define TOTAL_TIME_MASK					(0xffffffff << 0)
+
+/* DLL_CALIB_CTRL */
+#define ACK_WAIT_SHIFT					16
+#define ACK_WAIT_MASK					(0xf << 16)
+#define DLL_CALIB_INTERVAL_SHIFT			0
+#define DLL_CALIB_INTERVAL_MASK				(0x1ff << 0)
+
+/* END_OF_INTERRUPT */
+#define EOI_SHIFT					0
+#define EOI_MASK					(1 << 0)
+
+/* SYSTEM_OCP_INTERRUPT_RAW_STATUS */
+#define DNV_SYS_SHIFT					2
+#define DNV_SYS_MASK					(1 << 2)
+#define TA_SYS_SHIFT					1
+#define TA_SYS_MASK					(1 << 1)
+#define ERR_SYS_SHIFT					0
+#define ERR_SYS_MASK					(1 << 0)
+
+/* LOW_LATENCY_OCP_INTERRUPT_RAW_STATUS */
+#define DNV_LL_SHIFT					2
+#define DNV_LL_MASK					(1 << 2)
+#define TA_LL_SHIFT					1
+#define TA_LL_MASK					(1 << 1)
+#define ERR_LL_SHIFT					0
+#define ERR_LL_MASK					(1 << 0)
+
+/* SYSTEM_OCP_INTERRUPT_ENABLE_SET */
+#define EN_DNV_SYS_SHIFT				2
+#define EN_DNV_SYS_MASK					(1 << 2)
+#define EN_TA_SYS_SHIFT					1
+#define EN_TA_SYS_MASK					(1 << 1)
+#define EN_ERR_SYS_SHIFT					0
+#define EN_ERR_SYS_MASK					(1 << 0)
+
+/* LOW_LATENCY_OCP_INTERRUPT_ENABLE_SET */
+#define EN_DNV_LL_SHIFT					2
+#define EN_DNV_LL_MASK					(1 << 2)
+#define EN_TA_LL_SHIFT					1
+#define EN_TA_LL_MASK					(1 << 1)
+#define EN_ERR_LL_SHIFT					0
+#define EN_ERR_LL_MASK					(1 << 0)
+
+/* SDRAM_OUTPUT_IMPEDANCE_CALIBRATION_CONFIG */
+#define ZQ_CS1EN_SHIFT					31
+#define ZQ_CS1EN_MASK					(1 << 31)
+#define ZQ_CS0EN_SHIFT					30
+#define ZQ_CS0EN_MASK					(1 << 30)
+#define ZQ_DUALCALEN_SHIFT				29
+#define ZQ_DUALCALEN_MASK				(1 << 29)
+#define ZQ_SFEXITEN_SHIFT				28
+#define ZQ_SFEXITEN_MASK				(1 << 28)
+#define ZQ_ZQINIT_MULT_SHIFT				18
+#define ZQ_ZQINIT_MULT_MASK				(0x3 << 18)
+#define ZQ_ZQCL_MULT_SHIFT				16
+#define ZQ_ZQCL_MULT_MASK				(0x3 << 16)
+#define ZQ_REFINTERVAL_SHIFT				0
+#define ZQ_REFINTERVAL_MASK				(0xffff << 0)
+
+/* TEMPERATURE_ALERT_CONFIG */
+#define TA_CS1EN_SHIFT					31
+#define TA_CS1EN_MASK					(1 << 31)
+#define TA_CS0EN_SHIFT					30
+#define TA_CS0EN_MASK					(1 << 30)
+#define TA_SFEXITEN_SHIFT				28
+#define TA_SFEXITEN_MASK				(1 << 28)
+#define TA_DEVWDT_SHIFT					26
+#define TA_DEVWDT_MASK					(0x3 << 26)
+#define TA_DEVCNT_SHIFT					24
+#define TA_DEVCNT_MASK					(0x3 << 24)
+#define TA_REFINTERVAL_SHIFT				0
+#define TA_REFINTERVAL_MASK				(0x3fffff << 0)
+
+/* OCP_ERROR_LOG */
+#define MADDRSPACE_SHIFT				14
+#define MADDRSPACE_MASK					(0x3 << 14)
+#define MBURSTSEQ_SHIFT					11
+#define MBURSTSEQ_MASK					(0x7 << 11)
+#define MCMD_SHIFT					8
+#define MCMD_MASK					(0x7 << 8)
+#define MCONNID_SHIFT					0
+#define MCONNID_MASK					(0xff << 0)
+
+/* DDR_PHY_CTRL_1 - EMIF4D */
+#define DLL_SLAVE_DLY_CTRL_SHIFT_4D			4
+#define DLL_SLAVE_DLY_CTRL_MASK_4D			(0xFF << 4)
+#define READ_LATENCY_SHIFT_4D				0
+#define READ_LATENCY_MASK_4D				(0xf << 0)
+
+/* DDR_PHY_CTRL_1 - EMIF4D5 */
+#define DLL_HALF_DELAY_SHIFT_4D5			21
+#define DLL_HALF_DELAY_MASK_4D5				(1 << 21)
+#define READ_LATENCY_SHIFT_4D5				0
+#define READ_LATENCY_MASK_4D5				(0x1f << 0)
+
+/* DDR_PHY_CTRL_1_SHDW */
+#define DDR_PHY_CTRL_1_SHDW_SHIFT			5
+#define DDR_PHY_CTRL_1_SHDW_MASK			(0x7ffffff << 5)
+#define READ_LATENCY_SHDW_SHIFT				0
+#define READ_LATENCY_SHDW_MASK				(0x1f << 0)
+
+#ifndef __ASSEMBLY__
+/*
+ * Structure containing shadow of important registers in EMIF
+ * The calculation function fills in this structure to be later used for
+ * initialisation and DVFS
+ */
+struct emif_regs {
+	u32 freq;
+	u32 ref_ctrl_shdw;
+	u32 ref_ctrl_shdw_derated;
+	u32 sdram_tim1_shdw;
+	u32 sdram_tim1_shdw_derated;
+	u32 sdram_tim2_shdw;
+	u32 sdram_tim3_shdw;
+	u32 sdram_tim3_shdw_derated;
+	u32 pwr_mgmt_ctrl_shdw;
+	union {
+		u32 read_idle_ctrl_shdw_normal;
+		u32 dll_calib_ctrl_shdw_normal;
+	};
+	union {
+		u32 read_idle_ctrl_shdw_volt_ramp;
+		u32 dll_calib_ctrl_shdw_volt_ramp;
+	};
+
+	u32 phy_ctrl_1_shdw;
+	u32 ext_phy_ctrl_2_shdw;
+	u32 ext_phy_ctrl_3_shdw;
+	u32 ext_phy_ctrl_4_shdw;
+};
+#endif /* __ASSEMBLY__ */
+#endif /* __EMIF_H */
diff --git a/drivers/memory/tegra20-mc.c b/drivers/memory/tegra20-mc.c
new file mode 100644
index 00000000000..3ed49c1c2b9
--- /dev/null
+++ b/drivers/memory/tegra20-mc.c
@@ -0,0 +1,257 @@
+/*
+ * Tegra20 Memory Controller
+ *
+ * Copyright (c) 2012, 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, write to the Free Software Foundation, Inc.,
+ * 51 Franklin St - Fifth Floor, Boston, MA 02110-1301 USA.
+ */
+
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/ratelimit.h>
+#include <linux/platform_device.h>
+#include <linux/interrupt.h>
+#include <linux/io.h>
+
+#define DRV_NAME "tegra20-mc"
+
+#define MC_INTSTATUS			0x0
+#define MC_INTMASK			0x4
+
+#define MC_INT_ERR_SHIFT		6
+#define MC_INT_ERR_MASK			(0x1f << MC_INT_ERR_SHIFT)
+#define MC_INT_DECERR_EMEM		BIT(MC_INT_ERR_SHIFT)
+#define MC_INT_INVALID_GART_PAGE	BIT(MC_INT_ERR_SHIFT + 1)
+#define MC_INT_SECURITY_VIOLATION	BIT(MC_INT_ERR_SHIFT + 2)
+#define MC_INT_ARBITRATION_EMEM		BIT(MC_INT_ERR_SHIFT + 3)
+
+#define MC_GART_ERROR_REQ		0x30
+#define MC_DECERR_EMEM_OTHERS_STATUS	0x58
+#define MC_SECURITY_VIOLATION_STATUS	0x74
+
+#define SECURITY_VIOLATION_TYPE		BIT(30)	/* 0=TRUSTZONE, 1=CARVEOUT */
+
+#define MC_CLIENT_ID_MASK		0x3f
+
+#define NUM_MC_REG_BANKS		2
+
+struct tegra20_mc {
+	void __iomem *regs[NUM_MC_REG_BANKS];
+	struct device *dev;
+};
+
+static inline u32 mc_readl(struct tegra20_mc *mc, u32 offs)
+{
+	u32 val = 0;
+
+	if (offs < 0x24)
+		val = readl(mc->regs[0] + offs);
+	if (offs < 0x400)
+		val = readl(mc->regs[1] + offs - 0x3c);
+
+	return val;
+}
+
+static inline void mc_writel(struct tegra20_mc *mc, u32 val, u32 offs)
+{
+	if (offs < 0x24) {
+		writel(val, mc->regs[0] + offs);
+		return;
+	}
+	if (offs < 0x400) {
+		writel(val, mc->regs[1] + offs - 0x3c);
+		return;
+	}
+}
+
+static const char * const tegra20_mc_client[] = {
+	"cbr_display0a",
+	"cbr_display0ab",
+	"cbr_display0b",
+	"cbr_display0bb",
+	"cbr_display0c",
+	"cbr_display0cb",
+	"cbr_display1b",
+	"cbr_display1bb",
+	"cbr_eppup",
+	"cbr_g2pr",
+	"cbr_g2sr",
+	"cbr_mpeunifbr",
+	"cbr_viruv",
+	"csr_avpcarm7r",
+	"csr_displayhc",
+	"csr_displayhcb",
+	"csr_fdcdrd",
+	"csr_g2dr",
+	"csr_host1xdmar",
+	"csr_host1xr",
+	"csr_idxsrd",
+	"csr_mpcorer",
+	"csr_mpe_ipred",
+	"csr_mpeamemrd",
+	"csr_mpecsrd",
+	"csr_ppcsahbdmar",
+	"csr_ppcsahbslvr",
+	"csr_texsrd",
+	"csr_vdebsevr",
+	"csr_vdember",
+	"csr_vdemcer",
+	"csr_vdetper",
+	"cbw_eppu",
+	"cbw_eppv",
+	"cbw_eppy",
+	"cbw_mpeunifbw",
+	"cbw_viwsb",
+	"cbw_viwu",
+	"cbw_viwv",
+	"cbw_viwy",
+	"ccw_g2dw",
+	"csw_avpcarm7w",
+	"csw_fdcdwr",
+	"csw_host1xw",
+	"csw_ispw",
+	"csw_mpcorew",
+	"csw_mpecswr",
+	"csw_ppcsahbdmaw",
+	"csw_ppcsahbslvw",
+	"csw_vdebsevw",
+	"csw_vdembew",
+	"csw_vdetpmw",
+};
+
+static void tegra20_mc_decode(struct tegra20_mc *mc, int n)
+{
+	u32 addr, req;
+	const char *client = "Unknown";
+	int idx, cid;
+	const struct reg_info {
+		u32 offset;
+		u32 write_bit;	/* 0=READ, 1=WRITE */
+		int cid_shift;
+		char *message;
+	} reg[] = {
+		{
+			.offset = MC_DECERR_EMEM_OTHERS_STATUS,
+			.write_bit = 31,
+			.message = "MC_DECERR",
+		},
+		{
+			.offset	= MC_GART_ERROR_REQ,
+			.cid_shift = 1,
+			.message = "MC_GART_ERR",
+
+		},
+		{
+			.offset = MC_SECURITY_VIOLATION_STATUS,
+			.write_bit = 31,
+			.message = "MC_SECURITY_ERR",
+		},
+	};
+
+	idx = n - MC_INT_ERR_SHIFT;
+	if ((idx < 0) || (idx >= ARRAY_SIZE(reg))) {
+		dev_err_ratelimited(mc->dev, "Unknown interrupt status %08lx\n",
+				    BIT(n));
+		return;
+	}
+
+	req = mc_readl(mc, reg[idx].offset);
+	cid = (req >> reg[idx].cid_shift) & MC_CLIENT_ID_MASK;
+	if (cid < ARRAY_SIZE(tegra20_mc_client))
+		client = tegra20_mc_client[cid];
+
+	addr = mc_readl(mc, reg[idx].offset + sizeof(u32));
+
+	dev_err_ratelimited(mc->dev, "%s (0x%08x): 0x%08x %s (%s %s)\n",
+			   reg[idx].message, req, addr, client,
+			   (req & BIT(reg[idx].write_bit)) ? "write" : "read",
+			   (reg[idx].offset == MC_SECURITY_VIOLATION_STATUS) ?
+			   ((req & SECURITY_VIOLATION_TYPE) ?
+			    "carveout" : "trustzone") : "");
+}
+
+static const struct of_device_id tegra20_mc_of_match[] __devinitconst = {
+	{ .compatible = "nvidia,tegra20-mc", },
+	{},
+};
+
+static irqreturn_t tegra20_mc_isr(int irq, void *data)
+{
+	u32 stat, mask, bit;
+	struct tegra20_mc *mc = data;
+
+	stat = mc_readl(mc, MC_INTSTATUS);
+	mask = mc_readl(mc, MC_INTMASK);
+	mask &= stat;
+	if (!mask)
+		return IRQ_NONE;
+	while ((bit = ffs(mask)) != 0)
+		tegra20_mc_decode(mc, bit - 1);
+	mc_writel(mc, stat, MC_INTSTATUS);
+	return IRQ_HANDLED;
+}
+
+static int __devinit tegra20_mc_probe(struct platform_device *pdev)
+{
+	struct resource *irq;
+	struct tegra20_mc *mc;
+	int i, err;
+	u32 intmask;
+
+	mc = devm_kzalloc(&pdev->dev, sizeof(*mc), GFP_KERNEL);
+	if (!mc)
+		return -ENOMEM;
+	mc->dev = &pdev->dev;
+
+	for (i = 0; i < ARRAY_SIZE(mc->regs); i++) {
+		struct resource *res;
+
+		res = platform_get_resource(pdev, IORESOURCE_MEM, i);
+		if (!res)
+			return -ENODEV;
+		mc->regs[i] = devm_request_and_ioremap(&pdev->dev, res);
+		if (!mc->regs[i])
+			return -EBUSY;
+	}
+
+	irq = platform_get_resource(pdev, IORESOURCE_IRQ, 0);
+	if (!irq)
+		return -ENODEV;
+	err = devm_request_irq(&pdev->dev, irq->start, tegra20_mc_isr,
+			       IRQF_SHARED, dev_name(&pdev->dev), mc);
+	if (err)
+		return -ENODEV;
+
+	platform_set_drvdata(pdev, mc);
+
+	intmask = MC_INT_INVALID_GART_PAGE |
+		MC_INT_DECERR_EMEM | MC_INT_SECURITY_VIOLATION;
+	mc_writel(mc, intmask, MC_INTMASK);
+	return 0;
+}
+
+static struct platform_driver tegra20_mc_driver = {
+	.probe = tegra20_mc_probe,
+	.driver = {
+		.name = DRV_NAME,
+		.owner = THIS_MODULE,
+		.of_match_table = tegra20_mc_of_match,
+	},
+};
+module_platform_driver(tegra20_mc_driver);
+
+MODULE_AUTHOR("Hiroshi DOYU <hdoyu@nvidia.com>");
+MODULE_DESCRIPTION("Tegra20 MC driver");
+MODULE_LICENSE("GPL v2");
+MODULE_ALIAS("platform:" DRV_NAME);
diff --git a/drivers/memory/tegra30-mc.c b/drivers/memory/tegra30-mc.c
new file mode 100644
index 00000000000..e56ff04eb5c
--- /dev/null
+++ b/drivers/memory/tegra30-mc.c
@@ -0,0 +1,382 @@
+/*
+ * Tegra30 Memory Controller
+ *
+ * Copyright (c) 2012, 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, write to the Free Software Foundation, Inc.,
+ * 51 Franklin St - Fifth Floor, Boston, MA 02110-1301 USA.
+ */
+
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/ratelimit.h>
+#include <linux/platform_device.h>
+#include <linux/interrupt.h>
+#include <linux/io.h>
+
+#define DRV_NAME "tegra30-mc"
+
+#define MC_INTSTATUS			0x0
+#define MC_INTMASK			0x4
+
+#define MC_INT_ERR_SHIFT		6
+#define MC_INT_ERR_MASK			(0x1f << MC_INT_ERR_SHIFT)
+#define MC_INT_DECERR_EMEM		BIT(MC_INT_ERR_SHIFT)
+#define MC_INT_SECURITY_VIOLATION	BIT(MC_INT_ERR_SHIFT + 2)
+#define MC_INT_ARBITRATION_EMEM		BIT(MC_INT_ERR_SHIFT + 3)
+#define MC_INT_INVALID_SMMU_PAGE	BIT(MC_INT_ERR_SHIFT + 4)
+
+#define MC_ERR_STATUS			0x8
+#define MC_ERR_ADR			0xc
+
+#define MC_ERR_TYPE_SHIFT		28
+#define MC_ERR_TYPE_MASK		(7 << MC_ERR_TYPE_SHIFT)
+#define MC_ERR_TYPE_DECERR_EMEM		2
+#define MC_ERR_TYPE_SECURITY_TRUSTZONE	3
+#define MC_ERR_TYPE_SECURITY_CARVEOUT	4
+#define MC_ERR_TYPE_INVALID_SMMU_PAGE	6
+
+#define MC_ERR_INVALID_SMMU_PAGE_SHIFT	25
+#define MC_ERR_INVALID_SMMU_PAGE_MASK	(7 << MC_ERR_INVALID_SMMU_PAGE_SHIFT)
+#define MC_ERR_RW_SHIFT			16
+#define MC_ERR_RW			BIT(MC_ERR_RW_SHIFT)
+#define MC_ERR_SECURITY			BIT(MC_ERR_RW_SHIFT + 1)
+
+#define SECURITY_VIOLATION_TYPE		BIT(30)	/* 0=TRUSTZONE, 1=CARVEOUT */
+
+#define MC_EMEM_ARB_CFG			0x90
+#define MC_EMEM_ARB_OUTSTANDING_REQ	0x94
+#define MC_EMEM_ARB_TIMING_RCD		0x98
+#define MC_EMEM_ARB_TIMING_RP		0x9c
+#define MC_EMEM_ARB_TIMING_RC		0xa0
+#define MC_EMEM_ARB_TIMING_RAS		0xa4
+#define MC_EMEM_ARB_TIMING_FAW		0xa8
+#define MC_EMEM_ARB_TIMING_RRD		0xac
+#define MC_EMEM_ARB_TIMING_RAP2PRE	0xb0
+#define MC_EMEM_ARB_TIMING_WAP2PRE	0xb4
+#define MC_EMEM_ARB_TIMING_R2R		0xb8
+#define MC_EMEM_ARB_TIMING_W2W		0xbc
+#define MC_EMEM_ARB_TIMING_R2W		0xc0
+#define MC_EMEM_ARB_TIMING_W2R		0xc4
+
+#define MC_EMEM_ARB_DA_TURNS		0xd0
+#define MC_EMEM_ARB_DA_COVERS		0xd4
+#define MC_EMEM_ARB_MISC0		0xd8
+#define MC_EMEM_ARB_MISC1		0xdc
+
+#define MC_EMEM_ARB_RING3_THROTTLE	0xe4
+#define MC_EMEM_ARB_OVERRIDE		0xe8
+
+#define MC_TIMING_CONTROL		0xfc
+
+#define MC_CLIENT_ID_MASK		0x7f
+
+#define NUM_MC_REG_BANKS		4
+
+struct tegra30_mc {
+	void __iomem *regs[NUM_MC_REG_BANKS];
+	struct device *dev;
+	u32 ctx[0];
+};
+
+static inline u32 mc_readl(struct tegra30_mc *mc, u32 offs)
+{
+	u32 val = 0;
+
+	if (offs < 0x10)
+		val = readl(mc->regs[0] + offs);
+	if (offs < 0x1f0)
+		val = readl(mc->regs[1] + offs - 0x3c);
+	if (offs < 0x228)
+		val = readl(mc->regs[2] + offs - 0x200);
+	if (offs < 0x400)
+		val = readl(mc->regs[3] + offs - 0x284);
+
+	return val;
+}
+
+static inline void mc_writel(struct tegra30_mc *mc, u32 val, u32 offs)
+{
+	if (offs < 0x10) {
+		writel(val, mc->regs[0] + offs);
+		return;
+	}
+	if (offs < 0x1f0) {
+		writel(val, mc->regs[1] + offs - 0x3c);
+		return;
+	}
+	if (offs < 0x228) {
+		writel(val, mc->regs[2] + offs - 0x200);
+		return;
+	}
+	if (offs < 0x400) {
+		writel(val, mc->regs[3] + offs - 0x284);
+		return;
+	}
+}
+
+static const char * const tegra30_mc_client[] = {
+	"csr_ptcr",
+	"cbr_display0a",
+	"cbr_display0ab",
+	"cbr_display0b",
+	"cbr_display0bb",
+	"cbr_display0c",
+	"cbr_display0cb",
+	"cbr_display1b",
+	"cbr_display1bb",
+	"cbr_eppup",
+	"cbr_g2pr",
+	"cbr_g2sr",
+	"cbr_mpeunifbr",
+	"cbr_viruv",
+	"csr_afir",
+	"csr_avpcarm7r",
+	"csr_displayhc",
+	"csr_displayhcb",
+	"csr_fdcdrd",
+	"csr_fdcdrd2",
+	"csr_g2dr",
+	"csr_hdar",
+	"csr_host1xdmar",
+	"csr_host1xr",
+	"csr_idxsrd",
+	"csr_idxsrd2",
+	"csr_mpe_ipred",
+	"csr_mpeamemrd",
+	"csr_mpecsrd",
+	"csr_ppcsahbdmar",
+	"csr_ppcsahbslvr",
+	"csr_satar",
+	"csr_texsrd",
+	"csr_texsrd2",
+	"csr_vdebsevr",
+	"csr_vdember",
+	"csr_vdemcer",
+	"csr_vdetper",
+	"csr_mpcorelpr",
+	"csr_mpcorer",
+	"cbw_eppu",
+	"cbw_eppv",
+	"cbw_eppy",
+	"cbw_mpeunifbw",
+	"cbw_viwsb",
+	"cbw_viwu",
+	"cbw_viwv",
+	"cbw_viwy",
+	"ccw_g2dw",
+	"csw_afiw",
+	"csw_avpcarm7w",
+	"csw_fdcdwr",
+	"csw_fdcdwr2",
+	"csw_hdaw",
+	"csw_host1xw",
+	"csw_ispw",
+	"csw_mpcorelpw",
+	"csw_mpcorew",
+	"csw_mpecswr",
+	"csw_ppcsahbdmaw",
+	"csw_ppcsahbslvw",
+	"csw_sataw",
+	"csw_vdebsevw",
+	"csw_vdedbgw",
+	"csw_vdembew",
+	"csw_vdetpmw",
+};
+
+static void tegra30_mc_decode(struct tegra30_mc *mc, int n)
+{
+	u32 err, addr;
+	const char * const mc_int_err[] = {
+		"MC_DECERR",
+		"Unknown",
+		"MC_SECURITY_ERR",
+		"MC_ARBITRATION_EMEM",
+		"MC_SMMU_ERR",
+	};
+	const char * const err_type[] = {
+		"Unknown",
+		"Unknown",
+		"DECERR_EMEM",
+		"SECURITY_TRUSTZONE",
+		"SECURITY_CARVEOUT",
+		"Unknown",
+		"INVALID_SMMU_PAGE",
+		"Unknown",
+	};
+	char attr[6];
+	int cid, perm, type, idx;
+	const char *client = "Unknown";
+
+	idx = n - MC_INT_ERR_SHIFT;
+	if ((idx < 0) || (idx >= ARRAY_SIZE(mc_int_err)) || (idx == 1)) {
+		dev_err_ratelimited(mc->dev, "Unknown interrupt status %08lx\n",
+				    BIT(n));
+		return;
+	}
+
+	err = readl(mc + MC_ERR_STATUS);
+
+	type = (err & MC_ERR_TYPE_MASK) >> MC_ERR_TYPE_SHIFT;
+	perm = (err & MC_ERR_INVALID_SMMU_PAGE_MASK) >>
+		MC_ERR_INVALID_SMMU_PAGE_SHIFT;
+	if (type == MC_ERR_TYPE_INVALID_SMMU_PAGE)
+		sprintf(attr, "%c-%c-%c",
+			(perm & BIT(2)) ? 'R' : '-',
+			(perm & BIT(1)) ? 'W' : '-',
+			(perm & BIT(0)) ? 'S' : '-');
+	else
+		attr[0] = '\0';
+
+	cid = err & MC_CLIENT_ID_MASK;
+	if (cid < ARRAY_SIZE(tegra30_mc_client))
+		client = tegra30_mc_client[cid];
+
+	addr = readl(mc + MC_ERR_ADR);
+
+	dev_err_ratelimited(mc->dev, "%s (0x%08x): 0x%08x %s (%s %s %s %s)\n",
+			   mc_int_err[idx], err, addr, client,
+			   (err & MC_ERR_SECURITY) ? "secure" : "non-secure",
+			   (err & MC_ERR_RW) ? "write" : "read",
+			   err_type[type], attr);
+}
+
+static const u32 tegra30_mc_ctx[] = {
+	MC_EMEM_ARB_CFG,
+	MC_EMEM_ARB_OUTSTANDING_REQ,
+	MC_EMEM_ARB_TIMING_RCD,
+	MC_EMEM_ARB_TIMING_RP,
+	MC_EMEM_ARB_TIMING_RC,
+	MC_EMEM_ARB_TIMING_RAS,
+	MC_EMEM_ARB_TIMING_FAW,
+	MC_EMEM_ARB_TIMING_RRD,
+	MC_EMEM_ARB_TIMING_RAP2PRE,
+	MC_EMEM_ARB_TIMING_WAP2PRE,
+	MC_EMEM_ARB_TIMING_R2R,
+	MC_EMEM_ARB_TIMING_W2W,
+	MC_EMEM_ARB_TIMING_R2W,
+	MC_EMEM_ARB_TIMING_W2R,
+	MC_EMEM_ARB_DA_TURNS,
+	MC_EMEM_ARB_DA_COVERS,
+	MC_EMEM_ARB_MISC0,
+	MC_EMEM_ARB_MISC1,
+	MC_EMEM_ARB_RING3_THROTTLE,
+	MC_EMEM_ARB_OVERRIDE,
+	MC_INTMASK,
+};
+
+static int tegra30_mc_suspend(struct device *dev)
+{
+	int i;
+	struct tegra30_mc *mc = dev_get_drvdata(dev);
+
+	for (i = 0; i < ARRAY_SIZE(tegra30_mc_ctx); i++)
+		mc->ctx[i] = mc_readl(mc, tegra30_mc_ctx[i]);
+	return 0;
+}
+
+static int tegra30_mc_resume(struct device *dev)
+{
+	int i;
+	struct tegra30_mc *mc = dev_get_drvdata(dev);
+
+	for (i = 0; i < ARRAY_SIZE(tegra30_mc_ctx); i++)
+		mc_writel(mc, mc->ctx[i], tegra30_mc_ctx[i]);
+
+	mc_writel(mc, 1, MC_TIMING_CONTROL);
+	/* Read-back to ensure that write reached */
+	mc_readl(mc, MC_TIMING_CONTROL);
+	return 0;
+}
+
+static UNIVERSAL_DEV_PM_OPS(tegra30_mc_pm,
+			    tegra30_mc_suspend,
+			    tegra30_mc_resume, NULL);
+
+static const struct of_device_id tegra30_mc_of_match[] __devinitconst = {
+	{ .compatible = "nvidia,tegra30-mc", },
+	{},
+};
+
+static irqreturn_t tegra30_mc_isr(int irq, void *data)
+{
+	u32 stat, mask, bit;
+	struct tegra30_mc *mc = data;
+
+	stat = mc_readl(mc, MC_INTSTATUS);
+	mask = mc_readl(mc, MC_INTMASK);
+	mask &= stat;
+	if (!mask)
+		return IRQ_NONE;
+	while ((bit = ffs(mask)) != 0)
+		tegra30_mc_decode(mc, bit - 1);
+	mc_writel(mc, stat, MC_INTSTATUS);
+	return IRQ_HANDLED;
+}
+
+static int __devinit tegra30_mc_probe(struct platform_device *pdev)
+{
+	struct resource *irq;
+	struct tegra30_mc *mc;
+	size_t bytes;
+	int err, i;
+	u32 intmask;
+
+	bytes = sizeof(*mc) + sizeof(u32) * ARRAY_SIZE(tegra30_mc_ctx);
+	mc = devm_kzalloc(&pdev->dev, bytes, GFP_KERNEL);
+	if (!mc)
+		return -ENOMEM;
+	mc->dev = &pdev->dev;
+
+	for (i = 0; i < ARRAY_SIZE(mc->regs); i++) {
+		struct resource *res;
+
+		res = platform_get_resource(pdev, IORESOURCE_MEM, i);
+		if (!res)
+			return -ENODEV;
+		mc->regs[i] = devm_request_and_ioremap(&pdev->dev, res);
+		if (!mc->regs[i])
+			return -EBUSY;
+	}
+
+	irq = platform_get_resource(pdev, IORESOURCE_IRQ, 0);
+	if (!irq)
+		return -ENODEV;
+	err = devm_request_irq(&pdev->dev, irq->start, tegra30_mc_isr,
+			       IRQF_SHARED, dev_name(&pdev->dev), mc);
+	if (err)
+		return -ENODEV;
+
+	platform_set_drvdata(pdev, mc);
+
+	intmask = MC_INT_INVALID_SMMU_PAGE |
+		MC_INT_DECERR_EMEM | MC_INT_SECURITY_VIOLATION;
+	mc_writel(mc, intmask, MC_INTMASK);
+	return 0;
+}
+
+static struct platform_driver tegra30_mc_driver = {
+	.probe = tegra30_mc_probe,
+	.driver = {
+		.name = DRV_NAME,
+		.owner = THIS_MODULE,
+		.of_match_table = tegra30_mc_of_match,
+		.pm = &tegra30_mc_pm,
+	},
+};
+module_platform_driver(tegra30_mc_driver);
+
+MODULE_AUTHOR("Hiroshi DOYU <hdoyu@nvidia.com>");
+MODULE_DESCRIPTION("Tegra30 MC driver");
+MODULE_LICENSE("GPL v2");
+MODULE_ALIAS("platform:" DRV_NAME);
diff --git a/drivers/message/fusion/mptlan.h b/drivers/message/fusion/mptlan.h
index c171afa9323..69e9d546356 100644
--- a/drivers/message/fusion/mptlan.h
+++ b/drivers/message/fusion/mptlan.h
@@ -69,7 +69,6 @@
 #include <linux/spinlock.h>
 #include <linux/workqueue.h>
 #include <linux/delay.h>
-// #include <linux/trdevice.h>
 
 #include <asm/uaccess.h>
 #include <asm/io.h>
diff --git a/drivers/message/i2o/i2o_proc.c b/drivers/message/i2o/i2o_proc.c
index 6d115c7208a..506c36f6e1d 100644
--- a/drivers/message/i2o/i2o_proc.c
+++ b/drivers/message/i2o/i2o_proc.c
@@ -283,7 +283,6 @@ static char *bus_strings[] = {
 	"Local Bus",
 	"ISA",
 	"EISA",
-	"MCA",
 	"PCI",
 	"PCMCIA",
 	"NUBUS",
@@ -351,18 +350,6 @@ static int i2o_seq_show_hrt(struct seq_file *seq, void *v)
 					   EisaSlotNumber);
 				break;
 
-			case I2O_BUS_MCA:
-				seq_printf(seq, "     IOBase: %0#6x,",
-					   hrt->hrt_entry[i].bus.mca_bus.
-					   McaBaseIOPort);
-				seq_printf(seq, " MemoryBase: %0#10x,",
-					   hrt->hrt_entry[i].bus.mca_bus.
-					   McaBaseMemoryAddress);
-				seq_printf(seq, " Slot: %0#4x,",
-					   hrt->hrt_entry[i].bus.mca_bus.
-					   McaSlotNumber);
-				break;
-
 			case I2O_BUS_PCI:
 				seq_printf(seq, "     Bus: %0#4x",
 					   hrt->hrt_entry[i].bus.pci_bus.
diff --git a/drivers/mfd/Kconfig b/drivers/mfd/Kconfig
index 11e44386fa9..e129c820df7 100644
--- a/drivers/mfd/Kconfig
+++ b/drivers/mfd/Kconfig
@@ -106,6 +106,19 @@ config UCB1400_CORE
 	  To compile this driver as a module, choose M here: the
 	  module will be called ucb1400_core.
 
+config MFD_LM3533
+	tristate "LM3533 Lighting Power chip"
+	depends on I2C
+	select MFD_CORE
+	select REGMAP_I2C
+	help
+	  Say yes here to enable support for National Semiconductor / TI
+	  LM3533 Lighting Power chips.
+
+	  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 TPS6105X
 	tristate "TPS61050/61052 Boost Converters"
 	depends on I2C
@@ -162,6 +175,7 @@ config MFD_TPS6586X
 	bool "TPS6586x Power Management chips"
 	depends on I2C=y && GPIOLIB && GENERIC_HARDIRQS
 	select MFD_CORE
+	depends on REGULATOR
 	help
 	  If you say yes here you get support for the TPS6586X series of
 	  Power Management chips.
@@ -176,8 +190,8 @@ config MFD_TPS65910
 	bool "TPS65910 Power Management chip"
 	depends on I2C=y && GPIOLIB
 	select MFD_CORE
-	select GPIO_TPS65910
 	select REGMAP_I2C
+	select IRQ_DOMAIN
 	help
 	  if you say yes here you get support for the TPS65910 series of
 	  Power Management chips.
@@ -408,13 +422,26 @@ config PMIC_ADP5520
 	  individual components like LCD backlight, LEDs, GPIOs and Kepad
 	  under the corresponding menus.
 
+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_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 provies common support for
+	  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.
 
@@ -437,7 +464,7 @@ config MFD_MAX8998
 	help
 	  Say yes here to support for Maxim Semiconductor MAX8998 and
 	  National Semiconductor LP3974. This is a Power Management IC.
-	  This driver provies common support for accessing the device,
+	  This driver provides common support for accessing the device,
 	  additional drivers must be enabled in order to use the functionality
 	  of the device.
 
@@ -448,14 +475,14 @@ config MFD_S5M_CORE
 	select REGMAP_I2C
 	help
 	 Support for the Samsung Electronics S5M MFD series.
-	 This driver provies common support for accessing the device,
+	 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_WM8400
-	tristate "Support Wolfson Microelectronics WM8400"
+	bool "Support Wolfson Microelectronics WM8400"
 	select MFD_CORE
-	depends on I2C
+	depends on I2C=y
 	select REGMAP_I2C
 	help
 	  Support for the Wolfson Microelecronics WM8400 PMIC and audio
@@ -472,6 +499,7 @@ config MFD_WM831X_I2C
 	select MFD_CORE
 	select MFD_WM831X
 	select REGMAP_I2C
+	select IRQ_DOMAIN
 	depends on I2C=y && GENERIC_HARDIRQS
 	help
 	  Support for the Wolfson Microelecronics WM831x and WM832x PMICs
@@ -484,6 +512,7 @@ config MFD_WM831X_SPI
 	select MFD_CORE
 	select MFD_WM831X
 	select REGMAP_SPI
+	select IRQ_DOMAIN
 	depends on SPI_MASTER && GENERIC_HARDIRQS
 	help
 	  Support for the Wolfson Microelecronics WM831x and WM832x PMICs
@@ -596,17 +625,32 @@ config MFD_MC13783
 	tristate
 
 config MFD_MC13XXX
-	tristate "Support Freescale MC13783 and MC13892"
-	depends on SPI_MASTER
+	tristate
+	depends on SPI_MASTER || I2C
 	select MFD_CORE
 	select MFD_MC13783
 	help
-	  Support for the Freescale (Atlas) PMIC and audio CODECs
-	  MC13783 and MC13892.
-	  This driver provides common support for accessing  the device,
+	  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
+	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
+	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
@@ -648,26 +692,9 @@ config EZX_PCAP
 	  This enables the PCAP ASIC present on EZX Phones. This is
 	  needed for MMC, TouchScreen, Sound, USB, etc..
 
-config AB5500_CORE
-	bool "ST-Ericsson AB5500 Mixed Signal Power Management chip"
-	depends on ABX500_CORE && MFD_DB5500_PRCMU
-	select MFD_CORE
-	help
-	  Select this option to enable access to AB5500 power management
-	  chip. This connects to the db5500 chip via the I2C bus via PRCMU.
-	  This chip embeds various other multimedia funtionalities as well.
-
-config AB5500_DEBUG
-	bool "Enable debug info via debugfs"
-	depends on AB5500_CORE && DEBUG_FS
-	default y if DEBUG_FS
-	help
-	  Select this option if you want debug information from the AB5500
-	  using the debug filesystem, debugfs.
-
 config AB8500_CORE
 	bool "ST-Ericsson AB8500 Mixed Signal Power Management chip"
-	depends on GENERIC_HARDIRQS && ABX500_CORE
+	depends on GENERIC_HARDIRQS && ABX500_CORE && MFD_DB8500_PRCMU
 	select MFD_CORE
 	help
 	  Select this option to enable access to AB8500 power management
@@ -711,16 +738,6 @@ config MFD_DB8500_PRCMU
 	  system controller running an XP70 microprocessor, which is accessed
 	  through a register map.
 
-config MFD_DB5500_PRCMU
-	bool "ST-Ericsson DB5500 Power Reset Control Management Unit"
-	depends on UX500_SOC_DB5500
-	select MFD_CORE
-	help
-	  Select this option to enable support for the DB5500 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
@@ -748,6 +765,16 @@ config LPC_SCH
 	  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
+	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
@@ -880,6 +907,11 @@ config MFD_RC5T583
 	  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
+	select MFD_CORE
+
 config MFD_ANATOP
 	bool "Support for Freescale i.MX on-chip ANATOP controller"
 	depends on SOC_IMX6Q
@@ -888,6 +920,16 @@ config MFD_ANATOP
 	  MFD controller. This controller embeds regulator and
 	  thermal devices for Freescale i.MX platforms.
 
+config MFD_PALMAS
+	bool "Support for the TI Palmas series chips"
+	select MFD_CORE
+	select REGMAP_I2C
+	select REGMAP_IRQ
+	depends on I2C=y
+	help
+	  If you say yes here you get support for the Palmas
+	  series of PMIC chips from Texas Instruments.
+
 endmenu
 endif
 
diff --git a/drivers/mfd/Makefile b/drivers/mfd/Makefile
index 05fa538c5ef..75f6ed68a4b 100644
--- a/drivers/mfd/Makefile
+++ b/drivers/mfd/Makefile
@@ -15,6 +15,7 @@ obj-$(CONFIG_MFD_DAVINCI_VOICECODEC)	+= davinci_voicecodec.o
 obj-$(CONFIG_MFD_DM355EVM_MSP)	+= dm355evm_msp.o
 obj-$(CONFIG_MFD_TI_SSP)	+= ti-ssp.o
 
+obj-$(CONFIG_MFD_STA2X11)	+= sta2x11-mfd.o
 obj-$(CONFIG_MFD_STMPE)		+= stmpe.o
 obj-$(CONFIG_STMPE_I2C)		+= stmpe-i2c.o
 obj-$(CONFIG_STMPE_SPI)		+= stmpe-spi.o
@@ -54,6 +55,8 @@ obj-$(CONFIG_TWL6030_PWM)	+= twl6030-pwm.o
 obj-$(CONFIG_TWL6040_CORE)	+= twl6040-core.o twl6040-irq.o
 
 obj-$(CONFIG_MFD_MC13XXX)	+= mc13xxx-core.o
+obj-$(CONFIG_MFD_MC13XXX_SPI)	+= mc13xxx-spi.o
+obj-$(CONFIG_MFD_MC13XXX_I2C)	+= mc13xxx-i2c.o
 
 obj-$(CONFIG_MFD_CORE)		+= mfd-core.o
 
@@ -75,6 +78,7 @@ obj-$(CONFIG_PMIC_DA9052)	+= da9052-core.o
 obj-$(CONFIG_MFD_DA9052_SPI)	+= da9052-spi.o
 obj-$(CONFIG_MFD_DA9052_I2C)	+= da9052-i2c.o
 
+obj-$(CONFIG_MFD_MAX77693)	+= max77693.o max77693-irq.o
 max8925-objs			:= max8925-core.o max8925-i2c.o
 obj-$(CONFIG_MFD_MAX8925)	+= max8925.o
 obj-$(CONFIG_MFD_MAX8997)	+= max8997.o max8997-irq.o
@@ -87,18 +91,15 @@ obj-$(CONFIG_PCF50633_GPIO)	+= pcf50633-gpio.o
 obj-$(CONFIG_ABX500_CORE)	+= abx500-core.o
 obj-$(CONFIG_AB3100_CORE)	+= ab3100-core.o
 obj-$(CONFIG_AB3100_OTP)	+= ab3100-otp.o
-obj-$(CONFIG_AB5500_CORE)	+= ab5500-core.o
-obj-$(CONFIG_AB5500_DEBUG)	+= ab5500-debugfs.o
-obj-$(CONFIG_AB8500_CORE)	+= ab8500-core.o ab8500-sysctrl.o
 obj-$(CONFIG_AB8500_DEBUG)	+= ab8500-debugfs.o
 obj-$(CONFIG_AB8500_GPADC)	+= ab8500-gpadc.o
 obj-$(CONFIG_MFD_DB8500_PRCMU)	+= db8500-prcmu.o
-# ab8500-i2c need to come after db8500-prcmu (which provides the channel)
-obj-$(CONFIG_AB8500_I2C_CORE)	+= ab8500-i2c.o
-obj-$(CONFIG_MFD_DB5500_PRCMU)	+= db5500-prcmu.o
+# ab8500-core need to come after db8500-prcmu (which provides the channel)
+obj-$(CONFIG_AB8500_CORE)	+= ab8500-core.o ab8500-sysctrl.o
 obj-$(CONFIG_MFD_TIMBERDALE)    += timberdale.o
 obj-$(CONFIG_PMIC_ADP5520)	+= adp5520.o
 obj-$(CONFIG_LPC_SCH)		+= lpc_sch.o
+obj-$(CONFIG_LPC_ICH)		+= lpc_ich.o
 obj-$(CONFIG_MFD_RDC321X)	+= rdc321x-southbridge.o
 obj-$(CONFIG_MFD_JANZ_CMODIO)	+= janz-cmodio.o
 obj-$(CONFIG_MFD_JZ4740_ADC)	+= jz4740-adc.o
@@ -113,6 +114,8 @@ obj-$(CONFIG_TPS65911_COMPARATOR)	+= tps65911-comparator.o
 obj-$(CONFIG_MFD_TPS65090)	+= tps65090.o
 obj-$(CONFIG_MFD_AAT2870_CORE)	+= aat2870-core.o
 obj-$(CONFIG_MFD_INTEL_MSIC)	+= intel_msic.o
+obj-$(CONFIG_MFD_PALMAS)	+= palmas.o
 obj-$(CONFIG_MFD_RC5T583)	+= rc5t583.o rc5t583-irq.o
 obj-$(CONFIG_MFD_S5M_CORE)	+= s5m-core.o s5m-irq.o
 obj-$(CONFIG_MFD_ANATOP)	+= anatop-mfd.o
+obj-$(CONFIG_MFD_LM3533)	+= lm3533-core.o lm3533-ctrlbank.o
diff --git a/drivers/mfd/ab5500-core.c b/drivers/mfd/ab5500-core.c
deleted file mode 100644
index 54d0fe40845..00000000000
--- a/drivers/mfd/ab5500-core.c
+++ /dev/null
@@ -1,1439 +0,0 @@
-/*
- * Copyright (C) 2007-2011 ST-Ericsson
- * License terms: GNU General Public License (GPL) version 2
- * Low-level core for exclusive access to the AB5500 IC on the I2C bus
- * and some basic chip-configuration.
- * Author: Bengt Jonsson <bengt.g.jonsson@stericsson.com>
- * Author: Mattias Nilsson <mattias.i.nilsson@stericsson.com>
- * Author: Mattias Wallin <mattias.wallin@stericsson.com>
- * Author: Rickard Andersson <rickard.andersson@stericsson.com>
- * Author: Karl Komierowski  <karl.komierowski@stericsson.com>
- * Author: Bibek Basu <bibek.basu@stericsson.com>
- *
- * TODO: Event handling with irq_chip. Waiting for PRCMU fw support.
- */
-
-#include <linux/module.h>
-#include <linux/mutex.h>
-#include <linux/err.h>
-#include <linux/platform_device.h>
-#include <linux/slab.h>
-#include <linux/device.h>
-#include <linux/irq.h>
-#include <linux/interrupt.h>
-#include <linux/random.h>
-#include <linux/mfd/abx500.h>
-#include <linux/mfd/abx500/ab5500.h>
-#include <linux/list.h>
-#include <linux/bitops.h>
-#include <linux/spinlock.h>
-#include <linux/mfd/core.h>
-#include <linux/mfd/db5500-prcmu.h>
-
-#include "ab5500-core.h"
-#include "ab5500-debugfs.h"
-
-#define AB5500_NUM_EVENT_REG 23
-#define AB5500_IT_LATCH0_REG 0x40
-#define AB5500_IT_MASK0_REG 0x60
-
-/*
- * Permissible register ranges for reading and writing per device and bank.
- *
- * The ranges must be listed in increasing address order, and no overlaps are
- * allowed. It is assumed that write permission implies read permission
- * (i.e. only RO and RW permissions should be used).  Ranges with write
- * permission must not be split up.
- */
-
-#define NO_RANGE {.count = 0, .range = NULL,}
-static struct ab5500_i2c_banks ab5500_bank_ranges[AB5500_NUM_DEVICES] = {
-	[AB5500_DEVID_USB] =  {
-		.nbanks = 1,
-		.bank = (struct ab5500_i2c_ranges []) {
-			{
-				.bankid = AB5500_BANK_USB,
-				.nranges = 12,
-				.range = (struct ab5500_reg_range[]) {
-					{
-						.first = 0x01,
-						.last = 0x01,
-						.perm = AB5500_PERM_RW,
-					},
-					{
-						.first = 0x80,
-						.last = 0x83,
-						.perm = AB5500_PERM_RW,
-					},
-					{
-						.first = 0x87,
-						.last = 0x8A,
-						.perm = AB5500_PERM_RW,
-					},
-					{
-						.first = 0x8B,
-						.last = 0x8B,
-						.perm = AB5500_PERM_RO,
-					},
-					{
-						.first = 0x91,
-						.last = 0x92,
-						.perm = AB5500_PERM_RO,
-					},
-					{
-						.first = 0x93,
-						.last = 0x93,
-						.perm = AB5500_PERM_RW,
-					},
-					{
-						.first = 0x94,
-						.last = 0x94,
-						.perm = AB5500_PERM_RO,
-					},
-					{
-						.first = 0xA8,
-						.last = 0xB0,
-						.perm = AB5500_PERM_RO,
-					},
-					{
-						.first = 0xB2,
-						.last = 0xB2,
-						.perm = AB5500_PERM_RO,
-					},
-					{
-						.first = 0xB4,
-						.last = 0xBC,
-						.perm = AB5500_PERM_RO,
-					},
-					{
-						.first = 0xBF,
-						.last = 0xBF,
-						.perm = AB5500_PERM_RO,
-					},
-					{
-						.first = 0xC1,
-						.last = 0xC5,
-						.perm = AB5500_PERM_RO,
-					},
-				},
-			},
-		},
-	},
-	[AB5500_DEVID_ADC] =  {
-		.nbanks = 1,
-		.bank = (struct ab5500_i2c_ranges []) {
-			{
-				.bankid = AB5500_BANK_ADC,
-				.nranges = 6,
-				.range = (struct ab5500_reg_range[]) {
-					{
-						.first = 0x1F,
-						.last = 0x22,
-						.perm = AB5500_PERM_RO,
-					},
-					{
-						.first = 0x23,
-						.last = 0x24,
-						.perm = AB5500_PERM_RW,
-					},
-					{
-						.first = 0x26,
-						.last = 0x2D,
-						.perm = AB5500_PERM_RO,
-					},
-					{
-						.first = 0x2F,
-						.last = 0x34,
-						.perm = AB5500_PERM_RW,
-					},
-					{
-						.first = 0x37,
-						.last = 0x57,
-						.perm = AB5500_PERM_RW,
-					},
-					{
-						.first = 0x58,
-						.last = 0x58,
-						.perm = AB5500_PERM_RO,
-					},
-				},
-			},
-		},
-	},
-	[AB5500_DEVID_LEDS] =  {
-		.nbanks = 1,
-		.bank = (struct ab5500_i2c_ranges []) {
-			{
-				.bankid = AB5500_BANK_LED,
-				.nranges = 1,
-				.range = (struct ab5500_reg_range[]) {
-					{
-						.first = 0x00,
-						.last = 0x0C,
-						.perm = AB5500_PERM_RW,
-					},
-				},
-			},
-		},
-	},
-	[AB5500_DEVID_VIDEO] =   {
-		.nbanks = 1,
-		.bank = (struct ab5500_i2c_ranges []) {
-			{
-				.bankid = AB5500_BANK_VDENC,
-				.nranges = 12,
-				.range = (struct ab5500_reg_range[]) {
-					{
-						.first = 0x00,
-						.last = 0x08,
-						.perm = AB5500_PERM_RW,
-					},
-					{
-						.first = 0x09,
-						.last = 0x09,
-						.perm = AB5500_PERM_RO,
-					},
-					{
-						.first = 0x0A,
-						.last = 0x12,
-						.perm = AB5500_PERM_RW,
-					},
-					{
-						.first = 0x15,
-						.last = 0x19,
-						.perm = AB5500_PERM_RW,
-					},
-					{
-						.first = 0x1B,
-						.last = 0x21,
-						.perm = AB5500_PERM_RW,
-					},
-					{
-						.first = 0x27,
-						.last = 0x2C,
-						.perm = AB5500_PERM_RW,
-					},
-					{
-						.first = 0x41,
-						.last = 0x41,
-						.perm = AB5500_PERM_RW,
-					},
-					{
-						.first = 0x45,
-						.last = 0x5B,
-						.perm = AB5500_PERM_RW,
-					},
-					{
-						.first = 0x5D,
-						.last = 0x5D,
-						.perm = AB5500_PERM_RW,
-					},
-					{
-						.first = 0x69,
-						.last = 0x69,
-						.perm = AB5500_PERM_RW,
-					},
-					{
-						.first = 0x6C,
-						.last = 0x6D,
-						.perm = AB5500_PERM_RW,
-					},
-					{
-						.first = 0x80,
-						.last = 0x81,
-						.perm = AB5500_PERM_RW,
-					},
-				},
-			},
-		},
-	},
-	[AB5500_DEVID_REGULATORS] =   {
-		.nbanks = 2,
-		.bank =  (struct ab5500_i2c_ranges []) {
-			{
-				.bankid = AB5500_BANK_STARTUP,
-				.nranges = 12,
-				.range = (struct ab5500_reg_range[]) {
-					{
-						.first = 0x00,
-						.last = 0x01,
-						.perm = AB5500_PERM_RW,
-					},
-					{
-						.first = 0x1F,
-						.last = 0x1F,
-						.perm = AB5500_PERM_RW,
-					},
-					{
-						.first = 0x2E,
-						.last = 0x2E,
-						.perm = AB5500_PERM_RO,
-					},
-					{
-						.first = 0x2F,
-						.last = 0x30,
-						.perm = AB5500_PERM_RW,
-					},
-					{
-						.first = 0x50,
-						.last = 0x51,
-						.perm = AB5500_PERM_RW,
-					},
-					{
-						.first = 0x60,
-						.last = 0x61,
-						.perm = AB5500_PERM_RW,
-					},
-					{
-						.first = 0x66,
-						.last = 0x8A,
-						.perm = AB5500_PERM_RW,
-					},
-					{
-						.first = 0x8C,
-						.last = 0x96,
-						.perm = AB5500_PERM_RW,
-					},
-					{
-						.first = 0xAA,
-						.last = 0xB4,
-						.perm = AB5500_PERM_RW,
-					},
-					{
-						.first = 0xB7,
-						.last = 0xBF,
-						.perm = AB5500_PERM_RW,
-					},
-					{
-						.first = 0xC1,
-						.last = 0xCA,
-						.perm = AB5500_PERM_RW,
-					},
-					{
-						.first = 0xD3,
-						.last = 0xE0,
-						.perm = AB5500_PERM_RW,
-					},
-				},
-			},
-			{
-				.bankid = AB5500_BANK_SIM_USBSIM,
-				.nranges = 1,
-				.range = (struct ab5500_reg_range[]) {
-					{
-						.first = 0x13,
-						.last = 0x19,
-						.perm = AB5500_PERM_RW,
-					},
-				},
-			},
-		},
-	},
-	[AB5500_DEVID_SIM] =   {
-		.nbanks = 1,
-		.bank = (struct ab5500_i2c_ranges []) {
-			{
-				.bankid = AB5500_BANK_SIM_USBSIM,
-				.nranges = 1,
-				.range = (struct ab5500_reg_range[]) {
-					{
-						.first = 0x13,
-						.last = 0x19,
-						.perm = AB5500_PERM_RW,
-					},
-				},
-			},
-		},
-	},
-	[AB5500_DEVID_RTC] =   {
-		.nbanks = 1,
-		.bank = (struct ab5500_i2c_ranges []) {
-			{
-				.bankid = AB5500_BANK_RTC,
-				.nranges = 2,
-				.range = (struct ab5500_reg_range[]) {
-					{
-						.first = 0x00,
-						.last = 0x04,
-						.perm = AB5500_PERM_RW,
-					},
-					{
-						.first = 0x06,
-						.last = 0x0C,
-						.perm = AB5500_PERM_RW,
-					},
-				},
-			},
-		},
-	},
-	[AB5500_DEVID_CHARGER] =   {
-		.nbanks = 1,
-		.bank = (struct ab5500_i2c_ranges []) {
-			{
-				.bankid = AB5500_BANK_CHG,
-				.nranges = 2,
-				.range = (struct ab5500_reg_range[]) {
-					{
-						.first = 0x11,
-						.last = 0x11,
-						.perm = AB5500_PERM_RO,
-					},
-					{
-						.first = 0x12,
-						.last = 0x1B,
-						.perm = AB5500_PERM_RW,
-					},
-				},
-			},
-		},
-	},
-	[AB5500_DEVID_FUELGAUGE] =   {
-		.nbanks = 1,
-		.bank = (struct ab5500_i2c_ranges []) {
-			{
-				.bankid = AB5500_BANK_FG_BATTCOM_ACC,
-				.nranges = 2,
-				.range = (struct ab5500_reg_range[]) {
-					{
-						.first = 0x00,
-						.last = 0x0B,
-						.perm = AB5500_PERM_RO,
-					},
-					{
-						.first = 0x0C,
-						.last = 0x10,
-						.perm = AB5500_PERM_RW,
-					},
-				},
-			},
-		},
-	},
-	[AB5500_DEVID_VIBRATOR] =   {
-		.nbanks = 1,
-		.bank = (struct ab5500_i2c_ranges []) {
-			{
-				.bankid = AB5500_BANK_VIBRA,
-				.nranges = 2,
-				.range = (struct ab5500_reg_range[]) {
-					{
-						.first = 0x10,
-						.last = 0x13,
-						.perm = AB5500_PERM_RW,
-					},
-					{
-						.first = 0xFE,
-						.last = 0xFE,
-						.perm = AB5500_PERM_RW,
-					},
-				},
-			},
-		},
-	},
-	[AB5500_DEVID_CODEC] =   {
-		.nbanks = 1,
-		.bank = (struct ab5500_i2c_ranges []) {
-			{
-				.bankid = AB5500_BANK_AUDIO_HEADSETUSB,
-				.nranges = 2,
-				.range = (struct ab5500_reg_range[]) {
-					{
-						.first = 0x00,
-						.last = 0x48,
-						.perm = AB5500_PERM_RW,
-					},
-					{
-						.first = 0xEB,
-						.last = 0xFB,
-						.perm = AB5500_PERM_RW,
-					},
-				},
-			},
-		},
-	},
-	[AB5500_DEVID_POWER] = {
-		.nbanks	= 2,
-		.bank	= (struct ab5500_i2c_ranges []) {
-			{
-				.bankid = AB5500_BANK_STARTUP,
-				.nranges = 1,
-				.range = (struct ab5500_reg_range[]) {
-					{
-						.first = 0x30,
-						.last = 0x30,
-						.perm = AB5500_PERM_RW,
-					},
-				},
-			},
-			{
-				.bankid = AB5500_BANK_VIT_IO_I2C_CLK_TST_OTP,
-				.nranges = 1,
-				.range = (struct ab5500_reg_range[]) {
-					{
-						.first = 0x01,
-						.last = 0x01,
-						.perm = AB5500_PERM_RW,
-					},
-				},
-			},
-		},
-	},
-};
-
-#define AB5500_IRQ(bank, bit)	((bank) * 8 + (bit))
-
-/* I appologize for the resource names beeing a mix of upper case
- * and lower case but I want them to be exact as the documentation */
-static struct mfd_cell ab5500_devs[AB5500_NUM_DEVICES] = {
-	[AB5500_DEVID_LEDS] = {
-		.name = "ab5500-leds",
-		.id = AB5500_DEVID_LEDS,
-	},
-	[AB5500_DEVID_POWER] = {
-		.name = "ab5500-power",
-		.id = AB5500_DEVID_POWER,
-	},
-	[AB5500_DEVID_REGULATORS] = {
-		.name = "ab5500-regulator",
-		.id = AB5500_DEVID_REGULATORS,
-	},
-	[AB5500_DEVID_SIM] = {
-		.name = "ab5500-sim",
-		.id = AB5500_DEVID_SIM,
-		.num_resources = 1,
-		.resources = (struct resource[]) {
-			{
-				.name = "SIMOFF",
-				.flags = IORESOURCE_IRQ,
-				.start = AB5500_IRQ(2, 0), /*rising*/
-				.end = AB5500_IRQ(2, 1), /*falling*/
-			},
-		},
-	},
-	[AB5500_DEVID_RTC] = {
-		.name = "ab5500-rtc",
-		.id = AB5500_DEVID_RTC,
-		.num_resources = 1,
-		.resources = (struct resource[]) {
-			{
-				.name	= "RTC_Alarm",
-				.flags	= IORESOURCE_IRQ,
-				.start	= AB5500_IRQ(1, 7),
-				.end	= AB5500_IRQ(1, 7),
-			}
-		},
-	},
-	[AB5500_DEVID_CHARGER] = {
-		.name = "ab5500-charger",
-		.id = AB5500_DEVID_CHARGER,
-	},
-	[AB5500_DEVID_ADC] = {
-		.name = "ab5500-adc",
-		.id = AB5500_DEVID_ADC,
-		.num_resources = 10,
-		.resources = (struct resource[]) {
-			{
-				.name = "TRIGGER-0",
-				.flags = IORESOURCE_IRQ,
-				.start = AB5500_IRQ(0, 0),
-				.end = AB5500_IRQ(0, 0),
-			},
-			{
-				.name = "TRIGGER-1",
-				.flags = IORESOURCE_IRQ,
-				.start = AB5500_IRQ(0, 1),
-				.end = AB5500_IRQ(0, 1),
-			},
-			{
-				.name = "TRIGGER-2",
-				.flags = IORESOURCE_IRQ,
-				.start = AB5500_IRQ(0, 2),
-				.end = AB5500_IRQ(0, 2),
-			},
-			{
-				.name = "TRIGGER-3",
-				.flags = IORESOURCE_IRQ,
-				.start = AB5500_IRQ(0, 3),
-				.end = AB5500_IRQ(0, 3),
-			},
-			{
-				.name = "TRIGGER-4",
-				.flags = IORESOURCE_IRQ,
-				.start = AB5500_IRQ(0, 4),
-				.end = AB5500_IRQ(0, 4),
-			},
-			{
-				.name = "TRIGGER-5",
-				.flags = IORESOURCE_IRQ,
-				.start = AB5500_IRQ(0, 5),
-				.end = AB5500_IRQ(0, 5),
-			},
-			{
-				.name = "TRIGGER-6",
-				.flags = IORESOURCE_IRQ,
-				.start = AB5500_IRQ(0, 6),
-				.end = AB5500_IRQ(0, 6),
-			},
-			{
-				.name = "TRIGGER-7",
-				.flags = IORESOURCE_IRQ,
-				.start = AB5500_IRQ(0, 7),
-				.end = AB5500_IRQ(0, 7),
-			},
-			{
-				.name = "TRIGGER-VBAT",
-				.flags = IORESOURCE_IRQ,
-				.start = AB5500_IRQ(0, 8),
-				.end = AB5500_IRQ(0, 8),
-			},
-			{
-				.name = "TRIGGER-VBAT-TXON",
-				.flags = IORESOURCE_IRQ,
-				.start = AB5500_IRQ(0, 9),
-				.end = AB5500_IRQ(0, 9),
-			},
-		},
-	},
-	[AB5500_DEVID_FUELGAUGE] = {
-		.name = "ab5500-fuelgauge",
-		.id = AB5500_DEVID_FUELGAUGE,
-		.num_resources = 6,
-		.resources = (struct resource[]) {
-			{
-				.name = "Batt_attach",
-				.flags = IORESOURCE_IRQ,
-				.start = AB5500_IRQ(7, 5),
-				.end = AB5500_IRQ(7, 5),
-			},
-			{
-				.name = "Batt_removal",
-				.flags = IORESOURCE_IRQ,
-				.start = AB5500_IRQ(7, 6),
-				.end = AB5500_IRQ(7, 6),
-			},
-			{
-				.name = "UART_framing",
-				.flags = IORESOURCE_IRQ,
-				.start = AB5500_IRQ(7, 7),
-				.end = AB5500_IRQ(7, 7),
-			},
-			{
-				.name = "UART_overrun",
-				.flags = IORESOURCE_IRQ,
-				.start = AB5500_IRQ(8, 0),
-				.end = AB5500_IRQ(8, 0),
-			},
-			{
-				.name = "UART_Rdy_RX",
-				.flags = IORESOURCE_IRQ,
-				.start = AB5500_IRQ(8, 1),
-				.end = AB5500_IRQ(8, 1),
-			},
-			{
-				.name = "UART_Rdy_TX",
-				.flags = IORESOURCE_IRQ,
-				.start = AB5500_IRQ(8, 2),
-				.end = AB5500_IRQ(8, 2),
-			},
-		},
-	},
-	[AB5500_DEVID_VIBRATOR] = {
-		.name = "ab5500-vibrator",
-		.id = AB5500_DEVID_VIBRATOR,
-	},
-	[AB5500_DEVID_CODEC] = {
-		.name = "ab5500-codec",
-		.id = AB5500_DEVID_CODEC,
-		.num_resources = 3,
-		.resources = (struct resource[]) {
-			{
-				.name = "audio_spkr1_ovc",
-				.flags = IORESOURCE_IRQ,
-				.start = AB5500_IRQ(9, 5),
-				.end = AB5500_IRQ(9, 5),
-			},
-			{
-				.name = "audio_plllocked",
-				.flags = IORESOURCE_IRQ,
-				.start = AB5500_IRQ(9, 6),
-				.end = AB5500_IRQ(9, 6),
-			},
-			{
-				.name = "audio_spkr2_ovc",
-				.flags = IORESOURCE_IRQ,
-				.start = AB5500_IRQ(17, 4),
-				.end = AB5500_IRQ(17, 4),
-			},
-		},
-	},
-	[AB5500_DEVID_USB] = {
-		.name = "ab5500-usb",
-		.id = AB5500_DEVID_USB,
-		.num_resources = 36,
-		.resources = (struct resource[]) {
-			{
-				.name = "Link_Update",
-				.flags = IORESOURCE_IRQ,
-				.start = AB5500_IRQ(22, 1),
-				.end = AB5500_IRQ(22, 1),
-			},
-			{
-				.name = "DCIO",
-				.flags = IORESOURCE_IRQ,
-				.start = AB5500_IRQ(8, 3),
-				.end = AB5500_IRQ(8, 4),
-			},
-			{
-				.name = "VBUS_R",
-				.flags = IORESOURCE_IRQ,
-				.start = AB5500_IRQ(8, 5),
-				.end = AB5500_IRQ(8, 5),
-			},
-			{
-				.name = "VBUS_F",
-				.flags = IORESOURCE_IRQ,
-				.start = AB5500_IRQ(8, 6),
-				.end = AB5500_IRQ(8, 6),
-			},
-			{
-				.name = "CHGstate_10_PCVBUSchg",
-				.flags = IORESOURCE_IRQ,
-				.start = AB5500_IRQ(8, 7),
-				.end = AB5500_IRQ(8, 7),
-			},
-			{
-				.name = "DCIOreverse_ovc",
-				.flags = IORESOURCE_IRQ,
-				.start = AB5500_IRQ(9, 0),
-				.end = AB5500_IRQ(9, 0),
-			},
-			{
-				.name = "USBCharDetDone",
-				.flags = IORESOURCE_IRQ,
-				.start = AB5500_IRQ(9, 1),
-				.end = AB5500_IRQ(9, 1),
-			},
-			{
-				.name = "DCIO_no_limit",
-				.flags = IORESOURCE_IRQ,
-				.start = AB5500_IRQ(9, 2),
-				.end = AB5500_IRQ(9, 2),
-			},
-			{
-				.name = "USB_suspend",
-				.flags = IORESOURCE_IRQ,
-				.start = AB5500_IRQ(9, 3),
-				.end = AB5500_IRQ(9, 3),
-			},
-			{
-				.name = "DCIOreverse_fwdcurrent",
-				.flags = IORESOURCE_IRQ,
-				.start = AB5500_IRQ(9, 4),
-				.end = AB5500_IRQ(9, 4),
-			},
-			{
-				.name = "Vbus_Imeasmax_change",
-				.flags = IORESOURCE_IRQ,
-				.start = AB5500_IRQ(9, 5),
-				.end = AB5500_IRQ(9, 6),
-			},
-			{
-				.name = "OVV",
-				.flags = IORESOURCE_IRQ,
-				.start = AB5500_IRQ(14, 5),
-				.end = AB5500_IRQ(14, 5),
-			},
-			{
-				.name = "USBcharging_NOTok",
-				.flags = IORESOURCE_IRQ,
-				.start = AB5500_IRQ(15, 3),
-				.end = AB5500_IRQ(15, 3),
-			},
-			{
-				.name = "usb_adp_sensoroff",
-				.flags = IORESOURCE_IRQ,
-				.start = AB5500_IRQ(15, 6),
-				.end = AB5500_IRQ(15, 6),
-			},
-			{
-				.name = "usb_adp_probeplug",
-				.flags = IORESOURCE_IRQ,
-				.start = AB5500_IRQ(15, 7),
-				.end = AB5500_IRQ(15, 7),
-			},
-			{
-				.name = "usb_adp_sinkerror",
-				.flags = IORESOURCE_IRQ,
-				.start = AB5500_IRQ(16, 0),
-				.end = AB5500_IRQ(16, 6),
-			},
-			{
-				.name = "usb_adp_sourceerror",
-				.flags = IORESOURCE_IRQ,
-				.start = AB5500_IRQ(16, 1),
-				.end = AB5500_IRQ(16, 1),
-			},
-			{
-				.name = "usb_idgnd_r",
-				.flags = IORESOURCE_IRQ,
-				.start = AB5500_IRQ(16, 2),
-				.end = AB5500_IRQ(16, 2),
-			},
-			{
-				.name = "usb_idgnd_f",
-				.flags = IORESOURCE_IRQ,
-				.start = AB5500_IRQ(16, 3),
-				.end = AB5500_IRQ(16, 3),
-			},
-			{
-				.name = "usb_iddetR1",
-				.flags = IORESOURCE_IRQ,
-				.start = AB5500_IRQ(16, 4),
-				.end = AB5500_IRQ(16, 5),
-			},
-			{
-				.name = "usb_iddetR2",
-				.flags = IORESOURCE_IRQ,
-				.start = AB5500_IRQ(16, 6),
-				.end = AB5500_IRQ(16, 7),
-			},
-			{
-				.name = "usb_iddetR3",
-				.flags = IORESOURCE_IRQ,
-				.start = AB5500_IRQ(17, 0),
-				.end = AB5500_IRQ(17, 1),
-			},
-			{
-				.name = "usb_iddetR4",
-				.flags = IORESOURCE_IRQ,
-				.start = AB5500_IRQ(17, 2),
-				.end = AB5500_IRQ(17, 3),
-			},
-			{
-				.name = "CharTempWindowOk",
-				.flags = IORESOURCE_IRQ,
-				.start = AB5500_IRQ(17, 7),
-				.end = AB5500_IRQ(18, 0),
-			},
-			{
-				.name = "USB_SprDetect",
-				.flags = IORESOURCE_IRQ,
-				.start = AB5500_IRQ(18, 1),
-				.end = AB5500_IRQ(18, 1),
-			},
-			{
-				.name = "usb_adp_probe_unplug",
-				.flags = IORESOURCE_IRQ,
-				.start = AB5500_IRQ(18, 2),
-				.end = AB5500_IRQ(18, 2),
-			},
-			{
-				.name = "VBUSChDrop",
-				.flags = IORESOURCE_IRQ,
-				.start = AB5500_IRQ(18, 3),
-				.end = AB5500_IRQ(18, 4),
-			},
-			{
-				.name = "dcio_char_rec_done",
-				.flags = IORESOURCE_IRQ,
-				.start = AB5500_IRQ(18, 5),
-				.end = AB5500_IRQ(18, 5),
-			},
-			{
-				.name = "Charging_stopped_by_temp",
-				.flags = IORESOURCE_IRQ,
-				.start = AB5500_IRQ(18, 6),
-				.end = AB5500_IRQ(18, 6),
-			},
-			{
-				.name = "CHGstate_11_SafeModeVBUS",
-				.flags = IORESOURCE_IRQ,
-				.start = AB5500_IRQ(21, 1),
-				.end = AB5500_IRQ(21, 2),
-			},
-			{
-				.name = "CHGstate_12_comletedVBUS",
-				.flags = IORESOURCE_IRQ,
-				.start = AB5500_IRQ(21, 2),
-				.end = AB5500_IRQ(21, 2),
-			},
-			{
-				.name = "CHGstate_13_completedVBUS",
-				.flags = IORESOURCE_IRQ,
-				.start = AB5500_IRQ(21, 3),
-				.end = AB5500_IRQ(21, 3),
-			},
-			{
-				.name = "CHGstate_14_FullChgDCIO",
-				.flags = IORESOURCE_IRQ,
-				.start = AB5500_IRQ(21, 4),
-				.end = AB5500_IRQ(21, 4),
-			},
-			{
-				.name = "CHGstate_15_SafeModeDCIO",
-				.flags = IORESOURCE_IRQ,
-				.start = AB5500_IRQ(21, 5),
-				.end = AB5500_IRQ(21, 5),
-			},
-			{
-				.name = "CHGstate_16_OFFsuspendDCIO",
-				.flags = IORESOURCE_IRQ,
-				.start = AB5500_IRQ(21, 6),
-				.end = AB5500_IRQ(21, 6),
-			},
-			{
-				.name = "CHGstate_17_completedDCIO",
-				.flags = IORESOURCE_IRQ,
-				.start = AB5500_IRQ(21, 7),
-				.end = AB5500_IRQ(21, 7),
-			},
-		},
-	},
-	[AB5500_DEVID_OTP] = {
-		.name = "ab5500-otp",
-		.id = AB5500_DEVID_OTP,
-	},
-	[AB5500_DEVID_VIDEO] = {
-		.name = "ab5500-video",
-		.id = AB5500_DEVID_VIDEO,
-		.num_resources = 1,
-		.resources = (struct resource[]) {
-			{
-				.name = "plugTVdet",
-				.flags = IORESOURCE_IRQ,
-				.start = AB5500_IRQ(22, 2),
-				.end = AB5500_IRQ(22, 2),
-			},
-		},
-	},
-	[AB5500_DEVID_DBIECI] = {
-		.name = "ab5500-dbieci",
-		.id = AB5500_DEVID_DBIECI,
-		.num_resources = 10,
-		.resources = (struct resource[]) {
-			{
-				.name = "COLL",
-				.flags = IORESOURCE_IRQ,
-				.start = AB5500_IRQ(14, 0),
-				.end = AB5500_IRQ(14, 0),
-			},
-			{
-				.name = "RESERR",
-				.flags = IORESOURCE_IRQ,
-				.start = AB5500_IRQ(14, 1),
-				.end = AB5500_IRQ(14, 1),
-			},
-			{
-				.name = "FRAERR",
-				.flags = IORESOURCE_IRQ,
-				.start = AB5500_IRQ(14, 2),
-				.end = AB5500_IRQ(14, 2),
-			},
-			{
-				.name = "COMERR",
-				.flags = IORESOURCE_IRQ,
-				.start = AB5500_IRQ(14, 3),
-				.end = AB5500_IRQ(14, 3),
-			},
-			{
-				.name = "BSI_indicator",
-				.flags = IORESOURCE_IRQ,
-				.start = AB5500_IRQ(14, 4),
-				.end = AB5500_IRQ(14, 4),
-			},
-			{
-				.name = "SPDSET",
-				.flags = IORESOURCE_IRQ,
-				.start = AB5500_IRQ(14, 6),
-				.end = AB5500_IRQ(14, 6),
-			},
-			{
-				.name = "DSENT",
-				.flags = IORESOURCE_IRQ,
-				.start = AB5500_IRQ(14, 7),
-				.end = AB5500_IRQ(14, 7),
-			},
-			{
-				.name = "DREC",
-				.flags = IORESOURCE_IRQ,
-				.start = AB5500_IRQ(15, 0),
-				.end = AB5500_IRQ(15, 0),
-			},
-			{
-				.name = "ACCINT",
-				.flags = IORESOURCE_IRQ,
-				.start = AB5500_IRQ(15, 1),
-				.end = AB5500_IRQ(15, 1),
-			},
-			{
-				.name = "NOPINT",
-				.flags = IORESOURCE_IRQ,
-				.start = AB5500_IRQ(15, 2),
-				.end = AB5500_IRQ(15, 2),
-			},
-		},
-	},
-	[AB5500_DEVID_ONSWA] = {
-		.name = "ab5500-onswa",
-		.id = AB5500_DEVID_ONSWA,
-		.num_resources = 2,
-		.resources = (struct resource[]) {
-			{
-				.name	= "ONSWAn_rising",
-				.flags	= IORESOURCE_IRQ,
-				.start	= AB5500_IRQ(1, 3),
-				.end	= AB5500_IRQ(1, 3),
-			},
-			{
-				.name	= "ONSWAn_falling",
-				.flags	= IORESOURCE_IRQ,
-				.start	= AB5500_IRQ(1, 4),
-				.end	= AB5500_IRQ(1, 4),
-			},
-		},
-	},
-};
-
-/*
- * Functionality for getting/setting register values.
- */
-int ab5500_get_register_interruptible_raw(struct ab5500 *ab,
-					  u8 bank, u8 reg,
-					  u8 *value)
-{
-	int err;
-
-	if (bank >= AB5500_NUM_BANKS)
-		return -EINVAL;
-
-	err = mutex_lock_interruptible(&ab->access_mutex);
-	if (err)
-		return err;
-	err = db5500_prcmu_abb_read(bankinfo[bank].slave_addr, reg, value, 1);
-
-	mutex_unlock(&ab->access_mutex);
-	return err;
-}
-
-static int get_register_page_interruptible(struct ab5500 *ab, u8 bank,
-	u8 first_reg, u8 *regvals, u8 numregs)
-{
-	int err;
-
-	if (bank >= AB5500_NUM_BANKS)
-		return -EINVAL;
-
-	err = mutex_lock_interruptible(&ab->access_mutex);
-	if (err)
-		return err;
-
-	while (numregs) {
-		/* The hardware limit for get page is 4 */
-		u8 curnum = min_t(u8, numregs, 4u);
-
-		err = db5500_prcmu_abb_read(bankinfo[bank].slave_addr,
-					    first_reg, regvals, curnum);
-		if (err)
-			goto out;
-
-		numregs -= curnum;
-		first_reg += curnum;
-		regvals += curnum;
-	}
-
-out:
-	mutex_unlock(&ab->access_mutex);
-	return err;
-}
-
-int ab5500_mask_and_set_register_interruptible_raw(struct ab5500 *ab, u8 bank,
-	u8 reg, u8 bitmask, u8 bitvalues)
-{
-	int err = 0;
-
-	if (bank >= AB5500_NUM_BANKS)
-		return -EINVAL;
-
-	if (bitmask) {
-		u8 buf;
-
-		err = mutex_lock_interruptible(&ab->access_mutex);
-		if (err)
-			return err;
-
-		if (bitmask == 0xFF) /* No need to read in this case. */
-			buf = bitvalues;
-		else { /* Read and modify the register value. */
-			err = db5500_prcmu_abb_read(bankinfo[bank].slave_addr,
-				reg, &buf, 1);
-			if (err)
-				return err;
-
-			buf = ((~bitmask & buf) | (bitmask & bitvalues));
-		}
-		/* Write the new value. */
-		err = db5500_prcmu_abb_write(bankinfo[bank].slave_addr, reg,
-					     &buf, 1);
-
-		mutex_unlock(&ab->access_mutex);
-	}
-	return err;
-}
-
-static int
-set_register_interruptible(struct ab5500 *ab, u8 bank, u8 reg, u8 value)
-{
-	return ab5500_mask_and_set_register_interruptible_raw(ab, bank, reg,
-							      0xff, value);
-}
-
-/*
- * Read/write permission checking functions.
- */
-static const struct ab5500_i2c_ranges *get_bankref(u8 devid, u8 bank)
-{
-	u8 i;
-
-	if (devid < AB5500_NUM_DEVICES) {
-		for (i = 0; i < ab5500_bank_ranges[devid].nbanks; i++) {
-			if (ab5500_bank_ranges[devid].bank[i].bankid == bank)
-				return &ab5500_bank_ranges[devid].bank[i];
-		}
-	}
-	return NULL;
-}
-
-static bool page_write_allowed(u8 devid, u8 bank, u8 first_reg, u8 last_reg)
-{
-	u8 i; /* range loop index */
-	const struct ab5500_i2c_ranges *bankref;
-
-	bankref = get_bankref(devid, bank);
-	if (bankref == NULL || last_reg < first_reg)
-		return false;
-
-	for (i = 0; i < bankref->nranges; i++) {
-		if (first_reg < bankref->range[i].first)
-			break;
-		if ((last_reg <= bankref->range[i].last) &&
-			(bankref->range[i].perm & AB5500_PERM_WR))
-			return true;
-	}
-	return false;
-}
-
-static bool reg_write_allowed(u8 devid, u8 bank, u8 reg)
-{
-	return page_write_allowed(devid, bank, reg, reg);
-}
-
-static bool page_read_allowed(u8 devid, u8 bank, u8 first_reg, u8 last_reg)
-{
-	u8 i;
-	const struct ab5500_i2c_ranges *bankref;
-
-	bankref = get_bankref(devid, bank);
-	if (bankref == NULL || last_reg < first_reg)
-		return false;
-
-
-	/* Find the range (if it exists in the list) that includes first_reg. */
-	for (i = 0; i < bankref->nranges; i++) {
-		if (first_reg < bankref->range[i].first)
-			return false;
-		if (first_reg <= bankref->range[i].last)
-			break;
-	}
-	/* Make sure that the entire range up to and including last_reg is
-	 * readable. This may span several of the ranges in the list.
-	 */
-	while ((i < bankref->nranges) &&
-		(bankref->range[i].perm & AB5500_PERM_RD)) {
-		if (last_reg <= bankref->range[i].last)
-			return true;
-		if ((++i >= bankref->nranges) ||
-			(bankref->range[i].first !=
-				(bankref->range[i - 1].last + 1))) {
-			break;
-		}
-	}
-	return false;
-}
-
-static bool reg_read_allowed(u8 devid, u8 bank, u8 reg)
-{
-	return page_read_allowed(devid, bank, reg, reg);
-}
-
-
-/*
- * The exported register access functionality.
- */
-static int ab5500_get_chip_id(struct device *dev)
-{
-	struct ab5500 *ab = dev_get_drvdata(dev->parent);
-
-	return (int)ab->chip_id;
-}
-
-static int ab5500_mask_and_set_register_interruptible(struct device *dev,
-		u8 bank, u8 reg, u8 bitmask, u8 bitvalues)
-{
-	struct ab5500 *ab;
-	struct platform_device *pdev = to_platform_device(dev);
-
-	if ((AB5500_NUM_BANKS <= bank) ||
-		!reg_write_allowed(pdev->id, bank, reg))
-		return -EINVAL;
-
-	ab = dev_get_drvdata(dev->parent);
-	return ab5500_mask_and_set_register_interruptible_raw(ab, bank, reg,
-		bitmask, bitvalues);
-}
-
-static int ab5500_set_register_interruptible(struct device *dev, u8 bank,
-	u8 reg, u8 value)
-{
-	return ab5500_mask_and_set_register_interruptible(dev, bank, reg, 0xFF,
-		value);
-}
-
-static int ab5500_get_register_interruptible(struct device *dev, u8 bank,
-		u8 reg, u8 *value)
-{
-	struct ab5500 *ab;
-	struct platform_device *pdev = to_platform_device(dev);
-
-	if ((AB5500_NUM_BANKS <= bank) ||
-		!reg_read_allowed(pdev->id, bank, reg))
-		return -EINVAL;
-
-	ab = dev_get_drvdata(dev->parent);
-	return ab5500_get_register_interruptible_raw(ab, bank, reg, value);
-}
-
-static int ab5500_get_register_page_interruptible(struct device *dev, u8 bank,
-		u8 first_reg, u8 *regvals, u8 numregs)
-{
-	struct ab5500 *ab;
-	struct platform_device *pdev = to_platform_device(dev);
-
-	if ((AB5500_NUM_BANKS <= bank) ||
-		!page_read_allowed(pdev->id, bank,
-			first_reg, (first_reg + numregs - 1)))
-		return -EINVAL;
-
-	ab = dev_get_drvdata(dev->parent);
-	return get_register_page_interruptible(ab, bank, first_reg, regvals,
-		numregs);
-}
-
-static int
-ab5500_event_registers_startup_state_get(struct device *dev, u8 *event)
-{
-	struct ab5500 *ab;
-
-	ab = dev_get_drvdata(dev->parent);
-	if (!ab->startup_events_read)
-		return -EAGAIN; /* Try again later */
-
-	memcpy(event, ab->startup_events, AB5500_NUM_EVENT_REG);
-	return 0;
-}
-
-static struct abx500_ops ab5500_ops = {
-	.get_chip_id = ab5500_get_chip_id,
-	.get_register = ab5500_get_register_interruptible,
-	.set_register = ab5500_set_register_interruptible,
-	.get_register_page = ab5500_get_register_page_interruptible,
-	.set_register_page = NULL,
-	.mask_and_set_register = ab5500_mask_and_set_register_interruptible,
-	.event_registers_startup_state_get =
-		ab5500_event_registers_startup_state_get,
-	.startup_irq_enabled = NULL,
-};
-
-/*
- * ab5500_setup : Basic set-up, datastructure creation/destruction
- *		  and I2C interface.This sets up a default config
- *		  in the AB5500 chip so that it will work as expected.
- * @ab :	  Pointer to ab5500 structure
- * @settings :    Pointer to struct abx500_init_settings
- * @size :        Size of init data
- */
-static int __init ab5500_setup(struct ab5500 *ab,
-	struct abx500_init_settings *settings, unsigned int size)
-{
-	int err = 0;
-	int i;
-
-	for (i = 0; i < size; i++) {
-		err = ab5500_mask_and_set_register_interruptible_raw(ab,
-			settings[i].bank,
-			settings[i].reg,
-			0xFF, settings[i].setting);
-		if (err)
-			goto exit_no_setup;
-
-		/* If event mask register update the event mask in ab5500 */
-		if ((settings[i].bank == AB5500_BANK_IT) &&
-			(AB5500_MASK_BASE <= settings[i].reg) &&
-			(settings[i].reg <= AB5500_MASK_END)) {
-			ab->mask[settings[i].reg - AB5500_MASK_BASE] =
-				settings[i].setting;
-		}
-	}
-exit_no_setup:
-	return err;
-}
-
-struct ab_family_id {
-	u8	id;
-	char	*name;
-};
-
-static const struct ab_family_id ids[] __initdata = {
-	/* AB5500 */
-	{
-		.id = AB5500_1_0,
-		.name = "1.0"
-	},
-	{
-		.id = AB5500_1_1,
-		.name = "1.1"
-	},
-	/* Terminator */
-	{
-		.id = 0x00,
-	}
-};
-
-static int __init ab5500_probe(struct platform_device *pdev)
-{
-	struct ab5500 *ab;
-	struct ab5500_platform_data *ab5500_plf_data =
-		pdev->dev.platform_data;
-	int err;
-	int i;
-
-	ab = kzalloc(sizeof(struct ab5500), GFP_KERNEL);
-	if (!ab) {
-		dev_err(&pdev->dev,
-			"could not allocate ab5500 device\n");
-		return -ENOMEM;
-	}
-
-	/* Initialize data structure */
-	mutex_init(&ab->access_mutex);
-	mutex_init(&ab->irq_lock);
-	ab->dev = &pdev->dev;
-
-	platform_set_drvdata(pdev, ab);
-
-	/* Read chip ID register */
-	err = ab5500_get_register_interruptible_raw(ab,
-					AB5500_BANK_VIT_IO_I2C_CLK_TST_OTP,
-					AB5500_CHIP_ID, &ab->chip_id);
-	if (err) {
-		dev_err(&pdev->dev, "could not communicate with the analog "
-			"baseband chip\n");
-		goto exit_no_detect;
-	}
-
-	for (i = 0; ids[i].id != 0x0; i++) {
-		if (ids[i].id == ab->chip_id) {
-			snprintf(&ab->chip_name[0], sizeof(ab->chip_name) - 1,
-				"AB5500 %s", ids[i].name);
-			break;
-		}
-	}
-	if (ids[i].id == 0x0) {
-		dev_err(&pdev->dev, "unknown analog baseband chip id: 0x%x\n",
-			ab->chip_id);
-		dev_err(&pdev->dev, "driver not started!\n");
-		goto exit_no_detect;
-	}
-
-	/* Clear and mask all interrupts */
-	for (i = 0; i < AB5500_NUM_IRQ_REGS; i++) {
-		u8 latchreg = AB5500_IT_LATCH0_REG + i;
-		u8 maskreg = AB5500_IT_MASK0_REG + i;
-		u8 val;
-
-		ab5500_get_register_interruptible_raw(ab, AB5500_BANK_IT,
-						      latchreg, &val);
-		set_register_interruptible(ab, AB5500_BANK_IT, maskreg, 0xff);
-		ab->mask[i] = ab->oldmask[i] = 0xff;
-	}
-
-	err = abx500_register_ops(&pdev->dev, &ab5500_ops);
-	if (err) {
-		dev_err(&pdev->dev, "ab5500_register ops error\n");
-		goto exit_no_detect;
-	}
-
-	/* Set up and register the platform devices. */
-	for (i = 0; i < AB5500_NUM_DEVICES; i++) {
-		ab5500_devs[i].platform_data = ab5500_plf_data->dev_data[i];
-		ab5500_devs[i].pdata_size =
-			sizeof(ab5500_plf_data->dev_data[i]);
-	}
-
-	err = mfd_add_devices(&pdev->dev, 0, ab5500_devs,
-		ARRAY_SIZE(ab5500_devs), NULL,
-		ab5500_plf_data->irq.base);
-	if (err) {
-		dev_err(&pdev->dev, "ab5500_mfd_add_device error\n");
-		goto exit_no_detect;
-	}
-
-	err = ab5500_setup(ab, ab5500_plf_data->init_settings,
-		ab5500_plf_data->init_settings_sz);
-	if (err) {
-		dev_err(&pdev->dev, "ab5500_setup error\n");
-		goto exit_no_detect;
-	}
-
-	ab5500_setup_debugfs(ab);
-
-	dev_info(&pdev->dev, "detected AB chip: %s\n", &ab->chip_name[0]);
-	return 0;
-
-exit_no_detect:
-	kfree(ab);
-	return err;
-}
-
-static int __exit ab5500_remove(struct platform_device *pdev)
-{
-	struct ab5500 *ab = platform_get_drvdata(pdev);
-
-	ab5500_remove_debugfs();
-	mfd_remove_devices(&pdev->dev);
-	kfree(ab);
-	return 0;
-}
-
-static struct platform_driver ab5500_driver = {
-	.driver = {
-		.name = "ab5500-core",
-		.owner = THIS_MODULE,
-	},
-	.remove  = __exit_p(ab5500_remove),
-};
-
-static int __init ab5500_core_init(void)
-{
-	return platform_driver_probe(&ab5500_driver, ab5500_probe);
-}
-
-static void __exit ab5500_core_exit(void)
-{
-	platform_driver_unregister(&ab5500_driver);
-}
-
-subsys_initcall(ab5500_core_init);
-module_exit(ab5500_core_exit);
-
-MODULE_AUTHOR("Mattias Wallin <mattias.wallin@stericsson.com>");
-MODULE_DESCRIPTION("AB5500 core driver");
-MODULE_LICENSE("GPL");
diff --git a/drivers/mfd/ab5500-debugfs.c b/drivers/mfd/ab5500-debugfs.c
deleted file mode 100644
index 72006940937..00000000000
--- a/drivers/mfd/ab5500-debugfs.c
+++ /dev/null
@@ -1,807 +0,0 @@
-/*
- * Copyright (C) 2011 ST-Ericsson
- * License terms: GNU General Public License (GPL) version 2
- * Debugfs support for the AB5500 MFD driver
- */
-
-#include <linux/module.h>
-#include <linux/debugfs.h>
-#include <linux/seq_file.h>
-#include <linux/mfd/abx500.h>
-#include <linux/mfd/abx500/ab5500.h>
-#include <linux/uaccess.h>
-
-#include "ab5500-core.h"
-#include "ab5500-debugfs.h"
-
-static struct ab5500_i2c_ranges ab5500_reg_ranges[AB5500_NUM_BANKS] = {
-	[AB5500_BANK_LED] = {
-		.bankid = AB5500_BANK_LED,
-		.nranges = 1,
-		.range = (struct ab5500_reg_range[]) {
-			{
-				.first = 0x00,
-				.last = 0x0C,
-				.perm = AB5500_PERM_RW,
-			},
-		},
-	},
-	[AB5500_BANK_ADC] = {
-		.bankid = AB5500_BANK_ADC,
-		.nranges = 6,
-		.range = (struct ab5500_reg_range[]) {
-			{
-				.first = 0x1F,
-				.last = 0x22,
-				.perm = AB5500_PERM_RO,
-			},
-			{
-				.first = 0x23,
-				.last = 0x24,
-				.perm = AB5500_PERM_RW,
-			},
-			{
-				.first = 0x26,
-				.last = 0x2D,
-				.perm = AB5500_PERM_RO,
-			},
-			{
-				.first = 0x2F,
-				.last = 0x34,
-				.perm = AB5500_PERM_RW,
-			},
-			{
-				.first = 0x37,
-				.last = 0x57,
-				.perm = AB5500_PERM_RW,
-			},
-			{
-				.first = 0x58,
-				.last = 0x58,
-				.perm = AB5500_PERM_RO,
-			},
-		},
-	},
-	[AB5500_BANK_RTC] = {
-		.bankid = AB5500_BANK_RTC,
-		.nranges = 2,
-		.range = (struct ab5500_reg_range[]) {
-			{
-				.first = 0x00,
-				.last = 0x04,
-				.perm = AB5500_PERM_RW,
-			},
-			{
-				.first = 0x06,
-				.last = 0x0C,
-				.perm = AB5500_PERM_RW,
-			},
-		},
-	},
-	[AB5500_BANK_STARTUP] = {
-		.bankid = AB5500_BANK_STARTUP,
-		.nranges = 12,
-		.range = (struct ab5500_reg_range[]) {
-			{
-				.first = 0x00,
-				.last = 0x01,
-				.perm = AB5500_PERM_RW,
-			},
-			{
-				.first = 0x1F,
-				.last = 0x1F,
-				.perm = AB5500_PERM_RW,
-			},
-			{
-				.first = 0x2E,
-				.last = 0x2E,
-				.perm = AB5500_PERM_RO,
-			},
-			{
-				.first = 0x2F,
-				.last = 0x30,
-				.perm = AB5500_PERM_RW,
-			},
-			{
-				.first = 0x50,
-				.last = 0x51,
-				.perm = AB5500_PERM_RW,
-			},
-			{
-				.first = 0x60,
-				.last = 0x61,
-				.perm = AB5500_PERM_RW,
-			},
-			{
-				.first = 0x66,
-				.last = 0x8A,
-				.perm = AB5500_PERM_RW,
-			},
-			{
-				.first = 0x8C,
-				.last = 0x96,
-				.perm = AB5500_PERM_RW,
-			},
-			{
-				.first = 0xAA,
-				.last = 0xB4,
-				.perm = AB5500_PERM_RW,
-			},
-			{
-				.first = 0xB7,
-				.last = 0xBF,
-				.perm = AB5500_PERM_RW,
-			},
-			{
-				.first = 0xC1,
-				.last = 0xCA,
-				.perm = AB5500_PERM_RW,
-			},
-			{
-				.first = 0xD3,
-				.last = 0xE0,
-				.perm = AB5500_PERM_RW,
-			},
-		},
-	},
-	[AB5500_BANK_DBI_ECI] = {
-		.bankid = AB5500_BANK_DBI_ECI,
-		.nranges = 3,
-		.range = (struct ab5500_reg_range[]) {
-			{
-				.first = 0x00,
-				.last = 0x07,
-				.perm = AB5500_PERM_RW,
-			},
-			{
-				.first = 0x10,
-				.last = 0x10,
-				.perm = AB5500_PERM_RW,
-			},
-			{
-				.first = 0x13,
-				.last = 0x13,
-				.perm = AB5500_PERM_RW,
-			},
-		},
-	},
-	[AB5500_BANK_CHG] = {
-		.bankid = AB5500_BANK_CHG,
-		.nranges = 2,
-		.range = (struct ab5500_reg_range[]) {
-			{
-				.first = 0x11,
-				.last = 0x11,
-				.perm = AB5500_PERM_RO,
-			},
-			{
-				.first = 0x12,
-				.last = 0x1B,
-				.perm = AB5500_PERM_RW,
-			},
-		},
-	},
-	[AB5500_BANK_FG_BATTCOM_ACC] = {
-		.bankid = AB5500_BANK_FG_BATTCOM_ACC,
-		.nranges = 2,
-		.range = (struct ab5500_reg_range[]) {
-			{
-				.first = 0x00,
-				.last = 0x0B,
-				.perm = AB5500_PERM_RO,
-			},
-			{
-				.first = 0x0C,
-				.last = 0x10,
-				.perm = AB5500_PERM_RW,
-			},
-		},
-	},
-	[AB5500_BANK_USB] = {
-		.bankid = AB5500_BANK_USB,
-		.nranges = 12,
-		.range = (struct ab5500_reg_range[]) {
-			{
-				.first = 0x01,
-				.last = 0x01,
-				.perm = AB5500_PERM_RW,
-			},
-			{
-				.first = 0x80,
-				.last = 0x83,
-				.perm = AB5500_PERM_RW,
-			},
-			{
-				.first = 0x87,
-				.last = 0x8A,
-				.perm = AB5500_PERM_RW,
-			},
-			{
-				.first = 0x8B,
-				.last = 0x8B,
-				.perm = AB5500_PERM_RO,
-			},
-			{
-				.first = 0x91,
-				.last = 0x92,
-				.perm = AB5500_PERM_RO,
-			},
-			{
-				.first = 0x93,
-				.last = 0x93,
-				.perm = AB5500_PERM_RW,
-			},
-			{
-				.first = 0x94,
-				.last = 0x94,
-				.perm = AB5500_PERM_RO,
-			},
-			{
-				.first = 0xA8,
-				.last = 0xB0,
-				.perm = AB5500_PERM_RO,
-			},
-			{
-				.first = 0xB2,
-				.last = 0xB2,
-				.perm = AB5500_PERM_RO,
-			},
-			{
-				.first = 0xB4,
-				.last = 0xBC,
-				.perm = AB5500_PERM_RO,
-			},
-			{
-				.first = 0xBF,
-				.last = 0xBF,
-				.perm = AB5500_PERM_RO,
-			},
-			{
-				.first = 0xC1,
-				.last = 0xC5,
-				.perm = AB5500_PERM_RO,
-			},
-		},
-	},
-	[AB5500_BANK_IT] = {
-		.bankid = AB5500_BANK_IT,
-		.nranges = 4,
-		.range = (struct ab5500_reg_range[]) {
-			{
-				.first = 0x00,
-				.last = 0x02,
-				.perm = AB5500_PERM_RO,
-			},
-			{
-				.first = 0x20,
-				.last = 0x36,
-				.perm = AB5500_PERM_RO,
-			},
-			{
-				.first = 0x40,
-				.last = 0x56,
-				.perm = AB5500_PERM_RO,
-			},
-			{
-				.first = 0x60,
-				.last = 0x76,
-				.perm = AB5500_PERM_RO,
-			},
-		},
-	},
-	[AB5500_BANK_VDDDIG_IO_I2C_CLK_TST] = {
-		.bankid = AB5500_BANK_VDDDIG_IO_I2C_CLK_TST,
-		.nranges = 7,
-		.range = (struct ab5500_reg_range[]) {
-			{
-				.first = 0x02,
-				.last = 0x02,
-				.perm = AB5500_PERM_RW,
-			},
-			{
-				.first = 0x12,
-				.last = 0x12,
-				.perm = AB5500_PERM_RW,
-			},
-			{
-				.first = 0x30,
-				.last = 0x34,
-				.perm = AB5500_PERM_RW,
-			},
-			{
-				.first = 0x40,
-				.last = 0x44,
-				.perm = AB5500_PERM_RW,
-			},
-			{
-				.first = 0x50,
-				.last = 0x54,
-				.perm = AB5500_PERM_RW,
-			},
-			{
-				.first = 0x60,
-				.last = 0x64,
-				.perm = AB5500_PERM_RW,
-			},
-			{
-				.first = 0x70,
-				.last = 0x74,
-				.perm = AB5500_PERM_RW,
-			},
-		},
-	},
-	[AB5500_BANK_VIT_IO_I2C_CLK_TST_OTP] = {
-		.bankid = AB5500_BANK_VIT_IO_I2C_CLK_TST_OTP,
-		.nranges = 13,
-		.range = (struct ab5500_reg_range[]) {
-			{
-				.first = 0x01,
-				.last = 0x01,
-				.perm = AB5500_PERM_RW,
-			},
-			{
-				.first = 0x02,
-				.last = 0x02,
-				.perm = AB5500_PERM_RO,
-			},
-			{
-				.first = 0x0D,
-				.last = 0x0F,
-				.perm = AB5500_PERM_RW,
-			},
-			{
-				.first = 0x1C,
-				.last = 0x1C,
-				.perm = AB5500_PERM_RW,
-			},
-			{
-				.first = 0x1E,
-				.last = 0x1E,
-				.perm = AB5500_PERM_RW,
-			},
-			{
-				.first = 0x20,
-				.last = 0x21,
-				.perm = AB5500_PERM_RW,
-			},
-			{
-				.first = 0x25,
-				.last = 0x25,
-				.perm = AB5500_PERM_RW,
-			},
-			{
-				.first = 0x28,
-				.last = 0x2A,
-				.perm = AB5500_PERM_RW,
-			},
-			{
-				.first = 0x30,
-				.last = 0x33,
-				.perm = AB5500_PERM_RW,
-			},
-			{
-				.first = 0x40,
-				.last = 0x43,
-				.perm = AB5500_PERM_RW,
-			},
-			{
-				.first = 0x50,
-				.last = 0x53,
-				.perm = AB5500_PERM_RW,
-			},
-			{
-				.first = 0x60,
-				.last = 0x63,
-				.perm = AB5500_PERM_RW,
-			},
-			{
-				.first = 0x70,
-				.last = 0x73,
-				.perm = AB5500_PERM_RW,
-			},
-		},
-	},
-	[AB5500_BANK_VIBRA] = {
-		.bankid = AB5500_BANK_VIBRA,
-		.nranges = 2,
-		.range = (struct ab5500_reg_range[]) {
-			{
-				.first = 0x10,
-				.last = 0x13,
-				.perm = AB5500_PERM_RW,
-			},
-			{
-				.first = 0xFE,
-				.last = 0xFE,
-				.perm = AB5500_PERM_RW,
-			},
-		},
-	},
-	[AB5500_BANK_AUDIO_HEADSETUSB] = {
-		.bankid = AB5500_BANK_AUDIO_HEADSETUSB,
-		.nranges = 2,
-		.range = (struct ab5500_reg_range[]) {
-			{
-				.first = 0x00,
-				.last = 0x48,
-				.perm = AB5500_PERM_RW,
-			},
-			{
-				.first = 0xEB,
-				.last = 0xFB,
-				.perm = AB5500_PERM_RW,
-			},
-		},
-	},
-	[AB5500_BANK_SIM_USBSIM] = {
-		.bankid = AB5500_BANK_SIM_USBSIM,
-		.nranges = 1,
-		.range = (struct ab5500_reg_range[]) {
-			{
-				.first = 0x13,
-				.last = 0x19,
-				.perm = AB5500_PERM_RW,
-			},
-		},
-	},
-	[AB5500_BANK_VDENC] = {
-		.bankid = AB5500_BANK_VDENC,
-		.nranges = 12,
-		.range = (struct ab5500_reg_range[]) {
-			{
-				.first = 0x00,
-				.last = 0x08,
-				.perm = AB5500_PERM_RW,
-			},
-			{
-				.first = 0x09,
-				.last = 0x09,
-				.perm = AB5500_PERM_RO,
-			},
-			{
-				.first = 0x0A,
-				.last = 0x12,
-				.perm = AB5500_PERM_RW,
-			},
-			{
-				.first = 0x15,
-				.last = 0x19,
-				.perm = AB5500_PERM_RW,
-			},
-			{
-				.first = 0x1B,
-				.last = 0x21,
-				.perm = AB5500_PERM_RW,
-			},
-			{
-				.first = 0x27,
-				.last = 0x2C,
-				.perm = AB5500_PERM_RW,
-			},
-			{
-				.first = 0x41,
-				.last = 0x41,
-				.perm = AB5500_PERM_RW,
-			},
-			{
-				.first = 0x45,
-				.last = 0x5B,
-				.perm = AB5500_PERM_RW,
-			},
-			{
-				.first = 0x5D,
-				.last = 0x5D,
-				.perm = AB5500_PERM_RW,
-			},
-			{
-				.first = 0x69,
-				.last = 0x69,
-				.perm = AB5500_PERM_RW,
-			},
-			{
-				.first = 0x6C,
-				.last = 0x6D,
-				.perm = AB5500_PERM_RW,
-			},
-			{
-				.first = 0x80,
-				.last = 0x81,
-				.perm = AB5500_PERM_RW,
-			},
-		},
-	},
-};
-
-static int ab5500_registers_print(struct seq_file *s, void *p)
-{
-	struct ab5500 *ab = s->private;
-	unsigned int i;
-	u8 bank = (u8)ab->debug_bank;
-
-	seq_printf(s, "ab5500 register values:\n");
-	for (bank = 0; bank < AB5500_NUM_BANKS; bank++) {
-		seq_printf(s, " bank %u, %s (0x%x):\n", bank,
-				bankinfo[bank].name,
-				bankinfo[bank].slave_addr);
-		for (i = 0; i < ab5500_reg_ranges[bank].nranges; i++) {
-			u8 reg;
-			int err;
-
-			for (reg = ab5500_reg_ranges[bank].range[i].first;
-				reg <= ab5500_reg_ranges[bank].range[i].last;
-				reg++) {
-				u8 value;
-
-				err = ab5500_get_register_interruptible_raw(ab,
-								bank, reg,
-								&value);
-				if (err < 0) {
-					dev_err(ab->dev, "get_reg failed %d"
-						"bank 0x%x reg 0x%x\n",
-						err, bank, reg);
-					return err;
-				}
-
-				err = seq_printf(s, "[%d/0x%02X]: 0x%02X\n",
-						bank, reg, value);
-				if (err < 0) {
-					dev_err(ab->dev,
-						"seq_printf overflow\n");
-					/*
-					 * Error is not returned here since
-					 * the output is wanted in any case
-					 */
-					return 0;
-				}
-			}
-		}
-	}
-	return 0;
-}
-
-static int ab5500_registers_open(struct inode *inode, struct file *file)
-{
-	return single_open(file, ab5500_registers_print, inode->i_private);
-}
-
-static const struct file_operations ab5500_registers_fops = {
-	.open = ab5500_registers_open,
-	.read = seq_read,
-	.llseek = seq_lseek,
-	.release = single_release,
-	.owner = THIS_MODULE,
-};
-
-static int ab5500_bank_print(struct seq_file *s, void *p)
-{
-	struct ab5500 *ab = s->private;
-
-	seq_printf(s, "%d\n", ab->debug_bank);
-	return 0;
-}
-
-static int ab5500_bank_open(struct inode *inode, struct file *file)
-{
-	return single_open(file, ab5500_bank_print, inode->i_private);
-}
-
-static ssize_t ab5500_bank_write(struct file *file,
-	const char __user *user_buf,
-	size_t count, loff_t *ppos)
-{
-	struct ab5500 *ab = ((struct seq_file *)(file->private_data))->private;
-	char buf[32];
-	int buf_size;
-	unsigned long user_bank;
-	int err;
-
-	/* 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_bank);
-	if (err)
-		return -EINVAL;
-
-	if (user_bank >= AB5500_NUM_BANKS) {
-		dev_err(ab->dev,
-			"debugfs error input > number of banks\n");
-		return -EINVAL;
-	}
-
-	ab->debug_bank = user_bank;
-
-	return buf_size;
-}
-
-static int ab5500_address_print(struct seq_file *s, void *p)
-{
-	struct ab5500 *ab = s->private;
-
-	seq_printf(s, "0x%02X\n", ab->debug_address);
-	return 0;
-}
-
-static int ab5500_address_open(struct inode *inode, struct file *file)
-{
-	return single_open(file, ab5500_address_print, inode->i_private);
-}
-
-static ssize_t ab5500_address_write(struct file *file,
-	const char __user *user_buf,
-	size_t count, loff_t *ppos)
-{
-	struct ab5500 *ab = ((struct seq_file *)(file->private_data))->private;
-	char buf[32];
-	int buf_size;
-	unsigned long user_address;
-	int err;
-
-	/* 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_address);
-	if (err)
-		return -EINVAL;
-	if (user_address > 0xff) {
-		dev_err(ab->dev,
-			"debugfs error input > 0xff\n");
-		return -EINVAL;
-	}
-	ab->debug_address = user_address;
-	return buf_size;
-}
-
-static int ab5500_val_print(struct seq_file *s, void *p)
-{
-	struct ab5500 *ab = s->private;
-	int err;
-	u8 regvalue;
-
-	err = ab5500_get_register_interruptible_raw(ab, (u8)ab->debug_bank,
-		(u8)ab->debug_address, &regvalue);
-	if (err) {
-		dev_err(ab->dev, "get_reg failed %d, bank 0x%x"
-			", reg 0x%x\n", err, ab->debug_bank,
-			ab->debug_address);
-		return -EINVAL;
-	}
-	seq_printf(s, "0x%02X\n", regvalue);
-
-	return 0;
-}
-
-static int ab5500_val_open(struct inode *inode, struct file *file)
-{
-	return single_open(file, ab5500_val_print, inode->i_private);
-}
-
-static ssize_t ab5500_val_write(struct file *file,
-	const char __user *user_buf,
-	size_t count, loff_t *ppos)
-{
-	struct ab5500 *ab = ((struct seq_file *)(file->private_data))->private;
-	char buf[32];
-	int buf_size;
-	unsigned long user_val;
-	int err;
-	u8 regvalue;
-
-	/* 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);
-	if (err)
-		return -EINVAL;
-	if (user_val > 0xff) {
-		dev_err(ab->dev,
-			"debugfs error input > 0xff\n");
-		return -EINVAL;
-	}
-	err = ab5500_mask_and_set_register_interruptible_raw(
-		ab, (u8)ab->debug_bank,
-		(u8)ab->debug_address, 0xFF, (u8)user_val);
-	if (err)
-		return -EINVAL;
-
-	ab5500_get_register_interruptible_raw(ab, (u8)ab->debug_bank,
-		(u8)ab->debug_address, &regvalue);
-	if (err)
-		return -EINVAL;
-
-	return buf_size;
-}
-
-static const struct file_operations ab5500_bank_fops = {
-	.open = ab5500_bank_open,
-	.write = ab5500_bank_write,
-	.read = seq_read,
-	.llseek = seq_lseek,
-	.release = single_release,
-	.owner = THIS_MODULE,
-};
-
-static const struct file_operations ab5500_address_fops = {
-	.open = ab5500_address_open,
-	.write = ab5500_address_write,
-	.read = seq_read,
-	.llseek = seq_lseek,
-	.release = single_release,
-	.owner = THIS_MODULE,
-};
-
-static const struct file_operations ab5500_val_fops = {
-	.open = ab5500_val_open,
-	.write = ab5500_val_write,
-	.read = seq_read,
-	.llseek = seq_lseek,
-	.release = single_release,
-	.owner = THIS_MODULE,
-};
-
-static struct dentry *ab5500_dir;
-static struct dentry *ab5500_reg_file;
-static struct dentry *ab5500_bank_file;
-static struct dentry *ab5500_address_file;
-static struct dentry *ab5500_val_file;
-
-void __init ab5500_setup_debugfs(struct ab5500 *ab)
-{
-	ab->debug_bank = AB5500_BANK_VIT_IO_I2C_CLK_TST_OTP;
-	ab->debug_address = AB5500_CHIP_ID;
-
-	ab5500_dir = debugfs_create_dir("ab5500", NULL);
-	if (!ab5500_dir)
-		goto exit_no_debugfs;
-
-	ab5500_reg_file = debugfs_create_file("all-bank-registers",
-		S_IRUGO, ab5500_dir, ab, &ab5500_registers_fops);
-	if (!ab5500_reg_file)
-		goto exit_destroy_dir;
-
-	ab5500_bank_file = debugfs_create_file("register-bank",
-		(S_IRUGO | S_IWUGO), ab5500_dir, ab, &ab5500_bank_fops);
-	if (!ab5500_bank_file)
-		goto exit_destroy_reg;
-
-	ab5500_address_file = debugfs_create_file("register-address",
-		(S_IRUGO | S_IWUGO), ab5500_dir, ab, &ab5500_address_fops);
-	if (!ab5500_address_file)
-		goto exit_destroy_bank;
-
-	ab5500_val_file = debugfs_create_file("register-value",
-		(S_IRUGO | S_IWUGO), ab5500_dir, ab, &ab5500_val_fops);
-	if (!ab5500_val_file)
-		goto exit_destroy_address;
-
-	return;
-
-exit_destroy_address:
-	debugfs_remove(ab5500_address_file);
-exit_destroy_bank:
-	debugfs_remove(ab5500_bank_file);
-exit_destroy_reg:
-	debugfs_remove(ab5500_reg_file);
-exit_destroy_dir:
-	debugfs_remove(ab5500_dir);
-exit_no_debugfs:
-	dev_err(ab->dev, "failed to create debugfs entries.\n");
-	return;
-}
-
-void __exit ab5500_remove_debugfs(void)
-{
-	debugfs_remove(ab5500_val_file);
-	debugfs_remove(ab5500_address_file);
-	debugfs_remove(ab5500_bank_file);
-	debugfs_remove(ab5500_reg_file);
-	debugfs_remove(ab5500_dir);
-}
diff --git a/drivers/mfd/ab5500-debugfs.h b/drivers/mfd/ab5500-debugfs.h
deleted file mode 100644
index 7330a9b6afa..00000000000
--- a/drivers/mfd/ab5500-debugfs.h
+++ /dev/null
@@ -1,22 +0,0 @@
-/*
- * Copyright (C) 2011 ST-Ericsson
- * License terms: GNU General Public License (GPL) version 2
- * Debugfs interface to the AB5500 core driver
- */
-
-#ifdef CONFIG_DEBUG_FS
-
-void ab5500_setup_debugfs(struct ab5500 *ab);
-void ab5500_remove_debugfs(void);
-
-#else /* !CONFIG_DEBUG_FS */
-
-static inline void ab5500_setup_debugfs(struct ab5500 *ab)
-{
-}
-
-static inline void ab5500_remove_debugfs(void)
-{
-}
-
-#endif
diff --git a/drivers/mfd/ab8500-core.c b/drivers/mfd/ab8500-core.c
index 1f08704f7ae..dac0e299860 100644
--- a/drivers/mfd/ab8500-core.c
+++ b/drivers/mfd/ab8500-core.c
@@ -18,7 +18,10 @@
 #include <linux/mfd/core.h>
 #include <linux/mfd/abx500.h>
 #include <linux/mfd/abx500/ab8500.h>
+#include <linux/mfd/dbx500-prcmu.h>
 #include <linux/regulator/ab8500.h>
+#include <linux/of.h>
+#include <linux/of_device.h>
 
 /*
  * Interrupt register offsets
@@ -91,12 +94,24 @@
 #define AB8500_IT_MASK23_REG		0x56
 #define AB8500_IT_MASK24_REG		0x57
 
+/*
+ * latch hierarchy registers
+ */
+#define AB8500_IT_LATCHHIER1_REG	0x60
+#define AB8500_IT_LATCHHIER2_REG	0x61
+#define AB8500_IT_LATCHHIER3_REG	0x62
+
+#define AB8500_IT_LATCHHIER_NUM		3
+
 #define AB8500_REV_REG			0x80
 #define AB8500_IC_NAME_REG		0x82
 #define AB8500_SWITCH_OFF_STATUS	0x00
 
 #define AB8500_TURN_ON_STATUS		0x00
 
+static bool no_bm; /* No battery management */
+module_param(no_bm, bool, S_IRUGO);
+
 #define AB9540_MODEM_CTRL2_REG			0x23
 #define AB9540_MODEM_CTRL2_SWDBBRSTN_BIT	BIT(2)
 
@@ -125,6 +140,41 @@ static const char ab8500_version_str[][7] = {
 	[AB8500_VERSION_AB8540] = "AB8540",
 };
 
+static int ab8500_i2c_write(struct ab8500 *ab8500, u16 addr, u8 data)
+{
+	int ret;
+
+	ret = prcmu_abb_write((u8)(addr >> 8), (u8)(addr & 0xFF), &data, 1);
+	if (ret < 0)
+		dev_err(ab8500->dev, "prcmu i2c error %d\n", ret);
+	return ret;
+}
+
+static int ab8500_i2c_write_masked(struct ab8500 *ab8500, u16 addr, u8 mask,
+	u8 data)
+{
+	int ret;
+
+	ret = prcmu_abb_write_masked((u8)(addr >> 8), (u8)(addr & 0xFF), &data,
+		&mask, 1);
+	if (ret < 0)
+		dev_err(ab8500->dev, "prcmu i2c error %d\n", ret);
+	return ret;
+}
+
+static int ab8500_i2c_read(struct ab8500 *ab8500, u16 addr)
+{
+	int ret;
+	u8 data;
+
+	ret = prcmu_abb_read((u8)(addr >> 8), (u8)(addr & 0xFF), &data, 1);
+	if (ret < 0) {
+		dev_err(ab8500->dev, "prcmu i2c error %d\n", ret);
+		return ret;
+	}
+	return (int)data;
+}
+
 static int ab8500_get_chip_id(struct device *dev)
 {
 	struct ab8500 *ab8500;
@@ -161,9 +211,13 @@ static int set_register_interruptible(struct ab8500 *ab8500, u8 bank,
 static int ab8500_set_register(struct device *dev, u8 bank,
 	u8 reg, u8 value)
 {
+	int ret;
 	struct ab8500 *ab8500 = dev_get_drvdata(dev->parent);
 
-	return set_register_interruptible(ab8500, bank, reg, value);
+	atomic_inc(&ab8500->transfer_ongoing);
+	ret = set_register_interruptible(ab8500, bank, reg, value);
+	atomic_dec(&ab8500->transfer_ongoing);
+	return ret;
 }
 
 static int get_register_interruptible(struct ab8500 *ab8500, u8 bank,
@@ -192,9 +246,13 @@ static int get_register_interruptible(struct ab8500 *ab8500, u8 bank,
 static int ab8500_get_register(struct device *dev, u8 bank,
 	u8 reg, u8 *value)
 {
+	int ret;
 	struct ab8500 *ab8500 = dev_get_drvdata(dev->parent);
 
-	return get_register_interruptible(ab8500, bank, reg, value);
+	atomic_inc(&ab8500->transfer_ongoing);
+	ret = get_register_interruptible(ab8500, bank, reg, value);
+	atomic_dec(&ab8500->transfer_ongoing);
+	return ret;
 }
 
 static int mask_and_set_register_interruptible(struct ab8500 *ab8500, u8 bank,
@@ -241,11 +299,14 @@ out:
 static int ab8500_mask_and_set_register(struct device *dev,
 	u8 bank, u8 reg, u8 bitmask, u8 bitvalues)
 {
+	int ret;
 	struct ab8500 *ab8500 = dev_get_drvdata(dev->parent);
 
-	return mask_and_set_register_interruptible(ab8500, bank, reg,
-		bitmask, bitvalues);
-
+	atomic_inc(&ab8500->transfer_ongoing);
+	ret= mask_and_set_register_interruptible(ab8500, bank, reg,
+						 bitmask, bitvalues);
+	atomic_dec(&ab8500->transfer_ongoing);
+	return ret;
 }
 
 static struct abx500_ops ab8500_ops = {
@@ -264,6 +325,7 @@ static void ab8500_irq_lock(struct irq_data *data)
 	struct ab8500 *ab8500 = irq_data_get_irq_chip_data(data);
 
 	mutex_lock(&ab8500->irq_lock);
+	atomic_inc(&ab8500->transfer_ongoing);
 }
 
 static void ab8500_irq_sync_unlock(struct irq_data *data)
@@ -292,7 +354,7 @@ static void ab8500_irq_sync_unlock(struct irq_data *data)
 		reg = AB8500_IT_MASK1_REG + ab8500->irq_reg_offset[i];
 		set_register_interruptible(ab8500, AB8500_INTERRUPT, reg, new);
 	}
-
+	atomic_dec(&ab8500->transfer_ongoing);
 	mutex_unlock(&ab8500->irq_lock);
 }
 
@@ -325,6 +387,90 @@ static struct irq_chip ab8500_irq_chip = {
 	.irq_unmask		= ab8500_irq_unmask,
 };
 
+static int ab8500_handle_hierarchical_line(struct ab8500 *ab8500,
+					int latch_offset, u8 latch_val)
+{
+	int int_bit = __ffs(latch_val);
+	int 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;
+
+		if (i >= ab8500->mask_size) {
+			dev_err(ab8500->dev, "Register offset 0x%2x not declared\n",
+					latch_offset);
+			return -ENXIO;
+		}
+
+		line = (i << 3) + int_bit;
+		latch_val &= ~(1 << int_bit);
+
+		handle_nested_irq(ab8500->irq_base + line);
+	} while (latch_val);
+
+	return 0;
+}
+
+static int ab8500_handle_hierarchical_latch(struct ab8500 *ab8500,
+					int hier_offset, u8 hier_val)
+{
+	int latch_bit, status;
+	u8 latch_offset, latch_val;
+
+	do {
+		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;
+
+		status = get_register_interruptible(ab8500,
+				AB8500_INTERRUPT,
+				AB8500_IT_LATCH1_REG + latch_offset,
+				&latch_val);
+		if (status < 0 || latch_val == 0)
+			goto discard;
+
+		status = ab8500_handle_hierarchical_line(ab8500,
+				latch_offset, latch_val);
+		if (status < 0)
+			return status;
+discard:
+		hier_val &= ~(1 << latch_bit);
+	} while (hier_val);
+
+	return 0;
+}
+
+static irqreturn_t ab8500_hierarchical_irq(int irq, void *dev)
+{
+	struct ab8500 *ab8500 = dev;
+	u8 i;
+
+	dev_vdbg(ab8500->dev, "interrupt\n");
+
+	/*  Hierarchical interrupt version */
+	for (i = 0; i < AB8500_IT_LATCHHIER_NUM; i++) {
+		int status;
+		u8 hier_val;
+
+		status = get_register_interruptible(ab8500, AB8500_INTERRUPT,
+			AB8500_IT_LATCHHIER1_REG + i, &hier_val);
+		if (status < 0 || hier_val == 0)
+			continue;
+
+		status = ab8500_handle_hierarchical_latch(ab8500, i, hier_val);
+		if (status < 0)
+			break;
+	}
+	return IRQ_HANDLED;
+}
+
 static irqreturn_t ab8500_irq(int irq, void *dev)
 {
 	struct ab8500 *ab8500 = dev;
@@ -332,6 +478,8 @@ static irqreturn_t ab8500_irq(int irq, void *dev)
 
 	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;
@@ -355,9 +503,10 @@ static irqreturn_t ab8500_irq(int irq, void *dev)
 
 			handle_nested_irq(ab8500->irq_base + line);
 			value &= ~(1 << bit);
+
 		} while (value);
 	}
-
+	atomic_dec(&ab8500->transfer_ongoing);
 	return IRQ_HANDLED;
 }
 
@@ -411,6 +560,14 @@ static void ab8500_irq_remove(struct ab8500 *ab8500)
 	}
 }
 
+int ab8500_suspend(struct ab8500 *ab8500)
+{
+	if (atomic_read(&ab8500->transfer_ongoing))
+		return -EINVAL;
+	else
+		return 0;
+}
+
 /* AB8500 GPIO Resources */
 static struct resource __devinitdata ab8500_gpio_resources[] = {
 	{
@@ -744,6 +901,39 @@ static struct resource __devinitdata ab8500_usb_resources[] = {
 	},
 };
 
+static struct resource __devinitdata ab8505_iddet_resources[] = {
+	{
+		.name  = "KeyDeglitch",
+		.start = AB8505_INT_KEYDEGLITCH,
+		.end   = AB8505_INT_KEYDEGLITCH,
+		.flags = IORESOURCE_IRQ,
+	},
+	{
+		.name  = "KP",
+		.start = AB8505_INT_KP,
+		.end   = AB8505_INT_KP,
+		.flags = IORESOURCE_IRQ,
+	},
+	{
+		.name  = "IKP",
+		.start = AB8505_INT_IKP,
+		.end   = AB8505_INT_IKP,
+		.flags = IORESOURCE_IRQ,
+	},
+	{
+		.name  = "IKR",
+		.start = AB8505_INT_IKR,
+		.end   = AB8505_INT_IKR,
+		.flags = IORESOURCE_IRQ,
+	},
+	{
+		.name  = "KeyStuck",
+		.start = AB8505_INT_KEYSTUCK,
+		.end   = AB8505_INT_KEYSTUCK,
+		.flags = IORESOURCE_IRQ,
+	},
+};
+
 static struct resource __devinitdata ab8500_temp_resources[] = {
 	{
 		.name  = "AB8500_TEMP_WARM",
@@ -778,35 +968,11 @@ static struct mfd_cell __devinitdata abx500_common_devs[] = {
 		.resources = ab8500_rtc_resources,
 	},
 	{
-		.name = "ab8500-charger",
-		.num_resources = ARRAY_SIZE(ab8500_charger_resources),
-		.resources = ab8500_charger_resources,
-	},
-	{
-		.name = "ab8500-btemp",
-		.num_resources = ARRAY_SIZE(ab8500_btemp_resources),
-		.resources = ab8500_btemp_resources,
-	},
-	{
-		.name = "ab8500-fg",
-		.num_resources = ARRAY_SIZE(ab8500_fg_resources),
-		.resources = ab8500_fg_resources,
-	},
-	{
-		.name = "ab8500-chargalg",
-		.num_resources = ARRAY_SIZE(ab8500_chargalg_resources),
-		.resources = ab8500_chargalg_resources,
-	},
-	{
 		.name = "ab8500-acc-det",
 		.num_resources = ARRAY_SIZE(ab8500_av_acc_detect_resources),
 		.resources = ab8500_av_acc_detect_resources,
 	},
 	{
-		.name = "ab8500-codec",
-	},
-
-	{
 		.name = "ab8500-poweron-key",
 		.num_resources = ARRAY_SIZE(ab8500_poweronkey_db_resources),
 		.resources = ab8500_poweronkey_db_resources,
@@ -834,6 +1000,29 @@ static struct mfd_cell __devinitdata abx500_common_devs[] = {
 	},
 };
 
+static struct mfd_cell __devinitdata ab8500_bm_devs[] = {
+	{
+		.name = "ab8500-charger",
+		.num_resources = ARRAY_SIZE(ab8500_charger_resources),
+		.resources = ab8500_charger_resources,
+	},
+	{
+		.name = "ab8500-btemp",
+		.num_resources = ARRAY_SIZE(ab8500_btemp_resources),
+		.resources = ab8500_btemp_resources,
+	},
+	{
+		.name = "ab8500-fg",
+		.num_resources = ARRAY_SIZE(ab8500_fg_resources),
+		.resources = ab8500_fg_resources,
+	},
+	{
+		.name = "ab8500-chargalg",
+		.num_resources = ARRAY_SIZE(ab8500_chargalg_resources),
+		.resources = ab8500_chargalg_resources,
+	},
+};
+
 static struct mfd_cell __devinitdata ab8500_devs[] = {
 	{
 		.name = "ab8500-gpio",
@@ -845,6 +1034,9 @@ static struct mfd_cell __devinitdata ab8500_devs[] = {
 		.num_resources = ARRAY_SIZE(ab8500_usb_resources),
 		.resources = ab8500_usb_resources,
 	},
+	{
+		.name = "ab8500-codec",
+	},
 };
 
 static struct mfd_cell __devinitdata ab9540_devs[] = {
@@ -858,6 +1050,18 @@ static struct mfd_cell __devinitdata ab9540_devs[] = {
 		.num_resources = ARRAY_SIZE(ab8500_usb_resources),
 		.resources = ab8500_usb_resources,
 	},
+	{
+		.name = "ab9540-codec",
+	},
+};
+
+/* Device list common to ab9540 and ab8505 */
+static struct mfd_cell __devinitdata ab9540_ab8505_devs[] = {
+	{
+		.name = "ab-iddet",
+		.num_resources = ARRAY_SIZE(ab8505_iddet_resources),
+		.resources = ab8505_iddet_resources,
+	},
 };
 
 static ssize_t show_chip_id(struct device *dev,
@@ -1003,18 +1207,66 @@ static struct attribute_group ab9540_attr_group = {
 	.attrs	= ab9540_sysfs_entries,
 };
 
-int __devinit ab8500_init(struct ab8500 *ab8500, enum ab8500_version version)
+static const struct of_device_id ab8500_match[] = {
+	{
+		.compatible = "stericsson,ab8500",
+		.data = (void *)AB8500_VERSION_AB8500,
+	},
+	{},
+};
+
+static int __devinit ab8500_probe(struct platform_device *pdev)
 {
-	struct ab8500_platform_data *plat = dev_get_platdata(ab8500->dev);
+	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;
+	struct device_node *np = pdev->dev.of_node;
+	struct ab8500 *ab8500;
+	struct resource *resource;
 	int ret;
 	int i;
 	u8 value;
 
+	ab8500 = kzalloc(sizeof *ab8500, GFP_KERNEL);
+	if (!ab8500)
+		return -ENOMEM;
+
 	if (plat)
 		ab8500->irq_base = plat->irq_base;
+	else if (np)
+		ret = of_property_read_u32(np, "stericsson,irq-base", &ab8500->irq_base);
+
+	if (!ab8500->irq_base) {
+		dev_info(&pdev->dev, "couldn't find irq-base\n");
+		ret = -EINVAL;
+		goto out_free_ab8500;
+	}
+
+	ab8500->dev = &pdev->dev;
+
+	resource = platform_get_resource(pdev, IORESOURCE_IRQ, 0);
+	if (!resource) {
+		ret = -ENODEV;
+		goto out_free_ab8500;
+	}
+
+	ab8500->irq = resource->start;
+
+	ab8500->read = ab8500_i2c_read;
+	ab8500->write = ab8500_i2c_write;
+	ab8500->write_masked = ab8500_i2c_write_masked;
 
 	mutex_init(&ab8500->lock);
 	mutex_init(&ab8500->irq_lock);
+	atomic_set(&ab8500->transfer_ongoing, 0);
+
+	platform_set_drvdata(pdev, ab8500);
+
+	if (platid)
+		version = platid->driver_data;
+	else if (np)
+		version = (unsigned int)
+			of_match_device(ab8500_match, &pdev->dev)->data;
 
 	if (version != AB8500_VERSION_UNDEFINED)
 		ab8500->version = version;
@@ -1022,7 +1274,7 @@ int __devinit ab8500_init(struct ab8500 *ab8500, enum ab8500_version version)
 		ret = get_register_interruptible(ab8500, AB8500_MISC,
 			AB8500_IC_NAME_REG, &value);
 		if (ret < 0)
-			return ret;
+			goto out_free_ab8500;
 
 		ab8500->version = value;
 	}
@@ -1030,7 +1282,7 @@ int __devinit ab8500_init(struct ab8500 *ab8500, enum ab8500_version version)
 	ret = get_register_interruptible(ab8500, AB8500_MISC,
 		AB8500_REV_REG, &value);
 	if (ret < 0)
-		return ret;
+		goto out_free_ab8500;
 
 	ab8500->chip_id = value;
 
@@ -1105,30 +1357,57 @@ int __devinit ab8500_init(struct ab8500 *ab8500, enum ab8500_version version)
 		if (ret)
 			goto out_freeoldmask;
 
-		ret = request_threaded_irq(ab8500->irq, NULL, ab8500_irq,
-					   IRQF_ONESHOT | IRQF_NO_SUSPEND,
-					   "ab8500", ab8500);
+		/*  Activate this feature only in ab9540 */
+		/*  till tests are done on ab8500 1p2 or later*/
+		if (is_ab9540(ab8500))
+			ret = request_threaded_irq(ab8500->irq, NULL,
+					ab8500_hierarchical_irq,
+					IRQF_ONESHOT | IRQF_NO_SUSPEND,
+					"ab8500", ab8500);
+		else
+			ret = request_threaded_irq(ab8500->irq, NULL,
+					ab8500_irq,
+					IRQF_ONESHOT | IRQF_NO_SUSPEND,
+					"ab8500", ab8500);
 		if (ret)
 			goto out_removeirq;
 	}
 
-	ret = mfd_add_devices(ab8500->dev, 0, abx500_common_devs,
-			      ARRAY_SIZE(abx500_common_devs), NULL,
-			      ab8500->irq_base);
+	if (!np) {
+		ret = mfd_add_devices(ab8500->dev, 0, abx500_common_devs,
+				ARRAY_SIZE(abx500_common_devs), NULL,
+				ab8500->irq_base);
 
-	if (ret)
-		goto out_freeirq;
+		if (ret)
+			goto out_freeirq;
+
+		if (is_ab9540(ab8500))
+			ret = mfd_add_devices(ab8500->dev, 0, ab9540_devs,
+					ARRAY_SIZE(ab9540_devs), NULL,
+					ab8500->irq_base);
+		else
+			ret = mfd_add_devices(ab8500->dev, 0, ab8500_devs,
+					ARRAY_SIZE(ab8500_devs), NULL,
+					ab8500->irq_base);
+		if (ret)
+			goto out_freeirq;
 
-	if (is_ab9540(ab8500))
-		ret = mfd_add_devices(ab8500->dev, 0, ab9540_devs,
-			      ARRAY_SIZE(ab9540_devs), NULL,
-			      ab8500->irq_base);
-	else
-		ret = mfd_add_devices(ab8500->dev, 0, ab8500_devs,
-			      ARRAY_SIZE(ab9540_devs), NULL,
-			      ab8500->irq_base);
-	if (ret)
-		goto out_freeirq;
+		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);
+		if (ret)
+			goto out_freeirq;
+	}
+
+	if (!no_bm) {
+		/* Add battery management devices */
+		ret = mfd_add_devices(ab8500->dev, 0, ab8500_bm_devs,
+				      ARRAY_SIZE(ab8500_bm_devs), NULL,
+				      ab8500->irq_base);
+		if (ret)
+			dev_err(ab8500->dev, "error adding bm devices\n");
+	}
 
 	if (is_ab9540(ab8500))
 		ret = sysfs_create_group(&ab8500->dev->kobj,
@@ -1151,12 +1430,16 @@ out_freeoldmask:
 	kfree(ab8500->oldmask);
 out_freemask:
 	kfree(ab8500->mask);
+out_free_ab8500:
+	kfree(ab8500);
 
 	return ret;
 }
 
-int __devexit ab8500_exit(struct ab8500 *ab8500)
+static int __devexit ab8500_remove(struct platform_device *pdev)
 {
+	struct ab8500 *ab8500 = platform_get_drvdata(pdev);
+
 	if (is_ab9540(ab8500))
 		sysfs_remove_group(&ab8500->dev->kobj, &ab9540_attr_group);
 	else
@@ -1168,10 +1451,42 @@ int __devexit ab8500_exit(struct ab8500 *ab8500)
 	}
 	kfree(ab8500->oldmask);
 	kfree(ab8500->mask);
+	kfree(ab8500);
 
 	return 0;
 }
 
+static const struct platform_device_id ab8500_id[] = {
+	{ "ab8500-core", AB8500_VERSION_AB8500 },
+	{ "ab8505-i2c", AB8500_VERSION_AB8505 },
+	{ "ab9540-i2c", AB8500_VERSION_AB9540 },
+	{ "ab8540-i2c", AB8500_VERSION_AB8540 },
+	{ }
+};
+
+static struct platform_driver ab8500_core_driver = {
+	.driver = {
+		.name = "ab8500-core",
+		.owner = THIS_MODULE,
+		.of_match_table = ab8500_match,
+	},
+	.probe	= ab8500_probe,
+	.remove	= __devexit_p(ab8500_remove),
+	.id_table = ab8500_id,
+};
+
+static int __init ab8500_core_init(void)
+{
+	return platform_driver_register(&ab8500_core_driver);
+}
+
+static void __exit ab8500_core_exit(void)
+{
+	platform_driver_unregister(&ab8500_core_driver);
+}
+arch_initcall(ab8500_core_init);
+module_exit(ab8500_core_exit);
+
 MODULE_AUTHOR("Mattias Wallin, Srinidhi Kasagar, Rabin Vincent");
 MODULE_DESCRIPTION("AB8500 MFD core");
 MODULE_LICENSE("GPL v2");
diff --git a/drivers/mfd/ab8500-debugfs.c b/drivers/mfd/ab8500-debugfs.c
index 9a0211aa889..50c4c89ab22 100644
--- a/drivers/mfd/ab8500-debugfs.c
+++ b/drivers/mfd/ab8500-debugfs.c
@@ -608,10 +608,16 @@ static int __devexit ab8500_debug_remove(struct platform_device *plf)
 	return 0;
 }
 
+static const struct of_device_id ab8500_debug_match[] = {
+        { .compatible = "stericsson,ab8500-debug", },
+        {}
+};
+
 static struct platform_driver ab8500_debug_driver = {
 	.driver = {
 		.name = "ab8500-debug",
 		.owner = THIS_MODULE,
+		.of_match_table = ab8500_debug_match,
 	},
 	.probe  = ab8500_debug_probe,
 	.remove = __devexit_p(ab8500_debug_remove)
diff --git a/drivers/mfd/ab8500-gpadc.c b/drivers/mfd/ab8500-gpadc.c
index c39fc716e1d..b86fd8e1ec3 100644
--- a/drivers/mfd/ab8500-gpadc.c
+++ b/drivers/mfd/ab8500-gpadc.c
@@ -584,7 +584,7 @@ static int __devinit ab8500_gpadc_probe(struct platform_device *pdev)
 
 	gpadc->irq = platform_get_irq_byname(pdev, "SW_CONV_END");
 	if (gpadc->irq < 0) {
-		dev_err(gpadc->dev, "failed to get platform irq-%d\n",
+		dev_err(&pdev->dev, "failed to get platform irq-%d\n",
 			gpadc->irq);
 		ret = gpadc->irq;
 		goto fail;
@@ -648,12 +648,18 @@ static int __devexit ab8500_gpadc_remove(struct platform_device *pdev)
 	return 0;
 }
 
+static const struct of_device_id ab8500_gpadc_match[] = {
+	{ .compatible = "stericsson,ab8500-gpadc", },
+	{}
+};
+
 static struct platform_driver ab8500_gpadc_driver = {
 	.probe = ab8500_gpadc_probe,
 	.remove = __devexit_p(ab8500_gpadc_remove),
 	.driver = {
 		.name = "ab8500-gpadc",
 		.owner = THIS_MODULE,
+		.of_match_table = ab8500_gpadc_match,
 	},
 };
 
diff --git a/drivers/mfd/ab8500-i2c.c b/drivers/mfd/ab8500-i2c.c
deleted file mode 100644
index b83045f102b..00000000000
--- a/drivers/mfd/ab8500-i2c.c
+++ /dev/null
@@ -1,128 +0,0 @@
-/*
- * Copyright (C) ST-Ericsson SA 2010
- * Author: Mattias Wallin <mattias.wallin@stericsson.com> for ST-Ericsson.
- * License Terms: GNU General Public License v2
- * This file was based on drivers/mfd/ab8500-spi.c
- */
-
-#include <linux/kernel.h>
-#include <linux/slab.h>
-#include <linux/init.h>
-#include <linux/module.h>
-#include <linux/platform_device.h>
-#include <linux/mfd/abx500/ab8500.h>
-#include <linux/mfd/dbx500-prcmu.h>
-
-static int ab8500_i2c_write(struct ab8500 *ab8500, u16 addr, u8 data)
-{
-	int ret;
-
-	ret = prcmu_abb_write((u8)(addr >> 8), (u8)(addr & 0xFF), &data, 1);
-	if (ret < 0)
-		dev_err(ab8500->dev, "prcmu i2c error %d\n", ret);
-	return ret;
-}
-
-static int ab8500_i2c_write_masked(struct ab8500 *ab8500, u16 addr, u8 mask,
-	u8 data)
-{
-	int ret;
-
-	ret = prcmu_abb_write_masked((u8)(addr >> 8), (u8)(addr & 0xFF), &data,
-		&mask, 1);
-	if (ret < 0)
-		dev_err(ab8500->dev, "prcmu i2c error %d\n", ret);
-	return ret;
-}
-
-static int ab8500_i2c_read(struct ab8500 *ab8500, u16 addr)
-{
-	int ret;
-	u8 data;
-
-	ret = prcmu_abb_read((u8)(addr >> 8), (u8)(addr & 0xFF), &data, 1);
-	if (ret < 0) {
-		dev_err(ab8500->dev, "prcmu i2c error %d\n", ret);
-		return ret;
-	}
-	return (int)data;
-}
-
-static int __devinit ab8500_i2c_probe(struct platform_device *plf)
-{
-	const struct platform_device_id *platid = platform_get_device_id(plf);
-	struct ab8500 *ab8500;
-	struct resource *resource;
-	int ret;
-
-	ab8500 = kzalloc(sizeof *ab8500, GFP_KERNEL);
-	if (!ab8500)
-		return -ENOMEM;
-
-	ab8500->dev = &plf->dev;
-
-	resource = platform_get_resource(plf, IORESOURCE_IRQ, 0);
-	if (!resource) {
-		kfree(ab8500);
-		return -ENODEV;
-	}
-
-	ab8500->irq = resource->start;
-
-	ab8500->read = ab8500_i2c_read;
-	ab8500->write = ab8500_i2c_write;
-	ab8500->write_masked = ab8500_i2c_write_masked;
-
-	platform_set_drvdata(plf, ab8500);
-
-	ret = ab8500_init(ab8500, platid->driver_data);
-	if (ret)
-		kfree(ab8500);
-
-
-	return ret;
-}
-
-static int __devexit ab8500_i2c_remove(struct platform_device *plf)
-{
-	struct ab8500 *ab8500 = platform_get_drvdata(plf);
-
-	ab8500_exit(ab8500);
-	kfree(ab8500);
-
-	return 0;
-}
-
-static const struct platform_device_id ab8500_id[] = {
-	{ "ab8500-i2c", AB8500_VERSION_AB8500 },
-	{ "ab8505-i2c", AB8500_VERSION_AB8505 },
-	{ "ab9540-i2c", AB8500_VERSION_AB9540 },
-	{ "ab8540-i2c", AB8500_VERSION_AB8540 },
-	{ }
-};
-
-static struct platform_driver ab8500_i2c_driver = {
-	.driver = {
-		.name = "ab8500-i2c",
-		.owner = THIS_MODULE,
-	},
-	.probe	= ab8500_i2c_probe,
-	.remove	= __devexit_p(ab8500_i2c_remove),
-	.id_table = ab8500_id,
-};
-
-static int __init ab8500_i2c_init(void)
-{
-	return platform_driver_register(&ab8500_i2c_driver);
-}
-
-static void __exit ab8500_i2c_exit(void)
-{
-	platform_driver_unregister(&ab8500_i2c_driver);
-}
-arch_initcall(ab8500_i2c_init);
-module_exit(ab8500_i2c_exit);
-
-MODULE_AUTHOR("Mattias WALLIN <mattias.wallin@stericsson.com");
-MODULE_DESCRIPTION("AB8500 Core access via PRCMU I2C");
-MODULE_LICENSE("GPL v2");
diff --git a/drivers/mfd/ab8500-sysctrl.c b/drivers/mfd/ab8500-sysctrl.c
index c28d4eb1eff..5a3e51ccf25 100644
--- a/drivers/mfd/ab8500-sysctrl.c
+++ b/drivers/mfd/ab8500-sysctrl.c
@@ -61,10 +61,16 @@ static int __devexit ab8500_sysctrl_remove(struct platform_device *pdev)
 	return 0;
 }
 
+static const struct of_device_id ab8500_sysctrl_match[] = {
+	{ .compatible = "stericsson,ab8500-sysctrl", },
+	{}
+};
+
 static struct platform_driver ab8500_sysctrl_driver = {
 	.driver = {
 		.name = "ab8500-sysctrl",
 		.owner = THIS_MODULE,
+		.of_match_table = ab8500_sysctrl_match,
 	},
 	.probe = ab8500_sysctrl_probe,
 	.remove = __devexit_p(ab8500_sysctrl_remove),
diff --git a/drivers/mfd/anatop-mfd.c b/drivers/mfd/anatop-mfd.c
index 2af42480635..6da06341f6c 100644
--- a/drivers/mfd/anatop-mfd.c
+++ b/drivers/mfd/anatop-mfd.c
@@ -41,39 +41,26 @@
 #include <linux/of_address.h>
 #include <linux/mfd/anatop.h>
 
-u32 anatop_get_bits(struct anatop *adata, u32 addr, int bit_shift,
-		    int bit_width)
+u32 anatop_read_reg(struct anatop *adata, u32 addr)
 {
-	u32 val, mask;
-
-	if (bit_width == 32)
-		mask = ~0;
-	else
-		mask = (1 << bit_width) - 1;
-
-	val = readl(adata->ioreg + addr);
-	val = (val >> bit_shift) & mask;
-
-	return val;
+	return readl(adata->ioreg + addr);
 }
-EXPORT_SYMBOL_GPL(anatop_get_bits);
+EXPORT_SYMBOL_GPL(anatop_read_reg);
 
-void anatop_set_bits(struct anatop *adata, u32 addr, int bit_shift,
-		     int bit_width, u32 data)
+void anatop_write_reg(struct anatop *adata, u32 addr, u32 data, u32 mask)
 {
-	u32 val, mask;
+	u32 val;
 
-	if (bit_width == 32)
-		mask = ~0;
-	else
-		mask = (1 << bit_width) - 1;
+	data &= mask;
 
 	spin_lock(&adata->reglock);
-	val = readl(adata->ioreg + addr) & ~(mask << bit_shift);
-	writel((data << bit_shift) | val, adata->ioreg + addr);
+	val = readl(adata->ioreg + addr);
+	val &= ~mask;
+	val |= data;
+	writel(val, adata->ioreg + addr);
 	spin_unlock(&adata->reglock);
 }
-EXPORT_SYMBOL_GPL(anatop_set_bits);
+EXPORT_SYMBOL_GPL(anatop_write_reg);
 
 static const struct of_device_id of_anatop_match[] = {
 	{ .compatible = "fsl,imx6q-anatop", },
diff --git a/drivers/mfd/asic3.c b/drivers/mfd/asic3.c
index 1582c3d9525..383421bf576 100644
--- a/drivers/mfd/asic3.c
+++ b/drivers/mfd/asic3.c
@@ -353,12 +353,28 @@ static int asic3_gpio_irq_type(struct irq_data *data, unsigned int type)
 	return 0;
 }
 
+static int asic3_gpio_irq_set_wake(struct irq_data *data, unsigned int on)
+{
+	struct asic3 *asic = irq_data_get_irq_chip_data(data);
+	u32 bank, index;
+	u16 bit;
+
+	bank = asic3_irq_to_bank(asic, data->irq);
+	index = asic3_irq_to_index(asic, data->irq);
+	bit = 1<<index;
+
+	asic3_set_register(asic, bank + ASIC3_GPIO_SLEEP_MASK, bit, !on);
+
+	return 0;
+}
+
 static struct irq_chip asic3_gpio_irq_chip = {
 	.name		= "ASIC3-GPIO",
 	.irq_ack	= asic3_mask_gpio_irq,
 	.irq_mask	= asic3_mask_gpio_irq,
 	.irq_unmask	= asic3_unmask_gpio_irq,
 	.irq_set_type	= asic3_gpio_irq_type,
+	.irq_set_wake	= asic3_gpio_irq_set_wake,
 };
 
 static struct irq_chip asic3_irq_chip = {
@@ -529,7 +545,7 @@ static int asic3_gpio_to_irq(struct gpio_chip *chip, unsigned offset)
 {
 	struct asic3 *asic = container_of(chip, struct asic3, gpio);
 
-	return (offset < ASIC3_NUM_GPIOS) ? asic->irq_base + offset : -ENXIO;
+	return asic->irq_base + offset;
 }
 
 static __init int asic3_gpio_probe(struct platform_device *pdev,
@@ -894,10 +910,13 @@ static int __init asic3_mfd_probe(struct platform_device *pdev,
 	asic3_mmc_resources[0].start >>= asic->bus_shift;
 	asic3_mmc_resources[0].end   >>= asic->bus_shift;
 
-	ret = mfd_add_devices(&pdev->dev, pdev->id,
+	if (pdata->clock_rate) {
+		ds1wm_pdata.clock_rate = pdata->clock_rate;
+		ret = mfd_add_devices(&pdev->dev, pdev->id,
 			&asic3_cell_ds1wm, 1, mem, asic->irq_base);
-	if (ret < 0)
-		goto out;
+		if (ret < 0)
+			goto out;
+	}
 
 	if (mem_sdio && (irq >= 0)) {
 		ret = mfd_add_devices(&pdev->dev, pdev->id,
@@ -1000,6 +1019,9 @@ static int __init asic3_probe(struct platform_device *pdev)
 
 	asic3_mfd_probe(pdev, pdata, mem);
 
+	asic3_set_register(asic, ASIC3_OFFSET(EXTCF, SELECT),
+		(ASIC3_EXTCF_CF0_BUF_EN|ASIC3_EXTCF_CF0_PWAIT_EN), 1);
+
 	dev_info(asic->dev, "ASIC3 Core driver\n");
 
 	return 0;
@@ -1021,6 +1043,9 @@ static int __devexit asic3_remove(struct platform_device *pdev)
 	int ret;
 	struct asic3 *asic = platform_get_drvdata(pdev);
 
+	asic3_set_register(asic, ASIC3_OFFSET(EXTCF, SELECT),
+		(ASIC3_EXTCF_CF0_BUF_EN|ASIC3_EXTCF_CF0_PWAIT_EN), 0);
+
 	asic3_mfd_remove(pdev);
 
 	ret = asic3_gpio_remove(pdev);
diff --git a/drivers/mfd/cs5535-mfd.c b/drivers/mfd/cs5535-mfd.c
index 315fef5d466..3419e726de4 100644
--- a/drivers/mfd/cs5535-mfd.c
+++ b/drivers/mfd/cs5535-mfd.c
@@ -186,18 +186,7 @@ static struct pci_driver cs5535_mfd_driver = {
 	.remove = __devexit_p(cs5535_mfd_remove),
 };
 
-static int __init cs5535_mfd_init(void)
-{
-	return pci_register_driver(&cs5535_mfd_driver);
-}
-
-static void __exit cs5535_mfd_exit(void)
-{
-	pci_unregister_driver(&cs5535_mfd_driver);
-}
-
-module_init(cs5535_mfd_init);
-module_exit(cs5535_mfd_exit);
+module_pci_driver(cs5535_mfd_driver);
 
 MODULE_AUTHOR("Andres Salomon <dilinger@queued.net>");
 MODULE_DESCRIPTION("MFD driver for CS5535/CS5536 southbridge's ISA PCI device");
diff --git a/drivers/mfd/da9052-core.c b/drivers/mfd/da9052-core.c
index 7ff313fe9fb..1f1313c9057 100644
--- a/drivers/mfd/da9052-core.c
+++ b/drivers/mfd/da9052-core.c
@@ -318,6 +318,135 @@ static bool da9052_reg_volatile(struct device *dev, unsigned int reg)
 	}
 }
 
+/*
+ * TBAT look-up table is computed from the R90 reg (8 bit register)
+ * reading as below. The battery temperature is in milliCentigrade
+ * TBAT = (1/(t1+1/298) - 273) * 1000 mC
+ * where t1 = (1/B)* ln(( ADCval * 2.5)/(R25*ITBAT*255))
+ * Default values are R25 = 10e3, B = 3380, ITBAT = 50e-6
+ * Example:
+ * R25=10E3, B=3380, ITBAT=50e-6, ADCVAL=62d calculates
+ * TBAT = 20015 mili degrees Centrigrade
+ *
+*/
+static const int32_t tbat_lookup[255] = {
+	183258, 144221, 124334, 111336, 101826, 94397, 88343, 83257,
+	78889, 75071, 71688, 68656, 65914, 63414, 61120, 59001,
+	570366, 55204, 53490, 51881, 50364, 48931, 47574, 46285,
+	45059, 43889, 42772, 41703, 40678, 39694, 38748, 37838,
+	36961, 36115, 35297, 34507, 33743, 33002, 32284, 31588,
+	30911, 30254, 29615, 28994, 28389, 27799, 27225, 26664,
+	26117, 25584, 25062, 24553, 24054, 23567, 23091, 22624,
+	22167, 21719, 21281, 20851, 20429, 20015, 19610, 19211,
+	18820, 18436, 18058, 17688, 17323, 16965, 16612, 16266,
+	15925, 15589, 15259, 14933, 14613, 14298, 13987, 13681,
+	13379, 13082, 12788, 12499, 12214, 11933, 11655, 11382,
+	11112, 10845, 10582, 10322, 10066, 9812, 9562, 9315,
+	9071, 8830, 8591, 8356, 8123, 7893, 7665, 7440,
+	7218, 6998, 6780, 6565, 6352, 6141, 5933, 5726,
+	5522, 5320, 5120, 4922, 4726, 4532, 4340, 4149,
+	3961, 3774, 3589, 3406, 3225, 3045, 2867, 2690,
+	2516, 2342, 2170, 2000, 1831, 1664, 1498, 1334,
+	1171, 1009, 849, 690, 532, 376, 221, 67,
+	-84, -236, -386, -535, -683, -830, -975, -1119,
+	-1263, -1405, -1546, -1686, -1825, -1964, -2101, -2237,
+	-2372, -2506, -2639, -2771, -2902, -3033, -3162, -3291,
+	-3418, -3545, -3671, -3796, -3920, -4044, -4166, -4288,
+	-4409, -4529, -4649, -4767, -4885, -5002, -5119, -5235,
+	-5349, -5464, -5577, -5690, -5802, -5913, -6024, -6134,
+	-6244, -6352, -6461, -6568, -6675, -6781, -6887, -6992,
+	-7096, -7200, -7303, -7406, -7508, -7609, -7710, -7810,
+	-7910, -8009, -8108, -8206, -8304, -8401, -8497, -8593,
+	-8689, -8784, -8878, -8972, -9066, -9159, -9251, -9343,
+	-9435, -9526, -9617, -9707, -9796, -9886, -9975, -10063,
+	-10151, -10238, -10325, -10412, -10839, -10923, -11007, -11090,
+	-11173, -11256, -11338, -11420, -11501, -11583, -11663, -11744,
+	-11823, -11903, -11982
+};
+
+static const u8 chan_mux[DA9052_ADC_VBBAT + 1] = {
+	[DA9052_ADC_VDDOUT]	= DA9052_ADC_MAN_MUXSEL_VDDOUT,
+	[DA9052_ADC_ICH]	= DA9052_ADC_MAN_MUXSEL_ICH,
+	[DA9052_ADC_TBAT]	= DA9052_ADC_MAN_MUXSEL_TBAT,
+	[DA9052_ADC_VBAT]	= DA9052_ADC_MAN_MUXSEL_VBAT,
+	[DA9052_ADC_IN4]	= DA9052_ADC_MAN_MUXSEL_AD4,
+	[DA9052_ADC_IN5]	= DA9052_ADC_MAN_MUXSEL_AD5,
+	[DA9052_ADC_IN6]	= DA9052_ADC_MAN_MUXSEL_AD6,
+	[DA9052_ADC_VBBAT]	= DA9052_ADC_MAN_MUXSEL_VBBAT
+};
+
+int da9052_adc_manual_read(struct da9052 *da9052, unsigned char channel)
+{
+	int ret;
+	unsigned short calc_data;
+	unsigned short data;
+	unsigned char mux_sel;
+
+	if (channel > DA9052_ADC_VBBAT)
+		return -EINVAL;
+
+	mutex_lock(&da9052->auxadc_lock);
+
+	/* Channel gets activated on enabling the Conversion bit */
+	mux_sel = chan_mux[channel] | DA9052_ADC_MAN_MAN_CONV;
+
+	ret = da9052_reg_write(da9052, DA9052_ADC_MAN_REG, mux_sel);
+	if (ret < 0)
+		goto err;
+
+	/* Wait for an interrupt */
+	if (!wait_for_completion_timeout(&da9052->done,
+					 msecs_to_jiffies(500))) {
+		dev_err(da9052->dev,
+			"timeout waiting for ADC conversion interrupt\n");
+		ret = -ETIMEDOUT;
+		goto err;
+	}
+
+	ret = da9052_reg_read(da9052, DA9052_ADC_RES_H_REG);
+	if (ret < 0)
+		goto err;
+
+	calc_data = (unsigned short)ret;
+	data = calc_data << 2;
+
+	ret = da9052_reg_read(da9052, DA9052_ADC_RES_L_REG);
+	if (ret < 0)
+		goto err;
+
+	calc_data = (unsigned short)(ret & DA9052_ADC_RES_LSB);
+	data |= calc_data;
+
+	ret = data;
+
+err:
+	mutex_unlock(&da9052->auxadc_lock);
+	return ret;
+}
+EXPORT_SYMBOL_GPL(da9052_adc_manual_read);
+
+static irqreturn_t da9052_auxadc_irq(int irq, void *irq_data)
+{
+	struct da9052 *da9052 = irq_data;
+
+	complete(&da9052->done);
+
+	return IRQ_HANDLED;
+}
+
+int da9052_adc_read_temp(struct da9052 *da9052)
+{
+	int tbat;
+
+	tbat = da9052_reg_read(da9052, DA9052_TBAT_RES_REG);
+	if (tbat <= 0)
+		return tbat;
+
+	/* ARRAY_SIZE check is not needed since TBAT is a 8-bit register */
+	return tbat_lookup[tbat - 1];
+}
+EXPORT_SYMBOL_GPL(da9052_adc_read_temp);
+
 static struct resource da9052_rtc_resource = {
 	.name = "ALM",
 	.start = DA9052_IRQ_ALARM,
@@ -646,6 +775,9 @@ int __devinit da9052_device_init(struct da9052 *da9052, u8 chip_id)
 	struct irq_desc *desc;
 	int ret;
 
+	mutex_init(&da9052->auxadc_lock);
+	init_completion(&da9052->done);
+
 	if (pdata && pdata->init != NULL)
 		pdata->init(da9052);
 
@@ -659,12 +791,17 @@ int __devinit da9052_device_init(struct da9052 *da9052, u8 chip_id)
 	ret = regmap_add_irq_chip(da9052->regmap, da9052->chip_irq,
 				  IRQF_TRIGGER_LOW | IRQF_ONESHOT,
 				  da9052->irq_base, &da9052_regmap_irq_chip,
-				  NULL);
+				  &da9052->irq_data);
 	if (ret < 0)
 		goto regmap_err;
 
-	desc = irq_to_desc(da9052->chip_irq);
-	da9052->irq_base = regmap_irq_chip_get_base(desc->action->dev_id);
+	da9052->irq_base = regmap_irq_chip_get_base(da9052->irq_data);
+
+	ret = request_threaded_irq(DA9052_IRQ_ADC_EOM, NULL, da9052_auxadc_irq,
+				   IRQF_TRIGGER_LOW | IRQF_ONESHOT,
+				   "adc irq", da9052);
+	if (ret != 0)
+		dev_err(da9052->dev, "DA9052 ADC IRQ failed ret=%d\n", ret);
 
 	ret = mfd_add_devices(da9052->dev, -1, da9052_subdev_info,
 			      ARRAY_SIZE(da9052_subdev_info), NULL, 0);
@@ -674,6 +811,7 @@ int __devinit da9052_device_init(struct da9052 *da9052, u8 chip_id)
 	return 0;
 
 err:
+	free_irq(DA9052_IRQ_ADC_EOM, da9052);
 	mfd_remove_devices(da9052->dev);
 regmap_err:
 	return ret;
@@ -681,8 +819,8 @@ regmap_err:
 
 void da9052_device_exit(struct da9052 *da9052)
 {
-	regmap_del_irq_chip(da9052->chip_irq,
-			    irq_get_irq_data(da9052->irq_base)->chip_data);
+	free_irq(DA9052_IRQ_ADC_EOM, da9052);
+	regmap_del_irq_chip(da9052->chip_irq, da9052->irq_data);
 	mfd_remove_devices(da9052->dev);
 }
 
diff --git a/drivers/mfd/da9052-i2c.c b/drivers/mfd/da9052-i2c.c
index 36b88e39549..82c9d645028 100644
--- a/drivers/mfd/da9052-i2c.c
+++ b/drivers/mfd/da9052-i2c.c
@@ -22,6 +22,11 @@
 #include <linux/mfd/da9052/da9052.h>
 #include <linux/mfd/da9052/reg.h>
 
+#ifdef CONFIG_OF
+#include <linux/of.h>
+#include <linux/of_device.h>
+#endif
+
 static int da9052_i2c_enable_multiwrite(struct da9052 *da9052)
 {
 	int reg_val, ret;
@@ -41,13 +46,31 @@ static int da9052_i2c_enable_multiwrite(struct da9052 *da9052)
 	return 0;
 }
 
+static struct i2c_device_id da9052_i2c_id[] = {
+	{"da9052", DA9052},
+	{"da9053-aa", DA9053_AA},
+	{"da9053-ba", DA9053_BA},
+	{"da9053-bb", DA9053_BB},
+	{}
+};
+
+#ifdef CONFIG_OF
+static const struct of_device_id dialog_dt_ids[] = {
+	{ .compatible = "dlg,da9052", .data = &da9052_i2c_id[0] },
+	{ .compatible = "dlg,da9053-aa", .data = &da9052_i2c_id[1] },
+	{ .compatible = "dlg,da9053-ab", .data = &da9052_i2c_id[2] },
+	{ .compatible = "dlg,da9053-bb", .data = &da9052_i2c_id[3] },
+	{ /* sentinel */ }
+};
+#endif
+
 static int __devinit da9052_i2c_probe(struct i2c_client *client,
 				       const struct i2c_device_id *id)
 {
 	struct da9052 *da9052;
 	int ret;
 
-	da9052 = kzalloc(sizeof(struct da9052), GFP_KERNEL);
+	da9052 = devm_kzalloc(&client->dev, sizeof(struct da9052), GFP_KERNEL);
 	if (!da9052)
 		return -ENOMEM;
 
@@ -55,8 +78,7 @@ static int __devinit da9052_i2c_probe(struct i2c_client *client,
 				     I2C_FUNC_SMBUS_BYTE_DATA)) {
 		dev_info(&client->dev, "Error in %s:i2c_check_functionality\n",
 			 __func__);
-		ret = -ENODEV;
-		goto err;
+		return  -ENODEV;
 	}
 
 	da9052->dev = &client->dev;
@@ -64,29 +86,39 @@ static int __devinit da9052_i2c_probe(struct i2c_client *client,
 
 	i2c_set_clientdata(client, da9052);
 
-	da9052->regmap = regmap_init_i2c(client, &da9052_regmap_config);
+	da9052->regmap = devm_regmap_init_i2c(client, &da9052_regmap_config);
 	if (IS_ERR(da9052->regmap)) {
 		ret = PTR_ERR(da9052->regmap);
 		dev_err(&client->dev, "Failed to allocate register map: %d\n",
 			ret);
-		goto err;
+		return ret;
 	}
 
 	ret = da9052_i2c_enable_multiwrite(da9052);
 	if (ret < 0)
-		goto err_regmap;
+		return ret;
+
+#ifdef CONFIG_OF
+	if (!id) {
+		struct device_node *np = client->dev.of_node;
+		const struct of_device_id *deviceid;
+
+		deviceid = of_match_node(dialog_dt_ids, np);
+		id = (const struct i2c_device_id *)deviceid->data;
+	}
+#endif
+
+	if (!id) {
+		ret = -ENODEV;
+		dev_err(&client->dev, "id is null.\n");
+		return ret;
+	}
 
 	ret = da9052_device_init(da9052, id->driver_data);
 	if (ret != 0)
-		goto err_regmap;
+		return ret;
 
 	return 0;
-
-err_regmap:
-	regmap_exit(da9052->regmap);
-err:
-	kfree(da9052);
-	return ret;
 }
 
 static int __devexit da9052_i2c_remove(struct i2c_client *client)
@@ -94,20 +126,9 @@ static int __devexit da9052_i2c_remove(struct i2c_client *client)
 	struct da9052 *da9052 = i2c_get_clientdata(client);
 
 	da9052_device_exit(da9052);
-	regmap_exit(da9052->regmap);
-	kfree(da9052);
-
 	return 0;
 }
 
-static struct i2c_device_id da9052_i2c_id[] = {
-	{"da9052", DA9052},
-	{"da9053-aa", DA9053_AA},
-	{"da9053-ba", DA9053_BA},
-	{"da9053-bb", DA9053_BB},
-	{}
-};
-
 static struct i2c_driver da9052_i2c_driver = {
 	.probe = da9052_i2c_probe,
 	.remove = __devexit_p(da9052_i2c_remove),
@@ -115,6 +136,9 @@ static struct i2c_driver da9052_i2c_driver = {
 	.driver = {
 		.name = "da9052",
 		.owner = THIS_MODULE,
+#ifdef CONFIG_OF
+		.of_match_table = dialog_dt_ids,
+#endif
 	},
 };
 
diff --git a/drivers/mfd/da9052-spi.c b/drivers/mfd/da9052-spi.c
index 6faf149e8d9..dbeadc5a643 100644
--- a/drivers/mfd/da9052-spi.c
+++ b/drivers/mfd/da9052-spi.c
@@ -25,8 +25,9 @@ static int __devinit da9052_spi_probe(struct spi_device *spi)
 {
 	int ret;
 	const struct spi_device_id *id = spi_get_device_id(spi);
-	struct da9052 *da9052 = kzalloc(sizeof(struct da9052), GFP_KERNEL);
+	struct da9052 *da9052;
 
+	da9052 = devm_kzalloc(&spi->dev, sizeof(struct da9052), GFP_KERNEL);
 	if (!da9052)
 		return -ENOMEM;
 
@@ -42,25 +43,19 @@ static int __devinit da9052_spi_probe(struct spi_device *spi)
 	da9052_regmap_config.read_flag_mask = 1;
 	da9052_regmap_config.write_flag_mask = 0;
 
-	da9052->regmap = regmap_init_spi(spi, &da9052_regmap_config);
+	da9052->regmap = devm_regmap_init_spi(spi, &da9052_regmap_config);
 	if (IS_ERR(da9052->regmap)) {
 		ret = PTR_ERR(da9052->regmap);
 		dev_err(&spi->dev, "Failed to allocate register map: %d\n",
 			ret);
-		goto err;
+		return ret;
 	}
 
 	ret = da9052_device_init(da9052, id->driver_data);
 	if (ret != 0)
-		goto err_regmap;
+		return ret;
 
 	return 0;
-
-err_regmap:
-	regmap_exit(da9052->regmap);
-err:
-	kfree(da9052);
-	return ret;
 }
 
 static int __devexit da9052_spi_remove(struct spi_device *spi)
@@ -68,9 +63,6 @@ static int __devexit da9052_spi_remove(struct spi_device *spi)
 	struct da9052 *da9052 = dev_get_drvdata(&spi->dev);
 
 	da9052_device_exit(da9052);
-	regmap_exit(da9052->regmap);
-	kfree(da9052);
-
 	return 0;
 }
 
@@ -88,7 +80,6 @@ static struct spi_driver da9052_spi_driver = {
 	.id_table = da9052_spi_id,
 	.driver = {
 		.name = "da9052",
-		.bus = &spi_bus_type,
 		.owner = THIS_MODULE,
 	},
 };
diff --git a/drivers/mfd/db5500-prcmu.c b/drivers/mfd/db5500-prcmu.c
deleted file mode 100644
index bb115b2f04e..00000000000
--- a/drivers/mfd/db5500-prcmu.c
+++ /dev/null
@@ -1,451 +0,0 @@
-/*
- * Copyright (C) ST-Ericsson SA 2010
- *
- * License Terms: GNU General Public License v2
- * Author: Mattias Nilsson <mattias.i.nilsson@stericsson.com>
- *
- * U5500 PRCM Unit interface driver
- */
-#include <linux/module.h>
-#include <linux/kernel.h>
-#include <linux/delay.h>
-#include <linux/errno.h>
-#include <linux/err.h>
-#include <linux/spinlock.h>
-#include <linux/io.h>
-#include <linux/slab.h>
-#include <linux/mutex.h>
-#include <linux/completion.h>
-#include <linux/irq.h>
-#include <linux/jiffies.h>
-#include <linux/bitops.h>
-#include <linux/interrupt.h>
-#include <linux/mfd/dbx500-prcmu.h>
-#include <mach/hardware.h>
-#include <mach/irqs.h>
-#include <mach/db5500-regs.h>
-#include "dbx500-prcmu-regs.h"
-
-#define _PRCM_MB_HEADER (tcdm_base + 0xFE8)
-#define PRCM_REQ_MB0_HEADER (_PRCM_MB_HEADER + 0x0)
-#define PRCM_REQ_MB1_HEADER (_PRCM_MB_HEADER + 0x1)
-#define PRCM_REQ_MB2_HEADER (_PRCM_MB_HEADER + 0x2)
-#define PRCM_REQ_MB3_HEADER (_PRCM_MB_HEADER + 0x3)
-#define PRCM_REQ_MB4_HEADER (_PRCM_MB_HEADER + 0x4)
-#define PRCM_REQ_MB5_HEADER (_PRCM_MB_HEADER + 0x5)
-#define PRCM_REQ_MB6_HEADER (_PRCM_MB_HEADER + 0x6)
-#define PRCM_REQ_MB7_HEADER (_PRCM_MB_HEADER + 0x7)
-#define PRCM_ACK_MB0_HEADER (_PRCM_MB_HEADER + 0x8)
-#define PRCM_ACK_MB1_HEADER (_PRCM_MB_HEADER + 0x9)
-#define PRCM_ACK_MB2_HEADER (_PRCM_MB_HEADER + 0xa)
-#define PRCM_ACK_MB3_HEADER (_PRCM_MB_HEADER + 0xb)
-#define PRCM_ACK_MB4_HEADER (_PRCM_MB_HEADER + 0xc)
-#define PRCM_ACK_MB5_HEADER (_PRCM_MB_HEADER + 0xd)
-#define PRCM_ACK_MB6_HEADER (_PRCM_MB_HEADER + 0xe)
-#define PRCM_ACK_MB7_HEADER (_PRCM_MB_HEADER + 0xf)
-
-/* Req Mailboxes */
-#define PRCM_REQ_MB0 (tcdm_base + 0xFD8)
-#define PRCM_REQ_MB1 (tcdm_base + 0xFCC)
-#define PRCM_REQ_MB2 (tcdm_base + 0xFC4)
-#define PRCM_REQ_MB3 (tcdm_base + 0xFC0)
-#define PRCM_REQ_MB4 (tcdm_base + 0xF98)
-#define PRCM_REQ_MB5 (tcdm_base + 0xF90)
-#define PRCM_REQ_MB6 (tcdm_base + 0xF8C)
-#define PRCM_REQ_MB7 (tcdm_base + 0xF84)
-
-/* Ack Mailboxes */
-#define PRCM_ACK_MB0 (tcdm_base + 0xF38)
-#define PRCM_ACK_MB1 (tcdm_base + 0xF30)
-#define PRCM_ACK_MB2 (tcdm_base + 0xF24)
-#define PRCM_ACK_MB3 (tcdm_base + 0xF20)
-#define PRCM_ACK_MB4 (tcdm_base + 0xF1C)
-#define PRCM_ACK_MB5 (tcdm_base + 0xF14)
-#define PRCM_ACK_MB6 (tcdm_base + 0xF0C)
-#define PRCM_ACK_MB7 (tcdm_base + 0xF08)
-
-enum mb_return_code {
-	RC_SUCCESS,
-	RC_FAIL,
-};
-
-/* Mailbox 0 headers. */
-enum mb0_header {
-	/* request */
-	RMB0H_PWR_STATE_TRANS = 1,
-	RMB0H_WAKE_UP_CFG,
-	RMB0H_RD_WAKE_UP_ACK,
-	/* acknowledge */
-	AMB0H_WAKE_UP = 1,
-};
-
-/* Mailbox 5 headers. */
-enum mb5_header {
-	MB5H_I2C_WRITE = 1,
-	MB5H_I2C_READ,
-};
-
-/* Request mailbox 5 fields. */
-#define PRCM_REQ_MB5_I2C_SLAVE (PRCM_REQ_MB5 + 0)
-#define PRCM_REQ_MB5_I2C_REG (PRCM_REQ_MB5 + 1)
-#define PRCM_REQ_MB5_I2C_SIZE (PRCM_REQ_MB5 + 2)
-#define PRCM_REQ_MB5_I2C_DATA (PRCM_REQ_MB5 + 4)
-
-/* Acknowledge mailbox 5 fields. */
-#define PRCM_ACK_MB5_RETURN_CODE (PRCM_ACK_MB5 + 0)
-#define PRCM_ACK_MB5_I2C_DATA (PRCM_ACK_MB5 + 4)
-
-#define NUM_MB 8
-#define MBOX_BIT BIT
-#define ALL_MBOX_BITS (MBOX_BIT(NUM_MB) - 1)
-
-/*
-* Used by MCDE to setup all necessary PRCMU registers
-*/
-#define PRCMU_RESET_DSIPLL			0x00004000
-#define PRCMU_UNCLAMP_DSIPLL			0x00400800
-
-/* HDMI CLK MGT PLLSW=001 (PLLSOC0), PLLDIV=0x8, = 50 Mhz*/
-#define PRCMU_DSI_CLOCK_SETTING			0x00000128
-/* TVCLK_MGT PLLSW=001 (PLLSOC0) PLLDIV=0x13, = 19.05 MHZ */
-#define PRCMU_DSI_LP_CLOCK_SETTING		0x00000135
-#define PRCMU_PLLDSI_FREQ_SETTING		0x00020121
-#define PRCMU_DSI_PLLOUT_SEL_SETTING		0x00000002
-#define PRCMU_ENABLE_ESCAPE_CLOCK_DIV		0x03000201
-#define PRCMU_DISABLE_ESCAPE_CLOCK_DIV		0x00000101
-
-#define PRCMU_ENABLE_PLLDSI			0x00000001
-#define PRCMU_DISABLE_PLLDSI			0x00000000
-
-#define PRCMU_DSI_RESET_SW			0x00000003
-#define PRCMU_RESOUTN0_PIN			0x00000001
-#define PRCMU_RESOUTN1_PIN			0x00000002
-#define PRCMU_RESOUTN2_PIN			0x00000004
-
-#define PRCMU_PLLDSI_LOCKP_LOCKED		0x3
-
-/*
- * mb0_transfer - state needed for mailbox 0 communication.
- * @lock:		The transaction lock.
- */
-static struct {
-	spinlock_t lock;
-} mb0_transfer;
-
-/*
- * mb5_transfer - state needed for mailbox 5 communication.
- * @lock:	The transaction lock.
- * @work:	The transaction completion structure.
- * @ack:	Reply ("acknowledge") data.
- */
-static struct {
-	struct mutex lock;
-	struct completion work;
-	struct {
-		u8 header;
-		u8 status;
-		u8 value[4];
-	} ack;
-} mb5_transfer;
-
-/* PRCMU TCDM base IO address. */
-static __iomem void *tcdm_base;
-
-/**
- * db5500_prcmu_abb_read() - Read register value(s) from the ABB.
- * @slave:	The I2C slave address.
- * @reg:	The (start) register address.
- * @value:	The read out value(s).
- * @size:	The number of registers to read.
- *
- * Reads register value(s) from the ABB.
- * @size has to be <= 4.
- */
-int db5500_prcmu_abb_read(u8 slave, u8 reg, u8 *value, u8 size)
-{
-	int r;
-
-	if ((size < 1) || (4 < size))
-		return -EINVAL;
-
-	mutex_lock(&mb5_transfer.lock);
-
-	while (readl(PRCM_MBOX_CPU_VAL) & MBOX_BIT(5))
-		cpu_relax();
-	writeb(slave, PRCM_REQ_MB5_I2C_SLAVE);
-	writeb(reg, PRCM_REQ_MB5_I2C_REG);
-	writeb(size, PRCM_REQ_MB5_I2C_SIZE);
-	writeb(MB5H_I2C_READ, PRCM_REQ_MB5_HEADER);
-
-	writel(MBOX_BIT(5), PRCM_MBOX_CPU_SET);
-	wait_for_completion(&mb5_transfer.work);
-
-	r = 0;
-	if ((mb5_transfer.ack.header == MB5H_I2C_READ) &&
-		(mb5_transfer.ack.status == RC_SUCCESS))
-		memcpy(value, mb5_transfer.ack.value, (size_t)size);
-	else
-		r = -EIO;
-
-	mutex_unlock(&mb5_transfer.lock);
-
-	return r;
-}
-
-/**
- * db5500_prcmu_abb_write() - Write register value(s) to the ABB.
- * @slave:	The I2C slave address.
- * @reg:	The (start) register address.
- * @value:	The value(s) to write.
- * @size:	The number of registers to write.
- *
- * Writes register value(s) to the ABB.
- * @size has to be <= 4.
- */
-int db5500_prcmu_abb_write(u8 slave, u8 reg, u8 *value, u8 size)
-{
-	int r;
-
-	if ((size < 1) || (4 < size))
-		return -EINVAL;
-
-	mutex_lock(&mb5_transfer.lock);
-
-	while (readl(PRCM_MBOX_CPU_VAL) & MBOX_BIT(5))
-		cpu_relax();
-	writeb(slave, PRCM_REQ_MB5_I2C_SLAVE);
-	writeb(reg, PRCM_REQ_MB5_I2C_REG);
-	writeb(size, PRCM_REQ_MB5_I2C_SIZE);
-	memcpy_toio(PRCM_REQ_MB5_I2C_DATA, value, size);
-	writeb(MB5H_I2C_WRITE, PRCM_REQ_MB5_HEADER);
-
-	writel(MBOX_BIT(5), PRCM_MBOX_CPU_SET);
-	wait_for_completion(&mb5_transfer.work);
-
-	if ((mb5_transfer.ack.header == MB5H_I2C_WRITE) &&
-		(mb5_transfer.ack.status == RC_SUCCESS))
-		r = 0;
-	else
-		r = -EIO;
-
-	mutex_unlock(&mb5_transfer.lock);
-
-	return r;
-}
-
-int db5500_prcmu_enable_dsipll(void)
-{
-	int i;
-
-	/* Enable DSIPLL_RESETN resets */
-	writel(PRCMU_RESET_DSIPLL, PRCM_APE_RESETN_CLR);
-	/* Unclamp DSIPLL in/out */
-	writel(PRCMU_UNCLAMP_DSIPLL, PRCM_MMIP_LS_CLAMP_CLR);
-	/* Set DSI PLL FREQ */
-	writel(PRCMU_PLLDSI_FREQ_SETTING, PRCM_PLLDSI_FREQ);
-	writel(PRCMU_DSI_PLLOUT_SEL_SETTING,
-		PRCM_DSI_PLLOUT_SEL);
-	/* Enable Escape clocks */
-	writel(PRCMU_ENABLE_ESCAPE_CLOCK_DIV, PRCM_DSITVCLK_DIV);
-
-	/* Start DSI PLL */
-	writel(PRCMU_ENABLE_PLLDSI, PRCM_PLLDSI_ENABLE);
-	/* Reset DSI PLL */
-	writel(PRCMU_DSI_RESET_SW, PRCM_DSI_SW_RESET);
-	for (i = 0; i < 10; i++) {
-		if ((readl(PRCM_PLLDSI_LOCKP) &
-			PRCMU_PLLDSI_LOCKP_LOCKED) == PRCMU_PLLDSI_LOCKP_LOCKED)
-			break;
-		udelay(100);
-	}
-	/* Release DSIPLL_RESETN */
-	writel(PRCMU_RESET_DSIPLL, PRCM_APE_RESETN_SET);
-	return 0;
-}
-
-int db5500_prcmu_disable_dsipll(void)
-{
-	/* Disable dsi pll */
-	writel(PRCMU_DISABLE_PLLDSI, PRCM_PLLDSI_ENABLE);
-	/* Disable  escapeclock */
-	writel(PRCMU_DISABLE_ESCAPE_CLOCK_DIV, PRCM_DSITVCLK_DIV);
-	return 0;
-}
-
-int db5500_prcmu_set_display_clocks(void)
-{
-	/* HDMI and TVCLK Should be handled somewhere else */
-	/* PLLDIV=8, PLLSW=2, CLKEN=1 */
-	writel(PRCMU_DSI_CLOCK_SETTING, PRCM_HDMICLK_MGT);
-	/* PLLDIV=14, PLLSW=2, CLKEN=1 */
-	writel(PRCMU_DSI_LP_CLOCK_SETTING, PRCM_TVCLK_MGT);
-	return 0;
-}
-
-static void ack_dbb_wakeup(void)
-{
-	unsigned long flags;
-
-	spin_lock_irqsave(&mb0_transfer.lock, flags);
-
-	while (readl(PRCM_MBOX_CPU_VAL) & MBOX_BIT(0))
-		cpu_relax();
-
-	writeb(RMB0H_RD_WAKE_UP_ACK, PRCM_REQ_MB0_HEADER);
-	writel(MBOX_BIT(0), PRCM_MBOX_CPU_SET);
-
-	spin_unlock_irqrestore(&mb0_transfer.lock, flags);
-}
-
-static inline void print_unknown_header_warning(u8 n, u8 header)
-{
-	pr_warning("prcmu: Unknown message header (%d) in mailbox %d.\n",
-		header, n);
-}
-
-static bool read_mailbox_0(void)
-{
-	bool r;
-	u8 header;
-
-	header = readb(PRCM_ACK_MB0_HEADER);
-	switch (header) {
-	case AMB0H_WAKE_UP:
-		r = true;
-		break;
-	default:
-		print_unknown_header_warning(0, header);
-		r = false;
-		break;
-	}
-	writel(MBOX_BIT(0), PRCM_ARM_IT1_CLR);
-	return r;
-}
-
-static bool read_mailbox_1(void)
-{
-	writel(MBOX_BIT(1), PRCM_ARM_IT1_CLR);
-	return false;
-}
-
-static bool read_mailbox_2(void)
-{
-	writel(MBOX_BIT(2), PRCM_ARM_IT1_CLR);
-	return false;
-}
-
-static bool read_mailbox_3(void)
-{
-	writel(MBOX_BIT(3), PRCM_ARM_IT1_CLR);
-	return false;
-}
-
-static bool read_mailbox_4(void)
-{
-	writel(MBOX_BIT(4), PRCM_ARM_IT1_CLR);
-	return false;
-}
-
-static bool read_mailbox_5(void)
-{
-	u8 header;
-
-	header = readb(PRCM_ACK_MB5_HEADER);
-	switch (header) {
-	case MB5H_I2C_READ:
-		memcpy_fromio(mb5_transfer.ack.value, PRCM_ACK_MB5_I2C_DATA, 4);
-	case MB5H_I2C_WRITE:
-		mb5_transfer.ack.header = header;
-		mb5_transfer.ack.status = readb(PRCM_ACK_MB5_RETURN_CODE);
-		complete(&mb5_transfer.work);
-		break;
-	default:
-		print_unknown_header_warning(5, header);
-		break;
-	}
-	writel(MBOX_BIT(5), PRCM_ARM_IT1_CLR);
-	return false;
-}
-
-static bool read_mailbox_6(void)
-{
-	writel(MBOX_BIT(6), PRCM_ARM_IT1_CLR);
-	return false;
-}
-
-static bool read_mailbox_7(void)
-{
-	writel(MBOX_BIT(7), PRCM_ARM_IT1_CLR);
-	return false;
-}
-
-static bool (* const read_mailbox[NUM_MB])(void) = {
-	read_mailbox_0,
-	read_mailbox_1,
-	read_mailbox_2,
-	read_mailbox_3,
-	read_mailbox_4,
-	read_mailbox_5,
-	read_mailbox_6,
-	read_mailbox_7
-};
-
-static irqreturn_t prcmu_irq_handler(int irq, void *data)
-{
-	u32 bits;
-	u8 n;
-	irqreturn_t r;
-
-	bits = (readl(PRCM_ARM_IT1_VAL) & ALL_MBOX_BITS);
-	if (unlikely(!bits))
-		return IRQ_NONE;
-
-	r = IRQ_HANDLED;
-	for (n = 0; bits; n++) {
-		if (bits & MBOX_BIT(n)) {
-			bits -= MBOX_BIT(n);
-			if (read_mailbox[n]())
-				r = IRQ_WAKE_THREAD;
-		}
-	}
-	return r;
-}
-
-static irqreturn_t prcmu_irq_thread_fn(int irq, void *data)
-{
-	ack_dbb_wakeup();
-	return IRQ_HANDLED;
-}
-
-void __init db5500_prcmu_early_init(void)
-{
-	tcdm_base = __io_address(U5500_PRCMU_TCDM_BASE);
-	spin_lock_init(&mb0_transfer.lock);
-	mutex_init(&mb5_transfer.lock);
-	init_completion(&mb5_transfer.work);
-}
-
-/**
- * prcmu_fw_init - arch init call for the Linux PRCMU fw init logic
- *
- */
-int __init db5500_prcmu_init(void)
-{
-	int r = 0;
-
-	if (ux500_is_svp() || !cpu_is_u5500())
-		return -ENODEV;
-
-	/* Clean up the mailbox interrupts after pre-kernel code. */
-	writel(ALL_MBOX_BITS, PRCM_ARM_IT1_CLR);
-
-	r = request_threaded_irq(IRQ_DB5500_PRCMU1, prcmu_irq_handler,
-		prcmu_irq_thread_fn, 0, "prcmu", NULL);
-	if (r < 0) {
-		pr_err("prcmu: Failed to allocate IRQ_DB5500_PRCMU1.\n");
-		return -EBUSY;
-	}
-	return 0;
-}
-
-arch_initcall(db5500_prcmu_init);
diff --git a/drivers/mfd/db8500-prcmu.c b/drivers/mfd/db8500-prcmu.c
index 5be32489714..671c8bc14bb 100644
--- a/drivers/mfd/db8500-prcmu.c
+++ b/drivers/mfd/db8500-prcmu.c
@@ -2720,6 +2720,7 @@ static struct regulator_consumer_supply db8500_vape_consumers[] = {
 	REGULATOR_SUPPLY("v-i2c", "nmk-i2c.1"),
 	REGULATOR_SUPPLY("v-i2c", "nmk-i2c.2"),
 	REGULATOR_SUPPLY("v-i2c", "nmk-i2c.3"),
+	REGULATOR_SUPPLY("v-i2c", "nmk-i2c.4"),
 	/* "v-mmc" changed to "vcore" in the mainline kernel */
 	REGULATOR_SUPPLY("vcore", "sdi0"),
 	REGULATOR_SUPPLY("vcore", "sdi1"),
@@ -2958,9 +2959,10 @@ static struct mfd_cell db8500_prcmu_devs[] = {
  * prcmu_fw_init - arch init call for the Linux PRCMU fw init logic
  *
  */
-static int __init db8500_prcmu_probe(struct platform_device *pdev)
+static int __devinit db8500_prcmu_probe(struct platform_device *pdev)
 {
-	int err = 0;
+	struct device_node *np = pdev->dev.of_node;
+	int irq = 0, err = 0;
 
 	if (ux500_is_svp())
 		return -ENODEV;
@@ -2970,8 +2972,14 @@ static int __init db8500_prcmu_probe(struct platform_device *pdev)
 	/* Clean up the mailbox interrupts after pre-kernel code. */
 	writel(ALL_MBOX_BITS, PRCM_ARM_IT1_CLR);
 
-	err = request_threaded_irq(IRQ_DB8500_PRCMU1, prcmu_irq_handler,
-		prcmu_irq_thread_fn, IRQF_NO_SUSPEND, "prcmu", NULL);
+	if (np)
+		irq = platform_get_irq(pdev, 0);
+
+	if (!np || irq <= 0)
+		irq = IRQ_DB8500_PRCMU1;
+
+	err = request_threaded_irq(irq, prcmu_irq_handler,
+	        prcmu_irq_thread_fn, IRQF_NO_SUSPEND, "prcmu", NULL);
 	if (err < 0) {
 		pr_err("prcmu: Failed to allocate IRQ_DB8500_PRCMU1.\n");
 		err = -EBUSY;
@@ -2981,14 +2989,16 @@ static int __init db8500_prcmu_probe(struct platform_device *pdev)
 	if (cpu_is_u8500v20_or_later())
 		prcmu_config_esram0_deep_sleep(ESRAM0_DEEP_SLEEP_STATE_RET);
 
-	err = mfd_add_devices(&pdev->dev, 0, db8500_prcmu_devs,
-			      ARRAY_SIZE(db8500_prcmu_devs), NULL,
-			      0);
+	if (!np) {
+		err = mfd_add_devices(&pdev->dev, 0, db8500_prcmu_devs,
+				ARRAY_SIZE(db8500_prcmu_devs), NULL, 0);
+		if (err) {
+			pr_err("prcmu: Failed to add subdevices\n");
+			return err;
+		}
+	}
 
-	if (err)
-		pr_err("prcmu: Failed to add subdevices\n");
-	else
-		pr_info("DB8500 PRCMU initialized\n");
+	pr_info("DB8500 PRCMU initialized\n");
 
 no_irq_return:
 	return err;
@@ -2999,11 +3009,12 @@ static struct platform_driver db8500_prcmu_driver = {
 		.name = "db8500-prcmu",
 		.owner = THIS_MODULE,
 	},
+	.probe = db8500_prcmu_probe,
 };
 
 static int __init db8500_prcmu_init(void)
 {
-	return platform_driver_probe(&db8500_prcmu_driver, db8500_prcmu_probe);
+	return platform_driver_register(&db8500_prcmu_driver);
 }
 
 arch_initcall(db8500_prcmu_init);
diff --git a/drivers/mfd/intel_msic.c b/drivers/mfd/intel_msic.c
index b76657eb0c5..59df5584cb5 100644
--- a/drivers/mfd/intel_msic.c
+++ b/drivers/mfd/intel_msic.c
@@ -406,7 +406,7 @@ static int __devinit intel_msic_probe(struct platform_device *pdev)
 		return -ENXIO;
 	}
 
-	msic = kzalloc(sizeof(*msic), GFP_KERNEL);
+	msic = devm_kzalloc(&pdev->dev, sizeof(*msic), GFP_KERNEL);
 	if (!msic)
 		return -ENOMEM;
 
@@ -421,21 +421,13 @@ static int __devinit intel_msic_probe(struct platform_device *pdev)
 	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
 	if (!res) {
 		dev_err(&pdev->dev, "failed to get SRAM iomem resource\n");
-		ret = -ENODEV;
-		goto fail_free_msic;
+		return -ENODEV;
 	}
 
-	res = request_mem_region(res->start, resource_size(res), pdev->name);
-	if (!res) {
-		ret = -EBUSY;
-		goto fail_free_msic;
-	}
-
-	msic->irq_base = ioremap_nocache(res->start, resource_size(res));
+	msic->irq_base = devm_request_and_ioremap(&pdev->dev, res);
 	if (!msic->irq_base) {
 		dev_err(&pdev->dev, "failed to map SRAM memory\n");
-		ret = -ENOMEM;
-		goto fail_release_region;
+		return -ENOMEM;
 	}
 
 	platform_set_drvdata(pdev, msic);
@@ -443,7 +435,7 @@ static int __devinit intel_msic_probe(struct platform_device *pdev)
 	ret = intel_msic_init_devices(msic);
 	if (ret) {
 		dev_err(&pdev->dev, "failed to initialize MSIC devices\n");
-		goto fail_unmap_mem;
+		return ret;
 	}
 
 	dev_info(&pdev->dev, "Intel MSIC version %c%d (vendor %#x)\n",
@@ -451,27 +443,14 @@ static int __devinit intel_msic_probe(struct platform_device *pdev)
 		 msic->vendor);
 
 	return 0;
-
-fail_unmap_mem:
-	iounmap(msic->irq_base);
-fail_release_region:
-	release_mem_region(res->start, resource_size(res));
-fail_free_msic:
-	kfree(msic);
-
-	return ret;
 }
 
 static int __devexit intel_msic_remove(struct platform_device *pdev)
 {
 	struct intel_msic *msic = platform_get_drvdata(pdev);
-	struct resource *res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
 
 	intel_msic_remove_devices(msic);
 	platform_set_drvdata(pdev, NULL);
-	iounmap(msic->irq_base);
-	release_mem_region(res->start, resource_size(res));
-	kfree(msic);
 
 	return 0;
 }
diff --git a/drivers/mfd/janz-cmodio.c b/drivers/mfd/janz-cmodio.c
index a9223ed1b7c..2ea99989551 100644
--- a/drivers/mfd/janz-cmodio.c
+++ b/drivers/mfd/janz-cmodio.c
@@ -283,23 +283,8 @@ static struct pci_driver cmodio_pci_driver = {
 	.remove   = __devexit_p(cmodio_pci_remove),
 };
 
-/*
- * Module Init / Exit
- */
-
-static int __init cmodio_init(void)
-{
-	return pci_register_driver(&cmodio_pci_driver);
-}
-
-static void __exit cmodio_exit(void)
-{
-	pci_unregister_driver(&cmodio_pci_driver);
-}
+module_pci_driver(cmodio_pci_driver);
 
 MODULE_AUTHOR("Ira W. Snyder <iws@ovro.caltech.edu>");
 MODULE_DESCRIPTION("Janz CMOD-IO PCI MODULbus Carrier Board Driver");
 MODULE_LICENSE("GPL");
-
-module_init(cmodio_init);
-module_exit(cmodio_exit);
diff --git a/drivers/mfd/lm3533-core.c b/drivers/mfd/lm3533-core.c
new file mode 100644
index 00000000000..0b2879b87fd
--- /dev/null
+++ b/drivers/mfd/lm3533-core.c
@@ -0,0 +1,667 @@
+/*
+ * lm3533-core.c -- LM3533 Core
+ *
+ * Copyright (C) 2011-2012 Texas Instruments
+ *
+ * Author: Johan Hovold <jhovold@gmail.com>
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under  the terms of the GNU General  Public License as published by the
+ * Free Software Foundation;  either version 2 of the License, or (at your
+ * option) any later version.
+ */
+
+#include <linux/module.h>
+#include <linux/init.h>
+#include <linux/kernel.h>
+#include <linux/err.h>
+#include <linux/gpio.h>
+#include <linux/i2c.h>
+#include <linux/mfd/core.h>
+#include <linux/regmap.h>
+#include <linux/seq_file.h>
+#include <linux/slab.h>
+#include <linux/uaccess.h>
+
+#include <linux/mfd/lm3533.h>
+
+
+#define LM3533_BOOST_OVP_MASK		0x06
+#define LM3533_BOOST_OVP_SHIFT		1
+
+#define LM3533_BOOST_FREQ_MASK		0x01
+#define LM3533_BOOST_FREQ_SHIFT		0
+
+#define LM3533_BL_ID_MASK		1
+#define LM3533_LED_ID_MASK		3
+#define LM3533_BL_ID_MAX		1
+#define LM3533_LED_ID_MAX		3
+
+#define LM3533_HVLED_ID_MAX		2
+#define LM3533_LVLED_ID_MAX		5
+
+#define LM3533_REG_OUTPUT_CONF1		0x10
+#define LM3533_REG_OUTPUT_CONF2		0x11
+#define LM3533_REG_BOOST_PWM		0x2c
+
+#define LM3533_REG_MAX			0xb2
+
+
+static struct mfd_cell lm3533_als_devs[] = {
+	{
+		.name	= "lm3533-als",
+		.id	= -1,
+	},
+};
+
+static struct mfd_cell lm3533_bl_devs[] = {
+	{
+		.name	= "lm3533-backlight",
+		.id	= 0,
+	},
+	{
+		.name	= "lm3533-backlight",
+		.id	= 1,
+	},
+};
+
+static struct mfd_cell lm3533_led_devs[] = {
+	{
+		.name	= "lm3533-leds",
+		.id	= 0,
+	},
+	{
+		.name	= "lm3533-leds",
+		.id	= 1,
+	},
+	{
+		.name	= "lm3533-leds",
+		.id	= 2,
+	},
+	{
+		.name	= "lm3533-leds",
+		.id	= 3,
+	},
+};
+
+int lm3533_read(struct lm3533 *lm3533, u8 reg, u8 *val)
+{
+	int tmp;
+	int ret;
+
+	ret = regmap_read(lm3533->regmap, reg, &tmp);
+	if (ret < 0) {
+		dev_err(lm3533->dev, "failed to read register %02x: %d\n",
+								reg, ret);
+		return ret;
+	}
+
+	*val = tmp;
+
+	dev_dbg(lm3533->dev, "read [%02x]: %02x\n", reg, *val);
+
+	return ret;
+}
+EXPORT_SYMBOL_GPL(lm3533_read);
+
+int lm3533_write(struct lm3533 *lm3533, u8 reg, u8 val)
+{
+	int ret;
+
+	dev_dbg(lm3533->dev, "write [%02x]: %02x\n", reg, val);
+
+	ret = regmap_write(lm3533->regmap, reg, val);
+	if (ret < 0) {
+		dev_err(lm3533->dev, "failed to write register %02x: %d\n",
+								reg, ret);
+	}
+
+	return ret;
+}
+EXPORT_SYMBOL_GPL(lm3533_write);
+
+int lm3533_update(struct lm3533 *lm3533, u8 reg, u8 val, u8 mask)
+{
+	int ret;
+
+	dev_dbg(lm3533->dev, "update [%02x]: %02x/%02x\n", reg, val, mask);
+
+	ret = regmap_update_bits(lm3533->regmap, reg, mask, val);
+	if (ret < 0) {
+		dev_err(lm3533->dev, "failed to update register %02x: %d\n",
+								reg, ret);
+	}
+
+	return ret;
+}
+EXPORT_SYMBOL_GPL(lm3533_update);
+
+static int lm3533_set_boost_freq(struct lm3533 *lm3533,
+						enum lm3533_boost_freq freq)
+{
+	int ret;
+
+	ret = lm3533_update(lm3533, LM3533_REG_BOOST_PWM,
+					freq << LM3533_BOOST_FREQ_SHIFT,
+					LM3533_BOOST_FREQ_MASK);
+	if (ret)
+		dev_err(lm3533->dev, "failed to set boost frequency\n");
+
+	return ret;
+}
+
+
+static int lm3533_set_boost_ovp(struct lm3533 *lm3533,
+						enum lm3533_boost_ovp ovp)
+{
+	int ret;
+
+	ret = lm3533_update(lm3533, LM3533_REG_BOOST_PWM,
+					ovp << LM3533_BOOST_OVP_SHIFT,
+					LM3533_BOOST_OVP_MASK);
+	if (ret)
+		dev_err(lm3533->dev, "failed to set boost ovp\n");
+
+	return ret;
+}
+
+/*
+ * HVLED output config -- output hvled controlled by backlight bl
+ */
+static int lm3533_set_hvled_config(struct lm3533 *lm3533, u8 hvled, u8 bl)
+{
+	u8 val;
+	u8 mask;
+	int shift;
+	int ret;
+
+	if (hvled == 0 || hvled > LM3533_HVLED_ID_MAX)
+		return -EINVAL;
+
+	if (bl > LM3533_BL_ID_MAX)
+		return -EINVAL;
+
+	shift = hvled - 1;
+	mask = LM3533_BL_ID_MASK << shift;
+	val = bl << shift;
+
+	ret = lm3533_update(lm3533, LM3533_REG_OUTPUT_CONF1, val, mask);
+	if (ret)
+		dev_err(lm3533->dev, "failed to set hvled config\n");
+
+	return ret;
+}
+
+/*
+ * LVLED output config -- output lvled controlled by LED led
+ */
+static int lm3533_set_lvled_config(struct lm3533 *lm3533, u8 lvled, u8 led)
+{
+	u8 reg;
+	u8 val;
+	u8 mask;
+	int shift;
+	int ret;
+
+	if (lvled == 0 || lvled > LM3533_LVLED_ID_MAX)
+		return -EINVAL;
+
+	if (led > LM3533_LED_ID_MAX)
+		return -EINVAL;
+
+	if (lvled < 4) {
+		reg = LM3533_REG_OUTPUT_CONF1;
+		shift = 2 * lvled;
+	} else {
+		reg = LM3533_REG_OUTPUT_CONF2;
+		shift = 2 * (lvled - 4);
+	}
+
+	mask = LM3533_LED_ID_MASK << shift;
+	val = led << shift;
+
+	ret = lm3533_update(lm3533, reg, val, mask);
+	if (ret)
+		dev_err(lm3533->dev, "failed to set lvled config\n");
+
+	return ret;
+}
+
+static void lm3533_enable(struct lm3533 *lm3533)
+{
+	if (gpio_is_valid(lm3533->gpio_hwen))
+		gpio_set_value(lm3533->gpio_hwen, 1);
+}
+
+static void lm3533_disable(struct lm3533 *lm3533)
+{
+	if (gpio_is_valid(lm3533->gpio_hwen))
+		gpio_set_value(lm3533->gpio_hwen, 0);
+}
+
+enum lm3533_attribute_type {
+	LM3533_ATTR_TYPE_BACKLIGHT,
+	LM3533_ATTR_TYPE_LED,
+};
+
+struct lm3533_device_attribute {
+	struct device_attribute dev_attr;
+	enum lm3533_attribute_type type;
+	union {
+		struct {
+			u8 id;
+		} output;
+	} u;
+};
+
+#define to_lm3533_dev_attr(_attr) \
+	container_of(_attr, struct lm3533_device_attribute, dev_attr)
+
+static ssize_t show_output(struct device *dev,
+				struct device_attribute *attr, char *buf)
+{
+	struct lm3533 *lm3533 = dev_get_drvdata(dev);
+	struct lm3533_device_attribute *lattr = to_lm3533_dev_attr(attr);
+	int id = lattr->u.output.id;
+	u8 reg;
+	u8 val;
+	u8 mask;
+	int shift;
+	int ret;
+
+	if (lattr->type == LM3533_ATTR_TYPE_BACKLIGHT) {
+		reg = LM3533_REG_OUTPUT_CONF1;
+		shift = id - 1;
+		mask = LM3533_BL_ID_MASK << shift;
+	} else {
+		if (id < 4) {
+			reg = LM3533_REG_OUTPUT_CONF1;
+			shift = 2 * id;
+		} else {
+			reg = LM3533_REG_OUTPUT_CONF2;
+			shift = 2 * (id - 4);
+		}
+		mask = LM3533_LED_ID_MASK << shift;
+	}
+
+	ret = lm3533_read(lm3533, reg, &val);
+	if (ret)
+		return ret;
+
+	val = (val & mask) >> shift;
+
+	return scnprintf(buf, PAGE_SIZE, "%u\n", val);
+}
+
+static ssize_t store_output(struct device *dev,
+					struct device_attribute *attr,
+					const char *buf, size_t len)
+{
+	struct lm3533 *lm3533 = dev_get_drvdata(dev);
+	struct lm3533_device_attribute *lattr = to_lm3533_dev_attr(attr);
+	int id = lattr->u.output.id;
+	u8 val;
+	int ret;
+
+	if (kstrtou8(buf, 0, &val))
+		return -EINVAL;
+
+	if (lattr->type == LM3533_ATTR_TYPE_BACKLIGHT)
+		ret = lm3533_set_hvled_config(lm3533, id, val);
+	else
+		ret = lm3533_set_lvled_config(lm3533, id, val);
+
+	if (ret)
+		return ret;
+
+	return len;
+}
+
+#define LM3533_OUTPUT_ATTR(_name, _mode, _show, _store, _type, _id) \
+	struct lm3533_device_attribute lm3533_dev_attr_##_name = \
+		{ .dev_attr	= __ATTR(_name, _mode, _show, _store), \
+		  .type		= _type, \
+		  .u.output	= { .id = _id }, }
+
+#define LM3533_OUTPUT_ATTR_RW(_name, _type, _id) \
+	LM3533_OUTPUT_ATTR(output_##_name, S_IRUGO | S_IWUSR, \
+					show_output, store_output, _type, _id)
+
+#define LM3533_OUTPUT_HVLED_ATTR_RW(_nr) \
+	LM3533_OUTPUT_ATTR_RW(hvled##_nr, LM3533_ATTR_TYPE_BACKLIGHT, _nr)
+#define LM3533_OUTPUT_LVLED_ATTR_RW(_nr) \
+	LM3533_OUTPUT_ATTR_RW(lvled##_nr, LM3533_ATTR_TYPE_LED, _nr)
+/*
+ * Output config:
+ *
+ * output_hvled<nr>	0-1
+ * output_lvled<nr>	0-3
+ */
+static LM3533_OUTPUT_HVLED_ATTR_RW(1);
+static LM3533_OUTPUT_HVLED_ATTR_RW(2);
+static LM3533_OUTPUT_LVLED_ATTR_RW(1);
+static LM3533_OUTPUT_LVLED_ATTR_RW(2);
+static LM3533_OUTPUT_LVLED_ATTR_RW(3);
+static LM3533_OUTPUT_LVLED_ATTR_RW(4);
+static LM3533_OUTPUT_LVLED_ATTR_RW(5);
+
+static struct attribute *lm3533_attributes[] = {
+	&lm3533_dev_attr_output_hvled1.dev_attr.attr,
+	&lm3533_dev_attr_output_hvled2.dev_attr.attr,
+	&lm3533_dev_attr_output_lvled1.dev_attr.attr,
+	&lm3533_dev_attr_output_lvled2.dev_attr.attr,
+	&lm3533_dev_attr_output_lvled3.dev_attr.attr,
+	&lm3533_dev_attr_output_lvled4.dev_attr.attr,
+	&lm3533_dev_attr_output_lvled5.dev_attr.attr,
+	NULL,
+};
+
+#define to_dev_attr(_attr) \
+	container_of(_attr, struct device_attribute, attr)
+
+static umode_t lm3533_attr_is_visible(struct kobject *kobj,
+					     struct attribute *attr, int n)
+{
+	struct device *dev = container_of(kobj, struct device, kobj);
+	struct lm3533 *lm3533 = dev_get_drvdata(dev);
+	struct device_attribute *dattr = to_dev_attr(attr);
+	struct lm3533_device_attribute *lattr = to_lm3533_dev_attr(dattr);
+	enum lm3533_attribute_type type = lattr->type;
+	umode_t mode = attr->mode;
+
+	if (!lm3533->have_backlights && type == LM3533_ATTR_TYPE_BACKLIGHT)
+		mode = 0;
+	else if (!lm3533->have_leds && type == LM3533_ATTR_TYPE_LED)
+		mode = 0;
+
+	return mode;
+};
+
+static struct attribute_group lm3533_attribute_group = {
+	.is_visible	= lm3533_attr_is_visible,
+	.attrs		= lm3533_attributes
+};
+
+static int __devinit lm3533_device_als_init(struct lm3533 *lm3533)
+{
+	struct lm3533_platform_data *pdata = lm3533->dev->platform_data;
+	int ret;
+
+	if (!pdata->als)
+		return 0;
+
+	lm3533_als_devs[0].platform_data = pdata->als;
+	lm3533_als_devs[0].pdata_size = sizeof(*pdata->als);
+
+	ret = mfd_add_devices(lm3533->dev, 0, lm3533_als_devs, 1, NULL, 0);
+	if (ret) {
+		dev_err(lm3533->dev, "failed to add ALS device\n");
+		return ret;
+	}
+
+	lm3533->have_als = 1;
+
+	return 0;
+}
+
+static int __devinit lm3533_device_bl_init(struct lm3533 *lm3533)
+{
+	struct lm3533_platform_data *pdata = lm3533->dev->platform_data;
+	int i;
+	int ret;
+
+	if (!pdata->backlights || pdata->num_backlights == 0)
+		return 0;
+
+	if (pdata->num_backlights > ARRAY_SIZE(lm3533_bl_devs))
+		pdata->num_backlights = ARRAY_SIZE(lm3533_bl_devs);
+
+	for (i = 0; i < pdata->num_backlights; ++i) {
+		lm3533_bl_devs[i].platform_data = &pdata->backlights[i];
+		lm3533_bl_devs[i].pdata_size = sizeof(pdata->backlights[i]);
+	}
+
+	ret = mfd_add_devices(lm3533->dev, 0, lm3533_bl_devs,
+					pdata->num_backlights, NULL, 0);
+	if (ret) {
+		dev_err(lm3533->dev, "failed to add backlight devices\n");
+		return ret;
+	}
+
+	lm3533->have_backlights = 1;
+
+	return 0;
+}
+
+static int __devinit lm3533_device_led_init(struct lm3533 *lm3533)
+{
+	struct lm3533_platform_data *pdata = lm3533->dev->platform_data;
+	int i;
+	int ret;
+
+	if (!pdata->leds || pdata->num_leds == 0)
+		return 0;
+
+	if (pdata->num_leds > ARRAY_SIZE(lm3533_led_devs))
+		pdata->num_leds = ARRAY_SIZE(lm3533_led_devs);
+
+	for (i = 0; i < pdata->num_leds; ++i) {
+		lm3533_led_devs[i].platform_data = &pdata->leds[i];
+		lm3533_led_devs[i].pdata_size = sizeof(pdata->leds[i]);
+	}
+
+	ret = mfd_add_devices(lm3533->dev, 0, lm3533_led_devs,
+						pdata->num_leds, NULL, 0);
+	if (ret) {
+		dev_err(lm3533->dev, "failed to add LED devices\n");
+		return ret;
+	}
+
+	lm3533->have_leds = 1;
+
+	return 0;
+}
+
+static int __devinit lm3533_device_setup(struct lm3533 *lm3533,
+					struct lm3533_platform_data *pdata)
+{
+	int ret;
+
+	ret = lm3533_set_boost_freq(lm3533, pdata->boost_freq);
+	if (ret)
+		return ret;
+
+	ret = lm3533_set_boost_ovp(lm3533, pdata->boost_ovp);
+	if (ret)
+		return ret;
+
+	return 0;
+}
+
+static int __devinit lm3533_device_init(struct lm3533 *lm3533)
+{
+	struct lm3533_platform_data *pdata = lm3533->dev->platform_data;
+	int ret;
+
+	dev_dbg(lm3533->dev, "%s\n", __func__);
+
+	if (!pdata) {
+		dev_err(lm3533->dev, "no platform data\n");
+		return -EINVAL;
+	}
+
+	lm3533->gpio_hwen = pdata->gpio_hwen;
+
+	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");
+		if (ret < 0) {
+			dev_err(lm3533->dev,
+				"failed to request HWEN GPIO %d\n",
+				lm3533->gpio_hwen);
+			return ret;
+		}
+	}
+
+	lm3533_enable(lm3533);
+
+	ret = lm3533_device_setup(lm3533, pdata);
+	if (ret)
+		goto err_disable;
+
+	lm3533_device_als_init(lm3533);
+	lm3533_device_bl_init(lm3533);
+	lm3533_device_led_init(lm3533);
+
+	ret = sysfs_create_group(&lm3533->dev->kobj, &lm3533_attribute_group);
+	if (ret < 0) {
+		dev_err(lm3533->dev, "failed to create sysfs attributes\n");
+		goto err_unregister;
+	}
+
+	return 0;
+
+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;
+}
+
+static void __devexit lm3533_device_exit(struct lm3533 *lm3533)
+{
+	dev_dbg(lm3533->dev, "%s\n", __func__);
+
+	sysfs_remove_group(&lm3533->dev->kobj, &lm3533_attribute_group);
+
+	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)
+{
+	switch (reg) {
+	case 0x10 ... 0x2c:
+	case 0x30 ... 0x38:
+	case 0x40 ... 0x45:
+	case 0x50 ... 0x57:
+	case 0x60 ... 0x6e:
+	case 0x70 ... 0x75:
+	case 0x80 ... 0x85:
+	case 0x90 ... 0x95:
+	case 0xa0 ... 0xa5:
+	case 0xb0 ... 0xb2:
+		return true;
+	default:
+		return false;
+	}
+}
+
+static bool lm3533_volatile_register(struct device *dev, unsigned int reg)
+{
+	switch (reg) {
+	case 0x34 ... 0x36:	/* zone */
+	case 0x37 ... 0x38:	/* adc */
+	case 0xb0 ... 0xb1:	/* fault */
+		return true;
+	default:
+		return false;
+	}
+}
+
+static bool lm3533_precious_register(struct device *dev, unsigned int reg)
+{
+	switch (reg) {
+	case 0x34:		/* zone */
+		return true;
+	default:
+		return false;
+	}
+}
+
+static struct regmap_config regmap_config = {
+	.reg_bits	= 8,
+	.val_bits	= 8,
+	.max_register	= LM3533_REG_MAX,
+	.readable_reg	= lm3533_readable_register,
+	.volatile_reg	= lm3533_volatile_register,
+	.precious_reg	= lm3533_precious_register,
+};
+
+static int __devinit lm3533_i2c_probe(struct i2c_client *i2c,
+					const struct i2c_device_id *id)
+{
+	struct lm3533 *lm3533;
+	int ret;
+
+	dev_dbg(&i2c->dev, "%s\n", __func__);
+
+	lm3533 = devm_kzalloc(&i2c->dev, sizeof(*lm3533), GFP_KERNEL);
+	if (!lm3533)
+		return -ENOMEM;
+
+	i2c_set_clientdata(i2c, lm3533);
+
+	lm3533->regmap = devm_regmap_init_i2c(i2c, &regmap_config);
+	if (IS_ERR(lm3533->regmap))
+		return PTR_ERR(lm3533->regmap);
+
+	lm3533->dev = &i2c->dev;
+	lm3533->irq = i2c->irq;
+
+	ret = lm3533_device_init(lm3533);
+	if (ret)
+		return ret;
+
+	return 0;
+}
+
+static int __devexit lm3533_i2c_remove(struct i2c_client *i2c)
+{
+	struct lm3533 *lm3533 = i2c_get_clientdata(i2c);
+
+	dev_dbg(&i2c->dev, "%s\n", __func__);
+
+	lm3533_device_exit(lm3533);
+
+	return 0;
+}
+
+static const struct i2c_device_id lm3533_i2c_ids[] = {
+	{ "lm3533", 0 },
+	{ },
+};
+MODULE_DEVICE_TABLE(i2c, lm3533_i2c_ids);
+
+static struct i2c_driver lm3533_i2c_driver = {
+	.driver = {
+		   .name = "lm3533",
+		   .owner = THIS_MODULE,
+	},
+	.id_table	= lm3533_i2c_ids,
+	.probe		= lm3533_i2c_probe,
+	.remove		= __devexit_p(lm3533_i2c_remove),
+};
+
+static int __init lm3533_i2c_init(void)
+{
+	return i2c_add_driver(&lm3533_i2c_driver);
+}
+subsys_initcall(lm3533_i2c_init);
+
+static void __exit lm3533_i2c_exit(void)
+{
+	i2c_del_driver(&lm3533_i2c_driver);
+}
+module_exit(lm3533_i2c_exit);
+
+MODULE_AUTHOR("Johan Hovold <jhovold@gmail.com>");
+MODULE_DESCRIPTION("LM3533 Core");
+MODULE_LICENSE("GPL");
diff --git a/drivers/mfd/lm3533-ctrlbank.c b/drivers/mfd/lm3533-ctrlbank.c
new file mode 100644
index 00000000000..a4cb7a5220a
--- /dev/null
+++ b/drivers/mfd/lm3533-ctrlbank.c
@@ -0,0 +1,148 @@
+/*
+ * lm3533-ctrlbank.c -- LM3533 Generic Control Bank interface
+ *
+ * Copyright (C) 2011-2012 Texas Instruments
+ *
+ * Author: Johan Hovold <jhovold@gmail.com>
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under  the terms of the GNU General  Public License as published by the
+ * Free Software Foundation;  either version 2 of the License, or (at your
+ * option) any later version.
+ */
+
+#include <linux/device.h>
+#include <linux/module.h>
+
+#include <linux/mfd/lm3533.h>
+
+
+#define LM3533_MAX_CURRENT_MIN		5000
+#define LM3533_MAX_CURRENT_MAX		29800
+#define LM3533_MAX_CURRENT_STEP		800
+
+#define LM3533_BRIGHTNESS_MAX		255
+#define LM3533_PWM_MAX			0x3f
+
+#define LM3533_REG_PWM_BASE		0x14
+#define LM3533_REG_MAX_CURRENT_BASE	0x1f
+#define LM3533_REG_CTRLBANK_ENABLE	0x27
+#define LM3533_REG_BRIGHTNESS_BASE	0x40
+
+
+static inline u8 lm3533_ctrlbank_get_reg(struct lm3533_ctrlbank *cb, u8 base)
+{
+	return base + cb->id;
+}
+
+int lm3533_ctrlbank_enable(struct lm3533_ctrlbank *cb)
+{
+	u8 mask;
+	int ret;
+
+	dev_dbg(cb->dev, "%s - %d\n", __func__, cb->id);
+
+	mask = 1 << cb->id;
+	ret = lm3533_update(cb->lm3533, LM3533_REG_CTRLBANK_ENABLE,
+								mask, mask);
+	if (ret)
+		dev_err(cb->dev, "failed to enable ctrlbank %d\n", cb->id);
+
+	return ret;
+}
+EXPORT_SYMBOL_GPL(lm3533_ctrlbank_enable);
+
+int lm3533_ctrlbank_disable(struct lm3533_ctrlbank *cb)
+{
+	u8 mask;
+	int ret;
+
+	dev_dbg(cb->dev, "%s - %d\n", __func__, cb->id);
+
+	mask = 1 << cb->id;
+	ret = lm3533_update(cb->lm3533, LM3533_REG_CTRLBANK_ENABLE, 0, mask);
+	if (ret)
+		dev_err(cb->dev, "failed to disable ctrlbank %d\n", cb->id);
+
+	return ret;
+}
+EXPORT_SYMBOL_GPL(lm3533_ctrlbank_disable);
+
+/*
+ * Full-scale current.
+ *
+ * imax		5000 - 29800 uA (800 uA step)
+ */
+int lm3533_ctrlbank_set_max_current(struct lm3533_ctrlbank *cb, u16 imax)
+{
+	u8 reg;
+	u8 val;
+	int ret;
+
+	if (imax < LM3533_MAX_CURRENT_MIN || imax > LM3533_MAX_CURRENT_MAX)
+		return -EINVAL;
+
+	val = (imax - LM3533_MAX_CURRENT_MIN) / LM3533_MAX_CURRENT_STEP;
+
+	reg = lm3533_ctrlbank_get_reg(cb, LM3533_REG_MAX_CURRENT_BASE);
+	ret = lm3533_write(cb->lm3533, reg, val);
+	if (ret)
+		dev_err(cb->dev, "failed to set max current\n");
+
+	return ret;
+}
+EXPORT_SYMBOL_GPL(lm3533_ctrlbank_set_max_current);
+
+#define lm3533_ctrlbank_set(_name, _NAME)				\
+int lm3533_ctrlbank_set_##_name(struct lm3533_ctrlbank *cb, u8 val)	\
+{									\
+	u8 reg;								\
+	int ret;							\
+									\
+	if (val > LM3533_##_NAME##_MAX)					\
+		return -EINVAL;						\
+									\
+	reg = lm3533_ctrlbank_get_reg(cb, LM3533_REG_##_NAME##_BASE);	\
+	ret = lm3533_write(cb->lm3533, reg, val);			\
+	if (ret)							\
+		dev_err(cb->dev, "failed to set " #_name "\n");		\
+									\
+	return ret;							\
+}									\
+EXPORT_SYMBOL_GPL(lm3533_ctrlbank_set_##_name);
+
+#define lm3533_ctrlbank_get(_name, _NAME)				\
+int lm3533_ctrlbank_get_##_name(struct lm3533_ctrlbank *cb, u8 *val)	\
+{									\
+	u8 reg;								\
+	int ret;							\
+									\
+	reg = lm3533_ctrlbank_get_reg(cb, LM3533_REG_##_NAME##_BASE);	\
+	ret = lm3533_read(cb->lm3533, reg, val);			\
+	if (ret)							\
+		dev_err(cb->dev, "failed to get " #_name "\n");		\
+									\
+	return ret;							\
+}									\
+EXPORT_SYMBOL_GPL(lm3533_ctrlbank_get_##_name);
+
+lm3533_ctrlbank_set(brightness, BRIGHTNESS);
+lm3533_ctrlbank_get(brightness, BRIGHTNESS);
+
+/*
+ * PWM-input control mask:
+ *
+ *   bit 5 - PWM-input enabled in Zone 4
+ *   bit 4 - PWM-input enabled in Zone 3
+ *   bit 3 - PWM-input enabled in Zone 2
+ *   bit 2 - PWM-input enabled in Zone 1
+ *   bit 1 - PWM-input enabled in Zone 0
+ *   bit 0 - PWM-input enabled
+ */
+lm3533_ctrlbank_set(pwm, PWM);
+lm3533_ctrlbank_get(pwm, PWM);
+
+
+MODULE_AUTHOR("Johan Hovold <jhovold@gmail.com>");
+MODULE_DESCRIPTION("LM3533 Control Bank interface");
+MODULE_LICENSE("GPL");
diff --git a/drivers/mfd/lpc_ich.c b/drivers/mfd/lpc_ich.c
new file mode 100644
index 00000000000..027cc8f8613
--- /dev/null
+++ b/drivers/mfd/lpc_ich.c
@@ -0,0 +1,888 @@
+/*
+ *  lpc_ich.c - LPC interface for Intel ICH
+ *
+ *  LPC bridge function of the Intel ICH contains many other
+ *  functional units, such as Interrupt controllers, Timers,
+ *  Power Management, System Management, GPIO, RTC, and LPC
+ *  Configuration Registers.
+ *
+ *  This driver is derived from lpc_sch.
+
+ *  Copyright (c) 2011 Extreme Engineering Solution, Inc.
+ *  Author: Aaron Sierra <asierra@xes-inc.com>
+ *
+ *  This program is free software; you can redistribute it and/or modify
+ *  it under the terms of the GNU General Public License 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; see the file COPYING.  If not, write to
+ *  the Free Software Foundation, 675 Mass Ave, Cambridge, MA 02139, USA.
+ *
+ *  This driver supports the following I/O Controller hubs:
+ *	(See the intel documentation on http://developer.intel.com.)
+ *	document number 290655-003, 290677-014: 82801AA (ICH), 82801AB (ICHO)
+ *	document number 290687-002, 298242-027: 82801BA (ICH2)
+ *	document number 290733-003, 290739-013: 82801CA (ICH3-S)
+ *	document number 290716-001, 290718-007: 82801CAM (ICH3-M)
+ *	document number 290744-001, 290745-025: 82801DB (ICH4)
+ *	document number 252337-001, 252663-008: 82801DBM (ICH4-M)
+ *	document number 273599-001, 273645-002: 82801E (C-ICH)
+ *	document number 252516-001, 252517-028: 82801EB (ICH5), 82801ER (ICH5R)
+ *	document number 300641-004, 300884-013: 6300ESB
+ *	document number 301473-002, 301474-026: 82801F (ICH6)
+ *	document number 313082-001, 313075-006: 631xESB, 632xESB
+ *	document number 307013-003, 307014-024: 82801G (ICH7)
+ *	document number 322896-001, 322897-001: NM10
+ *	document number 313056-003, 313057-017: 82801H (ICH8)
+ *	document number 316972-004, 316973-012: 82801I (ICH9)
+ *	document number 319973-002, 319974-002: 82801J (ICH10)
+ *	document number 322169-001, 322170-003: 5 Series, 3400 Series (PCH)
+ *	document number 320066-003, 320257-008: EP80597 (IICH)
+ *	document number 324645-001, 324646-001: Cougar Point (CPT)
+ *	document number TBD : Patsburg (PBG)
+ *	document number TBD : DH89xxCC
+ *	document number TBD : Panther Point
+ *	document number TBD : Lynx Point
+ */
+
+#define pr_fmt(fmt) KBUILD_MODNAME ": " fmt
+
+#include <linux/init.h>
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/errno.h>
+#include <linux/acpi.h>
+#include <linux/pci.h>
+#include <linux/mfd/core.h>
+#include <linux/mfd/lpc_ich.h>
+
+#define ACPIBASE		0x40
+#define ACPIBASE_GPE_OFF	0x28
+#define ACPIBASE_GPE_END	0x2f
+#define ACPIBASE_SMI_OFF	0x30
+#define ACPIBASE_SMI_END	0x33
+#define ACPIBASE_TCO_OFF	0x60
+#define ACPIBASE_TCO_END	0x7f
+#define ACPICTRL		0x44
+
+#define ACPIBASE_GCS_OFF	0x3410
+#define ACPIBASE_GCS_END	0x3414
+
+#define GPIOBASE		0x48
+#define GPIOCTRL		0x4C
+
+#define RCBABASE		0xf0
+
+#define wdt_io_res(i) wdt_res(0, i)
+#define wdt_mem_res(i) wdt_res(ICH_RES_MEM_OFF, i)
+#define wdt_res(b, i) (&wdt_ich_res[(b) + (i)])
+
+static int lpc_ich_acpi_save = -1;
+static int lpc_ich_gpio_save = -1;
+
+static struct resource wdt_ich_res[] = {
+	/* ACPI - TCO */
+	{
+		.flags = IORESOURCE_IO,
+	},
+	/* ACPI - SMI */
+	{
+		.flags = IORESOURCE_IO,
+	},
+	/* GCS */
+	{
+		.flags = IORESOURCE_MEM,
+	},
+};
+
+static struct resource gpio_ich_res[] = {
+	/* GPIO */
+	{
+		.flags = IORESOURCE_IO,
+	},
+	/* ACPI - GPE0 */
+	{
+		.flags = IORESOURCE_IO,
+	},
+};
+
+enum lpc_cells {
+	LPC_WDT = 0,
+	LPC_GPIO,
+};
+
+static struct mfd_cell lpc_ich_cells[] = {
+	[LPC_WDT] = {
+		.name = "iTCO_wdt",
+		.num_resources = ARRAY_SIZE(wdt_ich_res),
+		.resources = wdt_ich_res,
+		.ignore_resource_conflicts = true,
+	},
+	[LPC_GPIO] = {
+		.name = "gpio_ich",
+		.num_resources = ARRAY_SIZE(gpio_ich_res),
+		.resources = gpio_ich_res,
+		.ignore_resource_conflicts = true,
+	},
+};
+
+/* chipset related info */
+enum lpc_chipsets {
+	LPC_ICH = 0,	/* ICH */
+	LPC_ICH0,	/* ICH0 */
+	LPC_ICH2,	/* ICH2 */
+	LPC_ICH2M,	/* ICH2-M */
+	LPC_ICH3,	/* ICH3-S */
+	LPC_ICH3M,	/* ICH3-M */
+	LPC_ICH4,	/* ICH4 */
+	LPC_ICH4M,	/* ICH4-M */
+	LPC_CICH,	/* C-ICH */
+	LPC_ICH5,	/* ICH5 & ICH5R */
+	LPC_6300ESB,	/* 6300ESB */
+	LPC_ICH6,	/* ICH6 & ICH6R */
+	LPC_ICH6M,	/* ICH6-M */
+	LPC_ICH6W,	/* ICH6W & ICH6RW */
+	LPC_631XESB,	/* 631xESB/632xESB */
+	LPC_ICH7,	/* ICH7 & ICH7R */
+	LPC_ICH7DH,	/* ICH7DH */
+	LPC_ICH7M,	/* ICH7-M & ICH7-U */
+	LPC_ICH7MDH,	/* ICH7-M DH */
+	LPC_NM10,	/* NM10 */
+	LPC_ICH8,	/* ICH8 & ICH8R */
+	LPC_ICH8DH,	/* ICH8DH */
+	LPC_ICH8DO,	/* ICH8DO */
+	LPC_ICH8M,	/* ICH8M */
+	LPC_ICH8ME,	/* ICH8M-E */
+	LPC_ICH9,	/* ICH9 */
+	LPC_ICH9R,	/* ICH9R */
+	LPC_ICH9DH,	/* ICH9DH */
+	LPC_ICH9DO,	/* ICH9DO */
+	LPC_ICH9M,	/* ICH9M */
+	LPC_ICH9ME,	/* ICH9M-E */
+	LPC_ICH10,	/* ICH10 */
+	LPC_ICH10R,	/* ICH10R */
+	LPC_ICH10D,	/* ICH10D */
+	LPC_ICH10DO,	/* ICH10DO */
+	LPC_PCH,	/* PCH Desktop Full Featured */
+	LPC_PCHM,	/* PCH Mobile Full Featured */
+	LPC_P55,	/* P55 */
+	LPC_PM55,	/* PM55 */
+	LPC_H55,	/* H55 */
+	LPC_QM57,	/* QM57 */
+	LPC_H57,	/* H57 */
+	LPC_HM55,	/* HM55 */
+	LPC_Q57,	/* Q57 */
+	LPC_HM57,	/* HM57 */
+	LPC_PCHMSFF,	/* PCH Mobile SFF Full Featured */
+	LPC_QS57,	/* QS57 */
+	LPC_3400,	/* 3400 */
+	LPC_3420,	/* 3420 */
+	LPC_3450,	/* 3450 */
+	LPC_EP80579,	/* EP80579 */
+	LPC_CPT,	/* Cougar Point */
+	LPC_CPTD,	/* Cougar Point Desktop */
+	LPC_CPTM,	/* Cougar Point Mobile */
+	LPC_PBG,	/* Patsburg */
+	LPC_DH89XXCC,	/* DH89xxCC */
+	LPC_PPT,	/* Panther Point */
+	LPC_LPT,	/* Lynx Point */
+};
+
+struct lpc_ich_info lpc_chipset_info[] __devinitdata = {
+	[LPC_ICH] = {
+		.name = "ICH",
+		.iTCO_version = 1,
+	},
+	[LPC_ICH0] = {
+		.name = "ICH0",
+		.iTCO_version = 1,
+	},
+	[LPC_ICH2] = {
+		.name = "ICH2",
+		.iTCO_version = 1,
+	},
+	[LPC_ICH2M] = {
+		.name = "ICH2-M",
+		.iTCO_version = 1,
+	},
+	[LPC_ICH3] = {
+		.name = "ICH3-S",
+		.iTCO_version = 1,
+	},
+	[LPC_ICH3M] = {
+		.name = "ICH3-M",
+		.iTCO_version = 1,
+	},
+	[LPC_ICH4] = {
+		.name = "ICH4",
+		.iTCO_version = 1,
+	},
+	[LPC_ICH4M] = {
+		.name = "ICH4-M",
+		.iTCO_version = 1,
+	},
+	[LPC_CICH] = {
+		.name = "C-ICH",
+		.iTCO_version = 1,
+	},
+	[LPC_ICH5] = {
+		.name = "ICH5 or ICH5R",
+		.iTCO_version = 1,
+	},
+	[LPC_6300ESB] = {
+		.name = "6300ESB",
+		.iTCO_version = 1,
+	},
+	[LPC_ICH6] = {
+		.name = "ICH6 or ICH6R",
+		.iTCO_version = 2,
+		.gpio_version = ICH_V6_GPIO,
+	},
+	[LPC_ICH6M] = {
+		.name = "ICH6-M",
+		.iTCO_version = 2,
+		.gpio_version = ICH_V6_GPIO,
+	},
+	[LPC_ICH6W] = {
+		.name = "ICH6W or ICH6RW",
+		.iTCO_version = 2,
+		.gpio_version = ICH_V6_GPIO,
+	},
+	[LPC_631XESB] = {
+		.name = "631xESB/632xESB",
+		.iTCO_version = 2,
+		.gpio_version = ICH_V6_GPIO,
+	},
+	[LPC_ICH7] = {
+		.name = "ICH7 or ICH7R",
+		.iTCO_version = 2,
+		.gpio_version = ICH_V7_GPIO,
+	},
+	[LPC_ICH7DH] = {
+		.name = "ICH7DH",
+		.iTCO_version = 2,
+		.gpio_version = ICH_V7_GPIO,
+	},
+	[LPC_ICH7M] = {
+		.name = "ICH7-M or ICH7-U",
+		.iTCO_version = 2,
+		.gpio_version = ICH_V7_GPIO,
+	},
+	[LPC_ICH7MDH] = {
+		.name = "ICH7-M DH",
+		.iTCO_version = 2,
+		.gpio_version = ICH_V7_GPIO,
+	},
+	[LPC_NM10] = {
+		.name = "NM10",
+		.iTCO_version = 2,
+	},
+	[LPC_ICH8] = {
+		.name = "ICH8 or ICH8R",
+		.iTCO_version = 2,
+		.gpio_version = ICH_V7_GPIO,
+	},
+	[LPC_ICH8DH] = {
+		.name = "ICH8DH",
+		.iTCO_version = 2,
+		.gpio_version = ICH_V7_GPIO,
+	},
+	[LPC_ICH8DO] = {
+		.name = "ICH8DO",
+		.iTCO_version = 2,
+		.gpio_version = ICH_V7_GPIO,
+	},
+	[LPC_ICH8M] = {
+		.name = "ICH8M",
+		.iTCO_version = 2,
+		.gpio_version = ICH_V7_GPIO,
+	},
+	[LPC_ICH8ME] = {
+		.name = "ICH8M-E",
+		.iTCO_version = 2,
+		.gpio_version = ICH_V7_GPIO,
+	},
+	[LPC_ICH9] = {
+		.name = "ICH9",
+		.iTCO_version = 2,
+		.gpio_version = ICH_V9_GPIO,
+	},
+	[LPC_ICH9R] = {
+		.name = "ICH9R",
+		.iTCO_version = 2,
+		.gpio_version = ICH_V9_GPIO,
+	},
+	[LPC_ICH9DH] = {
+		.name = "ICH9DH",
+		.iTCO_version = 2,
+		.gpio_version = ICH_V9_GPIO,
+	},
+	[LPC_ICH9DO] = {
+		.name = "ICH9DO",
+		.iTCO_version = 2,
+		.gpio_version = ICH_V9_GPIO,
+	},
+	[LPC_ICH9M] = {
+		.name = "ICH9M",
+		.iTCO_version = 2,
+		.gpio_version = ICH_V9_GPIO,
+	},
+	[LPC_ICH9ME] = {
+		.name = "ICH9M-E",
+		.iTCO_version = 2,
+		.gpio_version = ICH_V9_GPIO,
+	},
+	[LPC_ICH10] = {
+		.name = "ICH10",
+		.iTCO_version = 2,
+		.gpio_version = ICH_V10CONS_GPIO,
+	},
+	[LPC_ICH10R] = {
+		.name = "ICH10R",
+		.iTCO_version = 2,
+		.gpio_version = ICH_V10CONS_GPIO,
+	},
+	[LPC_ICH10D] = {
+		.name = "ICH10D",
+		.iTCO_version = 2,
+		.gpio_version = ICH_V10CORP_GPIO,
+	},
+	[LPC_ICH10DO] = {
+		.name = "ICH10DO",
+		.iTCO_version = 2,
+		.gpio_version = ICH_V10CORP_GPIO,
+	},
+	[LPC_PCH] = {
+		.name = "PCH Desktop Full Featured",
+		.iTCO_version = 2,
+		.gpio_version = ICH_V5_GPIO,
+	},
+	[LPC_PCHM] = {
+		.name = "PCH Mobile Full Featured",
+		.iTCO_version = 2,
+		.gpio_version = ICH_V5_GPIO,
+	},
+	[LPC_P55] = {
+		.name = "P55",
+		.iTCO_version = 2,
+		.gpio_version = ICH_V5_GPIO,
+	},
+	[LPC_PM55] = {
+		.name = "PM55",
+		.iTCO_version = 2,
+		.gpio_version = ICH_V5_GPIO,
+	},
+	[LPC_H55] = {
+		.name = "H55",
+		.iTCO_version = 2,
+		.gpio_version = ICH_V5_GPIO,
+	},
+	[LPC_QM57] = {
+		.name = "QM57",
+		.iTCO_version = 2,
+		.gpio_version = ICH_V5_GPIO,
+	},
+	[LPC_H57] = {
+		.name = "H57",
+		.iTCO_version = 2,
+		.gpio_version = ICH_V5_GPIO,
+	},
+	[LPC_HM55] = {
+		.name = "HM55",
+		.iTCO_version = 2,
+		.gpio_version = ICH_V5_GPIO,
+	},
+	[LPC_Q57] = {
+		.name = "Q57",
+		.iTCO_version = 2,
+		.gpio_version = ICH_V5_GPIO,
+	},
+	[LPC_HM57] = {
+		.name = "HM57",
+		.iTCO_version = 2,
+		.gpio_version = ICH_V5_GPIO,
+	},
+	[LPC_PCHMSFF] = {
+		.name = "PCH Mobile SFF Full Featured",
+		.iTCO_version = 2,
+		.gpio_version = ICH_V5_GPIO,
+	},
+	[LPC_QS57] = {
+		.name = "QS57",
+		.iTCO_version = 2,
+		.gpio_version = ICH_V5_GPIO,
+	},
+	[LPC_3400] = {
+		.name = "3400",
+		.iTCO_version = 2,
+		.gpio_version = ICH_V5_GPIO,
+	},
+	[LPC_3420] = {
+		.name = "3420",
+		.iTCO_version = 2,
+		.gpio_version = ICH_V5_GPIO,
+	},
+	[LPC_3450] = {
+		.name = "3450",
+		.iTCO_version = 2,
+		.gpio_version = ICH_V5_GPIO,
+	},
+	[LPC_EP80579] = {
+		.name = "EP80579",
+		.iTCO_version = 2,
+	},
+	[LPC_CPT] = {
+		.name = "Cougar Point",
+		.iTCO_version = 2,
+		.gpio_version = ICH_V5_GPIO,
+	},
+	[LPC_CPTD] = {
+		.name = "Cougar Point Desktop",
+		.iTCO_version = 2,
+		.gpio_version = ICH_V5_GPIO,
+	},
+	[LPC_CPTM] = {
+		.name = "Cougar Point Mobile",
+		.iTCO_version = 2,
+		.gpio_version = ICH_V5_GPIO,
+	},
+	[LPC_PBG] = {
+		.name = "Patsburg",
+		.iTCO_version = 2,
+	},
+	[LPC_DH89XXCC] = {
+		.name = "DH89xxCC",
+		.iTCO_version = 2,
+	},
+	[LPC_PPT] = {
+		.name = "Panther Point",
+		.iTCO_version = 2,
+	},
+	[LPC_LPT] = {
+		.name = "Lynx Point",
+		.iTCO_version = 2,
+	},
+};
+
+/*
+ * This data only exists for exporting the supported PCI ids
+ * via MODULE_DEVICE_TABLE.  We do not actually register a
+ * pci_driver, because the I/O Controller Hub has also other
+ * functions that probably will be registered by other drivers.
+ */
+static DEFINE_PCI_DEVICE_TABLE(lpc_ich_ids) = {
+	{ PCI_VDEVICE(INTEL, 0x2410), LPC_ICH},
+	{ PCI_VDEVICE(INTEL, 0x2420), LPC_ICH0},
+	{ PCI_VDEVICE(INTEL, 0x2440), LPC_ICH2},
+	{ PCI_VDEVICE(INTEL, 0x244c), LPC_ICH2M},
+	{ PCI_VDEVICE(INTEL, 0x2480), LPC_ICH3},
+	{ PCI_VDEVICE(INTEL, 0x248c), LPC_ICH3M},
+	{ PCI_VDEVICE(INTEL, 0x24c0), LPC_ICH4},
+	{ PCI_VDEVICE(INTEL, 0x24cc), LPC_ICH4M},
+	{ PCI_VDEVICE(INTEL, 0x2450), LPC_CICH},
+	{ PCI_VDEVICE(INTEL, 0x24d0), LPC_ICH5},
+	{ PCI_VDEVICE(INTEL, 0x25a1), LPC_6300ESB},
+	{ PCI_VDEVICE(INTEL, 0x2640), LPC_ICH6},
+	{ PCI_VDEVICE(INTEL, 0x2641), LPC_ICH6M},
+	{ PCI_VDEVICE(INTEL, 0x2642), LPC_ICH6W},
+	{ PCI_VDEVICE(INTEL, 0x2670), LPC_631XESB},
+	{ PCI_VDEVICE(INTEL, 0x2671), LPC_631XESB},
+	{ PCI_VDEVICE(INTEL, 0x2672), LPC_631XESB},
+	{ PCI_VDEVICE(INTEL, 0x2673), LPC_631XESB},
+	{ PCI_VDEVICE(INTEL, 0x2674), LPC_631XESB},
+	{ PCI_VDEVICE(INTEL, 0x2675), LPC_631XESB},
+	{ PCI_VDEVICE(INTEL, 0x2676), LPC_631XESB},
+	{ PCI_VDEVICE(INTEL, 0x2677), LPC_631XESB},
+	{ PCI_VDEVICE(INTEL, 0x2678), LPC_631XESB},
+	{ PCI_VDEVICE(INTEL, 0x2679), LPC_631XESB},
+	{ PCI_VDEVICE(INTEL, 0x267a), LPC_631XESB},
+	{ PCI_VDEVICE(INTEL, 0x267b), LPC_631XESB},
+	{ PCI_VDEVICE(INTEL, 0x267c), LPC_631XESB},
+	{ PCI_VDEVICE(INTEL, 0x267d), LPC_631XESB},
+	{ PCI_VDEVICE(INTEL, 0x267e), LPC_631XESB},
+	{ PCI_VDEVICE(INTEL, 0x267f), LPC_631XESB},
+	{ PCI_VDEVICE(INTEL, 0x27b8), LPC_ICH7},
+	{ PCI_VDEVICE(INTEL, 0x27b0), LPC_ICH7DH},
+	{ PCI_VDEVICE(INTEL, 0x27b9), LPC_ICH7M},
+	{ PCI_VDEVICE(INTEL, 0x27bd), LPC_ICH7MDH},
+	{ PCI_VDEVICE(INTEL, 0x27bc), LPC_NM10},
+	{ PCI_VDEVICE(INTEL, 0x2810), LPC_ICH8},
+	{ PCI_VDEVICE(INTEL, 0x2812), LPC_ICH8DH},
+	{ PCI_VDEVICE(INTEL, 0x2814), LPC_ICH8DO},
+	{ PCI_VDEVICE(INTEL, 0x2815), LPC_ICH8M},
+	{ PCI_VDEVICE(INTEL, 0x2811), LPC_ICH8ME},
+	{ PCI_VDEVICE(INTEL, 0x2918), LPC_ICH9},
+	{ PCI_VDEVICE(INTEL, 0x2916), LPC_ICH9R},
+	{ PCI_VDEVICE(INTEL, 0x2912), LPC_ICH9DH},
+	{ PCI_VDEVICE(INTEL, 0x2914), LPC_ICH9DO},
+	{ PCI_VDEVICE(INTEL, 0x2919), LPC_ICH9M},
+	{ PCI_VDEVICE(INTEL, 0x2917), LPC_ICH9ME},
+	{ PCI_VDEVICE(INTEL, 0x3a18), LPC_ICH10},
+	{ PCI_VDEVICE(INTEL, 0x3a16), LPC_ICH10R},
+	{ PCI_VDEVICE(INTEL, 0x3a1a), LPC_ICH10D},
+	{ PCI_VDEVICE(INTEL, 0x3a14), LPC_ICH10DO},
+	{ PCI_VDEVICE(INTEL, 0x3b00), LPC_PCH},
+	{ PCI_VDEVICE(INTEL, 0x3b01), LPC_PCHM},
+	{ PCI_VDEVICE(INTEL, 0x3b02), LPC_P55},
+	{ PCI_VDEVICE(INTEL, 0x3b03), LPC_PM55},
+	{ PCI_VDEVICE(INTEL, 0x3b06), LPC_H55},
+	{ PCI_VDEVICE(INTEL, 0x3b07), LPC_QM57},
+	{ PCI_VDEVICE(INTEL, 0x3b08), LPC_H57},
+	{ PCI_VDEVICE(INTEL, 0x3b09), LPC_HM55},
+	{ PCI_VDEVICE(INTEL, 0x3b0a), LPC_Q57},
+	{ PCI_VDEVICE(INTEL, 0x3b0b), LPC_HM57},
+	{ PCI_VDEVICE(INTEL, 0x3b0d), LPC_PCHMSFF},
+	{ PCI_VDEVICE(INTEL, 0x3b0f), LPC_QS57},
+	{ PCI_VDEVICE(INTEL, 0x3b12), LPC_3400},
+	{ PCI_VDEVICE(INTEL, 0x3b14), LPC_3420},
+	{ PCI_VDEVICE(INTEL, 0x3b16), LPC_3450},
+	{ PCI_VDEVICE(INTEL, 0x5031), LPC_EP80579},
+	{ PCI_VDEVICE(INTEL, 0x1c41), LPC_CPT},
+	{ PCI_VDEVICE(INTEL, 0x1c42), LPC_CPTD},
+	{ PCI_VDEVICE(INTEL, 0x1c43), LPC_CPTM},
+	{ PCI_VDEVICE(INTEL, 0x1c44), LPC_CPT},
+	{ PCI_VDEVICE(INTEL, 0x1c45), LPC_CPT},
+	{ PCI_VDEVICE(INTEL, 0x1c46), LPC_CPT},
+	{ PCI_VDEVICE(INTEL, 0x1c47), LPC_CPT},
+	{ PCI_VDEVICE(INTEL, 0x1c48), LPC_CPT},
+	{ PCI_VDEVICE(INTEL, 0x1c49), LPC_CPT},
+	{ PCI_VDEVICE(INTEL, 0x1c4a), LPC_CPT},
+	{ PCI_VDEVICE(INTEL, 0x1c4b), LPC_CPT},
+	{ PCI_VDEVICE(INTEL, 0x1c4c), LPC_CPT},
+	{ PCI_VDEVICE(INTEL, 0x1c4d), LPC_CPT},
+	{ PCI_VDEVICE(INTEL, 0x1c4e), LPC_CPT},
+	{ PCI_VDEVICE(INTEL, 0x1c4f), LPC_CPT},
+	{ PCI_VDEVICE(INTEL, 0x1c50), LPC_CPT},
+	{ PCI_VDEVICE(INTEL, 0x1c51), LPC_CPT},
+	{ PCI_VDEVICE(INTEL, 0x1c52), LPC_CPT},
+	{ PCI_VDEVICE(INTEL, 0x1c53), LPC_CPT},
+	{ PCI_VDEVICE(INTEL, 0x1c54), LPC_CPT},
+	{ PCI_VDEVICE(INTEL, 0x1c55), LPC_CPT},
+	{ PCI_VDEVICE(INTEL, 0x1c56), LPC_CPT},
+	{ PCI_VDEVICE(INTEL, 0x1c57), LPC_CPT},
+	{ PCI_VDEVICE(INTEL, 0x1c58), LPC_CPT},
+	{ PCI_VDEVICE(INTEL, 0x1c59), LPC_CPT},
+	{ PCI_VDEVICE(INTEL, 0x1c5a), LPC_CPT},
+	{ PCI_VDEVICE(INTEL, 0x1c5b), LPC_CPT},
+	{ PCI_VDEVICE(INTEL, 0x1c5c), LPC_CPT},
+	{ PCI_VDEVICE(INTEL, 0x1c5d), LPC_CPT},
+	{ PCI_VDEVICE(INTEL, 0x1c5e), LPC_CPT},
+	{ PCI_VDEVICE(INTEL, 0x1c5f), LPC_CPT},
+	{ PCI_VDEVICE(INTEL, 0x1d40), LPC_PBG},
+	{ PCI_VDEVICE(INTEL, 0x1d41), LPC_PBG},
+	{ PCI_VDEVICE(INTEL, 0x2310), LPC_DH89XXCC},
+	{ PCI_VDEVICE(INTEL, 0x1e40), LPC_PPT},
+	{ PCI_VDEVICE(INTEL, 0x1e41), LPC_PPT},
+	{ PCI_VDEVICE(INTEL, 0x1e42), LPC_PPT},
+	{ PCI_VDEVICE(INTEL, 0x1e43), LPC_PPT},
+	{ PCI_VDEVICE(INTEL, 0x1e44), LPC_PPT},
+	{ PCI_VDEVICE(INTEL, 0x1e45), LPC_PPT},
+	{ PCI_VDEVICE(INTEL, 0x1e46), LPC_PPT},
+	{ PCI_VDEVICE(INTEL, 0x1e47), LPC_PPT},
+	{ PCI_VDEVICE(INTEL, 0x1e48), LPC_PPT},
+	{ PCI_VDEVICE(INTEL, 0x1e49), LPC_PPT},
+	{ PCI_VDEVICE(INTEL, 0x1e4a), LPC_PPT},
+	{ PCI_VDEVICE(INTEL, 0x1e4b), LPC_PPT},
+	{ PCI_VDEVICE(INTEL, 0x1e4c), LPC_PPT},
+	{ PCI_VDEVICE(INTEL, 0x1e4d), LPC_PPT},
+	{ PCI_VDEVICE(INTEL, 0x1e4e), LPC_PPT},
+	{ PCI_VDEVICE(INTEL, 0x1e4f), LPC_PPT},
+	{ PCI_VDEVICE(INTEL, 0x1e50), LPC_PPT},
+	{ PCI_VDEVICE(INTEL, 0x1e51), LPC_PPT},
+	{ PCI_VDEVICE(INTEL, 0x1e52), LPC_PPT},
+	{ PCI_VDEVICE(INTEL, 0x1e53), LPC_PPT},
+	{ PCI_VDEVICE(INTEL, 0x1e54), LPC_PPT},
+	{ PCI_VDEVICE(INTEL, 0x1e55), LPC_PPT},
+	{ PCI_VDEVICE(INTEL, 0x1e56), LPC_PPT},
+	{ PCI_VDEVICE(INTEL, 0x1e57), LPC_PPT},
+	{ PCI_VDEVICE(INTEL, 0x1e58), LPC_PPT},
+	{ PCI_VDEVICE(INTEL, 0x1e59), LPC_PPT},
+	{ PCI_VDEVICE(INTEL, 0x1e5a), LPC_PPT},
+	{ PCI_VDEVICE(INTEL, 0x1e5b), LPC_PPT},
+	{ PCI_VDEVICE(INTEL, 0x1e5c), LPC_PPT},
+	{ PCI_VDEVICE(INTEL, 0x1e5d), LPC_PPT},
+	{ PCI_VDEVICE(INTEL, 0x1e5e), LPC_PPT},
+	{ PCI_VDEVICE(INTEL, 0x1e5f), LPC_PPT},
+	{ PCI_VDEVICE(INTEL, 0x8c40), LPC_LPT},
+	{ PCI_VDEVICE(INTEL, 0x8c41), LPC_LPT},
+	{ PCI_VDEVICE(INTEL, 0x8c42), LPC_LPT},
+	{ PCI_VDEVICE(INTEL, 0x8c43), LPC_LPT},
+	{ PCI_VDEVICE(INTEL, 0x8c44), LPC_LPT},
+	{ PCI_VDEVICE(INTEL, 0x8c45), LPC_LPT},
+	{ PCI_VDEVICE(INTEL, 0x8c46), LPC_LPT},
+	{ PCI_VDEVICE(INTEL, 0x8c47), LPC_LPT},
+	{ PCI_VDEVICE(INTEL, 0x8c48), LPC_LPT},
+	{ PCI_VDEVICE(INTEL, 0x8c49), LPC_LPT},
+	{ PCI_VDEVICE(INTEL, 0x8c4a), LPC_LPT},
+	{ PCI_VDEVICE(INTEL, 0x8c4b), LPC_LPT},
+	{ PCI_VDEVICE(INTEL, 0x8c4c), LPC_LPT},
+	{ PCI_VDEVICE(INTEL, 0x8c4d), LPC_LPT},
+	{ PCI_VDEVICE(INTEL, 0x8c4e), LPC_LPT},
+	{ PCI_VDEVICE(INTEL, 0x8c4f), LPC_LPT},
+	{ PCI_VDEVICE(INTEL, 0x8c50), LPC_LPT},
+	{ PCI_VDEVICE(INTEL, 0x8c51), LPC_LPT},
+	{ PCI_VDEVICE(INTEL, 0x8c52), LPC_LPT},
+	{ PCI_VDEVICE(INTEL, 0x8c53), LPC_LPT},
+	{ PCI_VDEVICE(INTEL, 0x8c54), LPC_LPT},
+	{ PCI_VDEVICE(INTEL, 0x8c55), LPC_LPT},
+	{ PCI_VDEVICE(INTEL, 0x8c56), LPC_LPT},
+	{ PCI_VDEVICE(INTEL, 0x8c57), LPC_LPT},
+	{ PCI_VDEVICE(INTEL, 0x8c58), LPC_LPT},
+	{ PCI_VDEVICE(INTEL, 0x8c59), LPC_LPT},
+	{ PCI_VDEVICE(INTEL, 0x8c5a), LPC_LPT},
+	{ PCI_VDEVICE(INTEL, 0x8c5b), LPC_LPT},
+	{ PCI_VDEVICE(INTEL, 0x8c5c), LPC_LPT},
+	{ PCI_VDEVICE(INTEL, 0x8c5d), LPC_LPT},
+	{ PCI_VDEVICE(INTEL, 0x8c5e), LPC_LPT},
+	{ PCI_VDEVICE(INTEL, 0x8c5f), LPC_LPT},
+	{ 0, },			/* End of list */
+};
+MODULE_DEVICE_TABLE(pci, lpc_ich_ids);
+
+static void lpc_ich_restore_config_space(struct pci_dev *dev)
+{
+	if (lpc_ich_acpi_save >= 0) {
+		pci_write_config_byte(dev, ACPICTRL, lpc_ich_acpi_save);
+		lpc_ich_acpi_save = -1;
+	}
+
+	if (lpc_ich_gpio_save >= 0) {
+		pci_write_config_byte(dev, GPIOCTRL, lpc_ich_gpio_save);
+		lpc_ich_gpio_save = -1;
+	}
+}
+
+static void __devinit lpc_ich_enable_acpi_space(struct pci_dev *dev)
+{
+	u8 reg_save;
+
+	pci_read_config_byte(dev, ACPICTRL, &reg_save);
+	pci_write_config_byte(dev, ACPICTRL, reg_save | 0x10);
+	lpc_ich_acpi_save = reg_save;
+}
+
+static void __devinit lpc_ich_enable_gpio_space(struct pci_dev *dev)
+{
+	u8 reg_save;
+
+	pci_read_config_byte(dev, GPIOCTRL, &reg_save);
+	pci_write_config_byte(dev, GPIOCTRL, reg_save | 0x10);
+	lpc_ich_gpio_save = reg_save;
+}
+
+static void __devinit lpc_ich_finalize_cell(struct mfd_cell *cell,
+					const struct pci_device_id *id)
+{
+	cell->platform_data = &lpc_chipset_info[id->driver_data];
+	cell->pdata_size = sizeof(struct lpc_ich_info);
+}
+
+static int __devinit lpc_ich_init_gpio(struct pci_dev *dev,
+				const struct pci_device_id *id)
+{
+	u32 base_addr_cfg;
+	u32 base_addr;
+	int ret;
+	bool acpi_conflict = false;
+	struct resource *res;
+
+	/* Setup power management base register */
+	pci_read_config_dword(dev, ACPIBASE, &base_addr_cfg);
+	base_addr = base_addr_cfg & 0x0000ff80;
+	if (!base_addr) {
+		dev_err(&dev->dev, "I/O space for ACPI uninitialized\n");
+		lpc_ich_cells[LPC_GPIO].num_resources--;
+		goto gpe0_done;
+	}
+
+	res = &gpio_ich_res[ICH_RES_GPE0];
+	res->start = base_addr + ACPIBASE_GPE_OFF;
+	res->end = base_addr + ACPIBASE_GPE_END;
+	ret = acpi_check_resource_conflict(res);
+	if (ret) {
+		/*
+		 * This isn't fatal for the GPIO, but we have to make sure that
+		 * the platform_device subsystem doesn't see this resource
+		 * or it will register an invalid region.
+		 */
+		lpc_ich_cells[LPC_GPIO].num_resources--;
+		acpi_conflict = true;
+	} else {
+		lpc_ich_enable_acpi_space(dev);
+	}
+
+gpe0_done:
+	/* Setup GPIO base register */
+	pci_read_config_dword(dev, GPIOBASE, &base_addr_cfg);
+	base_addr = base_addr_cfg & 0x0000ff80;
+	if (!base_addr) {
+		dev_err(&dev->dev, "I/O space for GPIO uninitialized\n");
+		ret = -ENODEV;
+		goto gpio_done;
+	}
+
+	/* Older devices provide fewer GPIO and have a smaller resource size. */
+	res = &gpio_ich_res[ICH_RES_GPIO];
+	res->start = base_addr;
+	switch (lpc_chipset_info[id->driver_data].gpio_version) {
+	case ICH_V5_GPIO:
+	case ICH_V10CORP_GPIO:
+		res->end = res->start + 128 - 1;
+		break;
+	default:
+		res->end = res->start + 64 - 1;
+		break;
+	}
+
+	ret = acpi_check_resource_conflict(res);
+	if (ret) {
+		/* this isn't necessarily fatal for the GPIO */
+		acpi_conflict = true;
+		goto gpio_done;
+	}
+	lpc_ich_enable_gpio_space(dev);
+
+	lpc_ich_finalize_cell(&lpc_ich_cells[LPC_GPIO], id);
+	ret = mfd_add_devices(&dev->dev, -1, &lpc_ich_cells[LPC_GPIO],
+				1, NULL, 0);
+
+gpio_done:
+	if (acpi_conflict)
+		pr_warn("Resource conflict(s) found affecting %s\n",
+				lpc_ich_cells[LPC_GPIO].name);
+	return ret;
+}
+
+static int __devinit lpc_ich_init_wdt(struct pci_dev *dev,
+				const struct pci_device_id *id)
+{
+	u32 base_addr_cfg;
+	u32 base_addr;
+	int ret;
+	bool acpi_conflict = false;
+	struct resource *res;
+
+	/* Setup power management base register */
+	pci_read_config_dword(dev, ACPIBASE, &base_addr_cfg);
+	base_addr = base_addr_cfg & 0x0000ff80;
+	if (!base_addr) {
+		dev_err(&dev->dev, "I/O space for ACPI uninitialized\n");
+		ret = -ENODEV;
+		goto wdt_done;
+	}
+
+	res = wdt_io_res(ICH_RES_IO_TCO);
+	res->start = base_addr + ACPIBASE_TCO_OFF;
+	res->end = base_addr + ACPIBASE_TCO_END;
+	ret = acpi_check_resource_conflict(res);
+	if (ret) {
+		acpi_conflict = true;
+		goto wdt_done;
+	}
+
+	res = wdt_io_res(ICH_RES_IO_SMI);
+	res->start = base_addr + ACPIBASE_SMI_OFF;
+	res->end = base_addr + ACPIBASE_SMI_END;
+	ret = acpi_check_resource_conflict(res);
+	if (ret) {
+		acpi_conflict = true;
+		goto wdt_done;
+	}
+	lpc_ich_enable_acpi_space(dev);
+
+	/*
+	 * Get the Memory-Mapped GCS register. To get access to it
+	 * we have to read RCBA from PCI Config space 0xf0 and use
+	 * it as base. GCS = RCBA + ICH6_GCS(0x3410).
+	 */
+	if (lpc_chipset_info[id->driver_data].iTCO_version == 2) {
+		pci_read_config_dword(dev, RCBABASE, &base_addr_cfg);
+		base_addr = base_addr_cfg & 0xffffc000;
+		if (!(base_addr_cfg & 1)) {
+			pr_err("RCBA is disabled by hardware/BIOS, "
+					"device disabled\n");
+			ret = -ENODEV;
+			goto wdt_done;
+		}
+		res = wdt_mem_res(ICH_RES_MEM_GCS);
+		res->start = base_addr + ACPIBASE_GCS_OFF;
+		res->end = base_addr + ACPIBASE_GCS_END;
+		ret = acpi_check_resource_conflict(res);
+		if (ret) {
+			acpi_conflict = true;
+			goto wdt_done;
+		}
+	}
+
+	lpc_ich_finalize_cell(&lpc_ich_cells[LPC_WDT], id);
+	ret = mfd_add_devices(&dev->dev, -1, &lpc_ich_cells[LPC_WDT],
+				1, NULL, 0);
+
+wdt_done:
+	if (acpi_conflict)
+		pr_warn("Resource conflict(s) found affecting %s\n",
+				lpc_ich_cells[LPC_WDT].name);
+	return ret;
+}
+
+static int __devinit lpc_ich_probe(struct pci_dev *dev,
+				const struct pci_device_id *id)
+{
+	int ret;
+	bool cell_added = false;
+
+	ret = lpc_ich_init_wdt(dev, id);
+	if (!ret)
+		cell_added = true;
+
+	ret = lpc_ich_init_gpio(dev, id);
+	if (!ret)
+		cell_added = true;
+
+	/*
+	 * We only care if at least one or none of the cells registered
+	 * successfully.
+	 */
+	if (!cell_added) {
+		lpc_ich_restore_config_space(dev);
+		return -ENODEV;
+	}
+
+	return 0;
+}
+
+static void __devexit lpc_ich_remove(struct pci_dev *dev)
+{
+	mfd_remove_devices(&dev->dev);
+	lpc_ich_restore_config_space(dev);
+}
+
+static struct pci_driver lpc_ich_driver = {
+	.name		= "lpc_ich",
+	.id_table	= lpc_ich_ids,
+	.probe		= lpc_ich_probe,
+	.remove		= __devexit_p(lpc_ich_remove),
+};
+
+static int __init lpc_ich_init(void)
+{
+	return pci_register_driver(&lpc_ich_driver);
+}
+
+static void __exit lpc_ich_exit(void)
+{
+	pci_unregister_driver(&lpc_ich_driver);
+}
+
+module_init(lpc_ich_init);
+module_exit(lpc_ich_exit);
+
+MODULE_AUTHOR("Aaron Sierra <asierra@xes-inc.com>");
+MODULE_DESCRIPTION("LPC interface for Intel ICH");
+MODULE_LICENSE("GPL");
diff --git a/drivers/mfd/lpc_sch.c b/drivers/mfd/lpc_sch.c
index abc421364a4..9f20abc5e39 100644
--- a/drivers/mfd/lpc_sch.c
+++ b/drivers/mfd/lpc_sch.c
@@ -36,6 +36,7 @@
 
 #define GPIOBASE	0x44
 #define GPIO_IO_SIZE	64
+#define GPIO_IO_SIZE_CENTERTON	128
 
 #define WDTBASE		0x84
 #define WDT_IO_SIZE	64
@@ -68,7 +69,7 @@ static struct resource wdt_sch_resource = {
 
 static struct mfd_cell tunnelcreek_cells[] = {
 	{
-		.name = "tunnelcreek_wdt",
+		.name = "ie6xx_wdt",
 		.num_resources = 1,
 		.resources = &wdt_sch_resource,
 	},
@@ -77,6 +78,7 @@ static struct mfd_cell tunnelcreek_cells[] = {
 static DEFINE_PCI_DEVICE_TABLE(lpc_sch_ids) = {
 	{ PCI_DEVICE(PCI_VENDOR_ID_INTEL, PCI_DEVICE_ID_INTEL_SCH_LPC) },
 	{ PCI_DEVICE(PCI_VENDOR_ID_INTEL, PCI_DEVICE_ID_INTEL_ITC_LPC) },
+	{ PCI_DEVICE(PCI_VENDOR_ID_INTEL, PCI_DEVICE_ID_INTEL_CENTERTON_ILB) },
 	{ 0, }
 };
 MODULE_DEVICE_TABLE(pci, lpc_sch_ids);
@@ -115,7 +117,11 @@ static int __devinit lpc_sch_probe(struct pci_dev *dev,
 	}
 
 	gpio_sch_resource.start = base_addr;
-	gpio_sch_resource.end = base_addr + GPIO_IO_SIZE - 1;
+
+	if (id->device == PCI_DEVICE_ID_INTEL_CENTERTON_ILB)
+		gpio_sch_resource.end = base_addr + GPIO_IO_SIZE_CENTERTON - 1;
+	else
+		gpio_sch_resource.end = base_addr + GPIO_IO_SIZE - 1;
 
 	for (i=0; i < ARRAY_SIZE(lpc_sch_cells); i++)
 		lpc_sch_cells[i].id = id->device;
@@ -125,7 +131,8 @@ static int __devinit lpc_sch_probe(struct pci_dev *dev,
 	if (ret)
 		goto out_dev;
 
-	if (id->device == PCI_DEVICE_ID_INTEL_ITC_LPC) {
+	if (id->device == PCI_DEVICE_ID_INTEL_ITC_LPC
+	 || id->device == PCI_DEVICE_ID_INTEL_CENTERTON_ILB) {
 		pci_read_config_dword(dev, WDTBASE, &base_addr_cfg);
 		if (!(base_addr_cfg & (1 << 31))) {
 			dev_err(&dev->dev, "Decode of the WDT I/O range disabled\n");
@@ -167,18 +174,7 @@ static struct pci_driver lpc_sch_driver = {
 	.remove		= __devexit_p(lpc_sch_remove),
 };
 
-static int __init lpc_sch_init(void)
-{
-	return pci_register_driver(&lpc_sch_driver);
-}
-
-static void __exit lpc_sch_exit(void)
-{
-	pci_unregister_driver(&lpc_sch_driver);
-}
-
-module_init(lpc_sch_init);
-module_exit(lpc_sch_exit);
+module_pci_driver(lpc_sch_driver);
 
 MODULE_AUTHOR("Denis Turischev <denis@compulab.co.il>");
 MODULE_DESCRIPTION("LPC interface for Intel Poulsbo SCH");
diff --git a/drivers/mfd/max77693-irq.c b/drivers/mfd/max77693-irq.c
new file mode 100644
index 00000000000..2b403569e0a
--- /dev/null
+++ b/drivers/mfd/max77693-irq.c
@@ -0,0 +1,309 @@
+/*
+ * max77693-irq.c - Interrupt controller support for MAX77693
+ *
+ * Copyright (C) 2012 Samsung Electronics Co.Ltd
+ * SangYoung Son <hello.son@samsung.com>
+ *
+ * This program is not provided / owned by Maxim Integrated Products.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
+ *
+ * This driver is based on max8997-irq.c
+ */
+
+#include <linux/err.h>
+#include <linux/irq.h>
+#include <linux/interrupt.h>
+#include <linux/module.h>
+#include <linux/irqdomain.h>
+#include <linux/mfd/max77693.h>
+#include <linux/mfd/max77693-private.h>
+
+static const u8 max77693_mask_reg[] = {
+	[LED_INT] = MAX77693_LED_REG_FLASH_INT_MASK,
+	[TOPSYS_INT] = MAX77693_PMIC_REG_TOPSYS_INT_MASK,
+	[CHG_INT] = MAX77693_CHG_REG_CHG_INT_MASK,
+	[MUIC_INT1] = MAX77693_MUIC_REG_INTMASK1,
+	[MUIC_INT2] = MAX77693_MUIC_REG_INTMASK2,
+	[MUIC_INT3] = MAX77693_MUIC_REG_INTMASK3,
+};
+
+static struct regmap *max77693_get_regmap(struct max77693_dev *max77693,
+				enum max77693_irq_source src)
+{
+	switch (src) {
+	case LED_INT ... CHG_INT:
+		return max77693->regmap;
+	case MUIC_INT1 ... MUIC_INT3:
+		return max77693->regmap_muic;
+	default:
+		return ERR_PTR(-EINVAL);
+	}
+}
+
+struct max77693_irq_data {
+	int mask;
+	enum max77693_irq_source group;
+};
+
+#define DECLARE_IRQ(idx, _group, _mask)		\
+	[(idx)] = { .group = (_group), .mask = (_mask) }
+static const struct max77693_irq_data max77693_irqs[] = {
+	DECLARE_IRQ(MAX77693_LED_IRQ_FLED2_OPEN,	LED_INT, 1 << 0),
+	DECLARE_IRQ(MAX77693_LED_IRQ_FLED2_SHORT,	LED_INT, 1 << 1),
+	DECLARE_IRQ(MAX77693_LED_IRQ_FLED1_OPEN,	LED_INT, 1 << 2),
+	DECLARE_IRQ(MAX77693_LED_IRQ_FLED1_SHORT,	LED_INT, 1 << 3),
+	DECLARE_IRQ(MAX77693_LED_IRQ_MAX_FLASH,		LED_INT, 1 << 4),
+
+	DECLARE_IRQ(MAX77693_TOPSYS_IRQ_T120C_INT,	TOPSYS_INT, 1 << 0),
+	DECLARE_IRQ(MAX77693_TOPSYS_IRQ_T140C_INT,	TOPSYS_INT, 1 << 1),
+	DECLARE_IRQ(MAX77693_TOPSYS_IRQ_LOWSYS_INT,	TOPSYS_INT, 1 << 3),
+
+	DECLARE_IRQ(MAX77693_CHG_IRQ_BYP_I,		CHG_INT, 1 << 0),
+	DECLARE_IRQ(MAX77693_CHG_IRQ_THM_I,		CHG_INT, 1 << 2),
+	DECLARE_IRQ(MAX77693_CHG_IRQ_BAT_I,		CHG_INT, 1 << 3),
+	DECLARE_IRQ(MAX77693_CHG_IRQ_CHG_I,		CHG_INT, 1 << 4),
+	DECLARE_IRQ(MAX77693_CHG_IRQ_CHGIN_I,		CHG_INT, 1 << 6),
+
+	DECLARE_IRQ(MAX77693_MUIC_IRQ_INT1_ADC,		MUIC_INT1, 1 << 0),
+	DECLARE_IRQ(MAX77693_MUIC_IRQ_INT1_ADC_LOW,	MUIC_INT1, 1 << 1),
+	DECLARE_IRQ(MAX77693_MUIC_IRQ_INT1_ADC_ERR,	MUIC_INT1, 1 << 2),
+	DECLARE_IRQ(MAX77693_MUIC_IRQ_INT1_ADC1K,	MUIC_INT1, 1 << 3),
+
+	DECLARE_IRQ(MAX77693_MUIC_IRQ_INT2_CHGTYP,	MUIC_INT2, 1 << 0),
+	DECLARE_IRQ(MAX77693_MUIC_IRQ_INT2_CHGDETREUN,	MUIC_INT2, 1 << 1),
+	DECLARE_IRQ(MAX77693_MUIC_IRQ_INT2_DCDTMR,	MUIC_INT2, 1 << 2),
+	DECLARE_IRQ(MAX77693_MUIC_IRQ_INT2_DXOVP,	MUIC_INT2, 1 << 3),
+	DECLARE_IRQ(MAX77693_MUIC_IRQ_INT2_VBVOLT,	MUIC_INT2, 1 << 4),
+	DECLARE_IRQ(MAX77693_MUIC_IRQ_INT2_VIDRM,	MUIC_INT2, 1 << 5),
+
+	DECLARE_IRQ(MAX77693_MUIC_IRQ_INT3_EOC,		MUIC_INT3, 1 << 0),
+	DECLARE_IRQ(MAX77693_MUIC_IRQ_INT3_CGMBC,	MUIC_INT3, 1 << 1),
+	DECLARE_IRQ(MAX77693_MUIC_IRQ_INT3_OVP,		MUIC_INT3, 1 << 2),
+	DECLARE_IRQ(MAX77693_MUIC_IRQ_INT3_MBCCHG_ERR,	MUIC_INT3, 1 << 3),
+	DECLARE_IRQ(MAX77693_MUIC_IRQ_INT3_CHG_ENABLED,	MUIC_INT3, 1 << 4),
+	DECLARE_IRQ(MAX77693_MUIC_IRQ_INT3_BAT_DET,	MUIC_INT3, 1 << 5),
+};
+
+static void max77693_irq_lock(struct irq_data *data)
+{
+	struct max77693_dev *max77693 = irq_get_chip_data(data->irq);
+
+	mutex_lock(&max77693->irqlock);
+}
+
+static void max77693_irq_sync_unlock(struct irq_data *data)
+{
+	struct max77693_dev *max77693 = irq_get_chip_data(data->irq);
+	int i;
+
+	for (i = 0; i < MAX77693_IRQ_GROUP_NR; i++) {
+		u8 mask_reg = max77693_mask_reg[i];
+		struct regmap *map = max77693_get_regmap(max77693, i);
+
+		if (mask_reg == MAX77693_REG_INVALID ||
+				IS_ERR_OR_NULL(map))
+			continue;
+		max77693->irq_masks_cache[i] = max77693->irq_masks_cur[i];
+
+		max77693_write_reg(map, max77693_mask_reg[i],
+				max77693->irq_masks_cur[i]);
+	}
+
+	mutex_unlock(&max77693->irqlock);
+}
+
+static const inline struct max77693_irq_data *
+irq_to_max77693_irq(struct max77693_dev *max77693, int irq)
+{
+	return &max77693_irqs[irq];
+}
+
+static void max77693_irq_mask(struct irq_data *data)
+{
+	struct max77693_dev *max77693 = irq_get_chip_data(data->irq);
+	const struct max77693_irq_data *irq_data =
+				irq_to_max77693_irq(max77693, data->irq);
+
+	if (irq_data->group >= MUIC_INT1 && irq_data->group <= MUIC_INT3)
+		max77693->irq_masks_cur[irq_data->group] &= ~irq_data->mask;
+	else
+		max77693->irq_masks_cur[irq_data->group] |= irq_data->mask;
+}
+
+static void max77693_irq_unmask(struct irq_data *data)
+{
+	struct max77693_dev *max77693 = irq_get_chip_data(data->irq);
+	const struct max77693_irq_data *irq_data =
+	    irq_to_max77693_irq(max77693, data->irq);
+
+	if (irq_data->group >= MUIC_INT1 && irq_data->group <= MUIC_INT3)
+		max77693->irq_masks_cur[irq_data->group] |= irq_data->mask;
+	else
+		max77693->irq_masks_cur[irq_data->group] &= ~irq_data->mask;
+}
+
+static struct irq_chip max77693_irq_chip = {
+	.name			= "max77693",
+	.irq_bus_lock		= max77693_irq_lock,
+	.irq_bus_sync_unlock	= max77693_irq_sync_unlock,
+	.irq_mask		= max77693_irq_mask,
+	.irq_unmask		= max77693_irq_unmask,
+};
+
+#define MAX77693_IRQSRC_CHG		(1 << 0)
+#define MAX77693_IRQSRC_TOP		(1 << 1)
+#define MAX77693_IRQSRC_FLASH		(1 << 2)
+#define MAX77693_IRQSRC_MUIC		(1 << 3)
+static irqreturn_t max77693_irq_thread(int irq, void *data)
+{
+	struct max77693_dev *max77693 = data;
+	u8 irq_reg[MAX77693_IRQ_GROUP_NR] = {};
+	u8 irq_src;
+	int ret;
+	int i, cur_irq;
+
+	ret = max77693_read_reg(max77693->regmap, MAX77693_PMIC_REG_INTSRC,
+				&irq_src);
+	if (ret < 0) {
+		dev_err(max77693->dev, "Failed to read interrupt source: %d\n",
+				ret);
+		return IRQ_NONE;
+	}
+
+	if (irq_src & MAX77693_IRQSRC_CHG)
+		/* CHG_INT */
+		ret = max77693_read_reg(max77693->regmap, MAX77693_CHG_REG_CHG_INT,
+				&irq_reg[CHG_INT]);
+
+	if (irq_src & MAX77693_IRQSRC_TOP)
+		/* TOPSYS_INT */
+		ret = max77693_read_reg(max77693->regmap,
+			MAX77693_PMIC_REG_TOPSYS_INT, &irq_reg[TOPSYS_INT]);
+
+	if (irq_src & MAX77693_IRQSRC_FLASH)
+		/* LED_INT */
+		ret = max77693_read_reg(max77693->regmap,
+			MAX77693_LED_REG_FLASH_INT, &irq_reg[LED_INT]);
+
+	if (irq_src & MAX77693_IRQSRC_MUIC)
+		/* MUIC INT1 ~ INT3 */
+		max77693_bulk_read(max77693->regmap, MAX77693_MUIC_REG_INT1,
+			MAX77693_NUM_IRQ_MUIC_REGS, &irq_reg[MUIC_INT1]);
+
+	/* Apply masking */
+	for (i = 0; i < MAX77693_IRQ_GROUP_NR; i++) {
+		if (i >= MUIC_INT1 && i <= MUIC_INT3)
+			irq_reg[i] &= max77693->irq_masks_cur[i];
+		else
+			irq_reg[i] &= ~max77693->irq_masks_cur[i];
+	}
+
+	/* Report */
+	for (i = 0; i < MAX77693_IRQ_NR; i++) {
+		if (irq_reg[max77693_irqs[i].group] & max77693_irqs[i].mask) {
+			cur_irq = irq_find_mapping(max77693->irq_domain, i);
+			if (cur_irq)
+				handle_nested_irq(cur_irq);
+		}
+	}
+
+	return IRQ_HANDLED;
+}
+
+int max77693_irq_resume(struct max77693_dev *max77693)
+{
+	if (max77693->irq)
+		max77693_irq_thread(0, max77693);
+
+	return 0;
+}
+
+static int max77693_irq_domain_map(struct irq_domain *d, unsigned int irq,
+				irq_hw_number_t hw)
+{
+	struct max77693_dev *max77693 = d->host_data;
+
+	irq_set_chip_data(irq, max77693);
+	irq_set_chip_and_handler(irq, &max77693_irq_chip, handle_edge_irq);
+	irq_set_nested_thread(irq, 1);
+#ifdef CONFIG_ARM
+	set_irq_flags(irq, IRQF_VALID);
+#else
+	irq_set_noprobe(irq);
+#endif
+	return 0;
+}
+
+static struct irq_domain_ops max77693_irq_domain_ops = {
+	.map = max77693_irq_domain_map,
+};
+
+int max77693_irq_init(struct max77693_dev *max77693)
+{
+	struct irq_domain *domain;
+	int i;
+	int ret;
+
+	mutex_init(&max77693->irqlock);
+
+	/* Mask individual interrupt sources */
+	for (i = 0; i < MAX77693_IRQ_GROUP_NR; i++) {
+		struct regmap *map;
+		/* MUIC IRQ  0:MASK 1:NOT MASK */
+		/* Other IRQ 1:MASK 0:NOT MASK */
+		if (i >= MUIC_INT1 && i <= MUIC_INT3) {
+			max77693->irq_masks_cur[i] = 0x00;
+			max77693->irq_masks_cache[i] = 0x00;
+		} else {
+			max77693->irq_masks_cur[i] = 0xff;
+			max77693->irq_masks_cache[i] = 0xff;
+		}
+		map = max77693_get_regmap(max77693, i);
+
+		if (IS_ERR_OR_NULL(map))
+			continue;
+		if (max77693_mask_reg[i] == MAX77693_REG_INVALID)
+			continue;
+		if (i >= MUIC_INT1 && i <= MUIC_INT3)
+			max77693_write_reg(map, max77693_mask_reg[i], 0x00);
+		else
+			max77693_write_reg(map, max77693_mask_reg[i], 0xff);
+	}
+
+	domain = irq_domain_add_linear(NULL, MAX77693_IRQ_NR,
+					&max77693_irq_domain_ops, max77693);
+	if (!domain) {
+		dev_err(max77693->dev, "could not create irq domain\n");
+		return -ENODEV;
+	}
+	max77693->irq_domain = domain;
+
+	ret = request_threaded_irq(max77693->irq, NULL, max77693_irq_thread,
+				   IRQF_TRIGGER_FALLING | IRQF_ONESHOT,
+				   "max77693-irq", max77693);
+
+	if (ret)
+		dev_err(max77693->dev, "Failed to request IRQ %d: %d\n",
+			max77693->irq, ret);
+
+	return 0;
+}
+
+void max77693_irq_exit(struct max77693_dev *max77693)
+{
+	if (max77693->irq)
+		free_irq(max77693->irq, max77693);
+}
diff --git a/drivers/mfd/max77693.c b/drivers/mfd/max77693.c
new file mode 100644
index 00000000000..e9e4278722f
--- /dev/null
+++ b/drivers/mfd/max77693.c
@@ -0,0 +1,249 @@
+/*
+ * max77693.c - mfd core driver for the MAX 77693
+ *
+ * Copyright (C) 2012 Samsung Electronics
+ * SangYoung Son <hello.son@smasung.com>
+ *
+ * This program is not provided / owned by Maxim Integrated Products.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
+ *
+ * This driver is based on max8997.c
+ */
+
+#include <linux/module.h>
+#include <linux/slab.h>
+#include <linux/i2c.h>
+#include <linux/err.h>
+#include <linux/interrupt.h>
+#include <linux/pm_runtime.h>
+#include <linux/mutex.h>
+#include <linux/mfd/core.h>
+#include <linux/mfd/max77693.h>
+#include <linux/mfd/max77693-private.h>
+#include <linux/regulator/machine.h>
+#include <linux/regmap.h>
+
+#define I2C_ADDR_PMIC	(0xCC >> 1)	/* Charger, Flash LED */
+#define I2C_ADDR_MUIC	(0x4A >> 1)
+#define I2C_ADDR_HAPTIC	(0x90 >> 1)
+
+static struct mfd_cell max77693_devs[] = {
+	{ .name = "max77693-pmic", },
+	{ .name = "max77693-charger", },
+	{ .name = "max77693-flash", },
+	{ .name = "max77693-muic", },
+	{ .name = "max77693-haptic", },
+};
+
+int max77693_read_reg(struct regmap *map, u8 reg, u8 *dest)
+{
+	unsigned int val;
+	int ret;
+
+	ret = regmap_read(map, reg, &val);
+	*dest = val;
+
+	return ret;
+}
+EXPORT_SYMBOL_GPL(max77693_read_reg);
+
+int max77693_bulk_read(struct regmap *map, u8 reg, int count, u8 *buf)
+{
+	int ret;
+
+	ret = regmap_bulk_read(map, reg, buf, count);
+
+	return ret;
+}
+EXPORT_SYMBOL_GPL(max77693_bulk_read);
+
+int max77693_write_reg(struct regmap *map, u8 reg, u8 value)
+{
+	int ret;
+
+	ret = regmap_write(map, reg, value);
+
+	return ret;
+}
+EXPORT_SYMBOL_GPL(max77693_write_reg);
+
+int max77693_bulk_write(struct regmap *map, u8 reg, int count, u8 *buf)
+{
+	int ret;
+
+	ret = regmap_bulk_write(map, reg, buf, count);
+
+	return ret;
+}
+EXPORT_SYMBOL_GPL(max77693_bulk_write);
+
+int max77693_update_reg(struct regmap *map, u8 reg, u8 val, u8 mask)
+{
+	int ret;
+
+	ret = regmap_update_bits(map, reg, mask, val);
+
+	return ret;
+}
+EXPORT_SYMBOL_GPL(max77693_update_reg);
+
+static const struct regmap_config max77693_regmap_config = {
+	.reg_bits = 8,
+	.val_bits = 8,
+	.max_register = MAX77693_PMIC_REG_END,
+};
+
+static int max77693_i2c_probe(struct i2c_client *i2c,
+			      const struct i2c_device_id *id)
+{
+	struct max77693_dev *max77693;
+	struct max77693_platform_data *pdata = i2c->dev.platform_data;
+	u8 reg_data;
+	int ret = 0;
+
+	max77693 = devm_kzalloc(&i2c->dev,
+			sizeof(struct max77693_dev), GFP_KERNEL);
+	if (max77693 == NULL)
+		return -ENOMEM;
+
+	max77693->regmap = devm_regmap_init_i2c(i2c, &max77693_regmap_config);
+	if (IS_ERR(max77693->regmap)) {
+		ret = PTR_ERR(max77693->regmap);
+		dev_err(max77693->dev,"failed to allocate register map: %d\n",
+				ret);
+		goto err_regmap;
+	}
+
+	i2c_set_clientdata(i2c, max77693);
+	max77693->dev = &i2c->dev;
+	max77693->i2c = i2c;
+	max77693->irq = i2c->irq;
+	max77693->type = id->driver_data;
+
+	if (!pdata)
+		goto err_regmap;
+
+	max77693->wakeup = pdata->wakeup;
+
+	mutex_init(&max77693->iolock);
+
+	if (max77693_read_reg(max77693->regmap,
+				MAX77693_PMIC_REG_PMIC_ID2, &reg_data) < 0) {
+		dev_err(max77693->dev, "device not found on this channel\n");
+		ret = -ENODEV;
+		goto err_regmap;
+	} else
+		dev_info(max77693->dev, "device ID: 0x%x\n", reg_data);
+
+	max77693->muic = i2c_new_dummy(i2c->adapter, I2C_ADDR_MUIC);
+	i2c_set_clientdata(max77693->muic, max77693);
+
+	max77693->haptic = i2c_new_dummy(i2c->adapter, I2C_ADDR_HAPTIC);
+	i2c_set_clientdata(max77693->haptic, max77693);
+
+	ret = max77693_irq_init(max77693);
+	if (ret < 0)
+		goto err_mfd;
+
+	pm_runtime_set_active(max77693->dev);
+
+	ret = mfd_add_devices(max77693->dev, -1, max77693_devs,
+			ARRAY_SIZE(max77693_devs), NULL, 0);
+	if (ret < 0)
+		goto err_mfd;
+
+	device_init_wakeup(max77693->dev, pdata->wakeup);
+
+	return ret;
+
+err_mfd:
+	i2c_unregister_device(max77693->muic);
+	i2c_unregister_device(max77693->haptic);
+err_regmap:
+	kfree(max77693);
+
+	return ret;
+}
+
+static int max77693_i2c_remove(struct i2c_client *i2c)
+{
+	struct max77693_dev *max77693 = i2c_get_clientdata(i2c);
+
+	mfd_remove_devices(max77693->dev);
+	i2c_unregister_device(max77693->muic);
+	i2c_unregister_device(max77693->haptic);
+
+	return 0;
+}
+
+static const struct i2c_device_id max77693_i2c_id[] = {
+	{ "max77693", TYPE_MAX77693 },
+	{ }
+};
+MODULE_DEVICE_TABLE(i2c, max77693_i2c_id);
+
+static int max77693_suspend(struct device *dev)
+{
+	struct i2c_client *i2c = container_of(dev, struct i2c_client, dev);
+	struct max77693_dev *max77693 = i2c_get_clientdata(i2c);
+
+	if (device_may_wakeup(dev))
+		irq_set_irq_wake(max77693->irq, 1);
+	return 0;
+}
+
+static int max77693_resume(struct device *dev)
+{
+	struct i2c_client *i2c = container_of(dev, struct i2c_client, dev);
+	struct max77693_dev *max77693 = i2c_get_clientdata(i2c);
+
+	if (device_may_wakeup(dev))
+		irq_set_irq_wake(max77693->irq, 0);
+	return max77693_irq_resume(max77693);
+}
+
+const struct dev_pm_ops max77693_pm = {
+	.suspend = max77693_suspend,
+	.resume = max77693_resume,
+};
+
+static struct i2c_driver max77693_i2c_driver = {
+	.driver = {
+		   .name = "max77693",
+		   .owner = THIS_MODULE,
+		   .pm = &max77693_pm,
+	},
+	.probe = max77693_i2c_probe,
+	.remove = max77693_i2c_remove,
+	.id_table = max77693_i2c_id,
+};
+
+static int __init max77693_i2c_init(void)
+{
+	return i2c_add_driver(&max77693_i2c_driver);
+}
+/* init early so consumer devices can complete system boot */
+subsys_initcall(max77693_i2c_init);
+
+static void __exit max77693_i2c_exit(void)
+{
+	i2c_del_driver(&max77693_i2c_driver);
+}
+module_exit(max77693_i2c_exit);
+
+MODULE_DESCRIPTION("MAXIM 77693 multi-function core driver");
+MODULE_AUTHOR("SangYoung, Son <hello.son@samsung.com>");
+MODULE_LICENSE("GPL");
diff --git a/drivers/mfd/mc13xxx-core.c b/drivers/mfd/mc13xxx-core.c
index 9fd4f63c45c..f0ea3b8b3e4 100644
--- a/drivers/mfd/mc13xxx-core.c
+++ b/drivers/mfd/mc13xxx-core.c
@@ -15,24 +15,13 @@
 #include <linux/platform_device.h>
 #include <linux/mutex.h>
 #include <linux/interrupt.h>
-#include <linux/spi/spi.h>
 #include <linux/mfd/core.h>
 #include <linux/mfd/mc13xxx.h>
 #include <linux/of.h>
 #include <linux/of_device.h>
 #include <linux/of_gpio.h>
 
-struct mc13xxx {
-	struct spi_device *spidev;
-	struct mutex lock;
-	int irq;
-	int flags;
-
-	irq_handler_t irqhandler[MC13XXX_NUM_IRQ];
-	void *irqdata[MC13XXX_NUM_IRQ];
-
-	int adcflags;
-};
+#include "mc13xxx.h"
 
 #define MC13XXX_IRQSTAT0	0
 #define MC13XXX_IRQSTAT0_ADCDONEI	(1 << 0)
@@ -139,34 +128,29 @@ struct mc13xxx {
 
 #define MC13XXX_ADC2		45
 
-#define MC13XXX_NUMREGS 0x3f
-
 void mc13xxx_lock(struct mc13xxx *mc13xxx)
 {
 	if (!mutex_trylock(&mc13xxx->lock)) {
-		dev_dbg(&mc13xxx->spidev->dev, "wait for %s from %pf\n",
+		dev_dbg(mc13xxx->dev, "wait for %s from %pf\n",
 				__func__, __builtin_return_address(0));
 
 		mutex_lock(&mc13xxx->lock);
 	}
-	dev_dbg(&mc13xxx->spidev->dev, "%s from %pf\n",
+	dev_dbg(mc13xxx->dev, "%s from %pf\n",
 			__func__, __builtin_return_address(0));
 }
 EXPORT_SYMBOL(mc13xxx_lock);
 
 void mc13xxx_unlock(struct mc13xxx *mc13xxx)
 {
-	dev_dbg(&mc13xxx->spidev->dev, "%s from %pf\n",
+	dev_dbg(mc13xxx->dev, "%s from %pf\n",
 			__func__, __builtin_return_address(0));
 	mutex_unlock(&mc13xxx->lock);
 }
 EXPORT_SYMBOL(mc13xxx_unlock);
 
-#define MC13XXX_REGOFFSET_SHIFT 25
 int mc13xxx_reg_read(struct mc13xxx *mc13xxx, unsigned int offset, u32 *val)
 {
-	struct spi_transfer t;
-	struct spi_message m;
 	int ret;
 
 	BUG_ON(!mutex_is_locked(&mc13xxx->lock));
@@ -174,84 +158,35 @@ int mc13xxx_reg_read(struct mc13xxx *mc13xxx, unsigned int offset, u32 *val)
 	if (offset > MC13XXX_NUMREGS)
 		return -EINVAL;
 
-	*val = offset << MC13XXX_REGOFFSET_SHIFT;
-
-	memset(&t, 0, sizeof(t));
-
-	t.tx_buf = val;
-	t.rx_buf = val;
-	t.len = sizeof(u32);
-
-	spi_message_init(&m);
-	spi_message_add_tail(&t, &m);
-
-	ret = spi_sync(mc13xxx->spidev, &m);
-
-	/* error in message.status implies error return from spi_sync */
-	BUG_ON(!ret && m.status);
+	ret = regmap_read(mc13xxx->regmap, offset, val);
+	dev_vdbg(mc13xxx->dev, "[0x%02x] -> 0x%06x\n", offset, *val);
 
-	if (ret)
-		return ret;
-
-	*val &= 0xffffff;
-
-	dev_vdbg(&mc13xxx->spidev->dev, "[0x%02x] -> 0x%06x\n", offset, *val);
-
-	return 0;
+	return ret;
 }
 EXPORT_SYMBOL(mc13xxx_reg_read);
 
 int mc13xxx_reg_write(struct mc13xxx *mc13xxx, unsigned int offset, u32 val)
 {
-	u32 buf;
-	struct spi_transfer t;
-	struct spi_message m;
-	int ret;
-
 	BUG_ON(!mutex_is_locked(&mc13xxx->lock));
 
-	dev_vdbg(&mc13xxx->spidev->dev, "[0x%02x] <- 0x%06x\n", offset, val);
+	dev_vdbg(mc13xxx->dev, "[0x%02x] <- 0x%06x\n", offset, val);
 
 	if (offset > MC13XXX_NUMREGS || val > 0xffffff)
 		return -EINVAL;
 
-	buf = 1 << 31 | offset << MC13XXX_REGOFFSET_SHIFT | val;
-
-	memset(&t, 0, sizeof(t));
-
-	t.tx_buf = &buf;
-	t.rx_buf = &buf;
-	t.len = sizeof(u32);
-
-	spi_message_init(&m);
-	spi_message_add_tail(&t, &m);
-
-	ret = spi_sync(mc13xxx->spidev, &m);
-
-	BUG_ON(!ret && m.status);
-
-	if (ret)
-		return ret;
-
-	return 0;
+	return regmap_write(mc13xxx->regmap, offset, val);
 }
 EXPORT_SYMBOL(mc13xxx_reg_write);
 
 int mc13xxx_reg_rmw(struct mc13xxx *mc13xxx, unsigned int offset,
 		u32 mask, u32 val)
 {
-	int ret;
-	u32 valread;
-
+	BUG_ON(!mutex_is_locked(&mc13xxx->lock));
 	BUG_ON(val & ~mask);
+	dev_vdbg(mc13xxx->dev, "[0x%02x] <- 0x%06x (mask: 0x%06x)\n",
+			offset, val, mask);
 
-	ret = mc13xxx_reg_read(mc13xxx, offset, &valread);
-	if (ret)
-		return ret;
-
-	valread = (valread & ~mask) | val;
-
-	return mc13xxx_reg_write(mc13xxx, offset, valread);
+	return regmap_update_bits(mc13xxx->regmap, offset, mask, val);
 }
 EXPORT_SYMBOL(mc13xxx_reg_rmw);
 
@@ -439,7 +374,7 @@ static int mc13xxx_irq_handle(struct mc13xxx *mc13xxx,
 			if (handled == IRQ_HANDLED)
 				num_handled++;
 		} else {
-			dev_err(&mc13xxx->spidev->dev,
+			dev_err(mc13xxx->dev,
 					"BUG: irq %u but no handler\n",
 					baseirq + irq);
 
@@ -475,25 +410,23 @@ static irqreturn_t mc13xxx_irq_thread(int irq, void *data)
 	return IRQ_RETVAL(handled);
 }
 
-enum mc13xxx_id {
-	MC13XXX_ID_MC13783,
-	MC13XXX_ID_MC13892,
-	MC13XXX_ID_INVALID,
-};
-
 static const char *mc13xxx_chipname[] = {
 	[MC13XXX_ID_MC13783] = "mc13783",
 	[MC13XXX_ID_MC13892] = "mc13892",
 };
 
 #define maskval(reg, mask)	(((reg) & (mask)) >> __ffs(mask))
-static int mc13xxx_identify(struct mc13xxx *mc13xxx, enum mc13xxx_id *id)
+static int mc13xxx_identify(struct mc13xxx *mc13xxx)
 {
 	u32 icid;
 	u32 revision;
-	const char *name;
 	int ret;
 
+	/*
+	 * Get the generation ID from register 46, as apparently some older
+	 * IC revisions only have this info at this location. Newer ICs seem to
+	 * have both.
+	 */
 	ret = mc13xxx_reg_read(mc13xxx, 46, &icid);
 	if (ret)
 		return ret;
@@ -502,26 +435,23 @@ static int mc13xxx_identify(struct mc13xxx *mc13xxx, enum mc13xxx_id *id)
 
 	switch (icid) {
 	case 2:
-		*id = MC13XXX_ID_MC13783;
-		name = "mc13783";
+		mc13xxx->ictype = MC13XXX_ID_MC13783;
 		break;
 	case 7:
-		*id = MC13XXX_ID_MC13892;
-		name = "mc13892";
+		mc13xxx->ictype = MC13XXX_ID_MC13892;
 		break;
 	default:
-		*id = MC13XXX_ID_INVALID;
+		mc13xxx->ictype = MC13XXX_ID_INVALID;
 		break;
 	}
 
-	if (*id == MC13XXX_ID_MC13783 || *id == MC13XXX_ID_MC13892) {
+	if (mc13xxx->ictype == MC13XXX_ID_MC13783 ||
+			mc13xxx->ictype == MC13XXX_ID_MC13892) {
 		ret = mc13xxx_reg_read(mc13xxx, MC13XXX_REVISION, &revision);
-		if (ret)
-			return ret;
 
-		dev_info(&mc13xxx->spidev->dev, "%s: rev: %d.%d, "
+		dev_info(mc13xxx->dev, "%s: rev: %d.%d, "
 				"fin: %d, fab: %d, icid: %d/%d\n",
-				mc13xxx_chipname[*id],
+				mc13xxx_chipname[mc13xxx->ictype],
 				maskval(revision, MC13XXX_REVISION_REVFULL),
 				maskval(revision, MC13XXX_REVISION_REVMETAL),
 				maskval(revision, MC13XXX_REVISION_FIN),
@@ -530,26 +460,12 @@ static int mc13xxx_identify(struct mc13xxx *mc13xxx, enum mc13xxx_id *id)
 				maskval(revision, MC13XXX_REVISION_ICIDCODE));
 	}
 
-	if (*id != MC13XXX_ID_INVALID) {
-		const struct spi_device_id *devid =
-			spi_get_device_id(mc13xxx->spidev);
-		if (!devid || devid->driver_data != *id)
-			dev_warn(&mc13xxx->spidev->dev, "device id doesn't "
-					"match auto detection!\n");
-	}
-
-	return 0;
+	return (mc13xxx->ictype == MC13XXX_ID_INVALID) ? -ENODEV : 0;
 }
 
 static const char *mc13xxx_get_chipname(struct mc13xxx *mc13xxx)
 {
-	const struct spi_device_id *devid =
-		spi_get_device_id(mc13xxx->spidev);
-
-	if (!devid)
-		return NULL;
-
-	return mc13xxx_chipname[devid->driver_data];
+	return mc13xxx_chipname[mc13xxx->ictype];
 }
 
 int mc13xxx_get_flags(struct mc13xxx *mc13xxx)
@@ -592,7 +508,7 @@ int mc13xxx_adc_do_conversion(struct mc13xxx *mc13xxx, unsigned int mode,
 	};
 	init_completion(&adcdone_data.done);
 
-	dev_dbg(&mc13xxx->spidev->dev, "%s\n", __func__);
+	dev_dbg(mc13xxx->dev, "%s\n", __func__);
 
 	mc13xxx_lock(mc13xxx);
 
@@ -637,7 +553,8 @@ int mc13xxx_adc_do_conversion(struct mc13xxx *mc13xxx, unsigned int mode,
 	adc1 |= ato << MC13783_ADC1_ATO_SHIFT;
 	if (atox)
 		adc1 |= MC13783_ADC1_ATOX;
-	dev_dbg(&mc13xxx->spidev->dev, "%s: request irq\n", __func__);
+
+	dev_dbg(mc13xxx->dev, "%s: request irq\n", __func__);
 	mc13xxx_irq_request(mc13xxx, MC13XXX_IRQ_ADCDONE,
 			mc13xxx_handler_adcdone, __func__, &adcdone_data);
 	mc13xxx_irq_ack(mc13xxx, MC13XXX_IRQ_ADCDONE);
@@ -695,7 +612,7 @@ static int mc13xxx_add_subdevice_pdata(struct mc13xxx *mc13xxx,
 	if (!cell.name)
 		return -ENOMEM;
 
-	return mfd_add_devices(&mc13xxx->spidev->dev, -1, &cell, 1, NULL, 0);
+	return mfd_add_devices(mc13xxx->dev, -1, &cell, 1, NULL, 0);
 }
 
 static int mc13xxx_add_subdevice(struct mc13xxx *mc13xxx, const char *format)
@@ -706,7 +623,7 @@ static int mc13xxx_add_subdevice(struct mc13xxx *mc13xxx, const char *format)
 #ifdef CONFIG_OF
 static int mc13xxx_probe_flags_dt(struct mc13xxx *mc13xxx)
 {
-	struct device_node *np = mc13xxx->spidev->dev.of_node;
+	struct device_node *np = mc13xxx->dev->of_node;
 
 	if (!np)
 		return -ENODEV;
@@ -732,55 +649,15 @@ static inline int mc13xxx_probe_flags_dt(struct mc13xxx *mc13xxx)
 }
 #endif
 
-static const struct spi_device_id mc13xxx_device_id[] = {
-	{
-		.name = "mc13783",
-		.driver_data = MC13XXX_ID_MC13783,
-	}, {
-		.name = "mc13892",
-		.driver_data = MC13XXX_ID_MC13892,
-	}, {
-		/* sentinel */
-	}
-};
-MODULE_DEVICE_TABLE(spi, mc13xxx_device_id);
-
-static const struct of_device_id mc13xxx_dt_ids[] = {
-	{ .compatible = "fsl,mc13783", .data = (void *) MC13XXX_ID_MC13783, },
-	{ .compatible = "fsl,mc13892", .data = (void *) MC13XXX_ID_MC13892, },
-	{ /* sentinel */ }
-};
-MODULE_DEVICE_TABLE(of, mc13xxx_dt_ids);
-
-static int mc13xxx_probe(struct spi_device *spi)
+int mc13xxx_common_init(struct mc13xxx *mc13xxx,
+		struct mc13xxx_platform_data *pdata, int irq)
 {
-	const struct of_device_id *of_id;
-	struct spi_driver *sdrv = to_spi_driver(spi->dev.driver);
-	struct mc13xxx *mc13xxx;
-	struct mc13xxx_platform_data *pdata = dev_get_platdata(&spi->dev);
-	enum mc13xxx_id id;
 	int ret;
 
-	of_id = of_match_device(mc13xxx_dt_ids, &spi->dev);
-	if (of_id)
-		sdrv->id_table = &mc13xxx_device_id[(enum mc13xxx_id) of_id->data];
-
-	mc13xxx = kzalloc(sizeof(*mc13xxx), GFP_KERNEL);
-	if (!mc13xxx)
-		return -ENOMEM;
-
-	dev_set_drvdata(&spi->dev, mc13xxx);
-	spi->mode = SPI_MODE_0 | SPI_CS_HIGH;
-	spi->bits_per_word = 32;
-	spi_setup(spi);
-
-	mc13xxx->spidev = spi;
-
-	mutex_init(&mc13xxx->lock);
 	mc13xxx_lock(mc13xxx);
 
-	ret = mc13xxx_identify(mc13xxx, &id);
-	if (ret || id == MC13XXX_ID_INVALID)
+	ret = mc13xxx_identify(mc13xxx);
+	if (ret)
 		goto err_revision;
 
 	/* mask all irqs */
@@ -792,18 +669,19 @@ static int mc13xxx_probe(struct spi_device *spi)
 	if (ret)
 		goto err_mask;
 
-	ret = request_threaded_irq(spi->irq, NULL, mc13xxx_irq_thread,
+	ret = request_threaded_irq(irq, NULL, mc13xxx_irq_thread,
 			IRQF_ONESHOT | IRQF_TRIGGER_HIGH, "mc13xxx", mc13xxx);
 
 	if (ret) {
 err_mask:
 err_revision:
 		mc13xxx_unlock(mc13xxx);
-		dev_set_drvdata(&spi->dev, NULL);
 		kfree(mc13xxx);
 		return ret;
 	}
 
+	mc13xxx->irq = irq;
+
 	mc13xxx_unlock(mc13xxx);
 
 	if (mc13xxx_probe_flags_dt(mc13xxx) < 0 && pdata)
@@ -813,7 +691,8 @@ err_revision:
 		mc13xxx_add_subdevice(mc13xxx, "%s-adc");
 
 	if (mc13xxx->flags & MC13XXX_USE_CODEC)
-		mc13xxx_add_subdevice(mc13xxx, "%s-codec");
+		mc13xxx_add_subdevice_pdata(mc13xxx, "%s-codec",
+					pdata->codec, sizeof(*pdata->codec));
 
 	if (mc13xxx->flags & MC13XXX_USE_RTC)
 		mc13xxx_add_subdevice(mc13xxx, "%s-rtc");
@@ -837,42 +716,19 @@ err_revision:
 
 	return 0;
 }
+EXPORT_SYMBOL_GPL(mc13xxx_common_init);
 
-static int __devexit mc13xxx_remove(struct spi_device *spi)
+void mc13xxx_common_cleanup(struct mc13xxx *mc13xxx)
 {
-	struct mc13xxx *mc13xxx = dev_get_drvdata(&spi->dev);
+	free_irq(mc13xxx->irq, mc13xxx);
 
-	free_irq(mc13xxx->spidev->irq, mc13xxx);
+	mfd_remove_devices(mc13xxx->dev);
 
-	mfd_remove_devices(&spi->dev);
+	regmap_exit(mc13xxx->regmap);
 
 	kfree(mc13xxx);
-
-	return 0;
-}
-
-static struct spi_driver mc13xxx_driver = {
-	.id_table = mc13xxx_device_id,
-	.driver = {
-		.name = "mc13xxx",
-		.owner = THIS_MODULE,
-		.of_match_table = mc13xxx_dt_ids,
-	},
-	.probe = mc13xxx_probe,
-	.remove = __devexit_p(mc13xxx_remove),
-};
-
-static int __init mc13xxx_init(void)
-{
-	return spi_register_driver(&mc13xxx_driver);
-}
-subsys_initcall(mc13xxx_init);
-
-static void __exit mc13xxx_exit(void)
-{
-	spi_unregister_driver(&mc13xxx_driver);
 }
-module_exit(mc13xxx_exit);
+EXPORT_SYMBOL_GPL(mc13xxx_common_cleanup);
 
 MODULE_DESCRIPTION("Core driver for Freescale MC13XXX PMIC");
 MODULE_AUTHOR("Uwe Kleine-Koenig <u.kleine-koenig@pengutronix.de>");
diff --git a/drivers/mfd/mc13xxx-i2c.c b/drivers/mfd/mc13xxx-i2c.c
new file mode 100644
index 00000000000..d22501dad6a
--- /dev/null
+++ b/drivers/mfd/mc13xxx-i2c.c
@@ -0,0 +1,128 @@
+/*
+ * Copyright 2009-2010 Creative Product Design
+ * Marc Reilly marc@cpdesign.com.au
+ *
+ * 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/slab.h>
+#include <linux/module.h>
+#include <linux/platform_device.h>
+#include <linux/mutex.h>
+#include <linux/mfd/core.h>
+#include <linux/mfd/mc13xxx.h>
+#include <linux/of.h>
+#include <linux/of_device.h>
+#include <linux/of_gpio.h>
+#include <linux/i2c.h>
+#include <linux/err.h>
+
+#include "mc13xxx.h"
+
+static const struct i2c_device_id mc13xxx_i2c_device_id[] = {
+	{
+		.name = "mc13892",
+		.driver_data = MC13XXX_ID_MC13892,
+	}, {
+		/* sentinel */
+	}
+};
+MODULE_DEVICE_TABLE(i2c, mc13xxx_i2c_device_id);
+
+static const struct of_device_id mc13xxx_dt_ids[] = {
+	{
+		.compatible = "fsl,mc13892",
+		.data = (void *) &mc13xxx_i2c_device_id[0],
+	}, {
+		/* sentinel */
+	}
+};
+MODULE_DEVICE_TABLE(of, mc13xxx_dt_ids);
+
+static struct regmap_config mc13xxx_regmap_i2c_config = {
+	.reg_bits = 8,
+	.val_bits = 24,
+
+	.max_register = MC13XXX_NUMREGS,
+
+	.cache_type = REGCACHE_NONE,
+};
+
+static int mc13xxx_i2c_probe(struct i2c_client *client,
+		const struct i2c_device_id *id)
+{
+	const struct of_device_id *of_id;
+	struct i2c_driver *idrv = to_i2c_driver(client->dev.driver);
+	struct mc13xxx *mc13xxx;
+	struct mc13xxx_platform_data *pdata = dev_get_platdata(&client->dev);
+	int ret;
+
+	of_id = of_match_device(mc13xxx_dt_ids, &client->dev);
+	if (of_id)
+		idrv->id_table = (const struct i2c_device_id*) of_id->data;
+
+	mc13xxx = kzalloc(sizeof(*mc13xxx), GFP_KERNEL);
+	if (!mc13xxx)
+		return -ENOMEM;
+
+	dev_set_drvdata(&client->dev, mc13xxx);
+
+	mc13xxx->dev = &client->dev;
+	mutex_init(&mc13xxx->lock);
+
+	mc13xxx->regmap = regmap_init_i2c(client, &mc13xxx_regmap_i2c_config);
+	if (IS_ERR(mc13xxx->regmap)) {
+		ret = PTR_ERR(mc13xxx->regmap);
+		dev_err(mc13xxx->dev, "Failed to initialize register map: %d\n",
+				ret);
+		dev_set_drvdata(&client->dev, NULL);
+		kfree(mc13xxx);
+		return ret;
+	}
+
+	ret = mc13xxx_common_init(mc13xxx, pdata, client->irq);
+
+	if (ret == 0 && (id->driver_data != mc13xxx->ictype))
+		dev_warn(mc13xxx->dev,
+				"device id doesn't match auto detection!\n");
+
+	return ret;
+}
+
+static int __devexit mc13xxx_i2c_remove(struct i2c_client *client)
+{
+	struct mc13xxx *mc13xxx = dev_get_drvdata(&client->dev);
+
+	mc13xxx_common_cleanup(mc13xxx);
+
+	return 0;
+}
+
+static struct i2c_driver mc13xxx_i2c_driver = {
+	.id_table = mc13xxx_i2c_device_id,
+	.driver = {
+		.owner = THIS_MODULE,
+		.name = "mc13xxx",
+		.of_match_table = mc13xxx_dt_ids,
+	},
+	.probe = mc13xxx_i2c_probe,
+	.remove = __devexit_p(mc13xxx_i2c_remove),
+};
+
+static int __init mc13xxx_i2c_init(void)
+{
+	return i2c_add_driver(&mc13xxx_i2c_driver);
+}
+subsys_initcall(mc13xxx_i2c_init);
+
+static void __exit mc13xxx_i2c_exit(void)
+{
+	i2c_del_driver(&mc13xxx_i2c_driver);
+}
+module_exit(mc13xxx_i2c_exit);
+
+MODULE_DESCRIPTION("i2c driver for Freescale MC13XXX PMIC");
+MODULE_AUTHOR("Marc Reilly <marc@cpdesign.com.au");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/mfd/mc13xxx-spi.c b/drivers/mfd/mc13xxx-spi.c
new file mode 100644
index 00000000000..3fcdab3eb8e
--- /dev/null
+++ b/drivers/mfd/mc13xxx-spi.c
@@ -0,0 +1,140 @@
+/*
+ * Copyright 2009-2010 Pengutronix
+ * Uwe Kleine-Koenig <u.kleine-koenig@pengutronix.de>
+ *
+ * loosely based on an earlier driver that has
+ * Copyright 2009 Pengutronix, Sascha Hauer <s.hauer@pengutronix.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.
+ */
+
+#include <linux/slab.h>
+#include <linux/module.h>
+#include <linux/platform_device.h>
+#include <linux/mutex.h>
+#include <linux/interrupt.h>
+#include <linux/mfd/core.h>
+#include <linux/mfd/mc13xxx.h>
+#include <linux/of.h>
+#include <linux/of_device.h>
+#include <linux/of_gpio.h>
+#include <linux/err.h>
+#include <linux/spi/spi.h>
+
+#include "mc13xxx.h"
+
+static const struct spi_device_id mc13xxx_device_id[] = {
+	{
+		.name = "mc13783",
+		.driver_data = MC13XXX_ID_MC13783,
+	}, {
+		.name = "mc13892",
+		.driver_data = MC13XXX_ID_MC13892,
+	}, {
+		/* sentinel */
+	}
+};
+MODULE_DEVICE_TABLE(spi, mc13xxx_device_id);
+
+static const struct of_device_id mc13xxx_dt_ids[] = {
+	{ .compatible = "fsl,mc13783", .data = (void *) MC13XXX_ID_MC13783, },
+	{ .compatible = "fsl,mc13892", .data = (void *) MC13XXX_ID_MC13892, },
+	{ /* sentinel */ }
+};
+MODULE_DEVICE_TABLE(of, mc13xxx_dt_ids);
+
+static struct regmap_config mc13xxx_regmap_spi_config = {
+	.reg_bits = 7,
+	.pad_bits = 1,
+	.val_bits = 24,
+
+	.max_register = MC13XXX_NUMREGS,
+
+	.cache_type = REGCACHE_NONE,
+};
+
+static int mc13xxx_spi_probe(struct spi_device *spi)
+{
+	const struct of_device_id *of_id;
+	struct spi_driver *sdrv = to_spi_driver(spi->dev.driver);
+	struct mc13xxx *mc13xxx;
+	struct mc13xxx_platform_data *pdata = dev_get_platdata(&spi->dev);
+	int ret;
+
+	of_id = of_match_device(mc13xxx_dt_ids, &spi->dev);
+	if (of_id)
+		sdrv->id_table = &mc13xxx_device_id[(enum mc13xxx_id) of_id->data];
+
+	mc13xxx = kzalloc(sizeof(*mc13xxx), GFP_KERNEL);
+	if (!mc13xxx)
+		return -ENOMEM;
+
+	dev_set_drvdata(&spi->dev, mc13xxx);
+	spi->mode = SPI_MODE_0 | SPI_CS_HIGH;
+	spi->bits_per_word = 32;
+
+	mc13xxx->dev = &spi->dev;
+	mutex_init(&mc13xxx->lock);
+
+	mc13xxx->regmap = regmap_init_spi(spi, &mc13xxx_regmap_spi_config);
+	if (IS_ERR(mc13xxx->regmap)) {
+		ret = PTR_ERR(mc13xxx->regmap);
+		dev_err(mc13xxx->dev, "Failed to initialize register map: %d\n",
+				ret);
+		dev_set_drvdata(&spi->dev, NULL);
+		kfree(mc13xxx);
+		return ret;
+	}
+
+	ret = mc13xxx_common_init(mc13xxx, pdata, spi->irq);
+
+	if (ret) {
+		dev_set_drvdata(&spi->dev, NULL);
+	} else {
+		const struct spi_device_id *devid =
+			spi_get_device_id(spi);
+		if (!devid || devid->driver_data != mc13xxx->ictype)
+			dev_warn(mc13xxx->dev,
+				"device id doesn't match auto detection!\n");
+	}
+
+	return ret;
+}
+
+static int __devexit mc13xxx_spi_remove(struct spi_device *spi)
+{
+	struct mc13xxx *mc13xxx = dev_get_drvdata(&spi->dev);
+
+	mc13xxx_common_cleanup(mc13xxx);
+
+	return 0;
+}
+
+static struct spi_driver mc13xxx_spi_driver = {
+	.id_table = mc13xxx_device_id,
+	.driver = {
+		.name = "mc13xxx",
+		.owner = THIS_MODULE,
+		.of_match_table = mc13xxx_dt_ids,
+	},
+	.probe = mc13xxx_spi_probe,
+	.remove = __devexit_p(mc13xxx_spi_remove),
+};
+
+static int __init mc13xxx_init(void)
+{
+	return spi_register_driver(&mc13xxx_spi_driver);
+}
+subsys_initcall(mc13xxx_init);
+
+static void __exit mc13xxx_exit(void)
+{
+	spi_unregister_driver(&mc13xxx_spi_driver);
+}
+module_exit(mc13xxx_exit);
+
+MODULE_DESCRIPTION("Core driver for Freescale MC13XXX PMIC");
+MODULE_AUTHOR("Uwe Kleine-Koenig <u.kleine-koenig@pengutronix.de>");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/mfd/mc13xxx.h b/drivers/mfd/mc13xxx.h
new file mode 100644
index 00000000000..bbba06feea0
--- /dev/null
+++ b/drivers/mfd/mc13xxx.h
@@ -0,0 +1,45 @@
+/*
+ * Copyright 2012 Creative Product Design
+ * Marc Reilly <marc@cpdesign.com.au>
+ *
+ * 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 __DRIVERS_MFD_MC13XXX_H
+#define __DRIVERS_MFD_MC13XXX_H
+
+#include <linux/mutex.h>
+#include <linux/regmap.h>
+#include <linux/mfd/mc13xxx.h>
+
+enum mc13xxx_id {
+	MC13XXX_ID_MC13783,
+	MC13XXX_ID_MC13892,
+	MC13XXX_ID_INVALID,
+};
+
+#define MC13XXX_NUMREGS 0x3f
+
+struct mc13xxx {
+	struct regmap *regmap;
+
+	struct device *dev;
+	enum mc13xxx_id ictype;
+
+	struct mutex lock;
+	int irq;
+	int flags;
+
+	irq_handler_t irqhandler[MC13XXX_NUM_IRQ];
+	void *irqdata[MC13XXX_NUM_IRQ];
+
+	int adcflags;
+};
+
+int mc13xxx_common_init(struct mc13xxx *mc13xxx,
+		struct mc13xxx_platform_data *pdata, int irq);
+
+void mc13xxx_common_cleanup(struct mc13xxx *mc13xxx);
+
+#endif /* __DRIVERS_MFD_MC13XXX_H */
diff --git a/drivers/mfd/palmas.c b/drivers/mfd/palmas.c
new file mode 100644
index 00000000000..00c0aba7eba
--- /dev/null
+++ b/drivers/mfd/palmas.c
@@ -0,0 +1,509 @@
+/*
+ * TI Palmas MFD Driver
+ *
+ * Copyright 2011-2012 Texas Instruments Inc.
+ *
+ * Author: Graeme Gregory <gg@slimlogic.co.uk>
+ *
+ *  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/moduleparam.h>
+#include <linux/init.h>
+#include <linux/slab.h>
+#include <linux/i2c.h>
+#include <linux/interrupt.h>
+#include <linux/irq.h>
+#include <linux/regmap.h>
+#include <linux/err.h>
+#include <linux/mfd/core.h>
+#include <linux/mfd/palmas.h>
+
+static const struct resource gpadc_resource[] = {
+	{
+		.name = "EOC_SW",
+		.start = PALMAS_GPADC_EOC_SW_IRQ,
+		.end = PALMAS_GPADC_EOC_SW_IRQ,
+		.flags = IORESOURCE_IRQ,
+	}
+};
+
+static const struct resource usb_resource[] = {
+	{
+		.name = "ID",
+		.start = PALMAS_ID_OTG_IRQ,
+		.end = PALMAS_ID_OTG_IRQ,
+		.flags = IORESOURCE_IRQ,
+	},
+	{
+		.name = "ID_WAKEUP",
+		.start = PALMAS_ID_IRQ,
+		.end = PALMAS_ID_IRQ,
+		.flags = IORESOURCE_IRQ,
+	},
+	{
+		.name = "VBUS",
+		.start = PALMAS_VBUS_OTG_IRQ,
+		.end = PALMAS_VBUS_OTG_IRQ,
+		.flags = IORESOURCE_IRQ,
+	},
+	{
+		.name = "VBUS_WAKEUP",
+		.start = PALMAS_VBUS_IRQ,
+		.end = PALMAS_VBUS_IRQ,
+		.flags = IORESOURCE_IRQ,
+	},
+};
+
+static const struct resource rtc_resource[] = {
+	{
+		.name = "RTC_ALARM",
+		.start = PALMAS_RTC_ALARM_IRQ,
+		.end = PALMAS_RTC_ALARM_IRQ,
+		.flags = IORESOURCE_IRQ,
+	},
+};
+
+static const struct resource pwron_resource[] = {
+	{
+		.name = "PWRON_BUTTON",
+		.start = PALMAS_PWRON_IRQ,
+		.end = PALMAS_PWRON_IRQ,
+		.flags = IORESOURCE_IRQ,
+	},
+};
+
+enum palmas_ids {
+	PALMAS_PMIC_ID,
+	PALMAS_GPIO_ID,
+	PALMAS_LEDS_ID,
+	PALMAS_WDT_ID,
+	PALMAS_RTC_ID,
+	PALMAS_PWRBUTTON_ID,
+	PALMAS_GPADC_ID,
+	PALMAS_RESOURCE_ID,
+	PALMAS_CLK_ID,
+	PALMAS_PWM_ID,
+	PALMAS_USB_ID,
+};
+
+static const struct mfd_cell palmas_children[] = {
+	{
+		.name = "palmas-pmic",
+		.id = PALMAS_PMIC_ID,
+	},
+	{
+		.name = "palmas-gpio",
+		.id = PALMAS_GPIO_ID,
+	},
+	{
+		.name = "palmas-leds",
+		.id = PALMAS_LEDS_ID,
+	},
+	{
+		.name = "palmas-wdt",
+		.id = PALMAS_WDT_ID,
+	},
+	{
+		.name = "palmas-rtc",
+		.num_resources = ARRAY_SIZE(rtc_resource),
+		.resources = rtc_resource,
+		.id = PALMAS_RTC_ID,
+	},
+	{
+		.name = "palmas-pwrbutton",
+		.num_resources = ARRAY_SIZE(pwron_resource),
+		.resources = pwron_resource,
+		.id = PALMAS_PWRBUTTON_ID,
+	},
+	{
+		.name = "palmas-gpadc",
+		.num_resources = ARRAY_SIZE(gpadc_resource),
+		.resources = gpadc_resource,
+		.id = PALMAS_GPADC_ID,
+	},
+	{
+		.name = "palmas-resource",
+		.id = PALMAS_RESOURCE_ID,
+	},
+	{
+		.name = "palmas-clk",
+		.id = PALMAS_CLK_ID,
+	},
+	{
+		.name = "palmas-pwm",
+		.id = PALMAS_PWM_ID,
+	},
+	{
+		.name = "palmas-usb",
+		.num_resources = ARRAY_SIZE(usb_resource),
+		.resources = usb_resource,
+		.id = PALMAS_USB_ID,
+	}
+};
+
+static const struct regmap_config palmas_regmap_config[PALMAS_NUM_CLIENTS] = {
+	{
+		.reg_bits = 8,
+		.val_bits = 8,
+		.max_register = PALMAS_BASE_TO_REG(PALMAS_PU_PD_OD_BASE,
+					PALMAS_PRIMARY_SECONDARY_PAD3),
+	},
+	{
+		.reg_bits = 8,
+		.val_bits = 8,
+		.max_register = PALMAS_BASE_TO_REG(PALMAS_GPADC_BASE,
+					PALMAS_GPADC_SMPS_VSEL_MONITORING),
+	},
+	{
+		.reg_bits = 8,
+		.val_bits = 8,
+		.max_register = PALMAS_BASE_TO_REG(PALMAS_TRIM_GPADC_BASE,
+					PALMAS_GPADC_TRIM16),
+	},
+};
+
+static const struct regmap_irq palmas_irqs[] = {
+	/* INT1 IRQs */
+	[PALMAS_CHARG_DET_N_VBUS_OVV_IRQ] = {
+		.mask = PALMAS_INT1_STATUS_CHARG_DET_N_VBUS_OVV,
+	},
+	[PALMAS_PWRON_IRQ] = {
+		.mask = PALMAS_INT1_STATUS_PWRON,
+	},
+	[PALMAS_LONG_PRESS_KEY_IRQ] = {
+		.mask = PALMAS_INT1_STATUS_LONG_PRESS_KEY,
+	},
+	[PALMAS_RPWRON_IRQ] = {
+		.mask = PALMAS_INT1_STATUS_RPWRON,
+	},
+	[PALMAS_PWRDOWN_IRQ] = {
+		.mask = PALMAS_INT1_STATUS_PWRDOWN,
+	},
+	[PALMAS_HOTDIE_IRQ] = {
+		.mask = PALMAS_INT1_STATUS_HOTDIE,
+	},
+	[PALMAS_VSYS_MON_IRQ] = {
+		.mask = PALMAS_INT1_STATUS_VSYS_MON,
+	},
+	[PALMAS_VBAT_MON_IRQ] = {
+		.mask = PALMAS_INT1_STATUS_VBAT_MON,
+	},
+	/* INT2 IRQs*/
+	[PALMAS_RTC_ALARM_IRQ] = {
+		.mask = PALMAS_INT2_STATUS_RTC_ALARM,
+		.reg_offset = 1,
+	},
+	[PALMAS_RTC_TIMER_IRQ] = {
+		.mask = PALMAS_INT2_STATUS_RTC_TIMER,
+		.reg_offset = 1,
+	},
+	[PALMAS_WDT_IRQ] = {
+		.mask = PALMAS_INT2_STATUS_WDT,
+		.reg_offset = 1,
+	},
+	[PALMAS_BATREMOVAL_IRQ] = {
+		.mask = PALMAS_INT2_STATUS_BATREMOVAL,
+		.reg_offset = 1,
+	},
+	[PALMAS_RESET_IN_IRQ] = {
+		.mask = PALMAS_INT2_STATUS_RESET_IN,
+		.reg_offset = 1,
+	},
+	[PALMAS_FBI_BB_IRQ] = {
+		.mask = PALMAS_INT2_STATUS_FBI_BB,
+		.reg_offset = 1,
+	},
+	[PALMAS_SHORT_IRQ] = {
+		.mask = PALMAS_INT2_STATUS_SHORT,
+		.reg_offset = 1,
+	},
+	[PALMAS_VAC_ACOK_IRQ] = {
+		.mask = PALMAS_INT2_STATUS_VAC_ACOK,
+		.reg_offset = 1,
+	},
+	/* INT3 IRQs */
+	[PALMAS_GPADC_AUTO_0_IRQ] = {
+		.mask = PALMAS_INT3_STATUS_GPADC_AUTO_0,
+		.reg_offset = 2,
+	},
+	[PALMAS_GPADC_AUTO_1_IRQ] = {
+		.mask = PALMAS_INT3_STATUS_GPADC_AUTO_1,
+		.reg_offset = 2,
+	},
+	[PALMAS_GPADC_EOC_SW_IRQ] = {
+		.mask = PALMAS_INT3_STATUS_GPADC_EOC_SW,
+		.reg_offset = 2,
+	},
+	[PALMAS_GPADC_EOC_RT_IRQ] = {
+		.mask = PALMAS_INT3_STATUS_GPADC_EOC_RT,
+		.reg_offset = 2,
+	},
+	[PALMAS_ID_OTG_IRQ] = {
+		.mask = PALMAS_INT3_STATUS_ID_OTG,
+		.reg_offset = 2,
+	},
+	[PALMAS_ID_IRQ] = {
+		.mask = PALMAS_INT3_STATUS_ID,
+		.reg_offset = 2,
+	},
+	[PALMAS_VBUS_OTG_IRQ] = {
+		.mask = PALMAS_INT3_STATUS_VBUS_OTG,
+		.reg_offset = 2,
+	},
+	[PALMAS_VBUS_IRQ] = {
+		.mask = PALMAS_INT3_STATUS_VBUS,
+		.reg_offset = 2,
+	},
+	/* INT4 IRQs */
+	[PALMAS_GPIO_0_IRQ] = {
+		.mask = PALMAS_INT4_STATUS_GPIO_0,
+		.reg_offset = 3,
+	},
+	[PALMAS_GPIO_1_IRQ] = {
+		.mask = PALMAS_INT4_STATUS_GPIO_1,
+		.reg_offset = 3,
+	},
+	[PALMAS_GPIO_2_IRQ] = {
+		.mask = PALMAS_INT4_STATUS_GPIO_2,
+		.reg_offset = 3,
+	},
+	[PALMAS_GPIO_3_IRQ] = {
+		.mask = PALMAS_INT4_STATUS_GPIO_3,
+		.reg_offset = 3,
+	},
+	[PALMAS_GPIO_4_IRQ] = {
+		.mask = PALMAS_INT4_STATUS_GPIO_4,
+		.reg_offset = 3,
+	},
+	[PALMAS_GPIO_5_IRQ] = {
+		.mask = PALMAS_INT4_STATUS_GPIO_5,
+		.reg_offset = 3,
+	},
+	[PALMAS_GPIO_6_IRQ] = {
+		.mask = PALMAS_INT4_STATUS_GPIO_6,
+		.reg_offset = 3,
+	},
+	[PALMAS_GPIO_7_IRQ] = {
+		.mask = PALMAS_INT4_STATUS_GPIO_7,
+		.reg_offset = 3,
+	},
+};
+
+static struct regmap_irq_chip palmas_irq_chip = {
+	.name = "palmas",
+	.irqs = palmas_irqs,
+	.num_irqs = ARRAY_SIZE(palmas_irqs),
+
+	.num_regs = 4,
+	.irq_reg_stride = 5,
+	.status_base = PALMAS_BASE_TO_REG(PALMAS_INTERRUPT_BASE,
+			PALMAS_INT1_STATUS),
+	.mask_base = PALMAS_BASE_TO_REG(PALMAS_INTERRUPT_BASE,
+			PALMAS_INT1_MASK),
+};
+
+static int __devinit palmas_i2c_probe(struct i2c_client *i2c,
+			    const struct i2c_device_id *id)
+{
+	struct palmas *palmas;
+	struct palmas_platform_data *pdata;
+	int ret = 0, i;
+	unsigned int reg, addr;
+	int slave;
+	struct mfd_cell *children;
+
+	pdata = dev_get_platdata(&i2c->dev);
+	if (!pdata)
+		return -EINVAL;
+
+	palmas = devm_kzalloc(&i2c->dev, sizeof(struct palmas), GFP_KERNEL);
+	if (palmas == NULL)
+		return -ENOMEM;
+
+	i2c_set_clientdata(i2c, palmas);
+	palmas->dev = &i2c->dev;
+	palmas->id = id->driver_data;
+	palmas->irq = i2c->irq;
+
+	for (i = 0; i < PALMAS_NUM_CLIENTS; i++) {
+		if (i == 0)
+			palmas->i2c_clients[i] = i2c;
+		else {
+			palmas->i2c_clients[i] =
+					i2c_new_dummy(i2c->adapter,
+							i2c->addr + i);
+			if (!palmas->i2c_clients[i]) {
+				dev_err(palmas->dev,
+					"can't attach client %d\n", i);
+				ret = -ENOMEM;
+				goto err;
+			}
+		}
+		palmas->regmap[i] = devm_regmap_init_i2c(palmas->i2c_clients[i],
+				&palmas_regmap_config[i]);
+		if (IS_ERR(palmas->regmap[i])) {
+			ret = PTR_ERR(palmas->regmap[i]);
+			dev_err(palmas->dev,
+				"Failed to allocate regmap %d, err: %d\n",
+				i, ret);
+			goto err;
+		}
+	}
+
+	ret = regmap_add_irq_chip(palmas->regmap[1], palmas->irq,
+			IRQF_ONESHOT | IRQF_TRIGGER_LOW, -1, &palmas_irq_chip,
+			&palmas->irq_data);
+	if (ret < 0)
+		goto err;
+
+	slave = PALMAS_BASE_TO_SLAVE(PALMAS_PU_PD_OD_BASE);
+	addr = PALMAS_BASE_TO_REG(PALMAS_PU_PD_OD_BASE,
+			PALMAS_PRIMARY_SECONDARY_PAD1);
+
+	if (pdata->mux_from_pdata) {
+		reg = pdata->pad1;
+		ret = regmap_write(palmas->regmap[slave], addr, reg);
+		if (ret)
+			goto err;
+	} else {
+		ret = regmap_read(palmas->regmap[slave], addr, &reg);
+		if (ret)
+			goto err;
+	}
+
+	if (!(reg & PALMAS_PRIMARY_SECONDARY_PAD1_GPIO_0))
+		palmas->gpio_muxed |= PALMAS_GPIO_0_MUXED;
+	if (!(reg & PALMAS_PRIMARY_SECONDARY_PAD1_GPIO_1_MASK))
+		palmas->gpio_muxed |= PALMAS_GPIO_1_MUXED;
+	else if ((reg & PALMAS_PRIMARY_SECONDARY_PAD1_GPIO_1_MASK) ==
+			(2 << PALMAS_PRIMARY_SECONDARY_PAD1_GPIO_1_SHIFT))
+		palmas->led_muxed |= PALMAS_LED1_MUXED;
+	else if ((reg & PALMAS_PRIMARY_SECONDARY_PAD1_GPIO_1_MASK) ==
+			(3 << PALMAS_PRIMARY_SECONDARY_PAD1_GPIO_1_SHIFT))
+		palmas->pwm_muxed |= PALMAS_PWM1_MUXED;
+	if (!(reg & PALMAS_PRIMARY_SECONDARY_PAD1_GPIO_2_MASK))
+		palmas->gpio_muxed |= PALMAS_GPIO_2_MUXED;
+	else if ((reg & PALMAS_PRIMARY_SECONDARY_PAD1_GPIO_2_MASK) ==
+			(2 << PALMAS_PRIMARY_SECONDARY_PAD1_GPIO_2_SHIFT))
+		palmas->led_muxed |= PALMAS_LED2_MUXED;
+	else if ((reg & PALMAS_PRIMARY_SECONDARY_PAD1_GPIO_2_MASK) ==
+			(3 << PALMAS_PRIMARY_SECONDARY_PAD1_GPIO_2_SHIFT))
+		palmas->pwm_muxed |= PALMAS_PWM2_MUXED;
+	if (!(reg & PALMAS_PRIMARY_SECONDARY_PAD1_GPIO_3))
+		palmas->gpio_muxed |= PALMAS_GPIO_3_MUXED;
+
+	addr = PALMAS_BASE_TO_REG(PALMAS_PU_PD_OD_BASE,
+			PALMAS_PRIMARY_SECONDARY_PAD2);
+
+	if (pdata->mux_from_pdata) {
+		reg = pdata->pad2;
+		ret = regmap_write(palmas->regmap[slave], addr, reg);
+		if (ret)
+			goto err;
+	} else {
+		ret = regmap_read(palmas->regmap[slave], addr, &reg);
+		if (ret)
+			goto err;
+	}
+
+	if (!(reg & PALMAS_PRIMARY_SECONDARY_PAD2_GPIO_4))
+		palmas->gpio_muxed |= PALMAS_GPIO_4_MUXED;
+	if (!(reg & PALMAS_PRIMARY_SECONDARY_PAD2_GPIO_5_MASK))
+		palmas->gpio_muxed |= PALMAS_GPIO_5_MUXED;
+	if (!(reg & PALMAS_PRIMARY_SECONDARY_PAD2_GPIO_6))
+		palmas->gpio_muxed |= PALMAS_GPIO_6_MUXED;
+	if (!(reg & PALMAS_PRIMARY_SECONDARY_PAD2_GPIO_7_MASK))
+		palmas->gpio_muxed |= PALMAS_GPIO_7_MUXED;
+
+	dev_info(palmas->dev, "Muxing GPIO %x, PWM %x, LED %x\n",
+			palmas->gpio_muxed, palmas->pwm_muxed,
+			palmas->led_muxed);
+
+	reg = pdata->power_ctrl;
+
+	slave = PALMAS_BASE_TO_SLAVE(PALMAS_PMU_CONTROL_BASE);
+	addr = PALMAS_BASE_TO_REG(PALMAS_PMU_CONTROL_BASE, PALMAS_POWER_CTRL);
+
+	ret = regmap_write(palmas->regmap[slave], addr, reg);
+	if (ret)
+		goto err;
+
+	children = kmemdup(palmas_children, sizeof(palmas_children),
+			   GFP_KERNEL);
+	if (!children) {
+		ret = -ENOMEM;
+		goto err;
+	}
+
+	ret = mfd_add_devices(palmas->dev, -1,
+			      children, ARRAY_SIZE(palmas_children),
+			      NULL, regmap_irq_chip_get_base(palmas->irq_data));
+	kfree(children);
+
+	if (ret < 0)
+		goto err;
+
+	return ret;
+
+err:
+	mfd_remove_devices(palmas->dev);
+	kfree(palmas);
+	return ret;
+}
+
+static int palmas_i2c_remove(struct i2c_client *i2c)
+{
+	struct palmas *palmas = i2c_get_clientdata(i2c);
+
+	mfd_remove_devices(palmas->dev);
+	regmap_del_irq_chip(palmas->irq, palmas->irq_data);
+
+	return 0;
+}
+
+static const struct i2c_device_id palmas_i2c_id[] = {
+	{ "palmas", },
+	{ "twl6035", },
+	{ "twl6037", },
+	{ "tps65913", },
+};
+MODULE_DEVICE_TABLE(i2c, palmas_i2c_id);
+
+static struct of_device_id __devinitdata of_palmas_match_tbl[] = {
+	{ .compatible = "ti,palmas", },
+	{ /* end */ }
+};
+
+static struct i2c_driver palmas_i2c_driver = {
+	.driver = {
+		   .name = "palmas",
+		   .of_match_table = of_palmas_match_tbl,
+		   .owner = THIS_MODULE,
+	},
+	.probe = palmas_i2c_probe,
+	.remove = palmas_i2c_remove,
+	.id_table = palmas_i2c_id,
+};
+
+static int __init palmas_i2c_init(void)
+{
+	return i2c_add_driver(&palmas_i2c_driver);
+}
+/* init early so consumer devices can complete system boot */
+subsys_initcall(palmas_i2c_init);
+
+static void __exit palmas_i2c_exit(void)
+{
+	i2c_del_driver(&palmas_i2c_driver);
+}
+module_exit(palmas_i2c_exit);
+
+MODULE_AUTHOR("Graeme Gregory <gg@slimlogic.co.uk>");
+MODULE_DESCRIPTION("Palmas chip family multi-function driver");
+MODULE_LICENSE("GPL");
diff --git a/drivers/mfd/pcf50633-core.c b/drivers/mfd/pcf50633-core.c
index 189c2f07b83..29c122bf28e 100644
--- a/drivers/mfd/pcf50633-core.c
+++ b/drivers/mfd/pcf50633-core.c
@@ -204,7 +204,7 @@ static int __devinit pcf50633_probe(struct i2c_client *client,
 		return -ENOENT;
 	}
 
-	pcf = kzalloc(sizeof(*pcf), GFP_KERNEL);
+	pcf = devm_kzalloc(&client->dev, sizeof(*pcf), GFP_KERNEL);
 	if (!pcf)
 		return -ENOMEM;
 
@@ -212,12 +212,11 @@ static int __devinit pcf50633_probe(struct i2c_client *client,
 
 	mutex_init(&pcf->lock);
 
-	pcf->regmap = regmap_init_i2c(client, &pcf50633_regmap_config);
+	pcf->regmap = devm_regmap_init_i2c(client, &pcf50633_regmap_config);
 	if (IS_ERR(pcf->regmap)) {
 		ret = PTR_ERR(pcf->regmap);
-		dev_err(pcf->dev, "Failed to allocate register map: %d\n",
-			ret);
-		goto err_free;
+		dev_err(pcf->dev, "Failed to allocate register map: %d\n", ret);
+		return ret;
 	}
 
 	i2c_set_clientdata(client, pcf);
@@ -228,7 +227,7 @@ static int __devinit pcf50633_probe(struct i2c_client *client,
 	if (version < 0 || variant < 0) {
 		dev_err(pcf->dev, "Unable to probe pcf50633\n");
 		ret = -ENODEV;
-		goto err_regmap;
+		return ret;
 	}
 
 	dev_info(pcf->dev, "Probed device version %d variant %d\n",
@@ -237,16 +236,11 @@ static int __devinit pcf50633_probe(struct i2c_client *client,
 	pcf50633_irq_init(pcf, client->irq);
 
 	/* Create sub devices */
-	pcf50633_client_dev_register(pcf, "pcf50633-input",
-						&pcf->input_pdev);
-	pcf50633_client_dev_register(pcf, "pcf50633-rtc",
-						&pcf->rtc_pdev);
-	pcf50633_client_dev_register(pcf, "pcf50633-mbc",
-						&pcf->mbc_pdev);
-	pcf50633_client_dev_register(pcf, "pcf50633-adc",
-						&pcf->adc_pdev);
-	pcf50633_client_dev_register(pcf, "pcf50633-backlight",
-						&pcf->bl_pdev);
+	pcf50633_client_dev_register(pcf, "pcf50633-input", &pcf->input_pdev);
+	pcf50633_client_dev_register(pcf, "pcf50633-rtc", &pcf->rtc_pdev);
+	pcf50633_client_dev_register(pcf, "pcf50633-mbc", &pcf->mbc_pdev);
+	pcf50633_client_dev_register(pcf, "pcf50633-adc", &pcf->adc_pdev);
+	pcf50633_client_dev_register(pcf, "pcf50633-backlight", &pcf->bl_pdev);
 
 
 	for (i = 0; i < PCF50633_NUM_REGULATORS; i++) {
@@ -274,13 +268,6 @@ static int __devinit pcf50633_probe(struct i2c_client *client,
 		pdata->probe_done(pcf);
 
 	return 0;
-
-err_regmap:
-	regmap_exit(pcf->regmap);
-err_free:
-	kfree(pcf);
-
-	return ret;
 }
 
 static int __devexit pcf50633_remove(struct i2c_client *client)
@@ -300,9 +287,6 @@ static int __devexit pcf50633_remove(struct i2c_client *client)
 	for (i = 0; i < PCF50633_NUM_REGULATORS; i++)
 		platform_device_unregister(pcf->regulator_pdev[i]);
 
-	regmap_exit(pcf->regmap);
-	kfree(pcf);
-
 	return 0;
 }
 
diff --git a/drivers/mfd/rc5t583.c b/drivers/mfd/rc5t583.c
index 44afae0a69c..cdc1df7fa0e 100644
--- a/drivers/mfd/rc5t583.c
+++ b/drivers/mfd/rc5t583.c
@@ -75,6 +75,7 @@ static struct deepsleep_control_data deepsleep_data[] = {
 	(RC5T583_EXT_PWRREQ1_CONTROL | RC5T583_EXT_PWRREQ2_CONTROL)
 
 static struct mfd_cell rc5t583_subdevs[] = {
+	{.name = "rc5t583-gpio",},
 	{.name = "rc5t583-regulator",},
 	{.name = "rc5t583-rtc",      },
 	{.name = "rc5t583-key",      }
@@ -267,7 +268,7 @@ static int __devinit rc5t583_i2c_probe(struct i2c_client *i2c,
 	rc5t583->dev = &i2c->dev;
 	i2c_set_clientdata(i2c, rc5t583);
 
-	rc5t583->regmap = regmap_init_i2c(i2c, &rc5t583_regmap_config);
+	rc5t583->regmap = devm_regmap_init_i2c(i2c, &rc5t583_regmap_config);
 	if (IS_ERR(rc5t583->regmap)) {
 		ret = PTR_ERR(rc5t583->regmap);
 		dev_err(&i2c->dev, "regmap initialization failed: %d\n", ret);
@@ -276,7 +277,7 @@ static int __devinit rc5t583_i2c_probe(struct i2c_client *i2c,
 
 	ret = rc5t583_clear_ext_power_req(rc5t583, pdata);
 	if (ret < 0)
-		goto err_irq_init;
+		return ret;
 
 	if (i2c->irq) {
 		ret = rc5t583_irq_init(rc5t583, i2c->irq, pdata->irq_base);
@@ -299,8 +300,6 @@ static int __devinit rc5t583_i2c_probe(struct i2c_client *i2c,
 err_add_devs:
 	if (irq_init_success)
 		rc5t583_irq_exit(rc5t583);
-err_irq_init:
-	regmap_exit(rc5t583->regmap);
 	return ret;
 }
 
@@ -310,7 +309,6 @@ static int  __devexit rc5t583_i2c_remove(struct i2c_client *i2c)
 
 	mfd_remove_devices(rc5t583->dev);
 	rc5t583_irq_exit(rc5t583);
-	regmap_exit(rc5t583->regmap);
 	return 0;
 }
 
diff --git a/drivers/mfd/rdc321x-southbridge.c b/drivers/mfd/rdc321x-southbridge.c
index 809bd4a6108..685d61e431a 100644
--- a/drivers/mfd/rdc321x-southbridge.c
+++ b/drivers/mfd/rdc321x-southbridge.c
@@ -108,18 +108,7 @@ static struct pci_driver rdc321x_sb_driver = {
 	.remove		= __devexit_p(rdc321x_sb_remove),
 };
 
-static int __init rdc321x_sb_init(void)
-{
-	return pci_register_driver(&rdc321x_sb_driver);
-}
-
-static void __exit rdc321x_sb_exit(void)
-{
-	pci_unregister_driver(&rdc321x_sb_driver);
-}
-
-module_init(rdc321x_sb_init);
-module_exit(rdc321x_sb_exit);
+module_pci_driver(rdc321x_sb_driver);
 
 MODULE_AUTHOR("Florian Fainelli <florian@openwrt.org>");
 MODULE_LICENSE("GPL");
diff --git a/drivers/mfd/s5m-core.c b/drivers/mfd/s5m-core.c
index 48949d998d1..dd170307e60 100644
--- a/drivers/mfd/s5m-core.c
+++ b/drivers/mfd/s5m-core.c
@@ -114,12 +114,12 @@ static int s5m87xx_i2c_probe(struct i2c_client *i2c,
 		s5m87xx->wakeup = pdata->wakeup;
 	}
 
-	s5m87xx->regmap = regmap_init_i2c(i2c, &s5m_regmap_config);
+	s5m87xx->regmap = devm_regmap_init_i2c(i2c, &s5m_regmap_config);
 	if (IS_ERR(s5m87xx->regmap)) {
 		ret = PTR_ERR(s5m87xx->regmap);
 		dev_err(&i2c->dev, "Failed to allocate register map: %d\n",
 			ret);
-		goto err;
+		return ret;
 	}
 
 	s5m87xx->rtc = i2c_new_dummy(i2c->adapter, RTC_I2C_ADDR);
@@ -159,7 +159,6 @@ err:
 	mfd_remove_devices(s5m87xx->dev);
 	s5m_irq_exit(s5m87xx);
 	i2c_unregister_device(s5m87xx->rtc);
-	regmap_exit(s5m87xx->regmap);
 	return ret;
 }
 
@@ -170,7 +169,6 @@ static int s5m87xx_i2c_remove(struct i2c_client *i2c)
 	mfd_remove_devices(s5m87xx->dev);
 	s5m_irq_exit(s5m87xx);
 	i2c_unregister_device(s5m87xx->rtc);
-	regmap_exit(s5m87xx->regmap);
 	return 0;
 }
 
diff --git a/drivers/mfd/sta2x11-mfd.c b/drivers/mfd/sta2x11-mfd.c
new file mode 100644
index 00000000000..d31fed07aef
--- /dev/null
+++ b/drivers/mfd/sta2x11-mfd.c
@@ -0,0 +1,467 @@
+/*
+ * Copyright (c) 2009-2011 Wind River Systems, Inc.
+ * Copyright (c) 2011 ST Microelectronics (Alessandro Rubini)
+ *
+ * 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/kernel.h>
+#include <linux/module.h>
+#include <linux/spinlock.h>
+#include <linux/errno.h>
+#include <linux/device.h>
+#include <linux/slab.h>
+#include <linux/list.h>
+#include <linux/io.h>
+#include <linux/ioport.h>
+#include <linux/pci.h>
+#include <linux/debugfs.h>
+#include <linux/seq_file.h>
+#include <linux/platform_device.h>
+#include <linux/mfd/core.h>
+#include <linux/mfd/sta2x11-mfd.h>
+
+#include <asm/sta2x11.h>
+
+/* This describes STA2X11 MFD chip for us, we may have several */
+struct sta2x11_mfd {
+	struct sta2x11_instance *instance;
+	spinlock_t lock;
+	struct list_head list;
+	void __iomem *sctl_regs;
+	void __iomem *apbreg_regs;
+};
+
+static LIST_HEAD(sta2x11_mfd_list);
+
+/* Three functions to act on the list */
+static struct sta2x11_mfd *sta2x11_mfd_find(struct pci_dev *pdev)
+{
+	struct sta2x11_instance *instance;
+	struct sta2x11_mfd *mfd;
+
+	if (!pdev && !list_empty(&sta2x11_mfd_list)) {
+		pr_warning("%s: Unspecified device, "
+			    "using first instance\n", __func__);
+		return list_entry(sta2x11_mfd_list.next,
+				  struct sta2x11_mfd, list);
+	}
+
+	instance = sta2x11_get_instance(pdev);
+	if (!instance)
+		return NULL;
+	list_for_each_entry(mfd, &sta2x11_mfd_list, list) {
+		if (mfd->instance == instance)
+			return mfd;
+	}
+	return NULL;
+}
+
+static int __devinit sta2x11_mfd_add(struct pci_dev *pdev, gfp_t flags)
+{
+	struct sta2x11_mfd *mfd = sta2x11_mfd_find(pdev);
+	struct sta2x11_instance *instance;
+
+	if (mfd)
+		return -EBUSY;
+	instance = sta2x11_get_instance(pdev);
+	if (!instance)
+		return -EINVAL;
+	mfd = kzalloc(sizeof(*mfd), flags);
+	if (!mfd)
+		return -ENOMEM;
+	INIT_LIST_HEAD(&mfd->list);
+	spin_lock_init(&mfd->lock);
+	mfd->instance = instance;
+	list_add(&mfd->list, &sta2x11_mfd_list);
+	return 0;
+}
+
+static int __devexit 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;
+}
+
+/* These two functions are exported and are not expected to fail */
+u32 sta2x11_sctl_mask(struct pci_dev *pdev, u32 reg, u32 mask, u32 val)
+{
+	struct sta2x11_mfd *mfd = sta2x11_mfd_find(pdev);
+	u32 r;
+	unsigned long flags;
+
+	if (!mfd) {
+		dev_warn(&pdev->dev, ": can't access sctl regs\n");
+		return 0;
+	}
+	if (!mfd->sctl_regs) {
+		dev_warn(&pdev->dev, ": system ctl not initialized\n");
+		return 0;
+	}
+	spin_lock_irqsave(&mfd->lock, flags);
+	r = readl(mfd->sctl_regs + reg);
+	r &= ~mask;
+	r |= val;
+	if (mask)
+		writel(r, mfd->sctl_regs + reg);
+	spin_unlock_irqrestore(&mfd->lock, flags);
+	return r;
+}
+EXPORT_SYMBOL(sta2x11_sctl_mask);
+
+u32 sta2x11_apbreg_mask(struct pci_dev *pdev, u32 reg, u32 mask, u32 val)
+{
+	struct sta2x11_mfd *mfd = sta2x11_mfd_find(pdev);
+	u32 r;
+	unsigned long flags;
+
+	if (!mfd) {
+		dev_warn(&pdev->dev, ": can't access apb regs\n");
+		return 0;
+	}
+	if (!mfd->apbreg_regs) {
+		dev_warn(&pdev->dev, ": apb bridge not initialized\n");
+		return 0;
+	}
+	spin_lock_irqsave(&mfd->lock, flags);
+	r = readl(mfd->apbreg_regs + reg);
+	r &= ~mask;
+	r |= val;
+	if (mask)
+		writel(r, mfd->apbreg_regs + reg);
+	spin_unlock_irqrestore(&mfd->lock, flags);
+	return r;
+}
+EXPORT_SYMBOL(sta2x11_apbreg_mask);
+
+/* Two debugfs files, for our registers (FIXME: one instance only) */
+#define REG(regname) {.name = #regname, .offset = SCTL_ ## regname}
+static struct debugfs_reg32 sta2x11_sctl_regs[] = {
+	REG(SCCTL), REG(ARMCFG), REG(SCPLLCTL), REG(SCPLLFCTRL),
+	REG(SCRESFRACT), REG(SCRESCTRL1), REG(SCRESXTRL2), REG(SCPEREN0),
+	REG(SCPEREN1), REG(SCPEREN2), REG(SCGRST), REG(SCPCIPMCR1),
+	REG(SCPCIPMCR2), REG(SCPCIPMSR1), REG(SCPCIPMSR2), REG(SCPCIPMSR3),
+	REG(SCINTREN), REG(SCRISR), REG(SCCLKSTAT0), REG(SCCLKSTAT1),
+	REG(SCCLKSTAT2), REG(SCRSTSTA),
+};
+#undef REG
+
+static struct debugfs_regset32 sctl_regset = {
+	.regs = sta2x11_sctl_regs,
+	.nregs = ARRAY_SIZE(sta2x11_sctl_regs),
+};
+
+#define REG(regname) {.name = #regname, .offset = regname}
+static struct debugfs_reg32 sta2x11_apbreg_regs[] = {
+	REG(APBREG_BSR), REG(APBREG_PAER), REG(APBREG_PWAC), REG(APBREG_PRAC),
+	REG(APBREG_PCG), REG(APBREG_PUR), REG(APBREG_EMU_PCG),
+};
+#undef REG
+
+static struct debugfs_regset32 apbreg_regset = {
+	.regs = sta2x11_apbreg_regs,
+	.nregs = ARRAY_SIZE(sta2x11_apbreg_regs),
+};
+
+static struct dentry *sta2x11_sctl_debugfs;
+static struct dentry *sta2x11_apbreg_debugfs;
+
+/* Probe for the two platform devices */
+static int sta2x11_sctl_probe(struct platform_device *dev)
+{
+	struct pci_dev **pdev;
+	struct sta2x11_mfd *mfd;
+	struct resource *res;
+
+	pdev = dev->dev.platform_data;
+	mfd = sta2x11_mfd_find(*pdev);
+	if (!mfd)
+		return -ENODEV;
+
+	res = platform_get_resource(dev, IORESOURCE_MEM, 0);
+	if (!res)
+		return -ENOMEM;
+
+	if (!request_mem_region(res->start, resource_size(res),
+				"sta2x11-sctl"))
+		return -EBUSY;
+
+	mfd->sctl_regs = ioremap(res->start, resource_size(res));
+	if (!mfd->sctl_regs) {
+		release_mem_region(res->start, resource_size(res));
+		return -ENOMEM;
+	}
+	sctl_regset.base = mfd->sctl_regs;
+	sta2x11_sctl_debugfs = debugfs_create_regset32("sta2x11-sctl",
+						  S_IFREG | S_IRUGO,
+						  NULL, &sctl_regset);
+	return 0;
+}
+
+static int sta2x11_apbreg_probe(struct platform_device *dev)
+{
+	struct pci_dev **pdev;
+	struct sta2x11_mfd *mfd;
+	struct resource *res;
+
+	pdev = dev->dev.platform_data;
+	dev_dbg(&dev->dev, "%s: pdata is %p\n", __func__, pdev);
+	dev_dbg(&dev->dev, "%s: *pdata is %p\n", __func__, *pdev);
+
+	mfd = sta2x11_mfd_find(*pdev);
+	if (!mfd)
+		return -ENODEV;
+
+	res = platform_get_resource(dev, IORESOURCE_MEM, 0);
+	if (!res)
+		return -ENOMEM;
+
+	if (!request_mem_region(res->start, resource_size(res),
+				"sta2x11-apbreg"))
+		return -EBUSY;
+
+	mfd->apbreg_regs = ioremap(res->start, resource_size(res));
+	if (!mfd->apbreg_regs) {
+		release_mem_region(res->start, resource_size(res));
+		return -ENOMEM;
+	}
+	dev_dbg(&dev->dev, "%s: regbase %p\n", __func__, mfd->apbreg_regs);
+
+	apbreg_regset.base = mfd->apbreg_regs;
+	sta2x11_apbreg_debugfs = debugfs_create_regset32("sta2x11-apbreg",
+						  S_IFREG | S_IRUGO,
+						  NULL, &apbreg_regset);
+	return 0;
+}
+
+/* The two platform drivers */
+static struct platform_driver sta2x11_sctl_platform_driver = {
+	.driver = {
+		.name	= "sta2x11-sctl",
+		.owner	= THIS_MODULE,
+	},
+	.probe		= sta2x11_sctl_probe,
+};
+
+static int __init sta2x11_sctl_init(void)
+{
+	pr_info("%s\n", __func__);
+	return platform_driver_register(&sta2x11_sctl_platform_driver);
+}
+
+static struct platform_driver sta2x11_platform_driver = {
+	.driver = {
+		.name	= "sta2x11-apbreg",
+		.owner	= THIS_MODULE,
+	},
+	.probe		= sta2x11_apbreg_probe,
+};
+
+static int __init sta2x11_apbreg_init(void)
+{
+	pr_info("%s\n", __func__);
+	return platform_driver_register(&sta2x11_platform_driver);
+}
+
+/*
+ * What follows is the PCI device that hosts the above two pdevs.
+ * Each logic block is 4kB and they are all consecutive: we use this info.
+ */
+
+/* Bar 0 */
+enum bar0_cells {
+	STA2X11_GPIO_0 = 0,
+	STA2X11_GPIO_1,
+	STA2X11_GPIO_2,
+	STA2X11_GPIO_3,
+	STA2X11_SCTL,
+	STA2X11_SCR,
+	STA2X11_TIME,
+};
+/* Bar 1 */
+enum bar1_cells {
+	STA2X11_APBREG = 0,
+};
+#define CELL_4K(_name, _cell) { \
+		.name = _name, \
+		.start = _cell * 4096, .end = _cell * 4096 + 4095, \
+		.flags = IORESOURCE_MEM, \
+		}
+
+static const __devinitconst struct resource gpio_resources[] = {
+	{
+		.name = "sta2x11_gpio", /* 4 consecutive cells, 1 driver */
+		.start = 0,
+		.end = (4 * 4096) - 1,
+		.flags = IORESOURCE_MEM,
+	}
+};
+static const __devinitconst struct resource sctl_resources[] = {
+	CELL_4K("sta2x11-sctl", STA2X11_SCTL),
+};
+static const __devinitconst struct resource scr_resources[] = {
+	CELL_4K("sta2x11-scr", STA2X11_SCR),
+};
+static const __devinitconst struct resource time_resources[] = {
+	CELL_4K("sta2x11-time", STA2X11_TIME),
+};
+
+static const __devinitconst struct resource apbreg_resources[] = {
+	CELL_4K("sta2x11-apbreg", STA2X11_APBREG),
+};
+
+#define DEV(_name, _r) \
+	{ .name = _name, .num_resources = ARRAY_SIZE(_r), .resources = _r, }
+
+static __devinitdata struct mfd_cell sta2x11_mfd_bar0[] = {
+	DEV("sta2x11-gpio", gpio_resources), /* offset 0: we add pdata later */
+	DEV("sta2x11-sctl", sctl_resources),
+	DEV("sta2x11-scr", scr_resources),
+	DEV("sta2x11-time", time_resources),
+};
+
+static __devinitdata struct mfd_cell sta2x11_mfd_bar1[] = {
+	DEV("sta2x11-apbreg", apbreg_resources),
+};
+
+static int sta2x11_mfd_suspend(struct pci_dev *pdev, pm_message_t state)
+{
+	pci_save_state(pdev);
+	pci_disable_device(pdev);
+	pci_set_power_state(pdev, pci_choose_state(pdev, state));
+
+	return 0;
+}
+
+static int sta2x11_mfd_resume(struct pci_dev *pdev)
+{
+	int err;
+
+	pci_set_power_state(pdev, 0);
+	err = pci_enable_device(pdev);
+	if (err)
+		return err;
+	pci_restore_state(pdev);
+
+	return 0;
+}
+
+static int __devinit sta2x11_mfd_probe(struct pci_dev *pdev,
+				       const struct pci_device_id *pci_id)
+{
+	int err, i;
+	struct sta2x11_gpio_pdata *gpio_data;
+
+	dev_info(&pdev->dev, "%s\n", __func__);
+
+	err = pci_enable_device(pdev);
+	if (err) {
+		dev_err(&pdev->dev, "Can't enable device.\n");
+		return err;
+	}
+
+	err = pci_enable_msi(pdev);
+	if (err)
+		dev_info(&pdev->dev, "Enable msi failed\n");
+
+	/* Read gpio config data as pci device's platform data */
+	gpio_data = dev_get_platdata(&pdev->dev);
+	if (!gpio_data)
+		dev_warn(&pdev->dev, "no gpio configuration\n");
+
+	dev_dbg(&pdev->dev, "%s, gpio_data = %p (%p)\n", __func__,
+		gpio_data, &gpio_data);
+	dev_dbg(&pdev->dev, "%s, pdev = %p (%p)\n", __func__,
+		pdev, &pdev);
+
+	/* platform data is the pci device for all of them */
+	for (i = 0; i < ARRAY_SIZE(sta2x11_mfd_bar0); i++) {
+		sta2x11_mfd_bar0[i].pdata_size = sizeof(pdev);
+		sta2x11_mfd_bar0[i].platform_data = &pdev;
+	}
+	sta2x11_mfd_bar1[0].pdata_size = sizeof(pdev);
+	sta2x11_mfd_bar1[0].platform_data = &pdev;
+
+	/* Record this pdev before mfd_add_devices: their probe looks for it */
+	sta2x11_mfd_add(pdev, GFP_ATOMIC);
+
+
+	err = mfd_add_devices(&pdev->dev, -1,
+			      sta2x11_mfd_bar0,
+			      ARRAY_SIZE(sta2x11_mfd_bar0),
+			      &pdev->resource[0],
+			      0);
+	if (err) {
+		dev_err(&pdev->dev, "mfd_add_devices[0] failed: %d\n", err);
+		goto err_disable;
+	}
+
+	err = mfd_add_devices(&pdev->dev, -1,
+			      sta2x11_mfd_bar1,
+			      ARRAY_SIZE(sta2x11_mfd_bar1),
+			      &pdev->resource[1],
+			      0);
+	if (err) {
+		dev_err(&pdev->dev, "mfd_add_devices[1] failed: %d\n", err);
+		goto err_disable;
+	}
+
+	return 0;
+
+err_disable:
+	mfd_remove_devices(&pdev->dev);
+	pci_disable_device(pdev);
+	pci_disable_msi(pdev);
+	return err;
+}
+
+static DEFINE_PCI_DEVICE_TABLE(sta2x11_mfd_tbl) = {
+	{PCI_DEVICE(PCI_VENDOR_ID_STMICRO, PCI_DEVICE_ID_STMICRO_GPIO)},
+	{0,},
+};
+
+static struct pci_driver sta2x11_mfd_driver = {
+	.name =		"sta2x11-mfd",
+	.id_table =	sta2x11_mfd_tbl,
+	.probe =	sta2x11_mfd_probe,
+	.suspend =	sta2x11_mfd_suspend,
+	.resume =	sta2x11_mfd_resume,
+};
+
+static int __init sta2x11_mfd_init(void)
+{
+	pr_info("%s\n", __func__);
+	return pci_register_driver(&sta2x11_mfd_driver);
+}
+
+/*
+ * All of this must be ready before "normal" devices like MMCI appear.
+ * But MFD (the pci device) can't be too early. The following choice
+ * prepares platform drivers very early and probe the PCI device later,
+ * but before other PCI devices.
+ */
+subsys_initcall(sta2x11_apbreg_init);
+subsys_initcall(sta2x11_sctl_init);
+rootfs_initcall(sta2x11_mfd_init);
+
+MODULE_LICENSE("GPL v2");
+MODULE_AUTHOR("Wind River");
+MODULE_DESCRIPTION("STA2x11 mfd for GPIO, SCTL and APBREG");
+MODULE_DEVICE_TABLE(pci, sta2x11_mfd_tbl);
diff --git a/drivers/mfd/stmpe-spi.c b/drivers/mfd/stmpe-spi.c
index b58c43c7ea9..afd459013ec 100644
--- a/drivers/mfd/stmpe-spi.c
+++ b/drivers/mfd/stmpe-spi.c
@@ -122,7 +122,6 @@ MODULE_DEVICE_TABLE(spi, stmpe_id);
 static struct spi_driver stmpe_spi_driver = {
 	.driver = {
 		.name	= "stmpe-spi",
-		.bus	= &spi_bus_type,
 		.owner	= THIS_MODULE,
 #ifdef CONFIG_PM
 		.pm	= &stmpe_dev_pm_ops,
diff --git a/drivers/mfd/tps65090.c b/drivers/mfd/tps65090.c
index a66d4df5129..396b9d1b6bd 100644
--- a/drivers/mfd/tps65090.c
+++ b/drivers/mfd/tps65090.c
@@ -78,17 +78,6 @@ static struct mfd_cell tps65090s[] = {
 	},
 };
 
-struct tps65090 {
-	struct mutex		lock;
-	struct device		*dev;
-	struct i2c_client	*client;
-	struct regmap		*rmap;
-	struct irq_chip		irq_chip;
-	struct mutex		irq_lock;
-	int			irq_base;
-	unsigned int		id;
-};
-
 int tps65090_write(struct device *dev, int reg, uint8_t val)
 {
 	struct tps65090 *tps = dev_get_drvdata(dev);
@@ -294,27 +283,24 @@ static int __devinit tps65090_i2c_probe(struct i2c_client *client,
 		}
 	}
 
-	tps65090->rmap = regmap_init_i2c(tps65090->client,
-		&tps65090_regmap_config);
+	tps65090->rmap = devm_regmap_init_i2c(tps65090->client,
+					      &tps65090_regmap_config);
 	if (IS_ERR(tps65090->rmap)) {
-		dev_err(&client->dev, "regmap_init failed with err: %ld\n",
-			PTR_ERR(tps65090->rmap));
+		ret = PTR_ERR(tps65090->rmap);
+		dev_err(&client->dev, "regmap_init failed with err: %d\n", ret);
 		goto err_irq_exit;
-	};
+	}
 
 	ret = mfd_add_devices(tps65090->dev, -1, tps65090s,
 		ARRAY_SIZE(tps65090s), NULL, 0);
 	if (ret) {
 		dev_err(&client->dev, "add mfd devices failed with err: %d\n",
 			ret);
-		goto err_regmap_exit;
+		goto err_irq_exit;
 	}
 
 	return 0;
 
-err_regmap_exit:
-	regmap_exit(tps65090->rmap);
-
 err_irq_exit:
 	if (client->irq)
 		free_irq(client->irq, tps65090);
@@ -327,29 +313,34 @@ static int __devexit tps65090_i2c_remove(struct i2c_client *client)
 	struct tps65090 *tps65090 = i2c_get_clientdata(client);
 
 	mfd_remove_devices(tps65090->dev);
-	regmap_exit(tps65090->rmap);
 	if (client->irq)
 		free_irq(client->irq, tps65090);
 
 	return 0;
 }
 
-#ifdef CONFIG_PM
-static int tps65090_i2c_suspend(struct i2c_client *client, pm_message_t state)
+#ifdef CONFIG_PM_SLEEP
+static int tps65090_suspend(struct device *dev)
 {
+	struct i2c_client *client = to_i2c_client(dev);
 	if (client->irq)
 		disable_irq(client->irq);
 	return 0;
 }
 
-static int tps65090_i2c_resume(struct i2c_client *client)
+static int tps65090_resume(struct device *dev)
 {
+	struct i2c_client *client = to_i2c_client(dev);
 	if (client->irq)
 		enable_irq(client->irq);
 	return 0;
 }
 #endif
 
+static const struct dev_pm_ops tps65090_pm_ops = {
+	SET_SYSTEM_SLEEP_PM_OPS(tps65090_suspend, tps65090_resume)
+};
+
 static const struct i2c_device_id tps65090_id_table[] = {
 	{ "tps65090", 0 },
 	{ },
@@ -360,13 +351,10 @@ static struct i2c_driver tps65090_driver = {
 	.driver	= {
 		.name	= "tps65090",
 		.owner	= THIS_MODULE,
+		.pm	= &tps65090_pm_ops,
 	},
 	.probe		= tps65090_i2c_probe,
 	.remove		= __devexit_p(tps65090_i2c_remove),
-#ifdef CONFIG_PM
-	.suspend	= tps65090_i2c_suspend,
-	.resume		= tps65090_i2c_resume,
-#endif
 	.id_table	= tps65090_id_table,
 };
 
diff --git a/drivers/mfd/tps65217.c b/drivers/mfd/tps65217.c
index f7d854e4cc6..db194e433c0 100644
--- a/drivers/mfd/tps65217.c
+++ b/drivers/mfd/tps65217.c
@@ -96,7 +96,7 @@ EXPORT_SYMBOL_GPL(tps65217_reg_write);
  * @val: Value to write.
  * @level: Password protected level
  */
-int tps65217_update_bits(struct tps65217 *tps, unsigned int reg,
+static int tps65217_update_bits(struct tps65217 *tps, unsigned int reg,
 		unsigned int mask, unsigned int val, unsigned int level)
 {
 	int ret;
@@ -150,7 +150,7 @@ static int __devinit tps65217_probe(struct i2c_client *client,
 		return -ENOMEM;
 
 	tps->pdata = pdata;
-	tps->regmap = regmap_init_i2c(client, &tps65217_regmap_config);
+	tps->regmap = devm_regmap_init_i2c(client, &tps65217_regmap_config);
 	if (IS_ERR(tps->regmap)) {
 		ret = PTR_ERR(tps->regmap);
 		dev_err(tps->dev, "Failed to allocate register map: %d\n",
@@ -163,9 +163,9 @@ static int __devinit tps65217_probe(struct i2c_client *client,
 
 	ret = tps65217_reg_read(tps, TPS65217_REG_CHIPID, &version);
 	if (ret < 0) {
-		dev_err(tps->dev, "Failed to read revision"
-					" register: %d\n", ret);
-		goto err_regmap;
+		dev_err(tps->dev, "Failed to read revision register: %d\n",
+			ret);
+		return ret;
 	}
 
 	dev_info(tps->dev, "TPS65217 ID %#x version 1.%d\n",
@@ -190,11 +190,6 @@ static int __devinit tps65217_probe(struct i2c_client *client,
 	}
 
 	return 0;
-
-err_regmap:
-	regmap_exit(tps->regmap);
-
-	return ret;
 }
 
 static int __devexit tps65217_remove(struct i2c_client *client)
@@ -205,8 +200,6 @@ static int __devexit tps65217_remove(struct i2c_client *client)
 	for (i = 0; i < TPS65217_NUM_REGULATOR; i++)
 		platform_device_unregister(tps->regulator_pdev[i]);
 
-	regmap_exit(tps->regmap);
-
 	return 0;
 }
 
diff --git a/drivers/mfd/tps6586x.c b/drivers/mfd/tps6586x.c
index a5ddf31b60c..c84b5506d5f 100644
--- a/drivers/mfd/tps6586x.c
+++ b/drivers/mfd/tps6586x.c
@@ -23,6 +23,7 @@
 #include <linux/slab.h>
 #include <linux/gpio.h>
 #include <linux/i2c.h>
+#include <linux/regulator/of_regulator.h>
 
 #include <linux/mfd/core.h>
 #include <linux/mfd/tps6586x.h>
@@ -460,6 +461,7 @@ static int __devinit tps6586x_add_subdevs(struct tps6586x *tps6586x,
 
 		pdev->dev.parent = tps6586x->dev;
 		pdev->dev.platform_data = subdev->platform_data;
+		pdev->dev.of_node = subdev->of_node;
 
 		ret = platform_device_add(pdev);
 		if (ret) {
@@ -474,6 +476,86 @@ failed:
 	return ret;
 }
 
+#ifdef CONFIG_OF
+static struct of_regulator_match tps6586x_matches[] = {
+	{ .name = "sm0",     .driver_data = (void *)TPS6586X_ID_SM_0    },
+	{ .name = "sm1",     .driver_data = (void *)TPS6586X_ID_SM_1    },
+	{ .name = "sm2",     .driver_data = (void *)TPS6586X_ID_SM_2    },
+	{ .name = "ldo0",    .driver_data = (void *)TPS6586X_ID_LDO_0   },
+	{ .name = "ldo1",    .driver_data = (void *)TPS6586X_ID_LDO_1   },
+	{ .name = "ldo2",    .driver_data = (void *)TPS6586X_ID_LDO_2   },
+	{ .name = "ldo3",    .driver_data = (void *)TPS6586X_ID_LDO_3   },
+	{ .name = "ldo4",    .driver_data = (void *)TPS6586X_ID_LDO_4   },
+	{ .name = "ldo5",    .driver_data = (void *)TPS6586X_ID_LDO_5   },
+	{ .name = "ldo6",    .driver_data = (void *)TPS6586X_ID_LDO_6   },
+	{ .name = "ldo7",    .driver_data = (void *)TPS6586X_ID_LDO_7   },
+	{ .name = "ldo8",    .driver_data = (void *)TPS6586X_ID_LDO_8   },
+	{ .name = "ldo9",    .driver_data = (void *)TPS6586X_ID_LDO_9   },
+	{ .name = "ldo_rtc", .driver_data = (void *)TPS6586X_ID_LDO_RTC },
+};
+
+static struct tps6586x_platform_data *tps6586x_parse_dt(struct i2c_client *client)
+{
+	const unsigned int num = ARRAY_SIZE(tps6586x_matches);
+	struct device_node *np = client->dev.of_node;
+	struct tps6586x_platform_data *pdata;
+	struct tps6586x_subdev_info *devs;
+	struct device_node *regs;
+	unsigned int count;
+	unsigned int i, j;
+	int err;
+
+	regs = of_find_node_by_name(np, "regulators");
+	if (!regs)
+		return NULL;
+
+	err = of_regulator_match(&client->dev, regs, tps6586x_matches, num);
+	if (err < 0) {
+		of_node_put(regs);
+		return NULL;
+	}
+
+	of_node_put(regs);
+	count = err;
+
+	devs = devm_kzalloc(&client->dev, count * sizeof(*devs), GFP_KERNEL);
+	if (!devs)
+		return NULL;
+
+	for (i = 0, j = 0; i < num && j < count; i++) {
+		if (!tps6586x_matches[i].init_data)
+			continue;
+
+		devs[j].name = "tps6586x-regulator";
+		devs[j].platform_data = tps6586x_matches[i].init_data;
+		devs[j].id = (int)tps6586x_matches[i].driver_data;
+		devs[j].of_node = tps6586x_matches[i].of_node;
+		j++;
+	}
+
+	pdata = devm_kzalloc(&client->dev, sizeof(*pdata), GFP_KERNEL);
+	if (!pdata)
+		return NULL;
+
+	pdata->num_subdevs = count;
+	pdata->subdevs = devs;
+	pdata->gpio_base = -1;
+	pdata->irq_base = -1;
+
+	return pdata;
+}
+
+static struct of_device_id tps6586x_of_match[] = {
+	{ .compatible = "ti,tps6586x", },
+	{ },
+};
+#else
+static struct tps6586x_platform_data *tps6586x_parse_dt(struct i2c_client *client)
+{
+	return NULL;
+}
+#endif
+
 static int __devinit tps6586x_i2c_probe(struct i2c_client *client,
 					const struct i2c_device_id *id)
 {
@@ -481,6 +563,9 @@ static int __devinit tps6586x_i2c_probe(struct i2c_client *client,
 	struct tps6586x *tps6586x;
 	int ret;
 
+	if (!pdata && client->dev.of_node)
+		pdata = tps6586x_parse_dt(client);
+
 	if (!pdata) {
 		dev_err(&client->dev, "tps6586x requires platform data\n");
 		return -ENOTSUPP;
@@ -573,6 +658,7 @@ static struct i2c_driver tps6586x_driver = {
 	.driver	= {
 		.name	= "tps6586x",
 		.owner	= THIS_MODULE,
+		.of_match_table = of_match_ptr(tps6586x_of_match),
 	},
 	.probe		= tps6586x_i2c_probe,
 	.remove		= __devexit_p(tps6586x_i2c_remove),
diff --git a/drivers/mfd/tps65910-irq.c b/drivers/mfd/tps65910-irq.c
index c9ed5c00a62..09aab3e4776 100644
--- a/drivers/mfd/tps65910-irq.c
+++ b/drivers/mfd/tps65910-irq.c
@@ -20,15 +20,10 @@
 #include <linux/device.h>
 #include <linux/interrupt.h>
 #include <linux/irq.h>
+#include <linux/irqdomain.h>
 #include <linux/gpio.h>
 #include <linux/mfd/tps65910.h>
 
-static inline int irq_to_tps65910_irq(struct tps65910 *tps65910,
-							int irq)
-{
-	return (irq - tps65910->irq_base);
-}
-
 /*
  * This is a threaded IRQ handler so can access I2C/SPI.  Since all
  * interrupts are clear on read the IRQ line will be reasserted and
@@ -41,28 +36,28 @@ static inline int irq_to_tps65910_irq(struct tps65910 *tps65910,
 static irqreturn_t tps65910_irq(int irq, void *irq_data)
 {
 	struct tps65910 *tps65910 = irq_data;
+	unsigned int reg;
 	u32 irq_sts;
 	u32 irq_mask;
-	u8 reg;
 	int i;
 
-	tps65910->read(tps65910, TPS65910_INT_STS, 1, &reg);
+	tps65910_reg_read(tps65910, TPS65910_INT_STS, &reg);
 	irq_sts = reg;
-	tps65910->read(tps65910, TPS65910_INT_STS2, 1, &reg);
+	tps65910_reg_read(tps65910, TPS65910_INT_STS2, &reg);
 	irq_sts |= reg << 8;
 	switch (tps65910_chip_id(tps65910)) {
 	case TPS65911:
-		tps65910->read(tps65910, TPS65910_INT_STS3, 1, &reg);
+		tps65910_reg_read(tps65910, TPS65910_INT_STS3, &reg);
 		irq_sts |= reg << 16;
 	}
 
-	tps65910->read(tps65910, TPS65910_INT_MSK, 1, &reg);
+	tps65910_reg_read(tps65910, TPS65910_INT_MSK, &reg);
 	irq_mask = reg;
-	tps65910->read(tps65910, TPS65910_INT_MSK2, 1, &reg);
+	tps65910_reg_read(tps65910, TPS65910_INT_MSK2, &reg);
 	irq_mask |= reg << 8;
 	switch (tps65910_chip_id(tps65910)) {
 	case TPS65911:
-		tps65910->read(tps65910, TPS65910_INT_MSK3, 1, &reg);
+		tps65910_reg_read(tps65910, TPS65910_INT_MSK3, &reg);
 		irq_mask |= reg << 16;
 	}
 
@@ -76,19 +71,19 @@ static irqreturn_t tps65910_irq(int irq, void *irq_data)
 		if (!(irq_sts & (1 << i)))
 			continue;
 
-		handle_nested_irq(tps65910->irq_base + i);
+		handle_nested_irq(irq_find_mapping(tps65910->domain, i));
 	}
 
 	/* Write the STS register back to clear IRQs we handled */
 	reg = irq_sts & 0xFF;
 	irq_sts >>= 8;
-	tps65910->write(tps65910, TPS65910_INT_STS, 1, &reg);
+	tps65910_reg_write(tps65910, TPS65910_INT_STS, reg);
 	reg = irq_sts & 0xFF;
-	tps65910->write(tps65910, TPS65910_INT_STS2, 1, &reg);
+	tps65910_reg_write(tps65910, TPS65910_INT_STS2, reg);
 	switch (tps65910_chip_id(tps65910)) {
 	case TPS65911:
 		reg = irq_sts >> 8;
-		tps65910->write(tps65910, TPS65910_INT_STS3, 1, &reg);
+		tps65910_reg_write(tps65910, TPS65910_INT_STS3, reg);
 	}
 
 	return IRQ_HANDLED;
@@ -105,27 +100,27 @@ static void tps65910_irq_sync_unlock(struct irq_data *data)
 {
 	struct tps65910 *tps65910 = irq_data_get_irq_chip_data(data);
 	u32 reg_mask;
-	u8 reg;
+	unsigned int reg;
 
-	tps65910->read(tps65910, TPS65910_INT_MSK, 1, &reg);
+	tps65910_reg_read(tps65910, TPS65910_INT_MSK, &reg);
 	reg_mask = reg;
-	tps65910->read(tps65910, TPS65910_INT_MSK2, 1, &reg);
+	tps65910_reg_read(tps65910, TPS65910_INT_MSK2, &reg);
 	reg_mask |= reg << 8;
 	switch (tps65910_chip_id(tps65910)) {
 	case TPS65911:
-		tps65910->read(tps65910, TPS65910_INT_MSK3, 1, &reg);
+		tps65910_reg_read(tps65910, TPS65910_INT_MSK3, &reg);
 		reg_mask |= reg << 16;
 	}
 
 	if (tps65910->irq_mask != reg_mask) {
 		reg = tps65910->irq_mask & 0xFF;
-		tps65910->write(tps65910, TPS65910_INT_MSK, 1, &reg);
+		tps65910_reg_write(tps65910, TPS65910_INT_MSK, reg);
 		reg = tps65910->irq_mask >> 8 & 0xFF;
-		tps65910->write(tps65910, TPS65910_INT_MSK2, 1, &reg);
+		tps65910_reg_write(tps65910, TPS65910_INT_MSK2, reg);
 		switch (tps65910_chip_id(tps65910)) {
 		case TPS65911:
 			reg = tps65910->irq_mask >> 16;
-			tps65910->write(tps65910, TPS65910_INT_MSK3, 1, &reg);
+			tps65910_reg_write(tps65910, TPS65910_INT_MSK3, reg);
 		}
 	}
 	mutex_unlock(&tps65910->irq_lock);
@@ -135,14 +130,14 @@ static void tps65910_irq_enable(struct irq_data *data)
 {
 	struct tps65910 *tps65910 = irq_data_get_irq_chip_data(data);
 
-	tps65910->irq_mask &= ~( 1 << irq_to_tps65910_irq(tps65910, data->irq));
+	tps65910->irq_mask &= ~(1 << data->hwirq);
 }
 
 static void tps65910_irq_disable(struct irq_data *data)
 {
 	struct tps65910 *tps65910 = irq_data_get_irq_chip_data(data);
 
-	tps65910->irq_mask |= ( 1 << irq_to_tps65910_irq(tps65910, data->irq));
+	tps65910->irq_mask |= (1 << data->hwirq);
 }
 
 #ifdef CONFIG_PM_SLEEP
@@ -164,10 +159,35 @@ static struct irq_chip tps65910_irq_chip = {
 	.irq_set_wake = tps65910_irq_set_wake,
 };
 
+static int tps65910_irq_map(struct irq_domain *h, unsigned int virq,
+				irq_hw_number_t hw)
+{
+	struct tps65910 *tps65910 = h->host_data;
+
+	irq_set_chip_data(virq, tps65910);
+	irq_set_chip_and_handler(virq, &tps65910_irq_chip, handle_edge_irq);
+	irq_set_nested_thread(virq, 1);
+
+	/* ARM needs us to explicitly flag the IRQ as valid
+	 * and will set them noprobe when we do so. */
+#ifdef CONFIG_ARM
+	set_irq_flags(virq, IRQF_VALID);
+#else
+	irq_set_noprobe(virq);
+#endif
+
+	return 0;
+}
+
+static struct irq_domain_ops tps65910_domain_ops = {
+	.map	= tps65910_irq_map,
+	.xlate	= irq_domain_xlate_twocell,
+};
+
 int tps65910_irq_init(struct tps65910 *tps65910, int irq,
 		    struct tps65910_platform_data *pdata)
 {
-	int ret, cur_irq;
+	int ret;
 	int flags = IRQF_ONESHOT;
 
 	if (!irq) {
@@ -175,17 +195,11 @@ int tps65910_irq_init(struct tps65910 *tps65910, int irq,
 		return -EINVAL;
 	}
 
-	if (!pdata || !pdata->irq_base) {
-		dev_warn(tps65910->dev, "No interrupt support, no IRQ base\n");
+	if (!pdata) {
+		dev_warn(tps65910->dev, "No interrupt support, no pdata\n");
 		return -EINVAL;
 	}
 
-	tps65910->irq_mask = 0xFFFFFF;
-
-	mutex_init(&tps65910->irq_lock);
-	tps65910->chip_irq = irq;
-	tps65910->irq_base = pdata->irq_base;
-
 	switch (tps65910_chip_id(tps65910)) {
 	case TPS65910:
 		tps65910->irq_num = TPS65910_NUM_IRQ;
@@ -195,22 +209,36 @@ int tps65910_irq_init(struct tps65910 *tps65910, int irq,
 		break;
 	}
 
-	/* Register with genirq */
-	for (cur_irq = tps65910->irq_base;
-	     cur_irq < tps65910->irq_num + tps65910->irq_base;
-	     cur_irq++) {
-		irq_set_chip_data(cur_irq, tps65910);
-		irq_set_chip_and_handler(cur_irq, &tps65910_irq_chip,
-					 handle_edge_irq);
-		irq_set_nested_thread(cur_irq, 1);
-
-		/* ARM needs us to explicitly flag the IRQ as valid
-		 * and will set them noprobe when we do so. */
-#ifdef CONFIG_ARM
-		set_irq_flags(cur_irq, IRQF_VALID);
-#else
-		irq_set_noprobe(cur_irq);
-#endif
+	if (pdata->irq_base > 0) {
+		pdata->irq_base = irq_alloc_descs(pdata->irq_base, 0,
+					tps65910->irq_num, -1);
+		if (pdata->irq_base < 0) {
+			dev_warn(tps65910->dev, "Failed to alloc IRQs: %d\n",
+					pdata->irq_base);
+			return pdata->irq_base;
+		}
+	}
+
+	tps65910->irq_mask = 0xFFFFFF;
+
+	mutex_init(&tps65910->irq_lock);
+	tps65910->chip_irq = irq;
+	tps65910->irq_base = pdata->irq_base;
+
+	if (pdata->irq_base > 0)
+		tps65910->domain = irq_domain_add_legacy(tps65910->dev->of_node,
+					tps65910->irq_num,
+					pdata->irq_base,
+					0,
+					&tps65910_domain_ops, tps65910);
+	else
+		tps65910->domain = irq_domain_add_linear(tps65910->dev->of_node,
+					tps65910->irq_num,
+					&tps65910_domain_ops, tps65910);
+
+	if (!tps65910->domain) {
+		dev_err(tps65910->dev, "Failed to create IRQ domain\n");
+		return -ENOMEM;
 	}
 
 	ret = request_threaded_irq(irq, NULL, tps65910_irq, flags,
diff --git a/drivers/mfd/tps65910.c b/drivers/mfd/tps65910.c
index bf2b25ebf2c..be9e07b7732 100644
--- a/drivers/mfd/tps65910.c
+++ b/drivers/mfd/tps65910.c
@@ -19,13 +19,16 @@
 #include <linux/err.h>
 #include <linux/slab.h>
 #include <linux/i2c.h>
-#include <linux/gpio.h>
 #include <linux/mfd/core.h>
 #include <linux/regmap.h>
 #include <linux/mfd/tps65910.h>
+#include <linux/of_device.h>
 
 static struct mfd_cell tps65910s[] = {
 	{
+		.name = "tps65910-gpio",
+	},
+	{
 		.name = "tps65910-pmic",
 	},
 	{
@@ -37,30 +40,6 @@ static struct mfd_cell tps65910s[] = {
 };
 
 
-static int tps65910_i2c_read(struct tps65910 *tps65910, u8 reg,
-				  int bytes, void *dest)
-{
-	return regmap_bulk_read(tps65910->regmap, reg, dest, bytes);
-}
-
-static int tps65910_i2c_write(struct tps65910 *tps65910, u8 reg,
-				  int bytes, void *src)
-{
-	return regmap_bulk_write(tps65910->regmap, reg, src, bytes);
-}
-
-int tps65910_set_bits(struct tps65910 *tps65910, u8 reg, u8 mask)
-{
-	return regmap_update_bits(tps65910->regmap, reg, mask, mask);
-}
-EXPORT_SYMBOL_GPL(tps65910_set_bits);
-
-int tps65910_clear_bits(struct tps65910 *tps65910, u8 reg, u8 mask)
-{
-	return regmap_update_bits(tps65910->regmap, reg, mask, 0);
-}
-EXPORT_SYMBOL_GPL(tps65910_clear_bits);
-
 static bool is_volatile_reg(struct device *dev, unsigned int reg)
 {
 	struct tps65910 *tps65910 = dev_get_drvdata(dev);
@@ -85,80 +64,197 @@ static const struct regmap_config tps65910_regmap_config = {
 	.reg_bits = 8,
 	.val_bits = 8,
 	.volatile_reg = is_volatile_reg,
-	.max_register = TPS65910_MAX_REGISTER,
-	.num_reg_defaults_raw = TPS65910_MAX_REGISTER,
+	.max_register = TPS65910_MAX_REGISTER - 1,
 	.cache_type = REGCACHE_RBTREE,
 };
 
-static int tps65910_i2c_probe(struct i2c_client *i2c,
-			    const struct i2c_device_id *id)
+static int __devinit tps65910_sleepinit(struct tps65910 *tps65910,
+		struct tps65910_board *pmic_pdata)
+{
+	struct device *dev = NULL;
+	int ret = 0;
+
+	dev = tps65910->dev;
+
+	if (!pmic_pdata->en_dev_slp)
+		return 0;
+
+	/* enabling SLEEP device state */
+	ret = tps65910_reg_set_bits(tps65910, TPS65910_DEVCTRL,
+				DEVCTRL_DEV_SLP_MASK);
+	if (ret < 0) {
+		dev_err(dev, "set dev_slp failed: %d\n", ret);
+		goto err_sleep_init;
+	}
+
+	/* Return if there is no sleep keepon data. */
+	if (!pmic_pdata->slp_keepon)
+		return 0;
+
+	if (pmic_pdata->slp_keepon->therm_keepon) {
+		ret = tps65910_reg_set_bits(tps65910,
+				TPS65910_SLEEP_KEEP_RES_ON,
+				SLEEP_KEEP_RES_ON_THERM_KEEPON_MASK);
+		if (ret < 0) {
+			dev_err(dev, "set therm_keepon failed: %d\n", ret);
+			goto disable_dev_slp;
+		}
+	}
+
+	if (pmic_pdata->slp_keepon->clkout32k_keepon) {
+		ret = tps65910_reg_set_bits(tps65910,
+				TPS65910_SLEEP_KEEP_RES_ON,
+				SLEEP_KEEP_RES_ON_CLKOUT32K_KEEPON_MASK);
+		if (ret < 0) {
+			dev_err(dev, "set clkout32k_keepon failed: %d\n", ret);
+			goto disable_dev_slp;
+		}
+	}
+
+	if (pmic_pdata->slp_keepon->i2chs_keepon) {
+		ret = tps65910_reg_set_bits(tps65910,
+				TPS65910_SLEEP_KEEP_RES_ON,
+				SLEEP_KEEP_RES_ON_I2CHS_KEEPON_MASK);
+		if (ret < 0) {
+			dev_err(dev, "set i2chs_keepon failed: %d\n", ret);
+			goto disable_dev_slp;
+		}
+	}
+
+	return 0;
+
+disable_dev_slp:
+	tps65910_reg_clear_bits(tps65910, TPS65910_DEVCTRL,
+				DEVCTRL_DEV_SLP_MASK);
+
+err_sleep_init:
+	return ret;
+}
+
+#ifdef CONFIG_OF
+static struct of_device_id tps65910_of_match[] = {
+	{ .compatible = "ti,tps65910", .data = (void *)TPS65910},
+	{ .compatible = "ti,tps65911", .data = (void *)TPS65911},
+	{ },
+};
+MODULE_DEVICE_TABLE(of, tps65910_of_match);
+
+static struct tps65910_board *tps65910_parse_dt(struct i2c_client *client,
+						int *chip_id)
+{
+	struct device_node *np = client->dev.of_node;
+	struct tps65910_board *board_info;
+	unsigned int prop;
+	const struct of_device_id *match;
+	int ret = 0;
+
+	match = of_match_device(tps65910_of_match, &client->dev);
+	if (!match) {
+		dev_err(&client->dev, "Failed to find matching dt id\n");
+		return NULL;
+	}
+
+	*chip_id  = (int)match->data;
+
+	board_info = devm_kzalloc(&client->dev, sizeof(*board_info),
+			GFP_KERNEL);
+	if (!board_info) {
+		dev_err(&client->dev, "Failed to allocate pdata\n");
+		return NULL;
+	}
+
+	ret = of_property_read_u32(np, "ti,vmbch-threshold", &prop);
+	if (!ret)
+		board_info->vmbch_threshold = prop;
+	else if (*chip_id == TPS65911)
+		dev_warn(&client->dev, "VMBCH-Threshold not specified");
+
+	ret = of_property_read_u32(np, "ti,vmbch2-threshold", &prop);
+	if (!ret)
+		board_info->vmbch2_threshold = prop;
+	else if (*chip_id == TPS65911)
+		dev_warn(&client->dev, "VMBCH2-Threshold not specified");
+
+	board_info->irq = client->irq;
+	board_info->irq_base = -1;
+
+	return board_info;
+}
+#else
+static inline
+struct tps65910_board *tps65910_parse_dt(struct i2c_client *client,
+					 int *chip_id)
+{
+	return NULL;
+}
+#endif
+
+static __devinit int tps65910_i2c_probe(struct i2c_client *i2c,
+					const struct i2c_device_id *id)
 {
 	struct tps65910 *tps65910;
 	struct tps65910_board *pmic_plat_data;
+	struct tps65910_board *of_pmic_plat_data = NULL;
 	struct tps65910_platform_data *init_data;
 	int ret = 0;
+	int chip_id = id->driver_data;
 
 	pmic_plat_data = dev_get_platdata(&i2c->dev);
+
+	if (!pmic_plat_data && i2c->dev.of_node) {
+		pmic_plat_data = tps65910_parse_dt(i2c, &chip_id);
+		of_pmic_plat_data = pmic_plat_data;
+	}
+
 	if (!pmic_plat_data)
 		return -EINVAL;
 
-	init_data = kzalloc(sizeof(struct tps65910_platform_data), GFP_KERNEL);
+	init_data = devm_kzalloc(&i2c->dev, sizeof(*init_data), GFP_KERNEL);
 	if (init_data == NULL)
 		return -ENOMEM;
 
-	tps65910 = kzalloc(sizeof(struct tps65910), GFP_KERNEL);
-	if (tps65910 == NULL) {
-		kfree(init_data);
+	tps65910 = devm_kzalloc(&i2c->dev, sizeof(*tps65910), GFP_KERNEL);
+	if (tps65910 == NULL)
 		return -ENOMEM;
-	}
 
+	tps65910->of_plat_data = of_pmic_plat_data;
 	i2c_set_clientdata(i2c, tps65910);
 	tps65910->dev = &i2c->dev;
 	tps65910->i2c_client = i2c;
-	tps65910->id = id->driver_data;
-	tps65910->read = tps65910_i2c_read;
-	tps65910->write = tps65910_i2c_write;
+	tps65910->id = chip_id;
 	mutex_init(&tps65910->io_mutex);
 
-	tps65910->regmap = regmap_init_i2c(i2c, &tps65910_regmap_config);
+	tps65910->regmap = devm_regmap_init_i2c(i2c, &tps65910_regmap_config);
 	if (IS_ERR(tps65910->regmap)) {
 		ret = PTR_ERR(tps65910->regmap);
 		dev_err(&i2c->dev, "regmap initialization failed: %d\n", ret);
-		goto regmap_err;
+		return ret;
 	}
 
 	ret = mfd_add_devices(tps65910->dev, -1,
 			      tps65910s, ARRAY_SIZE(tps65910s),
 			      NULL, 0);
-	if (ret < 0)
-		goto err;
+	if (ret < 0) {
+		dev_err(&i2c->dev, "mfd_add_devices failed: %d\n", ret);
+		return ret;
+	}
 
 	init_data->irq = pmic_plat_data->irq;
 	init_data->irq_base = pmic_plat_data->irq_base;
 
-	tps65910_gpio_init(tps65910, pmic_plat_data->gpio_base);
-
 	tps65910_irq_init(tps65910, init_data->irq, init_data);
 
-	kfree(init_data);
-	return ret;
+	tps65910_sleepinit(tps65910, pmic_plat_data);
 
-err:
-	regmap_exit(tps65910->regmap);
-regmap_err:
-	kfree(tps65910);
-	kfree(init_data);
 	return ret;
 }
 
-static int tps65910_i2c_remove(struct i2c_client *i2c)
+static __devexit int tps65910_i2c_remove(struct i2c_client *i2c)
 {
 	struct tps65910 *tps65910 = i2c_get_clientdata(i2c);
 
 	tps65910_irq_exit(tps65910);
 	mfd_remove_devices(tps65910->dev);
-	regmap_exit(tps65910->regmap);
-	kfree(tps65910);
 
 	return 0;
 }
@@ -175,9 +271,10 @@ static struct i2c_driver tps65910_i2c_driver = {
 	.driver = {
 		   .name = "tps65910",
 		   .owner = THIS_MODULE,
+		   .of_match_table = of_match_ptr(tps65910_of_match),
 	},
 	.probe = tps65910_i2c_probe,
-	.remove = tps65910_i2c_remove,
+	.remove = __devexit_p(tps65910_i2c_remove),
 	.id_table = tps65910_i2c_id,
 };
 
diff --git a/drivers/mfd/tps65911-comparator.c b/drivers/mfd/tps65911-comparator.c
index e7ff783aa31..5a62e6bf89a 100644
--- a/drivers/mfd/tps65911-comparator.c
+++ b/drivers/mfd/tps65911-comparator.c
@@ -26,7 +26,7 @@
 #define COMP1					1
 #define COMP2					2
 
-/* Comparator 1 voltage selection table in milivolts */
+/* Comparator 1 voltage selection table in millivolts */
 static const u16 COMP_VSEL_TABLE[] = {
 	0, 2500, 2500, 2500, 2500, 2550, 2600, 2650,
 	2700, 2750, 2800, 2850, 2900, 2950, 3000, 3050,
diff --git a/drivers/mfd/twl-core.c b/drivers/mfd/twl-core.c
index 7c2267e71f8..6fc90befa79 100644
--- a/drivers/mfd/twl-core.c
+++ b/drivers/mfd/twl-core.c
@@ -224,13 +224,6 @@
 #define HIGH_PERF_SQ			(1 << 3)
 #define CK32K_LOWPWR_EN			(1 << 7)
 
-
-/* chip-specific feature flags, for i2c_device_id.driver_data */
-#define TWL4030_VAUX2		BIT(0)	/* pre-5030 voltage ranges */
-#define TPS_SUBSET		BIT(1)	/* tps659[23]0 have fewer LDOs */
-#define TWL5031			BIT(2)  /* twl5031 has different registers */
-#define TWL6030_CLASS		BIT(3)	/* TWL6030 class */
-
 /*----------------------------------------------------------------------*/
 
 /* is driver active, bound to a chip? */
diff --git a/drivers/mfd/twl4030-irq.c b/drivers/mfd/twl4030-irq.c
index 5d656e81435..ad733d76207 100644
--- a/drivers/mfd/twl4030-irq.c
+++ b/drivers/mfd/twl4030-irq.c
@@ -757,6 +757,7 @@ int twl4030_init_irq(struct device *dev, int irq_num)
 		dev_err(dev, "could not claim irq%d: %d\n", irq_num, status);
 		goto fail_rqirq;
 	}
+	enable_irq_wake(irq_num);
 
 	return irq_base;
 fail_rqirq:
diff --git a/drivers/mfd/twl6040-core.c b/drivers/mfd/twl6040-core.c
index 2d6bedadca0..4ded9e7aa24 100644
--- a/drivers/mfd/twl6040-core.c
+++ b/drivers/mfd/twl6040-core.c
@@ -27,7 +27,12 @@
 #include <linux/types.h>
 #include <linux/slab.h>
 #include <linux/kernel.h>
+#include <linux/err.h>
 #include <linux/platform_device.h>
+#include <linux/of.h>
+#include <linux/of_irq.h>
+#include <linux/of_gpio.h>
+#include <linux/of_platform.h>
 #include <linux/gpio.h>
 #include <linux/delay.h>
 #include <linux/i2c.h>
@@ -35,8 +40,24 @@
 #include <linux/err.h>
 #include <linux/mfd/core.h>
 #include <linux/mfd/twl6040.h>
+#include <linux/regulator/consumer.h>
 
 #define VIBRACTRL_MEMBER(reg) ((reg == TWL6040_REG_VIBCTLL) ? 0 : 1)
+#define TWL6040_NUM_SUPPLIES	(2)
+
+static bool twl6040_has_vibra(struct twl6040_platform_data *pdata,
+			      struct device_node *node)
+{
+	if (pdata && pdata->vibra)
+		return true;
+
+#ifdef CONFIG_OF
+	if (of_find_node_by_name(node, "vibra"))
+		return true;
+#endif
+
+	return false;
+}
 
 int twl6040_reg_read(struct twl6040 *twl6040, unsigned int reg)
 {
@@ -502,17 +523,18 @@ static int __devinit twl6040_probe(struct i2c_client *client,
 				     const struct i2c_device_id *id)
 {
 	struct twl6040_platform_data *pdata = client->dev.platform_data;
+	struct device_node *node = client->dev.of_node;
 	struct twl6040 *twl6040;
 	struct mfd_cell *cell = NULL;
-	int ret, children = 0;
+	int irq, ret, children = 0;
 
-	if (!pdata) {
+	if (!pdata && !node) {
 		dev_err(&client->dev, "Platform data is missing\n");
 		return -EINVAL;
 	}
 
 	/* In order to operate correctly we need valid interrupt config */
-	if (!client->irq || !pdata->irq_base) {
+	if (!client->irq) {
 		dev_err(&client->dev, "Invalid IRQ configuration\n");
 		return -EINVAL;
 	}
@@ -524,7 +546,7 @@ static int __devinit twl6040_probe(struct i2c_client *client,
 		goto err;
 	}
 
-	twl6040->regmap = regmap_init_i2c(client, &twl6040_regmap_config);
+	twl6040->regmap = devm_regmap_init_i2c(client, &twl6040_regmap_config);
 	if (IS_ERR(twl6040->regmap)) {
 		ret = PTR_ERR(twl6040->regmap);
 		goto err;
@@ -532,9 +554,23 @@ static int __devinit twl6040_probe(struct i2c_client *client,
 
 	i2c_set_clientdata(client, twl6040);
 
+	twl6040->supplies[0].supply = "vio";
+	twl6040->supplies[1].supply = "v2v1";
+	ret = regulator_bulk_get(&client->dev, TWL6040_NUM_SUPPLIES,
+				 twl6040->supplies);
+	if (ret != 0) {
+		dev_err(&client->dev, "Failed to get supplies: %d\n", ret);
+		goto regulator_get_err;
+	}
+
+	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;
+	}
+
 	twl6040->dev = &client->dev;
 	twl6040->irq = client->irq;
-	twl6040->irq_base = pdata->irq_base;
 
 	mutex_init(&twl6040->mutex);
 	mutex_init(&twl6040->io_mutex);
@@ -543,22 +579,26 @@ static int __devinit twl6040_probe(struct i2c_client *client,
 	twl6040->rev = twl6040_reg_read(twl6040, TWL6040_REG_ASICREV);
 
 	/* ERRATA: Automatic power-up is not possible in ES1.0 */
-	if (twl6040_get_revid(twl6040) > TWL6040_REV_ES1_0)
-		twl6040->audpwron = pdata->audpwron_gpio;
-	else
+	if (twl6040_get_revid(twl6040) > TWL6040_REV_ES1_0) {
+		if (pdata)
+			twl6040->audpwron = pdata->audpwron_gpio;
+		else
+			twl6040->audpwron = of_get_named_gpio(node,
+						"ti,audpwron-gpio", 0);
+	} else
 		twl6040->audpwron = -EINVAL;
 
 	if (gpio_is_valid(twl6040->audpwron)) {
 		ret = gpio_request_one(twl6040->audpwron, GPIOF_OUT_INIT_LOW,
 				       "audpwron");
 		if (ret)
-			goto gpio1_err;
+			goto gpio_err;
 	}
 
 	/* codec interrupt */
 	ret = twl6040_irq_init(twl6040);
 	if (ret)
-		goto gpio2_err;
+		goto irq_init_err;
 
 	ret = request_threaded_irq(twl6040->irq_base + TWL6040_IRQ_READY,
 				   NULL, twl6040_naudint_handler, 0,
@@ -572,22 +612,27 @@ static int __devinit twl6040_probe(struct i2c_client *client,
 	/* dual-access registers controlled by I2C only */
 	twl6040_set_bits(twl6040, TWL6040_REG_ACCCTL, TWL6040_I2CSEL);
 
-	if (pdata->codec) {
-		int irq = twl6040->irq_base + TWL6040_IRQ_PLUG;
-
-		cell = &twl6040->cells[children];
-		cell->name = "twl6040-codec";
-		twl6040_codec_rsrc[0].start = irq;
-		twl6040_codec_rsrc[0].end = irq;
-		cell->resources = twl6040_codec_rsrc;
-		cell->num_resources = ARRAY_SIZE(twl6040_codec_rsrc);
+	/*
+	 * The main functionality of twl6040 to provide audio on OMAP4+ systems.
+	 * We can add the ASoC codec child whenever this driver has been loaded.
+	 * The ASoC codec can work without pdata, pass the platform_data only if
+	 * it has been provided.
+	 */
+	irq = twl6040->irq_base + TWL6040_IRQ_PLUG;
+	cell = &twl6040->cells[children];
+	cell->name = "twl6040-codec";
+	twl6040_codec_rsrc[0].start = irq;
+	twl6040_codec_rsrc[0].end = irq;
+	cell->resources = twl6040_codec_rsrc;
+	cell->num_resources = ARRAY_SIZE(twl6040_codec_rsrc);
+	if (pdata && pdata->codec) {
 		cell->platform_data = pdata->codec;
 		cell->pdata_size = sizeof(*pdata->codec);
-		children++;
 	}
+	children++;
 
-	if (pdata->vibra) {
-		int irq = twl6040->irq_base + TWL6040_IRQ_VIB;
+	if (twl6040_has_vibra(pdata, node)) {
+		irq = twl6040->irq_base + TWL6040_IRQ_VIB;
 
 		cell = &twl6040->cells[children];
 		cell->name = "twl6040-vibra";
@@ -596,21 +641,17 @@ static int __devinit twl6040_probe(struct i2c_client *client,
 		cell->resources = twl6040_vibra_rsrc;
 		cell->num_resources = ARRAY_SIZE(twl6040_vibra_rsrc);
 
-		cell->platform_data = pdata->vibra;
-		cell->pdata_size = sizeof(*pdata->vibra);
+		if (pdata && pdata->vibra) {
+			cell->platform_data = pdata->vibra;
+			cell->pdata_size = sizeof(*pdata->vibra);
+		}
 		children++;
 	}
 
-	if (children) {
-		ret = mfd_add_devices(&client->dev, -1, twl6040->cells,
-				      children, NULL, 0);
-		if (ret)
-			goto mfd_err;
-	} else {
-		dev_err(&client->dev, "No platform data found for children\n");
-		ret = -ENODEV;
+	ret = mfd_add_devices(&client->dev, -1, twl6040->cells, children,
+			      NULL, 0);
+	if (ret)
 		goto mfd_err;
-	}
 
 	return 0;
 
@@ -618,12 +659,15 @@ mfd_err:
 	free_irq(twl6040->irq_base + TWL6040_IRQ_READY, twl6040);
 irq_err:
 	twl6040_irq_exit(twl6040);
-gpio2_err:
+irq_init_err:
 	if (gpio_is_valid(twl6040->audpwron))
 		gpio_free(twl6040->audpwron);
-gpio1_err:
+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);
-	regmap_exit(twl6040->regmap);
 err:
 	return ret;
 }
@@ -643,7 +687,9 @@ static int __devexit twl6040_remove(struct i2c_client *client)
 
 	mfd_remove_devices(&client->dev);
 	i2c_set_clientdata(client, NULL);
-	regmap_exit(twl6040->regmap);
+
+	regulator_bulk_disable(TWL6040_NUM_SUPPLIES, twl6040->supplies);
+	regulator_bulk_free(TWL6040_NUM_SUPPLIES, twl6040->supplies);
 
 	return 0;
 }
diff --git a/drivers/mfd/twl6040-irq.c b/drivers/mfd/twl6040-irq.c
index b3f8ddaa28a..4b42543da22 100644
--- a/drivers/mfd/twl6040-irq.c
+++ b/drivers/mfd/twl6040-irq.c
@@ -23,7 +23,10 @@
 
 #include <linux/kernel.h>
 #include <linux/module.h>
+#include <linux/err.h>
 #include <linux/irq.h>
+#include <linux/of.h>
+#include <linux/irqdomain.h>
 #include <linux/interrupt.h>
 #include <linux/mfd/core.h>
 #include <linux/mfd/twl6040.h>
@@ -138,7 +141,8 @@ static irqreturn_t twl6040_irq_thread(int irq, void *data)
 
 int twl6040_irq_init(struct twl6040 *twl6040)
 {
-	int cur_irq, ret;
+	struct device_node *node = twl6040->dev->of_node;
+	int i, nr_irqs, irq_base, ret;
 	u8 val;
 
 	mutex_init(&twl6040->irq_mutex);
@@ -148,21 +152,31 @@ int twl6040_irq_init(struct twl6040 *twl6040)
 	twl6040->irq_masks_cache = TWL6040_ALLINT_MSK;
 	twl6040_reg_write(twl6040, TWL6040_REG_INTMR, TWL6040_ALLINT_MSK);
 
+	nr_irqs = ARRAY_SIZE(twl6040_irqs);
+
+	irq_base = irq_alloc_descs(-1, 0, nr_irqs, 0);
+	if (IS_ERR_VALUE(irq_base)) {
+		dev_err(twl6040->dev, "Fail to allocate IRQ descs\n");
+		return irq_base;
+	}
+	twl6040->irq_base = irq_base;
+
+	irq_domain_add_legacy(node, ARRAY_SIZE(twl6040_irqs), irq_base, 0,
+			      &irq_domain_simple_ops, NULL);
+
 	/* Register them with genirq */
-	for (cur_irq = twl6040->irq_base;
-	     cur_irq < twl6040->irq_base + ARRAY_SIZE(twl6040_irqs);
-	     cur_irq++) {
-		irq_set_chip_data(cur_irq, twl6040);
-		irq_set_chip_and_handler(cur_irq, &twl6040_irq_chip,
+	for (i = irq_base; i < irq_base + nr_irqs; i++) {
+		irq_set_chip_data(i, twl6040);
+		irq_set_chip_and_handler(i, &twl6040_irq_chip,
 					 handle_level_irq);
-		irq_set_nested_thread(cur_irq, 1);
+		irq_set_nested_thread(i, 1);
 
 		/* ARM needs us to explicitly flag the IRQ as valid
 		 * and will set them noprobe when we do so. */
 #ifdef CONFIG_ARM
-		set_irq_flags(cur_irq, IRQF_VALID);
+		set_irq_flags(i, IRQF_VALID);
 #else
-		irq_set_noprobe(cur_irq);
+		irq_set_noprobe(i);
 #endif
 	}
 
diff --git a/drivers/mfd/vx855.c b/drivers/mfd/vx855.c
index b73cc15e008..872aff21e4b 100644
--- a/drivers/mfd/vx855.c
+++ b/drivers/mfd/vx855.c
@@ -131,17 +131,7 @@ static struct pci_driver vx855_pci_driver = {
 	.remove		= __devexit_p(vx855_remove),
 };
 
-static int vx855_init(void)
-{
-	return pci_register_driver(&vx855_pci_driver);
-}
-module_init(vx855_init);
-
-static void vx855_exit(void)
-{
-	pci_unregister_driver(&vx855_pci_driver);
-}
-module_exit(vx855_exit);
+module_pci_driver(vx855_pci_driver);
 
 MODULE_LICENSE("GPL");
 MODULE_AUTHOR("Harald Welte <HaraldWelte@viatech.com>");
diff --git a/drivers/mfd/wm831x-auxadc.c b/drivers/mfd/wm831x-auxadc.c
index 87210954a06..6ee3018d865 100644
--- a/drivers/mfd/wm831x-auxadc.c
+++ b/drivers/mfd/wm831x-auxadc.c
@@ -280,11 +280,11 @@ void wm831x_auxadc_init(struct wm831x *wm831x)
 	mutex_init(&wm831x->auxadc_lock);
 	INIT_LIST_HEAD(&wm831x->auxadc_pending);
 
-	if (wm831x->irq && wm831x->irq_base) {
+	if (wm831x->irq) {
 		wm831x->auxadc_read = wm831x_auxadc_read_irq;
 
-		ret = request_threaded_irq(wm831x->irq_base +
-					   WM831X_IRQ_AUXADC_DATA,
+		ret = request_threaded_irq(wm831x_irq(wm831x,
+						      WM831X_IRQ_AUXADC_DATA),
 					   NULL, wm831x_auxadc_irq, 0,
 					   "auxadc", wm831x);
 		if (ret < 0) {
diff --git a/drivers/mfd/wm831x-core.c b/drivers/mfd/wm831x-core.c
index 838056c3493..946698fd2dc 100644
--- a/drivers/mfd/wm831x-core.c
+++ b/drivers/mfd/wm831x-core.c
@@ -614,8 +614,15 @@ int wm831x_set_bits(struct wm831x *wm831x, unsigned short reg,
 }
 EXPORT_SYMBOL_GPL(wm831x_set_bits);
 
+static struct resource wm831x_io_parent = {
+	.start = 0,
+	.end   = 0xffffffff,
+	.flags = IORESOURCE_IO,
+};
+
 static struct resource wm831x_dcdc1_resources[] = {
 	{
+		.parent = &wm831x_io_parent,
 		.start = WM831X_DC1_CONTROL_1,
 		.end   = WM831X_DC1_DVS_CONTROL,
 		.flags = IORESOURCE_IO,
@@ -637,6 +644,7 @@ static struct resource wm831x_dcdc1_resources[] = {
 
 static struct resource wm831x_dcdc2_resources[] = {
 	{
+		.parent = &wm831x_io_parent,
 		.start = WM831X_DC2_CONTROL_1,
 		.end   = WM831X_DC2_DVS_CONTROL,
 		.flags = IORESOURCE_IO,
@@ -657,6 +665,7 @@ static struct resource wm831x_dcdc2_resources[] = {
 
 static struct resource wm831x_dcdc3_resources[] = {
 	{
+		.parent = &wm831x_io_parent,
 		.start = WM831X_DC3_CONTROL_1,
 		.end   = WM831X_DC3_SLEEP_CONTROL,
 		.flags = IORESOURCE_IO,
@@ -671,6 +680,7 @@ static struct resource wm831x_dcdc3_resources[] = {
 
 static struct resource wm831x_dcdc4_resources[] = {
 	{
+		.parent = &wm831x_io_parent,
 		.start = WM831X_DC4_CONTROL,
 		.end   = WM831X_DC4_SLEEP_CONTROL,
 		.flags = IORESOURCE_IO,
@@ -685,6 +695,7 @@ static struct resource wm831x_dcdc4_resources[] = {
 
 static struct resource wm8320_dcdc4_buck_resources[] = {
 	{
+		.parent = &wm831x_io_parent,
 		.start = WM831X_DC4_CONTROL,
 		.end   = WM832X_DC4_SLEEP_CONTROL,
 		.flags = IORESOURCE_IO,
@@ -707,6 +718,7 @@ static struct resource wm831x_gpio_resources[] = {
 
 static struct resource wm831x_isink1_resources[] = {
 	{
+		.parent = &wm831x_io_parent,
 		.start = WM831X_CURRENT_SINK_1,
 		.end   = WM831X_CURRENT_SINK_1,
 		.flags = IORESOURCE_IO,
@@ -720,6 +732,7 @@ static struct resource wm831x_isink1_resources[] = {
 
 static struct resource wm831x_isink2_resources[] = {
 	{
+		.parent = &wm831x_io_parent,
 		.start = WM831X_CURRENT_SINK_2,
 		.end   = WM831X_CURRENT_SINK_2,
 		.flags = IORESOURCE_IO,
@@ -733,6 +746,7 @@ static struct resource wm831x_isink2_resources[] = {
 
 static struct resource wm831x_ldo1_resources[] = {
 	{
+		.parent = &wm831x_io_parent,
 		.start = WM831X_LDO1_CONTROL,
 		.end   = WM831X_LDO1_SLEEP_CONTROL,
 		.flags = IORESOURCE_IO,
@@ -747,6 +761,7 @@ static struct resource wm831x_ldo1_resources[] = {
 
 static struct resource wm831x_ldo2_resources[] = {
 	{
+		.parent = &wm831x_io_parent,
 		.start = WM831X_LDO2_CONTROL,
 		.end   = WM831X_LDO2_SLEEP_CONTROL,
 		.flags = IORESOURCE_IO,
@@ -761,6 +776,7 @@ static struct resource wm831x_ldo2_resources[] = {
 
 static struct resource wm831x_ldo3_resources[] = {
 	{
+		.parent = &wm831x_io_parent,
 		.start = WM831X_LDO3_CONTROL,
 		.end   = WM831X_LDO3_SLEEP_CONTROL,
 		.flags = IORESOURCE_IO,
@@ -775,6 +791,7 @@ static struct resource wm831x_ldo3_resources[] = {
 
 static struct resource wm831x_ldo4_resources[] = {
 	{
+		.parent = &wm831x_io_parent,
 		.start = WM831X_LDO4_CONTROL,
 		.end   = WM831X_LDO4_SLEEP_CONTROL,
 		.flags = IORESOURCE_IO,
@@ -789,6 +806,7 @@ static struct resource wm831x_ldo4_resources[] = {
 
 static struct resource wm831x_ldo5_resources[] = {
 	{
+		.parent = &wm831x_io_parent,
 		.start = WM831X_LDO5_CONTROL,
 		.end   = WM831X_LDO5_SLEEP_CONTROL,
 		.flags = IORESOURCE_IO,
@@ -803,6 +821,7 @@ static struct resource wm831x_ldo5_resources[] = {
 
 static struct resource wm831x_ldo6_resources[] = {
 	{
+		.parent = &wm831x_io_parent,
 		.start = WM831X_LDO6_CONTROL,
 		.end   = WM831X_LDO6_SLEEP_CONTROL,
 		.flags = IORESOURCE_IO,
@@ -817,6 +836,7 @@ static struct resource wm831x_ldo6_resources[] = {
 
 static struct resource wm831x_ldo7_resources[] = {
 	{
+		.parent = &wm831x_io_parent,
 		.start = WM831X_LDO7_CONTROL,
 		.end   = WM831X_LDO7_SLEEP_CONTROL,
 		.flags = IORESOURCE_IO,
@@ -831,6 +851,7 @@ static struct resource wm831x_ldo7_resources[] = {
 
 static struct resource wm831x_ldo8_resources[] = {
 	{
+		.parent = &wm831x_io_parent,
 		.start = WM831X_LDO8_CONTROL,
 		.end   = WM831X_LDO8_SLEEP_CONTROL,
 		.flags = IORESOURCE_IO,
@@ -845,6 +866,7 @@ static struct resource wm831x_ldo8_resources[] = {
 
 static struct resource wm831x_ldo9_resources[] = {
 	{
+		.parent = &wm831x_io_parent,
 		.start = WM831X_LDO9_CONTROL,
 		.end   = WM831X_LDO9_SLEEP_CONTROL,
 		.flags = IORESOURCE_IO,
@@ -859,6 +881,7 @@ static struct resource wm831x_ldo9_resources[] = {
 
 static struct resource wm831x_ldo10_resources[] = {
 	{
+		.parent = &wm831x_io_parent,
 		.start = WM831X_LDO10_CONTROL,
 		.end   = WM831X_LDO10_SLEEP_CONTROL,
 		.flags = IORESOURCE_IO,
@@ -873,6 +896,7 @@ static struct resource wm831x_ldo10_resources[] = {
 
 static struct resource wm831x_ldo11_resources[] = {
 	{
+		.parent = &wm831x_io_parent,
 		.start = WM831X_LDO11_ON_CONTROL,
 		.end   = WM831X_LDO11_SLEEP_CONTROL,
 		.flags = IORESOURCE_IO,
@@ -974,6 +998,7 @@ static struct resource wm831x_rtc_resources[] = {
 
 static struct resource wm831x_status1_resources[] = {
 	{
+		.parent = &wm831x_io_parent,
 		.start = WM831X_STATUS_LED_1,
 		.end   = WM831X_STATUS_LED_1,
 		.flags = IORESOURCE_IO,
@@ -982,6 +1007,7 @@ static struct resource wm831x_status1_resources[] = {
 
 static struct resource wm831x_status2_resources[] = {
 	{
+		.parent = &wm831x_io_parent,
 		.start = WM831X_STATUS_LED_2,
 		.end   = WM831X_STATUS_LED_2,
 		.flags = IORESOURCE_IO,
@@ -1787,27 +1813,27 @@ int wm831x_device_init(struct wm831x *wm831x, unsigned long id, int irq)
 	case WM8310:
 		ret = mfd_add_devices(wm831x->dev, wm831x_num,
 				      wm8310_devs, ARRAY_SIZE(wm8310_devs),
-				      NULL, wm831x->irq_base);
+				      NULL, 0);
 		break;
 
 	case WM8311:
 		ret = mfd_add_devices(wm831x->dev, wm831x_num,
 				      wm8311_devs, ARRAY_SIZE(wm8311_devs),
-				      NULL, wm831x->irq_base);
+				      NULL, 0);
 		if (!pdata || !pdata->disable_touch)
 			mfd_add_devices(wm831x->dev, wm831x_num,
 					touch_devs, ARRAY_SIZE(touch_devs),
-					NULL, wm831x->irq_base);
+					NULL, 0);
 		break;
 
 	case WM8312:
 		ret = mfd_add_devices(wm831x->dev, wm831x_num,
 				      wm8312_devs, ARRAY_SIZE(wm8312_devs),
-				      NULL, wm831x->irq_base);
+				      NULL, 0);
 		if (!pdata || !pdata->disable_touch)
 			mfd_add_devices(wm831x->dev, wm831x_num,
 					touch_devs, ARRAY_SIZE(touch_devs),
-					NULL, wm831x->irq_base);
+					NULL, 0);
 		break;
 
 	case WM8320:
@@ -1816,7 +1842,7 @@ int wm831x_device_init(struct wm831x *wm831x, unsigned long id, int irq)
 	case WM8326:
 		ret = mfd_add_devices(wm831x->dev, wm831x_num,
 				      wm8320_devs, ARRAY_SIZE(wm8320_devs),
-				      NULL, wm831x->irq_base);
+				      NULL, 0);
 		break;
 
 	default:
@@ -1841,7 +1867,7 @@ int wm831x_device_init(struct wm831x *wm831x, unsigned long id, int irq)
 	if (ret & WM831X_XTAL_ENA) {
 		ret = mfd_add_devices(wm831x->dev, wm831x_num,
 				      rtc_devs, ARRAY_SIZE(rtc_devs),
-				      NULL, wm831x->irq_base);
+				      NULL, 0);
 		if (ret != 0) {
 			dev_err(wm831x->dev, "Failed to add RTC: %d\n", ret);
 			goto err_irq;
@@ -1854,7 +1880,7 @@ int wm831x_device_init(struct wm831x *wm831x, unsigned long id, int irq)
 		/* Treat errors as non-critical */
 		ret = mfd_add_devices(wm831x->dev, wm831x_num, backlight_devs,
 				      ARRAY_SIZE(backlight_devs), NULL,
-				      wm831x->irq_base);
+				      0);
 		if (ret < 0)
 			dev_err(wm831x->dev, "Failed to add backlight: %d\n",
 				ret);
@@ -1883,8 +1909,7 @@ void wm831x_device_exit(struct wm831x *wm831x)
 {
 	wm831x_otp_exit(wm831x);
 	mfd_remove_devices(wm831x->dev);
-	if (wm831x->irq_base)
-		free_irq(wm831x->irq_base + WM831X_IRQ_AUXADC_DATA, wm831x);
+	free_irq(wm831x_irq(wm831x, WM831X_IRQ_AUXADC_DATA), wm831x);
 	wm831x_irq_exit(wm831x);
 }
 
diff --git a/drivers/mfd/wm831x-irq.c b/drivers/mfd/wm831x-irq.c
index bec4d053916..804e56ec99e 100644
--- a/drivers/mfd/wm831x-irq.c
+++ b/drivers/mfd/wm831x-irq.c
@@ -18,6 +18,7 @@
 #include <linux/irq.h>
 #include <linux/mfd/core.h>
 #include <linux/interrupt.h>
+#include <linux/irqdomain.h>
 
 #include <linux/mfd/wm831x/core.h>
 #include <linux/mfd/wm831x/pdata.h>
@@ -328,7 +329,7 @@ static inline int irq_data_to_status_reg(struct wm831x_irq_data *irq_data)
 static inline struct wm831x_irq_data *irq_to_wm831x_irq(struct wm831x *wm831x,
 							int irq)
 {
-	return &wm831x_irqs[irq - wm831x->irq_base];
+	return &wm831x_irqs[irq];
 }
 
 static void wm831x_irq_lock(struct irq_data *data)
@@ -374,7 +375,7 @@ static void wm831x_irq_enable(struct irq_data *data)
 {
 	struct wm831x *wm831x = irq_data_get_irq_chip_data(data);
 	struct wm831x_irq_data *irq_data = irq_to_wm831x_irq(wm831x,
-							     data->irq);
+							     data->hwirq);
 
 	wm831x->irq_masks_cur[irq_data->reg - 1] &= ~irq_data->mask;
 }
@@ -383,7 +384,7 @@ static void wm831x_irq_disable(struct irq_data *data)
 {
 	struct wm831x *wm831x = irq_data_get_irq_chip_data(data);
 	struct wm831x_irq_data *irq_data = irq_to_wm831x_irq(wm831x,
-							     data->irq);
+							     data->hwirq);
 
 	wm831x->irq_masks_cur[irq_data->reg - 1] |= irq_data->mask;
 }
@@ -393,7 +394,7 @@ static int wm831x_irq_set_type(struct irq_data *data, unsigned int type)
 	struct wm831x *wm831x = irq_data_get_irq_chip_data(data);
 	int irq;
 
-	irq = data->irq - wm831x->irq_base;
+	irq = data->hwirq;
 
 	if (irq < WM831X_IRQ_GPIO_1 || irq > WM831X_IRQ_GPIO_11) {
 		/* Ignore internal-only IRQs */
@@ -412,22 +413,25 @@ static int wm831x_irq_set_type(struct irq_data *data, unsigned int type)
 	 * do the update here as we can be called with the bus lock
 	 * held.
 	 */
+	wm831x->gpio_level_low[irq] = false;
+	wm831x->gpio_level_high[irq] = false;
 	switch (type) {
 	case IRQ_TYPE_EDGE_BOTH:
 		wm831x->gpio_update[irq] = 0x10000 | WM831X_GPN_INT_MODE;
-		wm831x->gpio_level[irq] = false;
 		break;
 	case IRQ_TYPE_EDGE_RISING:
 		wm831x->gpio_update[irq] = 0x10000 | WM831X_GPN_POL;
-		wm831x->gpio_level[irq] = false;
 		break;
 	case IRQ_TYPE_EDGE_FALLING:
 		wm831x->gpio_update[irq] = 0x10000;
-		wm831x->gpio_level[irq] = false;
 		break;
 	case IRQ_TYPE_LEVEL_HIGH:
 		wm831x->gpio_update[irq] = 0x10000 | WM831X_GPN_POL;
-		wm831x->gpio_level[irq] = true;
+		wm831x->gpio_level_high[irq] = true;
+		break;
+	case IRQ_TYPE_LEVEL_LOW:
+		wm831x->gpio_update[irq] = 0x10000;
+		wm831x->gpio_level_low[irq] = true;
 		break;
 	default:
 		return -EINVAL;
@@ -469,9 +473,11 @@ static irqreturn_t wm831x_irq_thread(int irq, void *data)
 	 * descriptors.
 	 */
 	if (primary & WM831X_TCHPD_INT)
-		handle_nested_irq(wm831x->irq_base + WM831X_IRQ_TCHPD);
+		handle_nested_irq(irq_find_mapping(wm831x->irq_domain,
+						   WM831X_IRQ_TCHPD));
 	if (primary & WM831X_TCHDATA_INT)
-		handle_nested_irq(wm831x->irq_base + WM831X_IRQ_TCHDATA);
+		handle_nested_irq(irq_find_mapping(wm831x->irq_domain,
+						   WM831X_IRQ_TCHDATA));
 	primary &= ~(WM831X_TCHDATA_EINT | WM831X_TCHPD_EINT);
 
 	for (i = 0; i < ARRAY_SIZE(wm831x_irqs); i++) {
@@ -507,16 +513,29 @@ static irqreturn_t wm831x_irq_thread(int irq, void *data)
 		}
 
 		if (*status & wm831x_irqs[i].mask)
-			handle_nested_irq(wm831x->irq_base + i);
+			handle_nested_irq(irq_find_mapping(wm831x->irq_domain,
+							   i));
 
 		/* Simulate an edge triggered IRQ by polling the input
 		 * status.  This is sucky but improves interoperability.
 		 */
 		if (primary == WM831X_GP_INT &&
-		    wm831x->gpio_level[i - WM831X_IRQ_GPIO_1]) {
+		    wm831x->gpio_level_high[i - WM831X_IRQ_GPIO_1]) {
 			ret = wm831x_reg_read(wm831x, WM831X_GPIO_LEVEL);
 			while (ret & 1 << (i - WM831X_IRQ_GPIO_1)) {
-				handle_nested_irq(wm831x->irq_base + i);
+				handle_nested_irq(irq_find_mapping(wm831x->irq_domain,
+								   i));
+				ret = wm831x_reg_read(wm831x,
+						      WM831X_GPIO_LEVEL);
+			}
+		}
+
+		if (primary == WM831X_GP_INT &&
+		    wm831x->gpio_level_low[i - WM831X_IRQ_GPIO_1]) {
+			ret = wm831x_reg_read(wm831x, WM831X_GPIO_LEVEL);
+			while (!(ret & 1 << (i - WM831X_IRQ_GPIO_1))) {
+				handle_nested_irq(irq_find_mapping(wm831x->irq_domain,
+								   i));
 				ret = wm831x_reg_read(wm831x,
 						      WM831X_GPIO_LEVEL);
 			}
@@ -527,10 +546,34 @@ out:
 	return IRQ_HANDLED;
 }
 
+static int wm831x_irq_map(struct irq_domain *h, unsigned int virq,
+			  irq_hw_number_t hw)
+{
+	irq_set_chip_data(virq, h->host_data);
+	irq_set_chip_and_handler(virq, &wm831x_irq_chip, handle_edge_irq);
+	irq_set_nested_thread(virq, 1);
+
+	/* ARM needs us to explicitly flag the IRQ as valid
+	 * and will set them noprobe when we do so. */
+#ifdef CONFIG_ARM
+	set_irq_flags(virq, IRQF_VALID);
+#else
+	irq_set_noprobe(virq);
+#endif
+
+	return 0;
+}
+
+static struct irq_domain_ops wm831x_irq_domain_ops = {
+	.map	= wm831x_irq_map,
+	.xlate	= irq_domain_xlate_twocell,
+};
+
 int wm831x_irq_init(struct wm831x *wm831x, int irq)
 {
 	struct wm831x_pdata *pdata = wm831x->dev->platform_data;
-	int i, cur_irq, ret;
+	struct irq_domain *domain;
+	int i, ret, irq_base;
 
 	mutex_init(&wm831x->irq_lock);
 
@@ -543,18 +586,33 @@ int wm831x_irq_init(struct wm831x *wm831x, int irq)
 	}
 
 	/* Try to dynamically allocate IRQs if no base is specified */
-	if (!pdata || !pdata->irq_base)
-		wm831x->irq_base = -1;
+	if (pdata && pdata->irq_base) {
+		irq_base = irq_alloc_descs(pdata->irq_base, 0,
+					   WM831X_NUM_IRQS, 0);
+		if (irq_base < 0) {
+			dev_warn(wm831x->dev, "Failed to allocate IRQs: %d\n",
+				 irq_base);
+			irq_base = 0;
+		}
+	} else {
+		irq_base = 0;
+	}
+
+	if (irq_base)
+		domain = irq_domain_add_legacy(wm831x->dev->of_node,
+					       ARRAY_SIZE(wm831x_irqs),
+					       irq_base, 0,
+					       &wm831x_irq_domain_ops,
+					       wm831x);
 	else
-		wm831x->irq_base = pdata->irq_base;
+		domain = irq_domain_add_linear(wm831x->dev->of_node,
+					       ARRAY_SIZE(wm831x_irqs),
+					       &wm831x_irq_domain_ops,
+					       wm831x);
 
-	wm831x->irq_base = irq_alloc_descs(wm831x->irq_base, 0,
-					   WM831X_NUM_IRQS, 0);
-	if (wm831x->irq_base < 0) {
-		dev_warn(wm831x->dev, "Failed to allocate IRQs: %d\n",
-			 wm831x->irq_base);
-		wm831x->irq_base = 0;
-		return 0;
+	if (!domain) {
+		dev_warn(wm831x->dev, "Failed to allocate IRQ domain\n");
+		return -EINVAL;
 	}
 
 	if (pdata && pdata->irq_cmos)
@@ -565,38 +623,22 @@ int wm831x_irq_init(struct wm831x *wm831x, int irq)
 	wm831x_set_bits(wm831x, WM831X_IRQ_CONFIG,
 			WM831X_IRQ_OD, i);
 
-	/* Try to flag /IRQ as a wake source; there are a number of
-	 * unconditional wake sources in the PMIC so this isn't
-	 * conditional but we don't actually care *too* much if it
-	 * fails.
-	 */
-	ret = enable_irq_wake(irq);
-	if (ret != 0) {
-		dev_warn(wm831x->dev, "Can't enable IRQ as wake source: %d\n",
-			 ret);
-	}
-
 	wm831x->irq = irq;
-
-	/* Register them with genirq */
-	for (cur_irq = wm831x->irq_base;
-	     cur_irq < ARRAY_SIZE(wm831x_irqs) + wm831x->irq_base;
-	     cur_irq++) {
-		irq_set_chip_data(cur_irq, wm831x);
-		irq_set_chip_and_handler(cur_irq, &wm831x_irq_chip,
-					 handle_edge_irq);
-		irq_set_nested_thread(cur_irq, 1);
-
-		/* ARM needs us to explicitly flag the IRQ as valid
-		 * and will set them noprobe when we do so. */
-#ifdef CONFIG_ARM
-		set_irq_flags(cur_irq, IRQF_VALID);
-#else
-		irq_set_noprobe(cur_irq);
-#endif
-	}
+	wm831x->irq_domain = domain;
 
 	if (irq) {
+		/* Try to flag /IRQ as a wake source; there are a number of
+		 * unconditional wake sources in the PMIC so this isn't
+		 * conditional but we don't actually care *too* much if it
+		 * fails.
+		 */
+		ret = enable_irq_wake(irq);
+		if (ret != 0) {
+			dev_warn(wm831x->dev,
+				 "Can't enable IRQ as wake source: %d\n",
+				 ret);
+		}
+
 		ret = request_threaded_irq(irq, NULL, wm831x_irq_thread,
 					   IRQF_TRIGGER_LOW | IRQF_ONESHOT,
 					   "wm831x", wm831x);
diff --git a/drivers/mfd/wm8350-core.c b/drivers/mfd/wm8350-core.c
index dd1caaac55e..8a9b11ca076 100644
--- a/drivers/mfd/wm8350-core.c
+++ b/drivers/mfd/wm8350-core.c
@@ -20,6 +20,7 @@
 #include <linux/device.h>
 #include <linux/delay.h>
 #include <linux/interrupt.h>
+#include <linux/regmap.h>
 #include <linux/workqueue.h>
 
 #include <linux/mfd/wm8350/core.h>
@@ -74,7 +75,7 @@ static int wm8350_phys_read(struct wm8350 *wm8350, u8 reg, int num_regs,
 	int bytes = num_regs * 2;
 
 	dev_dbg(wm8350->dev, "volatile read\n");
-	ret = wm8350->read_dev(wm8350, reg, bytes, (char *)dest);
+	ret = regmap_raw_read(wm8350->regmap, reg, dest, bytes);
 
 	for (i = reg; i < reg + num_regs; i++) {
 		/* Cache is CPU endian */
@@ -96,9 +97,6 @@ static int wm8350_read(struct wm8350 *wm8350, u8 reg, int num_regs, u16 *dest)
 	int ret = 0;
 	int bytes = num_regs * 2;
 
-	if (wm8350->read_dev == NULL)
-		return -ENODEV;
-
 	if ((reg + num_regs - 1) > WM8350_MAX_REGISTER) {
 		dev_err(wm8350->dev, "invalid reg %x\n",
 			reg + num_regs - 1);
@@ -149,9 +147,6 @@ static int wm8350_write(struct wm8350 *wm8350, u8 reg, int num_regs, u16 *src)
 	int end = reg + num_regs;
 	int bytes = num_regs * 2;
 
-	if (wm8350->write_dev == NULL)
-		return -ENODEV;
-
 	if ((reg + num_regs - 1) > WM8350_MAX_REGISTER) {
 		dev_err(wm8350->dev, "invalid reg %x\n",
 			reg + num_regs - 1);
@@ -182,7 +177,7 @@ static int wm8350_write(struct wm8350 *wm8350, u8 reg, int num_regs, u16 *src)
 	}
 
 	/* Actually write it out */
-	return wm8350->write_dev(wm8350, reg, bytes, (char *)src);
+	return regmap_raw_write(wm8350->regmap, reg, src, bytes);
 }
 
 /*
@@ -515,9 +510,8 @@ static int wm8350_create_cache(struct wm8350 *wm8350, int type, int mode)
 	 * a PMIC so the device many not be in a virgin state and we
 	 * can't rely on the silicon values.
 	 */
-	ret = wm8350->read_dev(wm8350, 0,
-			       sizeof(u16) * (WM8350_MAX_REGISTER + 1),
-			       wm8350->reg_cache);
+	ret = regmap_raw_read(wm8350->regmap, 0, wm8350->reg_cache,
+			      sizeof(u16) * (WM8350_MAX_REGISTER + 1));
 	if (ret < 0) {
 		dev_err(wm8350->dev,
 			"failed to read initial cache values\n");
@@ -570,35 +564,30 @@ int wm8350_device_init(struct wm8350 *wm8350, int irq,
 		       struct wm8350_platform_data *pdata)
 {
 	int ret;
-	u16 id1, id2, mask_rev;
-	u16 cust_id, mode, chip_rev;
+	unsigned int id1, id2, mask_rev;
+	unsigned int cust_id, mode, chip_rev;
 
 	dev_set_drvdata(wm8350->dev, wm8350);
 
 	/* get WM8350 revision and config mode */
-	ret = wm8350->read_dev(wm8350, WM8350_RESET_ID, sizeof(id1), &id1);
+	ret = regmap_read(wm8350->regmap, WM8350_RESET_ID, &id1);
 	if (ret != 0) {
 		dev_err(wm8350->dev, "Failed to read ID: %d\n", ret);
 		goto err;
 	}
 
-	ret = wm8350->read_dev(wm8350, WM8350_ID, sizeof(id2), &id2);
+	ret = regmap_read(wm8350->regmap, WM8350_ID, &id2);
 	if (ret != 0) {
 		dev_err(wm8350->dev, "Failed to read ID: %d\n", ret);
 		goto err;
 	}
 
-	ret = wm8350->read_dev(wm8350, WM8350_REVISION, sizeof(mask_rev),
-			       &mask_rev);
+	ret = regmap_read(wm8350->regmap, WM8350_REVISION, &mask_rev);
 	if (ret != 0) {
 		dev_err(wm8350->dev, "Failed to read revision: %d\n", ret);
 		goto err;
 	}
 
-	id1 = be16_to_cpu(id1);
-	id2 = be16_to_cpu(id2);
-	mask_rev = be16_to_cpu(mask_rev);
-
 	if (id1 != 0x6143) {
 		dev_err(wm8350->dev,
 			"Device with ID %x is not a WM8350\n", id1);
diff --git a/drivers/mfd/wm8350-i2c.c b/drivers/mfd/wm8350-i2c.c
index d955faaf27c..a68aceb4e48 100644
--- a/drivers/mfd/wm8350-i2c.c
+++ b/drivers/mfd/wm8350-i2c.c
@@ -15,47 +15,18 @@
 
 #include <linux/module.h>
 #include <linux/moduleparam.h>
+#include <linux/err.h>
 #include <linux/init.h>
 #include <linux/i2c.h>
 #include <linux/platform_device.h>
 #include <linux/mfd/wm8350/core.h>
+#include <linux/regmap.h>
 #include <linux/slab.h>
 
-static int wm8350_i2c_read_device(struct wm8350 *wm8350, char reg,
-				  int bytes, void *dest)
-{
-	int ret;
-
-	ret = i2c_master_send(wm8350->i2c_client, &reg, 1);
-	if (ret < 0)
-		return ret;
-	ret = i2c_master_recv(wm8350->i2c_client, dest, bytes);
-	if (ret < 0)
-		return ret;
-	if (ret != bytes)
-		return -EIO;
-	return 0;
-}
-
-static int wm8350_i2c_write_device(struct wm8350 *wm8350, char reg,
-				   int bytes, void *src)
-{
-	/* we add 1 byte for device register */
-	u8 msg[(WM8350_MAX_REGISTER << 1) + 1];
-	int ret;
-
-	if (bytes > ((WM8350_MAX_REGISTER << 1) + 1))
-		return -EINVAL;
-
-	msg[0] = reg;
-	memcpy(&msg[1], src, bytes);
-	ret = i2c_master_send(wm8350->i2c_client, msg, bytes + 1);
-	if (ret < 0)
-		return ret;
-	if (ret != bytes + 1)
-		return -EIO;
-	return 0;
-}
+static const struct regmap_config wm8350_regmap = {
+	.reg_bits = 8,
+	.val_bits = 16,
+};
 
 static int wm8350_i2c_probe(struct i2c_client *i2c,
 			    const struct i2c_device_id *id)
@@ -67,20 +38,18 @@ static int wm8350_i2c_probe(struct i2c_client *i2c,
 	if (wm8350 == NULL)
 		return -ENOMEM;
 
+	wm8350->regmap = devm_regmap_init_i2c(i2c, &wm8350_regmap);
+	if (IS_ERR(wm8350->regmap)) {
+		ret = PTR_ERR(wm8350->regmap);
+		dev_err(&i2c->dev, "Failed to allocate register map: %d\n",
+			ret);
+		return ret;
+	}
+
 	i2c_set_clientdata(i2c, wm8350);
 	wm8350->dev = &i2c->dev;
-	wm8350->i2c_client = i2c;
-	wm8350->read_dev = wm8350_i2c_read_device;
-	wm8350->write_dev = wm8350_i2c_write_device;
-
-	ret = wm8350_device_init(wm8350, i2c->irq, i2c->dev.platform_data);
-	if (ret < 0)
-		goto err;
-
-	return ret;
 
-err:
-	return ret;
+	return wm8350_device_init(wm8350, i2c->irq, i2c->dev.platform_data);
 }
 
 static int wm8350_i2c_remove(struct i2c_client *i2c)
diff --git a/drivers/mfd/wm8400-core.c b/drivers/mfd/wm8400-core.c
index 1189a17f0f2..4b7d378551d 100644
--- a/drivers/mfd/wm8400-core.c
+++ b/drivers/mfd/wm8400-core.c
@@ -23,136 +23,16 @@
 #include <linux/regmap.h>
 #include <linux/slab.h>
 
-static struct {
-	u16  readable;    /* Mask of readable bits */
-	u16  writable;    /* Mask of writable bits */
-	u16  vol;         /* Mask of volatile bits */
-	int  is_codec;    /* Register controlled by codec reset */
-	u16  default_val; /* Value on reset */
-} reg_data[] = {
-	{ 0xFFFF, 0xFFFF, 0x0000, 0, 0x6172 }, /* R0 */
-	{ 0x7000, 0x0000, 0x8000, 0, 0x0000 }, /* R1 */
-	{ 0xFF17, 0xFF17, 0x0000, 0, 0x0000 }, /* R2 */
-	{ 0xEBF3, 0xEBF3, 0x0000, 1, 0x6000 }, /* R3 */
-	{ 0x3CF3, 0x3CF3, 0x0000, 1, 0x0000 }, /* R4  */
-	{ 0xF1F8, 0xF1F8, 0x0000, 1, 0x4050 }, /* R5  */
-	{ 0xFC1F, 0xFC1F, 0x0000, 1, 0x4000 }, /* R6  */
-	{ 0xDFDE, 0xDFDE, 0x0000, 1, 0x01C8 }, /* R7  */
-	{ 0xFCFC, 0xFCFC, 0x0000, 1, 0x0000 }, /* R8  */
-	{ 0xEFFF, 0xEFFF, 0x0000, 1, 0x0040 }, /* R9  */
-	{ 0xEFFF, 0xEFFF, 0x0000, 1, 0x0040 }, /* R10 */
-	{ 0x27F7, 0x27F7, 0x0000, 1, 0x0004 }, /* R11 */
-	{ 0x01FF, 0x01FF, 0x0000, 1, 0x00C0 }, /* R12 */
-	{ 0x01FF, 0x01FF, 0x0000, 1, 0x00C0 }, /* R13 */
-	{ 0x1FEF, 0x1FEF, 0x0000, 1, 0x0000 }, /* R14 */
-	{ 0x0163, 0x0163, 0x0000, 1, 0x0100 }, /* R15 */
-	{ 0x01FF, 0x01FF, 0x0000, 1, 0x00C0 }, /* R16 */
-	{ 0x01FF, 0x01FF, 0x0000, 1, 0x00C0 }, /* R17 */
-	{ 0x1FFF, 0x0FFF, 0x0000, 1, 0x0000 }, /* R18 */
-	{ 0xFFFF, 0xFFFF, 0x0000, 1, 0x1000 }, /* R19 */
-	{ 0xFFFF, 0xFFFF, 0x0000, 1, 0x1010 }, /* R20 */
-	{ 0xFFFF, 0xFFFF, 0x0000, 1, 0x1010 }, /* R21 */
-	{ 0x0FDD, 0x0FDD, 0x0000, 1, 0x8000 }, /* R22 */
-	{ 0x1FFF, 0x1FFF, 0x0000, 1, 0x0800 }, /* R23 */
-	{ 0x0000, 0x01DF, 0x0000, 1, 0x008B }, /* R24 */
-	{ 0x0000, 0x01DF, 0x0000, 1, 0x008B }, /* R25 */
-	{ 0x0000, 0x01DF, 0x0000, 1, 0x008B }, /* R26 */
-	{ 0x0000, 0x01DF, 0x0000, 1, 0x008B }, /* R27 */
-	{ 0x0000, 0x01FF, 0x0000, 1, 0x0000 }, /* R28 */
-	{ 0x0000, 0x01FF, 0x0000, 1, 0x0000 }, /* R29 */
-	{ 0x0000, 0x0077, 0x0000, 1, 0x0066 }, /* R30 */
-	{ 0x0000, 0x0033, 0x0000, 1, 0x0022 }, /* R31 */
-	{ 0x0000, 0x01FF, 0x0000, 1, 0x0079 }, /* R32 */
-	{ 0x0000, 0x01FF, 0x0000, 1, 0x0079 }, /* R33 */
-	{ 0x0000, 0x0003, 0x0000, 1, 0x0003 }, /* R34 */
-	{ 0x0000, 0x01FF, 0x0000, 1, 0x0003 }, /* R35 */
-	{ 0x0000, 0x0000, 0x0000, 0, 0x0000 }, /* R36 */
-	{ 0x0000, 0x003F, 0x0000, 1, 0x0100 }, /* R37 */
-	{ 0x0000, 0x0000, 0x0000, 0, 0x0000 }, /* R38 */
-	{ 0x0000, 0x000F, 0x0000, 0, 0x0000 }, /* R39 */
-	{ 0x0000, 0x00FF, 0x0000, 1, 0x0000 }, /* R40 */
-	{ 0x0000, 0x01B7, 0x0000, 1, 0x0000 }, /* R41 */
-	{ 0x0000, 0x01B7, 0x0000, 1, 0x0000 }, /* R42 */
-	{ 0x0000, 0x01FF, 0x0000, 1, 0x0000 }, /* R43 */
-	{ 0x0000, 0x01FF, 0x0000, 1, 0x0000 }, /* R44 */
-	{ 0x0000, 0x00FD, 0x0000, 1, 0x0000 }, /* R45 */
-	{ 0x0000, 0x00FD, 0x0000, 1, 0x0000 }, /* R46 */
-	{ 0x0000, 0x01FF, 0x0000, 1, 0x0000 }, /* R47 */
-	{ 0x0000, 0x01FF, 0x0000, 1, 0x0000 }, /* R48 */
-	{ 0x0000, 0x01FF, 0x0000, 1, 0x0000 }, /* R49 */
-	{ 0x0000, 0x01FF, 0x0000, 1, 0x0000 }, /* R50 */
-	{ 0x0000, 0x01B3, 0x0000, 1, 0x0180 }, /* R51 */
-	{ 0x0000, 0x0077, 0x0000, 1, 0x0000 }, /* R52 */
-	{ 0x0000, 0x0077, 0x0000, 1, 0x0000 }, /* R53 */
-	{ 0x0000, 0x00FF, 0x0000, 1, 0x0000 }, /* R54 */
-	{ 0x0000, 0x0001, 0x0000, 1, 0x0000 }, /* R55 */
-	{ 0x0000, 0x003F, 0x0000, 1, 0x0000 }, /* R56 */
-	{ 0x0000, 0x004F, 0x0000, 1, 0x0000 }, /* R57 */
-	{ 0x0000, 0x00FD, 0x0000, 1, 0x0000 }, /* R58 */
-	{ 0x0000, 0x0000, 0x0000, 0, 0x0000 }, /* R59 */
-	{ 0x1FFF, 0x1FFF, 0x0000, 1, 0x0000 }, /* R60 */
-	{ 0xFFFF, 0xFFFF, 0x0000, 1, 0x0000 }, /* R61 */
-	{ 0x03FF, 0x03FF, 0x0000, 1, 0x0000 }, /* R62 */
-	{ 0x007F, 0x007F, 0x0000, 1, 0x0000 }, /* R63 */
-	{ 0x0000, 0x0000, 0x0000, 0, 0x0000 }, /* R64 */
-	{ 0xDFFF, 0xDFFF, 0x0000, 0, 0x0000 }, /* R65 */
-	{ 0xDFFF, 0xDFFF, 0x0000, 0, 0x0000 }, /* R66 */
-	{ 0xDFFF, 0xDFFF, 0x0000, 0, 0x0000 }, /* R67 */
-	{ 0xDFFF, 0xDFFF, 0x0000, 0, 0x0000 }, /* R68 */
-	{ 0x0000, 0x0000, 0x0000, 0, 0x0000 }, /* R69 */
-	{ 0xFFFF, 0xFFFF, 0x0000, 0, 0x4400 }, /* R70 */
-	{ 0x23FF, 0x23FF, 0x0000, 0, 0x0000 }, /* R71 */
-	{ 0xFFFF, 0xFFFF, 0x0000, 0, 0x4400 }, /* R72 */
-	{ 0x23FF, 0x23FF, 0x0000, 0, 0x0000 }, /* R73 */
-	{ 0x0000, 0x0000, 0x0000, 0, 0x0000 }, /* R74 */
-	{ 0x000E, 0x000E, 0x0000, 0, 0x0008 }, /* R75 */
-	{ 0xE00F, 0xE00F, 0x0000, 0, 0x0000 }, /* R76 */
-	{ 0x0000, 0x0000, 0x0000, 0, 0x0000 }, /* R77 */
-	{ 0x03C0, 0x03C0, 0x0000, 0, 0x02C0 }, /* R78 */
-	{ 0xFFFF, 0x0000, 0xffff, 0, 0x0000 }, /* R79 */
-	{ 0xFFFF, 0xFFFF, 0x0000, 0, 0x0000 }, /* R80 */
-	{ 0xFFFF, 0x0000, 0xffff, 0, 0x0000 }, /* R81 */
-	{ 0x2BFF, 0x0000, 0xffff, 0, 0x0000 }, /* R82 */
-	{ 0x0000, 0x0000, 0x0000, 0, 0x0000 }, /* R83 */
-	{ 0x80FF, 0x80FF, 0x0000, 0, 0x00ff }, /* R84 */
-};
-
-static int wm8400_read(struct wm8400 *wm8400, u8 reg, int num_regs, u16 *dest)
+static bool wm8400_volatile(struct device *dev, unsigned int reg)
 {
-	int i, ret = 0;
-
-	BUG_ON(reg + num_regs > ARRAY_SIZE(wm8400->reg_cache));
-
-	/* If there are any volatile reads then read back the entire block */
-	for (i = reg; i < reg + num_regs; i++)
-		if (reg_data[i].vol) {
-			ret = regmap_bulk_read(wm8400->regmap, reg, dest,
-					       num_regs);
-			return ret;
-		}
-
-	/* Otherwise use the cache */
-	memcpy(dest, &wm8400->reg_cache[reg], num_regs * sizeof(u16));
-
-	return 0;
-}
-
-static int wm8400_write(struct wm8400 *wm8400, u8 reg, int num_regs,
-			u16 *src)
-{
-	int ret, i;
-
-	BUG_ON(reg + num_regs > ARRAY_SIZE(wm8400->reg_cache));
-
-	for (i = 0; i < num_regs; i++) {
-		BUG_ON(!reg_data[reg + i].writable);
-		wm8400->reg_cache[reg + i] = src[i];
-		ret = regmap_write(wm8400->regmap, reg, src[i]);
-		if (ret != 0)
-			return ret;
+	switch (reg) {
+	case WM8400_INTERRUPT_STATUS_1:
+	case WM8400_INTERRUPT_LEVELS:
+	case WM8400_SHUTDOWN_REASON:
+		return true;
+	default:
+		return false;
 	}
-
-	return 0;
 }
 
 /**
@@ -165,13 +45,12 @@ static int wm8400_write(struct wm8400 *wm8400, u8 reg, int num_regs,
  */
 u16 wm8400_reg_read(struct wm8400 *wm8400, u8 reg)
 {
-	u16 val;
-
-	mutex_lock(&wm8400->io_lock);
-
-	wm8400_read(wm8400, reg, 1, &val);
+	unsigned int val;
+	int ret;
 
-	mutex_unlock(&wm8400->io_lock);
+	ret = regmap_read(wm8400->regmap, reg, &val);
+	if (ret < 0)
+		return ret;
 
 	return val;
 }
@@ -179,63 +58,10 @@ EXPORT_SYMBOL_GPL(wm8400_reg_read);
 
 int wm8400_block_read(struct wm8400 *wm8400, u8 reg, int count, u16 *data)
 {
-	int ret;
-
-	mutex_lock(&wm8400->io_lock);
-
-	ret = wm8400_read(wm8400, reg, count, data);
-
-	mutex_unlock(&wm8400->io_lock);
-
-	return ret;
+	return regmap_bulk_read(wm8400->regmap, reg, data, count);
 }
 EXPORT_SYMBOL_GPL(wm8400_block_read);
 
-/**
- * wm8400_set_bits - Bitmask write
- *
- * @wm8400: Pointer to wm8400 control structure
- * @reg:    Register to access
- * @mask:   Mask of bits to change
- * @val:    Value to set for masked bits
- */
-int wm8400_set_bits(struct wm8400 *wm8400, u8 reg, u16 mask, u16 val)
-{
-	u16 tmp;
-	int ret;
-
-	mutex_lock(&wm8400->io_lock);
-
-	ret = wm8400_read(wm8400, reg, 1, &tmp);
-	tmp = (tmp & ~mask) | val;
-	if (ret == 0)
-		ret = wm8400_write(wm8400, reg, 1, &tmp);
-
-	mutex_unlock(&wm8400->io_lock);
-
-	return ret;
-}
-EXPORT_SYMBOL_GPL(wm8400_set_bits);
-
-/**
- * wm8400_reset_codec_reg_cache - Reset cached codec registers to
- * their default values.
- */
-void wm8400_reset_codec_reg_cache(struct wm8400 *wm8400)
-{
-	int i;
-
-	mutex_lock(&wm8400->io_lock);
-
-	/* Reset all codec registers to their initial value */
-	for (i = 0; i < ARRAY_SIZE(wm8400->reg_cache); i++)
-		if (reg_data[i].is_codec)
-			wm8400->reg_cache[i] = reg_data[i].default_val;
-
-	mutex_unlock(&wm8400->io_lock);
-}
-EXPORT_SYMBOL_GPL(wm8400_reset_codec_reg_cache);
-
 static int wm8400_register_codec(struct wm8400 *wm8400)
 {
 	struct mfd_cell cell = {
@@ -257,44 +83,24 @@ static int wm8400_register_codec(struct wm8400 *wm8400)
 static int wm8400_init(struct wm8400 *wm8400,
 		       struct wm8400_platform_data *pdata)
 {
-	u16 reg;
-	int ret, i;
-
-	mutex_init(&wm8400->io_lock);
+	unsigned int reg;
+	int ret;
 
 	dev_set_drvdata(wm8400->dev, wm8400);
 
 	/* Check that this is actually a WM8400 */
-	ret = regmap_read(wm8400->regmap, WM8400_RESET_ID, &i);
+	ret = regmap_read(wm8400->regmap, WM8400_RESET_ID, &reg);
 	if (ret != 0) {
 		dev_err(wm8400->dev, "Chip ID register read failed\n");
 		return -EIO;
 	}
-	if (i != reg_data[WM8400_RESET_ID].default_val) {
-		dev_err(wm8400->dev, "Device is not a WM8400, ID is %x\n", i);
+	if (reg != 0x6172) {
+		dev_err(wm8400->dev, "Device is not a WM8400, ID is %x\n",
+			reg);
 		return -ENODEV;
 	}
 
-	/* We don't know what state the hardware is in and since this
-	 * is a PMIC we can't reset it safely so initialise the register
-	 * cache from the hardware.
-	 */
-	ret = regmap_raw_read(wm8400->regmap, 0, wm8400->reg_cache,
-			      ARRAY_SIZE(wm8400->reg_cache));
-	if (ret != 0) {
-		dev_err(wm8400->dev, "Register cache read failed\n");
-		return -EIO;
-	}
-	for (i = 0; i < ARRAY_SIZE(wm8400->reg_cache); i++)
-		wm8400->reg_cache[i] = be16_to_cpu(wm8400->reg_cache[i]);
-
-	/* If the codec is in reset use hard coded values */
-	if (!(wm8400->reg_cache[WM8400_POWER_MANAGEMENT_1] & WM8400_CODEC_ENA))
-		for (i = 0; i < ARRAY_SIZE(wm8400->reg_cache); i++)
-			if (reg_data[i].is_codec)
-				wm8400->reg_cache[i] = reg_data[i].default_val;
-
-	ret = wm8400_read(wm8400, WM8400_ID, 1, &reg);
+	ret = regmap_read(wm8400->regmap, WM8400_ID, &reg);
 	if (ret != 0) {
 		dev_err(wm8400->dev, "ID register read failed: %d\n", ret);
 		return ret;
@@ -334,8 +140,22 @@ static const struct regmap_config wm8400_regmap_config = {
 	.reg_bits = 8,
 	.val_bits = 16,
 	.max_register = WM8400_REGISTER_COUNT - 1,
+
+	.volatile_reg = wm8400_volatile,
+
+	.cache_type = REGCACHE_RBTREE,
 };
 
+/**
+ * wm8400_reset_codec_reg_cache - Reset cached codec registers to
+ * their default values.
+ */
+void wm8400_reset_codec_reg_cache(struct wm8400 *wm8400)
+{
+	regmap_reinit_cache(wm8400->regmap, &wm8400_regmap_config);
+}
+EXPORT_SYMBOL_GPL(wm8400_reset_codec_reg_cache);
+
 #if defined(CONFIG_I2C) || defined(CONFIG_I2C_MODULE)
 static int wm8400_i2c_probe(struct i2c_client *i2c,
 			    const struct i2c_device_id *id)
diff --git a/drivers/mfd/wm8994-core.c b/drivers/mfd/wm8994-core.c
index 9d7ca1e978f..1e321d34977 100644
--- a/drivers/mfd/wm8994-core.c
+++ b/drivers/mfd/wm8994-core.c
@@ -500,7 +500,8 @@ static __devinit int wm8994_device_init(struct wm8994 *wm8994, int irq)
 			ret);
 		goto err_enable;
 	}
-	wm8994->revision = ret;
+	wm8994->revision = ret & WM8994_CHIP_REV_MASK;
+	wm8994->cust_id = (ret & WM8994_CUST_ID_MASK) >> WM8994_CUST_ID_SHIFT;
 
 	switch (wm8994->type) {
 	case WM8994:
@@ -553,8 +554,8 @@ static __devinit int wm8994_device_init(struct wm8994 *wm8994, int irq)
 		break;
 	}
 
-	dev_info(wm8994->dev, "%s revision %c\n", devname,
-		 'A' + wm8994->revision);
+	dev_info(wm8994->dev, "%s revision %c CUST_ID %02x\n", devname,
+		 'A' + wm8994->revision, wm8994->cust_id);
 
 	switch (wm8994->type) {
 	case WM1811:
@@ -732,23 +733,7 @@ static struct i2c_driver wm8994_i2c_driver = {
 	.id_table = wm8994_i2c_id,
 };
 
-static int __init wm8994_i2c_init(void)
-{
-	int ret;
-
-	ret = i2c_add_driver(&wm8994_i2c_driver);
-	if (ret != 0)
-		pr_err("Failed to register wm8994 I2C driver: %d\n", ret);
-
-	return ret;
-}
-module_init(wm8994_i2c_init);
-
-static void __exit wm8994_i2c_exit(void)
-{
-	i2c_del_driver(&wm8994_i2c_driver);
-}
-module_exit(wm8994_i2c_exit);
+module_i2c_driver(wm8994_i2c_driver);
 
 MODULE_DESCRIPTION("Core support for the WM8994 audio CODEC");
 MODULE_LICENSE("GPL");
diff --git a/drivers/mfd/wm8994-irq.c b/drivers/mfd/wm8994-irq.c
index 46b20c445ec..f1837f66975 100644
--- a/drivers/mfd/wm8994-irq.c
+++ b/drivers/mfd/wm8994-irq.c
@@ -147,12 +147,6 @@ int wm8994_irq_init(struct wm8994 *wm8994)
 		return 0;
 	}
 
-	if (!wm8994->irq_base) {
-		dev_err(wm8994->dev,
-			"No interrupt base specified, no interrupts\n");
-		return 0;
-	}
-
 	ret = regmap_add_irq_chip(wm8994->regmap, wm8994->irq,
 				  IRQF_TRIGGER_HIGH | IRQF_ONESHOT,
 				  wm8994->irq_base, &wm8994_irq_chip,
diff --git a/drivers/mfd/wm8994-regmap.c b/drivers/mfd/wm8994-regmap.c
index bfd25af6ecb..52e9e294494 100644
--- a/drivers/mfd/wm8994-regmap.c
+++ b/drivers/mfd/wm8994-regmap.c
@@ -1122,7 +1122,6 @@ static bool wm8994_volatile_register(struct device *dev, unsigned int reg)
 	case WM8994_RATE_STATUS:
 	case WM8958_MIC_DETECT_3:
 	case WM8994_DC_SERVO_4E:
-	case WM8994_CHIP_REVISION:
 	case WM8994_INTERRUPT_STATUS_1:
 	case WM8994_INTERRUPT_STATUS_2:
 		return true;
diff --git a/drivers/misc/Kconfig b/drivers/misc/Kconfig
index c7795096d43..2661f6e366f 100644
--- a/drivers/misc/Kconfig
+++ b/drivers/misc/Kconfig
@@ -452,14 +452,32 @@ config ARM_CHARLCD
 	  still useful.
 
 config BMP085
-	tristate "BMP085 digital pressure sensor"
+	bool
+	depends on SYSFS
+
+config BMP085_I2C
+	tristate "BMP085 digital pressure sensor on I2C"
+	select BMP085
+	select REGMAP_I2C
 	depends on I2C && SYSFS
 	help
-	  If you say yes here you get support for the Bosch Sensortec
-	  BMP085 digital pressure sensor.
+	  Say Y here if you want to support Bosch Sensortec's digital pressure
+	  sensor hooked to an I2C bus.
 
 	  To compile this driver as a module, choose M here: the
-	  module will be called bmp085.
+	  module will be called bmp085-i2c.
+
+config BMP085_SPI
+	tristate "BMP085 digital pressure sensor on SPI"
+	select BMP085
+	select REGMAP_SPI
+	depends on SPI_MASTER && SYSFS
+	help
+	  Say Y here if you want to support Bosch Sensortec's digital pressure
+	  sensor hooked to an SPI bus.
+
+	  To compile this driver as a module, choose M here: the
+	  module will be called bmp085-spi.
 
 config PCH_PHUB
 	tristate "Intel EG20T PCH/LAPIS Semicon IOH(ML7213/ML7223/ML7831) PHUB"
@@ -490,14 +508,6 @@ config USB_SWITCH_FSA9480
 	  stereo and mono audio, video, microphone and UART data to use
 	  a common connector port.
 
-config MAX8997_MUIC
-	tristate "MAX8997 MUIC Support"
-	depends on MFD_MAX8997
-	help
-	  If you say yes here you get support for the MUIC device of
-	  Maxim MAX8997 PMIC.
-	  The MAX8997 MUIC is a USB port accessory detector and switch.
-
 source "drivers/misc/c2port/Kconfig"
 source "drivers/misc/eeprom/Kconfig"
 source "drivers/misc/cb710/Kconfig"
@@ -506,4 +516,5 @@ source "drivers/misc/ti-st/Kconfig"
 source "drivers/misc/lis3lv02d/Kconfig"
 source "drivers/misc/carma/Kconfig"
 source "drivers/misc/altera-stapl/Kconfig"
+source "drivers/misc/mei/Kconfig"
 endmenu
diff --git a/drivers/misc/Makefile b/drivers/misc/Makefile
index 3e1d80106f0..456972faaeb 100644
--- a/drivers/misc/Makefile
+++ b/drivers/misc/Makefile
@@ -11,6 +11,8 @@ obj-$(CONFIG_ATMEL_PWM)		+= atmel_pwm.o
 obj-$(CONFIG_ATMEL_SSC)		+= atmel-ssc.o
 obj-$(CONFIG_ATMEL_TCLIB)	+= atmel_tclib.o
 obj-$(CONFIG_BMP085)		+= bmp085.o
+obj-$(CONFIG_BMP085_I2C)	+= bmp085-i2c.o
+obj-$(CONFIG_BMP085_SPI)	+= bmp085-spi.o
 obj-$(CONFIG_ICS932S401)	+= ics932s401.o
 obj-$(CONFIG_LKDTM)		+= lkdtm.o
 obj-$(CONFIG_TIFM_CORE)       	+= tifm_core.o
@@ -48,4 +50,4 @@ obj-y				+= lis3lv02d/
 obj-y				+= carma/
 obj-$(CONFIG_USB_SWITCH_FSA9480) += fsa9480.o
 obj-$(CONFIG_ALTERA_STAPL)	+=altera-stapl/
-obj-$(CONFIG_MAX8997_MUIC)	+= max8997-muic.o
+obj-$(CONFIG_INTEL_MEI)		+= mei/
diff --git a/drivers/misc/ab8500-pwm.c b/drivers/misc/ab8500-pwm.c
index d7a9aa14e5d..042a8fe4efa 100644
--- a/drivers/misc/ab8500-pwm.c
+++ b/drivers/misc/ab8500-pwm.c
@@ -142,10 +142,16 @@ static int __devexit ab8500_pwm_remove(struct platform_device *pdev)
 	return 0;
 }
 
+static const struct of_device_id ab8500_pwm_match[] = {
+	{ .compatible = "stericsson,ab8500-pwm", },
+	{}
+};
+
 static struct platform_driver ab8500_pwm_driver = {
 	.driver = {
 		.name = "ab8500-pwm",
 		.owner = THIS_MODULE,
+		.of_match_table = ab8500_pwm_match,
 	},
 	.probe = ab8500_pwm_probe,
 	.remove = __devexit_p(ab8500_pwm_remove),
diff --git a/drivers/misc/ad525x_dpot.c b/drivers/misc/ad525x_dpot.c
index 1d1d4261591..6938f1be664 100644
--- a/drivers/misc/ad525x_dpot.c
+++ b/drivers/misc/ad525x_dpot.c
@@ -749,7 +749,7 @@ exit:
 }
 EXPORT_SYMBOL(ad_dpot_probe);
 
-__devexit int ad_dpot_remove(struct device *dev)
+int ad_dpot_remove(struct device *dev)
 {
 	struct dpot_data *data = dev_get_drvdata(dev);
 	int i;
diff --git a/drivers/misc/bh1780gli.c b/drivers/misc/bh1780gli.c
index 54f6f39f990..f1f9877f3fd 100644
--- a/drivers/misc/bh1780gli.c
+++ b/drivers/misc/bh1780gli.c
@@ -248,7 +248,7 @@ static const struct i2c_device_id bh1780_id[] = {
 
 static struct i2c_driver bh1780_driver = {
 	.probe		= bh1780_probe,
-	.remove		= bh1780_remove,
+	.remove		= __devexit_p(bh1780_remove),
 	.id_table	= bh1780_id,
 	.driver = {
 		.name = "bh1780",
diff --git a/drivers/misc/bmp085-i2c.c b/drivers/misc/bmp085-i2c.c
new file mode 100644
index 00000000000..9943971c13e
--- /dev/null
+++ b/drivers/misc/bmp085-i2c.c
@@ -0,0 +1,91 @@
+/*
+ * Copyright (c) 2012  Bosch Sensortec GmbH
+ * Copyright (c) 2012  Unixphere AB
+ *
+ * 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/module.h>
+#include <linux/i2c.h>
+#include <linux/err.h>
+#include "bmp085.h"
+
+#define BMP085_I2C_ADDRESS	0x77
+
+static const unsigned short normal_i2c[] = { BMP085_I2C_ADDRESS,
+							I2C_CLIENT_END };
+
+static int bmp085_i2c_detect(struct i2c_client *client,
+			     struct i2c_board_info *info)
+{
+	if (client->addr != BMP085_I2C_ADDRESS)
+		return -ENODEV;
+
+	return bmp085_detect(&client->dev);
+}
+
+static int __devinit bmp085_i2c_probe(struct i2c_client *client,
+				      const struct i2c_device_id *id)
+{
+	int err;
+	struct regmap *regmap = devm_regmap_init_i2c(client,
+						     &bmp085_regmap_config);
+
+	if (IS_ERR(regmap)) {
+		err = PTR_ERR(regmap);
+		dev_err(&client->dev, "Failed to init regmap: %d\n", err);
+		return err;
+	}
+
+	return bmp085_probe(&client->dev, regmap);
+}
+
+static int bmp085_i2c_remove(struct i2c_client *client)
+{
+	return bmp085_remove(&client->dev);
+}
+
+static const struct of_device_id bmp085_of_match[] = {
+	{ .compatible = "bosch,bmp085", },
+	{ },
+};
+MODULE_DEVICE_TABLE(of, bmp085_of_match);
+
+static const struct i2c_device_id bmp085_id[] = {
+	{ BMP085_NAME, 0 },
+	{ "bmp180", 0 },
+	{ }
+};
+MODULE_DEVICE_TABLE(i2c, bmp085_id);
+
+static struct i2c_driver bmp085_i2c_driver = {
+	.driver = {
+		.owner	= THIS_MODULE,
+		.name	= BMP085_NAME,
+		.of_match_table = bmp085_of_match
+	},
+	.id_table	= bmp085_id,
+	.probe		= bmp085_i2c_probe,
+	.remove		= __devexit_p(bmp085_i2c_remove),
+
+	.detect		= bmp085_i2c_detect,
+	.address_list	= normal_i2c
+};
+
+module_i2c_driver(bmp085_i2c_driver);
+
+MODULE_AUTHOR("Eric Andersson <eric.andersson@unixphere.com>");
+MODULE_DESCRIPTION("BMP085 I2C bus driver");
+MODULE_LICENSE("GPL");
diff --git a/drivers/misc/bmp085-spi.c b/drivers/misc/bmp085-spi.c
new file mode 100644
index 00000000000..78aaff9b523
--- /dev/null
+++ b/drivers/misc/bmp085-spi.c
@@ -0,0 +1,91 @@
+/*
+ * Copyright (c) 2012  Bosch Sensortec GmbH
+ * Copyright (c) 2012  Unixphere AB
+ *
+ * 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/module.h>
+#include <linux/spi/spi.h>
+#include <linux/err.h>
+#include "bmp085.h"
+
+static int __devinit bmp085_spi_probe(struct spi_device *client)
+{
+	int err;
+	struct regmap *regmap;
+
+	client->bits_per_word = 8;
+	err = spi_setup(client);
+	if (err < 0) {
+		dev_err(&client->dev, "spi_setup failed!\n");
+		return err;
+	}
+
+	regmap = devm_regmap_init_spi(client, &bmp085_regmap_config);
+	if (IS_ERR(regmap)) {
+		err = PTR_ERR(regmap);
+		dev_err(&client->dev, "Failed to init regmap: %d\n", err);
+		return err;
+	}
+
+	return bmp085_probe(&client->dev, regmap);
+}
+
+static int bmp085_spi_remove(struct spi_device *client)
+{
+	return bmp085_remove(&client->dev);
+}
+
+static const struct of_device_id bmp085_of_match[] = {
+	{ .compatible = "bosch,bmp085", },
+	{ },
+};
+MODULE_DEVICE_TABLE(of, bmp085_of_match);
+
+static const struct spi_device_id bmp085_id[] = {
+	{ "bmp180", 0 },
+	{ "bmp181", 0 },
+	{ }
+};
+MODULE_DEVICE_TABLE(spi, bmp085_id);
+
+static struct spi_driver bmp085_spi_driver = {
+	.driver = {
+		.owner	= THIS_MODULE,
+		.name	= BMP085_NAME,
+		.of_match_table = bmp085_of_match
+	},
+	.id_table	= bmp085_id,
+	.probe		= bmp085_spi_probe,
+	.remove		= __devexit_p(bmp085_spi_remove)
+};
+
+static int __init bmp085_spi_init(void)
+{
+	return spi_register_driver(&bmp085_spi_driver);
+}
+
+static void __exit bmp085_spi_exit(void)
+{
+	spi_unregister_driver(&bmp085_spi_driver);
+}
+
+MODULE_AUTHOR("Eric Andersson <eric.andersson@unixphere.com>");
+MODULE_DESCRIPTION("BMP085 SPI bus driver");
+MODULE_LICENSE("GPL");
+
+module_init(bmp085_spi_init);
+module_exit(bmp085_spi_exit);
diff --git a/drivers/misc/bmp085.c b/drivers/misc/bmp085.c
index 76c3064629f..62e418293b7 100644
--- a/drivers/misc/bmp085.c
+++ b/drivers/misc/bmp085.c
@@ -1,62 +1,62 @@
 /*  Copyright (c) 2010  Christoph Mair <christoph.mair@gmail.com>
-
-    This driver supports the bmp085 digital barometric pressure
-    and temperature sensor from Bosch Sensortec. The datasheet
-    is available from their website:
-    http://www.bosch-sensortec.com/content/language1/downloads/BST-BMP085-DS000-05.pdf
-
-    A pressure measurement is issued by reading from pressure0_input.
-    The return value ranges from 30000 to 110000 pascal with a resulution
-    of 1 pascal (0.01 millibar) which enables measurements from 9000m above
-    to 500m below sea level.
-
-    The temperature can be read from temp0_input. Values range from
-    -400 to 850 representing the ambient temperature in degree celsius
-    multiplied by 10.The resolution is 0.1 celsius.
-
-    Because ambient pressure is temperature dependent, a temperature
-    measurement will be executed automatically even if the user is reading
-    from pressure0_input. This happens if the last temperature measurement
-    has been executed more then one second ago.
-
-    To decrease RMS noise from pressure measurements, the bmp085 can
-    autonomously calculate the average of up to eight samples. This is
-    set up by writing to the oversampling sysfs file. Accepted values
-    are 0, 1, 2 and 3. 2^x when x is the value written to this file
-    specifies the number of samples used to calculate the ambient pressure.
-    RMS noise is specified with six pascal (without averaging) and decreases
-    down to 3 pascal when using an oversampling setting of 3.
-
-    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.
-*/
-
+ *  Copyright (c) 2012  Bosch Sensortec GmbH
+ *  Copyright (c) 2012  Unixphere AB
+ *
+ *  This driver supports the bmp085 and bmp18x digital barometric pressure
+ *  and temperature sensors from Bosch Sensortec. The datasheets
+ *  are available from their website:
+ *  http://www.bosch-sensortec.com/content/language1/downloads/BST-BMP085-DS000-05.pdf
+ *  http://www.bosch-sensortec.com/content/language1/downloads/BST-BMP180-DS000-07.pdf
+ *
+ *  A pressure measurement is issued by reading from pressure0_input.
+ *  The return value ranges from 30000 to 110000 pascal with a resulution
+ *  of 1 pascal (0.01 millibar) which enables measurements from 9000m above
+ *  to 500m below sea level.
+ *
+ *  The temperature can be read from temp0_input. Values range from
+ *  -400 to 850 representing the ambient temperature in degree celsius
+ *  multiplied by 10.The resolution is 0.1 celsius.
+ *
+ *  Because ambient pressure is temperature dependent, a temperature
+ *  measurement will be executed automatically even if the user is reading
+ *  from pressure0_input. This happens if the last temperature measurement
+ *  has been executed more then one second ago.
+ *
+ *  To decrease RMS noise from pressure measurements, the bmp085 can
+ *  autonomously calculate the average of up to eight samples. This is
+ *  set up by writing to the oversampling sysfs file. Accepted values
+ *  are 0, 1, 2 and 3. 2^x when x is the value written to this file
+ *  specifies the number of samples used to calculate the ambient pressure.
+ *  RMS noise is specified with six pascal (without averaging) and decreases
+ *  down to 3 pascal when using an oversampling setting of 3.
+ *
+ *  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/module.h>
+#include <linux/device.h>
 #include <linux/init.h>
-#include <linux/i2c.h>
 #include <linux/slab.h>
 #include <linux/delay.h>
+#include <linux/of.h>
+#include "bmp085.h"
 
-
-#define BMP085_I2C_ADDRESS		0x77
 #define BMP085_CHIP_ID			0x55
-
 #define BMP085_CALIBRATION_DATA_START	0xAA
 #define BMP085_CALIBRATION_DATA_LENGTH	11	/* 16 bit values */
 #define BMP085_CHIP_ID_REG		0xD0
-#define BMP085_VERSION_REG		0xD1
 #define BMP085_CTRL_REG			0xF4
 #define BMP085_TEMP_MEASUREMENT		0x2E
 #define BMP085_PRESSURE_MEASUREMENT	0x34
@@ -65,12 +65,6 @@
 #define BMP085_CONVERSION_REGISTER_XLSB	0xF8
 #define BMP085_TEMP_CONVERSION_TIME	5
 
-#define BMP085_CLIENT_NAME		"bmp085"
-
-
-static const unsigned short normal_i2c[] = { BMP085_I2C_ADDRESS,
-							I2C_CLIENT_END };
-
 struct bmp085_calibration_data {
 	s16 AC1, AC2, AC3;
 	u16 AC4, AC5, AC6;
@@ -78,35 +72,30 @@ struct bmp085_calibration_data {
 	s16 MB, MC, MD;
 };
 
-
-/* Each client has this additional data */
 struct bmp085_data {
-	struct i2c_client *client;
-	struct mutex lock;
-	struct bmp085_calibration_data calibration;
-	u32 raw_temperature;
-	u32 raw_pressure;
-	unsigned char oversampling_setting;
+	struct	device *dev;
+	struct  regmap *regmap;
+	struct	mutex lock;
+	struct	bmp085_calibration_data calibration;
+	u8	oversampling_setting;
+	u32	raw_temperature;
+	u32	raw_pressure;
+	u32	temp_measurement_period;
 	unsigned long last_temp_measurement;
-	s32 b6; /* calculated temperature correction coefficient */
+	u8	chip_id;
+	s32	b6; /* calculated temperature correction coefficient */
 };
 
-
-static s32 bmp085_read_calibration_data(struct i2c_client *client)
+static s32 bmp085_read_calibration_data(struct bmp085_data *data)
 {
 	u16 tmp[BMP085_CALIBRATION_DATA_LENGTH];
-	struct bmp085_data *data = i2c_get_clientdata(client);
 	struct bmp085_calibration_data *cali = &(data->calibration);
-	s32 status = i2c_smbus_read_i2c_block_data(client,
-				BMP085_CALIBRATION_DATA_START,
-				BMP085_CALIBRATION_DATA_LENGTH*sizeof(u16),
-				(u8 *)tmp);
+	s32 status = regmap_bulk_read(data->regmap,
+				BMP085_CALIBRATION_DATA_START, (u8 *)tmp,
+				(BMP085_CALIBRATION_DATA_LENGTH << 1));
 	if (status < 0)
 		return status;
 
-	if (status != BMP085_CALIBRATION_DATA_LENGTH*sizeof(u16))
-		return -EIO;
-
 	cali->AC1 =  be16_to_cpu(tmp[0]);
 	cali->AC2 =  be16_to_cpu(tmp[1]);
 	cali->AC3 =  be16_to_cpu(tmp[2]);
@@ -121,30 +110,26 @@ static s32 bmp085_read_calibration_data(struct i2c_client *client)
 	return 0;
 }
 
-
 static s32 bmp085_update_raw_temperature(struct bmp085_data *data)
 {
 	u16 tmp;
 	s32 status;
 
 	mutex_lock(&data->lock);
-	status = i2c_smbus_write_byte_data(data->client, BMP085_CTRL_REG,
-						BMP085_TEMP_MEASUREMENT);
-	if (status != 0) {
-		dev_err(&data->client->dev,
+	status = regmap_write(data->regmap, BMP085_CTRL_REG,
+			      BMP085_TEMP_MEASUREMENT);
+	if (status < 0) {
+		dev_err(data->dev,
 			"Error while requesting temperature measurement.\n");
 		goto exit;
 	}
 	msleep(BMP085_TEMP_CONVERSION_TIME);
 
-	status = i2c_smbus_read_i2c_block_data(data->client,
-		BMP085_CONVERSION_REGISTER_MSB, sizeof(tmp), (u8 *)&tmp);
-	if (status < 0)
-		goto exit;
-	if (status != sizeof(tmp)) {
-		dev_err(&data->client->dev,
+	status = regmap_bulk_read(data->regmap, BMP085_CONVERSION_REGISTER_MSB,
+				 &tmp, sizeof(tmp));
+	if (status < 0) {
+		dev_err(data->dev,
 			"Error while reading temperature measurement result\n");
-		status = -EIO;
 		goto exit;
 	}
 	data->raw_temperature = be16_to_cpu(tmp);
@@ -162,10 +147,11 @@ static s32 bmp085_update_raw_pressure(struct bmp085_data *data)
 	s32 status;
 
 	mutex_lock(&data->lock);
-	status = i2c_smbus_write_byte_data(data->client, BMP085_CTRL_REG,
-		BMP085_PRESSURE_MEASUREMENT + (data->oversampling_setting<<6));
-	if (status != 0) {
-		dev_err(&data->client->dev,
+	status = regmap_write(data->regmap, BMP085_CTRL_REG,
+			BMP085_PRESSURE_MEASUREMENT +
+			(data->oversampling_setting << 6));
+	if (status < 0) {
+		dev_err(data->dev,
 			"Error while requesting pressure measurement.\n");
 		goto exit;
 	}
@@ -174,14 +160,11 @@ static s32 bmp085_update_raw_pressure(struct bmp085_data *data)
 	msleep(2+(3 << data->oversampling_setting));
 
 	/* copy data into a u32 (4 bytes), but skip the first byte. */
-	status = i2c_smbus_read_i2c_block_data(data->client,
-			BMP085_CONVERSION_REGISTER_MSB, 3, ((u8 *)&tmp)+1);
-	if (status < 0)
-		goto exit;
-	if (status != 3) {
-		dev_err(&data->client->dev,
+	status = regmap_bulk_read(data->regmap, BMP085_CONVERSION_REGISTER_MSB,
+				 ((u8 *)&tmp)+1, 3);
+	if (status < 0) {
+		dev_err(data->dev,
 			"Error while reading pressure measurement results\n");
-		status = -EIO;
 		goto exit;
 	}
 	data->raw_pressure = be32_to_cpu((tmp));
@@ -193,7 +176,6 @@ exit:
 	return status;
 }
 
-
 /*
  * This function starts the temperature measurement and returns the value
  * in tenth of a degree celsius.
@@ -205,7 +187,7 @@ static s32 bmp085_get_temperature(struct bmp085_data *data, int *temperature)
 	int status;
 
 	status = bmp085_update_raw_temperature(data);
-	if (status != 0)
+	if (status < 0)
 		goto exit;
 
 	x1 = ((data->raw_temperature - cali->AC6) * cali->AC5) >> 15;
@@ -222,8 +204,10 @@ exit:
 /*
  * This function starts the pressure measurement and returns the value
  * in millibar. Since the pressure depends on the ambient temperature,
- * a temperature measurement is executed if the last known value is older
- * than one second.
+ * a temperature measurement is executed according to the given temperature
+ * measurement period (default is 1 sec boundary). This period could vary
+ * and needs to be adjusted according to the sensor environment, i.e. if big
+ * temperature variations then the temperature needs to be read out often.
  */
 static s32 bmp085_get_pressure(struct bmp085_data *data, int *pressure)
 {
@@ -234,16 +218,16 @@ static s32 bmp085_get_pressure(struct bmp085_data *data, int *pressure)
 	int status;
 
 	/* alt least every second force an update of the ambient temperature */
-	if (data->last_temp_measurement == 0 ||
-			time_is_before_jiffies(data->last_temp_measurement + 1*HZ)) {
+	if ((data->last_temp_measurement == 0) ||
+	    time_is_before_jiffies(data->last_temp_measurement + 1*HZ)) {
 		status = bmp085_get_temperature(data, NULL);
-		if (status != 0)
-			goto exit;
+		if (status < 0)
+			return status;
 	}
 
 	status = bmp085_update_raw_pressure(data);
-	if (status != 0)
-		goto exit;
+	if (status < 0)
+		return status;
 
 	x1 = (data->b6 * data->b6) >> 12;
 	x1 *= cali->B2;
@@ -274,15 +258,14 @@ static s32 bmp085_get_pressure(struct bmp085_data *data, int *pressure)
 
 	*pressure = p;
 
-exit:
-	return status;
+	return 0;
 }
 
 /*
  * This function sets the chip-internal oversampling. Valid values are 0..3.
  * The chip will use 2^oversampling samples for internal averaging.
  * This influences the measurement time and the accuracy; larger values
- * increase both. The datasheet gives on overview on how measurement time,
+ * increase both. The datasheet gives an overview on how measurement time,
  * accuracy and noise correlate.
  */
 static void bmp085_set_oversampling(struct bmp085_data *data,
@@ -306,22 +289,25 @@ static ssize_t set_oversampling(struct device *dev,
 				struct device_attribute *attr,
 				const char *buf, size_t count)
 {
-	struct i2c_client *client = to_i2c_client(dev);
-	struct bmp085_data *data = i2c_get_clientdata(client);
+	struct bmp085_data *data = dev_get_drvdata(dev);
 	unsigned long oversampling;
-	int success = strict_strtoul(buf, 10, &oversampling);
-	if (success == 0) {
+	int err = kstrtoul(buf, 10, &oversampling);
+
+	if (err == 0) {
+		mutex_lock(&data->lock);
 		bmp085_set_oversampling(data, oversampling);
+		mutex_unlock(&data->lock);
 		return count;
 	}
-	return success;
+
+	return err;
 }
 
 static ssize_t show_oversampling(struct device *dev,
 				 struct device_attribute *attr, char *buf)
 {
-	struct i2c_client *client = to_i2c_client(dev);
-	struct bmp085_data *data = i2c_get_clientdata(client);
+	struct bmp085_data *data = dev_get_drvdata(dev);
+
 	return sprintf(buf, "%u\n", bmp085_get_oversampling(data));
 }
 static DEVICE_ATTR(oversampling, S_IWUSR | S_IRUGO,
@@ -333,11 +319,10 @@ static ssize_t show_temperature(struct device *dev,
 {
 	int temperature;
 	int status;
-	struct i2c_client *client = to_i2c_client(dev);
-	struct bmp085_data *data = i2c_get_clientdata(client);
+	struct bmp085_data *data = dev_get_drvdata(dev);
 
 	status = bmp085_get_temperature(data, &temperature);
-	if (status != 0)
+	if (status < 0)
 		return status;
 	else
 		return sprintf(buf, "%d\n", temperature);
@@ -350,11 +335,10 @@ static ssize_t show_pressure(struct device *dev,
 {
 	int pressure;
 	int status;
-	struct i2c_client *client = to_i2c_client(dev);
-	struct bmp085_data *data = i2c_get_clientdata(client);
+	struct bmp085_data *data = dev_get_drvdata(dev);
 
 	status = bmp085_get_pressure(data, &pressure);
-	if (status != 0)
+	if (status < 0)
 		return status;
 	else
 		return sprintf(buf, "%d\n", pressure);
@@ -373,38 +357,70 @@ static const struct attribute_group bmp085_attr_group = {
 	.attrs = bmp085_attributes,
 };
 
-static int bmp085_detect(struct i2c_client *client, struct i2c_board_info *info)
+int bmp085_detect(struct device *dev)
 {
-	if (client->addr != BMP085_I2C_ADDRESS)
-		return -ENODEV;
+	struct bmp085_data *data = dev_get_drvdata(dev);
+	unsigned int id;
+	int ret;
 
-	if (i2c_smbus_read_byte_data(client, BMP085_CHIP_ID_REG) != BMP085_CHIP_ID)
+	ret = regmap_read(data->regmap, BMP085_CHIP_ID_REG, &id);
+	if (ret < 0)
+		return ret;
+
+	if (id != data->chip_id)
 		return -ENODEV;
 
 	return 0;
 }
+EXPORT_SYMBOL_GPL(bmp085_detect);
 
-static int bmp085_init_client(struct i2c_client *client)
+static void __init bmp085_get_of_properties(struct bmp085_data *data)
 {
-	unsigned char version;
-	int status;
-	struct bmp085_data *data = i2c_get_clientdata(client);
-	data->client = client;
-	status = bmp085_read_calibration_data(client);
-	if (status != 0)
-		goto exit;
-	version = i2c_smbus_read_byte_data(client, BMP085_VERSION_REG);
+#ifdef CONFIG_OF
+	struct device_node *np = data->dev->of_node;
+	u32 prop;
+
+	if (!np)
+		return;
+
+	if (!of_property_read_u32(np, "chip-id", &prop))
+		data->chip_id = prop & 0xff;
+
+	if (!of_property_read_u32(np, "temp-measurement-period", &prop))
+		data->temp_measurement_period = (prop/100)*HZ;
+
+	if (!of_property_read_u32(np, "default-oversampling", &prop))
+		data->oversampling_setting = prop & 0xff;
+#endif
+}
+
+static int bmp085_init_client(struct bmp085_data *data)
+{
+	int status = bmp085_read_calibration_data(data);
+
+	if (status < 0)
+		return status;
+
+	/* default settings */
+	data->chip_id = BMP085_CHIP_ID;
 	data->last_temp_measurement = 0;
+	data->temp_measurement_period = 1*HZ;
 	data->oversampling_setting = 3;
+
+	bmp085_get_of_properties(data);
+
 	mutex_init(&data->lock);
-	dev_info(&data->client->dev, "BMP085 ver. %d.%d found.\n",
-			(version & 0x0F), (version & 0xF0) >> 4);
-exit:
-	return status;
+
+	return 0;
 }
 
-static int __devinit bmp085_probe(struct i2c_client *client,
-			 const struct i2c_device_id *id)
+struct regmap_config bmp085_regmap_config = {
+	.reg_bits = 8,
+	.val_bits = 8
+};
+EXPORT_SYMBOL_GPL(bmp085_regmap_config);
+
+__devinit int bmp085_probe(struct device *dev, struct regmap *regmap)
 {
 	struct bmp085_data *data;
 	int err = 0;
@@ -415,58 +431,48 @@ static int __devinit bmp085_probe(struct i2c_client *client,
 		goto exit;
 	}
 
-	/* default settings after POR */
-	data->oversampling_setting = 0x00;
-
-	i2c_set_clientdata(client, data);
+	dev_set_drvdata(dev, data);
+	data->dev = dev;
+	data->regmap = regmap;
 
 	/* Initialize the BMP085 chip */
-	err = bmp085_init_client(client);
-	if (err != 0)
+	err = bmp085_init_client(data);
+	if (err < 0)
 		goto exit_free;
 
+	err = bmp085_detect(dev);
+	if (err < 0) {
+		dev_err(dev, "%s: chip_id failed!\n", BMP085_NAME);
+		goto exit_free;
+	}
+
 	/* Register sysfs hooks */
-	err = sysfs_create_group(&client->dev.kobj, &bmp085_attr_group);
+	err = sysfs_create_group(&dev->kobj, &bmp085_attr_group);
 	if (err)
 		goto exit_free;
 
-	dev_info(&data->client->dev, "Successfully initialized bmp085!\n");
-	goto exit;
+	dev_info(dev, "Successfully initialized %s!\n", BMP085_NAME);
+
+	return 0;
 
 exit_free:
 	kfree(data);
 exit:
 	return err;
 }
+EXPORT_SYMBOL_GPL(bmp085_probe);
 
-static int __devexit bmp085_remove(struct i2c_client *client)
+int bmp085_remove(struct device *dev)
 {
-	sysfs_remove_group(&client->dev.kobj, &bmp085_attr_group);
-	kfree(i2c_get_clientdata(client));
-	return 0;
-}
+	struct bmp085_data *data = dev_get_drvdata(dev);
 
-static const struct i2c_device_id bmp085_id[] = {
-	{ "bmp085", 0 },
-	{ }
-};
-MODULE_DEVICE_TABLE(i2c, bmp085_id);
-
-static struct i2c_driver bmp085_driver = {
-	.driver = {
-		.owner = THIS_MODULE,
-		.name	= "bmp085"
-	},
-	.id_table	= bmp085_id,
-	.probe		= bmp085_probe,
-	.remove		= __devexit_p(bmp085_remove),
-
-	.detect		= bmp085_detect,
-	.address_list	= normal_i2c
-};
+	sysfs_remove_group(&data->dev->kobj, &bmp085_attr_group);
+	kfree(data);
 
-module_i2c_driver(bmp085_driver);
+	return 0;
+}
+EXPORT_SYMBOL_GPL(bmp085_remove);
 
-MODULE_AUTHOR("Christoph Mair <christoph.mair@gmail.com");
+MODULE_AUTHOR("Christoph Mair <christoph.mair@gmail.com>");
 MODULE_DESCRIPTION("BMP085 driver");
 MODULE_LICENSE("GPL");
diff --git a/drivers/misc/bmp085.h b/drivers/misc/bmp085.h
new file mode 100644
index 00000000000..2b8f615bca9
--- /dev/null
+++ b/drivers/misc/bmp085.h
@@ -0,0 +1,33 @@
+/*
+ * Copyright (c) 2012  Bosch Sensortec GmbH
+ * Copyright (c) 2012  Unixphere AB
+ *
+ * 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 _BMP085_H
+#define _BMP085_H
+
+#include <linux/regmap.h>
+
+#define BMP085_NAME		"bmp085"
+
+extern struct regmap_config bmp085_regmap_config;
+
+int bmp085_probe(struct device *dev, struct regmap *regmap);
+int bmp085_remove(struct device *dev);
+int bmp085_detect(struct device *dev);
+
+#endif
diff --git a/drivers/misc/c2port/Kconfig b/drivers/misc/c2port/Kconfig
index e46af9a5810..33ee834e1b8 100644
--- a/drivers/misc/c2port/Kconfig
+++ b/drivers/misc/c2port/Kconfig
@@ -5,7 +5,7 @@
 menuconfig C2PORT
 	tristate "Silicon Labs C2 port support (EXPERIMENTAL)"
 	depends on EXPERIMENTAL
-	default no
+	default n
 	help
 	  This option enables support for Silicon Labs C2 port used to
 	  program Silicon micro controller chips (and other 8051 compatible).
@@ -23,8 +23,8 @@ if C2PORT
 
 config C2PORT_DURAMAR_2150
 	tristate "C2 port support for Eurotech's Duramar 2150 (EXPERIMENTAL)"
-	depends on X86 && C2PORT
-	default no
+	depends on X86
+	default n
 	help
 	  This option enables C2 support for the Eurotech's Duramar 2150
 	  on board micro controller.
diff --git a/drivers/misc/eeprom/at25.c b/drivers/misc/eeprom/at25.c
index 01ab3c9b4cf..0842c2994ee 100644
--- a/drivers/misc/eeprom/at25.c
+++ b/drivers/misc/eeprom/at25.c
@@ -50,6 +50,7 @@ struct at25_data {
 #define	AT25_SR_BP1	0x08
 #define	AT25_SR_WPEN	0x80		/* writeprotect enable */
 
+#define	AT25_INSTR_BIT3	0x08		/* Additional address bit in instr */
 
 #define EE_MAXADDRLEN	3		/* 24 bit addresses, up to 2 MBytes */
 
@@ -75,6 +76,7 @@ at25_ee_read(
 	ssize_t			status;
 	struct spi_transfer	t[2];
 	struct spi_message	m;
+	u8			instr;
 
 	if (unlikely(offset >= at25->bin.size))
 		return 0;
@@ -84,7 +86,12 @@ at25_ee_read(
 		return count;
 
 	cp = command;
-	*cp++ = AT25_READ;
+
+	instr = AT25_READ;
+	if (at25->chip.flags & EE_INSTR_BIT3_IS_ADDR)
+		if (offset >= (1U << (at25->addrlen * 8)))
+			instr |= AT25_INSTR_BIT3;
+	*cp++ = instr;
 
 	/* 8/16/24-bit address is written MSB first */
 	switch (at25->addrlen) {
@@ -167,14 +174,14 @@ at25_ee_write(struct at25_data *at25, const char *buf, loff_t off,
 	/* For write, rollover is within the page ... so we write at
 	 * most one page, then manually roll over to the next page.
 	 */
-	bounce[0] = AT25_WRITE;
 	mutex_lock(&at25->lock);
 	do {
 		unsigned long	timeout, retries;
 		unsigned	segment;
 		unsigned	offset = (unsigned) off;
-		u8		*cp = bounce + 1;
+		u8		*cp = bounce;
 		int		sr;
+		u8		instr;
 
 		*cp = AT25_WREN;
 		status = spi_write(at25->spi, cp, 1);
@@ -184,6 +191,12 @@ at25_ee_write(struct at25_data *at25, const char *buf, loff_t off,
 			break;
 		}
 
+		instr = AT25_WRITE;
+		if (at25->chip.flags & EE_INSTR_BIT3_IS_ADDR)
+			if (offset >= (1U << (at25->addrlen * 8)))
+				instr |= AT25_INSTR_BIT3;
+		*cp++ = instr;
+
 		/* 8/16/24-bit address is written MSB first */
 		switch (at25->addrlen) {
 		default:	/* case 3 */
diff --git a/drivers/staging/mei/Kconfig b/drivers/misc/mei/Kconfig
index 47d78a72db2..47d78a72db2 100644
--- a/drivers/staging/mei/Kconfig
+++ b/drivers/misc/mei/Kconfig
diff --git a/drivers/staging/mei/Makefile b/drivers/misc/mei/Makefile
index 57168db6c7e..57168db6c7e 100644
--- a/drivers/staging/mei/Makefile
+++ b/drivers/misc/mei/Makefile
diff --git a/drivers/staging/mei/hw.h b/drivers/misc/mei/hw.h
index 24c4c962819..24c4c962819 100644
--- a/drivers/staging/mei/hw.h
+++ b/drivers/misc/mei/hw.h
diff --git a/drivers/staging/mei/init.c b/drivers/misc/mei/init.c
index eab711fb5fc..a7d0bb0880e 100644
--- a/drivers/staging/mei/init.c
+++ b/drivers/misc/mei/init.c
@@ -22,7 +22,7 @@
 #include "mei_dev.h"
 #include "hw.h"
 #include "interface.h"
-#include "mei.h"
+#include <linux/mei.h>
 
 const uuid_le mei_amthi_guid  = UUID_LE(0x12f80028, 0xb4b7, 0x4b2d, 0xac,
 						0xa8, 0x46, 0xe0, 0xff, 0x65,
@@ -200,7 +200,7 @@ int mei_hw_init(struct mei_device *dev)
 		if (!(dev->me_hw_state & ME_RDY_HRA))
 			dev_dbg(&dev->pdev->dev, "ME turn off ME_RDY.\n");
 
-		printk(KERN_ERR "mei: link layer initialization failed.\n");
+		dev_err(&dev->pdev->dev, "link layer initialization failed.\n");
 		ret = -ENODEV;
 		goto out;
 	}
diff --git a/drivers/staging/mei/interface.c b/drivers/misc/mei/interface.c
index 9a2cfafc52a..428d21e3641 100644
--- a/drivers/staging/mei/interface.c
+++ b/drivers/misc/mei/interface.c
@@ -16,7 +16,7 @@
 
 #include <linux/pci.h>
 #include "mei_dev.h"
-#include "mei.h"
+#include <linux/mei.h>
 #include "interface.h"
 
 
diff --git a/drivers/staging/mei/interface.h b/drivers/misc/mei/interface.h
index fb90c6f8a75..ddff5d16616 100644
--- a/drivers/staging/mei/interface.h
+++ b/drivers/misc/mei/interface.h
@@ -19,7 +19,7 @@
 #ifndef _MEI_INTERFACE_H_
 #define _MEI_INTERFACE_H_
 
-#include "mei.h"
+#include <linux/mei.h>
 #include "mei_dev.h"
 
 
@@ -51,8 +51,7 @@ int mei_flow_ctrl_creds(struct mei_device *dev, struct mei_cl *cl);
 
 int mei_wd_send(struct mei_device *dev);
 int mei_wd_stop(struct mei_device *dev, bool preserve);
-bool mei_wd_host_init(struct mei_device *dev);
-void mei_wd_set_start_timeout(struct mei_device *dev, u16 timeout);
+int mei_wd_host_init(struct mei_device *dev);
 /*
  * mei_watchdog_register  - Registering watchdog interface
  *   once we got connection to the WD Client
diff --git a/drivers/staging/mei/interrupt.c b/drivers/misc/mei/interrupt.c
index 2007d2447b1..93936f1b75e 100644
--- a/drivers/staging/mei/interrupt.c
+++ b/drivers/misc/mei/interrupt.c
@@ -22,7 +22,7 @@
 #include <linux/jiffies.h>
 
 #include "mei_dev.h"
-#include "mei.h"
+#include <linux/mei.h>
 #include "hw.h"
 #include "interface.h"
 
diff --git a/drivers/staging/mei/iorw.c b/drivers/misc/mei/iorw.c
index 0a80dc4e62f..f9cced69b65 100644
--- a/drivers/staging/mei/iorw.c
+++ b/drivers/misc/mei/iorw.c
@@ -35,7 +35,7 @@
 
 #include "mei_dev.h"
 #include "hw.h"
-#include "mei.h"
+#include <linux/mei.h>
 #include "interface.h"
 
 
diff --git a/drivers/staging/mei/main.c b/drivers/misc/mei/main.c
index 7c9321fa7bb..c7033322833 100644
--- a/drivers/staging/mei/main.c
+++ b/drivers/misc/mei/main.c
@@ -14,6 +14,8 @@
  *
  */
 
+#define pr_fmt(fmt) KBUILD_MODNAME ": " fmt
+
 #include <linux/module.h>
 #include <linux/moduleparam.h>
 #include <linux/kernel.h>
@@ -36,19 +38,10 @@
 #include <linux/miscdevice.h>
 
 #include "mei_dev.h"
-#include "mei.h"
+#include <linux/mei.h>
 #include "interface.h"
 
-
-#define MEI_READ_TIMEOUT 45
-#define MEI_DRIVER_NAME	"mei"
-#define MEI_DEV_NAME "mei"
-
-/*
- *  mei driver strings
- */
-static char mei_driver_name[] = MEI_DRIVER_NAME;
-static const char mei_driver_string[] = "Intel(R) Management Engine Interface";
+static const char mei_driver_name[] = "mei";
 
 /* The device pointer */
 /* Currently this driver works as long as there is only a single AMT device. */
@@ -931,7 +924,7 @@ static const struct file_operations mei_fops = {
  * Misc Device Struct
  */
 static struct miscdevice  mei_misc_device = {
-		.name = MEI_DRIVER_NAME,
+		.name = "mei",
 		.fops = &mei_fops,
 		.minor = MISC_DYNAMIC_MINOR,
 };
@@ -958,7 +951,7 @@ static int __devinit mei_probe(struct pci_dev *pdev,
 	/* enable pci dev */
 	err = pci_enable_device(pdev);
 	if (err) {
-		printk(KERN_ERR "mei: Failed to enable pci device.\n");
+		dev_err(&pdev->dev, "failed to enable pci device.\n");
 		goto end;
 	}
 	/* set PCI host mastering  */
@@ -966,7 +959,7 @@ static int __devinit mei_probe(struct pci_dev *pdev,
 	/* pci request regions for mei driver */
 	err = pci_request_regions(pdev, mei_driver_name);
 	if (err) {
-		printk(KERN_ERR "mei: Failed to get pci regions.\n");
+		dev_err(&pdev->dev, "failed to get pci regions.\n");
 		goto disable_device;
 	}
 	/* allocates and initializes the mei dev structure */
@@ -978,7 +971,7 @@ static int __devinit mei_probe(struct pci_dev *pdev,
 	/* mapping  IO device memory */
 	dev->mem_addr = pci_iomap(pdev, 0, 0);
 	if (!dev->mem_addr) {
-		printk(KERN_ERR "mei: mapping I/O device memory failure.\n");
+		dev_err(&pdev->dev, "mapping I/O device memory failure.\n");
 		err = -ENOMEM;
 		goto free_device;
 	}
@@ -997,13 +990,13 @@ static int __devinit mei_probe(struct pci_dev *pdev,
 			IRQF_SHARED, mei_driver_name, dev);
 
 	if (err) {
-		printk(KERN_ERR "mei: request_threaded_irq failure. irq = %d\n",
+		dev_err(&pdev->dev, "request_threaded_irq failure. irq = %d\n",
 		       pdev->irq);
 		goto unmap_memory;
 	}
 	INIT_DELAYED_WORK(&dev->timer_work, mei_timer);
 	if (mei_hw_init(dev)) {
-		printk(KERN_ERR "mei: Init hw failure.\n");
+		dev_err(&pdev->dev, "init hw failure.\n");
 		err = -ENODEV;
 		goto release_irq;
 	}
@@ -1020,7 +1013,7 @@ static int __devinit mei_probe(struct pci_dev *pdev,
 
 	mutex_unlock(&mei_mutex);
 
-	pr_debug("mei: Driver initialization successful.\n");
+	pr_debug("initialization successful.\n");
 
 	return 0;
 
@@ -1041,7 +1034,7 @@ disable_device:
 	pci_disable_device(pdev);
 end:
 	mutex_unlock(&mei_mutex);
-	printk(KERN_ERR "mei: Driver initialization failed.\n");
+	dev_err(&pdev->dev, "initialization failed.\n");
 	return err;
 }
 
@@ -1160,8 +1153,8 @@ static int mei_pci_resume(struct device *device)
 			IRQF_SHARED, mei_driver_name, dev);
 
 	if (err) {
-		printk(KERN_ERR "mei: Request_irq failure. irq = %d\n",
-		       pdev->irq);
+		dev_err(&pdev->dev, "request_threaded_irq failed: irq = %d.\n",
+				pdev->irq);
 		return err;
 	}
 
@@ -1204,11 +1197,11 @@ static int __init mei_init_module(void)
 {
 	int ret;
 
-	pr_debug("mei: %s\n", mei_driver_string);
+	pr_debug("loading.\n");
 	/* init pci module */
 	ret = pci_register_driver(&mei_driver);
 	if (ret < 0)
-		printk(KERN_ERR "mei: Error registering driver.\n");
+		pr_err("error registering driver.\n");
 
 	return ret;
 }
@@ -1226,7 +1219,7 @@ static void __exit mei_exit_module(void)
 	misc_deregister(&mei_misc_device);
 	pci_unregister_driver(&mei_driver);
 
-	pr_debug("mei: Driver unloaded successfully.\n");
+	pr_debug("unloaded successfully.\n");
 }
 
 module_exit(mei_exit_module);
diff --git a/drivers/staging/mei/mei_dev.h b/drivers/misc/mei/mei_dev.h
index 10b1b4e2f8a..63d7ee97c5f 100644
--- a/drivers/staging/mei/mei_dev.h
+++ b/drivers/misc/mei/mei_dev.h
@@ -19,7 +19,7 @@
 
 #include <linux/types.h>
 #include <linux/watchdog.h>
-#include "mei.h"
+#include <linux/mei.h>
 #include "hw.h"
 
 /*
diff --git a/drivers/staging/mei/wd.c b/drivers/misc/mei/wd.c
index cf4c29d10e7..6be5605707b 100644
--- a/drivers/staging/mei/wd.c
+++ b/drivers/misc/mei/wd.c
@@ -24,7 +24,7 @@
 #include "mei_dev.h"
 #include "hw.h"
 #include "interface.h"
-#include "mei.h"
+#include <linux/mei.h>
 
 static const u8 mei_start_wd_params[] = { 0x02, 0x12, 0x13, 0x10 };
 static const u8 mei_stop_wd_params[] = { 0x02, 0x02, 0x14, 0x10 };
@@ -45,23 +45,22 @@ const uuid_le mei_wd_guid = UUID_LE(0x05B79A6F, 0x4628, 0x4D7F, 0x89,
 						0x9D, 0xA9, 0x15, 0x14, 0xCB,
 						0x32, 0xAB);
 
-void mei_wd_set_start_timeout(struct mei_device *dev, u16 timeout)
+static void mei_wd_set_start_timeout(struct mei_device *dev, u16 timeout)
 {
-	dev_dbg(&dev->pdev->dev, "timeout=%d.\n", timeout);
+	dev_dbg(&dev->pdev->dev, "wd: set timeout=%d.\n", timeout);
 	memcpy(dev->wd_data, mei_start_wd_params, MEI_WD_PARAMS_SIZE);
-	memcpy(dev->wd_data + MEI_WD_PARAMS_SIZE,
-			&timeout, sizeof(u16));
+	memcpy(dev->wd_data + MEI_WD_PARAMS_SIZE, &timeout, sizeof(u16));
 }
 
 /**
  * host_init_wd - mei initialization wd.
  *
  * @dev: the device structure
+ * returns -ENENT if wd client cannot be found
+ *         -EIO if write has failed
  */
-bool mei_wd_host_init(struct mei_device *dev)
+int mei_wd_host_init(struct mei_device *dev)
 {
-	bool ret = false;
-
 	mei_cl_init(&dev->wd_cl, dev);
 
 	/* look for WD client and connect to it */
@@ -72,25 +71,21 @@ bool mei_wd_host_init(struct mei_device *dev)
 	mei_find_me_client_update_filext(dev, &dev->wd_cl,
 				&mei_wd_guid, MEI_WD_HOST_CLIENT_ID);
 
-	dev_dbg(&dev->pdev->dev, "check wd_cl\n");
-	if (MEI_FILE_CONNECTING == dev->wd_cl.state) {
-		if (mei_connect(dev, &dev->wd_cl)) {
-			dev_dbg(&dev->pdev->dev, "Failed to connect to WD client\n");
-			dev->wd_cl.state = MEI_FILE_DISCONNECTED;
-			dev->wd_cl.host_client_id = 0;
-			ret = false;
-			goto end;
-		} else {
-			dev->wd_cl.timer_count = CONNECT_TIMEOUT;
-		}
-	} else {
-		dev_dbg(&dev->pdev->dev, "Failed to find WD client\n");
-		ret = false;
-		goto end;
+	dev_dbg(&dev->pdev->dev, "wd: check client\n");
+	if (MEI_FILE_CONNECTING != dev->wd_cl.state) {
+		dev_info(&dev->pdev->dev, "wd: failed to find the client\n");
+		return -ENOENT;
 	}
 
-end:
-	return ret;
+	if (mei_connect(dev, &dev->wd_cl)) {
+		dev_err(&dev->pdev->dev, "wd: failed to connect to the client\n");
+		dev->wd_cl.state = MEI_FILE_DISCONNECTED;
+		dev->wd_cl.host_client_id = 0;
+		return -EIO;
+	}
+	dev->wd_cl.timer_count = CONNECT_TIMEOUT;
+
+	return 0;
 }
 
 /**
@@ -159,7 +154,7 @@ int mei_wd_stop(struct mei_device *dev, bool preserve)
 			if (ret)
 				goto out;
 		} else {
-			dev_dbg(&dev->pdev->dev, "send stop WD failed\n");
+			dev_err(&dev->pdev->dev, "wd: send stop failed\n");
 		}
 
 		dev->wd_pending = false;
@@ -173,13 +168,13 @@ int mei_wd_stop(struct mei_device *dev, bool preserve)
 					dev->wd_stopped, 10 * HZ);
 	mutex_lock(&dev->device_lock);
 	if (dev->wd_stopped) {
-		dev_dbg(&dev->pdev->dev, "stop wd complete ret=%d.\n", ret);
+		dev_dbg(&dev->pdev->dev, "wd: stop completed ret=%d.\n", ret);
 		ret = 0;
 	} else {
 		if (!ret)
 			ret = -ETIMEDOUT;
 		dev_warn(&dev->pdev->dev,
-			"stop wd failed to complete ret=%d.\n", ret);
+			"wd: stop failed to complete ret=%d.\n", ret);
 	}
 
 	if (preserve)
@@ -208,13 +203,15 @@ static int mei_wd_ops_start(struct watchdog_device *wd_dev)
 	mutex_lock(&dev->device_lock);
 
 	if (dev->mei_state != MEI_ENABLED) {
-		dev_dbg(&dev->pdev->dev, "mei_state != MEI_ENABLED  mei_state= %d\n",
-		    dev->mei_state);
+		dev_dbg(&dev->pdev->dev,
+			"wd: mei_state != MEI_ENABLED  mei_state = %d\n",
+			dev->mei_state);
 		goto end_unlock;
 	}
 
 	if (dev->wd_cl.state != MEI_FILE_CONNECTED)	{
-		dev_dbg(&dev->pdev->dev, "MEI Driver is not connected to Watchdog Client\n");
+		dev_dbg(&dev->pdev->dev,
+			"MEI Driver is not connected to Watchdog Client\n");
 		goto end_unlock;
 	}
 
@@ -267,7 +264,7 @@ static int mei_wd_ops_ping(struct watchdog_device *wd_dev)
 	mutex_lock(&dev->device_lock);
 
 	if (dev->wd_cl.state != MEI_FILE_CONNECTED) {
-		dev_dbg(&dev->pdev->dev, "wd is not connected.\n");
+		dev_err(&dev->pdev->dev, "wd: not connected.\n");
 		ret = -ENODEV;
 		goto end;
 	}
@@ -277,16 +274,17 @@ static int mei_wd_ops_ping(struct watchdog_device *wd_dev)
 		mei_flow_ctrl_creds(dev, &dev->wd_cl) > 0) {
 
 		dev->mei_host_buffer_is_empty = false;
-		dev_dbg(&dev->pdev->dev, "sending watchdog ping\n");
+		dev_dbg(&dev->pdev->dev, "wd: sending ping\n");
 
 		if (mei_wd_send(dev)) {
-			dev_dbg(&dev->pdev->dev, "wd send failed.\n");
+			dev_err(&dev->pdev->dev, "wd: send failed.\n");
 			ret = -EIO;
 			goto end;
 		}
 
 		if (mei_flow_ctrl_reduce(dev, &dev->wd_cl)) {
-			dev_dbg(&dev->pdev->dev, "mei_flow_ctrl_reduce() failed.\n");
+			dev_err(&dev->pdev->dev,
+				"wd: mei_flow_ctrl_reduce() failed.\n");
 			ret = -EIO;
 			goto end;
 		}
@@ -346,7 +344,7 @@ static const struct watchdog_info wd_info = {
 		.options = WDIOF_KEEPALIVEPING,
 };
 
-struct watchdog_device amt_wd_dev = {
+static struct watchdog_device amt_wd_dev = {
 		.info = &wd_info,
 		.ops = &wd_ops,
 		.timeout = AMT_WD_DEFAULT_TIMEOUT,
@@ -362,10 +360,12 @@ void  mei_watchdog_register(struct mei_device *dev)
 	dev->wd_due_counter = !!dev->wd_timeout;
 
 	if (watchdog_register_device(&amt_wd_dev)) {
-		dev_err(&dev->pdev->dev, "unable to register watchdog device.\n");
+		dev_err(&dev->pdev->dev,
+			"wd: unable to register watchdog device.\n");
 		dev->wd_interface_reg = false;
 	} else {
-		dev_dbg(&dev->pdev->dev, "successfully register watchdog interface.\n");
+		dev_dbg(&dev->pdev->dev,
+			"wd: successfully register watchdog interface.\n");
 		dev->wd_interface_reg = true;
 	}
 }
diff --git a/drivers/misc/pch_phub.c b/drivers/misc/pch_phub.c
index 10fc4785dba..9fbcacd703d 100644
--- a/drivers/misc/pch_phub.c
+++ b/drivers/misc/pch_phub.c
@@ -65,10 +65,6 @@
 #define PCI_VENDOR_ID_ROHM			0x10db
 #define PCI_DEVICE_ID_ROHM_ML7213_PHUB		0x801A
 
-/* Macros for ML7213 */
-#define PCI_VENDOR_ID_ROHM			0x10db
-#define PCI_DEVICE_ID_ROHM_ML7213_PHUB		0x801A
-
 /* Macros for ML7223 */
 #define PCI_DEVICE_ID_ROHM_ML7223_mPHUB	0x8012 /* for Bus-m */
 #define PCI_DEVICE_ID_ROHM_ML7223_nPHUB	0x8002 /* for Bus-n */
diff --git a/drivers/misc/pti.c b/drivers/misc/pti.c
index 383133b201a..b7eb545394b 100644
--- a/drivers/misc/pti.c
+++ b/drivers/misc/pti.c
@@ -888,7 +888,7 @@ static struct pci_driver pti_pci_driver = {
 	.name		= PCINAME,
 	.id_table	= pci_ids,
 	.probe		= pti_pci_probe,
-	.remove		= pti_pci_remove,
+	.remove		= __devexit_p(pti_pci_remove),
 };
 
 /**
diff --git a/drivers/mmc/card/block.c b/drivers/mmc/card/block.c
index dabec556ebb..dd2d374dcc7 100644
--- a/drivers/mmc/card/block.c
+++ b/drivers/mmc/card/block.c
@@ -384,7 +384,7 @@ static int mmc_blk_ioctl_cmd(struct block_device *bdev,
 	md = mmc_blk_get(bdev->bd_disk);
 	if (!md) {
 		err = -EINVAL;
-		goto cmd_done;
+		goto cmd_err;
 	}
 
 	card = md->queue.card;
@@ -483,6 +483,7 @@ cmd_rel_host:
 
 cmd_done:
 	mmc_blk_put(md);
+cmd_err:
 	kfree(idata->buf);
 	kfree(idata);
 	return err;
@@ -1283,7 +1284,7 @@ static int mmc_blk_issue_rw_rq(struct mmc_queue *mq, struct request *rqc)
 	int ret = 1, disable_multi = 0, retry = 0, type;
 	enum mmc_blk_status status;
 	struct mmc_queue_req *mq_rq;
-	struct request *req;
+	struct request *req = rqc;
 	struct mmc_async_req *areq;
 
 	if (!rqc && !mq->mqrq_prev->req)
@@ -1291,6 +1292,16 @@ static int mmc_blk_issue_rw_rq(struct mmc_queue *mq, struct request *rqc)
 
 	do {
 		if (rqc) {
+			/*
+			 * When 4KB native sector is enabled, only 8 blocks
+			 * multiple read or write is allowed
+			 */
+			if ((brq->data.blocks & 0x07) &&
+			    (card->ext_csd.data_sector_size == 4096)) {
+				pr_err("%s: Transfer size is not 4KB sector size aligned\n",
+					req->rq_disk->disk_name);
+				goto cmd_abort;
+			}
 			mmc_blk_rw_rq_prep(mq->mqrq_cur, card, 0, mq);
 			areq = &mq->mqrq_cur->mmc_active;
 		} else
@@ -1538,7 +1549,12 @@ static struct mmc_blk_data *mmc_blk_alloc_req(struct mmc_card *card,
 	snprintf(md->disk->disk_name, sizeof(md->disk->disk_name),
 		 "mmcblk%d%s", md->name_idx, subname ? subname : "");
 
-	blk_queue_logical_block_size(md->queue.queue, 512);
+	if (mmc_card_mmc(card))
+		blk_queue_logical_block_size(md->queue.queue,
+					     card->ext_csd.data_sector_size);
+	else
+		blk_queue_logical_block_size(md->queue.queue, 512);
+
 	set_capacity(md->disk, size);
 
 	if (mmc_host_cmd23(card->host)) {
diff --git a/drivers/mmc/card/queue.c b/drivers/mmc/card/queue.c
index 996f8e36e23..e360a979857 100644
--- a/drivers/mmc/card/queue.c
+++ b/drivers/mmc/card/queue.c
@@ -96,7 +96,7 @@ static int mmc_queue_thread(void *d)
  * on any queue on this host, and attempt to issue it.  This may
  * not be the queue we were asked to process.
  */
-static void mmc_request(struct request_queue *q)
+static void mmc_request_fn(struct request_queue *q)
 {
 	struct mmc_queue *mq = q->queuedata;
 	struct request *req;
@@ -171,12 +171,10 @@ int mmc_init_queue(struct mmc_queue *mq, struct mmc_card *card,
 		limit = *mmc_dev(host)->dma_mask;
 
 	mq->card = card;
-	mq->queue = blk_init_queue(mmc_request, lock);
+	mq->queue = blk_init_queue(mmc_request_fn, lock);
 	if (!mq->queue)
 		return -ENOMEM;
 
-	memset(&mq->mqrq_cur, 0, sizeof(mq->mqrq_cur));
-	memset(&mq->mqrq_prev, 0, sizeof(mq->mqrq_prev));
 	mq->mqrq_cur = mqrq_cur;
 	mq->mqrq_prev = mqrq_prev;
 	mq->queue->queuedata = mq;
diff --git a/drivers/mmc/core/bus.c b/drivers/mmc/core/bus.c
index c60cee92a2b..9b68933f27e 100644
--- a/drivers/mmc/core/bus.c
+++ b/drivers/mmc/core/bus.c
@@ -122,6 +122,7 @@ static int mmc_bus_remove(struct device *dev)
 	return 0;
 }
 
+#ifdef CONFIG_PM_SLEEP
 static int mmc_bus_suspend(struct device *dev)
 {
 	struct mmc_driver *drv = to_mmc_driver(dev->driver);
@@ -143,6 +144,7 @@ static int mmc_bus_resume(struct device *dev)
 		ret = drv->resume(card);
 	return ret;
 }
+#endif
 
 #ifdef CONFIG_PM_RUNTIME
 
diff --git a/drivers/mmc/core/cd-gpio.c b/drivers/mmc/core/cd-gpio.c
index 2c14be73254..f13e38decea 100644
--- a/drivers/mmc/core/cd-gpio.c
+++ b/drivers/mmc/core/cd-gpio.c
@@ -73,6 +73,9 @@ void mmc_cd_gpio_free(struct mmc_host *host)
 {
 	struct mmc_cd_gpio *cd = host->hotplug.handler_priv;
 
+	if (!cd)
+		return;
+
 	free_irq(host->hotplug.irq, host);
 	gpio_free(cd->gpio);
 	kfree(cd);
diff --git a/drivers/mmc/core/core.c b/drivers/mmc/core/core.c
index ba821fe70bc..0b6141d29db 100644
--- a/drivers/mmc/core/core.c
+++ b/drivers/mmc/core/core.c
@@ -42,6 +42,7 @@
 #include "sdio_ops.h"
 
 static struct workqueue_struct *workqueue;
+static const unsigned freqs[] = { 400000, 300000, 200000, 100000 };
 
 /*
  * Enabling software CRCs on the data blocks can be a significant (30%)
@@ -1157,6 +1158,9 @@ static void mmc_power_up(struct mmc_host *host)
 {
 	int bit;
 
+	if (host->ios.power_mode == MMC_POWER_ON)
+		return;
+
 	mmc_host_clk_hold(host);
 
 	/* If ocr is set, we use it */
@@ -1199,6 +1203,10 @@ static void mmc_power_up(struct mmc_host *host)
 void mmc_power_off(struct mmc_host *host)
 {
 	int err = 0;
+
+	if (host->ios.power_mode == MMC_POWER_OFF)
+		return;
+
 	mmc_host_clk_hold(host);
 
 	host->ios.clock = 0;
@@ -2005,7 +2013,6 @@ EXPORT_SYMBOL(mmc_detect_card_removed);
 
 void mmc_rescan(struct work_struct *work)
 {
-	static const unsigned freqs[] = { 400000, 300000, 200000, 100000 };
 	struct mmc_host *host =
 		container_of(work, struct mmc_host, detect.work);
 	int i;
@@ -2044,8 +2051,12 @@ void mmc_rescan(struct work_struct *work)
 	 */
 	mmc_bus_put(host);
 
-	if (host->ops->get_cd && host->ops->get_cd(host) == 0)
+	if (host->ops->get_cd && host->ops->get_cd(host) == 0) {
+		mmc_claim_host(host);
+		mmc_power_off(host);
+		mmc_release_host(host);
 		goto out;
+	}
 
 	mmc_claim_host(host);
 	for (i = 0; i < ARRAY_SIZE(freqs); i++) {
@@ -2063,7 +2074,8 @@ void mmc_rescan(struct work_struct *work)
 
 void mmc_start_host(struct mmc_host *host)
 {
-	mmc_power_off(host);
+	host->f_init = max(freqs[0], host->f_min);
+	mmc_power_up(host);
 	mmc_detect_change(host, 0);
 }
 
diff --git a/drivers/mmc/core/mmc.c b/drivers/mmc/core/mmc.c
index 54df5adc041..2d4a4b74675 100644
--- a/drivers/mmc/core/mmc.c
+++ b/drivers/mmc/core/mmc.c
@@ -235,6 +235,36 @@ static int mmc_get_ext_csd(struct mmc_card *card, u8 **new_ext_csd)
 	return err;
 }
 
+static void mmc_select_card_type(struct mmc_card *card)
+{
+	struct mmc_host *host = card->host;
+	u8 card_type = card->ext_csd.raw_card_type & EXT_CSD_CARD_TYPE_MASK;
+	unsigned int caps = host->caps, caps2 = host->caps2;
+	unsigned int hs_max_dtr = 0;
+
+	if (card_type & EXT_CSD_CARD_TYPE_26)
+		hs_max_dtr = MMC_HIGH_26_MAX_DTR;
+
+	if (caps & MMC_CAP_MMC_HIGHSPEED &&
+			card_type & EXT_CSD_CARD_TYPE_52)
+		hs_max_dtr = MMC_HIGH_52_MAX_DTR;
+
+	if ((caps & MMC_CAP_1_8V_DDR &&
+			card_type & EXT_CSD_CARD_TYPE_DDR_1_8V) ||
+	    (caps & MMC_CAP_1_2V_DDR &&
+			card_type & EXT_CSD_CARD_TYPE_DDR_1_2V))
+		hs_max_dtr = MMC_HIGH_DDR_MAX_DTR;
+
+	if ((caps2 & MMC_CAP2_HS200_1_8V_SDR &&
+			card_type & EXT_CSD_CARD_TYPE_SDR_1_8V) ||
+	    (caps2 & MMC_CAP2_HS200_1_2V_SDR &&
+			card_type & EXT_CSD_CARD_TYPE_SDR_1_2V))
+		hs_max_dtr = MMC_HS200_MAX_DTR;
+
+	card->ext_csd.hs_max_dtr = hs_max_dtr;
+	card->ext_csd.card_type = card_type;
+}
+
 /*
  * Decode extended CSD.
  */
@@ -284,56 +314,9 @@ static int mmc_read_ext_csd(struct mmc_card *card, u8 *ext_csd)
 		if (card->ext_csd.sectors > (2u * 1024 * 1024 * 1024) / 512)
 			mmc_card_set_blockaddr(card);
 	}
+
 	card->ext_csd.raw_card_type = ext_csd[EXT_CSD_CARD_TYPE];
-	switch (ext_csd[EXT_CSD_CARD_TYPE] & EXT_CSD_CARD_TYPE_MASK) {
-	case EXT_CSD_CARD_TYPE_SDR_ALL:
-	case EXT_CSD_CARD_TYPE_SDR_ALL_DDR_1_8V:
-	case EXT_CSD_CARD_TYPE_SDR_ALL_DDR_1_2V:
-	case EXT_CSD_CARD_TYPE_SDR_ALL_DDR_52:
-		card->ext_csd.hs_max_dtr = 200000000;
-		card->ext_csd.card_type = EXT_CSD_CARD_TYPE_SDR_200;
-		break;
-	case EXT_CSD_CARD_TYPE_SDR_1_2V_ALL:
-	case EXT_CSD_CARD_TYPE_SDR_1_2V_DDR_1_8V:
-	case EXT_CSD_CARD_TYPE_SDR_1_2V_DDR_1_2V:
-	case EXT_CSD_CARD_TYPE_SDR_1_2V_DDR_52:
-		card->ext_csd.hs_max_dtr = 200000000;
-		card->ext_csd.card_type = EXT_CSD_CARD_TYPE_SDR_1_2V;
-		break;
-	case EXT_CSD_CARD_TYPE_SDR_1_8V_ALL:
-	case EXT_CSD_CARD_TYPE_SDR_1_8V_DDR_1_8V:
-	case EXT_CSD_CARD_TYPE_SDR_1_8V_DDR_1_2V:
-	case EXT_CSD_CARD_TYPE_SDR_1_8V_DDR_52:
-		card->ext_csd.hs_max_dtr = 200000000;
-		card->ext_csd.card_type = EXT_CSD_CARD_TYPE_SDR_1_8V;
-		break;
-	case EXT_CSD_CARD_TYPE_DDR_52 | EXT_CSD_CARD_TYPE_52 |
-	     EXT_CSD_CARD_TYPE_26:
-		card->ext_csd.hs_max_dtr = 52000000;
-		card->ext_csd.card_type = EXT_CSD_CARD_TYPE_DDR_52;
-		break;
-	case EXT_CSD_CARD_TYPE_DDR_1_2V | EXT_CSD_CARD_TYPE_52 |
-	     EXT_CSD_CARD_TYPE_26:
-		card->ext_csd.hs_max_dtr = 52000000;
-		card->ext_csd.card_type = EXT_CSD_CARD_TYPE_DDR_1_2V;
-		break;
-	case EXT_CSD_CARD_TYPE_DDR_1_8V | EXT_CSD_CARD_TYPE_52 |
-	     EXT_CSD_CARD_TYPE_26:
-		card->ext_csd.hs_max_dtr = 52000000;
-		card->ext_csd.card_type = EXT_CSD_CARD_TYPE_DDR_1_8V;
-		break;
-	case EXT_CSD_CARD_TYPE_52 | EXT_CSD_CARD_TYPE_26:
-		card->ext_csd.hs_max_dtr = 52000000;
-		break;
-	case EXT_CSD_CARD_TYPE_26:
-		card->ext_csd.hs_max_dtr = 26000000;
-		break;
-	default:
-		/* MMC v4 spec says this cannot happen */
-		pr_warning("%s: card is mmc v4 but doesn't "
-			"support any high-speed modes.\n",
-			mmc_hostname(card->host));
-	}
+	mmc_select_card_type(card);
 
 	card->ext_csd.raw_s_a_timeout = ext_csd[EXT_CSD_S_A_TIMEOUT];
 	card->ext_csd.raw_erase_timeout_mult =
@@ -533,6 +516,8 @@ static int mmc_read_ext_csd(struct mmc_card *card, u8 *ext_csd)
 		} else {
 			card->ext_csd.data_tag_unit_size = 0;
 		}
+	} else {
+		card->ext_csd.data_sector_size = 512;
 	}
 
 out:
@@ -556,14 +541,10 @@ static int mmc_compare_ext_csds(struct mmc_card *card, unsigned bus_width)
 	err = mmc_get_ext_csd(card, &bw_ext_csd);
 
 	if (err || bw_ext_csd == NULL) {
-		if (bus_width != MMC_BUS_WIDTH_1)
-			err = -EINVAL;
+		err = -EINVAL;
 		goto out;
 	}
 
-	if (bus_width == MMC_BUS_WIDTH_1)
-		goto out;
-
 	/* only compare read only fields */
 	err = !((card->ext_csd.raw_partition_support ==
 			bw_ext_csd[EXT_CSD_PARTITION_SUPPORT]) &&
@@ -736,6 +717,10 @@ static int mmc_select_powerclass(struct mmc_card *card,
 				 card->ext_csd.generic_cmd6_time);
 	}
 
+	if (err)
+		pr_err("%s: power class selection for ext_csd_bus_width %d"
+		       " failed\n", mmc_hostname(card->host), bus_width);
+
 	return err;
 }
 
@@ -745,7 +730,7 @@ static int mmc_select_powerclass(struct mmc_card *card,
  */
 static int mmc_select_hs200(struct mmc_card *card)
 {
-	int idx, err = 0;
+	int idx, err = -EINVAL;
 	struct mmc_host *host;
 	static unsigned ext_csd_bits[] = {
 		EXT_CSD_BUS_WIDTH_4,
@@ -761,10 +746,12 @@ static int mmc_select_hs200(struct mmc_card *card)
 	host = card->host;
 
 	if (card->ext_csd.card_type & EXT_CSD_CARD_TYPE_SDR_1_2V &&
-	    host->caps2 & MMC_CAP2_HS200_1_2V_SDR)
-		if (mmc_set_signal_voltage(host, MMC_SIGNAL_VOLTAGE_120, 0))
-			err = mmc_set_signal_voltage(host,
-						     MMC_SIGNAL_VOLTAGE_180, 0);
+			host->caps2 & MMC_CAP2_HS200_1_2V_SDR)
+		err = mmc_set_signal_voltage(host, MMC_SIGNAL_VOLTAGE_120, 0);
+
+	if (err && card->ext_csd.card_type & EXT_CSD_CARD_TYPE_SDR_1_8V &&
+			host->caps2 & MMC_CAP2_HS200_1_8V_SDR)
+		err = mmc_set_signal_voltage(host, MMC_SIGNAL_VOLTAGE_180, 0);
 
 	/* If fails try again during next card power cycle */
 	if (err)
@@ -1117,9 +1104,7 @@ static int mmc_init_card(struct mmc_host *host, u32 ocr,
 				EXT_CSD_BUS_WIDTH_8 : EXT_CSD_BUS_WIDTH_4;
 		err = mmc_select_powerclass(card, ext_csd_bits, ext_csd);
 		if (err)
-			pr_warning("%s: power class selection to bus width %d"
-				   " failed\n", mmc_hostname(card->host),
-				   1 << bus_width);
+			goto err;
 	}
 
 	/*
@@ -1151,10 +1136,7 @@ static int mmc_init_card(struct mmc_host *host, u32 ocr,
 			err = mmc_select_powerclass(card, ext_csd_bits[idx][0],
 						    ext_csd);
 			if (err)
-				pr_warning("%s: power class selection to "
-					   "bus width %d failed\n",
-					   mmc_hostname(card->host),
-					   1 << bus_width);
+				goto err;
 
 			err = mmc_switch(card, EXT_CSD_CMD_SET_NORMAL,
 					 EXT_CSD_BUS_WIDTH,
@@ -1182,10 +1164,7 @@ static int mmc_init_card(struct mmc_host *host, u32 ocr,
 			err = mmc_select_powerclass(card, ext_csd_bits[idx][1],
 						    ext_csd);
 			if (err)
-				pr_warning("%s: power class selection to "
-					   "bus width %d ddr %d failed\n",
-					   mmc_hostname(card->host),
-					   1 << bus_width, ddr);
+				goto err;
 
 			err = mmc_switch(card, EXT_CSD_CMD_SET_NORMAL,
 					 EXT_CSD_BUS_WIDTH,
diff --git a/drivers/mmc/core/sdio.c b/drivers/mmc/core/sdio.c
index 2c7c83f832d..13d0e95380a 100644
--- a/drivers/mmc/core/sdio.c
+++ b/drivers/mmc/core/sdio.c
@@ -947,7 +947,7 @@ static int mmc_sdio_resume(struct mmc_host *host)
 	}
 
 	if (!err && host->sdio_irqs)
-		mmc_signal_sdio_irq(host);
+		wake_up_process(host->sdio_irq_thread);
 	mmc_release_host(host);
 
 	/*
diff --git a/drivers/mmc/core/sdio_irq.c b/drivers/mmc/core/sdio_irq.c
index f573e7f9f74..3d8ceb4084d 100644
--- a/drivers/mmc/core/sdio_irq.c
+++ b/drivers/mmc/core/sdio_irq.c
@@ -28,18 +28,20 @@
 
 #include "sdio_ops.h"
 
-static int process_sdio_pending_irqs(struct mmc_card *card)
+static int process_sdio_pending_irqs(struct mmc_host *host)
 {
+	struct mmc_card *card = host->card;
 	int i, ret, count;
 	unsigned char pending;
 	struct sdio_func *func;
 
 	/*
 	 * Optimization, if there is only 1 function interrupt registered
-	 * call irq handler directly
+	 * and we know an IRQ was signaled then call irq handler directly.
+	 * Otherwise do the full probe.
 	 */
 	func = card->sdio_single_irq;
-	if (func) {
+	if (func && host->sdio_irq_pending) {
 		func->irq_handler(func);
 		return 1;
 	}
@@ -116,7 +118,8 @@ static int sdio_irq_thread(void *_host)
 		ret = __mmc_claim_host(host, &host->sdio_irq_thread_abort);
 		if (ret)
 			break;
-		ret = process_sdio_pending_irqs(host->card);
+		ret = process_sdio_pending_irqs(host);
+		host->sdio_irq_pending = false;
 		mmc_release_host(host);
 
 		/*
diff --git a/drivers/mmc/host/Kconfig b/drivers/mmc/host/Kconfig
index 2bc06e7344d..aa131b32e3b 100644
--- a/drivers/mmc/host/Kconfig
+++ b/drivers/mmc/host/Kconfig
@@ -278,10 +278,13 @@ choice
 	  Choose which driver to use for the Atmel MCI Silicon
 
 config MMC_AT91
-	tristate "AT91 SD/MMC Card Interface support"
+	tristate "AT91 SD/MMC Card Interface support (DEPRECATED)"
 	depends on ARCH_AT91
 	help
-	  This selects the AT91 MCI controller.
+	  This selects the AT91 MCI controller. This driver will
+	  be removed soon (for more information have a look to
+	  Documentation/feature-removal-schedule.txt). Please use
+	  MMC_ATMEL_MCI.
 
 	  If unsure, say N.
 
@@ -307,16 +310,6 @@ config MMC_ATMELMCI_DMA
 
 	  If unsure, say N.
 
-config MMC_IMX
-	tristate "Motorola i.MX Multimedia Card Interface support"
-	depends on ARCH_MX1
-	help
-	  This selects the Motorola i.MX Multimedia card Interface.
-	  If you have a i.MX platform with a Multimedia Card slot,
-	  say Y or M here.
-
-	  If unsure, say N.
-
 config MMC_MSM
 	tristate "Qualcomm SDCC Controller Support"
 	depends on MMC && ARCH_MSM
diff --git a/drivers/mmc/host/Makefile b/drivers/mmc/host/Makefile
index 3e7e26d0807..8922b06be92 100644
--- a/drivers/mmc/host/Makefile
+++ b/drivers/mmc/host/Makefile
@@ -4,7 +4,6 @@
 
 obj-$(CONFIG_MMC_ARMMMCI)	+= mmci.o
 obj-$(CONFIG_MMC_PXA)		+= pxamci.o
-obj-$(CONFIG_MMC_IMX)		+= imxmmc.o
 obj-$(CONFIG_MMC_MXC)		+= mxcmmc.o
 obj-$(CONFIG_MMC_MXS)		+= mxs-mmc.o
 obj-$(CONFIG_MMC_SDHCI)		+= sdhci.o
diff --git a/drivers/mmc/host/atmel-mci.c b/drivers/mmc/host/atmel-mci.c
index e94476beca1..420aca642b1 100644
--- a/drivers/mmc/host/atmel-mci.c
+++ b/drivers/mmc/host/atmel-mci.c
@@ -45,19 +45,19 @@
 #define ATMCI_DMA_THRESHOLD	16
 
 enum {
-	EVENT_CMD_COMPLETE = 0,
+	EVENT_CMD_RDY = 0,
 	EVENT_XFER_COMPLETE,
-	EVENT_DATA_COMPLETE,
+	EVENT_NOTBUSY,
 	EVENT_DATA_ERROR,
 };
 
 enum atmel_mci_state {
 	STATE_IDLE = 0,
 	STATE_SENDING_CMD,
-	STATE_SENDING_DATA,
-	STATE_DATA_BUSY,
+	STATE_DATA_XFER,
+	STATE_WAITING_NOTBUSY,
 	STATE_SENDING_STOP,
-	STATE_DATA_ERROR,
+	STATE_END_REQUEST,
 };
 
 enum atmci_xfer_dir {
@@ -78,6 +78,9 @@ struct atmel_mci_caps {
 	bool    has_highspeed;
 	bool    has_rwproof;
 	bool	has_odd_clk_div;
+	bool	has_bad_data_ordering;
+	bool	need_reset_after_xfer;
+	bool	need_blksz_mul_4;
 };
 
 struct atmel_mci_dma {
@@ -91,6 +94,11 @@ struct atmel_mci_dma {
  * @regs: Pointer to MMIO registers.
  * @sg: Scatterlist entry currently being processed by PIO or PDC code.
  * @pio_offset: Offset into the current scatterlist entry.
+ * @buffer: Buffer used if we don't have the r/w proof capability. We
+ *      don't have the time to switch pdc buffers so we have to use only
+ *      one buffer for the full transaction.
+ * @buf_size: size of the buffer.
+ * @phys_buf_addr: buffer address needed for pdc.
  * @cur_slot: The slot which is currently using the controller.
  * @mrq: The request currently being processed on @cur_slot,
  *	or NULL if the controller is idle.
@@ -116,6 +124,7 @@ struct atmel_mci_dma {
  * @queue: List of slots waiting for access to the controller.
  * @need_clock_update: Update the clock rate before the next request.
  * @need_reset: Reset controller before next request.
+ * @timer: Timer to balance the data timeout error flag which cannot rise.
  * @mode_reg: Value of the MR register.
  * @cfg_reg: Value of the CFG register.
  * @bus_hz: The rate of @mck in Hz. This forms the basis for MMC bus
@@ -166,6 +175,9 @@ struct atmel_mci {
 
 	struct scatterlist	*sg;
 	unsigned int		pio_offset;
+	unsigned int		*buffer;
+	unsigned int		buf_size;
+	dma_addr_t		buf_phys_addr;
 
 	struct atmel_mci_slot	*cur_slot;
 	struct mmc_request	*mrq;
@@ -189,6 +201,7 @@ struct atmel_mci {
 
 	bool			need_clock_update;
 	bool			need_reset;
+	struct timer_list	timer;
 	u32			mode_reg;
 	u32			cfg_reg;
 	unsigned long		bus_hz;
@@ -480,6 +493,32 @@ err:
 	dev_err(&mmc->class_dev, "failed to initialize debugfs for slot\n");
 }
 
+static inline unsigned int atmci_get_version(struct atmel_mci *host)
+{
+	return atmci_readl(host, ATMCI_VERSION) & 0x00000fff;
+}
+
+static void atmci_timeout_timer(unsigned long data)
+{
+	struct atmel_mci *host;
+
+	host = (struct atmel_mci *)data;
+
+	dev_dbg(&host->pdev->dev, "software timeout\n");
+
+	if (host->mrq->cmd->data) {
+		host->mrq->cmd->data->error = -ETIMEDOUT;
+		host->data = NULL;
+	} else {
+		host->mrq->cmd->error = -ETIMEDOUT;
+		host->cmd = NULL;
+	}
+	host->need_reset = 1;
+	host->state = STATE_END_REQUEST;
+	smp_wmb();
+	tasklet_schedule(&host->tasklet);
+}
+
 static inline unsigned int atmci_ns_to_clocks(struct atmel_mci *host,
 					unsigned int ns)
 {
@@ -591,6 +630,7 @@ static void atmci_send_command(struct atmel_mci *host,
 
 static void atmci_send_stop_cmd(struct atmel_mci *host, struct mmc_data *data)
 {
+	dev_dbg(&host->pdev->dev, "send stop command\n");
 	atmci_send_command(host, data->stop, host->stop_cmdr);
 	atmci_writel(host, ATMCI_IER, ATMCI_CMDRDY);
 }
@@ -603,6 +643,7 @@ static void atmci_pdc_set_single_buf(struct atmel_mci *host,
 	enum atmci_xfer_dir dir, enum atmci_pdc_buf buf_nb)
 {
 	u32 pointer_reg, counter_reg;
+	unsigned int buf_size;
 
 	if (dir == XFER_RECEIVE) {
 		pointer_reg = ATMEL_PDC_RPR;
@@ -617,8 +658,15 @@ static void atmci_pdc_set_single_buf(struct atmel_mci *host,
 		counter_reg += ATMEL_PDC_SCND_BUF_OFF;
 	}
 
-	atmci_writel(host, pointer_reg, sg_dma_address(host->sg));
-	if (host->data_size <= sg_dma_len(host->sg)) {
+	if (!host->caps.has_rwproof) {
+		buf_size = host->buf_size;
+		atmci_writel(host, pointer_reg, host->buf_phys_addr);
+	} else {
+		buf_size = sg_dma_len(host->sg);
+		atmci_writel(host, pointer_reg, sg_dma_address(host->sg));
+	}
+
+	if (host->data_size <= buf_size) {
 		if (host->data_size & 0x3) {
 			/* If size is different from modulo 4, transfer bytes */
 			atmci_writel(host, counter_reg, host->data_size);
@@ -670,7 +718,20 @@ static void atmci_pdc_cleanup(struct atmel_mci *host)
  */
 static void atmci_pdc_complete(struct atmel_mci *host)
 {
+	int transfer_size = host->data->blocks * host->data->blksz;
+	int i;
+
 	atmci_writel(host, ATMEL_PDC_PTCR, ATMEL_PDC_RXTDIS | ATMEL_PDC_TXTDIS);
+
+	if ((!host->caps.has_rwproof)
+	    && (host->data->flags & MMC_DATA_READ)) {
+		if (host->caps.has_bad_data_ordering)
+			for (i = 0; i < transfer_size; i++)
+				host->buffer[i] = swab32(host->buffer[i]);
+		sg_copy_from_buffer(host->data->sg, host->data->sg_len,
+		                    host->buffer, transfer_size);
+	}
+
 	atmci_pdc_cleanup(host);
 
 	/*
@@ -678,9 +739,10 @@ static void atmci_pdc_complete(struct atmel_mci *host)
 	 * to send the stop command or waiting for NBUSY in this case.
 	 */
 	if (host->data) {
+		dev_dbg(&host->pdev->dev,
+		        "(%s) set pending xfer complete\n", __func__);
 		atmci_set_pending(host, EVENT_XFER_COMPLETE);
 		tasklet_schedule(&host->tasklet);
-		atmci_writel(host, ATMCI_IER, ATMCI_NOTBUSY);
 	}
 }
 
@@ -716,6 +778,8 @@ static void atmci_dma_complete(void *arg)
 	 * to send the stop command or waiting for NBUSY in this case.
 	 */
 	if (data) {
+		dev_dbg(&host->pdev->dev,
+		        "(%s) set pending xfer complete\n", __func__);
 		atmci_set_pending(host, EVENT_XFER_COMPLETE);
 		tasklet_schedule(&host->tasklet);
 
@@ -791,6 +855,7 @@ atmci_prepare_data_pdc(struct atmel_mci *host, struct mmc_data *data)
 	u32 iflags, tmp;
 	unsigned int sg_len;
 	enum dma_data_direction dir;
+	int i;
 
 	data->error = -EINPROGRESS;
 
@@ -806,7 +871,7 @@ atmci_prepare_data_pdc(struct atmel_mci *host, struct mmc_data *data)
 		iflags |= ATMCI_ENDRX | ATMCI_RXBUFF;
 	} else {
 		dir = DMA_TO_DEVICE;
-		iflags |= ATMCI_ENDTX | ATMCI_TXBUFE;
+		iflags |= ATMCI_ENDTX | ATMCI_TXBUFE | ATMCI_BLKE;
 	}
 
 	/* Set BLKLEN */
@@ -818,6 +883,16 @@ atmci_prepare_data_pdc(struct atmel_mci *host, struct mmc_data *data)
 	/* Configure PDC */
 	host->data_size = data->blocks * data->blksz;
 	sg_len = dma_map_sg(&host->pdev->dev, data->sg, data->sg_len, dir);
+
+	if ((!host->caps.has_rwproof)
+	    && (host->data->flags & MMC_DATA_WRITE)) {
+		sg_copy_to_buffer(host->data->sg, host->data->sg_len,
+		                  host->buffer, host->data_size);
+		if (host->caps.has_bad_data_ordering)
+			for (i = 0; i < host->data_size; i++)
+				host->buffer[i] = swab32(host->buffer[i]);
+	}
+
 	if (host->data_size)
 		atmci_pdc_set_both_buf(host,
 			((dir == DMA_FROM_DEVICE) ? XFER_RECEIVE : XFER_TRANSMIT));
@@ -931,6 +1006,8 @@ atmci_submit_data_dma(struct atmel_mci *host, struct mmc_data *data)
 
 static void atmci_stop_transfer(struct atmel_mci *host)
 {
+	dev_dbg(&host->pdev->dev,
+	        "(%s) set pending xfer complete\n", __func__);
 	atmci_set_pending(host, EVENT_XFER_COMPLETE);
 	atmci_writel(host, ATMCI_IER, ATMCI_NOTBUSY);
 }
@@ -940,8 +1017,7 @@ static void atmci_stop_transfer(struct atmel_mci *host)
  */
 static void atmci_stop_transfer_pdc(struct atmel_mci *host)
 {
-	atmci_set_pending(host, EVENT_XFER_COMPLETE);
-	atmci_writel(host, ATMCI_IER, ATMCI_NOTBUSY);
+	atmci_writel(host, ATMEL_PDC_PTCR, ATMEL_PDC_RXTDIS | ATMEL_PDC_TXTDIS);
 }
 
 static void atmci_stop_transfer_dma(struct atmel_mci *host)
@@ -953,6 +1029,8 @@ static void atmci_stop_transfer_dma(struct atmel_mci *host)
 		atmci_dma_cleanup(host);
 	} else {
 		/* Data transfer was stopped by the interrupt handler */
+		dev_dbg(&host->pdev->dev,
+		        "(%s) set pending xfer complete\n", __func__);
 		atmci_set_pending(host, EVENT_XFER_COMPLETE);
 		atmci_writel(host, ATMCI_IER, ATMCI_NOTBUSY);
 	}
@@ -977,9 +1055,12 @@ static void atmci_start_request(struct atmel_mci *host,
 
 	host->pending_events = 0;
 	host->completed_events = 0;
+	host->cmd_status = 0;
 	host->data_status = 0;
 
-	if (host->need_reset) {
+	dev_dbg(&host->pdev->dev, "start request: cmd %u\n", mrq->cmd->opcode);
+
+	if (host->need_reset || host->caps.need_reset_after_xfer) {
 		iflags = atmci_readl(host, ATMCI_IMR);
 		iflags &= (ATMCI_SDIOIRQA | ATMCI_SDIOIRQB);
 		atmci_writel(host, ATMCI_CR, ATMCI_CR_SWRST);
@@ -994,7 +1075,7 @@ static void atmci_start_request(struct atmel_mci *host,
 
 	iflags = atmci_readl(host, ATMCI_IMR);
 	if (iflags & ~(ATMCI_SDIOIRQA | ATMCI_SDIOIRQB))
-		dev_warn(&slot->mmc->class_dev, "WARNING: IMR=0x%08x\n",
+		dev_dbg(&slot->mmc->class_dev, "WARNING: IMR=0x%08x\n",
 				iflags);
 
 	if (unlikely(test_and_clear_bit(ATMCI_CARD_NEED_INIT, &slot->flags))) {
@@ -1043,6 +1124,8 @@ static void atmci_start_request(struct atmel_mci *host,
 	 * prepared yet.)
 	 */
 	atmci_writel(host, ATMCI_IER, iflags);
+
+	mod_timer(&host->timer, jiffies +  msecs_to_jiffies(2000));
 }
 
 static void atmci_queue_request(struct atmel_mci *host,
@@ -1057,6 +1140,7 @@ static void atmci_queue_request(struct atmel_mci *host,
 		host->state = STATE_SENDING_CMD;
 		atmci_start_request(host, slot);
 	} else {
+		dev_dbg(&host->pdev->dev, "queue request\n");
 		list_add_tail(&slot->queue_node, &host->queue);
 	}
 	spin_unlock_bh(&host->lock);
@@ -1069,6 +1153,7 @@ static void atmci_request(struct mmc_host *mmc, struct mmc_request *mrq)
 	struct mmc_data		*data;
 
 	WARN_ON(slot->mrq);
+	dev_dbg(&host->pdev->dev, "MRQ: cmd %u\n", mrq->cmd->opcode);
 
 	/*
 	 * We may "know" the card is gone even though there's still an
@@ -1308,6 +1393,8 @@ static void atmci_request_end(struct atmel_mci *host, struct mmc_request *mrq)
 		host->state = STATE_IDLE;
 	}
 
+	del_timer(&host->timer);
+
 	spin_unlock(&host->lock);
 	mmc_request_done(prev_mmc, mrq);
 	spin_lock(&host->lock);
@@ -1330,21 +1417,13 @@ static void atmci_command_complete(struct atmel_mci *host,
 		cmd->error = -EILSEQ;
 	else if (status & (ATMCI_RINDE | ATMCI_RDIRE | ATMCI_RENDE))
 		cmd->error = -EIO;
-	else
-		cmd->error = 0;
-
-	if (cmd->error) {
-		dev_dbg(&host->pdev->dev,
-			"command error: status=0x%08x\n", status);
-
-		if (cmd->data) {
-			host->stop_transfer(host);
-			host->data = NULL;
-			atmci_writel(host, ATMCI_IDR, ATMCI_NOTBUSY
-					| ATMCI_TXRDY | ATMCI_RXRDY
-					| ATMCI_DATA_ERROR_FLAGS);
+	else if (host->mrq->data && (host->mrq->data->blksz & 3)) {
+		if (host->caps.need_blksz_mul_4) {
+			cmd->error = -EINVAL;
+			host->need_reset = 1;
 		}
-	}
+	} else
+		cmd->error = 0;
 }
 
 static void atmci_detect_change(unsigned long data)
@@ -1407,23 +1486,21 @@ static void atmci_detect_change(unsigned long data)
 					break;
 				case STATE_SENDING_CMD:
 					mrq->cmd->error = -ENOMEDIUM;
-					if (!mrq->data)
-						break;
-					/* fall through */
-				case STATE_SENDING_DATA:
+					if (mrq->data)
+						host->stop_transfer(host);
+					break;
+				case STATE_DATA_XFER:
 					mrq->data->error = -ENOMEDIUM;
 					host->stop_transfer(host);
 					break;
-				case STATE_DATA_BUSY:
-				case STATE_DATA_ERROR:
-					if (mrq->data->error == -EINPROGRESS)
-						mrq->data->error = -ENOMEDIUM;
-					if (!mrq->stop)
-						break;
-					/* fall through */
+				case STATE_WAITING_NOTBUSY:
+					mrq->data->error = -ENOMEDIUM;
+					break;
 				case STATE_SENDING_STOP:
 					mrq->stop->error = -ENOMEDIUM;
 					break;
+				case STATE_END_REQUEST:
+					break;
 				}
 
 				atmci_request_end(host, mrq);
@@ -1451,7 +1528,6 @@ static void atmci_tasklet_func(unsigned long priv)
 	struct atmel_mci	*host = (struct atmel_mci *)priv;
 	struct mmc_request	*mrq = host->mrq;
 	struct mmc_data		*data = host->data;
-	struct mmc_command	*cmd = host->cmd;
 	enum atmel_mci_state	state = host->state;
 	enum atmel_mci_state	prev_state;
 	u32			status;
@@ -1467,107 +1543,186 @@ static void atmci_tasklet_func(unsigned long priv)
 
 	do {
 		prev_state = state;
+		dev_dbg(&host->pdev->dev, "FSM: state=%d\n", state);
 
 		switch (state) {
 		case STATE_IDLE:
 			break;
 
 		case STATE_SENDING_CMD:
+			/*
+			 * Command has been sent, we are waiting for command
+			 * ready. Then we have three next states possible:
+			 * END_REQUEST by default, WAITING_NOTBUSY if it's a
+			 * command needing it or DATA_XFER if there is data.
+			 */
+			dev_dbg(&host->pdev->dev, "FSM: cmd ready?\n");
 			if (!atmci_test_and_clear_pending(host,
-						EVENT_CMD_COMPLETE))
+						EVENT_CMD_RDY))
 				break;
 
+			dev_dbg(&host->pdev->dev, "set completed cmd ready\n");
 			host->cmd = NULL;
-			atmci_set_completed(host, EVENT_CMD_COMPLETE);
+			atmci_set_completed(host, EVENT_CMD_RDY);
 			atmci_command_complete(host, mrq->cmd);
-			if (!mrq->data || cmd->error) {
-				atmci_request_end(host, host->mrq);
-				goto unlock;
-			}
+			if (mrq->data) {
+				dev_dbg(&host->pdev->dev,
+				        "command with data transfer");
+				/*
+				 * If there is a command error don't start
+				 * data transfer.
+				 */
+				if (mrq->cmd->error) {
+					host->stop_transfer(host);
+					host->data = NULL;
+					atmci_writel(host, ATMCI_IDR,
+					             ATMCI_TXRDY | ATMCI_RXRDY
+					             | ATMCI_DATA_ERROR_FLAGS);
+					state = STATE_END_REQUEST;
+				} else
+					state = STATE_DATA_XFER;
+			} else if ((!mrq->data) && (mrq->cmd->flags & MMC_RSP_BUSY)) {
+				dev_dbg(&host->pdev->dev,
+				        "command response need waiting notbusy");
+				atmci_writel(host, ATMCI_IER, ATMCI_NOTBUSY);
+				state = STATE_WAITING_NOTBUSY;
+			} else
+				state = STATE_END_REQUEST;
 
-			prev_state = state = STATE_SENDING_DATA;
-			/* fall through */
+			break;
 
-		case STATE_SENDING_DATA:
+		case STATE_DATA_XFER:
 			if (atmci_test_and_clear_pending(host,
 						EVENT_DATA_ERROR)) {
-				host->stop_transfer(host);
-				if (data->stop)
-					atmci_send_stop_cmd(host, data);
-				state = STATE_DATA_ERROR;
+				dev_dbg(&host->pdev->dev, "set completed data error\n");
+				atmci_set_completed(host, EVENT_DATA_ERROR);
+				state = STATE_END_REQUEST;
 				break;
 			}
 
+			/*
+			 * A data transfer is in progress. The event expected
+			 * to move to the next state depends of data transfer
+			 * type (PDC or DMA). Once transfer done we can move
+			 * to the next step which is WAITING_NOTBUSY in write
+			 * case and directly SENDING_STOP in read case.
+			 */
+			dev_dbg(&host->pdev->dev, "FSM: xfer complete?\n");
 			if (!atmci_test_and_clear_pending(host,
 						EVENT_XFER_COMPLETE))
 				break;
 
+			dev_dbg(&host->pdev->dev,
+			        "(%s) set completed xfer complete\n",
+				__func__);
 			atmci_set_completed(host, EVENT_XFER_COMPLETE);
-			prev_state = state = STATE_DATA_BUSY;
-			/* fall through */
 
-		case STATE_DATA_BUSY:
-			if (!atmci_test_and_clear_pending(host,
-						EVENT_DATA_COMPLETE))
-				break;
-
-			host->data = NULL;
-			atmci_set_completed(host, EVENT_DATA_COMPLETE);
-			status = host->data_status;
-			if (unlikely(status & ATMCI_DATA_ERROR_FLAGS)) {
-				if (status & ATMCI_DTOE) {
-					dev_dbg(&host->pdev->dev,
-							"data timeout error\n");
-					data->error = -ETIMEDOUT;
-				} else if (status & ATMCI_DCRCE) {
-					dev_dbg(&host->pdev->dev,
-							"data CRC error\n");
-					data->error = -EILSEQ;
-				} else {
-					dev_dbg(&host->pdev->dev,
-						"data FIFO error (status=%08x)\n",
-						status);
-					data->error = -EIO;
-				}
+			if (host->data->flags & MMC_DATA_WRITE) {
+				atmci_writel(host, ATMCI_IER, ATMCI_NOTBUSY);
+				state = STATE_WAITING_NOTBUSY;
+			} else if (host->mrq->stop) {
+				atmci_writel(host, ATMCI_IER, ATMCI_CMDRDY);
+				atmci_send_stop_cmd(host, data);
+				state = STATE_SENDING_STOP;
 			} else {
+				host->data = NULL;
 				data->bytes_xfered = data->blocks * data->blksz;
 				data->error = 0;
-				atmci_writel(host, ATMCI_IDR, ATMCI_DATA_ERROR_FLAGS);
+				state = STATE_END_REQUEST;
 			}
+			break;
 
-			if (!data->stop) {
-				atmci_request_end(host, host->mrq);
-				goto unlock;
-			}
+		case STATE_WAITING_NOTBUSY:
+			/*
+			 * We can be in the state for two reasons: a command
+			 * requiring waiting not busy signal (stop command
+			 * included) or a write operation. In the latest case,
+			 * we need to send a stop command.
+			 */
+			dev_dbg(&host->pdev->dev, "FSM: not busy?\n");
+			if (!atmci_test_and_clear_pending(host,
+						EVENT_NOTBUSY))
+				break;
 
-			prev_state = state = STATE_SENDING_STOP;
-			if (!data->error)
-				atmci_send_stop_cmd(host, data);
-			/* fall through */
+			dev_dbg(&host->pdev->dev, "set completed not busy\n");
+			atmci_set_completed(host, EVENT_NOTBUSY);
+
+			if (host->data) {
+				/*
+				 * For some commands such as CMD53, even if
+				 * there is data transfer, there is no stop
+				 * command to send.
+				 */
+				if (host->mrq->stop) {
+					atmci_writel(host, ATMCI_IER,
+					             ATMCI_CMDRDY);
+					atmci_send_stop_cmd(host, data);
+					state = STATE_SENDING_STOP;
+				} else {
+					host->data = NULL;
+					data->bytes_xfered = data->blocks
+					                     * data->blksz;
+					data->error = 0;
+					state = STATE_END_REQUEST;
+				}
+			} else
+				state = STATE_END_REQUEST;
+			break;
 
 		case STATE_SENDING_STOP:
+			/*
+			 * In this state, it is important to set host->data to
+			 * NULL (which is tested in the waiting notbusy state)
+			 * in order to go to the end request state instead of
+			 * sending stop again.
+			 */
+			dev_dbg(&host->pdev->dev, "FSM: cmd ready?\n");
 			if (!atmci_test_and_clear_pending(host,
-						EVENT_CMD_COMPLETE))
+						EVENT_CMD_RDY))
 				break;
 
+			dev_dbg(&host->pdev->dev, "FSM: cmd ready\n");
 			host->cmd = NULL;
+			host->data = NULL;
+			data->bytes_xfered = data->blocks * data->blksz;
+			data->error = 0;
 			atmci_command_complete(host, mrq->stop);
-			atmci_request_end(host, host->mrq);
-			goto unlock;
+			if (mrq->stop->error) {
+				host->stop_transfer(host);
+				atmci_writel(host, ATMCI_IDR,
+				             ATMCI_TXRDY | ATMCI_RXRDY
+				             | ATMCI_DATA_ERROR_FLAGS);
+				state = STATE_END_REQUEST;
+			} else {
+				atmci_writel(host, ATMCI_IER, ATMCI_NOTBUSY);
+				state = STATE_WAITING_NOTBUSY;
+			}
+			break;
 
-		case STATE_DATA_ERROR:
-			if (!atmci_test_and_clear_pending(host,
-						EVENT_XFER_COMPLETE))
-				break;
+		case STATE_END_REQUEST:
+			atmci_writel(host, ATMCI_IDR, ATMCI_TXRDY | ATMCI_RXRDY
+			                   | ATMCI_DATA_ERROR_FLAGS);
+			status = host->data_status;
+			if (unlikely(status)) {
+				host->stop_transfer(host);
+				host->data = NULL;
+				if (status & ATMCI_DTOE) {
+					data->error = -ETIMEDOUT;
+				} else if (status & ATMCI_DCRCE) {
+					data->error = -EILSEQ;
+				} else {
+					data->error = -EIO;
+				}
+			}
 
-			state = STATE_DATA_BUSY;
+			atmci_request_end(host, host->mrq);
+			state = STATE_IDLE;
 			break;
 		}
 	} while (state != prev_state);
 
 	host->state = state;
 
-unlock:
 	spin_unlock(&host->lock);
 }
 
@@ -1620,9 +1775,6 @@ static void atmci_read_data_pio(struct atmel_mci *host)
 						| ATMCI_DATA_ERROR_FLAGS));
 			host->data_status = status;
 			data->bytes_xfered += nbytes;
-			smp_wmb();
-			atmci_set_pending(host, EVENT_DATA_ERROR);
-			tasklet_schedule(&host->tasklet);
 			return;
 		}
 	} while (status & ATMCI_RXRDY);
@@ -1691,9 +1843,6 @@ static void atmci_write_data_pio(struct atmel_mci *host)
 						| ATMCI_DATA_ERROR_FLAGS));
 			host->data_status = status;
 			data->bytes_xfered += nbytes;
-			smp_wmb();
-			atmci_set_pending(host, EVENT_DATA_ERROR);
-			tasklet_schedule(&host->tasklet);
 			return;
 		}
 	} while (status & ATMCI_TXRDY);
@@ -1711,16 +1860,6 @@ done:
 	atmci_set_pending(host, EVENT_XFER_COMPLETE);
 }
 
-static void atmci_cmd_interrupt(struct atmel_mci *host, u32 status)
-{
-	atmci_writel(host, ATMCI_IDR, ATMCI_CMDRDY);
-
-	host->cmd_status = status;
-	smp_wmb();
-	atmci_set_pending(host, EVENT_CMD_COMPLETE);
-	tasklet_schedule(&host->tasklet);
-}
-
 static void atmci_sdio_interrupt(struct atmel_mci *host, u32 status)
 {
 	int	i;
@@ -1748,17 +1887,21 @@ static irqreturn_t atmci_interrupt(int irq, void *dev_id)
 			break;
 
 		if (pending & ATMCI_DATA_ERROR_FLAGS) {
+			dev_dbg(&host->pdev->dev, "IRQ: data error\n");
 			atmci_writel(host, ATMCI_IDR, ATMCI_DATA_ERROR_FLAGS
-					| ATMCI_RXRDY | ATMCI_TXRDY);
-			pending &= atmci_readl(host, ATMCI_IMR);
+					| ATMCI_RXRDY | ATMCI_TXRDY
+					| ATMCI_ENDRX | ATMCI_ENDTX
+					| ATMCI_RXBUFF | ATMCI_TXBUFE);
 
 			host->data_status = status;
+			dev_dbg(&host->pdev->dev, "set pending data error\n");
 			smp_wmb();
 			atmci_set_pending(host, EVENT_DATA_ERROR);
 			tasklet_schedule(&host->tasklet);
 		}
 
 		if (pending & ATMCI_TXBUFE) {
+			dev_dbg(&host->pdev->dev, "IRQ: tx buffer empty\n");
 			atmci_writel(host, ATMCI_IDR, ATMCI_TXBUFE);
 			atmci_writel(host, ATMCI_IDR, ATMCI_ENDTX);
 			/*
@@ -1774,6 +1917,7 @@ static irqreturn_t atmci_interrupt(int irq, void *dev_id)
 				atmci_pdc_complete(host);
 			}
 		} else if (pending & ATMCI_ENDTX) {
+			dev_dbg(&host->pdev->dev, "IRQ: end of tx buffer\n");
 			atmci_writel(host, ATMCI_IDR, ATMCI_ENDTX);
 
 			if (host->data_size) {
@@ -1784,6 +1928,7 @@ static irqreturn_t atmci_interrupt(int irq, void *dev_id)
 		}
 
 		if (pending & ATMCI_RXBUFF) {
+			dev_dbg(&host->pdev->dev, "IRQ: rx buffer full\n");
 			atmci_writel(host, ATMCI_IDR, ATMCI_RXBUFF);
 			atmci_writel(host, ATMCI_IDR, ATMCI_ENDRX);
 			/*
@@ -1799,6 +1944,7 @@ static irqreturn_t atmci_interrupt(int irq, void *dev_id)
 				atmci_pdc_complete(host);
 			}
 		} else if (pending & ATMCI_ENDRX) {
+			dev_dbg(&host->pdev->dev, "IRQ: end of rx buffer\n");
 			atmci_writel(host, ATMCI_IDR, ATMCI_ENDRX);
 
 			if (host->data_size) {
@@ -1808,23 +1954,44 @@ static irqreturn_t atmci_interrupt(int irq, void *dev_id)
 			}
 		}
 
+		/*
+		 * First mci IPs, so mainly the ones having pdc, have some
+		 * issues with the notbusy signal. You can't get it after
+		 * data transmission if you have not sent a stop command.
+		 * The appropriate workaround is to use the BLKE signal.
+		 */
+		if (pending & ATMCI_BLKE) {
+			dev_dbg(&host->pdev->dev, "IRQ: blke\n");
+			atmci_writel(host, ATMCI_IDR, ATMCI_BLKE);
+			smp_wmb();
+			dev_dbg(&host->pdev->dev, "set pending notbusy\n");
+			atmci_set_pending(host, EVENT_NOTBUSY);
+			tasklet_schedule(&host->tasklet);
+		}
 
 		if (pending & ATMCI_NOTBUSY) {
-			atmci_writel(host, ATMCI_IDR,
-					ATMCI_DATA_ERROR_FLAGS | ATMCI_NOTBUSY);
-			if (!host->data_status)
-				host->data_status = status;
+			dev_dbg(&host->pdev->dev, "IRQ: not_busy\n");
+			atmci_writel(host, ATMCI_IDR, ATMCI_NOTBUSY);
 			smp_wmb();
-			atmci_set_pending(host, EVENT_DATA_COMPLETE);
+			dev_dbg(&host->pdev->dev, "set pending notbusy\n");
+			atmci_set_pending(host, EVENT_NOTBUSY);
 			tasklet_schedule(&host->tasklet);
 		}
+
 		if (pending & ATMCI_RXRDY)
 			atmci_read_data_pio(host);
 		if (pending & ATMCI_TXRDY)
 			atmci_write_data_pio(host);
 
-		if (pending & ATMCI_CMDRDY)
-			atmci_cmd_interrupt(host, status);
+		if (pending & ATMCI_CMDRDY) {
+			dev_dbg(&host->pdev->dev, "IRQ: cmd ready\n");
+			atmci_writel(host, ATMCI_IDR, ATMCI_CMDRDY);
+			host->cmd_status = status;
+			smp_wmb();
+			dev_dbg(&host->pdev->dev, "set pending cmd rdy\n");
+			atmci_set_pending(host, EVENT_CMD_RDY);
+			tasklet_schedule(&host->tasklet);
+		}
 
 		if (pending & (ATMCI_SDIOIRQA | ATMCI_SDIOIRQB))
 			atmci_sdio_interrupt(host, status);
@@ -1877,13 +2044,26 @@ static int __init atmci_init_slot(struct atmel_mci *host,
 		mmc->caps |= MMC_CAP_SDIO_IRQ;
 	if (host->caps.has_highspeed)
 		mmc->caps |= MMC_CAP_SD_HIGHSPEED;
-	if (slot_data->bus_width >= 4)
+	/*
+	 * Without the read/write proof capability, it is strongly suggested to
+	 * use only one bit for data to prevent fifo underruns and overruns
+	 * which will corrupt data.
+	 */
+	if ((slot_data->bus_width >= 4) && host->caps.has_rwproof)
 		mmc->caps |= MMC_CAP_4_BIT_DATA;
 
-	mmc->max_segs = 64;
-	mmc->max_req_size = 32768 * 512;
-	mmc->max_blk_size = 32768;
-	mmc->max_blk_count = 512;
+	if (atmci_get_version(host) < 0x200) {
+		mmc->max_segs = 256;
+		mmc->max_blk_size = 4095;
+		mmc->max_blk_count = 256;
+		mmc->max_req_size = mmc->max_blk_size * mmc->max_blk_count;
+		mmc->max_seg_size = mmc->max_blk_size * mmc->max_segs;
+	} else {
+		mmc->max_segs = 64;
+		mmc->max_req_size = 32768 * 512;
+		mmc->max_blk_size = 32768;
+		mmc->max_blk_count = 512;
+	}
 
 	/* Assume card is present initially */
 	set_bit(ATMCI_CARD_PRESENT, &slot->flags);
@@ -2007,11 +2187,6 @@ static bool atmci_configure_dma(struct atmel_mci *host)
 	}
 }
 
-static inline unsigned int atmci_get_version(struct atmel_mci *host)
-{
-	return atmci_readl(host, ATMCI_VERSION) & 0x00000fff;
-}
-
 /*
  * HSMCI (High Speed MCI) module is not fully compatible with MCI module.
  * HSMCI provides DMA support and a new config register but no more supports
@@ -2032,6 +2207,9 @@ static void __init atmci_get_cap(struct atmel_mci *host)
 	host->caps.has_highspeed = 0;
 	host->caps.has_rwproof = 0;
 	host->caps.has_odd_clk_div = 0;
+	host->caps.has_bad_data_ordering = 1;
+	host->caps.need_reset_after_xfer = 1;
+	host->caps.need_blksz_mul_4 = 1;
 
 	/* keep only major version number */
 	switch (version & 0xf00) {
@@ -2051,7 +2229,11 @@ static void __init atmci_get_cap(struct atmel_mci *host)
 		host->caps.has_highspeed = 1;
 	case 0x200:
 		host->caps.has_rwproof = 1;
+		host->caps.need_blksz_mul_4 = 0;
 	case 0x100:
+		host->caps.has_bad_data_ordering = 0;
+		host->caps.need_reset_after_xfer = 0;
+	case 0x0:
 		break;
 	default:
 		host->caps.has_pdc = 0;
@@ -2138,14 +2320,20 @@ static int __init atmci_probe(struct platform_device *pdev)
 	if (pdata->slot[0].bus_width) {
 		ret = atmci_init_slot(host, &pdata->slot[0],
 				0, ATMCI_SDCSEL_SLOT_A, ATMCI_SDIOIRQA);
-		if (!ret)
+		if (!ret) {
 			nr_slots++;
+			host->buf_size = host->slot[0]->mmc->max_req_size;
+		}
 	}
 	if (pdata->slot[1].bus_width) {
 		ret = atmci_init_slot(host, &pdata->slot[1],
 				1, ATMCI_SDCSEL_SLOT_B, ATMCI_SDIOIRQB);
-		if (!ret)
+		if (!ret) {
 			nr_slots++;
+			if (host->slot[1]->mmc->max_req_size > host->buf_size)
+				host->buf_size =
+					host->slot[1]->mmc->max_req_size;
+		}
 	}
 
 	if (!nr_slots) {
@@ -2153,6 +2341,19 @@ static int __init atmci_probe(struct platform_device *pdev)
 		goto err_init_slot;
 	}
 
+	if (!host->caps.has_rwproof) {
+		host->buffer = dma_alloc_coherent(&pdev->dev, host->buf_size,
+		                                  &host->buf_phys_addr,
+						  GFP_KERNEL);
+		if (!host->buffer) {
+			ret = -ENOMEM;
+			dev_err(&pdev->dev, "buffer allocation failed\n");
+			goto err_init_slot;
+		}
+	}
+
+	setup_timer(&host->timer, atmci_timeout_timer, (unsigned long)host);
+
 	dev_info(&pdev->dev,
 			"Atmel MCI controller at 0x%08lx irq %d, %u slots\n",
 			host->mapbase, irq, nr_slots);
@@ -2179,6 +2380,10 @@ static int __exit atmci_remove(struct platform_device *pdev)
 
 	platform_set_drvdata(pdev, NULL);
 
+	if (host->buffer)
+		dma_free_coherent(&pdev->dev, host->buf_size,
+		                  host->buffer, host->buf_phys_addr);
+
 	for (i = 0; i < ATMCI_MAX_NR_SLOTS; i++) {
 		if (host->slot[i])
 			atmci_cleanup_slot(host->slot[i], i);
diff --git a/drivers/mmc/host/davinci_mmc.c b/drivers/mmc/host/davinci_mmc.c
index c1f3673ae1e..7cf6c624bf7 100644
--- a/drivers/mmc/host/davinci_mmc.c
+++ b/drivers/mmc/host/davinci_mmc.c
@@ -1533,4 +1533,5 @@ module_exit(davinci_mmcsd_exit);
 MODULE_AUTHOR("Texas Instruments India");
 MODULE_LICENSE("GPL");
 MODULE_DESCRIPTION("MMC/SD driver for Davinci MMC controller");
+MODULE_ALIAS("platform:davinci_mmc");
 
diff --git a/drivers/mmc/host/dw_mmc.c b/drivers/mmc/host/dw_mmc.c
index ab3fc461710..9bbf45f8c53 100644
--- a/drivers/mmc/host/dw_mmc.c
+++ b/drivers/mmc/host/dw_mmc.c
@@ -100,8 +100,6 @@ struct dw_mci_slot {
 	int			last_detect_state;
 };
 
-static struct workqueue_struct *dw_mci_card_workqueue;
-
 #if defined(CONFIG_DEBUG_FS)
 static int dw_mci_req_show(struct seq_file *s, void *v)
 {
@@ -859,10 +857,10 @@ static void dw_mci_enable_sdio_irq(struct mmc_host *mmc, int enb)
 	int_mask = mci_readl(host, INTMASK);
 	if (enb) {
 		mci_writel(host, INTMASK,
-			   (int_mask | (1 << SDMMC_INT_SDIO(slot->id))));
+			   (int_mask | SDMMC_INT_SDIO(slot->id)));
 	} else {
 		mci_writel(host, INTMASK,
-			   (int_mask & ~(1 << SDMMC_INT_SDIO(slot->id))));
+			   (int_mask & ~SDMMC_INT_SDIO(slot->id)));
 	}
 }
 
@@ -1605,7 +1603,7 @@ static irqreturn_t dw_mci_interrupt(int irq, void *dev_id)
 
 		if (pending & SDMMC_INT_CD) {
 			mci_writel(host, RINTSTS, SDMMC_INT_CD);
-			queue_work(dw_mci_card_workqueue, &host->card_work);
+			queue_work(host->card_workqueue, &host->card_work);
 		}
 
 		/* Handle SDIO Interrupts */
@@ -1844,7 +1842,7 @@ static int __init dw_mci_init_slot(struct dw_mci *host, unsigned int id)
 	 * Card may have been plugged in prior to boot so we
 	 * need to run the detect tasklet
 	 */
-	queue_work(dw_mci_card_workqueue, &host->card_work);
+	queue_work(host->card_workqueue, &host->card_work);
 
 	return 0;
 }
@@ -2021,9 +2019,9 @@ int dw_mci_probe(struct dw_mci *host)
 	mci_writel(host, CLKSRC, 0);
 
 	tasklet_init(&host->tasklet, dw_mci_tasklet_func, (unsigned long)host);
-	dw_mci_card_workqueue = alloc_workqueue("dw-mci-card",
+	host->card_workqueue = alloc_workqueue("dw-mci-card",
 			WQ_MEM_RECLAIM | WQ_NON_REENTRANT, 1);
-	if (!dw_mci_card_workqueue)
+	if (!host->card_workqueue)
 		goto err_dmaunmap;
 	INIT_WORK(&host->card_work, dw_mci_work_routine_card);
 	ret = request_irq(host->irq, dw_mci_interrupt, host->irq_flags, "dw-mci", host);
@@ -2085,7 +2083,7 @@ err_init_slot:
 	free_irq(host->irq, host);
 
 err_workqueue:
-	destroy_workqueue(dw_mci_card_workqueue);
+	destroy_workqueue(host->card_workqueue);
 
 err_dmaunmap:
 	if (host->use_dma && host->dma_ops->exit)
@@ -2119,7 +2117,7 @@ void dw_mci_remove(struct dw_mci *host)
 	mci_writel(host, CLKSRC, 0);
 
 	free_irq(host->irq, host);
-	destroy_workqueue(dw_mci_card_workqueue);
+	destroy_workqueue(host->card_workqueue);
 	dma_free_coherent(&host->dev, PAGE_SIZE, host->sg_cpu, host->sg_dma);
 
 	if (host->use_dma && host->dma_ops->exit)
diff --git a/drivers/mmc/host/imxmmc.c b/drivers/mmc/host/imxmmc.c
deleted file mode 100644
index ea0f3cedef2..00000000000
--- a/drivers/mmc/host/imxmmc.c
+++ /dev/null
@@ -1,1169 +0,0 @@
-/*
- *  linux/drivers/mmc/host/imxmmc.c - Motorola i.MX MMCI driver
- *
- *  Copyright (C) 2004 Sascha Hauer, Pengutronix <sascha@saschahauer.de>
- *  Copyright (C) 2006 Pavel Pisa, PiKRON <ppisa@pikron.com>
- *
- *  derived from pxamci.c by 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.
- *
- */
-
-#include <linux/module.h>
-#include <linux/init.h>
-#include <linux/ioport.h>
-#include <linux/platform_device.h>
-#include <linux/interrupt.h>
-#include <linux/blkdev.h>
-#include <linux/dma-mapping.h>
-#include <linux/mmc/host.h>
-#include <linux/mmc/card.h>
-#include <linux/delay.h>
-#include <linux/clk.h>
-#include <linux/io.h>
-
-#include <asm/dma.h>
-#include <asm/irq.h>
-#include <asm/sizes.h>
-#include <mach/mmc.h>
-#include <mach/imx-dma.h>
-
-#include "imxmmc.h"
-
-#define DRIVER_NAME "imx-mmc"
-
-#define IMXMCI_INT_MASK_DEFAULT (INT_MASK_BUF_READY | INT_MASK_DATA_TRAN | \
-				 INT_MASK_WRITE_OP_DONE | INT_MASK_END_CMD_RES | \
-				 INT_MASK_AUTO_CARD_DETECT | INT_MASK_DAT0_EN | INT_MASK_SDIO)
-
-struct imxmci_host {
-	struct mmc_host		*mmc;
-	spinlock_t		lock;
-	struct resource		*res;
-	void __iomem		*base;
-	int			irq;
-	imx_dmach_t		dma;
-	volatile unsigned int	imask;
-	unsigned int		power_mode;
-	unsigned int		present;
-	struct imxmmc_platform_data *pdata;
-
-	struct mmc_request	*req;
-	struct mmc_command	*cmd;
-	struct mmc_data		*data;
-
-	struct timer_list	timer;
-	struct tasklet_struct	tasklet;
-	unsigned int		status_reg;
-	unsigned long		pending_events;
-	/* Next two fields are there for CPU driven transfers to overcome SDHC deficiencies */
-	u16			*data_ptr;
-	unsigned int		data_cnt;
-	atomic_t		stuck_timeout;
-
-	unsigned int		dma_nents;
-	unsigned int		dma_size;
-	unsigned int		dma_dir;
-	int			dma_allocated;
-
-	unsigned char		actual_bus_width;
-
-	int			prev_cmd_code;
-
-	struct clk		*clk;
-};
-
-#define IMXMCI_PEND_IRQ_b	0
-#define IMXMCI_PEND_DMA_END_b	1
-#define IMXMCI_PEND_DMA_ERR_b	2
-#define IMXMCI_PEND_WAIT_RESP_b	3
-#define IMXMCI_PEND_DMA_DATA_b	4
-#define IMXMCI_PEND_CPU_DATA_b	5
-#define IMXMCI_PEND_CARD_XCHG_b	6
-#define IMXMCI_PEND_SET_INIT_b	7
-#define IMXMCI_PEND_STARTED_b	8
-
-#define IMXMCI_PEND_IRQ_m	(1 << IMXMCI_PEND_IRQ_b)
-#define IMXMCI_PEND_DMA_END_m	(1 << IMXMCI_PEND_DMA_END_b)
-#define IMXMCI_PEND_DMA_ERR_m	(1 << IMXMCI_PEND_DMA_ERR_b)
-#define IMXMCI_PEND_WAIT_RESP_m	(1 << IMXMCI_PEND_WAIT_RESP_b)
-#define IMXMCI_PEND_DMA_DATA_m	(1 << IMXMCI_PEND_DMA_DATA_b)
-#define IMXMCI_PEND_CPU_DATA_m	(1 << IMXMCI_PEND_CPU_DATA_b)
-#define IMXMCI_PEND_CARD_XCHG_m	(1 << IMXMCI_PEND_CARD_XCHG_b)
-#define IMXMCI_PEND_SET_INIT_m	(1 << IMXMCI_PEND_SET_INIT_b)
-#define IMXMCI_PEND_STARTED_m	(1 << IMXMCI_PEND_STARTED_b)
-
-static void imxmci_stop_clock(struct imxmci_host *host)
-{
-	int i = 0;
-	u16 reg;
-
-	reg = readw(host->base + MMC_REG_STR_STP_CLK);
-	writew(reg & ~STR_STP_CLK_START_CLK, host->base + MMC_REG_STR_STP_CLK);
-	while (i < 0x1000) {
-		if (!(i & 0x7f)) {
-			reg = readw(host->base + MMC_REG_STR_STP_CLK);
-			writew(reg | STR_STP_CLK_STOP_CLK,
-					host->base + MMC_REG_STR_STP_CLK);
-		}
-
-		reg = readw(host->base + MMC_REG_STATUS);
-		if (!(reg & STATUS_CARD_BUS_CLK_RUN)) {
-			/* Check twice before cut */
-			reg = readw(host->base + MMC_REG_STATUS);
-			if (!(reg & STATUS_CARD_BUS_CLK_RUN))
-				return;
-		}
-
-		i++;
-	}
-	dev_dbg(mmc_dev(host->mmc), "imxmci_stop_clock blocked, no luck\n");
-}
-
-static int imxmci_start_clock(struct imxmci_host *host)
-{
-	unsigned int trials = 0;
-	unsigned int delay_limit = 128;
-	unsigned long flags;
-	u16 reg;
-
-	reg = readw(host->base + MMC_REG_STR_STP_CLK);
-	writew(reg & ~STR_STP_CLK_STOP_CLK, host->base + MMC_REG_STR_STP_CLK);
-
-	clear_bit(IMXMCI_PEND_STARTED_b, &host->pending_events);
-
-	/*
-	 * Command start of the clock, this usually succeeds in less
-	 * then 6 delay loops, but during card detection (low clockrate)
-	 * it takes up to 5000 delay loops and sometimes fails for the first time
-	 */
-	reg = readw(host->base + MMC_REG_STR_STP_CLK);
-	writew(reg | STR_STP_CLK_START_CLK, host->base + MMC_REG_STR_STP_CLK);
-
-	do {
-		unsigned int delay = delay_limit;
-
-		while (delay--) {
-			reg = readw(host->base + MMC_REG_STATUS);
-			if (reg & STATUS_CARD_BUS_CLK_RUN) {
-				/* Check twice before cut */
-				reg = readw(host->base + MMC_REG_STATUS);
-				if (reg & STATUS_CARD_BUS_CLK_RUN)
-					return 0;
-			}
-
-			if (test_bit(IMXMCI_PEND_STARTED_b, &host->pending_events))
-				return 0;
-		}
-
-		local_irq_save(flags);
-		/*
-		 * Ensure, that request is not doubled under all possible circumstances.
-		 * It is possible, that cock running state is missed, because some other
-		 * IRQ or schedule delays this function execution and the clocks has
-		 * been already stopped by other means (response processing, SDHC HW)
-		 */
-		if (!test_bit(IMXMCI_PEND_STARTED_b, &host->pending_events)) {
-			reg = readw(host->base + MMC_REG_STR_STP_CLK);
-			writew(reg | STR_STP_CLK_START_CLK,
-					host->base + MMC_REG_STR_STP_CLK);
-		}
-		local_irq_restore(flags);
-
-	} while (++trials < 256);
-
-	dev_err(mmc_dev(host->mmc), "imxmci_start_clock blocked, no luck\n");
-
-	return -1;
-}
-
-static void imxmci_softreset(struct imxmci_host *host)
-{
-	int i;
-
-	/* reset sequence */
-	writew(0x08, host->base + MMC_REG_STR_STP_CLK);
-	writew(0x0D, host->base + MMC_REG_STR_STP_CLK);
-
-	for (i = 0; i < 8; i++)
-		writew(0x05, host->base + MMC_REG_STR_STP_CLK);
-
-	writew(0xff, host->base + MMC_REG_RES_TO);
-	writew(512, host->base + MMC_REG_BLK_LEN);
-	writew(1, host->base + MMC_REG_NOB);
-}
-
-static int imxmci_busy_wait_for_status(struct imxmci_host *host,
-				       unsigned int *pstat, unsigned int stat_mask,
-				       int timeout, const char *where)
-{
-	int loops = 0;
-
-	while (!(*pstat & stat_mask)) {
-		loops += 2;
-		if (loops >= timeout) {
-			dev_dbg(mmc_dev(host->mmc), "busy wait timeout in %s, STATUS = 0x%x (0x%x)\n",
-				where, *pstat, stat_mask);
-			return -1;
-		}
-		udelay(2);
-		*pstat |= readw(host->base + MMC_REG_STATUS);
-	}
-	if (!loops)
-		return 0;
-
-	/* The busy-wait is expected there for clock <8MHz due to SDHC hardware flaws */
-	if (!(stat_mask & STATUS_END_CMD_RESP) || (host->mmc->ios.clock >= 8000000))
-		dev_info(mmc_dev(host->mmc), "busy wait for %d usec in %s, STATUS = 0x%x (0x%x)\n",
-			 loops, where, *pstat, stat_mask);
-	return loops;
-}
-
-static void imxmci_setup_data(struct imxmci_host *host, struct mmc_data *data)
-{
-	unsigned int nob = data->blocks;
-	unsigned int blksz = data->blksz;
-	unsigned int datasz = nob * blksz;
-	int i;
-
-	if (data->flags & MMC_DATA_STREAM)
-		nob = 0xffff;
-
-	host->data = data;
-	data->bytes_xfered = 0;
-
-	writew(nob, host->base + MMC_REG_NOB);
-	writew(blksz, host->base + MMC_REG_BLK_LEN);
-
-	/*
-	 * DMA cannot be used for small block sizes, we have to use CPU driven transfers otherwise.
-	 * We are in big troubles for non-512 byte transfers according to note in the paragraph
-	 * 20.6.7 of User Manual anyway, but we need to be able to transfer SCR at least.
-	 * The situation is even more complex in reality. The SDHC in not able to handle wll
-	 * partial FIFO fills and reads. The length has to be rounded up to burst size multiple.
-	 * This is required for SCR read at least.
-	 */
-	if (datasz < 512) {
-		host->dma_size = datasz;
-		if (data->flags & MMC_DATA_READ) {
-			host->dma_dir = DMA_FROM_DEVICE;
-
-			/* Hack to enable read SCR */
-			writew(1, host->base + MMC_REG_NOB);
-			writew(512, host->base + MMC_REG_BLK_LEN);
-		} else {
-			host->dma_dir = DMA_TO_DEVICE;
-		}
-
-		/* Convert back to virtual address */
-		host->data_ptr = (u16 *)sg_virt(data->sg);
-		host->data_cnt = 0;
-
-		clear_bit(IMXMCI_PEND_DMA_DATA_b, &host->pending_events);
-		set_bit(IMXMCI_PEND_CPU_DATA_b, &host->pending_events);
-
-		return;
-	}
-
-	if (data->flags & MMC_DATA_READ) {
-		host->dma_dir = DMA_FROM_DEVICE;
-		host->dma_nents = dma_map_sg(mmc_dev(host->mmc), data->sg,
-					     data->sg_len,  host->dma_dir);
-
-		imx_dma_setup_sg(host->dma, data->sg, data->sg_len, datasz,
-				 host->res->start + MMC_REG_BUFFER_ACCESS,
-				 DMA_MODE_READ);
-
-		/*imx_dma_setup_mem2dev_ccr(host->dma, DMA_MODE_READ, IMX_DMA_WIDTH_16, CCR_REN);*/
-		CCR(host->dma) = CCR_DMOD_LINEAR | CCR_DSIZ_32 | CCR_SMOD_FIFO | CCR_SSIZ_16 | CCR_REN;
-	} else {
-		host->dma_dir = DMA_TO_DEVICE;
-
-		host->dma_nents = dma_map_sg(mmc_dev(host->mmc), data->sg,
-					     data->sg_len,  host->dma_dir);
-
-		imx_dma_setup_sg(host->dma, data->sg, data->sg_len, datasz,
-				 host->res->start + MMC_REG_BUFFER_ACCESS,
-				 DMA_MODE_WRITE);
-
-		/*imx_dma_setup_mem2dev_ccr(host->dma, DMA_MODE_WRITE, IMX_DMA_WIDTH_16, CCR_REN);*/
-		CCR(host->dma) = CCR_SMOD_LINEAR | CCR_SSIZ_32 | CCR_DMOD_FIFO | CCR_DSIZ_16 | CCR_REN;
-	}
-
-#if 1	/* This code is there only for consistency checking and can be disabled in future */
-	host->dma_size = 0;
-	for (i = 0; i < host->dma_nents; i++)
-		host->dma_size += data->sg[i].length;
-
-	if (datasz > host->dma_size) {
-		dev_err(mmc_dev(host->mmc), "imxmci_setup_data datasz 0x%x > 0x%x dm_size\n",
-			datasz, host->dma_size);
-	}
-#endif
-
-	host->dma_size = datasz;
-
-	wmb();
-
-	set_bit(IMXMCI_PEND_DMA_DATA_b, &host->pending_events);
-	clear_bit(IMXMCI_PEND_CPU_DATA_b, &host->pending_events);
-
-	/* start DMA engine for read, write is delayed after initial response */
-	if (host->dma_dir == DMA_FROM_DEVICE)
-		imx_dma_enable(host->dma);
-}
-
-static void imxmci_start_cmd(struct imxmci_host *host, struct mmc_command *cmd, unsigned int cmdat)
-{
-	unsigned long flags;
-	u32 imask;
-
-	WARN_ON(host->cmd != NULL);
-	host->cmd = cmd;
-
-	/* Ensure, that clock are stopped else command programming and start fails */
-	imxmci_stop_clock(host);
-
-	if (cmd->flags & MMC_RSP_BUSY)
-		cmdat |= CMD_DAT_CONT_BUSY;
-
-	switch (mmc_resp_type(cmd)) {
-	case MMC_RSP_R1: /* short CRC, OPCODE */
-	case MMC_RSP_R1B:/* short CRC, OPCODE, BUSY */
-		cmdat |= CMD_DAT_CONT_RESPONSE_FORMAT_R1;
-		break;
-	case MMC_RSP_R2: /* long 136 bit + CRC */
-		cmdat |= CMD_DAT_CONT_RESPONSE_FORMAT_R2;
-		break;
-	case MMC_RSP_R3: /* short */
-		cmdat |= CMD_DAT_CONT_RESPONSE_FORMAT_R3;
-		break;
-	default:
-		break;
-	}
-
-	if (test_and_clear_bit(IMXMCI_PEND_SET_INIT_b, &host->pending_events))
-		cmdat |= CMD_DAT_CONT_INIT; /* This command needs init */
-
-	if (host->actual_bus_width == MMC_BUS_WIDTH_4)
-		cmdat |= CMD_DAT_CONT_BUS_WIDTH_4;
-
-	writew(cmd->opcode, host->base + MMC_REG_CMD);
-	writew(cmd->arg >> 16, host->base + MMC_REG_ARGH);
-	writew(cmd->arg & 0xffff, host->base + MMC_REG_ARGL);
-	writew(cmdat, host->base + MMC_REG_CMD_DAT_CONT);
-
-	atomic_set(&host->stuck_timeout, 0);
-	set_bit(IMXMCI_PEND_WAIT_RESP_b, &host->pending_events);
-
-
-	imask = IMXMCI_INT_MASK_DEFAULT;
-	imask &= ~INT_MASK_END_CMD_RES;
-	if (cmdat & CMD_DAT_CONT_DATA_ENABLE) {
-		/* imask &= ~INT_MASK_BUF_READY; */
-		imask &= ~INT_MASK_DATA_TRAN;
-		if (cmdat & CMD_DAT_CONT_WRITE)
-			imask &= ~INT_MASK_WRITE_OP_DONE;
-		if (test_bit(IMXMCI_PEND_CPU_DATA_b, &host->pending_events))
-			imask &= ~INT_MASK_BUF_READY;
-	}
-
-	spin_lock_irqsave(&host->lock, flags);
-	host->imask = imask;
-	writew(host->imask, host->base + MMC_REG_INT_MASK);
-	spin_unlock_irqrestore(&host->lock, flags);
-
-	dev_dbg(mmc_dev(host->mmc), "CMD%02d (0x%02x) mask set to 0x%04x\n",
-		cmd->opcode, cmd->opcode, imask);
-
-	imxmci_start_clock(host);
-}
-
-static void imxmci_finish_request(struct imxmci_host *host, struct mmc_request *req)
-{
-	unsigned long flags;
-
-	spin_lock_irqsave(&host->lock, flags);
-
-	host->pending_events &= ~(IMXMCI_PEND_WAIT_RESP_m | IMXMCI_PEND_DMA_END_m |
-				  IMXMCI_PEND_DMA_DATA_m | IMXMCI_PEND_CPU_DATA_m);
-
-	host->imask = IMXMCI_INT_MASK_DEFAULT;
-	writew(host->imask, host->base + MMC_REG_INT_MASK);
-
-	spin_unlock_irqrestore(&host->lock, flags);
-
-	if (req && req->cmd)
-		host->prev_cmd_code = req->cmd->opcode;
-
-	host->req = NULL;
-	host->cmd = NULL;
-	host->data = NULL;
-	mmc_request_done(host->mmc, req);
-}
-
-static int imxmci_finish_data(struct imxmci_host *host, unsigned int stat)
-{
-	struct mmc_data *data = host->data;
-	int data_error;
-
-	if (test_and_clear_bit(IMXMCI_PEND_DMA_DATA_b, &host->pending_events)) {
-		imx_dma_disable(host->dma);
-		dma_unmap_sg(mmc_dev(host->mmc), data->sg, host->dma_nents,
-			     host->dma_dir);
-	}
-
-	if (stat & STATUS_ERR_MASK) {
-		dev_dbg(mmc_dev(host->mmc), "request failed. status: 0x%08x\n", stat);
-		if (stat & (STATUS_CRC_READ_ERR | STATUS_CRC_WRITE_ERR))
-			data->error = -EILSEQ;
-		else if (stat & STATUS_TIME_OUT_READ)
-			data->error = -ETIMEDOUT;
-		else
-			data->error = -EIO;
-	} else {
-		data->bytes_xfered = host->dma_size;
-	}
-
-	data_error = data->error;
-
-	host->data = NULL;
-
-	return data_error;
-}
-
-static int imxmci_cmd_done(struct imxmci_host *host, unsigned int stat)
-{
-	struct mmc_command *cmd = host->cmd;
-	int i;
-	u32 a, b, c;
-	struct mmc_data *data = host->data;
-
-	if (!cmd)
-		return 0;
-
-	host->cmd = NULL;
-
-	if (stat & STATUS_TIME_OUT_RESP) {
-		dev_dbg(mmc_dev(host->mmc), "CMD TIMEOUT\n");
-		cmd->error = -ETIMEDOUT;
-	} else if (stat & STATUS_RESP_CRC_ERR && cmd->flags & MMC_RSP_CRC) {
-		dev_dbg(mmc_dev(host->mmc), "cmd crc error\n");
-		cmd->error = -EILSEQ;
-	}
-
-	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);
-				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);
-			cmd->resp[0] = a << 24 | b << 8 | c >> 8;
-		}
-	}
-
-	dev_dbg(mmc_dev(host->mmc), "RESP 0x%08x, 0x%08x, 0x%08x, 0x%08x, error %d\n",
-		cmd->resp[0], cmd->resp[1], cmd->resp[2], cmd->resp[3], cmd->error);
-
-	if (data && !cmd->error && !(stat & STATUS_ERR_MASK)) {
-		if (host->req->data->flags & MMC_DATA_WRITE) {
-
-			/* Wait for FIFO to be empty before starting DMA write */
-
-			stat = readw(host->base + MMC_REG_STATUS);
-			if (imxmci_busy_wait_for_status(host, &stat,
-							STATUS_APPL_BUFF_FE,
-							40, "imxmci_cmd_done DMA WR") < 0) {
-				cmd->error = -EIO;
-				imxmci_finish_data(host, stat);
-				if (host->req)
-					imxmci_finish_request(host, host->req);
-				dev_warn(mmc_dev(host->mmc), "STATUS = 0x%04x\n",
-					 stat);
-				return 0;
-			}
-
-			if (test_bit(IMXMCI_PEND_DMA_DATA_b, &host->pending_events))
-				imx_dma_enable(host->dma);
-		}
-	} else {
-		struct mmc_request *req;
-		imxmci_stop_clock(host);
-		req = host->req;
-
-		if (data)
-			imxmci_finish_data(host, stat);
-
-		if (req)
-			imxmci_finish_request(host, req);
-		else
-			dev_warn(mmc_dev(host->mmc), "imxmci_cmd_done: no request to finish\n");
-	}
-
-	return 1;
-}
-
-static int imxmci_data_done(struct imxmci_host *host, unsigned int stat)
-{
-	struct mmc_data *data = host->data;
-	int data_error;
-
-	if (!data)
-		return 0;
-
-	data_error = imxmci_finish_data(host, stat);
-
-	if (host->req->stop) {
-		imxmci_stop_clock(host);
-		imxmci_start_cmd(host, host->req->stop, 0);
-	} else {
-		struct mmc_request *req;
-		req = host->req;
-		if (req)
-			imxmci_finish_request(host, req);
-		else
-			dev_warn(mmc_dev(host->mmc), "imxmci_data_done: no request to finish\n");
-	}
-
-	return 1;
-}
-
-static int imxmci_cpu_driven_data(struct imxmci_host *host, unsigned int *pstat)
-{
-	int i;
-	int burst_len;
-	int trans_done = 0;
-	unsigned int stat = *pstat;
-
-	if (host->actual_bus_width != MMC_BUS_WIDTH_4)
-		burst_len = 16;
-	else
-		burst_len = 64;
-
-	/* This is unfortunately required */
-	dev_dbg(mmc_dev(host->mmc), "imxmci_cpu_driven_data running STATUS = 0x%x\n",
-		stat);
-
-	udelay(20);	/* required for clocks < 8MHz*/
-
-	if (host->dma_dir == DMA_FROM_DEVICE) {
-		imxmci_busy_wait_for_status(host, &stat,
-					    STATUS_APPL_BUFF_FF | STATUS_DATA_TRANS_DONE |
-					    STATUS_TIME_OUT_READ,
-					    50, "imxmci_cpu_driven_data read");
-
-		while ((stat & (STATUS_APPL_BUFF_FF | STATUS_DATA_TRANS_DONE)) &&
-		       !(stat & STATUS_TIME_OUT_READ) &&
-		       (host->data_cnt < 512)) {
-
-			udelay(20);	/* required for clocks < 8MHz*/
-
-			for (i = burst_len; i >= 2 ; i -= 2) {
-				u16 data;
-				data = readw(host->base + MMC_REG_BUFFER_ACCESS);
-				udelay(10);	/* required for clocks < 8MHz*/
-				if (host->data_cnt+2 <= host->dma_size) {
-					*(host->data_ptr++) = data;
-				} else {
-					if (host->data_cnt < host->dma_size)
-						*(u8 *)(host->data_ptr) = data;
-				}
-				host->data_cnt += 2;
-			}
-
-			stat = readw(host->base + MMC_REG_STATUS);
-
-			dev_dbg(mmc_dev(host->mmc), "imxmci_cpu_driven_data read %d burst %d STATUS = 0x%x\n",
-				host->data_cnt, burst_len, stat);
-		}
-
-		if ((stat & STATUS_DATA_TRANS_DONE) && (host->data_cnt >= 512))
-			trans_done = 1;
-
-		if (host->dma_size & 0x1ff)
-			stat &= ~STATUS_CRC_READ_ERR;
-
-		if (stat & STATUS_TIME_OUT_READ) {
-			dev_dbg(mmc_dev(host->mmc), "imxmci_cpu_driven_data read timeout STATUS = 0x%x\n",
-				stat);
-			trans_done = -1;
-		}
-
-	} else {
-		imxmci_busy_wait_for_status(host, &stat,
-					    STATUS_APPL_BUFF_FE,
-					    20, "imxmci_cpu_driven_data write");
-
-		while ((stat & STATUS_APPL_BUFF_FE) &&
-		       (host->data_cnt < host->dma_size)) {
-			if (burst_len >= host->dma_size - host->data_cnt) {
-				burst_len = host->dma_size - host->data_cnt;
-				host->data_cnt = host->dma_size;
-				trans_done = 1;
-			} else {
-				host->data_cnt += burst_len;
-			}
-
-			for (i = burst_len; i > 0 ; i -= 2)
-				writew(*(host->data_ptr++), host->base + MMC_REG_BUFFER_ACCESS);
-
-			stat = readw(host->base + MMC_REG_STATUS);
-
-			dev_dbg(mmc_dev(host->mmc), "imxmci_cpu_driven_data write burst %d STATUS = 0x%x\n",
-				burst_len, stat);
-		}
-	}
-
-	*pstat = stat;
-
-	return trans_done;
-}
-
-static void imxmci_dma_irq(int dma, void *devid)
-{
-	struct imxmci_host *host = devid;
-	u32 stat = readw(host->base + MMC_REG_STATUS);
-
-	atomic_set(&host->stuck_timeout, 0);
-	host->status_reg = stat;
-	set_bit(IMXMCI_PEND_DMA_END_b, &host->pending_events);
-	tasklet_schedule(&host->tasklet);
-}
-
-static irqreturn_t imxmci_irq(int irq, void *devid)
-{
-	struct imxmci_host *host = devid;
-	u32 stat = readw(host->base + MMC_REG_STATUS);
-	int handled = 1;
-
-	writew(host->imask | INT_MASK_SDIO | INT_MASK_AUTO_CARD_DETECT,
-			host->base + MMC_REG_INT_MASK);
-
-	atomic_set(&host->stuck_timeout, 0);
-	host->status_reg = stat;
-	set_bit(IMXMCI_PEND_IRQ_b, &host->pending_events);
-	set_bit(IMXMCI_PEND_STARTED_b, &host->pending_events);
-	tasklet_schedule(&host->tasklet);
-
-	return IRQ_RETVAL(handled);
-}
-
-static void imxmci_tasklet_fnc(unsigned long data)
-{
-	struct imxmci_host *host = (struct imxmci_host *)data;
-	u32 stat;
-	unsigned int data_dir_mask = 0;	/* STATUS_WR_CRC_ERROR_CODE_MASK */
-	int timeout = 0;
-
-	if (atomic_read(&host->stuck_timeout) > 4) {
-		char *what;
-		timeout = 1;
-		stat = readw(host->base + MMC_REG_STATUS);
-		host->status_reg = stat;
-		if (test_bit(IMXMCI_PEND_WAIT_RESP_b, &host->pending_events))
-			if (test_bit(IMXMCI_PEND_DMA_DATA_b, &host->pending_events))
-				what = "RESP+DMA";
-			else
-				what = "RESP";
-		else
-			if (test_bit(IMXMCI_PEND_DMA_DATA_b, &host->pending_events))
-				if (test_bit(IMXMCI_PEND_DMA_END_b, &host->pending_events))
-					what = "DATA";
-				else
-					what = "DMA";
-			else
-				what = "???";
-
-		dev_err(mmc_dev(host->mmc),
-			"%s TIMEOUT, hardware stucked STATUS = 0x%04x IMASK = 0x%04x\n",
-			what, stat,
-			readw(host->base + MMC_REG_INT_MASK));
-		dev_err(mmc_dev(host->mmc),
-			"CMD_DAT_CONT = 0x%04x, MMC_BLK_LEN = 0x%04x, MMC_NOB = 0x%04x, DMA_CCR = 0x%08x\n",
-			readw(host->base + MMC_REG_CMD_DAT_CONT),
-			readw(host->base + MMC_REG_BLK_LEN),
-			readw(host->base + MMC_REG_NOB),
-			CCR(host->dma));
-		dev_err(mmc_dev(host->mmc), "CMD%d, prevCMD%d, bus %d-bit, dma_size = 0x%x\n",
-			host->cmd ? host->cmd->opcode : 0,
-			host->prev_cmd_code,
-			1 << host->actual_bus_width, host->dma_size);
-	}
-
-	if (!host->present || timeout)
-		host->status_reg = STATUS_TIME_OUT_RESP | STATUS_TIME_OUT_READ |
-			STATUS_CRC_READ_ERR | STATUS_CRC_WRITE_ERR;
-
-	if (test_bit(IMXMCI_PEND_IRQ_b, &host->pending_events) || timeout) {
-		clear_bit(IMXMCI_PEND_IRQ_b, &host->pending_events);
-
-		stat = readw(host->base + MMC_REG_STATUS);
-		/*
-		 * This is not required in theory, but there is chance to miss some flag
-		 * which clears automatically by mask write, FreeScale original code keeps
-		 * stat from IRQ time so do I
-		 */
-		stat |= host->status_reg;
-
-		if (test_bit(IMXMCI_PEND_CPU_DATA_b, &host->pending_events))
-			stat &= ~STATUS_CRC_READ_ERR;
-
-		if (test_bit(IMXMCI_PEND_WAIT_RESP_b, &host->pending_events)) {
-			imxmci_busy_wait_for_status(host, &stat,
-						    STATUS_END_CMD_RESP | STATUS_ERR_MASK,
-						    20, "imxmci_tasklet_fnc resp (ERRATUM #4)");
-		}
-
-		if (stat & (STATUS_END_CMD_RESP | STATUS_ERR_MASK)) {
-			if (test_and_clear_bit(IMXMCI_PEND_WAIT_RESP_b, &host->pending_events))
-				imxmci_cmd_done(host, stat);
-			if (host->data && (stat & STATUS_ERR_MASK))
-				imxmci_data_done(host, stat);
-		}
-
-		if (test_bit(IMXMCI_PEND_CPU_DATA_b, &host->pending_events)) {
-			stat |= readw(host->base + MMC_REG_STATUS);
-			if (imxmci_cpu_driven_data(host, &stat)) {
-				if (test_and_clear_bit(IMXMCI_PEND_WAIT_RESP_b, &host->pending_events))
-					imxmci_cmd_done(host, stat);
-				atomic_clear_mask(IMXMCI_PEND_IRQ_m|IMXMCI_PEND_CPU_DATA_m,
-						  &host->pending_events);
-				imxmci_data_done(host, stat);
-			}
-		}
-	}
-
-	if (test_bit(IMXMCI_PEND_DMA_END_b, &host->pending_events) &&
-	    !test_bit(IMXMCI_PEND_WAIT_RESP_b, &host->pending_events)) {
-
-		stat = readw(host->base + MMC_REG_STATUS);
-		/* Same as above */
-		stat |= host->status_reg;
-
-		if (host->dma_dir == DMA_TO_DEVICE)
-			data_dir_mask = STATUS_WRITE_OP_DONE;
-		else
-			data_dir_mask = STATUS_DATA_TRANS_DONE;
-
-		if (stat & data_dir_mask) {
-			clear_bit(IMXMCI_PEND_DMA_END_b, &host->pending_events);
-			imxmci_data_done(host, stat);
-		}
-	}
-
-	if (test_and_clear_bit(IMXMCI_PEND_CARD_XCHG_b, &host->pending_events)) {
-
-		if (host->cmd)
-			imxmci_cmd_done(host, STATUS_TIME_OUT_RESP);
-
-		if (host->data)
-			imxmci_data_done(host, STATUS_TIME_OUT_READ |
-					 STATUS_CRC_READ_ERR | STATUS_CRC_WRITE_ERR);
-
-		if (host->req)
-			imxmci_finish_request(host, host->req);
-
-		mmc_detect_change(host->mmc, msecs_to_jiffies(100));
-
-	}
-}
-
-static void imxmci_request(struct mmc_host *mmc, struct mmc_request *req)
-{
-	struct imxmci_host *host = mmc_priv(mmc);
-	unsigned int cmdat;
-
-	WARN_ON(host->req != NULL);
-
-	host->req = req;
-
-	cmdat = 0;
-
-	if (req->data) {
-		imxmci_setup_data(host, req->data);
-
-		cmdat |= CMD_DAT_CONT_DATA_ENABLE;
-
-		if (req->data->flags & MMC_DATA_WRITE)
-			cmdat |= CMD_DAT_CONT_WRITE;
-
-		if (req->data->flags & MMC_DATA_STREAM)
-			cmdat |= CMD_DAT_CONT_STREAM_BLOCK;
-	}
-
-	imxmci_start_cmd(host, req->cmd, cmdat);
-}
-
-#define CLK_RATE 19200000
-
-static void imxmci_set_ios(struct mmc_host *mmc, struct mmc_ios *ios)
-{
-	struct imxmci_host *host = mmc_priv(mmc);
-	int prescaler;
-
-	if (ios->bus_width == MMC_BUS_WIDTH_4) {
-		host->actual_bus_width = MMC_BUS_WIDTH_4;
-		imx_gpio_mode(PB11_PF_SD_DAT3);
-		BLR(host->dma) = 0;	/* burst 64 byte read/write */
-	} else {
-		host->actual_bus_width = MMC_BUS_WIDTH_1;
-		imx_gpio_mode(GPIO_PORTB | GPIO_IN | GPIO_PUEN | 11);
-		BLR(host->dma) = 16;	/* burst 16 byte read/write */
-	}
-
-	if (host->power_mode != ios->power_mode) {
-		switch (ios->power_mode) {
-		case MMC_POWER_OFF:
-			break;
-		case MMC_POWER_UP:
-			set_bit(IMXMCI_PEND_SET_INIT_b, &host->pending_events);
-			break;
-		case MMC_POWER_ON:
-			break;
-		}
-		host->power_mode = ios->power_mode;
-	}
-
-	if (ios->clock) {
-		unsigned int clk;
-		u16 reg;
-
-		/* The prescaler is 5 for PERCLK2 equal to 96MHz
-		 * then 96MHz / 5 = 19.2 MHz
-		 */
-		clk = clk_get_rate(host->clk);
-		prescaler = (clk + (CLK_RATE * 7) / 8) / CLK_RATE;
-		switch (prescaler) {
-		case 0:
-		case 1:	prescaler = 0;
-			break;
-		case 2:	prescaler = 1;
-			break;
-		case 3:	prescaler = 2;
-			break;
-		case 4:	prescaler = 4;
-			break;
-		default:
-		case 5:	prescaler = 5;
-			break;
-		}
-
-		dev_dbg(mmc_dev(host->mmc), "PERCLK2 %d MHz -> prescaler %d\n",
-			clk, prescaler);
-
-		for (clk = 0; clk < 8; clk++) {
-			int x;
-			x = CLK_RATE / (1 << clk);
-			if (x <= ios->clock)
-				break;
-		}
-
-		/* enable controller */
-		reg = readw(host->base + MMC_REG_STR_STP_CLK);
-		writew(reg | STR_STP_CLK_ENABLE,
-				host->base + MMC_REG_STR_STP_CLK);
-
-		imxmci_stop_clock(host);
-		writew((prescaler << 3) | clk, host->base + MMC_REG_CLK_RATE);
-		/*
-		 * Under my understanding, clock should not be started there, because it would
-		 * initiate SDHC sequencer and send last or random command into card
-		 */
-		/* imxmci_start_clock(host); */
-
-		dev_dbg(mmc_dev(host->mmc),
-			"MMC_CLK_RATE: 0x%08x\n",
-			readw(host->base + MMC_REG_CLK_RATE));
-	} else {
-		imxmci_stop_clock(host);
-	}
-}
-
-static int imxmci_get_ro(struct mmc_host *mmc)
-{
-	struct imxmci_host *host = mmc_priv(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.
-	 */
-	return -ENOSYS;
-}
-
-
-static const struct mmc_host_ops imxmci_ops = {
-	.request	= imxmci_request,
-	.set_ios	= imxmci_set_ios,
-	.get_ro		= imxmci_get_ro,
-};
-
-static void imxmci_check_status(unsigned long data)
-{
-	struct imxmci_host *host = (struct imxmci_host *)data;
-
-	if (host->pdata && host->pdata->card_present &&
-	    host->pdata->card_present(mmc_dev(host->mmc)) != host->present) {
-		host->present ^= 1;
-		dev_info(mmc_dev(host->mmc), "card %s\n",
-		      host->present ? "inserted" : "removed");
-
-		set_bit(IMXMCI_PEND_CARD_XCHG_b, &host->pending_events);
-		tasklet_schedule(&host->tasklet);
-	}
-
-	if (test_bit(IMXMCI_PEND_WAIT_RESP_b, &host->pending_events) ||
-	    test_bit(IMXMCI_PEND_DMA_DATA_b, &host->pending_events)) {
-		atomic_inc(&host->stuck_timeout);
-		if (atomic_read(&host->stuck_timeout) > 4)
-			tasklet_schedule(&host->tasklet);
-	} else {
-		atomic_set(&host->stuck_timeout, 0);
-
-	}
-
-	mod_timer(&host->timer, jiffies + (HZ>>1));
-}
-
-static int __init imxmci_probe(struct platform_device *pdev)
-{
-	struct mmc_host *mmc;
-	struct imxmci_host *host = NULL;
-	struct resource *r;
-	int ret = 0, irq;
-	u16 rev_no;
-
-	pr_info("i.MX mmc driver\n");
-
-	r = platform_get_resource(pdev, IORESOURCE_MEM, 0);
-	irq = platform_get_irq(pdev, 0);
-	if (!r || irq < 0)
-		return -ENXIO;
-
-	r = request_mem_region(r->start, resource_size(r), pdev->name);
-	if (!r)
-		return -EBUSY;
-
-	mmc = mmc_alloc_host(sizeof(struct imxmci_host), &pdev->dev);
-	if (!mmc) {
-		ret = -ENOMEM;
-		goto out;
-	}
-
-	mmc->ops = &imxmci_ops;
-	mmc->f_min = 150000;
-	mmc->f_max = CLK_RATE/2;
-	mmc->ocr_avail = MMC_VDD_32_33;
-	mmc->caps = MMC_CAP_4_BIT_DATA;
-
-	/* MMC core transfer sizes tunable parameters */
-	mmc->max_segs = 64;
-	mmc->max_seg_size = 64*512;	/* default PAGE_CACHE_SIZE */
-	mmc->max_req_size = 64*512;	/* default PAGE_CACHE_SIZE */
-	mmc->max_blk_size = 2048;
-	mmc->max_blk_count = 65535;
-
-	host = mmc_priv(mmc);
-	host->base = ioremap(r->start, resource_size(r));
-	if (!host->base) {
-		ret = -ENOMEM;
-		goto out;
-	}
-
-	host->mmc = mmc;
-	host->dma_allocated = 0;
-	host->pdata = pdev->dev.platform_data;
-	if (!host->pdata)
-		dev_warn(&pdev->dev, "No platform data provided!\n");
-
-	spin_lock_init(&host->lock);
-	host->res = r;
-	host->irq = irq;
-
-	host->clk = clk_get(&pdev->dev, "perclk2");
-	if (IS_ERR(host->clk)) {
-		ret = PTR_ERR(host->clk);
-		goto out;
-	}
-	clk_enable(host->clk);
-
-	imx_gpio_mode(PB8_PF_SD_DAT0);
-	imx_gpio_mode(PB9_PF_SD_DAT1);
-	imx_gpio_mode(PB10_PF_SD_DAT2);
-	/* Configured as GPIO with pull-up to ensure right MCC card mode */
-	/* Switched to PB11_PF_SD_DAT3 if 4 bit bus is configured */
-	imx_gpio_mode(GPIO_PORTB | GPIO_IN | GPIO_PUEN | 11);
-	/* imx_gpio_mode(PB11_PF_SD_DAT3); */
-	imx_gpio_mode(PB12_PF_SD_CLK);
-	imx_gpio_mode(PB13_PF_SD_CMD);
-
-	imxmci_softreset(host);
-
-	rev_no = readw(host->base + MMC_REG_REV_NO);
-	if (rev_no != 0x390) {
-		dev_err(mmc_dev(host->mmc), "wrong rev.no. 0x%08x. aborting.\n",
-			readw(host->base + MMC_REG_REV_NO));
-		goto out;
-	}
-
-	/* recommended in data sheet */
-	writew(0x2db4, host->base + MMC_REG_READ_TO);
-
-	host->imask = IMXMCI_INT_MASK_DEFAULT;
-	writew(host->imask, host->base + MMC_REG_INT_MASK);
-
-	host->dma = imx_dma_request_by_prio(DRIVER_NAME, DMA_PRIO_LOW);
-	if(host->dma < 0) {
-		dev_err(mmc_dev(host->mmc), "imx_dma_request_by_prio failed\n");
-		ret = -EBUSY;
-		goto out;
-	}
-	host->dma_allocated = 1;
-	imx_dma_setup_handlers(host->dma, imxmci_dma_irq, NULL, host);
-	RSSR(host->dma) = DMA_REQ_SDHC;
-
-	tasklet_init(&host->tasklet, imxmci_tasklet_fnc, (unsigned long)host);
-	host->status_reg=0;
-	host->pending_events=0;
-
-	ret = request_irq(host->irq, imxmci_irq, 0, DRIVER_NAME, host);
-	if (ret)
-		goto out;
-
-	if (host->pdata && host->pdata->card_present)
-		host->present = host->pdata->card_present(mmc_dev(mmc));
-	else	/* if there is no way to detect assume that card is present */
-		host->present = 1;
-
-	init_timer(&host->timer);
-	host->timer.data = (unsigned long)host;
-	host->timer.function = imxmci_check_status;
-	add_timer(&host->timer);
-	mod_timer(&host->timer, jiffies + (HZ >> 1));
-
-	platform_set_drvdata(pdev, mmc);
-
-	mmc_add_host(mmc);
-
-	return 0;
-
-out:
-	if (host) {
-		if (host->dma_allocated) {
-			imx_dma_free(host->dma);
-			host->dma_allocated = 0;
-		}
-		if (host->clk) {
-			clk_disable(host->clk);
-			clk_put(host->clk);
-		}
-		if (host->base)
-			iounmap(host->base);
-	}
-	if (mmc)
-		mmc_free_host(mmc);
-	release_mem_region(r->start, resource_size(r));
-	return ret;
-}
-
-static int __exit imxmci_remove(struct platform_device *pdev)
-{
-	struct mmc_host *mmc = platform_get_drvdata(pdev);
-
-	platform_set_drvdata(pdev, NULL);
-
-	if (mmc) {
-		struct imxmci_host *host = mmc_priv(mmc);
-
-		tasklet_disable(&host->tasklet);
-
-		del_timer_sync(&host->timer);
-		mmc_remove_host(mmc);
-
-		free_irq(host->irq, host);
-		iounmap(host->base);
-		if (host->dma_allocated) {
-			imx_dma_free(host->dma);
-			host->dma_allocated = 0;
-		}
-
-		tasklet_kill(&host->tasklet);
-
-		clk_disable(host->clk);
-		clk_put(host->clk);
-
-		release_mem_region(host->res->start, resource_size(host->res));
-
-		mmc_free_host(mmc);
-	}
-	return 0;
-}
-
-#ifdef CONFIG_PM
-static int imxmci_suspend(struct platform_device *dev, pm_message_t state)
-{
-	struct mmc_host *mmc = platform_get_drvdata(dev);
-	int ret = 0;
-
-	if (mmc)
-		ret = mmc_suspend_host(mmc);
-
-	return ret;
-}
-
-static int imxmci_resume(struct platform_device *dev)
-{
-	struct mmc_host *mmc = platform_get_drvdata(dev);
-	struct imxmci_host *host;
-	int ret = 0;
-
-	if (mmc) {
-		host = mmc_priv(mmc);
-		if (host)
-			set_bit(IMXMCI_PEND_SET_INIT_b, &host->pending_events);
-		ret = mmc_resume_host(mmc);
-	}
-
-	return ret;
-}
-#else
-#define imxmci_suspend  NULL
-#define imxmci_resume   NULL
-#endif /* CONFIG_PM */
-
-static struct platform_driver imxmci_driver = {
-	.remove		= __exit_p(imxmci_remove),
-	.suspend	= imxmci_suspend,
-	.resume		= imxmci_resume,
-	.driver		= {
-		.name		= DRIVER_NAME,
-		.owner		= THIS_MODULE,
-	}
-};
-
-static int __init imxmci_init(void)
-{
-	return platform_driver_probe(&imxmci_driver, imxmci_probe);
-}
-
-static void __exit imxmci_exit(void)
-{
-	platform_driver_unregister(&imxmci_driver);
-}
-
-module_init(imxmci_init);
-module_exit(imxmci_exit);
-
-MODULE_DESCRIPTION("i.MX Multimedia Card Interface Driver");
-MODULE_AUTHOR("Sascha Hauer, Pengutronix");
-MODULE_LICENSE("GPL");
-MODULE_ALIAS("platform:imx-mmc");
diff --git a/drivers/mmc/host/imxmmc.h b/drivers/mmc/host/imxmmc.h
deleted file mode 100644
index 09d5d4ee3a7..00000000000
--- a/drivers/mmc/host/imxmmc.h
+++ /dev/null
@@ -1,64 +0,0 @@
-#define MMC_REG_STR_STP_CLK		0x00
-#define MMC_REG_STATUS			0x04
-#define MMC_REG_CLK_RATE		0x08
-#define MMC_REG_CMD_DAT_CONT		0x0C
-#define MMC_REG_RES_TO			0x10
-#define MMC_REG_READ_TO			0x14
-#define MMC_REG_BLK_LEN			0x18
-#define MMC_REG_NOB			0x1C
-#define MMC_REG_REV_NO			0x20
-#define MMC_REG_INT_MASK		0x24
-#define MMC_REG_CMD			0x28
-#define MMC_REG_ARGH			0x2C
-#define MMC_REG_ARGL			0x30
-#define MMC_REG_RES_FIFO		0x34
-#define MMC_REG_BUFFER_ACCESS		0x38
-
-#define STR_STP_CLK_IPG_CLK_GATE_DIS    (1<<15)
-#define STR_STP_CLK_IPG_PERCLK_GATE_DIS (1<<14)
-#define STR_STP_CLK_ENDIAN              (1<<5)
-#define STR_STP_CLK_RESET               (1<<3)
-#define STR_STP_CLK_ENABLE              (1<<2)
-#define STR_STP_CLK_START_CLK           (1<<1)
-#define STR_STP_CLK_STOP_CLK            (1<<0)
-#define STATUS_CARD_PRESENCE            (1<<15)
-#define STATUS_SDIO_INT_ACTIVE          (1<<14)
-#define STATUS_END_CMD_RESP             (1<<13)
-#define STATUS_WRITE_OP_DONE            (1<<12)
-#define STATUS_DATA_TRANS_DONE          (1<<11)
-#define STATUS_WR_CRC_ERROR_CODE_MASK   (3<<10)
-#define STATUS_CARD_BUS_CLK_RUN         (1<<8)
-#define STATUS_APPL_BUFF_FF             (1<<7)
-#define STATUS_APPL_BUFF_FE             (1<<6)
-#define STATUS_RESP_CRC_ERR             (1<<5)
-#define STATUS_CRC_READ_ERR             (1<<3)
-#define STATUS_CRC_WRITE_ERR            (1<<2)
-#define STATUS_TIME_OUT_RESP            (1<<1)
-#define STATUS_TIME_OUT_READ            (1<<0)
-#define STATUS_ERR_MASK                 0x2f
-#define CLK_RATE_PRESCALER(x)           ((x) & 0x7)
-#define CLK_RATE_CLK_RATE(x)            (((x) & 0x7) << 3)
-#define CMD_DAT_CONT_CMD_RESP_LONG_OFF  (1<<12)
-#define CMD_DAT_CONT_STOP_READWAIT      (1<<11)
-#define CMD_DAT_CONT_START_READWAIT     (1<<10)
-#define CMD_DAT_CONT_BUS_WIDTH_1        (0<<8)
-#define CMD_DAT_CONT_BUS_WIDTH_4        (2<<8)
-#define CMD_DAT_CONT_INIT               (1<<7)
-#define CMD_DAT_CONT_BUSY               (1<<6)
-#define CMD_DAT_CONT_STREAM_BLOCK       (1<<5)
-#define CMD_DAT_CONT_WRITE              (1<<4)
-#define CMD_DAT_CONT_DATA_ENABLE        (1<<3)
-#define CMD_DAT_CONT_RESPONSE_FORMAT_R1 (1)
-#define CMD_DAT_CONT_RESPONSE_FORMAT_R2 (2)
-#define CMD_DAT_CONT_RESPONSE_FORMAT_R3 (3)
-#define CMD_DAT_CONT_RESPONSE_FORMAT_R4 (4)
-#define CMD_DAT_CONT_RESPONSE_FORMAT_R5 (5)
-#define CMD_DAT_CONT_RESPONSE_FORMAT_R6 (6)
-#define INT_MASK_AUTO_CARD_DETECT       (1<<6)
-#define INT_MASK_DAT0_EN                (1<<5)
-#define INT_MASK_SDIO                   (1<<4)
-#define INT_MASK_BUF_READY              (1<<3)
-#define INT_MASK_END_CMD_RES            (1<<2)
-#define INT_MASK_WRITE_OP_DONE          (1<<1)
-#define INT_MASK_DATA_TRAN              (1<<0)
-#define INT_ALL                         (0x7f)
diff --git a/drivers/mmc/host/mmci.c b/drivers/mmc/host/mmci.c
index 032b84791a1..f0fcce40cd8 100644
--- a/drivers/mmc/host/mmci.c
+++ b/drivers/mmc/host/mmci.c
@@ -15,6 +15,7 @@
 #include <linux/device.h>
 #include <linux/interrupt.h>
 #include <linux/kernel.h>
+#include <linux/slab.h>
 #include <linux/delay.h>
 #include <linux/err.h>
 #include <linux/highmem.h>
@@ -25,6 +26,7 @@
 #include <linux/clk.h>
 #include <linux/scatterlist.h>
 #include <linux/gpio.h>
+#include <linux/of_gpio.h>
 #include <linux/regulator/consumer.h>
 #include <linux/dmaengine.h>
 #include <linux/dma-mapping.h>
@@ -94,6 +96,17 @@ static struct variant_data variant_u300 = {
 	.signal_direction	= true,
 };
 
+static struct variant_data variant_nomadik = {
+	.fifosize		= 16 * 4,
+	.fifohalfsize		= 8 * 4,
+	.clkreg			= MCI_CLK_ENABLE,
+	.datalength_bits	= 24,
+	.sdio			= true,
+	.st_clkdiv		= true,
+	.pwrreg_powerup		= MCI_PWR_ON,
+	.signal_direction	= true,
+};
+
 static struct variant_data variant_ux500 = {
 	.fifosize		= 30 * 4,
 	.fifohalfsize		= 8 * 4,
@@ -1196,21 +1209,76 @@ static const struct mmc_host_ops mmci_ops = {
 	.get_cd		= mmci_get_cd,
 };
 
+#ifdef CONFIG_OF
+static void mmci_dt_populate_generic_pdata(struct device_node *np,
+					struct mmci_platform_data *pdata)
+{
+	int bus_width = 0;
+
+	pdata->gpio_wp = of_get_named_gpio(np, "wp-gpios", 0);
+	if (!pdata->gpio_wp)
+		pdata->gpio_wp = -1;
+
+	pdata->gpio_cd = of_get_named_gpio(np, "cd-gpios", 0);
+	if (!pdata->gpio_cd)
+		pdata->gpio_cd = -1;
+
+	if (of_get_property(np, "cd-inverted", NULL))
+		pdata->cd_invert = true;
+	else
+		pdata->cd_invert = false;
+
+	of_property_read_u32(np, "max-frequency", &pdata->f_max);
+	if (!pdata->f_max)
+		pr_warn("%s has no 'max-frequency' property\n", np->full_name);
+
+	if (of_get_property(np, "mmc-cap-mmc-highspeed", NULL))
+		pdata->capabilities |= MMC_CAP_MMC_HIGHSPEED;
+	if (of_get_property(np, "mmc-cap-sd-highspeed", NULL))
+		pdata->capabilities |= MMC_CAP_SD_HIGHSPEED;
+
+	of_property_read_u32(np, "bus-width", &bus_width);
+	switch (bus_width) {
+	case 0 :
+		/* No bus-width supplied. */
+		break;
+	case 4 :
+		pdata->capabilities |= MMC_CAP_4_BIT_DATA;
+		break;
+	case 8 :
+		pdata->capabilities |= MMC_CAP_8_BIT_DATA;
+		break;
+	default :
+		pr_warn("%s: Unsupported bus width\n", np->full_name);
+	}
+}
+#else
+static void mmci_dt_populate_generic_pdata(struct device_node *np,
+					struct mmci_platform_data *pdata)
+{
+	return;
+}
+#endif
+
 static int __devinit mmci_probe(struct amba_device *dev,
 	const struct amba_id *id)
 {
 	struct mmci_platform_data *plat = dev->dev.platform_data;
+	struct device_node *np = dev->dev.of_node;
 	struct variant_data *variant = id->data;
 	struct mmci_host *host;
 	struct mmc_host *mmc;
 	int ret;
 
-	/* must have platform data */
-	if (!plat) {
-		ret = -EINVAL;
-		goto out;
+	/* Must have platform data or Device Tree. */
+	if (!plat && !np) {
+		dev_err(&dev->dev, "No plat data or DT found\n");
+		return -EINVAL;
 	}
 
+	if (np)
+		mmci_dt_populate_generic_pdata(np, plat);
+
 	ret = amba_request_regions(dev, DRIVER_NAME);
 	if (ret)
 		goto out;
@@ -1397,7 +1465,7 @@ static int __devinit mmci_probe(struct amba_device *dev,
 	if (ret)
 		goto unmap;
 
-	if (dev->irq[1] == NO_IRQ || !dev->irq[1])
+	if (!dev->irq[1])
 		host->singleirq = true;
 	else {
 		ret = request_irq(dev->irq[1], mmci_pio_irq, IRQF_SHARED,
@@ -1569,6 +1637,11 @@ static struct amba_id mmci_ids[] = {
 		.data	= &variant_u300,
 	},
 	{
+		.id     = 0x10180180,
+		.mask   = 0xf0ffffff,
+		.data	= &variant_nomadik,
+	},
+	{
 		.id     = 0x00280180,
 		.mask   = 0x00ffffff,
 		.data	= &variant_u300,
diff --git a/drivers/mmc/host/mvsdio.c b/drivers/mmc/host/mvsdio.c
index eeb8cd125b0..3b9136c1a47 100644
--- a/drivers/mmc/host/mvsdio.c
+++ b/drivers/mmc/host/mvsdio.c
@@ -19,6 +19,7 @@
 #include <linux/dma-mapping.h>
 #include <linux/scatterlist.h>
 #include <linux/irq.h>
+#include <linux/clk.h>
 #include <linux/gpio.h>
 #include <linux/mmc/host.h>
 
@@ -51,6 +52,7 @@ struct mvsd_host {
 	struct device *dev;
 	struct resource *res;
 	int irq;
+	struct clk *clk;
 	int gpio_card_detect;
 	int gpio_write_protect;
 };
@@ -770,6 +772,13 @@ static int __init mvsd_probe(struct platform_device *pdev)
 	} else
 		host->irq = irq;
 
+	/* Not all platforms can gate the clock, so it is not
+	   an error if the clock does not exists. */
+	host->clk = clk_get(&pdev->dev, NULL);
+	if (!IS_ERR(host->clk)) {
+		clk_prepare_enable(host->clk);
+	}
+
 	if (mvsd_data->gpio_card_detect) {
 		ret = gpio_request(mvsd_data->gpio_card_detect,
 				   DRIVER_NAME " cd");
@@ -854,6 +863,11 @@ static int __exit mvsd_remove(struct platform_device *pdev)
 		mvsd_power_down(host);
 		iounmap(host->base);
 		release_resource(host->res);
+
+		if (!IS_ERR(host->clk)) {
+			clk_disable_unprepare(host->clk);
+			clk_put(host->clk);
+		}
 		mmc_free_host(mmc);
 	}
 	platform_set_drvdata(pdev, NULL);
diff --git a/drivers/mmc/host/mxcmmc.c b/drivers/mmc/host/mxcmmc.c
index b2058b43232..28ed52d58f7 100644
--- a/drivers/mmc/host/mxcmmc.c
+++ b/drivers/mmc/host/mxcmmc.c
@@ -136,7 +136,8 @@ struct mxcmci_host {
 	u16			rev_no;
 	unsigned int		cmdat;
 
-	struct clk		*clk;
+	struct clk		*clk_ipg;
+	struct clk		*clk_per;
 
 	int			clock;
 
@@ -672,7 +673,7 @@ static void mxcmci_set_clk_rate(struct mxcmci_host *host, unsigned int clk_ios)
 {
 	unsigned int divider;
 	int prescaler = 0;
-	unsigned int clk_in = clk_get_rate(host->clk);
+	unsigned int clk_in = clk_get_rate(host->clk_per);
 
 	while (prescaler <= 0x800) {
 		for (divider = 1; divider <= 0xF; divider++) {
@@ -900,12 +901,20 @@ static int mxcmci_probe(struct platform_device *pdev)
 	host->res = r;
 	host->irq = irq;
 
-	host->clk = clk_get(&pdev->dev, NULL);
-	if (IS_ERR(host->clk)) {
-		ret = PTR_ERR(host->clk);
+	host->clk_ipg = devm_clk_get(&pdev->dev, "ipg");
+	if (IS_ERR(host->clk_ipg)) {
+		ret = PTR_ERR(host->clk_ipg);
 		goto out_iounmap;
 	}
-	clk_enable(host->clk);
+
+	host->clk_per = devm_clk_get(&pdev->dev, "per");
+	if (IS_ERR(host->clk_per)) {
+		ret = PTR_ERR(host->clk_per);
+		goto out_iounmap;
+	}
+
+	clk_prepare_enable(host->clk_per);
+	clk_prepare_enable(host->clk_ipg);
 
 	mxcmci_softreset(host);
 
@@ -917,8 +926,8 @@ static int mxcmci_probe(struct platform_device *pdev)
 		goto out_clk_put;
 	}
 
-	mmc->f_min = clk_get_rate(host->clk) >> 16;
-	mmc->f_max = clk_get_rate(host->clk) >> 1;
+	mmc->f_min = clk_get_rate(host->clk_per) >> 16;
+	mmc->f_max = clk_get_rate(host->clk_per) >> 1;
 
 	/* recommended in data sheet */
 	writew(0x2db4, host->base + MMC_REG_READ_TO);
@@ -967,8 +976,8 @@ out_free_dma:
 	if (host->dma)
 		dma_release_channel(host->dma);
 out_clk_put:
-	clk_disable(host->clk);
-	clk_put(host->clk);
+	clk_disable_unprepare(host->clk_per);
+	clk_disable_unprepare(host->clk_ipg);
 out_iounmap:
 	iounmap(host->base);
 out_free:
@@ -999,8 +1008,8 @@ static int mxcmci_remove(struct platform_device *pdev)
 	if (host->dma)
 		dma_release_channel(host->dma);
 
-	clk_disable(host->clk);
-	clk_put(host->clk);
+	clk_disable_unprepare(host->clk_per);
+	clk_disable_unprepare(host->clk_ipg);
 
 	release_mem_region(host->res->start, resource_size(host->res));
 
@@ -1018,7 +1027,8 @@ static int mxcmci_suspend(struct device *dev)
 
 	if (mmc)
 		ret = mmc_suspend_host(mmc);
-	clk_disable(host->clk);
+	clk_disable_unprepare(host->clk_per);
+	clk_disable_unprepare(host->clk_ipg);
 
 	return ret;
 }
@@ -1029,7 +1039,8 @@ static int mxcmci_resume(struct device *dev)
 	struct mxcmci_host *host = mmc_priv(mmc);
 	int ret = 0;
 
-	clk_enable(host->clk);
+	clk_prepare_enable(host->clk_per);
+	clk_prepare_enable(host->clk_ipg);
 	if (mmc)
 		ret = mmc_resume_host(mmc);
 
diff --git a/drivers/mmc/host/mxs-mmc.c b/drivers/mmc/host/mxs-mmc.c
index e3f5af96ab8..34a90266ab1 100644
--- a/drivers/mmc/host/mxs-mmc.c
+++ b/drivers/mmc/host/mxs-mmc.c
@@ -23,6 +23,9 @@
 #include <linux/kernel.h>
 #include <linux/init.h>
 #include <linux/ioport.h>
+#include <linux/of.h>
+#include <linux/of_device.h>
+#include <linux/of_gpio.h>
 #include <linux/platform_device.h>
 #include <linux/delay.h>
 #include <linux/interrupt.h>
@@ -39,18 +42,16 @@
 #include <linux/regulator/consumer.h>
 #include <linux/module.h>
 #include <linux/fsl/mxs-dma.h>
-
-#include <mach/mxs.h>
-#include <mach/common.h>
-#include <mach/mmc.h>
+#include <linux/pinctrl/consumer.h>
+#include <linux/stmp_device.h>
+#include <linux/mmc/mxs-mmc.h>
 
 #define DRIVER_NAME	"mxs-mmc"
 
 /* card detect polling timeout */
 #define MXS_MMC_DETECT_TIMEOUT			(HZ/2)
 
-#define SSP_VERSION_LATEST	4
-#define ssp_is_old()		(host->version < SSP_VERSION_LATEST)
+#define ssp_is_old(host)	((host)->devid == IMX23_MMC)
 
 /* SSP registers */
 #define HW_SSP_CTRL0				0x000
@@ -85,14 +86,14 @@
 #define  BM_SSP_BLOCK_SIZE_BLOCK_COUNT		(0xffffff << 4)
 #define  BP_SSP_BLOCK_SIZE_BLOCK_SIZE		(0)
 #define  BM_SSP_BLOCK_SIZE_BLOCK_SIZE		(0xf)
-#define HW_SSP_TIMING				(ssp_is_old() ? 0x050 : 0x070)
+#define HW_SSP_TIMING(h)			(ssp_is_old(h) ? 0x050 : 0x070)
 #define  BP_SSP_TIMING_TIMEOUT			(16)
 #define  BM_SSP_TIMING_TIMEOUT			(0xffff << 16)
 #define  BP_SSP_TIMING_CLOCK_DIVIDE		(8)
 #define  BM_SSP_TIMING_CLOCK_DIVIDE		(0xff << 8)
 #define  BP_SSP_TIMING_CLOCK_RATE		(0)
 #define  BM_SSP_TIMING_CLOCK_RATE		(0xff)
-#define HW_SSP_CTRL1				(ssp_is_old() ? 0x060 : 0x080)
+#define HW_SSP_CTRL1(h)				(ssp_is_old(h) ? 0x060 : 0x080)
 #define  BM_SSP_CTRL1_SDIO_IRQ			(1 << 31)
 #define  BM_SSP_CTRL1_SDIO_IRQ_EN		(1 << 30)
 #define  BM_SSP_CTRL1_RESP_ERR_IRQ		(1 << 29)
@@ -115,15 +116,13 @@
 #define  BM_SSP_CTRL1_WORD_LENGTH		(0xf << 4)
 #define  BP_SSP_CTRL1_SSP_MODE			(0)
 #define  BM_SSP_CTRL1_SSP_MODE			(0xf)
-#define HW_SSP_SDRESP0				(ssp_is_old() ? 0x080 : 0x0a0)
-#define HW_SSP_SDRESP1				(ssp_is_old() ? 0x090 : 0x0b0)
-#define HW_SSP_SDRESP2				(ssp_is_old() ? 0x0a0 : 0x0c0)
-#define HW_SSP_SDRESP3				(ssp_is_old() ? 0x0b0 : 0x0d0)
-#define HW_SSP_STATUS				(ssp_is_old() ? 0x0c0 : 0x100)
+#define HW_SSP_SDRESP0(h)			(ssp_is_old(h) ? 0x080 : 0x0a0)
+#define HW_SSP_SDRESP1(h)			(ssp_is_old(h) ? 0x090 : 0x0b0)
+#define HW_SSP_SDRESP2(h)			(ssp_is_old(h) ? 0x0a0 : 0x0c0)
+#define HW_SSP_SDRESP3(h)			(ssp_is_old(h) ? 0x0b0 : 0x0d0)
+#define HW_SSP_STATUS(h)			(ssp_is_old(h) ? 0x0c0 : 0x100)
 #define  BM_SSP_STATUS_CARD_DETECT		(1 << 28)
 #define  BM_SSP_STATUS_SDIO_IRQ			(1 << 17)
-#define HW_SSP_VERSION				(cpu_is_mx23() ? 0x110 : 0x130)
-#define  BP_SSP_VERSION_MAJOR			(24)
 
 #define BF_SSP(value, field)	(((value) << BP_SSP_##field) & BM_SSP_##field)
 
@@ -138,6 +137,11 @@
 
 #define SSP_PIO_NUM	3
 
+enum mxs_mmc_id {
+	IMX23_MMC,
+	IMX28_MMC,
+};
+
 struct mxs_mmc_host {
 	struct mmc_host			*mmc;
 	struct mmc_request		*mrq;
@@ -145,9 +149,7 @@ struct mxs_mmc_host {
 	struct mmc_data			*data;
 
 	void __iomem			*base;
-	int				irq;
-	struct resource			*res;
-	struct resource			*dma_res;
+	int				dma_channel;
 	struct clk			*clk;
 	unsigned int			clk_rate;
 
@@ -157,32 +159,28 @@ struct mxs_mmc_host {
 	enum dma_transfer_direction	slave_dirn;
 	u32				ssp_pio_words[SSP_PIO_NUM];
 
-	unsigned int			version;
+	enum mxs_mmc_id			devid;
 	unsigned char			bus_width;
 	spinlock_t			lock;
 	int				sdio_irq_en;
+	int				wp_gpio;
 };
 
 static int mxs_mmc_get_ro(struct mmc_host *mmc)
 {
 	struct mxs_mmc_host *host = mmc_priv(mmc);
-	struct mxs_mmc_platform_data *pdata =
-		mmc_dev(host->mmc)->platform_data;
-
-	if (!pdata)
-		return -EFAULT;
 
-	if (!gpio_is_valid(pdata->wp_gpio))
+	if (!gpio_is_valid(host->wp_gpio))
 		return -EINVAL;
 
-	return gpio_get_value(pdata->wp_gpio);
+	return gpio_get_value(host->wp_gpio);
 }
 
 static int mxs_mmc_get_cd(struct mmc_host *mmc)
 {
 	struct mxs_mmc_host *host = mmc_priv(mmc);
 
-	return !(readl(host->base + HW_SSP_STATUS) &
+	return !(readl(host->base + HW_SSP_STATUS(host)) &
 		 BM_SSP_STATUS_CARD_DETECT);
 }
 
@@ -190,7 +188,7 @@ static void mxs_mmc_reset(struct mxs_mmc_host *host)
 {
 	u32 ctrl0, ctrl1;
 
-	mxs_reset_block(host->base);
+	stmp_reset_block(host->base);
 
 	ctrl0 = BM_SSP_CTRL0_IGNORE_CRC;
 	ctrl1 = BF_SSP(0x3, CTRL1_SSP_MODE) |
@@ -206,7 +204,7 @@ static void mxs_mmc_reset(struct mxs_mmc_host *host)
 	writel(BF_SSP(0xffff, TIMING_TIMEOUT) |
 	       BF_SSP(2, TIMING_CLOCK_DIVIDE) |
 	       BF_SSP(0, TIMING_CLOCK_RATE),
-	       host->base + HW_SSP_TIMING);
+	       host->base + HW_SSP_TIMING(host));
 
 	if (host->sdio_irq_en) {
 		ctrl0 |= BM_SSP_CTRL0_SDIO_IRQ_CHECK;
@@ -214,7 +212,7 @@ static void mxs_mmc_reset(struct mxs_mmc_host *host)
 	}
 
 	writel(ctrl0, host->base + HW_SSP_CTRL0);
-	writel(ctrl1, host->base + HW_SSP_CTRL1);
+	writel(ctrl1, host->base + HW_SSP_CTRL1(host));
 }
 
 static void mxs_mmc_start_cmd(struct mxs_mmc_host *host,
@@ -228,12 +226,12 @@ static void mxs_mmc_request_done(struct mxs_mmc_host *host)
 
 	if (mmc_resp_type(cmd) & MMC_RSP_PRESENT) {
 		if (mmc_resp_type(cmd) & MMC_RSP_136) {
-			cmd->resp[3] = readl(host->base + HW_SSP_SDRESP0);
-			cmd->resp[2] = readl(host->base + HW_SSP_SDRESP1);
-			cmd->resp[1] = readl(host->base + HW_SSP_SDRESP2);
-			cmd->resp[0] = readl(host->base + HW_SSP_SDRESP3);
+			cmd->resp[3] = readl(host->base + HW_SSP_SDRESP0(host));
+			cmd->resp[2] = readl(host->base + HW_SSP_SDRESP1(host));
+			cmd->resp[1] = readl(host->base + HW_SSP_SDRESP2(host));
+			cmd->resp[0] = readl(host->base + HW_SSP_SDRESP3(host));
 		} else {
-			cmd->resp[0] = readl(host->base + HW_SSP_SDRESP0);
+			cmd->resp[0] = readl(host->base + HW_SSP_SDRESP0(host));
 		}
 	}
 
@@ -276,9 +274,9 @@ static irqreturn_t mxs_mmc_irq_handler(int irq, void *dev_id)
 
 	spin_lock(&host->lock);
 
-	stat = readl(host->base + HW_SSP_CTRL1);
+	stat = readl(host->base + HW_SSP_CTRL1(host));
 	writel(stat & MXS_MMC_IRQ_BITS,
-	       host->base + HW_SSP_CTRL1 + MXS_CLR_ADDR);
+	       host->base + HW_SSP_CTRL1(host) + STMP_OFFSET_REG_CLR);
 
 	if ((stat & BM_SSP_CTRL1_SDIO_IRQ) && (stat & BM_SSP_CTRL1_SDIO_IRQ_EN))
 		mmc_signal_sdio_irq(host->mmc);
@@ -484,7 +482,7 @@ static void mxs_mmc_adtc(struct mxs_mmc_host *host)
 		blocks = 1;
 
 	/* xfer count, block size and count need to be set differently */
-	if (ssp_is_old()) {
+	if (ssp_is_old(host)) {
 		ctrl0 |= BF_SSP(data_size, CTRL0_XFER_COUNT);
 		cmd0 |= BF_SSP(log2_blksz, CMD0_BLOCK_SIZE) |
 			BF_SSP(blocks - 1, CMD0_BLOCK_COUNT);
@@ -508,10 +506,10 @@ static void mxs_mmc_adtc(struct mxs_mmc_host *host)
 
 	/* set the timeout count */
 	timeout = mxs_ns_to_ssp_ticks(host->clk_rate, data->timeout_ns);
-	val = readl(host->base + HW_SSP_TIMING);
+	val = readl(host->base + HW_SSP_TIMING(host));
 	val &= ~(BM_SSP_TIMING_TIMEOUT);
 	val |= BF_SSP(timeout, TIMING_TIMEOUT);
-	writel(val, host->base + HW_SSP_TIMING);
+	writel(val, host->base + HW_SSP_TIMING(host));
 
 	/* pio */
 	host->ssp_pio_words[0] = ctrl0;
@@ -597,11 +595,11 @@ static void mxs_mmc_set_clk_rate(struct mxs_mmc_host *host, unsigned int rate)
 
 	ssp_sck = ssp_clk / clock_divide / (1 + clock_rate);
 
-	val = readl(host->base + HW_SSP_TIMING);
+	val = readl(host->base + HW_SSP_TIMING(host));
 	val &= ~(BM_SSP_TIMING_CLOCK_DIVIDE | BM_SSP_TIMING_CLOCK_RATE);
 	val |= BF_SSP(clock_divide, TIMING_CLOCK_DIVIDE);
 	val |= BF_SSP(clock_rate, TIMING_CLOCK_RATE);
-	writel(val, host->base + HW_SSP_TIMING);
+	writel(val, host->base + HW_SSP_TIMING(host));
 
 	host->clk_rate = ssp_sck;
 
@@ -636,18 +634,19 @@ static void mxs_mmc_enable_sdio_irq(struct mmc_host *mmc, int enable)
 
 	if (enable) {
 		writel(BM_SSP_CTRL0_SDIO_IRQ_CHECK,
-		       host->base + HW_SSP_CTRL0 + MXS_SET_ADDR);
+		       host->base + HW_SSP_CTRL0 + STMP_OFFSET_REG_SET);
 		writel(BM_SSP_CTRL1_SDIO_IRQ_EN,
-		       host->base + HW_SSP_CTRL1 + MXS_SET_ADDR);
+		       host->base + HW_SSP_CTRL1(host) + STMP_OFFSET_REG_SET);
 
-		if (readl(host->base + HW_SSP_STATUS) & BM_SSP_STATUS_SDIO_IRQ)
+		if (readl(host->base + HW_SSP_STATUS(host)) &
+				BM_SSP_STATUS_SDIO_IRQ)
 			mmc_signal_sdio_irq(host->mmc);
 
 	} else {
 		writel(BM_SSP_CTRL0_SDIO_IRQ_CHECK,
-		       host->base + HW_SSP_CTRL0 + MXS_CLR_ADDR);
+		       host->base + HW_SSP_CTRL0 + STMP_OFFSET_REG_CLR);
 		writel(BM_SSP_CTRL1_SDIO_IRQ_EN,
-		       host->base + HW_SSP_CTRL1 + MXS_CLR_ADDR);
+		       host->base + HW_SSP_CTRL1(host) + STMP_OFFSET_REG_CLR);
 	}
 
 	spin_unlock_irqrestore(&host->lock, flags);
@@ -668,7 +667,7 @@ static bool mxs_mmc_dma_filter(struct dma_chan *chan, void *param)
 	if (!mxs_dma_is_apbh(chan))
 		return false;
 
-	if (chan->chan_id != host->dma_res->start)
+	if (chan->chan_id != host->dma_channel)
 		return false;
 
 	chan->private = &host->dma_data;
@@ -676,12 +675,36 @@ static bool mxs_mmc_dma_filter(struct dma_chan *chan, void *param)
 	return true;
 }
 
+static struct platform_device_id mxs_mmc_ids[] = {
+	{
+		.name = "imx23-mmc",
+		.driver_data = IMX23_MMC,
+	}, {
+		.name = "imx28-mmc",
+		.driver_data = IMX28_MMC,
+	}, {
+		/* sentinel */
+	}
+};
+MODULE_DEVICE_TABLE(platform, mxs_mmc_ids);
+
+static const struct of_device_id mxs_mmc_dt_ids[] = {
+	{ .compatible = "fsl,imx23-mmc", .data = (void *) IMX23_MMC, },
+	{ .compatible = "fsl,imx28-mmc", .data = (void *) IMX28_MMC, },
+	{ /* sentinel */ }
+};
+MODULE_DEVICE_TABLE(of, mxs_mmc_dt_ids);
+
 static int mxs_mmc_probe(struct platform_device *pdev)
 {
+	const struct of_device_id *of_id =
+			of_match_device(mxs_mmc_dt_ids, &pdev->dev);
+	struct device_node *np = pdev->dev.of_node;
 	struct mxs_mmc_host *host;
 	struct mmc_host *mmc;
-	struct resource *iores, *dmares, *r;
+	struct resource *iores, *dmares;
 	struct mxs_mmc_platform_data *pdata;
+	struct pinctrl *pinctrl;
 	int ret = 0, irq_err, irq_dma;
 	dma_cap_mask_t mask;
 
@@ -689,40 +712,51 @@ static int mxs_mmc_probe(struct platform_device *pdev)
 	dmares = platform_get_resource(pdev, IORESOURCE_DMA, 0);
 	irq_err = platform_get_irq(pdev, 0);
 	irq_dma = platform_get_irq(pdev, 1);
-	if (!iores || !dmares || irq_err < 0 || irq_dma < 0)
+	if (!iores || irq_err < 0 || irq_dma < 0)
 		return -EINVAL;
 
-	r = request_mem_region(iores->start, resource_size(iores), pdev->name);
-	if (!r)
-		return -EBUSY;
-
 	mmc = mmc_alloc_host(sizeof(struct mxs_mmc_host), &pdev->dev);
-	if (!mmc) {
-		ret = -ENOMEM;
-		goto out_release_mem;
-	}
+	if (!mmc)
+		return -ENOMEM;
 
 	host = mmc_priv(mmc);
-	host->base = ioremap(r->start, resource_size(r));
+	host->base = devm_request_and_ioremap(&pdev->dev, iores);
 	if (!host->base) {
-		ret = -ENOMEM;
+		ret = -EADDRNOTAVAIL;
 		goto out_mmc_free;
 	}
 
-	/* only major verion does matter */
-	host->version = readl(host->base + HW_SSP_VERSION) >>
-			BP_SSP_VERSION_MAJOR;
+	if (np) {
+		host->devid = (enum mxs_mmc_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",
+					   &host->dma_channel);
+		if (ret) {
+			dev_err(mmc_dev(host->mmc),
+				"failed to get dma channel\n");
+			goto out_mmc_free;
+		}
+	} else {
+		host->devid = pdev->id_entry->driver_data;
+		host->dma_channel = dmares->start;
+	}
 
 	host->mmc = mmc;
-	host->res = r;
-	host->dma_res = dmares;
-	host->irq = irq_err;
 	host->sdio_irq_en = 0;
 
+	pinctrl = devm_pinctrl_get_select_default(&pdev->dev);
+	if (IS_ERR(pinctrl)) {
+		ret = PTR_ERR(pinctrl);
+		goto out_mmc_free;
+	}
+
 	host->clk = clk_get(&pdev->dev, NULL);
 	if (IS_ERR(host->clk)) {
 		ret = PTR_ERR(host->clk);
-		goto out_iounmap;
+		goto out_mmc_free;
 	}
 	clk_prepare_enable(host->clk);
 
@@ -744,11 +778,20 @@ static int mxs_mmc_probe(struct platform_device *pdev)
 		    MMC_CAP_SDIO_IRQ | MMC_CAP_NEEDS_POLL;
 
 	pdata =	mmc_dev(host->mmc)->platform_data;
-	if (pdata) {
+	if (!pdata) {
+		u32 bus_width = 0;
+		of_property_read_u32(np, "bus-width", &bus_width);
+		if (bus_width == 4)
+			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->wp_gpio = of_get_named_gpio(np, "wp-gpios", 0);
+	} else {
 		if (pdata->flags & SLOTF_8_BIT_CAPABLE)
 			mmc->caps |= MMC_CAP_4_BIT_DATA | MMC_CAP_8_BIT_DATA;
 		if (pdata->flags & SLOTF_4_BIT_CAPABLE)
 			mmc->caps |= MMC_CAP_4_BIT_DATA;
+		host->wp_gpio = pdata->wp_gpio;
 	}
 
 	mmc->f_min = 400000;
@@ -757,13 +800,14 @@ static int mxs_mmc_probe(struct platform_device *pdev)
 
 	mmc->max_segs = 52;
 	mmc->max_blk_size = 1 << 0xf;
-	mmc->max_blk_count = (ssp_is_old()) ? 0xff : 0xffffff;
-	mmc->max_req_size = (ssp_is_old()) ? 0xffff : 0xffffffff;
+	mmc->max_blk_count = (ssp_is_old(host)) ? 0xff : 0xffffff;
+	mmc->max_req_size = (ssp_is_old(host)) ? 0xffff : 0xffffffff;
 	mmc->max_seg_size = dma_get_max_seg_size(host->dmach->device->dev);
 
 	platform_set_drvdata(pdev, mmc);
 
-	ret = request_irq(host->irq, mxs_mmc_irq_handler, 0, DRIVER_NAME, host);
+	ret = devm_request_irq(&pdev->dev, irq_err, mxs_mmc_irq_handler, 0,
+			       DRIVER_NAME, host);
 	if (ret)
 		goto out_free_dma;
 
@@ -771,26 +815,20 @@ static int mxs_mmc_probe(struct platform_device *pdev)
 
 	ret = mmc_add_host(mmc);
 	if (ret)
-		goto out_free_irq;
+		goto out_free_dma;
 
 	dev_info(mmc_dev(host->mmc), "initialized\n");
 
 	return 0;
 
-out_free_irq:
-	free_irq(host->irq, host);
 out_free_dma:
 	if (host->dmach)
 		dma_release_channel(host->dmach);
 out_clk_put:
 	clk_disable_unprepare(host->clk);
 	clk_put(host->clk);
-out_iounmap:
-	iounmap(host->base);
 out_mmc_free:
 	mmc_free_host(mmc);
-out_release_mem:
-	release_mem_region(iores->start, resource_size(iores));
 	return ret;
 }
 
@@ -798,12 +836,9 @@ static int mxs_mmc_remove(struct platform_device *pdev)
 {
 	struct mmc_host *mmc = platform_get_drvdata(pdev);
 	struct mxs_mmc_host *host = mmc_priv(mmc);
-	struct resource *res = host->res;
 
 	mmc_remove_host(mmc);
 
-	free_irq(host->irq, host);
-
 	platform_set_drvdata(pdev, NULL);
 
 	if (host->dmach)
@@ -812,12 +847,8 @@ static int mxs_mmc_remove(struct platform_device *pdev)
 	clk_disable_unprepare(host->clk);
 	clk_put(host->clk);
 
-	iounmap(host->base);
-
 	mmc_free_host(mmc);
 
-	release_mem_region(res->start, resource_size(res));
-
 	return 0;
 }
 
@@ -857,11 +888,13 @@ static const struct dev_pm_ops mxs_mmc_pm_ops = {
 static struct platform_driver mxs_mmc_driver = {
 	.probe		= mxs_mmc_probe,
 	.remove		= mxs_mmc_remove,
+	.id_table	= mxs_mmc_ids,
 	.driver		= {
 		.name	= DRIVER_NAME,
 		.owner	= THIS_MODULE,
 #ifdef CONFIG_PM
 		.pm	= &mxs_mmc_pm_ops,
+		.of_match_table = mxs_mmc_dt_ids,
 #endif
 	},
 };
diff --git a/drivers/mmc/host/omap.c b/drivers/mmc/host/omap.c
index 887c0e598cf..552196c764d 100644
--- a/drivers/mmc/host/omap.c
+++ b/drivers/mmc/host/omap.c
@@ -169,11 +169,11 @@ struct mmc_omap_host {
 	struct timer_list       clk_timer;
 	spinlock_t		clk_lock;     /* for changing enabled state */
 	unsigned int            fclk_enabled:1;
+	struct workqueue_struct *mmc_omap_wq;
 
 	struct omap_mmc_platform_data *pdata;
 };
 
-static struct workqueue_struct *mmc_omap_wq;
 
 static void mmc_omap_fclk_offdelay(struct mmc_omap_slot *slot)
 {
@@ -291,7 +291,7 @@ static void mmc_omap_release_slot(struct mmc_omap_slot *slot, int clk_enabled)
 		host->next_slot = new_slot;
 		host->mmc = new_slot->mmc;
 		spin_unlock_irqrestore(&host->slot_lock, flags);
-		queue_work(mmc_omap_wq, &host->slot_release_work);
+		queue_work(host->mmc_omap_wq, &host->slot_release_work);
 		return;
 	}
 
@@ -459,7 +459,7 @@ mmc_omap_xfer_done(struct mmc_omap_host *host, struct mmc_data *data)
 	}
 
 	host->stop_data = data;
-	queue_work(mmc_omap_wq, &host->send_stop_work);
+	queue_work(host->mmc_omap_wq, &host->send_stop_work);
 }
 
 static void
@@ -639,7 +639,7 @@ mmc_omap_cmd_timer(unsigned long data)
 		OMAP_MMC_WRITE(host, IE, 0);
 		disable_irq(host->irq);
 		host->abort = 1;
-		queue_work(mmc_omap_wq, &host->cmd_abort_work);
+		queue_work(host->mmc_omap_wq, &host->cmd_abort_work);
 	}
 	spin_unlock_irqrestore(&host->slot_lock, flags);
 }
@@ -828,7 +828,7 @@ static irqreturn_t mmc_omap_irq(int irq, void *dev_id)
 		host->abort = 1;
 		OMAP_MMC_WRITE(host, IE, 0);
 		disable_irq_nosync(host->irq);
-		queue_work(mmc_omap_wq, &host->cmd_abort_work);
+		queue_work(host->mmc_omap_wq, &host->cmd_abort_work);
 		return IRQ_HANDLED;
 	}
 
@@ -1389,13 +1389,13 @@ static void mmc_omap_remove_slot(struct mmc_omap_slot *slot)
 
 	tasklet_kill(&slot->cover_tasklet);
 	del_timer_sync(&slot->cover_timer);
-	flush_workqueue(mmc_omap_wq);
+	flush_workqueue(slot->host->mmc_omap_wq);
 
 	mmc_remove_host(mmc);
 	mmc_free_host(mmc);
 }
 
-static int __init mmc_omap_probe(struct platform_device *pdev)
+static int __devinit mmc_omap_probe(struct platform_device *pdev)
 {
 	struct omap_mmc_platform_data *pdata = pdev->dev.platform_data;
 	struct mmc_omap_host *host = NULL;
@@ -1497,6 +1497,10 @@ static int __init mmc_omap_probe(struct platform_device *pdev)
 
 	host->reg_shift = (cpu_is_omap7xx() ? 1 : 2);
 
+	host->mmc_omap_wq = alloc_workqueue("mmc_omap", 0, 0);
+	if (!host->mmc_omap_wq)
+		goto err_plat_cleanup;
+
 	return 0;
 
 err_plat_cleanup:
@@ -1518,7 +1522,7 @@ err_free_mem_region:
 	return ret;
 }
 
-static int mmc_omap_remove(struct platform_device *pdev)
+static int __devexit mmc_omap_remove(struct platform_device *pdev)
 {
 	struct mmc_omap_host *host = platform_get_drvdata(pdev);
 	int i;
@@ -1542,6 +1546,7 @@ static int mmc_omap_remove(struct platform_device *pdev)
 	iounmap(host->virt_base);
 	release_mem_region(pdev->resource[0].start,
 			   pdev->resource[0].end - pdev->resource[0].start + 1);
+	destroy_workqueue(host->mmc_omap_wq);
 
 	kfree(host);
 
@@ -1599,7 +1604,8 @@ static int mmc_omap_resume(struct platform_device *pdev)
 #endif
 
 static struct platform_driver mmc_omap_driver = {
-	.remove		= mmc_omap_remove,
+	.probe		= mmc_omap_probe,
+	.remove		= __devexit_p(mmc_omap_remove),
 	.suspend	= mmc_omap_suspend,
 	.resume		= mmc_omap_resume,
 	.driver		= {
@@ -1608,29 +1614,7 @@ static struct platform_driver mmc_omap_driver = {
 	},
 };
 
-static int __init mmc_omap_init(void)
-{
-	int ret;
-
-	mmc_omap_wq = alloc_workqueue("mmc_omap", 0, 0);
-	if (!mmc_omap_wq)
-		return -ENOMEM;
-
-	ret = platform_driver_probe(&mmc_omap_driver, mmc_omap_probe);
-	if (ret)
-		destroy_workqueue(mmc_omap_wq);
-	return ret;
-}
-
-static void __exit mmc_omap_exit(void)
-{
-	platform_driver_unregister(&mmc_omap_driver);
-	destroy_workqueue(mmc_omap_wq);
-}
-
-module_init(mmc_omap_init);
-module_exit(mmc_omap_exit);
-
+module_platform_driver(mmc_omap_driver);
 MODULE_DESCRIPTION("OMAP Multimedia Card driver");
 MODULE_LICENSE("GPL");
 MODULE_ALIAS("platform:" DRIVER_NAME);
diff --git a/drivers/mmc/host/omap_hsmmc.c b/drivers/mmc/host/omap_hsmmc.c
index 56d4499d438..9a7a60aeb19 100644
--- a/drivers/mmc/host/omap_hsmmc.c
+++ b/drivers/mmc/host/omap_hsmmc.c
@@ -85,12 +85,14 @@
 #define BRR_ENABLE		(1 << 5)
 #define DTO_ENABLE		(1 << 20)
 #define INIT_STREAM		(1 << 1)
+#define ACEN_ACMD12		(1 << 2)
 #define DP_SELECT		(1 << 21)
 #define DDIR			(1 << 4)
 #define DMA_EN			0x1
 #define MSBS			(1 << 5)
 #define BCE			(1 << 1)
 #define FOUR_BIT		(1 << 1)
+#define DDR			(1 << 19)
 #define DW8			(1 << 5)
 #define CC			0x1
 #define TC			0x02
@@ -115,6 +117,7 @@
 #define OMAP_MMC_MAX_CLOCK	52000000
 #define DRIVER_NAME		"omap_hsmmc"
 
+#define AUTO_CMD12		(1 << 0)	/* Auto CMD12 support */
 /*
  * One controller can have multiple slots, like on some omap boards using
  * omap.c controller driver. Luckily this is not currently done on any known
@@ -167,7 +170,6 @@ struct omap_hsmmc_host {
 	int			use_dma, dma_ch;
 	int			dma_line_tx, dma_line_rx;
 	int			slot_id;
-	int			got_dbclk;
 	int			response_busy;
 	int			context_loss;
 	int			vdd;
@@ -175,6 +177,7 @@ struct omap_hsmmc_host {
 	int			reqs_blocked;
 	int			use_reg;
 	int			req_in_progress;
+	unsigned int		flags;
 	struct omap_hsmmc_next	next_data;
 
 	struct	omap_mmc_platform_data	*pdata;
@@ -520,6 +523,10 @@ static void omap_hsmmc_set_bus_width(struct omap_hsmmc_host *host)
 	u32 con;
 
 	con = OMAP_HSMMC_READ(host->base, CON);
+	if (ios->timing == MMC_TIMING_UHS_DDR50)
+		con |= DDR;	/* configure in DDR mode */
+	else
+		con &= ~DDR;
 	switch (ios->bus_width) {
 	case MMC_BUS_WIDTH_8:
 		OMAP_HSMMC_WRITE(host->base, CON, con | DW8);
@@ -766,6 +773,8 @@ omap_hsmmc_start_command(struct omap_hsmmc_host *host, struct mmc_command *cmd,
 		cmdtype = 0x3;
 
 	cmdreg = (cmd->opcode << 24) | (resptype << 16) | (cmdtype << 22);
+	if ((host->flags & AUTO_CMD12) && mmc_op_multi(cmd->opcode))
+		cmdreg |= ACEN_ACMD12;
 
 	if (data) {
 		cmdreg |= DP_SELECT | MSBS | BCE;
@@ -796,11 +805,12 @@ omap_hsmmc_get_dma_dir(struct omap_hsmmc_host *host, struct mmc_data *data)
 static void omap_hsmmc_request_done(struct omap_hsmmc_host *host, struct mmc_request *mrq)
 {
 	int dma_ch;
+	unsigned long flags;
 
-	spin_lock(&host->irq_lock);
+	spin_lock_irqsave(&host->irq_lock, flags);
 	host->req_in_progress = 0;
 	dma_ch = host->dma_ch;
-	spin_unlock(&host->irq_lock);
+	spin_unlock_irqrestore(&host->irq_lock, flags);
 
 	omap_hsmmc_disable_irq(host);
 	/* Do not complete the request if DMA is still in progress */
@@ -837,11 +847,14 @@ omap_hsmmc_xfer_done(struct omap_hsmmc_host *host, struct mmc_data *data)
 	else
 		data->bytes_xfered = 0;
 
-	if (!data->stop) {
+	if (data->stop && ((!(host->flags & AUTO_CMD12)) || data->error)) {
+		omap_hsmmc_start_command(host, data->stop, NULL);
+	} else {
+		if (data->stop)
+			data->stop->resp[0] = OMAP_HSMMC_READ(host->base,
+							RSP76);
 		omap_hsmmc_request_done(host, data->mrq);
-		return;
 	}
-	omap_hsmmc_start_command(host, data->stop, NULL);
 }
 
 /*
@@ -874,13 +887,14 @@ omap_hsmmc_cmd_done(struct omap_hsmmc_host *host, struct mmc_command *cmd)
 static void omap_hsmmc_dma_cleanup(struct omap_hsmmc_host *host, int errno)
 {
 	int dma_ch;
+	unsigned long flags;
 
 	host->data->error = errno;
 
-	spin_lock(&host->irq_lock);
+	spin_lock_irqsave(&host->irq_lock, flags);
 	dma_ch = host->dma_ch;
 	host->dma_ch = -1;
-	spin_unlock(&host->irq_lock);
+	spin_unlock_irqrestore(&host->irq_lock, flags);
 
 	if (host->use_dma && dma_ch != -1) {
 		dma_unmap_sg(mmc_dev(host->mmc), host->data->sg,
@@ -1082,7 +1096,7 @@ static int omap_hsmmc_switch_opcond(struct omap_hsmmc_host *host, int vdd)
 
 	/* Disable the clocks */
 	pm_runtime_put_sync(host->dev);
-	if (host->got_dbclk)
+	if (host->dbclk)
 		clk_disable(host->dbclk);
 
 	/* Turn the power off */
@@ -1093,7 +1107,7 @@ static int omap_hsmmc_switch_opcond(struct omap_hsmmc_host *host, int vdd)
 		ret = mmc_slot(host).set_power(host->dev, host->slot_id, 1,
 					       vdd);
 	pm_runtime_get_sync(host->dev);
-	if (host->got_dbclk)
+	if (host->dbclk)
 		clk_enable(host->dbclk);
 
 	if (ret != 0)
@@ -1234,6 +1248,7 @@ static void omap_hsmmc_dma_cb(int lch, u16 ch_status, void *cb_data)
 	struct omap_hsmmc_host *host = cb_data;
 	struct mmc_data *data;
 	int dma_ch, req_in_progress;
+	unsigned long flags;
 
 	if (!(ch_status & OMAP_DMA_BLOCK_IRQ)) {
 		dev_warn(mmc_dev(host->mmc), "unexpected dma status %x\n",
@@ -1241,9 +1256,9 @@ static void omap_hsmmc_dma_cb(int lch, u16 ch_status, void *cb_data)
 		return;
 	}
 
-	spin_lock(&host->irq_lock);
+	spin_lock_irqsave(&host->irq_lock, flags);
 	if (host->dma_ch < 0) {
-		spin_unlock(&host->irq_lock);
+		spin_unlock_irqrestore(&host->irq_lock, flags);
 		return;
 	}
 
@@ -1253,7 +1268,7 @@ static void omap_hsmmc_dma_cb(int lch, u16 ch_status, void *cb_data)
 		/* Fire up the next transfer. */
 		omap_hsmmc_config_dma_params(host, data,
 					   data->sg + host->dma_sg_idx);
-		spin_unlock(&host->irq_lock);
+		spin_unlock_irqrestore(&host->irq_lock, flags);
 		return;
 	}
 
@@ -1264,7 +1279,7 @@ static void omap_hsmmc_dma_cb(int lch, u16 ch_status, void *cb_data)
 	req_in_progress = host->req_in_progress;
 	dma_ch = host->dma_ch;
 	host->dma_ch = -1;
-	spin_unlock(&host->irq_lock);
+	spin_unlock_irqrestore(&host->irq_lock, flags);
 
 	omap_free_dma(dma_ch);
 
@@ -1766,7 +1781,7 @@ static struct omap_mmc_platform_data *of_get_hsmmc_pdata(struct device *dev)
 		pdata->slots[0].nonremovable = true;
 		pdata->slots[0].no_regulator_off_init = true;
 	}
-	of_property_read_u32(np, "ti,bus-width", &bus_width);
+	of_property_read_u32(np, "bus-width", &bus_width);
 	if (bus_width == 4)
 		pdata->slots[0].caps |= MMC_CAP_4_BIT_DATA;
 	else if (bus_width == 8)
@@ -1844,6 +1859,7 @@ static int __devinit omap_hsmmc_probe(struct platform_device *pdev)
 	host->mapbase	= res->start + pdata->reg_offset;
 	host->base	= ioremap(host->mapbase, SZ_4K);
 	host->power_mode = MMC_POWER_OFF;
+	host->flags	= AUTO_CMD12;
 	host->next_data.cookie = 1;
 
 	platform_set_drvdata(pdev, host);
@@ -1885,21 +1901,17 @@ static int __devinit omap_hsmmc_probe(struct platform_device *pdev)
 
 	omap_hsmmc_context_save(host);
 
-	if (cpu_is_omap2430()) {
-		host->dbclk = clk_get(&pdev->dev, "mmchsdb_fck");
-		/*
-		 * MMC can still work without debounce clock.
-		 */
-		if (IS_ERR(host->dbclk))
-			dev_warn(mmc_dev(host->mmc),
-				"Failed to get debounce clock\n");
-		else
-			host->got_dbclk = 1;
-
-		if (host->got_dbclk)
-			if (clk_enable(host->dbclk) != 0)
-				dev_dbg(mmc_dev(host->mmc), "Enabling debounce"
-							" clk failed\n");
+	host->dbclk = clk_get(&pdev->dev, "mmchsdb_fck");
+	/*
+	 * MMC can still work without debounce clock.
+	 */
+	if (IS_ERR(host->dbclk)) {
+		dev_warn(mmc_dev(host->mmc), "Failed to get debounce clk\n");
+		host->dbclk = NULL;
+	} else if (clk_enable(host->dbclk) != 0) {
+		dev_warn(mmc_dev(host->mmc), "Failed to enable debounce clk\n");
+		clk_put(host->dbclk);
+		host->dbclk = NULL;
 	}
 
 	/* Since we do only SG emulation, we can have as many segs
@@ -1969,7 +1981,7 @@ static int __devinit omap_hsmmc_probe(struct platform_device *pdev)
 		ret = request_threaded_irq(mmc_slot(host).card_detect_irq,
 					   NULL,
 					   omap_hsmmc_detect,
-					   IRQF_TRIGGER_RISING | IRQF_TRIGGER_FALLING,
+					   IRQF_TRIGGER_RISING | IRQF_TRIGGER_FALLING | IRQF_ONESHOT,
 					   mmc_hostname(mmc), host);
 		if (ret) {
 			dev_dbg(mmc_dev(host->mmc),
@@ -2019,7 +2031,7 @@ err_irq:
 	pm_runtime_put_sync(host->dev);
 	pm_runtime_disable(host->dev);
 	clk_put(host->fclk);
-	if (host->got_dbclk) {
+	if (host->dbclk) {
 		clk_disable(host->dbclk);
 		clk_put(host->dbclk);
 	}
@@ -2030,7 +2042,9 @@ err1:
 err_alloc:
 	omap_hsmmc_gpio_free(pdata);
 err:
-	release_mem_region(res->start, resource_size(res));
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	if (res)
+		release_mem_region(res->start, resource_size(res));
 	return ret;
 }
 
@@ -2052,7 +2066,7 @@ static int __devexit omap_hsmmc_remove(struct platform_device *pdev)
 	pm_runtime_put_sync(host->dev);
 	pm_runtime_disable(host->dev);
 	clk_put(host->fclk);
-	if (host->got_dbclk) {
+	if (host->dbclk) {
 		clk_disable(host->dbclk);
 		clk_put(host->dbclk);
 	}
@@ -2110,7 +2124,7 @@ static int omap_hsmmc_suspend(struct device *dev)
 				OMAP_HSMMC_READ(host->base, HCTL) & ~SDBP);
 	}
 
-	if (host->got_dbclk)
+	if (host->dbclk)
 		clk_disable(host->dbclk);
 err:
 	pm_runtime_put_sync(host->dev);
@@ -2131,7 +2145,7 @@ static int omap_hsmmc_resume(struct device *dev)
 
 	pm_runtime_get_sync(host->dev);
 
-	if (host->got_dbclk)
+	if (host->dbclk)
 		clk_enable(host->dbclk);
 
 	if (!(host->mmc->pm_flags & MMC_PM_KEEP_POWER))
diff --git a/drivers/mmc/host/sdhci-esdhc-imx.c b/drivers/mmc/host/sdhci-esdhc-imx.c
index 8abdaf6697a..ebbe984e5d0 100644
--- a/drivers/mmc/host/sdhci-esdhc-imx.c
+++ b/drivers/mmc/host/sdhci-esdhc-imx.c
@@ -24,6 +24,7 @@
 #include <linux/of.h>
 #include <linux/of_device.h>
 #include <linux/of_gpio.h>
+#include <linux/pinctrl/consumer.h>
 #include <mach/esdhc.h>
 #include "sdhci-pltfm.h"
 #include "sdhci-esdhc.h"
@@ -68,7 +69,11 @@ struct pltfm_imx_data {
 	int flags;
 	u32 scratchpad;
 	enum imx_esdhc_type devtype;
+	struct pinctrl *pinctrl;
 	struct esdhc_platform_data boarddata;
+	struct clk *clk_ipg;
+	struct clk *clk_ahb;
+	struct clk *clk_per;
 };
 
 static struct platform_device_id imx_esdhc_devtype[] = {
@@ -402,7 +407,7 @@ sdhci_esdhc_imx_probe_dt(struct platform_device *pdev,
 	if (!np)
 		return -ENODEV;
 
-	if (of_get_property(np, "fsl,card-wired", NULL))
+	if (of_get_property(np, "non-removable", NULL))
 		boarddata->cd_type = ESDHC_CD_PERMANENT;
 
 	if (of_get_property(np, "fsl,cd-controller", NULL))
@@ -437,7 +442,6 @@ static int __devinit sdhci_esdhc_imx_probe(struct platform_device *pdev)
 	struct sdhci_pltfm_host *pltfm_host;
 	struct sdhci_host *host;
 	struct esdhc_platform_data *boarddata;
-	struct clk *clk;
 	int err;
 	struct pltfm_imx_data *imx_data;
 
@@ -458,14 +462,35 @@ static int __devinit sdhci_esdhc_imx_probe(struct platform_device *pdev)
 	imx_data->devtype = pdev->id_entry->driver_data;
 	pltfm_host->priv = imx_data;
 
-	clk = clk_get(mmc_dev(host->mmc), NULL);
-	if (IS_ERR(clk)) {
-		dev_err(mmc_dev(host->mmc), "clk err\n");
-		err = PTR_ERR(clk);
+	imx_data->clk_ipg = devm_clk_get(&pdev->dev, "ipg");
+	if (IS_ERR(imx_data->clk_ipg)) {
+		err = PTR_ERR(imx_data->clk_ipg);
 		goto err_clk_get;
 	}
-	clk_prepare_enable(clk);
-	pltfm_host->clk = clk;
+
+	imx_data->clk_ahb = devm_clk_get(&pdev->dev, "ahb");
+	if (IS_ERR(imx_data->clk_ahb)) {
+		err = PTR_ERR(imx_data->clk_ahb);
+		goto err_clk_get;
+	}
+
+	imx_data->clk_per = devm_clk_get(&pdev->dev, "per");
+	if (IS_ERR(imx_data->clk_per)) {
+		err = PTR_ERR(imx_data->clk_per);
+		goto err_clk_get;
+	}
+
+	pltfm_host->clk = imx_data->clk_per;
+
+	clk_prepare_enable(imx_data->clk_per);
+	clk_prepare_enable(imx_data->clk_ipg);
+	clk_prepare_enable(imx_data->clk_ahb);
+
+	imx_data->pinctrl = devm_pinctrl_get_select_default(&pdev->dev);
+	if (IS_ERR(imx_data->pinctrl)) {
+		err = PTR_ERR(imx_data->pinctrl);
+		goto pin_err;
+	}
 
 	host->quirks |= SDHCI_QUIRK_BROKEN_TIMEOUT_VAL;
 
@@ -558,8 +583,10 @@ no_card_detect_irq:
 		gpio_free(boarddata->wp_gpio);
 no_card_detect_pin:
 no_board_data:
-	clk_disable_unprepare(pltfm_host->clk);
-	clk_put(pltfm_host->clk);
+pin_err:
+	clk_disable_unprepare(imx_data->clk_per);
+	clk_disable_unprepare(imx_data->clk_ipg);
+	clk_disable_unprepare(imx_data->clk_ahb);
 err_clk_get:
 	kfree(imx_data);
 err_imx_data:
@@ -585,8 +612,10 @@ static int __devexit sdhci_esdhc_imx_remove(struct platform_device *pdev)
 		gpio_free(boarddata->cd_gpio);
 	}
 
-	clk_disable_unprepare(pltfm_host->clk);
-	clk_put(pltfm_host->clk);
+	clk_disable_unprepare(imx_data->clk_per);
+	clk_disable_unprepare(imx_data->clk_ipg);
+	clk_disable_unprepare(imx_data->clk_ahb);
+
 	kfree(imx_data);
 
 	sdhci_pltfm_free(pdev);
diff --git a/drivers/mmc/host/sdhci-pltfm.c b/drivers/mmc/host/sdhci-pltfm.c
index c5c2a48bdd9..d9a4ef4f1ed 100644
--- a/drivers/mmc/host/sdhci-pltfm.c
+++ b/drivers/mmc/host/sdhci-pltfm.c
@@ -42,7 +42,8 @@ static struct sdhci_ops sdhci_pltfm_ops = {
 #ifdef CONFIG_OF
 static bool sdhci_of_wp_inverted(struct device_node *np)
 {
-	if (of_get_property(np, "sdhci,wp-inverted", NULL))
+	if (of_get_property(np, "sdhci,wp-inverted", NULL) ||
+	    of_get_property(np, "wp-inverted", NULL))
 		return true;
 
 	/* Old device trees don't have the wp-inverted property. */
@@ -59,13 +60,16 @@ void sdhci_get_of_property(struct platform_device *pdev)
 	struct sdhci_host *host = platform_get_drvdata(pdev);
 	struct sdhci_pltfm_host *pltfm_host = sdhci_priv(host);
 	const __be32 *clk;
+	u32 bus_width;
 	int size;
 
 	if (of_device_is_available(np)) {
 		if (of_get_property(np, "sdhci,auto-cmd12", NULL))
 			host->quirks |= SDHCI_QUIRK_MULTIBLOCK_READ_ACMD12;
 
-		if (of_get_property(np, "sdhci,1-bit-only", NULL))
+		if (of_get_property(np, "sdhci,1-bit-only", NULL) ||
+		    (of_property_read_u32(np, "bus-width", &bus_width) == 0 &&
+		    bus_width == 1))
 			host->quirks |= SDHCI_QUIRK_FORCE_1_BIT_DATA;
 
 		if (sdhci_of_wp_inverted(np))
diff --git a/drivers/mmc/host/sdhci-spear.c b/drivers/mmc/host/sdhci-spear.c
index 6dfa82e03c7..1fe32dfa7cd 100644
--- a/drivers/mmc/host/sdhci-spear.c
+++ b/drivers/mmc/host/sdhci-spear.c
@@ -75,8 +75,6 @@ static int __devinit sdhci_probe(struct platform_device *pdev)
 	struct spear_sdhci *sdhci;
 	int ret;
 
-	BUG_ON(pdev == NULL);
-
 	iomem = platform_get_resource(pdev, IORESOURCE_MEM, 0);
 	if (!iomem) {
 		ret = -ENOMEM;
@@ -84,18 +82,18 @@ static int __devinit sdhci_probe(struct platform_device *pdev)
 		goto err;
 	}
 
-	if (!request_mem_region(iomem->start, resource_size(iomem),
-				"spear-sdhci")) {
+	if (!devm_request_mem_region(&pdev->dev, iomem->start,
+				resource_size(iomem), "spear-sdhci")) {
 		ret = -EBUSY;
 		dev_dbg(&pdev->dev, "cannot request region\n");
 		goto err;
 	}
 
-	sdhci = kzalloc(sizeof(*sdhci), GFP_KERNEL);
+	sdhci = devm_kzalloc(&pdev->dev, sizeof(*sdhci), GFP_KERNEL);
 	if (!sdhci) {
 		ret = -ENOMEM;
 		dev_dbg(&pdev->dev, "cannot allocate memory for sdhci\n");
-		goto err_kzalloc;
+		goto err;
 	}
 
 	/* clk enable */
@@ -103,13 +101,13 @@ static int __devinit sdhci_probe(struct platform_device *pdev)
 	if (IS_ERR(sdhci->clk)) {
 		ret = PTR_ERR(sdhci->clk);
 		dev_dbg(&pdev->dev, "Error getting clock\n");
-		goto err_clk_get;
+		goto err;
 	}
 
 	ret = clk_enable(sdhci->clk);
 	if (ret) {
 		dev_dbg(&pdev->dev, "Error enabling clock\n");
-		goto err_clk_enb;
+		goto put_clk;
 	}
 
 	/* overwrite platform_data */
@@ -124,7 +122,7 @@ static int __devinit sdhci_probe(struct platform_device *pdev)
 	if (IS_ERR(host)) {
 		ret = PTR_ERR(host);
 		dev_dbg(&pdev->dev, "error allocating host\n");
-		goto err_alloc_host;
+		goto disable_clk;
 	}
 
 	host->hw_name = "sdhci";
@@ -132,17 +130,18 @@ static int __devinit sdhci_probe(struct platform_device *pdev)
 	host->irq = platform_get_irq(pdev, 0);
 	host->quirks = SDHCI_QUIRK_BROKEN_ADMA;
 
-	host->ioaddr = ioremap(iomem->start, resource_size(iomem));
+	host->ioaddr = devm_ioremap(&pdev->dev, iomem->start,
+			resource_size(iomem));
 	if (!host->ioaddr) {
 		ret = -ENOMEM;
 		dev_dbg(&pdev->dev, "failed to remap registers\n");
-		goto err_ioremap;
+		goto free_host;
 	}
 
 	ret = sdhci_add_host(host);
 	if (ret) {
 		dev_dbg(&pdev->dev, "error adding host\n");
-		goto err_add_host;
+		goto free_host;
 	}
 
 	platform_set_drvdata(pdev, host);
@@ -161,11 +160,12 @@ static int __devinit sdhci_probe(struct platform_device *pdev)
 	if (sdhci->data->card_power_gpio >= 0) {
 		int val = 0;
 
-		ret = gpio_request(sdhci->data->card_power_gpio, "sdhci");
+		ret = devm_gpio_request(&pdev->dev,
+				sdhci->data->card_power_gpio, "sdhci");
 		if (ret < 0) {
 			dev_dbg(&pdev->dev, "gpio request fail: %d\n",
 					sdhci->data->card_power_gpio);
-			goto err_pgpio_request;
+			goto set_drvdata;
 		}
 
 		if (sdhci->data->power_always_enb)
@@ -177,60 +177,48 @@ static int __devinit sdhci_probe(struct platform_device *pdev)
 		if (ret) {
 			dev_dbg(&pdev->dev, "gpio set direction fail: %d\n",
 					sdhci->data->card_power_gpio);
-			goto err_pgpio_direction;
+			goto set_drvdata;
 		}
 	}
 
 	if (sdhci->data->card_int_gpio >= 0) {
-		ret = gpio_request(sdhci->data->card_int_gpio, "sdhci");
+		ret = devm_gpio_request(&pdev->dev, sdhci->data->card_int_gpio,
+				"sdhci");
 		if (ret < 0) {
 			dev_dbg(&pdev->dev, "gpio request fail: %d\n",
 					sdhci->data->card_int_gpio);
-			goto err_igpio_request;
+			goto set_drvdata;
 		}
 
 		ret = gpio_direction_input(sdhci->data->card_int_gpio);
 		if (ret) {
 			dev_dbg(&pdev->dev, "gpio set direction fail: %d\n",
 					sdhci->data->card_int_gpio);
-			goto err_igpio_direction;
+			goto set_drvdata;
 		}
-		ret = request_irq(gpio_to_irq(sdhci->data->card_int_gpio),
+		ret = devm_request_irq(&pdev->dev,
+				gpio_to_irq(sdhci->data->card_int_gpio),
 				sdhci_gpio_irq, IRQF_TRIGGER_LOW,
 				mmc_hostname(host->mmc), pdev);
 		if (ret) {
 			dev_dbg(&pdev->dev, "gpio request irq fail: %d\n",
 					sdhci->data->card_int_gpio);
-			goto err_igpio_request_irq;
+			goto set_drvdata;
 		}
 
 	}
 
 	return 0;
 
-err_igpio_request_irq:
-err_igpio_direction:
-	if (sdhci->data->card_int_gpio >= 0)
-		gpio_free(sdhci->data->card_int_gpio);
-err_igpio_request:
-err_pgpio_direction:
-	if (sdhci->data->card_power_gpio >= 0)
-		gpio_free(sdhci->data->card_power_gpio);
-err_pgpio_request:
+set_drvdata:
 	platform_set_drvdata(pdev, NULL);
 	sdhci_remove_host(host, 1);
-err_add_host:
-	iounmap(host->ioaddr);
-err_ioremap:
+free_host:
 	sdhci_free_host(host);
-err_alloc_host:
+disable_clk:
 	clk_disable(sdhci->clk);
-err_clk_enb:
+put_clk:
 	clk_put(sdhci->clk);
-err_clk_get:
-	kfree(sdhci);
-err_kzalloc:
-	release_mem_region(iomem->start, resource_size(iomem));
 err:
 	dev_err(&pdev->dev, "spear-sdhci probe failed: %d\n", ret);
 	return ret;
@@ -239,35 +227,19 @@ err:
 static int __devexit sdhci_remove(struct platform_device *pdev)
 {
 	struct sdhci_host *host = platform_get_drvdata(pdev);
-	struct resource *iomem = platform_get_resource(pdev, IORESOURCE_MEM, 0);
 	struct spear_sdhci *sdhci = dev_get_platdata(&pdev->dev);
-	int dead;
+	int dead = 0;
 	u32 scratch;
 
-	if (sdhci->data) {
-		if (sdhci->data->card_int_gpio >= 0) {
-			free_irq(gpio_to_irq(sdhci->data->card_int_gpio), pdev);
-			gpio_free(sdhci->data->card_int_gpio);
-		}
-
-		if (sdhci->data->card_power_gpio >= 0)
-			gpio_free(sdhci->data->card_power_gpio);
-	}
-
 	platform_set_drvdata(pdev, NULL);
-	dead = 0;
 	scratch = readl(host->ioaddr + SDHCI_INT_STATUS);
 	if (scratch == (u32)-1)
 		dead = 1;
 
 	sdhci_remove_host(host, dead);
-	iounmap(host->ioaddr);
 	sdhci_free_host(host);
 	clk_disable(sdhci->clk);
 	clk_put(sdhci->clk);
-	kfree(sdhci);
-	if (iomem)
-		release_mem_region(iomem->start, resource_size(iomem));
 
 	return 0;
 }
diff --git a/drivers/mmc/host/sdhci-tegra.c b/drivers/mmc/host/sdhci-tegra.c
index 53b26502f6e..b38d8a78f6a 100644
--- a/drivers/mmc/host/sdhci-tegra.c
+++ b/drivers/mmc/host/sdhci-tegra.c
@@ -32,8 +32,13 @@
 
 #include "sdhci-pltfm.h"
 
+/* Tegra SDHOST controller vendor register definitions */
+#define SDHCI_TEGRA_VENDOR_MISC_CTRL		0x120
+#define SDHCI_MISC_CTRL_ENABLE_SDHCI_SPEC_300	0x20
+
 #define NVQUIRK_FORCE_SDHCI_SPEC_200	BIT(0)
 #define NVQUIRK_ENABLE_BLOCK_GAP_DET	BIT(1)
+#define NVQUIRK_ENABLE_SDHCI_SPEC_300	BIT(2)
 
 struct sdhci_tegra_soc_data {
 	struct sdhci_pltfm_data *pdata;
@@ -120,6 +125,25 @@ static irqreturn_t carddetect_irq(int irq, void *data)
 	return IRQ_HANDLED;
 };
 
+static void tegra_sdhci_reset_exit(struct sdhci_host *host, u8 mask)
+{
+	struct sdhci_pltfm_host *pltfm_host = sdhci_priv(host);
+	struct sdhci_tegra *tegra_host = pltfm_host->priv;
+	const struct sdhci_tegra_soc_data *soc_data = tegra_host->soc_data;
+
+	if (!(mask & SDHCI_RESET_ALL))
+		return;
+
+	/* Erratum: Enable SDHCI spec v3.00 support */
+	if (soc_data->nvquirks & NVQUIRK_ENABLE_SDHCI_SPEC_300) {
+		u32 misc_ctrl;
+
+		misc_ctrl = sdhci_readb(host, SDHCI_TEGRA_VENDOR_MISC_CTRL);
+		misc_ctrl |= SDHCI_MISC_CTRL_ENABLE_SDHCI_SPEC_300;
+		sdhci_writeb(host, misc_ctrl, SDHCI_TEGRA_VENDOR_MISC_CTRL);
+	}
+}
+
 static int tegra_sdhci_8bit(struct sdhci_host *host, int bus_width)
 {
 	struct sdhci_pltfm_host *pltfm_host = sdhci_priv(host);
@@ -148,6 +172,7 @@ static struct sdhci_ops tegra_sdhci_ops = {
 	.read_w     = tegra_sdhci_readw,
 	.write_l    = tegra_sdhci_writel,
 	.platform_8bit_width = tegra_sdhci_8bit,
+	.platform_reset_exit = tegra_sdhci_reset_exit,
 };
 
 #ifdef CONFIG_ARCH_TEGRA_2x_SOC
@@ -178,6 +203,7 @@ static struct sdhci_pltfm_data sdhci_tegra30_pdata = {
 
 static struct sdhci_tegra_soc_data soc_data_tegra30 = {
 	.pdata = &sdhci_tegra30_pdata,
+	.nvquirks = NVQUIRK_ENABLE_SDHCI_SPEC_300,
 };
 #endif
 
@@ -269,7 +295,6 @@ static int __devinit sdhci_tegra_probe(struct platform_device *pdev)
 				"failed to allocate power gpio\n");
 			goto err_power_req;
 		}
-		tegra_gpio_enable(plat->power_gpio);
 		gpio_direction_output(plat->power_gpio, 1);
 	}
 
@@ -280,7 +305,6 @@ static int __devinit sdhci_tegra_probe(struct platform_device *pdev)
 				"failed to allocate cd gpio\n");
 			goto err_cd_req;
 		}
-		tegra_gpio_enable(plat->cd_gpio);
 		gpio_direction_input(plat->cd_gpio);
 
 		rc = request_irq(gpio_to_irq(plat->cd_gpio), carddetect_irq,
@@ -301,7 +325,6 @@ static int __devinit sdhci_tegra_probe(struct platform_device *pdev)
 				"failed to allocate wp gpio\n");
 			goto err_wp_req;
 		}
-		tegra_gpio_enable(plat->wp_gpio);
 		gpio_direction_input(plat->wp_gpio);
 	}
 
@@ -329,23 +352,17 @@ err_add_host:
 	clk_disable(pltfm_host->clk);
 	clk_put(pltfm_host->clk);
 err_clk_get:
-	if (gpio_is_valid(plat->wp_gpio)) {
-		tegra_gpio_disable(plat->wp_gpio);
+	if (gpio_is_valid(plat->wp_gpio))
 		gpio_free(plat->wp_gpio);
-	}
 err_wp_req:
 	if (gpio_is_valid(plat->cd_gpio))
 		free_irq(gpio_to_irq(plat->cd_gpio), host);
 err_cd_irq_req:
-	if (gpio_is_valid(plat->cd_gpio)) {
-		tegra_gpio_disable(plat->cd_gpio);
+	if (gpio_is_valid(plat->cd_gpio))
 		gpio_free(plat->cd_gpio);
-	}
 err_cd_req:
-	if (gpio_is_valid(plat->power_gpio)) {
-		tegra_gpio_disable(plat->power_gpio);
+	if (gpio_is_valid(plat->power_gpio))
 		gpio_free(plat->power_gpio);
-	}
 err_power_req:
 err_no_plat:
 	sdhci_pltfm_free(pdev);
@@ -362,21 +379,16 @@ static int __devexit sdhci_tegra_remove(struct platform_device *pdev)
 
 	sdhci_remove_host(host, dead);
 
-	if (gpio_is_valid(plat->wp_gpio)) {
-		tegra_gpio_disable(plat->wp_gpio);
+	if (gpio_is_valid(plat->wp_gpio))
 		gpio_free(plat->wp_gpio);
-	}
 
 	if (gpio_is_valid(plat->cd_gpio)) {
 		free_irq(gpio_to_irq(plat->cd_gpio), host);
-		tegra_gpio_disable(plat->cd_gpio);
 		gpio_free(plat->cd_gpio);
 	}
 
-	if (gpio_is_valid(plat->power_gpio)) {
-		tegra_gpio_disable(plat->power_gpio);
+	if (gpio_is_valid(plat->power_gpio))
 		gpio_free(plat->power_gpio);
-	}
 
 	clk_disable(pltfm_host->clk);
 	clk_put(pltfm_host->clk);
diff --git a/drivers/mmc/host/sdhci.c b/drivers/mmc/host/sdhci.c
index ccefdebeff1..e626732aff7 100644
--- a/drivers/mmc/host/sdhci.c
+++ b/drivers/mmc/host/sdhci.c
@@ -680,8 +680,8 @@ static u8 sdhci_calc_timeout(struct sdhci_host *host, struct mmc_command *cmd)
 	}
 
 	if (count >= 0xF) {
-		pr_warning("%s: Too large timeout requested for CMD%d!\n",
-		       mmc_hostname(host->mmc), cmd->opcode);
+		pr_warning("%s: Too large timeout 0x%x requested for CMD%d!\n",
+			   mmc_hostname(host->mmc), count, cmd->opcode);
 		count = 0xE;
 	}
 
diff --git a/drivers/mtd/devices/docg3.c b/drivers/mtd/devices/docg3.c
index 8272c02668d..50aa90aa7a7 100644
--- a/drivers/mtd/devices/docg3.c
+++ b/drivers/mtd/devices/docg3.c
@@ -383,7 +383,7 @@ static void doc_set_device_id(struct docg3 *docg3, int id)
  * leveling counters are stored.  To access this last area of 4 bytes, a special
  * mode must be input to the flash ASIC.
  *
- * Returns 0 if no error occured, -EIO else.
+ * Returns 0 if no error occurred, -EIO else.
  */
 static int doc_set_extra_page_mode(struct docg3 *docg3)
 {
@@ -681,7 +681,7 @@ out:
  *  - one read of 512 bytes at offset 0
  *  - one read of 512 bytes at offset 512 + 16
  *
- * Returns 0 if successful, -EIO if a read error occured.
+ * Returns 0 if successful, -EIO if a read error occurred.
  */
 static int doc_read_page_prepare(struct docg3 *docg3, int block0, int block1,
 				 int page, int offset)
@@ -839,7 +839,7 @@ static void calc_block_sector(loff_t from, int *block0, int *block1, int *page,
  *
  * Reads flash memory OOB area of pages.
  *
- * Returns 0 if read successfull, of -EIO, -EINVAL if an error occured
+ * Returns 0 if read successful, of -EIO, -EINVAL if an error occurred
  */
 static int doc_read_oob(struct mtd_info *mtd, loff_t from,
 			struct mtd_oob_ops *ops)
@@ -971,7 +971,7 @@ err_in_read:
  * Reads flash memory pages. This function does not read the OOB chunk, but only
  * the page data.
  *
- * Returns 0 if read successfull, of -EIO, -EINVAL if an error occured
+ * Returns 0 if read successful, of -EIO, -EINVAL if an error occurred
  */
 static int doc_read(struct mtd_info *mtd, loff_t from, size_t len,
 	     size_t *retlen, u_char *buf)
@@ -1109,7 +1109,7 @@ static int doc_get_op_status(struct docg3 *docg3)
  * Wait for the chip to be ready again after erase or write operation, and check
  * erase/write status.
  *
- * Returns 0 if erase successfull, -EIO if erase/write issue, -ETIMEOUT if
+ * Returns 0 if erase successful, -EIO if erase/write issue, -ETIMEOUT if
  * timeout
  */
 static int doc_write_erase_wait_status(struct docg3 *docg3)
@@ -1186,7 +1186,7 @@ static int doc_erase_block(struct docg3 *docg3, int block0, int block1)
  * Erase a bunch of contiguous blocks, by pairs, as a "mtd" page of 1024 is
  * split into 2 pages of 512 bytes on 2 contiguous blocks.
  *
- * Returns 0 if erase successful, -EINVAL if adressing error, -EIO if erase
+ * Returns 0 if erase successful, -EINVAL if addressing error, -EIO if erase
  * issue
  */
 static int doc_erase(struct mtd_info *mtd, struct erase_info *info)
@@ -1397,7 +1397,7 @@ static int doc_backup_oob(struct docg3 *docg3, loff_t to,
  * Or provide data without OOB, and then a all zeroed OOB will be used (ECC will
  * still be filled in if asked for).
  *
- * Returns 0 is successfull, EINVAL if length is not 14 bytes
+ * Returns 0 is successful, EINVAL if length is not 14 bytes
  */
 static int doc_write_oob(struct mtd_info *mtd, loff_t ofs,
 			 struct mtd_oob_ops *ops)
@@ -1945,7 +1945,7 @@ static void doc_release_device(struct mtd_info *mtd)
  * docg3_resume - Awakens docg3 floor
  * @pdev: platfrom device
  *
- * Returns 0 (always successfull)
+ * Returns 0 (always successful)
  */
 static int docg3_resume(struct platform_device *pdev)
 {
diff --git a/drivers/mtd/maps/lantiq-flash.c b/drivers/mtd/maps/lantiq-flash.c
index b5401e35574..c03456f1700 100644
--- a/drivers/mtd/maps/lantiq-flash.c
+++ b/drivers/mtd/maps/lantiq-flash.c
@@ -19,9 +19,9 @@
 #include <linux/mtd/cfi.h>
 #include <linux/platform_device.h>
 #include <linux/mtd/physmap.h>
+#include <linux/of.h>
 
 #include <lantiq_soc.h>
-#include <lantiq_platform.h>
 
 /*
  * The NOR flash is connected to the same external bus unit (EBU) as PCI.
@@ -44,8 +44,9 @@ struct ltq_mtd {
 	struct map_info *map;
 };
 
-static char ltq_map_name[] = "ltq_nor";
-static const char *ltq_probe_types[] __devinitconst = { "cmdlinepart", NULL };
+static const char ltq_map_name[] = "ltq_nor";
+static const char *ltq_probe_types[] __devinitconst = {
+					"cmdlinepart", "ofpart", NULL };
 
 static map_word
 ltq_read16(struct map_info *map, unsigned long adr)
@@ -108,42 +109,38 @@ ltq_copy_to(struct map_info *map, unsigned long to,
 	spin_unlock_irqrestore(&ebu_lock, flags);
 }
 
-static int __init
+static int __devinit
 ltq_mtd_probe(struct platform_device *pdev)
 {
-	struct physmap_flash_data *ltq_mtd_data = dev_get_platdata(&pdev->dev);
+	struct mtd_part_parser_data ppdata;
 	struct ltq_mtd *ltq_mtd;
-	struct resource *res;
 	struct cfi_private *cfi;
 	int err;
 
+	if (of_machine_is_compatible("lantiq,falcon") &&
+			(ltq_boot_select() != BS_FLASH)) {
+		dev_err(&pdev->dev, "invalid bootstrap options\n");
+		return -ENODEV;
+	}
+
 	ltq_mtd = kzalloc(sizeof(struct ltq_mtd), GFP_KERNEL);
 	platform_set_drvdata(pdev, ltq_mtd);
 
 	ltq_mtd->res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
 	if (!ltq_mtd->res) {
-		dev_err(&pdev->dev, "failed to get memory resource");
+		dev_err(&pdev->dev, "failed to get memory resource\n");
 		err = -ENOENT;
 		goto err_out;
 	}
 
-	res = devm_request_mem_region(&pdev->dev, ltq_mtd->res->start,
-		resource_size(ltq_mtd->res), dev_name(&pdev->dev));
-	if (!ltq_mtd->res) {
-		dev_err(&pdev->dev, "failed to request mem resource");
-		err = -EBUSY;
-		goto err_out;
-	}
-
 	ltq_mtd->map = kzalloc(sizeof(struct map_info), GFP_KERNEL);
-	ltq_mtd->map->phys = res->start;
-	ltq_mtd->map->size = resource_size(res);
-	ltq_mtd->map->virt = devm_ioremap_nocache(&pdev->dev,
-				ltq_mtd->map->phys, ltq_mtd->map->size);
+	ltq_mtd->map->phys = ltq_mtd->res->start;
+	ltq_mtd->map->size = resource_size(ltq_mtd->res);
+	ltq_mtd->map->virt = devm_request_and_ioremap(&pdev->dev, ltq_mtd->res);
 	if (!ltq_mtd->map->virt) {
-		dev_err(&pdev->dev, "failed to ioremap!\n");
-		err = -ENOMEM;
-		goto err_free;
+		dev_err(&pdev->dev, "failed to remap mem resource\n");
+		err = -EBUSY;
+		goto err_out;
 	}
 
 	ltq_mtd->map->name = ltq_map_name;
@@ -169,9 +166,9 @@ ltq_mtd_probe(struct platform_device *pdev)
 	cfi->addr_unlock1 ^= 1;
 	cfi->addr_unlock2 ^= 1;
 
-	err = mtd_device_parse_register(ltq_mtd->mtd, ltq_probe_types, NULL,
-					ltq_mtd_data->parts,
-					ltq_mtd_data->nr_parts);
+	ppdata.of_node = pdev->dev.of_node;
+	err = mtd_device_parse_register(ltq_mtd->mtd, ltq_probe_types,
+					&ppdata, NULL, 0);
 	if (err) {
 		dev_err(&pdev->dev, "failed to add partitions\n");
 		goto err_destroy;
@@ -204,32 +201,23 @@ ltq_mtd_remove(struct platform_device *pdev)
 	return 0;
 }
 
+static const struct of_device_id ltq_mtd_match[] = {
+	{ .compatible = "lantiq,nor" },
+	{},
+};
+MODULE_DEVICE_TABLE(of, ltq_mtd_match);
+
 static struct platform_driver ltq_mtd_driver = {
+	.probe = ltq_mtd_probe,
 	.remove = __devexit_p(ltq_mtd_remove),
 	.driver = {
-		.name = "ltq_nor",
+		.name = "ltq-nor",
 		.owner = THIS_MODULE,
+		.of_match_table = ltq_mtd_match,
 	},
 };
 
-static int __init
-init_ltq_mtd(void)
-{
-	int ret = platform_driver_probe(&ltq_mtd_driver, ltq_mtd_probe);
-
-	if (ret)
-		pr_err("ltq_nor: error registering platform driver");
-	return ret;
-}
-
-static void __exit
-exit_ltq_mtd(void)
-{
-	platform_driver_unregister(&ltq_mtd_driver);
-}
-
-module_init(init_ltq_mtd);
-module_exit(exit_ltq_mtd);
+module_platform_driver(ltq_mtd_driver);
 
 MODULE_LICENSE("GPL");
 MODULE_AUTHOR("John Crispin <blogic@openwrt.org>");
diff --git a/drivers/mtd/mtdchar.c b/drivers/mtd/mtdchar.c
index 58fc65f5c81..f2f482bec57 100644
--- a/drivers/mtd/mtdchar.c
+++ b/drivers/mtd/mtdchar.c
@@ -376,7 +376,7 @@ static int otp_select_filemode(struct mtd_file_info *mfi, int mode)
 	 * 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, -1, &retlen, NULL) == -EOPNOTSUPP)
+	if (mtd_read_fact_prot_reg(mtd, -1, 0, &retlen, NULL) == -EOPNOTSUPP)
 		return -EOPNOTSUPP;
 
 	switch (mode) {
diff --git a/drivers/mtd/nand/ams-delta.c b/drivers/mtd/nand/ams-delta.c
index 73416951f4c..861ca8f7e47 100644
--- a/drivers/mtd/nand/ams-delta.c
+++ b/drivers/mtd/nand/ams-delta.c
@@ -212,18 +212,17 @@ static int __devinit ams_delta_init(struct platform_device *pdev)
 	/* Link the private data with the MTD structure */
 	ams_delta_mtd->priv = this;
 
-	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_free;
-	}
+	/*
+	 * Don't try to request the memory region from here,
+	 * it should have been already requested from the
+	 * gpio-omap driver and requesting it again would fail.
+	 */
 
 	io_base = ioremap(res->start, resource_size(res));
 	if (io_base == NULL) {
 		dev_err(&pdev->dev, "ioremap failed\n");
 		err = -EIO;
-		goto out_release_io;
+		goto out_free;
 	}
 
 	this->priv = io_base;
@@ -271,8 +270,6 @@ out_gpio:
 	platform_set_drvdata(pdev, NULL);
 	gpio_free(AMS_DELTA_GPIO_PIN_NAND_RB);
 	iounmap(io_base);
-out_release_io:
-	release_mem_region(res->start, resource_size(res));
 out_free:
 	kfree(ams_delta_mtd);
  out:
@@ -285,7 +282,6 @@ out_free:
 static int __devexit ams_delta_cleanup(struct platform_device *pdev)
 {
 	void __iomem *io_base = platform_get_drvdata(pdev);
-	struct resource *res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
 
 	/* Release resources, unregister device */
 	nand_release(ams_delta_mtd);
@@ -293,7 +289,6 @@ static int __devexit ams_delta_cleanup(struct platform_device *pdev)
 	gpio_free_array(_mandatory_gpio, ARRAY_SIZE(_mandatory_gpio));
 	gpio_free(AMS_DELTA_GPIO_PIN_NAND_RB);
 	iounmap(io_base);
-	release_mem_region(res->start, resource_size(res));
 
 	/* Free the MTD device structure */
 	kfree(ams_delta_mtd);
diff --git a/drivers/mtd/nand/autcpu12.c b/drivers/mtd/nand/autcpu12.c
index 2e42ec2e8ff..04769a49a7c 100644
--- a/drivers/mtd/nand/autcpu12.c
+++ b/drivers/mtd/nand/autcpu12.c
@@ -102,10 +102,10 @@ static void autcpu12_hwcontrol(struct mtd_info *mtd, int cmd,
 		void __iomem *addr;
 		unsigned char bits;
 
-		addr = CS89712_VIRT_BASE + AUTCPU12_SMC_PORT_OFFSET;
-		bits = (ctrl & NAND_CLE) << 4;
+		bits = clps_readb(AUTCPU12_SMC_PORT_OFFSET) & ~0x30;
+		bits |= (ctrl & NAND_CLE) << 4;
 		bits |= (ctrl & NAND_ALE) << 2;
-		writeb((readb(addr) & ~0x30) | bits, addr);
+		clps_writeb(bits, AUTCPU12_SMC_PORT_OFFSET);
 
 		addr = autcpu12_fio_base + AUTCPU12_SMC_SELECT_OFFSET;
 		writeb((readb(addr) & ~0x1) | (ctrl & NAND_NCE), addr);
@@ -120,9 +120,7 @@ static void autcpu12_hwcontrol(struct mtd_info *mtd, int cmd,
  */
 int autcpu12_device_ready(struct mtd_info *mtd)
 {
-	void __iomem *addr = CS89712_VIRT_BASE + AUTCPU12_SMC_PORT_OFFSET;
-
-	return readb(addr) & AUTCPU12_SMC_RDY;
+	return clps_readb(AUTCPU12_SMC_PORT_OFFSET) & AUTCPU12_SMC_RDY;
 }
 
 /*
diff --git a/drivers/mtd/nand/gpmi-nand/gpmi-nand.c b/drivers/mtd/nand/gpmi-nand/gpmi-nand.c
index 9ec51cec2e1..b68e04310bd 100644
--- a/drivers/mtd/nand/gpmi-nand/gpmi-nand.c
+++ b/drivers/mtd/nand/gpmi-nand/gpmi-nand.c
@@ -24,6 +24,7 @@
 #include <linux/module.h>
 #include <linux/mtd/gpmi-nand.h>
 #include <linux/mtd/partitions.h>
+#include <linux/pinctrl/consumer.h>
 #include "gpmi-nand.h"
 
 /* add our owner bbt descriptor */
@@ -476,6 +477,7 @@ acquire_err:
 static int __devinit acquire_resources(struct gpmi_nand_data *this)
 {
 	struct resources *res = &this->resources;
+	struct pinctrl *pinctrl;
 	int ret;
 
 	ret = acquire_register_block(this, GPMI_NAND_GPMI_REGS_ADDR_RES_NAME);
@@ -494,6 +496,12 @@ static int __devinit acquire_resources(struct gpmi_nand_data *this)
 	if (ret)
 		goto exit_dma_channels;
 
+	pinctrl = devm_pinctrl_get_select_default(&this->pdev->dev);
+	if (IS_ERR(pinctrl)) {
+		ret = PTR_ERR(pinctrl);
+		goto exit_pin;
+	}
+
 	res->clock = clk_get(&this->pdev->dev, NULL);
 	if (IS_ERR(res->clock)) {
 		pr_err("can not get the clock\n");
@@ -503,6 +511,7 @@ static int __devinit acquire_resources(struct gpmi_nand_data *this)
 	return 0;
 
 exit_clock:
+exit_pin:
 	release_dma_channels(this);
 exit_dma_channels:
 	release_bch_irq(this);
diff --git a/drivers/mtd/nand/h1910.c b/drivers/mtd/nand/h1910.c
index 11e48781342..9bf5ce5fa22 100644
--- a/drivers/mtd/nand/h1910.c
+++ b/drivers/mtd/nand/h1910.c
@@ -24,7 +24,7 @@
 #include <linux/mtd/nand.h>
 #include <linux/mtd/partitions.h>
 #include <asm/io.h>
-#include <mach/hardware.h>	/* for CLPS7111_VIRT_BASE */
+#include <mach/hardware.h>
 #include <asm/sizes.h>
 #include <mach/h1900-gpio.h>
 #include <mach/ipaq.h>
diff --git a/drivers/mtd/nand/mxc_nand.c b/drivers/mtd/nand/mxc_nand.c
index cc0678a967c..9e374e9bd29 100644
--- a/drivers/mtd/nand/mxc_nand.c
+++ b/drivers/mtd/nand/mxc_nand.c
@@ -690,7 +690,7 @@ static void mxc_nand_select_chip(struct mtd_info *mtd, int chip)
 	if (chip == -1) {
 		/* Disable the NFC clock */
 		if (host->clk_act) {
-			clk_disable(host->clk);
+			clk_disable_unprepare(host->clk);
 			host->clk_act = 0;
 		}
 		return;
@@ -698,7 +698,7 @@ static void mxc_nand_select_chip(struct mtd_info *mtd, int chip)
 
 	if (!host->clk_act) {
 		/* Enable the NFC clock */
-		clk_enable(host->clk);
+		clk_prepare_enable(host->clk);
 		host->clk_act = 1;
 	}
 
@@ -1078,7 +1078,7 @@ static int __init mxcnd_probe(struct platform_device *pdev)
 		goto eclk;
 	}
 
-	clk_enable(host->clk);
+	clk_prepare_enable(host->clk);
 	host->clk_act = 1;
 
 	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
diff --git a/drivers/mtd/nand/orion_nand.c b/drivers/mtd/nand/orion_nand.c
index 1d3bfb26080..513dc88a05c 100644
--- a/drivers/mtd/nand/orion_nand.c
+++ b/drivers/mtd/nand/orion_nand.c
@@ -13,9 +13,12 @@
 #include <linux/slab.h>
 #include <linux/module.h>
 #include <linux/platform_device.h>
+#include <linux/of.h>
 #include <linux/mtd/mtd.h>
 #include <linux/mtd/nand.h>
 #include <linux/mtd/partitions.h>
+#include <linux/clk.h>
+#include <linux/err.h>
 #include <asm/io.h>
 #include <asm/sizes.h>
 #include <mach/hardware.h>
@@ -74,11 +77,14 @@ static void orion_nand_read_buf(struct mtd_info *mtd, uint8_t *buf, int len)
 static int __init orion_nand_probe(struct platform_device *pdev)
 {
 	struct mtd_info *mtd;
+	struct mtd_part_parser_data ppdata = {};
 	struct nand_chip *nc;
 	struct orion_nand_data *board;
 	struct resource *res;
+	struct clk *clk;
 	void __iomem *io_base;
 	int ret = 0;
+	u32 val = 0;
 
 	nc = kzalloc(sizeof(struct nand_chip) + sizeof(struct mtd_info), GFP_KERNEL);
 	if (!nc) {
@@ -101,7 +107,32 @@ static int __init orion_nand_probe(struct platform_device *pdev)
 		goto no_res;
 	}
 
-	board = pdev->dev.platform_data;
+	if (pdev->dev.of_node) {
+		board = devm_kzalloc(&pdev->dev, sizeof(struct orion_nand_data),
+					GFP_KERNEL);
+		if (!board) {
+			printk(KERN_ERR "orion_nand: failed to allocate board structure.\n");
+			ret = -ENOMEM;
+			goto no_res;
+		}
+		if (!of_property_read_u32(pdev->dev.of_node, "cle", &val))
+			board->cle = (u8)val;
+		else
+			board->cle = 0;
+		if (!of_property_read_u32(pdev->dev.of_node, "ale", &val))
+			board->ale = (u8)val;
+		else
+			board->ale = 1;
+		if (!of_property_read_u32(pdev->dev.of_node,
+						"bank-width", &val))
+			board->width = (u8)val * 8;
+		else
+			board->width = 8;
+		if (!of_property_read_u32(pdev->dev.of_node,
+						"chip-delay", &val))
+			board->chip_delay = (u8)val;
+	} else
+		board = pdev->dev.platform_data;
 
 	mtd->priv = nc;
 	mtd->owner = THIS_MODULE;
@@ -115,6 +146,10 @@ static int __init orion_nand_probe(struct platform_device *pdev)
 	if (board->chip_delay)
 		nc->chip_delay = board->chip_delay;
 
+	WARN(board->width > 16,
+		"%d bit bus width out of range",
+		board->width);
+
 	if (board->width == 16)
 		nc->options |= NAND_BUSWIDTH_16;
 
@@ -123,14 +158,23 @@ static int __init orion_nand_probe(struct platform_device *pdev)
 
 	platform_set_drvdata(pdev, mtd);
 
+	/* Not all platforms can gate the clock, so it is not
+	   an error if the clock does not exists. */
+	clk = clk_get(&pdev->dev, NULL);
+	if (!IS_ERR(clk)) {
+		clk_prepare_enable(clk);
+		clk_put(clk);
+	}
+
 	if (nand_scan(mtd, 1)) {
 		ret = -ENXIO;
 		goto no_dev;
 	}
 
 	mtd->name = "orion_nand";
-	ret = mtd_device_parse_register(mtd, NULL, NULL, board->parts,
-					board->nr_parts);
+	ppdata.of_node = pdev->dev.of_node;
+	ret = mtd_device_parse_register(mtd, NULL, &ppdata,
+			board->parts, board->nr_parts);
 	if (ret) {
 		nand_release(mtd);
 		goto no_dev;
@@ -151,6 +195,7 @@ static int __devexit orion_nand_remove(struct platform_device *pdev)
 {
 	struct mtd_info *mtd = platform_get_drvdata(pdev);
 	struct nand_chip *nc = mtd->priv;
+	struct clk *clk;
 
 	nand_release(mtd);
 
@@ -158,14 +203,28 @@ static int __devexit orion_nand_remove(struct platform_device *pdev)
 
 	kfree(nc);
 
+	clk = clk_get(&pdev->dev, NULL);
+	if (!IS_ERR(clk)) {
+		clk_disable_unprepare(clk);
+		clk_put(clk);
+	}
+
 	return 0;
 }
 
+#ifdef CONFIG_OF
+static struct of_device_id orion_nand_of_match_table[] = {
+	{ .compatible = "mrvl,orion-nand", },
+	{},
+};
+#endif
+
 static struct platform_driver orion_nand_driver = {
 	.remove		= __devexit_p(orion_nand_remove),
 	.driver		= {
 		.name	= "orion_nand",
 		.owner	= THIS_MODULE,
+		.of_match_table = of_match_ptr(orion_nand_of_match_table),
 	},
 };
 
diff --git a/drivers/mtd/ubi/Kconfig b/drivers/mtd/ubi/Kconfig
index 4dcc752a0c0..738ee8dc16c 100644
--- a/drivers/mtd/ubi/Kconfig
+++ b/drivers/mtd/ubi/Kconfig
@@ -52,12 +52,4 @@ config MTD_UBI_GLUEBI
 	   work on top of UBI. Do not enable this unless you use legacy
 	   software.
 
-config MTD_UBI_DEBUG
-	bool "UBI debugging"
-	depends on SYSFS
-	select DEBUG_FS
-	select KALLSYMS
-	help
-	  This option enables UBI debugging.
-
 endif # MTD_UBI
diff --git a/drivers/mtd/ubi/Makefile b/drivers/mtd/ubi/Makefile
index c9302a5452b..a0803ac7471 100644
--- a/drivers/mtd/ubi/Makefile
+++ b/drivers/mtd/ubi/Makefile
@@ -1,7 +1,6 @@
 obj-$(CONFIG_MTD_UBI) += ubi.o
 
-ubi-y += vtbl.o vmt.o upd.o build.o cdev.o kapi.o eba.o io.o wl.o scan.o
-ubi-y += misc.o
+ubi-y += vtbl.o vmt.o upd.o build.o cdev.o kapi.o eba.o io.o wl.o attach.o
+ubi-y += misc.o debug.o
 
-ubi-$(CONFIG_MTD_UBI_DEBUG) += debug.o
 obj-$(CONFIG_MTD_UBI_GLUEBI) += gluebi.o
diff --git a/drivers/mtd/ubi/scan.c b/drivers/mtd/ubi/attach.c
index 12c43b44f81..bd27cbbb406 100644
--- a/drivers/mtd/ubi/scan.c
+++ b/drivers/mtd/ubi/attach.c
@@ -19,21 +19,21 @@
  */
 
 /*
- * UBI scanning sub-system.
+ * UBI attaching sub-system.
  *
- * This sub-system is responsible for scanning the flash media, checking UBI
- * headers and providing complete information about the UBI flash image.
+ * This sub-system is responsible for attaching MTD devices and it also
+ * implements flash media scanning.
  *
- * The scanning information is represented by a &struct ubi_scan_info' object.
- * Information about found volumes is represented by &struct ubi_scan_volume
+ * The attaching information is represented by a &struct ubi_attach_info'
+ * object. Information about volumes is represented by &struct ubi_ainf_volume
  * objects which are kept in volume RB-tree with root at the @volumes field.
  * The RB-tree is indexed by the volume ID.
  *
- * Scanned logical eraseblocks are represented by &struct ubi_scan_leb objects.
- * These objects are kept in per-volume RB-trees with the root at the
- * corresponding &struct ubi_scan_volume object. To put it differently, we keep
- * an RB-tree of per-volume objects and each of these objects is the root of
- * RB-tree of per-eraseblock objects.
+ * Logical eraseblocks are represented by &struct ubi_ainf_peb objects. These
+ * objects are kept in per-volume RB-trees with the root at the corresponding
+ * &struct ubi_ainf_volume object. To put it differently, we keep an RB-tree of
+ * per-volume objects and each of these objects is the root of RB-tree of
+ * per-LEB objects.
  *
  * Corrupted physical eraseblocks are put to the @corr list, free physical
  * eraseblocks are put to the @free list and the physical eraseblock to be
@@ -51,28 +51,29 @@
  *
  * 1. Corruptions caused by power cuts. These are expected corruptions and UBI
  * tries to handle them gracefully, without printing too many warnings and
- * error messages. The idea is that we do not lose important data in these case
- * - we may lose only the data which was being written to the media just before
- * the power cut happened, and the upper layers (e.g., UBIFS) are supposed to
- * handle such data losses (e.g., by using the FS journal).
+ * error messages. The idea is that we do not lose important data in these
+ * cases - we may lose only the data which were being written to the media just
+ * before the power cut happened, and the upper layers (e.g., UBIFS) are
+ * supposed to handle such data losses (e.g., by using the FS journal).
  *
  * When UBI detects a corruption (CRC-32 mismatch) in a PEB, and it looks like
  * the reason is a power cut, UBI puts this PEB to the @erase list, and all
  * PEBs in the @erase list are scheduled for erasure later.
  *
  * 2. Unexpected corruptions which are not caused by power cuts. During
- * scanning, such PEBs are put to the @corr list and UBI preserves them.
+ * attaching, such PEBs are put to the @corr list and UBI preserves them.
  * Obviously, this lessens the amount of available PEBs, and if at some  point
  * UBI runs out of free PEBs, it switches to R/O mode. UBI also loudly informs
  * about such PEBs every time the MTD device is attached.
  *
  * However, it is difficult to reliably distinguish between these types of
- * corruptions and UBI's strategy is as follows. UBI assumes corruption type 2
- * if the VID header is corrupted and the data area does not contain all 0xFFs,
- * and there were no bit-flips or integrity errors while reading the data area.
- * Otherwise UBI assumes corruption type 1. So the decision criteria are as
- * follows.
- *   o If the data area contains only 0xFFs, there is no data, and it is safe
+ * corruptions and UBI's strategy is as follows (in case of attaching by
+ * scanning). UBI assumes corruption type 2 if the VID header is corrupted and
+ * the data area does not contain all 0xFFs, and there were no bit-flips or
+ * integrity errors (e.g., ECC errors in case of NAND) while reading the data
+ * area.  Otherwise UBI assumes corruption type 1. So the decision criteria
+ * are as follows.
+ *   o If the data area contains only 0xFFs, there are no data, and it is safe
  *     to just erase this PEB - this is corruption type 1.
  *   o If the data area has bit-flips or data integrity errors (ECC errors on
  *     NAND), it is probably a PEB which was being erased when power cut
@@ -88,11 +89,7 @@
 #include <linux/random.h>
 #include "ubi.h"
 
-#ifdef CONFIG_MTD_UBI_DEBUG
-static int paranoid_check_si(struct ubi_device *ubi, struct ubi_scan_info *si);
-#else
-#define paranoid_check_si(ubi, si) 0
-#endif
+static int self_check_ai(struct ubi_device *ubi, struct ubi_attach_info *ai);
 
 /* Temporary variables used during scanning */
 static struct ubi_ec_hdr *ech;
@@ -100,13 +97,18 @@ static struct ubi_vid_hdr *vidh;
 
 /**
  * add_to_list - add physical eraseblock to a list.
- * @si: scanning information
+ * @ai: attaching information
  * @pnum: physical eraseblock number to add
+ * @vol_id: the last used volume id for the PEB
+ * @lnum: the last used LEB number for the PEB
  * @ec: erase counter of the physical eraseblock
  * @to_head: if not zero, add to the head of the list
  * @list: the list to add to
  *
- * This function adds physical eraseblock @pnum to free, erase, or alien lists.
+ * This function allocates a 'struct ubi_ainf_peb' object for physical
+ * eraseblock @pnum and adds it to the "free", "erase", or "alien" lists.
+ * It stores the @lnum and @vol_id alongside, which can both be
+ * %UBI_UNKNOWN if they are not available, not readable, or not assigned.
  * If @to_head is not zero, PEB will be added to the head of the list, which
  * basically means it will be processed first later. E.g., we add corrupted
  * PEBs (corrupted due to power cuts) to the head of the erase list to make
@@ -114,65 +116,68 @@ static struct ubi_vid_hdr *vidh;
  * returns zero in case of success and a negative error code in case of
  * failure.
  */
-static int add_to_list(struct ubi_scan_info *si, int pnum, int ec, int to_head,
-		       struct list_head *list)
+static int add_to_list(struct ubi_attach_info *ai, int pnum, int vol_id,
+		       int lnum, int ec, int to_head, struct list_head *list)
 {
-	struct ubi_scan_leb *seb;
+	struct ubi_ainf_peb *aeb;
 
-	if (list == &si->free) {
+	if (list == &ai->free) {
 		dbg_bld("add to free: PEB %d, EC %d", pnum, ec);
-	} else if (list == &si->erase) {
+	} else if (list == &ai->erase) {
 		dbg_bld("add to erase: PEB %d, EC %d", pnum, ec);
-	} else if (list == &si->alien) {
+	} else if (list == &ai->alien) {
 		dbg_bld("add to alien: PEB %d, EC %d", pnum, ec);
-		si->alien_peb_count += 1;
+		ai->alien_peb_count += 1;
 	} else
 		BUG();
 
-	seb = kmem_cache_alloc(si->scan_leb_slab, GFP_KERNEL);
-	if (!seb)
+	aeb = kmem_cache_alloc(ai->aeb_slab_cache, GFP_KERNEL);
+	if (!aeb)
 		return -ENOMEM;
 
-	seb->pnum = pnum;
-	seb->ec = ec;
+	aeb->pnum = pnum;
+	aeb->vol_id = vol_id;
+	aeb->lnum = lnum;
+	aeb->ec = ec;
 	if (to_head)
-		list_add(&seb->u.list, list);
+		list_add(&aeb->u.list, list);
 	else
-		list_add_tail(&seb->u.list, list);
+		list_add_tail(&aeb->u.list, list);
 	return 0;
 }
 
 /**
  * add_corrupted - add a corrupted physical eraseblock.
- * @si: scanning information
+ * @ai: attaching information
  * @pnum: physical eraseblock number to add
  * @ec: erase counter of the physical eraseblock
  *
- * This function adds corrupted physical eraseblock @pnum to the 'corr' list.
- * The corruption was presumably not caused by a power cut. Returns zero in
- * case of success and a negative error code in case of failure.
+ * This function allocates a 'struct ubi_ainf_peb' object for a corrupted
+ * physical eraseblock @pnum and adds it to the 'corr' list.  The corruption
+ * was presumably not caused by a power cut. Returns zero in case of success
+ * and a negative error code in case of failure.
  */
-static int add_corrupted(struct ubi_scan_info *si, int pnum, int ec)
+static int add_corrupted(struct ubi_attach_info *ai, int pnum, int ec)
 {
-	struct ubi_scan_leb *seb;
+	struct ubi_ainf_peb *aeb;
 
 	dbg_bld("add to corrupted: PEB %d, EC %d", pnum, ec);
 
-	seb = kmem_cache_alloc(si->scan_leb_slab, GFP_KERNEL);
-	if (!seb)
+	aeb = kmem_cache_alloc(ai->aeb_slab_cache, GFP_KERNEL);
+	if (!aeb)
 		return -ENOMEM;
 
-	si->corr_peb_count += 1;
-	seb->pnum = pnum;
-	seb->ec = ec;
-	list_add(&seb->u.list, &si->corr);
+	ai->corr_peb_count += 1;
+	aeb->pnum = pnum;
+	aeb->ec = ec;
+	list_add(&aeb->u.list, &ai->corr);
 	return 0;
 }
 
 /**
  * validate_vid_hdr - check volume identifier header.
  * @vid_hdr: the volume identifier header to check
- * @sv: information about the volume this logical eraseblock belongs to
+ * @av: information about the volume this logical eraseblock belongs to
  * @pnum: physical eraseblock number the VID header came from
  *
  * This function checks that data stored in @vid_hdr is consistent. Returns
@@ -184,15 +189,15 @@ static int add_corrupted(struct ubi_scan_info *si, int pnum, int ec)
  * headers of the same volume.
  */
 static int validate_vid_hdr(const struct ubi_vid_hdr *vid_hdr,
-			    const struct ubi_scan_volume *sv, int pnum)
+			    const struct ubi_ainf_volume *av, int pnum)
 {
 	int vol_type = vid_hdr->vol_type;
 	int vol_id = be32_to_cpu(vid_hdr->vol_id);
 	int used_ebs = be32_to_cpu(vid_hdr->used_ebs);
 	int data_pad = be32_to_cpu(vid_hdr->data_pad);
 
-	if (sv->leb_count != 0) {
-		int sv_vol_type;
+	if (av->leb_count != 0) {
+		int av_vol_type;
 
 		/*
 		 * This is not the first logical eraseblock belonging to this
@@ -200,28 +205,28 @@ static int validate_vid_hdr(const struct ubi_vid_hdr *vid_hdr,
 		 * to the data in previous logical eraseblock headers.
 		 */
 
-		if (vol_id != sv->vol_id) {
-			dbg_err("inconsistent vol_id");
+		if (vol_id != av->vol_id) {
+			ubi_err("inconsistent vol_id");
 			goto bad;
 		}
 
-		if (sv->vol_type == UBI_STATIC_VOLUME)
-			sv_vol_type = UBI_VID_STATIC;
+		if (av->vol_type == UBI_STATIC_VOLUME)
+			av_vol_type = UBI_VID_STATIC;
 		else
-			sv_vol_type = UBI_VID_DYNAMIC;
+			av_vol_type = UBI_VID_DYNAMIC;
 
-		if (vol_type != sv_vol_type) {
-			dbg_err("inconsistent vol_type");
+		if (vol_type != av_vol_type) {
+			ubi_err("inconsistent vol_type");
 			goto bad;
 		}
 
-		if (used_ebs != sv->used_ebs) {
-			dbg_err("inconsistent used_ebs");
+		if (used_ebs != av->used_ebs) {
+			ubi_err("inconsistent used_ebs");
 			goto bad;
 		}
 
-		if (data_pad != sv->data_pad) {
-			dbg_err("inconsistent data_pad");
+		if (data_pad != av->data_pad) {
+			ubi_err("inconsistent data_pad");
 			goto bad;
 		}
 	}
@@ -230,74 +235,74 @@ static int validate_vid_hdr(const struct ubi_vid_hdr *vid_hdr,
 
 bad:
 	ubi_err("inconsistent VID header at PEB %d", pnum);
-	ubi_dbg_dump_vid_hdr(vid_hdr);
-	ubi_dbg_dump_sv(sv);
+	ubi_dump_vid_hdr(vid_hdr);
+	ubi_dump_av(av);
 	return -EINVAL;
 }
 
 /**
- * add_volume - add volume to the scanning information.
- * @si: scanning information
+ * add_volume - add volume to the attaching information.
+ * @ai: attaching information
  * @vol_id: ID of the volume to add
  * @pnum: physical eraseblock number
  * @vid_hdr: volume identifier header
  *
  * If the volume corresponding to the @vid_hdr logical eraseblock is already
- * present in the scanning information, this function does nothing. Otherwise
- * it adds corresponding volume to the scanning information. Returns a pointer
- * to the scanning volume object in case of success and a negative error code
- * in case of failure.
+ * present in the attaching information, this function does nothing. Otherwise
+ * it adds corresponding volume to the attaching information. Returns a pointer
+ * to the allocated "av" object in case of success and a negative error code in
+ * case of failure.
  */
-static struct ubi_scan_volume *add_volume(struct ubi_scan_info *si, int vol_id,
-					  int pnum,
+static struct ubi_ainf_volume *add_volume(struct ubi_attach_info *ai,
+					  int vol_id, int pnum,
 					  const struct ubi_vid_hdr *vid_hdr)
 {
-	struct ubi_scan_volume *sv;
-	struct rb_node **p = &si->volumes.rb_node, *parent = NULL;
+	struct ubi_ainf_volume *av;
+	struct rb_node **p = &ai->volumes.rb_node, *parent = NULL;
 
 	ubi_assert(vol_id == be32_to_cpu(vid_hdr->vol_id));
 
 	/* Walk the volume RB-tree to look if this volume is already present */
 	while (*p) {
 		parent = *p;
-		sv = rb_entry(parent, struct ubi_scan_volume, rb);
+		av = rb_entry(parent, struct ubi_ainf_volume, rb);
 
-		if (vol_id == sv->vol_id)
-			return sv;
+		if (vol_id == av->vol_id)
+			return av;
 
-		if (vol_id > sv->vol_id)
+		if (vol_id > av->vol_id)
 			p = &(*p)->rb_left;
 		else
 			p = &(*p)->rb_right;
 	}
 
 	/* The volume is absent - add it */
-	sv = kmalloc(sizeof(struct ubi_scan_volume), GFP_KERNEL);
-	if (!sv)
+	av = kmalloc(sizeof(struct ubi_ainf_volume), GFP_KERNEL);
+	if (!av)
 		return ERR_PTR(-ENOMEM);
 
-	sv->highest_lnum = sv->leb_count = 0;
-	sv->vol_id = vol_id;
-	sv->root = RB_ROOT;
-	sv->used_ebs = be32_to_cpu(vid_hdr->used_ebs);
-	sv->data_pad = be32_to_cpu(vid_hdr->data_pad);
-	sv->compat = vid_hdr->compat;
-	sv->vol_type = vid_hdr->vol_type == UBI_VID_DYNAMIC ? UBI_DYNAMIC_VOLUME
+	av->highest_lnum = av->leb_count = 0;
+	av->vol_id = vol_id;
+	av->root = RB_ROOT;
+	av->used_ebs = be32_to_cpu(vid_hdr->used_ebs);
+	av->data_pad = be32_to_cpu(vid_hdr->data_pad);
+	av->compat = vid_hdr->compat;
+	av->vol_type = vid_hdr->vol_type == UBI_VID_DYNAMIC ? UBI_DYNAMIC_VOLUME
 							    : UBI_STATIC_VOLUME;
-	if (vol_id > si->highest_vol_id)
-		si->highest_vol_id = vol_id;
+	if (vol_id > ai->highest_vol_id)
+		ai->highest_vol_id = vol_id;
 
-	rb_link_node(&sv->rb, parent, p);
-	rb_insert_color(&sv->rb, &si->volumes);
-	si->vols_found += 1;
+	rb_link_node(&av->rb, parent, p);
+	rb_insert_color(&av->rb, &ai->volumes);
+	ai->vols_found += 1;
 	dbg_bld("added volume %d", vol_id);
-	return sv;
+	return av;
 }
 
 /**
  * compare_lebs - find out which logical eraseblock is newer.
  * @ubi: UBI device description object
- * @seb: first logical eraseblock to compare
+ * @aeb: first logical eraseblock to compare
  * @pnum: physical eraseblock number of the second logical eraseblock to
  * compare
  * @vid_hdr: volume identifier header of the second logical eraseblock
@@ -306,7 +311,7 @@ static struct ubi_scan_volume *add_volume(struct ubi_scan_info *si, int vol_id,
  * case of success this function returns a positive value, in case of failure, a
  * negative error code is returned. The success return codes use the following
  * bits:
- *     o bit 0 is cleared: the first PEB (described by @seb) is newer than the
+ *     o bit 0 is cleared: the first PEB (described by @aeb) is newer than the
  *       second PEB (described by @pnum and @vid_hdr);
  *     o bit 0 is set: the second PEB is newer;
  *     o bit 1 is cleared: no bit-flips were detected in the newer LEB;
@@ -314,7 +319,7 @@ static struct ubi_scan_volume *add_volume(struct ubi_scan_info *si, int vol_id,
  *     o bit 2 is cleared: the older LEB is not corrupted;
  *     o bit 2 is set: the older LEB is corrupted.
  */
-static int compare_lebs(struct ubi_device *ubi, const struct ubi_scan_leb *seb,
+static int compare_lebs(struct ubi_device *ubi, const struct ubi_ainf_peb *aeb,
 			int pnum, const struct ubi_vid_hdr *vid_hdr)
 {
 	void *buf;
@@ -323,7 +328,7 @@ static int compare_lebs(struct ubi_device *ubi, const struct ubi_scan_leb *seb,
 	struct ubi_vid_hdr *vh = NULL;
 	unsigned long long sqnum2 = be64_to_cpu(vid_hdr->sqnum);
 
-	if (sqnum2 == seb->sqnum) {
+	if (sqnum2 == aeb->sqnum) {
 		/*
 		 * This must be a really ancient UBI image which has been
 		 * created before sequence numbers support has been added. At
@@ -337,7 +342,7 @@ static int compare_lebs(struct ubi_device *ubi, const struct ubi_scan_leb *seb,
 	}
 
 	/* Obviously the LEB with lower sequence counter is older */
-	second_is_newer = !!(sqnum2 > seb->sqnum);
+	second_is_newer = (sqnum2 > aeb->sqnum);
 
 	/*
 	 * Now we know which copy is newer. If the copy flag of the PEB with
@@ -356,7 +361,7 @@ static int compare_lebs(struct ubi_device *ubi, const struct ubi_scan_leb *seb,
 			return 1;
 		}
 	} else {
-		if (!seb->copy_flag) {
+		if (!aeb->copy_flag) {
 			/* It is not a copy, so it is newer */
 			dbg_bld("first PEB %d is newer, copy_flag is unset",
 				pnum);
@@ -367,13 +372,13 @@ static int compare_lebs(struct ubi_device *ubi, const struct ubi_scan_leb *seb,
 		if (!vh)
 			return -ENOMEM;
 
-		pnum = seb->pnum;
+		pnum = aeb->pnum;
 		err = ubi_io_read_vid_hdr(ubi, pnum, vh, 0);
 		if (err) {
 			if (err == UBI_IO_BITFLIPS)
 				bitflips = 1;
 			else {
-				dbg_err("VID of PEB %d header is bad, but it "
+				ubi_err("VID of PEB %d header is bad, but it "
 					"was OK earlier, err %d", pnum, err);
 				if (err > 0)
 					err = -EIO;
@@ -429,9 +434,9 @@ out_free_vidh:
 }
 
 /**
- * ubi_scan_add_used - add physical eraseblock to the scanning information.
+ * ubi_add_to_av - add used physical eraseblock to the attaching information.
  * @ubi: UBI device description object
- * @si: scanning information
+ * @ai: attaching information
  * @pnum: the physical eraseblock number
  * @ec: erase counter
  * @vid_hdr: the volume identifier header
@@ -444,14 +449,13 @@ out_free_vidh:
  * to be picked, while the older one has to be dropped. This function returns
  * zero in case of success and a negative error code in case of failure.
  */
-int ubi_scan_add_used(struct ubi_device *ubi, struct ubi_scan_info *si,
-		      int pnum, int ec, const struct ubi_vid_hdr *vid_hdr,
-		      int bitflips)
+int ubi_add_to_av(struct ubi_device *ubi, struct ubi_attach_info *ai, int pnum,
+		  int ec, const struct ubi_vid_hdr *vid_hdr, int bitflips)
 {
 	int err, vol_id, lnum;
 	unsigned long long sqnum;
-	struct ubi_scan_volume *sv;
-	struct ubi_scan_leb *seb;
+	struct ubi_ainf_volume *av;
+	struct ubi_ainf_peb *aeb;
 	struct rb_node **p, *parent = NULL;
 
 	vol_id = be32_to_cpu(vid_hdr->vol_id);
@@ -461,25 +465,25 @@ int ubi_scan_add_used(struct ubi_device *ubi, struct ubi_scan_info *si,
 	dbg_bld("PEB %d, LEB %d:%d, EC %d, sqnum %llu, bitflips %d",
 		pnum, vol_id, lnum, ec, sqnum, bitflips);
 
-	sv = add_volume(si, vol_id, pnum, vid_hdr);
-	if (IS_ERR(sv))
-		return PTR_ERR(sv);
+	av = add_volume(ai, vol_id, pnum, vid_hdr);
+	if (IS_ERR(av))
+		return PTR_ERR(av);
 
-	if (si->max_sqnum < sqnum)
-		si->max_sqnum = sqnum;
+	if (ai->max_sqnum < sqnum)
+		ai->max_sqnum = sqnum;
 
 	/*
 	 * Walk the RB-tree of logical eraseblocks of volume @vol_id to look
 	 * if this is the first instance of this logical eraseblock or not.
 	 */
-	p = &sv->root.rb_node;
+	p = &av->root.rb_node;
 	while (*p) {
 		int cmp_res;
 
 		parent = *p;
-		seb = rb_entry(parent, struct ubi_scan_leb, u.rb);
-		if (lnum != seb->lnum) {
-			if (lnum < seb->lnum)
+		aeb = rb_entry(parent, struct ubi_ainf_peb, u.rb);
+		if (lnum != aeb->lnum) {
+			if (lnum < aeb->lnum)
 				p = &(*p)->rb_left;
 			else
 				p = &(*p)->rb_right;
@@ -491,8 +495,8 @@ int ubi_scan_add_used(struct ubi_device *ubi, struct ubi_scan_info *si,
 		 * logical eraseblock present.
 		 */
 
-		dbg_bld("this LEB already exists: PEB %d, sqnum %llu, "
-			"EC %d", seb->pnum, seb->sqnum, seb->ec);
+		dbg_bld("this LEB already exists: PEB %d, sqnum %llu, EC %d",
+			aeb->pnum, aeb->sqnum, aeb->ec);
 
 		/*
 		 * Make sure that the logical eraseblocks have different
@@ -507,11 +511,11 @@ int ubi_scan_add_used(struct ubi_device *ubi, struct ubi_scan_info *si,
 		 * images, but refuse attaching old images with duplicated
 		 * logical eraseblocks because there was an unclean reboot.
 		 */
-		if (seb->sqnum == sqnum && sqnum != 0) {
+		if (aeb->sqnum == sqnum && sqnum != 0) {
 			ubi_err("two LEBs with same sequence number %llu",
 				sqnum);
-			ubi_dbg_dump_seb(seb, 0);
-			ubi_dbg_dump_vid_hdr(vid_hdr);
+			ubi_dump_aeb(aeb, 0);
+			ubi_dump_vid_hdr(vid_hdr);
 			return -EINVAL;
 		}
 
@@ -519,7 +523,7 @@ int ubi_scan_add_used(struct ubi_device *ubi, struct ubi_scan_info *si,
 		 * Now we have to drop the older one and preserve the newer
 		 * one.
 		 */
-		cmp_res = compare_lebs(ubi, seb, pnum, vid_hdr);
+		cmp_res = compare_lebs(ubi, aeb, pnum, vid_hdr);
 		if (cmp_res < 0)
 			return cmp_res;
 
@@ -528,23 +532,26 @@ int ubi_scan_add_used(struct ubi_device *ubi, struct ubi_scan_info *si,
 			 * This logical eraseblock is newer than the one
 			 * found earlier.
 			 */
-			err = validate_vid_hdr(vid_hdr, sv, pnum);
+			err = validate_vid_hdr(vid_hdr, av, pnum);
 			if (err)
 				return err;
 
-			err = add_to_list(si, seb->pnum, seb->ec, cmp_res & 4,
-					  &si->erase);
+			err = add_to_list(ai, aeb->pnum, aeb->vol_id,
+					  aeb->lnum, aeb->ec, cmp_res & 4,
+					  &ai->erase);
 			if (err)
 				return err;
 
-			seb->ec = ec;
-			seb->pnum = pnum;
-			seb->scrub = ((cmp_res & 2) || bitflips);
-			seb->copy_flag = vid_hdr->copy_flag;
-			seb->sqnum = sqnum;
+			aeb->ec = ec;
+			aeb->pnum = pnum;
+			aeb->vol_id = vol_id;
+			aeb->lnum = lnum;
+			aeb->scrub = ((cmp_res & 2) || bitflips);
+			aeb->copy_flag = vid_hdr->copy_flag;
+			aeb->sqnum = sqnum;
 
-			if (sv->highest_lnum == lnum)
-				sv->last_data_size =
+			if (av->highest_lnum == lnum)
+				av->last_data_size =
 					be32_to_cpu(vid_hdr->data_size);
 
 			return 0;
@@ -553,92 +560,64 @@ int ubi_scan_add_used(struct ubi_device *ubi, struct ubi_scan_info *si,
 			 * This logical eraseblock is older than the one found
 			 * previously.
 			 */
-			return add_to_list(si, pnum, ec, cmp_res & 4,
-					   &si->erase);
+			return add_to_list(ai, pnum, vol_id, lnum, ec,
+					   cmp_res & 4, &ai->erase);
 		}
 	}
 
 	/*
 	 * We've met this logical eraseblock for the first time, add it to the
-	 * scanning information.
+	 * attaching information.
 	 */
 
-	err = validate_vid_hdr(vid_hdr, sv, pnum);
+	err = validate_vid_hdr(vid_hdr, av, pnum);
 	if (err)
 		return err;
 
-	seb = kmem_cache_alloc(si->scan_leb_slab, GFP_KERNEL);
-	if (!seb)
+	aeb = kmem_cache_alloc(ai->aeb_slab_cache, GFP_KERNEL);
+	if (!aeb)
 		return -ENOMEM;
 
-	seb->ec = ec;
-	seb->pnum = pnum;
-	seb->lnum = lnum;
-	seb->scrub = bitflips;
-	seb->copy_flag = vid_hdr->copy_flag;
-	seb->sqnum = sqnum;
-
-	if (sv->highest_lnum <= lnum) {
-		sv->highest_lnum = lnum;
-		sv->last_data_size = be32_to_cpu(vid_hdr->data_size);
+	aeb->ec = ec;
+	aeb->pnum = pnum;
+	aeb->vol_id = vol_id;
+	aeb->lnum = lnum;
+	aeb->scrub = bitflips;
+	aeb->copy_flag = vid_hdr->copy_flag;
+	aeb->sqnum = sqnum;
+
+	if (av->highest_lnum <= lnum) {
+		av->highest_lnum = lnum;
+		av->last_data_size = be32_to_cpu(vid_hdr->data_size);
 	}
 
-	sv->leb_count += 1;
-	rb_link_node(&seb->u.rb, parent, p);
-	rb_insert_color(&seb->u.rb, &sv->root);
+	av->leb_count += 1;
+	rb_link_node(&aeb->u.rb, parent, p);
+	rb_insert_color(&aeb->u.rb, &av->root);
 	return 0;
 }
 
 /**
- * ubi_scan_find_sv - find volume in the scanning information.
- * @si: scanning information
+ * ubi_find_av - find volume in the attaching information.
+ * @ai: attaching information
  * @vol_id: the requested volume ID
  *
  * This function returns a pointer to the volume description or %NULL if there
- * are no data about this volume in the scanning information.
- */
-struct ubi_scan_volume *ubi_scan_find_sv(const struct ubi_scan_info *si,
-					 int vol_id)
-{
-	struct ubi_scan_volume *sv;
-	struct rb_node *p = si->volumes.rb_node;
-
-	while (p) {
-		sv = rb_entry(p, struct ubi_scan_volume, rb);
-
-		if (vol_id == sv->vol_id)
-			return sv;
-
-		if (vol_id > sv->vol_id)
-			p = p->rb_left;
-		else
-			p = p->rb_right;
-	}
-
-	return NULL;
-}
-
-/**
- * ubi_scan_find_seb - find LEB in the volume scanning information.
- * @sv: a pointer to the volume scanning information
- * @lnum: the requested logical eraseblock
- *
- * This function returns a pointer to the scanning logical eraseblock or %NULL
- * if there are no data about it in the scanning volume information.
+ * are no data about this volume in the attaching information.
  */
-struct ubi_scan_leb *ubi_scan_find_seb(const struct ubi_scan_volume *sv,
-				       int lnum)
+struct ubi_ainf_volume *ubi_find_av(const struct ubi_attach_info *ai,
+				    int vol_id)
 {
-	struct ubi_scan_leb *seb;
-	struct rb_node *p = sv->root.rb_node;
+	struct ubi_ainf_volume *av;
+	struct rb_node *p = ai->volumes.rb_node;
 
 	while (p) {
-		seb = rb_entry(p, struct ubi_scan_leb, u.rb);
+		av = rb_entry(p, struct ubi_ainf_volume, rb);
 
-		if (lnum == seb->lnum)
-			return seb;
+		if (vol_id == av->vol_id)
+			return av;
 
-		if (lnum > seb->lnum)
+		if (vol_id > av->vol_id)
 			p = p->rb_left;
 		else
 			p = p->rb_right;
@@ -648,34 +627,34 @@ struct ubi_scan_leb *ubi_scan_find_seb(const struct ubi_scan_volume *sv,
 }
 
 /**
- * ubi_scan_rm_volume - delete scanning information about a volume.
- * @si: scanning information
- * @sv: the volume scanning information to delete
+ * ubi_remove_av - delete attaching information about a volume.
+ * @ai: attaching information
+ * @av: the volume attaching information to delete
  */
-void ubi_scan_rm_volume(struct ubi_scan_info *si, struct ubi_scan_volume *sv)
+void ubi_remove_av(struct ubi_attach_info *ai, struct ubi_ainf_volume *av)
 {
 	struct rb_node *rb;
-	struct ubi_scan_leb *seb;
+	struct ubi_ainf_peb *aeb;
 
-	dbg_bld("remove scanning information about volume %d", sv->vol_id);
+	dbg_bld("remove attaching information about volume %d", av->vol_id);
 
-	while ((rb = rb_first(&sv->root))) {
-		seb = rb_entry(rb, struct ubi_scan_leb, u.rb);
-		rb_erase(&seb->u.rb, &sv->root);
-		list_add_tail(&seb->u.list, &si->erase);
+	while ((rb = rb_first(&av->root))) {
+		aeb = rb_entry(rb, struct ubi_ainf_peb, u.rb);
+		rb_erase(&aeb->u.rb, &av->root);
+		list_add_tail(&aeb->u.list, &ai->erase);
 	}
 
-	rb_erase(&sv->rb, &si->volumes);
-	kfree(sv);
-	si->vols_found -= 1;
+	rb_erase(&av->rb, &ai->volumes);
+	kfree(av);
+	ai->vols_found -= 1;
 }
 
 /**
- * ubi_scan_erase_peb - erase a physical eraseblock.
+ * early_erase_peb - erase a physical eraseblock.
  * @ubi: UBI device description object
- * @si: scanning information
+ * @ai: attaching information
  * @pnum: physical eraseblock number to erase;
- * @ec: erase counter value to write (%UBI_SCAN_UNKNOWN_EC if it is unknown)
+ * @ec: erase counter value to write (%UBI_UNKNOWN if it is unknown)
  *
  * This function erases physical eraseblock 'pnum', and writes the erase
  * counter header to it. This function should only be used on UBI device
@@ -683,8 +662,8 @@ void ubi_scan_rm_volume(struct ubi_scan_info *si, struct ubi_scan_volume *sv)
  * This function returns zero in case of success and a negative error code in
  * case of failure.
  */
-int ubi_scan_erase_peb(struct ubi_device *ubi, const struct ubi_scan_info *si,
-		       int pnum, int ec)
+static int early_erase_peb(struct ubi_device *ubi,
+			   const struct ubi_attach_info *ai, int pnum, int ec)
 {
 	int err;
 	struct ubi_ec_hdr *ec_hdr;
@@ -716,9 +695,9 @@ out_free:
 }
 
 /**
- * ubi_scan_get_free_peb - get a free physical eraseblock.
+ * ubi_early_get_peb - get a free physical eraseblock.
  * @ubi: UBI device description object
- * @si: scanning information
+ * @ai: attaching information
  *
  * This function returns a free physical eraseblock. It is supposed to be
  * called on the UBI initialization stages when the wear-leveling sub-system is
@@ -726,20 +705,20 @@ out_free:
  * the lists, writes the EC header if it is needed, and removes it from the
  * list.
  *
- * This function returns scanning physical eraseblock information in case of
- * success and an error code in case of failure.
+ * This function returns a pointer to the "aeb" of the found free PEB in case
+ * of success and an error code in case of failure.
  */
-struct ubi_scan_leb *ubi_scan_get_free_peb(struct ubi_device *ubi,
-					   struct ubi_scan_info *si)
+struct ubi_ainf_peb *ubi_early_get_peb(struct ubi_device *ubi,
+				       struct ubi_attach_info *ai)
 {
 	int err = 0;
-	struct ubi_scan_leb *seb, *tmp_seb;
+	struct ubi_ainf_peb *aeb, *tmp_aeb;
 
-	if (!list_empty(&si->free)) {
-		seb = list_entry(si->free.next, struct ubi_scan_leb, u.list);
-		list_del(&seb->u.list);
-		dbg_bld("return free PEB %d, EC %d", seb->pnum, seb->ec);
-		return seb;
+	if (!list_empty(&ai->free)) {
+		aeb = list_entry(ai->free.next, struct ubi_ainf_peb, u.list);
+		list_del(&aeb->u.list);
+		dbg_bld("return free PEB %d, EC %d", aeb->pnum, aeb->ec);
+		return aeb;
 	}
 
 	/*
@@ -748,18 +727,18 @@ struct ubi_scan_leb *ubi_scan_get_free_peb(struct ubi_device *ubi,
 	 * so forth. We don't want to take care about bad eraseblocks here -
 	 * they'll be handled later.
 	 */
-	list_for_each_entry_safe(seb, tmp_seb, &si->erase, u.list) {
-		if (seb->ec == UBI_SCAN_UNKNOWN_EC)
-			seb->ec = si->mean_ec;
+	list_for_each_entry_safe(aeb, tmp_aeb, &ai->erase, u.list) {
+		if (aeb->ec == UBI_UNKNOWN)
+			aeb->ec = ai->mean_ec;
 
-		err = ubi_scan_erase_peb(ubi, si, seb->pnum, seb->ec+1);
+		err = early_erase_peb(ubi, ai, aeb->pnum, aeb->ec+1);
 		if (err)
 			continue;
 
-		seb->ec += 1;
-		list_del(&seb->u.list);
-		dbg_bld("return PEB %d, EC %d", seb->pnum, seb->ec);
-		return seb;
+		aeb->ec += 1;
+		list_del(&aeb->u.list);
+		dbg_bld("return PEB %d, EC %d", aeb->pnum, aeb->ec);
+		return aeb;
 	}
 
 	ubi_err("no free eraseblocks");
@@ -814,7 +793,7 @@ static int check_corruption(struct ubi_device *ubi, struct ubi_vid_hdr *vid_hdr,
 	ubi_err("PEB %d contains corrupted VID header, and the data does not "
 		"contain all 0xFF, this may be a non-UBI PEB or a severe VID "
 		"header corruption which requires manual inspection", pnum);
-	ubi_dbg_dump_vid_hdr(vid_hdr);
+	ubi_dump_vid_hdr(vid_hdr);
 	dbg_msg("hexdump of PEB %d offset %d, length %d",
 		pnum, ubi->leb_start, ubi->leb_size);
 	ubi_dbg_print_hex_dump(KERN_DEBUG, "", DUMP_PREFIX_OFFSET, 32, 1,
@@ -827,16 +806,18 @@ out_unlock:
 }
 
 /**
- * process_eb - read, check UBI headers, and add them to scanning information.
+ * scan_peb - scan and process UBI headers of a PEB.
  * @ubi: UBI device description object
- * @si: scanning information
+ * @ai: attaching information
  * @pnum: the physical eraseblock number
  *
- * This function returns a zero if the physical eraseblock was successfully
- * handled and a negative error code in case of failure.
+ * This function reads UBI headers of PEB @pnum, checks them, and adds
+ * information about this PEB to the corresponding list or RB-tree in the
+ * "attaching info" structure. Returns zero if the physical eraseblock was
+ * successfully handled and a negative error code in case of failure.
  */
-static int process_eb(struct ubi_device *ubi, struct ubi_scan_info *si,
-		      int pnum)
+static int scan_peb(struct ubi_device *ubi, struct ubi_attach_info *ai,
+		    int pnum)
 {
 	long long uninitialized_var(ec);
 	int err, bitflips = 0, vol_id, ec_err = 0;
@@ -848,12 +829,7 @@ static int process_eb(struct ubi_device *ubi, struct ubi_scan_info *si,
 	if (err < 0)
 		return err;
 	else if (err) {
-		/*
-		 * FIXME: this is actually duty of the I/O sub-system to
-		 * initialize this, but MTD does not provide enough
-		 * information.
-		 */
-		si->bad_peb_count += 1;
+		ai->bad_peb_count += 1;
 		return 0;
 	}
 
@@ -867,13 +843,13 @@ static int process_eb(struct ubi_device *ubi, struct ubi_scan_info *si,
 		bitflips = 1;
 		break;
 	case UBI_IO_FF:
-		si->empty_peb_count += 1;
-		return add_to_list(si, pnum, UBI_SCAN_UNKNOWN_EC, 0,
-				   &si->erase);
+		ai->empty_peb_count += 1;
+		return add_to_list(ai, pnum, UBI_UNKNOWN, UBI_UNKNOWN,
+				   UBI_UNKNOWN, 0, &ai->erase);
 	case UBI_IO_FF_BITFLIPS:
-		si->empty_peb_count += 1;
-		return add_to_list(si, pnum, UBI_SCAN_UNKNOWN_EC, 1,
-				   &si->erase);
+		ai->empty_peb_count += 1;
+		return add_to_list(ai, pnum, UBI_UNKNOWN, UBI_UNKNOWN,
+				   UBI_UNKNOWN, 1, &ai->erase);
 	case UBI_IO_BAD_HDR_EBADMSG:
 	case UBI_IO_BAD_HDR:
 		/*
@@ -882,7 +858,7 @@ static int process_eb(struct ubi_device *ubi, struct ubi_scan_info *si,
 		 * moved and EC be re-created.
 		 */
 		ec_err = err;
-		ec = UBI_SCAN_UNKNOWN_EC;
+		ec = UBI_UNKNOWN;
 		bitflips = 1;
 		break;
 	default:
@@ -911,7 +887,7 @@ static int process_eb(struct ubi_device *ubi, struct ubi_scan_info *si,
 			 */
 			ubi_err("erase counter overflow, max is %d",
 				UBI_MAX_ERASECOUNTER);
-			ubi_dbg_dump_ec_hdr(ech);
+			ubi_dump_ec_hdr(ech);
 			return -EINVAL;
 		}
 
@@ -933,7 +909,7 @@ static int process_eb(struct ubi_device *ubi, struct ubi_scan_info *si,
 		    ubi->image_seq != image_seq) {
 			ubi_err("bad image sequence number %d in PEB %d, "
 				"expected %d", image_seq, pnum, ubi->image_seq);
-			ubi_dbg_dump_ec_hdr(ech);
+			ubi_dump_ec_hdr(ech);
 			return -EINVAL;
 		}
 	}
@@ -957,7 +933,7 @@ static int process_eb(struct ubi_device *ubi, struct ubi_scan_info *si,
 			 * PEB, bit it is not marked as bad yet. This may also
 			 * be a result of power cut during erasure.
 			 */
-			si->maybe_bad_peb_count += 1;
+			ai->maybe_bad_peb_count += 1;
 	case UBI_IO_BAD_HDR:
 		if (ec_err)
 			/*
@@ -984,23 +960,27 @@ static int process_eb(struct ubi_device *ubi, struct ubi_scan_info *si,
 			return err;
 		else if (!err)
 			/* This corruption is caused by a power cut */
-			err = add_to_list(si, pnum, ec, 1, &si->erase);
+			err = add_to_list(ai, pnum, UBI_UNKNOWN,
+					  UBI_UNKNOWN, ec, 1, &ai->erase);
 		else
 			/* This is an unexpected corruption */
-			err = add_corrupted(si, pnum, ec);
+			err = add_corrupted(ai, pnum, ec);
 		if (err)
 			return err;
 		goto adjust_mean_ec;
 	case UBI_IO_FF_BITFLIPS:
-		err = add_to_list(si, pnum, ec, 1, &si->erase);
+		err = add_to_list(ai, pnum, UBI_UNKNOWN, UBI_UNKNOWN,
+				  ec, 1, &ai->erase);
 		if (err)
 			return err;
 		goto adjust_mean_ec;
 	case UBI_IO_FF:
 		if (ec_err)
-			err = add_to_list(si, pnum, ec, 1, &si->erase);
+			err = add_to_list(ai, pnum, UBI_UNKNOWN,
+					  UBI_UNKNOWN, ec, 1, &ai->erase);
 		else
-			err = add_to_list(si, pnum, ec, 0, &si->free);
+			err = add_to_list(ai, pnum, UBI_UNKNOWN,
+					  UBI_UNKNOWN, ec, 0, &ai->free);
 		if (err)
 			return err;
 		goto adjust_mean_ec;
@@ -1019,7 +999,8 @@ static int process_eb(struct ubi_device *ubi, struct ubi_scan_info *si,
 		case UBI_COMPAT_DELETE:
 			ubi_msg("\"delete\" compatible internal volume %d:%d"
 				" found, will remove it", vol_id, lnum);
-			err = add_to_list(si, pnum, ec, 1, &si->erase);
+			err = add_to_list(ai, pnum, vol_id, lnum,
+					  ec, 1, &ai->erase);
 			if (err)
 				return err;
 			return 0;
@@ -1034,7 +1015,8 @@ static int process_eb(struct ubi_device *ubi, struct ubi_scan_info *si,
 		case UBI_COMPAT_PRESERVE:
 			ubi_msg("\"preserve\" compatible internal volume %d:%d"
 				" found", vol_id, lnum);
-			err = add_to_list(si, pnum, ec, 0, &si->alien);
+			err = add_to_list(ai, pnum, vol_id, lnum,
+					  ec, 0, &ai->alien);
 			if (err)
 				return err;
 			return 0;
@@ -1049,40 +1031,40 @@ static int process_eb(struct ubi_device *ubi, struct ubi_scan_info *si,
 	if (ec_err)
 		ubi_warn("valid VID header but corrupted EC header at PEB %d",
 			 pnum);
-	err = ubi_scan_add_used(ubi, si, pnum, ec, vidh, bitflips);
+	err = ubi_add_to_av(ubi, ai, pnum, ec, vidh, bitflips);
 	if (err)
 		return err;
 
 adjust_mean_ec:
 	if (!ec_err) {
-		si->ec_sum += ec;
-		si->ec_count += 1;
-		if (ec > si->max_ec)
-			si->max_ec = ec;
-		if (ec < si->min_ec)
-			si->min_ec = ec;
+		ai->ec_sum += ec;
+		ai->ec_count += 1;
+		if (ec > ai->max_ec)
+			ai->max_ec = ec;
+		if (ec < ai->min_ec)
+			ai->min_ec = ec;
 	}
 
 	return 0;
 }
 
 /**
- * check_what_we_have - check what PEB were found by scanning.
+ * late_analysis - analyze the overall situation with PEB.
  * @ubi: UBI device description object
- * @si: scanning information
+ * @ai: attaching information
  *
- * This is a helper function which takes a look what PEBs were found by
- * scanning, and decides whether the flash is empty and should be formatted and
- * whether there are too many corrupted PEBs and we should not attach this
- * MTD device. Returns zero if we should proceed with attaching the MTD device,
- * and %-EINVAL if we should not.
+ * This is a helper function which takes a look what PEBs we have after we
+ * gather information about all of them ("ai" is compete). It decides whether
+ * the flash is empty and should be formatted of whether there are too many
+ * corrupted PEBs and we should not attach this MTD device. Returns zero if we
+ * should proceed with attaching the MTD device, and %-EINVAL if we should not.
  */
-static int check_what_we_have(struct ubi_device *ubi, struct ubi_scan_info *si)
+static int late_analysis(struct ubi_device *ubi, struct ubi_attach_info *ai)
 {
-	struct ubi_scan_leb *seb;
+	struct ubi_ainf_peb *aeb;
 	int max_corr, peb_count;
 
-	peb_count = ubi->peb_count - si->bad_peb_count - si->alien_peb_count;
+	peb_count = ubi->peb_count - ai->bad_peb_count - ai->alien_peb_count;
 	max_corr = peb_count / 20 ?: 8;
 
 	/*
@@ -1090,25 +1072,25 @@ static int check_what_we_have(struct ubi_device *ubi, struct ubi_scan_info *si)
 	 * unclean reboots. However, many of them may indicate some problems
 	 * with the flash HW or driver.
 	 */
-	if (si->corr_peb_count) {
+	if (ai->corr_peb_count) {
 		ubi_err("%d PEBs are corrupted and preserved",
-			si->corr_peb_count);
+			ai->corr_peb_count);
 		printk(KERN_ERR "Corrupted PEBs are:");
-		list_for_each_entry(seb, &si->corr, u.list)
-			printk(KERN_CONT " %d", seb->pnum);
+		list_for_each_entry(aeb, &ai->corr, u.list)
+			printk(KERN_CONT " %d", aeb->pnum);
 		printk(KERN_CONT "\n");
 
 		/*
 		 * If too many PEBs are corrupted, we refuse attaching,
 		 * otherwise, only print a warning.
 		 */
-		if (si->corr_peb_count >= max_corr) {
+		if (ai->corr_peb_count >= max_corr) {
 			ubi_err("too many corrupted PEBs, refusing");
 			return -EINVAL;
 		}
 	}
 
-	if (si->empty_peb_count + si->maybe_bad_peb_count == peb_count) {
+	if (ai->empty_peb_count + ai->maybe_bad_peb_count == peb_count) {
 		/*
 		 * All PEBs are empty, or almost all - a couple PEBs look like
 		 * they may be bad PEBs which were not marked as bad yet.
@@ -1124,8 +1106,8 @@ static int check_what_we_have(struct ubi_device *ubi, struct ubi_scan_info *si)
 		 * 2. Flash contains non-UBI data and we do not want to format
 		 *    it and destroy possibly important information.
 		 */
-		if (si->maybe_bad_peb_count <= 2) {
-			si->is_empty = 1;
+		if (ai->maybe_bad_peb_count <= 2) {
+			ai->is_empty = 1;
 			ubi_msg("empty MTD device detected");
 			get_random_bytes(&ubi->image_seq,
 					 sizeof(ubi->image_seq));
@@ -1141,40 +1123,41 @@ static int check_what_we_have(struct ubi_device *ubi, struct ubi_scan_info *si)
 }
 
 /**
- * ubi_scan - scan an MTD device.
+ * scan_all - scan entire MTD device.
  * @ubi: UBI device description object
  *
  * This function does full scanning of an MTD device and returns complete
- * information about it. In case of failure, an error code is returned.
+ * information about it in form of a "struct ubi_attach_info" object. In case
+ * of failure, an error code is returned.
  */
-struct ubi_scan_info *ubi_scan(struct ubi_device *ubi)
+static struct ubi_attach_info *scan_all(struct ubi_device *ubi)
 {
 	int err, pnum;
 	struct rb_node *rb1, *rb2;
-	struct ubi_scan_volume *sv;
-	struct ubi_scan_leb *seb;
-	struct ubi_scan_info *si;
+	struct ubi_ainf_volume *av;
+	struct ubi_ainf_peb *aeb;
+	struct ubi_attach_info *ai;
 
-	si = kzalloc(sizeof(struct ubi_scan_info), GFP_KERNEL);
-	if (!si)
+	ai = kzalloc(sizeof(struct ubi_attach_info), GFP_KERNEL);
+	if (!ai)
 		return ERR_PTR(-ENOMEM);
 
-	INIT_LIST_HEAD(&si->corr);
-	INIT_LIST_HEAD(&si->free);
-	INIT_LIST_HEAD(&si->erase);
-	INIT_LIST_HEAD(&si->alien);
-	si->volumes = RB_ROOT;
+	INIT_LIST_HEAD(&ai->corr);
+	INIT_LIST_HEAD(&ai->free);
+	INIT_LIST_HEAD(&ai->erase);
+	INIT_LIST_HEAD(&ai->alien);
+	ai->volumes = RB_ROOT;
 
 	err = -ENOMEM;
-	si->scan_leb_slab = kmem_cache_create("ubi_scan_leb_slab",
-					      sizeof(struct ubi_scan_leb),
-					      0, 0, NULL);
-	if (!si->scan_leb_slab)
-		goto out_si;
+	ai->aeb_slab_cache = kmem_cache_create("ubi_aeb_slab_cache",
+					       sizeof(struct ubi_ainf_peb),
+					       0, 0, NULL);
+	if (!ai->aeb_slab_cache)
+		goto out_ai;
 
 	ech = kzalloc(ubi->ec_hdr_alsize, GFP_KERNEL);
 	if (!ech)
-		goto out_si;
+		goto out_ai;
 
 	vidh = ubi_zalloc_vid_hdr(ubi, GFP_KERNEL);
 	if (!vidh)
@@ -1184,7 +1167,7 @@ struct ubi_scan_info *ubi_scan(struct ubi_device *ubi)
 		cond_resched();
 
 		dbg_gen("process PEB %d", pnum);
-		err = process_eb(ubi, si, pnum);
+		err = scan_peb(ubi, ai, pnum);
 		if (err < 0)
 			goto out_vidh;
 	}
@@ -1192,10 +1175,10 @@ struct ubi_scan_info *ubi_scan(struct ubi_device *ubi)
 	dbg_msg("scanning is finished");
 
 	/* Calculate mean erase counter */
-	if (si->ec_count)
-		si->mean_ec = div_u64(si->ec_sum, si->ec_count);
+	if (ai->ec_count)
+		ai->mean_ec = div_u64(ai->ec_sum, ai->ec_count);
 
-	err = check_what_we_have(ubi, si);
+	err = late_analysis(ubi, ai);
 	if (err)
 		goto out_vidh;
 
@@ -1203,55 +1186,102 @@ struct ubi_scan_info *ubi_scan(struct ubi_device *ubi)
 	 * In case of unknown erase counter we use the mean erase counter
 	 * value.
 	 */
-	ubi_rb_for_each_entry(rb1, sv, &si->volumes, rb) {
-		ubi_rb_for_each_entry(rb2, seb, &sv->root, u.rb)
-			if (seb->ec == UBI_SCAN_UNKNOWN_EC)
-				seb->ec = si->mean_ec;
+	ubi_rb_for_each_entry(rb1, av, &ai->volumes, rb) {
+		ubi_rb_for_each_entry(rb2, aeb, &av->root, u.rb)
+			if (aeb->ec == UBI_UNKNOWN)
+				aeb->ec = ai->mean_ec;
 	}
 
-	list_for_each_entry(seb, &si->free, u.list) {
-		if (seb->ec == UBI_SCAN_UNKNOWN_EC)
-			seb->ec = si->mean_ec;
+	list_for_each_entry(aeb, &ai->free, u.list) {
+		if (aeb->ec == UBI_UNKNOWN)
+			aeb->ec = ai->mean_ec;
 	}
 
-	list_for_each_entry(seb, &si->corr, u.list)
-		if (seb->ec == UBI_SCAN_UNKNOWN_EC)
-			seb->ec = si->mean_ec;
+	list_for_each_entry(aeb, &ai->corr, u.list)
+		if (aeb->ec == UBI_UNKNOWN)
+			aeb->ec = ai->mean_ec;
 
-	list_for_each_entry(seb, &si->erase, u.list)
-		if (seb->ec == UBI_SCAN_UNKNOWN_EC)
-			seb->ec = si->mean_ec;
+	list_for_each_entry(aeb, &ai->erase, u.list)
+		if (aeb->ec == UBI_UNKNOWN)
+			aeb->ec = ai->mean_ec;
 
-	err = paranoid_check_si(ubi, si);
+	err = self_check_ai(ubi, ai);
 	if (err)
 		goto out_vidh;
 
 	ubi_free_vid_hdr(ubi, vidh);
 	kfree(ech);
 
-	return si;
+	return ai;
 
 out_vidh:
 	ubi_free_vid_hdr(ubi, vidh);
 out_ech:
 	kfree(ech);
-out_si:
-	ubi_scan_destroy_si(si);
+out_ai:
+	ubi_destroy_ai(ai);
 	return ERR_PTR(err);
 }
 
 /**
- * destroy_sv - free the scanning volume information
- * @sv: scanning volume information
- * @si: scanning information
+ * ubi_attach - attach an MTD device.
+ * @ubi: UBI device descriptor
  *
- * This function destroys the volume RB-tree (@sv->root) and the scanning
- * volume information.
+ * This function returns zero in case of success and a negative error code in
+ * case of failure.
  */
-static void destroy_sv(struct ubi_scan_info *si, struct ubi_scan_volume *sv)
+int ubi_attach(struct ubi_device *ubi)
 {
-	struct ubi_scan_leb *seb;
-	struct rb_node *this = sv->root.rb_node;
+	int err;
+	struct ubi_attach_info *ai;
+
+	ai = scan_all(ubi);
+	if (IS_ERR(ai))
+		return PTR_ERR(ai);
+
+	ubi->bad_peb_count = ai->bad_peb_count;
+	ubi->good_peb_count = ubi->peb_count - ubi->bad_peb_count;
+	ubi->corr_peb_count = ai->corr_peb_count;
+	ubi->max_ec = ai->max_ec;
+	ubi->mean_ec = ai->mean_ec;
+	ubi_msg("max. sequence number:       %llu", ai->max_sqnum);
+
+	err = ubi_read_volume_table(ubi, ai);
+	if (err)
+		goto out_ai;
+
+	err = ubi_wl_init(ubi, ai);
+	if (err)
+		goto out_vtbl;
+
+	err = ubi_eba_init(ubi, ai);
+	if (err)
+		goto out_wl;
+
+	ubi_destroy_ai(ai);
+	return 0;
+
+out_wl:
+	ubi_wl_close(ubi);
+out_vtbl:
+	ubi_free_internal_volumes(ubi);
+	vfree(ubi->vtbl);
+out_ai:
+	ubi_destroy_ai(ai);
+	return err;
+}
+
+/**
+ * destroy_av - free volume attaching information.
+ * @av: volume attaching information
+ * @ai: attaching information
+ *
+ * This function destroys the volume attaching information.
+ */
+static void destroy_av(struct ubi_attach_info *ai, struct ubi_ainf_volume *av)
+{
+	struct ubi_ainf_peb *aeb;
+	struct rb_node *this = av->root.rb_node;
 
 	while (this) {
 		if (this->rb_left)
@@ -1259,224 +1289,222 @@ static void destroy_sv(struct ubi_scan_info *si, struct ubi_scan_volume *sv)
 		else if (this->rb_right)
 			this = this->rb_right;
 		else {
-			seb = rb_entry(this, struct ubi_scan_leb, u.rb);
+			aeb = rb_entry(this, struct ubi_ainf_peb, u.rb);
 			this = rb_parent(this);
 			if (this) {
-				if (this->rb_left == &seb->u.rb)
+				if (this->rb_left == &aeb->u.rb)
 					this->rb_left = NULL;
 				else
 					this->rb_right = NULL;
 			}
 
-			kmem_cache_free(si->scan_leb_slab, seb);
+			kmem_cache_free(ai->aeb_slab_cache, aeb);
 		}
 	}
-	kfree(sv);
+	kfree(av);
 }
 
 /**
- * ubi_scan_destroy_si - destroy scanning information.
- * @si: scanning information
+ * ubi_destroy_ai - destroy attaching information.
+ * @ai: attaching information
  */
-void ubi_scan_destroy_si(struct ubi_scan_info *si)
+void ubi_destroy_ai(struct ubi_attach_info *ai)
 {
-	struct ubi_scan_leb *seb, *seb_tmp;
-	struct ubi_scan_volume *sv;
+	struct ubi_ainf_peb *aeb, *aeb_tmp;
+	struct ubi_ainf_volume *av;
 	struct rb_node *rb;
 
-	list_for_each_entry_safe(seb, seb_tmp, &si->alien, u.list) {
-		list_del(&seb->u.list);
-		kmem_cache_free(si->scan_leb_slab, seb);
+	list_for_each_entry_safe(aeb, aeb_tmp, &ai->alien, u.list) {
+		list_del(&aeb->u.list);
+		kmem_cache_free(ai->aeb_slab_cache, aeb);
 	}
-	list_for_each_entry_safe(seb, seb_tmp, &si->erase, u.list) {
-		list_del(&seb->u.list);
-		kmem_cache_free(si->scan_leb_slab, seb);
+	list_for_each_entry_safe(aeb, aeb_tmp, &ai->erase, u.list) {
+		list_del(&aeb->u.list);
+		kmem_cache_free(ai->aeb_slab_cache, aeb);
 	}
-	list_for_each_entry_safe(seb, seb_tmp, &si->corr, u.list) {
-		list_del(&seb->u.list);
-		kmem_cache_free(si->scan_leb_slab, seb);
+	list_for_each_entry_safe(aeb, aeb_tmp, &ai->corr, u.list) {
+		list_del(&aeb->u.list);
+		kmem_cache_free(ai->aeb_slab_cache, aeb);
 	}
-	list_for_each_entry_safe(seb, seb_tmp, &si->free, u.list) {
-		list_del(&seb->u.list);
-		kmem_cache_free(si->scan_leb_slab, seb);
+	list_for_each_entry_safe(aeb, aeb_tmp, &ai->free, u.list) {
+		list_del(&aeb->u.list);
+		kmem_cache_free(ai->aeb_slab_cache, aeb);
 	}
 
 	/* Destroy the volume RB-tree */
-	rb = si->volumes.rb_node;
+	rb = ai->volumes.rb_node;
 	while (rb) {
 		if (rb->rb_left)
 			rb = rb->rb_left;
 		else if (rb->rb_right)
 			rb = rb->rb_right;
 		else {
-			sv = rb_entry(rb, struct ubi_scan_volume, rb);
+			av = rb_entry(rb, struct ubi_ainf_volume, rb);
 
 			rb = rb_parent(rb);
 			if (rb) {
-				if (rb->rb_left == &sv->rb)
+				if (rb->rb_left == &av->rb)
 					rb->rb_left = NULL;
 				else
 					rb->rb_right = NULL;
 			}
 
-			destroy_sv(si, sv);
+			destroy_av(ai, av);
 		}
 	}
 
-	if (si->scan_leb_slab)
-		kmem_cache_destroy(si->scan_leb_slab);
+	if (ai->aeb_slab_cache)
+		kmem_cache_destroy(ai->aeb_slab_cache);
 
-	kfree(si);
+	kfree(ai);
 }
 
-#ifdef CONFIG_MTD_UBI_DEBUG
-
 /**
- * paranoid_check_si - check the scanning information.
+ * self_check_ai - check the attaching information.
  * @ubi: UBI device description object
- * @si: scanning information
+ * @ai: attaching information
  *
- * This function returns zero if the scanning information is all right, and a
+ * This function returns zero if the attaching information is all right, and a
  * negative error code if not or if an error occurred.
  */
-static int paranoid_check_si(struct ubi_device *ubi, struct ubi_scan_info *si)
+static int self_check_ai(struct ubi_device *ubi, struct ubi_attach_info *ai)
 {
 	int pnum, err, vols_found = 0;
 	struct rb_node *rb1, *rb2;
-	struct ubi_scan_volume *sv;
-	struct ubi_scan_leb *seb, *last_seb;
+	struct ubi_ainf_volume *av;
+	struct ubi_ainf_peb *aeb, *last_aeb;
 	uint8_t *buf;
 
 	if (!ubi->dbg->chk_gen)
 		return 0;
 
 	/*
-	 * At first, check that scanning information is OK.
+	 * At first, check that attaching information is OK.
 	 */
-	ubi_rb_for_each_entry(rb1, sv, &si->volumes, rb) {
+	ubi_rb_for_each_entry(rb1, av, &ai->volumes, rb) {
 		int leb_count = 0;
 
 		cond_resched();
 
 		vols_found += 1;
 
-		if (si->is_empty) {
+		if (ai->is_empty) {
 			ubi_err("bad is_empty flag");
-			goto bad_sv;
+			goto bad_av;
 		}
 
-		if (sv->vol_id < 0 || sv->highest_lnum < 0 ||
-		    sv->leb_count < 0 || sv->vol_type < 0 || sv->used_ebs < 0 ||
-		    sv->data_pad < 0 || sv->last_data_size < 0) {
+		if (av->vol_id < 0 || av->highest_lnum < 0 ||
+		    av->leb_count < 0 || av->vol_type < 0 || av->used_ebs < 0 ||
+		    av->data_pad < 0 || av->last_data_size < 0) {
 			ubi_err("negative values");
-			goto bad_sv;
+			goto bad_av;
 		}
 
-		if (sv->vol_id >= UBI_MAX_VOLUMES &&
-		    sv->vol_id < UBI_INTERNAL_VOL_START) {
+		if (av->vol_id >= UBI_MAX_VOLUMES &&
+		    av->vol_id < UBI_INTERNAL_VOL_START) {
 			ubi_err("bad vol_id");
-			goto bad_sv;
+			goto bad_av;
 		}
 
-		if (sv->vol_id > si->highest_vol_id) {
+		if (av->vol_id > ai->highest_vol_id) {
 			ubi_err("highest_vol_id is %d, but vol_id %d is there",
-				si->highest_vol_id, sv->vol_id);
+				ai->highest_vol_id, av->vol_id);
 			goto out;
 		}
 
-		if (sv->vol_type != UBI_DYNAMIC_VOLUME &&
-		    sv->vol_type != UBI_STATIC_VOLUME) {
+		if (av->vol_type != UBI_DYNAMIC_VOLUME &&
+		    av->vol_type != UBI_STATIC_VOLUME) {
 			ubi_err("bad vol_type");
-			goto bad_sv;
+			goto bad_av;
 		}
 
-		if (sv->data_pad > ubi->leb_size / 2) {
+		if (av->data_pad > ubi->leb_size / 2) {
 			ubi_err("bad data_pad");
-			goto bad_sv;
+			goto bad_av;
 		}
 
-		last_seb = NULL;
-		ubi_rb_for_each_entry(rb2, seb, &sv->root, u.rb) {
+		last_aeb = NULL;
+		ubi_rb_for_each_entry(rb2, aeb, &av->root, u.rb) {
 			cond_resched();
 
-			last_seb = seb;
+			last_aeb = aeb;
 			leb_count += 1;
 
-			if (seb->pnum < 0 || seb->ec < 0) {
+			if (aeb->pnum < 0 || aeb->ec < 0) {
 				ubi_err("negative values");
-				goto bad_seb;
+				goto bad_aeb;
 			}
 
-			if (seb->ec < si->min_ec) {
-				ubi_err("bad si->min_ec (%d), %d found",
-					si->min_ec, seb->ec);
-				goto bad_seb;
+			if (aeb->ec < ai->min_ec) {
+				ubi_err("bad ai->min_ec (%d), %d found",
+					ai->min_ec, aeb->ec);
+				goto bad_aeb;
 			}
 
-			if (seb->ec > si->max_ec) {
-				ubi_err("bad si->max_ec (%d), %d found",
-					si->max_ec, seb->ec);
-				goto bad_seb;
+			if (aeb->ec > ai->max_ec) {
+				ubi_err("bad ai->max_ec (%d), %d found",
+					ai->max_ec, aeb->ec);
+				goto bad_aeb;
 			}
 
-			if (seb->pnum >= ubi->peb_count) {
+			if (aeb->pnum >= ubi->peb_count) {
 				ubi_err("too high PEB number %d, total PEBs %d",
-					seb->pnum, ubi->peb_count);
-				goto bad_seb;
+					aeb->pnum, ubi->peb_count);
+				goto bad_aeb;
 			}
 
-			if (sv->vol_type == UBI_STATIC_VOLUME) {
-				if (seb->lnum >= sv->used_ebs) {
+			if (av->vol_type == UBI_STATIC_VOLUME) {
+				if (aeb->lnum >= av->used_ebs) {
 					ubi_err("bad lnum or used_ebs");
-					goto bad_seb;
+					goto bad_aeb;
 				}
 			} else {
-				if (sv->used_ebs != 0) {
+				if (av->used_ebs != 0) {
 					ubi_err("non-zero used_ebs");
-					goto bad_seb;
+					goto bad_aeb;
 				}
 			}
 
-			if (seb->lnum > sv->highest_lnum) {
+			if (aeb->lnum > av->highest_lnum) {
 				ubi_err("incorrect highest_lnum or lnum");
-				goto bad_seb;
+				goto bad_aeb;
 			}
 		}
 
-		if (sv->leb_count != leb_count) {
+		if (av->leb_count != leb_count) {
 			ubi_err("bad leb_count, %d objects in the tree",
 				leb_count);
-			goto bad_sv;
+			goto bad_av;
 		}
 
-		if (!last_seb)
+		if (!last_aeb)
 			continue;
 
-		seb = last_seb;
+		aeb = last_aeb;
 
-		if (seb->lnum != sv->highest_lnum) {
+		if (aeb->lnum != av->highest_lnum) {
 			ubi_err("bad highest_lnum");
-			goto bad_seb;
+			goto bad_aeb;
 		}
 	}
 
-	if (vols_found != si->vols_found) {
-		ubi_err("bad si->vols_found %d, should be %d",
-			si->vols_found, vols_found);
+	if (vols_found != ai->vols_found) {
+		ubi_err("bad ai->vols_found %d, should be %d",
+			ai->vols_found, vols_found);
 		goto out;
 	}
 
-	/* Check that scanning information is correct */
-	ubi_rb_for_each_entry(rb1, sv, &si->volumes, rb) {
-		last_seb = NULL;
-		ubi_rb_for_each_entry(rb2, seb, &sv->root, u.rb) {
+	/* Check that attaching information is correct */
+	ubi_rb_for_each_entry(rb1, av, &ai->volumes, rb) {
+		last_aeb = NULL;
+		ubi_rb_for_each_entry(rb2, aeb, &av->root, u.rb) {
 			int vol_type;
 
 			cond_resched();
 
-			last_seb = seb;
+			last_aeb = aeb;
 
-			err = ubi_io_read_vid_hdr(ubi, seb->pnum, vidh, 1);
+			err = ubi_io_read_vid_hdr(ubi, aeb->pnum, vidh, 1);
 			if (err && err != UBI_IO_BITFLIPS) {
 				ubi_err("VID header is not OK (%d)", err);
 				if (err > 0)
@@ -1486,52 +1514,52 @@ static int paranoid_check_si(struct ubi_device *ubi, struct ubi_scan_info *si)
 
 			vol_type = vidh->vol_type == UBI_VID_DYNAMIC ?
 				   UBI_DYNAMIC_VOLUME : UBI_STATIC_VOLUME;
-			if (sv->vol_type != vol_type) {
+			if (av->vol_type != vol_type) {
 				ubi_err("bad vol_type");
 				goto bad_vid_hdr;
 			}
 
-			if (seb->sqnum != be64_to_cpu(vidh->sqnum)) {
-				ubi_err("bad sqnum %llu", seb->sqnum);
+			if (aeb->sqnum != be64_to_cpu(vidh->sqnum)) {
+				ubi_err("bad sqnum %llu", aeb->sqnum);
 				goto bad_vid_hdr;
 			}
 
-			if (sv->vol_id != be32_to_cpu(vidh->vol_id)) {
-				ubi_err("bad vol_id %d", sv->vol_id);
+			if (av->vol_id != be32_to_cpu(vidh->vol_id)) {
+				ubi_err("bad vol_id %d", av->vol_id);
 				goto bad_vid_hdr;
 			}
 
-			if (sv->compat != vidh->compat) {
+			if (av->compat != vidh->compat) {
 				ubi_err("bad compat %d", vidh->compat);
 				goto bad_vid_hdr;
 			}
 
-			if (seb->lnum != be32_to_cpu(vidh->lnum)) {
-				ubi_err("bad lnum %d", seb->lnum);
+			if (aeb->lnum != be32_to_cpu(vidh->lnum)) {
+				ubi_err("bad lnum %d", aeb->lnum);
 				goto bad_vid_hdr;
 			}
 
-			if (sv->used_ebs != be32_to_cpu(vidh->used_ebs)) {
-				ubi_err("bad used_ebs %d", sv->used_ebs);
+			if (av->used_ebs != be32_to_cpu(vidh->used_ebs)) {
+				ubi_err("bad used_ebs %d", av->used_ebs);
 				goto bad_vid_hdr;
 			}
 
-			if (sv->data_pad != be32_to_cpu(vidh->data_pad)) {
-				ubi_err("bad data_pad %d", sv->data_pad);
+			if (av->data_pad != be32_to_cpu(vidh->data_pad)) {
+				ubi_err("bad data_pad %d", av->data_pad);
 				goto bad_vid_hdr;
 			}
 		}
 
-		if (!last_seb)
+		if (!last_aeb)
 			continue;
 
-		if (sv->highest_lnum != be32_to_cpu(vidh->lnum)) {
-			ubi_err("bad highest_lnum %d", sv->highest_lnum);
+		if (av->highest_lnum != be32_to_cpu(vidh->lnum)) {
+			ubi_err("bad highest_lnum %d", av->highest_lnum);
 			goto bad_vid_hdr;
 		}
 
-		if (sv->last_data_size != be32_to_cpu(vidh->data_size)) {
-			ubi_err("bad last_data_size %d", sv->last_data_size);
+		if (av->last_data_size != be32_to_cpu(vidh->data_size)) {
+			ubi_err("bad last_data_size %d", av->last_data_size);
 			goto bad_vid_hdr;
 		}
 	}
@@ -1553,21 +1581,21 @@ static int paranoid_check_si(struct ubi_device *ubi, struct ubi_scan_info *si)
 			buf[pnum] = 1;
 	}
 
-	ubi_rb_for_each_entry(rb1, sv, &si->volumes, rb)
-		ubi_rb_for_each_entry(rb2, seb, &sv->root, u.rb)
-			buf[seb->pnum] = 1;
+	ubi_rb_for_each_entry(rb1, av, &ai->volumes, rb)
+		ubi_rb_for_each_entry(rb2, aeb, &av->root, u.rb)
+			buf[aeb->pnum] = 1;
 
-	list_for_each_entry(seb, &si->free, u.list)
-		buf[seb->pnum] = 1;
+	list_for_each_entry(aeb, &ai->free, u.list)
+		buf[aeb->pnum] = 1;
 
-	list_for_each_entry(seb, &si->corr, u.list)
-		buf[seb->pnum] = 1;
+	list_for_each_entry(aeb, &ai->corr, u.list)
+		buf[aeb->pnum] = 1;
 
-	list_for_each_entry(seb, &si->erase, u.list)
-		buf[seb->pnum] = 1;
+	list_for_each_entry(aeb, &ai->erase, u.list)
+		buf[aeb->pnum] = 1;
 
-	list_for_each_entry(seb, &si->alien, u.list)
-		buf[seb->pnum] = 1;
+	list_for_each_entry(aeb, &ai->alien, u.list)
+		buf[aeb->pnum] = 1;
 
 	err = 0;
 	for (pnum = 0; pnum < ubi->peb_count; pnum++)
@@ -1581,25 +1609,23 @@ static int paranoid_check_si(struct ubi_device *ubi, struct ubi_scan_info *si)
 		goto out;
 	return 0;
 
-bad_seb:
-	ubi_err("bad scanning information about LEB %d", seb->lnum);
-	ubi_dbg_dump_seb(seb, 0);
-	ubi_dbg_dump_sv(sv);
+bad_aeb:
+	ubi_err("bad attaching information about LEB %d", aeb->lnum);
+	ubi_dump_aeb(aeb, 0);
+	ubi_dump_av(av);
 	goto out;
 
-bad_sv:
-	ubi_err("bad scanning information about volume %d", sv->vol_id);
-	ubi_dbg_dump_sv(sv);
+bad_av:
+	ubi_err("bad attaching information about volume %d", av->vol_id);
+	ubi_dump_av(av);
 	goto out;
 
 bad_vid_hdr:
-	ubi_err("bad scanning information about volume %d", sv->vol_id);
-	ubi_dbg_dump_sv(sv);
-	ubi_dbg_dump_vid_hdr(vidh);
+	ubi_err("bad attaching information about volume %d", av->vol_id);
+	ubi_dump_av(av);
+	ubi_dump_vid_hdr(vidh);
 
 out:
-	ubi_dbg_dump_stack();
+	dump_stack();
 	return -EINVAL;
 }
-
-#endif /* CONFIG_MTD_UBI_DEBUG */
diff --git a/drivers/mtd/ubi/build.c b/drivers/mtd/ubi/build.c
index 0fde9fc7d2e..2c5ed5ca9c3 100644
--- a/drivers/mtd/ubi/build.c
+++ b/drivers/mtd/ubi/build.c
@@ -27,10 +27,6 @@
  * module load parameters or the kernel boot parameters. If MTD devices were
  * specified, UBI does not attach any MTD device, but it is possible to do
  * later using the "UBI control device".
- *
- * At the moment we only attach UBI devices by scanning, which will become a
- * bottleneck when flashes reach certain large size. Then one may improve UBI
- * and add other methods, although it does not seem to be easy to do.
  */
 
 #include <linux/err.h>
@@ -554,10 +550,10 @@ static void uif_close(struct ubi_device *ubi)
 }
 
 /**
- * free_internal_volumes - free internal volumes.
+ * ubi_free_internal_volumes - free internal volumes.
  * @ubi: UBI device description object
  */
-static void free_internal_volumes(struct ubi_device *ubi)
+void ubi_free_internal_volumes(struct ubi_device *ubi)
 {
 	int i;
 
@@ -569,59 +565,6 @@ static void free_internal_volumes(struct ubi_device *ubi)
 }
 
 /**
- * attach_by_scanning - attach an MTD device using scanning method.
- * @ubi: UBI device descriptor
- *
- * This function returns zero in case of success and a negative error code in
- * case of failure.
- *
- * Note, currently this is the only method to attach UBI devices. Hopefully in
- * the future we'll have more scalable attaching methods and avoid full media
- * scanning. But even in this case scanning will be needed as a fall-back
- * attaching method if there are some on-flash table corruptions.
- */
-static int attach_by_scanning(struct ubi_device *ubi)
-{
-	int err;
-	struct ubi_scan_info *si;
-
-	si = ubi_scan(ubi);
-	if (IS_ERR(si))
-		return PTR_ERR(si);
-
-	ubi->bad_peb_count = si->bad_peb_count;
-	ubi->good_peb_count = ubi->peb_count - ubi->bad_peb_count;
-	ubi->corr_peb_count = si->corr_peb_count;
-	ubi->max_ec = si->max_ec;
-	ubi->mean_ec = si->mean_ec;
-	ubi_msg("max. sequence number:       %llu", si->max_sqnum);
-
-	err = ubi_read_volume_table(ubi, si);
-	if (err)
-		goto out_si;
-
-	err = ubi_wl_init_scan(ubi, si);
-	if (err)
-		goto out_vtbl;
-
-	err = ubi_eba_init_scan(ubi, si);
-	if (err)
-		goto out_wl;
-
-	ubi_scan_destroy_si(si);
-	return 0;
-
-out_wl:
-	ubi_wl_close(ubi);
-out_vtbl:
-	free_internal_volumes(ubi);
-	vfree(ubi->vtbl);
-out_si:
-	ubi_scan_destroy_si(si);
-	return err;
-}
-
-/**
  * io_init - initialize I/O sub-system for a given UBI device.
  * @ubi: UBI device description object
  *
@@ -790,11 +733,11 @@ static int io_init(struct ubi_device *ubi)
 	ubi_msg("data offset:                %d", ubi->leb_start);
 
 	/*
-	 * Note, ideally, we have to initialize ubi->bad_peb_count here. But
+	 * Note, ideally, we have to initialize @ubi->bad_peb_count here. But
 	 * unfortunately, MTD does not provide this information. We should loop
 	 * over all physical eraseblocks and invoke mtd->block_is_bad() for
-	 * each physical eraseblock. So, we skip ubi->bad_peb_count
-	 * uninitialized and initialize it after scanning.
+	 * each physical eraseblock. So, we leave @ubi->bad_peb_count
+	 * uninitialized so far.
 	 */
 
 	return 0;
@@ -805,7 +748,7 @@ static int io_init(struct ubi_device *ubi)
  * @ubi: UBI device description object
  * @vol_id: ID of the volume to re-size
  *
- * This function re-sizes the volume marked by the @UBI_VTBL_AUTORESIZE_FLG in
+ * This function re-sizes the volume marked by the %UBI_VTBL_AUTORESIZE_FLG in
  * the volume table to the largest possible size. See comments in ubi-header.h
  * for more description of the flag. Returns zero in case of success and a
  * negative error code in case of failure.
@@ -881,7 +824,7 @@ int ubi_attach_mtd_dev(struct mtd_info *mtd, int ubi_num, int vid_hdr_offset)
 	for (i = 0; i < UBI_MAX_DEVICES; i++) {
 		ubi = ubi_devices[i];
 		if (ubi && mtd->index == ubi->mtd->index) {
-			dbg_err("mtd%d is already attached to ubi%d",
+			ubi_err("mtd%d is already attached to ubi%d",
 				mtd->index, i);
 			return -EEXIST;
 		}
@@ -907,7 +850,7 @@ int ubi_attach_mtd_dev(struct mtd_info *mtd, int ubi_num, int vid_hdr_offset)
 			if (!ubi_devices[ubi_num])
 				break;
 		if (ubi_num == UBI_MAX_DEVICES) {
-			dbg_err("only %d UBI devices may be created",
+			ubi_err("only %d UBI devices may be created",
 				UBI_MAX_DEVICES);
 			return -ENFILE;
 		}
@@ -917,7 +860,7 @@ int ubi_attach_mtd_dev(struct mtd_info *mtd, int ubi_num, int vid_hdr_offset)
 
 		/* Make sure ubi_num is not busy */
 		if (ubi_devices[ubi_num]) {
-			dbg_err("ubi%d already exists", ubi_num);
+			ubi_err("ubi%d already exists", ubi_num);
 			return -EEXIST;
 		}
 	}
@@ -937,7 +880,7 @@ int ubi_attach_mtd_dev(struct mtd_info *mtd, int ubi_num, int vid_hdr_offset)
 	spin_lock_init(&ubi->volumes_lock);
 
 	ubi_msg("attaching mtd%d to ubi%d", mtd->index, ubi_num);
-	dbg_msg("sizeof(struct ubi_scan_leb) %zu", sizeof(struct ubi_scan_leb));
+	dbg_msg("sizeof(struct ubi_ainf_peb) %zu", sizeof(struct ubi_ainf_peb));
 	dbg_msg("sizeof(struct ubi_wl_entry) %zu", sizeof(struct ubi_wl_entry));
 
 	err = io_init(ubi);
@@ -953,9 +896,9 @@ int ubi_attach_mtd_dev(struct mtd_info *mtd, int ubi_num, int vid_hdr_offset)
 	if (err)
 		goto out_free;
 
-	err = attach_by_scanning(ubi);
+	err = ubi_attach(ubi);
 	if (err) {
-		dbg_err("failed to attach by scanning, error %d", err);
+		ubi_err("failed to attach mtd%d, error %d", mtd->index, err);
 		goto out_debugging;
 	}
 
@@ -1020,7 +963,7 @@ out_uif:
 	uif_close(ubi);
 out_detach:
 	ubi_wl_close(ubi);
-	free_internal_volumes(ubi);
+	ubi_free_internal_volumes(ubi);
 	vfree(ubi->vtbl);
 out_debugging:
 	ubi_debugging_exit_dev(ubi);
@@ -1092,7 +1035,7 @@ int ubi_detach_mtd_dev(int ubi_num, int anyway)
 	ubi_debugfs_exit_dev(ubi);
 	uif_close(ubi);
 	ubi_wl_close(ubi);
-	free_internal_volumes(ubi);
+	ubi_free_internal_volumes(ubi);
 	vfree(ubi->vtbl);
 	put_mtd_device(ubi->mtd);
 	ubi_debugging_exit_dev(ubi);
diff --git a/drivers/mtd/ubi/cdev.c b/drivers/mtd/ubi/cdev.c
index ad76592fb2f..acec85deb6a 100644
--- a/drivers/mtd/ubi/cdev.c
+++ b/drivers/mtd/ubi/cdev.c
@@ -63,7 +63,7 @@ static int get_exclusive(struct ubi_volume_desc *desc)
 	users = vol->readers + vol->writers + vol->exclusive;
 	ubi_assert(users > 0);
 	if (users > 1) {
-		dbg_err("%d users for volume %d", users, vol->vol_id);
+		ubi_err("%d users for volume %d", users, vol->vol_id);
 		err = -EBUSY;
 	} else {
 		vol->readers = vol->writers = 0;
@@ -159,7 +159,7 @@ static loff_t vol_cdev_llseek(struct file *file, loff_t offset, int origin)
 
 	if (vol->updating) {
 		/* Update is in progress, seeking is prohibited */
-		dbg_err("updating");
+		ubi_err("updating");
 		return -EBUSY;
 	}
 
@@ -178,7 +178,7 @@ static loff_t vol_cdev_llseek(struct file *file, loff_t offset, int origin)
 	}
 
 	if (new_offset < 0 || new_offset > vol->used_bytes) {
-		dbg_err("bad seek %lld", new_offset);
+		ubi_err("bad seek %lld", new_offset);
 		return -EINVAL;
 	}
 
@@ -216,11 +216,11 @@ static ssize_t vol_cdev_read(struct file *file, __user char *buf, size_t count,
 		count, *offp, vol->vol_id);
 
 	if (vol->updating) {
-		dbg_err("updating");
+		ubi_err("updating");
 		return -EBUSY;
 	}
 	if (vol->upd_marker) {
-		dbg_err("damaged volume, update marker is set");
+		ubi_err("damaged volume, update marker is set");
 		return -EBADF;
 	}
 	if (*offp == vol->used_bytes || count == 0)
@@ -300,7 +300,7 @@ static ssize_t vol_cdev_direct_write(struct file *file, const char __user *buf,
 
 	lnum = div_u64_rem(*offp, vol->usable_leb_size, &off);
 	if (off & (ubi->min_io_size - 1)) {
-		dbg_err("unaligned position");
+		ubi_err("unaligned position");
 		return -EINVAL;
 	}
 
@@ -309,7 +309,7 @@ static ssize_t vol_cdev_direct_write(struct file *file, const char __user *buf,
 
 	/* We can write only in fractions of the minimum I/O unit */
 	if (count & (ubi->min_io_size - 1)) {
-		dbg_err("unaligned write length");
+		ubi_err("unaligned write length");
 		return -EINVAL;
 	}
 
@@ -334,8 +334,7 @@ static ssize_t vol_cdev_direct_write(struct file *file, const char __user *buf,
 			break;
 		}
 
-		err = ubi_eba_write_leb(ubi, vol, lnum, tbuf, off, len,
-					UBI_UNKNOWN);
+		err = ubi_eba_write_leb(ubi, vol, lnum, tbuf, off, len);
 		if (err)
 			break;
 
@@ -477,9 +476,6 @@ static long vol_cdev_ioctl(struct file *file, unsigned int cmd,
 		if (req.lnum < 0 || req.lnum >= vol->reserved_pebs ||
 		    req.bytes < 0 || req.lnum >= vol->usable_leb_size)
 			break;
-		if (req.dtype != UBI_LONGTERM && req.dtype != UBI_SHORTTERM &&
-		    req.dtype != UBI_UNKNOWN)
-			break;
 
 		err = get_exclusive(desc);
 		if (err < 0)
@@ -518,7 +514,7 @@ static long vol_cdev_ioctl(struct file *file, unsigned int cmd,
 		if (err)
 			break;
 
-		err = ubi_wl_flush(ubi);
+		err = ubi_wl_flush(ubi, UBI_ALL, UBI_ALL);
 		break;
 	}
 
@@ -532,7 +528,7 @@ static long vol_cdev_ioctl(struct file *file, unsigned int cmd,
 			err = -EFAULT;
 			break;
 		}
-		err = ubi_leb_map(desc, req.lnum, req.dtype);
+		err = ubi_leb_map(desc, req.lnum);
 		break;
 	}
 
@@ -647,8 +643,8 @@ static int verify_mkvol_req(const struct ubi_device *ubi,
 	return 0;
 
 bad:
-	dbg_err("bad volume creation request");
-	ubi_dbg_dump_mkvol_req(req);
+	ubi_err("bad volume creation request");
+	ubi_dump_mkvol_req(req);
 	return err;
 }
 
@@ -713,12 +709,12 @@ static int rename_volumes(struct ubi_device *ubi,
 	for (i = 0; i < req->count - 1; i++) {
 		for (n = i + 1; n < req->count; n++) {
 			if (req->ents[i].vol_id == req->ents[n].vol_id) {
-				dbg_err("duplicated volume id %d",
+				ubi_err("duplicated volume id %d",
 					req->ents[i].vol_id);
 				return -EINVAL;
 			}
 			if (!strcmp(req->ents[i].name, req->ents[n].name)) {
-				dbg_err("duplicated volume name \"%s\"",
+				ubi_err("duplicated volume name \"%s\"",
 					req->ents[i].name);
 				return -EINVAL;
 			}
@@ -741,7 +737,7 @@ static int rename_volumes(struct ubi_device *ubi,
 		re->desc = ubi_open_volume(ubi->ubi_num, vol_id, UBI_EXCLUSIVE);
 		if (IS_ERR(re->desc)) {
 			err = PTR_ERR(re->desc);
-			dbg_err("cannot open volume %d, error %d", vol_id, err);
+			ubi_err("cannot open volume %d, error %d", vol_id, err);
 			kfree(re);
 			goto out_free;
 		}
@@ -800,7 +796,7 @@ static int rename_volumes(struct ubi_device *ubi,
 				continue;
 
 			/* The volume exists but busy, or an error occurred */
-			dbg_err("cannot open volume \"%s\", error %d",
+			ubi_err("cannot open volume \"%s\", error %d",
 				re->new_name, err);
 			goto out_free;
 		}
diff --git a/drivers/mtd/ubi/debug.c b/drivers/mtd/ubi/debug.c
index 61af9bb560a..9f957c2d48e 100644
--- a/drivers/mtd/ubi/debug.c
+++ b/drivers/mtd/ubi/debug.c
@@ -18,24 +18,49 @@
  * Author: Artem Bityutskiy (Битюцкий Артём)
  */
 
-/*
- * Here we keep all the UBI debugging stuff which should normally be disabled
- * and compiled-out, but it is extremely helpful when hunting bugs or doing big
- * changes.
- */
-
-#ifdef CONFIG_MTD_UBI_DEBUG
-
 #include "ubi.h"
 #include <linux/debugfs.h>
 #include <linux/uaccess.h>
 #include <linux/module.h>
 
+
+/**
+ * ubi_dump_flash - dump a region of flash.
+ * @ubi: UBI device description object
+ * @pnum: the physical eraseblock number to dump
+ * @offset: the starting offset within the physical eraseblock to dump
+ * @len: the length of the region to dump
+ */
+void ubi_dump_flash(struct ubi_device *ubi, int pnum, int offset, int len)
+{
+	int err;
+	size_t read;
+	void *buf;
+	loff_t addr = (loff_t)pnum * ubi->peb_size + offset;
+
+	buf = vmalloc(len);
+	if (!buf)
+		return;
+	err = mtd_read(ubi->mtd, addr, len, &read, buf);
+	if (err && err != -EUCLEAN) {
+		ubi_err("error %d while reading %d bytes from PEB %d:%d, "
+			"read %zd bytes", err, len, pnum, offset, read);
+		goto out;
+	}
+
+	ubi_msg("dumping %d bytes of data from PEB %d, offset %d",
+		len, pnum, offset);
+	print_hex_dump(KERN_DEBUG, "", DUMP_PREFIX_OFFSET, 32, 1, buf, len, 1);
+out:
+	vfree(buf);
+	return;
+}
+
 /**
- * ubi_dbg_dump_ec_hdr - dump an erase counter header.
+ * ubi_dump_ec_hdr - dump an erase counter header.
  * @ec_hdr: the erase counter header to dump
  */
-void ubi_dbg_dump_ec_hdr(const struct ubi_ec_hdr *ec_hdr)
+void ubi_dump_ec_hdr(const struct ubi_ec_hdr *ec_hdr)
 {
 	printk(KERN_DEBUG "Erase counter header dump:\n");
 	printk(KERN_DEBUG "\tmagic          %#08x\n",
@@ -57,10 +82,10 @@ void ubi_dbg_dump_ec_hdr(const struct ubi_ec_hdr *ec_hdr)
 }
 
 /**
- * ubi_dbg_dump_vid_hdr - dump a volume identifier header.
+ * ubi_dump_vid_hdr - dump a volume identifier header.
  * @vid_hdr: the volume identifier header to dump
  */
-void ubi_dbg_dump_vid_hdr(const struct ubi_vid_hdr *vid_hdr)
+void ubi_dump_vid_hdr(const struct ubi_vid_hdr *vid_hdr)
 {
 	printk(KERN_DEBUG "Volume identifier header dump:\n");
 	printk(KERN_DEBUG "\tmagic     %08x\n", be32_to_cpu(vid_hdr->magic));
@@ -82,10 +107,10 @@ void ubi_dbg_dump_vid_hdr(const struct ubi_vid_hdr *vid_hdr)
 }
 
 /**
- * ubi_dbg_dump_vol_info- dump volume information.
+ * ubi_dump_vol_info - dump volume information.
  * @vol: UBI volume description object
  */
-void ubi_dbg_dump_vol_info(const struct ubi_volume *vol)
+void ubi_dump_vol_info(const struct ubi_volume *vol)
 {
 	printk(KERN_DEBUG "Volume information dump:\n");
 	printk(KERN_DEBUG "\tvol_id          %d\n", vol->vol_id);
@@ -112,11 +137,11 @@ void ubi_dbg_dump_vol_info(const struct ubi_volume *vol)
 }
 
 /**
- * ubi_dbg_dump_vtbl_record - dump a &struct ubi_vtbl_record object.
+ * ubi_dump_vtbl_record - dump a &struct ubi_vtbl_record object.
  * @r: the object to dump
  * @idx: volume table index
  */
-void ubi_dbg_dump_vtbl_record(const struct ubi_vtbl_record *r, int idx)
+void ubi_dump_vtbl_record(const struct ubi_vtbl_record *r, int idx)
 {
 	int name_len = be16_to_cpu(r->name_len);
 
@@ -146,44 +171,44 @@ void ubi_dbg_dump_vtbl_record(const struct ubi_vtbl_record *r, int idx)
 }
 
 /**
- * ubi_dbg_dump_sv - dump a &struct ubi_scan_volume object.
- * @sv: the object to dump
+ * ubi_dump_av - dump a &struct ubi_ainf_volume object.
+ * @av: the object to dump
  */
-void ubi_dbg_dump_sv(const struct ubi_scan_volume *sv)
+void ubi_dump_av(const struct ubi_ainf_volume *av)
 {
-	printk(KERN_DEBUG "Volume scanning information dump:\n");
-	printk(KERN_DEBUG "\tvol_id         %d\n", sv->vol_id);
-	printk(KERN_DEBUG "\thighest_lnum   %d\n", sv->highest_lnum);
-	printk(KERN_DEBUG "\tleb_count      %d\n", sv->leb_count);
-	printk(KERN_DEBUG "\tcompat         %d\n", sv->compat);
-	printk(KERN_DEBUG "\tvol_type       %d\n", sv->vol_type);
-	printk(KERN_DEBUG "\tused_ebs       %d\n", sv->used_ebs);
-	printk(KERN_DEBUG "\tlast_data_size %d\n", sv->last_data_size);
-	printk(KERN_DEBUG "\tdata_pad       %d\n", sv->data_pad);
+	printk(KERN_DEBUG "Volume attaching information dump:\n");
+	printk(KERN_DEBUG "\tvol_id         %d\n", av->vol_id);
+	printk(KERN_DEBUG "\thighest_lnum   %d\n", av->highest_lnum);
+	printk(KERN_DEBUG "\tleb_count      %d\n", av->leb_count);
+	printk(KERN_DEBUG "\tcompat         %d\n", av->compat);
+	printk(KERN_DEBUG "\tvol_type       %d\n", av->vol_type);
+	printk(KERN_DEBUG "\tused_ebs       %d\n", av->used_ebs);
+	printk(KERN_DEBUG "\tlast_data_size %d\n", av->last_data_size);
+	printk(KERN_DEBUG "\tdata_pad       %d\n", av->data_pad);
 }
 
 /**
- * ubi_dbg_dump_seb - dump a &struct ubi_scan_leb object.
- * @seb: the object to dump
+ * ubi_dump_aeb - dump a &struct ubi_ainf_peb object.
+ * @aeb: the object to dump
  * @type: object type: 0 - not corrupted, 1 - corrupted
  */
-void ubi_dbg_dump_seb(const struct ubi_scan_leb *seb, int type)
+void ubi_dump_aeb(const struct ubi_ainf_peb *aeb, int type)
 {
-	printk(KERN_DEBUG "eraseblock scanning information dump:\n");
-	printk(KERN_DEBUG "\tec       %d\n", seb->ec);
-	printk(KERN_DEBUG "\tpnum     %d\n", seb->pnum);
+	printk(KERN_DEBUG "eraseblock attaching information dump:\n");
+	printk(KERN_DEBUG "\tec       %d\n", aeb->ec);
+	printk(KERN_DEBUG "\tpnum     %d\n", aeb->pnum);
 	if (type == 0) {
-		printk(KERN_DEBUG "\tlnum     %d\n", seb->lnum);
-		printk(KERN_DEBUG "\tscrub    %d\n", seb->scrub);
-		printk(KERN_DEBUG "\tsqnum    %llu\n", seb->sqnum);
+		printk(KERN_DEBUG "\tlnum     %d\n", aeb->lnum);
+		printk(KERN_DEBUG "\tscrub    %d\n", aeb->scrub);
+		printk(KERN_DEBUG "\tsqnum    %llu\n", aeb->sqnum);
 	}
 }
 
 /**
- * ubi_dbg_dump_mkvol_req - dump a &struct ubi_mkvol_req object.
+ * ubi_dump_mkvol_req - dump a &struct ubi_mkvol_req object.
  * @req: the object to dump
  */
-void ubi_dbg_dump_mkvol_req(const struct ubi_mkvol_req *req)
+void ubi_dump_mkvol_req(const struct ubi_mkvol_req *req)
 {
 	char nm[17];
 
@@ -200,38 +225,6 @@ void ubi_dbg_dump_mkvol_req(const struct ubi_mkvol_req *req)
 }
 
 /**
- * ubi_dbg_dump_flash - dump a region of flash.
- * @ubi: UBI device description object
- * @pnum: the physical eraseblock number to dump
- * @offset: the starting offset within the physical eraseblock to dump
- * @len: the length of the region to dump
- */
-void ubi_dbg_dump_flash(struct ubi_device *ubi, int pnum, int offset, int len)
-{
-	int err;
-	size_t read;
-	void *buf;
-	loff_t addr = (loff_t)pnum * ubi->peb_size + offset;
-
-	buf = vmalloc(len);
-	if (!buf)
-		return;
-	err = mtd_read(ubi->mtd, addr, len, &read, buf);
-	if (err && err != -EUCLEAN) {
-		ubi_err("error %d while reading %d bytes from PEB %d:%d, "
-			"read %zd bytes", err, len, pnum, offset, read);
-		goto out;
-	}
-
-	dbg_msg("dumping %d bytes of data from PEB %d, offset %d",
-		len, pnum, offset);
-	print_hex_dump(KERN_DEBUG, "", DUMP_PREFIX_OFFSET, 32, 1, buf, len, 1);
-out:
-	vfree(buf);
-	return;
-}
-
-/**
  * ubi_debugging_init_dev - initialize debugging for an UBI device.
  * @ubi: UBI device description object
  *
@@ -479,5 +472,3 @@ void ubi_debugfs_exit_dev(struct ubi_device *ubi)
 {
 	debugfs_remove_recursive(ubi->dbg->dfs_dir);
 }
-
-#endif /* CONFIG_MTD_UBI_DEBUG */
diff --git a/drivers/mtd/ubi/debug.h b/drivers/mtd/ubi/debug.h
index ead2cd16ba7..d5d2645b51a 100644
--- a/drivers/mtd/ubi/debug.h
+++ b/drivers/mtd/ubi/debug.h
@@ -21,21 +21,20 @@
 #ifndef __UBI_DEBUG_H__
 #define __UBI_DEBUG_H__
 
-#ifdef CONFIG_MTD_UBI_DEBUG
+void ubi_dump_flash(struct ubi_device *ubi, int pnum, int offset, int len);
+void ubi_dump_ec_hdr(const struct ubi_ec_hdr *ec_hdr);
+void ubi_dump_vid_hdr(const struct ubi_vid_hdr *vid_hdr);
+
 #include <linux/random.h>
 
 #define ubi_assert(expr)  do {                                               \
 	if (unlikely(!(expr))) {                                             \
 		printk(KERN_CRIT "UBI assert failed in %s at %u (pid %d)\n", \
 		       __func__, __LINE__, current->pid);                    \
-		ubi_dbg_dump_stack();                                        \
+		dump_stack();                                                \
 	}                                                                    \
 } while (0)
 
-#define dbg_err(fmt, ...) ubi_err(fmt, ##__VA_ARGS__)
-
-#define ubi_dbg_dump_stack() dump_stack()
-
 #define ubi_dbg_print_hex_dump(l, ps, pt, r, g, b, len, a)  \
 		print_hex_dump(l, ps, pt, r, g, b, len, a)
 
@@ -58,17 +57,13 @@
 /* Initialization and build messages */
 #define dbg_bld(fmt, ...) ubi_dbg_msg("bld", fmt, ##__VA_ARGS__)
 
-void ubi_dbg_dump_ec_hdr(const struct ubi_ec_hdr *ec_hdr);
-void ubi_dbg_dump_vid_hdr(const struct ubi_vid_hdr *vid_hdr);
-void ubi_dbg_dump_vol_info(const struct ubi_volume *vol);
-void ubi_dbg_dump_vtbl_record(const struct ubi_vtbl_record *r, int idx);
-void ubi_dbg_dump_sv(const struct ubi_scan_volume *sv);
-void ubi_dbg_dump_seb(const struct ubi_scan_leb *seb, int type);
-void ubi_dbg_dump_mkvol_req(const struct ubi_mkvol_req *req);
-void ubi_dbg_dump_flash(struct ubi_device *ubi, int pnum, int offset, int len);
-int ubi_dbg_check_all_ff(struct ubi_device *ubi, int pnum, int offset, int len);
-int ubi_dbg_check_write(struct ubi_device *ubi, const void *buf, int pnum,
-			int offset, int len);
+void ubi_dump_vol_info(const struct ubi_volume *vol);
+void ubi_dump_vtbl_record(const struct ubi_vtbl_record *r, int idx);
+void ubi_dump_av(const struct ubi_ainf_volume *av);
+void ubi_dump_aeb(const struct ubi_ainf_peb *aeb, int type);
+void ubi_dump_mkvol_req(const struct ubi_mkvol_req *req);
+int ubi_self_check_all_ff(struct ubi_device *ubi, int pnum, int offset,
+			  int len);
 int ubi_debugging_init_dev(struct ubi_device *ubi);
 void ubi_debugging_exit_dev(struct ubi_device *ubi);
 int ubi_debugfs_init(void);
@@ -167,73 +162,4 @@ static inline int ubi_dbg_is_erase_failure(const struct ubi_device *ubi)
 	return 0;
 }
 
-#else
-
-/* Use "if (0)" to make compiler check arguments even if debugging is off */
-#define ubi_assert(expr)  do {                                               \
-	if (0) {                                                             \
-		printk(KERN_CRIT "UBI assert failed in %s at %u (pid %d)\n", \
-		       __func__, __LINE__, current->pid);                    \
-	}                                                                    \
-} while (0)
-
-#define dbg_err(fmt, ...) do {                                               \
-	if (0)                                                               \
-		ubi_err(fmt, ##__VA_ARGS__);                                 \
-} while (0)
-
-#define ubi_dbg_msg(fmt, ...) do {                                           \
-	if (0)                                                               \
-		printk(KERN_DEBUG fmt "\n", ##__VA_ARGS__);                  \
-} while (0)
-
-#define dbg_msg(fmt, ...)  ubi_dbg_msg(fmt, ##__VA_ARGS__)
-#define dbg_gen(fmt, ...)  ubi_dbg_msg(fmt, ##__VA_ARGS__)
-#define dbg_eba(fmt, ...)  ubi_dbg_msg(fmt, ##__VA_ARGS__)
-#define dbg_wl(fmt, ...)   ubi_dbg_msg(fmt, ##__VA_ARGS__)
-#define dbg_io(fmt, ...)   ubi_dbg_msg(fmt, ##__VA_ARGS__)
-#define dbg_bld(fmt, ...)  ubi_dbg_msg(fmt, ##__VA_ARGS__)
-
-static inline void ubi_dbg_dump_stack(void)                          { return; }
-static inline void
-ubi_dbg_dump_ec_hdr(const struct ubi_ec_hdr *ec_hdr)                 { return; }
-static inline void
-ubi_dbg_dump_vid_hdr(const struct ubi_vid_hdr *vid_hdr)              { return; }
-static inline void
-ubi_dbg_dump_vol_info(const struct ubi_volume *vol)                  { return; }
-static inline void
-ubi_dbg_dump_vtbl_record(const struct ubi_vtbl_record *r, int idx)   { return; }
-static inline void ubi_dbg_dump_sv(const struct ubi_scan_volume *sv) { return; }
-static inline void ubi_dbg_dump_seb(const struct ubi_scan_leb *seb,
-				    int type)                        { return; }
-static inline void
-ubi_dbg_dump_mkvol_req(const struct ubi_mkvol_req *req)              { return; }
-static inline void ubi_dbg_dump_flash(struct ubi_device *ubi,
-				      int pnum, int offset, int len) { return; }
-static inline void
-ubi_dbg_print_hex_dump(const char *l, const char *ps, int pt, int r,
-		       int g, const void *b, size_t len, bool a)     { return; }
-static inline int ubi_dbg_check_all_ff(struct ubi_device *ubi,
-				       int pnum, int offset,
-				       int len)                    { return 0; }
-static inline int ubi_dbg_check_write(struct ubi_device *ubi,
-				      const void *buf, int pnum,
-				      int offset, int len)         { return 0; }
-
-static inline int ubi_debugging_init_dev(struct ubi_device *ubi)   { return 0; }
-static inline void ubi_debugging_exit_dev(struct ubi_device *ubi)  { return; }
-static inline int ubi_debugfs_init(void)                           { return 0; }
-static inline void ubi_debugfs_exit(void)                          { return; }
-static inline int ubi_debugfs_init_dev(struct ubi_device *ubi)     { return 0; }
-static inline void ubi_debugfs_exit_dev(struct ubi_device *ubi)    { return; }
-
-static inline int
-ubi_dbg_is_bgt_disabled(const struct ubi_device *ubi)              { return 0; }
-static inline int ubi_dbg_is_bitflip(const struct ubi_device *ubi) { return 0; }
-static inline int
-ubi_dbg_is_write_failure(const struct ubi_device *ubi)             { return 0; }
-static inline int
-ubi_dbg_is_erase_failure(const struct ubi_device *ubi)             { return 0; }
-
-#endif /* !CONFIG_MTD_UBI_DEBUG */
 #endif /* !__UBI_DEBUG_H__ */
diff --git a/drivers/mtd/ubi/eba.c b/drivers/mtd/ubi/eba.c
index 2455d620d96..b703ac7729c 100644
--- a/drivers/mtd/ubi/eba.c
+++ b/drivers/mtd/ubi/eba.c
@@ -341,7 +341,7 @@ int ubi_eba_unmap_leb(struct ubi_device *ubi, struct ubi_volume *vol,
 	dbg_eba("erase LEB %d:%d, PEB %d", vol_id, lnum, pnum);
 
 	vol->eba_tbl[lnum] = UBI_LEB_UNMAPPED;
-	err = ubi_wl_put_peb(ubi, pnum, 0);
+	err = ubi_wl_put_peb(ubi, vol_id, lnum, pnum, 0);
 
 out_unlock:
 	leb_write_unlock(ubi, vol_id, lnum);
@@ -507,7 +507,7 @@ static int recover_peb(struct ubi_device *ubi, int pnum, int vol_id, int lnum,
 		return -ENOMEM;
 
 retry:
-	new_pnum = ubi_wl_get_peb(ubi, UBI_UNKNOWN);
+	new_pnum = ubi_wl_get_peb(ubi);
 	if (new_pnum < 0) {
 		ubi_free_vid_hdr(ubi, vid_hdr);
 		return new_pnum;
@@ -550,7 +550,7 @@ retry:
 	ubi_free_vid_hdr(ubi, vid_hdr);
 
 	vol->eba_tbl[lnum] = new_pnum;
-	ubi_wl_put_peb(ubi, pnum, 1);
+	ubi_wl_put_peb(ubi, vol_id, lnum, pnum, 1);
 
 	ubi_msg("data was successfully recovered");
 	return 0;
@@ -558,7 +558,7 @@ retry:
 out_unlock:
 	mutex_unlock(&ubi->buf_mutex);
 out_put:
-	ubi_wl_put_peb(ubi, new_pnum, 1);
+	ubi_wl_put_peb(ubi, vol_id, lnum, new_pnum, 1);
 	ubi_free_vid_hdr(ubi, vid_hdr);
 	return err;
 
@@ -568,7 +568,7 @@ write_error:
 	 * get another one.
 	 */
 	ubi_warn("failed to write to PEB %d", new_pnum);
-	ubi_wl_put_peb(ubi, new_pnum, 1);
+	ubi_wl_put_peb(ubi, vol_id, lnum, new_pnum, 1);
 	if (++tries > UBI_IO_RETRIES) {
 		ubi_free_vid_hdr(ubi, vid_hdr);
 		return err;
@@ -585,7 +585,6 @@ write_error:
  * @buf: the data to write
  * @offset: offset within the logical eraseblock where to write
  * @len: how many bytes to write
- * @dtype: data type
  *
  * This function writes data to logical eraseblock @lnum of a dynamic volume
  * @vol. Returns zero in case of success and a negative error code in case
@@ -593,7 +592,7 @@ write_error:
  * written to the flash media, but may be some garbage.
  */
 int ubi_eba_write_leb(struct ubi_device *ubi, struct ubi_volume *vol, int lnum,
-		      const void *buf, int offset, int len, int dtype)
+		      const void *buf, int offset, int len)
 {
 	int err, pnum, tries = 0, vol_id = vol->vol_id;
 	struct ubi_vid_hdr *vid_hdr;
@@ -641,7 +640,7 @@ int ubi_eba_write_leb(struct ubi_device *ubi, struct ubi_volume *vol, int lnum,
 	vid_hdr->data_pad = cpu_to_be32(vol->data_pad);
 
 retry:
-	pnum = ubi_wl_get_peb(ubi, dtype);
+	pnum = ubi_wl_get_peb(ubi);
 	if (pnum < 0) {
 		ubi_free_vid_hdr(ubi, vid_hdr);
 		leb_write_unlock(ubi, vol_id, lnum);
@@ -687,7 +686,7 @@ write_error:
 	 * eraseblock, so just put it and request a new one. We assume that if
 	 * this physical eraseblock went bad, the erase code will handle that.
 	 */
-	err = ubi_wl_put_peb(ubi, pnum, 1);
+	err = ubi_wl_put_peb(ubi, vol_id, lnum, pnum, 1);
 	if (err || ++tries > UBI_IO_RETRIES) {
 		ubi_ro_mode(ubi);
 		leb_write_unlock(ubi, vol_id, lnum);
@@ -707,7 +706,6 @@ write_error:
  * @lnum: logical eraseblock number
  * @buf: data to write
  * @len: how many bytes to write
- * @dtype: data type
  * @used_ebs: how many logical eraseblocks will this volume contain
  *
  * This function writes data to logical eraseblock @lnum of static volume
@@ -724,8 +722,7 @@ write_error:
  * code in case of failure.
  */
 int ubi_eba_write_leb_st(struct ubi_device *ubi, struct ubi_volume *vol,
-			 int lnum, const void *buf, int len, int dtype,
-			 int used_ebs)
+			 int lnum, const void *buf, int len, int used_ebs)
 {
 	int err, pnum, tries = 0, data_size = len, vol_id = vol->vol_id;
 	struct ubi_vid_hdr *vid_hdr;
@@ -763,7 +760,7 @@ int ubi_eba_write_leb_st(struct ubi_device *ubi, struct ubi_volume *vol,
 	vid_hdr->data_crc = cpu_to_be32(crc);
 
 retry:
-	pnum = ubi_wl_get_peb(ubi, dtype);
+	pnum = ubi_wl_get_peb(ubi);
 	if (pnum < 0) {
 		ubi_free_vid_hdr(ubi, vid_hdr);
 		leb_write_unlock(ubi, vol_id, lnum);
@@ -807,7 +804,7 @@ write_error:
 		return err;
 	}
 
-	err = ubi_wl_put_peb(ubi, pnum, 1);
+	err = ubi_wl_put_peb(ubi, vol_id, lnum, pnum, 1);
 	if (err || ++tries > UBI_IO_RETRIES) {
 		ubi_ro_mode(ubi);
 		leb_write_unlock(ubi, vol_id, lnum);
@@ -827,7 +824,6 @@ write_error:
  * @lnum: logical eraseblock number
  * @buf: data to write
  * @len: how many bytes to write
- * @dtype: data type
  *
  * This function changes the contents of a logical eraseblock atomically. @buf
  * has to contain new logical eraseblock data, and @len - the length of the
@@ -839,7 +835,7 @@ write_error:
  * LEB change may be done at a time. This is ensured by @ubi->alc_mutex.
  */
 int ubi_eba_atomic_leb_change(struct ubi_device *ubi, struct ubi_volume *vol,
-			      int lnum, const void *buf, int len, int dtype)
+			      int lnum, const void *buf, int len)
 {
 	int err, pnum, tries = 0, vol_id = vol->vol_id;
 	struct ubi_vid_hdr *vid_hdr;
@@ -856,7 +852,7 @@ int ubi_eba_atomic_leb_change(struct ubi_device *ubi, struct ubi_volume *vol,
 		err = ubi_eba_unmap_leb(ubi, vol, lnum);
 		if (err)
 			return err;
-		return ubi_eba_write_leb(ubi, vol, lnum, NULL, 0, 0, dtype);
+		return ubi_eba_write_leb(ubi, vol, lnum, NULL, 0, 0);
 	}
 
 	vid_hdr = ubi_zalloc_vid_hdr(ubi, GFP_NOFS);
@@ -881,7 +877,7 @@ int ubi_eba_atomic_leb_change(struct ubi_device *ubi, struct ubi_volume *vol,
 	vid_hdr->data_crc = cpu_to_be32(crc);
 
 retry:
-	pnum = ubi_wl_get_peb(ubi, dtype);
+	pnum = ubi_wl_get_peb(ubi);
 	if (pnum < 0) {
 		err = pnum;
 		goto out_leb_unlock;
@@ -905,7 +901,7 @@ retry:
 	}
 
 	if (vol->eba_tbl[lnum] >= 0) {
-		err = ubi_wl_put_peb(ubi, vol->eba_tbl[lnum], 0);
+		err = ubi_wl_put_peb(ubi, vol_id, lnum, vol->eba_tbl[lnum], 0);
 		if (err)
 			goto out_leb_unlock;
 	}
@@ -930,7 +926,7 @@ write_error:
 		goto out_leb_unlock;
 	}
 
-	err = ubi_wl_put_peb(ubi, pnum, 1);
+	err = ubi_wl_put_peb(ubi, vol_id, lnum, pnum, 1);
 	if (err || ++tries > UBI_IO_RETRIES) {
 		ubi_ro_mode(ubi);
 		goto out_leb_unlock;
@@ -1171,7 +1167,7 @@ out_unlock_leb:
  * print_rsvd_warning - warn about not having enough reserved PEBs.
  * @ubi: UBI device description object
  *
- * This is a helper function for 'ubi_eba_init_scan()' which is called when UBI
+ * This is a helper function for 'ubi_eba_init()' which is called when UBI
  * cannot reserve enough PEBs for bad block handling. This function makes a
  * decision whether we have to print a warning or not. The algorithm is as
  * follows:
@@ -1186,13 +1182,13 @@ out_unlock_leb:
  * reported by real users.
  */
 static void print_rsvd_warning(struct ubi_device *ubi,
-			       struct ubi_scan_info *si)
+			       struct ubi_attach_info *ai)
 {
 	/*
 	 * The 1 << 18 (256KiB) number is picked randomly, just a reasonably
 	 * large number to distinguish between newly flashed and used images.
 	 */
-	if (si->max_sqnum > (1 << 18)) {
+	if (ai->max_sqnum > (1 << 18)) {
 		int min = ubi->beb_rsvd_level / 10;
 
 		if (!min)
@@ -1209,19 +1205,19 @@ static void print_rsvd_warning(struct ubi_device *ubi,
 }
 
 /**
- * ubi_eba_init_scan - initialize the EBA sub-system using scanning information.
+ * ubi_eba_init - initialize the EBA sub-system using attaching information.
  * @ubi: UBI device description object
- * @si: scanning information
+ * @ai: attaching information
  *
  * This function returns zero in case of success and a negative error code in
  * case of failure.
  */
-int ubi_eba_init_scan(struct ubi_device *ubi, struct ubi_scan_info *si)
+int ubi_eba_init(struct ubi_device *ubi, struct ubi_attach_info *ai)
 {
 	int i, j, err, num_volumes;
-	struct ubi_scan_volume *sv;
+	struct ubi_ainf_volume *av;
 	struct ubi_volume *vol;
-	struct ubi_scan_leb *seb;
+	struct ubi_ainf_peb *aeb;
 	struct rb_node *rb;
 
 	dbg_eba("initialize EBA sub-system");
@@ -1230,7 +1226,7 @@ int ubi_eba_init_scan(struct ubi_device *ubi, struct ubi_scan_info *si)
 	mutex_init(&ubi->alc_mutex);
 	ubi->ltree = RB_ROOT;
 
-	ubi->global_sqnum = si->max_sqnum + 1;
+	ubi->global_sqnum = ai->max_sqnum + 1;
 	num_volumes = ubi->vtbl_slots + UBI_INT_VOL_COUNT;
 
 	for (i = 0; i < num_volumes; i++) {
@@ -1250,18 +1246,18 @@ int ubi_eba_init_scan(struct ubi_device *ubi, struct ubi_scan_info *si)
 		for (j = 0; j < vol->reserved_pebs; j++)
 			vol->eba_tbl[j] = UBI_LEB_UNMAPPED;
 
-		sv = ubi_scan_find_sv(si, idx2vol_id(ubi, i));
-		if (!sv)
+		av = ubi_find_av(ai, idx2vol_id(ubi, i));
+		if (!av)
 			continue;
 
-		ubi_rb_for_each_entry(rb, seb, &sv->root, u.rb) {
-			if (seb->lnum >= vol->reserved_pebs)
+		ubi_rb_for_each_entry(rb, aeb, &av->root, u.rb) {
+			if (aeb->lnum >= vol->reserved_pebs)
 				/*
 				 * This may happen in case of an unclean reboot
 				 * during re-size.
 				 */
-				ubi_scan_move_to_list(sv, seb, &si->erase);
-			vol->eba_tbl[seb->lnum] = seb->pnum;
+				ubi_move_aeb_to_list(av, aeb, &ai->erase);
+			vol->eba_tbl[aeb->lnum] = aeb->pnum;
 		}
 	}
 
@@ -1283,7 +1279,7 @@ int ubi_eba_init_scan(struct ubi_device *ubi, struct ubi_scan_info *si)
 		if (ubi->avail_pebs < ubi->beb_rsvd_level) {
 			/* No enough free physical eraseblocks */
 			ubi->beb_rsvd_pebs = ubi->avail_pebs;
-			print_rsvd_warning(ubi, si);
+			print_rsvd_warning(ubi, ai);
 		} else
 			ubi->beb_rsvd_pebs = ubi->beb_rsvd_level;
 
diff --git a/drivers/mtd/ubi/gluebi.c b/drivers/mtd/ubi/gluebi.c
index 90b98822d9a..4e44bee4c56 100644
--- a/drivers/mtd/ubi/gluebi.c
+++ b/drivers/mtd/ubi/gluebi.c
@@ -227,7 +227,7 @@ static int gluebi_write(struct mtd_info *mtd, loff_t to, size_t len,
 		if (to_write > total_written)
 			to_write = total_written;
 
-		err = ubi_write(gluebi->desc, lnum, buf, offs, to_write);
+		err = ubi_leb_write(gluebi->desc, lnum, buf, offs, to_write);
 		if (err)
 			break;
 
diff --git a/drivers/mtd/ubi/io.c b/drivers/mtd/ubi/io.c
index 43f1a0011a5..a8d523794b5 100644
--- a/drivers/mtd/ubi/io.c
+++ b/drivers/mtd/ubi/io.c
@@ -91,21 +91,15 @@
 #include <linux/slab.h>
 #include "ubi.h"
 
-#ifdef CONFIG_MTD_UBI_DEBUG
-static int paranoid_check_not_bad(const struct ubi_device *ubi, int pnum);
-static int paranoid_check_peb_ec_hdr(const struct ubi_device *ubi, int pnum);
-static int paranoid_check_ec_hdr(const struct ubi_device *ubi, int pnum,
-				 const struct ubi_ec_hdr *ec_hdr);
-static int paranoid_check_peb_vid_hdr(const struct ubi_device *ubi, int pnum);
-static int paranoid_check_vid_hdr(const struct ubi_device *ubi, int pnum,
-				  const struct ubi_vid_hdr *vid_hdr);
-#else
-#define paranoid_check_not_bad(ubi, pnum) 0
-#define paranoid_check_peb_ec_hdr(ubi, pnum)  0
-#define paranoid_check_ec_hdr(ubi, pnum, ec_hdr)  0
-#define paranoid_check_peb_vid_hdr(ubi, pnum) 0
-#define paranoid_check_vid_hdr(ubi, pnum, vid_hdr) 0
-#endif
+static int self_check_not_bad(const struct ubi_device *ubi, int pnum);
+static int self_check_peb_ec_hdr(const struct ubi_device *ubi, int pnum);
+static int self_check_ec_hdr(const struct ubi_device *ubi, int pnum,
+			     const struct ubi_ec_hdr *ec_hdr);
+static int self_check_peb_vid_hdr(const struct ubi_device *ubi, int pnum);
+static int self_check_vid_hdr(const struct ubi_device *ubi, int pnum,
+			      const struct ubi_vid_hdr *vid_hdr);
+static int self_check_write(struct ubi_device *ubi, const void *buf, int pnum,
+			    int offset, int len);
 
 /**
  * ubi_io_read - read data from a physical eraseblock.
@@ -142,7 +136,7 @@ int ubi_io_read(const struct ubi_device *ubi, void *buf, int pnum, int offset,
 	ubi_assert(offset >= 0 && offset + len <= ubi->peb_size);
 	ubi_assert(len > 0);
 
-	err = paranoid_check_not_bad(ubi, pnum);
+	err = self_check_not_bad(ubi, pnum);
 	if (err)
 		return err;
 
@@ -189,16 +183,16 @@ retry:
 		}
 
 		if (retries++ < UBI_IO_RETRIES) {
-			dbg_io("error %d%s while reading %d bytes from PEB "
-			       "%d:%d, read only %zd bytes, retry",
-			       err, errstr, len, pnum, offset, read);
+			ubi_warn("error %d%s while reading %d bytes from PEB "
+				 "%d:%d, read only %zd bytes, retry",
+				 err, errstr, len, pnum, offset, read);
 			yield();
 			goto retry;
 		}
 
 		ubi_err("error %d%s while reading %d bytes from PEB %d:%d, "
 			"read %zd bytes", err, errstr, len, pnum, offset, read);
-		ubi_dbg_dump_stack();
+		dump_stack();
 
 		/*
 		 * The driver should never return -EBADMSG if it failed to read
@@ -257,14 +251,12 @@ int ubi_io_write(struct ubi_device *ubi, const void *buf, int pnum, int offset,
 		return -EROFS;
 	}
 
-	/* The below has to be compiled out if paranoid checks are disabled */
-
-	err = paranoid_check_not_bad(ubi, pnum);
+	err = self_check_not_bad(ubi, pnum);
 	if (err)
 		return err;
 
 	/* The area we are writing to has to contain all 0xFF bytes */
-	err = ubi_dbg_check_all_ff(ubi, pnum, offset, len);
+	err = ubi_self_check_all_ff(ubi, pnum, offset, len);
 	if (err)
 		return err;
 
@@ -273,18 +265,18 @@ int ubi_io_write(struct ubi_device *ubi, const void *buf, int pnum, int offset,
 		 * We write to the data area of the physical eraseblock. Make
 		 * sure it has valid EC and VID headers.
 		 */
-		err = paranoid_check_peb_ec_hdr(ubi, pnum);
+		err = self_check_peb_ec_hdr(ubi, pnum);
 		if (err)
 			return err;
-		err = paranoid_check_peb_vid_hdr(ubi, pnum);
+		err = self_check_peb_vid_hdr(ubi, pnum);
 		if (err)
 			return err;
 	}
 
 	if (ubi_dbg_is_write_failure(ubi)) {
-		dbg_err("cannot write %d bytes to PEB %d:%d "
+		ubi_err("cannot write %d bytes to PEB %d:%d "
 			"(emulated)", len, pnum, offset);
-		ubi_dbg_dump_stack();
+		dump_stack();
 		return -EIO;
 	}
 
@@ -293,13 +285,13 @@ int ubi_io_write(struct ubi_device *ubi, const void *buf, int pnum, int offset,
 	if (err) {
 		ubi_err("error %d while writing %d bytes to PEB %d:%d, written "
 			"%zd bytes", err, len, pnum, offset, written);
-		ubi_dbg_dump_stack();
-		ubi_dbg_dump_flash(ubi, pnum, offset, len);
+		dump_stack();
+		ubi_dump_flash(ubi, pnum, offset, len);
 	} else
 		ubi_assert(written == len);
 
 	if (!err) {
-		err = ubi_dbg_check_write(ubi, buf, pnum, offset, len);
+		err = self_check_write(ubi, buf, pnum, offset, len);
 		if (err)
 			return err;
 
@@ -310,7 +302,7 @@ int ubi_io_write(struct ubi_device *ubi, const void *buf, int pnum, int offset,
 		offset += len;
 		len = ubi->peb_size - offset;
 		if (len)
-			err = ubi_dbg_check_all_ff(ubi, pnum, offset, len);
+			err = ubi_self_check_all_ff(ubi, pnum, offset, len);
 	}
 
 	return err;
@@ -364,13 +356,13 @@ retry:
 	err = mtd_erase(ubi->mtd, &ei);
 	if (err) {
 		if (retries++ < UBI_IO_RETRIES) {
-			dbg_io("error %d while erasing PEB %d, retry",
-			       err, pnum);
+			ubi_warn("error %d while erasing PEB %d, retry",
+				 err, pnum);
 			yield();
 			goto retry;
 		}
 		ubi_err("cannot erase PEB %d, error %d", pnum, err);
-		ubi_dbg_dump_stack();
+		dump_stack();
 		return err;
 	}
 
@@ -383,21 +375,21 @@ retry:
 
 	if (ei.state == MTD_ERASE_FAILED) {
 		if (retries++ < UBI_IO_RETRIES) {
-			dbg_io("error while erasing PEB %d, retry", pnum);
+			ubi_warn("error while erasing PEB %d, retry", pnum);
 			yield();
 			goto retry;
 		}
 		ubi_err("cannot erase PEB %d", pnum);
-		ubi_dbg_dump_stack();
+		dump_stack();
 		return -EIO;
 	}
 
-	err = ubi_dbg_check_all_ff(ubi, pnum, 0, ubi->peb_size);
+	err = ubi_self_check_all_ff(ubi, pnum, 0, ubi->peb_size);
 	if (err)
 		return err;
 
 	if (ubi_dbg_is_erase_failure(ubi)) {
-		dbg_err("cannot erase PEB %d (emulated)", pnum);
+		ubi_err("cannot erase PEB %d (emulated)", pnum);
 		return -EIO;
 	}
 
@@ -521,8 +513,7 @@ static int nor_erase_prepare(struct ubi_device *ubi, int pnum)
 	 * It is important to first invalidate the EC header, and then the VID
 	 * header. Otherwise a power cut may lead to valid EC header and
 	 * invalid VID header, in which case UBI will treat this PEB as
-	 * corrupted and will try to preserve it, and print scary warnings (see
-	 * the header comment in scan.c for more information).
+	 * corrupted and will try to preserve it, and print scary warnings.
 	 */
 	addr = (loff_t)pnum * ubi->peb_size;
 	err = mtd_write(ubi->mtd, addr, 4, &written, (void *)&data);
@@ -563,7 +554,7 @@ static int nor_erase_prepare(struct ubi_device *ubi, int pnum)
 	 */
 	ubi_err("cannot invalidate PEB %d, write returned %d read returned %d",
 		pnum, err, err1);
-	ubi_dbg_dump_flash(ubi, pnum, 0, ubi->peb_size);
+	ubi_dump_flash(ubi, pnum, 0, ubi->peb_size);
 	return -EIO;
 }
 
@@ -589,7 +580,7 @@ int ubi_io_sync_erase(struct ubi_device *ubi, int pnum, int torture)
 
 	ubi_assert(pnum >= 0 && pnum < ubi->peb_count);
 
-	err = paranoid_check_not_bad(ubi, pnum);
+	err = self_check_not_bad(ubi, pnum);
 	if (err != 0)
 		return err;
 
@@ -721,8 +712,8 @@ static int validate_ec_hdr(const struct ubi_device *ubi,
 
 bad:
 	ubi_err("bad EC header");
-	ubi_dbg_dump_ec_hdr(ec_hdr);
-	ubi_dbg_dump_stack();
+	ubi_dump_ec_hdr(ec_hdr);
+	dump_stack();
 	return 1;
 }
 
@@ -803,7 +794,7 @@ int ubi_io_read_ec_hdr(struct ubi_device *ubi, int pnum,
 		if (verbose) {
 			ubi_warn("bad magic number at PEB %d: %08x instead of "
 				 "%08x", pnum, magic, UBI_EC_HDR_MAGIC);
-			ubi_dbg_dump_ec_hdr(ec_hdr);
+			ubi_dump_ec_hdr(ec_hdr);
 		}
 		dbg_bld("bad magic number at PEB %d: %08x instead of "
 			"%08x", pnum, magic, UBI_EC_HDR_MAGIC);
@@ -817,7 +808,7 @@ int ubi_io_read_ec_hdr(struct ubi_device *ubi, int pnum,
 		if (verbose) {
 			ubi_warn("bad EC header CRC at PEB %d, calculated "
 				 "%#08x, read %#08x", pnum, crc, hdr_crc);
-			ubi_dbg_dump_ec_hdr(ec_hdr);
+			ubi_dump_ec_hdr(ec_hdr);
 		}
 		dbg_bld("bad EC header CRC at PEB %d, calculated "
 			"%#08x, read %#08x", pnum, crc, hdr_crc);
@@ -874,7 +865,7 @@ int ubi_io_write_ec_hdr(struct ubi_device *ubi, int pnum,
 	crc = crc32(UBI_CRC32_INIT, ec_hdr, UBI_EC_HDR_SIZE_CRC);
 	ec_hdr->hdr_crc = cpu_to_be32(crc);
 
-	err = paranoid_check_ec_hdr(ubi, pnum, ec_hdr);
+	err = self_check_ec_hdr(ubi, pnum, ec_hdr);
 	if (err)
 		return err;
 
@@ -905,40 +896,40 @@ static int validate_vid_hdr(const struct ubi_device *ubi,
 	int usable_leb_size = ubi->leb_size - data_pad;
 
 	if (copy_flag != 0 && copy_flag != 1) {
-		dbg_err("bad copy_flag");
+		ubi_err("bad copy_flag");
 		goto bad;
 	}
 
 	if (vol_id < 0 || lnum < 0 || data_size < 0 || used_ebs < 0 ||
 	    data_pad < 0) {
-		dbg_err("negative values");
+		ubi_err("negative values");
 		goto bad;
 	}
 
 	if (vol_id >= UBI_MAX_VOLUMES && vol_id < UBI_INTERNAL_VOL_START) {
-		dbg_err("bad vol_id");
+		ubi_err("bad vol_id");
 		goto bad;
 	}
 
 	if (vol_id < UBI_INTERNAL_VOL_START && compat != 0) {
-		dbg_err("bad compat");
+		ubi_err("bad compat");
 		goto bad;
 	}
 
 	if (vol_id >= UBI_INTERNAL_VOL_START && compat != UBI_COMPAT_DELETE &&
 	    compat != UBI_COMPAT_RO && compat != UBI_COMPAT_PRESERVE &&
 	    compat != UBI_COMPAT_REJECT) {
-		dbg_err("bad compat");
+		ubi_err("bad compat");
 		goto bad;
 	}
 
 	if (vol_type != UBI_VID_DYNAMIC && vol_type != UBI_VID_STATIC) {
-		dbg_err("bad vol_type");
+		ubi_err("bad vol_type");
 		goto bad;
 	}
 
 	if (data_pad >= ubi->leb_size / 2) {
-		dbg_err("bad data_pad");
+		ubi_err("bad data_pad");
 		goto bad;
 	}
 
@@ -950,45 +941,45 @@ static int validate_vid_hdr(const struct ubi_device *ubi,
 		 * mapped logical eraseblocks.
 		 */
 		if (used_ebs == 0) {
-			dbg_err("zero used_ebs");
+			ubi_err("zero used_ebs");
 			goto bad;
 		}
 		if (data_size == 0) {
-			dbg_err("zero data_size");
+			ubi_err("zero data_size");
 			goto bad;
 		}
 		if (lnum < used_ebs - 1) {
 			if (data_size != usable_leb_size) {
-				dbg_err("bad data_size");
+				ubi_err("bad data_size");
 				goto bad;
 			}
 		} else if (lnum == used_ebs - 1) {
 			if (data_size == 0) {
-				dbg_err("bad data_size at last LEB");
+				ubi_err("bad data_size at last LEB");
 				goto bad;
 			}
 		} else {
-			dbg_err("too high lnum");
+			ubi_err("too high lnum");
 			goto bad;
 		}
 	} else {
 		if (copy_flag == 0) {
 			if (data_crc != 0) {
-				dbg_err("non-zero data CRC");
+				ubi_err("non-zero data CRC");
 				goto bad;
 			}
 			if (data_size != 0) {
-				dbg_err("non-zero data_size");
+				ubi_err("non-zero data_size");
 				goto bad;
 			}
 		} else {
 			if (data_size == 0) {
-				dbg_err("zero data_size of copy");
+				ubi_err("zero data_size of copy");
 				goto bad;
 			}
 		}
 		if (used_ebs != 0) {
-			dbg_err("bad used_ebs");
+			ubi_err("bad used_ebs");
 			goto bad;
 		}
 	}
@@ -997,8 +988,8 @@ static int validate_vid_hdr(const struct ubi_device *ubi,
 
 bad:
 	ubi_err("bad VID header");
-	ubi_dbg_dump_vid_hdr(vid_hdr);
-	ubi_dbg_dump_stack();
+	ubi_dump_vid_hdr(vid_hdr);
+	dump_stack();
 	return 1;
 }
 
@@ -1054,7 +1045,7 @@ int ubi_io_read_vid_hdr(struct ubi_device *ubi, int pnum,
 		if (verbose) {
 			ubi_warn("bad magic number at PEB %d: %08x instead of "
 				 "%08x", pnum, magic, UBI_VID_HDR_MAGIC);
-			ubi_dbg_dump_vid_hdr(vid_hdr);
+			ubi_dump_vid_hdr(vid_hdr);
 		}
 		dbg_bld("bad magic number at PEB %d: %08x instead of "
 			"%08x", pnum, magic, UBI_VID_HDR_MAGIC);
@@ -1068,7 +1059,7 @@ int ubi_io_read_vid_hdr(struct ubi_device *ubi, int pnum,
 		if (verbose) {
 			ubi_warn("bad CRC at PEB %d, calculated %#08x, "
 				 "read %#08x", pnum, crc, hdr_crc);
-			ubi_dbg_dump_vid_hdr(vid_hdr);
+			ubi_dump_vid_hdr(vid_hdr);
 		}
 		dbg_bld("bad CRC at PEB %d, calculated %#08x, "
 			"read %#08x", pnum, crc, hdr_crc);
@@ -1112,7 +1103,7 @@ int ubi_io_write_vid_hdr(struct ubi_device *ubi, int pnum,
 	dbg_io("write VID header to PEB %d", pnum);
 	ubi_assert(pnum >= 0 &&  pnum < ubi->peb_count);
 
-	err = paranoid_check_peb_ec_hdr(ubi, pnum);
+	err = self_check_peb_ec_hdr(ubi, pnum);
 	if (err)
 		return err;
 
@@ -1121,7 +1112,7 @@ int ubi_io_write_vid_hdr(struct ubi_device *ubi, int pnum,
 	crc = crc32(UBI_CRC32_INIT, vid_hdr, UBI_VID_HDR_SIZE_CRC);
 	vid_hdr->hdr_crc = cpu_to_be32(crc);
 
-	err = paranoid_check_vid_hdr(ubi, pnum, vid_hdr);
+	err = self_check_vid_hdr(ubi, pnum, vid_hdr);
 	if (err)
 		return err;
 
@@ -1131,17 +1122,15 @@ int ubi_io_write_vid_hdr(struct ubi_device *ubi, int pnum,
 	return err;
 }
 
-#ifdef CONFIG_MTD_UBI_DEBUG
-
 /**
- * paranoid_check_not_bad - ensure that a physical eraseblock is not bad.
+ * self_check_not_bad - ensure that a physical eraseblock is not bad.
  * @ubi: UBI device description object
  * @pnum: physical eraseblock number to check
  *
  * This function returns zero if the physical eraseblock is good, %-EINVAL if
  * it is bad and a negative error code if an error occurred.
  */
-static int paranoid_check_not_bad(const struct ubi_device *ubi, int pnum)
+static int self_check_not_bad(const struct ubi_device *ubi, int pnum)
 {
 	int err;
 
@@ -1152,13 +1141,13 @@ static int paranoid_check_not_bad(const struct ubi_device *ubi, int pnum)
 	if (!err)
 		return err;
 
-	ubi_err("paranoid check failed for PEB %d", pnum);
-	ubi_dbg_dump_stack();
+	ubi_err("self-check failed for PEB %d", pnum);
+	dump_stack();
 	return err > 0 ? -EINVAL : err;
 }
 
 /**
- * paranoid_check_ec_hdr - check if an erase counter header is all right.
+ * self_check_ec_hdr - check if an erase counter header is all right.
  * @ubi: UBI device description object
  * @pnum: physical eraseblock number the erase counter header belongs to
  * @ec_hdr: the erase counter header to check
@@ -1166,8 +1155,8 @@ static int paranoid_check_not_bad(const struct ubi_device *ubi, int pnum)
  * This function returns zero if the erase counter header contains valid
  * values, and %-EINVAL if not.
  */
-static int paranoid_check_ec_hdr(const struct ubi_device *ubi, int pnum,
-				 const struct ubi_ec_hdr *ec_hdr)
+static int self_check_ec_hdr(const struct ubi_device *ubi, int pnum,
+			     const struct ubi_ec_hdr *ec_hdr)
 {
 	int err;
 	uint32_t magic;
@@ -1184,27 +1173,27 @@ static int paranoid_check_ec_hdr(const struct ubi_device *ubi, int pnum,
 
 	err = validate_ec_hdr(ubi, ec_hdr);
 	if (err) {
-		ubi_err("paranoid check failed for PEB %d", pnum);
+		ubi_err("self-check failed for PEB %d", pnum);
 		goto fail;
 	}
 
 	return 0;
 
 fail:
-	ubi_dbg_dump_ec_hdr(ec_hdr);
-	ubi_dbg_dump_stack();
+	ubi_dump_ec_hdr(ec_hdr);
+	dump_stack();
 	return -EINVAL;
 }
 
 /**
- * paranoid_check_peb_ec_hdr - check erase counter header.
+ * self_check_peb_ec_hdr - check erase counter header.
  * @ubi: UBI device description object
  * @pnum: the physical eraseblock number to check
  *
  * This function returns zero if the erase counter header is all right and and
  * a negative error code if not or if an error occurred.
  */
-static int paranoid_check_peb_ec_hdr(const struct ubi_device *ubi, int pnum)
+static int self_check_peb_ec_hdr(const struct ubi_device *ubi, int pnum)
 {
 	int err;
 	uint32_t crc, hdr_crc;
@@ -1225,14 +1214,14 @@ static int paranoid_check_peb_ec_hdr(const struct ubi_device *ubi, int pnum)
 	hdr_crc = be32_to_cpu(ec_hdr->hdr_crc);
 	if (hdr_crc != crc) {
 		ubi_err("bad CRC, calculated %#08x, read %#08x", crc, hdr_crc);
-		ubi_err("paranoid check failed for PEB %d", pnum);
-		ubi_dbg_dump_ec_hdr(ec_hdr);
-		ubi_dbg_dump_stack();
+		ubi_err("self-check failed for PEB %d", pnum);
+		ubi_dump_ec_hdr(ec_hdr);
+		dump_stack();
 		err = -EINVAL;
 		goto exit;
 	}
 
-	err = paranoid_check_ec_hdr(ubi, pnum, ec_hdr);
+	err = self_check_ec_hdr(ubi, pnum, ec_hdr);
 
 exit:
 	kfree(ec_hdr);
@@ -1240,7 +1229,7 @@ exit:
 }
 
 /**
- * paranoid_check_vid_hdr - check that a volume identifier header is all right.
+ * self_check_vid_hdr - check that a volume identifier header is all right.
  * @ubi: UBI device description object
  * @pnum: physical eraseblock number the volume identifier header belongs to
  * @vid_hdr: the volume identifier header to check
@@ -1248,8 +1237,8 @@ exit:
  * This function returns zero if the volume identifier header is all right, and
  * %-EINVAL if not.
  */
-static int paranoid_check_vid_hdr(const struct ubi_device *ubi, int pnum,
-				  const struct ubi_vid_hdr *vid_hdr)
+static int self_check_vid_hdr(const struct ubi_device *ubi, int pnum,
+			      const struct ubi_vid_hdr *vid_hdr)
 {
 	int err;
 	uint32_t magic;
@@ -1266,29 +1255,29 @@ static int paranoid_check_vid_hdr(const struct ubi_device *ubi, int pnum,
 
 	err = validate_vid_hdr(ubi, vid_hdr);
 	if (err) {
-		ubi_err("paranoid check failed for PEB %d", pnum);
+		ubi_err("self-check failed for PEB %d", pnum);
 		goto fail;
 	}
 
 	return err;
 
 fail:
-	ubi_err("paranoid check failed for PEB %d", pnum);
-	ubi_dbg_dump_vid_hdr(vid_hdr);
-	ubi_dbg_dump_stack();
+	ubi_err("self-check failed for PEB %d", pnum);
+	ubi_dump_vid_hdr(vid_hdr);
+	dump_stack();
 	return -EINVAL;
 
 }
 
 /**
- * paranoid_check_peb_vid_hdr - check volume identifier header.
+ * self_check_peb_vid_hdr - check volume identifier header.
  * @ubi: UBI device description object
  * @pnum: the physical eraseblock number to check
  *
  * This function returns zero if the volume identifier header is all right,
  * and a negative error code if not or if an error occurred.
  */
-static int paranoid_check_peb_vid_hdr(const struct ubi_device *ubi, int pnum)
+static int self_check_peb_vid_hdr(const struct ubi_device *ubi, int pnum)
 {
 	int err;
 	uint32_t crc, hdr_crc;
@@ -1313,14 +1302,14 @@ static int paranoid_check_peb_vid_hdr(const struct ubi_device *ubi, int pnum)
 	if (hdr_crc != crc) {
 		ubi_err("bad VID header CRC at PEB %d, calculated %#08x, "
 			"read %#08x", pnum, crc, hdr_crc);
-		ubi_err("paranoid check failed for PEB %d", pnum);
-		ubi_dbg_dump_vid_hdr(vid_hdr);
-		ubi_dbg_dump_stack();
+		ubi_err("self-check failed for PEB %d", pnum);
+		ubi_dump_vid_hdr(vid_hdr);
+		dump_stack();
 		err = -EINVAL;
 		goto exit;
 	}
 
-	err = paranoid_check_vid_hdr(ubi, pnum, vid_hdr);
+	err = self_check_vid_hdr(ubi, pnum, vid_hdr);
 
 exit:
 	ubi_free_vid_hdr(ubi, vid_hdr);
@@ -1328,7 +1317,7 @@ exit:
 }
 
 /**
- * ubi_dbg_check_write - make sure write succeeded.
+ * self_check_write - make sure write succeeded.
  * @ubi: UBI device description object
  * @buf: buffer with data which were written
  * @pnum: physical eraseblock number the data were written to
@@ -1339,8 +1328,8 @@ exit:
  * the original data buffer - the data have to match. Returns zero if the data
  * match and a negative error code if not or in case of failure.
  */
-int ubi_dbg_check_write(struct ubi_device *ubi, const void *buf, int pnum,
-			int offset, int len)
+static int self_check_write(struct ubi_device *ubi, const void *buf, int pnum,
+			    int offset, int len)
 {
 	int err, i;
 	size_t read;
@@ -1368,7 +1357,7 @@ int ubi_dbg_check_write(struct ubi_device *ubi, const void *buf, int pnum,
 		if (c == c1)
 			continue;
 
-		ubi_err("paranoid check failed for PEB %d:%d, len %d",
+		ubi_err("self-check failed for PEB %d:%d, len %d",
 			pnum, offset, len);
 		ubi_msg("data differ at position %d", i);
 		dump_len = max_t(int, 128, len - i);
@@ -1380,7 +1369,7 @@ int ubi_dbg_check_write(struct ubi_device *ubi, const void *buf, int pnum,
 			i, i + dump_len);
 		print_hex_dump(KERN_DEBUG, "", DUMP_PREFIX_OFFSET, 32, 1,
 			       buf1 + i, dump_len, 1);
-		ubi_dbg_dump_stack();
+		dump_stack();
 		err = -EINVAL;
 		goto out_free;
 	}
@@ -1394,7 +1383,7 @@ out_free:
 }
 
 /**
- * ubi_dbg_check_all_ff - check that a region of flash is empty.
+ * ubi_self_check_all_ff - check that a region of flash is empty.
  * @ubi: UBI device description object
  * @pnum: the physical eraseblock number to check
  * @offset: the starting offset within the physical eraseblock to check
@@ -1404,7 +1393,7 @@ out_free:
  * @offset of the physical eraseblock @pnum, and a negative error code if not
  * or if an error occurred.
  */
-int ubi_dbg_check_all_ff(struct ubi_device *ubi, int pnum, int offset, int len)
+int ubi_self_check_all_ff(struct ubi_device *ubi, int pnum, int offset, int len)
 {
 	size_t read;
 	int err;
@@ -1438,14 +1427,12 @@ int ubi_dbg_check_all_ff(struct ubi_device *ubi, int pnum, int offset, int len)
 	return 0;
 
 fail:
-	ubi_err("paranoid check failed for PEB %d", pnum);
+	ubi_err("self-check failed for PEB %d", pnum);
 	ubi_msg("hex dump of the %d-%d region", offset, offset + len);
 	print_hex_dump(KERN_DEBUG, "", DUMP_PREFIX_OFFSET, 32, 1, buf, len, 1);
 	err = -EINVAL;
 error:
-	ubi_dbg_dump_stack();
+	dump_stack();
 	vfree(buf);
 	return err;
 }
-
-#endif /* CONFIG_MTD_UBI_DEBUG */
diff --git a/drivers/mtd/ubi/kapi.c b/drivers/mtd/ubi/kapi.c
index 9fdb35367fe..3aac1acceeb 100644
--- a/drivers/mtd/ubi/kapi.c
+++ b/drivers/mtd/ubi/kapi.c
@@ -221,7 +221,7 @@ out_free:
 	kfree(desc);
 out_put_ubi:
 	ubi_put_device(ubi);
-	dbg_err("cannot open device %d, volume %d, error %d",
+	ubi_err("cannot open device %d, volume %d, error %d",
 		ubi_num, vol_id, err);
 	return ERR_PTR(err);
 }
@@ -426,11 +426,9 @@ EXPORT_SYMBOL_GPL(ubi_leb_read);
  * @buf: data to write
  * @offset: offset within the logical eraseblock where to write
  * @len: how many bytes to write
- * @dtype: expected data type
  *
  * This function writes @len bytes of data from @buf to offset @offset of
- * logical eraseblock @lnum. The @dtype argument describes expected lifetime of
- * the data.
+ * logical eraseblock @lnum.
  *
  * This function takes care of physical eraseblock write failures. If write to
  * the physical eraseblock write operation fails, the logical eraseblock is
@@ -447,7 +445,7 @@ EXPORT_SYMBOL_GPL(ubi_leb_read);
  * returns immediately with %-EBADF code.
  */
 int ubi_leb_write(struct ubi_volume_desc *desc, int lnum, const void *buf,
-		  int offset, int len, int dtype)
+		  int offset, int len)
 {
 	struct ubi_volume *vol = desc->vol;
 	struct ubi_device *ubi = vol->ubi;
@@ -466,17 +464,13 @@ int ubi_leb_write(struct ubi_volume_desc *desc, int lnum, const void *buf,
 	    offset & (ubi->min_io_size - 1) || len & (ubi->min_io_size - 1))
 		return -EINVAL;
 
-	if (dtype != UBI_LONGTERM && dtype != UBI_SHORTTERM &&
-	    dtype != UBI_UNKNOWN)
-		return -EINVAL;
-
 	if (vol->upd_marker)
 		return -EBADF;
 
 	if (len == 0)
 		return 0;
 
-	return ubi_eba_write_leb(ubi, vol, lnum, buf, offset, len, dtype);
+	return ubi_eba_write_leb(ubi, vol, lnum, buf, offset, len);
 }
 EXPORT_SYMBOL_GPL(ubi_leb_write);
 
@@ -486,7 +480,6 @@ EXPORT_SYMBOL_GPL(ubi_leb_write);
  * @lnum: logical eraseblock number to change
  * @buf: data to write
  * @len: how many bytes to write
- * @dtype: expected data type
  *
  * This function changes the contents of a logical eraseblock atomically. @buf
  * has to contain new logical eraseblock data, and @len - the length of the
@@ -497,7 +490,7 @@ EXPORT_SYMBOL_GPL(ubi_leb_write);
  * code in case of failure.
  */
 int ubi_leb_change(struct ubi_volume_desc *desc, int lnum, const void *buf,
-		   int len, int dtype)
+		   int len)
 {
 	struct ubi_volume *vol = desc->vol;
 	struct ubi_device *ubi = vol->ubi;
@@ -515,17 +508,13 @@ int ubi_leb_change(struct ubi_volume_desc *desc, int lnum, const void *buf,
 	    len > vol->usable_leb_size || len & (ubi->min_io_size - 1))
 		return -EINVAL;
 
-	if (dtype != UBI_LONGTERM && dtype != UBI_SHORTTERM &&
-	    dtype != UBI_UNKNOWN)
-		return -EINVAL;
-
 	if (vol->upd_marker)
 		return -EBADF;
 
 	if (len == 0)
 		return 0;
 
-	return ubi_eba_atomic_leb_change(ubi, vol, lnum, buf, len, dtype);
+	return ubi_eba_atomic_leb_change(ubi, vol, lnum, buf, len);
 }
 EXPORT_SYMBOL_GPL(ubi_leb_change);
 
@@ -562,7 +551,7 @@ int ubi_leb_erase(struct ubi_volume_desc *desc, int lnum)
 	if (err)
 		return err;
 
-	return ubi_wl_flush(ubi);
+	return ubi_wl_flush(ubi, vol->vol_id, lnum);
 }
 EXPORT_SYMBOL_GPL(ubi_leb_erase);
 
@@ -626,7 +615,6 @@ EXPORT_SYMBOL_GPL(ubi_leb_unmap);
  * ubi_leb_map - map logical eraseblock to a physical eraseblock.
  * @desc: volume descriptor
  * @lnum: logical eraseblock number
- * @dtype: expected data type
  *
  * This function maps an un-mapped logical eraseblock @lnum to a physical
  * eraseblock. This means, that after a successful invocation of this
@@ -639,7 +627,7 @@ EXPORT_SYMBOL_GPL(ubi_leb_unmap);
  * eraseblock is already mapped, and other negative error codes in case of
  * other failures.
  */
-int ubi_leb_map(struct ubi_volume_desc *desc, int lnum, int dtype)
+int ubi_leb_map(struct ubi_volume_desc *desc, int lnum)
 {
 	struct ubi_volume *vol = desc->vol;
 	struct ubi_device *ubi = vol->ubi;
@@ -652,17 +640,13 @@ int ubi_leb_map(struct ubi_volume_desc *desc, int lnum, int dtype)
 	if (lnum < 0 || lnum >= vol->reserved_pebs)
 		return -EINVAL;
 
-	if (dtype != UBI_LONGTERM && dtype != UBI_SHORTTERM &&
-	    dtype != UBI_UNKNOWN)
-		return -EINVAL;
-
 	if (vol->upd_marker)
 		return -EBADF;
 
 	if (vol->eba_tbl[lnum] >= 0)
 		return -EBADMSG;
 
-	return ubi_eba_write_leb(ubi, vol, lnum, NULL, 0, 0, dtype);
+	return ubi_eba_write_leb(ubi, vol, lnum, NULL, 0, 0);
 }
 EXPORT_SYMBOL_GPL(ubi_leb_map);
 
@@ -720,6 +704,33 @@ int ubi_sync(int ubi_num)
 }
 EXPORT_SYMBOL_GPL(ubi_sync);
 
+/**
+ * ubi_flush - flush UBI work queue.
+ * @ubi_num: UBI device to flush work queue
+ * @vol_id: volume id to flush for
+ * @lnum: logical eraseblock number to flush for
+ *
+ * This function executes all pending works for a particular volume id / logical
+ * eraseblock number pair. If either value is set to %UBI_ALL, then it acts as
+ * a wildcard for all of the corresponding volume numbers or logical
+ * eraseblock numbers. It returns zero in case of success and a negative error
+ * code in case of failure.
+ */
+int ubi_flush(int ubi_num, int vol_id, int lnum)
+{
+	struct ubi_device *ubi;
+	int err = 0;
+
+	ubi = ubi_get_device(ubi_num);
+	if (!ubi)
+		return -ENODEV;
+
+	err = ubi_wl_flush(ubi, vol_id, lnum);
+	ubi_put_device(ubi);
+	return err;
+}
+EXPORT_SYMBOL_GPL(ubi_flush);
+
 BLOCKING_NOTIFIER_HEAD(ubi_notifiers);
 
 /**
diff --git a/drivers/mtd/ubi/scan.h b/drivers/mtd/ubi/scan.h
deleted file mode 100644
index d48aef15ab5..00000000000
--- a/drivers/mtd/ubi/scan.h
+++ /dev/null
@@ -1,174 +0,0 @@
-/*
- * Copyright (c) International Business Machines Corp., 2006
- *
- * 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
- *
- * Author: Artem Bityutskiy (Битюцкий Артём)
- */
-
-#ifndef __UBI_SCAN_H__
-#define __UBI_SCAN_H__
-
-/* The erase counter value for this physical eraseblock is unknown */
-#define UBI_SCAN_UNKNOWN_EC (-1)
-
-/**
- * struct ubi_scan_leb - scanning information about a physical eraseblock.
- * @ec: erase counter (%UBI_SCAN_UNKNOWN_EC if it is unknown)
- * @pnum: physical eraseblock number
- * @lnum: logical eraseblock number
- * @scrub: if this physical eraseblock needs scrubbing
- * @copy_flag: this LEB is a copy (@copy_flag is set in VID header of this LEB)
- * @sqnum: sequence number
- * @u: unions RB-tree or @list links
- * @u.rb: link in the per-volume RB-tree of &struct ubi_scan_leb objects
- * @u.list: link in one of the eraseblock lists
- *
- * One object of this type is allocated for each physical eraseblock during
- * scanning.
- */
-struct ubi_scan_leb {
-	int ec;
-	int pnum;
-	int lnum;
-	unsigned int scrub:1;
-	unsigned int copy_flag:1;
-	unsigned long long sqnum;
-	union {
-		struct rb_node rb;
-		struct list_head list;
-	} u;
-};
-
-/**
- * struct ubi_scan_volume - scanning information about a volume.
- * @vol_id: volume ID
- * @highest_lnum: highest logical eraseblock number in this volume
- * @leb_count: number of logical eraseblocks in this volume
- * @vol_type: volume type
- * @used_ebs: number of used logical eraseblocks in this volume (only for
- *            static volumes)
- * @last_data_size: amount of data in the last logical eraseblock of this
- *                  volume (always equivalent to the usable logical eraseblock
- *                  size in case of dynamic volumes)
- * @data_pad: how many bytes at the end of logical eraseblocks of this volume
- *            are not used (due to volume alignment)
- * @compat: compatibility flags of this volume
- * @rb: link in the volume RB-tree
- * @root: root of the RB-tree containing all the eraseblock belonging to this
- *        volume (&struct ubi_scan_leb objects)
- *
- * One object of this type is allocated for each volume during scanning.
- */
-struct ubi_scan_volume {
-	int vol_id;
-	int highest_lnum;
-	int leb_count;
-	int vol_type;
-	int used_ebs;
-	int last_data_size;
-	int data_pad;
-	int compat;
-	struct rb_node rb;
-	struct rb_root root;
-};
-
-/**
- * struct ubi_scan_info - UBI scanning information.
- * @volumes: root of the volume RB-tree
- * @corr: list of corrupted physical eraseblocks
- * @free: list of free physical eraseblocks
- * @erase: list of physical eraseblocks which have to be erased
- * @alien: list of physical eraseblocks which should not be used by UBI (e.g.,
- *         those belonging to "preserve"-compatible internal volumes)
- * @corr_peb_count: count of PEBs in the @corr list
- * @empty_peb_count: count of PEBs which are presumably empty (contain only
- *                   0xFF bytes)
- * @alien_peb_count: count of PEBs in the @alien list
- * @bad_peb_count: count of bad physical eraseblocks
- * @maybe_bad_peb_count: count of bad physical eraseblocks which are not marked
- *                       as bad yet, but which look like bad
- * @vols_found: number of volumes found during scanning
- * @highest_vol_id: highest volume ID
- * @is_empty: flag indicating whether the MTD device is empty or not
- * @min_ec: lowest erase counter value
- * @max_ec: highest erase counter value
- * @max_sqnum: highest sequence number value
- * @mean_ec: mean erase counter value
- * @ec_sum: a temporary variable used when calculating @mean_ec
- * @ec_count: a temporary variable used when calculating @mean_ec
- * @scan_leb_slab: slab cache for &struct ubi_scan_leb objects
- *
- * This data structure contains the result of scanning and may be used by other
- * UBI sub-systems to build final UBI data structures, further error-recovery
- * and so on.
- */
-struct ubi_scan_info {
-	struct rb_root volumes;
-	struct list_head corr;
-	struct list_head free;
-	struct list_head erase;
-	struct list_head alien;
-	int corr_peb_count;
-	int empty_peb_count;
-	int alien_peb_count;
-	int bad_peb_count;
-	int maybe_bad_peb_count;
-	int vols_found;
-	int highest_vol_id;
-	int is_empty;
-	int min_ec;
-	int max_ec;
-	unsigned long long max_sqnum;
-	int mean_ec;
-	uint64_t ec_sum;
-	int ec_count;
-	struct kmem_cache *scan_leb_slab;
-};
-
-struct ubi_device;
-struct ubi_vid_hdr;
-
-/*
- * ubi_scan_move_to_list - move a PEB from the volume tree to a list.
- *
- * @sv: volume scanning information
- * @seb: scanning eraseblock information
- * @list: the list to move to
- */
-static inline void ubi_scan_move_to_list(struct ubi_scan_volume *sv,
-					 struct ubi_scan_leb *seb,
-					 struct list_head *list)
-{
-		rb_erase(&seb->u.rb, &sv->root);
-		list_add_tail(&seb->u.list, list);
-}
-
-int ubi_scan_add_used(struct ubi_device *ubi, struct ubi_scan_info *si,
-		      int pnum, int ec, const struct ubi_vid_hdr *vid_hdr,
-		      int bitflips);
-struct ubi_scan_volume *ubi_scan_find_sv(const struct ubi_scan_info *si,
-					 int vol_id);
-struct ubi_scan_leb *ubi_scan_find_seb(const struct ubi_scan_volume *sv,
-				       int lnum);
-void ubi_scan_rm_volume(struct ubi_scan_info *si, struct ubi_scan_volume *sv);
-struct ubi_scan_leb *ubi_scan_get_free_peb(struct ubi_device *ubi,
-					   struct ubi_scan_info *si);
-int ubi_scan_erase_peb(struct ubi_device *ubi, const struct ubi_scan_info *si,
-		       int pnum, int ec);
-struct ubi_scan_info *ubi_scan(struct ubi_device *ubi);
-void ubi_scan_destroy_si(struct ubi_scan_info *si);
-
-#endif /* !__UBI_SCAN_H__ */
diff --git a/drivers/mtd/ubi/ubi-media.h b/drivers/mtd/ubi/ubi-media.h
index 6fb8ec2174a..468ffbc0eab 100644
--- a/drivers/mtd/ubi/ubi-media.h
+++ b/drivers/mtd/ubi/ubi-media.h
@@ -149,10 +149,10 @@ enum {
  * The @image_seq field is used to validate a UBI image that has been prepared
  * for a UBI device. The @image_seq value can be any value, but it must be the
  * same on all eraseblocks. UBI will ensure that all new erase counter headers
- * also contain this value, and will check the value when scanning at start-up.
+ * also contain this value, and will check the value when attaching the flash.
  * One way to make use of @image_seq is to increase its value by one every time
  * an image is flashed over an existing image, then, if the flashing does not
- * complete, UBI will detect the error when scanning.
+ * complete, UBI will detect the error when attaching the media.
  */
 struct ubi_ec_hdr {
 	__be32  magic;
@@ -298,8 +298,8 @@ struct ubi_vid_hdr {
 #define UBI_INT_VOL_COUNT 1
 
 /*
- * Starting ID of internal volumes. There is reserved room for 4096 internal
- * volumes.
+ * Starting ID of internal volumes: 0x7fffefff.
+ * There is reserved room for 4096 internal volumes.
  */
 #define UBI_INTERNAL_VOL_START (0x7FFFFFFF - 4096)
 
diff --git a/drivers/mtd/ubi/ubi.h b/drivers/mtd/ubi/ubi.h
index b162790790a..a1a81c9ea8c 100644
--- a/drivers/mtd/ubi/ubi.h
+++ b/drivers/mtd/ubi/ubi.h
@@ -43,7 +43,6 @@
 #include <asm/pgtable.h>
 
 #include "ubi-media.h"
-#include "scan.h"
 
 /* Maximum number of supported UBI devices */
 #define UBI_MAX_DEVICES 32
@@ -66,7 +65,10 @@
 /* Background thread name pattern */
 #define UBI_BGT_NAME_PATTERN "ubi_bgt%dd"
 
-/* This marker in the EBA table means that the LEB is um-mapped */
+/*
+ * This marker in the EBA table means that the LEB is um-mapped.
+ * NOTE! It has to have the same value as %UBI_ALL.
+ */
 #define UBI_LEB_UNMAPPED -1
 
 /*
@@ -82,6 +84,9 @@
  */
 #define UBI_PROT_QUEUE_LEN 10
 
+/* The volume ID/LEB number/erase counter is unknown */
+#define UBI_UNKNOWN -1
+
 /*
  * Error codes returned by the I/O sub-system.
  *
@@ -222,8 +227,6 @@ struct ubi_volume_desc;
  * @upd_ebs: how many eraseblocks are expected to be updated
  * @ch_lnum: LEB number which is being changing by the atomic LEB change
  *           operation
- * @ch_dtype: data persistency type which is being changing by the atomic LEB
- *            change operation
  * @upd_bytes: how many bytes are expected to be received for volume update or
  *             atomic LEB change
  * @upd_received: how many bytes were already received for volume update or
@@ -270,7 +273,6 @@ struct ubi_volume {
 
 	int upd_ebs;
 	int ch_lnum;
-	int ch_dtype;
 	long long upd_bytes;
 	long long upd_received;
 	void *upd_buf;
@@ -477,6 +479,124 @@ struct ubi_device {
 	struct ubi_debug_info *dbg;
 };
 
+/**
+ * struct ubi_ainf_peb - attach information about a physical eraseblock.
+ * @ec: erase counter (%UBI_UNKNOWN if it is unknown)
+ * @pnum: physical eraseblock number
+ * @vol_id: ID of the volume this LEB belongs to
+ * @lnum: logical eraseblock number
+ * @scrub: if this physical eraseblock needs scrubbing
+ * @copy_flag: this LEB is a copy (@copy_flag is set in VID header of this LEB)
+ * @sqnum: sequence number
+ * @u: unions RB-tree or @list links
+ * @u.rb: link in the per-volume RB-tree of &struct ubi_ainf_peb objects
+ * @u.list: link in one of the eraseblock lists
+ *
+ * One object of this type is allocated for each physical eraseblock when
+ * attaching an MTD device. Note, if this PEB does not belong to any LEB /
+ * volume, the @vol_id and @lnum fields are initialized to %UBI_UNKNOWN.
+ */
+struct ubi_ainf_peb {
+	int ec;
+	int pnum;
+	int vol_id;
+	int lnum;
+	unsigned int scrub:1;
+	unsigned int copy_flag:1;
+	unsigned long long sqnum;
+	union {
+		struct rb_node rb;
+		struct list_head list;
+	} u;
+};
+
+/**
+ * struct ubi_ainf_volume - attaching information about a volume.
+ * @vol_id: volume ID
+ * @highest_lnum: highest logical eraseblock number in this volume
+ * @leb_count: number of logical eraseblocks in this volume
+ * @vol_type: volume type
+ * @used_ebs: number of used logical eraseblocks in this volume (only for
+ *            static volumes)
+ * @last_data_size: amount of data in the last logical eraseblock of this
+ *                  volume (always equivalent to the usable logical eraseblock
+ *                  size in case of dynamic volumes)
+ * @data_pad: how many bytes at the end of logical eraseblocks of this volume
+ *            are not used (due to volume alignment)
+ * @compat: compatibility flags of this volume
+ * @rb: link in the volume RB-tree
+ * @root: root of the RB-tree containing all the eraseblock belonging to this
+ *        volume (&struct ubi_ainf_peb objects)
+ *
+ * One object of this type is allocated for each volume when attaching an MTD
+ * device.
+ */
+struct ubi_ainf_volume {
+	int vol_id;
+	int highest_lnum;
+	int leb_count;
+	int vol_type;
+	int used_ebs;
+	int last_data_size;
+	int data_pad;
+	int compat;
+	struct rb_node rb;
+	struct rb_root root;
+};
+
+/**
+ * struct ubi_attach_info - MTD device attaching information.
+ * @volumes: root of the volume RB-tree
+ * @corr: list of corrupted physical eraseblocks
+ * @free: list of free physical eraseblocks
+ * @erase: list of physical eraseblocks which have to be erased
+ * @alien: list of physical eraseblocks which should not be used by UBI (e.g.,
+ *         those belonging to "preserve"-compatible internal volumes)
+ * @corr_peb_count: count of PEBs in the @corr list
+ * @empty_peb_count: count of PEBs which are presumably empty (contain only
+ *                   0xFF bytes)
+ * @alien_peb_count: count of PEBs in the @alien list
+ * @bad_peb_count: count of bad physical eraseblocks
+ * @maybe_bad_peb_count: count of bad physical eraseblocks which are not marked
+ *                       as bad yet, but which look like bad
+ * @vols_found: number of volumes found
+ * @highest_vol_id: highest volume ID
+ * @is_empty: flag indicating whether the MTD device is empty or not
+ * @min_ec: lowest erase counter value
+ * @max_ec: highest erase counter value
+ * @max_sqnum: highest sequence number value
+ * @mean_ec: mean erase counter value
+ * @ec_sum: a temporary variable used when calculating @mean_ec
+ * @ec_count: a temporary variable used when calculating @mean_ec
+ * @aeb_slab_cache: slab cache for &struct ubi_ainf_peb objects
+ *
+ * This data structure contains the result of attaching an MTD device and may
+ * be used by other UBI sub-systems to build final UBI data structures, further
+ * error-recovery and so on.
+ */
+struct ubi_attach_info {
+	struct rb_root volumes;
+	struct list_head corr;
+	struct list_head free;
+	struct list_head erase;
+	struct list_head alien;
+	int corr_peb_count;
+	int empty_peb_count;
+	int alien_peb_count;
+	int bad_peb_count;
+	int maybe_bad_peb_count;
+	int vols_found;
+	int highest_vol_id;
+	int is_empty;
+	int min_ec;
+	int max_ec;
+	unsigned long long max_sqnum;
+	int mean_ec;
+	uint64_t ec_sum;
+	int ec_count;
+	struct kmem_cache *aeb_slab_cache;
+};
+
 #include "debug.h"
 
 extern struct kmem_cache *ubi_wl_entry_slab;
@@ -487,12 +607,23 @@ extern struct class *ubi_class;
 extern struct mutex ubi_devices_mutex;
 extern struct blocking_notifier_head ubi_notifiers;
 
+/* scan.c */
+int ubi_add_to_av(struct ubi_device *ubi, struct ubi_attach_info *ai, int pnum,
+		  int ec, const struct ubi_vid_hdr *vid_hdr, int bitflips);
+struct ubi_ainf_volume *ubi_find_av(const struct ubi_attach_info *ai,
+				    int vol_id);
+void ubi_remove_av(struct ubi_attach_info *ai, struct ubi_ainf_volume *av);
+struct ubi_ainf_peb *ubi_early_get_peb(struct ubi_device *ubi,
+				       struct ubi_attach_info *ai);
+int ubi_attach(struct ubi_device *ubi);
+void ubi_destroy_ai(struct ubi_attach_info *ai);
+
 /* vtbl.c */
 int ubi_change_vtbl_record(struct ubi_device *ubi, int idx,
 			   struct ubi_vtbl_record *vtbl_rec);
 int ubi_vtbl_rename_volumes(struct ubi_device *ubi,
 			    struct list_head *rename_list);
-int ubi_read_volume_table(struct ubi_device *ubi, struct ubi_scan_info *si);
+int ubi_read_volume_table(struct ubi_device *ubi, struct ubi_attach_info *ai);
 
 /* vmt.c */
 int ubi_create_volume(struct ubi_device *ubi, struct ubi_mkvol_req *req);
@@ -525,22 +656,22 @@ int ubi_eba_unmap_leb(struct ubi_device *ubi, struct ubi_volume *vol,
 int ubi_eba_read_leb(struct ubi_device *ubi, struct ubi_volume *vol, int lnum,
 		     void *buf, int offset, int len, int check);
 int ubi_eba_write_leb(struct ubi_device *ubi, struct ubi_volume *vol, int lnum,
-		      const void *buf, int offset, int len, int dtype);
+		      const void *buf, int offset, int len);
 int ubi_eba_write_leb_st(struct ubi_device *ubi, struct ubi_volume *vol,
-			 int lnum, const void *buf, int len, int dtype,
-			 int used_ebs);
+			 int lnum, const void *buf, int len, int used_ebs);
 int ubi_eba_atomic_leb_change(struct ubi_device *ubi, struct ubi_volume *vol,
-			      int lnum, const void *buf, int len, int dtype);
+			      int lnum, const void *buf, int len);
 int ubi_eba_copy_leb(struct ubi_device *ubi, int from, int to,
 		     struct ubi_vid_hdr *vid_hdr);
-int ubi_eba_init_scan(struct ubi_device *ubi, struct ubi_scan_info *si);
+int ubi_eba_init(struct ubi_device *ubi, struct ubi_attach_info *ai);
 
 /* wl.c */
-int ubi_wl_get_peb(struct ubi_device *ubi, int dtype);
-int ubi_wl_put_peb(struct ubi_device *ubi, int pnum, int torture);
-int ubi_wl_flush(struct ubi_device *ubi);
+int ubi_wl_get_peb(struct ubi_device *ubi);
+int ubi_wl_put_peb(struct ubi_device *ubi, int vol_id, int lnum,
+		   int pnum, int torture);
+int ubi_wl_flush(struct ubi_device *ubi, int vol_id, int lnum);
 int ubi_wl_scrub_peb(struct ubi_device *ubi, int pnum);
-int ubi_wl_init_scan(struct ubi_device *ubi, struct ubi_scan_info *si);
+int ubi_wl_init(struct ubi_device *ubi, struct ubi_attach_info *ai);
 void ubi_wl_close(struct ubi_device *ubi);
 int ubi_thread(void *u);
 
@@ -573,6 +704,7 @@ int ubi_volume_notify(struct ubi_device *ubi, struct ubi_volume *vol,
 int ubi_notify_all(struct ubi_device *ubi, int ntype,
 		   struct notifier_block *nb);
 int ubi_enumerate_volumes(struct notifier_block *nb);
+void ubi_free_internal_volumes(struct ubi_device *ubi);
 
 /* kapi.c */
 void ubi_do_get_device_info(struct ubi_device *ubi, struct ubi_device_info *di);
@@ -593,6 +725,21 @@ void ubi_do_get_volume_info(struct ubi_device *ubi, struct ubi_volume *vol,
 	     rb = rb_next(rb),                                               \
 	     pos = (rb ? container_of(rb, typeof(*pos), member) : NULL))
 
+/*
+ * ubi_move_aeb_to_list - move a PEB from the volume tree to a list.
+ *
+ * @av: volume attaching information
+ * @aeb: attaching eraseblock information
+ * @list: the list to move to
+ */
+static inline void ubi_move_aeb_to_list(struct ubi_ainf_volume *av,
+					 struct ubi_ainf_peb *aeb,
+					 struct list_head *list)
+{
+		rb_erase(&aeb->u.rb, &av->root);
+		list_add_tail(&aeb->u.list, list);
+}
+
 /**
  * ubi_zalloc_vid_hdr - allocate a volume identifier header object.
  * @ubi: UBI device description object
@@ -667,7 +814,7 @@ static inline void ubi_ro_mode(struct ubi_device *ubi)
 	if (!ubi->ro_mode) {
 		ubi->ro_mode = 1;
 		ubi_warn("switch to read-only mode");
-		ubi_dbg_dump_stack();
+		dump_stack();
 	}
 }
 
diff --git a/drivers/mtd/ubi/upd.c b/drivers/mtd/ubi/upd.c
index 425bf5a3edd..9f2ebd8750e 100644
--- a/drivers/mtd/ubi/upd.c
+++ b/drivers/mtd/ubi/upd.c
@@ -147,7 +147,7 @@ int ubi_start_update(struct ubi_device *ubi, struct ubi_volume *vol,
 	}
 
 	if (bytes == 0) {
-		err = ubi_wl_flush(ubi);
+		err = ubi_wl_flush(ubi, UBI_ALL, UBI_ALL);
 		if (err)
 			return err;
 
@@ -186,14 +186,12 @@ int ubi_start_leb_change(struct ubi_device *ubi, struct ubi_volume *vol,
 	dbg_gen("start changing LEB %d:%d, %u bytes",
 		vol->vol_id, req->lnum, req->bytes);
 	if (req->bytes == 0)
-		return ubi_eba_atomic_leb_change(ubi, vol, req->lnum, NULL, 0,
-						 req->dtype);
+		return ubi_eba_atomic_leb_change(ubi, vol, req->lnum, NULL, 0);
 
 	vol->upd_bytes = req->bytes;
 	vol->upd_received = 0;
 	vol->changing_leb = 1;
 	vol->ch_lnum = req->lnum;
-	vol->ch_dtype = req->dtype;
 
 	vol->upd_buf = vmalloc(req->bytes);
 	if (!vol->upd_buf)
@@ -246,8 +244,7 @@ static int write_leb(struct ubi_device *ubi, struct ubi_volume *vol, int lnum,
 			return 0;
 		}
 
-		err = ubi_eba_write_leb(ubi, vol, lnum, buf, 0, len,
-					UBI_UNKNOWN);
+		err = ubi_eba_write_leb(ubi, vol, lnum, buf, 0, len);
 	} else {
 		/*
 		 * When writing static volume, and this is the last logical
@@ -259,8 +256,7 @@ static int write_leb(struct ubi_device *ubi, struct ubi_volume *vol, int lnum,
 		 * contain zeros, not random trash.
 		 */
 		memset(buf + len, 0, vol->usable_leb_size - len);
-		err = ubi_eba_write_leb_st(ubi, vol, lnum, buf, len,
-					   UBI_UNKNOWN, used_ebs);
+		err = ubi_eba_write_leb_st(ubi, vol, lnum, buf, len, used_ebs);
 	}
 
 	return err;
@@ -365,7 +361,7 @@ int ubi_more_update_data(struct ubi_device *ubi, struct ubi_volume *vol,
 
 	ubi_assert(vol->upd_received <= vol->upd_bytes);
 	if (vol->upd_received == vol->upd_bytes) {
-		err = ubi_wl_flush(ubi);
+		err = ubi_wl_flush(ubi, UBI_ALL, UBI_ALL);
 		if (err)
 			return err;
 		/* The update is finished, clear the update marker */
@@ -421,7 +417,7 @@ int ubi_more_leb_change_data(struct ubi_device *ubi, struct ubi_volume *vol,
 		       len - vol->upd_bytes);
 		len = ubi_calc_data_len(ubi, vol->upd_buf, len);
 		err = ubi_eba_atomic_leb_change(ubi, vol, vol->ch_lnum,
-						vol->upd_buf, len, UBI_UNKNOWN);
+						vol->upd_buf, len);
 		if (err)
 			return err;
 	}
diff --git a/drivers/mtd/ubi/vmt.c b/drivers/mtd/ubi/vmt.c
index 863835f4aef..0669cff8ac3 100644
--- a/drivers/mtd/ubi/vmt.c
+++ b/drivers/mtd/ubi/vmt.c
@@ -29,11 +29,7 @@
 #include <linux/export.h>
 #include "ubi.h"
 
-#ifdef CONFIG_MTD_UBI_DEBUG
-static int paranoid_check_volumes(struct ubi_device *ubi);
-#else
-#define paranoid_check_volumes(ubi) 0
-#endif
+static int self_check_volumes(struct ubi_device *ubi);
 
 static ssize_t vol_attribute_show(struct device *dev,
 				  struct device_attribute *attr, char *buf);
@@ -227,7 +223,7 @@ int ubi_create_volume(struct ubi_device *ubi, struct ubi_mkvol_req *req)
 			}
 
 		if (vol_id == UBI_VOL_NUM_AUTO) {
-			dbg_err("out of volume IDs");
+			ubi_err("out of volume IDs");
 			err = -ENFILE;
 			goto out_unlock;
 		}
@@ -241,7 +237,7 @@ int ubi_create_volume(struct ubi_device *ubi, struct ubi_mkvol_req *req)
 	/* Ensure that this volume does not exist */
 	err = -EEXIST;
 	if (ubi->volumes[vol_id]) {
-		dbg_err("volume %d already exists", vol_id);
+		ubi_err("volume %d already exists", vol_id);
 		goto out_unlock;
 	}
 
@@ -250,7 +246,7 @@ int ubi_create_volume(struct ubi_device *ubi, struct ubi_mkvol_req *req)
 		if (ubi->volumes[i] &&
 		    ubi->volumes[i]->name_len == req->name_len &&
 		    !strcmp(ubi->volumes[i]->name, req->name)) {
-			dbg_err("volume \"%s\" exists (ID %d)", req->name, i);
+			ubi_err("volume \"%s\" exists (ID %d)", req->name, i);
 			goto out_unlock;
 		}
 
@@ -261,9 +257,9 @@ int ubi_create_volume(struct ubi_device *ubi, struct ubi_mkvol_req *req)
 
 	/* Reserve physical eraseblocks */
 	if (vol->reserved_pebs > ubi->avail_pebs) {
-		dbg_err("not enough PEBs, only %d available", ubi->avail_pebs);
+		ubi_err("not enough PEBs, only %d available", ubi->avail_pebs);
 		if (ubi->corr_peb_count)
-			dbg_err("%d PEBs are corrupted and not used",
+			ubi_err("%d PEBs are corrupted and not used",
 				ubi->corr_peb_count);
 		err = -ENOSPC;
 		goto out_unlock;
@@ -284,7 +280,7 @@ int ubi_create_volume(struct ubi_device *ubi, struct ubi_mkvol_req *req)
 	 * Finish all pending erases because there may be some LEBs belonging
 	 * to the same volume ID.
 	 */
-	err = ubi_wl_flush(ubi);
+	err = ubi_wl_flush(ubi, vol_id, UBI_ALL);
 	if (err)
 		goto out_acc;
 
@@ -360,8 +356,7 @@ int ubi_create_volume(struct ubi_device *ubi, struct ubi_mkvol_req *req)
 	spin_unlock(&ubi->volumes_lock);
 
 	ubi_volume_notify(ubi, vol, UBI_VOLUME_ADDED);
-	if (paranoid_check_volumes(ubi))
-		dbg_err("check failed while creating volume %d", vol_id);
+	self_check_volumes(ubi);
 	return err;
 
 out_sysfs:
@@ -461,8 +456,8 @@ int ubi_remove_volume(struct ubi_volume_desc *desc, int no_vtbl)
 	spin_unlock(&ubi->volumes_lock);
 
 	ubi_volume_notify(ubi, vol, UBI_VOLUME_REMOVED);
-	if (!no_vtbl && paranoid_check_volumes(ubi))
-		dbg_err("check failed while removing volume %d", vol_id);
+	if (!no_vtbl)
+		self_check_volumes(ubi);
 
 	return err;
 
@@ -500,7 +495,7 @@ int ubi_resize_volume(struct ubi_volume_desc *desc, int reserved_pebs)
 
 	if (vol->vol_type == UBI_STATIC_VOLUME &&
 	    reserved_pebs < vol->used_ebs) {
-		dbg_err("too small size %d, %d LEBs contain data",
+		ubi_err("too small size %d, %d LEBs contain data",
 			reserved_pebs, vol->used_ebs);
 		return -EINVAL;
 	}
@@ -529,10 +524,10 @@ int ubi_resize_volume(struct ubi_volume_desc *desc, int reserved_pebs)
 	if (pebs > 0) {
 		spin_lock(&ubi->volumes_lock);
 		if (pebs > ubi->avail_pebs) {
-			dbg_err("not enough PEBs: requested %d, available %d",
+			ubi_err("not enough PEBs: requested %d, available %d",
 				pebs, ubi->avail_pebs);
 			if (ubi->corr_peb_count)
-				dbg_err("%d PEBs are corrupted and not used",
+				ubi_err("%d PEBs are corrupted and not used",
 					ubi->corr_peb_count);
 			spin_unlock(&ubi->volumes_lock);
 			err = -ENOSPC;
@@ -588,8 +583,7 @@ int ubi_resize_volume(struct ubi_volume_desc *desc, int reserved_pebs)
 	}
 
 	ubi_volume_notify(ubi, vol, UBI_VOLUME_RESIZED);
-	if (paranoid_check_volumes(ubi))
-		dbg_err("check failed while re-sizing volume %d", vol_id);
+	self_check_volumes(ubi);
 	return err;
 
 out_acc:
@@ -638,8 +632,8 @@ int ubi_rename_volumes(struct ubi_device *ubi, struct list_head *rename_list)
 		}
 	}
 
-	if (!err && paranoid_check_volumes(ubi))
-		;
+	if (!err)
+		self_check_volumes(ubi);
 	return err;
 }
 
@@ -686,8 +680,7 @@ int ubi_add_volume(struct ubi_device *ubi, struct ubi_volume *vol)
 		return err;
 	}
 
-	if (paranoid_check_volumes(ubi))
-		dbg_err("check failed while adding volume %d", vol_id);
+	self_check_volumes(ubi);
 	return err;
 
 out_cdev:
@@ -712,16 +705,14 @@ void ubi_free_volume(struct ubi_device *ubi, struct ubi_volume *vol)
 	volume_sysfs_close(vol);
 }
 
-#ifdef CONFIG_MTD_UBI_DEBUG
-
 /**
- * paranoid_check_volume - check volume information.
+ * self_check_volume - check volume information.
  * @ubi: UBI device description object
  * @vol_id: volume ID
  *
  * Returns zero if volume is all right and a a negative error code if not.
  */
-static int paranoid_check_volume(struct ubi_device *ubi, int vol_id)
+static int self_check_volume(struct ubi_device *ubi, int vol_id)
 {
 	int idx = vol_id2idx(ubi, vol_id);
 	int reserved_pebs, alignment, data_pad, vol_type, name_len, upd_marker;
@@ -771,7 +762,7 @@ static int paranoid_check_volume(struct ubi_device *ubi, int vol_id)
 	}
 
 	if (vol->upd_marker && vol->corrupted) {
-		dbg_err("update marker and corrupted simultaneously");
+		ubi_err("update marker and corrupted simultaneously");
 		goto fail;
 	}
 
@@ -853,22 +844,22 @@ static int paranoid_check_volume(struct ubi_device *ubi, int vol_id)
 	return 0;
 
 fail:
-	ubi_err("paranoid check failed for volume %d", vol_id);
+	ubi_err("self-check failed for volume %d", vol_id);
 	if (vol)
-		ubi_dbg_dump_vol_info(vol);
-	ubi_dbg_dump_vtbl_record(&ubi->vtbl[vol_id], vol_id);
+		ubi_dump_vol_info(vol);
+	ubi_dump_vtbl_record(&ubi->vtbl[vol_id], vol_id);
 	dump_stack();
 	spin_unlock(&ubi->volumes_lock);
 	return -EINVAL;
 }
 
 /**
- * paranoid_check_volumes - check information about all volumes.
+ * self_check_volumes - check information about all volumes.
  * @ubi: UBI device description object
  *
  * Returns zero if volumes are all right and a a negative error code if not.
  */
-static int paranoid_check_volumes(struct ubi_device *ubi)
+static int self_check_volumes(struct ubi_device *ubi)
 {
 	int i, err = 0;
 
@@ -876,11 +867,10 @@ static int paranoid_check_volumes(struct ubi_device *ubi)
 		return 0;
 
 	for (i = 0; i < ubi->vtbl_slots; i++) {
-		err = paranoid_check_volume(ubi, i);
+		err = self_check_volume(ubi, i);
 		if (err)
 			break;
 	}
 
 	return err;
 }
-#endif
diff --git a/drivers/mtd/ubi/vtbl.c b/drivers/mtd/ubi/vtbl.c
index 17cec0c0154..437bc193e17 100644
--- a/drivers/mtd/ubi/vtbl.c
+++ b/drivers/mtd/ubi/vtbl.c
@@ -37,16 +37,15 @@
  * LEB 1. This scheme guarantees recoverability from unclean reboots.
  *
  * In this UBI implementation the on-flash volume table does not contain any
- * information about how many data static volumes contain. This information may
- * be found from the scanning data.
+ * information about how much data static volumes contain.
  *
  * But it would still be beneficial to store this information in the volume
  * table. For example, suppose we have a static volume X, and all its physical
  * eraseblocks became bad for some reasons. Suppose we are attaching the
- * corresponding MTD device, the scanning has found no logical eraseblocks
+ * corresponding MTD device, for some reason we find no logical eraseblocks
  * corresponding to the volume X. According to the volume table volume X does
  * exist. So we don't know whether it is just empty or all its physical
- * eraseblocks went bad. So we cannot alarm the user about this corruption.
+ * eraseblocks went bad. So we cannot alarm the user properly.
  *
  * The volume table also stores so-called "update marker", which is used for
  * volume updates. Before updating the volume, the update marker is set, and
@@ -62,11 +61,7 @@
 #include <asm/div64.h>
 #include "ubi.h"
 
-#ifdef CONFIG_MTD_UBI_DEBUG
-static void paranoid_vtbl_check(const struct ubi_device *ubi);
-#else
-#define paranoid_vtbl_check(ubi)
-#endif
+static void self_vtbl_check(const struct ubi_device *ubi);
 
 /* Empty volume table record */
 static struct ubi_vtbl_record empty_vtbl_record;
@@ -106,12 +101,12 @@ int ubi_change_vtbl_record(struct ubi_device *ubi, int idx,
 			return err;
 
 		err = ubi_eba_write_leb(ubi, layout_vol, i, ubi->vtbl, 0,
-					ubi->vtbl_size, UBI_LONGTERM);
+					ubi->vtbl_size);
 		if (err)
 			return err;
 	}
 
-	paranoid_vtbl_check(ubi);
+	self_vtbl_check(ubi);
 	return 0;
 }
 
@@ -158,7 +153,7 @@ int ubi_vtbl_rename_volumes(struct ubi_device *ubi,
 			return err;
 
 		err = ubi_eba_write_leb(ubi, layout_vol, i, ubi->vtbl, 0,
-					ubi->vtbl_size, UBI_LONGTERM);
+					ubi->vtbl_size);
 		if (err)
 			return err;
 	}
@@ -197,7 +192,7 @@ static int vtbl_check(const struct ubi_device *ubi,
 		if (be32_to_cpu(vtbl[i].crc) != crc) {
 			ubi_err("bad CRC at record %u: %#08x, not %#08x",
 				 i, crc, be32_to_cpu(vtbl[i].crc));
-			ubi_dbg_dump_vtbl_record(&vtbl[i], i);
+			ubi_dump_vtbl_record(&vtbl[i], i);
 			return 1;
 		}
 
@@ -229,7 +224,7 @@ static int vtbl_check(const struct ubi_device *ubi,
 
 		n = ubi->leb_size % alignment;
 		if (data_pad != n) {
-			dbg_err("bad data_pad, has to be %d", n);
+			ubi_err("bad data_pad, has to be %d", n);
 			err = 6;
 			goto bad;
 		}
@@ -245,7 +240,7 @@ static int vtbl_check(const struct ubi_device *ubi,
 		}
 
 		if (reserved_pebs > ubi->good_peb_count) {
-			dbg_err("too large reserved_pebs %d, good PEBs %d",
+			ubi_err("too large reserved_pebs %d, good PEBs %d",
 				reserved_pebs, ubi->good_peb_count);
 			err = 9;
 			goto bad;
@@ -277,8 +272,8 @@ static int vtbl_check(const struct ubi_device *ubi,
 			    !strncmp(vtbl[i].name, vtbl[n].name, len1)) {
 				ubi_err("volumes %d and %d have the same name"
 					" \"%s\"", i, n, vtbl[i].name);
-				ubi_dbg_dump_vtbl_record(&vtbl[i], i);
-				ubi_dbg_dump_vtbl_record(&vtbl[n], n);
+				ubi_dump_vtbl_record(&vtbl[i], i);
+				ubi_dump_vtbl_record(&vtbl[n], n);
 				return -EINVAL;
 			}
 		}
@@ -288,26 +283,26 @@ static int vtbl_check(const struct ubi_device *ubi,
 
 bad:
 	ubi_err("volume table check failed: record %d, error %d", i, err);
-	ubi_dbg_dump_vtbl_record(&vtbl[i], i);
+	ubi_dump_vtbl_record(&vtbl[i], i);
 	return -EINVAL;
 }
 
 /**
  * create_vtbl - create a copy of volume table.
  * @ubi: UBI device description object
- * @si: scanning information
+ * @ai: attaching information
  * @copy: number of the volume table copy
  * @vtbl: contents of the volume table
  *
  * This function returns zero in case of success and a negative error code in
  * case of failure.
  */
-static int create_vtbl(struct ubi_device *ubi, struct ubi_scan_info *si,
+static int create_vtbl(struct ubi_device *ubi, struct ubi_attach_info *ai,
 		       int copy, void *vtbl)
 {
 	int err, tries = 0;
 	struct ubi_vid_hdr *vid_hdr;
-	struct ubi_scan_leb *new_seb;
+	struct ubi_ainf_peb *new_aeb;
 
 	ubi_msg("create volume table (copy #%d)", copy + 1);
 
@@ -316,9 +311,9 @@ static int create_vtbl(struct ubi_device *ubi, struct ubi_scan_info *si,
 		return -ENOMEM;
 
 retry:
-	new_seb = ubi_scan_get_free_peb(ubi, si);
-	if (IS_ERR(new_seb)) {
-		err = PTR_ERR(new_seb);
+	new_aeb = ubi_early_get_peb(ubi, ai);
+	if (IS_ERR(new_aeb)) {
+		err = PTR_ERR(new_aeb);
 		goto out_free;
 	}
 
@@ -328,25 +323,24 @@ retry:
 	vid_hdr->data_size = vid_hdr->used_ebs =
 			     vid_hdr->data_pad = cpu_to_be32(0);
 	vid_hdr->lnum = cpu_to_be32(copy);
-	vid_hdr->sqnum = cpu_to_be64(++si->max_sqnum);
+	vid_hdr->sqnum = cpu_to_be64(++ai->max_sqnum);
 
 	/* The EC header is already there, write the VID header */
-	err = ubi_io_write_vid_hdr(ubi, new_seb->pnum, vid_hdr);
+	err = ubi_io_write_vid_hdr(ubi, new_aeb->pnum, vid_hdr);
 	if (err)
 		goto write_error;
 
 	/* Write the layout volume contents */
-	err = ubi_io_write_data(ubi, vtbl, new_seb->pnum, 0, ubi->vtbl_size);
+	err = ubi_io_write_data(ubi, vtbl, new_aeb->pnum, 0, ubi->vtbl_size);
 	if (err)
 		goto write_error;
 
 	/*
-	 * And add it to the scanning information. Don't delete the old version
-	 * of this LEB as it will be deleted and freed in 'ubi_scan_add_used()'.
+	 * And add it to the attaching information. Don't delete the old version
+	 * of this LEB as it will be deleted and freed in 'ubi_add_to_av()'.
 	 */
-	err = ubi_scan_add_used(ubi, si, new_seb->pnum, new_seb->ec,
-				vid_hdr, 0);
-	kfree(new_seb);
+	err = ubi_add_to_av(ubi, ai, new_aeb->pnum, new_aeb->ec, vid_hdr, 0);
+	kfree(new_aeb);
 	ubi_free_vid_hdr(ubi, vid_hdr);
 	return err;
 
@@ -356,10 +350,10 @@ write_error:
 		 * Probably this physical eraseblock went bad, try to pick
 		 * another one.
 		 */
-		list_add(&new_seb->u.list, &si->erase);
+		list_add(&new_aeb->u.list, &ai->erase);
 		goto retry;
 	}
-	kfree(new_seb);
+	kfree(new_aeb);
 out_free:
 	ubi_free_vid_hdr(ubi, vid_hdr);
 	return err;
@@ -369,20 +363,20 @@ out_free:
 /**
  * process_lvol - process the layout volume.
  * @ubi: UBI device description object
- * @si: scanning information
- * @sv: layout volume scanning information
+ * @ai: attaching information
+ * @av: layout volume attaching information
  *
  * This function is responsible for reading the layout volume, ensuring it is
  * not corrupted, and recovering from corruptions if needed. Returns volume
  * table in case of success and a negative error code in case of failure.
  */
 static struct ubi_vtbl_record *process_lvol(struct ubi_device *ubi,
-					    struct ubi_scan_info *si,
-					    struct ubi_scan_volume *sv)
+					    struct ubi_attach_info *ai,
+					    struct ubi_ainf_volume *av)
 {
 	int err;
 	struct rb_node *rb;
-	struct ubi_scan_leb *seb;
+	struct ubi_ainf_peb *aeb;
 	struct ubi_vtbl_record *leb[UBI_LAYOUT_VOLUME_EBS] = { NULL, NULL };
 	int leb_corrupted[UBI_LAYOUT_VOLUME_EBS] = {1, 1};
 
@@ -414,14 +408,14 @@ static struct ubi_vtbl_record *process_lvol(struct ubi_device *ubi,
 	dbg_gen("check layout volume");
 
 	/* Read both LEB 0 and LEB 1 into memory */
-	ubi_rb_for_each_entry(rb, seb, &sv->root, u.rb) {
-		leb[seb->lnum] = vzalloc(ubi->vtbl_size);
-		if (!leb[seb->lnum]) {
+	ubi_rb_for_each_entry(rb, aeb, &av->root, u.rb) {
+		leb[aeb->lnum] = vzalloc(ubi->vtbl_size);
+		if (!leb[aeb->lnum]) {
 			err = -ENOMEM;
 			goto out_free;
 		}
 
-		err = ubi_io_read_data(ubi, leb[seb->lnum], seb->pnum, 0,
+		err = ubi_io_read_data(ubi, leb[aeb->lnum], aeb->pnum, 0,
 				       ubi->vtbl_size);
 		if (err == UBI_IO_BITFLIPS || mtd_is_eccerr(err))
 			/*
@@ -429,12 +423,12 @@ static struct ubi_vtbl_record *process_lvol(struct ubi_device *ubi,
 			 * uncorrectable ECC error, but we have our own CRC and
 			 * the data will be checked later. If the data is OK,
 			 * the PEB will be scrubbed (because we set
-			 * seb->scrub). If the data is not OK, the contents of
+			 * aeb->scrub). If the data is not OK, the contents of
 			 * the PEB will be recovered from the second copy, and
-			 * seb->scrub will be cleared in
-			 * 'ubi_scan_add_used()'.
+			 * aeb->scrub will be cleared in
+			 * 'ubi_add_to_av()'.
 			 */
-			seb->scrub = 1;
+			aeb->scrub = 1;
 		else if (err)
 			goto out_free;
 	}
@@ -453,7 +447,7 @@ static struct ubi_vtbl_record *process_lvol(struct ubi_device *ubi,
 						  ubi->vtbl_size);
 		if (leb_corrupted[1]) {
 			ubi_warn("volume table copy #2 is corrupted");
-			err = create_vtbl(ubi, si, 1, leb[0]);
+			err = create_vtbl(ubi, ai, 1, leb[0]);
 			if (err)
 				goto out_free;
 			ubi_msg("volume table was restored");
@@ -476,7 +470,7 @@ static struct ubi_vtbl_record *process_lvol(struct ubi_device *ubi,
 		}
 
 		ubi_warn("volume table copy #1 is corrupted");
-		err = create_vtbl(ubi, si, 0, leb[1]);
+		err = create_vtbl(ubi, ai, 0, leb[1]);
 		if (err)
 			goto out_free;
 		ubi_msg("volume table was restored");
@@ -494,13 +488,13 @@ out_free:
 /**
  * create_empty_lvol - create empty layout volume.
  * @ubi: UBI device description object
- * @si: scanning information
+ * @ai: attaching information
  *
  * This function returns volume table contents in case of success and a
  * negative error code in case of failure.
  */
 static struct ubi_vtbl_record *create_empty_lvol(struct ubi_device *ubi,
-						 struct ubi_scan_info *si)
+						 struct ubi_attach_info *ai)
 {
 	int i;
 	struct ubi_vtbl_record *vtbl;
@@ -515,7 +509,7 @@ static struct ubi_vtbl_record *create_empty_lvol(struct ubi_device *ubi,
 	for (i = 0; i < UBI_LAYOUT_VOLUME_EBS; i++) {
 		int err;
 
-		err = create_vtbl(ubi, si, i, vtbl);
+		err = create_vtbl(ubi, ai, i, vtbl);
 		if (err) {
 			vfree(vtbl);
 			return ERR_PTR(err);
@@ -528,18 +522,19 @@ static struct ubi_vtbl_record *create_empty_lvol(struct ubi_device *ubi,
 /**
  * init_volumes - initialize volume information for existing volumes.
  * @ubi: UBI device description object
- * @si: scanning information
+ * @ai: scanning information
  * @vtbl: volume table
  *
  * This function allocates volume description objects for existing volumes.
  * Returns zero in case of success and a negative error code in case of
  * failure.
  */
-static int init_volumes(struct ubi_device *ubi, const struct ubi_scan_info *si,
+static int init_volumes(struct ubi_device *ubi,
+			const struct ubi_attach_info *ai,
 			const struct ubi_vtbl_record *vtbl)
 {
 	int i, reserved_pebs = 0;
-	struct ubi_scan_volume *sv;
+	struct ubi_ainf_volume *av;
 	struct ubi_volume *vol;
 
 	for (i = 0; i < ubi->vtbl_slots; i++) {
@@ -595,8 +590,8 @@ static int init_volumes(struct ubi_device *ubi, const struct ubi_scan_info *si,
 		}
 
 		/* Static volumes only */
-		sv = ubi_scan_find_sv(si, i);
-		if (!sv) {
+		av = ubi_find_av(ai, i);
+		if (!av) {
 			/*
 			 * No eraseblocks belonging to this volume found. We
 			 * don't actually know whether this static volume is
@@ -608,22 +603,22 @@ static int init_volumes(struct ubi_device *ubi, const struct ubi_scan_info *si,
 			continue;
 		}
 
-		if (sv->leb_count != sv->used_ebs) {
+		if (av->leb_count != av->used_ebs) {
 			/*
 			 * We found a static volume which misses several
 			 * eraseblocks. Treat it as corrupted.
 			 */
 			ubi_warn("static volume %d misses %d LEBs - corrupted",
-				 sv->vol_id, sv->used_ebs - sv->leb_count);
+				 av->vol_id, av->used_ebs - av->leb_count);
 			vol->corrupted = 1;
 			continue;
 		}
 
-		vol->used_ebs = sv->used_ebs;
+		vol->used_ebs = av->used_ebs;
 		vol->used_bytes =
 			(long long)(vol->used_ebs - 1) * vol->usable_leb_size;
-		vol->used_bytes += sv->last_data_size;
-		vol->last_eb_bytes = sv->last_data_size;
+		vol->used_bytes += av->last_data_size;
+		vol->last_eb_bytes = av->last_data_size;
 	}
 
 	/* And add the layout volume */
@@ -664,105 +659,104 @@ static int init_volumes(struct ubi_device *ubi, const struct ubi_scan_info *si,
 }
 
 /**
- * check_sv - check volume scanning information.
+ * check_av - check volume attaching information.
  * @vol: UBI volume description object
- * @sv: volume scanning information
+ * @av: volume attaching information
  *
- * This function returns zero if the volume scanning information is consistent
+ * This function returns zero if the volume attaching information is consistent
  * to the data read from the volume tabla, and %-EINVAL if not.
  */
-static int check_sv(const struct ubi_volume *vol,
-		    const struct ubi_scan_volume *sv)
+static int check_av(const struct ubi_volume *vol,
+		    const struct ubi_ainf_volume *av)
 {
 	int err;
 
-	if (sv->highest_lnum >= vol->reserved_pebs) {
+	if (av->highest_lnum >= vol->reserved_pebs) {
 		err = 1;
 		goto bad;
 	}
-	if (sv->leb_count > vol->reserved_pebs) {
+	if (av->leb_count > vol->reserved_pebs) {
 		err = 2;
 		goto bad;
 	}
-	if (sv->vol_type != vol->vol_type) {
+	if (av->vol_type != vol->vol_type) {
 		err = 3;
 		goto bad;
 	}
-	if (sv->used_ebs > vol->reserved_pebs) {
+	if (av->used_ebs > vol->reserved_pebs) {
 		err = 4;
 		goto bad;
 	}
-	if (sv->data_pad != vol->data_pad) {
+	if (av->data_pad != vol->data_pad) {
 		err = 5;
 		goto bad;
 	}
 	return 0;
 
 bad:
-	ubi_err("bad scanning information, error %d", err);
-	ubi_dbg_dump_sv(sv);
-	ubi_dbg_dump_vol_info(vol);
+	ubi_err("bad attaching information, error %d", err);
+	ubi_dump_av(av);
+	ubi_dump_vol_info(vol);
 	return -EINVAL;
 }
 
 /**
- * check_scanning_info - check that scanning information.
+ * check_attaching_info - check that attaching information.
  * @ubi: UBI device description object
- * @si: scanning information
+ * @ai: attaching information
  *
  * Even though we protect on-flash data by CRC checksums, we still don't trust
- * the media. This function ensures that scanning information is consistent to
- * the information read from the volume table. Returns zero if the scanning
+ * the media. This function ensures that attaching information is consistent to
+ * the information read from the volume table. Returns zero if the attaching
  * information is OK and %-EINVAL if it is not.
  */
-static int check_scanning_info(const struct ubi_device *ubi,
-			       struct ubi_scan_info *si)
+static int check_attaching_info(const struct ubi_device *ubi,
+			       struct ubi_attach_info *ai)
 {
 	int err, i;
-	struct ubi_scan_volume *sv;
+	struct ubi_ainf_volume *av;
 	struct ubi_volume *vol;
 
-	if (si->vols_found > UBI_INT_VOL_COUNT + ubi->vtbl_slots) {
-		ubi_err("scanning found %d volumes, maximum is %d + %d",
-			si->vols_found, UBI_INT_VOL_COUNT, ubi->vtbl_slots);
+	if (ai->vols_found > UBI_INT_VOL_COUNT + ubi->vtbl_slots) {
+		ubi_err("found %d volumes while attaching, maximum is %d + %d",
+			ai->vols_found, UBI_INT_VOL_COUNT, ubi->vtbl_slots);
 		return -EINVAL;
 	}
 
-	if (si->highest_vol_id >= ubi->vtbl_slots + UBI_INT_VOL_COUNT &&
-	    si->highest_vol_id < UBI_INTERNAL_VOL_START) {
-		ubi_err("too large volume ID %d found by scanning",
-			si->highest_vol_id);
+	if (ai->highest_vol_id >= ubi->vtbl_slots + UBI_INT_VOL_COUNT &&
+	    ai->highest_vol_id < UBI_INTERNAL_VOL_START) {
+		ubi_err("too large volume ID %d found", ai->highest_vol_id);
 		return -EINVAL;
 	}
 
 	for (i = 0; i < ubi->vtbl_slots + UBI_INT_VOL_COUNT; i++) {
 		cond_resched();
 
-		sv = ubi_scan_find_sv(si, i);
+		av = ubi_find_av(ai, i);
 		vol = ubi->volumes[i];
 		if (!vol) {
-			if (sv)
-				ubi_scan_rm_volume(si, sv);
+			if (av)
+				ubi_remove_av(ai, av);
 			continue;
 		}
 
 		if (vol->reserved_pebs == 0) {
 			ubi_assert(i < ubi->vtbl_slots);
 
-			if (!sv)
+			if (!av)
 				continue;
 
 			/*
-			 * During scanning we found a volume which does not
+			 * During attaching we found a volume which does not
 			 * exist according to the information in the volume
 			 * table. This must have happened due to an unclean
 			 * reboot while the volume was being removed. Discard
 			 * these eraseblocks.
 			 */
-			ubi_msg("finish volume %d removal", sv->vol_id);
-			ubi_scan_rm_volume(si, sv);
-		} else if (sv) {
-			err = check_sv(vol, sv);
+			ubi_msg("finish volume %d removal", av->vol_id);
+			ubi_remove_av(ai, av);
+		} else if (av) {
+			err = check_av(vol, av);
 			if (err)
 				return err;
 		}
@@ -774,16 +768,16 @@ static int check_scanning_info(const struct ubi_device *ubi,
 /**
  * ubi_read_volume_table - read the volume table.
  * @ubi: UBI device description object
- * @si: scanning information
+ * @ai: attaching information
  *
  * This function reads volume table, checks it, recover from errors if needed,
  * or creates it if needed. Returns zero in case of success and a negative
  * error code in case of failure.
  */
-int ubi_read_volume_table(struct ubi_device *ubi, struct ubi_scan_info *si)
+int ubi_read_volume_table(struct ubi_device *ubi, struct ubi_attach_info *ai)
 {
 	int i, err;
-	struct ubi_scan_volume *sv;
+	struct ubi_ainf_volume *av;
 
 	empty_vtbl_record.crc = cpu_to_be32(0xf116c36b);
 
@@ -798,8 +792,8 @@ int ubi_read_volume_table(struct ubi_device *ubi, struct ubi_scan_info *si)
 	ubi->vtbl_size = ubi->vtbl_slots * UBI_VTBL_RECORD_SIZE;
 	ubi->vtbl_size = ALIGN(ubi->vtbl_size, ubi->min_io_size);
 
-	sv = ubi_scan_find_sv(si, UBI_LAYOUT_VOLUME_ID);
-	if (!sv) {
+	av = ubi_find_av(ai, UBI_LAYOUT_VOLUME_ID);
+	if (!av) {
 		/*
 		 * No logical eraseblocks belonging to the layout volume were
 		 * found. This could mean that the flash is just empty. In
@@ -808,8 +802,8 @@ int ubi_read_volume_table(struct ubi_device *ubi, struct ubi_scan_info *si)
 		 * But if flash is not empty this must be a corruption or the
 		 * MTD device just contains garbage.
 		 */
-		if (si->is_empty) {
-			ubi->vtbl = create_empty_lvol(ubi, si);
+		if (ai->is_empty) {
+			ubi->vtbl = create_empty_lvol(ubi, ai);
 			if (IS_ERR(ubi->vtbl))
 				return PTR_ERR(ubi->vtbl);
 		} else {
@@ -817,14 +811,14 @@ int ubi_read_volume_table(struct ubi_device *ubi, struct ubi_scan_info *si)
 			return -EINVAL;
 		}
 	} else {
-		if (sv->leb_count > UBI_LAYOUT_VOLUME_EBS) {
+		if (av->leb_count > UBI_LAYOUT_VOLUME_EBS) {
 			/* This must not happen with proper UBI images */
-			dbg_err("too many LEBs (%d) in layout volume",
-				sv->leb_count);
+			ubi_err("too many LEBs (%d) in layout volume",
+				av->leb_count);
 			return -EINVAL;
 		}
 
-		ubi->vtbl = process_lvol(ubi, si, sv);
+		ubi->vtbl = process_lvol(ubi, ai, av);
 		if (IS_ERR(ubi->vtbl))
 			return PTR_ERR(ubi->vtbl);
 	}
@@ -835,15 +829,15 @@ int ubi_read_volume_table(struct ubi_device *ubi, struct ubi_scan_info *si)
 	 * The layout volume is OK, initialize the corresponding in-RAM data
 	 * structures.
 	 */
-	err = init_volumes(ubi, si, ubi->vtbl);
+	err = init_volumes(ubi, ai, ubi->vtbl);
 	if (err)
 		goto out_free;
 
 	/*
-	 * Make sure that the scanning information is consistent to the
+	 * Make sure that the attaching information is consistent to the
 	 * information stored in the volume table.
 	 */
-	err = check_scanning_info(ubi, si);
+	err = check_attaching_info(ubi, ai);
 	if (err)
 		goto out_free;
 
@@ -858,21 +852,17 @@ out_free:
 	return err;
 }
 
-#ifdef CONFIG_MTD_UBI_DEBUG
-
 /**
- * paranoid_vtbl_check - check volume table.
+ * self_vtbl_check - check volume table.
  * @ubi: UBI device description object
  */
-static void paranoid_vtbl_check(const struct ubi_device *ubi)
+static void self_vtbl_check(const struct ubi_device *ubi)
 {
 	if (!ubi->dbg->chk_gen)
 		return;
 
 	if (vtbl_check(ubi, ubi->vtbl)) {
-		ubi_err("paranoid check failed");
+		ubi_err("self-check failed");
 		BUG();
 	}
 }
-
-#endif /* CONFIG_MTD_UBI_DEBUG */
diff --git a/drivers/mtd/ubi/wl.c b/drivers/mtd/ubi/wl.c
index 7c1a9bf8ac8..9df100a4ec3 100644
--- a/drivers/mtd/ubi/wl.c
+++ b/drivers/mtd/ubi/wl.c
@@ -41,12 +41,6 @@
  * physical eraseblocks with low erase counter to free physical eraseblocks
  * with high erase counter.
  *
- * The 'ubi_wl_get_peb()' function accepts data type hints which help to pick
- * an "optimal" physical eraseblock. For example, when it is known that the
- * physical eraseblock will be "put" soon because it contains short-term data,
- * the WL sub-system may pick a free physical eraseblock with low erase
- * counter, and so forth.
- *
  * If the WL sub-system fails to erase a physical eraseblock, it marks it as
  * bad.
  *
@@ -70,8 +64,7 @@
  *    to the user; instead, we first want to let users fill them up with data;
  *
  *  o there is a chance that the user will put the physical eraseblock very
- *    soon, so it makes sense not to move it for some time, but wait; this is
- *    especially important in case of "short term" physical eraseblocks.
+ *    soon, so it makes sense not to move it for some time, but wait.
  *
  * Physical eraseblocks stay protected only for limited time. But the "time" is
  * measured in erase cycles in this case. This is implemented with help of the
@@ -147,6 +140,8 @@
  * @list: a link in the list of pending works
  * @func: worker function
  * @e: physical eraseblock to erase
+ * @vol_id: the volume ID on which this erasure is being performed
+ * @lnum: the logical eraseblock number
  * @torture: if the physical eraseblock has to be tortured
  *
  * The @func pointer points to the worker function. If the @cancel argument is
@@ -159,21 +154,16 @@ struct ubi_work {
 	int (*func)(struct ubi_device *ubi, struct ubi_work *wrk, int cancel);
 	/* The below fields are only relevant to erasure works */
 	struct ubi_wl_entry *e;
+	int vol_id;
+	int lnum;
 	int torture;
 };
 
-#ifdef CONFIG_MTD_UBI_DEBUG
-static int paranoid_check_ec(struct ubi_device *ubi, int pnum, int ec);
-static int paranoid_check_in_wl_tree(const struct ubi_device *ubi,
-				     struct ubi_wl_entry *e,
-				     struct rb_root *root);
-static int paranoid_check_in_pq(const struct ubi_device *ubi,
-				struct ubi_wl_entry *e);
-#else
-#define paranoid_check_ec(ubi, pnum, ec) 0
-#define paranoid_check_in_wl_tree(ubi, e, root)
-#define paranoid_check_in_pq(ubi, e) 0
-#endif
+static int self_check_ec(struct ubi_device *ubi, int pnum, int ec);
+static int self_check_in_wl_tree(const struct ubi_device *ubi,
+				 struct ubi_wl_entry *e, struct rb_root *root);
+static int self_check_in_pq(const struct ubi_device *ubi,
+			    struct ubi_wl_entry *e);
 
 /**
  * wl_tree_add - add a wear-leveling entry to a WL RB-tree.
@@ -383,19 +373,15 @@ static struct ubi_wl_entry *find_wl_entry(struct rb_root *root, int diff)
 /**
  * ubi_wl_get_peb - get a physical eraseblock.
  * @ubi: UBI device description object
- * @dtype: type of data which will be stored in this physical eraseblock
  *
  * This function returns a physical eraseblock in case of success and a
  * negative error code in case of failure. Might sleep.
  */
-int ubi_wl_get_peb(struct ubi_device *ubi, int dtype)
+int ubi_wl_get_peb(struct ubi_device *ubi)
 {
 	int err;
 	struct ubi_wl_entry *e, *first, *last;
 
-	ubi_assert(dtype == UBI_LONGTERM || dtype == UBI_SHORTTERM ||
-		   dtype == UBI_UNKNOWN);
-
 retry:
 	spin_lock(&ubi->wl_lock);
 	if (!ubi->free.rb_node) {
@@ -413,45 +399,15 @@ retry:
 		goto retry;
 	}
 
-	switch (dtype) {
-	case UBI_LONGTERM:
-		/*
-		 * For long term data we pick a physical eraseblock with high
-		 * erase counter. But the highest erase counter we can pick is
-		 * bounded by the the lowest erase counter plus
-		 * %WL_FREE_MAX_DIFF.
-		 */
-		e = find_wl_entry(&ubi->free, WL_FREE_MAX_DIFF);
-		break;
-	case UBI_UNKNOWN:
-		/*
-		 * For unknown data we pick a physical eraseblock with medium
-		 * erase counter. But we by no means can pick a physical
-		 * eraseblock with erase counter greater or equivalent than the
-		 * lowest erase counter plus %WL_FREE_MAX_DIFF/2.
-		 */
-		first = rb_entry(rb_first(&ubi->free), struct ubi_wl_entry,
-					u.rb);
-		last = rb_entry(rb_last(&ubi->free), struct ubi_wl_entry, u.rb);
-
-		if (last->ec - first->ec < WL_FREE_MAX_DIFF)
-			e = rb_entry(ubi->free.rb_node,
-					struct ubi_wl_entry, u.rb);
-		else
-			e = find_wl_entry(&ubi->free, WL_FREE_MAX_DIFF/2);
-		break;
-	case UBI_SHORTTERM:
-		/*
-		 * For short term data we pick a physical eraseblock with the
-		 * lowest erase counter as we expect it will be erased soon.
-		 */
-		e = rb_entry(rb_first(&ubi->free), struct ubi_wl_entry, u.rb);
-		break;
-	default:
-		BUG();
-	}
+	first = rb_entry(rb_first(&ubi->free), struct ubi_wl_entry, u.rb);
+	last = rb_entry(rb_last(&ubi->free), struct ubi_wl_entry, u.rb);
+
+	if (last->ec - first->ec < WL_FREE_MAX_DIFF)
+		e = rb_entry(ubi->free.rb_node, struct ubi_wl_entry, u.rb);
+	else
+		e = find_wl_entry(&ubi->free, WL_FREE_MAX_DIFF/2);
 
-	paranoid_check_in_wl_tree(ubi, e, &ubi->free);
+	self_check_in_wl_tree(ubi, e, &ubi->free);
 
 	/*
 	 * Move the physical eraseblock to the protection queue where it will
@@ -462,8 +418,8 @@ retry:
 	prot_queue_add(ubi, e);
 	spin_unlock(&ubi->wl_lock);
 
-	err = ubi_dbg_check_all_ff(ubi, e->pnum, ubi->vid_hdr_aloffset,
-				   ubi->peb_size - ubi->vid_hdr_aloffset);
+	err = ubi_self_check_all_ff(ubi, e->pnum, ubi->vid_hdr_aloffset,
+				    ubi->peb_size - ubi->vid_hdr_aloffset);
 	if (err) {
 		ubi_err("new PEB %d does not contain all 0xFF bytes", e->pnum);
 		return err;
@@ -488,7 +444,7 @@ static int prot_queue_del(struct ubi_device *ubi, int pnum)
 	if (!e)
 		return -ENODEV;
 
-	if (paranoid_check_in_pq(ubi, e))
+	if (self_check_in_pq(ubi, e))
 		return -ENODEV;
 
 	list_del(&e->u.list);
@@ -514,7 +470,7 @@ static int sync_erase(struct ubi_device *ubi, struct ubi_wl_entry *e,
 
 	dbg_wl("erase PEB %d, old EC %llu", e->pnum, ec);
 
-	err = paranoid_check_ec(ubi, e->pnum, e->ec);
+	err = self_check_ec(ubi, e->pnum, e->ec);
 	if (err)
 		return -EINVAL;
 
@@ -627,13 +583,15 @@ static int erase_worker(struct ubi_device *ubi, struct ubi_work *wl_wrk,
  * schedule_erase - schedule an erase work.
  * @ubi: UBI device description object
  * @e: the WL entry of the physical eraseblock to erase
+ * @vol_id: the volume ID that last used this PEB
+ * @lnum: the last used logical eraseblock number for the PEB
  * @torture: if the physical eraseblock has to be tortured
  *
  * This function returns zero in case of success and a %-ENOMEM in case of
  * failure.
  */
 static int schedule_erase(struct ubi_device *ubi, struct ubi_wl_entry *e,
-			  int torture)
+			  int vol_id, int lnum, int torture)
 {
 	struct ubi_work *wl_wrk;
 
@@ -646,6 +604,8 @@ static int schedule_erase(struct ubi_device *ubi, struct ubi_wl_entry *e,
 
 	wl_wrk->func = &erase_worker;
 	wl_wrk->e = e;
+	wl_wrk->vol_id = vol_id;
+	wl_wrk->lnum = lnum;
 	wl_wrk->torture = torture;
 
 	schedule_ubi_work(ubi, wl_wrk);
@@ -714,7 +674,7 @@ static int wear_leveling_worker(struct ubi_device *ubi, struct ubi_work *wrk,
 			       e1->ec, e2->ec);
 			goto out_cancel;
 		}
-		paranoid_check_in_wl_tree(ubi, e1, &ubi->used);
+		self_check_in_wl_tree(ubi, e1, &ubi->used);
 		rb_erase(&e1->u.rb, &ubi->used);
 		dbg_wl("move PEB %d EC %d to PEB %d EC %d",
 		       e1->pnum, e1->ec, e2->pnum, e2->ec);
@@ -723,12 +683,12 @@ static int wear_leveling_worker(struct ubi_device *ubi, struct ubi_work *wrk,
 		scrubbing = 1;
 		e1 = rb_entry(rb_first(&ubi->scrub), struct ubi_wl_entry, u.rb);
 		e2 = find_wl_entry(&ubi->free, WL_FREE_MAX_DIFF);
-		paranoid_check_in_wl_tree(ubi, e1, &ubi->scrub);
+		self_check_in_wl_tree(ubi, e1, &ubi->scrub);
 		rb_erase(&e1->u.rb, &ubi->scrub);
 		dbg_wl("scrub PEB %d to PEB %d", e1->pnum, e2->pnum);
 	}
 
-	paranoid_check_in_wl_tree(ubi, e2, &ubi->free);
+	self_check_in_wl_tree(ubi, e2, &ubi->free);
 	rb_erase(&e2->u.rb, &ubi->free);
 	ubi->move_from = e1;
 	ubi->move_to = e2;
@@ -846,7 +806,7 @@ static int wear_leveling_worker(struct ubi_device *ubi, struct ubi_work *wrk,
 	ubi->move_to_put = ubi->wl_scheduled = 0;
 	spin_unlock(&ubi->wl_lock);
 
-	err = schedule_erase(ubi, e1, 0);
+	err = schedule_erase(ubi, e1, vol_id, lnum, 0);
 	if (err) {
 		kmem_cache_free(ubi_wl_entry_slab, e1);
 		if (e2)
@@ -861,7 +821,7 @@ static int wear_leveling_worker(struct ubi_device *ubi, struct ubi_work *wrk,
 		 */
 		dbg_wl("PEB %d (LEB %d:%d) was put meanwhile, erase",
 		       e2->pnum, vol_id, lnum);
-		err = schedule_erase(ubi, e2, 0);
+		err = schedule_erase(ubi, e2, vol_id, lnum, 0);
 		if (err) {
 			kmem_cache_free(ubi_wl_entry_slab, e2);
 			goto out_ro;
@@ -900,7 +860,7 @@ out_not_moved:
 	spin_unlock(&ubi->wl_lock);
 
 	ubi_free_vid_hdr(ubi, vid_hdr);
-	err = schedule_erase(ubi, e2, torture);
+	err = schedule_erase(ubi, e2, vol_id, lnum, torture);
 	if (err) {
 		kmem_cache_free(ubi_wl_entry_slab, e2);
 		goto out_ro;
@@ -1019,6 +979,8 @@ static int erase_worker(struct ubi_device *ubi, struct ubi_work *wl_wrk,
 {
 	struct ubi_wl_entry *e = wl_wrk->e;
 	int pnum = e->pnum, err, need;
+	int vol_id = wl_wrk->vol_id;
+	int lnum = wl_wrk->lnum;
 
 	if (cancel) {
 		dbg_wl("cancel erasure of PEB %d EC %d", pnum, e->ec);
@@ -1027,7 +989,8 @@ static int erase_worker(struct ubi_device *ubi, struct ubi_work *wl_wrk,
 		return 0;
 	}
 
-	dbg_wl("erase PEB %d EC %d", pnum, e->ec);
+	dbg_wl("erase PEB %d EC %d LEB %d:%d",
+	       pnum, e->ec, wl_wrk->vol_id, wl_wrk->lnum);
 
 	err = sync_erase(ubi, e, wl_wrk->torture);
 	if (!err) {
@@ -1057,7 +1020,7 @@ static int erase_worker(struct ubi_device *ubi, struct ubi_work *wl_wrk,
 		int err1;
 
 		/* Re-schedule the LEB for erasure */
-		err1 = schedule_erase(ubi, e, 0);
+		err1 = schedule_erase(ubi, e, vol_id, lnum, 0);
 		if (err1) {
 			err = err1;
 			goto out_ro;
@@ -1125,6 +1088,8 @@ out_ro:
 /**
  * ubi_wl_put_peb - return a PEB to the wear-leveling sub-system.
  * @ubi: UBI device description object
+ * @vol_id: the volume ID that last used this PEB
+ * @lnum: the last used logical eraseblock number for the PEB
  * @pnum: physical eraseblock to return
  * @torture: if this physical eraseblock has to be tortured
  *
@@ -1133,7 +1098,8 @@ out_ro:
  * occurred to this @pnum and it has to be tested. This function returns zero
  * in case of success, and a negative error code in case of failure.
  */
-int ubi_wl_put_peb(struct ubi_device *ubi, int pnum, int torture)
+int ubi_wl_put_peb(struct ubi_device *ubi, int vol_id, int lnum,
+		   int pnum, int torture)
 {
 	int err;
 	struct ubi_wl_entry *e;
@@ -1175,13 +1141,13 @@ retry:
 		return 0;
 	} else {
 		if (in_wl_tree(e, &ubi->used)) {
-			paranoid_check_in_wl_tree(ubi, e, &ubi->used);
+			self_check_in_wl_tree(ubi, e, &ubi->used);
 			rb_erase(&e->u.rb, &ubi->used);
 		} else if (in_wl_tree(e, &ubi->scrub)) {
-			paranoid_check_in_wl_tree(ubi, e, &ubi->scrub);
+			self_check_in_wl_tree(ubi, e, &ubi->scrub);
 			rb_erase(&e->u.rb, &ubi->scrub);
 		} else if (in_wl_tree(e, &ubi->erroneous)) {
-			paranoid_check_in_wl_tree(ubi, e, &ubi->erroneous);
+			self_check_in_wl_tree(ubi, e, &ubi->erroneous);
 			rb_erase(&e->u.rb, &ubi->erroneous);
 			ubi->erroneous_peb_count -= 1;
 			ubi_assert(ubi->erroneous_peb_count >= 0);
@@ -1199,7 +1165,7 @@ retry:
 	}
 	spin_unlock(&ubi->wl_lock);
 
-	err = schedule_erase(ubi, e, torture);
+	err = schedule_erase(ubi, e, vol_id, lnum, torture);
 	if (err) {
 		spin_lock(&ubi->wl_lock);
 		wl_tree_add(e, &ubi->used);
@@ -1248,7 +1214,7 @@ retry:
 	}
 
 	if (in_wl_tree(e, &ubi->used)) {
-		paranoid_check_in_wl_tree(ubi, e, &ubi->used);
+		self_check_in_wl_tree(ubi, e, &ubi->used);
 		rb_erase(&e->u.rb, &ubi->used);
 	} else {
 		int err;
@@ -1275,44 +1241,55 @@ retry:
 /**
  * ubi_wl_flush - flush all pending works.
  * @ubi: UBI device description object
+ * @vol_id: the volume id to flush for
+ * @lnum: the logical eraseblock number to flush for
  *
- * This function returns zero in case of success and a negative error code in
- * case of failure.
+ * This function executes all pending works for a particular volume id /
+ * logical eraseblock number pair. If either value is set to %UBI_ALL, then it
+ * acts as a wildcard for all of the corresponding volume numbers or logical
+ * eraseblock numbers. It returns zero in case of success and a negative error
+ * code in case of failure.
  */
-int ubi_wl_flush(struct ubi_device *ubi)
+int ubi_wl_flush(struct ubi_device *ubi, int vol_id, int lnum)
 {
-	int err;
+	int err = 0;
+	int found = 1;
 
 	/*
 	 * Erase while the pending works queue is not empty, but not more than
 	 * the number of currently pending works.
 	 */
-	dbg_wl("flush (%d pending works)", ubi->works_count);
-	while (ubi->works_count) {
-		err = do_work(ubi);
-		if (err)
-			return err;
-	}
+	dbg_wl("flush pending work for LEB %d:%d (%d pending works)",
+	       vol_id, lnum, ubi->works_count);
 
-	/*
-	 * Make sure all the works which have been done in parallel are
-	 * finished.
-	 */
 	down_write(&ubi->work_sem);
-	up_write(&ubi->work_sem);
+	while (found) {
+		struct ubi_work *wrk;
+		found = 0;
 
-	/*
-	 * And in case last was the WL worker and it canceled the LEB
-	 * movement, flush again.
-	 */
-	while (ubi->works_count) {
-		dbg_wl("flush more (%d pending works)", ubi->works_count);
-		err = do_work(ubi);
-		if (err)
-			return err;
+		spin_lock(&ubi->wl_lock);
+		list_for_each_entry(wrk, &ubi->works, list) {
+			if ((vol_id == UBI_ALL || wrk->vol_id == vol_id) &&
+			    (lnum == UBI_ALL || wrk->lnum == lnum)) {
+				list_del(&wrk->list);
+				ubi->works_count -= 1;
+				ubi_assert(ubi->works_count >= 0);
+				spin_unlock(&ubi->wl_lock);
+
+				err = wrk->func(ubi, wrk, 0);
+				if (err)
+					goto out;
+				spin_lock(&ubi->wl_lock);
+				found = 1;
+				break;
+			}
+		}
+		spin_unlock(&ubi->wl_lock);
 	}
 
-	return 0;
+out:
+	up_write(&ubi->work_sem);
+	return err;
 }
 
 /**
@@ -1421,26 +1398,26 @@ static void cancel_pending(struct ubi_device *ubi)
 }
 
 /**
- * ubi_wl_init_scan - initialize the WL sub-system using scanning information.
+ * ubi_wl_init - initialize the WL sub-system using attaching information.
  * @ubi: UBI device description object
- * @si: scanning information
+ * @ai: attaching information
  *
  * This function returns zero in case of success, and a negative error code in
  * case of failure.
  */
-int ubi_wl_init_scan(struct ubi_device *ubi, struct ubi_scan_info *si)
+int ubi_wl_init(struct ubi_device *ubi, struct ubi_attach_info *ai)
 {
 	int err, i;
 	struct rb_node *rb1, *rb2;
-	struct ubi_scan_volume *sv;
-	struct ubi_scan_leb *seb, *tmp;
+	struct ubi_ainf_volume *av;
+	struct ubi_ainf_peb *aeb, *tmp;
 	struct ubi_wl_entry *e;
 
 	ubi->used = ubi->erroneous = ubi->free = ubi->scrub = RB_ROOT;
 	spin_lock_init(&ubi->wl_lock);
 	mutex_init(&ubi->move_mutex);
 	init_rwsem(&ubi->work_sem);
-	ubi->max_ec = si->max_ec;
+	ubi->max_ec = ai->max_ec;
 	INIT_LIST_HEAD(&ubi->works);
 
 	sprintf(ubi->bgt_name, UBI_BGT_NAME_PATTERN, ubi->ubi_num);
@@ -1454,48 +1431,48 @@ int ubi_wl_init_scan(struct ubi_device *ubi, struct ubi_scan_info *si)
 		INIT_LIST_HEAD(&ubi->pq[i]);
 	ubi->pq_head = 0;
 
-	list_for_each_entry_safe(seb, tmp, &si->erase, u.list) {
+	list_for_each_entry_safe(aeb, tmp, &ai->erase, u.list) {
 		cond_resched();
 
 		e = kmem_cache_alloc(ubi_wl_entry_slab, GFP_KERNEL);
 		if (!e)
 			goto out_free;
 
-		e->pnum = seb->pnum;
-		e->ec = seb->ec;
+		e->pnum = aeb->pnum;
+		e->ec = aeb->ec;
 		ubi->lookuptbl[e->pnum] = e;
-		if (schedule_erase(ubi, e, 0)) {
+		if (schedule_erase(ubi, e, aeb->vol_id, aeb->lnum, 0)) {
 			kmem_cache_free(ubi_wl_entry_slab, e);
 			goto out_free;
 		}
 	}
 
-	list_for_each_entry(seb, &si->free, u.list) {
+	list_for_each_entry(aeb, &ai->free, u.list) {
 		cond_resched();
 
 		e = kmem_cache_alloc(ubi_wl_entry_slab, GFP_KERNEL);
 		if (!e)
 			goto out_free;
 
-		e->pnum = seb->pnum;
-		e->ec = seb->ec;
+		e->pnum = aeb->pnum;
+		e->ec = aeb->ec;
 		ubi_assert(e->ec >= 0);
 		wl_tree_add(e, &ubi->free);
 		ubi->lookuptbl[e->pnum] = e;
 	}
 
-	ubi_rb_for_each_entry(rb1, sv, &si->volumes, rb) {
-		ubi_rb_for_each_entry(rb2, seb, &sv->root, u.rb) {
+	ubi_rb_for_each_entry(rb1, av, &ai->volumes, rb) {
+		ubi_rb_for_each_entry(rb2, aeb, &av->root, u.rb) {
 			cond_resched();
 
 			e = kmem_cache_alloc(ubi_wl_entry_slab, GFP_KERNEL);
 			if (!e)
 				goto out_free;
 
-			e->pnum = seb->pnum;
-			e->ec = seb->ec;
+			e->pnum = aeb->pnum;
+			e->ec = aeb->ec;
 			ubi->lookuptbl[e->pnum] = e;
-			if (!seb->scrub) {
+			if (!aeb->scrub) {
 				dbg_wl("add PEB %d EC %d to the used tree",
 				       e->pnum, e->ec);
 				wl_tree_add(e, &ubi->used);
@@ -1567,10 +1544,8 @@ void ubi_wl_close(struct ubi_device *ubi)
 	kfree(ubi->lookuptbl);
 }
 
-#ifdef CONFIG_MTD_UBI_DEBUG
-
 /**
- * paranoid_check_ec - make sure that the erase counter of a PEB is correct.
+ * self_check_ec - make sure that the erase counter of a PEB is correct.
  * @ubi: UBI device description object
  * @pnum: the physical eraseblock number to check
  * @ec: the erase counter to check
@@ -1579,7 +1554,7 @@ void ubi_wl_close(struct ubi_device *ubi)
  * is equivalent to @ec, and a negative error code if not or if an error
  * occurred.
  */
-static int paranoid_check_ec(struct ubi_device *ubi, int pnum, int ec)
+static int self_check_ec(struct ubi_device *ubi, int pnum, int ec)
 {
 	int err;
 	long long read_ec;
@@ -1601,9 +1576,9 @@ static int paranoid_check_ec(struct ubi_device *ubi, int pnum, int ec)
 
 	read_ec = be64_to_cpu(ec_hdr->ec);
 	if (ec != read_ec) {
-		ubi_err("paranoid check failed for PEB %d", pnum);
+		ubi_err("self-check failed for PEB %d", pnum);
 		ubi_err("read EC is %lld, should be %d", read_ec, ec);
-		ubi_dbg_dump_stack();
+		dump_stack();
 		err = 1;
 	} else
 		err = 0;
@@ -1614,7 +1589,7 @@ out_free:
 }
 
 /**
- * paranoid_check_in_wl_tree - check that wear-leveling entry is in WL RB-tree.
+ * self_check_in_wl_tree - check that wear-leveling entry is in WL RB-tree.
  * @ubi: UBI device description object
  * @e: the wear-leveling entry to check
  * @root: the root of the tree
@@ -1622,9 +1597,8 @@ out_free:
  * This function returns zero if @e is in the @root RB-tree and %-EINVAL if it
  * is not.
  */
-static int paranoid_check_in_wl_tree(const struct ubi_device *ubi,
-				     struct ubi_wl_entry *e,
-				     struct rb_root *root)
+static int self_check_in_wl_tree(const struct ubi_device *ubi,
+				 struct ubi_wl_entry *e, struct rb_root *root)
 {
 	if (!ubi->dbg->chk_gen)
 		return 0;
@@ -1632,22 +1606,22 @@ static int paranoid_check_in_wl_tree(const struct ubi_device *ubi,
 	if (in_wl_tree(e, root))
 		return 0;
 
-	ubi_err("paranoid check failed for PEB %d, EC %d, RB-tree %p ",
+	ubi_err("self-check failed for PEB %d, EC %d, RB-tree %p ",
 		e->pnum, e->ec, root);
-	ubi_dbg_dump_stack();
+	dump_stack();
 	return -EINVAL;
 }
 
 /**
- * paranoid_check_in_pq - check if wear-leveling entry is in the protection
+ * self_check_in_pq - check if wear-leveling entry is in the protection
  *                        queue.
  * @ubi: UBI device description object
  * @e: the wear-leveling entry to check
  *
  * This function returns zero if @e is in @ubi->pq and %-EINVAL if it is not.
  */
-static int paranoid_check_in_pq(const struct ubi_device *ubi,
-				struct ubi_wl_entry *e)
+static int self_check_in_pq(const struct ubi_device *ubi,
+			    struct ubi_wl_entry *e)
 {
 	struct ubi_wl_entry *p;
 	int i;
@@ -1660,10 +1634,8 @@ static int paranoid_check_in_pq(const struct ubi_device *ubi,
 			if (p == e)
 				return 0;
 
-	ubi_err("paranoid check failed for PEB %d, EC %d, Protect queue",
+	ubi_err("self-check failed for PEB %d, EC %d, Protect queue",
 		e->pnum, e->ec);
-	ubi_dbg_dump_stack();
+	dump_stack();
 	return -EINVAL;
 }
-
-#endif /* CONFIG_MTD_UBI_DEBUG */
diff --git a/drivers/net/Kconfig b/drivers/net/Kconfig
index b98285446a5..0c2bd806950 100644
--- a/drivers/net/Kconfig
+++ b/drivers/net/Kconfig
@@ -66,10 +66,7 @@ config DUMMY
 	  <http://www.tldp.org/docs.html#guide>.
 
 	  To compile this driver as a module, choose M here: the module
-	  will be called dummy.  If you want to use more than one dummy
-	  device at a time, you need to compile this driver as a module.
-	  Instead of 'dummy', the devices will then be called 'dummy0',
-	  'dummy1' etc.
+	  will be called dummy.
 
 config EQUALIZER
 	tristate "EQL (serial line load balancing) support"
@@ -285,8 +282,6 @@ source "drivers/net/slip/Kconfig"
 
 source "drivers/s390/net/Kconfig"
 
-source "drivers/net/tokenring/Kconfig"
-
 source "drivers/net/usb/Kconfig"
 
 source "drivers/net/wireless/Kconfig"
diff --git a/drivers/net/Makefile b/drivers/net/Makefile
index a6b8ce11a22..3d375ca128a 100644
--- a/drivers/net/Makefile
+++ b/drivers/net/Makefile
@@ -50,7 +50,6 @@ obj-$(CONFIG_SLIP) += slip/
 obj-$(CONFIG_SLHC) += slip/
 obj-$(CONFIG_NET_SB1000) += sb1000.o
 obj-$(CONFIG_SUNGEM_PHY) += sungem_phy.o
-obj-$(CONFIG_TR) += tokenring/
 obj-$(CONFIG_WAN) += wan/
 obj-$(CONFIG_WLAN) += wireless/
 obj-$(CONFIG_WIMAX) += wimax/
diff --git a/drivers/net/Space.c b/drivers/net/Space.c
index 88bbd8ffa7f..e3f0faca98d 100644
--- a/drivers/net/Space.c
+++ b/drivers/net/Space.c
@@ -29,7 +29,6 @@
  */
 #include <linux/netdevice.h>
 #include <linux/etherdevice.h>
-#include <linux/trdevice.h>
 #include <linux/errno.h>
 #include <linux/init.h>
 #include <linux/netlink.h>
@@ -134,22 +133,9 @@ static struct devprobe2 eisa_probes[] __initdata = {
 	{NULL, 0},
 };
 
-static struct devprobe2 mca_probes[] __initdata = {
-#ifdef CONFIG_NE2_MCA
-	{ne2_probe, 0},
-#endif
-#ifdef CONFIG_ELMC		/* 3c523 */
-	{elmc_probe, 0},
-#endif
-#ifdef CONFIG_ELMC_II		/* 3c527 */
-	{mc32_probe, 0},
-#endif
-	{NULL, 0},
-};
-
 /*
  * ISA probes that touch addresses < 0x400 (including those that also
- * look for EISA/PCI/MCA cards in addition to ISA cards).
+ * look for EISA/PCI cards in addition to ISA cards).
  */
 static struct devprobe2 isa_probes[] __initdata = {
 #if defined(CONFIG_HP100) && defined(CONFIG_ISA)	/* ISA, EISA */
@@ -279,51 +265,10 @@ static void __init ethif_probe2(int unit)
 
 	(void)(	probe_list2(unit, m68k_probes, base_addr == 0) &&
 		probe_list2(unit, eisa_probes, base_addr == 0) &&
-		probe_list2(unit, mca_probes, base_addr == 0) &&
 		probe_list2(unit, isa_probes, base_addr == 0) &&
 		probe_list2(unit, parport_probes, base_addr == 0));
 }
 
-#ifdef CONFIG_TR
-/* Token-ring device probe */
-extern int ibmtr_probe_card(struct net_device *);
-extern struct net_device *smctr_probe(int unit);
-
-static struct devprobe2 tr_probes2[] __initdata = {
-#ifdef CONFIG_SMCTR
-	{smctr_probe, 0},
-#endif
-	{NULL, 0},
-};
-
-static __init int trif_probe(int unit)
-{
-	int err = -ENODEV;
-#ifdef CONFIG_IBMTR
-	struct net_device *dev = alloc_trdev(0);
-	if (!dev)
-		return -ENOMEM;
-
-	sprintf(dev->name, "tr%d", unit);
-	netdev_boot_setup_check(dev);
-	err = ibmtr_probe_card(dev);
-	if (err)
-		free_netdev(dev);
-#endif
-	return err;
-}
-
-static void __init trif_probe2(int unit)
-{
-	unsigned long base_addr = netdev_boot_base("tr", unit);
-
-	if (base_addr == 1)
-		return;
-	probe_list2(unit, tr_probes2, base_addr == 0);
-}
-#endif
-
-
 /*  Statically configured drivers -- order matters here. */
 static int __init net_olddevs_init(void)
 {
@@ -333,11 +278,6 @@ static int __init net_olddevs_init(void)
 	for (num = 0; num < 8; ++num)
 		sbni_probe(num);
 #endif
-#ifdef CONFIG_TR
-	for (num = 0; num < 8; ++num)
-		if (!trif_probe(num))
-			trif_probe2(num);
-#endif
 	for (num = 0; num < 8; ++num)
 		ethif_probe2(num);
 
diff --git a/drivers/net/bonding/bond_3ad.c b/drivers/net/bonding/bond_3ad.c
index 793b0013827..3463b469e65 100644
--- a/drivers/net/bonding/bond_3ad.c
+++ b/drivers/net/bonding/bond_3ad.c
@@ -2173,9 +2173,10 @@ re_arm:
  * received frames (loopback). Since only the payload is given to this
  * function, it check for loopback.
  */
-static void bond_3ad_rx_indication(struct lacpdu *lacpdu, struct slave *slave, u16 length)
+static int bond_3ad_rx_indication(struct lacpdu *lacpdu, struct slave *slave, u16 length)
 {
 	struct port *port;
+	int ret = RX_HANDLER_ANOTHER;
 
 	if (length >= sizeof(struct lacpdu)) {
 
@@ -2184,11 +2185,12 @@ static void bond_3ad_rx_indication(struct lacpdu *lacpdu, struct slave *slave, u
 		if (!port->slave) {
 			pr_warning("%s: Warning: port of slave %s is uninitialized\n",
 				   slave->dev->name, slave->dev->master->name);
-			return;
+			return ret;
 		}
 
 		switch (lacpdu->subtype) {
 		case AD_TYPE_LACPDU:
+			ret = RX_HANDLER_CONSUMED;
 			pr_debug("Received LACPDU on port %d\n",
 				 port->actor_port_number);
 			/* Protect against concurrent state machines */
@@ -2198,6 +2200,7 @@ static void bond_3ad_rx_indication(struct lacpdu *lacpdu, struct slave *slave, u
 			break;
 
 		case AD_TYPE_MARKER:
+			ret = RX_HANDLER_CONSUMED;
 			// No need to convert fields to Little Endian since we don't use the marker's fields.
 
 			switch (((struct bond_marker *)lacpdu)->tlv_type) {
@@ -2219,6 +2222,7 @@ static void bond_3ad_rx_indication(struct lacpdu *lacpdu, struct slave *slave, u
 			}
 		}
 	}
+	return ret;
 }
 
 /**
@@ -2456,18 +2460,20 @@ out:
 	return NETDEV_TX_OK;
 }
 
-void bond_3ad_lacpdu_recv(struct sk_buff *skb, struct bonding *bond,
+int bond_3ad_lacpdu_recv(struct sk_buff *skb, struct bonding *bond,
 			  struct slave *slave)
 {
+	int ret = RX_HANDLER_ANOTHER;
 	if (skb->protocol != PKT_TYPE_LACPDU)
-		return;
+		return ret;
 
 	if (!pskb_may_pull(skb, sizeof(struct lacpdu)))
-		return;
+		return ret;
 
 	read_lock(&bond->lock);
-	bond_3ad_rx_indication((struct lacpdu *) skb->data, slave, skb->len);
+	ret = bond_3ad_rx_indication((struct lacpdu *) skb->data, slave, skb->len);
 	read_unlock(&bond->lock);
+	return ret;
 }
 
 /*
diff --git a/drivers/net/bonding/bond_3ad.h b/drivers/net/bonding/bond_3ad.h
index 235b2cc58b2..5ee7e3c45db 100644
--- a/drivers/net/bonding/bond_3ad.h
+++ b/drivers/net/bonding/bond_3ad.h
@@ -274,7 +274,7 @@ void bond_3ad_adapter_duplex_changed(struct slave *slave);
 void bond_3ad_handle_link_change(struct slave *slave, char link);
 int  bond_3ad_get_active_agg_info(struct bonding *bond, struct ad_info *ad_info);
 int bond_3ad_xmit_xor(struct sk_buff *skb, struct net_device *dev);
-void bond_3ad_lacpdu_recv(struct sk_buff *skb, struct bonding *bond,
+int bond_3ad_lacpdu_recv(struct sk_buff *skb, struct bonding *bond,
 			  struct slave *slave);
 int bond_3ad_set_carrier(struct bonding *bond);
 void bond_3ad_update_lacp_rate(struct bonding *bond);
diff --git a/drivers/net/bonding/bond_alb.c b/drivers/net/bonding/bond_alb.c
index 9abfde47931..0f59c1564e5 100644
--- a/drivers/net/bonding/bond_alb.c
+++ b/drivers/net/bonding/bond_alb.c
@@ -332,7 +332,7 @@ static void rlb_update_entry_from_arp(struct bonding *bond, struct arp_pkt *arp)
 	if ((client_info->assigned) &&
 	    (client_info->ip_src == arp->ip_dst) &&
 	    (client_info->ip_dst == arp->ip_src) &&
-	    (compare_ether_addr_64bits(client_info->mac_dst, arp->mac_src))) {
+	    (!ether_addr_equal_64bits(client_info->mac_dst, arp->mac_src))) {
 		/* update the clients MAC address */
 		memcpy(client_info->mac_dst, arp->mac_src, ETH_ALEN);
 		client_info->ntt = 1;
@@ -342,26 +342,26 @@ static void rlb_update_entry_from_arp(struct bonding *bond, struct arp_pkt *arp)
 	_unlock_rx_hashtbl_bh(bond);
 }
 
-static void rlb_arp_recv(struct sk_buff *skb, struct bonding *bond,
+static int rlb_arp_recv(struct sk_buff *skb, struct bonding *bond,
 			 struct slave *slave)
 {
 	struct arp_pkt *arp;
 
 	if (skb->protocol != cpu_to_be16(ETH_P_ARP))
-		return;
+		goto out;
 
 	arp = (struct arp_pkt *) skb->data;
 	if (!arp) {
 		pr_debug("Packet has no ARP data\n");
-		return;
+		goto out;
 	}
 
 	if (!pskb_may_pull(skb, arp_hdr_len(bond->dev)))
-		return;
+		goto out;
 
 	if (skb->len < sizeof(struct arp_pkt)) {
 		pr_debug("Packet is too small to be an ARP\n");
-		return;
+		goto out;
 	}
 
 	if (arp->op_code == htons(ARPOP_REPLY)) {
@@ -369,6 +369,8 @@ static void rlb_arp_recv(struct sk_buff *skb, struct bonding *bond,
 		rlb_update_entry_from_arp(bond, arp);
 		pr_debug("Server received an ARP Reply from client\n");
 	}
+out:
+	return RX_HANDLER_ANOTHER;
 }
 
 /* Caller must hold bond lock for read */
@@ -448,8 +450,8 @@ static void rlb_clear_slave(struct bonding *bond, struct slave *slave)
 
 			if (assigned_slave) {
 				rx_hash_table[index].slave = assigned_slave;
-				if (compare_ether_addr_64bits(rx_hash_table[index].mac_dst,
-							      mac_bcast)) {
+				if (!ether_addr_equal_64bits(rx_hash_table[index].mac_dst,
+							     mac_bcast)) {
 					bond_info->rx_hashtbl[index].ntt = 1;
 					bond_info->rx_ntt = 1;
 					/* A slave has been removed from the
@@ -561,7 +563,7 @@ static void rlb_req_update_slave_clients(struct bonding *bond, struct slave *sla
 		client_info = &(bond_info->rx_hashtbl[hash_index]);
 
 		if ((client_info->slave == slave) &&
-		    compare_ether_addr_64bits(client_info->mac_dst, mac_bcast)) {
+		    !ether_addr_equal_64bits(client_info->mac_dst, mac_bcast)) {
 			client_info->ntt = 1;
 			ntt = 1;
 		}
@@ -600,9 +602,9 @@ static void rlb_req_update_subnet_clients(struct bonding *bond, __be32 src_ip)
 		 * unicast mac address.
 		 */
 		if ((client_info->ip_src == src_ip) &&
-		    compare_ether_addr_64bits(client_info->slave->dev->dev_addr,
-			   bond->dev->dev_addr) &&
-		    compare_ether_addr_64bits(client_info->mac_dst, mac_bcast)) {
+		    !ether_addr_equal_64bits(client_info->slave->dev->dev_addr,
+					     bond->dev->dev_addr) &&
+		    !ether_addr_equal_64bits(client_info->mac_dst, mac_bcast)) {
 			client_info->ntt = 1;
 			bond_info->rx_ntt = 1;
 		}
@@ -629,7 +631,7 @@ static struct slave *rlb_choose_channel(struct sk_buff *skb, struct bonding *bon
 		if ((client_info->ip_src == arp->ip_src) &&
 		    (client_info->ip_dst == arp->ip_dst)) {
 			/* the entry is already assigned to this client */
-			if (compare_ether_addr_64bits(arp->mac_dst, mac_bcast)) {
+			if (!ether_addr_equal_64bits(arp->mac_dst, mac_bcast)) {
 				/* update mac address from arp */
 				memcpy(client_info->mac_dst, arp->mac_dst, ETH_ALEN);
 			}
@@ -664,7 +666,7 @@ static struct slave *rlb_choose_channel(struct sk_buff *skb, struct bonding *bon
 		memcpy(client_info->mac_dst, arp->mac_dst, ETH_ALEN);
 		client_info->slave = assigned_slave;
 
-		if (compare_ether_addr_64bits(client_info->mac_dst, mac_bcast)) {
+		if (!ether_addr_equal_64bits(client_info->mac_dst, mac_bcast)) {
 			client_info->ntt = 1;
 			bond->alb_info.rx_ntt = 1;
 		} else {
@@ -1009,18 +1011,18 @@ static void alb_change_hw_addr_on_detach(struct bonding *bond, struct slave *sla
 	int perm_curr_diff;
 	int perm_bond_diff;
 
-	perm_curr_diff = compare_ether_addr_64bits(slave->perm_hwaddr,
-						   slave->dev->dev_addr);
-	perm_bond_diff = compare_ether_addr_64bits(slave->perm_hwaddr,
-						   bond->dev->dev_addr);
+	perm_curr_diff = !ether_addr_equal_64bits(slave->perm_hwaddr,
+						  slave->dev->dev_addr);
+	perm_bond_diff = !ether_addr_equal_64bits(slave->perm_hwaddr,
+						  bond->dev->dev_addr);
 
 	if (perm_curr_diff && perm_bond_diff) {
 		struct slave *tmp_slave;
 		int i, found = 0;
 
 		bond_for_each_slave(bond, tmp_slave, i) {
-			if (!compare_ether_addr_64bits(slave->perm_hwaddr,
-						       tmp_slave->dev->dev_addr)) {
+			if (ether_addr_equal_64bits(slave->perm_hwaddr,
+						    tmp_slave->dev->dev_addr)) {
 				found = 1;
 				break;
 			}
@@ -1074,10 +1076,10 @@ static int alb_handle_addr_collision_on_attach(struct bonding *bond, struct slav
 	 * check uniqueness of slave's mac address against the other
 	 * slaves in the bond.
 	 */
-	if (compare_ether_addr_64bits(slave->perm_hwaddr, bond->dev->dev_addr)) {
+	if (!ether_addr_equal_64bits(slave->perm_hwaddr, bond->dev->dev_addr)) {
 		bond_for_each_slave(bond, tmp_slave1, i) {
-			if (!compare_ether_addr_64bits(tmp_slave1->dev->dev_addr,
-						       slave->dev->dev_addr)) {
+			if (ether_addr_equal_64bits(tmp_slave1->dev->dev_addr,
+						    slave->dev->dev_addr)) {
 				found = 1;
 				break;
 			}
@@ -1099,8 +1101,8 @@ static int alb_handle_addr_collision_on_attach(struct bonding *bond, struct slav
 	bond_for_each_slave(bond, tmp_slave1, i) {
 		found = 0;
 		bond_for_each_slave(bond, tmp_slave2, j) {
-			if (!compare_ether_addr_64bits(tmp_slave1->perm_hwaddr,
-						       tmp_slave2->dev->dev_addr)) {
+			if (ether_addr_equal_64bits(tmp_slave1->perm_hwaddr,
+						    tmp_slave2->dev->dev_addr)) {
 				found = 1;
 				break;
 			}
@@ -1115,8 +1117,8 @@ static int alb_handle_addr_collision_on_attach(struct bonding *bond, struct slav
 		}
 
 		if (!has_bond_addr) {
-			if (!compare_ether_addr_64bits(tmp_slave1->dev->dev_addr,
-						       bond->dev->dev_addr)) {
+			if (ether_addr_equal_64bits(tmp_slave1->dev->dev_addr,
+						    bond->dev->dev_addr)) {
 
 				has_bond_addr = tmp_slave1;
 			}
@@ -1257,7 +1259,7 @@ int bond_alb_xmit(struct sk_buff *skb, struct net_device *bond_dev)
 	case ETH_P_IP: {
 		const struct iphdr *iph = ip_hdr(skb);
 
-		if (!compare_ether_addr_64bits(eth_data->h_dest, mac_bcast) ||
+		if (ether_addr_equal_64bits(eth_data->h_dest, mac_bcast) ||
 		    (iph->daddr == ip_bcast) ||
 		    (iph->protocol == IPPROTO_IGMP)) {
 			do_tx_balance = 0;
@@ -1271,7 +1273,7 @@ int bond_alb_xmit(struct sk_buff *skb, struct net_device *bond_dev)
 		/* IPv6 doesn't really use broadcast mac address, but leave
 		 * that here just in case.
 		 */
-		if (!compare_ether_addr_64bits(eth_data->h_dest, mac_bcast)) {
+		if (ether_addr_equal_64bits(eth_data->h_dest, mac_bcast)) {
 			do_tx_balance = 0;
 			break;
 		}
@@ -1279,7 +1281,7 @@ int bond_alb_xmit(struct sk_buff *skb, struct net_device *bond_dev)
 		/* IPv6 uses all-nodes multicast as an equivalent to
 		 * broadcasts in IPv4.
 		 */
-		if (!compare_ether_addr_64bits(eth_data->h_dest, mac_v6_allmcast)) {
+		if (ether_addr_equal_64bits(eth_data->h_dest, mac_v6_allmcast)) {
 			do_tx_balance = 0;
 			break;
 		}
@@ -1603,8 +1605,8 @@ void bond_alb_handle_active_change(struct bonding *bond, struct slave *new_slave
 		struct slave *tmp_slave;
 		/* find slave that is holding the bond's mac address */
 		bond_for_each_slave(bond, tmp_slave, i) {
-			if (!compare_ether_addr_64bits(tmp_slave->dev->dev_addr,
-						       bond->dev->dev_addr)) {
+			if (ether_addr_equal_64bits(tmp_slave->dev->dev_addr,
+						    bond->dev->dev_addr)) {
 				swap_slave = tmp_slave;
 				break;
 			}
@@ -1681,8 +1683,8 @@ int bond_alb_set_mac_address(struct net_device *bond_dev, void *addr)
 	swap_slave = NULL;
 
 	bond_for_each_slave(bond, slave, i) {
-		if (!compare_ether_addr_64bits(slave->dev->dev_addr,
-					       bond_dev->dev_addr)) {
+		if (ether_addr_equal_64bits(slave->dev->dev_addr,
+					    bond_dev->dev_addr)) {
 			swap_slave = slave;
 			break;
 		}
diff --git a/drivers/net/bonding/bond_main.c b/drivers/net/bonding/bond_main.c
index 62d2409bb29..2ee8cf9e8a3 100644
--- a/drivers/net/bonding/bond_main.c
+++ b/drivers/net/bonding/bond_main.c
@@ -549,9 +549,9 @@ down:
  * Get link speed and duplex from the slave's base driver
  * using ethtool. If for some reason the call fails or the
  * values are invalid, set speed and duplex to -1,
- * and return error.
+ * and return.
  */
-static int bond_update_speed_duplex(struct slave *slave)
+static void bond_update_speed_duplex(struct slave *slave)
 {
 	struct net_device *slave_dev = slave->dev;
 	struct ethtool_cmd ecmd;
@@ -563,24 +563,24 @@ static int bond_update_speed_duplex(struct slave *slave)
 
 	res = __ethtool_get_settings(slave_dev, &ecmd);
 	if (res < 0)
-		return -1;
+		return;
 
 	slave_speed = ethtool_cmd_speed(&ecmd);
 	if (slave_speed == 0 || slave_speed == ((__u32) -1))
-		return -1;
+		return;
 
 	switch (ecmd.duplex) {
 	case DUPLEX_FULL:
 	case DUPLEX_HALF:
 		break;
 	default:
-		return -1;
+		return;
 	}
 
 	slave->speed = slave_speed;
 	slave->duplex = ecmd.duplex;
 
-	return 0;
+	return;
 }
 
 /*
@@ -1444,8 +1444,9 @@ static rx_handler_result_t bond_handle_frame(struct sk_buff **pskb)
 	struct sk_buff *skb = *pskb;
 	struct slave *slave;
 	struct bonding *bond;
-	void (*recv_probe)(struct sk_buff *, struct bonding *,
+	int (*recv_probe)(struct sk_buff *, struct bonding *,
 				struct slave *);
+	int ret = RX_HANDLER_ANOTHER;
 
 	skb = skb_share_check(skb, GFP_ATOMIC);
 	if (unlikely(!skb))
@@ -1464,8 +1465,12 @@ static rx_handler_result_t bond_handle_frame(struct sk_buff **pskb)
 		struct sk_buff *nskb = skb_clone(skb, GFP_ATOMIC);
 
 		if (likely(nskb)) {
-			recv_probe(nskb, bond, slave);
+			ret = recv_probe(nskb, bond, slave);
 			dev_kfree_skb(nskb);
+			if (ret == RX_HANDLER_CONSUMED) {
+				consume_skb(skb);
+				return ret;
+			}
 		}
 	}
 
@@ -1487,7 +1492,7 @@ static rx_handler_result_t bond_handle_frame(struct sk_buff **pskb)
 		memcpy(eth_hdr(skb)->h_dest, bond->dev->dev_addr, ETH_ALEN);
 	}
 
-	return RX_HANDLER_ANOTHER;
+	return ret;
 }
 
 /* enslave device <slave> to bond device <master> */
@@ -1726,7 +1731,8 @@ int bond_enslave(struct net_device *bond_dev, struct net_device *slave_dev)
 
 	read_lock(&bond->lock);
 
-	new_slave->last_arp_rx = jiffies;
+	new_slave->last_arp_rx = jiffies -
+		(msecs_to_jiffies(bond->params.arp_interval) + 1);
 
 	if (bond->params.miimon && !bond->params.use_carrier) {
 		link_reporting = bond_check_dev_link(bond, slave_dev, 1);
@@ -1751,22 +1757,30 @@ int bond_enslave(struct net_device *bond_dev, struct net_device *slave_dev)
 	}
 
 	/* check for initial state */
-	if (!bond->params.miimon ||
-	    (bond_check_dev_link(bond, slave_dev, 0) == BMSR_LSTATUS)) {
-		if (bond->params.updelay) {
-			pr_debug("Initial state of slave_dev is BOND_LINK_BACK\n");
-			new_slave->link  = BOND_LINK_BACK;
-			new_slave->delay = bond->params.updelay;
+	if (bond->params.miimon) {
+		if (bond_check_dev_link(bond, slave_dev, 0) == BMSR_LSTATUS) {
+			if (bond->params.updelay) {
+				new_slave->link = BOND_LINK_BACK;
+				new_slave->delay = bond->params.updelay;
+			} else {
+				new_slave->link = BOND_LINK_UP;
+			}
 		} else {
-			pr_debug("Initial state of slave_dev is BOND_LINK_UP\n");
-			new_slave->link  = BOND_LINK_UP;
+			new_slave->link = BOND_LINK_DOWN;
 		}
-		new_slave->jiffies = jiffies;
+	} else if (bond->params.arp_interval) {
+		new_slave->link = (netif_carrier_ok(slave_dev) ?
+			BOND_LINK_UP : BOND_LINK_DOWN);
 	} else {
-		pr_debug("Initial state of slave_dev is BOND_LINK_DOWN\n");
-		new_slave->link  = BOND_LINK_DOWN;
+		new_slave->link = BOND_LINK_UP;
 	}
 
+	if (new_slave->link != BOND_LINK_DOWN)
+		new_slave->jiffies = jiffies;
+	pr_debug("Initial state of slave_dev is BOND_LINK_%s\n",
+		new_slave->link == BOND_LINK_DOWN ? "DOWN" :
+			(new_slave->link == BOND_LINK_UP ? "UP" : "BACK"));
+
 	bond_update_speed_duplex(new_slave);
 
 	if (USES_PRIMARY(bond->params.mode) && bond->params.primary[0]) {
@@ -1952,7 +1966,7 @@ int bond_release(struct net_device *bond_dev, struct net_device *slave_dev)
 	write_lock_bh(&bond->lock);
 
 	if (!bond->params.fail_over_mac) {
-		if (!compare_ether_addr(bond_dev->dev_addr, slave->perm_hwaddr) &&
+		if (ether_addr_equal(bond_dev->dev_addr, slave->perm_hwaddr) &&
 		    bond->slave_cnt > 1)
 			pr_warning("%s: Warning: the permanent HWaddr of %s - %pM - is still in use by %s. Set the HWaddr of %s to a different address to avoid conflicts.\n",
 				   bond_dev->name, slave_dev->name,
@@ -2723,7 +2737,7 @@ static void bond_validate_arp(struct bonding *bond, struct slave *slave, __be32
 	}
 }
 
-static void bond_arp_rcv(struct sk_buff *skb, struct bonding *bond,
+static int bond_arp_rcv(struct sk_buff *skb, struct bonding *bond,
 			 struct slave *slave)
 {
 	struct arphdr *arp;
@@ -2731,7 +2745,7 @@ static void bond_arp_rcv(struct sk_buff *skb, struct bonding *bond,
 	__be32 sip, tip;
 
 	if (skb->protocol != __cpu_to_be16(ETH_P_ARP))
-		return;
+		return RX_HANDLER_ANOTHER;
 
 	read_lock(&bond->lock);
 
@@ -2776,6 +2790,7 @@ static void bond_arp_rcv(struct sk_buff *skb, struct bonding *bond,
 
 out_unlock:
 	read_unlock(&bond->lock);
+	return RX_HANDLER_ANOTHER;
 }
 
 /*
@@ -4820,12 +4835,9 @@ static int bond_validate(struct nlattr *tb[], struct nlattr *data[])
 	return 0;
 }
 
-static int bond_get_tx_queues(struct net *net, struct nlattr *tb[],
-			      unsigned int *num_queues,
-			      unsigned int *real_num_queues)
+static int bond_get_tx_queues(struct net *net, struct nlattr *tb[])
 {
-	*num_queues = tx_queues;
-	return 0;
+	return tx_queues;
 }
 
 static struct rtnl_link_ops bond_link_ops __read_mostly = {
diff --git a/drivers/net/bonding/bonding.h b/drivers/net/bonding/bonding.h
index 9f2bae6616d..4581aa5ccab 100644
--- a/drivers/net/bonding/bonding.h
+++ b/drivers/net/bonding/bonding.h
@@ -218,7 +218,7 @@ struct bonding {
 	struct   slave *primary_slave;
 	bool     force_primary;
 	s32      slave_cnt; /* never change this value outside the attach/detach wrappers */
-	void     (*recv_probe)(struct sk_buff *, struct bonding *,
+	int     (*recv_probe)(struct sk_buff *, struct bonding *,
 			       struct slave *);
 	rwlock_t lock;
 	rwlock_t curr_slave_lock;
diff --git a/drivers/net/caif/caif_hsi.c b/drivers/net/caif/caif_hsi.c
index 9c1c8cd5223..1520814c77c 100644
--- a/drivers/net/caif/caif_hsi.c
+++ b/drivers/net/caif/caif_hsi.c
@@ -6,6 +6,8 @@
  * 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/device.h>
@@ -19,6 +21,7 @@
 #include <linux/if_arp.h>
 #include <linux/timer.h>
 #include <linux/rtnetlink.h>
+#include <linux/pkt_sched.h>
 #include <net/caif/caif_layer.h>
 #include <net/caif/caif_hsi.h>
 
@@ -34,6 +37,10 @@ static int inactivity_timeout = 1000;
 module_param(inactivity_timeout, int, S_IRUGO | S_IWUSR);
 MODULE_PARM_DESC(inactivity_timeout, "Inactivity timeout on HSI, ms.");
 
+static int aggregation_timeout = 1;
+module_param(aggregation_timeout, int, S_IRUGO | S_IWUSR);
+MODULE_PARM_DESC(aggregation_timeout, "Aggregation timeout on HSI, ms.");
+
 /*
  * HSI padding options.
  * Warning: must be a base of 2 (& operation used) and can not be zero !
@@ -86,24 +93,84 @@ static void cfhsi_inactivity_tout(unsigned long arg)
 		queue_work(cfhsi->wq, &cfhsi->wake_down_work);
 }
 
+static void cfhsi_update_aggregation_stats(struct cfhsi *cfhsi,
+					   const struct sk_buff *skb,
+					   int direction)
+{
+	struct caif_payload_info *info;
+	int hpad, tpad, len;
+
+	info = (struct caif_payload_info *)&skb->cb;
+	hpad = 1 + PAD_POW2((info->hdr_len + 1), hsi_head_align);
+	tpad = PAD_POW2((skb->len + hpad), hsi_tail_align);
+	len = skb->len + hpad + tpad;
+
+	if (direction > 0)
+		cfhsi->aggregation_len += len;
+	else if (direction < 0)
+		cfhsi->aggregation_len -= len;
+}
+
+static bool cfhsi_can_send_aggregate(struct cfhsi *cfhsi)
+{
+	int i;
+
+	if (cfhsi->aggregation_timeout < 0)
+		return true;
+
+	for (i = 0; i < CFHSI_PRIO_BEBK; ++i) {
+		if (cfhsi->qhead[i].qlen)
+			return true;
+	}
+
+	/* TODO: Use aggregation_len instead */
+	if (cfhsi->qhead[CFHSI_PRIO_BEBK].qlen >= CFHSI_MAX_PKTS)
+		return true;
+
+	return false;
+}
+
+static struct sk_buff *cfhsi_dequeue(struct cfhsi *cfhsi)
+{
+	struct sk_buff *skb;
+	int i;
+
+	for (i = 0; i < CFHSI_PRIO_LAST; ++i) {
+		skb = skb_dequeue(&cfhsi->qhead[i]);
+		if (skb)
+			break;
+	}
+
+	return skb;
+}
+
+static int cfhsi_tx_queue_len(struct cfhsi *cfhsi)
+{
+	int i, len = 0;
+	for (i = 0; i < CFHSI_PRIO_LAST; ++i)
+		len += skb_queue_len(&cfhsi->qhead[i]);
+	return len;
+}
+
 static void cfhsi_abort_tx(struct cfhsi *cfhsi)
 {
 	struct sk_buff *skb;
 
 	for (;;) {
 		spin_lock_bh(&cfhsi->lock);
-		skb = skb_dequeue(&cfhsi->qhead);
+		skb = cfhsi_dequeue(cfhsi);
 		if (!skb)
 			break;
 
 		cfhsi->ndev->stats.tx_errors++;
 		cfhsi->ndev->stats.tx_dropped++;
+		cfhsi_update_aggregation_stats(cfhsi, skb, -1);
 		spin_unlock_bh(&cfhsi->lock);
 		kfree_skb(skb);
 	}
 	cfhsi->tx_state = CFHSI_TX_STATE_IDLE;
 	if (!test_bit(CFHSI_SHUTDOWN, &cfhsi->bits))
-		mod_timer(&cfhsi->timer,
+		mod_timer(&cfhsi->inactivity_timer,
 			jiffies + cfhsi->inactivity_timeout);
 	spin_unlock_bh(&cfhsi->lock);
 }
@@ -169,7 +236,7 @@ static int cfhsi_tx_frm(struct cfhsi_desc *desc, struct cfhsi *cfhsi)
 	struct sk_buff *skb;
 	u8 *pfrm = desc->emb_frm + CFHSI_MAX_EMB_FRM_SZ;
 
-	skb = skb_dequeue(&cfhsi->qhead);
+	skb = cfhsi_dequeue(cfhsi);
 	if (!skb)
 		return 0;
 
@@ -196,11 +263,16 @@ static int cfhsi_tx_frm(struct cfhsi_desc *desc, struct cfhsi *cfhsi)
 			pemb += hpad;
 
 			/* Update network statistics. */
+			spin_lock_bh(&cfhsi->lock);
 			cfhsi->ndev->stats.tx_packets++;
 			cfhsi->ndev->stats.tx_bytes += skb->len;
+			cfhsi_update_aggregation_stats(cfhsi, skb, -1);
+			spin_unlock_bh(&cfhsi->lock);
 
 			/* Copy in embedded CAIF frame. */
 			skb_copy_bits(skb, 0, pemb, skb->len);
+
+			/* Consume the SKB */
 			consume_skb(skb);
 			skb = NULL;
 		}
@@ -214,7 +286,7 @@ static int cfhsi_tx_frm(struct cfhsi_desc *desc, struct cfhsi *cfhsi)
 		int tpad = 0;
 
 		if (!skb)
-			skb = skb_dequeue(&cfhsi->qhead);
+			skb = cfhsi_dequeue(cfhsi);
 
 		if (!skb)
 			break;
@@ -233,8 +305,11 @@ static int cfhsi_tx_frm(struct cfhsi_desc *desc, struct cfhsi *cfhsi)
 		pfrm += hpad;
 
 		/* Update network statistics. */
+		spin_lock_bh(&cfhsi->lock);
 		cfhsi->ndev->stats.tx_packets++;
 		cfhsi->ndev->stats.tx_bytes += skb->len;
+		cfhsi_update_aggregation_stats(cfhsi, skb, -1);
+		spin_unlock_bh(&cfhsi->lock);
 
 		/* Copy in CAIF frame. */
 		skb_copy_bits(skb, 0, pfrm, skb->len);
@@ -244,6 +319,8 @@ static int cfhsi_tx_frm(struct cfhsi_desc *desc, struct cfhsi *cfhsi)
 
 		/* Update frame pointer. */
 		pfrm += skb->len + tpad;
+
+		/* Consume the SKB */
 		consume_skb(skb);
 		skb = NULL;
 
@@ -258,8 +335,7 @@ static int cfhsi_tx_frm(struct cfhsi_desc *desc, struct cfhsi *cfhsi)
 	}
 
 	/* Check if we can piggy-back another descriptor. */
-	skb = skb_peek(&cfhsi->qhead);
-	if (skb)
+	if (cfhsi_can_send_aggregate(cfhsi))
 		desc->header |= CFHSI_PIGGY_DESC;
 	else
 		desc->header &= ~CFHSI_PIGGY_DESC;
@@ -267,61 +343,71 @@ static int cfhsi_tx_frm(struct cfhsi_desc *desc, struct cfhsi *cfhsi)
 	return CFHSI_DESC_SZ + pld_len;
 }
 
-static void cfhsi_tx_done(struct cfhsi *cfhsi)
+static void cfhsi_start_tx(struct cfhsi *cfhsi)
 {
-	struct cfhsi_desc *desc = NULL;
-	int len = 0;
-	int res;
+	struct cfhsi_desc *desc = (struct cfhsi_desc *)cfhsi->tx_buf;
+	int len, res;
 
 	dev_dbg(&cfhsi->ndev->dev, "%s.\n", __func__);
 
 	if (test_bit(CFHSI_SHUTDOWN, &cfhsi->bits))
 		return;
 
-	desc = (struct cfhsi_desc *)cfhsi->tx_buf;
-
 	do {
-		/*
-		 * Send flow on if flow off has been previously signalled
-		 * and number of packets is below low water mark.
-		 */
-		spin_lock_bh(&cfhsi->lock);
-		if (cfhsi->flow_off_sent &&
-				cfhsi->qhead.qlen <= cfhsi->q_low_mark &&
-				cfhsi->cfdev.flowctrl) {
-
-			cfhsi->flow_off_sent = 0;
-			cfhsi->cfdev.flowctrl(cfhsi->ndev, ON);
-		}
-		spin_unlock_bh(&cfhsi->lock);
-
 		/* Create HSI frame. */
-		do {
-			len = cfhsi_tx_frm(desc, cfhsi);
-			if (!len) {
-				spin_lock_bh(&cfhsi->lock);
-				if (unlikely(skb_peek(&cfhsi->qhead))) {
-					spin_unlock_bh(&cfhsi->lock);
-					continue;
-				}
-				cfhsi->tx_state = CFHSI_TX_STATE_IDLE;
-				/* Start inactivity timer. */
-				mod_timer(&cfhsi->timer,
-					jiffies + cfhsi->inactivity_timeout);
+		len = cfhsi_tx_frm(desc, cfhsi);
+		if (!len) {
+			spin_lock_bh(&cfhsi->lock);
+			if (unlikely(cfhsi_tx_queue_len(cfhsi))) {
 				spin_unlock_bh(&cfhsi->lock);
-				goto done;
+				res = -EAGAIN;
+				continue;
 			}
-		} while (!len);
+			cfhsi->tx_state = CFHSI_TX_STATE_IDLE;
+			/* Start inactivity timer. */
+			mod_timer(&cfhsi->inactivity_timer,
+				jiffies + cfhsi->inactivity_timeout);
+			spin_unlock_bh(&cfhsi->lock);
+			break;
+		}
 
 		/* Set up new transfer. */
 		res = cfhsi->dev->cfhsi_tx(cfhsi->tx_buf, len, cfhsi->dev);
-		if (WARN_ON(res < 0)) {
+		if (WARN_ON(res < 0))
 			dev_err(&cfhsi->ndev->dev, "%s: TX error %d.\n",
 				__func__, res);
-		}
 	} while (res < 0);
+}
+
+static void cfhsi_tx_done(struct cfhsi *cfhsi)
+{
+	dev_dbg(&cfhsi->ndev->dev, "%s.\n", __func__);
+
+	if (test_bit(CFHSI_SHUTDOWN, &cfhsi->bits))
+		return;
+
+	/*
+	 * Send flow on if flow off has been previously signalled
+	 * and number of packets is below low water mark.
+	 */
+	spin_lock_bh(&cfhsi->lock);
+	if (cfhsi->flow_off_sent &&
+			cfhsi_tx_queue_len(cfhsi) <= cfhsi->q_low_mark &&
+			cfhsi->cfdev.flowctrl) {
+
+		cfhsi->flow_off_sent = 0;
+		cfhsi->cfdev.flowctrl(cfhsi->ndev, ON);
+	}
+
+	if (cfhsi_can_send_aggregate(cfhsi)) {
+		spin_unlock_bh(&cfhsi->lock);
+		cfhsi_start_tx(cfhsi);
+	} else {
+		mod_timer(&cfhsi->aggregation_timer,
+			jiffies + cfhsi->aggregation_timeout);
+		spin_unlock_bh(&cfhsi->lock);
+	}
 
-done:
 	return;
 }
 
@@ -560,7 +646,7 @@ static void cfhsi_rx_done(struct cfhsi *cfhsi)
 
 	/* Update inactivity timer if pending. */
 	spin_lock_bh(&cfhsi->lock);
-	mod_timer_pending(&cfhsi->timer,
+	mod_timer_pending(&cfhsi->inactivity_timer,
 			jiffies + cfhsi->inactivity_timeout);
 	spin_unlock_bh(&cfhsi->lock);
 
@@ -793,12 +879,12 @@ wake_ack:
 
 	spin_lock_bh(&cfhsi->lock);
 
-	/* Resume transmit if queue is not empty. */
-	if (!skb_peek(&cfhsi->qhead)) {
+	/* Resume transmit if queues are not empty. */
+	if (!cfhsi_tx_queue_len(cfhsi)) {
 		dev_dbg(&cfhsi->ndev->dev, "%s: Peer wake, start timer.\n",
 			__func__);
 		/* Start inactivity timer. */
-		mod_timer(&cfhsi->timer,
+		mod_timer(&cfhsi->inactivity_timer,
 				jiffies + cfhsi->inactivity_timeout);
 		spin_unlock_bh(&cfhsi->lock);
 		return;
@@ -934,20 +1020,53 @@ static void cfhsi_wake_down_cb(struct cfhsi_drv *drv)
 	wake_up_interruptible(&cfhsi->wake_down_wait);
 }
 
+static void cfhsi_aggregation_tout(unsigned long arg)
+{
+	struct cfhsi *cfhsi = (struct cfhsi *)arg;
+
+	dev_dbg(&cfhsi->ndev->dev, "%s.\n",
+		__func__);
+
+	cfhsi_start_tx(cfhsi);
+}
+
 static int cfhsi_xmit(struct sk_buff *skb, struct net_device *dev)
 {
 	struct cfhsi *cfhsi = NULL;
 	int start_xfer = 0;
 	int timer_active;
+	int prio;
 
 	if (!dev)
 		return -EINVAL;
 
 	cfhsi = netdev_priv(dev);
 
+	switch (skb->priority) {
+	case TC_PRIO_BESTEFFORT:
+	case TC_PRIO_FILLER:
+	case TC_PRIO_BULK:
+		prio = CFHSI_PRIO_BEBK;
+		break;
+	case TC_PRIO_INTERACTIVE_BULK:
+		prio = CFHSI_PRIO_VI;
+		break;
+	case TC_PRIO_INTERACTIVE:
+		prio = CFHSI_PRIO_VO;
+		break;
+	case TC_PRIO_CONTROL:
+	default:
+		prio = CFHSI_PRIO_CTL;
+		break;
+	}
+
 	spin_lock_bh(&cfhsi->lock);
 
-	skb_queue_tail(&cfhsi->qhead, skb);
+	/* Update aggregation statistics  */
+	cfhsi_update_aggregation_stats(cfhsi, skb, 1);
+
+	/* Queue the SKB */
+	skb_queue_tail(&cfhsi->qhead[prio], skb);
 
 	/* Sanity check; xmit should not be called after unregister_netdev */
 	if (WARN_ON(test_bit(CFHSI_SHUTDOWN, &cfhsi->bits))) {
@@ -958,7 +1077,7 @@ static int cfhsi_xmit(struct sk_buff *skb, struct net_device *dev)
 
 	/* Send flow off if number of packets is above high water mark. */
 	if (!cfhsi->flow_off_sent &&
-		cfhsi->qhead.qlen > cfhsi->q_high_mark &&
+		cfhsi_tx_queue_len(cfhsi) > cfhsi->q_high_mark &&
 		cfhsi->cfdev.flowctrl) {
 		cfhsi->flow_off_sent = 1;
 		cfhsi->cfdev.flowctrl(cfhsi->ndev, OFF);
@@ -970,12 +1089,18 @@ static int cfhsi_xmit(struct sk_buff *skb, struct net_device *dev)
 	}
 
 	if (!start_xfer) {
+		/* Send aggregate if it is possible */
+		bool aggregate_ready =
+			cfhsi_can_send_aggregate(cfhsi) &&
+			del_timer(&cfhsi->aggregation_timer) > 0;
 		spin_unlock_bh(&cfhsi->lock);
+		if (aggregate_ready)
+			cfhsi_start_tx(cfhsi);
 		return 0;
 	}
 
 	/* Delete inactivity timer if started. */
-	timer_active = del_timer_sync(&cfhsi->timer);
+	timer_active = del_timer_sync(&cfhsi->inactivity_timer);
 
 	spin_unlock_bh(&cfhsi->lock);
 
@@ -1004,28 +1129,11 @@ static int cfhsi_xmit(struct sk_buff *skb, struct net_device *dev)
 	return 0;
 }
 
-static int cfhsi_open(struct net_device *dev)
-{
-	netif_wake_queue(dev);
-
-	return 0;
-}
-
-static int cfhsi_close(struct net_device *dev)
-{
-	netif_stop_queue(dev);
-
-	return 0;
-}
-
-static const struct net_device_ops cfhsi_ops = {
-	.ndo_open = cfhsi_open,
-	.ndo_stop = cfhsi_close,
-	.ndo_start_xmit = cfhsi_xmit
-};
+static const struct net_device_ops cfhsi_ops;
 
 static void cfhsi_setup(struct net_device *dev)
 {
+	int i;
 	struct cfhsi *cfhsi = netdev_priv(dev);
 	dev->features = 0;
 	dev->netdev_ops = &cfhsi_ops;
@@ -1034,7 +1142,8 @@ static void cfhsi_setup(struct net_device *dev)
 	dev->mtu = CFHSI_MAX_CAIF_FRAME_SZ;
 	dev->tx_queue_len = 0;
 	dev->destructor = free_netdev;
-	skb_queue_head_init(&cfhsi->qhead);
+	for (i = 0; i < CFHSI_PRIO_LAST; ++i)
+		skb_queue_head_init(&cfhsi->qhead[i]);
 	cfhsi->cfdev.link_select = CAIF_LINK_HIGH_BANDW;
 	cfhsi->cfdev.use_frag = false;
 	cfhsi->cfdev.use_stx = false;
@@ -1046,7 +1155,7 @@ int cfhsi_probe(struct platform_device *pdev)
 {
 	struct cfhsi *cfhsi = NULL;
 	struct net_device *ndev;
-	struct cfhsi_dev *dev;
+
 	int res;
 
 	ndev = alloc_netdev(sizeof(struct cfhsi), "cfhsi%d", cfhsi_setup);
@@ -1057,6 +1166,34 @@ int cfhsi_probe(struct platform_device *pdev)
 	cfhsi->ndev = ndev;
 	cfhsi->pdev = pdev;
 
+	/* Assign the HSI device. */
+	cfhsi->dev = pdev->dev.platform_data;
+
+	/* Assign the driver to this HSI device. */
+	cfhsi->dev->drv = &cfhsi->drv;
+
+	/* Register network device. */
+	res = register_netdev(ndev);
+	if (res) {
+		dev_err(&ndev->dev, "%s: Registration error: %d.\n",
+			__func__, res);
+		free_netdev(ndev);
+	}
+	/* Add CAIF HSI device to list. */
+	spin_lock(&cfhsi_list_lock);
+	list_add_tail(&cfhsi->list, &cfhsi_list);
+	spin_unlock(&cfhsi_list_lock);
+
+	return res;
+}
+
+static int cfhsi_open(struct net_device *ndev)
+{
+	struct cfhsi *cfhsi = netdev_priv(ndev);
+	int res;
+
+	clear_bit(CFHSI_SHUTDOWN, &cfhsi->bits);
+
 	/* Initialize state vaiables. */
 	cfhsi->tx_state = CFHSI_TX_STATE_IDLE;
 	cfhsi->rx_state.state = CFHSI_RX_STATE_DESC;
@@ -1066,12 +1203,6 @@ int cfhsi_probe(struct platform_device *pdev)
 	cfhsi->q_low_mark = LOW_WATER_MARK;
 	cfhsi->q_high_mark = HIGH_WATER_MARK;
 
-	/* Assign the HSI device. */
-	dev = (struct cfhsi_dev *)pdev->dev.platform_data;
-	cfhsi->dev = dev;
-
-	/* Assign the driver to this HSI device. */
-	dev->drv = &cfhsi->drv;
 
 	/*
 	 * Allocate a TX buffer with the size of a HSI packet descriptors
@@ -1111,6 +1242,9 @@ int cfhsi_probe(struct platform_device *pdev)
 		cfhsi->inactivity_timeout = NEXT_TIMER_MAX_DELTA;
 	}
 
+	/* Initialize aggregation timeout */
+	cfhsi->aggregation_timeout = aggregation_timeout;
+
 	/* Initialize recieve vaiables. */
 	cfhsi->rx_ptr = cfhsi->rx_buf;
 	cfhsi->rx_len = CFHSI_DESC_SZ;
@@ -1136,9 +1270,9 @@ int cfhsi_probe(struct platform_device *pdev)
 	clear_bit(CFHSI_AWAKE, &cfhsi->bits);
 
 	/* Create work thread. */
-	cfhsi->wq = create_singlethread_workqueue(pdev->name);
+	cfhsi->wq = create_singlethread_workqueue(cfhsi->pdev->name);
 	if (!cfhsi->wq) {
-		dev_err(&ndev->dev, "%s: Failed to create work queue.\n",
+		dev_err(&cfhsi->ndev->dev, "%s: Failed to create work queue.\n",
 			__func__);
 		res = -ENODEV;
 		goto err_create_wq;
@@ -1150,18 +1284,17 @@ int cfhsi_probe(struct platform_device *pdev)
 	init_waitqueue_head(&cfhsi->flush_fifo_wait);
 
 	/* Setup the inactivity timer. */
-	init_timer(&cfhsi->timer);
-	cfhsi->timer.data = (unsigned long)cfhsi;
-	cfhsi->timer.function = cfhsi_inactivity_tout;
+	init_timer(&cfhsi->inactivity_timer);
+	cfhsi->inactivity_timer.data = (unsigned long)cfhsi;
+	cfhsi->inactivity_timer.function = cfhsi_inactivity_tout;
 	/* Setup the slowpath RX timer. */
 	init_timer(&cfhsi->rx_slowpath_timer);
 	cfhsi->rx_slowpath_timer.data = (unsigned long)cfhsi;
 	cfhsi->rx_slowpath_timer.function = cfhsi_rx_slowpath;
-
-	/* Add CAIF HSI device to list. */
-	spin_lock(&cfhsi_list_lock);
-	list_add_tail(&cfhsi->list, &cfhsi_list);
-	spin_unlock(&cfhsi_list_lock);
+	/* Setup the aggregation timer. */
+	init_timer(&cfhsi->aggregation_timer);
+	cfhsi->aggregation_timer.data = (unsigned long)cfhsi;
+	cfhsi->aggregation_timer.function = cfhsi_aggregation_tout;
 
 	/* Activate HSI interface. */
 	res = cfhsi->dev->cfhsi_up(cfhsi->dev);
@@ -1175,21 +1308,10 @@ int cfhsi_probe(struct platform_device *pdev)
 	/* Flush FIFO */
 	res = cfhsi_flush_fifo(cfhsi);
 	if (res) {
-		dev_err(&ndev->dev, "%s: Can't flush FIFO: %d.\n",
+		dev_err(&cfhsi->ndev->dev, "%s: Can't flush FIFO: %d.\n",
 			__func__, res);
 		goto err_net_reg;
 	}
-
-	/* Register network device. */
-	res = register_netdev(ndev);
-	if (res) {
-		dev_err(&ndev->dev, "%s: Registration error: %d.\n",
-			__func__, res);
-		goto err_net_reg;
-	}
-
-	netif_stop_queue(ndev);
-
 	return res;
 
  err_net_reg:
@@ -1203,18 +1325,14 @@ int cfhsi_probe(struct platform_device *pdev)
  err_alloc_rx:
 	kfree(cfhsi->tx_buf);
  err_alloc_tx:
-	free_netdev(ndev);
-
 	return res;
 }
 
-static void cfhsi_shutdown(struct cfhsi *cfhsi)
+static int cfhsi_close(struct net_device *ndev)
 {
+	struct cfhsi *cfhsi = netdev_priv(ndev);
 	u8 *tx_buf, *rx_buf, *flip_buf;
 
-	/* Stop TXing */
-	netif_tx_stop_all_queues(cfhsi->ndev);
-
 	/* going to shutdown driver */
 	set_bit(CFHSI_SHUTDOWN, &cfhsi->bits);
 
@@ -1222,8 +1340,9 @@ static void cfhsi_shutdown(struct cfhsi *cfhsi)
 	flush_workqueue(cfhsi->wq);
 
 	/* Delete timers if pending */
-	del_timer_sync(&cfhsi->timer);
+	del_timer_sync(&cfhsi->inactivity_timer);
 	del_timer_sync(&cfhsi->rx_slowpath_timer);
+	del_timer_sync(&cfhsi->aggregation_timer);
 
 	/* Cancel pending RX request (if any) */
 	cfhsi->dev->cfhsi_rx_cancel(cfhsi->dev);
@@ -1241,15 +1360,19 @@ static void cfhsi_shutdown(struct cfhsi *cfhsi)
 	/* Deactivate interface */
 	cfhsi->dev->cfhsi_down(cfhsi->dev);
 
-	/* Finally unregister the network device. */
-	unregister_netdev(cfhsi->ndev);
-
 	/* Free buffers. */
 	kfree(tx_buf);
 	kfree(rx_buf);
 	kfree(flip_buf);
+	return 0;
 }
 
+static const struct net_device_ops cfhsi_ops = {
+	.ndo_open = cfhsi_open,
+	.ndo_stop = cfhsi_close,
+	.ndo_start_xmit = cfhsi_xmit
+};
+
 int cfhsi_remove(struct platform_device *pdev)
 {
 	struct list_head *list_node;
@@ -1266,10 +1389,6 @@ int cfhsi_remove(struct platform_device *pdev)
 			/* Remove from list. */
 			list_del(list_node);
 			spin_unlock(&cfhsi_list_lock);
-
-			/* Shutdown driver. */
-			cfhsi_shutdown(cfhsi);
-
 			return 0;
 		}
 	}
@@ -1300,8 +1419,7 @@ static void __exit cfhsi_exit_module(void)
 		list_del(list_node);
 		spin_unlock(&cfhsi_list_lock);
 
-		/* Shutdown driver. */
-		cfhsi_shutdown(cfhsi);
+		unregister_netdevice(cfhsi->ndev);
 
 		spin_lock(&cfhsi_list_lock);
 	}
@@ -1326,8 +1444,6 @@ static int __init cfhsi_init_module(void)
 		goto err_dev_register;
 	}
 
-	return result;
-
  err_dev_register:
 	return result;
 }
diff --git a/drivers/net/caif/caif_shmcore.c b/drivers/net/caif/caif_shmcore.c
index 5b2041319a3..bc497d71885 100644
--- a/drivers/net/caif/caif_shmcore.c
+++ b/drivers/net/caif/caif_shmcore.c
@@ -13,6 +13,7 @@
 #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>
@@ -647,6 +648,9 @@ int caif_shmcore_probe(struct shmdev_layer *pshm_dev)
 		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);
 
diff --git a/drivers/net/can/dev.c b/drivers/net/can/dev.c
index c5fe3a3db8c..f03d7a481a8 100644
--- a/drivers/net/can/dev.c
+++ b/drivers/net/can/dev.c
@@ -687,18 +687,19 @@ static int can_fill_info(struct sk_buff *skb, const struct net_device *dev)
 
 	if (priv->do_get_state)
 		priv->do_get_state(dev, &state);
-	NLA_PUT_U32(skb, IFLA_CAN_STATE, state);
-	NLA_PUT(skb, IFLA_CAN_CTRLMODE, sizeof(cm), &cm);
-	NLA_PUT_U32(skb, IFLA_CAN_RESTART_MS, priv->restart_ms);
-	NLA_PUT(skb, IFLA_CAN_BITTIMING,
-		sizeof(priv->bittiming), &priv->bittiming);
-	NLA_PUT(skb, IFLA_CAN_CLOCK, sizeof(cm), &priv->clock);
-	if (priv->do_get_berr_counter && !priv->do_get_berr_counter(dev, &bec))
-		NLA_PUT(skb, IFLA_CAN_BERR_COUNTER, sizeof(bec), &bec);
-	if (priv->bittiming_const)
-		NLA_PUT(skb, IFLA_CAN_BITTIMING_CONST,
-			sizeof(*priv->bittiming_const), priv->bittiming_const);
-
+	if (nla_put_u32(skb, IFLA_CAN_STATE, state) ||
+	    nla_put(skb, IFLA_CAN_CTRLMODE, sizeof(cm), &cm) ||
+	    nla_put_u32(skb, IFLA_CAN_RESTART_MS, priv->restart_ms) ||
+	    nla_put(skb, IFLA_CAN_BITTIMING,
+		    sizeof(priv->bittiming), &priv->bittiming) ||
+	    nla_put(skb, IFLA_CAN_CLOCK, sizeof(cm), &priv->clock) ||
+	    (priv->do_get_berr_counter &&
+	     !priv->do_get_berr_counter(dev, &bec) &&
+	     nla_put(skb, IFLA_CAN_BERR_COUNTER, sizeof(bec), &bec)) ||
+	    (priv->bittiming_const &&
+	     nla_put(skb, IFLA_CAN_BITTIMING_CONST,
+		     sizeof(*priv->bittiming_const), priv->bittiming_const)))
+		goto nla_put_failure;
 	return 0;
 
 nla_put_failure:
@@ -714,9 +715,9 @@ static int can_fill_xstats(struct sk_buff *skb, const struct net_device *dev)
 {
 	struct can_priv *priv = netdev_priv(dev);
 
-	NLA_PUT(skb, IFLA_INFO_XSTATS,
-		sizeof(priv->can_stats), &priv->can_stats);
-
+	if (nla_put(skb, IFLA_INFO_XSTATS,
+		    sizeof(priv->can_stats), &priv->can_stats))
+		goto nla_put_failure;
 	return 0;
 
 nla_put_failure:
diff --git a/drivers/net/can/flexcan.c b/drivers/net/can/flexcan.c
index 1efb08386c6..38c0690df5c 100644
--- a/drivers/net/can/flexcan.c
+++ b/drivers/net/can/flexcan.c
@@ -35,6 +35,7 @@
 #include <linux/module.h>
 #include <linux/of.h>
 #include <linux/platform_device.h>
+#include <linux/pinctrl/consumer.h>
 
 #define DRV_NAME			"flexcan"
 
@@ -927,11 +928,16 @@ static int __devinit flexcan_probe(struct platform_device *pdev)
 	struct flexcan_priv *priv;
 	struct resource *mem;
 	struct clk *clk = NULL;
+	struct pinctrl *pinctrl;
 	void __iomem *base;
 	resource_size_t mem_size;
 	int err, irq;
 	u32 clock_freq = 0;
 
+	pinctrl = devm_pinctrl_get_select_default(&pdev->dev);
+	if (IS_ERR(pinctrl))
+		return PTR_ERR(pinctrl);
+
 	if (pdev->dev.of_node) {
 		const u32 *clock_freq_p;
 
diff --git a/drivers/net/can/pch_can.c b/drivers/net/can/pch_can.c
index 2bb215e00eb..1226297e767 100644
--- a/drivers/net/can/pch_can.c
+++ b/drivers/net/can/pch_can.c
@@ -1274,17 +1274,7 @@ static struct pci_driver pch_can_pci_driver = {
 	.resume = pch_can_resume,
 };
 
-static int __init pch_can_pci_init(void)
-{
-	return pci_register_driver(&pch_can_pci_driver);
-}
-module_init(pch_can_pci_init);
-
-static void __exit pch_can_pci_exit(void)
-{
-	pci_unregister_driver(&pch_can_pci_driver);
-}
-module_exit(pch_can_pci_exit);
+module_pci_driver(pch_can_pci_driver);
 
 MODULE_DESCRIPTION("Intel EG20T PCH CAN(Controller Area Network) Driver");
 MODULE_LICENSE("GPL v2");
diff --git a/drivers/net/can/sja1000/Kconfig b/drivers/net/can/sja1000/Kconfig
index b60d6c5f29a..03df9a8f2bb 100644
--- a/drivers/net/can/sja1000/Kconfig
+++ b/drivers/net/can/sja1000/Kconfig
@@ -75,7 +75,7 @@ config CAN_KVASER_PCI
 	tristate "Kvaser PCIcanx and Kvaser PCIcan PCI Cards"
 	depends on PCI
 	---help---
-	  This driver is for the the PCIcanx and PCIcan cards (1, 2 or
+	  This driver is for the PCIcanx and PCIcan cards (1, 2 or
 	  4 channel) from Kvaser (http://www.kvaser.com).
 
 config CAN_PLX_PCI
diff --git a/drivers/net/can/sja1000/ems_pci.c b/drivers/net/can/sja1000/ems_pci.c
index 36f4f9780c3..5c6d412bafb 100644
--- a/drivers/net/can/sja1000/ems_pci.c
+++ b/drivers/net/can/sja1000/ems_pci.c
@@ -371,16 +371,4 @@ static struct pci_driver ems_pci_driver = {
 	.remove = ems_pci_del_card,
 };
 
-static int __init ems_pci_init(void)
-{
-	return pci_register_driver(&ems_pci_driver);
-}
-
-static void __exit ems_pci_exit(void)
-{
-	pci_unregister_driver(&ems_pci_driver);
-}
-
-module_init(ems_pci_init);
-module_exit(ems_pci_exit);
-
+module_pci_driver(ems_pci_driver);
diff --git a/drivers/net/can/sja1000/kvaser_pci.c b/drivers/net/can/sja1000/kvaser_pci.c
index ed004cebd31..23ed6ea4c7c 100644
--- a/drivers/net/can/sja1000/kvaser_pci.c
+++ b/drivers/net/can/sja1000/kvaser_pci.c
@@ -397,15 +397,4 @@ static struct pci_driver kvaser_pci_driver = {
 	.remove = __devexit_p(kvaser_pci_remove_one),
 };
 
-static int __init kvaser_pci_init(void)
-{
-	return pci_register_driver(&kvaser_pci_driver);
-}
-
-static void __exit kvaser_pci_exit(void)
-{
-	pci_unregister_driver(&kvaser_pci_driver);
-}
-
-module_init(kvaser_pci_init);
-module_exit(kvaser_pci_exit);
+module_pci_driver(kvaser_pci_driver);
diff --git a/drivers/net/can/sja1000/peak_pci.c b/drivers/net/can/sja1000/peak_pci.c
index 5f92b865f64..f0a12962f7b 100644
--- a/drivers/net/can/sja1000/peak_pci.c
+++ b/drivers/net/can/sja1000/peak_pci.c
@@ -749,14 +749,4 @@ static struct pci_driver peak_pci_driver = {
 	.remove = __devexit_p(peak_pci_remove),
 };
 
-static int __init peak_pci_init(void)
-{
-	return pci_register_driver(&peak_pci_driver);
-}
-module_init(peak_pci_init);
-
-static void __exit peak_pci_exit(void)
-{
-	pci_unregister_driver(&peak_pci_driver);
-}
-module_exit(peak_pci_exit);
+module_pci_driver(peak_pci_driver);
diff --git a/drivers/net/can/sja1000/plx_pci.c b/drivers/net/can/sja1000/plx_pci.c
index a227586ddd5..8bc95982840 100644
--- a/drivers/net/can/sja1000/plx_pci.c
+++ b/drivers/net/can/sja1000/plx_pci.c
@@ -609,15 +609,4 @@ static struct pci_driver plx_pci_driver = {
 	.remove = plx_pci_del_card,
 };
 
-static int __init plx_pci_init(void)
-{
-	return pci_register_driver(&plx_pci_driver);
-}
-
-static void __exit plx_pci_exit(void)
-{
-	pci_unregister_driver(&plx_pci_driver);
-}
-
-module_init(plx_pci_init);
-module_exit(plx_pci_exit);
+module_pci_driver(plx_pci_driver);
diff --git a/drivers/net/cris/eth_v10.c b/drivers/net/cris/eth_v10.c
index ec03b401620..9c755db6b16 100644
--- a/drivers/net/cris/eth_v10.c
+++ b/drivers/net/cris/eth_v10.c
@@ -1131,7 +1131,6 @@ static irqreturn_t
 e100rxtx_interrupt(int irq, void *dev_id)
 {
 	struct net_device *dev = (struct net_device *)dev_id;
-	struct net_local *np = netdev_priv(dev);
 	unsigned long irqbits;
 
 	/*
diff --git a/drivers/net/ethernet/3com/3c509.c b/drivers/net/ethernet/3com/3c509.c
index 41719da2e17..1a8eef2c3d5 100644
--- a/drivers/net/ethernet/3com/3c509.c
+++ b/drivers/net/ethernet/3com/3c509.c
@@ -69,7 +69,6 @@
 #define TX_TIMEOUT  (400*HZ/1000)
 
 #include <linux/module.h>
-#include <linux/mca.h>
 #include <linux/isa.h>
 #include <linux/pnp.h>
 #include <linux/string.h>
@@ -102,7 +101,7 @@ static int el3_debug = 2;
 #endif
 
 /* Used to do a global count of all the cards in the system.  Must be
- * a global variable so that the mca/eisa probe routines can increment
+ * a global variable so that the eisa probe routines can increment
  * it */
 static int el3_cards = 0;
 #define EL3_MAX_CARDS 8
@@ -163,7 +162,7 @@ enum RxFilter {
  */
 #define SKB_QUEUE_SIZE	64
 
-enum el3_cardtype { EL3_ISA, EL3_PNP, EL3_MCA, EL3_EISA };
+enum el3_cardtype { EL3_ISA, EL3_PNP, EL3_EISA };
 
 struct el3_private {
 	spinlock_t lock;
@@ -505,41 +504,6 @@ static struct eisa_driver el3_eisa_driver = {
 static int eisa_registered;
 #endif
 
-#ifdef CONFIG_MCA
-static int el3_mca_probe(struct device *dev);
-
-static short el3_mca_adapter_ids[] __initdata = {
-		0x627c,
-		0x627d,
-		0x62db,
-		0x62f6,
-		0x62f7,
-		0x0000
-};
-
-static char *el3_mca_adapter_names[] __initdata = {
-		"3Com 3c529 EtherLink III (10base2)",
-		"3Com 3c529 EtherLink III (10baseT)",
-		"3Com 3c529 EtherLink III (test mode)",
-		"3Com 3c529 EtherLink III (TP or coax)",
-		"3Com 3c529 EtherLink III (TP)",
-		NULL
-};
-
-static struct mca_driver el3_mca_driver = {
-		.id_table = el3_mca_adapter_ids,
-		.driver = {
-				.name = "3c529",
-				.bus = &mca_bus_type,
-				.probe = el3_mca_probe,
-				.remove = __devexit_p(el3_device_remove),
-				.suspend = el3_suspend,
-				.resume  = el3_resume,
-		},
-};
-static int mca_registered;
-#endif /* CONFIG_MCA */
-
 static const struct net_device_ops netdev_ops = {
 	.ndo_open 		= el3_open,
 	.ndo_stop	 	= el3_close,
@@ -600,76 +564,6 @@ static void el3_common_remove (struct net_device *dev)
 	free_netdev (dev);
 }
 
-#ifdef CONFIG_MCA
-static int __init el3_mca_probe(struct device *device)
-{
-	/* Based on Erik Nygren's (nygren@mit.edu) 3c529 patch,
-	 * heavily modified by Chris Beauregard
-	 * (cpbeaure@csclub.uwaterloo.ca) to support standard MCA
-	 * probing.
-	 *
-	 * redone for multi-card detection by ZP Gu (zpg@castle.net)
-	 * now works as a module */
-
-	short i;
-	int ioaddr, irq, if_port;
-	__be16 phys_addr[3];
-	struct net_device *dev = NULL;
-	u_char pos4, pos5;
-	struct mca_device *mdev = to_mca_device(device);
-	int slot = mdev->slot;
-	int err;
-
-	pos4 = mca_device_read_stored_pos(mdev, 4);
-	pos5 = mca_device_read_stored_pos(mdev, 5);
-
-	ioaddr = ((short)((pos4&0xfc)|0x02)) << 8;
-	irq = pos5 & 0x0f;
-
-
-	pr_info("3c529: found %s at slot %d\n",
-		el3_mca_adapter_names[mdev->index], slot + 1);
-
-	/* claim the slot */
-	strncpy(mdev->name, el3_mca_adapter_names[mdev->index],
-			sizeof(mdev->name));
-	mca_device_set_claim(mdev, 1);
-
-	if_port = pos4 & 0x03;
-
-	irq = mca_device_transform_irq(mdev, irq);
-	ioaddr = mca_device_transform_ioport(mdev, ioaddr);
-	if (el3_debug > 2) {
-		pr_debug("3c529: irq %d  ioaddr 0x%x  ifport %d\n", irq, ioaddr, if_port);
-	}
-	EL3WINDOW(0);
-	for (i = 0; i < 3; i++)
-		phys_addr[i] = htons(read_eeprom(ioaddr, i));
-
-	dev = alloc_etherdev(sizeof (struct el3_private));
-	if (dev == NULL) {
-		release_region(ioaddr, EL3_IO_EXTENT);
-		return -ENOMEM;
-	}
-
-	netdev_boot_setup_check(dev);
-
-	el3_dev_fill(dev, phys_addr, ioaddr, irq, if_port, EL3_MCA);
-	dev_set_drvdata(device, dev);
-	err = el3_common_init(dev);
-
-	if (err) {
-		dev_set_drvdata(device, NULL);
-		free_netdev(dev);
-		return -ENOMEM;
-	}
-
-	el3_devs[el3_cards++] = dev;
-	return 0;
-}
-
-#endif /* CONFIG_MCA */
-
 #ifdef CONFIG_EISA
 static int __init el3_eisa_probe (struct device *device)
 {
@@ -1547,11 +1441,6 @@ static int __init el3_init_module(void)
 	if (!ret)
 		eisa_registered = 1;
 #endif
-#ifdef CONFIG_MCA
-	ret = mca_register_driver(&el3_mca_driver);
-	if (!ret)
-		mca_registered = 1;
-#endif
 
 #ifdef CONFIG_PNP
 	if (pnp_registered)
@@ -1563,10 +1452,6 @@ static int __init el3_init_module(void)
 	if (eisa_registered)
 		ret = 0;
 #endif
-#ifdef CONFIG_MCA
-	if (mca_registered)
-		ret = 0;
-#endif
 	return ret;
 }
 
@@ -1584,10 +1469,6 @@ static void __exit el3_cleanup_module(void)
 	if (eisa_registered)
 		eisa_driver_unregister(&el3_eisa_driver);
 #endif
-#ifdef CONFIG_MCA
-	if (mca_registered)
-		mca_unregister_driver(&el3_mca_driver);
-#endif
 }
 
 module_init (el3_init_module);
diff --git a/drivers/net/ethernet/3com/typhoon.c b/drivers/net/ethernet/3com/typhoon.c
index 1234a14b2b7..b1536663514 100644
--- a/drivers/net/ethernet/3com/typhoon.c
+++ b/drivers/net/ethernet/3com/typhoon.c
@@ -2549,8 +2549,7 @@ typhoon_init(void)
 static void __exit
 typhoon_cleanup(void)
 {
-	if (typhoon_fw)
-		release_firmware(typhoon_fw);
+	release_firmware(typhoon_fw);
 	pci_unregister_driver(&typhoon_driver);
 }
 
diff --git a/drivers/net/ethernet/8390/Kconfig b/drivers/net/ethernet/8390/Kconfig
index e04ade44424..2e538676924 100644
--- a/drivers/net/ethernet/8390/Kconfig
+++ b/drivers/net/ethernet/8390/Kconfig
@@ -60,6 +60,7 @@ config PCMCIA_AXNET
 config AX88796
 	tristate "ASIX AX88796 NE2000 clone support"
 	depends on (ARM || MIPS || SUPERH)
+	select CRC32
 	select PHYLIB
 	select MDIO_BITBANG
 	---help---
@@ -181,18 +182,6 @@ config NE2000
 	  To compile this driver as a module, choose M here. The module
 	  will be called ne.
 
-config NE2_MCA
-	tristate "NE/2 (ne2000 MCA version) support"
-	depends on MCA_LEGACY
-	select CRC32
-	---help---
-	  If you have a network (Ethernet) card of this type, say Y and read
-	  the Ethernet-HOWTO, available from
-	  <http://www.tldp.org/docs.html#howto>.
-
-	  To compile this driver as a module, choose M here. The module
-	  will be called ne2.
-
 config NE2K_PCI
 	tristate "PCI NE2000 and clones support (see help)"
 	depends on PCI
@@ -266,18 +255,6 @@ config STNIC
 
 	  If unsure, say N.
 
-config ULTRAMCA
-	tristate "SMC Ultra MCA support"
-	depends on MCA
-	select CRC32
-	---help---
-	  If you have a network (Ethernet) card of this type and are running
-	  an MCA based system (PS/2), say Y and read the Ethernet-HOWTO,
-	  available from <http://www.tldp.org/docs.html#howto>.
-
-	  To compile this driver as a module, choose M here. The module
-	  will be called smc-mca.
-
 config ULTRA
 	tristate "SMC Ultra support"
 	depends on ISA
diff --git a/drivers/net/ethernet/8390/Makefile b/drivers/net/ethernet/8390/Makefile
index 3337d7fb434..d13790b7fd2 100644
--- a/drivers/net/ethernet/8390/Makefile
+++ b/drivers/net/ethernet/8390/Makefile
@@ -24,6 +24,5 @@ obj-$(CONFIG_PCMCIA_PCNET) += pcnet_cs.o 8390.o
 obj-$(CONFIG_STNIC) += stnic.o 8390.o
 obj-$(CONFIG_ULTRA) += smc-ultra.o 8390.o
 obj-$(CONFIG_ULTRA32) += smc-ultra32.o 8390.o
-obj-$(CONFIG_ULTRAMCA) += smc-mca.o 8390.o
 obj-$(CONFIG_WD80x3) += wd.o 8390.o
 obj-$(CONFIG_ZORRO8390) += zorro8390.o 8390.o
diff --git a/drivers/net/ethernet/8390/ax88796.c b/drivers/net/ethernet/8390/ax88796.c
index 11476ca95e9..203ff9dccad 100644
--- a/drivers/net/ethernet/8390/ax88796.c
+++ b/drivers/net/ethernet/8390/ax88796.c
@@ -501,6 +501,7 @@ static const struct ethtool_ops ax_ethtool_ops = {
 	.get_settings		= ax_get_settings,
 	.set_settings		= ax_set_settings,
 	.get_link		= ethtool_op_get_link,
+	.get_ts_info		= ethtool_op_get_ts_info,
 };
 
 #ifdef CONFIG_AX88796_93CX6
diff --git a/drivers/net/ethernet/8390/etherh.c b/drivers/net/ethernet/8390/etherh.c
index dbefd5658c1..8322c54972f 100644
--- a/drivers/net/ethernet/8390/etherh.c
+++ b/drivers/net/ethernet/8390/etherh.c
@@ -635,6 +635,7 @@ static const struct ethtool_ops etherh_ethtool_ops = {
 	.get_settings	= etherh_get_settings,
 	.set_settings	= etherh_set_settings,
 	.get_drvinfo	= etherh_get_drvinfo,
+	.get_ts_info	= ethtool_op_get_ts_info,
 };
 
 static const struct net_device_ops etherh_netdev_ops = {
diff --git a/drivers/net/ethernet/8390/ne2.c b/drivers/net/ethernet/8390/ne2.c
deleted file mode 100644
index ef85839f43d..00000000000
--- a/drivers/net/ethernet/8390/ne2.c
+++ /dev/null
@@ -1,798 +0,0 @@
-/* ne2.c: A NE/2 Ethernet Driver for Linux. */
-/*
-   Based on the NE2000 driver written by Donald Becker (1992-94).
-   modified by Wim Dumon (Apr 1996)
-
-   This software may be used and distributed according to the terms
-   of the GNU General Public License, incorporated herein by reference.
-
-   The author may be reached as wimpie@linux.cc.kuleuven.ac.be
-
-   Currently supported: NE/2
-   This patch was never tested on other MCA-ethernet adapters, but it
-   might work. Just give it a try and let me know if you have problems.
-   Also mail me if it really works, please!
-
-   Changelog:
-   Mon Feb  3 16:26:02 MET 1997
-   - adapted the driver to work with the 2.1.25 kernel
-   - multiple ne2 support (untested)
-   - module support (untested)
-
-   Fri Aug 28 00:18:36 CET 1998 (David Weinehall)
-   - fixed a few minor typos
-   - made the MODULE_PARM conditional (it only works with the v2.1.x kernels)
-   - fixed the module support (Now it's working...)
-
-   Mon Sep  7 19:01:44 CET 1998 (David Weinehall)
-   - added support for Arco Electronics AE/2-card (experimental)
-
-   Mon Sep 14 09:53:42 CET 1998 (David Weinehall)
-   - added support for Compex ENET-16MC/P (experimental)
-
-   Tue Sep 15 16:21:12 CET 1998 (David Weinehall, Magnus Jonsson, Tomas Ogren)
-   - Miscellaneous bugfixes
-
-   Tue Sep 19 16:21:12 CET 1998 (Magnus Jonsson)
-   - Cleanup
-
-   Wed Sep 23 14:33:34 CET 1998 (David Weinehall)
-   - Restructuring and rewriting for v2.1.x compliance
-
-   Wed Oct 14 17:19:21 CET 1998 (David Weinehall)
-   - Added code that unregisters irq and proc-info
-   - Version# bump
-
-   Mon Nov 16 15:28:23 CET 1998 (Wim Dumon)
-   - pass 'dev' as last parameter of request_irq in stead of 'NULL'
-
-   Wed Feb  7 21:24:00 CET 2001 (Alfred Arnold)
-   - added support for the D-Link DE-320CT
-
-   *    WARNING
-	-------
-	This is alpha-test software.  It is not guaranteed to work. As a
-	matter of fact, I'm quite sure there are *LOTS* of bugs in here. I
-	would like to hear from you if you use this driver, even if it works.
-	If it doesn't work, be sure to send me a mail with the problems !
-*/
-
-static const char *version = "ne2.c:v0.91 Nov 16 1998 Wim Dumon <wimpie@kotnet.org>\n";
-
-#include <linux/module.h>
-#include <linux/kernel.h>
-#include <linux/types.h>
-#include <linux/fcntl.h>
-#include <linux/interrupt.h>
-#include <linux/ioport.h>
-#include <linux/in.h>
-#include <linux/string.h>
-#include <linux/errno.h>
-#include <linux/init.h>
-#include <linux/mca-legacy.h>
-#include <linux/netdevice.h>
-#include <linux/etherdevice.h>
-#include <linux/skbuff.h>
-#include <linux/bitops.h>
-#include <linux/jiffies.h>
-
-#include <asm/io.h>
-#include <asm/dma.h>
-
-#include "8390.h"
-
-#define DRV_NAME "ne2"
-
-/* Some defines that people can play with if so inclined. */
-
-/* Do we perform extra sanity checks on stuff ? */
-/* #define NE_SANITY_CHECK */
-
-/* Do we implement the read before write bugfix ? */
-/* #define NE_RW_BUGFIX */
-
-/* Do we have a non std. amount of memory? (in units of 256 byte pages) */
-/* #define PACKETBUF_MEMSIZE	0x40 */
-
-
-/* ---- No user-serviceable parts below ---- */
-
-#define NE_BASE	 (dev->base_addr)
-#define NE_CMD	 	0x00
-#define NE_DATAPORT	0x10	/* NatSemi-defined port window offset. */
-#define NE_RESET	0x20	/* Issue a read to reset, a write to clear. */
-#define NE_IO_EXTENT	0x30
-
-#define NE1SM_START_PG	0x20	/* First page of TX buffer */
-#define NE1SM_STOP_PG 	0x40	/* Last page +1 of RX ring */
-#define NESM_START_PG	0x40	/* First page of TX buffer */
-#define NESM_STOP_PG	0x80	/* Last page +1 of RX ring */
-
-/* From the .ADF file: */
-static unsigned int addresses[7] __initdata =
-		{0x1000, 0x2020, 0x8020, 0xa0a0, 0xb0b0, 0xc0c0, 0xc3d0};
-static int irqs[4] __initdata = {3, 4, 5, 9};
-
-/* From the D-Link ADF file: */
-static unsigned int dlink_addresses[4] __initdata =
-                {0x300, 0x320, 0x340, 0x360};
-static int dlink_irqs[8] __initdata = {3, 4, 5, 9, 10, 11, 14, 15};
-
-struct ne2_adapters_t {
-	unsigned int	id;
-	char		*name;
-};
-
-static struct ne2_adapters_t ne2_adapters[] __initdata = {
-	{ 0x6354, "Arco Ethernet Adapter AE/2" },
-	{ 0x70DE, "Compex ENET-16 MC/P" },
-	{ 0x7154, "Novell Ethernet Adapter NE/2" },
-        { 0x56ea, "D-Link DE-320CT" },
-	{ 0x0000, NULL }
-};
-
-extern int netcard_probe(struct net_device *dev);
-
-static int ne2_probe1(struct net_device *dev, int slot);
-
-static void ne_reset_8390(struct net_device *dev);
-static void ne_get_8390_hdr(struct net_device *dev, struct e8390_pkt_hdr *hdr,
-		int ring_page);
-static void ne_block_input(struct net_device *dev, int count,
-		struct sk_buff *skb, int ring_offset);
-static void ne_block_output(struct net_device *dev, const int count,
-		const unsigned char *buf, const int start_page);
-
-
-/*
- * special code to read the DE-320's MAC address EEPROM.  In contrast to a
- * standard NE design, this is a serial EEPROM (93C46) that has to be read
- * bit by bit.  The EEPROM cotrol port at base + 0x1e has the following
- * layout:
- *
- * Bit 0 = Data out (read from EEPROM)
- * Bit 1 = Data in  (write to EEPROM)
- * Bit 2 = Clock
- * Bit 3 = Chip Select
- * Bit 7 = ~50 kHz clock for defined delays
- *
- */
-
-static void __init dlink_put_eeprom(unsigned char value, unsigned int addr)
-{
-	int z;
-	unsigned char v1, v2;
-
-	/* write the value to the NIC EEPROM register */
-
-	outb(value, addr + 0x1e);
-
-	/* now wait the clock line to toggle twice.  Effectively, we are
-	   waiting (at least) for one clock cycle */
-
-	for (z = 0; z < 2; z++) {
-		do {
-			v1 = inb(addr + 0x1e);
-			v2 = inb(addr + 0x1e);
-		}
-		while (!((v1 ^ v2) & 0x80));
-	}
-}
-
-static void __init dlink_send_eeprom_bit(unsigned int bit, unsigned int addr)
-{
-	/* shift data bit into correct position */
-
-	bit = bit << 1;
-
-	/* write value, keep clock line high for two cycles */
-
-	dlink_put_eeprom(0x09 | bit, addr);
-	dlink_put_eeprom(0x0d | bit, addr);
-	dlink_put_eeprom(0x0d | bit, addr);
-	dlink_put_eeprom(0x09 | bit, addr);
-}
-
-static void __init dlink_send_eeprom_word(unsigned int value, unsigned int len, unsigned int addr)
-{
-	int z;
-
-	/* adjust bits so that they are left-aligned in a 16-bit-word */
-
-	value = value << (16 - len);
-
-	/* shift bits out to the EEPROM */
-
-	for (z = 0; z < len; z++) {
-		dlink_send_eeprom_bit((value & 0x8000) >> 15, addr);
-		value = value << 1;
-	}
-}
-
-static unsigned int __init dlink_get_eeprom(unsigned int eeaddr, unsigned int addr)
-{
-	int z;
-	unsigned int value = 0;
-
-	/* pull the CS line low for a moment.  This resets the EEPROM-
-	   internal logic, and makes it ready for a new command. */
-
-	dlink_put_eeprom(0x01, addr);
-	dlink_put_eeprom(0x09, addr);
-
-	/* send one start bit, read command (1 - 0), plus the address to
-           the EEPROM */
-
-	dlink_send_eeprom_word(0x0180 | (eeaddr & 0x3f), 9, addr);
-
-	/* get the data word.  We clock by sending 0s to the EEPROM, which
-	   get ignored during the read process */
-
-	for (z = 0; z < 16; z++) {
-		dlink_send_eeprom_bit(0, addr);
-		value = (value << 1) | (inb(addr + 0x1e) & 0x01);
-	}
-
-	return value;
-}
-
-/*
- * Note that at boot, this probe only picks up one card at a time.
- */
-
-static int __init do_ne2_probe(struct net_device *dev)
-{
-	static int current_mca_slot = -1;
-	int i;
-	int adapter_found = 0;
-
-	/* Do not check any supplied i/o locations.
-	   POS registers usually don't fail :) */
-
-	/* MCA cards have POS registers.
-	   Autodetecting MCA cards is extremely simple.
-	   Just search for the card. */
-
-	for(i = 0; (ne2_adapters[i].name != NULL) && !adapter_found; i++) {
-		current_mca_slot =
-			mca_find_unused_adapter(ne2_adapters[i].id, 0);
-
-		if((current_mca_slot != MCA_NOTFOUND) && !adapter_found) {
-			int res;
-			mca_set_adapter_name(current_mca_slot,
-					ne2_adapters[i].name);
-			mca_mark_as_used(current_mca_slot);
-
-			res = ne2_probe1(dev, current_mca_slot);
-			if (res)
-				mca_mark_as_unused(current_mca_slot);
-			return res;
-		}
-	}
-	return -ENODEV;
-}
-
-#ifndef MODULE
-struct net_device * __init ne2_probe(int unit)
-{
-	struct net_device *dev = alloc_eip_netdev();
-	int err;
-
-	if (!dev)
-		return ERR_PTR(-ENOMEM);
-
-	sprintf(dev->name, "eth%d", unit);
-	netdev_boot_setup_check(dev);
-
-	err = do_ne2_probe(dev);
-	if (err)
-		goto out;
-	return dev;
-out:
-	free_netdev(dev);
-	return ERR_PTR(err);
-}
-#endif
-
-static int ne2_procinfo(char *buf, int slot, struct net_device *dev)
-{
-	int len=0;
-
-	len += sprintf(buf+len, "The NE/2 Ethernet Adapter\n" );
-	len += sprintf(buf+len, "Driver written by Wim Dumon ");
-	len += sprintf(buf+len, "<wimpie@kotnet.org>\n");
-	len += sprintf(buf+len, "Modified by ");
-	len += sprintf(buf+len, "David Weinehall <tao@acc.umu.se>\n");
-	len += sprintf(buf+len, "and by Magnus Jonsson <bigfoot@acc.umu.se>\n");
-	len += sprintf(buf+len, "Based on the original NE2000 drivers\n" );
-	len += sprintf(buf+len, "Base IO: %#x\n", (unsigned int)dev->base_addr);
-	len += sprintf(buf+len, "IRQ    : %d\n", dev->irq);
-	len += sprintf(buf+len, "HW addr : %pM\n", dev->dev_addr);
-
-	return len;
-}
-
-static int __init ne2_probe1(struct net_device *dev, int slot)
-{
-	int i, base_addr, irq, retval;
-	unsigned char POS;
-	unsigned char SA_prom[32];
-	const char *name = "NE/2";
-	int start_page, stop_page;
-	static unsigned version_printed;
-
-	if (ei_debug && version_printed++ == 0)
-		printk(version);
-
-	printk("NE/2 ethercard found in slot %d:", slot);
-
-	/* Read base IO and IRQ from the POS-registers */
-	POS = mca_read_stored_pos(slot, 2);
-	if(!(POS % 2)) {
-		printk(" disabled.\n");
-		return -ENODEV;
-	}
-
-	/* handle different POS register structure for D-Link card */
-
-	if (mca_read_stored_pos(slot, 0) == 0xea) {
-		base_addr = dlink_addresses[(POS >> 5) & 0x03];
-		irq = dlink_irqs[(POS >> 2) & 0x07];
-	}
-        else {
-		i = (POS & 0xE)>>1;
-		/* printk("Halleluja sdog, als er na de pijl een 1 staat is 1 - 1 == 0"
-	   	" en zou het moeten werken -> %d\n", i);
-	   	The above line was for remote testing, thanx to sdog ... */
-		base_addr = addresses[i - 1];
-		irq = irqs[(POS & 0x60)>>5];
-	}
-
-	if (!request_region(base_addr, NE_IO_EXTENT, DRV_NAME))
-		return -EBUSY;
-
-#ifdef DEBUG
-	printk("POS info : pos 2 = %#x ; base = %#x ; irq = %ld\n", POS,
-			base_addr, irq);
-#endif
-
-#ifndef CRYNWR_WAY
-	/* Reset the card the way they do it in the Crynwr packet driver */
-	for (i=0; i<8; i++)
-		outb(0x0, base_addr + NE_RESET);
-	inb(base_addr + NE_RESET);
-	outb(0x21, base_addr + NE_CMD);
-	if (inb(base_addr + NE_CMD) != 0x21) {
-		printk("NE/2 adapter not responding\n");
-		retval = -ENODEV;
-		goto out;
-	}
-
-	/* In the crynwr sources they do a RAM-test here. I skip it. I suppose
-	   my RAM is okay.  Suppose your memory is broken.  Then this test
-	   should fail and you won't be able to use your card.  But if I do not
-	   test, you won't be able to use your card, neither.  So this test
-	   won't help you. */
-
-#else  /* _I_ never tested it this way .. Go ahead and try ...*/
-	/* Reset card. Who knows what dain-bramaged state it was left in. */
-	{
-		unsigned long reset_start_time = jiffies;
-
-		/* DON'T change these to inb_p/outb_p or reset will fail on
-		   clones.. */
-		outb(inb(base_addr + NE_RESET), base_addr + NE_RESET);
-
-		while ((inb_p(base_addr + EN0_ISR) & ENISR_RESET) == 0)
-			if (time_after(jiffies, reset_start_time + 2*HZ/100)) {
-				printk(" not found (no reset ack).\n");
-				retval = -ENODEV;
-				goto out;
-			}
-
-		outb_p(0xff, base_addr + EN0_ISR);         /* Ack all intr. */
-	}
-#endif
-
-
-	/* Read the 16 bytes of station address PROM.
-	   We must first initialize registers, similar to
-	   NS8390p_init(eifdev, 0).
-	   We can't reliably read the SAPROM address without this.
-	   (I learned the hard way!). */
-	{
-		struct {
-			unsigned char value, offset;
-		} program_seq[] = {
-						/* Select page 0 */
-			{E8390_NODMA+E8390_PAGE0+E8390_STOP, E8390_CMD},
-			{0x49,	EN0_DCFG},  /* Set WORD-wide (0x49) access. */
-			{0x00,	EN0_RCNTLO},  /* Clear the count regs. */
-			{0x00,	EN0_RCNTHI},
-			{0x00,	EN0_IMR},  /* Mask completion irq. */
-			{0xFF,	EN0_ISR},
-			{E8390_RXOFF, EN0_RXCR},  /* 0x20  Set to monitor */
-			{E8390_TXOFF, EN0_TXCR},  /* 0x02  and loopback mode. */
-			{32,	EN0_RCNTLO},
-			{0x00,	EN0_RCNTHI},
-			{0x00,	EN0_RSARLO},  /* DMA starting at 0x0000. */
-			{0x00,	EN0_RSARHI},
-			{E8390_RREAD+E8390_START, E8390_CMD},
-		};
-
-		for (i = 0; i < ARRAY_SIZE(program_seq); i++)
-			outb_p(program_seq[i].value, base_addr +
-				program_seq[i].offset);
-
-	}
-	for(i = 0; i < 6 /*sizeof(SA_prom)*/; i+=1) {
-		SA_prom[i] = inb(base_addr + NE_DATAPORT);
-	}
-
-	/* I don't know whether the previous sequence includes the general
-           board reset procedure, so better don't omit it and just overwrite
-           the garbage read from a DE-320 with correct stuff. */
-
-	if (mca_read_stored_pos(slot, 0) == 0xea) {
-		unsigned int v;
-
-		for (i = 0; i < 3; i++) {
- 			v = dlink_get_eeprom(i, base_addr);
-			SA_prom[(i << 1)    ] = v & 0xff;
-			SA_prom[(i << 1) + 1] = (v >> 8) & 0xff;
-		}
-	}
-
-	start_page = NESM_START_PG;
-	stop_page = NESM_STOP_PG;
-
-	dev->irq=irq;
-
-	/* Snarf the interrupt now.  There's no point in waiting since we cannot
-	   share and the board will usually be enabled. */
-	retval = request_irq(dev->irq, eip_interrupt, 0, DRV_NAME, dev);
-	if (retval) {
-		printk (" unable to get IRQ %d (irqval=%d).\n",
-				dev->irq, retval);
-		goto out;
-	}
-
-	dev->base_addr = base_addr;
-
-	for (i = 0; i < ETH_ALEN; i++)
-		dev->dev_addr[i] = SA_prom[i];
-
-	printk(" %pM\n", dev->dev_addr);
-
-	printk("%s: %s found at %#x, using IRQ %d.\n",
-			dev->name, name, base_addr, dev->irq);
-
-	mca_set_adapter_procfn(slot, (MCA_ProcFn) ne2_procinfo, dev);
-
-	ei_status.name = name;
-	ei_status.tx_start_page = start_page;
-	ei_status.stop_page = stop_page;
-	ei_status.word16 = (2 == 2);
-
-	ei_status.rx_start_page = start_page + TX_PAGES;
-#ifdef PACKETBUF_MEMSIZE
-	/* Allow the packet buffer size to be overridden by know-it-alls. */
-	ei_status.stop_page = ei_status.tx_start_page + PACKETBUF_MEMSIZE;
-#endif
-
-	ei_status.reset_8390 = &ne_reset_8390;
-	ei_status.block_input = &ne_block_input;
-	ei_status.block_output = &ne_block_output;
-	ei_status.get_8390_hdr = &ne_get_8390_hdr;
-
-	ei_status.priv = slot;
-
-	dev->netdev_ops = &eip_netdev_ops;
-	NS8390p_init(dev, 0);
-
-	retval = register_netdev(dev);
-	if (retval)
-		goto out1;
-	return 0;
-out1:
-	mca_set_adapter_procfn( ei_status.priv, NULL, NULL);
-	free_irq(dev->irq, dev);
-out:
-	release_region(base_addr, NE_IO_EXTENT);
-	return retval;
-}
-
-/* Hard reset the card.  This used to pause for the same period that a
-   8390 reset command required, but that shouldn't be necessary. */
-static void ne_reset_8390(struct net_device *dev)
-{
-	unsigned long reset_start_time = jiffies;
-
-	if (ei_debug > 1)
-		printk("resetting the 8390 t=%ld...", jiffies);
-
-	/* DON'T change these to inb_p/outb_p or reset will fail on clones. */
-	outb(inb(NE_BASE + NE_RESET), NE_BASE + NE_RESET);
-
-	ei_status.txing = 0;
-	ei_status.dmaing = 0;
-
-	/* This check _should_not_ be necessary, omit eventually. */
-	while ((inb_p(NE_BASE+EN0_ISR) & ENISR_RESET) == 0)
-		if (time_after(jiffies, reset_start_time + 2*HZ/100)) {
-			printk("%s: ne_reset_8390() did not complete.\n",
-					dev->name);
-			break;
-		}
-	outb_p(ENISR_RESET, NE_BASE + EN0_ISR);	/* Ack intr. */
-}
-
-/* Grab the 8390 specific header. Similar to the block_input routine, but
-   we don't need to be concerned with ring wrap as the header will be at
-   the start of a page, so we optimize accordingly. */
-
-static void ne_get_8390_hdr(struct net_device *dev, struct e8390_pkt_hdr *hdr,
-		int ring_page)
-{
-
-	int nic_base = dev->base_addr;
-
-	/* This *shouldn't* happen.
-	   If it does, it's the last thing you'll see */
-	if (ei_status.dmaing) {
-		printk("%s: DMAing conflict in ne_get_8390_hdr "
-				"[DMAstat:%d][irqlock:%d].\n",
-				dev->name, ei_status.dmaing, ei_status.irqlock);
-		return;
-	}
-
-	ei_status.dmaing |= 0x01;
-	outb_p(E8390_NODMA+E8390_PAGE0+E8390_START, nic_base+ NE_CMD);
-	outb_p(sizeof(struct e8390_pkt_hdr), nic_base + EN0_RCNTLO);
-	outb_p(0, nic_base + EN0_RCNTHI);
-	outb_p(0, nic_base + EN0_RSARLO);		/* On page boundary */
-	outb_p(ring_page, nic_base + EN0_RSARHI);
-	outb_p(E8390_RREAD+E8390_START, nic_base + NE_CMD);
-
-	if (ei_status.word16)
-		insw(NE_BASE + NE_DATAPORT, hdr,
-				sizeof(struct e8390_pkt_hdr)>>1);
-	else
-		insb(NE_BASE + NE_DATAPORT, hdr,
-				sizeof(struct e8390_pkt_hdr));
-
-	outb_p(ENISR_RDC, nic_base + EN0_ISR);	/* Ack intr. */
-	ei_status.dmaing &= ~0x01;
-}
-
-/* Block input and output, similar to the Crynwr packet driver.  If you
-   are porting to a new ethercard, look at the packet driver source for
-   hints. The NEx000 doesn't share the on-board packet memory -- you have
-   to put the packet out through the "remote DMA" dataport using outb. */
-
-static void ne_block_input(struct net_device *dev, int count, struct sk_buff *skb,
-		int ring_offset)
-{
-#ifdef NE_SANITY_CHECK
-	int xfer_count = count;
-#endif
-	int nic_base = dev->base_addr;
-	char *buf = skb->data;
-
-	/* This *shouldn't* happen.
-	   If it does, it's the last thing you'll see */
-	if (ei_status.dmaing) {
-		printk("%s: DMAing conflict in ne_block_input "
-				"[DMAstat:%d][irqlock:%d].\n",
-				dev->name, ei_status.dmaing, ei_status.irqlock);
-		return;
-	}
-	ei_status.dmaing |= 0x01;
-	outb_p(E8390_NODMA+E8390_PAGE0+E8390_START, nic_base+ NE_CMD);
-	outb_p(count & 0xff, nic_base + EN0_RCNTLO);
-	outb_p(count >> 8, nic_base + EN0_RCNTHI);
-	outb_p(ring_offset & 0xff, nic_base + EN0_RSARLO);
-	outb_p(ring_offset >> 8, nic_base + EN0_RSARHI);
-	outb_p(E8390_RREAD+E8390_START, nic_base + NE_CMD);
-	if (ei_status.word16) {
-		insw(NE_BASE + NE_DATAPORT,buf,count>>1);
-		if (count & 0x01) {
-			buf[count-1] = inb(NE_BASE + NE_DATAPORT);
-#ifdef NE_SANITY_CHECK
-			xfer_count++;
-#endif
-		}
-	} else {
-		insb(NE_BASE + NE_DATAPORT, buf, count);
-	}
-
-#ifdef NE_SANITY_CHECK
-	/* This was for the ALPHA version only, but enough people have
-	   been encountering problems so it is still here.  If you see
-	   this message you either 1) have a slightly incompatible clone
-	   or 2) have noise/speed problems with your bus. */
-	if (ei_debug > 1) {	/* DMA termination address check... */
-		int addr, tries = 20;
-		do {
-			/* DON'T check for 'inb_p(EN0_ISR) & ENISR_RDC' here
-			   -- it's broken for Rx on some cards! */
-			int high = inb_p(nic_base + EN0_RSARHI);
-			int low = inb_p(nic_base + EN0_RSARLO);
-			addr = (high << 8) + low;
-			if (((ring_offset + xfer_count) & 0xff) == low)
-				break;
-		} while (--tries > 0);
-		if (tries <= 0)
-			printk("%s: RX transfer address mismatch,"
-				"%#4.4x (expected) vs. %#4.4x (actual).\n",
-				dev->name, ring_offset + xfer_count, addr);
-	}
-#endif
-	outb_p(ENISR_RDC, nic_base + EN0_ISR);	/* Ack intr. */
-	ei_status.dmaing &= ~0x01;
-}
-
-static void ne_block_output(struct net_device *dev, int count,
-		const unsigned char *buf, const int start_page)
-{
-	int nic_base = NE_BASE;
-	unsigned long dma_start;
-#ifdef NE_SANITY_CHECK
-	int retries = 0;
-#endif
-
-	/* Round the count up for word writes. Do we need to do this?
-	   What effect will an odd byte count have on the 8390?
-	   I should check someday. */
-	if (ei_status.word16 && (count & 0x01))
-		count++;
-
-	/* This *shouldn't* happen.
-	   If it does, it's the last thing you'll see */
-	if (ei_status.dmaing) {
-		printk("%s: DMAing conflict in ne_block_output."
-				"[DMAstat:%d][irqlock:%d]\n",
-				dev->name, ei_status.dmaing, ei_status.irqlock);
-		return;
-	}
-	ei_status.dmaing |= 0x01;
-	/* We should already be in page 0, but to be safe... */
-	outb_p(E8390_PAGE0+E8390_START+E8390_NODMA, nic_base + NE_CMD);
-
-#ifdef NE_SANITY_CHECK
-retry:
-#endif
-
-#ifdef NE8390_RW_BUGFIX
-	/* Handle the read-before-write bug the same way as the
-	   Crynwr packet driver -- the NatSemi method doesn't work.
-	   Actually this doesn't always work either, but if you have
-	   problems with your NEx000 this is better than nothing! */
-	outb_p(0x42, nic_base + EN0_RCNTLO);
-	outb_p(0x00, nic_base + EN0_RCNTHI);
-	outb_p(0x42, nic_base + EN0_RSARLO);
-	outb_p(0x00, nic_base + EN0_RSARHI);
-	outb_p(E8390_RREAD+E8390_START, nic_base + NE_CMD);
-	/* Make certain that the dummy read has occurred. */
-	SLOW_DOWN_IO;
-	SLOW_DOWN_IO;
-	SLOW_DOWN_IO;
-#endif
-
-	outb_p(ENISR_RDC, nic_base + EN0_ISR);
-
-	/* Now the normal output. */
-	outb_p(count & 0xff, nic_base + EN0_RCNTLO);
-	outb_p(count >> 8,   nic_base + EN0_RCNTHI);
-	outb_p(0x00, nic_base + EN0_RSARLO);
-	outb_p(start_page, nic_base + EN0_RSARHI);
-
-	outb_p(E8390_RWRITE+E8390_START, nic_base + NE_CMD);
-	if (ei_status.word16) {
-		outsw(NE_BASE + NE_DATAPORT, buf, count>>1);
-	} else {
-		outsb(NE_BASE + NE_DATAPORT, buf, count);
-	}
-
-	dma_start = jiffies;
-
-#ifdef NE_SANITY_CHECK
-	/* This was for the ALPHA version only, but enough people have
-	   been encountering problems so it is still here. */
-
-	if (ei_debug > 1) {		/* DMA termination address check... */
-		int addr, tries = 20;
-		do {
-			int high = inb_p(nic_base + EN0_RSARHI);
-			int low = inb_p(nic_base + EN0_RSARLO);
-			addr = (high << 8) + low;
-			if ((start_page << 8) + count == addr)
-				break;
-		} while (--tries > 0);
-		if (tries <= 0) {
-			printk("%s: Tx packet transfer address mismatch,"
-					"%#4.4x (expected) vs. %#4.4x (actual).\n",
-					dev->name, (start_page << 8) + count, addr);
-			if (retries++ == 0)
-				goto retry;
-		}
-	}
-#endif
-
-	while ((inb_p(nic_base + EN0_ISR) & ENISR_RDC) == 0)
-		if (time_after(jiffies, dma_start + 2*HZ/100)) {		/* 20ms */
-			printk("%s: timeout waiting for Tx RDC.\n", dev->name);
-			ne_reset_8390(dev);
-			NS8390p_init(dev, 1);
-			break;
-		}
-
-	outb_p(ENISR_RDC, nic_base + EN0_ISR);	/* Ack intr. */
-	ei_status.dmaing &= ~0x01;
-}
-
-
-#ifdef MODULE
-#define MAX_NE_CARDS	4	/* Max number of NE cards per module */
-static struct net_device *dev_ne[MAX_NE_CARDS];
-static int io[MAX_NE_CARDS];
-static int irq[MAX_NE_CARDS];
-static int bad[MAX_NE_CARDS];	/* 0xbad = bad sig or no reset ack */
-MODULE_LICENSE("GPL");
-
-module_param_array(io, int, NULL, 0);
-module_param_array(irq, int, NULL, 0);
-module_param_array(bad, int, NULL, 0);
-MODULE_PARM_DESC(io, "(ignored)");
-MODULE_PARM_DESC(irq, "(ignored)");
-MODULE_PARM_DESC(bad, "(ignored)");
-
-/* Module code fixed by David Weinehall */
-
-int __init init_module(void)
-{
-	struct net_device *dev;
-	int this_dev, found = 0;
-
-	for (this_dev = 0; this_dev < MAX_NE_CARDS; this_dev++) {
-		dev = alloc_eip_netdev();
-		if (!dev)
-			break;
-		dev->irq = irq[this_dev];
-		dev->mem_end = bad[this_dev];
-		dev->base_addr = io[this_dev];
-		if (do_ne2_probe(dev) == 0) {
-			dev_ne[found++] = dev;
-			continue;
-		}
-		free_netdev(dev);
-		break;
-	}
-	if (found)
-		return 0;
-	printk(KERN_WARNING "ne2.c: No NE/2 card found\n");
-	return -ENXIO;
-}
-
-static void cleanup_card(struct net_device *dev)
-{
-	mca_mark_as_unused(ei_status.priv);
-	mca_set_adapter_procfn( ei_status.priv, NULL, NULL);
-	free_irq(dev->irq, dev);
-	release_region(dev->base_addr, NE_IO_EXTENT);
-}
-
-void __exit cleanup_module(void)
-{
-	int this_dev;
-
-	for (this_dev = 0; this_dev < MAX_NE_CARDS; this_dev++) {
-		struct net_device *dev = dev_ne[this_dev];
-		if (dev) {
-			unregister_netdev(dev);
-			cleanup_card(dev);
-			free_netdev(dev);
-		}
-	}
-}
-#endif /* MODULE */
diff --git a/drivers/net/ethernet/8390/smc-mca.c b/drivers/net/ethernet/8390/smc-mca.c
deleted file mode 100644
index 7a68590f280..00000000000
--- a/drivers/net/ethernet/8390/smc-mca.c
+++ /dev/null
@@ -1,575 +0,0 @@
-/* smc-mca.c: A SMC Ultra ethernet driver for linux. */
-/*
-    Most of this driver, except for ultramca_probe is nearly
-    verbatim from smc-ultra.c by Donald Becker. The rest is
-    written and copyright 1996 by David Weis, weisd3458@uni.edu
-
-    This is a driver for the SMC Ultra and SMC EtherEZ ethercards.
-
-    This driver uses the cards in the 8390-compatible, shared memory mode.
-    Most of the run-time complexity is handled by the generic code in
-    8390.c.
-
-    This driver enables the shared memory only when doing the actual data
-    transfers to avoid a bug in early version of the card that corrupted
-    data transferred by a AHA1542.
-
-    This driver does not support the programmed-I/O data transfer mode of
-    the EtherEZ.  That support (if available) is smc-ez.c.  Nor does it
-    use the non-8390-compatible "Altego" mode. (No support currently planned.)
-
-    Changelog:
-
-    Paul Gortmaker	 : multiple card support for module users.
-    David Weis		 : Micro Channel-ized it.
-    Tom Sightler	 : Added support for IBM PS/2 Ethernet Adapter/A
-    Christopher Turcksin : Changed MCA-probe so that multiple adapters are
-			   found correctly (Jul 16, 1997)
-    Chris Beauregard	 : Tried to merge the two changes above (Dec 15, 1997)
-    Tom Sightler	 : Fixed minor detection bug caused by above merge
-    Tom Sightler	 : Added support for three more Western Digital
-			   MCA-adapters
-    Tom Sightler	 : Added support for 2.2.x mca_find_unused_adapter
-    Hartmut Schmidt	 : - Modified parameter detection to handle each
-			     card differently depending on a switch-list
-			   - 'card_ver' removed from the adapter list
-			   - Some minor bug fixes
-*/
-
-#include <linux/mca.h>
-#include <linux/module.h>
-#include <linux/kernel.h>
-#include <linux/errno.h>
-#include <linux/string.h>
-#include <linux/init.h>
-#include <linux/interrupt.h>
-#include <linux/netdevice.h>
-#include <linux/etherdevice.h>
-
-#include <asm/io.h>
-
-#include "8390.h"
-
-#define DRV_NAME "smc-mca"
-
-static int ultramca_open(struct net_device *dev);
-static void ultramca_reset_8390(struct net_device *dev);
-static void ultramca_get_8390_hdr(struct net_device *dev,
-                                  struct e8390_pkt_hdr *hdr,
-                                  int ring_page);
-static void ultramca_block_input(struct net_device *dev, int count,
-                                 struct sk_buff *skb,
-                                 int ring_offset);
-static void ultramca_block_output(struct net_device *dev, int count,
-                                  const unsigned char *buf,
-                                  const int start_page);
-static int ultramca_close_card(struct net_device *dev);
-
-#define START_PG        0x00    /* First page of TX buffer */
-
-#define ULTRA_CMDREG 0      /* Offset to ASIC command register. */
-#define ULTRA_RESET  0x80   /* Board reset, in ULTRA_CMDREG. */
-#define ULTRA_MEMENB 0x40   /* Enable the shared memory. */
-#define ULTRA_NIC_OFFSET 16 /* NIC register offset from the base_addr. */
-#define ULTRA_IO_EXTENT 32
-#define EN0_ERWCNT      0x08  /* Early receive warning count. */
-
-#define _61c8_SMC_Ethercard_PLUS_Elite_A_BNC_AUI_WD8013EP_A            0
-#define _61c9_SMC_Ethercard_PLUS_Elite_A_UTP_AUI_WD8013EP_A            1
-#define _6fc0_WD_Ethercard_PLUS_A_WD8003E_A_OR_WD8003ET_A              2
-#define _6fc1_WD_Starcard_PLUS_A_WD8003ST_A                            3
-#define _6fc2_WD_Ethercard_PLUS_10T_A_WD8003W_A                        4
-#define _efd4_IBM_PS2_Adapter_A_for_Ethernet_UTP_AUI_WD8013WP_A        5
-#define _efd5_IBM_PS2_Adapter_A_for_Ethernet_BNC_AUI_WD8013WP_A        6
-#define _efe5_IBM_PS2_Adapter_A_for_Ethernet                           7
-
-struct smc_mca_adapters_t {
-	unsigned int id;
-	char *name;
-};
-
-#define MAX_ULTRAMCA_CARDS 4	/* Max number of Ultra cards per module */
-
-static int ultra_io[MAX_ULTRAMCA_CARDS];
-static int ultra_irq[MAX_ULTRAMCA_CARDS];
-MODULE_LICENSE("GPL");
-
-module_param_array(ultra_io, int, NULL, 0);
-module_param_array(ultra_irq, int, NULL, 0);
-MODULE_PARM_DESC(ultra_io, "SMC Ultra/EtherEZ MCA I/O base address(es)");
-MODULE_PARM_DESC(ultra_irq, "SMC Ultra/EtherEZ MCA IRQ number(s)");
-
-static const struct {
-  unsigned int base_addr;
-} addr_table[] = {
-    { 0x0800 },
-    { 0x1800 },
-    { 0x2800 },
-    { 0x3800 },
-    { 0x4800 },
-    { 0x5800 },
-    { 0x6800 },
-    { 0x7800 },
-    { 0x8800 },
-    { 0x9800 },
-    { 0xa800 },
-    { 0xb800 },
-    { 0xc800 },
-    { 0xd800 },
-    { 0xe800 },
-    { 0xf800 }
-};
-
-#define MEM_MASK 64
-
-static const struct {
-  unsigned char mem_index;
-  unsigned long mem_start;
-  unsigned char num_pages;
-} mem_table[] = {
-    { 16, 0x0c0000, 40 },
-    { 18, 0x0c4000, 40 },
-    { 20, 0x0c8000, 40 },
-    { 22, 0x0cc000, 40 },
-    { 24, 0x0d0000, 40 },
-    { 26, 0x0d4000, 40 },
-    { 28, 0x0d8000, 40 },
-    { 30, 0x0dc000, 40 },
-    {144, 0xfc0000, 40 },
-    {148, 0xfc8000, 40 },
-    {154, 0xfd0000, 40 },
-    {156, 0xfd8000, 40 },
-    {  0, 0x0c0000, 20 },
-    {  1, 0x0c2000, 20 },
-    {  2, 0x0c4000, 20 },
-    {  3, 0x0c6000, 20 }
-};
-
-#define IRQ_MASK 243
-static const struct {
-   unsigned char new_irq;
-   unsigned char old_irq;
-} irq_table[] = {
-   {  3,  3 },
-   {  4,  4 },
-   { 10, 10 },
-   { 14, 15 }
-};
-
-static short smc_mca_adapter_ids[] __initdata = {
-	0x61c8,
-	0x61c9,
-	0x6fc0,
-	0x6fc1,
-	0x6fc2,
-	0xefd4,
-	0xefd5,
-	0xefe5,
-	0x0000
-};
-
-static char *smc_mca_adapter_names[] __initdata = {
-	"SMC Ethercard PLUS Elite/A BNC/AUI (WD8013EP/A)",
-	"SMC Ethercard PLUS Elite/A UTP/AUI (WD8013WP/A)",
-	"WD Ethercard PLUS/A (WD8003E/A or WD8003ET/A)",
-	"WD Starcard PLUS/A (WD8003ST/A)",
-	"WD Ethercard PLUS 10T/A (WD8003W/A)",
-	"IBM PS/2 Adapter/A for Ethernet UTP/AUI (WD8013WP/A)",
-	"IBM PS/2 Adapter/A for Ethernet BNC/AUI (WD8013EP/A)",
-	"IBM PS/2 Adapter/A for Ethernet",
-	NULL
-};
-
-static int ultra_found = 0;
-
-
-static const struct net_device_ops ultramca_netdev_ops = {
-	.ndo_open		= ultramca_open,
-	.ndo_stop		= ultramca_close_card,
-
-	.ndo_start_xmit		= ei_start_xmit,
-	.ndo_tx_timeout		= ei_tx_timeout,
-	.ndo_get_stats		= ei_get_stats,
-	.ndo_set_rx_mode	= ei_set_multicast_list,
-	.ndo_validate_addr	= eth_validate_addr,
-	.ndo_set_mac_address 	= eth_mac_addr,
-	.ndo_change_mtu		= eth_change_mtu,
-#ifdef CONFIG_NET_POLL_CONTROLLER
-	.ndo_poll_controller 	= ei_poll,
-#endif
-};
-
-static int __init ultramca_probe(struct device *gen_dev)
-{
-	unsigned short ioaddr;
-	struct net_device *dev;
-	unsigned char reg4, num_pages;
-	struct mca_device *mca_dev = to_mca_device(gen_dev);
-	char slot = mca_dev->slot;
-	unsigned char pos2 = 0xff, pos3 = 0xff, pos4 = 0xff, pos5 = 0xff;
-	int i, rc;
-	int adapter = mca_dev->index;
-	int tbase = 0;
-	int tirq = 0;
-	int base_addr = ultra_io[ultra_found];
-	int irq = ultra_irq[ultra_found];
-
-	if (base_addr || irq) {
-		printk(KERN_INFO "Probing for SMC MCA adapter");
-		if (base_addr) {
-			printk(KERN_INFO " at I/O address 0x%04x%c",
-			       base_addr, irq ? ' ' : '\n');
-		}
-		if (irq) {
-			printk(KERN_INFO "using irq %d\n", irq);
-		}
-	}
-
-	tirq = 0;
-	tbase = 0;
-
-	/* If we're trying to match a specificied irq or io address,
-	 * we'll reject the adapter found unless it's the one we're
-	 * looking for */
-
-	pos2 = mca_device_read_stored_pos(mca_dev, 2); /* io_addr */
-	pos3 = mca_device_read_stored_pos(mca_dev, 3); /* shared mem */
-	pos4 = mca_device_read_stored_pos(mca_dev, 4); /* ROM bios addr range */
-	pos5 = mca_device_read_stored_pos(mca_dev, 5); /* irq, media and RIPL */
-
-	/* Test the following conditions:
-	 * - If an irq parameter is supplied, compare it
-	 *   with the irq of the adapter we found
-	 * - If a base_addr paramater is given, compare it
-	 *   with the base_addr of the adapter we found
-	 * - Check that the irq and the base_addr of the
-	 *   adapter we found is not already in use by
-	 *   this driver
-	 */
-
-	switch (mca_dev->index) {
-	case _61c8_SMC_Ethercard_PLUS_Elite_A_BNC_AUI_WD8013EP_A:
-	case _61c9_SMC_Ethercard_PLUS_Elite_A_UTP_AUI_WD8013EP_A:
-	case _efd4_IBM_PS2_Adapter_A_for_Ethernet_UTP_AUI_WD8013WP_A:
-	case _efd5_IBM_PS2_Adapter_A_for_Ethernet_BNC_AUI_WD8013WP_A:
-		{
-			tbase = addr_table[(pos2 & 0xf0) >> 4].base_addr;
-			tirq  = irq_table[(pos5 & 0xc) >> 2].new_irq;
-			break;
-		}
-	case _6fc0_WD_Ethercard_PLUS_A_WD8003E_A_OR_WD8003ET_A:
-	case _6fc1_WD_Starcard_PLUS_A_WD8003ST_A:
-	case _6fc2_WD_Ethercard_PLUS_10T_A_WD8003W_A:
-	case _efe5_IBM_PS2_Adapter_A_for_Ethernet:
-		{
-			tbase = ((pos2 & 0x0fe) * 0x10);
-			tirq  = irq_table[(pos5 & 3)].old_irq;
-			break;
-		}
-	}
-
-	if(!tirq || !tbase ||
-	   (irq && irq != tirq) ||
-	   (base_addr && tbase != base_addr))
-		/* FIXME: we're trying to force the ordering of the
-		 * devices here, there should be a way of getting this
-		 * to happen */
-		return -ENXIO;
-
-        /* Adapter found. */
-	dev  = alloc_ei_netdev();
-	if(!dev)
-		return -ENODEV;
-
-	SET_NETDEV_DEV(dev, gen_dev);
-	mca_device_set_name(mca_dev, smc_mca_adapter_names[adapter]);
-	mca_device_set_claim(mca_dev, 1);
-
-	printk(KERN_INFO "smc_mca: %s found in slot %d\n",
-		       smc_mca_adapter_names[adapter], slot + 1);
-
-	ultra_found++;
-
-	dev->base_addr = ioaddr = mca_device_transform_ioport(mca_dev, tbase);
-	dev->irq       = mca_device_transform_irq(mca_dev, tirq);
-	dev->mem_start = 0;
-	num_pages      = 40;
-
-	switch (adapter) {	/* card-# in const array above [hs] */
-		case _61c8_SMC_Ethercard_PLUS_Elite_A_BNC_AUI_WD8013EP_A:
-		case _61c9_SMC_Ethercard_PLUS_Elite_A_UTP_AUI_WD8013EP_A:
-		{
-			for (i = 0; i < 16; i++) { /* taking 16 counts
-						    * up to 15 [hs] */
-				if (mem_table[i].mem_index == (pos3 & ~MEM_MASK)) {
-					dev->mem_start = (unsigned long)
-					  mca_device_transform_memory(mca_dev, (void *)mem_table[i].mem_start);
-					num_pages = mem_table[i].num_pages;
-				}
-			}
-			break;
-		}
-		case _6fc0_WD_Ethercard_PLUS_A_WD8003E_A_OR_WD8003ET_A:
-		case _6fc1_WD_Starcard_PLUS_A_WD8003ST_A:
-		case _6fc2_WD_Ethercard_PLUS_10T_A_WD8003W_A:
-		case _efe5_IBM_PS2_Adapter_A_for_Ethernet:
-		{
-			dev->mem_start = (unsigned long)
-			  mca_device_transform_memory(mca_dev, (void *)((pos3 & 0xfc) * 0x1000));
-			num_pages = 0x40;
-			break;
-		}
-		case _efd4_IBM_PS2_Adapter_A_for_Ethernet_UTP_AUI_WD8013WP_A:
-		case _efd5_IBM_PS2_Adapter_A_for_Ethernet_BNC_AUI_WD8013WP_A:
-		{
-			/* courtesy of gamera@quartz.ocn.ne.jp, pos3 indicates
-			 * the index of the 0x2000 step.
-			 * beware different number of pages [hs]
-			 */
-			dev->mem_start = (unsigned long)
-			  mca_device_transform_memory(mca_dev, (void *)(0xc0000 + (0x2000 * (pos3 & 0xf))));
-			num_pages = 0x20 + (2 * (pos3 & 0x10));
-			break;
-		}
-	}
-
-	/* sanity check, shouldn't happen */
-	if (dev->mem_start == 0) {
-		rc = -ENODEV;
-		goto err_unclaim;
-	}
-
-	if (!request_region(ioaddr, ULTRA_IO_EXTENT, DRV_NAME)) {
-		rc = -ENODEV;
-		goto err_unclaim;
-	}
-
-	reg4 = inb(ioaddr + 4) & 0x7f;
-	outb(reg4, ioaddr + 4);
-
-	for (i = 0; i < 6; i++)
-		dev->dev_addr[i] = inb(ioaddr + 8 + i);
-
-	printk(KERN_INFO "smc_mca[%d]: Parameters: %#3x, %pM",
-	       slot + 1, ioaddr, dev->dev_addr);
-
-	/* Switch from the station address to the alternate register set
-	 * and read the useful registers there.
-	 */
-
-	outb(0x80 | reg4, ioaddr + 4);
-
-	/* Enable FINE16 mode to avoid BIOS ROM width mismatches @ reboot.
-	 */
-
-	outb(0x80 | inb(ioaddr + 0x0c), ioaddr + 0x0c);
-
-	/* Switch back to the station address register set so that
-	 * the MS-DOS driver can find the card after a warm boot.
-	 */
-
-	outb(reg4, ioaddr + 4);
-
-	dev_set_drvdata(gen_dev, dev);
-
-	/* The 8390 isn't at the base address, so fake the offset
-	 */
-
-	dev->base_addr = ioaddr + ULTRA_NIC_OFFSET;
-
-	ei_status.name = "SMC Ultra MCA";
-	ei_status.word16 = 1;
-	ei_status.tx_start_page = START_PG;
-	ei_status.rx_start_page = START_PG + TX_PAGES;
-	ei_status.stop_page = num_pages;
-
-	ei_status.mem = ioremap(dev->mem_start, (ei_status.stop_page - START_PG) * 256);
-	if (!ei_status.mem) {
-		rc = -ENOMEM;
-		goto err_release_region;
-	}
-
-	dev->mem_end = dev->mem_start + (ei_status.stop_page - START_PG) * 256;
-
-	printk(", IRQ %d memory %#lx-%#lx.\n",
-	dev->irq, dev->mem_start, dev->mem_end - 1);
-
-	ei_status.reset_8390 = &ultramca_reset_8390;
-	ei_status.block_input = &ultramca_block_input;
-	ei_status.block_output = &ultramca_block_output;
-	ei_status.get_8390_hdr = &ultramca_get_8390_hdr;
-
-	ei_status.priv = slot;
-
-	dev->netdev_ops = &ultramca_netdev_ops;
-
-	NS8390_init(dev, 0);
-
-	rc = register_netdev(dev);
-	if (rc)
-		goto err_unmap;
-
-	return 0;
-
-err_unmap:
-	iounmap(ei_status.mem);
-err_release_region:
-	release_region(ioaddr, ULTRA_IO_EXTENT);
-err_unclaim:
-	mca_device_set_claim(mca_dev, 0);
-	free_netdev(dev);
-	return rc;
-}
-
-static int ultramca_open(struct net_device *dev)
-{
-	int ioaddr = dev->base_addr - ULTRA_NIC_OFFSET; /* ASIC addr */
-	int retval;
-
-	if ((retval = request_irq(dev->irq, ei_interrupt, 0, dev->name, dev)))
-		return retval;
-
-	outb(ULTRA_MEMENB, ioaddr); /* Enable memory */
-	outb(0x80, ioaddr + 5);     /* ??? */
-	outb(0x01, ioaddr + 6);     /* Enable interrupts and memory. */
-	outb(0x04, ioaddr + 5);     /* ??? */
-
-	/* Set the early receive warning level in window 0 high enough not
-	 * to receive ERW interrupts.
-	 */
-
-	/* outb_p(E8390_NODMA + E8390_PAGE0, dev->base_addr);
-	 * outb(0xff, dev->base_addr + EN0_ERWCNT);
-	 */
-
-	ei_open(dev);
-	return 0;
-}
-
-static void ultramca_reset_8390(struct net_device *dev)
-{
-	int ioaddr = dev->base_addr - ULTRA_NIC_OFFSET; /* ASIC addr */
-
-	outb(ULTRA_RESET, ioaddr);
-	if (ei_debug > 1)
-		printk("resetting Ultra, t=%ld...", jiffies);
-	ei_status.txing = 0;
-
-	outb(0x80, ioaddr + 5);     /* ??? */
-	outb(0x01, ioaddr + 6);     /* Enable interrupts and memory. */
-
-	if (ei_debug > 1)
-		printk("reset done\n");
-}
-
-/* Grab the 8390 specific header. Similar to the block_input routine, but
- * we don't need to be concerned with ring wrap as the header will be at
- * the start of a page, so we optimize accordingly.
- */
-
-static void ultramca_get_8390_hdr(struct net_device *dev, struct e8390_pkt_hdr *hdr, int ring_page)
-{
-	void __iomem *hdr_start = ei_status.mem + ((ring_page - START_PG) << 8);
-
-#ifdef notdef
-	/* Officially this is what we are doing, but the readl() is faster */
-	memcpy_fromio(hdr, hdr_start, sizeof(struct e8390_pkt_hdr));
-#else
-	((unsigned int*)hdr)[0] = readl(hdr_start);
-#endif
-}
-
-/* Block input and output are easy on shared memory ethercards, the only
- * complication is when the ring buffer wraps.
- */
-
-static void ultramca_block_input(struct net_device *dev, int count, struct sk_buff *skb, int ring_offset)
-{
-	void __iomem *xfer_start = ei_status.mem + ring_offset - START_PG * 256;
-
-	if (ring_offset + count > ei_status.stop_page * 256) {
-		/* We must wrap the input move. */
-		int semi_count = ei_status.stop_page * 256 - ring_offset;
-		memcpy_fromio(skb->data, xfer_start, semi_count);
-		count -= semi_count;
-		memcpy_fromio(skb->data + semi_count, ei_status.mem + TX_PAGES * 256, count);
-	} else {
-		memcpy_fromio(skb->data, xfer_start, count);
-	}
-
-}
-
-static void ultramca_block_output(struct net_device *dev, int count, const unsigned char *buf,
-                int start_page)
-{
-	void __iomem *shmem = ei_status.mem + ((start_page - START_PG) << 8);
-
-	memcpy_toio(shmem, buf, count);
-}
-
-static int ultramca_close_card(struct net_device *dev)
-{
-	int ioaddr = dev->base_addr - ULTRA_NIC_OFFSET; /* ASIC addr */
-
-	netif_stop_queue(dev);
-
-	if (ei_debug > 1)
-		printk("%s: Shutting down ethercard.\n", dev->name);
-
-	outb(0x00, ioaddr + 6);     /* Disable interrupts. */
-	free_irq(dev->irq, dev);
-
-	NS8390_init(dev, 0);
-	/* We should someday disable shared memory and change to 8-bit mode
-         * "just in case"...
-	 */
-
-	return 0;
-}
-
-static int ultramca_remove(struct device *gen_dev)
-{
-	struct mca_device *mca_dev = to_mca_device(gen_dev);
-	struct net_device *dev = dev_get_drvdata(gen_dev);
-
-	if (dev) {
-		/* NB: ultra_close_card() does free_irq */
-		int ioaddr = dev->base_addr - ULTRA_NIC_OFFSET;
-
-		unregister_netdev(dev);
-		mca_device_set_claim(mca_dev, 0);
-		release_region(ioaddr, ULTRA_IO_EXTENT);
-		iounmap(ei_status.mem);
-		free_netdev(dev);
-	}
-	return 0;
-}
-
-
-static struct mca_driver ultra_driver = {
-	.id_table = smc_mca_adapter_ids,
-	.driver = {
-		.name = "smc-mca",
-		.bus = &mca_bus_type,
-		.probe = ultramca_probe,
-		.remove = ultramca_remove,
-	}
-};
-
-static int __init ultramca_init_module(void)
-{
-	if(!MCA_bus)
-		return -ENXIO;
-
-	mca_register_driver(&ultra_driver);
-
-	return ultra_found ? 0 : -ENXIO;
-}
-
-static void __exit ultramca_cleanup_module(void)
-{
-	mca_unregister_driver(&ultra_driver);
-}
-module_init(ultramca_init_module);
-module_exit(ultramca_cleanup_module);
-
diff --git a/drivers/net/ethernet/Kconfig b/drivers/net/ethernet/Kconfig
index c63a64cb608..a11af5cc484 100644
--- a/drivers/net/ethernet/Kconfig
+++ b/drivers/net/ethernet/Kconfig
@@ -174,6 +174,7 @@ source "drivers/net/ethernet/tile/Kconfig"
 source "drivers/net/ethernet/toshiba/Kconfig"
 source "drivers/net/ethernet/tundra/Kconfig"
 source "drivers/net/ethernet/via/Kconfig"
+source "drivers/net/ethernet/wiznet/Kconfig"
 source "drivers/net/ethernet/xilinx/Kconfig"
 source "drivers/net/ethernet/xircom/Kconfig"
 
diff --git a/drivers/net/ethernet/Makefile b/drivers/net/ethernet/Makefile
index 9676a5109d9..878ad32b93f 100644
--- a/drivers/net/ethernet/Makefile
+++ b/drivers/net/ethernet/Makefile
@@ -73,5 +73,6 @@ obj-$(CONFIG_TILE_NET) += tile/
 obj-$(CONFIG_NET_VENDOR_TOSHIBA) += toshiba/
 obj-$(CONFIG_NET_VENDOR_TUNDRA) += tundra/
 obj-$(CONFIG_NET_VENDOR_VIA) += via/
+obj-$(CONFIG_NET_VENDOR_WIZNET) += wiznet/
 obj-$(CONFIG_NET_VENDOR_XILINX) += xilinx/
 obj-$(CONFIG_NET_VENDOR_XIRCOM) += xircom/
diff --git a/drivers/net/ethernet/adaptec/starfire.c b/drivers/net/ethernet/adaptec/starfire.c
index d896816512c..d920a529ba2 100644
--- a/drivers/net/ethernet/adaptec/starfire.c
+++ b/drivers/net/ethernet/adaptec/starfire.c
@@ -114,15 +114,6 @@ static int rx_copybreak /* = 0 */;
 #define DMA_BURST_SIZE 128
 #endif
 
-/* Used to pass the media type, etc.
-   Both 'options[]' and 'full_duplex[]' exist for driver interoperability.
-   The media type is usually passed in 'options[]'.
-   These variables are deprecated, use ethtool instead. -Ion
-*/
-#define MAX_UNITS 8		/* More are supported, limit only on options */
-static int options[MAX_UNITS] = {0, };
-static int full_duplex[MAX_UNITS] = {0, };
-
 /* Operational parameters that are set at compile time. */
 
 /* The "native" ring sizes are either 256 or 2048.
@@ -192,8 +183,6 @@ module_param(debug, int, 0);
 module_param(rx_copybreak, int, 0);
 module_param(intr_latency, int, 0);
 module_param(small_frames, int, 0);
-module_param_array(options, int, NULL, 0);
-module_param_array(full_duplex, int, NULL, 0);
 module_param(enable_hw_cksum, int, 0);
 MODULE_PARM_DESC(max_interrupt_work, "Maximum events handled per interrupt");
 MODULE_PARM_DESC(mtu, "MTU (all boards)");
@@ -201,8 +190,6 @@ MODULE_PARM_DESC(debug, "Debug level (0-6)");
 MODULE_PARM_DESC(rx_copybreak, "Copy breakpoint for copy-only-tiny-frames");
 MODULE_PARM_DESC(intr_latency, "Maximum interrupt latency, in microseconds");
 MODULE_PARM_DESC(small_frames, "Maximum size of receive frames that bypass interrupt latency (0,64,128,256,512)");
-MODULE_PARM_DESC(options, "Deprecated: Bits 0-3: media type, bit 17: full duplex");
-MODULE_PARM_DESC(full_duplex, "Deprecated: Forced full-duplex setting (0/1)");
 MODULE_PARM_DESC(enable_hw_cksum, "Enable/disable hardware cksum support (0/1)");
 
 /*
@@ -657,10 +644,10 @@ static const struct net_device_ops netdev_ops = {
 static int __devinit starfire_init_one(struct pci_dev *pdev,
 				       const struct pci_device_id *ent)
 {
+	struct device *d = &pdev->dev;
 	struct netdev_private *np;
-	int i, irq, option, chip_idx = ent->driver_data;
+	int i, irq, chip_idx = ent->driver_data;
 	struct net_device *dev;
-	static int card_idx = -1;
 	long ioaddr;
 	void __iomem *base;
 	int drv_flags, io_size;
@@ -673,15 +660,13 @@ static int __devinit starfire_init_one(struct pci_dev *pdev,
 		printk(version);
 #endif
 
-	card_idx++;
-
 	if (pci_enable_device (pdev))
 		return -EIO;
 
 	ioaddr = pci_resource_start(pdev, 0);
 	io_size = pci_resource_len(pdev, 0);
 	if (!ioaddr || ((pci_resource_flags(pdev, 0) & IORESOURCE_MEM) == 0)) {
-		printk(KERN_ERR DRV_NAME " %d: no PCI MEM resources, aborting\n", card_idx);
+		dev_err(d, "no PCI MEM resources, aborting\n");
 		return -ENODEV;
 	}
 
@@ -694,14 +679,14 @@ static int __devinit starfire_init_one(struct pci_dev *pdev,
 	irq = pdev->irq;
 
 	if (pci_request_regions (pdev, DRV_NAME)) {
-		printk(KERN_ERR DRV_NAME " %d: cannot reserve PCI resources, aborting\n", card_idx);
+		dev_err(d, "cannot reserve PCI resources, aborting\n");
 		goto err_out_free_netdev;
 	}
 
 	base = ioremap(ioaddr, io_size);
 	if (!base) {
-		printk(KERN_ERR DRV_NAME " %d: cannot remap %#x @ %#lx, aborting\n",
-			card_idx, io_size, ioaddr);
+		dev_err(d, "cannot remap %#x @ %#lx, aborting\n",
+			io_size, ioaddr);
 		goto err_out_free_res;
 	}
 
@@ -753,9 +738,6 @@ static int __devinit starfire_init_one(struct pci_dev *pdev,
 	/* wait a little longer */
 	udelay(1000);
 
-	dev->base_addr = (unsigned long)base;
-	dev->irq = irq;
-
 	np = netdev_priv(dev);
 	np->dev = dev;
 	np->base = base;
@@ -772,21 +754,6 @@ static int __devinit starfire_init_one(struct pci_dev *pdev,
 
 	drv_flags = netdrv_tbl[chip_idx].drv_flags;
 
-	option = card_idx < MAX_UNITS ? options[card_idx] : 0;
-	if (dev->mem_start)
-		option = dev->mem_start;
-
-	/* The lower four bits are the media type. */
-	if (option & 0x200)
-		np->mii_if.full_duplex = 1;
-
-	if (card_idx < MAX_UNITS && full_duplex[card_idx] > 0)
-		np->mii_if.full_duplex = 1;
-
-	if (np->mii_if.full_duplex)
-		np->mii_if.force_media = 1;
-	else
-		np->mii_if.force_media = 0;
 	np->speed100 = 1;
 
 	/* timer resolution is 128 * 0.8us */
@@ -909,13 +876,14 @@ static int netdev_open(struct net_device *dev)
 	const __be32 *fw_rx_data, *fw_tx_data;
 	struct netdev_private *np = netdev_priv(dev);
 	void __iomem *ioaddr = np->base;
+	const int irq = np->pci_dev->irq;
 	int i, retval;
 	size_t tx_size, rx_size;
 	size_t tx_done_q_size, rx_done_q_size, tx_ring_size, rx_ring_size;
 
 	/* Do we ever need to reset the chip??? */
 
-	retval = request_irq(dev->irq, intr_handler, IRQF_SHARED, dev->name, dev);
+	retval = request_irq(irq, intr_handler, IRQF_SHARED, dev->name, dev);
 	if (retval)
 		return retval;
 
@@ -924,7 +892,7 @@ static int netdev_open(struct net_device *dev)
 	writel(1, ioaddr + PCIDeviceConfig);
 	if (debug > 1)
 		printk(KERN_DEBUG "%s: netdev_open() irq %d.\n",
-		       dev->name, dev->irq);
+		       dev->name, irq);
 
 	/* Allocate the various queues. */
 	if (!np->queue_mem) {
@@ -935,7 +903,7 @@ static int netdev_open(struct net_device *dev)
 		np->queue_mem_size = tx_done_q_size + rx_done_q_size + tx_ring_size + rx_ring_size;
 		np->queue_mem = pci_alloc_consistent(np->pci_dev, np->queue_mem_size, &np->queue_mem_dma);
 		if (np->queue_mem == NULL) {
-			free_irq(dev->irq, dev);
+			free_irq(irq, dev);
 			return -ENOMEM;
 		}
 
@@ -1962,7 +1930,7 @@ static int netdev_close(struct net_device *dev)
 		}
 	}
 
-	free_irq(dev->irq, dev);
+	free_irq(np->pci_dev->irq, dev);
 
 	/* Free all the skbuffs in the Rx queue. */
 	for (i = 0; i < RX_RING_SIZE; i++) {
diff --git a/drivers/net/ethernet/adi/bfin_mac.c b/drivers/net/ethernet/adi/bfin_mac.c
index ab4daeccdf9..f816426e108 100644
--- a/drivers/net/ethernet/adi/bfin_mac.c
+++ b/drivers/net/ethernet/adi/bfin_mac.c
@@ -548,6 +548,25 @@ static int bfin_mac_ethtool_setwol(struct net_device *dev,
 	return 0;
 }
 
+static int bfin_mac_ethtool_get_ts_info(struct net_device *dev,
+	struct ethtool_ts_info *info)
+{
+	info->so_timestamping =
+		SOF_TIMESTAMPING_TX_HARDWARE |
+		SOF_TIMESTAMPING_RX_HARDWARE |
+		SOF_TIMESTAMPING_SYS_HARDWARE;
+	info->phc_index = -1;
+	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_V2_L2_EVENT) |
+		(1 << HWTSTAMP_FILTER_PTP_V2_L4_EVENT);
+	return 0;
+}
+
 static const struct ethtool_ops bfin_mac_ethtool_ops = {
 	.get_settings = bfin_mac_ethtool_getsettings,
 	.set_settings = bfin_mac_ethtool_setsettings,
@@ -555,6 +574,7 @@ static const struct ethtool_ops bfin_mac_ethtool_ops = {
 	.get_drvinfo = bfin_mac_ethtool_getdrvinfo,
 	.get_wol = bfin_mac_ethtool_getwol,
 	.set_wol = bfin_mac_ethtool_setwol,
+	.get_ts_info = bfin_mac_ethtool_get_ts_info,
 };
 
 /**************************************************************************/
diff --git a/drivers/net/ethernet/amd/ariadne.c b/drivers/net/ethernet/amd/ariadne.c
index f4c228e4d76..f2958df9a1e 100644
--- a/drivers/net/ethernet/amd/ariadne.c
+++ b/drivers/net/ethernet/amd/ariadne.c
@@ -213,10 +213,10 @@ static int ariadne_rx(struct net_device *dev)
 						(const void *)priv->rx_buff[entry],
 						pkt_len);
 			skb->protocol = eth_type_trans(skb, dev);
-			netdev_dbg(dev, "RX pkt type 0x%04x from %pM to %pM data 0x%08x len %d\n",
+			netdev_dbg(dev, "RX pkt type 0x%04x from %pM to %pM data %p len %u\n",
 				   ((u_short *)skb->data)[6],
 				   skb->data + 6, skb->data,
-				   (int)skb->data, (int)skb->len);
+				   skb->data, skb->len);
 
 			netif_rx(skb);
 			dev->stats.rx_packets++;
@@ -566,10 +566,10 @@ static netdev_tx_t ariadne_start_xmit(struct sk_buff *skb,
 
 	/* Fill in a Tx ring entry */
 
-	netdev_dbg(dev, "TX pkt type 0x%04x from %pM to %pM data 0x%08x len %d\n",
+	netdev_dbg(dev, "TX pkt type 0x%04x from %pM to %pM data %p len %u\n",
 		   ((u_short *)skb->data)[6],
 		   skb->data + 6, skb->data,
-		   (int)skb->data, (int)skb->len);
+		   skb->data, skb->len);
 
 	local_irq_save(flags);
 
diff --git a/drivers/net/ethernet/amd/atarilance.c b/drivers/net/ethernet/amd/atarilance.c
index 70ed79c4624..84219df72f5 100644
--- a/drivers/net/ethernet/amd/atarilance.c
+++ b/drivers/net/ethernet/amd/atarilance.c
@@ -558,21 +558,18 @@ static unsigned long __init lance_probe1( struct net_device *dev,
 			printk( "Lance: request for irq %d failed\n", IRQ_AUTO_5 );
 			return 0;
 		}
-		dev->irq = (unsigned short)IRQ_AUTO_5;
+		dev->irq = IRQ_AUTO_5;
 	}
 	else {
-		/* For VME-RieblCards, request a free VME int;
-		 * (This must be unsigned long, since dev->irq is short and the
-		 * IRQ_MACHSPEC bit would be cut off...)
-		 */
-		unsigned long irq = atari_register_vme_int();
+		/* For VME-RieblCards, request a free VME int */
+		unsigned int irq = atari_register_vme_int();
 		if (!irq) {
 			printk( "Lance: request for VME interrupt failed\n" );
 			return 0;
 		}
 		if (request_irq(irq, lance_interrupt, IRQ_TYPE_PRIO,
 		            "Riebl-VME Ethernet", dev)) {
-			printk( "Lance: request for irq %ld failed\n", irq );
+			printk( "Lance: request for irq %u failed\n", irq );
 			return 0;
 		}
 		dev->irq = irq;
diff --git a/drivers/net/ethernet/amd/depca.c b/drivers/net/ethernet/amd/depca.c
index 86dd95766a6..c771de71612 100644
--- a/drivers/net/ethernet/amd/depca.c
+++ b/drivers/net/ethernet/amd/depca.c
@@ -155,23 +155,10 @@
     2 depca's in a PC).
 
     ************************************************************************
-    Support for MCA EtherWORKS cards added 11-3-98.
+    Support for MCA EtherWORKS cards added 11-3-98. (MCA since deleted)
     Verified to work with up to 2 DE212 cards in a system (although not
       fully stress-tested).
 
-    Currently known bugs/limitations:
-
-    Note:  with the MCA stuff as a module, it trusts the MCA configuration,
-           not the command line for IRQ and memory address.  You can
-           specify them if you want, but it will throw your values out.
-           You still have to pass the IO address it was configured as
-           though.
-
-    ************************************************************************
-    TO DO:
-    ------
-
-
     Revision History
     ----------------
 
@@ -261,10 +248,6 @@
 #include <asm/io.h>
 #include <asm/dma.h>
 
-#ifdef CONFIG_MCA
-#include <linux/mca.h>
-#endif
-
 #ifdef CONFIG_EISA
 #include <linux/eisa.h>
 #endif
@@ -360,44 +343,6 @@ static struct eisa_driver depca_eisa_driver = {
 };
 #endif
 
-#ifdef CONFIG_MCA
-/*
-** Adapter ID for the MCA EtherWORKS DE210/212 adapter
-*/
-#define DE210_ID 0x628d
-#define DE212_ID 0x6def
-
-static short depca_mca_adapter_ids[] = {
-	DE210_ID,
-	DE212_ID,
-	0x0000
-};
-
-static char *depca_mca_adapter_name[] = {
-	"DEC EtherWORKS MC Adapter (DE210)",
-	"DEC EtherWORKS MC Adapter (DE212)",
-	NULL
-};
-
-static enum depca_type depca_mca_adapter_type[] = {
-	de210,
-	de212,
-	0
-};
-
-static int depca_mca_probe (struct device *);
-
-static struct mca_driver depca_mca_driver = {
-	.id_table = depca_mca_adapter_ids,
-	.driver   = {
-		.name   = depca_string,
-		.bus    = &mca_bus_type,
-		.probe  = depca_mca_probe,
-		.remove = __devexit_p(depca_device_remove),
-	},
-};
-#endif
-
 static int depca_isa_probe (struct platform_device *);
 
 static int __devexit depca_isa_remove(struct platform_device *pdev)
@@ -464,8 +409,7 @@ struct depca_private {
 	char adapter_name[DEPCA_STRLEN];	/* /proc/ioports string                  */
 	enum depca_type adapter;		/* Adapter type */
 	enum {
-                DEPCA_BUS_MCA = 1,
-                DEPCA_BUS_ISA,
+                DEPCA_BUS_ISA = 1,
                 DEPCA_BUS_EISA,
         } depca_bus;	        /* type of bus */
 	struct depca_init init_block;	/* Shadow Initialization block            */
@@ -624,12 +568,6 @@ static int __init depca_hw_init (struct net_device *dev, struct device *device)
 	       dev_name(device), depca_signature[lp->adapter], ioaddr);
 
 	switch (lp->depca_bus) {
-#ifdef CONFIG_MCA
-	case DEPCA_BUS_MCA:
-		printk(" (MCA slot %d)", to_mca_device(device)->slot + 1);
-		break;
-#endif
-
 #ifdef CONFIG_EISA
 	case DEPCA_BUS_EISA:
 		printk(" (EISA slot %d)", to_eisa_device(device)->slot);
@@ -661,10 +599,7 @@ static int __init depca_hw_init (struct net_device *dev, struct device *device)
 	if (nicsr & BUF) {
 		nicsr &= ~BS;	/* DEPCA RAM in top 32k */
 		netRAM -= 32;
-
-		/* Only EISA/ISA needs start address to be re-computed */
-		if (lp->depca_bus != DEPCA_BUS_MCA)
-			mem_start += 0x8000;
+		mem_start += 0x8000;
 	}
 
 	if ((mem_len = (NUM_RX_DESC * (sizeof(struct depca_rx_desc) + RX_BUFF_SZ) + NUM_TX_DESC * (sizeof(struct depca_tx_desc) + TX_BUFF_SZ) + sizeof(struct depca_init)))
@@ -1079,7 +1014,8 @@ static int depca_rx(struct net_device *dev)
 						} else {
 							lp->pktStats.multicast++;
 						}
-					} else if (compare_ether_addr(buf, dev->dev_addr) == 0) {
+					} else if (ether_addr_equal(buf,
+								    dev->dev_addr)) {
 						lp->pktStats.unicast++;
 					}
 
@@ -1324,130 +1260,6 @@ static int __init depca_common_init (u_long ioaddr, struct net_device **devp)
 	return status;
 }
 
-#ifdef CONFIG_MCA
-/*
-** Microchannel bus I/O device probe
-*/
-static int __init depca_mca_probe(struct device *device)
-{
-	unsigned char pos[2];
-	unsigned char where;
-	unsigned long iobase, mem_start;
-	int irq, err;
-	struct mca_device *mdev = to_mca_device (device);
-	struct net_device *dev;
-	struct depca_private *lp;
-
-	/*
-	** Search for the adapter.  If an address has been given, search
-	** specifically for the card at that address.  Otherwise find the
-	** first card in the system.
-	*/
-
-	pos[0] = mca_device_read_stored_pos(mdev, 2);
-	pos[1] = mca_device_read_stored_pos(mdev, 3);
-
-	/*
-	** IO of card is handled by bits 1 and 2 of pos0.
-	**
-	**    bit2 bit1    IO
-	**       0    0    0x2c00
-	**       0    1    0x2c10
-	**       1    0    0x2c20
-	**       1    1    0x2c30
-	*/
-	where = (pos[0] & 6) >> 1;
-	iobase = 0x2c00 + (0x10 * where);
-
-	/*
-	** Found the adapter we were looking for. Now start setting it up.
-	**
-	** First work on decoding the IRQ.  It's stored in the lower 4 bits
-	** of pos1.  Bits are as follows (from the ADF file):
-	**
-	**      Bits
-	**   3   2   1   0    IRQ
-	**   --------------------
-	**   0   0   1   0     5
-	**   0   0   0   1     9
-	**   0   1   0   0    10
-	**   1   0   0   0    11
-	*/
-	where = pos[1] & 0x0f;
-	switch (where) {
-	case 1:
-		irq = 9;
-		break;
-	case 2:
-		irq = 5;
-		break;
-	case 4:
-		irq = 10;
-		break;
-	case 8:
-		irq = 11;
-		break;
-	default:
-		printk("%s: mca_probe IRQ error.  You should never get here (%d).\n", mdev->name, where);
-		return -EINVAL;
-	}
-
-	/*
-	** Shared memory address of adapter is stored in bits 3-5 of pos0.
-	** They are mapped as follows:
-	**
-	**    Bit
-	**   5  4  3       Memory Addresses
-	**   0  0  0       C0000-CFFFF (64K)
-	**   1  0  0       C8000-CFFFF (32K)
-	**   0  0  1       D0000-DFFFF (64K)
-	**   1  0  1       D8000-DFFFF (32K)
-	**   0  1  0       E0000-EFFFF (64K)
-	**   1  1  0       E8000-EFFFF (32K)
-	*/
-	where = (pos[0] & 0x18) >> 3;
-	mem_start = 0xc0000 + (where * 0x10000);
-	if (pos[0] & 0x20) {
-		mem_start += 0x8000;
-	}
-
-	/* claim the slot */
-	strncpy(mdev->name, depca_mca_adapter_name[mdev->index],
-		sizeof(mdev->name));
-	mca_device_set_claim(mdev, 1);
-
-        /*
-	** Get everything allocated and initialized...  (almost just
-	** like the ISA and EISA probes)
-	*/
-	irq = mca_device_transform_irq(mdev, irq);
-	iobase = mca_device_transform_ioport(mdev, iobase);
-
-	if ((err = depca_common_init (iobase, &dev)))
-		goto out_unclaim;
-
-	dev->irq = irq;
-	dev->base_addr = iobase;
-	lp = netdev_priv(dev);
-	lp->depca_bus = DEPCA_BUS_MCA;
-	lp->adapter = depca_mca_adapter_type[mdev->index];
-	lp->mem_start = mem_start;
-
-	if ((err = depca_hw_init(dev, device)))
-		goto out_free;
-
-	return 0;
-
- out_free:
-	free_netdev (dev);
-	release_region (iobase, DEPCA_TOTAL_SIZE);
- out_unclaim:
-	mca_device_set_claim(mdev, 0);
-
-	return err;
-}
-#endif
-
 /*
 ** ISA bus I/O device probe
 */
@@ -2058,15 +1870,10 @@ static int __init depca_module_init (void)
 {
 	int err = 0;
 
-#ifdef CONFIG_MCA
-	err = mca_register_driver(&depca_mca_driver);
-	if (err)
-		goto err;
-#endif
 #ifdef CONFIG_EISA
 	err = eisa_driver_register(&depca_eisa_driver);
 	if (err)
-		goto err_mca;
+		goto err_eisa;
 #endif
 	err = platform_driver_register(&depca_isa_driver);
 	if (err)
@@ -2078,11 +1885,6 @@ static int __init depca_module_init (void)
 err_eisa:
 #ifdef CONFIG_EISA
 	eisa_driver_unregister(&depca_eisa_driver);
-err_mca:
-#endif
-#ifdef CONFIG_MCA
-	mca_unregister_driver(&depca_mca_driver);
-err:
 #endif
 	return err;
 }
@@ -2090,9 +1892,6 @@ err:
 static void __exit depca_module_exit (void)
 {
 	int i;
-#ifdef CONFIG_MCA
-        mca_unregister_driver (&depca_mca_driver);
-#endif
 #ifdef CONFIG_EISA
         eisa_driver_unregister (&depca_eisa_driver);
 #endif
diff --git a/drivers/net/ethernet/atheros/atl1c/atl1c.h b/drivers/net/ethernet/atheros/atl1c/atl1c.h
index ca70e16b6e2..b2bf324631d 100644
--- a/drivers/net/ethernet/atheros/atl1c/atl1c.h
+++ b/drivers/net/ethernet/atheros/atl1c/atl1c.h
@@ -74,8 +74,6 @@
 
 #define AT_RX_BUF_SIZE		(ETH_FRAME_LEN + VLAN_HLEN + ETH_FCS_LEN)
 #define MAX_JUMBO_FRAME_SIZE	(6*1024)
-#define MAX_TSO_FRAME_SIZE      (7*1024)
-#define MAX_TX_OFFLOAD_THRESH	(9*1024)
 
 #define AT_MAX_RECEIVE_QUEUE    4
 #define AT_DEF_RECEIVE_QUEUE	1
@@ -100,7 +98,7 @@
 #define ATL1C_ASPM_L0s_ENABLE		0x0001
 #define ATL1C_ASPM_L1_ENABLE		0x0002
 
-#define AT_REGS_LEN	(75 * sizeof(u32))
+#define AT_REGS_LEN	(74 * sizeof(u32))
 #define AT_EEPROM_LEN 	512
 
 #define ATL1C_GET_DESC(R, i, type)	(&(((type *)((R)->desc))[i]))
@@ -297,20 +295,6 @@ enum atl1c_dma_req_block {
 	atl1c_dma_req_4096 = 5
 };
 
-enum atl1c_rss_mode {
-	atl1c_rss_mode_disable = 0,
-	atl1c_rss_sig_que = 1,
-	atl1c_rss_mul_que_sig_int = 2,
-	atl1c_rss_mul_que_mul_int = 4,
-};
-
-enum atl1c_rss_type {
-	atl1c_rss_disable = 0,
-	atl1c_rss_ipv4 = 1,
-	atl1c_rss_ipv4_tcp = 2,
-	atl1c_rss_ipv6 = 4,
-	atl1c_rss_ipv6_tcp = 8
-};
 
 enum atl1c_nic_type {
 	athr_l1c = 0,
@@ -388,7 +372,6 @@ struct atl1c_hw {
 	enum atl1c_dma_order dma_order;
 	enum atl1c_dma_rcb   rcb_value;
 	enum atl1c_dma_req_block dmar_block;
-	enum atl1c_dma_req_block dmaw_block;
 
 	u16 device_id;
 	u16 vendor_id;
@@ -399,8 +382,6 @@ struct atl1c_hw {
 	u16 phy_id2;
 
 	u32 intr_mask;
-	u8 dmaw_dly_cnt;
-	u8 dmar_dly_cnt;
 
 	u8 preamble_len;
 	u16 max_frame_size;
@@ -440,10 +421,6 @@ struct atl1c_hw {
 #define ATL1C_FPGA_VERSION              0x8000
 	u16 link_cap_flags;
 #define ATL1C_LINK_CAP_1000M		0x0001
-	u16 cmb_tpd;
-	u16 cmb_rrd;
-	u16 cmb_rx_timer; /* 2us resolution */
-	u16 cmb_tx_timer;
 	u32 smb_timer;
 
 	u16 rrd_thresh; /* Threshold of number of RRD produced to trigger
@@ -451,9 +428,6 @@ struct atl1c_hw {
 	u16 tpd_thresh;
 	u8 tpd_burst;   /* Number of TPD to prefetch in cache-aligned burst. */
 	u8 rfd_burst;
-	enum atl1c_rss_type rss_type;
-	enum atl1c_rss_mode rss_mode;
-	u8 rss_hash_bits;
 	u32 base_cpu;
 	u32 indirect_tab;
 	u8 mac_addr[ETH_ALEN];
@@ -462,12 +436,12 @@ struct atl1c_hw {
 	bool phy_configured;
 	bool re_autoneg;
 	bool emi_ca;
+	bool msi_lnkpatch;	/* link patch for specific platforms */
 };
 
 /*
  * atl1c_ring_header represents a single, contiguous block of DMA space
- * mapped for the three descriptor rings (tpd, rfd, rrd) and the two
- * message blocks (cmb, smb) described below
+ * mapped for the three descriptor rings (tpd, rfd, rrd) described below
  */
 struct atl1c_ring_header {
 	void *desc;		/* virtual address */
@@ -541,16 +515,6 @@ struct atl1c_rrd_ring {
 	u16 next_to_clean;
 };
 
-struct atl1c_cmb {
-	void *cmb;
-	dma_addr_t dma;
-};
-
-struct atl1c_smb {
-	void *smb;
-	dma_addr_t dma;
-};
-
 /* board specific private data structure */
 struct atl1c_adapter {
 	struct net_device   *netdev;
@@ -586,11 +550,8 @@ struct atl1c_adapter {
 	/* All Descriptor memory */
 	struct atl1c_ring_header ring_header;
 	struct atl1c_tpd_ring tpd_ring[AT_MAX_TRANSMIT_QUEUE];
-	struct atl1c_rfd_ring rfd_ring[AT_MAX_RECEIVE_QUEUE];
-	struct atl1c_rrd_ring rrd_ring[AT_MAX_RECEIVE_QUEUE];
-	struct atl1c_cmb cmb;
-	struct atl1c_smb smb;
-	int num_rx_queues;
+	struct atl1c_rfd_ring rfd_ring;
+	struct atl1c_rrd_ring rrd_ring;
 	u32 bd_number;     /* board number;*/
 };
 
@@ -618,8 +579,14 @@ struct atl1c_adapter {
 #define AT_WRITE_REGW(a, reg, value) (\
 		writew((value), ((a)->hw_addr + reg)))
 
-#define AT_READ_REGW(a, reg) (\
-		readw((a)->hw_addr + reg))
+#define AT_READ_REGW(a, reg, pdata) do {				\
+		if (unlikely((a)->hibernate)) {				\
+			readw((a)->hw_addr + reg);			\
+			*(u16 *)pdata = readw((a)->hw_addr + reg);	\
+		} else {						\
+			*(u16 *)pdata = readw((a)->hw_addr + reg);	\
+		}							\
+	} while (0)
 
 #define AT_WRITE_REG_ARRAY(a, reg, offset, value) ( \
 		writel((value), (((a)->hw_addr + reg) + ((offset) << 2))))
diff --git a/drivers/net/ethernet/atheros/atl1c/atl1c_ethtool.c b/drivers/net/ethernet/atheros/atl1c/atl1c_ethtool.c
index 0a9326aa58b..859ea844ba0 100644
--- a/drivers/net/ethernet/atheros/atl1c/atl1c_ethtool.c
+++ b/drivers/net/ethernet/atheros/atl1c/atl1c_ethtool.c
@@ -141,8 +141,7 @@ static void atl1c_get_regs(struct net_device *netdev,
 
 	memset(p, 0, AT_REGS_LEN);
 
-	regs->version = 0;
-	AT_READ_REG(hw, REG_VPD_CAP, 		  p++);
+	regs->version = 1;
 	AT_READ_REG(hw, REG_PM_CTRL, 		  p++);
 	AT_READ_REG(hw, REG_MAC_HALF_DUPLX_CTRL,  p++);
 	AT_READ_REG(hw, REG_TWSI_CTRL, 		  p++);
@@ -154,7 +153,7 @@ static void atl1c_get_regs(struct net_device *netdev,
 	AT_READ_REG(hw, REG_LINK_CTRL, 		  p++);
 	AT_READ_REG(hw, REG_IDLE_STATUS, 	  p++);
 	AT_READ_REG(hw, REG_MDIO_CTRL, 		  p++);
-	AT_READ_REG(hw, REG_SERDES_LOCK, 	  p++);
+	AT_READ_REG(hw, REG_SERDES,		  p++);
 	AT_READ_REG(hw, REG_MAC_CTRL, 		  p++);
 	AT_READ_REG(hw, REG_MAC_IPG_IFG, 	  p++);
 	AT_READ_REG(hw, REG_MAC_STA_ADDR, 	  p++);
@@ -167,9 +166,9 @@ static void atl1c_get_regs(struct net_device *netdev,
 	AT_READ_REG(hw, REG_WOL_CTRL, 		  p++);
 
 	atl1c_read_phy_reg(hw, MII_BMCR, &phy_data);
-	regs_buff[73] =	(u32) phy_data;
+	regs_buff[AT_REGS_LEN/sizeof(u32) - 2] = (u32) phy_data;
 	atl1c_read_phy_reg(hw, MII_BMSR, &phy_data);
-	regs_buff[74] = (u32) phy_data;
+	regs_buff[AT_REGS_LEN/sizeof(u32) - 1] = (u32) phy_data;
 }
 
 static int atl1c_get_eeprom_len(struct net_device *netdev)
diff --git a/drivers/net/ethernet/atheros/atl1c/atl1c_hw.c b/drivers/net/ethernet/atheros/atl1c/atl1c_hw.c
index bd1667cbffa..ff9c73859d4 100644
--- a/drivers/net/ethernet/atheros/atl1c/atl1c_hw.c
+++ b/drivers/net/ethernet/atheros/atl1c/atl1c_hw.c
@@ -43,7 +43,7 @@ int atl1c_check_eeprom_exist(struct atl1c_hw *hw)
 	return 0;
 }
 
-void atl1c_hw_set_mac_addr(struct atl1c_hw *hw)
+void atl1c_hw_set_mac_addr(struct atl1c_hw *hw, u8 *mac_addr)
 {
 	u32 value;
 	/*
@@ -51,35 +51,48 @@ void atl1c_hw_set_mac_addr(struct atl1c_hw *hw)
 	 * 0:  6AF600DC 1: 000B
 	 * low dword
 	 */
-	value = (((u32)hw->mac_addr[2]) << 24) |
-		(((u32)hw->mac_addr[3]) << 16) |
-		(((u32)hw->mac_addr[4]) << 8)  |
-		(((u32)hw->mac_addr[5])) ;
+	value = mac_addr[2] << 24 |
+		mac_addr[3] << 16 |
+		mac_addr[4] << 8  |
+		mac_addr[5];
 	AT_WRITE_REG_ARRAY(hw, REG_MAC_STA_ADDR, 0, value);
 	/* hight dword */
-	value = (((u32)hw->mac_addr[0]) << 8) |
-		(((u32)hw->mac_addr[1])) ;
+	value = mac_addr[0] << 8 |
+		mac_addr[1];
 	AT_WRITE_REG_ARRAY(hw, REG_MAC_STA_ADDR, 1, value);
 }
 
+/* read mac address from hardware register */
+static bool atl1c_read_current_addr(struct atl1c_hw *hw, u8 *eth_addr)
+{
+	u32 addr[2];
+
+	AT_READ_REG(hw, REG_MAC_STA_ADDR, &addr[0]);
+	AT_READ_REG(hw, REG_MAC_STA_ADDR + 4, &addr[1]);
+
+	*(u32 *) &eth_addr[2] = htonl(addr[0]);
+	*(u16 *) &eth_addr[0] = htons((u16)addr[1]);
+
+	return is_valid_ether_addr(eth_addr);
+}
+
 /*
  * atl1c_get_permanent_address
  * return 0 if get valid mac address,
  */
 static int atl1c_get_permanent_address(struct atl1c_hw *hw)
 {
-	u32 addr[2];
 	u32 i;
 	u32 otp_ctrl_data;
 	u32 twsi_ctrl_data;
-	u32 ltssm_ctrl_data;
-	u32 wol_data;
-	u8  eth_addr[ETH_ALEN];
 	u16 phy_data;
 	bool raise_vol = false;
 
+	/* MAC-address from BIOS is the 1st priority */
+	if (atl1c_read_current_addr(hw, hw->perm_mac_addr))
+		return 0;
+
 	/* init */
-	addr[0] = addr[1] = 0;
 	AT_READ_REG(hw, REG_OTP_CTRL, &otp_ctrl_data);
 	if (atl1c_check_eeprom_exist(hw)) {
 		if (hw->nic_type == athr_l1c || hw->nic_type == athr_l2c) {
@@ -91,33 +104,17 @@ static int atl1c_get_permanent_address(struct atl1c_hw *hw)
 				msleep(1);
 			}
 		}
-
-		if (hw->nic_type == athr_l2c_b ||
-		    hw->nic_type == athr_l2c_b2 ||
-		    hw->nic_type == athr_l1d) {
-			atl1c_write_phy_reg(hw, MII_DBG_ADDR, 0x00);
-			if (atl1c_read_phy_reg(hw, MII_DBG_DATA, &phy_data))
-				goto out;
-			phy_data &= 0xFF7F;
-			atl1c_write_phy_reg(hw, MII_DBG_DATA, phy_data);
-
-			atl1c_write_phy_reg(hw, MII_DBG_ADDR, 0x3B);
-			if (atl1c_read_phy_reg(hw, MII_DBG_DATA, &phy_data))
-				goto out;
-			phy_data |= 0x8;
-			atl1c_write_phy_reg(hw, MII_DBG_DATA, phy_data);
+		/* raise voltage temporally for l2cb */
+		if (hw->nic_type == athr_l2c_b || hw->nic_type == athr_l2c_b2) {
+			atl1c_read_phy_dbg(hw, MIIDBG_ANACTRL, &phy_data);
+			phy_data &= ~ANACTRL_HB_EN;
+			atl1c_write_phy_dbg(hw, MIIDBG_ANACTRL, phy_data);
+			atl1c_read_phy_dbg(hw, MIIDBG_VOLT_CTRL, &phy_data);
+			phy_data |= VOLT_CTRL_SWLOWEST;
+			atl1c_write_phy_dbg(hw, MIIDBG_VOLT_CTRL, phy_data);
 			udelay(20);
 			raise_vol = true;
 		}
-		/* close open bit of ReadOnly*/
-		AT_READ_REG(hw, REG_LTSSM_ID_CTRL, &ltssm_ctrl_data);
-		ltssm_ctrl_data &= ~LTSSM_ID_EN_WRO;
-		AT_WRITE_REG(hw, REG_LTSSM_ID_CTRL, ltssm_ctrl_data);
-
-		/* clear any WOL settings */
-		AT_WRITE_REG(hw, REG_WOL_CTRL, 0);
-		AT_READ_REG(hw, REG_WOL_CTRL, &wol_data);
-
 
 		AT_READ_REG(hw, REG_TWSI_CTRL, &twsi_ctrl_data);
 		twsi_ctrl_data |= TWSI_CTRL_SW_LDSTART;
@@ -138,37 +135,18 @@ static int atl1c_get_permanent_address(struct atl1c_hw *hw)
 		msleep(1);
 	}
 	if (raise_vol) {
-		if (hw->nic_type == athr_l2c_b ||
-		    hw->nic_type == athr_l2c_b2 ||
-		    hw->nic_type == athr_l1d ||
-		    hw->nic_type == athr_l1d_2) {
-			atl1c_write_phy_reg(hw, MII_DBG_ADDR, 0x00);
-			if (atl1c_read_phy_reg(hw, MII_DBG_DATA, &phy_data))
-				goto out;
-			phy_data |= 0x80;
-			atl1c_write_phy_reg(hw, MII_DBG_DATA, phy_data);
-
-			atl1c_write_phy_reg(hw, MII_DBG_ADDR, 0x3B);
-			if (atl1c_read_phy_reg(hw, MII_DBG_DATA, &phy_data))
-				goto out;
-			phy_data &= 0xFFF7;
-			atl1c_write_phy_reg(hw, MII_DBG_DATA, phy_data);
-			udelay(20);
-		}
+		atl1c_read_phy_dbg(hw, MIIDBG_ANACTRL, &phy_data);
+		phy_data |= ANACTRL_HB_EN;
+		atl1c_write_phy_dbg(hw, MIIDBG_ANACTRL, phy_data);
+		atl1c_read_phy_dbg(hw, MIIDBG_VOLT_CTRL, &phy_data);
+		phy_data &= ~VOLT_CTRL_SWLOWEST;
+		atl1c_write_phy_dbg(hw, MIIDBG_VOLT_CTRL, phy_data);
+		udelay(20);
 	}
 
-	/* maybe MAC-address is from BIOS */
-	AT_READ_REG(hw, REG_MAC_STA_ADDR, &addr[0]);
-	AT_READ_REG(hw, REG_MAC_STA_ADDR + 4, &addr[1]);
-	*(u32 *) &eth_addr[2] = swab32(addr[0]);
-	*(u16 *) &eth_addr[0] = swab16(*(u16 *)&addr[1]);
-
-	if (is_valid_ether_addr(eth_addr)) {
-		memcpy(hw->perm_mac_addr, eth_addr, ETH_ALEN);
+	if (atl1c_read_current_addr(hw, hw->perm_mac_addr))
 		return 0;
-	}
 
-out:
 	return -1;
 }
 
@@ -278,33 +256,158 @@ void atl1c_hash_set(struct atl1c_hw *hw, u32 hash_value)
 }
 
 /*
- * Reads the value from a PHY register
- * hw - Struct containing variables accessed by shared code
- * reg_addr - address of the PHY register to read
+ * wait mdio module be idle
+ * return true: idle
+ *        false: still busy
  */
-int atl1c_read_phy_reg(struct atl1c_hw *hw, u16 reg_addr, u16 *phy_data)
+bool atl1c_wait_mdio_idle(struct atl1c_hw *hw)
 {
 	u32 val;
 	int i;
 
-	val = ((u32)(reg_addr & MDIO_REG_ADDR_MASK)) << MDIO_REG_ADDR_SHIFT |
-		MDIO_START | MDIO_SUP_PREAMBLE | MDIO_RW |
-		MDIO_CLK_25_4 << MDIO_CLK_SEL_SHIFT;
+	for (i = 0; i < MDIO_MAX_AC_TO; i++) {
+		AT_READ_REG(hw, REG_MDIO_CTRL, &val);
+		if (!(val & (MDIO_CTRL_BUSY | MDIO_CTRL_START)))
+			break;
+		udelay(10);
+	}
+
+	return i != MDIO_MAX_AC_TO;
+}
+
+void atl1c_stop_phy_polling(struct atl1c_hw *hw)
+{
+	if (!(hw->ctrl_flags & ATL1C_FPGA_VERSION))
+		return;
+
+	AT_WRITE_REG(hw, REG_MDIO_CTRL, 0);
+	atl1c_wait_mdio_idle(hw);
+}
+
+void atl1c_start_phy_polling(struct atl1c_hw *hw, u16 clk_sel)
+{
+	u32 val;
+
+	if (!(hw->ctrl_flags & ATL1C_FPGA_VERSION))
+		return;
 
+	val = MDIO_CTRL_SPRES_PRMBL |
+		FIELDX(MDIO_CTRL_CLK_SEL, clk_sel) |
+		FIELDX(MDIO_CTRL_REG, 1) |
+		MDIO_CTRL_START |
+		MDIO_CTRL_OP_READ;
+	AT_WRITE_REG(hw, REG_MDIO_CTRL, val);
+	atl1c_wait_mdio_idle(hw);
+	val |= MDIO_CTRL_AP_EN;
+	val &= ~MDIO_CTRL_START;
 	AT_WRITE_REG(hw, REG_MDIO_CTRL, val);
+	udelay(30);
+}
 
-	for (i = 0; i < MDIO_WAIT_TIMES; i++) {
-		udelay(2);
-		AT_READ_REG(hw, REG_MDIO_CTRL, &val);
-		if (!(val & (MDIO_START | MDIO_BUSY)))
-			break;
+
+/*
+ * atl1c_read_phy_core
+ * core funtion to read register in PHY via MDIO control regsiter.
+ * ext: extension register (see IEEE 802.3)
+ * dev: device address (see IEEE 802.3 DEVAD, PRTAD is fixed to 0)
+ * reg: reg to read
+ */
+int atl1c_read_phy_core(struct atl1c_hw *hw, bool ext, u8 dev,
+			u16 reg, u16 *phy_data)
+{
+	u32 val;
+	u16 clk_sel = MDIO_CTRL_CLK_25_4;
+
+	atl1c_stop_phy_polling(hw);
+
+	*phy_data = 0;
+
+	/* only l2c_b2 & l1d_2 could use slow clock */
+	if ((hw->nic_type == athr_l2c_b2 || hw->nic_type == athr_l1d_2) &&
+		hw->hibernate)
+		clk_sel = MDIO_CTRL_CLK_25_128;
+	if (ext) {
+		val = FIELDX(MDIO_EXTN_DEVAD, dev) | FIELDX(MDIO_EXTN_REG, reg);
+		AT_WRITE_REG(hw, REG_MDIO_EXTN, val);
+		val = MDIO_CTRL_SPRES_PRMBL |
+			FIELDX(MDIO_CTRL_CLK_SEL, clk_sel) |
+			MDIO_CTRL_START |
+			MDIO_CTRL_MODE_EXT |
+			MDIO_CTRL_OP_READ;
+	} else {
+		val = MDIO_CTRL_SPRES_PRMBL |
+			FIELDX(MDIO_CTRL_CLK_SEL, clk_sel) |
+			FIELDX(MDIO_CTRL_REG, reg) |
+			MDIO_CTRL_START |
+			MDIO_CTRL_OP_READ;
 	}
-	if (!(val & (MDIO_START | MDIO_BUSY))) {
-		*phy_data = (u16)val;
-		return 0;
+	AT_WRITE_REG(hw, REG_MDIO_CTRL, val);
+
+	if (!atl1c_wait_mdio_idle(hw))
+		return -1;
+
+	AT_READ_REG(hw, REG_MDIO_CTRL, &val);
+	*phy_data = (u16)FIELD_GETX(val, MDIO_CTRL_DATA);
+
+	atl1c_start_phy_polling(hw, clk_sel);
+
+	return 0;
+}
+
+/*
+ * atl1c_write_phy_core
+ * core funtion to write to register in PHY via MDIO control regsiter.
+ * ext: extension register (see IEEE 802.3)
+ * dev: device address (see IEEE 802.3 DEVAD, PRTAD is fixed to 0)
+ * reg: reg to write
+ */
+int atl1c_write_phy_core(struct atl1c_hw *hw, bool ext, u8 dev,
+			u16 reg, u16 phy_data)
+{
+	u32 val;
+	u16 clk_sel = MDIO_CTRL_CLK_25_4;
+
+	atl1c_stop_phy_polling(hw);
+
+
+	/* only l2c_b2 & l1d_2 could use slow clock */
+	if ((hw->nic_type == athr_l2c_b2 || hw->nic_type == athr_l1d_2) &&
+		hw->hibernate)
+		clk_sel = MDIO_CTRL_CLK_25_128;
+
+	if (ext) {
+		val = FIELDX(MDIO_EXTN_DEVAD, dev) | FIELDX(MDIO_EXTN_REG, reg);
+		AT_WRITE_REG(hw, REG_MDIO_EXTN, val);
+		val = MDIO_CTRL_SPRES_PRMBL |
+			FIELDX(MDIO_CTRL_CLK_SEL, clk_sel) |
+			FIELDX(MDIO_CTRL_DATA, phy_data) |
+			MDIO_CTRL_START |
+			MDIO_CTRL_MODE_EXT;
+	} else {
+		val = MDIO_CTRL_SPRES_PRMBL |
+			FIELDX(MDIO_CTRL_CLK_SEL, clk_sel) |
+			FIELDX(MDIO_CTRL_DATA, phy_data) |
+			FIELDX(MDIO_CTRL_REG, reg) |
+			MDIO_CTRL_START;
 	}
+	AT_WRITE_REG(hw, REG_MDIO_CTRL, val);
 
-	return -1;
+	if (!atl1c_wait_mdio_idle(hw))
+		return -1;
+
+	atl1c_start_phy_polling(hw, clk_sel);
+
+	return 0;
+}
+
+/*
+ * Reads the value from a PHY register
+ * hw - Struct containing variables accessed by shared code
+ * reg_addr - address of the PHY register to read
+ */
+int atl1c_read_phy_reg(struct atl1c_hw *hw, u16 reg_addr, u16 *phy_data)
+{
+	return atl1c_read_phy_core(hw, false, 0, reg_addr, phy_data);
 }
 
 /*
@@ -315,27 +418,47 @@ int atl1c_read_phy_reg(struct atl1c_hw *hw, u16 reg_addr, u16 *phy_data)
  */
 int atl1c_write_phy_reg(struct atl1c_hw *hw, u32 reg_addr, u16 phy_data)
 {
-	int i;
-	u32 val;
+	return atl1c_write_phy_core(hw, false, 0, reg_addr, phy_data);
+}
 
-	val = ((u32)(phy_data & MDIO_DATA_MASK)) << MDIO_DATA_SHIFT   |
-	       (reg_addr & MDIO_REG_ADDR_MASK) << MDIO_REG_ADDR_SHIFT |
-	       MDIO_SUP_PREAMBLE | MDIO_START |
-	       MDIO_CLK_25_4 << MDIO_CLK_SEL_SHIFT;
+/* read from PHY extension register */
+int atl1c_read_phy_ext(struct atl1c_hw *hw, u8 dev_addr,
+			u16 reg_addr, u16 *phy_data)
+{
+	return atl1c_read_phy_core(hw, true, dev_addr, reg_addr, phy_data);
+}
 
-	AT_WRITE_REG(hw, REG_MDIO_CTRL, val);
+/* write to PHY extension register */
+int atl1c_write_phy_ext(struct atl1c_hw *hw, u8 dev_addr,
+			u16 reg_addr, u16 phy_data)
+{
+	return atl1c_write_phy_core(hw, true, dev_addr, reg_addr, phy_data);
+}
 
-	for (i = 0; i < MDIO_WAIT_TIMES; i++) {
-		udelay(2);
-		AT_READ_REG(hw, REG_MDIO_CTRL, &val);
-		if (!(val & (MDIO_START | MDIO_BUSY)))
-			break;
-	}
+int atl1c_read_phy_dbg(struct atl1c_hw *hw, u16 reg_addr, u16 *phy_data)
+{
+	int err;
 
-	if (!(val & (MDIO_START | MDIO_BUSY)))
-		return 0;
+	err = atl1c_write_phy_reg(hw, MII_DBG_ADDR, reg_addr);
+	if (unlikely(err))
+		return err;
+	else
+		err = atl1c_read_phy_reg(hw, MII_DBG_DATA, phy_data);
 
-	return -1;
+	return err;
+}
+
+int atl1c_write_phy_dbg(struct atl1c_hw *hw, u16 reg_addr, u16 phy_data)
+{
+	int err;
+
+	err = atl1c_write_phy_reg(hw, MII_DBG_ADDR, reg_addr);
+	if (unlikely(err))
+		return err;
+	else
+		err = atl1c_write_phy_reg(hw, MII_DBG_DATA, phy_data);
+
+	return err;
 }
 
 /*
@@ -380,119 +503,100 @@ static int atl1c_phy_setup_adv(struct atl1c_hw *hw)
 
 void atl1c_phy_disable(struct atl1c_hw *hw)
 {
-	AT_WRITE_REGW(hw, REG_GPHY_CTRL,
-			GPHY_CTRL_PW_WOL_DIS | GPHY_CTRL_EXT_RESET);
+	atl1c_power_saving(hw, 0);
 }
 
-static void atl1c_phy_magic_data(struct atl1c_hw *hw)
-{
-	u16 data;
-
-	data = ANA_LOOP_SEL_10BT | ANA_EN_MASK_TB | ANA_EN_10BT_IDLE |
-		((1 & ANA_INTERVAL_SEL_TIMER_MASK) <<
-		ANA_INTERVAL_SEL_TIMER_SHIFT);
-
-	atl1c_write_phy_reg(hw, MII_DBG_ADDR, MII_ANA_CTRL_18);
-	atl1c_write_phy_reg(hw, MII_DBG_DATA, data);
-
-	data = (2 & ANA_SERDES_CDR_BW_MASK) | ANA_MS_PAD_DBG |
-		ANA_SERDES_EN_DEEM | ANA_SERDES_SEL_HSP | ANA_SERDES_EN_PLL |
-		ANA_SERDES_EN_LCKDT;
-
-	atl1c_write_phy_reg(hw, MII_DBG_ADDR, MII_ANA_CTRL_5);
-	atl1c_write_phy_reg(hw, MII_DBG_DATA, data);
-
-	data = (44 & ANA_LONG_CABLE_TH_100_MASK) |
-		((33 & ANA_SHORT_CABLE_TH_100_MASK) <<
-		ANA_SHORT_CABLE_TH_100_SHIFT) | ANA_BP_BAD_LINK_ACCUM |
-		ANA_BP_SMALL_BW;
-
-	atl1c_write_phy_reg(hw, MII_DBG_ADDR, MII_ANA_CTRL_54);
-	atl1c_write_phy_reg(hw, MII_DBG_DATA, data);
-
-	data = (11 & ANA_IECHO_ADJ_MASK) | ((11 & ANA_IECHO_ADJ_MASK) <<
-		ANA_IECHO_ADJ_2_SHIFT) | ((8 & ANA_IECHO_ADJ_MASK) <<
-		ANA_IECHO_ADJ_1_SHIFT) | ((8 & ANA_IECHO_ADJ_MASK) <<
-		ANA_IECHO_ADJ_0_SHIFT);
-
-	atl1c_write_phy_reg(hw, MII_DBG_ADDR, MII_ANA_CTRL_4);
-	atl1c_write_phy_reg(hw, MII_DBG_DATA, data);
-
-	data = ANA_RESTART_CAL | ((7 & ANA_MANUL_SWICH_ON_MASK) <<
-		ANA_MANUL_SWICH_ON_SHIFT) | ANA_MAN_ENABLE |
-		ANA_SEL_HSP | ANA_EN_HB | ANA_OEN_125M;
-
-	atl1c_write_phy_reg(hw, MII_DBG_ADDR, MII_ANA_CTRL_0);
-	atl1c_write_phy_reg(hw, MII_DBG_DATA, data);
-
-	if (hw->ctrl_flags & ATL1C_HIB_DISABLE) {
-		atl1c_write_phy_reg(hw, MII_DBG_ADDR, MII_ANA_CTRL_41);
-		if (atl1c_read_phy_reg(hw, MII_DBG_DATA, &data) != 0)
-			return;
-		data &= ~ANA_TOP_PS_EN;
-		atl1c_write_phy_reg(hw, MII_DBG_DATA, data);
-
-		atl1c_write_phy_reg(hw, MII_DBG_ADDR, MII_ANA_CTRL_11);
-		if (atl1c_read_phy_reg(hw, MII_DBG_DATA, &data) != 0)
-			return;
-		data &= ~ANA_PS_HIB_EN;
-		atl1c_write_phy_reg(hw, MII_DBG_DATA, data);
-	}
-}
 
 int atl1c_phy_reset(struct atl1c_hw *hw)
 {
 	struct atl1c_adapter *adapter = hw->adapter;
 	struct pci_dev *pdev = adapter->pdev;
 	u16 phy_data;
-	u32 phy_ctrl_data = GPHY_CTRL_DEFAULT;
-	u32 mii_ier_data = IER_LINK_UP | IER_LINK_DOWN;
+	u32 phy_ctrl_data, lpi_ctrl;
 	int err;
 
-	if (hw->ctrl_flags & ATL1C_HIB_DISABLE)
-		phy_ctrl_data &= ~GPHY_CTRL_HIB_EN;
-
+	/* reset PHY core */
+	AT_READ_REG(hw, REG_GPHY_CTRL, &phy_ctrl_data);
+	phy_ctrl_data &= ~(GPHY_CTRL_EXT_RESET | GPHY_CTRL_PHY_IDDQ |
+		GPHY_CTRL_GATE_25M_EN | GPHY_CTRL_PWDOWN_HW | GPHY_CTRL_CLS);
+	phy_ctrl_data |= GPHY_CTRL_SEL_ANA_RST;
+	if (!(hw->ctrl_flags & ATL1C_HIB_DISABLE))
+		phy_ctrl_data |= (GPHY_CTRL_HIB_EN | GPHY_CTRL_HIB_PULSE);
+	else
+		phy_ctrl_data &= ~(GPHY_CTRL_HIB_EN | GPHY_CTRL_HIB_PULSE);
 	AT_WRITE_REG(hw, REG_GPHY_CTRL, phy_ctrl_data);
 	AT_WRITE_FLUSH(hw);
-	msleep(40);
-	phy_ctrl_data |= GPHY_CTRL_EXT_RESET;
-	AT_WRITE_REG(hw, REG_GPHY_CTRL, phy_ctrl_data);
+	udelay(10);
+	AT_WRITE_REG(hw, REG_GPHY_CTRL, phy_ctrl_data | GPHY_CTRL_EXT_RESET);
 	AT_WRITE_FLUSH(hw);
-	msleep(10);
+	udelay(10 * GPHY_CTRL_EXT_RST_TO);	/* delay 800us */
 
+	/* switch clock */
 	if (hw->nic_type == athr_l2c_b) {
-		atl1c_write_phy_reg(hw, MII_DBG_ADDR, 0x0A);
-		atl1c_read_phy_reg(hw, MII_DBG_DATA, &phy_data);
-		atl1c_write_phy_reg(hw, MII_DBG_DATA, phy_data & 0xDFFF);
+		atl1c_read_phy_dbg(hw, MIIDBG_CFGLPSPD, &phy_data);
+		atl1c_write_phy_dbg(hw, MIIDBG_CFGLPSPD,
+			phy_data & ~CFGLPSPD_RSTCNT_CLK125SW);
 	}
 
-	if (hw->nic_type == athr_l2c_b ||
-	    hw->nic_type == athr_l2c_b2 ||
-	    hw->nic_type == athr_l1d ||
-	    hw->nic_type == athr_l1d_2) {
-		atl1c_write_phy_reg(hw, MII_DBG_ADDR, 0x3B);
-		atl1c_read_phy_reg(hw, MII_DBG_DATA, &phy_data);
-		atl1c_write_phy_reg(hw, MII_DBG_DATA, phy_data & 0xFFF7);
-		msleep(20);
+	/* tx-half amplitude issue fix */
+	if (hw->nic_type == athr_l2c_b || hw->nic_type == athr_l2c_b2) {
+		atl1c_read_phy_dbg(hw, MIIDBG_CABLE1TH_DET, &phy_data);
+		phy_data |= CABLE1TH_DET_EN;
+		atl1c_write_phy_dbg(hw, MIIDBG_CABLE1TH_DET, phy_data);
 	}
-	if (hw->nic_type == athr_l1d) {
-		atl1c_write_phy_reg(hw, MII_DBG_ADDR, 0x29);
-		atl1c_write_phy_reg(hw, MII_DBG_DATA, 0x929D);
+
+	/* clear bit3 of dbgport 3B to lower voltage */
+	if (!(hw->ctrl_flags & ATL1C_HIB_DISABLE)) {
+		if (hw->nic_type == athr_l2c_b || hw->nic_type == athr_l2c_b2) {
+			atl1c_read_phy_dbg(hw, MIIDBG_VOLT_CTRL, &phy_data);
+			phy_data &= ~VOLT_CTRL_SWLOWEST;
+			atl1c_write_phy_dbg(hw, MIIDBG_VOLT_CTRL, phy_data);
+		}
+		/* power saving config */
+		phy_data =
+			hw->nic_type == athr_l1d || hw->nic_type == athr_l1d_2 ?
+			L1D_LEGCYPS_DEF : L1C_LEGCYPS_DEF;
+		atl1c_write_phy_dbg(hw, MIIDBG_LEGCYPS, phy_data);
+		/* hib */
+		atl1c_write_phy_dbg(hw, MIIDBG_SYSMODCTRL,
+			SYSMODCTRL_IECHOADJ_DEF);
+	} else {
+		/* disable pws */
+		atl1c_read_phy_dbg(hw, MIIDBG_LEGCYPS, &phy_data);
+		atl1c_write_phy_dbg(hw, MIIDBG_LEGCYPS,
+			phy_data & ~LEGCYPS_EN);
+		/* disable hibernate */
+		atl1c_read_phy_dbg(hw, MIIDBG_HIBNEG, &phy_data);
+		atl1c_write_phy_dbg(hw, MIIDBG_HIBNEG,
+			phy_data & HIBNEG_PSHIB_EN);
 	}
-	if (hw->nic_type == athr_l1c || hw->nic_type == athr_l2c_b2
-		|| hw->nic_type == athr_l2c) {
-		atl1c_write_phy_reg(hw, MII_DBG_ADDR, 0x29);
-		atl1c_write_phy_reg(hw, MII_DBG_DATA, 0xB6DD);
+	/* disable AZ(EEE) by default */
+	if (hw->nic_type == athr_l1d || hw->nic_type == athr_l1d_2 ||
+	    hw->nic_type == athr_l2c_b2) {
+		AT_READ_REG(hw, REG_LPI_CTRL, &lpi_ctrl);
+		AT_WRITE_REG(hw, REG_LPI_CTRL, lpi_ctrl & ~LPI_CTRL_EN);
+		atl1c_write_phy_ext(hw, MIIEXT_ANEG, MIIEXT_LOCAL_EEEADV, 0);
+		atl1c_write_phy_ext(hw, MIIEXT_PCS, MIIEXT_CLDCTRL3,
+			L2CB_CLDCTRL3);
 	}
-	err = atl1c_write_phy_reg(hw, MII_IER, mii_ier_data);
+
+	/* other debug port to set */
+	atl1c_write_phy_dbg(hw, MIIDBG_ANACTRL, ANACTRL_DEF);
+	atl1c_write_phy_dbg(hw, MIIDBG_SRDSYSMOD, SRDSYSMOD_DEF);
+	atl1c_write_phy_dbg(hw, MIIDBG_TST10BTCFG, TST10BTCFG_DEF);
+	/* UNH-IOL test issue, set bit7 */
+	atl1c_write_phy_dbg(hw, MIIDBG_TST100BTCFG,
+		TST100BTCFG_DEF | TST100BTCFG_LITCH_EN);
+
+	/* set phy interrupt mask */
+	phy_data = IER_LINK_UP | IER_LINK_DOWN;
+	err = atl1c_write_phy_reg(hw, MII_IER, phy_data);
 	if (err) {
 		if (netif_msg_hw(adapter))
 			dev_err(&pdev->dev,
 				"Error enable PHY linkChange Interrupt\n");
 		return err;
 	}
-	if (!(hw->ctrl_flags & ATL1C_FPGA_VERSION))
-		atl1c_phy_magic_data(hw);
 	return 0;
 }
 
@@ -589,7 +693,8 @@ int atl1c_get_speed_and_duplex(struct atl1c_hw *hw, u16 *speed, u16 *duplex)
 	return 0;
 }
 
-int atl1c_phy_power_saving(struct atl1c_hw *hw)
+/* select one link mode to get lower power consumption */
+int atl1c_phy_to_ps_link(struct atl1c_hw *hw)
 {
 	struct atl1c_adapter *adapter = (struct atl1c_adapter *)hw->adapter;
 	struct pci_dev *pdev = adapter->pdev;
@@ -660,3 +765,101 @@ int atl1c_restart_autoneg(struct atl1c_hw *hw)
 
 	return atl1c_write_phy_reg(hw, MII_BMCR, mii_bmcr_data);
 }
+
+int atl1c_power_saving(struct atl1c_hw *hw, u32 wufc)
+{
+	struct atl1c_adapter *adapter = (struct atl1c_adapter *)hw->adapter;
+	struct pci_dev *pdev = adapter->pdev;
+	u32 master_ctrl, mac_ctrl, phy_ctrl;
+	u32 wol_ctrl, speed;
+	u16 phy_data;
+
+	wol_ctrl = 0;
+	speed = adapter->link_speed == SPEED_1000 ?
+		MAC_CTRL_SPEED_1000 : MAC_CTRL_SPEED_10_100;
+
+	AT_READ_REG(hw, REG_MASTER_CTRL, &master_ctrl);
+	AT_READ_REG(hw, REG_MAC_CTRL, &mac_ctrl);
+	AT_READ_REG(hw, REG_GPHY_CTRL, &phy_ctrl);
+
+	master_ctrl &= ~MASTER_CTRL_CLK_SEL_DIS;
+	mac_ctrl = FIELD_SETX(mac_ctrl, MAC_CTRL_SPEED, speed);
+	mac_ctrl &= ~(MAC_CTRL_DUPLX | MAC_CTRL_RX_EN | MAC_CTRL_TX_EN);
+	if (adapter->link_duplex == FULL_DUPLEX)
+		mac_ctrl |= MAC_CTRL_DUPLX;
+	phy_ctrl &= ~(GPHY_CTRL_EXT_RESET | GPHY_CTRL_CLS);
+	phy_ctrl |= GPHY_CTRL_SEL_ANA_RST | GPHY_CTRL_HIB_PULSE |
+		GPHY_CTRL_HIB_EN;
+	if (!wufc) { /* without WoL */
+		master_ctrl |= MASTER_CTRL_CLK_SEL_DIS;
+		phy_ctrl |= GPHY_CTRL_PHY_IDDQ | GPHY_CTRL_PWDOWN_HW;
+		AT_WRITE_REG(hw, REG_MASTER_CTRL, master_ctrl);
+		AT_WRITE_REG(hw, REG_MAC_CTRL, mac_ctrl);
+		AT_WRITE_REG(hw, REG_GPHY_CTRL, phy_ctrl);
+		AT_WRITE_REG(hw, REG_WOL_CTRL, 0);
+		hw->phy_configured = false; /* re-init PHY when resume */
+		return 0;
+	}
+	phy_ctrl |= GPHY_CTRL_EXT_RESET;
+	if (wufc & AT_WUFC_MAG) {
+		mac_ctrl |= MAC_CTRL_RX_EN | MAC_CTRL_BC_EN;
+		wol_ctrl |= WOL_MAGIC_EN | WOL_MAGIC_PME_EN;
+		if (hw->nic_type == athr_l2c_b && hw->revision_id == L2CB_V11)
+			wol_ctrl |= WOL_PATTERN_EN | WOL_PATTERN_PME_EN;
+	}
+	if (wufc & AT_WUFC_LNKC) {
+		wol_ctrl |= WOL_LINK_CHG_EN | WOL_LINK_CHG_PME_EN;
+		if (atl1c_write_phy_reg(hw, MII_IER, IER_LINK_UP) != 0) {
+			dev_dbg(&pdev->dev, "%s: write phy MII_IER faild.\n",
+				atl1c_driver_name);
+		}
+	}
+	/* clear PHY interrupt */
+	atl1c_read_phy_reg(hw, MII_ISR, &phy_data);
+
+	dev_dbg(&pdev->dev, "%s: suspend MAC=%x,MASTER=%x,PHY=0x%x,WOL=%x\n",
+		atl1c_driver_name, mac_ctrl, master_ctrl, phy_ctrl, wol_ctrl);
+	AT_WRITE_REG(hw, REG_MASTER_CTRL, master_ctrl);
+	AT_WRITE_REG(hw, REG_MAC_CTRL, mac_ctrl);
+	AT_WRITE_REG(hw, REG_GPHY_CTRL, phy_ctrl);
+	AT_WRITE_REG(hw, REG_WOL_CTRL, wol_ctrl);
+
+	return 0;
+}
+
+
+/* configure phy after Link change Event */
+void atl1c_post_phy_linkchg(struct atl1c_hw *hw, u16 link_speed)
+{
+	u16 phy_val;
+	bool adj_thresh = false;
+
+	if (hw->nic_type == athr_l2c_b || hw->nic_type == athr_l2c_b2 ||
+	    hw->nic_type == athr_l1d || hw->nic_type == athr_l1d_2)
+		adj_thresh = true;
+
+	if (link_speed != SPEED_0) { /* link up */
+		/* az with brcm, half-amp */
+		if (hw->nic_type == athr_l1d_2) {
+			atl1c_read_phy_ext(hw, MIIEXT_PCS, MIIEXT_CLDCTRL6,
+				&phy_val);
+			phy_val = FIELD_GETX(phy_val, CLDCTRL6_CAB_LEN);
+			phy_val = phy_val > CLDCTRL6_CAB_LEN_SHORT ?
+				AZ_ANADECT_LONG : AZ_ANADECT_DEF;
+			atl1c_write_phy_dbg(hw, MIIDBG_AZ_ANADECT, phy_val);
+		}
+		/* threshold adjust */
+		if (adj_thresh && link_speed == SPEED_100 && hw->msi_lnkpatch) {
+			atl1c_write_phy_dbg(hw, MIIDBG_MSE16DB, L1D_MSE16DB_UP);
+			atl1c_write_phy_dbg(hw, MIIDBG_SYSMODCTRL,
+				L1D_SYSMODCTRL_IECHOADJ_DEF);
+		}
+	} else { /* link down */
+		if (adj_thresh && hw->msi_lnkpatch) {
+			atl1c_write_phy_dbg(hw, MIIDBG_SYSMODCTRL,
+				SYSMODCTRL_IECHOADJ_DEF);
+			atl1c_write_phy_dbg(hw, MIIDBG_MSE16DB,
+				L1D_MSE16DB_DOWN);
+		}
+	}
+}
diff --git a/drivers/net/ethernet/atheros/atl1c/atl1c_hw.h b/drivers/net/ethernet/atheros/atl1c/atl1c_hw.h
index 655fc6c4a8a..17d935bdde0 100644
--- a/drivers/net/ethernet/atheros/atl1c/atl1c_hw.h
+++ b/drivers/net/ethernet/atheros/atl1c/atl1c_hw.h
@@ -25,12 +25,18 @@
 #include <linux/types.h>
 #include <linux/mii.h>
 
+#define FIELD_GETX(_x, _name)   ((_x) >> (_name##_SHIFT) & (_name##_MASK))
+#define FIELD_SETX(_x, _name, _v) \
+(((_x) & ~((_name##_MASK) << (_name##_SHIFT))) |\
+(((_v) & (_name##_MASK)) << (_name##_SHIFT)))
+#define FIELDX(_name, _v) (((_v) & (_name##_MASK)) << (_name##_SHIFT))
+
 struct atl1c_adapter;
 struct atl1c_hw;
 
 /* function prototype */
 void atl1c_phy_disable(struct atl1c_hw *hw);
-void atl1c_hw_set_mac_addr(struct atl1c_hw *hw);
+void atl1c_hw_set_mac_addr(struct atl1c_hw *hw, u8 *mac_addr);
 int atl1c_phy_reset(struct atl1c_hw *hw);
 int atl1c_read_mac_addr(struct atl1c_hw *hw);
 int atl1c_get_speed_and_duplex(struct atl1c_hw *hw, u16 *speed, u16 *duplex);
@@ -42,47 +48,45 @@ bool atl1c_read_eeprom(struct atl1c_hw *hw, u32 offset, u32 *p_value);
 int atl1c_phy_init(struct atl1c_hw *hw);
 int atl1c_check_eeprom_exist(struct atl1c_hw *hw);
 int atl1c_restart_autoneg(struct atl1c_hw *hw);
-int atl1c_phy_power_saving(struct atl1c_hw *hw);
+int atl1c_phy_to_ps_link(struct atl1c_hw *hw);
+int atl1c_power_saving(struct atl1c_hw *hw, u32 wufc);
+bool atl1c_wait_mdio_idle(struct atl1c_hw *hw);
+void atl1c_stop_phy_polling(struct atl1c_hw *hw);
+void atl1c_start_phy_polling(struct atl1c_hw *hw, u16 clk_sel);
+int atl1c_read_phy_core(struct atl1c_hw *hw, bool ext, u8 dev,
+			u16 reg, u16 *phy_data);
+int atl1c_write_phy_core(struct atl1c_hw *hw, bool ext, u8 dev,
+			u16 reg, u16 phy_data);
+int atl1c_read_phy_ext(struct atl1c_hw *hw, u8 dev_addr,
+			u16 reg_addr, u16 *phy_data);
+int atl1c_write_phy_ext(struct atl1c_hw *hw, u8 dev_addr,
+			u16 reg_addr, u16 phy_data);
+int atl1c_read_phy_dbg(struct atl1c_hw *hw, u16 reg_addr, u16 *phy_data);
+int atl1c_write_phy_dbg(struct atl1c_hw *hw, u16 reg_addr, u16 phy_data);
+void atl1c_post_phy_linkchg(struct atl1c_hw *hw, u16 link_speed);
+
+/* hw-ids */
+#define PCI_DEVICE_ID_ATTANSIC_L2C      0x1062
+#define PCI_DEVICE_ID_ATTANSIC_L1C      0x1063
+#define PCI_DEVICE_ID_ATHEROS_L2C_B	0x2060 /* AR8152 v1.1 Fast 10/100 */
+#define PCI_DEVICE_ID_ATHEROS_L2C_B2	0x2062 /* AR8152 v2.0 Fast 10/100 */
+#define PCI_DEVICE_ID_ATHEROS_L1D	0x1073 /* AR8151 v1.0 Gigabit 1000 */
+#define PCI_DEVICE_ID_ATHEROS_L1D_2_0	0x1083 /* AR8151 v2.0 Gigabit 1000 */
+#define L2CB_V10			0xc0
+#define L2CB_V11			0xc1
+
 /* register definition */
 #define REG_DEVICE_CAP              	0x5C
 #define DEVICE_CAP_MAX_PAYLOAD_MASK     0x7
 #define DEVICE_CAP_MAX_PAYLOAD_SHIFT    0
 
-#define REG_DEVICE_CTRL			0x60
-#define DEVICE_CTRL_MAX_PAYLOAD_MASK    0x7
-#define DEVICE_CTRL_MAX_PAYLOAD_SHIFT   5
-#define DEVICE_CTRL_MAX_RREQ_SZ_MASK    0x7
-#define DEVICE_CTRL_MAX_RREQ_SZ_SHIFT   12
+#define DEVICE_CTRL_MAXRRS_MIN		2
 
 #define REG_LINK_CTRL			0x68
 #define LINK_CTRL_L0S_EN		0x01
 #define LINK_CTRL_L1_EN			0x02
 #define LINK_CTRL_EXT_SYNC		0x80
 
-#define REG_VPD_CAP			0x6C
-#define VPD_CAP_ID_MASK                 0xff
-#define VPD_CAP_ID_SHIFT                0
-#define VPD_CAP_NEXT_PTR_MASK           0xFF
-#define VPD_CAP_NEXT_PTR_SHIFT          8
-#define VPD_CAP_VPD_ADDR_MASK           0x7FFF
-#define VPD_CAP_VPD_ADDR_SHIFT          16
-#define VPD_CAP_VPD_FLAG                0x80000000
-
-#define REG_VPD_DATA                	0x70
-
-#define REG_PCIE_UC_SEVERITY		0x10C
-#define PCIE_UC_SERVRITY_TRN		0x00000001
-#define PCIE_UC_SERVRITY_DLP		0x00000010
-#define PCIE_UC_SERVRITY_PSN_TLP	0x00001000
-#define PCIE_UC_SERVRITY_FCP		0x00002000
-#define PCIE_UC_SERVRITY_CPL_TO		0x00004000
-#define PCIE_UC_SERVRITY_CA		0x00008000
-#define PCIE_UC_SERVRITY_UC		0x00010000
-#define PCIE_UC_SERVRITY_ROV		0x00020000
-#define PCIE_UC_SERVRITY_MLFP		0x00040000
-#define PCIE_UC_SERVRITY_ECRC		0x00080000
-#define PCIE_UC_SERVRITY_UR		0x00100000
-
 #define REG_DEV_SERIALNUM_CTRL		0x200
 #define REG_DEV_MAC_SEL_MASK		0x0 /* 0:EUI; 1:MAC */
 #define REG_DEV_MAC_SEL_SHIFT		0
@@ -90,25 +94,17 @@ int atl1c_phy_power_saving(struct atl1c_hw *hw);
 #define REG_DEV_SERIAL_NUM_EN_SHIFT	1
 
 #define REG_TWSI_CTRL               	0x218
+#define TWSI_CTLR_FREQ_MASK		0x3UL
+#define TWSI_CTRL_FREQ_SHIFT		24
+#define TWSI_CTRL_FREQ_100K		0
+#define TWSI_CTRL_FREQ_200K		1
+#define TWSI_CTRL_FREQ_300K		2
+#define TWSI_CTRL_FREQ_400K		3
+#define TWSI_CTRL_LD_EXIST		BIT(23)
+#define TWSI_CTRL_HW_LDSTAT		BIT(12)	/* 0:finish,1:in progress */
+#define TWSI_CTRL_SW_LDSTART            BIT(11)
 #define TWSI_CTRL_LD_OFFSET_MASK        0xFF
 #define TWSI_CTRL_LD_OFFSET_SHIFT       0
-#define TWSI_CTRL_LD_SLV_ADDR_MASK      0x7
-#define TWSI_CTRL_LD_SLV_ADDR_SHIFT     8
-#define TWSI_CTRL_SW_LDSTART            0x800
-#define TWSI_CTRL_HW_LDSTART            0x1000
-#define TWSI_CTRL_SMB_SLV_ADDR_MASK     0x7F
-#define TWSI_CTRL_SMB_SLV_ADDR_SHIFT    15
-#define TWSI_CTRL_LD_EXIST              0x400000
-#define TWSI_CTRL_READ_FREQ_SEL_MASK    0x3
-#define TWSI_CTRL_READ_FREQ_SEL_SHIFT   23
-#define TWSI_CTRL_FREQ_SEL_100K         0
-#define TWSI_CTRL_FREQ_SEL_200K         1
-#define TWSI_CTRL_FREQ_SEL_300K         2
-#define TWSI_CTRL_FREQ_SEL_400K         3
-#define TWSI_CTRL_SMB_SLV_ADDR
-#define TWSI_CTRL_WRITE_FREQ_SEL_MASK   0x3
-#define TWSI_CTRL_WRITE_FREQ_SEL_SHIFT  24
-
 
 #define REG_PCIE_DEV_MISC_CTRL      	0x21C
 #define PCIE_DEV_MISC_EXT_PIPE     	0x2
@@ -118,16 +114,23 @@ int atl1c_phy_power_saving(struct atl1c_hw *hw);
 #define PCIE_DEV_MISC_SERDES_SEL_DIN   	0x10
 
 #define REG_PCIE_PHYMISC	    	0x1000
-#define PCIE_PHYMISC_FORCE_RCV_DET	0x4
+#define PCIE_PHYMISC_FORCE_RCV_DET	BIT(2)
+#define PCIE_PHYMISC_NFTS_MASK		0xFFUL
+#define PCIE_PHYMISC_NFTS_SHIFT		16
 
 #define REG_PCIE_PHYMISC2		0x1004
-#define PCIE_PHYMISC2_SERDES_CDR_MASK	0x3
-#define PCIE_PHYMISC2_SERDES_CDR_SHIFT	16
-#define PCIE_PHYMISC2_SERDES_TH_MASK	0x3
-#define PCIE_PHYMISC2_SERDES_TH_SHIFT	18
+#define PCIE_PHYMISC2_L0S_TH_MASK	0x3UL
+#define PCIE_PHYMISC2_L0S_TH_SHIFT	18
+#define L2CB1_PCIE_PHYMISC2_L0S_TH	3
+#define PCIE_PHYMISC2_CDR_BW_MASK	0x3UL
+#define PCIE_PHYMISC2_CDR_BW_SHIFT	16
+#define L2CB1_PCIE_PHYMISC2_CDR_BW	3
 
 #define REG_TWSI_DEBUG			0x1108
-#define TWSI_DEBUG_DEV_EXIST		0x20000000
+#define TWSI_DEBUG_DEV_EXIST		BIT(29)
+
+#define REG_DMA_DBG			0x1114
+#define DMA_DBG_VENDOR_MSG		BIT(0)
 
 #define REG_EEPROM_CTRL			0x12C0
 #define EEPROM_CTRL_DATA_HI_MASK	0xFFFF
@@ -140,56 +143,81 @@ int atl1c_phy_power_saving(struct atl1c_hw *hw);
 #define REG_EEPROM_DATA_LO		0x12C4
 
 #define REG_OTP_CTRL			0x12F0
-#define OTP_CTRL_CLK_EN			0x0002
+#define OTP_CTRL_CLK_EN			BIT(1)
 
 #define REG_PM_CTRL			0x12F8
-#define PM_CTRL_SDES_EN			0x00000001
-#define PM_CTRL_RBER_EN			0x00000002
-#define PM_CTRL_CLK_REQ_EN		0x00000004
-#define PM_CTRL_ASPM_L1_EN		0x00000008
-#define PM_CTRL_SERDES_L1_EN		0x00000010
-#define PM_CTRL_SERDES_PLL_L1_EN	0x00000020
-#define PM_CTRL_SERDES_PD_EX_L1		0x00000040
-#define PM_CTRL_SERDES_BUDS_RX_L1_EN	0x00000080
-#define PM_CTRL_L0S_ENTRY_TIMER_MASK	0xF
-#define PM_CTRL_L0S_ENTRY_TIMER_SHIFT	8
-#define PM_CTRL_ASPM_L0S_EN		0x00001000
-#define PM_CTRL_CLK_SWH_L1		0x00002000
-#define PM_CTRL_CLK_PWM_VER1_1		0x00004000
-#define PM_CTRL_RCVR_WT_TIMER		0x00008000
-#define PM_CTRL_L1_ENTRY_TIMER_MASK	0xF
-#define PM_CTRL_L1_ENTRY_TIMER_SHIFT	16
-#define PM_CTRL_PM_REQ_TIMER_MASK	0xF
-#define PM_CTRL_PM_REQ_TIMER_SHIFT	20
-#define PM_CTRL_LCKDET_TIMER_MASK	0xF
+#define PM_CTRL_HOTRST			BIT(31)
+#define PM_CTRL_MAC_ASPM_CHK		BIT(30)	/* L0s/L1 dis by MAC based on
+						 * thrghput(setting in 15A0) */
+#define PM_CTRL_SA_DLY_EN		BIT(29)
+#define PM_CTRL_L0S_BUFSRX_EN		BIT(28)
+#define PM_CTRL_LCKDET_TIMER_MASK	0xFUL
 #define PM_CTRL_LCKDET_TIMER_SHIFT	24
-#define PM_CTRL_EN_BUFS_RX_L0S		0x10000000
-#define PM_CTRL_SA_DLY_EN		0x20000000
-#define PM_CTRL_MAC_ASPM_CHK		0x40000000
-#define PM_CTRL_HOTRST			0x80000000
+#define PM_CTRL_LCKDET_TIMER_DEF	0xC
+#define PM_CTRL_PM_REQ_TIMER_MASK	0xFUL
+#define PM_CTRL_PM_REQ_TIMER_SHIFT	20	/* pm_request_l1 time > @
+						 * ->L0s not L1 */
+#define PM_CTRL_PM_REQ_TO_DEF		0xF
+#define PMCTRL_TXL1_AFTER_L0S		BIT(19)	/* l1dv2.0+ */
+#define L1D_PMCTRL_L1_ENTRY_TM_MASK	7UL	/* l1dv2.0+, 3bits */
+#define L1D_PMCTRL_L1_ENTRY_TM_SHIFT	16
+#define L1D_PMCTRL_L1_ENTRY_TM_DIS	0
+#define L1D_PMCTRL_L1_ENTRY_TM_2US	1
+#define L1D_PMCTRL_L1_ENTRY_TM_4US	2
+#define L1D_PMCTRL_L1_ENTRY_TM_8US	3
+#define L1D_PMCTRL_L1_ENTRY_TM_16US	4
+#define L1D_PMCTRL_L1_ENTRY_TM_24US	5
+#define L1D_PMCTRL_L1_ENTRY_TM_32US	6
+#define L1D_PMCTRL_L1_ENTRY_TM_63US	7
+#define PM_CTRL_L1_ENTRY_TIMER_MASK	0xFUL  /* l1C 4bits */
+#define PM_CTRL_L1_ENTRY_TIMER_SHIFT	16
+#define L2CB1_PM_CTRL_L1_ENTRY_TM	7
+#define L1C_PM_CTRL_L1_ENTRY_TM		0xF
+#define PM_CTRL_RCVR_WT_TIMER		BIT(15)	/* 1:1us, 0:2ms */
+#define PM_CTRL_CLK_PWM_VER1_1		BIT(14)	/* 0:1.0a,1:1.1 */
+#define PM_CTRL_CLK_SWH_L1		BIT(13)	/* en pcie clk sw in L1 */
+#define PM_CTRL_ASPM_L0S_EN		BIT(12)
+#define PM_CTRL_RXL1_AFTER_L0S		BIT(11)	/* l1dv2.0+ */
+#define L1D_PMCTRL_L0S_TIMER_MASK	7UL	/* l1d2.0+, 3bits*/
+#define L1D_PMCTRL_L0S_TIMER_SHIFT	8
+#define PM_CTRL_L0S_ENTRY_TIMER_MASK	0xFUL	/* l1c, 4bits */
+#define PM_CTRL_L0S_ENTRY_TIMER_SHIFT	8
+#define PM_CTRL_SERDES_BUFS_RX_L1_EN	BIT(7)
+#define PM_CTRL_SERDES_PD_EX_L1		BIT(6)	/* power down serdes rx */
+#define PM_CTRL_SERDES_PLL_L1_EN	BIT(5)
+#define PM_CTRL_SERDES_L1_EN		BIT(4)
+#define PM_CTRL_ASPM_L1_EN		BIT(3)
+#define PM_CTRL_CLK_REQ_EN		BIT(2)
+#define PM_CTRL_RBER_EN			BIT(1)
+#define PM_CTRL_SPRSDWER_EN		BIT(0)
 
 #define REG_LTSSM_ID_CTRL		0x12FC
 #define LTSSM_ID_EN_WRO			0x1000
+
+
 /* Selene Master Control Register */
 #define REG_MASTER_CTRL			0x1400
-#define MASTER_CTRL_SOFT_RST            0x1
-#define MASTER_CTRL_TEST_MODE_MASK	0x3
-#define MASTER_CTRL_TEST_MODE_SHIFT	2
-#define MASTER_CTRL_BERT_START		0x10
-#define MASTER_CTRL_OOB_DIS_OFF		0x40
-#define MASTER_CTRL_SA_TIMER_EN		0x80
-#define MASTER_CTRL_MTIMER_EN           0x100
-#define MASTER_CTRL_MANUAL_INT          0x200
-#define MASTER_CTRL_TX_ITIMER_EN	0x400
-#define MASTER_CTRL_RX_ITIMER_EN	0x800
-#define MASTER_CTRL_CLK_SEL_DIS		0x1000
-#define MASTER_CTRL_CLK_SWH_MODE	0x2000
-#define MASTER_CTRL_INT_RDCLR		0x4000
-#define MASTER_CTRL_REV_NUM_SHIFT	16
-#define MASTER_CTRL_REV_NUM_MASK	0xff
-#define MASTER_CTRL_DEV_ID_SHIFT	24
-#define MASTER_CTRL_DEV_ID_MASK		0x7f
-#define MASTER_CTRL_OTP_SEL		0x80000000
+#define MASTER_CTRL_OTP_SEL		BIT(31)
+#define MASTER_DEV_NUM_MASK		0x7FUL
+#define MASTER_DEV_NUM_SHIFT		24
+#define MASTER_REV_NUM_MASK		0xFFUL
+#define MASTER_REV_NUM_SHIFT		16
+#define MASTER_CTRL_INT_RDCLR		BIT(14)
+#define MASTER_CTRL_CLK_SEL_DIS		BIT(12)	/* 1:alwys sel pclk from
+						 * serdes, not sw to 25M */
+#define MASTER_CTRL_RX_ITIMER_EN	BIT(11)	/* IRQ MODURATION FOR RX */
+#define MASTER_CTRL_TX_ITIMER_EN	BIT(10)	/* MODURATION FOR TX/RX */
+#define MASTER_CTRL_MANU_INT		BIT(9)	/* SOFT MANUAL INT */
+#define MASTER_CTRL_MANUTIMER_EN	BIT(8)
+#define MASTER_CTRL_SA_TIMER_EN		BIT(7)	/* SYS ALIVE TIMER EN */
+#define MASTER_CTRL_OOB_DIS		BIT(6)	/* OUT OF BOX DIS */
+#define MASTER_CTRL_WAKEN_25M		BIT(5)	/* WAKE WO. PCIE CLK */
+#define MASTER_CTRL_BERT_START		BIT(4)
+#define MASTER_PCIE_TSTMOD_MASK		3UL
+#define MASTER_PCIE_TSTMOD_SHIFT	2
+#define MASTER_PCIE_RST			BIT(1)
+#define MASTER_CTRL_SOFT_RST		BIT(0)	/* RST MAC & DMA */
+#define DMA_MAC_RST_TO			50
 
 /* Timer Initial Value Register */
 #define REG_MANUAL_TIMER_INIT       	0x1404
@@ -201,87 +229,85 @@ int atl1c_phy_power_saving(struct atl1c_hw *hw);
 #define IRQ_MODRT_RX_TIMER_SHIFT	16
 
 #define REG_GPHY_CTRL               	0x140C
-#define GPHY_CTRL_EXT_RESET         	0x1
-#define GPHY_CTRL_RTL_MODE		0x2
-#define GPHY_CTRL_LED_MODE		0x4
-#define GPHY_CTRL_ANEG_NOW		0x8
-#define GPHY_CTRL_REV_ANEG		0x10
-#define GPHY_CTRL_GATE_25M_EN       	0x20
-#define GPHY_CTRL_LPW_EXIT          	0x40
-#define GPHY_CTRL_PHY_IDDQ          	0x80
-#define GPHY_CTRL_PHY_IDDQ_DIS      	0x100
-#define GPHY_CTRL_GIGA_DIS		0x200
-#define GPHY_CTRL_HIB_EN            	0x400
-#define GPHY_CTRL_HIB_PULSE         	0x800
-#define GPHY_CTRL_SEL_ANA_RST       	0x1000
-#define GPHY_CTRL_PHY_PLL_ON        	0x2000
-#define GPHY_CTRL_PWDOWN_HW		0x4000
-#define GPHY_CTRL_PHY_PLL_BYPASS	0x8000
-
-#define GPHY_CTRL_DEFAULT (		 \
-		GPHY_CTRL_SEL_ANA_RST	|\
-		GPHY_CTRL_HIB_PULSE	|\
-		GPHY_CTRL_HIB_EN)
-
-#define GPHY_CTRL_PW_WOL_DIS (		 \
-		GPHY_CTRL_SEL_ANA_RST	|\
-		GPHY_CTRL_HIB_PULSE	|\
-		GPHY_CTRL_HIB_EN	|\
-		GPHY_CTRL_PWDOWN_HW	|\
-		GPHY_CTRL_PHY_IDDQ)
-
-#define GPHY_CTRL_POWER_SAVING (	\
-		GPHY_CTRL_SEL_ANA_RST	|\
-		GPHY_CTRL_HIB_EN	|\
-		GPHY_CTRL_HIB_PULSE	|\
-		GPHY_CTRL_PWDOWN_HW	|\
-		GPHY_CTRL_PHY_IDDQ)
+#define GPHY_CTRL_ADDR_MASK		0x1FUL
+#define GPHY_CTRL_ADDR_SHIFT		19
+#define GPHY_CTRL_BP_VLTGSW		BIT(18)
+#define GPHY_CTRL_100AB_EN		BIT(17)
+#define GPHY_CTRL_10AB_EN		BIT(16)
+#define GPHY_CTRL_PHY_PLL_BYPASS	BIT(15)
+#define GPHY_CTRL_PWDOWN_HW		BIT(14)	/* affect MAC&PHY, to low pw */
+#define GPHY_CTRL_PHY_PLL_ON		BIT(13)	/* 1:pll always on, 0:can sw */
+#define GPHY_CTRL_SEL_ANA_RST		BIT(12)
+#define GPHY_CTRL_HIB_PULSE		BIT(11)
+#define GPHY_CTRL_HIB_EN		BIT(10)
+#define GPHY_CTRL_GIGA_DIS		BIT(9)
+#define GPHY_CTRL_PHY_IDDQ_DIS		BIT(8)	/* pw on RST */
+#define GPHY_CTRL_PHY_IDDQ		BIT(7)	/* bit8 affect bit7 while rb */
+#define GPHY_CTRL_LPW_EXIT		BIT(6)
+#define GPHY_CTRL_GATE_25M_EN		BIT(5)
+#define GPHY_CTRL_REV_ANEG		BIT(4)
+#define GPHY_CTRL_ANEG_NOW		BIT(3)
+#define GPHY_CTRL_LED_MODE		BIT(2)
+#define GPHY_CTRL_RTL_MODE		BIT(1)
+#define GPHY_CTRL_EXT_RESET		BIT(0)	/* 1:out of DSP RST status */
+#define GPHY_CTRL_EXT_RST_TO		80	/* 800us atmost */
+#define GPHY_CTRL_CLS			(\
+	GPHY_CTRL_LED_MODE		|\
+	GPHY_CTRL_100AB_EN		|\
+	GPHY_CTRL_PHY_PLL_ON)
+
 /* Block IDLE Status Register */
-#define REG_IDLE_STATUS  		0x1410
-#define IDLE_STATUS_MASK		0x00FF
-#define IDLE_STATUS_RXMAC_NO_IDLE      	0x1
-#define IDLE_STATUS_TXMAC_NO_IDLE      	0x2
-#define IDLE_STATUS_RXQ_NO_IDLE        	0x4
-#define IDLE_STATUS_TXQ_NO_IDLE        	0x8
-#define IDLE_STATUS_DMAR_NO_IDLE       	0x10
-#define IDLE_STATUS_DMAW_NO_IDLE       	0x20
-#define IDLE_STATUS_SMB_NO_IDLE        	0x40
-#define IDLE_STATUS_CMB_NO_IDLE        	0x80
+#define REG_IDLE_STATUS			0x1410
+#define IDLE_STATUS_SFORCE_MASK		0xFUL
+#define IDLE_STATUS_SFORCE_SHIFT	14
+#define IDLE_STATUS_CALIB_DONE		BIT(13)
+#define IDLE_STATUS_CALIB_RES_MASK	0x1FUL
+#define IDLE_STATUS_CALIB_RES_SHIFT	8
+#define IDLE_STATUS_CALIBERR_MASK	0xFUL
+#define IDLE_STATUS_CALIBERR_SHIFT	4
+#define IDLE_STATUS_TXQ_BUSY		BIT(3)
+#define IDLE_STATUS_RXQ_BUSY		BIT(2)
+#define IDLE_STATUS_TXMAC_BUSY		BIT(1)
+#define IDLE_STATUS_RXMAC_BUSY		BIT(0)
+#define IDLE_STATUS_MASK		(\
+	IDLE_STATUS_TXQ_BUSY		|\
+	IDLE_STATUS_RXQ_BUSY		|\
+	IDLE_STATUS_TXMAC_BUSY		|\
+	IDLE_STATUS_RXMAC_BUSY)
 
 /* MDIO Control Register */
 #define REG_MDIO_CTRL           	0x1414
-#define MDIO_DATA_MASK          	0xffff  /* On MDIO write, the 16-bit
-						 * control data to write to PHY
-						 * MII management register */
-#define MDIO_DATA_SHIFT         	0       /* On MDIO read, the 16-bit
-						 * status data that was read
-						 * from the PHY MII management register */
-#define MDIO_REG_ADDR_MASK      	0x1f    /* MDIO register address */
-#define MDIO_REG_ADDR_SHIFT     	16
-#define MDIO_RW                 	0x200000  /* 1: read, 0: write */
-#define MDIO_SUP_PREAMBLE       	0x400000  /* Suppress preamble */
-#define MDIO_START              	0x800000  /* Write 1 to initiate the MDIO
-						   * master. And this bit is self
-						   * cleared after one cycle */
-#define MDIO_CLK_SEL_SHIFT      	24
-#define MDIO_CLK_25_4           	0
-#define MDIO_CLK_25_6           	2
-#define MDIO_CLK_25_8           	3
-#define MDIO_CLK_25_10          	4
-#define MDIO_CLK_25_14          	5
-#define MDIO_CLK_25_20          	6
-#define MDIO_CLK_25_28          	7
-#define MDIO_BUSY               	0x8000000
-#define MDIO_AP_EN              	0x10000000
-#define MDIO_WAIT_TIMES         	10
-
-/* MII PHY Status Register */
-#define REG_PHY_STATUS           	0x1418
-#define PHY_GENERAL_STATUS_MASK		0xFFFF
-#define PHY_STATUS_RECV_ENABLE		0x0001
-#define PHY_OE_PWSP_STATUS_MASK		0x07FF
-#define PHY_OE_PWSP_STATUS_SHIFT	16
-#define PHY_STATUS_LPW_STATE		0x80000000
+#define MDIO_CTRL_MODE_EXT		BIT(30)
+#define MDIO_CTRL_POST_READ		BIT(29)
+#define MDIO_CTRL_AP_EN			BIT(28)
+#define MDIO_CTRL_BUSY			BIT(27)
+#define MDIO_CTRL_CLK_SEL_MASK		0x7UL
+#define MDIO_CTRL_CLK_SEL_SHIFT		24
+#define MDIO_CTRL_CLK_25_4		0	/* 25MHz divide 4 */
+#define MDIO_CTRL_CLK_25_6		2
+#define MDIO_CTRL_CLK_25_8		3
+#define MDIO_CTRL_CLK_25_10		4
+#define MDIO_CTRL_CLK_25_32		5
+#define MDIO_CTRL_CLK_25_64		6
+#define MDIO_CTRL_CLK_25_128		7
+#define MDIO_CTRL_START			BIT(23)
+#define MDIO_CTRL_SPRES_PRMBL		BIT(22)
+#define MDIO_CTRL_OP_READ		BIT(21)	/* 1:read, 0:write */
+#define MDIO_CTRL_REG_MASK		0x1FUL
+#define MDIO_CTRL_REG_SHIFT		16
+#define MDIO_CTRL_DATA_MASK		0xFFFFUL
+#define MDIO_CTRL_DATA_SHIFT		0
+#define MDIO_MAX_AC_TO			120	/* 1.2ms timeout for slow clk */
+
+/* for extension reg access */
+#define REG_MDIO_EXTN			0x1448
+#define MDIO_EXTN_PORTAD_MASK		0x1FUL
+#define MDIO_EXTN_PORTAD_SHIFT		21
+#define MDIO_EXTN_DEVAD_MASK		0x1FUL
+#define MDIO_EXTN_DEVAD_SHIFT		16
+#define MDIO_EXTN_REG_MASK		0xFFFFUL
+#define MDIO_EXTN_REG_SHIFT		0
+
 /* BIST Control and Status Register0 (for the Packet Memory) */
 #define REG_BIST0_CTRL              	0x141c
 #define BIST0_NOW                   	0x1
@@ -299,50 +325,81 @@ int atl1c_phy_power_saving(struct atl1c_hw *hw);
 #define BIST1_FUSE_FLAG             	0x4
 
 /* SerDes Lock Detect Control and Status Register */
-#define REG_SERDES_LOCK            	0x1424
-#define SERDES_LOCK_DETECT          	0x1  /* SerDes lock detected. This signal
-					      * comes from Analog SerDes */
-#define SERDES_LOCK_DETECT_EN       	0x2  /* 1: Enable SerDes Lock detect function */
-#define SERDES_LOCK_STS_SELFB_PLL_SHIFT 0xE
-#define SERDES_LOCK_STS_SELFB_PLL_MASK  0x3
-#define SERDES_OVCLK_18_25		0x0
-#define SERDES_OVCLK_12_18		0x1
-#define SERDES_OVCLK_0_4		0x2
-#define SERDES_OVCLK_4_12		0x3
-#define SERDES_MAC_CLK_SLOWDOWN		0x20000
-#define SERDES_PYH_CLK_SLOWDOWN		0x40000
+#define REG_SERDES			0x1424
+#define SERDES_PHY_CLK_SLOWDOWN		BIT(18)
+#define SERDES_MAC_CLK_SLOWDOWN		BIT(17)
+#define SERDES_SELFB_PLL_MASK		0x3UL
+#define SERDES_SELFB_PLL_SHIFT		14
+#define SERDES_PHYCLK_SEL_GTX		BIT(13)	/* 1:gtx_clk, 0:25M */
+#define SERDES_PCIECLK_SEL_SRDS		BIT(12)	/* 1:serdes,0:25M */
+#define SERDES_BUFS_RX_EN		BIT(11)
+#define SERDES_PD_RX			BIT(10)
+#define SERDES_PLL_EN			BIT(9)
+#define SERDES_EN			BIT(8)
+#define SERDES_SELFB_PLL_SEL_CSR	BIT(6)	/* 0:state-machine,1:csr */
+#define SERDES_SELFB_PLL_CSR_MASK	0x3UL
+#define SERDES_SELFB_PLL_CSR_SHIFT	4
+#define SERDES_SELFB_PLL_CSR_4		3	/* 4-12% OV-CLK */
+#define SERDES_SELFB_PLL_CSR_0		2	/* 0-4% OV-CLK */
+#define SERDES_SELFB_PLL_CSR_12		1	/* 12-18% OV-CLK */
+#define SERDES_SELFB_PLL_CSR_18		0	/* 18-25% OV-CLK */
+#define SERDES_VCO_SLOW			BIT(3)
+#define SERDES_VCO_FAST			BIT(2)
+#define SERDES_LOCK_DETECT_EN		BIT(1)
+#define SERDES_LOCK_DETECT		BIT(0)
+
+#define REG_LPI_DECISN_TIMER            0x143C
+#define L2CB_LPI_DESISN_TIMER		0x7D00
+
+#define REG_LPI_CTRL                    0x1440
+#define LPI_CTRL_CHK_DA			BIT(31)
+#define LPI_CTRL_ENH_TO_MASK		0x1FFFUL
+#define LPI_CTRL_ENH_TO_SHIFT		12
+#define LPI_CTRL_ENH_TH_MASK		0x1FUL
+#define LPI_CTRL_ENH_TH_SHIFT		6
+#define LPI_CTRL_ENH_EN			BIT(5)
+#define LPI_CTRL_CHK_RX			BIT(4)
+#define LPI_CTRL_CHK_STATE		BIT(3)
+#define LPI_CTRL_GMII			BIT(2)
+#define LPI_CTRL_TO_PHY			BIT(1)
+#define LPI_CTRL_EN			BIT(0)
+
+#define REG_LPI_WAIT			0x1444
+#define LPI_WAIT_TIMER_MASK		0xFFFFUL
+#define LPI_WAIT_TIMER_SHIFT		0
 
 /* MAC Control Register  */
 #define REG_MAC_CTRL         		0x1480
-#define MAC_CTRL_TX_EN			0x1
-#define MAC_CTRL_RX_EN			0x2
-#define MAC_CTRL_TX_FLOW		0x4
-#define MAC_CTRL_RX_FLOW            	0x8
-#define MAC_CTRL_LOOPBACK          	0x10
-#define MAC_CTRL_DUPLX              	0x20
-#define MAC_CTRL_ADD_CRC            	0x40
-#define MAC_CTRL_PAD                	0x80
-#define MAC_CTRL_LENCHK             	0x100
-#define MAC_CTRL_HUGE_EN            	0x200
-#define MAC_CTRL_PRMLEN_SHIFT       	10
-#define MAC_CTRL_PRMLEN_MASK        	0xf
-#define MAC_CTRL_RMV_VLAN           	0x4000
-#define MAC_CTRL_PROMIS_EN          	0x8000
-#define MAC_CTRL_TX_PAUSE           	0x10000
-#define MAC_CTRL_SCNT               	0x20000
-#define MAC_CTRL_SRST_TX            	0x40000
-#define MAC_CTRL_TX_SIMURST         	0x80000
-#define MAC_CTRL_SPEED_SHIFT        	20
-#define MAC_CTRL_SPEED_MASK         	0x3
-#define MAC_CTRL_DBG_TX_BKPRESURE   	0x400000
-#define MAC_CTRL_TX_HUGE            	0x800000
-#define MAC_CTRL_RX_CHKSUM_EN       	0x1000000
-#define MAC_CTRL_MC_ALL_EN          	0x2000000
-#define MAC_CTRL_BC_EN              	0x4000000
-#define MAC_CTRL_DBG                	0x8000000
-#define MAC_CTRL_SINGLE_PAUSE_EN	0x10000000
-#define MAC_CTRL_HASH_ALG_CRC32		0x20000000
-#define MAC_CTRL_SPEED_MODE_SW		0x40000000
+#define MAC_CTRL_SPEED_MODE_SW		BIT(30) /* 0:phy,1:sw */
+#define MAC_CTRL_HASH_ALG_CRC32		BIT(29) /* 1:legacy,0:lw_5b */
+#define MAC_CTRL_SINGLE_PAUSE_EN	BIT(28)
+#define MAC_CTRL_DBG			BIT(27)
+#define MAC_CTRL_BC_EN			BIT(26)
+#define MAC_CTRL_MC_ALL_EN		BIT(25)
+#define MAC_CTRL_RX_CHKSUM_EN		BIT(24)
+#define MAC_CTRL_TX_HUGE		BIT(23)
+#define MAC_CTRL_DBG_TX_BKPRESURE	BIT(22)
+#define MAC_CTRL_SPEED_MASK		3UL
+#define MAC_CTRL_SPEED_SHIFT		20
+#define MAC_CTRL_SPEED_10_100		1
+#define MAC_CTRL_SPEED_1000		2
+#define MAC_CTRL_TX_SIMURST		BIT(19)
+#define MAC_CTRL_SCNT			BIT(17)
+#define MAC_CTRL_TX_PAUSE		BIT(16)
+#define MAC_CTRL_PROMIS_EN		BIT(15)
+#define MAC_CTRL_RMV_VLAN		BIT(14)
+#define MAC_CTRL_PRMLEN_MASK		0xFUL
+#define MAC_CTRL_PRMLEN_SHIFT		10
+#define MAC_CTRL_HUGE_EN		BIT(9)
+#define MAC_CTRL_LENCHK			BIT(8)
+#define MAC_CTRL_PAD			BIT(7)
+#define MAC_CTRL_ADD_CRC		BIT(6)
+#define MAC_CTRL_DUPLX			BIT(5)
+#define MAC_CTRL_LOOPBACK		BIT(4)
+#define MAC_CTRL_RX_FLOW		BIT(3)
+#define MAC_CTRL_TX_FLOW		BIT(2)
+#define MAC_CTRL_RX_EN			BIT(1)
+#define MAC_CTRL_TX_EN			BIT(0)
 
 /* MAC IPG/IFG Control Register  */
 #define REG_MAC_IPG_IFG             	0x1484
@@ -386,34 +443,53 @@ int atl1c_phy_power_saving(struct atl1c_hw *hw);
 
 /* Wake-On-Lan control register */
 #define REG_WOL_CTRL                	0x14a0
-#define WOL_PATTERN_EN              	0x00000001
-#define WOL_PATTERN_PME_EN              0x00000002
-#define WOL_MAGIC_EN                    0x00000004
-#define WOL_MAGIC_PME_EN                0x00000008
-#define WOL_LINK_CHG_EN                 0x00000010
-#define WOL_LINK_CHG_PME_EN             0x00000020
-#define WOL_PATTERN_ST                  0x00000100
-#define WOL_MAGIC_ST                    0x00000200
-#define WOL_LINKCHG_ST                  0x00000400
-#define WOL_CLK_SWITCH_EN               0x00008000
-#define WOL_PT0_EN                      0x00010000
-#define WOL_PT1_EN                      0x00020000
-#define WOL_PT2_EN                      0x00040000
-#define WOL_PT3_EN                      0x00080000
-#define WOL_PT4_EN                      0x00100000
-#define WOL_PT5_EN                      0x00200000
-#define WOL_PT6_EN                      0x00400000
+#define WOL_PT7_MATCH			BIT(31)
+#define WOL_PT6_MATCH			BIT(30)
+#define WOL_PT5_MATCH			BIT(29)
+#define WOL_PT4_MATCH			BIT(28)
+#define WOL_PT3_MATCH			BIT(27)
+#define WOL_PT2_MATCH			BIT(26)
+#define WOL_PT1_MATCH			BIT(25)
+#define WOL_PT0_MATCH			BIT(24)
+#define WOL_PT7_EN			BIT(23)
+#define WOL_PT6_EN			BIT(22)
+#define WOL_PT5_EN			BIT(21)
+#define WOL_PT4_EN			BIT(20)
+#define WOL_PT3_EN			BIT(19)
+#define WOL_PT2_EN			BIT(18)
+#define WOL_PT1_EN			BIT(17)
+#define WOL_PT0_EN			BIT(16)
+#define WOL_LNKCHG_ST			BIT(10)
+#define WOL_MAGIC_ST			BIT(9)
+#define WOL_PATTERN_ST			BIT(8)
+#define WOL_OOB_EN			BIT(6)
+#define WOL_LINK_CHG_PME_EN		BIT(5)
+#define WOL_LINK_CHG_EN			BIT(4)
+#define WOL_MAGIC_PME_EN		BIT(3)
+#define WOL_MAGIC_EN			BIT(2)
+#define WOL_PATTERN_PME_EN		BIT(1)
+#define WOL_PATTERN_EN			BIT(0)
 
 /* WOL Length ( 2 DWORD ) */
-#define REG_WOL_PATTERN_LEN         	0x14a4
-#define WOL_PT_LEN_MASK                 0x7f
-#define WOL_PT0_LEN_SHIFT               0
-#define WOL_PT1_LEN_SHIFT               8
-#define WOL_PT2_LEN_SHIFT               16
-#define WOL_PT3_LEN_SHIFT               24
-#define WOL_PT4_LEN_SHIFT               0
-#define WOL_PT5_LEN_SHIFT               8
-#define WOL_PT6_LEN_SHIFT               16
+#define REG_WOL_PTLEN1			0x14A4
+#define WOL_PTLEN1_3_MASK		0xFFUL
+#define WOL_PTLEN1_3_SHIFT		24
+#define WOL_PTLEN1_2_MASK		0xFFUL
+#define WOL_PTLEN1_2_SHIFT		16
+#define WOL_PTLEN1_1_MASK		0xFFUL
+#define WOL_PTLEN1_1_SHIFT		8
+#define WOL_PTLEN1_0_MASK		0xFFUL
+#define WOL_PTLEN1_0_SHIFT		0
+
+#define REG_WOL_PTLEN2			0x14A8
+#define WOL_PTLEN2_7_MASK		0xFFUL
+#define WOL_PTLEN2_7_SHIFT		24
+#define WOL_PTLEN2_6_MASK		0xFFUL
+#define WOL_PTLEN2_6_SHIFT		16
+#define WOL_PTLEN2_5_MASK		0xFFUL
+#define WOL_PTLEN2_5_SHIFT		8
+#define WOL_PTLEN2_4_MASK		0xFFUL
+#define WOL_PTLEN2_4_SHIFT		0
 
 /* Internal SRAM Partition Register */
 #define RFDX_HEAD_ADDR_MASK		0x03FF
@@ -458,66 +534,50 @@ int atl1c_phy_power_saving(struct atl1c_hw *hw);
  */
 #define REG_RX_BASE_ADDR_HI		0x1540
 #define REG_TX_BASE_ADDR_HI		0x1544
-#define REG_SMB_BASE_ADDR_HI		0x1548
-#define REG_SMB_BASE_ADDR_LO		0x154C
 #define REG_RFD0_HEAD_ADDR_LO		0x1550
-#define REG_RFD1_HEAD_ADDR_LO		0x1554
-#define REG_RFD2_HEAD_ADDR_LO		0x1558
-#define REG_RFD3_HEAD_ADDR_LO		0x155C
 #define REG_RFD_RING_SIZE		0x1560
 #define RFD_RING_SIZE_MASK		0x0FFF
 #define REG_RX_BUF_SIZE			0x1564
 #define RX_BUF_SIZE_MASK		0xFFFF
 #define REG_RRD0_HEAD_ADDR_LO		0x1568
-#define REG_RRD1_HEAD_ADDR_LO		0x156C
-#define REG_RRD2_HEAD_ADDR_LO		0x1570
-#define REG_RRD3_HEAD_ADDR_LO		0x1574
 #define REG_RRD_RING_SIZE		0x1578
 #define RRD_RING_SIZE_MASK		0x0FFF
-#define REG_HTPD_HEAD_ADDR_LO		0x157C
-#define REG_NTPD_HEAD_ADDR_LO		0x1580
+#define REG_TPD_PRI1_ADDR_LO		0x157C
+#define REG_TPD_PRI0_ADDR_LO		0x1580
 #define REG_TPD_RING_SIZE		0x1584
 #define TPD_RING_SIZE_MASK		0xFFFF
-#define REG_CMB_BASE_ADDR_LO		0x1588
-
-/* RSS about */
-#define REG_RSS_KEY0                    0x14B0
-#define REG_RSS_KEY1                    0x14B4
-#define REG_RSS_KEY2                    0x14B8
-#define REG_RSS_KEY3                    0x14BC
-#define REG_RSS_KEY4                    0x14C0
-#define REG_RSS_KEY5                    0x14C4
-#define REG_RSS_KEY6                    0x14C8
-#define REG_RSS_KEY7                    0x14CC
-#define REG_RSS_KEY8                    0x14D0
-#define REG_RSS_KEY9                    0x14D4
-#define REG_IDT_TABLE0                	0x14E0
-#define REG_IDT_TABLE1                  0x14E4
-#define REG_IDT_TABLE2                  0x14E8
-#define REG_IDT_TABLE3                  0x14EC
-#define REG_IDT_TABLE4                  0x14F0
-#define REG_IDT_TABLE5                  0x14F4
-#define REG_IDT_TABLE6                  0x14F8
-#define REG_IDT_TABLE7                  0x14FC
-#define REG_IDT_TABLE                   REG_IDT_TABLE0
-#define REG_RSS_HASH_VALUE              0x15B0
-#define REG_RSS_HASH_FLAG               0x15B4
-#define REG_BASE_CPU_NUMBER             0x15B8
 
 /* TXQ Control Register */
-#define REG_TXQ_CTRL                	0x1590
-#define	TXQ_NUM_TPD_BURST_MASK     	0xF
-#define TXQ_NUM_TPD_BURST_SHIFT    	0
-#define TXQ_CTRL_IP_OPTION_EN		0x10
-#define TXQ_CTRL_EN                     0x20
-#define TXQ_CTRL_ENH_MODE               0x40
-#define TXQ_CTRL_LS_8023_EN		0x80
-#define TXQ_TXF_BURST_NUM_SHIFT    	16
-#define TXQ_TXF_BURST_NUM_MASK     	0xFFFF
+#define REG_TXQ_CTRL			0x1590
+#define TXQ_TXF_BURST_NUM_MASK          0xFFFFUL
+#define TXQ_TXF_BURST_NUM_SHIFT		16
+#define L1C_TXQ_TXF_BURST_PREF          0x200
+#define L2CB_TXQ_TXF_BURST_PREF         0x40
+#define TXQ_CTRL_PEDING_CLR             BIT(8)
+#define TXQ_CTRL_LS_8023_EN             BIT(7)
+#define TXQ_CTRL_ENH_MODE               BIT(6)
+#define TXQ_CTRL_EN                     BIT(5)
+#define TXQ_CTRL_IP_OPTION_EN           BIT(4)
+#define TXQ_NUM_TPD_BURST_MASK          0xFUL
+#define TXQ_NUM_TPD_BURST_SHIFT         0
+#define TXQ_NUM_TPD_BURST_DEF           5
+#define TXQ_CFGV			(\
+	FIELDX(TXQ_NUM_TPD_BURST, TXQ_NUM_TPD_BURST_DEF) |\
+	TXQ_CTRL_ENH_MODE |\
+	TXQ_CTRL_LS_8023_EN |\
+	TXQ_CTRL_IP_OPTION_EN)
+#define L1C_TXQ_CFGV			(\
+	TXQ_CFGV |\
+	FIELDX(TXQ_TXF_BURST_NUM, L1C_TXQ_TXF_BURST_PREF))
+#define L2CB_TXQ_CFGV			(\
+	TXQ_CFGV |\
+	FIELDX(TXQ_TXF_BURST_NUM, L2CB_TXQ_TXF_BURST_PREF))
+
 
 /* Jumbo packet Threshold for task offload */
 #define REG_TX_TSO_OFFLOAD_THRESH	0x1594 /* In 8-bytes */
 #define TX_TSO_OFFLOAD_THRESH_MASK	0x07FF
+#define MAX_TSO_FRAME_SIZE		(7*1024)
 
 #define	REG_TXF_WATER_MARK		0x1598 /* In 8-bytes */
 #define TXF_WATER_MARK_MASK		0x0FFF
@@ -537,26 +597,21 @@ int atl1c_phy_power_saving(struct atl1c_hw *hw);
 #define ASPM_THRUPUT_LIMIT_NO		0x00
 #define ASPM_THRUPUT_LIMIT_1M		0x01
 #define ASPM_THRUPUT_LIMIT_10M		0x02
-#define ASPM_THRUPUT_LIMIT_100M		0x04
-#define RXQ1_CTRL_EN			0x10
-#define RXQ2_CTRL_EN			0x20
-#define RXQ3_CTRL_EN			0x40
-#define IPV6_CHKSUM_CTRL_EN		0x80
-#define RSS_HASH_BITS_MASK		0x00FF
-#define RSS_HASH_BITS_SHIFT		8
-#define RSS_HASH_IPV4			0x10000
-#define RSS_HASH_IPV4_TCP		0x20000
-#define RSS_HASH_IPV6			0x40000
-#define RSS_HASH_IPV6_TCP		0x80000
+#define ASPM_THRUPUT_LIMIT_100M		0x03
+#define IPV6_CHKSUM_CTRL_EN		BIT(7)
 #define RXQ_RFD_BURST_NUM_MASK		0x003F
 #define RXQ_RFD_BURST_NUM_SHIFT		20
-#define RSS_MODE_MASK			0x0003
+#define RXQ_NUM_RFD_PREF_DEF		8
+#define RSS_MODE_MASK			3UL
 #define RSS_MODE_SHIFT			26
-#define RSS_NIP_QUEUE_SEL_MASK		0x1
-#define RSS_NIP_QUEUE_SEL_SHIFT		28
-#define RRS_HASH_CTRL_EN		0x20000000
-#define RX_CUT_THRU_EN			0x40000000
-#define RXQ_CTRL_EN			0x80000000
+#define RSS_MODE_DIS			0
+#define RSS_MODE_SQSI			1
+#define RSS_MODE_MQSI			2
+#define RSS_MODE_MQMI			3
+#define RSS_NIP_QUEUE_SEL		BIT(28) /* 0:q0, 1:table */
+#define RRS_HASH_CTRL_EN		BIT(29)
+#define RX_CUT_THRU_EN			BIT(30)
+#define RXQ_CTRL_EN			BIT(31)
 
 #define REG_RFD_FREE_THRESH		0x15A4
 #define RFD_FREE_THRESH_MASK		0x003F
@@ -577,57 +632,45 @@ int atl1c_phy_power_saving(struct atl1c_hw *hw);
 #define RXD_DMA_DOWN_TIMER_SHIFT	16
 
 /* DMA Engine Control Register */
-#define REG_DMA_CTRL                	0x15C0
-#define DMA_CTRL_DMAR_IN_ORDER          0x1
-#define DMA_CTRL_DMAR_ENH_ORDER         0x2
-#define DMA_CTRL_DMAR_OUT_ORDER         0x4
-#define DMA_CTRL_RCB_VALUE              0x8
-#define DMA_CTRL_DMAR_BURST_LEN_MASK    0x0007
-#define DMA_CTRL_DMAR_BURST_LEN_SHIFT   4
-#define DMA_CTRL_DMAW_BURST_LEN_MASK    0x0007
-#define DMA_CTRL_DMAW_BURST_LEN_SHIFT   7
-#define DMA_CTRL_DMAR_REQ_PRI           0x400
-#define DMA_CTRL_DMAR_DLY_CNT_MASK      0x001F
-#define DMA_CTRL_DMAR_DLY_CNT_SHIFT     11
-#define DMA_CTRL_DMAW_DLY_CNT_MASK      0x000F
-#define DMA_CTRL_DMAW_DLY_CNT_SHIFT     16
-#define DMA_CTRL_CMB_EN               	0x100000
-#define DMA_CTRL_SMB_EN			0x200000
-#define DMA_CTRL_CMB_NOW		0x400000
-#define MAC_CTRL_SMB_DIS		0x1000000
-#define DMA_CTRL_SMB_NOW		0x80000000
-
-/* CMB/SMB Control Register */
+#define REG_DMA_CTRL			0x15C0
+#define DMA_CTRL_SMB_NOW                BIT(31)
+#define DMA_CTRL_WPEND_CLR              BIT(30)
+#define DMA_CTRL_RPEND_CLR              BIT(29)
+#define DMA_CTRL_WDLY_CNT_MASK          0xFUL
+#define DMA_CTRL_WDLY_CNT_SHIFT         16
+#define DMA_CTRL_WDLY_CNT_DEF           4
+#define DMA_CTRL_RDLY_CNT_MASK          0x1FUL
+#define DMA_CTRL_RDLY_CNT_SHIFT         11
+#define DMA_CTRL_RDLY_CNT_DEF           15
+#define DMA_CTRL_RREQ_PRI_DATA          BIT(10)      /* 0:tpd, 1:data */
+#define DMA_CTRL_WREQ_BLEN_MASK         7UL
+#define DMA_CTRL_WREQ_BLEN_SHIFT        7
+#define DMA_CTRL_RREQ_BLEN_MASK         7UL
+#define DMA_CTRL_RREQ_BLEN_SHIFT        4
+#define L1C_CTRL_DMA_RCB_LEN128         BIT(3)   /* 0:64bytes,1:128bytes */
+#define DMA_CTRL_RORDER_MODE_MASK       7UL
+#define DMA_CTRL_RORDER_MODE_SHIFT      0
+#define DMA_CTRL_RORDER_MODE_OUT        4
+#define DMA_CTRL_RORDER_MODE_ENHANCE    2
+#define DMA_CTRL_RORDER_MODE_IN         1
+
+/* INT-triggle/SMB Control Register */
 #define REG_SMB_STAT_TIMER		0x15C4	/* 2us resolution */
 #define SMB_STAT_TIMER_MASK		0xFFFFFF
-#define REG_CMB_TPD_THRESH		0x15C8
-#define CMB_TPD_THRESH_MASK		0xFFFF
-#define REG_CMB_TX_TIMER		0x15CC	/* 2us resolution */
-#define CMB_TX_TIMER_MASK		0xFFFF
+#define REG_TINT_TPD_THRESH             0x15C8 /* tpd th to trig intrrupt */
 
 /* Mail box */
 #define MB_RFDX_PROD_IDX_MASK		0xFFFF
 #define REG_MB_RFD0_PROD_IDX		0x15E0
-#define REG_MB_RFD1_PROD_IDX		0x15E4
-#define REG_MB_RFD2_PROD_IDX		0x15E8
-#define REG_MB_RFD3_PROD_IDX		0x15EC
 
-#define MB_PRIO_PROD_IDX_MASK		0xFFFF
-#define REG_MB_PRIO_PROD_IDX		0x15F0
-#define MB_HTPD_PROD_IDX_SHIFT		0
-#define MB_NTPD_PROD_IDX_SHIFT		16
-
-#define MB_PRIO_CONS_IDX_MASK		0xFFFF
-#define REG_MB_PRIO_CONS_IDX		0x15F4
-#define MB_HTPD_CONS_IDX_SHIFT		0
-#define MB_NTPD_CONS_IDX_SHIFT		16
+#define REG_TPD_PRI1_PIDX               0x15F0	/* 16bit,hi-tpd producer idx */
+#define REG_TPD_PRI0_PIDX		0x15F2	/* 16bit,lo-tpd producer idx */
+#define REG_TPD_PRI1_CIDX		0x15F4	/* 16bit,hi-tpd consumer idx */
+#define REG_TPD_PRI0_CIDX		0x15F6	/* 16bit,lo-tpd consumer idx */
 
 #define REG_MB_RFD01_CONS_IDX		0x15F8
 #define MB_RFD0_CONS_IDX_MASK		0x0000FFFF
 #define MB_RFD1_CONS_IDX_MASK		0xFFFF0000
-#define REG_MB_RFD23_CONS_IDX		0x15FC
-#define MB_RFD2_CONS_IDX_MASK		0x0000FFFF
-#define MB_RFD3_CONS_IDX_MASK		0xFFFF0000
 
 /* Interrupt Status Register */
 #define REG_ISR    			0x1600
@@ -705,13 +748,6 @@ int atl1c_phy_power_saving(struct atl1c_hw *hw);
 #define REG_INT_RETRIG_TIMER		0x1608
 #define INT_RETRIG_TIMER_MASK		0xFFFF
 
-#define REG_HDS_CTRL			0x160C
-#define HDS_CTRL_EN			0x0001
-#define HDS_CTRL_BACKFILLSIZE_SHIFT	8
-#define HDS_CTRL_BACKFILLSIZE_MASK	0x0FFF
-#define HDS_CTRL_MAX_HDRSIZE_SHIFT	20
-#define HDS_CTRL_MAC_HDRSIZE_MASK	0x0FFF
-
 #define REG_MAC_RX_STATUS_BIN 		0x1700
 #define REG_MAC_RX_STATUS_END 		0x175c
 #define REG_MAC_TX_STATUS_BIN 		0x1760
@@ -796,73 +832,188 @@ int atl1c_phy_power_saving(struct atl1c_hw *hw);
 #define MII_DBG_ADDR			0x1D
 #define MII_DBG_DATA			0x1E
 
-#define MII_ANA_CTRL_0			0x0
-#define ANA_RESTART_CAL			0x0001
-#define ANA_MANUL_SWICH_ON_SHIFT	0x1
-#define ANA_MANUL_SWICH_ON_MASK		0xF
-#define ANA_MAN_ENABLE			0x0020
-#define ANA_SEL_HSP			0x0040
-#define ANA_EN_HB			0x0080
-#define ANA_EN_HBIAS			0x0100
-#define ANA_OEN_125M			0x0200
-#define ANA_EN_LCKDT			0x0400
-#define ANA_LCKDT_PHY			0x0800
-#define ANA_AFE_MODE			0x1000
-#define ANA_VCO_SLOW			0x2000
-#define ANA_VCO_FAST			0x4000
-#define ANA_SEL_CLK125M_DSP		0x8000
-
-#define MII_ANA_CTRL_4			0x4
-#define ANA_IECHO_ADJ_MASK		0xF
-#define ANA_IECHO_ADJ_3_SHIFT		0
-#define ANA_IECHO_ADJ_2_SHIFT		4
-#define ANA_IECHO_ADJ_1_SHIFT		8
-#define ANA_IECHO_ADJ_0_SHIFT		12
-
-#define MII_ANA_CTRL_5			0x5
-#define ANA_SERDES_CDR_BW_SHIFT		0
-#define ANA_SERDES_CDR_BW_MASK		0x3
-#define ANA_MS_PAD_DBG			0x0004
-#define ANA_SPEEDUP_DBG			0x0008
-#define ANA_SERDES_TH_LOS_SHIFT		4
-#define ANA_SERDES_TH_LOS_MASK		0x3
-#define ANA_SERDES_EN_DEEM		0x0040
-#define ANA_SERDES_TXELECIDLE		0x0080
-#define ANA_SERDES_BEACON		0x0100
-#define ANA_SERDES_HALFTXDR		0x0200
-#define ANA_SERDES_SEL_HSP		0x0400
-#define ANA_SERDES_EN_PLL		0x0800
-#define ANA_SERDES_EN			0x1000
-#define ANA_SERDES_EN_LCKDT		0x2000
-
-#define MII_ANA_CTRL_11			0xB
-#define ANA_PS_HIB_EN			0x8000
-
-#define MII_ANA_CTRL_18			0x12
-#define ANA_TEST_MODE_10BT_01SHIFT	0
-#define ANA_TEST_MODE_10BT_01MASK	0x3
-#define ANA_LOOP_SEL_10BT		0x0004
-#define ANA_RGMII_MODE_SW		0x0008
-#define ANA_EN_LONGECABLE		0x0010
-#define ANA_TEST_MODE_10BT_2		0x0020
-#define ANA_EN_10BT_IDLE		0x0400
-#define ANA_EN_MASK_TB			0x0800
-#define ANA_TRIGGER_SEL_TIMER_SHIFT	12
-#define ANA_TRIGGER_SEL_TIMER_MASK	0x3
-#define ANA_INTERVAL_SEL_TIMER_SHIFT	14
-#define ANA_INTERVAL_SEL_TIMER_MASK	0x3
-
-#define MII_ANA_CTRL_41			0x29
-#define ANA_TOP_PS_EN			0x8000
-
-#define MII_ANA_CTRL_54			0x36
-#define ANA_LONG_CABLE_TH_100_SHIFT	0
-#define ANA_LONG_CABLE_TH_100_MASK	0x3F
-#define ANA_DESERVED			0x0040
-#define ANA_EN_LIT_CH			0x0080
-#define ANA_SHORT_CABLE_TH_100_SHIFT	8
-#define ANA_SHORT_CABLE_TH_100_MASK	0x3F
-#define ANA_BP_BAD_LINK_ACCUM		0x4000
-#define ANA_BP_SMALL_BW			0x8000
+/***************************** debug port *************************************/
+
+#define MIIDBG_ANACTRL                  0x00
+#define ANACTRL_CLK125M_DELAY_EN        0x8000
+#define ANACTRL_VCO_FAST                0x4000
+#define ANACTRL_VCO_SLOW                0x2000
+#define ANACTRL_AFE_MODE_EN             0x1000
+#define ANACTRL_LCKDET_PHY              0x800
+#define ANACTRL_LCKDET_EN               0x400
+#define ANACTRL_OEN_125M                0x200
+#define ANACTRL_HBIAS_EN                0x100
+#define ANACTRL_HB_EN                   0x80
+#define ANACTRL_SEL_HSP                 0x40
+#define ANACTRL_CLASSA_EN               0x20
+#define ANACTRL_MANUSWON_SWR_MASK       3U
+#define ANACTRL_MANUSWON_SWR_SHIFT      2
+#define ANACTRL_MANUSWON_SWR_2V         0
+#define ANACTRL_MANUSWON_SWR_1P9V       1
+#define ANACTRL_MANUSWON_SWR_1P8V       2
+#define ANACTRL_MANUSWON_SWR_1P7V       3
+#define ANACTRL_MANUSWON_BW3_4M         0x2
+#define ANACTRL_RESTART_CAL             0x1
+#define ANACTRL_DEF                     0x02EF
+
+#define MIIDBG_SYSMODCTRL               0x04
+#define SYSMODCTRL_IECHOADJ_PFMH_PHY    0x8000
+#define SYSMODCTRL_IECHOADJ_BIASGEN     0x4000
+#define SYSMODCTRL_IECHOADJ_PFML_PHY    0x2000
+#define SYSMODCTRL_IECHOADJ_PS_MASK     3U
+#define SYSMODCTRL_IECHOADJ_PS_SHIFT    10
+#define SYSMODCTRL_IECHOADJ_PS_40       3
+#define SYSMODCTRL_IECHOADJ_PS_20       2
+#define SYSMODCTRL_IECHOADJ_PS_0        1
+#define SYSMODCTRL_IECHOADJ_10BT_100MV  0x40 /* 1:100mv, 0:200mv */
+#define SYSMODCTRL_IECHOADJ_HLFAP_MASK  3U
+#define SYSMODCTRL_IECHOADJ_HLFAP_SHIFT 4
+#define SYSMODCTRL_IECHOADJ_VDFULBW     0x8
+#define SYSMODCTRL_IECHOADJ_VDBIASHLF   0x4
+#define SYSMODCTRL_IECHOADJ_VDAMPHLF    0x2
+#define SYSMODCTRL_IECHOADJ_VDLANSW     0x1
+#define SYSMODCTRL_IECHOADJ_DEF         0x88BB /* ???? */
+
+/* for l1d & l2cb */
+#define SYSMODCTRL_IECHOADJ_CUR_ADD     0x8000
+#define SYSMODCTRL_IECHOADJ_CUR_MASK    7U
+#define SYSMODCTRL_IECHOADJ_CUR_SHIFT   12
+#define SYSMODCTRL_IECHOADJ_VOL_MASK    0xFU
+#define SYSMODCTRL_IECHOADJ_VOL_SHIFT   8
+#define SYSMODCTRL_IECHOADJ_VOL_17ALL   3
+#define SYSMODCTRL_IECHOADJ_VOL_100M15  1
+#define SYSMODCTRL_IECHOADJ_VOL_10M17   0
+#define SYSMODCTRL_IECHOADJ_BIAS1_MASK  0xFU
+#define SYSMODCTRL_IECHOADJ_BIAS1_SHIFT 4
+#define SYSMODCTRL_IECHOADJ_BIAS2_MASK  0xFU
+#define SYSMODCTRL_IECHOADJ_BIAS2_SHIFT 0
+#define L1D_SYSMODCTRL_IECHOADJ_DEF     0x4FBB
+
+#define MIIDBG_SRDSYSMOD                0x05
+#define SRDSYSMOD_LCKDET_EN             0x2000
+#define SRDSYSMOD_PLL_EN                0x800
+#define SRDSYSMOD_SEL_HSP               0x400
+#define SRDSYSMOD_HLFTXDR               0x200
+#define SRDSYSMOD_TXCLK_DELAY_EN        0x100
+#define SRDSYSMOD_TXELECIDLE            0x80
+#define SRDSYSMOD_DEEMP_EN              0x40
+#define SRDSYSMOD_MS_PAD                0x4
+#define SRDSYSMOD_CDR_ADC_VLTG          0x2
+#define SRDSYSMOD_CDR_DAC_1MA           0x1
+#define SRDSYSMOD_DEF                   0x2C46
+
+#define MIIDBG_CFGLPSPD                 0x0A
+#define CFGLPSPD_RSTCNT_MASK            3U
+#define CFGLPSPD_RSTCNT_SHIFT           14
+#define CFGLPSPD_RSTCNT_CLK125SW        0x2000
+
+#define MIIDBG_HIBNEG                   0x0B
+#define HIBNEG_PSHIB_EN                 0x8000
+#define HIBNEG_WAKE_BOTH                0x4000
+#define HIBNEG_ONOFF_ANACHG_SUDEN       0x2000
+#define HIBNEG_HIB_PULSE                0x1000
+#define HIBNEG_GATE_25M_EN              0x800
+#define HIBNEG_RST_80U                  0x400
+#define HIBNEG_RST_TIMER_MASK           3U
+#define HIBNEG_RST_TIMER_SHIFT          8
+#define HIBNEG_GTX_CLK_DELAY_MASK       3U
+#define HIBNEG_GTX_CLK_DELAY_SHIFT      5
+#define HIBNEG_BYPSS_BRKTIMER           0x10
+#define HIBNEG_DEF                      0xBC40
+
+#define MIIDBG_TST10BTCFG               0x12
+#define TST10BTCFG_INTV_TIMER_MASK      3U
+#define TST10BTCFG_INTV_TIMER_SHIFT     14
+#define TST10BTCFG_TRIGER_TIMER_MASK    3U
+#define TST10BTCFG_TRIGER_TIMER_SHIFT   12
+#define TST10BTCFG_DIV_MAN_MLT3_EN      0x800
+#define TST10BTCFG_OFF_DAC_IDLE         0x400
+#define TST10BTCFG_LPBK_DEEP            0x4 /* 1:deep,0:shallow */
+#define TST10BTCFG_DEF                  0x4C04
+
+#define MIIDBG_AZ_ANADECT		0x15
+#define AZ_ANADECT_10BTRX_TH		0x8000
+#define AZ_ANADECT_BOTH_01CHNL		0x4000
+#define AZ_ANADECT_INTV_MASK		0x3FU
+#define AZ_ANADECT_INTV_SHIFT		8
+#define AZ_ANADECT_THRESH_MASK		0xFU
+#define AZ_ANADECT_THRESH_SHIFT		4
+#define AZ_ANADECT_CHNL_MASK		0xFU
+#define AZ_ANADECT_CHNL_SHIFT		0
+#define AZ_ANADECT_DEF			0x3220
+#define AZ_ANADECT_LONG                 0xb210
+
+#define MIIDBG_MSE16DB			0x18	/* l1d */
+#define L1D_MSE16DB_UP			0x05EA
+#define L1D_MSE16DB_DOWN		0x02EA
+
+#define MIIDBG_LEGCYPS                  0x29
+#define LEGCYPS_EN                      0x8000
+#define LEGCYPS_DAC_AMP1000_MASK        7U
+#define LEGCYPS_DAC_AMP1000_SHIFT       12
+#define LEGCYPS_DAC_AMP100_MASK         7U
+#define LEGCYPS_DAC_AMP100_SHIFT        9
+#define LEGCYPS_DAC_AMP10_MASK          7U
+#define LEGCYPS_DAC_AMP10_SHIFT         6
+#define LEGCYPS_UNPLUG_TIMER_MASK       7U
+#define LEGCYPS_UNPLUG_TIMER_SHIFT      3
+#define LEGCYPS_UNPLUG_DECT_EN          0x4
+#define LEGCYPS_ECNC_PS_EN              0x1
+#define L1D_LEGCYPS_DEF                 0x129D
+#define L1C_LEGCYPS_DEF                 0x36DD
+
+#define MIIDBG_TST100BTCFG              0x36
+#define TST100BTCFG_NORMAL_BW_EN        0x8000
+#define TST100BTCFG_BADLNK_BYPASS       0x4000
+#define TST100BTCFG_SHORTCABL_TH_MASK   0x3FU
+#define TST100BTCFG_SHORTCABL_TH_SHIFT  8
+#define TST100BTCFG_LITCH_EN            0x80
+#define TST100BTCFG_VLT_SW              0x40
+#define TST100BTCFG_LONGCABL_TH_MASK    0x3FU
+#define TST100BTCFG_LONGCABL_TH_SHIFT   0
+#define TST100BTCFG_DEF                 0xE12C
+
+#define MIIDBG_VOLT_CTRL                0x3B	/* only for l2cb 1 & 2 */
+#define VOLT_CTRL_CABLE1TH_MASK         0x1FFU
+#define VOLT_CTRL_CABLE1TH_SHIFT        7
+#define VOLT_CTRL_AMPCTRL_MASK          3U
+#define VOLT_CTRL_AMPCTRL_SHIFT         5
+#define VOLT_CTRL_SW_BYPASS             0x10
+#define VOLT_CTRL_SWLOWEST              0x8
+#define VOLT_CTRL_DACAMP10_MASK         7U
+#define VOLT_CTRL_DACAMP10_SHIFT        0
+
+#define MIIDBG_CABLE1TH_DET             0x3E
+#define CABLE1TH_DET_EN                 0x8000
+
+
+/******* dev 3 *********/
+#define MIIEXT_PCS                      3
+
+#define MIIEXT_CLDCTRL3                 0x8003
+#define CLDCTRL3_BP_CABLE1TH_DET_GT     0x8000
+#define CLDCTRL3_AZ_DISAMP              0x1000
+#define L2CB_CLDCTRL3                   0x4D19
+#define L1D_CLDCTRL3                    0xDD19
+
+#define MIIEXT_CLDCTRL6			0x8006
+#define CLDCTRL6_CAB_LEN_MASK		0x1FFU
+#define CLDCTRL6_CAB_LEN_SHIFT          0
+#define CLDCTRL6_CAB_LEN_SHORT          0x50
+
+/********* dev 7 **********/
+#define MIIEXT_ANEG                     7
+
+#define MIIEXT_LOCAL_EEEADV             0x3C
+#define LOCAL_EEEADV_1000BT             0x4
+#define LOCAL_EEEADV_100BT              0x2
+
+#define MIIEXT_REMOTE_EEEADV            0x3D
+#define REMOTE_EEEADV_1000BT            0x4
+#define REMOTE_EEEADV_100BT             0x2
+
+#define MIIEXT_EEE_ANEG                 0x8000
+#define EEE_ANEG_1000M                  0x4
+#define EEE_ANEG_100M                   0x2
 
 #endif /*_ATL1C_HW_H_*/
diff --git a/drivers/net/ethernet/atheros/atl1c/atl1c_main.c b/drivers/net/ethernet/atheros/atl1c/atl1c_main.c
index 1ef0c9275de..9cc15701101 100644
--- a/drivers/net/ethernet/atheros/atl1c/atl1c_main.c
+++ b/drivers/net/ethernet/atheros/atl1c/atl1c_main.c
@@ -24,14 +24,6 @@
 #define ATL1C_DRV_VERSION "1.0.1.0-NAPI"
 char atl1c_driver_name[] = "atl1c";
 char atl1c_driver_version[] = ATL1C_DRV_VERSION;
-#define PCI_DEVICE_ID_ATTANSIC_L2C      0x1062
-#define PCI_DEVICE_ID_ATTANSIC_L1C      0x1063
-#define PCI_DEVICE_ID_ATHEROS_L2C_B	0x2060 /* AR8152 v1.1 Fast 10/100 */
-#define PCI_DEVICE_ID_ATHEROS_L2C_B2	0x2062 /* AR8152 v2.0 Fast 10/100 */
-#define PCI_DEVICE_ID_ATHEROS_L1D	0x1073 /* AR8151 v1.0 Gigabit 1000 */
-#define PCI_DEVICE_ID_ATHEROS_L1D_2_0	0x1083 /* AR8151 v2.0 Gigabit 1000 */
-#define L2CB_V10			0xc0
-#define L2CB_V11			0xc1
 
 /*
  * atl1c_pci_tbl - PCI Device ID Table
@@ -54,70 +46,72 @@ static DEFINE_PCI_DEVICE_TABLE(atl1c_pci_tbl) = {
 };
 MODULE_DEVICE_TABLE(pci, atl1c_pci_tbl);
 
-MODULE_AUTHOR("Jie Yang <jie.yang@atheros.com>");
-MODULE_DESCRIPTION("Atheros 1000M Ethernet Network Driver");
+MODULE_AUTHOR("Jie Yang");
+MODULE_AUTHOR("Qualcomm Atheros Inc., <nic-devel@qualcomm.com>");
+MODULE_DESCRIPTION("Qualcom Atheros 100/1000M Ethernet Network Driver");
 MODULE_LICENSE("GPL");
 MODULE_VERSION(ATL1C_DRV_VERSION);
 
 static int atl1c_stop_mac(struct atl1c_hw *hw);
-static void atl1c_enable_rx_ctrl(struct atl1c_hw *hw);
-static void atl1c_enable_tx_ctrl(struct atl1c_hw *hw);
 static void atl1c_disable_l0s_l1(struct atl1c_hw *hw);
-static void atl1c_set_aspm(struct atl1c_hw *hw, bool linkup);
-static void atl1c_setup_mac_ctrl(struct atl1c_adapter *adapter);
-static void atl1c_clean_rx_irq(struct atl1c_adapter *adapter, u8 que,
+static void atl1c_set_aspm(struct atl1c_hw *hw, u16 link_speed);
+static void atl1c_start_mac(struct atl1c_adapter *adapter);
+static void atl1c_clean_rx_irq(struct atl1c_adapter *adapter,
 		   int *work_done, int work_to_do);
 static int atl1c_up(struct atl1c_adapter *adapter);
 static void atl1c_down(struct atl1c_adapter *adapter);
+static int atl1c_reset_mac(struct atl1c_hw *hw);
+static void atl1c_reset_dma_ring(struct atl1c_adapter *adapter);
+static int atl1c_configure(struct atl1c_adapter *adapter);
+static int atl1c_alloc_rx_buffer(struct atl1c_adapter *adapter);
 
 static const u16 atl1c_pay_load_size[] = {
 	128, 256, 512, 1024, 2048, 4096,
 };
 
-static const u16 atl1c_rfd_prod_idx_regs[AT_MAX_RECEIVE_QUEUE] =
-{
-	REG_MB_RFD0_PROD_IDX,
-	REG_MB_RFD1_PROD_IDX,
-	REG_MB_RFD2_PROD_IDX,
-	REG_MB_RFD3_PROD_IDX
-};
-
-static const u16 atl1c_rfd_addr_lo_regs[AT_MAX_RECEIVE_QUEUE] =
-{
-	REG_RFD0_HEAD_ADDR_LO,
-	REG_RFD1_HEAD_ADDR_LO,
-	REG_RFD2_HEAD_ADDR_LO,
-	REG_RFD3_HEAD_ADDR_LO
-};
-
-static const u16 atl1c_rrd_addr_lo_regs[AT_MAX_RECEIVE_QUEUE] =
-{
-	REG_RRD0_HEAD_ADDR_LO,
-	REG_RRD1_HEAD_ADDR_LO,
-	REG_RRD2_HEAD_ADDR_LO,
-	REG_RRD3_HEAD_ADDR_LO
-};
 
 static const u32 atl1c_default_msg = NETIF_MSG_DRV | NETIF_MSG_PROBE |
 	NETIF_MSG_LINK | NETIF_MSG_TIMER | NETIF_MSG_IFDOWN | NETIF_MSG_IFUP;
 static void atl1c_pcie_patch(struct atl1c_hw *hw)
 {
-	u32 data;
+	u32 mst_data, data;
 
-	AT_READ_REG(hw, REG_PCIE_PHYMISC, &data);
-	data |= PCIE_PHYMISC_FORCE_RCV_DET;
-	AT_WRITE_REG(hw, REG_PCIE_PHYMISC, data);
+	/* pclk sel could switch to 25M */
+	AT_READ_REG(hw, REG_MASTER_CTRL, &mst_data);
+	mst_data &= ~MASTER_CTRL_CLK_SEL_DIS;
+	AT_WRITE_REG(hw, REG_MASTER_CTRL, mst_data);
 
+	/* WoL/PCIE related settings */
+	if (hw->nic_type == athr_l1c || hw->nic_type == athr_l2c) {
+		AT_READ_REG(hw, REG_PCIE_PHYMISC, &data);
+		data |= PCIE_PHYMISC_FORCE_RCV_DET;
+		AT_WRITE_REG(hw, REG_PCIE_PHYMISC, data);
+	} else { /* new dev set bit5 of MASTER */
+		if (!(mst_data & MASTER_CTRL_WAKEN_25M))
+			AT_WRITE_REG(hw, REG_MASTER_CTRL,
+				mst_data | MASTER_CTRL_WAKEN_25M);
+	}
+	/* aspm/PCIE setting only for l2cb 1.0 */
 	if (hw->nic_type == athr_l2c_b && hw->revision_id == L2CB_V10) {
 		AT_READ_REG(hw, REG_PCIE_PHYMISC2, &data);
-
-		data &= ~(PCIE_PHYMISC2_SERDES_CDR_MASK <<
-			PCIE_PHYMISC2_SERDES_CDR_SHIFT);
-		data |= 3 << PCIE_PHYMISC2_SERDES_CDR_SHIFT;
-		data &= ~(PCIE_PHYMISC2_SERDES_TH_MASK <<
-			PCIE_PHYMISC2_SERDES_TH_SHIFT);
-		data |= 3 << PCIE_PHYMISC2_SERDES_TH_SHIFT;
+		data = FIELD_SETX(data, PCIE_PHYMISC2_CDR_BW,
+			L2CB1_PCIE_PHYMISC2_CDR_BW);
+		data = FIELD_SETX(data, PCIE_PHYMISC2_L0S_TH,
+			L2CB1_PCIE_PHYMISC2_L0S_TH);
 		AT_WRITE_REG(hw, REG_PCIE_PHYMISC2, data);
+		/* extend L1 sync timer */
+		AT_READ_REG(hw, REG_LINK_CTRL, &data);
+		data |= LINK_CTRL_EXT_SYNC;
+		AT_WRITE_REG(hw, REG_LINK_CTRL, data);
+	}
+	/* l2cb 1.x & l1d 1.x */
+	if (hw->nic_type == athr_l2c_b || hw->nic_type == athr_l1d) {
+		AT_READ_REG(hw, REG_PM_CTRL, &data);
+		data |= PM_CTRL_L0S_BUFSRX_EN;
+		AT_WRITE_REG(hw, REG_PM_CTRL, data);
+		/* clear vendor msg */
+		AT_READ_REG(hw, REG_DMA_DBG, &data);
+		AT_WRITE_REG(hw, REG_DMA_DBG, data & ~DMA_DBG_VENDOR_MSG);
 	}
 }
 
@@ -130,6 +124,7 @@ static void atl1c_reset_pcie(struct atl1c_hw *hw, u32 flag)
 	u32 data;
 	u32 pci_cmd;
 	struct pci_dev *pdev = hw->adapter->pdev;
+	int pos;
 
 	AT_READ_REG(hw, PCI_COMMAND, &pci_cmd);
 	pci_cmd &= ~PCI_COMMAND_INTX_DISABLE;
@@ -142,14 +137,23 @@ static void atl1c_reset_pcie(struct atl1c_hw *hw, u32 flag)
 	 */
 	pci_enable_wake(pdev, PCI_D3hot, 0);
 	pci_enable_wake(pdev, PCI_D3cold, 0);
+	/* wol sts read-clear */
+	AT_READ_REG(hw, REG_WOL_CTRL, &data);
+	AT_WRITE_REG(hw, REG_WOL_CTRL, 0);
 
 	/*
 	 * Mask some pcie error bits
 	 */
-	AT_READ_REG(hw, REG_PCIE_UC_SEVERITY, &data);
-	data &= ~PCIE_UC_SERVRITY_DLP;
-	data &= ~PCIE_UC_SERVRITY_FCP;
-	AT_WRITE_REG(hw, REG_PCIE_UC_SEVERITY, data);
+	pos = pci_find_ext_capability(pdev, PCI_EXT_CAP_ID_ERR);
+	pci_read_config_dword(pdev, pos + PCI_ERR_UNCOR_SEVER, &data);
+	data &= ~(PCI_ERR_UNC_DLP | PCI_ERR_UNC_FCP);
+	pci_write_config_dword(pdev, pos + PCI_ERR_UNCOR_SEVER, data);
+	/* clear error status */
+	pci_write_config_word(pdev, pci_pcie_cap(pdev) + PCI_EXP_DEVSTA,
+			PCI_EXP_DEVSTA_NFED |
+			PCI_EXP_DEVSTA_FED |
+			PCI_EXP_DEVSTA_CED |
+			PCI_EXP_DEVSTA_URD);
 
 	AT_READ_REG(hw, REG_LTSSM_ID_CTRL, &data);
 	data &= ~LTSSM_ID_EN_WRO;
@@ -158,11 +162,6 @@ static void atl1c_reset_pcie(struct atl1c_hw *hw, u32 flag)
 	atl1c_pcie_patch(hw);
 	if (flag & ATL1C_PCIE_L0S_L1_DISABLE)
 		atl1c_disable_l0s_l1(hw);
-	if (flag & ATL1C_PCIE_PHY_RESET)
-		AT_WRITE_REG(hw, REG_GPHY_CTRL, GPHY_CTRL_DEFAULT);
-	else
-		AT_WRITE_REG(hw, REG_GPHY_CTRL,
-			GPHY_CTRL_DEFAULT | GPHY_CTRL_EXT_RESET);
 
 	msleep(5);
 }
@@ -207,14 +206,14 @@ static inline void atl1c_irq_reset(struct atl1c_adapter *adapter)
  * atl1c_wait_until_idle - wait up to AT_HW_MAX_IDLE_DELAY reads
  * of the idle status register until the device is actually idle
  */
-static u32 atl1c_wait_until_idle(struct atl1c_hw *hw)
+static u32 atl1c_wait_until_idle(struct atl1c_hw *hw, u32 modu_ctrl)
 {
 	int timeout;
 	u32 data;
 
 	for (timeout = 0; timeout < AT_HW_MAX_IDLE_DELAY; timeout++) {
 		AT_READ_REG(hw, REG_IDLE_STATUS, &data);
-		if ((data & IDLE_STATUS_MASK) == 0)
+		if ((data & modu_ctrl) == 0)
 			return 0;
 		msleep(1);
 	}
@@ -261,15 +260,16 @@ static void atl1c_check_link_status(struct atl1c_adapter *adapter)
 
 	if ((phy_data & BMSR_LSTATUS) == 0) {
 		/* link down */
-		hw->hibernate = true;
-		if (atl1c_stop_mac(hw) != 0)
-			if (netif_msg_hw(adapter))
-				dev_warn(&pdev->dev, "stop mac failed\n");
-		atl1c_set_aspm(hw, false);
 		netif_carrier_off(netdev);
 		netif_stop_queue(netdev);
-		atl1c_phy_reset(hw);
-		atl1c_phy_init(&adapter->hw);
+		hw->hibernate = true;
+		if (atl1c_reset_mac(hw) != 0)
+			if (netif_msg_hw(adapter))
+				dev_warn(&pdev->dev, "reset mac failed\n");
+		atl1c_set_aspm(hw, SPEED_0);
+		atl1c_post_phy_linkchg(hw, SPEED_0);
+		atl1c_reset_dma_ring(adapter);
+		atl1c_configure(adapter);
 	} else {
 		/* Link Up */
 		hw->hibernate = false;
@@ -283,10 +283,9 @@ static void atl1c_check_link_status(struct atl1c_adapter *adapter)
 		    adapter->link_duplex != duplex) {
 			adapter->link_speed  = speed;
 			adapter->link_duplex = duplex;
-			atl1c_set_aspm(hw, true);
-			atl1c_enable_tx_ctrl(hw);
-			atl1c_enable_rx_ctrl(hw);
-			atl1c_setup_mac_ctrl(adapter);
+			atl1c_set_aspm(hw, speed);
+			atl1c_post_phy_linkchg(hw, speed);
+			atl1c_start_mac(adapter);
 			if (netif_msg_link(adapter))
 				dev_info(&pdev->dev,
 					"%s: %s NIC Link is Up<%d Mbps %s>\n",
@@ -337,6 +336,9 @@ static void atl1c_common_task(struct work_struct *work)
 	adapter = container_of(work, struct atl1c_adapter, common_task);
 	netdev = adapter->netdev;
 
+	if (test_bit(__AT_DOWN, &adapter->flags))
+		return;
+
 	if (test_and_clear_bit(ATL1C_WORK_EVENT_RESET, &adapter->work_event)) {
 		netif_device_detach(netdev);
 		atl1c_down(adapter);
@@ -345,8 +347,11 @@ static void atl1c_common_task(struct work_struct *work)
 	}
 
 	if (test_and_clear_bit(ATL1C_WORK_EVENT_LINK_CHANGE,
-		&adapter->work_event))
+		&adapter->work_event)) {
+		atl1c_irq_disable(adapter);
 		atl1c_check_link_status(adapter);
+		atl1c_irq_enable(adapter);
+	}
 }
 
 
@@ -470,7 +475,7 @@ static int atl1c_set_mac_addr(struct net_device *netdev, void *p)
 	memcpy(adapter->hw.mac_addr, addr->sa_data, netdev->addr_len);
 	netdev->addr_assign_type &= ~NET_ADDR_RANDOM;
 
-	atl1c_hw_set_mac_addr(&adapter->hw);
+	atl1c_hw_set_mac_addr(&adapter->hw, adapter->hw.mac_addr);
 
 	return 0;
 }
@@ -523,11 +528,16 @@ static int atl1c_set_features(struct net_device *netdev,
 static int atl1c_change_mtu(struct net_device *netdev, int new_mtu)
 {
 	struct atl1c_adapter *adapter = netdev_priv(netdev);
+	struct atl1c_hw *hw = &adapter->hw;
 	int old_mtu   = netdev->mtu;
 	int max_frame = new_mtu + ETH_HLEN + ETH_FCS_LEN + VLAN_HLEN;
 
-	if ((max_frame < ETH_ZLEN + ETH_FCS_LEN) ||
-			(max_frame > MAX_JUMBO_FRAME_SIZE)) {
+	/* Fast Ethernet controller doesn't support jumbo packet */
+	if (((hw->nic_type == athr_l2c ||
+	      hw->nic_type == athr_l2c_b ||
+	      hw->nic_type == athr_l2c_b2) && new_mtu > ETH_DATA_LEN) ||
+	      max_frame < ETH_ZLEN + ETH_FCS_LEN ||
+	      max_frame > MAX_JUMBO_FRAME_SIZE) {
 		if (netif_msg_link(adapter))
 			dev_warn(&adapter->pdev->dev, "invalid MTU setting\n");
 		return -EINVAL;
@@ -543,14 +553,6 @@ static int atl1c_change_mtu(struct net_device *netdev, int new_mtu)
 		netdev_update_features(netdev);
 		atl1c_up(adapter);
 		clear_bit(__AT_RESETTING, &adapter->flags);
-		if (adapter->hw.ctrl_flags & ATL1C_FPGA_VERSION) {
-			u32 phy_data;
-
-			AT_READ_REG(&adapter->hw, 0x1414, &phy_data);
-			phy_data |= 0x10000000;
-			AT_WRITE_REG(&adapter->hw, 0x1414, phy_data);
-		}
-
 	}
 	return 0;
 }
@@ -563,7 +565,7 @@ static int atl1c_mdio_read(struct net_device *netdev, int phy_id, int reg_num)
 	struct atl1c_adapter *adapter = netdev_priv(netdev);
 	u16 result;
 
-	atl1c_read_phy_reg(&adapter->hw, reg_num & MDIO_REG_ADDR_MASK, &result);
+	atl1c_read_phy_reg(&adapter->hw, reg_num, &result);
 	return result;
 }
 
@@ -572,7 +574,7 @@ static void atl1c_mdio_write(struct net_device *netdev, int phy_id,
 {
 	struct atl1c_adapter *adapter = netdev_priv(netdev);
 
-	atl1c_write_phy_reg(&adapter->hw, reg_num & MDIO_REG_ADDR_MASK, val);
+	atl1c_write_phy_reg(&adapter->hw, reg_num, val);
 }
 
 /*
@@ -687,21 +689,15 @@ static void atl1c_set_mac_type(struct atl1c_hw *hw)
 
 static int atl1c_setup_mac_funcs(struct atl1c_hw *hw)
 {
-	u32 phy_status_data;
 	u32 link_ctrl_data;
 
 	atl1c_set_mac_type(hw);
-	AT_READ_REG(hw, REG_PHY_STATUS, &phy_status_data);
 	AT_READ_REG(hw, REG_LINK_CTRL, &link_ctrl_data);
 
 	hw->ctrl_flags = ATL1C_INTR_MODRT_ENABLE  |
 			 ATL1C_TXQ_MODE_ENHANCE;
-	if (link_ctrl_data & LINK_CTRL_L0S_EN)
-		hw->ctrl_flags |= ATL1C_ASPM_L0S_SUPPORT;
-	if (link_ctrl_data & LINK_CTRL_L1_EN)
-		hw->ctrl_flags |= ATL1C_ASPM_L1_SUPPORT;
-	if (link_ctrl_data & LINK_CTRL_EXT_SYNC)
-		hw->ctrl_flags |= ATL1C_LINK_EXT_SYNC;
+	hw->ctrl_flags |= ATL1C_ASPM_L0S_SUPPORT |
+			  ATL1C_ASPM_L1_SUPPORT;
 	hw->ctrl_flags |= ATL1C_ASPM_CTRL_MON;
 
 	if (hw->nic_type == athr_l1c ||
@@ -710,6 +706,55 @@ static int atl1c_setup_mac_funcs(struct atl1c_hw *hw)
 		hw->link_cap_flags |= ATL1C_LINK_CAP_1000M;
 	return 0;
 }
+
+struct atl1c_platform_patch {
+	u16 pci_did;
+	u8  pci_revid;
+	u16 subsystem_vid;
+	u16 subsystem_did;
+	u32 patch_flag;
+#define ATL1C_LINK_PATCH	0x1
+};
+static const struct atl1c_platform_patch plats[] __devinitdata = {
+{0x2060, 0xC1, 0x1019, 0x8152, 0x1},
+{0x2060, 0xC1, 0x1019, 0x2060, 0x1},
+{0x2060, 0xC1, 0x1019, 0xE000, 0x1},
+{0x2062, 0xC0, 0x1019, 0x8152, 0x1},
+{0x2062, 0xC0, 0x1019, 0x2062, 0x1},
+{0x2062, 0xC0, 0x1458, 0xE000, 0x1},
+{0x2062, 0xC1, 0x1019, 0x8152, 0x1},
+{0x2062, 0xC1, 0x1019, 0x2062, 0x1},
+{0x2062, 0xC1, 0x1458, 0xE000, 0x1},
+{0x2062, 0xC1, 0x1565, 0x2802, 0x1},
+{0x2062, 0xC1, 0x1565, 0x2801, 0x1},
+{0x1073, 0xC0, 0x1019, 0x8151, 0x1},
+{0x1073, 0xC0, 0x1019, 0x1073, 0x1},
+{0x1073, 0xC0, 0x1458, 0xE000, 0x1},
+{0x1083, 0xC0, 0x1458, 0xE000, 0x1},
+{0x1083, 0xC0, 0x1019, 0x8151, 0x1},
+{0x1083, 0xC0, 0x1019, 0x1083, 0x1},
+{0x1083, 0xC0, 0x1462, 0x7680, 0x1},
+{0x1083, 0xC0, 0x1565, 0x2803, 0x1},
+{0},
+};
+
+static void __devinit atl1c_patch_assign(struct atl1c_hw *hw)
+{
+	int i = 0;
+
+	hw->msi_lnkpatch = false;
+
+	while (plats[i].pci_did != 0) {
+		if (plats[i].pci_did == hw->device_id &&
+		    plats[i].pci_revid == hw->revision_id &&
+		    plats[i].subsystem_vid == hw->subsystem_vendor_id &&
+		    plats[i].subsystem_did == hw->subsystem_id) {
+			if (plats[i].patch_flag & ATL1C_LINK_PATCH)
+				hw->msi_lnkpatch = true;
+		}
+		i++;
+	}
+}
 /*
  * atl1c_sw_init - Initialize general software structures (struct atl1c_adapter)
  * @adapter: board private structure to initialize
@@ -729,9 +774,8 @@ static int __devinit atl1c_sw_init(struct atl1c_adapter *adapter)
 	device_set_wakeup_enable(&pdev->dev, false);
 	adapter->link_speed = SPEED_0;
 	adapter->link_duplex = FULL_DUPLEX;
-	adapter->num_rx_queues = AT_DEF_RECEIVE_QUEUE;
 	adapter->tpd_ring[0].count = 1024;
-	adapter->rfd_ring[0].count = 512;
+	adapter->rfd_ring.count = 512;
 
 	hw->vendor_id = pdev->vendor;
 	hw->device_id = pdev->device;
@@ -746,26 +790,18 @@ static int __devinit atl1c_sw_init(struct atl1c_adapter *adapter)
 		dev_err(&pdev->dev, "set mac function pointers failed\n");
 		return -1;
 	}
+	atl1c_patch_assign(hw);
+
 	hw->intr_mask = IMR_NORMAL_MASK;
 	hw->phy_configured = false;
 	hw->preamble_len = 7;
 	hw->max_frame_size = adapter->netdev->mtu;
-	if (adapter->num_rx_queues < 2) {
-		hw->rss_type = atl1c_rss_disable;
-		hw->rss_mode = atl1c_rss_mode_disable;
-	} else {
-		hw->rss_type = atl1c_rss_ipv4;
-		hw->rss_mode = atl1c_rss_mul_que_mul_int;
-		hw->rss_hash_bits = 16;
-	}
 	hw->autoneg_advertised = ADVERTISED_Autoneg;
 	hw->indirect_tab = 0xE4E4E4E4;
 	hw->base_cpu = 0;
 
 	hw->ict = 50000;		/* 100ms */
 	hw->smb_timer = 200000;	  	/* 400ms */
-	hw->cmb_tpd = 4;
-	hw->cmb_tx_timer = 1;		/* 2 us  */
 	hw->rx_imt = 200;
 	hw->tx_imt = 1000;
 
@@ -773,9 +809,6 @@ static int __devinit atl1c_sw_init(struct atl1c_adapter *adapter)
 	hw->rfd_burst = 8;
 	hw->dma_order = atl1c_dma_ord_out;
 	hw->dmar_block = atl1c_dma_req_1024;
-	hw->dmaw_block = atl1c_dma_req_1024;
-	hw->dmar_dly_cnt = 15;
-	hw->dmaw_dly_cnt = 4;
 
 	if (atl1c_alloc_queues(adapter)) {
 		dev_err(&pdev->dev, "Unable to allocate memory for queues\n");
@@ -851,24 +884,22 @@ static void atl1c_clean_tx_ring(struct atl1c_adapter *adapter,
  */
 static void atl1c_clean_rx_ring(struct atl1c_adapter *adapter)
 {
-	struct atl1c_rfd_ring *rfd_ring = adapter->rfd_ring;
-	struct atl1c_rrd_ring *rrd_ring = adapter->rrd_ring;
+	struct atl1c_rfd_ring *rfd_ring = &adapter->rfd_ring;
+	struct atl1c_rrd_ring *rrd_ring = &adapter->rrd_ring;
 	struct atl1c_buffer *buffer_info;
 	struct pci_dev *pdev = adapter->pdev;
-	int i, j;
+	int j;
 
-	for (i = 0; i < adapter->num_rx_queues; i++) {
-		for (j = 0; j < rfd_ring[i].count; j++) {
-			buffer_info = &rfd_ring[i].buffer_info[j];
-			atl1c_clean_buffer(pdev, buffer_info, 0);
-		}
-		/* zero out the descriptor ring */
-		memset(rfd_ring[i].desc, 0, rfd_ring[i].size);
-		rfd_ring[i].next_to_clean = 0;
-		rfd_ring[i].next_to_use = 0;
-		rrd_ring[i].next_to_use = 0;
-		rrd_ring[i].next_to_clean = 0;
+	for (j = 0; j < rfd_ring->count; j++) {
+		buffer_info = &rfd_ring->buffer_info[j];
+		atl1c_clean_buffer(pdev, buffer_info, 0);
 	}
+	/* zero out the descriptor ring */
+	memset(rfd_ring->desc, 0, rfd_ring->size);
+	rfd_ring->next_to_clean = 0;
+	rfd_ring->next_to_use = 0;
+	rrd_ring->next_to_use = 0;
+	rrd_ring->next_to_clean = 0;
 }
 
 /*
@@ -877,8 +908,8 @@ static void atl1c_clean_rx_ring(struct atl1c_adapter *adapter)
 static void atl1c_init_ring_ptrs(struct atl1c_adapter *adapter)
 {
 	struct atl1c_tpd_ring *tpd_ring = adapter->tpd_ring;
-	struct atl1c_rfd_ring *rfd_ring = adapter->rfd_ring;
-	struct atl1c_rrd_ring *rrd_ring = adapter->rrd_ring;
+	struct atl1c_rfd_ring *rfd_ring = &adapter->rfd_ring;
+	struct atl1c_rrd_ring *rrd_ring = &adapter->rrd_ring;
 	struct atl1c_buffer *buffer_info;
 	int i, j;
 
@@ -890,15 +921,13 @@ static void atl1c_init_ring_ptrs(struct atl1c_adapter *adapter)
 			ATL1C_SET_BUFFER_STATE(&buffer_info[i],
 					ATL1C_BUFFER_FREE);
 	}
-	for (i = 0; i < adapter->num_rx_queues; i++) {
-		rfd_ring[i].next_to_use = 0;
-		rfd_ring[i].next_to_clean = 0;
-		rrd_ring[i].next_to_use = 0;
-		rrd_ring[i].next_to_clean = 0;
-		for (j = 0; j < rfd_ring[i].count; j++) {
-			buffer_info = &rfd_ring[i].buffer_info[j];
-			ATL1C_SET_BUFFER_STATE(buffer_info, ATL1C_BUFFER_FREE);
-		}
+	rfd_ring->next_to_use = 0;
+	rfd_ring->next_to_clean = 0;
+	rrd_ring->next_to_use = 0;
+	rrd_ring->next_to_clean = 0;
+	for (j = 0; j < rfd_ring->count; j++) {
+		buffer_info = &rfd_ring->buffer_info[j];
+		ATL1C_SET_BUFFER_STATE(buffer_info, ATL1C_BUFFER_FREE);
 	}
 }
 
@@ -935,27 +964,23 @@ static int atl1c_setup_ring_resources(struct atl1c_adapter *adapter)
 {
 	struct pci_dev *pdev = adapter->pdev;
 	struct atl1c_tpd_ring *tpd_ring = adapter->tpd_ring;
-	struct atl1c_rfd_ring *rfd_ring = adapter->rfd_ring;
-	struct atl1c_rrd_ring *rrd_ring = adapter->rrd_ring;
+	struct atl1c_rfd_ring *rfd_ring = &adapter->rfd_ring;
+	struct atl1c_rrd_ring *rrd_ring = &adapter->rrd_ring;
 	struct atl1c_ring_header *ring_header = &adapter->ring_header;
-	int num_rx_queues = adapter->num_rx_queues;
 	int size;
 	int i;
 	int count = 0;
 	int rx_desc_count = 0;
 	u32 offset = 0;
 
-	rrd_ring[0].count = rfd_ring[0].count;
+	rrd_ring->count = rfd_ring->count;
 	for (i = 1; i < AT_MAX_TRANSMIT_QUEUE; i++)
 		tpd_ring[i].count = tpd_ring[0].count;
 
-	for (i = 1; i < adapter->num_rx_queues; i++)
-		rfd_ring[i].count = rrd_ring[i].count = rfd_ring[0].count;
-
 	/* 2 tpd queue, one high priority queue,
 	 * another normal priority queue */
 	size = sizeof(struct atl1c_buffer) * (tpd_ring->count * 2 +
-		rfd_ring->count * num_rx_queues);
+		rfd_ring->count);
 	tpd_ring->buffer_info = kzalloc(size, GFP_KERNEL);
 	if (unlikely(!tpd_ring->buffer_info)) {
 		dev_err(&pdev->dev, "kzalloc failed, size = %d\n",
@@ -968,12 +993,11 @@ static int atl1c_setup_ring_resources(struct atl1c_adapter *adapter)
 		count += tpd_ring[i].count;
 	}
 
-	for (i = 0; i < num_rx_queues; i++) {
-		rfd_ring[i].buffer_info =
-			(struct atl1c_buffer *) (tpd_ring->buffer_info + count);
-		count += rfd_ring[i].count;
-		rx_desc_count += rfd_ring[i].count;
-	}
+	rfd_ring->buffer_info =
+		(struct atl1c_buffer *) (tpd_ring->buffer_info + count);
+	count += rfd_ring->count;
+	rx_desc_count += rfd_ring->count;
+
 	/*
 	 * real ring DMA buffer
 	 * each ring/block may need up to 8 bytes for alignment, hence the
@@ -983,8 +1007,7 @@ static int atl1c_setup_ring_resources(struct atl1c_adapter *adapter)
 		sizeof(struct atl1c_tpd_desc) * tpd_ring->count * 2 +
 		sizeof(struct atl1c_rx_free_desc) * rx_desc_count +
 		sizeof(struct atl1c_recv_ret_status) * rx_desc_count +
-		sizeof(struct atl1c_hw_stats) +
-		8 * 4 + 8 * 2 * num_rx_queues;
+		8 * 4;
 
 	ring_header->desc = pci_alloc_consistent(pdev, ring_header->size,
 				&ring_header->dma);
@@ -1005,25 +1028,18 @@ static int atl1c_setup_ring_resources(struct atl1c_adapter *adapter)
 		offset += roundup(tpd_ring[i].size, 8);
 	}
 	/* init RFD ring */
-	for (i = 0; i < num_rx_queues; i++) {
-		rfd_ring[i].dma = ring_header->dma + offset;
-		rfd_ring[i].desc = (u8 *) ring_header->desc + offset;
-		rfd_ring[i].size = sizeof(struct atl1c_rx_free_desc) *
-				rfd_ring[i].count;
-		offset += roundup(rfd_ring[i].size, 8);
-	}
+	rfd_ring->dma = ring_header->dma + offset;
+	rfd_ring->desc = (u8 *) ring_header->desc + offset;
+	rfd_ring->size = sizeof(struct atl1c_rx_free_desc) * rfd_ring->count;
+	offset += roundup(rfd_ring->size, 8);
 
 	/* init RRD ring */
-	for (i = 0; i < num_rx_queues; i++) {
-		rrd_ring[i].dma = ring_header->dma + offset;
-		rrd_ring[i].desc = (u8 *) ring_header->desc + offset;
-		rrd_ring[i].size = sizeof(struct atl1c_recv_ret_status) *
-				rrd_ring[i].count;
-		offset += roundup(rrd_ring[i].size, 8);
-	}
+	rrd_ring->dma = ring_header->dma + offset;
+	rrd_ring->desc = (u8 *) ring_header->desc + offset;
+	rrd_ring->size = sizeof(struct atl1c_recv_ret_status) *
+		rrd_ring->count;
+	offset += roundup(rrd_ring->size, 8);
 
-	adapter->smb.dma = ring_header->dma + offset;
-	adapter->smb.smb = (u8 *)ring_header->desc + offset;
 	return 0;
 
 err_nomem:
@@ -1034,26 +1050,20 @@ err_nomem:
 static void atl1c_configure_des_ring(struct atl1c_adapter *adapter)
 {
 	struct atl1c_hw *hw = &adapter->hw;
-	struct atl1c_rfd_ring *rfd_ring = (struct atl1c_rfd_ring *)
-				adapter->rfd_ring;
-	struct atl1c_rrd_ring *rrd_ring = (struct atl1c_rrd_ring *)
-				adapter->rrd_ring;
+	struct atl1c_rfd_ring *rfd_ring = &adapter->rfd_ring;
+	struct atl1c_rrd_ring *rrd_ring = &adapter->rrd_ring;
 	struct atl1c_tpd_ring *tpd_ring = (struct atl1c_tpd_ring *)
 				adapter->tpd_ring;
-	struct atl1c_cmb *cmb = (struct atl1c_cmb *) &adapter->cmb;
-	struct atl1c_smb *smb = (struct atl1c_smb *) &adapter->smb;
-	int i;
-	u32 data;
 
 	/* TPD */
 	AT_WRITE_REG(hw, REG_TX_BASE_ADDR_HI,
 			(u32)((tpd_ring[atl1c_trans_normal].dma &
 				AT_DMA_HI_ADDR_MASK) >> 32));
 	/* just enable normal priority TX queue */
-	AT_WRITE_REG(hw, REG_NTPD_HEAD_ADDR_LO,
+	AT_WRITE_REG(hw, REG_TPD_PRI0_ADDR_LO,
 			(u32)(tpd_ring[atl1c_trans_normal].dma &
 				AT_DMA_LO_ADDR_MASK));
-	AT_WRITE_REG(hw, REG_HTPD_HEAD_ADDR_LO,
+	AT_WRITE_REG(hw, REG_TPD_PRI1_ADDR_LO,
 			(u32)(tpd_ring[atl1c_trans_high].dma &
 				AT_DMA_LO_ADDR_MASK));
 	AT_WRITE_REG(hw, REG_TPD_RING_SIZE,
@@ -1062,31 +1072,21 @@ static void atl1c_configure_des_ring(struct atl1c_adapter *adapter)
 
 	/* RFD */
 	AT_WRITE_REG(hw, REG_RX_BASE_ADDR_HI,
-			(u32)((rfd_ring[0].dma & AT_DMA_HI_ADDR_MASK) >> 32));
-	for (i = 0; i < adapter->num_rx_queues; i++)
-		AT_WRITE_REG(hw, atl1c_rfd_addr_lo_regs[i],
-			(u32)(rfd_ring[i].dma & AT_DMA_LO_ADDR_MASK));
+			(u32)((rfd_ring->dma & AT_DMA_HI_ADDR_MASK) >> 32));
+	AT_WRITE_REG(hw, REG_RFD0_HEAD_ADDR_LO,
+			(u32)(rfd_ring->dma & AT_DMA_LO_ADDR_MASK));
 
 	AT_WRITE_REG(hw, REG_RFD_RING_SIZE,
-			rfd_ring[0].count & RFD_RING_SIZE_MASK);
+			rfd_ring->count & RFD_RING_SIZE_MASK);
 	AT_WRITE_REG(hw, REG_RX_BUF_SIZE,
 			adapter->rx_buffer_len & RX_BUF_SIZE_MASK);
 
 	/* RRD */
-	for (i = 0; i < adapter->num_rx_queues; i++)
-		AT_WRITE_REG(hw, atl1c_rrd_addr_lo_regs[i],
-			(u32)(rrd_ring[i].dma & AT_DMA_LO_ADDR_MASK));
+	AT_WRITE_REG(hw, REG_RRD0_HEAD_ADDR_LO,
+			(u32)(rrd_ring->dma & AT_DMA_LO_ADDR_MASK));
 	AT_WRITE_REG(hw, REG_RRD_RING_SIZE,
-			(rrd_ring[0].count & RRD_RING_SIZE_MASK));
+			(rrd_ring->count & RRD_RING_SIZE_MASK));
 
-	/* CMB */
-	AT_WRITE_REG(hw, REG_CMB_BASE_ADDR_LO, cmb->dma & AT_DMA_LO_ADDR_MASK);
-
-	/* SMB */
-	AT_WRITE_REG(hw, REG_SMB_BASE_ADDR_HI,
-			(u32)((smb->dma & AT_DMA_HI_ADDR_MASK) >> 32));
-	AT_WRITE_REG(hw, REG_SMB_BASE_ADDR_LO,
-			(u32)(smb->dma & AT_DMA_LO_ADDR_MASK));
 	if (hw->nic_type == athr_l2c_b) {
 		AT_WRITE_REG(hw, REG_SRAM_RXF_LEN, 0x02a0L);
 		AT_WRITE_REG(hw, REG_SRAM_TXF_LEN, 0x0100L);
@@ -1097,13 +1097,6 @@ static void atl1c_configure_des_ring(struct atl1c_adapter *adapter)
 		AT_WRITE_REG(hw, REG_TXF_WATER_MARK, 0);	/* TX watermark, to enter l1 state.*/
 		AT_WRITE_REG(hw, REG_RXD_DMA_CTRL, 0);		/* RXD threshold.*/
 	}
-	if (hw->nic_type == athr_l2c_b || hw->nic_type == athr_l1d_2) {
-			/* Power Saving for L2c_B */
-		AT_READ_REG(hw, REG_SERDES_LOCK, &data);
-		data |= SERDES_MAC_CLK_SLOWDOWN;
-		data |= SERDES_PYH_CLK_SLOWDOWN;
-		AT_WRITE_REG(hw, REG_SERDES_LOCK, data);
-	}
 	/* Load all of base address above */
 	AT_WRITE_REG(hw, REG_LOAD_PTR, 1);
 }
@@ -1111,32 +1104,26 @@ static void atl1c_configure_des_ring(struct atl1c_adapter *adapter)
 static void atl1c_configure_tx(struct atl1c_adapter *adapter)
 {
 	struct atl1c_hw *hw = &adapter->hw;
-	u32 dev_ctrl_data;
-	u32 max_pay_load;
+	int max_pay_load;
 	u16 tx_offload_thresh;
 	u32 txq_ctrl_data;
-	u32 max_pay_load_data;
 
-	tx_offload_thresh = MAX_TX_OFFLOAD_THRESH;
+	tx_offload_thresh = MAX_TSO_FRAME_SIZE;
 	AT_WRITE_REG(hw, REG_TX_TSO_OFFLOAD_THRESH,
 		(tx_offload_thresh >> 3) & TX_TSO_OFFLOAD_THRESH_MASK);
-	AT_READ_REG(hw, REG_DEVICE_CTRL, &dev_ctrl_data);
-	max_pay_load  = (dev_ctrl_data >> DEVICE_CTRL_MAX_PAYLOAD_SHIFT) &
-			DEVICE_CTRL_MAX_PAYLOAD_MASK;
-	hw->dmaw_block = min_t(u32, max_pay_load, hw->dmaw_block);
-	max_pay_load  = (dev_ctrl_data >> DEVICE_CTRL_MAX_RREQ_SZ_SHIFT) &
-			DEVICE_CTRL_MAX_RREQ_SZ_MASK;
+	max_pay_load = pcie_get_readrq(adapter->pdev) >> 8;
 	hw->dmar_block = min_t(u32, max_pay_load, hw->dmar_block);
-
-	txq_ctrl_data = (hw->tpd_burst & TXQ_NUM_TPD_BURST_MASK) <<
-			TXQ_NUM_TPD_BURST_SHIFT;
-	if (hw->ctrl_flags & ATL1C_TXQ_MODE_ENHANCE)
-		txq_ctrl_data |= TXQ_CTRL_ENH_MODE;
-	max_pay_load_data = (atl1c_pay_load_size[hw->dmar_block] &
-			TXQ_TXF_BURST_NUM_MASK) << TXQ_TXF_BURST_NUM_SHIFT;
-	if (hw->nic_type == athr_l2c_b || hw->nic_type == athr_l2c_b2)
-		max_pay_load_data >>= 1;
-	txq_ctrl_data |= max_pay_load_data;
+	/*
+	 * if BIOS had changed the dam-read-max-length to an invalid value,
+	 * restore it to default value
+	 */
+	if (hw->dmar_block < DEVICE_CTRL_MAXRRS_MIN) {
+		pcie_set_readrq(adapter->pdev, 128 << DEVICE_CTRL_MAXRRS_MIN);
+		hw->dmar_block = DEVICE_CTRL_MAXRRS_MIN;
+	}
+	txq_ctrl_data =
+		hw->nic_type == athr_l2c_b || hw->nic_type == athr_l2c_b2 ?
+		L2CB_TXQ_CFGV : L1C_TXQ_CFGV;
 
 	AT_WRITE_REG(hw, REG_TXQ_CTRL, txq_ctrl_data);
 }
@@ -1151,34 +1138,13 @@ static void atl1c_configure_rx(struct atl1c_adapter *adapter)
 
 	if (hw->ctrl_flags & ATL1C_RX_IPV6_CHKSUM)
 		rxq_ctrl_data |= IPV6_CHKSUM_CTRL_EN;
-	if (hw->rss_type == atl1c_rss_ipv4)
-		rxq_ctrl_data |= RSS_HASH_IPV4;
-	if (hw->rss_type == atl1c_rss_ipv4_tcp)
-		rxq_ctrl_data |= RSS_HASH_IPV4_TCP;
-	if (hw->rss_type == atl1c_rss_ipv6)
-		rxq_ctrl_data |= RSS_HASH_IPV6;
-	if (hw->rss_type == atl1c_rss_ipv6_tcp)
-		rxq_ctrl_data |= RSS_HASH_IPV6_TCP;
-	if (hw->rss_type != atl1c_rss_disable)
-		rxq_ctrl_data |= RRS_HASH_CTRL_EN;
-
-	rxq_ctrl_data |= (hw->rss_mode & RSS_MODE_MASK) <<
-			RSS_MODE_SHIFT;
-	rxq_ctrl_data |= (hw->rss_hash_bits & RSS_HASH_BITS_MASK) <<
-			RSS_HASH_BITS_SHIFT;
-	if (hw->ctrl_flags & ATL1C_ASPM_CTRL_MON)
-		rxq_ctrl_data |= (ASPM_THRUPUT_LIMIT_1M &
-			ASPM_THRUPUT_LIMIT_MASK) << ASPM_THRUPUT_LIMIT_SHIFT;
 
-	AT_WRITE_REG(hw, REG_RXQ_CTRL, rxq_ctrl_data);
-}
-
-static void atl1c_configure_rss(struct atl1c_adapter *adapter)
-{
-	struct atl1c_hw *hw = &adapter->hw;
+	/* aspm for gigabit */
+	if (hw->nic_type != athr_l1d_2 && (hw->device_id & 1) != 0)
+		rxq_ctrl_data = FIELD_SETX(rxq_ctrl_data, ASPM_THRUPUT_LIMIT,
+			ASPM_THRUPUT_LIMIT_100M);
 
-	AT_WRITE_REG(hw, REG_IDT_TABLE, hw->indirect_tab);
-	AT_WRITE_REG(hw, REG_BASE_CPU_NUMBER, hw->base_cpu);
+	AT_WRITE_REG(hw, REG_RXQ_CTRL, rxq_ctrl_data);
 }
 
 static void atl1c_configure_dma(struct atl1c_adapter *adapter)
@@ -1186,36 +1152,11 @@ static void atl1c_configure_dma(struct atl1c_adapter *adapter)
 	struct atl1c_hw *hw = &adapter->hw;
 	u32 dma_ctrl_data;
 
-	dma_ctrl_data = DMA_CTRL_DMAR_REQ_PRI;
-	if (hw->ctrl_flags & ATL1C_CMB_ENABLE)
-		dma_ctrl_data |= DMA_CTRL_CMB_EN;
-	if (hw->ctrl_flags & ATL1C_SMB_ENABLE)
-		dma_ctrl_data |= DMA_CTRL_SMB_EN;
-	else
-		dma_ctrl_data |= MAC_CTRL_SMB_DIS;
-
-	switch (hw->dma_order) {
-	case atl1c_dma_ord_in:
-		dma_ctrl_data |= DMA_CTRL_DMAR_IN_ORDER;
-		break;
-	case atl1c_dma_ord_enh:
-		dma_ctrl_data |= DMA_CTRL_DMAR_ENH_ORDER;
-		break;
-	case atl1c_dma_ord_out:
-		dma_ctrl_data |= DMA_CTRL_DMAR_OUT_ORDER;
-		break;
-	default:
-		break;
-	}
-
-	dma_ctrl_data |= (((u32)hw->dmar_block) & DMA_CTRL_DMAR_BURST_LEN_MASK)
-		<< DMA_CTRL_DMAR_BURST_LEN_SHIFT;
-	dma_ctrl_data |= (((u32)hw->dmaw_block) & DMA_CTRL_DMAW_BURST_LEN_MASK)
-		<< DMA_CTRL_DMAW_BURST_LEN_SHIFT;
-	dma_ctrl_data |= (((u32)hw->dmar_dly_cnt) & DMA_CTRL_DMAR_DLY_CNT_MASK)
-		<< DMA_CTRL_DMAR_DLY_CNT_SHIFT;
-	dma_ctrl_data |= (((u32)hw->dmaw_dly_cnt) & DMA_CTRL_DMAW_DLY_CNT_MASK)
-		<< DMA_CTRL_DMAW_DLY_CNT_SHIFT;
+	dma_ctrl_data = FIELDX(DMA_CTRL_RORDER_MODE, DMA_CTRL_RORDER_MODE_OUT) |
+		DMA_CTRL_RREQ_PRI_DATA |
+		FIELDX(DMA_CTRL_RREQ_BLEN, hw->dmar_block) |
+		FIELDX(DMA_CTRL_WDLY_CNT, DMA_CTRL_WDLY_CNT_DEF) |
+		FIELDX(DMA_CTRL_RDLY_CNT, DMA_CTRL_RDLY_CNT_DEF);
 
 	AT_WRITE_REG(hw, REG_DMA_CTRL, dma_ctrl_data);
 }
@@ -1230,52 +1171,53 @@ static int atl1c_stop_mac(struct atl1c_hw *hw)
 	u32 data;
 
 	AT_READ_REG(hw, REG_RXQ_CTRL, &data);
-	data &= ~(RXQ1_CTRL_EN | RXQ2_CTRL_EN |
-		  RXQ3_CTRL_EN | RXQ_CTRL_EN);
+	data &= ~RXQ_CTRL_EN;
 	AT_WRITE_REG(hw, REG_RXQ_CTRL, data);
 
 	AT_READ_REG(hw, REG_TXQ_CTRL, &data);
 	data &= ~TXQ_CTRL_EN;
-	AT_WRITE_REG(hw, REG_TWSI_CTRL, data);
+	AT_WRITE_REG(hw, REG_TXQ_CTRL, data);
 
-	atl1c_wait_until_idle(hw);
+	atl1c_wait_until_idle(hw, IDLE_STATUS_RXQ_BUSY | IDLE_STATUS_TXQ_BUSY);
 
 	AT_READ_REG(hw, REG_MAC_CTRL, &data);
 	data &= ~(MAC_CTRL_TX_EN | MAC_CTRL_RX_EN);
 	AT_WRITE_REG(hw, REG_MAC_CTRL, data);
 
-	return (int)atl1c_wait_until_idle(hw);
-}
-
-static void atl1c_enable_rx_ctrl(struct atl1c_hw *hw)
-{
-	u32 data;
-
-	AT_READ_REG(hw, REG_RXQ_CTRL, &data);
-	switch (hw->adapter->num_rx_queues) {
-	case 4:
-		data |= (RXQ3_CTRL_EN | RXQ2_CTRL_EN | RXQ1_CTRL_EN);
-		break;
-	case 3:
-		data |= (RXQ2_CTRL_EN | RXQ1_CTRL_EN);
-		break;
-	case 2:
-		data |= RXQ1_CTRL_EN;
-		break;
-	default:
-		break;
-	}
-	data |= RXQ_CTRL_EN;
-	AT_WRITE_REG(hw, REG_RXQ_CTRL, data);
+	return (int)atl1c_wait_until_idle(hw,
+		IDLE_STATUS_TXMAC_BUSY | IDLE_STATUS_RXMAC_BUSY);
 }
 
-static void atl1c_enable_tx_ctrl(struct atl1c_hw *hw)
+static void atl1c_start_mac(struct atl1c_adapter *adapter)
 {
-	u32 data;
+	struct atl1c_hw *hw = &adapter->hw;
+	u32 mac, txq, rxq;
+
+	hw->mac_duplex = adapter->link_duplex == FULL_DUPLEX ? true : false;
+	hw->mac_speed = adapter->link_speed == SPEED_1000 ?
+		atl1c_mac_speed_1000 : atl1c_mac_speed_10_100;
+
+	AT_READ_REG(hw, REG_TXQ_CTRL, &txq);
+	AT_READ_REG(hw, REG_RXQ_CTRL, &rxq);
+	AT_READ_REG(hw, REG_MAC_CTRL, &mac);
+
+	txq |= TXQ_CTRL_EN;
+	rxq |= RXQ_CTRL_EN;
+	mac |= MAC_CTRL_TX_EN | MAC_CTRL_TX_FLOW |
+	       MAC_CTRL_RX_EN | MAC_CTRL_RX_FLOW |
+	       MAC_CTRL_ADD_CRC | MAC_CTRL_PAD |
+	       MAC_CTRL_BC_EN | MAC_CTRL_SINGLE_PAUSE_EN |
+	       MAC_CTRL_HASH_ALG_CRC32;
+	if (hw->mac_duplex)
+		mac |= MAC_CTRL_DUPLX;
+	else
+		mac &= ~MAC_CTRL_DUPLX;
+	mac = FIELD_SETX(mac, MAC_CTRL_SPEED, hw->mac_speed);
+	mac = FIELD_SETX(mac, MAC_CTRL_PRMLEN, hw->preamble_len);
 
-	AT_READ_REG(hw, REG_TXQ_CTRL, &data);
-	data |= TXQ_CTRL_EN;
-	AT_WRITE_REG(hw, REG_TXQ_CTRL, data);
+	AT_WRITE_REG(hw, REG_TXQ_CTRL, txq);
+	AT_WRITE_REG(hw, REG_RXQ_CTRL, rxq);
+	AT_WRITE_REG(hw, REG_MAC_CTRL, mac);
 }
 
 /*
@@ -1287,10 +1229,7 @@ static int atl1c_reset_mac(struct atl1c_hw *hw)
 {
 	struct atl1c_adapter *adapter = (struct atl1c_adapter *)hw->adapter;
 	struct pci_dev *pdev = adapter->pdev;
-	u32 master_ctrl_data = 0;
-
-	AT_WRITE_REG(hw, REG_IMR, 0);
-	AT_WRITE_REG(hw, REG_ISR, ISR_DIS_INT);
+	u32 ctrl_data = 0;
 
 	atl1c_stop_mac(hw);
 	/*
@@ -1299,194 +1238,148 @@ static int atl1c_reset_mac(struct atl1c_hw *hw)
 	 * the current PCI configuration.  The global reset bit is self-
 	 * clearing, and should clear within a microsecond.
 	 */
-	AT_READ_REG(hw, REG_MASTER_CTRL, &master_ctrl_data);
-	master_ctrl_data |= MASTER_CTRL_OOB_DIS_OFF;
-	AT_WRITE_REGW(hw, REG_MASTER_CTRL, ((master_ctrl_data | MASTER_CTRL_SOFT_RST)
-			& 0xFFFF));
+	AT_READ_REG(hw, REG_MASTER_CTRL, &ctrl_data);
+	ctrl_data |= MASTER_CTRL_OOB_DIS;
+	AT_WRITE_REG(hw, REG_MASTER_CTRL, ctrl_data | MASTER_CTRL_SOFT_RST);
 
 	AT_WRITE_FLUSH(hw);
 	msleep(10);
 	/* Wait at least 10ms for All module to be Idle */
 
-	if (atl1c_wait_until_idle(hw)) {
+	if (atl1c_wait_until_idle(hw, IDLE_STATUS_MASK)) {
 		dev_err(&pdev->dev,
 			"MAC state machine can't be idle since"
 			" disabled for 10ms second\n");
 		return -1;
 	}
+	AT_WRITE_REG(hw, REG_MASTER_CTRL, ctrl_data);
+
+	/* driver control speed/duplex */
+	AT_READ_REG(hw, REG_MAC_CTRL, &ctrl_data);
+	AT_WRITE_REG(hw, REG_MAC_CTRL, ctrl_data | MAC_CTRL_SPEED_MODE_SW);
+
+	/* clk switch setting */
+	AT_READ_REG(hw, REG_SERDES, &ctrl_data);
+	switch (hw->nic_type) {
+	case athr_l2c_b:
+		ctrl_data &= ~(SERDES_PHY_CLK_SLOWDOWN |
+				SERDES_MAC_CLK_SLOWDOWN);
+		AT_WRITE_REG(hw, REG_SERDES, ctrl_data);
+		break;
+	case athr_l2c_b2:
+	case athr_l1d_2:
+		ctrl_data |= SERDES_PHY_CLK_SLOWDOWN | SERDES_MAC_CLK_SLOWDOWN;
+		AT_WRITE_REG(hw, REG_SERDES, ctrl_data);
+		break;
+	default:
+		break;
+	}
+
 	return 0;
 }
 
 static void atl1c_disable_l0s_l1(struct atl1c_hw *hw)
 {
-	u32 pm_ctrl_data;
+	u16 ctrl_flags = hw->ctrl_flags;
 
-	AT_READ_REG(hw, REG_PM_CTRL, &pm_ctrl_data);
-	pm_ctrl_data &= ~(PM_CTRL_L1_ENTRY_TIMER_MASK <<
-			PM_CTRL_L1_ENTRY_TIMER_SHIFT);
-	pm_ctrl_data &= ~PM_CTRL_CLK_SWH_L1;
-	pm_ctrl_data &= ~PM_CTRL_ASPM_L0S_EN;
-	pm_ctrl_data &= ~PM_CTRL_ASPM_L1_EN;
-	pm_ctrl_data &= ~PM_CTRL_MAC_ASPM_CHK;
-	pm_ctrl_data &= ~PM_CTRL_SERDES_PD_EX_L1;
-
-	pm_ctrl_data |= PM_CTRL_SERDES_BUDS_RX_L1_EN;
-	pm_ctrl_data |= PM_CTRL_SERDES_PLL_L1_EN;
-	pm_ctrl_data |=	PM_CTRL_SERDES_L1_EN;
-	AT_WRITE_REG(hw, REG_PM_CTRL, pm_ctrl_data);
+	hw->ctrl_flags &= ~(ATL1C_ASPM_L0S_SUPPORT | ATL1C_ASPM_L1_SUPPORT);
+	atl1c_set_aspm(hw, SPEED_0);
+	hw->ctrl_flags = ctrl_flags;
 }
 
 /*
  * Set ASPM state.
  * Enable/disable L0s/L1 depend on link state.
  */
-static void atl1c_set_aspm(struct atl1c_hw *hw, bool linkup)
+static void atl1c_set_aspm(struct atl1c_hw *hw, u16 link_speed)
 {
 	u32 pm_ctrl_data;
-	u32 link_ctrl_data;
-	u32 link_l1_timer = 0xF;
+	u32 link_l1_timer;
 
 	AT_READ_REG(hw, REG_PM_CTRL, &pm_ctrl_data);
-	AT_READ_REG(hw, REG_LINK_CTRL, &link_ctrl_data);
+	pm_ctrl_data &= ~(PM_CTRL_ASPM_L1_EN |
+			  PM_CTRL_ASPM_L0S_EN |
+			  PM_CTRL_MAC_ASPM_CHK);
+	/* L1 timer */
+	if (hw->nic_type == athr_l2c_b2 || hw->nic_type == athr_l1d_2) {
+		pm_ctrl_data &= ~PMCTRL_TXL1_AFTER_L0S;
+		link_l1_timer =
+			link_speed == SPEED_1000 || link_speed == SPEED_100 ?
+			L1D_PMCTRL_L1_ENTRY_TM_16US : 1;
+		pm_ctrl_data = FIELD_SETX(pm_ctrl_data,
+			L1D_PMCTRL_L1_ENTRY_TM, link_l1_timer);
+	} else {
+		link_l1_timer = hw->nic_type == athr_l2c_b ?
+			L2CB1_PM_CTRL_L1_ENTRY_TM : L1C_PM_CTRL_L1_ENTRY_TM;
+		if (link_speed != SPEED_1000 && link_speed != SPEED_100)
+			link_l1_timer = 1;
+		pm_ctrl_data = FIELD_SETX(pm_ctrl_data,
+			PM_CTRL_L1_ENTRY_TIMER, link_l1_timer);
+	}
 
-	pm_ctrl_data &= ~PM_CTRL_SERDES_PD_EX_L1;
-	pm_ctrl_data &=  ~(PM_CTRL_L1_ENTRY_TIMER_MASK <<
-			PM_CTRL_L1_ENTRY_TIMER_SHIFT);
-	pm_ctrl_data &= ~(PM_CTRL_LCKDET_TIMER_MASK <<
-			PM_CTRL_LCKDET_TIMER_SHIFT);
-	pm_ctrl_data |= AT_LCKDET_TIMER	<< PM_CTRL_LCKDET_TIMER_SHIFT;
+	/* L0S/L1 enable */
+	if ((hw->ctrl_flags & ATL1C_ASPM_L0S_SUPPORT) && link_speed != SPEED_0)
+		pm_ctrl_data |= PM_CTRL_ASPM_L0S_EN | PM_CTRL_MAC_ASPM_CHK;
+	if (hw->ctrl_flags & ATL1C_ASPM_L1_SUPPORT)
+		pm_ctrl_data |= PM_CTRL_ASPM_L1_EN | PM_CTRL_MAC_ASPM_CHK;
 
+	/* l2cb & l1d & l2cb2 & l1d2 */
 	if (hw->nic_type == athr_l2c_b || hw->nic_type == athr_l1d ||
-		hw->nic_type == athr_l2c_b2 || hw->nic_type == athr_l1d_2) {
-		link_ctrl_data &= ~LINK_CTRL_EXT_SYNC;
-		if (!(hw->ctrl_flags & ATL1C_APS_MODE_ENABLE)) {
-			if (hw->nic_type == athr_l2c_b && hw->revision_id == L2CB_V10)
-				link_ctrl_data |= LINK_CTRL_EXT_SYNC;
-		}
-
-		AT_WRITE_REG(hw, REG_LINK_CTRL, link_ctrl_data);
-
-		pm_ctrl_data |= PM_CTRL_RCVR_WT_TIMER;
-		pm_ctrl_data &= ~(PM_CTRL_PM_REQ_TIMER_MASK <<
-			PM_CTRL_PM_REQ_TIMER_SHIFT);
-		pm_ctrl_data |= AT_ASPM_L1_TIMER <<
-			PM_CTRL_PM_REQ_TIMER_SHIFT;
-		pm_ctrl_data &= ~PM_CTRL_SA_DLY_EN;
-		pm_ctrl_data &= ~PM_CTRL_HOTRST;
-		pm_ctrl_data |= 1 << PM_CTRL_L1_ENTRY_TIMER_SHIFT;
-		pm_ctrl_data |= PM_CTRL_SERDES_PD_EX_L1;
-	}
-	pm_ctrl_data |= PM_CTRL_MAC_ASPM_CHK;
-	if (linkup) {
-		pm_ctrl_data &= ~PM_CTRL_ASPM_L1_EN;
-		pm_ctrl_data &= ~PM_CTRL_ASPM_L0S_EN;
-		if (hw->ctrl_flags & ATL1C_ASPM_L1_SUPPORT)
-			pm_ctrl_data |= PM_CTRL_ASPM_L1_EN;
-		if (hw->ctrl_flags & ATL1C_ASPM_L0S_SUPPORT)
-			pm_ctrl_data |= PM_CTRL_ASPM_L0S_EN;
-
-		if (hw->nic_type == athr_l2c_b || hw->nic_type == athr_l1d ||
-			hw->nic_type == athr_l2c_b2 || hw->nic_type == athr_l1d_2) {
-			if (hw->nic_type == athr_l2c_b)
-				if (!(hw->ctrl_flags & ATL1C_APS_MODE_ENABLE))
-					pm_ctrl_data &= ~PM_CTRL_ASPM_L0S_EN;
-			pm_ctrl_data &= ~PM_CTRL_SERDES_L1_EN;
-			pm_ctrl_data &= ~PM_CTRL_SERDES_PLL_L1_EN;
-			pm_ctrl_data &= ~PM_CTRL_SERDES_BUDS_RX_L1_EN;
-			pm_ctrl_data |= PM_CTRL_CLK_SWH_L1;
-		if (hw->adapter->link_speed == SPEED_100 ||
-				hw->adapter->link_speed == SPEED_1000) {
-				pm_ctrl_data &=  ~(PM_CTRL_L1_ENTRY_TIMER_MASK <<
-					PM_CTRL_L1_ENTRY_TIMER_SHIFT);
-				if (hw->nic_type == athr_l2c_b)
-					link_l1_timer = 7;
-				else if (hw->nic_type == athr_l2c_b2 ||
-					hw->nic_type == athr_l1d_2)
-					link_l1_timer = 4;
-				pm_ctrl_data |= link_l1_timer <<
-					PM_CTRL_L1_ENTRY_TIMER_SHIFT;
-			}
-		} else {
-			pm_ctrl_data |= PM_CTRL_SERDES_L1_EN;
-			pm_ctrl_data |= PM_CTRL_SERDES_PLL_L1_EN;
-			pm_ctrl_data |= PM_CTRL_SERDES_BUDS_RX_L1_EN;
-			pm_ctrl_data &= ~PM_CTRL_CLK_SWH_L1;
+	    hw->nic_type == athr_l2c_b2 || hw->nic_type == athr_l1d_2) {
+		pm_ctrl_data = FIELD_SETX(pm_ctrl_data,
+			PM_CTRL_PM_REQ_TIMER, PM_CTRL_PM_REQ_TO_DEF);
+		pm_ctrl_data |= PM_CTRL_RCVR_WT_TIMER |
+				PM_CTRL_SERDES_PD_EX_L1 |
+				PM_CTRL_CLK_SWH_L1;
+		pm_ctrl_data &= ~(PM_CTRL_SERDES_L1_EN |
+				  PM_CTRL_SERDES_PLL_L1_EN |
+				  PM_CTRL_SERDES_BUFS_RX_L1_EN |
+				  PM_CTRL_SA_DLY_EN |
+				  PM_CTRL_HOTRST);
+		/* disable l0s if link down or l2cb */
+		if (link_speed == SPEED_0 || hw->nic_type == athr_l2c_b)
 			pm_ctrl_data &= ~PM_CTRL_ASPM_L0S_EN;
-			pm_ctrl_data &= ~PM_CTRL_ASPM_L1_EN;
-
+	} else { /* l1c */
+		pm_ctrl_data =
+			FIELD_SETX(pm_ctrl_data, PM_CTRL_L1_ENTRY_TIMER, 0);
+		if (link_speed != SPEED_0) {
+			pm_ctrl_data |= PM_CTRL_SERDES_L1_EN |
+					PM_CTRL_SERDES_PLL_L1_EN |
+					PM_CTRL_SERDES_BUFS_RX_L1_EN;
+			pm_ctrl_data &= ~(PM_CTRL_SERDES_PD_EX_L1 |
+					  PM_CTRL_CLK_SWH_L1 |
+					  PM_CTRL_ASPM_L0S_EN |
+					  PM_CTRL_ASPM_L1_EN);
+		} else { /* link down */
+			pm_ctrl_data |= PM_CTRL_CLK_SWH_L1;
+			pm_ctrl_data &= ~(PM_CTRL_SERDES_L1_EN |
+					  PM_CTRL_SERDES_PLL_L1_EN |
+					  PM_CTRL_SERDES_BUFS_RX_L1_EN |
+					  PM_CTRL_ASPM_L0S_EN);
 		}
-	} else {
-		pm_ctrl_data &= ~PM_CTRL_SERDES_L1_EN;
-		pm_ctrl_data &= ~PM_CTRL_ASPM_L0S_EN;
-		pm_ctrl_data &= ~PM_CTRL_SERDES_PLL_L1_EN;
-		pm_ctrl_data |= PM_CTRL_CLK_SWH_L1;
-
-		if (hw->ctrl_flags & ATL1C_ASPM_L1_SUPPORT)
-			pm_ctrl_data |= PM_CTRL_ASPM_L1_EN;
-		else
-			pm_ctrl_data &= ~PM_CTRL_ASPM_L1_EN;
 	}
 	AT_WRITE_REG(hw, REG_PM_CTRL, pm_ctrl_data);
 
 	return;
 }
 
-static void atl1c_setup_mac_ctrl(struct atl1c_adapter *adapter)
-{
-	struct atl1c_hw *hw = &adapter->hw;
-	struct net_device *netdev = adapter->netdev;
-	u32 mac_ctrl_data;
-
-	mac_ctrl_data = MAC_CTRL_TX_EN | MAC_CTRL_RX_EN;
-	mac_ctrl_data |= (MAC_CTRL_TX_FLOW | MAC_CTRL_RX_FLOW);
-
-	if (adapter->link_duplex == FULL_DUPLEX) {
-		hw->mac_duplex = true;
-		mac_ctrl_data |= MAC_CTRL_DUPLX;
-	}
-
-	if (adapter->link_speed == SPEED_1000)
-		hw->mac_speed = atl1c_mac_speed_1000;
-	else
-		hw->mac_speed = atl1c_mac_speed_10_100;
-
-	mac_ctrl_data |= (hw->mac_speed & MAC_CTRL_SPEED_MASK) <<
-			MAC_CTRL_SPEED_SHIFT;
-
-	mac_ctrl_data |= (MAC_CTRL_ADD_CRC | MAC_CTRL_PAD);
-	mac_ctrl_data |= ((hw->preamble_len & MAC_CTRL_PRMLEN_MASK) <<
-			MAC_CTRL_PRMLEN_SHIFT);
-
-	__atl1c_vlan_mode(netdev->features, &mac_ctrl_data);
-
-	mac_ctrl_data |= MAC_CTRL_BC_EN;
-	if (netdev->flags & IFF_PROMISC)
-		mac_ctrl_data |= MAC_CTRL_PROMIS_EN;
-	if (netdev->flags & IFF_ALLMULTI)
-		mac_ctrl_data |= MAC_CTRL_MC_ALL_EN;
-
-	mac_ctrl_data |= MAC_CTRL_SINGLE_PAUSE_EN;
-	if (hw->nic_type == athr_l1d || hw->nic_type == athr_l2c_b2 ||
-	    hw->nic_type == athr_l1d_2) {
-		mac_ctrl_data |= MAC_CTRL_SPEED_MODE_SW;
-		mac_ctrl_data |= MAC_CTRL_HASH_ALG_CRC32;
-	}
-	AT_WRITE_REG(hw, REG_MAC_CTRL, mac_ctrl_data);
-}
-
 /*
  * atl1c_configure - Configure Transmit&Receive Unit after Reset
  * @adapter: board private structure
  *
  * Configure the Tx /Rx unit of the MAC after a reset.
  */
-static int atl1c_configure(struct atl1c_adapter *adapter)
+static int atl1c_configure_mac(struct atl1c_adapter *adapter)
 {
 	struct atl1c_hw *hw = &adapter->hw;
 	u32 master_ctrl_data = 0;
 	u32 intr_modrt_data;
 	u32 data;
 
+	AT_READ_REG(hw, REG_MASTER_CTRL, &master_ctrl_data);
+	master_ctrl_data &= ~(MASTER_CTRL_TX_ITIMER_EN |
+			      MASTER_CTRL_RX_ITIMER_EN |
+			      MASTER_CTRL_INT_RDCLR);
 	/* clear interrupt status */
 	AT_WRITE_REG(hw, REG_ISR, 0xFFFFFFFF);
 	/*  Clear any WOL status */
@@ -1525,30 +1418,39 @@ static int atl1c_configure(struct atl1c_adapter *adapter)
 	master_ctrl_data |= MASTER_CTRL_SA_TIMER_EN;
 	AT_WRITE_REG(hw, REG_MASTER_CTRL, master_ctrl_data);
 
-	if (hw->ctrl_flags & ATL1C_CMB_ENABLE) {
-		AT_WRITE_REG(hw, REG_CMB_TPD_THRESH,
-			hw->cmb_tpd & CMB_TPD_THRESH_MASK);
-		AT_WRITE_REG(hw, REG_CMB_TX_TIMER,
-			hw->cmb_tx_timer & CMB_TX_TIMER_MASK);
-	}
+	AT_WRITE_REG(hw, REG_SMB_STAT_TIMER,
+		hw->smb_timer & SMB_STAT_TIMER_MASK);
 
-	if (hw->ctrl_flags & ATL1C_SMB_ENABLE)
-		AT_WRITE_REG(hw, REG_SMB_STAT_TIMER,
-			hw->smb_timer & SMB_STAT_TIMER_MASK);
 	/* set MTU */
 	AT_WRITE_REG(hw, REG_MTU, hw->max_frame_size + ETH_HLEN +
 			VLAN_HLEN + ETH_FCS_LEN);
-	/* HDS, disable */
-	AT_WRITE_REG(hw, REG_HDS_CTRL, 0);
 
 	atl1c_configure_tx(adapter);
 	atl1c_configure_rx(adapter);
-	atl1c_configure_rss(adapter);
 	atl1c_configure_dma(adapter);
 
 	return 0;
 }
 
+static int atl1c_configure(struct atl1c_adapter *adapter)
+{
+	struct net_device *netdev = adapter->netdev;
+	int num;
+
+	atl1c_init_ring_ptrs(adapter);
+	atl1c_set_multi(netdev);
+	atl1c_restore_vlan(adapter);
+
+	num = atl1c_alloc_rx_buffer(adapter);
+	if (unlikely(num == 0))
+		return -ENOMEM;
+
+	if (atl1c_configure_mac(adapter))
+		return -EIO;
+
+	return 0;
+}
+
 static void atl1c_update_hw_stats(struct atl1c_adapter *adapter)
 {
 	u16 hw_reg_addr = 0;
@@ -1635,16 +1537,11 @@ static bool atl1c_clean_tx_irq(struct atl1c_adapter *adapter,
 	struct pci_dev *pdev = adapter->pdev;
 	u16 next_to_clean = atomic_read(&tpd_ring->next_to_clean);
 	u16 hw_next_to_clean;
-	u16 shift;
-	u32 data;
+	u16 reg;
 
-	if (type == atl1c_trans_high)
-		shift = MB_HTPD_CONS_IDX_SHIFT;
-	else
-		shift = MB_NTPD_CONS_IDX_SHIFT;
+	reg = type == atl1c_trans_high ? REG_TPD_PRI1_CIDX : REG_TPD_PRI0_CIDX;
 
-	AT_READ_REG(&adapter->hw, REG_MB_PRIO_CONS_IDX, &data);
-	hw_next_to_clean = (data >> shift) & MB_PRIO_PROD_IDX_MASK;
+	AT_READ_REGW(&adapter->hw, reg, &hw_next_to_clean);
 
 	while (next_to_clean != hw_next_to_clean) {
 		buffer_info = &tpd_ring->buffer_info[next_to_clean];
@@ -1746,9 +1643,9 @@ static inline void atl1c_rx_checksum(struct atl1c_adapter *adapter,
 	skb_checksum_none_assert(skb);
 }
 
-static int atl1c_alloc_rx_buffer(struct atl1c_adapter *adapter, const int ringid)
+static int atl1c_alloc_rx_buffer(struct atl1c_adapter *adapter)
 {
-	struct atl1c_rfd_ring *rfd_ring = &adapter->rfd_ring[ringid];
+	struct atl1c_rfd_ring *rfd_ring = &adapter->rfd_ring;
 	struct pci_dev *pdev = adapter->pdev;
 	struct atl1c_buffer *buffer_info, *next_info;
 	struct sk_buff *skb;
@@ -1800,7 +1697,7 @@ static int atl1c_alloc_rx_buffer(struct atl1c_adapter *adapter, const int ringid
 		/* TODO: update mailbox here */
 		wmb();
 		rfd_ring->next_to_use = rfd_next_to_use;
-		AT_WRITE_REG(&adapter->hw, atl1c_rfd_prod_idx_regs[ringid],
+		AT_WRITE_REG(&adapter->hw, REG_MB_RFD0_PROD_IDX,
 			rfd_ring->next_to_use & MB_RFDX_PROD_IDX_MASK);
 	}
 
@@ -1839,7 +1736,7 @@ static void atl1c_clean_rfd(struct atl1c_rfd_ring *rfd_ring,
 	rfd_ring->next_to_clean = rfd_index;
 }
 
-static void atl1c_clean_rx_irq(struct atl1c_adapter *adapter, u8 que,
+static void atl1c_clean_rx_irq(struct atl1c_adapter *adapter,
 		   int *work_done, int work_to_do)
 {
 	u16 rfd_num, rfd_index;
@@ -1847,8 +1744,8 @@ static void atl1c_clean_rx_irq(struct atl1c_adapter *adapter, u8 que,
 	u16 length;
 	struct pci_dev *pdev = adapter->pdev;
 	struct net_device *netdev  = adapter->netdev;
-	struct atl1c_rfd_ring *rfd_ring = &adapter->rfd_ring[que];
-	struct atl1c_rrd_ring *rrd_ring = &adapter->rrd_ring[que];
+	struct atl1c_rfd_ring *rfd_ring = &adapter->rfd_ring;
+	struct atl1c_rrd_ring *rrd_ring = &adapter->rrd_ring;
 	struct sk_buff *skb;
 	struct atl1c_recv_ret_status *rrs;
 	struct atl1c_buffer *buffer_info;
@@ -1914,7 +1811,7 @@ rrs_checked:
 		count++;
 	}
 	if (count)
-		atl1c_alloc_rx_buffer(adapter, que);
+		atl1c_alloc_rx_buffer(adapter);
 }
 
 /*
@@ -1931,7 +1828,7 @@ static int atl1c_clean(struct napi_struct *napi, int budget)
 	if (!netif_carrier_ok(adapter->netdev))
 		goto quit_polling;
 	/* just enable one RXQ */
-	atl1c_clean_rx_irq(adapter, 0, &work_done, budget);
+	atl1c_clean_rx_irq(adapter, &work_done, budget);
 
 	if (work_done < budget) {
 quit_polling:
@@ -2206,23 +2103,10 @@ static void atl1c_tx_queue(struct atl1c_adapter *adapter, struct sk_buff *skb,
 			   struct atl1c_tpd_desc *tpd, enum atl1c_trans_queue type)
 {
 	struct atl1c_tpd_ring *tpd_ring = &adapter->tpd_ring[type];
-	u32 prod_data;
+	u16 reg;
 
-	AT_READ_REG(&adapter->hw, REG_MB_PRIO_PROD_IDX, &prod_data);
-	switch (type) {
-	case atl1c_trans_high:
-		prod_data &= 0xFFFF0000;
-		prod_data |= tpd_ring->next_to_use & 0xFFFF;
-		break;
-	case atl1c_trans_normal:
-		prod_data &= 0x0000FFFF;
-		prod_data |= (tpd_ring->next_to_use & 0xFFFF) << 16;
-		break;
-	default:
-		break;
-	}
-	wmb();
-	AT_WRITE_REG(&adapter->hw, REG_MB_PRIO_PROD_IDX, prod_data);
+	reg = type == atl1c_trans_high ? REG_TPD_PRI1_PIDX : REG_TPD_PRI0_PIDX;
+	AT_WRITE_REGW(&adapter->hw, reg, tpd_ring->next_to_use);
 }
 
 static netdev_tx_t atl1c_xmit_frame(struct sk_buff *skb,
@@ -2307,8 +2191,7 @@ static int atl1c_request_irq(struct atl1c_adapter *adapter)
 				"Unable to allocate MSI interrupt Error: %d\n",
 				err);
 		adapter->have_msi = false;
-	} else
-		netdev->irq = pdev->irq;
+	}
 
 	if (!adapter->have_msi)
 		flags |= IRQF_SHARED;
@@ -2328,44 +2211,38 @@ static int atl1c_request_irq(struct atl1c_adapter *adapter)
 	return err;
 }
 
+
+static void atl1c_reset_dma_ring(struct atl1c_adapter *adapter)
+{
+	/* release tx-pending skbs and reset tx/rx ring index */
+	atl1c_clean_tx_ring(adapter, atl1c_trans_normal);
+	atl1c_clean_tx_ring(adapter, atl1c_trans_high);
+	atl1c_clean_rx_ring(adapter);
+}
+
 static int atl1c_up(struct atl1c_adapter *adapter)
 {
 	struct net_device *netdev = adapter->netdev;
-	int num;
 	int err;
-	int i;
 
 	netif_carrier_off(netdev);
-	atl1c_init_ring_ptrs(adapter);
-	atl1c_set_multi(netdev);
-	atl1c_restore_vlan(adapter);
 
-	for (i = 0; i < adapter->num_rx_queues; i++) {
-		num = atl1c_alloc_rx_buffer(adapter, i);
-		if (unlikely(num == 0)) {
-			err = -ENOMEM;
-			goto err_alloc_rx;
-		}
-	}
-
-	if (atl1c_configure(adapter)) {
-		err = -EIO;
+	err = atl1c_configure(adapter);
+	if (unlikely(err))
 		goto err_up;
-	}
 
 	err = atl1c_request_irq(adapter);
 	if (unlikely(err))
 		goto err_up;
 
+	atl1c_check_link_status(adapter);
 	clear_bit(__AT_DOWN, &adapter->flags);
 	napi_enable(&adapter->napi);
 	atl1c_irq_enable(adapter);
-	atl1c_check_link_status(adapter);
 	netif_start_queue(netdev);
 	return err;
 
 err_up:
-err_alloc_rx:
 	atl1c_clean_rx_ring(adapter);
 	return err;
 }
@@ -2383,15 +2260,15 @@ static void atl1c_down(struct atl1c_adapter *adapter)
 	napi_disable(&adapter->napi);
 	atl1c_irq_disable(adapter);
 	atl1c_free_irq(adapter);
+	/* disable ASPM if device inactive */
+	atl1c_disable_l0s_l1(&adapter->hw);
 	/* reset MAC to disable all RX/TX */
 	atl1c_reset_mac(&adapter->hw);
 	msleep(1);
 
 	adapter->link_speed = SPEED_0;
 	adapter->link_duplex = -1;
-	atl1c_clean_tx_ring(adapter, atl1c_trans_normal);
-	atl1c_clean_tx_ring(adapter, atl1c_trans_high);
-	atl1c_clean_rx_ring(adapter);
+	atl1c_reset_dma_ring(adapter);
 }
 
 /*
@@ -2424,13 +2301,6 @@ static int atl1c_open(struct net_device *netdev)
 	if (unlikely(err))
 		goto err_up;
 
-	if (adapter->hw.ctrl_flags & ATL1C_FPGA_VERSION) {
-		u32 phy_data;
-
-		AT_READ_REG(&adapter->hw, REG_MDIO_CTRL, &phy_data);
-		phy_data |= MDIO_AP_EN;
-		AT_WRITE_REG(&adapter->hw, REG_MDIO_CTRL, phy_data);
-	}
 	return 0;
 
 err_up:
@@ -2456,6 +2326,8 @@ static int atl1c_close(struct net_device *netdev)
 	struct atl1c_adapter *adapter = netdev_priv(netdev);
 
 	WARN_ON(test_bit(__AT_RESETTING, &adapter->flags));
+	set_bit(__AT_DOWN, &adapter->flags);
+	cancel_work_sync(&adapter->common_task);
 	atl1c_down(adapter);
 	atl1c_free_ring_resources(adapter);
 	return 0;
@@ -2467,10 +2339,6 @@ static int atl1c_suspend(struct device *dev)
 	struct net_device *netdev = pci_get_drvdata(pdev);
 	struct atl1c_adapter *adapter = netdev_priv(netdev);
 	struct atl1c_hw *hw = &adapter->hw;
-	u32 mac_ctrl_data = 0;
-	u32 master_ctrl_data = 0;
-	u32 wol_ctrl_data = 0;
-	u16 mii_intr_status_data = 0;
 	u32 wufc = adapter->wol;
 
 	atl1c_disable_l0s_l1(hw);
@@ -2481,75 +2349,10 @@ static int atl1c_suspend(struct device *dev)
 	netif_device_detach(netdev);
 
 	if (wufc)
-		if (atl1c_phy_power_saving(hw) != 0)
+		if (atl1c_phy_to_ps_link(hw) != 0)
 			dev_dbg(&pdev->dev, "phy power saving failed");
 
-	AT_READ_REG(hw, REG_MASTER_CTRL, &master_ctrl_data);
-	AT_READ_REG(hw, REG_MAC_CTRL, &mac_ctrl_data);
-
-	master_ctrl_data &= ~MASTER_CTRL_CLK_SEL_DIS;
-	mac_ctrl_data &= ~(MAC_CTRL_PRMLEN_MASK << MAC_CTRL_PRMLEN_SHIFT);
-	mac_ctrl_data |= (((u32)adapter->hw.preamble_len &
-			MAC_CTRL_PRMLEN_MASK) <<
-			MAC_CTRL_PRMLEN_SHIFT);
-	mac_ctrl_data &= ~(MAC_CTRL_SPEED_MASK << MAC_CTRL_SPEED_SHIFT);
-	mac_ctrl_data &= ~MAC_CTRL_DUPLX;
-
-	if (wufc) {
-		mac_ctrl_data |= MAC_CTRL_RX_EN;
-		if (adapter->link_speed == SPEED_1000 ||
-			adapter->link_speed == SPEED_0) {
-			mac_ctrl_data |= atl1c_mac_speed_1000 <<
-					MAC_CTRL_SPEED_SHIFT;
-			mac_ctrl_data |= MAC_CTRL_DUPLX;
-		} else
-			mac_ctrl_data |= atl1c_mac_speed_10_100 <<
-					MAC_CTRL_SPEED_SHIFT;
-
-		if (adapter->link_duplex == DUPLEX_FULL)
-			mac_ctrl_data |= MAC_CTRL_DUPLX;
-
-		/* turn on magic packet wol */
-		if (wufc & AT_WUFC_MAG)
-			wol_ctrl_data |= WOL_MAGIC_EN | WOL_MAGIC_PME_EN;
-
-		if (wufc & AT_WUFC_LNKC) {
-			wol_ctrl_data |=  WOL_LINK_CHG_EN | WOL_LINK_CHG_PME_EN;
-			/* only link up can wake up */
-			if (atl1c_write_phy_reg(hw, MII_IER, IER_LINK_UP) != 0) {
-				dev_dbg(&pdev->dev, "%s: read write phy "
-						  "register failed.\n",
-						  atl1c_driver_name);
-			}
-		}
-		/* clear phy interrupt */
-		atl1c_read_phy_reg(hw, MII_ISR, &mii_intr_status_data);
-		/* Config MAC Ctrl register */
-		__atl1c_vlan_mode(netdev->features, &mac_ctrl_data);
-
-		/* magic packet maybe Broadcast&multicast&Unicast frame */
-		if (wufc & AT_WUFC_MAG)
-			mac_ctrl_data |= MAC_CTRL_BC_EN;
-
-		dev_dbg(&pdev->dev,
-			"%s: suspend MAC=0x%x\n",
-			atl1c_driver_name, mac_ctrl_data);
-		AT_WRITE_REG(hw, REG_MASTER_CTRL, master_ctrl_data);
-		AT_WRITE_REG(hw, REG_WOL_CTRL, wol_ctrl_data);
-		AT_WRITE_REG(hw, REG_MAC_CTRL, mac_ctrl_data);
-
-		AT_WRITE_REG(hw, REG_GPHY_CTRL, GPHY_CTRL_DEFAULT |
-			GPHY_CTRL_EXT_RESET);
-	} else {
-		AT_WRITE_REG(hw, REG_GPHY_CTRL, GPHY_CTRL_POWER_SAVING);
-		master_ctrl_data |= MASTER_CTRL_CLK_SEL_DIS;
-		mac_ctrl_data |= atl1c_mac_speed_10_100 << MAC_CTRL_SPEED_SHIFT;
-		mac_ctrl_data |= MAC_CTRL_DUPLX;
-		AT_WRITE_REG(hw, REG_MASTER_CTRL, master_ctrl_data);
-		AT_WRITE_REG(hw, REG_MAC_CTRL, mac_ctrl_data);
-		AT_WRITE_REG(hw, REG_WOL_CTRL, 0);
-		hw->phy_configured = false; /* re-init PHY when resume */
-	}
+	atl1c_power_saving(hw, wufc);
 
 	return 0;
 }
@@ -2562,8 +2365,7 @@ static int atl1c_resume(struct device *dev)
 	struct atl1c_adapter *adapter = netdev_priv(netdev);
 
 	AT_WRITE_REG(&adapter->hw, REG_WOL_CTRL, 0);
-	atl1c_reset_pcie(&adapter->hw, ATL1C_PCIE_L0S_L1_DISABLE |
-			ATL1C_PCIE_PHY_RESET);
+	atl1c_reset_pcie(&adapter->hw, ATL1C_PCIE_L0S_L1_DISABLE);
 
 	atl1c_phy_reset(&adapter->hw);
 	atl1c_reset_mac(&adapter->hw);
@@ -2616,7 +2418,6 @@ static int atl1c_init_netdev(struct net_device *netdev, struct pci_dev *pdev)
 	SET_NETDEV_DEV(netdev, &pdev->dev);
 	pci_set_drvdata(pdev, netdev);
 
-	netdev->irq  = pdev->irq;
 	netdev->netdev_ops = &atl1c_netdev_ops;
 	netdev->watchdog_timeo = AT_TX_WATCHDOG;
 	atl1c_set_ethtool_ops(netdev);
@@ -2706,14 +2507,13 @@ static int __devinit atl1c_probe(struct pci_dev *pdev,
 		dev_err(&pdev->dev, "cannot map device registers\n");
 		goto err_ioremap;
 	}
-	netdev->base_addr = (unsigned long)adapter->hw.hw_addr;
 
 	/* init mii data */
 	adapter->mii.dev = netdev;
 	adapter->mii.mdio_read  = atl1c_mdio_read;
 	adapter->mii.mdio_write = atl1c_mdio_write;
 	adapter->mii.phy_id_mask = 0x1f;
-	adapter->mii.reg_num_mask = MDIO_REG_ADDR_MASK;
+	adapter->mii.reg_num_mask = MDIO_CTRL_REG_MASK;
 	netif_napi_add(netdev, &adapter->napi, atl1c_clean, 64);
 	setup_timer(&adapter->phy_config_timer, atl1c_phy_config,
 			(unsigned long)adapter);
@@ -2723,8 +2523,7 @@ static int __devinit atl1c_probe(struct pci_dev *pdev,
 		dev_err(&pdev->dev, "net device private data init failed\n");
 		goto err_sw_init;
 	}
-	atl1c_reset_pcie(&adapter->hw, ATL1C_PCIE_L0S_L1_DISABLE |
-			ATL1C_PCIE_PHY_RESET);
+	atl1c_reset_pcie(&adapter->hw, ATL1C_PCIE_L0S_L1_DISABLE);
 
 	/* Init GPHY as early as possible due to power saving issue  */
 	atl1c_phy_reset(&adapter->hw);
@@ -2752,7 +2551,7 @@ static int __devinit atl1c_probe(struct pci_dev *pdev,
 		dev_dbg(&pdev->dev, "mac address : %pM\n",
 			adapter->hw.mac_addr);
 
-	atl1c_hw_set_mac_addr(&adapter->hw);
+	atl1c_hw_set_mac_addr(&adapter->hw, adapter->hw.mac_addr);
 	INIT_WORK(&adapter->common_task, atl1c_common_task);
 	adapter->work_event = 0;
 	err = register_netdev(netdev);
@@ -2796,6 +2595,8 @@ static void __devexit atl1c_remove(struct pci_dev *pdev)
 	struct atl1c_adapter *adapter = netdev_priv(netdev);
 
 	unregister_netdev(netdev);
+	/* restore permanent address */
+	atl1c_hw_set_mac_addr(&adapter->hw, adapter->hw.perm_mac_addr);
 	atl1c_phy_disable(&adapter->hw);
 
 	iounmap(adapter->hw.hw_addr);
diff --git a/drivers/net/ethernet/atheros/atl1e/atl1e_main.c b/drivers/net/ethernet/atheros/atl1e/atl1e_main.c
index 93ff2b23128..1220e511ced 100644
--- a/drivers/net/ethernet/atheros/atl1e/atl1e_main.c
+++ b/drivers/net/ethernet/atheros/atl1e/atl1e_main.c
@@ -1883,27 +1883,24 @@ static int atl1e_request_irq(struct atl1e_adapter *adapter)
 	int err = 0;
 
 	adapter->have_msi = true;
-	err = pci_enable_msi(adapter->pdev);
+	err = pci_enable_msi(pdev);
 	if (err) {
-		netdev_dbg(adapter->netdev,
+		netdev_dbg(netdev,
 			   "Unable to allocate MSI interrupt Error: %d\n", err);
 		adapter->have_msi = false;
-	} else
-		netdev->irq = pdev->irq;
-
+	}
 
 	if (!adapter->have_msi)
 		flags |= IRQF_SHARED;
-	err = request_irq(adapter->pdev->irq, atl1e_intr, flags,
-			netdev->name, netdev);
+	err = request_irq(pdev->irq, atl1e_intr, flags, netdev->name, netdev);
 	if (err) {
 		netdev_dbg(adapter->netdev,
 			   "Unable to allocate interrupt Error: %d\n", err);
 		if (adapter->have_msi)
-			pci_disable_msi(adapter->pdev);
+			pci_disable_msi(pdev);
 		return err;
 	}
-	netdev_dbg(adapter->netdev, "atl1e_request_irq OK\n");
+	netdev_dbg(netdev, "atl1e_request_irq OK\n");
 	return err;
 }
 
@@ -2233,7 +2230,6 @@ static int atl1e_init_netdev(struct net_device *netdev, struct pci_dev *pdev)
 	SET_NETDEV_DEV(netdev, &pdev->dev);
 	pci_set_drvdata(pdev, netdev);
 
-	netdev->irq  = pdev->irq;
 	netdev->netdev_ops = &atl1e_netdev_ops;
 
 	netdev->watchdog_timeo = AT_TX_WATCHDOG;
@@ -2319,7 +2315,6 @@ static int __devinit atl1e_probe(struct pci_dev *pdev,
 		netdev_err(netdev, "cannot map device registers\n");
 		goto err_ioremap;
 	}
-	netdev->base_addr = (unsigned long)adapter->hw.hw_addr;
 
 	/* init mii data */
 	adapter->mii.dev = netdev;
diff --git a/drivers/net/ethernet/atheros/atlx/atl1.c b/drivers/net/ethernet/atheros/atlx/atl1.c
index c926857e820..5d10884e508 100644
--- a/drivers/net/ethernet/atheros/atlx/atl1.c
+++ b/drivers/net/ethernet/atheros/atlx/atl1.c
@@ -266,7 +266,7 @@ static s32 atl1_reset_hw(struct atl1_hw *hw)
 	 * interrupts & Clear any pending interrupt events
 	 */
 	/*
-	 * iowrite32(0, hw->hw_addr + REG_IMR);
+	 * atlx_irq_disable(adapter);
 	 * iowrite32(0xffffffff, hw->hw_addr + REG_ISR);
 	 */
 
@@ -1917,7 +1917,7 @@ next:
 	return num_alloc;
 }
 
-static void atl1_intr_rx(struct atl1_adapter *adapter)
+static int atl1_intr_rx(struct atl1_adapter *adapter, int budget)
 {
 	int i, count;
 	u16 length;
@@ -1933,7 +1933,7 @@ static void atl1_intr_rx(struct atl1_adapter *adapter)
 
 	rrd_next_to_clean = atomic_read(&rrd_ring->next_to_clean);
 
-	while (1) {
+	while (count < budget) {
 		rrd = ATL1_RRD_DESC(rrd_ring, rrd_next_to_clean);
 		i = 1;
 		if (likely(rrd->xsz.valid)) {	/* packet valid */
@@ -2032,7 +2032,7 @@ rrd_ok:
 
 			__vlan_hwaccel_put_tag(skb, vlan_tag);
 		}
-		netif_rx(skb);
+		netif_receive_skb(skb);
 
 		/* let protocol layer free skb */
 		buffer_info->skb = NULL;
@@ -2065,14 +2065,17 @@ rrd_ok:
 		iowrite32(value, adapter->hw.hw_addr + REG_MAILBOX);
 		spin_unlock(&adapter->mb_lock);
 	}
+
+	return count;
 }
 
-static void atl1_intr_tx(struct atl1_adapter *adapter)
+static int atl1_intr_tx(struct atl1_adapter *adapter)
 {
 	struct atl1_tpd_ring *tpd_ring = &adapter->tpd_ring;
 	struct atl1_buffer *buffer_info;
 	u16 sw_tpd_next_to_clean;
 	u16 cmb_tpd_next_to_clean;
+	int count = 0;
 
 	sw_tpd_next_to_clean = atomic_read(&tpd_ring->next_to_clean);
 	cmb_tpd_next_to_clean = le16_to_cpu(adapter->cmb.cmb->tpd_cons_idx);
@@ -2092,12 +2095,16 @@ static void atl1_intr_tx(struct atl1_adapter *adapter)
 
 		if (++sw_tpd_next_to_clean == tpd_ring->count)
 			sw_tpd_next_to_clean = 0;
+
+		count++;
 	}
 	atomic_set(&tpd_ring->next_to_clean, sw_tpd_next_to_clean);
 
 	if (netif_queue_stopped(adapter->netdev) &&
 	    netif_carrier_ok(adapter->netdev))
 		netif_wake_queue(adapter->netdev);
+
+	return count;
 }
 
 static u16 atl1_tpd_avail(struct atl1_tpd_ring *tpd_ring)
@@ -2439,6 +2446,49 @@ static netdev_tx_t atl1_xmit_frame(struct sk_buff *skb,
 	return NETDEV_TX_OK;
 }
 
+static int atl1_rings_clean(struct napi_struct *napi, int budget)
+{
+	struct atl1_adapter *adapter = container_of(napi, struct atl1_adapter, napi);
+	int work_done = atl1_intr_rx(adapter, budget);
+
+	if (atl1_intr_tx(adapter))
+		work_done = budget;
+
+	/* Let's come again to process some more packets */
+	if (work_done >= budget)
+		return work_done;
+
+	napi_complete(napi);
+	/* re-enable Interrupt */
+	if (likely(adapter->int_enabled))
+		atlx_imr_set(adapter, IMR_NORMAL_MASK);
+	return work_done;
+}
+
+static inline int atl1_sched_rings_clean(struct atl1_adapter* adapter)
+{
+	if (!napi_schedule_prep(&adapter->napi))
+		/* It is possible in case even the RX/TX ints are disabled via IMR
+		 * register the ISR bits are set anyway (but do not produce IRQ).
+		 * To handle such situation the napi functions used to check is
+		 * something scheduled or not.
+		 */
+		return 0;
+
+	__napi_schedule(&adapter->napi);
+
+	/*
+	 * Disable RX/TX ints via IMR register if it is
+	 * allowed. NAPI handler must reenable them in same
+	 * way.
+	 */
+	if (!adapter->int_enabled)
+		return 1;
+
+	atlx_imr_set(adapter, IMR_NORXTX_MASK);
+	return 1;
+}
+
 /*
  * atl1_intr - Interrupt Handler
  * @irq: interrupt number
@@ -2449,78 +2499,74 @@ static irqreturn_t atl1_intr(int irq, void *data)
 {
 	struct atl1_adapter *adapter = netdev_priv(data);
 	u32 status;
-	int max_ints = 10;
 
 	status = adapter->cmb.cmb->int_stats;
 	if (!status)
 		return IRQ_NONE;
 
-	do {
-		/* clear CMB interrupt status at once */
-		adapter->cmb.cmb->int_stats = 0;
-
-		if (status & ISR_GPHY)	/* clear phy status */
-			atlx_clear_phy_int(adapter);
+	/* clear CMB interrupt status at once,
+	 * but leave rx/tx interrupt status in case it should be dropped
+	 * only if rx/tx processing queued. In other case interrupt
+	 * can be lost.
+	 */
+	adapter->cmb.cmb->int_stats = status & (ISR_CMB_TX | ISR_CMB_RX);
 
-		/* clear ISR status, and Enable CMB DMA/Disable Interrupt */
-		iowrite32(status | ISR_DIS_INT, adapter->hw.hw_addr + REG_ISR);
+	if (status & ISR_GPHY)	/* clear phy status */
+		atlx_clear_phy_int(adapter);
 
-		/* check if SMB intr */
-		if (status & ISR_SMB)
-			atl1_inc_smb(adapter);
+	/* clear ISR status, and Enable CMB DMA/Disable Interrupt */
+	iowrite32(status | ISR_DIS_INT, adapter->hw.hw_addr + REG_ISR);
 
-		/* check if PCIE PHY Link down */
-		if (status & ISR_PHY_LINKDOWN) {
-			if (netif_msg_intr(adapter))
-				dev_printk(KERN_DEBUG, &adapter->pdev->dev,
-					"pcie phy link down %x\n", status);
-			if (netif_running(adapter->netdev)) {	/* reset MAC */
-				iowrite32(0, adapter->hw.hw_addr + REG_IMR);
-				schedule_work(&adapter->reset_dev_task);
-				return IRQ_HANDLED;
-			}
-		}
+	/* check if SMB intr */
+	if (status & ISR_SMB)
+		atl1_inc_smb(adapter);
 
-		/* check if DMA read/write error ? */
-		if (status & (ISR_DMAR_TO_RST | ISR_DMAW_TO_RST)) {
-			if (netif_msg_intr(adapter))
-				dev_printk(KERN_DEBUG, &adapter->pdev->dev,
-					"pcie DMA r/w error (status = 0x%x)\n",
-					status);
-			iowrite32(0, adapter->hw.hw_addr + REG_IMR);
+	/* check if PCIE PHY Link down */
+	if (status & ISR_PHY_LINKDOWN) {
+		if (netif_msg_intr(adapter))
+			dev_printk(KERN_DEBUG, &adapter->pdev->dev,
+				"pcie phy link down %x\n", status);
+		if (netif_running(adapter->netdev)) {	/* reset MAC */
+			atlx_irq_disable(adapter);
 			schedule_work(&adapter->reset_dev_task);
 			return IRQ_HANDLED;
 		}
+	}
 
-		/* link event */
-		if (status & ISR_GPHY) {
-			adapter->soft_stats.tx_carrier_errors++;
-			atl1_check_for_link(adapter);
-		}
+	/* check if DMA read/write error ? */
+	if (status & (ISR_DMAR_TO_RST | ISR_DMAW_TO_RST)) {
+		if (netif_msg_intr(adapter))
+			dev_printk(KERN_DEBUG, &adapter->pdev->dev,
+				"pcie DMA r/w error (status = 0x%x)\n",
+				status);
+		atlx_irq_disable(adapter);
+		schedule_work(&adapter->reset_dev_task);
+		return IRQ_HANDLED;
+	}
 
-		/* transmit event */
-		if (status & ISR_CMB_TX)
-			atl1_intr_tx(adapter);
-
-		/* rx exception */
-		if (unlikely(status & (ISR_RXF_OV | ISR_RFD_UNRUN |
-			ISR_RRD_OV | ISR_HOST_RFD_UNRUN |
-			ISR_HOST_RRD_OV | ISR_CMB_RX))) {
-			if (status & (ISR_RXF_OV | ISR_RFD_UNRUN |
-				ISR_RRD_OV | ISR_HOST_RFD_UNRUN |
-				ISR_HOST_RRD_OV))
-				if (netif_msg_intr(adapter))
-					dev_printk(KERN_DEBUG,
-						&adapter->pdev->dev,
-						"rx exception, ISR = 0x%x\n",
-						status);
-			atl1_intr_rx(adapter);
-		}
+	/* link event */
+	if (status & ISR_GPHY) {
+		adapter->soft_stats.tx_carrier_errors++;
+		atl1_check_for_link(adapter);
+	}
 
-		if (--max_ints < 0)
-			break;
+	/* transmit or receive event */
+	if (status & (ISR_CMB_TX | ISR_CMB_RX) &&
+	    atl1_sched_rings_clean(adapter))
+		adapter->cmb.cmb->int_stats = adapter->cmb.cmb->int_stats &
+					      ~(ISR_CMB_TX | ISR_CMB_RX);
 
-	} while ((status = adapter->cmb.cmb->int_stats));
+	/* rx exception */
+	if (unlikely(status & (ISR_RXF_OV | ISR_RFD_UNRUN |
+		ISR_RRD_OV | ISR_HOST_RFD_UNRUN |
+		ISR_HOST_RRD_OV))) {
+		if (netif_msg_intr(adapter))
+			dev_printk(KERN_DEBUG,
+				&adapter->pdev->dev,
+				"rx exception, ISR = 0x%x\n",
+				status);
+		atl1_sched_rings_clean(adapter);
+	}
 
 	/* re-enable Interrupt */
 	iowrite32(ISR_DIS_SMB | ISR_DIS_DMA, adapter->hw.hw_addr + REG_ISR);
@@ -2599,6 +2645,7 @@ static s32 atl1_up(struct atl1_adapter *adapter)
 	if (unlikely(err))
 		goto err_up;
 
+	napi_enable(&adapter->napi);
 	atlx_irq_enable(adapter);
 	atl1_check_link(adapter);
 	netif_start_queue(netdev);
@@ -2615,6 +2662,7 @@ static void atl1_down(struct atl1_adapter *adapter)
 {
 	struct net_device *netdev = adapter->netdev;
 
+	napi_disable(&adapter->napi);
 	netif_stop_queue(netdev);
 	del_timer_sync(&adapter->phy_config_timer);
 	adapter->phy_timer_pending = false;
@@ -2971,6 +3019,7 @@ static int __devinit atl1_probe(struct pci_dev *pdev,
 
 	netdev->netdev_ops = &atl1_netdev_ops;
 	netdev->watchdog_timeo = 5 * HZ;
+	netif_napi_add(netdev, &adapter->napi, atl1_rings_clean, 64);
 
 	netdev->ethtool_ops = &atl1_ethtool_ops;
 	adapter->bd_number = cards_found;
diff --git a/drivers/net/ethernet/atheros/atlx/atl1.h b/drivers/net/ethernet/atheros/atlx/atl1.h
index e04bf4d71e4..3bf79a56220 100644
--- a/drivers/net/ethernet/atheros/atlx/atl1.h
+++ b/drivers/net/ethernet/atheros/atlx/atl1.h
@@ -275,13 +275,17 @@ static u32 atl1_check_link(struct atl1_adapter *adapter);
 #define ISR_DIS_SMB				0x20000000
 #define ISR_DIS_DMA				0x40000000
 
-/* Normal Interrupt mask  */
-#define IMR_NORMAL_MASK	(\
+/* Normal Interrupt mask without RX/TX enabled */
+#define IMR_NORXTX_MASK	(\
 	ISR_SMB		|\
 	ISR_GPHY	|\
 	ISR_PHY_LINKDOWN|\
 	ISR_DMAR_TO_RST	|\
-	ISR_DMAW_TO_RST	|\
+	ISR_DMAW_TO_RST)
+
+/* Normal Interrupt mask  */
+#define IMR_NORMAL_MASK	(\
+	IMR_NORXTX_MASK	|\
 	ISR_CMB_TX	|\
 	ISR_CMB_RX)
 
@@ -758,6 +762,7 @@ struct atl1_adapter {
 	u16 link_speed;
 	u16 link_duplex;
 	spinlock_t lock;
+	struct napi_struct napi;
 	struct work_struct reset_dev_task;
 	struct work_struct link_chg_task;
 
@@ -781,6 +786,12 @@ struct atl1_adapter {
 	u16 ict;		/* interrupt clear timer (2us resolution */
 	struct mii_if_info mii;	/* MII interface info */
 
+	/*
+	 * Use this value to check is napi handler allowed to
+	 * enable ints or not
+	 */
+	bool int_enabled;
+
 	u32 bd_number;		/* board number */
 	bool pci_using_64;
 	struct atl1_hw hw;
diff --git a/drivers/net/ethernet/atheros/atlx/atlx.c b/drivers/net/ethernet/atheros/atlx/atlx.c
index c9e9dc57986..b4f3aa49a7f 100644
--- a/drivers/net/ethernet/atheros/atlx/atlx.c
+++ b/drivers/net/ethernet/atheros/atlx/atlx.c
@@ -155,14 +155,21 @@ static void atlx_set_multi(struct net_device *netdev)
 	}
 }
 
+static inline void atlx_imr_set(struct atlx_adapter *adapter,
+				unsigned int imr)
+{
+	iowrite32(imr, adapter->hw.hw_addr + REG_IMR);
+	ioread32(adapter->hw.hw_addr + REG_IMR);
+}
+
 /*
  * atlx_irq_enable - Enable default interrupt generation settings
  * @adapter: board private structure
  */
 static void atlx_irq_enable(struct atlx_adapter *adapter)
 {
-	iowrite32(IMR_NORMAL_MASK, adapter->hw.hw_addr + REG_IMR);
-	ioread32(adapter->hw.hw_addr + REG_IMR);
+	atlx_imr_set(adapter, IMR_NORMAL_MASK);
+	adapter->int_enabled = true;
 }
 
 /*
@@ -171,8 +178,8 @@ static void atlx_irq_enable(struct atlx_adapter *adapter)
  */
 static void atlx_irq_disable(struct atlx_adapter *adapter)
 {
-	iowrite32(0, adapter->hw.hw_addr + REG_IMR);
-	ioread32(adapter->hw.hw_addr + REG_IMR);
+	adapter->int_enabled = false;
+	atlx_imr_set(adapter, 0);
 	synchronize_irq(adapter->pdev->irq);
 }
 
diff --git a/drivers/net/ethernet/broadcom/bnx2.c b/drivers/net/ethernet/broadcom/bnx2.c
index 8297e286873..ac7b7448853 100644
--- a/drivers/net/ethernet/broadcom/bnx2.c
+++ b/drivers/net/ethernet/broadcom/bnx2.c
@@ -3006,7 +3006,7 @@ error:
 
 	dma_unmap_single(&bp->pdev->dev, dma_addr, bp->rx_buf_use_size,
 			 PCI_DMA_FROMDEVICE);
-	skb = build_skb(data);
+	skb = build_skb(data, 0);
 	if (!skb) {
 		kfree(data);
 		goto error;
@@ -7343,8 +7343,7 @@ static struct {
 	{ "rx_fw_discards" },
 };
 
-#define BNX2_NUM_STATS (sizeof(bnx2_stats_str_arr)/\
-			sizeof(bnx2_stats_str_arr[0]))
+#define BNX2_NUM_STATS ARRAY_SIZE(bnx2_stats_str_arr)
 
 #define STATS_OFFSET32(offset_name) (offsetof(struct statistics_block, offset_name) / 4)
 
@@ -7976,7 +7975,6 @@ static int __devinit
 bnx2_init_board(struct pci_dev *pdev, struct net_device *dev)
 {
 	struct bnx2 *bp;
-	unsigned long mem_len;
 	int rc, i, j;
 	u32 reg;
 	u64 dma_mask, persist_dma_mask;
@@ -8036,13 +8034,8 @@ bnx2_init_board(struct pci_dev *pdev, struct net_device *dev)
 #endif
 	INIT_WORK(&bp->reset_task, bnx2_reset_task);
 
-	dev->base_addr = dev->mem_start = pci_resource_start(pdev, 0);
-	mem_len = MB_GET_CID_ADDR(TX_TSS_CID + TX_MAX_TSS_RINGS + 1);
-	dev->mem_end = dev->mem_start + mem_len;
-	dev->irq = pdev->irq;
-
-	bp->regview = ioremap_nocache(dev->base_addr, mem_len);
-
+	bp->regview = pci_iomap(pdev, 0, MB_GET_CID_ADDR(TX_TSS_CID +
+							 TX_MAX_TSS_RINGS + 1));
 	if (!bp->regview) {
 		dev_err(&pdev->dev, "Cannot map register space, aborting\n");
 		rc = -ENOMEM;
@@ -8346,10 +8339,8 @@ err_out_unmap:
 		bp->flags &= ~BNX2_FLAG_AER_ENABLED;
 	}
 
-	if (bp->regview) {
-		iounmap(bp->regview);
-		bp->regview = NULL;
-	}
+	pci_iounmap(pdev, bp->regview);
+	bp->regview = NULL;
 
 err_out_release:
 	pci_release_regions(pdev);
@@ -8432,7 +8423,7 @@ static int __devinit
 bnx2_init_one(struct pci_dev *pdev, const struct pci_device_id *ent)
 {
 	static int version_printed = 0;
-	struct net_device *dev = NULL;
+	struct net_device *dev;
 	struct bnx2 *bp;
 	int rc;
 	char str[40];
@@ -8442,15 +8433,12 @@ bnx2_init_one(struct pci_dev *pdev, const struct pci_device_id *ent)
 
 	/* dev zeroed in init_etherdev */
 	dev = alloc_etherdev_mq(sizeof(*bp), TX_MAX_RINGS);
-
 	if (!dev)
 		return -ENOMEM;
 
 	rc = bnx2_init_board(pdev, dev);
-	if (rc < 0) {
-		free_netdev(dev);
-		return rc;
-	}
+	if (rc < 0)
+		goto err_free;
 
 	dev->netdev_ops = &bnx2_netdev_ops;
 	dev->watchdog_timeo = TX_TIMEOUT;
@@ -8480,22 +8468,21 @@ bnx2_init_one(struct pci_dev *pdev, const struct pci_device_id *ent)
 		goto error;
 	}
 
-	netdev_info(dev, "%s (%c%d) %s found at mem %lx, IRQ %d, node addr %pM\n",
-		    board_info[ent->driver_data].name,
+	netdev_info(dev, "%s (%c%d) %s found at mem %lx, IRQ %d, "
+		    "node addr %pM\n", board_info[ent->driver_data].name,
 		    ((CHIP_ID(bp) & 0xf000) >> 12) + 'A',
 		    ((CHIP_ID(bp) & 0x0ff0) >> 4),
-		    bnx2_bus_string(bp, str),
-		    dev->base_addr,
-		    bp->pdev->irq, dev->dev_addr);
+		    bnx2_bus_string(bp, str), (long)pci_resource_start(pdev, 0),
+		    pdev->irq, dev->dev_addr);
 
 	return 0;
 
 error:
-	if (bp->regview)
-		iounmap(bp->regview);
+	iounmap(bp->regview);
 	pci_release_regions(pdev);
 	pci_disable_device(pdev);
 	pci_set_drvdata(pdev, NULL);
+err_free:
 	free_netdev(dev);
 	return rc;
 }
@@ -8511,8 +8498,7 @@ bnx2_remove_one(struct pci_dev *pdev)
 	del_timer_sync(&bp->timer);
 	cancel_work_sync(&bp->reset_task);
 
-	if (bp->regview)
-		iounmap(bp->regview);
+	pci_iounmap(bp->pdev, bp->regview);
 
 	kfree(bp->temp_stats_blk);
 
diff --git a/drivers/net/ethernet/broadcom/bnx2x/bnx2x.h b/drivers/net/ethernet/broadcom/bnx2x/bnx2x.h
index 2c9ee552dff..e30e2a2f354 100644
--- a/drivers/net/ethernet/broadcom/bnx2x/bnx2x.h
+++ b/drivers/net/ethernet/broadcom/bnx2x/bnx2x.h
@@ -23,13 +23,17 @@
  * (you will need to reboot afterwards) */
 /* #define BNX2X_STOP_ON_ERROR */
 
-#define DRV_MODULE_VERSION      "1.72.10-0"
-#define DRV_MODULE_RELDATE      "2012/02/20"
+#define DRV_MODULE_VERSION      "1.72.50-0"
+#define DRV_MODULE_RELDATE      "2012/04/23"
 #define BNX2X_BC_VER            0x040200
 
 #if defined(CONFIG_DCB)
 #define BCM_DCBNL
 #endif
+
+
+#include "bnx2x_hsi.h"
+
 #if defined(CONFIG_CNIC) || defined(CONFIG_CNIC_MODULE)
 #define BCM_CNIC 1
 #include "../cnic_if.h"
@@ -345,7 +349,6 @@ union db_prod {
 #define SGE_PAGE_SIZE		PAGE_SIZE
 #define SGE_PAGE_SHIFT		PAGE_SHIFT
 #define SGE_PAGE_ALIGN(addr)	PAGE_ALIGN((typeof(PAGE_SIZE))(addr))
-#define SGE_PAGES		(SGE_PAGE_SIZE * PAGES_PER_SGE)
 
 /* SGE ring related macros */
 #define NUM_RX_SGE_PAGES	2
@@ -815,6 +818,8 @@ struct bnx2x_common {
 #define CHIP_NUM_57800_MF		0x16a5
 #define CHIP_NUM_57810			0x168e
 #define CHIP_NUM_57810_MF		0x16ae
+#define CHIP_NUM_57811			0x163d
+#define CHIP_NUM_57811_MF		0x163e
 #define CHIP_NUM_57840			0x168d
 #define CHIP_NUM_57840_MF		0x16ab
 #define CHIP_IS_E1(bp)			(CHIP_NUM(bp) == CHIP_NUM_57710)
@@ -826,6 +831,8 @@ struct bnx2x_common {
 #define CHIP_IS_57800_MF(bp)		(CHIP_NUM(bp) == CHIP_NUM_57800_MF)
 #define CHIP_IS_57810(bp)		(CHIP_NUM(bp) == CHIP_NUM_57810)
 #define CHIP_IS_57810_MF(bp)		(CHIP_NUM(bp) == CHIP_NUM_57810_MF)
+#define CHIP_IS_57811(bp)		(CHIP_NUM(bp) == CHIP_NUM_57811)
+#define CHIP_IS_57811_MF(bp)		(CHIP_NUM(bp) == CHIP_NUM_57811_MF)
 #define CHIP_IS_57840(bp)		(CHIP_NUM(bp) == CHIP_NUM_57840)
 #define CHIP_IS_57840_MF(bp)		(CHIP_NUM(bp) == CHIP_NUM_57840_MF)
 #define CHIP_IS_E1H(bp)			(CHIP_IS_57711(bp) || \
@@ -836,6 +843,8 @@ struct bnx2x_common {
 					 CHIP_IS_57800_MF(bp) || \
 					 CHIP_IS_57810(bp) || \
 					 CHIP_IS_57810_MF(bp) || \
+					 CHIP_IS_57811(bp) || \
+					 CHIP_IS_57811_MF(bp) || \
 					 CHIP_IS_57840(bp) || \
 					 CHIP_IS_57840_MF(bp))
 #define CHIP_IS_E1x(bp)			(CHIP_IS_E1((bp)) || CHIP_IS_E1H((bp)))
@@ -1053,6 +1062,13 @@ struct bnx2x_slowpath {
 		struct flow_control_configuration pfc_config;
 	} func_rdata;
 
+	/* afex ramrod can not be a part of func_rdata union because these
+	 * events might arrive in parallel to other events from func_rdata.
+	 * Therefore, if they would have been defined in the same union,
+	 * data can get corrupted.
+	 */
+	struct afex_vif_list_ramrod_data func_afex_rdata;
+
 	/* used by dmae command executer */
 	struct dmae_command		dmae[MAX_DMAE_C];
 
@@ -1169,6 +1185,7 @@ struct bnx2x_fw_stats_data {
 enum {
 	BNX2X_SP_RTNL_SETUP_TC,
 	BNX2X_SP_RTNL_TX_TIMEOUT,
+	BNX2X_SP_RTNL_AFEX_F_UPDATE,
 	BNX2X_SP_RTNL_FAN_FAILURE,
 };
 
@@ -1222,7 +1239,6 @@ struct bnx2x {
 #define ETH_MAX_JUMBO_PACKET_SIZE	9600
 /* TCP with Timestamp Option (32) + IPv6 (40) */
 #define ETH_MAX_TPA_HEADER_SIZE		72
-#define ETH_MIN_TPA_HEADER_SIZE		40
 
 	/* Max supported alignment is 256 (8 shift) */
 #define BNX2X_RX_ALIGN_SHIFT		min(8, L1_CACHE_SHIFT)
@@ -1300,6 +1316,7 @@ struct bnx2x {
 #define NO_ISCSI_FLAG			(1 << 14)
 #define NO_FCOE_FLAG			(1 << 15)
 #define BC_SUPPORTS_PFC_STATS		(1 << 17)
+#define USING_SINGLE_MSIX_FLAG		(1 << 20)
 
 #define NO_ISCSI(bp)		((bp)->flags & NO_ISCSI_FLAG)
 #define NO_ISCSI_OOO(bp)	((bp)->flags & NO_ISCSI_OOO_FLAG)
@@ -1329,21 +1346,20 @@ struct bnx2x {
 	struct bnx2x_common	common;
 	struct bnx2x_port	port;
 
-	struct cmng_struct_per_port cmng;
-	u32			vn_weight_sum;
+	struct cmng_init	cmng;
+
 	u32			mf_config[E1HVN_MAX];
-	u32			mf2_config[E2_FUNC_MAX];
+	u32			mf_ext_config;
 	u32			path_has_ovlan; /* E3 */
 	u16			mf_ov;
 	u8			mf_mode;
 #define IS_MF(bp)		(bp->mf_mode != 0)
 #define IS_MF_SI(bp)		(bp->mf_mode == MULTI_FUNCTION_SI)
 #define IS_MF_SD(bp)		(bp->mf_mode == MULTI_FUNCTION_SD)
+#define IS_MF_AFEX(bp)		(bp->mf_mode == MULTI_FUNCTION_AFEX)
 
 	u8			wol;
 
-	bool			gro_check;
-
 	int			rx_ring_size;
 
 	u16			tx_quick_cons_trip_int;
@@ -1371,7 +1387,6 @@ struct bnx2x {
 #define BNX2X_STATE_DIAG		0xe000
 #define BNX2X_STATE_ERROR		0xf000
 
-	int			multi_mode;
 #define BNX2X_MAX_PRIORITY		8
 #define BNX2X_MAX_ENTRIES_PER_PRI	16
 #define BNX2X_MAX_COS			3
@@ -1582,6 +1597,9 @@ struct bnx2x {
 	struct dcbx_features			dcbx_remote_feat;
 	u32					dcbx_remote_flags;
 #endif
+	/* AFEX: store default vlan used */
+	int					afex_def_vlan_tag;
+	enum mf_cfg_afex_vlan_mode		afex_vlan_mode;
 	u32					pending_max;
 
 	/* multiple tx classes of service */
@@ -2138,9 +2156,16 @@ void bnx2x_notify_link_changed(struct bnx2x *bp);
 #define IS_MF_ISCSI_SD(bp) (IS_MF_SD(bp) && BNX2X_IS_MF_SD_PROTOCOL_ISCSI(bp))
 #define IS_MF_FCOE_SD(bp) (IS_MF_SD(bp) && BNX2X_IS_MF_SD_PROTOCOL_FCOE(bp))
 
+#define BNX2X_MF_EXT_PROTOCOL_FCOE(bp)  ((bp)->mf_ext_config & \
+					 MACP_FUNC_CFG_FLAGS_FCOE_OFFLOAD)
+
+#define IS_MF_FCOE_AFEX(bp) (IS_MF_AFEX(bp) && BNX2X_MF_EXT_PROTOCOL_FCOE(bp))
 #define IS_MF_STORAGE_SD(bp) (IS_MF_SD(bp) && \
 				(BNX2X_IS_MF_SD_PROTOCOL_ISCSI(bp) || \
 				 BNX2X_IS_MF_SD_PROTOCOL_FCOE(bp)))
+#else
+#define IS_MF_FCOE_AFEX(bp)	false
 #endif
 
+
 #endif /* bnx2x.h */
diff --git a/drivers/net/ethernet/broadcom/bnx2x/bnx2x_cmn.c b/drivers/net/ethernet/broadcom/bnx2x/bnx2x_cmn.c
index 4b054812713..ad0743bf4bd 100644
--- a/drivers/net/ethernet/broadcom/bnx2x/bnx2x_cmn.c
+++ b/drivers/net/ethernet/broadcom/bnx2x/bnx2x_cmn.c
@@ -23,7 +23,6 @@
 #include <linux/ip.h>
 #include <net/ipv6.h>
 #include <net/ip6_checksum.h>
-#include <linux/firmware.h>
 #include <linux/prefetch.h>
 #include "bnx2x_cmn.h"
 #include "bnx2x_init.h"
@@ -329,16 +328,6 @@ static void bnx2x_tpa_start(struct bnx2x_fastpath *fp, u16 queue,
 		u16 gro_size = le16_to_cpu(cqe->pkt_len_or_gro_seg_len);
 		tpa_info->full_page =
 			SGE_PAGE_SIZE * PAGES_PER_SGE / gro_size * gro_size;
-		/*
-		 * FW 7.2.16 BUG workaround:
-		 * if SGE size is (exactly) multiple gro_size
-		 * fw will place one less frag on SGE.
-		 * the calculation is done only for potentially
-		 * dangerous MTUs.
-		 */
-		if (unlikely(bp->gro_check))
-			if (!(SGE_PAGE_SIZE * PAGES_PER_SGE % gro_size))
-				tpa_info->full_page -= gro_size;
 		tpa_info->gro_size = gro_size;
 	}
 
@@ -369,8 +358,8 @@ static void bnx2x_tpa_start(struct bnx2x_fastpath *fp, u16 queue,
  * Approximate value of the MSS for this aggregation calculated using
  * the first packet of it.
  */
-static inline u16 bnx2x_set_lro_mss(struct bnx2x *bp, u16 parsing_flags,
-				    u16 len_on_bd)
+static u16 bnx2x_set_lro_mss(struct bnx2x *bp, u16 parsing_flags,
+			     u16 len_on_bd)
 {
 	/*
 	 * TPA arrgregation won't have either IP options or TCP options
@@ -396,6 +385,36 @@ static inline u16 bnx2x_set_lro_mss(struct bnx2x *bp, u16 parsing_flags,
 	return len_on_bd - hdrs_len;
 }
 
+static int bnx2x_alloc_rx_sge(struct bnx2x *bp,
+			      struct bnx2x_fastpath *fp, u16 index)
+{
+	struct page *page = alloc_pages(GFP_ATOMIC, PAGES_PER_SGE_SHIFT);
+	struct sw_rx_page *sw_buf = &fp->rx_page_ring[index];
+	struct eth_rx_sge *sge = &fp->rx_sge_ring[index];
+	dma_addr_t mapping;
+
+	if (unlikely(page == NULL)) {
+		BNX2X_ERR("Can't alloc sge\n");
+		return -ENOMEM;
+	}
+
+	mapping = dma_map_page(&bp->pdev->dev, page, 0,
+			       SGE_PAGE_SIZE*PAGES_PER_SGE, DMA_FROM_DEVICE);
+	if (unlikely(dma_mapping_error(&bp->pdev->dev, mapping))) {
+		__free_pages(page, PAGES_PER_SGE_SHIFT);
+		BNX2X_ERR("Can't map sge\n");
+		return -ENOMEM;
+	}
+
+	sw_buf->page = page;
+	dma_unmap_addr_set(sw_buf, mapping, mapping);
+
+	sge->addr_hi = cpu_to_le32(U64_HI(mapping));
+	sge->addr_lo = cpu_to_le32(U64_LO(mapping));
+
+	return 0;
+}
+
 static int bnx2x_fill_frag_skb(struct bnx2x *bp, struct bnx2x_fastpath *fp,
 			       struct bnx2x_agg_info *tpa_info,
 			       u16 pages,
@@ -494,11 +513,11 @@ static int bnx2x_fill_frag_skb(struct bnx2x *bp, struct bnx2x_fastpath *fp,
 	return 0;
 }
 
-static inline void bnx2x_tpa_stop(struct bnx2x *bp, struct bnx2x_fastpath *fp,
-				  struct bnx2x_agg_info *tpa_info,
-				  u16 pages,
-				  struct eth_end_agg_rx_cqe *cqe,
-				  u16 cqe_idx)
+static void bnx2x_tpa_stop(struct bnx2x *bp, struct bnx2x_fastpath *fp,
+			   struct bnx2x_agg_info *tpa_info,
+			   u16 pages,
+			   struct eth_end_agg_rx_cqe *cqe,
+			   u16 cqe_idx)
 {
 	struct sw_rx_bd *rx_buf = &tpa_info->first_buf;
 	u8 pad = tpa_info->placement_offset;
@@ -524,7 +543,7 @@ static inline void bnx2x_tpa_stop(struct bnx2x *bp, struct bnx2x_fastpath *fp,
 	dma_unmap_single(&bp->pdev->dev, dma_unmap_addr(rx_buf, mapping),
 			 fp->rx_buf_size, DMA_FROM_DEVICE);
 	if (likely(new_data))
-		skb = build_skb(data);
+		skb = build_skb(data, 0);
 
 	if (likely(skb)) {
 #ifdef BNX2X_STOP_ON_ERROR
@@ -568,6 +587,36 @@ drop:
 	fp->eth_q_stats.rx_skb_alloc_failed++;
 }
 
+static int bnx2x_alloc_rx_data(struct bnx2x *bp,
+			       struct bnx2x_fastpath *fp, u16 index)
+{
+	u8 *data;
+	struct sw_rx_bd *rx_buf = &fp->rx_buf_ring[index];
+	struct eth_rx_bd *rx_bd = &fp->rx_desc_ring[index];
+	dma_addr_t mapping;
+
+	data = kmalloc(fp->rx_buf_size + NET_SKB_PAD, GFP_ATOMIC);
+	if (unlikely(data == NULL))
+		return -ENOMEM;
+
+	mapping = dma_map_single(&bp->pdev->dev, data + NET_SKB_PAD,
+				 fp->rx_buf_size,
+				 DMA_FROM_DEVICE);
+	if (unlikely(dma_mapping_error(&bp->pdev->dev, mapping))) {
+		kfree(data);
+		BNX2X_ERR("Can't map rx data\n");
+		return -ENOMEM;
+	}
+
+	rx_buf->data = data;
+	dma_unmap_addr_set(rx_buf, mapping, mapping);
+
+	rx_bd->addr_hi = cpu_to_le32(U64_HI(mapping));
+	rx_bd->addr_lo = cpu_to_le32(U64_LO(mapping));
+
+	return 0;
+}
+
 
 int bnx2x_rx_int(struct bnx2x_fastpath *fp, int budget)
 {
@@ -732,7 +781,7 @@ int bnx2x_rx_int(struct bnx2x_fastpath *fp, int budget)
 						 dma_unmap_addr(rx_buf, mapping),
 						 fp->rx_buf_size,
 						 DMA_FROM_DEVICE);
-				skb = build_skb(data);
+				skb = build_skb(data, 0);
 				if (unlikely(!skb)) {
 					kfree(data);
 					fp->eth_q_stats.rx_skb_alloc_failed++;
@@ -881,8 +930,8 @@ u16 bnx2x_get_mf_speed(struct bnx2x *bp)
  *
  * It uses a none-atomic bit operations because is called under the mutex.
  */
-static inline void bnx2x_fill_report_data(struct bnx2x *bp,
-					  struct bnx2x_link_report_data *data)
+static void bnx2x_fill_report_data(struct bnx2x *bp,
+				   struct bnx2x_link_report_data *data)
 {
 	u16 line_speed = bnx2x_get_mf_speed(bp);
 
@@ -1000,6 +1049,47 @@ void __bnx2x_link_report(struct bnx2x *bp)
 	}
 }
 
+static void bnx2x_set_next_page_sgl(struct bnx2x_fastpath *fp)
+{
+	int i;
+
+	for (i = 1; i <= NUM_RX_SGE_PAGES; i++) {
+		struct eth_rx_sge *sge;
+
+		sge = &fp->rx_sge_ring[RX_SGE_CNT * i - 2];
+		sge->addr_hi =
+			cpu_to_le32(U64_HI(fp->rx_sge_mapping +
+			BCM_PAGE_SIZE*(i % NUM_RX_SGE_PAGES)));
+
+		sge->addr_lo =
+			cpu_to_le32(U64_LO(fp->rx_sge_mapping +
+			BCM_PAGE_SIZE*(i % NUM_RX_SGE_PAGES)));
+	}
+}
+
+static void bnx2x_free_tpa_pool(struct bnx2x *bp,
+				struct bnx2x_fastpath *fp, int last)
+{
+	int i;
+
+	for (i = 0; i < last; i++) {
+		struct bnx2x_agg_info *tpa_info = &fp->tpa_info[i];
+		struct sw_rx_bd *first_buf = &tpa_info->first_buf;
+		u8 *data = first_buf->data;
+
+		if (data == NULL) {
+			DP(NETIF_MSG_IFDOWN, "tpa bin %d empty on free\n", i);
+			continue;
+		}
+		if (tpa_info->tpa_state == BNX2X_TPA_START)
+			dma_unmap_single(&bp->pdev->dev,
+					 dma_unmap_addr(first_buf, mapping),
+					 fp->rx_buf_size, DMA_FROM_DEVICE);
+		kfree(data);
+		first_buf->data = NULL;
+	}
+}
+
 void bnx2x_init_rx_rings(struct bnx2x *bp)
 {
 	int func = BP_FUNC(bp);
@@ -1212,16 +1302,15 @@ static void bnx2x_free_msix_irqs(struct bnx2x *bp, int nvecs)
 
 void bnx2x_free_irq(struct bnx2x *bp)
 {
-	if (bp->flags & USING_MSIX_FLAG)
+	if (bp->flags & USING_MSIX_FLAG &&
+	    !(bp->flags & USING_SINGLE_MSIX_FLAG))
 		bnx2x_free_msix_irqs(bp, BNX2X_NUM_ETH_QUEUES(bp) +
 				     CNIC_PRESENT + 1);
-	else if (bp->flags & USING_MSI_FLAG)
-		free_irq(bp->pdev->irq, bp->dev);
 	else
-		free_irq(bp->pdev->irq, bp->dev);
+		free_irq(bp->dev->irq, bp->dev);
 }
 
-int bnx2x_enable_msix(struct bnx2x *bp)
+int __devinit bnx2x_enable_msix(struct bnx2x *bp)
 {
 	int msix_vec = 0, i, rc, req_cnt;
 
@@ -1261,8 +1350,8 @@ int bnx2x_enable_msix(struct bnx2x *bp)
 		rc = pci_enable_msix(bp->pdev, &bp->msix_table[0], rc);
 
 		if (rc) {
-			BNX2X_DEV_INFO("MSI-X is not attainable  rc %d\n", rc);
-			return rc;
+			BNX2X_DEV_INFO("MSI-X is not attainable rc %d\n", rc);
+			goto no_msix;
 		}
 		/*
 		 * decrease number of queues by number of unallocated entries
@@ -1270,18 +1359,34 @@ int bnx2x_enable_msix(struct bnx2x *bp)
 		bp->num_queues -= diff;
 
 		BNX2X_DEV_INFO("New queue configuration set: %d\n",
-				  bp->num_queues);
-	} else if (rc) {
-		/* fall to INTx if not enough memory */
-		if (rc == -ENOMEM)
-			bp->flags |= DISABLE_MSI_FLAG;
+			       bp->num_queues);
+	} else if (rc > 0) {
+		/* Get by with single vector */
+		rc = pci_enable_msix(bp->pdev, &bp->msix_table[0], 1);
+		if (rc) {
+			BNX2X_DEV_INFO("Single MSI-X is not attainable rc %d\n",
+				       rc);
+			goto no_msix;
+		}
+
+		BNX2X_DEV_INFO("Using single MSI-X vector\n");
+		bp->flags |= USING_SINGLE_MSIX_FLAG;
+
+	} else if (rc < 0) {
 		BNX2X_DEV_INFO("MSI-X is not attainable  rc %d\n", rc);
-		return rc;
+		goto no_msix;
 	}
 
 	bp->flags |= USING_MSIX_FLAG;
 
 	return 0;
+
+no_msix:
+	/* fall to INTx if not enough memory */
+	if (rc == -ENOMEM)
+		bp->flags |= DISABLE_MSI_FLAG;
+
+	return rc;
 }
 
 static int bnx2x_req_msix_irqs(struct bnx2x *bp)
@@ -1343,22 +1448,26 @@ int bnx2x_enable_msi(struct bnx2x *bp)
 static int bnx2x_req_irq(struct bnx2x *bp)
 {
 	unsigned long flags;
-	int rc;
+	unsigned int irq;
 
-	if (bp->flags & USING_MSI_FLAG)
+	if (bp->flags & (USING_MSI_FLAG | USING_MSIX_FLAG))
 		flags = 0;
 	else
 		flags = IRQF_SHARED;
 
-	rc = request_irq(bp->pdev->irq, bnx2x_interrupt, flags,
-			 bp->dev->name, bp->dev);
-	return rc;
+	if (bp->flags & USING_MSIX_FLAG)
+		irq = bp->msix_table[0].vector;
+	else
+		irq = bp->pdev->irq;
+
+	return request_irq(irq, bnx2x_interrupt, flags, bp->dev->name, bp->dev);
 }
 
-static inline int bnx2x_setup_irqs(struct bnx2x *bp)
+static int bnx2x_setup_irqs(struct bnx2x *bp)
 {
 	int rc = 0;
-	if (bp->flags & USING_MSIX_FLAG) {
+	if (bp->flags & USING_MSIX_FLAG &&
+	    !(bp->flags & USING_SINGLE_MSIX_FLAG)) {
 		rc = bnx2x_req_msix_irqs(bp);
 		if (rc)
 			return rc;
@@ -1371,15 +1480,20 @@ static inline int bnx2x_setup_irqs(struct bnx2x *bp)
 		}
 		if (bp->flags & USING_MSI_FLAG) {
 			bp->dev->irq = bp->pdev->irq;
-			netdev_info(bp->dev, "using MSI  IRQ %d\n",
-			       bp->pdev->irq);
+			netdev_info(bp->dev, "using MSI IRQ %d\n",
+				    bp->dev->irq);
+		}
+		if (bp->flags & USING_MSIX_FLAG) {
+			bp->dev->irq = bp->msix_table[0].vector;
+			netdev_info(bp->dev, "using MSIX IRQ %d\n",
+				    bp->dev->irq);
 		}
 	}
 
 	return 0;
 }
 
-static inline void bnx2x_napi_enable(struct bnx2x *bp)
+static void bnx2x_napi_enable(struct bnx2x *bp)
 {
 	int i;
 
@@ -1387,7 +1501,7 @@ static inline void bnx2x_napi_enable(struct bnx2x *bp)
 		napi_enable(&bnx2x_fp(bp, i, napi));
 }
 
-static inline void bnx2x_napi_disable(struct bnx2x *bp)
+static void bnx2x_napi_disable(struct bnx2x *bp)
 {
 	int i;
 
@@ -1437,24 +1551,15 @@ u16 bnx2x_select_queue(struct net_device *dev, struct sk_buff *skb)
 	return __skb_tx_hash(dev, skb, BNX2X_NUM_ETH_QUEUES(bp));
 }
 
+
 void bnx2x_set_num_queues(struct bnx2x *bp)
 {
-	switch (bp->multi_mode) {
-	case ETH_RSS_MODE_DISABLED:
-		bp->num_queues = 1;
-		break;
-	case ETH_RSS_MODE_REGULAR:
-		bp->num_queues = bnx2x_calc_num_queues(bp);
-		break;
-
-	default:
-		bp->num_queues = 1;
-		break;
-	}
+	/* RSS queues */
+	bp->num_queues = bnx2x_calc_num_queues(bp);
 
 #ifdef BCM_CNIC
-	/* override in STORAGE SD mode */
-	if (IS_MF_STORAGE_SD(bp))
+	/* override in STORAGE SD modes */
+	if (IS_MF_STORAGE_SD(bp) || IS_MF_FCOE_AFEX(bp))
 		bp->num_queues = 1;
 #endif
 	/* Add special queues */
@@ -1483,7 +1588,7 @@ void bnx2x_set_num_queues(struct bnx2x *bp)
  * bnx2x_setup_tc() takes care of the proper TC mappings so that __skb_tx_hash()
  * will return a proper Tx index if TC is enabled (netdev->num_tc > 0).
  */
-static inline int bnx2x_set_real_num_queues(struct bnx2x *bp)
+static int bnx2x_set_real_num_queues(struct bnx2x *bp)
 {
 	int rc, tx, rx;
 
@@ -1515,7 +1620,7 @@ static inline int bnx2x_set_real_num_queues(struct bnx2x *bp)
 	return rc;
 }
 
-static inline void bnx2x_set_rx_buf_size(struct bnx2x *bp)
+static void bnx2x_set_rx_buf_size(struct bnx2x *bp)
 {
 	int i;
 
@@ -1543,22 +1648,19 @@ static inline void bnx2x_set_rx_buf_size(struct bnx2x *bp)
 	}
 }
 
-static inline int bnx2x_init_rss_pf(struct bnx2x *bp)
+static int bnx2x_init_rss_pf(struct bnx2x *bp)
 {
 	int i;
 	u8 ind_table[T_ETH_INDIRECTION_TABLE_SIZE] = {0};
 	u8 num_eth_queues = BNX2X_NUM_ETH_QUEUES(bp);
 
-	/*
-	 * Prepare the inital contents fo the indirection table if RSS is
+	/* Prepare the initial contents fo the indirection table if RSS is
 	 * enabled
 	 */
-	if (bp->multi_mode != ETH_RSS_MODE_DISABLED) {
-		for (i = 0; i < sizeof(ind_table); i++)
-			ind_table[i] =
-				bp->fp->cl_id +
-				ethtool_rxfh_indir_default(i, num_eth_queues);
-	}
+	for (i = 0; i < sizeof(ind_table); i++)
+		ind_table[i] =
+			bp->fp->cl_id +
+			ethtool_rxfh_indir_default(i, num_eth_queues);
 
 	/*
 	 * For 57710 and 57711 SEARCHER configuration (rss_keys) is
@@ -1568,11 +1670,12 @@ static inline int bnx2x_init_rss_pf(struct bnx2x *bp)
 	 * For 57712 and newer on the other hand it's a per-function
 	 * configuration.
 	 */
-	return bnx2x_config_rss_pf(bp, ind_table,
-				   bp->port.pmf || !CHIP_IS_E1x(bp));
+	return bnx2x_config_rss_eth(bp, ind_table,
+				    bp->port.pmf || !CHIP_IS_E1x(bp));
 }
 
-int bnx2x_config_rss_pf(struct bnx2x *bp, u8 *ind_table, bool config_hash)
+int bnx2x_config_rss_pf(struct bnx2x *bp, struct bnx2x_rss_config_obj *rss_obj,
+			u8 *ind_table, bool config_hash)
 {
 	struct bnx2x_config_rss_params params = {NULL};
 	int i;
@@ -1584,58 +1687,35 @@ int bnx2x_config_rss_pf(struct bnx2x *bp, u8 *ind_table, bool config_hash)
 	 *      bp->multi_mode = ETH_RSS_MODE_DISABLED;
 	 */
 
-	params.rss_obj = &bp->rss_conf_obj;
+	params.rss_obj = rss_obj;
 
 	__set_bit(RAMROD_COMP_WAIT, &params.ramrod_flags);
 
-	/* RSS mode */
-	switch (bp->multi_mode) {
-	case ETH_RSS_MODE_DISABLED:
-		__set_bit(BNX2X_RSS_MODE_DISABLED, &params.rss_flags);
-		break;
-	case ETH_RSS_MODE_REGULAR:
-		__set_bit(BNX2X_RSS_MODE_REGULAR, &params.rss_flags);
-		break;
-	case ETH_RSS_MODE_VLAN_PRI:
-		__set_bit(BNX2X_RSS_MODE_VLAN_PRI, &params.rss_flags);
-		break;
-	case ETH_RSS_MODE_E1HOV_PRI:
-		__set_bit(BNX2X_RSS_MODE_E1HOV_PRI, &params.rss_flags);
-		break;
-	case ETH_RSS_MODE_IP_DSCP:
-		__set_bit(BNX2X_RSS_MODE_IP_DSCP, &params.rss_flags);
-		break;
-	default:
-		BNX2X_ERR("Unknown multi_mode: %d\n", bp->multi_mode);
-		return -EINVAL;
-	}
+	__set_bit(BNX2X_RSS_MODE_REGULAR, &params.rss_flags);
 
-	/* If RSS is enabled */
-	if (bp->multi_mode != ETH_RSS_MODE_DISABLED) {
-		/* RSS configuration */
-		__set_bit(BNX2X_RSS_IPV4, &params.rss_flags);
-		__set_bit(BNX2X_RSS_IPV4_TCP, &params.rss_flags);
-		__set_bit(BNX2X_RSS_IPV6, &params.rss_flags);
-		__set_bit(BNX2X_RSS_IPV6_TCP, &params.rss_flags);
+	/* RSS configuration */
+	__set_bit(BNX2X_RSS_IPV4, &params.rss_flags);
+	__set_bit(BNX2X_RSS_IPV4_TCP, &params.rss_flags);
+	__set_bit(BNX2X_RSS_IPV6, &params.rss_flags);
+	__set_bit(BNX2X_RSS_IPV6_TCP, &params.rss_flags);
 
-		/* Hash bits */
-		params.rss_result_mask = MULTI_MASK;
+	/* Hash bits */
+	params.rss_result_mask = MULTI_MASK;
 
-		memcpy(params.ind_table, ind_table, sizeof(params.ind_table));
+	memcpy(params.ind_table, ind_table, sizeof(params.ind_table));
 
-		if (config_hash) {
-			/* RSS keys */
-			for (i = 0; i < sizeof(params.rss_key) / 4; i++)
-				params.rss_key[i] = random32();
+	if (config_hash) {
+		/* RSS keys */
+		for (i = 0; i < sizeof(params.rss_key) / 4; i++)
+			params.rss_key[i] = random32();
 
-			__set_bit(BNX2X_RSS_SET_SRCH, &params.rss_flags);
-		}
+		__set_bit(BNX2X_RSS_SET_SRCH, &params.rss_flags);
 	}
 
 	return bnx2x_config_rss(bp, &params);
 }
 
-static inline int bnx2x_init_hw(struct bnx2x *bp, u32 load_code)
+static int bnx2x_init_hw(struct bnx2x *bp, u32 load_code)
 {
 	struct bnx2x_func_state_params func_params = {NULL};
 
@@ -1744,6 +1824,87 @@ bool bnx2x_test_firmware_version(struct bnx2x *bp, bool is_err)
 	return true;
 }
 
+/**
+ * bnx2x_bz_fp - zero content of the fastpath structure.
+ *
+ * @bp:		driver handle
+ * @index:	fastpath index to be zeroed
+ *
+ * Makes sure the contents of the bp->fp[index].napi is kept
+ * intact.
+ */
+static void bnx2x_bz_fp(struct bnx2x *bp, int index)
+{
+	struct bnx2x_fastpath *fp = &bp->fp[index];
+	struct napi_struct orig_napi = fp->napi;
+	/* bzero bnx2x_fastpath contents */
+	if (bp->stats_init)
+		memset(fp, 0, sizeof(*fp));
+	else {
+		/* Keep Queue statistics */
+		struct bnx2x_eth_q_stats *tmp_eth_q_stats;
+		struct bnx2x_eth_q_stats_old *tmp_eth_q_stats_old;
+
+		tmp_eth_q_stats = kzalloc(sizeof(struct bnx2x_eth_q_stats),
+					  GFP_KERNEL);
+		if (tmp_eth_q_stats)
+			memcpy(tmp_eth_q_stats, &fp->eth_q_stats,
+			       sizeof(struct bnx2x_eth_q_stats));
+
+		tmp_eth_q_stats_old =
+			kzalloc(sizeof(struct bnx2x_eth_q_stats_old),
+				GFP_KERNEL);
+		if (tmp_eth_q_stats_old)
+			memcpy(tmp_eth_q_stats_old, &fp->eth_q_stats_old,
+			       sizeof(struct bnx2x_eth_q_stats_old));
+
+		memset(fp, 0, sizeof(*fp));
+
+		if (tmp_eth_q_stats) {
+			memcpy(&fp->eth_q_stats, tmp_eth_q_stats,
+				   sizeof(struct bnx2x_eth_q_stats));
+			kfree(tmp_eth_q_stats);
+		}
+
+		if (tmp_eth_q_stats_old) {
+			memcpy(&fp->eth_q_stats_old, tmp_eth_q_stats_old,
+			       sizeof(struct bnx2x_eth_q_stats_old));
+			kfree(tmp_eth_q_stats_old);
+		}
+
+	}
+
+	/* Restore the NAPI object as it has been already initialized */
+	fp->napi = orig_napi;
+
+	fp->bp = bp;
+	fp->index = index;
+	if (IS_ETH_FP(fp))
+		fp->max_cos = bp->max_cos;
+	else
+		/* Special queues support only one CoS */
+		fp->max_cos = 1;
+
+	/*
+	 * set the tpa flag for each queue. The tpa flag determines the queue
+	 * minimal size so it must be set prior to queue memory allocation
+	 */
+	fp->disable_tpa = !(bp->flags & TPA_ENABLE_FLAG ||
+				  (bp->flags & GRO_ENABLE_FLAG &&
+				   bnx2x_mtu_allows_gro(bp->dev->mtu)));
+	if (bp->flags & TPA_ENABLE_FLAG)
+		fp->mode = TPA_MODE_LRO;
+	else if (bp->flags & GRO_ENABLE_FLAG)
+		fp->mode = TPA_MODE_GRO;
+
+#ifdef BCM_CNIC
+	/* We don't want TPA on an FCoE L2 ring */
+	if (IS_FCOE_FP(fp))
+		fp->disable_tpa = 1;
+#endif
+}
+
+
 /* must be called with rtnl_lock */
 int bnx2x_nic_load(struct bnx2x *bp, int load_mode)
 {
@@ -1911,8 +2072,14 @@ int bnx2x_nic_load(struct bnx2x *bp, int load_mode)
 			SHMEM2_WR(bp, dcc_support,
 				  (SHMEM_DCC_SUPPORT_DISABLE_ENABLE_PF_TLV |
 				   SHMEM_DCC_SUPPORT_BANDWIDTH_ALLOCATION_TLV));
+		if (SHMEM2_HAS(bp, afex_driver_support))
+			SHMEM2_WR(bp, afex_driver_support,
+				  SHMEM_AFEX_SUPPORTED_VERSION_ONE);
 	}
 
+	/* Set AFEX default VLAN tag to an invalid value */
+	bp->afex_def_vlan_tag = -1;
+
 	bp->state = BNX2X_STATE_OPENING_WAIT4_PORT;
 	rc = bnx2x_func_start(bp);
 	if (rc) {
@@ -2968,6 +3135,8 @@ netdev_tx_t bnx2x_start_xmit(struct sk_buff *skb, struct net_device *dev)
 
 	netdev_tx_sent_queue(txq, skb->len);
 
+	skb_tx_timestamp(skb);
+
 	txdata->tx_pkt_prod++;
 	/*
 	 * Make sure that the BD data is updated before updating the producer
@@ -3084,7 +3253,8 @@ int bnx2x_change_mac_addr(struct net_device *dev, void *p)
 	}
 
 #ifdef BCM_CNIC
-	if (IS_MF_STORAGE_SD(bp) && !is_zero_ether_addr(addr->sa_data)) {
+	if ((IS_MF_STORAGE_SD(bp) || IS_MF_FCOE_AFEX(bp)) &&
+	    !is_zero_ether_addr(addr->sa_data)) {
 		BNX2X_ERR("Can't configure non-zero address on iSCSI or FCoE functions in MF-SD mode\n");
 		return -EINVAL;
 	}
@@ -3181,7 +3351,7 @@ void bnx2x_free_fp_mem(struct bnx2x *bp)
 		bnx2x_free_fp_mem_at(bp, i);
 }
 
-static inline void set_sb_shortcuts(struct bnx2x *bp, int index)
+static void set_sb_shortcuts(struct bnx2x *bp, int index)
 {
 	union host_hc_status_block status_blk = bnx2x_fp(bp, index, status_blk);
 	if (!CHIP_IS_E1x(bp)) {
@@ -3197,6 +3367,63 @@ static inline void set_sb_shortcuts(struct bnx2x *bp, int index)
 	}
 }
 
+/* Returns the number of actually allocated BDs */
+static int bnx2x_alloc_rx_bds(struct bnx2x_fastpath *fp,
+			      int rx_ring_size)
+{
+	struct bnx2x *bp = fp->bp;
+	u16 ring_prod, cqe_ring_prod;
+	int i, failure_cnt = 0;
+
+	fp->rx_comp_cons = 0;
+	cqe_ring_prod = ring_prod = 0;
+
+	/* This routine is called only during fo init so
+	 * fp->eth_q_stats.rx_skb_alloc_failed = 0
+	 */
+	for (i = 0; i < rx_ring_size; i++) {
+		if (bnx2x_alloc_rx_data(bp, fp, ring_prod) < 0) {
+			failure_cnt++;
+			continue;
+		}
+		ring_prod = NEXT_RX_IDX(ring_prod);
+		cqe_ring_prod = NEXT_RCQ_IDX(cqe_ring_prod);
+		WARN_ON(ring_prod <= (i - failure_cnt));
+	}
+
+	if (failure_cnt)
+		BNX2X_ERR("was only able to allocate %d rx skbs on queue[%d]\n",
+			  i - failure_cnt, fp->index);
+
+	fp->rx_bd_prod = ring_prod;
+	/* Limit the CQE producer by the CQE ring size */
+	fp->rx_comp_prod = min_t(u16, NUM_RCQ_RINGS*RCQ_DESC_CNT,
+			       cqe_ring_prod);
+	fp->rx_pkt = fp->rx_calls = 0;
+
+	fp->eth_q_stats.rx_skb_alloc_failed += failure_cnt;
+
+	return i - failure_cnt;
+}
+
+static void bnx2x_set_next_page_rx_cq(struct bnx2x_fastpath *fp)
+{
+	int i;
+
+	for (i = 1; i <= NUM_RCQ_RINGS; i++) {
+		struct eth_rx_cqe_next_page *nextpg;
+
+		nextpg = (struct eth_rx_cqe_next_page *)
+			&fp->rx_comp_ring[RCQ_DESC_CNT * i - 1];
+		nextpg->addr_hi =
+			cpu_to_le32(U64_HI(fp->rx_comp_mapping +
+				   BCM_PAGE_SIZE*(i % NUM_RCQ_RINGS)));
+		nextpg->addr_lo =
+			cpu_to_le32(U64_LO(fp->rx_comp_mapping +
+				   BCM_PAGE_SIZE*(i % NUM_RCQ_RINGS)));
+	}
+}
+
 static int bnx2x_alloc_fp_mem_at(struct bnx2x *bp, int index)
 {
 	union host_hc_status_block *sb;
@@ -3206,7 +3433,8 @@ static int bnx2x_alloc_fp_mem_at(struct bnx2x *bp, int index)
 	int rx_ring_size = 0;
 
 #ifdef BCM_CNIC
-	if (!bp->rx_ring_size && IS_MF_STORAGE_SD(bp)) {
+	if (!bp->rx_ring_size &&
+	    (IS_MF_STORAGE_SD(bp) || IS_MF_FCOE_AFEX(bp))) {
 		rx_ring_size = MIN_RX_SIZE_NONTPA;
 		bp->rx_ring_size = rx_ring_size;
 	} else
@@ -3528,8 +3756,6 @@ int bnx2x_change_mtu(struct net_device *dev, int new_mtu)
 	 */
 	dev->mtu = new_mtu;
 
-	bp->gro_check = bnx2x_need_gro_check(new_mtu);
-
 	return bnx2x_reload_if_running(dev);
 }
 
@@ -3687,9 +3913,9 @@ void bnx2x_set_ctx_validation(struct bnx2x *bp, struct eth_context *cxt,
 			CDU_REGION_NUMBER_XCM_AG, ETH_CONNECTION_TYPE);
 }
 
-static inline void storm_memset_hc_timeout(struct bnx2x *bp, u8 port,
-					     u8 fw_sb_id, u8 sb_index,
-					     u8 ticks)
+static void storm_memset_hc_timeout(struct bnx2x *bp, u8 port,
+				    u8 fw_sb_id, u8 sb_index,
+				    u8 ticks)
 {
 
 	u32 addr = BAR_CSTRORM_INTMEM +
@@ -3700,9 +3926,9 @@ static inline void storm_memset_hc_timeout(struct bnx2x *bp, u8 port,
 	   port, fw_sb_id, sb_index, ticks);
 }
 
-static inline void storm_memset_hc_disable(struct bnx2x *bp, u8 port,
-					     u16 fw_sb_id, u8 sb_index,
-					     u8 disable)
+static void storm_memset_hc_disable(struct bnx2x *bp, u8 port,
+				    u16 fw_sb_id, u8 sb_index,
+				    u8 disable)
 {
 	u32 enable_flag = disable ? 0 : (1 << HC_INDEX_DATA_HC_ENABLED_SHIFT);
 	u32 addr = BAR_CSTRORM_INTMEM +
diff --git a/drivers/net/ethernet/broadcom/bnx2x/bnx2x_cmn.h b/drivers/net/ethernet/broadcom/bnx2x/bnx2x_cmn.h
index 5c27454d2ec..7cd99b75347 100644
--- a/drivers/net/ethernet/broadcom/bnx2x/bnx2x_cmn.h
+++ b/drivers/net/ethernet/broadcom/bnx2x/bnx2x_cmn.h
@@ -86,13 +86,15 @@ u32 bnx2x_send_unload_req(struct bnx2x *bp, int unload_mode);
 void bnx2x_send_unload_done(struct bnx2x *bp);
 
 /**
- * bnx2x_config_rss_pf - configure RSS parameters.
+ * bnx2x_config_rss_pf - configure RSS parameters in a PF.
  *
  * @bp:			driver handle
+ * @rss_obj		RSS object to use
  * @ind_table:		indirection table to configure
  * @config_hash:	re-configure RSS hash keys configuration
  */
-int bnx2x_config_rss_pf(struct bnx2x *bp, u8 *ind_table, bool config_hash);
+int bnx2x_config_rss_pf(struct bnx2x *bp, struct bnx2x_rss_config_obj *rss_obj,
+			u8 *ind_table, bool config_hash);
 
 /**
  * bnx2x__init_func_obj - init function object
@@ -485,7 +487,7 @@ void bnx2x_netif_start(struct bnx2x *bp);
  * fills msix_table, requests vectors, updates num_queues
  * according to number of available vectors.
  */
-int bnx2x_enable_msix(struct bnx2x *bp);
+int __devinit bnx2x_enable_msix(struct bnx2x *bp);
 
 /**
  * bnx2x_enable_msi - request msi mode from OS, updated internals accordingly
@@ -610,53 +612,6 @@ static inline void bnx2x_igu_ack_sb_gen(struct bnx2x *bp, u8 igu_sb_id,
 	barrier();
 }
 
-static inline void bnx2x_igu_clear_sb_gen(struct bnx2x *bp, u8 func,
-					  u8 idu_sb_id, bool is_Pf)
-{
-	u32 data, ctl, cnt = 100;
-	u32 igu_addr_data = IGU_REG_COMMAND_REG_32LSB_DATA;
-	u32 igu_addr_ctl = IGU_REG_COMMAND_REG_CTRL;
-	u32 igu_addr_ack = IGU_REG_CSTORM_TYPE_0_SB_CLEANUP + (idu_sb_id/32)*4;
-	u32 sb_bit =  1 << (idu_sb_id%32);
-	u32 func_encode = func | (is_Pf ? 1 : 0) << IGU_FID_ENCODE_IS_PF_SHIFT;
-	u32 addr_encode = IGU_CMD_E2_PROD_UPD_BASE + idu_sb_id;
-
-	/* Not supported in BC mode */
-	if (CHIP_INT_MODE_IS_BC(bp))
-		return;
-
-	data = (IGU_USE_REGISTER_cstorm_type_0_sb_cleanup
-			<< IGU_REGULAR_CLEANUP_TYPE_SHIFT)	|
-		IGU_REGULAR_CLEANUP_SET				|
-		IGU_REGULAR_BCLEANUP;
-
-	ctl = addr_encode << IGU_CTRL_REG_ADDRESS_SHIFT		|
-	      func_encode << IGU_CTRL_REG_FID_SHIFT		|
-	      IGU_CTRL_CMD_TYPE_WR << IGU_CTRL_REG_TYPE_SHIFT;
-
-	DP(NETIF_MSG_HW, "write 0x%08x to IGU(via GRC) addr 0x%x\n",
-			 data, igu_addr_data);
-	REG_WR(bp, igu_addr_data, data);
-	mmiowb();
-	barrier();
-	DP(NETIF_MSG_HW, "write 0x%08x to IGU(via GRC) addr 0x%x\n",
-			  ctl, igu_addr_ctl);
-	REG_WR(bp, igu_addr_ctl, ctl);
-	mmiowb();
-	barrier();
-
-	/* wait for clean up to finish */
-	while (!(REG_RD(bp, igu_addr_ack) & sb_bit) && --cnt)
-		msleep(20);
-
-
-	if (!(REG_RD(bp, igu_addr_ack) & sb_bit)) {
-		DP(NETIF_MSG_HW,
-		   "Unable to finish IGU cleanup: idu_sb_id %d offset %d bit %d (cnt %d)\n",
-			  idu_sb_id, idu_sb_id/32, idu_sb_id%32, cnt);
-	}
-}
-
 static inline void bnx2x_hc_ack_sb(struct bnx2x *bp, u8 sb_id,
 				   u8 storm, u16 index, u8 op, u8 update)
 {
@@ -843,7 +798,7 @@ static inline void bnx2x_disable_msi(struct bnx2x *bp)
 {
 	if (bp->flags & USING_MSIX_FLAG) {
 		pci_disable_msix(bp->pdev);
-		bp->flags &= ~USING_MSIX_FLAG;
+		bp->flags &= ~(USING_MSIX_FLAG | USING_SINGLE_MSIX_FLAG);
 	} else if (bp->flags & USING_MSI_FLAG) {
 		pci_disable_msi(bp->pdev);
 		bp->flags &= ~USING_MSI_FLAG;
@@ -883,66 +838,6 @@ static inline void bnx2x_init_sge_ring_bit_mask(struct bnx2x_fastpath *fp)
 	bnx2x_clear_sge_mask_next_elems(fp);
 }
 
-static inline int bnx2x_alloc_rx_sge(struct bnx2x *bp,
-				     struct bnx2x_fastpath *fp, u16 index)
-{
-	struct page *page = alloc_pages(GFP_ATOMIC, PAGES_PER_SGE_SHIFT);
-	struct sw_rx_page *sw_buf = &fp->rx_page_ring[index];
-	struct eth_rx_sge *sge = &fp->rx_sge_ring[index];
-	dma_addr_t mapping;
-
-	if (unlikely(page == NULL)) {
-		BNX2X_ERR("Can't alloc sge\n");
-		return -ENOMEM;
-	}
-
-	mapping = dma_map_page(&bp->pdev->dev, page, 0,
-			       SGE_PAGE_SIZE*PAGES_PER_SGE, DMA_FROM_DEVICE);
-	if (unlikely(dma_mapping_error(&bp->pdev->dev, mapping))) {
-		__free_pages(page, PAGES_PER_SGE_SHIFT);
-		BNX2X_ERR("Can't map sge\n");
-		return -ENOMEM;
-	}
-
-	sw_buf->page = page;
-	dma_unmap_addr_set(sw_buf, mapping, mapping);
-
-	sge->addr_hi = cpu_to_le32(U64_HI(mapping));
-	sge->addr_lo = cpu_to_le32(U64_LO(mapping));
-
-	return 0;
-}
-
-static inline int bnx2x_alloc_rx_data(struct bnx2x *bp,
-				      struct bnx2x_fastpath *fp, u16 index)
-{
-	u8 *data;
-	struct sw_rx_bd *rx_buf = &fp->rx_buf_ring[index];
-	struct eth_rx_bd *rx_bd = &fp->rx_desc_ring[index];
-	dma_addr_t mapping;
-
-	data = kmalloc(fp->rx_buf_size + NET_SKB_PAD, GFP_ATOMIC);
-	if (unlikely(data == NULL))
-		return -ENOMEM;
-
-	mapping = dma_map_single(&bp->pdev->dev, data + NET_SKB_PAD,
-				 fp->rx_buf_size,
-				 DMA_FROM_DEVICE);
-	if (unlikely(dma_mapping_error(&bp->pdev->dev, mapping))) {
-		kfree(data);
-		BNX2X_ERR("Can't map rx data\n");
-		return -ENOMEM;
-	}
-
-	rx_buf->data = data;
-	dma_unmap_addr_set(rx_buf, mapping, mapping);
-
-	rx_bd->addr_hi = cpu_to_le32(U64_HI(mapping));
-	rx_bd->addr_lo = cpu_to_le32(U64_LO(mapping));
-
-	return 0;
-}
-
 /* note that we are not allocating a new buffer,
  * we are just moving one from cons to prod
  * we are not creating a new mapping,
@@ -964,6 +859,19 @@ static inline void bnx2x_reuse_rx_data(struct bnx2x_fastpath *fp,
 
 /************************* Init ******************************************/
 
+/* returns func by VN for current port */
+static inline int func_by_vn(struct bnx2x *bp, int vn)
+{
+	return 2 * vn + BP_PORT(bp);
+}
+
+static inline int bnx2x_config_rss_eth(struct bnx2x *bp, u8 *ind_table,
+				       bool config_hash)
+{
+	return bnx2x_config_rss_pf(bp, &bp->rss_conf_obj, ind_table,
+				   config_hash);
+}
+
 /**
  * bnx2x_func_start - init function
  *
@@ -1027,66 +935,6 @@ static inline void bnx2x_free_rx_sge_range(struct bnx2x *bp,
 		bnx2x_free_rx_sge(bp, fp, i);
 }
 
-static inline void bnx2x_free_tpa_pool(struct bnx2x *bp,
-				       struct bnx2x_fastpath *fp, int last)
-{
-	int i;
-
-	for (i = 0; i < last; i++) {
-		struct bnx2x_agg_info *tpa_info = &fp->tpa_info[i];
-		struct sw_rx_bd *first_buf = &tpa_info->first_buf;
-		u8 *data = first_buf->data;
-
-		if (data == NULL) {
-			DP(NETIF_MSG_IFDOWN, "tpa bin %d empty on free\n", i);
-			continue;
-		}
-		if (tpa_info->tpa_state == BNX2X_TPA_START)
-			dma_unmap_single(&bp->pdev->dev,
-					 dma_unmap_addr(first_buf, mapping),
-					 fp->rx_buf_size, DMA_FROM_DEVICE);
-		kfree(data);
-		first_buf->data = NULL;
-	}
-}
-
-static inline void bnx2x_init_tx_ring_one(struct bnx2x_fp_txdata *txdata)
-{
-	int i;
-
-	for (i = 1; i <= NUM_TX_RINGS; i++) {
-		struct eth_tx_next_bd *tx_next_bd =
-			&txdata->tx_desc_ring[TX_DESC_CNT * i - 1].next_bd;
-
-		tx_next_bd->addr_hi =
-			cpu_to_le32(U64_HI(txdata->tx_desc_mapping +
-				    BCM_PAGE_SIZE*(i % NUM_TX_RINGS)));
-		tx_next_bd->addr_lo =
-			cpu_to_le32(U64_LO(txdata->tx_desc_mapping +
-				    BCM_PAGE_SIZE*(i % NUM_TX_RINGS)));
-	}
-
-	SET_FLAG(txdata->tx_db.data.header.header, DOORBELL_HDR_DB_TYPE, 1);
-	txdata->tx_db.data.zero_fill1 = 0;
-	txdata->tx_db.data.prod = 0;
-
-	txdata->tx_pkt_prod = 0;
-	txdata->tx_pkt_cons = 0;
-	txdata->tx_bd_prod = 0;
-	txdata->tx_bd_cons = 0;
-	txdata->tx_pkt = 0;
-}
-
-static inline void bnx2x_init_tx_rings(struct bnx2x *bp)
-{
-	int i;
-	u8 cos;
-
-	for_each_tx_queue(bp, i)
-		for_each_cos_in_tx_queue(&bp->fp[i], cos)
-			bnx2x_init_tx_ring_one(&bp->fp[i].txdata[cos]);
-}
-
 static inline void bnx2x_set_next_page_rx_bd(struct bnx2x_fastpath *fp)
 {
 	int i;
@@ -1104,80 +952,6 @@ static inline void bnx2x_set_next_page_rx_bd(struct bnx2x_fastpath *fp)
 	}
 }
 
-static inline void bnx2x_set_next_page_sgl(struct bnx2x_fastpath *fp)
-{
-	int i;
-
-	for (i = 1; i <= NUM_RX_SGE_PAGES; i++) {
-		struct eth_rx_sge *sge;
-
-		sge = &fp->rx_sge_ring[RX_SGE_CNT * i - 2];
-		sge->addr_hi =
-			cpu_to_le32(U64_HI(fp->rx_sge_mapping +
-			BCM_PAGE_SIZE*(i % NUM_RX_SGE_PAGES)));
-
-		sge->addr_lo =
-			cpu_to_le32(U64_LO(fp->rx_sge_mapping +
-			BCM_PAGE_SIZE*(i % NUM_RX_SGE_PAGES)));
-	}
-}
-
-static inline void bnx2x_set_next_page_rx_cq(struct bnx2x_fastpath *fp)
-{
-	int i;
-	for (i = 1; i <= NUM_RCQ_RINGS; i++) {
-		struct eth_rx_cqe_next_page *nextpg;
-
-		nextpg = (struct eth_rx_cqe_next_page *)
-			&fp->rx_comp_ring[RCQ_DESC_CNT * i - 1];
-		nextpg->addr_hi =
-			cpu_to_le32(U64_HI(fp->rx_comp_mapping +
-				   BCM_PAGE_SIZE*(i % NUM_RCQ_RINGS)));
-		nextpg->addr_lo =
-			cpu_to_le32(U64_LO(fp->rx_comp_mapping +
-				   BCM_PAGE_SIZE*(i % NUM_RCQ_RINGS)));
-	}
-}
-
-/* Returns the number of actually allocated BDs */
-static inline int bnx2x_alloc_rx_bds(struct bnx2x_fastpath *fp,
-				      int rx_ring_size)
-{
-	struct bnx2x *bp = fp->bp;
-	u16 ring_prod, cqe_ring_prod;
-	int i, failure_cnt = 0;
-
-	fp->rx_comp_cons = 0;
-	cqe_ring_prod = ring_prod = 0;
-
-	/* This routine is called only during fo init so
-	 * fp->eth_q_stats.rx_skb_alloc_failed = 0
-	 */
-	for (i = 0; i < rx_ring_size; i++) {
-		if (bnx2x_alloc_rx_data(bp, fp, ring_prod) < 0) {
-			failure_cnt++;
-			continue;
-		}
-		ring_prod = NEXT_RX_IDX(ring_prod);
-		cqe_ring_prod = NEXT_RCQ_IDX(cqe_ring_prod);
-		WARN_ON(ring_prod <= (i - failure_cnt));
-	}
-
-	if (failure_cnt)
-		BNX2X_ERR("was only able to allocate %d rx skbs on queue[%d]\n",
-			  i - failure_cnt, fp->index);
-
-	fp->rx_bd_prod = ring_prod;
-	/* Limit the CQE producer by the CQE ring size */
-	fp->rx_comp_prod = min_t(u16, NUM_RCQ_RINGS*RCQ_DESC_CNT,
-			       cqe_ring_prod);
-	fp->rx_pkt = fp->rx_calls = 0;
-
-	fp->eth_q_stats.rx_skb_alloc_failed += failure_cnt;
-
-	return i - failure_cnt;
-}
-
 /* Statistics ID are global per chip/path, while Client IDs for E1x are per
  * port.
  */
@@ -1406,30 +1180,6 @@ static inline void __storm_memset_struct(struct bnx2x *bp,
 		REG_WR(bp, addr + (i * 4), data[i]);
 }
 
-static inline void storm_memset_func_cfg(struct bnx2x *bp,
-				struct tstorm_eth_function_common_config *tcfg,
-				u16 abs_fid)
-{
-	size_t size = sizeof(struct tstorm_eth_function_common_config);
-
-	u32 addr = BAR_TSTRORM_INTMEM +
-			TSTORM_FUNCTION_COMMON_CONFIG_OFFSET(abs_fid);
-
-	__storm_memset_struct(bp, addr, size, (u32 *)tcfg);
-}
-
-static inline void storm_memset_cmng(struct bnx2x *bp,
-				struct cmng_struct_per_port *cmng,
-				u8 port)
-{
-	size_t size = sizeof(struct cmng_struct_per_port);
-
-	u32 addr = BAR_XSTRORM_INTMEM +
-			XSTORM_CMNG_PER_PORT_VARS_OFFSET(port);
-
-	__storm_memset_struct(bp, addr, size, (u32 *)cmng);
-}
-
 /**
  * bnx2x_wait_sp_comp - wait for the outstanding SP commands.
  *
@@ -1512,93 +1262,6 @@ static inline bool bnx2x_mtu_allows_gro(int mtu)
 	 */
 	return mtu <= SGE_PAGE_SIZE && (U_ETH_SGL_SIZE * fpp) <= MAX_SKB_FRAGS;
 }
-
-static inline bool bnx2x_need_gro_check(int mtu)
-{
-	return (SGE_PAGES / (mtu - ETH_MAX_TPA_HEADER_SIZE - 1)) !=
-		(SGE_PAGES / (mtu - ETH_MIN_TPA_HEADER_SIZE + 1));
-}
-
-/**
- * bnx2x_bz_fp - zero content of the fastpath structure.
- *
- * @bp:		driver handle
- * @index:	fastpath index to be zeroed
- *
- * Makes sure the contents of the bp->fp[index].napi is kept
- * intact.
- */
-static inline void bnx2x_bz_fp(struct bnx2x *bp, int index)
-{
-	struct bnx2x_fastpath *fp = &bp->fp[index];
-	struct napi_struct orig_napi = fp->napi;
-	/* bzero bnx2x_fastpath contents */
-	if (bp->stats_init)
-		memset(fp, 0, sizeof(*fp));
-	else {
-		/* Keep Queue statistics */
-		struct bnx2x_eth_q_stats *tmp_eth_q_stats;
-		struct bnx2x_eth_q_stats_old *tmp_eth_q_stats_old;
-
-		tmp_eth_q_stats = kzalloc(sizeof(struct bnx2x_eth_q_stats),
-					  GFP_KERNEL);
-		if (tmp_eth_q_stats)
-			memcpy(tmp_eth_q_stats, &fp->eth_q_stats,
-			       sizeof(struct bnx2x_eth_q_stats));
-
-		tmp_eth_q_stats_old =
-			kzalloc(sizeof(struct bnx2x_eth_q_stats_old),
-				GFP_KERNEL);
-		if (tmp_eth_q_stats_old)
-			memcpy(tmp_eth_q_stats_old, &fp->eth_q_stats_old,
-			       sizeof(struct bnx2x_eth_q_stats_old));
-
-		memset(fp, 0, sizeof(*fp));
-
-		if (tmp_eth_q_stats) {
-			memcpy(&fp->eth_q_stats, tmp_eth_q_stats,
-				   sizeof(struct bnx2x_eth_q_stats));
-			kfree(tmp_eth_q_stats);
-		}
-
-		if (tmp_eth_q_stats_old) {
-			memcpy(&fp->eth_q_stats_old, tmp_eth_q_stats_old,
-			       sizeof(struct bnx2x_eth_q_stats_old));
-			kfree(tmp_eth_q_stats_old);
-		}
-
-	}
-
-	/* Restore the NAPI object as it has been already initialized */
-	fp->napi = orig_napi;
-
-	fp->bp = bp;
-	fp->index = index;
-	if (IS_ETH_FP(fp))
-		fp->max_cos = bp->max_cos;
-	else
-		/* Special queues support only one CoS */
-		fp->max_cos = 1;
-
-	/*
-	 * set the tpa flag for each queue. The tpa flag determines the queue
-	 * minimal size so it must be set prior to queue memory allocation
-	 */
-	fp->disable_tpa = !(bp->flags & TPA_ENABLE_FLAG ||
-				  (bp->flags & GRO_ENABLE_FLAG &&
-				   bnx2x_mtu_allows_gro(bp->dev->mtu)));
-	if (bp->flags & TPA_ENABLE_FLAG)
-		fp->mode = TPA_MODE_LRO;
-	else if (bp->flags & GRO_ENABLE_FLAG)
-		fp->mode = TPA_MODE_GRO;
-
-#ifdef BCM_CNIC
-	/* We don't want TPA on an FCoE L2 ring */
-	if (IS_FCOE_FP(fp))
-		fp->disable_tpa = 1;
-#endif
-}
-
 #ifdef BCM_CNIC
 /**
  * bnx2x_get_iscsi_info - update iSCSI params according to licensing info.
@@ -1608,11 +1271,6 @@ static inline void bnx2x_bz_fp(struct bnx2x *bp, int index)
  */
 void bnx2x_get_iscsi_info(struct bnx2x *bp);
 #endif
-/* returns func by VN for current port */
-static inline int func_by_vn(struct bnx2x *bp, int vn)
-{
-	return 2 * vn + BP_PORT(bp);
-}
 
 /**
  * bnx2x_link_sync_notify - send notification to other functions.
@@ -1667,7 +1325,8 @@ static inline bool bnx2x_is_valid_ether_addr(struct bnx2x *bp, u8 *addr)
 	if (is_valid_ether_addr(addr))
 		return true;
 #ifdef BCM_CNIC
-	if (is_zero_ether_addr(addr) && IS_MF_STORAGE_SD(bp))
+	if (is_zero_ether_addr(addr) &&
+	    (IS_MF_STORAGE_SD(bp) || IS_MF_FCOE_AFEX(bp)))
 		return true;
 #endif
 	return false;
diff --git a/drivers/net/ethernet/broadcom/bnx2x/bnx2x_ethtool.c b/drivers/net/ethernet/broadcom/bnx2x/bnx2x_ethtool.c
index 2cc0a170397..ddc18ee5c5a 100644
--- a/drivers/net/ethernet/broadcom/bnx2x/bnx2x_ethtool.c
+++ b/drivers/net/ethernet/broadcom/bnx2x/bnx2x_ethtool.c
@@ -22,13 +22,10 @@
 #include <linux/types.h>
 #include <linux/sched.h>
 #include <linux/crc32.h>
-
-
 #include "bnx2x.h"
 #include "bnx2x_cmn.h"
 #include "bnx2x_dump.h"
 #include "bnx2x_init.h"
-#include "bnx2x_sp.h"
 
 /* Note: in the format strings below %s is replaced by the queue-name which is
  * either its index or 'fcoe' for the fcoe queue. Make sure the format string
@@ -595,8 +592,8 @@ static int bnx2x_set_settings(struct net_device *dev, struct ethtool_cmd *cmd)
 #define IS_E3_ONLINE(info)	(((info) & RI_E3_ONLINE) == RI_E3_ONLINE)
 #define IS_E3B0_ONLINE(info)	(((info) & RI_E3B0_ONLINE) == RI_E3B0_ONLINE)
 
-static inline bool bnx2x_is_reg_online(struct bnx2x *bp,
-				       const struct reg_addr *reg_info)
+static bool bnx2x_is_reg_online(struct bnx2x *bp,
+				const struct reg_addr *reg_info)
 {
 	if (CHIP_IS_E1(bp))
 		return IS_E1_ONLINE(reg_info->info);
@@ -613,7 +610,7 @@ static inline bool bnx2x_is_reg_online(struct bnx2x *bp,
 }
 
 /******* Paged registers info selectors ********/
-static inline const u32 *__bnx2x_get_page_addr_ar(struct bnx2x *bp)
+static const u32 *__bnx2x_get_page_addr_ar(struct bnx2x *bp)
 {
 	if (CHIP_IS_E2(bp))
 		return page_vals_e2;
@@ -623,7 +620,7 @@ static inline const u32 *__bnx2x_get_page_addr_ar(struct bnx2x *bp)
 		return NULL;
 }
 
-static inline u32 __bnx2x_get_page_reg_num(struct bnx2x *bp)
+static u32 __bnx2x_get_page_reg_num(struct bnx2x *bp)
 {
 	if (CHIP_IS_E2(bp))
 		return PAGE_MODE_VALUES_E2;
@@ -633,7 +630,7 @@ static inline u32 __bnx2x_get_page_reg_num(struct bnx2x *bp)
 		return 0;
 }
 
-static inline const u32 *__bnx2x_get_page_write_ar(struct bnx2x *bp)
+static const u32 *__bnx2x_get_page_write_ar(struct bnx2x *bp)
 {
 	if (CHIP_IS_E2(bp))
 		return page_write_regs_e2;
@@ -643,7 +640,7 @@ static inline const u32 *__bnx2x_get_page_write_ar(struct bnx2x *bp)
 		return NULL;
 }
 
-static inline u32 __bnx2x_get_page_write_num(struct bnx2x *bp)
+static u32 __bnx2x_get_page_write_num(struct bnx2x *bp)
 {
 	if (CHIP_IS_E2(bp))
 		return PAGE_WRITE_REGS_E2;
@@ -653,7 +650,7 @@ static inline u32 __bnx2x_get_page_write_num(struct bnx2x *bp)
 		return 0;
 }
 
-static inline const struct reg_addr *__bnx2x_get_page_read_ar(struct bnx2x *bp)
+static const struct reg_addr *__bnx2x_get_page_read_ar(struct bnx2x *bp)
 {
 	if (CHIP_IS_E2(bp))
 		return page_read_regs_e2;
@@ -663,7 +660,7 @@ static inline const struct reg_addr *__bnx2x_get_page_read_ar(struct bnx2x *bp)
 		return NULL;
 }
 
-static inline u32 __bnx2x_get_page_read_num(struct bnx2x *bp)
+static u32 __bnx2x_get_page_read_num(struct bnx2x *bp)
 {
 	if (CHIP_IS_E2(bp))
 		return PAGE_READ_REGS_E2;
@@ -673,7 +670,7 @@ static inline u32 __bnx2x_get_page_read_num(struct bnx2x *bp)
 		return 0;
 }
 
-static inline int __bnx2x_get_regs_len(struct bnx2x *bp)
+static int __bnx2x_get_regs_len(struct bnx2x *bp)
 {
 	int num_pages = __bnx2x_get_page_reg_num(bp);
 	int page_write_num = __bnx2x_get_page_write_num(bp);
@@ -718,7 +715,7 @@ static int bnx2x_get_regs_len(struct net_device *dev)
  * ("read address"). There may be more than one write address per "page" and
  * more than one read address per write address.
  */
-static inline void bnx2x_read_pages_regs(struct bnx2x *bp, u32 *p)
+static void bnx2x_read_pages_regs(struct bnx2x *bp, u32 *p)
 {
 	u32 i, j, k, n;
 	/* addresses of the paged registers */
@@ -747,7 +744,7 @@ static inline void bnx2x_read_pages_regs(struct bnx2x *bp, u32 *p)
 	}
 }
 
-static inline void __bnx2x_get_regs(struct bnx2x *bp, u32 *p)
+static void __bnx2x_get_regs(struct bnx2x *bp, u32 *p)
 {
 	u32 i, j;
 
@@ -1433,7 +1430,7 @@ static void bnx2x_get_ringparam(struct net_device *dev,
 	else
 		ering->rx_pending = MAX_RX_AVAIL;
 
-	ering->tx_max_pending = MAX_TX_AVAIL;
+	ering->tx_max_pending = IS_MF_FCOE_AFEX(bp) ? 0 : MAX_TX_AVAIL;
 	ering->tx_pending = bp->tx_ring_size;
 }
 
@@ -1451,7 +1448,7 @@ static int bnx2x_set_ringparam(struct net_device *dev,
 	if ((ering->rx_pending > MAX_RX_AVAIL) ||
 	    (ering->rx_pending < (bp->disable_tpa ? MIN_RX_SIZE_NONTPA :
 						    MIN_RX_SIZE_TPA)) ||
-	    (ering->tx_pending > MAX_TX_AVAIL) ||
+	    (ering->tx_pending > (IS_MF_FCOE_AFEX(bp) ? 0 : MAX_TX_AVAIL)) ||
 	    (ering->tx_pending <= MAX_SKB_FRAGS + 4)) {
 		DP(BNX2X_MSG_ETHTOOL, "Command parameters not supported\n");
 		return -EINVAL;
@@ -2212,7 +2209,7 @@ static void bnx2x_self_test(struct net_device *dev,
 /* ethtool statistics are displayed for all regular ethernet queues and the
  * fcoe L2 queue if not disabled
  */
-static inline int bnx2x_num_stat_queues(struct bnx2x *bp)
+static int bnx2x_num_stat_queues(struct bnx2x *bp)
 {
 	return BNX2X_NUM_ETH_QUEUES(bp);
 }
@@ -2396,10 +2393,7 @@ static int bnx2x_get_rxnfc(struct net_device *dev, struct ethtool_rxnfc *info,
 
 static u32 bnx2x_get_rxfh_indir_size(struct net_device *dev)
 {
-	struct bnx2x *bp = netdev_priv(dev);
-
-	return (bp->multi_mode == ETH_RSS_MODE_DISABLED ?
-		0 : T_ETH_INDIRECTION_TABLE_SIZE);
+	return T_ETH_INDIRECTION_TABLE_SIZE;
 }
 
 static int bnx2x_get_rxfh_indir(struct net_device *dev, u32 *indir)
@@ -2445,7 +2439,7 @@ static int bnx2x_set_rxfh_indir(struct net_device *dev, const u32 *indir)
 		ind_table[i] = indir[i] + bp->fp->cl_id;
 	}
 
-	return bnx2x_config_rss_pf(bp, ind_table, false);
+	return bnx2x_config_rss_eth(bp, ind_table, false);
 }
 
 static const struct ethtool_ops bnx2x_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 b9b26332343..426f77aa721 100644
--- a/drivers/net/ethernet/broadcom/bnx2x/bnx2x_fw_defs.h
+++ b/drivers/net/ethernet/broadcom/bnx2x/bnx2x_fw_defs.h
@@ -387,7 +387,7 @@
 
 #define STATS_QUERY_CMD_COUNT 16
 
-#define NIV_LIST_TABLE_SIZE 4096
+#define AFEX_LIST_TABLE_SIZE 4096
 
 #define INVALID_VNIC_ID	0xFF
 
diff --git a/drivers/net/ethernet/broadcom/bnx2x/bnx2x_hsi.h b/drivers/net/ethernet/broadcom/bnx2x/bnx2x_hsi.h
index dbff5915b81..a440a8ba85f 100644
--- a/drivers/net/ethernet/broadcom/bnx2x/bnx2x_hsi.h
+++ b/drivers/net/ethernet/broadcom/bnx2x/bnx2x_hsi.h
@@ -833,6 +833,7 @@ struct shared_feat_cfg {		 /* NVRAM Offset */
 		#define SHARED_FEAT_CFG_FORCE_SF_MODE_FORCED_SF      0x00000100
 		#define SHARED_FEAT_CFG_FORCE_SF_MODE_SPIO4          0x00000200
 		#define SHARED_FEAT_CFG_FORCE_SF_MODE_SWITCH_INDEPT  0x00000300
+		#define SHARED_FEAT_CFG_FORCE_SF_MODE_AFEX_MODE      0x00000400
 
 	/* The interval in seconds between sending LLDP packets. Set to zero
 	   to disable the feature */
@@ -1235,6 +1236,8 @@ struct drv_func_mb {
 	#define REQ_BC_VER_4_VRFY_FIRST_PHY_OPT_MDL     0x00050006
 	#define DRV_MSG_CODE_VRFY_SPECIFIC_PHY_OPT_MDL  0xa1000000
 	#define REQ_BC_VER_4_VRFY_SPECIFIC_PHY_OPT_MDL  0x00050234
+	#define DRV_MSG_CODE_VRFY_AFEX_SUPPORTED        0xa2000000
+	#define REQ_BC_VER_4_VRFY_AFEX_SUPPORTED        0x00070002
 	#define REQ_BC_VER_4_SFP_TX_DISABLE_SUPPORTED   0x00070014
 	#define REQ_BC_VER_4_PFC_STATS_SUPPORTED        0x00070201
 
@@ -1242,6 +1245,13 @@ struct drv_func_mb {
 	#define DRV_MSG_CODE_DCBX_PMF_DRV_OK            0xb2000000
 
 	#define DRV_MSG_CODE_VF_DISABLED_DONE           0xc0000000
+
+	#define DRV_MSG_CODE_AFEX_DRIVER_SETMAC         0xd0000000
+	#define DRV_MSG_CODE_AFEX_LISTGET_ACK           0xd1000000
+	#define DRV_MSG_CODE_AFEX_LISTSET_ACK           0xd2000000
+	#define DRV_MSG_CODE_AFEX_STATSGET_ACK          0xd3000000
+	#define DRV_MSG_CODE_AFEX_VIFSET_ACK            0xd4000000
+
 	#define DRV_MSG_CODE_DRV_INFO_ACK               0xd8000000
 	#define DRV_MSG_CODE_DRV_INFO_NACK              0xd9000000
 
@@ -1299,6 +1309,14 @@ struct drv_func_mb {
 	#define FW_MSG_CODE_VRFY_OPT_MDL_INVLD_IMG      0xa0200000
 	#define FW_MSG_CODE_VRFY_OPT_MDL_UNAPPROVED     0xa0300000
 	#define FW_MSG_CODE_VF_DISABLED_DONE            0xb0000000
+	#define FW_MSG_CODE_HW_SET_INVALID_IMAGE        0xb0100000
+
+	#define FW_MSG_CODE_AFEX_DRIVER_SETMAC_DONE     0xd0100000
+	#define FW_MSG_CODE_AFEX_LISTGET_ACK            0xd1100000
+	#define FW_MSG_CODE_AFEX_LISTSET_ACK            0xd2100000
+	#define FW_MSG_CODE_AFEX_STATSGET_ACK           0xd3100000
+	#define FW_MSG_CODE_AFEX_VIFSET_ACK             0xd4100000
+
 	#define FW_MSG_CODE_DRV_INFO_ACK                0xd8100000
 	#define FW_MSG_CODE_DRV_INFO_NACK               0xd9100000
 
@@ -1357,6 +1375,12 @@ struct drv_func_mb {
 
 	#define DRV_STATUS_DCBX_EVENT_MASK              0x000f0000
 	#define DRV_STATUS_DCBX_NEGOTIATION_RESULTS     0x00010000
+	#define DRV_STATUS_AFEX_EVENT_MASK              0x03f00000
+	#define DRV_STATUS_AFEX_LISTGET_REQ             0x00100000
+	#define DRV_STATUS_AFEX_LISTSET_REQ             0x00200000
+	#define DRV_STATUS_AFEX_STATSGET_REQ            0x00400000
+	#define DRV_STATUS_AFEX_VIFSET_REQ              0x00800000
+
 	#define DRV_STATUS_DRV_INFO_REQ                 0x04000000
 
 	u32 virt_mac_upper;
@@ -1448,7 +1472,26 @@ struct func_mf_cfg {
 	#define FUNC_MF_CFG_E1HOV_TAG_SHIFT             0
 	#define FUNC_MF_CFG_E1HOV_TAG_DEFAULT         FUNC_MF_CFG_E1HOV_TAG_MASK
 
-	u32 reserved[2];
+	/* afex default VLAN ID - 12 bits */
+	#define FUNC_MF_CFG_AFEX_VLAN_MASK              0x0fff0000
+	#define FUNC_MF_CFG_AFEX_VLAN_SHIFT             16
+
+	u32 afex_config;
+	#define FUNC_MF_CFG_AFEX_COS_FILTER_MASK                     0x000000ff
+	#define FUNC_MF_CFG_AFEX_COS_FILTER_SHIFT                    0
+	#define FUNC_MF_CFG_AFEX_MBA_ENABLED_MASK                    0x0000ff00
+	#define FUNC_MF_CFG_AFEX_MBA_ENABLED_SHIFT                   8
+	#define FUNC_MF_CFG_AFEX_MBA_ENABLED_VAL                     0x00000100
+	#define FUNC_MF_CFG_AFEX_VLAN_MODE_MASK                      0x000f0000
+	#define FUNC_MF_CFG_AFEX_VLAN_MODE_SHIFT                     16
+
+	u32 reserved;
+};
+
+enum mf_cfg_afex_vlan_mode {
+	FUNC_MF_CFG_AFEX_VLAN_TRUNK_MODE = 0,
+	FUNC_MF_CFG_AFEX_VLAN_ACCESS_MODE,
+	FUNC_MF_CFG_AFEX_VLAN_TRUNK_TAG_NATIVE_MODE
 };
 
 /* This structure is not applicable and should not be accessed on 57711 */
@@ -1945,18 +1988,29 @@ struct shmem2_region {
 
 	u32 nvm_retain_bitmap_addr;			/* 0x0070 */
 
-	u32	reserved1;	/* 0x0074 */
+	/* afex support of that driver */
+	u32 afex_driver_support;			/* 0x0074 */
+	#define SHMEM_AFEX_VERSION_MASK                  0x100f
+	#define SHMEM_AFEX_SUPPORTED_VERSION_ONE         0x1001
+	#define SHMEM_AFEX_REDUCED_DRV_LOADED            0x8000
 
-	u32	reserved2[E2_FUNC_MAX];
+	/* driver receives addr in scratchpad to which it should respond */
+	u32 afex_scratchpad_addr_to_write[E2_FUNC_MAX];
 
-	u32	reserved3[E2_FUNC_MAX];/* 0x0088 */
-	u32	reserved4[E2_FUNC_MAX];/* 0x0098 */
+	/* generic params from MCP to driver (value depends on the msg sent
+	 * to driver
+	 */
+	u32 afex_param1_to_driver[E2_FUNC_MAX];		/* 0x0088 */
+	u32 afex_param2_to_driver[E2_FUNC_MAX];		/* 0x0098 */
 
 	u32 swim_base_addr;				/* 0x0108 */
 	u32 swim_funcs;
 	u32 swim_main_cb;
 
-	u32	reserved5[2];
+	/* bitmap notifying which VIF profiles stored in nvram are enabled by
+	 * switch
+	 */
+	u32 afex_profiles_enabled[2];
 
 	/* generic flags controlled by the driver */
 	u32 drv_flags;
@@ -2696,10 +2750,51 @@ union drv_info_to_mcp {
 	struct fcoe_stats_info	fcoe_stat;
 	struct iscsi_stats_info	iscsi_stat;
 };
+
+/* stats collected for afex.
+ * NOTE: structure is exactly as expected to be received by the switch.
+ *       order must remain exactly as is unless protocol changes !
+ */
+struct afex_stats {
+	u32 tx_unicast_frames_hi;
+	u32 tx_unicast_frames_lo;
+	u32 tx_unicast_bytes_hi;
+	u32 tx_unicast_bytes_lo;
+	u32 tx_multicast_frames_hi;
+	u32 tx_multicast_frames_lo;
+	u32 tx_multicast_bytes_hi;
+	u32 tx_multicast_bytes_lo;
+	u32 tx_broadcast_frames_hi;
+	u32 tx_broadcast_frames_lo;
+	u32 tx_broadcast_bytes_hi;
+	u32 tx_broadcast_bytes_lo;
+	u32 tx_frames_discarded_hi;
+	u32 tx_frames_discarded_lo;
+	u32 tx_frames_dropped_hi;
+	u32 tx_frames_dropped_lo;
+
+	u32 rx_unicast_frames_hi;
+	u32 rx_unicast_frames_lo;
+	u32 rx_unicast_bytes_hi;
+	u32 rx_unicast_bytes_lo;
+	u32 rx_multicast_frames_hi;
+	u32 rx_multicast_frames_lo;
+	u32 rx_multicast_bytes_hi;
+	u32 rx_multicast_bytes_lo;
+	u32 rx_broadcast_frames_hi;
+	u32 rx_broadcast_frames_lo;
+	u32 rx_broadcast_bytes_hi;
+	u32 rx_broadcast_bytes_lo;
+	u32 rx_frames_discarded_hi;
+	u32 rx_frames_discarded_lo;
+	u32 rx_frames_dropped_hi;
+	u32 rx_frames_dropped_lo;
+};
+
 #define BCM_5710_FW_MAJOR_VERSION			7
 #define BCM_5710_FW_MINOR_VERSION			2
-#define BCM_5710_FW_REVISION_VERSION		16
-#define BCM_5710_FW_ENGINEERING_VERSION		0
+#define BCM_5710_FW_REVISION_VERSION			51
+#define BCM_5710_FW_ENGINEERING_VERSION			0
 #define BCM_5710_FW_COMPILE_FLAGS			1
 
 
@@ -3389,7 +3484,7 @@ struct client_init_tx_data {
 #define CLIENT_INIT_TX_DATA_RESERVED1 (0xFFF<<4)
 #define CLIENT_INIT_TX_DATA_RESERVED1_SHIFT 4
 	u8 default_vlan_flg;
-	u8 reserved2;
+	u8 force_default_pri_flg;
 	__le32 reserved3;
 };
 
@@ -4375,8 +4470,21 @@ struct fcoe_statistics_params {
 
 
 /*
+ * The data afex vif list ramrod need
+ */
+struct afex_vif_list_ramrod_data {
+	u8 afex_vif_list_command;
+	u8 func_bit_map;
+	__le16 vif_list_index;
+	u8 func_to_clear;
+	u8 echo;
+	__le16 reserved1;
+};
+
+
+/*
  * cfc delete event data
-*/
+ */
 struct cfc_del_event_data {
 	u32 cid;
 	u32 reserved0;
@@ -4448,6 +4556,65 @@ struct cmng_struct_per_port {
 	struct cmng_flags_per_port flags;
 };
 
+/*
+ * a single rate shaping counter. can be used as protocol or vnic counter
+ */
+struct rate_shaping_counter {
+	u32 quota;
+#if defined(__BIG_ENDIAN)
+	u16 __reserved0;
+	u16 rate;
+#elif defined(__LITTLE_ENDIAN)
+	u16 rate;
+	u16 __reserved0;
+#endif
+};
+
+/*
+ * per-vnic rate shaping variables
+ */
+struct rate_shaping_vars_per_vn {
+	struct rate_shaping_counter vn_counter;
+};
+
+/*
+ * per-vnic fairness variables
+ */
+struct fairness_vars_per_vn {
+	u32 cos_credit_delta[MAX_COS_NUMBER];
+	u32 vn_credit_delta;
+	u32 __reserved0;
+};
+
+/*
+ * cmng port init state
+ */
+struct cmng_vnic {
+	struct rate_shaping_vars_per_vn vnic_max_rate[4];
+	struct fairness_vars_per_vn vnic_min_rate[4];
+};
+
+/*
+ * cmng port init state
+ */
+struct cmng_init {
+	struct cmng_struct_per_port port;
+	struct cmng_vnic vnic;
+};
+
+
+/*
+ * driver parameters for congestion management init, all rates are in Mbps
+ */
+struct cmng_init_input {
+	u32 port_rate;
+	u16 vnic_min_rate[4];
+	u16 vnic_max_rate[4];
+	u16 cos_min_rate[MAX_COS_NUMBER];
+	u16 cos_to_pause_mask[MAX_COS_NUMBER];
+	struct cmng_flags_per_port flags;
+};
+
 
 /*
  * Protocol-common command ID for slow path elements
@@ -4462,7 +4629,7 @@ enum common_spqe_cmd_id {
 	RAMROD_CMD_ID_COMMON_STAT_QUERY,
 	RAMROD_CMD_ID_COMMON_STOP_TRAFFIC,
 	RAMROD_CMD_ID_COMMON_START_TRAFFIC,
-	RAMROD_CMD_ID_COMMON_RESERVED1,
+	RAMROD_CMD_ID_COMMON_AFEX_VIF_LISTS,
 	MAX_COMMON_SPQE_CMD_ID
 };
 
@@ -4670,6 +4837,17 @@ struct malicious_vf_event_data {
 };
 
 /*
+ * vif list event data
+ */
+struct vif_list_event_data {
+	u8 func_bit_map;
+	u8 echo;
+	__le16 reserved0;
+	__le32 reserved1;
+	__le32 reserved2;
+};
+
+/*
  * union for all event ring message types
  */
 union event_data {
@@ -4678,6 +4856,7 @@ union event_data {
 	struct cfc_del_event_data cfc_del_event;
 	struct vf_flr_event_data vf_flr_event;
 	struct malicious_vf_event_data malicious_vf_event;
+	struct vif_list_event_data vif_list_event;
 };
 
 
@@ -4743,7 +4922,7 @@ enum event_ring_opcode {
 	EVENT_RING_OPCODE_FORWARD_SETUP,
 	EVENT_RING_OPCODE_RSS_UPDATE_RULES,
 	EVENT_RING_OPCODE_FUNCTION_UPDATE,
-	EVENT_RING_OPCODE_RESERVED1,
+	EVENT_RING_OPCODE_AFEX_VIF_LISTS,
 	EVENT_RING_OPCODE_SET_MAC,
 	EVENT_RING_OPCODE_CLASSIFICATION_RULES,
 	EVENT_RING_OPCODE_FILTERS_RULES,
@@ -4763,16 +4942,6 @@ enum fairness_mode {
 
 
 /*
- * per-vnic fairness variables
- */
-struct fairness_vars_per_vn {
-	u32 cos_credit_delta[MAX_COS_NUMBER];
-	u32 vn_credit_delta;
-	u32 __reserved0;
-};
-
-
-/*
  * Priority and cos
  */
 struct priority_cos {
@@ -4800,12 +4969,27 @@ struct flow_control_configuration {
 struct function_start_data {
 	__le16 function_mode;
 	__le16 sd_vlan_tag;
-	u16 reserved;
+	__le16 vif_id;
 	u8 path_id;
 	u8 network_cos_mode;
 };
 
 
+struct function_update_data {
+	u8 vif_id_change_flg;
+	u8 afex_default_vlan_change_flg;
+	u8 allowed_priorities_change_flg;
+	u8 network_cos_mode_change_flg;
+	__le16 vif_id;
+	__le16 afex_default_vlan;
+	u8 allowed_priorities;
+	u8 network_cos_mode;
+	u8 lb_mode_en;
+	u8 reserved0;
+	__le32 reserved1;
+};
+
+
 /*
  * FW version stored in the Xstorm RAM
  */
@@ -5003,7 +5187,7 @@ enum mf_mode {
 	SINGLE_FUNCTION,
 	MULTI_FUNCTION_SD,
 	MULTI_FUNCTION_SI,
-	MULTI_FUNCTION_RESERVED,
+	MULTI_FUNCTION_AFEX,
 	MAX_MF_MODE
 };
 
@@ -5128,6 +5312,7 @@ union protocol_common_specific_data {
 	u8 protocol_data[8];
 	struct regpair phy_address;
 	struct regpair mac_config_addr;
+	struct afex_vif_list_ramrod_data afex_vif_list_data;
 };
 
 /*
@@ -5140,29 +5325,6 @@ struct protocol_common_spe {
 
 
 /*
- * a single rate shaping counter. can be used as protocol or vnic counter
- */
-struct rate_shaping_counter {
-	u32 quota;
-#if defined(__BIG_ENDIAN)
-	u16 __reserved0;
-	u16 rate;
-#elif defined(__LITTLE_ENDIAN)
-	u16 rate;
-	u16 __reserved0;
-#endif
-};
-
-
-/*
- * per-vnic rate shaping variables
- */
-struct rate_shaping_vars_per_vn {
-	struct rate_shaping_counter vn_counter;
-};
-
-
-/*
  * The send queue element
  */
 struct slow_path_element {
@@ -5330,6 +5492,18 @@ enum vf_pf_channel_state {
 
 
 /*
+ * vif_list_rule_kind
+ */
+enum vif_list_rule_kind {
+	VIF_LIST_RULE_SET,
+	VIF_LIST_RULE_GET,
+	VIF_LIST_RULE_CLEAR_ALL,
+	VIF_LIST_RULE_CLEAR_FUNC,
+	MAX_VIF_LIST_RULE_KIND
+};
+
+
+/*
  * zone A per-queue data
  */
 struct xstorm_queue_zone_data {
diff --git a/drivers/net/ethernet/broadcom/bnx2x/bnx2x_init.h b/drivers/net/ethernet/broadcom/bnx2x/bnx2x_init.h
index 29f5c3cca31..559c396d45c 100644
--- a/drivers/net/ethernet/broadcom/bnx2x/bnx2x_init.h
+++ b/drivers/net/ethernet/broadcom/bnx2x/bnx2x_init.h
@@ -125,7 +125,7 @@ enum {
 	MODE_MF                        = 0x00000100,
 	MODE_MF_SD                     = 0x00000200,
 	MODE_MF_SI                     = 0x00000400,
-	MODE_MF_NIV                    = 0x00000800,
+	MODE_MF_AFEX                   = 0x00000800,
 	MODE_E3_A0                     = 0x00001000,
 	MODE_E3_B0                     = 0x00002000,
 	MODE_COS3                      = 0x00004000,
@@ -241,7 +241,8 @@ static inline void bnx2x_map_q_cos(struct bnx2x *bp, u32 q_num, u32 new_cos)
 			REG_WR(bp, reg_addr, reg_bit_map | q_bit_map);
 
 			/* set/clear queue bit in command-queue bit map
-			(E2/E3A0 only, valid COS values are 0/1) */
+			 * (E2/E3A0 only, valid COS values are 0/1)
+			 */
 			if (!(INIT_MODE_FLAGS(bp) & MODE_E3_B0)) {
 				reg_addr = BNX2X_Q_CMDQ_REG_ADDR(pf_q_num);
 				reg_bit_map = REG_RD(bp, reg_addr);
@@ -277,7 +278,215 @@ static inline void bnx2x_dcb_config_qm(struct bnx2x *bp, enum cos_mode mode,
 }
 
 
-/* Returns the index of start or end of a specific block stage in ops array*/
+/* congestion managment port init api description
+ * the api works as follows:
+ * the driver should pass the cmng_init_input struct, the port_init function
+ * will prepare the required internal ram structure which will be passed back
+ * to the driver (cmng_init) that will write it into the internal ram.
+ *
+ * IMPORTANT REMARKS:
+ * 1. the cmng_init struct does not represent the contiguous internal ram
+ *    structure. the driver should use the XSTORM_CMNG_PERPORT_VARS_OFFSET
+ *    offset in order to write the port sub struct and the
+ *    PFID_FROM_PORT_AND_VNIC offset for writing the vnic sub struct (in other
+ *    words - don't use memcpy!).
+ * 2. although the cmng_init struct is filled for the maximal vnic number
+ *    possible, the driver should only write the valid vnics into the internal
+ *    ram according to the appropriate port mode.
+ */
+#define BITS_TO_BYTES(x) ((x)/8)
+
+/* CMNG constants, as derived from system spec calculations */
+
+/* default MIN rate in case VNIC min rate is configured to zero- 100Mbps */
+#define DEF_MIN_RATE 100
+
+/* resolution of the rate shaping timer - 400 usec */
+#define RS_PERIODIC_TIMEOUT_USEC 400
+
+/* number of bytes in single QM arbitration cycle -
+ * coefficient for calculating the fairness timer
+ */
+#define QM_ARB_BYTES 160000
+
+/* resolution of Min algorithm 1:100 */
+#define MIN_RES 100
+
+/* how many bytes above threshold for
+ * the minimal credit of Min algorithm
+ */
+#define MIN_ABOVE_THRESH 32768
+
+/* Fairness algorithm integration time coefficient -
+ * for calculating the actual Tfair
+ */
+#define T_FAIR_COEF ((MIN_ABOVE_THRESH + QM_ARB_BYTES) * 8 * MIN_RES)
+
+/* Memory of fairness algorithm - 2 cycles */
+#define FAIR_MEM 2
+#define SAFC_TIMEOUT_USEC 52
+
+#define SDM_TICKS 4
+
+
+static inline void bnx2x_init_max(const struct cmng_init_input *input_data,
+				  u32 r_param, struct cmng_init *ram_data)
+{
+	u32 vnic;
+	struct cmng_vnic *vdata = &ram_data->vnic;
+	struct cmng_struct_per_port *pdata = &ram_data->port;
+	/* rate shaping per-port variables
+	 * 100 micro seconds in SDM ticks = 25
+	 * since each tick is 4 microSeconds
+	 */
+
+	pdata->rs_vars.rs_periodic_timeout =
+	RS_PERIODIC_TIMEOUT_USEC / SDM_TICKS;
+
+	/* this is the threshold below which no timer arming will occur.
+	 * 1.25 coefficient is for the threshold to be a little bigger
+	 * then the real time to compensate for timer in-accuracy
+	 */
+	pdata->rs_vars.rs_threshold =
+	(5 * RS_PERIODIC_TIMEOUT_USEC * r_param)/4;
+
+	/* rate shaping per-vnic variables */
+	for (vnic = 0; vnic < BNX2X_PORT2_MODE_NUM_VNICS; vnic++) {
+		/* global vnic counter */
+		vdata->vnic_max_rate[vnic].vn_counter.rate =
+		input_data->vnic_max_rate[vnic];
+		/* maximal Mbps for this vnic
+		 * the quota in each timer period - number of bytes
+		 * transmitted in this period
+		 */
+		vdata->vnic_max_rate[vnic].vn_counter.quota =
+			RS_PERIODIC_TIMEOUT_USEC *
+			(u32)vdata->vnic_max_rate[vnic].vn_counter.rate / 8;
+	}
+
+}
+
+static inline void bnx2x_init_min(const struct cmng_init_input *input_data,
+				  u32 r_param, struct cmng_init *ram_data)
+{
+	u32 vnic, fair_periodic_timeout_usec, vnicWeightSum, tFair;
+	struct cmng_vnic *vdata = &ram_data->vnic;
+	struct cmng_struct_per_port *pdata = &ram_data->port;
+
+	/* this is the resolution of the fairness timer */
+	fair_periodic_timeout_usec = QM_ARB_BYTES / r_param;
+
+	/* fairness per-port variables
+	 * for 10G it is 1000usec. for 1G it is 10000usec.
+	 */
+	tFair = T_FAIR_COEF / input_data->port_rate;
+
+	/* this is the threshold below which we won't arm the timer anymore */
+	pdata->fair_vars.fair_threshold = QM_ARB_BYTES;
+
+	/* we multiply by 1e3/8 to get bytes/msec. We don't want the credits
+	 * to pass a credit of the T_FAIR*FAIR_MEM (algorithm resolution)
+	 */
+	pdata->fair_vars.upper_bound = r_param * tFair * FAIR_MEM;
+
+	/* since each tick is 4 microSeconds */
+	pdata->fair_vars.fairness_timeout =
+				fair_periodic_timeout_usec / SDM_TICKS;
+
+	/* calculate sum of weights */
+	vnicWeightSum = 0;
+
+	for (vnic = 0; vnic < BNX2X_PORT2_MODE_NUM_VNICS; vnic++)
+		vnicWeightSum += input_data->vnic_min_rate[vnic];
+
+	/* global vnic counter */
+	if (vnicWeightSum > 0) {
+		/* fairness per-vnic variables */
+		for (vnic = 0; vnic < BNX2X_PORT2_MODE_NUM_VNICS; vnic++) {
+			/* this is the credit for each period of the fairness
+			 * algorithm - number of bytes in T_FAIR (this vnic
+			 * share of the port rate)
+			 */
+			vdata->vnic_min_rate[vnic].vn_credit_delta =
+				(u32)input_data->vnic_min_rate[vnic] * 100 *
+				(T_FAIR_COEF / (8 * 100 * vnicWeightSum));
+			if (vdata->vnic_min_rate[vnic].vn_credit_delta <
+			    pdata->fair_vars.fair_threshold +
+			    MIN_ABOVE_THRESH) {
+				vdata->vnic_min_rate[vnic].vn_credit_delta =
+					pdata->fair_vars.fair_threshold +
+					MIN_ABOVE_THRESH;
+			}
+		}
+	}
+}
+
+static inline void bnx2x_init_fw_wrr(const struct cmng_init_input *input_data,
+				     u32 r_param, struct cmng_init *ram_data)
+{
+	u32 vnic, cos;
+	u32 cosWeightSum = 0;
+	struct cmng_vnic *vdata = &ram_data->vnic;
+	struct cmng_struct_per_port *pdata = &ram_data->port;
+
+	for (cos = 0; cos < MAX_COS_NUMBER; cos++)
+		cosWeightSum += input_data->cos_min_rate[cos];
+
+	if (cosWeightSum > 0) {
+
+		for (vnic = 0; vnic < BNX2X_PORT2_MODE_NUM_VNICS; vnic++) {
+			/* Since cos and vnic shouldn't work together the rate
+			 * to divide between the coses is the port rate.
+			 */
+			u32 *ccd = vdata->vnic_min_rate[vnic].cos_credit_delta;
+			for (cos = 0; cos < MAX_COS_NUMBER; cos++) {
+				/* this is the credit for each period of
+				 * the fairness algorithm - number of bytes
+				 * in T_FAIR (this cos share of the vnic rate)
+				 */
+				ccd[cos] =
+				    (u32)input_data->cos_min_rate[cos] * 100 *
+				    (T_FAIR_COEF / (8 * 100 * cosWeightSum));
+				 if (ccd[cos] < pdata->fair_vars.fair_threshold
+						+ MIN_ABOVE_THRESH) {
+					ccd[cos] =
+					    pdata->fair_vars.fair_threshold +
+					    MIN_ABOVE_THRESH;
+				}
+			}
+		}
+	}
+}
+
+static inline void bnx2x_init_safc(const struct cmng_init_input *input_data,
+				   struct cmng_init *ram_data)
+{
+	/* in microSeconds */
+	ram_data->port.safc_vars.safc_timeout_usec = SAFC_TIMEOUT_USEC;
+}
+
+/* Congestion management port init */
+static inline void bnx2x_init_cmng(const struct cmng_init_input *input_data,
+				   struct cmng_init *ram_data)
+{
+	u32 r_param;
+	memset(ram_data, 0, sizeof(struct cmng_init));
+
+	ram_data->port.flags = input_data->flags;
+
+	/* number of bytes transmitted in a rate of 10Gbps
+	 * in one usec = 1.25KB.
+	 */
+	r_param = BITS_TO_BYTES(input_data->port_rate);
+	bnx2x_init_max(input_data, r_param, ram_data);
+	bnx2x_init_min(input_data, r_param, ram_data);
+	bnx2x_init_fw_wrr(input_data, r_param, ram_data);
+	bnx2x_init_safc(input_data, ram_data);
+}
+
+
+
+/* Returns the index of start or end of a specific block stage in ops array */
 #define BLOCK_OPS_IDX(block, stage, end) \
 			(2*(((block)*NUM_OF_INIT_PHASES) + (stage)) + (end))
 
@@ -499,9 +708,7 @@ static inline void bnx2x_disable_blocks_parity(struct bnx2x *bp)
 	bnx2x_set_mcp_parity(bp, false);
 }
 
-/**
- * Clear the parity error status registers.
- */
+/* Clear the parity error status registers. */
 static inline void bnx2x_clear_blocks_parity(struct bnx2x *bp)
 {
 	int i;
diff --git a/drivers/net/ethernet/broadcom/bnx2x/bnx2x_link.c b/drivers/net/ethernet/broadcom/bnx2x/bnx2x_link.c
index 64392ec410a..a3fb7215cd8 100644
--- a/drivers/net/ethernet/broadcom/bnx2x/bnx2x_link.c
+++ b/drivers/net/ethernet/broadcom/bnx2x/bnx2x_link.c
@@ -138,7 +138,6 @@
 
 
 
-/* */
 #define SFP_EEPROM_CON_TYPE_ADDR		0x2
 	#define SFP_EEPROM_CON_TYPE_VAL_LC	0x7
 	#define SFP_EEPROM_CON_TYPE_VAL_COPPER	0x21
@@ -404,8 +403,7 @@ static void bnx2x_ets_e2e3a0_disabled(struct link_params *params)
 
 	DP(NETIF_MSG_LINK, "ETS E2E3 disabled configuration\n");
 
-	/*
-	 * mapping between entry  priority to client number (0,1,2 -debug and
+	/* mapping between entry  priority to client number (0,1,2 -debug and
 	 * management clients, 3 - COS0 client, 4 - COS client)(HIGHEST)
 	 * 3bits client num.
 	 *   PRI4    |    PRI3    |    PRI2    |    PRI1    |    PRI0
@@ -413,8 +411,7 @@ static void bnx2x_ets_e2e3a0_disabled(struct link_params *params)
 	 */
 
 	REG_WR(bp, NIG_REG_P0_TX_ARB_PRIORITY_CLIENT, 0x4688);
-	/*
-	 * Bitmap of 5bits length. Each bit specifies whether the entry behaves
+	/* Bitmap of 5bits length. Each bit specifies whether the entry behaves
 	 * as strict.  Bits 0,1,2 - debug and management entries, 3 -
 	 * COS0 entry, 4 - COS1 entry.
 	 * COS1 | COS0 | DEBUG1 | DEBUG0 | MGMT
@@ -425,13 +422,11 @@ static void bnx2x_ets_e2e3a0_disabled(struct link_params *params)
 	REG_WR(bp, NIG_REG_P0_TX_ARB_CLIENT_IS_STRICT, 0x7);
 	/* defines which entries (clients) are subjected to WFQ arbitration */
 	REG_WR(bp, NIG_REG_P0_TX_ARB_CLIENT_IS_SUBJECT2WFQ, 0);
-	/*
-	 * For strict priority entries defines the number of consecutive
+	/* For strict priority entries defines the number of consecutive
 	 * slots for the highest priority.
 	 */
 	REG_WR(bp, NIG_REG_P0_TX_ARB_NUM_STRICT_ARB_SLOTS, 0x100);
-	/*
-	 * mapping between the CREDIT_WEIGHT registers and actual client
+	/* mapping between the CREDIT_WEIGHT registers and actual client
 	 * numbers
 	 */
 	REG_WR(bp, NIG_REG_P0_TX_ARB_CLIENT_CREDIT_MAP, 0);
@@ -443,8 +438,7 @@ static void bnx2x_ets_e2e3a0_disabled(struct link_params *params)
 	REG_WR(bp, PBF_REG_HIGH_PRIORITY_COS_NUM, 0);
 	/* ETS mode disable */
 	REG_WR(bp, PBF_REG_ETS_ENABLED, 0);
-	/*
-	 * If ETS mode is enabled (there is no strict priority) defines a WFQ
+	/* If ETS mode is enabled (there is no strict priority) defines a WFQ
 	 * weight for COS0/COS1.
 	 */
 	REG_WR(bp, PBF_REG_COS0_WEIGHT, 0x2710);
@@ -471,10 +465,9 @@ static u32 bnx2x_ets_get_min_w_val_nig(const struct link_vars *vars)
 			min_w_val = ETS_E3B0_NIG_MIN_W_VAL_UP_TO_10GBPS;
 	} else
 		min_w_val = ETS_E3B0_NIG_MIN_W_VAL_20GBPS;
-	/**
-	 *  If the link isn't up (static configuration for example ) The
-	 *  link will be according to 20GBPS.
-	*/
+	/* If the link isn't up (static configuration for example ) The
+	 * link will be according to 20GBPS.
+	 */
 	return min_w_val;
 }
 /******************************************************************************
@@ -538,8 +531,7 @@ static void bnx2x_ets_e3b0_nig_disabled(const struct link_params *params,
 	struct bnx2x *bp = params->bp;
 	const u8 port = params->port;
 	const u32 min_w_val = bnx2x_ets_get_min_w_val_nig(vars);
-	/**
-	 * mapping between entry  priority to client number (0,1,2 -debug and
+	/* Mapping between entry  priority to client number (0,1,2 -debug and
 	 * management clients, 3 - COS0 client, 4 - COS1, ... 8 -
 	 * COS5)(HIGHEST) 4bits client num.TODO_ETS - Should be done by
 	 * reset value or init tool
@@ -551,18 +543,14 @@ static void bnx2x_ets_e3b0_nig_disabled(const struct link_params *params,
 		REG_WR(bp, NIG_REG_P0_TX_ARB_PRIORITY_CLIENT2_LSB, 0x76543210);
 		REG_WR(bp, NIG_REG_P0_TX_ARB_PRIORITY_CLIENT2_MSB, 0x8);
 	}
-	/**
-	* For strict priority entries defines the number of consecutive
-	* slots for the highest priority.
-	*/
-	/* TODO_ETS - Should be done by reset value or init tool */
+	/* For strict priority entries defines the number of consecutive
+	 * slots for the highest priority.
+	 */
 	REG_WR(bp, (port) ? NIG_REG_P1_TX_ARB_NUM_STRICT_ARB_SLOTS :
 		   NIG_REG_P1_TX_ARB_NUM_STRICT_ARB_SLOTS, 0x100);
-	/**
-	 * mapping between the CREDIT_WEIGHT registers and actual client
+	/* Mapping between the CREDIT_WEIGHT registers and actual client
 	 * numbers
 	 */
-	/* TODO_ETS - Should be done by reset value or init tool */
 	if (port) {
 		/*Port 1 has 6 COS*/
 		REG_WR(bp, NIG_REG_P1_TX_ARB_CLIENT_CREDIT_MAP2_LSB, 0x210543);
@@ -574,8 +562,7 @@ static void bnx2x_ets_e3b0_nig_disabled(const struct link_params *params,
 		REG_WR(bp, NIG_REG_P0_TX_ARB_CLIENT_CREDIT_MAP2_MSB, 0x5);
 	}
 
-	/**
-	 * Bitmap of 5bits length. Each bit specifies whether the entry behaves
+	/* Bitmap of 5bits length. Each bit specifies whether the entry behaves
 	 * as strict.  Bits 0,1,2 - debug and management entries, 3 -
 	 * COS0 entry, 4 - COS1 entry.
 	 * COS1 | COS0 | DEBUG1 | DEBUG0 | MGMT
@@ -590,13 +577,12 @@ static void bnx2x_ets_e3b0_nig_disabled(const struct link_params *params,
 	REG_WR(bp, (port) ? NIG_REG_P1_TX_ARB_CLIENT_IS_SUBJECT2WFQ :
 		   NIG_REG_P0_TX_ARB_CLIENT_IS_SUBJECT2WFQ, 0);
 
-	/**
-	* Please notice the register address are note continuous and a
-	* for here is note appropriate.In 2 port mode port0 only COS0-5
-	* can be used. DEBUG1,DEBUG1,MGMT are never used for WFQ* In 4
-	* port mode port1 only COS0-2 can be used. DEBUG1,DEBUG1,MGMT
-	* are never used for WFQ
-	*/
+	/* Please notice the register address are note continuous and a
+	 * for here is note appropriate.In 2 port mode port0 only COS0-5
+	 * can be used. DEBUG1,DEBUG1,MGMT are never used for WFQ* In 4
+	 * port mode port1 only COS0-2 can be used. DEBUG1,DEBUG1,MGMT
+	 * are never used for WFQ
+	 */
 	REG_WR(bp, (port) ? NIG_REG_P1_TX_ARB_CREDIT_WEIGHT_0 :
 		   NIG_REG_P0_TX_ARB_CREDIT_WEIGHT_0, 0x0);
 	REG_WR(bp, (port) ? NIG_REG_P1_TX_ARB_CREDIT_WEIGHT_1 :
@@ -633,10 +619,9 @@ static void bnx2x_ets_e3b0_set_credit_upper_bound_pbf(
 	u32 base_upper_bound = 0;
 	u8 max_cos = 0;
 	u8 i = 0;
-	/**
-	* In 2 port mode port0 has COS0-5 that can be used for WFQ.In 4
-	* port mode port1 has COS0-2 that can be used for WFQ.
-	*/
+	/* In 2 port mode port0 has COS0-5 that can be used for WFQ.In 4
+	 * port mode port1 has COS0-2 that can be used for WFQ.
+	 */
 	if (!port) {
 		base_upper_bound = PBF_REG_COS0_UPPER_BOUND_P0;
 		max_cos = DCBX_E3B0_MAX_NUM_COS_PORT0;
@@ -666,8 +651,7 @@ static void bnx2x_ets_e3b0_pbf_disabled(const struct link_params *params)
 	u32 base_weight = 0;
 	u8 max_cos = 0;
 
-	/**
-	 * mapping between entry  priority to client number 0 - COS0
+	/* Mapping between entry  priority to client number 0 - COS0
 	 * client, 2 - COS1, ... 5 - COS5)(HIGHEST) 4bits client num.
 	 * TODO_ETS - Should be done by reset value or init tool
 	 */
@@ -695,10 +679,9 @@ static void bnx2x_ets_e3b0_pbf_disabled(const struct link_params *params)
 
 	REG_WR(bp, (port) ? PBF_REG_ETS_ARB_CLIENT_IS_SUBJECT2WFQ_P1 :
 		   PBF_REG_ETS_ARB_CLIENT_IS_SUBJECT2WFQ_P0 , 0);
-	/**
-	* In 2 port mode port0 has COS0-5 that can be used for WFQ.
-	* In 4 port mode port1 has COS0-2 that can be used for WFQ.
-	*/
+	/* In 2 port mode port0 has COS0-5 that can be used for WFQ.
+	 * In 4 port mode port1 has COS0-2 that can be used for WFQ.
+	 */
 	if (!port) {
 		base_weight = PBF_REG_COS0_WEIGHT_P0;
 		max_cos = DCBX_E3B0_MAX_NUM_COS_PORT0;
@@ -738,7 +721,7 @@ static int bnx2x_ets_e3b0_disabled(const struct link_params *params,
 /******************************************************************************
 * Description:
 *	Disable will return basicly the values to init values.
-*.
+*
 ******************************************************************************/
 int bnx2x_ets_disabled(struct link_params *params,
 		      struct link_vars *vars)
@@ -867,7 +850,7 @@ static int bnx2x_ets_e3b0_set_cos_bw(struct bnx2x *bp,
 /******************************************************************************
 * Description:
 *	Calculate the total BW.A value of 0 isn't legal.
-*.
+*
 ******************************************************************************/
 static int bnx2x_ets_e3b0_get_total_bw(
 	const struct link_params *params,
@@ -879,7 +862,6 @@ static int bnx2x_ets_e3b0_get_total_bw(
 	u8 is_bw_cos_exist = 0;
 
 	*total_bw = 0 ;
-
 	/* Calculate total BW requested */
 	for (cos_idx = 0; cos_idx < ets_params->num_of_cos; cos_idx++) {
 		if (ets_params->cos[cos_idx].state == bnx2x_cos_state_bw) {
@@ -887,10 +869,9 @@ static int bnx2x_ets_e3b0_get_total_bw(
 			if (!ets_params->cos[cos_idx].params.bw_params.bw) {
 				DP(NETIF_MSG_LINK, "bnx2x_ets_E3B0_config BW"
 						   "was set to 0\n");
-				/*
-				 * This is to prevent a state when ramrods
+				/* This is to prevent a state when ramrods
 				 * can't be sent
-				*/
+				 */
 				ets_params->cos[cos_idx].params.bw_params.bw
 					 = 1;
 			}
@@ -908,8 +889,7 @@ static int bnx2x_ets_e3b0_get_total_bw(
 		}
 		DP(NETIF_MSG_LINK,
 		   "bnx2x_ets_E3B0_config total BW should be 100\n");
-		/*
-		 * We can handle a case whre the BW isn't 100 this can happen
+		/* We can handle a case whre the BW isn't 100 this can happen
 		 * if the TC are joined.
 		 */
 	}
@@ -919,7 +899,7 @@ static int bnx2x_ets_e3b0_get_total_bw(
 /******************************************************************************
 * Description:
 *	Invalidate all the sp_pri_to_cos.
-*.
+*
 ******************************************************************************/
 static void bnx2x_ets_e3b0_sp_pri_to_cos_init(u8 *sp_pri_to_cos)
 {
@@ -931,7 +911,7 @@ static void bnx2x_ets_e3b0_sp_pri_to_cos_init(u8 *sp_pri_to_cos)
 * Description:
 *	Calculate and set the SP (ARB_PRIORITY_CLIENT) NIG and PBF registers
 *	according to sp_pri_to_cos.
-*.
+*
 ******************************************************************************/
 static int bnx2x_ets_e3b0_sp_pri_to_cos_set(const struct link_params *params,
 					    u8 *sp_pri_to_cos, const u8 pri,
@@ -964,7 +944,7 @@ static int bnx2x_ets_e3b0_sp_pri_to_cos_set(const struct link_params *params,
 * Description:
 *	Returns the correct value according to COS and priority in
 *	the sp_pri_cli register.
-*.
+*
 ******************************************************************************/
 static u64 bnx2x_e3b0_sp_get_pri_cli_reg(const u8 cos, const u8 cos_offset,
 					 const u8 pri_set,
@@ -981,7 +961,7 @@ static u64 bnx2x_e3b0_sp_get_pri_cli_reg(const u8 cos, const u8 cos_offset,
 * Description:
 *	Returns the correct value according to COS and priority in the
 *	sp_pri_cli register for NIG.
-*.
+*
 ******************************************************************************/
 static u64 bnx2x_e3b0_sp_get_pri_cli_reg_nig(const u8 cos, const u8 pri_set)
 {
@@ -997,7 +977,7 @@ static u64 bnx2x_e3b0_sp_get_pri_cli_reg_nig(const u8 cos, const u8 pri_set)
 * Description:
 *	Returns the correct value according to COS and priority in the
 *	sp_pri_cli register for PBF.
-*.
+*
 ******************************************************************************/
 static u64 bnx2x_e3b0_sp_get_pri_cli_reg_pbf(const u8 cos, const u8 pri_set)
 {
@@ -1013,7 +993,7 @@ static u64 bnx2x_e3b0_sp_get_pri_cli_reg_pbf(const u8 cos, const u8 pri_set)
 * Description:
 *	Calculate and set the SP (ARB_PRIORITY_CLIENT) NIG and PBF registers
 *	according to sp_pri_to_cos.(which COS has higher priority)
-*.
+*
 ******************************************************************************/
 static int bnx2x_ets_e3b0_sp_set_pri_cli_reg(const struct link_params *params,
 					     u8 *sp_pri_to_cos)
@@ -1149,8 +1129,7 @@ int bnx2x_ets_e3b0_config(const struct link_params *params,
 		return -EINVAL;
 	}
 
-	/*
-	 * Upper bound is set according to current link speed (min_w_val
+	/* Upper bound is set according to current link speed (min_w_val
 	 * should be the same for upper bound and COS credit val).
 	 */
 	bnx2x_ets_e3b0_set_credit_upper_bound_nig(params, min_w_val_nig);
@@ -1160,8 +1139,7 @@ int bnx2x_ets_e3b0_config(const struct link_params *params,
 	for (cos_entry = 0; cos_entry < ets_params->num_of_cos; cos_entry++) {
 		if (bnx2x_cos_state_bw == ets_params->cos[cos_entry].state) {
 			cos_bw_bitmap |= (1 << cos_entry);
-			/*
-			 * The function also sets the BW in HW(not the mappin
+			/* The function also sets the BW in HW(not the mappin
 			 * yet)
 			 */
 			bnx2x_status = bnx2x_ets_e3b0_set_cos_bw(
@@ -1217,14 +1195,12 @@ static void bnx2x_ets_bw_limit_common(const struct link_params *params)
 	/* ETS disabled configuration */
 	struct bnx2x *bp = params->bp;
 	DP(NETIF_MSG_LINK, "ETS enabled BW limit configuration\n");
-	/*
-	 * defines which entries (clients) are subjected to WFQ arbitration
+	/* Defines which entries (clients) are subjected to WFQ arbitration
 	 * COS0 0x8
 	 * COS1 0x10
 	 */
 	REG_WR(bp, NIG_REG_P0_TX_ARB_CLIENT_IS_SUBJECT2WFQ, 0x18);
-	/*
-	 * mapping between the ARB_CREDIT_WEIGHT registers and actual
+	/* Mapping between the ARB_CREDIT_WEIGHT registers and actual
 	 * client numbers (WEIGHT_0 does not actually have to represent
 	 * client 0)
 	 *    PRI4    |    PRI3    |    PRI2    |    PRI1    |    PRI0
@@ -1242,8 +1218,7 @@ static void bnx2x_ets_bw_limit_common(const struct link_params *params)
 
 	/* Defines the number of consecutive slots for the strict priority */
 	REG_WR(bp, PBF_REG_NUM_STRICT_ARB_SLOTS, 0);
-	/*
-	 * Bitmap of 5bits length. Each bit specifies whether the entry behaves
+	/* Bitmap of 5bits length. Each bit specifies whether the entry behaves
 	 * as strict.  Bits 0,1,2 - debug and management entries, 3 - COS0
 	 * entry, 4 - COS1 entry.
 	 * COS1 | COS0 | DEBUG21 | DEBUG0 | MGMT
@@ -1298,8 +1273,7 @@ int bnx2x_ets_strict(const struct link_params *params, const u8 strict_cos)
 	u32 val	= 0;
 
 	DP(NETIF_MSG_LINK, "ETS enabled strict configuration\n");
-	/*
-	 * Bitmap of 5bits length. Each bit specifies whether the entry behaves
+	/* Bitmap of 5bits length. Each bit specifies whether the entry behaves
 	 * as strict.  Bits 0,1,2 - debug and management entries,
 	 * 3 - COS0 entry, 4 - COS1 entry.
 	 *  COS1 | COS0 | DEBUG21 | DEBUG0 | MGMT
@@ -1307,8 +1281,7 @@ int bnx2x_ets_strict(const struct link_params *params, const u8 strict_cos)
 	 * MCP and debug are strict
 	 */
 	REG_WR(bp, NIG_REG_P0_TX_ARB_CLIENT_IS_STRICT, 0x1F);
-	/*
-	 * For strict priority entries defines the number of consecutive slots
+	/* For strict priority entries defines the number of consecutive slots
 	 * for the highest priority.
 	 */
 	REG_WR(bp, NIG_REG_P0_TX_ARB_NUM_STRICT_ARB_SLOTS, 0x100);
@@ -1320,8 +1293,7 @@ int bnx2x_ets_strict(const struct link_params *params, const u8 strict_cos)
 	/* Defines the number of consecutive slots for the strict priority */
 	REG_WR(bp, PBF_REG_HIGH_PRIORITY_COS_NUM, strict_cos);
 
-	/*
-	 * mapping between entry  priority to client number (0,1,2 -debug and
+	/* Mapping between entry  priority to client number (0,1,2 -debug and
 	 * management clients, 3 - COS0 client, 4 - COS client)(HIGHEST)
 	 * 3bits client num.
 	 *   PRI4    |    PRI3    |    PRI2    |    PRI1    |    PRI0
@@ -1356,15 +1328,12 @@ static void bnx2x_update_pfc_xmac(struct link_params *params,
 	if (!(params->feature_config_flags &
 	      FEATURE_CONFIG_PFC_ENABLED)) {
 
-		/*
-		 * RX flow control - Process pause frame in receive direction
+		/* RX flow control - Process pause frame in receive direction
 		 */
 		if (vars->flow_ctrl & BNX2X_FLOW_CTRL_RX)
 			pause_val |= XMAC_PAUSE_CTRL_REG_RX_PAUSE_EN;
 
-		/*
-		 * TX flow control - Send pause packet when buffer is full
-		 */
+		/* TX flow control - Send pause packet when buffer is full */
 		if (vars->flow_ctrl & BNX2X_FLOW_CTRL_TX)
 			pause_val |= XMAC_PAUSE_CTRL_REG_TX_PAUSE_EN;
 	} else {/* PFC support */
@@ -1457,8 +1426,7 @@ void bnx2x_pfc_statistic(struct link_params *params, struct link_vars *vars,
 static void bnx2x_set_mdio_clk(struct bnx2x *bp, u32 chip_id, u8 port)
 {
 	u32 mode, emac_base;
-	/**
-	 * Set clause 45 mode, slow down the MDIO clock to 2.5MHz
+	/* Set clause 45 mode, slow down the MDIO clock to 2.5MHz
 	 * (a value of 49==0x31) and make sure that the AUTO poll is off
 	 */
 
@@ -1578,15 +1546,6 @@ static void bnx2x_umac_enable(struct link_params *params,
 
 	DP(NETIF_MSG_LINK, "enabling UMAC\n");
 
-	/**
-	 * This register determines on which events the MAC will assert
-	 * error on the i/f to the NIG along w/ EOP.
-	 */
-
-	/**
-	 * BD REG_WR(bp, NIG_REG_P0_MAC_RSV_ERR_MASK +
-	 * params->port*0x14,      0xfffff.
-	 */
 	/* This register opens the gate for the UMAC despite its name */
 	REG_WR(bp, NIG_REG_EGRESS_EMAC0_PORT + params->port*4, 1);
 
@@ -1649,8 +1608,7 @@ static void bnx2x_umac_enable(struct link_params *params,
 		val |= UMAC_COMMAND_CONFIG_REG_LOOP_ENA;
 	REG_WR(bp, umac_base + UMAC_REG_COMMAND_CONFIG, val);
 
-	/*
-	 * Maximum Frame Length (RW). Defines a 14-Bit maximum frame
+	/* Maximum Frame Length (RW). Defines a 14-Bit maximum frame
 	 * length used by the MAC receive logic to check frames.
 	 */
 	REG_WR(bp, umac_base + UMAC_REG_MAXFR, 0x2710);
@@ -1666,8 +1624,7 @@ static void bnx2x_xmac_init(struct link_params *params, u32 max_speed)
 	struct bnx2x *bp = params->bp;
 	u32 is_port4mode = bnx2x_is_4_port_mode(bp);
 
-	/*
-	 * In 4-port mode, need to set the mode only once, so if XMAC is
+	/* In 4-port mode, need to set the mode only once, so if XMAC is
 	 * already out of reset, it means the mode has already been set,
 	 * and it must not* reset the XMAC again, since it controls both
 	 * ports of the path
@@ -1691,13 +1648,13 @@ static void bnx2x_xmac_init(struct link_params *params, u32 max_speed)
 	if (is_port4mode) {
 		DP(NETIF_MSG_LINK, "Init XMAC to 2 ports x 10G per path\n");
 
-		/*  Set the number of ports on the system side to up to 2 */
+		/* Set the number of ports on the system side to up to 2 */
 		REG_WR(bp, MISC_REG_XMAC_CORE_PORT_MODE, 1);
 
 		/* Set the number of ports on the Warp Core to 10G */
 		REG_WR(bp, MISC_REG_XMAC_PHY_PORT_MODE, 3);
 	} else {
-		/*  Set the number of ports on the system side to 1 */
+		/* Set the number of ports on the system side to 1 */
 		REG_WR(bp, MISC_REG_XMAC_CORE_PORT_MODE, 0);
 		if (max_speed == SPEED_10000) {
 			DP(NETIF_MSG_LINK,
@@ -1729,8 +1686,7 @@ static void bnx2x_xmac_disable(struct link_params *params)
 
 	if (REG_RD(bp, MISC_REG_RESET_REG_2) &
 	    MISC_REGISTERS_RESET_REG_2_XMAC) {
-		/*
-		 * Send an indication to change the state in the NIG back to XON
+		/* Send an indication to change the state in the NIG back to XON
 		 * Clearing this bit enables the next set of this bit to get
 		 * rising edge
 		 */
@@ -1755,13 +1711,11 @@ static int bnx2x_xmac_enable(struct link_params *params,
 
 	bnx2x_xmac_init(params, vars->line_speed);
 
-	/*
-	 * This register determines on which events the MAC will assert
+	/* This register determines on which events the MAC will assert
 	 * error on the i/f to the NIG along w/ EOP.
 	 */
 
-	/*
-	 * This register tells the NIG whether to send traffic to UMAC
+	/* This register tells the NIG whether to send traffic to UMAC
 	 * or XMAC
 	 */
 	REG_WR(bp, NIG_REG_EGRESS_EMAC0_PORT + params->port*4, 0);
@@ -1863,8 +1817,7 @@ static int bnx2x_emac_enable(struct link_params *params,
 	val = REG_RD(bp, emac_base + EMAC_REG_EMAC_RX_MODE);
 	val |= EMAC_RX_MODE_KEEP_VLAN_TAG | EMAC_RX_MODE_PROMISCUOUS;
 
-	/*
-	 * Setting this bit causes MAC control frames (except for pause
+	/* Setting this bit causes MAC control frames (except for pause
 	 * frames) to be passed on for processing. This setting has no
 	 * affect on the operation of the pause frames. This bit effects
 	 * all packets regardless of RX Parser packet sorting logic.
@@ -1963,8 +1916,7 @@ static void bnx2x_update_pfc_bmac2(struct link_params *params,
 				   struct link_vars *vars,
 				   u8 is_lb)
 {
-	/*
-	 * Set rx control: Strip CRC and enable BigMAC to relay
+	/* Set rx control: Strip CRC and enable BigMAC to relay
 	 * control packets to the system as well
 	 */
 	u32 wb_data[2];
@@ -2016,8 +1968,7 @@ static void bnx2x_update_pfc_bmac2(struct link_params *params,
 
 	REG_WR_DMAE(bp, bmac_addr + BIGMAC2_REGISTER_PFC_CONTROL, wb_data, 2);
 
-	/*
-	 * Set Time (based unit is 512 bit time) between automatic
+	/* Set Time (based unit is 512 bit time) between automatic
 	 * re-sending of PP packets amd enable automatic re-send of
 	 * Per-Priroity Packet as long as pp_gen is asserted and
 	 * pp_disable is low.
@@ -2086,7 +2037,7 @@ static int bnx2x_pfc_brb_get_config_params(
 	config_val->default_class1.full_xon = 0;
 
 	if (CHIP_IS_E2(bp)) {
-		/*  class0 defaults */
+		/* Class0 defaults */
 		config_val->default_class0.pause_xoff =
 			DEFAULT0_E2_BRB_MAC_PAUSE_XOFF_THR;
 		config_val->default_class0.pause_xon =
@@ -2095,7 +2046,7 @@ static int bnx2x_pfc_brb_get_config_params(
 			DEFAULT0_E2_BRB_MAC_FULL_XOFF_THR;
 		config_val->default_class0.full_xon =
 			DEFAULT0_E2_BRB_MAC_FULL_XON_THR;
-		/*  pause able*/
+		/* Pause able*/
 		config_val->pauseable_th.pause_xoff =
 			PFC_E2_BRB_MAC_PAUSE_XOFF_THR_PAUSE;
 		config_val->pauseable_th.pause_xon =
@@ -2114,7 +2065,7 @@ static int bnx2x_pfc_brb_get_config_params(
 		config_val->non_pauseable_th.full_xon =
 			PFC_E2_BRB_MAC_FULL_XON_THR_NON_PAUSE;
 	} else if (CHIP_IS_E3A0(bp)) {
-		/*  class0 defaults */
+		/* Class0 defaults */
 		config_val->default_class0.pause_xoff =
 			DEFAULT0_E3A0_BRB_MAC_PAUSE_XOFF_THR;
 		config_val->default_class0.pause_xon =
@@ -2123,7 +2074,7 @@ static int bnx2x_pfc_brb_get_config_params(
 			DEFAULT0_E3A0_BRB_MAC_FULL_XOFF_THR;
 		config_val->default_class0.full_xon =
 			DEFAULT0_E3A0_BRB_MAC_FULL_XON_THR;
-		/*  pause able */
+		/* Pause able */
 		config_val->pauseable_th.pause_xoff =
 			PFC_E3A0_BRB_MAC_PAUSE_XOFF_THR_PAUSE;
 		config_val->pauseable_th.pause_xon =
@@ -2142,7 +2093,7 @@ static int bnx2x_pfc_brb_get_config_params(
 		config_val->non_pauseable_th.full_xon =
 			PFC_E3A0_BRB_MAC_FULL_XON_THR_NON_PAUSE;
 	} else if (CHIP_IS_E3B0(bp)) {
-		/*  class0 defaults */
+		/* Class0 defaults */
 		config_val->default_class0.pause_xoff =
 			DEFAULT0_E3B0_BRB_MAC_PAUSE_XOFF_THR;
 		config_val->default_class0.pause_xon =
@@ -2305,27 +2256,23 @@ static int bnx2x_update_pfc_brb(struct link_params *params,
 			reg_th_config = &config_val.non_pauseable_th;
 	} else
 		reg_th_config = &config_val.default_class0;
-	/*
-	 * The number of free blocks below which the pause signal to class 0
+	/* The number of free blocks below which the pause signal to class 0
 	 * of MAC #n is asserted. n=0,1
 	 */
 	REG_WR(bp, (port) ? BRB1_REG_PAUSE_0_XOFF_THRESHOLD_1 :
 	       BRB1_REG_PAUSE_0_XOFF_THRESHOLD_0 ,
 	       reg_th_config->pause_xoff);
-	/*
-	 * The number of free blocks above which the pause signal to class 0
+	/* The number of free blocks above which the pause signal to class 0
 	 * of MAC #n is de-asserted. n=0,1
 	 */
 	REG_WR(bp, (port) ? BRB1_REG_PAUSE_0_XON_THRESHOLD_1 :
 	       BRB1_REG_PAUSE_0_XON_THRESHOLD_0 , reg_th_config->pause_xon);
-	/*
-	 * The number of free blocks below which the full signal to class 0
+	/* The number of free blocks below which the full signal to class 0
 	 * of MAC #n is asserted. n=0,1
 	 */
 	REG_WR(bp, (port) ? BRB1_REG_FULL_0_XOFF_THRESHOLD_1 :
 	       BRB1_REG_FULL_0_XOFF_THRESHOLD_0 , reg_th_config->full_xoff);
-	/*
-	 * The number of free blocks above which the full signal to class 0
+	/* The number of free blocks above which the full signal to class 0
 	 * of MAC #n is de-asserted. n=0,1
 	 */
 	REG_WR(bp, (port) ? BRB1_REG_FULL_0_XON_THRESHOLD_1 :
@@ -2339,30 +2286,26 @@ static int bnx2x_update_pfc_brb(struct link_params *params,
 			reg_th_config = &config_val.non_pauseable_th;
 	} else
 		reg_th_config = &config_val.default_class1;
-	/*
-	 * The number of free blocks below which the pause signal to
+	/* The number of free blocks below which the pause signal to
 	 * class 1 of MAC #n is asserted. n=0,1
 	 */
 	REG_WR(bp, (port) ? BRB1_REG_PAUSE_1_XOFF_THRESHOLD_1 :
 	       BRB1_REG_PAUSE_1_XOFF_THRESHOLD_0,
 	       reg_th_config->pause_xoff);
 
-	/*
-	 * The number of free blocks above which the pause signal to
+	/* The number of free blocks above which the pause signal to
 	 * class 1 of MAC #n is de-asserted. n=0,1
 	 */
 	REG_WR(bp, (port) ? BRB1_REG_PAUSE_1_XON_THRESHOLD_1 :
 	       BRB1_REG_PAUSE_1_XON_THRESHOLD_0,
 	       reg_th_config->pause_xon);
-	/*
-	 * The number of free blocks below which the full signal to
+	/* The number of free blocks below which the full signal to
 	 * class 1 of MAC #n is asserted. n=0,1
 	 */
 	REG_WR(bp, (port) ? BRB1_REG_FULL_1_XOFF_THRESHOLD_1 :
 	       BRB1_REG_FULL_1_XOFF_THRESHOLD_0,
 	       reg_th_config->full_xoff);
-	/*
-	 * The number of free blocks above which the full signal to
+	/* The number of free blocks above which the full signal to
 	 * class 1 of MAC #n is de-asserted. n=0,1
 	 */
 	REG_WR(bp, (port) ? BRB1_REG_FULL_1_XON_THRESHOLD_1 :
@@ -2379,49 +2322,41 @@ static int bnx2x_update_pfc_brb(struct link_params *params,
 		REG_WR(bp, BRB1_REG_PER_CLASS_GUARANTY_MODE,
 			   e3b0_val.per_class_guaranty_mode);
 
-		/*
-		 * The hysteresis on the guarantied buffer space for the Lb
+		/* The hysteresis on the guarantied buffer space for the Lb
 		 * port before signaling XON.
 		 */
 		REG_WR(bp, BRB1_REG_LB_GUARANTIED_HYST,
 			   e3b0_val.lb_guarantied_hyst);
 
-		/*
-		 * The number of free blocks below which the full signal to the
+		/* The number of free blocks below which the full signal to the
 		 * LB port is asserted.
 		 */
 		REG_WR(bp, BRB1_REG_FULL_LB_XOFF_THRESHOLD,
 		       e3b0_val.full_lb_xoff_th);
-		/*
-		 * The number of free blocks above which the full signal to the
+		/* The number of free blocks above which the full signal to the
 		 * LB port is de-asserted.
 		 */
 		REG_WR(bp, BRB1_REG_FULL_LB_XON_THRESHOLD,
 		       e3b0_val.full_lb_xon_threshold);
-		/*
-		 * The number of blocks guarantied for the MAC #n port. n=0,1
+		/* The number of blocks guarantied for the MAC #n port. n=0,1
 		 */
 
-		/* The number of blocks guarantied for the LB port.*/
+		/* The number of blocks guarantied for the LB port. */
 		REG_WR(bp, BRB1_REG_LB_GUARANTIED,
 		       e3b0_val.lb_guarantied);
 
-		/*
-		 * The number of blocks guarantied for the MAC #n port.
-		 */
+		/* The number of blocks guarantied for the MAC #n port. */
 		REG_WR(bp, BRB1_REG_MAC_GUARANTIED_0,
 		       2 * e3b0_val.mac_0_class_t_guarantied);
 		REG_WR(bp, BRB1_REG_MAC_GUARANTIED_1,
 		       2 * e3b0_val.mac_1_class_t_guarantied);
-		/*
-		 * The number of blocks guarantied for class #t in MAC0. t=0,1
+		/* The number of blocks guarantied for class #t in MAC0. t=0,1
 		 */
 		REG_WR(bp, BRB1_REG_MAC_0_CLASS_0_GUARANTIED,
 		       e3b0_val.mac_0_class_t_guarantied);
 		REG_WR(bp, BRB1_REG_MAC_0_CLASS_1_GUARANTIED,
 		       e3b0_val.mac_0_class_t_guarantied);
-		/*
-		 * The hysteresis on the guarantied buffer space for class in
+		/* The hysteresis on the guarantied buffer space for class in
 		 * MAC0.  t=0,1
 		 */
 		REG_WR(bp, BRB1_REG_MAC_0_CLASS_0_GUARANTIED_HYST,
@@ -2429,15 +2364,13 @@ static int bnx2x_update_pfc_brb(struct link_params *params,
 		REG_WR(bp, BRB1_REG_MAC_0_CLASS_1_GUARANTIED_HYST,
 		       e3b0_val.mac_0_class_t_guarantied_hyst);
 
-		/*
-		 * The number of blocks guarantied for class #t in MAC1.t=0,1
+		/* The number of blocks guarantied for class #t in MAC1.t=0,1
 		 */
 		REG_WR(bp, BRB1_REG_MAC_1_CLASS_0_GUARANTIED,
 		       e3b0_val.mac_1_class_t_guarantied);
 		REG_WR(bp, BRB1_REG_MAC_1_CLASS_1_GUARANTIED,
 		       e3b0_val.mac_1_class_t_guarantied);
-		/*
-		 * The hysteresis on the guarantied buffer space for class #t
+		/* The hysteresis on the guarantied buffer space for class #t
 		 * in MAC1.  t=0,1
 		 */
 		REG_WR(bp, BRB1_REG_MAC_1_CLASS_0_GUARANTIED_HYST,
@@ -2520,15 +2453,13 @@ static void bnx2x_update_pfc_nig(struct link_params *params,
 		FEATURE_CONFIG_PFC_ENABLED;
 	DP(NETIF_MSG_LINK, "updating pfc nig parameters\n");
 
-	/*
-	 * When NIG_LLH0_XCM_MASK_REG_LLHX_XCM_MASK_BCN bit is set
+	/* When NIG_LLH0_XCM_MASK_REG_LLHX_XCM_MASK_BCN bit is set
 	 * MAC control frames (that are not pause packets)
 	 * will be forwarded to the XCM.
 	 */
 	xcm_mask = REG_RD(bp, port ? NIG_REG_LLH1_XCM_MASK :
 			  NIG_REG_LLH0_XCM_MASK);
-	/*
-	 * nig params will override non PFC params, since it's possible to
+	/* NIG params will override non PFC params, since it's possible to
 	 * do transition from PFC to SAFC
 	 */
 	if (set_pfc) {
@@ -2548,7 +2479,7 @@ static void bnx2x_update_pfc_nig(struct link_params *params,
 			llfc_out_en = nig_params->llfc_out_en;
 			llfc_enable = nig_params->llfc_enable;
 			pause_enable = nig_params->pause_enable;
-		} else  /*defaul non PFC mode - PAUSE */
+		} else  /* Default non PFC mode - PAUSE */
 			pause_enable = 1;
 
 		xcm_mask |= (port ? NIG_LLH1_XCM_MASK_REG_LLH1_XCM_MASK_BCN :
@@ -2608,8 +2539,7 @@ int bnx2x_update_pfc(struct link_params *params,
 		      struct link_vars *vars,
 		      struct bnx2x_nig_brb_pfc_port_params *pfc_params)
 {
-	/*
-	 * The PFC and pause are orthogonal to one another, meaning when
+	/* The PFC and pause are orthogonal to one another, meaning when
 	 * PFC is enabled, the pause are disabled, and when PFC is
 	 * disabled, pause are set according to the pause result.
 	 */
@@ -3148,7 +3078,6 @@ static int bnx2x_cl45_write(struct bnx2x *bp, struct bnx2x_phy *phy,
 			      EMAC_MDIO_STATUS_10MB);
 
 	/* address */
-
 	tmp = ((phy->addr << 21) | (devad << 16) | reg |
 	       EMAC_MDIO_COMM_COMMAND_ADDRESS |
 	       EMAC_MDIO_COMM_START_BUSY);
@@ -3337,8 +3266,7 @@ int bnx2x_phy_read(struct link_params *params, u8 phy_addr,
 		   u8 devad, u16 reg, u16 *ret_val)
 {
 	u8 phy_index;
-	/*
-	 * Probe for the phy according to the given phy_addr, and execute
+	/* Probe for the phy according to the given phy_addr, and execute
 	 * the read request on it
 	 */
 	for (phy_index = 0; phy_index < params->num_phys; phy_index++) {
@@ -3355,8 +3283,7 @@ int bnx2x_phy_write(struct link_params *params, u8 phy_addr,
 		    u8 devad, u16 reg, u16 val)
 {
 	u8 phy_index;
-	/*
-	 * Probe for the phy according to the given phy_addr, and execute
+	/* Probe for the phy according to the given phy_addr, and execute
 	 * the write request on it
 	 */
 	for (phy_index = 0; phy_index < params->num_phys; phy_index++) {
@@ -3382,7 +3309,7 @@ static u8 bnx2x_get_warpcore_lane(struct bnx2x_phy *phy,
 	if (bnx2x_is_4_port_mode(bp)) {
 		u32 port_swap, port_swap_ovr;
 
-		/*figure out path swap value */
+		/* Figure out path swap value */
 		path_swap_ovr = REG_RD(bp, MISC_REG_FOUR_PORT_PATH_SWAP_OVWR);
 		if (path_swap_ovr & 0x1)
 			path_swap = (path_swap_ovr & 0x2);
@@ -3392,7 +3319,7 @@ static u8 bnx2x_get_warpcore_lane(struct bnx2x_phy *phy,
 		if (path_swap)
 			path = path ^ 1;
 
-		/*figure out port swap value */
+		/* Figure out port swap value */
 		port_swap_ovr = REG_RD(bp, MISC_REG_FOUR_PORT_PORT_SWAP_OVWR);
 		if (port_swap_ovr & 0x1)
 			port_swap = (port_swap_ovr & 0x2);
@@ -3405,7 +3332,7 @@ static u8 bnx2x_get_warpcore_lane(struct bnx2x_phy *phy,
 		lane = (port<<1) + path;
 	} else { /* two port mode - no port swap */
 
-		/*figure out path swap value */
+		/* Figure out path swap value */
 		path_swap_ovr =
 			REG_RD(bp, MISC_REG_TWO_PORT_PATH_SWAP_OVWR);
 		if (path_swap_ovr & 0x1) {
@@ -3437,8 +3364,7 @@ static void bnx2x_set_aer_mmd(struct link_params *params,
 
 	if (USES_WARPCORE(bp)) {
 		aer_val = bnx2x_get_warpcore_lane(phy, params);
-		/*
-		 * In Dual-lane mode, two lanes are joined together,
+		/* In Dual-lane mode, two lanes are joined together,
 		 * so in order to configure them, the AER broadcast method is
 		 * used here.
 		 * 0x200 is the broadcast address for lanes 0,1
@@ -3518,8 +3444,7 @@ static void bnx2x_calc_ieee_aneg_adv(struct bnx2x_phy *phy,
 {
 	struct bnx2x *bp = params->bp;
 	*ieee_fc = MDIO_COMBO_IEEE0_AUTO_NEG_ADV_FULL_DUPLEX;
-	/**
-	 * resolve pause mode and advertisement Please refer to Table
+	/* Resolve pause mode and advertisement Please refer to Table
 	 * 28B-3 of the 802.3ab-1999 spec
 	 */
 
@@ -3642,6 +3567,7 @@ static void bnx2x_pause_resolve(struct link_vars *vars, u32 pause_result)
 		vars->link_status |= LINK_STATUS_LINK_PARTNER_SYMMETRIC_PAUSE;
 	if (pause_result & (1<<1))
 		vars->link_status |= LINK_STATUS_LINK_PARTNER_ASYMMETRIC_PAUSE;
+
 }
 
 static void bnx2x_ext_phy_update_adv_fc(struct bnx2x_phy *phy,
@@ -3698,6 +3624,7 @@ static void bnx2x_ext_phy_update_adv_fc(struct bnx2x_phy *phy,
 	bnx2x_pause_resolve(vars, pause_result);
 
 }
+
 static u8 bnx2x_ext_phy_resolve_fc(struct bnx2x_phy *phy,
 				   struct link_params *params,
 				   struct link_vars *vars)
@@ -3819,9 +3746,7 @@ static void bnx2x_warpcore_enable_AN_KR(struct bnx2x_phy *phy,
 
 	/* Advertise pause */
 	bnx2x_ext_phy_set_pause(params, phy, vars);
-
-	/*
-	 * Set KR Autoneg Work-Around flag for Warpcore version older than D108
+	/* Set KR Autoneg Work-Around flag for Warpcore version older than D108
 	 */
 	bnx2x_cl45_read(bp, phy, MDIO_WC_DEVAD,
 			MDIO_WC_REG_UC_INFO_B1_VERSION, &val16);
@@ -3829,7 +3754,6 @@ static void bnx2x_warpcore_enable_AN_KR(struct bnx2x_phy *phy,
 		DP(NETIF_MSG_LINK, "Enable AN KR work-around\n");
 		vars->rx_tx_asic_rst = MAX_KR_LINK_RETRY;
 	}
-
 	bnx2x_cl45_read(bp, phy, MDIO_WC_DEVAD,
 			MDIO_WC_REG_DIGITAL5_MISC7, &val16);
 
@@ -3903,7 +3827,7 @@ static void bnx2x_warpcore_set_10G_KR(struct bnx2x_phy *phy,
 	bnx2x_cl45_write(bp, phy, MDIO_PMA_DEVAD,
 			 MDIO_WC_REG_IEEE0BLK_AUTONEGNP, 0xB);
 
-	/*Enable encoded forced speed */
+	/* Enable encoded forced speed */
 	bnx2x_cl45_write(bp, phy, MDIO_WC_DEVAD,
 			 MDIO_WC_REG_SERDESDIGITAL_MISC2, 0x30);
 
@@ -4265,8 +4189,7 @@ static int bnx2x_get_mod_abs_int_cfg(struct bnx2x *bp,
 				PORT_HW_CFG_E3_MOD_ABS_MASK) >>
 				PORT_HW_CFG_E3_MOD_ABS_SHIFT;
 
-		/*
-		 * Should not happen. This function called upon interrupt
+		/* Should not happen. This function called upon interrupt
 		 * triggered by GPIO ( since EPIO can only generate interrupts
 		 * to MCP).
 		 * So if this function was called and none of the GPIOs was set,
@@ -4366,7 +4289,7 @@ static void bnx2x_warpcore_config_runtime(struct bnx2x_phy *phy,
 					"link up, rx_tx_asic_rst 0x%x\n",
 					vars->rx_tx_asic_rst);
 			} else {
-				/*reset the lane to see if link comes up.*/
+				/* Reset the lane to see if link comes up.*/
 				bnx2x_warpcore_reset_lane(bp, phy, 1);
 				bnx2x_warpcore_reset_lane(bp, phy, 0);
 
@@ -4387,7 +4310,6 @@ static void bnx2x_warpcore_config_runtime(struct bnx2x_phy *phy,
 	} /*params->rx_tx_asic_rst*/
 
 }
-
 static void bnx2x_warpcore_config_init(struct bnx2x_phy *phy,
 				       struct link_params *params,
 				       struct link_vars *vars)
@@ -4545,7 +4467,7 @@ static void bnx2x_warpcore_link_reset(struct bnx2x_phy *phy,
 	/* Update those 1-copy registers */
 	CL22_WR_OVER_CL45(bp, phy, MDIO_REG_BANK_AER_BLOCK,
 			  MDIO_AER_BLOCK_AER_REG, 0);
-		/* Enable 1G MDIO (1-copy) */
+	/* Enable 1G MDIO (1-copy) */
 	bnx2x_cl45_read(bp, phy, MDIO_WC_DEVAD,
 			MDIO_WC_REG_XGXSBLK0_XGXSCONTROL,
 			&val16);
@@ -4624,43 +4546,43 @@ void bnx2x_sync_link(struct link_params *params,
 		vars->duplex = DUPLEX_FULL;
 		switch (vars->link_status &
 			LINK_STATUS_SPEED_AND_DUPLEX_MASK) {
-			case LINK_10THD:
-				vars->duplex = DUPLEX_HALF;
-				/* fall thru */
-			case LINK_10TFD:
-				vars->line_speed = SPEED_10;
-				break;
+		case LINK_10THD:
+			vars->duplex = DUPLEX_HALF;
+			/* Fall thru */
+		case LINK_10TFD:
+			vars->line_speed = SPEED_10;
+			break;
 
-			case LINK_100TXHD:
-				vars->duplex = DUPLEX_HALF;
-				/* fall thru */
-			case LINK_100T4:
-			case LINK_100TXFD:
-				vars->line_speed = SPEED_100;
-				break;
+		case LINK_100TXHD:
+			vars->duplex = DUPLEX_HALF;
+			/* Fall thru */
+		case LINK_100T4:
+		case LINK_100TXFD:
+			vars->line_speed = SPEED_100;
+			break;
 
-			case LINK_1000THD:
-				vars->duplex = DUPLEX_HALF;
-				/* fall thru */
-			case LINK_1000TFD:
-				vars->line_speed = SPEED_1000;
-				break;
+		case LINK_1000THD:
+			vars->duplex = DUPLEX_HALF;
+			/* Fall thru */
+		case LINK_1000TFD:
+			vars->line_speed = SPEED_1000;
+			break;
 
-			case LINK_2500THD:
-				vars->duplex = DUPLEX_HALF;
-				/* fall thru */
-			case LINK_2500TFD:
-				vars->line_speed = SPEED_2500;
-				break;
+		case LINK_2500THD:
+			vars->duplex = DUPLEX_HALF;
+			/* Fall thru */
+		case LINK_2500TFD:
+			vars->line_speed = SPEED_2500;
+			break;
 
-			case LINK_10GTFD:
-				vars->line_speed = SPEED_10000;
-				break;
-			case LINK_20GTFD:
-				vars->line_speed = SPEED_20000;
-				break;
-			default:
-				break;
+		case LINK_10GTFD:
+			vars->line_speed = SPEED_10000;
+			break;
+		case LINK_20GTFD:
+			vars->line_speed = SPEED_20000;
+			break;
+		default:
+			break;
 		}
 		vars->flow_ctrl = 0;
 		if (vars->link_status & LINK_STATUS_TX_FLOW_CONTROL_ENABLED)
@@ -4835,9 +4757,8 @@ static void bnx2x_set_swap_lanes(struct link_params *params,
 				 struct bnx2x_phy *phy)
 {
 	struct bnx2x *bp = params->bp;
-	/*
-	 *  Each two bits represents a lane number:
-	 *  No swap is 0123 => 0x1b no need to enable the swap
+	/* Each two bits represents a lane number:
+	 * No swap is 0123 => 0x1b no need to enable the swap
 	 */
 	u16 rx_lane_swap, tx_lane_swap;
 
@@ -5051,8 +4972,7 @@ static void bnx2x_program_serdes(struct bnx2x_phy *phy,
 			  MDIO_REG_BANK_COMBO_IEEE0,
 			  MDIO_COMBO_IEEE0_MII_CONTROL, reg_val);
 
-	/*
-	 * program speed
+	/* Program speed
 	 *  - needed only if the speed is greater than 1G (2.5G or 10G)
 	 */
 	CL22_RD_OVER_CL45(bp, phy,
@@ -5087,8 +5007,6 @@ static void bnx2x_set_brcm_cl37_advertisement(struct bnx2x_phy *phy,
 	struct bnx2x *bp = params->bp;
 	u16 val = 0;
 
-	/* configure the 48 bits for BAM AN */
-
 	/* set extended capabilities */
 	if (phy->speed_cap_mask & PORT_HW_CFG_SPEED_CAPABILITY_D0_2_5G)
 		val |= MDIO_OVER_1G_UP1_2_5G;
@@ -5234,11 +5152,8 @@ static void bnx2x_initialize_sgmii_process(struct bnx2x_phy *phy,
 	}
 }
 
-
-/*
- * link management
+/* Link management
  */
-
 static int bnx2x_direct_parallel_detect_used(struct bnx2x_phy *phy,
 					     struct link_params *params)
 {
@@ -5383,8 +5298,7 @@ static void bnx2x_check_fallback_to_cl37(struct bnx2x_phy *phy,
 			     "ustat_val(0x8371) = 0x%x\n", ustat_val);
 		return;
 	}
-	/*
-	 * Step 3: Check CL37 Message Pages received to indicate LP
+	/* Step 3: Check CL37 Message Pages received to indicate LP
 	 * supports only CL37
 	 */
 	CL22_RD_OVER_CL45(bp, phy,
@@ -5401,8 +5315,7 @@ static void bnx2x_check_fallback_to_cl37(struct bnx2x_phy *phy,
 			 cl37_fsm_received);
 		return;
 	}
-	/*
-	 * The combined cl37/cl73 fsm state information indicating that
+	/* The combined cl37/cl73 fsm state information indicating that
 	 * we are connected to a device which does not support cl73, but
 	 * does support cl37 BAM. In this case we disable cl73 and
 	 * restart cl37 auto-neg
@@ -5973,8 +5886,7 @@ static void bnx2x_rearm_latch_signal(struct bnx2x *bp, u8 port,
 {
 	u32 latch_status = 0;
 
-	/*
-	 * Disable the MI INT ( external phy int ) by writing 1 to the
+	/* Disable the MI INT ( external phy int ) by writing 1 to the
 	 * status register. Link down indication is high-active-signal,
 	 * so in this case we need to write the status to clear the XOR
 	 */
@@ -6009,8 +5921,7 @@ static void bnx2x_link_int_ack(struct link_params *params,
 	struct bnx2x *bp = params->bp;
 	u8 port = params->port;
 	u32 mask;
-	/*
-	 * First reset all status we assume only one line will be
+	/* First reset all status we assume only one line will be
 	 * change at a time
 	 */
 	bnx2x_bits_dis(bp, NIG_REG_STATUS_INTERRUPT_PORT0 + port*4,
@@ -6024,8 +5935,7 @@ static void bnx2x_link_int_ack(struct link_params *params,
 			if (is_10g_plus)
 				mask = NIG_STATUS_XGXS0_LINK10G;
 			else if (params->switch_cfg == SWITCH_CFG_10G) {
-				/*
-				 * Disable the link interrupt by writing 1 to
+				/* Disable the link interrupt by writing 1 to
 				 * the relevant lane in the status register
 				 */
 				u32 ser_lane =
@@ -6227,8 +6137,7 @@ int bnx2x_set_led(struct link_params *params,
 		break;
 
 	case LED_MODE_OPER:
-		/*
-		 * For all other phys, OPER mode is same as ON, so in case
+		/* For all other phys, OPER mode is same as ON, so in case
 		 * link is down, do nothing
 		 */
 		if (!vars->link_up)
@@ -6239,9 +6148,7 @@ int bnx2x_set_led(struct link_params *params,
 			 (params->phy[EXT_PHY1].type ==
 			  PORT_HW_CFG_XGXS_EXT_PHY_TYPE_BCM8722)) &&
 		    CHIP_IS_E2(bp) && params->num_phys == 2) {
-			/*
-			 * This is a work-around for E2+8727 Configurations
-			 */
+			/* This is a work-around for E2+8727 Configurations */
 			if (mode == LED_MODE_ON ||
 				speed == SPEED_10000){
 				REG_WR(bp, NIG_REG_LED_MODE_P0 + port*4, 0);
@@ -6250,8 +6157,7 @@ int bnx2x_set_led(struct link_params *params,
 				tmp = EMAC_RD(bp, EMAC_REG_EMAC_LED);
 				EMAC_WR(bp, EMAC_REG_EMAC_LED,
 					(tmp | EMAC_LED_OVERRIDE));
-				/*
-				 * return here without enabling traffic
+				/* Return here without enabling traffic
 				 * LED blink and setting rate in ON mode.
 				 * In oper mode, enabling LED blink
 				 * and setting rate is needed.
@@ -6260,8 +6166,7 @@ int bnx2x_set_led(struct link_params *params,
 					return rc;
 			}
 		} else if (SINGLE_MEDIA_DIRECT(params)) {
-			/*
-			 * This is a work-around for HW issue found when link
+			/* This is a work-around for HW issue found when link
 			 * is up in CL73
 			 */
 			if ((!CHIP_IS_E3(bp)) ||
@@ -6310,10 +6215,7 @@ int bnx2x_set_led(struct link_params *params,
 		     (speed == SPEED_1000) ||
 		     (speed == SPEED_100) ||
 		     (speed == SPEED_10))) {
-			/*
-			 * On Everest 1 Ax chip versions for speeds less than
-			 * 10G LED scheme is different
-			 */
+			/* For speeds less than 10G LED scheme is different */
 			REG_WR(bp, NIG_REG_LED_CONTROL_OVERRIDE_TRAFFIC_P0
 			       + port*4, 1);
 			REG_WR(bp, NIG_REG_LED_CONTROL_TRAFFIC_P0 +
@@ -6333,8 +6235,7 @@ int bnx2x_set_led(struct link_params *params,
 
 }
 
-/*
- * This function comes to reflect the actual link state read DIRECTLY from the
+/* This function comes to reflect the actual link state read DIRECTLY from the
  * HW
  */
 int bnx2x_test_link(struct link_params *params, struct link_vars *vars,
@@ -6422,16 +6323,14 @@ static int bnx2x_link_initialize(struct link_params *params,
 	int rc = 0;
 	u8 phy_index, non_ext_phy;
 	struct bnx2x *bp = params->bp;
-	/*
-	 * In case of external phy existence, the line speed would be the
+	/* In case of external phy existence, the line speed would be the
 	 * line speed linked up by the external phy. In case it is direct
 	 * only, then the line_speed during initialization will be
 	 * equal to the req_line_speed
 	 */
 	vars->line_speed = params->phy[INT_PHY].req_line_speed;
 
-	/*
-	 * Initialize the internal phy in case this is a direct board
+	/* Initialize the internal phy in case this is a direct board
 	 * (no external phys), or this board has external phy which requires
 	 * to first.
 	 */
@@ -6463,8 +6362,7 @@ static int bnx2x_link_initialize(struct link_params *params,
 	} else {
 		for (phy_index = EXT_PHY1; phy_index < params->num_phys;
 		      phy_index++) {
-			/*
-			 * No need to initialize second phy in case of first
+			/* No need to initialize second phy in case of first
 			 * phy only selection. In case of second phy, we do
 			 * need to initialize the first phy, since they are
 			 * connected.
@@ -6492,7 +6390,6 @@ static int bnx2x_link_initialize(struct link_params *params,
 			NIG_STATUS_XGXS0_LINK_STATUS |
 			NIG_STATUS_SERDES0_LINK_STATUS |
 			NIG_MASK_MI_INT));
-	bnx2x_update_mng(params, vars->link_status);
 	return rc;
 }
 
@@ -6577,7 +6474,7 @@ static int bnx2x_update_link_up(struct link_params *params,
 				u8 link_10g)
 {
 	struct bnx2x *bp = params->bp;
-	u8 port = params->port;
+	u8 phy_idx, port = params->port;
 	int rc = 0;
 
 	vars->link_status |= (LINK_STATUS_LINK_UP |
@@ -6641,11 +6538,18 @@ static int bnx2x_update_link_up(struct link_params *params,
 
 	/* update shared memory */
 	bnx2x_update_mng(params, vars->link_status);
+
+	/* Check remote fault */
+	for (phy_idx = INT_PHY; phy_idx < MAX_PHYS; phy_idx++) {
+		if (params->phy[phy_idx].flags & FLAGS_TX_ERROR_CHECK) {
+			bnx2x_check_half_open_conn(params, vars, 0);
+			break;
+		}
+	}
 	msleep(20);
 	return rc;
 }
-/*
- * The bnx2x_link_update function should be called upon link
+/* The bnx2x_link_update function should be called upon link
  * interrupt.
  * Link is considered up as follows:
  * - DIRECT_SINGLE_MEDIA - Only XGXS link (internal link) needs
@@ -6702,8 +6606,7 @@ int bnx2x_link_update(struct link_params *params, struct link_vars *vars)
 	if (!CHIP_IS_E3(bp))
 		REG_WR(bp, NIG_REG_NIG_EMAC0_EN + port*4, 0);
 
-	/*
-	 * Step 1:
+	/* Step 1:
 	 * Check external link change only for external phys, and apply
 	 * priority selection between them in case the link on both phys
 	 * is up. Note that instead of the common vars, a temporary
@@ -6734,23 +6637,20 @@ int bnx2x_link_update(struct link_params *params, struct link_vars *vars)
 			switch (bnx2x_phy_selection(params)) {
 			case PORT_HW_CFG_PHY_SELECTION_HARDWARE_DEFAULT:
 			case PORT_HW_CFG_PHY_SELECTION_FIRST_PHY_PRIORITY:
-			/*
-			 * In this option, the first PHY makes sure to pass the
+			/* In this option, the first PHY makes sure to pass the
 			 * traffic through itself only.
 			 * Its not clear how to reset the link on the second phy
 			 */
 				active_external_phy = EXT_PHY1;
 				break;
 			case PORT_HW_CFG_PHY_SELECTION_SECOND_PHY_PRIORITY:
-			/*
-			 * In this option, the first PHY makes sure to pass the
+			/* In this option, the first PHY makes sure to pass the
 			 * traffic through the second PHY.
 			 */
 				active_external_phy = EXT_PHY2;
 				break;
 			default:
-			/*
-			 * Link indication on both PHYs with the following cases
+			/* Link indication on both PHYs with the following cases
 			 * is invalid:
 			 * - FIRST_PHY means that second phy wasn't initialized,
 			 * hence its link is expected to be down
@@ -6767,8 +6667,7 @@ int bnx2x_link_update(struct link_params *params, struct link_vars *vars)
 		}
 	}
 	prev_line_speed = vars->line_speed;
-	/*
-	 * Step 2:
+	/* Step 2:
 	 * Read the status of the internal phy. In case of
 	 * DIRECT_SINGLE_MEDIA board, this link is the external link,
 	 * otherwise this is the link between the 577xx and the first
@@ -6778,8 +6677,7 @@ int bnx2x_link_update(struct link_params *params, struct link_vars *vars)
 		params->phy[INT_PHY].read_status(
 			&params->phy[INT_PHY],
 			params, vars);
-	/*
-	 * The INT_PHY flow control reside in the vars. This include the
+	/* The INT_PHY flow control reside in the vars. This include the
 	 * case where the speed or flow control are not set to AUTO.
 	 * Otherwise, the active external phy flow control result is set
 	 * to the vars. The ext_phy_line_speed is needed to check if the
@@ -6788,14 +6686,12 @@ int bnx2x_link_update(struct link_params *params, struct link_vars *vars)
 	 */
 	if (active_external_phy > INT_PHY) {
 		vars->flow_ctrl = phy_vars[active_external_phy].flow_ctrl;
-		/*
-		 * Link speed is taken from the XGXS. AN and FC result from
+		/* Link speed is taken from the XGXS. AN and FC result from
 		 * the external phy.
 		 */
 		vars->link_status |= phy_vars[active_external_phy].link_status;
 
-		/*
-		 * if active_external_phy is first PHY and link is up - disable
+		/* if active_external_phy is first PHY and link is up - disable
 		 * disable TX on second external PHY
 		 */
 		if (active_external_phy == EXT_PHY1) {
@@ -6832,8 +6728,7 @@ int bnx2x_link_update(struct link_params *params, struct link_vars *vars)
 	DP(NETIF_MSG_LINK, "vars->flow_ctrl = 0x%x, vars->link_status = 0x%x,"
 		   " ext_phy_line_speed = %d\n", vars->flow_ctrl,
 		   vars->link_status, ext_phy_line_speed);
-	/*
-	 * Upon link speed change set the NIG into drain mode. Comes to
+	/* Upon link speed change set the NIG into drain mode. Comes to
 	 * deals with possible FIFO glitch due to clk change when speed
 	 * is decreased without link down indicator
 	 */
@@ -6858,8 +6753,7 @@ int bnx2x_link_update(struct link_params *params, struct link_vars *vars)
 
 	bnx2x_link_int_ack(params, vars, link_10g_plus);
 
-	/*
-	 * In case external phy link is up, and internal link is down
+	/* In case external phy link is up, and internal link is down
 	 * (not initialized yet probably after link initialization, it
 	 * needs to be initialized.
 	 * Note that after link down-up as result of cable plug, the xgxs
@@ -6887,8 +6781,7 @@ int bnx2x_link_update(struct link_params *params, struct link_vars *vars)
 						vars);
 		}
 	}
-	/*
-	 * Link is up only if both local phy and external phy (in case of
+	/* Link is up only if both local phy and external phy (in case of
 	 * non-direct board) are up and no fault detected on active PHY.
 	 */
 	vars->link_up = (vars->phy_link_up &&
@@ -6907,6 +6800,10 @@ int bnx2x_link_update(struct link_params *params, struct link_vars *vars)
 	else
 		rc = bnx2x_update_link_down(params, vars);
 
+	/* Update MCP link status was changed */
+	if (params->feature_config_flags & FEATURE_CONFIG_BC_SUPPORTS_AFEX)
+		bnx2x_fw_command(bp, DRV_MSG_CODE_LINK_STATUS_CHANGED, 0);
+
 	return rc;
 }
 
@@ -7120,8 +7017,7 @@ static int bnx2x_8073_xaui_wa(struct bnx2x *bp, struct bnx2x_phy *phy)
 	}
 	/* XAUI workaround in 8073 A0: */
 
-	/*
-	 * After loading the boot ROM and restarting Autoneg, poll
+	/* After loading the boot ROM and restarting Autoneg, poll
 	 * Dev1, Reg $C820:
 	 */
 
@@ -7130,8 +7026,7 @@ static int bnx2x_8073_xaui_wa(struct bnx2x *bp, struct bnx2x_phy *phy)
 				MDIO_PMA_DEVAD,
 				MDIO_PMA_REG_8073_SPEED_LINK_STATUS,
 				&val);
-		  /*
-		   * If bit [14] = 0 or bit [13] = 0, continue on with
+		  /* If bit [14] = 0 or bit [13] = 0, continue on with
 		   * system initialization (XAUI work-around not required, as
 		   * these bits indicate 2.5G or 1G link up).
 		   */
@@ -7140,8 +7035,7 @@ static int bnx2x_8073_xaui_wa(struct bnx2x *bp, struct bnx2x_phy *phy)
 			return 0;
 		} else if (!(val & (1<<15))) {
 			DP(NETIF_MSG_LINK, "bit 15 went off\n");
-			/*
-			 * If bit 15 is 0, then poll Dev1, Reg $C841 until it's
+			/* If bit 15 is 0, then poll Dev1, Reg $C841 until it's
 			 * MSB (bit15) goes to 1 (indicating that the XAUI
 			 * workaround has completed), then continue on with
 			 * system initialization.
@@ -7291,8 +7185,7 @@ static int bnx2x_8073_config_init(struct bnx2x_phy *phy,
 			val = (1<<7);
 		} else if (phy->req_line_speed ==  SPEED_2500) {
 			val = (1<<5);
-			/*
-			 * Note that 2.5G works only when used with 1G
+			/* Note that 2.5G works only when used with 1G
 			 * advertisement
 			 */
 		} else
@@ -7343,8 +7236,7 @@ static int bnx2x_8073_config_init(struct bnx2x_phy *phy,
 	/* Add support for CL37 (passive mode) III */
 	bnx2x_cl45_write(bp, phy, MDIO_AN_DEVAD, MDIO_AN_REG_CL37_AN, 0x1000);
 
-	/*
-	 * The SNR will improve about 2db by changing BW and FEE main
+	/* The SNR will improve about 2db by changing BW and FEE main
 	 * tap. Rest commands are executed after link is up
 	 * Change FFE main cursor to 5 in EDC register
 	 */
@@ -7431,8 +7323,7 @@ static u8 bnx2x_8073_read_status(struct bnx2x_phy *phy,
 
 	link_up = (((val1 & 4) == 4) || (an1000_status & (1<<1)));
 	if (link_up && bnx2x_8073_is_snr_needed(bp, phy)) {
-		/*
-		 * The SNR will improve about 2dbby changing the BW and FEE main
+		/* The SNR will improve about 2dbby changing the BW and FEE main
 		 * tap. The 1st write to change FFE main tap is set before
 		 * restart AN. Change PLL Bandwidth in EDC register
 		 */
@@ -7479,8 +7370,7 @@ static u8 bnx2x_8073_read_status(struct bnx2x_phy *phy,
 			bnx2x_cl45_read(bp, phy,
 					MDIO_XS_DEVAD,
 					MDIO_XS_REG_8073_RX_CTRL_PCIE, &val1);
-			/*
-			 * Set bit 3 to invert Rx in 1G mode and clear this bit
+			/* Set bit 3 to invert Rx in 1G mode and clear this bit
 			 * when it`s in 10G mode.
 			 */
 			if (vars->line_speed == SPEED_1000) {
@@ -7602,8 +7492,7 @@ static void bnx2x_set_disable_pmd_transmit(struct link_params *params,
 					   u8 pmd_dis)
 {
 	struct bnx2x *bp = params->bp;
-	/*
-	 * Disable transmitter only for bootcodes which can enable it afterwards
+	/* Disable transmitter only for bootcodes which can enable it afterwards
 	 * (for D3 link)
 	 */
 	if (pmd_dis) {
@@ -7780,9 +7669,6 @@ static int bnx2x_warpcore_read_sfp_module_eeprom(struct bnx2x_phy *phy,
 	u32 data_array[4];
 	u16 addr32;
 	struct bnx2x *bp = params->bp;
-	/*DP(NETIF_MSG_LINK, "bnx2x_direct_read_sfp_module_eeprom:"
-					" addr %d, cnt %d\n",
-					addr, byte_cnt);*/
 	if (byte_cnt > 16) {
 		DP(NETIF_MSG_LINK,
 		   "Reading from eeprom is limited to 16 bytes\n");
@@ -7847,8 +7733,7 @@ static int bnx2x_8727_read_sfp_module_eeprom(struct bnx2x_phy *phy,
 			 MDIO_PMA_DEVAD,
 			 MDIO_PMA_REG_SFP_TWO_WIRE_CTRL,
 			 0x8002);
-	/*
-	 * Wait appropriate time for two-wire command to finish before
+	/* Wait appropriate time for two-wire command to finish before
 	 * polling the status register
 	 */
 	msleep(1);
@@ -7941,8 +7826,7 @@ static int bnx2x_get_edc_mode(struct bnx2x_phy *phy,
 	{
 		u8 copper_module_type;
 		phy->media_type = ETH_PHY_DA_TWINAX;
-		/*
-		 * Check if its active cable (includes SFP+ module)
+		/* Check if its active cable (includes SFP+ module)
 		 * of passive cable
 		 */
 		if (bnx2x_read_sfp_module_eeprom(phy,
@@ -8019,8 +7903,7 @@ static int bnx2x_get_edc_mode(struct bnx2x_phy *phy,
 	DP(NETIF_MSG_LINK, "EDC mode is set to 0x%x\n", *edc_mode);
 	return 0;
 }
-/*
- * This function read the relevant field from the module (SFP+), and verify it
+/* This function read the relevant field from the module (SFP+), and verify it
  * is compliant with this board
  */
 static int bnx2x_verify_sfp_module(struct bnx2x_phy *phy,
@@ -8102,8 +7985,7 @@ static int bnx2x_wait_for_sfp_module_initialized(struct bnx2x_phy *phy,
 	u8 val;
 	struct bnx2x *bp = params->bp;
 	u16 timeout;
-	/*
-	 * Initialization time after hot-plug may take up to 300ms for
+	/* Initialization time after hot-plug may take up to 300ms for
 	 * some phys type ( e.g. JDSU )
 	 */
 
@@ -8125,8 +8007,7 @@ static void bnx2x_8727_power_module(struct bnx2x *bp,
 				    u8 is_power_up) {
 	/* Make sure GPIOs are not using for LED mode */
 	u16 val;
-	/*
-	 * In the GPIO register, bit 4 is use to determine if the GPIOs are
+	/* In the GPIO register, bit 4 is use to determine if the GPIOs are
 	 * operating as INPUT or as OUTPUT. Bit 1 is for input, and 0 for
 	 * output
 	 * Bits 0-1 determine the GPIOs value for OUTPUT in case bit 4 val is 0
@@ -8142,8 +8023,7 @@ static void bnx2x_8727_power_module(struct bnx2x *bp,
 	if (is_power_up)
 		val = (1<<4);
 	else
-		/*
-		 * Set GPIO control to OUTPUT, and set the power bit
+		/* Set GPIO control to OUTPUT, and set the power bit
 		 * to according to the is_power_up
 		 */
 		val = (1<<1);
@@ -8177,8 +8057,7 @@ static int bnx2x_8726_set_limiting_mode(struct bnx2x *bp,
 
 		DP(NETIF_MSG_LINK, "Setting LRM MODE\n");
 
-		/*
-		 * Changing to LRM mode takes quite few seconds. So do it only
+		/* Changing to LRM mode takes quite few seconds. So do it only
 		 * if current mode is limiting (default is LRM)
 		 */
 		if (cur_limiting_mode != EDC_MODE_LIMITING)
@@ -8313,8 +8192,7 @@ static void bnx2x_set_sfp_module_fault_led(struct link_params *params,
 	struct bnx2x *bp = params->bp;
 	DP(NETIF_MSG_LINK, "Setting SFP+ module fault LED to %d\n", gpio_mode);
 	if (CHIP_IS_E3(bp)) {
-		/*
-		 * Low ==> if SFP+ module is supported otherwise
+		/* Low ==> if SFP+ module is supported otherwise
 		 * High ==> if SFP+ module is not on the approved vendor list
 		 */
 		bnx2x_set_e3_module_fault_led(params, gpio_mode);
@@ -8339,8 +8217,7 @@ static void bnx2x_warpcore_power_module(struct link_params *params,
 		return;
 	DP(NETIF_MSG_LINK, "Setting SFP+ module power to %d using pin cfg %d\n",
 		       power, pin_cfg);
-	/*
-	 * Low ==> corresponding SFP+ module is powered
+	/* Low ==> corresponding SFP+ module is powered
 	 * high ==> the SFP+ module is powered down
 	 */
 	bnx2x_set_cfg_pin(bp, pin_cfg, power ^ 1);
@@ -8474,14 +8351,12 @@ int bnx2x_sfp_module_detection(struct bnx2x_phy *phy,
 		bnx2x_set_sfp_module_fault_led(params, MISC_REGISTERS_GPIO_LOW);
 	}
 
-	/*
-	 * Check and set limiting mode / LRM mode on 8726. On 8727 it
+	/* Check and set limiting mode / LRM mode on 8726. On 8727 it
 	 * is done automatically
 	 */
 	bnx2x_set_limiting_mode(params, phy, edc_mode);
 
-	/*
-	 * Enable transmit for this module if the module is approved, or
+	/* Enable transmit for this module if the module is approved, or
 	 * if unapproved modules should also enable the Tx laser
 	 */
 	if (rc == 0 ||
@@ -8536,8 +8411,7 @@ void bnx2x_handle_module_detect_int(struct link_params *params)
 		bnx2x_set_gpio_int(bp, gpio_num,
 				   MISC_REGISTERS_GPIO_INT_OUTPUT_SET,
 				   gpio_port);
-		/*
-		 * Module was plugged out.
+		/* Module was plugged out.
 		 * Disable transmit for this module
 		 */
 		phy->media_type = ETH_PHY_NOT_PRESENT;
@@ -8607,8 +8481,7 @@ static u8 bnx2x_8706_8726_read_status(struct bnx2x_phy *phy,
 
 	DP(NETIF_MSG_LINK, "8706/8726 rx_sd 0x%x pcs_status 0x%x 1Gbps"
 			" link_status 0x%x\n", rx_sd, pcs_status, val2);
-	/*
-	 * link is up if both bit 0 of pmd_rx_sd and bit 0 of pcs_status
+	/* Link is up if both bit 0 of pmd_rx_sd and bit 0 of pcs_status
 	 * are set, or if the autoneg bit 1 is set
 	 */
 	link_up = ((rx_sd & pcs_status & 0x1) || (val2 & (1<<1)));
@@ -8722,8 +8595,7 @@ static u8 bnx2x_8706_config_init(struct bnx2x_phy *phy,
 	}
 	bnx2x_save_bcm_spirom_ver(bp, phy, params->port);
 
-	/*
-	 * If TX Laser is controlled by GPIO_0, do not let PHY go into low
+	/* If TX Laser is controlled by GPIO_0, do not let PHY go into low
 	 * power mode, if TX Laser is disabled
 	 */
 
@@ -8833,8 +8705,7 @@ static int bnx2x_8726_config_init(struct bnx2x_phy *phy,
 
 	bnx2x_8726_external_rom_boot(phy, params);
 
-	/*
-	 * Need to call module detected on initialization since the module
+	/* Need to call module detected on initialization since the module
 	 * detection triggered by actual module insertion might occur before
 	 * driver is loaded, and when driver is loaded, it reset all
 	 * registers, including the transmitter
@@ -8871,8 +8742,7 @@ static int bnx2x_8726_config_init(struct bnx2x_phy *phy,
 				 MDIO_AN_DEVAD, MDIO_AN_REG_CL37_AN, 0x1000);
 		bnx2x_cl45_write(bp, phy,
 				MDIO_AN_DEVAD, MDIO_AN_REG_CTRL, 0x1200);
-		/*
-		 * Enable RX-ALARM control to receive interrupt for 1G speed
+		/* Enable RX-ALARM control to receive interrupt for 1G speed
 		 * change
 		 */
 		bnx2x_cl45_write(bp, phy,
@@ -8973,8 +8843,7 @@ static void bnx2x_8727_hw_reset(struct bnx2x_phy *phy,
 				struct link_params *params) {
 	u32 swap_val, swap_override;
 	u8 port;
-	/*
-	 * The PHY reset is controlled by GPIO 1. Fake the port number
+	/* The PHY reset is controlled by GPIO 1. Fake the port number
 	 * to cancel the swap done in set_gpio()
 	 */
 	struct bnx2x *bp = params->bp;
@@ -9012,14 +8881,12 @@ static int bnx2x_8727_config_init(struct bnx2x_phy *phy,
 	bnx2x_cl45_write(bp, phy,
 			 MDIO_PMA_DEVAD, MDIO_PMA_LASI_CTRL, lasi_ctrl_val);
 
-	/*
-	 * Initially configure MOD_ABS to interrupt when module is
+	/* Initially configure MOD_ABS to interrupt when module is
 	 * presence( bit 8)
 	 */
 	bnx2x_cl45_read(bp, phy,
 			MDIO_PMA_DEVAD, MDIO_PMA_REG_PHY_IDENTIFIER, &mod_abs);
-	/*
-	 * Set EDC off by setting OPTXLOS signal input to low (bit 9).
+	/* Set EDC off by setting OPTXLOS signal input to low (bit 9).
 	 * When the EDC is off it locks onto a reference clock and avoids
 	 * becoming 'lost'
 	 */
@@ -9040,8 +8907,7 @@ static int bnx2x_8727_config_init(struct bnx2x_phy *phy,
 	if (phy->flags & FLAGS_NOC)
 		val |= (3<<5);
 
-	/*
-	 * Set 8727 GPIOs to input to allow reading from the 8727 GPIO0
+	/* Set 8727 GPIOs to input to allow reading from the 8727 GPIO0
 	 * status which reflect SFP+ module over-current
 	 */
 	if (!(phy->flags & FLAGS_NOC))
@@ -9067,8 +8933,7 @@ static int bnx2x_8727_config_init(struct bnx2x_phy *phy,
 		bnx2x_cl45_read(bp, phy,
 				MDIO_PMA_DEVAD, MDIO_PMA_REG_10G_CTRL2, &tmp1);
 		DP(NETIF_MSG_LINK, "1.7 = 0x%x\n", tmp1);
-		/*
-		 * Power down the XAUI until link is up in case of dual-media
+		/* Power down the XAUI until link is up in case of dual-media
 		 * and 1G
 		 */
 		if (DUAL_MEDIA(params)) {
@@ -9093,8 +8958,7 @@ static int bnx2x_8727_config_init(struct bnx2x_phy *phy,
 		bnx2x_cl45_write(bp, phy,
 				 MDIO_AN_DEVAD, MDIO_AN_REG_CL37_AN, 0x1300);
 	} else {
-		/*
-		 * Since the 8727 has only single reset pin, need to set the 10G
+		/* Since the 8727 has only single reset pin, need to set the 10G
 		 * registers although it is default
 		 */
 		bnx2x_cl45_write(bp, phy,
@@ -9109,8 +8973,7 @@ static int bnx2x_8727_config_init(struct bnx2x_phy *phy,
 				 0x0008);
 	}
 
-	/*
-	 * Set 2-wire transfer rate of SFP+ module EEPROM
+	/* Set 2-wire transfer rate of SFP+ module EEPROM
 	 * to 100Khz since some DACs(direct attached cables) do
 	 * not work at 400Khz.
 	 */
@@ -9133,8 +8996,7 @@ static int bnx2x_8727_config_init(struct bnx2x_phy *phy,
 				 phy->tx_preemphasis[1]);
 	}
 
-	/*
-	 * If TX Laser is controlled by GPIO_0, do not let PHY go into low
+	/* If TX Laser is controlled by GPIO_0, do not let PHY go into low
 	 * power mode, if TX Laser is disabled
 	 */
 	tx_en_mode = REG_RD(bp, params->shmem_base +
@@ -9180,8 +9042,7 @@ static void bnx2x_8727_handle_mod_abs(struct bnx2x_phy *phy,
 		DP(NETIF_MSG_LINK,
 		   "MOD_ABS indication show module is absent\n");
 		phy->media_type = ETH_PHY_NOT_PRESENT;
-		/*
-		 * 1. Set mod_abs to detect next module
+		/* 1. Set mod_abs to detect next module
 		 *    presence event
 		 * 2. Set EDC off by setting OPTXLOS signal input to low
 		 *    (bit 9).
@@ -9195,8 +9056,7 @@ static void bnx2x_8727_handle_mod_abs(struct bnx2x_phy *phy,
 				 MDIO_PMA_DEVAD,
 				 MDIO_PMA_REG_PHY_IDENTIFIER, mod_abs);
 
-		/*
-		 * Clear RX alarm since it stays up as long as
+		/* Clear RX alarm since it stays up as long as
 		 * the mod_abs wasn't changed
 		 */
 		bnx2x_cl45_read(bp, phy,
@@ -9207,8 +9067,7 @@ static void bnx2x_8727_handle_mod_abs(struct bnx2x_phy *phy,
 		/* Module is present */
 		DP(NETIF_MSG_LINK,
 		   "MOD_ABS indication show module is present\n");
-		/*
-		 * First disable transmitter, and if the module is ok, the
+		/* First disable transmitter, and if the module is ok, the
 		 * module_detection will enable it
 		 * 1. Set mod_abs to detect next module absent event ( bit 8)
 		 * 2. Restore the default polarity of the OPRXLOS signal and
@@ -9222,8 +9081,7 @@ static void bnx2x_8727_handle_mod_abs(struct bnx2x_phy *phy,
 				 MDIO_PMA_DEVAD,
 				 MDIO_PMA_REG_PHY_IDENTIFIER, mod_abs);
 
-		/*
-		 * Clear RX alarm since it stays up as long as the mod_abs
+		/* Clear RX alarm since it stays up as long as the mod_abs
 		 * wasn't changed. This is need to be done before calling the
 		 * module detection, otherwise it will clear* the link update
 		 * alarm
@@ -9284,8 +9142,7 @@ static u8 bnx2x_8727_read_status(struct bnx2x_phy *phy,
 	bnx2x_cl45_read(bp, phy,
 			MDIO_PMA_DEVAD, MDIO_PMA_REG_M8051_MSGOUT_REG, &val1);
 
-	/*
-	 * If a module is present and there is need to check
+	/* If a module is present and there is need to check
 	 * for over current
 	 */
 	if (!(phy->flags & FLAGS_NOC) && !(rx_alarm_status & (1<<5))) {
@@ -9350,8 +9207,7 @@ static u8 bnx2x_8727_read_status(struct bnx2x_phy *phy,
 			MDIO_PMA_DEVAD,
 			MDIO_PMA_REG_8073_SPEED_LINK_STATUS, &link_status);
 
-	/*
-	 * Bits 0..2 --> speed detected,
+	/* Bits 0..2 --> speed detected,
 	 * Bits 13..15--> link is down
 	 */
 	if ((link_status & (1<<2)) && (!(link_status & (1<<15)))) {
@@ -9394,8 +9250,7 @@ static u8 bnx2x_8727_read_status(struct bnx2x_phy *phy,
 		bnx2x_cl45_read(bp, phy,
 				MDIO_PMA_DEVAD,
 				MDIO_PMA_REG_8727_PCS_GP, &val1);
-		/*
-		 * In case of dual-media board and 1G, power up the XAUI side,
+		/* In case of dual-media board and 1G, power up the XAUI side,
 		 * otherwise power it down. For 10G it is done automatically
 		 */
 		if (link_up)
@@ -9561,8 +9416,7 @@ static int bnx2x_848xx_cmn_config_init(struct bnx2x_phy *phy,
 		/* Save spirom version */
 		bnx2x_save_848xx_spirom_version(phy, bp, params->port);
 	}
-	/*
-	 * This phy uses the NIG latch mechanism since link indication
+	/* This phy uses the NIG latch mechanism since link indication
 	 * arrives through its LED4 and not via its LASI signal, so we
 	 * get steady signal instead of clear on read
 	 */
@@ -9667,8 +9521,7 @@ static int bnx2x_848xx_cmn_config_init(struct bnx2x_phy *phy,
 	if (phy->req_duplex == DUPLEX_FULL)
 		autoneg_val |= (1<<8);
 
-	/*
-	 * Always write this if this is not 84833.
+	/* Always write this if this is not 84833.
 	 * For 84833, write it only when it's a forced speed.
 	 */
 	if ((phy->type != PORT_HW_CFG_XGXS_EXT_PHY_TYPE_BCM84833) ||
@@ -9916,8 +9769,7 @@ static int bnx2x_848x3_config_init(struct bnx2x_phy *phy,
 	/* Wait for GPHY to come out of reset */
 	msleep(50);
 	if (phy->type != PORT_HW_CFG_XGXS_EXT_PHY_TYPE_BCM84833) {
-		/*
-		 * BCM84823 requires that XGXS links up first @ 10G for normal
+		/* BCM84823 requires that XGXS links up first @ 10G for normal
 		 * behavior.
 		 */
 		u16 temp;
@@ -10393,8 +10245,7 @@ static void bnx2x_848xx_set_link_led(struct bnx2x_phy *phy,
 		break;
 	}
 
-	/*
-	 * This is a workaround for E3+84833 until autoneg
+	/* This is a workaround for E3+84833 until autoneg
 	 * restart is fixed in f/w
 	 */
 	if (CHIP_IS_E3(bp)) {
@@ -10418,8 +10269,7 @@ static int bnx2x_54618se_config_init(struct bnx2x_phy *phy,
 	DP(NETIF_MSG_LINK, "54618SE cfg init\n");
 	usleep_range(1000, 1000);
 
-	/*
-	 * This works with E3 only, no need to check the chip
+	/* This works with E3 only, no need to check the chip
 	 * before determining the port.
 	 */
 	port = params->port;
@@ -10441,7 +10291,7 @@ static int bnx2x_54618se_config_init(struct bnx2x_phy *phy,
 			 MDIO_PMA_REG_CTRL, 0x8000);
 	bnx2x_wait_reset_complete(bp, phy, params);
 
-	/*wait for GPHY to reset */
+	/* Wait for GPHY to reset */
 	msleep(50);
 
 	/* Configure LED4: set to INTR (0x6). */
@@ -10647,13 +10497,11 @@ static void bnx2x_54618se_link_reset(struct bnx2x_phy *phy,
 	u32 cfg_pin;
 	u8 port;
 
-	/*
-	 * In case of no EPIO routed to reset the GPHY, put it
+	/* In case of no EPIO routed to reset the GPHY, put it
 	 * in low power mode.
 	 */
 	bnx2x_cl22_write(bp, phy, MDIO_PMA_REG_CTRL, 0x800);
-	/*
-	 * This works with E3 only, no need to check the chip
+	/* This works with E3 only, no need to check the chip
 	 * before determining the port.
 	 */
 	port = params->port;
@@ -10762,7 +10610,7 @@ static u8 bnx2x_54618se_read_status(struct bnx2x_phy *phy,
 		bnx2x_ext_phy_resolve_fc(phy, params, vars);
 
 		if (vars->link_status & LINK_STATUS_AUTO_NEGOTIATE_COMPLETE) {
-			/* report LP advertised speeds */
+			/* Report LP advertised speeds */
 			bnx2x_cl22_read(bp, phy, 0x5, &val);
 
 			if (val & (1<<5))
@@ -10827,8 +10675,7 @@ static void bnx2x_54618se_config_loopback(struct bnx2x_phy *phy,
 	/* This register opens the gate for the UMAC despite its name */
 	REG_WR(bp, NIG_REG_EGRESS_EMAC0_PORT + params->port*4, 1);
 
-	/*
-	 * Maximum Frame Length (RW). Defines a 14-Bit maximum frame
+	/* Maximum Frame Length (RW). Defines a 14-Bit maximum frame
 	 * length used by the MAC receive logic to check frames.
 	 */
 	REG_WR(bp, umac_base + UMAC_REG_MAXFR, 0x2710);
@@ -11101,22 +10948,23 @@ static struct bnx2x_phy phy_warpcore = {
 	.type		= PORT_HW_CFG_XGXS_EXT_PHY_TYPE_DIRECT,
 	.addr		= 0xff,
 	.def_md_devad	= 0,
-	.flags		= FLAGS_HW_LOCK_REQUIRED,
+	.flags		= (FLAGS_HW_LOCK_REQUIRED |
+			   FLAGS_TX_ERROR_CHECK),
 	.rx_preemphasis	= {0xffff, 0xffff, 0xffff, 0xffff},
 	.tx_preemphasis	= {0xffff, 0xffff, 0xffff, 0xffff},
 	.mdio_ctrl	= 0,
 	.supported	= (SUPPORTED_10baseT_Half |
-			     SUPPORTED_10baseT_Full |
-			     SUPPORTED_100baseT_Half |
-			     SUPPORTED_100baseT_Full |
-			     SUPPORTED_1000baseT_Full |
-			     SUPPORTED_10000baseT_Full |
-			     SUPPORTED_20000baseKR2_Full |
-			     SUPPORTED_20000baseMLD2_Full |
-			     SUPPORTED_FIBRE |
-			     SUPPORTED_Autoneg |
-			     SUPPORTED_Pause |
-			     SUPPORTED_Asym_Pause),
+			   SUPPORTED_10baseT_Full |
+			   SUPPORTED_100baseT_Half |
+			   SUPPORTED_100baseT_Full |
+			   SUPPORTED_1000baseT_Full |
+			   SUPPORTED_10000baseT_Full |
+			   SUPPORTED_20000baseKR2_Full |
+			   SUPPORTED_20000baseMLD2_Full |
+			   SUPPORTED_FIBRE |
+			   SUPPORTED_Autoneg |
+			   SUPPORTED_Pause |
+			   SUPPORTED_Asym_Pause),
 	.media_type	= ETH_PHY_UNSPECIFIED,
 	.ver_addr	= 0,
 	.req_flow_ctrl	= 0,
@@ -11258,7 +11106,8 @@ static struct bnx2x_phy phy_8726 = {
 	.addr		= 0xff,
 	.def_md_devad	= 0,
 	.flags		= (FLAGS_HW_LOCK_REQUIRED |
-			   FLAGS_INIT_XGXS_FIRST),
+			   FLAGS_INIT_XGXS_FIRST |
+			   FLAGS_TX_ERROR_CHECK),
 	.rx_preemphasis	= {0xffff, 0xffff, 0xffff, 0xffff},
 	.tx_preemphasis	= {0xffff, 0xffff, 0xffff, 0xffff},
 	.mdio_ctrl	= 0,
@@ -11289,7 +11138,8 @@ static struct bnx2x_phy phy_8727 = {
 	.type		= PORT_HW_CFG_XGXS_EXT_PHY_TYPE_BCM8727,
 	.addr		= 0xff,
 	.def_md_devad	= 0,
-	.flags		= FLAGS_FAN_FAILURE_DET_REQ,
+	.flags		= (FLAGS_FAN_FAILURE_DET_REQ |
+			   FLAGS_TX_ERROR_CHECK),
 	.rx_preemphasis	= {0xffff, 0xffff, 0xffff, 0xffff},
 	.tx_preemphasis	= {0xffff, 0xffff, 0xffff, 0xffff},
 	.mdio_ctrl	= 0,
@@ -11354,8 +11204,9 @@ static struct bnx2x_phy phy_84823 = {
 	.type		= PORT_HW_CFG_XGXS_EXT_PHY_TYPE_BCM84823,
 	.addr		= 0xff,
 	.def_md_devad	= 0,
-	.flags		= FLAGS_FAN_FAILURE_DET_REQ |
-			  FLAGS_REARM_LATCH_SIGNAL,
+	.flags		= (FLAGS_FAN_FAILURE_DET_REQ |
+			   FLAGS_REARM_LATCH_SIGNAL |
+			   FLAGS_TX_ERROR_CHECK),
 	.rx_preemphasis	= {0xffff, 0xffff, 0xffff, 0xffff},
 	.tx_preemphasis	= {0xffff, 0xffff, 0xffff, 0xffff},
 	.mdio_ctrl	= 0,
@@ -11390,8 +11241,9 @@ static struct bnx2x_phy phy_84833 = {
 	.type		= PORT_HW_CFG_XGXS_EXT_PHY_TYPE_BCM84833,
 	.addr		= 0xff,
 	.def_md_devad	= 0,
-	.flags		= FLAGS_FAN_FAILURE_DET_REQ |
-			    FLAGS_REARM_LATCH_SIGNAL,
+	.flags		= (FLAGS_FAN_FAILURE_DET_REQ |
+			   FLAGS_REARM_LATCH_SIGNAL |
+			   FLAGS_TX_ERROR_CHECK),
 	.rx_preemphasis	= {0xffff, 0xffff, 0xffff, 0xffff},
 	.tx_preemphasis	= {0xffff, 0xffff, 0xffff, 0xffff},
 	.mdio_ctrl	= 0,
@@ -11466,9 +11318,8 @@ static void bnx2x_populate_preemphasis(struct bnx2x *bp, u32 shmem_base,
 	/* Get the 4 lanes xgxs config rx and tx */
 	u32 rx = 0, tx = 0, i;
 	for (i = 0; i < 2; i++) {
-		/*
-		 * INT_PHY and EXT_PHY1 share the same value location in the
-		 * shmem. When num_phys is greater than 1, than this value
+		/* INT_PHY and EXT_PHY1 share the same value location in
+		 * the shmem. When num_phys is greater than 1, than this value
 		 * applies only to EXT_PHY1
 		 */
 		if (phy_index == INT_PHY || phy_index == EXT_PHY1) {
@@ -11546,8 +11397,7 @@ static int bnx2x_populate_int_phy(struct bnx2x *bp, u32 shmem_base, u8 port,
 					offsetof(struct shmem_region, dev_info.
 					port_hw_config[port].default_cfg)) &
 				 PORT_HW_CFG_NET_SERDES_IF_MASK);
-		/*
-		 * Set the appropriate supported and flags indications per
+		/* Set the appropriate supported and flags indications per
 		 * interface type of the chip
 		 */
 		switch (serdes_net_if) {
@@ -11605,8 +11455,7 @@ static int bnx2x_populate_int_phy(struct bnx2x *bp, u32 shmem_base, u8 port,
 			break;
 		}
 
-		/*
-		 * Enable MDC/MDIO work-around for E3 A0 since free running MDC
+		/* Enable MDC/MDIO work-around for E3 A0 since free running MDC
 		 * was not set as expected. For B0, ECO will be enabled so there
 		 * won't be an issue there
 		 */
@@ -11719,8 +11568,7 @@ static int bnx2x_populate_ext_phy(struct bnx2x *bp,
 	phy->addr = XGXS_EXT_PHY_ADDR(ext_phy_config);
 	bnx2x_populate_preemphasis(bp, shmem_base, phy, port, phy_index);
 
-	/*
-	 * The shmem address of the phy version is located on different
+	/* The shmem address of the phy version is located on different
 	 * structures. In case this structure is too old, do not set
 	 * the address
 	 */
@@ -11754,8 +11602,7 @@ static int bnx2x_populate_ext_phy(struct bnx2x *bp,
 
 	if ((phy->type == PORT_HW_CFG_XGXS_EXT_PHY_TYPE_BCM84833) &&
 	    (phy->ver_addr)) {
-		/*
-		 * Remove 100Mb link supported for BCM84833 when phy fw
+		/* Remove 100Mb link supported for BCM84833 when phy fw
 		 * version lower than or equal to 1.39
 		 */
 		u32 raw_ver = REG_RD(bp, phy->ver_addr);
@@ -11765,8 +11612,7 @@ static int bnx2x_populate_ext_phy(struct bnx2x *bp,
 					    SUPPORTED_100baseT_Full);
 	}
 
-	/*
-	 * In case mdc/mdio_access of the external phy is different than the
+	/* In case mdc/mdio_access of the external phy is different than the
 	 * mdc/mdio access of the XGXS, a HW lock must be taken in each access
 	 * to prevent one port interfere with another port's CL45 operations.
 	 */
@@ -11936,13 +11782,16 @@ int bnx2x_phy_probe(struct link_params *params)
 		if (phy->type == PORT_HW_CFG_XGXS_EXT_PHY_TYPE_NOT_CONN)
 			break;
 
+		if (params->feature_config_flags &
+		    FEATURE_CONFIG_DISABLE_REMOTE_FAULT_DET)
+			phy->flags &= ~FLAGS_TX_ERROR_CHECK;
+
 		sync_offset = params->shmem_base +
 			offsetof(struct shmem_region,
 			dev_info.port_hw_config[params->port].media_type);
 		media_types = REG_RD(bp, sync_offset);
 
-		/*
-		 * Update media type for non-PMF sync only for the first time
+		/* Update media type for non-PMF sync only for the first time
 		 * In case the media type changes afterwards, it will be updated
 		 * using the update_status function
 		 */
@@ -12016,8 +11865,7 @@ void bnx2x_init_xmac_loopback(struct link_params *params,
 	vars->flow_ctrl = BNX2X_FLOW_CTRL_NONE;
 	vars->mac_type = MAC_TYPE_XMAC;
 	vars->phy_flags = PHY_XGXS_FLAG;
-	/*
-	 * Set WC to loopback mode since link is required to provide clock
+	/* Set WC to loopback mode since link is required to provide clock
 	 * to the XMAC in 20G mode
 	 */
 	bnx2x_set_aer_mmd(params, &params->phy[0]);
@@ -12162,6 +12010,7 @@ int bnx2x_phy_init(struct link_params *params, struct link_vars *vars)
 		bnx2x_link_int_enable(params);
 		break;
 	}
+	bnx2x_update_mng(params, vars->link_status);
 	return 0;
 }
 
@@ -12302,7 +12151,8 @@ static int bnx2x_8073_common_init_phy(struct bnx2x *bp,
 				NIG_MASK_MI_INT));
 
 		/* Need to take the phy out of low power mode in order
-			to write to access its registers */
+		 * to write to access its registers
+		 */
 		bnx2x_set_gpio(bp, MISC_REGISTERS_GPIO_2,
 			       MISC_REGISTERS_GPIO_OUTPUT_HIGH,
 			       port);
@@ -12350,8 +12200,7 @@ static int bnx2x_8073_common_init_phy(struct bnx2x *bp,
 				 (val | 1<<10));
 	}
 
-	/*
-	 * Toggle Transmitter: Power down and then up with 600ms delay
+	/* Toggle Transmitter: Power down and then up with 600ms delay
 	 * between
 	 */
 	msleep(600);
@@ -12494,8 +12343,7 @@ static int bnx2x_8727_common_init_phy(struct bnx2x *bp,
 	reset_gpio = MISC_REGISTERS_GPIO_1;
 	port = 1;
 
-	/*
-	 * Retrieve the reset gpio/port which control the reset.
+	/* Retrieve the reset gpio/port which control the reset.
 	 * Default is GPIO1, PORT1
 	 */
 	bnx2x_get_ext_phy_reset_gpio(bp, shmem_base_path[0],
@@ -12670,8 +12518,7 @@ static int bnx2x_ext_phy_common_init(struct bnx2x *bp, u32 shmem_base_path[],
 		break;
 
 	case PORT_HW_CFG_XGXS_EXT_PHY_TYPE_BCM8726:
-		/*
-		 * GPIO1 affects both ports, so there's need to pull
+		/* GPIO1 affects both ports, so there's need to pull
 		 * it for single port alone
 		 */
 		rc = bnx2x_8726_common_init_phy(bp, shmem_base_path,
@@ -12679,8 +12526,7 @@ static int bnx2x_ext_phy_common_init(struct bnx2x *bp, u32 shmem_base_path[],
 						phy_index, chip_id);
 		break;
 	case PORT_HW_CFG_XGXS_EXT_PHY_TYPE_BCM84833:
-		/*
-		 * GPIO3's are linked, and so both need to be toggled
+		/* GPIO3's are linked, and so both need to be toggled
 		 * to obtain required 2us pulse.
 		 */
 		rc = bnx2x_84833_common_init_phy(bp, shmem_base_path,
@@ -12779,7 +12625,8 @@ static void bnx2x_check_over_curr(struct link_params *params,
 }
 
 static void bnx2x_analyze_link_error(struct link_params *params,
-				     struct link_vars *vars, u32 lss_status)
+				     struct link_vars *vars, u32 lss_status,
+				     u8 notify)
 {
 	struct bnx2x *bp = params->bp;
 	/* Compare new value with previous value */
@@ -12793,8 +12640,7 @@ static void bnx2x_analyze_link_error(struct link_params *params,
 	DP(NETIF_MSG_LINK, "Link changed:%x %x->%x\n", vars->link_up,
 		       half_open_conn, lss_status);
 
-	/*
-	 * a. Update shmem->link_status accordingly
+	/* a. Update shmem->link_status accordingly
 	 * b. Update link_vars->link_up
 	 */
 	if (lss_status) {
@@ -12802,8 +12648,10 @@ static void bnx2x_analyze_link_error(struct link_params *params,
 		vars->link_status &= ~LINK_STATUS_LINK_UP;
 		vars->link_up = 0;
 		vars->phy_flags |= PHY_HALF_OPEN_CONN_FLAG;
-		/*
-		 * Set LED mode to off since the PHY doesn't know about these
+
+		/* activate nig drain */
+		REG_WR(bp, NIG_REG_EGRESS_DRAIN0_MODE + params->port*4, 1);
+		/* Set LED mode to off since the PHY doesn't know about these
 		 * errors
 		 */
 		led_mode = LED_MODE_OFF;
@@ -12813,7 +12661,11 @@ static void bnx2x_analyze_link_error(struct link_params *params,
 		vars->link_up = 1;
 		vars->phy_flags &= ~PHY_HALF_OPEN_CONN_FLAG;
 		led_mode = LED_MODE_OPER;
+
+		/* Clear nig drain */
+		REG_WR(bp, NIG_REG_EGRESS_DRAIN0_MODE + params->port*4, 0);
 	}
+	bnx2x_sync_link(params, vars);
 	/* Update the LED according to the link state */
 	bnx2x_set_led(params, vars, led_mode, SPEED_10000);
 
@@ -12822,7 +12674,8 @@ static void bnx2x_analyze_link_error(struct link_params *params,
 
 	/* C. Trigger General Attention */
 	vars->periodic_flags |= PERIODIC_FLAGS_LINK_EVENT;
-	bnx2x_notify_link_changed(bp);
+	if (notify)
+		bnx2x_notify_link_changed(bp);
 }
 
 /******************************************************************************
@@ -12834,22 +12687,23 @@ static void bnx2x_analyze_link_error(struct link_params *params,
 *	a fault, for example, due to break in the TX side of fiber.
 *
 ******************************************************************************/
-static void bnx2x_check_half_open_conn(struct link_params *params,
-				       struct link_vars *vars)
+int bnx2x_check_half_open_conn(struct link_params *params,
+				struct link_vars *vars,
+				u8 notify)
 {
 	struct bnx2x *bp = params->bp;
 	u32 lss_status = 0;
 	u32 mac_base;
 	/* In case link status is physically up @ 10G do */
-	if ((vars->phy_flags & PHY_PHYSICAL_LINK_FLAG) == 0)
-		return;
+	if (((vars->phy_flags & PHY_PHYSICAL_LINK_FLAG) == 0) ||
+	    (REG_RD(bp, NIG_REG_EGRESS_EMAC0_PORT + params->port*4)))
+		return 0;
 
 	if (CHIP_IS_E3(bp) &&
 	    (REG_RD(bp, MISC_REG_RESET_REG_2) &
 	      (MISC_REGISTERS_RESET_REG_2_XMAC))) {
 		/* Check E3 XMAC */
-		/*
-		 * Note that link speed cannot be queried here, since it may be
+		/* Note that link speed cannot be queried here, since it may be
 		 * zero while link is down. In case UMAC is active, LSS will
 		 * simply not be set
 		 */
@@ -12863,7 +12717,7 @@ static void bnx2x_check_half_open_conn(struct link_params *params,
 		if (REG_RD(bp, mac_base + XMAC_REG_RX_LSS_STATUS))
 			lss_status = 1;
 
-		bnx2x_analyze_link_error(params, vars, lss_status);
+		bnx2x_analyze_link_error(params, vars, lss_status, notify);
 	} else if (REG_RD(bp, MISC_REG_RESET_REG_2) &
 		   (MISC_REGISTERS_RESET_REG_2_RST_BMAC0 << params->port)) {
 		/* Check E1X / E2 BMAC */
@@ -12880,18 +12734,21 @@ static void bnx2x_check_half_open_conn(struct link_params *params,
 		REG_RD_DMAE(bp, mac_base + lss_status_reg, wb_data, 2);
 		lss_status = (wb_data[0] > 0);
 
-		bnx2x_analyze_link_error(params, vars, lss_status);
+		bnx2x_analyze_link_error(params, vars, lss_status, notify);
 	}
+	return 0;
 }
 
 void bnx2x_period_func(struct link_params *params, struct link_vars *vars)
 {
-	struct bnx2x *bp = params->bp;
 	u16 phy_idx;
+	struct bnx2x *bp = params->bp;
 	for (phy_idx = INT_PHY; phy_idx < MAX_PHYS; phy_idx++) {
 		if (params->phy[phy_idx].flags & FLAGS_TX_ERROR_CHECK) {
 			bnx2x_set_aer_mmd(params, &params->phy[phy_idx]);
-			bnx2x_check_half_open_conn(params, vars);
+			if (bnx2x_check_half_open_conn(params, vars, 1) !=
+			    0)
+				DP(NETIF_MSG_LINK, "Fault detection failed\n");
 			break;
 		}
 	}
diff --git a/drivers/net/ethernet/broadcom/bnx2x/bnx2x_link.h b/drivers/net/ethernet/broadcom/bnx2x/bnx2x_link.h
index 763535ee483..ea4371f4335 100644
--- a/drivers/net/ethernet/broadcom/bnx2x/bnx2x_link.h
+++ b/drivers/net/ethernet/broadcom/bnx2x/bnx2x_link.h
@@ -254,8 +254,10 @@ struct link_params {
 #define FEATURE_CONFIG_PFC_ENABLED			(1<<1)
 #define FEATURE_CONFIG_BC_SUPPORTS_OPT_MDL_VRFY		(1<<2)
 #define FEATURE_CONFIG_BC_SUPPORTS_DUAL_PHY_OPT_MDL_VRFY	(1<<3)
+#define FEATURE_CONFIG_BC_SUPPORTS_AFEX			(1<<8)
 #define FEATURE_CONFIG_AUTOGREEEN_ENABLED			(1<<9)
 #define FEATURE_CONFIG_BC_SUPPORTS_SFP_TX_DISABLED		(1<<10)
+#define FEATURE_CONFIG_DISABLE_REMOTE_FAULT_DET		(1<<11)
 	/* Will be populated during common init */
 	struct bnx2x_phy phy[MAX_PHYS];
 
@@ -495,4 +497,6 @@ int bnx2x_sfp_module_detection(struct bnx2x_phy *phy,
 
 void bnx2x_period_func(struct link_params *params, struct link_vars *vars);
 
+int bnx2x_check_half_open_conn(struct link_params *params,
+			       struct link_vars *vars, u8 notify);
 #endif /* BNX2X_LINK_H */
diff --git a/drivers/net/ethernet/broadcom/bnx2x/bnx2x_main.c b/drivers/net/ethernet/broadcom/bnx2x/bnx2x_main.c
index e077d250872..f755a665dab 100644
--- a/drivers/net/ethernet/broadcom/bnx2x/bnx2x_main.c
+++ b/drivers/net/ethernet/broadcom/bnx2x/bnx2x_main.c
@@ -39,7 +39,6 @@
 #include <linux/time.h>
 #include <linux/ethtool.h>
 #include <linux/mii.h>
-#include <linux/if.h>
 #include <linux/if_vlan.h>
 #include <net/ip.h>
 #include <net/ipv6.h>
@@ -93,15 +92,11 @@ MODULE_FIRMWARE(FW_FILE_NAME_E1);
 MODULE_FIRMWARE(FW_FILE_NAME_E1H);
 MODULE_FIRMWARE(FW_FILE_NAME_E2);
 
-static int multi_mode = 1;
-module_param(multi_mode, int, 0);
-MODULE_PARM_DESC(multi_mode, " Multi queue mode "
-			     "(0 Disable; 1 Enable (default))");
 
 int num_queues;
 module_param(num_queues, int, 0);
-MODULE_PARM_DESC(num_queues, " Number of queues for multi_mode=1"
-				" (default is as a number of CPUs)");
+MODULE_PARM_DESC(num_queues,
+		 " Set number of queues (default is as a number of CPUs)");
 
 static int disable_tpa;
 module_param(disable_tpa, int, 0);
@@ -141,7 +136,9 @@ enum bnx2x_board_type {
 	BCM57810,
 	BCM57810_MF,
 	BCM57840,
-	BCM57840_MF
+	BCM57840_MF,
+	BCM57811,
+	BCM57811_MF
 };
 
 /* indexed by board_type, above */
@@ -158,8 +155,9 @@ static struct {
 	{ "Broadcom NetXtreme II BCM57810 10 Gigabit Ethernet" },
 	{ "Broadcom NetXtreme II BCM57810 10 Gigabit Ethernet Multi Function" },
 	{ "Broadcom NetXtreme II BCM57840 10/20 Gigabit Ethernet" },
-	{ "Broadcom NetXtreme II BCM57840 10/20 Gigabit "
-						"Ethernet Multi Function"}
+	{ "Broadcom NetXtreme II BCM57840 10/20 Gigabit Ethernet Multi Function"},
+	{ "Broadcom NetXtreme II BCM57811 10 Gigabit Ethernet"},
+	{ "Broadcom NetXtreme II BCM57811 10 Gigabit Ethernet Multi Function"},
 };
 
 #ifndef PCI_DEVICE_ID_NX2_57710
@@ -195,6 +193,12 @@ static struct {
 #ifndef PCI_DEVICE_ID_NX2_57840_MF
 #define PCI_DEVICE_ID_NX2_57840_MF	CHIP_NUM_57840_MF
 #endif
+#ifndef PCI_DEVICE_ID_NX2_57811
+#define PCI_DEVICE_ID_NX2_57811		CHIP_NUM_57811
+#endif
+#ifndef PCI_DEVICE_ID_NX2_57811_MF
+#define PCI_DEVICE_ID_NX2_57811_MF	CHIP_NUM_57811_MF
+#endif
 static DEFINE_PCI_DEVICE_TABLE(bnx2x_pci_tbl) = {
 	{ PCI_VDEVICE(BROADCOM, PCI_DEVICE_ID_NX2_57710), BCM57710 },
 	{ PCI_VDEVICE(BROADCOM, PCI_DEVICE_ID_NX2_57711), BCM57711 },
@@ -207,6 +211,8 @@ static DEFINE_PCI_DEVICE_TABLE(bnx2x_pci_tbl) = {
 	{ PCI_VDEVICE(BROADCOM, PCI_DEVICE_ID_NX2_57810_MF), BCM57810_MF },
 	{ PCI_VDEVICE(BROADCOM, PCI_DEVICE_ID_NX2_57840), BCM57840 },
 	{ PCI_VDEVICE(BROADCOM, PCI_DEVICE_ID_NX2_57840_MF), BCM57840_MF },
+	{ PCI_VDEVICE(BROADCOM, PCI_DEVICE_ID_NX2_57811), BCM57811 },
+	{ PCI_VDEVICE(BROADCOM, PCI_DEVICE_ID_NX2_57811_MF), BCM57811_MF },
 	{ 0 }
 };
 
@@ -220,15 +226,15 @@ static LIST_HEAD(bnx2x_prev_list);
 * General service functions
 ****************************************************************************/
 
-static inline void __storm_memset_dma_mapping(struct bnx2x *bp,
+static void __storm_memset_dma_mapping(struct bnx2x *bp,
 				       u32 addr, dma_addr_t mapping)
 {
 	REG_WR(bp,  addr, U64_LO(mapping));
 	REG_WR(bp,  addr + 4, U64_HI(mapping));
 }
 
-static inline void storm_memset_spq_addr(struct bnx2x *bp,
-					 dma_addr_t mapping, u16 abs_fid)
+static void storm_memset_spq_addr(struct bnx2x *bp,
+				  dma_addr_t mapping, u16 abs_fid)
 {
 	u32 addr = XSEM_REG_FAST_MEMORY +
 			XSTORM_SPQ_PAGE_BASE_OFFSET(abs_fid);
@@ -236,8 +242,8 @@ static inline void storm_memset_spq_addr(struct bnx2x *bp,
 	__storm_memset_dma_mapping(bp, addr, mapping);
 }
 
-static inline void storm_memset_vf_to_pf(struct bnx2x *bp, u16 abs_fid,
-					 u16 pf_id)
+static void storm_memset_vf_to_pf(struct bnx2x *bp, u16 abs_fid,
+				  u16 pf_id)
 {
 	REG_WR8(bp, BAR_XSTRORM_INTMEM + XSTORM_VF_TO_PF_OFFSET(abs_fid),
 		pf_id);
@@ -249,8 +255,8 @@ static inline void storm_memset_vf_to_pf(struct bnx2x *bp, u16 abs_fid,
 		pf_id);
 }
 
-static inline void storm_memset_func_en(struct bnx2x *bp, u16 abs_fid,
-					u8 enable)
+static void storm_memset_func_en(struct bnx2x *bp, u16 abs_fid,
+				 u8 enable)
 {
 	REG_WR8(bp, BAR_XSTRORM_INTMEM + XSTORM_FUNC_EN_OFFSET(abs_fid),
 		enable);
@@ -262,8 +268,8 @@ static inline void storm_memset_func_en(struct bnx2x *bp, u16 abs_fid,
 		enable);
 }
 
-static inline void storm_memset_eq_data(struct bnx2x *bp,
-				struct event_ring_data *eq_data,
+static void storm_memset_eq_data(struct bnx2x *bp,
+				 struct event_ring_data *eq_data,
 				u16 pfid)
 {
 	size_t size = sizeof(struct event_ring_data);
@@ -273,8 +279,8 @@ static inline void storm_memset_eq_data(struct bnx2x *bp,
 	__storm_memset_struct(bp, addr, size, (u32 *)eq_data);
 }
 
-static inline void storm_memset_eq_prod(struct bnx2x *bp, u16 eq_prod,
-					u16 pfid)
+static void storm_memset_eq_prod(struct bnx2x *bp, u16 eq_prod,
+				 u16 pfid)
 {
 	u32 addr = BAR_CSTRORM_INTMEM + CSTORM_EVENT_RING_PROD_OFFSET(pfid);
 	REG_WR16(bp, addr, eq_prod);
@@ -309,67 +315,6 @@ static u32 bnx2x_reg_rd_ind(struct bnx2x *bp, u32 addr)
 #define DMAE_DP_DST_PCI		"pci dst_addr [%x:%08x]"
 #define DMAE_DP_DST_NONE	"dst_addr [none]"
 
-static void bnx2x_dp_dmae(struct bnx2x *bp, struct dmae_command *dmae,
-			  int msglvl)
-{
-	u32 src_type = dmae->opcode & DMAE_COMMAND_SRC;
-
-	switch (dmae->opcode & DMAE_COMMAND_DST) {
-	case DMAE_CMD_DST_PCI:
-		if (src_type == DMAE_CMD_SRC_PCI)
-			DP(msglvl, "DMAE: opcode 0x%08x\n"
-			   "src [%x:%08x], len [%d*4], dst [%x:%08x]\n"
-			   "comp_addr [%x:%08x], comp_val 0x%08x\n",
-			   dmae->opcode, dmae->src_addr_hi, dmae->src_addr_lo,
-			   dmae->len, dmae->dst_addr_hi, dmae->dst_addr_lo,
-			   dmae->comp_addr_hi, dmae->comp_addr_lo,
-			   dmae->comp_val);
-		else
-			DP(msglvl, "DMAE: opcode 0x%08x\n"
-			   "src [%08x], len [%d*4], dst [%x:%08x]\n"
-			   "comp_addr [%x:%08x], comp_val 0x%08x\n",
-			   dmae->opcode, dmae->src_addr_lo >> 2,
-			   dmae->len, dmae->dst_addr_hi, dmae->dst_addr_lo,
-			   dmae->comp_addr_hi, dmae->comp_addr_lo,
-			   dmae->comp_val);
-		break;
-	case DMAE_CMD_DST_GRC:
-		if (src_type == DMAE_CMD_SRC_PCI)
-			DP(msglvl, "DMAE: opcode 0x%08x\n"
-			   "src [%x:%08x], len [%d*4], dst_addr [%08x]\n"
-			   "comp_addr [%x:%08x], comp_val 0x%08x\n",
-			   dmae->opcode, dmae->src_addr_hi, dmae->src_addr_lo,
-			   dmae->len, dmae->dst_addr_lo >> 2,
-			   dmae->comp_addr_hi, dmae->comp_addr_lo,
-			   dmae->comp_val);
-		else
-			DP(msglvl, "DMAE: opcode 0x%08x\n"
-			   "src [%08x], len [%d*4], dst [%08x]\n"
-			   "comp_addr [%x:%08x], comp_val 0x%08x\n",
-			   dmae->opcode, dmae->src_addr_lo >> 2,
-			   dmae->len, dmae->dst_addr_lo >> 2,
-			   dmae->comp_addr_hi, dmae->comp_addr_lo,
-			   dmae->comp_val);
-		break;
-	default:
-		if (src_type == DMAE_CMD_SRC_PCI)
-			DP(msglvl, "DMAE: opcode 0x%08x\n"
-			   "src_addr [%x:%08x]  len [%d * 4]  dst_addr [none]\n"
-			   "comp_addr [%x:%08x]  comp_val 0x%08x\n",
-			   dmae->opcode, dmae->src_addr_hi, dmae->src_addr_lo,
-			   dmae->len, dmae->comp_addr_hi, dmae->comp_addr_lo,
-			   dmae->comp_val);
-		else
-			DP(msglvl, "DMAE: opcode 0x%08x\n"
-			   "src_addr [%08x]  len [%d * 4]  dst_addr [none]\n"
-			   "comp_addr [%x:%08x]  comp_val 0x%08x\n",
-			   dmae->opcode, dmae->src_addr_lo >> 2,
-			   dmae->len, dmae->comp_addr_hi, dmae->comp_addr_lo,
-			   dmae->comp_val);
-		break;
-	}
-
-}
 
 /* copy command into DMAE command memory and set DMAE command go */
 void bnx2x_post_dmae(struct bnx2x *bp, struct dmae_command *dmae, int idx)
@@ -506,8 +451,6 @@ void bnx2x_write_dmae(struct bnx2x *bp, dma_addr_t dma_addr, u32 dst_addr,
 	dmae.dst_addr_hi = 0;
 	dmae.len = len32;
 
-	bnx2x_dp_dmae(bp, &dmae, BNX2X_MSG_OFF);
-
 	/* issue the command and wait for completion */
 	bnx2x_issue_dmae_with_comp(bp, &dmae);
 }
@@ -540,8 +483,6 @@ void bnx2x_read_dmae(struct bnx2x *bp, u32 src_addr, u32 len32)
 	dmae.dst_addr_hi = U64_HI(bnx2x_sp_mapping(bp, wb_data));
 	dmae.len = len32;
 
-	bnx2x_dp_dmae(bp, &dmae, BNX2X_MSG_OFF);
-
 	/* issue the command and wait for completion */
 	bnx2x_issue_dmae_with_comp(bp, &dmae);
 }
@@ -562,27 +503,6 @@ static void bnx2x_write_dmae_phys_len(struct bnx2x *bp, dma_addr_t phys_addr,
 	bnx2x_write_dmae(bp, phys_addr + offset, addr + offset, len);
 }
 
-/* used only for slowpath so not inlined */
-static void bnx2x_wb_wr(struct bnx2x *bp, int reg, u32 val_hi, u32 val_lo)
-{
-	u32 wb_write[2];
-
-	wb_write[0] = val_hi;
-	wb_write[1] = val_lo;
-	REG_WR_DMAE(bp, reg, wb_write, 2);
-}
-
-#ifdef USE_WB_RD
-static u64 bnx2x_wb_rd(struct bnx2x *bp, int reg)
-{
-	u32 wb_data[2];
-
-	REG_RD_DMAE(bp, reg, wb_data, 2);
-
-	return HILO_U64(wb_data[0], wb_data[1]);
-}
-#endif
-
 static int bnx2x_mc_assert(struct bnx2x *bp)
 {
 	char last_idx;
@@ -756,7 +676,7 @@ void bnx2x_fw_dump_lvl(struct bnx2x *bp, const char *lvl)
 	printk("%s" "end of fw dump\n", lvl);
 }
 
-static inline void bnx2x_fw_dump(struct bnx2x *bp)
+static void bnx2x_fw_dump(struct bnx2x *bp)
 {
 	bnx2x_fw_dump_lvl(bp, KERN_ERR);
 }
@@ -1076,8 +996,8 @@ static void bnx2x_pbf_pN_cmd_flushed(struct bnx2x *bp,
 	   poll_count-cur_cnt, FLR_WAIT_INTERVAL, regs->pN);
 }
 
-static inline u32 bnx2x_flr_clnup_reg_poll(struct bnx2x *bp, u32 reg,
-				     u32 expected, u32 poll_count)
+static u32 bnx2x_flr_clnup_reg_poll(struct bnx2x *bp, u32 reg,
+				    u32 expected, u32 poll_count)
 {
 	u32 cur_cnt = poll_count;
 	u32 val;
@@ -1088,8 +1008,8 @@ static inline u32 bnx2x_flr_clnup_reg_poll(struct bnx2x *bp, u32 reg,
 	return val;
 }
 
-static inline int bnx2x_flr_clnup_poll_hw_counter(struct bnx2x *bp, u32 reg,
-						  char *msg, u32 poll_cnt)
+static int bnx2x_flr_clnup_poll_hw_counter(struct bnx2x *bp, u32 reg,
+					   char *msg, u32 poll_cnt)
 {
 	u32 val = bnx2x_flr_clnup_reg_poll(bp, reg, 0, poll_cnt);
 	if (val != 0) {
@@ -1186,7 +1106,7 @@ static void bnx2x_tx_hw_flushed(struct bnx2x *bp, u32 poll_count)
 	(((index) << SDM_OP_GEN_AGG_VECT_IDX_SHIFT) & SDM_OP_GEN_AGG_VECT_IDX)
 
 
-static inline int bnx2x_send_final_clnup(struct bnx2x *bp, u8 clnup_func,
+static int bnx2x_send_final_clnup(struct bnx2x *bp, u8 clnup_func,
 					 u32 poll_cnt)
 {
 	struct sdm_op_gen op_gen = {0};
@@ -1220,7 +1140,7 @@ static inline int bnx2x_send_final_clnup(struct bnx2x *bp, u8 clnup_func,
 	return ret;
 }
 
-static inline u8 bnx2x_is_pcie_pending(struct pci_dev *dev)
+static u8 bnx2x_is_pcie_pending(struct pci_dev *dev)
 {
 	int pos;
 	u16 status;
@@ -1361,14 +1281,17 @@ static void bnx2x_hc_int_enable(struct bnx2x *bp)
 	int port = BP_PORT(bp);
 	u32 addr = port ? HC_REG_CONFIG_1 : HC_REG_CONFIG_0;
 	u32 val = REG_RD(bp, addr);
-	int msix = (bp->flags & USING_MSIX_FLAG) ? 1 : 0;
-	int msi = (bp->flags & USING_MSI_FLAG) ? 1 : 0;
+	bool msix = (bp->flags & USING_MSIX_FLAG) ? true : false;
+	bool single_msix = (bp->flags & USING_SINGLE_MSIX_FLAG) ? true : false;
+	bool msi = (bp->flags & USING_MSI_FLAG) ? true : false;
 
 	if (msix) {
 		val &= ~(HC_CONFIG_0_REG_SINGLE_ISR_EN_0 |
 			 HC_CONFIG_0_REG_INT_LINE_EN_0);
 		val |= (HC_CONFIG_0_REG_MSI_MSIX_INT_EN_0 |
 			HC_CONFIG_0_REG_ATTN_BIT_EN_0);
+		if (single_msix)
+			val |= HC_CONFIG_0_REG_SINGLE_ISR_EN_0;
 	} else if (msi) {
 		val &= ~HC_CONFIG_0_REG_INT_LINE_EN_0;
 		val |= (HC_CONFIG_0_REG_SINGLE_ISR_EN_0 |
@@ -1425,8 +1348,9 @@ static void bnx2x_hc_int_enable(struct bnx2x *bp)
 static void bnx2x_igu_int_enable(struct bnx2x *bp)
 {
 	u32 val;
-	int msix = (bp->flags & USING_MSIX_FLAG) ? 1 : 0;
-	int msi = (bp->flags & USING_MSI_FLAG) ? 1 : 0;
+	bool msix = (bp->flags & USING_MSIX_FLAG) ? true : false;
+	bool single_msix = (bp->flags & USING_SINGLE_MSIX_FLAG) ? true : false;
+	bool msi = (bp->flags & USING_MSI_FLAG) ? true : false;
 
 	val = REG_RD(bp, IGU_REG_PF_CONFIGURATION);
 
@@ -1436,6 +1360,9 @@ static void bnx2x_igu_int_enable(struct bnx2x *bp)
 		val |= (IGU_PF_CONF_FUNC_EN |
 			IGU_PF_CONF_MSI_MSIX_EN |
 			IGU_PF_CONF_ATTN_BIT_EN);
+
+		if (single_msix)
+			val |= IGU_PF_CONF_SINGLE_ISR_EN;
 	} else if (msi) {
 		val &= ~IGU_PF_CONF_INT_LINE_EN;
 		val |= (IGU_PF_CONF_FUNC_EN |
@@ -1455,6 +1382,9 @@ static void bnx2x_igu_int_enable(struct bnx2x *bp)
 
 	REG_WR(bp, IGU_REG_PF_CONFIGURATION, val);
 
+	if (val & IGU_PF_CONF_INT_LINE_EN)
+		pci_intx(bp->pdev, true);
+
 	barrier();
 
 	/* init leading/trailing edge */
@@ -1623,7 +1553,7 @@ static bool bnx2x_trylock_hw_lock(struct bnx2x *bp, u32 resource)
  * Returns the recovery leader resource id according to the engine this function
  * belongs to. Currently only only 2 engines is supported.
  */
-static inline int bnx2x_get_leader_lock_resource(struct bnx2x *bp)
+static int bnx2x_get_leader_lock_resource(struct bnx2x *bp)
 {
 	if (BP_PATH(bp))
 		return HW_LOCK_RESOURCE_RECOVERY_LEADER_1;
@@ -1636,9 +1566,9 @@ static inline int bnx2x_get_leader_lock_resource(struct bnx2x *bp)
  *
  * @bp: driver handle
  *
- * Tries to aquire a leader lock for cuurent engine.
+ * Tries to aquire a leader lock for current engine.
  */
-static inline bool bnx2x_trylock_leader_lock(struct bnx2x *bp)
+static bool bnx2x_trylock_leader_lock(struct bnx2x *bp)
 {
 	return bnx2x_trylock_hw_lock(bp, bnx2x_get_leader_lock_resource(bp));
 }
@@ -1719,6 +1649,27 @@ void bnx2x_sp_event(struct bnx2x_fastpath *fp, union eth_rx_cqe *rr_cqe)
 
 	DP(BNX2X_MSG_SP, "bp->cq_spq_left %x\n", atomic_read(&bp->cq_spq_left));
 
+	if ((drv_cmd == BNX2X_Q_CMD_UPDATE) && (IS_FCOE_FP(fp)) &&
+	    (!!test_bit(BNX2X_AFEX_FCOE_Q_UPDATE_PENDING, &bp->sp_state))) {
+		/* if Q update ramrod is completed for last Q in AFEX vif set
+		 * flow, then ACK MCP at the end
+		 *
+		 * mark pending ACK to MCP bit.
+		 * prevent case that both bits are cleared.
+		 * At the end of load/unload driver checks that
+		 * sp_state is cleaerd, and this order prevents
+		 * races
+		 */
+		smp_mb__before_clear_bit();
+		set_bit(BNX2X_AFEX_PENDING_VIFSET_MCP_ACK, &bp->sp_state);
+		wmb();
+		clear_bit(BNX2X_AFEX_FCOE_Q_UPDATE_PENDING, &bp->sp_state);
+		smp_mb__after_clear_bit();
+
+		/* schedule workqueue to send ack to MCP */
+		queue_delayed_work(bnx2x_wq, &bp->sp_task, 0);
+	}
+
 	return;
 }
 
@@ -2229,40 +2180,6 @@ u8 bnx2x_link_test(struct bnx2x *bp, u8 is_serdes)
 	return rc;
 }
 
-static void bnx2x_init_port_minmax(struct bnx2x *bp)
-{
-	u32 r_param = bp->link_vars.line_speed / 8;
-	u32 fair_periodic_timeout_usec;
-	u32 t_fair;
-
-	memset(&(bp->cmng.rs_vars), 0,
-	       sizeof(struct rate_shaping_vars_per_port));
-	memset(&(bp->cmng.fair_vars), 0, sizeof(struct fairness_vars_per_port));
-
-	/* 100 usec in SDM ticks = 25 since each tick is 4 usec */
-	bp->cmng.rs_vars.rs_periodic_timeout = RS_PERIODIC_TIMEOUT_USEC / 4;
-
-	/* this is the threshold below which no timer arming will occur
-	   1.25 coefficient is for the threshold to be a little bigger
-	   than the real time, to compensate for timer in-accuracy */
-	bp->cmng.rs_vars.rs_threshold =
-				(RS_PERIODIC_TIMEOUT_USEC * r_param * 5) / 4;
-
-	/* resolution of fairness timer */
-	fair_periodic_timeout_usec = QM_ARB_BYTES / r_param;
-	/* for 10G it is 1000usec. for 1G it is 10000usec. */
-	t_fair = T_FAIR_COEF / bp->link_vars.line_speed;
-
-	/* this is the threshold below which we won't arm the timer anymore */
-	bp->cmng.fair_vars.fair_threshold = QM_ARB_BYTES;
-
-	/* we multiply by 1e3/8 to get bytes/msec.
-	   We don't want the credits to pass a credit
-	   of the t_fair*FAIR_MEM (algorithm resolution) */
-	bp->cmng.fair_vars.upper_bound = r_param * t_fair * FAIR_MEM;
-	/* since each tick is 4 usec */
-	bp->cmng.fair_vars.fairness_timeout = fair_periodic_timeout_usec / 4;
-}
 
 /* Calculates the sum of vn_min_rates.
    It's needed for further normalizing of the min_rates.
@@ -2273,12 +2190,12 @@ static void bnx2x_init_port_minmax(struct bnx2x *bp)
      In the later case fainess algorithm should be deactivated.
      If not all min_rates are zero then those that are zeroes will be set to 1.
  */
-static void bnx2x_calc_vn_weight_sum(struct bnx2x *bp)
+static void bnx2x_calc_vn_min(struct bnx2x *bp,
+				      struct cmng_init_input *input)
 {
 	int all_zero = 1;
 	int vn;
 
-	bp->vn_weight_sum = 0;
 	for (vn = VN_0; vn < BP_MAX_VN_NUM(bp); vn++) {
 		u32 vn_cfg = bp->mf_config[vn];
 		u32 vn_min_rate = ((vn_cfg & FUNC_MF_CFG_MIN_BW_MASK) >>
@@ -2286,106 +2203,56 @@ static void bnx2x_calc_vn_weight_sum(struct bnx2x *bp)
 
 		/* Skip hidden vns */
 		if (vn_cfg & FUNC_MF_CFG_FUNC_HIDE)
-			continue;
-
+			vn_min_rate = 0;
 		/* If min rate is zero - set it to 1 */
-		if (!vn_min_rate)
+		else if (!vn_min_rate)
 			vn_min_rate = DEF_MIN_RATE;
 		else
 			all_zero = 0;
 
-		bp->vn_weight_sum += vn_min_rate;
+		input->vnic_min_rate[vn] = vn_min_rate;
 	}
 
 	/* if ETS or all min rates are zeros - disable fairness */
 	if (BNX2X_IS_ETS_ENABLED(bp)) {
-		bp->cmng.flags.cmng_enables &=
+		input->flags.cmng_enables &=
 					~CMNG_FLAGS_PER_PORT_FAIRNESS_VN;
 		DP(NETIF_MSG_IFUP, "Fairness will be disabled due to ETS\n");
 	} else if (all_zero) {
-		bp->cmng.flags.cmng_enables &=
+		input->flags.cmng_enables &=
 					~CMNG_FLAGS_PER_PORT_FAIRNESS_VN;
-		DP(NETIF_MSG_IFUP, "All MIN values are zeroes"
-		   "  fairness will be disabled\n");
+		DP(NETIF_MSG_IFUP,
+		   "All MIN values are zeroes fairness will be disabled\n");
 	} else
-		bp->cmng.flags.cmng_enables |=
+		input->flags.cmng_enables |=
 					CMNG_FLAGS_PER_PORT_FAIRNESS_VN;
 }
 
-static void bnx2x_init_vn_minmax(struct bnx2x *bp, int vn)
+static void bnx2x_calc_vn_max(struct bnx2x *bp, int vn,
+				    struct cmng_init_input *input)
 {
-	struct rate_shaping_vars_per_vn m_rs_vn;
-	struct fairness_vars_per_vn m_fair_vn;
+	u16 vn_max_rate;
 	u32 vn_cfg = bp->mf_config[vn];
-	int func = func_by_vn(bp, vn);
-	u16 vn_min_rate, vn_max_rate;
-	int i;
 
-	/* If function is hidden - set min and max to zeroes */
-	if (vn_cfg & FUNC_MF_CFG_FUNC_HIDE) {
-		vn_min_rate = 0;
+	if (vn_cfg & FUNC_MF_CFG_FUNC_HIDE)
 		vn_max_rate = 0;
-
-	} else {
+	else {
 		u32 maxCfg = bnx2x_extract_max_cfg(bp, vn_cfg);
 
-		vn_min_rate = ((vn_cfg & FUNC_MF_CFG_MIN_BW_MASK) >>
-				FUNC_MF_CFG_MIN_BW_SHIFT) * 100;
-		/* If fairness is enabled (not all min rates are zeroes) and
-		   if current min rate is zero - set it to 1.
-		   This is a requirement of the algorithm. */
-		if (bp->vn_weight_sum && (vn_min_rate == 0))
-			vn_min_rate = DEF_MIN_RATE;
-
-		if (IS_MF_SI(bp))
+		if (IS_MF_SI(bp)) {
 			/* maxCfg in percents of linkspeed */
 			vn_max_rate = (bp->link_vars.line_speed * maxCfg) / 100;
-		else
+		} else /* SD modes */
 			/* maxCfg is absolute in 100Mb units */
 			vn_max_rate = maxCfg * 100;
 	}
 
-	DP(NETIF_MSG_IFUP,
-	   "func %d: vn_min_rate %d  vn_max_rate %d  vn_weight_sum %d\n",
-	   func, vn_min_rate, vn_max_rate, bp->vn_weight_sum);
-
-	memset(&m_rs_vn, 0, sizeof(struct rate_shaping_vars_per_vn));
-	memset(&m_fair_vn, 0, sizeof(struct fairness_vars_per_vn));
-
-	/* global vn counter - maximal Mbps for this vn */
-	m_rs_vn.vn_counter.rate = vn_max_rate;
-
-	/* quota - number of bytes transmitted in this period */
-	m_rs_vn.vn_counter.quota =
-				(vn_max_rate * RS_PERIODIC_TIMEOUT_USEC) / 8;
-
-	if (bp->vn_weight_sum) {
-		/* credit for each period of the fairness algorithm:
-		   number of bytes in T_FAIR (the vn share the port rate).
-		   vn_weight_sum should not be larger than 10000, thus
-		   T_FAIR_COEF / (8 * vn_weight_sum) will always be greater
-		   than zero */
-		m_fair_vn.vn_credit_delta =
-			max_t(u32, (vn_min_rate * (T_FAIR_COEF /
-						   (8 * bp->vn_weight_sum))),
-			      (bp->cmng.fair_vars.fair_threshold +
-							MIN_ABOVE_THRESH));
-		DP(NETIF_MSG_IFUP, "m_fair_vn.vn_credit_delta %d\n",
-		   m_fair_vn.vn_credit_delta);
-	}
-
-	/* Store it to internal memory */
-	for (i = 0; i < sizeof(struct rate_shaping_vars_per_vn)/4; i++)
-		REG_WR(bp, BAR_XSTRORM_INTMEM +
-		       XSTORM_RATE_SHAPING_PER_VN_VARS_OFFSET(func) + i * 4,
-		       ((u32 *)(&m_rs_vn))[i]);
-
-	for (i = 0; i < sizeof(struct fairness_vars_per_vn)/4; i++)
-		REG_WR(bp, BAR_XSTRORM_INTMEM +
-		       XSTORM_FAIRNESS_PER_VN_VARS_OFFSET(func) + i * 4,
-		       ((u32 *)(&m_fair_vn))[i]);
+	DP(NETIF_MSG_IFUP, "vn %d: vn_max_rate %d\n", vn, vn_max_rate);
+
+	input->vnic_max_rate[vn] = vn_max_rate;
 }
 
+
 static int bnx2x_get_cmng_fns_mode(struct bnx2x *bp)
 {
 	if (CHIP_REV_IS_SLOW(bp))
@@ -2423,38 +2290,42 @@ void bnx2x_read_mf_cfg(struct bnx2x *bp)
 		bp->mf_config[vn] =
 			MF_CFG_RD(bp, func_mf_config[func].config);
 	}
+	if (bp->mf_config[BP_VN(bp)] & FUNC_MF_CFG_FUNC_DISABLED) {
+		DP(NETIF_MSG_IFUP, "mf_cfg function disabled\n");
+		bp->flags |= MF_FUNC_DIS;
+	} else {
+		DP(NETIF_MSG_IFUP, "mf_cfg function enabled\n");
+		bp->flags &= ~MF_FUNC_DIS;
+	}
 }
 
 static void bnx2x_cmng_fns_init(struct bnx2x *bp, u8 read_cfg, u8 cmng_type)
 {
+	struct cmng_init_input input;
+	memset(&input, 0, sizeof(struct cmng_init_input));
+
+	input.port_rate = bp->link_vars.line_speed;
 
 	if (cmng_type == CMNG_FNS_MINMAX) {
 		int vn;
 
-		/* clear cmng_enables */
-		bp->cmng.flags.cmng_enables = 0;
-
 		/* read mf conf from shmem */
 		if (read_cfg)
 			bnx2x_read_mf_cfg(bp);
 
-		/* Init rate shaping and fairness contexts */
-		bnx2x_init_port_minmax(bp);
-
 		/* vn_weight_sum and enable fairness if not 0 */
-		bnx2x_calc_vn_weight_sum(bp);
+		bnx2x_calc_vn_min(bp, &input);
 
 		/* calculate and set min-max rate for each vn */
 		if (bp->port.pmf)
 			for (vn = VN_0; vn < BP_MAX_VN_NUM(bp); vn++)
-				bnx2x_init_vn_minmax(bp, vn);
+				bnx2x_calc_vn_max(bp, vn, &input);
 
 		/* always enable rate shaping and fairness */
-		bp->cmng.flags.cmng_enables |=
+		input.flags.cmng_enables |=
 					CMNG_FLAGS_PER_PORT_RATE_SHAPING_VN;
-		if (!bp->vn_weight_sum)
-			DP(NETIF_MSG_IFUP, "All MIN values are zeroes"
-				   "  fairness will be disabled\n");
+
+		bnx2x_init_cmng(&input, &bp->cmng);
 		return;
 	}
 
@@ -2463,6 +2334,35 @@ static void bnx2x_cmng_fns_init(struct bnx2x *bp, u8 read_cfg, u8 cmng_type)
 	   "rate shaping and fairness are disabled\n");
 }
 
+static void storm_memset_cmng(struct bnx2x *bp,
+			      struct cmng_init *cmng,
+			      u8 port)
+{
+	int vn;
+	size_t size = sizeof(struct cmng_struct_per_port);
+
+	u32 addr = BAR_XSTRORM_INTMEM +
+			XSTORM_CMNG_PER_PORT_VARS_OFFSET(port);
+
+	__storm_memset_struct(bp, addr, size, (u32 *)&cmng->port);
+
+	for (vn = VN_0; vn < BP_MAX_VN_NUM(bp); vn++) {
+		int func = func_by_vn(bp, vn);
+
+		addr = BAR_XSTRORM_INTMEM +
+		       XSTORM_RATE_SHAPING_PER_VN_VARS_OFFSET(func);
+		size = sizeof(struct rate_shaping_vars_per_vn);
+		__storm_memset_struct(bp, addr, size,
+				      (u32 *)&cmng->vnic.vnic_max_rate[vn]);
+
+		addr = BAR_XSTRORM_INTMEM +
+		       XSTORM_FAIRNESS_PER_VN_VARS_OFFSET(func);
+		size = sizeof(struct fairness_vars_per_vn);
+		__storm_memset_struct(bp, addr, size,
+				      (u32 *)&cmng->vnic.vnic_min_rate[vn]);
+	}
+}
+
 /* This function is called upon link interrupt */
 static void bnx2x_link_attn(struct bnx2x *bp)
 {
@@ -2535,6 +2435,190 @@ void bnx2x__link_status_update(struct bnx2x *bp)
 	bnx2x_link_report(bp);
 }
 
+static int bnx2x_afex_func_update(struct bnx2x *bp, u16 vifid,
+				  u16 vlan_val, u8 allowed_prio)
+{
+	struct bnx2x_func_state_params func_params = {0};
+	struct bnx2x_func_afex_update_params *f_update_params =
+		&func_params.params.afex_update;
+
+	func_params.f_obj = &bp->func_obj;
+	func_params.cmd = BNX2X_F_CMD_AFEX_UPDATE;
+
+	/* no need to wait for RAMROD completion, so don't
+	 * set RAMROD_COMP_WAIT flag
+	 */
+
+	f_update_params->vif_id = vifid;
+	f_update_params->afex_default_vlan = vlan_val;
+	f_update_params->allowed_priorities = allowed_prio;
+
+	/* if ramrod can not be sent, response to MCP immediately */
+	if (bnx2x_func_state_change(bp, &func_params) < 0)
+		bnx2x_fw_command(bp, DRV_MSG_CODE_AFEX_VIFSET_ACK, 0);
+
+	return 0;
+}
+
+static int bnx2x_afex_handle_vif_list_cmd(struct bnx2x *bp, u8 cmd_type,
+					  u16 vif_index, u8 func_bit_map)
+{
+	struct bnx2x_func_state_params func_params = {0};
+	struct bnx2x_func_afex_viflists_params *update_params =
+		&func_params.params.afex_viflists;
+	int rc;
+	u32 drv_msg_code;
+
+	/* validate only LIST_SET and LIST_GET are received from switch */
+	if ((cmd_type != VIF_LIST_RULE_GET) && (cmd_type != VIF_LIST_RULE_SET))
+		BNX2X_ERR("BUG! afex_handle_vif_list_cmd invalid type 0x%x\n",
+			  cmd_type);
+
+	func_params.f_obj = &bp->func_obj;
+	func_params.cmd = BNX2X_F_CMD_AFEX_VIFLISTS;
+
+	/* set parameters according to cmd_type */
+	update_params->afex_vif_list_command = cmd_type;
+	update_params->vif_list_index = cpu_to_le16(vif_index);
+	update_params->func_bit_map =
+		(cmd_type == VIF_LIST_RULE_GET) ? 0 : func_bit_map;
+	update_params->func_to_clear = 0;
+	drv_msg_code =
+		(cmd_type == VIF_LIST_RULE_GET) ?
+		DRV_MSG_CODE_AFEX_LISTGET_ACK :
+		DRV_MSG_CODE_AFEX_LISTSET_ACK;
+
+	/* if ramrod can not be sent, respond to MCP immediately for
+	 * SET and GET requests (other are not triggered from MCP)
+	 */
+	rc = bnx2x_func_state_change(bp, &func_params);
+	if (rc < 0)
+		bnx2x_fw_command(bp, drv_msg_code, 0);
+
+	return 0;
+}
+
+static void bnx2x_handle_afex_cmd(struct bnx2x *bp, u32 cmd)
+{
+	struct afex_stats afex_stats;
+	u32 func = BP_ABS_FUNC(bp);
+	u32 mf_config;
+	u16 vlan_val;
+	u32 vlan_prio;
+	u16 vif_id;
+	u8 allowed_prio;
+	u8 vlan_mode;
+	u32 addr_to_write, vifid, addrs, stats_type, i;
+
+	if (cmd & DRV_STATUS_AFEX_LISTGET_REQ) {
+		vifid = SHMEM2_RD(bp, afex_param1_to_driver[BP_FW_MB_IDX(bp)]);
+		DP(BNX2X_MSG_MCP,
+		   "afex: got MCP req LISTGET_REQ for vifid 0x%x\n", vifid);
+		bnx2x_afex_handle_vif_list_cmd(bp, VIF_LIST_RULE_GET, vifid, 0);
+	}
+
+	if (cmd & DRV_STATUS_AFEX_LISTSET_REQ) {
+		vifid = SHMEM2_RD(bp, afex_param1_to_driver[BP_FW_MB_IDX(bp)]);
+		addrs = SHMEM2_RD(bp, afex_param2_to_driver[BP_FW_MB_IDX(bp)]);
+		DP(BNX2X_MSG_MCP,
+		   "afex: got MCP req LISTSET_REQ for vifid 0x%x addrs 0x%x\n",
+		   vifid, addrs);
+		bnx2x_afex_handle_vif_list_cmd(bp, VIF_LIST_RULE_SET, vifid,
+					       addrs);
+	}
+
+	if (cmd & DRV_STATUS_AFEX_STATSGET_REQ) {
+		addr_to_write = SHMEM2_RD(bp,
+			afex_scratchpad_addr_to_write[BP_FW_MB_IDX(bp)]);
+		stats_type = SHMEM2_RD(bp,
+			afex_param1_to_driver[BP_FW_MB_IDX(bp)]);
+
+		DP(BNX2X_MSG_MCP,
+		   "afex: got MCP req STATSGET_REQ, write to addr 0x%x\n",
+		   addr_to_write);
+
+		bnx2x_afex_collect_stats(bp, (void *)&afex_stats, stats_type);
+
+		/* write response to scratchpad, for MCP */
+		for (i = 0; i < (sizeof(struct afex_stats)/sizeof(u32)); i++)
+			REG_WR(bp, addr_to_write + i*sizeof(u32),
+			       *(((u32 *)(&afex_stats))+i));
+
+		/* send ack message to MCP */
+		bnx2x_fw_command(bp, DRV_MSG_CODE_AFEX_STATSGET_ACK, 0);
+	}
+
+	if (cmd & DRV_STATUS_AFEX_VIFSET_REQ) {
+		mf_config = MF_CFG_RD(bp, func_mf_config[func].config);
+		bp->mf_config[BP_VN(bp)] = mf_config;
+		DP(BNX2X_MSG_MCP,
+		   "afex: got MCP req VIFSET_REQ, mf_config 0x%x\n",
+		   mf_config);
+
+		/* if VIF_SET is "enabled" */
+		if (!(mf_config & FUNC_MF_CFG_FUNC_DISABLED)) {
+			/* set rate limit directly to internal RAM */
+			struct cmng_init_input cmng_input;
+			struct rate_shaping_vars_per_vn m_rs_vn;
+			size_t size = sizeof(struct rate_shaping_vars_per_vn);
+			u32 addr = BAR_XSTRORM_INTMEM +
+			    XSTORM_RATE_SHAPING_PER_VN_VARS_OFFSET(BP_FUNC(bp));
+
+			bp->mf_config[BP_VN(bp)] = mf_config;
+
+			bnx2x_calc_vn_max(bp, BP_VN(bp), &cmng_input);
+			m_rs_vn.vn_counter.rate =
+				cmng_input.vnic_max_rate[BP_VN(bp)];
+			m_rs_vn.vn_counter.quota =
+				(m_rs_vn.vn_counter.rate *
+				 RS_PERIODIC_TIMEOUT_USEC) / 8;
+
+			__storm_memset_struct(bp, addr, size, (u32 *)&m_rs_vn);
+
+			/* read relevant values from mf_cfg struct in shmem */
+			vif_id =
+				(MF_CFG_RD(bp, func_mf_config[func].e1hov_tag) &
+				 FUNC_MF_CFG_E1HOV_TAG_MASK) >>
+				FUNC_MF_CFG_E1HOV_TAG_SHIFT;
+			vlan_val =
+				(MF_CFG_RD(bp, func_mf_config[func].e1hov_tag) &
+				 FUNC_MF_CFG_AFEX_VLAN_MASK) >>
+				FUNC_MF_CFG_AFEX_VLAN_SHIFT;
+			vlan_prio = (mf_config &
+				     FUNC_MF_CFG_TRANSMIT_PRIORITY_MASK) >>
+				    FUNC_MF_CFG_TRANSMIT_PRIORITY_SHIFT;
+			vlan_val |= (vlan_prio << VLAN_PRIO_SHIFT);
+			vlan_mode =
+				(MF_CFG_RD(bp,
+					   func_mf_config[func].afex_config) &
+				 FUNC_MF_CFG_AFEX_VLAN_MODE_MASK) >>
+				FUNC_MF_CFG_AFEX_VLAN_MODE_SHIFT;
+			allowed_prio =
+				(MF_CFG_RD(bp,
+					   func_mf_config[func].afex_config) &
+				 FUNC_MF_CFG_AFEX_COS_FILTER_MASK) >>
+				FUNC_MF_CFG_AFEX_COS_FILTER_SHIFT;
+
+			/* send ramrod to FW, return in case of failure */
+			if (bnx2x_afex_func_update(bp, vif_id, vlan_val,
+						   allowed_prio))
+				return;
+
+			bp->afex_def_vlan_tag = vlan_val;
+			bp->afex_vlan_mode = vlan_mode;
+		} else {
+			/* notify link down because BP->flags is disabled */
+			bnx2x_link_report(bp);
+
+			/* send INVALID VIF ramrod to FW */
+			bnx2x_afex_func_update(bp, 0xFFFF, 0, 0);
+
+			/* Reset the default afex VLAN */
+			bp->afex_def_vlan_tag = -1;
+		}
+	}
+}
+
 static void bnx2x_pmf_update(struct bnx2x *bp)
 {
 	int port = BP_PORT(bp);
@@ -2619,6 +2703,18 @@ u32 bnx2x_fw_command(struct bnx2x *bp, u32 command, u32 param)
 }
 
 
+static void storm_memset_func_cfg(struct bnx2x *bp,
+				 struct tstorm_eth_function_common_config *tcfg,
+				 u16 abs_fid)
+{
+	size_t size = sizeof(struct tstorm_eth_function_common_config);
+
+	u32 addr = BAR_TSTRORM_INTMEM +
+			TSTORM_FUNCTION_COMMON_CONFIG_OFFSET(abs_fid);
+
+	__storm_memset_struct(bp, addr, size, (u32 *)tcfg);
+}
+
 void bnx2x_func_init(struct bnx2x *bp, struct bnx2x_func_init_params *p)
 {
 	if (CHIP_IS_E1x(bp)) {
@@ -2648,9 +2744,9 @@ void bnx2x_func_init(struct bnx2x *bp, struct bnx2x_func_init_params *p)
  *
  * Return the flags that are common for the Tx-only and not normal connections.
  */
-static inline unsigned long bnx2x_get_common_flags(struct bnx2x *bp,
-						   struct bnx2x_fastpath *fp,
-						   bool zero_stats)
+static unsigned long bnx2x_get_common_flags(struct bnx2x *bp,
+					    struct bnx2x_fastpath *fp,
+					    bool zero_stats)
 {
 	unsigned long flags = 0;
 
@@ -2670,9 +2766,9 @@ static inline unsigned long bnx2x_get_common_flags(struct bnx2x *bp,
 	return flags;
 }
 
-static inline unsigned long bnx2x_get_q_flags(struct bnx2x *bp,
-					      struct bnx2x_fastpath *fp,
-					      bool leading)
+static unsigned long bnx2x_get_q_flags(struct bnx2x *bp,
+				       struct bnx2x_fastpath *fp,
+				       bool leading)
 {
 	unsigned long flags = 0;
 
@@ -2680,8 +2776,11 @@ static inline unsigned long bnx2x_get_q_flags(struct bnx2x *bp,
 	if (IS_MF_SD(bp))
 		__set_bit(BNX2X_Q_FLG_OV, &flags);
 
-	if (IS_FCOE_FP(fp))
+	if (IS_FCOE_FP(fp)) {
 		__set_bit(BNX2X_Q_FLG_FCOE, &flags);
+		/* For FCoE - force usage of default priority (for afex) */
+		__set_bit(BNX2X_Q_FLG_FORCE_DEFAULT_PRI, &flags);
+	}
 
 	if (!fp->disable_tpa) {
 		__set_bit(BNX2X_Q_FLG_TPA, &flags);
@@ -2698,6 +2797,10 @@ static inline unsigned long bnx2x_get_q_flags(struct bnx2x *bp,
 	/* Always set HW VLAN stripping */
 	__set_bit(BNX2X_Q_FLG_VLAN, &flags);
 
+	/* configure silent vlan removal */
+	if (IS_MF_AFEX(bp))
+		__set_bit(BNX2X_Q_FLG_SILENT_VLAN_REM, &flags);
+
 
 	return flags | bnx2x_get_common_flags(bp, fp, true);
 }
@@ -2800,6 +2903,13 @@ static void bnx2x_pf_rx_q_prep(struct bnx2x *bp,
 		rxq_init->sb_cq_index = HC_SP_INDEX_ETH_FCOE_RX_CQ_CONS;
 	else
 		rxq_init->sb_cq_index = HC_INDEX_ETH_RX_CQ_CONS;
+	/* configure silent vlan removal
+	 * if multi function mode is afex, then mask default vlan
+	 */
+	if (IS_MF_AFEX(bp)) {
+		rxq_init->silent_removal_value = bp->afex_def_vlan_tag;
+		rxq_init->silent_removal_mask = VLAN_VID_MASK;
+	}
 }
 
 static void bnx2x_pf_tx_q_prep(struct bnx2x *bp,
@@ -3051,7 +3161,7 @@ static void bnx2x_drv_info_iscsi_stat(struct bnx2x *bp)
  *	configure FW
  *	notify others function about the change
  */
-static inline void bnx2x_config_mf_bw(struct bnx2x *bp)
+static void bnx2x_config_mf_bw(struct bnx2x *bp)
 {
 	if (bp->link_vars.link_up) {
 		bnx2x_cmng_fns_init(bp, true, CMNG_FNS_MINMAX);
@@ -3060,7 +3170,7 @@ static inline void bnx2x_config_mf_bw(struct bnx2x *bp)
 	storm_memset_cmng(bp, &bp->cmng, BP_PORT(bp));
 }
 
-static inline void bnx2x_set_mf_bw(struct bnx2x *bp)
+static void bnx2x_set_mf_bw(struct bnx2x *bp)
 {
 	bnx2x_config_mf_bw(bp);
 	bnx2x_fw_command(bp, DRV_MSG_CODE_SET_MF_BW_ACK, 0);
@@ -3147,7 +3257,7 @@ static void bnx2x_dcc_event(struct bnx2x *bp, u32 dcc_event)
 }
 
 /* must be called under the spq lock */
-static inline struct eth_spe *bnx2x_sp_get_next(struct bnx2x *bp)
+static struct eth_spe *bnx2x_sp_get_next(struct bnx2x *bp)
 {
 	struct eth_spe *next_spe = bp->spq_prod_bd;
 
@@ -3163,7 +3273,7 @@ static inline struct eth_spe *bnx2x_sp_get_next(struct bnx2x *bp)
 }
 
 /* must be called under the spq lock */
-static inline void bnx2x_sp_prod_update(struct bnx2x *bp)
+static void bnx2x_sp_prod_update(struct bnx2x *bp)
 {
 	int func = BP_FUNC(bp);
 
@@ -3185,7 +3295,7 @@ static inline void bnx2x_sp_prod_update(struct bnx2x *bp)
  * @cmd:	command to check
  * @cmd_type:	command type
  */
-static inline bool bnx2x_is_contextless_ramrod(int cmd, int cmd_type)
+static bool bnx2x_is_contextless_ramrod(int cmd, int cmd_type)
 {
 	if ((cmd_type == NONE_CONNECTION_TYPE) ||
 	    (cmd == RAMROD_CMD_ID_ETH_FORWARD_SETUP) ||
@@ -3319,7 +3429,7 @@ static void bnx2x_release_alr(struct bnx2x *bp)
 #define BNX2X_DEF_SB_ATT_IDX	0x0001
 #define BNX2X_DEF_SB_IDX	0x0002
 
-static inline u16 bnx2x_update_dsb_idx(struct bnx2x *bp)
+static u16 bnx2x_update_dsb_idx(struct bnx2x *bp)
 {
 	struct host_sp_status_block *def_sb = bp->def_status_blk;
 	u16 rc = 0;
@@ -3451,7 +3561,7 @@ static void bnx2x_attn_int_asserted(struct bnx2x *bp, u32 asserted)
 	}
 }
 
-static inline void bnx2x_fan_failure(struct bnx2x *bp)
+static void bnx2x_fan_failure(struct bnx2x *bp)
 {
 	int port = BP_PORT(bp);
 	u32 ext_phy_config;
@@ -3481,7 +3591,7 @@ static inline void bnx2x_fan_failure(struct bnx2x *bp)
 
 }
 
-static inline void bnx2x_attn_int_deasserted0(struct bnx2x *bp, u32 attn)
+static void bnx2x_attn_int_deasserted0(struct bnx2x *bp, u32 attn)
 {
 	int port = BP_PORT(bp);
 	int reg_offset;
@@ -3521,7 +3631,7 @@ static inline void bnx2x_attn_int_deasserted0(struct bnx2x *bp, u32 attn)
 	}
 }
 
-static inline void bnx2x_attn_int_deasserted1(struct bnx2x *bp, u32 attn)
+static void bnx2x_attn_int_deasserted1(struct bnx2x *bp, u32 attn)
 {
 	u32 val;
 
@@ -3552,7 +3662,7 @@ static inline void bnx2x_attn_int_deasserted1(struct bnx2x *bp, u32 attn)
 	}
 }
 
-static inline void bnx2x_attn_int_deasserted2(struct bnx2x *bp, u32 attn)
+static void bnx2x_attn_int_deasserted2(struct bnx2x *bp, u32 attn)
 {
 	u32 val;
 
@@ -3596,7 +3706,7 @@ static inline void bnx2x_attn_int_deasserted2(struct bnx2x *bp, u32 attn)
 	}
 }
 
-static inline void bnx2x_attn_int_deasserted3(struct bnx2x *bp, u32 attn)
+static void bnx2x_attn_int_deasserted3(struct bnx2x *bp, u32 attn)
 {
 	u32 val;
 
@@ -3606,6 +3716,7 @@ static inline void bnx2x_attn_int_deasserted3(struct bnx2x *bp, u32 attn)
 			int func = BP_FUNC(bp);
 
 			REG_WR(bp, MISC_REG_AEU_GENERAL_ATTN_12 + func*4, 0);
+			bnx2x_read_mf_cfg(bp);
 			bp->mf_config[BP_VN(bp)] = MF_CFG_RD(bp,
 					func_mf_config[BP_ABS_FUNC(bp)].config);
 			val = SHMEM_RD(bp,
@@ -3628,6 +3739,9 @@ static inline void bnx2x_attn_int_deasserted3(struct bnx2x *bp, u32 attn)
 				/* start dcbx state machine */
 				bnx2x_dcbx_set_params(bp,
 					BNX2X_DCBX_STATE_NEG_RECEIVED);
+			if (val & DRV_STATUS_AFEX_EVENT_MASK)
+				bnx2x_handle_afex_cmd(bp,
+					val & DRV_STATUS_AFEX_EVENT_MASK);
 			if (bp->link_vars.periodic_flags &
 			    PERIODIC_FLAGS_LINK_EVENT) {
 				/*  sync with link */
@@ -3722,7 +3836,7 @@ void bnx2x_set_reset_global(struct bnx2x *bp)
  *
  * Should be run under rtnl lock
  */
-static inline void bnx2x_clear_reset_global(struct bnx2x *bp)
+static void bnx2x_clear_reset_global(struct bnx2x *bp)
 {
 	u32 val;
 	bnx2x_acquire_hw_lock(bp, HW_LOCK_RESOURCE_RECOVERY_REG);
@@ -3736,7 +3850,7 @@ static inline void bnx2x_clear_reset_global(struct bnx2x *bp)
  *
  * should be run under rtnl lock
  */
-static inline bool bnx2x_reset_is_global(struct bnx2x *bp)
+static bool bnx2x_reset_is_global(struct bnx2x *bp)
 {
 	u32 val	= REG_RD(bp, BNX2X_RECOVERY_GLOB_REG);
 
@@ -3749,7 +3863,7 @@ static inline bool bnx2x_reset_is_global(struct bnx2x *bp)
  *
  * Should be run under rtnl lock
  */
-static inline void bnx2x_set_reset_done(struct bnx2x *bp)
+static void bnx2x_set_reset_done(struct bnx2x *bp)
 {
 	u32 val;
 	u32 bit = BP_PATH(bp) ?
@@ -3874,7 +3988,7 @@ bool bnx2x_clear_pf_load(struct bnx2x *bp)
  *
  * should be run under rtnl lock
  */
-static inline bool bnx2x_get_load_status(struct bnx2x *bp, int engine)
+static bool bnx2x_get_load_status(struct bnx2x *bp, int engine)
 {
 	u32 mask = (engine ? BNX2X_PATH1_LOAD_CNT_MASK :
 			     BNX2X_PATH0_LOAD_CNT_MASK);
@@ -3895,7 +4009,7 @@ static inline bool bnx2x_get_load_status(struct bnx2x *bp, int engine)
 /*
  * Reset the load status for the current engine.
  */
-static inline void bnx2x_clear_load_status(struct bnx2x *bp)
+static void bnx2x_clear_load_status(struct bnx2x *bp)
 {
 	u32 val;
 	u32 mask = (BP_PATH(bp) ? BNX2X_PATH1_LOAD_CNT_MASK :
@@ -3906,13 +4020,13 @@ static inline void bnx2x_clear_load_status(struct bnx2x *bp)
 	bnx2x_release_hw_lock(bp, HW_LOCK_RESOURCE_RECOVERY_REG);
 }
 
-static inline void _print_next_block(int idx, const char *blk)
+static void _print_next_block(int idx, const char *blk)
 {
 	pr_cont("%s%s", idx ? ", " : "", blk);
 }
 
-static inline int bnx2x_check_blocks_with_parity0(u32 sig, int par_num,
-						  bool print)
+static int bnx2x_check_blocks_with_parity0(u32 sig, int par_num,
+					   bool print)
 {
 	int i = 0;
 	u32 cur_bit = 0;
@@ -3959,8 +4073,8 @@ static inline int bnx2x_check_blocks_with_parity0(u32 sig, int par_num,
 	return par_num;
 }
 
-static inline int bnx2x_check_blocks_with_parity1(u32 sig, int par_num,
-						  bool *global, bool print)
+static int bnx2x_check_blocks_with_parity1(u32 sig, int par_num,
+					   bool *global, bool print)
 {
 	int i = 0;
 	u32 cur_bit = 0;
@@ -4045,8 +4159,8 @@ static inline int bnx2x_check_blocks_with_parity1(u32 sig, int par_num,
 	return par_num;
 }
 
-static inline int bnx2x_check_blocks_with_parity2(u32 sig, int par_num,
-						  bool print)
+static int bnx2x_check_blocks_with_parity2(u32 sig, int par_num,
+					   bool print)
 {
 	int i = 0;
 	u32 cur_bit = 0;
@@ -4097,8 +4211,8 @@ static inline int bnx2x_check_blocks_with_parity2(u32 sig, int par_num,
 	return par_num;
 }
 
-static inline int bnx2x_check_blocks_with_parity3(u32 sig, int par_num,
-						  bool *global, bool print)
+static int bnx2x_check_blocks_with_parity3(u32 sig, int par_num,
+					   bool *global, bool print)
 {
 	int i = 0;
 	u32 cur_bit = 0;
@@ -4139,8 +4253,8 @@ static inline int bnx2x_check_blocks_with_parity3(u32 sig, int par_num,
 	return par_num;
 }
 
-static inline int bnx2x_check_blocks_with_parity4(u32 sig, int par_num,
-						  bool print)
+static int bnx2x_check_blocks_with_parity4(u32 sig, int par_num,
+					   bool print)
 {
 	int i = 0;
 	u32 cur_bit = 0;
@@ -4166,8 +4280,8 @@ static inline int bnx2x_check_blocks_with_parity4(u32 sig, int par_num,
 	return par_num;
 }
 
-static inline bool bnx2x_parity_attn(struct bnx2x *bp, bool *global, bool print,
-				     u32 *sig)
+static bool bnx2x_parity_attn(struct bnx2x *bp, bool *global, bool print,
+			      u32 *sig)
 {
 	if ((sig[0] & HW_PRTY_ASSERT_SET_0) ||
 	    (sig[1] & HW_PRTY_ASSERT_SET_1) ||
@@ -4238,7 +4352,7 @@ bool bnx2x_chk_parity_attn(struct bnx2x *bp, bool *global, bool print)
 }
 
 
-static inline void bnx2x_attn_int_deasserted4(struct bnx2x *bp, u32 attn)
+static void bnx2x_attn_int_deasserted4(struct bnx2x *bp, u32 attn)
 {
 	u32 val;
 	if (attn & AEU_INPUTS_ATTN_BITS_PGLUE_HW_INTERRUPT) {
@@ -4430,7 +4544,7 @@ void bnx2x_igu_ack_sb(struct bnx2x *bp, u8 igu_sb_id, u8 segment,
 			     igu_addr);
 }
 
-static inline void bnx2x_update_eq_prod(struct bnx2x *bp, u16 prod)
+static void bnx2x_update_eq_prod(struct bnx2x *bp, u16 prod)
 {
 	/* No memory barriers */
 	storm_memset_eq_prod(bp, prod, BP_FUNC(bp));
@@ -4461,7 +4575,7 @@ static int  bnx2x_cnic_handle_cfc_del(struct bnx2x *bp, u32 cid,
 }
 #endif
 
-static inline void bnx2x_handle_mcast_eqe(struct bnx2x *bp)
+static void bnx2x_handle_mcast_eqe(struct bnx2x *bp)
 {
 	struct bnx2x_mcast_ramrod_params rparam;
 	int rc;
@@ -4486,8 +4600,8 @@ static inline void bnx2x_handle_mcast_eqe(struct bnx2x *bp)
 	netif_addr_unlock_bh(bp->dev);
 }
 
-static inline void bnx2x_handle_classification_eqe(struct bnx2x *bp,
-						   union event_ring_elem *elem)
+static void bnx2x_handle_classification_eqe(struct bnx2x *bp,
+					    union event_ring_elem *elem)
 {
 	unsigned long ramrod_flags = 0;
 	int rc = 0;
@@ -4534,7 +4648,7 @@ static inline void bnx2x_handle_classification_eqe(struct bnx2x *bp,
 static void bnx2x_set_iscsi_eth_rx_mode(struct bnx2x *bp, bool start);
 #endif
 
-static inline void bnx2x_handle_rx_mode_eqe(struct bnx2x *bp)
+static void bnx2x_handle_rx_mode_eqe(struct bnx2x *bp)
 {
 	netif_addr_lock_bh(bp->dev);
 
@@ -4555,7 +4669,94 @@ static inline void bnx2x_handle_rx_mode_eqe(struct bnx2x *bp)
 	netif_addr_unlock_bh(bp->dev);
 }
 
-static inline struct bnx2x_queue_sp_obj *bnx2x_cid_to_q_obj(
+static void bnx2x_after_afex_vif_lists(struct bnx2x *bp,
+					      union event_ring_elem *elem)
+{
+	if (elem->message.data.vif_list_event.echo == VIF_LIST_RULE_GET) {
+		DP(BNX2X_MSG_SP,
+		   "afex: ramrod completed VIF LIST_GET, addrs 0x%x\n",
+		   elem->message.data.vif_list_event.func_bit_map);
+		bnx2x_fw_command(bp, DRV_MSG_CODE_AFEX_LISTGET_ACK,
+			elem->message.data.vif_list_event.func_bit_map);
+	} else if (elem->message.data.vif_list_event.echo ==
+		   VIF_LIST_RULE_SET) {
+		DP(BNX2X_MSG_SP, "afex: ramrod completed VIF LIST_SET\n");
+		bnx2x_fw_command(bp, DRV_MSG_CODE_AFEX_LISTSET_ACK, 0);
+	}
+}
+
+/* called with rtnl_lock */
+static void bnx2x_after_function_update(struct bnx2x *bp)
+{
+	int q, rc;
+	struct bnx2x_fastpath *fp;
+	struct bnx2x_queue_state_params queue_params = {NULL};
+	struct bnx2x_queue_update_params *q_update_params =
+		&queue_params.params.update;
+
+	/* Send Q update command with afex vlan removal values	for all Qs */
+	queue_params.cmd = BNX2X_Q_CMD_UPDATE;
+
+	/* set silent vlan removal values according to vlan mode */
+	__set_bit(BNX2X_Q_UPDATE_SILENT_VLAN_REM_CHNG,
+		  &q_update_params->update_flags);
+	__set_bit(BNX2X_Q_UPDATE_SILENT_VLAN_REM,
+		  &q_update_params->update_flags);
+	__set_bit(RAMROD_COMP_WAIT, &queue_params.ramrod_flags);
+
+	/* in access mode mark mask and value are 0 to strip all vlans */
+	if (bp->afex_vlan_mode == FUNC_MF_CFG_AFEX_VLAN_ACCESS_MODE) {
+		q_update_params->silent_removal_value = 0;
+		q_update_params->silent_removal_mask = 0;
+	} else {
+		q_update_params->silent_removal_value =
+			(bp->afex_def_vlan_tag & VLAN_VID_MASK);
+		q_update_params->silent_removal_mask = VLAN_VID_MASK;
+	}
+
+	for_each_eth_queue(bp, q) {
+		/* Set the appropriate Queue object */
+		fp = &bp->fp[q];
+		queue_params.q_obj = &fp->q_obj;
+
+		/* send the ramrod */
+		rc = bnx2x_queue_state_change(bp, &queue_params);
+		if (rc < 0)
+			BNX2X_ERR("Failed to config silent vlan rem for Q %d\n",
+				  q);
+	}
+
+#ifdef BCM_CNIC
+	if (!NO_FCOE(bp)) {
+		fp = &bp->fp[FCOE_IDX];
+		queue_params.q_obj = &fp->q_obj;
+
+		/* clear pending completion bit */
+		__clear_bit(RAMROD_COMP_WAIT, &queue_params.ramrod_flags);
+
+		/* mark latest Q bit */
+		smp_mb__before_clear_bit();
+		set_bit(BNX2X_AFEX_FCOE_Q_UPDATE_PENDING, &bp->sp_state);
+		smp_mb__after_clear_bit();
+
+		/* send Q update ramrod for FCoE Q */
+		rc = bnx2x_queue_state_change(bp, &queue_params);
+		if (rc < 0)
+			BNX2X_ERR("Failed to config silent vlan rem for Q %d\n",
+				  q);
+	} else {
+		/* If no FCoE ring - ACK MCP now */
+		bnx2x_link_report(bp);
+		bnx2x_fw_command(bp, DRV_MSG_CODE_AFEX_VIFSET_ACK, 0);
+	}
+#else
+	/* If no FCoE ring - ACK MCP now */
+	bnx2x_link_report(bp);
+	bnx2x_fw_command(bp, DRV_MSG_CODE_AFEX_VIFSET_ACK, 0);
+#endif /* BCM_CNIC */
+}
+
+static struct bnx2x_queue_sp_obj *bnx2x_cid_to_q_obj(
 	struct bnx2x *bp, u32 cid)
 {
 	DP(BNX2X_MSG_SP, "retrieving fp from cid %d\n", cid);
@@ -4653,6 +4854,28 @@ static void bnx2x_eq_int(struct bnx2x *bp)
 				break;
 			bnx2x_dcbx_set_params(bp, BNX2X_DCBX_STATE_TX_RELEASED);
 			goto next_spqe;
+		case EVENT_RING_OPCODE_FUNCTION_UPDATE:
+			DP(BNX2X_MSG_SP | BNX2X_MSG_MCP,
+			   "AFEX: ramrod completed FUNCTION_UPDATE\n");
+			f_obj->complete_cmd(bp, f_obj, BNX2X_F_CMD_AFEX_UPDATE);
+
+			/* We will perform the Queues update from sp_rtnl task
+			 * as all Queue SP operations should run under
+			 * rtnl_lock.
+			 */
+			smp_mb__before_clear_bit();
+			set_bit(BNX2X_SP_RTNL_AFEX_F_UPDATE,
+				&bp->sp_rtnl_state);
+			smp_mb__after_clear_bit();
+
+			schedule_delayed_work(&bp->sp_rtnl_task, 0);
+			goto next_spqe;
+
+		case EVENT_RING_OPCODE_AFEX_VIF_LISTS:
+			f_obj->complete_cmd(bp, f_obj,
+					    BNX2X_F_CMD_AFEX_VIFLISTS);
+			bnx2x_after_afex_vif_lists(bp, elem);
+			goto next_spqe;
 		case EVENT_RING_OPCODE_FUNCTION_START:
 			DP(BNX2X_MSG_SP | NETIF_MSG_IFUP,
 			   "got FUNC_START ramrod\n");
@@ -4784,6 +5007,13 @@ static void bnx2x_sp_task(struct work_struct *work)
 
 	bnx2x_ack_sb(bp, bp->igu_dsb_id, ATTENTION_ID,
 	     le16_to_cpu(bp->def_att_idx), IGU_INT_ENABLE, 1);
+
+	/* afex - poll to check if VIFSET_ACK should be sent to MFW */
+	if (test_and_clear_bit(BNX2X_AFEX_PENDING_VIFSET_MCP_ACK,
+			       &bp->sp_state)) {
+		bnx2x_link_report(bp);
+		bnx2x_fw_command(bp, DRV_MSG_CODE_AFEX_VIFSET_ACK, 0);
+	}
 }
 
 irqreturn_t bnx2x_msix_sp_int(int irq, void *dev_instance)
@@ -4870,7 +5100,7 @@ static void bnx2x_timer(unsigned long data)
  * nic init service functions
  */
 
-static inline void bnx2x_fill(struct bnx2x *bp, u32 addr, int fill, u32 len)
+static void bnx2x_fill(struct bnx2x *bp, u32 addr, int fill, u32 len)
 {
 	u32 i;
 	if (!(len%4) && !(addr%4))
@@ -4883,10 +5113,10 @@ static inline void bnx2x_fill(struct bnx2x *bp, u32 addr, int fill, u32 len)
 }
 
 /* helper: writes FP SP data to FW - data_size in dwords */
-static inline void bnx2x_wr_fp_sb_data(struct bnx2x *bp,
-				       int fw_sb_id,
-				       u32 *sb_data_p,
-				       u32 data_size)
+static void bnx2x_wr_fp_sb_data(struct bnx2x *bp,
+				int fw_sb_id,
+				u32 *sb_data_p,
+				u32 data_size)
 {
 	int index;
 	for (index = 0; index < data_size; index++)
@@ -4896,7 +5126,7 @@ static inline void bnx2x_wr_fp_sb_data(struct bnx2x *bp,
 			*(sb_data_p + index));
 }
 
-static inline void bnx2x_zero_fp_sb(struct bnx2x *bp, int fw_sb_id)
+static void bnx2x_zero_fp_sb(struct bnx2x *bp, int fw_sb_id)
 {
 	u32 *sb_data_p;
 	u32 data_size = 0;
@@ -4929,7 +5159,7 @@ static inline void bnx2x_zero_fp_sb(struct bnx2x *bp, int fw_sb_id)
 }
 
 /* helper:  writes SP SB data to FW */
-static inline void bnx2x_wr_sp_sb_data(struct bnx2x *bp,
+static void bnx2x_wr_sp_sb_data(struct bnx2x *bp,
 		struct hc_sp_status_block_data *sp_sb_data)
 {
 	int func = BP_FUNC(bp);
@@ -4941,7 +5171,7 @@ static inline void bnx2x_wr_sp_sb_data(struct bnx2x *bp,
 			*((u32 *)sp_sb_data + i));
 }
 
-static inline void bnx2x_zero_sp_sb(struct bnx2x *bp)
+static void bnx2x_zero_sp_sb(struct bnx2x *bp)
 {
 	int func = BP_FUNC(bp);
 	struct hc_sp_status_block_data sp_sb_data;
@@ -4962,8 +5192,7 @@ static inline void bnx2x_zero_sp_sb(struct bnx2x *bp)
 }
 
 
-static inline
-void bnx2x_setup_ndsb_state_machine(struct hc_status_block_sm *hc_sm,
+static void bnx2x_setup_ndsb_state_machine(struct hc_status_block_sm *hc_sm,
 					   int igu_sb_id, int igu_seg_id)
 {
 	hc_sm->igu_sb_id = igu_sb_id;
@@ -4974,8 +5203,7 @@ void bnx2x_setup_ndsb_state_machine(struct hc_status_block_sm *hc_sm,
 
 
 /* allocates state machine ids. */
-static inline
-void bnx2x_map_sb_state_machines(struct hc_index_data *index_data)
+static void bnx2x_map_sb_state_machines(struct hc_index_data *index_data)
 {
 	/* zero out state machine indices */
 	/* rx indices */
@@ -5383,7 +5611,7 @@ static inline u8 bnx2x_fp_fw_sb_id(struct bnx2x_fastpath *fp)
 	return fp->bp->base_fw_ndsb + fp->index + CNIC_PRESENT;
 }
 
-static inline u8 bnx2x_fp_cl_id(struct bnx2x_fastpath *fp)
+static u8 bnx2x_fp_cl_id(struct bnx2x_fastpath *fp)
 {
 	if (CHIP_IS_E1x(fp->bp))
 		return BP_L_ID(fp->bp) + fp->index;
@@ -5444,6 +5672,43 @@ static void bnx2x_init_eth_fp(struct bnx2x *bp, int fp_idx)
 	bnx2x_update_fpsb_idx(fp);
 }
 
+static void bnx2x_init_tx_ring_one(struct bnx2x_fp_txdata *txdata)
+{
+	int i;
+
+	for (i = 1; i <= NUM_TX_RINGS; i++) {
+		struct eth_tx_next_bd *tx_next_bd =
+			&txdata->tx_desc_ring[TX_DESC_CNT * i - 1].next_bd;
+
+		tx_next_bd->addr_hi =
+			cpu_to_le32(U64_HI(txdata->tx_desc_mapping +
+				    BCM_PAGE_SIZE*(i % NUM_TX_RINGS)));
+		tx_next_bd->addr_lo =
+			cpu_to_le32(U64_LO(txdata->tx_desc_mapping +
+				    BCM_PAGE_SIZE*(i % NUM_TX_RINGS)));
+	}
+
+	SET_FLAG(txdata->tx_db.data.header.header, DOORBELL_HDR_DB_TYPE, 1);
+	txdata->tx_db.data.zero_fill1 = 0;
+	txdata->tx_db.data.prod = 0;
+
+	txdata->tx_pkt_prod = 0;
+	txdata->tx_pkt_cons = 0;
+	txdata->tx_bd_prod = 0;
+	txdata->tx_bd_cons = 0;
+	txdata->tx_pkt = 0;
+}
+
+static void bnx2x_init_tx_rings(struct bnx2x *bp)
+{
+	int i;
+	u8 cos;
+
+	for_each_tx_queue(bp, i)
+		for_each_cos_in_tx_queue(&bp->fp[i], cos)
+			bnx2x_init_tx_ring_one(&bp->fp[i].txdata[cos]);
+}
+
 void bnx2x_nic_init(struct bnx2x *bp, u32 load_code)
 {
 	int i;
@@ -5968,7 +6233,7 @@ void bnx2x_pf_disable(struct bnx2x *bp)
 	REG_WR(bp, CFC_REG_WEAK_ENABLE_PF, 0);
 }
 
-static inline void bnx2x__common_init_phy(struct bnx2x *bp)
+static void bnx2x__common_init_phy(struct bnx2x *bp)
 {
 	u32 shmem_base[2], shmem2_base[2];
 	shmem_base[0] =  bp->common.shmem_base;
@@ -6255,12 +6520,24 @@ static int bnx2x_init_hw_common(struct bnx2x *bp)
 	if (!CHIP_IS_E1(bp))
 		REG_WR(bp, PRS_REG_E1HOV_MODE, bp->path_has_ovlan);
 
-	if (!CHIP_IS_E1x(bp) && !CHIP_IS_E3B0(bp))
-		/* Bit-map indicating which L2 hdrs may appear
-		 * after the basic Ethernet header
-		 */
-		REG_WR(bp, PRS_REG_HDRS_AFTER_BASIC,
-		       bp->path_has_ovlan ? 7 : 6);
+	if (!CHIP_IS_E1x(bp) && !CHIP_IS_E3B0(bp)) {
+		if (IS_MF_AFEX(bp)) {
+			/* configure that VNTag and VLAN headers must be
+			 * received in afex mode
+			 */
+			REG_WR(bp, PRS_REG_HDRS_AFTER_BASIC, 0xE);
+			REG_WR(bp, PRS_REG_MUST_HAVE_HDRS, 0xA);
+			REG_WR(bp, PRS_REG_HDRS_AFTER_TAG_0, 0x6);
+			REG_WR(bp, PRS_REG_TAG_ETHERTYPE_0, 0x8926);
+			REG_WR(bp, PRS_REG_TAG_LEN_0, 0x4);
+		} else {
+			/* Bit-map indicating which L2 hdrs may appear
+			 * after the basic Ethernet header
+			 */
+			REG_WR(bp, PRS_REG_HDRS_AFTER_BASIC,
+			       bp->path_has_ovlan ? 7 : 6);
+		}
+	}
 
 	bnx2x_init_block(bp, BLOCK_TSDM, PHASE_COMMON);
 	bnx2x_init_block(bp, BLOCK_CSDM, PHASE_COMMON);
@@ -6294,9 +6571,21 @@ static int bnx2x_init_hw_common(struct bnx2x *bp)
 	bnx2x_init_block(bp, BLOCK_XPB, PHASE_COMMON);
 	bnx2x_init_block(bp, BLOCK_PBF, PHASE_COMMON);
 
-	if (!CHIP_IS_E1x(bp))
-		REG_WR(bp, PBF_REG_HDRS_AFTER_BASIC,
-		       bp->path_has_ovlan ? 7 : 6);
+	if (!CHIP_IS_E1x(bp)) {
+		if (IS_MF_AFEX(bp)) {
+			/* configure that VNTag and VLAN headers must be
+			 * sent in afex mode
+			 */
+			REG_WR(bp, PBF_REG_HDRS_AFTER_BASIC, 0xE);
+			REG_WR(bp, PBF_REG_MUST_HAVE_HDRS, 0xA);
+			REG_WR(bp, PBF_REG_HDRS_AFTER_TAG_0, 0x6);
+			REG_WR(bp, PBF_REG_TAG_ETHERTYPE_0, 0x8926);
+			REG_WR(bp, PBF_REG_TAG_LEN_0, 0x4);
+		} else {
+			REG_WR(bp, PBF_REG_HDRS_AFTER_BASIC,
+			       bp->path_has_ovlan ? 7 : 6);
+		}
+	}
 
 	REG_WR(bp, SRC_REG_SOFT_RST, 1);
 
@@ -6514,15 +6803,29 @@ static int bnx2x_init_hw_port(struct bnx2x *bp)
 
 
 	bnx2x_init_block(bp, BLOCK_PRS, init_phase);
-	if (CHIP_IS_E3B0(bp))
-		/* Ovlan exists only if we are in multi-function +
-		 * switch-dependent mode, in switch-independent there
-		 * is no ovlan headers
-		 */
-		REG_WR(bp, BP_PORT(bp) ?
-		       PRS_REG_HDRS_AFTER_BASIC_PORT_1 :
-		       PRS_REG_HDRS_AFTER_BASIC_PORT_0,
-		       (bp->path_has_ovlan ? 7 : 6));
+	if (CHIP_IS_E3B0(bp)) {
+		if (IS_MF_AFEX(bp)) {
+			/* configure headers for AFEX mode */
+			REG_WR(bp, BP_PORT(bp) ?
+			       PRS_REG_HDRS_AFTER_BASIC_PORT_1 :
+			       PRS_REG_HDRS_AFTER_BASIC_PORT_0, 0xE);
+			REG_WR(bp, BP_PORT(bp) ?
+			       PRS_REG_HDRS_AFTER_TAG_0_PORT_1 :
+			       PRS_REG_HDRS_AFTER_TAG_0_PORT_0, 0x6);
+			REG_WR(bp, BP_PORT(bp) ?
+			       PRS_REG_MUST_HAVE_HDRS_PORT_1 :
+			       PRS_REG_MUST_HAVE_HDRS_PORT_0, 0xA);
+		} else {
+			/* Ovlan exists only if we are in multi-function +
+			 * switch-dependent mode, in switch-independent there
+			 * is no ovlan headers
+			 */
+			REG_WR(bp, BP_PORT(bp) ?
+			       PRS_REG_HDRS_AFTER_BASIC_PORT_1 :
+			       PRS_REG_HDRS_AFTER_BASIC_PORT_0,
+			       (bp->path_has_ovlan ? 7 : 6));
+		}
+	}
 
 	bnx2x_init_block(bp, BLOCK_TSDM, init_phase);
 	bnx2x_init_block(bp, BLOCK_CSDM, init_phase);
@@ -6584,10 +6887,15 @@ static int bnx2x_init_hw_port(struct bnx2x *bp)
 		/* Bit-map indicating which L2 hdrs may appear after the
 		 * basic Ethernet header
 		 */
-		REG_WR(bp, BP_PORT(bp) ?
-			   NIG_REG_P1_HDRS_AFTER_BASIC :
-			   NIG_REG_P0_HDRS_AFTER_BASIC,
-			   IS_MF_SD(bp) ? 7 : 6);
+		if (IS_MF_AFEX(bp))
+			REG_WR(bp, BP_PORT(bp) ?
+			       NIG_REG_P1_HDRS_AFTER_BASIC :
+			       NIG_REG_P0_HDRS_AFTER_BASIC, 0xE);
+		else
+			REG_WR(bp, BP_PORT(bp) ?
+			       NIG_REG_P1_HDRS_AFTER_BASIC :
+			       NIG_REG_P0_HDRS_AFTER_BASIC,
+			       IS_MF_SD(bp) ? 7 : 6);
 
 		if (CHIP_IS_E3(bp))
 			REG_WR(bp, BP_PORT(bp) ?
@@ -6609,6 +6917,7 @@ static int bnx2x_init_hw_port(struct bnx2x *bp)
 				val = 1;
 				break;
 			case MULTI_FUNCTION_SI:
+			case MULTI_FUNCTION_AFEX:
 				val = 2;
 				break;
 			}
@@ -6640,21 +6949,71 @@ static int bnx2x_init_hw_port(struct bnx2x *bp)
 static void bnx2x_ilt_wr(struct bnx2x *bp, u32 index, dma_addr_t addr)
 {
 	int reg;
+	u32 wb_write[2];
 
 	if (CHIP_IS_E1(bp))
 		reg = PXP2_REG_RQ_ONCHIP_AT + index*8;
 	else
 		reg = PXP2_REG_RQ_ONCHIP_AT_B0 + index*8;
 
-	bnx2x_wb_wr(bp, reg, ONCHIP_ADDR1(addr), ONCHIP_ADDR2(addr));
+	wb_write[0] = ONCHIP_ADDR1(addr);
+	wb_write[1] = ONCHIP_ADDR2(addr);
+	REG_WR_DMAE(bp, reg, wb_write, 2);
 }
 
-static inline void bnx2x_igu_clear_sb(struct bnx2x *bp, u8 idu_sb_id)
+static void bnx2x_igu_clear_sb_gen(struct bnx2x *bp, u8 func,
+				   u8 idu_sb_id, bool is_Pf)
+{
+	u32 data, ctl, cnt = 100;
+	u32 igu_addr_data = IGU_REG_COMMAND_REG_32LSB_DATA;
+	u32 igu_addr_ctl = IGU_REG_COMMAND_REG_CTRL;
+	u32 igu_addr_ack = IGU_REG_CSTORM_TYPE_0_SB_CLEANUP + (idu_sb_id/32)*4;
+	u32 sb_bit =  1 << (idu_sb_id%32);
+	u32 func_encode = func | (is_Pf ? 1 : 0) << IGU_FID_ENCODE_IS_PF_SHIFT;
+	u32 addr_encode = IGU_CMD_E2_PROD_UPD_BASE + idu_sb_id;
+
+	/* Not supported in BC mode */
+	if (CHIP_INT_MODE_IS_BC(bp))
+		return;
+
+	data = (IGU_USE_REGISTER_cstorm_type_0_sb_cleanup
+			<< IGU_REGULAR_CLEANUP_TYPE_SHIFT)	|
+		IGU_REGULAR_CLEANUP_SET				|
+		IGU_REGULAR_BCLEANUP;
+
+	ctl = addr_encode << IGU_CTRL_REG_ADDRESS_SHIFT		|
+	      func_encode << IGU_CTRL_REG_FID_SHIFT		|
+	      IGU_CTRL_CMD_TYPE_WR << IGU_CTRL_REG_TYPE_SHIFT;
+
+	DP(NETIF_MSG_HW, "write 0x%08x to IGU(via GRC) addr 0x%x\n",
+			 data, igu_addr_data);
+	REG_WR(bp, igu_addr_data, data);
+	mmiowb();
+	barrier();
+	DP(NETIF_MSG_HW, "write 0x%08x to IGU(via GRC) addr 0x%x\n",
+			  ctl, igu_addr_ctl);
+	REG_WR(bp, igu_addr_ctl, ctl);
+	mmiowb();
+	barrier();
+
+	/* wait for clean up to finish */
+	while (!(REG_RD(bp, igu_addr_ack) & sb_bit) && --cnt)
+		msleep(20);
+
+
+	if (!(REG_RD(bp, igu_addr_ack) & sb_bit)) {
+		DP(NETIF_MSG_HW,
+		   "Unable to finish IGU cleanup: idu_sb_id %d offset %d bit %d (cnt %d)\n",
+			  idu_sb_id, idu_sb_id/32, idu_sb_id%32, cnt);
+	}
+}
+
+static void bnx2x_igu_clear_sb(struct bnx2x *bp, u8 idu_sb_id)
 {
 	bnx2x_igu_clear_sb_gen(bp, BP_FUNC(bp), idu_sb_id, true /*PF*/);
 }
 
-static inline void bnx2x_clear_func_ilt(struct bnx2x *bp, u32 func)
+static void bnx2x_clear_func_ilt(struct bnx2x *bp, u32 func)
 {
 	u32 i, base = FUNC_ILT_BASE(func);
 	for (i = base; i < base + ILT_PER_FUNC; i++)
@@ -7005,7 +7364,7 @@ void bnx2x_free_mem(struct bnx2x *bp)
 		       BCM_PAGE_SIZE * NUM_EQ_PAGES);
 }
 
-static inline int bnx2x_alloc_fw_stats_mem(struct bnx2x *bp)
+static int bnx2x_alloc_fw_stats_mem(struct bnx2x *bp)
 {
 	int num_groups;
 	int is_fcoe_stats = NO_FCOE(bp) ? 0 : 1;
@@ -7192,7 +7551,8 @@ int bnx2x_set_eth_mac(struct bnx2x *bp, bool set)
 	unsigned long ramrod_flags = 0;
 
 #ifdef BCM_CNIC
-	if (is_zero_ether_addr(bp->dev->dev_addr) && IS_MF_STORAGE_SD(bp)) {
+	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,
 		   "Ignoring Zero MAC for STORAGE SD mode\n");
 		return 0;
@@ -7230,7 +7590,7 @@ static void __devinit bnx2x_set_int_mode(struct bnx2x *bp)
 		BNX2X_DEV_INFO("set number of queues to 1\n");
 		break;
 	default:
-		/* Set number of queues according to bp->multi_mode value */
+		/* Set number of queues for MSI-X mode */
 		bnx2x_set_num_queues(bp);
 
 		BNX2X_DEV_INFO("set number of queues to %d\n", bp->num_queues);
@@ -7239,15 +7599,17 @@ static void __devinit bnx2x_set_int_mode(struct bnx2x *bp)
 		 * so try to enable MSI-X with the requested number of fp's
 		 * and fallback to MSI or legacy INTx with one fp
 		 */
-		if (bnx2x_enable_msix(bp)) {
-			/* failed to enable MSI-X */
-			BNX2X_DEV_INFO("Failed to enable MSI-X (%d), set number of queues to %d\n",
+		if (bnx2x_enable_msix(bp) ||
+		    bp->flags & USING_SINGLE_MSIX_FLAG) {
+			/* failed to enable multiple MSI-X */
+			BNX2X_DEV_INFO("Failed to enable multiple MSI-X (%d), set number of queues to %d\n",
 				       bp->num_queues, 1 + NON_ETH_CONTEXT_USE);
 
 			bp->num_queues = 1 + NON_ETH_CONTEXT_USE;
 
 			/* Try to enable MSI */
-			if (!(bp->flags & DISABLE_MSI_FLAG))
+			if (!(bp->flags & USING_SINGLE_MSIX_FLAG) &&
+			    !(bp->flags & DISABLE_MSI_FLAG))
 				bnx2x_enable_msi(bp);
 		}
 		break;
@@ -7368,7 +7730,7 @@ void bnx2x_ilt_set_info(struct bnx2x *bp)
  *      - HC configuration
  *      - Queue's CDU context
  */
-static inline void bnx2x_pf_q_prep_init(struct bnx2x *bp,
+static void bnx2x_pf_q_prep_init(struct bnx2x *bp,
 	struct bnx2x_fastpath *fp, struct bnx2x_queue_init_params *init_params)
 {
 
@@ -7718,7 +8080,7 @@ static void bnx2x_reset_port(struct bnx2x *bp)
 	/* TODO: Close Doorbell port? */
 }
 
-static inline int bnx2x_reset_hw(struct bnx2x *bp, u32 load_code)
+static int bnx2x_reset_hw(struct bnx2x *bp, u32 load_code)
 {
 	struct bnx2x_func_state_params func_params = {NULL};
 
@@ -7733,7 +8095,7 @@ static inline int bnx2x_reset_hw(struct bnx2x *bp, u32 load_code)
 	return bnx2x_func_state_change(bp, &func_params);
 }
 
-static inline int bnx2x_func_stop(struct bnx2x *bp)
+static int bnx2x_func_stop(struct bnx2x *bp)
 {
 	struct bnx2x_func_state_params func_params = {NULL};
 	int rc;
@@ -7848,7 +8210,7 @@ void bnx2x_send_unload_done(struct bnx2x *bp)
 		bnx2x_fw_command(bp, DRV_MSG_CODE_UNLOAD_DONE, 0);
 }
 
-static inline int bnx2x_func_wait_started(struct bnx2x *bp)
+static int bnx2x_func_wait_started(struct bnx2x *bp)
 {
 	int tout = 50;
 	int msix = (bp->flags & USING_MSIX_FLAG) ? 1 : 0;
@@ -8158,7 +8520,7 @@ static void bnx2x_reset_mcp_prep(struct bnx2x *bp, u32 *magic_val)
  *
  * @bp:	driver handle
  */
-static inline void bnx2x_mcp_wait_one(struct bnx2x *bp)
+static void bnx2x_mcp_wait_one(struct bnx2x *bp)
 {
 	/* special handling for emulation and FPGA,
 	   wait 10 times longer */
@@ -8494,7 +8856,7 @@ exit_leader_reset:
 	return rc;
 }
 
-static inline void bnx2x_recovery_failed(struct bnx2x *bp)
+static void bnx2x_recovery_failed(struct bnx2x *bp)
 {
 	netdev_err(bp->dev, "Recovery has failed. Power cycle is needed.\n");
 
@@ -8727,7 +9089,8 @@ sp_rtnl_not_reset:
 #endif
 	if (test_and_clear_bit(BNX2X_SP_RTNL_SETUP_TC, &bp->sp_rtnl_state))
 		bnx2x_setup_tc(bp->dev, bp->dcbx_port_params.ets.num_of_cos);
-
+	if (test_and_clear_bit(BNX2X_SP_RTNL_AFEX_F_UPDATE, &bp->sp_rtnl_state))
+		bnx2x_after_function_update(bp);
 	/*
 	 * in case of fan failure we need to reset id if the "stop on error"
 	 * debug flag is set, since we trying to prevent permanent overheating
@@ -9122,13 +9485,34 @@ static int __devinit bnx2x_prev_unload_common(struct bnx2x *bp)
 	return bnx2x_prev_mcp_done(bp);
 }
 
+/* previous driver DMAE transaction may have occurred when pre-boot stage ended
+ * and boot began, or when kdump kernel was loaded. Either case would invalidate
+ * the addresses of the transaction, resulting in was-error bit set in the pci
+ * causing all hw-to-host pcie transactions to timeout. If this happened we want
+ * to clear the interrupt which detected this from the pglueb and the was done
+ * bit
+ */
+static void __devinit bnx2x_prev_interrupted_dmae(struct bnx2x *bp)
+{
+	u32 val = REG_RD(bp, PGLUE_B_REG_PGLUE_B_INT_STS);
+	if (val & PGLUE_B_PGLUE_B_INT_STS_REG_WAS_ERROR_ATTN) {
+		BNX2X_ERR("was error bit was found to be set in pglueb upon startup. Clearing");
+		REG_WR(bp, PGLUE_B_REG_WAS_ERROR_PF_7_0_CLR, 1 << BP_FUNC(bp));
+	}
+}
+
 static int __devinit bnx2x_prev_unload(struct bnx2x *bp)
 {
 	int time_counter = 10;
 	u32 rc, fw, hw_lock_reg, hw_lock_val;
 	BNX2X_DEV_INFO("Entering Previous Unload Flow\n");
 
-       /* Release previously held locks */
+	/* clear hw from errors which may have resulted from an interrupted
+	 * dmae transaction.
+	 */
+	bnx2x_prev_interrupted_dmae(bp);
+
+	/* Release previously held locks */
 	hw_lock_reg = (BP_FUNC(bp) <= 5) ?
 		      (MISC_REG_DRIVER_CONTROL_1 + BP_FUNC(bp) * 8) :
 		      (MISC_REG_DRIVER_CONTROL_7 + (BP_FUNC(bp) - 6) * 8);
@@ -9201,6 +9585,17 @@ static void __devinit bnx2x_get_common_hwinfo(struct bnx2x *bp)
 	id |= (val & 0xf);
 	bp->common.chip_id = id;
 
+	/* force 57811 according to MISC register */
+	if (REG_RD(bp, MISC_REG_CHIP_TYPE) & MISC_REG_CHIP_TYPE_57811_MASK) {
+		if (CHIP_IS_57810(bp))
+			bp->common.chip_id = (CHIP_NUM_57811 << 16) |
+				(bp->common.chip_id & 0x0000FFFF);
+		else if (CHIP_IS_57810_MF(bp))
+			bp->common.chip_id = (CHIP_NUM_57811_MF << 16) |
+				(bp->common.chip_id & 0x0000FFFF);
+		bp->common.chip_id |= 0x1;
+	}
+
 	/* Set doorbell size */
 	bp->db_size = (1 << BNX2X_DB_SHIFT);
 
@@ -9293,7 +9688,9 @@ static void __devinit bnx2x_get_common_hwinfo(struct bnx2x *bp)
 	bp->link_params.feature_config_flags |=
 		(val >= REQ_BC_VER_4_VRFY_SPECIFIC_PHY_OPT_MDL) ?
 		FEATURE_CONFIG_BC_SUPPORTS_DUAL_PHY_OPT_MDL_VRFY : 0;
-
+	bp->link_params.feature_config_flags |=
+		(val >= REQ_BC_VER_4_VRFY_AFEX_SUPPORTED) ?
+		FEATURE_CONFIG_BC_SUPPORTS_AFEX : 0;
 	bp->link_params.feature_config_flags |=
 		(val >= REQ_BC_VER_4_SFP_TX_DISABLE_SUPPORTED) ?
 		FEATURE_CONFIG_BC_SUPPORTS_SFP_TX_DISABLED : 0;
@@ -9925,6 +10322,9 @@ static void __devinit bnx2x_get_mac_hwinfo(struct bnx2x *bp)
 
 			} else
 				bp->flags |= NO_FCOE_FLAG;
+
+			bp->mf_ext_config = cfg;
+
 		} else { /* SD MODE */
 			if (IS_MF_STORAGE_SD(bp)) {
 				if (BNX2X_IS_MF_SD_PROTOCOL_ISCSI(bp)) {
@@ -9946,6 +10346,11 @@ static void __devinit bnx2x_get_mac_hwinfo(struct bnx2x *bp)
 				memset(bp->dev->dev_addr, 0, ETH_ALEN);
 			}
 		}
+
+		if (IS_MF_FCOE_AFEX(bp))
+			/* use FIP MAC as primary MAC */
+			memcpy(bp->dev->dev_addr, fip_mac, ETH_ALEN);
+
 #endif
 	} else {
 		/* in SF read MACs from port configuration */
@@ -10118,6 +10523,19 @@ static int __devinit bnx2x_get_hwinfo(struct bnx2x *bp)
 				} else
 					BNX2X_DEV_INFO("illegal MAC address for SI\n");
 				break;
+			case SHARED_FEAT_CFG_FORCE_SF_MODE_AFEX_MODE:
+				if ((!CHIP_IS_E1x(bp)) &&
+				    (MF_CFG_RD(bp, func_mf_config[func].
+					       mac_upper) != 0xffff) &&
+				    (SHMEM2_HAS(bp,
+						afex_driver_support))) {
+					bp->mf_mode = MULTI_FUNCTION_AFEX;
+					bp->mf_config[vn] = MF_CFG_RD(bp,
+						func_mf_config[func].config);
+				} else {
+					BNX2X_DEV_INFO("can not configure afex mode\n");
+				}
+				break;
 			case SHARED_FEAT_CFG_FORCE_SF_MODE_MF_ALLOWED:
 				/* get OV configuration */
 				val = MF_CFG_RD(bp,
@@ -10158,6 +10576,9 @@ static int __devinit bnx2x_get_hwinfo(struct bnx2x *bp)
 				return -EPERM;
 			}
 			break;
+		case MULTI_FUNCTION_AFEX:
+			BNX2X_DEV_INFO("func %d is in MF afex mode\n", func);
+			break;
 		case MULTI_FUNCTION_SI:
 			BNX2X_DEV_INFO("func %d is in MF switch-independent mode\n",
 				       func);
@@ -10325,6 +10746,9 @@ static void __devinit bnx2x_set_modes_bitmap(struct bnx2x *bp)
 		case MULTI_FUNCTION_SI:
 			SET_FLAGS(flags, MODE_MF_SI);
 			break;
+		case MULTI_FUNCTION_AFEX:
+			SET_FLAGS(flags, MODE_MF_AFEX);
+			break;
 		}
 	} else
 		SET_FLAGS(flags, MODE_SF);
@@ -10384,12 +10808,10 @@ static int __devinit bnx2x_init_bp(struct bnx2x *bp)
 	if (BP_NOMCP(bp) && (func == 0))
 		dev_err(&bp->pdev->dev, "MCP disabled, must load devices in order!\n");
 
-	bp->multi_mode = multi_mode;
-
 	bp->disable_tpa = disable_tpa;
 
 #ifdef BCM_CNIC
-	bp->disable_tpa |= IS_MF_STORAGE_SD(bp);
+	bp->disable_tpa |= IS_MF_STORAGE_SD(bp) || IS_MF_FCOE_AFEX(bp);
 #endif
 
 	/* Set TPA flags */
@@ -10408,7 +10830,7 @@ static int __devinit bnx2x_init_bp(struct bnx2x *bp)
 
 	bp->mrrs = mrrs;
 
-	bp->tx_ring_size = MAX_TX_AVAIL;
+	bp->tx_ring_size = IS_MF_FCOE_AFEX(bp) ? 0 : MAX_TX_AVAIL;
 
 	/* make sure that the numbers are in the right granularity */
 	bp->tx_ticks = (50 / BNX2X_BTR) * BNX2X_BTR;
@@ -10439,8 +10861,6 @@ static int __devinit bnx2x_init_bp(struct bnx2x *bp)
 	if (CHIP_IS_E3B0(bp))
 		bp->max_cos = BNX2X_MULTI_TX_COS_E3B0;
 
-	bp->gro_check = bnx2x_need_gro_check(bp->dev->mtu);
-
 	return rc;
 }
 
@@ -10530,8 +10950,8 @@ static int bnx2x_close(struct net_device *dev)
 	return 0;
 }
 
-static inline int bnx2x_init_mcast_macs_list(struct bnx2x *bp,
-					 struct bnx2x_mcast_ramrod_params *p)
+static int bnx2x_init_mcast_macs_list(struct bnx2x *bp,
+				      struct bnx2x_mcast_ramrod_params *p)
 {
 	int mc_count = netdev_mc_count(bp->dev);
 	struct bnx2x_mcast_list_elem *mc_mac =
@@ -10554,7 +10974,7 @@ static inline int bnx2x_init_mcast_macs_list(struct bnx2x *bp,
 	return 0;
 }
 
-static inline void bnx2x_free_mcast_macs_list(
+static void bnx2x_free_mcast_macs_list(
 	struct bnx2x_mcast_ramrod_params *p)
 {
 	struct bnx2x_mcast_list_elem *mc_mac =
@@ -10572,7 +10992,7 @@ static inline void bnx2x_free_mcast_macs_list(
  *
  * We will use zero (0) as a MAC type for these MACs.
  */
-static inline int bnx2x_set_uc_list(struct bnx2x *bp)
+static int bnx2x_set_uc_list(struct bnx2x *bp)
 {
 	int rc;
 	struct net_device *dev = bp->dev;
@@ -10603,7 +11023,7 @@ static inline int bnx2x_set_uc_list(struct bnx2x *bp)
 				 BNX2X_UC_LIST_MAC, &ramrod_flags);
 }
 
-static inline int bnx2x_set_mc_list(struct bnx2x *bp)
+static int bnx2x_set_mc_list(struct bnx2x *bp)
 {
 	struct net_device *dev = bp->dev;
 	struct bnx2x_mcast_ramrod_params rparam = {NULL};
@@ -10789,7 +11209,7 @@ static const struct net_device_ops bnx2x_netdev_ops = {
 #endif
 };
 
-static inline int bnx2x_set_coherency_mask(struct bnx2x *bp)
+static int bnx2x_set_coherency_mask(struct bnx2x *bp)
 {
 	struct device *dev = &bp->pdev->dev;
 
@@ -11055,7 +11475,7 @@ static int bnx2x_check_firmware(struct bnx2x *bp)
 	return 0;
 }
 
-static inline void be32_to_cpu_n(const u8 *_source, u8 *_target, u32 n)
+static void be32_to_cpu_n(const u8 *_source, u8 *_target, u32 n)
 {
 	const __be32 *source = (const __be32 *)_source;
 	u32 *target = (u32 *)_target;
@@ -11069,7 +11489,7 @@ static inline void be32_to_cpu_n(const u8 *_source, u8 *_target, u32 n)
    Ops array is stored in the following format:
    {op(8bit), offset(24bit, big endian), data(32bit, big endian)}
  */
-static inline void bnx2x_prep_ops(const u8 *_source, u8 *_target, u32 n)
+static void bnx2x_prep_ops(const u8 *_source, u8 *_target, u32 n)
 {
 	const __be32 *source = (const __be32 *)_source;
 	struct raw_op *target = (struct raw_op *)_target;
@@ -11087,7 +11507,7 @@ static inline void bnx2x_prep_ops(const u8 *_source, u8 *_target, u32 n)
  * IRO array is stored in the following format:
  * {base(24bit), m1(16bit), m2(16bit), m3(16bit), size(16bit) }
  */
-static inline void bnx2x_prep_iro(const u8 *_source, u8 *_target, u32 n)
+static void bnx2x_prep_iro(const u8 *_source, u8 *_target, u32 n)
 {
 	const __be32 *source = (const __be32 *)_source;
 	struct iro *target = (struct iro *)_target;
@@ -11107,7 +11527,7 @@ static inline void bnx2x_prep_iro(const u8 *_source, u8 *_target, u32 n)
 	}
 }
 
-static inline void be16_to_cpu_n(const u8 *_source, u8 *_target, u32 n)
+static void be16_to_cpu_n(const u8 *_source, u8 *_target, u32 n)
 {
 	const __be16 *source = (const __be16 *)_source;
 	u16 *target = (u16 *)_target;
@@ -11244,11 +11664,13 @@ void bnx2x__init_func_obj(struct bnx2x *bp)
 	bnx2x_init_func_obj(bp, &bp->func_obj,
 			    bnx2x_sp(bp, func_rdata),
 			    bnx2x_sp_mapping(bp, func_rdata),
+			    bnx2x_sp(bp, func_afex_rdata),
+			    bnx2x_sp_mapping(bp, func_afex_rdata),
 			    &bnx2x_func_sp_drv);
 }
 
 /* must be called after sriov-enable */
-static inline int bnx2x_set_qm_cid_count(struct bnx2x *bp)
+static int bnx2x_set_qm_cid_count(struct bnx2x *bp)
 {
 	int cid_count = BNX2X_L2_CID_COUNT(bp);
 
@@ -11264,7 +11686,7 @@ static inline int bnx2x_set_qm_cid_count(struct bnx2x *bp)
  * @dev:	pci device
  *
  */
-static inline int bnx2x_get_num_non_def_sbs(struct pci_dev *pdev)
+static int bnx2x_get_num_non_def_sbs(struct pci_dev *pdev)
 {
 	int pos;
 	u16 control;
@@ -11325,6 +11747,8 @@ static int __devinit bnx2x_init_one(struct pci_dev *pdev,
 	case BCM57810_MF:
 	case BCM57840:
 	case BCM57840_MF:
+	case BCM57811:
+	case BCM57811_MF:
 		max_cos_est = BNX2X_MULTI_TX_COS_E3B0;
 		break;
 
@@ -11738,7 +12162,7 @@ module_exit(bnx2x_cleanup);
  * This function will wait until the ramdord completion returns.
  * Return 0 if success, -ENODEV if ramrod doesn't return.
  */
-static inline int bnx2x_set_iscsi_eth_mac_addr(struct bnx2x *bp)
+static int bnx2x_set_iscsi_eth_mac_addr(struct bnx2x *bp)
 {
 	unsigned long ramrod_flags = 0;
 
diff --git a/drivers/net/ethernet/broadcom/bnx2x/bnx2x_reg.h b/drivers/net/ethernet/broadcom/bnx2x/bnx2x_reg.h
index c25803b9c0c..bbd387492a8 100644
--- a/drivers/net/ethernet/broadcom/bnx2x/bnx2x_reg.h
+++ b/drivers/net/ethernet/broadcom/bnx2x/bnx2x_reg.h
@@ -1483,6 +1483,11 @@
    starts at 0x0 for the A0 tape-out and increments by one for each
    all-layer tape-out. */
 #define MISC_REG_CHIP_REV					 0xa40c
+/* [R 14] otp_misc_do[100:0] spare bits collection: 13:11-
+ * otp_misc_do[100:98]; 10:7 - otp_misc_do[87:84]; 6:3 - otp_misc_do[75:72];
+ * 2:1 - otp_misc_do[51:50]; 0 - otp_misc_do[1]. */
+#define MISC_REG_CHIP_TYPE					 0xac60
+#define MISC_REG_CHIP_TYPE_57811_MASK				 (1<<1)
 /* [RW 32] The following driver registers(1...16) represent 16 drivers and
    32 clients. Each client can be controlled by one driver only. One in each
    bit represent that this driver control the appropriate client (Ex: bit 5
diff --git a/drivers/net/ethernet/broadcom/bnx2x/bnx2x_sp.c b/drivers/net/ethernet/broadcom/bnx2x/bnx2x_sp.c
index 51357332162..6c14b4a4e82 100644
--- a/drivers/net/ethernet/broadcom/bnx2x/bnx2x_sp.c
+++ b/drivers/net/ethernet/broadcom/bnx2x/bnx2x_sp.c
@@ -633,14 +633,17 @@ static inline u8 bnx2x_vlan_mac_get_rx_tx_flag(struct bnx2x_vlan_mac_obj *o)
 }
 
 
-static inline void bnx2x_set_mac_in_nig(struct bnx2x *bp,
-				 bool add, unsigned char *dev_addr, int index)
+void bnx2x_set_mac_in_nig(struct bnx2x *bp,
+			  bool add, unsigned char *dev_addr, int index)
 {
 	u32 wb_data[2];
 	u32 reg_offset = BP_PORT(bp) ? NIG_REG_LLH1_FUNC_MEM :
 			 NIG_REG_LLH0_FUNC_MEM;
 
-	if (!IS_MF_SI(bp) || index > BNX2X_LLH_CAM_MAX_PF_LINE)
+	if (!IS_MF_SI(bp) && !IS_MF_AFEX(bp))
+		return;
+
+	if (index > BNX2X_LLH_CAM_MAX_PF_LINE)
 		return;
 
 	DP(BNX2X_MSG_SP, "Going to %s LLH configuration at entry %d\n",
@@ -4090,12 +4093,6 @@ static int bnx2x_setup_rss(struct bnx2x *bp,
 		rss_mode = ETH_RSS_MODE_DISABLED;
 	else if (test_bit(BNX2X_RSS_MODE_REGULAR, &p->rss_flags))
 		rss_mode = ETH_RSS_MODE_REGULAR;
-	else if (test_bit(BNX2X_RSS_MODE_VLAN_PRI, &p->rss_flags))
-		rss_mode = ETH_RSS_MODE_VLAN_PRI;
-	else if (test_bit(BNX2X_RSS_MODE_E1HOV_PRI, &p->rss_flags))
-		rss_mode = ETH_RSS_MODE_E1HOV_PRI;
-	else if (test_bit(BNX2X_RSS_MODE_IP_DSCP, &p->rss_flags))
-		rss_mode = ETH_RSS_MODE_IP_DSCP;
 
 	data->rss_mode = rss_mode;
 
@@ -4404,6 +4401,9 @@ static void bnx2x_q_fill_init_tx_data(struct bnx2x_queue_sp_obj *o,
 		test_bit(BNX2X_Q_FLG_TX_SWITCH, flags);
 	tx_data->anti_spoofing_flg =
 		test_bit(BNX2X_Q_FLG_ANTI_SPOOF, flags);
+	tx_data->force_default_pri_flg =
+		test_bit(BNX2X_Q_FLG_FORCE_DEFAULT_PRI, flags);
+
 	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;
@@ -5331,6 +5331,17 @@ static int bnx2x_func_chk_transition(struct bnx2x *bp,
 	case BNX2X_F_STATE_STARTED:
 		if (cmd == BNX2X_F_CMD_STOP)
 			next_state = BNX2X_F_STATE_INITIALIZED;
+		/* afex ramrods can be sent only in started mode, and only
+		 * if not pending for function_stop ramrod completion
+		 * for these events - next state remained STARTED.
+		 */
+		else if ((cmd == BNX2X_F_CMD_AFEX_UPDATE) &&
+			 (!test_bit(BNX2X_F_CMD_STOP, &o->pending)))
+			next_state = BNX2X_F_STATE_STARTED;
+
+		else if ((cmd == BNX2X_F_CMD_AFEX_VIFLISTS) &&
+			 (!test_bit(BNX2X_F_CMD_STOP, &o->pending)))
+			next_state = BNX2X_F_STATE_STARTED;
 		else if (cmd == BNX2X_F_CMD_TX_STOP)
 			next_state = BNX2X_F_STATE_TX_STOPPED;
 
@@ -5618,6 +5629,83 @@ static inline int bnx2x_func_send_start(struct bnx2x *bp,
 			     U64_LO(data_mapping), NONE_CONNECTION_TYPE);
 }
 
+static inline int bnx2x_func_send_afex_update(struct bnx2x *bp,
+					 struct bnx2x_func_state_params *params)
+{
+	struct bnx2x_func_sp_obj *o = params->f_obj;
+	struct function_update_data *rdata =
+		(struct function_update_data *)o->afex_rdata;
+	dma_addr_t data_mapping = o->afex_rdata_mapping;
+	struct bnx2x_func_afex_update_params *afex_update_params =
+		&params->params.afex_update;
+
+	memset(rdata, 0, sizeof(*rdata));
+
+	/* Fill the ramrod data with provided parameters */
+	rdata->vif_id_change_flg = 1;
+	rdata->vif_id = cpu_to_le16(afex_update_params->vif_id);
+	rdata->afex_default_vlan_change_flg = 1;
+	rdata->afex_default_vlan =
+		cpu_to_le16(afex_update_params->afex_default_vlan);
+	rdata->allowed_priorities_change_flg = 1;
+	rdata->allowed_priorities = afex_update_params->allowed_priorities;
+
+	/*  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()).
+	 */
+	DP(BNX2X_MSG_SP,
+	   "afex: sending func_update vif_id 0x%x dvlan 0x%x prio 0x%x\n",
+	   rdata->vif_id,
+	   rdata->afex_default_vlan, rdata->allowed_priorities);
+
+	return bnx2x_sp_post(bp, RAMROD_CMD_ID_COMMON_FUNCTION_UPDATE, 0,
+			     U64_HI(data_mapping),
+			     U64_LO(data_mapping), NONE_CONNECTION_TYPE);
+}
+
+static
+inline int bnx2x_func_send_afex_viflists(struct bnx2x *bp,
+					 struct bnx2x_func_state_params *params)
+{
+	struct bnx2x_func_sp_obj *o = params->f_obj;
+	struct afex_vif_list_ramrod_data *rdata =
+		(struct afex_vif_list_ramrod_data *)o->afex_rdata;
+	struct bnx2x_func_afex_viflists_params *afex_viflist_params =
+		&params->params.afex_viflists;
+	u64 *p_rdata = (u64 *)rdata;
+
+	memset(rdata, 0, sizeof(*rdata));
+
+	/* Fill the ramrod data with provided parameters */
+	rdata->vif_list_index = afex_viflist_params->vif_list_index;
+	rdata->func_bit_map = afex_viflist_params->func_bit_map;
+	rdata->afex_vif_list_command =
+		afex_viflist_params->afex_vif_list_command;
+	rdata->func_to_clear = afex_viflist_params->func_to_clear;
+
+	/* send in echo type of sub command */
+	rdata->echo = afex_viflist_params->afex_vif_list_command;
+
+	/*  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()).
+	 */
+
+	DP(BNX2X_MSG_SP, "afex: ramrod lists, cmd 0x%x index 0x%x func_bit_map 0x%x func_to_clr 0x%x\n",
+	   rdata->afex_vif_list_command, rdata->vif_list_index,
+	   rdata->func_bit_map, rdata->func_to_clear);
+
+	/* this ramrod sends data directly and not through DMA mapping */
+	return bnx2x_sp_post(bp, RAMROD_CMD_ID_COMMON_AFEX_VIF_LISTS, 0,
+			     U64_HI(*p_rdata), U64_LO(*p_rdata),
+			     NONE_CONNECTION_TYPE);
+}
+
 static inline int bnx2x_func_send_stop(struct bnx2x *bp,
 				       struct bnx2x_func_state_params *params)
 {
@@ -5669,6 +5757,10 @@ static int bnx2x_func_send_cmd(struct bnx2x *bp,
 		return bnx2x_func_send_stop(bp, params);
 	case BNX2X_F_CMD_HW_RESET:
 		return bnx2x_func_hw_reset(bp, params);
+	case BNX2X_F_CMD_AFEX_UPDATE:
+		return bnx2x_func_send_afex_update(bp, params);
+	case BNX2X_F_CMD_AFEX_VIFLISTS:
+		return bnx2x_func_send_afex_viflists(bp, params);
 	case BNX2X_F_CMD_TX_STOP:
 		return bnx2x_func_send_tx_stop(bp, params);
 	case BNX2X_F_CMD_TX_START:
@@ -5682,6 +5774,7 @@ static int bnx2x_func_send_cmd(struct bnx2x *bp,
 void bnx2x_init_func_obj(struct bnx2x *bp,
 			 struct bnx2x_func_sp_obj *obj,
 			 void *rdata, dma_addr_t rdata_mapping,
+			 void *afex_rdata, dma_addr_t afex_rdata_mapping,
 			 struct bnx2x_func_sp_drv_ops *drv_iface)
 {
 	memset(obj, 0, sizeof(*obj));
@@ -5690,7 +5783,8 @@ void bnx2x_init_func_obj(struct bnx2x *bp,
 
 	obj->rdata = rdata;
 	obj->rdata_mapping = rdata_mapping;
-
+	obj->afex_rdata = afex_rdata;
+	obj->afex_rdata_mapping = afex_rdata_mapping;
 	obj->send_cmd = bnx2x_func_send_cmd;
 	obj->check_transition = bnx2x_func_chk_transition;
 	obj->complete_cmd = bnx2x_func_comp_cmd;
diff --git a/drivers/net/ethernet/broadcom/bnx2x/bnx2x_sp.h b/drivers/net/ethernet/broadcom/bnx2x/bnx2x_sp.h
index 61a7670adfc..efd80bdd0df 100644
--- a/drivers/net/ethernet/broadcom/bnx2x/bnx2x_sp.h
+++ b/drivers/net/ethernet/broadcom/bnx2x/bnx2x_sp.h
@@ -62,6 +62,8 @@ enum {
 	BNX2X_FILTER_MCAST_PENDING,
 	BNX2X_FILTER_MCAST_SCHED,
 	BNX2X_FILTER_RSS_CONF_PENDING,
+	BNX2X_AFEX_FCOE_Q_UPDATE_PENDING,
+	BNX2X_AFEX_PENDING_VIFSET_MCP_ACK
 };
 
 struct bnx2x_raw_obj {
@@ -432,6 +434,8 @@ enum {
 	BNX2X_LLH_CAM_MAX_PF_LINE = NIG_REG_LLH1_FUNC_MEM_SIZE / 2
 };
 
+void bnx2x_set_mac_in_nig(struct bnx2x *bp,
+			  bool add, unsigned char *dev_addr, int index);
 
 /** RX_MODE verbs:DROP_ALL/ACCEPT_ALL/ACCEPT_ALL_MULTI/ACCEPT_ALL_VLAN/NORMAL */
 
@@ -685,9 +689,6 @@ enum {
 	/* RSS_MODE bits are mutually exclusive */
 	BNX2X_RSS_MODE_DISABLED,
 	BNX2X_RSS_MODE_REGULAR,
-	BNX2X_RSS_MODE_VLAN_PRI,
-	BNX2X_RSS_MODE_E1HOV_PRI,
-	BNX2X_RSS_MODE_IP_DSCP,
 
 	BNX2X_RSS_SET_SRCH, /* Setup searcher, E1x specific flag */
 
@@ -801,7 +802,8 @@ enum {
 	BNX2X_Q_FLG_TX_SWITCH,
 	BNX2X_Q_FLG_TX_SEC,
 	BNX2X_Q_FLG_ANTI_SPOOF,
-	BNX2X_Q_FLG_SILENT_VLAN_REM
+	BNX2X_Q_FLG_SILENT_VLAN_REM,
+	BNX2X_Q_FLG_FORCE_DEFAULT_PRI
 };
 
 /* Queue type options: queue type may be a compination of below. */
@@ -963,6 +965,11 @@ struct bnx2x_queue_state_params {
 	} params;
 };
 
+struct bnx2x_viflist_params {
+	u8 echo_res;
+	u8 func_bit_map_res;
+};
+
 struct bnx2x_queue_sp_obj {
 	u32		cids[BNX2X_MULTI_TX_COS];
 	u8		cl_id;
@@ -1045,6 +1052,8 @@ enum bnx2x_func_cmd {
 	BNX2X_F_CMD_START,
 	BNX2X_F_CMD_STOP,
 	BNX2X_F_CMD_HW_RESET,
+	BNX2X_F_CMD_AFEX_UPDATE,
+	BNX2X_F_CMD_AFEX_VIFLISTS,
 	BNX2X_F_CMD_TX_STOP,
 	BNX2X_F_CMD_TX_START,
 	BNX2X_F_CMD_MAX,
@@ -1089,6 +1098,18 @@ struct bnx2x_func_start_params {
 	u8 network_cos_mode;
 };
 
+struct bnx2x_func_afex_update_params {
+	u16 vif_id;
+	u16 afex_default_vlan;
+	u8 allowed_priorities;
+};
+
+struct bnx2x_func_afex_viflists_params {
+	u16 vif_list_index;
+	u8 func_bit_map;
+	u8 afex_vif_list_command;
+	u8 func_to_clear;
+};
 struct bnx2x_func_tx_start_params {
 	struct priority_cos traffic_type_to_priority_cos[MAX_TRAFFIC_TYPES];
 	u8 dcb_enabled;
@@ -1110,6 +1131,8 @@ struct bnx2x_func_state_params {
 		struct bnx2x_func_hw_init_params hw_init;
 		struct bnx2x_func_hw_reset_params hw_reset;
 		struct bnx2x_func_start_params start;
+		struct bnx2x_func_afex_update_params afex_update;
+		struct bnx2x_func_afex_viflists_params afex_viflists;
 		struct bnx2x_func_tx_start_params tx_start;
 	} params;
 };
@@ -1154,6 +1177,13 @@ struct bnx2x_func_sp_obj {
 	void			*rdata;
 	dma_addr_t		rdata_mapping;
 
+	/* Buffer to use as a afex ramrod data and its mapping.
+	 * This can't be same rdata as above because afex ramrod requests
+	 * can arrive to the object in parallel to other ramrod requests.
+	 */
+	void			*afex_rdata;
+	dma_addr_t		afex_rdata_mapping;
+
 	/* this mutex validates that when pending flag is taken, the next
 	 * ramrod to be sent will be the one set the pending bit
 	 */
@@ -1197,6 +1227,7 @@ union bnx2x_qable_obj {
 void bnx2x_init_func_obj(struct bnx2x *bp,
 			 struct bnx2x_func_sp_obj *obj,
 			 void *rdata, dma_addr_t rdata_mapping,
+			 void *afex_rdata, dma_addr_t afex_rdata_mapping,
 			 struct bnx2x_func_sp_drv_ops *drv_iface);
 
 int bnx2x_func_state_change(struct bnx2x *bp,
diff --git a/drivers/net/ethernet/broadcom/bnx2x/bnx2x_stats.c b/drivers/net/ethernet/broadcom/bnx2x/bnx2x_stats.c
index e1c9310fb07..1e2785cd11d 100644
--- a/drivers/net/ethernet/broadcom/bnx2x/bnx2x_stats.c
+++ b/drivers/net/ethernet/broadcom/bnx2x/bnx2x_stats.c
@@ -1316,7 +1316,7 @@ static void bnx2x_port_stats_base_init(struct bnx2x *bp)
  *
  * @param bp
  */
-static inline void bnx2x_prep_fw_stats_req(struct bnx2x *bp)
+static void bnx2x_prep_fw_stats_req(struct bnx2x *bp)
 {
 	int i;
 	int first_queue_query_index;
@@ -1561,3 +1561,274 @@ void bnx2x_save_statistics(struct bnx2x *bp)
 		UPDATE_FW_STAT_OLD(mac_discard);
 	}
 }
+
+void bnx2x_afex_collect_stats(struct bnx2x *bp, void *void_afex_stats,
+			      u32 stats_type)
+{
+	int i;
+	struct afex_stats *afex_stats = (struct afex_stats *)void_afex_stats;
+	struct bnx2x_eth_stats *estats = &bp->eth_stats;
+	struct per_queue_stats *fcoe_q_stats =
+		&bp->fw_stats_data->queue_stats[FCOE_IDX];
+
+	struct tstorm_per_queue_stats *fcoe_q_tstorm_stats =
+		&fcoe_q_stats->tstorm_queue_statistics;
+
+	struct ustorm_per_queue_stats *fcoe_q_ustorm_stats =
+		&fcoe_q_stats->ustorm_queue_statistics;
+
+	struct xstorm_per_queue_stats *fcoe_q_xstorm_stats =
+		&fcoe_q_stats->xstorm_queue_statistics;
+
+	struct fcoe_statistics_params *fw_fcoe_stat =
+		&bp->fw_stats_data->fcoe;
+
+	memset(afex_stats, 0, sizeof(struct afex_stats));
+
+	for_each_eth_queue(bp, i) {
+		struct bnx2x_fastpath *fp = &bp->fp[i];
+		struct bnx2x_eth_q_stats *qstats = &fp->eth_q_stats;
+
+		ADD_64(afex_stats->rx_unicast_bytes_hi,
+		       qstats->total_unicast_bytes_received_hi,
+		       afex_stats->rx_unicast_bytes_lo,
+		       qstats->total_unicast_bytes_received_lo);
+
+		ADD_64(afex_stats->rx_broadcast_bytes_hi,
+		       qstats->total_broadcast_bytes_received_hi,
+		       afex_stats->rx_broadcast_bytes_lo,
+		       qstats->total_broadcast_bytes_received_lo);
+
+		ADD_64(afex_stats->rx_multicast_bytes_hi,
+		       qstats->total_multicast_bytes_received_hi,
+		       afex_stats->rx_multicast_bytes_lo,
+		       qstats->total_multicast_bytes_received_lo);
+
+		ADD_64(afex_stats->rx_unicast_frames_hi,
+		       qstats->total_unicast_packets_received_hi,
+		       afex_stats->rx_unicast_frames_lo,
+		       qstats->total_unicast_packets_received_lo);
+
+		ADD_64(afex_stats->rx_broadcast_frames_hi,
+		       qstats->total_broadcast_packets_received_hi,
+		       afex_stats->rx_broadcast_frames_lo,
+		       qstats->total_broadcast_packets_received_lo);
+
+		ADD_64(afex_stats->rx_multicast_frames_hi,
+		       qstats->total_multicast_packets_received_hi,
+		       afex_stats->rx_multicast_frames_lo,
+		       qstats->total_multicast_packets_received_lo);
+
+		/* sum to rx_frames_discarded all discraded
+		 * packets due to size, ttl0 and checksum
+		 */
+		ADD_64(afex_stats->rx_frames_discarded_hi,
+		       qstats->total_packets_received_checksum_discarded_hi,
+		       afex_stats->rx_frames_discarded_lo,
+		       qstats->total_packets_received_checksum_discarded_lo);
+
+		ADD_64(afex_stats->rx_frames_discarded_hi,
+		       qstats->total_packets_received_ttl0_discarded_hi,
+		       afex_stats->rx_frames_discarded_lo,
+		       qstats->total_packets_received_ttl0_discarded_lo);
+
+		ADD_64(afex_stats->rx_frames_discarded_hi,
+		       qstats->etherstatsoverrsizepkts_hi,
+		       afex_stats->rx_frames_discarded_lo,
+		       qstats->etherstatsoverrsizepkts_lo);
+
+		ADD_64(afex_stats->rx_frames_dropped_hi,
+		       qstats->no_buff_discard_hi,
+		       afex_stats->rx_frames_dropped_lo,
+		       qstats->no_buff_discard_lo);
+
+		ADD_64(afex_stats->tx_unicast_bytes_hi,
+		       qstats->total_unicast_bytes_transmitted_hi,
+		       afex_stats->tx_unicast_bytes_lo,
+		       qstats->total_unicast_bytes_transmitted_lo);
+
+		ADD_64(afex_stats->tx_broadcast_bytes_hi,
+		       qstats->total_broadcast_bytes_transmitted_hi,
+		       afex_stats->tx_broadcast_bytes_lo,
+		       qstats->total_broadcast_bytes_transmitted_lo);
+
+		ADD_64(afex_stats->tx_multicast_bytes_hi,
+		       qstats->total_multicast_bytes_transmitted_hi,
+		       afex_stats->tx_multicast_bytes_lo,
+		       qstats->total_multicast_bytes_transmitted_lo);
+
+		ADD_64(afex_stats->tx_unicast_frames_hi,
+		       qstats->total_unicast_packets_transmitted_hi,
+		       afex_stats->tx_unicast_frames_lo,
+		       qstats->total_unicast_packets_transmitted_lo);
+
+		ADD_64(afex_stats->tx_broadcast_frames_hi,
+		       qstats->total_broadcast_packets_transmitted_hi,
+		       afex_stats->tx_broadcast_frames_lo,
+		       qstats->total_broadcast_packets_transmitted_lo);
+
+		ADD_64(afex_stats->tx_multicast_frames_hi,
+		       qstats->total_multicast_packets_transmitted_hi,
+		       afex_stats->tx_multicast_frames_lo,
+		       qstats->total_multicast_packets_transmitted_lo);
+
+		ADD_64(afex_stats->tx_frames_dropped_hi,
+		       qstats->total_transmitted_dropped_packets_error_hi,
+		       afex_stats->tx_frames_dropped_lo,
+		       qstats->total_transmitted_dropped_packets_error_lo);
+	}
+
+	/* now add FCoE statistics which are collected separately
+	 * (both offloaded and non offloaded)
+	 */
+	if (!NO_FCOE(bp)) {
+		ADD_64_LE(afex_stats->rx_unicast_bytes_hi,
+			  LE32_0,
+			  afex_stats->rx_unicast_bytes_lo,
+			  fw_fcoe_stat->rx_stat0.fcoe_rx_byte_cnt);
+
+		ADD_64_LE(afex_stats->rx_unicast_bytes_hi,
+			  fcoe_q_tstorm_stats->rcv_ucast_bytes.hi,
+			  afex_stats->rx_unicast_bytes_lo,
+			  fcoe_q_tstorm_stats->rcv_ucast_bytes.lo);
+
+		ADD_64_LE(afex_stats->rx_broadcast_bytes_hi,
+			  fcoe_q_tstorm_stats->rcv_bcast_bytes.hi,
+			  afex_stats->rx_broadcast_bytes_lo,
+			  fcoe_q_tstorm_stats->rcv_bcast_bytes.lo);
+
+		ADD_64_LE(afex_stats->rx_multicast_bytes_hi,
+			  fcoe_q_tstorm_stats->rcv_mcast_bytes.hi,
+			  afex_stats->rx_multicast_bytes_lo,
+			  fcoe_q_tstorm_stats->rcv_mcast_bytes.lo);
+
+		ADD_64_LE(afex_stats->rx_unicast_frames_hi,
+			  LE32_0,
+			  afex_stats->rx_unicast_frames_lo,
+			  fw_fcoe_stat->rx_stat0.fcoe_rx_pkt_cnt);
+
+		ADD_64_LE(afex_stats->rx_unicast_frames_hi,
+			  LE32_0,
+			  afex_stats->rx_unicast_frames_lo,
+			  fcoe_q_tstorm_stats->rcv_ucast_pkts);
+
+		ADD_64_LE(afex_stats->rx_broadcast_frames_hi,
+			  LE32_0,
+			  afex_stats->rx_broadcast_frames_lo,
+			  fcoe_q_tstorm_stats->rcv_bcast_pkts);
+
+		ADD_64_LE(afex_stats->rx_multicast_frames_hi,
+			  LE32_0,
+			  afex_stats->rx_multicast_frames_lo,
+			  fcoe_q_tstorm_stats->rcv_ucast_pkts);
+
+		ADD_64_LE(afex_stats->rx_frames_discarded_hi,
+			  LE32_0,
+			  afex_stats->rx_frames_discarded_lo,
+			  fcoe_q_tstorm_stats->checksum_discard);
+
+		ADD_64_LE(afex_stats->rx_frames_discarded_hi,
+			  LE32_0,
+			  afex_stats->rx_frames_discarded_lo,
+			  fcoe_q_tstorm_stats->pkts_too_big_discard);
+
+		ADD_64_LE(afex_stats->rx_frames_discarded_hi,
+			  LE32_0,
+			  afex_stats->rx_frames_discarded_lo,
+			  fcoe_q_tstorm_stats->ttl0_discard);
+
+		ADD_64_LE16(afex_stats->rx_frames_dropped_hi,
+			    LE16_0,
+			    afex_stats->rx_frames_dropped_lo,
+			    fcoe_q_tstorm_stats->no_buff_discard);
+
+		ADD_64_LE(afex_stats->rx_frames_dropped_hi,
+			  LE32_0,
+			  afex_stats->rx_frames_dropped_lo,
+			  fcoe_q_ustorm_stats->ucast_no_buff_pkts);
+
+		ADD_64_LE(afex_stats->rx_frames_dropped_hi,
+			  LE32_0,
+			  afex_stats->rx_frames_dropped_lo,
+			  fcoe_q_ustorm_stats->mcast_no_buff_pkts);
+
+		ADD_64_LE(afex_stats->rx_frames_dropped_hi,
+			  LE32_0,
+			  afex_stats->rx_frames_dropped_lo,
+			  fcoe_q_ustorm_stats->bcast_no_buff_pkts);
+
+		ADD_64_LE(afex_stats->rx_frames_dropped_hi,
+			  LE32_0,
+			  afex_stats->rx_frames_dropped_lo,
+			  fw_fcoe_stat->rx_stat1.fcoe_rx_drop_pkt_cnt);
+
+		ADD_64_LE(afex_stats->rx_frames_dropped_hi,
+			  LE32_0,
+			  afex_stats->rx_frames_dropped_lo,
+			  fw_fcoe_stat->rx_stat2.fcoe_rx_drop_pkt_cnt);
+
+		ADD_64_LE(afex_stats->tx_unicast_bytes_hi,
+			  LE32_0,
+			  afex_stats->tx_unicast_bytes_lo,
+			  fw_fcoe_stat->tx_stat.fcoe_tx_byte_cnt);
+
+		ADD_64_LE(afex_stats->tx_unicast_bytes_hi,
+			  fcoe_q_xstorm_stats->ucast_bytes_sent.hi,
+			  afex_stats->tx_unicast_bytes_lo,
+			  fcoe_q_xstorm_stats->ucast_bytes_sent.lo);
+
+		ADD_64_LE(afex_stats->tx_broadcast_bytes_hi,
+			  fcoe_q_xstorm_stats->bcast_bytes_sent.hi,
+			  afex_stats->tx_broadcast_bytes_lo,
+			  fcoe_q_xstorm_stats->bcast_bytes_sent.lo);
+
+		ADD_64_LE(afex_stats->tx_multicast_bytes_hi,
+			  fcoe_q_xstorm_stats->mcast_bytes_sent.hi,
+			  afex_stats->tx_multicast_bytes_lo,
+			  fcoe_q_xstorm_stats->mcast_bytes_sent.lo);
+
+		ADD_64_LE(afex_stats->tx_unicast_frames_hi,
+			  LE32_0,
+			  afex_stats->tx_unicast_frames_lo,
+			  fw_fcoe_stat->tx_stat.fcoe_tx_pkt_cnt);
+
+		ADD_64_LE(afex_stats->tx_unicast_frames_hi,
+			  LE32_0,
+			  afex_stats->tx_unicast_frames_lo,
+			  fcoe_q_xstorm_stats->ucast_pkts_sent);
+
+		ADD_64_LE(afex_stats->tx_broadcast_frames_hi,
+			  LE32_0,
+			  afex_stats->tx_broadcast_frames_lo,
+			  fcoe_q_xstorm_stats->bcast_pkts_sent);
+
+		ADD_64_LE(afex_stats->tx_multicast_frames_hi,
+			  LE32_0,
+			  afex_stats->tx_multicast_frames_lo,
+			  fcoe_q_xstorm_stats->mcast_pkts_sent);
+
+		ADD_64_LE(afex_stats->tx_frames_dropped_hi,
+			  LE32_0,
+			  afex_stats->tx_frames_dropped_lo,
+			  fcoe_q_xstorm_stats->error_drop_pkts);
+	}
+
+	/* if port stats are requested, add them to the PMF
+	 * stats, as anyway they will be accumulated by the
+	 * MCP before sent to the switch
+	 */
+	if ((bp->port.pmf) && (stats_type == VICSTATST_UIF_INDEX)) {
+		ADD_64(afex_stats->rx_frames_dropped_hi,
+		       0,
+		       afex_stats->rx_frames_dropped_lo,
+		       estats->mac_filter_discard);
+		ADD_64(afex_stats->rx_frames_dropped_hi,
+		       0,
+		       afex_stats->rx_frames_dropped_lo,
+		       estats->brb_truncate_discard);
+		ADD_64(afex_stats->rx_frames_discarded_hi,
+		       0,
+		       afex_stats->rx_frames_discarded_lo,
+		       estats->mac_discard);
+	}
+}
diff --git a/drivers/net/ethernet/broadcom/bnx2x/bnx2x_stats.h b/drivers/net/ethernet/broadcom/bnx2x/bnx2x_stats.h
index 2b46e1eb7fd..93e689fdfed 100644
--- a/drivers/net/ethernet/broadcom/bnx2x/bnx2x_stats.h
+++ b/drivers/net/ethernet/broadcom/bnx2x/bnx2x_stats.h
@@ -338,6 +338,18 @@ struct bnx2x_fw_port_stats_old {
 		s_hi += a_hi + ((s_lo < a_lo) ? 1 : 0); \
 	} while (0)
 
+#define LE32_0 ((__force __le32) 0)
+#define LE16_0 ((__force __le16) 0)
+
+/* The _force is for cases where high value is 0 */
+#define ADD_64_LE(s_hi, a_hi_le, s_lo, a_lo_le) \
+		ADD_64(s_hi, le32_to_cpu(a_hi_le), \
+		       s_lo, le32_to_cpu(a_lo_le))
+
+#define ADD_64_LE16(s_hi, a_hi_le, s_lo, a_lo_le) \
+		ADD_64(s_hi, le16_to_cpu(a_hi_le), \
+		       s_lo, le16_to_cpu(a_lo_le))
+
 /* difference = minuend - subtrahend */
 #define DIFF_64(d_hi, m_hi, s_hi, d_lo, m_lo, s_lo) \
 	do { \
@@ -529,4 +541,7 @@ void bnx2x_stats_handle(struct bnx2x *bp, enum bnx2x_stats_event event);
  * @bp:		driver handle
  */
 void bnx2x_save_statistics(struct bnx2x *bp);
+
+void bnx2x_afex_collect_stats(struct bnx2x *bp, void *void_afex_stats,
+			      u32 stats_type);
 #endif /* BNX2X_STATS_H */
diff --git a/drivers/net/ethernet/broadcom/tg3.c b/drivers/net/ethernet/broadcom/tg3.c
index ceeab8e852e..edeeb516807 100644
--- a/drivers/net/ethernet/broadcom/tg3.c
+++ b/drivers/net/ethernet/broadcom/tg3.c
@@ -5622,17 +5622,29 @@ static void tg3_tx(struct tg3_napi *tnapi)
 	}
 }
 
+static void tg3_frag_free(bool is_frag, void *data)
+{
+	if (is_frag)
+		put_page(virt_to_head_page(data));
+	else
+		kfree(data);
+}
+
 static void tg3_rx_data_free(struct tg3 *tp, struct ring_info *ri, u32 map_sz)
 {
+	unsigned int skb_size = SKB_DATA_ALIGN(map_sz + TG3_RX_OFFSET(tp)) +
+		   SKB_DATA_ALIGN(sizeof(struct skb_shared_info));
+
 	if (!ri->data)
 		return;
 
 	pci_unmap_single(tp->pdev, dma_unmap_addr(ri, mapping),
 			 map_sz, PCI_DMA_FROMDEVICE);
-	kfree(ri->data);
+	tg3_frag_free(skb_size <= PAGE_SIZE, ri->data);
 	ri->data = NULL;
 }
 
+
 /* Returns size of skb allocated or < 0 on error.
  *
  * We only need to fill in the address because the other members
@@ -5645,7 +5657,8 @@ static void tg3_rx_data_free(struct tg3 *tp, struct ring_info *ri, u32 map_sz)
  * (to fetch the error flags, vlan tag, checksum, and opaque cookie).
  */
 static int tg3_alloc_rx_data(struct tg3 *tp, struct tg3_rx_prodring_set *tpr,
-			    u32 opaque_key, u32 dest_idx_unmasked)
+			     u32 opaque_key, u32 dest_idx_unmasked,
+			     unsigned int *frag_size)
 {
 	struct tg3_rx_buffer_desc *desc;
 	struct ring_info *map;
@@ -5680,7 +5693,13 @@ static int tg3_alloc_rx_data(struct tg3 *tp, struct tg3_rx_prodring_set *tpr,
 	 */
 	skb_size = SKB_DATA_ALIGN(data_size + TG3_RX_OFFSET(tp)) +
 		   SKB_DATA_ALIGN(sizeof(struct skb_shared_info));
-	data = kmalloc(skb_size, GFP_ATOMIC);
+	if (skb_size <= PAGE_SIZE) {
+		data = netdev_alloc_frag(skb_size);
+		*frag_size = skb_size;
+	} else {
+		data = kmalloc(skb_size, GFP_ATOMIC);
+		*frag_size = 0;
+	}
 	if (!data)
 		return -ENOMEM;
 
@@ -5688,8 +5707,8 @@ static int tg3_alloc_rx_data(struct tg3 *tp, struct tg3_rx_prodring_set *tpr,
 				 data + TG3_RX_OFFSET(tp),
 				 data_size,
 				 PCI_DMA_FROMDEVICE);
-	if (pci_dma_mapping_error(tp->pdev, mapping)) {
-		kfree(data);
+	if (unlikely(pci_dma_mapping_error(tp->pdev, mapping))) {
+		tg3_frag_free(skb_size <= PAGE_SIZE, data);
 		return -EIO;
 	}
 
@@ -5840,18 +5859,19 @@ static int tg3_rx(struct tg3_napi *tnapi, int budget)
 
 		if (len > TG3_RX_COPY_THRESH(tp)) {
 			int skb_size;
+			unsigned int frag_size;
 
 			skb_size = tg3_alloc_rx_data(tp, tpr, opaque_key,
-						    *post_ptr);
+						    *post_ptr, &frag_size);
 			if (skb_size < 0)
 				goto drop_it;
 
 			pci_unmap_single(tp->pdev, dma_addr, skb_size,
 					 PCI_DMA_FROMDEVICE);
 
-			skb = build_skb(data);
+			skb = build_skb(data, frag_size);
 			if (!skb) {
-				kfree(data);
+				tg3_frag_free(frag_size != 0, data);
 				goto drop_it_no_recycle;
 			}
 			skb_reserve(skb, TG3_RX_OFFSET(tp));
@@ -7287,7 +7307,10 @@ static int tg3_rx_prodring_alloc(struct tg3 *tp,
 
 	/* Now allocate fresh SKBs for each rx ring. */
 	for (i = 0; i < tp->rx_pending; i++) {
-		if (tg3_alloc_rx_data(tp, tpr, RXD_OPAQUE_RING_STD, i) < 0) {
+		unsigned int frag_size;
+
+		if (tg3_alloc_rx_data(tp, tpr, RXD_OPAQUE_RING_STD, i,
+				      &frag_size) < 0) {
 			netdev_warn(tp->dev,
 				    "Using a smaller RX standard ring. Only "
 				    "%d out of %d buffers were allocated "
@@ -7319,7 +7342,10 @@ static int tg3_rx_prodring_alloc(struct tg3 *tp,
 	}
 
 	for (i = 0; i < tp->rx_jumbo_pending; i++) {
-		if (tg3_alloc_rx_data(tp, tpr, RXD_OPAQUE_RING_JUMBO, i) < 0) {
+		unsigned int frag_size;
+
+		if (tg3_alloc_rx_data(tp, tpr, RXD_OPAQUE_RING_JUMBO, i,
+				      &frag_size) < 0) {
 			netdev_warn(tp->dev,
 				    "Using a smaller RX jumbo ring. Only %d "
 				    "out of %d buffers were allocated "
@@ -12248,6 +12274,7 @@ static const struct ethtool_ops tg3_ethtool_ops = {
 	.get_rxfh_indir_size    = tg3_get_rxfh_indir_size,
 	.get_rxfh_indir		= tg3_get_rxfh_indir,
 	.set_rxfh_indir		= tg3_set_rxfh_indir,
+	.get_ts_info		= ethtool_op_get_ts_info,
 };
 
 static struct rtnl_link_stats64 *tg3_get_stats64(struct net_device *dev,
@@ -15856,8 +15883,7 @@ static void __devexit tg3_remove_one(struct pci_dev *pdev)
 	if (dev) {
 		struct tg3 *tp = netdev_priv(dev);
 
-		if (tp->fw)
-			release_firmware(tp->fw);
+		release_firmware(tp->fw);
 
 		tg3_reset_task_cancel(tp);
 
diff --git a/drivers/net/ethernet/brocade/bna/bfa_ioc.c b/drivers/net/ethernet/brocade/bna/bfa_ioc.c
index 77977d735dd..0b640fafbda 100644
--- a/drivers/net/ethernet/brocade/bna/bfa_ioc.c
+++ b/drivers/net/ethernet/brocade/bna/bfa_ioc.c
@@ -70,7 +70,6 @@ static void bfa_ioc_reset(struct bfa_ioc *ioc, bool force);
 static void bfa_ioc_mbox_poll(struct bfa_ioc *ioc);
 static void bfa_ioc_mbox_flush(struct bfa_ioc *ioc);
 static void bfa_ioc_recover(struct bfa_ioc *ioc);
-static void bfa_ioc_check_attr_wwns(struct bfa_ioc *ioc);
 static void bfa_ioc_event_notify(struct bfa_ioc *, enum bfa_ioc_event);
 static void bfa_ioc_disable_comp(struct bfa_ioc *ioc);
 static void bfa_ioc_lpu_stop(struct bfa_ioc *ioc);
@@ -346,8 +345,6 @@ bfa_ioc_sm_getattr(struct bfa_ioc *ioc, enum ioc_event event)
 	switch (event) {
 	case IOC_E_FWRSP_GETATTR:
 		del_timer(&ioc->ioc_timer);
-		bfa_ioc_check_attr_wwns(ioc);
-		bfa_ioc_hb_monitor(ioc);
 		bfa_fsm_set_state(ioc, bfa_ioc_sm_op);
 		break;
 
@@ -380,6 +377,7 @@ bfa_ioc_sm_op_entry(struct bfa_ioc *ioc)
 {
 	ioc->cbfn->enable_cbfn(ioc->bfa, BFA_STATUS_OK);
 	bfa_ioc_event_notify(ioc, BFA_IOC_E_ENABLED);
+	bfa_ioc_hb_monitor(ioc);
 }
 
 static void
@@ -1207,27 +1205,62 @@ bfa_nw_ioc_sem_release(void __iomem *sem_reg)
 	writel(1, sem_reg);
 }
 
+/* Clear fwver hdr */
+static void
+bfa_ioc_fwver_clear(struct bfa_ioc *ioc)
+{
+	u32 pgnum, pgoff, loff = 0;
+	int i;
+
+	pgnum = PSS_SMEM_PGNUM(ioc->ioc_regs.smem_pg0, loff);
+	pgoff = PSS_SMEM_PGOFF(loff);
+	writel(pgnum, ioc->ioc_regs.host_page_num_fn);
+
+	for (i = 0; i < (sizeof(struct bfi_ioc_image_hdr) / sizeof(u32)); i++) {
+		writel(0, ioc->ioc_regs.smem_page_start + loff);
+		loff += sizeof(u32);
+	}
+}
+
+
 static void
 bfa_ioc_hw_sem_init(struct bfa_ioc *ioc)
 {
 	struct bfi_ioc_image_hdr fwhdr;
-	u32 fwstate = readl(ioc->ioc_regs.ioc_fwstate);
+	u32 fwstate, r32;
 
-	if (fwstate == BFI_IOC_UNINIT)
+	/* Spin on init semaphore to serialize. */
+	r32 = readl(ioc->ioc_regs.ioc_init_sem_reg);
+	while (r32 & 0x1) {
+		udelay(20);
+		r32 = readl(ioc->ioc_regs.ioc_init_sem_reg);
+	}
+
+	fwstate = readl(ioc->ioc_regs.ioc_fwstate);
+	if (fwstate == BFI_IOC_UNINIT) {
+		writel(1, ioc->ioc_regs.ioc_init_sem_reg);
 		return;
+	}
 
 	bfa_nw_ioc_fwver_get(ioc, &fwhdr);
 
-	if (swab32(fwhdr.exec) == BFI_FWBOOT_TYPE_NORMAL)
+	if (swab32(fwhdr.exec) == BFI_FWBOOT_TYPE_NORMAL) {
+		writel(1, ioc->ioc_regs.ioc_init_sem_reg);
 		return;
+	}
 
+	bfa_ioc_fwver_clear(ioc);
 	writel(BFI_IOC_UNINIT, ioc->ioc_regs.ioc_fwstate);
+	writel(BFI_IOC_UNINIT, ioc->ioc_regs.alt_ioc_fwstate);
 
 	/*
 	 * Try to lock and then unlock the semaphore.
 	 */
 	readl(ioc->ioc_regs.ioc_sem_reg);
 	writel(1, ioc->ioc_regs.ioc_sem_reg);
+
+	/* Unlock init semaphore */
+	writel(1, ioc->ioc_regs.ioc_init_sem_reg);
 }
 
 static void
@@ -1585,11 +1618,6 @@ bfa_ioc_download_fw(struct bfa_ioc *ioc, u32 boot_type,
 	u32 i;
 	u32 asicmode;
 
-	/**
-	 * Initialize LMEM first before code download
-	 */
-	bfa_ioc_lmem_init(ioc);
-
 	fwimg = bfa_cb_image_get_chunk(bfa_ioc_asic_gen(ioc), chunkno);
 
 	pgnum = bfa_ioc_smem_pgnum(ioc, loff);
@@ -1914,6 +1942,10 @@ bfa_ioc_pll_init(struct bfa_ioc *ioc)
 	bfa_ioc_pll_init_asic(ioc);
 
 	ioc->pllinit = true;
+
+	/* Initialize LMEM */
+	bfa_ioc_lmem_init(ioc);
+
 	/*
 	 *  release semaphore.
 	 */
@@ -2513,13 +2545,6 @@ bfa_ioc_recover(struct bfa_ioc *ioc)
 	bfa_fsm_send_event(ioc, IOC_E_HBFAIL);
 }
 
-static void
-bfa_ioc_check_attr_wwns(struct bfa_ioc *ioc)
-{
-	if (bfa_ioc_get_type(ioc) == BFA_IOC_TYPE_LL)
-		return;
-}
-
 /**
  * @dg hal_iocpf_pvt BFA IOC PF private functions
  * @{
diff --git a/drivers/net/ethernet/brocade/bna/bfa_ioc_ct.c b/drivers/net/ethernet/brocade/bna/bfa_ioc_ct.c
index 348479bbfa3..b6b036a143a 100644
--- a/drivers/net/ethernet/brocade/bna/bfa_ioc_ct.c
+++ b/drivers/net/ethernet/brocade/bna/bfa_ioc_ct.c
@@ -199,9 +199,9 @@ bfa_ioc_ct_notify_fail(struct bfa_ioc *ioc)
  * Host to LPU mailbox message addresses
  */
 static const struct {
-	u32 	hfn_mbox;
-	u32 	lpu_mbox;
-	u32 	hfn_pgn;
+	u32	hfn_mbox;
+	u32	lpu_mbox;
+	u32	hfn_pgn;
 } ct_fnreg[] = {
 	{ HOSTFN0_LPU_MBOX0_0, LPU_HOSTFN0_MBOX0_0, HOST_PAGE_NUM_FN0 },
 	{ HOSTFN1_LPU_MBOX0_8, LPU_HOSTFN1_MBOX0_8, HOST_PAGE_NUM_FN1 },
@@ -803,17 +803,72 @@ bfa_ioc_ct2_mac_reset(void __iomem *rb)
 }
 
 #define CT2_NFC_MAX_DELAY       1000
+#define CT2_NFC_VER_VALID       0x143
+#define BFA_IOC_PLL_POLL        1000000
+
+static bool
+bfa_ioc_ct2_nfc_halted(void __iomem *rb)
+{
+	volatile u32 r32;
+
+	r32 = readl(rb + CT2_NFC_CSR_SET_REG);
+	if (r32 & __NFC_CONTROLLER_HALTED)
+		return true;
+
+	return false;
+}
+
+static void
+bfa_ioc_ct2_nfc_resume(void __iomem *rb)
+{
+	volatile u32 r32;
+	int i;
+
+	writel(__HALT_NFC_CONTROLLER, rb + CT2_NFC_CSR_CLR_REG);
+	for (i = 0; i < CT2_NFC_MAX_DELAY; i++) {
+		r32 = readl(rb + CT2_NFC_CSR_SET_REG);
+		if (!(r32 & __NFC_CONTROLLER_HALTED))
+			return;
+		udelay(1000);
+	}
+	BUG_ON(1);
+}
+
 static enum bfa_status
 bfa_ioc_ct2_pll_init(void __iomem *rb, enum bfi_asic_mode asic_mode)
 {
 	volatile u32 wgn, r32;
-	int i;
+	u32 nfc_ver, i;
 
-	/*
-	 * Initialize PLL if not already done by NFC
-	 */
 	wgn = readl(rb + CT2_WGN_STATUS);
-	if (!(wgn & __GLBL_PF_VF_CFG_RDY)) {
+
+	nfc_ver = readl(rb + CT2_RSC_GPR15_REG);
+
+	if ((wgn == (__A2T_AHB_LOAD | __WGN_READY)) &&
+		(nfc_ver >= CT2_NFC_VER_VALID)) {
+		if (bfa_ioc_ct2_nfc_halted(rb))
+			bfa_ioc_ct2_nfc_resume(rb);
+		writel(__RESET_AND_START_SCLK_LCLK_PLLS,
+				rb + CT2_CSI_FW_CTL_SET_REG);
+
+		for (i = 0; i < BFA_IOC_PLL_POLL; i++) {
+			r32 = readl(rb + CT2_APP_PLL_LCLK_CTL_REG);
+			if (r32 & __RESET_AND_START_SCLK_LCLK_PLLS)
+				break;
+		}
+		BUG_ON(!(r32 & __RESET_AND_START_SCLK_LCLK_PLLS));
+
+		for (i = 0; i < BFA_IOC_PLL_POLL; i++) {
+			r32 = readl(rb + CT2_APP_PLL_LCLK_CTL_REG);
+			if (!(r32 & __RESET_AND_START_SCLK_LCLK_PLLS))
+				break;
+		}
+		BUG_ON(r32 & __RESET_AND_START_SCLK_LCLK_PLLS);
+		udelay(1000);
+
+		r32 = readl(rb + CT2_CSI_FW_CTL_REG);
+		BUG_ON(r32 & __RESET_AND_START_SCLK_LCLK_PLLS);
+	} else {
 		writel(__HALT_NFC_CONTROLLER, (rb + CT2_NFC_CSR_SET_REG));
 		for (i = 0; i < CT2_NFC_MAX_DELAY; i++) {
 			r32 = readl(rb + CT2_NFC_CSR_SET_REG);
@@ -821,53 +876,48 @@ bfa_ioc_ct2_pll_init(void __iomem *rb, enum bfi_asic_mode asic_mode)
 				break;
 			udelay(1000);
 		}
+
+		bfa_ioc_ct2_mac_reset(rb);
+		bfa_ioc_ct2_sclk_init(rb);
+		bfa_ioc_ct2_lclk_init(rb);
+
+		/* release soft reset on s_clk & l_clk */
+		r32 = readl((rb + CT2_APP_PLL_SCLK_CTL_REG));
+		writel(r32 & ~__APP_PLL_SCLK_LOGIC_SOFT_RESET,
+				rb + CT2_APP_PLL_SCLK_CTL_REG);
+		r32 = readl((rb + CT2_APP_PLL_LCLK_CTL_REG));
+		writel(r32 & ~__APP_PLL_LCLK_LOGIC_SOFT_RESET,
+				rb + CT2_APP_PLL_LCLK_CTL_REG);
+	}
+
+	/* Announce flash device presence, if flash was corrupted. */
+	if (wgn == (__WGN_READY | __GLBL_PF_VF_CFG_RDY)) {
+		r32 = readl((rb + PSS_GPIO_OUT_REG));
+		writel(r32 & ~1, rb + PSS_GPIO_OUT_REG);
+		r32 = readl((rb + PSS_GPIO_OE_REG));
+		writel(r32 | 1, rb + PSS_GPIO_OE_REG);
 	}
 
 	/*
 	 * Mask the interrupts and clear any
 	 * pending interrupts left by BIOS/EFI
 	 */
-
 	writel(1, (rb + CT2_LPU0_HOSTFN_MBOX0_MSK));
 	writel(1, (rb + CT2_LPU1_HOSTFN_MBOX0_MSK));
 
-	r32 = readl((rb + CT2_LPU0_HOSTFN_CMD_STAT));
-	if (r32 == 1) {
-		writel(1, (rb + CT2_LPU0_HOSTFN_CMD_STAT));
-		readl((rb + CT2_LPU0_HOSTFN_CMD_STAT));
-	}
-	r32 = readl((rb + CT2_LPU1_HOSTFN_CMD_STAT));
-	if (r32 == 1) {
-		writel(1, (rb + CT2_LPU1_HOSTFN_CMD_STAT));
-		readl((rb + CT2_LPU1_HOSTFN_CMD_STAT));
-	}
-
-	bfa_ioc_ct2_mac_reset(rb);
-	bfa_ioc_ct2_sclk_init(rb);
-	bfa_ioc_ct2_lclk_init(rb);
-
-	/*
-	 * release soft reset on s_clk & l_clk
-	 */
-	r32 = readl((rb + CT2_APP_PLL_SCLK_CTL_REG));
-	writel((r32 & ~__APP_PLL_SCLK_LOGIC_SOFT_RESET),
-			(rb + CT2_APP_PLL_SCLK_CTL_REG));
-
-	/*
-	 * release soft reset on s_clk & l_clk
-	 */
-	r32 = readl((rb + CT2_APP_PLL_LCLK_CTL_REG));
-	writel(r32 & ~__APP_PLL_LCLK_LOGIC_SOFT_RESET,
-		      (rb + CT2_APP_PLL_LCLK_CTL_REG));
-
-	/*
-	 * Announce flash device presence, if flash was corrupted.
-	 */
-	if (wgn == (__WGN_READY | __GLBL_PF_VF_CFG_RDY)) {
-		r32 = readl((rb + PSS_GPIO_OUT_REG));
-		writel((r32 & ~1), (rb + PSS_GPIO_OUT_REG));
-		r32 = readl((rb + PSS_GPIO_OE_REG));
-		writel((r32 | 1), (rb + PSS_GPIO_OE_REG));
+	/* For first time initialization, no need to clear interrupts */
+	r32 = readl(rb + HOST_SEM5_REG);
+	if (r32 & 0x1) {
+		r32 = readl((rb + CT2_LPU0_HOSTFN_CMD_STAT));
+		if (r32 == 1) {
+			writel(1, (rb + CT2_LPU0_HOSTFN_CMD_STAT));
+			readl((rb + CT2_LPU0_HOSTFN_CMD_STAT));
+		}
+		r32 = readl((rb + CT2_LPU1_HOSTFN_CMD_STAT));
+		if (r32 == 1) {
+			writel(1, (rb + CT2_LPU1_HOSTFN_CMD_STAT));
+			readl((rb + CT2_LPU1_HOSTFN_CMD_STAT));
+		}
 	}
 
 	bfa_ioc_ct2_mem_init(rb);
diff --git a/drivers/net/ethernet/brocade/bna/bfi_reg.h b/drivers/net/ethernet/brocade/bna/bfi_reg.h
index efacff3ab51..0e094fe46df 100644
--- a/drivers/net/ethernet/brocade/bna/bfi_reg.h
+++ b/drivers/net/ethernet/brocade/bna/bfi_reg.h
@@ -339,10 +339,16 @@ enum {
 #define __A2T_AHB_LOAD			0x00000800
 #define __WGN_READY			0x00000400
 #define __GLBL_PF_VF_CFG_RDY		0x00000200
+#define CT2_NFC_CSR_CLR_REG             0x00027420
 #define CT2_NFC_CSR_SET_REG		0x00027424
 #define __HALT_NFC_CONTROLLER		0x00000002
 #define __NFC_CONTROLLER_HALTED		0x00001000
 
+#define CT2_RSC_GPR15_REG		0x0002765c
+#define CT2_CSI_FW_CTL_REG              0x00027080
+#define __RESET_AND_START_SCLK_LCLK_PLLS 0x00010000
+#define CT2_CSI_FW_CTL_SET_REG          0x00027088
+
 #define CT2_CSI_MAC0_CONTROL_REG	0x000270d0
 #define __CSI_MAC_RESET			0x00000010
 #define __CSI_MAC_AHB_RESET		0x00000008
diff --git a/drivers/net/ethernet/brocade/bna/bnad.c b/drivers/net/ethernet/brocade/bna/bnad.c
index ff78f770dec..67cd2ed0306 100644
--- a/drivers/net/ethernet/brocade/bna/bnad.c
+++ b/drivers/net/ethernet/brocade/bna/bnad.c
@@ -80,8 +80,6 @@ do {								\
 	(sizeof(struct bnad_skb_unmap) * ((_depth) - 1));	\
 } while (0)
 
-#define BNAD_TXRX_SYNC_MDELAY	250	/* 250 msecs */
-
 static void
 bnad_add_to_list(struct bnad *bnad)
 {
@@ -103,7 +101,7 @@ bnad_remove_from_list(struct bnad *bnad)
  * Reinitialize completions in CQ, once Rx is taken down
  */
 static void
-bnad_cq_cmpl_init(struct bnad *bnad, struct bna_ccb *ccb)
+bnad_cq_cleanup(struct bnad *bnad, struct bna_ccb *ccb)
 {
 	struct bna_cq_entry *cmpl, *next_cmpl;
 	unsigned int wi_range, wis = 0, ccb_prod = 0;
@@ -141,7 +139,8 @@ bnad_pci_unmap_skb(struct device *pdev, struct bnad_skb_unmap *array,
 
 	for (j = 0; j < frag; j++) {
 		dma_unmap_page(pdev, dma_unmap_addr(&array[index], dma_addr),
-			  skb_frag_size(&skb_shinfo(skb)->frags[j]), DMA_TO_DEVICE);
+			  skb_frag_size(&skb_shinfo(skb)->frags[j]),
+						DMA_TO_DEVICE);
 		dma_unmap_addr_set(&array[index], dma_addr, 0);
 		BNA_QE_INDX_ADD(index, 1, depth);
 	}
@@ -155,7 +154,7 @@ bnad_pci_unmap_skb(struct device *pdev, struct bnad_skb_unmap *array,
  * so DMA unmap & freeing is fine.
  */
 static void
-bnad_free_all_txbufs(struct bnad *bnad,
+bnad_txq_cleanup(struct bnad *bnad,
 		 struct bna_tcb *tcb)
 {
 	u32		unmap_cons;
@@ -183,13 +182,12 @@ bnad_free_all_txbufs(struct bnad *bnad,
 /* Data Path Handlers */
 
 /*
- * bnad_free_txbufs : Frees the Tx bufs on Tx completion
+ * bnad_txcmpl_process : Frees the Tx bufs on Tx completion
  * Can be called in a) Interrupt context
  *		    b) Sending context
- *		    c) Tasklet context
  */
 static u32
-bnad_free_txbufs(struct bnad *bnad,
+bnad_txcmpl_process(struct bnad *bnad,
 		 struct bna_tcb *tcb)
 {
 	u32		unmap_cons, sent_packets = 0, sent_bytes = 0;
@@ -198,13 +196,7 @@ bnad_free_txbufs(struct bnad *bnad,
 	struct bnad_skb_unmap *unmap_array;
 	struct sk_buff		*skb;
 
-	/*
-	 * Just return if TX is stopped. This check is useful
-	 * when bnad_free_txbufs() runs out of a tasklet scheduled
-	 * before bnad_cb_tx_cleanup() cleared BNAD_TXQ_TX_STARTED bit
-	 * but this routine runs actually after the cleanup has been
-	 * executed.
-	 */
+	/* Just return if TX is stopped */
 	if (!test_bit(BNAD_TXQ_TX_STARTED, &tcb->flags))
 		return 0;
 
@@ -243,57 +235,8 @@ bnad_free_txbufs(struct bnad *bnad,
 	return sent_packets;
 }
 
-/* Tx Free Tasklet function */
-/* Frees for all the tcb's in all the Tx's */
-/*
- * Scheduled from sending context, so that
- * the fat Tx lock is not held for too long
- * in the sending context.
- */
-static void
-bnad_tx_free_tasklet(unsigned long bnad_ptr)
-{
-	struct bnad *bnad = (struct bnad *)bnad_ptr;
-	struct bna_tcb *tcb;
-	u32		acked = 0;
-	int			i, j;
-
-	for (i = 0; i < bnad->num_tx; i++) {
-		for (j = 0; j < bnad->num_txq_per_tx; j++) {
-			tcb = bnad->tx_info[i].tcb[j];
-			if (!tcb)
-				continue;
-			if (((u16) (*tcb->hw_consumer_index) !=
-				tcb->consumer_index) &&
-				(!test_and_set_bit(BNAD_TXQ_FREE_SENT,
-						  &tcb->flags))) {
-				acked = bnad_free_txbufs(bnad, tcb);
-				if (likely(test_bit(BNAD_TXQ_TX_STARTED,
-					&tcb->flags)))
-					bna_ib_ack(tcb->i_dbell, acked);
-				smp_mb__before_clear_bit();
-				clear_bit(BNAD_TXQ_FREE_SENT, &tcb->flags);
-			}
-			if (unlikely(!test_bit(BNAD_TXQ_TX_STARTED,
-						&tcb->flags)))
-				continue;
-			if (netif_queue_stopped(bnad->netdev)) {
-				if (acked && netif_carrier_ok(bnad->netdev) &&
-					BNA_QE_FREE_CNT(tcb, tcb->q_depth) >=
-						BNAD_NETIF_WAKE_THRESHOLD) {
-					netif_wake_queue(bnad->netdev);
-					/* TODO */
-					/* Counters for individual TxQs? */
-					BNAD_UPDATE_CTR(bnad,
-						netif_queue_wakeup);
-				}
-			}
-		}
-	}
-}
-
 static u32
-bnad_tx(struct bnad *bnad, struct bna_tcb *tcb)
+bnad_tx_complete(struct bnad *bnad, struct bna_tcb *tcb)
 {
 	struct net_device *netdev = bnad->netdev;
 	u32 sent = 0;
@@ -301,7 +244,7 @@ bnad_tx(struct bnad *bnad, struct bna_tcb *tcb)
 	if (test_and_set_bit(BNAD_TXQ_FREE_SENT, &tcb->flags))
 		return 0;
 
-	sent = bnad_free_txbufs(bnad, tcb);
+	sent = bnad_txcmpl_process(bnad, tcb);
 	if (sent) {
 		if (netif_queue_stopped(netdev) &&
 		    netif_carrier_ok(netdev) &&
@@ -330,13 +273,13 @@ bnad_msix_tx(int irq, void *data)
 	struct bna_tcb *tcb = (struct bna_tcb *)data;
 	struct bnad *bnad = tcb->bnad;
 
-	bnad_tx(bnad, tcb);
+	bnad_tx_complete(bnad, tcb);
 
 	return IRQ_HANDLED;
 }
 
 static void
-bnad_reset_rcb(struct bnad *bnad, struct bna_rcb *rcb)
+bnad_rcb_cleanup(struct bnad *bnad, struct bna_rcb *rcb)
 {
 	struct bnad_unmap_q *unmap_q = rcb->unmap_q;
 
@@ -348,7 +291,7 @@ bnad_reset_rcb(struct bnad *bnad, struct bna_rcb *rcb)
 }
 
 static void
-bnad_free_all_rxbufs(struct bnad *bnad, struct bna_rcb *rcb)
+bnad_rxq_cleanup(struct bnad *bnad, struct bna_rcb *rcb)
 {
 	struct bnad_unmap_q *unmap_q;
 	struct bnad_skb_unmap *unmap_array;
@@ -369,11 +312,11 @@ bnad_free_all_rxbufs(struct bnad *bnad, struct bna_rcb *rcb)
 				 DMA_FROM_DEVICE);
 		dev_kfree_skb(skb);
 	}
-	bnad_reset_rcb(bnad, rcb);
+	bnad_rcb_cleanup(bnad, rcb);
 }
 
 static void
-bnad_alloc_n_post_rxbufs(struct bnad *bnad, struct bna_rcb *rcb)
+bnad_rxq_post(struct bnad *bnad, struct bna_rcb *rcb)
 {
 	u16 to_alloc, alloced, unmap_prod, wi_range;
 	struct bnad_unmap_q *unmap_q = rcb->unmap_q;
@@ -434,14 +377,14 @@ bnad_refill_rxq(struct bnad *bnad, struct bna_rcb *rcb)
 	if (!test_and_set_bit(BNAD_RXQ_REFILL, &rcb->flags)) {
 		if (BNA_QE_FREE_CNT(unmap_q, unmap_q->q_depth)
 			 >> BNAD_RXQ_REFILL_THRESHOLD_SHIFT)
-			bnad_alloc_n_post_rxbufs(bnad, rcb);
+			bnad_rxq_post(bnad, rcb);
 		smp_mb__before_clear_bit();
 		clear_bit(BNAD_RXQ_REFILL, &rcb->flags);
 	}
 }
 
 static u32
-bnad_poll_cq(struct bnad *bnad, struct bna_ccb *ccb, int budget)
+bnad_cq_process(struct bnad *bnad, struct bna_ccb *ccb, int budget)
 {
 	struct bna_cq_entry *cmpl, *next_cmpl;
 	struct bna_rcb *rcb = NULL;
@@ -453,12 +396,8 @@ bnad_poll_cq(struct bnad *bnad, struct bna_ccb *ccb, int budget)
 	struct bna_pkt_rate *pkt_rt = &ccb->pkt_rate;
 	struct bnad_rx_ctrl *rx_ctrl = (struct bnad_rx_ctrl *)(ccb->ctrl);
 
-	set_bit(BNAD_FP_IN_RX_PATH, &rx_ctrl->flags);
-
-	if (!test_bit(BNAD_RXQ_STARTED, &ccb->rcb[0]->flags)) {
-		clear_bit(BNAD_FP_IN_RX_PATH, &rx_ctrl->flags);
+	if (!test_bit(BNAD_RXQ_STARTED, &ccb->rcb[0]->flags))
 		return 0;
-	}
 
 	prefetch(bnad->netdev);
 	BNA_CQ_QPGE_PTR_GET(ccb->producer_index, ccb->sw_qpt, cmpl,
@@ -533,9 +472,8 @@ bnad_poll_cq(struct bnad *bnad, struct bna_ccb *ccb, int budget)
 
 		if (skb->ip_summed == CHECKSUM_UNNECESSARY)
 			napi_gro_receive(&rx_ctrl->napi, skb);
-		else {
+		else
 			netif_receive_skb(skb);
-		}
 
 next:
 		cmpl->valid = 0;
@@ -646,7 +584,7 @@ bnad_isr(int irq, void *data)
 		for (j = 0; j < bnad->num_txq_per_tx; j++) {
 			tcb = bnad->tx_info[i].tcb[j];
 			if (tcb && test_bit(BNAD_TXQ_TX_STARTED, &tcb->flags))
-				bnad_tx(bnad, bnad->tx_info[i].tcb[j]);
+				bnad_tx_complete(bnad, bnad->tx_info[i].tcb[j]);
 		}
 	}
 	/* Rx processing */
@@ -839,20 +777,9 @@ bnad_cb_tcb_destroy(struct bnad *bnad, struct bna_tcb *tcb)
 {
 	struct bnad_tx_info *tx_info =
 			(struct bnad_tx_info *)tcb->txq->tx->priv;
-	struct bnad_unmap_q *unmap_q = tcb->unmap_q;
-
-	while (test_and_set_bit(BNAD_TXQ_FREE_SENT, &tcb->flags))
-		cpu_relax();
-
-	bnad_free_all_txbufs(bnad, tcb);
-
-	unmap_q->producer_index = 0;
-	unmap_q->consumer_index = 0;
-
-	smp_mb__before_clear_bit();
-	clear_bit(BNAD_TXQ_FREE_SENT, &tcb->flags);
 
 	tx_info->tcb[tcb->id] = NULL;
+	tcb->priv = NULL;
 }
 
 static void
@@ -866,12 +793,6 @@ bnad_cb_rcb_setup(struct bnad *bnad, struct bna_rcb *rcb)
 }
 
 static void
-bnad_cb_rcb_destroy(struct bnad *bnad, struct bna_rcb *rcb)
-{
-	bnad_free_all_rxbufs(bnad, rcb);
-}
-
-static void
 bnad_cb_ccb_setup(struct bnad *bnad, struct bna_ccb *ccb)
 {
 	struct bnad_rx_info *rx_info =
@@ -916,7 +837,6 @@ bnad_cb_tx_resume(struct bnad *bnad, struct bna_tx *tx)
 {
 	struct bnad_tx_info *tx_info = (struct bnad_tx_info *)tx->priv;
 	struct bna_tcb *tcb;
-	struct bnad_unmap_q *unmap_q;
 	u32 txq_id;
 	int i;
 
@@ -926,23 +846,9 @@ bnad_cb_tx_resume(struct bnad *bnad, struct bna_tx *tx)
 			continue;
 		txq_id = tcb->id;
 
-		unmap_q = tcb->unmap_q;
-
-		if (test_bit(BNAD_TXQ_TX_STARTED, &tcb->flags))
-			continue;
-
-		while (test_and_set_bit(BNAD_TXQ_FREE_SENT, &tcb->flags))
-			cpu_relax();
-
-		bnad_free_all_txbufs(bnad, tcb);
-
-		unmap_q->producer_index = 0;
-		unmap_q->consumer_index = 0;
-
-		smp_mb__before_clear_bit();
-		clear_bit(BNAD_TXQ_FREE_SENT, &tcb->flags);
-
+		BUG_ON(test_bit(BNAD_TXQ_TX_STARTED, &tcb->flags));
 		set_bit(BNAD_TXQ_TX_STARTED, &tcb->flags);
+		BUG_ON(*(tcb->hw_consumer_index) != 0);
 
 		if (netif_carrier_ok(bnad->netdev)) {
 			printk(KERN_INFO "bna: %s %d TXQ_STARTED\n",
@@ -963,6 +869,54 @@ bnad_cb_tx_resume(struct bnad *bnad, struct bna_tx *tx)
 	}
 }
 
+/*
+ * Free all TxQs buffers and then notify TX_E_CLEANUP_DONE to Tx fsm.
+ */
+static void
+bnad_tx_cleanup(struct delayed_work *work)
+{
+	struct bnad_tx_info *tx_info =
+		container_of(work, struct bnad_tx_info, tx_cleanup_work);
+	struct bnad *bnad = NULL;
+	struct bnad_unmap_q *unmap_q;
+	struct bna_tcb *tcb;
+	unsigned long flags;
+	uint32_t i, pending = 0;
+
+	for (i = 0; i < BNAD_MAX_TXQ_PER_TX; i++) {
+		tcb = tx_info->tcb[i];
+		if (!tcb)
+			continue;
+
+		bnad = tcb->bnad;
+
+		if (test_and_set_bit(BNAD_TXQ_FREE_SENT, &tcb->flags)) {
+			pending++;
+			continue;
+		}
+
+		bnad_txq_cleanup(bnad, tcb);
+
+		unmap_q = tcb->unmap_q;
+		unmap_q->producer_index = 0;
+		unmap_q->consumer_index = 0;
+
+		smp_mb__before_clear_bit();
+		clear_bit(BNAD_TXQ_FREE_SENT, &tcb->flags);
+	}
+
+	if (pending) {
+		queue_delayed_work(bnad->work_q, &tx_info->tx_cleanup_work,
+			msecs_to_jiffies(1));
+		return;
+	}
+
+	spin_lock_irqsave(&bnad->bna_lock, flags);
+	bna_tx_cleanup_complete(tx_info->tx);
+	spin_unlock_irqrestore(&bnad->bna_lock, flags);
+}
+
+
 static void
 bnad_cb_tx_cleanup(struct bnad *bnad, struct bna_tx *tx)
 {
@@ -976,8 +930,7 @@ bnad_cb_tx_cleanup(struct bnad *bnad, struct bna_tx *tx)
 			continue;
 	}
 
-	mdelay(BNAD_TXRX_SYNC_MDELAY);
-	bna_tx_cleanup_complete(tx);
+	queue_delayed_work(bnad->work_q, &tx_info->tx_cleanup_work, 0);
 }
 
 static void
@@ -1001,6 +954,44 @@ bnad_cb_rx_stall(struct bnad *bnad, struct bna_rx *rx)
 	}
 }
 
+/*
+ * Free all RxQs buffers and then notify RX_E_CLEANUP_DONE to Rx fsm.
+ */
+static void
+bnad_rx_cleanup(void *work)
+{
+	struct bnad_rx_info *rx_info =
+		container_of(work, struct bnad_rx_info, rx_cleanup_work);
+	struct bnad_rx_ctrl *rx_ctrl;
+	struct bnad *bnad = NULL;
+	unsigned long flags;
+	uint32_t i;
+
+	for (i = 0; i < BNAD_MAX_RXP_PER_RX; i++) {
+		rx_ctrl = &rx_info->rx_ctrl[i];
+
+		if (!rx_ctrl->ccb)
+			continue;
+
+		bnad = rx_ctrl->ccb->bnad;
+
+		/*
+		 * Wait till the poll handler has exited
+		 * and nothing can be scheduled anymore
+		 */
+		napi_disable(&rx_ctrl->napi);
+
+		bnad_cq_cleanup(bnad, rx_ctrl->ccb);
+		bnad_rxq_cleanup(bnad, rx_ctrl->ccb->rcb[0]);
+		if (rx_ctrl->ccb->rcb[1])
+			bnad_rxq_cleanup(bnad, rx_ctrl->ccb->rcb[1]);
+	}
+
+	spin_lock_irqsave(&bnad->bna_lock, flags);
+	bna_rx_cleanup_complete(rx_info->rx);
+	spin_unlock_irqrestore(&bnad->bna_lock, flags);
+}
+
 static void
 bnad_cb_rx_cleanup(struct bnad *bnad, struct bna_rx *rx)
 {
@@ -1009,8 +1000,6 @@ bnad_cb_rx_cleanup(struct bnad *bnad, struct bna_rx *rx)
 	struct bnad_rx_ctrl *rx_ctrl;
 	int i;
 
-	mdelay(BNAD_TXRX_SYNC_MDELAY);
-
 	for (i = 0; i < BNAD_MAX_RXP_PER_RX; i++) {
 		rx_ctrl = &rx_info->rx_ctrl[i];
 		ccb = rx_ctrl->ccb;
@@ -1021,12 +1010,9 @@ bnad_cb_rx_cleanup(struct bnad *bnad, struct bna_rx *rx)
 
 		if (ccb->rcb[1])
 			clear_bit(BNAD_RXQ_STARTED, &ccb->rcb[1]->flags);
-
-		while (test_bit(BNAD_FP_IN_RX_PATH, &rx_ctrl->flags))
-			cpu_relax();
 	}
 
-	bna_rx_cleanup_complete(rx);
+	queue_work(bnad->work_q, &rx_info->rx_cleanup_work);
 }
 
 static void
@@ -1046,13 +1032,12 @@ bnad_cb_rx_post(struct bnad *bnad, struct bna_rx *rx)
 		if (!ccb)
 			continue;
 
-		bnad_cq_cmpl_init(bnad, ccb);
+		napi_enable(&rx_ctrl->napi);
 
 		for (j = 0; j < BNAD_MAX_RXQ_PER_RXP; j++) {
 			rcb = ccb->rcb[j];
 			if (!rcb)
 				continue;
-			bnad_free_all_rxbufs(bnad, rcb);
 
 			set_bit(BNAD_RXQ_STARTED, &rcb->flags);
 			set_bit(BNAD_RXQ_POST_OK, &rcb->flags);
@@ -1063,7 +1048,7 @@ bnad_cb_rx_post(struct bnad *bnad, struct bna_rx *rx)
 			if (!test_and_set_bit(BNAD_RXQ_REFILL, &rcb->flags)) {
 				if (BNA_QE_FREE_CNT(unmap_q, unmap_q->q_depth)
 					>> BNAD_RXQ_REFILL_THRESHOLD_SHIFT)
-					bnad_alloc_n_post_rxbufs(bnad, rcb);
+					bnad_rxq_post(bnad, rcb);
 					smp_mb__before_clear_bit();
 				clear_bit(BNAD_RXQ_REFILL, &rcb->flags);
 			}
@@ -1687,7 +1672,7 @@ bnad_napi_poll_rx(struct napi_struct *napi, int budget)
 	if (!netif_carrier_ok(bnad->netdev))
 		goto poll_exit;
 
-	rcvd = bnad_poll_cq(bnad, rx_ctrl->ccb, budget);
+	rcvd = bnad_cq_process(bnad, rx_ctrl->ccb, budget);
 	if (rcvd >= budget)
 		return rcvd;
 
@@ -1704,7 +1689,7 @@ poll_exit:
 
 #define BNAD_NAPI_POLL_QUOTA		64
 static void
-bnad_napi_init(struct bnad *bnad, u32 rx_id)
+bnad_napi_add(struct bnad *bnad, u32 rx_id)
 {
 	struct bnad_rx_ctrl *rx_ctrl;
 	int i;
@@ -1718,34 +1703,18 @@ bnad_napi_init(struct bnad *bnad, u32 rx_id)
 }
 
 static void
-bnad_napi_enable(struct bnad *bnad, u32 rx_id)
-{
-	struct bnad_rx_ctrl *rx_ctrl;
-	int i;
-
-	/* Initialize & enable NAPI */
-	for (i = 0; i <	bnad->num_rxp_per_rx; i++) {
-		rx_ctrl = &bnad->rx_info[rx_id].rx_ctrl[i];
-
-		napi_enable(&rx_ctrl->napi);
-	}
-}
-
-static void
-bnad_napi_disable(struct bnad *bnad, u32 rx_id)
+bnad_napi_delete(struct bnad *bnad, u32 rx_id)
 {
 	int i;
 
 	/* First disable and then clean up */
-	for (i = 0; i < bnad->num_rxp_per_rx; i++) {
-		napi_disable(&bnad->rx_info[rx_id].rx_ctrl[i].napi);
+	for (i = 0; i < bnad->num_rxp_per_rx; i++)
 		netif_napi_del(&bnad->rx_info[rx_id].rx_ctrl[i].napi);
-	}
 }
 
 /* Should be held with conf_lock held */
 void
-bnad_cleanup_tx(struct bnad *bnad, u32 tx_id)
+bnad_destroy_tx(struct bnad *bnad, u32 tx_id)
 {
 	struct bnad_tx_info *tx_info = &bnad->tx_info[tx_id];
 	struct bna_res_info *res_info = &bnad->tx_res_info[tx_id].res_info[0];
@@ -1764,9 +1733,6 @@ bnad_cleanup_tx(struct bnad *bnad, u32 tx_id)
 		bnad_tx_msix_unregister(bnad, tx_info,
 			bnad->num_txq_per_tx);
 
-	if (0 == tx_id)
-		tasklet_kill(&bnad->tx_free_tasklet);
-
 	spin_lock_irqsave(&bnad->bna_lock, flags);
 	bna_tx_destroy(tx_info->tx);
 	spin_unlock_irqrestore(&bnad->bna_lock, flags);
@@ -1832,6 +1798,9 @@ bnad_setup_tx(struct bnad *bnad, u32 tx_id)
 		goto err_return;
 	tx_info->tx = tx;
 
+	INIT_DELAYED_WORK(&tx_info->tx_cleanup_work,
+			(work_func_t)bnad_tx_cleanup);
+
 	/* Register ISR for the Tx object */
 	if (intr_info->intr_type == BNA_INTR_T_MSIX) {
 		err = bnad_tx_msix_register(bnad, tx_info,
@@ -1896,7 +1865,7 @@ bnad_rx_ctrl_init(struct bnad *bnad, u32 rx_id)
 
 /* Called with mutex_lock(&bnad->conf_mutex) held */
 void
-bnad_cleanup_rx(struct bnad *bnad, u32 rx_id)
+bnad_destroy_rx(struct bnad *bnad, u32 rx_id)
 {
 	struct bnad_rx_info *rx_info = &bnad->rx_info[rx_id];
 	struct bna_rx_config *rx_config = &bnad->rx_config[rx_id];
@@ -1928,7 +1897,7 @@ bnad_cleanup_rx(struct bnad *bnad, u32 rx_id)
 	if (rx_info->rx_ctrl[0].ccb->intr_type == BNA_INTR_T_MSIX)
 		bnad_rx_msix_unregister(bnad, rx_info, rx_config->num_paths);
 
-	bnad_napi_disable(bnad, rx_id);
+	bnad_napi_delete(bnad, rx_id);
 
 	spin_lock_irqsave(&bnad->bna_lock, flags);
 	bna_rx_destroy(rx_info->rx);
@@ -1952,7 +1921,7 @@ bnad_setup_rx(struct bnad *bnad, u32 rx_id)
 	struct bna_rx_config *rx_config = &bnad->rx_config[rx_id];
 	static const struct bna_rx_event_cbfn rx_cbfn = {
 		.rcb_setup_cbfn = bnad_cb_rcb_setup,
-		.rcb_destroy_cbfn = bnad_cb_rcb_destroy,
+		.rcb_destroy_cbfn = NULL,
 		.ccb_setup_cbfn = bnad_cb_ccb_setup,
 		.ccb_destroy_cbfn = bnad_cb_ccb_destroy,
 		.rx_stall_cbfn = bnad_cb_rx_stall,
@@ -1998,11 +1967,14 @@ bnad_setup_rx(struct bnad *bnad, u32 rx_id)
 	rx_info->rx = rx;
 	spin_unlock_irqrestore(&bnad->bna_lock, flags);
 
+	INIT_WORK(&rx_info->rx_cleanup_work,
+			(work_func_t)(bnad_rx_cleanup));
+
 	/*
 	 * Init NAPI, so that state is set to NAPI_STATE_SCHED,
 	 * so that IRQ handler cannot schedule NAPI at this point.
 	 */
-	bnad_napi_init(bnad, rx_id);
+	bnad_napi_add(bnad, rx_id);
 
 	/* Register ISR for the Rx object */
 	if (intr_info->intr_type == BNA_INTR_T_MSIX) {
@@ -2028,13 +2000,10 @@ bnad_setup_rx(struct bnad *bnad, u32 rx_id)
 	bna_rx_enable(rx);
 	spin_unlock_irqrestore(&bnad->bna_lock, flags);
 
-	/* Enable scheduling of NAPI */
-	bnad_napi_enable(bnad, rx_id);
-
 	return 0;
 
 err_return:
-	bnad_cleanup_rx(bnad, rx_id);
+	bnad_destroy_rx(bnad, rx_id);
 	return err;
 }
 
@@ -2519,7 +2488,7 @@ bnad_open(struct net_device *netdev)
 	return 0;
 
 cleanup_tx:
-	bnad_cleanup_tx(bnad, 0);
+	bnad_destroy_tx(bnad, 0);
 
 err_return:
 	mutex_unlock(&bnad->conf_mutex);
@@ -2546,8 +2515,8 @@ bnad_stop(struct net_device *netdev)
 
 	wait_for_completion(&bnad->bnad_completions.enet_comp);
 
-	bnad_cleanup_tx(bnad, 0);
-	bnad_cleanup_rx(bnad, 0);
+	bnad_destroy_tx(bnad, 0);
+	bnad_destroy_rx(bnad, 0);
 
 	/* Synchronize mailbox IRQ */
 	bnad_mbox_irq_sync(bnad);
@@ -2620,7 +2589,7 @@ bnad_start_xmit(struct sk_buff *skb, struct net_device *netdev)
 		if ((u16) (*tcb->hw_consumer_index) !=
 		    tcb->consumer_index &&
 		    !test_and_set_bit(BNAD_TXQ_FREE_SENT, &tcb->flags)) {
-			acked = bnad_free_txbufs(bnad, tcb);
+			acked = bnad_txcmpl_process(bnad, tcb);
 			if (likely(test_bit(BNAD_TXQ_TX_STARTED, &tcb->flags)))
 				bna_ib_ack(tcb->i_dbell, acked);
 			smp_mb__before_clear_bit();
@@ -2843,9 +2812,6 @@ bnad_start_xmit(struct sk_buff *skb, struct net_device *netdev)
 	bna_txq_prod_indx_doorbell(tcb);
 	smp_mb();
 
-	if ((u16) (*tcb->hw_consumer_index) != tcb->consumer_index)
-		tasklet_schedule(&bnad->tx_free_tasklet);
-
 	return NETDEV_TX_OK;
 }
 
@@ -3127,8 +3093,8 @@ bnad_netdev_init(struct bnad *bnad, bool using_dac)
 /*
  * 1. Initialize the bnad structure
  * 2. Setup netdev pointer in pci_dev
- * 3. Initialze Tx free tasklet
- * 4. Initialize no. of TxQ & CQs & MSIX vectors
+ * 3. Initialize no. of TxQ & CQs & MSIX vectors
+ * 4. Initialize work queue.
  */
 static int
 bnad_init(struct bnad *bnad,
@@ -3171,8 +3137,11 @@ bnad_init(struct bnad *bnad,
 	bnad->tx_coalescing_timeo = BFI_TX_COALESCING_TIMEO;
 	bnad->rx_coalescing_timeo = BFI_RX_COALESCING_TIMEO;
 
-	tasklet_init(&bnad->tx_free_tasklet, bnad_tx_free_tasklet,
-		     (unsigned long)bnad);
+	sprintf(bnad->wq_name, "%s_wq_%d", BNAD_NAME, bnad->id);
+	bnad->work_q = create_singlethread_workqueue(bnad->wq_name);
+
+	if (!bnad->work_q)
+		return -ENOMEM;
 
 	return 0;
 }
@@ -3185,6 +3154,12 @@ bnad_init(struct bnad *bnad,
 static void
 bnad_uninit(struct bnad *bnad)
 {
+	if (bnad->work_q) {
+		flush_workqueue(bnad->work_q);
+		destroy_workqueue(bnad->work_q);
+		bnad->work_q = NULL;
+	}
+
 	if (bnad->bar0)
 		iounmap(bnad->bar0);
 	pci_set_drvdata(bnad->pcidev, NULL);
@@ -3304,7 +3279,6 @@ bnad_pci_probe(struct pci_dev *pdev,
 	/*
 	 * Initialize bnad structure
 	 * Setup relation between pci_dev & netdev
-	 * Init Tx free tasklet
 	 */
 	err = bnad_init(bnad, pdev, netdev);
 	if (err)
@@ -3546,9 +3520,7 @@ static void __exit
 bnad_module_exit(void)
 {
 	pci_unregister_driver(&bnad_pci_driver);
-
-	if (bfi_fw)
-		release_firmware(bfi_fw);
+	release_firmware(bfi_fw);
 }
 
 module_init(bnad_module_init);
diff --git a/drivers/net/ethernet/brocade/bna/bnad.h b/drivers/net/ethernet/brocade/bna/bnad.h
index 55824d92699..72742be1127 100644
--- a/drivers/net/ethernet/brocade/bna/bnad.h
+++ b/drivers/net/ethernet/brocade/bna/bnad.h
@@ -71,7 +71,7 @@ struct bnad_rx_ctrl {
 #define BNAD_NAME			"bna"
 #define BNAD_NAME_LEN			64
 
-#define BNAD_VERSION			"3.0.2.2"
+#define BNAD_VERSION			"3.0.23.0"
 
 #define BNAD_MAILBOX_MSIX_INDEX		0
 #define BNAD_MAILBOX_MSIX_VECTORS	1
@@ -210,6 +210,7 @@ struct bnad_tx_info {
 	struct bna_tx *tx; /* 1:1 between tx_info & tx */
 	struct bna_tcb *tcb[BNAD_MAX_TXQ_PER_TX];
 	u32 tx_id;
+	struct delayed_work tx_cleanup_work;
 } ____cacheline_aligned;
 
 struct bnad_rx_info {
@@ -217,6 +218,7 @@ struct bnad_rx_info {
 
 	struct bnad_rx_ctrl rx_ctrl[BNAD_MAX_RXP_PER_RX];
 	u32 rx_id;
+	struct work_struct rx_cleanup_work;
 } ____cacheline_aligned;
 
 /* Unmap queues for Tx / Rx cleanup */
@@ -318,7 +320,7 @@ struct bnad {
 	/* Burnt in MAC address */
 	mac_t			perm_addr;
 
-	struct tasklet_struct	tx_free_tasklet;
+	struct workqueue_struct *work_q;
 
 	/* Statistics */
 	struct bnad_stats stats;
@@ -328,6 +330,7 @@ struct bnad {
 	char			adapter_name[BNAD_NAME_LEN];
 	char			port_name[BNAD_NAME_LEN];
 	char			mbox_irq_name[BNAD_NAME_LEN];
+	char			wq_name[BNAD_NAME_LEN];
 
 	/* debugfs specific data */
 	char	*regdata;
@@ -370,8 +373,8 @@ extern void bnad_rx_coalescing_timeo_set(struct bnad *bnad);
 
 extern int bnad_setup_rx(struct bnad *bnad, u32 rx_id);
 extern int bnad_setup_tx(struct bnad *bnad, u32 tx_id);
-extern void bnad_cleanup_tx(struct bnad *bnad, u32 tx_id);
-extern void bnad_cleanup_rx(struct bnad *bnad, u32 rx_id);
+extern void bnad_destroy_tx(struct bnad *bnad, u32 tx_id);
+extern void bnad_destroy_rx(struct bnad *bnad, u32 rx_id);
 
 /* Timer start/stop protos */
 extern void bnad_dim_timer_start(struct bnad *bnad);
diff --git a/drivers/net/ethernet/brocade/bna/bnad_ethtool.c b/drivers/net/ethernet/brocade/bna/bnad_ethtool.c
index ab753d7334a..40e1e84f498 100644
--- a/drivers/net/ethernet/brocade/bna/bnad_ethtool.c
+++ b/drivers/net/ethernet/brocade/bna/bnad_ethtool.c
@@ -464,7 +464,7 @@ bnad_set_ringparam(struct net_device *netdev,
 		for (i = 0; i < bnad->num_rx; i++) {
 			if (!bnad->rx_info[i].rx)
 				continue;
-			bnad_cleanup_rx(bnad, i);
+			bnad_destroy_rx(bnad, i);
 			current_err = bnad_setup_rx(bnad, i);
 			if (current_err && !err)
 				err = current_err;
@@ -492,7 +492,7 @@ bnad_set_ringparam(struct net_device *netdev,
 		for (i = 0; i < bnad->num_tx; i++) {
 			if (!bnad->tx_info[i].tx)
 				continue;
-			bnad_cleanup_tx(bnad, i);
+			bnad_destroy_tx(bnad, i);
 			current_err = bnad_setup_tx(bnad, i);
 			if (current_err && !err)
 				err = current_err;
@@ -539,7 +539,7 @@ bnad_set_pauseparam(struct net_device *netdev,
 }
 
 static void
-bnad_get_strings(struct net_device *netdev, u32 stringset, u8 * string)
+bnad_get_strings(struct net_device *netdev, u32 stringset, u8 *string)
 {
 	struct bnad *bnad = netdev_priv(netdev);
 	int i, j, q_num;
diff --git a/drivers/net/ethernet/cadence/at91_ether.c b/drivers/net/ethernet/cadence/at91_ether.c
index 906117016fc..77884191a8c 100644
--- a/drivers/net/ethernet/cadence/at91_ether.c
+++ b/drivers/net/ethernet/cadence/at91_ether.c
@@ -30,6 +30,7 @@
 #include <linux/platform_device.h>
 #include <linux/clk.h>
 #include <linux/gfp.h>
+#include <linux/phy.h>
 
 #include <asm/io.h>
 #include <asm/uaccess.h>
@@ -51,21 +52,17 @@
 /*
  * Read from a EMAC register.
  */
-static inline unsigned long at91_emac_read(unsigned int reg)
+static inline unsigned long at91_emac_read(struct at91_private *lp, unsigned int reg)
 {
-	void __iomem *emac_base = (void __iomem *)AT91_VA_BASE_EMAC;
-
-	return __raw_readl(emac_base + reg);
+	return __raw_readl(lp->emac_base + reg);
 }
 
 /*
  * Write to a EMAC register.
  */
-static inline void at91_emac_write(unsigned int reg, unsigned long value)
+static inline void at91_emac_write(struct at91_private *lp, unsigned int reg, unsigned long value)
 {
-	void __iomem *emac_base = (void __iomem *)AT91_VA_BASE_EMAC;
-
-	__raw_writel(value, emac_base + reg);
+	__raw_writel(value, lp->emac_base + reg);
 }
 
 /* ........................... PHY INTERFACE ........................... */
@@ -75,32 +72,33 @@ static inline void at91_emac_write(unsigned int reg, unsigned long value)
  * When not called from an interrupt-handler, access to the PHY must be
  *  protected by a spinlock.
  */
-static void enable_mdi(void)
+static void enable_mdi(struct at91_private *lp)
 {
 	unsigned long ctl;
 
-	ctl = at91_emac_read(AT91_EMAC_CTL);
-	at91_emac_write(AT91_EMAC_CTL, ctl | AT91_EMAC_MPE);	/* enable management port */
+	ctl = at91_emac_read(lp, AT91_EMAC_CTL);
+	at91_emac_write(lp, AT91_EMAC_CTL, ctl | AT91_EMAC_MPE);	/* enable management port */
 }
 
 /*
  * Disable the MDIO bit in the MAC control register
  */
-static void disable_mdi(void)
+static void disable_mdi(struct at91_private *lp)
 {
 	unsigned long ctl;
 
-	ctl = at91_emac_read(AT91_EMAC_CTL);
-	at91_emac_write(AT91_EMAC_CTL, ctl & ~AT91_EMAC_MPE);	/* disable management port */
+	ctl = at91_emac_read(lp, AT91_EMAC_CTL);
+	at91_emac_write(lp, AT91_EMAC_CTL, ctl & ~AT91_EMAC_MPE);	/* disable management port */
 }
 
 /*
  * Wait until the PHY operation is complete.
  */
-static inline void at91_phy_wait(void) {
+static inline void at91_phy_wait(struct at91_private *lp)
+{
 	unsigned long timeout = jiffies + 2;
 
-	while (!(at91_emac_read(AT91_EMAC_SR) & AT91_EMAC_SR_IDLE)) {
+	while (!(at91_emac_read(lp, AT91_EMAC_SR) & AT91_EMAC_SR_IDLE)) {
 		if (time_after(jiffies, timeout)) {
 			printk("at91_ether: MIO timeout\n");
 			break;
@@ -113,28 +111,28 @@ static inline void at91_phy_wait(void) {
  * Write value to the a PHY register
  * Note: MDI interface is assumed to already have been enabled.
  */
-static void write_phy(unsigned char phy_addr, unsigned char address, unsigned int value)
+static void write_phy(struct at91_private *lp, unsigned char phy_addr, unsigned char address, unsigned int value)
 {
-	at91_emac_write(AT91_EMAC_MAN, AT91_EMAC_MAN_802_3 | AT91_EMAC_RW_W
+	at91_emac_write(lp, AT91_EMAC_MAN, AT91_EMAC_MAN_802_3 | AT91_EMAC_RW_W
 		| ((phy_addr & 0x1f) << 23) | (address << 18) | (value & AT91_EMAC_DATA));
 
 	/* Wait until IDLE bit in Network Status register is cleared */
-	at91_phy_wait();
+	at91_phy_wait(lp);
 }
 
 /*
  * Read value stored in a PHY register.
  * Note: MDI interface is assumed to already have been enabled.
  */
-static void read_phy(unsigned char phy_addr, unsigned char address, unsigned int *value)
+static void read_phy(struct at91_private *lp, unsigned char phy_addr, unsigned char address, unsigned int *value)
 {
-	at91_emac_write(AT91_EMAC_MAN, AT91_EMAC_MAN_802_3 | AT91_EMAC_RW_R
+	at91_emac_write(lp, AT91_EMAC_MAN, AT91_EMAC_MAN_802_3 | AT91_EMAC_RW_R
 		| ((phy_addr & 0x1f) << 23) | (address << 18));
 
 	/* Wait until IDLE bit in Network Status register is cleared */
-	at91_phy_wait();
+	at91_phy_wait(lp);
 
-	*value = at91_emac_read(AT91_EMAC_MAN) & AT91_EMAC_DATA;
+	*value = at91_emac_read(lp, AT91_EMAC_MAN) & AT91_EMAC_DATA;
 }
 
 /* ........................... PHY MANAGEMENT .......................... */
@@ -158,13 +156,13 @@ static void update_linkspeed(struct net_device *dev, int silent)
 	}
 
 	/* Link up, or auto-negotiation still in progress */
-	read_phy(lp->phy_address, MII_BMSR, &bmsr);
-	read_phy(lp->phy_address, MII_BMCR, &bmcr);
+	read_phy(lp, lp->phy_address, MII_BMSR, &bmsr);
+	read_phy(lp, lp->phy_address, MII_BMCR, &bmcr);
 	if (bmcr & BMCR_ANENABLE) {				/* AutoNegotiation is enabled */
 		if (!(bmsr & BMSR_ANEGCOMPLETE))
 			return;			/* Do nothing - another interrupt generated when negotiation complete */
 
-		read_phy(lp->phy_address, MII_LPA, &lpa);
+		read_phy(lp, lp->phy_address, MII_LPA, &lpa);
 		if ((lpa & LPA_100FULL) || (lpa & LPA_100HALF)) speed = SPEED_100;
 		else speed = SPEED_10;
 		if ((lpa & LPA_100FULL) || (lpa & LPA_10FULL)) duplex = DUPLEX_FULL;
@@ -175,7 +173,7 @@ static void update_linkspeed(struct net_device *dev, int silent)
 	}
 
 	/* Update the MAC */
-	mac_cfg = at91_emac_read(AT91_EMAC_CFG) & ~(AT91_EMAC_SPD | AT91_EMAC_FD);
+	mac_cfg = at91_emac_read(lp, AT91_EMAC_CFG) & ~(AT91_EMAC_SPD | AT91_EMAC_FD);
 	if (speed == SPEED_100) {
 		if (duplex == DUPLEX_FULL)		/* 100 Full Duplex */
 			mac_cfg |= AT91_EMAC_SPD | AT91_EMAC_FD;
@@ -186,7 +184,7 @@ static void update_linkspeed(struct net_device *dev, int silent)
 			mac_cfg |= AT91_EMAC_FD;
 		else {}					/* 10 Half Duplex */
 	}
-	at91_emac_write(AT91_EMAC_CFG, mac_cfg);
+	at91_emac_write(lp, AT91_EMAC_CFG, mac_cfg);
 
 	if (!silent)
 		printk(KERN_INFO "%s: Link now %i-%s\n", dev->name, speed, (duplex == DUPLEX_FULL) ? "FullDuplex" : "HalfDuplex");
@@ -207,34 +205,34 @@ static irqreturn_t at91ether_phy_interrupt(int irq, void *dev_id)
 	 * level-triggering.  We therefore have to check if the PHY actually has
 	 * an IRQ pending.
 	 */
-	enable_mdi();
+	enable_mdi(lp);
 	if ((lp->phy_type == MII_DM9161_ID) || (lp->phy_type == MII_DM9161A_ID)) {
-		read_phy(lp->phy_address, MII_DSINTR_REG, &phy);	/* ack interrupt in Davicom PHY */
+		read_phy(lp, lp->phy_address, MII_DSINTR_REG, &phy);	/* ack interrupt in Davicom PHY */
 		if (!(phy & (1 << 0)))
 			goto done;
 	}
 	else if (lp->phy_type == MII_LXT971A_ID) {
-		read_phy(lp->phy_address, MII_ISINTS_REG, &phy);	/* ack interrupt in Intel PHY */
+		read_phy(lp, lp->phy_address, MII_ISINTS_REG, &phy);	/* ack interrupt in Intel PHY */
 		if (!(phy & (1 << 2)))
 			goto done;
 	}
 	else if (lp->phy_type == MII_BCM5221_ID) {
-		read_phy(lp->phy_address, MII_BCMINTR_REG, &phy);	/* ack interrupt in Broadcom PHY */
+		read_phy(lp, lp->phy_address, MII_BCMINTR_REG, &phy);	/* ack interrupt in Broadcom PHY */
 		if (!(phy & (1 << 0)))
 			goto done;
 	}
 	else if (lp->phy_type == MII_KS8721_ID) {
-		read_phy(lp->phy_address, MII_TPISTATUS, &phy);		/* ack interrupt in Micrel PHY */
+		read_phy(lp, lp->phy_address, MII_TPISTATUS, &phy);		/* ack interrupt in Micrel PHY */
 		if (!(phy & ((1 << 2) | 1)))
 			goto done;
 	}
-	else if (lp->phy_type == MII_T78Q21x3_ID) {			/* ack interrupt in Teridian PHY */
-		read_phy(lp->phy_address, MII_T78Q21INT_REG, &phy);
+	else if (lp->phy_type == MII_T78Q21x3_ID) {					/* ack interrupt in Teridian PHY */
+		read_phy(lp, lp->phy_address, MII_T78Q21INT_REG, &phy);
 		if (!(phy & ((1 << 2) | 1)))
 			goto done;
 	}
 	else if (lp->phy_type == MII_DP83848_ID) {
-		read_phy(lp->phy_address, MII_DPPHYSTS_REG, &phy);	/* ack interrupt in DP83848 PHY */
+		read_phy(lp, lp->phy_address, MII_DPPHYSTS_REG, &phy);	/* ack interrupt in DP83848 PHY */
 		if (!(phy & (1 << 7)))
 			goto done;
 	}
@@ -242,7 +240,7 @@ static irqreturn_t at91ether_phy_interrupt(int irq, void *dev_id)
 	update_linkspeed(dev, 0);
 
 done:
-	disable_mdi();
+	disable_mdi(lp);
 
 	return IRQ_HANDLED;
 }
@@ -265,7 +263,7 @@ static void enable_phyirq(struct net_device *dev)
 		return;
 	}
 
-	irq_number = lp->board_data.phy_irq_pin;
+	irq_number = gpio_to_irq(lp->board_data.phy_irq_pin);
 	status = request_irq(irq_number, at91ether_phy_interrupt, 0, dev->name, dev);
 	if (status) {
 		printk(KERN_ERR "at91_ether: PHY IRQ %d request failed - status %d!\n", irq_number, status);
@@ -273,41 +271,41 @@ static void enable_phyirq(struct net_device *dev)
 	}
 
 	spin_lock_irq(&lp->lock);
-	enable_mdi();
+	enable_mdi(lp);
 
 	if ((lp->phy_type == MII_DM9161_ID) || (lp->phy_type == MII_DM9161A_ID)) {	/* for Davicom PHY */
-		read_phy(lp->phy_address, MII_DSINTR_REG, &dsintr);
+		read_phy(lp, lp->phy_address, MII_DSINTR_REG, &dsintr);
 		dsintr = dsintr & ~0xf00;		/* clear bits 8..11 */
-		write_phy(lp->phy_address, MII_DSINTR_REG, dsintr);
+		write_phy(lp, lp->phy_address, MII_DSINTR_REG, dsintr);
 	}
 	else if (lp->phy_type == MII_LXT971A_ID) {	/* for Intel PHY */
-		read_phy(lp->phy_address, MII_ISINTE_REG, &dsintr);
+		read_phy(lp, lp->phy_address, MII_ISINTE_REG, &dsintr);
 		dsintr = dsintr | 0xf2;			/* set bits 1, 4..7 */
-		write_phy(lp->phy_address, MII_ISINTE_REG, dsintr);
+		write_phy(lp, lp->phy_address, MII_ISINTE_REG, dsintr);
 	}
 	else if (lp->phy_type == MII_BCM5221_ID) {	/* for Broadcom PHY */
 		dsintr = (1 << 15) | ( 1 << 14);
-		write_phy(lp->phy_address, MII_BCMINTR_REG, dsintr);
+		write_phy(lp, lp->phy_address, MII_BCMINTR_REG, dsintr);
 	}
 	else if (lp->phy_type == MII_KS8721_ID) {	/* for Micrel PHY */
 		dsintr = (1 << 10) | ( 1 << 8);
-		write_phy(lp->phy_address, MII_TPISTATUS, dsintr);
+		write_phy(lp, lp->phy_address, MII_TPISTATUS, dsintr);
 	}
 	else if (lp->phy_type == MII_T78Q21x3_ID) {	/* for Teridian PHY */
-		read_phy(lp->phy_address, MII_T78Q21INT_REG, &dsintr);
+		read_phy(lp, lp->phy_address, MII_T78Q21INT_REG, &dsintr);
 		dsintr = dsintr | 0x500;		/* set bits 8, 10 */
-		write_phy(lp->phy_address, MII_T78Q21INT_REG, dsintr);
+		write_phy(lp, lp->phy_address, MII_T78Q21INT_REG, dsintr);
 	}
 	else if (lp->phy_type == MII_DP83848_ID) {	/* National Semiconductor DP83848 PHY */
-		read_phy(lp->phy_address, MII_DPMISR_REG, &dsintr);
+		read_phy(lp, lp->phy_address, MII_DPMISR_REG, &dsintr);
 		dsintr = dsintr | 0x3c;			/* set bits 2..5 */
-		write_phy(lp->phy_address, MII_DPMISR_REG, dsintr);
-		read_phy(lp->phy_address, MII_DPMICR_REG, &dsintr);
+		write_phy(lp, lp->phy_address, MII_DPMISR_REG, dsintr);
+		read_phy(lp, lp->phy_address, MII_DPMICR_REG, &dsintr);
 		dsintr = dsintr | 0x3;			/* set bits 0,1 */
-		write_phy(lp->phy_address, MII_DPMICR_REG, dsintr);
+		write_phy(lp, lp->phy_address, MII_DPMICR_REG, dsintr);
 	}
 
-	disable_mdi();
+	disable_mdi(lp);
 	spin_unlock_irq(&lp->lock);
 }
 
@@ -326,46 +324,46 @@ static void disable_phyirq(struct net_device *dev)
 	}
 
 	spin_lock_irq(&lp->lock);
-	enable_mdi();
+	enable_mdi(lp);
 
 	if ((lp->phy_type == MII_DM9161_ID) || (lp->phy_type == MII_DM9161A_ID)) {	/* for Davicom PHY */
-		read_phy(lp->phy_address, MII_DSINTR_REG, &dsintr);
+		read_phy(lp, lp->phy_address, MII_DSINTR_REG, &dsintr);
 		dsintr = dsintr | 0xf00;			/* set bits 8..11 */
-		write_phy(lp->phy_address, MII_DSINTR_REG, dsintr);
+		write_phy(lp, lp->phy_address, MII_DSINTR_REG, dsintr);
 	}
 	else if (lp->phy_type == MII_LXT971A_ID) {	/* for Intel PHY */
-		read_phy(lp->phy_address, MII_ISINTE_REG, &dsintr);
+		read_phy(lp, lp->phy_address, MII_ISINTE_REG, &dsintr);
 		dsintr = dsintr & ~0xf2;			/* clear bits 1, 4..7 */
-		write_phy(lp->phy_address, MII_ISINTE_REG, dsintr);
+		write_phy(lp, lp->phy_address, MII_ISINTE_REG, dsintr);
 	}
 	else if (lp->phy_type == MII_BCM5221_ID) {	/* for Broadcom PHY */
-		read_phy(lp->phy_address, MII_BCMINTR_REG, &dsintr);
+		read_phy(lp, lp->phy_address, MII_BCMINTR_REG, &dsintr);
 		dsintr = ~(1 << 14);
-		write_phy(lp->phy_address, MII_BCMINTR_REG, dsintr);
+		write_phy(lp, lp->phy_address, MII_BCMINTR_REG, dsintr);
 	}
 	else if (lp->phy_type == MII_KS8721_ID) {	/* for Micrel PHY */
-		read_phy(lp->phy_address, MII_TPISTATUS, &dsintr);
+		read_phy(lp, lp->phy_address, MII_TPISTATUS, &dsintr);
 		dsintr = ~((1 << 10) | (1 << 8));
-		write_phy(lp->phy_address, MII_TPISTATUS, dsintr);
+		write_phy(lp, lp->phy_address, MII_TPISTATUS, dsintr);
 	}
 	else if (lp->phy_type == MII_T78Q21x3_ID) {	/* for Teridian PHY */
-		read_phy(lp->phy_address, MII_T78Q21INT_REG, &dsintr);
+		read_phy(lp, lp->phy_address, MII_T78Q21INT_REG, &dsintr);
 		dsintr = dsintr & ~0x500;			/* clear bits 8, 10 */
-		write_phy(lp->phy_address, MII_T78Q21INT_REG, dsintr);
+		write_phy(lp, lp->phy_address, MII_T78Q21INT_REG, dsintr);
 	}
 	else if (lp->phy_type == MII_DP83848_ID) {	/* National Semiconductor DP83848 PHY */
-		read_phy(lp->phy_address, MII_DPMICR_REG, &dsintr);
+		read_phy(lp, lp->phy_address, MII_DPMICR_REG, &dsintr);
 		dsintr = dsintr & ~0x3;				/* clear bits 0, 1 */
-		write_phy(lp->phy_address, MII_DPMICR_REG, dsintr);
-		read_phy(lp->phy_address, MII_DPMISR_REG, &dsintr);
+		write_phy(lp, lp->phy_address, MII_DPMICR_REG, dsintr);
+		read_phy(lp, lp->phy_address, MII_DPMISR_REG, &dsintr);
 		dsintr = dsintr & ~0x3c;			/* clear bits 2..5 */
-		write_phy(lp->phy_address, MII_DPMISR_REG, dsintr);
+		write_phy(lp, lp->phy_address, MII_DPMISR_REG, dsintr);
 	}
 
-	disable_mdi();
+	disable_mdi(lp);
 	spin_unlock_irq(&lp->lock);
 
-	irq_number = lp->board_data.phy_irq_pin;
+	irq_number = gpio_to_irq(lp->board_data.phy_irq_pin);
 	free_irq(irq_number, dev);			/* Free interrupt handler */
 }
 
@@ -379,17 +377,17 @@ static void reset_phy(struct net_device *dev)
 	unsigned int bmcr;
 
 	spin_lock_irq(&lp->lock);
-	enable_mdi();
+	enable_mdi(lp);
 
 	/* Perform PHY reset */
-	write_phy(lp->phy_address, MII_BMCR, BMCR_RESET);
+	write_phy(lp, lp->phy_address, MII_BMCR, BMCR_RESET);
 
 	/* Wait until PHY reset is complete */
 	do {
-		read_phy(lp->phy_address, MII_BMCR, &bmcr);
+		read_phy(lp, lp->phy_address, MII_BMCR, &bmcr);
 	} while (!(bmcr & BMCR_RESET));
 
-	disable_mdi();
+	disable_mdi(lp);
 	spin_unlock_irq(&lp->lock);
 }
 #endif
@@ -399,13 +397,37 @@ static void at91ether_check_link(unsigned long dev_id)
 	struct net_device *dev = (struct net_device *) dev_id;
 	struct at91_private *lp = netdev_priv(dev);
 
-	enable_mdi();
+	enable_mdi(lp);
 	update_linkspeed(dev, 1);
-	disable_mdi();
+	disable_mdi(lp);
 
 	mod_timer(&lp->check_timer, jiffies + LINK_POLL_INTERVAL);
 }
 
+/*
+ * Perform any PHY-specific initialization.
+ */
+static void __init initialize_phy(struct at91_private *lp)
+{
+	unsigned int val;
+
+	spin_lock_irq(&lp->lock);
+	enable_mdi(lp);
+
+	if ((lp->phy_type == MII_DM9161_ID) || (lp->phy_type == MII_DM9161A_ID)) {
+		read_phy(lp, lp->phy_address, MII_DSCR_REG, &val);
+		if ((val & (1 << 10)) == 0)			/* DSCR bit 10 is 0 -- fiber mode */
+			lp->phy_media = PORT_FIBRE;
+	} else if (machine_is_csb337()) {
+		/* mix link activity status into LED2 link state */
+		write_phy(lp, lp->phy_address, MII_LEDCTRL_REG, 0x0d22);
+	} else if (machine_is_ecbat91())
+		write_phy(lp, lp->phy_address, MII_LEDCTRL_REG, 0x156A);
+
+	disable_mdi(lp);
+	spin_unlock_irq(&lp->lock);
+}
+
 /* ......................... ADDRESS MANAGEMENT ........................ */
 
 /*
@@ -454,17 +476,19 @@ static short __init unpack_mac_address(struct net_device *dev, unsigned int hi,
  */
 static void __init get_mac_address(struct net_device *dev)
 {
+	struct at91_private *lp = netdev_priv(dev);
+
 	/* Check Specific-Address 1 */
-	if (unpack_mac_address(dev, at91_emac_read(AT91_EMAC_SA1H), at91_emac_read(AT91_EMAC_SA1L)))
+	if (unpack_mac_address(dev, at91_emac_read(lp, AT91_EMAC_SA1H), at91_emac_read(lp, AT91_EMAC_SA1L)))
 		return;
 	/* Check Specific-Address 2 */
-	if (unpack_mac_address(dev, at91_emac_read(AT91_EMAC_SA2H), at91_emac_read(AT91_EMAC_SA2L)))
+	if (unpack_mac_address(dev, at91_emac_read(lp, AT91_EMAC_SA2H), at91_emac_read(lp, AT91_EMAC_SA2L)))
 		return;
 	/* Check Specific-Address 3 */
-	if (unpack_mac_address(dev, at91_emac_read(AT91_EMAC_SA3H), at91_emac_read(AT91_EMAC_SA3L)))
+	if (unpack_mac_address(dev, at91_emac_read(lp, AT91_EMAC_SA3H), at91_emac_read(lp, AT91_EMAC_SA3L)))
 		return;
 	/* Check Specific-Address 4 */
-	if (unpack_mac_address(dev, at91_emac_read(AT91_EMAC_SA4H), at91_emac_read(AT91_EMAC_SA4L)))
+	if (unpack_mac_address(dev, at91_emac_read(lp, AT91_EMAC_SA4H), at91_emac_read(lp, AT91_EMAC_SA4L)))
 		return;
 
 	printk(KERN_ERR "at91_ether: Your bootloader did not configure a MAC address.\n");
@@ -475,11 +499,13 @@ static void __init get_mac_address(struct net_device *dev)
  */
 static void update_mac_address(struct net_device *dev)
 {
-	at91_emac_write(AT91_EMAC_SA1L, (dev->dev_addr[3] << 24) | (dev->dev_addr[2] << 16) | (dev->dev_addr[1] << 8) | (dev->dev_addr[0]));
-	at91_emac_write(AT91_EMAC_SA1H, (dev->dev_addr[5] << 8) | (dev->dev_addr[4]));
+	struct at91_private *lp = netdev_priv(dev);
 
-	at91_emac_write(AT91_EMAC_SA2L, 0);
-	at91_emac_write(AT91_EMAC_SA2H, 0);
+	at91_emac_write(lp, AT91_EMAC_SA1L, (dev->dev_addr[3] << 24) | (dev->dev_addr[2] << 16) | (dev->dev_addr[1] << 8) | (dev->dev_addr[0]));
+	at91_emac_write(lp, AT91_EMAC_SA1H, (dev->dev_addr[5] << 8) | (dev->dev_addr[4]));
+
+	at91_emac_write(lp, AT91_EMAC_SA2L, 0);
+	at91_emac_write(lp, AT91_EMAC_SA2H, 0);
 }
 
 /*
@@ -559,6 +585,7 @@ static int hash_get_index(__u8 *addr)
  */
 static void at91ether_sethashtable(struct net_device *dev)
 {
+	struct at91_private *lp = netdev_priv(dev);
 	struct netdev_hw_addr *ha;
 	unsigned long mc_filter[2];
 	unsigned int bitnr;
@@ -570,8 +597,8 @@ static void at91ether_sethashtable(struct net_device *dev)
 		mc_filter[bitnr >> 5] |= 1 << (bitnr & 31);
 	}
 
-	at91_emac_write(AT91_EMAC_HSL, mc_filter[0]);
-	at91_emac_write(AT91_EMAC_HSH, mc_filter[1]);
+	at91_emac_write(lp, AT91_EMAC_HSL, mc_filter[0]);
+	at91_emac_write(lp, AT91_EMAC_HSH, mc_filter[1]);
 }
 
 /*
@@ -579,9 +606,10 @@ static void at91ether_sethashtable(struct net_device *dev)
  */
 static void at91ether_set_multicast_list(struct net_device *dev)
 {
+	struct at91_private *lp = netdev_priv(dev);
 	unsigned long cfg;
 
-	cfg = at91_emac_read(AT91_EMAC_CFG);
+	cfg = at91_emac_read(lp, AT91_EMAC_CFG);
 
 	if (dev->flags & IFF_PROMISC)			/* Enable promiscuous mode */
 		cfg |= AT91_EMAC_CAF;
@@ -589,34 +617,37 @@ static void at91ether_set_multicast_list(struct net_device *dev)
 		cfg &= ~AT91_EMAC_CAF;
 
 	if (dev->flags & IFF_ALLMULTI) {		/* Enable all multicast mode */
-		at91_emac_write(AT91_EMAC_HSH, -1);
-		at91_emac_write(AT91_EMAC_HSL, -1);
+		at91_emac_write(lp, AT91_EMAC_HSH, -1);
+		at91_emac_write(lp, AT91_EMAC_HSL, -1);
 		cfg |= AT91_EMAC_MTI;
 	} else if (!netdev_mc_empty(dev)) { /* Enable specific multicasts */
 		at91ether_sethashtable(dev);
 		cfg |= AT91_EMAC_MTI;
 	} else if (dev->flags & (~IFF_ALLMULTI)) {	/* Disable all multicast mode */
-		at91_emac_write(AT91_EMAC_HSH, 0);
-		at91_emac_write(AT91_EMAC_HSL, 0);
+		at91_emac_write(lp, AT91_EMAC_HSH, 0);
+		at91_emac_write(lp, AT91_EMAC_HSL, 0);
 		cfg &= ~AT91_EMAC_MTI;
 	}
 
-	at91_emac_write(AT91_EMAC_CFG, cfg);
+	at91_emac_write(lp, AT91_EMAC_CFG, cfg);
 }
 
 /* ......................... ETHTOOL SUPPORT ........................... */
 
 static int mdio_read(struct net_device *dev, int phy_id, int location)
 {
+	struct at91_private *lp = netdev_priv(dev);
 	unsigned int value;
 
-	read_phy(phy_id, location, &value);
+	read_phy(lp, phy_id, location, &value);
 	return value;
 }
 
 static void mdio_write(struct net_device *dev, int phy_id, int location, int value)
 {
-	write_phy(phy_id, location, value);
+	struct at91_private *lp = netdev_priv(dev);
+
+	write_phy(lp, phy_id, location, value);
 }
 
 static int at91ether_get_settings(struct net_device *dev, struct ethtool_cmd *cmd)
@@ -625,11 +656,11 @@ static int at91ether_get_settings(struct net_device *dev, struct ethtool_cmd *cm
 	int ret;
 
 	spin_lock_irq(&lp->lock);
-	enable_mdi();
+	enable_mdi(lp);
 
 	ret = mii_ethtool_gset(&lp->mii, cmd);
 
-	disable_mdi();
+	disable_mdi(lp);
 	spin_unlock_irq(&lp->lock);
 
 	if (lp->phy_media == PORT_FIBRE) {		/* override media type since mii.c doesn't know */
@@ -646,11 +677,11 @@ static int at91ether_set_settings(struct net_device *dev, struct ethtool_cmd *cm
 	int ret;
 
 	spin_lock_irq(&lp->lock);
-	enable_mdi();
+	enable_mdi(lp);
 
 	ret = mii_ethtool_sset(&lp->mii, cmd);
 
-	disable_mdi();
+	disable_mdi(lp);
 	spin_unlock_irq(&lp->lock);
 
 	return ret;
@@ -662,11 +693,11 @@ static int at91ether_nwayreset(struct net_device *dev)
 	int ret;
 
 	spin_lock_irq(&lp->lock);
-	enable_mdi();
+	enable_mdi(lp);
 
 	ret = mii_nway_restart(&lp->mii);
 
-	disable_mdi();
+	disable_mdi(lp);
 	spin_unlock_irq(&lp->lock);
 
 	return ret;
@@ -696,9 +727,9 @@ static int at91ether_ioctl(struct net_device *dev, struct ifreq *rq, int cmd)
 		return -EINVAL;
 
 	spin_lock_irq(&lp->lock);
-	enable_mdi();
+	enable_mdi(lp);
 	res = generic_mii_ioctl(&lp->mii, if_mii(rq), cmd, NULL);
-	disable_mdi();
+	disable_mdi(lp);
 	spin_unlock_irq(&lp->lock);
 
 	return res;
@@ -731,11 +762,11 @@ static void at91ether_start(struct net_device *dev)
 	lp->rxBuffIndex = 0;
 
 	/* Program address of descriptor list in Rx Buffer Queue register */
-	at91_emac_write(AT91_EMAC_RBQP, (unsigned long) dlist_phys);
+	at91_emac_write(lp, AT91_EMAC_RBQP, (unsigned long) dlist_phys);
 
 	/* Enable Receive and Transmit */
-	ctl = at91_emac_read(AT91_EMAC_CTL);
-	at91_emac_write(AT91_EMAC_CTL, ctl | AT91_EMAC_RE | AT91_EMAC_TE);
+	ctl = at91_emac_read(lp, AT91_EMAC_CTL);
+	at91_emac_write(lp, AT91_EMAC_CTL, ctl | AT91_EMAC_RE | AT91_EMAC_TE);
 }
 
 /*
@@ -752,8 +783,8 @@ static int at91ether_open(struct net_device *dev)
 	clk_enable(lp->ether_clk);		/* Re-enable Peripheral clock */
 
 	/* Clear internal statistics */
-	ctl = at91_emac_read(AT91_EMAC_CTL);
-	at91_emac_write(AT91_EMAC_CTL, ctl | AT91_EMAC_CSR);
+	ctl = at91_emac_read(lp, AT91_EMAC_CTL);
+	at91_emac_write(lp, AT91_EMAC_CTL, ctl | AT91_EMAC_CSR);
 
 	/* Update the MAC address (incase user has changed it) */
 	update_mac_address(dev);
@@ -762,15 +793,15 @@ static int at91ether_open(struct net_device *dev)
 	enable_phyirq(dev);
 
 	/* Enable MAC interrupts */
-	at91_emac_write(AT91_EMAC_IER, AT91_EMAC_RCOM | AT91_EMAC_RBNA
+	at91_emac_write(lp, AT91_EMAC_IER, AT91_EMAC_RCOM | AT91_EMAC_RBNA
 				| AT91_EMAC_TUND | AT91_EMAC_RTRY | AT91_EMAC_TCOM
 				| AT91_EMAC_ROVR | AT91_EMAC_ABT);
 
 	/* Determine current link speed */
 	spin_lock_irq(&lp->lock);
-	enable_mdi();
+	enable_mdi(lp);
 	update_linkspeed(dev, 0);
-	disable_mdi();
+	disable_mdi(lp);
 	spin_unlock_irq(&lp->lock);
 
 	at91ether_start(dev);
@@ -787,14 +818,14 @@ static int at91ether_close(struct net_device *dev)
 	unsigned long ctl;
 
 	/* Disable Receiver and Transmitter */
-	ctl = at91_emac_read(AT91_EMAC_CTL);
-	at91_emac_write(AT91_EMAC_CTL, ctl & ~(AT91_EMAC_TE | AT91_EMAC_RE));
+	ctl = at91_emac_read(lp, AT91_EMAC_CTL);
+	at91_emac_write(lp, AT91_EMAC_CTL, ctl & ~(AT91_EMAC_TE | AT91_EMAC_RE));
 
 	/* Disable PHY interrupt */
 	disable_phyirq(dev);
 
 	/* Disable MAC interrupts */
-	at91_emac_write(AT91_EMAC_IDR, AT91_EMAC_RCOM | AT91_EMAC_RBNA
+	at91_emac_write(lp, AT91_EMAC_IDR, AT91_EMAC_RCOM | AT91_EMAC_RBNA
 				| AT91_EMAC_TUND | AT91_EMAC_RTRY | AT91_EMAC_TCOM
 				| AT91_EMAC_ROVR | AT91_EMAC_ABT);
 
@@ -812,7 +843,7 @@ static int at91ether_start_xmit(struct sk_buff *skb, struct net_device *dev)
 {
 	struct at91_private *lp = netdev_priv(dev);
 
-	if (at91_emac_read(AT91_EMAC_TSR) & AT91_EMAC_TSR_BNQ) {
+	if (at91_emac_read(lp, AT91_EMAC_TSR) & AT91_EMAC_TSR_BNQ) {
 		netif_stop_queue(dev);
 
 		/* Store packet information (to free when Tx completed) */
@@ -822,9 +853,9 @@ static int at91ether_start_xmit(struct sk_buff *skb, struct net_device *dev)
 		dev->stats.tx_bytes += skb->len;
 
 		/* Set address of the data in the Transmit Address register */
-		at91_emac_write(AT91_EMAC_TAR, lp->skb_physaddr);
+		at91_emac_write(lp, AT91_EMAC_TAR, lp->skb_physaddr);
 		/* Set length of the packet in the Transmit Control register */
-		at91_emac_write(AT91_EMAC_TCR, skb->len);
+		at91_emac_write(lp, AT91_EMAC_TCR, skb->len);
 
 	} else {
 		printk(KERN_ERR "at91_ether.c: at91ether_start_xmit() called, but device is busy!\n");
@@ -841,31 +872,32 @@ static int at91ether_start_xmit(struct sk_buff *skb, struct net_device *dev)
  */
 static struct net_device_stats *at91ether_stats(struct net_device *dev)
 {
+	struct at91_private *lp = netdev_priv(dev);
 	int ale, lenerr, seqe, lcol, ecol;
 
 	if (netif_running(dev)) {
-		dev->stats.rx_packets += at91_emac_read(AT91_EMAC_OK);		/* Good frames received */
-		ale = at91_emac_read(AT91_EMAC_ALE);
+		dev->stats.rx_packets += at91_emac_read(lp, AT91_EMAC_OK);	/* Good frames received */
+		ale = at91_emac_read(lp, AT91_EMAC_ALE);
 		dev->stats.rx_frame_errors += ale;				/* Alignment errors */
-		lenerr = at91_emac_read(AT91_EMAC_ELR) + at91_emac_read(AT91_EMAC_USF);
+		lenerr = at91_emac_read(lp, AT91_EMAC_ELR) + at91_emac_read(lp, AT91_EMAC_USF);
 		dev->stats.rx_length_errors += lenerr;				/* Excessive Length or Undersize Frame error */
-		seqe = at91_emac_read(AT91_EMAC_SEQE);
+		seqe = at91_emac_read(lp, AT91_EMAC_SEQE);
 		dev->stats.rx_crc_errors += seqe;				/* CRC error */
-		dev->stats.rx_fifo_errors += at91_emac_read(AT91_EMAC_DRFC);	/* Receive buffer not available */
+		dev->stats.rx_fifo_errors += at91_emac_read(lp, AT91_EMAC_DRFC);/* Receive buffer not available */
 		dev->stats.rx_errors += (ale + lenerr + seqe
-			+ at91_emac_read(AT91_EMAC_CDE) + at91_emac_read(AT91_EMAC_RJB));
+			+ at91_emac_read(lp, AT91_EMAC_CDE) + at91_emac_read(lp, AT91_EMAC_RJB));
 
-		dev->stats.tx_packets += at91_emac_read(AT91_EMAC_FRA);		/* Frames successfully transmitted */
-		dev->stats.tx_fifo_errors += at91_emac_read(AT91_EMAC_TUE);	/* Transmit FIFO underruns */
-		dev->stats.tx_carrier_errors += at91_emac_read(AT91_EMAC_CSE);	/* Carrier Sense errors */
-		dev->stats.tx_heartbeat_errors += at91_emac_read(AT91_EMAC_SQEE);/* Heartbeat error */
+		dev->stats.tx_packets += at91_emac_read(lp, AT91_EMAC_FRA);	/* Frames successfully transmitted */
+		dev->stats.tx_fifo_errors += at91_emac_read(lp, AT91_EMAC_TUE);	/* Transmit FIFO underruns */
+		dev->stats.tx_carrier_errors += at91_emac_read(lp, AT91_EMAC_CSE);	/* Carrier Sense errors */
+		dev->stats.tx_heartbeat_errors += at91_emac_read(lp, AT91_EMAC_SQEE);/* Heartbeat error */
 
-		lcol = at91_emac_read(AT91_EMAC_LCOL);
-		ecol = at91_emac_read(AT91_EMAC_ECOL);
+		lcol = at91_emac_read(lp, AT91_EMAC_LCOL);
+		ecol = at91_emac_read(lp, AT91_EMAC_ECOL);
 		dev->stats.tx_window_errors += lcol;			/* Late collisions */
 		dev->stats.tx_aborted_errors += ecol;			/* 16 collisions */
 
-		dev->stats.collisions += (at91_emac_read(AT91_EMAC_SCOL) + at91_emac_read(AT91_EMAC_MCOL) + lcol + ecol);
+		dev->stats.collisions += (at91_emac_read(lp, AT91_EMAC_SCOL) + at91_emac_read(lp, AT91_EMAC_MCOL) + lcol + ecol);
 	}
 	return &dev->stats;
 }
@@ -922,7 +954,7 @@ static irqreturn_t at91ether_interrupt(int irq, void *dev_id)
 
 	/* MAC Interrupt Status register indicates what interrupts are pending.
 	   It is automatically cleared once read. */
-	intstatus = at91_emac_read(AT91_EMAC_ISR);
+	intstatus = at91_emac_read(lp, AT91_EMAC_ISR);
 
 	if (intstatus & AT91_EMAC_RCOM)		/* Receive complete */
 		at91ether_rx(dev);
@@ -942,9 +974,9 @@ static irqreturn_t at91ether_interrupt(int irq, void *dev_id)
 
 	/* Work-around for Errata #11 */
 	if (intstatus & AT91_EMAC_RBNA) {
-		ctl = at91_emac_read(AT91_EMAC_CTL);
-		at91_emac_write(AT91_EMAC_CTL, ctl & ~AT91_EMAC_RE);
-		at91_emac_write(AT91_EMAC_CTL, ctl | AT91_EMAC_RE);
+		ctl = at91_emac_read(lp, AT91_EMAC_CTL);
+		at91_emac_write(lp, AT91_EMAC_CTL, ctl & ~AT91_EMAC_RE);
+		at91_emac_write(lp, AT91_EMAC_CTL, ctl | AT91_EMAC_RE);
 	}
 
 	if (intstatus & AT91_EMAC_ROVR)
@@ -980,189 +1012,199 @@ static const struct net_device_ops at91ether_netdev_ops = {
 };
 
 /*
- * Initialize the ethernet interface
+ * Detect the PHY type, and its address.
  */
-static int __init at91ether_setup(unsigned long phy_type, unsigned short phy_address,
-			struct platform_device *pdev, struct clk *ether_clk)
+static int __init at91ether_phy_detect(struct at91_private *lp)
+{
+	unsigned int phyid1, phyid2;
+	unsigned long phy_id;
+	unsigned short phy_address = 0;
+
+	while (phy_address < PHY_MAX_ADDR) {
+		/* Read the PHY ID registers */
+		enable_mdi(lp);
+		read_phy(lp, phy_address, MII_PHYSID1, &phyid1);
+		read_phy(lp, phy_address, MII_PHYSID2, &phyid2);
+		disable_mdi(lp);
+
+		phy_id = (phyid1 << 16) | (phyid2 & 0xfff0);
+		switch (phy_id) {
+			case MII_DM9161_ID:		/* Davicom 9161: PHY_ID1 = 0x181, PHY_ID2 = B881 */
+			case MII_DM9161A_ID:		/* Davicom 9161A: PHY_ID1 = 0x181, PHY_ID2 = B8A0 */
+			case MII_LXT971A_ID:		/* Intel LXT971A: PHY_ID1 = 0x13, PHY_ID2 = 78E0 */
+			case MII_RTL8201_ID:		/* Realtek RTL8201: PHY_ID1 = 0, PHY_ID2 = 0x8201 */
+			case MII_BCM5221_ID:		/* Broadcom BCM5221: PHY_ID1 = 0x40, PHY_ID2 = 0x61e0 */
+			case MII_DP83847_ID:		/* National Semiconductor DP83847:  */
+			case MII_DP83848_ID:		/* National Semiconductor DP83848:  */
+			case MII_AC101L_ID:		/* Altima AC101L: PHY_ID1 = 0x22, PHY_ID2 = 0x5520 */
+			case MII_KS8721_ID:		/* Micrel KS8721: PHY_ID1 = 0x22, PHY_ID2 = 0x1610 */
+			case MII_T78Q21x3_ID:		/* Teridian 78Q21x3: PHY_ID1 = 0x0E, PHY_ID2 = 7237 */
+			case MII_LAN83C185_ID:		/* SMSC LAN83C185: PHY_ID1 = 0x0007, PHY_ID2 = 0xC0A1 */
+				/* store detected values */
+				lp->phy_type = phy_id;		/* Type of PHY connected */
+				lp->phy_address = phy_address;	/* MDI address of PHY */
+				return 1;
+		}
+
+		phy_address++;
+	}
+
+	return 0;		/* not detected */
+}
+
+
+/*
+ * Detect MAC & PHY and perform ethernet interface initialization
+ */
+static int __init at91ether_probe(struct platform_device *pdev)
 {
 	struct macb_platform_data *board_data = pdev->dev.platform_data;
+	struct resource *regs;
 	struct net_device *dev;
 	struct at91_private *lp;
-	unsigned int val;
 	int res;
 
+	regs = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	if (!regs)
+		return -ENOENT;
+
 	dev = alloc_etherdev(sizeof(struct at91_private));
 	if (!dev)
 		return -ENOMEM;
 
-	dev->base_addr = AT91_VA_BASE_EMAC;
-	dev->irq = AT91RM9200_ID_EMAC;
+	lp = netdev_priv(dev);
+	lp->board_data = *board_data;
+	spin_lock_init(&lp->lock);
+
+	dev->base_addr = regs->start;		/* physical base address */
+	lp->emac_base = ioremap(regs->start, regs->end - regs->start + 1);
+	if (!lp->emac_base) {
+		res = -ENOMEM;
+		goto err_free_dev;
+	}
+
+	/* Clock */
+	lp->ether_clk = clk_get(&pdev->dev, "ether_clk");
+	if (IS_ERR(lp->ether_clk)) {
+		res = -ENODEV;
+		goto err_ioumap;
+	}
+	clk_enable(lp->ether_clk);
 
 	/* Install the interrupt handler */
+	dev->irq = platform_get_irq(pdev, 0);
 	if (request_irq(dev->irq, at91ether_interrupt, 0, dev->name, dev)) {
-		free_netdev(dev);
-		return -EBUSY;
+		res = -EBUSY;
+		goto err_disable_clock;
 	}
 
 	/* Allocate memory for DMA Receive descriptors */
-	lp = netdev_priv(dev);
 	lp->dlist = (struct recv_desc_bufs *) dma_alloc_coherent(NULL, sizeof(struct recv_desc_bufs), (dma_addr_t *) &lp->dlist_phys, GFP_KERNEL);
 	if (lp->dlist == NULL) {
-		free_irq(dev->irq, dev);
-		free_netdev(dev);
-		return -ENOMEM;
+		res = -ENOMEM;
+		goto err_free_irq;
 	}
-	lp->board_data = *board_data;
-	lp->ether_clk = ether_clk;
-	platform_set_drvdata(pdev, dev);
-
-	spin_lock_init(&lp->lock);
 
 	ether_setup(dev);
 	dev->netdev_ops = &at91ether_netdev_ops;
 	dev->ethtool_ops = &at91ether_ethtool_ops;
-
+	platform_set_drvdata(pdev, dev);
 	SET_NETDEV_DEV(dev, &pdev->dev);
 
 	get_mac_address(dev);		/* Get ethernet address and store it in dev->dev_addr */
 	update_mac_address(dev);	/* Program ethernet address into MAC */
 
-	at91_emac_write(AT91_EMAC_CTL, 0);
+	at91_emac_write(lp, AT91_EMAC_CTL, 0);
 
-	if (lp->board_data.is_rmii)
-		at91_emac_write(AT91_EMAC_CFG, AT91_EMAC_CLK_DIV32 | AT91_EMAC_BIG | AT91_EMAC_RMII);
+	if (board_data->is_rmii)
+		at91_emac_write(lp, AT91_EMAC_CFG, AT91_EMAC_CLK_DIV32 | AT91_EMAC_BIG | AT91_EMAC_RMII);
 	else
-		at91_emac_write(AT91_EMAC_CFG, AT91_EMAC_CLK_DIV32 | AT91_EMAC_BIG);
+		at91_emac_write(lp, AT91_EMAC_CFG, AT91_EMAC_CLK_DIV32 | AT91_EMAC_BIG);
 
-	/* Perform PHY-specific initialization */
-	spin_lock_irq(&lp->lock);
-	enable_mdi();
-	if ((phy_type == MII_DM9161_ID) || (lp->phy_type == MII_DM9161A_ID)) {
-		read_phy(phy_address, MII_DSCR_REG, &val);
-		if ((val & (1 << 10)) == 0)			/* DSCR bit 10 is 0 -- fiber mode */
-			lp->phy_media = PORT_FIBRE;
-	} else if (machine_is_csb337()) {
-		/* mix link activity status into LED2 link state */
-		write_phy(phy_address, MII_LEDCTRL_REG, 0x0d22);
-	} else if (machine_is_ecbat91())
-		write_phy(phy_address, MII_LEDCTRL_REG, 0x156A);
+	/* Detect PHY */
+	if (!at91ether_phy_detect(lp)) {
+		printk(KERN_ERR "at91_ether: Could not detect ethernet PHY\n");
+		res = -ENODEV;
+		goto err_free_dmamem;
+	}
 
-	disable_mdi();
-	spin_unlock_irq(&lp->lock);
+	initialize_phy(lp);
 
 	lp->mii.dev = dev;		/* Support for ethtool */
 	lp->mii.mdio_read = mdio_read;
 	lp->mii.mdio_write = mdio_write;
-	lp->mii.phy_id = phy_address;
+	lp->mii.phy_id = lp->phy_address;
 	lp->mii.phy_id_mask = 0x1f;
 	lp->mii.reg_num_mask = 0x1f;
 
-	lp->phy_type = phy_type;	/* Type of PHY connected */
-	lp->phy_address = phy_address;	/* MDI address of PHY */
-
 	/* Register the network interface */
 	res = register_netdev(dev);
-	if (res) {
-		free_irq(dev->irq, dev);
-		free_netdev(dev);
-		dma_free_coherent(NULL, sizeof(struct recv_desc_bufs), lp->dlist, (dma_addr_t)lp->dlist_phys);
-		return res;
-	}
+	if (res)
+		goto err_free_dmamem;
 
 	/* Determine current link speed */
 	spin_lock_irq(&lp->lock);
-	enable_mdi();
+	enable_mdi(lp);
 	update_linkspeed(dev, 0);
-	disable_mdi();
+	disable_mdi(lp);
 	spin_unlock_irq(&lp->lock);
 	netif_carrier_off(dev);		/* will be enabled in open() */
 
 	/* If board has no PHY IRQ, use a timer to poll the PHY */
-	if (!gpio_is_valid(lp->board_data.phy_irq_pin)) {
+	if (gpio_is_valid(lp->board_data.phy_irq_pin)) {
+		gpio_request(board_data->phy_irq_pin, "ethernet_phy");
+	} else {
+		/* If board has no PHY IRQ, use a timer to poll the PHY */
 		init_timer(&lp->check_timer);
 		lp->check_timer.data = (unsigned long)dev;
 		lp->check_timer.function = at91ether_check_link;
-	} else if (lp->board_data.phy_irq_pin >= 32)
-		gpio_request(lp->board_data.phy_irq_pin, "ethernet_phy");
+	}
 
 	/* Display ethernet banner */
 	printk(KERN_INFO "%s: AT91 ethernet at 0x%08x int=%d %s%s (%pM)\n",
 	       dev->name, (uint) dev->base_addr, dev->irq,
-	       at91_emac_read(AT91_EMAC_CFG) & AT91_EMAC_SPD ? "100-" : "10-",
-	       at91_emac_read(AT91_EMAC_CFG) & AT91_EMAC_FD ? "FullDuplex" : "HalfDuplex",
+	       at91_emac_read(lp, AT91_EMAC_CFG) & AT91_EMAC_SPD ? "100-" : "10-",
+	       at91_emac_read(lp, AT91_EMAC_CFG) & AT91_EMAC_FD ? "FullDuplex" : "HalfDuplex",
 	       dev->dev_addr);
-	if ((phy_type == MII_DM9161_ID) || (lp->phy_type == MII_DM9161A_ID))
+	if ((lp->phy_type == MII_DM9161_ID) || (lp->phy_type == MII_DM9161A_ID))
 		printk(KERN_INFO "%s: Davicom 9161 PHY %s\n", dev->name, (lp->phy_media == PORT_FIBRE) ? "(Fiber)" : "(Copper)");
-	else if (phy_type == MII_LXT971A_ID)
+	else if (lp->phy_type == MII_LXT971A_ID)
 		printk(KERN_INFO "%s: Intel LXT971A PHY\n", dev->name);
-	else if (phy_type == MII_RTL8201_ID)
+	else if (lp->phy_type == MII_RTL8201_ID)
 		printk(KERN_INFO "%s: Realtek RTL8201(B)L PHY\n", dev->name);
-	else if (phy_type == MII_BCM5221_ID)
+	else if (lp->phy_type == MII_BCM5221_ID)
 		printk(KERN_INFO "%s: Broadcom BCM5221 PHY\n", dev->name);
-	else if (phy_type == MII_DP83847_ID)
+	else if (lp->phy_type == MII_DP83847_ID)
 		printk(KERN_INFO "%s: National Semiconductor DP83847 PHY\n", dev->name);
-	else if (phy_type == MII_DP83848_ID)
+	else if (lp->phy_type == MII_DP83848_ID)
 		printk(KERN_INFO "%s: National Semiconductor DP83848 PHY\n", dev->name);
-	else if (phy_type == MII_AC101L_ID)
+	else if (lp->phy_type == MII_AC101L_ID)
 		printk(KERN_INFO "%s: Altima AC101L PHY\n", dev->name);
-	else if (phy_type == MII_KS8721_ID)
+	else if (lp->phy_type == MII_KS8721_ID)
 		printk(KERN_INFO "%s: Micrel KS8721 PHY\n", dev->name);
-	else if (phy_type == MII_T78Q21x3_ID)
+	else if (lp->phy_type == MII_T78Q21x3_ID)
 		printk(KERN_INFO "%s: Teridian 78Q21x3 PHY\n", dev->name);
-	else if (phy_type == MII_LAN83C185_ID)
+	else if (lp->phy_type == MII_LAN83C185_ID)
 		printk(KERN_INFO "%s: SMSC LAN83C185 PHY\n", dev->name);
 
-	return 0;
-}
-
-/*
- * Detect MAC and PHY and perform initialization
- */
-static int __init at91ether_probe(struct platform_device *pdev)
-{
-	unsigned int phyid1, phyid2;
-	int detected = -1;
-	unsigned long phy_id;
-	unsigned short phy_address = 0;
-	struct clk *ether_clk;
-
-	ether_clk = clk_get(&pdev->dev, "ether_clk");
-	if (IS_ERR(ether_clk)) {
-		printk(KERN_ERR "at91_ether: no clock defined\n");
-		return -ENODEV;
-	}
-	clk_enable(ether_clk);					/* Enable Peripheral clock */
-
-	while ((detected != 0) && (phy_address < 32)) {
-		/* Read the PHY ID registers */
-		enable_mdi();
-		read_phy(phy_address, MII_PHYSID1, &phyid1);
-		read_phy(phy_address, MII_PHYSID2, &phyid2);
-		disable_mdi();
-
-		phy_id = (phyid1 << 16) | (phyid2 & 0xfff0);
-		switch (phy_id) {
-			case MII_DM9161_ID:		/* Davicom 9161: PHY_ID1 = 0x181, PHY_ID2 = B881 */
-			case MII_DM9161A_ID:		/* Davicom 9161A: PHY_ID1 = 0x181, PHY_ID2 = B8A0 */
-			case MII_LXT971A_ID:		/* Intel LXT971A: PHY_ID1 = 0x13, PHY_ID2 = 78E0 */
-			case MII_RTL8201_ID:		/* Realtek RTL8201: PHY_ID1 = 0, PHY_ID2 = 0x8201 */
-			case MII_BCM5221_ID:		/* Broadcom BCM5221: PHY_ID1 = 0x40, PHY_ID2 = 0x61e0 */
-			case MII_DP83847_ID:		/* National Semiconductor DP83847:  */
-			case MII_DP83848_ID:		/* National Semiconductor DP83848:  */
-			case MII_AC101L_ID:		/* Altima AC101L: PHY_ID1 = 0x22, PHY_ID2 = 0x5520 */
-			case MII_KS8721_ID:		/* Micrel KS8721: PHY_ID1 = 0x22, PHY_ID2 = 0x1610 */
-			case MII_T78Q21x3_ID:		/* Teridian 78Q21x3: PHY_ID1 = 0x0E, PHY_ID2 = 7237 */
-			case MII_LAN83C185_ID:		/* SMSC LAN83C185: PHY_ID1 = 0x0007, PHY_ID2 = 0xC0A1 */
-				detected = at91ether_setup(phy_id, phy_address, pdev, ether_clk);
-				break;
-		}
+	clk_disable(lp->ether_clk);					/* Disable Peripheral clock */
 
-		phy_address++;
-	}
+	return 0;
 
-	clk_disable(ether_clk);					/* Disable Peripheral clock */
 
-	return detected;
+err_free_dmamem:
+	platform_set_drvdata(pdev, NULL);
+	dma_free_coherent(NULL, sizeof(struct recv_desc_bufs), lp->dlist, (dma_addr_t)lp->dlist_phys);
+err_free_irq:
+	free_irq(dev->irq, dev);
+err_disable_clock:
+	clk_disable(lp->ether_clk);
+	clk_put(lp->ether_clk);
+err_ioumap:
+	iounmap(lp->emac_base);
+err_free_dev:
+	free_netdev(dev);
+	return res;
 }
 
 static int __devexit at91ether_remove(struct platform_device *pdev)
@@ -1170,8 +1212,7 @@ static int __devexit at91ether_remove(struct platform_device *pdev)
 	struct net_device *dev = platform_get_drvdata(pdev);
 	struct at91_private *lp = netdev_priv(dev);
 
-	if (gpio_is_valid(lp->board_data.phy_irq_pin) &&
-	    lp->board_data.phy_irq_pin >= 32)
+	if (gpio_is_valid(lp->board_data.phy_irq_pin))
 		gpio_free(lp->board_data.phy_irq_pin);
 
 	unregister_netdev(dev);
@@ -1193,7 +1234,7 @@ static int at91ether_suspend(struct platform_device *pdev, pm_message_t mesg)
 
 	if (netif_running(net_dev)) {
 		if (gpio_is_valid(lp->board_data.phy_irq_pin)) {
-			int phy_irq = lp->board_data.phy_irq_pin;
+			int phy_irq = gpio_to_irq(lp->board_data.phy_irq_pin);
 			disable_irq(phy_irq);
 		}
 
@@ -1217,7 +1258,7 @@ static int at91ether_resume(struct platform_device *pdev)
 		netif_start_queue(net_dev);
 
 		if (gpio_is_valid(lp->board_data.phy_irq_pin)) {
-			int phy_irq = lp->board_data.phy_irq_pin;
+			int phy_irq = gpio_to_irq(lp->board_data.phy_irq_pin);
 			enable_irq(phy_irq);
 		}
 	}
diff --git a/drivers/net/ethernet/cadence/at91_ether.h b/drivers/net/ethernet/cadence/at91_ether.h
index 3725fbb0def..0ef6328fa7f 100644
--- a/drivers/net/ethernet/cadence/at91_ether.h
+++ b/drivers/net/ethernet/cadence/at91_ether.h
@@ -88,6 +88,7 @@ struct at91_private
 	struct macb_platform_data board_data;	/* board-specific
 						 * configuration (shared with
 						 * macb for common data */
+	void __iomem *emac_base;		/* base register address */
 	struct clk *ether_clk;			/* clock */
 
 	/* PHY */
diff --git a/drivers/net/ethernet/cadence/macb.c b/drivers/net/ethernet/cadence/macb.c
index c4834c23be3..1466bc4e3dd 100644
--- a/drivers/net/ethernet/cadence/macb.c
+++ b/drivers/net/ethernet/cadence/macb.c
@@ -1213,6 +1213,7 @@ static const struct ethtool_ops macb_ethtool_ops = {
 	.set_settings		= macb_set_settings,
 	.get_drvinfo		= macb_get_drvinfo,
 	.get_link		= ethtool_op_get_link,
+	.get_ts_info		= ethtool_op_get_ts_info,
 };
 
 static int macb_ioctl(struct net_device *dev, struct ifreq *rq, int cmd)
diff --git a/drivers/net/ethernet/chelsio/cxgb4/cxgb4.h b/drivers/net/ethernet/chelsio/cxgb4/cxgb4.h
index 0fe18850c83..ec2dafe8ae5 100644
--- a/drivers/net/ethernet/chelsio/cxgb4/cxgb4.h
+++ b/drivers/net/ethernet/chelsio/cxgb4/cxgb4.h
@@ -51,6 +51,8 @@
 #define FW_VERSION_MINOR 1
 #define FW_VERSION_MICRO 0
 
+#define CH_WARN(adap, fmt, ...) dev_warn(adap->pdev_dev, fmt, ## __VA_ARGS__)
+
 enum {
 	MAX_NPORTS = 4,     /* max # of ports */
 	SERNUM_LEN = 24,    /* Serial # length */
@@ -64,6 +66,15 @@ enum {
 	MEM_MC
 };
 
+enum {
+	MEMWIN0_APERTURE = 65536,
+	MEMWIN0_BASE     = 0x30000,
+	MEMWIN1_APERTURE = 32768,
+	MEMWIN1_BASE     = 0x28000,
+	MEMWIN2_APERTURE = 2048,
+	MEMWIN2_BASE     = 0x1b800,
+};
+
 enum dev_master {
 	MASTER_CANT,
 	MASTER_MAY,
@@ -403,6 +414,9 @@ struct sge_txq {
 	struct tx_sw_desc *sdesc;   /* address of SW Tx descriptor ring */
 	struct sge_qstat *stat;     /* queue status entry */
 	dma_addr_t    phys_addr;    /* physical address of the ring */
+	spinlock_t db_lock;
+	int db_disabled;
+	unsigned short db_pidx;
 };
 
 struct sge_eth_txq {                /* state for an SGE Ethernet Tx queue */
@@ -475,6 +489,7 @@ struct adapter {
 	void __iomem *regs;
 	struct pci_dev *pdev;
 	struct device *pdev_dev;
+	unsigned int mbox;
 	unsigned int fn;
 	unsigned int flags;
 
@@ -504,6 +519,8 @@ struct adapter {
 	void **tid_release_head;
 	spinlock_t tid_release_lock;
 	struct work_struct tid_release_task;
+	struct work_struct db_full_task;
+	struct work_struct db_drop_task;
 	bool tid_release_task_busy;
 
 	struct dentry *debugfs_root;
@@ -605,6 +622,7 @@ irqreturn_t t4_sge_intr_msix(int irq, void *cookie);
 void t4_sge_init(struct adapter *adap);
 void t4_sge_start(struct adapter *adap);
 void t4_sge_stop(struct adapter *adap);
+extern int dbfifo_int_thresh;
 
 #define for_each_port(adapter, iter) \
 	for (iter = 0; iter < (adapter)->params.nports; ++iter)
@@ -719,4 +737,9 @@ int t4_ctrl_eq_free(struct adapter *adap, unsigned int mbox, unsigned int pf,
 int t4_ofld_eq_free(struct adapter *adap, unsigned int mbox, unsigned int pf,
 		    unsigned int vf, unsigned int eqid);
 int t4_handle_fw_rpl(struct adapter *adap, const __be64 *rpl);
+void t4_db_full(struct adapter *adapter);
+void t4_db_dropped(struct adapter *adapter);
+int t4_mem_win_read_len(struct adapter *adap, u32 addr, __be32 *data, int len);
+int t4_fwaddrspace_write(struct adapter *adap, unsigned int mbox,
+			 u32 addr, u32 val);
 #endif /* __CXGB4_H__ */
diff --git a/drivers/net/ethernet/chelsio/cxgb4/cxgb4_main.c b/drivers/net/ethernet/chelsio/cxgb4/cxgb4_main.c
index b126b98065a..e1f96fbb48c 100644
--- a/drivers/net/ethernet/chelsio/cxgb4/cxgb4_main.c
+++ b/drivers/net/ethernet/chelsio/cxgb4/cxgb4_main.c
@@ -149,15 +149,6 @@ static unsigned int pfvfres_pmask(struct adapter *adapter,
 #endif
 
 enum {
-	MEMWIN0_APERTURE = 65536,
-	MEMWIN0_BASE     = 0x30000,
-	MEMWIN1_APERTURE = 32768,
-	MEMWIN1_BASE     = 0x28000,
-	MEMWIN2_APERTURE = 2048,
-	MEMWIN2_BASE     = 0x1b800,
-};
-
-enum {
 	MAX_TXQ_ENTRIES      = 16384,
 	MAX_CTRL_TXQ_ENTRIES = 1024,
 	MAX_RSPQ_ENTRIES     = 16384,
@@ -371,6 +362,15 @@ static int set_addr_filters(const struct net_device *dev, bool sleep)
 				uhash | mhash, sleep);
 }
 
+int dbfifo_int_thresh = 10; /* 10 == 640 entry threshold */
+module_param(dbfifo_int_thresh, int, 0644);
+MODULE_PARM_DESC(dbfifo_int_thresh, "doorbell fifo interrupt threshold");
+
+int dbfifo_drain_delay = 1000; /* usecs to sleep while draining the dbfifo */
+module_param(dbfifo_drain_delay, int, 0644);
+MODULE_PARM_DESC(dbfifo_drain_delay,
+		 "usecs to sleep while draining the dbfifo");
+
 /*
  * Set Rx properties of a port, such as promiscruity, address filters, and MTU.
  * If @mtu is -1 it is left unchanged.
@@ -389,6 +389,8 @@ static int set_rxmode(struct net_device *dev, int mtu, bool sleep_ok)
 	return ret;
 }
 
+static struct workqueue_struct *workq;
+
 /**
  *	link_start - enable a port
  *	@dev: the port to enable
@@ -2196,7 +2198,7 @@ static void cxgb4_queue_tid_release(struct tid_info *t, unsigned int chan,
 	adap->tid_release_head = (void **)((uintptr_t)p | chan);
 	if (!adap->tid_release_task_busy) {
 		adap->tid_release_task_busy = true;
-		schedule_work(&adap->tid_release_task);
+		queue_work(workq, &adap->tid_release_task);
 	}
 	spin_unlock_bh(&adap->tid_release_lock);
 }
@@ -2366,6 +2368,16 @@ unsigned int cxgb4_port_chan(const struct net_device *dev)
 }
 EXPORT_SYMBOL(cxgb4_port_chan);
 
+unsigned int cxgb4_dbfifo_count(const struct net_device *dev, int lpfifo)
+{
+	struct adapter *adap = netdev2adap(dev);
+	u32 v;
+
+	v = t4_read_reg(adap, A_SGE_DBFIFO_STATUS);
+	return lpfifo ? G_LP_COUNT(v) : G_HP_COUNT(v);
+}
+EXPORT_SYMBOL(cxgb4_dbfifo_count);
+
 /**
  *	cxgb4_port_viid - get the VI id of a port
  *	@dev: the net device for the port
@@ -2413,6 +2425,59 @@ void cxgb4_iscsi_init(struct net_device *dev, unsigned int tag_mask,
 }
 EXPORT_SYMBOL(cxgb4_iscsi_init);
 
+int cxgb4_flush_eq_cache(struct net_device *dev)
+{
+	struct adapter *adap = netdev2adap(dev);
+	int ret;
+
+	ret = t4_fwaddrspace_write(adap, adap->mbox,
+				   0xe1000000 + A_SGE_CTXT_CMD, 0x20000000);
+	return ret;
+}
+EXPORT_SYMBOL(cxgb4_flush_eq_cache);
+
+static int read_eq_indices(struct adapter *adap, u16 qid, u16 *pidx, u16 *cidx)
+{
+	u32 addr = t4_read_reg(adap, A_SGE_DBQ_CTXT_BADDR) + 24 * qid + 8;
+	__be64 indices;
+	int ret;
+
+	ret = t4_mem_win_read_len(adap, addr, (__be32 *)&indices, 8);
+	if (!ret) {
+		indices = be64_to_cpu(indices);
+		*cidx = (indices >> 25) & 0xffff;
+		*pidx = (indices >> 9) & 0xffff;
+	}
+	return ret;
+}
+
+int cxgb4_sync_txq_pidx(struct net_device *dev, u16 qid, u16 pidx,
+			u16 size)
+{
+	struct adapter *adap = netdev2adap(dev);
+	u16 hw_pidx, hw_cidx;
+	int ret;
+
+	ret = read_eq_indices(adap, qid, &hw_pidx, &hw_cidx);
+	if (ret)
+		goto out;
+
+	if (pidx != hw_pidx) {
+		u16 delta;
+
+		if (pidx >= hw_pidx)
+			delta = pidx - hw_pidx;
+		else
+			delta = size - hw_pidx + pidx;
+		wmb();
+		t4_write_reg(adap, MYPF_REG(A_SGE_PF_KDOORBELL),
+			     V_QID(qid) | V_PIDX(delta));
+	}
+out:
+	return ret;
+}
+EXPORT_SYMBOL(cxgb4_sync_txq_pidx);
+
 static struct pci_driver cxgb4_driver;
 
 static void check_neigh_update(struct neighbour *neigh)
@@ -2446,6 +2511,144 @@ static struct notifier_block cxgb4_netevent_nb = {
 	.notifier_call = netevent_cb
 };
 
+static void drain_db_fifo(struct adapter *adap, int usecs)
+{
+	u32 v;
+
+	do {
+		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);
+}
+
+static void disable_txq_db(struct sge_txq *q)
+{
+	spin_lock_irq(&q->db_lock);
+	q->db_disabled = 1;
+	spin_unlock_irq(&q->db_lock);
+}
+
+static void enable_txq_db(struct sge_txq *q)
+{
+	spin_lock_irq(&q->db_lock);
+	q->db_disabled = 0;
+	spin_unlock_irq(&q->db_lock);
+}
+
+static void disable_dbs(struct adapter *adap)
+{
+	int i;
+
+	for_each_ethrxq(&adap->sge, i)
+		disable_txq_db(&adap->sge.ethtxq[i].q);
+	for_each_ofldrxq(&adap->sge, i)
+		disable_txq_db(&adap->sge.ofldtxq[i].q);
+	for_each_port(adap, i)
+		disable_txq_db(&adap->sge.ctrlq[i].q);
+}
+
+static void enable_dbs(struct adapter *adap)
+{
+	int i;
+
+	for_each_ethrxq(&adap->sge, i)
+		enable_txq_db(&adap->sge.ethtxq[i].q);
+	for_each_ofldrxq(&adap->sge, i)
+		enable_txq_db(&adap->sge.ofldtxq[i].q);
+	for_each_port(adap, i)
+		enable_txq_db(&adap->sge.ctrlq[i].q);
+}
+
+static void sync_txq_pidx(struct adapter *adap, struct sge_txq *q)
+{
+	u16 hw_pidx, hw_cidx;
+	int ret;
+
+	spin_lock_bh(&q->db_lock);
+	ret = read_eq_indices(adap, (u16)q->cntxt_id, &hw_pidx, &hw_cidx);
+	if (ret)
+		goto out;
+	if (q->db_pidx != hw_pidx) {
+		u16 delta;
+
+		if (q->db_pidx >= hw_pidx)
+			delta = q->db_pidx - hw_pidx;
+		else
+			delta = q->size - hw_pidx + q->db_pidx;
+		wmb();
+		t4_write_reg(adap, MYPF_REG(A_SGE_PF_KDOORBELL),
+				V_QID(q->cntxt_id) | V_PIDX(delta));
+	}
+out:
+	q->db_disabled = 0;
+	spin_unlock_bh(&q->db_lock);
+	if (ret)
+		CH_WARN(adap, "DB drop recovery failed.\n");
+}
+static void recover_all_queues(struct adapter *adap)
+{
+	int i;
+
+	for_each_ethrxq(&adap->sge, i)
+		sync_txq_pidx(adap, &adap->sge.ethtxq[i].q);
+	for_each_ofldrxq(&adap->sge, i)
+		sync_txq_pidx(adap, &adap->sge.ofldtxq[i].q);
+	for_each_port(adap, i)
+		sync_txq_pidx(adap, &adap->sge.ctrlq[i].q);
+}
+
+static void notify_rdma_uld(struct adapter *adap, enum cxgb4_control cmd)
+{
+	mutex_lock(&uld_mutex);
+	if (adap->uld_handle[CXGB4_ULD_RDMA])
+		ulds[CXGB4_ULD_RDMA].control(adap->uld_handle[CXGB4_ULD_RDMA],
+				cmd);
+	mutex_unlock(&uld_mutex);
+}
+
+static void process_db_full(struct work_struct *work)
+{
+	struct adapter *adap;
+
+	adap = container_of(work, struct adapter, db_full_task);
+
+	notify_rdma_uld(adap, CXGB4_CONTROL_DB_FULL);
+	drain_db_fifo(adap, dbfifo_drain_delay);
+	t4_set_reg_field(adap, A_SGE_INT_ENABLE3,
+			F_DBFIFO_HP_INT | F_DBFIFO_LP_INT,
+			F_DBFIFO_HP_INT | F_DBFIFO_LP_INT);
+	notify_rdma_uld(adap, CXGB4_CONTROL_DB_EMPTY);
+}
+
+static void process_db_drop(struct work_struct *work)
+{
+	struct adapter *adap;
+
+	adap = container_of(work, struct adapter, db_drop_task);
+
+	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, A_SGE_INT_ENABLE3,
+			F_DBFIFO_HP_INT | F_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);
+}
+
 static void uld_attach(struct adapter *adap, unsigned int uld)
 {
 	void *handle;
@@ -2479,6 +2682,7 @@ static void uld_attach(struct adapter *adap, unsigned int uld)
 	lli.gts_reg = adap->regs + MYPF_REG(SGE_PF_GTS);
 	lli.db_reg = adap->regs + MYPF_REG(SGE_PF_KDOORBELL);
 	lli.fw_vers = adap->params.fw_vers;
+	lli.dbfifo_int_thresh = dbfifo_int_thresh;
 
 	handle = ulds[uld].add(&lli);
 	if (IS_ERR(handle)) {
@@ -2649,6 +2853,8 @@ static void cxgb_down(struct adapter *adapter)
 {
 	t4_intr_disable(adapter);
 	cancel_work_sync(&adapter->tid_release_task);
+	cancel_work_sync(&adapter->db_full_task);
+	cancel_work_sync(&adapter->db_drop_task);
 	adapter->tid_release_task_busy = false;
 	adapter->tid_release_head = NULL;
 
@@ -3593,6 +3799,7 @@ static int __devinit init_one(struct pci_dev *pdev,
 
 	adapter->pdev = pdev;
 	adapter->pdev_dev = &pdev->dev;
+	adapter->mbox = func;
 	adapter->fn = func;
 	adapter->msg_enable = dflt_msg_enable;
 	memset(adapter->chan_map, 0xff, sizeof(adapter->chan_map));
@@ -3601,6 +3808,8 @@ static int __devinit init_one(struct pci_dev *pdev,
 	spin_lock_init(&adapter->tid_release_lock);
 
 	INIT_WORK(&adapter->tid_release_task, process_tid_release_list);
+	INIT_WORK(&adapter->db_full_task, process_db_full);
+	INIT_WORK(&adapter->db_drop_task, process_db_drop);
 
 	err = t4_prep_adapter(adapter);
 	if (err)
@@ -3788,6 +3997,10 @@ static int __init cxgb4_init_module(void)
 {
 	int ret;
 
+	workq = create_singlethread_workqueue("cxgb4");
+	if (!workq)
+		return -ENOMEM;
+
 	/* Debugfs support is optional, just warn if this fails */
 	cxgb4_debugfs_root = debugfs_create_dir(KBUILD_MODNAME, NULL);
 	if (!cxgb4_debugfs_root)
@@ -3803,6 +4016,8 @@ static void __exit cxgb4_cleanup_module(void)
 {
 	pci_unregister_driver(&cxgb4_driver);
 	debugfs_remove(cxgb4_debugfs_root);  /* NULL ok */
+	flush_workqueue(workq);
+	destroy_workqueue(workq);
 }
 
 module_init(cxgb4_init_module);
diff --git a/drivers/net/ethernet/chelsio/cxgb4/cxgb4_uld.h b/drivers/net/ethernet/chelsio/cxgb4/cxgb4_uld.h
index b1d39b8d141..d79980c5fc6 100644
--- a/drivers/net/ethernet/chelsio/cxgb4/cxgb4_uld.h
+++ b/drivers/net/ethernet/chelsio/cxgb4/cxgb4_uld.h
@@ -163,6 +163,12 @@ enum cxgb4_state {
 	CXGB4_STATE_DETACH
 };
 
+enum cxgb4_control {
+	CXGB4_CONTROL_DB_FULL,
+	CXGB4_CONTROL_DB_EMPTY,
+	CXGB4_CONTROL_DB_DROP,
+};
+
 struct pci_dev;
 struct l2t_data;
 struct net_device;
@@ -212,6 +218,7 @@ struct cxgb4_lld_info {
 	unsigned short ucq_density;          /* # of user CQs/page */
 	void __iomem *gts_reg;               /* address of GTS register */
 	void __iomem *db_reg;                /* address of kernel doorbell */
+	int dbfifo_int_thresh;		     /* doorbell fifo int threshold */
 };
 
 struct cxgb4_uld_info {
@@ -220,11 +227,13 @@ struct cxgb4_uld_info {
 	int (*rx_handler)(void *handle, const __be64 *rsp,
 			  const struct pkt_gl *gl);
 	int (*state_change)(void *handle, enum cxgb4_state new_state);
+	int (*control)(void *handle, enum cxgb4_control control, ...);
 };
 
 int cxgb4_register_uld(enum cxgb4_uld type, const struct cxgb4_uld_info *p);
 int cxgb4_unregister_uld(enum cxgb4_uld type);
 int cxgb4_ofld_send(struct net_device *dev, struct sk_buff *skb);
+unsigned int cxgb4_dbfifo_count(const struct net_device *dev, int lpfifo);
 unsigned int cxgb4_port_chan(const struct net_device *dev);
 unsigned int cxgb4_port_viid(const struct net_device *dev);
 unsigned int cxgb4_port_idx(const struct net_device *dev);
@@ -236,4 +245,6 @@ void cxgb4_iscsi_init(struct net_device *dev, unsigned int tag_mask,
 		      const unsigned int *pgsz_order);
 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);
 #endif  /* !__CXGB4_OFLD_H */
diff --git a/drivers/net/ethernet/chelsio/cxgb4/sge.c b/drivers/net/ethernet/chelsio/cxgb4/sge.c
index 2dae7959f00..e111d974afd 100644
--- a/drivers/net/ethernet/chelsio/cxgb4/sge.c
+++ b/drivers/net/ethernet/chelsio/cxgb4/sge.c
@@ -767,8 +767,13 @@ 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)
 {
 	wmb();            /* write descriptors before telling HW */
-	t4_write_reg(adap, MYPF_REG(SGE_PF_KDOORBELL),
-		     QID(q->cntxt_id) | PIDX(n));
+	spin_lock(&q->db_lock);
+	if (!q->db_disabled) {
+		t4_write_reg(adap, MYPF_REG(A_SGE_PF_KDOORBELL),
+			     V_QID(q->cntxt_id) | V_PIDX(n));
+	}
+	q->db_pidx = q->pidx;
+	spin_unlock(&q->db_lock);
 }
 
 /**
@@ -2081,6 +2086,7 @@ static void init_txq(struct adapter *adap, struct sge_txq *q, unsigned int id)
 	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;
 }
 
@@ -2415,6 +2421,18 @@ void t4_sge_init(struct adapter *adap)
 			 RXPKTCPLMODE |
 			 (STAT_LEN == 128 ? EGRSTATUSPAGESIZE : 0));
 
+	/*
+	 * 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));
+	t4_set_reg_field(adap, A_SGE_DOORBELL_CONTROL, F_ENABLE_DROP,
+			F_ENABLE_DROP);
+
 	for (i = v = 0; i < 32; i += 4)
 		v |= (PAGE_SHIFT - 10) << i;
 	t4_write_reg(adap, SGE_HOST_PAGE_SIZE, v);
diff --git a/drivers/net/ethernet/chelsio/cxgb4/t4_hw.c b/drivers/net/ethernet/chelsio/cxgb4/t4_hw.c
index d1ec111aebd..32e1dd566a1 100644
--- a/drivers/net/ethernet/chelsio/cxgb4/t4_hw.c
+++ b/drivers/net/ethernet/chelsio/cxgb4/t4_hw.c
@@ -868,11 +868,14 @@ int t4_restart_aneg(struct adapter *adap, unsigned int mbox, unsigned int port)
 	return t4_wr_mbox(adap, mbox, &c, sizeof(c), NULL);
 }
 
+typedef void (*int_handler_t)(struct adapter *adap);
+
 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 */
+	int_handler_t int_handler; /* platform-specific int handler */
 };
 
 /**
@@ -905,6 +908,8 @@ static int t4_handle_intr_status(struct adapter *adapter, unsigned int reg,
 		} else if (acts->msg && printk_ratelimit())
 			dev_warn(adapter->pdev_dev, "%s (0x%x)\n", acts->msg,
 				 status & acts->mask);
+		if (acts->int_handler)
+			acts->int_handler(adapter);
 		mask |= acts->mask;
 	}
 	status &= mask;
@@ -1013,7 +1018,9 @@ static void sge_intr_handler(struct adapter *adapter)
 		{ ERR_INVALID_CIDX_INC,
 		  "SGE GTS CIDX increment too large", -1, 0 },
 		{ ERR_CPL_OPCODE_0, "SGE received 0-length CPL", -1, 0 },
-		{ ERR_DROPPED_DB, "SGE doorbell dropped", -1, 0 },
+		{ F_DBFIFO_LP_INT, NULL, -1, 0, t4_db_full },
+		{ F_DBFIFO_HP_INT, NULL, -1, 0, t4_db_full },
+		{ F_ERR_DROPPED_DB, NULL, -1, 0, t4_db_dropped },
 		{ ERR_DATA_CPL_ON_HIGH_QID1 | ERR_DATA_CPL_ON_HIGH_QID0,
 		  "SGE IQID > 1023 received CPL for FL", -1, 0 },
 		{ ERR_BAD_DB_PIDX3, "SGE DBP 3 pidx increment too large", -1,
@@ -1034,10 +1041,10 @@ static void sge_intr_handler(struct adapter *adapter)
 	};
 
 	v = (u64)t4_read_reg(adapter, SGE_INT_CAUSE1) |
-	    ((u64)t4_read_reg(adapter, SGE_INT_CAUSE2) << 32);
+		((u64)t4_read_reg(adapter, SGE_INT_CAUSE2) << 32);
 	if (v) {
 		dev_alert(adapter->pdev_dev, "SGE parity error (%#llx)\n",
-			 (unsigned long long)v);
+				(unsigned long long)v);
 		t4_write_reg(adapter, SGE_INT_CAUSE1, v);
 		t4_write_reg(adapter, SGE_INT_CAUSE2, v >> 32);
 	}
@@ -1513,6 +1520,7 @@ void t4_intr_enable(struct adapter *adapter)
 		     ERR_BAD_DB_PIDX2 | ERR_BAD_DB_PIDX1 |
 		     ERR_BAD_DB_PIDX0 | ERR_ING_CTXT_PRIO |
 		     ERR_EGR_CTXT_PRIO | INGRESS_SIZE_ERR |
+		     F_DBFIFO_HP_INT | F_DBFIFO_LP_INT |
 		     EGRESS_SIZE_ERR);
 	t4_write_reg(adapter, MYPF_REG(PL_PF_INT_ENABLE), PF_INTR_MASK);
 	t4_set_reg_field(adapter, PL_INT_MAP0, 0, 1 << pf);
@@ -1986,6 +1994,54 @@ int t4_wol_pat_enable(struct adapter *adap, unsigned int port, unsigned int map,
 	(var).retval_len16 = htonl(FW_LEN16(var)); \
 } while (0)
 
+int t4_fwaddrspace_write(struct adapter *adap, unsigned int mbox,
+			  u32 addr, u32 val)
+{
+	struct fw_ldst_cmd c;
+
+	memset(&c, 0, sizeof(c));
+	c.op_to_addrspace = htonl(V_FW_CMD_OP(FW_LDST_CMD) | F_FW_CMD_REQUEST |
+			    F_FW_CMD_WRITE |
+			    V_FW_LDST_CMD_ADDRSPACE(FW_LDST_ADDRSPC_FIRMWARE));
+	c.cycles_to_len16 = htonl(FW_LEN16(c));
+	c.u.addrval.addr = htonl(addr);
+	c.u.addrval.val = htonl(val);
+
+	return t4_wr_mbox(adap, mbox, &c, sizeof(c), NULL);
+}
+
+/*
+ *     t4_mem_win_read_len - read memory through PCIE memory window
+ *     @adap: the adapter
+ *     @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.
+ *
+ *     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;
+
+	/*
+	 * Align on a 16B boundary.
+	 */
+	off = addr & 15;
+	if ((addr & 3) || (len + off) > MEMWIN0_APERTURE)
+		return -EINVAL;
+
+	t4_write_reg(adap, A_PCIE_MEM_ACCESS_OFFSET, addr & ~15);
+	t4_read_reg(adap, A_PCIE_MEM_ACCESS_OFFSET);
+
+	for (i = 0; i < len; i += 4)
+		*data++ = t4_read_reg(adap, (MEMWIN0_BASE + off + i));
+
+	return 0;
+}
+
 /**
  *	t4_mdio_rd - read a PHY register through MDIO
  *	@adap: the adapter
diff --git a/drivers/net/ethernet/chelsio/cxgb4/t4_regs.h b/drivers/net/ethernet/chelsio/cxgb4/t4_regs.h
index 0adc5bcec7c..111fc323f15 100644
--- a/drivers/net/ethernet/chelsio/cxgb4/t4_regs.h
+++ b/drivers/net/ethernet/chelsio/cxgb4/t4_regs.h
@@ -190,6 +190,59 @@
 #define SGE_DEBUG_DATA_LOW 0x10d4
 #define SGE_INGRESS_QUEUES_PER_PAGE_PF 0x10f4
 
+#define S_LP_INT_THRESH    12
+#define V_LP_INT_THRESH(x) ((x) << S_LP_INT_THRESH)
+#define S_HP_INT_THRESH    28
+#define V_HP_INT_THRESH(x) ((x) << S_HP_INT_THRESH)
+#define A_SGE_DBFIFO_STATUS 0x10a4
+
+#define S_ENABLE_DROP    13
+#define V_ENABLE_DROP(x) ((x) << S_ENABLE_DROP)
+#define F_ENABLE_DROP    V_ENABLE_DROP(1U)
+#define A_SGE_DOORBELL_CONTROL 0x10a8
+
+#define A_SGE_CTXT_CMD 0x11fc
+#define A_SGE_DBQ_CTXT_BADDR 0x1084
+
+#define A_SGE_PF_KDOORBELL 0x0
+
+#define S_QID 15
+#define V_QID(x) ((x) << S_QID)
+
+#define S_PIDX 0
+#define V_PIDX(x) ((x) << S_PIDX)
+
+#define M_LP_COUNT 0x7ffU
+#define S_LP_COUNT 0
+#define G_LP_COUNT(x) (((x) >> S_LP_COUNT) & M_LP_COUNT)
+
+#define M_HP_COUNT 0x7ffU
+#define S_HP_COUNT 16
+#define G_HP_COUNT(x) (((x) >> S_HP_COUNT) & M_HP_COUNT)
+
+#define A_SGE_INT_ENABLE3 0x1040
+
+#define S_DBFIFO_HP_INT 8
+#define V_DBFIFO_HP_INT(x) ((x) << S_DBFIFO_HP_INT)
+#define F_DBFIFO_HP_INT V_DBFIFO_HP_INT(1U)
+
+#define S_DBFIFO_LP_INT 7
+#define V_DBFIFO_LP_INT(x) ((x) << S_DBFIFO_LP_INT)
+#define F_DBFIFO_LP_INT V_DBFIFO_LP_INT(1U)
+
+#define S_DROPPED_DB 0
+#define V_DROPPED_DB(x) ((x) << S_DROPPED_DB)
+#define F_DROPPED_DB V_DROPPED_DB(1U)
+
+#define S_ERR_DROPPED_DB 18
+#define V_ERR_DROPPED_DB(x) ((x) << S_ERR_DROPPED_DB)
+#define F_ERR_DROPPED_DB V_ERR_DROPPED_DB(1U)
+
+#define A_PCIE_MEM_ACCESS_OFFSET 0x306c
+
+#define M_HP_INT_THRESH 0xfU
+#define M_LP_INT_THRESH 0xfU
+
 #define PCIE_PF_CLI 0x44
 #define PCIE_INT_CAUSE 0x3004
 #define  UNXSPLCPLERR  0x20000000U
diff --git a/drivers/net/ethernet/chelsio/cxgb4/t4fw_api.h b/drivers/net/ethernet/chelsio/cxgb4/t4fw_api.h
index edcfd7ec780..ad53f796b57 100644
--- a/drivers/net/ethernet/chelsio/cxgb4/t4fw_api.h
+++ b/drivers/net/ethernet/chelsio/cxgb4/t4fw_api.h
@@ -1620,4 +1620,19 @@ struct fw_hdr {
 #define FW_HDR_FW_VER_MINOR_GET(x) (((x) >> 16) & 0xff)
 #define FW_HDR_FW_VER_MICRO_GET(x) (((x) >> 8) & 0xff)
 #define FW_HDR_FW_VER_BUILD_GET(x) (((x) >> 0) & 0xff)
+
+#define S_FW_CMD_OP 24
+#define V_FW_CMD_OP(x) ((x) << S_FW_CMD_OP)
+
+#define S_FW_CMD_REQUEST 23
+#define V_FW_CMD_REQUEST(x) ((x) << S_FW_CMD_REQUEST)
+#define F_FW_CMD_REQUEST V_FW_CMD_REQUEST(1U)
+
+#define S_FW_CMD_WRITE 21
+#define V_FW_CMD_WRITE(x) ((x) << S_FW_CMD_WRITE)
+#define F_FW_CMD_WRITE V_FW_CMD_WRITE(1U)
+
+#define S_FW_LDST_CMD_ADDRSPACE 0
+#define V_FW_LDST_CMD_ADDRSPACE(x) ((x) << S_FW_LDST_CMD_ADDRSPACE)
+
 #endif /* _T4FW_INTERFACE_H_ */
diff --git a/drivers/net/ethernet/cirrus/cs89x0.c b/drivers/net/ethernet/cirrus/cs89x0.c
index b9406cbfc18..845b2020f29 100644
--- a/drivers/net/ethernet/cirrus/cs89x0.c
+++ b/drivers/net/ethernet/cirrus/cs89x0.c
@@ -1,105 +1,27 @@
 /* cs89x0.c: A Crystal Semiconductor (Now Cirrus Logic) CS89[02]0
- *  driver for linux.
+ *           driver for linux.
+ * Written 1996 by Russell Nelson, with reference to skeleton.c
+ * written 1993-1994 by Donald Becker.
+ *
+ * This software may be used and distributed according to the terms
+ * of the GNU General Public License, incorporated herein by reference.
+ *
+ * The author may be reached at nelson@crynwr.com, Crynwr
+ * Software, 521 Pleasant Valley Rd., Potsdam, NY 13676
+ *
+ * Other contributors:
+ * Mike Cruse        : mcruse@cti-ltd.com
+ * Russ Nelson
+ * Melody Lee        : ethernet@crystal.cirrus.com
+ * Alan Cox
+ * Andrew Morton
+ * Oskar Schirmer    : oskar@scara.com
+ * Deepak Saxena     : dsaxena@plexity.net
+ * Dmitry Pervushin  : dpervushin@ru.mvista.com
+ * Deepak Saxena     : dsaxena@plexity.net
+ * Domenico Andreoli : cavokz@gmail.com
  */
 
-/*
-	Written 1996 by Russell Nelson, with reference to skeleton.c
-	written 1993-1994 by Donald Becker.
-
-	This software may be used and distributed according to the terms
-	of the GNU General Public License, incorporated herein by reference.
-
-        The author may be reached at nelson@crynwr.com, Crynwr
-        Software, 521 Pleasant Valley Rd., Potsdam, NY 13676
-
-  Changelog:
-
-  Mike Cruse        : mcruse@cti-ltd.com
-                    : Changes for Linux 2.0 compatibility.
-                    : Added dev_id parameter in net_interrupt(),
-                    : request_irq() and free_irq(). Just NULL for now.
-
-  Mike Cruse        : Added MOD_INC_USE_COUNT and MOD_DEC_USE_COUNT macros
-                    : in net_open() and net_close() so kerneld would know
-                    : that the module is in use and wouldn't eject the
-                    : driver prematurely.
-
-  Mike Cruse        : Rewrote init_module() and cleanup_module using 8390.c
-                    : as an example. Disabled autoprobing in init_module(),
-                    : not a good thing to do to other devices while Linux
-                    : is running from all accounts.
-
-  Russ Nelson       : Jul 13 1998.  Added RxOnly DMA support.
-
-  Melody Lee        : Aug 10 1999.  Changes for Linux 2.2.5 compatibility.
-                    : email: ethernet@crystal.cirrus.com
-
-  Alan Cox          : Removed 1.2 support, added 2.1 extra counters.
-
-  Andrew Morton     : Kernel 2.3.48
-                    : Handle kmalloc() failures
-                    : Other resource allocation fixes
-                    : Add SMP locks
-                    : Integrate Russ Nelson's ALLOW_DMA functionality back in.
-                    : If ALLOW_DMA is true, make DMA runtime selectable
-                    : Folded in changes from Cirrus (Melody Lee
-                    : <klee@crystal.cirrus.com>)
-                    : Don't call netif_wake_queue() in net_send_packet()
-                    : Fixed an out-of-mem bug in dma_rx()
-                    : Updated Documentation/networking/cs89x0.txt
-
-  Andrew Morton     : Kernel 2.3.99-pre1
-                    : Use skb_reserve to longword align IP header (two places)
-                    : Remove a delay loop from dma_rx()
-                    : Replace '100' with HZ
-                    : Clean up a couple of skb API abuses
-                    : Added 'cs89x0_dma=N' kernel boot option
-                    : Correctly initialise lp->lock in non-module compile
-
-  Andrew Morton     : Kernel 2.3.99-pre4-1
-                    : MOD_INC/DEC race fix (see
-                    : http://www.uwsg.indiana.edu/hypermail/linux/kernel/0003.3/1532.html)
-
-  Andrew Morton     : Kernel 2.4.0-test7-pre2
-                    : Enhanced EEPROM support to cover more devices,
-                    :   abstracted IRQ mapping to support CONFIG_ARCH_CLPS7500 arch
-                    :   (Jason Gunthorpe <jgg@ualberta.ca>)
-
-  Andrew Morton     : Kernel 2.4.0-test11-pre4
-                    : Use dev->name in request_*() (Andrey Panin)
-                    : Fix an error-path memleak in init_module()
-                    : Preserve return value from request_irq()
-                    : Fix type of `media' module parm (Keith Owens)
-                    : Use SET_MODULE_OWNER()
-                    : Tidied up strange request_irq() abuse in net_open().
-
-  Andrew Morton     : Kernel 2.4.3-pre1
-                    : Request correct number of pages for DMA (Hugh Dickens)
-                    : Select PP_ChipID _after_ unregister_netdev in cleanup_module()
-                    :  because unregister_netdev() calls get_stats.
-                    : Make `version[]' __initdata
-                    : Uninlined the read/write reg/word functions.
-
-  Oskar Schirmer    : oskar@scara.com
-                    : HiCO.SH4 (superh) support added (irq#1, cs89x0_media=)
-
-  Deepak Saxena     : dsaxena@plexity.net
-                    : Intel IXDP2x01 (XScale ixp2x00 NPU) platform support
-
-  Dmitry Pervushin  : dpervushin@ru.mvista.com
-                    : PNX010X platform support
-
-  Deepak Saxena     : dsaxena@plexity.net
-                    : Intel IXDP2351 platform support
-
-  Dmitry Pervushin  : dpervushin@ru.mvista.com
-                    : PNX010X platform support
-
-  Domenico Andreoli : cavokz@gmail.com
-                    : QQ2440 platform support
-
-*/
-
 
 /*
  * Set this to zero to disable DMA code
@@ -119,14 +41,12 @@
  */
 #define DEBUGGING	1
 
-/*
-  Sources:
-
-	Crynwr packet driver epktisa.
-
-	Crystal Semiconductor data sheets.
+/* Sources:
+ *	Crynwr packet driver epktisa.
+ *	Crystal Semiconductor data sheets.
+ */
 
-*/
+#define pr_fmt(fmt) KBUILD_MODNAME ": " fmt
 
 #include <linux/module.h>
 #include <linux/printk.h>
@@ -147,8 +67,8 @@
 #include <linux/bitops.h>
 #include <linux/delay.h>
 #include <linux/gfp.h>
+#include <linux/io.h>
 
-#include <asm/io.h>
 #include <asm/irq.h>
 #include <linux/atomic.h>
 #if ALLOW_DMA
@@ -157,35 +77,55 @@
 
 #include "cs89x0.h"
 
+#define cs89_dbg(val, level, fmt, ...)				\
+do {								\
+	if (val <= net_debug)					\
+		pr_##level(fmt, ##__VA_ARGS__);			\
+} while (0)
+
 static char version[] __initdata =
-"cs89x0.c: v2.4.3-pre1 Russell Nelson <nelson@crynwr.com>, Andrew Morton\n";
+	"v2.4.3-pre1 Russell Nelson <nelson@crynwr.com>, Andrew Morton";
 
 #define DRV_NAME "cs89x0"
 
 /* First, a few definitions that the brave might change.
-   A zero-terminated list of I/O addresses to be probed. Some special flags..
-      Addr & 1 = Read back the address port, look for signature and reset
-                 the page window before probing
-      Addr & 3 = Reset the page window and probe
-   The CLPS eval board has the Cirrus chip at 0x80090300, in ARM IO space,
-   but it is possible that a Cirrus board could be plugged into the ISA
-   slots. */
+ * A zero-terminated list of I/O addresses to be probed. Some special flags..
+ * Addr & 1 = Read back the address port, look for signature and reset
+ * the page window before probing
+ * Addr & 3 = Reset the page window and probe
+ * The CLPS eval board has the Cirrus chip at 0x80090300, in ARM IO space,
+ * but it is possible that a Cirrus board could be plugged into the ISA
+ * slots.
+ */
 /* The cs8900 has 4 IRQ pins, software selectable. cs8900_irq_map maps
-   them to system IRQ numbers. This mapping is card specific and is set to
-   the configuration of the Cirrus Eval board for this chip. */
+ * 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};
+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};
+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, 0x260, 0x280, 0x2a0, 0x2c0, 0x2e0, 0};
-static unsigned int cs8900_irq_map[] = {10,11,12,5};
+static unsigned int netcard_portlist[] __used __initdata = {
+	0x300, 0x320, 0x340, 0x360, 0x200, 0x220, 0x240,
+	0x260, 0x280, 0x2a0, 0x2c0, 0x2e0, 0
+};
+static unsigned int cs8900_irq_map[] = {
+	10, 11, 12, 5
+};
 #endif
 #endif
 
@@ -222,6 +162,8 @@ struct net_local {
 	int send_underrun;	/* keep track of how many underruns in a row we get */
 	int force;		/* force various values; see FORCE* above. */
 	spinlock_t lock;
+	void __iomem *virt_addr;/* CS89x0 virtual address. */
+	unsigned long size;	/* Length of CS89x0 memory region. */
 #if ALLOW_DMA
 	int use_dma;		/* Flag: we're using dma */
 	int dma;		/* DMA channel */
@@ -230,119 +172,42 @@ struct net_local {
 	unsigned char *end_dma_buff;	/* points to the end of the buffer */
 	unsigned char *rx_dma_ptr;	/* points to the next packet  */
 #endif
-#ifdef CONFIG_CS89x0_PLATFORM
-	void __iomem *virt_addr;/* Virtual address for accessing the CS89x0. */
-	unsigned long phys_addr;/* Physical address for accessing the CS89x0. */
-	unsigned long size;	/* Length of CS89x0 memory region. */
-#endif
 };
 
-/* Index to functions, as function prototypes. */
-
-static int cs89x0_probe1(struct net_device *dev, unsigned long ioaddr, int modular);
-static int net_open(struct net_device *dev);
-static netdev_tx_t net_send_packet(struct sk_buff *skb, struct net_device *dev);
-static irqreturn_t net_interrupt(int irq, void *dev_id);
-static void set_multicast_list(struct net_device *dev);
-static void net_timeout(struct net_device *dev);
-static void net_rx(struct net_device *dev);
-static int net_close(struct net_device *dev);
-static struct net_device_stats *net_get_stats(struct net_device *dev);
-static void reset_chip(struct net_device *dev);
-static int get_eeprom_data(struct net_device *dev, int off, int len, int *buffer);
-static int get_eeprom_cksum(int off, int len, int *buffer);
-static int set_mac_address(struct net_device *dev, void *addr);
-static void count_rx_errors(int status, struct net_device *dev);
-#ifdef CONFIG_NET_POLL_CONTROLLER
-static void net_poll_controller(struct net_device *dev);
-#endif
-#if ALLOW_DMA
-static void get_dma_channel(struct net_device *dev);
-static void release_dma_buff(struct net_local *lp);
-#endif
-
 /* Example routines you must write ;->. */
 #define tx_done(dev) 1
 
 /*
  * Permit 'cs89x0_dma=N' in the kernel boot environment
  */
-#if !defined(MODULE) && (ALLOW_DMA != 0)
+#if !defined(MODULE)
+#if ALLOW_DMA
 static int g_cs89x0_dma;
 
 static int __init dma_fn(char *str)
 {
-	g_cs89x0_dma = simple_strtol(str,NULL,0);
+	g_cs89x0_dma = simple_strtol(str, NULL, 0);
 	return 1;
 }
 
 __setup("cs89x0_dma=", dma_fn);
-#endif	/* !defined(MODULE) && (ALLOW_DMA != 0) */
+#endif	/* ALLOW_DMA */
 
-#ifndef MODULE
 static int g_cs89x0_media__force;
 
 static int __init media_fn(char *str)
 {
-	if (!strcmp(str, "rj45")) g_cs89x0_media__force = FORCE_RJ45;
-	else if (!strcmp(str, "aui")) g_cs89x0_media__force = FORCE_AUI;
-	else if (!strcmp(str, "bnc")) g_cs89x0_media__force = FORCE_BNC;
+	if (!strcmp(str, "rj45"))
+		g_cs89x0_media__force = FORCE_RJ45;
+	else if (!strcmp(str, "aui"))
+		g_cs89x0_media__force = FORCE_AUI;
+	else if (!strcmp(str, "bnc"))
+		g_cs89x0_media__force = FORCE_BNC;
+
 	return 1;
 }
 
 __setup("cs89x0_media=", media_fn);
-
-
-#ifndef CONFIG_CS89x0_PLATFORM
-/* Check for a network adaptor of this type, and return '0' iff one exists.
-   If dev->base_addr == 0, probe all likely locations.
-   If dev->base_addr == 1, always return failure.
-   If dev->base_addr == 2, allocate space for the device and return success
-   (detachable devices only).
-   Return 0 on success.
-   */
-
-struct net_device * __init cs89x0_probe(int unit)
-{
-	struct net_device *dev = alloc_etherdev(sizeof(struct net_local));
-	unsigned *port;
-	int err = 0;
-	int irq;
-	int io;
-
-	if (!dev)
-		return ERR_PTR(-ENODEV);
-
-	sprintf(dev->name, "eth%d", unit);
-	netdev_boot_setup_check(dev);
-	io = dev->base_addr;
-	irq = dev->irq;
-
-	if (net_debug)
-		printk("cs89x0:cs89x0_probe(0x%x)\n", io);
-
-	if (io > 0x1ff)	{	/* Check a single specified location. */
-		err = cs89x0_probe1(dev, io, 0);
-	} else if (io != 0) {	/* Don't probe at all. */
-		err = -ENXIO;
-	} else {
-		for (port = netcard_portlist; *port; port++) {
-			if (cs89x0_probe1(dev, *port, 0) == 0)
-				break;
-			dev->irq = irq;
-		}
-		if (!*port)
-			err = -ENODEV;
-	}
-	if (err)
-		goto out;
-	return dev;
-out:
-	free_netdev(dev);
-	printk(KERN_WARNING "cs89x0: no cs8900 or cs8920 detected.  Be sure to disable PnP with SETUP\n");
-	return ERR_PTR(err);
-}
-#endif
 #endif
 
 #if defined(CONFIG_MACH_IXDP2351)
@@ -369,36 +234,22 @@ writeword(unsigned long base_addr, int portno, u16 value)
 {
 	__raw_writel(value, base_addr + (portno << 1));
 }
-#else
-static u16
-readword(unsigned long base_addr, int portno)
-{
-	return inw(base_addr + portno);
-}
-
-static void
-writeword(unsigned long base_addr, int portno, u16 value)
-{
-	outw(value, base_addr + portno);
-}
 #endif
 
-static void
-readwords(unsigned long base_addr, int portno, void *buf, int length)
+static void readwords(struct net_local *lp, int portno, void *buf, int length)
 {
 	u8 *buf8 = (u8 *)buf;
 
 	do {
 		u16 tmp16;
 
-		tmp16 = readword(base_addr, portno);
+		tmp16 = ioread16(lp->virt_addr + portno);
 		*buf8++ = (u8)tmp16;
 		*buf8++ = (u8)(tmp16 >> 8);
 	} while (--length);
 }
 
-static void
-writewords(unsigned long base_addr, int portno, void *buf, int length)
+static void writewords(struct net_local *lp, int portno, void *buf, int length)
 {
 	u8 *buf8 = (u8 *)buf;
 
@@ -407,32 +258,37 @@ writewords(unsigned long base_addr, int portno, void *buf, int length)
 
 		tmp16 = *buf8++;
 		tmp16 |= (*buf8++) << 8;
-		writeword(base_addr, portno, tmp16);
+		iowrite16(tmp16, lp->virt_addr + portno);
 	} while (--length);
 }
 
 static u16
 readreg(struct net_device *dev, u16 regno)
 {
-	writeword(dev->base_addr, ADD_PORT, regno);
-	return readword(dev->base_addr, DATA_PORT);
+	struct net_local *lp = netdev_priv(dev);
+
+	iowrite16(regno, lp->virt_addr + ADD_PORT);
+	return ioread16(lp->virt_addr + DATA_PORT);
 }
 
 static void
 writereg(struct net_device *dev, u16 regno, u16 value)
 {
-	writeword(dev->base_addr, ADD_PORT, regno);
-	writeword(dev->base_addr, DATA_PORT, value);
+	struct net_local *lp = netdev_priv(dev);
+
+	iowrite16(regno, lp->virt_addr + ADD_PORT);
+	iowrite16(value, lp->virt_addr + DATA_PORT);
 }
 
 static int __init
 wait_eeprom_ready(struct net_device *dev)
 {
 	int timeout = jiffies;
-	/* check to see if the EEPROM is ready, a timeout is used -
-	   just in case EEPROM is ready when SI_BUSY in the
-	   PP_SelfST is clear */
-	while(readreg(dev, PP_SelfST) & SI_BUSY)
+	/* check to see if the EEPROM is ready,
+	 * a timeout is used just in case EEPROM is ready when
+	 * SI_BUSY in the PP_SelfST is clear
+	 */
+	while (readreg(dev, PP_SelfST) & SI_BUSY)
 		if (jiffies - timeout >= 40)
 			return -1;
 	return 0;
@@ -443,17 +299,19 @@ get_eeprom_data(struct net_device *dev, int off, int len, int *buffer)
 {
 	int i;
 
-	if (net_debug > 3) printk("EEPROM data from %x for %x:\n",off,len);
+	cs89_dbg(3, info, "EEPROM data from %x for %x:", off, len);
 	for (i = 0; i < len; i++) {
-		if (wait_eeprom_ready(dev) < 0) return -1;
+		if (wait_eeprom_ready(dev) < 0)
+			return -1;
 		/* Now send the EEPROM read command and EEPROM location to read */
 		writereg(dev, PP_EECMD, (off + i) | EEPROM_READ_CMD);
-		if (wait_eeprom_ready(dev) < 0) return -1;
+		if (wait_eeprom_ready(dev) < 0)
+			return -1;
 		buffer[i] = readreg(dev, PP_EEData);
-		if (net_debug > 3) printk("%04x ", buffer[i]);
+		cs89_dbg(3, cont, " %04x", buffer[i]);
 	}
-	if (net_debug > 3) printk("\n");
-        return 0;
+	cs89_dbg(3, cont, "\n");
+	return 0;
 }
 
 static int  __init
@@ -470,341 +328,52 @@ get_eeprom_cksum(int off, int len, int *buffer)
 	return -1;
 }
 
-#ifdef CONFIG_NET_POLL_CONTROLLER
-/*
- * Polling receive - used by netconsole and other diagnostic tools
- * to allow network i/o with interrupts disabled.
- */
-static void net_poll_controller(struct net_device *dev)
-{
-	disable_irq(dev->irq);
-	net_interrupt(dev->irq, dev);
-	enable_irq(dev->irq);
-}
-#endif
-
-static const struct net_device_ops net_ops = {
-	.ndo_open		= net_open,
-	.ndo_stop		= net_close,
-	.ndo_tx_timeout		= net_timeout,
-	.ndo_start_xmit 	= net_send_packet,
-	.ndo_get_stats		= net_get_stats,
-	.ndo_set_rx_mode	= set_multicast_list,
-	.ndo_set_mac_address 	= set_mac_address,
-#ifdef CONFIG_NET_POLL_CONTROLLER
-	.ndo_poll_controller	= net_poll_controller,
-#endif
-	.ndo_change_mtu		= eth_change_mtu,
-	.ndo_validate_addr	= eth_validate_addr,
-};
-
-/* This is the real probe routine.  Linux has a history of friendly device
-   probes on the ISA bus.  A good device probes avoids doing writes, and
-   verifies that the correct device exists and functions.
-   Return 0 on success.
- */
-
-static int __init
-cs89x0_probe1(struct net_device *dev, unsigned long ioaddr, int modular)
+static void
+write_irq(struct net_device *dev, int chip_type, int irq)
 {
-	struct net_local *lp = netdev_priv(dev);
-	static unsigned version_printed;
 	int i;
-	int tmp;
-	unsigned rev_type = 0;
-	int eeprom_buff[CHKSUM_LEN];
-	int retval;
-
-	/* Initialize the device structure. */
-	if (!modular) {
-		memset(lp, 0, sizeof(*lp));
-		spin_lock_init(&lp->lock);
-#ifndef MODULE
-#if ALLOW_DMA
-		if (g_cs89x0_dma) {
-			lp->use_dma = 1;
-			lp->dma = g_cs89x0_dma;
-			lp->dmasize = 16;	/* Could make this an option... */
-		}
-#endif
-		lp->force = g_cs89x0_media__force;
-#endif
-
-        }
-
-	/* Grab the region so we can find another board if autoIRQ fails. */
-	/* WTF is going on here? */
-	if (!request_region(ioaddr & ~3, NETCARD_IO_EXTENT, DRV_NAME)) {
-		printk(KERN_ERR "%s: request_region(0x%lx, 0x%x) failed\n",
-				DRV_NAME, ioaddr, NETCARD_IO_EXTENT);
-		retval = -EBUSY;
-		goto out1;
-	}
-
-	/* if they give us an odd I/O address, then do ONE write to
-           the address port, to get it back to address zero, where we
-           expect to find the EISA signature word. An IO with a base of 0x3
-	   will skip the test for the ADD_PORT. */
-	if (ioaddr & 1) {
-		if (net_debug > 1)
-			printk(KERN_INFO "%s: odd ioaddr 0x%lx\n", dev->name, ioaddr);
-	        if ((ioaddr & 2) != 2)
-	        	if ((readword(ioaddr & ~3, ADD_PORT) & ADD_MASK) != ADD_SIG) {
-				printk(KERN_ERR "%s: bad signature 0x%x\n",
-					dev->name, readword(ioaddr & ~3, ADD_PORT));
-		        	retval = -ENODEV;
-				goto out2;
-			}
-	}
-
-	ioaddr &= ~3;
-	printk(KERN_DEBUG "PP_addr at %lx[%x]: 0x%x\n",
-			ioaddr, ADD_PORT, readword(ioaddr, ADD_PORT));
-	writeword(ioaddr, ADD_PORT, PP_ChipID);
-
-	tmp = readword(ioaddr, DATA_PORT);
-	if (tmp != CHIP_EISA_ID_SIG) {
-		printk(KERN_DEBUG "%s: incorrect signature at %lx[%x]: 0x%x!="
-			CHIP_EISA_ID_SIG_STR "\n",
-			dev->name, ioaddr, DATA_PORT, tmp);
-  		retval = -ENODEV;
-  		goto out2;
-	}
-
-	/* Fill in the 'dev' fields. */
-	dev->base_addr = ioaddr;
-
-	/* get the chip type */
-	rev_type = readreg(dev, PRODUCT_ID_ADD);
-	lp->chip_type = rev_type &~ REVISON_BITS;
-	lp->chip_revision = ((rev_type & REVISON_BITS) >> 8) + 'A';
-
-	/* Check the chip type and revision in order to set the correct send command
-	CS8920 revision C and CS8900 revision F can use the faster send. */
-	lp->send_cmd = TX_AFTER_381;
-	if (lp->chip_type == CS8900 && lp->chip_revision >= 'F')
-		lp->send_cmd = TX_NOW;
-	if (lp->chip_type != CS8900 && lp->chip_revision >= 'C')
-		lp->send_cmd = TX_NOW;
-
-	if (net_debug  &&  version_printed++ == 0)
-		printk(version);
-
-	printk(KERN_INFO "%s: cs89%c0%s rev %c found at %#3lx ",
-	       dev->name,
-	       lp->chip_type==CS8900?'0':'2',
-	       lp->chip_type==CS8920M?"M":"",
-	       lp->chip_revision,
-	       dev->base_addr);
-
-	reset_chip(dev);
-
-        /* Here we read the current configuration of the chip. If there
-	   is no Extended EEPROM then the idea is to not disturb the chip
-	   configuration, it should have been correctly setup by automatic
-	   EEPROM read on reset. So, if the chip says it read the EEPROM
-	   the driver will always do *something* instead of complain that
-	   adapter_cnf is 0. */
-
-
-        if ((readreg(dev, PP_SelfST) & (EEPROM_OK | EEPROM_PRESENT)) ==
-	      (EEPROM_OK|EEPROM_PRESENT)) {
-	        /* Load the MAC. */
-		for (i=0; i < ETH_ALEN/2; i++) {
-	                unsigned int Addr;
-			Addr = readreg(dev, PP_IA+i*2);
-		        dev->dev_addr[i*2] = Addr & 0xFF;
-		        dev->dev_addr[i*2+1] = Addr >> 8;
-		}
-
-	   	/* Load the Adapter Configuration.
-		   Note:  Barring any more specific information from some
-		   other source (ie EEPROM+Schematics), we would not know
-		   how to operate a 10Base2 interface on the AUI port.
-		   However, since we  do read the status of HCB1 and use
-		   settings that always result in calls to control_dc_dc(dev,0)
-		   a BNC interface should work if the enable pin
-		   (dc/dc converter) is on HCB1. It will be called AUI
-		   however. */
-
-		lp->adapter_cnf = 0;
-		i = readreg(dev, PP_LineCTL);
-		/* Preserve the setting of the HCB1 pin. */
-		if ((i & (HCB1 | HCB1_ENBL)) ==  (HCB1 | HCB1_ENBL))
-			lp->adapter_cnf |= A_CNF_DC_DC_POLARITY;
-		/* Save the sqelch bit */
-		if ((i & LOW_RX_SQUELCH) == LOW_RX_SQUELCH)
-			lp->adapter_cnf |= A_CNF_EXTND_10B_2 | A_CNF_LOW_RX_SQUELCH;
-		/* Check if the card is in 10Base-t only mode */
-		if ((i & (AUI_ONLY | AUTO_AUI_10BASET)) == 0)
-			lp->adapter_cnf |=  A_CNF_10B_T | A_CNF_MEDIA_10B_T;
-		/* Check if the card is in AUI only mode */
-		if ((i & (AUI_ONLY | AUTO_AUI_10BASET)) == AUI_ONLY)
-			lp->adapter_cnf |=  A_CNF_AUI | A_CNF_MEDIA_AUI;
-		/* Check if the card is in Auto mode. */
-		if ((i & (AUI_ONLY | AUTO_AUI_10BASET)) == AUTO_AUI_10BASET)
-			lp->adapter_cnf |=  A_CNF_AUI | A_CNF_10B_T |
-			A_CNF_MEDIA_AUI | A_CNF_MEDIA_10B_T | A_CNF_MEDIA_AUTO;
-
-		if (net_debug > 1)
-			printk(KERN_INFO "%s: PP_LineCTL=0x%x, adapter_cnf=0x%x\n",
-					dev->name, i, lp->adapter_cnf);
-
-		/* IRQ. Other chips already probe, see below. */
-		if (lp->chip_type == CS8900)
-			lp->isa_config = readreg(dev, PP_CS8900_ISAINT) & INT_NO_MASK;
-
-		printk( "[Cirrus EEPROM] ");
-	}
-
-        printk("\n");
-
-	/* First check to see if an EEPROM is attached. */
-
-	if ((readreg(dev, PP_SelfST) & EEPROM_PRESENT) == 0)
-		printk(KERN_WARNING "cs89x0: No EEPROM, relying on command line....\n");
-	else if (get_eeprom_data(dev, START_EEPROM_DATA,CHKSUM_LEN,eeprom_buff) < 0) {
-		printk(KERN_WARNING "\ncs89x0: EEPROM read failed, relying on command line.\n");
-        } else if (get_eeprom_cksum(START_EEPROM_DATA,CHKSUM_LEN,eeprom_buff) < 0) {
-		/* Check if the chip was able to read its own configuration starting
-		   at 0 in the EEPROM*/
-		if ((readreg(dev, PP_SelfST) & (EEPROM_OK | EEPROM_PRESENT)) !=
-		    (EEPROM_OK|EEPROM_PRESENT))
-                	printk(KERN_WARNING "cs89x0: Extended EEPROM checksum bad and no Cirrus EEPROM, relying on command line\n");
-
-        } else {
-		/* This reads an extended EEPROM that is not documented
-		   in the CS8900 datasheet. */
-
-                /* get transmission control word  but keep the autonegotiation bits */
-                if (!lp->auto_neg_cnf) lp->auto_neg_cnf = eeprom_buff[AUTO_NEG_CNF_OFFSET/2];
-                /* Store adapter configuration */
-                if (!lp->adapter_cnf) lp->adapter_cnf = eeprom_buff[ADAPTER_CNF_OFFSET/2];
-                /* Store ISA configuration */
-                lp->isa_config = eeprom_buff[ISA_CNF_OFFSET/2];
-                dev->mem_start = eeprom_buff[PACKET_PAGE_OFFSET/2] << 8;
-
-                /* eeprom_buff has 32-bit ints, so we can't just memcpy it */
-                /* store the initial memory base address */
-                for (i = 0; i < ETH_ALEN/2; i++) {
-                        dev->dev_addr[i*2] = eeprom_buff[i];
-                        dev->dev_addr[i*2+1] = eeprom_buff[i] >> 8;
-                }
-		if (net_debug > 1)
-			printk(KERN_DEBUG "%s: new adapter_cnf: 0x%x\n",
-				dev->name, lp->adapter_cnf);
-        }
-
-        /* allow them to force multiple transceivers.  If they force multiple, autosense */
-        {
-		int count = 0;
-		if (lp->force & FORCE_RJ45)	{lp->adapter_cnf |= A_CNF_10B_T; count++; }
-		if (lp->force & FORCE_AUI) 	{lp->adapter_cnf |= A_CNF_AUI; count++; }
-		if (lp->force & FORCE_BNC)	{lp->adapter_cnf |= A_CNF_10B_2; count++; }
-		if (count > 1)			{lp->adapter_cnf |= A_CNF_MEDIA_AUTO; }
-		else if (lp->force & FORCE_RJ45){lp->adapter_cnf |= A_CNF_MEDIA_10B_T; }
-		else if (lp->force & FORCE_AUI)	{lp->adapter_cnf |= A_CNF_MEDIA_AUI; }
-		else if (lp->force & FORCE_BNC)	{lp->adapter_cnf |= A_CNF_MEDIA_10B_2; }
-        }
-
-	if (net_debug > 1)
-		printk(KERN_DEBUG "%s: after force 0x%x, adapter_cnf=0x%x\n",
-			dev->name, lp->force, lp->adapter_cnf);
-
-        /* FIXME: We don't let you set dc-dc polarity or low RX squelch from the command line: add it here */
-
-        /* FIXME: We don't let you set the IMM bit from the command line: add it to lp->auto_neg_cnf here */
-
-        /* FIXME: we don't set the Ethernet address on the command line.  Use
-           ifconfig IFACE hw ether AABBCCDDEEFF */
-
-	printk(KERN_INFO "cs89x0 media %s%s%s",
-	       (lp->adapter_cnf & A_CNF_10B_T)?"RJ-45,":"",
-	       (lp->adapter_cnf & A_CNF_AUI)?"AUI,":"",
-	       (lp->adapter_cnf & A_CNF_10B_2)?"BNC,":"");
-
-	lp->irq_map = 0xffff;
 
-	/* If this is a CS8900 then no pnp soft */
-	if (lp->chip_type != CS8900 &&
-	    /* Check if the ISA IRQ has been set  */
-		(i = readreg(dev, PP_CS8920_ISAINT) & 0xff,
-		 (i != 0 && i < CS8920_NO_INTS))) {
-		if (!dev->irq)
-			dev->irq = i;
-	} else {
-		i = lp->isa_config & INT_NO_MASK;
+	if (chip_type == CS8900) {
 #ifndef CONFIG_CS89x0_PLATFORM
-		if (lp->chip_type == CS8900) {
-#ifdef CS89x0_NONISA_IRQ
-		        i = cs8900_irq_map[0];
+		/* Search the mapping table for the corresponding IRQ pin. */
+		for (i = 0; i != ARRAY_SIZE(cs8900_irq_map); i++)
+			if (cs8900_irq_map[i] == irq)
+				break;
+		/* Not found */
+		if (i == ARRAY_SIZE(cs8900_irq_map))
+			i = 3;
 #else
-			/* Translate the IRQ using the IRQ mapping table. */
-			if (i >= ARRAY_SIZE(cs8900_irq_map))
-				printk("\ncs89x0: invalid ISA interrupt number %d\n", i);
-			else
-				i = cs8900_irq_map[i];
-
-			lp->irq_map = CS8900_IRQ_MAP; /* fixed IRQ map for CS8900 */
-		} else {
-			int irq_map_buff[IRQ_MAP_LEN/2];
-
-			if (get_eeprom_data(dev, IRQ_MAP_EEPROM_DATA,
-					    IRQ_MAP_LEN/2,
-					    irq_map_buff) >= 0) {
-				if ((irq_map_buff[0] & 0xff) == PNP_IRQ_FRMT)
-					lp->irq_map = (irq_map_buff[0]>>8) | (irq_map_buff[1] << 8);
-			}
-#endif
-		}
-#endif
-		if (!dev->irq)
-			dev->irq = i;
-	}
-
-	printk(" IRQ %d", dev->irq);
-
-#if ALLOW_DMA
-	if (lp->use_dma) {
-		get_dma_channel(dev);
-		printk(", DMA %d", dev->dma);
-	}
-	else
+		/* INTRQ0 pin is used for interrupt generation. */
+		i = 0;
 #endif
-	{
-		printk(", programmed I/O");
+		writereg(dev, PP_CS8900_ISAINT, i);
+	} else {
+		writereg(dev, PP_CS8920_ISAINT, irq);
 	}
-
-	/* print the ethernet address. */
-	printk(", MAC %pM", dev->dev_addr);
-
-	dev->netdev_ops	= &net_ops;
-	dev->watchdog_timeo = HZ;
-
-	printk("\n");
-	if (net_debug)
-		printk("cs89x0_probe1() successful\n");
-
-	retval = register_netdev(dev);
-	if (retval)
-		goto out3;
-	return 0;
-out3:
-	writeword(dev->base_addr, ADD_PORT, PP_ChipID);
-out2:
-	release_region(ioaddr & ~3, NETCARD_IO_EXTENT);
-out1:
-	return retval;
 }
 
+static void
+count_rx_errors(int status, struct net_device *dev)
+{
+	dev->stats.rx_errors++;
+	if (status & RX_RUNT)
+		dev->stats.rx_length_errors++;
+	if (status & RX_EXTRA_DATA)
+		dev->stats.rx_length_errors++;
+	if ((status & RX_CRC_ERROR) && !(status & (RX_EXTRA_DATA | RX_RUNT)))
+		/* per str 172 */
+		dev->stats.rx_crc_errors++;
+	if (status & RX_DRIBBLE)
+		dev->stats.rx_frame_errors++;
+}
 
 /*********************************
  * This page contains DMA routines
-**********************************/
+ *********************************/
 
 #if ALLOW_DMA
 
-#define dma_page_eq(ptr1, ptr2) ((long)(ptr1)>>17 == (long)(ptr2)>>17)
+#define dma_page_eq(ptr1, ptr2) ((long)(ptr1) >> 17 == (long)(ptr2) >> 17)
 
 static void
 get_dma_channel(struct net_device *dev)
@@ -833,11 +402,10 @@ write_dma(struct net_device *dev, int chip_type, int dma)
 	struct net_local *lp = netdev_priv(dev);
 	if ((lp->isa_config & ANY_ISA_DMA) == 0)
 		return;
-	if (chip_type == CS8900) {
-		writereg(dev, PP_CS8900_ISADMA, dma-5);
-	} else {
+	if (chip_type == CS8900)
+		writereg(dev, PP_CS8900_ISADMA, dma - 5);
+	else
 		writereg(dev, PP_CS8920_ISADMA, dma);
-	}
 }
 
 static void
@@ -847,18 +415,15 @@ set_dma_cfg(struct net_device *dev)
 
 	if (lp->use_dma) {
 		if ((lp->isa_config & ANY_ISA_DMA) == 0) {
-			if (net_debug > 3)
-				printk("set_dma_cfg(): no DMA\n");
+			cs89_dbg(3, err, "set_dma_cfg(): no DMA\n");
 			return;
 		}
 		if (lp->isa_config & ISA_RxDMA) {
 			lp->curr_rx_cfg |= RX_DMA_ONLY;
-			if (net_debug > 3)
-				printk("set_dma_cfg(): RX_DMA_ONLY\n");
+			cs89_dbg(3, info, "set_dma_cfg(): RX_DMA_ONLY\n");
 		} else {
 			lp->curr_rx_cfg |= AUTO_RX_DMA;	/* not that we support it... */
-			if (net_debug > 3)
-				printk("set_dma_cfg(): AUTO_RX_DMA\n");
+			cs89_dbg(3, info, "set_dma_cfg(): AUTO_RX_DMA\n");
 		}
 	}
 }
@@ -868,7 +433,7 @@ dma_bufcfg(struct net_device *dev)
 {
 	struct net_local *lp = netdev_priv(dev);
 	if (lp->use_dma)
-		return (lp->isa_config & ANY_ISA_DMA)? RX_DMA_ENBL : 0;
+		return (lp->isa_config & ANY_ISA_DMA) ? RX_DMA_ENBL : 0;
 	else
 		return 0;
 }
@@ -898,13 +463,13 @@ dma_rx(struct net_device *dev)
 	int status, length;
 	unsigned char *bp = lp->rx_dma_ptr;
 
-	status = bp[0] + (bp[1]<<8);
-	length = bp[2] + (bp[3]<<8);
+	status = bp[0] + (bp[1] << 8);
+	length = bp[2] + (bp[3] << 8);
 	bp += 4;
-	if (net_debug > 5) {
-		printk(	"%s: receiving DMA packet at %lx, status %x, length %x\n",
-			dev->name, (unsigned long)bp, status, length);
-	}
+
+	cs89_dbg(5, debug, "%s: receiving DMA packet at %lx, status %x, length %x\n",
+		 dev->name, (unsigned long)bp, status, length);
+
 	if ((status & RX_OK) == 0) {
 		count_rx_errors(status, dev);
 		goto skip_this_frame;
@@ -913,14 +478,16 @@ dma_rx(struct net_device *dev)
 	/* Malloc up new buffer. */
 	skb = netdev_alloc_skb(dev, length + 2);
 	if (skb == NULL) {
-		if (net_debug)	/* I don't think we want to do this to a stressed system */
-			printk("%s: Memory squeeze, dropping packet.\n", dev->name);
+		/* 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 */
 skip_this_frame:
 		bp += (length + 3) & ~3;
-		if (bp >= lp->end_dma_buff) bp -= lp->dmasize*1024;
+		if (bp >= lp->end_dma_buff)
+			bp -= lp->dmasize * 1024;
 		lp->rx_dma_ptr = bp;
 		return;
 	}
@@ -928,63 +495,38 @@ skip_this_frame:
 
 	if (bp + length > lp->end_dma_buff) {
 		int semi_cnt = lp->end_dma_buff - bp;
-		memcpy(skb_put(skb,semi_cnt), bp, semi_cnt);
-		memcpy(skb_put(skb,length - semi_cnt), lp->dma_buff,
+		memcpy(skb_put(skb, semi_cnt), bp, semi_cnt);
+		memcpy(skb_put(skb, length - semi_cnt), lp->dma_buff,
 		       length - semi_cnt);
 	} else {
-		memcpy(skb_put(skb,length), bp, length);
+		memcpy(skb_put(skb, length), bp, length);
 	}
 	bp += (length + 3) & ~3;
-	if (bp >= lp->end_dma_buff) bp -= lp->dmasize*1024;
+	if (bp >= lp->end_dma_buff)
+		bp -= lp->dmasize*1024;
 	lp->rx_dma_ptr = bp;
 
-	if (net_debug > 3) {
-		printk(	"%s: received %d byte DMA packet of type %x\n",
-			dev->name, length,
-			(skb->data[ETH_ALEN+ETH_ALEN] << 8) | skb->data[ETH_ALEN+ETH_ALEN+1]);
-	}
-        skb->protocol=eth_type_trans(skb,dev);
+	cs89_dbg(3, info, "%s: received %d byte DMA packet of type %x\n",
+		 dev->name, length,
+		 ((skb->data[ETH_ALEN + ETH_ALEN] << 8) |
+		  skb->data[ETH_ALEN + ETH_ALEN + 1]));
+
+	skb->protocol = eth_type_trans(skb, dev);
 	netif_rx(skb);
 	dev->stats.rx_packets++;
 	dev->stats.rx_bytes += length;
 }
 
-#endif	/* ALLOW_DMA */
-
-static void __init reset_chip(struct net_device *dev)
+static void release_dma_buff(struct net_local *lp)
 {
-#if !defined(CONFIG_MACH_MX31ADS)
-#if !defined(CS89x0_NONISA_IRQ)
-	struct net_local *lp = netdev_priv(dev);
-	int ioaddr = dev->base_addr;
-#endif /* CS89x0_NONISA_IRQ */
-	int reset_start_time;
-
-	writereg(dev, PP_SelfCTL, readreg(dev, PP_SelfCTL) | POWER_ON_RESET);
-
-	/* 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 */
-		writeword(ioaddr, ADD_PORT, PP_CS8920_ISAINT);
-		outb(dev->irq, ioaddr + DATA_PORT);
-		outb(0,      ioaddr + DATA_PORT + 1);
-
-		writeword(ioaddr, ADD_PORT, PP_CS8920_ISAMemB);
-		outb((dev->mem_start >> 16) & 0xff, ioaddr + DATA_PORT);
-		outb((dev->mem_start >> 8) & 0xff,   ioaddr + DATA_PORT + 1);
+	if (lp->dma_buff) {
+		free_pages((unsigned long)(lp->dma_buff),
+			   get_order(lp->dmasize * 1024));
+		lp->dma_buff = NULL;
 	}
-#endif /* CS89x0_NONISA_IRQ */
-
-	/* Wait until the chip is reset */
-	reset_start_time = jiffies;
-	while( (readreg(dev, PP_SelfST) & INIT_DONE) == 0 && jiffies - reset_start_time < 2)
-		;
-#endif /* !CONFIG_MACH_MX31ADS */
 }
 
+#endif	/* ALLOW_DMA */
 
 static void
 control_dc_dc(struct net_device *dev, int on_not_off)
@@ -993,8 +535,9 @@ control_dc_dc(struct net_device *dev, int on_not_off)
 	unsigned int selfcontrol;
 	int timenow = jiffies;
 	/* control the DC to DC convertor in the SelfControl register.
-	   Note: This is hooked up to a general purpose pin, might not
-	   always be a DC to DC convertor. */
+	 * Note: This is hooked up to a general purpose pin, might not
+	 * always be a DC to DC convertor.
+	 */
 
 	selfcontrol = HCB1_ENBL; /* Enable the HCB1 bit as an output */
 	if (((lp->adapter_cnf & A_CNF_DC_DC_POLARITY) != 0) ^ on_not_off)
@@ -1008,6 +551,49 @@ control_dc_dc(struct net_device *dev, int on_not_off)
 		;
 }
 
+/* send a test packet - return true if carrier bits are ok */
+static int
+send_test_pkt(struct net_device *dev)
+{
+	struct net_local *lp = netdev_priv(dev);
+	char test_packet[] = {
+		0, 0, 0, 0, 0, 0,  0, 0, 0, 0, 0, 0,
+		0, 46,		/* A 46 in network order */
+		0, 0,		/* DSAP=0 & SSAP=0 fields */
+		0xf3, 0		/* Control (Test Req + P bit set) */
+	};
+	long timenow = jiffies;
+
+	writereg(dev, PP_LineCTL, readreg(dev, PP_LineCTL) | SERIAL_TX_ON);
+
+	memcpy(test_packet,            dev->dev_addr, ETH_ALEN);
+	memcpy(test_packet + ETH_ALEN, dev->dev_addr, ETH_ALEN);
+
+	iowrite16(TX_AFTER_ALL, lp->virt_addr + TX_CMD_PORT);
+	iowrite16(ETH_ZLEN, lp->virt_addr + TX_LEN_PORT);
+
+	/* Test to see if the chip has allocated memory for the packet */
+	while (jiffies - timenow < 5)
+		if (readreg(dev, PP_BusST) & READY_FOR_TX_NOW)
+			break;
+	if (jiffies - timenow >= 5)
+		return 0;	/* this shouldn't happen */
+
+	/* Write the contents of the packet */
+	writewords(lp, TX_FRAME_PORT, test_packet, (ETH_ZLEN + 1) >> 1);
+
+	cs89_dbg(1, debug, "Sending test packet ");
+	/* wait a couple of jiffies for packet to be received */
+	for (timenow = jiffies; jiffies - timenow < 3;)
+		;
+	if ((readreg(dev, PP_TxEvent) & TX_SEND_OK_BITS) == TX_OK) {
+		cs89_dbg(1, cont, "succeeded\n");
+		return 1;
+	}
+	cs89_dbg(1, cont, "failed\n");
+	return 0;
+}
+
 #define DETECTED_NONE  0
 #define DETECTED_RJ45H 1
 #define DETECTED_RJ45F 2
@@ -1021,40 +607,46 @@ detect_tp(struct net_device *dev)
 	int timenow = jiffies;
 	int fdx;
 
-	if (net_debug > 1) printk("%s: Attempting TP\n", dev->name);
+	cs89_dbg(1, debug, "%s: Attempting TP\n", dev->name);
 
-        /* If connected to another full duplex capable 10-Base-T card the link pulses
-           seem to be lost when the auto detect bit in the LineCTL is set.
-           To overcome this the auto detect bit will be cleared whilst testing the
-           10-Base-T interface.  This would not be necessary for the sparrow chip but
-           is simpler to do it anyway. */
-	writereg(dev, PP_LineCTL, lp->linectl &~ AUI_ONLY);
+	/* If connected to another full duplex capable 10-Base-T card
+	 * the link pulses seem to be lost when the auto detect bit in
+	 * the LineCTL is set.  To overcome this the auto detect bit will
+	 * be cleared whilst testing the 10-Base-T interface.  This would
+	 * not be necessary for the sparrow chip but is simpler to do it
+	 * anyway.
+	 */
+	writereg(dev, PP_LineCTL, lp->linectl & ~AUI_ONLY);
 	control_dc_dc(dev, 0);
 
-        /* Delay for the hardware to work out if the TP cable is present - 150ms */
-	for (timenow = jiffies; jiffies - timenow < 15; )
-                ;
+	/* Delay for the hardware to work out if the TP cable is present
+	 * - 150ms
+	 */
+	for (timenow = jiffies; jiffies - timenow < 15;)
+		;
 	if ((readreg(dev, PP_LineST) & LINK_OK) == 0)
 		return DETECTED_NONE;
 
 	if (lp->chip_type == CS8900) {
-                switch (lp->force & 0xf0) {
+		switch (lp->force & 0xf0) {
 #if 0
-                case FORCE_AUTO:
-			printk("%s: cs8900 doesn't autonegotiate\n",dev->name);
-                        return DETECTED_NONE;
+		case FORCE_AUTO:
+			pr_info("%s: cs8900 doesn't autonegotiate\n",
+				dev->name);
+			return DETECTED_NONE;
 #endif
-		/* CS8900 doesn't support AUTO, change to HALF*/
-                case FORCE_AUTO:
+			/* CS8900 doesn't support AUTO, change to HALF*/
+		case FORCE_AUTO:
 			lp->force &= ~FORCE_AUTO;
-                        lp->force |= FORCE_HALF;
+			lp->force |= FORCE_HALF;
 			break;
 		case FORCE_HALF:
 			break;
-                case FORCE_FULL:
-			writereg(dev, PP_TestCTL, readreg(dev, PP_TestCTL) | FDX_8900);
+		case FORCE_FULL:
+			writereg(dev, PP_TestCTL,
+				 readreg(dev, PP_TestCTL) | FDX_8900);
 			break;
-                }
+		}
 		fdx = readreg(dev, PP_TestCTL) & FDX_8900;
 	} else {
 		switch (lp->force & 0xf0) {
@@ -1067,15 +659,15 @@ detect_tp(struct net_device *dev)
 		case FORCE_FULL:
 			lp->auto_neg_cnf = RE_NEG_NOW | ALLOW_FDX;
 			break;
-                }
+		}
 
 		writereg(dev, PP_AutoNegCTL, lp->auto_neg_cnf & AUTO_NEG_MASK);
 
 		if ((lp->auto_neg_cnf & AUTO_NEG_BITS) == AUTO_NEG_ENABLE) {
-			printk(KERN_INFO "%s: negotiating duplex...\n",dev->name);
+			pr_info("%s: negotiating duplex...\n", dev->name);
 			while (readreg(dev, PP_AutoNegST) & AUTO_NEG_BUSY) {
 				if (jiffies - timenow > 4000) {
-					printk(KERN_ERR "**** Full / half duplex auto-negotiation timed out ****\n");
+					pr_err("**** Full / half duplex auto-negotiation timed out ****\n");
 					break;
 				}
 			}
@@ -1088,56 +680,31 @@ detect_tp(struct net_device *dev)
 		return DETECTED_RJ45H;
 }
 
-/* send a test packet - return true if carrier bits are ok */
 static int
-send_test_pkt(struct net_device *dev)
+detect_bnc(struct net_device *dev)
 {
-	char test_packet[] = { 0,0,0,0,0,0, 0,0,0,0,0,0,
-				 0, 46, /* A 46 in network order */
-				 0, 0, /* DSAP=0 & SSAP=0 fields */
-				 0xf3, 0 /* Control (Test Req + P bit set) */ };
-	long timenow = jiffies;
-
-	writereg(dev, PP_LineCTL, readreg(dev, PP_LineCTL) | SERIAL_TX_ON);
-
-	memcpy(test_packet,          dev->dev_addr, ETH_ALEN);
-	memcpy(test_packet+ETH_ALEN, dev->dev_addr, ETH_ALEN);
-
-        writeword(dev->base_addr, TX_CMD_PORT, TX_AFTER_ALL);
-        writeword(dev->base_addr, TX_LEN_PORT, ETH_ZLEN);
+	struct net_local *lp = netdev_priv(dev);
 
-	/* Test to see if the chip has allocated memory for the packet */
-	while (jiffies - timenow < 5)
-		if (readreg(dev, PP_BusST) & READY_FOR_TX_NOW)
-			break;
-	if (jiffies - timenow >= 5)
-		return 0;	/* this shouldn't happen */
+	cs89_dbg(1, debug, "%s: Attempting BNC\n", dev->name);
+	control_dc_dc(dev, 1);
 
-	/* Write the contents of the packet */
-	writewords(dev->base_addr, TX_FRAME_PORT,test_packet,(ETH_ZLEN+1) >>1);
+	writereg(dev, PP_LineCTL, (lp->linectl & ~AUTO_AUI_10BASET) | AUI_ONLY);
 
-	if (net_debug > 1) printk("Sending test packet ");
-	/* wait a couple of jiffies for packet to be received */
-	for (timenow = jiffies; jiffies - timenow < 3; )
-                ;
-        if ((readreg(dev, PP_TxEvent) & TX_SEND_OK_BITS) == TX_OK) {
-                if (net_debug > 1) printk("succeeded\n");
-                return 1;
-        }
-	if (net_debug > 1) printk("failed\n");
-	return 0;
+	if (send_test_pkt(dev))
+		return DETECTED_BNC;
+	else
+		return DETECTED_NONE;
 }
 
-
 static int
 detect_aui(struct net_device *dev)
 {
 	struct net_local *lp = netdev_priv(dev);
 
-	if (net_debug > 1) printk("%s: Attempting AUI\n", dev->name);
+	cs89_dbg(1, debug, "%s: Attempting AUI\n", dev->name);
 	control_dc_dc(dev, 0);
 
-	writereg(dev, PP_LineCTL, (lp->linectl &~ AUTO_AUI_10BASET) | AUI_ONLY);
+	writereg(dev, PP_LineCTL, (lp->linectl & ~AUTO_AUI_10BASET) | AUI_ONLY);
 
 	if (send_test_pkt(dev))
 		return DETECTED_AUI;
@@ -1145,45 +712,154 @@ detect_aui(struct net_device *dev)
 		return DETECTED_NONE;
 }
 
-static int
-detect_bnc(struct net_device *dev)
+/* We have a good packet(s), get it/them out of the buffers. */
+static void
+net_rx(struct net_device *dev)
 {
 	struct net_local *lp = netdev_priv(dev);
+	struct sk_buff *skb;
+	int status, length;
 
-	if (net_debug > 1) printk("%s: Attempting BNC\n", dev->name);
-	control_dc_dc(dev, 1);
+	status = ioread16(lp->virt_addr + RX_FRAME_PORT);
+	length = ioread16(lp->virt_addr + RX_FRAME_PORT);
 
-	writereg(dev, PP_LineCTL, (lp->linectl &~ AUTO_AUI_10BASET) | AUI_ONLY);
+	if ((status & RX_OK) == 0) {
+		count_rx_errors(status, dev);
+		return;
+	}
 
-	if (send_test_pkt(dev))
-		return DETECTED_BNC;
-	else
-		return DETECTED_NONE;
+	/* 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;
+	}
+	skb_reserve(skb, 2);	/* longword align L3 header */
+
+	readwords(lp, RX_FRAME_PORT, skb_put(skb, length), length >> 1);
+	if (length & 1)
+		skb->data[length-1] = ioread16(lp->virt_addr + RX_FRAME_PORT);
+
+	cs89_dbg(3, debug, "%s: received %d byte packet of type %x\n",
+		 dev->name, length,
+		 (skb->data[ETH_ALEN + ETH_ALEN] << 8) |
+		 skb->data[ETH_ALEN + ETH_ALEN + 1]);
+
+	skb->protocol = eth_type_trans(skb, dev);
+	netif_rx(skb);
+	dev->stats.rx_packets++;
+	dev->stats.rx_bytes += length;
 }
 
+/* The typical workload of the driver:
+ * Handle the network interface interrupts.
+ */
 
-static void
-write_irq(struct net_device *dev, int chip_type, int irq)
+static irqreturn_t net_interrupt(int irq, void *dev_id)
 {
-	int i;
+	struct net_device *dev = dev_id;
+	struct net_local *lp;
+	int status;
+	int handled = 0;
 
-	if (chip_type == CS8900) {
-#ifndef CONFIG_CS89x0_PLATFORM
-		/* Search the mapping table for the corresponding IRQ pin. */
-		for (i = 0; i != ARRAY_SIZE(cs8900_irq_map); i++)
-			if (cs8900_irq_map[i] == irq)
-				break;
-		/* Not found */
-		if (i == ARRAY_SIZE(cs8900_irq_map))
-			i = 3;
-#else
-		/* INTRQ0 pin is used for interrupt generation. */
-		i = 0;
+	lp = netdev_priv(dev);
+
+	/* we MUST read all the events out of the ISQ, otherwise we'll never
+	 * get interrupted again.  As a consequence, we can't have any limit
+	 * on the number of times we loop in the interrupt handler.  The
+	 * hardware guarantees that eventually we'll run out of events.  Of
+	 * course, if you're on a slow machine, and packets are arriving
+	 * faster than you can read them off, you're screwed.  Hasta la
+	 * vista, baby!
+	 */
+	while ((status = ioread16(lp->virt_addr + ISQ_PORT))) {
+		cs89_dbg(4, debug, "%s: event=%04x\n", dev->name, status);
+		handled = 1;
+		switch (status & ISQ_EVENT_MASK) {
+		case ISQ_RECEIVER_EVENT:
+			/* Got a packet(s). */
+			net_rx(dev);
+			break;
+		case ISQ_TRANSMITTER_EVENT:
+			dev->stats.tx_packets++;
+			netif_wake_queue(dev);	/* Inform upper layers. */
+			if ((status & (TX_OK |
+				       TX_LOST_CRS |
+				       TX_SQE_ERROR |
+				       TX_LATE_COL |
+				       TX_16_COL)) != TX_OK) {
+				if ((status & TX_OK) == 0)
+					dev->stats.tx_errors++;
+				if (status & TX_LOST_CRS)
+					dev->stats.tx_carrier_errors++;
+				if (status & TX_SQE_ERROR)
+					dev->stats.tx_heartbeat_errors++;
+				if (status & TX_LATE_COL)
+					dev->stats.tx_window_errors++;
+				if (status & TX_16_COL)
+					dev->stats.tx_aborted_errors++;
+			}
+			break;
+		case ISQ_BUFFER_EVENT:
+			if (status & READY_FOR_TX) {
+				/* we tried to transmit a packet earlier,
+				 * but inexplicably ran out of buffers.
+				 * That shouldn't happen since we only ever
+				 * load one packet.  Shrug.  Do the right
+				 * thing anyway.
+				 */
+				netif_wake_queue(dev);	/* Inform upper layers. */
+			}
+			if (status & TX_UNDERRUN) {
+				cs89_dbg(0, err, "%s: transmit underrun\n",
+					 dev->name);
+				lp->send_underrun++;
+				if (lp->send_underrun == 3)
+					lp->send_cmd = TX_AFTER_381;
+				else if (lp->send_underrun == 6)
+					lp->send_cmd = TX_AFTER_ALL;
+				/* transmit cycle is done, although
+				 * frame wasn't transmitted - this
+				 * avoids having to wait for the upper
+				 * layers to timeout on us, in the
+				 * event of a tx underrun
+				 */
+				netif_wake_queue(dev);	/* Inform upper layers. */
+			}
+#if ALLOW_DMA
+			if (lp->use_dma && (status & RX_DMA)) {
+				int count = readreg(dev, PP_DmaFrameCnt);
+				while (count) {
+					cs89_dbg(5, debug,
+						 "%s: receiving %d DMA frames\n",
+						 dev->name, count);
+					if (count > 1)
+						cs89_dbg(2, debug,
+							 "%s: receiving %d DMA frames\n",
+							 dev->name, count);
+					dma_rx(dev);
+					if (--count == 0)
+						count = readreg(dev, PP_DmaFrameCnt);
+					if (count > 0)
+						cs89_dbg(2, debug,
+							 "%s: continuing with %d DMA frames\n",
+							 dev->name, count);
+				}
+			}
 #endif
-		writereg(dev, PP_CS8900_ISAINT, i);
-	} else {
-		writereg(dev, PP_CS8920_ISAINT, irq);
+			break;
+		case ISQ_RX_MISS_EVENT:
+			dev->stats.rx_missed_errors += (status >> 6);
+			break;
+		case ISQ_TX_COL_EVENT:
+			dev->stats.collisions += (status >> 6);
+			break;
+		}
 	}
+	return IRQ_RETVAL(handled);
 }
 
 /* Open/initialize the board.  This is called (in the current kernel)
@@ -1192,7 +868,7 @@ write_irq(struct net_device *dev, int chip_type, int irq)
    This routine should set everything up anew at each open, even
    registers that "should" only need to be set once at boot, so that
    there is non-reboot way to recover if something goes wrong.
-   */
+*/
 
 /* AKPM: do we need to do any locking here? */
 
@@ -1208,14 +884,15 @@ net_open(struct net_device *dev)
 		/* Allow interrupts to be generated by the chip */
 /* Cirrus' release had this: */
 #if 0
-		writereg(dev, PP_BusCTL, readreg(dev, PP_BusCTL)|ENABLE_IRQ );
+		writereg(dev, PP_BusCTL, readreg(dev, PP_BusCTL) | ENABLE_IRQ);
 #endif
 /* And 2.3.47 had this: */
 		writereg(dev, PP_BusCTL, ENABLE_IRQ | MEMORY_ON);
 
 		for (i = 2; i < CS8920_NO_INTS; i++) {
 			if ((1 << i) & lp->irq_map) {
-				if (request_irq(i, net_interrupt, 0, dev->name, dev) == 0) {
+				if (request_irq(i, net_interrupt, 0, dev->name,
+						dev) == 0) {
 					dev->irq = i;
 					write_irq(dev, lp->chip_type, i);
 					/* writereg(dev, PP_BufCFG, GENERATE_SW_INTERRUPT); */
@@ -1226,23 +903,21 @@ net_open(struct net_device *dev)
 
 		if (i >= CS8920_NO_INTS) {
 			writereg(dev, PP_BusCTL, 0);	/* disable interrupts. */
-			printk(KERN_ERR "cs89x0: can't get an interrupt\n");
+			pr_err("can't get an interrupt\n");
 			ret = -EAGAIN;
 			goto bad_out;
 		}
-	}
-	else
-	{
+	} else {
 #if !defined(CS89x0_NONISA_IRQ) && !defined(CONFIG_CS89x0_PLATFORM)
 		if (((1 << dev->irq) & lp->irq_map) == 0) {
-			printk(KERN_ERR "%s: IRQ %d is not in our map of allowable IRQs, which is %x\n",
-                               dev->name, dev->irq, lp->irq_map);
+			pr_err("%s: IRQ %d is not in our map of allowable IRQs, which is %x\n",
+			       dev->name, dev->irq, lp->irq_map);
 			ret = -EAGAIN;
 			goto bad_out;
 		}
 #endif
 /* FIXME: Cirrus' release had this: */
-		writereg(dev, PP_BusCTL, readreg(dev, PP_BusCTL)|ENABLE_IRQ );
+		writereg(dev, PP_BusCTL, readreg(dev, PP_BusCTL)|ENABLE_IRQ);
 /* And 2.3.47 had this: */
 #if 0
 		writereg(dev, PP_BusCTL, ENABLE_IRQ | MEMORY_ON);
@@ -1250,147 +925,168 @@ net_open(struct net_device *dev)
 		write_irq(dev, lp->chip_type, dev->irq);
 		ret = request_irq(dev->irq, net_interrupt, 0, dev->name, dev);
 		if (ret) {
-			printk(KERN_ERR "cs89x0: request_irq(%d) failed\n", dev->irq);
+			pr_err("request_irq(%d) failed\n", dev->irq);
 			goto bad_out;
 		}
 	}
 
 #if ALLOW_DMA
-	if (lp->use_dma) {
-		if (lp->isa_config & ANY_ISA_DMA) {
-			unsigned long flags;
-			lp->dma_buff = (unsigned char *)__get_dma_pages(GFP_KERNEL,
-							get_order(lp->dmasize * 1024));
-
-			if (!lp->dma_buff) {
-				printk(KERN_ERR "%s: cannot get %dK memory for DMA\n", dev->name, lp->dmasize);
-				goto release_irq;
-			}
-			if (net_debug > 1) {
-				printk(	"%s: dma %lx %lx\n",
-					dev->name,
-					(unsigned long)lp->dma_buff,
-					(unsigned long)isa_virt_to_bus(lp->dma_buff));
-			}
-			if ((unsigned long) lp->dma_buff >= MAX_DMA_ADDRESS ||
-			    !dma_page_eq(lp->dma_buff, lp->dma_buff+lp->dmasize*1024-1)) {
-				printk(KERN_ERR "%s: not usable as DMA buffer\n", dev->name);
-				goto release_irq;
-			}
-			memset(lp->dma_buff, 0, lp->dmasize * 1024);	/* Why? */
-			if (request_dma(dev->dma, dev->name)) {
-				printk(KERN_ERR "%s: cannot get dma channel %d\n", dev->name, dev->dma);
-				goto release_irq;
-			}
-			write_dma(dev, lp->chip_type, dev->dma);
-			lp->rx_dma_ptr = lp->dma_buff;
-			lp->end_dma_buff = lp->dma_buff + lp->dmasize*1024;
-			spin_lock_irqsave(&lp->lock, flags);
-			disable_dma(dev->dma);
-			clear_dma_ff(dev->dma);
-			set_dma_mode(dev->dma, DMA_RX_MODE); /* auto_init as well */
-			set_dma_addr(dev->dma, isa_virt_to_bus(lp->dma_buff));
-			set_dma_count(dev->dma, lp->dmasize*1024);
-			enable_dma(dev->dma);
-			spin_unlock_irqrestore(&lp->lock, flags);
+	if (lp->use_dma && (lp->isa_config & ANY_ISA_DMA)) {
+		unsigned long flags;
+		lp->dma_buff = (unsigned char *)__get_dma_pages(GFP_KERNEL,
+								get_order(lp->dmasize * 1024));
+		if (!lp->dma_buff) {
+			pr_err("%s: cannot get %dK memory for DMA\n",
+			       dev->name, lp->dmasize);
+			goto release_irq;
+		}
+		cs89_dbg(1, debug, "%s: dma %lx %lx\n",
+			 dev->name,
+			 (unsigned long)lp->dma_buff,
+			 (unsigned long)isa_virt_to_bus(lp->dma_buff));
+		if ((unsigned long)lp->dma_buff >= MAX_DMA_ADDRESS ||
+		    !dma_page_eq(lp->dma_buff,
+				 lp->dma_buff + lp->dmasize * 1024 - 1)) {
+			pr_err("%s: not usable as DMA buffer\n", dev->name);
+			goto release_irq;
 		}
+		memset(lp->dma_buff, 0, lp->dmasize * 1024);	/* Why? */
+		if (request_dma(dev->dma, dev->name)) {
+			pr_err("%s: cannot get dma channel %d\n",
+			       dev->name, dev->dma);
+			goto release_irq;
+		}
+		write_dma(dev, lp->chip_type, dev->dma);
+		lp->rx_dma_ptr = lp->dma_buff;
+		lp->end_dma_buff = lp->dma_buff + lp->dmasize * 1024;
+		spin_lock_irqsave(&lp->lock, flags);
+		disable_dma(dev->dma);
+		clear_dma_ff(dev->dma);
+		set_dma_mode(dev->dma, DMA_RX_MODE); /* auto_init as well */
+		set_dma_addr(dev->dma, isa_virt_to_bus(lp->dma_buff));
+		set_dma_count(dev->dma, lp->dmasize * 1024);
+		enable_dma(dev->dma);
+		spin_unlock_irqrestore(&lp->lock, flags);
 	}
 #endif	/* ALLOW_DMA */
 
 	/* set the Ethernet address */
-	for (i=0; i < ETH_ALEN/2; i++)
-		writereg(dev, PP_IA+i*2, dev->dev_addr[i*2] | (dev->dev_addr[i*2+1] << 8));
+	for (i = 0; i < ETH_ALEN / 2; i++)
+		writereg(dev, PP_IA + i * 2,
+			 (dev->dev_addr[i * 2] |
+			  (dev->dev_addr[i * 2 + 1] << 8)));
 
 	/* while we're testing the interface, leave interrupts disabled */
 	writereg(dev, PP_BusCTL, MEMORY_ON);
 
 	/* Set the LineCTL quintuplet based on adapter configuration read from EEPROM */
-	if ((lp->adapter_cnf & A_CNF_EXTND_10B_2) && (lp->adapter_cnf & A_CNF_LOW_RX_SQUELCH))
-                lp->linectl = LOW_RX_SQUELCH;
+	if ((lp->adapter_cnf & A_CNF_EXTND_10B_2) &&
+	    (lp->adapter_cnf & A_CNF_LOW_RX_SQUELCH))
+		lp->linectl = LOW_RX_SQUELCH;
 	else
-                lp->linectl = 0;
-
-        /* check to make sure that they have the "right" hardware available */
-	switch(lp->adapter_cnf & A_CNF_MEDIA_TYPE) {
-	case A_CNF_MEDIA_10B_T: result = lp->adapter_cnf & A_CNF_10B_T; break;
-	case A_CNF_MEDIA_AUI:   result = lp->adapter_cnf & A_CNF_AUI; break;
-	case A_CNF_MEDIA_10B_2: result = lp->adapter_cnf & A_CNF_10B_2; break;
-        default: result = lp->adapter_cnf & (A_CNF_10B_T | A_CNF_AUI | A_CNF_10B_2);
-        }
-        if (!result) {
-                printk(KERN_ERR "%s: EEPROM is configured for unavailable media\n", dev->name);
+		lp->linectl = 0;
+
+	/* check to make sure that they have the "right" hardware available */
+	switch (lp->adapter_cnf & A_CNF_MEDIA_TYPE) {
+	case A_CNF_MEDIA_10B_T:
+		result = lp->adapter_cnf & A_CNF_10B_T;
+		break;
+	case A_CNF_MEDIA_AUI:
+		result = lp->adapter_cnf & A_CNF_AUI;
+		break;
+	case A_CNF_MEDIA_10B_2:
+		result = lp->adapter_cnf & A_CNF_10B_2;
+		break;
+	default:
+		result = lp->adapter_cnf & (A_CNF_10B_T |
+					    A_CNF_AUI |
+					    A_CNF_10B_2);
+	}
+	if (!result) {
+		pr_err("%s: EEPROM is configured for unavailable media\n",
+		       dev->name);
 release_dma:
 #if ALLOW_DMA
 		free_dma(dev->dma);
 release_irq:
 		release_dma_buff(lp);
 #endif
-                writereg(dev, PP_LineCTL, readreg(dev, PP_LineCTL) & ~(SERIAL_TX_ON | SERIAL_RX_ON));
-                free_irq(dev->irq, dev);
+		writereg(dev, PP_LineCTL,
+			 readreg(dev, PP_LineCTL) & ~(SERIAL_TX_ON | SERIAL_RX_ON));
+		free_irq(dev->irq, dev);
 		ret = -EAGAIN;
 		goto bad_out;
 	}
 
-        /* set the hardware to the configured choice */
-	switch(lp->adapter_cnf & A_CNF_MEDIA_TYPE) {
+	/* set the hardware to the configured choice */
+	switch (lp->adapter_cnf & A_CNF_MEDIA_TYPE) {
 	case A_CNF_MEDIA_10B_T:
-                result = detect_tp(dev);
-                if (result==DETECTED_NONE) {
-                        printk(KERN_WARNING "%s: 10Base-T (RJ-45) has no cable\n", dev->name);
-                        if (lp->auto_neg_cnf & IMM_BIT) /* check "ignore missing media" bit */
-                                result = DETECTED_RJ45H; /* Yes! I don't care if I see a link pulse */
-                }
+		result = detect_tp(dev);
+		if (result == DETECTED_NONE) {
+			pr_warn("%s: 10Base-T (RJ-45) has no cable\n",
+				dev->name);
+			if (lp->auto_neg_cnf & IMM_BIT) /* check "ignore missing media" bit */
+				result = DETECTED_RJ45H; /* Yes! I don't care if I see a link pulse */
+		}
 		break;
 	case A_CNF_MEDIA_AUI:
-                result = detect_aui(dev);
-                if (result==DETECTED_NONE) {
-                        printk(KERN_WARNING "%s: 10Base-5 (AUI) has no cable\n", dev->name);
-                        if (lp->auto_neg_cnf & IMM_BIT) /* check "ignore missing media" bit */
-                                result = DETECTED_AUI; /* Yes! I don't care if I see a carrrier */
-                }
+		result = detect_aui(dev);
+		if (result == DETECTED_NONE) {
+			pr_warn("%s: 10Base-5 (AUI) has no cable\n", dev->name);
+			if (lp->auto_neg_cnf & IMM_BIT) /* check "ignore missing media" bit */
+				result = DETECTED_AUI; /* Yes! I don't care if I see a carrrier */
+		}
 		break;
 	case A_CNF_MEDIA_10B_2:
-                result = detect_bnc(dev);
-                if (result==DETECTED_NONE) {
-                        printk(KERN_WARNING "%s: 10Base-2 (BNC) has no cable\n", dev->name);
-                        if (lp->auto_neg_cnf & IMM_BIT) /* check "ignore missing media" bit */
-                                result = DETECTED_BNC; /* Yes! I don't care if I can xmit a packet */
-                }
+		result = detect_bnc(dev);
+		if (result == DETECTED_NONE) {
+			pr_warn("%s: 10Base-2 (BNC) has no cable\n", dev->name);
+			if (lp->auto_neg_cnf & IMM_BIT) /* check "ignore missing media" bit */
+				result = DETECTED_BNC; /* Yes! I don't care if I can xmit a packet */
+		}
 		break;
 	case A_CNF_MEDIA_AUTO:
 		writereg(dev, PP_LineCTL, lp->linectl | AUTO_AUI_10BASET);
-		if (lp->adapter_cnf & A_CNF_10B_T)
-			if ((result = detect_tp(dev)) != DETECTED_NONE)
+		if (lp->adapter_cnf & A_CNF_10B_T) {
+			result = detect_tp(dev);
+			if (result != DETECTED_NONE)
 				break;
-		if (lp->adapter_cnf & A_CNF_AUI)
-			if ((result = detect_aui(dev)) != DETECTED_NONE)
+		}
+		if (lp->adapter_cnf & A_CNF_AUI) {
+			result = detect_aui(dev);
+			if (result != DETECTED_NONE)
 				break;
-		if (lp->adapter_cnf & A_CNF_10B_2)
-			if ((result = detect_bnc(dev)) != DETECTED_NONE)
+		}
+		if (lp->adapter_cnf & A_CNF_10B_2) {
+			result = detect_bnc(dev);
+			if (result != DETECTED_NONE)
 				break;
-		printk(KERN_ERR "%s: no media detected\n", dev->name);
+		}
+		pr_err("%s: no media detected\n", dev->name);
 		goto release_dma;
 	}
-	switch(result) {
+	switch (result) {
 	case DETECTED_NONE:
-		printk(KERN_ERR "%s: no network cable attached to configured media\n", dev->name);
+		pr_err("%s: no network cable attached to configured media\n",
+		       dev->name);
 		goto release_dma;
 	case DETECTED_RJ45H:
-		printk(KERN_INFO "%s: using half-duplex 10Base-T (RJ-45)\n", dev->name);
+		pr_info("%s: using half-duplex 10Base-T (RJ-45)\n", dev->name);
 		break;
 	case DETECTED_RJ45F:
-		printk(KERN_INFO "%s: using full-duplex 10Base-T (RJ-45)\n", dev->name);
+		pr_info("%s: using full-duplex 10Base-T (RJ-45)\n", dev->name);
 		break;
 	case DETECTED_AUI:
-		printk(KERN_INFO "%s: using 10Base-5 (AUI)\n", dev->name);
+		pr_info("%s: using 10Base-5 (AUI)\n", dev->name);
 		break;
 	case DETECTED_BNC:
-		printk(KERN_INFO "%s: using 10Base-2 (BNC)\n", dev->name);
+		pr_info("%s: using 10Base-2 (BNC)\n", dev->name);
 		break;
 	}
 
 	/* Turn on both receive and transmit operations */
-	writereg(dev, PP_LineCTL, readreg(dev, PP_LineCTL) | SERIAL_RX_ON | SERIAL_TX_ON);
+	writereg(dev, PP_LineCTL,
+		 readreg(dev, PP_LineCTL) | SERIAL_RX_ON | SERIAL_TX_ON);
 
 	/* Receive only error free packets addressed to this card */
 	lp->rx_mode = 0;
@@ -1405,358 +1101,653 @@ release_irq:
 #endif
 	writereg(dev, PP_RxCFG, lp->curr_rx_cfg);
 
-	writereg(dev, PP_TxCFG, TX_LOST_CRS_ENBL | TX_SQE_ERROR_ENBL | TX_OK_ENBL |
-		TX_LATE_COL_ENBL | TX_JBR_ENBL | TX_ANY_COL_ENBL | TX_16_COL_ENBL);
+	writereg(dev, PP_TxCFG, (TX_LOST_CRS_ENBL |
+				 TX_SQE_ERROR_ENBL |
+				 TX_OK_ENBL |
+				 TX_LATE_COL_ENBL |
+				 TX_JBR_ENBL |
+				 TX_ANY_COL_ENBL |
+				 TX_16_COL_ENBL));
 
-	writereg(dev, PP_BufCFG, READY_FOR_TX_ENBL | RX_MISS_COUNT_OVRFLOW_ENBL |
+	writereg(dev, PP_BufCFG, (READY_FOR_TX_ENBL |
+				  RX_MISS_COUNT_OVRFLOW_ENBL |
 #if ALLOW_DMA
-		dma_bufcfg(dev) |
+				  dma_bufcfg(dev) |
 #endif
-		TX_COL_COUNT_OVRFLOW_ENBL | TX_UNDERRUN_ENBL);
+				  TX_COL_COUNT_OVRFLOW_ENBL |
+				  TX_UNDERRUN_ENBL));
 
 	/* now that we've got our act together, enable everything */
-	writereg(dev, PP_BusCTL, ENABLE_IRQ
-		 | (dev->mem_start?MEMORY_ON : 0) /* turn memory on */
+	writereg(dev, PP_BusCTL, (ENABLE_IRQ
+				  | (dev->mem_start ? MEMORY_ON : 0) /* turn memory on */
 #if ALLOW_DMA
-		 | dma_busctl(dev)
+				  | dma_busctl(dev)
 #endif
-                 );
-        netif_start_queue(dev);
-	if (net_debug > 1)
-		printk("cs89x0: net_open() succeeded\n");
+			 ));
+	netif_start_queue(dev);
+	cs89_dbg(1, debug, "net_open() succeeded\n");
 	return 0;
 bad_out:
 	return ret;
 }
 
+/* The inverse routine to net_open(). */
+static int
+net_close(struct net_device *dev)
+{
+#if ALLOW_DMA
+	struct net_local *lp = netdev_priv(dev);
+#endif
+
+	netif_stop_queue(dev);
+
+	writereg(dev, PP_RxCFG, 0);
+	writereg(dev, PP_TxCFG, 0);
+	writereg(dev, PP_BufCFG, 0);
+	writereg(dev, PP_BusCTL, 0);
+
+	free_irq(dev->irq, dev);
+
+#if ALLOW_DMA
+	if (lp->use_dma && lp->dma) {
+		free_dma(dev->dma);
+		release_dma_buff(lp);
+	}
+#endif
+
+	/* Update the statistics here. */
+	return 0;
+}
+
+/* Get the current statistics.
+ * This may be called with the card open or closed.
+ */
+static struct net_device_stats *
+net_get_stats(struct net_device *dev)
+{
+	struct net_local *lp = netdev_priv(dev);
+	unsigned long flags;
+
+	spin_lock_irqsave(&lp->lock, flags);
+	/* Update the statistics from the device registers. */
+	dev->stats.rx_missed_errors += (readreg(dev, PP_RxMiss) >> 6);
+	dev->stats.collisions += (readreg(dev, PP_TxCol) >> 6);
+	spin_unlock_irqrestore(&lp->lock, flags);
+
+	return &dev->stats;
+}
+
 static void net_timeout(struct net_device *dev)
 {
 	/* If we get here, some higher level has decided we are broken.
 	   There should really be a "kick me" function call instead. */
-	if (net_debug > 0) printk("%s: transmit timed out, %s?\n", dev->name,
-		   tx_done(dev) ? "IRQ conflict ?" : "network cable problem");
+	cs89_dbg(0, err, "%s: transmit timed out, %s?\n",
+		 dev->name,
+		 tx_done(dev) ? "IRQ conflict" : "network cable problem");
 	/* Try to restart the adaptor. */
 	netif_wake_queue(dev);
 }
 
-static netdev_tx_t net_send_packet(struct sk_buff *skb,struct net_device *dev)
+static netdev_tx_t net_send_packet(struct sk_buff *skb, struct net_device *dev)
 {
 	struct net_local *lp = netdev_priv(dev);
 	unsigned long flags;
 
-	if (net_debug > 3) {
-		printk("%s: sent %d byte packet of type %x\n",
-			dev->name, skb->len,
-			(skb->data[ETH_ALEN+ETH_ALEN] << 8) | skb->data[ETH_ALEN+ETH_ALEN+1]);
-	}
+	cs89_dbg(3, debug, "%s: sent %d byte packet of type %x\n",
+		 dev->name, skb->len,
+		 ((skb->data[ETH_ALEN + ETH_ALEN] << 8) |
+		  skb->data[ETH_ALEN + ETH_ALEN + 1]));
 
 	/* keep the upload from being interrupted, since we
-                  ask the chip to start transmitting before the
-                  whole packet has been completely uploaded. */
+	 * ask the chip to start transmitting before the
+	 * whole packet has been completely uploaded.
+	 */
 
 	spin_lock_irqsave(&lp->lock, flags);
 	netif_stop_queue(dev);
 
 	/* initiate a transmit sequence */
-	writeword(dev->base_addr, TX_CMD_PORT, lp->send_cmd);
-	writeword(dev->base_addr, TX_LEN_PORT, skb->len);
+	iowrite16(lp->send_cmd, lp->virt_addr + TX_CMD_PORT);
+	iowrite16(skb->len, lp->virt_addr + TX_LEN_PORT);
 
 	/* Test to see if the chip has allocated memory for the packet */
 	if ((readreg(dev, PP_BusST) & READY_FOR_TX_NOW) == 0) {
-		/*
-		 * Gasp!  It hasn't.  But that shouldn't happen since
+		/* Gasp!  It hasn't.  But that shouldn't happen since
 		 * we're waiting for TxOk, so return 1 and requeue this packet.
 		 */
 
 		spin_unlock_irqrestore(&lp->lock, flags);
-		if (net_debug) printk("cs89x0: Tx buffer not free!\n");
+		cs89_dbg(0, err, "Tx buffer not free!\n");
 		return NETDEV_TX_BUSY;
 	}
 	/* Write the contents of the packet */
-	writewords(dev->base_addr, TX_FRAME_PORT,skb->data,(skb->len+1) >>1);
+	writewords(lp, TX_FRAME_PORT, skb->data, (skb->len + 1) >> 1);
 	spin_unlock_irqrestore(&lp->lock, flags);
 	dev->stats.tx_bytes += skb->len;
-	dev_kfree_skb (skb);
+	dev_kfree_skb(skb);
 
-	/*
-	 * We DO NOT call netif_wake_queue() here.
+	/* We DO NOT call netif_wake_queue() here.
 	 * We also DO NOT call netif_start_queue().
 	 *
 	 * Either of these would cause another bottom half run through
-	 * net_send_packet() before this packet has fully gone out.  That causes
-	 * us to hit the "Gasp!" above and the send is rescheduled.  it runs like
-	 * a dog.  We just return and wait for the Tx completion interrupt handler
-	 * to restart the netdevice layer
+	 * net_send_packet() before this packet has fully gone out.
+	 * That causes us to hit the "Gasp!" above and the send is rescheduled.
+	 * it runs like a dog.  We just return and wait for the Tx completion
+	 * interrupt handler to restart the netdevice layer
 	 */
 
 	return NETDEV_TX_OK;
 }
 
-/* The typical workload of the driver:
-   Handle the network interface interrupts. */
+static void set_multicast_list(struct net_device *dev)
+{
+	struct net_local *lp = netdev_priv(dev);
+	unsigned long flags;
 
-static irqreturn_t net_interrupt(int irq, void *dev_id)
+	spin_lock_irqsave(&lp->lock, flags);
+	if (dev->flags & IFF_PROMISC)
+		lp->rx_mode = RX_ALL_ACCEPT;
+	else if ((dev->flags & IFF_ALLMULTI) || !netdev_mc_empty(dev))
+		/* The multicast-accept list is initialized to accept-all,
+		 * and we rely on higher-level filtering for now.
+		 */
+		lp->rx_mode = RX_MULTCAST_ACCEPT;
+	else
+		lp->rx_mode = 0;
+
+	writereg(dev, PP_RxCTL, DEF_RX_ACCEPT | lp->rx_mode);
+
+	/* in promiscuous mode, we accept errored packets,
+	 * so we have to enable interrupts on them also
+	 */
+	writereg(dev, PP_RxCFG,
+		 (lp->curr_rx_cfg |
+		  (lp->rx_mode == RX_ALL_ACCEPT)
+		  ? (RX_CRC_ERROR_ENBL | RX_RUNT_ENBL | RX_EXTRA_DATA_ENBL)
+		  : 0));
+	spin_unlock_irqrestore(&lp->lock, flags);
+}
+
+static int set_mac_address(struct net_device *dev, void *p)
 {
-	struct net_device *dev = dev_id;
-	struct net_local *lp;
-	int ioaddr, status;
- 	int handled = 0;
+	int i;
+	struct sockaddr *addr = p;
 
-	ioaddr = dev->base_addr;
-	lp = netdev_priv(dev);
+	if (netif_running(dev))
+		return -EBUSY;
 
-	/* we MUST read all the events out of the ISQ, otherwise we'll never
-           get interrupted again.  As a consequence, we can't have any limit
-           on the number of times we loop in the interrupt handler.  The
-           hardware guarantees that eventually we'll run out of events.  Of
-           course, if you're on a slow machine, and packets are arriving
-           faster than you can read them off, you're screwed.  Hasta la
-           vista, baby!  */
-	while ((status = readword(dev->base_addr, ISQ_PORT))) {
-		if (net_debug > 4)printk("%s: event=%04x\n", dev->name, status);
-		handled = 1;
-		switch(status & ISQ_EVENT_MASK) {
-		case ISQ_RECEIVER_EVENT:
-			/* Got a packet(s). */
-			net_rx(dev);
-			break;
-		case ISQ_TRANSMITTER_EVENT:
-			dev->stats.tx_packets++;
-			netif_wake_queue(dev);	/* Inform upper layers. */
-			if ((status & (	TX_OK |
-					TX_LOST_CRS |
-					TX_SQE_ERROR |
-					TX_LATE_COL |
-					TX_16_COL)) != TX_OK) {
-				if ((status & TX_OK) == 0)
-					dev->stats.tx_errors++;
-				if (status & TX_LOST_CRS)
-					dev->stats.tx_carrier_errors++;
-				if (status & TX_SQE_ERROR)
-					dev->stats.tx_heartbeat_errors++;
-				if (status & TX_LATE_COL)
-					dev->stats.tx_window_errors++;
-				if (status & TX_16_COL)
-					dev->stats.tx_aborted_errors++;
-			}
-			break;
-		case ISQ_BUFFER_EVENT:
-			if (status & READY_FOR_TX) {
-				/* we tried to transmit a packet earlier,
-                                   but inexplicably ran out of buffers.
-                                   That shouldn't happen since we only ever
-                                   load one packet.  Shrug.  Do the right
-                                   thing anyway. */
-				netif_wake_queue(dev);	/* Inform upper layers. */
-			}
-			if (status & TX_UNDERRUN) {
-				if (net_debug > 0) printk("%s: transmit underrun\n", dev->name);
-                                lp->send_underrun++;
-                                if (lp->send_underrun == 3) lp->send_cmd = TX_AFTER_381;
-                                else if (lp->send_underrun == 6) lp->send_cmd = TX_AFTER_ALL;
-				/* transmit cycle is done, although
-				   frame wasn't transmitted - this
-				   avoids having to wait for the upper
-				   layers to timeout on us, in the
-				   event of a tx underrun */
-				netif_wake_queue(dev);	/* Inform upper layers. */
-                        }
-#if ALLOW_DMA
-			if (lp->use_dma && (status & RX_DMA)) {
-				int count = readreg(dev, PP_DmaFrameCnt);
-				while(count) {
-					if (net_debug > 5)
-						printk("%s: receiving %d DMA frames\n", dev->name, count);
-					if (net_debug > 2 && count >1)
-						printk("%s: receiving %d DMA frames\n", dev->name, count);
-					dma_rx(dev);
-					if (--count == 0)
-						count = readreg(dev, PP_DmaFrameCnt);
-					if (net_debug > 2 && count > 0)
-						printk("%s: continuing with %d DMA frames\n", dev->name, count);
-				}
-			}
-#endif
-			break;
-		case ISQ_RX_MISS_EVENT:
-			dev->stats.rx_missed_errors += (status >> 6);
-			break;
-		case ISQ_TX_COL_EVENT:
-			dev->stats.collisions += (status >> 6);
-			break;
-		}
-	}
-	return IRQ_RETVAL(handled);
+	memcpy(dev->dev_addr, addr->sa_data, dev->addr_len);
+
+	cs89_dbg(0, debug, "%s: Setting MAC address to %pM\n",
+		 dev->name, dev->dev_addr);
+
+	/* set the Ethernet address */
+	for (i = 0; i < ETH_ALEN / 2; i++)
+		writereg(dev, PP_IA + i * 2,
+			 (dev->dev_addr[i * 2] |
+			  (dev->dev_addr[i * 2 + 1] << 8)));
+
+	return 0;
 }
 
-static void
-count_rx_errors(int status, struct net_device *dev)
+#ifdef CONFIG_NET_POLL_CONTROLLER
+/*
+ * Polling receive - used by netconsole and other diagnostic tools
+ * to allow network i/o with interrupts disabled.
+ */
+static void net_poll_controller(struct net_device *dev)
 {
-	dev->stats.rx_errors++;
-	if (status & RX_RUNT)
-		dev->stats.rx_length_errors++;
-	if (status & RX_EXTRA_DATA)
-		dev->stats.rx_length_errors++;
-	if ((status & RX_CRC_ERROR) && !(status & (RX_EXTRA_DATA|RX_RUNT)))
-		/* per str 172 */
-		dev->stats.rx_crc_errors++;
-	if (status & RX_DRIBBLE)
-		dev->stats.rx_frame_errors++;
+	disable_irq(dev->irq);
+	net_interrupt(dev->irq, dev);
+	enable_irq(dev->irq);
 }
+#endif
 
-/* We have a good packet(s), get it/them out of the buffers. */
-static void
-net_rx(struct net_device *dev)
+static const struct net_device_ops net_ops = {
+	.ndo_open		= net_open,
+	.ndo_stop		= net_close,
+	.ndo_tx_timeout		= net_timeout,
+	.ndo_start_xmit		= net_send_packet,
+	.ndo_get_stats		= net_get_stats,
+	.ndo_set_rx_mode	= set_multicast_list,
+	.ndo_set_mac_address	= set_mac_address,
+#ifdef CONFIG_NET_POLL_CONTROLLER
+	.ndo_poll_controller	= net_poll_controller,
+#endif
+	.ndo_change_mtu		= eth_change_mtu,
+	.ndo_validate_addr	= eth_validate_addr,
+};
+
+static void __init reset_chip(struct net_device *dev)
 {
-	struct sk_buff *skb;
-	int status, length;
+#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;
 
-	int ioaddr = dev->base_addr;
-	status = readword(ioaddr, RX_FRAME_PORT);
-	length = readword(ioaddr, RX_FRAME_PORT);
+	writereg(dev, PP_SelfCTL, readreg(dev, PP_SelfCTL) | POWER_ON_RESET);
 
-	if ((status & RX_OK) == 0) {
-		count_rx_errors(status, dev);
-		return;
+	/* 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);
+		iowrite8(dev->irq, lp->virt_addr + DATA_PORT);
+		iowrite8(0, lp->virt_addr + DATA_PORT + 1);
+
+		iowrite16(PP_CS8920_ISAMemB, lp->virt_addr + ADD_PORT);
+		iowrite8((dev->mem_start >> 16) & 0xff,
+			 lp->virt_addr + DATA_PORT);
+		iowrite8((dev->mem_start >> 8) & 0xff,
+			 lp->virt_addr + DATA_PORT + 1);
 	}
+#endif /* CS89x0_NONISA_IRQ */
 
-	/* Malloc up new buffer. */
-	skb = netdev_alloc_skb(dev, length + 2);
-	if (skb == NULL) {
-#if 0		/* Again, this seems a cruel thing to do */
-		printk(KERN_WARNING "%s: Memory squeeze, dropping packet.\n", dev->name);
+	/* Wait until the chip is reset */
+	reset_start_time = jiffies;
+	while ((readreg(dev, PP_SelfST) & INIT_DONE) == 0 &&
+	       jiffies - reset_start_time < 2)
+		;
+#endif /* !CONFIG_MACH_MX31ADS */
+}
+
+/* This is the real probe routine.
+ * Linux has a history of friendly device probes on the ISA bus.
+ * A good device probes avoids doing writes, and
+ * verifies that the correct device exists and functions.
+ * Return 0 on success.
+ */
+static int __init
+cs89x0_probe1(struct net_device *dev, void __iomem *ioaddr, int modular)
+{
+	struct net_local *lp = netdev_priv(dev);
+	int i;
+	int tmp;
+	unsigned rev_type = 0;
+	int eeprom_buff[CHKSUM_LEN];
+	int retval;
+
+	/* Initialize the device structure. */
+	if (!modular) {
+		memset(lp, 0, sizeof(*lp));
+		spin_lock_init(&lp->lock);
+#ifndef MODULE
+#if ALLOW_DMA
+		if (g_cs89x0_dma) {
+			lp->use_dma = 1;
+			lp->dma = g_cs89x0_dma;
+			lp->dmasize = 16;	/* Could make this an option... */
+		}
+#endif
+		lp->force = g_cs89x0_media__force;
 #endif
-		dev->stats.rx_dropped++;
-		return;
 	}
-	skb_reserve(skb, 2);	/* longword align L3 header */
 
-	readwords(ioaddr, RX_FRAME_PORT, skb_put(skb, length), length >> 1);
-	if (length & 1)
-		skb->data[length-1] = readword(ioaddr, RX_FRAME_PORT);
+	pr_debug("PP_addr at %p[%x]: 0x%x\n",
+		 ioaddr, ADD_PORT, ioread16(ioaddr + ADD_PORT));
+	iowrite16(PP_ChipID, ioaddr + ADD_PORT);
 
-	if (net_debug > 3) {
-		printk(	"%s: received %d byte packet of type %x\n",
-			dev->name, length,
-			(skb->data[ETH_ALEN+ETH_ALEN] << 8) | skb->data[ETH_ALEN+ETH_ALEN+1]);
+	tmp = ioread16(ioaddr + DATA_PORT);
+	if (tmp != CHIP_EISA_ID_SIG) {
+		pr_debug("%s: incorrect signature at %p[%x]: 0x%x!="
+			 CHIP_EISA_ID_SIG_STR "\n",
+			 dev->name, ioaddr, DATA_PORT, tmp);
+		retval = -ENODEV;
+		goto out1;
 	}
 
-        skb->protocol=eth_type_trans(skb,dev);
-	netif_rx(skb);
-	dev->stats.rx_packets++;
-	dev->stats.rx_bytes += length;
-}
+	lp->virt_addr = ioaddr;
 
-#if ALLOW_DMA
-static void release_dma_buff(struct net_local *lp)
-{
-	if (lp->dma_buff) {
-		free_pages((unsigned long)(lp->dma_buff), get_order(lp->dmasize * 1024));
-		lp->dma_buff = NULL;
+	/* get the chip type */
+	rev_type = readreg(dev, PRODUCT_ID_ADD);
+	lp->chip_type = rev_type & ~REVISON_BITS;
+	lp->chip_revision = ((rev_type & REVISON_BITS) >> 8) + 'A';
+
+	/* Check the chip type and revision in order to set the correct
+	 * send command.  CS8920 revision C and CS8900 revision F can use
+	 * the faster send.
+	 */
+	lp->send_cmd = TX_AFTER_381;
+	if (lp->chip_type == CS8900 && lp->chip_revision >= 'F')
+		lp->send_cmd = TX_NOW;
+	if (lp->chip_type != CS8900 && lp->chip_revision >= 'C')
+		lp->send_cmd = TX_NOW;
+
+	pr_info_once("%s\n", version);
+
+	pr_info("%s: cs89%c0%s rev %c found at %p ",
+		dev->name,
+		lp->chip_type == CS8900  ? '0' : '2',
+		lp->chip_type == CS8920M ? "M" : "",
+		lp->chip_revision,
+		lp->virt_addr);
+
+	reset_chip(dev);
+
+	/* Here we read the current configuration of the chip.
+	 * If there is no Extended EEPROM then the idea is to not disturb
+	 * the chip configuration, it should have been correctly setup by
+	 * automatic EEPROM read on reset. So, if the chip says it read
+	 * the EEPROM the driver will always do *something* instead of
+	 * complain that adapter_cnf is 0.
+	 */
+
+	if ((readreg(dev, PP_SelfST) & (EEPROM_OK | EEPROM_PRESENT)) ==
+	    (EEPROM_OK | EEPROM_PRESENT)) {
+		/* Load the MAC. */
+		for (i = 0; i < ETH_ALEN / 2; i++) {
+			unsigned int Addr;
+			Addr = readreg(dev, PP_IA + i * 2);
+			dev->dev_addr[i * 2] = Addr & 0xFF;
+			dev->dev_addr[i * 2 + 1] = Addr >> 8;
+		}
+
+		/* Load the Adapter Configuration.
+		 * Note:  Barring any more specific information from some
+		 * other source (ie EEPROM+Schematics), we would not know
+		 * how to operate a 10Base2 interface on the AUI port.
+		 * However, since we  do read the status of HCB1 and use
+		 * settings that always result in calls to control_dc_dc(dev,0)
+		 * a BNC interface should work if the enable pin
+		 * (dc/dc converter) is on HCB1.
+		 * It will be called AUI however.
+		 */
+
+		lp->adapter_cnf = 0;
+		i = readreg(dev, PP_LineCTL);
+		/* Preserve the setting of the HCB1 pin. */
+		if ((i & (HCB1 | HCB1_ENBL)) == (HCB1 | HCB1_ENBL))
+			lp->adapter_cnf |= A_CNF_DC_DC_POLARITY;
+		/* Save the sqelch bit */
+		if ((i & LOW_RX_SQUELCH) == LOW_RX_SQUELCH)
+			lp->adapter_cnf |= A_CNF_EXTND_10B_2 | A_CNF_LOW_RX_SQUELCH;
+		/* Check if the card is in 10Base-t only mode */
+		if ((i & (AUI_ONLY | AUTO_AUI_10BASET)) == 0)
+			lp->adapter_cnf |=  A_CNF_10B_T | A_CNF_MEDIA_10B_T;
+		/* Check if the card is in AUI only mode */
+		if ((i & (AUI_ONLY | AUTO_AUI_10BASET)) == AUI_ONLY)
+			lp->adapter_cnf |=  A_CNF_AUI | A_CNF_MEDIA_AUI;
+		/* Check if the card is in Auto mode. */
+		if ((i & (AUI_ONLY | AUTO_AUI_10BASET)) == AUTO_AUI_10BASET)
+			lp->adapter_cnf |=  A_CNF_AUI | A_CNF_10B_T |
+				A_CNF_MEDIA_AUI | A_CNF_MEDIA_10B_T | A_CNF_MEDIA_AUTO;
+
+		cs89_dbg(1, info, "%s: PP_LineCTL=0x%x, adapter_cnf=0x%x\n",
+			 dev->name, i, lp->adapter_cnf);
+
+		/* IRQ. Other chips already probe, see below. */
+		if (lp->chip_type == CS8900)
+			lp->isa_config = readreg(dev, PP_CS8900_ISAINT) & INT_NO_MASK;
+
+		pr_cont("[Cirrus EEPROM] ");
 	}
-}
-#endif
 
-/* The inverse routine to net_open(). */
-static int
-net_close(struct net_device *dev)
-{
-#if ALLOW_DMA
-	struct net_local *lp = netdev_priv(dev);
-#endif
+	pr_cont("\n");
 
-	netif_stop_queue(dev);
+	/* First check to see if an EEPROM is attached. */
 
-	writereg(dev, PP_RxCFG, 0);
-	writereg(dev, PP_TxCFG, 0);
-	writereg(dev, PP_BufCFG, 0);
-	writereg(dev, PP_BusCTL, 0);
+	if ((readreg(dev, PP_SelfST) & EEPROM_PRESENT) == 0)
+		pr_warn("No EEPROM, relying on command line....\n");
+	else if (get_eeprom_data(dev, START_EEPROM_DATA, CHKSUM_LEN, eeprom_buff) < 0) {
+		pr_warn("EEPROM read failed, relying on command line\n");
+	} else if (get_eeprom_cksum(START_EEPROM_DATA, CHKSUM_LEN, eeprom_buff) < 0) {
+		/* Check if the chip was able to read its own configuration starting
+		   at 0 in the EEPROM*/
+		if ((readreg(dev, PP_SelfST) & (EEPROM_OK | EEPROM_PRESENT)) !=
+		    (EEPROM_OK | EEPROM_PRESENT))
+			pr_warn("Extended EEPROM checksum bad and no Cirrus EEPROM, relying on command line\n");
 
-	free_irq(dev->irq, dev);
+	} else {
+		/* This reads an extended EEPROM that is not documented
+		 * in the CS8900 datasheet.
+		 */
 
-#if ALLOW_DMA
-	if (lp->use_dma && lp->dma) {
-		free_dma(dev->dma);
-		release_dma_buff(lp);
+		/* get transmission control word  but keep the autonegotiation bits */
+		if (!lp->auto_neg_cnf)
+			lp->auto_neg_cnf = eeprom_buff[AUTO_NEG_CNF_OFFSET / 2];
+		/* Store adapter configuration */
+		if (!lp->adapter_cnf)
+			lp->adapter_cnf = eeprom_buff[ADAPTER_CNF_OFFSET / 2];
+		/* Store ISA configuration */
+		lp->isa_config = eeprom_buff[ISA_CNF_OFFSET / 2];
+		dev->mem_start = eeprom_buff[PACKET_PAGE_OFFSET / 2] << 8;
+
+		/* eeprom_buff has 32-bit ints, so we can't just memcpy it */
+		/* store the initial memory base address */
+		for (i = 0; i < ETH_ALEN / 2; i++) {
+			dev->dev_addr[i * 2] = eeprom_buff[i];
+			dev->dev_addr[i * 2 + 1] = eeprom_buff[i] >> 8;
+		}
+		cs89_dbg(1, debug, "%s: new adapter_cnf: 0x%x\n",
+			 dev->name, lp->adapter_cnf);
 	}
+
+	/* allow them to force multiple transceivers.  If they force multiple, autosense */
+	{
+		int count = 0;
+		if (lp->force & FORCE_RJ45) {
+			lp->adapter_cnf |= A_CNF_10B_T;
+			count++;
+		}
+		if (lp->force & FORCE_AUI) {
+			lp->adapter_cnf |= A_CNF_AUI;
+			count++;
+		}
+		if (lp->force & FORCE_BNC) {
+			lp->adapter_cnf |= A_CNF_10B_2;
+			count++;
+		}
+		if (count > 1)
+			lp->adapter_cnf |= A_CNF_MEDIA_AUTO;
+		else if (lp->force & FORCE_RJ45)
+			lp->adapter_cnf |= A_CNF_MEDIA_10B_T;
+		else if (lp->force & FORCE_AUI)
+			lp->adapter_cnf |= A_CNF_MEDIA_AUI;
+		else if (lp->force & FORCE_BNC)
+			lp->adapter_cnf |= A_CNF_MEDIA_10B_2;
+	}
+
+	cs89_dbg(1, debug, "%s: after force 0x%x, adapter_cnf=0x%x\n",
+		 dev->name, lp->force, lp->adapter_cnf);
+
+	/* FIXME: We don't let you set dc-dc polarity or low RX squelch from the command line: add it here */
+
+	/* FIXME: We don't let you set the IMM bit from the command line: add it to lp->auto_neg_cnf here */
+
+	/* FIXME: we don't set the Ethernet address on the command line.  Use
+	 * ifconfig IFACE hw ether AABBCCDDEEFF
+	 */
+
+	pr_info("media %s%s%s",
+		(lp->adapter_cnf & A_CNF_10B_T) ? "RJ-45," : "",
+		(lp->adapter_cnf & A_CNF_AUI) ? "AUI," : "",
+		(lp->adapter_cnf & A_CNF_10B_2) ? "BNC," : "");
+
+	lp->irq_map = 0xffff;
+
+	/* If this is a CS8900 then no pnp soft */
+	if (lp->chip_type != CS8900 &&
+	    /* Check if the ISA IRQ has been set  */
+	    (i = readreg(dev, PP_CS8920_ISAINT) & 0xff,
+	     (i != 0 && i < CS8920_NO_INTS))) {
+		if (!dev->irq)
+			dev->irq = i;
+	} else {
+		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);
+			else
+				i = cs8900_irq_map[i];
+
+			lp->irq_map = CS8900_IRQ_MAP; /* fixed IRQ map for CS8900 */
+		} else {
+			int irq_map_buff[IRQ_MAP_LEN/2];
+
+			if (get_eeprom_data(dev, IRQ_MAP_EEPROM_DATA,
+					    IRQ_MAP_LEN / 2,
+					    irq_map_buff) >= 0) {
+				if ((irq_map_buff[0] & 0xff) == PNP_IRQ_FRMT)
+					lp->irq_map = ((irq_map_buff[0] >> 8) |
+						       (irq_map_buff[1] << 8));
+			}
 #endif
+		}
+#endif
+		if (!dev->irq)
+			dev->irq = i;
+	}
 
-	/* Update the statistics here. */
-	return 0;
-}
+	pr_cont(" IRQ %d", dev->irq);
 
-/* Get the current statistics.	This may be called with the card open or
-   closed. */
-static struct net_device_stats *
-net_get_stats(struct net_device *dev)
-{
-	struct net_local *lp = netdev_priv(dev);
-	unsigned long flags;
+#if ALLOW_DMA
+	if (lp->use_dma) {
+		get_dma_channel(dev);
+		pr_cont(", DMA %d", dev->dma);
+	} else
+#endif
+		pr_cont(", programmed I/O");
 
-	spin_lock_irqsave(&lp->lock, flags);
-	/* Update the statistics from the device registers. */
-	dev->stats.rx_missed_errors += (readreg(dev, PP_RxMiss) >> 6);
-	dev->stats.collisions += (readreg(dev, PP_TxCol) >> 6);
-	spin_unlock_irqrestore(&lp->lock, flags);
+	/* print the ethernet address. */
+	pr_cont(", MAC %pM\n", dev->dev_addr);
 
-	return &dev->stats;
+	dev->netdev_ops	= &net_ops;
+	dev->watchdog_timeo = HZ;
+
+	cs89_dbg(0, info, "cs89x0_probe1() successful\n");
+
+	retval = register_netdev(dev);
+	if (retval)
+		goto out2;
+	return 0;
+out2:
+	iowrite16(PP_ChipID, lp->virt_addr + ADD_PORT);
+out1:
+	return retval;
 }
 
-static void set_multicast_list(struct net_device *dev)
+#ifndef CONFIG_CS89x0_PLATFORM
+/*
+ * This function converts the I/O port addres used by the cs89x0_probe() and
+ * init_module() functions to the I/O memory address used by the
+ * cs89x0_probe1() function.
+ */
+static int __init
+cs89x0_ioport_probe(struct net_device *dev, unsigned long ioport, int modular)
 {
 	struct net_local *lp = netdev_priv(dev);
-	unsigned long flags;
+	int ret;
+	void __iomem *io_mem;
 
-	spin_lock_irqsave(&lp->lock, flags);
-	if(dev->flags&IFF_PROMISC)
-	{
-		lp->rx_mode = RX_ALL_ACCEPT;
+	if (!lp)
+		return -ENOMEM;
+
+	dev->base_addr = ioport;
+
+	if (!request_region(ioport, NETCARD_IO_EXTENT, DRV_NAME)) {
+		ret = -EBUSY;
+		goto out;
 	}
-	else if ((dev->flags & IFF_ALLMULTI) || !netdev_mc_empty(dev))
-	{
-		/* The multicast-accept list is initialized to accept-all, and we
-		   rely on higher-level filtering for now. */
-		lp->rx_mode = RX_MULTCAST_ACCEPT;
+
+	io_mem = ioport_map(ioport & ~3, NETCARD_IO_EXTENT);
+	if (!io_mem) {
+		ret = -ENOMEM;
+		goto release;
 	}
-	else
-		lp->rx_mode = 0;
 
-	writereg(dev, PP_RxCTL, DEF_RX_ACCEPT | lp->rx_mode);
+	/* if they give us an odd I/O address, then do ONE write to
+	 * the address port, to get it back to address zero, where we
+	 * expect to find the EISA signature word. An IO with a base of 0x3
+	 * will skip the test for the ADD_PORT.
+	 */
+	if (ioport & 1) {
+		cs89_dbg(1, info, "%s: odd ioaddr 0x%lx\n", dev->name, ioport);
+		if ((ioport & 2) != 2) {
+			if ((ioread16(io_mem + ADD_PORT) & ADD_MASK) !=
+			    ADD_SIG) {
+				pr_err("%s: bad signature 0x%x\n",
+				       dev->name, ioread16(io_mem + ADD_PORT));
+				ret = -ENODEV;
+				goto unmap;
+			}
+		}
+	}
 
-	/* in promiscuous mode, we accept errored packets, so we have to enable interrupts on them also */
-	writereg(dev, PP_RxCFG, lp->curr_rx_cfg |
-	     (lp->rx_mode == RX_ALL_ACCEPT? (RX_CRC_ERROR_ENBL|RX_RUNT_ENBL|RX_EXTRA_DATA_ENBL) : 0));
-	spin_unlock_irqrestore(&lp->lock, flags);
+	ret = cs89x0_probe1(dev, io_mem, modular);
+	if (!ret)
+		goto out;
+unmap:
+	ioport_unmap(io_mem);
+release:
+	release_region(ioport, NETCARD_IO_EXTENT);
+out:
+	return ret;
 }
 
+#ifndef MODULE
+/* Check for a network adaptor of this type, and return '0' iff one exists.
+ * If dev->base_addr == 0, probe all likely locations.
+ * If dev->base_addr == 1, always return failure.
+ * If dev->base_addr == 2, allocate space for the device and return success
+ * (detachable devices only).
+ * Return 0 on success.
+ */
 
-static int set_mac_address(struct net_device *dev, void *p)
+struct net_device * __init cs89x0_probe(int unit)
 {
-	int i;
-	struct sockaddr *addr = p;
-
-	if (netif_running(dev))
-		return -EBUSY;
+	struct net_device *dev = alloc_etherdev(sizeof(struct net_local));
+	unsigned *port;
+	int err = 0;
+	int irq;
+	int io;
 
-	memcpy(dev->dev_addr, addr->sa_data, dev->addr_len);
+	if (!dev)
+		return ERR_PTR(-ENODEV);
 
-	if (net_debug)
-		printk("%s: Setting MAC address to %pM.\n",
-		       dev->name, dev->dev_addr);
+	sprintf(dev->name, "eth%d", unit);
+	netdev_boot_setup_check(dev);
+	io = dev->base_addr;
+	irq = dev->irq;
 
-	/* set the Ethernet address */
-	for (i=0; i < ETH_ALEN/2; i++)
-		writereg(dev, PP_IA+i*2, dev->dev_addr[i*2] | (dev->dev_addr[i*2+1] << 8));
+	cs89_dbg(0, info, "cs89x0_probe(0x%x)\n", io);
 
-	return 0;
+	if (io > 0x1ff)	{	/* Check a single specified location. */
+		err = cs89x0_ioport_probe(dev, io, 0);
+	} else if (io != 0) {	/* Don't probe at all. */
+		err = -ENXIO;
+	} else {
+		for (port = netcard_portlist; *port; port++) {
+			if (cs89x0_ioport_probe(dev, *port, 0) == 0)
+				break;
+			dev->irq = irq;
+		}
+		if (!*port)
+			err = -ENODEV;
+	}
+	if (err)
+		goto out;
+	return dev;
+out:
+	free_netdev(dev);
+	pr_warn("no cs8900 or cs8920 detected.  Be sure to disable PnP with SETUP\n");
+	return ERR_PTR(err);
 }
+#endif
+#endif
 
 #if defined(MODULE) && !defined(CONFIG_CS89x0_PLATFORM)
 
 static struct net_device *dev_cs89x0;
 
-/*
- * Support the 'debug' module parm even if we're compiled for non-debug to
+/* Support the 'debug' module parm even if we're compiled for non-debug to
  * avoid breaking someone's startup scripts
  */
 
@@ -1764,11 +1755,11 @@ static int io;
 static int irq;
 static int debug;
 static char media[8];
-static int duplex=-1;
+static int duplex = -1;
 
 static int use_dma;			/* These generate unused var warnings if ALLOW_DMA = 0 */
 static int dma;
-static int dmasize=16;			/* or 64 */
+static int dmasize = 16;		/* or 64 */
 
 module_param(io, int, 0);
 module_param(irq, int, 0);
@@ -1801,32 +1792,28 @@ MODULE_PARM_DESC(use_dma , "(ignored)");
 MODULE_AUTHOR("Mike Cruse, Russwll Nelson <nelson@crynwr.com>, Andrew Morton");
 MODULE_LICENSE("GPL");
 
-
 /*
-* media=t             - specify media type
-   or media=2
-   or media=aui
-   or medai=auto
-* duplex=0            - specify forced half/full/autonegotiate duplex
-* debug=#             - debug level
-
-
-* Default Chip Configuration:
-  * DMA Burst = enabled
-  * IOCHRDY Enabled = enabled
-    * UseSA = enabled
-    * CS8900 defaults to half-duplex if not specified on command-line
-    * CS8920 defaults to autoneg if not specified on command-line
-    * Use reset defaults for other config parameters
-
-* Assumptions:
-  * media type specified is supported (circuitry is present)
-  * if memory address is > 1MB, then required mem decode hw is present
-  * if 10B-2, then agent other than driver will enable DC/DC converter
-    (hw or software util)
-
-
-*/
+ * media=t             - specify media type
+ * or media=2
+ * or media=aui
+ * or medai=auto
+ * duplex=0            - specify forced half/full/autonegotiate duplex
+ * debug=#             - debug level
+ *
+ * Default Chip Configuration:
+ * DMA Burst = enabled
+ * IOCHRDY Enabled = enabled
+ * UseSA = enabled
+ * CS8900 defaults to half-duplex if not specified on command-line
+ * CS8920 defaults to autoneg if not specified on command-line
+ * Use reset defaults for other config parameters
+ *
+ * Assumptions:
+ * media type specified is supported (circuitry is present)
+ * if memory address is > 1MB, then required mem decode hw is present
+ * if 10B-2, then agent other than driver will enable DC/DC converter
+ * (hw or software util)
+ */
 
 int __init init_module(void)
 {
@@ -1856,8 +1843,8 @@ int __init init_module(void)
 
 	spin_lock_init(&lp->lock);
 
-        /* boy, they'd better get these right */
-        if (!strcmp(media, "rj45"))
+	/* boy, they'd better get these right */
+	if (!strcmp(media, "rj45"))
 		lp->adapter_cnf = A_CNF_MEDIA_10B_T | A_CNF_10B_T;
 	else if (!strcmp(media, "aui"))
 		lp->adapter_cnf = A_CNF_MEDIA_AUI   | A_CNF_AUI;
@@ -1866,27 +1853,28 @@ int __init init_module(void)
 	else
 		lp->adapter_cnf = A_CNF_MEDIA_10B_T | A_CNF_10B_T;
 
-        if (duplex==-1)
+	if (duplex == -1)
 		lp->auto_neg_cnf = AUTO_NEG_ENABLE;
 
-        if (io == 0) {
-                printk(KERN_ERR "cs89x0.c: Module autoprobing not allowed.\n");
-                printk(KERN_ERR "cs89x0.c: Append io=0xNNN\n");
-                ret = -EPERM;
+	if (io == 0) {
+		pr_err("Module autoprobing not allowed\n");
+		pr_err("Append io=0xNNN\n");
+		ret = -EPERM;
 		goto out;
-        } else if (io <= 0x1ff) {
+	} else if (io <= 0x1ff) {
 		ret = -ENXIO;
 		goto out;
 	}
 
 #if ALLOW_DMA
 	if (use_dma && dmasize != 16 && dmasize != 64) {
-		printk(KERN_ERR "cs89x0.c: dma size must be either 16K or 64K, not %dK\n", dmasize);
+		pr_err("dma size must be either 16K or 64K, not %dK\n",
+		       dmasize);
 		ret = -EPERM;
 		goto out;
 	}
 #endif
-	ret = cs89x0_probe1(dev, io, 1);
+	ret = cs89x0_ioport_probe(dev, io, 1);
 	if (ret)
 		goto out;
 
@@ -1900,8 +1888,11 @@ out:
 void __exit
 cleanup_module(void)
 {
+	struct net_local *lp = netdev_priv(dev_cs89x0);
+
 	unregister_netdev(dev_cs89x0);
-	writeword(dev_cs89x0->base_addr, ADD_PORT, PP_ChipID);
+	iowrite16(PP_ChipID, lp->virt_addr + ADD_PORT);
+	ioport_unmap(lp->virt_addr);
 	release_region(dev_cs89x0->base_addr, NETCARD_IO_EXTENT);
 	free_netdev(dev_cs89x0);
 }
@@ -1913,6 +1904,7 @@ static int __init cs89x0_platform_probe(struct platform_device *pdev)
 	struct net_device *dev = alloc_etherdev(sizeof(struct net_local));
 	struct net_local *lp;
 	struct resource *mem_res;
+	void __iomem *virt_addr;
 	int err;
 
 	if (!dev)
@@ -1923,29 +1915,28 @@ static int __init cs89x0_platform_probe(struct platform_device *pdev)
 	mem_res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
 	dev->irq = platform_get_irq(pdev, 0);
 	if (mem_res == NULL || dev->irq <= 0) {
-		dev_warn(&dev->dev, "memory/interrupt resource missing.\n");
+		dev_warn(&dev->dev, "memory/interrupt resource missing\n");
 		err = -ENXIO;
 		goto free;
 	}
 
-	lp->phys_addr = mem_res->start;
 	lp->size = resource_size(mem_res);
-	if (!request_mem_region(lp->phys_addr, lp->size, DRV_NAME)) {
-		dev_warn(&dev->dev, "request_mem_region() failed.\n");
+	if (!request_mem_region(mem_res->start, lp->size, DRV_NAME)) {
+		dev_warn(&dev->dev, "request_mem_region() failed\n");
 		err = -EBUSY;
 		goto free;
 	}
 
-	lp->virt_addr = ioremap(lp->phys_addr, lp->size);
-	if (!lp->virt_addr) {
-		dev_warn(&dev->dev, "ioremap() failed.\n");
+	virt_addr = ioremap(mem_res->start, lp->size);
+	if (!virt_addr) {
+		dev_warn(&dev->dev, "ioremap() failed\n");
 		err = -ENOMEM;
 		goto release;
 	}
 
-	err = cs89x0_probe1(dev, (unsigned long)lp->virt_addr, 0);
+	err = cs89x0_probe1(dev, virt_addr, 0);
 	if (err) {
-		dev_warn(&dev->dev, "no cs8900 or cs8920 detected.\n");
+		dev_warn(&dev->dev, "no cs8900 or cs8920 detected\n");
 		goto unmap;
 	}
 
@@ -1953,9 +1944,9 @@ static int __init cs89x0_platform_probe(struct platform_device *pdev)
 	return 0;
 
 unmap:
-	iounmap(lp->virt_addr);
+	iounmap(virt_addr);
 release:
-	release_mem_region(lp->phys_addr, lp->size);
+	release_mem_region(mem_res->start, lp->size);
 free:
 	free_netdev(dev);
 	return err;
@@ -1965,10 +1956,16 @@ static int cs89x0_platform_remove(struct platform_device *pdev)
 {
 	struct net_device *dev = platform_get_drvdata(pdev);
 	struct net_local *lp = netdev_priv(dev);
+	struct resource *mem_res;
 
+	/* This platform_get_resource() call will not return NULL, because
+	 * the same call in cs89x0_platform_probe() has returned a non NULL
+	 * value.
+	 */
+	mem_res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
 	unregister_netdev(dev);
 	iounmap(lp->virt_addr);
-	release_mem_region(lp->phys_addr, lp->size);
+	release_mem_region(mem_res->start, lp->size);
 	free_netdev(dev);
 	return 0;
 }
@@ -1996,13 +1993,3 @@ static void __exit cs89x0_cleanup(void)
 module_exit(cs89x0_cleanup);
 
 #endif /* CONFIG_CS89x0_PLATFORM */
-
-/*
- * Local variables:
- *  version-control: t
- *  kept-new-versions: 5
- *  c-indent-level: 8
- *  tab-width: 8
- * End:
- *
- */
diff --git a/drivers/net/ethernet/cisco/enic/enic_main.c b/drivers/net/ethernet/cisco/enic/enic_main.c
index 77b4e873f91..8132c785cea 100644
--- a/drivers/net/ethernet/cisco/enic/enic_main.c
+++ b/drivers/net/ethernet/cisco/enic/enic_main.c
@@ -944,8 +944,7 @@ static void enic_update_multicast_addr_list(struct enic *enic)
 
 	for (i = 0; i < enic->mc_count; i++) {
 		for (j = 0; j < mc_count; j++)
-			if (compare_ether_addr(enic->mc_addr[i],
-				mc_addr[j]) == 0)
+			if (ether_addr_equal(enic->mc_addr[i], mc_addr[j]))
 				break;
 		if (j == mc_count)
 			enic_dev_del_addr(enic, enic->mc_addr[i]);
@@ -953,8 +952,7 @@ static void enic_update_multicast_addr_list(struct enic *enic)
 
 	for (i = 0; i < mc_count; i++) {
 		for (j = 0; j < enic->mc_count; j++)
-			if (compare_ether_addr(mc_addr[i],
-				enic->mc_addr[j]) == 0)
+			if (ether_addr_equal(mc_addr[i], enic->mc_addr[j]))
 				break;
 		if (j == enic->mc_count)
 			enic_dev_add_addr(enic, mc_addr[i]);
@@ -999,8 +997,7 @@ static void enic_update_unicast_addr_list(struct enic *enic)
 
 	for (i = 0; i < enic->uc_count; i++) {
 		for (j = 0; j < uc_count; j++)
-			if (compare_ether_addr(enic->uc_addr[i],
-				uc_addr[j]) == 0)
+			if (ether_addr_equal(enic->uc_addr[i], uc_addr[j]))
 				break;
 		if (j == uc_count)
 			enic_dev_del_addr(enic, enic->uc_addr[i]);
@@ -1008,8 +1005,7 @@ static void enic_update_unicast_addr_list(struct enic *enic)
 
 	for (i = 0; i < uc_count; i++) {
 		for (j = 0; j < enic->uc_count; j++)
-			if (compare_ether_addr(uc_addr[i],
-				enic->uc_addr[j]) == 0)
+			if (ether_addr_equal(uc_addr[i], enic->uc_addr[j]))
 				break;
 		if (j == enic->uc_count)
 			enic_dev_add_addr(enic, uc_addr[i]);
@@ -1193,18 +1189,16 @@ static int enic_get_vf_port(struct net_device *netdev, int vf,
 	if (err)
 		return err;
 
-	NLA_PUT_U16(skb, IFLA_PORT_REQUEST, pp->request);
-	NLA_PUT_U16(skb, IFLA_PORT_RESPONSE, response);
-	if (pp->set & ENIC_SET_NAME)
-		NLA_PUT(skb, IFLA_PORT_PROFILE, PORT_PROFILE_MAX,
-			pp->name);
-	if (pp->set & ENIC_SET_INSTANCE)
-		NLA_PUT(skb, IFLA_PORT_INSTANCE_UUID, PORT_UUID_MAX,
-			pp->instance_uuid);
-	if (pp->set & ENIC_SET_HOST)
-		NLA_PUT(skb, IFLA_PORT_HOST_UUID, PORT_UUID_MAX,
-			pp->host_uuid);
-
+	if (nla_put_u16(skb, IFLA_PORT_REQUEST, pp->request) ||
+	    nla_put_u16(skb, IFLA_PORT_RESPONSE, response) ||
+	    ((pp->set & ENIC_SET_NAME) &&
+	     nla_put(skb, IFLA_PORT_PROFILE, PORT_PROFILE_MAX, pp->name)) ||
+	    ((pp->set & ENIC_SET_INSTANCE) &&
+	     nla_put(skb, IFLA_PORT_INSTANCE_UUID, PORT_UUID_MAX,
+		     pp->instance_uuid)) ||
+	    ((pp->set & ENIC_SET_HOST) &&
+	     nla_put(skb, IFLA_PORT_HOST_UUID, PORT_UUID_MAX, pp->host_uuid)))
+		goto nla_put_failure;
 	return 0;
 
 nla_put_failure:
diff --git a/drivers/net/ethernet/cisco/enic/enic_pp.c b/drivers/net/ethernet/cisco/enic/enic_pp.c
index dafea1ecb7b..43464f0a4f9 100644
--- a/drivers/net/ethernet/cisco/enic/enic_pp.c
+++ b/drivers/net/ethernet/cisco/enic/enic_pp.c
@@ -184,7 +184,7 @@ static int (*enic_pp_handlers[])(struct enic *enic, int vf,
 };
 
 static const int enic_pp_handlers_count =
-			sizeof(enic_pp_handlers)/sizeof(*enic_pp_handlers);
+			ARRAY_SIZE(enic_pp_handlers);
 
 static int enic_pp_preassociate(struct enic *enic, int vf,
 	struct enic_port_profile *prev_pp, int *restore_pp)
diff --git a/drivers/net/ethernet/davicom/Kconfig b/drivers/net/ethernet/davicom/Kconfig
index 972b62b3183..9745fe5e803 100644
--- a/drivers/net/ethernet/davicom/Kconfig
+++ b/drivers/net/ethernet/davicom/Kconfig
@@ -4,7 +4,7 @@
 
 config DM9000
 	tristate "DM9000 support"
-	depends on ARM || BLACKFIN || MIPS
+	depends on ARM || BLACKFIN || MIPS || COLDFIRE
 	select CRC32
 	select NET_CORE
 	select MII
diff --git a/drivers/net/ethernet/dec/ewrk3.c b/drivers/net/ethernet/dec/ewrk3.c
index 1879f84a25a..17ae8c61968 100644
--- a/drivers/net/ethernet/dec/ewrk3.c
+++ b/drivers/net/ethernet/dec/ewrk3.c
@@ -1016,7 +1016,8 @@ static int ewrk3_rx(struct net_device *dev)
 							} else {
 								lp->pktStats.multicast++;
 							}
-						} else if (compare_ether_addr(p, dev->dev_addr) == 0) {
+						} else if (ether_addr_equal(p,
+									    dev->dev_addr)) {
 							lp->pktStats.unicast++;
 						}
 						lp->pktStats.bins[0]++;		/* Duplicates stats.rx_packets */
diff --git a/drivers/net/ethernet/dec/tulip/de2104x.c b/drivers/net/ethernet/dec/tulip/de2104x.c
index 68f1c39184d..61cc0934286 100644
--- a/drivers/net/ethernet/dec/tulip/de2104x.c
+++ b/drivers/net/ethernet/dec/tulip/de2104x.c
@@ -1380,6 +1380,7 @@ static void de_free_rings (struct de_private *de)
 static int de_open (struct net_device *dev)
 {
 	struct de_private *de = netdev_priv(dev);
+	const int irq = de->pdev->irq;
 	int rc;
 
 	netif_dbg(de, ifup, dev, "enabling interface\n");
@@ -1394,10 +1395,9 @@ static int de_open (struct net_device *dev)
 
 	dw32(IntrMask, 0);
 
-	rc = request_irq(dev->irq, de_interrupt, IRQF_SHARED, dev->name, dev);
+	rc = request_irq(irq, de_interrupt, IRQF_SHARED, dev->name, dev);
 	if (rc) {
-		netdev_err(dev, "IRQ %d request failure, err=%d\n",
-			   dev->irq, rc);
+		netdev_err(dev, "IRQ %d request failure, err=%d\n", irq, rc);
 		goto err_out_free;
 	}
 
@@ -1413,7 +1413,7 @@ static int de_open (struct net_device *dev)
 	return 0;
 
 err_out_free_irq:
-	free_irq(dev->irq, dev);
+	free_irq(irq, dev);
 err_out_free:
 	de_free_rings(de);
 	return rc;
@@ -1434,7 +1434,7 @@ static int de_close (struct net_device *dev)
 	netif_carrier_off(dev);
 	spin_unlock_irqrestore(&de->lock, flags);
 
-	free_irq(dev->irq, dev);
+	free_irq(de->pdev->irq, dev);
 
 	de_free_rings(de);
 	de_adapter_sleep(de);
@@ -1444,6 +1444,7 @@ static int de_close (struct net_device *dev)
 static void de_tx_timeout (struct net_device *dev)
 {
 	struct de_private *de = netdev_priv(dev);
+	const int irq = de->pdev->irq;
 
 	netdev_dbg(dev, "NIC status %08x mode %08x sia %08x desc %u/%u/%u\n",
 		   dr32(MacStatus), dr32(MacMode), dr32(SIAStatus),
@@ -1451,7 +1452,7 @@ static void de_tx_timeout (struct net_device *dev)
 
 	del_timer_sync(&de->media_timer);
 
-	disable_irq(dev->irq);
+	disable_irq(irq);
 	spin_lock_irq(&de->lock);
 
 	de_stop_hw(de);
@@ -1459,12 +1460,12 @@ static void de_tx_timeout (struct net_device *dev)
 	netif_carrier_off(dev);
 
 	spin_unlock_irq(&de->lock);
-	enable_irq(dev->irq);
+	enable_irq(irq);
 
 	/* Update the error counts. */
 	__de_get_stats(de);
 
-	synchronize_irq(dev->irq);
+	synchronize_irq(irq);
 	de_clean_rings(de);
 
 	de_init_rings(de);
@@ -2024,8 +2025,6 @@ static int __devinit de_init_one (struct pci_dev *pdev,
 		goto err_out_res;
 	}
 
-	dev->irq = pdev->irq;
-
 	/* obtain and check validity of PCI I/O address */
 	pciaddr = pci_resource_start(pdev, 1);
 	if (!pciaddr) {
@@ -2050,7 +2049,6 @@ static int __devinit de_init_one (struct pci_dev *pdev,
 		       pciaddr, pci_name(pdev));
 		goto err_out_res;
 	}
-	dev->base_addr = (unsigned long) regs;
 	de->regs = regs;
 
 	de_adapter_wake(de);
@@ -2078,11 +2076,9 @@ static int __devinit de_init_one (struct pci_dev *pdev,
 		goto err_out_iomap;
 
 	/* print info about board and interface just registered */
-	netdev_info(dev, "%s at 0x%lx, %pM, IRQ %d\n",
+	netdev_info(dev, "%s at %p, %pM, IRQ %d\n",
 		    de->de21040 ? "21040" : "21041",
-		    dev->base_addr,
-		    dev->dev_addr,
-		    dev->irq);
+		    regs, dev->dev_addr, pdev->irq);
 
 	pci_set_drvdata(pdev, dev);
 
@@ -2130,9 +2126,11 @@ static int de_suspend (struct pci_dev *pdev, pm_message_t state)
 
 	rtnl_lock();
 	if (netif_running (dev)) {
+		const int irq = pdev->irq;
+
 		del_timer_sync(&de->media_timer);
 
-		disable_irq(dev->irq);
+		disable_irq(irq);
 		spin_lock_irq(&de->lock);
 
 		de_stop_hw(de);
@@ -2141,12 +2139,12 @@ static int de_suspend (struct pci_dev *pdev, pm_message_t state)
 		netif_carrier_off(dev);
 
 		spin_unlock_irq(&de->lock);
-		enable_irq(dev->irq);
+		enable_irq(irq);
 
 		/* Update the error counts. */
 		__de_get_stats(de);
 
-		synchronize_irq(dev->irq);
+		synchronize_irq(irq);
 		de_clean_rings(de);
 
 		de_adapter_sleep(de);
diff --git a/drivers/net/ethernet/dec/tulip/de4x5.c b/drivers/net/ethernet/dec/tulip/de4x5.c
index 18b106cc6d2..d3cd489d11a 100644
--- a/drivers/net/ethernet/dec/tulip/de4x5.c
+++ b/drivers/net/ethernet/dec/tulip/de4x5.c
@@ -1874,7 +1874,7 @@ de4x5_local_stats(struct net_device *dev, char *buf, int pkt_len)
 	} else {
 	    lp->pktStats.multicast++;
 	}
-    } else if (compare_ether_addr(buf, dev->dev_addr) == 0) {
+    } else if (ether_addr_equal(buf, dev->dev_addr)) {
         lp->pktStats.unicast++;
     }
 
diff --git a/drivers/net/ethernet/dec/tulip/dmfe.c b/drivers/net/ethernet/dec/tulip/dmfe.c
index 1eccf494548..4d6fe604fa6 100644
--- a/drivers/net/ethernet/dec/tulip/dmfe.c
+++ b/drivers/net/ethernet/dec/tulip/dmfe.c
@@ -150,6 +150,12 @@
 #define DMFE_TX_TIMEOUT ((3*HZ)/2)	/* tx packet time-out time 1.5 s" */
 #define DMFE_TX_KICK 	(HZ/2)	/* tx packet Kick-out time 0.5 s" */
 
+#define dw32(reg, val)	iowrite32(val, ioaddr + (reg))
+#define dw16(reg, val)	iowrite16(val, ioaddr + (reg))
+#define dr32(reg)	ioread32(ioaddr + (reg))
+#define dr16(reg)	ioread16(ioaddr + (reg))
+#define dr8(reg)	ioread8(ioaddr + (reg))
+
 #define DMFE_DBUG(dbug_now, msg, value)			\
 	do {						\
 		if (dmfe_debug || (dbug_now))		\
@@ -178,14 +184,6 @@
 
 #define SROM_V41_CODE   0x14
 
-#define SROM_CLK_WRITE(data, ioaddr) \
-	outl(data|CR9_SROM_READ|CR9_SRCS,ioaddr); \
-	udelay(5); \
-	outl(data|CR9_SROM_READ|CR9_SRCS|CR9_SRCLK,ioaddr); \
-	udelay(5); \
-	outl(data|CR9_SROM_READ|CR9_SRCS,ioaddr); \
-	udelay(5);
-
 #define __CHK_IO_SIZE(pci_id, dev_rev) \
  (( ((pci_id)==PCI_DM9132_ID) || ((dev_rev) >= 0x30) ) ? \
 	DM9102A_IO_SIZE: DM9102_IO_SIZE)
@@ -213,11 +211,11 @@ struct rx_desc {
 struct dmfe_board_info {
 	u32 chip_id;			/* Chip vendor/Device ID */
 	u8 chip_revision;		/* Chip revision */
-	struct DEVICE *next_dev;	/* next device */
+	struct net_device *next_dev;	/* next device */
 	struct pci_dev *pdev;		/* PCI device */
 	spinlock_t lock;
 
-	long ioaddr;			/* I/O base address */
+	void __iomem *ioaddr;		/* I/O base address */
 	u32 cr0_data;
 	u32 cr5_data;
 	u32 cr6_data;
@@ -320,20 +318,20 @@ static netdev_tx_t dmfe_start_xmit(struct sk_buff *, struct DEVICE *);
 static int dmfe_stop(struct DEVICE *);
 static void dmfe_set_filter_mode(struct DEVICE *);
 static const struct ethtool_ops netdev_ethtool_ops;
-static u16 read_srom_word(long ,int);
+static u16 read_srom_word(void __iomem *, int);
 static irqreturn_t dmfe_interrupt(int , void *);
 #ifdef CONFIG_NET_POLL_CONTROLLER
 static void poll_dmfe (struct net_device *dev);
 #endif
-static void dmfe_descriptor_init(struct net_device *, unsigned long);
+static void dmfe_descriptor_init(struct net_device *);
 static void allocate_rx_buffer(struct net_device *);
-static void update_cr6(u32, unsigned long);
+static void update_cr6(u32, void __iomem *);
 static void send_filter_frame(struct DEVICE *);
 static void dm9132_id_table(struct DEVICE *);
-static u16 phy_read(unsigned long, u8, u8, u32);
-static void phy_write(unsigned long, u8, u8, u16, u32);
-static void phy_write_1bit(unsigned long, u32);
-static u16 phy_read_1bit(unsigned long);
+static u16 phy_read(void __iomem *, u8, u8, u32);
+static void phy_write(void __iomem *, u8, u8, u16, u32);
+static void phy_write_1bit(void __iomem *, u32);
+static u16 phy_read_1bit(void __iomem *);
 static u8 dmfe_sense_speed(struct dmfe_board_info *);
 static void dmfe_process_mode(struct dmfe_board_info *);
 static void dmfe_timer(unsigned long);
@@ -462,14 +460,16 @@ static int __devinit dmfe_init_one (struct pci_dev *pdev,
 	db->buf_pool_dma_start = db->buf_pool_dma_ptr;
 
 	db->chip_id = ent->driver_data;
-	db->ioaddr = pci_resource_start(pdev, 0);
+	/* IO type range. */
+	db->ioaddr = pci_iomap(pdev, 0, 0);
+	if (!db->ioaddr)
+		goto err_out_free_buf;
+
 	db->chip_revision = pdev->revision;
 	db->wol_mode = 0;
 
 	db->pdev = pdev;
 
-	dev->base_addr = db->ioaddr;
-	dev->irq = pdev->irq;
 	pci_set_drvdata(pdev, dev);
 	dev->netdev_ops = &netdev_ops;
 	dev->ethtool_ops = &netdev_ethtool_ops;
@@ -484,9 +484,10 @@ static int __devinit dmfe_init_one (struct pci_dev *pdev,
 		db->chip_type = 0;
 
 	/* read 64 word srom data */
-	for (i = 0; i < 64; i++)
+	for (i = 0; i < 64; i++) {
 		((__le16 *) db->srom)[i] =
 			cpu_to_le16(read_srom_word(db->ioaddr, i));
+	}
 
 	/* Set Node address */
 	for (i = 0; i < 6; i++)
@@ -494,16 +495,18 @@ static int __devinit dmfe_init_one (struct pci_dev *pdev,
 
 	err = register_netdev (dev);
 	if (err)
-		goto err_out_free_buf;
+		goto err_out_unmap;
 
 	dev_info(&dev->dev, "Davicom DM%04lx at pci%s, %pM, irq %d\n",
 		 ent->driver_data >> 16,
-		 pci_name(pdev), dev->dev_addr, dev->irq);
+		 pci_name(pdev), dev->dev_addr, pdev->irq);
 
 	pci_set_master(pdev);
 
 	return 0;
 
+err_out_unmap:
+	pci_iounmap(pdev, db->ioaddr);
 err_out_free_buf:
 	pci_free_consistent(pdev, TX_BUF_ALLOC * TX_DESC_CNT + 4,
 			    db->buf_pool_ptr, db->buf_pool_dma_ptr);
@@ -532,7 +535,7 @@ static void __devexit dmfe_remove_one (struct pci_dev *pdev)
  	if (dev) {
 
 		unregister_netdev(dev);
-
+		pci_iounmap(db->pdev, db->ioaddr);
 		pci_free_consistent(db->pdev, sizeof(struct tx_desc) *
 					DESC_ALL_CNT + 0x20, db->desc_pool_ptr,
  					db->desc_pool_dma_ptr);
@@ -555,13 +558,13 @@ static void __devexit dmfe_remove_one (struct pci_dev *pdev)
 
 static int dmfe_open(struct DEVICE *dev)
 {
-	int ret;
 	struct dmfe_board_info *db = netdev_priv(dev);
+	const int irq = db->pdev->irq;
+	int ret;
 
 	DMFE_DBUG(0, "dmfe_open", 0);
 
-	ret = request_irq(dev->irq, dmfe_interrupt,
-			  IRQF_SHARED, dev->name, dev);
+	ret = request_irq(irq, dmfe_interrupt, IRQF_SHARED, dev->name, dev);
 	if (ret)
 		return ret;
 
@@ -615,14 +618,14 @@ static int dmfe_open(struct DEVICE *dev)
 static void dmfe_init_dm910x(struct DEVICE *dev)
 {
 	struct dmfe_board_info *db = netdev_priv(dev);
-	unsigned long ioaddr = db->ioaddr;
+	void __iomem *ioaddr = db->ioaddr;
 
 	DMFE_DBUG(0, "dmfe_init_dm910x()", 0);
 
 	/* Reset DM910x MAC controller */
-	outl(DM910X_RESET, ioaddr + DCR0);	/* RESET MAC */
+	dw32(DCR0, DM910X_RESET);	/* RESET MAC */
 	udelay(100);
-	outl(db->cr0_data, ioaddr + DCR0);
+	dw32(DCR0, db->cr0_data);
 	udelay(5);
 
 	/* Phy addr : DM910(A)2/DM9132/9801, phy address = 1 */
@@ -633,12 +636,12 @@ static void dmfe_init_dm910x(struct DEVICE *dev)
 	db->media_mode = dmfe_media_mode;
 
 	/* RESET Phyxcer Chip by GPR port bit 7 */
-	outl(0x180, ioaddr + DCR12);		/* Let bit 7 output port */
+	dw32(DCR12, 0x180);		/* Let bit 7 output port */
 	if (db->chip_id == PCI_DM9009_ID) {
-		outl(0x80, ioaddr + DCR12);	/* Issue RESET signal */
+		dw32(DCR12, 0x80);	/* Issue RESET signal */
 		mdelay(300);			/* Delay 300 ms */
 	}
-	outl(0x0, ioaddr + DCR12);	/* Clear RESET signal */
+	dw32(DCR12, 0x0);	/* Clear RESET signal */
 
 	/* Process Phyxcer Media Mode */
 	if ( !(db->media_mode & 0x10) )	/* Force 1M mode */
@@ -649,7 +652,7 @@ static void dmfe_init_dm910x(struct DEVICE *dev)
 		db->op_mode = db->media_mode; 	/* Force Mode */
 
 	/* Initialize Transmit/Receive decriptor and CR3/4 */
-	dmfe_descriptor_init(dev, ioaddr);
+	dmfe_descriptor_init(dev);
 
 	/* Init CR6 to program DM910x operation */
 	update_cr6(db->cr6_data, ioaddr);
@@ -662,10 +665,10 @@ static void dmfe_init_dm910x(struct DEVICE *dev)
 
 	/* Init CR7, interrupt active bit */
 	db->cr7_data = CR7_DEFAULT;
-	outl(db->cr7_data, ioaddr + DCR7);
+	dw32(DCR7, db->cr7_data);
 
 	/* Init CR15, Tx jabber and Rx watchdog timer */
-	outl(db->cr15_data, ioaddr + DCR15);
+	dw32(DCR15, db->cr15_data);
 
 	/* Enable DM910X Tx/Rx function */
 	db->cr6_data |= CR6_RXSC | CR6_TXSC | 0x40000;
@@ -682,6 +685,7 @@ static netdev_tx_t dmfe_start_xmit(struct sk_buff *skb,
 					 struct DEVICE *dev)
 {
 	struct dmfe_board_info *db = netdev_priv(dev);
+	void __iomem *ioaddr = db->ioaddr;
 	struct tx_desc *txptr;
 	unsigned long flags;
 
@@ -707,7 +711,7 @@ static netdev_tx_t dmfe_start_xmit(struct sk_buff *skb,
 	}
 
 	/* Disable NIC interrupt */
-	outl(0, dev->base_addr + DCR7);
+	dw32(DCR7, 0);
 
 	/* transmit this packet */
 	txptr = db->tx_insert_ptr;
@@ -721,11 +725,11 @@ static netdev_tx_t dmfe_start_xmit(struct sk_buff *skb,
 	if ( (!db->tx_queue_cnt) && (db->tx_packet_cnt < TX_MAX_SEND_CNT) ) {
 		txptr->tdes0 = cpu_to_le32(0x80000000);	/* Set owner bit */
 		db->tx_packet_cnt++;			/* Ready to send */
-		outl(0x1, dev->base_addr + DCR1);	/* Issue Tx polling */
+		dw32(DCR1, 0x1);			/* Issue Tx polling */
 		dev->trans_start = jiffies;		/* saved time stamp */
 	} else {
 		db->tx_queue_cnt++;			/* queue TX packet */
-		outl(0x1, dev->base_addr + DCR1);	/* Issue Tx polling */
+		dw32(DCR1, 0x1);			/* Issue Tx polling */
 	}
 
 	/* Tx resource check */
@@ -734,7 +738,7 @@ static netdev_tx_t dmfe_start_xmit(struct sk_buff *skb,
 
 	/* Restore CR7 to enable interrupt */
 	spin_unlock_irqrestore(&db->lock, flags);
-	outl(db->cr7_data, dev->base_addr + DCR7);
+	dw32(DCR7, db->cr7_data);
 
 	/* free this SKB */
 	dev_kfree_skb(skb);
@@ -751,7 +755,7 @@ static netdev_tx_t dmfe_start_xmit(struct sk_buff *skb,
 static int dmfe_stop(struct DEVICE *dev)
 {
 	struct dmfe_board_info *db = netdev_priv(dev);
-	unsigned long ioaddr = dev->base_addr;
+	void __iomem *ioaddr = db->ioaddr;
 
 	DMFE_DBUG(0, "dmfe_stop", 0);
 
@@ -762,12 +766,12 @@ static int dmfe_stop(struct DEVICE *dev)
 	del_timer_sync(&db->timer);
 
 	/* Reset & stop DM910X board */
-	outl(DM910X_RESET, ioaddr + DCR0);
-	udelay(5);
-	phy_write(db->ioaddr, db->phy_addr, 0, 0x8000, db->chip_id);
+	dw32(DCR0, DM910X_RESET);
+	udelay(100);
+	phy_write(ioaddr, db->phy_addr, 0, 0x8000, db->chip_id);
 
 	/* free interrupt */
-	free_irq(dev->irq, dev);
+	free_irq(db->pdev->irq, dev);
 
 	/* free allocated rx buffer */
 	dmfe_free_rxbuffer(db);
@@ -794,7 +798,7 @@ static irqreturn_t dmfe_interrupt(int irq, void *dev_id)
 {
 	struct DEVICE *dev = dev_id;
 	struct dmfe_board_info *db = netdev_priv(dev);
-	unsigned long ioaddr = dev->base_addr;
+	void __iomem *ioaddr = db->ioaddr;
 	unsigned long flags;
 
 	DMFE_DBUG(0, "dmfe_interrupt()", 0);
@@ -802,15 +806,15 @@ static irqreturn_t dmfe_interrupt(int irq, void *dev_id)
 	spin_lock_irqsave(&db->lock, flags);
 
 	/* Got DM910X status */
-	db->cr5_data = inl(ioaddr + DCR5);
-	outl(db->cr5_data, ioaddr + DCR5);
+	db->cr5_data = dr32(DCR5);
+	dw32(DCR5, db->cr5_data);
 	if ( !(db->cr5_data & 0xc1) ) {
 		spin_unlock_irqrestore(&db->lock, flags);
 		return IRQ_HANDLED;
 	}
 
 	/* Disable all interrupt in CR7 to solve the interrupt edge problem */
-	outl(0, ioaddr + DCR7);
+	dw32(DCR7, 0);
 
 	/* Check system status */
 	if (db->cr5_data & 0x2000) {
@@ -838,11 +842,11 @@ static irqreturn_t dmfe_interrupt(int irq, void *dev_id)
 	if (db->dm910x_chk_mode & 0x2) {
 		db->dm910x_chk_mode = 0x4;
 		db->cr6_data |= 0x100;
-		update_cr6(db->cr6_data, db->ioaddr);
+		update_cr6(db->cr6_data, ioaddr);
 	}
 
 	/* Restore CR7 to enable interrupt mask */
-	outl(db->cr7_data, ioaddr + DCR7);
+	dw32(DCR7, db->cr7_data);
 
 	spin_unlock_irqrestore(&db->lock, flags);
 	return IRQ_HANDLED;
@@ -858,11 +862,14 @@ static irqreturn_t dmfe_interrupt(int irq, void *dev_id)
 
 static void poll_dmfe (struct net_device *dev)
 {
+	struct dmfe_board_info *db = netdev_priv(dev);
+	const int irq = db->pdev->irq;
+
 	/* disable_irq here is not very nice, but with the lockless
 	   interrupt handler we have no other choice. */
-	disable_irq(dev->irq);
-	dmfe_interrupt (dev->irq, dev);
-	enable_irq(dev->irq);
+	disable_irq(irq);
+	dmfe_interrupt (irq, dev);
+	enable_irq(irq);
 }
 #endif
 
@@ -873,7 +880,7 @@ static void poll_dmfe (struct net_device *dev)
 static void dmfe_free_tx_pkt(struct DEVICE *dev, struct dmfe_board_info * db)
 {
 	struct tx_desc *txptr;
-	unsigned long ioaddr = dev->base_addr;
+	void __iomem *ioaddr = db->ioaddr;
 	u32 tdes0;
 
 	txptr = db->tx_remove_ptr;
@@ -897,7 +904,7 @@ static void dmfe_free_tx_pkt(struct DEVICE *dev, struct dmfe_board_info * db)
 					db->tx_fifo_underrun++;
 					if ( !(db->cr6_data & CR6_SFT) ) {
 						db->cr6_data = db->cr6_data | CR6_SFT;
-						update_cr6(db->cr6_data, db->ioaddr);
+						update_cr6(db->cr6_data, ioaddr);
 					}
 				}
 				if (tdes0 & 0x0100)
@@ -924,7 +931,7 @@ static void dmfe_free_tx_pkt(struct DEVICE *dev, struct dmfe_board_info * db)
 		txptr->tdes0 = cpu_to_le32(0x80000000);	/* Set owner bit */
 		db->tx_packet_cnt++;			/* Ready to send */
 		db->tx_queue_cnt--;
-		outl(0x1, ioaddr + DCR1);		/* Issue Tx polling */
+		dw32(DCR1, 0x1);			/* Issue Tx polling */
 		dev->trans_start = jiffies;		/* saved time stamp */
 	}
 
@@ -1087,12 +1094,7 @@ static void dmfe_ethtool_get_drvinfo(struct net_device *dev,
 
 	strlcpy(info->driver, DRV_NAME, sizeof(info->driver));
 	strlcpy(info->version, DRV_VERSION, sizeof(info->version));
-	if (np->pdev)
-		strlcpy(info->bus_info, pci_name(np->pdev),
-			sizeof(info->bus_info));
-	else
-		sprintf(info->bus_info, "EISA 0x%lx %d",
-			dev->base_addr, dev->irq);
+	strlcpy(info->bus_info, pci_name(np->pdev), sizeof(info->bus_info));
 }
 
 static int dmfe_ethtool_set_wol(struct net_device *dev,
@@ -1132,10 +1134,11 @@ static const struct ethtool_ops netdev_ethtool_ops = {
 
 static void dmfe_timer(unsigned long data)
 {
+	struct net_device *dev = (struct net_device *)data;
+	struct dmfe_board_info *db = netdev_priv(dev);
+	void __iomem *ioaddr = db->ioaddr;
 	u32 tmp_cr8;
 	unsigned char tmp_cr12;
-	struct DEVICE *dev = (struct DEVICE *) data;
-	struct dmfe_board_info *db = netdev_priv(dev);
  	unsigned long flags;
 
 	int link_ok, link_ok_phy;
@@ -1148,11 +1151,10 @@ static void dmfe_timer(unsigned long data)
 		db->first_in_callback = 1;
 		if (db->chip_type && (db->chip_id==PCI_DM9102_ID)) {
 			db->cr6_data &= ~0x40000;
-			update_cr6(db->cr6_data, db->ioaddr);
-			phy_write(db->ioaddr,
-				  db->phy_addr, 0, 0x1000, db->chip_id);
+			update_cr6(db->cr6_data, ioaddr);
+			phy_write(ioaddr, db->phy_addr, 0, 0x1000, db->chip_id);
 			db->cr6_data |= 0x40000;
-			update_cr6(db->cr6_data, db->ioaddr);
+			update_cr6(db->cr6_data, ioaddr);
 			db->timer.expires = DMFE_TIMER_WUT + HZ * 2;
 			add_timer(&db->timer);
 			spin_unlock_irqrestore(&db->lock, flags);
@@ -1167,7 +1169,7 @@ static void dmfe_timer(unsigned long data)
 		db->dm910x_chk_mode = 0x4;
 
 	/* Dynamic reset DM910X : system error or transmit time-out */
-	tmp_cr8 = inl(db->ioaddr + DCR8);
+	tmp_cr8 = dr32(DCR8);
 	if ( (db->interval_rx_cnt==0) && (tmp_cr8) ) {
 		db->reset_cr8++;
 		db->wait_reset = 1;
@@ -1177,7 +1179,7 @@ static void dmfe_timer(unsigned long data)
 	/* TX polling kick monitor */
 	if ( db->tx_packet_cnt &&
 	     time_after(jiffies, dev_trans_start(dev) + DMFE_TX_KICK) ) {
-		outl(0x1, dev->base_addr + DCR1);   /* Tx polling again */
+		dw32(DCR1, 0x1);   /* Tx polling again */
 
 		/* TX Timeout */
 		if (time_after(jiffies, dev_trans_start(dev) + DMFE_TX_TIMEOUT) ) {
@@ -1200,9 +1202,9 @@ static void dmfe_timer(unsigned long data)
 
 	/* Link status check, Dynamic media type change */
 	if (db->chip_id == PCI_DM9132_ID)
-		tmp_cr12 = inb(db->ioaddr + DCR9 + 3);	/* DM9132 */
+		tmp_cr12 = dr8(DCR9 + 3);	/* DM9132 */
 	else
-		tmp_cr12 = inb(db->ioaddr + DCR12);	/* DM9102/DM9102A */
+		tmp_cr12 = dr8(DCR12);		/* DM9102/DM9102A */
 
 	if ( ((db->chip_id == PCI_DM9102_ID) &&
 		(db->chip_revision == 0x30)) ||
@@ -1251,7 +1253,7 @@ static void dmfe_timer(unsigned long data)
 			/* 10/100M link failed, used 1M Home-Net */
 			db->cr6_data|=0x00040000;	/* bit18=1, MII */
 			db->cr6_data&=~0x00000200;	/* bit9=0, HD mode */
-			update_cr6(db->cr6_data, db->ioaddr);
+			update_cr6(db->cr6_data, ioaddr);
 		}
 	} else if (!netif_carrier_ok(dev)) {
 
@@ -1288,17 +1290,18 @@ static void dmfe_timer(unsigned long data)
  *	Re-initialize DM910X board
  */
 
-static void dmfe_dynamic_reset(struct DEVICE *dev)
+static void dmfe_dynamic_reset(struct net_device *dev)
 {
 	struct dmfe_board_info *db = netdev_priv(dev);
+	void __iomem *ioaddr = db->ioaddr;
 
 	DMFE_DBUG(0, "dmfe_dynamic_reset()", 0);
 
 	/* Sopt MAC controller */
 	db->cr6_data &= ~(CR6_RXSC | CR6_TXSC);	/* Disable Tx/Rx */
-	update_cr6(db->cr6_data, dev->base_addr);
-	outl(0, dev->base_addr + DCR7);		/* Disable Interrupt */
-	outl(inl(dev->base_addr + DCR5), dev->base_addr + DCR5);
+	update_cr6(db->cr6_data, ioaddr);
+	dw32(DCR7, 0);				/* Disable Interrupt */
+	dw32(DCR5, dr32(DCR5));
 
 	/* Disable upper layer interface */
 	netif_stop_queue(dev);
@@ -1364,9 +1367,10 @@ static void dmfe_reuse_skb(struct dmfe_board_info *db, struct sk_buff * skb)
  *	Using Chain structure, and allocate Tx/Rx buffer
  */
 
-static void dmfe_descriptor_init(struct net_device *dev, unsigned long ioaddr)
+static void dmfe_descriptor_init(struct net_device *dev)
 {
 	struct dmfe_board_info *db = netdev_priv(dev);
+	void __iomem *ioaddr = db->ioaddr;
 	struct tx_desc *tmp_tx;
 	struct rx_desc *tmp_rx;
 	unsigned char *tmp_buf;
@@ -1379,7 +1383,7 @@ static void dmfe_descriptor_init(struct net_device *dev, unsigned long ioaddr)
 	/* tx descriptor start pointer */
 	db->tx_insert_ptr = db->first_tx_desc;
 	db->tx_remove_ptr = db->first_tx_desc;
-	outl(db->first_tx_desc_dma, ioaddr + DCR4);     /* TX DESC address */
+	dw32(DCR4, db->first_tx_desc_dma);     /* TX DESC address */
 
 	/* rx descriptor start pointer */
 	db->first_rx_desc = (void *)db->first_tx_desc +
@@ -1389,7 +1393,7 @@ static void dmfe_descriptor_init(struct net_device *dev, unsigned long ioaddr)
 			sizeof(struct tx_desc) * TX_DESC_CNT;
 	db->rx_insert_ptr = db->first_rx_desc;
 	db->rx_ready_ptr = db->first_rx_desc;
-	outl(db->first_rx_desc_dma, ioaddr + DCR3);	/* RX DESC address */
+	dw32(DCR3, db->first_rx_desc_dma);		/* RX DESC address */
 
 	/* Init Transmit chain */
 	tmp_buf = db->buf_pool_start;
@@ -1431,14 +1435,14 @@ static void dmfe_descriptor_init(struct net_device *dev, unsigned long ioaddr)
  *	Firstly stop DM910X , then written value and start
  */
 
-static void update_cr6(u32 cr6_data, unsigned long ioaddr)
+static void update_cr6(u32 cr6_data, void __iomem *ioaddr)
 {
 	u32 cr6_tmp;
 
 	cr6_tmp = cr6_data & ~0x2002;           /* stop Tx/Rx */
-	outl(cr6_tmp, ioaddr + DCR6);
+	dw32(DCR6, cr6_tmp);
 	udelay(5);
-	outl(cr6_data, ioaddr + DCR6);
+	dw32(DCR6, cr6_data);
 	udelay(5);
 }
 
@@ -1448,24 +1452,19 @@ static void update_cr6(u32 cr6_data, unsigned long ioaddr)
  *	This setup frame initialize DM910X address filter mode
 */
 
-static void dm9132_id_table(struct DEVICE *dev)
+static void dm9132_id_table(struct net_device *dev)
 {
+	struct dmfe_board_info *db = netdev_priv(dev);
+	void __iomem *ioaddr = db->ioaddr + 0xc0;
+	u16 *addrptr = (u16 *)dev->dev_addr;
 	struct netdev_hw_addr *ha;
-	u16 * addrptr;
-	unsigned long ioaddr = dev->base_addr+0xc0;		/* ID Table */
-	u32 hash_val;
 	u16 i, hash_table[4];
 
-	DMFE_DBUG(0, "dm9132_id_table()", 0);
-
 	/* Node address */
-	addrptr = (u16 *) dev->dev_addr;
-	outw(addrptr[0], ioaddr);
-	ioaddr += 4;
-	outw(addrptr[1], ioaddr);
-	ioaddr += 4;
-	outw(addrptr[2], ioaddr);
-	ioaddr += 4;
+	for (i = 0; i < 3; i++) {
+		dw16(0, addrptr[i]);
+		ioaddr += 4;
+	}
 
 	/* Clear Hash Table */
 	memset(hash_table, 0, sizeof(hash_table));
@@ -1475,13 +1474,14 @@ static void dm9132_id_table(struct DEVICE *dev)
 
 	/* the multicast address in Hash Table : 64 bits */
 	netdev_for_each_mc_addr(ha, dev) {
-		hash_val = cal_CRC((char *) ha->addr, 6, 0) & 0x3f;
+		u32 hash_val = cal_CRC((char *)ha->addr, 6, 0) & 0x3f;
+
 		hash_table[hash_val / 16] |= (u16) 1 << (hash_val % 16);
 	}
 
 	/* Write the hash table to MAC MD table */
 	for (i = 0; i < 4; i++, ioaddr += 4)
-		outw(hash_table[i], ioaddr);
+		dw16(0, hash_table[i]);
 }
 
 
@@ -1490,7 +1490,7 @@ static void dm9132_id_table(struct DEVICE *dev)
  *	This setup frame initialize DM910X address filter mode
  */
 
-static void send_filter_frame(struct DEVICE *dev)
+static void send_filter_frame(struct net_device *dev)
 {
 	struct dmfe_board_info *db = netdev_priv(dev);
 	struct netdev_hw_addr *ha;
@@ -1535,12 +1535,14 @@ static void send_filter_frame(struct DEVICE *dev)
 
 	/* Resource Check and Send the setup packet */
 	if (!db->tx_packet_cnt) {
+		void __iomem *ioaddr = db->ioaddr;
+
 		/* Resource Empty */
 		db->tx_packet_cnt++;
 		txptr->tdes0 = cpu_to_le32(0x80000000);
-		update_cr6(db->cr6_data | 0x2000, dev->base_addr);
-		outl(0x1, dev->base_addr + DCR1);	/* Issue Tx polling */
-		update_cr6(db->cr6_data, dev->base_addr);
+		update_cr6(db->cr6_data | 0x2000, ioaddr);
+		dw32(DCR1, 0x1);	/* Issue Tx polling */
+		update_cr6(db->cr6_data, ioaddr);
 		dev->trans_start = jiffies;
 	} else
 		db->tx_queue_cnt++;	/* Put in TX queue */
@@ -1575,43 +1577,59 @@ static void allocate_rx_buffer(struct net_device *dev)
 	db->rx_insert_ptr = rxptr;
 }
 
+static void srom_clk_write(void __iomem *ioaddr, u32 data)
+{
+	static const u32 cmd[] = {
+		CR9_SROM_READ | CR9_SRCS,
+		CR9_SROM_READ | CR9_SRCS | CR9_SRCLK,
+		CR9_SROM_READ | CR9_SRCS
+	};
+	int i;
+
+	for (i = 0; i < ARRAY_SIZE(cmd); i++) {
+		dw32(DCR9, data | cmd[i]);
+		udelay(5);
+	}
+}
 
 /*
  *	Read one word data from the serial ROM
  */
-
-static u16 read_srom_word(long ioaddr, int offset)
+static u16 read_srom_word(void __iomem *ioaddr, int offset)
 {
+	u16 srom_data;
 	int i;
-	u16 srom_data = 0;
-	long cr9_ioaddr = ioaddr + DCR9;
 
-	outl(CR9_SROM_READ, cr9_ioaddr);
-	outl(CR9_SROM_READ | CR9_SRCS, cr9_ioaddr);
+	dw32(DCR9, CR9_SROM_READ);
+	udelay(5);
+	dw32(DCR9, CR9_SROM_READ | CR9_SRCS);
+	udelay(5);
 
 	/* Send the Read Command 110b */
-	SROM_CLK_WRITE(SROM_DATA_1, cr9_ioaddr);
-	SROM_CLK_WRITE(SROM_DATA_1, cr9_ioaddr);
-	SROM_CLK_WRITE(SROM_DATA_0, cr9_ioaddr);
+	srom_clk_write(ioaddr, SROM_DATA_1);
+	srom_clk_write(ioaddr, SROM_DATA_1);
+	srom_clk_write(ioaddr, SROM_DATA_0);
 
 	/* Send the offset */
 	for (i = 5; i >= 0; i--) {
 		srom_data = (offset & (1 << i)) ? SROM_DATA_1 : SROM_DATA_0;
-		SROM_CLK_WRITE(srom_data, cr9_ioaddr);
+		srom_clk_write(ioaddr, srom_data);
 	}
 
-	outl(CR9_SROM_READ | CR9_SRCS, cr9_ioaddr);
+	dw32(DCR9, CR9_SROM_READ | CR9_SRCS);
+	udelay(5);
 
 	for (i = 16; i > 0; i--) {
-		outl(CR9_SROM_READ | CR9_SRCS | CR9_SRCLK, cr9_ioaddr);
+		dw32(DCR9, CR9_SROM_READ | CR9_SRCS | CR9_SRCLK);
 		udelay(5);
 		srom_data = (srom_data << 1) |
-				((inl(cr9_ioaddr) & CR9_CRDOUT) ? 1 : 0);
-		outl(CR9_SROM_READ | CR9_SRCS, cr9_ioaddr);
+				((dr32(DCR9) & CR9_CRDOUT) ? 1 : 0);
+		dw32(DCR9, CR9_SROM_READ | CR9_SRCS);
 		udelay(5);
 	}
 
-	outl(CR9_SROM_READ, cr9_ioaddr);
+	dw32(DCR9, CR9_SROM_READ);
+	udelay(5);
 	return srom_data;
 }
 
@@ -1620,13 +1638,14 @@ static u16 read_srom_word(long ioaddr, int offset)
  *	Auto sense the media mode
  */
 
-static u8 dmfe_sense_speed(struct dmfe_board_info * db)
+static u8 dmfe_sense_speed(struct dmfe_board_info *db)
 {
+	void __iomem *ioaddr = db->ioaddr;
 	u8 ErrFlag = 0;
 	u16 phy_mode;
 
 	/* CR6 bit18=0, select 10/100M */
-	update_cr6( (db->cr6_data & ~0x40000), db->ioaddr);
+	update_cr6(db->cr6_data & ~0x40000, ioaddr);
 
 	phy_mode = phy_read(db->ioaddr, db->phy_addr, 1, db->chip_id);
 	phy_mode = phy_read(db->ioaddr, db->phy_addr, 1, db->chip_id);
@@ -1665,11 +1684,12 @@ static u8 dmfe_sense_speed(struct dmfe_board_info * db)
 
 static void dmfe_set_phyxcer(struct dmfe_board_info *db)
 {
+	void __iomem *ioaddr = db->ioaddr;
 	u16 phy_reg;
 
 	/* Select 10/100M phyxcer */
 	db->cr6_data &= ~0x40000;
-	update_cr6(db->cr6_data, db->ioaddr);
+	update_cr6(db->cr6_data, ioaddr);
 
 	/* DM9009 Chip: Phyxcer reg18 bit12=0 */
 	if (db->chip_id == PCI_DM9009_ID) {
@@ -1765,18 +1785,15 @@ static void dmfe_process_mode(struct dmfe_board_info *db)
  *	Write a word to Phy register
  */
 
-static void phy_write(unsigned long iobase, u8 phy_addr, u8 offset,
+static void phy_write(void __iomem *ioaddr, u8 phy_addr, u8 offset,
 		      u16 phy_data, u32 chip_id)
 {
 	u16 i;
-	unsigned long ioaddr;
 
 	if (chip_id == PCI_DM9132_ID) {
-		ioaddr = iobase + 0x80 + offset * 4;
-		outw(phy_data, ioaddr);
+		dw16(0x80 + offset * 4, phy_data);
 	} else {
 		/* DM9102/DM9102A Chip */
-		ioaddr = iobase + DCR9;
 
 		/* Send 33 synchronization clock to Phy controller */
 		for (i = 0; i < 35; i++)
@@ -1816,19 +1833,16 @@ static void phy_write(unsigned long iobase, u8 phy_addr, u8 offset,
  *	Read a word data from phy register
  */
 
-static u16 phy_read(unsigned long iobase, u8 phy_addr, u8 offset, u32 chip_id)
+static u16 phy_read(void __iomem *ioaddr, u8 phy_addr, u8 offset, u32 chip_id)
 {
 	int i;
 	u16 phy_data;
-	unsigned long ioaddr;
 
 	if (chip_id == PCI_DM9132_ID) {
 		/* DM9132 Chip */
-		ioaddr = iobase + 0x80 + offset * 4;
-		phy_data = inw(ioaddr);
+		phy_data = dr16(0x80 + offset * 4);
 	} else {
 		/* DM9102/DM9102A Chip */
-		ioaddr = iobase + DCR9;
 
 		/* Send 33 synchronization clock to Phy controller */
 		for (i = 0; i < 35; i++)
@@ -1870,13 +1884,13 @@ static u16 phy_read(unsigned long iobase, u8 phy_addr, u8 offset, u32 chip_id)
  *	Write one bit data to Phy Controller
  */
 
-static void phy_write_1bit(unsigned long ioaddr, u32 phy_data)
+static void phy_write_1bit(void __iomem *ioaddr, u32 phy_data)
 {
-	outl(phy_data, ioaddr);			/* MII Clock Low */
+	dw32(DCR9, phy_data);		/* MII Clock Low */
 	udelay(1);
-	outl(phy_data | MDCLKH, ioaddr);	/* MII Clock High */
+	dw32(DCR9, phy_data | MDCLKH);	/* MII Clock High */
 	udelay(1);
-	outl(phy_data, ioaddr);			/* MII Clock Low */
+	dw32(DCR9, phy_data);		/* MII Clock Low */
 	udelay(1);
 }
 
@@ -1885,14 +1899,14 @@ static void phy_write_1bit(unsigned long ioaddr, u32 phy_data)
  *	Read one bit phy data from PHY controller
  */
 
-static u16 phy_read_1bit(unsigned long ioaddr)
+static u16 phy_read_1bit(void __iomem *ioaddr)
 {
 	u16 phy_data;
 
-	outl(0x50000, ioaddr);
+	dw32(DCR9, 0x50000);
 	udelay(1);
-	phy_data = ( inl(ioaddr) >> 19 ) & 0x1;
-	outl(0x40000, ioaddr);
+	phy_data = (dr32(DCR9) >> 19) & 0x1;
+	dw32(DCR9, 0x40000);
 	udelay(1);
 
 	return phy_data;
@@ -1978,7 +1992,7 @@ static void dmfe_parse_srom(struct dmfe_board_info * db)
 
 	/* Check DM9801 or DM9802 present or not */
 	db->HPNA_present = 0;
-	update_cr6(db->cr6_data|0x40000, db->ioaddr);
+	update_cr6(db->cr6_data | 0x40000, db->ioaddr);
 	tmp_reg = phy_read(db->ioaddr, db->phy_addr, 3, db->chip_id);
 	if ( ( tmp_reg & 0xfff0 ) == 0xb900 ) {
 		/* DM9801 or DM9802 present */
@@ -2095,6 +2109,7 @@ static int dmfe_suspend(struct pci_dev *pci_dev, pm_message_t state)
 {
 	struct net_device *dev = pci_get_drvdata(pci_dev);
 	struct dmfe_board_info *db = netdev_priv(dev);
+	void __iomem *ioaddr = db->ioaddr;
 	u32 tmp;
 
 	/* Disable upper layer interface */
@@ -2102,11 +2117,11 @@ static int dmfe_suspend(struct pci_dev *pci_dev, pm_message_t state)
 
 	/* Disable Tx/Rx */
 	db->cr6_data &= ~(CR6_RXSC | CR6_TXSC);
-	update_cr6(db->cr6_data, dev->base_addr);
+	update_cr6(db->cr6_data, ioaddr);
 
 	/* Disable Interrupt */
-	outl(0, dev->base_addr + DCR7);
-	outl(inl (dev->base_addr + DCR5), dev->base_addr + DCR5);
+	dw32(DCR7, 0);
+	dw32(DCR5, dr32(DCR5));
 
 	/* Fre RX buffers */
 	dmfe_free_rxbuffer(db);
diff --git a/drivers/net/ethernet/dec/tulip/tulip_core.c b/drivers/net/ethernet/dec/tulip/tulip_core.c
index fea3641d939..c4f37aca226 100644
--- a/drivers/net/ethernet/dec/tulip/tulip_core.c
+++ b/drivers/net/ethernet/dec/tulip/tulip_core.c
@@ -328,7 +328,7 @@ static void tulip_up(struct net_device *dev)
 	udelay(100);
 
 	if (tulip_debug > 1)
-		netdev_dbg(dev, "tulip_up(), irq==%d\n", dev->irq);
+		netdev_dbg(dev, "tulip_up(), irq==%d\n", tp->pdev->irq);
 
 	iowrite32(tp->rx_ring_dma, ioaddr + CSR3);
 	iowrite32(tp->tx_ring_dma, ioaddr + CSR4);
@@ -515,11 +515,13 @@ media_picked:
 static int
 tulip_open(struct net_device *dev)
 {
+	struct tulip_private *tp = netdev_priv(dev);
 	int retval;
 
 	tulip_init_ring (dev);
 
-	retval = request_irq(dev->irq, tulip_interrupt, IRQF_SHARED, dev->name, dev);
+	retval = request_irq(tp->pdev->irq, tulip_interrupt, IRQF_SHARED,
+			     dev->name, dev);
 	if (retval)
 		goto free_ring;
 
@@ -841,7 +843,7 @@ static int tulip_close (struct net_device *dev)
 		netdev_dbg(dev, "Shutting down ethercard, status was %02x\n",
 			   ioread32 (ioaddr + CSR5));
 
-	free_irq (dev->irq, dev);
+	free_irq (tp->pdev->irq, dev);
 
 	tulip_free_ring (dev);
 
@@ -1489,8 +1491,6 @@ static int __devinit tulip_init_one (struct pci_dev *pdev,
 
 	INIT_WORK(&tp->media_work, tulip_tbl[tp->chip_id].media_task);
 
-	dev->base_addr = (unsigned long)ioaddr;
-
 #ifdef CONFIG_TULIP_MWI
 	if (!force_csr0 && (tp->flags & HAS_PCI_MWI))
 		tulip_mwi_config (pdev, dev);
@@ -1650,7 +1650,6 @@ static int __devinit tulip_init_one (struct pci_dev *pdev,
 	for (i = 0; i < 6; i++)
 		last_phys_addr[i] = dev->dev_addr[i];
 	last_irq = irq;
-	dev->irq = irq;
 
 	/* The lower four bits are the media type. */
 	if (board_idx >= 0  &&  board_idx < MAX_UNITS) {
@@ -1858,7 +1857,8 @@ static int tulip_suspend (struct pci_dev *pdev, pm_message_t state)
 	tulip_down(dev);
 
 	netif_device_detach(dev);
-	free_irq(dev->irq, dev);
+	/* FIXME: it needlessly adds an error path. */
+	free_irq(tp->pdev->irq, dev);
 
 save_state:
 	pci_save_state(pdev);
@@ -1900,7 +1900,9 @@ static int tulip_resume(struct pci_dev *pdev)
 		return retval;
 	}
 
-	if ((retval = request_irq(dev->irq, tulip_interrupt, IRQF_SHARED, dev->name, dev))) {
+	retval = request_irq(pdev->irq, tulip_interrupt, IRQF_SHARED,
+			     dev->name, dev);
+	if (retval) {
 		pr_err("request_irq failed in resume\n");
 		return retval;
 	}
@@ -1960,11 +1962,14 @@ static void __devexit tulip_remove_one (struct pci_dev *pdev)
 
 static void poll_tulip (struct net_device *dev)
 {
+	struct tulip_private *tp = netdev_priv(dev);
+	const int irq = tp->pdev->irq;
+
 	/* disable_irq here is not very nice, but with the lockless
 	   interrupt handler we have no other choice. */
-	disable_irq(dev->irq);
-	tulip_interrupt (dev->irq, dev);
-	enable_irq(dev->irq);
+	disable_irq(irq);
+	tulip_interrupt (irq, dev);
+	enable_irq(irq);
 }
 #endif
 
diff --git a/drivers/net/ethernet/dec/tulip/uli526x.c b/drivers/net/ethernet/dec/tulip/uli526x.c
index fc4001f6a5e..75d45f8a37d 100644
--- a/drivers/net/ethernet/dec/tulip/uli526x.c
+++ b/drivers/net/ethernet/dec/tulip/uli526x.c
@@ -42,6 +42,8 @@
 #include <asm/dma.h>
 #include <asm/uaccess.h>
 
+#define uw32(reg, val)	iowrite32(val, ioaddr + (reg))
+#define ur32(reg)	ioread32(ioaddr + (reg))
 
 /* Board/System/Debug information/definition ---------------- */
 #define PCI_ULI5261_ID  0x526110B9	/* ULi M5261 ID*/
@@ -110,14 +112,6 @@ do {								\
 
 #define SROM_V41_CODE   0x14
 
-#define SROM_CLK_WRITE(data, ioaddr)					\
-		outl(data|CR9_SROM_READ|CR9_SRCS,ioaddr);		\
-		udelay(5);						\
-		outl(data|CR9_SROM_READ|CR9_SRCS|CR9_SRCLK,ioaddr);	\
-		udelay(5);						\
-		outl(data|CR9_SROM_READ|CR9_SRCS,ioaddr);		\
-		udelay(5);
-
 /* Structure/enum declaration ------------------------------- */
 struct tx_desc {
         __le32 tdes0, tdes1, tdes2, tdes3; /* Data for the card */
@@ -132,12 +126,15 @@ struct rx_desc {
 } __attribute__(( aligned(32) ));
 
 struct uli526x_board_info {
-	u32 chip_id;			/* Chip vendor/Device ID */
+	struct uli_phy_ops {
+		void (*write)(struct uli526x_board_info *, u8, u8, u16);
+		u16 (*read)(struct uli526x_board_info *, u8, u8);
+	} phy;
 	struct net_device *next_dev;	/* next device */
 	struct pci_dev *pdev;		/* PCI device */
 	spinlock_t lock;
 
-	long ioaddr;			/* I/O base address */
+	void __iomem *ioaddr;		/* I/O base address */
 	u32 cr0_data;
 	u32 cr5_data;
 	u32 cr6_data;
@@ -227,21 +224,21 @@ static netdev_tx_t uli526x_start_xmit(struct sk_buff *,
 static int uli526x_stop(struct net_device *);
 static void uli526x_set_filter_mode(struct net_device *);
 static const struct ethtool_ops netdev_ethtool_ops;
-static u16 read_srom_word(long, int);
+static u16 read_srom_word(struct uli526x_board_info *, int);
 static irqreturn_t uli526x_interrupt(int, void *);
 #ifdef CONFIG_NET_POLL_CONTROLLER
 static void uli526x_poll(struct net_device *dev);
 #endif
-static void uli526x_descriptor_init(struct net_device *, unsigned long);
+static void uli526x_descriptor_init(struct net_device *, void __iomem *);
 static void allocate_rx_buffer(struct net_device *);
-static void update_cr6(u32, unsigned long);
+static void update_cr6(u32, void __iomem *);
 static void send_filter_frame(struct net_device *, int);
-static u16 phy_read(unsigned long, u8, u8, u32);
-static u16 phy_readby_cr10(unsigned long, u8, u8);
-static void phy_write(unsigned long, u8, u8, u16, u32);
-static void phy_writeby_cr10(unsigned long, u8, u8, u16);
-static void phy_write_1bit(unsigned long, u32, u32);
-static u16 phy_read_1bit(unsigned long, u32);
+static u16 phy_readby_cr9(struct uli526x_board_info *, u8, u8);
+static u16 phy_readby_cr10(struct uli526x_board_info *, u8, u8);
+static void phy_writeby_cr9(struct uli526x_board_info *, u8, u8, u16);
+static void phy_writeby_cr10(struct uli526x_board_info *, u8, u8, u16);
+static void phy_write_1bit(struct uli526x_board_info *db, u32);
+static u16 phy_read_1bit(struct uli526x_board_info *db);
 static u8 uli526x_sense_speed(struct uli526x_board_info *);
 static void uli526x_process_mode(struct uli526x_board_info *);
 static void uli526x_timer(unsigned long);
@@ -253,6 +250,18 @@ static void uli526x_free_rxbuffer(struct uli526x_board_info *);
 static void uli526x_init(struct net_device *);
 static void uli526x_set_phyxcer(struct uli526x_board_info *);
 
+static void srom_clk_write(struct uli526x_board_info *db, u32 data)
+{
+	void __iomem *ioaddr = db->ioaddr;
+
+	uw32(DCR9, data | CR9_SROM_READ | CR9_SRCS);
+	udelay(5);
+	uw32(DCR9, data | CR9_SROM_READ | CR9_SRCS | CR9_SRCLK);
+	udelay(5);
+	uw32(DCR9, data | CR9_SROM_READ | CR9_SRCS);
+	udelay(5);
+}
+
 /* ULI526X network board routine ---------------------------- */
 
 static const struct net_device_ops netdev_ops = {
@@ -277,6 +286,7 @@ static int __devinit uli526x_init_one (struct pci_dev *pdev,
 {
 	struct uli526x_board_info *db;	/* board information structure */
 	struct net_device *dev;
+	void __iomem *ioaddr;
 	int i, err;
 
 	ULI526X_DBUG(0, "uli526x_init_one()", 0);
@@ -313,9 +323,9 @@ static int __devinit uli526x_init_one (struct pci_dev *pdev,
 		goto err_out_disable;
 	}
 
-	if (pci_request_regions(pdev, DRV_NAME)) {
+	err = pci_request_regions(pdev, DRV_NAME);
+	if (err < 0) {
 		pr_err("Failed to request PCI regions\n");
-		err = -ENODEV;
 		goto err_out_disable;
 	}
 
@@ -323,32 +333,41 @@ static int __devinit uli526x_init_one (struct pci_dev *pdev,
 	db = netdev_priv(dev);
 
 	/* Allocate Tx/Rx descriptor memory */
+	err = -ENOMEM;
+
 	db->desc_pool_ptr = pci_alloc_consistent(pdev, sizeof(struct tx_desc) * DESC_ALL_CNT + 0x20, &db->desc_pool_dma_ptr);
-	if(db->desc_pool_ptr == NULL)
-	{
-		err = -ENOMEM;
-		goto err_out_nomem;
-	}
+	if (!db->desc_pool_ptr)
+		goto err_out_release;
+
 	db->buf_pool_ptr = pci_alloc_consistent(pdev, TX_BUF_ALLOC * TX_DESC_CNT + 4, &db->buf_pool_dma_ptr);
-	if(db->buf_pool_ptr == NULL)
-	{
-		err = -ENOMEM;
-		goto err_out_nomem;
-	}
+	if (!db->buf_pool_ptr)
+		goto err_out_free_tx_desc;
 
 	db->first_tx_desc = (struct tx_desc *) db->desc_pool_ptr;
 	db->first_tx_desc_dma = db->desc_pool_dma_ptr;
 	db->buf_pool_start = db->buf_pool_ptr;
 	db->buf_pool_dma_start = db->buf_pool_dma_ptr;
 
-	db->chip_id = ent->driver_data;
-	db->ioaddr = pci_resource_start(pdev, 0);
+	switch (ent->driver_data) {
+	case PCI_ULI5263_ID:
+		db->phy.write	= phy_writeby_cr10;
+		db->phy.read	= phy_readby_cr10;
+		break;
+	default:
+		db->phy.write	= phy_writeby_cr9;
+		db->phy.read	= phy_readby_cr9;
+		break;
+	}
+
+	/* IO region. */
+	ioaddr = pci_iomap(pdev, 0, 0);
+	if (!ioaddr)
+		goto err_out_free_tx_buf;
 
+	db->ioaddr = ioaddr;
 	db->pdev = pdev;
 	db->init = 1;
 
-	dev->base_addr = db->ioaddr;
-	dev->irq = pdev->irq;
 	pci_set_drvdata(pdev, dev);
 
 	/* Register some necessary functions */
@@ -360,24 +379,24 @@ static int __devinit uli526x_init_one (struct pci_dev *pdev,
 
 	/* read 64 word srom data */
 	for (i = 0; i < 64; i++)
-		((__le16 *) db->srom)[i] = cpu_to_le16(read_srom_word(db->ioaddr, i));
+		((__le16 *) db->srom)[i] = cpu_to_le16(read_srom_word(db, i));
 
 	/* Set Node address */
 	if(((u16 *) db->srom)[0] == 0xffff || ((u16 *) db->srom)[0] == 0)		/* SROM absent, so read MAC address from ID Table */
 	{
-		outl(0x10000, db->ioaddr + DCR0);	//Diagnosis mode
-		outl(0x1c0, db->ioaddr + DCR13);	//Reset dianostic pointer port
-		outl(0, db->ioaddr + DCR14);		//Clear reset port
-		outl(0x10, db->ioaddr + DCR14);		//Reset ID Table pointer
-		outl(0, db->ioaddr + DCR14);		//Clear reset port
-		outl(0, db->ioaddr + DCR13);		//Clear CR13
-		outl(0x1b0, db->ioaddr + DCR13);	//Select ID Table access port
+		uw32(DCR0, 0x10000);	//Diagnosis mode
+		uw32(DCR13, 0x1c0);	//Reset dianostic pointer port
+		uw32(DCR14, 0);		//Clear reset port
+		uw32(DCR14, 0x10);	//Reset ID Table pointer
+		uw32(DCR14, 0);		//Clear reset port
+		uw32(DCR13, 0);		//Clear CR13
+		uw32(DCR13, 0x1b0);	//Select ID Table access port
 		//Read MAC address from CR14
 		for (i = 0; i < 6; i++)
-			dev->dev_addr[i] = inl(db->ioaddr + DCR14);
+			dev->dev_addr[i] = ur32(DCR14);
 		//Read end
-		outl(0, db->ioaddr + DCR13);	//Clear CR13
-		outl(0, db->ioaddr + DCR0);		//Clear CR0
+		uw32(DCR13, 0);		//Clear CR13
+		uw32(DCR0, 0);		//Clear CR0
 		udelay(10);
 	}
 	else		/*Exist SROM*/
@@ -387,26 +406,26 @@ static int __devinit uli526x_init_one (struct pci_dev *pdev,
 	}
 	err = register_netdev (dev);
 	if (err)
-		goto err_out_res;
+		goto err_out_unmap;
 
 	netdev_info(dev, "ULi M%04lx at pci%s, %pM, irq %d\n",
 		    ent->driver_data >> 16, pci_name(pdev),
-		    dev->dev_addr, dev->irq);
+		    dev->dev_addr, pdev->irq);
 
 	pci_set_master(pdev);
 
 	return 0;
 
-err_out_res:
+err_out_unmap:
+	pci_iounmap(pdev, db->ioaddr);
+err_out_free_tx_buf:
+	pci_free_consistent(pdev, TX_BUF_ALLOC * TX_DESC_CNT + 4,
+			    db->buf_pool_ptr, db->buf_pool_dma_ptr);
+err_out_free_tx_desc:
+	pci_free_consistent(pdev, sizeof(struct tx_desc) * DESC_ALL_CNT + 0x20,
+			    db->desc_pool_ptr, db->desc_pool_dma_ptr);
+err_out_release:
 	pci_release_regions(pdev);
-err_out_nomem:
-	if(db->desc_pool_ptr)
-		pci_free_consistent(pdev, sizeof(struct tx_desc) * DESC_ALL_CNT + 0x20,
-			db->desc_pool_ptr, db->desc_pool_dma_ptr);
-
-	if(db->buf_pool_ptr != NULL)
-		pci_free_consistent(pdev, TX_BUF_ALLOC * TX_DESC_CNT + 4,
-			db->buf_pool_ptr, db->buf_pool_dma_ptr);
 err_out_disable:
 	pci_disable_device(pdev);
 err_out_free:
@@ -422,19 +441,17 @@ static void __devexit uli526x_remove_one (struct pci_dev *pdev)
 	struct net_device *dev = pci_get_drvdata(pdev);
 	struct uli526x_board_info *db = netdev_priv(dev);
 
-	ULI526X_DBUG(0, "uli526x_remove_one()", 0);
-
+	unregister_netdev(dev);
+	pci_iounmap(pdev, db->ioaddr);
 	pci_free_consistent(db->pdev, sizeof(struct tx_desc) *
 				DESC_ALL_CNT + 0x20, db->desc_pool_ptr,
  				db->desc_pool_dma_ptr);
 	pci_free_consistent(db->pdev, TX_BUF_ALLOC * TX_DESC_CNT + 4,
 				db->buf_pool_ptr, db->buf_pool_dma_ptr);
-	unregister_netdev(dev);
 	pci_release_regions(pdev);
-	free_netdev(dev);	/* free board information */
-	pci_set_drvdata(pdev, NULL);
 	pci_disable_device(pdev);
-	ULI526X_DBUG(0, "uli526x_remove_one() exit", 0);
+	pci_set_drvdata(pdev, NULL);
+	free_netdev(dev);
 }
 
 
@@ -468,7 +485,8 @@ static int uli526x_open(struct net_device *dev)
 	/* Initialize ULI526X board */
 	uli526x_init(dev);
 
-	ret = request_irq(dev->irq, uli526x_interrupt, IRQF_SHARED, dev->name, dev);
+	ret = request_irq(db->pdev->irq, uli526x_interrupt, IRQF_SHARED,
+			  dev->name, dev);
 	if (ret)
 		return ret;
 
@@ -496,57 +514,57 @@ static int uli526x_open(struct net_device *dev)
 static void uli526x_init(struct net_device *dev)
 {
 	struct uli526x_board_info *db = netdev_priv(dev);
-	unsigned long ioaddr = db->ioaddr;
+	struct uli_phy_ops *phy = &db->phy;
+	void __iomem *ioaddr = db->ioaddr;
 	u8	phy_tmp;
 	u8	timeout;
-	u16	phy_value;
 	u16 phy_reg_reset;
 
 
 	ULI526X_DBUG(0, "uli526x_init()", 0);
 
 	/* Reset M526x MAC controller */
-	outl(ULI526X_RESET, ioaddr + DCR0);	/* RESET MAC */
+	uw32(DCR0, ULI526X_RESET);	/* RESET MAC */
 	udelay(100);
-	outl(db->cr0_data, ioaddr + DCR0);
+	uw32(DCR0, db->cr0_data);
 	udelay(5);
 
 	/* Phy addr : In some boards,M5261/M5263 phy address != 1 */
 	db->phy_addr = 1;
-	for(phy_tmp=0;phy_tmp<32;phy_tmp++)
-	{
-		phy_value=phy_read(db->ioaddr,phy_tmp,3,db->chip_id);//peer add
-		if(phy_value != 0xffff&&phy_value!=0)
-		{
+	for (phy_tmp = 0; phy_tmp < 32; phy_tmp++) {
+		u16 phy_value;
+
+		phy_value = phy->read(db, phy_tmp, 3);	//peer add
+		if (phy_value != 0xffff && phy_value != 0) {
 			db->phy_addr = phy_tmp;
 			break;
 		}
 	}
-	if(phy_tmp == 32)
+
+	if (phy_tmp == 32)
 		pr_warn("Can not find the phy address!!!\n");
 	/* Parser SROM and media mode */
 	db->media_mode = uli526x_media_mode;
 
 	/* phyxcer capability setting */
-	phy_reg_reset = phy_read(db->ioaddr, db->phy_addr, 0, db->chip_id);
+	phy_reg_reset = phy->read(db, db->phy_addr, 0);
 	phy_reg_reset = (phy_reg_reset | 0x8000);
-	phy_write(db->ioaddr, db->phy_addr, 0, phy_reg_reset, db->chip_id);
+	phy->write(db, db->phy_addr, 0, phy_reg_reset);
 
 	/* See IEEE 802.3-2002.pdf (Section 2, Chapter "22.2.4 Management
 	 * functions") or phy data sheet for details on phy reset
 	 */
 	udelay(500);
 	timeout = 10;
-	while (timeout-- &&
-		phy_read(db->ioaddr, db->phy_addr, 0, db->chip_id) & 0x8000)
-			udelay(100);
+	while (timeout-- && phy->read(db, db->phy_addr, 0) & 0x8000)
+		udelay(100);
 
 	/* Process Phyxcer Media Mode */
 	uli526x_set_phyxcer(db);
 
 	/* Media Mode Process */
 	if ( !(db->media_mode & ULI526X_AUTO) )
-		db->op_mode = db->media_mode; 	/* Force Mode */
+		db->op_mode = db->media_mode;		/* Force Mode */
 
 	/* Initialize Transmit/Receive decriptor and CR3/4 */
 	uli526x_descriptor_init(dev, ioaddr);
@@ -559,10 +577,10 @@ static void uli526x_init(struct net_device *dev)
 
 	/* Init CR7, interrupt active bit */
 	db->cr7_data = CR7_DEFAULT;
-	outl(db->cr7_data, ioaddr + DCR7);
+	uw32(DCR7, db->cr7_data);
 
 	/* Init CR15, Tx jabber and Rx watchdog timer */
-	outl(db->cr15_data, ioaddr + DCR15);
+	uw32(DCR15, db->cr15_data);
 
 	/* Enable ULI526X Tx/Rx function */
 	db->cr6_data |= CR6_RXSC | CR6_TXSC;
@@ -579,6 +597,7 @@ static netdev_tx_t uli526x_start_xmit(struct sk_buff *skb,
 					    struct net_device *dev)
 {
 	struct uli526x_board_info *db = netdev_priv(dev);
+	void __iomem *ioaddr = db->ioaddr;
 	struct tx_desc *txptr;
 	unsigned long flags;
 
@@ -604,7 +623,7 @@ static netdev_tx_t uli526x_start_xmit(struct sk_buff *skb,
 	}
 
 	/* Disable NIC interrupt */
-	outl(0, dev->base_addr + DCR7);
+	uw32(DCR7, 0);
 
 	/* transmit this packet */
 	txptr = db->tx_insert_ptr;
@@ -615,10 +634,10 @@ static netdev_tx_t uli526x_start_xmit(struct sk_buff *skb,
 	db->tx_insert_ptr = txptr->next_tx_desc;
 
 	/* Transmit Packet Process */
-	if ( (db->tx_packet_cnt < TX_DESC_CNT) ) {
+	if (db->tx_packet_cnt < TX_DESC_CNT) {
 		txptr->tdes0 = cpu_to_le32(0x80000000);	/* Set owner bit */
 		db->tx_packet_cnt++;			/* Ready to send */
-		outl(0x1, dev->base_addr + DCR1);	/* Issue Tx polling */
+		uw32(DCR1, 0x1);			/* Issue Tx polling */
 		dev->trans_start = jiffies;		/* saved time stamp */
 	}
 
@@ -628,7 +647,7 @@ static netdev_tx_t uli526x_start_xmit(struct sk_buff *skb,
 
 	/* Restore CR7 to enable interrupt */
 	spin_unlock_irqrestore(&db->lock, flags);
-	outl(db->cr7_data, dev->base_addr + DCR7);
+	uw32(DCR7, db->cr7_data);
 
 	/* free this SKB */
 	dev_kfree_skb(skb);
@@ -645,9 +664,7 @@ static netdev_tx_t uli526x_start_xmit(struct sk_buff *skb,
 static int uli526x_stop(struct net_device *dev)
 {
 	struct uli526x_board_info *db = netdev_priv(dev);
-	unsigned long ioaddr = dev->base_addr;
-
-	ULI526X_DBUG(0, "uli526x_stop", 0);
+	void __iomem *ioaddr = db->ioaddr;
 
 	/* disable system */
 	netif_stop_queue(dev);
@@ -656,12 +673,12 @@ static int uli526x_stop(struct net_device *dev)
 	del_timer_sync(&db->timer);
 
 	/* Reset & stop ULI526X board */
-	outl(ULI526X_RESET, ioaddr + DCR0);
+	uw32(DCR0, ULI526X_RESET);
 	udelay(5);
-	phy_write(db->ioaddr, db->phy_addr, 0, 0x8000, db->chip_id);
+	db->phy.write(db, db->phy_addr, 0, 0x8000);
 
 	/* free interrupt */
-	free_irq(dev->irq, dev);
+	free_irq(db->pdev->irq, dev);
 
 	/* free allocated rx buffer */
 	uli526x_free_rxbuffer(db);
@@ -679,18 +696,18 @@ static irqreturn_t uli526x_interrupt(int irq, void *dev_id)
 {
 	struct net_device *dev = dev_id;
 	struct uli526x_board_info *db = netdev_priv(dev);
-	unsigned long ioaddr = dev->base_addr;
+	void __iomem *ioaddr = db->ioaddr;
 	unsigned long flags;
 
 	spin_lock_irqsave(&db->lock, flags);
-	outl(0, ioaddr + DCR7);
+	uw32(DCR7, 0);
 
 	/* Got ULI526X status */
-	db->cr5_data = inl(ioaddr + DCR5);
-	outl(db->cr5_data, ioaddr + DCR5);
+	db->cr5_data = ur32(DCR5);
+	uw32(DCR5, db->cr5_data);
 	if ( !(db->cr5_data & 0x180c1) ) {
 		/* Restore CR7 to enable interrupt mask */
-		outl(db->cr7_data, ioaddr + DCR7);
+		uw32(DCR7, db->cr7_data);
 		spin_unlock_irqrestore(&db->lock, flags);
 		return IRQ_HANDLED;
 	}
@@ -718,7 +735,7 @@ static irqreturn_t uli526x_interrupt(int irq, void *dev_id)
 		uli526x_free_tx_pkt(dev, db);
 
 	/* Restore CR7 to enable interrupt mask */
-	outl(db->cr7_data, ioaddr + DCR7);
+	uw32(DCR7, db->cr7_data);
 
 	spin_unlock_irqrestore(&db->lock, flags);
 	return IRQ_HANDLED;
@@ -727,8 +744,10 @@ static irqreturn_t uli526x_interrupt(int irq, void *dev_id)
 #ifdef CONFIG_NET_POLL_CONTROLLER
 static void uli526x_poll(struct net_device *dev)
 {
+	struct uli526x_board_info *db = netdev_priv(dev);
+
 	/* ISR grabs the irqsave lock, so this should be safe */
-	uli526x_interrupt(dev->irq, dev);
+	uli526x_interrupt(db->pdev->irq, dev);
 }
 #endif
 
@@ -962,12 +981,7 @@ static void netdev_get_drvinfo(struct net_device *dev,
 
 	strlcpy(info->driver, DRV_NAME, sizeof(info->driver));
 	strlcpy(info->version, DRV_VERSION, sizeof(info->version));
-	if (np->pdev)
-		strlcpy(info->bus_info, pci_name(np->pdev),
-			sizeof(info->bus_info));
-	else
-		sprintf(info->bus_info, "EISA 0x%lx %d",
-			dev->base_addr, dev->irq);
+	strlcpy(info->bus_info, pci_name(np->pdev), sizeof(info->bus_info));
 }
 
 static int netdev_get_settings(struct net_device *dev, struct ethtool_cmd *cmd) {
@@ -1007,18 +1021,20 @@ static const struct ethtool_ops netdev_ethtool_ops = {
 
 static void uli526x_timer(unsigned long data)
 {
-	u32 tmp_cr8;
-	unsigned char tmp_cr12=0;
 	struct net_device *dev = (struct net_device *) data;
 	struct uli526x_board_info *db = netdev_priv(dev);
+	struct uli_phy_ops *phy = &db->phy;
+	void __iomem *ioaddr = db->ioaddr;
  	unsigned long flags;
+	u8 tmp_cr12 = 0;
+	u32 tmp_cr8;
 
 	//ULI526X_DBUG(0, "uli526x_timer()", 0);
 	spin_lock_irqsave(&db->lock, flags);
 
 
 	/* Dynamic reset ULI526X : system error or transmit time-out */
-	tmp_cr8 = inl(db->ioaddr + DCR8);
+	tmp_cr8 = ur32(DCR8);
 	if ( (db->interval_rx_cnt==0) && (tmp_cr8) ) {
 		db->reset_cr8++;
 		db->wait_reset = 1;
@@ -1028,7 +1044,7 @@ static void uli526x_timer(unsigned long data)
 	/* TX polling kick monitor */
 	if ( db->tx_packet_cnt &&
 	     time_after(jiffies, dev_trans_start(dev) + ULI526X_TX_KICK) ) {
-		outl(0x1, dev->base_addr + DCR1);   // Tx polling again
+		uw32(DCR1, 0x1);   // Tx polling again
 
 		// TX Timeout
 		if ( time_after(jiffies, dev_trans_start(dev) + ULI526X_TX_TIMEOUT) ) {
@@ -1049,7 +1065,7 @@ static void uli526x_timer(unsigned long data)
 	}
 
 	/* Link status check, Dynamic media type change */
-	if((phy_read(db->ioaddr, db->phy_addr, 5, db->chip_id) & 0x01e0)!=0)
+	if ((phy->read(db, db->phy_addr, 5) & 0x01e0)!=0)
 		tmp_cr12 = 3;
 
 	if ( !(tmp_cr12 & 0x3) && !db->link_failed ) {
@@ -1062,7 +1078,7 @@ static void uli526x_timer(unsigned long data)
 		/* For Force 10/100M Half/Full mode: Enable Auto-Nego mode */
 		/* AUTO don't need */
 		if ( !(db->media_mode & 0x8) )
-			phy_write(db->ioaddr, db->phy_addr, 0, 0x1000, db->chip_id);
+			phy->write(db, db->phy_addr, 0, 0x1000);
 
 		/* AUTO mode, if INT phyxcer link failed, select EXT device */
 		if (db->media_mode & ULI526X_AUTO) {
@@ -1119,12 +1135,13 @@ static void uli526x_timer(unsigned long data)
 static void uli526x_reset_prepare(struct net_device *dev)
 {
 	struct uli526x_board_info *db = netdev_priv(dev);
+	void __iomem *ioaddr = db->ioaddr;
 
 	/* Sopt MAC controller */
 	db->cr6_data &= ~(CR6_RXSC | CR6_TXSC);	/* Disable Tx/Rx */
-	update_cr6(db->cr6_data, dev->base_addr);
-	outl(0, dev->base_addr + DCR7);		/* Disable Interrupt */
-	outl(inl(dev->base_addr + DCR5), dev->base_addr + DCR5);
+	update_cr6(db->cr6_data, ioaddr);
+	uw32(DCR7, 0);				/* Disable Interrupt */
+	uw32(DCR5, ur32(DCR5));
 
 	/* Disable upper layer interface */
 	netif_stop_queue(dev);
@@ -1289,7 +1306,7 @@ static void uli526x_reuse_skb(struct uli526x_board_info *db, struct sk_buff * sk
  *	Using Chain structure, and allocate Tx/Rx buffer
  */
 
-static void uli526x_descriptor_init(struct net_device *dev, unsigned long ioaddr)
+static void uli526x_descriptor_init(struct net_device *dev, void __iomem *ioaddr)
 {
 	struct uli526x_board_info *db = netdev_priv(dev);
 	struct tx_desc *tmp_tx;
@@ -1304,14 +1321,14 @@ static void uli526x_descriptor_init(struct net_device *dev, unsigned long ioaddr
 	/* tx descriptor start pointer */
 	db->tx_insert_ptr = db->first_tx_desc;
 	db->tx_remove_ptr = db->first_tx_desc;
-	outl(db->first_tx_desc_dma, ioaddr + DCR4);     /* TX DESC address */
+	uw32(DCR4, db->first_tx_desc_dma);	/* TX DESC address */
 
 	/* rx descriptor start pointer */
 	db->first_rx_desc = (void *)db->first_tx_desc + sizeof(struct tx_desc) * TX_DESC_CNT;
 	db->first_rx_desc_dma =  db->first_tx_desc_dma + sizeof(struct tx_desc) * TX_DESC_CNT;
 	db->rx_insert_ptr = db->first_rx_desc;
 	db->rx_ready_ptr = db->first_rx_desc;
-	outl(db->first_rx_desc_dma, ioaddr + DCR3);	/* RX DESC address */
+	uw32(DCR3, db->first_rx_desc_dma);	/* RX DESC address */
 
 	/* Init Transmit chain */
 	tmp_buf = db->buf_pool_start;
@@ -1352,11 +1369,9 @@ static void uli526x_descriptor_init(struct net_device *dev, unsigned long ioaddr
  *	Update CR6 value
  *	Firstly stop ULI526X, then written value and start
  */
-
-static void update_cr6(u32 cr6_data, unsigned long ioaddr)
+static void update_cr6(u32 cr6_data, void __iomem *ioaddr)
 {
-
-	outl(cr6_data, ioaddr + DCR6);
+	uw32(DCR6, cr6_data);
 	udelay(5);
 }
 
@@ -1375,6 +1390,7 @@ static void update_cr6(u32 cr6_data, unsigned long ioaddr)
 static void send_filter_frame(struct net_device *dev, int mc_cnt)
 {
 	struct uli526x_board_info *db = netdev_priv(dev);
+	void __iomem *ioaddr = db->ioaddr;
 	struct netdev_hw_addr *ha;
 	struct tx_desc *txptr;
 	u16 * addrptr;
@@ -1420,9 +1436,9 @@ static void send_filter_frame(struct net_device *dev, int mc_cnt)
 		/* Resource Empty */
 		db->tx_packet_cnt++;
 		txptr->tdes0 = cpu_to_le32(0x80000000);
-		update_cr6(db->cr6_data | 0x2000, dev->base_addr);
-		outl(0x1, dev->base_addr + DCR1);	/* Issue Tx polling */
-		update_cr6(db->cr6_data, dev->base_addr);
+		update_cr6(db->cr6_data | 0x2000, ioaddr);
+		uw32(DCR1, 0x1);	/* Issue Tx polling */
+		update_cr6(db->cr6_data, ioaddr);
 		dev->trans_start = jiffies;
 	} else
 		netdev_err(dev, "No Tx resource - Send_filter_frame!\n");
@@ -1465,37 +1481,38 @@ static void allocate_rx_buffer(struct net_device *dev)
  *	Read one word data from the serial ROM
  */
 
-static u16 read_srom_word(long ioaddr, int offset)
+static u16 read_srom_word(struct uli526x_board_info *db, int offset)
 {
-	int i;
+	void __iomem *ioaddr = db->ioaddr;
 	u16 srom_data = 0;
-	long cr9_ioaddr = ioaddr + DCR9;
+	int i;
 
-	outl(CR9_SROM_READ, cr9_ioaddr);
-	outl(CR9_SROM_READ | CR9_SRCS, cr9_ioaddr);
+	uw32(DCR9, CR9_SROM_READ);
+	uw32(DCR9, CR9_SROM_READ | CR9_SRCS);
 
 	/* Send the Read Command 110b */
-	SROM_CLK_WRITE(SROM_DATA_1, cr9_ioaddr);
-	SROM_CLK_WRITE(SROM_DATA_1, cr9_ioaddr);
-	SROM_CLK_WRITE(SROM_DATA_0, cr9_ioaddr);
+	srom_clk_write(db, SROM_DATA_1);
+	srom_clk_write(db, SROM_DATA_1);
+	srom_clk_write(db, SROM_DATA_0);
 
 	/* Send the offset */
 	for (i = 5; i >= 0; i--) {
 		srom_data = (offset & (1 << i)) ? SROM_DATA_1 : SROM_DATA_0;
-		SROM_CLK_WRITE(srom_data, cr9_ioaddr);
+		srom_clk_write(db, srom_data);
 	}
 
-	outl(CR9_SROM_READ | CR9_SRCS, cr9_ioaddr);
+	uw32(DCR9, CR9_SROM_READ | CR9_SRCS);
 
 	for (i = 16; i > 0; i--) {
-		outl(CR9_SROM_READ | CR9_SRCS | CR9_SRCLK, cr9_ioaddr);
+		uw32(DCR9, CR9_SROM_READ | CR9_SRCS | CR9_SRCLK);
 		udelay(5);
-		srom_data = (srom_data << 1) | ((inl(cr9_ioaddr) & CR9_CRDOUT) ? 1 : 0);
-		outl(CR9_SROM_READ | CR9_SRCS, cr9_ioaddr);
+		srom_data = (srom_data << 1) |
+			    ((ur32(DCR9) & CR9_CRDOUT) ? 1 : 0);
+		uw32(DCR9, CR9_SROM_READ | CR9_SRCS);
 		udelay(5);
 	}
 
-	outl(CR9_SROM_READ, cr9_ioaddr);
+	uw32(DCR9, CR9_SROM_READ);
 	return srom_data;
 }
 
@@ -1506,15 +1523,16 @@ static u16 read_srom_word(long ioaddr, int offset)
 
 static u8 uli526x_sense_speed(struct uli526x_board_info * db)
 {
+	struct uli_phy_ops *phy = &db->phy;
 	u8 ErrFlag = 0;
 	u16 phy_mode;
 
-	phy_mode = phy_read(db->ioaddr, db->phy_addr, 1, db->chip_id);
-	phy_mode = phy_read(db->ioaddr, db->phy_addr, 1, db->chip_id);
+	phy_mode = phy->read(db, db->phy_addr, 1);
+	phy_mode = phy->read(db, db->phy_addr, 1);
 
 	if ( (phy_mode & 0x24) == 0x24 ) {
 
-		phy_mode = ((phy_read(db->ioaddr, db->phy_addr, 5, db->chip_id) & 0x01e0)<<7);
+		phy_mode = ((phy->read(db, db->phy_addr, 5) & 0x01e0)<<7);
 		if(phy_mode&0x8000)
 			phy_mode = 0x8000;
 		else if(phy_mode&0x4000)
@@ -1549,10 +1567,11 @@ static u8 uli526x_sense_speed(struct uli526x_board_info * db)
 
 static void uli526x_set_phyxcer(struct uli526x_board_info *db)
 {
+	struct uli_phy_ops *phy = &db->phy;
 	u16 phy_reg;
 
 	/* Phyxcer capability setting */
-	phy_reg = phy_read(db->ioaddr, db->phy_addr, 4, db->chip_id) & ~0x01e0;
+	phy_reg = phy->read(db, db->phy_addr, 4) & ~0x01e0;
 
 	if (db->media_mode & ULI526X_AUTO) {
 		/* AUTO Mode */
@@ -1573,10 +1592,10 @@ static void uli526x_set_phyxcer(struct uli526x_board_info *db)
 		phy_reg|=db->PHY_reg4;
 		db->media_mode|=ULI526X_AUTO;
 	}
-	phy_write(db->ioaddr, db->phy_addr, 4, phy_reg, db->chip_id);
+	phy->write(db, db->phy_addr, 4, phy_reg);
 
  	/* Restart Auto-Negotiation */
-	phy_write(db->ioaddr, db->phy_addr, 0, 0x1200, db->chip_id);
+	phy->write(db, db->phy_addr, 0, 0x1200);
 	udelay(50);
 }
 
@@ -1590,6 +1609,7 @@ static void uli526x_set_phyxcer(struct uli526x_board_info *db)
 
 static void uli526x_process_mode(struct uli526x_board_info *db)
 {
+	struct uli_phy_ops *phy = &db->phy;
 	u16 phy_reg;
 
 	/* Full Duplex Mode Check */
@@ -1601,10 +1621,10 @@ static void uli526x_process_mode(struct uli526x_board_info *db)
 	update_cr6(db->cr6_data, db->ioaddr);
 
 	/* 10/100M phyxcer force mode need */
-	if ( !(db->media_mode & 0x8)) {
+	if (!(db->media_mode & 0x8)) {
 		/* Forece Mode */
-		phy_reg = phy_read(db->ioaddr, db->phy_addr, 6, db->chip_id);
-		if ( !(phy_reg & 0x1) ) {
+		phy_reg = phy->read(db, db->phy_addr, 6);
+		if (!(phy_reg & 0x1)) {
 			/* parter without N-Way capability */
 			phy_reg = 0x0;
 			switch(db->op_mode) {
@@ -1613,148 +1633,126 @@ static void uli526x_process_mode(struct uli526x_board_info *db)
 			case ULI526X_100MHF: phy_reg = 0x2000; break;
 			case ULI526X_100MFD: phy_reg = 0x2100; break;
 			}
-			phy_write(db->ioaddr, db->phy_addr, 0, phy_reg, db->chip_id);
+			phy->write(db, db->phy_addr, 0, phy_reg);
 		}
 	}
 }
 
 
-/*
- *	Write a word to Phy register
- */
-
-static void phy_write(unsigned long iobase, u8 phy_addr, u8 offset, u16 phy_data, u32 chip_id)
+/* M5261/M5263 Chip */
+static void phy_writeby_cr9(struct uli526x_board_info *db, u8 phy_addr,
+			    u8 offset, u16 phy_data)
 {
 	u16 i;
-	unsigned long ioaddr;
-
-	if(chip_id == PCI_ULI5263_ID)
-	{
-		phy_writeby_cr10(iobase, phy_addr, offset, phy_data);
-		return;
-	}
-	/* M5261/M5263 Chip */
-	ioaddr = iobase + DCR9;
 
 	/* Send 33 synchronization clock to Phy controller */
 	for (i = 0; i < 35; i++)
-		phy_write_1bit(ioaddr, PHY_DATA_1, chip_id);
+		phy_write_1bit(db, PHY_DATA_1);
 
 	/* Send start command(01) to Phy */
-	phy_write_1bit(ioaddr, PHY_DATA_0, chip_id);
-	phy_write_1bit(ioaddr, PHY_DATA_1, chip_id);
+	phy_write_1bit(db, PHY_DATA_0);
+	phy_write_1bit(db, PHY_DATA_1);
 
 	/* Send write command(01) to Phy */
-	phy_write_1bit(ioaddr, PHY_DATA_0, chip_id);
-	phy_write_1bit(ioaddr, PHY_DATA_1, chip_id);
+	phy_write_1bit(db, PHY_DATA_0);
+	phy_write_1bit(db, PHY_DATA_1);
 
 	/* Send Phy address */
 	for (i = 0x10; i > 0; i = i >> 1)
-		phy_write_1bit(ioaddr, phy_addr & i ? PHY_DATA_1 : PHY_DATA_0, chip_id);
+		phy_write_1bit(db, phy_addr & i ? PHY_DATA_1 : PHY_DATA_0);
 
 	/* Send register address */
 	for (i = 0x10; i > 0; i = i >> 1)
-		phy_write_1bit(ioaddr, offset & i ? PHY_DATA_1 : PHY_DATA_0, chip_id);
+		phy_write_1bit(db, offset & i ? PHY_DATA_1 : PHY_DATA_0);
 
 	/* written trasnition */
-	phy_write_1bit(ioaddr, PHY_DATA_1, chip_id);
-	phy_write_1bit(ioaddr, PHY_DATA_0, chip_id);
+	phy_write_1bit(db, PHY_DATA_1);
+	phy_write_1bit(db, PHY_DATA_0);
 
 	/* Write a word data to PHY controller */
-	for ( i = 0x8000; i > 0; i >>= 1)
-		phy_write_1bit(ioaddr, phy_data & i ? PHY_DATA_1 : PHY_DATA_0, chip_id);
-
+	for (i = 0x8000; i > 0; i >>= 1)
+		phy_write_1bit(db, phy_data & i ? PHY_DATA_1 : PHY_DATA_0);
 }
 
-
-/*
- *	Read a word data from phy register
- */
-
-static u16 phy_read(unsigned long iobase, u8 phy_addr, u8 offset, u32 chip_id)
+static u16 phy_readby_cr9(struct uli526x_board_info *db, u8 phy_addr, u8 offset)
 {
-	int i;
 	u16 phy_data;
-	unsigned long ioaddr;
-
-	if(chip_id == PCI_ULI5263_ID)
-		return phy_readby_cr10(iobase, phy_addr, offset);
-	/* M5261/M5263 Chip */
-	ioaddr = iobase + DCR9;
+	int i;
 
 	/* Send 33 synchronization clock to Phy controller */
 	for (i = 0; i < 35; i++)
-		phy_write_1bit(ioaddr, PHY_DATA_1, chip_id);
+		phy_write_1bit(db, PHY_DATA_1);
 
 	/* Send start command(01) to Phy */
-	phy_write_1bit(ioaddr, PHY_DATA_0, chip_id);
-	phy_write_1bit(ioaddr, PHY_DATA_1, chip_id);
+	phy_write_1bit(db, PHY_DATA_0);
+	phy_write_1bit(db, PHY_DATA_1);
 
 	/* Send read command(10) to Phy */
-	phy_write_1bit(ioaddr, PHY_DATA_1, chip_id);
-	phy_write_1bit(ioaddr, PHY_DATA_0, chip_id);
+	phy_write_1bit(db, PHY_DATA_1);
+	phy_write_1bit(db, PHY_DATA_0);
 
 	/* Send Phy address */
 	for (i = 0x10; i > 0; i = i >> 1)
-		phy_write_1bit(ioaddr, phy_addr & i ? PHY_DATA_1 : PHY_DATA_0, chip_id);
+		phy_write_1bit(db, phy_addr & i ? PHY_DATA_1 : PHY_DATA_0);
 
 	/* Send register address */
 	for (i = 0x10; i > 0; i = i >> 1)
-		phy_write_1bit(ioaddr, offset & i ? PHY_DATA_1 : PHY_DATA_0, chip_id);
+		phy_write_1bit(db, offset & i ? PHY_DATA_1 : PHY_DATA_0);
 
 	/* Skip transition state */
-	phy_read_1bit(ioaddr, chip_id);
+	phy_read_1bit(db);
 
 	/* read 16bit data */
 	for (phy_data = 0, i = 0; i < 16; i++) {
 		phy_data <<= 1;
-		phy_data |= phy_read_1bit(ioaddr, chip_id);
+		phy_data |= phy_read_1bit(db);
 	}
 
 	return phy_data;
 }
 
-static u16 phy_readby_cr10(unsigned long iobase, u8 phy_addr, u8 offset)
+static u16 phy_readby_cr10(struct uli526x_board_info *db, u8 phy_addr,
+			   u8 offset)
 {
-	unsigned long ioaddr,cr10_value;
+	void __iomem *ioaddr = db->ioaddr;
+	u32 cr10_value = phy_addr;
 
-	ioaddr = iobase + DCR10;
-	cr10_value = phy_addr;
-	cr10_value = (cr10_value<<5) + offset;
-	cr10_value = (cr10_value<<16) + 0x08000000;
-	outl(cr10_value,ioaddr);
+	cr10_value = (cr10_value <<  5) + offset;
+	cr10_value = (cr10_value << 16) + 0x08000000;
+	uw32(DCR10, cr10_value);
 	udelay(1);
-	while(1)
-	{
-		cr10_value = inl(ioaddr);
-		if(cr10_value&0x10000000)
+	while (1) {
+		cr10_value = ur32(DCR10);
+		if (cr10_value & 0x10000000)
 			break;
 	}
 	return cr10_value & 0x0ffff;
 }
 
-static void phy_writeby_cr10(unsigned long iobase, u8 phy_addr, u8 offset, u16 phy_data)
+static void phy_writeby_cr10(struct uli526x_board_info *db, u8 phy_addr,
+			     u8 offset, u16 phy_data)
 {
-	unsigned long ioaddr,cr10_value;
+	void __iomem *ioaddr = db->ioaddr;
+	u32 cr10_value = phy_addr;
 
-	ioaddr = iobase + DCR10;
-	cr10_value = phy_addr;
-	cr10_value = (cr10_value<<5) + offset;
-	cr10_value = (cr10_value<<16) + 0x04000000 + phy_data;
-	outl(cr10_value,ioaddr);
+	cr10_value = (cr10_value <<  5) + offset;
+	cr10_value = (cr10_value << 16) + 0x04000000 + phy_data;
+	uw32(DCR10, cr10_value);
 	udelay(1);
 }
 /*
  *	Write one bit data to Phy Controller
  */
 
-static void phy_write_1bit(unsigned long ioaddr, u32 phy_data, u32 chip_id)
+static void phy_write_1bit(struct uli526x_board_info *db, u32 data)
 {
-	outl(phy_data , ioaddr);			/* MII Clock Low */
+	void __iomem *ioaddr = db->ioaddr;
+
+	uw32(DCR9, data);		/* MII Clock Low */
 	udelay(1);
-	outl(phy_data  | MDCLKH, ioaddr);	/* MII Clock High */
+	uw32(DCR9, data | MDCLKH);	/* MII Clock High */
 	udelay(1);
-	outl(phy_data , ioaddr);			/* MII Clock Low */
+	uw32(DCR9, data);		/* MII Clock Low */
 	udelay(1);
 }
 
@@ -1763,14 +1761,15 @@ static void phy_write_1bit(unsigned long ioaddr, u32 phy_data, u32 chip_id)
  *	Read one bit phy data from PHY controller
  */
 
-static u16 phy_read_1bit(unsigned long ioaddr, u32 chip_id)
+static u16 phy_read_1bit(struct uli526x_board_info *db)
 {
+	void __iomem *ioaddr = db->ioaddr;
 	u16 phy_data;
 
-	outl(0x50000 , ioaddr);
+	uw32(DCR9, 0x50000);
 	udelay(1);
-	phy_data = ( inl(ioaddr) >> 19 ) & 0x1;
-	outl(0x40000 , ioaddr);
+	phy_data = (ur32(DCR9) >> 19) & 0x1;
+	uw32(DCR9, 0x40000);
 	udelay(1);
 
 	return phy_data;
diff --git a/drivers/net/ethernet/dec/tulip/winbond-840.c b/drivers/net/ethernet/dec/tulip/winbond-840.c
index 2ac6fff0363..4d1ffca83c8 100644
--- a/drivers/net/ethernet/dec/tulip/winbond-840.c
+++ b/drivers/net/ethernet/dec/tulip/winbond-840.c
@@ -400,9 +400,6 @@ static int __devinit w840_probe1 (struct pci_dev *pdev,
 	   No hold time required! */
 	iowrite32(0x00000001, ioaddr + PCIBusCfg);
 
-	dev->base_addr = (unsigned long)ioaddr;
-	dev->irq = irq;
-
 	np = netdev_priv(dev);
 	np->pci_dev = pdev;
 	np->chip_id = chip_idx;
@@ -635,17 +632,18 @@ static int netdev_open(struct net_device *dev)
 {
 	struct netdev_private *np = netdev_priv(dev);
 	void __iomem *ioaddr = np->base_addr;
+	const int irq = np->pci_dev->irq;
 	int i;
 
 	iowrite32(0x00000001, ioaddr + PCIBusCfg);		/* Reset */
 
 	netif_device_detach(dev);
-	i = request_irq(dev->irq, intr_handler, IRQF_SHARED, dev->name, dev);
+	i = request_irq(irq, intr_handler, IRQF_SHARED, dev->name, dev);
 	if (i)
 		goto out_err;
 
 	if (debug > 1)
-		netdev_dbg(dev, "w89c840_open() irq %d\n", dev->irq);
+		netdev_dbg(dev, "w89c840_open() irq %d\n", irq);
 
 	if((i=alloc_ringdesc(dev)))
 		goto out_err;
@@ -932,6 +930,7 @@ static void tx_timeout(struct net_device *dev)
 {
 	struct netdev_private *np = netdev_priv(dev);
 	void __iomem *ioaddr = np->base_addr;
+	const int irq = np->pci_dev->irq;
 
 	dev_warn(&dev->dev, "Transmit timed out, status %08x, resetting...\n",
 		 ioread32(ioaddr + IntrStatus));
@@ -951,7 +950,7 @@ static void tx_timeout(struct net_device *dev)
 	       np->cur_tx, np->dirty_tx, np->tx_full, np->tx_q_bytes);
 	printk(KERN_DEBUG "Tx Descriptor addr %xh\n", ioread32(ioaddr+0x4C));
 
-	disable_irq(dev->irq);
+	disable_irq(irq);
 	spin_lock_irq(&np->lock);
 	/*
 	 * Under high load dirty_tx and the internal tx descriptor pointer
@@ -966,7 +965,7 @@ static void tx_timeout(struct net_device *dev)
 	init_rxtx_rings(dev);
 	init_registers(dev);
 	spin_unlock_irq(&np->lock);
-	enable_irq(dev->irq);
+	enable_irq(irq);
 
 	netif_wake_queue(dev);
 	dev->trans_start = jiffies; /* prevent tx timeout */
@@ -1500,7 +1499,7 @@ static int netdev_close(struct net_device *dev)
 	iowrite32(0x0000, ioaddr + IntrEnable);
 	spin_unlock_irq(&np->lock);
 
-	free_irq(dev->irq, dev);
+	free_irq(np->pci_dev->irq, dev);
 	wmb();
 	netif_device_attach(dev);
 
@@ -1589,7 +1588,7 @@ static int w840_suspend (struct pci_dev *pdev, pm_message_t state)
 		iowrite32(0, ioaddr + IntrEnable);
 		spin_unlock_irq(&np->lock);
 
-		synchronize_irq(dev->irq);
+		synchronize_irq(np->pci_dev->irq);
 		netif_tx_disable(dev);
 
 		np->stats.rx_missed_errors += ioread32(ioaddr + RxMissed) & 0xffff;
diff --git a/drivers/net/ethernet/dec/tulip/xircom_cb.c b/drivers/net/ethernet/dec/tulip/xircom_cb.c
index fdb329fe6e8..138bf83bc98 100644
--- a/drivers/net/ethernet/dec/tulip/xircom_cb.c
+++ b/drivers/net/ethernet/dec/tulip/xircom_cb.c
@@ -41,7 +41,9 @@ MODULE_DESCRIPTION("Xircom Cardbus ethernet driver");
 MODULE_AUTHOR("Arjan van de Ven <arjanv@redhat.com>");
 MODULE_LICENSE("GPL");
 
-
+#define xw32(reg, val)	iowrite32(val, ioaddr + (reg))
+#define xr32(reg)	ioread32(ioaddr + (reg))
+#define xr8(reg)	ioread8(ioaddr + (reg))
 
 /* IO registers on the card, offsets */
 #define CSR0	0x00
@@ -83,7 +85,7 @@ struct xircom_private {
 
 	struct sk_buff *tx_skb[4];
 
-	unsigned long io_port;
+	void __iomem *ioaddr;
 	int open;
 
 	/* transmit_used is the rotating counter that indicates which transmit
@@ -137,7 +139,7 @@ static int link_status(struct xircom_private *card);
 
 
 static DEFINE_PCI_DEVICE_TABLE(xircom_pci_table) = {
-	{0x115D, 0x0003, PCI_ANY_ID, PCI_ANY_ID,},
+	{ PCI_VDEVICE(XIRCOM, 0x0003), },
 	{0,},
 };
 MODULE_DEVICE_TABLE(pci, xircom_pci_table);
@@ -146,9 +148,7 @@ static struct pci_driver xircom_ops = {
 	.name		= "xircom_cb",
 	.id_table	= xircom_pci_table,
 	.probe		= xircom_probe,
-	.remove		= xircom_remove,
-	.suspend =NULL,
-	.resume =NULL
+	.remove		= __devexit_p(xircom_remove),
 };
 
 
@@ -192,15 +192,18 @@ static const struct net_device_ops netdev_ops = {
  */
 static int __devinit xircom_probe(struct pci_dev *pdev, const struct pci_device_id *id)
 {
+	struct device *d = &pdev->dev;
 	struct net_device *dev = NULL;
 	struct xircom_private *private;
 	unsigned long flags;
 	unsigned short tmp16;
+	int rc;
 
 	/* First do the PCI initialisation */
 
-	if (pci_enable_device(pdev))
-		return -ENODEV;
+	rc = pci_enable_device(pdev);
+	if (rc < 0)
+		goto out;
 
 	/* disable all powermanagement */
 	pci_write_config_dword(pdev, PCI_POWERMGMT, 0x0000);
@@ -211,11 +214,13 @@ static int __devinit xircom_probe(struct pci_dev *pdev, const struct pci_device_
 	pci_read_config_word (pdev,PCI_STATUS, &tmp16);
 	pci_write_config_word (pdev, PCI_STATUS,tmp16);
 
-	if (!request_region(pci_resource_start(pdev, 0), 128, "xircom_cb")) {
+	rc = pci_request_regions(pdev, "xircom_cb");
+	if (rc < 0) {
 		pr_err("%s: failed to allocate io-region\n", __func__);
-		return -ENODEV;
+		goto err_disable;
 	}
 
+	rc = -ENOMEM;
 	/*
 	   Before changing the hardware, allocate the memory.
 	   This way, we can fail gracefully if not enough memory
@@ -223,17 +228,21 @@ static int __devinit xircom_probe(struct pci_dev *pdev, const struct pci_device_
 	 */
 	dev = alloc_etherdev(sizeof(struct xircom_private));
 	if (!dev)
-		goto device_fail;
+		goto err_release;
 
 	private = netdev_priv(dev);
 
 	/* Allocate the send/receive buffers */
-	private->rx_buffer = pci_alloc_consistent(pdev,8192,&private->rx_dma_handle);
+	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__);
 		goto rx_buf_fail;
 	}
-	private->tx_buffer = pci_alloc_consistent(pdev,8192,&private->tx_dma_handle);
+	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__);
 		goto tx_buf_fail;
@@ -244,10 +253,13 @@ static int __devinit xircom_probe(struct pci_dev *pdev, const struct pci_device_
 
 	private->dev = dev;
 	private->pdev = pdev;
-	private->io_port = pci_resource_start(pdev, 0);
+
+	/* IO range. */
+	private->ioaddr = pci_iomap(pdev, 0, 0);
+	if (!private->ioaddr)
+		goto reg_fail;
+
 	spin_lock_init(&private->lock);
-	dev->irq = pdev->irq;
-	dev->base_addr = private->io_port;
 
 	initialize_card(private);
 	read_mac_address(private);
@@ -256,9 +268,10 @@ static int __devinit xircom_probe(struct pci_dev *pdev, const struct pci_device_
 	dev->netdev_ops = &netdev_ops;
 	pci_set_drvdata(pdev, dev);
 
-	if (register_netdev(dev)) {
+	rc = register_netdev(dev);
+	if (rc < 0) {
 		pr_err("%s: netdevice registration failed\n", __func__);
-		goto reg_fail;
+		goto err_unmap;
 	}
 
 	netdev_info(dev, "Xircom cardbus revision %i at irq %i\n",
@@ -273,17 +286,23 @@ static int __devinit xircom_probe(struct pci_dev *pdev, const struct pci_device_
 	spin_unlock_irqrestore(&private->lock,flags);
 
 	trigger_receive(private);
+out:
+	return rc;
 
-	return 0;
-
+err_unmap:
+	pci_iounmap(pdev, private->ioaddr);
 reg_fail:
-	kfree(private->tx_buffer);
+	pci_set_drvdata(pdev, NULL);
+	dma_free_coherent(d, 8192, private->tx_buffer, private->tx_dma_handle);
 tx_buf_fail:
-	kfree(private->rx_buffer);
+	dma_free_coherent(d, 8192, private->rx_buffer, private->rx_dma_handle);
 rx_buf_fail:
 	free_netdev(dev);
-device_fail:
-	return -ENODEV;
+err_release:
+	pci_release_regions(pdev);
+err_disable:
+	pci_disable_device(pdev);
+	goto out;
 }
 
 
@@ -297,25 +316,28 @@ static void __devexit xircom_remove(struct pci_dev *pdev)
 {
 	struct net_device *dev = pci_get_drvdata(pdev);
 	struct xircom_private *card = netdev_priv(dev);
+	struct device *d = &pdev->dev;
 
-	pci_free_consistent(pdev,8192,card->rx_buffer,card->rx_dma_handle);
-	pci_free_consistent(pdev,8192,card->tx_buffer,card->tx_dma_handle);
-
-	release_region(dev->base_addr, 128);
 	unregister_netdev(dev);
-	free_netdev(dev);
+	pci_iounmap(pdev, card->ioaddr);
 	pci_set_drvdata(pdev, NULL);
+	dma_free_coherent(d, 8192, card->tx_buffer, card->tx_dma_handle);
+	dma_free_coherent(d, 8192, card->rx_buffer, card->rx_dma_handle);
+	free_netdev(dev);
+	pci_release_regions(pdev);
+	pci_disable_device(pdev);
 }
 
 static irqreturn_t xircom_interrupt(int irq, void *dev_instance)
 {
 	struct net_device *dev = (struct net_device *) dev_instance;
 	struct xircom_private *card = netdev_priv(dev);
+	void __iomem *ioaddr = card->ioaddr;
 	unsigned int status;
 	int i;
 
 	spin_lock(&card->lock);
-	status = inl(card->io_port+CSR5);
+	status = xr32(CSR5);
 
 #if defined DEBUG && DEBUG > 1
 	print_binary(status);
@@ -345,7 +367,7 @@ static irqreturn_t xircom_interrupt(int irq, void *dev_instance)
 	/* Clear all remaining interrupts */
 	status |= 0xffffffff; /* FIXME: make this clear only the
 				        real existing bits */
-	outl(status,card->io_port+CSR5);
+	xw32(CSR5, status);
 
 
 	for (i=0;i<NUMDESCRIPTORS;i++)
@@ -423,11 +445,11 @@ static netdev_tx_t xircom_start_xmit(struct sk_buff *skb,
 static int xircom_open(struct net_device *dev)
 {
 	struct xircom_private *xp = netdev_priv(dev);
+	const int irq = xp->pdev->irq;
 	int retval;
 
-	netdev_info(dev, "xircom cardbus adaptor found, using irq %i\n",
-		    dev->irq);
-	retval = request_irq(dev->irq, xircom_interrupt, IRQF_SHARED, dev->name, dev);
+	netdev_info(dev, "xircom cardbus adaptor found, using irq %i\n", irq);
+	retval = request_irq(irq, xircom_interrupt, IRQF_SHARED, dev->name, dev);
 	if (retval)
 		return retval;
 
@@ -459,7 +481,7 @@ static int xircom_close(struct net_device *dev)
 	spin_unlock_irqrestore(&card->lock,flags);
 
 	card->open = 0;
-	free_irq(dev->irq,dev);
+	free_irq(card->pdev->irq, dev);
 
 	return 0;
 
@@ -469,35 +491,39 @@ static int xircom_close(struct net_device *dev)
 #ifdef CONFIG_NET_POLL_CONTROLLER
 static void xircom_poll_controller(struct net_device *dev)
 {
-	disable_irq(dev->irq);
-	xircom_interrupt(dev->irq, dev);
-	enable_irq(dev->irq);
+	struct xircom_private *xp = netdev_priv(dev);
+	const int irq = xp->pdev->irq;
+
+	disable_irq(irq);
+	xircom_interrupt(irq, dev);
+	enable_irq(irq);
 }
 #endif
 
 
 static void initialize_card(struct xircom_private *card)
 {
-	unsigned int val;
+	void __iomem *ioaddr = card->ioaddr;
 	unsigned long flags;
+	u32 val;
 
 	spin_lock_irqsave(&card->lock, flags);
 
 	/* First: reset the card */
-	val = inl(card->io_port + CSR0);
+	val = xr32(CSR0);
 	val |= 0x01;		/* Software reset */
-	outl(val, card->io_port + CSR0);
+	xw32(CSR0, val);
 
 	udelay(100);		/* give the card some time to reset */
 
-	val = inl(card->io_port + CSR0);
+	val = xr32(CSR0);
 	val &= ~0x01;		/* disable Software reset */
-	outl(val, card->io_port + CSR0);
+	xw32(CSR0, val);
 
 
 	val = 0;		/* Value 0x00 is a safe and conservative value
 				   for the PCI configuration settings */
-	outl(val, card->io_port + CSR0);
+	xw32(CSR0, val);
 
 
 	disable_all_interrupts(card);
@@ -515,10 +541,9 @@ ignored; I chose zero.
 */
 static void trigger_transmit(struct xircom_private *card)
 {
-	unsigned int val;
+	void __iomem *ioaddr = card->ioaddr;
 
-	val = 0;
-	outl(val, card->io_port + CSR1);
+	xw32(CSR1, 0);
 }
 
 /*
@@ -530,10 +555,9 @@ ignored; I chose zero.
 */
 static void trigger_receive(struct xircom_private *card)
 {
-	unsigned int val;
+	void __iomem *ioaddr = card->ioaddr;
 
-	val = 0;
-	outl(val, card->io_port + CSR2);
+	xw32(CSR2, 0);
 }
 
 /*
@@ -542,6 +566,7 @@ descriptors and programs the addresses into the card.
 */
 static void setup_descriptors(struct xircom_private *card)
 {
+	void __iomem *ioaddr = card->ioaddr;
 	u32 address;
 	int i;
 
@@ -571,7 +596,7 @@ static void setup_descriptors(struct xircom_private *card)
 	wmb();
 	/* Write the receive descriptor ring address to the card */
 	address = card->rx_dma_handle;
-	outl(address, card->io_port + CSR3);	/* Receive descr list address */
+	xw32(CSR3, address);	/* Receive descr list address */
 
 
 	/* transmit descriptors */
@@ -596,7 +621,7 @@ static void setup_descriptors(struct xircom_private *card)
 	wmb();
 	/* wite the transmit descriptor ring to the card */
 	address = card->tx_dma_handle;
-	outl(address, card->io_port + CSR4);	/* xmit descr list address */
+	xw32(CSR4, address);	/* xmit descr list address */
 }
 
 /*
@@ -605,11 +630,12 @@ valid by setting the address in the card to 0x00.
 */
 static void remove_descriptors(struct xircom_private *card)
 {
+	void __iomem *ioaddr = card->ioaddr;
 	unsigned int val;
 
 	val = 0;
-	outl(val, card->io_port + CSR3);	/* Receive descriptor address */
-	outl(val, card->io_port + CSR4);	/* Send descriptor address */
+	xw32(CSR3, val);	/* Receive descriptor address */
+	xw32(CSR4, val);	/* Send descriptor address */
 }
 
 /*
@@ -620,17 +646,17 @@ This function also clears the status-bit.
 */
 static int link_status_changed(struct xircom_private *card)
 {
+	void __iomem *ioaddr = card->ioaddr;
 	unsigned int val;
 
-	val = inl(card->io_port + CSR5);	/* Status register */
-
-	if ((val & (1 << 27)) == 0)		/* no change */
+	val = xr32(CSR5);	/* Status register */
+	if (!(val & (1 << 27)))	/* no change */
 		return 0;
 
 	/* clear the event by writing a 1 to the bit in the
 	   status register. */
 	val = (1 << 27);
-	outl(val, card->io_port + CSR5);
+	xw32(CSR5, val);
 
 	return 1;
 }
@@ -642,11 +668,9 @@ in a non-stopped state.
 */
 static int transmit_active(struct xircom_private *card)
 {
-	unsigned int val;
-
-	val = inl(card->io_port + CSR5);	/* Status register */
+	void __iomem *ioaddr = card->ioaddr;
 
-	if ((val & (7 << 20)) == 0)		/* transmitter disabled */
+	if (!(xr32(CSR5) & (7 << 20)))	/* transmitter disabled */
 		return 0;
 
 	return 1;
@@ -658,11 +682,9 @@ in a non-stopped state.
 */
 static int receive_active(struct xircom_private *card)
 {
-	unsigned int val;
-
-	val = inl(card->io_port + CSR5);	/* Status register */
+	void __iomem *ioaddr = card->ioaddr;
 
-	if ((val & (7 << 17)) == 0)		/* receiver disabled */
+	if (!(xr32(CSR5) & (7 << 17)))	/* receiver disabled */
 		return 0;
 
 	return 1;
@@ -680,10 +702,11 @@ must be called with the lock held and interrupts disabled.
 */
 static void activate_receiver(struct xircom_private *card)
 {
+	void __iomem *ioaddr = card->ioaddr;
 	unsigned int val;
 	int counter;
 
-	val = inl(card->io_port + CSR6);	/* Operation mode */
+	val = xr32(CSR6);	/* Operation mode */
 
 	/* If the "active" bit is set and the receiver is already
 	   active, no need to do the expensive thing */
@@ -692,7 +715,7 @@ static void activate_receiver(struct xircom_private *card)
 
 
 	val = val & ~2;		/* disable the receiver */
-	outl(val, card->io_port + CSR6);
+	xw32(CSR6, val);
 
 	counter = 10;
 	while (counter > 0) {
@@ -706,9 +729,9 @@ static void activate_receiver(struct xircom_private *card)
 	}
 
 	/* enable the receiver */
-	val = inl(card->io_port + CSR6);	/* Operation mode */
-	val = val | 2;				/* enable the receiver */
-	outl(val, card->io_port + CSR6);
+	val = xr32(CSR6);	/* Operation mode */
+	val = val | 2;		/* enable the receiver */
+	xw32(CSR6, val);
 
 	/* now wait for the card to activate again */
 	counter = 10;
@@ -733,12 +756,13 @@ must be called with the lock held and interrupts disabled.
 */
 static void deactivate_receiver(struct xircom_private *card)
 {
+	void __iomem *ioaddr = card->ioaddr;
 	unsigned int val;
 	int counter;
 
-	val = inl(card->io_port + CSR6);	/* Operation mode */
-	val = val & ~2;				/* disable the receiver */
-	outl(val, card->io_port + CSR6);
+	val = xr32(CSR6);	/* Operation mode */
+	val = val & ~2;		/* disable the receiver */
+	xw32(CSR6, val);
 
 	counter = 10;
 	while (counter > 0) {
@@ -765,10 +789,11 @@ must be called with the lock held and interrupts disabled.
 */
 static void activate_transmitter(struct xircom_private *card)
 {
+	void __iomem *ioaddr = card->ioaddr;
 	unsigned int val;
 	int counter;
 
-	val = inl(card->io_port + CSR6);	/* Operation mode */
+	val = xr32(CSR6);	/* Operation mode */
 
 	/* If the "active" bit is set and the receiver is already
 	   active, no need to do the expensive thing */
@@ -776,7 +801,7 @@ static void activate_transmitter(struct xircom_private *card)
 		return;
 
 	val = val & ~(1 << 13);	/* disable the transmitter */
-	outl(val, card->io_port + CSR6);
+	xw32(CSR6, val);
 
 	counter = 10;
 	while (counter > 0) {
@@ -791,9 +816,9 @@ static void activate_transmitter(struct xircom_private *card)
 	}
 
 	/* enable the transmitter */
-	val = inl(card->io_port + CSR6);	/* Operation mode */
+	val = xr32(CSR6);	/* Operation mode */
 	val = val | (1 << 13);	/* enable the transmitter */
-	outl(val, card->io_port + CSR6);
+	xw32(CSR6, val);
 
 	/* now wait for the card to activate again */
 	counter = 10;
@@ -818,12 +843,13 @@ must be called with the lock held and interrupts disabled.
 */
 static void deactivate_transmitter(struct xircom_private *card)
 {
+	void __iomem *ioaddr = card->ioaddr;
 	unsigned int val;
 	int counter;
 
-	val = inl(card->io_port + CSR6);	/* Operation mode */
+	val = xr32(CSR6);	/* Operation mode */
 	val = val & ~2;		/* disable the transmitter */
-	outl(val, card->io_port + CSR6);
+	xw32(CSR6, val);
 
 	counter = 20;
 	while (counter > 0) {
@@ -846,11 +872,12 @@ must be called with the lock held and interrupts disabled.
 */
 static void enable_transmit_interrupt(struct xircom_private *card)
 {
+	void __iomem *ioaddr = card->ioaddr;
 	unsigned int val;
 
-	val = inl(card->io_port + CSR7);	/* Interrupt enable register */
-	val |= 1;				/* enable the transmit interrupt */
-	outl(val, card->io_port + CSR7);
+	val = xr32(CSR7);	/* Interrupt enable register */
+	val |= 1;		/* enable the transmit interrupt */
+	xw32(CSR7, val);
 }
 
 
@@ -861,11 +888,12 @@ must be called with the lock held and interrupts disabled.
 */
 static void enable_receive_interrupt(struct xircom_private *card)
 {
+	void __iomem *ioaddr = card->ioaddr;
 	unsigned int val;
 
-	val = inl(card->io_port + CSR7);	/* Interrupt enable register */
-	val = val | (1 << 6);			/* enable the receive interrupt */
-	outl(val, card->io_port + CSR7);
+	val = xr32(CSR7);	/* Interrupt enable register */
+	val = val | (1 << 6);	/* enable the receive interrupt */
+	xw32(CSR7, val);
 }
 
 /*
@@ -875,11 +903,12 @@ must be called with the lock held and interrupts disabled.
 */
 static void enable_link_interrupt(struct xircom_private *card)
 {
+	void __iomem *ioaddr = card->ioaddr;
 	unsigned int val;
 
-	val = inl(card->io_port + CSR7);	/* Interrupt enable register */
-	val = val | (1 << 27);			/* enable the link status chage interrupt */
-	outl(val, card->io_port + CSR7);
+	val = xr32(CSR7);	/* Interrupt enable register */
+	val = val | (1 << 27);	/* enable the link status chage interrupt */
+	xw32(CSR7, val);
 }
 
 
@@ -891,10 +920,9 @@ must be called with the lock held and interrupts disabled.
 */
 static void disable_all_interrupts(struct xircom_private *card)
 {
-	unsigned int val;
+	void __iomem *ioaddr = card->ioaddr;
 
-	val = 0;				/* disable all interrupts */
-	outl(val, card->io_port + CSR7);
+	xw32(CSR7, 0);
 }
 
 /*
@@ -904,9 +932,10 @@ must be called with the lock held and interrupts disabled.
 */
 static void enable_common_interrupts(struct xircom_private *card)
 {
+	void __iomem *ioaddr = card->ioaddr;
 	unsigned int val;
 
-	val = inl(card->io_port + CSR7);	/* Interrupt enable register */
+	val = xr32(CSR7);	/* Interrupt enable register */
 	val |= (1<<16); /* Normal Interrupt Summary */
 	val |= (1<<15); /* Abnormal Interrupt Summary */
 	val |= (1<<13); /* Fatal bus error */
@@ -915,7 +944,7 @@ static void enable_common_interrupts(struct xircom_private *card)
 	val |= (1<<5);  /* Transmit Underflow */
 	val |= (1<<2);  /* Transmit Buffer Unavailable */
 	val |= (1<<1);  /* Transmit Process Stopped */
-	outl(val, card->io_port + CSR7);
+	xw32(CSR7, val);
 }
 
 /*
@@ -925,11 +954,12 @@ must be called with the lock held and interrupts disabled.
 */
 static int enable_promisc(struct xircom_private *card)
 {
+	void __iomem *ioaddr = card->ioaddr;
 	unsigned int val;
 
-	val = inl(card->io_port + CSR6);
+	val = xr32(CSR6);
 	val = val | (1 << 6);
-	outl(val, card->io_port + CSR6);
+	xw32(CSR6, val);
 
 	return 1;
 }
@@ -944,13 +974,16 @@ Must be called in locked state with interrupts disabled
 */
 static int link_status(struct xircom_private *card)
 {
-	unsigned int val;
+	void __iomem *ioaddr = card->ioaddr;
+	u8 val;
 
-	val = inb(card->io_port + CSR12);
+	val = xr8(CSR12);
 
-	if (!(val&(1<<2)))  /* bit 2 is 0 for 10mbit link, 1 for not an 10mbit link */
+	/* bit 2 is 0 for 10mbit link, 1 for not an 10mbit link */
+	if (!(val & (1 << 2)))
 		return 10;
-	if (!(val&(1<<1)))  /* bit 1 is 0 for 100mbit link, 1 for not an 100mbit link */
+	/* bit 1 is 0 for 100mbit link, 1 for not an 100mbit link */
+	if (!(val & (1 << 1)))
 		return 100;
 
 	/* If we get here -> no link at all */
@@ -969,29 +1002,31 @@ static int link_status(struct xircom_private *card)
  */
 static void read_mac_address(struct xircom_private *card)
 {
-	unsigned char j, tuple, link, data_id, data_count;
+	void __iomem *ioaddr = card->ioaddr;
 	unsigned long flags;
+	u8 link;
 	int i;
 
 	spin_lock_irqsave(&card->lock, flags);
 
-	outl(1 << 12, card->io_port + CSR9);	/* enable boot rom access */
+	xw32(CSR9, 1 << 12);	/* enable boot rom access */
 	for (i = 0x100; i < 0x1f7; i += link + 2) {
-		outl(i, card->io_port + CSR10);
-		tuple = inl(card->io_port + CSR9) & 0xff;
-		outl(i + 1, card->io_port + CSR10);
-		link = inl(card->io_port + CSR9) & 0xff;
-		outl(i + 2, card->io_port + CSR10);
-		data_id = inl(card->io_port + CSR9) & 0xff;
-		outl(i + 3, card->io_port + CSR10);
-		data_count = inl(card->io_port + CSR9) & 0xff;
+		u8 tuple, data_id, data_count;
+
+		xw32(CSR10, i);
+		tuple = xr32(CSR9);
+		xw32(CSR10, i + 1);
+		link = xr32(CSR9);
+		xw32(CSR10, i + 2);
+		data_id = xr32(CSR9);
+		xw32(CSR10, i + 3);
+		data_count = xr32(CSR9);
 		if ((tuple == 0x22) && (data_id == 0x04) && (data_count == 0x06)) {
-			/*
-			 * This is it.  We have the data we want.
-			 */
+			int j;
+
 			for (j = 0; j < 6; j++) {
-				outl(i + j + 4, card->io_port + CSR10);
-				card->dev->dev_addr[j] = inl(card->io_port + CSR9) & 0xff;
+				xw32(CSR10, i + j + 4);
+				card->dev->dev_addr[j] = xr32(CSR9) & 0xff;
 			}
 			break;
 		} else if (link == 0) {
@@ -1010,6 +1045,7 @@ static void read_mac_address(struct xircom_private *card)
  */
 static void transceiver_voodoo(struct xircom_private *card)
 {
+	void __iomem *ioaddr = card->ioaddr;
 	unsigned long flags;
 
 	/* disable all powermanagement */
@@ -1019,14 +1055,14 @@ static void transceiver_voodoo(struct xircom_private *card)
 
 	spin_lock_irqsave(&card->lock, flags);
 
-	outl(0x0008, card->io_port + CSR15);
-        udelay(25);
-        outl(0xa8050000, card->io_port + CSR15);
-        udelay(25);
-        outl(0xa00f0000, card->io_port + CSR15);
-        udelay(25);
+	xw32(CSR15, 0x0008);
+	udelay(25);
+	xw32(CSR15, 0xa8050000);
+	udelay(25);
+	xw32(CSR15, 0xa00f0000);
+	udelay(25);
 
-        spin_unlock_irqrestore(&card->lock, flags);
+	spin_unlock_irqrestore(&card->lock, flags);
 
 	netif_start_queue(card->dev);
 }
diff --git a/drivers/net/ethernet/dlink/dl2k.c b/drivers/net/ethernet/dlink/dl2k.c
index 2e09edb9cdf..a059f0c27e2 100644
--- a/drivers/net/ethernet/dlink/dl2k.c
+++ b/drivers/net/ethernet/dlink/dl2k.c
@@ -16,6 +16,13 @@
 #include "dl2k.h"
 #include <linux/dma-mapping.h>
 
+#define dw32(reg, val)	iowrite32(val, ioaddr + (reg))
+#define dw16(reg, val)	iowrite16(val, ioaddr + (reg))
+#define dw8(reg, val)	iowrite8(val, ioaddr + (reg))
+#define dr32(reg)	ioread32(ioaddr + (reg))
+#define dr16(reg)	ioread16(ioaddr + (reg))
+#define dr8(reg)	ioread8(ioaddr + (reg))
+
 static char version[] __devinitdata =
       KERN_INFO DRV_NAME " " DRV_VERSION " " DRV_RELDATE "\n";
 #define MAX_UNITS 8
@@ -49,8 +56,13 @@ module_param(tx_coalesce, int, 0); /* HW xmit count each TxDMAComplete */
 /* Enable the default interrupts */
 #define DEFAULT_INTR (RxDMAComplete | HostError | IntRequested | TxDMAComplete| \
        UpdateStats | LinkEvent)
-#define EnableInt() \
-writew(DEFAULT_INTR, ioaddr + IntEnable)
+
+static void dl2k_enable_int(struct netdev_private *np)
+{
+	void __iomem *ioaddr = np->ioaddr;
+
+	dw16(IntEnable, DEFAULT_INTR);
+}
 
 static const int max_intrloop = 50;
 static const int multicast_filter_limit = 0x40;
@@ -73,7 +85,7 @@ static int rio_ioctl (struct net_device *dev, struct ifreq *rq, int cmd);
 static int rio_close (struct net_device *dev);
 static int find_miiphy (struct net_device *dev);
 static int parse_eeprom (struct net_device *dev);
-static int read_eeprom (long ioaddr, int eep_addr);
+static int read_eeprom (struct netdev_private *, int eep_addr);
 static int mii_wait_link (struct net_device *dev, int wait);
 static int mii_set_media (struct net_device *dev);
 static int mii_get_media (struct net_device *dev);
@@ -106,7 +118,7 @@ rio_probe1 (struct pci_dev *pdev, const struct pci_device_id *ent)
 	static int card_idx;
 	int chip_idx = ent->driver_data;
 	int err, irq;
-	long ioaddr;
+	void __iomem *ioaddr;
 	static int version_printed;
 	void *ring_space;
 	dma_addr_t ring_dma;
@@ -124,26 +136,29 @@ rio_probe1 (struct pci_dev *pdev, const struct pci_device_id *ent)
 		goto err_out_disable;
 
 	pci_set_master (pdev);
+
+	err = -ENOMEM;
+
 	dev = alloc_etherdev (sizeof (*np));
-	if (!dev) {
-		err = -ENOMEM;
+	if (!dev)
 		goto err_out_res;
-	}
 	SET_NETDEV_DEV(dev, &pdev->dev);
 
-#ifdef MEM_MAPPING
-	ioaddr = pci_resource_start (pdev, 1);
-	ioaddr = (long) ioremap (ioaddr, RIO_IO_SIZE);
-	if (!ioaddr) {
-		err = -ENOMEM;
+	np = netdev_priv(dev);
+
+	/* IO registers range. */
+	ioaddr = pci_iomap(pdev, 0, 0);
+	if (!ioaddr)
 		goto err_out_dev;
-	}
-#else
-	ioaddr = pci_resource_start (pdev, 0);
+	np->eeprom_addr = ioaddr;
+
+#ifdef MEM_MAPPING
+	/* MM registers range. */
+	ioaddr = pci_iomap(pdev, 1, 0);
+	if (!ioaddr)
+		goto err_out_iounmap;
 #endif
-	dev->base_addr = ioaddr;
-	dev->irq = irq;
-	np = netdev_priv(dev);
+	np->ioaddr = ioaddr;
 	np->chip_id = chip_idx;
 	np->pdev = pdev;
 	spin_lock_init (&np->tx_lock);
@@ -239,7 +254,7 @@ rio_probe1 (struct pci_dev *pdev, const struct pci_device_id *ent)
 		goto err_out_unmap_rx;
 
 	/* Fiber device? */
-	np->phy_media = (readw(ioaddr + ASICCtrl) & PhyMedia) ? 1 : 0;
+	np->phy_media = (dr16(ASICCtrl) & PhyMedia) ? 1 : 0;
 	np->link_status = 0;
 	/* Set media and reset PHY */
 	if (np->phy_media) {
@@ -276,22 +291,20 @@ rio_probe1 (struct pci_dev *pdev, const struct pci_device_id *ent)
 		printk(KERN_INFO "vlan(id):\t%d\n", np->vlan);
 	return 0;
 
-      err_out_unmap_rx:
+err_out_unmap_rx:
 	pci_free_consistent (pdev, RX_TOTAL_SIZE, np->rx_ring, np->rx_ring_dma);
-      err_out_unmap_tx:
+err_out_unmap_tx:
 	pci_free_consistent (pdev, TX_TOTAL_SIZE, np->tx_ring, np->tx_ring_dma);
-      err_out_iounmap:
+err_out_iounmap:
 #ifdef MEM_MAPPING
-	iounmap ((void *) ioaddr);
-
-      err_out_dev:
+	pci_iounmap(pdev, np->ioaddr);
 #endif
+	pci_iounmap(pdev, np->eeprom_addr);
+err_out_dev:
 	free_netdev (dev);
-
-      err_out_res:
+err_out_res:
 	pci_release_regions (pdev);
-
-      err_out_disable:
+err_out_disable:
 	pci_disable_device (pdev);
 	return err;
 }
@@ -299,11 +312,9 @@ rio_probe1 (struct pci_dev *pdev, const struct pci_device_id *ent)
 static int
 find_miiphy (struct net_device *dev)
 {
+	struct netdev_private *np = netdev_priv(dev);
 	int i, phy_found = 0;
-	struct netdev_private *np;
-	long ioaddr;
 	np = netdev_priv(dev);
-	ioaddr = dev->base_addr;
 	np->phy_addr = 1;
 
 	for (i = 31; i >= 0; i--) {
@@ -323,26 +334,19 @@ find_miiphy (struct net_device *dev)
 static int
 parse_eeprom (struct net_device *dev)
 {
+	struct netdev_private *np = netdev_priv(dev);
+	void __iomem *ioaddr = np->ioaddr;
 	int i, j;
-	long ioaddr = dev->base_addr;
 	u8 sromdata[256];
 	u8 *psib;
 	u32 crc;
 	PSROM_t psrom = (PSROM_t) sromdata;
-	struct netdev_private *np = netdev_priv(dev);
 
 	int cid, next;
 
-#ifdef	MEM_MAPPING
-	ioaddr = pci_resource_start (np->pdev, 0);
-#endif
-	/* Read eeprom */
-	for (i = 0; i < 128; i++) {
-		((__le16 *) sromdata)[i] = cpu_to_le16(read_eeprom (ioaddr, i));
-	}
-#ifdef	MEM_MAPPING
-	ioaddr = dev->base_addr;
-#endif
+	for (i = 0; i < 128; i++)
+		((__le16 *) sromdata)[i] = cpu_to_le16(read_eeprom(np, i));
+
 	if (np->pdev->vendor == PCI_VENDOR_ID_DLINK) {	/* D-Link Only */
 		/* Check CRC */
 		crc = ~ether_crc_le (256 - 4, sromdata);
@@ -378,8 +382,7 @@ parse_eeprom (struct net_device *dev)
 			return 0;
 		case 2:	/* Duplex Polarity */
 			np->duplex_polarity = psib[i];
-			writeb (readb (ioaddr + PhyCtrl) | psib[i],
-				ioaddr + PhyCtrl);
+			dw8(PhyCtrl, dr8(PhyCtrl) | psib[i]);
 			break;
 		case 3:	/* Wake Polarity */
 			np->wake_polarity = psib[i];
@@ -407,59 +410,57 @@ static int
 rio_open (struct net_device *dev)
 {
 	struct netdev_private *np = netdev_priv(dev);
-	long ioaddr = dev->base_addr;
+	void __iomem *ioaddr = np->ioaddr;
+	const int irq = np->pdev->irq;
 	int i;
 	u16 macctrl;
 
-	i = request_irq (dev->irq, rio_interrupt, IRQF_SHARED, dev->name, dev);
+	i = request_irq(irq, rio_interrupt, IRQF_SHARED, dev->name, dev);
 	if (i)
 		return i;
 
 	/* Reset all logic functions */
-	writew (GlobalReset | DMAReset | FIFOReset | NetworkReset | HostReset,
-		ioaddr + ASICCtrl + 2);
+	dw16(ASICCtrl + 2,
+	     GlobalReset | DMAReset | FIFOReset | NetworkReset | HostReset);
 	mdelay(10);
 
 	/* DebugCtrl bit 4, 5, 9 must set */
-	writel (readl (ioaddr + DebugCtrl) | 0x0230, ioaddr + DebugCtrl);
+	dw32(DebugCtrl, dr32(DebugCtrl) | 0x0230);
 
 	/* Jumbo frame */
 	if (np->jumbo != 0)
-		writew (MAX_JUMBO+14, ioaddr + MaxFrameSize);
+		dw16(MaxFrameSize, MAX_JUMBO+14);
 
 	alloc_list (dev);
 
 	/* Get station address */
 	for (i = 0; i < 6; i++)
-		writeb (dev->dev_addr[i], ioaddr + StationAddr0 + i);
+		dw8(StationAddr0 + i, dev->dev_addr[i]);
 
 	set_multicast (dev);
 	if (np->coalesce) {
-		writel (np->rx_coalesce | np->rx_timeout << 16,
-			ioaddr + RxDMAIntCtrl);
+		dw32(RxDMAIntCtrl, np->rx_coalesce | np->rx_timeout << 16);
 	}
 	/* Set RIO to poll every N*320nsec. */
-	writeb (0x20, ioaddr + RxDMAPollPeriod);
-	writeb (0xff, ioaddr + TxDMAPollPeriod);
-	writeb (0x30, ioaddr + RxDMABurstThresh);
-	writeb (0x30, ioaddr + RxDMAUrgentThresh);
-	writel (0x0007ffff, ioaddr + RmonStatMask);
+	dw8(RxDMAPollPeriod, 0x20);
+	dw8(TxDMAPollPeriod, 0xff);
+	dw8(RxDMABurstThresh, 0x30);
+	dw8(RxDMAUrgentThresh, 0x30);
+	dw32(RmonStatMask, 0x0007ffff);
 	/* clear statistics */
 	clear_stats (dev);
 
 	/* VLAN supported */
 	if (np->vlan) {
 		/* priority field in RxDMAIntCtrl  */
-		writel (readl(ioaddr + RxDMAIntCtrl) | 0x7 << 10,
-			ioaddr + RxDMAIntCtrl);
+		dw32(RxDMAIntCtrl, dr32(RxDMAIntCtrl) | 0x7 << 10);
 		/* VLANId */
-		writew (np->vlan, ioaddr + VLANId);
+		dw16(VLANId, np->vlan);
 		/* Length/Type should be 0x8100 */
-		writel (0x8100 << 16 | np->vlan, ioaddr + VLANTag);
+		dw32(VLANTag, 0x8100 << 16 | np->vlan);
 		/* Enable AutoVLANuntagging, but disable AutoVLANtagging.
 		   VLAN information tagged by TFC' VID, CFI fields. */
-		writel (readl (ioaddr + MACCtrl) | AutoVLANuntagging,
-			ioaddr + MACCtrl);
+		dw32(MACCtrl, dr32(MACCtrl) | AutoVLANuntagging);
 	}
 
 	init_timer (&np->timer);
@@ -469,20 +470,18 @@ rio_open (struct net_device *dev)
 	add_timer (&np->timer);
 
 	/* Start Tx/Rx */
-	writel (readl (ioaddr + MACCtrl) | StatsEnable | RxEnable | TxEnable,
-			ioaddr + MACCtrl);
+	dw32(MACCtrl, dr32(MACCtrl) | StatsEnable | RxEnable | TxEnable);
 
 	macctrl = 0;
 	macctrl |= (np->vlan) ? AutoVLANuntagging : 0;
 	macctrl |= (np->full_duplex) ? DuplexSelect : 0;
 	macctrl |= (np->tx_flow) ? TxFlowControlEnable : 0;
 	macctrl |= (np->rx_flow) ? RxFlowControlEnable : 0;
-	writew(macctrl,	ioaddr + MACCtrl);
+	dw16(MACCtrl, macctrl);
 
 	netif_start_queue (dev);
 
-	/* Enable default interrupts */
-	EnableInt ();
+	dl2k_enable_int(np);
 	return 0;
 }
 
@@ -533,10 +532,11 @@ rio_timer (unsigned long data)
 static void
 rio_tx_timeout (struct net_device *dev)
 {
-	long ioaddr = dev->base_addr;
+	struct netdev_private *np = netdev_priv(dev);
+	void __iomem *ioaddr = np->ioaddr;
 
 	printk (KERN_INFO "%s: Tx timed out (%4.4x), is buffer full?\n",
-		dev->name, readl (ioaddr + TxStatus));
+		dev->name, dr32(TxStatus));
 	rio_free_tx(dev, 0);
 	dev->if_port = 0;
 	dev->trans_start = jiffies; /* prevent tx timeout */
@@ -547,6 +547,7 @@ static void
 alloc_list (struct net_device *dev)
 {
 	struct netdev_private *np = netdev_priv(dev);
+	void __iomem *ioaddr = np->ioaddr;
 	int i;
 
 	np->cur_rx = np->cur_tx = 0;
@@ -594,24 +595,23 @@ alloc_list (struct net_device *dev)
 	}
 
 	/* Set RFDListPtr */
-	writel (np->rx_ring_dma, dev->base_addr + RFDListPtr0);
-	writel (0, dev->base_addr + RFDListPtr1);
+	dw32(RFDListPtr0, np->rx_ring_dma);
+	dw32(RFDListPtr1, 0);
 }
 
 static netdev_tx_t
 start_xmit (struct sk_buff *skb, struct net_device *dev)
 {
 	struct netdev_private *np = netdev_priv(dev);
+	void __iomem *ioaddr = np->ioaddr;
 	struct netdev_desc *txdesc;
 	unsigned entry;
-	u32 ioaddr;
 	u64 tfc_vlan_tag = 0;
 
 	if (np->link_status == 0) {	/* Link Down */
 		dev_kfree_skb(skb);
 		return NETDEV_TX_OK;
 	}
-	ioaddr = dev->base_addr;
 	entry = np->cur_tx % TX_RING_SIZE;
 	np->tx_skbuff[entry] = skb;
 	txdesc = &np->tx_ring[entry];
@@ -646,9 +646,9 @@ start_xmit (struct sk_buff *skb, struct net_device *dev)
 					      (1 << FragCountShift));
 
 	/* TxDMAPollNow */
-	writel (readl (ioaddr + DMACtrl) | 0x00001000, ioaddr + DMACtrl);
+	dw32(DMACtrl, dr32(DMACtrl) | 0x00001000);
 	/* Schedule ISR */
-	writel(10000, ioaddr + CountDown);
+	dw32(CountDown, 10000);
 	np->cur_tx = (np->cur_tx + 1) % TX_RING_SIZE;
 	if ((np->cur_tx - np->old_tx + TX_RING_SIZE) % TX_RING_SIZE
 			< TX_QUEUE_LEN - 1 && np->speed != 10) {
@@ -658,10 +658,10 @@ start_xmit (struct sk_buff *skb, struct net_device *dev)
 	}
 
 	/* The first TFDListPtr */
-	if (readl (dev->base_addr + TFDListPtr0) == 0) {
-		writel (np->tx_ring_dma + entry * sizeof (struct netdev_desc),
-			dev->base_addr + TFDListPtr0);
-		writel (0, dev->base_addr + TFDListPtr1);
+	if (!dr32(TFDListPtr0)) {
+		dw32(TFDListPtr0, np->tx_ring_dma +
+		     entry * sizeof (struct netdev_desc));
+		dw32(TFDListPtr1, 0);
 	}
 
 	return NETDEV_TX_OK;
@@ -671,17 +671,15 @@ static irqreturn_t
 rio_interrupt (int irq, void *dev_instance)
 {
 	struct net_device *dev = dev_instance;
-	struct netdev_private *np;
+	struct netdev_private *np = netdev_priv(dev);
+	void __iomem *ioaddr = np->ioaddr;
 	unsigned int_status;
-	long ioaddr;
 	int cnt = max_intrloop;
 	int handled = 0;
 
-	ioaddr = dev->base_addr;
-	np = netdev_priv(dev);
 	while (1) {
-		int_status = readw (ioaddr + IntStatus);
-		writew (int_status, ioaddr + IntStatus);
+		int_status = dr16(IntStatus);
+		dw16(IntStatus, int_status);
 		int_status &= DEFAULT_INTR;
 		if (int_status == 0 || --cnt < 0)
 			break;
@@ -692,7 +690,7 @@ rio_interrupt (int irq, void *dev_instance)
 		/* TxDMAComplete interrupt */
 		if ((int_status & (TxDMAComplete|IntRequested))) {
 			int tx_status;
-			tx_status = readl (ioaddr + TxStatus);
+			tx_status = dr32(TxStatus);
 			if (tx_status & 0x01)
 				tx_error (dev, tx_status);
 			/* Free used tx skbuffs */
@@ -705,7 +703,7 @@ rio_interrupt (int irq, void *dev_instance)
 			rio_error (dev, int_status);
 	}
 	if (np->cur_tx != np->old_tx)
-		writel (100, ioaddr + CountDown);
+		dw32(CountDown, 100);
 	return IRQ_RETVAL(handled);
 }
 
@@ -765,13 +763,11 @@ rio_free_tx (struct net_device *dev, int irq)
 static void
 tx_error (struct net_device *dev, int tx_status)
 {
-	struct netdev_private *np;
-	long ioaddr = dev->base_addr;
+	struct netdev_private *np = netdev_priv(dev);
+	void __iomem *ioaddr = np->ioaddr;
 	int frame_id;
 	int i;
 
-	np = netdev_priv(dev);
-
 	frame_id = (tx_status & 0xffff0000);
 	printk (KERN_ERR "%s: Transmit error, TxStatus %4.4x, FrameId %d.\n",
 		dev->name, tx_status, frame_id);
@@ -779,23 +775,21 @@ tx_error (struct net_device *dev, int tx_status)
 	/* Ttransmit Underrun */
 	if (tx_status & 0x10) {
 		np->stats.tx_fifo_errors++;
-		writew (readw (ioaddr + TxStartThresh) + 0x10,
-			ioaddr + TxStartThresh);
+		dw16(TxStartThresh, dr16(TxStartThresh) + 0x10);
 		/* Transmit Underrun need to set TxReset, DMARest, FIFOReset */
-		writew (TxReset | DMAReset | FIFOReset | NetworkReset,
-			ioaddr + ASICCtrl + 2);
+		dw16(ASICCtrl + 2,
+		     TxReset | DMAReset | FIFOReset | NetworkReset);
 		/* Wait for ResetBusy bit clear */
 		for (i = 50; i > 0; i--) {
-			if ((readw (ioaddr + ASICCtrl + 2) & ResetBusy) == 0)
+			if (!(dr16(ASICCtrl + 2) & ResetBusy))
 				break;
 			mdelay (1);
 		}
 		rio_free_tx (dev, 1);
 		/* Reset TFDListPtr */
-		writel (np->tx_ring_dma +
-			np->old_tx * sizeof (struct netdev_desc),
-			dev->base_addr + TFDListPtr0);
-		writel (0, dev->base_addr + TFDListPtr1);
+		dw32(TFDListPtr0, np->tx_ring_dma +
+		     np->old_tx * sizeof (struct netdev_desc));
+		dw32(TFDListPtr1, 0);
 
 		/* Let TxStartThresh stay default value */
 	}
@@ -803,10 +797,10 @@ tx_error (struct net_device *dev, int tx_status)
 	if (tx_status & 0x04) {
 		np->stats.tx_fifo_errors++;
 		/* TxReset and clear FIFO */
-		writew (TxReset | FIFOReset, ioaddr + ASICCtrl + 2);
+		dw16(ASICCtrl + 2, TxReset | FIFOReset);
 		/* Wait reset done */
 		for (i = 50; i > 0; i--) {
-			if ((readw (ioaddr + ASICCtrl + 2) & ResetBusy) == 0)
+			if (!(dr16(ASICCtrl + 2) & ResetBusy))
 				break;
 			mdelay (1);
 		}
@@ -821,7 +815,7 @@ tx_error (struct net_device *dev, int tx_status)
 		np->stats.collisions++;
 #endif
 	/* Restart the Tx */
-	writel (readw (dev->base_addr + MACCtrl) | TxEnable, ioaddr + MACCtrl);
+	dw32(MACCtrl, dr16(MACCtrl) | TxEnable);
 }
 
 static int
@@ -931,8 +925,8 @@ receive_packet (struct net_device *dev)
 static void
 rio_error (struct net_device *dev, int int_status)
 {
-	long ioaddr = dev->base_addr;
 	struct netdev_private *np = netdev_priv(dev);
+	void __iomem *ioaddr = np->ioaddr;
 	u16 macctrl;
 
 	/* Link change event */
@@ -954,7 +948,7 @@ rio_error (struct net_device *dev, int int_status)
 				TxFlowControlEnable : 0;
 			macctrl |= (np->rx_flow) ?
 				RxFlowControlEnable : 0;
-			writew(macctrl,	ioaddr + MACCtrl);
+			dw16(MACCtrl, macctrl);
 			np->link_status = 1;
 			netif_carrier_on(dev);
 		} else {
@@ -974,7 +968,7 @@ rio_error (struct net_device *dev, int int_status)
 	if (int_status & HostError) {
 		printk (KERN_ERR "%s: HostError! IntStatus %4.4x.\n",
 			dev->name, int_status);
-		writew (GlobalReset | HostReset, ioaddr + ASICCtrl + 2);
+		dw16(ASICCtrl + 2, GlobalReset | HostReset);
 		mdelay (500);
 	}
 }
@@ -982,8 +976,8 @@ rio_error (struct net_device *dev, int int_status)
 static struct net_device_stats *
 get_stats (struct net_device *dev)
 {
-	long ioaddr = dev->base_addr;
 	struct netdev_private *np = netdev_priv(dev);
+	void __iomem *ioaddr = np->ioaddr;
 #ifdef MEM_MAPPING
 	int i;
 #endif
@@ -992,106 +986,107 @@ get_stats (struct net_device *dev)
 	/* All statistics registers need to be acknowledged,
 	   else statistic overflow could cause problems */
 
-	np->stats.rx_packets += readl (ioaddr + FramesRcvOk);
-	np->stats.tx_packets += readl (ioaddr + FramesXmtOk);
-	np->stats.rx_bytes += readl (ioaddr + OctetRcvOk);
-	np->stats.tx_bytes += readl (ioaddr + OctetXmtOk);
+	np->stats.rx_packets += dr32(FramesRcvOk);
+	np->stats.tx_packets += dr32(FramesXmtOk);
+	np->stats.rx_bytes += dr32(OctetRcvOk);
+	np->stats.tx_bytes += dr32(OctetXmtOk);
 
-	np->stats.multicast = readl (ioaddr + McstFramesRcvdOk);
-	np->stats.collisions += readl (ioaddr + SingleColFrames)
-			     +  readl (ioaddr + MultiColFrames);
+	np->stats.multicast = dr32(McstFramesRcvdOk);
+	np->stats.collisions += dr32(SingleColFrames)
+			     +  dr32(MultiColFrames);
 
 	/* detailed tx errors */
-	stat_reg = readw (ioaddr + FramesAbortXSColls);
+	stat_reg = dr16(FramesAbortXSColls);
 	np->stats.tx_aborted_errors += stat_reg;
 	np->stats.tx_errors += stat_reg;
 
-	stat_reg = readw (ioaddr + CarrierSenseErrors);
+	stat_reg = dr16(CarrierSenseErrors);
 	np->stats.tx_carrier_errors += stat_reg;
 	np->stats.tx_errors += stat_reg;
 
 	/* Clear all other statistic register. */
-	readl (ioaddr + McstOctetXmtOk);
-	readw (ioaddr + BcstFramesXmtdOk);
-	readl (ioaddr + McstFramesXmtdOk);
-	readw (ioaddr + BcstFramesRcvdOk);
-	readw (ioaddr + MacControlFramesRcvd);
-	readw (ioaddr + FrameTooLongErrors);
-	readw (ioaddr + InRangeLengthErrors);
-	readw (ioaddr + FramesCheckSeqErrors);
-	readw (ioaddr + FramesLostRxErrors);
-	readl (ioaddr + McstOctetXmtOk);
-	readl (ioaddr + BcstOctetXmtOk);
-	readl (ioaddr + McstFramesXmtdOk);
-	readl (ioaddr + FramesWDeferredXmt);
-	readl (ioaddr + LateCollisions);
-	readw (ioaddr + BcstFramesXmtdOk);
-	readw (ioaddr + MacControlFramesXmtd);
-	readw (ioaddr + FramesWEXDeferal);
+	dr32(McstOctetXmtOk);
+	dr16(BcstFramesXmtdOk);
+	dr32(McstFramesXmtdOk);
+	dr16(BcstFramesRcvdOk);
+	dr16(MacControlFramesRcvd);
+	dr16(FrameTooLongErrors);
+	dr16(InRangeLengthErrors);
+	dr16(FramesCheckSeqErrors);
+	dr16(FramesLostRxErrors);
+	dr32(McstOctetXmtOk);
+	dr32(BcstOctetXmtOk);
+	dr32(McstFramesXmtdOk);
+	dr32(FramesWDeferredXmt);
+	dr32(LateCollisions);
+	dr16(BcstFramesXmtdOk);
+	dr16(MacControlFramesXmtd);
+	dr16(FramesWEXDeferal);
 
 #ifdef MEM_MAPPING
 	for (i = 0x100; i <= 0x150; i += 4)
-		readl (ioaddr + i);
+		dr32(i);
 #endif
-	readw (ioaddr + TxJumboFrames);
-	readw (ioaddr + RxJumboFrames);
-	readw (ioaddr + TCPCheckSumErrors);
-	readw (ioaddr + UDPCheckSumErrors);
-	readw (ioaddr + IPCheckSumErrors);
+	dr16(TxJumboFrames);
+	dr16(RxJumboFrames);
+	dr16(TCPCheckSumErrors);
+	dr16(UDPCheckSumErrors);
+	dr16(IPCheckSumErrors);
 	return &np->stats;
 }
 
 static int
 clear_stats (struct net_device *dev)
 {
-	long ioaddr = dev->base_addr;
+	struct netdev_private *np = netdev_priv(dev);
+	void __iomem *ioaddr = np->ioaddr;
 #ifdef MEM_MAPPING
 	int i;
 #endif
 
 	/* All statistics registers need to be acknowledged,
 	   else statistic overflow could cause problems */
-	readl (ioaddr + FramesRcvOk);
-	readl (ioaddr + FramesXmtOk);
-	readl (ioaddr + OctetRcvOk);
-	readl (ioaddr + OctetXmtOk);
-
-	readl (ioaddr + McstFramesRcvdOk);
-	readl (ioaddr + SingleColFrames);
-	readl (ioaddr + MultiColFrames);
-	readl (ioaddr + LateCollisions);
+	dr32(FramesRcvOk);
+	dr32(FramesXmtOk);
+	dr32(OctetRcvOk);
+	dr32(OctetXmtOk);
+
+	dr32(McstFramesRcvdOk);
+	dr32(SingleColFrames);
+	dr32(MultiColFrames);
+	dr32(LateCollisions);
 	/* detailed rx errors */
-	readw (ioaddr + FrameTooLongErrors);
-	readw (ioaddr + InRangeLengthErrors);
-	readw (ioaddr + FramesCheckSeqErrors);
-	readw (ioaddr + FramesLostRxErrors);
+	dr16(FrameTooLongErrors);
+	dr16(InRangeLengthErrors);
+	dr16(FramesCheckSeqErrors);
+	dr16(FramesLostRxErrors);
 
 	/* detailed tx errors */
-	readw (ioaddr + FramesAbortXSColls);
-	readw (ioaddr + CarrierSenseErrors);
+	dr16(FramesAbortXSColls);
+	dr16(CarrierSenseErrors);
 
 	/* Clear all other statistic register. */
-	readl (ioaddr + McstOctetXmtOk);
-	readw (ioaddr + BcstFramesXmtdOk);
-	readl (ioaddr + McstFramesXmtdOk);
-	readw (ioaddr + BcstFramesRcvdOk);
-	readw (ioaddr + MacControlFramesRcvd);
-	readl (ioaddr + McstOctetXmtOk);
-	readl (ioaddr + BcstOctetXmtOk);
-	readl (ioaddr + McstFramesXmtdOk);
-	readl (ioaddr + FramesWDeferredXmt);
-	readw (ioaddr + BcstFramesXmtdOk);
-	readw (ioaddr + MacControlFramesXmtd);
-	readw (ioaddr + FramesWEXDeferal);
+	dr32(McstOctetXmtOk);
+	dr16(BcstFramesXmtdOk);
+	dr32(McstFramesXmtdOk);
+	dr16(BcstFramesRcvdOk);
+	dr16(MacControlFramesRcvd);
+	dr32(McstOctetXmtOk);
+	dr32(BcstOctetXmtOk);
+	dr32(McstFramesXmtdOk);
+	dr32(FramesWDeferredXmt);
+	dr16(BcstFramesXmtdOk);
+	dr16(MacControlFramesXmtd);
+	dr16(FramesWEXDeferal);
 #ifdef MEM_MAPPING
 	for (i = 0x100; i <= 0x150; i += 4)
-		readl (ioaddr + i);
+		dr32(i);
 #endif
-	readw (ioaddr + TxJumboFrames);
-	readw (ioaddr + RxJumboFrames);
-	readw (ioaddr + TCPCheckSumErrors);
-	readw (ioaddr + UDPCheckSumErrors);
-	readw (ioaddr + IPCheckSumErrors);
+	dr16(TxJumboFrames);
+	dr16(RxJumboFrames);
+	dr16(TCPCheckSumErrors);
+	dr16(UDPCheckSumErrors);
+	dr16(IPCheckSumErrors);
 	return 0;
 }
 
@@ -1114,10 +1109,10 @@ change_mtu (struct net_device *dev, int new_mtu)
 static void
 set_multicast (struct net_device *dev)
 {
-	long ioaddr = dev->base_addr;
+	struct netdev_private *np = netdev_priv(dev);
+	void __iomem *ioaddr = np->ioaddr;
 	u32 hash_table[2];
 	u16 rx_mode = 0;
-	struct netdev_private *np = netdev_priv(dev);
 
 	hash_table[0] = hash_table[1] = 0;
 	/* RxFlowcontrol DA: 01-80-C2-00-00-01. Hash index=0x39 */
@@ -1153,9 +1148,9 @@ set_multicast (struct net_device *dev)
 		rx_mode |= ReceiveVLANMatch;
 	}
 
-	writel (hash_table[0], ioaddr + HashTable0);
-	writel (hash_table[1], ioaddr + HashTable1);
-	writew (rx_mode, ioaddr + ReceiveMode);
+	dw32(HashTable0, hash_table[0]);
+	dw32(HashTable1, hash_table[1]);
+	dw16(ReceiveMode, rx_mode);
 }
 
 static void rio_get_drvinfo(struct net_device *dev, struct ethtool_drvinfo *info)
@@ -1284,15 +1279,15 @@ rio_ioctl (struct net_device *dev, struct ifreq *rq, int cmd)
 #define EEP_BUSY 0x8000
 /* Read the EEPROM word */
 /* We use I/O instruction to read/write eeprom to avoid fail on some machines */
-static int
-read_eeprom (long ioaddr, int eep_addr)
+static int read_eeprom(struct netdev_private *np, int eep_addr)
 {
+	void __iomem *ioaddr = np->eeprom_addr;
 	int i = 1000;
-	outw (EEP_READ | (eep_addr & 0xff), ioaddr + EepromCtrl);
+
+	dw16(EepromCtrl, EEP_READ | (eep_addr & 0xff));
 	while (i-- > 0) {
-		if (!(inw (ioaddr + EepromCtrl) & EEP_BUSY)) {
-			return inw (ioaddr + EepromData);
-		}
+		if (!(dr16(EepromCtrl) & EEP_BUSY))
+			return dr16(EepromData);
 	}
 	return 0;
 }
@@ -1302,38 +1297,40 @@ enum phy_ctrl_bits {
 	MII_DUPLEX = 0x08,
 };
 
-#define mii_delay() readb(ioaddr)
+#define mii_delay() dr8(PhyCtrl)
 static void
 mii_sendbit (struct net_device *dev, u32 data)
 {
-	long ioaddr = dev->base_addr + PhyCtrl;
-	data = (data) ? MII_DATA1 : 0;
-	data |= MII_WRITE;
-	data |= (readb (ioaddr) & 0xf8) | MII_WRITE;
-	writeb (data, ioaddr);
+	struct netdev_private *np = netdev_priv(dev);
+	void __iomem *ioaddr = np->ioaddr;
+
+	data = ((data) ? MII_DATA1 : 0) | (dr8(PhyCtrl) & 0xf8) | MII_WRITE;
+	dw8(PhyCtrl, data);
 	mii_delay ();
-	writeb (data | MII_CLK, ioaddr);
+	dw8(PhyCtrl, data | MII_CLK);
 	mii_delay ();
 }
 
 static int
 mii_getbit (struct net_device *dev)
 {
-	long ioaddr = dev->base_addr + PhyCtrl;
+	struct netdev_private *np = netdev_priv(dev);
+	void __iomem *ioaddr = np->ioaddr;
 	u8 data;
 
-	data = (readb (ioaddr) & 0xf8) | MII_READ;
-	writeb (data, ioaddr);
+	data = (dr8(PhyCtrl) & 0xf8) | MII_READ;
+	dw8(PhyCtrl, data);
 	mii_delay ();
-	writeb (data | MII_CLK, ioaddr);
+	dw8(PhyCtrl, data | MII_CLK);
 	mii_delay ();
-	return ((readb (ioaddr) >> 1) & 1);
+	return (dr8(PhyCtrl) >> 1) & 1;
 }
 
 static void
 mii_send_bits (struct net_device *dev, u32 data, int len)
 {
 	int i;
+
 	for (i = len - 1; i >= 0; i--) {
 		mii_sendbit (dev, data & (1 << i));
 	}
@@ -1687,28 +1684,29 @@ mii_set_media_pcs (struct net_device *dev)
 static int
 rio_close (struct net_device *dev)
 {
-	long ioaddr = dev->base_addr;
 	struct netdev_private *np = netdev_priv(dev);
+	void __iomem *ioaddr = np->ioaddr;
+
+	struct pci_dev *pdev = np->pdev;
 	struct sk_buff *skb;
 	int i;
 
 	netif_stop_queue (dev);
 
 	/* Disable interrupts */
-	writew (0, ioaddr + IntEnable);
+	dw16(IntEnable, 0);
 
 	/* Stop Tx and Rx logics */
-	writel (TxDisable | RxDisable | StatsDisable, ioaddr + MACCtrl);
+	dw32(MACCtrl, TxDisable | RxDisable | StatsDisable);
 
-	free_irq (dev->irq, dev);
+	free_irq(pdev->irq, dev);
 	del_timer_sync (&np->timer);
 
 	/* Free all the skbuffs in the queue. */
 	for (i = 0; i < RX_RING_SIZE; i++) {
 		skb = np->rx_skbuff[i];
 		if (skb) {
-			pci_unmap_single(np->pdev,
-					 desc_to_dma(&np->rx_ring[i]),
+			pci_unmap_single(pdev, desc_to_dma(&np->rx_ring[i]),
 					 skb->len, PCI_DMA_FROMDEVICE);
 			dev_kfree_skb (skb);
 			np->rx_skbuff[i] = NULL;
@@ -1719,8 +1717,7 @@ rio_close (struct net_device *dev)
 	for (i = 0; i < TX_RING_SIZE; i++) {
 		skb = np->tx_skbuff[i];
 		if (skb) {
-			pci_unmap_single(np->pdev,
-					 desc_to_dma(&np->tx_ring[i]),
+			pci_unmap_single(pdev, desc_to_dma(&np->tx_ring[i]),
 					 skb->len, PCI_DMA_TODEVICE);
 			dev_kfree_skb (skb);
 			np->tx_skbuff[i] = NULL;
@@ -1744,8 +1741,9 @@ rio_remove1 (struct pci_dev *pdev)
 		pci_free_consistent (pdev, TX_TOTAL_SIZE, np->tx_ring,
 				     np->tx_ring_dma);
 #ifdef MEM_MAPPING
-		iounmap ((char *) (dev->base_addr));
+		pci_iounmap(pdev, np->ioaddr);
 #endif
+		pci_iounmap(pdev, np->eeprom_addr);
 		free_netdev (dev);
 		pci_release_regions (pdev);
 		pci_disable_device (pdev);
diff --git a/drivers/net/ethernet/dlink/dl2k.h b/drivers/net/ethernet/dlink/dl2k.h
index 30c2da3de54..3699565704c 100644
--- a/drivers/net/ethernet/dlink/dl2k.h
+++ b/drivers/net/ethernet/dlink/dl2k.h
@@ -42,23 +42,6 @@
 #define TX_TOTAL_SIZE	TX_RING_SIZE*sizeof(struct netdev_desc)
 #define RX_TOTAL_SIZE	RX_RING_SIZE*sizeof(struct netdev_desc)
 
-/* This driver was written to use PCI memory space, however x86-oriented
-   hardware often uses I/O space accesses. */
-#ifndef MEM_MAPPING
-#undef readb
-#undef readw
-#undef readl
-#undef writeb
-#undef writew
-#undef writel
-#define readb inb
-#define readw inw
-#define readl inl
-#define writeb outb
-#define writew outw
-#define writel outl
-#endif
-
 /* Offsets to the device registers.
    Unlike software-only systems, device drivers interact with complex hardware.
    It's not useful to define symbolic names for every register bit in the
@@ -384,6 +367,8 @@ struct netdev_private {
 	dma_addr_t tx_ring_dma;
 	dma_addr_t rx_ring_dma;
 	struct pci_dev *pdev;
+	void __iomem *ioaddr;
+	void __iomem *eeprom_addr;
 	spinlock_t tx_lock;
 	spinlock_t rx_lock;
 	struct net_device_stats stats;
diff --git a/drivers/net/ethernet/dlink/sundance.c b/drivers/net/ethernet/dlink/sundance.c
index d783f4f96ec..d7bb52a7bda 100644
--- a/drivers/net/ethernet/dlink/sundance.c
+++ b/drivers/net/ethernet/dlink/sundance.c
@@ -522,9 +522,6 @@ static int __devinit sundance_probe1 (struct pci_dev *pdev,
 			cpu_to_le16(eeprom_read(ioaddr, i + EEPROM_SA_OFFSET));
 	memcpy(dev->perm_addr, dev->dev_addr, dev->addr_len);
 
-	dev->base_addr = (unsigned long)ioaddr;
-	dev->irq = irq;
-
 	np = netdev_priv(dev);
 	np->base = ioaddr;
 	np->pci_dev = pdev;
@@ -828,18 +825,19 @@ static int netdev_open(struct net_device *dev)
 {
 	struct netdev_private *np = netdev_priv(dev);
 	void __iomem *ioaddr = np->base;
+	const int irq = np->pci_dev->irq;
 	unsigned long flags;
 	int i;
 
 	/* Do we need to reset the chip??? */
 
-	i = request_irq(dev->irq, intr_handler, IRQF_SHARED, dev->name, dev);
+	i = request_irq(irq, intr_handler, IRQF_SHARED, dev->name, dev);
 	if (i)
 		return i;
 
 	if (netif_msg_ifup(np))
-		printk(KERN_DEBUG "%s: netdev_open() irq %d.\n",
-			   dev->name, dev->irq);
+		printk(KERN_DEBUG "%s: netdev_open() irq %d\n", dev->name, irq);
+
 	init_ring(dev);
 
 	iowrite32(np->rx_ring_dma, ioaddr + RxListPtr);
@@ -1814,7 +1812,7 @@ static int netdev_close(struct net_device *dev)
 	}
 #endif /* __i386__ debugging only */
 
-	free_irq(dev->irq, dev);
+	free_irq(np->pci_dev->irq, dev);
 
 	del_timer_sync(&np->timer);
 
diff --git a/drivers/net/ethernet/dnet.c b/drivers/net/ethernet/dnet.c
index b276469f74e..290b26f868c 100644
--- a/drivers/net/ethernet/dnet.c
+++ b/drivers/net/ethernet/dnet.c
@@ -815,6 +815,7 @@ static const struct ethtool_ops dnet_ethtool_ops = {
 	.set_settings		= dnet_set_settings,
 	.get_drvinfo		= dnet_get_drvinfo,
 	.get_link		= ethtool_op_get_link,
+	.get_ts_info		= ethtool_op_get_ts_info,
 };
 
 static const struct net_device_ops dnet_netdev_ops = {
diff --git a/drivers/net/ethernet/emulex/benet/Makefile b/drivers/net/ethernet/emulex/benet/Makefile
index a60cd805113..1a91b276940 100644
--- a/drivers/net/ethernet/emulex/benet/Makefile
+++ b/drivers/net/ethernet/emulex/benet/Makefile
@@ -4,4 +4,4 @@
 
 obj-$(CONFIG_BE2NET) += be2net.o
 
-be2net-y :=  be_main.o be_cmds.o be_ethtool.o
+be2net-y :=  be_main.o be_cmds.o be_ethtool.o be_roce.o
diff --git a/drivers/net/ethernet/emulex/benet/be.h b/drivers/net/ethernet/emulex/benet/be.h
index 9576ac002c2..c5c4c0e83bd 100644
--- a/drivers/net/ethernet/emulex/benet/be.h
+++ b/drivers/net/ethernet/emulex/benet/be.h
@@ -32,8 +32,9 @@
 #include <linux/u64_stats_sync.h>
 
 #include "be_hw.h"
+#include "be_roce.h"
 
-#define DRV_VER			"4.2.116u"
+#define DRV_VER			"4.2.220u"
 #define DRV_NAME		"be2net"
 #define BE_NAME			"ServerEngines BladeEngine2 10Gbps NIC"
 #define BE3_NAME		"ServerEngines BladeEngine3 10Gbps NIC"
@@ -102,7 +103,8 @@ static inline char *nic_name(struct pci_dev *pdev)
 #define MAX_RX_QS		(MAX_RSS_QS + 1) /* RSS qs + 1 def Rx */
 
 #define MAX_TX_QS		8
-#define MAX_MSIX_VECTORS	MAX_RSS_QS
+#define MAX_ROCE_EQS		5
+#define MAX_MSIX_VECTORS	(MAX_RSS_QS + MAX_ROCE_EQS) /* RSS qs + RoCE */
 #define BE_TX_BUDGET		256
 #define BE_NAPI_WEIGHT		64
 #define MAX_RX_POST		BE_NAPI_WEIGHT /* Frags posted at a time */
@@ -162,6 +164,11 @@ static inline void queue_head_inc(struct be_queue_info *q)
 	index_inc(&q->head, q->len);
 }
 
+static inline void index_dec(u16 *index, u16 limit)
+{
+	*index = MODULO((*index - 1), limit);
+}
+
 static inline void queue_tail_inc(struct be_queue_info *q)
 {
 	index_inc(&q->tail, q->len);
@@ -308,11 +315,33 @@ struct be_vf_cfg {
 	u32 tx_rate;
 };
 
+enum vf_state {
+	ENABLED = 0,
+	ASSIGNED = 1
+};
+
 #define BE_FLAGS_LINK_STATUS_INIT		1
 #define BE_FLAGS_WORKER_SCHEDULED		(1 << 3)
 #define BE_UC_PMAC_COUNT		30
 #define BE_VF_UC_PMAC_COUNT		2
 
+struct phy_info {
+	u8 transceiver;
+	u8 autoneg;
+	u8 fc_autoneg;
+	u8 port_type;
+	u16 phy_type;
+	u16 interface_type;
+	u32 misc_params;
+	u16 auto_speeds_supported;
+	u16 fixed_speeds_supported;
+	int link_speed;
+	int forced_port_speed;
+	u32 dac_cable_len;
+	u32 advertising;
+	u32 supported;
+};
+
 struct be_adapter {
 	struct pci_dev *pdev;
 	struct net_device *netdev;
@@ -377,29 +406,41 @@ struct be_adapter {
 	u32 rx_fc;		/* Rx flow control */
 	u32 tx_fc;		/* Tx flow control */
 	bool stats_cmd_sent;
-	int link_speed;
-	u8 port_type;
-	u8 transceiver;
-	u8 autoneg;
 	u8 generation;		/* BladeEngine ASIC generation */
+	u32 if_type;
+	struct {
+		u8 __iomem *base;	/* Door Bell */
+		u32 size;
+		u32 total_size;
+		u64 io_addr;
+	} roce_db;
+	u32 num_msix_roce_vec;
+	struct ocrdma_dev *ocrdma_dev;
+	struct list_head entry;
+
 	u32 flash_status;
 	struct completion flash_compl;
 
-	u32 num_vfs;
-	u8 is_virtfn;
+	u32 num_vfs;		/* Number of VFs provisioned by PF driver */
+	u32 dev_num_vfs;	/* Number of VFs supported by HW */
+	u8 virtfn;
 	struct be_vf_cfg *vf_cfg;
 	bool be3_native;
 	u32 sli_family;
 	u8 hba_port_num;
 	u16 pvid;
+	struct phy_info phy;
 	u8 wol_cap;
 	bool wol;
 	u32 max_pmac_cnt;	/* Max secondary UC MACs programmable */
 	u32 uc_macs;		/* Count of secondary UC MAC programmed */
+	u32 msg_enable;
 };
 
-#define be_physfn(adapter) (!adapter->is_virtfn)
+#define be_physfn(adapter)		(!adapter->virtfn)
 #define	sriov_enabled(adapter)		(adapter->num_vfs > 0)
+#define	sriov_want(adapter)		(adapter->dev_num_vfs && num_vfs && \
+					 be_physfn(adapter))
 #define for_all_vfs(adapter, vf_cfg, i)					\
 	for (i = 0, vf_cfg = &adapter->vf_cfg[i]; i < adapter->num_vfs;	\
 		i++, vf_cfg++)
@@ -413,6 +454,10 @@ struct be_adapter {
 #define lancer_chip(adapter)	((adapter->pdev->device == OC_DEVICE_ID3) || \
 				 (adapter->pdev->device == OC_DEVICE_ID4))
 
+#define be_roce_supported(adapter) ((adapter->if_type == SLI_INTF_TYPE_3 || \
+				adapter->sli_family == SKYHAWK_SLI_FAMILY) && \
+				(adapter->function_mode & RDMA_ENABLED))
+
 extern const struct ethtool_ops be_ethtool_ops;
 
 #define msix_enabled(adapter)		(adapter->num_msix_vec > 0)
@@ -528,14 +573,6 @@ static inline u8 is_udp_pkt(struct sk_buff *skb)
 	return val;
 }
 
-static inline void be_check_sriov_fn_type(struct be_adapter *adapter)
-{
-	u32 sli_intf;
-
-	pci_read_config_dword(adapter->pdev, SLI_INTF_REG_OFFSET, &sli_intf);
-	adapter->is_virtfn = (sli_intf & SLI_INTF_FT_MASK) ? 1 : 0;
-}
-
 static inline void be_vf_eth_addr_generate(struct be_adapter *adapter, u8 *mac)
 {
 	u32 addr;
@@ -577,10 +614,31 @@ static inline bool be_is_wol_excluded(struct be_adapter *adapter)
 	}
 }
 
+static inline bool be_type_2_3(struct be_adapter *adapter)
+{
+	return (adapter->if_type == SLI_INTF_TYPE_2 ||
+		adapter->if_type == SLI_INTF_TYPE_3) ? true : false;
+}
+
 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);
 extern void be_parse_stats(struct be_adapter *adapter);
 extern int be_load_fw(struct be_adapter *adapter, u8 *func);
 extern bool be_is_wol_supported(struct be_adapter *adapter);
+extern bool be_pause_supported(struct be_adapter *adapter);
+extern u32 be_get_fw_log_level(struct be_adapter *adapter);
+
+/*
+ * internal function to initialize-cleanup roce device.
+ */
+extern void be_roce_dev_add(struct be_adapter *);
+extern void be_roce_dev_remove(struct be_adapter *);
+
+/*
+ * internal function to open-close roce device during ifup-ifdown.
+ */
+extern void be_roce_dev_open(struct be_adapter *);
+extern void be_roce_dev_close(struct be_adapter *);
+
 #endif				/* BE_H */
diff --git a/drivers/net/ethernet/emulex/benet/be_cmds.c b/drivers/net/ethernet/emulex/benet/be_cmds.c
index 67b030d72df..8d06ea38174 100644
--- a/drivers/net/ethernet/emulex/benet/be_cmds.c
+++ b/drivers/net/ethernet/emulex/benet/be_cmds.c
@@ -15,6 +15,7 @@
  * Costa Mesa, CA 92626
  */
 
+#include <linux/module.h>
 #include "be.h"
 #include "be_cmds.h"
 
@@ -61,10 +62,21 @@ static inline void be_mcc_compl_use(struct be_mcc_compl *compl)
 	compl->flags = 0;
 }
 
+static struct be_cmd_resp_hdr *be_decode_resp_hdr(u32 tag0, u32 tag1)
+{
+	unsigned long addr;
+
+	addr = tag1;
+	addr = ((addr << 16) << 16) | tag0;
+	return (void *)addr;
+}
+
 static int be_mcc_compl_process(struct be_adapter *adapter,
-	struct be_mcc_compl *compl)
+				struct be_mcc_compl *compl)
 {
 	u16 compl_status, extd_status;
+	struct be_cmd_resp_hdr *resp_hdr;
+	u8 opcode = 0, subsystem = 0;
 
 	/* Just swap the status to host endian; mcc tag is opaquely copied
 	 * from mcc_wrb */
@@ -73,32 +85,36 @@ static int be_mcc_compl_process(struct be_adapter *adapter,
 	compl_status = (compl->status >> CQE_STATUS_COMPL_SHIFT) &
 				CQE_STATUS_COMPL_MASK;
 
-	if (((compl->tag0 == OPCODE_COMMON_WRITE_FLASHROM) ||
-		(compl->tag0 == OPCODE_COMMON_WRITE_OBJECT)) &&
-		(compl->tag1 == CMD_SUBSYSTEM_COMMON)) {
+	resp_hdr = be_decode_resp_hdr(compl->tag0, compl->tag1);
+
+	if (resp_hdr) {
+		opcode = resp_hdr->opcode;
+		subsystem = resp_hdr->subsystem;
+	}
+
+	if (((opcode == OPCODE_COMMON_WRITE_FLASHROM) ||
+	     (opcode == OPCODE_COMMON_WRITE_OBJECT)) &&
+	    (subsystem == CMD_SUBSYSTEM_COMMON)) {
 		adapter->flash_status = compl_status;
 		complete(&adapter->flash_compl);
 	}
 
 	if (compl_status == MCC_STATUS_SUCCESS) {
-		if (((compl->tag0 == OPCODE_ETH_GET_STATISTICS) ||
-			 (compl->tag0 == OPCODE_ETH_GET_PPORT_STATS)) &&
-			(compl->tag1 == CMD_SUBSYSTEM_ETH)) {
+		if (((opcode == OPCODE_ETH_GET_STATISTICS) ||
+		     (opcode == OPCODE_ETH_GET_PPORT_STATS)) &&
+		    (subsystem == CMD_SUBSYSTEM_ETH)) {
 			be_parse_stats(adapter);
 			adapter->stats_cmd_sent = false;
 		}
-		if (compl->tag0 ==
-				OPCODE_COMMON_GET_CNTL_ADDITIONAL_ATTRIBUTES) {
-			struct be_mcc_wrb *mcc_wrb =
-				queue_index_node(&adapter->mcc_obj.q,
-						compl->tag1);
+		if (opcode == OPCODE_COMMON_GET_CNTL_ADDITIONAL_ATTRIBUTES &&
+		    subsystem == CMD_SUBSYSTEM_COMMON) {
 			struct be_cmd_resp_get_cntl_addnl_attribs *resp =
-				embedded_payload(mcc_wrb);
+				(void *)resp_hdr;
 			adapter->drv_stats.be_on_die_temperature =
 				resp->on_die_temperature;
 		}
 	} else {
-		if (compl->tag0 == OPCODE_COMMON_GET_CNTL_ADDITIONAL_ATTRIBUTES)
+		if (opcode == OPCODE_COMMON_GET_CNTL_ADDITIONAL_ATTRIBUTES)
 			be_get_temp_freq = 0;
 
 		if (compl_status == MCC_STATUS_NOT_SUPPORTED ||
@@ -108,13 +124,13 @@ static int be_mcc_compl_process(struct be_adapter *adapter,
 		if (compl_status == MCC_STATUS_UNAUTHORIZED_REQUEST) {
 			dev_warn(&adapter->pdev->dev, "This domain(VM) is not "
 				"permitted to execute this cmd (opcode %d)\n",
-				compl->tag0);
+				opcode);
 		} else {
 			extd_status = (compl->status >> CQE_STATUS_EXTD_SHIFT) &
 					CQE_STATUS_EXTD_MASK;
 			dev_err(&adapter->pdev->dev, "Cmd (opcode %d) failed:"
 				"status %d, extd-status %d\n",
-				compl->tag0, compl_status, extd_status);
+				opcode, compl_status, extd_status);
 		}
 	}
 done:
@@ -126,7 +142,7 @@ static void be_async_link_state_process(struct be_adapter *adapter,
 		struct be_async_event_link_state *evt)
 {
 	/* When link status changes, link speed must be re-queried from FW */
-	adapter->link_speed = -1;
+	adapter->phy.link_speed = -1;
 
 	/* For the initial link status do not rely on the ASYNC event as
 	 * it may not be received in some cases.
@@ -153,7 +169,7 @@ static void be_async_grp5_qos_speed_process(struct be_adapter *adapter,
 {
 	if (evt->physical_port == adapter->port_num) {
 		/* qos_link_speed is in units of 10 Mbps */
-		adapter->link_speed = evt->qos_link_speed * 10;
+		adapter->phy.link_speed = evt->qos_link_speed * 10;
 	}
 }
 
@@ -286,7 +302,7 @@ static int be_mcc_wait_compl(struct be_adapter *adapter)
 	if (i == mcc_timeout) {
 		dev_err(&adapter->pdev->dev, "FW not responding\n");
 		adapter->fw_timeout = true;
-		return -1;
+		return -EIO;
 	}
 	return status;
 }
@@ -294,8 +310,26 @@ static int be_mcc_wait_compl(struct be_adapter *adapter)
 /* Notify MCC requests and wait for completion */
 static int be_mcc_notify_wait(struct be_adapter *adapter)
 {
+	int status;
+	struct be_mcc_wrb *wrb;
+	struct be_mcc_obj *mcc_obj = &adapter->mcc_obj;
+	u16 index = mcc_obj->q.head;
+	struct be_cmd_resp_hdr *resp;
+
+	index_dec(&index, mcc_obj->q.len);
+	wrb = queue_index_node(&mcc_obj->q, index);
+
+	resp = be_decode_resp_hdr(wrb->tag0, wrb->tag1);
+
 	be_mcc_notify(adapter);
-	return be_mcc_wait_compl(adapter);
+
+	status = be_mcc_wait_compl(adapter);
+	if (status == -EIO)
+		goto out;
+
+	status = resp->status;
+out:
+	return status;
 }
 
 static int be_mbox_db_ready_wait(struct be_adapter *adapter, void __iomem *db)
@@ -435,14 +469,17 @@ static void be_wrb_cmd_hdr_prepare(struct be_cmd_req_hdr *req_hdr,
 				struct be_mcc_wrb *wrb, struct be_dma_mem *mem)
 {
 	struct be_sge *sge;
+	unsigned long addr = (unsigned long)req_hdr;
+	u64 req_addr = addr;
 
 	req_hdr->opcode = opcode;
 	req_hdr->subsystem = subsystem;
 	req_hdr->request_length = cpu_to_le32(cmd_len - sizeof(*req_hdr));
 	req_hdr->version = 0;
 
-	wrb->tag0 = opcode;
-	wrb->tag1 = subsystem;
+	wrb->tag0 = req_addr & 0xFFFFFFFF;
+	wrb->tag1 = upper_32_bits(req_addr);
+
 	wrb->payload_length = cmd_len;
 	if (mem) {
 		wrb->embedded |= (1 & MCC_WRB_SGE_CNT_MASK) <<
@@ -1221,7 +1258,7 @@ int lancer_cmd_get_pport_stats(struct be_adapter *adapter,
 			OPCODE_ETH_GET_PPORT_STATS, nonemb_cmd->size, wrb,
 			nonemb_cmd);
 
-	req->cmd_params.params.pport_num = cpu_to_le16(adapter->port_num);
+	req->cmd_params.params.pport_num = cpu_to_le16(adapter->hba_port_num);
 	req->cmd_params.params.reset_stats = 0;
 
 	be_mcc_notify(adapter);
@@ -1283,13 +1320,10 @@ int be_cmd_get_die_temperature(struct be_adapter *adapter)
 {
 	struct be_mcc_wrb *wrb;
 	struct be_cmd_req_get_cntl_addnl_attribs *req;
-	u16 mccq_index;
 	int status;
 
 	spin_lock_bh(&adapter->mcc_lock);
 
-	mccq_index = adapter->mcc_obj.q.head;
-
 	wrb = wrb_from_mccq(adapter);
 	if (!wrb) {
 		status = -EBUSY;
@@ -1301,8 +1335,6 @@ int be_cmd_get_die_temperature(struct be_adapter *adapter)
 		OPCODE_COMMON_GET_CNTL_ADDITIONAL_ATTRIBUTES, sizeof(*req),
 		wrb, NULL);
 
-	wrb->tag1 = mccq_index;
-
 	be_mcc_notify(adapter);
 
 err:
@@ -1824,18 +1856,16 @@ 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(12000)))
+					 msecs_to_jiffies(30000)))
 		status = -1;
 	else
 		status = adapter->flash_status;
 
 	resp = embedded_payload(wrb);
-	if (!status) {
+	if (!status)
 		*data_written = le32_to_cpu(resp->actual_write_len);
-	} else {
+	else
 		*addn_status = resp->additional_status;
-		status = resp->status;
-	}
 
 	return status;
 
@@ -1950,7 +1980,7 @@ int be_cmd_get_flash_crc(struct be_adapter *adapter, u8 *flashed_crc,
 	be_wrb_cmd_hdr_prepare(&req->hdr, CMD_SUBSYSTEM_COMMON,
 		OPCODE_COMMON_READ_FLASHROM, sizeof(*req)+4, wrb, NULL);
 
-	req->params.op_type = cpu_to_le32(IMG_TYPE_REDBOOT);
+	req->params.op_type = cpu_to_le32(OPTYPE_REDBOOT);
 	req->params.op_code = cpu_to_le32(FLASHROM_OPER_REPORT);
 	req->params.offset = cpu_to_le32(offset);
 	req->params.data_buf_size = cpu_to_le32(0x4);
@@ -2136,8 +2166,7 @@ err:
 	return status;
 }
 
-int be_cmd_get_phy_info(struct be_adapter *adapter,
-				struct be_phy_info *phy_info)
+int be_cmd_get_phy_info(struct be_adapter *adapter)
 {
 	struct be_mcc_wrb *wrb;
 	struct be_cmd_req_get_phy_info *req;
@@ -2170,9 +2199,15 @@ int be_cmd_get_phy_info(struct be_adapter *adapter,
 	if (!status) {
 		struct be_phy_info *resp_phy_info =
 				cmd.va + sizeof(struct be_cmd_req_hdr);
-		phy_info->phy_type = le16_to_cpu(resp_phy_info->phy_type);
-		phy_info->interface_type =
+		adapter->phy.phy_type = le16_to_cpu(resp_phy_info->phy_type);
+		adapter->phy.interface_type =
 			le16_to_cpu(resp_phy_info->interface_type);
+		adapter->phy.auto_speeds_supported =
+			le16_to_cpu(resp_phy_info->auto_speeds_supported);
+		adapter->phy.fixed_speeds_supported =
+			le16_to_cpu(resp_phy_info->fixed_speeds_supported);
+		adapter->phy.misc_params =
+			le32_to_cpu(resp_phy_info->misc_params);
 	}
 	pci_free_consistent(adapter->pdev, cmd.size,
 				cmd.va, cmd.dma);
@@ -2555,4 +2590,98 @@ err:
 	mutex_unlock(&adapter->mbox_lock);
 	pci_free_consistent(adapter->pdev, cmd.size, cmd.va, cmd.dma);
 	return status;
+
+}
+int be_cmd_get_ext_fat_capabilites(struct be_adapter *adapter,
+				   struct be_dma_mem *cmd)
+{
+	struct be_mcc_wrb *wrb;
+	struct be_cmd_req_get_ext_fat_caps *req;
+	int status;
+
+	if (mutex_lock_interruptible(&adapter->mbox_lock))
+		return -1;
+
+	wrb = wrb_from_mbox(adapter);
+	if (!wrb) {
+		status = -EBUSY;
+		goto err;
+	}
+
+	req = cmd->va;
+	be_wrb_cmd_hdr_prepare(&req->hdr, CMD_SUBSYSTEM_COMMON,
+			       OPCODE_COMMON_GET_EXT_FAT_CAPABILITES,
+			       cmd->size, wrb, cmd);
+	req->parameter_type = cpu_to_le32(1);
+
+	status = be_mbox_notify_wait(adapter);
+err:
+	mutex_unlock(&adapter->mbox_lock);
+	return status;
+}
+
+int be_cmd_set_ext_fat_capabilites(struct be_adapter *adapter,
+				   struct be_dma_mem *cmd,
+				   struct be_fat_conf_params *configs)
+{
+	struct be_mcc_wrb *wrb;
+	struct be_cmd_req_set_ext_fat_caps *req;
+	int status;
+
+	spin_lock_bh(&adapter->mcc_lock);
+
+	wrb = wrb_from_mccq(adapter);
+	if (!wrb) {
+		status = -EBUSY;
+		goto err;
+	}
+
+	req = cmd->va;
+	memcpy(&req->set_params, configs, sizeof(struct be_fat_conf_params));
+	be_wrb_cmd_hdr_prepare(&req->hdr, CMD_SUBSYSTEM_COMMON,
+			       OPCODE_COMMON_SET_EXT_FAT_CAPABILITES,
+			       cmd->size, wrb, cmd);
+
+	status = be_mcc_notify_wait(adapter);
+err:
+	spin_unlock_bh(&adapter->mcc_lock);
+	return status;
+}
+
+int be_roce_mcc_cmd(void *netdev_handle, void *wrb_payload,
+			int wrb_payload_size, u16 *cmd_status, u16 *ext_status)
+{
+	struct be_adapter *adapter = netdev_priv(netdev_handle);
+	struct be_mcc_wrb *wrb;
+	struct be_cmd_req_hdr *hdr = (struct be_cmd_req_hdr *) wrb_payload;
+	struct be_cmd_req_hdr *req;
+	struct be_cmd_resp_hdr *resp;
+	int status;
+
+	spin_lock_bh(&adapter->mcc_lock);
+
+	wrb = wrb_from_mccq(adapter);
+	if (!wrb) {
+		status = -EBUSY;
+		goto err;
+	}
+	req = embedded_payload(wrb);
+	resp = embedded_payload(wrb);
+
+	be_wrb_cmd_hdr_prepare(req, hdr->subsystem,
+			       hdr->opcode, wrb_payload_size, wrb, NULL);
+	memcpy(req, wrb_payload, wrb_payload_size);
+	be_dws_cpu_to_le(req, wrb_payload_size);
+
+	status = be_mcc_notify_wait(adapter);
+	if (cmd_status)
+		*cmd_status = (status & 0xffff);
+	if (ext_status)
+		*ext_status = 0;
+	memcpy(wrb_payload, resp, sizeof(*resp) + resp->response_length);
+	be_dws_le_to_cpu(wrb_payload, sizeof(*resp) + resp->response_length);
+err:
+	spin_unlock_bh(&adapter->mcc_lock);
+	return status;
 }
+EXPORT_SYMBOL(be_roce_mcc_cmd);
diff --git a/drivers/net/ethernet/emulex/benet/be_cmds.h b/drivers/net/ethernet/emulex/benet/be_cmds.h
index d5b680c56af..9625bf420c1 100644
--- a/drivers/net/ethernet/emulex/benet/be_cmds.h
+++ b/drivers/net/ethernet/emulex/benet/be_cmds.h
@@ -189,6 +189,8 @@ struct be_mcc_mailbox {
 #define OPCODE_COMMON_GET_PHY_DETAILS			102
 #define OPCODE_COMMON_SET_DRIVER_FUNCTION_CAP		103
 #define OPCODE_COMMON_GET_CNTL_ADDITIONAL_ATTRIBUTES	121
+#define OPCODE_COMMON_GET_EXT_FAT_CAPABILITES		125
+#define OPCODE_COMMON_SET_EXT_FAT_CAPABILITES		126
 #define OPCODE_COMMON_GET_MAC_LIST			147
 #define OPCODE_COMMON_SET_MAC_LIST			148
 #define OPCODE_COMMON_GET_HSW_CONFIG			152
@@ -225,8 +227,12 @@ struct be_cmd_req_hdr {
 #define RESP_HDR_INFO_OPCODE_SHIFT	0	/* bits 0 - 7 */
 #define RESP_HDR_INFO_SUBSYS_SHIFT	8 	/* bits 8 - 15 */
 struct be_cmd_resp_hdr {
-	u32 info;		/* dword 0 */
-	u32 status;		/* dword 1 */
+	u8 opcode;		/* dword 0 */
+	u8 subsystem;		/* dword 0 */
+	u8 rsvd[2];		/* dword 0 */
+	u8 status;		/* dword 1 */
+	u8 add_status;		/* dword 1 */
+	u8 rsvd1[2];		/* dword 1 */
 	u32 response_length;	/* dword 2 */
 	u32 actual_resp_len;	/* dword 3 */
 };
@@ -1056,6 +1062,7 @@ struct be_cmd_resp_modify_eq_delay {
 /* The HW can come up in either of the following multi-channel modes
  * based on the skew/IPL.
  */
+#define RDMA_ENABLED				0x4
 #define FLEX10_MODE				0x400
 #define VNIC_MODE				0x20000
 #define UMC_ENABLED				0x1000000
@@ -1309,9 +1316,36 @@ enum {
 	PHY_TYPE_KX4_10GB,
 	PHY_TYPE_BASET_10GB,
 	PHY_TYPE_BASET_1GB,
+	PHY_TYPE_BASEX_1GB,
+	PHY_TYPE_SGMII,
 	PHY_TYPE_DISABLED = 255
 };
 
+#define BE_SUPPORTED_SPEED_NONE		0
+#define BE_SUPPORTED_SPEED_10MBPS	1
+#define BE_SUPPORTED_SPEED_100MBPS	2
+#define BE_SUPPORTED_SPEED_1GBPS	4
+#define BE_SUPPORTED_SPEED_10GBPS	8
+
+#define BE_AN_EN			0x2
+#define BE_PAUSE_SYM_EN			0x80
+
+/* MAC speed valid values */
+#define SPEED_DEFAULT  0x0
+#define SPEED_FORCED_10GB  0x1
+#define SPEED_FORCED_1GB  0x2
+#define SPEED_AUTONEG_10GB  0x3
+#define SPEED_AUTONEG_1GB  0x4
+#define SPEED_AUTONEG_100MB  0x5
+#define SPEED_AUTONEG_10GB_1GB 0x6
+#define SPEED_AUTONEG_10GB_1GB_100MB 0x7
+#define SPEED_AUTONEG_1GB_100MB  0x8
+#define SPEED_AUTONEG_10MB  0x9
+#define SPEED_AUTONEG_1GB_100MB_10MB 0xa
+#define SPEED_AUTONEG_100MB_10MB 0xb
+#define SPEED_FORCED_100MB  0xc
+#define SPEED_FORCED_10MB  0xd
+
 struct be_cmd_req_get_phy_info {
 	struct be_cmd_req_hdr hdr;
 	u8 rsvd0[24];
@@ -1321,7 +1355,11 @@ struct be_phy_info {
 	u16 phy_type;
 	u16 interface_type;
 	u32 misc_params;
-	u32 future_use[4];
+	u16 ext_phy_details;
+	u16 rsvd;
+	u16 auto_speeds_supported;
+	u16 fixed_speeds_supported;
+	u32 future_use[2];
 };
 
 struct be_cmd_resp_get_phy_info {
@@ -1567,6 +1605,56 @@ static inline void *be_erx_stats_from_cmd(struct be_adapter *adapter)
 	}
 }
 
+
+/************** get fat capabilites *******************/
+#define MAX_MODULES 27
+#define MAX_MODES 4
+#define MODE_UART 0
+#define FW_LOG_LEVEL_DEFAULT 48
+#define FW_LOG_LEVEL_FATAL 64
+
+struct ext_fat_mode {
+	u8 mode;
+	u8 rsvd0;
+	u16 port_mask;
+	u32 dbg_lvl;
+	u64 fun_mask;
+} __packed;
+
+struct ext_fat_modules {
+	u8 modules_str[32];
+	u32 modules_id;
+	u32 num_modes;
+	struct ext_fat_mode trace_lvl[MAX_MODES];
+} __packed;
+
+struct be_fat_conf_params {
+	u32 max_log_entries;
+	u32 log_entry_size;
+	u8 log_type;
+	u8 max_log_funs;
+	u8 max_log_ports;
+	u8 rsvd0;
+	u32 supp_modes;
+	u32 num_modules;
+	struct ext_fat_modules module[MAX_MODULES];
+} __packed;
+
+struct be_cmd_req_get_ext_fat_caps {
+	struct be_cmd_req_hdr hdr;
+	u32 parameter_type;
+};
+
+struct be_cmd_resp_get_ext_fat_caps {
+	struct be_cmd_resp_hdr hdr;
+	struct be_fat_conf_params get_params;
+};
+
+struct be_cmd_req_set_ext_fat_caps {
+	struct be_cmd_req_hdr hdr;
+	struct be_fat_conf_params set_params;
+};
+
 extern int be_pci_fnum_get(struct be_adapter *adapter);
 extern int be_cmd_POST(struct be_adapter *adapter);
 extern int be_cmd_mac_addr_query(struct be_adapter *adapter, u8 *mac_addr,
@@ -1655,8 +1743,7 @@ extern int be_cmd_get_seeprom_data(struct be_adapter *adapter,
 				struct be_dma_mem *nonemb_cmd);
 extern int be_cmd_set_loopback(struct be_adapter *adapter, u8 port_num,
 				u8 loopback_type, u8 enable);
-extern int be_cmd_get_phy_info(struct be_adapter *adapter,
-				struct be_phy_info *phy_info);
+extern int be_cmd_get_phy_info(struct be_adapter *adapter);
 extern int be_cmd_set_qos(struct be_adapter *adapter, u32 bps, u32 domain);
 extern void be_detect_dump_ue(struct be_adapter *adapter);
 extern int be_cmd_get_die_temperature(struct be_adapter *adapter);
@@ -1673,4 +1760,9 @@ extern int be_cmd_set_hsw_config(struct be_adapter *adapter, u16 pvid,
 extern int be_cmd_get_hsw_config(struct be_adapter *adapter, u16 *pvid,
 			u32 domain, u16 intf_id);
 extern int be_cmd_get_acpi_wol_cap(struct be_adapter *adapter);
+extern int be_cmd_get_ext_fat_capabilites(struct be_adapter *adapter,
+					  struct be_dma_mem *cmd);
+extern int be_cmd_set_ext_fat_capabilites(struct be_adapter *adapter,
+					  struct be_dma_mem *cmd,
+					  struct be_fat_conf_params *cfgs);
 
diff --git a/drivers/net/ethernet/emulex/benet/be_ethtool.c b/drivers/net/ethernet/emulex/benet/be_ethtool.c
index c1ff73cb0e6..63e51d47690 100644
--- a/drivers/net/ethernet/emulex/benet/be_ethtool.c
+++ b/drivers/net/ethernet/emulex/benet/be_ethtool.c
@@ -433,102 +433,193 @@ static int be_get_sset_count(struct net_device *netdev, int stringset)
 	}
 }
 
+static u32 be_get_port_type(u32 phy_type, u32 dac_cable_len)
+{
+	u32 port;
+
+	switch (phy_type) {
+	case PHY_TYPE_BASET_1GB:
+	case PHY_TYPE_BASEX_1GB:
+	case PHY_TYPE_SGMII:
+		port = PORT_TP;
+		break;
+	case PHY_TYPE_SFP_PLUS_10GB:
+		port = dac_cable_len ? PORT_DA : PORT_FIBRE;
+		break;
+	case PHY_TYPE_XFP_10GB:
+	case PHY_TYPE_SFP_1GB:
+		port = PORT_FIBRE;
+		break;
+	case PHY_TYPE_BASET_10GB:
+		port = PORT_TP;
+		break;
+	default:
+		port = PORT_OTHER;
+	}
+
+	return port;
+}
+
+static u32 convert_to_et_setting(u32 if_type, u32 if_speeds)
+{
+	u32 val = 0;
+
+	switch (if_type) {
+	case PHY_TYPE_BASET_1GB:
+	case PHY_TYPE_BASEX_1GB:
+	case PHY_TYPE_SGMII:
+		val |= SUPPORTED_TP;
+		if (if_speeds & BE_SUPPORTED_SPEED_1GBPS)
+			val |= SUPPORTED_1000baseT_Full;
+		if (if_speeds & BE_SUPPORTED_SPEED_100MBPS)
+			val |= SUPPORTED_100baseT_Full;
+		if (if_speeds & BE_SUPPORTED_SPEED_10MBPS)
+			val |= SUPPORTED_10baseT_Full;
+		break;
+	case PHY_TYPE_KX4_10GB:
+		val |= SUPPORTED_Backplane;
+		if (if_speeds & BE_SUPPORTED_SPEED_1GBPS)
+			val |= SUPPORTED_1000baseKX_Full;
+		if (if_speeds & BE_SUPPORTED_SPEED_10GBPS)
+			val |= SUPPORTED_10000baseKX4_Full;
+		break;
+	case PHY_TYPE_KR_10GB:
+		val |= SUPPORTED_Backplane |
+				SUPPORTED_10000baseKR_Full;
+		break;
+	case PHY_TYPE_SFP_PLUS_10GB:
+	case PHY_TYPE_XFP_10GB:
+	case PHY_TYPE_SFP_1GB:
+		val |= SUPPORTED_FIBRE;
+		if (if_speeds & BE_SUPPORTED_SPEED_10GBPS)
+			val |= SUPPORTED_10000baseT_Full;
+		if (if_speeds & BE_SUPPORTED_SPEED_1GBPS)
+			val |= SUPPORTED_1000baseT_Full;
+		break;
+	case PHY_TYPE_BASET_10GB:
+		val |= SUPPORTED_TP;
+		if (if_speeds & BE_SUPPORTED_SPEED_10GBPS)
+			val |= SUPPORTED_10000baseT_Full;
+		if (if_speeds & BE_SUPPORTED_SPEED_1GBPS)
+			val |= SUPPORTED_1000baseT_Full;
+		if (if_speeds & BE_SUPPORTED_SPEED_100MBPS)
+			val |= SUPPORTED_100baseT_Full;
+		break;
+	default:
+		val |= SUPPORTED_TP;
+	}
+
+	return val;
+}
+
+static int convert_to_et_speed(u32 be_speed)
+{
+	int et_speed = SPEED_10000;
+
+	switch (be_speed) {
+	case PHY_LINK_SPEED_10MBPS:
+		et_speed = SPEED_10;
+		break;
+	case PHY_LINK_SPEED_100MBPS:
+		et_speed = SPEED_100;
+		break;
+	case PHY_LINK_SPEED_1GBPS:
+		et_speed = SPEED_1000;
+		break;
+	case PHY_LINK_SPEED_10GBPS:
+		et_speed = SPEED_10000;
+		break;
+	}
+
+	return et_speed;
+}
+
+bool be_pause_supported(struct be_adapter *adapter)
+{
+	return (adapter->phy.interface_type == PHY_TYPE_SFP_PLUS_10GB ||
+		adapter->phy.interface_type == PHY_TYPE_XFP_10GB) ?
+		false : true;
+}
+
 static int be_get_settings(struct net_device *netdev, struct ethtool_cmd *ecmd)
 {
 	struct be_adapter *adapter = netdev_priv(netdev);
-	struct be_phy_info phy_info;
-	u8 mac_speed = 0;
+	u8 port_speed = 0;
 	u16 link_speed = 0;
 	u8 link_status;
+	u32 et_speed = 0;
 	int status;
 
-	if ((adapter->link_speed < 0) || (!(netdev->flags & IFF_UP))) {
-		status = be_cmd_link_status_query(adapter, &mac_speed,
-						  &link_speed, &link_status, 0);
-		if (!status)
-			be_link_status_update(adapter, link_status);
-
-		/* link_speed is in units of 10 Mbps */
-		if (link_speed) {
-			ethtool_cmd_speed_set(ecmd, link_speed*10);
+	if (adapter->phy.link_speed < 0 || !(netdev->flags & IFF_UP)) {
+		if (adapter->phy.forced_port_speed < 0) {
+			status = be_cmd_link_status_query(adapter, &port_speed,
+						&link_speed, &link_status, 0);
+			if (!status)
+				be_link_status_update(adapter, link_status);
+			if (link_speed)
+				et_speed = link_speed * 10;
+			else if (link_status)
+				et_speed = convert_to_et_speed(port_speed);
 		} else {
-			switch (mac_speed) {
-			case PHY_LINK_SPEED_10MBPS:
-				ethtool_cmd_speed_set(ecmd, SPEED_10);
-				break;
-			case PHY_LINK_SPEED_100MBPS:
-				ethtool_cmd_speed_set(ecmd, SPEED_100);
-				break;
-			case PHY_LINK_SPEED_1GBPS:
-				ethtool_cmd_speed_set(ecmd, SPEED_1000);
-				break;
-			case PHY_LINK_SPEED_10GBPS:
-				ethtool_cmd_speed_set(ecmd, SPEED_10000);
-				break;
-			case PHY_LINK_SPEED_ZERO:
-				ethtool_cmd_speed_set(ecmd, 0);
-				break;
-			}
+			et_speed = adapter->phy.forced_port_speed;
 		}
 
-		status = be_cmd_get_phy_info(adapter, &phy_info);
-		if (!status) {
-			switch (phy_info.interface_type) {
-			case PHY_TYPE_XFP_10GB:
-			case PHY_TYPE_SFP_1GB:
-			case PHY_TYPE_SFP_PLUS_10GB:
-				ecmd->port = PORT_FIBRE;
-				break;
-			default:
-				ecmd->port = PORT_TP;
-				break;
-			}
+		ethtool_cmd_speed_set(ecmd, et_speed);
+
+		status = be_cmd_get_phy_info(adapter);
+		if (status)
+			return status;
+
+		ecmd->supported =
+			convert_to_et_setting(adapter->phy.interface_type,
+					adapter->phy.auto_speeds_supported |
+					adapter->phy.fixed_speeds_supported);
+		ecmd->advertising =
+			convert_to_et_setting(adapter->phy.interface_type,
+					adapter->phy.auto_speeds_supported);
+
+		ecmd->port = be_get_port_type(adapter->phy.interface_type,
+					      adapter->phy.dac_cable_len);
+
+		if (adapter->phy.auto_speeds_supported) {
+			ecmd->supported |= SUPPORTED_Autoneg;
+			ecmd->autoneg = AUTONEG_ENABLE;
+			ecmd->advertising |= ADVERTISED_Autoneg;
+		}
 
-			switch (phy_info.interface_type) {
-			case PHY_TYPE_KR_10GB:
-			case PHY_TYPE_KX4_10GB:
-				ecmd->autoneg = AUTONEG_ENABLE;
+		if (be_pause_supported(adapter)) {
+			ecmd->supported |= SUPPORTED_Pause;
+			ecmd->advertising |= ADVERTISED_Pause;
+		}
+
+		switch (adapter->phy.interface_type) {
+		case PHY_TYPE_KR_10GB:
+		case PHY_TYPE_KX4_10GB:
 			ecmd->transceiver = XCVR_INTERNAL;
-				break;
-			default:
-				ecmd->autoneg = AUTONEG_DISABLE;
-				ecmd->transceiver = XCVR_EXTERNAL;
-				break;
-			}
+			break;
+		default:
+			ecmd->transceiver = XCVR_EXTERNAL;
+			break;
 		}
 
 		/* Save for future use */
-		adapter->link_speed = ethtool_cmd_speed(ecmd);
-		adapter->port_type = ecmd->port;
-		adapter->transceiver = ecmd->transceiver;
-		adapter->autoneg = ecmd->autoneg;
+		adapter->phy.link_speed = ethtool_cmd_speed(ecmd);
+		adapter->phy.port_type = ecmd->port;
+		adapter->phy.transceiver = ecmd->transceiver;
+		adapter->phy.autoneg = ecmd->autoneg;
+		adapter->phy.advertising = ecmd->advertising;
+		adapter->phy.supported = ecmd->supported;
 	} else {
-		ethtool_cmd_speed_set(ecmd, adapter->link_speed);
-		ecmd->port = adapter->port_type;
-		ecmd->transceiver = adapter->transceiver;
-		ecmd->autoneg = adapter->autoneg;
+		ethtool_cmd_speed_set(ecmd, adapter->phy.link_speed);
+		ecmd->port = adapter->phy.port_type;
+		ecmd->transceiver = adapter->phy.transceiver;
+		ecmd->autoneg = adapter->phy.autoneg;
+		ecmd->advertising = adapter->phy.advertising;
+		ecmd->supported = adapter->phy.supported;
 	}
 
-	ecmd->duplex = DUPLEX_FULL;
+	ecmd->duplex = netif_carrier_ok(netdev) ? DUPLEX_FULL : DUPLEX_UNKNOWN;
 	ecmd->phy_address = adapter->port_num;
-	switch (ecmd->port) {
-	case PORT_FIBRE:
-		ecmd->supported = (SUPPORTED_10000baseT_Full | SUPPORTED_FIBRE);
-		break;
-	case PORT_TP:
-		ecmd->supported = (SUPPORTED_10000baseT_Full | SUPPORTED_TP);
-		break;
-	case PORT_AUI:
-		ecmd->supported = (SUPPORTED_10000baseT_Full | SUPPORTED_AUI);
-		break;
-	}
-
-	if (ecmd->autoneg) {
-		ecmd->supported |= SUPPORTED_1000baseT_Full;
-		ecmd->supported |= SUPPORTED_Autoneg;
-		ecmd->advertising |= (ADVERTISED_10000baseT_Full |
-				ADVERTISED_1000baseT_Full);
-	}
 
 	return 0;
 }
@@ -548,7 +639,7 @@ be_get_pauseparam(struct net_device *netdev, struct ethtool_pauseparam *ecmd)
 	struct be_adapter *adapter = netdev_priv(netdev);
 
 	be_cmd_get_flow_control(adapter, &ecmd->tx_pause, &ecmd->rx_pause);
-	ecmd->autoneg = 0;
+	ecmd->autoneg = adapter->phy.fc_autoneg;
 }
 
 static int
@@ -702,7 +793,7 @@ be_self_test(struct net_device *netdev, struct ethtool_test *test, u64 *data)
 		}
 	}
 
-	if (be_test_ddr_dma(adapter) != 0) {
+	if (!lancer_chip(adapter) && be_test_ddr_dma(adapter) != 0) {
 		data[3] = 1;
 		test->flags |= ETH_TEST_FL_FAILED;
 	}
@@ -787,6 +878,81 @@ be_read_eeprom(struct net_device *netdev, struct ethtool_eeprom *eeprom,
 	return status;
 }
 
+static u32 be_get_msg_level(struct net_device *netdev)
+{
+	struct be_adapter *adapter = netdev_priv(netdev);
+
+	if (lancer_chip(adapter)) {
+		dev_err(&adapter->pdev->dev, "Operation not supported\n");
+		return -EOPNOTSUPP;
+	}
+
+	return adapter->msg_enable;
+}
+
+static void be_set_fw_log_level(struct be_adapter *adapter, u32 level)
+{
+	struct be_dma_mem extfat_cmd;
+	struct be_fat_conf_params *cfgs;
+	int status;
+	int i, j;
+
+	memset(&extfat_cmd, 0, sizeof(struct be_dma_mem));
+	extfat_cmd.size = sizeof(struct be_cmd_resp_get_ext_fat_caps);
+	extfat_cmd.va = pci_alloc_consistent(adapter->pdev, extfat_cmd.size,
+					     &extfat_cmd.dma);
+	if (!extfat_cmd.va) {
+		dev_err(&adapter->pdev->dev, "%s: Memory allocation failure\n",
+			__func__);
+		goto err;
+	}
+	status = be_cmd_get_ext_fat_capabilites(adapter, &extfat_cmd);
+	if (!status) {
+		cfgs = (struct be_fat_conf_params *)(extfat_cmd.va +
+					sizeof(struct be_cmd_resp_hdr));
+		for (i = 0; i < cfgs->num_modules; i++) {
+			for (j = 0; j < cfgs->module[i].num_modes; j++) {
+				if (cfgs->module[i].trace_lvl[j].mode ==
+								MODE_UART)
+					cfgs->module[i].trace_lvl[j].dbg_lvl =
+							cpu_to_le32(level);
+			}
+		}
+		status = be_cmd_set_ext_fat_capabilites(adapter, &extfat_cmd,
+							cfgs);
+		if (status)
+			dev_err(&adapter->pdev->dev,
+				"Message level set failed\n");
+	} else {
+		dev_err(&adapter->pdev->dev, "Message level get failed\n");
+	}
+
+	pci_free_consistent(adapter->pdev, extfat_cmd.size, extfat_cmd.va,
+			    extfat_cmd.dma);
+err:
+	return;
+}
+
+static void be_set_msg_level(struct net_device *netdev, u32 level)
+{
+	struct be_adapter *adapter = netdev_priv(netdev);
+
+	if (lancer_chip(adapter)) {
+		dev_err(&adapter->pdev->dev, "Operation not supported\n");
+		return;
+	}
+
+	if (adapter->msg_enable == level)
+		return;
+
+	if ((level & NETIF_MSG_HW) != (adapter->msg_enable & NETIF_MSG_HW))
+		be_set_fw_log_level(adapter, level & NETIF_MSG_HW ?
+				    FW_LOG_LEVEL_DEFAULT : FW_LOG_LEVEL_FATAL);
+	adapter->msg_enable = level;
+
+	return;
+}
+
 const struct ethtool_ops be_ethtool_ops = {
 	.get_settings = be_get_settings,
 	.get_drvinfo = be_get_drvinfo,
@@ -802,6 +968,8 @@ const struct ethtool_ops be_ethtool_ops = {
 	.set_pauseparam = be_set_pauseparam,
 	.get_strings = be_get_stat_strings,
 	.set_phys_id = be_set_phys_id,
+	.get_msglevel = be_get_msg_level,
+	.set_msglevel = be_set_msg_level,
 	.get_sset_count = be_get_sset_count,
 	.get_ethtool_stats = be_get_ethtool_stats,
 	.get_regs_len = be_get_reg_len,
diff --git a/drivers/net/ethernet/emulex/benet/be_hw.h b/drivers/net/ethernet/emulex/benet/be_hw.h
index f2c89e3ccab..d9fb0c501fa 100644
--- a/drivers/net/ethernet/emulex/benet/be_hw.h
+++ b/drivers/net/ethernet/emulex/benet/be_hw.h
@@ -58,6 +58,8 @@
 
 #define SLI_PORT_CONTROL_IP_MASK	0x08000000
 
+#define PCICFG_CUST_SCRATCHPAD_CSR	0x1EC
+
 /********* Memory BAR register ************/
 #define PCICFG_MEMBAR_CTRL_INT_CTRL_OFFSET 	0xfc
 /* Host Interrupt Enable, if set interrupts are enabled although "PCI Interrupt
@@ -98,11 +100,13 @@
 #define SLI_INTF_REV_SHIFT			4
 #define SLI_INTF_FT_MASK			0x00000001
 
+#define SLI_INTF_TYPE_2		2
+#define SLI_INTF_TYPE_3		3
 
 /* SLI family */
 #define BE_SLI_FAMILY		0x0
 #define LANCER_A0_SLI_FAMILY	0xA
-
+#define SKYHAWK_SLI_FAMILY      0x2
 
 /********* ISR0 Register offset **********/
 #define CEV_ISR0_OFFSET 			0xC18
@@ -162,22 +166,23 @@
 #define QUERY_FAT	1
 
 /* Flashrom related descriptors */
+#define MAX_FLASH_COMP			32
 #define IMAGE_TYPE_FIRMWARE		160
 #define IMAGE_TYPE_BOOTCODE		224
 #define IMAGE_TYPE_OPTIONROM		32
 
 #define NUM_FLASHDIR_ENTRIES		32
 
-#define IMG_TYPE_ISCSI_ACTIVE		0
-#define IMG_TYPE_REDBOOT		1
-#define IMG_TYPE_BIOS			2
-#define IMG_TYPE_PXE_BIOS		3
-#define IMG_TYPE_FCOE_BIOS		8
-#define IMG_TYPE_ISCSI_BACKUP		9
-#define IMG_TYPE_FCOE_FW_ACTIVE		10
-#define IMG_TYPE_FCOE_FW_BACKUP 	11
-#define IMG_TYPE_NCSI_FW		13
-#define IMG_TYPE_PHY_FW			99
+#define OPTYPE_ISCSI_ACTIVE		0
+#define OPTYPE_REDBOOT			1
+#define OPTYPE_BIOS			2
+#define OPTYPE_PXE_BIOS			3
+#define OPTYPE_FCOE_BIOS		8
+#define OPTYPE_ISCSI_BACKUP		9
+#define OPTYPE_FCOE_FW_ACTIVE		10
+#define OPTYPE_FCOE_FW_BACKUP		11
+#define OPTYPE_NCSI_FW			13
+#define OPTYPE_PHY_FW			99
 #define TN_8022				13
 
 #define ILLEGAL_IOCTL_REQ		2
@@ -223,6 +228,24 @@
 #define FLASH_REDBOOT_START_g3             (262144)
 #define FLASH_PHY_FW_START_g3		   1310720
 
+#define IMAGE_NCSI			16
+#define IMAGE_OPTION_ROM_PXE		32
+#define IMAGE_OPTION_ROM_FCoE		33
+#define IMAGE_OPTION_ROM_ISCSI		34
+#define IMAGE_FLASHISM_JUMPVECTOR	48
+#define IMAGE_FLASH_ISM			49
+#define IMAGE_JUMP_VECTOR		50
+#define IMAGE_FIRMWARE_iSCSI		160
+#define IMAGE_FIRMWARE_COMP_iSCSI	161
+#define IMAGE_FIRMWARE_FCoE		162
+#define IMAGE_FIRMWARE_COMP_FCoE	163
+#define IMAGE_FIRMWARE_BACKUP_iSCSI	176
+#define IMAGE_FIRMWARE_BACKUP_COMP_iSCSI 177
+#define IMAGE_FIRMWARE_BACKUP_FCoE	178
+#define IMAGE_FIRMWARE_BACKUP_COMP_FCoE 179
+#define IMAGE_FIRMWARE_PHY		192
+#define IMAGE_BOOT_CODE			224
+
 /************* Rx Packet Type Encoding **************/
 #define BE_UNICAST_PACKET		0
 #define BE_MULTICAST_PACKET		1
@@ -445,6 +468,7 @@ struct flash_comp {
 	unsigned long offset;
 	int optype;
 	int size;
+	int img_type;
 };
 
 struct image_hdr {
@@ -481,17 +505,19 @@ struct flash_section_hdr {
 	u32 format_rev;
 	u32 cksum;
 	u32 antidote;
-	u32 build_no;
-	u8 id_string[64];
-	u32 active_entry_mask;
-	u32 valid_entry_mask;
-	u32 org_content_mask;
-	u32 rsvd0;
-	u32 rsvd1;
-	u32 rsvd2;
-	u32 rsvd3;
-	u32 rsvd4;
-};
+	u32 num_images;
+	u8 id_string[128];
+	u32 rsvd[4];
+} __packed;
+
+struct flash_section_hdr_g2 {
+	u32 format_rev;
+	u32 cksum;
+	u32 antidote;
+	u32 build_num;
+	u8 id_string[128];
+	u32 rsvd[8];
+} __packed;
 
 struct flash_section_entry {
 	u32 type;
@@ -503,10 +529,16 @@ struct flash_section_entry {
 	u32 rsvd0;
 	u32 rsvd1;
 	u8 ver_data[32];
-};
+} __packed;
 
 struct flash_section_info {
 	u8 cookie[32];
 	struct flash_section_hdr fsec_hdr;
 	struct flash_section_entry fsec_entry[32];
-};
+} __packed;
+
+struct flash_section_info_g2 {
+	u8 cookie[32];
+	struct flash_section_hdr_g2 fsec_hdr;
+	struct flash_section_entry fsec_entry[32];
+} __packed;
diff --git a/drivers/net/ethernet/emulex/benet/be_main.c b/drivers/net/ethernet/emulex/benet/be_main.c
index 528a886bc2c..08efd308d78 100644
--- a/drivers/net/ethernet/emulex/benet/be_main.c
+++ b/drivers/net/ethernet/emulex/benet/be_main.c
@@ -421,6 +421,9 @@ void be_parse_stats(struct be_adapter *adapter)
 		populate_be2_stats(adapter);
 	}
 
+	if (lancer_chip(adapter))
+		goto done;
+
 	/* as erx_v1 is longer than v0, ok to use v1 defn for v0 access */
 	for_all_rx_queues(adapter, rxo, i) {
 		/* below erx HW counter can actually wrap around after
@@ -429,6 +432,8 @@ void be_parse_stats(struct be_adapter *adapter)
 		accumulate_16bit_val(&rx_stats(rxo)->rx_drops_no_frags,
 				(u16)erx->rx_drops_no_fragments[rxo->q.id]);
 	}
+done:
+	return;
 }
 
 static struct rtnl_link_stats64 *be_get_stats64(struct net_device *netdev,
@@ -797,22 +802,30 @@ static int be_vid_config(struct be_adapter *adapter, bool vf, u32 vf_num)
 	if (adapter->promiscuous)
 		return 0;
 
-	if (adapter->vlans_added <= adapter->max_vlans)  {
-		/* Construct VLAN Table to give to HW */
-		for (i = 0; i < VLAN_N_VID; i++) {
-			if (adapter->vlan_tag[i]) {
-				vtag[ntags] = cpu_to_le16(i);
-				ntags++;
-			}
-		}
-		status = be_cmd_vlan_config(adapter, adapter->if_handle,
-					vtag, ntags, 1, 0);
-	} else {
-		status = be_cmd_vlan_config(adapter, adapter->if_handle,
-					NULL, 0, 1, 1);
+	if (adapter->vlans_added > adapter->max_vlans)
+		goto set_vlan_promisc;
+
+	/* Construct VLAN Table to give to HW */
+	for (i = 0; i < VLAN_N_VID; i++)
+		if (adapter->vlan_tag[i])
+			vtag[ntags++] = cpu_to_le16(i);
+
+	status = be_cmd_vlan_config(adapter, adapter->if_handle,
+				    vtag, ntags, 1, 0);
+
+	/* Set to VLAN promisc mode as setting VLAN filter failed */
+	if (status) {
+		dev_info(&adapter->pdev->dev, "Exhausted VLAN HW filters.\n");
+		dev_info(&adapter->pdev->dev, "Disabling HW VLAN filtering.\n");
+		goto set_vlan_promisc;
 	}
 
 	return status;
+
+set_vlan_promisc:
+	status = be_cmd_vlan_config(adapter, adapter->if_handle,
+				    NULL, 0, 1, 1);
+	return status;
 }
 
 static int be_vlan_add_vid(struct net_device *netdev, u16 vid)
@@ -862,6 +875,7 @@ ret:
 static void be_set_rx_mode(struct net_device *netdev)
 {
 	struct be_adapter *adapter = netdev_priv(netdev);
+	int status;
 
 	if (netdev->flags & IFF_PROMISC) {
 		be_cmd_rx_filter(adapter, IFF_PROMISC, ON);
@@ -908,7 +922,14 @@ static void be_set_rx_mode(struct net_device *netdev)
 		}
 	}
 
-	be_cmd_rx_filter(adapter, IFF_MULTICAST, ON);
+	status = be_cmd_rx_filter(adapter, IFF_MULTICAST, ON);
+
+	/* Set to MCAST promisc mode if setting MULTICAST address fails */
+	if (status) {
+		dev_info(&adapter->pdev->dev, "Exhausted multicast HW filters.\n");
+		dev_info(&adapter->pdev->dev, "Disabling HW multicast filtering.\n");
+		be_cmd_rx_filter(adapter, IFF_ALLMULTI, ON);
+	}
 done:
 	return;
 }
@@ -1028,6 +1049,29 @@ static int be_set_vf_tx_rate(struct net_device *netdev,
 	return status;
 }
 
+static int be_find_vfs(struct be_adapter *adapter, int vf_state)
+{
+	struct pci_dev *dev, *pdev = adapter->pdev;
+	int vfs = 0, assigned_vfs = 0, pos, vf_fn;
+	u16 offset, stride;
+
+	pos = pci_find_ext_capability(pdev, PCI_EXT_CAP_ID_SRIOV);
+	pci_read_config_word(pdev, pos + PCI_SRIOV_VF_OFFSET, &offset);
+	pci_read_config_word(pdev, pos + PCI_SRIOV_VF_STRIDE, &stride);
+
+	dev = pci_get_device(pdev->vendor, PCI_ANY_ID, NULL);
+	while (dev) {
+		vf_fn = (pdev->devfn + offset + stride * vfs) & 0xFFFF;
+		if (dev->is_virtfn && dev->devfn == vf_fn) {
+			vfs++;
+			if (dev->dev_flags & PCI_DEV_FLAGS_ASSIGNED)
+				assigned_vfs++;
+		}
+		dev = pci_get_device(pdev->vendor, PCI_ANY_ID, dev);
+	}
+	return (vf_state == ASSIGNED) ? assigned_vfs : vfs;
+}
+
 static void be_eqd_update(struct be_adapter *adapter, struct be_eq_obj *eqo)
 {
 	struct be_rx_stats *stats = rx_stats(&adapter->rx_obj[eqo->idx]);
@@ -1238,6 +1282,7 @@ static void be_rx_compl_process(struct be_rx_obj *rxo,
 		skb_checksum_none_assert(skb);
 
 	skb->protocol = eth_type_trans(skb, netdev);
+	skb_record_rx_queue(skb, rxo - &adapter->rx_obj[0]);
 	if (netdev->features & NETIF_F_RXHASH)
 		skb->rxhash = rxcp->rss_hash;
 
@@ -1294,6 +1339,7 @@ void be_rx_compl_process_gro(struct be_rx_obj *rxo, struct napi_struct *napi,
 	skb->len = rxcp->pkt_size;
 	skb->data_len = rxcp->pkt_size;
 	skb->ip_summed = CHECKSUM_UNNECESSARY;
+	skb_record_rx_queue(skb, rxo - &adapter->rx_obj[0]);
 	if (adapter->netdev->features & NETIF_F_RXHASH)
 		skb->rxhash = rxcp->rss_hash;
 
@@ -1555,7 +1601,9 @@ static int event_handle(struct be_eq_obj *eqo)
 	if (!num)
 		rearm = true;
 
-	be_eq_notify(eqo->adapter, eqo->q.id, rearm, true, num);
+	if (num || msix_enabled(eqo->adapter))
+		be_eq_notify(eqo->adapter, eqo->q.id, rearm, true, num);
+
 	if (num)
 		napi_schedule(&eqo->napi);
 
@@ -1764,9 +1812,9 @@ static void be_tx_queues_destroy(struct be_adapter *adapter)
 
 static int be_num_txqs_want(struct be_adapter *adapter)
 {
-	if (sriov_enabled(adapter) || be_is_mc(adapter) ||
-		lancer_chip(adapter) || !be_physfn(adapter) ||
-		adapter->generation == BE_GEN2)
+	if (sriov_want(adapter) || be_is_mc(adapter) ||
+	    lancer_chip(adapter) || !be_physfn(adapter) ||
+	    adapter->generation == BE_GEN2)
 		return 1;
 	else
 		return MAX_TX_QS;
@@ -2093,7 +2141,7 @@ static void be_msix_disable(struct be_adapter *adapter)
 static uint be_num_rss_want(struct be_adapter *adapter)
 {
 	if ((adapter->function_caps & BE_FUNCTION_CAPS_RSS) &&
-	     adapter->num_vfs == 0 && be_physfn(adapter) &&
+	     !sriov_want(adapter) && be_physfn(adapter) &&
 	     !be_is_mc(adapter))
 		return (adapter->be3_native) ? BE3_MAX_RSS_QS : BE2_MAX_RSS_QS;
 	else
@@ -2103,10 +2151,17 @@ static uint be_num_rss_want(struct be_adapter *adapter)
 static void be_msix_enable(struct be_adapter *adapter)
 {
 #define BE_MIN_MSIX_VECTORS		1
-	int i, status, num_vec;
+	int i, status, num_vec, num_roce_vec = 0;
 
 	/* If RSS queues are not used, need a vec for default RX Q */
 	num_vec = min(be_num_rss_want(adapter), num_online_cpus());
+	if (be_roce_supported(adapter)) {
+		num_roce_vec = min_t(u32, MAX_ROCE_MSIX_VECTORS,
+					(num_online_cpus() + 1));
+		num_roce_vec = min(num_roce_vec, MAX_ROCE_EQS);
+		num_vec += num_roce_vec;
+		num_vec = min(num_vec, MAX_MSIX_VECTORS);
+	}
 	num_vec = max(num_vec, BE_MIN_MSIX_VECTORS);
 
 	for (i = 0; i < num_vec; i++)
@@ -2123,55 +2178,18 @@ static void be_msix_enable(struct be_adapter *adapter)
 	}
 	return;
 done:
-	adapter->num_msix_vec = num_vec;
-	return;
-}
-
-static int be_sriov_enable(struct be_adapter *adapter)
-{
-	be_check_sriov_fn_type(adapter);
-
-#ifdef CONFIG_PCI_IOV
-	if (be_physfn(adapter) && num_vfs) {
-		int status, pos;
-		u16 dev_vfs;
-
-		pos = pci_find_ext_capability(adapter->pdev,
-						PCI_EXT_CAP_ID_SRIOV);
-		pci_read_config_word(adapter->pdev,
-				     pos + PCI_SRIOV_TOTAL_VF, &dev_vfs);
-
-		adapter->num_vfs = min_t(u16, num_vfs, dev_vfs);
-		if (adapter->num_vfs != num_vfs)
-			dev_info(&adapter->pdev->dev,
-				 "Device supports %d VFs and not %d\n",
-				 adapter->num_vfs, num_vfs);
-
-		status = pci_enable_sriov(adapter->pdev, adapter->num_vfs);
-		if (status)
-			adapter->num_vfs = 0;
-
-		if (adapter->num_vfs) {
-			adapter->vf_cfg = kcalloc(num_vfs,
-						sizeof(struct be_vf_cfg),
-						GFP_KERNEL);
-			if (!adapter->vf_cfg)
-				return -ENOMEM;
+	if (be_roce_supported(adapter)) {
+		if (num_vec > num_roce_vec) {
+			adapter->num_msix_vec = num_vec - num_roce_vec;
+			adapter->num_msix_roce_vec =
+				num_vec - adapter->num_msix_vec;
+		} else {
+			adapter->num_msix_vec = num_vec;
+			adapter->num_msix_roce_vec = 0;
 		}
-	}
-#endif
-	return 0;
-}
-
-static void be_sriov_disable(struct be_adapter *adapter)
-{
-#ifdef CONFIG_PCI_IOV
-	if (sriov_enabled(adapter)) {
-		pci_disable_sriov(adapter->pdev);
-		kfree(adapter->vf_cfg);
-		adapter->num_vfs = 0;
-	}
-#endif
+	} else
+		adapter->num_msix_vec = num_vec;
+	return;
 }
 
 static inline int be_msix_vec_get(struct be_adapter *adapter,
@@ -2282,6 +2300,8 @@ static int be_close(struct net_device *netdev)
 	struct be_eq_obj *eqo;
 	int i;
 
+	be_roce_dev_close(adapter);
+
 	be_async_mcc_disable(adapter);
 
 	if (!lancer_chip(adapter))
@@ -2390,6 +2410,7 @@ static int be_open(struct net_device *netdev)
 	if (!status)
 		be_link_status_update(adapter, link_status);
 
+	be_roce_dev_open(adapter);
 	return 0;
 err:
 	be_close(adapter->netdev);
@@ -2475,6 +2496,11 @@ static void be_vf_clear(struct be_adapter *adapter)
 	struct be_vf_cfg *vf_cfg;
 	u32 vf;
 
+	if (be_find_vfs(adapter, ASSIGNED)) {
+		dev_warn(&adapter->pdev->dev, "VFs are assigned to VMs\n");
+		goto done;
+	}
+
 	for_all_vfs(adapter, vf_cfg, vf) {
 		if (lancer_chip(adapter))
 			be_cmd_set_mac_list(adapter, NULL, 0, vf + 1);
@@ -2484,6 +2510,10 @@ 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;
 }
 
 static int be_clear(struct be_adapter *adapter)
@@ -2513,29 +2543,60 @@ static int be_clear(struct be_adapter *adapter)
 	be_cmd_fw_clean(adapter);
 
 	be_msix_disable(adapter);
-	kfree(adapter->pmac_id);
+	pci_write_config_dword(adapter->pdev, PCICFG_CUST_SCRATCHPAD_CSR, 0);
 	return 0;
 }
 
-static void be_vf_setup_init(struct be_adapter *adapter)
+static int be_vf_setup_init(struct be_adapter *adapter)
 {
 	struct be_vf_cfg *vf_cfg;
 	int vf;
 
+	adapter->vf_cfg = kcalloc(adapter->num_vfs, sizeof(*vf_cfg),
+				  GFP_KERNEL);
+	if (!adapter->vf_cfg)
+		return -ENOMEM;
+
 	for_all_vfs(adapter, vf_cfg, vf) {
 		vf_cfg->if_handle = -1;
 		vf_cfg->pmac_id = -1;
 	}
+	return 0;
 }
 
 static int be_vf_setup(struct be_adapter *adapter)
 {
 	struct be_vf_cfg *vf_cfg;
+	struct device *dev = &adapter->pdev->dev;
 	u32 cap_flags, en_flags, vf;
 	u16 def_vlan, lnk_speed;
-	int status;
+	int status, enabled_vfs;
+
+	enabled_vfs = be_find_vfs(adapter, ENABLED);
+	if (enabled_vfs) {
+		dev_warn(dev, "%d VFs are already enabled\n", enabled_vfs);
+		dev_warn(dev, "Ignoring num_vfs=%d setting\n", num_vfs);
+		return 0;
+	}
+
+	if (num_vfs > adapter->dev_num_vfs) {
+		dev_warn(dev, "Device supports %d VFs and not %d\n",
+			 adapter->dev_num_vfs, num_vfs);
+		num_vfs = adapter->dev_num_vfs;
+	}
 
-	be_vf_setup_init(adapter);
+	status = pci_enable_sriov(adapter->pdev, num_vfs);
+	if (!status) {
+		adapter->num_vfs = num_vfs;
+	} else {
+		/* Platform doesn't support SRIOV though device supports it */
+		dev_warn(dev, "SRIOV enable failed\n");
+		return 0;
+	}
+
+	status = be_vf_setup_init(adapter);
+	if (status)
+		goto err;
 
 	cap_flags = en_flags = BE_IF_FLAGS_UNTAGGED | BE_IF_FLAGS_BROADCAST |
 				BE_IF_FLAGS_MULTICAST;
@@ -2546,9 +2607,11 @@ static int be_vf_setup(struct be_adapter *adapter)
 			goto err;
 	}
 
-	status = be_vf_eth_addr_config(adapter);
-	if (status)
-		goto err;
+	if (!enabled_vfs) {
+		status = be_vf_eth_addr_config(adapter);
+		if (status)
+			goto err;
+	}
 
 	for_all_vfs(adapter, vf_cfg, vf) {
 		status = be_cmd_link_status_query(adapter, NULL, &lnk_speed,
@@ -2571,11 +2634,12 @@ err:
 static void be_setup_init(struct be_adapter *adapter)
 {
 	adapter->vlan_prio_bmap = 0xff;
-	adapter->link_speed = -1;
+	adapter->phy.link_speed = -1;
 	adapter->if_handle = -1;
 	adapter->be3_native = false;
 	adapter->promiscuous = false;
 	adapter->eq_next_idx = 0;
+	adapter->phy.forced_port_speed = -1;
 }
 
 static int be_add_mac_from_list(struct be_adapter *adapter, u8 *mac)
@@ -2604,9 +2668,25 @@ do_none:
 	return status;
 }
 
+/* Routine to query per function resource limits */
+static int be_get_config(struct be_adapter *adapter)
+{
+	int pos;
+	u16 dev_num_vfs;
+
+	pos = pci_find_ext_capability(adapter->pdev, PCI_EXT_CAP_ID_SRIOV);
+	if (pos) {
+		pci_read_config_word(adapter->pdev, pos + PCI_SRIOV_TOTAL_VF,
+				     &dev_num_vfs);
+		adapter->dev_num_vfs = dev_num_vfs;
+	}
+	return 0;
+}
+
 static int be_setup(struct be_adapter *adapter)
 {
 	struct net_device *netdev = adapter->netdev;
+	struct device *dev = &adapter->pdev->dev;
 	u32 cap_flags, en_flags;
 	u32 tx_fc, rx_fc;
 	int status;
@@ -2614,6 +2694,8 @@ static int be_setup(struct be_adapter *adapter)
 
 	be_setup_init(adapter);
 
+	be_get_config(adapter);
+
 	be_cmd_req_native_mode(adapter);
 
 	be_msix_enable(adapter);
@@ -2680,36 +2762,33 @@ static int be_setup(struct be_adapter *adapter)
 
 	be_cmd_get_fw_ver(adapter, adapter->fw_ver, NULL);
 
-	status = be_vid_config(adapter, false, 0);
-	if (status)
-		goto err;
+	be_vid_config(adapter, false, 0);
 
 	be_set_rx_mode(adapter->netdev);
 
-	status = be_cmd_get_flow_control(adapter, &tx_fc, &rx_fc);
-	/* For Lancer: It is legal for this cmd to fail on VF */
-	if (status && (be_physfn(adapter) || !lancer_chip(adapter)))
-		goto err;
+	be_cmd_get_flow_control(adapter, &tx_fc, &rx_fc);
 
-	if (rx_fc != adapter->rx_fc || tx_fc != adapter->tx_fc) {
-		status = be_cmd_set_flow_control(adapter, adapter->tx_fc,
+	if (rx_fc != adapter->rx_fc || tx_fc != adapter->tx_fc)
+		be_cmd_set_flow_control(adapter, adapter->tx_fc,
 					adapter->rx_fc);
-		/* For Lancer: It is legal for this cmd to fail on VF */
-		if (status && (be_physfn(adapter) || !lancer_chip(adapter)))
-			goto err;
-	}
 
 	pcie_set_readrq(adapter->pdev, 4096);
 
-	if (sriov_enabled(adapter)) {
-		status = be_vf_setup(adapter);
-		if (status)
-			goto err;
+	if (be_physfn(adapter) && num_vfs) {
+		if (adapter->dev_num_vfs)
+			be_vf_setup(adapter);
+		else
+			dev_warn(dev, "device doesn't support SRIOV\n");
 	}
 
+	be_cmd_get_phy_info(adapter);
+	if (be_pause_supported(adapter))
+		adapter->phy.fc_autoneg = 1;
+
 	schedule_delayed_work(&adapter->work, msecs_to_jiffies(1000));
 	adapter->flags |= BE_FLAGS_WORKER_SCHEDULED;
 
+	pci_write_config_dword(adapter->pdev, PCICFG_CUST_SCRATCHPAD_CSR, 1);
 	return 0;
 err:
 	be_clear(adapter);
@@ -2731,6 +2810,8 @@ static void be_netpoll(struct net_device *netdev)
 #endif
 
 #define FW_FILE_HDR_SIGN 	"ServerEngines Corp. "
+char flash_cookie[2][16] =      {"*** SE FLAS", "H DIRECTORY *** "};
+
 static bool be_flash_redboot(struct be_adapter *adapter,
 			const u8 *p, u32 img_start, int image_size,
 			int hdr_size)
@@ -2760,71 +2841,105 @@ static bool be_flash_redboot(struct be_adapter *adapter,
 
 static bool phy_flashing_required(struct be_adapter *adapter)
 {
-	int status = 0;
-	struct be_phy_info phy_info;
+	return (adapter->phy.phy_type == TN_8022 &&
+		adapter->phy.interface_type == PHY_TYPE_BASET_10GB);
+}
 
-	status = be_cmd_get_phy_info(adapter, &phy_info);
-	if (status)
-		return false;
-	if ((phy_info.phy_type == TN_8022) &&
-		(phy_info.interface_type == PHY_TYPE_BASET_10GB)) {
-		return true;
+static bool is_comp_in_ufi(struct be_adapter *adapter,
+			   struct flash_section_info *fsec, int type)
+{
+	int i = 0, img_type = 0;
+	struct flash_section_info_g2 *fsec_g2 = NULL;
+
+	if (adapter->generation != BE_GEN3)
+		fsec_g2 = (struct flash_section_info_g2 *)fsec;
+
+	for (i = 0; i < MAX_FLASH_COMP; i++) {
+		if (fsec_g2)
+			img_type = le32_to_cpu(fsec_g2->fsec_entry[i].type);
+		else
+			img_type = le32_to_cpu(fsec->fsec_entry[i].type);
+
+		if (img_type == type)
+			return true;
 	}
 	return false;
+
+}
+
+struct flash_section_info *get_fsec_info(struct be_adapter *adapter,
+					 int header_size,
+					 const struct firmware *fw)
+{
+	struct flash_section_info *fsec = NULL;
+	const u8 *p = fw->data;
+
+	p += header_size;
+	while (p < (fw->data + fw->size)) {
+		fsec = (struct flash_section_info *)p;
+		if (!memcmp(flash_cookie, fsec->cookie, sizeof(flash_cookie)))
+			return fsec;
+		p += 32;
+	}
+	return NULL;
 }
 
 static int be_flash_data(struct be_adapter *adapter,
-			const struct firmware *fw,
-			struct be_dma_mem *flash_cmd, int num_of_images)
+			 const struct firmware *fw,
+			 struct be_dma_mem *flash_cmd,
+			 int num_of_images)
 
 {
 	int status = 0, i, filehdr_size = 0;
+	int img_hdrs_size = (num_of_images * sizeof(struct image_hdr));
 	u32 total_bytes = 0, flash_op;
 	int num_bytes;
 	const u8 *p = fw->data;
 	struct be_cmd_write_flashrom *req = flash_cmd->va;
 	const struct flash_comp *pflashcomp;
-	int num_comp;
-
-	static const struct flash_comp gen3_flash_types[10] = {
-		{ FLASH_iSCSI_PRIMARY_IMAGE_START_g3, IMG_TYPE_ISCSI_ACTIVE,
-			FLASH_IMAGE_MAX_SIZE_g3},
-		{ FLASH_REDBOOT_START_g3, IMG_TYPE_REDBOOT,
-			FLASH_REDBOOT_IMAGE_MAX_SIZE_g3},
-		{ FLASH_iSCSI_BIOS_START_g3, IMG_TYPE_BIOS,
-			FLASH_BIOS_IMAGE_MAX_SIZE_g3},
-		{ FLASH_PXE_BIOS_START_g3, IMG_TYPE_PXE_BIOS,
-			FLASH_BIOS_IMAGE_MAX_SIZE_g3},
-		{ FLASH_FCoE_BIOS_START_g3, IMG_TYPE_FCOE_BIOS,
-			FLASH_BIOS_IMAGE_MAX_SIZE_g3},
-		{ FLASH_iSCSI_BACKUP_IMAGE_START_g3, IMG_TYPE_ISCSI_BACKUP,
-			FLASH_IMAGE_MAX_SIZE_g3},
-		{ FLASH_FCoE_PRIMARY_IMAGE_START_g3, IMG_TYPE_FCOE_FW_ACTIVE,
-			FLASH_IMAGE_MAX_SIZE_g3},
-		{ FLASH_FCoE_BACKUP_IMAGE_START_g3, IMG_TYPE_FCOE_FW_BACKUP,
-			FLASH_IMAGE_MAX_SIZE_g3},
-		{ FLASH_NCSI_START_g3, IMG_TYPE_NCSI_FW,
-			FLASH_NCSI_IMAGE_MAX_SIZE_g3},
-		{ FLASH_PHY_FW_START_g3, IMG_TYPE_PHY_FW,
-			FLASH_PHY_FW_IMAGE_MAX_SIZE_g3}
+	int num_comp, hdr_size;
+	struct flash_section_info *fsec = NULL;
+
+	struct flash_comp gen3_flash_types[] = {
+		{ FLASH_iSCSI_PRIMARY_IMAGE_START_g3, OPTYPE_ISCSI_ACTIVE,
+			FLASH_IMAGE_MAX_SIZE_g3, IMAGE_FIRMWARE_iSCSI},
+		{ FLASH_REDBOOT_START_g3, OPTYPE_REDBOOT,
+			FLASH_REDBOOT_IMAGE_MAX_SIZE_g3, IMAGE_BOOT_CODE},
+		{ FLASH_iSCSI_BIOS_START_g3, OPTYPE_BIOS,
+			FLASH_BIOS_IMAGE_MAX_SIZE_g3, IMAGE_OPTION_ROM_ISCSI},
+		{ FLASH_PXE_BIOS_START_g3, OPTYPE_PXE_BIOS,
+			FLASH_BIOS_IMAGE_MAX_SIZE_g3, IMAGE_OPTION_ROM_PXE},
+		{ FLASH_FCoE_BIOS_START_g3, OPTYPE_FCOE_BIOS,
+			FLASH_BIOS_IMAGE_MAX_SIZE_g3, IMAGE_OPTION_ROM_FCoE},
+		{ FLASH_iSCSI_BACKUP_IMAGE_START_g3, OPTYPE_ISCSI_BACKUP,
+			FLASH_IMAGE_MAX_SIZE_g3, IMAGE_FIRMWARE_BACKUP_iSCSI},
+		{ FLASH_FCoE_PRIMARY_IMAGE_START_g3, OPTYPE_FCOE_FW_ACTIVE,
+			FLASH_IMAGE_MAX_SIZE_g3, IMAGE_FIRMWARE_FCoE},
+		{ FLASH_FCoE_BACKUP_IMAGE_START_g3, OPTYPE_FCOE_FW_BACKUP,
+			FLASH_IMAGE_MAX_SIZE_g3, IMAGE_FIRMWARE_BACKUP_FCoE},
+		{ FLASH_NCSI_START_g3, OPTYPE_NCSI_FW,
+			FLASH_NCSI_IMAGE_MAX_SIZE_g3, IMAGE_NCSI},
+		{ FLASH_PHY_FW_START_g3, OPTYPE_PHY_FW,
+			FLASH_PHY_FW_IMAGE_MAX_SIZE_g3, IMAGE_FIRMWARE_PHY}
 	};
-	static const struct flash_comp gen2_flash_types[8] = {
-		{ FLASH_iSCSI_PRIMARY_IMAGE_START_g2, IMG_TYPE_ISCSI_ACTIVE,
-			FLASH_IMAGE_MAX_SIZE_g2},
-		{ FLASH_REDBOOT_START_g2, IMG_TYPE_REDBOOT,
-			FLASH_REDBOOT_IMAGE_MAX_SIZE_g2},
-		{ FLASH_iSCSI_BIOS_START_g2, IMG_TYPE_BIOS,
-			FLASH_BIOS_IMAGE_MAX_SIZE_g2},
-		{ FLASH_PXE_BIOS_START_g2, IMG_TYPE_PXE_BIOS,
-			FLASH_BIOS_IMAGE_MAX_SIZE_g2},
-		{ FLASH_FCoE_BIOS_START_g2, IMG_TYPE_FCOE_BIOS,
-			FLASH_BIOS_IMAGE_MAX_SIZE_g2},
-		{ FLASH_iSCSI_BACKUP_IMAGE_START_g2, IMG_TYPE_ISCSI_BACKUP,
-			FLASH_IMAGE_MAX_SIZE_g2},
-		{ FLASH_FCoE_PRIMARY_IMAGE_START_g2, IMG_TYPE_FCOE_FW_ACTIVE,
-			FLASH_IMAGE_MAX_SIZE_g2},
-		{ FLASH_FCoE_BACKUP_IMAGE_START_g2, IMG_TYPE_FCOE_FW_BACKUP,
-			 FLASH_IMAGE_MAX_SIZE_g2}
+
+	struct flash_comp gen2_flash_types[] = {
+		{ FLASH_iSCSI_PRIMARY_IMAGE_START_g2, OPTYPE_ISCSI_ACTIVE,
+			FLASH_IMAGE_MAX_SIZE_g2, IMAGE_FIRMWARE_iSCSI},
+		{ FLASH_REDBOOT_START_g2, OPTYPE_REDBOOT,
+			FLASH_REDBOOT_IMAGE_MAX_SIZE_g2, IMAGE_BOOT_CODE},
+		{ FLASH_iSCSI_BIOS_START_g2, OPTYPE_BIOS,
+			FLASH_BIOS_IMAGE_MAX_SIZE_g2, IMAGE_OPTION_ROM_ISCSI},
+		{ FLASH_PXE_BIOS_START_g2, OPTYPE_PXE_BIOS,
+			FLASH_BIOS_IMAGE_MAX_SIZE_g2, IMAGE_OPTION_ROM_PXE},
+		{ FLASH_FCoE_BIOS_START_g2, OPTYPE_FCOE_BIOS,
+			FLASH_BIOS_IMAGE_MAX_SIZE_g2, IMAGE_OPTION_ROM_FCoE},
+		{ FLASH_iSCSI_BACKUP_IMAGE_START_g2, OPTYPE_ISCSI_BACKUP,
+			FLASH_IMAGE_MAX_SIZE_g2, IMAGE_FIRMWARE_BACKUP_iSCSI},
+		{ FLASH_FCoE_PRIMARY_IMAGE_START_g2, OPTYPE_FCOE_FW_ACTIVE,
+			FLASH_IMAGE_MAX_SIZE_g2, IMAGE_FIRMWARE_FCoE},
+		{ FLASH_FCoE_BACKUP_IMAGE_START_g2, OPTYPE_FCOE_FW_BACKUP,
+			 FLASH_IMAGE_MAX_SIZE_g2, IMAGE_FIRMWARE_BACKUP_FCoE}
 	};
 
 	if (adapter->generation == BE_GEN3) {
@@ -2836,22 +2951,37 @@ static int be_flash_data(struct be_adapter *adapter,
 		filehdr_size = sizeof(struct flash_file_hdr_g2);
 		num_comp = ARRAY_SIZE(gen2_flash_types);
 	}
+	/* Get flash section info*/
+	fsec = get_fsec_info(adapter, filehdr_size + img_hdrs_size, fw);
+	if (!fsec) {
+		dev_err(&adapter->pdev->dev,
+			"Invalid Cookie. UFI corrupted ?\n");
+		return -1;
+	}
 	for (i = 0; i < num_comp; i++) {
-		if ((pflashcomp[i].optype == IMG_TYPE_NCSI_FW) &&
-				memcmp(adapter->fw_ver, "3.102.148.0", 11) < 0)
+		if (!is_comp_in_ufi(adapter, fsec, pflashcomp[i].img_type))
+			continue;
+
+		if ((pflashcomp[i].optype == OPTYPE_NCSI_FW) &&
+		    memcmp(adapter->fw_ver, "3.102.148.0", 11) < 0)
 			continue;
-		if (pflashcomp[i].optype == IMG_TYPE_PHY_FW) {
+
+		if (pflashcomp[i].optype == OPTYPE_PHY_FW) {
 			if (!phy_flashing_required(adapter))
 				continue;
 		}
-		if ((pflashcomp[i].optype == IMG_TYPE_REDBOOT) &&
-			(!be_flash_redboot(adapter, fw->data,
-			pflashcomp[i].offset, pflashcomp[i].size, filehdr_size +
-			(num_of_images * sizeof(struct image_hdr)))))
+
+		hdr_size = filehdr_size +
+			   (num_of_images * sizeof(struct image_hdr));
+
+		if ((pflashcomp[i].optype == OPTYPE_REDBOOT) &&
+		    (!be_flash_redboot(adapter, fw->data, pflashcomp[i].offset,
+				       pflashcomp[i].size, hdr_size)))
 			continue;
+
+		/* Flash the component */
 		p = fw->data;
-		p += filehdr_size + pflashcomp[i].offset
-			+ (num_of_images * sizeof(struct image_hdr));
+		p += filehdr_size + pflashcomp[i].offset + img_hdrs_size;
 		if (p + pflashcomp[i].size > fw->data + fw->size)
 			return -1;
 		total_bytes = pflashcomp[i].size;
@@ -2862,12 +2992,12 @@ static int be_flash_data(struct be_adapter *adapter,
 				num_bytes = total_bytes;
 			total_bytes -= num_bytes;
 			if (!total_bytes) {
-				if (pflashcomp[i].optype == IMG_TYPE_PHY_FW)
+				if (pflashcomp[i].optype == OPTYPE_PHY_FW)
 					flash_op = FLASHROM_OPER_PHY_FLASH;
 				else
 					flash_op = FLASHROM_OPER_FLASH;
 			} else {
-				if (pflashcomp[i].optype == IMG_TYPE_PHY_FW)
+				if (pflashcomp[i].optype == OPTYPE_PHY_FW)
 					flash_op = FLASHROM_OPER_PHY_SAVE;
 				else
 					flash_op = FLASHROM_OPER_SAVE;
@@ -2879,7 +3009,7 @@ static int be_flash_data(struct be_adapter *adapter,
 			if (status) {
 				if ((status == ILLEGAL_IOCTL_REQ) &&
 					(pflashcomp[i].optype ==
-						IMG_TYPE_PHY_FW))
+						OPTYPE_PHY_FW))
 					break;
 				dev_err(&adapter->pdev->dev,
 					"cmd to write to flash rom failed.\n");
@@ -3122,6 +3252,24 @@ static void be_unmap_pci_bars(struct be_adapter *adapter)
 		iounmap(adapter->csr);
 	if (adapter->db)
 		iounmap(adapter->db);
+	if (adapter->roce_db.base)
+		pci_iounmap(adapter->pdev, adapter->roce_db.base);
+}
+
+static int lancer_roce_map_pci_bars(struct be_adapter *adapter)
+{
+	struct pci_dev *pdev = adapter->pdev;
+	u8 __iomem *addr;
+
+	addr = pci_iomap(pdev, 2, 0);
+	if (addr == NULL)
+		return -ENOMEM;
+
+	adapter->roce_db.base = addr;
+	adapter->roce_db.io_addr = pci_resource_start(pdev, 2);
+	adapter->roce_db.size = 8192;
+	adapter->roce_db.total_size = pci_resource_len(pdev, 2);
+	return 0;
 }
 
 static int be_map_pci_bars(struct be_adapter *adapter)
@@ -3130,11 +3278,18 @@ static int be_map_pci_bars(struct be_adapter *adapter)
 	int db_reg;
 
 	if (lancer_chip(adapter)) {
-		addr = ioremap_nocache(pci_resource_start(adapter->pdev, 0),
-			pci_resource_len(adapter->pdev, 0));
-		if (addr == NULL)
-			return -ENOMEM;
-		adapter->db = addr;
+		if (be_type_2_3(adapter)) {
+			addr = ioremap_nocache(
+					pci_resource_start(adapter->pdev, 0),
+					pci_resource_len(adapter->pdev, 0));
+			if (addr == NULL)
+				return -ENOMEM;
+			adapter->db = addr;
+		}
+		if (adapter->if_type == SLI_INTF_TYPE_3) {
+			if (lancer_roce_map_pci_bars(adapter))
+				goto pci_map_err;
+		}
 		return 0;
 	}
 
@@ -3159,14 +3314,19 @@ static int be_map_pci_bars(struct be_adapter *adapter)
 	if (addr == NULL)
 		goto pci_map_err;
 	adapter->db = addr;
-
+	if (adapter->sli_family == SKYHAWK_SLI_FAMILY) {
+		adapter->roce_db.size = 4096;
+		adapter->roce_db.io_addr =
+				pci_resource_start(adapter->pdev, db_reg);
+		adapter->roce_db.total_size =
+				pci_resource_len(adapter->pdev, db_reg);
+	}
 	return 0;
 pci_map_err:
 	be_unmap_pci_bars(adapter);
 	return -ENOMEM;
 }
 
-
 static void be_ctrl_cleanup(struct be_adapter *adapter)
 {
 	struct be_dma_mem *mem = &adapter->mbox_mem_alloced;
@@ -3272,6 +3432,8 @@ static void __devexit be_remove(struct pci_dev *pdev)
 	if (!adapter)
 		return;
 
+	be_roce_dev_remove(adapter);
+
 	unregister_netdev(adapter->netdev);
 
 	be_clear(adapter);
@@ -3280,8 +3442,6 @@ static void __devexit be_remove(struct pci_dev *pdev)
 
 	be_ctrl_cleanup(adapter);
 
-	be_sriov_disable(adapter);
-
 	pci_set_drvdata(pdev, NULL);
 	pci_release_regions(pdev);
 	pci_disable_device(pdev);
@@ -3295,9 +3455,43 @@ bool be_is_wol_supported(struct be_adapter *adapter)
 		!be_is_wol_excluded(adapter)) ? true : false;
 }
 
-static int be_get_config(struct be_adapter *adapter)
+u32 be_get_fw_log_level(struct be_adapter *adapter)
 {
+	struct be_dma_mem extfat_cmd;
+	struct be_fat_conf_params *cfgs;
 	int status;
+	u32 level = 0;
+	int j;
+
+	memset(&extfat_cmd, 0, sizeof(struct be_dma_mem));
+	extfat_cmd.size = sizeof(struct be_cmd_resp_get_ext_fat_caps);
+	extfat_cmd.va = pci_alloc_consistent(adapter->pdev, extfat_cmd.size,
+					     &extfat_cmd.dma);
+
+	if (!extfat_cmd.va) {
+		dev_err(&adapter->pdev->dev, "%s: Memory allocation failure\n",
+			__func__);
+		goto err;
+	}
+
+	status = be_cmd_get_ext_fat_capabilites(adapter, &extfat_cmd);
+	if (!status) {
+		cfgs = (struct be_fat_conf_params *)(extfat_cmd.va +
+						sizeof(struct be_cmd_resp_hdr));
+		for (j = 0; j < cfgs->module[0].num_modes; j++) {
+			if (cfgs->module[0].trace_lvl[j].mode == MODE_UART)
+				level = cfgs->module[0].trace_lvl[j].dbg_lvl;
+		}
+	}
+	pci_free_consistent(adapter->pdev, extfat_cmd.size, extfat_cmd.va,
+			    extfat_cmd.dma);
+err:
+	return level;
+}
+static int be_get_initial_config(struct be_adapter *adapter)
+{
+	int status;
+	u32 level;
 
 	status = be_cmd_query_fw_cfg(adapter, &adapter->port_num,
 			&adapter->function_mode, &adapter->function_caps);
@@ -3335,10 +3529,13 @@ static int be_get_config(struct be_adapter *adapter)
 	if (be_is_wol_supported(adapter))
 		adapter->wol = true;
 
+	level = be_get_fw_log_level(adapter);
+	adapter->msg_enable = level <= FW_LOG_LEVEL_DEFAULT ? NETIF_MSG_HW : 0;
+
 	return 0;
 }
 
-static int be_dev_family_check(struct be_adapter *adapter)
+static int be_dev_type_check(struct be_adapter *adapter)
 {
 	struct pci_dev *pdev = adapter->pdev;
 	u32 sli_intf = 0, if_type;
@@ -3350,17 +3547,27 @@ static int be_dev_family_check(struct be_adapter *adapter)
 		break;
 	case BE_DEVICE_ID2:
 	case OC_DEVICE_ID2:
-	case OC_DEVICE_ID5:
 		adapter->generation = BE_GEN3;
 		break;
 	case OC_DEVICE_ID3:
 	case OC_DEVICE_ID4:
 		pci_read_config_dword(pdev, SLI_INTF_REG_OFFSET, &sli_intf);
+		adapter->if_type = (sli_intf & SLI_INTF_IF_TYPE_MASK) >>
+						SLI_INTF_IF_TYPE_SHIFT;
 		if_type = (sli_intf & SLI_INTF_IF_TYPE_MASK) >>
 						SLI_INTF_IF_TYPE_SHIFT;
-
 		if (((sli_intf & SLI_INTF_VALID_MASK) != SLI_INTF_VALID) ||
-			if_type != 0x02) {
+			!be_type_2_3(adapter)) {
+			dev_err(&pdev->dev, "SLI_INTF reg val is not valid\n");
+			return -EINVAL;
+		}
+		adapter->sli_family = ((sli_intf & SLI_INTF_FAMILY_MASK) >>
+					 SLI_INTF_FAMILY_SHIFT);
+		adapter->generation = BE_GEN3;
+		break;
+	case OC_DEVICE_ID5:
+		pci_read_config_dword(pdev, SLI_INTF_REG_OFFSET, &sli_intf);
+		if ((sli_intf & SLI_INTF_VALID_MASK) != SLI_INTF_VALID) {
 			dev_err(&pdev->dev, "SLI_INTF reg val is not valid\n");
 			return -EINVAL;
 		}
@@ -3371,6 +3578,9 @@ static int be_dev_family_check(struct be_adapter *adapter)
 	default:
 		adapter->generation = 0;
 	}
+
+	pci_read_config_dword(adapter->pdev, SLI_INTF_REG_OFFSET, &sli_intf);
+	adapter->virtfn = (sli_intf & SLI_INTF_FT_MASK) ? 1 : 0;
 	return 0;
 }
 
@@ -3514,6 +3724,14 @@ reschedule:
 	schedule_delayed_work(&adapter->work, msecs_to_jiffies(1000));
 }
 
+static bool be_reset_required(struct be_adapter *adapter)
+{
+	u32 reg;
+
+	pci_read_config_dword(adapter->pdev, PCICFG_CUST_SCRATCHPAD_CSR, &reg);
+	return reg;
+}
+
 static int __devinit be_probe(struct pci_dev *pdev,
 			const struct pci_device_id *pdev_id)
 {
@@ -3539,7 +3757,7 @@ static int __devinit be_probe(struct pci_dev *pdev,
 	adapter->pdev = pdev;
 	pci_set_drvdata(pdev, adapter);
 
-	status = be_dev_family_check(adapter);
+	status = be_dev_type_check(adapter);
 	if (status)
 		goto free_netdev;
 
@@ -3557,13 +3775,9 @@ static int __devinit be_probe(struct pci_dev *pdev,
 		}
 	}
 
-	status = be_sriov_enable(adapter);
-	if (status)
-		goto free_netdev;
-
 	status = be_ctrl_init(adapter);
 	if (status)
-		goto disable_sriov;
+		goto free_netdev;
 
 	if (lancer_chip(adapter)) {
 		status = lancer_wait_ready(adapter);
@@ -3590,9 +3804,11 @@ static int __devinit be_probe(struct pci_dev *pdev,
 	if (status)
 		goto ctrl_clean;
 
-	status = be_cmd_reset_function(adapter);
-	if (status)
-		goto ctrl_clean;
+	if (be_reset_required(adapter)) {
+		status = be_cmd_reset_function(adapter);
+		if (status)
+			goto ctrl_clean;
+	}
 
 	/* The INTR bit may be set in the card when probed by a kdump kernel
 	 * after a crash.
@@ -3604,7 +3820,7 @@ static int __devinit be_probe(struct pci_dev *pdev,
 	if (status)
 		goto ctrl_clean;
 
-	status = be_get_config(adapter);
+	status = be_get_initial_config(adapter);
 	if (status)
 		goto stats_clean;
 
@@ -3620,6 +3836,8 @@ static int __devinit be_probe(struct pci_dev *pdev,
 	if (status != 0)
 		goto unsetup;
 
+	be_roce_dev_add(adapter);
+
 	dev_info(&pdev->dev, "%s: %s port %d\n", netdev->name, nic_name(pdev),
 		adapter->port_num);
 
@@ -3633,8 +3851,6 @@ stats_clean:
 	be_stats_cleanup(adapter);
 ctrl_clean:
 	be_ctrl_cleanup(adapter);
-disable_sriov:
-	be_sriov_disable(adapter);
 free_netdev:
 	free_netdev(netdev);
 	pci_set_drvdata(pdev, NULL);
@@ -3749,6 +3965,11 @@ static pci_ers_result_t be_eeh_err_detected(struct pci_dev *pdev,
 
 	pci_disable_device(pdev);
 
+	/* The error could cause the FW to trigger a flash debug dump.
+	 * Resetting the card while flash dump is in progress
+	 * can cause it not to recover; wait for it to finish
+	 */
+	ssleep(30);
 	return PCI_ERS_RESULT_NEED_RESET;
 }
 
diff --git a/drivers/net/ethernet/emulex/benet/be_roce.c b/drivers/net/ethernet/emulex/benet/be_roce.c
new file mode 100644
index 00000000000..deecc44b361
--- /dev/null
+++ b/drivers/net/ethernet/emulex/benet/be_roce.c
@@ -0,0 +1,182 @@
+/*
+ * Copyright (C) 2005 - 2011 Emulex
+ * 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. The full GNU General
+ * Public License is included in this distribution in the file called COPYING.
+ *
+ * Contact Information:
+ * linux-drivers@emulex.com
+ *
+ * Emulex
+ * 3333 Susan Street
+ * Costa Mesa, CA 92626
+ */
+
+#include <linux/mutex.h>
+#include <linux/list.h>
+#include <linux/netdevice.h>
+#include <linux/module.h>
+
+#include "be.h"
+#include "be_cmds.h"
+
+static struct ocrdma_driver *ocrdma_drv;
+static LIST_HEAD(be_adapter_list);
+static DEFINE_MUTEX(be_adapter_list_lock);
+
+static void _be_roce_dev_add(struct be_adapter *adapter)
+{
+	struct be_dev_info dev_info;
+	int i, num_vec;
+	struct pci_dev *pdev = adapter->pdev;
+
+	if (!ocrdma_drv)
+		return;
+	if (pdev->device == OC_DEVICE_ID5) {
+		/* only msix is supported on these devices */
+		if (!msix_enabled(adapter))
+			return;
+		/* DPP region address and length */
+		dev_info.dpp_unmapped_addr = pci_resource_start(pdev, 2);
+		dev_info.dpp_unmapped_len = pci_resource_len(pdev, 2);
+	} else {
+		dev_info.dpp_unmapped_addr = 0;
+		dev_info.dpp_unmapped_len = 0;
+	}
+	dev_info.pdev = adapter->pdev;
+	if (adapter->sli_family == SKYHAWK_SLI_FAMILY)
+		dev_info.db = adapter->db;
+	else
+		dev_info.db = adapter->roce_db.base;
+	dev_info.unmapped_db = adapter->roce_db.io_addr;
+	dev_info.db_page_size = adapter->roce_db.size;
+	dev_info.db_total_size = adapter->roce_db.total_size;
+	dev_info.netdev = adapter->netdev;
+	memcpy(dev_info.mac_addr, adapter->netdev->dev_addr, ETH_ALEN);
+	dev_info.dev_family = adapter->sli_family;
+	if (msix_enabled(adapter)) {
+		/* provide all the vectors, so that EQ creation response
+		 * can decide which one to use.
+		 */
+		num_vec = adapter->num_msix_vec + adapter->num_msix_roce_vec;
+		dev_info.intr_mode = BE_INTERRUPT_MODE_MSIX;
+		dev_info.msix.num_vectors = min(num_vec, MAX_ROCE_MSIX_VECTORS);
+		/* provide start index of the vector,
+		 * so in case of linear usage,
+		 * it can use the base as starting point.
+		 */
+		dev_info.msix.start_vector = adapter->num_evt_qs;
+		for (i = 0; i < dev_info.msix.num_vectors; i++) {
+			dev_info.msix.vector_list[i] =
+			    adapter->msix_entries[i].vector;
+		}
+	} else {
+		dev_info.msix.num_vectors = 0;
+		dev_info.intr_mode = BE_INTERRUPT_MODE_INTX;
+	}
+	adapter->ocrdma_dev = ocrdma_drv->add(&dev_info);
+}
+
+void be_roce_dev_add(struct be_adapter *adapter)
+{
+	if (be_roce_supported(adapter)) {
+		INIT_LIST_HEAD(&adapter->entry);
+		mutex_lock(&be_adapter_list_lock);
+		list_add_tail(&adapter->entry, &be_adapter_list);
+
+		/* invoke add() routine of roce driver only if
+		 * valid driver registered with add method and add() is not yet
+		 * invoked on a given adapter.
+		 */
+		_be_roce_dev_add(adapter);
+		mutex_unlock(&be_adapter_list_lock);
+	}
+}
+
+void _be_roce_dev_remove(struct be_adapter *adapter)
+{
+	if (ocrdma_drv && ocrdma_drv->remove && adapter->ocrdma_dev)
+		ocrdma_drv->remove(adapter->ocrdma_dev);
+	adapter->ocrdma_dev = NULL;
+}
+
+void be_roce_dev_remove(struct be_adapter *adapter)
+{
+	if (be_roce_supported(adapter)) {
+		mutex_lock(&be_adapter_list_lock);
+		_be_roce_dev_remove(adapter);
+		list_del(&adapter->entry);
+		mutex_unlock(&be_adapter_list_lock);
+	}
+}
+
+void _be_roce_dev_open(struct be_adapter *adapter)
+{
+	if (ocrdma_drv && adapter->ocrdma_dev &&
+	    ocrdma_drv->state_change_handler)
+		ocrdma_drv->state_change_handler(adapter->ocrdma_dev, 0);
+}
+
+void be_roce_dev_open(struct be_adapter *adapter)
+{
+	if (be_roce_supported(adapter)) {
+		mutex_lock(&be_adapter_list_lock);
+		_be_roce_dev_open(adapter);
+		mutex_unlock(&be_adapter_list_lock);
+	}
+}
+
+void _be_roce_dev_close(struct be_adapter *adapter)
+{
+	if (ocrdma_drv && adapter->ocrdma_dev &&
+	    ocrdma_drv->state_change_handler)
+		ocrdma_drv->state_change_handler(adapter->ocrdma_dev, 1);
+}
+
+void be_roce_dev_close(struct be_adapter *adapter)
+{
+	if (be_roce_supported(adapter)) {
+		mutex_lock(&be_adapter_list_lock);
+		_be_roce_dev_close(adapter);
+		mutex_unlock(&be_adapter_list_lock);
+	}
+}
+
+int be_roce_register_driver(struct ocrdma_driver *drv)
+{
+	struct be_adapter *dev;
+
+	mutex_lock(&be_adapter_list_lock);
+	if (ocrdma_drv) {
+		mutex_unlock(&be_adapter_list_lock);
+		return -EINVAL;
+	}
+	ocrdma_drv = drv;
+	list_for_each_entry(dev, &be_adapter_list, entry) {
+		struct net_device *netdev;
+		_be_roce_dev_add(dev);
+		netdev = dev->netdev;
+		if (netif_running(netdev) && netif_oper_up(netdev))
+			_be_roce_dev_open(dev);
+	}
+	mutex_unlock(&be_adapter_list_lock);
+	return 0;
+}
+EXPORT_SYMBOL(be_roce_register_driver);
+
+void be_roce_unregister_driver(struct ocrdma_driver *drv)
+{
+	struct be_adapter *dev;
+
+	mutex_lock(&be_adapter_list_lock);
+	list_for_each_entry(dev, &be_adapter_list, entry) {
+		if (dev->ocrdma_dev)
+			_be_roce_dev_remove(dev);
+	}
+	ocrdma_drv = NULL;
+	mutex_unlock(&be_adapter_list_lock);
+}
+EXPORT_SYMBOL(be_roce_unregister_driver);
diff --git a/drivers/net/ethernet/emulex/benet/be_roce.h b/drivers/net/ethernet/emulex/benet/be_roce.h
new file mode 100644
index 00000000000..db4ea8081c0
--- /dev/null
+++ b/drivers/net/ethernet/emulex/benet/be_roce.h
@@ -0,0 +1,75 @@
+/*
+ * Copyright (C) 2005 - 2011 Emulex
+ * 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. The full GNU General
+ * Public License is included in this distribution in the file called COPYING.
+ *
+ * Contact Information:
+ * linux-drivers@emulex.com
+ *
+ * Emulex
+ * 3333 Susan Street
+ * Costa Mesa, CA 92626
+ */
+
+#ifndef BE_ROCE_H
+#define BE_ROCE_H
+
+#include <linux/pci.h>
+#include <linux/netdevice.h>
+
+struct ocrdma_dev;
+
+enum be_interrupt_mode {
+	BE_INTERRUPT_MODE_MSIX	= 0,
+	BE_INTERRUPT_MODE_INTX	= 1,
+	BE_INTERRUPT_MODE_MSI	= 2,
+};
+
+#define MAX_ROCE_MSIX_VECTORS   16
+struct be_dev_info {
+	u8 __iomem *db;
+	u64 unmapped_db;
+	u32 db_page_size;
+	u32 db_total_size;
+	u64 dpp_unmapped_addr;
+	u32 dpp_unmapped_len;
+	struct pci_dev *pdev;
+	struct net_device *netdev;
+	u8 mac_addr[ETH_ALEN];
+	u32 dev_family;
+	enum be_interrupt_mode intr_mode;
+	struct {
+		int num_vectors;
+		int start_vector;
+		u32 vector_list[MAX_ROCE_MSIX_VECTORS];
+	} msix;
+};
+
+/* ocrdma driver register's the callback functions with nic driver. */
+struct ocrdma_driver {
+	unsigned char name[32];
+	struct ocrdma_dev *(*add) (struct be_dev_info *dev_info);
+	void (*remove) (struct ocrdma_dev *);
+	void (*state_change_handler) (struct ocrdma_dev *, u32 new_state);
+};
+
+enum {
+	BE_DEV_UP	= 0,
+	BE_DEV_DOWN	= 1
+};
+
+/* APIs for RoCE driver to register callback handlers,
+ * which will be invoked when device is added, removed, ifup, ifdown
+ */
+int be_roce_register_driver(struct ocrdma_driver *drv);
+void be_roce_unregister_driver(struct ocrdma_driver *drv);
+
+/* API for RoCE driver to issue mailbox commands */
+int be_roce_mcc_cmd(void *netdev_handle, void *wrb_payload,
+		    int wrb_payload_size, u16 *cmd_status, u16 *ext_status);
+
+#endif /* BE_ROCE_H */
diff --git a/drivers/net/ethernet/fealnx.c b/drivers/net/ethernet/fealnx.c
index 1637b986229..9d71c9cc300 100644
--- a/drivers/net/ethernet/fealnx.c
+++ b/drivers/net/ethernet/fealnx.c
@@ -545,9 +545,6 @@ static int __devinit fealnx_init_one(struct pci_dev *pdev,
 	/* Reset the chip to erase previous misconfiguration. */
 	iowrite32(0x00000001, ioaddr + BCR);
 
-	dev->base_addr = (unsigned long)ioaddr;
-	dev->irq = irq;
-
 	/* Make certain the descriptor lists are aligned. */
 	np = netdev_priv(dev);
 	np->mem = ioaddr;
@@ -832,11 +829,13 @@ static int netdev_open(struct net_device *dev)
 {
 	struct netdev_private *np = netdev_priv(dev);
 	void __iomem *ioaddr = np->mem;
-	int i;
+	const int irq = np->pci_dev->irq;
+	int rc, i;
 
 	iowrite32(0x00000001, ioaddr + BCR);	/* Reset */
 
-	if (request_irq(dev->irq, intr_handler, IRQF_SHARED, dev->name, dev))
+	rc = request_irq(irq, intr_handler, IRQF_SHARED, dev->name, dev);
+	if (rc)
 		return -EAGAIN;
 
 	for (i = 0; i < 3; i++)
@@ -924,8 +923,7 @@ static int netdev_open(struct net_device *dev)
 	np->reset_timer.data = (unsigned long) dev;
 	np->reset_timer.function = reset_timer;
 	np->reset_timer_armed = 0;
-
-	return 0;
+	return rc;
 }
 
 
@@ -1910,7 +1908,7 @@ static int netdev_close(struct net_device *dev)
 	del_timer_sync(&np->timer);
 	del_timer_sync(&np->reset_timer);
 
-	free_irq(dev->irq, dev);
+	free_irq(np->pci_dev->irq, dev);
 
 	/* Free all the skbuffs in the Rx queue. */
 	for (i = 0; i < RX_RING_SIZE; i++) {
diff --git a/drivers/net/ethernet/freescale/fec.c b/drivers/net/ethernet/freescale/fec.c
index a12b3f5bc02..ff7f4c5115a 100644
--- a/drivers/net/ethernet/freescale/fec.c
+++ b/drivers/net/ethernet/freescale/fec.c
@@ -48,6 +48,7 @@
 #include <linux/of_device.h>
 #include <linux/of_gpio.h>
 #include <linux/of_net.h>
+#include <linux/pinctrl/consumer.h>
 
 #include <asm/cacheflush.h>
 
@@ -206,7 +207,8 @@ struct fec_enet_private {
 
 	struct net_device *netdev;
 
-	struct clk *clk;
+	struct clk *clk_ipg;
+	struct clk *clk_ahb;
 
 	/* The saved address of a sent-in-place packet/buffer, for skfree(). */
 	unsigned char *tx_bounce[TX_RING_SIZE];
@@ -1064,7 +1066,7 @@ static int fec_enet_mii_init(struct platform_device *pdev)
 	 * Reference Manual has an error on this, and gets fixed on i.MX6Q
 	 * document.
 	 */
-	fep->phy_speed = DIV_ROUND_UP(clk_get_rate(fep->clk), 5000000);
+	fep->phy_speed = DIV_ROUND_UP(clk_get_rate(fep->clk_ahb), 5000000);
 	if (id_entry->driver_data & FEC_QUIRK_ENET_MAC)
 		fep->phy_speed--;
 	fep->phy_speed <<= 1;
@@ -1161,6 +1163,7 @@ static const struct ethtool_ops fec_enet_ethtool_ops = {
 	.set_settings		= fec_enet_set_settings,
 	.get_drvinfo		= fec_enet_get_drvinfo,
 	.get_link		= ethtool_op_get_link,
+	.get_ts_info		= ethtool_op_get_ts_info,
 };
 
 static int fec_enet_ioctl(struct net_device *ndev, struct ifreq *rq, int cmd)
@@ -1542,6 +1545,7 @@ fec_probe(struct platform_device *pdev)
 	struct resource *r;
 	const struct of_device_id *of_id;
 	static int dev_id;
+	struct pinctrl *pinctrl;
 
 	of_id = of_match_device(fec_dt_ids, &pdev->dev);
 	if (of_id)
@@ -1609,12 +1613,26 @@ fec_probe(struct platform_device *pdev)
 		}
 	}
 
-	fep->clk = clk_get(&pdev->dev, NULL);
-	if (IS_ERR(fep->clk)) {
-		ret = PTR_ERR(fep->clk);
+	pinctrl = devm_pinctrl_get_select_default(&pdev->dev);
+	if (IS_ERR(pinctrl)) {
+		ret = PTR_ERR(pinctrl);
+		goto failed_pin;
+	}
+
+	fep->clk_ipg = devm_clk_get(&pdev->dev, "ipg");
+	if (IS_ERR(fep->clk_ipg)) {
+		ret = PTR_ERR(fep->clk_ipg);
+		goto failed_clk;
+	}
+
+	fep->clk_ahb = devm_clk_get(&pdev->dev, "ahb");
+	if (IS_ERR(fep->clk_ahb)) {
+		ret = PTR_ERR(fep->clk_ahb);
 		goto failed_clk;
 	}
-	clk_prepare_enable(fep->clk);
+
+	clk_prepare_enable(fep->clk_ahb);
+	clk_prepare_enable(fep->clk_ipg);
 
 	ret = fec_enet_init(ndev);
 	if (ret)
@@ -1637,8 +1655,9 @@ failed_register:
 	fec_enet_mii_remove(fep);
 failed_mii_init:
 failed_init:
-	clk_disable_unprepare(fep->clk);
-	clk_put(fep->clk);
+	clk_disable_unprepare(fep->clk_ahb);
+	clk_disable_unprepare(fep->clk_ipg);
+failed_pin:
 failed_clk:
 	for (i = 0; i < FEC_IRQ_NUM; i++) {
 		irq = platform_get_irq(pdev, i);
@@ -1670,8 +1689,8 @@ fec_drv_remove(struct platform_device *pdev)
 		if (irq > 0)
 			free_irq(irq, ndev);
 	}
-	clk_disable_unprepare(fep->clk);
-	clk_put(fep->clk);
+	clk_disable_unprepare(fep->clk_ahb);
+	clk_disable_unprepare(fep->clk_ipg);
 	iounmap(fep->hwp);
 	free_netdev(ndev);
 
@@ -1695,7 +1714,8 @@ fec_suspend(struct device *dev)
 		fec_stop(ndev);
 		netif_device_detach(ndev);
 	}
-	clk_disable_unprepare(fep->clk);
+	clk_disable_unprepare(fep->clk_ahb);
+	clk_disable_unprepare(fep->clk_ipg);
 
 	return 0;
 }
@@ -1706,7 +1726,8 @@ 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);
+	clk_prepare_enable(fep->clk_ahb);
+	clk_prepare_enable(fep->clk_ipg);
 	if (netif_running(ndev)) {
 		fec_restart(ndev, fep->full_duplex);
 		netif_device_attach(ndev);
diff --git a/drivers/net/ethernet/freescale/fec_mpc52xx.c b/drivers/net/ethernet/freescale/fec_mpc52xx.c
index 7b34d8c698d..97f947b3d94 100644
--- a/drivers/net/ethernet/freescale/fec_mpc52xx.c
+++ b/drivers/net/ethernet/freescale/fec_mpc52xx.c
@@ -811,6 +811,7 @@ static const struct ethtool_ops mpc52xx_fec_ethtool_ops = {
 	.get_link = ethtool_op_get_link,
 	.get_msglevel = mpc52xx_fec_get_msglevel,
 	.set_msglevel = mpc52xx_fec_set_msglevel,
+	.get_ts_info = ethtool_op_get_ts_info,
 };
 
 
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 e4e6cd2c5f8..2b7633f766d 100644
--- a/drivers/net/ethernet/freescale/fs_enet/fs_enet-main.c
+++ b/drivers/net/ethernet/freescale/fs_enet/fs_enet-main.c
@@ -963,6 +963,7 @@ static const struct ethtool_ops fs_ethtool_ops = {
 	.get_msglevel = fs_get_msglevel,
 	.set_msglevel = fs_set_msglevel,
 	.get_regs = fs_get_regs,
+	.get_ts_info = ethtool_op_get_ts_info,
 };
 
 static int fs_ioctl(struct net_device *dev, struct ifreq *rq, int cmd)
diff --git a/drivers/net/ethernet/freescale/gianfar.c b/drivers/net/ethernet/freescale/gianfar.c
index e7bed530399..0741aded9eb 100644
--- a/drivers/net/ethernet/freescale/gianfar.c
+++ b/drivers/net/ethernet/freescale/gianfar.c
@@ -136,7 +136,7 @@ static void gfar_netpoll(struct net_device *dev);
 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 int gfar_process_frame(struct net_device *dev, struct sk_buff *skb,
-			      int amount_pull);
+			      int amount_pull, struct napi_struct *napi);
 void gfar_halt(struct net_device *dev);
 static void gfar_halt_nodisable(struct net_device *dev);
 void gfar_start(struct net_device *dev);
@@ -1082,7 +1082,7 @@ static int gfar_probe(struct platform_device *ofdev)
 
 	if (dev->features & NETIF_F_IP_CSUM ||
 			priv->device_flags & FSL_GIANFAR_DEV_HAS_TIMER)
-		dev->hard_header_len += GMAC_FCB_LEN;
+		dev->needed_headroom = GMAC_FCB_LEN;
 
 	/* Program the isrg regs only if number of grps > 1 */
 	if (priv->num_grps > 1) {
@@ -2675,12 +2675,12 @@ static inline void gfar_rx_checksum(struct sk_buff *skb, struct rxfcb *fcb)
 /* gfar_process_frame() -- handle one incoming packet if skb
  * isn't NULL.  */
 static int gfar_process_frame(struct net_device *dev, struct sk_buff *skb,
-			      int amount_pull)
+			      int amount_pull, struct napi_struct *napi)
 {
 	struct gfar_private *priv = netdev_priv(dev);
 	struct rxfcb *fcb = NULL;
 
-	int ret;
+	gro_result_t ret;
 
 	/* fcb is at the beginning if exists */
 	fcb = (struct rxfcb *)skb->data;
@@ -2719,9 +2719,9 @@ static int gfar_process_frame(struct net_device *dev, struct sk_buff *skb,
 		__vlan_hwaccel_put_tag(skb, fcb->vlctl);
 
 	/* Send the packet up the stack */
-	ret = netif_receive_skb(skb);
+	ret = napi_gro_receive(napi, skb);
 
-	if (NET_RX_DROP == ret)
+	if (GRO_DROP == ret)
 		priv->extra_stats.kernel_dropped++;
 
 	return 0;
@@ -2783,7 +2783,8 @@ int gfar_clean_rx_ring(struct gfar_priv_rx_q *rx_queue, int rx_work_limit)
 				skb_put(skb, pkt_len);
 				rx_queue->stats.rx_bytes += pkt_len;
 				skb_record_rx_queue(skb, rx_queue->qindex);
-				gfar_process_frame(dev, skb, amount_pull);
+				gfar_process_frame(dev, skb, amount_pull,
+						&rx_queue->grp->napi);
 
 			} else {
 				netif_warn(priv, rx_err, dev, "Missing skb!\n");
diff --git a/drivers/net/ethernet/freescale/gianfar.h b/drivers/net/ethernet/freescale/gianfar.h
index 4c9f8d487db..2136c7ff5e6 100644
--- a/drivers/net/ethernet/freescale/gianfar.h
+++ b/drivers/net/ethernet/freescale/gianfar.h
@@ -1210,4 +1210,7 @@ struct filer_table {
 	struct gfar_filer_entry fe[MAX_FILER_CACHE_IDX + 20];
 };
 
+/* The gianfar_ptp module will set this variable */
+extern int gfar_phc_index;
+
 #endif /* __GIANFAR_H */
diff --git a/drivers/net/ethernet/freescale/gianfar_ethtool.c b/drivers/net/ethernet/freescale/gianfar_ethtool.c
index 8d74efd04bb..8a025570d97 100644
--- a/drivers/net/ethernet/freescale/gianfar_ethtool.c
+++ b/drivers/net/ethernet/freescale/gianfar_ethtool.c
@@ -26,6 +26,7 @@
 #include <linux/delay.h>
 #include <linux/netdevice.h>
 #include <linux/etherdevice.h>
+#include <linux/net_tstamp.h>
 #include <linux/skbuff.h>
 #include <linux/spinlock.h>
 #include <linux/mm.h>
@@ -1739,6 +1740,34 @@ static int gfar_get_nfc(struct net_device *dev, struct ethtool_rxnfc *cmd,
 	return ret;
 }
 
+int gfar_phc_index = -1;
+
+static int gfar_get_ts_info(struct net_device *dev,
+			    struct ethtool_ts_info *info)
+{
+	struct gfar_private *priv = netdev_priv(dev);
+
+	if (!(priv->device_flags & FSL_GIANFAR_DEV_HAS_TIMER)) {
+		info->so_timestamping =
+			SOF_TIMESTAMPING_RX_SOFTWARE |
+			SOF_TIMESTAMPING_SOFTWARE;
+		info->phc_index = -1;
+		return 0;
+	}
+	info->so_timestamping =
+		SOF_TIMESTAMPING_TX_HARDWARE |
+		SOF_TIMESTAMPING_RX_HARDWARE |
+		SOF_TIMESTAMPING_RAW_HARDWARE;
+	info->phc_index = gfar_phc_index;
+	info->tx_types =
+		(1 << HWTSTAMP_TX_OFF) |
+		(1 << HWTSTAMP_TX_ON);
+	info->rx_filters =
+		(1 << HWTSTAMP_FILTER_NONE) |
+		(1 << HWTSTAMP_FILTER_ALL);
+	return 0;
+}
+
 const struct ethtool_ops gfar_ethtool_ops = {
 	.get_settings = gfar_gsettings,
 	.set_settings = gfar_ssettings,
@@ -1761,4 +1790,5 @@ const struct ethtool_ops gfar_ethtool_ops = {
 #endif
 	.set_rxnfc = gfar_set_nfc,
 	.get_rxnfc = gfar_get_nfc,
+	.get_ts_info = gfar_get_ts_info,
 };
diff --git a/drivers/net/ethernet/freescale/gianfar_ptp.c b/drivers/net/ethernet/freescale/gianfar_ptp.c
index 5fd620bec15..c08e5d40fec 100644
--- a/drivers/net/ethernet/freescale/gianfar_ptp.c
+++ b/drivers/net/ethernet/freescale/gianfar_ptp.c
@@ -515,6 +515,7 @@ static int gianfar_ptp_probe(struct platform_device *dev)
 		err = PTR_ERR(etsects->clock);
 		goto no_clock;
 	}
+	gfar_phc_clock = ptp_clock_index(etsects->clock);
 
 	dev_set_drvdata(&dev->dev, etsects);
 
@@ -538,6 +539,7 @@ static int gianfar_ptp_remove(struct platform_device *dev)
 	gfar_write(&etsects->regs->tmr_temask, 0);
 	gfar_write(&etsects->regs->tmr_ctrl,   0);
 
+	gfar_phc_clock = -1;
 	ptp_clock_unregister(etsects->clock);
 	iounmap(etsects->regs);
 	release_resource(etsects->rsrc);
diff --git a/drivers/net/ethernet/freescale/ucc_geth_ethtool.c b/drivers/net/ethernet/freescale/ucc_geth_ethtool.c
index a97257f91a3..37b03530601 100644
--- a/drivers/net/ethernet/freescale/ucc_geth_ethtool.c
+++ b/drivers/net/ethernet/freescale/ucc_geth_ethtool.c
@@ -415,6 +415,7 @@ static const struct ethtool_ops uec_ethtool_ops = {
 	.get_ethtool_stats      = uec_get_ethtool_stats,
 	.get_wol		= uec_get_wol,
 	.set_wol		= uec_set_wol,
+	.get_ts_info		= ethtool_op_get_ts_info,
 };
 
 void uec_set_ethtool_ops(struct net_device *netdev)
diff --git a/drivers/net/ethernet/fujitsu/at1700.c b/drivers/net/ethernet/fujitsu/at1700.c
index 3d94797c8f9..4b80dc4531a 100644
--- a/drivers/net/ethernet/fujitsu/at1700.c
+++ b/drivers/net/ethernet/fujitsu/at1700.c
@@ -27,7 +27,7 @@
 	ATI provided their EEPROM configuration code header file.
     Thanks to NIIBE Yutaka <gniibe@mri.co.jp> for bug fixes.
 
-    MCA bus (AT1720) support by Rene Schmit <rene@bss.lu>
+    MCA bus (AT1720) support (now deleted) by Rene Schmit <rene@bss.lu>
 
   Bugs:
 	The MB86965 has a design flaw that makes all probes unreliable.  Not
@@ -38,7 +38,6 @@
 #include <linux/errno.h>
 #include <linux/netdevice.h>
 #include <linux/etherdevice.h>
-#include <linux/mca-legacy.h>
 #include <linux/module.h>
 #include <linux/kernel.h>
 #include <linux/types.h>
@@ -79,24 +78,6 @@ static unsigned at1700_probe_list[] __initdata = {
 	0x260, 0x280, 0x2a0, 0x240, 0x340, 0x320, 0x380, 0x300, 0
 };
 
-/*
- *	MCA
- */
-#ifdef CONFIG_MCA_LEGACY
-static int at1700_ioaddr_pattern[] __initdata = {
-	0x00, 0x04, 0x01, 0x05, 0x02, 0x06, 0x03, 0x07
-};
-
-static int at1700_mca_probe_list[] __initdata = {
-	0x400, 0x1400, 0x2400, 0x3400, 0x4400, 0x5400, 0x6400, 0x7400, 0
-};
-
-static int at1700_irq_pattern[] __initdata = {
-	0x00, 0x00, 0x00, 0x30, 0x70, 0xb0, 0x00, 0x00,
-	0x00, 0xf0, 0x34, 0x74, 0xb4, 0x00, 0x00, 0xf4, 0x00
-};
-#endif
-
 /* use 0 for production, 1 for verification, >2 for debug */
 #ifndef NET_DEBUG
 #define NET_DEBUG 1
@@ -114,7 +95,6 @@ struct net_local {
 	uint tx_queue_ready:1;			/* Tx queue is ready to be sent. */
 	uint rx_started:1;			/* Packets are Rxing. */
 	uchar tx_queue;				/* Number of packet on the Tx queue. */
-	char mca_slot;				/* -1 means ISA */
 	ushort tx_queue_len;			/* Current length of the Tx queue. */
 };
 
@@ -166,21 +146,6 @@ static void set_rx_mode(struct net_device *dev);
 static void net_tx_timeout (struct net_device *dev);
 
 
-#ifdef CONFIG_MCA_LEGACY
-struct at1720_mca_adapters_struct {
-	char* name;
-	int id;
-};
-/* rEnE : maybe there are others I don't know off... */
-
-static struct at1720_mca_adapters_struct at1720_mca_adapters[] __initdata = {
-	{ "Allied Telesys AT1720AT",	0x6410 },
-	{ "Allied Telesys AT1720BT", 	0x6413 },
-	{ "Allied Telesys AT1720T",	0x6416 },
-	{ NULL, 0 },
-};
-#endif
-
 /* Check for a network adaptor of this type, and return '0' iff one exists.
    If dev->base_addr == 0, probe all likely locations.
    If dev->base_addr == 1, always return failure.
@@ -194,11 +159,6 @@ static int irq;
 
 static void cleanup_card(struct net_device *dev)
 {
-#ifdef CONFIG_MCA_LEGACY
-	struct net_local *lp = netdev_priv(dev);
-	if (lp->mca_slot >= 0)
-		mca_mark_as_unused(lp->mca_slot);
-#endif
 	free_irq(dev->irq, NULL);
 	release_region(dev->base_addr, AT1700_IO_EXTENT);
 }
@@ -273,7 +233,7 @@ static int __init at1700_probe1(struct net_device *dev, int ioaddr)
 	static const char fmv_irqmap_pnp[8] = {3, 4, 5, 7, 9, 10, 11, 15};
 	static const char at1700_irqmap[8] = {3, 4, 5, 9, 10, 11, 14, 15};
 	unsigned int i, irq, is_fmv18x = 0, is_at1700 = 0;
-	int slot, ret = -ENODEV;
+	int ret = -ENODEV;
 	struct net_local *lp = netdev_priv(dev);
 
 	if (!request_region(ioaddr, AT1700_IO_EXTENT, DRV_NAME))
@@ -288,64 +248,6 @@ static int __init at1700_probe1(struct net_device *dev, int ioaddr)
 		   ioaddr, read_eeprom(ioaddr, 4), read_eeprom(ioaddr, 5),
 		   read_eeprom(ioaddr, 6), inw(ioaddr + EEPROM_Ctrl));
 #endif
-
-#ifdef CONFIG_MCA_LEGACY
-	/* rEnE (rene@bss.lu): got this from 3c509 driver source , adapted for AT1720 */
-
-    /* Based on Erik Nygren's (nygren@mit.edu) 3c529 patch, heavily
-	modified by Chris Beauregard (cpbeaure@csclub.uwaterloo.ca)
-	to support standard MCA probing. */
-
-	/* redone for multi-card detection by ZP Gu (zpg@castle.net) */
-	/* now works as a module */
-
-	if (MCA_bus) {
-		int j;
-		int l_i;
-		u_char pos3, pos4;
-
-		for (j = 0; at1720_mca_adapters[j].name != NULL; j ++) {
-			slot = 0;
-			while (slot != MCA_NOTFOUND) {
-
-				slot = mca_find_unused_adapter( at1720_mca_adapters[j].id, slot );
-				if (slot == MCA_NOTFOUND) break;
-
-				/* if we get this far, an adapter has been detected and is
-				enabled */
-
-				pos3 = mca_read_stored_pos( slot, 3 );
-				pos4 = mca_read_stored_pos( slot, 4 );
-
-				for (l_i = 0; l_i < 8; l_i++)
-					if (( pos3 & 0x07) == at1700_ioaddr_pattern[l_i])
-						break;
-				ioaddr = at1700_mca_probe_list[l_i];
-
-				for (irq = 0; irq < 0x10; irq++)
-					if (((((pos4>>4) & 0x0f) | (pos3 & 0xf0)) & 0xff) == at1700_irq_pattern[irq])
-						break;
-
-					/* probing for a card at a particular IO/IRQ */
-				if ((dev->irq && dev->irq != irq) ||
-				    (dev->base_addr && dev->base_addr != ioaddr)) {
-				  	slot++;		/* probing next slot */
-				  	continue;
-				}
-
-				dev->irq = irq;
-
-				/* claim the slot */
-				mca_set_adapter_name( slot, at1720_mca_adapters[j].name );
-				mca_mark_as_used(slot);
-
-				goto found;
-			}
-		}
-		/* if we get here, we didn't find an MCA adapter - try ISA */
-	}
-#endif
-	slot = -1;
 	/* We must check for the EEPROM-config boards first, else accessing
 	   IOCONFIG0 will move the board! */
 	if (at1700_probe_list[inb(ioaddr + IOCONFIG1) & 0x07] == ioaddr &&
@@ -360,11 +262,7 @@ static int __init at1700_probe1(struct net_device *dev, int ioaddr)
 		goto err_out;
 	}
 
-#ifdef CONFIG_MCA_LEGACY
-found:
-#endif
-
-		/* Reset the internal state machines. */
+	/* Reset the internal state machines. */
 	outb(0, ioaddr + RESET);
 
 	if (is_at1700) {
@@ -380,11 +278,11 @@ found:
 					break;
 			}
 			if (i == 8) {
-				goto err_mca;
+				goto err_out;
 			}
 		} else {
 			if (fmv18x_probe_list[inb(ioaddr + IOCONFIG) & 0x07] != ioaddr)
-				goto err_mca;
+				goto err_out;
 			irq = fmv_irqmap[(inb(ioaddr + IOCONFIG)>>6) & 0x03];
 		}
 	}
@@ -464,23 +362,17 @@ found:
 	spin_lock_init(&lp->lock);
 
 	lp->jumpered = is_fmv18x;
-	lp->mca_slot = slot;
 	/* Snarf the interrupt vector now. */
 	ret = request_irq(irq, net_interrupt, 0, DRV_NAME, dev);
 	if (ret) {
 		printk(KERN_ERR "AT1700 at %#3x is unusable due to a "
 		       "conflict on IRQ %d.\n",
 		       ioaddr, irq);
-		goto err_mca;
+		goto err_out;
 	}
 
 	return 0;
 
-err_mca:
-#ifdef CONFIG_MCA_LEGACY
-	if (slot >= 0)
-		mca_mark_as_unused(slot);
-#endif
 err_out:
 	release_region(ioaddr, AT1700_IO_EXTENT);
 	return ret;
diff --git a/drivers/net/ethernet/i825xx/3c523.c b/drivers/net/ethernet/i825xx/3c523.c
deleted file mode 100644
index 8451ecd4c1e..00000000000
--- a/drivers/net/ethernet/i825xx/3c523.c
+++ /dev/null
@@ -1,1312 +0,0 @@
-/*
-   net-3-driver for the 3c523 Etherlink/MC card (i82586 Ethernet chip)
-
-
-   This is an extension to the Linux operating system, and is covered by the
-   same GNU General Public License that covers that work.
-
-   Copyright 1995, 1996 by Chris Beauregard (cpbeaure@undergrad.math.uwaterloo.ca)
-
-   This is basically Michael Hipp's ni52 driver, with a new probing
-   algorithm and some minor changes to the 82586 CA and reset routines.
-   Thanks a lot Michael for a really clean i82586 implementation!  Unless
-   otherwise documented in ni52.c, any bugs are mine.
-
-   Contrary to the Ethernet-HOWTO, this isn't based on the 3c507 driver in
-   any way.  The ni52 is a lot easier to modify.
-
-   sources:
-   ni52.c
-
-   Crynwr packet driver collection was a great reference for my first
-   attempt at this sucker.  The 3c507 driver also helped, until I noticed
-   that ni52.c was a lot nicer.
-
-   EtherLink/MC: Micro Channel Ethernet Adapter Technical Reference
-   Manual, courtesy of 3Com CardFacts, documents the 3c523-specific
-   stuff.  Information on CardFacts is found in the Ethernet HOWTO.
-   Also see <a href="http://www.3com.com/">
-
-   Microprocessor Communications Support Chips, T.J. Byers, ISBN
-   0-444-01224-9, has a section on the i82586.  It tells you just enough
-   to know that you really don't want to learn how to program the chip.
-
-   The original device probe code was stolen from ps2esdi.c
-
-   Known Problems:
-   Since most of the code was stolen from ni52.c, you'll run across the
-   same bugs in the 0.62 version of ni52.c, plus maybe a few because of
-   the 3c523 idiosynchacies.  The 3c523 has 16K of RAM though, so there
-   shouldn't be the overrun problem that the 8K ni52 has.
-
-   This driver is for a 16K adapter.  It should work fine on the 64K
-   adapters, but it will only use one of the 4 banks of RAM.  Modifying
-   this for the 64K version would require a lot of heinous bank
-   switching, which I'm sure not interested in doing.  If you try to
-   implement a bank switching version, you'll basically have to remember
-   what bank is enabled and do a switch every time you access a memory
-   location that's not current.  You'll also have to remap pointers on
-   the driver side, because it only knows about 16K of the memory.
-   Anyone desperate or masochistic enough to try?
-
-   It seems to be stable now when multiple transmit buffers are used.  I
-   can't see any performance difference, but then I'm working on a 386SX.
-
-   Multicast doesn't work.  It doesn't even pretend to work.  Don't use
-   it.  Don't compile your kernel with multicast support.  I don't know
-   why.
-
-   Features:
-   This driver is useable as a loadable module.  If you try to specify an
-   IRQ or a IO address (via insmod 3c523.o irq=xx io=0xyyy), it will
-   search the MCA slots until it finds a 3c523 with the specified
-   parameters.
-
-   This driver does support multiple ethernet cards when used as a module
-   (up to MAX_3C523_CARDS, the default being 4)
-
-   This has been tested with both BNC and TP versions, internal and
-   external transceivers.  Haven't tested with the 64K version (that I
-   know of).
-
-   History:
-   Jan 1st, 1996
-   first public release
-   Feb 4th, 1996
-   update to 1.3.59, incorporated multicast diffs from ni52.c
-   Feb 15th, 1996
-   added shared irq support
-   Apr 1999
-   added support for multiple cards when used as a module
-   added option to disable multicast as is causes problems
-       Ganesh Sittampalam <ganesh.sittampalam@magdalen.oxford.ac.uk>
-       Stuart Adamson <stuart.adamson@compsoc.net>
-   Nov 2001
-   added support for ethtool (jgarzik)
-
-   $Header: /fsys2/home/chrisb/linux-1.3.59-MCA/drivers/net/RCS/3c523.c,v 1.1 1996/02/05 01:53:46 chrisb Exp chrisb $
- */
-
-#define DRV_NAME		"3c523"
-#define DRV_VERSION		"17-Nov-2001"
-
-#include <linux/init.h>
-#include <linux/netdevice.h>
-#include <linux/etherdevice.h>
-#include <linux/module.h>
-#include <linux/kernel.h>
-#include <linux/string.h>
-#include <linux/errno.h>
-#include <linux/ioport.h>
-#include <linux/skbuff.h>
-#include <linux/interrupt.h>
-#include <linux/delay.h>
-#include <linux/mca-legacy.h>
-#include <linux/ethtool.h>
-#include <linux/bitops.h>
-#include <linux/jiffies.h>
-
-#include <asm/uaccess.h>
-#include <asm/processor.h>
-#include <asm/io.h>
-
-#include "3c523.h"
-
-/*************************************************************************/
-#define DEBUG			/* debug on */
-#define SYSBUSVAL 0		/* 1 = 8 Bit, 0 = 16 bit - 3c523 only does 16 bit */
-#undef ELMC_MULTICAST		/* Disable multicast support as it is somewhat seriously broken at the moment */
-
-#define make32(ptr16) (p->memtop + (short) (ptr16) )
-#define make24(ptr32) ((char *) (ptr32) - p->base)
-#define make16(ptr32) ((unsigned short) ((unsigned long) (ptr32) - (unsigned long) p->memtop ))
-
-/*************************************************************************/
-/*
-   Tables to which we can map values in the configuration registers.
- */
-static int irq_table[] __initdata = {
-	12, 7, 3, 9
-};
-
-static int csr_table[] __initdata = {
-	0x300, 0x1300, 0x2300, 0x3300
-};
-
-static int shm_table[] __initdata = {
-	0x0c0000, 0x0c8000, 0x0d0000, 0x0d8000
-};
-
-/******************* how to calculate the buffers *****************************
-
-
-  * IMPORTANT NOTE: if you configure only one NUM_XMIT_BUFFS, the driver works
-  * --------------- in a different (more stable?) mode. Only in this mode it's
-  *                 possible to configure the driver with 'NO_NOPCOMMANDS'
-
-sizeof(scp)=12; sizeof(scb)=16; sizeof(iscp)=8;
-sizeof(scp)+sizeof(iscp)+sizeof(scb) = 36 = INIT
-sizeof(rfd) = 24; sizeof(rbd) = 12;
-sizeof(tbd) = 8; sizeof(transmit_cmd) = 16;
-sizeof(nop_cmd) = 8;
-
-  * if you don't know the driver, better do not change this values: */
-
-#define RECV_BUFF_SIZE 1524	/* slightly oversized */
-#define XMIT_BUFF_SIZE 1524	/* slightly oversized */
-#define NUM_XMIT_BUFFS 1	/* config for both, 8K and 16K shmem */
-#define NUM_RECV_BUFFS_8  4	/* config for 8K shared mem */
-#define NUM_RECV_BUFFS_16 9	/* config for 16K shared mem */
-
-#if (NUM_XMIT_BUFFS == 1)
-#define NO_NOPCOMMANDS		/* only possible with NUM_XMIT_BUFFS=1 */
-#endif
-
-/**************************************************************************/
-
-#define DELAY(x) { mdelay(32 * x); }
-
-/* a much shorter delay: */
-#define DELAY_16(); { udelay(16) ; }
-
-/* wait for command with timeout: */
-#define WAIT_4_SCB_CMD() { int i; \
-  for(i=0;i<1024;i++) { \
-    if(!p->scb->cmd) break; \
-    DELAY_16(); \
-    if(i == 1023) { \
-      pr_warning("%s:%d: scb_cmd timed out .. resetting i82586\n",\
-      	dev->name,__LINE__); \
-      elmc_id_reset586(); } } }
-
-static irqreturn_t elmc_interrupt(int irq, void *dev_id);
-static int elmc_open(struct net_device *dev);
-static int elmc_close(struct net_device *dev);
-static netdev_tx_t elmc_send_packet(struct sk_buff *, struct net_device *);
-static struct net_device_stats *elmc_get_stats(struct net_device *dev);
-static void elmc_timeout(struct net_device *dev);
-#ifdef ELMC_MULTICAST
-static void set_multicast_list(struct net_device *dev);
-#endif
-static const struct ethtool_ops netdev_ethtool_ops;
-
-/* helper-functions */
-static int init586(struct net_device *dev);
-static int check586(struct net_device *dev, unsigned long where, unsigned size);
-static void alloc586(struct net_device *dev);
-static void startrecv586(struct net_device *dev);
-static void *alloc_rfa(struct net_device *dev, void *ptr);
-static void elmc_rcv_int(struct net_device *dev);
-static void elmc_xmt_int(struct net_device *dev);
-static void elmc_rnr_int(struct net_device *dev);
-
-struct priv {
-	unsigned long base;
-	char *memtop;
-	unsigned long mapped_start;		/* Start of ioremap */
-	volatile struct rfd_struct *rfd_last, *rfd_top, *rfd_first;
-	volatile struct scp_struct *scp;	/* volatile is important */
-	volatile struct iscp_struct *iscp;	/* volatile is important */
-	volatile struct scb_struct *scb;	/* volatile is important */
-	volatile struct tbd_struct *xmit_buffs[NUM_XMIT_BUFFS];
-#if (NUM_XMIT_BUFFS == 1)
-	volatile struct transmit_cmd_struct *xmit_cmds[2];
-	volatile struct nop_cmd_struct *nop_cmds[2];
-#else
-	volatile struct transmit_cmd_struct *xmit_cmds[NUM_XMIT_BUFFS];
-	volatile struct nop_cmd_struct *nop_cmds[NUM_XMIT_BUFFS];
-#endif
-	volatile int nop_point, num_recv_buffs;
-	volatile char *xmit_cbuffs[NUM_XMIT_BUFFS];
-	volatile int xmit_count, xmit_last;
-	volatile int slot;
-};
-
-#define elmc_attn586()  {elmc_do_attn586(dev->base_addr,ELMC_CTRL_INTE);}
-#define elmc_reset586() {elmc_do_reset586(dev->base_addr,ELMC_CTRL_INTE);}
-
-/* with interrupts disabled - this will clear the interrupt bit in the
-   3c523 control register, and won't put it back.  This effectively
-   disables interrupts on the card. */
-#define elmc_id_attn586()  {elmc_do_attn586(dev->base_addr,0);}
-#define elmc_id_reset586() {elmc_do_reset586(dev->base_addr,0);}
-
-/*************************************************************************/
-/*
-   Do a Channel Attention on the 3c523.  This is extremely board dependent.
- */
-static void elmc_do_attn586(int ioaddr, int ints)
-{
-	/* the 3c523 requires a minimum of 500 ns.  The delays here might be
-	   a little too large, and hence they may cut the performance of the
-	   card slightly.  If someone who knows a little more about Linux
-	   timing would care to play with these, I'd appreciate it. */
-
-	/* this bit masking stuff is crap.  I'd rather have separate
-	   registers with strobe triggers for each of these functions.  <sigh>
-	   Ya take what ya got. */
-
-	outb(ELMC_CTRL_RST | 0x3 | ELMC_CTRL_CA | ints, ioaddr + ELMC_CTRL);
-	DELAY_16();		/* > 500 ns */
-	outb(ELMC_CTRL_RST | 0x3 | ints, ioaddr + ELMC_CTRL);
-}
-
-/*************************************************************************/
-/*
-   Reset the 82586 on the 3c523.  Also very board dependent.
- */
-static void elmc_do_reset586(int ioaddr, int ints)
-{
-	/* toggle the RST bit low then high */
-	outb(0x3 | ELMC_CTRL_LBK, ioaddr + ELMC_CTRL);
-	DELAY_16();		/* > 500 ns */
-	outb(ELMC_CTRL_RST | ELMC_CTRL_LBK | 0x3, ioaddr + ELMC_CTRL);
-
-	elmc_do_attn586(ioaddr, ints);
-}
-
-/**********************************************
- * close device
- */
-
-static int elmc_close(struct net_device *dev)
-{
-	netif_stop_queue(dev);
-	elmc_id_reset586();	/* the hard way to stop the receiver */
-	free_irq(dev->irq, dev);
-	return 0;
-}
-
-/**********************************************
- * open device
- */
-
-static int elmc_open(struct net_device *dev)
-{
-	int ret;
-
-	elmc_id_attn586();	/* disable interrupts */
-
-	ret = request_irq(dev->irq, elmc_interrupt, IRQF_SHARED,
-			  dev->name, dev);
-	if (ret) {
-		pr_err("%s: couldn't get irq %d\n", dev->name, dev->irq);
-		elmc_id_reset586();
-		return ret;
-	}
-	alloc586(dev);
-	init586(dev);
-	startrecv586(dev);
-	netif_start_queue(dev);
-	return 0;		/* most done by init */
-}
-
-/**********************************************
- * Check to see if there's an 82586 out there.
- */
-
-static int __init check586(struct net_device *dev, unsigned long where, unsigned size)
-{
-	struct priv *p = netdev_priv(dev);
-	char *iscp_addrs[2];
-	int i = 0;
-
-	p->base = (unsigned long) isa_bus_to_virt((unsigned long)where) + size - 0x01000000;
-	p->memtop = isa_bus_to_virt((unsigned long)where) + size;
-	p->scp = (struct scp_struct *)(p->base + SCP_DEFAULT_ADDRESS);
-	memset((char *) p->scp, 0, sizeof(struct scp_struct));
-	p->scp->sysbus = SYSBUSVAL;	/* 1 = 8Bit-Bus, 0 = 16 Bit */
-
-	iscp_addrs[0] = isa_bus_to_virt((unsigned long)where);
-	iscp_addrs[1] = (char *) p->scp - sizeof(struct iscp_struct);
-
-	for (i = 0; i < 2; i++) {
-		p->iscp = (struct iscp_struct *) iscp_addrs[i];
-		memset((char *) p->iscp, 0, sizeof(struct iscp_struct));
-
-		p->scp->iscp = make24(p->iscp);
-		p->iscp->busy = 1;
-
-		elmc_id_reset586();
-
-		/* reset586 does an implicit CA */
-
-		/* apparently, you sometimes have to kick the 82586 twice... */
-		elmc_id_attn586();
-		DELAY(1);
-
-		if (p->iscp->busy) {	/* i82586 clears 'busy' after successful init */
-			return 0;
-		}
-	}
-	return 1;
-}
-
-/******************************************************************
- * set iscp at the right place, called by elmc_probe and open586.
- */
-
-static void alloc586(struct net_device *dev)
-{
-	struct priv *p = netdev_priv(dev);
-
-	elmc_id_reset586();
-	DELAY(2);
-
-	p->scp = (struct scp_struct *) (p->base + SCP_DEFAULT_ADDRESS);
-	p->scb = (struct scb_struct *) isa_bus_to_virt(dev->mem_start);
-	p->iscp = (struct iscp_struct *) ((char *) p->scp - sizeof(struct iscp_struct));
-
-	memset((char *) p->iscp, 0, sizeof(struct iscp_struct));
-	memset((char *) p->scp, 0, sizeof(struct scp_struct));
-
-	p->scp->iscp = make24(p->iscp);
-	p->scp->sysbus = SYSBUSVAL;
-	p->iscp->scb_offset = make16(p->scb);
-
-	p->iscp->busy = 1;
-	elmc_id_reset586();
-	elmc_id_attn586();
-
-	DELAY(2);
-
-	if (p->iscp->busy)
-		pr_err("%s: Init-Problems (alloc).\n", dev->name);
-
-	memset((char *) p->scb, 0, sizeof(struct scb_struct));
-}
-
-/*****************************************************************/
-
-static int elmc_getinfo(char *buf, int slot, void *d)
-{
-	int len = 0;
-	struct net_device *dev = d;
-
-	if (dev == NULL)
-		return len;
-
-	len += sprintf(buf + len, "Revision: 0x%x\n",
-		       inb(dev->base_addr + ELMC_REVISION) & 0xf);
-	len += sprintf(buf + len, "IRQ: %d\n", dev->irq);
-	len += sprintf(buf + len, "IO Address: %#lx-%#lx\n", dev->base_addr,
-		       dev->base_addr + ELMC_IO_EXTENT);
-	len += sprintf(buf + len, "Memory: %#lx-%#lx\n", dev->mem_start,
-		       dev->mem_end - 1);
-	len += sprintf(buf + len, "Transceiver: %s\n", dev->if_port ?
-		       "External" : "Internal");
-	len += sprintf(buf + len, "Device: %s\n", dev->name);
-	len += sprintf(buf + len, "Hardware Address: %pM\n",
-		       dev->dev_addr);
-
-	return len;
-}				/* elmc_getinfo() */
-
-static const struct net_device_ops netdev_ops = {
-	.ndo_open 		= elmc_open,
-	.ndo_stop		= elmc_close,
-	.ndo_get_stats		= elmc_get_stats,
-	.ndo_start_xmit		= elmc_send_packet,
-	.ndo_tx_timeout		= elmc_timeout,
-#ifdef ELMC_MULTICAST
-	.ndo_set_rx_mode	= set_multicast_list,
-#endif
-	.ndo_change_mtu		= eth_change_mtu,
-	.ndo_set_mac_address 	= eth_mac_addr,
-	.ndo_validate_addr	= eth_validate_addr,
-};
-
-/*****************************************************************/
-
-static int __init do_elmc_probe(struct net_device *dev)
-{
-	static int slot;
-	int base_addr = dev->base_addr;
-	int irq = dev->irq;
-	u_char status = 0;
-	u_char revision = 0;
-	int i = 0;
-	unsigned int size = 0;
-	int retval;
-	struct priv *pr = netdev_priv(dev);
-
-	if (MCA_bus == 0) {
-		return -ENODEV;
-	}
-	/* search through the slots for the 3c523. */
-	slot = mca_find_adapter(ELMC_MCA_ID, 0);
-	while (slot != -1) {
-		status = mca_read_stored_pos(slot, 2);
-
-		dev->irq=irq_table[(status & ELMC_STATUS_IRQ_SELECT) >> 6];
-		dev->base_addr=csr_table[(status & ELMC_STATUS_CSR_SELECT) >> 1];
-
-		/*
-		   If we're trying to match a specified irq or IO address,
-		   we'll reject a match unless it's what we're looking for.
-		   Also reject it if the card is already in use.
-		 */
-
-		if ((irq && irq != dev->irq) ||
-		    (base_addr && base_addr != dev->base_addr)) {
-			slot = mca_find_adapter(ELMC_MCA_ID, slot + 1);
-			continue;
-		}
-		if (!request_region(dev->base_addr, ELMC_IO_EXTENT, DRV_NAME)) {
-			slot = mca_find_adapter(ELMC_MCA_ID, slot + 1);
-			continue;
-		}
-
-		/* found what we're looking for... */
-		break;
-	}
-
-	/* we didn't find any 3c523 in the slots we checked for */
-	if (slot == MCA_NOTFOUND)
-		return (base_addr || irq) ? -ENXIO : -ENODEV;
-
-	mca_set_adapter_name(slot, "3Com 3c523 Etherlink/MC");
-	mca_set_adapter_procfn(slot, (MCA_ProcFn) elmc_getinfo, dev);
-
-	/* if we get this far, adapter has been found - carry on */
-	pr_info("%s: 3c523 adapter found in slot %d\n", dev->name, slot + 1);
-
-	/* Now we extract configuration info from the card.
-	   The 3c523 provides information in two of the POS registers, but
-	   the second one is only needed if we want to tell the card what IRQ
-	   to use.  I suspect that whoever sets the thing up initially would
-	   prefer we don't screw with those things.
-
-	   Note that we read the status info when we found the card...
-
-	   See 3c523.h for more details.
-	 */
-
-	/* revision is stored in the first 4 bits of the revision register */
-	revision = inb(dev->base_addr + ELMC_REVISION) & 0xf;
-
-	/* according to docs, we read the interrupt and write it back to
-	   the IRQ select register, since the POST might not configure the IRQ
-	   properly. */
-	switch (dev->irq) {
-	case 3:
-		mca_write_pos(slot, 3, 0x04);
-		break;
-	case 7:
-		mca_write_pos(slot, 3, 0x02);
-		break;
-	case 9:
-		mca_write_pos(slot, 3, 0x08);
-		break;
-	case 12:
-		mca_write_pos(slot, 3, 0x01);
-		break;
-	}
-
-	pr->slot = slot;
-
-	pr_info("%s: 3Com 3c523 Rev 0x%x at %#lx\n", dev->name, (int) revision,
-	       dev->base_addr);
-
-	/* Determine if we're using the on-board transceiver (i.e. coax) or
-	   an external one.  The information is pretty much useless, but I
-	   guess it's worth brownie points. */
-	dev->if_port = (status & ELMC_STATUS_DISABLE_THIN);
-
-	/* The 3c523 has a 24K chunk of memory.  The first 16K is the
-	   shared memory, while the last 8K is for the EtherStart BIOS ROM.
-	   Which we don't care much about here.  We'll just tell Linux that
-	   we're using 16K.  MCA won't permit address space conflicts caused
-	   by not mapping the other 8K. */
-	dev->mem_start = shm_table[(status & ELMC_STATUS_MEMORY_SELECT) >> 3];
-
-	/* We're using MCA, so it's a given that the information about memory
-	   size is correct.  The Crynwr drivers do something like this. */
-
-	elmc_id_reset586();	/* seems like a good idea before checking it... */
-
-	size = 0x4000;		/* check for 16K mem */
-	if (!check586(dev, dev->mem_start, size)) {
-		pr_err("%s: memprobe, Can't find memory at 0x%lx!\n", dev->name,
-		       dev->mem_start);
-		retval = -ENODEV;
-		goto err_out;
-	}
-	dev->mem_end = dev->mem_start + size;	/* set mem_end showed by 'ifconfig' */
-
-	pr->memtop = isa_bus_to_virt(dev->mem_start) + size;
-	pr->base = (unsigned long) isa_bus_to_virt(dev->mem_start) + size - 0x01000000;
-	alloc586(dev);
-
-	elmc_id_reset586();	/* make sure it doesn't generate spurious ints */
-
-	/* set number of receive-buffs according to memsize */
-	pr->num_recv_buffs = NUM_RECV_BUFFS_16;
-
-	/* dump all the assorted information */
-	pr_info("%s: IRQ %d, %sternal xcvr, memory %#lx-%#lx.\n", dev->name,
-	       dev->irq, dev->if_port ? "ex" : "in",
-	       dev->mem_start, dev->mem_end - 1);
-
-	/* The hardware address for the 3c523 is stored in the first six
-	   bytes of the IO address. */
-	for (i = 0; i < 6; i++)
-		dev->dev_addr[i] = inb(dev->base_addr + i);
-
-	pr_info("%s: hardware address %pM\n",
-	       dev->name, dev->dev_addr);
-
-	dev->netdev_ops = &netdev_ops;
-	dev->watchdog_timeo = HZ;
-	dev->ethtool_ops = &netdev_ethtool_ops;
-
-	/* note that we haven't actually requested the IRQ from the kernel.
-	   That gets done in elmc_open().  I'm not sure that's such a good idea,
-	   but it works, so I'll go with it. */
-
-#ifndef ELMC_MULTICAST
-        dev->flags&=~IFF_MULTICAST;     /* Multicast doesn't work */
-#endif
-
-	retval = register_netdev(dev);
-	if (retval)
-		goto err_out;
-
-	return 0;
-err_out:
-	mca_set_adapter_procfn(slot, NULL, NULL);
-	release_region(dev->base_addr, ELMC_IO_EXTENT);
-	return retval;
-}
-
-#ifdef MODULE
-static void cleanup_card(struct net_device *dev)
-{
-	mca_set_adapter_procfn(((struct priv *)netdev_priv(dev))->slot,
-				NULL, NULL);
-	release_region(dev->base_addr, ELMC_IO_EXTENT);
-}
-#else
-struct net_device * __init elmc_probe(int unit)
-{
-	struct net_device *dev = alloc_etherdev(sizeof(struct priv));
-	int err;
-
-	if (!dev)
-		return ERR_PTR(-ENOMEM);
-
-	sprintf(dev->name, "eth%d", unit);
-	netdev_boot_setup_check(dev);
-
-	err = do_elmc_probe(dev);
-	if (err)
-		goto out;
-	return dev;
-out:
-	free_netdev(dev);
-	return ERR_PTR(err);
-}
-#endif
-
-/**********************************************
- * init the chip (elmc-interrupt should be disabled?!)
- * needs a correct 'allocated' memory
- */
-
-static int init586(struct net_device *dev)
-{
-	void *ptr;
-	unsigned long s;
-	int i, result = 0;
-	struct priv *p = netdev_priv(dev);
-	volatile struct configure_cmd_struct *cfg_cmd;
-	volatile struct iasetup_cmd_struct *ias_cmd;
-	volatile struct tdr_cmd_struct *tdr_cmd;
-	volatile struct mcsetup_cmd_struct *mc_cmd;
-	struct netdev_hw_addr *ha;
-	int num_addrs = netdev_mc_count(dev);
-
-	ptr = (void *) ((char *) p->scb + sizeof(struct scb_struct));
-
-	cfg_cmd = (struct configure_cmd_struct *) ptr;	/* configure-command */
-	cfg_cmd->cmd_status = 0;
-	cfg_cmd->cmd_cmd = CMD_CONFIGURE | CMD_LAST;
-	cfg_cmd->cmd_link = 0xffff;
-
-	cfg_cmd->byte_cnt = 0x0a;	/* number of cfg bytes */
-	cfg_cmd->fifo = 0x08;	/* fifo-limit (8=tx:32/rx:64) */
-	cfg_cmd->sav_bf = 0x40;	/* hold or discard bad recv frames (bit 7) */
-	cfg_cmd->adr_len = 0x2e;	/* addr_len |!src_insert |pre-len |loopback */
-	cfg_cmd->priority = 0x00;
-	cfg_cmd->ifs = 0x60;
-	cfg_cmd->time_low = 0x00;
-	cfg_cmd->time_high = 0xf2;
-	cfg_cmd->promisc = 0;
-	if (dev->flags & (IFF_ALLMULTI | IFF_PROMISC))
-		cfg_cmd->promisc = 1;
-	cfg_cmd->carr_coll = 0x00;
-
-	p->scb->cbl_offset = make16(cfg_cmd);
-
-	p->scb->cmd = CUC_START;	/* cmd.-unit start */
-	elmc_id_attn586();
-
-	s = jiffies;		/* warning: only active with interrupts on !! */
-	while (!(cfg_cmd->cmd_status & STAT_COMPL)) {
-		if (time_after(jiffies, s + 30*HZ/100))
-			break;
-	}
-
-	if ((cfg_cmd->cmd_status & (STAT_OK | STAT_COMPL)) != (STAT_COMPL | STAT_OK)) {
-		pr_warning("%s (elmc): configure command failed: %x\n", dev->name, cfg_cmd->cmd_status);
-		return 1;
-	}
-	/*
-	 * individual address setup
-	 */
-	ias_cmd = (struct iasetup_cmd_struct *) ptr;
-
-	ias_cmd->cmd_status = 0;
-	ias_cmd->cmd_cmd = CMD_IASETUP | CMD_LAST;
-	ias_cmd->cmd_link = 0xffff;
-
-	memcpy((char *) &ias_cmd->iaddr, (char *) dev->dev_addr, ETH_ALEN);
-
-	p->scb->cbl_offset = make16(ias_cmd);
-
-	p->scb->cmd = CUC_START;	/* cmd.-unit start */
-	elmc_id_attn586();
-
-	s = jiffies;
-	while (!(ias_cmd->cmd_status & STAT_COMPL)) {
-		if (time_after(jiffies, s + 30*HZ/100))
-			break;
-	}
-
-	if ((ias_cmd->cmd_status & (STAT_OK | STAT_COMPL)) != (STAT_OK | STAT_COMPL)) {
-		pr_warning("%s (elmc): individual address setup command failed: %04x\n",
-			dev->name, ias_cmd->cmd_status);
-		return 1;
-	}
-	/*
-	 * TDR, wire check .. e.g. no resistor e.t.c
-	 */
-	tdr_cmd = (struct tdr_cmd_struct *) ptr;
-
-	tdr_cmd->cmd_status = 0;
-	tdr_cmd->cmd_cmd = CMD_TDR | CMD_LAST;
-	tdr_cmd->cmd_link = 0xffff;
-	tdr_cmd->status = 0;
-
-	p->scb->cbl_offset = make16(tdr_cmd);
-
-	p->scb->cmd = CUC_START;	/* cmd.-unit start */
-	elmc_attn586();
-
-	s = jiffies;
-	while (!(tdr_cmd->cmd_status & STAT_COMPL)) {
-		if (time_after(jiffies, s + 30*HZ/100)) {
-			pr_warning("%s: %d Problems while running the TDR.\n", dev->name, __LINE__);
-			result = 1;
-			break;
-		}
-	}
-
-	if (!result) {
-		DELAY(2);	/* wait for result */
-		result = tdr_cmd->status;
-
-		p->scb->cmd = p->scb->status & STAT_MASK;
-		elmc_id_attn586();	/* ack the interrupts */
-
-		if (result & TDR_LNK_OK) {
-			/* empty */
-		} else if (result & TDR_XCVR_PRB) {
-			pr_warning("%s: TDR: Transceiver problem!\n", dev->name);
-		} else if (result & TDR_ET_OPN) {
-			pr_warning("%s: TDR: No correct termination %d clocks away.\n", dev->name, result & TDR_TIMEMASK);
-		} else if (result & TDR_ET_SRT) {
-			if (result & TDR_TIMEMASK)	/* time == 0 -> strange :-) */
-				pr_warning("%s: TDR: Detected a short circuit %d clocks away.\n", dev->name, result & TDR_TIMEMASK);
-		} else {
-			pr_warning("%s: TDR: Unknown status %04x\n", dev->name, result);
-		}
-	}
-	/*
-	 * ack interrupts
-	 */
-	p->scb->cmd = p->scb->status & STAT_MASK;
-	elmc_id_attn586();
-
-	/*
-	 * alloc nop/xmit-cmds
-	 */
-#if (NUM_XMIT_BUFFS == 1)
-	for (i = 0; i < 2; i++) {
-		p->nop_cmds[i] = (struct nop_cmd_struct *) ptr;
-		p->nop_cmds[i]->cmd_cmd = CMD_NOP;
-		p->nop_cmds[i]->cmd_status = 0;
-		p->nop_cmds[i]->cmd_link = make16((p->nop_cmds[i]));
-		ptr = (char *) ptr + sizeof(struct nop_cmd_struct);
-	}
-	p->xmit_cmds[0] = (struct transmit_cmd_struct *) ptr;	/* transmit cmd/buff 0 */
-	ptr = (char *) ptr + sizeof(struct transmit_cmd_struct);
-#else
-	for (i = 0; i < NUM_XMIT_BUFFS; i++) {
-		p->nop_cmds[i] = (struct nop_cmd_struct *) ptr;
-		p->nop_cmds[i]->cmd_cmd = CMD_NOP;
-		p->nop_cmds[i]->cmd_status = 0;
-		p->nop_cmds[i]->cmd_link = make16((p->nop_cmds[i]));
-		ptr = (char *) ptr + sizeof(struct nop_cmd_struct);
-		p->xmit_cmds[i] = (struct transmit_cmd_struct *) ptr;	/*transmit cmd/buff 0 */
-		ptr = (char *) ptr + sizeof(struct transmit_cmd_struct);
-	}
-#endif
-
-	ptr = alloc_rfa(dev, (void *) ptr);	/* init receive-frame-area */
-
-	/*
-	 * Multicast setup
-	 */
-
-	if (num_addrs) {
-		/* I don't understand this: do we really need memory after the init? */
-		int len = ((char *) p->iscp - (char *) ptr - 8) / 6;
-		if (len <= 0) {
-			pr_err("%s: Ooooops, no memory for MC-Setup!\n", dev->name);
-		} else {
-			if (len < num_addrs) {
-				num_addrs = len;
-				pr_warning("%s: Sorry, can only apply %d MC-Address(es).\n",
-				       dev->name, num_addrs);
-			}
-			mc_cmd = (struct mcsetup_cmd_struct *) ptr;
-			mc_cmd->cmd_status = 0;
-			mc_cmd->cmd_cmd = CMD_MCSETUP | CMD_LAST;
-			mc_cmd->cmd_link = 0xffff;
-			mc_cmd->mc_cnt = num_addrs * 6;
-			i = 0;
-			netdev_for_each_mc_addr(ha, dev)
-				memcpy((char *) mc_cmd->mc_list[i++],
-				       ha->addr, 6);
-			p->scb->cbl_offset = make16(mc_cmd);
-			p->scb->cmd = CUC_START;
-			elmc_id_attn586();
-			s = jiffies;
-			while (!(mc_cmd->cmd_status & STAT_COMPL)) {
-				if (time_after(jiffies, s + 30*HZ/100))
-					break;
-			}
-			if (!(mc_cmd->cmd_status & STAT_COMPL)) {
-				pr_warning("%s: Can't apply multicast-address-list.\n", dev->name);
-			}
-		}
-	}
-	/*
-	 * alloc xmit-buffs / init xmit_cmds
-	 */
-	for (i = 0; i < NUM_XMIT_BUFFS; i++) {
-		p->xmit_cbuffs[i] = (char *) ptr;	/* char-buffs */
-		ptr = (char *) ptr + XMIT_BUFF_SIZE;
-		p->xmit_buffs[i] = (struct tbd_struct *) ptr;	/* TBD */
-		ptr = (char *) ptr + sizeof(struct tbd_struct);
-		if ((void *) ptr > (void *) p->iscp) {
-			pr_err("%s: not enough shared-mem for your configuration!\n", dev->name);
-			return 1;
-		}
-		memset((char *) (p->xmit_cmds[i]), 0, sizeof(struct transmit_cmd_struct));
-		memset((char *) (p->xmit_buffs[i]), 0, sizeof(struct tbd_struct));
-		p->xmit_cmds[i]->cmd_status = STAT_COMPL;
-		p->xmit_cmds[i]->cmd_cmd = CMD_XMIT | CMD_INT;
-		p->xmit_cmds[i]->tbd_offset = make16((p->xmit_buffs[i]));
-		p->xmit_buffs[i]->next = 0xffff;
-		p->xmit_buffs[i]->buffer = make24((p->xmit_cbuffs[i]));
-	}
-
-	p->xmit_count = 0;
-	p->xmit_last = 0;
-#ifndef NO_NOPCOMMANDS
-	p->nop_point = 0;
-#endif
-
-	/*
-	 * 'start transmitter' (nop-loop)
-	 */
-#ifndef NO_NOPCOMMANDS
-	p->scb->cbl_offset = make16(p->nop_cmds[0]);
-	p->scb->cmd = CUC_START;
-	elmc_id_attn586();
-	WAIT_4_SCB_CMD();
-#else
-	p->xmit_cmds[0]->cmd_link = 0xffff;
-	p->xmit_cmds[0]->cmd_cmd = CMD_XMIT | CMD_LAST | CMD_INT;
-#endif
-
-	return 0;
-}
-
-/******************************************************
- * This is a helper routine for elmc_rnr_int() and init586().
- * It sets up the Receive Frame Area (RFA).
- */
-
-static void *alloc_rfa(struct net_device *dev, void *ptr)
-{
-	volatile struct rfd_struct *rfd = (struct rfd_struct *) ptr;
-	volatile struct rbd_struct *rbd;
-	int i;
-	struct priv *p = netdev_priv(dev);
-
-	memset((char *) rfd, 0, sizeof(struct rfd_struct) * p->num_recv_buffs);
-	p->rfd_first = rfd;
-
-	for (i = 0; i < p->num_recv_buffs; i++) {
-		rfd[i].next = make16(rfd + (i + 1) % p->num_recv_buffs);
-	}
-	rfd[p->num_recv_buffs - 1].last = RFD_SUSP;	/* RU suspend */
-
-	ptr = (void *) (rfd + p->num_recv_buffs);
-
-	rbd = (struct rbd_struct *) ptr;
-	ptr = (void *) (rbd + p->num_recv_buffs);
-
-	/* clr descriptors */
-	memset((char *) rbd, 0, sizeof(struct rbd_struct) * p->num_recv_buffs);
-
-	for (i = 0; i < p->num_recv_buffs; i++) {
-		rbd[i].next = make16((rbd + (i + 1) % p->num_recv_buffs));
-		rbd[i].size = RECV_BUFF_SIZE;
-		rbd[i].buffer = make24(ptr);
-		ptr = (char *) ptr + RECV_BUFF_SIZE;
-	}
-
-	p->rfd_top = p->rfd_first;
-	p->rfd_last = p->rfd_first + p->num_recv_buffs - 1;
-
-	p->scb->rfa_offset = make16(p->rfd_first);
-	p->rfd_first->rbd_offset = make16(rbd);
-
-	return ptr;
-}
-
-
-/**************************************************
- * Interrupt Handler ...
- */
-
-static irqreturn_t
-elmc_interrupt(int irq, void *dev_id)
-{
-	struct net_device *dev = dev_id;
-	unsigned short stat;
-	struct priv *p;
-
-	if (!netif_running(dev)) {
-		/* The 3c523 has this habit of generating interrupts during the
-		   reset.  I'm not sure if the ni52 has this same problem, but it's
-		   really annoying if we haven't finished initializing it.  I was
-		   hoping all the elmc_id_* commands would disable this, but I
-		   might have missed a few. */
-
-		elmc_id_attn586();	/* ack inter. and disable any more */
-		return IRQ_HANDLED;
-	} else if (!(ELMC_CTRL_INT & inb(dev->base_addr + ELMC_CTRL))) {
-		/* wasn't this device */
-		return IRQ_NONE;
-	}
-	/* reading ELMC_CTRL also clears the INT bit. */
-
-	p = netdev_priv(dev);
-
-	while ((stat = p->scb->status & STAT_MASK))
-	{
-		p->scb->cmd = stat;
-		elmc_attn586();	/* ack inter. */
-
-		if (stat & STAT_CX) {
-			/* command with I-bit set complete */
-			elmc_xmt_int(dev);
-		}
-		if (stat & STAT_FR) {
-			/* received a frame */
-			elmc_rcv_int(dev);
-		}
-#ifndef NO_NOPCOMMANDS
-		if (stat & STAT_CNA) {
-			/* CU went 'not ready' */
-			if (netif_running(dev)) {
-				pr_warning("%s: oops! CU has left active state. stat: %04x/%04x.\n",
-					dev->name, (int) stat, (int) p->scb->status);
-			}
-		}
-#endif
-
-		if (stat & STAT_RNR) {
-			/* RU went 'not ready' */
-
-			if (p->scb->status & RU_SUSPEND) {
-				/* special case: RU_SUSPEND */
-
-				WAIT_4_SCB_CMD();
-				p->scb->cmd = RUC_RESUME;
-				elmc_attn586();
-			} else {
-				pr_warning("%s: Receiver-Unit went 'NOT READY': %04x/%04x.\n",
-					dev->name, (int) stat, (int) p->scb->status);
-				elmc_rnr_int(dev);
-			}
-		}
-		WAIT_4_SCB_CMD();	/* wait for ack. (elmc_xmt_int can be faster than ack!!) */
-		if (p->scb->cmd) {	/* timed out? */
-			break;
-		}
-	}
-	return IRQ_HANDLED;
-}
-
-/*******************************************************
- * receive-interrupt
- */
-
-static void elmc_rcv_int(struct net_device *dev)
-{
-	int status;
-	unsigned short totlen;
-	struct sk_buff *skb;
-	struct rbd_struct *rbd;
-	struct priv *p = netdev_priv(dev);
-
-	for (; (status = p->rfd_top->status) & STAT_COMPL;) {
-		rbd = (struct rbd_struct *) make32(p->rfd_top->rbd_offset);
-
-		if (status & STAT_OK) {		/* frame received without error? */
-			if ((totlen = rbd->status) & RBD_LAST) {	/* the first and the last buffer? */
-				totlen &= RBD_MASK;	/* length of this frame */
-				rbd->status = 0;
-				skb = netdev_alloc_skb(dev, totlen + 2);
-				if (skb != NULL) {
-					skb_reserve(skb, 2);	/* 16 byte alignment */
-					skb_put(skb,totlen);
-					skb_copy_to_linear_data(skb, (char *) p->base+(unsigned long) rbd->buffer,totlen);
-					skb->protocol = eth_type_trans(skb, dev);
-					netif_rx(skb);
-					dev->stats.rx_packets++;
-					dev->stats.rx_bytes += totlen;
-				} else {
-					dev->stats.rx_dropped++;
-				}
-			} else {
-				pr_warning("%s: received oversized frame.\n", dev->name);
-				dev->stats.rx_dropped++;
-			}
-		} else {	/* frame !(ok), only with 'save-bad-frames' */
-			pr_warning("%s: oops! rfd-error-status: %04x\n", dev->name, status);
-			dev->stats.rx_errors++;
-		}
-		p->rfd_top->status = 0;
-		p->rfd_top->last = RFD_SUSP;
-		p->rfd_last->last = 0;	/* delete RU_SUSP  */
-		p->rfd_last = p->rfd_top;
-		p->rfd_top = (struct rfd_struct *) make32(p->rfd_top->next);	/* step to next RFD */
-	}
-}
-
-/**********************************************************
- * handle 'Receiver went not ready'.
- */
-
-static void elmc_rnr_int(struct net_device *dev)
-{
-	struct priv *p = netdev_priv(dev);
-
-	dev->stats.rx_errors++;
-
-	WAIT_4_SCB_CMD();	/* wait for the last cmd */
-	p->scb->cmd = RUC_ABORT;	/* usually the RU is in the 'no resource'-state .. abort it now. */
-	elmc_attn586();
-	WAIT_4_SCB_CMD();	/* wait for accept cmd. */
-
-	alloc_rfa(dev, (char *) p->rfd_first);
-	startrecv586(dev);	/* restart RU */
-
-	pr_warning("%s: Receive-Unit restarted. Status: %04x\n", dev->name, p->scb->status);
-
-}
-
-/**********************************************************
- * handle xmit - interrupt
- */
-
-static void elmc_xmt_int(struct net_device *dev)
-{
-	int status;
-	struct priv *p = netdev_priv(dev);
-
-	status = p->xmit_cmds[p->xmit_last]->cmd_status;
-	if (!(status & STAT_COMPL)) {
-		pr_warning("%s: strange .. xmit-int without a 'COMPLETE'\n", dev->name);
-	}
-	if (status & STAT_OK) {
-		dev->stats.tx_packets++;
-		dev->stats.collisions += (status & TCMD_MAXCOLLMASK);
-	} else {
-		dev->stats.tx_errors++;
-		if (status & TCMD_LATECOLL) {
-			pr_warning("%s: late collision detected.\n", dev->name);
-			dev->stats.collisions++;
-		} else if (status & TCMD_NOCARRIER) {
-			dev->stats.tx_carrier_errors++;
-			pr_warning("%s: no carrier detected.\n", dev->name);
-		} else if (status & TCMD_LOSTCTS) {
-			pr_warning("%s: loss of CTS detected.\n", dev->name);
-		} else if (status & TCMD_UNDERRUN) {
-			dev->stats.tx_fifo_errors++;
-			pr_warning("%s: DMA underrun detected.\n", dev->name);
-		} else if (status & TCMD_MAXCOLL) {
-			pr_warning("%s: Max. collisions exceeded.\n", dev->name);
-			dev->stats.collisions += 16;
-		}
-	}
-
-#if (NUM_XMIT_BUFFS != 1)
-	if ((++p->xmit_last) == NUM_XMIT_BUFFS) {
-		p->xmit_last = 0;
-	}
-#endif
-
-	netif_wake_queue(dev);
-}
-
-/***********************************************************
- * (re)start the receiver
- */
-
-static void startrecv586(struct net_device *dev)
-{
-	struct priv *p = netdev_priv(dev);
-
-	p->scb->rfa_offset = make16(p->rfd_first);
-	p->scb->cmd = RUC_START;
-	elmc_attn586();		/* start cmd. */
-	WAIT_4_SCB_CMD();	/* wait for accept cmd. (no timeout!!) */
-}
-
-/******************************************************
- * timeout
- */
-
-static void elmc_timeout(struct net_device *dev)
-{
-	struct priv *p = netdev_priv(dev);
-	/* COMMAND-UNIT active? */
-	if (p->scb->status & CU_ACTIVE) {
-		pr_debug("%s: strange ... timeout with CU active?!?\n", dev->name);
-		pr_debug("%s: X0: %04x N0: %04x N1: %04x %d\n", dev->name,
-			(int)p->xmit_cmds[0]->cmd_status,
-			(int)p->nop_cmds[0]->cmd_status,
-			(int)p->nop_cmds[1]->cmd_status, (int)p->nop_point);
-		p->scb->cmd = CUC_ABORT;
-		elmc_attn586();
-		WAIT_4_SCB_CMD();
-		p->scb->cbl_offset = make16(p->nop_cmds[p->nop_point]);
-		p->scb->cmd = CUC_START;
-		elmc_attn586();
-		WAIT_4_SCB_CMD();
-		netif_wake_queue(dev);
-	} else {
-		pr_debug("%s: xmitter timed out, try to restart! stat: %04x\n",
-			dev->name, p->scb->status);
-		pr_debug("%s: command-stats: %04x %04x\n", dev->name,
-			p->xmit_cmds[0]->cmd_status, p->xmit_cmds[1]->cmd_status);
-		elmc_close(dev);
-		elmc_open(dev);
-	}
-}
-
-/******************************************************
- * send frame
- */
-
-static netdev_tx_t elmc_send_packet(struct sk_buff *skb, struct net_device *dev)
-{
-	int len;
-	int i;
-#ifndef NO_NOPCOMMANDS
-	int next_nop;
-#endif
-	struct priv *p = netdev_priv(dev);
-
-	netif_stop_queue(dev);
-
-	len = (ETH_ZLEN < skb->len) ? skb->len : ETH_ZLEN;
-
-	if (len != skb->len)
-		memset((char *) p->xmit_cbuffs[p->xmit_count], 0, ETH_ZLEN);
-	skb_copy_from_linear_data(skb, (char *) p->xmit_cbuffs[p->xmit_count], skb->len);
-
-#if (NUM_XMIT_BUFFS == 1)
-#ifdef NO_NOPCOMMANDS
-	p->xmit_buffs[0]->size = TBD_LAST | len;
-	for (i = 0; i < 16; i++) {
-		p->scb->cbl_offset = make16(p->xmit_cmds[0]);
-		p->scb->cmd = CUC_START;
-		p->xmit_cmds[0]->cmd_status = 0;
-			elmc_attn586();
-		if (!i) {
-			dev_kfree_skb(skb);
-		}
-		WAIT_4_SCB_CMD();
-		if ((p->scb->status & CU_ACTIVE)) {	/* test it, because CU sometimes doesn't start immediately */
-			break;
-		}
-		if (p->xmit_cmds[0]->cmd_status) {
-			break;
-		}
-		if (i == 15) {
-			pr_warning("%s: Can't start transmit-command.\n", dev->name);
-		}
-	}
-#else
-	next_nop = (p->nop_point + 1) & 0x1;
-	p->xmit_buffs[0]->size = TBD_LAST | len;
-
-	p->xmit_cmds[0]->cmd_link = p->nop_cmds[next_nop]->cmd_link
-	    = make16((p->nop_cmds[next_nop]));
-	p->xmit_cmds[0]->cmd_status = p->nop_cmds[next_nop]->cmd_status = 0;
-
-	p->nop_cmds[p->nop_point]->cmd_link = make16((p->xmit_cmds[0]));
-	p->nop_point = next_nop;
-	dev_kfree_skb(skb);
-#endif
-#else
-	p->xmit_buffs[p->xmit_count]->size = TBD_LAST | len;
-	if ((next_nop = p->xmit_count + 1) == NUM_XMIT_BUFFS) {
-		next_nop = 0;
-	}
-	p->xmit_cmds[p->xmit_count]->cmd_status = 0;
-	p->xmit_cmds[p->xmit_count]->cmd_link = p->nop_cmds[next_nop]->cmd_link
-	    = make16((p->nop_cmds[next_nop]));
-	p->nop_cmds[next_nop]->cmd_status = 0;
-		p->nop_cmds[p->xmit_count]->cmd_link = make16((p->xmit_cmds[p->xmit_count]));
-	p->xmit_count = next_nop;
-	if (p->xmit_count != p->xmit_last)
-		netif_wake_queue(dev);
-	dev_kfree_skb(skb);
-#endif
-	return NETDEV_TX_OK;
-}
-
-/*******************************************
- * Someone wanna have the statistics
- */
-
-static struct net_device_stats *elmc_get_stats(struct net_device *dev)
-{
-	struct priv *p = netdev_priv(dev);
-	unsigned short crc, aln, rsc, ovrn;
-
-	crc = p->scb->crc_errs;	/* get error-statistic from the ni82586 */
-	p->scb->crc_errs -= crc;
-	aln = p->scb->aln_errs;
-	p->scb->aln_errs -= aln;
-	rsc = p->scb->rsc_errs;
-	p->scb->rsc_errs -= rsc;
-	ovrn = p->scb->ovrn_errs;
-	p->scb->ovrn_errs -= ovrn;
-
-	dev->stats.rx_crc_errors += crc;
-	dev->stats.rx_fifo_errors += ovrn;
-	dev->stats.rx_frame_errors += aln;
-	dev->stats.rx_dropped += rsc;
-
-	return &dev->stats;
-}
-
-/********************************************************
- * Set MC list ..
- */
-
-#ifdef ELMC_MULTICAST
-static void set_multicast_list(struct net_device *dev)
-{
-	if (!dev->start) {
-		/* without a running interface, promiscuous doesn't work */
-		return;
-	}
-	dev->start = 0;
-	alloc586(dev);
-	init586(dev);
-	startrecv586(dev);
-	dev->start = 1;
-}
-#endif
-
-static void netdev_get_drvinfo(struct net_device *dev,
-			       struct ethtool_drvinfo *info)
-{
-	strcpy(info->driver, DRV_NAME);
-	strcpy(info->version, DRV_VERSION);
-	sprintf(info->bus_info, "MCA 0x%lx", dev->base_addr);
-}
-
-static const struct ethtool_ops netdev_ethtool_ops = {
-	.get_drvinfo		= netdev_get_drvinfo,
-};
-
-#ifdef MODULE
-
-/* Increase if needed ;) */
-#define MAX_3C523_CARDS 4
-
-static struct net_device *dev_elmc[MAX_3C523_CARDS];
-static int irq[MAX_3C523_CARDS];
-static int io[MAX_3C523_CARDS];
-module_param_array(irq, int, NULL, 0);
-module_param_array(io, int, NULL, 0);
-MODULE_PARM_DESC(io, "EtherLink/MC I/O base address(es)");
-MODULE_PARM_DESC(irq, "EtherLink/MC IRQ number(s)");
-MODULE_LICENSE("GPL");
-
-int __init init_module(void)
-{
-	int this_dev,found = 0;
-
-	/* Loop until we either can't find any more cards, or we have MAX_3C523_CARDS */
-	for(this_dev=0; this_dev<MAX_3C523_CARDS; this_dev++) {
-		struct net_device *dev = alloc_etherdev(sizeof(struct priv));
-		if (!dev)
-			break;
-		dev->irq=irq[this_dev];
-		dev->base_addr=io[this_dev];
-		if (do_elmc_probe(dev) == 0) {
-			dev_elmc[this_dev] = dev;
-			found++;
-			continue;
-		}
-		free_netdev(dev);
-		if (io[this_dev]==0)
-			break;
-		pr_warning("3c523.c: No 3c523 card found at io=%#x\n",io[this_dev]);
-	}
-
-	if(found==0) {
-		if (io[0]==0)
-			pr_notice("3c523.c: No 3c523 cards found\n");
-		return -ENXIO;
-	} else return 0;
-}
-
-void __exit cleanup_module(void)
-{
-	int this_dev;
-	for (this_dev=0; this_dev<MAX_3C523_CARDS; this_dev++) {
-		struct net_device *dev = dev_elmc[this_dev];
-		if (dev) {
-			unregister_netdev(dev);
-			cleanup_card(dev);
-			free_netdev(dev);
-		}
-	}
-}
-
-#endif				/* MODULE */
diff --git a/drivers/net/ethernet/i825xx/3c523.h b/drivers/net/ethernet/i825xx/3c523.h
deleted file mode 100644
index 6956441687b..00000000000
--- a/drivers/net/ethernet/i825xx/3c523.h
+++ /dev/null
@@ -1,355 +0,0 @@
-#ifndef _3c523_INCLUDE_
-#define _3c523_INCLUDE_
-/*
-	This is basically a hacked version of ni52.h, for the 3c523
-	Etherlink/MC.
-*/
-
-/*
- * Intel i82586 Ethernet definitions
- *
- * This is an extension to the Linux operating system, and is covered by the
- * same GNU General Public License that covers that work.
- *
- * Copyright 1995 by Chris Beauregard (cpbeaure@undergrad.math.uwaterloo.ca)
- *
- * See 3c523.c for details.
- *
- * $Header: /home/chrisb/linux-1.2.13-3c523/drivers/net/RCS/3c523.h,v 1.6 1996/01/20 05:09:00 chrisb Exp chrisb $
- */
-
-/*
- * where to find the System Configuration Pointer (SCP)
- */
-#define SCP_DEFAULT_ADDRESS 0xfffff4
-
-
-/*
- * System Configuration Pointer Struct
- */
-
-struct scp_struct
-{
-  unsigned short zero_dum0;	/* has to be zero */
-  unsigned char  sysbus;	/* 0=16Bit,1=8Bit */
-  unsigned char  zero_dum1;	/* has to be zero for 586 */
-  unsigned short zero_dum2;
-  unsigned short zero_dum3;
-  char          *iscp;		/* pointer to the iscp-block */
-};
-
-
-/*
- * Intermediate System Configuration Pointer (ISCP)
- */
-struct iscp_struct
-{
-  unsigned char  busy;          /* 586 clears after successful init */
-  unsigned char  zero_dummy;    /* hast to be zero */
-  unsigned short scb_offset;    /* pointeroffset to the scb_base */
-  char          *scb_base;      /* base-address of all 16-bit offsets */
-};
-
-/*
- * System Control Block (SCB)
- */
-struct scb_struct
-{
-  unsigned short status;        /* status word */
-  unsigned short cmd;           /* command word */
-  unsigned short cbl_offset;    /* pointeroffset, command block list */
-  unsigned short rfa_offset;    /* pointeroffset, receive frame area */
-  unsigned short crc_errs;      /* CRC-Error counter */
-  unsigned short aln_errs;      /* alignmenterror counter */
-  unsigned short rsc_errs;      /* Resourceerror counter */
-  unsigned short ovrn_errs;     /* OVerrunerror counter */
-};
-
-/*
- * possible command values for the command word
- */
-#define RUC_MASK	0x0070	/* mask for RU commands */
-#define RUC_NOP		0x0000	/* NOP-command */
-#define RUC_START	0x0010	/* start RU */
-#define RUC_RESUME	0x0020	/* resume RU after suspend */
-#define RUC_SUSPEND	0x0030	/* suspend RU */
-#define RUC_ABORT	0x0040	/* abort receiver operation immediately */
-
-#define CUC_MASK	0x0700	/* mask for CU command */
-#define CUC_NOP		0x0000	/* NOP-command */
-#define CUC_START	0x0100	/* start execution of 1. cmd on the CBL */
-#define CUC_RESUME	0x0200	/* resume after suspend */
-#define CUC_SUSPEND	0x0300	/* Suspend CU */
-#define CUC_ABORT	0x0400	/* abort command operation immediately */
-
-#define ACK_MASK	0xf000	/* mask for ACK command */
-#define ACK_CX		0x8000	/* acknowledges STAT_CX */
-#define ACK_FR		0x4000	/* ack. STAT_FR */
-#define ACK_CNA		0x2000	/* ack. STAT_CNA */
-#define ACK_RNR		0x1000	/* ack. STAT_RNR */
-
-/*
- * possible status values for the status word
- */
-#define STAT_MASK	0xf000	/* mask for cause of interrupt */
-#define STAT_CX		0x8000	/* CU finished cmd with its I bit set */
-#define STAT_FR		0x4000	/* RU finished receiving a frame */
-#define STAT_CNA	0x2000	/* CU left active state */
-#define STAT_RNR	0x1000	/* RU left ready state */
-
-#define CU_STATUS	0x700	/* CU status, 0=idle */
-#define CU_SUSPEND	0x100	/* CU is suspended */
-#define CU_ACTIVE	0x200	/* CU is active */
-
-#define RU_STATUS	0x70	/* RU status, 0=idle */
-#define RU_SUSPEND	0x10	/* RU suspended */
-#define RU_NOSPACE	0x20	/* RU no resources */
-#define RU_READY	0x40	/* RU is ready */
-
-/*
- * Receive Frame Descriptor (RFD)
- */
-struct rfd_struct
-{
-  unsigned short status;	/* status word */
-  unsigned short last;		/* Bit15,Last Frame on List / Bit14,suspend */
-  unsigned short next;		/* linkoffset to next RFD */
-  unsigned short rbd_offset;	/* pointeroffset to RBD-buffer */
-  unsigned char  dest[6];	/* ethernet-address, destination */
-  unsigned char  source[6];	/* ethernet-address, source */
-  unsigned short length;	/* 802.3 frame-length */
-  unsigned short zero_dummy;	/* dummy */
-};
-
-#define RFD_LAST     0x8000	/* last: last rfd in the list */
-#define RFD_SUSP     0x4000	/* last: suspend RU after  */
-#define RFD_ERRMASK  0x0fe1     /* status: errormask */
-#define RFD_MATCHADD 0x0002     /* status: Destinationaddress !matches IA */
-#define RFD_RNR      0x0200	/* status: receiver out of resources */
-
-/*
- * Receive Buffer Descriptor (RBD)
- */
-struct rbd_struct
-{
-  unsigned short status;	/* status word,number of used bytes in buff */
-  unsigned short next;		/* pointeroffset to next RBD */
-  char          *buffer;	/* receive buffer address pointer */
-  unsigned short size;		/* size of this buffer */
-  unsigned short zero_dummy;    /* dummy */
-};
-
-#define RBD_LAST	0x8000	/* last buffer */
-#define RBD_USED	0x4000	/* this buffer has data */
-#define RBD_MASK	0x3fff	/* size-mask for length */
-
-/*
- * Statusvalues for Commands/RFD
- */
-#define STAT_COMPL   0x8000	/* status: frame/command is complete */
-#define STAT_BUSY    0x4000	/* status: frame/command is busy */
-#define STAT_OK      0x2000	/* status: frame/command is ok */
-
-/*
- * Action-Commands
- */
-#define CMD_NOP		0x0000	/* NOP */
-#define CMD_IASETUP	0x0001	/* initial address setup command */
-#define CMD_CONFIGURE	0x0002	/* configure command */
-#define CMD_MCSETUP	0x0003	/* MC setup command */
-#define CMD_XMIT	0x0004	/* transmit command */
-#define CMD_TDR		0x0005	/* time domain reflectometer (TDR) command */
-#define CMD_DUMP	0x0006	/* dump command */
-#define CMD_DIAGNOSE	0x0007	/* diagnose command */
-
-/*
- * Action command bits
- */
-#define CMD_LAST	0x8000	/* indicates last command in the CBL */
-#define CMD_SUSPEND	0x4000	/* suspend CU after this CB */
-#define CMD_INT		0x2000	/* generate interrupt after execution */
-
-/*
- * NOP - command
- */
-struct nop_cmd_struct
-{
-  unsigned short cmd_status;	/* status of this command */
-  unsigned short cmd_cmd;       /* the command itself (+bits) */
-  unsigned short cmd_link;      /* offsetpointer to next command */
-};
-
-/*
- * IA Setup command
- */
-struct iasetup_cmd_struct
-{
-  unsigned short cmd_status;
-  unsigned short cmd_cmd;
-  unsigned short cmd_link;
-  unsigned char  iaddr[6];
-};
-
-/*
- * Configure command
- */
-struct configure_cmd_struct
-{
-  unsigned short cmd_status;
-  unsigned short cmd_cmd;
-  unsigned short cmd_link;
-  unsigned char  byte_cnt;   /* size of the config-cmd */
-  unsigned char  fifo;       /* fifo/recv monitor */
-  unsigned char  sav_bf;     /* save bad frames (bit7=1)*/
-  unsigned char  adr_len;    /* adr_len(0-2),al_loc(3),pream(4-5),loopbak(6-7)*/
-  unsigned char  priority;   /* lin_prio(0-2),exp_prio(4-6),bof_metd(7) */
-  unsigned char  ifs;        /* inter frame spacing */
-  unsigned char  time_low;   /* slot time low */
-  unsigned char  time_high;  /* slot time high(0-2) and max. retries(4-7) */
-  unsigned char  promisc;    /* promisc-mode(0) , et al (1-7) */
-  unsigned char  carr_coll;  /* carrier(0-3)/collision(4-7) stuff */
-  unsigned char  fram_len;   /* minimal frame len */
-  unsigned char  dummy;	     /* dummy */
-};
-
-/*
- * Multicast Setup command
- */
-struct mcsetup_cmd_struct
-{
-  unsigned short cmd_status;
-  unsigned short cmd_cmd;
-  unsigned short cmd_link;
-  unsigned short mc_cnt;		/* number of bytes in the MC-List */
-  unsigned char  mc_list[0][6];  	/* pointer to 6 bytes entries */
-};
-
-/*
- * transmit command
- */
-struct transmit_cmd_struct
-{
-  unsigned short cmd_status;
-  unsigned short cmd_cmd;
-  unsigned short cmd_link;
-  unsigned short tbd_offset;	/* pointeroffset to TBD */
-  unsigned char  dest[6];       /* destination address of the frame */
-  unsigned short length;	/* user defined: 802.3 length / Ether type */
-};
-
-#define TCMD_ERRMASK     0x0fa0
-#define TCMD_MAXCOLLMASK 0x000f
-#define TCMD_MAXCOLL     0x0020
-#define TCMD_HEARTBEAT   0x0040
-#define TCMD_DEFERRED    0x0080
-#define TCMD_UNDERRUN    0x0100
-#define TCMD_LOSTCTS     0x0200
-#define TCMD_NOCARRIER   0x0400
-#define TCMD_LATECOLL    0x0800
-
-struct tdr_cmd_struct
-{
-  unsigned short cmd_status;
-  unsigned short cmd_cmd;
-  unsigned short cmd_link;
-  unsigned short status;
-};
-
-#define TDR_LNK_OK	0x8000	/* No link problem identified */
-#define TDR_XCVR_PRB	0x4000	/* indicates a transceiver problem */
-#define TDR_ET_OPN	0x2000	/* open, no correct termination */
-#define TDR_ET_SRT	0x1000	/* TDR detected a short circuit */
-#define TDR_TIMEMASK	0x07ff	/* mask for the time field */
-
-/*
- * Transmit Buffer Descriptor (TBD)
- */
-struct tbd_struct
-{
-  unsigned short size;		/* size + EOF-Flag(15) */
-  unsigned short next;          /* pointeroffset to next TBD */
-  char          *buffer;        /* pointer to buffer */
-};
-
-#define TBD_LAST 0x8000         /* EOF-Flag, indicates last buffer in list */
-
-/*************************************************************************/
-/*
-Verbatim from the Crynwyr stuff:
-
-    The 3c523 responds with adapter code 0x6042 at slot
-registers xxx0 and xxx1.  The setup register is at xxx2 and
-contains the following bits:
-
-0: card enable
-2,1: csr address select
-    00 = 0300
-    01 = 1300
-    10 = 2300
-    11 = 3300
-4,3: shared memory address select
-    00 = 0c0000
-    01 = 0c8000
-    10 = 0d0000
-    11 = 0d8000
-5: set to disable on-board thinnet
-7,6: (read-only) shows selected irq
-    00 = 12
-    01 = 7
-    10 = 3
-    11 = 9
-
-The interrupt-select register is at xxx3 and uses one bit per irq.
-
-0: int 12
-1: int 7
-2: int 3
-3: int 9
-
-    Again, the documentation stresses that the setup register
-should never be written.  The interrupt-select register may be
-written with the value corresponding to bits 7.6 in
-the setup register to insure corret setup.
-*/
-
-/* Offsets from the base I/O address. */
-#define	ELMC_SA		0	/* first 6 bytes are IEEE network address */
-#define ELMC_CTRL	6	/* control & status register */
-#define ELMC_REVISION	7	/* revision register, first 4 bits only */
-#define ELMC_IO_EXTENT  8
-
-/* these are the bit selects for the port register 2 */
-#define ELMC_STATUS_ENABLED	0x01
-#define ELMC_STATUS_CSR_SELECT	0x06
-#define ELMC_STATUS_MEMORY_SELECT	0x18
-#define ELMC_STATUS_DISABLE_THIN	0x20
-#define ELMC_STATUS_IRQ_SELECT	0xc0
-
-/* this is the card id used in the detection code.  You might recognize
-it from @6042.adf */
-#define ELMC_MCA_ID 0x6042
-
-/*
-   The following define the bits for the control & status register
-
-   The bank select registers can be used if more than 16K of memory is
-   on the card.  For some stupid reason, bank 3 is the one for the
-   bottom 16K, and the card defaults to bank 0.  So we have to set the
-   bank to 3 before the card will even think of operating.  To get bank
-   3, set BS0 and BS1 to high (of course...)
-*/
-#define ELMC_CTRL_BS0	0x01	/* RW bank select */
-#define ELMC_CTRL_BS1	0x02	/* RW bank select */
-#define ELMC_CTRL_INTE	0x04	/* RW interrupt enable, assert high */
-#define ELMC_CTRL_INT	0x08	/* R interrupt active, assert high */
-/*#define ELMC_CTRL_*	0x10*/	/* reserved */
-#define ELMC_CTRL_LBK	0x20	/* RW loopback enable, assert high */
-#define ELMC_CTRL_CA	0x40	/* RW channel attention, assert high */
-#define ELMC_CTRL_RST	0x80	/* RW 82586 reset, assert low */
-
-/* some handy compound bits */
-
-/* normal operation should have bank 3 and RST high, ints enabled */
-#define ELMC_NORMAL (ELMC_CTRL_INTE|ELMC_CTRL_RST|0x3)
-
-#endif /* _3c523_INCLUDE_ */
diff --git a/drivers/net/ethernet/i825xx/3c527.c b/drivers/net/ethernet/i825xx/3c527.c
deleted file mode 100644
index 278e791afe0..00000000000
--- a/drivers/net/ethernet/i825xx/3c527.c
+++ /dev/null
@@ -1,1660 +0,0 @@
-/* 3c527.c: 3Com Etherlink/MC32 driver for Linux 2.4 and 2.6.
- *
- *	(c) Copyright 1998 Red Hat Software Inc
- *	Written by Alan Cox.
- *	Further debugging by Carl Drougge.
- *      Initial SMP support by Felipe W Damasio <felipewd@terra.com.br>
- *      Heavily modified by Richard Procter <rnp@paradise.net.nz>
- *
- *	Based on skeleton.c written 1993-94 by Donald Becker and ne2.c
- *	(for the MCA stuff) written by Wim Dumon.
- *
- *	Thanks to 3Com for making this possible by providing me with the
- *	documentation.
- *
- *	This software may be used and distributed according to the terms
- *	of the GNU General Public License, incorporated herein by reference.
- *
- */
-
-#define DRV_NAME		"3c527"
-#define DRV_VERSION		"0.7-SMP"
-#define DRV_RELDATE		"2003/09/21"
-
-static const char *version =
-DRV_NAME ".c:v" DRV_VERSION " " DRV_RELDATE " Richard Procter <rnp@paradise.net.nz>\n";
-
-/**
- * DOC: Traps for the unwary
- *
- *	The diagram (Figure 1-1) and the POS summary disagree with the
- *	"Interrupt Level" section in the manual.
- *
- *	The manual contradicts itself when describing the minimum number
- *	buffers in the 'configure lists' command.
- *	My card accepts a buffer config of 4/4.
- *
- *	Setting the SAV BP bit does not save bad packets, but
- *	only enables RX on-card stats collection.
- *
- *	The documentation in places seems to miss things. In actual fact
- *	I've always eventually found everything is documented, it just
- *	requires careful study.
- *
- * DOC: Theory Of Operation
- *
- *	The 3com 3c527 is a 32bit MCA bus mastering adapter with a large
- *	amount of on board intelligence that housekeeps a somewhat dumber
- *	Intel NIC. For performance we want to keep the transmit queue deep
- *	as the card can transmit packets while fetching others from main
- *	memory by bus master DMA. Transmission and reception are driven by
- *	circular buffer queues.
- *
- *	The mailboxes can be used for controlling how the card traverses
- *	its buffer rings, but are used only for initial setup in this
- *	implementation.  The exec mailbox allows a variety of commands to
- *	be executed. Each command must complete before the next is
- *	executed. Primarily we use the exec mailbox for controlling the
- *	multicast lists.  We have to do a certain amount of interesting
- *	hoop jumping as the multicast list changes can occur in interrupt
- *	state when the card has an exec command pending. We defer such
- *	events until the command completion interrupt.
- *
- *	A copy break scheme (taken from 3c59x.c) is employed whereby
- *	received frames exceeding a configurable length are passed
- *	directly to the higher networking layers without incuring a copy,
- *	in what amounts to a time/space trade-off.
- *
- *	The card also keeps a large amount of statistical information
- *	on-board. In a perfect world, these could be used safely at no
- *	cost. However, lacking information to the contrary, processing
- *	them without races would involve so much extra complexity as to
- *	make it unworthwhile to do so. In the end, a hybrid SW/HW
- *	implementation was made necessary --- see mc32_update_stats().
- *
- * DOC: Notes
- *
- *	It should be possible to use two or more cards, but at this stage
- *	only by loading two copies of the same module.
- *
- *	The on-board 82586 NIC has trouble receiving multiple
- *	back-to-back frames and so is likely to drop packets from fast
- *	senders.
-**/
-
-#include <linux/module.h>
-
-#include <linux/errno.h>
-#include <linux/netdevice.h>
-#include <linux/etherdevice.h>
-#include <linux/if_ether.h>
-#include <linux/init.h>
-#include <linux/kernel.h>
-#include <linux/types.h>
-#include <linux/fcntl.h>
-#include <linux/interrupt.h>
-#include <linux/mca-legacy.h>
-#include <linux/ioport.h>
-#include <linux/in.h>
-#include <linux/skbuff.h>
-#include <linux/slab.h>
-#include <linux/string.h>
-#include <linux/wait.h>
-#include <linux/ethtool.h>
-#include <linux/completion.h>
-#include <linux/bitops.h>
-#include <linux/semaphore.h>
-
-#include <asm/uaccess.h>
-#include <asm/io.h>
-#include <asm/dma.h>
-
-#include "3c527.h"
-
-MODULE_LICENSE("GPL");
-
-/*
- * The name of the card. Is used for messages and in the requests for
- * io regions, irqs and dma channels
- */
-static const char* cardname = DRV_NAME;
-
-/* use 0 for production, 1 for verification, >2 for debug */
-#ifndef NET_DEBUG
-#define NET_DEBUG 2
-#endif
-
-static unsigned int mc32_debug = NET_DEBUG;
-
-/* The number of low I/O ports used by the ethercard. */
-#define MC32_IO_EXTENT	8
-
-/* As implemented, values must be a power-of-2 -- 4/8/16/32 */
-#define TX_RING_LEN     32       /* Typically the card supports 37  */
-#define RX_RING_LEN     8        /*     "       "        "          */
-
-/* Copy break point, see above for details.
- * Setting to > 1512 effectively disables this feature.	*/
-#define RX_COPYBREAK    200      /* Value from 3c59x.c */
-
-/* Issue the 82586 workaround command - this is for "busy lans", but
- * basically means for all lans now days - has a performance (latency)
- * cost, but best set. */
-static const int WORKAROUND_82586=1;
-
-/* Pointers to buffers and their on-card records */
-struct mc32_ring_desc
-{
-	volatile struct skb_header *p;
-	struct sk_buff *skb;
-};
-
-/* Information that needs to be kept for each board. */
-struct mc32_local
-{
-	int slot;
-
-	u32 base;
-	volatile struct mc32_mailbox *rx_box;
-	volatile struct mc32_mailbox *tx_box;
-	volatile struct mc32_mailbox *exec_box;
-        volatile struct mc32_stats *stats;    /* Start of on-card statistics */
-        u16 tx_chain;           /* Transmit list start offset */
-	u16 rx_chain;           /* Receive list start offset */
-        u16 tx_len;             /* Transmit list count */
-        u16 rx_len;             /* Receive list count */
-
-	u16 xceiver_desired_state; /* HALTED or RUNNING */
-	u16 cmd_nonblocking;    /* Thread is uninterested in command result */
-	u16 mc_reload_wait;	/* A multicast load request is pending */
-	u32 mc_list_valid;	/* True when the mclist is set */
-
-	struct mc32_ring_desc tx_ring[TX_RING_LEN];	/* Host Transmit ring */
-	struct mc32_ring_desc rx_ring[RX_RING_LEN];	/* Host Receive ring */
-
-	atomic_t tx_count;	/* buffers left */
-	atomic_t tx_ring_head;  /* index to tx en-queue end */
-	u16 tx_ring_tail;       /* index to tx de-queue end */
-
-	u16 rx_ring_tail;       /* index to rx de-queue end */
-
-	struct semaphore cmd_mutex;    /* Serialises issuing of execute commands */
-        struct completion execution_cmd; /* Card has completed an execute command */
-	struct completion xceiver_cmd;   /* Card has completed a tx or rx command */
-};
-
-/* The station (ethernet) address prefix, used for a sanity check. */
-#define SA_ADDR0 0x02
-#define SA_ADDR1 0x60
-#define SA_ADDR2 0xAC
-
-struct mca_adapters_t {
-	unsigned int	id;
-	char		*name;
-};
-
-static const struct mca_adapters_t mc32_adapters[] = {
-	{ 0x0041, "3COM EtherLink MC/32" },
-	{ 0x8EF5, "IBM High Performance Lan Adapter" },
-	{ 0x0000, NULL }
-};
-
-
-/* Macros for ring index manipulations */
-static inline u16 next_rx(u16 rx) { return (rx+1)&(RX_RING_LEN-1); };
-static inline u16 prev_rx(u16 rx) { return (rx-1)&(RX_RING_LEN-1); };
-
-static inline u16 next_tx(u16 tx) { return (tx+1)&(TX_RING_LEN-1); };
-
-
-/* Index to functions, as function prototypes. */
-static int	mc32_probe1(struct net_device *dev, int ioaddr);
-static int      mc32_command(struct net_device *dev, u16 cmd, void *data, int len);
-static int	mc32_open(struct net_device *dev);
-static void	mc32_timeout(struct net_device *dev);
-static netdev_tx_t mc32_send_packet(struct sk_buff *skb,
-				    struct net_device *dev);
-static irqreturn_t mc32_interrupt(int irq, void *dev_id);
-static int	mc32_close(struct net_device *dev);
-static struct	net_device_stats *mc32_get_stats(struct net_device *dev);
-static void	mc32_set_multicast_list(struct net_device *dev);
-static void	mc32_reset_multicast_list(struct net_device *dev);
-static const struct ethtool_ops netdev_ethtool_ops;
-
-static void cleanup_card(struct net_device *dev)
-{
-	struct mc32_local *lp = netdev_priv(dev);
-	unsigned slot = lp->slot;
-	mca_mark_as_unused(slot);
-	mca_set_adapter_name(slot, NULL);
-	free_irq(dev->irq, dev);
-	release_region(dev->base_addr, MC32_IO_EXTENT);
-}
-
-/**
- * mc32_probe 	-	Search for supported boards
- * @unit: interface number to use
- *
- * Because MCA bus is a real bus and we can scan for cards we could do a
- * single scan for all boards here. Right now we use the passed in device
- * structure and scan for only one board. This needs fixing for modules
- * in particular.
- */
-
-struct net_device *__init mc32_probe(int unit)
-{
-	struct net_device *dev = alloc_etherdev(sizeof(struct mc32_local));
-	static int current_mca_slot = -1;
-	int i;
-	int err;
-
-	if (!dev)
-		return ERR_PTR(-ENOMEM);
-
-	if (unit >= 0)
-		sprintf(dev->name, "eth%d", unit);
-
-	/* Do not check any supplied i/o locations.
-	   POS registers usually don't fail :) */
-
-	/* MCA cards have POS registers.
-	   Autodetecting MCA cards is extremely simple.
-	   Just search for the card. */
-
-	for(i = 0; (mc32_adapters[i].name != NULL); i++) {
-		current_mca_slot =
-			mca_find_unused_adapter(mc32_adapters[i].id, 0);
-
-		if(current_mca_slot != MCA_NOTFOUND) {
-			if(!mc32_probe1(dev, current_mca_slot))
-			{
-				mca_set_adapter_name(current_mca_slot,
-						mc32_adapters[i].name);
-				mca_mark_as_used(current_mca_slot);
-				err = register_netdev(dev);
-				if (err) {
-					cleanup_card(dev);
-					free_netdev(dev);
-					dev = ERR_PTR(err);
-				}
-				return dev;
-			}
-
-		}
-	}
-	free_netdev(dev);
-	return ERR_PTR(-ENODEV);
-}
-
-static const struct net_device_ops netdev_ops = {
-	.ndo_open		= mc32_open,
-	.ndo_stop		= mc32_close,
-	.ndo_start_xmit		= mc32_send_packet,
-	.ndo_get_stats		= mc32_get_stats,
-	.ndo_set_rx_mode	= mc32_set_multicast_list,
-	.ndo_tx_timeout		= mc32_timeout,
-	.ndo_change_mtu		= eth_change_mtu,
-	.ndo_set_mac_address 	= eth_mac_addr,
-	.ndo_validate_addr	= eth_validate_addr,
-};
-
-/**
- * mc32_probe1	-	Check a given slot for a board and test the card
- * @dev:  Device structure to fill in
- * @slot: The MCA bus slot being used by this card
- *
- * Decode the slot data and configure the card structures. Having done this we
- * can reset the card and configure it. The card does a full self test cycle
- * in firmware so we have to wait for it to return and post us either a
- * failure case or some addresses we use to find the board internals.
- */
-
-static int __init mc32_probe1(struct net_device *dev, int slot)
-{
-	static unsigned version_printed;
-	int i, err;
-	u8 POS;
-	u32 base;
-	struct mc32_local *lp = netdev_priv(dev);
-	static const u16 mca_io_bases[] = {
-		0x7280,0x7290,
-		0x7680,0x7690,
-		0x7A80,0x7A90,
-		0x7E80,0x7E90
-	};
-	static const u32 mca_mem_bases[] = {
-		0x00C0000,
-		0x00C4000,
-		0x00C8000,
-		0x00CC000,
-		0x00D0000,
-		0x00D4000,
-		0x00D8000,
-		0x00DC000
-	};
-	static const char * const failures[] = {
-		"Processor instruction",
-		"Processor data bus",
-		"Processor data bus",
-		"Processor data bus",
-		"Adapter bus",
-		"ROM checksum",
-		"Base RAM",
-		"Extended RAM",
-		"82586 internal loopback",
-		"82586 initialisation failure",
-		"Adapter list configuration error"
-	};
-
-	/* Time to play MCA games */
-
-	if (mc32_debug  &&  version_printed++ == 0)
-		pr_debug("%s", version);
-
-	pr_info("%s: %s found in slot %d: ", dev->name, cardname, slot);
-
-	POS = mca_read_stored_pos(slot, 2);
-
-	if(!(POS&1))
-	{
-		pr_cont("disabled.\n");
-		return -ENODEV;
-	}
-
-	/* Fill in the 'dev' fields. */
-	dev->base_addr = mca_io_bases[(POS>>1)&7];
-	dev->mem_start = mca_mem_bases[(POS>>4)&7];
-
-	POS = mca_read_stored_pos(slot, 4);
-	if(!(POS&1))
-	{
-		pr_cont("memory window disabled.\n");
-		return -ENODEV;
-	}
-
-	POS = mca_read_stored_pos(slot, 5);
-
-	i=(POS>>4)&3;
-	if(i==3)
-	{
-		pr_cont("invalid memory window.\n");
-		return -ENODEV;
-	}
-
-	i*=16384;
-	i+=16384;
-
-	dev->mem_end=dev->mem_start + i;
-
-	dev->irq = ((POS>>2)&3)+9;
-
-	if(!request_region(dev->base_addr, MC32_IO_EXTENT, cardname))
-	{
-		pr_cont("io 0x%3lX, which is busy.\n", dev->base_addr);
-		return -EBUSY;
-	}
-
-	pr_cont("io 0x%3lX irq %d mem 0x%lX (%dK)\n",
-		dev->base_addr, dev->irq, dev->mem_start, i/1024);
-
-
-	/* We ought to set the cache line size here.. */
-
-
-	/*
-	 *	Go PROM browsing
-	 */
-
-	/* Retrieve and print the ethernet address. */
-	for (i = 0; i < 6; i++)
-	{
-		mca_write_pos(slot, 6, i+12);
-		mca_write_pos(slot, 7, 0);
-
-		dev->dev_addr[i] = mca_read_pos(slot,3);
-	}
-
-	pr_info("%s: Address %pM ", dev->name, dev->dev_addr);
-
-	mca_write_pos(slot, 6, 0);
-	mca_write_pos(slot, 7, 0);
-
-	POS = mca_read_stored_pos(slot, 4);
-
-	if(POS&2)
-		pr_cont(": BNC port selected.\n");
-	else
-		pr_cont(": AUI port selected.\n");
-
-	POS=inb(dev->base_addr+HOST_CTRL);
-	POS|=HOST_CTRL_ATTN|HOST_CTRL_RESET;
-	POS&=~HOST_CTRL_INTE;
-	outb(POS, dev->base_addr+HOST_CTRL);
-	/* Reset adapter */
-	udelay(100);
-	/* Reset off */
-	POS&=~(HOST_CTRL_ATTN|HOST_CTRL_RESET);
-	outb(POS, dev->base_addr+HOST_CTRL);
-
-	udelay(300);
-
-	/*
-	 *	Grab the IRQ
-	 */
-
-	err = request_irq(dev->irq, mc32_interrupt, IRQF_SHARED, DRV_NAME, dev);
-	if (err) {
-		release_region(dev->base_addr, MC32_IO_EXTENT);
-		pr_err("%s: unable to get IRQ %d.\n", DRV_NAME, dev->irq);
-		goto err_exit_ports;
-	}
-
-	memset(lp, 0, sizeof(struct mc32_local));
-	lp->slot = slot;
-
-	i=0;
-
-	base = inb(dev->base_addr);
-
-	while(base == 0xFF)
-	{
-		i++;
-		if(i == 1000)
-		{
-			pr_err("%s: failed to boot adapter.\n", dev->name);
-			err = -ENODEV;
-			goto err_exit_irq;
-		}
-		udelay(1000);
-		if(inb(dev->base_addr+2)&(1<<5))
-			base = inb(dev->base_addr);
-	}
-
-	if(base>0)
-	{
-		if(base < 0x0C)
-			pr_err("%s: %s%s.\n", dev->name, failures[base-1],
-				base<0x0A?" test failure":"");
-		else
-			pr_err("%s: unknown failure %d.\n", dev->name, base);
-		err = -ENODEV;
-		goto err_exit_irq;
-	}
-
-	base=0;
-	for(i=0;i<4;i++)
-	{
-		int n=0;
-
-		while(!(inb(dev->base_addr+2)&(1<<5)))
-		{
-			n++;
-			udelay(50);
-			if(n>100)
-			{
-				pr_err("%s: mailbox read fail (%d).\n", dev->name, i);
-				err = -ENODEV;
-				goto err_exit_irq;
-			}
-		}
-
-		base|=(inb(dev->base_addr)<<(8*i));
-	}
-
-	lp->exec_box=isa_bus_to_virt(dev->mem_start+base);
-
-	base=lp->exec_box->data[1]<<16|lp->exec_box->data[0];
-
-	lp->base = dev->mem_start+base;
-
-	lp->rx_box=isa_bus_to_virt(lp->base + lp->exec_box->data[2]);
-	lp->tx_box=isa_bus_to_virt(lp->base + lp->exec_box->data[3]);
-
-	lp->stats = isa_bus_to_virt(lp->base + lp->exec_box->data[5]);
-
-	/*
-	 *	Descriptor chains (card relative)
-	 */
-
-	lp->tx_chain 		= lp->exec_box->data[8];   /* Transmit list start offset */
-	lp->rx_chain 		= lp->exec_box->data[10];  /* Receive list start offset */
-	lp->tx_len 		= lp->exec_box->data[9];   /* Transmit list count */
-	lp->rx_len 		= lp->exec_box->data[11];  /* Receive list count */
-
-	sema_init(&lp->cmd_mutex, 0);
-	init_completion(&lp->execution_cmd);
-	init_completion(&lp->xceiver_cmd);
-
-	pr_info("%s: Firmware Rev %d. %d RX buffers, %d TX buffers. Base of 0x%08X.\n",
-		dev->name, lp->exec_box->data[12], lp->rx_len, lp->tx_len, lp->base);
-
-	dev->netdev_ops		= &netdev_ops;
-	dev->watchdog_timeo	= HZ*5;	/* Board does all the work */
-	dev->ethtool_ops	= &netdev_ethtool_ops;
-
-	return 0;
-
-err_exit_irq:
-	free_irq(dev->irq, dev);
-err_exit_ports:
-	release_region(dev->base_addr, MC32_IO_EXTENT);
-	return err;
-}
-
-
-/**
- *	mc32_ready_poll		-	wait until we can feed it a command
- *	@dev:	The device to wait for
- *
- *	Wait until the card becomes ready to accept a command via the
- *	command register. This tells us nothing about the completion
- *	status of any pending commands and takes very little time at all.
- */
-
-static inline void mc32_ready_poll(struct net_device *dev)
-{
-	int ioaddr = dev->base_addr;
-	while(!(inb(ioaddr+HOST_STATUS)&HOST_STATUS_CRR));
-}
-
-
-/**
- *	mc32_command_nowait	-	send a command non blocking
- *	@dev: The 3c527 to issue the command to
- *	@cmd: The command word to write to the mailbox
- *	@data: A data block if the command expects one
- *	@len: Length of the data block
- *
- *	Send a command from interrupt state. If there is a command
- *	currently being executed then we return an error of -1. It
- *	simply isn't viable to wait around as commands may be
- *	slow. This can theoretically be starved on SMP, but it's hard
- *	to see a realistic situation.  We do not wait for the command
- *	to complete --- we rely on the interrupt handler to tidy up
- *	after us.
- */
-
-static int mc32_command_nowait(struct net_device *dev, u16 cmd, void *data, int len)
-{
-	struct mc32_local *lp = netdev_priv(dev);
-	int ioaddr = dev->base_addr;
-	int ret = -1;
-
-	if (down_trylock(&lp->cmd_mutex) == 0)
-	{
-		lp->cmd_nonblocking=1;
-		lp->exec_box->mbox=0;
-		lp->exec_box->mbox=cmd;
-		memcpy((void *)lp->exec_box->data, data, len);
-		barrier();	/* the memcpy forgot the volatile so be sure */
-
-		/* Send the command */
-		mc32_ready_poll(dev);
-		outb(1<<6, ioaddr+HOST_CMD);
-
-		ret = 0;
-
-		/* Interrupt handler will signal mutex on completion */
-	}
-
-	return ret;
-}
-
-
-/**
- *	mc32_command	-	send a command and sleep until completion
- *	@dev: The 3c527 card to issue the command to
- *	@cmd: The command word to write to the mailbox
- *	@data: A data block if the command expects one
- *	@len: Length of the data block
- *
- *	Sends exec commands in a user context. This permits us to wait around
- *	for the replies and also to wait for the command buffer to complete
- *	from a previous command before we execute our command. After our
- *	command completes we will attempt any pending multicast reload
- *	we blocked off by hogging the exec buffer.
- *
- *	You feed the card a command, you wait, it interrupts you get a
- *	reply. All well and good. The complication arises because you use
- *	commands for filter list changes which come in at bh level from things
- *	like IPV6 group stuff.
- */
-
-static int mc32_command(struct net_device *dev, u16 cmd, void *data, int len)
-{
-	struct mc32_local *lp = netdev_priv(dev);
-	int ioaddr = dev->base_addr;
-	int ret = 0;
-
-	down(&lp->cmd_mutex);
-
-	/*
-	 *     My Turn
-	 */
-
-	lp->cmd_nonblocking=0;
-	lp->exec_box->mbox=0;
-	lp->exec_box->mbox=cmd;
-	memcpy((void *)lp->exec_box->data, data, len);
-	barrier();	/* the memcpy forgot the volatile so be sure */
-
-	mc32_ready_poll(dev);
-	outb(1<<6, ioaddr+HOST_CMD);
-
-	wait_for_completion(&lp->execution_cmd);
-
-	if(lp->exec_box->mbox&(1<<13))
-		ret = -1;
-
-	up(&lp->cmd_mutex);
-
-	/*
-	 *	A multicast set got blocked - try it now
-         */
-
-	if(lp->mc_reload_wait)
-	{
-		mc32_reset_multicast_list(dev);
-	}
-
-	return ret;
-}
-
-
-/**
- *	mc32_start_transceiver	-	tell board to restart tx/rx
- *	@dev: The 3c527 card to issue the command to
- *
- *	This may be called from the interrupt state, where it is used
- *	to restart the rx ring if the card runs out of rx buffers.
- *
- * 	We must first check if it's ok to (re)start the transceiver. See
- *      mc32_close for details.
- */
-
-static void mc32_start_transceiver(struct net_device *dev) {
-
-	struct mc32_local *lp = netdev_priv(dev);
-	int ioaddr = dev->base_addr;
-
-	/* Ignore RX overflow on device closure */
-	if (lp->xceiver_desired_state==HALTED)
-		return;
-
-	/* Give the card the offset to the post-EOL-bit RX descriptor */
-	mc32_ready_poll(dev);
-	lp->rx_box->mbox=0;
-	lp->rx_box->data[0]=lp->rx_ring[prev_rx(lp->rx_ring_tail)].p->next;
-	outb(HOST_CMD_START_RX, ioaddr+HOST_CMD);
-
-	mc32_ready_poll(dev);
-	lp->tx_box->mbox=0;
-	outb(HOST_CMD_RESTRT_TX, ioaddr+HOST_CMD);   /* card ignores this on RX restart */
-
-	/* We are not interrupted on start completion */
-}
-
-
-/**
- *	mc32_halt_transceiver	-	tell board to stop tx/rx
- *	@dev: The 3c527 card to issue the command to
- *
- *	We issue the commands to halt the card's transceiver. In fact,
- *	after some experimenting we now simply tell the card to
- *	suspend. When issuing aborts occasionally odd things happened.
- *
- *	We then sleep until the card has notified us that both rx and
- *	tx have been suspended.
- */
-
-static void mc32_halt_transceiver(struct net_device *dev)
-{
-	struct mc32_local *lp = netdev_priv(dev);
-	int ioaddr = dev->base_addr;
-
-	mc32_ready_poll(dev);
-	lp->rx_box->mbox=0;
-	outb(HOST_CMD_SUSPND_RX, ioaddr+HOST_CMD);
-	wait_for_completion(&lp->xceiver_cmd);
-
-	mc32_ready_poll(dev);
-	lp->tx_box->mbox=0;
-	outb(HOST_CMD_SUSPND_TX, ioaddr+HOST_CMD);
-	wait_for_completion(&lp->xceiver_cmd);
-}
-
-
-/**
- *	mc32_load_rx_ring	-	load the ring of receive buffers
- *	@dev: 3c527 to build the ring for
- *
- *	This initialises the on-card and driver datastructures to
- *	the point where mc32_start_transceiver() can be called.
- *
- *	The card sets up the receive ring for us. We are required to use the
- *	ring it provides, although the size of the ring is configurable.
- *
- * 	We allocate an sk_buff for each ring entry in turn and
- * 	initialise its house-keeping info. At the same time, we read
- * 	each 'next' pointer in our rx_ring array. This reduces slow
- * 	shared-memory reads and makes it easy to access predecessor
- * 	descriptors.
- *
- *	We then set the end-of-list bit for the last entry so that the
- * 	card will know when it has run out of buffers.
- */
-
-static int mc32_load_rx_ring(struct net_device *dev)
-{
-	struct mc32_local *lp = netdev_priv(dev);
-	int i;
-	u16 rx_base;
-	volatile struct skb_header *p;
-
-	rx_base=lp->rx_chain;
-
-	for(i=0; i<RX_RING_LEN; i++) {
-		lp->rx_ring[i].skb=alloc_skb(1532, GFP_KERNEL);
-		if (lp->rx_ring[i].skb==NULL) {
-			for (;i>=0;i--)
-				kfree_skb(lp->rx_ring[i].skb);
-			return -ENOBUFS;
-		}
-		skb_reserve(lp->rx_ring[i].skb, 18);
-
-		p=isa_bus_to_virt(lp->base+rx_base);
-
-		p->control=0;
-		p->data=isa_virt_to_bus(lp->rx_ring[i].skb->data);
-		p->status=0;
-		p->length=1532;
-
-		lp->rx_ring[i].p=p;
-		rx_base=p->next;
-	}
-
-	lp->rx_ring[i-1].p->control |= CONTROL_EOL;
-
-	lp->rx_ring_tail=0;
-
-	return 0;
-}
-
-
-/**
- *	mc32_flush_rx_ring	-	free the ring of receive buffers
- *	@lp: Local data of 3c527 to flush the rx ring of
- *
- *	Free the buffer for each ring slot. This may be called
- *      before mc32_load_rx_ring(), eg. on error in mc32_open().
- *      Requires rx skb pointers to point to a valid skb, or NULL.
- */
-
-static void mc32_flush_rx_ring(struct net_device *dev)
-{
-	struct mc32_local *lp = netdev_priv(dev);
-	int i;
-
-	for(i=0; i < RX_RING_LEN; i++)
-	{
-		if (lp->rx_ring[i].skb) {
-			dev_kfree_skb(lp->rx_ring[i].skb);
-			lp->rx_ring[i].skb = NULL;
-		}
-		lp->rx_ring[i].p=NULL;
-	}
-}
-
-
-/**
- *	mc32_load_tx_ring	-	load transmit ring
- *	@dev: The 3c527 card to issue the command to
- *
- *	This sets up the host transmit data-structures.
- *
- *	First, we obtain from the card it's current position in the tx
- *	ring, so that we will know where to begin transmitting
- *	packets.
- *
- * 	Then, we read the 'next' pointers from the on-card tx ring into
- *  	our tx_ring array to reduce slow shared-mem reads. Finally, we
- * 	intitalise the tx house keeping variables.
- *
- */
-
-static void mc32_load_tx_ring(struct net_device *dev)
-{
-	struct mc32_local *lp = netdev_priv(dev);
-	volatile struct skb_header *p;
-	int i;
-	u16 tx_base;
-
-	tx_base=lp->tx_box->data[0];
-
-	for(i=0 ; i<TX_RING_LEN ; i++)
-	{
-		p=isa_bus_to_virt(lp->base+tx_base);
-		lp->tx_ring[i].p=p;
-		lp->tx_ring[i].skb=NULL;
-
-		tx_base=p->next;
-	}
-
-	/* -1 so that tx_ring_head cannot "lap" tx_ring_tail */
-	/* see mc32_tx_ring */
-
-	atomic_set(&lp->tx_count, TX_RING_LEN-1);
-	atomic_set(&lp->tx_ring_head, 0);
-	lp->tx_ring_tail=0;
-}
-
-
-/**
- *	mc32_flush_tx_ring 	-	free transmit ring
- *	@lp: Local data of 3c527 to flush the tx ring of
- *
- *      If the ring is non-empty, zip over the it, freeing any
- *      allocated skb_buffs.  The tx ring house-keeping variables are
- *      then reset. Requires rx skb pointers to point to a valid skb,
- *      or NULL.
- */
-
-static void mc32_flush_tx_ring(struct net_device *dev)
-{
-	struct mc32_local *lp = netdev_priv(dev);
-	int i;
-
-	for (i=0; i < TX_RING_LEN; i++)
-	{
-		if (lp->tx_ring[i].skb)
-		{
-			dev_kfree_skb(lp->tx_ring[i].skb);
-			lp->tx_ring[i].skb = NULL;
-		}
-	}
-
-	atomic_set(&lp->tx_count, 0);
-	atomic_set(&lp->tx_ring_head, 0);
-	lp->tx_ring_tail=0;
-}
-
-
-/**
- *	mc32_open	-	handle 'up' of card
- *	@dev: device to open
- *
- *	The user is trying to bring the card into ready state. This requires
- *	a brief dialogue with the card. Firstly we enable interrupts and then
- *	'indications'. Without these enabled the card doesn't bother telling
- *	us what it has done. This had me puzzled for a week.
- *
- *	We configure the number of card descriptors, then load the network
- *	address and multicast filters. Turn on the workaround mode. This
- *	works around a bug in the 82586 - it asks the firmware to do
- *	so. It has a performance (latency) hit but is needed on busy
- *	[read most] lans. We load the ring with buffers then we kick it
- *	all off.
- */
-
-static int mc32_open(struct net_device *dev)
-{
-	int ioaddr = dev->base_addr;
-	struct mc32_local *lp = netdev_priv(dev);
-	u8 one=1;
-	u8 regs;
-	u16 descnumbuffs[2] = {TX_RING_LEN, RX_RING_LEN};
-
-	/*
-	 *	Interrupts enabled
-	 */
-
-	regs=inb(ioaddr+HOST_CTRL);
-	regs|=HOST_CTRL_INTE;
-	outb(regs, ioaddr+HOST_CTRL);
-
-	/*
-	 *      Allow ourselves to issue commands
-	 */
-
-	up(&lp->cmd_mutex);
-
-
-	/*
-	 *	Send the indications on command
-	 */
-
-	mc32_command(dev, 4, &one, 2);
-
-	/*
-	 *	Poke it to make sure it's really dead.
-	 */
-
-	mc32_halt_transceiver(dev);
-	mc32_flush_tx_ring(dev);
-
-	/*
-	 *	Ask card to set up on-card descriptors to our spec
-	 */
-
-	if(mc32_command(dev, 8, descnumbuffs, 4)) {
-		pr_info("%s: %s rejected our buffer configuration!\n",
-	 	       dev->name, cardname);
-		mc32_close(dev);
-		return -ENOBUFS;
-	}
-
-	/* Report new configuration */
-	mc32_command(dev, 6, NULL, 0);
-
-	lp->tx_chain 		= lp->exec_box->data[8];   /* Transmit list start offset */
-	lp->rx_chain 		= lp->exec_box->data[10];  /* Receive list start offset */
-	lp->tx_len 		= lp->exec_box->data[9];   /* Transmit list count */
-	lp->rx_len 		= lp->exec_box->data[11];  /* Receive list count */
-
-	/* Set Network Address */
-	mc32_command(dev, 1, dev->dev_addr, 6);
-
-	/* Set the filters */
-	mc32_set_multicast_list(dev);
-
-	if (WORKAROUND_82586) {
-		u16 zero_word=0;
-		mc32_command(dev, 0x0D, &zero_word, 2);   /* 82586 bug workaround on  */
-	}
-
-	mc32_load_tx_ring(dev);
-
-	if(mc32_load_rx_ring(dev))
-	{
-		mc32_close(dev);
-		return -ENOBUFS;
-	}
-
-	lp->xceiver_desired_state = RUNNING;
-
-	/* And finally, set the ball rolling... */
-	mc32_start_transceiver(dev);
-
-	netif_start_queue(dev);
-
-	return 0;
-}
-
-
-/**
- *	mc32_timeout	-	handle a timeout from the network layer
- *	@dev: 3c527 that timed out
- *
- *	Handle a timeout on transmit from the 3c527. This normally means
- *	bad things as the hardware handles cable timeouts and mess for
- *	us.
- *
- */
-
-static void mc32_timeout(struct net_device *dev)
-{
-	pr_warning("%s: transmit timed out?\n", dev->name);
-	/* Try to restart the adaptor. */
-	netif_wake_queue(dev);
-}
-
-
-/**
- *	mc32_send_packet	-	queue a frame for transmit
- *	@skb: buffer to transmit
- *	@dev: 3c527 to send it out of
- *
- *	Transmit a buffer. This normally means throwing the buffer onto
- *	the transmit queue as the queue is quite large. If the queue is
- *	full then we set tx_busy and return. Once the interrupt handler
- *	gets messages telling it to reclaim transmit queue entries, we will
- *	clear tx_busy and the kernel will start calling this again.
- *
- *      We do not disable interrupts or acquire any locks; this can
- *      run concurrently with mc32_tx_ring(), and the function itself
- *      is serialised at a higher layer. However, similarly for the
- *      card itself, we must ensure that we update tx_ring_head only
- *      after we've established a valid packet on the tx ring (and
- *      before we let the card "see" it, to prevent it racing with the
- *      irq handler).
- *
- */
-
-static netdev_tx_t mc32_send_packet(struct sk_buff *skb,
-				    struct net_device *dev)
-{
-	struct mc32_local *lp = netdev_priv(dev);
-	u32 head = atomic_read(&lp->tx_ring_head);
-
-	volatile struct skb_header *p, *np;
-
-	netif_stop_queue(dev);
-
-	if(atomic_read(&lp->tx_count)==0) {
-		return NETDEV_TX_BUSY;
-	}
-
-	if (skb_padto(skb, ETH_ZLEN)) {
-		netif_wake_queue(dev);
-		return NETDEV_TX_OK;
-	}
-
-	atomic_dec(&lp->tx_count);
-
-	/* P is the last sending/sent buffer as a pointer */
-	p=lp->tx_ring[head].p;
-
-	head = next_tx(head);
-
-	/* NP is the buffer we will be loading */
-	np=lp->tx_ring[head].p;
-
-	/* We will need this to flush the buffer out */
-	lp->tx_ring[head].skb=skb;
-
-	np->length      = unlikely(skb->len < ETH_ZLEN) ? ETH_ZLEN : skb->len;
-	np->data	= isa_virt_to_bus(skb->data);
-	np->status	= 0;
-	np->control     = CONTROL_EOP | CONTROL_EOL;
-	wmb();
-
-	/*
-	 * The new frame has been setup; we can now
-	 * let the interrupt handler and card "see" it
-	 */
-
-	atomic_set(&lp->tx_ring_head, head);
-	p->control     &= ~CONTROL_EOL;
-
-	netif_wake_queue(dev);
-	return NETDEV_TX_OK;
-}
-
-
-/**
- *	mc32_update_stats	-	pull off the on board statistics
- *	@dev: 3c527 to service
- *
- *
- *	Query and reset the on-card stats. There's the small possibility
- *	of a race here, which would result in an underestimation of
- *	actual errors. As such, we'd prefer to keep all our stats
- *	collection in software. As a rule, we do. However it can't be
- *	used for rx errors and collisions as, by default, the card discards
- *	bad rx packets.
- *
- *	Setting the SAV BP in the rx filter command supposedly
- *	stops this behaviour. However, testing shows that it only seems to
- *	enable the collation of on-card rx statistics --- the driver
- *	never sees an RX descriptor with an error status set.
- *
- */
-
-static void mc32_update_stats(struct net_device *dev)
-{
-	struct mc32_local *lp = netdev_priv(dev);
-	volatile struct mc32_stats *st = lp->stats;
-
-	u32 rx_errors=0;
-
-	rx_errors+=dev->stats.rx_crc_errors   +=st->rx_crc_errors;
-	                                           st->rx_crc_errors=0;
-	rx_errors+=dev->stats.rx_fifo_errors  +=st->rx_overrun_errors;
-	                                           st->rx_overrun_errors=0;
-	rx_errors+=dev->stats.rx_frame_errors +=st->rx_alignment_errors;
- 	                                           st->rx_alignment_errors=0;
-	rx_errors+=dev->stats.rx_length_errors+=st->rx_tooshort_errors;
-	                                           st->rx_tooshort_errors=0;
-	rx_errors+=dev->stats.rx_missed_errors+=st->rx_outofresource_errors;
-	                                           st->rx_outofresource_errors=0;
-        dev->stats.rx_errors=rx_errors;
-
-	/* Number of packets which saw one collision */
-	dev->stats.collisions+=st->dataC[10];
-	st->dataC[10]=0;
-
-	/* Number of packets which saw 2--15 collisions */
-	dev->stats.collisions+=st->dataC[11];
-	st->dataC[11]=0;
-}
-
-
-/**
- *	mc32_rx_ring	-	process the receive ring
- *	@dev: 3c527 that needs its receive ring processing
- *
- *
- *	We have received one or more indications from the card that a
- *	receive has completed. The buffer ring thus contains dirty
- *	entries. We walk the ring by iterating over the circular rx_ring
- *	array, starting at the next dirty buffer (which happens to be the
- *	one we finished up at last time around).
- *
- *	For each completed packet, we will either copy it and pass it up
- * 	the stack or, if the packet is near MTU sized, we allocate
- *	another buffer and flip the old one up the stack.
- *
- *	We must succeed in keeping a buffer on the ring. If necessary we
- *	will toss a received packet rather than lose a ring entry. Once
- *	the first uncompleted descriptor is found, we move the
- *	End-Of-List bit to include the buffers just processed.
- *
- */
-
-static void mc32_rx_ring(struct net_device *dev)
-{
-	struct mc32_local *lp = netdev_priv(dev);
-	volatile struct skb_header *p;
-	u16 rx_ring_tail;
-	u16 rx_old_tail;
-	int x=0;
-
-	rx_old_tail = rx_ring_tail = lp->rx_ring_tail;
-
-	do
-	{
-		p=lp->rx_ring[rx_ring_tail].p;
-
-		if(!(p->status & (1<<7))) { /* Not COMPLETED */
-			break;
-		}
-		if(p->status & (1<<6)) /* COMPLETED_OK */
-		{
-
-			u16 length=p->length;
-			struct sk_buff *skb;
-			struct sk_buff *newskb;
-
-			/* Try to save time by avoiding a copy on big frames */
-
-			if ((length > RX_COPYBREAK) &&
-			    ((newskb = netdev_alloc_skb(dev, 1532)) != NULL))
-			{
-				skb=lp->rx_ring[rx_ring_tail].skb;
-				skb_put(skb, length);
-
-				skb_reserve(newskb,18);
-				lp->rx_ring[rx_ring_tail].skb=newskb;
-				p->data=isa_virt_to_bus(newskb->data);
-			}
-			else
-			{
-				skb = netdev_alloc_skb(dev, length + 2);
-
-				if(skb==NULL) {
-					dev->stats.rx_dropped++;
-					goto dropped;
-				}
-
-				skb_reserve(skb,2);
-				memcpy(skb_put(skb, length),
-				       lp->rx_ring[rx_ring_tail].skb->data, length);
-			}
-
-			skb->protocol=eth_type_trans(skb,dev);
- 			dev->stats.rx_packets++;
- 			dev->stats.rx_bytes += length;
-			netif_rx(skb);
-		}
-
-	dropped:
-		p->length = 1532;
-		p->status = 0;
-
-		rx_ring_tail=next_rx(rx_ring_tail);
-	}
-        while(x++<48);
-
-	/* If there was actually a frame to be processed, place the EOL bit */
-	/* at the descriptor prior to the one to be filled next */
-
-	if (rx_ring_tail != rx_old_tail)
-	{
-		lp->rx_ring[prev_rx(rx_ring_tail)].p->control |=  CONTROL_EOL;
-		lp->rx_ring[prev_rx(rx_old_tail)].p->control  &= ~CONTROL_EOL;
-
-		lp->rx_ring_tail=rx_ring_tail;
-	}
-}
-
-
-/**
- *	mc32_tx_ring	-	process completed transmits
- *	@dev: 3c527 that needs its transmit ring processing
- *
- *
- *	This operates in a similar fashion to mc32_rx_ring. We iterate
- *	over the transmit ring. For each descriptor which has been
- *	processed by the card, we free its associated buffer and note
- *	any errors. This continues until the transmit ring is emptied
- *	or we reach a descriptor that hasn't yet been processed by the
- *	card.
- *
- */
-
-static void mc32_tx_ring(struct net_device *dev)
-{
-	struct mc32_local *lp = netdev_priv(dev);
-	volatile struct skb_header *np;
-
-	/*
-	 * We rely on head==tail to mean 'queue empty'.
-	 * This is why lp->tx_count=TX_RING_LEN-1: in order to prevent
-	 * tx_ring_head wrapping to tail and confusing a 'queue empty'
-	 * condition with 'queue full'
-	 */
-
-	while (lp->tx_ring_tail != atomic_read(&lp->tx_ring_head))
-	{
-		u16 t;
-
-		t=next_tx(lp->tx_ring_tail);
-		np=lp->tx_ring[t].p;
-
-		if(!(np->status & (1<<7)))
-		{
-			/* Not COMPLETED */
-			break;
-		}
-		dev->stats.tx_packets++;
-		if(!(np->status & (1<<6))) /* Not COMPLETED_OK */
-		{
-			dev->stats.tx_errors++;
-
-			switch(np->status&0x0F)
-			{
-				case 1:
-					dev->stats.tx_aborted_errors++;
-					break; /* Max collisions */
-				case 2:
-					dev->stats.tx_fifo_errors++;
-					break;
-				case 3:
-					dev->stats.tx_carrier_errors++;
-					break;
-				case 4:
-					dev->stats.tx_window_errors++;
-					break;  /* CTS Lost */
-				case 5:
-					dev->stats.tx_aborted_errors++;
-					break; /* Transmit timeout */
-			}
-		}
-		/* Packets are sent in order - this is
-		    basically a FIFO queue of buffers matching
-		    the card ring */
-		dev->stats.tx_bytes+=lp->tx_ring[t].skb->len;
-		dev_kfree_skb_irq(lp->tx_ring[t].skb);
-		lp->tx_ring[t].skb=NULL;
-		atomic_inc(&lp->tx_count);
-		netif_wake_queue(dev);
-
-		lp->tx_ring_tail=t;
-	}
-
-}
-
-
-/**
- *	mc32_interrupt		-	handle an interrupt from a 3c527
- *	@irq: Interrupt number
- *	@dev_id: 3c527 that requires servicing
- *	@regs: Registers (unused)
- *
- *
- *	An interrupt is raised whenever the 3c527 writes to the command
- *	register. This register contains the message it wishes to send us
- *	packed into a single byte field. We keep reading status entries
- *	until we have processed all the control items, but simply count
- *	transmit and receive reports. When all reports are in we empty the
- *	transceiver rings as appropriate. This saves the overhead of
- *	multiple command requests.
- *
- *	Because MCA is level-triggered, we shouldn't miss indications.
- *	Therefore, we needn't ask the card to suspend interrupts within
- *	this handler. The card receives an implicit acknowledgment of the
- *	current interrupt when we read the command register.
- *
- */
-
-static irqreturn_t mc32_interrupt(int irq, void *dev_id)
-{
-	struct net_device *dev = dev_id;
-	struct mc32_local *lp;
-	int ioaddr, status, boguscount = 0;
-	int rx_event = 0;
-	int tx_event = 0;
-
-	ioaddr = dev->base_addr;
-	lp = netdev_priv(dev);
-
-	/* See whats cooking */
-
-	while((inb(ioaddr+HOST_STATUS)&HOST_STATUS_CWR) && boguscount++<2000)
-	{
-		status=inb(ioaddr+HOST_CMD);
-
-		pr_debug("Status TX%d RX%d EX%d OV%d BC%d\n",
-			(status&7), (status>>3)&7, (status>>6)&1,
-			(status>>7)&1, boguscount);
-
-		switch(status&7)
-		{
-			case 0:
-				break;
-			case 6: /* TX fail */
-			case 2:	/* TX ok */
-				tx_event = 1;
-				break;
-			case 3: /* Halt */
-			case 4: /* Abort */
-				complete(&lp->xceiver_cmd);
-				break;
-			default:
-				pr_notice("%s: strange tx ack %d\n", dev->name, status&7);
-		}
-		status>>=3;
-		switch(status&7)
-		{
-			case 0:
-				break;
-			case 2:	/* RX */
-				rx_event=1;
-				break;
-			case 3: /* Halt */
-			case 4: /* Abort */
-				complete(&lp->xceiver_cmd);
-				break;
-			case 6:
-				/* Out of RX buffers stat */
-				/* Must restart rx */
-				dev->stats.rx_dropped++;
-				mc32_rx_ring(dev);
-				mc32_start_transceiver(dev);
-				break;
-			default:
-				pr_notice("%s: strange rx ack %d\n",
-					dev->name, status&7);
-		}
-		status>>=3;
-		if(status&1)
-		{
-			/*
-			 * No thread is waiting: we need to tidy
-			 * up ourself.
-			 */
-
-			if (lp->cmd_nonblocking) {
-				up(&lp->cmd_mutex);
-				if (lp->mc_reload_wait)
-					mc32_reset_multicast_list(dev);
-			}
-			else complete(&lp->execution_cmd);
-		}
-		if(status&2)
-		{
-			/*
-			 *	We get interrupted once per
-			 *	counter that is about to overflow.
-			 */
-
-			mc32_update_stats(dev);
-		}
-	}
-
-
-	/*
-	 *	Process the transmit and receive rings
-         */
-
-	if(tx_event)
-		mc32_tx_ring(dev);
-
-	if(rx_event)
-		mc32_rx_ring(dev);
-
-	return IRQ_HANDLED;
-}
-
-
-/**
- *	mc32_close	-	user configuring the 3c527 down
- *	@dev: 3c527 card to shut down
- *
- *	The 3c527 is a bus mastering device. We must be careful how we
- *	shut it down. It may also be running shared interrupt so we have
- *	to be sure to silence it properly
- *
- *	We indicate that the card is closing to the rest of the
- *	driver.  Otherwise, it is possible that the card may run out
- *	of receive buffers and restart the transceiver while we're
- *	trying to close it.
- *
- *	We abort any receive and transmits going on and then wait until
- *	any pending exec commands have completed in other code threads.
- *	In theory we can't get here while that is true, in practice I am
- *	paranoid
- *
- *	We turn off the interrupt enable for the board to be sure it can't
- *	intefere with other devices.
- */
-
-static int mc32_close(struct net_device *dev)
-{
-	struct mc32_local *lp = netdev_priv(dev);
-	int ioaddr = dev->base_addr;
-
-	u8 regs;
-	u16 one=1;
-
-	lp->xceiver_desired_state = HALTED;
-	netif_stop_queue(dev);
-
-	/*
-	 *	Send the indications on command (handy debug check)
-	 */
-
-	mc32_command(dev, 4, &one, 2);
-
-	/* Shut down the transceiver */
-
-	mc32_halt_transceiver(dev);
-
-	/* Ensure we issue no more commands beyond this point */
-
-	down(&lp->cmd_mutex);
-
-	/* Ok the card is now stopping */
-
-	regs=inb(ioaddr+HOST_CTRL);
-	regs&=~HOST_CTRL_INTE;
-	outb(regs, ioaddr+HOST_CTRL);
-
-	mc32_flush_rx_ring(dev);
-	mc32_flush_tx_ring(dev);
-
-	mc32_update_stats(dev);
-
-	return 0;
-}
-
-
-/**
- *	mc32_get_stats		-	hand back stats to network layer
- *	@dev: The 3c527 card to handle
- *
- *	We've collected all the stats we can in software already. Now
- *	it's time to update those kept on-card and return the lot.
- *
- */
-
-static struct net_device_stats *mc32_get_stats(struct net_device *dev)
-{
-	mc32_update_stats(dev);
-	return &dev->stats;
-}
-
-
-/**
- *	do_mc32_set_multicast_list	-	attempt to update multicasts
- *	@dev: 3c527 device to load the list on
- *	@retry: indicates this is not the first call.
- *
- *
- * 	Actually set or clear the multicast filter for this adaptor. The
- *	locking issues are handled by this routine. We have to track
- *	state as it may take multiple calls to get the command sequence
- *	completed. We just keep trying to schedule the loads until we
- *	manage to process them all.
- *
- *	num_addrs == -1	Promiscuous mode, receive all packets
- *
- *	num_addrs == 0	Normal mode, clear multicast list
- *
- *	num_addrs > 0	Multicast mode, receive normal and MC packets,
- *			and do best-effort filtering.
- *
- *	See mc32_update_stats() regards setting the SAV BP bit.
- *
- */
-
-static void do_mc32_set_multicast_list(struct net_device *dev, int retry)
-{
-	struct mc32_local *lp = netdev_priv(dev);
-	u16 filt = (1<<2); /* Save Bad Packets, for stats purposes */
-
-	if ((dev->flags&IFF_PROMISC) ||
-	    (dev->flags&IFF_ALLMULTI) ||
-	    netdev_mc_count(dev) > 10)
-		/* Enable promiscuous mode */
-		filt |= 1;
-	else if (!netdev_mc_empty(dev))
-	{
-		unsigned char block[62];
-		unsigned char *bp;
-		struct netdev_hw_addr *ha;
-
-		if(retry==0)
-			lp->mc_list_valid = 0;
-		if(!lp->mc_list_valid)
-		{
-			block[1]=0;
-			block[0]=netdev_mc_count(dev);
-			bp=block+2;
-
-			netdev_for_each_mc_addr(ha, dev) {
-				memcpy(bp, ha->addr, 6);
-				bp+=6;
-			}
-			if(mc32_command_nowait(dev, 2, block,
-					       2+6*netdev_mc_count(dev))==-1)
-			{
-				lp->mc_reload_wait = 1;
-				return;
-			}
-			lp->mc_list_valid=1;
-		}
-	}
-
-	if(mc32_command_nowait(dev, 0, &filt, 2)==-1)
-	{
-		lp->mc_reload_wait = 1;
-	}
-	else {
-		lp->mc_reload_wait = 0;
-	}
-}
-
-
-/**
- *	mc32_set_multicast_list	-	queue multicast list update
- *	@dev: The 3c527 to use
- *
- *	Commence loading the multicast list. This is called when the kernel
- *	changes the lists. It will override any pending list we are trying to
- *	load.
- */
-
-static void mc32_set_multicast_list(struct net_device *dev)
-{
-	do_mc32_set_multicast_list(dev,0);
-}
-
-
-/**
- *	mc32_reset_multicast_list	-	reset multicast list
- *	@dev: The 3c527 to use
- *
- *	Attempt the next step in loading the multicast lists. If this attempt
- *	fails to complete then it will be scheduled and this function called
- *	again later from elsewhere.
- */
-
-static void mc32_reset_multicast_list(struct net_device *dev)
-{
-	do_mc32_set_multicast_list(dev,1);
-}
-
-static void netdev_get_drvinfo(struct net_device *dev,
-			       struct ethtool_drvinfo *info)
-{
-	strcpy(info->driver, DRV_NAME);
-	strcpy(info->version, DRV_VERSION);
-	sprintf(info->bus_info, "MCA 0x%lx", dev->base_addr);
-}
-
-static u32 netdev_get_msglevel(struct net_device *dev)
-{
-	return mc32_debug;
-}
-
-static void netdev_set_msglevel(struct net_device *dev, u32 level)
-{
-	mc32_debug = level;
-}
-
-static const struct ethtool_ops netdev_ethtool_ops = {
-	.get_drvinfo		= netdev_get_drvinfo,
-	.get_msglevel		= netdev_get_msglevel,
-	.set_msglevel		= netdev_set_msglevel,
-};
-
-#ifdef MODULE
-
-static struct net_device *this_device;
-
-/**
- *	init_module		-	entry point
- *
- *	Probe and locate a 3c527 card. This really should probe and locate
- *	all the 3c527 cards in the machine not just one of them. Yes you can
- *	insmod multiple modules for now but it's a hack.
- */
-
-int __init init_module(void)
-{
-	this_device = mc32_probe(-1);
-	if (IS_ERR(this_device))
-		return PTR_ERR(this_device);
-	return 0;
-}
-
-/**
- *	cleanup_module	-	free resources for an unload
- *
- *	Unloading time. We release the MCA bus resources and the interrupt
- *	at which point everything is ready to unload. The card must be stopped
- *	at this point or we would not have been called. When we unload we
- *	leave the card stopped but not totally shut down. When the card is
- *	initialized it must be rebooted or the rings reloaded before any
- *	transmit operations are allowed to start scribbling into memory.
- */
-
-void __exit cleanup_module(void)
-{
-	unregister_netdev(this_device);
-	cleanup_card(this_device);
-	free_netdev(this_device);
-}
-
-#endif /* MODULE */
diff --git a/drivers/net/ethernet/i825xx/3c527.h b/drivers/net/ethernet/i825xx/3c527.h
deleted file mode 100644
index d693b8d15cd..00000000000
--- a/drivers/net/ethernet/i825xx/3c527.h
+++ /dev/null
@@ -1,81 +0,0 @@
-/*
- *	3COM "EtherLink MC/32" Descriptions
- */
-
-/*
- *	Registers
- */
-
-#define HOST_CMD		0
-#define         HOST_CMD_START_RX   (1<<3)
-#define         HOST_CMD_SUSPND_RX  (3<<3)
-#define         HOST_CMD_RESTRT_RX  (5<<3)
-
-#define         HOST_CMD_SUSPND_TX  3
-#define         HOST_CMD_RESTRT_TX  5
-
-
-#define HOST_STATUS		2
-#define		HOST_STATUS_CRR	(1<<6)
-#define		HOST_STATUS_CWR	(1<<5)
-
-
-#define HOST_CTRL		6
-#define		HOST_CTRL_ATTN	(1<<7)
-#define 	HOST_CTRL_RESET	(1<<6)
-#define 	HOST_CTRL_INTE	(1<<2)
-
-#define HOST_RAMPAGE		8
-
-#define HALTED 0
-#define RUNNING 1
-
-struct mc32_mailbox
-{
- 	u16 mbox;
- 	u16 data[1];
-} __packed;
-
-struct skb_header
-{
-	u8 status;
-	u8 control;
-	u16 next;	/* Do not change! */
-	u16 length;
-	u32 data;
-} __packed;
-
-struct mc32_stats
-{
-	/* RX Errors */
-	u32 rx_crc_errors;
-	u32 rx_alignment_errors;
-	u32 rx_overrun_errors;
-	u32 rx_tooshort_errors;
-	u32 rx_toolong_errors;
-	u32 rx_outofresource_errors;
-
-	u32 rx_discarded;  /* via card pattern match filter */
-
-	/* TX Errors */
-	u32 tx_max_collisions;
-	u32 tx_carrier_errors;
-	u32 tx_underrun_errors;
-	u32 tx_cts_errors;
-	u32 tx_timeout_errors;
-
-	/* various cruft */
-	u32 dataA[6];
-	u16 dataB[5];
-	u32 dataC[14];
-} __packed;
-
-#define STATUS_MASK	0x0F
-#define COMPLETED	(1<<7)
-#define COMPLETED_OK	(1<<6)
-#define BUFFER_BUSY	(1<<5)
-
-#define CONTROL_EOP	(1<<7)	/* End Of Packet */
-#define CONTROL_EOL	(1<<6)	/* End of List */
-
-#define MCA_MC32_ID	0x0041	/* Our MCA ident */
diff --git a/drivers/net/ethernet/i825xx/Kconfig b/drivers/net/ethernet/i825xx/Kconfig
index ca1ae985c6d..fed5080a6b6 100644
--- a/drivers/net/ethernet/i825xx/Kconfig
+++ b/drivers/net/ethernet/i825xx/Kconfig
@@ -43,28 +43,6 @@ config EL16
 	  To compile this driver as a module, choose M here. The module
 	  will be called 3c507.
 
-config ELMC
-	tristate "3c523 \"EtherLink/MC\" support"
-	depends on MCA_LEGACY
-	---help---
-	  If you have a network (Ethernet) card of this type, say Y and read
-	  the Ethernet-HOWTO, available from
-	  <http://www.tldp.org/docs.html#howto>.
-
-	  To compile this driver as a module, choose M here. The module
-	  will be called 3c523.
-
-config ELMC_II
-	tristate "3c527 \"EtherLink/MC 32\" support (EXPERIMENTAL)"
-	depends on MCA && MCA_LEGACY
-	---help---
-	  If you have a network (Ethernet) card of this type, say Y and read
-	  the Ethernet-HOWTO, available from
-	  <http://www.tldp.org/docs.html#howto>.
-
-	  To compile this driver as a module, choose M here. The module
-	  will be called 3c527.
-
 config ARM_ETHER1
 	tristate "Acorn Ether1 support"
 	depends on ARM && ARCH_ACORN
diff --git a/drivers/net/ethernet/i825xx/Makefile b/drivers/net/ethernet/i825xx/Makefile
index f68a3694968..6adff85e8ec 100644
--- a/drivers/net/ethernet/i825xx/Makefile
+++ b/drivers/net/ethernet/i825xx/Makefile
@@ -7,8 +7,6 @@ obj-$(CONFIG_EEXPRESS) += eexpress.o
 obj-$(CONFIG_EEXPRESS_PRO) += eepro.o
 obj-$(CONFIG_ELPLUS) += 3c505.o
 obj-$(CONFIG_EL16) += 3c507.o
-obj-$(CONFIG_ELMC) += 3c523.o
-obj-$(CONFIG_ELMC_II) += 3c527.o
 obj-$(CONFIG_LP486E) += lp486e.o
 obj-$(CONFIG_NI52) += ni52.o
 obj-$(CONFIG_SUN3_82586) += sun3_82586.o
diff --git a/drivers/net/ethernet/i825xx/eexpress.c b/drivers/net/ethernet/i825xx/eexpress.c
index cc2e66ad443..7a6a2f04c5b 100644
--- a/drivers/net/ethernet/i825xx/eexpress.c
+++ b/drivers/net/ethernet/i825xx/eexpress.c
@@ -9,7 +9,7 @@
  * Many modifications, and currently maintained, by
  *  Philip Blundell <philb@gnu.org>
  * Added the Compaq LTE  Alan Cox <alan@lxorguk.ukuu.org.uk>
- * Added MCA support Adam Fritzler
+ * Added MCA support Adam Fritzler (now deleted)
  *
  * Note - this driver is experimental still - it has problems on faster
  * machines. Someone needs to sit down and go through it line by line with
@@ -111,7 +111,6 @@
 #include <linux/netdevice.h>
 #include <linux/etherdevice.h>
 #include <linux/skbuff.h>
-#include <linux/mca-legacy.h>
 #include <linux/spinlock.h>
 #include <linux/bitops.h>
 #include <linux/jiffies.h>
@@ -227,16 +226,6 @@ static unsigned short start_code[] = {
 /* maps irq number to EtherExpress magic value */
 static char irqrmap[] = { 0,0,1,2,3,4,0,0,0,1,5,6,0,0,0,0 };
 
-#ifdef CONFIG_MCA_LEGACY
-/* mapping of the first four bits of the second POS register */
-static unsigned short mca_iomap[] = {
-	0x270, 0x260, 0x250, 0x240, 0x230, 0x220, 0x210, 0x200,
-	0x370, 0x360, 0x350, 0x340, 0x330, 0x320, 0x310, 0x300
-};
-/* bits 5-7 of the second POS register */
-static char mca_irqmap[] = { 12, 9, 3, 4, 5, 10, 11, 15 };
-#endif
-
 /*
  * Prototypes for Linux interface
  */
@@ -340,53 +329,6 @@ static int __init do_express_probe(struct net_device *dev)
 
 	dev->if_port = 0xff; /* not set */
 
-#ifdef CONFIG_MCA_LEGACY
-	if (MCA_bus) {
-		int slot = 0;
-
-		/*
-		 * Only find one card at a time.  Subsequent calls
-		 * will find others, however, proper multicard MCA
-		 * probing and setup can't be done with the
-		 * old-style Space.c init routines.  -- ASF
-		 */
-		while (slot != MCA_NOTFOUND) {
-			int pos0, pos1;
-
-			slot = mca_find_unused_adapter(0x628B, slot);
-			if (slot == MCA_NOTFOUND)
-				break;
-
-			pos0 = mca_read_stored_pos(slot, 2);
-			pos1 = mca_read_stored_pos(slot, 3);
-			ioaddr = mca_iomap[pos1&0xf];
-
-			dev->irq = mca_irqmap[(pos1>>4)&0x7];
-
-			/*
-			 * XXX: Transceiver selection is done
-			 * differently on the MCA version.
-			 * How to get it to select something
-			 * other than external/AUI is currently
-			 * unknown.  This code is just for looks. -- ASF
-			 */
-			if ((pos0 & 0x7) == 0x1)
-				dev->if_port = AUI;
-			else if ((pos0 & 0x7) == 0x5) {
-				if (pos1 & 0x80)
-					dev->if_port = BNC;
-				else
-					dev->if_port = TPE;
-			}
-
-			mca_set_adapter_name(slot, "Intel EtherExpress 16 MCA");
-			mca_set_adapter_procfn(slot, NULL, dev);
-			mca_mark_as_used(slot);
-
-			break;
-		}
-	}
-#endif
 	if (ioaddr&0xfe00) {
 		if (!request_region(ioaddr, EEXP_IO_EXTENT, "EtherExpress"))
 			return -EBUSY;
diff --git a/drivers/net/ethernet/ibm/ehea/ehea_main.c b/drivers/net/ethernet/ibm/ehea/ehea_main.c
index c9069a28832..f4d2da0db1b 100644
--- a/drivers/net/ethernet/ibm/ehea/ehea_main.c
+++ b/drivers/net/ethernet/ibm/ehea/ehea_main.c
@@ -3335,6 +3335,8 @@ static int __devinit ehea_probe_adapter(struct platform_device *dev,
 		goto out_shutdown_ports;
 	}
 
+	/* Handle any events that might be pending. */
+	tasklet_hi_schedule(&adapter->neq_tasklet);
 
 	ret = 0;
 	goto out;
diff --git a/drivers/net/ethernet/intel/Kconfig b/drivers/net/ethernet/intel/Kconfig
index 76213162fbe..79b07ec6726 100644
--- a/drivers/net/ethernet/intel/Kconfig
+++ b/drivers/net/ethernet/intel/Kconfig
@@ -7,7 +7,7 @@ config NET_VENDOR_INTEL
 	default y
 	depends on PCI || PCI_MSI || ISA || ISA_DMA_API || ARM || \
 		   ARCH_ACORN || MCA || MCA_LEGACY || SNI_RM || SUN3 || \
-		   GSC || BVME6000 || MVME16x || ARCH_ENP2611 || \
+		   GSC || BVME6000 || MVME16x || \
 		   (ARM && ARCH_IXP4XX && IXP4XX_NPE && IXP4XX_QMGR) || \
 		   EXPERIMENTAL
 	---help---
@@ -120,6 +120,17 @@ config IGB_DCA
 	  driver.  DCA is a method for warming the CPU cache before data
 	  is used, with the intent of lessening the impact of cache misses.
 
+config IGB_PTP
+	bool "PTP Hardware Clock (PHC)"
+	default y
+	depends on IGB && PTP_1588_CLOCK
+	---help---
+	  Say Y here if you want to use PTP Hardware Clock (PHC) in the
+	  driver.  Only the basic clock operations have been implemented.
+
+	  Every timestamp and clock read operations must consult the
+	  overflow counter to form a correct time value.
+
 config IGBVF
 	tristate "Intel(R) 82576 Virtual Function Ethernet support"
 	depends on PCI
@@ -182,6 +193,14 @@ config IXGBE
 	  To compile this driver as a module, choose M here. The module
 	  will be called ixgbe.
 
+config IXGBE_HWMON
+	bool "Intel(R) 10GbE PCI Express adapters HWMON support"
+	default y
+	depends on IXGBE && HWMON && !(IXGBE=y && HWMON=m)
+	---help---
+	  Say Y if you want to expose the thermal sensor data on some of
+	  our cards, via a hwmon sysfs interface.
+
 config IXGBE_DCA
 	bool "Direct Cache Access (DCA) Support"
 	default y
@@ -201,6 +220,17 @@ config IXGBE_DCB
 
 	  If unsure, say N.
 
+config IXGBE_PTP
+	bool "PTP Clock Support"
+	default n
+	depends on IXGBE && PTP_1588_CLOCK
+	---help---
+	  Say Y here if you want support for 1588 Timestamping with a
+	  PHC device, using the PTP 1588 Clock support. This is
+	  required to enable timestamping support for the device.
+
+	  If unsure, say N.
+
 config IXGBEVF
 	tristate "Intel(R) 82599 Virtual Function Ethernet support"
 	depends on PCI_MSI
diff --git a/drivers/net/ethernet/intel/e100.c b/drivers/net/ethernet/intel/e100.c
index e498effb85d..ada720b42ff 100644
--- a/drivers/net/ethernet/intel/e100.c
+++ b/drivers/net/ethernet/intel/e100.c
@@ -1759,6 +1759,7 @@ static void e100_xmit_prepare(struct nic *nic, struct cb *cb,
 		skb->data, skb->len, PCI_DMA_TODEVICE));
 	/* check for mapping failure? */
 	cb->u.tcb.tbd.size = cpu_to_le16(skb->len);
+	skb_tx_timestamp(skb);
 }
 
 static netdev_tx_t e100_xmit_frame(struct sk_buff *skb,
@@ -2733,6 +2734,7 @@ static const struct ethtool_ops e100_ethtool_ops = {
 	.set_phys_id		= e100_set_phys_id,
 	.get_ethtool_stats	= e100_get_ethtool_stats,
 	.get_sset_count		= e100_get_sset_count,
+	.get_ts_info		= ethtool_op_get_ts_info,
 };
 
 static int e100_do_ioctl(struct net_device *netdev, struct ifreq *ifr, int cmd)
diff --git a/drivers/net/ethernet/intel/e1000/e1000_main.c b/drivers/net/ethernet/intel/e1000/e1000_main.c
index 4348b6fd44f..95731c84104 100644
--- a/drivers/net/ethernet/intel/e1000/e1000_main.c
+++ b/drivers/net/ethernet/intel/e1000/e1000_main.c
@@ -493,7 +493,11 @@ out:
 static void e1000_down_and_stop(struct e1000_adapter *adapter)
 {
 	set_bit(__E1000_DOWN, &adapter->flags);
-	cancel_work_sync(&adapter->reset_task);
+
+	/* Only kill reset task if adapter is not resetting */
+	if (!test_bit(__E1000_RESETTING, &adapter->flags))
+		cancel_work_sync(&adapter->reset_task);
+
 	cancel_delayed_work_sync(&adapter->watchdog_task);
 	cancel_delayed_work_sync(&adapter->phy_info_task);
 	cancel_delayed_work_sync(&adapter->fifo_stall_task);
@@ -827,9 +831,10 @@ static int e1000_set_features(struct net_device *netdev,
 	if (changed & NETIF_F_HW_VLAN_RX)
 		e1000_vlan_mode(netdev, features);
 
-	if (!(changed & NETIF_F_RXCSUM))
+	if (!(changed & (NETIF_F_RXCSUM | NETIF_F_RXALL)))
 		return 0;
 
+	netdev->features = features;
 	adapter->rx_csum = !!(features & NETIF_F_RXCSUM);
 
 	if (netif_running(netdev))
@@ -1074,6 +1079,7 @@ static int __devinit e1000_probe(struct pci_dev *pdev,
 
 	netdev->features |= netdev->hw_features;
 	netdev->hw_features |= NETIF_F_RXCSUM;
+	netdev->hw_features |= NETIF_F_RXALL;
 	netdev->hw_features |= NETIF_F_RXFCS;
 
 	if (pci_using_dac) {
@@ -1841,6 +1847,22 @@ static void e1000_setup_rctl(struct e1000_adapter *adapter)
 			break;
 	}
 
+	/* 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 */
+		rctl |= (E1000_RCTL_SBP | /* Receive bad packets */
+			 E1000_RCTL_BAM | /* RX All Bcast Pkts */
+			 E1000_RCTL_PMCF); /* RX All MAC Ctrl Pkts */
+
+		rctl &= ~(E1000_RCTL_VFE | /* Disable VLAN filter */
+			  E1000_RCTL_DPF | /* Allow filtered pause */
+			  E1000_RCTL_CFIEN); /* Dis VLAN CFIEN Filter */
+		/* Do not mess with E1000_CTRL_VME, it affects transmit as well,
+		 * and that breaks VLANs.
+		 */
+	}
+
 	ew32(RCTL, rctl);
 }
 
@@ -3243,6 +3265,8 @@ static netdev_tx_t e1000_xmit_frame(struct sk_buff *skb,
 	                     nr_frags, mss);
 
 	if (count) {
+		skb_tx_timestamp(skb);
+
 		e1000_tx_queue(adapter, tx_ring, tx_flags, count);
 		/* Make sure there is space in the ring for the next send. */
 		e1000_maybe_stop_tx(netdev, tx_ring, MAX_SKB_FRAGS + 2);
@@ -3380,7 +3404,7 @@ static void e1000_dump(struct e1000_adapter *adapter)
 	for (i = 0; tx_ring->desc && (i < tx_ring->count); i++) {
 		struct e1000_tx_desc *tx_desc = E1000_TX_DESC(*tx_ring, i);
 		struct e1000_buffer *buffer_info = &tx_ring->buffer_info[i];
-		struct my_u { u64 a; u64 b; };
+		struct my_u { __le64 a; __le64 b; };
 		struct my_u *u = (struct my_u *)tx_desc;
 		const char *type;
 
@@ -3424,7 +3448,7 @@ rx_ring_summary:
 	for (i = 0; rx_ring->desc && (i < rx_ring->count); i++) {
 		struct e1000_rx_desc *rx_desc = E1000_RX_DESC(*rx_ring, i);
 		struct e1000_buffer *buffer_info = &rx_ring->buffer_info[i];
-		struct my_u { u64 a; u64 b; };
+		struct my_u { __le64 a; __le64 b; };
 		struct my_u *u = (struct my_u *)rx_desc;
 		const char *type;
 
@@ -4046,7 +4070,11 @@ static bool e1000_clean_jumbo_rx_irq(struct e1000_adapter *adapter,
 		/* errors is only valid for DD + EOP descriptors */
 		if (unlikely((status & E1000_RXD_STAT_EOP) &&
 		    (rx_desc->errors & E1000_RXD_ERR_FRAME_ERR_MASK))) {
-			u8 last_byte = *(skb->data + length - 1);
+			u8 *mapped;
+			u8 last_byte;
+
+			mapped = page_address(buffer_info->page);
+			last_byte = *(mapped + length - 1);
 			if (TBI_ACCEPT(hw, status, rx_desc->errors, length,
 				       last_byte)) {
 				spin_lock_irqsave(&adapter->stats_lock,
@@ -4057,6 +4085,8 @@ static bool e1000_clean_jumbo_rx_irq(struct e1000_adapter *adapter,
 				                       irq_flags);
 				length--;
 			} else {
+				if (netdev->features & NETIF_F_RXALL)
+					goto process_skb;
 				/* recycle both page and skb */
 				buffer_info->skb = skb;
 				/* an error means any chain goes out the window
@@ -4069,6 +4099,7 @@ static bool e1000_clean_jumbo_rx_irq(struct e1000_adapter *adapter,
 		}
 
 #define rxtop rx_ring->rx_skb_top
+process_skb:
 		if (!(status & E1000_RXD_STAT_EOP)) {
 			/* this descriptor is only the beginning (or middle) */
 			if (!rxtop) {
@@ -4276,12 +4307,15 @@ static bool e1000_clean_rx_irq(struct e1000_adapter *adapter,
 				                       flags);
 				length--;
 			} else {
+				if (netdev->features & NETIF_F_RXALL)
+					goto process_skb;
 				/* recycle */
 				buffer_info->skb = skb;
 				goto next_desc;
 			}
 		}
 
+process_skb:
 		total_rx_bytes += (length - 4); /* don't count FCS */
 		total_rx_packets++;
 
@@ -4365,30 +4399,6 @@ e1000_alloc_jumbo_rx_buffers(struct e1000_adapter *adapter,
 			break;
 		}
 
-		/* Fix for errata 23, can't cross 64kB boundary */
-		if (!e1000_check_64k_bound(adapter, skb->data, bufsz)) {
-			struct sk_buff *oldskb = skb;
-			e_err(rx_err, "skb align check failed: %u bytes at "
-			      "%p\n", bufsz, skb->data);
-			/* Try again, without freeing the previous */
-			skb = netdev_alloc_skb_ip_align(netdev, bufsz);
-			/* Failed allocation, critical failure */
-			if (!skb) {
-				dev_kfree_skb(oldskb);
-				adapter->alloc_rx_buff_failed++;
-				break;
-			}
-
-			if (!e1000_check_64k_bound(adapter, skb->data, bufsz)) {
-				/* give up */
-				dev_kfree_skb(skb);
-				dev_kfree_skb(oldskb);
-				break; /* while (cleaned_count--) */
-			}
-
-			/* Use new allocation */
-			dev_kfree_skb(oldskb);
-		}
 		buffer_info->skb = skb;
 		buffer_info->length = adapter->rx_buffer_len;
 check_page:
diff --git a/drivers/net/ethernet/intel/e1000e/80003es2lan.c b/drivers/net/ethernet/intel/e1000e/80003es2lan.c
index bac9dda31b6..4dd18a1f45d 100644
--- a/drivers/net/ethernet/intel/e1000e/80003es2lan.c
+++ b/drivers/net/ethernet/intel/e1000e/80003es2lan.c
@@ -228,9 +228,7 @@ static s32 e1000_init_mac_params_80003es2lan(struct e1000_hw *hw)
 	/* FWSM register */
 	mac->has_fwsm = true;
 	/* ARC supported; valid only if manageability features are enabled. */
-	mac->arc_subsystem_valid =
-	        (er32(FWSM) & E1000_FWSM_MODE_MASK)
-	                ? true : false;
+	mac->arc_subsystem_valid = !!(er32(FWSM) & E1000_FWSM_MODE_MASK);
 	/* Adaptive IFS not supported */
 	mac->adaptive_ifs = false;
 
@@ -766,6 +764,7 @@ static s32 e1000_reset_hw_80003es2lan(struct e1000_hw *hw)
 {
 	u32 ctrl;
 	s32 ret_val;
+	u16 kum_reg_data;
 
 	/*
 	 * Prevent the PCI-E bus from sticking if there is no TLP connection
@@ -791,6 +790,13 @@ static s32 e1000_reset_hw_80003es2lan(struct e1000_hw *hw)
 	ew32(CTRL, ctrl | E1000_CTRL_RST);
 	e1000_release_phy_80003es2lan(hw);
 
+	/* Disable IBIST slave mode (far-end loopback) */
+	e1000_read_kmrn_reg_80003es2lan(hw, E1000_KMRNCTRLSTA_INBAND_PARAM,
+					&kum_reg_data);
+	kum_reg_data |= E1000_KMRNCTRLSTA_IBIST_DISABLE;
+	e1000_write_kmrn_reg_80003es2lan(hw, E1000_KMRNCTRLSTA_INBAND_PARAM,
+					 kum_reg_data);
+
 	ret_val = e1000e_get_auto_rd_done(hw);
 	if (ret_val)
 		/* We don't want to continue accessing MAC registers. */
@@ -938,6 +944,14 @@ static void e1000_initialize_hw_bits_80003es2lan(struct e1000_hw *hw)
 	else
 		reg |= (1 << 28);
 	ew32(TARC(1), reg);
+
+	/*
+	 * Disable IPv6 extension header parsing because some malformed
+	 * IPv6 headers can hang the Rx.
+	 */
+	reg = er32(RFCTL);
+	reg |= (E1000_RFCTL_IPV6_EX_DIS | E1000_RFCTL_NEW_IPV6_EXT_DIS);
+	ew32(RFCTL, reg);
 }
 
 /**
@@ -1433,6 +1447,7 @@ static const struct e1000_mac_operations es2_mac_ops = {
 	/* setup_physical_interface dependent on media type */
 	.setup_led		= e1000e_setup_led_generic,
 	.config_collision_dist	= e1000e_config_collision_dist_generic,
+	.rar_set		= e1000e_rar_set_generic,
 };
 
 static const struct e1000_phy_operations es2_phy_ops = {
diff --git a/drivers/net/ethernet/intel/e1000e/82571.c b/drivers/net/ethernet/intel/e1000e/82571.c
index b3fdc6977f2..36db4df09ae 100644
--- a/drivers/net/ethernet/intel/e1000e/82571.c
+++ b/drivers/net/ethernet/intel/e1000e/82571.c
@@ -295,9 +295,8 @@ static s32 e1000_init_mac_params_82571(struct e1000_hw *hw)
 		 * ARC supported; valid only if manageability features are
 		 * enabled.
 		 */
-		mac->arc_subsystem_valid =
-			(er32(FWSM) & E1000_FWSM_MODE_MASK)
-			? true : false;
+		mac->arc_subsystem_valid = !!(er32(FWSM) &
+					      E1000_FWSM_MODE_MASK);
 		break;
 	case e1000_82574:
 	case e1000_82583:
@@ -798,7 +797,7 @@ static s32 e1000_update_nvm_checksum_82571(struct e1000_hw *hw)
 	/* Check for pending operations. */
 	for (i = 0; i < E1000_FLASH_UPDATES; i++) {
 		usleep_range(1000, 2000);
-		if ((er32(EECD) & E1000_EECD_FLUPD) == 0)
+		if (!(er32(EECD) & E1000_EECD_FLUPD))
 			break;
 	}
 
@@ -822,7 +821,7 @@ static s32 e1000_update_nvm_checksum_82571(struct e1000_hw *hw)
 
 	for (i = 0; i < E1000_FLASH_UPDATES; i++) {
 		usleep_range(1000, 2000);
-		if ((er32(EECD) & E1000_EECD_FLUPD) == 0)
+		if (!(er32(EECD) & E1000_EECD_FLUPD))
 			break;
 	}
 
@@ -1000,7 +999,7 @@ static s32 e1000_set_d0_lplu_state_82571(struct e1000_hw *hw, bool active)
  **/
 static s32 e1000_reset_hw_82571(struct e1000_hw *hw)
 {
-	u32 ctrl, ctrl_ext;
+	u32 ctrl, ctrl_ext, eecd;
 	s32 ret_val;
 
 	/*
@@ -1073,6 +1072,16 @@ static s32 e1000_reset_hw_82571(struct e1000_hw *hw)
 	 */
 
 	switch (hw->mac.type) {
+	case e1000_82571:
+	case e1000_82572:
+		/*
+		 * REQ and GNT bits need to be cleared when using AUTO_RD
+		 * to access the EEPROM.
+		 */
+		eecd = er32(EECD);
+		eecd &= ~(E1000_EECD_REQ | E1000_EECD_GNT);
+		ew32(EECD, eecd);
+		break;
 	case e1000_82573:
 	case e1000_82574:
 	case e1000_82583:
@@ -1280,6 +1289,16 @@ static void e1000_initialize_hw_bits_82571(struct e1000_hw *hw)
 		ew32(CTRL_EXT, reg);
 	}
 
+	/*
+	 * Disable IPv6 extension header parsing because some malformed
+	 * IPv6 headers can hang the Rx.
+	 */
+	if (hw->mac.type <= e1000_82573) {
+		reg = er32(RFCTL);
+		reg |= (E1000_RFCTL_IPV6_EX_DIS | E1000_RFCTL_NEW_IPV6_EXT_DIS);
+		ew32(RFCTL, reg);
+	}
+
 	/* PCI-Ex Control Registers */
 	switch (hw->mac.type) {
 	case e1000_82574:
@@ -1763,7 +1782,8 @@ void e1000e_set_laa_state_82571(struct e1000_hw *hw, bool state)
 		 * incoming packets directed to this port are dropped.
 		 * Eventually the LAA will be in RAR[0] and RAR[14].
 		 */
-		e1000e_rar_set(hw, hw->mac.addr, hw->mac.rar_entry_count - 1);
+		hw->mac.ops.rar_set(hw, hw->mac.addr,
+				    hw->mac.rar_entry_count - 1);
 }
 
 /**
@@ -1927,6 +1947,7 @@ static const struct e1000_mac_operations e82571_mac_ops = {
 	.setup_led		= e1000e_setup_led_generic,
 	.config_collision_dist	= e1000e_config_collision_dist_generic,
 	.read_mac_addr		= e1000_read_mac_addr_82571,
+	.rar_set		= e1000e_rar_set_generic,
 };
 
 static const struct e1000_phy_operations e82_phy_ops_igp = {
@@ -2061,9 +2082,11 @@ const struct e1000_info e1000_82574_info = {
 				  | FLAG_HAS_SMART_POWER_DOWN
 				  | FLAG_HAS_AMT
 				  | FLAG_HAS_CTRLEXT_ON_LOAD,
-	.flags2			  = FLAG2_CHECK_PHY_HANG
+	.flags2			 = FLAG2_CHECK_PHY_HANG
 				  | FLAG2_DISABLE_ASPM_L0S
-				  | FLAG2_NO_DISABLE_RX,
+				  | FLAG2_DISABLE_ASPM_L1
+				  | FLAG2_NO_DISABLE_RX
+				  | FLAG2_DMA_BURST,
 	.pba			= 32,
 	.max_hw_frame_size	= DEFAULT_JUMBO,
 	.get_variants		= e1000_get_variants_82571,
diff --git a/drivers/net/ethernet/intel/e1000e/defines.h b/drivers/net/ethernet/intel/e1000e/defines.h
index 3a502591716..351a4097b2b 100644
--- a/drivers/net/ethernet/intel/e1000e/defines.h
+++ b/drivers/net/ethernet/intel/e1000e/defines.h
@@ -74,7 +74,9 @@
 #define E1000_WUS_BC           E1000_WUFC_BC
 
 /* Extended Device Control */
+#define E1000_CTRL_EXT_LPCD  0x00000004     /* LCD Power Cycle Done */
 #define E1000_CTRL_EXT_SDP3_DATA 0x00000080 /* Value of SW Definable Pin 3 */
+#define E1000_CTRL_EXT_FORCE_SMBUS 0x00000800 /* Force SMBus mode */
 #define E1000_CTRL_EXT_EE_RST    0x00002000 /* Reinitialize from EEPROM */
 #define E1000_CTRL_EXT_SPD_BYPS  0x00008000 /* Speed Select Bypass */
 #define E1000_CTRL_EXT_RO_DIS    0x00020000 /* Relaxed Ordering disable */
@@ -573,6 +575,7 @@
 #define NWAY_AR_ASM_DIR          0x0800   /* Asymmetric Pause Direction bit */
 
 /* Link Partner Ability Register (Base Page) */
+#define NWAY_LPAR_100TX_FD_CAPS  0x0100 /* LP 100TX Full Dplx Capable */
 #define NWAY_LPAR_PAUSE          0x0400 /* LP Pause operation desired */
 #define NWAY_LPAR_ASM_DIR        0x0800 /* LP Asymmetric Pause Direction bit */
 
@@ -739,6 +742,7 @@
 #define I82577_E_PHY_ID      0x01540050
 #define I82578_E_PHY_ID      0x004DD040
 #define I82579_E_PHY_ID      0x01540090
+#define I217_E_PHY_ID        0x015400A0
 
 /* M88E1000 Specific Registers */
 #define M88E1000_PHY_SPEC_CTRL     0x10  /* PHY Specific Control Register */
@@ -850,4 +854,8 @@
 /* SerDes Control */
 #define E1000_GEN_POLL_TIMEOUT          640
 
+/* FW Semaphore */
+#define E1000_FWSM_WLOCK_MAC_MASK	0x0380
+#define E1000_FWSM_WLOCK_MAC_SHIFT	7
+
 #endif /* _E1000_DEFINES_H_ */
diff --git a/drivers/net/ethernet/intel/e1000e/e1000.h b/drivers/net/ethernet/intel/e1000e/e1000.h
index b83897f76ee..6e6fffb3458 100644
--- a/drivers/net/ethernet/intel/e1000e/e1000.h
+++ b/drivers/net/ethernet/intel/e1000e/e1000.h
@@ -206,6 +206,7 @@ enum e1000_boards {
 	board_ich10lan,
 	board_pchlan,
 	board_pch2lan,
+	board_pch_lpt,
 };
 
 struct e1000_ps_page {
@@ -528,6 +529,7 @@ extern const struct e1000_info e1000_ich9_info;
 extern const struct e1000_info e1000_ich10_info;
 extern const struct e1000_info e1000_pch_info;
 extern const struct e1000_info e1000_pch2_info;
+extern const struct e1000_info e1000_pch_lpt_info;
 extern const struct e1000_info e1000_es2_info;
 
 extern s32 e1000_read_pba_string_generic(struct e1000_hw *hw, u8 *pba_num,
@@ -576,7 +578,7 @@ extern void e1000e_init_rx_addrs(struct e1000_hw *hw, u16 rar_count);
 extern void e1000e_update_mc_addr_list_generic(struct e1000_hw *hw,
 					       u8 *mc_addr_list,
 					       u32 mc_addr_count);
-extern void e1000e_rar_set(struct e1000_hw *hw, u8 *addr, u32 index);
+extern void e1000e_rar_set_generic(struct e1000_hw *hw, u8 *addr, u32 index);
 extern s32 e1000e_set_fc_watermarks(struct e1000_hw *hw);
 extern void e1000e_set_pcie_no_snoop(struct e1000_hw *hw, u32 no_snoop);
 extern s32 e1000e_get_hw_semaphore(struct e1000_hw *hw);
@@ -673,11 +675,21 @@ static inline s32 e1e_rphy(struct e1000_hw *hw, u32 offset, u16 *data)
 	return hw->phy.ops.read_reg(hw, offset, data);
 }
 
+static inline s32 e1e_rphy_locked(struct e1000_hw *hw, u32 offset, u16 *data)
+{
+	return hw->phy.ops.read_reg_locked(hw, offset, data);
+}
+
 static inline s32 e1e_wphy(struct e1000_hw *hw, u32 offset, u16 data)
 {
 	return hw->phy.ops.write_reg(hw, offset, data);
 }
 
+static inline s32 e1e_wphy_locked(struct e1000_hw *hw, u32 offset, u16 data)
+{
+	return hw->phy.ops.write_reg_locked(hw, offset, data);
+}
+
 static inline s32 e1000_get_cable_length(struct e1000_hw *hw)
 {
 	return hw->phy.ops.get_cable_length(hw);
@@ -735,9 +747,46 @@ static inline u32 __er32(struct e1000_hw *hw, unsigned long reg)
 	return readl(hw->hw_addr + reg);
 }
 
+#define er32(reg)	__er32(hw, E1000_##reg)
+
+/**
+ * __ew32_prepare - prepare to write to MAC CSR register on certain parts
+ * @hw: pointer to the HW structure
+ *
+ * When updating the MAC CSR registers, the Manageability Engine (ME) could
+ * be accessing the registers at the same time.  Normally, this is handled in
+ * h/w by an arbiter but on some parts there is a bug that acknowledges Host
+ * accesses later than it should which could result in the register to have
+ * an incorrect value.  Workaround this by checking the FWSM register which
+ * has bit 24 set while ME is accessing MAC CSR registers, wait if it is set
+ * and try again a number of times.
+ **/
+static inline s32 __ew32_prepare(struct e1000_hw *hw)
+{
+	s32 i = E1000_ICH_FWSM_PCIM2PCI_COUNT;
+
+	while ((er32(FWSM) & E1000_ICH_FWSM_PCIM2PCI) && --i)
+		udelay(50);
+
+	return i;
+}
+
 static inline void __ew32(struct e1000_hw *hw, unsigned long reg, u32 val)
 {
+	if (hw->adapter->flags2 & FLAG2_PCIM2PCI_ARBITER_WA)
+		__ew32_prepare(hw);
+
 	writel(val, hw->hw_addr + reg);
 }
 
+#define ew32(reg, val)	__ew32(hw, E1000_##reg, (val))
+
+#define e1e_flush()	er32(STATUS)
+
+#define E1000_WRITE_REG_ARRAY(a, reg, offset, value) \
+	(__ew32((a), (reg + ((offset) << 2)), (value)))
+
+#define E1000_READ_REG_ARRAY(a, reg, offset) \
+	(readl((a)->hw_addr + reg + ((offset) << 2)))
+
 #endif /* _E1000_H_ */
diff --git a/drivers/net/ethernet/intel/e1000e/ethtool.c b/drivers/net/ethernet/intel/e1000e/ethtool.c
index db35dd5d96d..d863075df7a 100644
--- a/drivers/net/ethernet/intel/e1000e/ethtool.c
+++ b/drivers/net/ethernet/intel/e1000e/ethtool.c
@@ -259,8 +259,7 @@ static int e1000_set_settings(struct net_device *netdev,
 	 * cannot be changed
 	 */
 	if (hw->phy.ops.check_reset_block(hw)) {
-		e_err("Cannot change link characteristics when SoL/IDER is "
-		      "active.\n");
+		e_err("Cannot change link characteristics when SoL/IDER is active.\n");
 		return -EINVAL;
 	}
 
@@ -403,15 +402,15 @@ static void e1000_get_regs(struct net_device *netdev,
 	regs_buff[1]  = er32(STATUS);
 
 	regs_buff[2]  = er32(RCTL);
-	regs_buff[3]  = er32(RDLEN);
-	regs_buff[4]  = er32(RDH);
-	regs_buff[5]  = er32(RDT);
+	regs_buff[3]  = er32(RDLEN(0));
+	regs_buff[4]  = er32(RDH(0));
+	regs_buff[5]  = er32(RDT(0));
 	regs_buff[6]  = er32(RDTR);
 
 	regs_buff[7]  = er32(TCTL);
-	regs_buff[8]  = er32(TDLEN);
-	regs_buff[9]  = er32(TDH);
-	regs_buff[10] = er32(TDT);
+	regs_buff[8]  = er32(TDLEN(0));
+	regs_buff[9]  = er32(TDH(0));
+	regs_buff[10] = er32(TDT(0));
 	regs_buff[11] = er32(TIDV);
 
 	regs_buff[12] = adapter->hw.phy.type;  /* PHY type (IGP=1, M88=0) */
@@ -727,9 +726,8 @@ static bool reg_pattern_test(struct e1000_adapter *adapter, u64 *data,
 				      (test[pat] & write));
 		val = E1000_READ_REG_ARRAY(&adapter->hw, reg, offset);
 		if (val != (test[pat] & write & mask)) {
-			e_err("pattern test reg %04X failed: got 0x%08X "
-			      "expected 0x%08X\n", reg + offset, val,
-			      (test[pat] & write & mask));
+			e_err("pattern test reg %04X failed: got 0x%08X expected 0x%08X\n",
+			      reg + offset, val, (test[pat] & write & mask));
 			*data = reg;
 			return 1;
 		}
@@ -744,8 +742,8 @@ static bool reg_set_and_check(struct e1000_adapter *adapter, u64 *data,
 	__ew32(&adapter->hw, reg, write & mask);
 	val = __er32(&adapter->hw, reg);
 	if ((write & mask) != (val & mask)) {
-		e_err("set/check reg %04X test failed: got 0x%08X "
-		      "expected 0x%08X\n", reg, (val & mask), (write & mask));
+		e_err("set/check reg %04X test failed: got 0x%08X expected 0x%08X\n",
+		      reg, (val & mask), (write & mask));
 		*data = reg;
 		return 1;
 	}
@@ -775,6 +773,7 @@ static int e1000_reg_test(struct e1000_adapter *adapter, u64 *data)
 	u32 i;
 	u32 toggle;
 	u32 mask;
+	u32 wlock_mac = 0;
 
 	/*
 	 * The status register is Read Only, so a write should fail.
@@ -797,8 +796,8 @@ static int e1000_reg_test(struct e1000_adapter *adapter, u64 *data)
 	ew32(STATUS, toggle);
 	after = er32(STATUS) & toggle;
 	if (value != after) {
-		e_err("failed STATUS register test got: 0x%08X expected: "
-		      "0x%08X\n", after, value);
+		e_err("failed STATUS register test got: 0x%08X expected: 0x%08X\n",
+		      after, value);
 		*data = 1;
 		return 1;
 	}
@@ -813,15 +812,15 @@ static int e1000_reg_test(struct e1000_adapter *adapter, u64 *data)
 	}
 
 	REG_PATTERN_TEST(E1000_RDTR, 0x0000FFFF, 0xFFFFFFFF);
-	REG_PATTERN_TEST(E1000_RDBAH, 0xFFFFFFFF, 0xFFFFFFFF);
-	REG_PATTERN_TEST(E1000_RDLEN, 0x000FFF80, 0x000FFFFF);
-	REG_PATTERN_TEST(E1000_RDH, 0x0000FFFF, 0x0000FFFF);
-	REG_PATTERN_TEST(E1000_RDT, 0x0000FFFF, 0x0000FFFF);
+	REG_PATTERN_TEST(E1000_RDBAH(0), 0xFFFFFFFF, 0xFFFFFFFF);
+	REG_PATTERN_TEST(E1000_RDLEN(0), 0x000FFF80, 0x000FFFFF);
+	REG_PATTERN_TEST(E1000_RDH(0), 0x0000FFFF, 0x0000FFFF);
+	REG_PATTERN_TEST(E1000_RDT(0), 0x0000FFFF, 0x0000FFFF);
 	REG_PATTERN_TEST(E1000_FCRTH, 0x0000FFF8, 0x0000FFF8);
 	REG_PATTERN_TEST(E1000_FCTTV, 0x0000FFFF, 0x0000FFFF);
 	REG_PATTERN_TEST(E1000_TIPG, 0x3FFFFFFF, 0x3FFFFFFF);
-	REG_PATTERN_TEST(E1000_TDBAH, 0xFFFFFFFF, 0xFFFFFFFF);
-	REG_PATTERN_TEST(E1000_TDLEN, 0x000FFF80, 0x000FFFFF);
+	REG_PATTERN_TEST(E1000_TDBAH(0), 0xFFFFFFFF, 0xFFFFFFFF);
+	REG_PATTERN_TEST(E1000_TDLEN(0), 0x000FFF80, 0x000FFFFF);
 
 	REG_SET_AND_CHECK(E1000_RCTL, 0xFFFFFFFF, 0x00000000);
 
@@ -830,29 +829,41 @@ static int e1000_reg_test(struct e1000_adapter *adapter, u64 *data)
 	REG_SET_AND_CHECK(E1000_TCTL, 0xFFFFFFFF, 0x00000000);
 
 	REG_SET_AND_CHECK(E1000_RCTL, before, 0xFFFFFFFF);
-	REG_PATTERN_TEST(E1000_RDBAL, 0xFFFFFFF0, 0xFFFFFFFF);
+	REG_PATTERN_TEST(E1000_RDBAL(0), 0xFFFFFFF0, 0xFFFFFFFF);
 	if (!(adapter->flags & FLAG_IS_ICH))
 		REG_PATTERN_TEST(E1000_TXCW, 0xC000FFFF, 0x0000FFFF);
-	REG_PATTERN_TEST(E1000_TDBAL, 0xFFFFFFF0, 0xFFFFFFFF);
+	REG_PATTERN_TEST(E1000_TDBAL(0), 0xFFFFFFF0, 0xFFFFFFFF);
 	REG_PATTERN_TEST(E1000_TIDV, 0x0000FFFF, 0x0000FFFF);
 	mask = 0x8003FFFF;
 	switch (mac->type) {
 	case e1000_ich10lan:
 	case e1000_pchlan:
 	case e1000_pch2lan:
+	case e1000_pch_lpt:
 		mask |= (1 << 18);
 		break;
 	default:
 		break;
 	}
-	for (i = 0; i < mac->rar_entry_count; i++)
+
+	if (mac->type == e1000_pch_lpt)
+		wlock_mac = (er32(FWSM) & E1000_FWSM_WLOCK_MAC_MASK) >>
+		    E1000_FWSM_WLOCK_MAC_SHIFT;
+
+	for (i = 0; i < mac->rar_entry_count; i++) {
+		/* Cannot test write-protected SHRAL[n] registers */
+		if ((wlock_mac == 1) || (wlock_mac && (i > wlock_mac)))
+			continue;
+
 		REG_PATTERN_TEST_ARRAY(E1000_RA, ((i << 1) + 1),
-		                       mask, 0xFFFFFFFF);
+				       mask, 0xFFFFFFFF);
+	}
 
 	for (i = 0; i < mac->mta_reg_count; i++)
 		REG_PATTERN_TEST_ARRAY(E1000_MTA, i, 0xFFFFFFFF, 0xFFFFFFFF);
 
 	*data = 0;
+
 	return 0;
 }
 
@@ -1104,11 +1115,11 @@ static int e1000_setup_desc_rings(struct e1000_adapter *adapter)
 	tx_ring->next_to_use = 0;
 	tx_ring->next_to_clean = 0;
 
-	ew32(TDBAL, ((u64) tx_ring->dma & 0x00000000FFFFFFFF));
-	ew32(TDBAH, ((u64) tx_ring->dma >> 32));
-	ew32(TDLEN, tx_ring->count * sizeof(struct e1000_tx_desc));
-	ew32(TDH, 0);
-	ew32(TDT, 0);
+	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);
 	ew32(TCTL, E1000_TCTL_PSP | E1000_TCTL_EN | E1000_TCTL_MULR |
 	     E1000_COLLISION_THRESHOLD << E1000_CT_SHIFT |
 	     E1000_COLLISION_DISTANCE << E1000_COLD_SHIFT);
@@ -1168,11 +1179,11 @@ 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, ((u64) rx_ring->dma & 0xFFFFFFFF));
-	ew32(RDBAH, ((u64) rx_ring->dma >> 32));
-	ew32(RDLEN, rx_ring->size);
-	ew32(RDH, 0);
-	ew32(RDT, 0);
+	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 |
@@ -1534,7 +1545,7 @@ static int e1000_run_loopback_test(struct e1000_adapter *adapter)
 	int ret_val = 0;
 	unsigned long time;
 
-	ew32(RDT, rx_ring->count - 1);
+	ew32(RDT(0), rx_ring->count - 1);
 
 	/*
 	 * Calculate the loop count based on the largest descriptor ring
@@ -1561,7 +1572,7 @@ static int e1000_run_loopback_test(struct e1000_adapter *adapter)
 			if (k == tx_ring->count)
 				k = 0;
 		}
-		ew32(TDT, k);
+		ew32(TDT(0), k);
 		e1e_flush();
 		msleep(200);
 		time = jiffies; /* set the start time for the receive */
@@ -1791,8 +1802,7 @@ static void e1000_get_wol(struct net_device *netdev,
 		wol->supported &= ~WAKE_UCAST;
 
 		if (adapter->wol & E1000_WUFC_EX)
-			e_err("Interface does not support directed (unicast) "
-			      "frame wake-up packets\n");
+			e_err("Interface does not support directed (unicast) frame wake-up packets\n");
 	}
 
 	if (adapter->wol & E1000_WUFC_EX)
diff --git a/drivers/net/ethernet/intel/e1000e/hw.h b/drivers/net/ethernet/intel/e1000e/hw.h
index f82ecf536c8..ed5b40985ed 100644
--- a/drivers/net/ethernet/intel/e1000e/hw.h
+++ b/drivers/net/ethernet/intel/e1000e/hw.h
@@ -36,16 +36,6 @@ struct e1000_adapter;
 
 #include "defines.h"
 
-#define er32(reg)	__er32(hw, E1000_##reg)
-#define ew32(reg,val)	__ew32(hw, E1000_##reg, (val))
-#define e1e_flush()	er32(STATUS)
-
-#define E1000_WRITE_REG_ARRAY(a, reg, offset, value) \
-	(writel((value), ((a)->hw_addr + reg + ((offset) << 2))))
-
-#define E1000_READ_REG_ARRAY(a, reg, offset) \
-	(readl((a)->hw_addr + reg + ((offset) << 2)))
-
 enum e1e_registers {
 	E1000_CTRL     = 0x00000, /* Device Control - RW */
 	E1000_STATUS   = 0x00008, /* Device Status - RO */
@@ -61,6 +51,7 @@ enum e1e_registers {
 	E1000_FEXTNVM  = 0x00028, /* Future Extended NVM - RW */
 	E1000_FCT      = 0x00030, /* Flow Control Type - RW */
 	E1000_VET      = 0x00038, /* VLAN Ether Type - RW */
+	E1000_FEXTNVM3 = 0x0003C, /* Future Extended NVM 3 - RW */
 	E1000_ICR      = 0x000C0, /* Interrupt Cause Read - R/clr */
 	E1000_ITR      = 0x000C4, /* Interrupt Throttling Rate - RW */
 	E1000_ICS      = 0x000C8, /* Interrupt Cause Set - WO */
@@ -94,31 +85,40 @@ enum e1e_registers {
 	E1000_FCRTL    = 0x02160, /* Flow Control Receive Threshold Low - RW */
 	E1000_FCRTH    = 0x02168, /* Flow Control Receive Threshold High - RW */
 	E1000_PSRCTL   = 0x02170, /* Packet Split Receive Control - RW */
-	E1000_RDBAL    = 0x02800, /* Rx Descriptor Base Address Low - RW */
-	E1000_RDBAH    = 0x02804, /* Rx Descriptor Base Address High - RW */
-	E1000_RDLEN    = 0x02808, /* Rx Descriptor Length - RW */
-	E1000_RDH      = 0x02810, /* Rx Descriptor Head - RW */
-	E1000_RDT      = 0x02818, /* Rx Descriptor Tail - RW */
-	E1000_RDTR     = 0x02820, /* Rx Delay Timer - RW */
-	E1000_RXDCTL_BASE = 0x02828, /* Rx Descriptor Control - RW */
-#define E1000_RXDCTL(_n)   (E1000_RXDCTL_BASE + (_n << 8))
-	E1000_RADV     = 0x0282C, /* Rx Interrupt Absolute Delay Timer - RW */
-
-/* Convenience macros
+/*
+ * Convenience macros
  *
  * Note: "_n" is the queue number of the register to be written to.
  *
  * Example usage:
- * E1000_RDBAL_REG(current_rx_queue)
- *
+ * E1000_RDBAL(current_rx_queue)
  */
-#define E1000_RDBAL_REG(_n)   (E1000_RDBAL + (_n << 8))
+	E1000_RDBAL_BASE = 0x02800, /* Rx Descriptor Base Address Low - RW */
+#define E1000_RDBAL(_n)	(E1000_RDBAL_BASE + (_n << 8))
+	E1000_RDBAH_BASE = 0x02804, /* Rx Descriptor Base Address High - RW */
+#define E1000_RDBAH(_n)	(E1000_RDBAH_BASE + (_n << 8))
+	E1000_RDLEN_BASE = 0x02808, /* Rx Descriptor Length - RW */
+#define E1000_RDLEN(_n)	(E1000_RDLEN_BASE + (_n << 8))
+	E1000_RDH_BASE = 0x02810, /* Rx Descriptor Head - RW */
+#define E1000_RDH(_n)	(E1000_RDH_BASE + (_n << 8))
+	E1000_RDT_BASE = 0x02818, /* Rx Descriptor Tail - RW */
+#define E1000_RDT(_n)	(E1000_RDT_BASE + (_n << 8))
+	E1000_RDTR     = 0x02820, /* Rx Delay Timer - RW */
+	E1000_RXDCTL_BASE = 0x02828, /* Rx Descriptor Control - RW */
+#define E1000_RXDCTL(_n)   (E1000_RXDCTL_BASE + (_n << 8))
+	E1000_RADV     = 0x0282C, /* Rx Interrupt Absolute Delay Timer - RW */
+
 	E1000_KABGTXD  = 0x03004, /* AFE Band Gap Transmit Ref Data */
-	E1000_TDBAL    = 0x03800, /* Tx Descriptor Base Address Low - RW */
-	E1000_TDBAH    = 0x03804, /* Tx Descriptor Base Address High - RW */
-	E1000_TDLEN    = 0x03808, /* Tx Descriptor Length - RW */
-	E1000_TDH      = 0x03810, /* Tx Descriptor Head - RW */
-	E1000_TDT      = 0x03818, /* Tx Descriptor Tail - RW */
+	E1000_TDBAL_BASE = 0x03800, /* Tx Descriptor Base Address Low - RW */
+#define E1000_TDBAL(_n)	(E1000_TDBAL_BASE + (_n << 8))
+	E1000_TDBAH_BASE = 0x03804, /* Tx Descriptor Base Address High - RW */
+#define E1000_TDBAH(_n)	(E1000_TDBAH_BASE + (_n << 8))
+	E1000_TDLEN_BASE = 0x03808, /* Tx Descriptor Length - RW */
+#define E1000_TDLEN(_n)	(E1000_TDLEN_BASE + (_n << 8))
+	E1000_TDH_BASE = 0x03810, /* Tx Descriptor Head - RW */
+#define E1000_TDH(_n)	(E1000_TDH_BASE + (_n << 8))
+	E1000_TDT_BASE = 0x03818, /* Tx Descriptor Tail - RW */
+#define E1000_TDT(_n)	(E1000_TDT_BASE + (_n << 8))
 	E1000_TIDV     = 0x03820, /* Tx Interrupt Delay Value - RW */
 	E1000_TXDCTL_BASE = 0x03828, /* Tx Descriptor Control - RW */
 #define E1000_TXDCTL(_n)   (E1000_TXDCTL_BASE + (_n << 8))
@@ -200,6 +200,14 @@ enum e1e_registers {
 #define E1000_RA        (E1000_RAL(0))
 	E1000_RAH_BASE = 0x05404, /* Receive Address High - RW */
 #define E1000_RAH(_n)   (E1000_RAH_BASE + ((_n) * 8))
+	E1000_SHRAL_PCH_LPT_BASE = 0x05408,
+#define E1000_SHRAL_PCH_LPT(_n)   (E1000_SHRAL_PCH_LPT_BASE + ((_n) * 8))
+	E1000_SHRAH_PCH_LTP_BASE = 0x0540C,
+#define E1000_SHRAH_PCH_LPT(_n)   (E1000_SHRAH_PCH_LTP_BASE + ((_n) * 8))
+	E1000_SHRAL_BASE = 0x05438, /* Shared Receive Address Low - RW */
+#define E1000_SHRAL(_n)   (E1000_SHRAL_BASE + ((_n) * 8))
+	E1000_SHRAH_BASE = 0x0543C, /* Shared Receive Address High - RW */
+#define E1000_SHRAH(_n)   (E1000_SHRAH_BASE + ((_n) * 8))
 	E1000_VFTA     = 0x05600, /* VLAN Filter Table Array - RW Array */
 	E1000_WUC      = 0x05800, /* Wakeup Control - RW */
 	E1000_WUFC     = 0x05808, /* Wakeup Filter Control - RW */
@@ -402,6 +410,8 @@ enum e1e_registers {
 #define E1000_DEV_ID_PCH_D_HV_DC		0x10F0
 #define E1000_DEV_ID_PCH2_LV_LM			0x1502
 #define E1000_DEV_ID_PCH2_LV_V			0x1503
+#define E1000_DEV_ID_PCH_LPT_I217_LM		0x153A
+#define E1000_DEV_ID_PCH_LPT_I217_V		0x153B
 
 #define E1000_REVISION_4 4
 
@@ -422,6 +432,7 @@ enum e1000_mac_type {
 	e1000_ich10lan,
 	e1000_pchlan,
 	e1000_pch2lan,
+	e1000_pch_lpt,
 };
 
 enum e1000_media_type {
@@ -459,6 +470,7 @@ enum e1000_phy_type {
 	e1000_phy_82578,
 	e1000_phy_82577,
 	e1000_phy_82579,
+	e1000_phy_i217,
 };
 
 enum e1000_bus_width {
@@ -782,6 +794,7 @@ struct e1000_mac_operations {
 	s32  (*setup_led)(struct e1000_hw *);
 	void (*write_vfta)(struct e1000_hw *, u32, u32);
 	void (*config_collision_dist)(struct e1000_hw *);
+	void (*rar_set)(struct e1000_hw *, u8 *, u32);
 	s32  (*read_mac_addr)(struct e1000_hw *);
 };
 
@@ -966,6 +979,7 @@ struct e1000_dev_spec_ich8lan {
 	struct e1000_shadow_ram shadow_ram[E1000_ICH8_SHADOW_RAM_WORDS];
 	bool nvm_k1_enabled;
 	bool eee_disable;
+	u16 eee_lp_ability;
 };
 
 struct e1000_hw {
diff --git a/drivers/net/ethernet/intel/e1000e/ich8lan.c b/drivers/net/ethernet/intel/e1000e/ich8lan.c
index b461c24945e..bbf70ba367d 100644
--- a/drivers/net/ethernet/intel/e1000e/ich8lan.c
+++ b/drivers/net/ethernet/intel/e1000e/ich8lan.c
@@ -105,6 +105,9 @@
 #define E1000_FEXTNVM_SW_CONFIG		1
 #define E1000_FEXTNVM_SW_CONFIG_ICH8M (1 << 27) /* Bit redefined for ICH8M :/ */
 
+#define E1000_FEXTNVM3_PHY_CFG_COUNTER_MASK    0x0C000000
+#define E1000_FEXTNVM3_PHY_CFG_COUNTER_50MSEC  0x08000000
+
 #define E1000_FEXTNVM4_BEACON_DURATION_MASK    0x7
 #define E1000_FEXTNVM4_BEACON_DURATION_8USEC   0x7
 #define E1000_FEXTNVM4_BEACON_DURATION_16USEC  0x3
@@ -112,6 +115,8 @@
 #define PCIE_ICH8_SNOOP_ALL		PCIE_NO_SNOOP_ALL
 
 #define E1000_ICH_RAR_ENTRIES		7
+#define E1000_PCH2_RAR_ENTRIES		5 /* RAR[0], SHRA[0-3] */
+#define E1000_PCH_LPT_RAR_ENTRIES	12 /* RAR[0], SHRA[0-10] */
 
 #define PHY_PAGE_SHIFT 5
 #define PHY_REG(page, reg) (((page) << PHY_PAGE_SHIFT) | \
@@ -127,14 +132,22 @@
 
 #define SW_FLAG_TIMEOUT    1000 /* SW Semaphore flag timeout in milliseconds */
 
+/* SMBus Control Phy Register */
+#define CV_SMB_CTRL		PHY_REG(769, 23)
+#define CV_SMB_CTRL_FORCE_SMBUS	0x0001
+
 /* SMBus Address Phy Register */
 #define HV_SMB_ADDR            PHY_REG(768, 26)
 #define HV_SMB_ADDR_MASK       0x007F
 #define HV_SMB_ADDR_PEC_EN     0x0200
 #define HV_SMB_ADDR_VALID      0x0080
+#define HV_SMB_ADDR_FREQ_MASK           0x1100
+#define HV_SMB_ADDR_FREQ_LOW_SHIFT      8
+#define HV_SMB_ADDR_FREQ_HIGH_SHIFT     12
 
 /* PHY Power Management Control */
 #define HV_PM_CTRL		PHY_REG(770, 17)
+#define HV_PM_CTRL_PLL_STOP_IN_K1_GIGA	0x100
 
 /* PHY Low Power Idle Control */
 #define I82579_LPI_CTRL				PHY_REG(772, 20)
@@ -147,11 +160,26 @@
 #define I82579_LPI_UPDATE_TIMER 0x4805	/* in 40ns units + 40 ns base value */
 #define I82579_MSE_THRESHOLD    0x084F	/* Mean Square Error Threshold */
 #define I82579_MSE_LINK_DOWN    0x2411	/* MSE count before dropping link */
+#define I217_EEE_ADVERTISEMENT  0x8001	/* IEEE MMD Register 7.60 */
+#define I217_EEE_LP_ABILITY     0x8002	/* IEEE MMD Register 7.61 */
+#define I217_EEE_100_SUPPORTED  (1 << 1)	/* 100BaseTx EEE supported */
+
+/* Intel Rapid Start Technology Support */
+#define I217_PROXY_CTRL                 PHY_REG(BM_WUC_PAGE, 70)
+#define I217_PROXY_CTRL_AUTO_DISABLE    0x0080
+#define I217_SxCTRL                     PHY_REG(BM_PORT_CTRL_PAGE, 28)
+#define I217_SxCTRL_MASK                0x1000
+#define I217_CGFREG                     PHY_REG(772, 29)
+#define I217_CGFREG_MASK                0x0002
+#define I217_MEMPWR                     PHY_REG(772, 26)
+#define I217_MEMPWR_MASK                0x0010
 
 /* Strapping Option Register - RO */
 #define E1000_STRAP                     0x0000C
 #define E1000_STRAP_SMBUS_ADDRESS_MASK  0x00FE0000
 #define E1000_STRAP_SMBUS_ADDRESS_SHIFT 17
+#define E1000_STRAP_SMT_FREQ_MASK       0x00003000
+#define E1000_STRAP_SMT_FREQ_SHIFT      12
 
 /* OEM Bits Phy Register */
 #define HV_OEM_BITS            PHY_REG(768, 25)
@@ -255,6 +283,8 @@ static s32  e1000_k1_gig_workaround_hv(struct e1000_hw *hw, bool link);
 static s32 e1000_set_mdio_slow_mode_hv(struct e1000_hw *hw);
 static bool e1000_check_mng_mode_ich8lan(struct e1000_hw *hw);
 static bool e1000_check_mng_mode_pchlan(struct e1000_hw *hw);
+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);
 
@@ -283,18 +313,161 @@ static inline void __ew32flash(struct e1000_hw *hw, unsigned long reg, u32 val)
 #define ew16flash(reg, val)	__ew16flash(hw, (reg), (val))
 #define ew32flash(reg, val)	__ew32flash(hw, (reg), (val))
 
-static void e1000_toggle_lanphypc_value_ich8lan(struct e1000_hw *hw)
+/**
+ *  e1000_phy_is_accessible_pchlan - Check if able to access PHY registers
+ *  @hw: pointer to the HW structure
+ *
+ *  Test access to the PHY registers by reading the PHY ID registers.  If
+ *  the PHY ID is already known (e.g. resume path) compare it with known ID,
+ *  otherwise assume the read PHY ID is correct if it is valid.
+ *
+ *  Assumes the sw/fw/hw semaphore is already acquired.
+ **/
+static bool e1000_phy_is_accessible_pchlan(struct e1000_hw *hw)
 {
-	u32 ctrl;
+	u16 phy_reg;
+	u32 phy_id;
 
-	ctrl = er32(CTRL);
-	ctrl |= E1000_CTRL_LANPHYPC_OVERRIDE;
-	ctrl &= ~E1000_CTRL_LANPHYPC_VALUE;
-	ew32(CTRL, ctrl);
-	e1e_flush();
-	udelay(10);
-	ctrl &= ~E1000_CTRL_LANPHYPC_OVERRIDE;
-	ew32(CTRL, ctrl);
+	e1e_rphy_locked(hw, PHY_ID1, &phy_reg);
+	phy_id = (u32)(phy_reg << 16);
+	e1e_rphy_locked(hw, PHY_ID2, &phy_reg);
+	phy_id |= (u32)(phy_reg & PHY_REVISION_MASK);
+
+	if (hw->phy.id) {
+		if (hw->phy.id == phy_id)
+			return true;
+	} else {
+		if ((phy_id != 0) && (phy_id != PHY_REVISION_MASK))
+			hw->phy.id = phy_id;
+		return true;
+	}
+
+	return false;
+}
+
+/**
+ *  e1000_init_phy_workarounds_pchlan - PHY initialization workarounds
+ *  @hw: pointer to the HW structure
+ *
+ *  Workarounds/flow necessary for PHY initialization during driver load
+ *  and resume paths.
+ **/
+static s32 e1000_init_phy_workarounds_pchlan(struct e1000_hw *hw)
+{
+	u32 mac_reg, fwsm = er32(FWSM);
+	s32 ret_val;
+	u16 phy_reg;
+
+	ret_val = hw->phy.ops.acquire(hw);
+	if (ret_val) {
+		e_dbg("Failed to initialize PHY flow\n");
+		return ret_val;
+	}
+
+	/*
+	 * The MAC-PHY interconnect may be in SMBus mode.  If the PHY is
+	 * inaccessible and resetting the PHY is not blocked, toggle the
+	 * LANPHYPC Value bit to force the interconnect to PCIe mode.
+	 */
+	switch (hw->mac.type) {
+	case e1000_pch_lpt:
+		if (e1000_phy_is_accessible_pchlan(hw))
+			break;
+
+		/*
+		 * Before toggling LANPHYPC, see if PHY is accessible by
+		 * forcing MAC to SMBus mode first.
+		 */
+		mac_reg = er32(CTRL_EXT);
+		mac_reg |= E1000_CTRL_EXT_FORCE_SMBUS;
+		ew32(CTRL_EXT, mac_reg);
+
+		/* fall-through */
+	case e1000_pch2lan:
+		/*
+		 * Gate automatic PHY configuration by hardware on
+		 * non-managed 82579
+		 */
+		if ((hw->mac.type == e1000_pch2lan) &&
+		    !(fwsm & E1000_ICH_FWSM_FW_VALID))
+			e1000_gate_hw_phy_config_ich8lan(hw, true);
+
+		if (e1000_phy_is_accessible_pchlan(hw)) {
+			if (hw->mac.type == e1000_pch_lpt) {
+				/* Unforce SMBus mode in PHY */
+				e1e_rphy_locked(hw, CV_SMB_CTRL, &phy_reg);
+				phy_reg &= ~CV_SMB_CTRL_FORCE_SMBUS;
+				e1e_wphy_locked(hw, CV_SMB_CTRL, phy_reg);
+
+				/* Unforce SMBus mode in MAC */
+				mac_reg = er32(CTRL_EXT);
+				mac_reg &= ~E1000_CTRL_EXT_FORCE_SMBUS;
+				ew32(CTRL_EXT, mac_reg);
+			}
+			break;
+		}
+
+		/* fall-through */
+	case e1000_pchlan:
+		if ((hw->mac.type == e1000_pchlan) &&
+		    (fwsm & E1000_ICH_FWSM_FW_VALID))
+			break;
+
+		if (hw->phy.ops.check_reset_block(hw)) {
+			e_dbg("Required LANPHYPC toggle blocked by ME\n");
+			break;
+		}
+
+		e_dbg("Toggling LANPHYPC\n");
+
+		/* Set Phy Config Counter to 50msec */
+		mac_reg = er32(FEXTNVM3);
+		mac_reg &= ~E1000_FEXTNVM3_PHY_CFG_COUNTER_MASK;
+		mac_reg |= E1000_FEXTNVM3_PHY_CFG_COUNTER_50MSEC;
+		ew32(FEXTNVM3, mac_reg);
+
+		/* Toggle LANPHYPC Value bit */
+		mac_reg = er32(CTRL);
+		mac_reg |= E1000_CTRL_LANPHYPC_OVERRIDE;
+		mac_reg &= ~E1000_CTRL_LANPHYPC_VALUE;
+		ew32(CTRL, mac_reg);
+		e1e_flush();
+		udelay(10);
+		mac_reg &= ~E1000_CTRL_LANPHYPC_OVERRIDE;
+		ew32(CTRL, mac_reg);
+		e1e_flush();
+		if (hw->mac.type < e1000_pch_lpt) {
+			msleep(50);
+		} else {
+			u16 count = 20;
+			do {
+				usleep_range(5000, 10000);
+			} while (!(er32(CTRL_EXT) &
+				   E1000_CTRL_EXT_LPCD) && count--);
+		}
+		break;
+	default:
+		break;
+	}
+
+	hw->phy.ops.release(hw);
+
+	/*
+	 * Reset the PHY before any access to it.  Doing so, ensures
+	 * that the PHY is in a known good state before we read/write
+	 * PHY registers.  The generic reset is sufficient here,
+	 * because we haven't determined the PHY type yet.
+	 */
+	ret_val = e1000e_phy_hw_reset_generic(hw);
+
+	/* Ungate automatic PHY configuration on non-managed 82579 */
+	if ((hw->mac.type == e1000_pch2lan) &&
+	    !(fwsm & E1000_ICH_FWSM_FW_VALID)) {
+		usleep_range(10000, 20000);
+		e1000_gate_hw_phy_config_ich8lan(hw, false);
+	}
+
+	return ret_val;
 }
 
 /**
@@ -324,70 +497,41 @@ static s32 e1000_init_phy_params_pchlan(struct e1000_hw *hw)
 	phy->ops.power_down           = e1000_power_down_phy_copper_ich8lan;
 	phy->autoneg_mask             = AUTONEG_ADVERTISE_SPEED_DEFAULT;
 
-	if (!hw->phy.ops.check_reset_block(hw)) {
-		u32 fwsm = er32(FWSM);
-
-		/*
-		 * The MAC-PHY interconnect may still be in SMBus mode after
-		 * Sx->S0.  If resetting the PHY is not blocked, toggle the
-		 * LANPHYPC Value bit to force the interconnect to PCIe mode.
-		 */
-		e1000_toggle_lanphypc_value_ich8lan(hw);
-		msleep(50);
-
-		/*
-		 * Gate automatic PHY configuration by hardware on
-		 * non-managed 82579
-		 */
-		if ((hw->mac.type == e1000_pch2lan) &&
-		    !(fwsm & E1000_ICH_FWSM_FW_VALID))
-			e1000_gate_hw_phy_config_ich8lan(hw, true);
-
-		/*
-		 * Reset the PHY before any access to it.  Doing so, ensures
-		 * that the PHY is in a known good state before we read/write
-		 * PHY registers.  The generic reset is sufficient here,
-		 * because we haven't determined the PHY type yet.
-		 */
-		ret_val = e1000e_phy_hw_reset_generic(hw);
-		if (ret_val)
-			return ret_val;
+	phy->id = e1000_phy_unknown;
 
-		/* Ungate automatic PHY configuration on non-managed 82579 */
-		if ((hw->mac.type == e1000_pch2lan) &&
-		    !(fwsm & E1000_ICH_FWSM_FW_VALID)) {
-			usleep_range(10000, 20000);
-			e1000_gate_hw_phy_config_ich8lan(hw, false);
-		}
-	}
+	ret_val = e1000_init_phy_workarounds_pchlan(hw);
+	if (ret_val)
+		return ret_val;
 
-	phy->id = e1000_phy_unknown;
-	switch (hw->mac.type) {
-	default:
-		ret_val = e1000e_get_phy_id(hw);
-		if (ret_val)
-			return ret_val;
-		if ((phy->id != 0) && (phy->id != PHY_REVISION_MASK))
+	if (phy->id == e1000_phy_unknown)
+		switch (hw->mac.type) {
+		default:
+			ret_val = e1000e_get_phy_id(hw);
+			if (ret_val)
+				return ret_val;
+			if ((phy->id != 0) && (phy->id != PHY_REVISION_MASK))
+				break;
+			/* fall-through */
+		case e1000_pch2lan:
+		case e1000_pch_lpt:
+			/*
+			 * In case the PHY needs to be in mdio slow mode,
+			 * set slow mode and try to get the PHY id again.
+			 */
+			ret_val = e1000_set_mdio_slow_mode_hv(hw);
+			if (ret_val)
+				return ret_val;
+			ret_val = e1000e_get_phy_id(hw);
+			if (ret_val)
+				return ret_val;
 			break;
-		/* fall-through */
-	case e1000_pch2lan:
-		/*
-		 * In case the PHY needs to be in mdio slow mode,
-		 * set slow mode and try to get the PHY id again.
-		 */
-		ret_val = e1000_set_mdio_slow_mode_hv(hw);
-		if (ret_val)
-			return ret_val;
-		ret_val = e1000e_get_phy_id(hw);
-		if (ret_val)
-			return ret_val;
-		break;
-	}
+		}
 	phy->type = e1000e_get_phy_type_from_id(phy->id);
 
 	switch (phy->type) {
 	case e1000_phy_82577:
 	case e1000_phy_82579:
+	case e1000_phy_i217:
 		phy->ops.check_polarity = e1000_check_polarity_82577;
 		phy->ops.force_speed_duplex =
 		    e1000_phy_force_speed_duplex_82577;
@@ -572,7 +716,7 @@ static s32 e1000_init_mac_params_ich8lan(struct e1000_hw *hw)
 	/* Adaptive IFS supported */
 	mac->adaptive_ifs = true;
 
-	/* LED operations */
+	/* LED and other operations */
 	switch (mac->type) {
 	case e1000_ich8lan:
 	case e1000_ich9lan:
@@ -591,8 +735,12 @@ static s32 e1000_init_mac_params_ich8lan(struct e1000_hw *hw)
 		mac->ops.led_on = e1000_led_on_ich8lan;
 		mac->ops.led_off = e1000_led_off_ich8lan;
 		break;
-	case e1000_pchlan:
 	case e1000_pch2lan:
+		mac->rar_entry_count = E1000_PCH2_RAR_ENTRIES;
+		mac->ops.rar_set = e1000_rar_set_pch2lan;
+		/* fall-through */
+	case e1000_pch_lpt:
+	case e1000_pchlan:
 		/* check management mode */
 		mac->ops.check_mng_mode = e1000_check_mng_mode_pchlan;
 		/* ID LED init */
@@ -609,12 +757,20 @@ static s32 e1000_init_mac_params_ich8lan(struct e1000_hw *hw)
 		break;
 	}
 
+	if (mac->type == e1000_pch_lpt) {
+		mac->rar_entry_count = E1000_PCH_LPT_RAR_ENTRIES;
+		mac->ops.rar_set = e1000_rar_set_pch_lpt;
+	}
+
 	/* Enable PCS Lock-loss workaround for ICH8 */
 	if (mac->type == e1000_ich8lan)
 		e1000e_set_kmrn_lock_loss_workaround_ich8lan(hw, true);
 
-	/* Gate automatic PHY configuration by hardware on managed 82579 */
-	if ((mac->type == e1000_pch2lan) &&
+	/*
+	 * Gate automatic PHY configuration by hardware on managed
+	 * 82579 and i217
+	 */
+	if ((mac->type == e1000_pch2lan || mac->type == e1000_pch_lpt) &&
 	    (er32(FWSM) & E1000_ICH_FWSM_FW_VALID))
 		e1000_gate_hw_phy_config_ich8lan(hw, true);
 
@@ -630,22 +786,50 @@ static s32 e1000_init_mac_params_ich8lan(struct e1000_hw *hw)
  **/
 static s32 e1000_set_eee_pchlan(struct e1000_hw *hw)
 {
+	struct e1000_dev_spec_ich8lan *dev_spec = &hw->dev_spec.ich8lan;
 	s32 ret_val = 0;
 	u16 phy_reg;
 
-	if (hw->phy.type != e1000_phy_82579)
+	if ((hw->phy.type != e1000_phy_82579) &&
+	    (hw->phy.type != e1000_phy_i217))
 		return 0;
 
 	ret_val = e1e_rphy(hw, I82579_LPI_CTRL, &phy_reg);
 	if (ret_val)
 		return ret_val;
 
-	if (hw->dev_spec.ich8lan.eee_disable)
+	if (dev_spec->eee_disable)
 		phy_reg &= ~I82579_LPI_CTRL_ENABLE_MASK;
 	else
 		phy_reg |= I82579_LPI_CTRL_ENABLE_MASK;
 
-	return e1e_wphy(hw, I82579_LPI_CTRL, phy_reg);
+	ret_val = e1e_wphy(hw, I82579_LPI_CTRL, phy_reg);
+	if (ret_val)
+		return ret_val;
+
+	if ((hw->phy.type == e1000_phy_i217) && !dev_spec->eee_disable) {
+		/* Save off link partner's EEE ability */
+		ret_val = hw->phy.ops.acquire(hw);
+		if (ret_val)
+			return ret_val;
+		ret_val = e1e_wphy_locked(hw, I82579_EMI_ADDR,
+					  I217_EEE_LP_ABILITY);
+		if (ret_val)
+			goto release;
+		e1e_rphy_locked(hw, I82579_EMI_DATA, &dev_spec->eee_lp_ability);
+
+		/*
+		 * EEE is not supported in 100Half, so ignore partner's EEE
+		 * in 100 ability if full-duplex is not advertised.
+		 */
+		e1e_rphy_locked(hw, PHY_LP_ABILITY, &phy_reg);
+		if (!(phy_reg & NWAY_LPAR_100TX_FD_CAPS))
+			dev_spec->eee_lp_ability &= ~I217_EEE_100_SUPPORTED;
+release:
+		hw->phy.ops.release(hw);
+	}
+
+	return 0;
 }
 
 /**
@@ -687,6 +871,9 @@ static s32 e1000_check_for_copper_link_ich8lan(struct e1000_hw *hw)
 			return ret_val;
 	}
 
+	/* Clear link partner's EEE ability */
+	hw->dev_spec.ich8lan.eee_lp_ability = 0;
+
 	if (!link)
 		return 0; /* No link detected */
 
@@ -782,6 +969,7 @@ static s32 e1000_get_variants_ich8lan(struct e1000_adapter *adapter)
 		break;
 	case e1000_pchlan:
 	case e1000_pch2lan:
+	case e1000_pch_lpt:
 		rc = e1000_init_phy_params_pchlan(hw);
 		break;
 	default:
@@ -967,6 +1155,145 @@ static bool e1000_check_mng_mode_pchlan(struct e1000_hw *hw)
 }
 
 /**
+ *  e1000_rar_set_pch2lan - Set receive address register
+ *  @hw: pointer to the HW structure
+ *  @addr: pointer to the receive address
+ *  @index: receive address array register
+ *
+ *  Sets the receive address array register at index to the address passed
+ *  in by addr.  For 82579, RAR[0] is the base address register that is to
+ *  contain the MAC address but RAR[1-6] are reserved for manageability (ME).
+ *  Use SHRA[0-3] in place of those reserved for ME.
+ **/
+static void e1000_rar_set_pch2lan(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
+	 * 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));
+
+	rar_high = ((u32)addr[4] | ((u32)addr[5] << 8));
+
+	/* If MAC address zero, no need to set the AV bit */
+	if (rar_low || rar_high)
+		rar_high |= E1000_RAH_AV;
+
+	if (index == 0) {
+		ew32(RAL(index), rar_low);
+		e1e_flush();
+		ew32(RAH(index), rar_high);
+		e1e_flush();
+		return;
+	}
+
+	if (index < hw->mac.rar_entry_count) {
+		s32 ret_val;
+
+		ret_val = e1000_acquire_swflag_ich8lan(hw);
+		if (ret_val)
+			goto out;
+
+		ew32(SHRAL(index - 1), rar_low);
+		e1e_flush();
+		ew32(SHRAH(index - 1), rar_high);
+		e1e_flush();
+
+		e1000_release_swflag_ich8lan(hw);
+
+		/* verify the register updates */
+		if ((er32(SHRAL(index - 1)) == rar_low) &&
+		    (er32(SHRAH(index - 1)) == rar_high))
+			return;
+
+		e_dbg("SHRA[%d] might be locked by ME - FWSM=0x%8.8x\n",
+		      (index - 1), er32(FWSM));
+	}
+
+out:
+	e_dbg("Failed to write receive address at index %d\n", index);
+}
+
+/**
+ *  e1000_rar_set_pch_lpt - Set receive address registers
+ *  @hw: pointer to the HW structure
+ *  @addr: pointer to the receive address
+ *  @index: receive address array register
+ *
+ *  Sets the receive address register array at index to the address passed
+ *  in by addr. For LPT, RAR[0] is the base address register that is to
+ *  contain the MAC address. SHRA[0-10] are the shared receive address
+ *  registers that are shared between the Host and manageability engine (ME).
+ **/
+static void e1000_rar_set_pch_lpt(struct e1000_hw *hw, u8 *addr, u32 index)
+{
+	u32 rar_low, rar_high;
+	u32 wlock_mac;
+
+	/*
+	 * 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] | ((u32)addr[1] << 8) |
+		   ((u32)addr[2] << 16) | ((u32)addr[3] << 24));
+
+	rar_high = ((u32)addr[4] | ((u32)addr[5] << 8));
+
+	/* If MAC address zero, no need to set the AV bit */
+	if (rar_low || rar_high)
+		rar_high |= E1000_RAH_AV;
+
+	if (index == 0) {
+		ew32(RAL(index), rar_low);
+		e1e_flush();
+		ew32(RAH(index), rar_high);
+		e1e_flush();
+		return;
+	}
+
+	/*
+	 * The manageability engine (ME) can lock certain SHRAR registers that
+	 * it is using - those registers are unavailable for use.
+	 */
+	if (index < hw->mac.rar_entry_count) {
+		wlock_mac = er32(FWSM) & E1000_FWSM_WLOCK_MAC_MASK;
+		wlock_mac >>= E1000_FWSM_WLOCK_MAC_SHIFT;
+
+		/* Check if all SHRAR registers are locked */
+		if (wlock_mac == 1)
+			goto out;
+
+		if ((wlock_mac == 0) || (index <= wlock_mac)) {
+			s32 ret_val;
+
+			ret_val = e1000_acquire_swflag_ich8lan(hw);
+
+			if (ret_val)
+				goto out;
+
+			ew32(SHRAL_PCH_LPT(index - 1), rar_low);
+			e1e_flush();
+			ew32(SHRAH_PCH_LPT(index - 1), rar_high);
+			e1e_flush();
+
+			e1000_release_swflag_ich8lan(hw);
+
+			/* verify the register updates */
+			if ((er32(SHRAL_PCH_LPT(index - 1)) == rar_low) &&
+			    (er32(SHRAH_PCH_LPT(index - 1)) == rar_high))
+				return;
+		}
+	}
+
+out:
+	e_dbg("Failed to write receive address at index %d\n", index);
+}
+
+/**
  *  e1000_check_reset_block_ich8lan - Check if PHY reset is blocked
  *  @hw: pointer to the HW structure
  *
@@ -994,6 +1321,8 @@ static s32 e1000_write_smbus_addr(struct e1000_hw *hw)
 {
 	u16 phy_data;
 	u32 strap = er32(STRAP);
+	u32 freq = (strap & E1000_STRAP_SMT_FREQ_MASK) >>
+	    E1000_STRAP_SMT_FREQ_SHIFT;
 	s32 ret_val = 0;
 
 	strap &= E1000_STRAP_SMBUS_ADDRESS_MASK;
@@ -1006,6 +1335,19 @@ static s32 e1000_write_smbus_addr(struct e1000_hw *hw)
 	phy_data |= (strap >> E1000_STRAP_SMBUS_ADDRESS_SHIFT);
 	phy_data |= HV_SMB_ADDR_PEC_EN | HV_SMB_ADDR_VALID;
 
+	if (hw->phy.type == e1000_phy_i217) {
+		/* Restore SMBus frequency */
+		if (freq--) {
+			phy_data &= ~HV_SMB_ADDR_FREQ_MASK;
+			phy_data |= (freq & (1 << 0)) <<
+			    HV_SMB_ADDR_FREQ_LOW_SHIFT;
+			phy_data |= (freq & (1 << 1)) <<
+			    (HV_SMB_ADDR_FREQ_HIGH_SHIFT - 1);
+		} else {
+			e_dbg("Unsupported SMB frequency in PHY\n");
+		}
+	}
+
 	return e1000_write_phy_reg_hv_locked(hw, HV_SMB_ADDR, phy_data);
 }
 
@@ -1043,6 +1385,7 @@ static s32 e1000_sw_lcd_config_ich8lan(struct e1000_hw *hw)
 		/* Fall-thru */
 	case e1000_pchlan:
 	case e1000_pch2lan:
+	case e1000_pch_lpt:
 		sw_cfg_mask = E1000_FEXTNVM_SW_CONFIG_ICH8M;
 		break;
 	default:
@@ -1062,10 +1405,9 @@ static s32 e1000_sw_lcd_config_ich8lan(struct e1000_hw *hw)
 	 * extended configuration before SW configuration
 	 */
 	data = er32(EXTCNF_CTRL);
-	if (!(hw->mac.type == e1000_pch2lan)) {
-		if (data & E1000_EXTCNF_CTRL_LCD_WRITE_ENABLE)
-			goto release;
-	}
+	if ((hw->mac.type < e1000_pch2lan) &&
+	    (data & E1000_EXTCNF_CTRL_LCD_WRITE_ENABLE))
+		goto release;
 
 	cnf_size = er32(EXTCNF_SIZE);
 	cnf_size &= E1000_EXTCNF_SIZE_EXT_PCIE_LENGTH_MASK;
@@ -1076,9 +1418,9 @@ static s32 e1000_sw_lcd_config_ich8lan(struct e1000_hw *hw)
 	cnf_base_addr = data & E1000_EXTCNF_CTRL_EXT_CNF_POINTER_MASK;
 	cnf_base_addr >>= E1000_EXTCNF_CTRL_EXT_CNF_POINTER_SHIFT;
 
-	if ((!(data & E1000_EXTCNF_CTRL_OEM_WRITE_ENABLE) &&
-	    (hw->mac.type == e1000_pchlan)) ||
-	     (hw->mac.type == e1000_pch2lan)) {
+	if (((hw->mac.type == e1000_pchlan) &&
+	     !(data & E1000_EXTCNF_CTRL_OEM_WRITE_ENABLE)) ||
+	    (hw->mac.type > e1000_pchlan)) {
 		/*
 		 * HW configures the SMBus address and LEDs when the
 		 * OEM and LCD Write Enable bits are set in the NVM.
@@ -1121,8 +1463,7 @@ static s32 e1000_sw_lcd_config_ich8lan(struct e1000_hw *hw)
 		reg_addr &= PHY_REG_MASK;
 		reg_addr |= phy_page;
 
-		ret_val = phy->ops.write_reg_locked(hw, (u32)reg_addr,
-						    reg_data);
+		ret_val = e1e_wphy_locked(hw, (u32)reg_addr, reg_data);
 		if (ret_val)
 			goto release;
 	}
@@ -1159,8 +1500,8 @@ static s32 e1000_k1_gig_workaround_hv(struct e1000_hw *hw, bool link)
 	/* Disable K1 when link is 1Gbps, otherwise use the NVM setting */
 	if (link) {
 		if (hw->phy.type == e1000_phy_82578) {
-			ret_val = hw->phy.ops.read_reg_locked(hw, BM_CS_STATUS,
-			                                          &status_reg);
+			ret_val = e1e_rphy_locked(hw, BM_CS_STATUS,
+						  &status_reg);
 			if (ret_val)
 				goto release;
 
@@ -1175,8 +1516,7 @@ static s32 e1000_k1_gig_workaround_hv(struct e1000_hw *hw, bool link)
 		}
 
 		if (hw->phy.type == e1000_phy_82577) {
-			ret_val = hw->phy.ops.read_reg_locked(hw, HV_M_STATUS,
-			                                          &status_reg);
+			ret_val = e1e_rphy_locked(hw, HV_M_STATUS, &status_reg);
 			if (ret_val)
 				goto release;
 
@@ -1191,15 +1531,13 @@ static s32 e1000_k1_gig_workaround_hv(struct e1000_hw *hw, bool link)
 		}
 
 		/* Link stall fix for link up */
-		ret_val = hw->phy.ops.write_reg_locked(hw, PHY_REG(770, 19),
-		                                           0x0100);
+		ret_val = e1e_wphy_locked(hw, PHY_REG(770, 19), 0x0100);
 		if (ret_val)
 			goto release;
 
 	} else {
 		/* Link stall fix for link down */
-		ret_val = hw->phy.ops.write_reg_locked(hw, PHY_REG(770, 19),
-		                                           0x4100);
+		ret_val = e1e_wphy_locked(hw, PHY_REG(770, 19), 0x4100);
 		if (ret_val)
 			goto release;
 	}
@@ -1279,14 +1617,14 @@ static s32 e1000_oem_bits_config_ich8lan(struct e1000_hw *hw, bool d0_state)
 	u32 mac_reg;
 	u16 oem_reg;
 
-	if ((hw->mac.type != e1000_pch2lan) && (hw->mac.type != e1000_pchlan))
+	if (hw->mac.type < e1000_pchlan)
 		return ret_val;
 
 	ret_val = hw->phy.ops.acquire(hw);
 	if (ret_val)
 		return ret_val;
 
-	if (!(hw->mac.type == e1000_pch2lan)) {
+	if (hw->mac.type == e1000_pchlan) {
 		mac_reg = er32(EXTCNF_CTRL);
 		if (mac_reg & E1000_EXTCNF_CTRL_OEM_WRITE_ENABLE)
 			goto release;
@@ -1298,7 +1636,7 @@ static s32 e1000_oem_bits_config_ich8lan(struct e1000_hw *hw, bool d0_state)
 
 	mac_reg = er32(PHY_CTRL);
 
-	ret_val = hw->phy.ops.read_reg_locked(hw, HV_OEM_BITS, &oem_reg);
+	ret_val = e1e_rphy_locked(hw, HV_OEM_BITS, &oem_reg);
 	if (ret_val)
 		goto release;
 
@@ -1325,7 +1663,7 @@ static s32 e1000_oem_bits_config_ich8lan(struct e1000_hw *hw, bool d0_state)
 	    !hw->phy.ops.check_reset_block(hw))
 		oem_reg |= HV_OEM_BITS_RESTART_AN;
 
-	ret_val = hw->phy.ops.write_reg_locked(hw, HV_OEM_BITS, oem_reg);
+	ret_val = e1e_wphy_locked(hw, HV_OEM_BITS, oem_reg);
 
 release:
 	hw->phy.ops.release(hw);
@@ -1421,11 +1759,10 @@ static s32 e1000_hv_phy_workarounds_ich8lan(struct e1000_hw *hw)
 	ret_val = hw->phy.ops.acquire(hw);
 	if (ret_val)
 		return ret_val;
-	ret_val = hw->phy.ops.read_reg_locked(hw, BM_PORT_GEN_CFG, &phy_data);
+	ret_val = e1e_rphy_locked(hw, BM_PORT_GEN_CFG, &phy_data);
 	if (ret_val)
 		goto release;
-	ret_val = hw->phy.ops.write_reg_locked(hw, BM_PORT_GEN_CFG,
-					       phy_data & 0x00FF);
+	ret_val = e1e_wphy_locked(hw, BM_PORT_GEN_CFG, phy_data & 0x00FF);
 release:
 	hw->phy.ops.release(hw);
 
@@ -1484,7 +1821,7 @@ s32 e1000_lv_jumbo_workaround_ich8lan(struct e1000_hw *hw, bool enable)
 	u32 mac_reg;
 	u16 i;
 
-	if (hw->mac.type != e1000_pch2lan)
+	if (hw->mac.type < e1000_pch2lan)
 		return 0;
 
 	/* disable Rx path while enabling/disabling workaround */
@@ -1657,20 +1994,18 @@ static s32 e1000_lv_phy_workarounds_ich8lan(struct e1000_hw *hw)
 	ret_val = hw->phy.ops.acquire(hw);
 	if (ret_val)
 		return ret_val;
-	ret_val = hw->phy.ops.write_reg_locked(hw, I82579_EMI_ADDR,
-					       I82579_MSE_THRESHOLD);
+	ret_val = e1e_wphy_locked(hw, I82579_EMI_ADDR, I82579_MSE_THRESHOLD);
 	if (ret_val)
 		goto release;
 	/* set MSE higher to enable link to stay up when noise is high */
-	ret_val = hw->phy.ops.write_reg_locked(hw, I82579_EMI_DATA, 0x0034);
+	ret_val = e1e_wphy_locked(hw, I82579_EMI_DATA, 0x0034);
 	if (ret_val)
 		goto release;
-	ret_val = hw->phy.ops.write_reg_locked(hw, I82579_EMI_ADDR,
-					       I82579_MSE_LINK_DOWN);
+	ret_val = e1e_wphy_locked(hw, I82579_EMI_ADDR, I82579_MSE_LINK_DOWN);
 	if (ret_val)
 		goto release;
 	/* drop link after 5 times MSE threshold was reached */
-	ret_val = hw->phy.ops.write_reg_locked(hw, I82579_EMI_DATA, 0x0005);
+	ret_val = e1e_wphy_locked(hw, I82579_EMI_DATA, 0x0005);
 release:
 	hw->phy.ops.release(hw);
 
@@ -1708,8 +2043,18 @@ static s32 e1000_k1_workaround_lv(struct e1000_hw *hw)
 			return ret_val;
 
 		if (status_reg & HV_M_STATUS_SPEED_1000) {
+			u16 pm_phy_reg;
+
 			mac_reg |= E1000_FEXTNVM4_BEACON_DURATION_8USEC;
 			phy_reg &= ~I82579_LPI_CTRL_FORCE_PLL_LOCK_COUNT;
+			/* LV 1G Packet drop issue wa  */
+			ret_val = e1e_rphy(hw, HV_PM_CTRL, &pm_phy_reg);
+			if (ret_val)
+				return ret_val;
+			pm_phy_reg &= ~HV_PM_CTRL_PLL_STOP_IN_K1_GIGA;
+			ret_val = e1e_wphy(hw, HV_PM_CTRL, pm_phy_reg);
+			if (ret_val)
+				return ret_val;
 		} else {
 			mac_reg |= E1000_FEXTNVM4_BEACON_DURATION_16USEC;
 			phy_reg |= I82579_LPI_CTRL_FORCE_PLL_LOCK_COUNT;
@@ -1733,7 +2078,7 @@ static void e1000_gate_hw_phy_config_ich8lan(struct e1000_hw *hw, bool gate)
 {
 	u32 extcnf_ctrl;
 
-	if (hw->mac.type != e1000_pch2lan)
+	if (hw->mac.type < e1000_pch2lan)
 		return;
 
 	extcnf_ctrl = er32(EXTCNF_CTRL);
@@ -1835,12 +2180,10 @@ static s32 e1000_post_phy_reset_ich8lan(struct e1000_hw *hw)
 		ret_val = hw->phy.ops.acquire(hw);
 		if (ret_val)
 			return ret_val;
-		ret_val = hw->phy.ops.write_reg_locked(hw, I82579_EMI_ADDR,
-						       I82579_LPI_UPDATE_TIMER);
+		ret_val = e1e_wphy_locked(hw, I82579_EMI_ADDR,
+					  I82579_LPI_UPDATE_TIMER);
 		if (!ret_val)
-			ret_val = hw->phy.ops.write_reg_locked(hw,
-							       I82579_EMI_DATA,
-							       0x1387);
+			ret_val = e1e_wphy_locked(hw, I82579_EMI_DATA, 0x1387);
 		hw->phy.ops.release(hw);
 	}
 
@@ -2213,7 +2556,7 @@ static s32 e1000_flash_cycle_init_ich8lan(struct e1000_hw *hw)
 	hsfsts.regval = er16flash(ICH_FLASH_HSFSTS);
 
 	/* Check if the flash descriptor is valid */
-	if (hsfsts.hsf_status.fldesvalid == 0) {
+	if (!hsfsts.hsf_status.fldesvalid) {
 		e_dbg("Flash descriptor invalid.  SW Sequencing must be used.\n");
 		return -E1000_ERR_NVM;
 	}
@@ -2233,7 +2576,7 @@ static s32 e1000_flash_cycle_init_ich8lan(struct e1000_hw *hw)
 	 * completed.
 	 */
 
-	if (hsfsts.hsf_status.flcinprog == 0) {
+	if (!hsfsts.hsf_status.flcinprog) {
 		/*
 		 * There is no cycle running at present,
 		 * so we can start a cycle.
@@ -2251,7 +2594,7 @@ static s32 e1000_flash_cycle_init_ich8lan(struct e1000_hw *hw)
 		 */
 		for (i = 0; i < ICH_FLASH_READ_COMMAND_TIMEOUT; i++) {
 			hsfsts.regval = er16flash(ICH_FLASH_HSFSTS);
-			if (hsfsts.hsf_status.flcinprog == 0) {
+			if (!hsfsts.hsf_status.flcinprog) {
 				ret_val = 0;
 				break;
 			}
@@ -2293,12 +2636,12 @@ static s32 e1000_flash_cycle_ich8lan(struct e1000_hw *hw, u32 timeout)
 	/* wait till FDONE bit is set to 1 */
 	do {
 		hsfsts.regval = er16flash(ICH_FLASH_HSFSTS);
-		if (hsfsts.hsf_status.flcdone == 1)
+		if (hsfsts.hsf_status.flcdone)
 			break;
 		udelay(1);
 	} while (i++ < timeout);
 
-	if (hsfsts.hsf_status.flcdone == 1 && hsfsts.hsf_status.flcerr == 0)
+	if (hsfsts.hsf_status.flcdone && !hsfsts.hsf_status.flcerr)
 		return 0;
 
 	return -E1000_ERR_NVM;
@@ -2409,10 +2752,10 @@ static s32 e1000_read_flash_data_ich8lan(struct e1000_hw *hw, u32 offset,
 			 * ICH_FLASH_CYCLE_REPEAT_COUNT times.
 			 */
 			hsfsts.regval = er16flash(ICH_FLASH_HSFSTS);
-			if (hsfsts.hsf_status.flcerr == 1) {
+			if (hsfsts.hsf_status.flcerr) {
 				/* Repeat for some time before giving up. */
 				continue;
-			} else if (hsfsts.hsf_status.flcdone == 0) {
+			} else if (!hsfsts.hsf_status.flcdone) {
 				e_dbg("Timeout error - flash cycle did not complete.\n");
 				break;
 			}
@@ -2642,7 +2985,7 @@ static s32 e1000_validate_nvm_checksum_ich8lan(struct e1000_hw *hw)
 	if (ret_val)
 		return ret_val;
 
-	if ((data & 0x40) == 0) {
+	if (!(data & 0x40)) {
 		data |= 0x40;
 		ret_val = e1000_write_nvm(hw, 0x19, 1, &data);
 		if (ret_val)
@@ -2760,10 +3103,10 @@ static s32 e1000_write_flash_data_ich8lan(struct e1000_hw *hw, u32 offset,
 		 * try...ICH_FLASH_CYCLE_REPEAT_COUNT times.
 		 */
 		hsfsts.regval = er16flash(ICH_FLASH_HSFSTS);
-		if (hsfsts.hsf_status.flcerr == 1)
+		if (hsfsts.hsf_status.flcerr)
 			/* Repeat for some time before giving up. */
 			continue;
-		if (hsfsts.hsf_status.flcdone == 0) {
+		if (!hsfsts.hsf_status.flcdone) {
 			e_dbg("Timeout error - flash cycle did not complete.\n");
 			break;
 		}
@@ -2915,10 +3258,10 @@ static s32 e1000_erase_flash_bank_ich8lan(struct e1000_hw *hw, u32 bank)
 			 * a few more times else Done
 			 */
 			hsfsts.regval = er16flash(ICH_FLASH_HSFSTS);
-			if (hsfsts.hsf_status.flcerr == 1)
+			if (hsfsts.hsf_status.flcerr)
 				/* repeat for some time before giving up */
 				continue;
-			else if (hsfsts.hsf_status.flcdone == 0)
+			else if (!hsfsts.hsf_status.flcdone)
 				return ret_val;
 		} while (++count < ICH_FLASH_CYCLE_REPEAT_COUNT);
 	}
@@ -3060,8 +3403,8 @@ static s32 e1000_get_bus_info_ich8lan(struct e1000_hw *hw)
 static s32 e1000_reset_hw_ich8lan(struct e1000_hw *hw)
 {
 	struct e1000_dev_spec_ich8lan *dev_spec = &hw->dev_spec.ich8lan;
-	u16 reg;
-	u32 ctrl, kab;
+	u16 kum_cfg;
+	u32 ctrl, reg;
 	s32 ret_val;
 
 	/*
@@ -3095,12 +3438,12 @@ static s32 e1000_reset_hw_ich8lan(struct e1000_hw *hw)
 	}
 
 	if (hw->mac.type == e1000_pchlan) {
-		/* Save the NVM K1 bit setting*/
-		ret_val = e1000_read_nvm(hw, E1000_NVM_K1_CONFIG, 1, &reg);
+		/* Save the NVM K1 bit setting */
+		ret_val = e1000_read_nvm(hw, E1000_NVM_K1_CONFIG, 1, &kum_cfg);
 		if (ret_val)
 			return ret_val;
 
-		if (reg & E1000_NVM_K1_ENABLE)
+		if (kum_cfg & E1000_NVM_K1_ENABLE)
 			dev_spec->nvm_k1_enabled = true;
 		else
 			dev_spec->nvm_k1_enabled = false;
@@ -3130,6 +3473,14 @@ static s32 e1000_reset_hw_ich8lan(struct e1000_hw *hw)
 	/* cannot issue a flush here because it hangs the hardware */
 	msleep(20);
 
+	/* Set Phy Config Counter to 50msec */
+	if (hw->mac.type == e1000_pch2lan) {
+		reg = er32(FEXTNVM3);
+		reg &= ~E1000_FEXTNVM3_PHY_CFG_COUNTER_MASK;
+		reg |= E1000_FEXTNVM3_PHY_CFG_COUNTER_50MSEC;
+		ew32(FEXTNVM3, reg);
+	}
+
 	if (!ret_val)
 		clear_bit(__E1000_ACCESS_SHARED_RESOURCE, &hw->adapter->state);
 
@@ -3154,9 +3505,9 @@ static s32 e1000_reset_hw_ich8lan(struct e1000_hw *hw)
 	ew32(IMC, 0xffffffff);
 	er32(ICR);
 
-	kab = er32(KABGTXD);
-	kab |= E1000_KABGTXD_BGSQLBIAS;
-	ew32(KABGTXD, kab);
+	reg = er32(KABGTXD);
+	reg |= E1000_KABGTXD_BGSQLBIAS;
+	ew32(KABGTXD, reg);
 
 	return 0;
 }
@@ -3309,6 +3660,13 @@ static void e1000_initialize_hw_bits_ich8lan(struct e1000_hw *hw)
 	 */
 	reg = er32(RFCTL);
 	reg |= (E1000_RFCTL_NFSW_DIS | E1000_RFCTL_NFSR_DIS);
+
+	/*
+	 * Disable IPv6 extension header parsing because some malformed
+	 * IPv6 headers can hang the Rx.
+	 */
+	if (hw->mac.type == e1000_ich8lan)
+		reg |= (E1000_RFCTL_IPV6_EX_DIS | E1000_RFCTL_NEW_IPV6_EXT_DIS);
 	ew32(RFCTL, reg);
 }
 
@@ -3359,6 +3717,7 @@ static s32 e1000_setup_link_ich8lan(struct e1000_hw *hw)
 	ew32(FCTTV, hw->fc.pause_time);
 	if ((hw->phy.type == e1000_phy_82578) ||
 	    (hw->phy.type == e1000_phy_82579) ||
+	    (hw->phy.type == e1000_phy_i217) ||
 	    (hw->phy.type == e1000_phy_82577)) {
 		ew32(FCRTV_PCH, hw->fc.refresh_time);
 
@@ -3422,6 +3781,7 @@ 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;
@@ -3668,14 +4028,88 @@ void e1000e_gig_downshift_workaround_ich8lan(struct e1000_hw *hw)
  *  the LPLU setting in the NVM or custom setting.  For PCH and newer parts,
  *  the OEM bits PHY register (LED, GbE disable and LPLU configurations) also
  *  needs to be written.
+ *  Parts that support (and are linked to a partner which support) EEE in
+ *  100Mbps should disable LPLU since 100Mbps w/ EEE requires less power
+ *  than 10Mbps w/o EEE.
  **/
 void e1000_suspend_workarounds_ich8lan(struct e1000_hw *hw)
 {
+	struct e1000_dev_spec_ich8lan *dev_spec = &hw->dev_spec.ich8lan;
 	u32 phy_ctrl;
 	s32 ret_val;
 
 	phy_ctrl = er32(PHY_CTRL);
 	phy_ctrl |= E1000_PHY_CTRL_GBE_DISABLE;
+	if (hw->phy.type == e1000_phy_i217) {
+		u16 phy_reg;
+
+		ret_val = hw->phy.ops.acquire(hw);
+		if (ret_val)
+			goto out;
+
+		if (!dev_spec->eee_disable) {
+			u16 eee_advert;
+
+			ret_val = e1e_wphy_locked(hw, I82579_EMI_ADDR,
+						  I217_EEE_ADVERTISEMENT);
+			if (ret_val)
+				goto release;
+			e1e_rphy_locked(hw, I82579_EMI_DATA, &eee_advert);
+
+			/*
+			 * Disable LPLU if both link partners support 100BaseT
+			 * EEE and 100Full is advertised on both ends of the
+			 * link.
+			 */
+			if ((eee_advert & I217_EEE_100_SUPPORTED) &&
+			    (dev_spec->eee_lp_ability &
+			     I217_EEE_100_SUPPORTED) &&
+			    (hw->phy.autoneg_advertised & ADVERTISE_100_FULL))
+				phy_ctrl &= ~(E1000_PHY_CTRL_D0A_LPLU |
+					      E1000_PHY_CTRL_NOND0A_LPLU);
+		}
+
+		/*
+		 * For i217 Intel Rapid Start Technology support,
+		 * when the system is going into Sx and no manageability engine
+		 * is present, the driver must configure proxy to reset only on
+		 * power good.  LPI (Low Power Idle) state must also reset only
+		 * on power good, as well as the MTA (Multicast table array).
+		 * The SMBus release must also be disabled on LCD reset.
+		 */
+		if (!(er32(FWSM) & E1000_ICH_FWSM_FW_VALID)) {
+
+			/* Enable proxy to reset only on power good. */
+			e1e_rphy_locked(hw, I217_PROXY_CTRL, &phy_reg);
+			phy_reg |= I217_PROXY_CTRL_AUTO_DISABLE;
+			e1e_wphy_locked(hw, I217_PROXY_CTRL, phy_reg);
+
+			/*
+			 * Set bit enable LPI (EEE) to reset only on
+			 * power good.
+			 */
+			e1e_rphy_locked(hw, I217_SxCTRL, &phy_reg);
+			phy_reg |= I217_SxCTRL_MASK;
+			e1e_wphy_locked(hw, I217_SxCTRL, phy_reg);
+
+			/* Disable the SMB release on LCD reset. */
+			e1e_rphy_locked(hw, I217_MEMPWR, &phy_reg);
+			phy_reg &= ~I217_MEMPWR;
+			e1e_wphy_locked(hw, I217_MEMPWR, phy_reg);
+		}
+
+		/*
+		 * Enable MTA to reset for Intel Rapid Start Technology
+		 * Support
+		 */
+		e1e_rphy_locked(hw, I217_CGFREG, &phy_reg);
+		phy_reg |= I217_CGFREG_MASK;
+		e1e_wphy_locked(hw, I217_CGFREG, phy_reg);
+
+release:
+		hw->phy.ops.release(hw);
+	}
+out:
 	ew32(PHY_CTRL, phy_ctrl);
 
 	if (hw->mac.type == e1000_ich8lan)
@@ -3704,44 +4138,61 @@ void e1000_suspend_workarounds_ich8lan(struct e1000_hw *hw)
  *  on which PHY resets are not blocked, if the PHY registers cannot be
  *  accessed properly by the s/w toggle the LANPHYPC value to power cycle
  *  the PHY.
+ *  On i217, setup Intel Rapid Start Technology.
  **/
 void e1000_resume_workarounds_pchlan(struct e1000_hw *hw)
 {
-	u16 phy_id1, phy_id2;
 	s32 ret_val;
 
-	if ((hw->mac.type != e1000_pch2lan) ||
-	    hw->phy.ops.check_reset_block(hw))
+	if (hw->mac.type < e1000_pch2lan)
 		return;
 
-	ret_val = hw->phy.ops.acquire(hw);
+	ret_val = e1000_init_phy_workarounds_pchlan(hw);
 	if (ret_val) {
-		e_dbg("Failed to acquire PHY semaphore in resume\n");
+		e_dbg("Failed to init PHY flow ret_val=%d\n", ret_val);
 		return;
 	}
 
-	/* Test access to the PHY registers by reading the ID regs */
-	ret_val = hw->phy.ops.read_reg_locked(hw, PHY_ID1, &phy_id1);
-	if (ret_val)
-		goto release;
-	ret_val = hw->phy.ops.read_reg_locked(hw, PHY_ID2, &phy_id2);
-	if (ret_val)
-		goto release;
-
-	if (hw->phy.id == ((u32)(phy_id1 << 16) |
-			   (u32)(phy_id2 & PHY_REVISION_MASK)))
-		goto release;
+	/*
+	 * For i217 Intel Rapid Start Technology support when the system
+	 * is transitioning from Sx and no manageability engine is present
+	 * configure SMBus to restore on reset, disable proxy, and enable
+	 * the reset on MTA (Multicast table array).
+	 */
+	if (hw->phy.type == e1000_phy_i217) {
+		u16 phy_reg;
 
-	e1000_toggle_lanphypc_value_ich8lan(hw);
+		ret_val = hw->phy.ops.acquire(hw);
+		if (ret_val) {
+			e_dbg("Failed to setup iRST\n");
+			return;
+		}
 
-	hw->phy.ops.release(hw);
-	msleep(50);
-	e1000_phy_hw_reset(hw);
-	msleep(50);
-	return;
+		if (!(er32(FWSM) & E1000_ICH_FWSM_FW_VALID)) {
+			/*
+			 * Restore clear on SMB if no manageability engine
+			 * is present
+			 */
+			ret_val = e1e_rphy_locked(hw, I217_MEMPWR, &phy_reg);
+			if (ret_val)
+				goto release;
+			phy_reg |= I217_MEMPWR_MASK;
+			e1e_wphy_locked(hw, I217_MEMPWR, phy_reg);
 
+			/* Disable Proxy */
+			e1e_wphy_locked(hw, I217_PROXY_CTRL, 0);
+		}
+		/* Enable reset on MTA */
+		ret_val = e1e_rphy_locked(hw, I217_CGFREG, &phy_reg);
+		if (ret_val)
+			goto release;
+		phy_reg &= ~I217_CGFREG_MASK;
+		e1e_wphy_locked(hw, I217_CGFREG, phy_reg);
 release:
-	hw->phy.ops.release(hw);
+		if (ret_val)
+			e_dbg("Error %d in resume workarounds\n", ret_val);
+		hw->phy.ops.release(hw);
+	}
 }
 
 /**
@@ -3921,7 +4372,7 @@ static s32 e1000_get_cfg_done_ich8lan(struct e1000_hw *hw)
 
 	/* If EEPROM is not marked present, init the IGP 3 PHY manually */
 	if (hw->mac.type <= e1000_ich9lan) {
-		if (((er32(EECD) & E1000_EECD_PRES) == 0) &&
+		if (!(er32(EECD) & E1000_EECD_PRES) &&
 		    (hw->phy.type == e1000_phy_igp_3)) {
 			e1000e_phy_init_script_igp3(hw);
 		}
@@ -3982,6 +4433,7 @@ static void e1000_clear_hw_cntrs_ich8lan(struct e1000_hw *hw)
 	/* Clear PHY statistics registers */
 	if ((hw->phy.type == e1000_phy_82578) ||
 	    (hw->phy.type == e1000_phy_82579) ||
+	    (hw->phy.type == e1000_phy_i217) ||
 	    (hw->phy.type == e1000_phy_82577)) {
 		ret_val = hw->phy.ops.acquire(hw);
 		if (ret_val)
@@ -4026,6 +4478,7 @@ static const struct e1000_mac_operations ich8_mac_ops = {
 	.setup_physical_interface= e1000_setup_copper_link_ich8lan,
 	/* id_led_init dependent on mac type */
 	.config_collision_dist	= e1000e_config_collision_dist_generic,
+	.rar_set		= e1000e_rar_set_generic,
 };
 
 static const struct e1000_phy_operations ich8_phy_ops = {
@@ -4140,3 +4593,22 @@ const struct e1000_info e1000_pch2_info = {
 	.phy_ops		= &ich8_phy_ops,
 	.nvm_ops		= &ich8_nvm_ops,
 };
+
+const struct e1000_info e1000_pch_lpt_info = {
+	.mac			= e1000_pch_lpt,
+	.flags			= FLAG_IS_ICH
+				  | FLAG_HAS_WOL
+				  | FLAG_HAS_CTRLEXT_ON_LOAD
+				  | FLAG_HAS_AMT
+				  | FLAG_HAS_FLASH
+				  | FLAG_HAS_JUMBO_FRAMES
+				  | FLAG_APME_IN_WUC,
+	.flags2			= FLAG2_HAS_PHY_STATS
+				  | FLAG2_HAS_EEE,
+	.pba			= 26,
+	.max_hw_frame_size	= DEFAULT_JUMBO,
+	.get_variants		= e1000_get_variants_ich8lan,
+	.mac_ops		= &ich8_mac_ops,
+	.phy_ops		= &ich8_phy_ops,
+	.nvm_ops		= &ich8_nvm_ops,
+};
diff --git a/drivers/net/ethernet/intel/e1000e/mac.c b/drivers/net/ethernet/intel/e1000e/mac.c
index decad98c105..026e8b3ab52 100644
--- a/drivers/net/ethernet/intel/e1000e/mac.c
+++ b/drivers/net/ethernet/intel/e1000e/mac.c
@@ -143,12 +143,12 @@ void e1000e_init_rx_addrs(struct e1000_hw *hw, u16 rar_count)
 	/* Setup the receive address */
 	e_dbg("Programming MAC Address into RAR[0]\n");
 
-	e1000e_rar_set(hw, hw->mac.addr, 0);
+	hw->mac.ops.rar_set(hw, hw->mac.addr, 0);
 
 	/* Zero out the other (rar_entry_count - 1) receive addresses */
 	e_dbg("Clearing RAR[1-%u]\n", rar_count - 1);
 	for (i = 1; i < rar_count; i++)
-		e1000e_rar_set(hw, mac_addr, i);
+		hw->mac.ops.rar_set(hw, mac_addr, i);
 }
 
 /**
@@ -215,13 +215,13 @@ s32 e1000_check_alt_mac_addr_generic(struct e1000_hw *hw)
 	 * same as the normal permanent MAC address stored by the HW into the
 	 * RAR. Do this by mapping this address into RAR0.
 	 */
-	e1000e_rar_set(hw, alt_mac_addr, 0);
+	hw->mac.ops.rar_set(hw, alt_mac_addr, 0);
 
 	return 0;
 }
 
 /**
- *  e1000e_rar_set - Set receive address register
+ *  e1000e_rar_set_generic - Set receive address register
  *  @hw: pointer to the HW structure
  *  @addr: pointer to the receive address
  *  @index: receive address array register
@@ -229,7 +229,7 @@ s32 e1000_check_alt_mac_addr_generic(struct e1000_hw *hw)
  *  Sets the receive address array register at index to the address passed
  *  in by addr.
  **/
-void e1000e_rar_set(struct e1000_hw *hw, u8 *addr, u32 index)
+void e1000e_rar_set_generic(struct e1000_hw *hw, u8 *addr, u32 index)
 {
 	u32 rar_low, rar_high;
 
@@ -681,7 +681,7 @@ static s32 e1000_set_default_fc_generic(struct e1000_hw *hw)
 		return ret_val;
 	}
 
-	if ((nvm_data & NVM_WORD0F_PAUSE_MASK) == 0)
+	if (!(nvm_data & NVM_WORD0F_PAUSE_MASK))
 		hw->fc.requested_mode = e1000_fc_none;
 	else if ((nvm_data & NVM_WORD0F_PAUSE_MASK) == NVM_WORD0F_ASM_DIR)
 		hw->fc.requested_mode = e1000_fc_tx_pause;
diff --git a/drivers/net/ethernet/intel/e1000e/manage.c b/drivers/net/ethernet/intel/e1000e/manage.c
index 473f8e71151..bacc950fc68 100644
--- a/drivers/net/ethernet/intel/e1000e/manage.c
+++ b/drivers/net/ethernet/intel/e1000e/manage.c
@@ -85,7 +85,7 @@ static s32 e1000_mng_enable_host_if(struct e1000_hw *hw)
 
 	/* Check that the host interface is enabled. */
 	hicr = er32(HICR);
-	if ((hicr & E1000_HICR_EN) == 0) {
+	if (!(hicr & E1000_HICR_EN)) {
 		e_dbg("E1000_HOST_EN bit disabled.\n");
 		return -E1000_ERR_HOST_INTERFACE_COMMAND;
 	}
diff --git a/drivers/net/ethernet/intel/e1000e/netdev.c b/drivers/net/ethernet/intel/e1000e/netdev.c
index 9520a6ac1f3..a4b0435b00d 100644
--- a/drivers/net/ethernet/intel/e1000e/netdev.c
+++ b/drivers/net/ethernet/intel/e1000e/netdev.c
@@ -56,7 +56,7 @@
 
 #define DRV_EXTRAVERSION "-k"
 
-#define DRV_VERSION "1.9.5" DRV_EXTRAVERSION
+#define DRV_VERSION "2.0.0" DRV_EXTRAVERSION
 char e1000e_driver_name[] = "e1000e";
 const char e1000e_driver_version[] = DRV_VERSION;
 
@@ -79,6 +79,7 @@ static const struct e1000_info *e1000_info_tbl[] = {
 	[board_ich10lan]	= &e1000_ich10_info,
 	[board_pchlan]		= &e1000_pch_info,
 	[board_pch2lan]		= &e1000_pch2_info,
+	[board_pch_lpt]		= &e1000_pch_lpt_info,
 };
 
 struct e1000_reg_info {
@@ -110,14 +111,14 @@ static const struct e1000_reg_info e1000_reg_info_tbl[] = {
 
 	/* Rx Registers */
 	{E1000_RCTL, "RCTL"},
-	{E1000_RDLEN, "RDLEN"},
-	{E1000_RDH, "RDH"},
-	{E1000_RDT, "RDT"},
+	{E1000_RDLEN(0), "RDLEN"},
+	{E1000_RDH(0), "RDH"},
+	{E1000_RDT(0), "RDT"},
 	{E1000_RDTR, "RDTR"},
 	{E1000_RXDCTL(0), "RXDCTL"},
 	{E1000_ERT, "ERT"},
-	{E1000_RDBAL, "RDBAL"},
-	{E1000_RDBAH, "RDBAH"},
+	{E1000_RDBAL(0), "RDBAL"},
+	{E1000_RDBAH(0), "RDBAH"},
 	{E1000_RDFH, "RDFH"},
 	{E1000_RDFT, "RDFT"},
 	{E1000_RDFHS, "RDFHS"},
@@ -126,11 +127,11 @@ static const struct e1000_reg_info e1000_reg_info_tbl[] = {
 
 	/* Tx Registers */
 	{E1000_TCTL, "TCTL"},
-	{E1000_TDBAL, "TDBAL"},
-	{E1000_TDBAH, "TDBAH"},
-	{E1000_TDLEN, "TDLEN"},
-	{E1000_TDH, "TDH"},
-	{E1000_TDT, "TDT"},
+	{E1000_TDBAL(0), "TDBAL"},
+	{E1000_TDBAH(0), "TDBAH"},
+	{E1000_TDLEN(0), "TDLEN"},
+	{E1000_TDH(0), "TDH"},
+	{E1000_TDT(0), "TDT"},
 	{E1000_TIDV, "TIDV"},
 	{E1000_TXDCTL(0), "TXDCTL"},
 	{E1000_TADV, "TADV"},
@@ -538,43 +539,15 @@ static void e1000_rx_checksum(struct e1000_adapter *adapter, u32 status_err,
 	adapter->hw_csum_good++;
 }
 
-/**
- * e1000e_update_tail_wa - helper function for e1000e_update_[rt]dt_wa()
- * @hw: pointer to the HW structure
- * @tail: address of tail descriptor register
- * @i: value to write to tail descriptor register
- *
- * When updating the tail register, the ME could be accessing Host CSR
- * registers at the same time.  Normally, this is handled in h/w by an
- * arbiter but on some parts there is a bug that acknowledges Host accesses
- * later than it should which could result in the descriptor register to
- * have an incorrect value.  Workaround this by checking the FWSM register
- * which has bit 24 set while ME is accessing Host CSR registers, wait
- * if it is set and try again a number of times.
- **/
-static inline s32 e1000e_update_tail_wa(struct e1000_hw *hw, void __iomem *tail,
-					unsigned int i)
-{
-	unsigned int j = 0;
-
-	while ((j++ < E1000_ICH_FWSM_PCIM2PCI_COUNT) &&
-	       (er32(FWSM) & E1000_ICH_FWSM_PCIM2PCI))
-		udelay(50);
-
-	writel(i, tail);
-
-	if ((j == E1000_ICH_FWSM_PCIM2PCI_COUNT) && (i != readl(tail)))
-		return E1000_ERR_SWFW_SYNC;
-
-	return 0;
-}
-
 static void e1000e_update_rdt_wa(struct e1000_ring *rx_ring, unsigned int i)
 {
 	struct e1000_adapter *adapter = rx_ring->adapter;
 	struct e1000_hw *hw = &adapter->hw;
+	s32 ret_val = __ew32_prepare(hw);
 
-	if (e1000e_update_tail_wa(hw, rx_ring->tail, i)) {
+	writel(i, rx_ring->tail);
+
+	if (unlikely(!ret_val && (i != readl(rx_ring->tail)))) {
 		u32 rctl = er32(RCTL);
 		ew32(RCTL, rctl & ~E1000_RCTL_EN);
 		e_err("ME firmware caused invalid RDT - resetting\n");
@@ -586,8 +559,11 @@ static void e1000e_update_tdt_wa(struct e1000_ring *tx_ring, unsigned int i)
 {
 	struct e1000_adapter *adapter = tx_ring->adapter;
 	struct e1000_hw *hw = &adapter->hw;
+	s32 ret_val = __ew32_prepare(hw);
+
+	writel(i, tx_ring->tail);
 
-	if (e1000e_update_tail_wa(hw, tx_ring->tail, i)) {
+	if (unlikely(!ret_val && (i != readl(tx_ring->tail)))) {
 		u32 tctl = er32(TCTL);
 		ew32(TCTL, tctl & ~E1000_TCTL_EN);
 		e_err("ME firmware caused invalid TDT - resetting\n");
@@ -1053,7 +1029,8 @@ static void e1000_print_hw_hang(struct work_struct *work)
 
 	if (!adapter->tx_hang_recheck &&
 	    (adapter->flags2 & FLAG2_DMA_BURST)) {
-		/* May be block on write-back, flush and detect again
+		/*
+		 * May be block on write-back, flush and detect again
 		 * flush pending descriptor writebacks to memory
 		 */
 		ew32(TIDV, adapter->tx_int_delay | E1000_TIDV_FPD);
@@ -1108,6 +1085,10 @@ static void e1000_print_hw_hang(struct work_struct *work)
 	      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))
+		e_err("Try turning off Tx pause (flow control) via ethtool\n");
 }
 
 /**
@@ -1645,7 +1626,10 @@ static void e1000_clean_rx_ring(struct e1000_ring *rx_ring)
 	adapter->flags2 &= ~FLAG2_IS_DISCARDING;
 
 	writel(0, rx_ring->head);
-	writel(0, rx_ring->tail);
+	if (rx_ring->adapter->flags2 & FLAG2_PCIM2PCI_ARBITER_WA)
+		e1000e_update_rdt_wa(rx_ring, 0);
+	else
+		writel(0, rx_ring->tail);
 }
 
 static void e1000e_downshift_workaround(struct work_struct *work)
@@ -2318,7 +2302,10 @@ static void e1000_clean_tx_ring(struct e1000_ring *tx_ring)
 	tx_ring->next_to_clean = 0;
 
 	writel(0, tx_ring->head);
-	writel(0, tx_ring->tail);
+	if (tx_ring->adapter->flags2 & FLAG2_PCIM2PCI_ARBITER_WA)
+		e1000e_update_tdt_wa(tx_ring, 0);
+	else
+		writel(0, tx_ring->tail);
 }
 
 /**
@@ -2530,33 +2517,31 @@ err:
 }
 
 /**
- * e1000_clean - NAPI Rx polling callback
+ * e1000e_poll - NAPI Rx polling callback
  * @napi: struct associated with this polling callback
- * @budget: amount of packets driver is allowed to process this poll
+ * @weight: number of packets driver is allowed to process this poll
  **/
-static int e1000_clean(struct napi_struct *napi, int budget)
+static int e1000e_poll(struct napi_struct *napi, int weight)
 {
-	struct e1000_adapter *adapter = container_of(napi, struct e1000_adapter, napi);
+	struct e1000_adapter *adapter = container_of(napi, struct e1000_adapter,
+						     napi);
 	struct e1000_hw *hw = &adapter->hw;
 	struct net_device *poll_dev = adapter->netdev;
 	int tx_cleaned = 1, work_done = 0;
 
 	adapter = netdev_priv(poll_dev);
 
-	if (adapter->msix_entries &&
-	    !(adapter->rx_ring->ims_val & adapter->tx_ring->ims_val))
-		goto clean_rx;
-
-	tx_cleaned = e1000_clean_tx_irq(adapter->tx_ring);
+	if (!adapter->msix_entries ||
+	    (adapter->rx_ring->ims_val & adapter->tx_ring->ims_val))
+		tx_cleaned = e1000_clean_tx_irq(adapter->tx_ring);
 
-clean_rx:
-	adapter->clean_rx(adapter->rx_ring, &work_done, budget);
+	adapter->clean_rx(adapter->rx_ring, &work_done, weight);
 
 	if (!tx_cleaned)
-		work_done = budget;
+		work_done = weight;
 
-	/* If budget not fully consumed, exit the polling mode */
-	if (work_done < budget) {
+	/* If weight not fully consumed, exit the polling mode */
+	if (work_done < weight) {
 		if (adapter->itr_setting & 3)
 			e1000_set_itr(adapter);
 		napi_complete(napi);
@@ -2800,13 +2785,13 @@ static void e1000_configure_tx(struct e1000_adapter *adapter)
 	/* Setup the HW Tx Head and Tail descriptor pointers */
 	tdba = tx_ring->dma;
 	tdlen = tx_ring->count * sizeof(struct e1000_tx_desc);
-	ew32(TDBAL, (tdba & DMA_BIT_MASK(32)));
-	ew32(TDBAH, (tdba >> 32));
-	ew32(TDLEN, tdlen);
-	ew32(TDH, 0);
-	ew32(TDT, 0);
-	tx_ring->head = adapter->hw.hw_addr + E1000_TDH;
-	tx_ring->tail = adapter->hw.hw_addr + E1000_TDT;
+	ew32(TDBAL(0), (tdba & DMA_BIT_MASK(32)));
+	ew32(TDBAH(0), (tdba >> 32));
+	ew32(TDLEN(0), tdlen);
+	ew32(TDH(0), 0);
+	ew32(TDT(0), 0);
+	tx_ring->head = adapter->hw.hw_addr + E1000_TDH(0);
+	tx_ring->tail = adapter->hw.hw_addr + E1000_TDT(0);
 
 	/* Set the Tx Interrupt Delay register */
 	ew32(TIDV, adapter->tx_int_delay);
@@ -2879,8 +2864,8 @@ static void e1000_setup_rctl(struct e1000_adapter *adapter)
 	u32 rctl, rfctl;
 	u32 pages = 0;
 
-	/* Workaround Si errata on 82579 - configure jumbo frame flow */
-	if (hw->mac.type == e1000_pch2lan) {
+	/* Workaround Si errata on PCHx - configure jumbo frame flow */
+	if (hw->mac.type >= e1000_pch2lan) {
 		s32 ret_val;
 
 		if (adapter->netdev->mtu > ETH_DATA_LEN)
@@ -2955,6 +2940,7 @@ static void e1000_setup_rctl(struct e1000_adapter *adapter)
 	/* Enable Extended Status in all Receive Descriptors */
 	rfctl = er32(RFCTL);
 	rfctl |= E1000_RFCTL_EXTEN;
+	ew32(RFCTL, rfctl);
 
 	/*
 	 * 82571 and greater support packet-split where the protocol
@@ -2980,13 +2966,6 @@ static void e1000_setup_rctl(struct e1000_adapter *adapter)
 	if (adapter->rx_ps_pages) {
 		u32 psrctl = 0;
 
-		/*
-		 * disable packet split support for IPv6 extension headers,
-		 * because some malformed IPv6 headers can hang the Rx
-		 */
-		rfctl |= (E1000_RFCTL_IPV6_EX_DIS |
-			  E1000_RFCTL_NEW_IPV6_EXT_DIS);
-
 		/* Enable Packet split descriptors */
 		rctl |= E1000_RCTL_DTYP_PS;
 
@@ -3025,7 +3004,6 @@ static void e1000_setup_rctl(struct e1000_adapter *adapter)
 		 */
 	}
 
-	ew32(RFCTL, rfctl);
 	ew32(RCTL, rctl);
 	/* just started the receive unit, no need to restart */
 	adapter->flags &= ~FLAG_RX_RESTART_NOW;
@@ -3110,13 +3088,13 @@ static void e1000_configure_rx(struct e1000_adapter *adapter)
 	 * the Base and Length of the Rx Descriptor Ring
 	 */
 	rdba = rx_ring->dma;
-	ew32(RDBAL, (rdba & DMA_BIT_MASK(32)));
-	ew32(RDBAH, (rdba >> 32));
-	ew32(RDLEN, rdlen);
-	ew32(RDH, 0);
-	ew32(RDT, 0);
-	rx_ring->head = adapter->hw.hw_addr + E1000_RDH;
-	rx_ring->tail = adapter->hw.hw_addr + E1000_RDT;
+	ew32(RDBAL(0), (rdba & DMA_BIT_MASK(32)));
+	ew32(RDBAH(0), (rdba >> 32));
+	ew32(RDLEN(0), rdlen);
+	ew32(RDH(0), 0);
+	ew32(RDT(0), 0);
+	rx_ring->head = adapter->hw.hw_addr + E1000_RDH(0);
+	rx_ring->tail = adapter->hw.hw_addr + E1000_RDT(0);
 
 	/* Enable Receive Checksum Offload for TCP and UDP */
 	rxcsum = er32(RXCSUM);
@@ -3229,7 +3207,7 @@ static int e1000e_write_uc_addr_list(struct net_device *netdev)
 		netdev_for_each_uc_addr(ha, netdev) {
 			if (!rar_entries)
 				break;
-			e1000e_rar_set(hw, ha->addr, rar_entries--);
+			hw->mac.ops.rar_set(hw, ha->addr, rar_entries--);
 			count++;
 		}
 	}
@@ -3510,6 +3488,7 @@ void e1000e_reset(struct e1000_adapter *adapter)
 		fc->refresh_time = 0x1000;
 		break;
 	case e1000_pch2lan:
+	case e1000_pch_lpt:
 		fc->high_water = 0x05C20;
 		fc->low_water = 0x05048;
 		fc->pause_time = 0x0650;
@@ -4038,6 +4017,7 @@ static int e1000_close(struct net_device *netdev)
 static int e1000_set_mac(struct net_device *netdev, void *p)
 {
 	struct e1000_adapter *adapter = netdev_priv(netdev);
+	struct e1000_hw *hw = &adapter->hw;
 	struct sockaddr *addr = p;
 
 	if (!is_valid_ether_addr(addr->sa_data))
@@ -4046,7 +4026,7 @@ static int e1000_set_mac(struct net_device *netdev, void *p)
 	memcpy(netdev->dev_addr, addr->sa_data, netdev->addr_len);
 	memcpy(adapter->hw.mac.addr, addr->sa_data, netdev->addr_len);
 
-	e1000e_rar_set(&adapter->hw, adapter->hw.mac.addr, 0);
+	hw->mac.ops.rar_set(&adapter->hw, adapter->hw.mac.addr, 0);
 
 	if (adapter->flags & FLAG_RESET_OVERWRITES_LAA) {
 		/* activate the work around */
@@ -4060,9 +4040,8 @@ static int e1000_set_mac(struct net_device *netdev, void *p)
 		 * are dropped. Eventually the LAA will be in RAR[0] and
 		 * RAR[14]
 		 */
-		e1000e_rar_set(&adapter->hw,
-			      adapter->hw.mac.addr,
-			      adapter->hw.mac.rar_entry_count - 1);
+		hw->mac.ops.rar_set(&adapter->hw, adapter->hw.mac.addr,
+				    adapter->hw.mac.rar_entry_count - 1);
 	}
 
 	return 0;
@@ -4641,7 +4620,7 @@ link_up:
 	 * reset from the other port. Set the appropriate LAA in RAR[0]
 	 */
 	if (e1000e_get_laa_state_82571(hw))
-		e1000e_rar_set(hw, adapter->hw.mac.addr, 0);
+		hw->mac.ops.rar_set(hw, adapter->hw.mac.addr, 0);
 
 	if (adapter->flags2 & FLAG2_CHECK_PHY_HANG)
 		e1000e_check_82574_phy_workaround(adapter);
@@ -5151,6 +5130,8 @@ static netdev_tx_t e1000_xmit_frame(struct sk_buff *skb,
 	/* if count is 0 then mapping error has occurred */
 	count = e1000_tx_map(tx_ring, skb, first, max_per_txd, nr_frags, mss);
 	if (count) {
+		skb_tx_timestamp(skb);
+
 		netdev_sent_queue(netdev, skb->len);
 		e1000_tx_queue(tx_ring, tx_flags, count);
 		/* Make sure there is space in the ring for the next send. */
@@ -5285,22 +5266,14 @@ static int e1000_change_mtu(struct net_device *netdev, int new_mtu)
 		return -EINVAL;
 	}
 
-	/* Jumbo frame workaround on 82579 requires CRC be stripped */
-	if ((adapter->hw.mac.type == e1000_pch2lan) &&
+	/* Jumbo frame workaround on 82579 and newer requires CRC be stripped */
+	if ((adapter->hw.mac.type >= e1000_pch2lan) &&
 	    !(adapter->flags2 & FLAG2_CRC_STRIPPING) &&
 	    (new_mtu > ETH_DATA_LEN)) {
-		e_err("Jumbo Frames not supported on 82579 when CRC stripping is disabled.\n");
+		e_err("Jumbo Frames not supported on this device when CRC stripping is disabled.\n");
 		return -EINVAL;
 	}
 
-	/* 82573 Errata 17 */
-	if (((adapter->hw.mac.type == e1000_82573) ||
-	     (adapter->hw.mac.type == e1000_82574)) &&
-	    (max_frame > ETH_FRAME_LEN + ETH_FCS_LEN)) {
-		adapter->flags2 |= FLAG2_DISABLE_ASPM_L1;
-		e1000e_disable_aspm(adapter->pdev, PCIE_LINK_STATE_L1);
-	}
-
 	while (test_and_set_bit(__E1000_RESETTING, &adapter->state))
 		usleep_range(1000, 2000);
 	/* e1000e_down -> e1000e_reset dependent on max_frame_size & mtu */
@@ -5694,7 +5667,7 @@ static int __e1000_resume(struct pci_dev *pdev)
 			return err;
 	}
 
-	if (hw->mac.type == e1000_pch2lan)
+	if (hw->mac.type >= e1000_pch2lan)
 		e1000_resume_workarounds_pchlan(&adapter->hw);
 
 	e1000e_power_up_phy(adapter);
@@ -6226,7 +6199,7 @@ static int __devinit e1000_probe(struct pci_dev *pdev,
 	netdev->netdev_ops		= &e1000e_netdev_ops;
 	e1000e_set_ethtool_ops(netdev);
 	netdev->watchdog_timeo		= 5 * HZ;
-	netif_napi_add(netdev, &adapter->napi, e1000_clean, 64);
+	netif_napi_add(netdev, &adapter->napi, e1000e_poll, 64);
 	strlcpy(netdev->name, pci_name(pdev), sizeof(netdev->name));
 
 	netdev->mem_start = mmio_start;
@@ -6593,6 +6566,9 @@ static DEFINE_PCI_DEVICE_TABLE(e1000_pci_tbl) = {
 	{ PCI_VDEVICE(INTEL, E1000_DEV_ID_PCH2_LV_LM), board_pch2lan },
 	{ PCI_VDEVICE(INTEL, E1000_DEV_ID_PCH2_LV_V), board_pch2lan },
 
+	{ PCI_VDEVICE(INTEL, E1000_DEV_ID_PCH_LPT_I217_LM), board_pch_lpt },
+	{ PCI_VDEVICE(INTEL, E1000_DEV_ID_PCH_LPT_I217_V), board_pch_lpt },
+
 	{ 0, 0, 0, 0, 0, 0, 0 }	/* terminate list */
 };
 MODULE_DEVICE_TABLE(pci, e1000_pci_tbl);
diff --git a/drivers/net/ethernet/intel/e1000e/param.c b/drivers/net/ethernet/intel/e1000e/param.c
index 16adeb9418a..55cc1565bc2 100644
--- a/drivers/net/ethernet/intel/e1000e/param.c
+++ b/drivers/net/ethernet/intel/e1000e/param.c
@@ -166,8 +166,8 @@ E1000_PARAM(WriteProtectNVM, "Write-protect NVM [WARNING: disabling this can lea
  *
  * Default Value: 1 (enabled)
  */
-E1000_PARAM(CrcStripping, "Enable CRC Stripping, disable if your BMC needs " \
-                          "the CRC");
+E1000_PARAM(CrcStripping,
+	    "Enable CRC Stripping, disable if your BMC needs the CRC");
 
 struct e1000_option {
 	enum { enable_option, range_option, list_option } type;
@@ -347,8 +347,8 @@ void __devinit e1000e_check_options(struct e1000_adapter *adapter)
 
 			/*
 			 * Make sure a message is printed for non-special
-			 * values.  And in case of an invalid option, display
-			 * warning, use default and got through itr/itr_setting
+			 * values. And in case of an invalid option, display
+			 * warning, use default and go through itr/itr_setting
 			 * adjustment logic below
 			 */
 			if ((adapter->itr > 4) &&
@@ -365,7 +365,7 @@ void __devinit e1000e_check_options(struct e1000_adapter *adapter)
 			 * Make sure a message is printed for non-special
 			 * default values
 			 */
-			if (adapter->itr > 40)
+			if (adapter->itr > 4)
 				e_info("%s set to default %d\n", opt.name,
 				       adapter->itr);
 		}
diff --git a/drivers/net/ethernet/intel/e1000e/phy.c b/drivers/net/ethernet/intel/e1000e/phy.c
index 35b45578c60..0334d013bc3 100644
--- a/drivers/net/ethernet/intel/e1000e/phy.c
+++ b/drivers/net/ethernet/intel/e1000e/phy.c
@@ -639,6 +639,45 @@ s32 e1000e_write_kmrn_reg_locked(struct e1000_hw *hw, u32 offset, u16 data)
 }
 
 /**
+ *  e1000_set_master_slave_mode - Setup PHY for Master/slave mode
+ *  @hw: pointer to the HW structure
+ *
+ *  Sets up Master/slave mode
+ **/
+static s32 e1000_set_master_slave_mode(struct e1000_hw *hw)
+{
+	s32 ret_val;
+	u16 phy_data;
+
+	/* Resolve Master/Slave mode */
+	ret_val = e1e_rphy(hw, PHY_1000T_CTRL, &phy_data);
+	if (ret_val)
+		return ret_val;
+
+	/* load defaults for future use */
+	hw->phy.original_ms_type = (phy_data & CR_1000T_MS_ENABLE) ?
+	    ((phy_data & CR_1000T_MS_VALUE) ?
+	     e1000_ms_force_master : e1000_ms_force_slave) : e1000_ms_auto;
+
+	switch (hw->phy.ms_type) {
+	case e1000_ms_force_master:
+		phy_data |= (CR_1000T_MS_ENABLE | CR_1000T_MS_VALUE);
+		break;
+	case e1000_ms_force_slave:
+		phy_data |= CR_1000T_MS_ENABLE;
+		phy_data &= ~(CR_1000T_MS_VALUE);
+		break;
+	case e1000_ms_auto:
+		phy_data &= ~CR_1000T_MS_ENABLE;
+		/* fall-through */
+	default:
+		break;
+	}
+
+	return e1e_wphy(hw, PHY_1000T_CTRL, phy_data);
+}
+
+/**
  *  e1000_copper_link_setup_82577 - Setup 82577 PHY for copper link
  *  @hw: pointer to the HW structure
  *
@@ -659,7 +698,11 @@ s32 e1000_copper_link_setup_82577(struct e1000_hw *hw)
 	/* Enable downshift */
 	phy_data |= I82577_CFG_ENABLE_DOWNSHIFT;
 
-	return e1e_wphy(hw, I82577_CFG_REG, phy_data);
+	ret_val = e1e_wphy(hw, I82577_CFG_REG, phy_data);
+	if (ret_val)
+		return ret_val;
+
+	return e1000_set_master_slave_mode(hw);
 }
 
 /**
@@ -718,12 +761,28 @@ s32 e1000e_copper_link_setup_m88(struct e1000_hw *hw)
 	 *   1 - Enabled
 	 */
 	phy_data &= ~M88E1000_PSCR_POLARITY_REVERSAL;
-	if (phy->disable_polarity_correction == 1)
+	if (phy->disable_polarity_correction)
 		phy_data |= M88E1000_PSCR_POLARITY_REVERSAL;
 
 	/* Enable downshift on BM (disabled by default) */
-	if (phy->type == e1000_phy_bm)
+	if (phy->type == e1000_phy_bm) {
+		/* For 82574/82583, first disable then enable downshift */
+		if (phy->id == BME1000_E_PHY_ID_R2) {
+			phy_data &= ~BME1000_PSCR_ENABLE_DOWNSHIFT;
+			ret_val = e1e_wphy(hw, M88E1000_PHY_SPEC_CTRL,
+					   phy_data);
+			if (ret_val)
+				return ret_val;
+			/* Commit the changes. */
+			ret_val = e1000e_commit_phy(hw);
+			if (ret_val) {
+				e_dbg("Error committing the PHY changes\n");
+				return ret_val;
+			}
+		}
+
 		phy_data |= BME1000_PSCR_ENABLE_DOWNSHIFT;
+	}
 
 	ret_val = e1e_wphy(hw, M88E1000_PHY_SPEC_CTRL, phy_data);
 	if (ret_val)
@@ -879,31 +938,7 @@ s32 e1000e_copper_link_setup_igp(struct e1000_hw *hw)
 				return ret_val;
 		}
 
-		ret_val = e1e_rphy(hw, PHY_1000T_CTRL, &data);
-		if (ret_val)
-			return ret_val;
-
-		/* load defaults for future use */
-		phy->original_ms_type = (data & CR_1000T_MS_ENABLE) ?
-			((data & CR_1000T_MS_VALUE) ?
-			e1000_ms_force_master :
-			e1000_ms_force_slave) :
-			e1000_ms_auto;
-
-		switch (phy->ms_type) {
-		case e1000_ms_force_master:
-			data |= (CR_1000T_MS_ENABLE | CR_1000T_MS_VALUE);
-			break;
-		case e1000_ms_force_slave:
-			data |= CR_1000T_MS_ENABLE;
-			data &= ~(CR_1000T_MS_VALUE);
-			break;
-		case e1000_ms_auto:
-			data &= ~CR_1000T_MS_ENABLE;
-		default:
-			break;
-		}
-		ret_val = e1e_wphy(hw, PHY_1000T_CTRL, data);
+		ret_val = e1000_set_master_slave_mode(hw);
 	}
 
 	return ret_val;
@@ -1090,7 +1125,7 @@ static s32 e1000_copper_link_autoneg(struct e1000_hw *hw)
 	 * 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)
+	if (!phy->autoneg_advertised)
 		phy->autoneg_advertised = phy->autoneg_mask;
 
 	e_dbg("Reconfiguring auto-neg advertisement params\n");
@@ -1596,7 +1631,7 @@ s32 e1000e_check_downshift(struct e1000_hw *hw)
 	ret_val = e1e_rphy(hw, offset, &phy_data);
 
 	if (!ret_val)
-		phy->speed_downgraded = (phy_data & mask);
+		phy->speed_downgraded = !!(phy_data & mask);
 
 	return ret_val;
 }
@@ -1925,8 +1960,8 @@ s32 e1000e_get_phy_info_m88(struct e1000_hw *hw)
 	if (ret_val)
 		return ret_val;
 
-	phy->polarity_correction = (phy_data &
-				    M88E1000_PSCR_POLARITY_REVERSAL);
+	phy->polarity_correction = !!(phy_data &
+				      M88E1000_PSCR_POLARITY_REVERSAL);
 
 	ret_val = e1000_check_polarity_m88(hw);
 	if (ret_val)
@@ -1936,7 +1971,7 @@ s32 e1000e_get_phy_info_m88(struct e1000_hw *hw)
 	if (ret_val)
 		return ret_val;
 
-	phy->is_mdix = (phy_data & M88E1000_PSSR_MDIX);
+	phy->is_mdix = !!(phy_data & M88E1000_PSSR_MDIX);
 
 	if ((phy_data & M88E1000_PSSR_SPEED) == M88E1000_PSSR_1000MBS) {
 		ret_val = e1000_get_cable_length(hw);
@@ -1999,7 +2034,7 @@ s32 e1000e_get_phy_info_igp(struct e1000_hw *hw)
 	if (ret_val)
 		return ret_val;
 
-	phy->is_mdix = (data & IGP01E1000_PSSR_MDIX);
+	phy->is_mdix = !!(data & IGP01E1000_PSSR_MDIX);
 
 	if ((data & IGP01E1000_PSSR_SPEED_MASK) ==
 	    IGP01E1000_PSSR_SPEED_1000MBPS) {
@@ -2052,8 +2087,7 @@ s32 e1000_get_phy_info_ife(struct e1000_hw *hw)
 	ret_val = e1e_rphy(hw, IFE_PHY_SPECIAL_CONTROL, &data);
 	if (ret_val)
 		return ret_val;
-	phy->polarity_correction = (data & IFE_PSC_AUTO_POLARITY_DISABLE)
-	                           ? false : true;
+	phy->polarity_correction = !(data & IFE_PSC_AUTO_POLARITY_DISABLE);
 
 	if (phy->polarity_correction) {
 		ret_val = e1000_check_polarity_ife(hw);
@@ -2070,7 +2104,7 @@ s32 e1000_get_phy_info_ife(struct e1000_hw *hw)
 	if (ret_val)
 		return ret_val;
 
-	phy->is_mdix = (data & IFE_PMC_MDIX_STATUS) ? true : false;
+	phy->is_mdix = !!(data & IFE_PMC_MDIX_STATUS);
 
 	/* The following parameters are undefined for 10/100 operation. */
 	phy->cable_length = E1000_CABLE_LENGTH_UNDEFINED;
@@ -2320,6 +2354,9 @@ enum e1000_phy_type e1000e_get_phy_type_from_id(u32 phy_id)
 	case I82579_E_PHY_ID:
 		phy_type = e1000_phy_82579;
 		break;
+	case I217_E_PHY_ID:
+		phy_type = e1000_phy_i217;
+		break;
 	default:
 		phy_type = e1000_phy_unknown;
 		break;
@@ -2979,7 +3016,7 @@ static s32 __e1000_write_phy_reg_hv(struct e1000_hw *hw, u32 offset, u16 data,
 		if ((hw->phy.type == e1000_phy_82578) &&
 		    (hw->phy.revision >= 1) &&
 		    (hw->phy.addr == 2) &&
-		    ((MAX_PHY_REG_ADDRESS & reg) == 0) && (data & (1 << 11))) {
+		    !(MAX_PHY_REG_ADDRESS & reg) && (data & (1 << 11))) {
 			u16 data2 = 0x7EFF;
 			ret_val = e1000_access_phy_debug_regs_hv(hw,
 								 (1 << 6) | 0x3,
@@ -3265,7 +3302,7 @@ s32 e1000_get_phy_info_82577(struct e1000_hw *hw)
 	if (ret_val)
 		return ret_val;
 
-	phy->is_mdix = (data & I82577_PHY_STATUS2_MDIX) ? true : false;
+	phy->is_mdix = !!(data & I82577_PHY_STATUS2_MDIX);
 
 	if ((data & I82577_PHY_STATUS2_SPEED_MASK) ==
 	    I82577_PHY_STATUS2_SPEED_1000MBPS) {
diff --git a/drivers/net/ethernet/intel/igb/Makefile b/drivers/net/ethernet/intel/igb/Makefile
index 6565c463185..97c197fd4a8 100644
--- a/drivers/net/ethernet/intel/igb/Makefile
+++ b/drivers/net/ethernet/intel/igb/Makefile
@@ -33,5 +33,7 @@
 obj-$(CONFIG_IGB) += igb.o
 
 igb-objs := igb_main.o igb_ethtool.o e1000_82575.o \
-	    e1000_mac.o e1000_nvm.o e1000_phy.o e1000_mbx.o
+	    e1000_mac.o e1000_nvm.o e1000_phy.o e1000_mbx.o \
+	    e1000_i210.o
 
+igb-$(CONFIG_IGB_PTP) += igb_ptp.o
diff --git a/drivers/net/ethernet/intel/igb/e1000_82575.c b/drivers/net/ethernet/intel/igb/e1000_82575.c
index 08bdc33715e..e6508395842 100644
--- a/drivers/net/ethernet/intel/igb/e1000_82575.c
+++ b/drivers/net/ethernet/intel/igb/e1000_82575.c
@@ -36,6 +36,7 @@
 
 #include "e1000_mac.h"
 #include "e1000_82575.h"
+#include "e1000_i210.h"
 
 static s32  igb_get_invariants_82575(struct e1000_hw *);
 static s32  igb_acquire_phy_82575(struct e1000_hw *);
@@ -52,6 +53,8 @@ static s32  igb_write_phy_reg_82580(struct e1000_hw *, u32, u16);
 static s32  igb_reset_hw_82575(struct e1000_hw *);
 static s32  igb_reset_hw_82580(struct e1000_hw *);
 static s32  igb_set_d0_lplu_state_82575(struct e1000_hw *, bool);
+static s32  igb_set_d0_lplu_state_82580(struct e1000_hw *, bool);
+static s32  igb_set_d3_lplu_state_82580(struct e1000_hw *, bool);
 static s32  igb_setup_copper_link_82575(struct e1000_hw *);
 static s32  igb_setup_serdes_link_82575(struct e1000_hw *);
 static s32  igb_write_phy_reg_sgmii_82575(struct e1000_hw *, u32, u16);
@@ -96,6 +99,8 @@ static bool igb_sgmii_uses_mdio_82575(struct e1000_hw *hw)
 		break;
 	case e1000_82580:
 	case e1000_i350:
+	case e1000_i210:
+	case e1000_i211:
 		reg = rd32(E1000_MDICNFG);
 		ext_mdio = !!(reg & E1000_MDICNFG_EXT_MDIO);
 		break;
@@ -150,6 +155,17 @@ static s32 igb_get_invariants_82575(struct e1000_hw *hw)
 	case E1000_DEV_ID_I350_SGMII:
 		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:
+		mac->type = e1000_i210;
+		break;
+	case E1000_DEV_ID_I211_COPPER:
+		mac->type = e1000_i211;
+		break;
 	default:
 		return -E1000_ERR_MAC_INIT;
 		break;
@@ -182,26 +198,44 @@ static s32 igb_get_invariants_82575(struct e1000_hw *hw)
 	/* Set mta register count */
 	mac->mta_reg_count = 128;
 	/* Set rar entry count */
-	mac->rar_entry_count = E1000_RAR_ENTRIES_82575;
-	if (mac->type == e1000_82576)
+	switch (mac->type) {
+	case e1000_82576:
 		mac->rar_entry_count = E1000_RAR_ENTRIES_82576;
-	if (mac->type == e1000_82580)
+		break;
+	case e1000_82580:
 		mac->rar_entry_count = E1000_RAR_ENTRIES_82580;
-	if (mac->type == e1000_i350)
+		break;
+	case e1000_i350:
+	case e1000_i210:
+	case e1000_i211:
 		mac->rar_entry_count = E1000_RAR_ENTRIES_I350;
+		break;
+	default:
+		mac->rar_entry_count = E1000_RAR_ENTRIES_82575;
+		break;
+	}
 	/* reset */
 	if (mac->type >= e1000_82580)
 		mac->ops.reset_hw = igb_reset_hw_82580;
 	else
 		mac->ops.reset_hw = igb_reset_hw_82575;
+
+	if (mac->type >= e1000_i210) {
+		mac->ops.acquire_swfw_sync = igb_acquire_swfw_sync_i210;
+		mac->ops.release_swfw_sync = igb_release_swfw_sync_i210;
+	} else {
+		mac->ops.acquire_swfw_sync = igb_acquire_swfw_sync_82575;
+		mac->ops.release_swfw_sync = igb_release_swfw_sync_82575;
+	}
+
 	/* Set if part includes ASF firmware */
 	mac->asf_firmware_present = true;
 	/* Set if manageability features are enabled. */
 	mac->arc_subsystem_valid =
 		(rd32(E1000_FWSM) & E1000_FWSM_MODE_MASK)
 			? true : false;
-	/* enable EEE on i350 parts */
-	if (mac->type == e1000_i350)
+	/* enable EEE on i350 parts and later parts */
+	if (mac->type >= e1000_i350)
 		dev_spec->eee_disable = false;
 	else
 		dev_spec->eee_disable = true;
@@ -213,26 +247,6 @@ static s32 igb_get_invariants_82575(struct e1000_hw *hw)
 
 	/* NVM initialization */
 	eecd = rd32(E1000_EECD);
-
-	nvm->opcode_bits        = 8;
-	nvm->delay_usec         = 1;
-	switch (nvm->override) {
-	case e1000_nvm_override_spi_large:
-		nvm->page_size    = 32;
-		nvm->address_bits = 16;
-		break;
-	case e1000_nvm_override_spi_small:
-		nvm->page_size    = 8;
-		nvm->address_bits = 8;
-		break;
-	default:
-		nvm->page_size    = eecd & E1000_EECD_ADDR_BITS ? 32 : 8;
-		nvm->address_bits = eecd & E1000_EECD_ADDR_BITS ? 16 : 8;
-		break;
-	}
-
-	nvm->type = e1000_nvm_eeprom_spi;
-
 	size = (u16)((eecd & E1000_EECD_SIZE_EX_MASK) >>
 		     E1000_EECD_SIZE_EX_SHIFT);
 
@@ -242,6 +256,33 @@ static s32 igb_get_invariants_82575(struct e1000_hw *hw)
 	 */
 	size += NVM_WORD_SIZE_BASE_SHIFT;
 
+	nvm->word_size = 1 << size;
+	if (hw->mac.type < e1000_i210) {
+		nvm->opcode_bits        = 8;
+		nvm->delay_usec         = 1;
+		switch (nvm->override) {
+		case e1000_nvm_override_spi_large:
+			nvm->page_size    = 32;
+			nvm->address_bits = 16;
+			break;
+		case e1000_nvm_override_spi_small:
+			nvm->page_size    = 8;
+			nvm->address_bits = 8;
+			break;
+		default:
+			nvm->page_size    = eecd
+				& E1000_EECD_ADDR_BITS ? 32 : 8;
+			nvm->address_bits = eecd
+				& E1000_EECD_ADDR_BITS ? 16 : 8;
+			break;
+		}
+		if (nvm->word_size == (1 << 15))
+			nvm->page_size = 128;
+
+		nvm->type = e1000_nvm_eeprom_spi;
+	} else
+		nvm->type = e1000_nvm_flash_hw;
+
 	/*
 	 * Check for invalid size
 	 */
@@ -249,32 +290,60 @@ static s32 igb_get_invariants_82575(struct e1000_hw *hw)
 		pr_notice("The NVM size is not valid, defaulting to 32K\n");
 		size = 15;
 	}
-	nvm->word_size = 1 << size;
-	if (nvm->word_size == (1 << 15))
-		nvm->page_size = 128;
 
 	/* NVM Function Pointers */
-	nvm->ops.acquire = igb_acquire_nvm_82575;
-	if (nvm->word_size < (1 << 15))
-		nvm->ops.read = igb_read_nvm_eerd;
-	else
-		nvm->ops.read = igb_read_nvm_spi;
-
-	nvm->ops.release = igb_release_nvm_82575;
 	switch (hw->mac.type) {
 	case e1000_82580:
 		nvm->ops.validate = igb_validate_nvm_checksum_82580;
 		nvm->ops.update = igb_update_nvm_checksum_82580;
+		nvm->ops.acquire = igb_acquire_nvm_82575;
+		nvm->ops.release = igb_release_nvm_82575;
+		if (nvm->word_size < (1 << 15))
+			nvm->ops.read = igb_read_nvm_eerd;
+		else
+			nvm->ops.read = igb_read_nvm_spi;
+		nvm->ops.write = igb_write_nvm_spi;
 		break;
 	case e1000_i350:
 		nvm->ops.validate = igb_validate_nvm_checksum_i350;
 		nvm->ops.update = igb_update_nvm_checksum_i350;
+		nvm->ops.acquire = igb_acquire_nvm_82575;
+		nvm->ops.release = igb_release_nvm_82575;
+		if (nvm->word_size < (1 << 15))
+			nvm->ops.read = igb_read_nvm_eerd;
+		else
+			nvm->ops.read = igb_read_nvm_spi;
+		nvm->ops.write = igb_write_nvm_spi;
+		break;
+	case e1000_i210:
+		nvm->ops.validate = igb_validate_nvm_checksum_i210;
+		nvm->ops.update   = igb_update_nvm_checksum_i210;
+		nvm->ops.acquire = igb_acquire_nvm_i210;
+		nvm->ops.release = igb_release_nvm_i210;
+		nvm->ops.read    = igb_read_nvm_srrd_i210;
+		nvm->ops.valid_led_default = igb_valid_led_default_i210;
+		break;
+	case e1000_i211:
+		nvm->ops.acquire  = igb_acquire_nvm_i210;
+		nvm->ops.release  = igb_release_nvm_i210;
+		nvm->ops.read     = igb_read_nvm_i211;
+		nvm->ops.valid_led_default = igb_valid_led_default_i210;
+		nvm->ops.validate = NULL;
+		nvm->ops.update   = NULL;
+		nvm->ops.write    = NULL;
 		break;
 	default:
 		nvm->ops.validate = igb_validate_nvm_checksum;
 		nvm->ops.update = igb_update_nvm_checksum;
+		nvm->ops.acquire = igb_acquire_nvm_82575;
+		nvm->ops.release = igb_release_nvm_82575;
+		if (nvm->word_size < (1 << 15))
+			nvm->ops.read = igb_read_nvm_eerd;
+		else
+			nvm->ops.read = igb_read_nvm_spi;
+		nvm->ops.write = igb_write_nvm_spi;
+		break;
 	}
-	nvm->ops.write = igb_write_nvm_spi;
 
 	/* if part supports SR-IOV then initialize mailbox parameters */
 	switch (mac->type) {
@@ -312,9 +381,13 @@ static s32 igb_get_invariants_82575(struct e1000_hw *hw)
 	if (igb_sgmii_active_82575(hw) && !igb_sgmii_uses_mdio_82575(hw)) {
 		phy->ops.read_reg   = igb_read_phy_reg_sgmii_82575;
 		phy->ops.write_reg  = igb_write_phy_reg_sgmii_82575;
-	} else if (hw->mac.type >= e1000_82580) {
+	} else if ((hw->mac.type == e1000_82580)
+		|| (hw->mac.type == e1000_i350)) {
 		phy->ops.read_reg   = igb_read_phy_reg_82580;
 		phy->ops.write_reg  = igb_write_phy_reg_82580;
+	} else if (hw->phy.type >= e1000_phy_i210) {
+		phy->ops.read_reg   = igb_read_phy_reg_gs40g;
+		phy->ops.write_reg  = igb_write_phy_reg_gs40g;
 	} else {
 		phy->ops.read_reg   = igb_read_phy_reg_igp;
 		phy->ops.write_reg  = igb_write_phy_reg_igp;
@@ -343,6 +416,14 @@ static s32 igb_get_invariants_82575(struct e1000_hw *hw)
 		else
 			phy->ops.get_cable_length = igb_get_cable_length_m88;
 
+		if (phy->id == I210_I_PHY_ID) {
+			phy->ops.get_cable_length =
+					 igb_get_cable_length_m88_gen2;
+			phy->ops.set_d0_lplu_state =
+					igb_set_d0_lplu_state_82580;
+			phy->ops.set_d3_lplu_state =
+					igb_set_d3_lplu_state_82580;
+		}
 		phy->ops.force_speed_duplex = igb_phy_force_speed_duplex_m88;
 		break;
 	case IGP03E1000_E_PHY_ID:
@@ -359,6 +440,17 @@ static s32 igb_get_invariants_82575(struct e1000_hw *hw)
 		phy->ops.force_speed_duplex = igb_phy_force_speed_duplex_82580;
 		phy->ops.get_cable_length   = igb_get_cable_length_82580;
 		phy->ops.get_phy_info       = igb_get_phy_info_82580;
+		phy->ops.set_d0_lplu_state  = igb_set_d0_lplu_state_82580;
+		phy->ops.set_d3_lplu_state  = igb_set_d3_lplu_state_82580;
+		break;
+	case I210_I_PHY_ID:
+		phy->type                   = e1000_phy_i210;
+		phy->ops.get_phy_info       = igb_get_phy_info_m88;
+		phy->ops.check_polarity     = igb_check_polarity_m88;
+		phy->ops.get_cable_length   = igb_get_cable_length_m88_gen2;
+		phy->ops.set_d0_lplu_state  = igb_set_d0_lplu_state_82580;
+		phy->ops.set_d3_lplu_state  = igb_set_d3_lplu_state_82580;
+		phy->ops.force_speed_duplex = igb_phy_force_speed_duplex_m88;
 		break;
 	default:
 		return -E1000_ERR_PHY;
@@ -385,7 +477,7 @@ static s32 igb_acquire_phy_82575(struct e1000_hw *hw)
 	else if (hw->bus.func == E1000_FUNC_3)
 		mask = E1000_SWFW_PHY3_SM;
 
-	return igb_acquire_swfw_sync_82575(hw, mask);
+	return hw->mac.ops.acquire_swfw_sync(hw, mask);
 }
 
 /**
@@ -406,7 +498,7 @@ static void igb_release_phy_82575(struct e1000_hw *hw)
 	else if (hw->bus.func == E1000_FUNC_3)
 		mask = E1000_SWFW_PHY3_SM;
 
-	igb_release_swfw_sync_82575(hw, mask);
+	hw->mac.ops.release_swfw_sync(hw, mask);
 }
 
 /**
@@ -510,6 +602,8 @@ static s32 igb_get_phy_id_82575(struct e1000_hw *hw)
 			break;
 		case e1000_82580:
 		case e1000_i350:
+		case e1000_i210:
+		case e1000_i211:
 			mdic = rd32(E1000_MDICNFG);
 			mdic &= E1000_MDICNFG_PHY_MASK;
 			phy->addr = mdic >> E1000_MDICNFG_PHY_SHIFT;
@@ -674,6 +768,96 @@ out:
 }
 
 /**
+ *  igb_set_d0_lplu_state_82580 - Set Low Power Linkup D0 state
+ *  @hw: pointer to the HW structure
+ *  @active: true to enable LPLU, false to disable
+ *
+ *  Sets the LPLU D0 state according to the active flag.  When
+ *  activating LPLU this function also disables smart speed
+ *  and vice versa.  LPLU will not be activated unless the
+ *  device autonegotiation advertisement meets standards of
+ *  either 10 or 10/100 or 10/100/1000 at all duplexes.
+ *  This is a function pointer entry point only called by
+ *  PHY setup routines.
+ **/
+static s32 igb_set_d0_lplu_state_82580(struct e1000_hw *hw, bool active)
+{
+	struct e1000_phy_info *phy = &hw->phy;
+	s32 ret_val = 0;
+	u16 data;
+
+	data = rd32(E1000_82580_PHY_POWER_MGMT);
+
+	if (active) {
+		data |= E1000_82580_PM_D0_LPLU;
+
+		/* When LPLU is enabled, we should disable SmartSpeed */
+		data &= ~E1000_82580_PM_SPD;
+	} else {
+		data &= ~E1000_82580_PM_D0_LPLU;
+
+		/*
+		 * 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.
+		 */
+		if (phy->smart_speed == e1000_smart_speed_on)
+			data |= E1000_82580_PM_SPD;
+		else if (phy->smart_speed == e1000_smart_speed_off)
+			data &= ~E1000_82580_PM_SPD; }
+
+	wr32(E1000_82580_PHY_POWER_MGMT, data);
+	return ret_val;
+}
+
+/**
+ *  igb_set_d3_lplu_state_82580 - Sets low power link up state for D3
+ *  @hw: pointer to the HW structure
+ *  @active: boolean used to enable/disable lplu
+ *
+ *  Success returns 0, Failure returns 1
+ *
+ *  The low power link up (lplu) state is set to the power management level D3
+ *  and SmartSpeed is disabled when active is true, else clear lplu for D3
+ *  and enable Smartspeed.  LPLU and Smartspeed are mutually exclusive.  LPLU
+ *  is used during Dx states where the power conservation is most important.
+ *  During driver activity, SmartSpeed should be enabled so performance is
+ *  maintained.
+ **/
+s32 igb_set_d3_lplu_state_82580(struct e1000_hw *hw, bool active)
+{
+	struct e1000_phy_info *phy = &hw->phy;
+	s32 ret_val = 0;
+	u16 data;
+
+	data = rd32(E1000_82580_PHY_POWER_MGMT);
+
+	if (!active) {
+		data &= ~E1000_82580_PM_D3_LPLU;
+		/*
+		 * 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.
+		 */
+		if (phy->smart_speed == e1000_smart_speed_on)
+			data |= E1000_82580_PM_SPD;
+		else if (phy->smart_speed == e1000_smart_speed_off)
+			data &= ~E1000_82580_PM_SPD;
+	} else if ((phy->autoneg_advertised == E1000_ALL_SPEED_DUPLEX) ||
+		   (phy->autoneg_advertised == E1000_ALL_NOT_GIG) ||
+		   (phy->autoneg_advertised == E1000_ALL_10_SPEED)) {
+		data |= E1000_82580_PM_D3_LPLU;
+		/* When LPLU is enabled, we should disable SmartSpeed */
+		data &= ~E1000_82580_PM_SPD;
+	}
+
+	wr32(E1000_82580_PHY_POWER_MGMT, data);
+	return ret_val;
+}
+
+/**
  *  igb_acquire_nvm_82575 - Request for access to EEPROM
  *  @hw: pointer to the HW structure
  *
@@ -686,14 +870,14 @@ static s32 igb_acquire_nvm_82575(struct e1000_hw *hw)
 {
 	s32 ret_val;
 
-	ret_val = igb_acquire_swfw_sync_82575(hw, E1000_SWFW_EEP_SM);
+	ret_val = hw->mac.ops.acquire_swfw_sync(hw, E1000_SWFW_EEP_SM);
 	if (ret_val)
 		goto out;
 
 	ret_val = igb_acquire_nvm(hw);
 
 	if (ret_val)
-		igb_release_swfw_sync_82575(hw, E1000_SWFW_EEP_SM);
+		hw->mac.ops.release_swfw_sync(hw, E1000_SWFW_EEP_SM);
 
 out:
 	return ret_val;
@@ -709,7 +893,7 @@ out:
 static void igb_release_nvm_82575(struct e1000_hw *hw)
 {
 	igb_release_nvm(hw);
-	igb_release_swfw_sync_82575(hw, E1000_SWFW_EEP_SM);
+	hw->mac.ops.release_swfw_sync(hw, E1000_SWFW_EEP_SM);
 }
 
 /**
@@ -1080,7 +1264,6 @@ static s32 igb_init_hw_82575(struct e1000_hw *hw)
 	 * is no link.
 	 */
 	igb_clear_hw_cntrs_82575(hw);
-
 	return ret_val;
 }
 
@@ -1117,6 +1300,7 @@ static s32 igb_setup_copper_link_82575(struct e1000_hw *hw)
 		}
 	}
 	switch (hw->phy.type) {
+	case e1000_phy_i210:
 	case e1000_phy_m88:
 		if (hw->phy.id == I347AT4_E_PHY_ID ||
 		    hw->phy.id == M88E1112_E_PHY_ID)
@@ -1757,7 +1941,7 @@ static s32 igb_reset_hw_82580(struct e1000_hw *hw)
 
 	/* Determine whether or not a global dev reset is requested */
 	if (global_device_reset &&
-		igb_acquire_swfw_sync_82575(hw, swmbsw_mask))
+		hw->mac.ops.acquire_swfw_sync(hw, swmbsw_mask))
 			global_device_reset = false;
 
 	if (global_device_reset &&
@@ -1803,7 +1987,7 @@ static s32 igb_reset_hw_82580(struct e1000_hw *hw)
 
 	/* Release semaphore */
 	if (global_device_reset)
-		igb_release_swfw_sync_82575(hw, swmbsw_mask);
+		hw->mac.ops.release_swfw_sync(hw, swmbsw_mask);
 
 	return ret_val;
 }
diff --git a/drivers/net/ethernet/intel/igb/e1000_82575.h b/drivers/net/ethernet/intel/igb/e1000_82575.h
index b927d79ab53..e85c453f542 100644
--- a/drivers/net/ethernet/intel/igb/e1000_82575.h
+++ b/drivers/net/ethernet/intel/igb/e1000_82575.h
@@ -55,10 +55,11 @@ extern void igb_rx_fifo_flush_82575(struct e1000_hw *hw);
 #define E1000_SRRCTL_DROP_EN                            0x80000000
 #define E1000_SRRCTL_TIMESTAMP                          0x40000000
 
+
 #define E1000_MRQC_ENABLE_RSS_4Q            0x00000002
 #define E1000_MRQC_ENABLE_VMDQ              0x00000003
-#define E1000_MRQC_ENABLE_VMDQ_RSS_2Q       0x00000005
 #define E1000_MRQC_RSS_FIELD_IPV4_UDP       0x00400000
+#define E1000_MRQC_ENABLE_VMDQ_RSS_2Q       0x00000005
 #define E1000_MRQC_RSS_FIELD_IPV6_UDP       0x00800000
 #define E1000_MRQC_RSS_FIELD_IPV6_UDP_EX    0x01000000
 
diff --git a/drivers/net/ethernet/intel/igb/e1000_defines.h b/drivers/net/ethernet/intel/igb/e1000_defines.h
index 89eb1f85b9f..ec7e4fe3e3e 100644
--- a/drivers/net/ethernet/intel/igb/e1000_defines.h
+++ b/drivers/net/ethernet/intel/igb/e1000_defines.h
@@ -301,6 +301,8 @@
 							* transactions */
 #define E1000_DMACR_DMAC_LX_SHIFT       28
 #define E1000_DMACR_DMAC_EN             0x80000000 /* Enable DMA Coalescing */
+/* DMA Coalescing BMC-to-OS Watchdog Enable */
+#define E1000_DMACR_DC_BMC2OSW_EN	0x00008000
 
 #define E1000_DMCTXTH_DMCTTHR_MASK      0x00000FFF /* DMA Coalescing Transmit
 							* Threshold */
@@ -458,6 +460,7 @@
 #define E1000_ERR_INVALID_ARGUMENT  16
 #define E1000_ERR_NO_SPACE          17
 #define E1000_ERR_NVM_PBA_SECTION   18
+#define E1000_ERR_INVM_VALUE_NOT_FOUND	19
 
 /* Loop limit on how long we wait for auto-negotiation to complete */
 #define COPPER_LINK_UP_LIMIT              10
@@ -595,6 +598,25 @@
 #define E1000_EECD_AUTO_RD          0x00000200  /* NVM Auto Read done */
 #define E1000_EECD_SIZE_EX_MASK     0x00007800  /* NVM Size */
 #define E1000_EECD_SIZE_EX_SHIFT     11
+#define E1000_EECD_FLUPD_I210		0x00800000 /* Update FLASH */
+#define E1000_EECD_FLUDONE_I210		0x04000000 /* Update FLASH done*/
+#define E1000_FLUDONE_ATTEMPTS		20000
+#define E1000_EERD_EEWR_MAX_COUNT	512 /* buffered EEPROM words rw */
+#define E1000_I210_FIFO_SEL_RX		0x00
+#define E1000_I210_FIFO_SEL_TX_QAV(_i)	(0x02 + (_i))
+#define E1000_I210_FIFO_SEL_TX_LEGACY	E1000_I210_FIFO_SEL_TX_QAV(0)
+#define E1000_I210_FIFO_SEL_BMC2OS_TX	0x06
+#define E1000_I210_FIFO_SEL_BMC2OS_RX	0x01
+#define E1000_EECD_FLUPD_I210		0x00800000 /* Update FLASH */
+#define E1000_EECD_FLUDONE_I210		0x04000000 /* Update FLASH done*/
+#define E1000_FLUDONE_ATTEMPTS		20000
+#define E1000_EERD_EEWR_MAX_COUNT	512 /* buffered EEPROM words rw */
+#define E1000_I210_FIFO_SEL_RX		0x00
+#define E1000_I210_FIFO_SEL_TX_QAV(_i)	(0x02 + (_i))
+#define E1000_I210_FIFO_SEL_TX_LEGACY	E1000_I210_FIFO_SEL_TX_QAV(0)
+#define E1000_I210_FIFO_SEL_BMC2OS_TX	0x06
+#define E1000_I210_FIFO_SEL_BMC2OS_RX	0x01
+
 
 /* Offset to data in NVM read/write registers */
 #define E1000_NVM_RW_REG_DATA   16
@@ -613,6 +635,16 @@
 #define NVM_CHECKSUM_REG           0x003F
 #define NVM_COMPATIBILITY_REG_3    0x0003
 #define NVM_COMPATIBILITY_BIT_MASK 0x8000
+#define NVM_MAC_ADDR               0x0000
+#define NVM_SUB_DEV_ID             0x000B
+#define NVM_SUB_VEN_ID             0x000C
+#define NVM_DEV_ID                 0x000D
+#define NVM_VEN_ID                 0x000E
+#define NVM_INIT_CTRL_2            0x000F
+#define NVM_INIT_CTRL_4            0x0013
+#define NVM_LED_1_CFG              0x001C
+#define NVM_LED_0_2_CFG            0x001F
+
 
 #define E1000_NVM_CFG_DONE_PORT_0  0x040000 /* MNG config cycle done */
 #define E1000_NVM_CFG_DONE_PORT_1  0x080000 /* ...for second port */
@@ -639,6 +671,7 @@
 
 #define NVM_PBA_OFFSET_0           8
 #define NVM_PBA_OFFSET_1           9
+#define NVM_RESERVED_WORD		0xFFFF
 #define NVM_PBA_PTR_GUARD          0xFAFA
 #define NVM_WORD_SIZE_BASE_SHIFT   6
 
@@ -696,6 +729,7 @@
 #define I82580_I_PHY_ID      0x015403A0
 #define I350_I_PHY_ID        0x015403B0
 #define M88_VENDOR           0x0141
+#define I210_I_PHY_ID        0x01410C00
 
 /* M88E1000 Specific Registers */
 #define M88E1000_PHY_SPEC_CTRL     0x10  /* PHY Specific Control Register */
@@ -815,6 +849,7 @@
 #define E1000_IPCNFG_EEE_100M_AN     0x00000004  /* EEE Enable 100M AN */
 #define E1000_EEER_TX_LPI_EN         0x00010000  /* EEE Tx LPI Enable */
 #define E1000_EEER_RX_LPI_EN         0x00020000  /* EEE Rx LPI Enable */
+#define E1000_EEER_FRC_AN            0x10000000 /* Enable EEE in loopback */
 #define E1000_EEER_LPI_FC            0x00040000  /* EEE Enable on FC */
 
 /* SerDes Control */
diff --git a/drivers/net/ethernet/intel/igb/e1000_hw.h b/drivers/net/ethernet/intel/igb/e1000_hw.h
index f67cbd3fa30..c2a51dcda55 100644
--- a/drivers/net/ethernet/intel/igb/e1000_hw.h
+++ b/drivers/net/ethernet/intel/igb/e1000_hw.h
@@ -63,6 +63,13 @@ struct e1000_hw;
 #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_REVISION_2 2
 #define E1000_REVISION_4 4
@@ -83,6 +90,8 @@ enum e1000_mac_type {
 	e1000_82576,
 	e1000_82580,
 	e1000_i350,
+	e1000_i210,
+	e1000_i211,
 	e1000_num_macs  /* List is 1-based, so subtract 1 for true count. */
 };
 
@@ -117,6 +126,7 @@ enum e1000_phy_type {
 	e1000_phy_igp_3,
 	e1000_phy_ife,
 	e1000_phy_82580,
+	e1000_phy_i210,
 };
 
 enum e1000_bus_type {
@@ -313,6 +323,9 @@ struct e1000_mac_operations {
 	void (*rar_set)(struct e1000_hw *, u8 *, u32);
 	s32  (*read_mac_addr)(struct e1000_hw *);
 	s32  (*get_speed_and_duplex)(struct e1000_hw *, u16 *, u16 *);
+	s32  (*acquire_swfw_sync)(struct e1000_hw *, u16);
+	void (*release_swfw_sync)(struct e1000_hw *, u16);
+
 };
 
 struct e1000_phy_operations {
@@ -338,6 +351,7 @@ struct e1000_nvm_operations {
 	s32  (*write)(struct e1000_hw *, u16, u16, u16 *);
 	s32  (*update)(struct e1000_hw *);
 	s32  (*validate)(struct e1000_hw *);
+	s32  (*valid_led_default)(struct e1000_hw *, u16 *);
 };
 
 struct e1000_info {
diff --git a/drivers/net/ethernet/intel/igb/e1000_i210.c b/drivers/net/ethernet/intel/igb/e1000_i210.c
new file mode 100644
index 00000000000..77a5f939bc7
--- /dev/null
+++ b/drivers/net/ethernet/intel/igb/e1000_i210.c
@@ -0,0 +1,603 @@
+/*******************************************************************************
+
+  Intel(R) Gigabit Ethernet Linux driver
+  Copyright(c) 2007-2012 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.
+
+  The full GNU General Public License is included in this distribution in
+  the file called "COPYING".
+
+  Contact Information:
+  e1000-devel Mailing List <e1000-devel@lists.sourceforge.net>
+  Intel Corporation, 5200 N.E. Elam Young Parkway, Hillsboro, OR 97124-6497
+
+******************************************************************************/
+
+/* e1000_i210
+ * e1000_i211
+ */
+
+#include <linux/types.h>
+#include <linux/if_ether.h>
+
+#include "e1000_hw.h"
+#include "e1000_i210.h"
+
+static s32 igb_get_hw_semaphore_i210(struct e1000_hw *hw);
+static void igb_put_hw_semaphore_i210(struct e1000_hw *hw);
+static s32 igb_write_nvm_srwr(struct e1000_hw *hw, u16 offset, u16 words,
+				u16 *data);
+static s32 igb_pool_flash_update_done_i210(struct e1000_hw *hw);
+
+/**
+ *  igb_acquire_nvm_i210 - Request for access to EEPROM
+ *  @hw: pointer to the HW structure
+ *
+ *  Acquire the necessary semaphores for exclusive access to the EEPROM.
+ *  Set the EEPROM access request bit and wait for EEPROM access grant bit.
+ *  Return successful if access grant bit set, else clear the request for
+ *  EEPROM access and return -E1000_ERR_NVM (-1).
+ **/
+s32 igb_acquire_nvm_i210(struct e1000_hw *hw)
+{
+	return igb_acquire_swfw_sync_i210(hw, E1000_SWFW_EEP_SM);
+}
+
+/**
+ *  igb_release_nvm_i210 - Release exclusive access to EEPROM
+ *  @hw: pointer to the HW structure
+ *
+ *  Stop any current commands to the EEPROM and clear the EEPROM request bit,
+ *  then release the semaphores acquired.
+ **/
+void igb_release_nvm_i210(struct e1000_hw *hw)
+{
+	igb_release_swfw_sync_i210(hw, E1000_SWFW_EEP_SM);
+}
+
+/**
+ *  igb_acquire_swfw_sync_i210 - Acquire SW/FW semaphore
+ *  @hw: pointer to the HW structure
+ *  @mask: specifies which semaphore to acquire
+ *
+ *  Acquire the SW/FW semaphore to access the PHY or NVM.  The mask
+ *  will also specify which port we're acquiring the lock for.
+ **/
+s32 igb_acquire_swfw_sync_i210(struct e1000_hw *hw, u16 mask)
+{
+	u32 swfw_sync;
+	u32 swmask = mask;
+	u32 fwmask = mask << 16;
+	s32 ret_val = E1000_SUCCESS;
+	s32 i = 0, timeout = 200; /* FIXME: find real value to use here */
+
+	while (i < timeout) {
+		if (igb_get_hw_semaphore_i210(hw)) {
+			ret_val = -E1000_ERR_SWFW_SYNC;
+			goto out;
+		}
+
+		swfw_sync = rd32(E1000_SW_FW_SYNC);
+		if (!(swfw_sync & fwmask))
+			break;
+
+		/*
+		 * Firmware currently using resource (fwmask)
+		 */
+		igb_put_hw_semaphore_i210(hw);
+		mdelay(5);
+		i++;
+	}
+
+	if (i == timeout) {
+		hw_dbg("Driver can't access resource, SW_FW_SYNC timeout.\n");
+		ret_val = -E1000_ERR_SWFW_SYNC;
+		goto out;
+	}
+
+	swfw_sync |= swmask;
+	wr32(E1000_SW_FW_SYNC, swfw_sync);
+
+	igb_put_hw_semaphore_i210(hw);
+out:
+	return ret_val;
+}
+
+/**
+ *  igb_release_swfw_sync_i210 - Release SW/FW semaphore
+ *  @hw: pointer to the HW structure
+ *  @mask: specifies which semaphore to acquire
+ *
+ *  Release the SW/FW semaphore used to access the PHY or NVM.  The mask
+ *  will also specify which port we're releasing the lock for.
+ **/
+void igb_release_swfw_sync_i210(struct e1000_hw *hw, u16 mask)
+{
+	u32 swfw_sync;
+
+	while (igb_get_hw_semaphore_i210(hw) != E1000_SUCCESS)
+		; /* Empty */
+
+	swfw_sync = rd32(E1000_SW_FW_SYNC);
+	swfw_sync &= ~mask;
+	wr32(E1000_SW_FW_SYNC, swfw_sync);
+
+	igb_put_hw_semaphore_i210(hw);
+}
+
+/**
+ *  igb_get_hw_semaphore_i210 - Acquire hardware semaphore
+ *  @hw: pointer to the HW structure
+ *
+ *  Acquire the HW semaphore to access the PHY or NVM
+ **/
+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 FW semaphore. */
+	for (i = 0; i < timeout; i++) {
+		swsm = rd32(E1000_SWSM);
+		wr32(E1000_SWSM, swsm | E1000_SWSM_SWESMBI);
+
+		/* Semaphore acquired if bit latched */
+		if (rd32(E1000_SWSM) & E1000_SWSM_SWESMBI)
+			break;
+
+		udelay(50);
+	}
+
+	if (i == timeout) {
+		/* Release semaphores */
+		igb_put_hw_semaphore(hw);
+		hw_dbg("Driver can't access the NVM\n");
+		ret_val = -E1000_ERR_NVM;
+		goto out;
+	}
+
+out:
+	return ret_val;
+}
+
+/**
+ *  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_read_nvm_srrd_i210 - Reads Shadow Ram using EERD register
+ *  @hw: pointer to the HW structure
+ *  @offset: offset of word in the Shadow Ram to read
+ *  @words: number of words to read
+ *  @data: word read from the Shadow Ram
+ *
+ *  Reads a 16 bit word from the Shadow Ram using the EERD register.
+ *  Uses necessary synchronization semaphores.
+ **/
+s32 igb_read_nvm_srrd_i210(struct e1000_hw *hw, u16 offset, u16 words,
+			     u16 *data)
+{
+	s32 status = E1000_SUCCESS;
+	u16 i, count;
+
+	/* 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. */
+	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);
+		if (hw->nvm.ops.acquire(hw) == E1000_SUCCESS) {
+			status = igb_read_nvm_eerd(hw, offset, count,
+						     data + i);
+			hw->nvm.ops.release(hw);
+		} else {
+			status = E1000_ERR_SWFW_SYNC;
+		}
+
+		if (status != E1000_SUCCESS)
+			break;
+	}
+
+	return status;
+}
+
+/**
+ *  igb_write_nvm_srwr_i210 - Write to Shadow RAM using EEWR
+ *  @hw: pointer to the HW structure
+ *  @offset: offset within the Shadow RAM to be written to
+ *  @words: number of words to write
+ *  @data: 16 bit word(s) to be written to the Shadow RAM
+ *
+ *  Writes data to Shadow RAM at offset using EEWR register.
+ *
+ *  If e1000_update_nvm_checksum is not called after this function , the
+ *  data will not be committed to FLASH and also Shadow RAM will most likely
+ *  contain an invalid checksum.
+ *
+ *  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)
+{
+	s32 status = E1000_SUCCESS;
+	u16 i, count;
+
+	/* We cannot hold synchronization semaphores for too long,
+	 * because of forceful takeover procedure. However it is more efficient
+	 * to write 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);
+		if (hw->nvm.ops.acquire(hw) == E1000_SUCCESS) {
+			status = igb_write_nvm_srwr(hw, offset, count,
+						      data + i);
+			hw->nvm.ops.release(hw);
+		} else {
+			status = E1000_ERR_SWFW_SYNC;
+		}
+
+		if (status != E1000_SUCCESS)
+			break;
+	}
+
+	return status;
+}
+
+/**
+ *  igb_write_nvm_srwr - Write to Shadow Ram using EEWR
+ *  @hw: pointer to the HW structure
+ *  @offset: offset within the Shadow Ram to be written to
+ *  @words: number of words to write
+ *  @data: 16 bit word(s) to be written to the Shadow Ram
+ *
+ *  Writes data to Shadow Ram at offset using EEWR register.
+ *
+ *  If igb_update_nvm_checksum is not called after this function , the
+ *  Shadow Ram will most likely contain an invalid checksum.
+ **/
+static s32 igb_write_nvm_srwr(struct e1000_hw *hw, u16 offset, u16 words,
+				u16 *data)
+{
+	struct e1000_nvm_info *nvm = &hw->nvm;
+	u32 i, k, eewr = 0;
+	u32 attempts = 100000;
+	s32 ret_val = E1000_SUCCESS;
+
+	/*
+	 * 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)) ||
+	    (words == 0)) {
+		hw_dbg("nvm parameter(s) out of bounds\n");
+		ret_val = -E1000_ERR_NVM;
+		goto out;
+	}
+
+	for (i = 0; i < words; i++) {
+		eewr = ((offset+i) << E1000_NVM_RW_ADDR_SHIFT) |
+			(data[i] << E1000_NVM_RW_REG_DATA) |
+			E1000_NVM_RW_REG_START;
+
+		wr32(E1000_SRWR, eewr);
+
+		for (k = 0; k < attempts; k++) {
+			if (E1000_NVM_RW_REG_DONE &
+			    rd32(E1000_SRWR)) {
+				ret_val = E1000_SUCCESS;
+				break;
+			}
+			udelay(5);
+	}
+
+		if (ret_val != E1000_SUCCESS) {
+			hw_dbg("Shadow RAM write EEWR timed out\n");
+			break;
+		}
+	}
+
+out:
+	return ret_val;
+}
+
+/**
+ *  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
+ *  @data: pointer to the data read
+ *
+ *  Wrapper function to return data formerly found in the NVM.
+ **/
+s32 igb_read_nvm_i211(struct e1000_hw *hw, u16 offset, u16 words,
+			       u16 *data)
+{
+	s32 ret_val = E1000_SUCCESS;
+
+	/* Only the MAC addr is required to be present in the iNVM */
+	switch (offset) {
+	case NVM_MAC_ADDR:
+		ret_val = igb_read_invm_i211(hw, offset, &data[0]);
+		ret_val |= igb_read_invm_i211(hw, offset+1, &data[1]);
+		ret_val |= igb_read_invm_i211(hw, offset+2, &data[2]);
+		if (ret_val != E1000_SUCCESS)
+			hw_dbg("MAC Addr not found in iNVM\n");
+		break;
+	case NVM_ID_LED_SETTINGS:
+	case NVM_INIT_CTRL_2:
+	case NVM_INIT_CTRL_4:
+	case NVM_LED_1_CFG:
+	case NVM_LED_0_2_CFG:
+		igb_read_invm_i211(hw, offset, data);
+		break;
+	case NVM_COMPAT:
+		*data = ID_LED_DEFAULT_I210;
+		break;
+	case NVM_SUB_DEV_ID:
+		*data = hw->subsystem_device_id;
+		break;
+	case NVM_SUB_VEN_ID:
+		*data = hw->subsystem_vendor_id;
+		break;
+	case NVM_DEV_ID:
+		*data = hw->device_id;
+		break;
+	case NVM_VEN_ID:
+		*data = hw->vendor_id;
+		break;
+	default:
+		hw_dbg("NVM word 0x%02x is not mapped.\n", offset);
+		*data = NVM_RESERVED_WORD;
+		break;
+	}
+	return ret_val;
+}
+
+/**
+ *  igb_read_invm_i211 - Reads OTP
+ *  @hw: pointer to the HW structure
+ *  @address: the word address (aka eeprom offset) to read
+ *  @data: pointer to the data read
+ *
+ *  Reads 16-bit words from the OTP. Return error when the word is not
+ *  stored in OTP.
+ **/
+s32 igb_read_invm_i211(struct e1000_hw *hw, u16 address, u16 *data)
+{
+	s32 status = -E1000_ERR_INVM_VALUE_NOT_FOUND;
+	u32 invm_dword;
+	u16 i;
+	u8 record_type, word_address;
+
+	for (i = 0; i < E1000_INVM_SIZE; i++) {
+		invm_dword = rd32(E1000_INVM_DATA_REG(i));
+		/* Get record type */
+		record_type = INVM_DWORD_TO_RECORD_TYPE(invm_dword);
+		if (record_type == E1000_INVM_UNINITIALIZED_STRUCTURE)
+			break;
+		if (record_type == E1000_INVM_CSR_AUTOLOAD_STRUCTURE)
+			i += E1000_INVM_CSR_AUTOLOAD_DATA_SIZE_IN_DWORDS;
+		if (record_type == E1000_INVM_RSA_KEY_SHA256_STRUCTURE)
+			i += E1000_INVM_RSA_KEY_SHA256_DATA_SIZE_IN_DWORDS;
+		if (record_type == E1000_INVM_WORD_AUTOLOAD_STRUCTURE) {
+			word_address = INVM_DWORD_TO_WORD_ADDRESS(invm_dword);
+			if (word_address == (u8)address) {
+				*data = INVM_DWORD_TO_WORD_DATA(invm_dword);
+				hw_dbg("Read INVM Word 0x%02x = %x",
+					  address, *data);
+				status = E1000_SUCCESS;
+				break;
+			}
+		}
+	}
+	if (status != E1000_SUCCESS)
+		hw_dbg("Requested word 0x%02x not found in OTP\n", address);
+	return status;
+}
+
+/**
+ *  igb_validate_nvm_checksum_i210 - Validate EEPROM checksum
+ *  @hw: pointer to the HW structure
+ *
+ *  Calculates the EEPROM checksum by reading/adding each word of the EEPROM
+ *  and then verifies that the sum of the EEPROM is equal to 0xBABA.
+ **/
+s32 igb_validate_nvm_checksum_i210(struct e1000_hw *hw)
+{
+	s32 status = E1000_SUCCESS;
+	s32 (*read_op_ptr)(struct e1000_hw *, u16, u16, u16 *);
+
+	if (hw->nvm.ops.acquire(hw) == E1000_SUCCESS) {
+
+		/*
+		 * Replace the read function with semaphore grabbing with
+		 * the one that skips this for a while.
+		 * We have semaphore taken already here.
+		 */
+		read_op_ptr = hw->nvm.ops.read;
+		hw->nvm.ops.read = igb_read_nvm_eerd;
+
+		status = igb_validate_nvm_checksum(hw);
+
+		/* Revert original read operation. */
+		hw->nvm.ops.read = read_op_ptr;
+
+		hw->nvm.ops.release(hw);
+	} else {
+		status = E1000_ERR_SWFW_SYNC;
+	}
+
+	return status;
+}
+
+
+/**
+ *  igb_update_nvm_checksum_i210 - Update EEPROM checksum
+ *  @hw: pointer to the HW structure
+ *
+ *  Updates the EEPROM checksum by reading/adding each word of the EEPROM
+ *  up to the checksum.  Then calculates the EEPROM checksum and writes the
+ *  value to the EEPROM. Next commit EEPROM data onto the Flash.
+ **/
+s32 igb_update_nvm_checksum_i210(struct e1000_hw *hw)
+{
+	s32 ret_val = E1000_SUCCESS;
+	u16 checksum = 0;
+	u16 i, nvm_data;
+
+	/*
+	 * 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
+	 */
+	ret_val = igb_read_nvm_eerd(hw, 0, 1, &nvm_data);
+	if (ret_val != E1000_SUCCESS) {
+		hw_dbg("EEPROM read failed\n");
+		goto out;
+	}
+
+	if (hw->nvm.ops.acquire(hw) == E1000_SUCCESS) {
+		/*
+		 * Do not use hw->nvm.ops.write, hw->nvm.ops.read
+		 * because we do not want to take the synchronization
+		 * semaphores twice here.
+		 */
+
+		for (i = 0; i < NVM_CHECKSUM_REG; i++) {
+			ret_val = igb_read_nvm_eerd(hw, i, 1, &nvm_data);
+			if (ret_val) {
+				hw->nvm.ops.release(hw);
+				hw_dbg("NVM Read Error while updating checksum.\n");
+				goto out;
+			}
+			checksum += nvm_data;
+		}
+		checksum = (u16) NVM_SUM - checksum;
+		ret_val = igb_write_nvm_srwr(hw, NVM_CHECKSUM_REG, 1,
+						&checksum);
+		if (ret_val != E1000_SUCCESS) {
+			hw->nvm.ops.release(hw);
+			hw_dbg("NVM Write Error while updating checksum.\n");
+			goto out;
+		}
+
+		hw->nvm.ops.release(hw);
+
+		ret_val = igb_update_flash_i210(hw);
+	} else {
+		ret_val = -E1000_ERR_SWFW_SYNC;
+	}
+out:
+	return ret_val;
+}
+
+/**
+ *  igb_update_flash_i210 - Commit EEPROM to the flash
+ *  @hw: pointer to the HW structure
+ *
+ **/
+s32 igb_update_flash_i210(struct e1000_hw *hw)
+{
+	s32 ret_val = E1000_SUCCESS;
+	u32 flup;
+
+	ret_val = igb_pool_flash_update_done_i210(hw);
+	if (ret_val == -E1000_ERR_NVM) {
+		hw_dbg("Flash update time out\n");
+		goto out;
+	}
+
+	flup = rd32(E1000_EECD) | E1000_EECD_FLUPD_I210;
+	wr32(E1000_EECD, flup);
+
+	ret_val = igb_pool_flash_update_done_i210(hw);
+	if (ret_val == E1000_SUCCESS)
+		hw_dbg("Flash update complete\n");
+	else
+		hw_dbg("Flash update time out\n");
+
+out:
+	return ret_val;
+}
+
+/**
+ *  igb_pool_flash_update_done_i210 - Pool FLUDONE status.
+ *  @hw: pointer to the HW structure
+ *
+ **/
+s32 igb_pool_flash_update_done_i210(struct e1000_hw *hw)
+{
+	s32 ret_val = -E1000_ERR_NVM;
+	u32 i, reg;
+
+	for (i = 0; i < E1000_FLUDONE_ATTEMPTS; i++) {
+		reg = rd32(E1000_EECD);
+		if (reg & E1000_EECD_FLUDONE_I210) {
+			ret_val = E1000_SUCCESS;
+			break;
+		}
+		udelay(5);
+	}
+
+	return ret_val;
+}
+
+/**
+ *  igb_valid_led_default_i210 - Verify a valid default LED config
+ *  @hw: pointer to the HW structure
+ *  @data: pointer to the NVM (EEPROM)
+ *
+ *  Read the EEPROM for the current default LED configuration.  If the
+ *  LED configuration is not valid, set to a valid LED configuration.
+ **/
+s32 igb_valid_led_default_i210(struct e1000_hw *hw, u16 *data)
+{
+	s32 ret_val;
+
+	ret_val = hw->nvm.ops.read(hw, NVM_ID_LED_SETTINGS, 1, data);
+	if (ret_val) {
+		hw_dbg("NVM Read Error\n");
+		goto out;
+	}
+
+	if (*data == ID_LED_RESERVED_0000 || *data == ID_LED_RESERVED_FFFF) {
+		switch (hw->phy.media_type) {
+		case e1000_media_type_internal_serdes:
+			*data = ID_LED_DEFAULT_I210_SERDES;
+			break;
+		case e1000_media_type_copper:
+		default:
+			*data = ID_LED_DEFAULT_I210;
+			break;
+		}
+	}
+out:
+	return ret_val;
+}
diff --git a/drivers/net/ethernet/intel/igb/e1000_i210.h b/drivers/net/ethernet/intel/igb/e1000_i210.h
new file mode 100644
index 00000000000..5dc2bd3f50b
--- /dev/null
+++ b/drivers/net/ethernet/intel/igb/e1000_i210.h
@@ -0,0 +1,76 @@
+/*******************************************************************************
+
+  Intel(R) Gigabit Ethernet Linux driver
+  Copyright(c) 2007-2012 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.
+
+  The full GNU General Public License is included in this distribution in
+  the file called "COPYING".
+
+  Contact Information:
+  e1000-devel Mailing List <e1000-devel@lists.sourceforge.net>
+  Intel Corporation, 5200 N.E. Elam Young Parkway, Hillsboro, OR 97124-6497
+
+*******************************************************************************/
+
+#ifndef _E1000_I210_H_
+#define _E1000_I210_H_
+
+extern s32 igb_update_flash_i210(struct e1000_hw *hw);
+extern s32 igb_update_nvm_checksum_i210(struct e1000_hw *hw);
+extern s32 igb_validate_nvm_checksum_i210(struct e1000_hw *hw);
+extern s32 igb_write_nvm_srwr_i210(struct e1000_hw *hw, u16 offset,
+			      u16 words, u16 *data);
+extern s32 igb_read_nvm_srrd_i210(struct e1000_hw *hw, u16 offset,
+			     u16 words, u16 *data);
+extern s32 igb_read_invm_i211(struct e1000_hw *hw, u16 address, u16 *data);
+extern s32 igb_acquire_swfw_sync_i210(struct e1000_hw *hw, u16 mask);
+extern void igb_release_swfw_sync_i210(struct e1000_hw *hw, u16 mask);
+extern s32 igb_acquire_nvm_i210(struct e1000_hw *hw);
+extern void igb_release_nvm_i210(struct e1000_hw *hw);
+extern s32 igb_valid_led_default_i210(struct e1000_hw *hw, u16 *data);
+extern s32 igb_read_nvm_i211(struct e1000_hw *hw, u16 offset, u16 words,
+			       u16 *data);
+
+#define E1000_STM_OPCODE		0xDB00
+#define E1000_EEPROM_FLASH_SIZE_WORD	0x11
+
+#define INVM_DWORD_TO_RECORD_TYPE(invm_dword) \
+	(u8)((invm_dword) & 0x7)
+#define INVM_DWORD_TO_WORD_ADDRESS(invm_dword) \
+	(u8)(((invm_dword) & 0x0000FE00) >> 9)
+#define INVM_DWORD_TO_WORD_DATA(invm_dword) \
+	(u16)(((invm_dword) & 0xFFFF0000) >> 16)
+
+enum E1000_INVM_STRUCTURE_TYPE {
+	E1000_INVM_UNINITIALIZED_STRUCTURE		= 0x00,
+	E1000_INVM_WORD_AUTOLOAD_STRUCTURE		= 0x01,
+	E1000_INVM_CSR_AUTOLOAD_STRUCTURE		= 0x02,
+	E1000_INVM_PHY_REGISTER_AUTOLOAD_STRUCTURE	= 0x03,
+	E1000_INVM_RSA_KEY_SHA256_STRUCTURE		= 0x04,
+	E1000_INVM_INVALIDATED_STRUCTURE		= 0x0F,
+};
+
+#define E1000_INVM_RSA_KEY_SHA256_DATA_SIZE_IN_DWORDS	8
+#define E1000_INVM_CSR_AUTOLOAD_DATA_SIZE_IN_DWORDS	1
+
+#define ID_LED_DEFAULT_I210		((ID_LED_OFF1_ON2  << 8) | \
+					 (ID_LED_OFF1_OFF2 <<  4) | \
+					 (ID_LED_DEF1_DEF2))
+#define ID_LED_DEFAULT_I210_SERDES	((ID_LED_DEF1_DEF2 << 8) | \
+					 (ID_LED_DEF1_DEF2 <<  4) | \
+					 (ID_LED_DEF1_DEF2))
+
+#endif
diff --git a/drivers/net/ethernet/intel/igb/e1000_mac.c b/drivers/net/ethernet/intel/igb/e1000_mac.c
index f57338afd71..819c145ac76 100644
--- a/drivers/net/ethernet/intel/igb/e1000_mac.c
+++ b/drivers/net/ethernet/intel/igb/e1000_mac.c
@@ -658,6 +658,7 @@ s32 igb_setup_link(struct e1000_hw *hw)
 	ret_val = igb_set_fc_watermarks(hw);
 
 out:
+
 	return ret_val;
 }
 
diff --git a/drivers/net/ethernet/intel/igb/e1000_nvm.c b/drivers/net/ethernet/intel/igb/e1000_nvm.c
index fa2c6ba6213..aa5fcdf3f35 100644
--- a/drivers/net/ethernet/intel/igb/e1000_nvm.c
+++ b/drivers/net/ethernet/intel/igb/e1000_nvm.c
@@ -710,4 +710,3 @@ s32 igb_update_nvm_checksum(struct e1000_hw *hw)
 out:
 	return ret_val;
 }
-
diff --git a/drivers/net/ethernet/intel/igb/e1000_phy.c b/drivers/net/ethernet/intel/igb/e1000_phy.c
index 789de5b83aa..7be98b6f105 100644
--- a/drivers/net/ethernet/intel/igb/e1000_phy.c
+++ b/drivers/net/ethernet/intel/igb/e1000_phy.c
@@ -35,6 +35,7 @@ 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);
 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[] =
@@ -570,6 +571,11 @@ s32 igb_copper_link_setup_m88(struct e1000_hw *hw)
 		hw_dbg("Error committing the PHY changes\n");
 		goto out;
 	}
+	if (phy->type == e1000_phy_i210) {
+		ret_val = igb_set_master_slave_mode(hw);
+		if (ret_val)
+			return ret_val;
+	}
 
 out:
 	return ret_val;
@@ -1213,12 +1219,22 @@ s32 igb_phy_force_speed_duplex_m88(struct e1000_hw *hw)
 			goto out;
 
 		if (!link) {
-			if (hw->phy.type != e1000_phy_m88 ||
-			    hw->phy.id == I347AT4_E_PHY_ID ||
-			    hw->phy.id == M88E1112_E_PHY_ID) {
+			bool reset_dsp = true;
+
+			switch (hw->phy.id) {
+			case I347AT4_E_PHY_ID:
+			case M88E1112_E_PHY_ID:
+			case I210_I_PHY_ID:
+				reset_dsp = false;
+				break;
+			default:
+				if (hw->phy.type != e1000_phy_m88)
+					reset_dsp = false;
+				break;
+			}
+			if (!reset_dsp)
 				hw_dbg("Link taking longer than expected.\n");
-			} else {
-
+			else {
 				/*
 				 * We didn't get link.
 				 * Reset the DSP and cross our fingers.
@@ -1243,7 +1259,8 @@ s32 igb_phy_force_speed_duplex_m88(struct e1000_hw *hw)
 
 	if (hw->phy.type != e1000_phy_m88 ||
 	    hw->phy.id == I347AT4_E_PHY_ID ||
-	    hw->phy.id == M88E1112_E_PHY_ID)
+	    hw->phy.id == M88E1112_E_PHY_ID ||
+	    hw->phy.id == I210_I_PHY_ID)
 		goto out;
 
 	ret_val = phy->ops.read_reg(hw, M88E1000_EXT_PHY_SPEC_CTRL, &phy_data);
@@ -1441,6 +1458,7 @@ s32 igb_check_downshift(struct e1000_hw *hw)
 	u16 phy_data, offset, mask;
 
 	switch (phy->type) {
+	case e1000_phy_i210:
 	case e1000_phy_m88:
 	case e1000_phy_gg82563:
 		offset	= M88E1000_PHY_SPEC_STATUS;
@@ -1476,7 +1494,7 @@ out:
  *
  *  Polarity is determined based on the PHY specific status register.
  **/
-static s32 igb_check_polarity_m88(struct e1000_hw *hw)
+s32 igb_check_polarity_m88(struct e1000_hw *hw)
 {
 	struct e1000_phy_info *phy = &hw->phy;
 	s32 ret_val;
@@ -1665,6 +1683,7 @@ s32 igb_get_cable_length_m88_gen2(struct e1000_hw *hw)
 	u16 phy_data, phy_data2, index, default_page, is_cm;
 
 	switch (hw->phy.id) {
+	case I210_I_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,
@@ -2129,10 +2148,16 @@ s32 igb_phy_init_script_igp3(struct e1000_hw *hw)
 void igb_power_up_phy_copper(struct e1000_hw *hw)
 {
 	u16 mii_reg = 0;
+	u16 power_reg = 0;
 
 	/* The PHY will retain its settings across a power down/up cycle */
 	hw->phy.ops.read_reg(hw, PHY_CONTROL, &mii_reg);
 	mii_reg &= ~MII_CR_POWER_DOWN;
+	if (hw->phy.type == e1000_phy_i210) {
+		hw->phy.ops.read_reg(hw, GS40G_COPPER_SPEC, &power_reg);
+		power_reg &= ~GS40G_CS_POWER_DOWN;
+		hw->phy.ops.write_reg(hw, GS40G_COPPER_SPEC, power_reg);
+	}
 	hw->phy.ops.write_reg(hw, PHY_CONTROL, mii_reg);
 }
 
@@ -2146,10 +2171,18 @@ void igb_power_up_phy_copper(struct e1000_hw *hw)
 void igb_power_down_phy_copper(struct e1000_hw *hw)
 {
 	u16 mii_reg = 0;
+	u16 power_reg = 0;
 
 	/* The PHY will retain its settings across a power down/up cycle */
 	hw->phy.ops.read_reg(hw, PHY_CONTROL, &mii_reg);
 	mii_reg |= MII_CR_POWER_DOWN;
+
+	/* i210 Phy requires an additional bit for power up/down */
+	if (hw->phy.type == e1000_phy_i210) {
+		hw->phy.ops.read_reg(hw, GS40G_COPPER_SPEC, &power_reg);
+		power_reg |= GS40G_CS_POWER_DOWN;
+		hw->phy.ops.write_reg(hw, GS40G_COPPER_SPEC, power_reg);
+	}
 	hw->phy.ops.write_reg(hw, PHY_CONTROL, mii_reg);
 	msleep(1);
 }
@@ -2345,3 +2378,103 @@ s32 igb_get_cable_length_82580(struct e1000_hw *hw)
 out:
 	return ret_val;
 }
+
+/**
+ *  igb_write_phy_reg_gs40g - Write GS40G PHY register
+ *  @hw: pointer to the HW structure
+ *  @offset: lower half is register offset to write to
+ *     upper half is page to use.
+ *  @data: data to write at register offset
+ *
+ *  Acquires semaphore, if necessary, then writes the data to PHY register
+ *  at the offset.  Release any acquired semaphores before exiting.
+ **/
+s32 igb_write_phy_reg_gs40g(struct e1000_hw *hw, u32 offset, u16 data)
+{
+	s32 ret_val;
+	u16 page = offset >> GS40G_PAGE_SHIFT;
+
+	offset = offset & GS40G_OFFSET_MASK;
+	ret_val = hw->phy.ops.acquire(hw);
+	if (ret_val)
+		return ret_val;
+
+	ret_val = igb_write_phy_reg_mdic(hw, GS40G_PAGE_SELECT, page);
+	if (ret_val)
+		goto release;
+	ret_val = igb_write_phy_reg_mdic(hw, offset, data);
+
+release:
+	hw->phy.ops.release(hw);
+	return ret_val;
+}
+
+/**
+ *  igb_read_phy_reg_gs40g - Read GS40G  PHY register
+ *  @hw: pointer to the HW structure
+ *  @offset: lower half is register offset to read to
+ *     upper half is page to use.
+ *  @data: data to read at register offset
+ *
+ *  Acquires semaphore, if necessary, then reads the data in the PHY register
+ *  at the offset.  Release any acquired semaphores before exiting.
+ **/
+s32 igb_read_phy_reg_gs40g(struct e1000_hw *hw, u32 offset, u16 *data)
+{
+	s32 ret_val;
+	u16 page = offset >> GS40G_PAGE_SHIFT;
+
+	offset = offset & GS40G_OFFSET_MASK;
+	ret_val = hw->phy.ops.acquire(hw);
+	if (ret_val)
+		return ret_val;
+
+	ret_val = igb_write_phy_reg_mdic(hw, GS40G_PAGE_SELECT, page);
+	if (ret_val)
+		goto release;
+	ret_val = igb_read_phy_reg_mdic(hw, offset, data);
+
+release:
+	hw->phy.ops.release(hw);
+	return ret_val;
+}
+
+/**
+ *  igb_set_master_slave_mode - Setup PHY for Master/slave mode
+ *  @hw: pointer to the HW structure
+ *
+ *  Sets up Master/slave mode
+ **/
+static s32 igb_set_master_slave_mode(struct e1000_hw *hw)
+{
+	s32 ret_val;
+	u16 phy_data;
+
+	/* Resolve Master/Slave mode */
+	ret_val = hw->phy.ops.read_reg(hw, PHY_1000T_CTRL, &phy_data);
+	if (ret_val)
+		return ret_val;
+
+	/* load defaults for future use */
+	hw->phy.original_ms_type = (phy_data & CR_1000T_MS_ENABLE) ?
+				   ((phy_data & CR_1000T_MS_VALUE) ?
+				    e1000_ms_force_master :
+				    e1000_ms_force_slave) : e1000_ms_auto;
+
+	switch (hw->phy.ms_type) {
+	case e1000_ms_force_master:
+		phy_data |= (CR_1000T_MS_ENABLE | CR_1000T_MS_VALUE);
+		break;
+	case e1000_ms_force_slave:
+		phy_data |= CR_1000T_MS_ENABLE;
+		phy_data &= ~(CR_1000T_MS_VALUE);
+		break;
+	case e1000_ms_auto:
+		phy_data &= ~CR_1000T_MS_ENABLE;
+		/* fall-through */
+	default:
+		break;
+	}
+
+	return hw->phy.ops.write_reg(hw, PHY_1000T_CTRL, phy_data);
+}
diff --git a/drivers/net/ethernet/intel/igb/e1000_phy.h b/drivers/net/ethernet/intel/igb/e1000_phy.h
index 4c32ac66ff3..34e40619f16 100644
--- a/drivers/net/ethernet/intel/igb/e1000_phy.h
+++ b/drivers/net/ethernet/intel/igb/e1000_phy.h
@@ -73,6 +73,9 @@ s32  igb_copper_link_setup_82580(struct e1000_hw *hw);
 s32  igb_get_phy_info_82580(struct e1000_hw *hw);
 s32  igb_phy_force_speed_duplex_82580(struct e1000_hw *hw);
 s32  igb_get_cable_length_82580(struct e1000_hw *hw);
+s32  igb_read_phy_reg_gs40g(struct e1000_hw *hw, u32 offset, u16 *data);
+s32  igb_write_phy_reg_gs40g(struct e1000_hw *hw, u32 offset, u16 data);
+s32  igb_check_polarity_m88(struct e1000_hw *hw);
 
 /* IGP01E1000 Specific Registers */
 #define IGP01E1000_PHY_PORT_CONFIG        0x10 /* Port Config */
@@ -114,6 +117,13 @@ s32  igb_get_cable_length_82580(struct e1000_hw *hw);
 /* I82580 PHY Diagnostics Status */
 #define I82580_DSTATUS_CABLE_LENGTH       0x03FC
 #define I82580_DSTATUS_CABLE_LENGTH_SHIFT 2
+
+/* 82580 PHY Power Management */
+#define E1000_82580_PHY_POWER_MGMT	0xE14
+#define E1000_82580_PM_SPD		0x0001 /* Smart Power Down */
+#define E1000_82580_PM_D0_LPLU		0x0002 /* For D0a states */
+#define E1000_82580_PM_D3_LPLU		0x0004 /* For all other states */
+
 /* Enable flexible speed on link-up */
 #define IGP02E1000_PM_D0_LPLU             0x0002 /* For D0a states */
 #define IGP02E1000_PM_D3_LPLU             0x0004 /* For all other states */
@@ -133,4 +143,16 @@ s32  igb_get_cable_length_82580(struct e1000_hw *hw);
 
 #define E1000_CABLE_LENGTH_UNDEFINED      0xFF
 
+/* GS40G - I210 PHY defines */
+#define GS40G_PAGE_SELECT		0x16
+#define GS40G_PAGE_SHIFT		16
+#define GS40G_OFFSET_MASK		0xFFFF
+#define GS40G_PAGE_2			0x20000
+#define GS40G_MAC_REG2			0x15
+#define GS40G_MAC_LB			0x4140
+#define GS40G_MAC_SPEED_1G		0X0006
+#define GS40G_COPPER_SPEC		0x0010
+#define GS40G_CS_POWER_DOWN		0x0002
+#define GS40G_LINE_LB			0x4000
+
 #endif
diff --git a/drivers/net/ethernet/intel/igb/e1000_regs.h b/drivers/net/ethernet/intel/igb/e1000_regs.h
index ccdf36d503f..35d1e4f2c92 100644
--- a/drivers/net/ethernet/intel/igb/e1000_regs.h
+++ b/drivers/net/ethernet/intel/igb/e1000_regs.h
@@ -352,4 +352,18 @@
 #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
+#define E1000_I210_FLMNGDATA	0x1203C
+#define E1000_I210_FLMNGCNT	0x12040
+
+#define E1000_I210_FLSWCTL	0x12048
+#define E1000_I210_FLSWDATA	0x1204C
+#define E1000_I210_FLSWCNT	0x12050
+
+#define E1000_I210_FLA		0x1201C
+
+#define E1000_INVM_DATA_REG(_n)	(0x12120 + 4*(_n))
+#define E1000_INVM_SIZE		64 /* Number of INVM Data Registers */
+
 #endif
diff --git a/drivers/net/ethernet/intel/igb/igb.h b/drivers/net/ethernet/intel/igb/igb.h
index 8e33bdd33ee..ae6d3f393a5 100644
--- a/drivers/net/ethernet/intel/igb/igb.h
+++ b/drivers/net/ethernet/intel/igb/igb.h
@@ -35,8 +35,8 @@
 #include "e1000_82575.h"
 
 #include <linux/clocksource.h>
-#include <linux/timecompare.h>
 #include <linux/net_tstamp.h>
+#include <linux/ptp_clock_kernel.h>
 #include <linux/bitops.h>
 #include <linux/if_vlan.h>
 
@@ -65,10 +65,13 @@ struct igb_adapter;
 #define MAX_Q_VECTORS                      8
 
 /* Transmit and receive queues */
-#define IGB_MAX_RX_QUEUES                  (adapter->vfs_allocated_count ? 2 : \
-                                           (hw->mac.type > e1000_82575 ? 8 : 4))
+#define IGB_MAX_RX_QUEUES		((adapter->vfs_allocated_count ? 2 : \
+					(hw->mac.type > e1000_82575 ? 8 : 4)))
+#define IGB_MAX_RX_QUEUES_I210             4
+#define IGB_MAX_RX_QUEUES_I211             2
 #define IGB_MAX_TX_QUEUES                  16
-
+#define IGB_MAX_TX_QUEUES_I210             4
+#define IGB_MAX_TX_QUEUES_I211             2
 #define IGB_MAX_VF_MC_ENTRIES              30
 #define IGB_MAX_VF_FUNCTIONS               8
 #define IGB_MAX_VFTA_ENTRIES               128
@@ -328,9 +331,6 @@ struct igb_adapter {
 
 	/* OS defined structs */
 	struct pci_dev *pdev;
-	struct cyclecounter cycles;
-	struct timecounter clock;
-	struct timecompare compare;
 	struct hwtstamp_config hwtstamp_config;
 
 	spinlock_t stats64_lock;
@@ -364,6 +364,13 @@ struct igb_adapter {
 	u32 wvbr;
 	int node;
 	u32 *shadow_vfta;
+
+	struct ptp_clock *ptp_clock;
+	struct ptp_clock_info caps;
+	struct delayed_work overflow_work;
+	spinlock_t tmreg_lock;
+	struct cyclecounter cc;
+	struct timecounter tc;
 };
 
 #define IGB_FLAG_HAS_MSI           (1 << 0)
@@ -378,7 +385,6 @@ struct igb_adapter {
 #define IGB_DMCTLX_DCFLUSH_DIS     0x80000000  /* Disable DMA Coal Flush */
 
 #define IGB_82576_TSYNC_SHIFT 19
-#define IGB_82580_TSYNC_SHIFT 24
 #define IGB_TS_HDR_LEN        16
 enum e1000_state_t {
 	__IGB_TESTING,
@@ -414,7 +420,15 @@ extern void igb_update_stats(struct igb_adapter *, struct rtnl_link_stats64 *);
 extern bool igb_has_link(struct igb_adapter *adapter);
 extern void igb_set_ethtool_ops(struct net_device *);
 extern void igb_power_up_link(struct igb_adapter *);
+#ifdef CONFIG_IGB_PTP
+extern void igb_ptp_init(struct igb_adapter *adapter);
+extern void igb_ptp_remove(struct igb_adapter *adapter);
 
+extern void igb_systim_to_hwtstamp(struct igb_adapter *adapter,
+				   struct skb_shared_hwtstamps *hwtstamps,
+				   u64 systim);
+
+#endif
 static inline s32 igb_reset_phy(struct e1000_hw *hw)
 {
 	if (hw->phy.ops.reset)
diff --git a/drivers/net/ethernet/intel/igb/igb_ethtool.c b/drivers/net/ethernet/intel/igb/igb_ethtool.c
index e10821a0f24..812d4f963bd 100644
--- a/drivers/net/ethernet/intel/igb/igb_ethtool.c
+++ b/drivers/net/ethernet/intel/igb/igb_ethtool.c
@@ -335,7 +335,7 @@ static void igb_set_msglevel(struct net_device *netdev, u32 data)
 
 static int igb_get_regs_len(struct net_device *netdev)
 {
-#define IGB_REGS_LEN 551
+#define IGB_REGS_LEN 739
 	return IGB_REGS_LEN * sizeof(u32);
 }
 
@@ -552,10 +552,49 @@ static void igb_get_regs(struct net_device *netdev,
 	regs_buff[548] = rd32(E1000_TDFT);
 	regs_buff[549] = rd32(E1000_TDFHS);
 	regs_buff[550] = rd32(E1000_TDFPC);
-	regs_buff[551] = adapter->stats.o2bgptc;
-	regs_buff[552] = adapter->stats.b2ospc;
-	regs_buff[553] = adapter->stats.o2bspc;
-	regs_buff[554] = adapter->stats.b2ogprc;
+
+	if (hw->mac.type > e1000_82580) {
+		regs_buff[551] = adapter->stats.o2bgptc;
+		regs_buff[552] = adapter->stats.b2ospc;
+		regs_buff[553] = adapter->stats.o2bspc;
+		regs_buff[554] = adapter->stats.b2ogprc;
+	}
+
+	if (hw->mac.type != e1000_82576)
+		return;
+	for (i = 0; i < 12; i++)
+		regs_buff[555 + i] = rd32(E1000_SRRCTL(i + 4));
+	for (i = 0; i < 4; i++)
+		regs_buff[567 + i] = rd32(E1000_PSRTYPE(i + 4));
+	for (i = 0; i < 12; i++)
+		regs_buff[571 + i] = rd32(E1000_RDBAL(i + 4));
+	for (i = 0; i < 12; i++)
+		regs_buff[583 + i] = rd32(E1000_RDBAH(i + 4));
+	for (i = 0; i < 12; i++)
+		regs_buff[595 + i] = rd32(E1000_RDLEN(i + 4));
+	for (i = 0; i < 12; i++)
+		regs_buff[607 + i] = rd32(E1000_RDH(i + 4));
+	for (i = 0; i < 12; i++)
+		regs_buff[619 + i] = rd32(E1000_RDT(i + 4));
+	for (i = 0; i < 12; i++)
+		regs_buff[631 + i] = rd32(E1000_RXDCTL(i + 4));
+
+	for (i = 0; i < 12; i++)
+		regs_buff[643 + i] = rd32(E1000_TDBAL(i + 4));
+	for (i = 0; i < 12; i++)
+		regs_buff[655 + i] = rd32(E1000_TDBAH(i + 4));
+	for (i = 0; i < 12; i++)
+		regs_buff[667 + i] = rd32(E1000_TDLEN(i + 4));
+	for (i = 0; i < 12; i++)
+		regs_buff[679 + i] = rd32(E1000_TDH(i + 4));
+	for (i = 0; i < 12; i++)
+		regs_buff[691 + i] = rd32(E1000_TDT(i + 4));
+	for (i = 0; i < 12; i++)
+		regs_buff[703 + i] = rd32(E1000_TXDCTL(i + 4));
+	for (i = 0; i < 12; i++)
+		regs_buff[715 + i] = rd32(E1000_TDWBAL(i + 4));
+	for (i = 0; i < 12; i++)
+		regs_buff[727 + i] = rd32(E1000_TDWBAH(i + 4));
 }
 
 static int igb_get_eeprom_len(struct net_device *netdev)
@@ -624,6 +663,9 @@ static int igb_set_eeprom(struct net_device *netdev,
 	if (eeprom->len == 0)
 		return -EOPNOTSUPP;
 
+	if (hw->mac.type == e1000_i211)
+		return -EOPNOTSUPP;
+
 	if (eeprom->magic != (hw->vendor_id | (hw->device_id << 16)))
 		return -EFAULT;
 
@@ -851,6 +893,36 @@ struct igb_reg_test {
 #define TABLE64_TEST_LO	5
 #define TABLE64_TEST_HI	6
 
+/* i210 reg test */
+static struct igb_reg_test reg_test_i210[] = {
+	{ E1000_FCAL,	   0x100, 1,  PATTERN_TEST, 0xFFFFFFFF, 0xFFFFFFFF },
+	{ E1000_FCAH,	   0x100, 1,  PATTERN_TEST, 0x0000FFFF, 0xFFFFFFFF },
+	{ E1000_FCT,	   0x100, 1,  PATTERN_TEST, 0x0000FFFF, 0xFFFFFFFF },
+	{ E1000_RDBAL(0),  0x100, 4,  PATTERN_TEST, 0xFFFFFF80, 0xFFFFFFFF },
+	{ E1000_RDBAH(0),  0x100, 4,  PATTERN_TEST, 0xFFFFFFFF, 0xFFFFFFFF },
+	{ E1000_RDLEN(0),  0x100, 4,  PATTERN_TEST, 0x000FFF80, 0x000FFFFF },
+	/* RDH is read-only for i210, only test RDT. */
+	{ E1000_RDT(0),	   0x100, 4,  PATTERN_TEST, 0x0000FFFF, 0x0000FFFF },
+	{ E1000_FCRTH,	   0x100, 1,  PATTERN_TEST, 0x0000FFF0, 0x0000FFF0 },
+	{ E1000_FCTTV,	   0x100, 1,  PATTERN_TEST, 0x0000FFFF, 0x0000FFFF },
+	{ E1000_TIPG,	   0x100, 1,  PATTERN_TEST, 0x3FFFFFFF, 0x3FFFFFFF },
+	{ E1000_TDBAL(0),  0x100, 4,  PATTERN_TEST, 0xFFFFFF80, 0xFFFFFFFF },
+	{ E1000_TDBAH(0),  0x100, 4,  PATTERN_TEST, 0xFFFFFFFF, 0xFFFFFFFF },
+	{ E1000_TDLEN(0),  0x100, 4,  PATTERN_TEST, 0x000FFF80, 0x000FFFFF },
+	{ E1000_TDT(0),	   0x100, 4,  PATTERN_TEST, 0x0000FFFF, 0x0000FFFF },
+	{ E1000_RCTL,	   0x100, 1,  SET_READ_TEST, 0xFFFFFFFF, 0x00000000 },
+	{ E1000_RCTL,	   0x100, 1,  SET_READ_TEST, 0x04CFB0FE, 0x003FFFFB },
+	{ E1000_RCTL,	   0x100, 1,  SET_READ_TEST, 0x04CFB0FE, 0xFFFFFFFF },
+	{ E1000_TCTL,	   0x100, 1,  SET_READ_TEST, 0xFFFFFFFF, 0x00000000 },
+	{ E1000_RA,	   0, 16, TABLE64_TEST_LO,
+						0xFFFFFFFF, 0xFFFFFFFF },
+	{ E1000_RA,	   0, 16, TABLE64_TEST_HI,
+						0x900FFFFF, 0xFFFFFFFF },
+	{ E1000_MTA,	   0, 128, TABLE32_TEST,
+						0xFFFFFFFF, 0xFFFFFFFF },
+	{ 0, 0, 0, 0, 0 }
+};
+
 /* i350 reg test */
 static struct igb_reg_test reg_test_i350[] = {
 	{ E1000_FCAL,	   0x100, 1,  PATTERN_TEST, 0xFFFFFFFF, 0xFFFFFFFF },
@@ -1073,6 +1145,11 @@ static int igb_reg_test(struct igb_adapter *adapter, u64 *data)
 		test = reg_test_i350;
 		toggle = 0x7FEFF3FF;
 		break;
+	case e1000_i210:
+	case e1000_i211:
+		test = reg_test_i210;
+		toggle = 0x7FEFF3FF;
+		break;
 	case e1000_82580:
 		test = reg_test_82580;
 		toggle = 0x7FEFF3FF;
@@ -1154,23 +1231,13 @@ static int igb_reg_test(struct igb_adapter *adapter, u64 *data)
 
 static int igb_eeprom_test(struct igb_adapter *adapter, u64 *data)
 {
-	u16 temp;
-	u16 checksum = 0;
-	u16 i;
-
 	*data = 0;
-	/* Read and add up the contents of the EEPROM */
-	for (i = 0; i < (NVM_CHECKSUM_REG + 1); i++) {
-		if ((adapter->hw.nvm.ops.read(&adapter->hw, i, 1, &temp)) < 0) {
-			*data = 1;
-			break;
-		}
-		checksum += temp;
-	}
 
-	/* If Checksum is not Correct return error else test passed */
-	if ((checksum != (u16) NVM_SUM) && !(*data))
-		*data = 2;
+	/* Validate eeprom on all parts but i211 */
+	if (adapter->hw.mac.type != e1000_i211) {
+		if (adapter->hw.nvm.ops.validate(&adapter->hw) < 0)
+			*data = 2;
+	}
 
 	return *data;
 }
@@ -1236,6 +1303,8 @@ static int igb_intr_test(struct igb_adapter *adapter, u64 *data)
 		ics_mask = 0x77DCFED5;
 		break;
 	case e1000_i350:
+	case e1000_i210:
+	case e1000_i211:
 		ics_mask = 0x77DCFED5;
 		break;
 	default:
@@ -1402,23 +1471,35 @@ static int igb_integrated_phy_loopback(struct igb_adapter *adapter)
 {
 	struct e1000_hw *hw = &adapter->hw;
 	u32 ctrl_reg = 0;
+	u16 phy_reg = 0;
 
 	hw->mac.autoneg = false;
 
-	if (hw->phy.type == e1000_phy_m88) {
+	switch (hw->phy.type) {
+	case e1000_phy_m88:
 		/* Auto-MDI/MDIX Off */
 		igb_write_phy_reg(hw, M88E1000_PHY_SPEC_CTRL, 0x0808);
 		/* reset to update Auto-MDI/MDIX */
 		igb_write_phy_reg(hw, PHY_CONTROL, 0x9140);
 		/* autoneg off */
 		igb_write_phy_reg(hw, PHY_CONTROL, 0x8140);
-	} else if (hw->phy.type == e1000_phy_82580) {
+		break;
+	case e1000_phy_82580:
 		/* enable MII loopback */
 		igb_write_phy_reg(hw, I82580_PHY_LBK_CTRL, 0x8041);
+		break;
+	case e1000_phy_i210:
+		/* set loopback speed in PHY */
+		igb_read_phy_reg(hw, (GS40G_PAGE_SELECT & GS40G_PAGE_2),
+					&phy_reg);
+		phy_reg |= GS40G_MAC_SPEED_1G;
+		igb_write_phy_reg(hw, (GS40G_PAGE_SELECT & GS40G_PAGE_2),
+					phy_reg);
+		ctrl_reg = rd32(E1000_CTRL_EXT);
+	default:
+		break;
 	}
 
-	ctrl_reg = rd32(E1000_CTRL);
-
 	/* force 1000, set loopback */
 	igb_write_phy_reg(hw, PHY_CONTROL, 0x4140);
 
@@ -1431,7 +1512,7 @@ static int igb_integrated_phy_loopback(struct igb_adapter *adapter)
 		     E1000_CTRL_FD |	 /* Force Duplex to FULL */
 		     E1000_CTRL_SLU);	 /* Set link up enable bit */
 
-	if (hw->phy.type == e1000_phy_m88)
+	if ((hw->phy.type == e1000_phy_m88) || (hw->phy.type == e1000_phy_i210))
 		ctrl_reg |= E1000_CTRL_ILOS; /* Invert Loss of Signal */
 
 	wr32(E1000_CTRL, ctrl_reg);
@@ -1439,7 +1520,7 @@ static int igb_integrated_phy_loopback(struct igb_adapter *adapter)
 	/* Disable the receiver on the PHY so when a cable is plugged in, the
 	 * PHY does not begin to autoneg when a cable is reconnected to the NIC.
 	 */
-	if (hw->phy.type == e1000_phy_m88)
+	if ((hw->phy.type == e1000_phy_m88) || (hw->phy.type == e1000_phy_i210))
 		igb_phy_disable_receiver(adapter);
 
 	udelay(500);
@@ -1704,6 +1785,14 @@ static int igb_loopback_test(struct igb_adapter *adapter, u64 *data)
 		*data = 0;
 		goto out;
 	}
+	if ((adapter->hw.mac.type == e1000_i210)
+		|| (adapter->hw.mac.type == e1000_i210)) {
+		dev_err(&adapter->pdev->dev,
+			"Loopback test not supported "
+			"on this part at this time.\n");
+		*data = 0;
+		goto out;
+	}
 	*data = igb_setup_desc_rings(adapter);
 	if (*data)
 		goto out;
diff --git a/drivers/net/ethernet/intel/igb/igb_main.c b/drivers/net/ethernet/intel/igb/igb_main.c
index 5ec31598ee4..dd3bfe8cd36 100644
--- a/drivers/net/ethernet/intel/igb/igb_main.c
+++ b/drivers/net/ethernet/intel/igb/igb_main.c
@@ -60,8 +60,8 @@
 #include "igb.h"
 
 #define MAJ 3
-#define MIN 2
-#define BUILD 10
+#define MIN 4
+#define BUILD 7
 #define DRV_VERSION __stringify(MAJ) "." __stringify(MIN) "." \
 __stringify(BUILD) "-k"
 char igb_driver_name[] = "igb";
@@ -75,6 +75,11 @@ static const struct e1000_info *igb_info_tbl[] = {
 };
 
 static DEFINE_PCI_DEVICE_TABLE(igb_pci_tbl) = {
+	{ 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 },
+	{ PCI_VDEVICE(INTEL, E1000_DEV_ID_I210_SERDES), board_82575 },
+	{ PCI_VDEVICE(INTEL, E1000_DEV_ID_I210_SGMII), board_82575 },
 	{ PCI_VDEVICE(INTEL, E1000_DEV_ID_I350_COPPER), board_82575 },
 	{ PCI_VDEVICE(INTEL, E1000_DEV_ID_I350_FIBER), board_82575 },
 	{ PCI_VDEVICE(INTEL, E1000_DEV_ID_I350_SERDES), board_82575 },
@@ -114,7 +119,6 @@ static void igb_free_all_rx_resources(struct igb_adapter *);
 static void igb_setup_mrqc(struct igb_adapter *);
 static int igb_probe(struct pci_dev *, const struct pci_device_id *);
 static void __devexit igb_remove(struct pci_dev *pdev);
-static void igb_init_hw_timer(struct igb_adapter *adapter);
 static int igb_sw_init(struct igb_adapter *);
 static int igb_open(struct net_device *);
 static int igb_close(struct net_device *);
@@ -565,33 +569,6 @@ exit:
 	return;
 }
 
-
-/**
- * igb_read_clock - read raw cycle counter (to be used by time counter)
- */
-static cycle_t igb_read_clock(const struct cyclecounter *tc)
-{
-	struct igb_adapter *adapter =
-		container_of(tc, struct igb_adapter, cycles);
-	struct e1000_hw *hw = &adapter->hw;
-	u64 stamp = 0;
-	int shift = 0;
-
-	/*
-	 * The timestamp latches on lowest register read. For the 82580
-	 * the lowest register is SYSTIMR instead of SYSTIML.  However we never
-	 * adjusted TIMINCA so SYSTIMR will just read as all 0s so ignore it.
-	 */
-	if (hw->mac.type >= e1000_82580) {
-		stamp = rd32(E1000_SYSTIMR) >> 8;
-		shift = IGB_82580_TSYNC_SHIFT;
-	}
-
-	stamp |= (u64)rd32(E1000_SYSTIML) << shift;
-	stamp |= (u64)rd32(E1000_SYSTIMH) << (shift + 32);
-	return stamp;
-}
-
 /**
  * igb_get_hw_dev - return device
  * used by hardware layer to print debugging information
@@ -669,6 +646,8 @@ static void igb_cache_ring_register(struct igb_adapter *adapter)
 	case e1000_82575:
 	case e1000_82580:
 	case e1000_i350:
+	case e1000_i210:
+	case e1000_i211:
 	default:
 		for (; i < adapter->num_rx_queues; i++)
 			adapter->rx_ring[i]->reg_idx = rbase_offset + i;
@@ -755,8 +734,11 @@ static int igb_alloc_queues(struct igb_adapter *adapter)
 		if (adapter->hw.mac.type >= e1000_82576)
 			set_bit(IGB_RING_FLAG_RX_SCTP_CSUM, &ring->flags);
 
-		/* On i350, loopback VLAN packets have the tag byte-swapped. */
-		if (adapter->hw.mac.type == e1000_i350)
+		/*
+		 * On i350, 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);
 
 		adapter->rx_ring[i] = ring;
@@ -850,6 +832,8 @@ static void igb_assign_vector(struct igb_q_vector *q_vector, int msix_vector)
 		break;
 	case e1000_82580:
 	case e1000_i350:
+	case e1000_i210:
+	case e1000_i211:
 		/*
 		 * On 82580 and newer adapters the scheme is similar to 82576
 		 * however instead of ordering column-major we have things
@@ -916,6 +900,8 @@ static void igb_configure_msix(struct igb_adapter *adapter)
 	case e1000_82576:
 	case e1000_82580:
 	case e1000_i350:
+	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. */
 		wr32(E1000_GPIE, E1000_GPIE_MSIX_MODE |
@@ -1062,6 +1048,11 @@ static int igb_set_interrupt_capability(struct igb_adapter *adapter)
 	if (!(adapter->flags & IGB_FLAG_QUEUE_PAIRS))
 		numvecs += adapter->num_tx_queues;
 
+	/* i210 and i211 can only have 4 MSIX vectors for rx/tx queues. */
+	if ((adapter->hw.mac.type == e1000_i210)
+		|| (adapter->hw.mac.type == e1000_i211))
+		numvecs = 4;
+
 	/* store the number of vectors reserved for queues */
 	adapter->num_q_vectors = numvecs;
 
@@ -1069,6 +1060,7 @@ static int igb_set_interrupt_capability(struct igb_adapter *adapter)
 	numvecs++;
 	adapter->msix_entries = kcalloc(numvecs, sizeof(struct msix_entry),
 					GFP_KERNEL);
+
 	if (!adapter->msix_entries)
 		goto msi_only;
 
@@ -1111,9 +1103,12 @@ msi_only:
 		adapter->flags |= IGB_FLAG_HAS_MSI;
 out:
 	/* Notify the stack of the (possibly) reduced queue counts. */
+	rtnl_lock();
 	netif_set_real_num_tx_queues(adapter->netdev, adapter->num_tx_queues);
-	return netif_set_real_num_rx_queues(adapter->netdev,
-					    adapter->num_rx_queues);
+	err = netif_set_real_num_rx_queues(adapter->netdev,
+		adapter->num_rx_queues);
+	rtnl_unlock();
+	return err;
 }
 
 /**
@@ -1659,6 +1654,8 @@ void igb_reset(struct igb_adapter *adapter)
 		pba &= E1000_RXPBS_SIZE_MASK_82576;
 		break;
 	case e1000_82575:
+	case e1000_i210:
+	case e1000_i211:
 	default:
 		pba = E1000_PBA_34K;
 		break;
@@ -1743,6 +1740,13 @@ 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
+	 * control is off when forcing speed.
+	 */
+	if (!hw->mac.autoneg)
+		igb_force_mac_fc(hw);
+
 	igb_init_dmac(adapter, pba);
 	if (!netif_running(adapter->netdev))
 		igb_power_down_link(adapter);
@@ -1847,7 +1851,7 @@ static int __devinit igb_probe(struct pci_dev *pdev,
 	 */
 	if (pdev->is_virtfn) {
 		WARN(1, KERN_ERR "%s (%hx:%hx) should not be a VF!\n",
-		     pci_name(pdev), pdev->vendor, pdev->device);
+			pci_name(pdev), pdev->vendor, pdev->device);
 		return -EINVAL;
 	}
 
@@ -2001,11 +2005,16 @@ static int __devinit igb_probe(struct pci_dev *pdev,
 	 * known good starting state */
 	hw->mac.ops.reset_hw(hw);
 
-	/* make sure the NVM is good */
-	if (hw->nvm.ops.validate(hw) < 0) {
-		dev_err(&pdev->dev, "The NVM Checksum Is Not Valid\n");
-		err = -EIO;
-		goto err_eeprom;
+	/*
+	 * make sure the NVM is good , i211 parts have special NVM that
+	 * doesn't contain a checksum
+	 */
+	if (hw->mac.type != e1000_i211) {
+		if (hw->nvm.ops.validate(hw) < 0) {
+			dev_err(&pdev->dev, "The NVM Checksum Is Not Valid\n");
+			err = -EIO;
+			goto err_eeprom;
+		}
 	}
 
 	/* copy the MAC address out of the NVM */
@@ -2110,9 +2119,11 @@ static int __devinit igb_probe(struct pci_dev *pdev,
 	}
 
 #endif
+#ifdef CONFIG_IGB_PTP
 	/* do hw tstamp init after resetting */
-	igb_init_hw_timer(adapter);
+	igb_ptp_init(adapter);
 
+#endif
 	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",
@@ -2137,6 +2148,8 @@ static int __devinit igb_probe(struct pci_dev *pdev,
 		adapter->num_rx_queues, adapter->num_tx_queues);
 	switch (hw->mac.type) {
 	case e1000_i350:
+	case e1000_i210:
+	case e1000_i211:
 		igb_set_eee_i350(hw);
 		break;
 	default:
@@ -2184,7 +2197,10 @@ static void __devexit igb_remove(struct pci_dev *pdev)
 	struct e1000_hw *hw = &adapter->hw;
 
 	pm_runtime_get_noresume(&pdev->dev);
+#ifdef CONFIG_IGB_PTP
+	igb_ptp_remove(adapter);
 
+#endif
 	/*
 	 * The watchdog timer may be rescheduled, so explicitly
 	 * disable watchdog from being rescheduled.
@@ -2260,9 +2276,14 @@ static void __devinit igb_probe_vfs(struct igb_adapter * adapter)
 {
 #ifdef CONFIG_PCI_IOV
 	struct pci_dev *pdev = adapter->pdev;
+	struct e1000_hw *hw = &adapter->hw;
 	int old_vfs = igb_find_enabled_vfs(adapter);
 	int i;
 
+	/* Virtualization features not supported on i210 family. */
+	if ((hw->mac.type == e1000_i210) || (hw->mac.type == e1000_i211))
+		return;
+
 	if (old_vfs) {
 		dev_info(&pdev->dev, "%d pre-allocated VFs found - override "
 			 "max_vfs setting of %d\n", old_vfs, max_vfs);
@@ -2274,6 +2295,7 @@ static void __devinit igb_probe_vfs(struct igb_adapter * adapter)
 
 	adapter->vf_data = kcalloc(adapter->vfs_allocated_count,
 				sizeof(struct vf_data_storage), GFP_KERNEL);
+
 	/* if allocation failed then we do not support SR-IOV */
 	if (!adapter->vf_data) {
 		adapter->vfs_allocated_count = 0;
@@ -2304,112 +2326,6 @@ out:
 }
 
 /**
- * igb_init_hw_timer - Initialize hardware timer used with IEEE 1588 timestamp
- * @adapter: board private structure to initialize
- *
- * igb_init_hw_timer initializes the function pointer and values for the hw
- * timer found in hardware.
- **/
-static void igb_init_hw_timer(struct igb_adapter *adapter)
-{
-	struct e1000_hw *hw = &adapter->hw;
-
-	switch (hw->mac.type) {
-	case e1000_i350:
-	case e1000_82580:
-		memset(&adapter->cycles, 0, sizeof(adapter->cycles));
-		adapter->cycles.read = igb_read_clock;
-		adapter->cycles.mask = CLOCKSOURCE_MASK(64);
-		adapter->cycles.mult = 1;
-		/*
-		 * The 82580 timesync updates the system timer every 8ns by 8ns
-		 * and the value cannot be shifted.  Instead we need to shift
-		 * the registers to generate a 64bit timer value.  As a result
-		 * SYSTIMR/L/H, TXSTMPL/H, RXSTMPL/H all have to be shifted by
-		 * 24 in order to generate a larger value for synchronization.
-		 */
-		adapter->cycles.shift = IGB_82580_TSYNC_SHIFT;
-		/* disable system timer temporarily by setting bit 31 */
-		wr32(E1000_TSAUXC, 0x80000000);
-		wrfl();
-
-		/* Set registers so that rollover occurs soon to test this. */
-		wr32(E1000_SYSTIMR, 0x00000000);
-		wr32(E1000_SYSTIML, 0x80000000);
-		wr32(E1000_SYSTIMH, 0x000000FF);
-		wrfl();
-
-		/* enable system timer by clearing bit 31 */
-		wr32(E1000_TSAUXC, 0x0);
-		wrfl();
-
-		timecounter_init(&adapter->clock,
-				 &adapter->cycles,
-				 ktime_to_ns(ktime_get_real()));
-		/*
-		 * Synchronize our NIC clock against system wall clock. NIC
-		 * time stamp reading requires ~3us per sample, each sample
-		 * was pretty stable even under load => only require 10
-		 * samples for each offset comparison.
-		 */
-		memset(&adapter->compare, 0, sizeof(adapter->compare));
-		adapter->compare.source = &adapter->clock;
-		adapter->compare.target = ktime_get_real;
-		adapter->compare.num_samples = 10;
-		timecompare_update(&adapter->compare, 0);
-		break;
-	case e1000_82576:
-		/*
-		 * Initialize hardware timer: we keep it running just in case
-		 * that some program needs it later on.
-		 */
-		memset(&adapter->cycles, 0, sizeof(adapter->cycles));
-		adapter->cycles.read = igb_read_clock;
-		adapter->cycles.mask = CLOCKSOURCE_MASK(64);
-		adapter->cycles.mult = 1;
-		/**
-		 * Scale the NIC clock cycle by a large factor so that
-		 * relatively small clock corrections can be added or
-		 * subtracted at each clock tick. The drawbacks of a large
-		 * factor are a) that the clock register overflows more quickly
-		 * (not such a big deal) and b) that the increment per tick has
-		 * to fit into 24 bits.  As a result we need to use a shift of
-		 * 19 so we can fit a value of 16 into the TIMINCA register.
-		 */
-		adapter->cycles.shift = IGB_82576_TSYNC_SHIFT;
-		wr32(E1000_TIMINCA,
-		                (1 << E1000_TIMINCA_16NS_SHIFT) |
-		                (16 << IGB_82576_TSYNC_SHIFT));
-
-		/* Set registers so that rollover occurs soon to test this. */
-		wr32(E1000_SYSTIML, 0x00000000);
-		wr32(E1000_SYSTIMH, 0xFF800000);
-		wrfl();
-
-		timecounter_init(&adapter->clock,
-				 &adapter->cycles,
-				 ktime_to_ns(ktime_get_real()));
-		/*
-		 * Synchronize our NIC clock against system wall clock. NIC
-		 * time stamp reading requires ~3us per sample, each sample
-		 * was pretty stable even under load => only require 10
-		 * samples for each offset comparison.
-		 */
-		memset(&adapter->compare, 0, sizeof(adapter->compare));
-		adapter->compare.source = &adapter->clock;
-		adapter->compare.target = ktime_get_real;
-		adapter->compare.num_samples = 10;
-		timecompare_update(&adapter->compare, 0);
-		break;
-	case e1000_82575:
-		/* 82575 does not support timesync */
-	default:
-		break;
-	}
-
-}
-
-/**
  * igb_sw_init - Initialize general software structures (struct igb_adapter)
  * @adapter: board private structure to initialize
  *
@@ -2454,11 +2370,28 @@ static int __devinit igb_sw_init(struct igb_adapter *adapter)
 		} else
 			adapter->vfs_allocated_count = max_vfs;
 		break;
+	case e1000_i210:
+	case e1000_i211:
+		adapter->vfs_allocated_count = 0;
+		break;
 	default:
 		break;
 	}
 #endif /* CONFIG_PCI_IOV */
-	adapter->rss_queues = min_t(u32, IGB_MAX_RX_QUEUES, num_online_cpus());
+	switch (hw->mac.type) {
+	case e1000_i210:
+		adapter->rss_queues = min_t(u32, IGB_MAX_RX_QUEUES_I210,
+			num_online_cpus());
+		break;
+	case e1000_i211:
+		adapter->rss_queues = min_t(u32, IGB_MAX_RX_QUEUES_I211,
+			num_online_cpus());
+		break;
+	default:
+		adapter->rss_queues = min_t(u32, IGB_MAX_RX_QUEUES,
+		num_online_cpus());
+		break;
+	}
 	/* i350 cannot do RSS and SR-IOV at the same time */
 	if (hw->mac.type == e1000_i350 && adapter->vfs_allocated_count)
 		adapter->rss_queues = 1;
@@ -2488,7 +2421,7 @@ static int __devinit igb_sw_init(struct igb_adapter *adapter)
 	/* Explicitly disable IRQ since the NIC can be in any state. */
 	igb_irq_disable(adapter);
 
-	if (hw->mac.type == e1000_i350)
+	if (hw->mac.type >= e1000_i350)
 		adapter->flags &= ~IGB_FLAG_DMAC;
 
 	set_bit(__IGB_DOWN, &adapter->state);
@@ -2771,8 +2704,6 @@ void igb_configure_tx_ring(struct igb_adapter *adapter,
 
 	txdctl |= E1000_TXDCTL_QUEUE_ENABLE;
 	wr32(E1000_TXDCTL(reg_idx), txdctl);
-
-	netdev_tx_reset_queue(txring_txq(ring));
 }
 
 /**
@@ -2943,6 +2874,17 @@ static void igb_setup_mrqc(struct igb_adapter *adapter)
 
 	/* Don't need to set TUOFL or IPOFL, they default to 1 */
 	wr32(E1000_RXCSUM, rxcsum);
+	/*
+	 * Generate RSS hash based on TCP port numbers and/or
+	 * IPv4/v6 src and dst addresses since UDP cannot be
+	 * hashed reliably due to IP fragmentation
+	 */
+
+	mrqc = E1000_MRQC_RSS_FIELD_IPV4 |
+	       E1000_MRQC_RSS_FIELD_IPV4_TCP |
+	       E1000_MRQC_RSS_FIELD_IPV6 |
+	       E1000_MRQC_RSS_FIELD_IPV6_TCP |
+	       E1000_MRQC_RSS_FIELD_IPV6_TCP_EX;
 
 	/* 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
@@ -2958,25 +2900,15 @@ static void igb_setup_mrqc(struct igb_adapter *adapter)
 			wr32(E1000_VT_CTL, vtctl);
 		}
 		if (adapter->rss_queues > 1)
-			mrqc = E1000_MRQC_ENABLE_VMDQ_RSS_2Q;
+			mrqc |= E1000_MRQC_ENABLE_VMDQ_RSS_2Q;
 		else
-			mrqc = E1000_MRQC_ENABLE_VMDQ;
+			mrqc |= E1000_MRQC_ENABLE_VMDQ;
 	} else {
-		mrqc = E1000_MRQC_ENABLE_RSS_4Q;
+		if (hw->mac.type != e1000_i211)
+			mrqc |= E1000_MRQC_ENABLE_RSS_4Q;
 	}
 	igb_vmm_control(adapter);
 
-	/*
-	 * Generate RSS hash based on TCP port numbers and/or
-	 * IPv4/v6 src and dst addresses since UDP cannot be
-	 * hashed reliably due to IP fragmentation
-	 */
-	mrqc |= E1000_MRQC_RSS_FIELD_IPV4 |
-		E1000_MRQC_RSS_FIELD_IPV4_TCP |
-		E1000_MRQC_RSS_FIELD_IPV6 |
-		E1000_MRQC_RSS_FIELD_IPV6_TCP |
-		E1000_MRQC_RSS_FIELD_IPV6_TCP_EX;
-
 	wr32(E1000_MRQC, mrqc);
 }
 
@@ -3282,6 +3214,8 @@ static void igb_clean_tx_ring(struct igb_ring *tx_ring)
 		igb_unmap_and_free_tx_resource(tx_ring, buffer_info);
 	}
 
+	netdev_tx_reset_queue(txring_txq(tx_ring));
+
 	size = sizeof(struct igb_tx_buffer) * tx_ring->count;
 	memset(tx_ring->tx_buffer_info, 0, size);
 
@@ -3576,7 +3510,7 @@ static void igb_set_rx_mode(struct net_device *netdev)
 	 * we will have issues with VLAN tag stripping not being done for frames
 	 * that are only arriving because we are the default pool
 	 */
-	if (hw->mac.type < e1000_82576)
+	if ((hw->mac.type < e1000_82576) || (hw->mac.type > e1000_i350))
 		return;
 
 	vmolr |= rd32(E1000_VMOLR(vfn)) &
@@ -3673,7 +3607,7 @@ static bool igb_thermal_sensor_event(struct e1000_hw *hw, u32 event)
 	bool ret = false;
 	u32 ctrl_ext, thstat;
 
-	/* check for thermal sensor event on i350, copper only */
+	/* check for thermal sensor event on i350 copper only */
 	if (hw->mac.type == e1000_i350) {
 		thstat = rd32(E1000_THSTAT);
 		ctrl_ext = rd32(E1000_CTRL_EXT);
@@ -5718,35 +5652,7 @@ static int igb_poll(struct napi_struct *napi, int budget)
 	return 0;
 }
 
-/**
- * igb_systim_to_hwtstamp - convert system time value to hw timestamp
- * @adapter: board private structure
- * @shhwtstamps: timestamp structure to update
- * @regval: unsigned 64bit system time value.
- *
- * We need to convert the system time value stored in the RX/TXSTMP registers
- * into a hwtstamp which can be used by the upper level timestamping functions
- */
-static void igb_systim_to_hwtstamp(struct igb_adapter *adapter,
-                                   struct skb_shared_hwtstamps *shhwtstamps,
-                                   u64 regval)
-{
-	u64 ns;
-
-	/*
-	 * The 82580 starts with 1ns at bit 0 in RX/TXSTMPL, shift this up to
-	 * 24 to match clock shift we setup earlier.
-	 */
-	if (adapter->hw.mac.type >= e1000_82580)
-		regval <<= IGB_82580_TSYNC_SHIFT;
-
-	ns = timecounter_cyc2time(&adapter->clock, regval);
-	timecompare_update(&adapter->compare, ns);
-	memset(shhwtstamps, 0, sizeof(struct skb_shared_hwtstamps));
-	shhwtstamps->hwtstamp = ns_to_ktime(ns);
-	shhwtstamps->syststamp = timecompare_transform(&adapter->compare, ns);
-}
-
+#ifdef CONFIG_IGB_PTP
 /**
  * igb_tx_hwtstamp - utility function which checks for TX time stamp
  * @q_vector: pointer to q_vector containing needed info
@@ -5776,6 +5682,7 @@ static void igb_tx_hwtstamp(struct igb_q_vector *q_vector,
 	skb_tstamp_tx(buffer_info->skb, &shhwtstamps);
 }
 
+#endif
 /**
  * igb_clean_tx_irq - Reclaim resources after transmit completes
  * @q_vector: pointer to q_vector containing needed info
@@ -5819,9 +5726,11 @@ static bool igb_clean_tx_irq(struct igb_q_vector *q_vector)
 		total_bytes += tx_buffer->bytecount;
 		total_packets += tx_buffer->gso_segs;
 
+#ifdef CONFIG_IGB_PTP
 		/* retrieve hardware timestamp */
 		igb_tx_hwtstamp(q_vector, tx_buffer);
 
+#endif
 		/* free the skb */
 		dev_kfree_skb_any(tx_buffer->skb);
 		tx_buffer->skb = NULL;
@@ -5993,6 +5902,7 @@ static inline void igb_rx_hash(struct igb_ring *ring,
 		skb->rxhash = le32_to_cpu(rx_desc->wb.lower.hi_dword.rss);
 }
 
+#ifdef CONFIG_IGB_PTP
 static void igb_rx_hwtstamp(struct igb_q_vector *q_vector,
 			    union e1000_adv_rx_desc *rx_desc,
 			    struct sk_buff *skb)
@@ -6032,6 +5942,7 @@ static void igb_rx_hwtstamp(struct igb_q_vector *q_vector,
 	igb_systim_to_hwtstamp(adapter, skb_hwtstamps(skb), regval);
 }
 
+#endif
 static void igb_rx_vlan(struct igb_ring *ring,
 			union e1000_adv_rx_desc *rx_desc,
 			struct sk_buff *skb)
@@ -6142,7 +6053,9 @@ static bool igb_clean_rx_irq(struct igb_q_vector *q_vector, int budget)
 			goto next_desc;
 		}
 
+#ifdef CONFIG_IGB_PTP
 		igb_rx_hwtstamp(q_vector, rx_desc, skb);
+#endif
 		igb_rx_hash(rx_ring, rx_desc, skb);
 		igb_rx_checksum(rx_ring, rx_desc, skb);
 		igb_rx_vlan(rx_ring, rx_desc, skb);
@@ -6796,18 +6709,7 @@ static int igb_resume(struct device *dev)
 	pci_enable_wake(pdev, PCI_D3hot, 0);
 	pci_enable_wake(pdev, PCI_D3cold, 0);
 
-	if (!rtnl_is_locked()) {
-		/*
-		 * shut up ASSERT_RTNL() warning in
-		 * netif_set_real_num_tx/rx_queues.
-		 */
-		rtnl_lock();
-		err = igb_init_interrupt_scheme(adapter);
-		rtnl_unlock();
-	} else {
-		err = igb_init_interrupt_scheme(adapter);
-	}
-	if (err) {
+	if (igb_init_interrupt_scheme(adapter)) {
 		dev_err(&pdev->dev, "Unable to allocate memory for queues\n");
 		return -ENOMEM;
 	}
@@ -7170,6 +7072,8 @@ static void igb_vmm_control(struct igb_adapter *adapter)
 
 	switch (hw->mac.type) {
 	case e1000_82575:
+	case e1000_i210:
+	case e1000_i211:
 	default:
 		/* replication is not supported for 82575 */
 		return;
@@ -7243,6 +7147,9 @@ static void igb_init_dmac(struct igb_adapter *adapter, u32 pba)
 
 			/* watchdog timer= +-1000 usec in 32usec intervals */
 			reg |= (1000 >> 5);
+
+			/* Disable BMC-to-OS Watchdog Enable */
+			reg &= ~E1000_DMACR_DC_BMC2OSW_EN;
 			wr32(E1000_DMACR, reg);
 
 			/*
diff --git a/drivers/net/ethernet/intel/igb/igb_ptp.c b/drivers/net/ethernet/intel/igb/igb_ptp.c
new file mode 100644
index 00000000000..d5ee7fa5072
--- /dev/null
+++ b/drivers/net/ethernet/intel/igb/igb_ptp.c
@@ -0,0 +1,385 @@
+/*
+ * PTP Hardware Clock (PHC) driver for the Intel 82576 and 82580
+ *
+ * Copyright (C) 2011 Richard Cochran <richardcochran@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.,
+ * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
+ */
+#include <linux/module.h>
+#include <linux/device.h>
+#include <linux/pci.h>
+
+#include "igb.h"
+
+#define INCVALUE_MASK		0x7fffffff
+#define ISGN			0x80000000
+
+/*
+ * 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
+ * nanoseconds time values for very long. For the 82580, SYSTIM always
+ * counts nanoseconds, but the upper 24 bits are not availible. The
+ * frequency is adjusted by changing the 32 bit fractional nanoseconds
+ * register, TIMINCA.
+ *
+ * For the 82576, the SYSTIM register time unit is affect by the
+ * choice of the 24 bit TININCA:IV (incvalue) field. Five bits of this
+ * field are needed to provide the nominal 16 nanosecond period,
+ * leaving 19 bits for fractional nanoseconds.
+ *
+ * We scale the NIC clock cycle by a large factor so that relatively
+ * small clock corrections can be added or subtracted at each clock
+ * tick. The drawbacks of a large factor are a) that the clock
+ * register overflows more quickly (not such a big deal) and b) that
+ * the increment per tick has to fit into 24 bits.  As a result we
+ * need to use a shift of 19 so we can fit a value of 16 into the
+ * TIMINCA register.
+ *
+ *
+ *             SYSTIMH            SYSTIML
+ *        +--------------+   +---+---+------+
+ *  82576 |      32      |   | 8 | 5 |  19  |
+ *        +--------------+   +---+---+------+
+ *         \________ 45 bits _______/  fract
+ *
+ *        +----------+---+   +--------------+
+ *  82580 |    24    | 8 |   |      32      |
+ *        +----------+---+   +--------------+
+ *          reserved  \______ 40 bits _____/
+ *
+ *
+ * The 45 bit 82576 SYSTIM overflows every
+ *   2^45 * 10^-9 / 3600 = 9.77 hours.
+ *
+ * The 40 bit 82580 SYSTIM overflows every
+ *   2^40 * 10^-9 /  60  = 18.3 minutes.
+ */
+
+#define IGB_OVERFLOW_PERIOD	(HZ * 60 * 9)
+#define INCPERIOD_82576		(1 << E1000_TIMINCA_16NS_SHIFT)
+#define INCVALUE_82576_MASK	((1 << E1000_TIMINCA_16NS_SHIFT) - 1)
+#define INCVALUE_82576		(16 << IGB_82576_TSYNC_SHIFT)
+#define IGB_NBITS_82580		40
+
+/*
+ * SYSTIM read access for the 82576
+ */
+
+static cycle_t igb_82576_systim_read(const struct cyclecounter *cc)
+{
+	u64 val;
+	u32 lo, hi;
+	struct igb_adapter *igb = container_of(cc, struct igb_adapter, cc);
+	struct e1000_hw *hw = &igb->hw;
+
+	lo = rd32(E1000_SYSTIML);
+	hi = rd32(E1000_SYSTIMH);
+
+	val = ((u64) hi) << 32;
+	val |= lo;
+
+	return val;
+}
+
+/*
+ * SYSTIM read access for the 82580
+ */
+
+static cycle_t igb_82580_systim_read(const struct cyclecounter *cc)
+{
+	u64 val;
+	u32 lo, hi, jk;
+	struct igb_adapter *igb = container_of(cc, struct igb_adapter, cc);
+	struct e1000_hw *hw = &igb->hw;
+
+	/*
+	 * 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.
+	 */
+	jk = rd32(E1000_SYSTIMR);
+	lo = rd32(E1000_SYSTIML);
+	hi = rd32(E1000_SYSTIMH);
+
+	val = ((u64) hi) << 32;
+	val |= lo;
+
+	return val;
+}
+
+/*
+ * PTP clock operations
+ */
+
+static int ptp_82576_adjfreq(struct ptp_clock_info *ptp, s32 ppb)
+{
+	u64 rate;
+	u32 incvalue;
+	int neg_adj = 0;
+	struct igb_adapter *igb = container_of(ptp, struct igb_adapter, caps);
+	struct e1000_hw *hw = &igb->hw;
+
+	if (ppb < 0) {
+		neg_adj = 1;
+		ppb = -ppb;
+	}
+	rate = ppb;
+	rate <<= 14;
+	rate = div_u64(rate, 1953125);
+
+	incvalue = 16 << IGB_82576_TSYNC_SHIFT;
+
+	if (neg_adj)
+		incvalue -= rate;
+	else
+		incvalue += rate;
+
+	wr32(E1000_TIMINCA, INCPERIOD_82576 | (incvalue & INCVALUE_82576_MASK));
+
+	return 0;
+}
+
+static int ptp_82580_adjfreq(struct ptp_clock_info *ptp, s32 ppb)
+{
+	u64 rate;
+	u32 inca;
+	int neg_adj = 0;
+	struct igb_adapter *igb = container_of(ptp, struct igb_adapter, caps);
+	struct e1000_hw *hw = &igb->hw;
+
+	if (ppb < 0) {
+		neg_adj = 1;
+		ppb = -ppb;
+	}
+	rate = ppb;
+	rate <<= 26;
+	rate = div_u64(rate, 1953125);
+
+	inca = rate & INCVALUE_MASK;
+	if (neg_adj)
+		inca |= ISGN;
+
+	wr32(E1000_TIMINCA, inca);
+
+	return 0;
+}
+
+static int igb_adjtime(struct ptp_clock_info *ptp, s64 delta)
+{
+	s64 now;
+	unsigned long flags;
+	struct igb_adapter *igb = container_of(ptp, struct igb_adapter, caps);
+
+	spin_lock_irqsave(&igb->tmreg_lock, flags);
+
+	now = timecounter_read(&igb->tc);
+	now += delta;
+	timecounter_init(&igb->tc, &igb->cc, now);
+
+	spin_unlock_irqrestore(&igb->tmreg_lock, flags);
+
+	return 0;
+}
+
+static int igb_gettime(struct ptp_clock_info *ptp, struct timespec *ts)
+{
+	u64 ns;
+	u32 remainder;
+	unsigned long flags;
+	struct igb_adapter *igb = container_of(ptp, struct igb_adapter, caps);
+
+	spin_lock_irqsave(&igb->tmreg_lock, flags);
+
+	ns = timecounter_read(&igb->tc);
+
+	spin_unlock_irqrestore(&igb->tmreg_lock, flags);
+
+	ts->tv_sec = div_u64_rem(ns, 1000000000, &remainder);
+	ts->tv_nsec = remainder;
+
+	return 0;
+}
+
+static int igb_settime(struct ptp_clock_info *ptp, const struct timespec *ts)
+{
+	u64 ns;
+	unsigned long flags;
+	struct igb_adapter *igb = container_of(ptp, struct igb_adapter, caps);
+
+	ns = ts->tv_sec * 1000000000ULL;
+	ns += ts->tv_nsec;
+
+	spin_lock_irqsave(&igb->tmreg_lock, flags);
+
+	timecounter_init(&igb->tc, &igb->cc, ns);
+
+	spin_unlock_irqrestore(&igb->tmreg_lock, flags);
+
+	return 0;
+}
+
+static int ptp_82576_enable(struct ptp_clock_info *ptp,
+			    struct ptp_clock_request *rq, int on)
+{
+	return -EOPNOTSUPP;
+}
+
+static int ptp_82580_enable(struct ptp_clock_info *ptp,
+			    struct ptp_clock_request *rq, int on)
+{
+	return -EOPNOTSUPP;
+}
+
+static void igb_overflow_check(struct work_struct *work)
+{
+	struct timespec ts;
+	struct igb_adapter *igb =
+		container_of(work, struct igb_adapter, overflow_work.work);
+
+	igb_gettime(&igb->caps, &ts);
+
+	pr_debug("igb overflow check at %ld.%09lu\n", ts.tv_sec, ts.tv_nsec);
+
+	schedule_delayed_work(&igb->overflow_work, IGB_OVERFLOW_PERIOD);
+}
+
+void igb_ptp_init(struct igb_adapter *adapter)
+{
+	struct e1000_hw *hw = &adapter->hw;
+
+	switch (hw->mac.type) {
+	case e1000_i210:
+	case e1000_i211:
+	case e1000_i350:
+	case e1000_82580:
+		adapter->caps.owner	= THIS_MODULE;
+		strcpy(adapter->caps.name, "igb-82580");
+		adapter->caps.max_adj	= 62499999;
+		adapter->caps.n_ext_ts	= 0;
+		adapter->caps.pps	= 0;
+		adapter->caps.adjfreq	= ptp_82580_adjfreq;
+		adapter->caps.adjtime	= igb_adjtime;
+		adapter->caps.gettime	= igb_gettime;
+		adapter->caps.settime	= igb_settime;
+		adapter->caps.enable	= ptp_82580_enable;
+		adapter->cc.read	= igb_82580_systim_read;
+		adapter->cc.mask	= CLOCKSOURCE_MASK(IGB_NBITS_82580);
+		adapter->cc.mult	= 1;
+		adapter->cc.shift	= 0;
+		/* Enable the timer functions by clearing bit 31. */
+		wr32(E1000_TSAUXC, 0x0);
+		break;
+
+	case e1000_82576:
+		adapter->caps.owner	= THIS_MODULE;
+		strcpy(adapter->caps.name, "igb-82576");
+		adapter->caps.max_adj	= 1000000000;
+		adapter->caps.n_ext_ts	= 0;
+		adapter->caps.pps	= 0;
+		adapter->caps.adjfreq	= ptp_82576_adjfreq;
+		adapter->caps.adjtime	= igb_adjtime;
+		adapter->caps.gettime	= igb_gettime;
+		adapter->caps.settime	= igb_settime;
+		adapter->caps.enable	= ptp_82576_enable;
+		adapter->cc.read	= igb_82576_systim_read;
+		adapter->cc.mask	= CLOCKSOURCE_MASK(64);
+		adapter->cc.mult	= 1;
+		adapter->cc.shift	= IGB_82576_TSYNC_SHIFT;
+		/* Dial the nominal frequency. */
+		wr32(E1000_TIMINCA, INCPERIOD_82576 | INCVALUE_82576);
+		break;
+
+	default:
+		adapter->ptp_clock = NULL;
+		return;
+	}
+
+	wrfl();
+
+	timecounter_init(&adapter->tc, &adapter->cc,
+			 ktime_to_ns(ktime_get_real()));
+
+	INIT_DELAYED_WORK(&adapter->overflow_work, igb_overflow_check);
+
+	spin_lock_init(&adapter->tmreg_lock);
+
+	schedule_delayed_work(&adapter->overflow_work, IGB_OVERFLOW_PERIOD);
+
+	adapter->ptp_clock = ptp_clock_register(&adapter->caps);
+	if (IS_ERR(adapter->ptp_clock)) {
+		adapter->ptp_clock = NULL;
+		dev_err(&adapter->pdev->dev, "ptp_clock_register failed\n");
+	} else
+		dev_info(&adapter->pdev->dev, "added PHC on %s\n",
+			 adapter->netdev->name);
+}
+
+void igb_ptp_remove(struct igb_adapter *adapter)
+{
+	cancel_delayed_work_sync(&adapter->overflow_work);
+
+	if (adapter->ptp_clock) {
+		ptp_clock_unregister(adapter->ptp_clock);
+		dev_info(&adapter->pdev->dev, "removed PHC on %s\n",
+			 adapter->netdev->name);
+	}
+}
+
+/**
+ * igb_systim_to_hwtstamp - convert system time value to hw timestamp
+ * @adapter: board private structure
+ * @hwtstamps: timestamp structure to update
+ * @systim: unsigned 64bit system time value.
+ *
+ * We need to convert the system time value stored in the RX/TXSTMP registers
+ * into a hwtstamp which can be used by the upper level timestamping functions.
+ *
+ * The 'tmreg_lock' spinlock is used to protect the consistency of the
+ * system time value. This is needed because reading the 64 bit time
+ * value involves reading two (or three) 32 bit registers. The first
+ * read latches the value. Ditto for writing.
+ *
+ * In addition, here have extended the system time with an overflow
+ * counter in software.
+ **/
+void igb_systim_to_hwtstamp(struct igb_adapter *adapter,
+			    struct skb_shared_hwtstamps *hwtstamps,
+			    u64 systim)
+{
+	u64 ns;
+	unsigned long flags;
+
+	switch (adapter->hw.mac.type) {
+	case e1000_i210:
+	case e1000_i211:
+	case e1000_i350:
+	case e1000_82580:
+	case e1000_82576:
+		break;
+	default:
+		return;
+	}
+
+	spin_lock_irqsave(&adapter->tmreg_lock, flags);
+
+	ns = timecounter_cyc2time(&adapter->tc, systim);
+
+	spin_unlock_irqrestore(&adapter->tmreg_lock, flags);
+
+	memset(hwtstamps, 0, sizeof(*hwtstamps));
+	hwtstamps->hwtstamp = ns_to_ktime(ns);
+}
diff --git a/drivers/net/ethernet/intel/ixgbe/Makefile b/drivers/net/ethernet/intel/ixgbe/Makefile
index 8be1d1b2132..0bdf06bc5c4 100644
--- a/drivers/net/ethernet/intel/ixgbe/Makefile
+++ b/drivers/net/ethernet/intel/ixgbe/Makefile
@@ -34,9 +34,11 @@ obj-$(CONFIG_IXGBE) += ixgbe.o
 
 ixgbe-objs := ixgbe_main.o ixgbe_common.o ixgbe_ethtool.o \
               ixgbe_82599.o ixgbe_82598.o ixgbe_phy.o ixgbe_sriov.o \
-              ixgbe_mbx.o ixgbe_x540.o ixgbe_lib.o
+              ixgbe_mbx.o ixgbe_x540.o ixgbe_sysfs.o ixgbe_lib.o
 
 ixgbe-$(CONFIG_IXGBE_DCB) +=  ixgbe_dcb.o ixgbe_dcb_82598.o \
                               ixgbe_dcb_82599.o ixgbe_dcb_nl.o
 
+ixgbe-$(CONFIG_IXGBE_PTP) += ixgbe_ptp.o
+
 ixgbe-$(CONFIG_FCOE:m=y) += ixgbe_fcoe.o
diff --git a/drivers/net/ethernet/intel/ixgbe/ixgbe.h b/drivers/net/ethernet/intel/ixgbe/ixgbe.h
index 74e192107f9..3ef3c5284e5 100644
--- a/drivers/net/ethernet/intel/ixgbe/ixgbe.h
+++ b/drivers/net/ethernet/intel/ixgbe/ixgbe.h
@@ -36,6 +36,12 @@
 #include <linux/aer.h>
 #include <linux/if_vlan.h>
 
+#ifdef CONFIG_IXGBE_PTP
+#include <linux/clocksource.h>
+#include <linux/net_tstamp.h>
+#include <linux/ptp_clock_kernel.h>
+#endif /* CONFIG_IXGBE_PTP */
+
 #include "ixgbe_type.h"
 #include "ixgbe_common.h"
 #include "ixgbe_dcb.h"
@@ -96,6 +102,7 @@
 #define IXGBE_TX_FLAGS_FCOE		(u32)(1 << 5)
 #define IXGBE_TX_FLAGS_FSO		(u32)(1 << 6)
 #define IXGBE_TX_FLAGS_TXSW		(u32)(1 << 7)
+#define IXGBE_TX_FLAGS_TSTAMP		(u32)(1 << 8)
 #define IXGBE_TX_FLAGS_VLAN_MASK	0xffff0000
 #define IXGBE_TX_FLAGS_VLAN_PRIO_MASK	0xe0000000
 #define IXGBE_TX_FLAGS_VLAN_PRIO_SHIFT  29
@@ -331,6 +338,26 @@ struct ixgbe_q_vector {
 	/* for dynamic allocation of rings associated with this q_vector */
 	struct ixgbe_ring ring[0] ____cacheline_internodealigned_in_smp;
 };
+#ifdef CONFIG_IXGBE_HWMON
+
+#define IXGBE_HWMON_TYPE_LOC		0
+#define IXGBE_HWMON_TYPE_TEMP		1
+#define IXGBE_HWMON_TYPE_CAUTION	2
+#define IXGBE_HWMON_TYPE_MAX		3
+
+struct hwmon_attr {
+	struct device_attribute dev_attr;
+	struct ixgbe_hw *hw;
+	struct ixgbe_thermal_diode_data *sensor;
+	char name[12];
+};
+
+struct hwmon_buff {
+	struct device *device;
+	struct hwmon_attr *hwmon_list;
+	unsigned int n_hwmon;
+};
+#endif /* CONFIG_IXGBE_HWMON */
 
 /*
  * microsecond values for various ITR rates shifted by 2 to fit itr register
@@ -438,6 +465,8 @@ struct ixgbe_adapter {
 #define IXGBE_FLAG2_FDIR_REQUIRES_REINIT        (u32)(1 << 7)
 #define IXGBE_FLAG2_RSS_FIELD_IPV4_UDP		(u32)(1 << 8)
 #define IXGBE_FLAG2_RSS_FIELD_IPV6_UDP		(u32)(1 << 9)
+#define IXGBE_FLAG2_OVERFLOW_CHECK_ENABLED	(u32)(1 << 10)
+#define IXGBE_FLAG2_PTP_PPS_ENABLED		(u32)(1 << 11)
 
 	/* Tx fast path data */
 	int num_tx_queues;
@@ -525,6 +554,17 @@ struct ixgbe_adapter {
 	u32 interrupt_event;
 	u32 led_reg;
 
+#ifdef CONFIG_IXGBE_PTP
+	struct ptp_clock *ptp_clock;
+	struct ptp_clock_info ptp_caps;
+	unsigned long last_overflow_check;
+	spinlock_t tmreg_lock;
+	struct cyclecounter cc;
+	struct timecounter tc;
+	u32 base_incval;
+	u32 cycle_speed;
+#endif /* CONFIG_IXGBE_PTP */
+
 	/* SR-IOV */
 	DECLARE_BITMAP(active_vfs, IXGBE_MAX_VF_FUNCTIONS);
 	unsigned int num_vfs;
@@ -535,6 +575,10 @@ struct ixgbe_adapter {
 
 	u32 timer_event_accumulator;
 	u32 vferr_refcount;
+	struct kobject *info_kobj;
+#ifdef CONFIG_IXGBE_HWMON
+	struct hwmon_buff ixgbe_hwmon_buff;
+#endif /* CONFIG_IXGBE_HWMON */
 };
 
 struct ixgbe_fdir_filter {
@@ -574,9 +618,6 @@ extern struct ixgbe_info ixgbe_82599_info;
 extern struct ixgbe_info ixgbe_X540_info;
 #ifdef CONFIG_IXGBE_DCB
 extern const struct dcbnl_rtnl_ops dcbnl_ops;
-extern int ixgbe_copy_dcb_cfg(struct ixgbe_dcb_config *src_dcb_cfg,
-                              struct ixgbe_dcb_config *dst_dcb_cfg,
-                              int tc_max);
 #endif
 
 extern char ixgbe_driver_name[];
@@ -600,6 +641,8 @@ extern void ixgbe_disable_rx_queue(struct ixgbe_adapter *adapter,
 				   struct ixgbe_ring *);
 extern void ixgbe_update_stats(struct ixgbe_adapter *adapter);
 extern int ixgbe_init_interrupt_scheme(struct ixgbe_adapter *adapter);
+extern int ixgbe_wol_supported(struct ixgbe_adapter *adapter, u16 device_id,
+			       u16 subdevice_id);
 extern void ixgbe_clear_interrupt_scheme(struct ixgbe_adapter *adapter);
 extern netdev_tx_t ixgbe_xmit_frame_ring(struct sk_buff *,
 					 struct ixgbe_adapter *,
@@ -629,10 +672,15 @@ extern void ixgbe_atr_compute_perfect_hash_82599(union ixgbe_atr_input *input,
 						 union ixgbe_atr_input *mask);
 extern void ixgbe_set_rx_mode(struct net_device *netdev);
 #ifdef CONFIG_IXGBE_DCB
+extern void ixgbe_set_rx_drop_en(struct ixgbe_adapter *adapter);
 extern int ixgbe_setup_tc(struct net_device *dev, u8 tc);
 #endif
 extern void ixgbe_tx_ctxtdesc(struct ixgbe_ring *, u32, u32, u32, u32);
 extern void ixgbe_do_reset(struct net_device *netdev);
+#ifdef CONFIG_IXGBE_HWMON
+extern void ixgbe_sysfs_exit(struct ixgbe_adapter *adapter);
+extern int ixgbe_sysfs_init(struct ixgbe_adapter *adapter);
+#endif /* CONFIG_IXGBE_HWMON */
 #ifdef IXGBE_FCOE
 extern void ixgbe_configure_fcoe(struct ixgbe_adapter *adapter);
 extern int ixgbe_fso(struct ixgbe_ring *tx_ring,
@@ -663,4 +711,18 @@ static inline struct netdev_queue *txring_txq(const struct ixgbe_ring *ring)
 	return netdev_get_tx_queue(ring->netdev, ring->queue_index);
 }
 
+#ifdef CONFIG_IXGBE_PTP
+extern void ixgbe_ptp_init(struct ixgbe_adapter *adapter);
+extern void ixgbe_ptp_stop(struct ixgbe_adapter *adapter);
+extern void ixgbe_ptp_overflow_check(struct ixgbe_adapter *adapter);
+extern void ixgbe_ptp_tx_hwtstamp(struct ixgbe_q_vector *q_vector,
+				  struct sk_buff *skb);
+extern void ixgbe_ptp_rx_hwtstamp(struct ixgbe_q_vector *q_vector,
+				  struct sk_buff *skb);
+extern int ixgbe_ptp_hwtstamp_ioctl(struct ixgbe_adapter *adapter,
+				    struct ifreq *ifr, int cmd);
+extern void ixgbe_ptp_start_cyclecounter(struct ixgbe_adapter *adapter);
+extern void ixgbe_ptp_check_pps_event(struct ixgbe_adapter *adapter, u32 eicr);
+#endif /* CONFIG_IXGBE_PTP */
+
 #endif /* _IXGBE_H_ */
diff --git a/drivers/net/ethernet/intel/ixgbe/ixgbe_82598.c b/drivers/net/ethernet/intel/ixgbe/ixgbe_82598.c
index 85d2e2c4ce4..42537336110 100644
--- a/drivers/net/ethernet/intel/ixgbe/ixgbe_82598.c
+++ b/drivers/net/ethernet/intel/ixgbe/ixgbe_82598.c
@@ -91,29 +91,6 @@ out:
 	IXGBE_WRITE_REG(hw, IXGBE_GCR, gcr);
 }
 
-/**
- *  ixgbe_get_pcie_msix_count_82598 - Gets MSI-X vector count
- *  @hw: pointer to hardware structure
- *
- *  Read PCIe configuration space, and get the MSI-X vector count from
- *  the capabilities table.
- **/
-static u16 ixgbe_get_pcie_msix_count_82598(struct ixgbe_hw *hw)
-{
-	struct ixgbe_adapter *adapter = hw->back;
-	u16 msix_count;
-	pci_read_config_word(adapter->pdev, IXGBE_PCIE_MSIX_82598_CAPS,
-	                     &msix_count);
-	msix_count &= IXGBE_PCIE_MSIX_TBL_SZ_MASK;
-
-	/* MSI-X count is zero-based in HW, so increment to give proper value */
-	msix_count++;
-
-	return msix_count;
-}
-
-/**
- */
 static s32 ixgbe_get_invariants_82598(struct ixgbe_hw *hw)
 {
 	struct ixgbe_mac_info *mac = &hw->mac;
@@ -126,7 +103,7 @@ static s32 ixgbe_get_invariants_82598(struct ixgbe_hw *hw)
 	mac->num_rar_entries = IXGBE_82598_RAR_ENTRIES;
 	mac->max_rx_queues = IXGBE_82598_MAX_RX_QUEUES;
 	mac->max_tx_queues = IXGBE_82598_MAX_TX_QUEUES;
-	mac->max_msix_vectors = ixgbe_get_pcie_msix_count_82598(hw);
+	mac->max_msix_vectors = ixgbe_get_pcie_msix_count_generic(hw);
 
 	return 0;
 }
@@ -347,24 +324,33 @@ out:
 /**
  *  ixgbe_fc_enable_82598 - Enable flow control
  *  @hw: pointer to hardware structure
- *  @packetbuf_num: packet buffer number (0-7)
  *
  *  Enable flow control according to the current settings.
  **/
-static s32 ixgbe_fc_enable_82598(struct ixgbe_hw *hw, s32 packetbuf_num)
+static s32 ixgbe_fc_enable_82598(struct ixgbe_hw *hw)
 {
 	s32 ret_val = 0;
 	u32 fctrl_reg;
 	u32 rmcs_reg;
 	u32 reg;
+	u32 fcrtl, fcrth;
 	u32 link_speed = 0;
+	int i;
 	bool link_up;
 
-#ifdef CONFIG_DCB
-	if (hw->fc.requested_mode == ixgbe_fc_pfc)
+	/*
+	 * Validate the water mark configuration for packet buffer 0.  Zero
+	 * water marks indicate that the packet buffer was not configured
+	 * and the watermarks for packet buffer 0 should always be configured.
+	 */
+	if (!hw->fc.low_water ||
+	    !hw->fc.high_water[0] ||
+	    !hw->fc.pause_time) {
+		hw_dbg(hw, "Invalid water mark configuration\n");
+		ret_val = IXGBE_ERR_INVALID_LINK_SETTINGS;
 		goto out;
+	}
 
-#endif /* CONFIG_DCB */
 	/*
 	 * On 82598 having Rx FC on causes resets while doing 1G
 	 * so if it's on turn it off once we know link_speed. For
@@ -386,9 +372,7 @@ static s32 ixgbe_fc_enable_82598(struct ixgbe_hw *hw, s32 packetbuf_num)
 	}
 
 	/* Negotiate the fc mode to use */
-	ret_val = ixgbe_fc_autoneg(hw);
-	if (ret_val == IXGBE_ERR_FLOW_CONTROL)
-		goto out;
+	ixgbe_fc_autoneg(hw);
 
 	/* Disable any previous flow control settings */
 	fctrl_reg = IXGBE_READ_REG(hw, IXGBE_FCTRL);
@@ -405,9 +389,6 @@ static s32 ixgbe_fc_enable_82598(struct ixgbe_hw *hw, s32 packetbuf_num)
 	 * 2: Tx flow control is enabled (we can send pause frames but
 	 *     we do not support receiving pause frames).
 	 * 3: Both Rx and Tx flow control (symmetric) are enabled.
-#ifdef CONFIG_DCB
-	 * 4: Priority Flow Control is enabled.
-#endif
 	 * other: Invalid.
 	 */
 	switch (hw->fc.current_mode) {
@@ -440,11 +421,6 @@ static s32 ixgbe_fc_enable_82598(struct ixgbe_hw *hw, s32 packetbuf_num)
 		fctrl_reg |= IXGBE_FCTRL_RFCE;
 		rmcs_reg |= IXGBE_RMCS_TFCE_802_3X;
 		break;
-#ifdef CONFIG_DCB
-	case ixgbe_fc_pfc:
-		goto out;
-		break;
-#endif /* CONFIG_DCB */
 	default:
 		hw_dbg(hw, "Flow control param set incorrectly\n");
 		ret_val = IXGBE_ERR_CONFIG;
@@ -457,29 +433,29 @@ static s32 ixgbe_fc_enable_82598(struct ixgbe_hw *hw, s32 packetbuf_num)
 	IXGBE_WRITE_REG(hw, IXGBE_FCTRL, fctrl_reg);
 	IXGBE_WRITE_REG(hw, IXGBE_RMCS, rmcs_reg);
 
-	/* Set up and enable Rx high/low water mark thresholds, enable XON. */
-	if (hw->fc.current_mode & ixgbe_fc_tx_pause) {
-		reg = hw->fc.low_water << 6;
-		if (hw->fc.send_xon)
-			reg |= IXGBE_FCRTL_XONE;
-
-		IXGBE_WRITE_REG(hw, IXGBE_FCRTL(packetbuf_num), reg);
+	fcrtl = (hw->fc.low_water << 10) | IXGBE_FCRTL_XONE;
 
-		reg = hw->fc.high_water[packetbuf_num] << 6;
-		reg |= IXGBE_FCRTH_FCEN;
+	/* Set up and enable Rx high/low water mark thresholds, enable XON. */
+	for (i = 0; i < MAX_TRAFFIC_CLASS; i++) {
+		if ((hw->fc.current_mode & ixgbe_fc_tx_pause) &&
+		    hw->fc.high_water[i]) {
+			fcrth = (hw->fc.high_water[i] << 10) | IXGBE_FCRTH_FCEN;
+			IXGBE_WRITE_REG(hw, IXGBE_FCRTL(i), fcrtl);
+			IXGBE_WRITE_REG(hw, IXGBE_FCRTH(i), fcrth);
+		} else {
+			IXGBE_WRITE_REG(hw, IXGBE_FCRTL(i), 0);
+			IXGBE_WRITE_REG(hw, IXGBE_FCRTH(i), 0);
+		}
 
-		IXGBE_WRITE_REG(hw, IXGBE_FCRTH(packetbuf_num), reg);
 	}
 
 	/* Configure pause time (2 TCs per register) */
-	reg = IXGBE_READ_REG(hw, IXGBE_FCTTV(packetbuf_num / 2));
-	if ((packetbuf_num & 1) == 0)
-		reg = (reg & 0xFFFF0000) | hw->fc.pause_time;
-	else
-		reg = (reg & 0x0000FFFF) | (hw->fc.pause_time << 16);
-	IXGBE_WRITE_REG(hw, IXGBE_FCTTV(packetbuf_num / 2), reg);
+	reg = hw->fc.pause_time * 0x00010001;
+	for (i = 0; i < (MAX_TRAFFIC_CLASS / 2); i++)
+		IXGBE_WRITE_REG(hw, IXGBE_FCTTV(i), reg);
 
-	IXGBE_WRITE_REG(hw, IXGBE_FCRTV, (hw->fc.pause_time >> 1));
+	/* Configure flow control refresh threshold value */
+	IXGBE_WRITE_REG(hw, IXGBE_FCRTV, hw->fc.pause_time / 2);
 
 out:
 	return ret_val;
@@ -1300,6 +1276,8 @@ static struct ixgbe_mac_operations mac_ops_82598 = {
 	.set_fw_drv_ver         = NULL,
 	.acquire_swfw_sync      = &ixgbe_acquire_swfw_sync,
 	.release_swfw_sync      = &ixgbe_release_swfw_sync,
+	.get_thermal_sensor_data = NULL,
+	.init_thermal_sensor_thresh = 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 9c14685358e..dee64d2703f 100644
--- a/drivers/net/ethernet/intel/ixgbe/ixgbe_82599.c
+++ b/drivers/net/ethernet/intel/ixgbe/ixgbe_82599.c
@@ -2119,6 +2119,8 @@ static struct ixgbe_mac_operations mac_ops_82599 = {
 	.set_vlan_anti_spoofing = &ixgbe_set_vlan_anti_spoofing,
 	.acquire_swfw_sync      = &ixgbe_acquire_swfw_sync,
 	.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,
 
 };
 
diff --git a/drivers/net/ethernet/intel/ixgbe/ixgbe_common.c b/drivers/net/ethernet/intel/ixgbe/ixgbe_common.c
index 49aa41fe7b8..77ac41feb0f 100644
--- a/drivers/net/ethernet/intel/ixgbe/ixgbe_common.c
+++ b/drivers/net/ethernet/intel/ixgbe/ixgbe_common.c
@@ -47,13 +47,6 @@ static void ixgbe_lower_eeprom_clk(struct ixgbe_hw *hw, u32 *eec);
 static void ixgbe_release_eeprom(struct ixgbe_hw *hw);
 
 static s32 ixgbe_mta_vector(struct ixgbe_hw *hw, u8 *mc_addr);
-static s32 ixgbe_fc_autoneg_fiber(struct ixgbe_hw *hw);
-static s32 ixgbe_fc_autoneg_backplane(struct ixgbe_hw *hw);
-static s32 ixgbe_fc_autoneg_copper(struct ixgbe_hw *hw);
-static s32 ixgbe_device_supports_autoneg_fc(struct ixgbe_hw *hw);
-static s32 ixgbe_negotiate_fc(struct ixgbe_hw *hw, u32 adv_reg, u32 lp_reg,
-			      u32 adv_sym, u32 adv_asm, u32 lp_sym, u32 lp_asm);
-static s32 ixgbe_setup_fc(struct ixgbe_hw *hw, s32 packetbuf_num);
 static s32 ixgbe_poll_eerd_eewr_done(struct ixgbe_hw *hw, u32 ee_reg);
 static s32 ixgbe_read_eeprom_buffer_bit_bang(struct ixgbe_hw *hw, u16 offset,
 					     u16 words, u16 *data);
@@ -64,6 +57,172 @@ static s32 ixgbe_detect_eeprom_page_size_generic(struct ixgbe_hw *hw,
 static s32 ixgbe_disable_pcie_master(struct ixgbe_hw *hw);
 
 /**
+ *  ixgbe_device_supports_autoneg_fc - Check if phy supports autoneg flow
+ *  control
+ *  @hw: pointer to hardware structure
+ *
+ *  There are several phys that do not support autoneg flow control. This
+ *  function check the device id to see if the associated phy supports
+ *  autoneg flow control.
+ **/
+static s32 ixgbe_device_supports_autoneg_fc(struct ixgbe_hw *hw)
+{
+
+	switch (hw->device_id) {
+	case IXGBE_DEV_ID_X540T:
+		return 0;
+	case IXGBE_DEV_ID_82599_T3_LOM:
+		return 0;
+	default:
+		return IXGBE_ERR_FC_NOT_SUPPORTED;
+	}
+}
+
+/**
+ *  ixgbe_setup_fc - Set up flow control
+ *  @hw: pointer to hardware structure
+ *
+ *  Called at init time to set up flow control.
+ **/
+static s32 ixgbe_setup_fc(struct ixgbe_hw *hw)
+{
+	s32 ret_val = 0;
+	u32 reg = 0, reg_bp = 0;
+	u16 reg_cu = 0;
+
+	/*
+	 * Validate the requested mode.  Strict IEEE mode does not allow
+	 * ixgbe_fc_rx_pause because it will cause us to fail at UNH.
+	 */
+	if (hw->fc.strict_ieee && hw->fc.requested_mode == ixgbe_fc_rx_pause) {
+		hw_dbg(hw, "ixgbe_fc_rx_pause not valid in strict IEEE mode\n");
+		ret_val = IXGBE_ERR_INVALID_LINK_SETTINGS;
+		goto out;
+	}
+
+	/*
+	 * 10gig parts do not have a word in the EEPROM to determine the
+	 * default flow control setting, so we explicitly set it to full.
+	 */
+	if (hw->fc.requested_mode == ixgbe_fc_default)
+		hw->fc.requested_mode = ixgbe_fc_full;
+
+	/*
+	 * Set up the 1G and 10G flow control advertisement registers so the
+	 * HW will be able to do fc autoneg once the cable is plugged in.  If
+	 * we link at 10G, the 1G advertisement is harmless and vice versa.
+	 */
+	switch (hw->phy.media_type) {
+	case ixgbe_media_type_fiber:
+	case ixgbe_media_type_backplane:
+		reg = IXGBE_READ_REG(hw, IXGBE_PCS1GANA);
+		reg_bp = IXGBE_READ_REG(hw, IXGBE_AUTOC);
+		break;
+	case ixgbe_media_type_copper:
+		hw->phy.ops.read_reg(hw, MDIO_AN_ADVERTISE,
+					MDIO_MMD_AN, &reg_cu);
+		break;
+	default:
+		break;
+	}
+
+	/*
+	 * The possible values of fc.requested_mode are:
+	 * 0: Flow control is completely disabled
+	 * 1: Rx flow control is enabled (we can receive pause frames,
+	 *    but not send pause frames).
+	 * 2: Tx flow control is enabled (we can send pause frames but
+	 *    we do not support receiving pause frames).
+	 * 3: Both Rx and Tx flow control (symmetric) are enabled.
+	 * other: Invalid.
+	 */
+	switch (hw->fc.requested_mode) {
+	case ixgbe_fc_none:
+		/* Flow control completely disabled by software override. */
+		reg &= ~(IXGBE_PCS1GANA_SYM_PAUSE | IXGBE_PCS1GANA_ASM_PAUSE);
+		if (hw->phy.media_type == ixgbe_media_type_backplane)
+			reg_bp &= ~(IXGBE_AUTOC_SYM_PAUSE |
+				    IXGBE_AUTOC_ASM_PAUSE);
+		else if (hw->phy.media_type == ixgbe_media_type_copper)
+			reg_cu &= ~(IXGBE_TAF_SYM_PAUSE | IXGBE_TAF_ASM_PAUSE);
+		break;
+	case ixgbe_fc_tx_pause:
+		/*
+		 * Tx Flow control is enabled, and Rx Flow control is
+		 * disabled by software override.
+		 */
+		reg |= IXGBE_PCS1GANA_ASM_PAUSE;
+		reg &= ~IXGBE_PCS1GANA_SYM_PAUSE;
+		if (hw->phy.media_type == ixgbe_media_type_backplane) {
+			reg_bp |= IXGBE_AUTOC_ASM_PAUSE;
+			reg_bp &= ~IXGBE_AUTOC_SYM_PAUSE;
+		} else if (hw->phy.media_type == ixgbe_media_type_copper) {
+			reg_cu |= IXGBE_TAF_ASM_PAUSE;
+			reg_cu &= ~IXGBE_TAF_SYM_PAUSE;
+		}
+		break;
+	case ixgbe_fc_rx_pause:
+		/*
+		 * Rx Flow control is enabled and Tx Flow control is
+		 * disabled by software override. Since there really
+		 * isn't a way to advertise that we are capable of RX
+		 * Pause ONLY, we will advertise that we support both
+		 * symmetric and asymmetric Rx PAUSE, as such we fall
+		 * through to the fc_full statement.  Later, we will
+		 * disable the adapter's ability to send PAUSE frames.
+		 */
+	case ixgbe_fc_full:
+		/* Flow control (both Rx and Tx) is enabled by SW override. */
+		reg |= IXGBE_PCS1GANA_SYM_PAUSE | IXGBE_PCS1GANA_ASM_PAUSE;
+		if (hw->phy.media_type == ixgbe_media_type_backplane)
+			reg_bp |= IXGBE_AUTOC_SYM_PAUSE |
+				  IXGBE_AUTOC_ASM_PAUSE;
+		else if (hw->phy.media_type == ixgbe_media_type_copper)
+			reg_cu |= IXGBE_TAF_SYM_PAUSE | IXGBE_TAF_ASM_PAUSE;
+		break;
+	default:
+		hw_dbg(hw, "Flow control param set incorrectly\n");
+		ret_val = IXGBE_ERR_CONFIG;
+		goto out;
+		break;
+	}
+
+	if (hw->mac.type != ixgbe_mac_X540) {
+		/*
+		 * Enable auto-negotiation between the MAC & PHY;
+		 * the MAC will advertise clause 37 flow control.
+		 */
+		IXGBE_WRITE_REG(hw, IXGBE_PCS1GANA, reg);
+		reg = IXGBE_READ_REG(hw, IXGBE_PCS1GLCTL);
+
+		/* Disable AN timeout */
+		if (hw->fc.strict_ieee)
+			reg &= ~IXGBE_PCS1GLCTL_AN_1G_TIMEOUT_EN;
+
+		IXGBE_WRITE_REG(hw, IXGBE_PCS1GLCTL, reg);
+		hw_dbg(hw, "Set up FC; PCS1GLCTL = 0x%08X\n", reg);
+	}
+
+	/*
+	 * AUTOC restart handles negotiation of 1G and 10G on backplane
+	 * and copper. There is no need to set the PCS1GCTL register.
+	 *
+	 */
+	if (hw->phy.media_type == ixgbe_media_type_backplane) {
+		reg_bp |= IXGBE_AUTOC_AN_RESTART;
+		IXGBE_WRITE_REG(hw, IXGBE_AUTOC, reg_bp);
+	} else if ((hw->phy.media_type == ixgbe_media_type_copper) &&
+		    (ixgbe_device_supports_autoneg_fc(hw) == 0)) {
+		hw->phy.ops.write_reg(hw, MDIO_AN_ADVERTISE,
+				      MDIO_MMD_AN, reg_cu);
+	}
+
+	hw_dbg(hw, "Set up FC; IXGBE_AUTOC = 0x%08X\n", reg);
+out:
+	return ret_val;
+}
+
+/**
  *  ixgbe_start_hw_generic - Prepare hardware for Tx/Rx
  *  @hw: pointer to hardware structure
  *
@@ -95,7 +254,7 @@ s32 ixgbe_start_hw_generic(struct ixgbe_hw *hw)
 	IXGBE_WRITE_FLUSH(hw);
 
 	/* Setup flow control */
-	ixgbe_setup_fc(hw, 0);
+	ixgbe_setup_fc(hw);
 
 	/* Clear adapter stopped flag */
 	hw->adapter_stopped = false;
@@ -1923,30 +2082,36 @@ s32 ixgbe_disable_mc_generic(struct ixgbe_hw *hw)
 /**
  *  ixgbe_fc_enable_generic - Enable flow control
  *  @hw: pointer to hardware structure
- *  @packetbuf_num: packet buffer number (0-7)
  *
  *  Enable flow control according to the current settings.
  **/
-s32 ixgbe_fc_enable_generic(struct ixgbe_hw *hw, s32 packetbuf_num)
+s32 ixgbe_fc_enable_generic(struct ixgbe_hw *hw)
 {
 	s32 ret_val = 0;
 	u32 mflcn_reg, fccfg_reg;
 	u32 reg;
 	u32 fcrtl, fcrth;
+	int i;
 
-#ifdef CONFIG_DCB
-	if (hw->fc.requested_mode == ixgbe_fc_pfc)
+	/*
+	 * Validate the water mark configuration for packet buffer 0.  Zero
+	 * water marks indicate that the packet buffer was not configured
+	 * and the watermarks for packet buffer 0 should always be configured.
+	 */
+	if (!hw->fc.low_water ||
+	    !hw->fc.high_water[0] ||
+	    !hw->fc.pause_time) {
+		hw_dbg(hw, "Invalid water mark configuration\n");
+		ret_val = IXGBE_ERR_INVALID_LINK_SETTINGS;
 		goto out;
+	}
 
-#endif /* CONFIG_DCB */
 	/* Negotiate the fc mode to use */
-	ret_val = ixgbe_fc_autoneg(hw);
-	if (ret_val == IXGBE_ERR_FLOW_CONTROL)
-		goto out;
+	ixgbe_fc_autoneg(hw);
 
 	/* Disable any previous flow control settings */
 	mflcn_reg = IXGBE_READ_REG(hw, IXGBE_MFLCN);
-	mflcn_reg &= ~(IXGBE_MFLCN_RFCE | IXGBE_MFLCN_RPFCE);
+	mflcn_reg &= ~(IXGBE_MFLCN_RPFCE_MASK | IXGBE_MFLCN_RFCE);
 
 	fccfg_reg = IXGBE_READ_REG(hw, IXGBE_FCCFG);
 	fccfg_reg &= ~(IXGBE_FCCFG_TFCE_802_3X | IXGBE_FCCFG_TFCE_PRIORITY);
@@ -1959,9 +2124,6 @@ s32 ixgbe_fc_enable_generic(struct ixgbe_hw *hw, s32 packetbuf_num)
 	 * 2: Tx flow control is enabled (we can send pause frames but
 	 *    we do not support receiving pause frames).
 	 * 3: Both Rx and Tx flow control (symmetric) are enabled.
-#ifdef CONFIG_DCB
-	 * 4: Priority Flow Control is enabled.
-#endif
 	 * other: Invalid.
 	 */
 	switch (hw->fc.current_mode) {
@@ -1994,11 +2156,6 @@ s32 ixgbe_fc_enable_generic(struct ixgbe_hw *hw, s32 packetbuf_num)
 		mflcn_reg |= IXGBE_MFLCN_RFCE;
 		fccfg_reg |= IXGBE_FCCFG_TFCE_802_3X;
 		break;
-#ifdef CONFIG_DCB
-	case ixgbe_fc_pfc:
-		goto out;
-		break;
-#endif /* CONFIG_DCB */
 	default:
 		hw_dbg(hw, "Flow control param set incorrectly\n");
 		ret_val = IXGBE_ERR_CONFIG;
@@ -2011,100 +2168,86 @@ s32 ixgbe_fc_enable_generic(struct ixgbe_hw *hw, s32 packetbuf_num)
 	IXGBE_WRITE_REG(hw, IXGBE_MFLCN, mflcn_reg);
 	IXGBE_WRITE_REG(hw, IXGBE_FCCFG, fccfg_reg);
 
-	fcrtl = hw->fc.low_water << 10;
+	fcrtl = (hw->fc.low_water << 10) | IXGBE_FCRTL_XONE;
 
-	if (hw->fc.current_mode & ixgbe_fc_tx_pause) {
-		fcrth = hw->fc.high_water[packetbuf_num] << 10;
-		fcrth |= IXGBE_FCRTH_FCEN;
-		if (hw->fc.send_xon)
-			fcrtl |= IXGBE_FCRTL_XONE;
-	} else {
-		/*
-		 * If Tx flow control is disabled, set our high water mark
-		 * to Rx FIFO size minus 32 in order prevent Tx switch
-		 * loopback from stalling on DMA.
-		 */
-		fcrth = IXGBE_READ_REG(hw, IXGBE_RXPBSIZE(packetbuf_num)) - 32;
-	}
+	/* Set up and enable Rx high/low water mark thresholds, enable XON. */
+	for (i = 0; i < MAX_TRAFFIC_CLASS; i++) {
+		if ((hw->fc.current_mode & ixgbe_fc_tx_pause) &&
+		    hw->fc.high_water[i]) {
+			IXGBE_WRITE_REG(hw, IXGBE_FCRTL_82599(i), fcrtl);
+			fcrth = (hw->fc.high_water[i] << 10) | IXGBE_FCRTH_FCEN;
+		} else {
+			IXGBE_WRITE_REG(hw, IXGBE_FCRTL_82599(i), 0);
+			/*
+			 * In order to prevent Tx hangs when the internal Tx
+			 * switch is enabled we must set the high water mark
+			 * to the maximum FCRTH value.  This allows the Tx
+			 * switch to function even under heavy Rx workloads.
+			 */
+			fcrth = IXGBE_READ_REG(hw, IXGBE_RXPBSIZE(i)) - 32;
+		}
 
-	IXGBE_WRITE_REG(hw, IXGBE_FCRTH_82599(packetbuf_num), fcrth);
-	IXGBE_WRITE_REG(hw, IXGBE_FCRTL_82599(packetbuf_num), fcrtl);
+		IXGBE_WRITE_REG(hw, IXGBE_FCRTH_82599(i), fcrth);
+	}
 
 	/* Configure pause time (2 TCs per register) */
-	reg = IXGBE_READ_REG(hw, IXGBE_FCTTV(packetbuf_num / 2));
-	if ((packetbuf_num & 1) == 0)
-		reg = (reg & 0xFFFF0000) | hw->fc.pause_time;
-	else
-		reg = (reg & 0x0000FFFF) | (hw->fc.pause_time << 16);
-	IXGBE_WRITE_REG(hw, IXGBE_FCTTV(packetbuf_num / 2), reg);
+	reg = hw->fc.pause_time * 0x00010001;
+	for (i = 0; i < (MAX_TRAFFIC_CLASS / 2); i++)
+		IXGBE_WRITE_REG(hw, IXGBE_FCTTV(i), reg);
 
-	IXGBE_WRITE_REG(hw, IXGBE_FCRTV, (hw->fc.pause_time >> 1));
+	IXGBE_WRITE_REG(hw, IXGBE_FCRTV, hw->fc.pause_time / 2);
 
 out:
 	return ret_val;
 }
 
 /**
- *  ixgbe_fc_autoneg - Configure flow control
+ *  ixgbe_negotiate_fc - Negotiate flow control
  *  @hw: pointer to hardware structure
+ *  @adv_reg: flow control advertised settings
+ *  @lp_reg: link partner's flow control settings
+ *  @adv_sym: symmetric pause bit in advertisement
+ *  @adv_asm: asymmetric pause bit in advertisement
+ *  @lp_sym: symmetric pause bit in link partner advertisement
+ *  @lp_asm: asymmetric pause bit in link partner advertisement
  *
- *  Compares our advertised flow control capabilities to those advertised by
- *  our link partner, and determines the proper flow control mode to use.
+ *  Find the intersection between advertised settings and link partner's
+ *  advertised settings
  **/
-s32 ixgbe_fc_autoneg(struct ixgbe_hw *hw)
+static s32 ixgbe_negotiate_fc(struct ixgbe_hw *hw, u32 adv_reg, u32 lp_reg,
+			      u32 adv_sym, u32 adv_asm, u32 lp_sym, u32 lp_asm)
 {
-	s32 ret_val = IXGBE_ERR_FC_NOT_NEGOTIATED;
-	ixgbe_link_speed speed;
-	bool link_up;
-
-	if (hw->fc.disable_fc_autoneg)
-		goto out;
-
-	/*
-	 * AN should have completed when the cable was plugged in.
-	 * Look for reasons to bail out.  Bail out if:
-	 * - FC autoneg is disabled, or if
-	 * - link is not up.
-	 *
-	 * Since we're being called from an LSC, link is already known to be up.
-	 * So use link_up_wait_to_complete=false.
-	 */
-	hw->mac.ops.check_link(hw, &speed, &link_up, false);
-	if (!link_up) {
-		ret_val = IXGBE_ERR_FLOW_CONTROL;
-		goto out;
-	}
-
-	switch (hw->phy.media_type) {
-	/* Autoneg flow control on fiber adapters */
-	case ixgbe_media_type_fiber:
-		if (speed == IXGBE_LINK_SPEED_1GB_FULL)
-			ret_val = ixgbe_fc_autoneg_fiber(hw);
-		break;
-
-	/* Autoneg flow control on backplane adapters */
-	case ixgbe_media_type_backplane:
-		ret_val = ixgbe_fc_autoneg_backplane(hw);
-		break;
-
-	/* Autoneg flow control on copper adapters */
-	case ixgbe_media_type_copper:
-		if (ixgbe_device_supports_autoneg_fc(hw) == 0)
-			ret_val = ixgbe_fc_autoneg_copper(hw);
-		break;
-
-	default:
-		break;
-	}
+	if ((!(adv_reg)) ||  (!(lp_reg)))
+		return IXGBE_ERR_FC_NOT_NEGOTIATED;
 
-out:
-	if (ret_val == 0) {
-		hw->fc.fc_was_autonegged = true;
+	if ((adv_reg & adv_sym) && (lp_reg & lp_sym)) {
+		/*
+		 * 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
+		 * turn OFF the TRANSMISSION of PAUSE frames.
+		 */
+		if (hw->fc.requested_mode == ixgbe_fc_full) {
+			hw->fc.current_mode = ixgbe_fc_full;
+			hw_dbg(hw, "Flow Control = FULL.\n");
+		} else {
+			hw->fc.current_mode = ixgbe_fc_rx_pause;
+			hw_dbg(hw, "Flow Control=RX PAUSE frames only\n");
+		}
+	} else if (!(adv_reg & adv_sym) && (adv_reg & adv_asm) &&
+		   (lp_reg & lp_sym) && (lp_reg & lp_asm)) {
+		hw->fc.current_mode = ixgbe_fc_tx_pause;
+		hw_dbg(hw, "Flow Control = TX PAUSE frames only.\n");
+	} else if ((adv_reg & adv_sym) && (adv_reg & adv_asm) &&
+		   !(lp_reg & lp_sym) && (lp_reg & lp_asm)) {
+		hw->fc.current_mode = ixgbe_fc_rx_pause;
+		hw_dbg(hw, "Flow Control = RX PAUSE frames only.\n");
 	} else {
-		hw->fc.fc_was_autonegged = false;
-		hw->fc.current_mode = hw->fc.requested_mode;
+		hw->fc.current_mode = ixgbe_fc_none;
+		hw_dbg(hw, "Flow Control = NONE.\n");
 	}
-	return ret_val;
+	return 0;
 }
 
 /**
@@ -2116,7 +2259,7 @@ out:
 static s32 ixgbe_fc_autoneg_fiber(struct ixgbe_hw *hw)
 {
 	u32 pcs_anadv_reg, pcs_lpab_reg, linkstat;
-	s32 ret_val;
+	s32 ret_val = IXGBE_ERR_FC_NOT_NEGOTIATED;
 
 	/*
 	 * On multispeed fiber at 1g, bail out if
@@ -2126,10 +2269,8 @@ static s32 ixgbe_fc_autoneg_fiber(struct ixgbe_hw *hw)
 
 	linkstat = IXGBE_READ_REG(hw, IXGBE_PCS1GLSTA);
 	if ((!!(linkstat & IXGBE_PCS1GLSTA_AN_COMPLETE) == 0) ||
-	    (!!(linkstat & IXGBE_PCS1GLSTA_AN_TIMED_OUT) == 1)) {
-		ret_val = IXGBE_ERR_FC_NOT_NEGOTIATED;
+	    (!!(linkstat & IXGBE_PCS1GLSTA_AN_TIMED_OUT) == 1))
 		goto out;
-	}
 
 	pcs_anadv_reg = IXGBE_READ_REG(hw, IXGBE_PCS1GANA);
 	pcs_lpab_reg = IXGBE_READ_REG(hw, IXGBE_PCS1GANLP);
@@ -2153,7 +2294,7 @@ out:
 static s32 ixgbe_fc_autoneg_backplane(struct ixgbe_hw *hw)
 {
 	u32 links2, anlp1_reg, autoc_reg, links;
-	s32 ret_val;
+	s32 ret_val = IXGBE_ERR_FC_NOT_NEGOTIATED;
 
 	/*
 	 * On backplane, bail out if
@@ -2161,21 +2302,13 @@ static s32 ixgbe_fc_autoneg_backplane(struct ixgbe_hw *hw)
 	 * - we are 82599 and link partner is not AN enabled
 	 */
 	links = IXGBE_READ_REG(hw, IXGBE_LINKS);
-	if ((links & IXGBE_LINKS_KX_AN_COMP) == 0) {
-		hw->fc.fc_was_autonegged = false;
-		hw->fc.current_mode = hw->fc.requested_mode;
-		ret_val = IXGBE_ERR_FC_NOT_NEGOTIATED;
+	if ((links & IXGBE_LINKS_KX_AN_COMP) == 0)
 		goto out;
-	}
 
 	if (hw->mac.type == ixgbe_mac_82599EB) {
 		links2 = IXGBE_READ_REG(hw, IXGBE_LINKS2);
-		if ((links2 & IXGBE_LINKS2_AN_SUPPORTED) == 0) {
-			hw->fc.fc_was_autonegged = false;
-			hw->fc.current_mode = hw->fc.requested_mode;
-			ret_val = IXGBE_ERR_FC_NOT_NEGOTIATED;
+		if ((links2 & IXGBE_LINKS2_AN_SUPPORTED) == 0)
 			goto out;
-		}
 	}
 	/*
 	 * Read the 10g AN autoc and LP ability registers and resolve
@@ -2217,241 +2350,63 @@ static s32 ixgbe_fc_autoneg_copper(struct ixgbe_hw *hw)
 }
 
 /**
- *  ixgbe_negotiate_fc - Negotiate flow control
- *  @hw: pointer to hardware structure
- *  @adv_reg: flow control advertised settings
- *  @lp_reg: link partner's flow control settings
- *  @adv_sym: symmetric pause bit in advertisement
- *  @adv_asm: asymmetric pause bit in advertisement
- *  @lp_sym: symmetric pause bit in link partner advertisement
- *  @lp_asm: asymmetric pause bit in link partner advertisement
- *
- *  Find the intersection between advertised settings and link partner's
- *  advertised settings
- **/
-static s32 ixgbe_negotiate_fc(struct ixgbe_hw *hw, u32 adv_reg, u32 lp_reg,
-			      u32 adv_sym, u32 adv_asm, u32 lp_sym, u32 lp_asm)
-{
-	if ((!(adv_reg)) ||  (!(lp_reg)))
-		return IXGBE_ERR_FC_NOT_NEGOTIATED;
-
-	if ((adv_reg & adv_sym) && (lp_reg & lp_sym)) {
-		/*
-		 * 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
-		 * turn OFF the TRANSMISSION of PAUSE frames.
-		 */
-		if (hw->fc.requested_mode == ixgbe_fc_full) {
-			hw->fc.current_mode = ixgbe_fc_full;
-			hw_dbg(hw, "Flow Control = FULL.\n");
-		} else {
-			hw->fc.current_mode = ixgbe_fc_rx_pause;
-			hw_dbg(hw, "Flow Control=RX PAUSE frames only\n");
-		}
-	} else if (!(adv_reg & adv_sym) && (adv_reg & adv_asm) &&
-		   (lp_reg & lp_sym) && (lp_reg & lp_asm)) {
-		hw->fc.current_mode = ixgbe_fc_tx_pause;
-		hw_dbg(hw, "Flow Control = TX PAUSE frames only.\n");
-	} else if ((adv_reg & adv_sym) && (adv_reg & adv_asm) &&
-		   !(lp_reg & lp_sym) && (lp_reg & lp_asm)) {
-		hw->fc.current_mode = ixgbe_fc_rx_pause;
-		hw_dbg(hw, "Flow Control = RX PAUSE frames only.\n");
-	} else {
-		hw->fc.current_mode = ixgbe_fc_none;
-		hw_dbg(hw, "Flow Control = NONE.\n");
-	}
-	return 0;
-}
-
-/**
- *  ixgbe_setup_fc - Set up flow control
+ *  ixgbe_fc_autoneg - Configure flow control
  *  @hw: pointer to hardware structure
  *
- *  Called at init time to set up flow control.
+ *  Compares our advertised flow control capabilities to those advertised by
+ *  our link partner, and determines the proper flow control mode to use.
  **/
-static s32 ixgbe_setup_fc(struct ixgbe_hw *hw, s32 packetbuf_num)
+void ixgbe_fc_autoneg(struct ixgbe_hw *hw)
 {
-	s32 ret_val = 0;
-	u32 reg = 0, reg_bp = 0;
-	u16 reg_cu = 0;
-
-#ifdef CONFIG_DCB
-	if (hw->fc.requested_mode == ixgbe_fc_pfc) {
-		hw->fc.current_mode = hw->fc.requested_mode;
-		goto out;
-	}
-
-#endif /* CONFIG_DCB */
-	/* Validate the packetbuf configuration */
-	if (packetbuf_num < 0 || packetbuf_num > 7) {
-		hw_dbg(hw, "Invalid packet buffer number [%d], expected range "
-		       "is 0-7\n", packetbuf_num);
-		ret_val = IXGBE_ERR_INVALID_LINK_SETTINGS;
-		goto out;
-	}
+	s32 ret_val = IXGBE_ERR_FC_NOT_NEGOTIATED;
+	ixgbe_link_speed speed;
+	bool link_up;
 
 	/*
-	 * Validate the water mark configuration.  Zero water marks are invalid
-	 * because it causes the controller to just blast out fc packets.
+	 * AN should have completed when the cable was plugged in.
+	 * Look for reasons to bail out.  Bail out if:
+	 * - FC autoneg is disabled, or if
+	 * - link is not up.
+	 *
+	 * Since we're being called from an LSC, link is already known to be up.
+	 * So use link_up_wait_to_complete=false.
 	 */
-	if (!hw->fc.low_water ||
-	    !hw->fc.high_water[packetbuf_num] ||
-	    !hw->fc.pause_time) {
-		hw_dbg(hw, "Invalid water mark configuration\n");
-		ret_val = IXGBE_ERR_INVALID_LINK_SETTINGS;
+	if (hw->fc.disable_fc_autoneg)
 		goto out;
-	}
 
-	/*
-	 * Validate the requested mode.  Strict IEEE mode does not allow
-	 * ixgbe_fc_rx_pause because it will cause us to fail at UNH.
-	 */
-	if (hw->fc.strict_ieee && hw->fc.requested_mode == ixgbe_fc_rx_pause) {
-		hw_dbg(hw, "ixgbe_fc_rx_pause not valid in strict "
-		       "IEEE mode\n");
-		ret_val = IXGBE_ERR_INVALID_LINK_SETTINGS;
+	hw->mac.ops.check_link(hw, &speed, &link_up, false);
+	if (!link_up)
 		goto out;
-	}
-
-	/*
-	 * 10gig parts do not have a word in the EEPROM to determine the
-	 * default flow control setting, so we explicitly set it to full.
-	 */
-	if (hw->fc.requested_mode == ixgbe_fc_default)
-		hw->fc.requested_mode = ixgbe_fc_full;
-
-	/*
-	 * Set up the 1G and 10G flow control advertisement registers so the
-	 * HW will be able to do fc autoneg once the cable is plugged in.  If
-	 * we link at 10G, the 1G advertisement is harmless and vice versa.
-	 */
 
 	switch (hw->phy.media_type) {
+	/* Autoneg flow control on fiber adapters */
 	case ixgbe_media_type_fiber:
+		if (speed == IXGBE_LINK_SPEED_1GB_FULL)
+			ret_val = ixgbe_fc_autoneg_fiber(hw);
+		break;
+
+	/* Autoneg flow control on backplane adapters */
 	case ixgbe_media_type_backplane:
-		reg = IXGBE_READ_REG(hw, IXGBE_PCS1GANA);
-		reg_bp = IXGBE_READ_REG(hw, IXGBE_AUTOC);
+		ret_val = ixgbe_fc_autoneg_backplane(hw);
 		break;
 
+	/* Autoneg flow control on copper adapters */
 	case ixgbe_media_type_copper:
-		hw->phy.ops.read_reg(hw, MDIO_AN_ADVERTISE,
-					MDIO_MMD_AN, &reg_cu);
+		if (ixgbe_device_supports_autoneg_fc(hw) == 0)
+			ret_val = ixgbe_fc_autoneg_copper(hw);
 		break;
 
 	default:
-		;
-	}
-
-	/*
-	 * The possible values of fc.requested_mode are:
-	 * 0: Flow control is completely disabled
-	 * 1: Rx flow control is enabled (we can receive pause frames,
-	 *    but not send pause frames).
-	 * 2: Tx flow control is enabled (we can send pause frames but
-	 *    we do not support receiving pause frames).
-	 * 3: Both Rx and Tx flow control (symmetric) are enabled.
-#ifdef CONFIG_DCB
-	 * 4: Priority Flow Control is enabled.
-#endif
-	 * other: Invalid.
-	 */
-	switch (hw->fc.requested_mode) {
-	case ixgbe_fc_none:
-		/* Flow control completely disabled by software override. */
-		reg &= ~(IXGBE_PCS1GANA_SYM_PAUSE | IXGBE_PCS1GANA_ASM_PAUSE);
-		if (hw->phy.media_type == ixgbe_media_type_backplane)
-			reg_bp &= ~(IXGBE_AUTOC_SYM_PAUSE |
-				    IXGBE_AUTOC_ASM_PAUSE);
-		else if (hw->phy.media_type == ixgbe_media_type_copper)
-			reg_cu &= ~(IXGBE_TAF_SYM_PAUSE | IXGBE_TAF_ASM_PAUSE);
-		break;
-	case ixgbe_fc_rx_pause:
-		/*
-		 * Rx Flow control is enabled and Tx Flow control is
-		 * disabled by software override. Since there really
-		 * isn't a way to advertise that we are capable of RX
-		 * Pause ONLY, we will advertise that we support both
-		 * symmetric and asymmetric Rx PAUSE.  Later, we will
-		 * disable the adapter's ability to send PAUSE frames.
-		 */
-		reg |= (IXGBE_PCS1GANA_SYM_PAUSE | IXGBE_PCS1GANA_ASM_PAUSE);
-		if (hw->phy.media_type == ixgbe_media_type_backplane)
-			reg_bp |= (IXGBE_AUTOC_SYM_PAUSE |
-				   IXGBE_AUTOC_ASM_PAUSE);
-		else if (hw->phy.media_type == ixgbe_media_type_copper)
-			reg_cu |= (IXGBE_TAF_SYM_PAUSE | IXGBE_TAF_ASM_PAUSE);
-		break;
-	case ixgbe_fc_tx_pause:
-		/*
-		 * Tx Flow control is enabled, and Rx Flow control is
-		 * disabled by software override.
-		 */
-		reg |= (IXGBE_PCS1GANA_ASM_PAUSE);
-		reg &= ~(IXGBE_PCS1GANA_SYM_PAUSE);
-		if (hw->phy.media_type == ixgbe_media_type_backplane) {
-			reg_bp |= (IXGBE_AUTOC_ASM_PAUSE);
-			reg_bp &= ~(IXGBE_AUTOC_SYM_PAUSE);
-		} else if (hw->phy.media_type == ixgbe_media_type_copper) {
-			reg_cu |= (IXGBE_TAF_ASM_PAUSE);
-			reg_cu &= ~(IXGBE_TAF_SYM_PAUSE);
-		}
 		break;
-	case ixgbe_fc_full:
-		/* Flow control (both Rx and Tx) is enabled by SW override. */
-		reg |= (IXGBE_PCS1GANA_SYM_PAUSE | IXGBE_PCS1GANA_ASM_PAUSE);
-		if (hw->phy.media_type == ixgbe_media_type_backplane)
-			reg_bp |= (IXGBE_AUTOC_SYM_PAUSE |
-				   IXGBE_AUTOC_ASM_PAUSE);
-		else if (hw->phy.media_type == ixgbe_media_type_copper)
-			reg_cu |= (IXGBE_TAF_SYM_PAUSE | IXGBE_TAF_ASM_PAUSE);
-		break;
-#ifdef CONFIG_DCB
-	case ixgbe_fc_pfc:
-		goto out;
-		break;
-#endif /* CONFIG_DCB */
-	default:
-		hw_dbg(hw, "Flow control param set incorrectly\n");
-		ret_val = IXGBE_ERR_CONFIG;
-		goto out;
-		break;
-	}
-
-	if (hw->mac.type != ixgbe_mac_X540) {
-		/*
-		 * Enable auto-negotiation between the MAC & PHY;
-		 * the MAC will advertise clause 37 flow control.
-		 */
-		IXGBE_WRITE_REG(hw, IXGBE_PCS1GANA, reg);
-		reg = IXGBE_READ_REG(hw, IXGBE_PCS1GLCTL);
-
-		/* Disable AN timeout */
-		if (hw->fc.strict_ieee)
-			reg &= ~IXGBE_PCS1GLCTL_AN_1G_TIMEOUT_EN;
-
-		IXGBE_WRITE_REG(hw, IXGBE_PCS1GLCTL, reg);
-		hw_dbg(hw, "Set up FC; PCS1GLCTL = 0x%08X\n", reg);
-	}
-
-	/*
-	 * AUTOC restart handles negotiation of 1G and 10G on backplane
-	 * and copper. There is no need to set the PCS1GCTL register.
-	 *
-	 */
-	if (hw->phy.media_type == ixgbe_media_type_backplane) {
-		reg_bp |= IXGBE_AUTOC_AN_RESTART;
-		IXGBE_WRITE_REG(hw, IXGBE_AUTOC, reg_bp);
-	} else if ((hw->phy.media_type == ixgbe_media_type_copper) &&
-		    (ixgbe_device_supports_autoneg_fc(hw) == 0)) {
-		hw->phy.ops.write_reg(hw, MDIO_AN_ADVERTISE,
-				      MDIO_MMD_AN, reg_cu);
 	}
 
-	hw_dbg(hw, "Set up FC; IXGBE_AUTOC = 0x%08X\n", reg);
 out:
-	return ret_val;
+	if (ret_val == 0) {
+		hw->fc.fc_was_autonegged = true;
+	} else {
+		hw->fc.fc_was_autonegged = false;
+		hw->fc.current_mode = hw->fc.requested_mode;
+	}
 }
 
 /**
@@ -2606,7 +2561,7 @@ s32 ixgbe_disable_rx_buff_generic(struct ixgbe_hw *hw)
 			break;
 		else
 			/* Use interrupt-safe sleep just in case */
-			udelay(10);
+			udelay(1000);
 	}
 
 	/* For informational purposes only */
@@ -2783,17 +2738,36 @@ san_mac_addr_out:
  *  Read PCIe configuration space, and get the MSI-X vector count from
  *  the capabilities table.
  **/
-u32 ixgbe_get_pcie_msix_count_generic(struct ixgbe_hw *hw)
+u16 ixgbe_get_pcie_msix_count_generic(struct ixgbe_hw *hw)
 {
 	struct ixgbe_adapter *adapter = hw->back;
-	u16 msix_count;
-	pci_read_config_word(adapter->pdev, IXGBE_PCIE_MSIX_82599_CAPS,
-	                     &msix_count);
+	u16 msix_count = 1;
+	u16 max_msix_count;
+	u16 pcie_offset;
+
+	switch (hw->mac.type) {
+	case ixgbe_mac_82598EB:
+		pcie_offset = IXGBE_PCIE_MSIX_82598_CAPS;
+		max_msix_count = IXGBE_MAX_MSIX_VECTORS_82598;
+		break;
+	case ixgbe_mac_82599EB:
+	case ixgbe_mac_X540:
+		pcie_offset = IXGBE_PCIE_MSIX_82599_CAPS;
+		max_msix_count = IXGBE_MAX_MSIX_VECTORS_82599;
+		break;
+	default:
+		return msix_count;
+	}
+
+	pci_read_config_word(adapter->pdev, pcie_offset, &msix_count);
 	msix_count &= IXGBE_PCIE_MSIX_TBL_SZ_MASK;
 
-	/* MSI-X count is zero-based in HW, so increment to give proper value */
+	/* MSI-X count is zero-based in HW */
 	msix_count++;
 
+	if (msix_count > max_msix_count)
+		msix_count = max_msix_count;
+
 	return msix_count;
 }
 
@@ -3203,28 +3177,6 @@ wwn_prefix_out:
 }
 
 /**
- *  ixgbe_device_supports_autoneg_fc - Check if phy supports autoneg flow
- *  control
- *  @hw: pointer to hardware structure
- *
- *  There are several phys that do not support autoneg flow control. This
- *  function check the device id to see if the associated phy supports
- *  autoneg flow control.
- **/
-static s32 ixgbe_device_supports_autoneg_fc(struct ixgbe_hw *hw)
-{
-
-	switch (hw->device_id) {
-	case IXGBE_DEV_ID_X540T:
-		return 0;
-	case IXGBE_DEV_ID_82599_T3_LOM:
-		return 0;
-	default:
-		return IXGBE_ERR_FC_NOT_SUPPORTED;
-	}
-}
-
-/**
  *  ixgbe_set_mac_anti_spoofing - Enable/Disable MAC anti-spoofing
  *  @hw: pointer to hardware structure
  *  @enable: enable or disable switch for anti-spoofing
@@ -3585,3 +3537,172 @@ void ixgbe_clear_tx_pending(struct ixgbe_hw *hw)
 	IXGBE_WRITE_REG(hw, IXGBE_GCR_EXT, gcr_ext);
 	IXGBE_WRITE_REG(hw, IXGBE_HLREG0, hlreg0);
 }
+
+static const u8 ixgbe_emc_temp_data[4] = {
+	IXGBE_EMC_INTERNAL_DATA,
+	IXGBE_EMC_DIODE1_DATA,
+	IXGBE_EMC_DIODE2_DATA,
+	IXGBE_EMC_DIODE3_DATA
+};
+static const u8 ixgbe_emc_therm_limit[4] = {
+	IXGBE_EMC_INTERNAL_THERM_LIMIT,
+	IXGBE_EMC_DIODE1_THERM_LIMIT,
+	IXGBE_EMC_DIODE2_THERM_LIMIT,
+	IXGBE_EMC_DIODE3_THERM_LIMIT
+};
+
+/**
+ *  ixgbe_get_ets_data - Extracts the ETS bit data
+ *  @hw: pointer to hardware structure
+ *  @ets_cfg: extected ETS data
+ *  @ets_offset: offset of ETS data
+ *
+ *  Returns error code.
+ **/
+static s32 ixgbe_get_ets_data(struct ixgbe_hw *hw, u16 *ets_cfg,
+			      u16 *ets_offset)
+{
+	s32 status = 0;
+
+	status = hw->eeprom.ops.read(hw, IXGBE_ETS_CFG, ets_offset);
+	if (status)
+		goto out;
+
+	if ((*ets_offset == 0x0000) || (*ets_offset == 0xFFFF)) {
+		status = IXGBE_NOT_IMPLEMENTED;
+		goto out;
+	}
+
+	status = hw->eeprom.ops.read(hw, *ets_offset, ets_cfg);
+	if (status)
+		goto out;
+
+	if ((*ets_cfg & IXGBE_ETS_TYPE_MASK) != IXGBE_ETS_TYPE_EMC_SHIFTED) {
+		status = IXGBE_NOT_IMPLEMENTED;
+		goto out;
+	}
+
+out:
+	return status;
+}
+
+/**
+ *  ixgbe_get_thermal_sensor_data - Gathers thermal sensor data
+ *  @hw: pointer to hardware structure
+ *
+ *  Returns the thermal sensor data structure
+ **/
+s32 ixgbe_get_thermal_sensor_data_generic(struct ixgbe_hw *hw)
+{
+	s32 status = 0;
+	u16 ets_offset;
+	u16 ets_cfg;
+	u16 ets_sensor;
+	u8  num_sensors;
+	u8  i;
+	struct ixgbe_thermal_sensor_data *data = &hw->mac.thermal_sensor_data;
+
+	/* Only support thermal sensors attached to physical port 0 */
+	if ((IXGBE_READ_REG(hw, IXGBE_STATUS) & IXGBE_STATUS_LAN_ID_1)) {
+		status = IXGBE_NOT_IMPLEMENTED;
+		goto out;
+	}
+
+	status = ixgbe_get_ets_data(hw, &ets_cfg, &ets_offset);
+	if (status)
+		goto out;
+
+	num_sensors = (ets_cfg & IXGBE_ETS_NUM_SENSORS_MASK);
+	if (num_sensors > IXGBE_MAX_SENSORS)
+		num_sensors = IXGBE_MAX_SENSORS;
+
+	for (i = 0; i < num_sensors; i++) {
+		u8  sensor_index;
+		u8  sensor_location;
+
+		status = hw->eeprom.ops.read(hw, (ets_offset + 1 + i),
+					     &ets_sensor);
+		if (status)
+			goto out;
+
+		sensor_index = ((ets_sensor & IXGBE_ETS_DATA_INDEX_MASK) >>
+				IXGBE_ETS_DATA_INDEX_SHIFT);
+		sensor_location = ((ets_sensor & IXGBE_ETS_DATA_LOC_MASK) >>
+				   IXGBE_ETS_DATA_LOC_SHIFT);
+
+		if (sensor_location != 0) {
+			status = hw->phy.ops.read_i2c_byte(hw,
+					ixgbe_emc_temp_data[sensor_index],
+					IXGBE_I2C_THERMAL_SENSOR_ADDR,
+					&data->sensor[i].temp);
+			if (status)
+				goto out;
+		}
+	}
+out:
+	return status;
+}
+
+/**
+ * ixgbe_init_thermal_sensor_thresh_generic - Inits thermal sensor thresholds
+ * @hw: pointer to hardware structure
+ *
+ * Inits the thermal sensor thresholds according to the NVM map
+ * and save off the threshold and location values into mac.thermal_sensor_data
+ **/
+s32 ixgbe_init_thermal_sensor_thresh_generic(struct ixgbe_hw *hw)
+{
+	s32 status = 0;
+	u16 ets_offset;
+	u16 ets_cfg;
+	u16 ets_sensor;
+	u8  low_thresh_delta;
+	u8  num_sensors;
+	u8  therm_limit;
+	u8  i;
+	struct ixgbe_thermal_sensor_data *data = &hw->mac.thermal_sensor_data;
+
+	memset(data, 0, sizeof(struct ixgbe_thermal_sensor_data));
+
+	/* Only support thermal sensors attached to physical port 0 */
+	if ((IXGBE_READ_REG(hw, IXGBE_STATUS) & IXGBE_STATUS_LAN_ID_1)) {
+		status = IXGBE_NOT_IMPLEMENTED;
+		goto out;
+	}
+
+	status = ixgbe_get_ets_data(hw, &ets_cfg, &ets_offset);
+	if (status)
+		goto out;
+
+	low_thresh_delta = ((ets_cfg & IXGBE_ETS_LTHRES_DELTA_MASK) >>
+			     IXGBE_ETS_LTHRES_DELTA_SHIFT);
+	num_sensors = (ets_cfg & IXGBE_ETS_NUM_SENSORS_MASK);
+	if (num_sensors > IXGBE_MAX_SENSORS)
+		num_sensors = IXGBE_MAX_SENSORS;
+
+	for (i = 0; i < num_sensors; i++) {
+		u8  sensor_index;
+		u8  sensor_location;
+
+		hw->eeprom.ops.read(hw, (ets_offset + 1 + i), &ets_sensor);
+		sensor_index = ((ets_sensor & IXGBE_ETS_DATA_INDEX_MASK) >>
+				IXGBE_ETS_DATA_INDEX_SHIFT);
+		sensor_location = ((ets_sensor & IXGBE_ETS_DATA_LOC_MASK) >>
+				   IXGBE_ETS_DATA_LOC_SHIFT);
+		therm_limit = ets_sensor & IXGBE_ETS_DATA_HTHRESH_MASK;
+
+		hw->phy.ops.write_i2c_byte(hw,
+			ixgbe_emc_therm_limit[sensor_index],
+			IXGBE_I2C_THERMAL_SENSOR_ADDR, therm_limit);
+
+		if (sensor_location == 0)
+			continue;
+
+		data->sensor[i].location = sensor_location;
+		data->sensor[i].caution_thresh = therm_limit;
+		data->sensor[i].max_op_thresh = therm_limit - low_thresh_delta;
+	}
+out:
+	return status;
+}
+
diff --git a/drivers/net/ethernet/intel/ixgbe/ixgbe_common.h b/drivers/net/ethernet/intel/ixgbe/ixgbe_common.h
index 204f06235b4..6222fdb3d3f 100644
--- a/drivers/net/ethernet/intel/ixgbe/ixgbe_common.h
+++ b/drivers/net/ethernet/intel/ixgbe/ixgbe_common.h
@@ -31,7 +31,7 @@
 #include "ixgbe_type.h"
 #include "ixgbe.h"
 
-u32 ixgbe_get_pcie_msix_count_generic(struct ixgbe_hw *hw);
+u16 ixgbe_get_pcie_msix_count_generic(struct ixgbe_hw *hw);
 s32 ixgbe_init_ops_generic(struct ixgbe_hw *hw);
 s32 ixgbe_init_hw_generic(struct ixgbe_hw *hw);
 s32 ixgbe_start_hw_generic(struct ixgbe_hw *hw);
@@ -77,8 +77,8 @@ s32 ixgbe_disable_mc_generic(struct ixgbe_hw *hw);
 s32 ixgbe_disable_rx_buff_generic(struct ixgbe_hw *hw);
 s32 ixgbe_enable_rx_buff_generic(struct ixgbe_hw *hw);
 s32 ixgbe_enable_rx_dma_generic(struct ixgbe_hw *hw, u32 regval);
-s32 ixgbe_fc_enable_generic(struct ixgbe_hw *hw, s32 packetbuf_num);
-s32 ixgbe_fc_autoneg(struct ixgbe_hw *hw);
+s32 ixgbe_fc_enable_generic(struct ixgbe_hw *hw);
+void ixgbe_fc_autoneg(struct ixgbe_hw *hw);
 
 s32 ixgbe_validate_mac_addr(u8 *mac_addr);
 s32 ixgbe_acquire_swfw_sync(struct ixgbe_hw *hw, u16 mask);
@@ -107,6 +107,19 @@ void ixgbe_clear_tx_pending(struct ixgbe_hw *hw);
 void ixgbe_set_rxpba_generic(struct ixgbe_hw *hw, int num_pb,
 			     u32 headroom, int strategy);
 
+#define IXGBE_I2C_THERMAL_SENSOR_ADDR	0xF8
+#define IXGBE_EMC_INTERNAL_DATA		0x00
+#define IXGBE_EMC_INTERNAL_THERM_LIMIT	0x20
+#define IXGBE_EMC_DIODE1_DATA		0x01
+#define IXGBE_EMC_DIODE1_THERM_LIMIT	0x19
+#define IXGBE_EMC_DIODE2_DATA		0x23
+#define IXGBE_EMC_DIODE2_THERM_LIMIT	0x1A
+#define IXGBE_EMC_DIODE3_DATA		0x2A
+#define IXGBE_EMC_DIODE3_THERM_LIMIT	0x30
+
+s32 ixgbe_get_thermal_sensor_data_generic(struct ixgbe_hw *hw);
+s32 ixgbe_init_thermal_sensor_thresh_generic(struct ixgbe_hw *hw);
+
 #define IXGBE_WRITE_REG(a, reg, value) writel((value), ((a)->hw_addr + (reg)))
 
 #ifndef writeq
diff --git a/drivers/net/ethernet/intel/ixgbe/ixgbe_dcb_82598.c b/drivers/net/ethernet/intel/ixgbe/ixgbe_dcb_82598.c
index d3695edfcb8..87592b458c9 100644
--- a/drivers/net/ethernet/intel/ixgbe/ixgbe_dcb_82598.c
+++ b/drivers/net/ethernet/intel/ixgbe/ixgbe_dcb_82598.c
@@ -191,53 +191,46 @@ s32 ixgbe_dcb_config_tx_data_arbiter_82598(struct ixgbe_hw *hw,
  */
 s32 ixgbe_dcb_config_pfc_82598(struct ixgbe_hw *hw, u8 pfc_en)
 {
-	u32 reg;
+	u32 fcrtl, reg;
 	u8  i;
 
-	if (pfc_en) {
-		/* Enable Transmit Priority Flow Control */
-		reg = IXGBE_READ_REG(hw, IXGBE_RMCS);
-		reg &= ~IXGBE_RMCS_TFCE_802_3X;
-		/* correct the reporting of our flow control status */
-		reg |= IXGBE_RMCS_TFCE_PRIORITY;
-		IXGBE_WRITE_REG(hw, IXGBE_RMCS, reg);
-
-		/* Enable Receive Priority Flow Control */
-		reg = IXGBE_READ_REG(hw, IXGBE_FCTRL);
-		reg &= ~IXGBE_FCTRL_RFCE;
-		reg |= IXGBE_FCTRL_RPFCE;
-		IXGBE_WRITE_REG(hw, IXGBE_FCTRL, reg);
-
-		/* Configure pause time */
-		for (i = 0; i < (MAX_TRAFFIC_CLASS >> 1); i++)
-			IXGBE_WRITE_REG(hw, IXGBE_FCTTV(i), 0x68006800);
+	/* Enable Transmit Priority Flow Control */
+	reg = IXGBE_READ_REG(hw, IXGBE_RMCS);
+	reg &= ~IXGBE_RMCS_TFCE_802_3X;
+	reg |= IXGBE_RMCS_TFCE_PRIORITY;
+	IXGBE_WRITE_REG(hw, IXGBE_RMCS, reg);
 
-		/* Configure flow control refresh threshold value */
-		IXGBE_WRITE_REG(hw, IXGBE_FCRTV, 0x3400);
-	}
+	/* Enable Receive Priority Flow Control */
+	reg = IXGBE_READ_REG(hw, IXGBE_FCTRL);
+	reg &= ~(IXGBE_FCTRL_RPFCE | IXGBE_FCTRL_RFCE);
 
-	/*
-	 * Configure flow control thresholds and enable priority flow control
-	 * for each traffic class.
-	 */
-	for (i = 0; i < MAX_TRAFFIC_CLASS; i++) {
-		int enabled = pfc_en & (1 << i);
+	if (pfc_en)
+		reg |= IXGBE_FCTRL_RPFCE;
 
-		reg = hw->fc.low_water << 10;
+	IXGBE_WRITE_REG(hw, IXGBE_FCTRL, reg);
 
-		if (enabled == pfc_enabled_tx ||
-		    enabled == pfc_enabled_full)
-			reg |= IXGBE_FCRTL_XONE;
+	fcrtl = (hw->fc.low_water << 10) | IXGBE_FCRTL_XONE;
+	/* Configure PFC Tx thresholds per TC */
+	for (i = 0; i < MAX_TRAFFIC_CLASS; i++) {
+		if (!(pfc_en & (1 << i))) {
+			IXGBE_WRITE_REG(hw, IXGBE_FCRTL(i), 0);
+			IXGBE_WRITE_REG(hw, IXGBE_FCRTH(i), 0);
+			continue;
+		}
+
+		reg = (hw->fc.high_water[i] << 10) | IXGBE_FCRTH_FCEN;
+		IXGBE_WRITE_REG(hw, IXGBE_FCRTL(i), fcrtl);
+		IXGBE_WRITE_REG(hw, IXGBE_FCRTH(i), reg);
+	}
 
-		IXGBE_WRITE_REG(hw, IXGBE_FCRTL(i), reg);
+	/* Configure pause time */
+	reg = hw->fc.pause_time * 0x00010001;
+	for (i = 0; i < (MAX_TRAFFIC_CLASS / 2); i++)
+		IXGBE_WRITE_REG(hw, IXGBE_FCTTV(i), reg);
 
-		reg = hw->fc.high_water[i] << 10;
-		if (enabled == pfc_enabled_tx ||
-		    enabled == pfc_enabled_full)
-			reg |= IXGBE_FCRTH_FCEN;
+	/* Configure flow control refresh threshold value */
+	IXGBE_WRITE_REG(hw, IXGBE_FCRTV, hw->fc.pause_time / 2);
 
-		IXGBE_WRITE_REG(hw, IXGBE_FCRTH(i), reg);
-	}
 
 	return 0;
 }
diff --git a/drivers/net/ethernet/intel/ixgbe/ixgbe_dcb_82599.c b/drivers/net/ethernet/intel/ixgbe/ixgbe_dcb_82599.c
index 888a419dc3d..4eac80d0185 100644
--- a/drivers/net/ethernet/intel/ixgbe/ixgbe_dcb_82599.c
+++ b/drivers/net/ethernet/intel/ixgbe/ixgbe_dcb_82599.c
@@ -211,24 +211,42 @@ s32 ixgbe_dcb_config_tx_data_arbiter_82599(struct ixgbe_hw *hw,
  */
 s32 ixgbe_dcb_config_pfc_82599(struct ixgbe_hw *hw, u8 pfc_en, u8 *prio_tc)
 {
-	u32 i, j, reg;
+	u32 i, j, fcrtl, reg;
 	u8 max_tc = 0;
 
-	for (i = 0; i < MAX_USER_PRIORITY; i++)
+	/* Enable Transmit Priority Flow Control */
+	IXGBE_WRITE_REG(hw, IXGBE_FCCFG, IXGBE_FCCFG_TFCE_PRIORITY);
+
+	/* Enable Receive Priority Flow Control */
+	reg = IXGBE_READ_REG(hw, IXGBE_MFLCN);
+	reg |= IXGBE_MFLCN_DPF;
+
+	/*
+	 * X540 supports per TC Rx priority flow control.  So
+	 * clear all TCs and only enable those that should be
+	 * enabled.
+	 */
+	reg &= ~(IXGBE_MFLCN_RPFCE_MASK | IXGBE_MFLCN_RFCE);
+
+	if (hw->mac.type == ixgbe_mac_X540)
+		reg |= pfc_en << IXGBE_MFLCN_RPFCE_SHIFT;
+
+	if (pfc_en)
+		reg |= IXGBE_MFLCN_RPFCE;
+
+	IXGBE_WRITE_REG(hw, IXGBE_MFLCN, reg);
+
+	for (i = 0; i < MAX_USER_PRIORITY; i++) {
 		if (prio_tc[i] > max_tc)
 			max_tc = prio_tc[i];
+	}
+
+	fcrtl = (hw->fc.low_water << 10) | IXGBE_FCRTL_XONE;
 
 	/* Configure PFC Tx thresholds per TC */
-	for (i = 0; i < MAX_TRAFFIC_CLASS; i++) {
+	for (i = 0; i <= max_tc; i++) {
 		int enabled = 0;
 
-		if (i > max_tc) {
-			reg = 0;
-			IXGBE_WRITE_REG(hw, IXGBE_FCRTL_82599(i), reg);
-			IXGBE_WRITE_REG(hw, IXGBE_FCRTH_82599(i), reg);
-			continue;
-		}
-
 		for (j = 0; j < MAX_USER_PRIORITY; j++) {
 			if ((prio_tc[j] == i) && (pfc_en & (1 << j))) {
 				enabled = 1;
@@ -236,61 +254,29 @@ s32 ixgbe_dcb_config_pfc_82599(struct ixgbe_hw *hw, u8 pfc_en, u8 *prio_tc)
 			}
 		}
 
-		reg = hw->fc.low_water << 10;
-
-		if (enabled)
-			reg |= IXGBE_FCRTL_XONE;
-		IXGBE_WRITE_REG(hw, IXGBE_FCRTL_82599(i), reg);
+		if (enabled) {
+			reg = (hw->fc.high_water[i] << 10) | IXGBE_FCRTH_FCEN;
+			IXGBE_WRITE_REG(hw, IXGBE_FCRTL_82599(i), fcrtl);
+		} else {
+			reg = IXGBE_READ_REG(hw, IXGBE_RXPBSIZE(i)) - 32;
+			IXGBE_WRITE_REG(hw, IXGBE_FCRTL_82599(i), 0);
+		}
 
-		reg = hw->fc.high_water[i] << 10;
-		if (enabled)
-			reg |= IXGBE_FCRTH_FCEN;
 		IXGBE_WRITE_REG(hw, IXGBE_FCRTH_82599(i), reg);
 	}
 
-	if (pfc_en) {
-		/* Configure pause time (2 TCs per register) */
-		reg = hw->fc.pause_time | (hw->fc.pause_time << 16);
-		for (i = 0; i < (MAX_TRAFFIC_CLASS / 2); i++)
-			IXGBE_WRITE_REG(hw, IXGBE_FCTTV(i), reg);
-
-		/* Configure flow control refresh threshold value */
-		IXGBE_WRITE_REG(hw, IXGBE_FCRTV, hw->fc.pause_time / 2);
-
-
-		reg = IXGBE_FCCFG_TFCE_PRIORITY;
-		IXGBE_WRITE_REG(hw, IXGBE_FCCFG, reg);
-		/*
-		 * Enable Receive PFC
-		 * 82599 will always honor XOFF frames we receive when
-		 * we are in PFC mode however X540 only honors enabled
-		 * traffic classes.
-		 */
-		reg = IXGBE_READ_REG(hw, IXGBE_MFLCN);
-		reg &= ~IXGBE_MFLCN_RFCE;
-		reg |= IXGBE_MFLCN_RPFCE | IXGBE_MFLCN_DPF;
-
-		if (hw->mac.type == ixgbe_mac_X540) {
-			reg &= ~IXGBE_MFLCN_RPFCE_MASK;
-			reg |= pfc_en << IXGBE_MFLCN_RPFCE_SHIFT;
-		}
+	for (; i < MAX_TRAFFIC_CLASS; i++) {
+		IXGBE_WRITE_REG(hw, IXGBE_FCRTL_82599(i), 0);
+		IXGBE_WRITE_REG(hw, IXGBE_FCRTH_82599(i), 0);
+	}
 
-		IXGBE_WRITE_REG(hw, IXGBE_MFLCN, reg);
-
-	} else {
-		/* X540 devices have a RX bit that should be cleared
-		 * if PFC is disabled on all TCs but PFC features is
-		 * enabled.
-		 */
-		if (hw->mac.type == ixgbe_mac_X540) {
-			reg = IXGBE_READ_REG(hw, IXGBE_MFLCN);
-			reg &= ~IXGBE_MFLCN_RPFCE_MASK;
-			IXGBE_WRITE_REG(hw, IXGBE_MFLCN, reg);
-		}
+	/* Configure pause time (2 TCs per register) */
+	reg = hw->fc.pause_time * 0x00010001;
+	for (i = 0; i < (MAX_TRAFFIC_CLASS / 2); i++)
+		IXGBE_WRITE_REG(hw, IXGBE_FCTTV(i), reg);
 
-		for (i = 0; i < MAX_TRAFFIC_CLASS; i++)
-			hw->mac.ops.fc_enable(hw, i);
-	}
+	/* Configure flow control refresh threshold value */
+	IXGBE_WRITE_REG(hw, IXGBE_FCRTV, hw->fc.pause_time / 2);
 
 	return 0;
 }
diff --git a/drivers/net/ethernet/intel/ixgbe/ixgbe_dcb_nl.c b/drivers/net/ethernet/intel/ixgbe/ixgbe_dcb_nl.c
index 652e4b09546..5164a21b13c 100644
--- a/drivers/net/ethernet/intel/ixgbe/ixgbe_dcb_nl.c
+++ b/drivers/net/ethernet/intel/ixgbe/ixgbe_dcb_nl.c
@@ -44,18 +44,26 @@
 #define DCB_NO_HW_CHG   1  /* DCB configuration did not change */
 #define DCB_HW_CHG      2  /* DCB configuration changed, no reset */
 
-int ixgbe_copy_dcb_cfg(struct ixgbe_dcb_config *scfg,
-		       struct ixgbe_dcb_config *dcfg, int tc_max)
+static int ixgbe_copy_dcb_cfg(struct ixgbe_adapter *adapter, int tc_max)
 {
+	struct ixgbe_dcb_config *scfg = &adapter->temp_dcb_cfg;
+	struct ixgbe_dcb_config *dcfg = &adapter->dcb_cfg;
 	struct tc_configuration *src = NULL;
 	struct tc_configuration *dst = NULL;
 	int i, j;
 	int tx = DCB_TX_CONFIG;
 	int rx = DCB_RX_CONFIG;
 	int changes = 0;
+#ifdef IXGBE_FCOE
+	struct dcb_app app = {
+			      .selector = DCB_APP_IDTYPE_ETHTYPE,
+			      .protocol = ETH_P_FCOE,
+			     };
+	u8 up = dcb_getapp(adapter->netdev, &app);
 
-	if (!scfg || !dcfg)
-		return changes;
+	if (up && !(up & (1 << adapter->fcoe.up)))
+		changes |= BIT_APP_UPCHG;
+#endif
 
 	for (i = DCB_PG_ATTR_TC_0; i < tc_max + DCB_PG_ATTR_TC_0; i++) {
 		src = &scfg->tc_config[i - DCB_PG_ATTR_TC_0];
@@ -330,60 +338,20 @@ static void ixgbe_dcbnl_devreset(struct net_device *dev)
 static u8 ixgbe_dcbnl_set_all(struct net_device *netdev)
 {
 	struct ixgbe_adapter *adapter = netdev_priv(netdev);
+	struct ixgbe_dcb_config *dcb_cfg = &adapter->dcb_cfg;
+	struct ixgbe_hw *hw = &adapter->hw;
 	int ret = DCB_NO_HW_CHG;
 	int i;
-#ifdef IXGBE_FCOE
-	struct dcb_app app = {
-			      .selector = DCB_APP_IDTYPE_ETHTYPE,
-			      .protocol = ETH_P_FCOE,
-			     };
-	u8 up;
-
-	/* In IEEE mode, use the IEEE Ethertype selector value */
-	if (adapter->dcbx_cap & DCB_CAP_DCBX_VER_IEEE) {
-		app.selector = IEEE_8021QAZ_APP_SEL_ETHERTYPE;
-		up = dcb_ieee_getapp_mask(netdev, &app);
-	} else {
-		up = dcb_getapp(netdev, &app);
-	}
-#endif
 
 	/* Fail command if not in CEE mode */
 	if (!(adapter->dcbx_cap & DCB_CAP_DCBX_VER_CEE))
 		return ret;
 
-	adapter->dcb_set_bitmap |= ixgbe_copy_dcb_cfg(&adapter->temp_dcb_cfg,
-						      &adapter->dcb_cfg,
+	adapter->dcb_set_bitmap |= ixgbe_copy_dcb_cfg(adapter,
 						      MAX_TRAFFIC_CLASS);
 	if (!adapter->dcb_set_bitmap)
 		return ret;
 
-	if (adapter->dcb_cfg.pfc_mode_enable) {
-		switch (adapter->hw.mac.type) {
-		case ixgbe_mac_82599EB:
-		case ixgbe_mac_X540:
-			if (adapter->hw.fc.current_mode != ixgbe_fc_pfc)
-				adapter->last_lfc_mode =
-				                  adapter->hw.fc.current_mode;
-			break;
-		default:
-			break;
-		}
-		adapter->hw.fc.requested_mode = ixgbe_fc_pfc;
-	} else {
-		switch (adapter->hw.mac.type) {
-		case ixgbe_mac_82598EB:
-			adapter->hw.fc.requested_mode = ixgbe_fc_none;
-			break;
-		case ixgbe_mac_82599EB:
-		case ixgbe_mac_X540:
-			adapter->hw.fc.requested_mode = adapter->last_lfc_mode;
-			break;
-		default:
-			break;
-		}
-	}
-
 	if (adapter->dcb_set_bitmap & (BIT_PG_TX|BIT_PG_RX)) {
 		u16 refill[MAX_TRAFFIC_CLASS], max[MAX_TRAFFIC_CLASS];
 		u8 bwg_id[MAX_TRAFFIC_CLASS], prio_type[MAX_TRAFFIC_CLASS];
@@ -396,23 +364,19 @@ static u8 ixgbe_dcbnl_set_all(struct net_device *netdev)
 			max_frame = max(max_frame, IXGBE_FCOE_JUMBO_FRAME_SIZE);
 #endif
 
-		ixgbe_dcb_calculate_tc_credits(&adapter->hw, &adapter->dcb_cfg,
-					       max_frame, DCB_TX_CONFIG);
-		ixgbe_dcb_calculate_tc_credits(&adapter->hw, &adapter->dcb_cfg,
-					       max_frame, DCB_RX_CONFIG);
+		ixgbe_dcb_calculate_tc_credits(hw, dcb_cfg, max_frame,
+					       DCB_TX_CONFIG);
+		ixgbe_dcb_calculate_tc_credits(hw, dcb_cfg, max_frame,
+					       DCB_RX_CONFIG);
 
-		ixgbe_dcb_unpack_refill(&adapter->dcb_cfg,
-					DCB_TX_CONFIG, refill);
-		ixgbe_dcb_unpack_max(&adapter->dcb_cfg, max);
-		ixgbe_dcb_unpack_bwgid(&adapter->dcb_cfg,
-				       DCB_TX_CONFIG, bwg_id);
-		ixgbe_dcb_unpack_prio(&adapter->dcb_cfg,
-				      DCB_TX_CONFIG, prio_type);
-		ixgbe_dcb_unpack_map(&adapter->dcb_cfg,
-				     DCB_TX_CONFIG, prio_tc);
+		ixgbe_dcb_unpack_refill(dcb_cfg, DCB_TX_CONFIG, refill);
+		ixgbe_dcb_unpack_max(dcb_cfg, max);
+		ixgbe_dcb_unpack_bwgid(dcb_cfg, DCB_TX_CONFIG, bwg_id);
+		ixgbe_dcb_unpack_prio(dcb_cfg, DCB_TX_CONFIG, prio_type);
+		ixgbe_dcb_unpack_map(dcb_cfg, DCB_TX_CONFIG, prio_tc);
 
-		ixgbe_dcb_hw_ets_config(&adapter->hw, refill, max,
-					bwg_id, prio_type, prio_tc);
+		ixgbe_dcb_hw_ets_config(hw, refill, max, bwg_id,
+					prio_type, prio_tc);
 
 		for (i = 0; i < IEEE_8021QAZ_MAX_TCS; i++)
 			netdev_set_prio_tc_map(netdev, i, prio_tc[i]);
@@ -421,27 +385,34 @@ static u8 ixgbe_dcbnl_set_all(struct net_device *netdev)
 	}
 
 	if (adapter->dcb_set_bitmap & BIT_PFC) {
-		u8 pfc_en;
-		u8 prio_tc[MAX_USER_PRIORITY];
+		if (dcb_cfg->pfc_mode_enable) {
+			u8 pfc_en;
+			u8 prio_tc[MAX_USER_PRIORITY];
+
+			ixgbe_dcb_unpack_map(dcb_cfg, DCB_TX_CONFIG, prio_tc);
+			ixgbe_dcb_unpack_pfc(dcb_cfg, &pfc_en);
+			ixgbe_dcb_hw_pfc_config(hw, pfc_en, prio_tc);
+		} else {
+			hw->mac.ops.fc_enable(hw);
+		}
 
-		ixgbe_dcb_unpack_map(&adapter->dcb_cfg,
-				     DCB_TX_CONFIG, prio_tc);
-		ixgbe_dcb_unpack_pfc(&adapter->dcb_cfg, &pfc_en);
-		ixgbe_dcb_hw_pfc_config(&adapter->hw, pfc_en, prio_tc);
-		if (ret != DCB_HW_CHG_RST)
-			ret = DCB_HW_CHG;
-	}
+		ixgbe_set_rx_drop_en(adapter);
 
-	if (adapter->dcb_cfg.pfc_mode_enable)
-		adapter->hw.fc.current_mode = ixgbe_fc_pfc;
+		ret = DCB_HW_CHG;
+	}
 
 #ifdef IXGBE_FCOE
 	/* Reprogam FCoE hardware offloads when the traffic class
 	 * FCoE is using changes. This happens if the APP info
 	 * changes or the up2tc mapping is updated.
 	 */
-	if ((up && !(up & (1 << adapter->fcoe.up))) ||
-	    (adapter->dcb_set_bitmap & BIT_APP_UPCHG)) {
+	if (adapter->dcb_set_bitmap & BIT_APP_UPCHG) {
+		struct dcb_app app = {
+				      .selector = DCB_APP_IDTYPE_ETHTYPE,
+				      .protocol = ETH_P_FCOE,
+				     };
+		u8 up = dcb_getapp(netdev, &app);
+
 		adapter->fcoe.up = ffs(up) - 1;
 		ixgbe_dcbnl_devreset(netdev);
 		ret = DCB_HW_CHG_RST;
@@ -650,7 +621,9 @@ static int ixgbe_dcbnl_ieee_setpfc(struct net_device *dev,
 				   struct ieee_pfc *pfc)
 {
 	struct ixgbe_adapter *adapter = netdev_priv(dev);
+	struct ixgbe_hw *hw = &adapter->hw;
 	u8 *prio_tc;
+	int err;
 
 	if (!(adapter->dcbx_cap & DCB_CAP_DCBX_VER_IEEE))
 		return -EINVAL;
@@ -664,7 +637,16 @@ static int ixgbe_dcbnl_ieee_setpfc(struct net_device *dev,
 
 	prio_tc = adapter->ixgbe_ieee_ets->prio_tc;
 	memcpy(adapter->ixgbe_ieee_pfc, pfc, sizeof(*adapter->ixgbe_ieee_pfc));
-	return ixgbe_dcb_hw_pfc_config(&adapter->hw, pfc->pfc_en, prio_tc);
+
+	/* Enable link flow control parameters if PFC is disabled */
+	if (pfc->pfc_en)
+		err = ixgbe_dcb_hw_pfc_config(hw, pfc->pfc_en, prio_tc);
+	else
+		err = hw->mac.ops.fc_enable(hw);
+
+	ixgbe_set_rx_drop_en(adapter);
+
+	return err;
 }
 
 static int ixgbe_dcbnl_ieee_setapp(struct net_device *dev,
diff --git a/drivers/net/ethernet/intel/ixgbe/ixgbe_ethtool.c b/drivers/net/ethernet/intel/ixgbe/ixgbe_ethtool.c
index 31a2bf76a34..3178f1ec371 100644
--- a/drivers/net/ethernet/intel/ixgbe/ixgbe_ethtool.c
+++ b/drivers/net/ethernet/intel/ixgbe/ixgbe_ethtool.c
@@ -391,11 +391,6 @@ static void ixgbe_get_pauseparam(struct net_device *netdev,
 	} else if (hw->fc.current_mode == ixgbe_fc_full) {
 		pause->rx_pause = 1;
 		pause->tx_pause = 1;
-#ifdef CONFIG_DCB
-	} else if (hw->fc.current_mode == ixgbe_fc_pfc) {
-		pause->rx_pause = 0;
-		pause->tx_pause = 0;
-#endif
 	}
 }
 
@@ -404,21 +399,14 @@ static int ixgbe_set_pauseparam(struct net_device *netdev,
 {
 	struct ixgbe_adapter *adapter = netdev_priv(netdev);
 	struct ixgbe_hw *hw = &adapter->hw;
-	struct ixgbe_fc_info fc;
+	struct ixgbe_fc_info fc = hw->fc;
 
-#ifdef CONFIG_DCB
-	if (adapter->dcb_cfg.pfc_mode_enable ||
-		((hw->mac.type == ixgbe_mac_82598EB) &&
-		(adapter->flags & IXGBE_FLAG_DCB_ENABLED)))
+	/* 82598 does no support link flow control with DCB enabled */
+	if ((hw->mac.type == ixgbe_mac_82598EB) &&
+	    (adapter->flags & IXGBE_FLAG_DCB_ENABLED))
 		return -EINVAL;
 
-#endif
-	fc = hw->fc;
-
-	if (pause->autoneg != AUTONEG_ENABLE)
-		fc.disable_fc_autoneg = true;
-	else
-		fc.disable_fc_autoneg = false;
+	fc.disable_fc_autoneg = (pause->autoneg != AUTONEG_ENABLE);
 
 	if ((pause->rx_pause && pause->tx_pause) || pause->autoneg)
 		fc.requested_mode = ixgbe_fc_full;
@@ -426,14 +414,8 @@ static int ixgbe_set_pauseparam(struct net_device *netdev,
 		fc.requested_mode = ixgbe_fc_rx_pause;
 	else if (!pause->rx_pause && pause->tx_pause)
 		fc.requested_mode = ixgbe_fc_tx_pause;
-	else if (!pause->rx_pause && !pause->tx_pause)
-		fc.requested_mode = ixgbe_fc_none;
 	else
-		return -EINVAL;
-
-#ifdef CONFIG_DCB
-	adapter->last_lfc_mode = fc.requested_mode;
-#endif
+		fc.requested_mode = ixgbe_fc_none;
 
 	/* if the thing changed then we'll update and use new autoneg */
 	if (memcmp(&fc, &hw->fc, sizeof(struct ixgbe_fc_info))) {
@@ -1780,6 +1762,8 @@ static u16 ixgbe_clean_test_rings(struct ixgbe_ring *rx_ring,
 		rx_desc = IXGBE_RX_DESC(rx_ring, rx_ntc);
 	}
 
+	netdev_tx_reset_queue(txring_txq(tx_ring));
+
 	/* re-map buffers to ring, store next to clean values */
 	ixgbe_alloc_rx_buffers(rx_ring, count);
 	rx_ring->next_to_clean = rx_ntc;
@@ -1969,53 +1953,12 @@ static int ixgbe_wol_exclusion(struct ixgbe_adapter *adapter,
                                struct ethtool_wolinfo *wol)
 {
 	struct ixgbe_hw *hw = &adapter->hw;
-	int retval = 1;
-	u16 wol_cap = adapter->eeprom_cap & IXGBE_DEVICE_CAPS_WOL_MASK;
-
-	/* WOL not supported except for the following */
-	switch(hw->device_id) {
-	case IXGBE_DEV_ID_82599_SFP:
-		/* Only these subdevices could supports WOL */
-		switch (hw->subsystem_device_id) {
-		case IXGBE_SUBDEV_ID_82599_560FLR:
-			/* only support first port */
-			if (hw->bus.func != 0) {
-				wol->supported = 0;
-				break;
-			}
-		case IXGBE_SUBDEV_ID_82599_SFP:
-			retval = 0;
-			break;
-		default:
-			wol->supported = 0;
-			break;
-		}
-		break;
-	case IXGBE_DEV_ID_82599_COMBO_BACKPLANE:
-		/* All except this subdevice support WOL */
-		if (hw->subsystem_device_id ==
-		    IXGBE_SUBDEV_ID_82599_KX4_KR_MEZZ) {
-			wol->supported = 0;
-			break;
-		}
-		retval = 0;
-		break;
-	case IXGBE_DEV_ID_82599_KX4:
-		retval = 0;
-		break;
-	case IXGBE_DEV_ID_X540T:
-		/* check eeprom to see if enabled wol */
-		if ((wol_cap == IXGBE_DEVICE_CAPS_WOL_PORT0_1) ||
-		    ((wol_cap == IXGBE_DEVICE_CAPS_WOL_PORT0) &&
-		     (hw->bus.func == 0))) {
-			retval = 0;
-			break;
-		}
+	int retval = 0;
 
-		/* All others not supported */
-		wol->supported = 0;
-		break;
-	default:
+	/* WOL not supported for all devices */
+	if (!ixgbe_wol_supported(adapter, hw->device_id,
+				 hw->subsystem_device_id)) {
+		retval = 1;
 		wol->supported = 0;
 	}
 
@@ -2753,6 +2696,46 @@ static int ixgbe_set_rxnfc(struct net_device *dev, struct ethtool_rxnfc *cmd)
 	return ret;
 }
 
+static int ixgbe_get_ts_info(struct net_device *dev,
+			     struct ethtool_ts_info *info)
+{
+	struct ixgbe_adapter *adapter = netdev_priv(dev);
+
+	switch (adapter->hw.mac.type) {
+#ifdef CONFIG_IXGBE_PTP
+	case ixgbe_mac_X540:
+	case ixgbe_mac_82599EB:
+		info->so_timestamping =
+			SOF_TIMESTAMPING_TX_HARDWARE |
+			SOF_TIMESTAMPING_RX_HARDWARE |
+			SOF_TIMESTAMPING_RAW_HARDWARE;
+
+		if (adapter->ptp_clock)
+			info->phc_index = ptp_clock_index(adapter->ptp_clock);
+		else
+			info->phc_index = -1;
+
+		info->tx_types =
+			(1 << HWTSTAMP_TX_OFF) |
+			(1 << HWTSTAMP_TX_ON);
+
+		info->rx_filters =
+			(1 << HWTSTAMP_FILTER_NONE) |
+			(1 << HWTSTAMP_FILTER_PTP_V1_L4_SYNC) |
+			(1 << HWTSTAMP_FILTER_PTP_V1_L4_DELAY_REQ) |
+			(1 << HWTSTAMP_FILTER_PTP_V2_SYNC) |
+			(1 << HWTSTAMP_FILTER_PTP_V2_DELAY_REQ) |
+			(1 << HWTSTAMP_FILTER_PTP_V2_EVENT) |
+			(1 << HWTSTAMP_FILTER_SOME);
+		break;
+#endif /* CONFIG_IXGBE_PTP */
+	default:
+		return ethtool_op_get_ts_info(dev, info);
+		break;
+	}
+	return 0;
+}
+
 static const struct ethtool_ops ixgbe_ethtool_ops = {
 	.get_settings           = ixgbe_get_settings,
 	.set_settings           = ixgbe_set_settings,
@@ -2781,6 +2764,7 @@ static const struct ethtool_ops ixgbe_ethtool_ops = {
 	.set_coalesce           = ixgbe_set_coalesce,
 	.get_rxnfc		= ixgbe_get_rxnfc,
 	.set_rxnfc		= ixgbe_set_rxnfc,
+	.get_ts_info		= ixgbe_get_ts_info,
 };
 
 void ixgbe_set_ethtool_ops(struct net_device *netdev)
diff --git a/drivers/net/ethernet/intel/ixgbe/ixgbe_lib.c b/drivers/net/ethernet/intel/ixgbe/ixgbe_lib.c
index ed1b47dc083..af1a5314b49 100644
--- a/drivers/net/ethernet/intel/ixgbe/ixgbe_lib.c
+++ b/drivers/net/ethernet/intel/ixgbe/ixgbe_lib.c
@@ -523,11 +523,17 @@ static void ixgbe_add_ring(struct ixgbe_ring *ring,
 /**
  * ixgbe_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.
  **/
-static int ixgbe_alloc_q_vector(struct ixgbe_adapter *adapter, int v_idx,
+static int ixgbe_alloc_q_vector(struct ixgbe_adapter *adapter,
+				int v_count, int v_idx,
 				int txr_count, int txr_idx,
 				int rxr_count, int rxr_idx)
 {
@@ -598,7 +604,7 @@ static int ixgbe_alloc_q_vector(struct ixgbe_adapter *adapter, int v_idx,
 
 		/* update count and index */
 		txr_count--;
-		txr_idx++;
+		txr_idx += v_count;
 
 		/* push pointer to next ring */
 		ring++;
@@ -641,7 +647,7 @@ static int ixgbe_alloc_q_vector(struct ixgbe_adapter *adapter, int v_idx,
 
 		/* update count and index */
 		rxr_count--;
-		rxr_idx++;
+		rxr_idx += v_count;
 
 		/* push pointer to next ring */
 		ring++;
@@ -700,24 +706,23 @@ static int ixgbe_alloc_q_vectors(struct ixgbe_adapter *adapter)
 		q_vectors = 1;
 
 	if (q_vectors >= (rxr_remaining + txr_remaining)) {
-		for (; rxr_remaining; v_idx++, q_vectors--) {
-			int rqpv = DIV_ROUND_UP(rxr_remaining, q_vectors);
-			err = ixgbe_alloc_q_vector(adapter, v_idx,
-						   0, 0, rqpv, rxr_idx);
+		for (; rxr_remaining; v_idx++) {
+			err = ixgbe_alloc_q_vector(adapter, q_vectors, v_idx,
+						   0, 0, 1, rxr_idx);
 
 			if (err)
 				goto err_out;
 
 			/* update counts and index */
-			rxr_remaining -= rqpv;
-			rxr_idx += rqpv;
+			rxr_remaining--;
+			rxr_idx++;
 		}
 	}
 
-	for (; q_vectors; v_idx++, q_vectors--) {
-		int rqpv = DIV_ROUND_UP(rxr_remaining, q_vectors);
-		int tqpv = DIV_ROUND_UP(txr_remaining, q_vectors);
-		err = ixgbe_alloc_q_vector(adapter, v_idx,
+	for (; v_idx < q_vectors; v_idx++) {
+		int rqpv = DIV_ROUND_UP(rxr_remaining, q_vectors - v_idx);
+		int tqpv = DIV_ROUND_UP(txr_remaining, q_vectors - v_idx);
+		err = ixgbe_alloc_q_vector(adapter, q_vectors, v_idx,
 					   tqpv, txr_idx,
 					   rqpv, rxr_idx);
 
@@ -726,9 +731,9 @@ static int ixgbe_alloc_q_vectors(struct ixgbe_adapter *adapter)
 
 		/* update counts and index */
 		rxr_remaining -= rqpv;
-		rxr_idx += rqpv;
 		txr_remaining -= tqpv;
-		txr_idx += tqpv;
+		rxr_idx++;
+		txr_idx++;
 	}
 
 	return 0;
diff --git a/drivers/net/ethernet/intel/ixgbe/ixgbe_main.c b/drivers/net/ethernet/intel/ixgbe/ixgbe_main.c
index 88f6b2e9b72..bf20457ea23 100644
--- a/drivers/net/ethernet/intel/ixgbe/ixgbe_main.c
+++ b/drivers/net/ethernet/intel/ixgbe/ixgbe_main.c
@@ -63,8 +63,8 @@ static char ixgbe_default_device_descr[] =
 			      "Intel(R) 10 Gigabit Network Connection";
 #endif
 #define MAJ 3
-#define MIN 8
-#define BUILD 21
+#define MIN 9
+#define BUILD 15
 #define DRV_VERSION __stringify(MAJ) "." __stringify(MIN) "." \
 	__stringify(BUILD) "-k"
 const char ixgbe_driver_version[] = DRV_VERSION;
@@ -133,7 +133,7 @@ static struct notifier_block dca_notifier = {
 static unsigned int max_vfs;
 module_param(max_vfs, uint, 0);
 MODULE_PARM_DESC(max_vfs,
-		 "Maximum number of virtual functions to allocate per physical function");
+		 "Maximum number of virtual functions to allocate per physical function - default is zero and maximum value is 63");
 #endif /* CONFIG_PCI_IOV */
 
 static unsigned int allow_unsupported_sfp;
@@ -610,35 +610,50 @@ void ixgbe_unmap_and_free_tx_resource(struct ixgbe_ring *ring,
 	/* tx_buffer must be completely set up in the transmit path */
 }
 
-static void ixgbe_update_xoff_received(struct ixgbe_adapter *adapter)
+static void ixgbe_update_xoff_rx_lfc(struct ixgbe_adapter *adapter)
 {
 	struct ixgbe_hw *hw = &adapter->hw;
 	struct ixgbe_hw_stats *hwstats = &adapter->stats;
-	u32 data = 0;
-	u32 xoff[8] = {0};
 	int i;
+	u32 data;
 
-	if ((hw->fc.current_mode == ixgbe_fc_full) ||
-	    (hw->fc.current_mode == ixgbe_fc_rx_pause)) {
-		switch (hw->mac.type) {
-		case ixgbe_mac_82598EB:
-			data = IXGBE_READ_REG(hw, IXGBE_LXOFFRXC);
-			break;
-		default:
-			data = IXGBE_READ_REG(hw, IXGBE_LXOFFRXCNT);
-		}
-		hwstats->lxoffrxc += data;
+	if ((hw->fc.current_mode != ixgbe_fc_full) &&
+	    (hw->fc.current_mode != ixgbe_fc_rx_pause))
+		return;
 
-		/* refill credits (no tx hang) if we received xoff */
-		if (!data)
-			return;
+	switch (hw->mac.type) {
+	case ixgbe_mac_82598EB:
+		data = IXGBE_READ_REG(hw, IXGBE_LXOFFRXC);
+		break;
+	default:
+		data = IXGBE_READ_REG(hw, IXGBE_LXOFFRXCNT);
+	}
+	hwstats->lxoffrxc += data;
 
-		for (i = 0; i < adapter->num_tx_queues; i++)
-			clear_bit(__IXGBE_HANG_CHECK_ARMED,
-				  &adapter->tx_ring[i]->state);
+	/* refill credits (no tx hang) if we received xoff */
+	if (!data)
 		return;
-	} else if (!(adapter->dcb_cfg.pfc_mode_enable))
+
+	for (i = 0; i < adapter->num_tx_queues; i++)
+		clear_bit(__IXGBE_HANG_CHECK_ARMED,
+			  &adapter->tx_ring[i]->state);
+}
+
+static void ixgbe_update_xoff_received(struct ixgbe_adapter *adapter)
+{
+	struct ixgbe_hw *hw = &adapter->hw;
+	struct ixgbe_hw_stats *hwstats = &adapter->stats;
+	u32 xoff[8] = {0};
+	int i;
+	bool pfc_en = adapter->dcb_cfg.pfc_mode_enable;
+
+	if (adapter->ixgbe_ieee_pfc)
+		pfc_en |= !!(adapter->ixgbe_ieee_pfc->pfc_en);
+
+	if (!(adapter->flags & IXGBE_FLAG_DCB_ENABLED) || !pfc_en) {
+		ixgbe_update_xoff_rx_lfc(adapter);
 		return;
+	}
 
 	/* update stats for each tc, only valid with PFC enabled */
 	for (i = 0; i < MAX_TX_PACKET_BUFFERS; i++) {
@@ -774,6 +789,13 @@ static bool ixgbe_clean_tx_irq(struct ixgbe_q_vector *q_vector,
 		total_bytes += tx_buffer->bytecount;
 		total_packets += tx_buffer->gso_segs;
 
+#ifdef CONFIG_IXGBE_PTP
+		if (unlikely(tx_buffer->tx_flags &
+			     IXGBE_TX_FLAGS_TSTAMP))
+			ixgbe_ptp_tx_hwtstamp(q_vector,
+					      tx_buffer->skb);
+
+#endif
 		/* free the skb */
 		dev_kfree_skb_any(tx_buffer->skb);
 
@@ -1144,7 +1166,7 @@ static bool ixgbe_alloc_mapped_page(struct ixgbe_ring *rx_ring,
 	 * there isn't much point in holding memory we can't use
 	 */
 	if (dma_mapping_error(rx_ring->dev, dma)) {
-		put_page(page);
+		__free_pages(page, ixgbe_rx_pg_order(rx_ring));
 		bi->page = NULL;
 
 		rx_ring->rx_stats.alloc_rx_page_failed++;
@@ -1374,6 +1396,11 @@ static void ixgbe_process_skb_fields(struct ixgbe_ring *rx_ring,
 
 	ixgbe_rx_checksum(rx_ring, rx_desc, skb);
 
+#ifdef CONFIG_IXGBE_PTP
+	if (ixgbe_test_staterr(rx_desc, IXGBE_RXDADV_STAT_TS))
+		ixgbe_ptp_rx_hwtstamp(rx_ring->q_vector, skb);
+#endif
+
 	if (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);
@@ -2295,6 +2322,9 @@ static irqreturn_t ixgbe_msix_other(int irq, void *data)
 	}
 
 	ixgbe_check_fan_failure(adapter, eicr);
+#ifdef CONFIG_IXGBE_PTP
+	ixgbe_ptp_check_pps_event(adapter, eicr);
+#endif
 
 	/* re-enable the original interrupt state, no lsc, no queues */
 	if (!test_bit(__IXGBE_DOWN, &adapter->state))
@@ -2487,6 +2517,9 @@ static irqreturn_t ixgbe_intr(int irq, void *data)
 	}
 
 	ixgbe_check_fan_failure(adapter, eicr);
+#ifdef CONFIG_IXGBE_PTP
+	ixgbe_ptp_check_pps_event(adapter, eicr);
+#endif
 
 	/* would disable interrupts here but EIAM disabled it */
 	napi_schedule(&q_vector->napi);
@@ -2671,8 +2704,6 @@ void ixgbe_configure_tx_ring(struct ixgbe_adapter *adapter,
 	/* enable queue */
 	IXGBE_WRITE_REG(hw, IXGBE_TXDCTL(reg_idx), txdctl);
 
-	netdev_tx_reset_queue(txring_txq(ring));
-
 	/* TXDCTL.EN will return 0 on 82598 if link is down, so skip it */
 	if (hw->mac.type == ixgbe_mac_82598EB &&
 	    !(IXGBE_READ_REG(hw, IXGBE_LINKS) & IXGBE_LINKS_UP))
@@ -2758,6 +2789,61 @@ static void ixgbe_configure_tx(struct ixgbe_adapter *adapter)
 		ixgbe_configure_tx_ring(adapter, adapter->tx_ring[i]);
 }
 
+static void ixgbe_enable_rx_drop(struct ixgbe_adapter *adapter,
+				 struct ixgbe_ring *ring)
+{
+	struct ixgbe_hw *hw = &adapter->hw;
+	u8 reg_idx = ring->reg_idx;
+	u32 srrctl = IXGBE_READ_REG(hw, IXGBE_SRRCTL(reg_idx));
+
+	srrctl |= IXGBE_SRRCTL_DROP_EN;
+
+	IXGBE_WRITE_REG(hw, IXGBE_SRRCTL(reg_idx), srrctl);
+}
+
+static void ixgbe_disable_rx_drop(struct ixgbe_adapter *adapter,
+				  struct ixgbe_ring *ring)
+{
+	struct ixgbe_hw *hw = &adapter->hw;
+	u8 reg_idx = ring->reg_idx;
+	u32 srrctl = IXGBE_READ_REG(hw, IXGBE_SRRCTL(reg_idx));
+
+	srrctl &= ~IXGBE_SRRCTL_DROP_EN;
+
+	IXGBE_WRITE_REG(hw, IXGBE_SRRCTL(reg_idx), srrctl);
+}
+
+#ifdef CONFIG_IXGBE_DCB
+void ixgbe_set_rx_drop_en(struct ixgbe_adapter *adapter)
+#else
+static void ixgbe_set_rx_drop_en(struct ixgbe_adapter *adapter)
+#endif
+{
+	int i;
+	bool pfc_en = adapter->dcb_cfg.pfc_mode_enable;
+
+	if (adapter->ixgbe_ieee_pfc)
+		pfc_en |= !!(adapter->ixgbe_ieee_pfc->pfc_en);
+
+	/*
+	 * We should set the drop enable bit if:
+	 *  SR-IOV is enabled
+	 *   or
+	 *  Number of Rx queues > 1 and flow control is disabled
+	 *
+	 *  This allows us to avoid head of line blocking for security
+	 *  and performance reasons.
+	 */
+	if (adapter->num_vfs || (adapter->num_rx_queues > 1 &&
+	    !(adapter->hw.fc.current_mode & ixgbe_fc_tx_pause) && !pfc_en)) {
+		for (i = 0; i < adapter->num_rx_queues; i++)
+			ixgbe_enable_rx_drop(adapter, adapter->rx_ring[i]);
+	} else {
+		for (i = 0; i < adapter->num_rx_queues; i++)
+			ixgbe_disable_rx_drop(adapter, adapter->rx_ring[i]);
+	}
+}
+
 #define IXGBE_SRRCTL_BSIZEHDRSIZE_SHIFT 2
 
 static void ixgbe_configure_srrctl(struct ixgbe_adapter *adapter,
@@ -2904,33 +2990,6 @@ static void ixgbe_configure_rscctl(struct ixgbe_adapter *adapter,
 	IXGBE_WRITE_REG(hw, IXGBE_RSCCTL(reg_idx), rscctrl);
 }
 
-/**
- *  ixgbe_set_uta - Set unicast filter table address
- *  @adapter: board private structure
- *
- *  The unicast table address is a register array of 32-bit registers.
- *  The table is meant to be used in a way similar to how the MTA is used
- *  however due to certain limitations in the hardware it is necessary to
- *  set all the hash bits to 1 and use the VMOLR ROPE bit as a promiscuous
- *  enable bit to allow vlan tag stripping when promiscuous mode is enabled
- **/
-static void ixgbe_set_uta(struct ixgbe_adapter *adapter)
-{
-	struct ixgbe_hw *hw = &adapter->hw;
-	int i;
-
-	/* The UTA table only exists on 82599 hardware and newer */
-	if (hw->mac.type < ixgbe_mac_82599EB)
-		return;
-
-	/* we only need to do this if VMDq is enabled */
-	if (!(adapter->flags & IXGBE_FLAG_SRIOV_ENABLED))
-		return;
-
-	for (i = 0; i < 128; i++)
-		IXGBE_WRITE_REG(hw, IXGBE_UTA(i), ~0);
-}
-
 #define IXGBE_MAX_RX_DESC_POLL 10
 static void ixgbe_rx_desc_queue_enable(struct ixgbe_adapter *adapter,
 				       struct ixgbe_ring *ring)
@@ -3216,8 +3275,6 @@ static void ixgbe_configure_rx(struct ixgbe_adapter *adapter)
 	/* Program registers for the distribution of queues */
 	ixgbe_setup_mrqc(adapter);
 
-	ixgbe_set_uta(adapter);
-
 	/* set_rx_buffer_len must be called before ring initialization */
 	ixgbe_set_rx_buffer_len(adapter);
 
@@ -3454,16 +3511,17 @@ void ixgbe_set_rx_mode(struct net_device *netdev)
 		}
 		ixgbe_vlan_filter_enable(adapter);
 		hw->addr_ctrl.user_set_promisc = false;
-		/*
-		 * Write addresses to available RAR registers, if there is not
-		 * sufficient space to store all the addresses then enable
-		 * unicast promiscuous mode
-		 */
-		count = ixgbe_write_uc_addr_list(netdev);
-		if (count < 0) {
-			fctrl |= IXGBE_FCTRL_UPE;
-			vmolr |= IXGBE_VMOLR_ROPE;
-		}
+	}
+
+	/*
+	 * Write addresses to available RAR registers, if there is not
+	 * sufficient space to store all the addresses then enable
+	 * unicast promiscuous mode
+	 */
+	count = ixgbe_write_uc_addr_list(netdev);
+	if (count < 0) {
+		fctrl |= IXGBE_FCTRL_UPE;
+		vmolr |= IXGBE_VMOLR_ROPE;
 	}
 
 	if (adapter->num_vfs) {
@@ -4130,7 +4188,8 @@ static void ixgbe_clean_rx_ring(struct ixgbe_ring *rx_ring)
 				       DMA_FROM_DEVICE);
 		rx_buffer->dma = 0;
 		if (rx_buffer->page)
-			put_page(rx_buffer->page);
+			__free_pages(rx_buffer->page,
+				     ixgbe_rx_pg_order(rx_ring));
 		rx_buffer->page = NULL;
 	}
 
@@ -4167,6 +4226,8 @@ static void ixgbe_clean_tx_ring(struct ixgbe_ring *tx_ring)
 		ixgbe_unmap_and_free_tx_resource(tx_ring, tx_buffer_info);
 	}
 
+	netdev_tx_reset_queue(txring_txq(tx_ring));
+
 	size = sizeof(struct ixgbe_tx_buffer) * tx_ring->count;
 	memset(tx_ring->tx_buffer_info, 0, size);
 
@@ -4418,17 +4479,14 @@ static int __devinit ixgbe_sw_init(struct ixgbe_adapter *adapter)
 	adapter->dcb_cfg.pfc_mode_enable = false;
 	adapter->dcb_set_bitmap = 0x00;
 	adapter->dcbx_cap = DCB_CAP_DCBX_HOST | DCB_CAP_DCBX_VER_CEE;
-	ixgbe_copy_dcb_cfg(&adapter->dcb_cfg, &adapter->temp_dcb_cfg,
-			   MAX_TRAFFIC_CLASS);
+	memcpy(&adapter->temp_dcb_cfg, &adapter->dcb_cfg,
+	       sizeof(adapter->temp_dcb_cfg));
 
 #endif
 
 	/* default flow control settings */
 	hw->fc.requested_mode = ixgbe_fc_full;
 	hw->fc.current_mode = ixgbe_fc_full;	/* init for ethtool output */
-#ifdef CONFIG_DCB
-	adapter->last_lfc_mode = hw->fc.current_mode;
-#endif
 	ixgbe_pbthresh_setup(adapter);
 	hw->fc.pause_time = IXGBE_DEFAULT_FCPAUSE;
 	hw->fc.send_xon = true;
@@ -4866,10 +4924,12 @@ static int __ixgbe_shutdown(struct pci_dev *pdev, bool *enable_wake)
 	netif_device_detach(netdev);
 
 	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();
 	}
 
 	ixgbe_clear_interrupt_scheme(adapter);
@@ -4991,9 +5051,6 @@ void ixgbe_update_stats(struct ixgbe_adapter *adapter)
 	if (adapter->flags2 & IXGBE_FLAG2_RSC_ENABLED) {
 		u64 rsc_count = 0;
 		u64 rsc_flush = 0;
-		for (i = 0; i < 16; i++)
-			adapter->hw_rx_no_dma_resources +=
-				IXGBE_READ_REG(hw, IXGBE_QPRDC(i));
 		for (i = 0; i < adapter->num_rx_queues; i++) {
 			rsc_count += adapter->rx_ring[i]->rx_stats.rsc_count;
 			rsc_flush += adapter->rx_ring[i]->rx_stats.rsc_flush;
@@ -5096,6 +5153,9 @@ void ixgbe_update_stats(struct ixgbe_adapter *adapter)
 		hwstats->b2ospc += IXGBE_READ_REG(hw, IXGBE_B2OSPC);
 		hwstats->b2ogprc += IXGBE_READ_REG(hw, IXGBE_B2OGPRC);
 	case ixgbe_mac_82599EB:
+		for (i = 0; i < 16; i++)
+			adapter->hw_rx_no_dma_resources +=
+					     IXGBE_READ_REG(hw, IXGBE_QPRDC(i));
 		hwstats->gorc += IXGBE_READ_REG(hw, IXGBE_GORCL);
 		IXGBE_READ_REG(hw, IXGBE_GORCH); /* to clear */
 		hwstats->gotc += IXGBE_READ_REG(hw, IXGBE_GOTCL);
@@ -5273,7 +5333,7 @@ static void ixgbe_watchdog_update_link(struct ixgbe_adapter *adapter)
 	struct ixgbe_hw *hw = &adapter->hw;
 	u32 link_speed = adapter->link_speed;
 	bool link_up = adapter->link_up;
-	int i;
+	bool pfc_en = adapter->dcb_cfg.pfc_mode_enable;
 
 	if (!(adapter->flags & IXGBE_FLAG_NEED_LINK_UPDATE))
 		return;
@@ -5285,13 +5345,13 @@ static void ixgbe_watchdog_update_link(struct ixgbe_adapter *adapter)
 		link_speed = IXGBE_LINK_SPEED_10GB_FULL;
 		link_up = true;
 	}
-	if (link_up) {
-		if (adapter->flags & IXGBE_FLAG_DCB_ENABLED) {
-			for (i = 0; i < MAX_TRAFFIC_CLASS; i++)
-				hw->mac.ops.fc_enable(hw, i);
-		} else {
-			hw->mac.ops.fc_enable(hw, 0);
-		}
+
+	if (adapter->ixgbe_ieee_pfc)
+		pfc_en |= !!(adapter->ixgbe_ieee_pfc->pfc_en);
+
+	if (link_up && !((adapter->flags & IXGBE_FLAG_DCB_ENABLED) && pfc_en)) {
+		hw->mac.ops.fc_enable(hw);
+		ixgbe_set_rx_drop_en(adapter);
 	}
 
 	if (link_up ||
@@ -5345,6 +5405,11 @@ static void ixgbe_watchdog_link_is_up(struct ixgbe_adapter *adapter)
 		flow_rx = false;
 		break;
 	}
+
+#ifdef CONFIG_IXGBE_PTP
+	ixgbe_ptp_start_cyclecounter(adapter);
+#endif
+
 	e_info(drv, "NIC Link is Up %s, Flow Control: %s\n",
 	       (link_speed == IXGBE_LINK_SPEED_10GB_FULL ?
 	       "10 Gbps" :
@@ -5382,6 +5447,10 @@ static void ixgbe_watchdog_link_is_down(struct ixgbe_adapter *adapter)
 	if (ixgbe_is_sfp(hw) && hw->mac.type == ixgbe_mac_82598EB)
 		adapter->flags2 |= IXGBE_FLAG2_SEARCH_FOR_SFP;
 
+#ifdef CONFIG_IXGBE_PTP
+	ixgbe_ptp_start_cyclecounter(adapter);
+#endif
+
 	e_info(drv, "NIC Link is Down\n");
 	netif_carrier_off(netdev);
 }
@@ -5681,6 +5750,9 @@ static void ixgbe_service_task(struct work_struct *work)
 	ixgbe_watchdog_subtask(adapter);
 	ixgbe_fdir_reinit_subtask(adapter);
 	ixgbe_check_hang_subtask(adapter);
+#ifdef CONFIG_IXGBE_PTP
+	ixgbe_ptp_overflow_check(adapter);
+#endif
 
 	ixgbe_service_event_complete(adapter);
 }
@@ -5831,6 +5903,11 @@ static __le32 ixgbe_tx_cmd_type(u32 tx_flags)
 	if (tx_flags & IXGBE_TX_FLAGS_HW_VLAN)
 		cmd_type |= cpu_to_le32(IXGBE_ADVTXD_DCMD_VLE);
 
+#ifdef CONFIG_IXGBE_PTP
+	if (tx_flags & IXGBE_TX_FLAGS_TSTAMP)
+		cmd_type |= cpu_to_le32(IXGBE_ADVTXD_MAC_TSTAMP);
+#endif
+
 	/* set segmentation enable bits for TSO/FSO */
 #ifdef IXGBE_FCOE
 	if (tx_flags & (IXGBE_TX_FLAGS_TSO | IXGBE_TX_FLAGS_FSO))
@@ -6221,6 +6298,15 @@ netdev_tx_t ixgbe_xmit_frame_ring(struct sk_buff *skb,
 		tx_flags |= IXGBE_TX_FLAGS_SW_VLAN;
 	}
 
+	skb_tx_timestamp(skb);
+
+#ifdef CONFIG_IXGBE_PTP
+	if (unlikely(skb_shinfo(skb)->tx_flags & SKBTX_HW_TSTAMP)) {
+		skb_shinfo(skb)->tx_flags |= SKBTX_IN_PROGRESS;
+		tx_flags |= IXGBE_TX_FLAGS_TSTAMP;
+	}
+#endif
+
 #ifdef CONFIG_PCI_IOV
 	/*
 	 * Use the l2switch_enable flag - would be false if the DMA
@@ -6373,7 +6459,14 @@ static int ixgbe_ioctl(struct net_device *netdev, struct ifreq *req, int cmd)
 {
 	struct ixgbe_adapter *adapter = netdev_priv(netdev);
 
-	return mdio_mii_ioctl(&adapter->hw.phy.mdio, if_mii(req), cmd);
+	switch (cmd) {
+#ifdef CONFIG_IXGBE_PTP
+	case SIOCSHWTSTAMP:
+		return ixgbe_ptp_hwtstamp_ioctl(adapter, req, cmd);
+#endif
+	default:
+		return mdio_mii_ioctl(&adapter->hw.phy.mdio, if_mii(req), cmd);
+	}
 }
 
 /**
@@ -6565,15 +6658,17 @@ int ixgbe_setup_tc(struct net_device *dev, u8 tc)
 
 	if (tc) {
 		netdev_set_num_tc(dev, tc);
-		adapter->last_lfc_mode = adapter->hw.fc.current_mode;
 		adapter->flags |= IXGBE_FLAG_DCB_ENABLED;
 		adapter->flags &= ~IXGBE_FLAG_FDIR_HASH_CAPABLE;
 
-		if (adapter->hw.mac.type == ixgbe_mac_82598EB)
+		if (adapter->hw.mac.type == ixgbe_mac_82598EB) {
+			adapter->last_lfc_mode = adapter->hw.fc.requested_mode;
 			adapter->hw.fc.requested_mode = ixgbe_fc_none;
+		}
 	} else {
 		netdev_reset_tc(dev);
-		adapter->hw.fc.requested_mode = adapter->last_lfc_mode;
+		if (adapter->hw.mac.type == ixgbe_mac_82598EB)
+			adapter->hw.fc.requested_mode = adapter->last_lfc_mode;
 
 		adapter->flags &= ~IXGBE_FLAG_DCB_ENABLED;
 		adapter->flags |= IXGBE_FLAG_FDIR_HASH_CAPABLE;
@@ -6622,7 +6717,7 @@ static netdev_features_t ixgbe_fix_features(struct net_device *netdev,
 	/* Turn off LRO if not RSC capable */
 	if (!(adapter->flags2 & IXGBE_FLAG2_RSC_CAPABLE))
 		features &= ~NETIF_F_LRO;
-	
+
 
 	return features;
 }
@@ -6681,6 +6776,74 @@ static int ixgbe_set_features(struct net_device *netdev,
 	return 0;
 }
 
+static int ixgbe_ndo_fdb_add(struct ndmsg *ndm,
+			     struct net_device *dev,
+			     unsigned char *addr,
+			     u16 flags)
+{
+	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_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;
+}
+
+static int ixgbe_ndo_fdb_del(struct ndmsg *ndm,
+			     struct net_device *dev,
+			     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 const struct net_device_ops ixgbe_netdev_ops = {
 	.ndo_open		= ixgbe_open,
 	.ndo_stop		= ixgbe_close,
@@ -6717,6 +6880,9 @@ static const struct net_device_ops ixgbe_netdev_ops = {
 #endif /* IXGBE_FCOE */
 	.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,
 };
 
 static void __devinit ixgbe_probe_vf(struct ixgbe_adapter *adapter,
@@ -6731,14 +6897,66 @@ static void __devinit ixgbe_probe_vf(struct ixgbe_adapter *adapter,
 	/* The 82599 supports up to 64 VFs per physical function
 	 * but this implementation limits allocation to 63 so that
 	 * basic networking resources are still available to the
-	 * physical function
+	 * physical function.  If the user requests greater thn
+	 * 63 VFs then it is an error - reset to default of zero.
 	 */
-	adapter->num_vfs = (max_vfs > 63) ? 63 : max_vfs;
+	adapter->num_vfs = (max_vfs > 63) ? 0 : max_vfs;
 	ixgbe_enable_sriov(adapter, ii);
 #endif /* CONFIG_PCI_IOV */
 }
 
 /**
+ * ixgbe_wol_supported - Check whether device supports WoL
+ * @hw: hw specific details
+ * @device_id: the device ID
+ * @subdev_id: the subsystem device ID
+ *
+ * This function is used by probe and ethtool to determine
+ * which devices have WoL support
+ *
+ **/
+int ixgbe_wol_supported(struct ixgbe_adapter *adapter, u16 device_id,
+			u16 subdevice_id)
+{
+	struct ixgbe_hw *hw = &adapter->hw;
+	u16 wol_cap = adapter->eeprom_cap & IXGBE_DEVICE_CAPS_WOL_MASK;
+	int is_wol_supported = 0;
+
+	switch (device_id) {
+	case IXGBE_DEV_ID_82599_SFP:
+		/* Only these subdevices could supports WOL */
+		switch (subdevice_id) {
+		case IXGBE_SUBDEV_ID_82599_560FLR:
+			/* only support first port */
+			if (hw->bus.func != 0)
+				break;
+		case IXGBE_SUBDEV_ID_82599_SFP:
+			is_wol_supported = 1;
+			break;
+		}
+		break;
+	case IXGBE_DEV_ID_82599_COMBO_BACKPLANE:
+		/* All except this subdevice support WOL */
+		if (subdevice_id != IXGBE_SUBDEV_ID_82599_KX4_KR_MEZZ)
+			is_wol_supported = 1;
+		break;
+	case IXGBE_DEV_ID_82599_KX4:
+		is_wol_supported = 1;
+		break;
+	case IXGBE_DEV_ID_X540T:
+		/* check eeprom to see if enabled wol */
+		if ((wol_cap == IXGBE_DEVICE_CAPS_WOL_PORT0_1) ||
+		    ((wol_cap == IXGBE_DEVICE_CAPS_WOL_PORT0) &&
+		     (hw->bus.func == 0))) {
+			is_wol_supported = 1;
+		}
+		break;
+	}
+
+	return is_wol_supported;
+}
+
+/**
  * ixgbe_probe - Device Initialization Routine
  * @pdev: PCI device information struct
  * @ent: entry in ixgbe_pci_tbl
@@ -6764,7 +6982,6 @@ static int __devinit ixgbe_probe(struct pci_dev *pdev,
 	u16 device_caps;
 #endif
 	u32 eec;
-	u16 wol_cap;
 
 	/* Catch broken hardware that put the wrong VF device ID in
 	 * the PCIe SR-IOV capability.
@@ -7028,42 +7245,18 @@ static int __devinit ixgbe_probe(struct pci_dev *pdev,
 		netdev->features &= ~NETIF_F_RXHASH;
 	}
 
-	/* WOL not supported for all but the following */
+	/* WOL not supported for all devices */
 	adapter->wol = 0;
-	switch (pdev->device) {
-	case IXGBE_DEV_ID_82599_SFP:
-		/* Only these subdevice supports WOL */
-		switch (pdev->subsystem_device) {
-		case IXGBE_SUBDEV_ID_82599_560FLR:
-			/* only support first port */
-			if (hw->bus.func != 0)
-				break;
-		case IXGBE_SUBDEV_ID_82599_SFP:
-			adapter->wol = IXGBE_WUFC_MAG;
-			break;
-		}
-		break;
-	case IXGBE_DEV_ID_82599_COMBO_BACKPLANE:
-		/* All except this subdevice support WOL */
-		if (pdev->subsystem_device != IXGBE_SUBDEV_ID_82599_KX4_KR_MEZZ)
-			adapter->wol = IXGBE_WUFC_MAG;
-		break;
-	case IXGBE_DEV_ID_82599_KX4:
+	hw->eeprom.ops.read(hw, 0x2c, &adapter->eeprom_cap);
+	if (ixgbe_wol_supported(adapter, pdev->device, pdev->subsystem_device))
 		adapter->wol = IXGBE_WUFC_MAG;
-		break;
-	case IXGBE_DEV_ID_X540T:
-		/* Check eeprom to see if it is enabled */
-		hw->eeprom.ops.read(hw, 0x2c, &adapter->eeprom_cap);
-		wol_cap = adapter->eeprom_cap & IXGBE_DEVICE_CAPS_WOL_MASK;
 
-		if ((wol_cap == IXGBE_DEVICE_CAPS_WOL_PORT0_1) ||
-		    ((wol_cap == IXGBE_DEVICE_CAPS_WOL_PORT0) &&
-		     (hw->bus.func == 0)))
-			adapter->wol = IXGBE_WUFC_MAG;
-		break;
-	}
 	device_set_wakeup_enable(&adapter->pdev->dev, adapter->wol);
 
+#ifdef CONFIG_IXGBE_PTP
+	ixgbe_ptp_init(adapter);
+#endif /* CONFIG_IXGBE_PTP*/
+
 	/* save off EEPROM version number */
 	hw->eeprom.ops.read(hw, 0x2e, &adapter->eeprom_verh);
 	hw->eeprom.ops.read(hw, 0x2d, &adapter->eeprom_verl);
@@ -7150,6 +7343,12 @@ static int __devinit ixgbe_probe(struct pci_dev *pdev,
 
 	e_dev_info("%s\n", ixgbe_default_device_descr);
 	cards_found++;
+
+#ifdef CONFIG_IXGBE_HWMON
+	if (ixgbe_sysfs_init(adapter))
+		e_err(probe, "failed to allocate sysfs resources\n");
+#endif /* CONFIG_IXGBE_HWMON */
+
 	return 0;
 
 err_register:
@@ -7188,6 +7387,10 @@ static void __devexit ixgbe_remove(struct pci_dev *pdev)
 	set_bit(__IXGBE_DOWN, &adapter->state);
 	cancel_work_sync(&adapter->service_task);
 
+#ifdef CONFIG_IXGBE_PTP
+	ixgbe_ptp_stop(adapter);
+#endif
+
 #ifdef CONFIG_IXGBE_DCA
 	if (adapter->flags & IXGBE_FLAG_DCA_ENABLED) {
 		adapter->flags &= ~IXGBE_FLAG_DCA_ENABLED;
@@ -7196,6 +7399,10 @@ static void __devexit ixgbe_remove(struct pci_dev *pdev)
 	}
 
 #endif
+#ifdef CONFIG_IXGBE_HWMON
+	ixgbe_sysfs_exit(adapter);
+#endif /* CONFIG_IXGBE_HWMON */
+
 #ifdef IXGBE_FCOE
 	if (adapter->flags & IXGBE_FLAG_FCOE_ENABLED)
 		ixgbe_cleanup_fcoe(adapter);
diff --git a/drivers/net/ethernet/intel/ixgbe/ixgbe_phy.c b/drivers/net/ethernet/intel/ixgbe/ixgbe_phy.c
index bf9f82f4b1a..24117709d6a 100644
--- a/drivers/net/ethernet/intel/ixgbe/ixgbe_phy.c
+++ b/drivers/net/ethernet/intel/ixgbe/ixgbe_phy.c
@@ -1582,13 +1582,21 @@ static s32 ixgbe_clock_out_i2c_bit(struct ixgbe_hw *hw, bool data)
  **/
 static void ixgbe_raise_i2c_clk(struct ixgbe_hw *hw, u32 *i2cctl)
 {
-	*i2cctl |= IXGBE_I2C_CLK_OUT;
-
-	IXGBE_WRITE_REG(hw, IXGBE_I2CCTL, *i2cctl);
-	IXGBE_WRITE_FLUSH(hw);
+	u32 i = 0;
+	u32 timeout = IXGBE_I2C_CLOCK_STRETCHING_TIMEOUT;
+	u32 i2cctl_r = 0;
 
-	/* SCL rise time (1000ns) */
-	udelay(IXGBE_I2C_T_RISE);
+	for (i = 0; i < timeout; i++) {
+		*i2cctl |= IXGBE_I2C_CLK_OUT;
+		IXGBE_WRITE_REG(hw, IXGBE_I2CCTL, *i2cctl);
+		IXGBE_WRITE_FLUSH(hw);
+		/* SCL rise time (1000ns) */
+		udelay(IXGBE_I2C_T_RISE);
+
+		i2cctl_r = IXGBE_READ_REG(hw, IXGBE_I2CCTL);
+		if (i2cctl_r & IXGBE_I2C_CLK_IN)
+			break;
+	}
 }
 
 /**
diff --git a/drivers/net/ethernet/intel/ixgbe/ixgbe_ptp.c b/drivers/net/ethernet/intel/ixgbe/ixgbe_ptp.c
new file mode 100644
index 00000000000..ddc6a4d1930
--- /dev/null
+++ b/drivers/net/ethernet/intel/ixgbe/ixgbe_ptp.c
@@ -0,0 +1,900 @@
+/*******************************************************************************
+
+  Intel 10 Gigabit PCI Express Linux driver
+  Copyright(c) 1999 - 2012 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.
+
+  The full GNU General Public License is included in this distribution in
+  the file called "COPYING".
+
+  Contact Information:
+  e1000-devel Mailing List <e1000-devel@lists.sourceforge.net>
+  Intel Corporation, 5200 N.E. Elam Young Parkway, Hillsboro, OR 97124-6497
+
+*******************************************************************************/
+#include "ixgbe.h"
+#include <linux/export.h>
+
+/*
+ * The 82599 and the X540 do not have true 64bit nanosecond scale
+ * counter registers. Instead, SYSTIME is defined by a fixed point
+ * system which allows the user to define the scale counter increment
+ * value at every level change of the oscillator driving the SYSTIME
+ * value. For both devices the TIMINCA:IV field defines this
+ * increment. On the X540 device, 31 bits are provided. However on the
+ * 82599 only provides 24 bits. The time unit is determined by the
+ * clock frequency of the oscillator in combination with the TIMINCA
+ * register. When these devices link at 10Gb the oscillator has a
+ * period of 6.4ns. In order to convert the scale counter into
+ * nanoseconds the cyclecounter and timecounter structures are
+ * used. The SYSTIME registers need to be converted to ns values by use
+ * of only a right shift (division by power of 2). The following math
+ * determines the largest incvalue that will fit into the available
+ * bits in the TIMINCA register.
+ *
+ * PeriodWidth: Number of bits to store the clock period
+ * MaxWidth: The maximum width value of the TIMINCA register
+ * Period: The clock period for the oscillator
+ * round(): discard the fractional portion of the calculation
+ *
+ * Period * [ 2 ^ ( MaxWidth - PeriodWidth ) ]
+ *
+ * For the X540, MaxWidth is 31 bits, and the base period is 6.4 ns
+ * For the 82599, MaxWidth is 24 bits, and the base period is 6.4 ns
+ *
+ * The period also changes based on the link speed:
+ * At 10Gb link or no link, the period remains the same.
+ * At 1Gb link, the period is multiplied by 10. (64ns)
+ * At 100Mb link, the period is multiplied by 100. (640ns)
+ *
+ * The calculated value allows us to right shift the SYSTIME register
+ * value in order to quickly convert it into a nanosecond clock,
+ * while allowing for the maximum possible adjustment value.
+ *
+ * These diagrams are only for the 10Gb link period
+ *
+ *           SYSTIMEH            SYSTIMEL
+ *       +--------------+  +--------------+
+ * X540  |      32      |  | 1 | 3 |  28  |
+ *       *--------------+  +--------------+
+ *        \________ 36 bits ______/  fract
+ *
+ *       +--------------+  +--------------+
+ * 82599 |      32      |  | 8 | 3 |  21  |
+ *       *--------------+  +--------------+
+ *        \________ 43 bits ______/  fract
+ *
+ * The 36 bit X540 SYSTIME overflows every
+ *   2^36 * 10^-9 / 60 = 1.14 minutes or 69 seconds
+ *
+ * The 43 bit 82599 SYSTIME overflows every
+ *   2^43 * 10^-9 / 3600 = 2.4 hours
+ */
+#define IXGBE_INCVAL_10GB 0x66666666
+#define IXGBE_INCVAL_1GB  0x40000000
+#define IXGBE_INCVAL_100  0x50000000
+
+#define IXGBE_INCVAL_SHIFT_10GB  28
+#define IXGBE_INCVAL_SHIFT_1GB   24
+#define IXGBE_INCVAL_SHIFT_100   21
+
+#define IXGBE_INCVAL_SHIFT_82599 7
+#define IXGBE_INCPER_SHIFT_82599 24
+#define IXGBE_MAX_TIMEADJ_VALUE  0x7FFFFFFFFFFFFFFFULL
+
+#define IXGBE_OVERFLOW_PERIOD    (HZ * 30)
+
+#ifndef NSECS_PER_SEC
+#define NSECS_PER_SEC 1000000000ULL
+#endif
+
+/**
+ * ixgbe_ptp_read - read raw cycle counter (to be used by time counter)
+ * @cc - the cyclecounter structure
+ *
+ * this function reads the cyclecounter registers and is called by the
+ * cyclecounter structure used to construct a ns counter from the
+ * arbitrary fixed point registers
+ */
+static cycle_t ixgbe_ptp_read(const struct cyclecounter *cc)
+{
+	struct ixgbe_adapter *adapter =
+		container_of(cc, struct ixgbe_adapter, cc);
+	struct ixgbe_hw *hw = &adapter->hw;
+	u64 stamp = 0;
+
+	stamp |= (u64)IXGBE_READ_REG(hw, IXGBE_SYSTIML);
+	stamp |= (u64)IXGBE_READ_REG(hw, IXGBE_SYSTIMH) << 32;
+
+	return stamp;
+}
+
+/**
+ * ixgbe_ptp_adjfreq
+ * @ptp - the ptp clock structure
+ * @ppb - parts per billion adjustment from base
+ *
+ * adjust the frequency of the ptp cycle counter by the
+ * indicated ppb from the base frequency.
+ */
+static int ixgbe_ptp_adjfreq(struct ptp_clock_info *ptp, s32 ppb)
+{
+	struct ixgbe_adapter *adapter =
+		container_of(ptp, struct ixgbe_adapter, ptp_caps);
+	struct ixgbe_hw *hw = &adapter->hw;
+	u64 freq;
+	u32 diff, incval;
+	int neg_adj = 0;
+
+	if (ppb < 0) {
+		neg_adj = 1;
+		ppb = -ppb;
+	}
+
+	smp_mb();
+	incval = ACCESS_ONCE(adapter->base_incval);
+
+	freq = incval;
+	freq *= ppb;
+	diff = div_u64(freq, 1000000000ULL);
+
+	incval = neg_adj ? (incval - diff) : (incval + diff);
+
+	switch (hw->mac.type) {
+	case ixgbe_mac_X540:
+		IXGBE_WRITE_REG(hw, IXGBE_TIMINCA, incval);
+		break;
+	case ixgbe_mac_82599EB:
+		IXGBE_WRITE_REG(hw, IXGBE_TIMINCA,
+				(1 << IXGBE_INCPER_SHIFT_82599) |
+				incval);
+		break;
+	default:
+		break;
+	}
+
+	return 0;
+}
+
+/**
+ * ixgbe_ptp_adjtime
+ * @ptp - the ptp clock structure
+ * @delta - offset to adjust the cycle counter by
+ *
+ * adjust the timer by resetting the timecounter structure.
+ */
+static int ixgbe_ptp_adjtime(struct ptp_clock_info *ptp, s64 delta)
+{
+	struct ixgbe_adapter *adapter =
+		container_of(ptp, struct ixgbe_adapter, ptp_caps);
+	unsigned long flags;
+	u64 now;
+
+	spin_lock_irqsave(&adapter->tmreg_lock, flags);
+
+	now = timecounter_read(&adapter->tc);
+	now += delta;
+
+	/* reset the timecounter */
+	timecounter_init(&adapter->tc,
+			 &adapter->cc,
+			 now);
+
+	spin_unlock_irqrestore(&adapter->tmreg_lock, flags);
+	return 0;
+}
+
+/**
+ * ixgbe_ptp_gettime
+ * @ptp - the ptp clock structure
+ * @ts - timespec structure to hold the current time value
+ *
+ * read the timecounter and return the correct value on ns,
+ * after converting it into a struct timespec.
+ */
+static int ixgbe_ptp_gettime(struct ptp_clock_info *ptp, struct timespec *ts)
+{
+	struct ixgbe_adapter *adapter =
+		container_of(ptp, struct ixgbe_adapter, ptp_caps);
+	u64 ns;
+	u32 remainder;
+	unsigned long flags;
+
+	spin_lock_irqsave(&adapter->tmreg_lock, flags);
+	ns = timecounter_read(&adapter->tc);
+	spin_unlock_irqrestore(&adapter->tmreg_lock, flags);
+
+	ts->tv_sec = div_u64_rem(ns, 1000000000ULL, &remainder);
+	ts->tv_nsec = remainder;
+
+	return 0;
+}
+
+/**
+ * ixgbe_ptp_settime
+ * @ptp - the ptp clock structure
+ * @ts - the timespec containing the new time for the cycle counter
+ *
+ * reset the timecounter to use a new base value instead of the kernel
+ * wall timer value.
+ */
+static int ixgbe_ptp_settime(struct ptp_clock_info *ptp,
+			     const struct timespec *ts)
+{
+	struct ixgbe_adapter *adapter =
+		container_of(ptp, struct ixgbe_adapter, ptp_caps);
+	u64 ns;
+	unsigned long flags;
+
+	ns = ts->tv_sec * 1000000000ULL;
+	ns += ts->tv_nsec;
+
+	/* reset the timecounter */
+	spin_lock_irqsave(&adapter->tmreg_lock, flags);
+	timecounter_init(&adapter->tc, &adapter->cc, ns);
+	spin_unlock_irqrestore(&adapter->tmreg_lock, flags);
+
+	return 0;
+}
+
+/**
+ * ixgbe_ptp_enable
+ * @ptp - the ptp clock structure
+ * @rq - the requested feature to change
+ * @on - whether to enable or disable the feature
+ *
+ * enable (or disable) ancillary features of the phc subsystem.
+ * our driver only supports the PPS feature on the X540
+ */
+static int ixgbe_ptp_enable(struct ptp_clock_info *ptp,
+			    struct ptp_clock_request *rq, int on)
+{
+	struct ixgbe_adapter *adapter =
+		container_of(ptp, struct ixgbe_adapter, ptp_caps);
+
+	/**
+	 * When PPS is enabled, unmask the interrupt for the ClockOut
+	 * feature, so that the interrupt handler can send the PPS
+	 * event when the clock SDP triggers. Clear mask when PPS is
+	 * disabled
+	 */
+	if (rq->type == PTP_CLK_REQ_PPS) {
+		switch (adapter->hw.mac.type) {
+		case ixgbe_mac_X540:
+			if (on)
+				adapter->flags2 |= IXGBE_FLAG2_PTP_PPS_ENABLED;
+			else
+				adapter->flags2 &=
+					~IXGBE_FLAG2_PTP_PPS_ENABLED;
+			return 0;
+		default:
+			break;
+		}
+	}
+
+	return -ENOTSUPP;
+}
+
+/**
+ * ixgbe_ptp_check_pps_event
+ * @adapter - the private adapter structure
+ * @eicr - the interrupt cause register value
+ *
+ * This function is called by the interrupt routine when checking for
+ * interrupts. It will check and handle a pps event.
+ */
+void ixgbe_ptp_check_pps_event(struct ixgbe_adapter *adapter, u32 eicr)
+{
+	struct ixgbe_hw *hw = &adapter->hw;
+	struct ptp_clock_event event;
+
+	event.type = PTP_CLOCK_PPS;
+
+	/* Make sure ptp clock is valid, and PPS event enabled */
+	if (!adapter->ptp_clock ||
+	    !(adapter->flags2 & IXGBE_FLAG2_PTP_PPS_ENABLED))
+		return;
+
+	switch (hw->mac.type) {
+	case ixgbe_mac_X540:
+		if (eicr & IXGBE_EICR_TIMESYNC)
+			ptp_clock_event(adapter->ptp_clock, &event);
+		break;
+	default:
+		break;
+	}
+}
+
+/**
+ * ixgbe_ptp_enable_sdp
+ * @hw - the hardware private structure
+ * @shift - the clock shift for calculating nanoseconds
+ *
+ * this function enables the clock out feature on the sdp0 for the
+ * X540 device. It will create a 1second periodic output that can be
+ * used as the PPS (via an interrupt).
+ *
+ * It calculates when the systime will be on an exact second, and then
+ * aligns the start of the PPS signal to that value. The shift is
+ * necessary because it can change based on the link speed.
+ */
+static void ixgbe_ptp_enable_sdp(struct ixgbe_hw *hw, int shift)
+{
+	u32 esdp, tsauxc, clktiml, clktimh, trgttiml, trgttimh;
+	u64 clock_edge = 0;
+	u32 rem;
+
+	switch (hw->mac.type) {
+	case ixgbe_mac_X540:
+		esdp = IXGBE_READ_REG(hw, IXGBE_ESDP);
+
+		/*
+		 * enable the SDP0 pin as output, and connected to the native
+		 * function for Timesync (ClockOut)
+		 */
+		esdp |= (IXGBE_ESDP_SDP0_DIR |
+			 IXGBE_ESDP_SDP0_NATIVE);
+
+		/*
+		 * enable the Clock Out feature on SDP0, and allow interrupts
+		 * to occur when the pin changes
+		 */
+		tsauxc = (IXGBE_TSAUXC_EN_CLK |
+			  IXGBE_TSAUXC_SYNCLK |
+			  IXGBE_TSAUXC_SDP0_INT);
+
+		/* clock period (or pulse length) */
+		clktiml = (u32)(NSECS_PER_SEC << shift);
+		clktimh = (u32)((NSECS_PER_SEC << shift) >> 32);
+
+		clock_edge |= (u64)IXGBE_READ_REG(hw, IXGBE_SYSTIML);
+		clock_edge |= (u64)IXGBE_READ_REG(hw, IXGBE_SYSTIMH) << 32;
+
+		/*
+		 * account for the fact that we can't do u64 division
+		 * with remainder, by converting the clock values into
+		 * nanoseconds first
+		 */
+		clock_edge >>= shift;
+		div_u64_rem(clock_edge, NSECS_PER_SEC, &rem);
+		clock_edge += (NSECS_PER_SEC - rem);
+		clock_edge <<= shift;
+
+		/* specify the initial clock start time */
+		trgttiml = (u32)clock_edge;
+		trgttimh = (u32)(clock_edge >> 32);
+
+		IXGBE_WRITE_REG(hw, IXGBE_CLKTIML, clktiml);
+		IXGBE_WRITE_REG(hw, IXGBE_CLKTIMH, clktimh);
+		IXGBE_WRITE_REG(hw, IXGBE_TRGTTIML0, trgttiml);
+		IXGBE_WRITE_REG(hw, IXGBE_TRGTTIMH0, trgttimh);
+
+		IXGBE_WRITE_REG(hw, IXGBE_ESDP, esdp);
+		IXGBE_WRITE_REG(hw, IXGBE_TSAUXC, tsauxc);
+
+		IXGBE_WRITE_REG(hw, IXGBE_EIMS, IXGBE_EICR_TIMESYNC);
+		break;
+	default:
+		break;
+	}
+}
+
+/**
+ * ixgbe_ptp_disable_sdp
+ * @hw - the private hardware structure
+ *
+ * this function disables the auxiliary SDP clock out feature
+ */
+static void ixgbe_ptp_disable_sdp(struct ixgbe_hw *hw)
+{
+	IXGBE_WRITE_REG(hw, IXGBE_EIMC, IXGBE_EICR_TIMESYNC);
+	IXGBE_WRITE_REG(hw, IXGBE_TSAUXC, 0);
+}
+
+/**
+ * ixgbe_ptp_overflow_check - delayed work to detect SYSTIME overflow
+ * @work: structure containing information about this work task
+ *
+ * this work function is scheduled to continue reading the timecounter
+ * in order to prevent missing when the system time registers wrap
+ * around. This needs to be run approximately twice a minute when no
+ * PTP activity is occurring.
+ */
+void ixgbe_ptp_overflow_check(struct ixgbe_adapter *adapter)
+{
+	unsigned long elapsed_jiffies = adapter->last_overflow_check - jiffies;
+	struct timespec ts;
+
+	if ((adapter->flags2 & IXGBE_FLAG2_OVERFLOW_CHECK_ENABLED) &&
+	    (elapsed_jiffies >= IXGBE_OVERFLOW_PERIOD)) {
+		ixgbe_ptp_gettime(&adapter->ptp_caps, &ts);
+		adapter->last_overflow_check = jiffies;
+	}
+}
+
+/**
+ * ixgbe_ptp_tx_hwtstamp - utility function which checks for TX time stamp
+ * @q_vector: structure containing interrupt and ring information
+ * @skb: particular skb to send timestamp with
+ *
+ * if the timestamp is valid, we convert it into the timecounter ns
+ * value, then store that result into the shhwtstamps structure which
+ * is passed up the network stack
+ */
+void ixgbe_ptp_tx_hwtstamp(struct ixgbe_q_vector *q_vector,
+			   struct sk_buff *skb)
+{
+	struct ixgbe_adapter *adapter;
+	struct ixgbe_hw *hw;
+	struct skb_shared_hwtstamps shhwtstamps;
+	u64 regval = 0, ns;
+	u32 tsynctxctl;
+	unsigned long flags;
+
+	/* we cannot process timestamps on a ring without a q_vector */
+	if (!q_vector || !q_vector->adapter)
+		return;
+
+	adapter = q_vector->adapter;
+	hw = &adapter->hw;
+
+	tsynctxctl = IXGBE_READ_REG(hw, IXGBE_TSYNCTXCTL);
+	regval |= (u64)IXGBE_READ_REG(hw, IXGBE_TXSTMPL);
+	regval |= (u64)IXGBE_READ_REG(hw, IXGBE_TXSTMPH) << 32;
+
+	/*
+	 * if TX timestamp is not valid, exit after clearing the
+	 * timestamp registers
+	 */
+	if (!(tsynctxctl & IXGBE_TSYNCTXCTL_VALID))
+		return;
+
+	spin_lock_irqsave(&adapter->tmreg_lock, flags);
+	ns = timecounter_cyc2time(&adapter->tc, regval);
+	spin_unlock_irqrestore(&adapter->tmreg_lock, flags);
+
+	memset(&shhwtstamps, 0, sizeof(shhwtstamps));
+	shhwtstamps.hwtstamp = ns_to_ktime(ns);
+	skb_tstamp_tx(skb, &shhwtstamps);
+}
+
+/**
+ * ixgbe_ptp_rx_hwtstamp - utility function which checks for RX time stamp
+ * @q_vector: structure containing interrupt and ring information
+ * @skb: particular skb to send timestamp with
+ *
+ * if the timestamp is valid, we convert it into the timecounter ns
+ * value, then store that result into the shhwtstamps structure which
+ * is passed up the network stack
+ */
+void ixgbe_ptp_rx_hwtstamp(struct ixgbe_q_vector *q_vector,
+			   struct sk_buff *skb)
+{
+	struct ixgbe_adapter *adapter;
+	struct ixgbe_hw *hw;
+	struct skb_shared_hwtstamps *shhwtstamps;
+	u64 regval = 0, ns;
+	u32 tsyncrxctl;
+	unsigned long flags;
+
+	/* we cannot process timestamps on a ring without a q_vector */
+	if (!q_vector || !q_vector->adapter)
+		return;
+
+	adapter = q_vector->adapter;
+	hw = &adapter->hw;
+
+	tsyncrxctl = IXGBE_READ_REG(hw, IXGBE_TSYNCRXCTL);
+	regval |= (u64)IXGBE_READ_REG(hw, IXGBE_RXSTMPL);
+	regval |= (u64)IXGBE_READ_REG(hw, IXGBE_RXSTMPH) << 32;
+
+	/*
+	 * 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
+	 * values must belong to this one here and therefore we don't need to
+	 * compare any of the additional attributes stored for it.
+	 *
+	 * If nothing went wrong, then it should have a skb_shared_tx that we
+	 * can turn into a skb_shared_hwtstamps.
+	 */
+	if (!(tsyncrxctl & IXGBE_TSYNCRXCTL_VALID))
+		return;
+
+	spin_lock_irqsave(&adapter->tmreg_lock, flags);
+	ns = timecounter_cyc2time(&adapter->tc, regval);
+	spin_unlock_irqrestore(&adapter->tmreg_lock, flags);
+
+	shhwtstamps = skb_hwtstamps(skb);
+	shhwtstamps->hwtstamp = ns_to_ktime(ns);
+}
+
+/**
+ * ixgbe_ptp_hwtstamp_ioctl - control hardware time stamping
+ * @adapter: pointer to adapter struct
+ * @ifreq: ioctl data
+ * @cmd: particular ioctl requested
+ *
+ * Outgoing time stamping can be enabled and disabled. Play nice and
+ * disable it when requested, although it shouldn't case any overhead
+ * when no packet needs it. At most one packet in the queue may be
+ * marked for time stamping, otherwise it would be impossible to tell
+ * for sure to which packet the hardware time stamp belongs.
+ *
+ * Incoming time stamping has to be configured via the hardware
+ * filters. Not all combinations are supported, in particular event
+ * 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 ixgbe_ptp_hwtstamp_ioctl(struct ixgbe_adapter *adapter,
+			     struct ifreq *ifr, int cmd)
+{
+	struct ixgbe_hw *hw = &adapter->hw;
+	struct hwtstamp_config config;
+	u32 tsync_tx_ctl = IXGBE_TSYNCTXCTL_ENABLED;
+	u32 tsync_rx_ctl = IXGBE_TSYNCRXCTL_ENABLED;
+	u32 tsync_rx_mtrl = 0;
+	bool is_l4 = false;
+	bool is_l2 = false;
+	u32 regval;
+
+	if (copy_from_user(&config, ifr->ifr_data, sizeof(config)))
+		return -EFAULT;
+
+	/* reserved for future extensions */
+	if (config.flags)
+		return -EINVAL;
+
+	switch (config.tx_type) {
+	case HWTSTAMP_TX_OFF:
+		tsync_tx_ctl = 0;
+	case HWTSTAMP_TX_ON:
+		break;
+	default:
+		return -ERANGE;
+	}
+
+	switch (config.rx_filter) {
+	case HWTSTAMP_FILTER_NONE:
+		tsync_rx_ctl = 0;
+		break;
+	case HWTSTAMP_FILTER_PTP_V1_L4_SYNC:
+		tsync_rx_ctl |= IXGBE_TSYNCRXCTL_TYPE_L4_V1;
+		tsync_rx_mtrl = IXGBE_RXMTRL_V1_SYNC_MSG;
+		is_l4 = true;
+		break;
+	case HWTSTAMP_FILTER_PTP_V1_L4_DELAY_REQ:
+		tsync_rx_ctl |= IXGBE_TSYNCRXCTL_TYPE_L4_V1;
+		tsync_rx_mtrl = IXGBE_RXMTRL_V1_DELAY_REQ_MSG;
+		is_l4 = true;
+		break;
+	case HWTSTAMP_FILTER_PTP_V2_SYNC:
+	case HWTSTAMP_FILTER_PTP_V2_L2_SYNC:
+	case HWTSTAMP_FILTER_PTP_V2_L4_SYNC:
+		tsync_rx_ctl |= IXGBE_TSYNCRXCTL_TYPE_L2_L4_V2;
+		tsync_rx_mtrl = IXGBE_RXMTRL_V2_SYNC_MSG;
+		is_l2 = true;
+		is_l4 = true;
+		config.rx_filter = HWTSTAMP_FILTER_SOME;
+		break;
+	case HWTSTAMP_FILTER_PTP_V2_DELAY_REQ:
+	case HWTSTAMP_FILTER_PTP_V2_L2_DELAY_REQ:
+	case HWTSTAMP_FILTER_PTP_V2_L4_DELAY_REQ:
+		tsync_rx_ctl |= IXGBE_TSYNCRXCTL_TYPE_L2_L4_V2;
+		tsync_rx_mtrl = IXGBE_RXMTRL_V2_DELAY_REQ_MSG;
+		is_l2 = true;
+		is_l4 = true;
+		config.rx_filter = HWTSTAMP_FILTER_SOME;
+		break;
+	case HWTSTAMP_FILTER_PTP_V2_L4_EVENT:
+	case HWTSTAMP_FILTER_PTP_V2_L2_EVENT:
+	case HWTSTAMP_FILTER_PTP_V2_EVENT:
+		tsync_rx_ctl |= IXGBE_TSYNCRXCTL_TYPE_EVENT_V2;
+		config.rx_filter = HWTSTAMP_FILTER_PTP_V2_EVENT;
+		is_l2 = true;
+		is_l4 = true;
+		break;
+	case HWTSTAMP_FILTER_PTP_V1_L4_EVENT:
+	case HWTSTAMP_FILTER_ALL:
+	default:
+		/*
+		 * register RXMTRL must be set, therefore it is not
+		 * possible to time stamp both V1 Sync and Delay_Req messages
+		 * and hardware does not support timestamping all packets
+		 * => return error
+		 */
+		return -ERANGE;
+	}
+
+	if (hw->mac.type == ixgbe_mac_82598EB) {
+		if (tsync_rx_ctl | tsync_tx_ctl)
+			return -ERANGE;
+		return 0;
+	}
+
+	/* define ethertype filter for timestamped packets */
+	if (is_l2)
+		IXGBE_WRITE_REG(hw, IXGBE_ETQF(3),
+				(IXGBE_ETQF_FILTER_EN | /* enable filter */
+				 IXGBE_ETQF_1588 | /* enable timestamping */
+				 ETH_P_1588));     /* 1588 eth protocol type */
+	else
+		IXGBE_WRITE_REG(hw, IXGBE_ETQF(3), 0);
+
+#define PTP_PORT 319
+	/* L4 Queue Filter[3]: filter by destination port and protocol */
+	if (is_l4) {
+		u32 ftqf = (IXGBE_FTQF_PROTOCOL_UDP /* UDP */
+			    | IXGBE_FTQF_POOL_MASK_EN /* Pool not compared */
+			    | IXGBE_FTQF_QUEUE_ENABLE);
+
+		ftqf |= ((IXGBE_FTQF_PROTOCOL_COMP_MASK /* protocol check */
+			  & IXGBE_FTQF_DEST_PORT_MASK /* dest check */
+			  & IXGBE_FTQF_SOURCE_PORT_MASK) /* source check */
+			 << IXGBE_FTQF_5TUPLE_MASK_SHIFT);
+
+		IXGBE_WRITE_REG(hw, IXGBE_L34T_IMIR(3),
+				(3 << IXGBE_IMIR_RX_QUEUE_SHIFT_82599 |
+				 IXGBE_IMIR_SIZE_BP_82599));
+
+		/* enable port check */
+		IXGBE_WRITE_REG(hw, IXGBE_SDPQF(3),
+				(htons(PTP_PORT) |
+				 htons(PTP_PORT) << 16));
+
+		IXGBE_WRITE_REG(hw, IXGBE_FTQF(3), ftqf);
+
+		tsync_rx_mtrl |= PTP_PORT << 16;
+	} else {
+		IXGBE_WRITE_REG(hw, IXGBE_FTQF(3), 0);
+	}
+
+	/* enable/disable TX */
+	regval = IXGBE_READ_REG(hw, IXGBE_TSYNCTXCTL);
+	regval &= ~IXGBE_TSYNCTXCTL_ENABLED;
+	regval |= tsync_tx_ctl;
+	IXGBE_WRITE_REG(hw, IXGBE_TSYNCTXCTL, regval);
+
+	/* enable/disable RX */
+	regval = IXGBE_READ_REG(hw, IXGBE_TSYNCRXCTL);
+	regval &= ~(IXGBE_TSYNCRXCTL_ENABLED | IXGBE_TSYNCRXCTL_TYPE_MASK);
+	regval |= tsync_rx_ctl;
+	IXGBE_WRITE_REG(hw, IXGBE_TSYNCRXCTL, regval);
+
+	/* define which PTP packets are time stamped */
+	IXGBE_WRITE_REG(hw, IXGBE_RXMTRL, tsync_rx_mtrl);
+
+	IXGBE_WRITE_FLUSH(hw);
+
+	/* clear TX/RX time stamp registers, just to be sure */
+	regval = IXGBE_READ_REG(hw, IXGBE_TXSTMPH);
+	regval = IXGBE_READ_REG(hw, IXGBE_RXSTMPH);
+
+	return copy_to_user(ifr->ifr_data, &config, sizeof(config)) ?
+		-EFAULT : 0;
+}
+
+/**
+ * ixgbe_ptp_start_cyclecounter - create the cycle counter from hw
+ * @adapter - pointer to the adapter structure
+ *
+ * this function initializes the timecounter and cyclecounter
+ * structures for use in generated a ns counter from the arbitrary
+ * fixed point cycles registers in the hardware.
+ *
+ * A change in link speed impacts the frequency of the DMA clock on
+ * the device, which is used to generate the cycle counter
+ * registers. Therefor this function is called whenever the link speed
+ * changes.
+ *
+ * This function also turns on the SDP pin for clock out feature (X540
+ * only), because this is where the shift is first calculated.
+ */
+void ixgbe_ptp_start_cyclecounter(struct ixgbe_adapter *adapter)
+{
+	struct ixgbe_hw *hw = &adapter->hw;
+	u32 incval = 0;
+	u32 shift = 0;
+	u32 cycle_speed;
+	unsigned long flags;
+
+	/**
+	 * Determine what speed we need to set the cyclecounter
+	 * for. It should be different for 100Mb, 1Gb, and 10Gb. Treat
+	 * unknown speeds as 10Gb. (Hence why we can't just copy the
+	 * link_speed.
+	 */
+	switch (adapter->link_speed) {
+	case IXGBE_LINK_SPEED_100_FULL:
+	case IXGBE_LINK_SPEED_1GB_FULL:
+	case IXGBE_LINK_SPEED_10GB_FULL:
+		cycle_speed = adapter->link_speed;
+		break;
+	default:
+		/* cycle speed should be 10Gb when there is no link */
+		cycle_speed = IXGBE_LINK_SPEED_10GB_FULL;
+		break;
+	}
+
+	/* Bail if the cycle speed didn't change */
+	if (adapter->cycle_speed == cycle_speed)
+		return;
+
+	/* disable the SDP clock out */
+	ixgbe_ptp_disable_sdp(hw);
+
+	/**
+	 * Scale the NIC cycle counter by a large factor so that
+	 * relatively small corrections to the frequency can be added
+	 * or subtracted. The drawbacks of a large factor include
+	 * (a) the clock register overflows more quickly, (b) the cycle
+	 * counter structure must be able to convert the systime value
+	 * to nanoseconds using only a multiplier and a right-shift,
+	 * and (c) the value must fit within the timinca register space
+	 * => math based on internal DMA clock rate and available bits
+	 */
+	switch (cycle_speed) {
+	case IXGBE_LINK_SPEED_100_FULL:
+		incval = IXGBE_INCVAL_100;
+		shift = IXGBE_INCVAL_SHIFT_100;
+		break;
+	case IXGBE_LINK_SPEED_1GB_FULL:
+		incval = IXGBE_INCVAL_1GB;
+		shift = IXGBE_INCVAL_SHIFT_1GB;
+		break;
+	case IXGBE_LINK_SPEED_10GB_FULL:
+		incval = IXGBE_INCVAL_10GB;
+		shift = IXGBE_INCVAL_SHIFT_10GB;
+		break;
+	}
+
+	/**
+	 * Modify the calculated values to fit within the correct
+	 * number of bits specified by the hardware. The 82599 doesn't
+	 * have the same space as the X540, so bitshift the calculated
+	 * values to fit.
+	 */
+	switch (hw->mac.type) {
+	case ixgbe_mac_X540:
+		IXGBE_WRITE_REG(hw, IXGBE_TIMINCA, incval);
+		break;
+	case ixgbe_mac_82599EB:
+		incval >>= IXGBE_INCVAL_SHIFT_82599;
+		shift -= IXGBE_INCVAL_SHIFT_82599;
+		IXGBE_WRITE_REG(hw, IXGBE_TIMINCA,
+				(1 << IXGBE_INCPER_SHIFT_82599) |
+				incval);
+		break;
+	default:
+		/* other devices aren't supported */
+		return;
+	}
+
+	/* reset the system time registers */
+	IXGBE_WRITE_REG(hw, IXGBE_SYSTIML, 0x00000000);
+	IXGBE_WRITE_REG(hw, IXGBE_SYSTIMH, 0x00000000);
+	IXGBE_WRITE_FLUSH(hw);
+
+	/* now that the shift has been calculated and the systime
+	 * registers reset, (re-)enable the Clock out feature*/
+	ixgbe_ptp_enable_sdp(hw, shift);
+
+	/* store the new cycle speed */
+	adapter->cycle_speed = cycle_speed;
+
+	ACCESS_ONCE(adapter->base_incval) = incval;
+	smp_mb();
+
+	/* grab the ptp lock */
+	spin_lock_irqsave(&adapter->tmreg_lock, flags);
+
+	memset(&adapter->cc, 0, sizeof(adapter->cc));
+	adapter->cc.read = ixgbe_ptp_read;
+	adapter->cc.mask = CLOCKSOURCE_MASK(64);
+	adapter->cc.shift = shift;
+	adapter->cc.mult = 1;
+
+	/* reset the ns time counter */
+	timecounter_init(&adapter->tc, &adapter->cc,
+			 ktime_to_ns(ktime_get_real()));
+
+	spin_unlock_irqrestore(&adapter->tmreg_lock, flags);
+}
+
+/**
+ * ixgbe_ptp_init
+ * @adapter - the ixgbe private adapter structure
+ *
+ * This function performs the required steps for enabling ptp
+ * support. If ptp support has already been loaded it simply calls the
+ * cyclecounter init routine and exits.
+ */
+void ixgbe_ptp_init(struct ixgbe_adapter *adapter)
+{
+	struct net_device *netdev = adapter->netdev;
+
+	switch (adapter->hw.mac.type) {
+	case ixgbe_mac_X540:
+		snprintf(adapter->ptp_caps.name, 16, "%pm", netdev->dev_addr);
+		adapter->ptp_caps.owner = THIS_MODULE;
+		adapter->ptp_caps.max_adj = 250000000;
+		adapter->ptp_caps.n_alarm = 0;
+		adapter->ptp_caps.n_ext_ts = 0;
+		adapter->ptp_caps.n_per_out = 0;
+		adapter->ptp_caps.pps = 1;
+		adapter->ptp_caps.adjfreq = ixgbe_ptp_adjfreq;
+		adapter->ptp_caps.adjtime = ixgbe_ptp_adjtime;
+		adapter->ptp_caps.gettime = ixgbe_ptp_gettime;
+		adapter->ptp_caps.settime = ixgbe_ptp_settime;
+		adapter->ptp_caps.enable = ixgbe_ptp_enable;
+		break;
+	case ixgbe_mac_82599EB:
+		snprintf(adapter->ptp_caps.name, 16, "%pm", netdev->dev_addr);
+		adapter->ptp_caps.owner = THIS_MODULE;
+		adapter->ptp_caps.max_adj = 250000000;
+		adapter->ptp_caps.n_alarm = 0;
+		adapter->ptp_caps.n_ext_ts = 0;
+		adapter->ptp_caps.n_per_out = 0;
+		adapter->ptp_caps.pps = 0;
+		adapter->ptp_caps.adjfreq = ixgbe_ptp_adjfreq;
+		adapter->ptp_caps.adjtime = ixgbe_ptp_adjtime;
+		adapter->ptp_caps.gettime = ixgbe_ptp_gettime;
+		adapter->ptp_caps.settime = ixgbe_ptp_settime;
+		adapter->ptp_caps.enable = ixgbe_ptp_enable;
+		break;
+	default:
+		adapter->ptp_clock = NULL;
+		return;
+	}
+
+	spin_lock_init(&adapter->tmreg_lock);
+
+	ixgbe_ptp_start_cyclecounter(adapter);
+
+	/* (Re)start the overflow check */
+	adapter->flags2 |= IXGBE_FLAG2_OVERFLOW_CHECK_ENABLED;
+
+	adapter->ptp_clock = ptp_clock_register(&adapter->ptp_caps);
+	if (IS_ERR(adapter->ptp_clock)) {
+		adapter->ptp_clock = NULL;
+		e_dev_err("ptp_clock_register failed\n");
+	} else
+		e_dev_info("registered PHC device on %s\n", netdev->name);
+
+	return;
+}
+
+/**
+ * ixgbe_ptp_stop - disable ptp device and stop the overflow check
+ * @adapter: pointer to adapter struct
+ *
+ * this function stops the ptp support, and cancels the delayed work.
+ */
+void ixgbe_ptp_stop(struct ixgbe_adapter *adapter)
+{
+	ixgbe_ptp_disable_sdp(&adapter->hw);
+
+	/* stop the overflow check task */
+	adapter->flags2 &= ~IXGBE_FLAG2_OVERFLOW_CHECK_ENABLED;
+
+	if (adapter->ptp_clock) {
+		ptp_clock_unregister(adapter->ptp_clock);
+		adapter->ptp_clock = NULL;
+		e_dev_info("removed PHC on %s\n",
+			   adapter->netdev->name);
+	}
+}
diff --git a/drivers/net/ethernet/intel/ixgbe/ixgbe_sriov.c b/drivers/net/ethernet/intel/ixgbe/ixgbe_sriov.c
index 88a58cb0856..2d971d18696 100644
--- a/drivers/net/ethernet/intel/ixgbe/ixgbe_sriov.c
+++ b/drivers/net/ethernet/intel/ixgbe/ixgbe_sriov.c
@@ -544,13 +544,18 @@ static int ixgbe_rcv_msg_from_vf(struct ixgbe_adapter *adapter, u32 vf)
 
 	retval = ixgbe_read_mbx(hw, msgbuf, mbx_size, vf);
 
-	if (retval)
+	if (retval) {
 		pr_err("Error receiving message from VF\n");
+		return retval;
+	}
 
 	/* this is a message we already processed, do nothing */
 	if (msgbuf[0] & (IXGBE_VT_MSGTYPE_ACK | IXGBE_VT_MSGTYPE_NACK))
 		return retval;
 
+	/* flush the ack before we write any messages back */
+	IXGBE_WRITE_FLUSH(hw);
+
 	/*
 	 * until the vf completes a virtual function reset it should not be
 	 * allowed to start any configuration.
@@ -637,6 +642,12 @@ static int ixgbe_rcv_msg_from_vf(struct ixgbe_adapter *adapter, u32 vf)
 	case IXGBE_VF_SET_MACVLAN:
 		index = (msgbuf[0] & IXGBE_VT_MSGINFO_MASK) >>
 			IXGBE_VT_MSGINFO_SHIFT;
+		if (adapter->vfinfo[vf].pf_set_mac && index > 0) {
+			e_warn(drv, "VF %d requested MACVLAN filter but is "
+				    "administratively denied\n", vf);
+			retval = -1;
+			break;
+		}
 		/*
 		 * If the VF is allowed to set MAC filters then turn off
 		 * anti-spoofing to avoid false positives.  An index
diff --git a/drivers/net/ethernet/intel/ixgbe/ixgbe_sysfs.c b/drivers/net/ethernet/intel/ixgbe/ixgbe_sysfs.c
new file mode 100644
index 00000000000..1d80b1cefa6
--- /dev/null
+++ b/drivers/net/ethernet/intel/ixgbe/ixgbe_sysfs.c
@@ -0,0 +1,245 @@
+/*******************************************************************************
+
+  Intel 10 Gigabit PCI Express Linux driver
+  Copyright(c) 1999 - 2012 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.
+
+  The full GNU General Public License is included in this distribution in
+  the file called "COPYING".
+
+  Contact Information:
+  e1000-devel Mailing List <e1000-devel@lists.sourceforge.net>
+  Intel Corporation, 5200 N.E. Elam Young Parkway, Hillsboro, OR 97124-6497
+
+*******************************************************************************/
+
+#include "ixgbe.h"
+#include "ixgbe_common.h"
+#include "ixgbe_type.h"
+
+#include <linux/module.h>
+#include <linux/types.h>
+#include <linux/sysfs.h>
+#include <linux/kobject.h>
+#include <linux/device.h>
+#include <linux/netdevice.h>
+#include <linux/hwmon.h>
+
+#ifdef CONFIG_IXGBE_HWMON
+/* hwmon callback functions */
+static ssize_t ixgbe_hwmon_show_location(struct device *dev,
+					 struct device_attribute *attr,
+					 char *buf)
+{
+	struct hwmon_attr *ixgbe_attr = container_of(attr, struct hwmon_attr,
+						     dev_attr);
+	return sprintf(buf, "loc%u\n",
+		       ixgbe_attr->sensor->location);
+}
+
+static ssize_t ixgbe_hwmon_show_temp(struct device *dev,
+				     struct device_attribute *attr,
+				     char *buf)
+{
+	struct hwmon_attr *ixgbe_attr = container_of(attr, struct hwmon_attr,
+						     dev_attr);
+	unsigned int value;
+
+	/* reset the temp field */
+	ixgbe_attr->hw->mac.ops.get_thermal_sensor_data(ixgbe_attr->hw);
+
+	value = ixgbe_attr->sensor->temp;
+
+	/* display millidegree */
+	value *= 1000;
+
+	return sprintf(buf, "%u\n", value);
+}
+
+static ssize_t ixgbe_hwmon_show_cautionthresh(struct device *dev,
+				     struct device_attribute *attr,
+				     char *buf)
+{
+	struct hwmon_attr *ixgbe_attr = container_of(attr, struct hwmon_attr,
+						     dev_attr);
+	unsigned int value = ixgbe_attr->sensor->caution_thresh;
+
+	/* display millidegree */
+	value *= 1000;
+
+	return sprintf(buf, "%u\n", value);
+}
+
+static ssize_t ixgbe_hwmon_show_maxopthresh(struct device *dev,
+				     struct device_attribute *attr,
+				     char *buf)
+{
+	struct hwmon_attr *ixgbe_attr = container_of(attr, struct hwmon_attr,
+						     dev_attr);
+	unsigned int value = ixgbe_attr->sensor->max_op_thresh;
+
+	/* display millidegree */
+	value *= 1000;
+
+	return sprintf(buf, "%u\n", value);
+}
+
+/*
+ * ixgbe_add_hwmon_attr - Create hwmon attr table for a hwmon sysfs file.
+ * @ adapter: pointer to the adapter structure
+ * @ offset: offset in the eeprom sensor data table
+ * @ type: type of sensor data to display
+ *
+ * For each file we want in hwmon's sysfs interface we need a device_attribute
+ * This is included in our hwmon_attr struct that contains the references to
+ * the data structures we need to get the data to display.
+ */
+static int ixgbe_add_hwmon_attr(struct ixgbe_adapter *adapter,
+				unsigned int offset, int type) {
+	int rc;
+	unsigned int n_attr;
+	struct hwmon_attr *ixgbe_attr;
+
+	n_attr = adapter->ixgbe_hwmon_buff.n_hwmon;
+	ixgbe_attr = &adapter->ixgbe_hwmon_buff.hwmon_list[n_attr];
+
+	switch (type) {
+	case IXGBE_HWMON_TYPE_LOC:
+		ixgbe_attr->dev_attr.show = ixgbe_hwmon_show_location;
+		snprintf(ixgbe_attr->name, sizeof(ixgbe_attr->name),
+			 "temp%u_label", offset);
+		break;
+	case IXGBE_HWMON_TYPE_TEMP:
+		ixgbe_attr->dev_attr.show = ixgbe_hwmon_show_temp;
+		snprintf(ixgbe_attr->name, sizeof(ixgbe_attr->name),
+			 "temp%u_input", offset);
+		break;
+	case IXGBE_HWMON_TYPE_CAUTION:
+		ixgbe_attr->dev_attr.show = ixgbe_hwmon_show_cautionthresh;
+		snprintf(ixgbe_attr->name, sizeof(ixgbe_attr->name),
+			 "temp%u_max", offset);
+		break;
+	case IXGBE_HWMON_TYPE_MAX:
+		ixgbe_attr->dev_attr.show = ixgbe_hwmon_show_maxopthresh;
+		snprintf(ixgbe_attr->name, sizeof(ixgbe_attr->name),
+			 "temp%u_crit", offset);
+		break;
+	default:
+		rc = -EPERM;
+		return rc;
+	}
+
+	/* These always the same regardless of type */
+	ixgbe_attr->sensor =
+		&adapter->hw.mac.thermal_sensor_data.sensor[offset];
+	ixgbe_attr->hw = &adapter->hw;
+	ixgbe_attr->dev_attr.store = NULL;
+	ixgbe_attr->dev_attr.attr.mode = S_IRUGO;
+	ixgbe_attr->dev_attr.attr.name = ixgbe_attr->name;
+
+	rc = device_create_file(&adapter->pdev->dev,
+				&ixgbe_attr->dev_attr);
+
+	if (rc == 0)
+		++adapter->ixgbe_hwmon_buff.n_hwmon;
+
+	return rc;
+}
+
+static void ixgbe_sysfs_del_adapter(struct ixgbe_adapter *adapter)
+{
+	int i;
+
+	if (adapter == NULL)
+		return;
+
+	for (i = 0; i < adapter->ixgbe_hwmon_buff.n_hwmon; i++) {
+		device_remove_file(&adapter->pdev->dev,
+			   &adapter->ixgbe_hwmon_buff.hwmon_list[i].dev_attr);
+	}
+
+	kfree(adapter->ixgbe_hwmon_buff.hwmon_list);
+
+	if (adapter->ixgbe_hwmon_buff.device)
+		hwmon_device_unregister(adapter->ixgbe_hwmon_buff.device);
+}
+
+/* called from ixgbe_main.c */
+void ixgbe_sysfs_exit(struct ixgbe_adapter *adapter)
+{
+	ixgbe_sysfs_del_adapter(adapter);
+}
+
+/* called from ixgbe_main.c */
+int ixgbe_sysfs_init(struct ixgbe_adapter *adapter)
+{
+	struct hwmon_buff *ixgbe_hwmon = &adapter->ixgbe_hwmon_buff;
+	unsigned int i;
+	int n_attrs;
+	int rc = 0;
+
+	/* If this method isn't defined we don't support thermals */
+	if (adapter->hw.mac.ops.init_thermal_sensor_thresh == NULL) {
+		goto exit;
+	}
+
+	/* Don't create thermal hwmon interface if no sensors present */
+	if (adapter->hw.mac.ops.init_thermal_sensor_thresh(&adapter->hw))
+		goto exit;
+
+	/*
+	 * Allocation space for max attributs
+	 * max num sensors * values (loc, temp, max, caution)
+	 */
+	n_attrs = IXGBE_MAX_SENSORS * 4;
+	ixgbe_hwmon->hwmon_list = kcalloc(n_attrs, sizeof(struct hwmon_attr),
+					  GFP_KERNEL);
+	if (!ixgbe_hwmon->hwmon_list) {
+		rc = -ENOMEM;
+		goto err;
+	}
+
+	ixgbe_hwmon->device = hwmon_device_register(&adapter->pdev->dev);
+	if (IS_ERR(ixgbe_hwmon->device)) {
+		rc = PTR_ERR(ixgbe_hwmon->device);
+		goto err;
+	}
+
+	for (i = 0; i < IXGBE_MAX_SENSORS; i++) {
+		/*
+		 * Only create hwmon sysfs entries for sensors that have
+		 * meaningful data for.
+		 */
+		if (adapter->hw.mac.thermal_sensor_data.sensor[i].location == 0)
+			continue;
+
+		/* Bail if any hwmon attr struct fails to initialize */
+		rc = ixgbe_add_hwmon_attr(adapter, i, IXGBE_HWMON_TYPE_CAUTION);
+		rc |= ixgbe_add_hwmon_attr(adapter, i, IXGBE_HWMON_TYPE_LOC);
+		rc |= ixgbe_add_hwmon_attr(adapter, i, IXGBE_HWMON_TYPE_TEMP);
+		rc |= ixgbe_add_hwmon_attr(adapter, i, IXGBE_HWMON_TYPE_MAX);
+		if (rc)
+			goto err;
+	}
+
+	goto exit;
+
+err:
+	ixgbe_sysfs_del_adapter(adapter);
+exit:
+	return rc;
+}
+#endif /* CONFIG_IXGBE_HWMON */
+
diff --git a/drivers/net/ethernet/intel/ixgbe/ixgbe_type.h b/drivers/net/ethernet/intel/ixgbe/ixgbe_type.h
index 8636e8344fc..204848d2448 100644
--- a/drivers/net/ethernet/intel/ixgbe/ixgbe_type.h
+++ b/drivers/net/ethernet/intel/ixgbe/ixgbe_type.h
@@ -110,6 +110,28 @@
 #define IXGBE_I2C_CLK_OUT   0x00000002
 #define IXGBE_I2C_DATA_IN   0x00000004
 #define IXGBE_I2C_DATA_OUT  0x00000008
+#define IXGBE_I2C_CLOCK_STRETCHING_TIMEOUT	500
+
+#define IXGBE_I2C_THERMAL_SENSOR_ADDR	0xF8
+#define IXGBE_EMC_INTERNAL_DATA		0x00
+#define IXGBE_EMC_INTERNAL_THERM_LIMIT	0x20
+#define IXGBE_EMC_DIODE1_DATA		0x01
+#define IXGBE_EMC_DIODE1_THERM_LIMIT	0x19
+#define IXGBE_EMC_DIODE2_DATA		0x23
+#define IXGBE_EMC_DIODE2_THERM_LIMIT	0x1A
+
+#define IXGBE_MAX_SENSORS		3
+
+struct ixgbe_thermal_diode_data {
+	u8 location;
+	u8 temp;
+	u8 caution_thresh;
+	u8 max_op_thresh;
+};
+
+struct ixgbe_thermal_sensor_data {
+	struct ixgbe_thermal_diode_data sensor[IXGBE_MAX_SENSORS];
+};
 
 /* Interrupt Registers */
 #define IXGBE_EICR      0x00800
@@ -802,6 +824,8 @@
 #define IXGBE_TRGTTIMH0  0x08C28 /* Target Time Register 0 High - RW */
 #define IXGBE_TRGTTIML1  0x08C2C /* Target Time Register 1 Low - RW */
 #define IXGBE_TRGTTIMH1  0x08C30 /* Target Time Register 1 High - RW */
+#define IXGBE_CLKTIML    0x08C34 /* Clock Out Time Register Low - RW */
+#define IXGBE_CLKTIMH    0x08C38 /* Clock Out Time Register High - RW */
 #define IXGBE_FREQOUT0   0x08C34 /* Frequency Out 0 Control register - RW */
 #define IXGBE_FREQOUT1   0x08C38 /* Frequency Out 1 Control register - RW */
 #define IXGBE_AUXSTMPL0  0x08C3C /* Auxiliary Time Stamp 0 register Low - RO */
@@ -1287,6 +1311,7 @@ enum {
 #define IXGBE_EICR_LINKSEC      0x00200000 /* PN Threshold */
 #define IXGBE_EICR_MNG          0x00400000 /* Manageability Event Interrupt */
 #define IXGBE_EICR_TS           0x00800000 /* Thermal Sensor Event */
+#define IXGBE_EICR_TIMESYNC     0x01000000 /* Timesync Event */
 #define IXGBE_EICR_GPI_SDP0     0x01000000 /* Gen Purpose Interrupt on SDP0 */
 #define IXGBE_EICR_GPI_SDP1     0x02000000 /* Gen Purpose Interrupt on SDP1 */
 #define IXGBE_EICR_GPI_SDP2     0x04000000 /* Gen Purpose Interrupt on SDP2 */
@@ -1304,6 +1329,7 @@ enum {
 #define IXGBE_EICS_MAILBOX      IXGBE_EICR_MAILBOX   /* VF to PF Mailbox Int */
 #define IXGBE_EICS_LSC          IXGBE_EICR_LSC       /* Link Status Change */
 #define IXGBE_EICS_MNG          IXGBE_EICR_MNG       /* MNG Event Interrupt */
+#define IXGBE_EICS_TIMESYNC     IXGBE_EICR_TIMESYNC  /* Timesync Event */
 #define IXGBE_EICS_GPI_SDP0     IXGBE_EICR_GPI_SDP0  /* SDP0 Gen Purpose Int */
 #define IXGBE_EICS_GPI_SDP1     IXGBE_EICR_GPI_SDP1  /* SDP1 Gen Purpose Int */
 #define IXGBE_EICS_GPI_SDP2     IXGBE_EICR_GPI_SDP2  /* SDP2 Gen Purpose Int */
@@ -1322,6 +1348,7 @@ enum {
 #define IXGBE_EIMS_LSC          IXGBE_EICR_LSC       /* Link Status Change */
 #define IXGBE_EIMS_MNG          IXGBE_EICR_MNG       /* MNG Event Interrupt */
 #define IXGBE_EIMS_TS           IXGBE_EICR_TS        /* Thermel Sensor Event */
+#define IXGBE_EIMS_TIMESYNC     IXGBE_EICR_TIMESYNC  /* Timesync Event */
 #define IXGBE_EIMS_GPI_SDP0     IXGBE_EICR_GPI_SDP0  /* SDP0 Gen Purpose Int */
 #define IXGBE_EIMS_GPI_SDP1     IXGBE_EICR_GPI_SDP1  /* SDP1 Gen Purpose Int */
 #define IXGBE_EIMS_GPI_SDP2     IXGBE_EICR_GPI_SDP2  /* SDP2 Gen Purpose Int */
@@ -1339,6 +1366,7 @@ enum {
 #define IXGBE_EIMC_MAILBOX      IXGBE_EICR_MAILBOX   /* VF to PF Mailbox Int */
 #define IXGBE_EIMC_LSC          IXGBE_EICR_LSC       /* Link Status Change */
 #define IXGBE_EIMC_MNG          IXGBE_EICR_MNG       /* MNG Event Interrupt */
+#define IXGBE_EIMC_TIMESYNC     IXGBE_EICR_TIMESYNC  /* Timesync Event */
 #define IXGBE_EIMC_GPI_SDP0     IXGBE_EICR_GPI_SDP0  /* SDP0 Gen Purpose Int */
 #define IXGBE_EIMC_GPI_SDP1     IXGBE_EICR_GPI_SDP1  /* SDP1 Gen Purpose Int */
 #define IXGBE_EIMC_GPI_SDP2     IXGBE_EICR_GPI_SDP2  /* SDP2 Gen Purpose Int */
@@ -1479,8 +1507,10 @@ enum {
 #define IXGBE_ESDP_SDP4 0x00000010 /* SDP4 Data Value */
 #define IXGBE_ESDP_SDP5 0x00000020 /* SDP5 Data Value */
 #define IXGBE_ESDP_SDP6 0x00000040 /* SDP6 Data Value */
+#define IXGBE_ESDP_SDP0_DIR     0x00000100 /* SDP0 IO direction */
 #define IXGBE_ESDP_SDP4_DIR     0x00000004 /* SDP4 IO direction */
 #define IXGBE_ESDP_SDP5_DIR     0x00002000 /* SDP5 IO direction */
+#define IXGBE_ESDP_SDP0_NATIVE  0x00010000 /* SDP0 Native Function */
 
 /* LEDCTL Bit Masks */
 #define IXGBE_LED_IVRT_BASE      0x00000040
@@ -1677,11 +1707,29 @@ enum {
 #define IXGBE_PBANUM0_PTR       0x15
 #define IXGBE_PBANUM1_PTR       0x16
 #define IXGBE_FREE_SPACE_PTR    0X3E
+
+/* External Thermal Sensor Config */
+#define IXGBE_ETS_CFG                   0x26
+#define IXGBE_ETS_LTHRES_DELTA_MASK     0x07C0
+#define IXGBE_ETS_LTHRES_DELTA_SHIFT    6
+#define IXGBE_ETS_TYPE_MASK             0x0038
+#define IXGBE_ETS_TYPE_SHIFT            3
+#define IXGBE_ETS_TYPE_EMC              0x000
+#define IXGBE_ETS_TYPE_EMC_SHIFTED      0x000
+#define IXGBE_ETS_NUM_SENSORS_MASK      0x0007
+#define IXGBE_ETS_DATA_LOC_MASK         0x3C00
+#define IXGBE_ETS_DATA_LOC_SHIFT        10
+#define IXGBE_ETS_DATA_INDEX_MASK       0x0300
+#define IXGBE_ETS_DATA_INDEX_SHIFT      8
+#define IXGBE_ETS_DATA_HTHRESH_MASK     0x00FF
+
 #define IXGBE_SAN_MAC_ADDR_PTR  0x28
 #define IXGBE_DEVICE_CAPS       0x2C
 #define IXGBE_SERIAL_NUMBER_MAC_ADDR 0x11
 #define IXGBE_PCIE_MSIX_82599_CAPS  0x72
+#define IXGBE_MAX_MSIX_VECTORS_82599	0x40
 #define IXGBE_PCIE_MSIX_82598_CAPS  0x62
+#define IXGBE_MAX_MSIX_VECTORS_82598	0x13
 
 /* MSI-X capability fields masks */
 #define IXGBE_PCIE_MSIX_TBL_SZ_MASK     0x7FF
@@ -1839,6 +1887,40 @@ enum {
 #define IXGBE_RXDCTL_RLPML_EN   0x00008000
 #define IXGBE_RXDCTL_VME        0x40000000  /* VLAN mode enable */
 
+#define IXGBE_TSAUXC_EN_CLK   0x00000004
+#define IXGBE_TSAUXC_SYNCLK   0x00000008
+#define IXGBE_TSAUXC_SDP0_INT 0x00000040
+
+#define IXGBE_TSYNCTXCTL_VALID		0x00000001 /* Tx timestamp valid */
+#define IXGBE_TSYNCTXCTL_ENABLED	0x00000010 /* Tx timestamping enabled */
+
+#define IXGBE_TSYNCRXCTL_VALID		0x00000001 /* Rx timestamp valid */
+#define IXGBE_TSYNCRXCTL_TYPE_MASK	0x0000000E /* Rx type mask */
+#define IXGBE_TSYNCRXCTL_TYPE_L2_V2	0x00
+#define IXGBE_TSYNCRXCTL_TYPE_L4_V1	0x02
+#define IXGBE_TSYNCRXCTL_TYPE_L2_L4_V2	0x04
+#define IXGBE_TSYNCRXCTL_TYPE_EVENT_V2	0x0A
+#define IXGBE_TSYNCRXCTL_ENABLED	0x00000010 /* Rx Timestamping enabled */
+
+#define IXGBE_RXMTRL_V1_CTRLT_MASK	0x000000FF
+#define IXGBE_RXMTRL_V1_SYNC_MSG	0x00
+#define IXGBE_RXMTRL_V1_DELAY_REQ_MSG	0x01
+#define IXGBE_RXMTRL_V1_FOLLOWUP_MSG	0x02
+#define IXGBE_RXMTRL_V1_DELAY_RESP_MSG	0x03
+#define IXGBE_RXMTRL_V1_MGMT_MSG	0x04
+
+#define IXGBE_RXMTRL_V2_MSGID_MASK		0x0000FF00
+#define IXGBE_RXMTRL_V2_SYNC_MSG		0x0000
+#define IXGBE_RXMTRL_V2_DELAY_REQ_MSG		0x0100
+#define IXGBE_RXMTRL_V2_PDELAY_REQ_MSG		0x0200
+#define IXGBE_RXMTRL_V2_PDELAY_RESP_MSG		0x0300
+#define IXGBE_RXMTRL_V2_FOLLOWUP_MSG		0x0800
+#define IXGBE_RXMTRL_V2_DELAY_RESP_MSG		0x0900
+#define IXGBE_RXMTRL_V2_PDELAY_FOLLOWUP_MSG	0x0A00
+#define IXGBE_RXMTRL_V2_ANNOUNCE_MSG		0x0B00
+#define IXGBE_RXMTRL_V2_SIGNALING_MSG		0x0C00
+#define IXGBE_RXMTRL_V2_MGMT_MSG		0x0D00
+
 #define IXGBE_FCTRL_SBP 0x00000002 /* Store Bad Packet */
 #define IXGBE_FCTRL_MPE 0x00000100 /* Multicast Promiscuous Ena*/
 #define IXGBE_FCTRL_UPE 0x00000200 /* Unicast Promiscuous Ena */
@@ -1852,7 +1934,7 @@ enum {
 #define IXGBE_MFLCN_DPF         0x00000002 /* Discard Pause Frame */
 #define IXGBE_MFLCN_RPFCE       0x00000004 /* Receive Priority FC Enable */
 #define IXGBE_MFLCN_RFCE        0x00000008 /* Receive FC Enable */
-#define IXGBE_MFLCN_RPFCE_MASK	0x00000FF0 /* Receive FC Mask */
+#define IXGBE_MFLCN_RPFCE_MASK	0x00000FF4 /* Receive FC Mask */
 
 #define IXGBE_MFLCN_RPFCE_SHIFT		 4
 
@@ -1968,6 +2050,7 @@ enum {
 #define IXGBE_RXDADV_STAT_FCSTAT_NODDP  0x00000010 /* 01: Ctxt w/o DDP */
 #define IXGBE_RXDADV_STAT_FCSTAT_FCPRSP 0x00000020 /* 10: Recv. FCP_RSP */
 #define IXGBE_RXDADV_STAT_FCSTAT_DDP    0x00000030 /* 11: Ctxt w/ DDP */
+#define IXGBE_RXDADV_STAT_TS		0x00010000 /* IEEE 1588 Time Stamp */
 
 /* PSRTYPE bit definitions */
 #define IXGBE_PSRTYPE_TCPHDR    0x00000010
@@ -2245,6 +2328,7 @@ struct ixgbe_adv_tx_context_desc {
 /* Adv Transmit Descriptor Config Masks */
 #define IXGBE_ADVTXD_DTALEN_MASK      0x0000FFFF /* Data buf length(bytes) */
 #define IXGBE_ADVTXD_MAC_LINKSEC      0x00040000 /* Insert LinkSec */
+#define IXGBE_ADVTXD_MAC_TSTAMP	      0x00080000 /* IEEE 1588 Time Stamp */
 #define IXGBE_ADVTXD_IPSEC_SA_INDEX_MASK   0x000003FF /* IPSec SA index */
 #define IXGBE_ADVTXD_IPSEC_ESP_LEN_MASK    0x000001FF /* IPSec ESP length */
 #define IXGBE_ADVTXD_DTYP_MASK  0x00F00000 /* DTYP mask */
@@ -2533,9 +2617,6 @@ enum ixgbe_fc_mode {
 	ixgbe_fc_rx_pause,
 	ixgbe_fc_tx_pause,
 	ixgbe_fc_full,
-#ifdef CONFIG_DCB
-	ixgbe_fc_pfc,
-#endif
 	ixgbe_fc_default
 };
 
@@ -2768,10 +2849,12 @@ struct ixgbe_mac_operations {
 	void (*set_vlan_anti_spoofing)(struct ixgbe_hw *, bool, int);
 
 	/* Flow Control */
-	s32 (*fc_enable)(struct ixgbe_hw *, s32);
+	s32 (*fc_enable)(struct ixgbe_hw *);
 
 	/* Manageability interface */
 	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);
 };
 
 struct ixgbe_phy_operations {
@@ -2813,6 +2896,7 @@ struct ixgbe_mac_info {
 	u16                             wwnn_prefix;
 	/* prefix for World Wide Port Name (WWPN) */
 	u16                             wwpn_prefix;
+	u16				max_msix_vectors;
 #define IXGBE_MAX_MTA			128
 	u32				mta_shadow[IXGBE_MAX_MTA];
 	s32                             mc_filter_type;
@@ -2823,12 +2907,12 @@ struct ixgbe_mac_info {
 	u32				rx_pb_size;
 	u32                             max_tx_queues;
 	u32                             max_rx_queues;
-	u32                             max_msix_vectors;
 	u32                             orig_autoc;
 	u32                             orig_autoc2;
 	bool                            orig_link_settings_stored;
 	bool                            autotry_restart;
 	u8                              flags;
+	struct ixgbe_thermal_sensor_data  thermal_sensor_data;
 };
 
 struct ixgbe_phy_info {
@@ -2938,7 +3022,6 @@ struct ixgbe_info {
 #define IXGBE_ERR_OVERTEMP                      -26
 #define IXGBE_ERR_FC_NOT_NEGOTIATED             -27
 #define IXGBE_ERR_FC_NOT_SUPPORTED              -28
-#define IXGBE_ERR_FLOW_CONTROL                  -29
 #define IXGBE_ERR_SFP_SETUP_NOT_COMPLETE        -30
 #define IXGBE_ERR_PBA_SECTION                   -31
 #define IXGBE_ERR_INVALID_ARGUMENT              -32
diff --git a/drivers/net/ethernet/intel/ixgbe/ixgbe_x540.c b/drivers/net/ethernet/intel/ixgbe/ixgbe_x540.c
index 97a991403bb..f90ec078ece 100644
--- a/drivers/net/ethernet/intel/ixgbe/ixgbe_x540.c
+++ b/drivers/net/ethernet/intel/ixgbe/ixgbe_x540.c
@@ -849,6 +849,8 @@ static struct ixgbe_mac_operations mac_ops_X540 = {
 	.release_swfw_sync      = &ixgbe_release_swfw_sync_X540,
 	.disable_rx_buff	= &ixgbe_disable_rx_buff_generic,
 	.enable_rx_buff		= &ixgbe_enable_rx_buff_generic,
+	.get_thermal_sensor_data = NULL,
+	.init_thermal_sensor_thresh = NULL,
 };
 
 static struct ixgbe_eeprom_operations eeprom_ops_X540 = {
diff --git a/drivers/net/ethernet/intel/ixgbevf/defines.h b/drivers/net/ethernet/intel/ixgbevf/defines.h
index 947b5c83073..e09a6cc633b 100644
--- a/drivers/net/ethernet/intel/ixgbevf/defines.h
+++ b/drivers/net/ethernet/intel/ixgbevf/defines.h
@@ -40,6 +40,7 @@
 typedef u32 ixgbe_link_speed;
 #define IXGBE_LINK_SPEED_1GB_FULL       0x0020
 #define IXGBE_LINK_SPEED_10GB_FULL      0x0080
+#define IXGBE_LINK_SPEED_100_FULL	0x0008
 
 #define IXGBE_CTRL_RST              0x04000000 /* Reset (SW) */
 #define IXGBE_RXDCTL_ENABLE         0x02000000 /* Enable specific Rx Queue */
@@ -48,6 +49,7 @@ typedef u32 ixgbe_link_speed;
 #define IXGBE_LINKS_SPEED_82599     0x30000000
 #define IXGBE_LINKS_SPEED_10G_82599 0x30000000
 #define IXGBE_LINKS_SPEED_1G_82599  0x20000000
+#define IXGBE_LINKS_SPEED_100_82599 0x10000000
 
 /* Number of Transmit and Receive Descriptors must be a multiple of 8 */
 #define IXGBE_REQ_TX_DESCRIPTOR_MULTIPLE  8
diff --git a/drivers/net/ethernet/intel/ixgbevf/ethtool.c b/drivers/net/ethernet/intel/ixgbevf/ethtool.c
index 2bfe0d1d795..e8dddf572d3 100644
--- a/drivers/net/ethernet/intel/ixgbevf/ethtool.c
+++ b/drivers/net/ethernet/intel/ixgbevf/ethtool.c
@@ -107,10 +107,20 @@ static int ixgbevf_get_settings(struct net_device *netdev,
 	hw->mac.ops.check_link(hw, &link_speed, &link_up, false);
 
 	if (link_up) {
-		ethtool_cmd_speed_set(
-			ecmd,
-			(link_speed == IXGBE_LINK_SPEED_10GB_FULL) ?
-			SPEED_10000 : SPEED_1000);
+		__u32 speed = SPEED_10000;
+		switch (link_speed) {
+		case IXGBE_LINK_SPEED_10GB_FULL:
+			speed = SPEED_10000;
+			break;
+		case IXGBE_LINK_SPEED_1GB_FULL:
+			speed = SPEED_1000;
+			break;
+		case IXGBE_LINK_SPEED_100_FULL:
+			speed = SPEED_100;
+			break;
+		}
+
+		ethtool_cmd_speed_set(ecmd, speed);
 		ecmd->duplex = DUPLEX_FULL;
 	} else {
 		ethtool_cmd_speed_set(ecmd, -1);
diff --git a/drivers/net/ethernet/intel/ixgbevf/ixgbevf.h b/drivers/net/ethernet/intel/ixgbevf/ixgbevf.h
index dfed420a1bf..0a1b99240d4 100644
--- a/drivers/net/ethernet/intel/ixgbevf/ixgbevf.h
+++ b/drivers/net/ethernet/intel/ixgbevf/ixgbevf.h
@@ -287,7 +287,7 @@ extern const struct ixgbe_mbx_operations ixgbevf_mbx_ops;
 extern const char ixgbevf_driver_name[];
 extern const char ixgbevf_driver_version[];
 
-extern int ixgbevf_up(struct ixgbevf_adapter *adapter);
+extern void ixgbevf_up(struct ixgbevf_adapter *adapter);
 extern void ixgbevf_down(struct ixgbevf_adapter *adapter);
 extern void ixgbevf_reinit_locked(struct ixgbevf_adapter *adapter);
 extern void ixgbevf_reset(struct ixgbevf_adapter *adapter);
diff --git a/drivers/net/ethernet/intel/ixgbevf/ixgbevf_main.c b/drivers/net/ethernet/intel/ixgbevf/ixgbevf_main.c
index 307611ae831..f69ec4288b1 100644
--- a/drivers/net/ethernet/intel/ixgbevf/ixgbevf_main.c
+++ b/drivers/net/ethernet/intel/ixgbevf/ixgbevf_main.c
@@ -57,7 +57,7 @@ const char ixgbevf_driver_name[] = "ixgbevf";
 static const char ixgbevf_driver_string[] =
 	"Intel(R) 10 Gigabit PCI Express Virtual Function Network Driver";
 
-#define DRV_VERSION "2.2.0-k"
+#define DRV_VERSION "2.6.0-k"
 const char ixgbevf_driver_version[] = DRV_VERSION;
 static char ixgbevf_copyright[] =
 	"Copyright (c) 2009 - 2012 Intel Corporation.";
@@ -1608,13 +1608,14 @@ static void ixgbevf_init_last_counter_stats(struct ixgbevf_adapter *adapter)
 	adapter->stats.base_vfmprc = adapter->stats.last_vfmprc;
 }
 
-static int ixgbevf_up_complete(struct ixgbevf_adapter *adapter)
+static void ixgbevf_up_complete(struct ixgbevf_adapter *adapter)
 {
 	struct net_device *netdev = adapter->netdev;
 	struct ixgbe_hw *hw = &adapter->hw;
 	int i, j = 0;
 	int num_rx_rings = adapter->num_rx_queues;
 	u32 txdctl, rxdctl;
+	u32 msg[2];
 
 	for (i = 0; i < adapter->num_tx_queues; i++) {
 		j = adapter->tx_ring[i].reg_idx;
@@ -1653,6 +1654,10 @@ static int ixgbevf_up_complete(struct ixgbevf_adapter *adapter)
 			hw->mac.ops.set_rar(hw, 0, hw->mac.perm_addr, 0);
 	}
 
+	msg[0] = IXGBE_VF_SET_LPE;
+	msg[1] = netdev->mtu + ETH_HLEN + ETH_FCS_LEN;
+	hw->mbx.ops.write_posted(hw, msg, 2);
+
 	clear_bit(__IXGBEVF_DOWN, &adapter->state);
 	ixgbevf_napi_enable_all(adapter);
 
@@ -1667,24 +1672,20 @@ static int ixgbevf_up_complete(struct ixgbevf_adapter *adapter)
 	adapter->flags |= IXGBE_FLAG_NEED_LINK_UPDATE;
 	adapter->link_check_timeout = jiffies;
 	mod_timer(&adapter->watchdog_timer, jiffies);
-	return 0;
 }
 
-int ixgbevf_up(struct ixgbevf_adapter *adapter)
+void ixgbevf_up(struct ixgbevf_adapter *adapter)
 {
-	int err;
 	struct ixgbe_hw *hw = &adapter->hw;
 
 	ixgbevf_configure(adapter);
 
-	err = ixgbevf_up_complete(adapter);
+	ixgbevf_up_complete(adapter);
 
 	/* clear any pending interrupts, may auto mask */
 	IXGBE_READ_REG(hw, IXGBE_VTEICR);
 
 	ixgbevf_irq_enable(adapter, true, true);
-
-	return err;
 }
 
 /**
@@ -2673,9 +2674,7 @@ static int ixgbevf_open(struct net_device *netdev)
 	 */
 	ixgbevf_map_rings_to_vectors(adapter);
 
-	err = ixgbevf_up_complete(adapter);
-	if (err)
-		goto err_up;
+	ixgbevf_up_complete(adapter);
 
 	/* clear any pending interrupts, may auto mask */
 	IXGBE_READ_REG(hw, IXGBE_VTEICR);
@@ -2689,7 +2688,6 @@ static int ixgbevf_open(struct net_device *netdev)
 
 err_req_irq:
 	ixgbevf_down(adapter);
-err_up:
 	ixgbevf_free_irq(adapter);
 err_setup_rx:
 	ixgbevf_free_all_rx_resources(adapter);
@@ -3196,9 +3194,11 @@ static int ixgbevf_change_mtu(struct net_device *netdev, int new_mtu)
 	/* must set new MTU before calling down or up */
 	netdev->mtu = new_mtu;
 
-	msg[0] = IXGBE_VF_SET_LPE;
-	msg[1] = max_frame;
-	hw->mbx.ops.write_posted(hw, msg, 2);
+	if (!netif_running(netdev)) {
+		msg[0] = IXGBE_VF_SET_LPE;
+		msg[1] = max_frame;
+		hw->mbx.ops.write_posted(hw, msg, 2);
+	}
 
 	if (netif_running(netdev))
 		ixgbevf_reinit_locked(adapter);
diff --git a/drivers/net/ethernet/intel/ixgbevf/vf.c b/drivers/net/ethernet/intel/ixgbevf/vf.c
index 74be7411242..ec89b86f7ca 100644
--- a/drivers/net/ethernet/intel/ixgbevf/vf.c
+++ b/drivers/net/ethernet/intel/ixgbevf/vf.c
@@ -404,11 +404,17 @@ static s32 ixgbevf_check_mac_link_vf(struct ixgbe_hw *hw,
 	else
 		*link_up = false;
 
-	if ((links_reg & IXGBE_LINKS_SPEED_82599) ==
-	    IXGBE_LINKS_SPEED_10G_82599)
+	switch (links_reg & IXGBE_LINKS_SPEED_82599) {
+	case IXGBE_LINKS_SPEED_10G_82599:
 		*speed = IXGBE_LINK_SPEED_10GB_FULL;
-	else
+		break;
+	case IXGBE_LINKS_SPEED_1G_82599:
 		*speed = IXGBE_LINK_SPEED_1GB_FULL;
+		break;
+	case IXGBE_LINKS_SPEED_100_82599:
+		*speed = IXGBE_LINK_SPEED_100_FULL;
+		break;
+	}
 
 	return 0;
 }
diff --git a/drivers/net/ethernet/marvell/mv643xx_eth.c b/drivers/net/ethernet/marvell/mv643xx_eth.c
index 5e1ca0f0509..04d901d0ff6 100644
--- a/drivers/net/ethernet/marvell/mv643xx_eth.c
+++ b/drivers/net/ethernet/marvell/mv643xx_eth.c
@@ -57,6 +57,7 @@
 #include <linux/types.h>
 #include <linux/inet_lro.h>
 #include <linux/slab.h>
+#include <linux/clk.h>
 
 static char mv643xx_eth_driver_name[] = "mv643xx_eth";
 static char mv643xx_eth_driver_version[] = "1.4";
@@ -289,10 +290,10 @@ struct mv643xx_eth_shared_private {
 	/*
 	 * Hardware-specific parameters.
 	 */
-	unsigned int t_clk;
 	int extended_rx_coal_limit;
 	int tx_bw_control;
 	int tx_csum_limit;
+
 };
 
 #define TX_BW_CONTROL_ABSENT		0
@@ -431,6 +432,12 @@ struct mv643xx_eth_private {
 	int tx_desc_sram_size;
 	int txq_count;
 	struct tx_queue txq[8];
+
+	/*
+	 * Hardware-specific parameters.
+	 */
+	struct clk *clk;
+	unsigned int t_clk;
 };
 
 
@@ -1010,7 +1017,7 @@ static void tx_set_rate(struct mv643xx_eth_private *mp, int rate, int burst)
 	int mtu;
 	int bucket_size;
 
-	token_rate = ((rate / 1000) * 64) / (mp->shared->t_clk / 1000);
+	token_rate = ((rate / 1000) * 64) / (mp->t_clk / 1000);
 	if (token_rate > 1023)
 		token_rate = 1023;
 
@@ -1042,7 +1049,7 @@ static void txq_set_rate(struct tx_queue *txq, int rate, int burst)
 	int token_rate;
 	int bucket_size;
 
-	token_rate = ((rate / 1000) * 64) / (mp->shared->t_clk / 1000);
+	token_rate = ((rate / 1000) * 64) / (mp->t_clk / 1000);
 	if (token_rate > 1023)
 		token_rate = 1023;
 
@@ -1309,7 +1316,7 @@ static unsigned int get_rx_coal(struct mv643xx_eth_private *mp)
 		temp = (val & 0x003fff00) >> 8;
 
 	temp *= 64000000;
-	do_div(temp, mp->shared->t_clk);
+	do_div(temp, mp->t_clk);
 
 	return (unsigned int)temp;
 }
@@ -1319,7 +1326,7 @@ static void set_rx_coal(struct mv643xx_eth_private *mp, unsigned int usec)
 	u64 temp;
 	u32 val;
 
-	temp = (u64)usec * mp->shared->t_clk;
+	temp = (u64)usec * mp->t_clk;
 	temp += 31999999;
 	do_div(temp, 64000000);
 
@@ -1345,7 +1352,7 @@ static unsigned int get_tx_coal(struct mv643xx_eth_private *mp)
 
 	temp = (rdlp(mp, TX_FIFO_URGENT_THRESHOLD) & 0x3fff0) >> 4;
 	temp *= 64000000;
-	do_div(temp, mp->shared->t_clk);
+	do_div(temp, mp->t_clk);
 
 	return (unsigned int)temp;
 }
@@ -1354,7 +1361,7 @@ static void set_tx_coal(struct mv643xx_eth_private *mp, unsigned int usec)
 {
 	u64 temp;
 
-	temp = (u64)usec * mp->shared->t_clk;
+	temp = (u64)usec * mp->t_clk;
 	temp += 31999999;
 	do_div(temp, 64000000);
 
@@ -1665,6 +1672,7 @@ static const struct ethtool_ops mv643xx_eth_ethtool_ops = {
 	.get_strings		= mv643xx_eth_get_strings,
 	.get_ethtool_stats	= mv643xx_eth_get_ethtool_stats,
 	.get_sset_count		= mv643xx_eth_get_sset_count,
+	.get_ts_info		= ethtool_op_get_ts_info,
 };
 
 
@@ -2662,10 +2670,6 @@ static int mv643xx_eth_shared_probe(struct platform_device *pdev)
 	if (dram)
 		mv643xx_eth_conf_mbus_windows(msp, dram);
 
-	/*
-	 * Detect hardware parameters.
-	 */
-	msp->t_clk = (pd != NULL && pd->t_clk != 0) ? pd->t_clk : 133000000;
 	msp->tx_csum_limit = (pd != NULL && pd->tx_csum_limit) ?
 					pd->tx_csum_limit : 9 * 1024;
 	infer_hw_params(msp);
@@ -2890,6 +2894,18 @@ static int mv643xx_eth_probe(struct platform_device *pdev)
 
 	mp->dev = dev;
 
+	/*
+	 * Get the clk rate, if there is one, otherwise use the default.
+	 */
+	mp->clk = clk_get(&pdev->dev, (pdev->id ? "1" : "0"));
+	if (!IS_ERR(mp->clk)) {
+		clk_prepare_enable(mp->clk);
+		mp->t_clk = clk_get_rate(mp->clk);
+	} else {
+		mp->t_clk = 133000000;
+		printk(KERN_WARNING "Unable to get clock");
+	}
+
 	set_params(mp, pd);
 	netif_set_real_num_tx_queues(dev, mp->txq_count);
 	netif_set_real_num_rx_queues(dev, mp->rxq_count);
@@ -2978,6 +2994,11 @@ static int mv643xx_eth_remove(struct platform_device *pdev)
 	if (mp->phy != NULL)
 		phy_detach(mp->phy);
 	cancel_work_sync(&mp->tx_timeout_task);
+
+	if (!IS_ERR(mp->clk)) {
+		clk_disable_unprepare(mp->clk);
+		clk_put(mp->clk);
+	}
 	free_netdev(mp->dev);
 
 	platform_set_drvdata(pdev, NULL);
diff --git a/drivers/net/ethernet/marvell/pxa168_eth.c b/drivers/net/ethernet/marvell/pxa168_eth.c
index efec6b60b32..1db023b075a 100644
--- a/drivers/net/ethernet/marvell/pxa168_eth.c
+++ b/drivers/net/ethernet/marvell/pxa168_eth.c
@@ -1456,6 +1456,7 @@ static const struct ethtool_ops pxa168_ethtool_ops = {
 	.set_settings = pxa168_set_settings,
 	.get_drvinfo = pxa168_get_drvinfo,
 	.get_link = ethtool_op_get_link,
+	.get_ts_info = ethtool_op_get_ts_info,
 };
 
 static const struct net_device_ops pxa168_eth_netdev_ops = {
diff --git a/drivers/net/ethernet/marvell/sky2.c b/drivers/net/ethernet/marvell/sky2.c
index 487a6c8bd4e..cace36f2ab9 100644
--- a/drivers/net/ethernet/marvell/sky2.c
+++ b/drivers/net/ethernet/marvell/sky2.c
@@ -4825,14 +4825,14 @@ static int __devinit sky2_test_msi(struct sky2_hw *hw)
 
 	init_waitqueue_head(&hw->msi_wait);
 
-	sky2_write32(hw, B0_IMSK, Y2_IS_IRQ_SW);
-
 	err = request_irq(pdev->irq, sky2_test_intr, 0, DRV_NAME, hw);
 	if (err) {
 		dev_err(&pdev->dev, "cannot assign irq %d\n", pdev->irq);
 		return err;
 	}
 
+	sky2_write32(hw, B0_IMSK, Y2_IS_IRQ_SW);
+
 	sky2_write8(hw, B0_CTST, CS_ST_SW_IRQ);
 	sky2_read8(hw, B0_CTST);
 
diff --git a/drivers/net/ethernet/mellanox/mlx4/Kconfig b/drivers/net/ethernet/mellanox/mlx4/Kconfig
index 1bb93531f1b..5f027f95cc8 100644
--- a/drivers/net/ethernet/mellanox/mlx4/Kconfig
+++ b/drivers/net/ethernet/mellanox/mlx4/Kconfig
@@ -11,6 +11,18 @@ config MLX4_EN
 	  This driver supports Mellanox Technologies ConnectX Ethernet
 	  devices.
 
+config MLX4_EN_DCB
+	bool "Data Center Bridging (DCB) Support"
+	default y
+	depends on MLX4_EN && DCB
+	---help---
+	  Say Y here if you want to use Data Center Bridging (DCB) in the
+	  driver.
+	  If set to N, will not be able to configure QoS and ratelimit attributes.
+	  This flag is depended on the kernel's DCB support.
+
+	  If unsure, set to Y
+
 config MLX4_CORE
 	tristate
 	depends on PCI
diff --git a/drivers/net/ethernet/mellanox/mlx4/Makefile b/drivers/net/ethernet/mellanox/mlx4/Makefile
index 4a40ab967ee..293127d28b3 100644
--- a/drivers/net/ethernet/mellanox/mlx4/Makefile
+++ b/drivers/net/ethernet/mellanox/mlx4/Makefile
@@ -7,3 +7,4 @@ 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
+mlx4_en-$(CONFIG_MLX4_EN_DCB) += en_dcb_nl.o
diff --git a/drivers/net/ethernet/mellanox/mlx4/alloc.c b/drivers/net/ethernet/mellanox/mlx4/alloc.c
index 8be20e7ea3d..06fef5b44f7 100644
--- a/drivers/net/ethernet/mellanox/mlx4/alloc.c
+++ b/drivers/net/ethernet/mellanox/mlx4/alloc.c
@@ -124,9 +124,6 @@ void mlx4_bitmap_free_range(struct mlx4_bitmap *bitmap, u32 obj, int cnt)
 
 	spin_lock(&bitmap->lock);
 	bitmap_clear(bitmap->table, obj, cnt);
-	bitmap->last = min(bitmap->last, obj);
-	bitmap->top = (bitmap->top + bitmap->max + bitmap->reserved_top)
-			& bitmap->mask;
 	bitmap->avail += cnt;
 	spin_unlock(&bitmap->lock);
 }
diff --git a/drivers/net/ethernet/mellanox/mlx4/cmd.c b/drivers/net/ethernet/mellanox/mlx4/cmd.c
index 773c70ea3f6..1bcead1fa2f 100644
--- a/drivers/net/ethernet/mellanox/mlx4/cmd.c
+++ b/drivers/net/ethernet/mellanox/mlx4/cmd.c
@@ -1254,7 +1254,6 @@ static void mlx4_master_do_cmd(struct mlx4_dev *dev, int slave, u8 cmd,
 	struct mlx4_priv *priv = mlx4_priv(dev);
 	struct mlx4_slave_state *slave_state = priv->mfunc.master.slave_state;
 	u32 reply;
-	u32 slave_status = 0;
 	u8 is_going_down = 0;
 	int i;
 
@@ -1274,10 +1273,8 @@ static void mlx4_master_do_cmd(struct mlx4_dev *dev, int slave, u8 cmd,
 		}
 		/*check if we are in the middle of FLR process,
 		if so return "retry" status to the slave*/
-		if (MLX4_COMM_CMD_FLR == slave_state[slave].last_cmd) {
-			slave_status = MLX4_DELAY_RESET_SLAVE;
+		if (MLX4_COMM_CMD_FLR == slave_state[slave].last_cmd)
 			goto inform_slave_state;
-		}
 
 		/* write the version in the event field */
 		reply |= mlx4_comm_get_version();
@@ -1557,7 +1554,7 @@ int mlx4_multi_func_init(struct mlx4_dev *dev)
 	return 0;
 
 err_resource:
-	mlx4_free_resource_tracker(dev);
+	mlx4_free_resource_tracker(dev, RES_TR_FREE_ALL);
 err_thread:
 	flush_workqueue(priv->mfunc.master.comm_wq);
 	destroy_workqueue(priv->mfunc.master.comm_wq);
diff --git a/drivers/net/ethernet/mellanox/mlx4/en_cq.c b/drivers/net/ethernet/mellanox/mlx4/en_cq.c
index 00b81272e31..908a460d8db 100644
--- a/drivers/net/ethernet/mellanox/mlx4/en_cq.c
+++ b/drivers/net/ethernet/mellanox/mlx4/en_cq.c
@@ -124,11 +124,7 @@ int mlx4_en_activate_cq(struct mlx4_en_priv *priv, struct mlx4_en_cq *cq,
 	cq->mcq.comp  = cq->is_tx ? mlx4_en_tx_irq : mlx4_en_rx_irq;
 	cq->mcq.event = mlx4_en_cq_event;
 
-	if (cq->is_tx) {
-		init_timer(&cq->timer);
-		cq->timer.function = mlx4_en_poll_tx_cq;
-		cq->timer.data = (unsigned long) cq;
-	} else {
+	if (!cq->is_tx) {
 		netif_napi_add(cq->dev, &cq->napi, mlx4_en_poll_rx_cq, 64);
 		napi_enable(&cq->napi);
 	}
@@ -151,16 +147,12 @@ void mlx4_en_destroy_cq(struct mlx4_en_priv *priv, struct mlx4_en_cq *cq)
 
 void mlx4_en_deactivate_cq(struct mlx4_en_priv *priv, struct mlx4_en_cq *cq)
 {
-	struct mlx4_en_dev *mdev = priv->mdev;
-
-	if (cq->is_tx)
-		del_timer(&cq->timer);
-	else {
+	if (!cq->is_tx) {
 		napi_disable(&cq->napi);
 		netif_napi_del(&cq->napi);
 	}
 
-	mlx4_cq_free(mdev->dev, &cq->mcq);
+	mlx4_cq_free(priv->mdev->dev, &cq->mcq);
 }
 
 /* Set rx cq moderation parameters */
diff --git a/drivers/net/ethernet/mellanox/mlx4/en_dcb_nl.c b/drivers/net/ethernet/mellanox/mlx4/en_dcb_nl.c
new file mode 100644
index 00000000000..5d36795877c
--- /dev/null
+++ b/drivers/net/ethernet/mellanox/mlx4/en_dcb_nl.c
@@ -0,0 +1,255 @@
+/*
+ * Copyright (c) 2011 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/dcbnl.h>
+#include <linux/math64.h>
+
+#include "mlx4_en.h"
+
+static int mlx4_en_dcbnl_ieee_getets(struct net_device *dev,
+				   struct ieee_ets *ets)
+{
+	struct mlx4_en_priv *priv = netdev_priv(dev);
+	struct ieee_ets *my_ets = &priv->ets;
+
+	/* No IEEE PFC settings available */
+	if (!my_ets)
+		return -EINVAL;
+
+	ets->ets_cap = IEEE_8021QAZ_MAX_TCS;
+	ets->cbs = my_ets->cbs;
+	memcpy(ets->tc_tx_bw, my_ets->tc_tx_bw, sizeof(ets->tc_tx_bw));
+	memcpy(ets->tc_tsa, my_ets->tc_tsa, sizeof(ets->tc_tsa));
+	memcpy(ets->prio_tc, my_ets->prio_tc, sizeof(ets->prio_tc));
+
+	return 0;
+}
+
+static int mlx4_en_ets_validate(struct mlx4_en_priv *priv, struct ieee_ets *ets)
+{
+	int i;
+	int total_ets_bw = 0;
+	int has_ets_tc = 0;
+
+	for (i = 0; i < IEEE_8021QAZ_MAX_TCS; i++) {
+		if (ets->prio_tc[i] > MLX4_EN_NUM_UP) {
+			en_err(priv, "Bad priority in UP <=> TC mapping. TC: %d, UP: %d\n",
+					i, ets->prio_tc[i]);
+			return -EINVAL;
+		}
+
+		switch (ets->tc_tsa[i]) {
+		case IEEE_8021QAZ_TSA_STRICT:
+			break;
+		case IEEE_8021QAZ_TSA_ETS:
+			has_ets_tc = 1;
+			total_ets_bw += ets->tc_tx_bw[i];
+			break;
+		default:
+			en_err(priv, "TC[%d]: Not supported TSA: %d\n",
+					i, ets->tc_tsa[i]);
+			return -ENOTSUPP;
+		}
+	}
+
+	if (has_ets_tc && total_ets_bw != MLX4_EN_BW_MAX) {
+		en_err(priv, "Bad ETS BW sum: %d. Should be exactly 100%%\n",
+				total_ets_bw);
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+static int mlx4_en_config_port_scheduler(struct mlx4_en_priv *priv,
+		struct ieee_ets *ets, u16 *ratelimit)
+{
+	struct mlx4_en_dev *mdev = priv->mdev;
+	int num_strict = 0;
+	int i;
+	__u8 tc_tx_bw[IEEE_8021QAZ_MAX_TCS] = { 0 };
+	__u8 pg[IEEE_8021QAZ_MAX_TCS] = { 0 };
+
+	ets = ets ?: &priv->ets;
+	ratelimit = ratelimit ?: priv->maxrate;
+
+	/* higher TC means higher priority => lower pg */
+	for (i = IEEE_8021QAZ_MAX_TCS - 1; i >= 0; i--) {
+		switch (ets->tc_tsa[i]) {
+		case IEEE_8021QAZ_TSA_STRICT:
+			pg[i] = num_strict++;
+			tc_tx_bw[i] = MLX4_EN_BW_MAX;
+			break;
+		case IEEE_8021QAZ_TSA_ETS:
+			pg[i] = MLX4_EN_TC_ETS;
+			tc_tx_bw[i] = ets->tc_tx_bw[i] ?: MLX4_EN_BW_MIN;
+			break;
+		}
+	}
+
+	return mlx4_SET_PORT_SCHEDULER(mdev->dev, priv->port, tc_tx_bw, pg,
+			ratelimit);
+}
+
+static int
+mlx4_en_dcbnl_ieee_setets(struct net_device *dev, struct ieee_ets *ets)
+{
+	struct mlx4_en_priv *priv = netdev_priv(dev);
+	struct mlx4_en_dev *mdev = priv->mdev;
+	int err;
+
+	err = mlx4_en_ets_validate(priv, ets);
+	if (err)
+		return err;
+
+	err = mlx4_SET_PORT_PRIO2TC(mdev->dev, priv->port, ets->prio_tc);
+	if (err)
+		return err;
+
+	err = mlx4_en_config_port_scheduler(priv, ets, NULL);
+	if (err)
+		return err;
+
+	memcpy(&priv->ets, ets, sizeof(priv->ets));
+
+	return 0;
+}
+
+static int mlx4_en_dcbnl_ieee_getpfc(struct net_device *dev,
+		struct ieee_pfc *pfc)
+{
+	struct mlx4_en_priv *priv = netdev_priv(dev);
+
+	pfc->pfc_cap = IEEE_8021QAZ_MAX_TCS;
+	pfc->pfc_en = priv->prof->tx_ppp;
+
+	return 0;
+}
+
+static int mlx4_en_dcbnl_ieee_setpfc(struct net_device *dev,
+		struct ieee_pfc *pfc)
+{
+	struct mlx4_en_priv *priv = netdev_priv(dev);
+	struct mlx4_en_dev *mdev = priv->mdev;
+	int err;
+
+	en_dbg(DRV, priv, "cap: 0x%x en: 0x%x mbc: 0x%x delay: %d\n",
+			pfc->pfc_cap,
+			pfc->pfc_en,
+			pfc->mbc,
+			pfc->delay);
+
+	priv->prof->rx_pause = priv->prof->tx_pause = !!pfc->pfc_en;
+	priv->prof->rx_ppp = priv->prof->tx_ppp = pfc->pfc_en;
+
+	err = mlx4_SET_PORT_general(mdev->dev, priv->port,
+				    priv->rx_skb_size + ETH_FCS_LEN,
+				    priv->prof->tx_pause,
+				    priv->prof->tx_ppp,
+				    priv->prof->rx_pause,
+				    priv->prof->rx_ppp);
+	if (err)
+		en_err(priv, "Failed setting pause params\n");
+
+	return err;
+}
+
+static u8 mlx4_en_dcbnl_getdcbx(struct net_device *dev)
+{
+	return DCB_CAP_DCBX_VER_IEEE;
+}
+
+static u8 mlx4_en_dcbnl_setdcbx(struct net_device *dev, u8 mode)
+{
+	if ((mode & DCB_CAP_DCBX_LLD_MANAGED) ||
+	    (mode & DCB_CAP_DCBX_VER_CEE) ||
+	    !(mode & DCB_CAP_DCBX_VER_IEEE) ||
+	    !(mode & DCB_CAP_DCBX_HOST))
+		return 1;
+
+	return 0;
+}
+
+#define MLX4_RATELIMIT_UNITS_IN_KB 100000 /* rate-limit HW unit in Kbps */
+static int mlx4_en_dcbnl_ieee_getmaxrate(struct net_device *dev,
+				   struct ieee_maxrate *maxrate)
+{
+	struct mlx4_en_priv *priv = netdev_priv(dev);
+	int i;
+
+	if (!priv->maxrate)
+		return -EINVAL;
+
+	for (i = 0; i < IEEE_8021QAZ_MAX_TCS; i++)
+		maxrate->tc_maxrate[i] =
+			priv->maxrate[i] * MLX4_RATELIMIT_UNITS_IN_KB;
+
+	return 0;
+}
+
+static int mlx4_en_dcbnl_ieee_setmaxrate(struct net_device *dev,
+		struct ieee_maxrate *maxrate)
+{
+	struct mlx4_en_priv *priv = netdev_priv(dev);
+	u16 tmp[IEEE_8021QAZ_MAX_TCS];
+	int i, err;
+
+	for (i = 0; i < IEEE_8021QAZ_MAX_TCS; i++) {
+		/* Convert from Kbps into HW units, rounding result up.
+		 * Setting to 0, means unlimited BW.
+		 */
+		tmp[i] = div_u64(maxrate->tc_maxrate[i] +
+				 MLX4_RATELIMIT_UNITS_IN_KB - 1,
+				 MLX4_RATELIMIT_UNITS_IN_KB);
+	}
+
+	err = mlx4_en_config_port_scheduler(priv, NULL, tmp);
+	if (err)
+		return err;
+
+	memcpy(priv->maxrate, tmp, sizeof(*priv->maxrate));
+
+	return 0;
+}
+
+const struct dcbnl_rtnl_ops mlx4_en_dcbnl_ops = {
+	.ieee_getets	= mlx4_en_dcbnl_ieee_getets,
+	.ieee_setets	= mlx4_en_dcbnl_ieee_setets,
+	.ieee_getmaxrate = mlx4_en_dcbnl_ieee_getmaxrate,
+	.ieee_setmaxrate = mlx4_en_dcbnl_ieee_setmaxrate,
+	.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 70346fd7f9c..72901ce2b08 100644
--- a/drivers/net/ethernet/mellanox/mlx4/en_ethtool.c
+++ b/drivers/net/ethernet/mellanox/mlx4/en_ethtool.c
@@ -83,7 +83,7 @@ static const char main_strings[][ETH_GSTRING_LEN] = {
 #define NUM_ALL_STATS	(NUM_MAIN_STATS + NUM_PORT_STATS + NUM_PKT_STATS + NUM_PERF_STATS)
 
 static const char mlx4_en_test_names[][ETH_GSTRING_LEN]= {
-	"Interupt Test",
+	"Interrupt Test",
 	"Link Test",
 	"Speed Test",
 	"Register Test",
@@ -359,8 +359,8 @@ static int mlx4_en_get_coalesce(struct net_device *dev,
 {
 	struct mlx4_en_priv *priv = netdev_priv(dev);
 
-	coal->tx_coalesce_usecs = 0;
-	coal->tx_max_coalesced_frames = 0;
+	coal->tx_coalesce_usecs = priv->tx_usecs;
+	coal->tx_max_coalesced_frames = priv->tx_frames;
 	coal->rx_coalesce_usecs = priv->rx_usecs;
 	coal->rx_max_coalesced_frames = priv->rx_frames;
 
@@ -388,6 +388,21 @@ static int mlx4_en_set_coalesce(struct net_device *dev,
 				MLX4_EN_RX_COAL_TIME :
 				coal->rx_coalesce_usecs;
 
+	/* Setting TX coalescing parameters */
+	if (coal->tx_coalesce_usecs != priv->tx_usecs ||
+	    coal->tx_max_coalesced_frames != priv->tx_frames) {
+		priv->tx_usecs = coal->tx_coalesce_usecs;
+		priv->tx_frames = coal->tx_max_coalesced_frames;
+		for (i = 0; i < priv->tx_ring_num; i++) {
+			priv->tx_cq[i].moder_cnt = priv->tx_frames;
+			priv->tx_cq[i].moder_time = priv->tx_usecs;
+			if (mlx4_en_set_cq_moder(priv, &priv->tx_cq[i])) {
+				en_warn(priv, "Failed changing moderation "
+					      "for TX cq %d\n", i);
+			}
+		}
+	}
+
 	/* Set adaptive coalescing params */
 	priv->pkt_rate_low = coal->pkt_rate_low;
 	priv->rx_usecs_low = coal->rx_coalesce_usecs_low;
diff --git a/drivers/net/ethernet/mellanox/mlx4/en_main.c b/drivers/net/ethernet/mellanox/mlx4/en_main.c
index 2097a7d3c5b..988b2424e1c 100644
--- a/drivers/net/ethernet/mellanox/mlx4/en_main.c
+++ b/drivers/net/ethernet/mellanox/mlx4/en_main.c
@@ -101,6 +101,8 @@ static int mlx4_en_get_profile(struct mlx4_en_dev *mdev)
 	int i;
 
 	params->udp_rss = udp_rss;
+	params->num_tx_rings_p_up = min_t(int, num_online_cpus(),
+			MLX4_EN_MAX_TX_RING_P_UP);
 	if (params->udp_rss && !(mdev->dev->caps.flags
 					& MLX4_DEV_CAP_FLAG_UDP_RSS)) {
 		mlx4_warn(mdev, "UDP RSS is not supported on this device.\n");
@@ -113,8 +115,8 @@ static int mlx4_en_get_profile(struct mlx4_en_dev *mdev)
 		params->prof[i].tx_ppp = pfctx;
 		params->prof[i].tx_ring_size = MLX4_EN_DEF_TX_RING_SIZE;
 		params->prof[i].rx_ring_size = MLX4_EN_DEF_RX_RING_SIZE;
-		params->prof[i].tx_ring_num = MLX4_EN_NUM_TX_RINGS +
-			(!!pfcrx) * MLX4_EN_NUM_PPP_RINGS;
+		params->prof[i].tx_ring_num = params->num_tx_rings_p_up *
+			MLX4_EN_NUM_UP;
 		params->prof[i].rss_rings = 0;
 	}
 
diff --git a/drivers/net/ethernet/mellanox/mlx4/en_netdev.c b/drivers/net/ethernet/mellanox/mlx4/en_netdev.c
index 31b455a4927..926d8aac941 100644
--- a/drivers/net/ethernet/mellanox/mlx4/en_netdev.c
+++ b/drivers/net/ethernet/mellanox/mlx4/en_netdev.c
@@ -45,6 +45,27 @@
 #include "mlx4_en.h"
 #include "en_port.h"
 
+static int mlx4_en_setup_tc(struct net_device *dev, u8 up)
+{
+	struct mlx4_en_priv *priv = netdev_priv(dev);
+	int i;
+	unsigned int q, offset = 0;
+
+	if (up && up != MLX4_EN_NUM_UP)
+		return -EINVAL;
+
+	netdev_set_num_tc(dev, up);
+
+	/* Partition Tx queues evenly amongst UP's */
+	q = priv->tx_ring_num / up;
+	for (i = 0; i < up; i++) {
+		netdev_set_tc_queue(dev, i, q, offset);
+		offset += q;
+	}
+
+	return 0;
+}
+
 static int mlx4_en_vlan_rx_add_vid(struct net_device *dev, unsigned short vid)
 {
 	struct mlx4_en_priv *priv = netdev_priv(dev);
@@ -421,6 +442,8 @@ static void mlx4_en_set_default_moderation(struct mlx4_en_priv *priv)
 	 */
 	priv->rx_frames = MLX4_EN_RX_COAL_TARGET;
 	priv->rx_usecs = MLX4_EN_RX_COAL_TIME;
+	priv->tx_frames = MLX4_EN_TX_COAL_PKTS;
+	priv->tx_usecs = MLX4_EN_TX_COAL_TIME;
 	en_dbg(INTR, priv, "Default coalesing params for mtu:%d - "
 			   "rx_frames:%d rx_usecs:%d\n",
 		 priv->dev->mtu, priv->rx_frames, priv->rx_usecs);
@@ -437,8 +460,8 @@ static void mlx4_en_set_default_moderation(struct mlx4_en_priv *priv)
 
 	for (i = 0; i < priv->tx_ring_num; i++) {
 		cq = &priv->tx_cq[i];
-		cq->moder_cnt = MLX4_EN_TX_COAL_PKTS;
-		cq->moder_time = MLX4_EN_TX_COAL_TIME;
+		cq->moder_cnt = priv->tx_frames;
+		cq->moder_time = priv->tx_usecs;
 	}
 
 	/* Reset auto-moderation params */
@@ -650,12 +673,18 @@ int mlx4_en_start_port(struct net_device *dev)
 
 		/* Configure ring */
 		tx_ring = &priv->tx_ring[i];
-		err = mlx4_en_activate_tx_ring(priv, tx_ring, cq->mcq.cqn);
+		err = mlx4_en_activate_tx_ring(priv, tx_ring, cq->mcq.cqn,
+			i / priv->mdev->profile.num_tx_rings_p_up);
 		if (err) {
 			en_err(priv, "Failed allocating Tx ring\n");
 			mlx4_en_deactivate_cq(priv, cq);
 			goto tx_err;
 		}
+		tx_ring->tx_queue = netdev_get_tx_queue(dev, i);
+
+		/* Arm CQ for TX completions */
+		mlx4_en_arm_cq(priv, cq);
+
 		/* Set initial ownership of all Tx TXBBs to SW (1) */
 		for (j = 0; j < tx_ring->buf_size; j += STAMP_STRIDE)
 			*((u32 *) (tx_ring->buf + j)) = 0xffffffff;
@@ -797,12 +826,15 @@ static void mlx4_en_restart(struct work_struct *work)
 						 watchdog_task);
 	struct mlx4_en_dev *mdev = priv->mdev;
 	struct net_device *dev = priv->dev;
+	int i;
 
 	en_dbg(DRV, priv, "Watchdog task called for port %d\n", priv->port);
 
 	mutex_lock(&mdev->state_lock);
 	if (priv->port_up) {
 		mlx4_en_stop_port(dev);
+		for (i = 0; i < priv->tx_ring_num; i++)
+			netdev_tx_reset_queue(priv->tx_ring[i].tx_queue);
 		if (mlx4_en_start_port(dev))
 			en_err(priv, "Failed restarting port %d\n", priv->port);
 	}
@@ -966,6 +998,10 @@ void mlx4_en_destroy_netdev(struct net_device *dev)
 	mutex_unlock(&mdev->state_lock);
 
 	mlx4_en_free_resources(priv);
+
+	kfree(priv->tx_ring);
+	kfree(priv->tx_cq);
+
 	free_netdev(dev);
 }
 
@@ -1036,6 +1072,7 @@ static const struct net_device_ops mlx4_netdev_ops = {
 	.ndo_poll_controller	= mlx4_en_netpoll,
 #endif
 	.ndo_set_features	= mlx4_en_set_features,
+	.ndo_setup_tc		= mlx4_en_setup_tc,
 };
 
 int mlx4_en_init_netdev(struct mlx4_en_dev *mdev, int port,
@@ -1070,6 +1107,18 @@ int mlx4_en_init_netdev(struct mlx4_en_dev *mdev, int port,
 	priv->ctrl_flags = cpu_to_be32(MLX4_WQE_CTRL_CQ_UPDATE |
 			MLX4_WQE_CTRL_SOLICITED);
 	priv->tx_ring_num = prof->tx_ring_num;
+	priv->tx_ring = kzalloc(sizeof(struct mlx4_en_tx_ring) *
+			priv->tx_ring_num, GFP_KERNEL);
+	if (!priv->tx_ring) {
+		err = -ENOMEM;
+		goto out;
+	}
+	priv->tx_cq = kzalloc(sizeof(struct mlx4_en_cq) * priv->tx_ring_num,
+			GFP_KERNEL);
+	if (!priv->tx_cq) {
+		err = -ENOMEM;
+		goto out;
+	}
 	priv->rx_ring_num = prof->rx_ring_num;
 	priv->mac_index = -1;
 	priv->msg_enable = MLX4_EN_MSG_LEVEL;
@@ -1079,6 +1128,10 @@ 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);
+#ifdef CONFIG_MLX4_EN_DCB
+	if (!mlx4_is_slave(priv->mdev->dev))
+		dev->dcbnl_ops = &mlx4_en_dcbnl_ops;
+#endif
 
 	/* Query for default mac and max mtu */
 	priv->max_mtu = mdev->dev->caps.eth_mtu_cap[priv->port];
diff --git a/drivers/net/ethernet/mellanox/mlx4/en_port.h b/drivers/net/ethernet/mellanox/mlx4/en_port.h
index 6934fd7e66e..745090b49d9 100644
--- a/drivers/net/ethernet/mellanox/mlx4/en_port.h
+++ b/drivers/net/ethernet/mellanox/mlx4/en_port.h
@@ -39,6 +39,8 @@
 #define SET_PORT_PROMISC_SHIFT	31
 #define SET_PORT_MC_PROMISC_SHIFT	30
 
+#define MLX4_EN_NUM_TC		8
+
 #define VLAN_FLTR_SIZE	128
 struct mlx4_set_vlan_fltr_mbox {
 	__be32 entry[VLAN_FLTR_SIZE];
diff --git a/drivers/net/ethernet/mellanox/mlx4/en_resources.c b/drivers/net/ethernet/mellanox/mlx4/en_resources.c
index bcbc54c1694..10c24c784b7 100644
--- a/drivers/net/ethernet/mellanox/mlx4/en_resources.c
+++ b/drivers/net/ethernet/mellanox/mlx4/en_resources.c
@@ -39,7 +39,7 @@
 
 void mlx4_en_fill_qp_context(struct mlx4_en_priv *priv, int size, int stride,
 			     int is_tx, int rss, int qpn, int cqn,
-			     struct mlx4_qp_context *context)
+			     int user_prio, struct mlx4_qp_context *context)
 {
 	struct mlx4_en_dev *mdev = priv->mdev;
 
@@ -57,6 +57,10 @@ void mlx4_en_fill_qp_context(struct mlx4_en_priv *priv, int size, int stride,
 	context->local_qpn = cpu_to_be32(qpn);
 	context->pri_path.ackto = 1 & 0x07;
 	context->pri_path.sched_queue = 0x83 | (priv->port - 1) << 6;
+	if (user_prio >= 0) {
+		context->pri_path.sched_queue |= user_prio << 3;
+		context->pri_path.feup = 1 << 6;
+	}
 	context->pri_path.counter_index = 0xff;
 	context->cqn_send = cpu_to_be32(cqn);
 	context->cqn_recv = cpu_to_be32(cqn);
diff --git a/drivers/net/ethernet/mellanox/mlx4/en_rx.c b/drivers/net/ethernet/mellanox/mlx4/en_rx.c
index 9adbd53da52..d49a7ac3187 100644
--- a/drivers/net/ethernet/mellanox/mlx4/en_rx.c
+++ b/drivers/net/ethernet/mellanox/mlx4/en_rx.c
@@ -823,7 +823,7 @@ static int mlx4_en_config_rss_qp(struct mlx4_en_priv *priv, int qpn,
 
 	memset(context, 0, sizeof *context);
 	mlx4_en_fill_qp_context(priv, ring->actual_size, ring->stride, 0, 0,
-				qpn, ring->cqn, context);
+				qpn, ring->cqn, -1, context);
 	context->db_rec_addr = cpu_to_be64(ring->wqres.db.dma);
 
 	/* Cancel FCS removal if FW allows */
@@ -890,7 +890,7 @@ int mlx4_en_config_rss_steer(struct mlx4_en_priv *priv)
 	}
 	rss_map->indir_qp.event = mlx4_en_sqp_event;
 	mlx4_en_fill_qp_context(priv, 0, 0, 0, 1, priv->base_qpn,
-				priv->rx_ring[0].cqn, &context);
+				priv->rx_ring[0].cqn, -1, &context);
 
 	if (!priv->prof->rss_rings || priv->prof->rss_rings > priv->rx_ring_num)
 		rss_rings = priv->rx_ring_num;
diff --git a/drivers/net/ethernet/mellanox/mlx4/en_tx.c b/drivers/net/ethernet/mellanox/mlx4/en_tx.c
index 17968244c39..019d856b133 100644
--- a/drivers/net/ethernet/mellanox/mlx4/en_tx.c
+++ b/drivers/net/ethernet/mellanox/mlx4/en_tx.c
@@ -67,8 +67,6 @@ int mlx4_en_create_tx_ring(struct mlx4_en_priv *priv,
 
 	inline_thold = min(inline_thold, MAX_INLINE);
 
-	spin_lock_init(&ring->comp_lock);
-
 	tmp = size * sizeof(struct mlx4_en_tx_info);
 	ring->tx_info = vmalloc(tmp);
 	if (!ring->tx_info)
@@ -156,7 +154,7 @@ void mlx4_en_destroy_tx_ring(struct mlx4_en_priv *priv,
 
 int mlx4_en_activate_tx_ring(struct mlx4_en_priv *priv,
 			     struct mlx4_en_tx_ring *ring,
-			     int cq)
+			     int cq, int user_prio)
 {
 	struct mlx4_en_dev *mdev = priv->mdev;
 	int err;
@@ -174,7 +172,7 @@ int mlx4_en_activate_tx_ring(struct mlx4_en_priv *priv,
 	ring->doorbell_qpn = ring->qp.qpn << 8;
 
 	mlx4_en_fill_qp_context(priv, ring->size, ring->stride, 1, 0, ring->qpn,
-				ring->cqn, &ring->context);
+				ring->cqn, user_prio, &ring->context);
 	if (ring->bf_enabled)
 		ring->context.usr_page = cpu_to_be32(ring->bf.uar->index);
 
@@ -317,6 +315,8 @@ static void mlx4_en_process_tx_cq(struct net_device *dev, struct mlx4_en_cq *cq)
 	int size = cq->size;
 	u32 size_mask = ring->size_mask;
 	struct mlx4_cqe *buf = cq->buf;
+	u32 packets = 0;
+	u32 bytes = 0;
 
 	if (!priv->port_up)
 		return;
@@ -345,6 +345,8 @@ static void mlx4_en_process_tx_cq(struct net_device *dev, struct mlx4_en_cq *cq)
 					priv, ring, ring_index,
 					!!((ring->cons + txbbs_skipped) &
 							ring->size));
+			packets++;
+			bytes += ring->tx_info[ring_index].nr_bytes;
 		} while (ring_index != new_index);
 
 		++cons_index;
@@ -361,13 +363,14 @@ static void mlx4_en_process_tx_cq(struct net_device *dev, struct mlx4_en_cq *cq)
 	mlx4_cq_set_ci(mcq);
 	wmb();
 	ring->cons += txbbs_skipped;
+	netdev_tx_completed_queue(ring->tx_queue, packets, bytes);
 
 	/* Wakeup Tx queue if this ring stopped it */
 	if (unlikely(ring->blocked)) {
 		if ((u32) (ring->prod - ring->cons) <=
 		     ring->size - HEADROOM - MAX_DESC_TXBBS) {
 			ring->blocked = 0;
-			netif_tx_wake_queue(netdev_get_tx_queue(dev, cq->ring));
+			netif_tx_wake_queue(ring->tx_queue);
 			priv->port_stats.wake_queue++;
 		}
 	}
@@ -377,41 +380,12 @@ void mlx4_en_tx_irq(struct mlx4_cq *mcq)
 {
 	struct mlx4_en_cq *cq = container_of(mcq, struct mlx4_en_cq, mcq);
 	struct mlx4_en_priv *priv = netdev_priv(cq->dev);
-	struct mlx4_en_tx_ring *ring = &priv->tx_ring[cq->ring];
 
-	if (!spin_trylock(&ring->comp_lock))
-		return;
 	mlx4_en_process_tx_cq(cq->dev, cq);
-	mod_timer(&cq->timer, jiffies + 1);
-	spin_unlock(&ring->comp_lock);
+	mlx4_en_arm_cq(priv, cq);
 }
 
 
-void mlx4_en_poll_tx_cq(unsigned long data)
-{
-	struct mlx4_en_cq *cq = (struct mlx4_en_cq *) data;
-	struct mlx4_en_priv *priv = netdev_priv(cq->dev);
-	struct mlx4_en_tx_ring *ring = &priv->tx_ring[cq->ring];
-	u32 inflight;
-
-	INC_PERF_COUNTER(priv->pstats.tx_poll);
-
-	if (!spin_trylock_irq(&ring->comp_lock)) {
-		mod_timer(&cq->timer, jiffies + MLX4_EN_TX_POLL_TIMEOUT);
-		return;
-	}
-	mlx4_en_process_tx_cq(cq->dev, cq);
-	inflight = (u32) (ring->prod - ring->cons - ring->last_nr_txbb);
-
-	/* If there are still packets in flight and the timer has not already
-	 * been scheduled by the Tx routine then schedule it here to guarantee
-	 * completion processing of these packets */
-	if (inflight && priv->port_up)
-		mod_timer(&cq->timer, jiffies + MLX4_EN_TX_POLL_TIMEOUT);
-
-	spin_unlock_irq(&ring->comp_lock);
-}
-
 static struct mlx4_en_tx_desc *mlx4_en_bounce_to_desc(struct mlx4_en_priv *priv,
 						      struct mlx4_en_tx_ring *ring,
 						      u32 index,
@@ -440,25 +414,6 @@ static struct mlx4_en_tx_desc *mlx4_en_bounce_to_desc(struct mlx4_en_priv *priv,
 	return ring->buf + index * TXBB_SIZE;
 }
 
-static inline void mlx4_en_xmit_poll(struct mlx4_en_priv *priv, int tx_ind)
-{
-	struct mlx4_en_cq *cq = &priv->tx_cq[tx_ind];
-	struct mlx4_en_tx_ring *ring = &priv->tx_ring[tx_ind];
-	unsigned long flags;
-
-	/* If we don't have a pending timer, set one up to catch our recent
-	   post in case the interface becomes idle */
-	if (!timer_pending(&cq->timer))
-		mod_timer(&cq->timer, jiffies + MLX4_EN_TX_POLL_TIMEOUT);
-
-	/* Poll the CQ every mlx4_en_TX_MODER_POLL packets */
-	if ((++ring->poll_cnt & (MLX4_EN_TX_POLL_MODER - 1)) == 0)
-		if (spin_trylock_irqsave(&ring->comp_lock, flags)) {
-			mlx4_en_process_tx_cq(priv->dev, cq);
-			spin_unlock_irqrestore(&ring->comp_lock, flags);
-		}
-}
-
 static int is_inline(struct sk_buff *skb, void **pfrag)
 {
 	void *ptr;
@@ -571,17 +526,16 @@ static void build_inline_wqe(struct mlx4_en_tx_desc *tx_desc, struct sk_buff *sk
 u16 mlx4_en_select_queue(struct net_device *dev, struct sk_buff *skb)
 {
 	struct mlx4_en_priv *priv = netdev_priv(dev);
-	u16 vlan_tag = 0;
+	u16 rings_p_up = priv->mdev->profile.num_tx_rings_p_up;
+	u8 up = 0;
 
-	/* If we support per priority flow control and the packet contains
-	 * a vlan tag, send the packet to the TX ring assigned to that priority
-	 */
-	if (priv->prof->rx_ppp && vlan_tx_tag_present(skb)) {
-		vlan_tag = vlan_tx_tag_get(skb);
-		return MLX4_EN_NUM_TX_RINGS + (vlan_tag >> 13);
-	}
+	if (dev->num_tc)
+		return skb_tx_hash(dev, skb);
 
-	return skb_tx_hash(dev, skb);
+	if (vlan_tx_tag_present(skb))
+		up = vlan_tx_tag_get(skb) >> VLAN_PRIO_SHIFT;
+
+	return __skb_tx_hash(dev, skb, rings_p_up) + up * rings_p_up;
 }
 
 static void mlx4_bf_copy(void __iomem *dst, unsigned long *src, unsigned bytecnt)
@@ -594,7 +548,6 @@ netdev_tx_t mlx4_en_xmit(struct sk_buff *skb, struct net_device *dev)
 	struct mlx4_en_priv *priv = netdev_priv(dev);
 	struct mlx4_en_dev *mdev = priv->mdev;
 	struct mlx4_en_tx_ring *ring;
-	struct mlx4_en_cq *cq;
 	struct mlx4_en_tx_desc *tx_desc;
 	struct mlx4_wqe_data_seg *data;
 	struct skb_frag_struct *frag;
@@ -638,13 +591,10 @@ netdev_tx_t mlx4_en_xmit(struct sk_buff *skb, struct net_device *dev)
 	if (unlikely(((int)(ring->prod - ring->cons)) >
 		     ring->size - HEADROOM - MAX_DESC_TXBBS)) {
 		/* every full Tx ring stops queue */
-		netif_tx_stop_queue(netdev_get_tx_queue(dev, tx_ind));
+		netif_tx_stop_queue(ring->tx_queue);
 		ring->blocked = 1;
 		priv->port_stats.queue_stopped++;
 
-		/* Use interrupts to find out when queue opened */
-		cq = &priv->tx_cq[tx_ind];
-		mlx4_en_arm_cq(priv, cq);
 		return NETDEV_TX_BUSY;
 	}
 
@@ -707,7 +657,7 @@ netdev_tx_t mlx4_en_xmit(struct sk_buff *skb, struct net_device *dev)
 		priv->port_stats.tso_packets++;
 		i = ((skb->len - lso_header_size) / skb_shinfo(skb)->gso_size) +
 			!!((skb->len - lso_header_size) % skb_shinfo(skb)->gso_size);
-		ring->bytes += skb->len + (i - 1) * lso_header_size;
+		tx_info->nr_bytes = skb->len + (i - 1) * lso_header_size;
 		ring->packets += i;
 	} else {
 		/* Normal (Non LSO) packet */
@@ -715,10 +665,12 @@ netdev_tx_t mlx4_en_xmit(struct sk_buff *skb, struct net_device *dev)
 			((ring->prod & ring->size) ?
 			 cpu_to_be32(MLX4_EN_BIT_DESC_OWN) : 0);
 		data = &tx_desc->data;
-		ring->bytes += max(skb->len, (unsigned int) ETH_ZLEN);
+		tx_info->nr_bytes = max_t(unsigned int, skb->len, ETH_ZLEN);
 		ring->packets++;
 
 	}
+	ring->bytes += tx_info->nr_bytes;
+	netdev_tx_sent_queue(ring->tx_queue, tx_info->nr_bytes);
 	AVG_PERF_COUNTER(priv->pstats.tx_pktsz_avg, skb->len);
 
 
@@ -792,9 +744,6 @@ netdev_tx_t mlx4_en_xmit(struct sk_buff *skb, struct net_device *dev)
 		iowrite32be(ring->doorbell_qpn, ring->bf.uar->map + MLX4_SEND_DOORBELL);
 	}
 
-	/* Poll CQ here */
-	mlx4_en_xmit_poll(priv, tx_ind);
-
 	return NETDEV_TX_OK;
 
 tx_drop:
diff --git a/drivers/net/ethernet/mellanox/mlx4/fw.c b/drivers/net/ethernet/mellanox/mlx4/fw.c
index 2a02ba522e6..68f5cd6cb3c 100644
--- a/drivers/net/ethernet/mellanox/mlx4/fw.c
+++ b/drivers/net/ethernet/mellanox/mlx4/fw.c
@@ -118,6 +118,20 @@ static void dump_dev_cap_flags(struct mlx4_dev *dev, u64 flags)
 			mlx4_dbg(dev, "    %s\n", fname[i]);
 }
 
+static void dump_dev_cap_flags2(struct mlx4_dev *dev, u64 flags)
+{
+	static const char * const fname[] = {
+		[0] = "RSS support",
+		[1] = "RSS Toeplitz Hash Function support",
+		[2] = "RSS XOR Hash Function support"
+	};
+	int i;
+
+	for (i = 0; i < ARRAY_SIZE(fname); ++i)
+		if (fname[i] && (flags & (1LL << i)))
+			mlx4_dbg(dev, "    %s\n", fname[i]);
+}
+
 int mlx4_MOD_STAT_CFG(struct mlx4_dev *dev, struct mlx4_mod_stat_cfg *cfg)
 {
 	struct mlx4_cmd_mailbox *mailbox;
@@ -346,6 +360,7 @@ int mlx4_QUERY_DEV_CAP(struct mlx4_dev *dev, struct mlx4_dev_cap *dev_cap)
 #define QUERY_DEV_CAP_MAX_REQ_QP_OFFSET		0x29
 #define QUERY_DEV_CAP_MAX_RES_QP_OFFSET		0x2b
 #define QUERY_DEV_CAP_MAX_GSO_OFFSET		0x2d
+#define QUERY_DEV_CAP_RSS_OFFSET		0x2e
 #define QUERY_DEV_CAP_MAX_RDMA_OFFSET		0x2f
 #define QUERY_DEV_CAP_RSZ_SRQ_OFFSET		0x33
 #define QUERY_DEV_CAP_ACK_DELAY_OFFSET		0x35
@@ -390,6 +405,7 @@ int mlx4_QUERY_DEV_CAP(struct mlx4_dev *dev, struct mlx4_dev_cap *dev_cap)
 #define QUERY_DEV_CAP_RSVD_LKEY_OFFSET		0x98
 #define QUERY_DEV_CAP_MAX_ICM_SZ_OFFSET		0xa0
 
+	dev_cap->flags2 = 0;
 	mailbox = mlx4_alloc_cmd_mailbox(dev);
 	if (IS_ERR(mailbox))
 		return PTR_ERR(mailbox);
@@ -439,6 +455,17 @@ int mlx4_QUERY_DEV_CAP(struct mlx4_dev *dev, struct mlx4_dev_cap *dev_cap)
 	else
 		dev_cap->max_gso_sz = 1 << field;
 
+	MLX4_GET(field, outbox, QUERY_DEV_CAP_RSS_OFFSET);
+	if (field & 0x20)
+		dev_cap->flags2 |= MLX4_DEV_CAP_FLAG2_RSS_XOR;
+	if (field & 0x10)
+		dev_cap->flags2 |= MLX4_DEV_CAP_FLAG2_RSS_TOP;
+	field &= 0xf;
+	if (field) {
+		dev_cap->flags2 |= MLX4_DEV_CAP_FLAG2_RSS;
+		dev_cap->max_rss_tbl_sz = 1 << field;
+	} else
+		dev_cap->max_rss_tbl_sz = 0;
 	MLX4_GET(field, outbox, QUERY_DEV_CAP_MAX_RDMA_OFFSET);
 	dev_cap->max_rdma_global = 1 << (field & 0x3f);
 	MLX4_GET(field, outbox, QUERY_DEV_CAP_ACK_DELAY_OFFSET);
@@ -632,8 +659,10 @@ int mlx4_QUERY_DEV_CAP(struct mlx4_dev *dev, struct mlx4_dev_cap *dev_cap)
 		 dev_cap->max_rq_desc_sz, dev_cap->max_rq_sg);
 	mlx4_dbg(dev, "Max GSO size: %d\n", dev_cap->max_gso_sz);
 	mlx4_dbg(dev, "Max counters: %d\n", dev_cap->max_counters);
+	mlx4_dbg(dev, "Max RSS Table size: %d\n", dev_cap->max_rss_tbl_sz);
 
 	dump_dev_cap_flags(dev, dev_cap->flags);
+	dump_dev_cap_flags2(dev, dev_cap->flags2);
 
 out:
 	mlx4_free_cmd_mailbox(dev, mailbox);
@@ -1164,9 +1193,8 @@ int mlx4_INIT_PORT_wrapper(struct mlx4_dev *dev, int slave,
 			       MLX4_CMD_TIME_CLASS_A, MLX4_CMD_NATIVE);
 		if (err)
 			return err;
-		priv->mfunc.master.slave_state[slave].init_port_mask |=
-			(1 << port);
 	}
+	priv->mfunc.master.slave_state[slave].init_port_mask |= (1 << port);
 	++priv->mfunc.master.init_port_ref[port];
 	return 0;
 }
diff --git a/drivers/net/ethernet/mellanox/mlx4/fw.h b/drivers/net/ethernet/mellanox/mlx4/fw.h
index e1a5fa56bcb..64c0399e4b7 100644
--- a/drivers/net/ethernet/mellanox/mlx4/fw.h
+++ b/drivers/net/ethernet/mellanox/mlx4/fw.h
@@ -79,6 +79,7 @@ struct mlx4_dev_cap {
 	u64 trans_code[MLX4_MAX_PORTS + 1];
 	u16 stat_rate_support;
 	u64 flags;
+	u64 flags2;
 	int reserved_uars;
 	int uar_size;
 	int min_page_sz;
@@ -110,6 +111,7 @@ struct mlx4_dev_cap {
 	u32 reserved_lkey;
 	u64 max_icm_sz;
 	int max_gso_sz;
+	int max_rss_tbl_sz;
 	u8  supported_port_types[MLX4_MAX_PORTS + 1];
 	u8  suggested_type[MLX4_MAX_PORTS + 1];
 	u8  default_sense[MLX4_MAX_PORTS + 1];
diff --git a/drivers/net/ethernet/mellanox/mlx4/main.c b/drivers/net/ethernet/mellanox/mlx4/main.c
index 8bb05b46db8..2e024a68fa8 100644
--- a/drivers/net/ethernet/mellanox/mlx4/main.c
+++ b/drivers/net/ethernet/mellanox/mlx4/main.c
@@ -272,10 +272,12 @@ static int mlx4_dev_cap(struct mlx4_dev *dev, struct mlx4_dev_cap *dev_cap)
 	dev->caps.max_msg_sz         = dev_cap->max_msg_sz;
 	dev->caps.page_size_cap	     = ~(u32) (dev_cap->min_page_sz - 1);
 	dev->caps.flags		     = dev_cap->flags;
+	dev->caps.flags2	     = dev_cap->flags2;
 	dev->caps.bmme_flags	     = dev_cap->bmme_flags;
 	dev->caps.reserved_lkey	     = dev_cap->reserved_lkey;
 	dev->caps.stat_rate_support  = dev_cap->stat_rate_support;
 	dev->caps.max_gso_sz	     = dev_cap->max_gso_sz;
+	dev->caps.max_rss_tbl_sz     = dev_cap->max_rss_tbl_sz;
 
 	/* Sense port always allowed on supported devices for ConnectX1 and 2 */
 	if (dev->pdev->device != 0x1003)
@@ -1306,7 +1308,7 @@ static void mlx4_cleanup_counters_table(struct mlx4_dev *dev)
 	mlx4_bitmap_cleanup(&mlx4_priv(dev)->counters_bitmap);
 }
 
-int mlx4_counter_alloc(struct mlx4_dev *dev, u32 *idx)
+int __mlx4_counter_alloc(struct mlx4_dev *dev, u32 *idx)
 {
 	struct mlx4_priv *priv = mlx4_priv(dev);
 
@@ -1319,13 +1321,44 @@ int mlx4_counter_alloc(struct mlx4_dev *dev, u32 *idx)
 
 	return 0;
 }
+
+int mlx4_counter_alloc(struct mlx4_dev *dev, u32 *idx)
+{
+	u64 out_param;
+	int err;
+
+	if (mlx4_is_mfunc(dev)) {
+		err = mlx4_cmd_imm(dev, 0, &out_param, RES_COUNTER,
+				   RES_OP_RESERVE, MLX4_CMD_ALLOC_RES,
+				   MLX4_CMD_TIME_CLASS_A, MLX4_CMD_WRAPPED);
+		if (!err)
+			*idx = get_param_l(&out_param);
+
+		return err;
+	}
+	return __mlx4_counter_alloc(dev, idx);
+}
 EXPORT_SYMBOL_GPL(mlx4_counter_alloc);
 
-void mlx4_counter_free(struct mlx4_dev *dev, u32 idx)
+void __mlx4_counter_free(struct mlx4_dev *dev, u32 idx)
 {
 	mlx4_bitmap_free(&mlx4_priv(dev)->counters_bitmap, idx);
 	return;
 }
+
+void mlx4_counter_free(struct mlx4_dev *dev, u32 idx)
+{
+	u64 in_param;
+
+	if (mlx4_is_mfunc(dev)) {
+		set_param_l(&in_param, idx);
+		mlx4_cmd(dev, in_param, RES_COUNTER, RES_OP_RESERVE,
+			 MLX4_CMD_FREE_RES, MLX4_CMD_TIME_CLASS_A,
+			 MLX4_CMD_WRAPPED);
+		return;
+	}
+	__mlx4_counter_free(dev, idx);
+}
 EXPORT_SYMBOL_GPL(mlx4_counter_free);
 
 static int mlx4_setup_hca(struct mlx4_dev *dev)
@@ -1865,7 +1898,6 @@ static int __mlx4_init_one(struct pci_dev *pdev, const struct pci_device_id *id)
 				mlx4_err(dev, "Failed to enable sriov,"
 					 "continuing without sriov enabled"
 					 " (err = %d).\n", err);
-				num_vfs = 0;
 				err = 0;
 			} else {
 				mlx4_warn(dev, "Running in master mode\n");
@@ -2022,7 +2054,7 @@ err_cmd:
 	mlx4_cmd_cleanup(dev);
 
 err_sriov:
-	if (num_vfs && (dev->flags & MLX4_FLAG_SRIOV))
+	if (dev->flags & MLX4_FLAG_SRIOV)
 		pci_disable_sriov(pdev);
 
 err_rel_own:
@@ -2070,6 +2102,10 @@ static void mlx4_remove_one(struct pci_dev *pdev)
 			mlx4_CLOSE_PORT(dev, p);
 		}
 
+		if (mlx4_is_master(dev))
+			mlx4_free_resource_tracker(dev,
+						   RES_TR_FREE_SLAVES_ONLY);
+
 		mlx4_cleanup_counters_table(dev);
 		mlx4_cleanup_mcg_table(dev);
 		mlx4_cleanup_qp_table(dev);
@@ -2082,7 +2118,8 @@ static void mlx4_remove_one(struct pci_dev *pdev)
 		mlx4_cleanup_pd_table(dev);
 
 		if (mlx4_is_master(dev))
-			mlx4_free_resource_tracker(dev);
+			mlx4_free_resource_tracker(dev,
+						   RES_TR_FREE_STRUCTS_ONLY);
 
 		iounmap(priv->kar);
 		mlx4_uar_free(dev, &priv->driver_uar);
@@ -2099,7 +2136,7 @@ static void mlx4_remove_one(struct pci_dev *pdev)
 
 		if (dev->flags & MLX4_FLAG_MSI_X)
 			pci_disable_msix(pdev);
-		if (num_vfs && (dev->flags & MLX4_FLAG_SRIOV)) {
+		if (dev->flags & MLX4_FLAG_SRIOV) {
 			mlx4_warn(dev, "Disabling sriov\n");
 			pci_disable_sriov(pdev);
 		}
diff --git a/drivers/net/ethernet/mellanox/mlx4/mcg.c b/drivers/net/ethernet/mellanox/mlx4/mcg.c
index 4799e824052..f4a8f98e402 100644
--- a/drivers/net/ethernet/mellanox/mlx4/mcg.c
+++ b/drivers/net/ethernet/mellanox/mlx4/mcg.c
@@ -357,7 +357,6 @@ static int add_promisc_qp(struct mlx4_dev *dev, u8 port,
 	u32 prot;
 	int i;
 	bool found;
-	int last_index;
 	int err;
 	struct mlx4_priv *priv = mlx4_priv(dev);
 
@@ -419,7 +418,6 @@ static int add_promisc_qp(struct mlx4_dev *dev, u8 port,
 			if (err)
 				goto out_mailbox;
 		}
-		last_index = entry->index;
 	}
 
 	/* add the new qpn to list of promisc qps */
diff --git a/drivers/net/ethernet/mellanox/mlx4/mlx4.h b/drivers/net/ethernet/mellanox/mlx4/mlx4.h
index 2a0ff2cc718..86b6e5a2fab 100644
--- a/drivers/net/ethernet/mellanox/mlx4/mlx4.h
+++ b/drivers/net/ethernet/mellanox/mlx4/mlx4.h
@@ -53,6 +53,26 @@
 #define DRV_VERSION	"1.1"
 #define DRV_RELDATE	"Dec, 2011"
 
+#define MLX4_NUM_UP		8
+#define MLX4_NUM_TC		8
+#define MLX4_RATELIMIT_UNITS 3 /* 100 Mbps */
+#define MLX4_RATELIMIT_DEFAULT 0xffff
+
+struct mlx4_set_port_prio2tc_context {
+	u8 prio2tc[4];
+};
+
+struct mlx4_port_scheduler_tc_cfg_be {
+	__be16 pg;
+	__be16 bw_precentage;
+	__be16 max_bw_units; /* 3-100Mbps, 4-1Gbps, other values - reserved */
+	__be16 max_bw_value;
+};
+
+struct mlx4_set_port_scheduler_context {
+	struct mlx4_port_scheduler_tc_cfg_be tc[MLX4_NUM_TC];
+};
+
 enum {
 	MLX4_HCR_BASE		= 0x80680,
 	MLX4_HCR_SIZE		= 0x0001c,
@@ -126,6 +146,11 @@ enum mlx4_alloc_mode {
 	RES_OP_MAP_ICM,
 };
 
+enum mlx4_res_tracker_free_type {
+	RES_TR_FREE_ALL,
+	RES_TR_FREE_SLAVES_ONLY,
+	RES_TR_FREE_STRUCTS_ONLY,
+};
 
 /*
  *Virtual HCR structures.
@@ -851,6 +876,10 @@ void __mlx4_unregister_mac(struct mlx4_dev *dev, u8 port, u64 mac);
 int __mlx4_replace_mac(struct mlx4_dev *dev, u8 port, int qpn, u64 new_mac);
 int __mlx4_write_mtt(struct mlx4_dev *dev, struct mlx4_mtt *mtt,
 		     int start_index, int npages, u64 *page_list);
+int __mlx4_counter_alloc(struct mlx4_dev *dev, u32 *idx);
+void __mlx4_counter_free(struct mlx4_dev *dev, u32 idx);
+int __mlx4_xrcd_alloc(struct mlx4_dev *dev, u32 *xrcdn);
+void __mlx4_xrcd_free(struct mlx4_dev *dev, u32 xrcdn);
 
 void mlx4_start_catas_poll(struct mlx4_dev *dev);
 void mlx4_stop_catas_poll(struct mlx4_dev *dev);
@@ -1007,7 +1036,8 @@ int mlx4_get_slave_from_resource_id(struct mlx4_dev *dev,
 void mlx4_delete_all_resources_for_slave(struct mlx4_dev *dev, int slave_id);
 int mlx4_init_resource_tracker(struct mlx4_dev *dev);
 
-void mlx4_free_resource_tracker(struct mlx4_dev *dev);
+void mlx4_free_resource_tracker(struct mlx4_dev *dev,
+				enum mlx4_res_tracker_free_type type);
 
 int mlx4_SET_PORT_wrapper(struct mlx4_dev *dev, int slave,
 			  struct mlx4_vhcr *vhcr,
diff --git a/drivers/net/ethernet/mellanox/mlx4/mlx4_en.h b/drivers/net/ethernet/mellanox/mlx4/mlx4_en.h
index d69fee41f24..6ae350921b1 100644
--- a/drivers/net/ethernet/mellanox/mlx4/mlx4_en.h
+++ b/drivers/net/ethernet/mellanox/mlx4/mlx4_en.h
@@ -40,6 +40,9 @@
 #include <linux/mutex.h>
 #include <linux/netdevice.h>
 #include <linux/if_vlan.h>
+#ifdef CONFIG_MLX4_EN_DCB
+#include <linux/dcbnl.h>
+#endif
 
 #include <linux/mlx4/device.h>
 #include <linux/mlx4/qp.h>
@@ -108,9 +111,8 @@ enum {
 #define MLX4_EN_MIN_TX_SIZE	(4096 / TXBB_SIZE)
 
 #define MLX4_EN_SMALL_PKT_SIZE		64
-#define MLX4_EN_NUM_TX_RINGS		8
-#define MLX4_EN_NUM_PPP_RINGS		8
-#define MAX_TX_RINGS			(MLX4_EN_NUM_TX_RINGS + MLX4_EN_NUM_PPP_RINGS)
+#define MLX4_EN_MAX_TX_RING_P_UP	32
+#define MLX4_EN_NUM_UP			8
 #define MLX4_EN_DEF_TX_RING_SIZE	512
 #define MLX4_EN_DEF_RX_RING_SIZE  	1024
 
@@ -118,7 +120,7 @@ enum {
 #define MLX4_EN_RX_COAL_TARGET	44
 #define MLX4_EN_RX_COAL_TIME	0x10
 
-#define MLX4_EN_TX_COAL_PKTS	5
+#define MLX4_EN_TX_COAL_PKTS	16
 #define MLX4_EN_TX_COAL_TIME	0x80
 
 #define MLX4_EN_RX_RATE_LOW		400000
@@ -196,6 +198,7 @@ enum cq_type {
 struct mlx4_en_tx_info {
 	struct sk_buff *skb;
 	u32 nr_txbb;
+	u32 nr_bytes;
 	u8 linear;
 	u8 data_offset;
 	u8 inl;
@@ -251,9 +254,9 @@ struct mlx4_en_tx_ring {
 	unsigned long bytes;
 	unsigned long packets;
 	unsigned long tx_csum;
-	spinlock_t comp_lock;
 	struct mlx4_bf bf;
 	bool bf_enabled;
+	struct netdev_queue *tx_queue;
 };
 
 struct mlx4_en_rx_desc {
@@ -304,8 +307,6 @@ struct mlx4_en_cq {
 	spinlock_t              lock;
 	struct net_device      *dev;
 	struct napi_struct	napi;
-	/* Per-core Tx cq processing support */
-	struct timer_list timer;
 	int size;
 	int buf_size;
 	unsigned vector;
@@ -336,6 +337,7 @@ struct mlx4_en_profile {
 	u32 active_ports;
 	u32 small_pkt_int;
 	u8 no_reset;
+	u8 num_tx_rings_p_up;
 	struct mlx4_en_port_profile prof[MLX4_MAX_PORTS + 1];
 };
 
@@ -411,6 +413,15 @@ struct mlx4_en_frag_info {
 
 };
 
+#ifdef CONFIG_MLX4_EN_DCB
+/* Minimal TC BW - setting to 0 will block traffic */
+#define MLX4_EN_BW_MIN 1
+#define MLX4_EN_BW_MAX 100 /* Utilize 100% of the line */
+
+#define MLX4_EN_TC_ETS 7
+
+#endif
+
 struct mlx4_en_priv {
 	struct mlx4_en_dev *mdev;
 	struct mlx4_en_port_profile *prof;
@@ -465,9 +476,9 @@ struct mlx4_en_priv {
 	u16 num_frags;
 	u16 log_rx_info;
 
-	struct mlx4_en_tx_ring tx_ring[MAX_TX_RINGS];
+	struct mlx4_en_tx_ring *tx_ring;
 	struct mlx4_en_rx_ring rx_ring[MAX_RX_RINGS];
-	struct mlx4_en_cq tx_cq[MAX_TX_RINGS];
+	struct mlx4_en_cq *tx_cq;
 	struct mlx4_en_cq rx_cq[MAX_RX_RINGS];
 	struct work_struct mcast_task;
 	struct work_struct mac_task;
@@ -484,6 +495,11 @@ struct mlx4_en_priv {
 	int vids[128];
 	bool wol;
 	struct device *ddev;
+
+#ifdef CONFIG_MLX4_EN_DCB
+	struct ieee_ets ets;
+	u16 maxrate[IEEE_8021QAZ_MAX_TCS];
+#endif
 };
 
 enum mlx4_en_wol {
@@ -512,7 +528,6 @@ void mlx4_en_deactivate_cq(struct mlx4_en_priv *priv, struct mlx4_en_cq *cq);
 int mlx4_en_set_cq_moder(struct mlx4_en_priv *priv, struct mlx4_en_cq *cq);
 int mlx4_en_arm_cq(struct mlx4_en_priv *priv, struct mlx4_en_cq *cq);
 
-void mlx4_en_poll_tx_cq(unsigned long data);
 void mlx4_en_tx_irq(struct mlx4_cq *mcq);
 u16 mlx4_en_select_queue(struct net_device *dev, struct sk_buff *skb);
 netdev_tx_t mlx4_en_xmit(struct sk_buff *skb, struct net_device *dev);
@@ -522,7 +537,7 @@ int mlx4_en_create_tx_ring(struct mlx4_en_priv *priv, struct mlx4_en_tx_ring *ri
 void mlx4_en_destroy_tx_ring(struct mlx4_en_priv *priv, struct mlx4_en_tx_ring *ring);
 int mlx4_en_activate_tx_ring(struct mlx4_en_priv *priv,
 			     struct mlx4_en_tx_ring *ring,
-			     int cq);
+			     int cq, int user_prio);
 void mlx4_en_deactivate_tx_ring(struct mlx4_en_priv *priv,
 				struct mlx4_en_tx_ring *ring);
 
@@ -540,8 +555,8 @@ int mlx4_en_process_rx_cq(struct net_device *dev,
 			  int budget);
 int mlx4_en_poll_rx_cq(struct napi_struct *napi, int budget);
 void mlx4_en_fill_qp_context(struct mlx4_en_priv *priv, int size, int stride,
-			     int is_tx, int rss, int qpn, int cqn,
-			     struct mlx4_qp_context *context);
+		int is_tx, int rss, int qpn, int cqn, int user_prio,
+		struct mlx4_qp_context *context);
 void mlx4_en_sqp_event(struct mlx4_qp *qp, enum mlx4_event event);
 int mlx4_en_map_buffer(struct mlx4_buf *buf);
 void mlx4_en_unmap_buffer(struct mlx4_buf *buf);
@@ -558,6 +573,10 @@ int mlx4_SET_VLAN_FLTR(struct mlx4_dev *dev, struct mlx4_en_priv *priv);
 int mlx4_en_DUMP_ETH_STATS(struct mlx4_en_dev *mdev, u8 port, u8 reset);
 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;
+#endif
+
 #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);
diff --git a/drivers/net/ethernet/mellanox/mlx4/mr.c b/drivers/net/ethernet/mellanox/mlx4/mr.c
index fe2ac8449c1..af55b7ce534 100644
--- a/drivers/net/ethernet/mellanox/mlx4/mr.c
+++ b/drivers/net/ethernet/mellanox/mlx4/mr.c
@@ -788,7 +788,6 @@ int mlx4_fmr_alloc(struct mlx4_dev *dev, u32 pd, u32 access, int max_pages,
 		   int max_maps, u8 page_shift, struct mlx4_fmr *fmr)
 {
 	struct mlx4_priv *priv = mlx4_priv(dev);
-	u64 mtt_offset;
 	int err = -ENOMEM;
 
 	if (max_maps > dev->caps.max_fmr_maps)
@@ -811,8 +810,6 @@ int mlx4_fmr_alloc(struct mlx4_dev *dev, u32 pd, u32 access, int max_pages,
 	if (err)
 		return err;
 
-	mtt_offset = fmr->mr.mtt.offset * dev->caps.mtt_entry_sz;
-
 	fmr->mtts = mlx4_table_find(&priv->mr_table.mtt_table,
 				    fmr->mr.mtt.offset,
 				    &fmr->dma_handle);
@@ -895,6 +892,6 @@ EXPORT_SYMBOL_GPL(mlx4_fmr_free);
 int mlx4_SYNC_TPT(struct mlx4_dev *dev)
 {
 	return mlx4_cmd(dev, 0, 0, 0, MLX4_CMD_SYNC_TPT, 1000,
-			MLX4_CMD_WRAPPED);
+			MLX4_CMD_NATIVE);
 }
 EXPORT_SYMBOL_GPL(mlx4_SYNC_TPT);
diff --git a/drivers/net/ethernet/mellanox/mlx4/pd.c b/drivers/net/ethernet/mellanox/mlx4/pd.c
index db4746d0dca..1ac88637ad9 100644
--- a/drivers/net/ethernet/mellanox/mlx4/pd.c
+++ b/drivers/net/ethernet/mellanox/mlx4/pd.c
@@ -63,7 +63,7 @@ void mlx4_pd_free(struct mlx4_dev *dev, u32 pdn)
 }
 EXPORT_SYMBOL_GPL(mlx4_pd_free);
 
-int mlx4_xrcd_alloc(struct mlx4_dev *dev, u32 *xrcdn)
+int __mlx4_xrcd_alloc(struct mlx4_dev *dev, u32 *xrcdn)
 {
 	struct mlx4_priv *priv = mlx4_priv(dev);
 
@@ -73,12 +73,47 @@ int mlx4_xrcd_alloc(struct mlx4_dev *dev, u32 *xrcdn)
 
 	return 0;
 }
+
+int mlx4_xrcd_alloc(struct mlx4_dev *dev, u32 *xrcdn)
+{
+	u64 out_param;
+	int err;
+
+	if (mlx4_is_mfunc(dev)) {
+		err = mlx4_cmd_imm(dev, 0, &out_param,
+				   RES_XRCD, RES_OP_RESERVE,
+				   MLX4_CMD_ALLOC_RES,
+				   MLX4_CMD_TIME_CLASS_A, MLX4_CMD_WRAPPED);
+		if (err)
+			return err;
+
+		*xrcdn = get_param_l(&out_param);
+		return 0;
+	}
+	return __mlx4_xrcd_alloc(dev, xrcdn);
+}
 EXPORT_SYMBOL_GPL(mlx4_xrcd_alloc);
 
-void mlx4_xrcd_free(struct mlx4_dev *dev, u32 xrcdn)
+void __mlx4_xrcd_free(struct mlx4_dev *dev, u32 xrcdn)
 {
 	mlx4_bitmap_free(&mlx4_priv(dev)->xrcd_bitmap, xrcdn);
 }
+
+void mlx4_xrcd_free(struct mlx4_dev *dev, u32 xrcdn)
+{
+	u64 in_param;
+	int err;
+
+	if (mlx4_is_mfunc(dev)) {
+		set_param_l(&in_param, xrcdn);
+		err = mlx4_cmd(dev, in_param, RES_XRCD,
+			       RES_OP_RESERVE, MLX4_CMD_FREE_RES,
+			       MLX4_CMD_TIME_CLASS_A, MLX4_CMD_WRAPPED);
+		if (err)
+			mlx4_warn(dev, "Failed to release xrcdn %d\n", xrcdn);
+	} else
+		__mlx4_xrcd_free(dev, xrcdn);
+}
 EXPORT_SYMBOL_GPL(mlx4_xrcd_free);
 
 int mlx4_init_pd_table(struct mlx4_dev *dev)
diff --git a/drivers/net/ethernet/mellanox/mlx4/port.c b/drivers/net/ethernet/mellanox/mlx4/port.c
index 77535ff18f1..1fe2c7a8b40 100644
--- a/drivers/net/ethernet/mellanox/mlx4/port.c
+++ b/drivers/net/ethernet/mellanox/mlx4/port.c
@@ -338,13 +338,12 @@ EXPORT_SYMBOL_GPL(__mlx4_unregister_mac);
 void mlx4_unregister_mac(struct mlx4_dev *dev, u8 port, u64 mac)
 {
 	u64 out_param;
-	int err;
 
 	if (mlx4_is_mfunc(dev)) {
 		set_param_l(&out_param, port);
-		err = mlx4_cmd_imm(dev, mac, &out_param, RES_MAC,
-				   RES_OP_RESERVE_AND_MAP, MLX4_CMD_FREE_RES,
-				   MLX4_CMD_TIME_CLASS_A, MLX4_CMD_WRAPPED);
+		(void) mlx4_cmd_imm(dev, mac, &out_param, RES_MAC,
+				    RES_OP_RESERVE_AND_MAP, MLX4_CMD_FREE_RES,
+				    MLX4_CMD_TIME_CLASS_A, MLX4_CMD_WRAPPED);
 		return;
 	}
 	__mlx4_unregister_mac(dev, port, mac);
@@ -834,6 +833,68 @@ int mlx4_SET_PORT_qpn_calc(struct mlx4_dev *dev, u8 port, u32 base_qpn,
 }
 EXPORT_SYMBOL(mlx4_SET_PORT_qpn_calc);
 
+int mlx4_SET_PORT_PRIO2TC(struct mlx4_dev *dev, u8 port, u8 *prio2tc)
+{
+	struct mlx4_cmd_mailbox *mailbox;
+	struct mlx4_set_port_prio2tc_context *context;
+	int err;
+	u32 in_mod;
+	int i;
+
+	mailbox = mlx4_alloc_cmd_mailbox(dev);
+	if (IS_ERR(mailbox))
+		return PTR_ERR(mailbox);
+	context = mailbox->buf;
+	memset(context, 0, sizeof *context);
+
+	for (i = 0; i < MLX4_NUM_UP; i += 2)
+		context->prio2tc[i >> 1] = prio2tc[i] << 4 | prio2tc[i + 1];
+
+	in_mod = MLX4_SET_PORT_PRIO2TC << 8 | port;
+	err = mlx4_cmd(dev, mailbox->dma, in_mod, 1, MLX4_CMD_SET_PORT,
+		       MLX4_CMD_TIME_CLASS_B, MLX4_CMD_NATIVE);
+
+	mlx4_free_cmd_mailbox(dev, mailbox);
+	return err;
+}
+EXPORT_SYMBOL(mlx4_SET_PORT_PRIO2TC);
+
+int mlx4_SET_PORT_SCHEDULER(struct mlx4_dev *dev, u8 port, u8 *tc_tx_bw,
+		u8 *pg, u16 *ratelimit)
+{
+	struct mlx4_cmd_mailbox *mailbox;
+	struct mlx4_set_port_scheduler_context *context;
+	int err;
+	u32 in_mod;
+	int i;
+
+	mailbox = mlx4_alloc_cmd_mailbox(dev);
+	if (IS_ERR(mailbox))
+		return PTR_ERR(mailbox);
+	context = mailbox->buf;
+	memset(context, 0, sizeof *context);
+
+	for (i = 0; i < MLX4_NUM_TC; i++) {
+		struct mlx4_port_scheduler_tc_cfg_be *tc = &context->tc[i];
+		u16 r = ratelimit && ratelimit[i] ? ratelimit[i] :
+			MLX4_RATELIMIT_DEFAULT;
+
+		tc->pg = htons(pg[i]);
+		tc->bw_precentage = htons(tc_tx_bw[i]);
+
+		tc->max_bw_units = htons(MLX4_RATELIMIT_UNITS);
+		tc->max_bw_value = htons(r);
+	}
+
+	in_mod = MLX4_SET_PORT_SCHEDULER << 8 | port;
+	err = mlx4_cmd(dev, mailbox->dma, in_mod, 1, MLX4_CMD_SET_PORT,
+		       MLX4_CMD_TIME_CLASS_B, MLX4_CMD_NATIVE);
+
+	mlx4_free_cmd_mailbox(dev, mailbox);
+	return err;
+}
+EXPORT_SYMBOL(mlx4_SET_PORT_SCHEDULER);
+
 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/resource_tracker.c b/drivers/net/ethernet/mellanox/mlx4/resource_tracker.c
index 8752e6e0816..b45d0e7f6ab 100644
--- a/drivers/net/ethernet/mellanox/mlx4/resource_tracker.c
+++ b/drivers/net/ethernet/mellanox/mlx4/resource_tracker.c
@@ -89,17 +89,6 @@ enum res_qp_states {
 	RES_QP_HW
 };
 
-static inline const char *qp_states_str(enum res_qp_states state)
-{
-	switch (state) {
-	case RES_QP_BUSY: return "RES_QP_BUSY";
-	case RES_QP_RESERVED: return "RES_QP_RESERVED";
-	case RES_QP_MAPPED: return "RES_QP_MAPPED";
-	case RES_QP_HW: return "RES_QP_HW";
-	default: return "Unknown";
-	}
-}
-
 struct res_qp {
 	struct res_common	com;
 	struct res_mtt	       *mtt;
@@ -173,16 +162,6 @@ enum res_srq_states {
 	RES_SRQ_HW,
 };
 
-static inline const char *srq_states_str(enum res_srq_states state)
-{
-	switch (state) {
-	case RES_SRQ_BUSY: return "RES_SRQ_BUSY";
-	case RES_SRQ_ALLOCATED: return "RES_SRQ_ALLOCATED";
-	case RES_SRQ_HW: return "RES_SRQ_HW";
-	default: return "Unknown";
-	}
-}
-
 struct res_srq {
 	struct res_common	com;
 	struct res_mtt	       *mtt;
@@ -195,20 +174,21 @@ enum res_counter_states {
 	RES_COUNTER_ALLOCATED,
 };
 
-static inline const char *counter_states_str(enum res_counter_states state)
-{
-	switch (state) {
-	case RES_COUNTER_BUSY: return "RES_COUNTER_BUSY";
-	case RES_COUNTER_ALLOCATED: return "RES_COUNTER_ALLOCATED";
-	default: return "Unknown";
-	}
-}
-
 struct res_counter {
 	struct res_common	com;
 	int			port;
 };
 
+enum res_xrcdn_states {
+	RES_XRCD_BUSY = RES_ANY_BUSY,
+	RES_XRCD_ALLOCATED,
+};
+
+struct res_xrcdn {
+	struct res_common	com;
+	int			port;
+};
+
 /* For Debug uses */
 static const char *ResourceType(enum mlx4_resource rt)
 {
@@ -221,6 +201,7 @@ static const char *ResourceType(enum mlx4_resource rt)
 	case RES_MAC: return  "RES_MAC";
 	case RES_EQ: return "RES_EQ";
 	case RES_COUNTER: return "RES_COUNTER";
+	case RES_XRCD: return "RES_XRCD";
 	default: return "Unknown resource type !!!";
 	};
 }
@@ -254,16 +235,23 @@ int mlx4_init_resource_tracker(struct mlx4_dev *dev)
 	return 0 ;
 }
 
-void mlx4_free_resource_tracker(struct mlx4_dev *dev)
+void mlx4_free_resource_tracker(struct mlx4_dev *dev,
+				enum mlx4_res_tracker_free_type type)
 {
 	struct mlx4_priv *priv = mlx4_priv(dev);
 	int i;
 
 	if (priv->mfunc.master.res_tracker.slave_list) {
-		for (i = 0 ; i < dev->num_slaves; i++)
-			mlx4_delete_all_resources_for_slave(dev, i);
-
-		kfree(priv->mfunc.master.res_tracker.slave_list);
+		if (type != RES_TR_FREE_STRUCTS_ONLY)
+			for (i = 0 ; i < dev->num_slaves; i++)
+				if (type == RES_TR_FREE_ALL ||
+				    dev->caps.function != i)
+					mlx4_delete_all_resources_for_slave(dev, i);
+
+		if (type != RES_TR_FREE_SLAVES_ONLY) {
+			kfree(priv->mfunc.master.res_tracker.slave_list);
+			priv->mfunc.master.res_tracker.slave_list = NULL;
+		}
 	}
 }
 
@@ -471,6 +459,20 @@ static struct res_common *alloc_counter_tr(int id)
 	return &ret->com;
 }
 
+static struct res_common *alloc_xrcdn_tr(int id)
+{
+	struct res_xrcdn *ret;
+
+	ret = kzalloc(sizeof *ret, GFP_KERNEL);
+	if (!ret)
+		return NULL;
+
+	ret->com.res_id = id;
+	ret->com.state = RES_XRCD_ALLOCATED;
+
+	return &ret->com;
+}
+
 static struct res_common *alloc_tr(int id, enum mlx4_resource type, int slave,
 				   int extra)
 {
@@ -501,7 +503,9 @@ static struct res_common *alloc_tr(int id, enum mlx4_resource type, int slave,
 	case RES_COUNTER:
 		ret = alloc_counter_tr(id);
 		break;
-
+	case RES_XRCD:
+		ret = alloc_xrcdn_tr(id);
+		break;
 	default:
 		return NULL;
 	}
@@ -624,6 +628,16 @@ static int remove_counter_ok(struct res_counter *res)
 	return 0;
 }
 
+static int remove_xrcdn_ok(struct res_xrcdn *res)
+{
+	if (res->com.state == RES_XRCD_BUSY)
+		return -EBUSY;
+	else if (res->com.state != RES_XRCD_ALLOCATED)
+		return -EPERM;
+
+	return 0;
+}
+
 static int remove_cq_ok(struct res_cq *res)
 {
 	if (res->com.state == RES_CQ_BUSY)
@@ -663,6 +677,8 @@ static int remove_ok(struct res_common *res, enum mlx4_resource type, int extra)
 		return remove_eq_ok((struct res_eq *)res);
 	case RES_COUNTER:
 		return remove_counter_ok((struct res_counter *)res);
+	case RES_XRCD:
+		return remove_xrcdn_ok((struct res_xrcdn *)res);
 	default:
 		return -EINVAL;
 	}
@@ -1269,6 +1285,50 @@ static int vlan_alloc_res(struct mlx4_dev *dev, int slave, int op, int cmd,
 	return 0;
 }
 
+static int counter_alloc_res(struct mlx4_dev *dev, int slave, int op, int cmd,
+			     u64 in_param, u64 *out_param)
+{
+	u32 index;
+	int err;
+
+	if (op != RES_OP_RESERVE)
+		return -EINVAL;
+
+	err = __mlx4_counter_alloc(dev, &index);
+	if (err)
+		return err;
+
+	err = add_res_range(dev, slave, index, 1, RES_COUNTER, 0);
+	if (err)
+		__mlx4_counter_free(dev, index);
+	else
+		set_param_l(out_param, index);
+
+	return err;
+}
+
+static int xrcdn_alloc_res(struct mlx4_dev *dev, int slave, int op, int cmd,
+			   u64 in_param, u64 *out_param)
+{
+	u32 xrcdn;
+	int err;
+
+	if (op != RES_OP_RESERVE)
+		return -EINVAL;
+
+	err = __mlx4_xrcd_alloc(dev, &xrcdn);
+	if (err)
+		return err;
+
+	err = add_res_range(dev, slave, xrcdn, 1, RES_XRCD, 0);
+	if (err)
+		__mlx4_xrcd_free(dev, xrcdn);
+	else
+		set_param_l(out_param, xrcdn);
+
+	return err;
+}
+
 int mlx4_ALLOC_RES_wrapper(struct mlx4_dev *dev, int slave,
 			   struct mlx4_vhcr *vhcr,
 			   struct mlx4_cmd_mailbox *inbox,
@@ -1314,6 +1374,16 @@ int mlx4_ALLOC_RES_wrapper(struct mlx4_dev *dev, int slave,
 				    vhcr->in_param, &vhcr->out_param);
 		break;
 
+	case RES_COUNTER:
+		err = counter_alloc_res(dev, slave, vhcr->op_modifier, alop,
+					vhcr->in_param, &vhcr->out_param);
+		break;
+
+	case RES_XRCD:
+		err = xrcdn_alloc_res(dev, slave, vhcr->op_modifier, alop,
+				      vhcr->in_param, &vhcr->out_param);
+		break;
+
 	default:
 		err = -EINVAL;
 		break;
@@ -1496,6 +1566,44 @@ static int vlan_free_res(struct mlx4_dev *dev, int slave, int op, int cmd,
 	return 0;
 }
 
+static int counter_free_res(struct mlx4_dev *dev, int slave, int op, int cmd,
+			    u64 in_param, u64 *out_param)
+{
+	int index;
+	int err;
+
+	if (op != RES_OP_RESERVE)
+		return -EINVAL;
+
+	index = get_param_l(&in_param);
+	err = rem_res_range(dev, slave, index, 1, RES_COUNTER, 0);
+	if (err)
+		return err;
+
+	__mlx4_counter_free(dev, index);
+
+	return err;
+}
+
+static int xrcdn_free_res(struct mlx4_dev *dev, int slave, int op, int cmd,
+			  u64 in_param, u64 *out_param)
+{
+	int xrcdn;
+	int err;
+
+	if (op != RES_OP_RESERVE)
+		return -EINVAL;
+
+	xrcdn = get_param_l(&in_param);
+	err = rem_res_range(dev, slave, xrcdn, 1, RES_XRCD, 0);
+	if (err)
+		return err;
+
+	__mlx4_xrcd_free(dev, xrcdn);
+
+	return err;
+}
+
 int mlx4_FREE_RES_wrapper(struct mlx4_dev *dev, int slave,
 			  struct mlx4_vhcr *vhcr,
 			  struct mlx4_cmd_mailbox *inbox,
@@ -1541,6 +1649,15 @@ int mlx4_FREE_RES_wrapper(struct mlx4_dev *dev, int slave,
 				   vhcr->in_param, &vhcr->out_param);
 		break;
 
+	case RES_COUNTER:
+		err = counter_free_res(dev, slave, vhcr->op_modifier, alop,
+				       vhcr->in_param, &vhcr->out_param);
+		break;
+
+	case RES_XRCD:
+		err = xrcdn_free_res(dev, slave, vhcr->op_modifier, alop,
+				     vhcr->in_param, &vhcr->out_param);
+
 	default:
 		break;
 	}
@@ -2536,7 +2653,7 @@ int mlx4_QP_ATTACH_wrapper(struct mlx4_dev *dev, int slave,
 	struct mlx4_qp qp; /* dummy for calling attach/detach */
 	u8 *gid = inbox->buf;
 	enum mlx4_protocol prot = (vhcr->in_modifier >> 28) & 0x7;
-	int err, err1;
+	int err;
 	int qpn;
 	struct res_qp *rqp;
 	int attach = vhcr->op_modifier;
@@ -2571,7 +2688,7 @@ int mlx4_QP_ATTACH_wrapper(struct mlx4_dev *dev, int slave,
 
 ex_rem:
 	/* ignore error return below, already in error */
-	err1 = rem_mcg_res(dev, slave, rqp, gid, prot, type);
+	(void) rem_mcg_res(dev, slave, rqp, gid, prot, type);
 ex_put:
 	put_res(dev, slave, qpn, RES_QP);
 
@@ -2604,13 +2721,12 @@ static void detach_qp(struct mlx4_dev *dev, int slave, struct res_qp *rqp)
 {
 	struct res_gid *rgid;
 	struct res_gid *tmp;
-	int err;
 	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;
-		err = mlx4_qp_detach_common(dev, &qp, rgid->gid, rgid->prot,
-					    rgid->steer);
+		(void) mlx4_qp_detach_common(dev, &qp, rgid->gid, rgid->prot,
+					     rgid->steer);
 		list_del(&rgid->list);
 		kfree(rgid);
 	}
@@ -3036,14 +3152,13 @@ static void rem_slave_eqs(struct mlx4_dev *dev, int slave)
 							   MLX4_CMD_HW2SW_EQ,
 							   MLX4_CMD_TIME_CLASS_A,
 							   MLX4_CMD_NATIVE);
-					mlx4_dbg(dev, "rem_slave_eqs: failed"
-						 " to move slave %d eqs %d to"
-						 " SW ownership\n", slave, eqn);
+					if (err)
+						mlx4_dbg(dev, "rem_slave_eqs: failed"
+							 " to move slave %d eqs %d to"
+							 " SW ownership\n", slave, eqn);
 					mlx4_free_cmd_mailbox(dev, mailbox);
-					if (!err) {
-						atomic_dec(&eq->mtt->ref_count);
-						state = RES_EQ_RESERVED;
-					}
+					atomic_dec(&eq->mtt->ref_count);
+					state = RES_EQ_RESERVED;
 					break;
 
 				default:
@@ -3056,6 +3171,64 @@ static void rem_slave_eqs(struct mlx4_dev *dev, int slave)
 	spin_unlock_irq(mlx4_tlock(dev));
 }
 
+static void rem_slave_counters(struct mlx4_dev *dev, int slave)
+{
+	struct mlx4_priv *priv = mlx4_priv(dev);
+	struct mlx4_resource_tracker *tracker = &priv->mfunc.master.res_tracker;
+	struct list_head *counter_list =
+		&tracker->slave_list[slave].res_list[RES_COUNTER];
+	struct res_counter *counter;
+	struct res_counter *tmp;
+	int err;
+	int index;
+
+	err = move_all_busy(dev, slave, RES_COUNTER);
+	if (err)
+		mlx4_warn(dev, "rem_slave_counters: Could not move all counters to "
+			  "busy for slave %d\n", slave);
+
+	spin_lock_irq(mlx4_tlock(dev));
+	list_for_each_entry_safe(counter, tmp, counter_list, com.list) {
+		if (counter->com.owner == slave) {
+			index = counter->com.res_id;
+			radix_tree_delete(&tracker->res_tree[RES_COUNTER], index);
+			list_del(&counter->com.list);
+			kfree(counter);
+			__mlx4_counter_free(dev, index);
+		}
+	}
+	spin_unlock_irq(mlx4_tlock(dev));
+}
+
+static void rem_slave_xrcdns(struct mlx4_dev *dev, int slave)
+{
+	struct mlx4_priv *priv = mlx4_priv(dev);
+	struct mlx4_resource_tracker *tracker = &priv->mfunc.master.res_tracker;
+	struct list_head *xrcdn_list =
+		&tracker->slave_list[slave].res_list[RES_XRCD];
+	struct res_xrcdn *xrcd;
+	struct res_xrcdn *tmp;
+	int err;
+	int xrcdn;
+
+	err = move_all_busy(dev, slave, RES_XRCD);
+	if (err)
+		mlx4_warn(dev, "rem_slave_xrcdns: Could not move all xrcdns to "
+			  "busy for slave %d\n", slave);
+
+	spin_lock_irq(mlx4_tlock(dev));
+	list_for_each_entry_safe(xrcd, tmp, xrcdn_list, com.list) {
+		if (xrcd->com.owner == slave) {
+			xrcdn = xrcd->com.res_id;
+			radix_tree_delete(&tracker->res_tree[RES_XRCD], xrcdn);
+			list_del(&xrcd->com.list);
+			kfree(xrcd);
+			__mlx4_xrcd_free(dev, xrcdn);
+		}
+	}
+	spin_unlock_irq(mlx4_tlock(dev));
+}
+
 void mlx4_delete_all_resources_for_slave(struct mlx4_dev *dev, int slave)
 {
 	struct mlx4_priv *priv = mlx4_priv(dev);
@@ -3069,5 +3242,7 @@ void mlx4_delete_all_resources_for_slave(struct mlx4_dev *dev, int slave)
 	rem_slave_mrs(dev, slave);
 	rem_slave_eqs(dev, slave);
 	rem_slave_mtts(dev, slave);
+	rem_slave_counters(dev, slave);
+	rem_slave_xrcdns(dev, slave);
 	mutex_unlock(&priv->mfunc.master.res_tracker.slave_list[slave].mutex);
 }
diff --git a/drivers/net/ethernet/micrel/ks8842.c b/drivers/net/ethernet/micrel/ks8842.c
index f84dd2dc82b..24fb049ac2f 100644
--- a/drivers/net/ethernet/micrel/ks8842.c
+++ b/drivers/net/ethernet/micrel/ks8842.c
@@ -1262,7 +1262,7 @@ static struct platform_driver ks8842_platform_driver = {
 		.owner	= THIS_MODULE,
 	},
 	.probe		= ks8842_probe,
-	.remove		= ks8842_remove,
+	.remove		= __devexit_p(ks8842_remove),
 };
 
 module_platform_driver(ks8842_platform_driver);
diff --git a/drivers/net/ethernet/micrel/ks8851.c b/drivers/net/ethernet/micrel/ks8851.c
index f8dda009d3c..5e313e9a252 100644
--- a/drivers/net/ethernet/micrel/ks8851.c
+++ b/drivers/net/ethernet/micrel/ks8851.c
@@ -618,10 +618,8 @@ static void ks8851_irq_work(struct work_struct *work)
 	netif_dbg(ks, intr, ks->netdev,
 		  "%s: status 0x%04x\n", __func__, status);
 
-	if (status & IRQ_LCI) {
-		/* should do something about checking link status */
+	if (status & IRQ_LCI)
 		handled |= IRQ_LCI;
-	}
 
 	if (status & IRQ_LDI) {
 		u16 pmecr = ks8851_rdreg16(ks, KS_PMECR);
@@ -684,6 +682,9 @@ static void ks8851_irq_work(struct work_struct *work)
 
 	mutex_unlock(&ks->lock);
 
+	if (status & IRQ_LCI)
+		mii_check_link(&ks->mii);
+
 	if (status & IRQ_TXI)
 		netif_wake_queue(ks->netdev);
 
diff --git a/drivers/net/ethernet/myricom/myri10ge/myri10ge.c b/drivers/net/ethernet/myricom/myri10ge/myri10ge.c
index 27273ae1a6e..90153fc983c 100644
--- a/drivers/net/ethernet/myricom/myri10ge/myri10ge.c
+++ b/drivers/net/ethernet/myricom/myri10ge/myri10ge.c
@@ -4033,7 +4033,6 @@ static int myri10ge_probe(struct pci_dev *pdev, const struct pci_device_id *ent)
 
 	netdev->netdev_ops = &myri10ge_netdev_ops;
 	netdev->mtu = myri10ge_initial_mtu;
-	netdev->base_addr = mgp->iomem_base;
 	netdev->hw_features = mgp->features | NETIF_F_LRO | NETIF_F_RXCSUM;
 	netdev->features = netdev->hw_features;
 
@@ -4047,12 +4046,10 @@ static int myri10ge_probe(struct pci_dev *pdev, const struct pci_device_id *ent)
 		netdev->vlan_features &= ~NETIF_F_TSO;
 
 	/* make sure we can get an irq, and that MSI can be
-	 * setup (if available).  Also ensure netdev->irq
-	 * is set to correct value if MSI is enabled */
+	 * setup (if available). */
 	status = myri10ge_request_irq(mgp);
 	if (status != 0)
 		goto abort_with_firmware;
-	netdev->irq = pdev->irq;
 	myri10ge_free_irq(mgp);
 
 	/* Save configuration space to be restored if the
@@ -4077,7 +4074,7 @@ static int myri10ge_probe(struct pci_dev *pdev, const struct pci_device_id *ent)
 	else
 		dev_info(dev, "%s IRQ %d, tx bndry %d, fw %s, WC %s\n",
 			 mgp->msi_enabled ? "MSI" : "xPIC",
-			 netdev->irq, mgp->tx_boundary, mgp->fw_name,
+			 pdev->irq, mgp->tx_boundary, mgp->fw_name,
 			 (mgp->wc_enabled ? "Enabled" : "Disabled"));
 
 	board_number++;
diff --git a/drivers/net/ethernet/natsemi/Kconfig b/drivers/net/ethernet/natsemi/Kconfig
index eb836f770f5..f157334579f 100644
--- a/drivers/net/ethernet/natsemi/Kconfig
+++ b/drivers/net/ethernet/natsemi/Kconfig
@@ -6,9 +6,8 @@ config NET_VENDOR_NATSEMI
 	bool "National Semi-conductor devices"
 	default y
 	depends on AMIGA_PCMCIA || ARM || EISA || EXPERIMENTAL || H8300 || \
-		   ISA || M32R || MAC || MACH_JAZZ || MACH_TX49XX || MCA || \
-		   MCA_LEGACY || MIPS || PCI || PCMCIA || SUPERH || \
-		   XTENSA_PLATFORM_XT2000 || ZORRO
+		   ISA || M32R || MAC || MACH_JAZZ || MACH_TX49XX || MIPS || \
+		   PCI || PCMCIA || SUPERH || XTENSA_PLATFORM_XT2000 || ZORRO
 	---help---
 	  If you have a network (Ethernet) card belonging to this class, say Y
 	  and read the Ethernet-HOWTO, available from
@@ -21,21 +20,6 @@ config NET_VENDOR_NATSEMI
 
 if NET_VENDOR_NATSEMI
 
-config IBMLANA
-	tristate "IBM LAN Adapter/A support"
-	depends on MCA
-	---help---
-	  This is a Micro Channel Ethernet adapter.  You need to set
-	  CONFIG_MCA to use this driver.  It is both available as an in-kernel
-	  driver and as a module.
-
-	  To compile this driver as a module, choose M here. The only
-	  currently supported card is the IBM LAN Adapter/A for Ethernet.  It
-	  will both support 16K and 32K memory windows, however a 32K window
-	  gives a better security against packet losses.  Usage of multiple
-	  boards with this driver should be possible, but has not been tested
-	  up to now due to lack of hardware.
-
 config MACSONIC
 	tristate "Macintosh SONIC based ethernet (onboard, NuBus, LC, CS)"
 	depends on MAC
diff --git a/drivers/net/ethernet/natsemi/Makefile b/drivers/net/ethernet/natsemi/Makefile
index 9aa5dea52b3..764c532a96d 100644
--- a/drivers/net/ethernet/natsemi/Makefile
+++ b/drivers/net/ethernet/natsemi/Makefile
@@ -2,7 +2,6 @@
 # Makefile for the National Semi-conductor Sonic devices.
 #
 
-obj-$(CONFIG_IBMLANA) += ibmlana.o
 obj-$(CONFIG_MACSONIC) += macsonic.o
 obj-$(CONFIG_MIPS_JAZZ_SONIC) += jazzsonic.o
 obj-$(CONFIG_NATSEMI) += natsemi.o
diff --git a/drivers/net/ethernet/natsemi/natsemi.c b/drivers/net/ethernet/natsemi/natsemi.c
index d38e48d4f43..5b61d12f8b9 100644
--- a/drivers/net/ethernet/natsemi/natsemi.c
+++ b/drivers/net/ethernet/natsemi/natsemi.c
@@ -547,6 +547,7 @@ struct netdev_private {
 	struct sk_buff *tx_skbuff[TX_RING_SIZE];
 	dma_addr_t tx_dma[TX_RING_SIZE];
 	struct net_device *dev;
+	void __iomem *ioaddr;
 	struct napi_struct napi;
 	/* Media monitoring timer */
 	struct timer_list timer;
@@ -699,7 +700,9 @@ static ssize_t natsemi_set_dspcfg_workaround(struct device *dev,
 
 static inline void __iomem *ns_ioaddr(struct net_device *dev)
 {
-	return (void __iomem *) dev->base_addr;
+	struct netdev_private *np = netdev_priv(dev);
+
+	return np->ioaddr;
 }
 
 static inline void natsemi_irq_enable(struct net_device *dev)
@@ -863,10 +866,9 @@ static int __devinit natsemi_probe1 (struct pci_dev *pdev,
 	/* Store MAC Address in perm_addr */
 	memcpy(dev->perm_addr, dev->dev_addr, ETH_ALEN);
 
-	dev->base_addr = (unsigned long __force) ioaddr;
-	dev->irq = irq;
-
 	np = netdev_priv(dev);
+	np->ioaddr = ioaddr;
+
 	netif_napi_add(dev, &np->napi, natsemi_poll, 64);
 	np->dev = dev;
 
@@ -914,9 +916,6 @@ static int __devinit natsemi_probe1 (struct pci_dev *pdev,
 	}
 
 	option = find_cnt < MAX_UNITS ? options[find_cnt] : 0;
-	if (dev->mem_start)
-		option = dev->mem_start;
-
 	/* The lower four bits are the media type. */
 	if (option) {
 		if (option & 0x200)
@@ -1532,20 +1531,21 @@ static int netdev_open(struct net_device *dev)
 {
 	struct netdev_private *np = netdev_priv(dev);
 	void __iomem * ioaddr = ns_ioaddr(dev);
+	const int irq = np->pci_dev->irq;
 	int i;
 
 	/* Reset the chip, just in case. */
 	natsemi_reset(dev);
 
-	i = request_irq(dev->irq, intr_handler, IRQF_SHARED, dev->name, dev);
+	i = request_irq(irq, intr_handler, IRQF_SHARED, dev->name, dev);
 	if (i) return i;
 
 	if (netif_msg_ifup(np))
 		printk(KERN_DEBUG "%s: netdev_open() irq %d.\n",
-			dev->name, dev->irq);
+			dev->name, irq);
 	i = alloc_ring(dev);
 	if (i < 0) {
-		free_irq(dev->irq, dev);
+		free_irq(irq, dev);
 		return i;
 	}
 	napi_enable(&np->napi);
@@ -1794,6 +1794,7 @@ static void netdev_timer(unsigned long data)
 	struct netdev_private *np = netdev_priv(dev);
 	void __iomem * ioaddr = ns_ioaddr(dev);
 	int next_tick = NATSEMI_TIMER_FREQ;
+	const int irq = np->pci_dev->irq;
 
 	if (netif_msg_timer(np)) {
 		/* DO NOT read the IntrStatus register,
@@ -1817,14 +1818,14 @@ static void netdev_timer(unsigned long data)
 				if (netif_msg_drv(np))
 					printk(KERN_NOTICE "%s: possible phy reset: "
 						"re-initializing\n", dev->name);
-				disable_irq(dev->irq);
+				disable_irq(irq);
 				spin_lock_irq(&np->lock);
 				natsemi_stop_rxtx(dev);
 				dump_ring(dev);
 				reinit_ring(dev);
 				init_registers(dev);
 				spin_unlock_irq(&np->lock);
-				enable_irq(dev->irq);
+				enable_irq(irq);
 			} else {
 				/* hurry back */
 				next_tick = HZ;
@@ -1841,10 +1842,10 @@ static void netdev_timer(unsigned long data)
 		spin_unlock_irq(&np->lock);
 	}
 	if (np->oom) {
-		disable_irq(dev->irq);
+		disable_irq(irq);
 		np->oom = 0;
 		refill_rx(dev);
-		enable_irq(dev->irq);
+		enable_irq(irq);
 		if (!np->oom) {
 			writel(RxOn, ioaddr + ChipCmd);
 		} else {
@@ -1885,8 +1886,9 @@ static void ns_tx_timeout(struct net_device *dev)
 {
 	struct netdev_private *np = netdev_priv(dev);
 	void __iomem * ioaddr = ns_ioaddr(dev);
+	const int irq = np->pci_dev->irq;
 
-	disable_irq(dev->irq);
+	disable_irq(irq);
 	spin_lock_irq(&np->lock);
 	if (!np->hands_off) {
 		if (netif_msg_tx_err(np))
@@ -1905,7 +1907,7 @@ static void ns_tx_timeout(struct net_device *dev)
 			dev->name);
 	}
 	spin_unlock_irq(&np->lock);
-	enable_irq(dev->irq);
+	enable_irq(irq);
 
 	dev->trans_start = jiffies; /* prevent tx timeout */
 	dev->stats.tx_errors++;
@@ -2470,9 +2472,12 @@ static struct net_device_stats *get_stats(struct net_device *dev)
 #ifdef CONFIG_NET_POLL_CONTROLLER
 static void natsemi_poll_controller(struct net_device *dev)
 {
-	disable_irq(dev->irq);
-	intr_handler(dev->irq, dev);
-	enable_irq(dev->irq);
+	struct netdev_private *np = netdev_priv(dev);
+	const int irq = np->pci_dev->irq;
+
+	disable_irq(irq);
+	intr_handler(irq, dev);
+	enable_irq(irq);
 }
 #endif
 
@@ -2523,8 +2528,9 @@ static int natsemi_change_mtu(struct net_device *dev, int new_mtu)
 	if (netif_running(dev)) {
 		struct netdev_private *np = netdev_priv(dev);
 		void __iomem * ioaddr = ns_ioaddr(dev);
+		const int irq = np->pci_dev->irq;
 
-		disable_irq(dev->irq);
+		disable_irq(irq);
 		spin_lock(&np->lock);
 		/* stop engines */
 		natsemi_stop_rxtx(dev);
@@ -2537,7 +2543,7 @@ static int natsemi_change_mtu(struct net_device *dev, int new_mtu)
 		/* restart engines */
 		writel(RxOn | TxOn, ioaddr + ChipCmd);
 		spin_unlock(&np->lock);
-		enable_irq(dev->irq);
+		enable_irq(irq);
 	}
 	return 0;
 }
@@ -3135,6 +3141,7 @@ static int netdev_close(struct net_device *dev)
 {
 	void __iomem * ioaddr = ns_ioaddr(dev);
 	struct netdev_private *np = netdev_priv(dev);
+	const int irq = np->pci_dev->irq;
 
 	if (netif_msg_ifdown(np))
 		printk(KERN_DEBUG
@@ -3156,14 +3163,14 @@ static int netdev_close(struct net_device *dev)
 	 */
 
 	del_timer_sync(&np->timer);
-	disable_irq(dev->irq);
+	disable_irq(irq);
 	spin_lock_irq(&np->lock);
 	natsemi_irq_disable(dev);
 	np->hands_off = 1;
 	spin_unlock_irq(&np->lock);
-	enable_irq(dev->irq);
+	enable_irq(irq);
 
-	free_irq(dev->irq, dev);
+	free_irq(irq, dev);
 
 	/* Interrupt disabled, interrupt handler released,
 	 * queue stopped, timer deleted, rtnl_lock held
@@ -3256,9 +3263,11 @@ static int natsemi_suspend (struct pci_dev *pdev, pm_message_t state)
 
 	rtnl_lock();
 	if (netif_running (dev)) {
+		const int irq = np->pci_dev->irq;
+
 		del_timer_sync(&np->timer);
 
-		disable_irq(dev->irq);
+		disable_irq(irq);
 		spin_lock_irq(&np->lock);
 
 		natsemi_irq_disable(dev);
@@ -3267,7 +3276,7 @@ static int natsemi_suspend (struct pci_dev *pdev, pm_message_t state)
 		netif_stop_queue(dev);
 
 		spin_unlock_irq(&np->lock);
-		enable_irq(dev->irq);
+		enable_irq(irq);
 
 		napi_disable(&np->napi);
 
@@ -3307,6 +3316,8 @@ static int natsemi_resume (struct pci_dev *pdev)
 	if (netif_device_present(dev))
 		goto out;
 	if (netif_running(dev)) {
+		const int irq = np->pci_dev->irq;
+
 		BUG_ON(!np->hands_off);
 		ret = pci_enable_device(pdev);
 		if (ret < 0) {
@@ -3320,13 +3331,13 @@ static int natsemi_resume (struct pci_dev *pdev)
 
 		natsemi_reset(dev);
 		init_ring(dev);
-		disable_irq(dev->irq);
+		disable_irq(irq);
 		spin_lock_irq(&np->lock);
 		np->hands_off = 0;
 		init_registers(dev);
 		netif_device_attach(dev);
 		spin_unlock_irq(&np->lock);
-		enable_irq(dev->irq);
+		enable_irq(irq);
 
 		mod_timer(&np->timer, round_jiffies(jiffies + 1*HZ));
 	}
diff --git a/drivers/net/ethernet/neterion/s2io.c b/drivers/net/ethernet/neterion/s2io.c
index 6338ef8606a..bb367582c1e 100644
--- a/drivers/net/ethernet/neterion/s2io.c
+++ b/drivers/net/ethernet/neterion/s2io.c
@@ -2846,6 +2846,7 @@ static int s2io_poll_inta(struct napi_struct *napi, int budget)
 static void s2io_netpoll(struct net_device *dev)
 {
 	struct s2io_nic *nic = netdev_priv(dev);
+	const int irq = nic->pdev->irq;
 	struct XENA_dev_config __iomem *bar0 = nic->bar0;
 	u64 val64 = 0xFFFFFFFFFFFFFFFFULL;
 	int i;
@@ -2855,7 +2856,7 @@ static void s2io_netpoll(struct net_device *dev)
 	if (pci_channel_offline(nic->pdev))
 		return;
 
-	disable_irq(dev->irq);
+	disable_irq(irq);
 
 	writeq(val64, &bar0->rx_traffic_int);
 	writeq(val64, &bar0->tx_traffic_int);
@@ -2884,7 +2885,7 @@ static void s2io_netpoll(struct net_device *dev)
 			break;
 		}
 	}
-	enable_irq(dev->irq);
+	enable_irq(irq);
 }
 #endif
 
@@ -3897,9 +3898,7 @@ static void remove_msix_isr(struct s2io_nic *sp)
 
 static void remove_inta_isr(struct s2io_nic *sp)
 {
-	struct net_device *dev = sp->dev;
-
-	free_irq(sp->pdev->irq, dev);
+	free_irq(sp->pdev->irq, sp->dev);
 }
 
 /* ********************************************************* *
@@ -7046,7 +7045,7 @@ static int s2io_add_isr(struct s2io_nic *sp)
 		}
 	}
 	if (sp->config.intr_type == INTA) {
-		err = request_irq((int)sp->pdev->irq, s2io_isr, IRQF_SHARED,
+		err = request_irq(sp->pdev->irq, s2io_isr, IRQF_SHARED,
 				  sp->name, dev);
 		if (err) {
 			DBG_PRINT(ERR_DBG, "%s: ISR registration failed\n",
@@ -7908,9 +7907,6 @@ s2io_init_nic(struct pci_dev *pdev, const struct pci_device_id *pre)
 		goto bar1_remap_failed;
 	}
 
-	dev->irq = pdev->irq;
-	dev->base_addr = (unsigned long)sp->bar0;
-
 	/* Initializing the BAR1 address as the start of the FIFO pointer. */
 	for (j = 0; j < MAX_TX_FIFOS; j++) {
 		mac_control->tx_FIFO_start[j] = sp->bar1 + (j * 0x00020000);
diff --git a/drivers/net/ethernet/neterion/vxge/vxge-main.c b/drivers/net/ethernet/neterion/vxge/vxge-main.c
index ef76725454d..51387c31914 100644
--- a/drivers/net/ethernet/neterion/vxge/vxge-main.c
+++ b/drivers/net/ethernet/neterion/vxge/vxge-main.c
@@ -1882,25 +1882,24 @@ static int vxge_poll_inta(struct napi_struct *napi, int budget)
  */
 static void vxge_netpoll(struct net_device *dev)
 {
-	struct __vxge_hw_device *hldev;
-	struct vxgedev *vdev;
-
-	vdev = netdev_priv(dev);
-	hldev = pci_get_drvdata(vdev->pdev);
+	struct vxgedev *vdev = netdev_priv(dev);
+	struct pci_dev *pdev = vdev->pdev;
+	struct __vxge_hw_device *hldev = pci_get_drvdata(pdev);
+	const int irq = pdev->irq;
 
 	vxge_debug_entryexit(VXGE_TRACE, "%s:%d", __func__, __LINE__);
 
-	if (pci_channel_offline(vdev->pdev))
+	if (pci_channel_offline(pdev))
 		return;
 
-	disable_irq(dev->irq);
+	disable_irq(irq);
 	vxge_hw_device_clear_tx_rx(hldev);
 
 	vxge_hw_device_clear_tx_rx(hldev);
 	VXGE_COMPLETE_ALL_RX(vdev);
 	VXGE_COMPLETE_ALL_TX(vdev);
 
-	enable_irq(dev->irq);
+	enable_irq(irq);
 
 	vxge_debug_entryexit(VXGE_TRACE,
 		"%s:%d  Exiting...", __func__, __LINE__);
@@ -2860,12 +2859,12 @@ static int vxge_open(struct net_device *dev)
 		vdev->config.rx_pause_enable);
 
 	if (vdev->vp_reset_timer.function == NULL)
-		vxge_os_timer(vdev->vp_reset_timer,
-			vxge_poll_vp_reset, vdev, (HZ/2));
+		vxge_os_timer(&vdev->vp_reset_timer, vxge_poll_vp_reset, vdev,
+			      HZ / 2);
 
 	/* There is no need to check for RxD leak and RxD lookup on Titan1A */
 	if (vdev->titan1 && vdev->vp_lockup_timer.function == NULL)
-		vxge_os_timer(vdev->vp_lockup_timer, vxge_poll_vp_lockup, vdev,
+		vxge_os_timer(&vdev->vp_lockup_timer, vxge_poll_vp_lockup, vdev,
 			      HZ / 2);
 
 	set_bit(__VXGE_STATE_CARD_UP, &vdev->state);
@@ -3424,9 +3423,6 @@ static int __devinit vxge_device_register(struct __vxge_hw_device *hldev,
 	ndev->features |= ndev->hw_features |
 		NETIF_F_HW_VLAN_RX | NETIF_F_HW_VLAN_FILTER;
 
-	/*  Driver entry points */
-	ndev->irq = vdev->pdev->irq;
-	ndev->base_addr = (unsigned long) hldev->bar0;
 
 	ndev->netdev_ops = &vxge_netdev_ops;
 
diff --git a/drivers/net/ethernet/neterion/vxge/vxge-main.h b/drivers/net/ethernet/neterion/vxge/vxge-main.h
index f52a42d1dbb..35f3e7552ec 100644
--- a/drivers/net/ethernet/neterion/vxge/vxge-main.h
+++ b/drivers/net/ethernet/neterion/vxge/vxge-main.h
@@ -416,12 +416,15 @@ struct vxge_tx_priv {
 	static int p = val; \
 	module_param(p, int, 0)
 
-#define vxge_os_timer(timer, handle, arg, exp) do { \
-		init_timer(&timer); \
-		timer.function = handle; \
-		timer.data = (unsigned long) arg; \
-		mod_timer(&timer, (jiffies + exp)); \
-	} while (0);
+static inline
+void vxge_os_timer(struct timer_list *timer, void (*func)(unsigned long data),
+		   struct vxgedev *vdev, unsigned long timeout)
+{
+	init_timer(timer);
+	timer->function = func;
+	timer->data = (unsigned long)vdev;
+	mod_timer(timer, jiffies + timeout);
+}
 
 void vxge_initialize_ethtool_ops(struct net_device *ndev);
 enum vxge_hw_status vxge_reset_all_vpaths(struct vxgedev *vdev);
diff --git a/drivers/net/ethernet/nvidia/forcedeth.c b/drivers/net/ethernet/nvidia/forcedeth.c
index aca13046e43..928913c4f3f 100644
--- a/drivers/net/ethernet/nvidia/forcedeth.c
+++ b/drivers/net/ethernet/nvidia/forcedeth.c
@@ -2279,6 +2279,8 @@ static netdev_tx_t nv_start_xmit(struct sk_buff *skb, struct net_device *dev)
 
 	netdev_sent_queue(np->dev, skb->len);
 
+	skb_tx_timestamp(skb);
+
 	np->put_tx.orig = put_tx;
 
 	spin_unlock_irqrestore(&np->lock, flags);
@@ -2426,6 +2428,8 @@ static netdev_tx_t nv_start_xmit_optimized(struct sk_buff *skb,
 
 	netdev_sent_queue(np->dev, skb->len);
 
+	skb_tx_timestamp(skb);
+
 	np->put_tx.ex = put_tx;
 
 	spin_unlock_irqrestore(&np->lock, flags);
@@ -3942,13 +3946,11 @@ static int nv_request_irq(struct net_device *dev, int intr_test)
 		ret = pci_enable_msi(np->pci_dev);
 		if (ret == 0) {
 			np->msi_flags |= NV_MSI_ENABLED;
-			dev->irq = np->pci_dev->irq;
 			if (request_irq(np->pci_dev->irq, handler, IRQF_SHARED, dev->name, dev) != 0) {
 				netdev_info(dev, "request_irq failed %d\n",
 					    ret);
 				pci_disable_msi(np->pci_dev);
 				np->msi_flags &= ~NV_MSI_ENABLED;
-				dev->irq = np->pci_dev->irq;
 				goto out_err;
 			}
 
@@ -5649,9 +5651,6 @@ static int __devinit nv_probe(struct pci_dev *pci_dev, const struct pci_device_i
 	np->base = ioremap(addr, np->register_size);
 	if (!np->base)
 		goto out_relreg;
-	dev->base_addr = (unsigned long)np->base;
-
-	dev->irq = pci_dev->irq;
 
 	np->rx_ring_size = RX_RING_DEFAULT;
 	np->tx_ring_size = TX_RING_DEFAULT;
diff --git a/drivers/net/ethernet/nxp/lpc_eth.c b/drivers/net/ethernet/nxp/lpc_eth.c
index 6dfc26d85e4..8d2666fcffd 100644
--- a/drivers/net/ethernet/nxp/lpc_eth.c
+++ b/drivers/net/ethernet/nxp/lpc_eth.c
@@ -40,6 +40,7 @@
 #include <linux/skbuff.h>
 #include <linux/phy.h>
 #include <linux/dma-mapping.h>
+#include <linux/of.h>
 #include <linux/of_net.h>
 #include <linux/types.h>
 
@@ -340,13 +341,17 @@
  */
 #define LPC_POWERDOWN_MACAHB			(1 << 31)
 
-/* Upon the upcoming introduction of device tree usage in LPC32xx,
- * lpc_phy_interface_mode() and use_iram_for_net() will be extended with a
- * device parameter for access to device tree information at runtime, instead
- * of defining the values at compile time
- */
-static inline phy_interface_t lpc_phy_interface_mode(void)
+static phy_interface_t lpc_phy_interface_mode(struct device *dev)
 {
+	if (dev && dev->of_node) {
+		const char *mode = of_get_property(dev->of_node,
+						   "phy-mode", NULL);
+		if (mode && !strcmp(mode, "mii"))
+			return PHY_INTERFACE_MODE_MII;
+		return PHY_INTERFACE_MODE_RMII;
+	}
+
+	/* non-DT */
 #ifdef CONFIG_ARCH_LPC32XX_MII_SUPPORT
 	return PHY_INTERFACE_MODE_MII;
 #else
@@ -354,12 +359,16 @@ static inline phy_interface_t lpc_phy_interface_mode(void)
 #endif
 }
 
-static inline int use_iram_for_net(void)
+static bool use_iram_for_net(struct device *dev)
 {
+	if (dev && dev->of_node)
+		return of_property_read_bool(dev->of_node, "use-iram");
+
+	/* non-DT */
 #ifdef CONFIG_ARCH_LPC32XX_IRAM_FOR_NET
-	return 1;
+	return true;
 #else
-	return 0;
+	return false;
 #endif
 }
 
@@ -664,7 +673,7 @@ static void __lpc_eth_init(struct netdata_local *pldat)
 	       LPC_ENET_CLRT(pldat->net_base));
 	writel(LPC_IPGR_LOAD_PART2(0x12), LPC_ENET_IPGR(pldat->net_base));
 
-	if (lpc_phy_interface_mode() == PHY_INTERFACE_MODE_MII)
+	if (lpc_phy_interface_mode(&pldat->pdev->dev) == PHY_INTERFACE_MODE_MII)
 		writel(LPC_COMMAND_PASSRUNTFRAME,
 		       LPC_ENET_COMMAND(pldat->net_base));
 	else {
@@ -804,12 +813,13 @@ static int lpc_mii_probe(struct net_device *ndev)
 	}
 
 	/* Attach to the PHY */
-	if (lpc_phy_interface_mode() == PHY_INTERFACE_MODE_MII)
+	if (lpc_phy_interface_mode(&pldat->pdev->dev) == PHY_INTERFACE_MODE_MII)
 		netdev_info(ndev, "using MII interface\n");
 	else
 		netdev_info(ndev, "using RMII interface\n");
 	phydev = phy_connect(ndev, dev_name(&phydev->dev),
-		&lpc_handle_link_change, 0, lpc_phy_interface_mode());
+			     &lpc_handle_link_change, 0,
+			     lpc_phy_interface_mode(&pldat->pdev->dev));
 
 	if (IS_ERR(phydev)) {
 		netdev_err(ndev, "Could not attach to PHY\n");
@@ -843,7 +853,7 @@ static int lpc_mii_init(struct netdata_local *pldat)
 	}
 
 	/* Setup MII mode */
-	if (lpc_phy_interface_mode() == PHY_INTERFACE_MODE_MII)
+	if (lpc_phy_interface_mode(&pldat->pdev->dev) == PHY_INTERFACE_MODE_MII)
 		writel(LPC_COMMAND_PASSRUNTFRAME,
 		       LPC_ENET_COMMAND(pldat->net_base));
 	else {
@@ -990,10 +1000,10 @@ static int __lpc_handle_recv(struct net_device *ndev, int budget)
 			ndev->stats.rx_errors++;
 		} else {
 			/* Packet is good */
-			skb = dev_alloc_skb(len + 8);
-			if (!skb)
+			skb = dev_alloc_skb(len);
+			if (!skb) {
 				ndev->stats.rx_dropped++;
-			else {
+			} else {
 				prdbuf = skb_put(skb, len);
 
 				/* Copy packet from buffer */
@@ -1315,18 +1325,26 @@ static const struct net_device_ops lpc_netdev_ops = {
 static int lpc_eth_drv_probe(struct platform_device *pdev)
 {
 	struct resource *res;
-	struct resource *dma_res;
 	struct net_device *ndev;
 	struct netdata_local *pldat;
 	struct phy_device *phydev;
 	dma_addr_t dma_handle;
 	int irq, ret;
+	u32 tmp;
+
+	/* Setup network interface for RMII or MII mode */
+	tmp = __raw_readl(LPC32XX_CLKPWR_MACCLK_CTRL);
+	tmp &= ~LPC32XX_CLKPWR_MACCTRL_PINS_MSK;
+	if (lpc_phy_interface_mode(&pdev->dev) == PHY_INTERFACE_MODE_MII)
+		tmp |= LPC32XX_CLKPWR_MACCTRL_USE_MII_PINS;
+	else
+		tmp |= LPC32XX_CLKPWR_MACCTRL_USE_RMII_PINS;
+	__raw_writel(tmp, LPC32XX_CLKPWR_MACCLK_CTRL);
 
 	/* Get platform resources */
 	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
-	dma_res = platform_get_resource(pdev, IORESOURCE_MEM, 1);
 	irq = platform_get_irq(pdev, 0);
-	if ((!res) || (!dma_res) || (irq < 0) || (irq >= NR_IRQS)) {
+	if ((!res) || (irq < 0) || (irq >= NR_IRQS)) {
 		dev_err(&pdev->dev, "error getting resources.\n");
 		ret = -ENXIO;
 		goto err_exit;
@@ -1389,17 +1407,19 @@ static int lpc_eth_drv_probe(struct platform_device *pdev)
 		sizeof(struct txrx_desc_t) + sizeof(struct rx_status_t));
 	pldat->dma_buff_base_v = 0;
 
-	if (use_iram_for_net()) {
-		dma_handle = dma_res->start;
+	if (use_iram_for_net(&pldat->pdev->dev)) {
+		dma_handle = LPC32XX_IRAM_BASE;
 		if (pldat->dma_buff_size <= lpc32xx_return_iram_size())
 			pldat->dma_buff_base_v =
-				io_p2v(dma_res->start);
+				io_p2v(LPC32XX_IRAM_BASE);
 		else
 			netdev_err(ndev,
 				"IRAM not big enough for net buffers, using SDRAM instead.\n");
 	}
 
 	if (pldat->dma_buff_base_v == 0) {
+		pldat->pdev->dev.coherent_dma_mask = 0xFFFFFFFF;
+		pldat->pdev->dev.dma_mask = &pldat->pdev->dev.coherent_dma_mask;
 		pldat->dma_buff_size = PAGE_ALIGN(pldat->dma_buff_size);
 
 		/* Allocate a chunk of memory for the DMA ethernet buffers
@@ -1488,7 +1508,7 @@ err_out_unregister_netdev:
 	platform_set_drvdata(pdev, NULL);
 	unregister_netdev(ndev);
 err_out_dma_unmap:
-	if (!use_iram_for_net() ||
+	if (!use_iram_for_net(&pldat->pdev->dev) ||
 	    pldat->dma_buff_size > lpc32xx_return_iram_size())
 		dma_free_coherent(&pldat->pdev->dev, pldat->dma_buff_size,
 				  pldat->dma_buff_base_v,
@@ -1515,7 +1535,7 @@ static int lpc_eth_drv_remove(struct platform_device *pdev)
 	unregister_netdev(ndev);
 	platform_set_drvdata(pdev, NULL);
 
-	if (!use_iram_for_net() ||
+	if (!use_iram_for_net(&pldat->pdev->dev) ||
 	    pldat->dma_buff_size > lpc32xx_return_iram_size())
 		dma_free_coherent(&pldat->pdev->dev, pldat->dma_buff_size,
 				  pldat->dma_buff_base_v,
@@ -1584,6 +1604,14 @@ static int lpc_eth_drv_resume(struct platform_device *pdev)
 }
 #endif
 
+#ifdef CONFIG_OF
+static const struct of_device_id lpc_eth_match[] = {
+	{ .compatible = "nxp,lpc-eth" },
+	{ }
+};
+MODULE_DEVICE_TABLE(of, lpc_eth_match);
+#endif
+
 static struct platform_driver lpc_eth_driver = {
 	.probe		= lpc_eth_drv_probe,
 	.remove		= __devexit_p(lpc_eth_drv_remove),
@@ -1593,6 +1621,7 @@ static struct platform_driver lpc_eth_driver = {
 #endif
 	.driver		= {
 		.name	= MODNAME,
+		.of_match_table = of_match_ptr(lpc_eth_match),
 	},
 };
 
diff --git a/drivers/net/ethernet/oki-semi/pch_gbe/pch_gbe.h b/drivers/net/ethernet/oki-semi/pch_gbe/pch_gbe.h
index dd14915f54b..b07311eaa69 100644
--- a/drivers/net/ethernet/oki-semi/pch_gbe/pch_gbe.h
+++ b/drivers/net/ethernet/oki-semi/pch_gbe/pch_gbe.h
@@ -584,7 +584,6 @@ struct pch_gbe_hw_stats {
 /**
  * struct pch_gbe_adapter - board specific private data structure
  * @stats_lock:	Spinlock structure for status
- * @tx_queue_lock:	Spinlock structure for transmit
  * @ethtool_lock:	Spinlock structure for ethtool
  * @irq_sem:		Semaphore for interrupt
  * @netdev:		Pointer of network device structure
@@ -609,7 +608,6 @@ struct pch_gbe_hw_stats {
 
 struct pch_gbe_adapter {
 	spinlock_t stats_lock;
-	spinlock_t tx_queue_lock;
 	spinlock_t ethtool_lock;
 	atomic_t irq_sem;
 	struct net_device *netdev;
@@ -660,6 +658,7 @@ extern u32 pch_src_uuid_lo_read(struct pci_dev *pdev);
 extern u32 pch_src_uuid_hi_read(struct pci_dev *pdev);
 extern u64 pch_rx_snap_read(struct pci_dev *pdev);
 extern u64 pch_tx_snap_read(struct pci_dev *pdev);
+extern int pch_set_station_address(u8 *addr, struct pci_dev *pdev);
 #endif
 
 /* pch_gbe_param.c */
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 8035e5ff6e0..3787c64ee71 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
@@ -79,7 +79,6 @@ const char pch_driver_version[] = DRV_VERSION;
 #define	PCH_GBE_PAUSE_PKT4_VALUE    0x01000888
 #define	PCH_GBE_PAUSE_PKT5_VALUE    0x0000FFFF
 
-#define PCH_GBE_ETH_ALEN            6
 
 /* This defines the bits that are set in the Interrupt Mask
  * Set/Read Register.  Each bit is documented below:
@@ -101,18 +100,19 @@ const char pch_driver_version[] = DRV_VERSION;
 
 #ifdef CONFIG_PCH_PTP
 /* Macros for ieee1588 */
-#define TICKS_NS_SHIFT  5
-
 /* 0x40 Time Synchronization Channel Control Register Bits */
 #define MASTER_MODE   (1<<0)
-#define SLAVE_MODE    (0<<0)
+#define SLAVE_MODE    (0)
 #define V2_MODE       (1<<31)
-#define CAP_MODE0     (0<<16)
+#define CAP_MODE0     (0)
 #define CAP_MODE2     (1<<17)
 
 /* 0x44 Time Synchronization Channel Event Register Bits */
 #define TX_SNAPSHOT_LOCKED (1<<0)
 #define RX_SNAPSHOT_LOCKED (1<<1)
+
+#define PTP_L4_MULTICAST_SA "01:00:5e:00:01:81"
+#define PTP_L2_MULTICAST_SA "01:1b:19:00:00:00"
 #endif
 
 static unsigned int copybreak __read_mostly = PCH_GBE_COPYBREAK_DEFAULT;
@@ -120,6 +120,7 @@ static unsigned int copybreak __read_mostly = PCH_GBE_COPYBREAK_DEFAULT;
 static int pch_gbe_mdio_read(struct net_device *netdev, int addr, int reg);
 static void pch_gbe_mdio_write(struct net_device *netdev, int addr, int reg,
 			       int data);
+static void pch_gbe_set_multi(struct net_device *netdev);
 
 #ifdef CONFIG_PCH_PTP
 static struct sock_filter ptp_filter[] = {
@@ -133,10 +134,8 @@ static int pch_ptp_match(struct sk_buff *skb, u16 uid_hi, u32 uid_lo, u16 seqid)
 	u16 *hi, *id;
 	u32 lo;
 
-	if ((sk_run_filter(skb, ptp_filter) != PTP_CLASS_V2_IPV4) &&
-		(sk_run_filter(skb, ptp_filter) != PTP_CLASS_V1_IPV4)) {
+	if (sk_run_filter(skb, ptp_filter) == PTP_CLASS_NONE)
 		return 0;
-	}
 
 	offset = ETH_HLEN + IPV4_HLEN(data) + UDP_HLEN;
 
@@ -153,8 +152,8 @@ static int pch_ptp_match(struct sk_buff *skb, u16 uid_hi, u32 uid_lo, u16 seqid)
 		seqid  == *id);
 }
 
-static void pch_rx_timestamp(
-			struct pch_gbe_adapter *adapter, struct sk_buff *skb)
+static void
+pch_rx_timestamp(struct pch_gbe_adapter *adapter, struct sk_buff *skb)
 {
 	struct skb_shared_hwtstamps *shhwtstamps;
 	struct pci_dev *pdev;
@@ -183,7 +182,6 @@ static void pch_rx_timestamp(
 		goto out;
 
 	ns = pch_rx_snap_read(pdev);
-	ns <<= TICKS_NS_SHIFT;
 
 	shhwtstamps = skb_hwtstamps(skb);
 	memset(shhwtstamps, 0, sizeof(*shhwtstamps));
@@ -192,8 +190,8 @@ out:
 	pch_ch_event_write(pdev, RX_SNAPSHOT_LOCKED);
 }
 
-static void pch_tx_timestamp(
-			struct pch_gbe_adapter *adapter, struct sk_buff *skb)
+static void
+pch_tx_timestamp(struct pch_gbe_adapter *adapter, struct sk_buff *skb)
 {
 	struct skb_shared_hwtstamps shhwtstamps;
 	struct pci_dev *pdev;
@@ -202,17 +200,16 @@ static void pch_tx_timestamp(
 	u32 cnt, val;
 
 	shtx = skb_shinfo(skb);
-	if (unlikely(shtx->tx_flags & SKBTX_HW_TSTAMP && adapter->hwts_tx_en))
-		shtx->tx_flags |= SKBTX_IN_PROGRESS;
-	else
+	if (likely(!(shtx->tx_flags & SKBTX_HW_TSTAMP && adapter->hwts_tx_en)))
 		return;
 
+	shtx->tx_flags |= SKBTX_IN_PROGRESS;
+
 	/* Get ieee1588's dev information */
 	pdev = adapter->ptp_pdev;
 
 	/*
 	 * This really stinks, but we have to poll for the Tx time stamp.
-	 * Usually, the time stamp is ready after 4 to 6 microseconds.
 	 */
 	for (cnt = 0; cnt < 100; cnt++) {
 		val = pch_ch_event_read(pdev);
@@ -226,7 +223,6 @@ static void pch_tx_timestamp(
 	}
 
 	ns = pch_tx_snap_read(pdev);
-	ns <<= TICKS_NS_SHIFT;
 
 	memset(&shhwtstamps, 0, sizeof(shhwtstamps));
 	shhwtstamps.hwtstamp = ns_to_ktime(ns);
@@ -240,6 +236,7 @@ static int hwtstamp_ioctl(struct net_device *netdev, struct ifreq *ifr, int cmd)
 	struct hwtstamp_config cfg;
 	struct pch_gbe_adapter *adapter = netdev_priv(netdev);
 	struct pci_dev *pdev;
+	u8 station[20];
 
 	if (copy_from_user(&cfg, ifr->ifr_data, sizeof(cfg)))
 		return -EFAULT;
@@ -267,15 +264,23 @@ static int hwtstamp_ioctl(struct net_device *netdev, struct ifreq *ifr, int cmd)
 		break;
 	case HWTSTAMP_FILTER_PTP_V1_L4_SYNC:
 		adapter->hwts_rx_en = 0;
-		pch_ch_control_write(pdev, (SLAVE_MODE | CAP_MODE0));
+		pch_ch_control_write(pdev, SLAVE_MODE | CAP_MODE0);
 		break;
 	case HWTSTAMP_FILTER_PTP_V1_L4_DELAY_REQ:
 		adapter->hwts_rx_en = 1;
-		pch_ch_control_write(pdev, (MASTER_MODE | CAP_MODE0));
+		pch_ch_control_write(pdev, MASTER_MODE | CAP_MODE0);
+		break;
+	case HWTSTAMP_FILTER_PTP_V2_L4_EVENT:
+		adapter->hwts_rx_en = 1;
+		pch_ch_control_write(pdev, V2_MODE | CAP_MODE2);
+		strcpy(station, PTP_L4_MULTICAST_SA);
+		pch_set_station_address(station, pdev);
 		break;
-	case HWTSTAMP_FILTER_PTP_V2_EVENT:
+	case HWTSTAMP_FILTER_PTP_V2_L2_EVENT:
 		adapter->hwts_rx_en = 1;
-		pch_ch_control_write(pdev, (V2_MODE | CAP_MODE2));
+		pch_ch_control_write(pdev, V2_MODE | CAP_MODE2);
+		strcpy(station, PTP_L2_MULTICAST_SA);
+		pch_set_station_address(station, pdev);
 		break;
 	default:
 		return -ERANGE;
@@ -399,18 +404,18 @@ static void pch_gbe_mac_reset_hw(struct pch_gbe_hw *hw)
 	iowrite32(PCH_GBE_MODE_GMII_ETHER, &hw->reg->MODE);
 #endif
 	pch_gbe_wait_clr_bit(&hw->reg->RESET, PCH_GBE_ALL_RST);
-	/* Setup the receive address */
+	/* Setup the receive addresses */
 	pch_gbe_mac_mar_set(hw, hw->mac.addr, 0);
 	return;
 }
 
 static void pch_gbe_mac_reset_rx(struct pch_gbe_hw *hw)
 {
-	/* Read the MAC address. and store to the private data */
+	/* Read the MAC addresses. and store to the private data */
 	pch_gbe_mac_read_mac_addr(hw);
 	iowrite32(PCH_GBE_RX_RST, &hw->reg->RESET);
 	pch_gbe_wait_clr_bit_irq(&hw->reg->RESET, PCH_GBE_RX_RST);
-	/* Setup the MAC address */
+	/* Setup the MAC addresses */
 	pch_gbe_mac_mar_set(hw, hw->mac.addr, 0);
 	return;
 }
@@ -460,7 +465,7 @@ static void pch_gbe_mac_mc_addr_list_update(struct pch_gbe_hw *hw,
 		if (mc_addr_count) {
 			pch_gbe_mac_mar_set(hw, mc_addr_list, i);
 			mc_addr_count--;
-			mc_addr_list += PCH_GBE_ETH_ALEN;
+			mc_addr_list += ETH_ALEN;
 		} else {
 			/* Clear MAC address mask */
 			adrmask = ioread32(&hw->reg->ADDR_MASK);
@@ -640,14 +645,11 @@ static void pch_gbe_mac_set_pause_packet(struct pch_gbe_hw *hw)
  */
 static int pch_gbe_alloc_queues(struct pch_gbe_adapter *adapter)
 {
-	int size;
-
-	size = (int)sizeof(struct pch_gbe_tx_ring);
-	adapter->tx_ring = kzalloc(size, GFP_KERNEL);
+	adapter->tx_ring = kzalloc(sizeof(*adapter->tx_ring), GFP_KERNEL);
 	if (!adapter->tx_ring)
 		return -ENOMEM;
-	size = (int)sizeof(struct pch_gbe_rx_ring);
-	adapter->rx_ring = kzalloc(size, GFP_KERNEL);
+
+	adapter->rx_ring = kzalloc(sizeof(*adapter->rx_ring), GFP_KERNEL);
 	if (!adapter->rx_ring) {
 		kfree(adapter->tx_ring);
 		return -ENOMEM;
@@ -778,6 +780,8 @@ void pch_gbe_reinit_locked(struct pch_gbe_adapter *adapter)
 void pch_gbe_reset(struct pch_gbe_adapter *adapter)
 {
 	pch_gbe_mac_reset_hw(&adapter->hw);
+	/* reprogram multicast address register after reset */
+	pch_gbe_set_multi(adapter->netdev);
 	/* Setup the receive address. */
 	pch_gbe_mac_init_rx_addrs(&adapter->hw, PCH_GBE_MAR_ENTRIES);
 	if (pch_gbe_hal_init_hw(&adapter->hw))
@@ -1162,7 +1166,6 @@ static void pch_gbe_tx_queue(struct pch_gbe_adapter *adapter,
 	struct sk_buff *tmp_skb;
 	unsigned int frame_ctrl;
 	unsigned int ring_num;
-	unsigned long flags;
 
 	/*-- Set frame control --*/
 	frame_ctrl = 0;
@@ -1182,8 +1185,6 @@ static void pch_gbe_tx_queue(struct pch_gbe_adapter *adapter,
 		if (skb->protocol == htons(ETH_P_IP)) {
 			struct iphdr *iph = ip_hdr(skb);
 			unsigned int offset;
-			iph->check = 0;
-			iph->check = ip_fast_csum((u8 *) iph, iph->ihl);
 			offset = skb_transport_offset(skb);
 			if (iph->protocol == IPPROTO_TCP) {
 				skb->csum = 0;
@@ -1211,14 +1212,14 @@ static void pch_gbe_tx_queue(struct pch_gbe_adapter *adapter,
 			}
 		}
 	}
-	spin_lock_irqsave(&tx_ring->tx_lock, flags);
+
 	ring_num = tx_ring->next_to_use;
 	if (unlikely((ring_num + 1) == tx_ring->count))
 		tx_ring->next_to_use = 0;
 	else
 		tx_ring->next_to_use = ring_num + 1;
 
-	spin_unlock_irqrestore(&tx_ring->tx_lock, flags);
+
 	buffer_info = &tx_ring->buffer_info[ring_num];
 	tmp_skb = buffer_info->skb;
 
@@ -1342,6 +1343,8 @@ static void pch_gbe_stop_receive(struct pch_gbe_adapter *adapter)
 		/* Stop Receive */
 		pch_gbe_mac_reset_rx(hw);
 	}
+	/* reprogram multicast address register after reset */
+	pch_gbe_set_multi(adapter->netdev);
 }
 
 static void pch_gbe_start_receive(struct pch_gbe_hw *hw)
@@ -1518,7 +1521,7 @@ pch_gbe_alloc_rx_buffers_pool(struct pch_gbe_adapter *adapter,
 						&rx_ring->rx_buff_pool_logic,
 						GFP_KERNEL);
 	if (!rx_ring->rx_buff_pool) {
-		pr_err("Unable to allocate memory for the receive poll buffer\n");
+		pr_err("Unable to allocate memory for the receive pool buffer\n");
 		return -ENOMEM;
 	}
 	memset(rx_ring->rx_buff_pool, 0, size);
@@ -1637,15 +1640,17 @@ pch_gbe_clean_tx(struct pch_gbe_adapter *adapter,
 	pr_debug("called pch_gbe_unmap_and_free_tx_resource() %d count\n",
 		 cleaned_count);
 	/* Recover from running out of Tx resources in xmit_frame */
+	spin_lock(&tx_ring->tx_lock);
 	if (unlikely(cleaned && (netif_queue_stopped(adapter->netdev)))) {
 		netif_wake_queue(adapter->netdev);
 		adapter->stats.tx_restart_count++;
 		pr_debug("Tx wake queue\n");
 	}
-	spin_lock(&adapter->tx_queue_lock);
+
 	tx_ring->next_to_clean = i;
-	spin_unlock(&adapter->tx_queue_lock);
+
 	pr_debug("next_to_clean : %d\n", tx_ring->next_to_clean);
+	spin_unlock(&tx_ring->tx_lock);
 	return cleaned;
 }
 
@@ -1924,7 +1929,6 @@ static int pch_gbe_request_irq(struct pch_gbe_adapter *adapter)
 }
 
 
-static void pch_gbe_set_multi(struct net_device *netdev);
 /**
  * pch_gbe_up - Up GbE network device
  * @adapter:  Board private structure
@@ -2037,7 +2041,6 @@ static int pch_gbe_sw_init(struct pch_gbe_adapter *adapter)
 		return -ENOMEM;
 	}
 	spin_lock_init(&adapter->hw.miim_lock);
-	spin_lock_init(&adapter->tx_queue_lock);
 	spin_lock_init(&adapter->stats_lock);
 	spin_lock_init(&adapter->ethtool_lock);
 	atomic_set(&adapter->irq_sem, 0);
@@ -2142,10 +2145,10 @@ static int pch_gbe_xmit_frame(struct sk_buff *skb, struct net_device *netdev)
 			 tx_ring->next_to_use, tx_ring->next_to_clean);
 		return NETDEV_TX_BUSY;
 	}
-	spin_unlock_irqrestore(&tx_ring->tx_lock, flags);
 
 	/* CRC,ITAG no support */
 	pch_gbe_tx_queue(adapter, tx_ring, skb);
+	spin_unlock_irqrestore(&tx_ring->tx_lock, flags);
 	return NETDEV_TX_OK;
 }
 
diff --git a/drivers/net/ethernet/packetengines/hamachi.c b/drivers/net/ethernet/packetengines/hamachi.c
index 0d29f5f4b8e..c2367158350 100644
--- a/drivers/net/ethernet/packetengines/hamachi.c
+++ b/drivers/net/ethernet/packetengines/hamachi.c
@@ -683,8 +683,6 @@ static int __devinit hamachi_init_one (struct pci_dev *pdev,
 	}
 
 	hmp->base = ioaddr;
-	dev->base_addr = (unsigned long)ioaddr;
-	dev->irq = irq;
 	pci_set_drvdata(pdev, dev);
 
 	hmp->chip_id = chip_id;
@@ -859,14 +857,11 @@ static int hamachi_open(struct net_device *dev)
 	u32 rx_int_var, tx_int_var;
 	u16 fifo_info;
 
-	i = request_irq(dev->irq, hamachi_interrupt, IRQF_SHARED, dev->name, dev);
+	i = request_irq(hmp->pci_dev->irq, hamachi_interrupt, IRQF_SHARED,
+			dev->name, dev);
 	if (i)
 		return i;
 
-	if (hamachi_debug > 1)
-		printk(KERN_DEBUG "%s: hamachi_open() irq %d.\n",
-			   dev->name, dev->irq);
-
 	hamachi_init_ring(dev);
 
 #if ADDRLEN == 64
@@ -1705,7 +1700,7 @@ static int hamachi_close(struct net_device *dev)
 	}
 #endif /* __i386__ debugging only */
 
-	free_irq(dev->irq, dev);
+	free_irq(hmp->pci_dev->irq, dev);
 
 	del_timer_sync(&hmp->timer);
 
diff --git a/drivers/net/ethernet/packetengines/yellowfin.c b/drivers/net/ethernet/packetengines/yellowfin.c
index 7757b80ef92..04e622fd468 100644
--- a/drivers/net/ethernet/packetengines/yellowfin.c
+++ b/drivers/net/ethernet/packetengines/yellowfin.c
@@ -427,9 +427,6 @@ static int __devinit yellowfin_init_one(struct pci_dev *pdev,
 	/* Reset the chip. */
 	iowrite32(0x80000000, ioaddr + DMACtrl);
 
-	dev->base_addr = (unsigned long)ioaddr;
-	dev->irq = irq;
-
 	pci_set_drvdata(pdev, dev);
 	spin_lock_init(&np->lock);
 
@@ -569,25 +566,20 @@ static void mdio_write(void __iomem *ioaddr, int phy_id, int location, int value
 static int yellowfin_open(struct net_device *dev)
 {
 	struct yellowfin_private *yp = netdev_priv(dev);
+	const int irq = yp->pci_dev->irq;
 	void __iomem *ioaddr = yp->base;
-	int i, ret;
+	int i, rc;
 
 	/* Reset the chip. */
 	iowrite32(0x80000000, ioaddr + DMACtrl);
 
-	ret = request_irq(dev->irq, yellowfin_interrupt, IRQF_SHARED, dev->name, dev);
-	if (ret)
-		return ret;
-
-	if (yellowfin_debug > 1)
-		netdev_printk(KERN_DEBUG, dev, "%s() irq %d\n",
-			      __func__, dev->irq);
+	rc = request_irq(irq, yellowfin_interrupt, IRQF_SHARED, dev->name, dev);
+	if (rc)
+		return rc;
 
-	ret = yellowfin_init_ring(dev);
-	if (ret) {
-		free_irq(dev->irq, dev);
-		return ret;
-	}
+	rc = yellowfin_init_ring(dev);
+	if (rc < 0)
+		goto err_free_irq;
 
 	iowrite32(yp->rx_ring_dma, ioaddr + RxPtr);
 	iowrite32(yp->tx_ring_dma, ioaddr + TxPtr);
@@ -647,8 +639,12 @@ static int yellowfin_open(struct net_device *dev)
 	yp->timer.data = (unsigned long)dev;
 	yp->timer.function = yellowfin_timer;				/* timer handler */
 	add_timer(&yp->timer);
+out:
+	return rc;
 
-	return 0;
+err_free_irq:
+	free_irq(irq, dev);
+	goto out;
 }
 
 static void yellowfin_timer(unsigned long data)
@@ -1251,7 +1247,7 @@ static int yellowfin_close(struct net_device *dev)
 	}
 #endif /* __i386__ debugging only */
 
-	free_irq(dev->irq, dev);
+	free_irq(yp->pci_dev->irq, dev);
 
 	/* Free all the skbuffs in the Rx queue. */
 	for (i = 0; i < RX_RING_SIZE; i++) {
diff --git a/drivers/net/ethernet/pasemi/pasemi_mac.c b/drivers/net/ethernet/pasemi/pasemi_mac.c
index ddc95b0ac78..e559dfa06d6 100644
--- a/drivers/net/ethernet/pasemi/pasemi_mac.c
+++ b/drivers/net/ethernet/pasemi/pasemi_mac.c
@@ -623,7 +623,7 @@ static void pasemi_mac_free_rx_resources(struct pasemi_mac *mac)
 	mac->rx = NULL;
 }
 
-static void pasemi_mac_replenish_rx_ring(const struct net_device *dev,
+static void pasemi_mac_replenish_rx_ring(struct net_device *dev,
 					 const int limit)
 {
 	const struct pasemi_mac *mac = netdev_priv(dev);
diff --git a/drivers/net/ethernet/qlogic/netxen/netxen_nic.h b/drivers/net/ethernet/qlogic/netxen/netxen_nic.h
index b5de8a7b90f..37ccbe54e62 100644
--- a/drivers/net/ethernet/qlogic/netxen/netxen_nic.h
+++ b/drivers/net/ethernet/qlogic/netxen/netxen_nic.h
@@ -53,8 +53,8 @@
 
 #define _NETXEN_NIC_LINUX_MAJOR 4
 #define _NETXEN_NIC_LINUX_MINOR 0
-#define _NETXEN_NIC_LINUX_SUBVERSION 78
-#define NETXEN_NIC_LINUX_VERSIONID  "4.0.78"
+#define _NETXEN_NIC_LINUX_SUBVERSION 79
+#define NETXEN_NIC_LINUX_VERSIONID  "4.0.79"
 
 #define NETXEN_VERSION_CODE(a, b, c)	(((a) << 24) + ((b) << 16) + (c))
 #define _major(v)	(((v) >> 24) & 0xff)
@@ -419,6 +419,8 @@ struct rcv_desc {
 	(((sts_data) >> 52) & 0x1)
 #define netxen_get_lro_sts_seq_number(sts_data)		\
 	((sts_data) & 0x0FFFFFFFF)
+#define netxen_get_lro_sts_mss(sts_data1)		\
+	((sts_data1 >> 32) & 0x0FFFF)
 
 
 struct status_desc {
@@ -794,6 +796,7 @@ struct netxen_cmd_args {
 #define NX_CAP0_JUMBO_CONTIGUOUS	NX_CAP_BIT(0, 7)
 #define NX_CAP0_LRO_CONTIGUOUS		NX_CAP_BIT(0, 8)
 #define NX_CAP0_HW_LRO			NX_CAP_BIT(0, 10)
+#define NX_CAP0_HW_LRO_MSS		NX_CAP_BIT(0, 21)
 
 /*
  * Context state
@@ -1073,6 +1076,8 @@ typedef struct {
 #define NX_FW_CAPABILITY_FVLANTX		(1 << 9)
 #define NX_FW_CAPABILITY_HW_LRO			(1 << 10)
 #define NX_FW_CAPABILITY_GBE_LINK_CFG		(1 << 11)
+#define NX_FW_CAPABILITY_MORE_CAPS		(1 << 31)
+#define NX_FW_CAPABILITY_2_LRO_MAX_TCP_SEG	(1 << 2)
 
 /* module types */
 #define LINKEVENT_MODULE_NOT_PRESENT			1
@@ -1155,6 +1160,7 @@ typedef struct {
 #define NETXEN_NIC_BRIDGE_ENABLED       0X10
 #define NETXEN_NIC_DIAG_ENABLED		0x20
 #define NETXEN_FW_RESET_OWNER           0x40
+#define NETXEN_FW_MSS_CAP	        0x80
 #define NETXEN_IS_MSI_FAMILY(adapter) \
 	((adapter)->flags & (NETXEN_NIC_MSI_ENABLED | NETXEN_NIC_MSIX_ENABLED))
 
@@ -1201,6 +1207,9 @@ typedef struct {
 #define NX_FORCE_FW_RESET               0xdeaddead
 
 
+/* Fw dump levels */
+static const u32 FW_DUMP_LEVELS[] = { 0x3, 0x7, 0xf, 0x1f, 0x3f, 0x7f, 0xff };
+
 /* Flash read/write address */
 #define NX_FW_DUMP_REG1         0x00130060
 #define NX_FW_DUMP_REG2         0x001e0000
@@ -1814,6 +1823,13 @@ struct netxen_brdinfo {
 	char short_name[NETXEN_MAX_SHORT_NAME];
 };
 
+struct netxen_dimm_cfg {
+	u8 presence;
+	u8 mem_type;
+	u8 dimm_type;
+	u32 size;
+};
+
 static const struct netxen_brdinfo netxen_boards[] = {
 	{NETXEN_BRDTYPE_P2_SB31_10G_CX4, 1, "XGb CX4"},
 	{NETXEN_BRDTYPE_P2_SB31_10G_HMEZ, 1, "XGb HMEZ"},
diff --git a/drivers/net/ethernet/qlogic/netxen/netxen_nic_ctx.c b/drivers/net/ethernet/qlogic/netxen/netxen_nic_ctx.c
index f3c0057a802..7f556a84925 100644
--- a/drivers/net/ethernet/qlogic/netxen/netxen_nic_ctx.c
+++ b/drivers/net/ethernet/qlogic/netxen/netxen_nic_ctx.c
@@ -229,7 +229,7 @@ netxen_setup_minidump(struct netxen_adapter *adapter)
 				adapter->mdump.md_template;
 	adapter->mdump.md_capture_buff = NULL;
 	adapter->mdump.fw_supports_md = 1;
-	adapter->mdump.md_enabled = 1;
+	adapter->mdump.md_enabled = 0;
 
 	return err;
 
@@ -328,6 +328,9 @@ nx_fw_cmd_create_rx_ctx(struct netxen_adapter *adapter)
 	cap = (NX_CAP0_LEGACY_CONTEXT | NX_CAP0_LEGACY_MN);
 	cap |= (NX_CAP0_JUMBO_CONTIGUOUS | NX_CAP0_LRO_CONTIGUOUS);
 
+	if (adapter->flags & NETXEN_FW_MSS_CAP)
+		cap |= NX_CAP0_HW_LRO_MSS;
+
 	prq->capabilities[0] = cpu_to_le32(cap);
 	prq->host_int_crb_mode =
 		cpu_to_le32(NX_HOST_INT_CRB_MODE_SHARED);
diff --git a/drivers/net/ethernet/qlogic/netxen/netxen_nic_ethtool.c b/drivers/net/ethernet/qlogic/netxen/netxen_nic_ethtool.c
index 8c39299331a..39730403782 100644
--- a/drivers/net/ethernet/qlogic/netxen/netxen_nic_ethtool.c
+++ b/drivers/net/ethernet/qlogic/netxen/netxen_nic_ethtool.c
@@ -834,7 +834,7 @@ netxen_get_dump_flag(struct net_device *netdev, struct ethtool_dump *dump)
 static int
 netxen_set_dump(struct net_device *netdev, struct ethtool_dump *val)
 {
-	int ret = 0;
+	int i;
 	struct netxen_adapter *adapter = netdev_priv(netdev);
 	struct netxen_minidump *mdump = &adapter->mdump;
 
@@ -844,7 +844,7 @@ netxen_set_dump(struct net_device *netdev, struct ethtool_dump *val)
 			mdump->md_enabled = 1;
 		if (adapter->fw_mdump_rdy) {
 			netdev_info(netdev, "Previous dump not cleared, not forcing dump\n");
-			return ret;
+			return 0;
 		}
 		netdev_info(netdev, "Forcing a fw dump\n");
 		nx_dev_request_reset(adapter);
@@ -867,19 +867,21 @@ netxen_set_dump(struct net_device *netdev, struct ethtool_dump *val)
 		adapter->flags &= ~NETXEN_FW_RESET_OWNER;
 		break;
 	default:
-		if (val->flag <= NX_DUMP_MASK_MAX &&
-			val->flag >= NX_DUMP_MASK_MIN) {
-			mdump->md_capture_mask = val->flag & 0xff;
-			netdev_info(netdev, "Driver mask changed to: 0x%x\n",
+		for (i = 0; i < ARRAY_SIZE(FW_DUMP_LEVELS); i++) {
+			if (val->flag == FW_DUMP_LEVELS[i]) {
+				mdump->md_capture_mask = val->flag;
+				netdev_info(netdev,
+					"Driver mask changed to: 0x%x\n",
 					mdump->md_capture_mask);
-			break;
+				return 0;
+			}
 		}
 		netdev_info(netdev,
 			"Invalid dump level: 0x%x\n", val->flag);
 		return -EINVAL;
 	}
 
-	return ret;
+	return 0;
 }
 
 static int
diff --git a/drivers/net/ethernet/qlogic/netxen/netxen_nic_hdr.h b/drivers/net/ethernet/qlogic/netxen/netxen_nic_hdr.h
index b1a897cd9a8..28e076960bc 100644
--- a/drivers/net/ethernet/qlogic/netxen/netxen_nic_hdr.h
+++ b/drivers/net/ethernet/qlogic/netxen/netxen_nic_hdr.h
@@ -776,6 +776,7 @@ enum {
 #define CRB_SW_INT_MASK_3		(NETXEN_NIC_REG(0x1e8))
 
 #define CRB_FW_CAPABILITIES_1		(NETXEN_CAM_RAM(0x128))
+#define CRB_FW_CAPABILITIES_2		(NETXEN_CAM_RAM(0x12c))
 #define CRB_MAC_BLOCK_START		(NETXEN_CAM_RAM(0x1c0))
 
 /*
@@ -955,6 +956,31 @@ enum {
 #define NX_CRB_DEV_REF_COUNT		(NETXEN_CAM_RAM(0x138))
 #define NX_CRB_DEV_STATE		(NETXEN_CAM_RAM(0x140))
 
+/* MiniDIMM related macros */
+#define NETXEN_DIMM_CAPABILITY		(NETXEN_CAM_RAM(0x258))
+#define NETXEN_DIMM_PRESENT			0x1
+#define NETXEN_DIMM_MEMTYPE_DDR2_SDRAM	0x2
+#define NETXEN_DIMM_SIZE			0x4
+#define NETXEN_DIMM_MEMTYPE(VAL)		((VAL >> 3) & 0xf)
+#define	NETXEN_DIMM_NUMROWS(VAL)		((VAL >> 7) & 0xf)
+#define	NETXEN_DIMM_NUMCOLS(VAL)		((VAL >> 11) & 0xf)
+#define	NETXEN_DIMM_NUMRANKS(VAL)		((VAL >> 15) & 0x3)
+#define NETXEN_DIMM_DATAWIDTH(VAL)		((VAL >> 18) & 0x3)
+#define NETXEN_DIMM_NUMBANKS(VAL)		((VAL >> 21) & 0xf)
+#define NETXEN_DIMM_TYPE(VAL)		((VAL >> 25) & 0x3f)
+#define NETXEN_DIMM_VALID_FLAG		0x80000000
+
+#define NETXEN_DIMM_MEM_DDR2_SDRAM	0x8
+
+#define NETXEN_DIMM_STD_MEM_SIZE	512
+
+#define NETXEN_DIMM_TYPE_RDIMM	0x1
+#define NETXEN_DIMM_TYPE_UDIMM	0x2
+#define NETXEN_DIMM_TYPE_SO_DIMM	0x4
+#define NETXEN_DIMM_TYPE_Micro_DIMM	0x8
+#define NETXEN_DIMM_TYPE_Mini_RDIMM	0x10
+#define NETXEN_DIMM_TYPE_Mini_UDIMM	0x20
+
 /* Device State */
 #define NX_DEV_COLD		1
 #define NX_DEV_INITALIZING	2
diff --git a/drivers/net/ethernet/qlogic/netxen/netxen_nic_init.c b/drivers/net/ethernet/qlogic/netxen/netxen_nic_init.c
index 718b2744035..8694124ef77 100644
--- a/drivers/net/ethernet/qlogic/netxen/netxen_nic_init.c
+++ b/drivers/net/ethernet/qlogic/netxen/netxen_nic_init.c
@@ -1131,7 +1131,6 @@ netxen_validate_firmware(struct netxen_adapter *adapter)
 		 _build(file_fw_ver));
 		return -EINVAL;
 	}
-
 	val = nx_get_bios_version(adapter);
 	netxen_rom_fast_read(adapter, NX_BIOS_VERSION_OFFSET, (int *)&bios);
 	if ((__force u32)val != bios) {
@@ -1261,8 +1260,7 @@ next:
 void
 netxen_release_firmware(struct netxen_adapter *adapter)
 {
-	if (adapter->fw)
-		release_firmware(adapter->fw);
+	release_firmware(adapter->fw);
 	adapter->fw = NULL;
 }
 
@@ -1661,6 +1659,9 @@ netxen_process_lro(struct netxen_adapter *adapter,
 
 	length = skb->len;
 
+	if (adapter->flags & NETXEN_FW_MSS_CAP)
+		skb_shinfo(skb)->gso_size  =  netxen_get_lro_sts_mss(sts_data1);
+
 	netif_receive_skb(skb);
 
 	adapter->stats.lro_pkts++;
diff --git a/drivers/net/ethernet/qlogic/netxen/netxen_nic_main.c b/drivers/net/ethernet/qlogic/netxen/netxen_nic_main.c
index 65a718f9ccd..342b3a79bd0 100644
--- a/drivers/net/ethernet/qlogic/netxen/netxen_nic_main.c
+++ b/drivers/net/ethernet/qlogic/netxen/netxen_nic_main.c
@@ -1184,6 +1184,7 @@ netxen_nic_attach(struct netxen_adapter *adapter)
 	int err, ring;
 	struct nx_host_rds_ring *rds_ring;
 	struct nx_host_tx_ring *tx_ring;
+	u32 capab2;
 
 	if (adapter->is_up == NETXEN_ADAPTER_UP_MAGIC)
 		return 0;
@@ -1192,6 +1193,13 @@ netxen_nic_attach(struct netxen_adapter *adapter)
 	if (err)
 		return err;
 
+	adapter->flags &= ~NETXEN_FW_MSS_CAP;
+	if (adapter->capabilities & NX_FW_CAPABILITY_MORE_CAPS) {
+		capab2 = NXRD32(adapter, CRB_FW_CAPABILITIES_2);
+		if (capab2 & NX_FW_CAPABILITY_2_LRO_MAX_TCP_SEG)
+			adapter->flags |= NETXEN_FW_MSS_CAP;
+	}
+
 	err = netxen_napi_add(adapter, netdev);
 	if (err)
 		return err;
@@ -1810,7 +1818,6 @@ netxen_tso_check(struct net_device *netdev,
 		flags = FLAGS_VLAN_TAGGED;
 
 	} else if (vlan_tx_tag_present(skb)) {
-
 		flags = FLAGS_VLAN_OOB;
 		vid = vlan_tx_tag_get(skb);
 		netxen_set_tx_vlan_tci(first_desc, vid);
@@ -2926,6 +2933,134 @@ static struct bin_attribute bin_attr_mem = {
 	.write = netxen_sysfs_write_mem,
 };
 
+static ssize_t
+netxen_sysfs_read_dimm(struct file *filp, struct kobject *kobj,
+		struct bin_attribute *attr,
+		char *buf, loff_t offset, size_t size)
+{
+	struct device *dev = container_of(kobj, struct device, kobj);
+	struct netxen_adapter *adapter = dev_get_drvdata(dev);
+	struct net_device *netdev = adapter->netdev;
+	struct netxen_dimm_cfg dimm;
+	u8 dw, rows, cols, banks, ranks;
+	u32 val;
+
+	if (size != sizeof(struct netxen_dimm_cfg)) {
+		netdev_err(netdev, "Invalid size\n");
+		return -1;
+	}
+
+	memset(&dimm, 0, sizeof(struct netxen_dimm_cfg));
+	val = NXRD32(adapter, NETXEN_DIMM_CAPABILITY);
+
+	/* Checks if DIMM info is valid. */
+	if (val & NETXEN_DIMM_VALID_FLAG) {
+		netdev_err(netdev, "Invalid DIMM flag\n");
+		dimm.presence = 0xff;
+		goto out;
+	}
+
+	rows = NETXEN_DIMM_NUMROWS(val);
+	cols = NETXEN_DIMM_NUMCOLS(val);
+	ranks = NETXEN_DIMM_NUMRANKS(val);
+	banks = NETXEN_DIMM_NUMBANKS(val);
+	dw = NETXEN_DIMM_DATAWIDTH(val);
+
+	dimm.presence = (val & NETXEN_DIMM_PRESENT);
+
+	/* Checks if DIMM info is present. */
+	if (!dimm.presence) {
+		netdev_err(netdev, "DIMM not present\n");
+		goto out;
+	}
+
+	dimm.dimm_type = NETXEN_DIMM_TYPE(val);
+
+	switch (dimm.dimm_type) {
+	case NETXEN_DIMM_TYPE_RDIMM:
+	case NETXEN_DIMM_TYPE_UDIMM:
+	case NETXEN_DIMM_TYPE_SO_DIMM:
+	case NETXEN_DIMM_TYPE_Micro_DIMM:
+	case NETXEN_DIMM_TYPE_Mini_RDIMM:
+	case NETXEN_DIMM_TYPE_Mini_UDIMM:
+		break;
+	default:
+		netdev_err(netdev, "Invalid DIMM type %x\n", dimm.dimm_type);
+		goto out;
+	}
+
+	if (val & NETXEN_DIMM_MEMTYPE_DDR2_SDRAM)
+		dimm.mem_type = NETXEN_DIMM_MEM_DDR2_SDRAM;
+	else
+		dimm.mem_type = NETXEN_DIMM_MEMTYPE(val);
+
+	if (val & NETXEN_DIMM_SIZE) {
+		dimm.size = NETXEN_DIMM_STD_MEM_SIZE;
+		goto out;
+	}
+
+	if (!rows) {
+		netdev_err(netdev, "Invalid no of rows %x\n", rows);
+		goto out;
+	}
+
+	if (!cols) {
+		netdev_err(netdev, "Invalid no of columns %x\n", cols);
+		goto out;
+	}
+
+	if (!banks) {
+		netdev_err(netdev, "Invalid no of banks %x\n", banks);
+		goto out;
+	}
+
+	ranks += 1;
+
+	switch (dw) {
+	case 0x0:
+		dw = 32;
+		break;
+	case 0x1:
+		dw = 33;
+		break;
+	case 0x2:
+		dw = 36;
+		break;
+	case 0x3:
+		dw = 64;
+		break;
+	case 0x4:
+		dw = 72;
+		break;
+	case 0x5:
+		dw = 80;
+		break;
+	case 0x6:
+		dw = 128;
+		break;
+	case 0x7:
+		dw = 144;
+		break;
+	default:
+		netdev_err(netdev, "Invalid data-width %x\n", dw);
+		goto out;
+	}
+
+	dimm.size = ((1 << rows) * (1 << cols) * dw * banks * ranks) / 8;
+	/* Size returned in MB. */
+	dimm.size = (dimm.size) / 0x100000;
+out:
+	memcpy(buf, &dimm, sizeof(struct netxen_dimm_cfg));
+	return sizeof(struct netxen_dimm_cfg);
+
+}
+
+static struct bin_attribute bin_attr_dimm = {
+	.attr = { .name = "dimm", .mode = (S_IRUGO | S_IWUSR) },
+	.size = 0,
+	.read = netxen_sysfs_read_dimm,
+};
+
 
 static void
 netxen_create_sysfs_entries(struct netxen_adapter *adapter)
@@ -2963,6 +3098,8 @@ netxen_create_diag_entries(struct netxen_adapter *adapter)
 		dev_info(dev, "failed to create crb sysfs entry\n");
 	if (device_create_bin_file(dev, &bin_attr_mem))
 		dev_info(dev, "failed to create mem sysfs entry\n");
+	if (device_create_bin_file(dev, &bin_attr_dimm))
+		dev_info(dev, "failed to create dimm sysfs entry\n");
 }
 
 
@@ -2975,6 +3112,7 @@ netxen_remove_diag_entries(struct netxen_adapter *adapter)
 	device_remove_file(dev, &dev_attr_diag_mode);
 	device_remove_bin_file(dev, &bin_attr_crb);
 	device_remove_bin_file(dev, &bin_attr_mem);
+	device_remove_bin_file(dev, &bin_attr_dimm);
 }
 
 #ifdef CONFIG_INET
diff --git a/drivers/net/ethernet/qlogic/qlcnic/qlcnic.h b/drivers/net/ethernet/qlogic/qlcnic/qlcnic.h
index 385a4d5c7c2..8680a5dae4a 100644
--- a/drivers/net/ethernet/qlogic/qlcnic/qlcnic.h
+++ b/drivers/net/ethernet/qlogic/qlcnic/qlcnic.h
@@ -36,8 +36,8 @@
 
 #define _QLCNIC_LINUX_MAJOR 5
 #define _QLCNIC_LINUX_MINOR 0
-#define _QLCNIC_LINUX_SUBVERSION 27
-#define QLCNIC_LINUX_VERSIONID  "5.0.27"
+#define _QLCNIC_LINUX_SUBVERSION 28
+#define QLCNIC_LINUX_VERSIONID  "5.0.28"
 #define QLCNIC_DRV_IDC_VER  0x01
 #define QLCNIC_DRIVER_VERSION  ((_QLCNIC_LINUX_MAJOR << 16) |\
 		 (_QLCNIC_LINUX_MINOR << 8) | (_QLCNIC_LINUX_SUBVERSION))
@@ -607,6 +607,7 @@ struct qlcnic_recv_context {
 #define QLCNIC_CDRP_CMD_CONFIG_PORT		0x0000002E
 #define QLCNIC_CDRP_CMD_TEMP_SIZE		0x0000002f
 #define QLCNIC_CDRP_CMD_GET_TEMP_HDR		0x00000030
+#define QLCNIC_CDRP_CMD_GET_MAC_STATS		0x00000037
 
 #define QLCNIC_RCODE_SUCCESS		0
 #define QLCNIC_RCODE_NOT_SUPPORTED	9
@@ -1180,18 +1181,62 @@ struct qlcnic_esw_func_cfg {
 #define QLCNIC_STATS_ESWITCH		2
 #define QLCNIC_QUERY_RX_COUNTER		0
 #define QLCNIC_QUERY_TX_COUNTER		1
-#define QLCNIC_ESW_STATS_NOT_AVAIL	0xffffffffffffffffULL
+#define QLCNIC_STATS_NOT_AVAIL	0xffffffffffffffffULL
+#define QLCNIC_FILL_STATS(VAL1) \
+	(((VAL1) == QLCNIC_STATS_NOT_AVAIL) ? 0 : VAL1)
+#define QLCNIC_MAC_STATS 1
+#define QLCNIC_ESW_STATS 2
 
 #define QLCNIC_ADD_ESW_STATS(VAL1, VAL2)\
 do {	\
-	if (((VAL1) == QLCNIC_ESW_STATS_NOT_AVAIL) && \
-	    ((VAL2) != QLCNIC_ESW_STATS_NOT_AVAIL)) \
+	if (((VAL1) == QLCNIC_STATS_NOT_AVAIL) && \
+	    ((VAL2) != QLCNIC_STATS_NOT_AVAIL)) \
 		(VAL1) = (VAL2); \
-	else if (((VAL1) != QLCNIC_ESW_STATS_NOT_AVAIL) && \
-		 ((VAL2) != QLCNIC_ESW_STATS_NOT_AVAIL)) \
+	else if (((VAL1) != QLCNIC_STATS_NOT_AVAIL) && \
+		 ((VAL2) != QLCNIC_STATS_NOT_AVAIL)) \
 			(VAL1) += (VAL2); \
 } while (0)
 
+struct qlcnic_mac_statistics{
+	__le64	mac_tx_frames;
+	__le64	mac_tx_bytes;
+	__le64	mac_tx_mcast_pkts;
+	__le64	mac_tx_bcast_pkts;
+	__le64	mac_tx_pause_cnt;
+	__le64	mac_tx_ctrl_pkt;
+	__le64	mac_tx_lt_64b_pkts;
+	__le64	mac_tx_lt_127b_pkts;
+	__le64	mac_tx_lt_255b_pkts;
+	__le64	mac_tx_lt_511b_pkts;
+	__le64	mac_tx_lt_1023b_pkts;
+	__le64	mac_tx_lt_1518b_pkts;
+	__le64	mac_tx_gt_1518b_pkts;
+	__le64	rsvd1[3];
+
+	__le64	mac_rx_frames;
+	__le64	mac_rx_bytes;
+	__le64	mac_rx_mcast_pkts;
+	__le64	mac_rx_bcast_pkts;
+	__le64	mac_rx_pause_cnt;
+	__le64	mac_rx_ctrl_pkt;
+	__le64	mac_rx_lt_64b_pkts;
+	__le64	mac_rx_lt_127b_pkts;
+	__le64	mac_rx_lt_255b_pkts;
+	__le64	mac_rx_lt_511b_pkts;
+	__le64	mac_rx_lt_1023b_pkts;
+	__le64	mac_rx_lt_1518b_pkts;
+	__le64	mac_rx_gt_1518b_pkts;
+	__le64	rsvd2[3];
+
+	__le64	mac_rx_length_error;
+	__le64	mac_rx_length_small;
+	__le64	mac_rx_length_large;
+	__le64	mac_rx_jabber;
+	__le64	mac_rx_dropped;
+	__le64	mac_rx_crc_error;
+	__le64	mac_align_error;
+} __packed;
+
 struct __qlcnic_esw_statistics {
 	__le16 context_id;
 	__le16 version;
@@ -1352,6 +1397,8 @@ enum op_codes {
 #define QLCNIC_ENABLE_FW_DUMP		0xaddfeed
 #define QLCNIC_DISABLE_FW_DUMP		0xbadfeed
 #define QLCNIC_FORCE_FW_RESET		0xdeaddead
+#define QLCNIC_SET_QUIESCENT		0xadd00010
+#define QLCNIC_RESET_QUIESCENT		0xadd00020
 
 struct qlcnic_dump_operations {
 	enum op_codes opcode;
@@ -1510,6 +1557,7 @@ int qlcnic_get_port_stats(struct qlcnic_adapter *, const u8, const u8,
 int qlcnic_get_eswitch_stats(struct qlcnic_adapter *, const u8, u8,
 					struct __qlcnic_esw_statistics *);
 int qlcnic_clear_esw_stats(struct qlcnic_adapter *adapter, u8, u8, u8);
+int qlcnic_get_mac_stats(struct qlcnic_adapter *, struct qlcnic_mac_statistics *);
 extern int qlcnic_config_tso;
 
 /*
@@ -1559,6 +1607,7 @@ static inline u32 qlcnic_tx_avail(struct qlcnic_host_tx_ring *tx_ring)
 }
 
 extern const struct ethtool_ops qlcnic_ethtool_ops;
+extern const struct ethtool_ops qlcnic_ethtool_failed_ops;
 
 struct qlcnic_nic_template {
 	int (*config_bridged_mode) (struct qlcnic_adapter *, u32);
diff --git a/drivers/net/ethernet/qlogic/qlcnic/qlcnic_ctx.c b/drivers/net/ethernet/qlogic/qlcnic/qlcnic_ctx.c
index 569a837d2ac..8db85244e8a 100644
--- a/drivers/net/ethernet/qlogic/qlcnic/qlcnic_ctx.c
+++ b/drivers/net/ethernet/qlogic/qlcnic/qlcnic_ctx.c
@@ -905,6 +905,65 @@ int qlcnic_get_port_stats(struct qlcnic_adapter *adapter, const u8 func,
 	return err;
 }
 
+/* This routine will retrieve the MAC statistics from firmware */
+int qlcnic_get_mac_stats(struct qlcnic_adapter *adapter,
+		struct qlcnic_mac_statistics *mac_stats)
+{
+	struct qlcnic_mac_statistics *stats;
+	struct qlcnic_cmd_args cmd;
+	size_t stats_size = sizeof(struct qlcnic_mac_statistics);
+	dma_addr_t stats_dma_t;
+	void *stats_addr;
+	int err;
+
+	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__);
+		return -ENOMEM;
+	}
+	memset(stats_addr, 0, stats_size);
+	memset(&cmd, 0, sizeof(cmd));
+	cmd.req.cmd = QLCNIC_CDRP_CMD_GET_MAC_STATS;
+	cmd.req.arg1 = stats_size << 16;
+	cmd.req.arg2 = MSD(stats_dma_t);
+	cmd.req.arg3 = LSD(stats_dma_t);
+
+	qlcnic_issue_cmd(adapter, &cmd);
+	err = cmd.rsp.cmd;
+
+	if (!err) {
+		stats = stats_addr;
+		mac_stats->mac_tx_frames = le64_to_cpu(stats->mac_tx_frames);
+		mac_stats->mac_tx_bytes = le64_to_cpu(stats->mac_tx_bytes);
+		mac_stats->mac_tx_mcast_pkts =
+					le64_to_cpu(stats->mac_tx_mcast_pkts);
+		mac_stats->mac_tx_bcast_pkts =
+					le64_to_cpu(stats->mac_tx_bcast_pkts);
+		mac_stats->mac_rx_frames = le64_to_cpu(stats->mac_rx_frames);
+		mac_stats->mac_rx_bytes = le64_to_cpu(stats->mac_rx_bytes);
+		mac_stats->mac_rx_mcast_pkts =
+					le64_to_cpu(stats->mac_rx_mcast_pkts);
+		mac_stats->mac_rx_length_error =
+				le64_to_cpu(stats->mac_rx_length_error);
+		mac_stats->mac_rx_length_small =
+				le64_to_cpu(stats->mac_rx_length_small);
+		mac_stats->mac_rx_length_large =
+				le64_to_cpu(stats->mac_rx_length_large);
+		mac_stats->mac_rx_jabber = le64_to_cpu(stats->mac_rx_jabber);
+		mac_stats->mac_rx_dropped = le64_to_cpu(stats->mac_rx_dropped);
+		mac_stats->mac_rx_crc_error = le64_to_cpu(stats->mac_rx_crc_error);
+	} else {
+		dev_info(&adapter->pdev->dev,
+			"%s: Get mac stats failed =%d.\n", __func__, err);
+	}
+
+	dma_free_coherent(&adapter->pdev->dev, stats_size, stats_addr,
+		stats_dma_t);
+	return err;
+}
+
 int qlcnic_get_eswitch_stats(struct qlcnic_adapter *adapter, const u8 eswitch,
 		const u8 rx_tx, struct __qlcnic_esw_statistics *esw_stats) {
 
@@ -920,13 +979,13 @@ int qlcnic_get_eswitch_stats(struct qlcnic_adapter *adapter, const u8 eswitch,
 		return -EIO;
 
 	memset(esw_stats, 0, sizeof(u64));
-	esw_stats->unicast_frames = QLCNIC_ESW_STATS_NOT_AVAIL;
-	esw_stats->multicast_frames = QLCNIC_ESW_STATS_NOT_AVAIL;
-	esw_stats->broadcast_frames = QLCNIC_ESW_STATS_NOT_AVAIL;
-	esw_stats->dropped_frames = QLCNIC_ESW_STATS_NOT_AVAIL;
-	esw_stats->errors = QLCNIC_ESW_STATS_NOT_AVAIL;
-	esw_stats->local_frames = QLCNIC_ESW_STATS_NOT_AVAIL;
-	esw_stats->numbytes = QLCNIC_ESW_STATS_NOT_AVAIL;
+	esw_stats->unicast_frames = QLCNIC_STATS_NOT_AVAIL;
+	esw_stats->multicast_frames = QLCNIC_STATS_NOT_AVAIL;
+	esw_stats->broadcast_frames = QLCNIC_STATS_NOT_AVAIL;
+	esw_stats->dropped_frames = QLCNIC_STATS_NOT_AVAIL;
+	esw_stats->errors = QLCNIC_STATS_NOT_AVAIL;
+	esw_stats->local_frames = QLCNIC_STATS_NOT_AVAIL;
+	esw_stats->numbytes = QLCNIC_STATS_NOT_AVAIL;
 	esw_stats->context_id = eswitch;
 
 	for (i = 0; i < QLCNIC_MAX_PCI_FUNC; i++) {
diff --git a/drivers/net/ethernet/qlogic/qlcnic/qlcnic_ethtool.c b/drivers/net/ethernet/qlogic/qlcnic/qlcnic_ethtool.c
index 89ddf7f7d7d..9e9e78a5c4d 100644
--- a/drivers/net/ethernet/qlogic/qlcnic/qlcnic_ethtool.c
+++ b/drivers/net/ethernet/qlogic/qlcnic/qlcnic_ethtool.c
@@ -78,8 +78,46 @@ static const char qlcnic_device_gstrings_stats[][ETH_GSTRING_LEN] = {
 	"tx numbytes",
 };
 
-#define QLCNIC_STATS_LEN	ARRAY_SIZE(qlcnic_gstrings_stats)
+static const char qlcnic_mac_stats_strings [][ETH_GSTRING_LEN] = {
+	"mac_tx_frames",
+	"mac_tx_bytes",
+	"mac_tx_mcast_pkts",
+	"mac_tx_bcast_pkts",
+	"mac_tx_pause_cnt",
+	"mac_tx_ctrl_pkt",
+	"mac_tx_lt_64b_pkts",
+	"mac_tx_lt_127b_pkts",
+	"mac_tx_lt_255b_pkts",
+	"mac_tx_lt_511b_pkts",
+	"mac_tx_lt_1023b_pkts",
+	"mac_tx_lt_1518b_pkts",
+	"mac_tx_gt_1518b_pkts",
+	"mac_rx_frames",
+	"mac_rx_bytes",
+	"mac_rx_mcast_pkts",
+	"mac_rx_bcast_pkts",
+	"mac_rx_pause_cnt",
+	"mac_rx_ctrl_pkt",
+	"mac_rx_lt_64b_pkts",
+	"mac_rx_lt_127b_pkts",
+	"mac_rx_lt_255b_pkts",
+	"mac_rx_lt_511b_pkts",
+	"mac_rx_lt_1023b_pkts",
+	"mac_rx_lt_1518b_pkts",
+	"mac_rx_gt_1518b_pkts",
+	"mac_rx_length_error",
+	"mac_rx_length_small",
+	"mac_rx_length_large",
+	"mac_rx_jabber",
+	"mac_rx_dropped",
+	"mac_rx_crc_error",
+	"mac_align_error",
+};
+
+#define QLCNIC_STATS_LEN ARRAY_SIZE(qlcnic_gstrings_stats)
+#define QLCNIC_MAC_STATS_LEN ARRAY_SIZE(qlcnic_mac_stats_strings)
 #define QLCNIC_DEVICE_STATS_LEN	ARRAY_SIZE(qlcnic_device_gstrings_stats)
+#define QLCNIC_TOTAL_STATS_LEN QLCNIC_STATS_LEN + QLCNIC_MAC_STATS_LEN
 
 static const char qlcnic_gstrings_test[][ETH_GSTRING_LEN] = {
 	"Register_Test_on_offline",
@@ -644,8 +682,8 @@ static int qlcnic_get_sset_count(struct net_device *dev, int sset)
 		return QLCNIC_TEST_LEN;
 	case ETH_SS_STATS:
 		if (adapter->flags & QLCNIC_ESWITCH_ENABLED)
-			return QLCNIC_STATS_LEN + QLCNIC_DEVICE_STATS_LEN;
-		return QLCNIC_STATS_LEN;
+			return QLCNIC_TOTAL_STATS_LEN + QLCNIC_DEVICE_STATS_LEN;
+		return QLCNIC_TOTAL_STATS_LEN;
 	default:
 		return -EOPNOTSUPP;
 	}
@@ -851,7 +889,7 @@ static void
 qlcnic_get_strings(struct net_device *dev, u32 stringset, u8 * data)
 {
 	struct qlcnic_adapter *adapter = netdev_priv(dev);
-	int index, i;
+	int index, i, j;
 
 	switch (stringset) {
 	case ETH_SS_TEST:
@@ -864,6 +902,11 @@ qlcnic_get_strings(struct net_device *dev, u32 stringset, u8 * data)
 			       qlcnic_gstrings_stats[index].stat_string,
 			       ETH_GSTRING_LEN);
 		}
+		for (j = 0; j < QLCNIC_MAC_STATS_LEN; index++, j++) {
+			memcpy(data + index * ETH_GSTRING_LEN,
+			       qlcnic_mac_stats_strings[j],
+			       ETH_GSTRING_LEN);
+		}
 		if (!(adapter->flags & QLCNIC_ESWITCH_ENABLED))
 			return;
 		for (i = 0; i < QLCNIC_DEVICE_STATS_LEN; index++, i++) {
@@ -874,22 +917,64 @@ qlcnic_get_strings(struct net_device *dev, u32 stringset, u8 * data)
 	}
 }
 
-#define QLCNIC_FILL_ESWITCH_STATS(VAL1) \
-	(((VAL1) == QLCNIC_ESW_STATS_NOT_AVAIL) ? 0 : VAL1)
-
 static void
-qlcnic_fill_device_stats(int *index, u64 *data,
-		struct __qlcnic_esw_statistics *stats)
+qlcnic_fill_stats(int *index, u64 *data, void *stats, int type)
 {
 	int ind = *index;
 
-	data[ind++] = QLCNIC_FILL_ESWITCH_STATS(stats->unicast_frames);
-	data[ind++] = QLCNIC_FILL_ESWITCH_STATS(stats->multicast_frames);
-	data[ind++] = QLCNIC_FILL_ESWITCH_STATS(stats->broadcast_frames);
-	data[ind++] = QLCNIC_FILL_ESWITCH_STATS(stats->dropped_frames);
-	data[ind++] = QLCNIC_FILL_ESWITCH_STATS(stats->errors);
-	data[ind++] = QLCNIC_FILL_ESWITCH_STATS(stats->local_frames);
-	data[ind++] = QLCNIC_FILL_ESWITCH_STATS(stats->numbytes);
+	if (type == QLCNIC_MAC_STATS) {
+		struct qlcnic_mac_statistics *mac_stats =
+					(struct qlcnic_mac_statistics *)stats;
+		data[ind++] = QLCNIC_FILL_STATS(mac_stats->mac_tx_frames);
+		data[ind++] = QLCNIC_FILL_STATS(mac_stats->mac_tx_bytes);
+		data[ind++] = QLCNIC_FILL_STATS(mac_stats->mac_tx_mcast_pkts);
+		data[ind++] = QLCNIC_FILL_STATS(mac_stats->mac_tx_bcast_pkts);
+		data[ind++] = QLCNIC_FILL_STATS(mac_stats->mac_tx_pause_cnt);
+		data[ind++] = QLCNIC_FILL_STATS(mac_stats->mac_tx_ctrl_pkt);
+		data[ind++] = QLCNIC_FILL_STATS(mac_stats->mac_tx_lt_64b_pkts);
+		data[ind++] = QLCNIC_FILL_STATS(mac_stats->mac_tx_lt_127b_pkts);
+		data[ind++] = QLCNIC_FILL_STATS(mac_stats->mac_tx_lt_255b_pkts);
+		data[ind++] = QLCNIC_FILL_STATS(mac_stats->mac_tx_lt_511b_pkts);
+		data[ind++] =
+			QLCNIC_FILL_STATS(mac_stats->mac_tx_lt_1023b_pkts);
+		data[ind++] =
+			QLCNIC_FILL_STATS(mac_stats->mac_tx_lt_1518b_pkts);
+		data[ind++] =
+			QLCNIC_FILL_STATS(mac_stats->mac_tx_gt_1518b_pkts);
+		data[ind++] = QLCNIC_FILL_STATS(mac_stats->mac_rx_frames);
+		data[ind++] = QLCNIC_FILL_STATS(mac_stats->mac_rx_bytes);
+		data[ind++] = QLCNIC_FILL_STATS(mac_stats->mac_rx_mcast_pkts);
+		data[ind++] = QLCNIC_FILL_STATS(mac_stats->mac_rx_bcast_pkts);
+		data[ind++] = QLCNIC_FILL_STATS(mac_stats->mac_rx_pause_cnt);
+		data[ind++] = QLCNIC_FILL_STATS(mac_stats->mac_rx_ctrl_pkt);
+		data[ind++] = QLCNIC_FILL_STATS(mac_stats->mac_rx_lt_64b_pkts);
+		data[ind++] = QLCNIC_FILL_STATS(mac_stats->mac_rx_lt_127b_pkts);
+		data[ind++] = QLCNIC_FILL_STATS(mac_stats->mac_rx_lt_255b_pkts);
+		data[ind++] = QLCNIC_FILL_STATS(mac_stats->mac_rx_lt_511b_pkts);
+		data[ind++] =
+			QLCNIC_FILL_STATS(mac_stats->mac_rx_lt_1023b_pkts);
+		data[ind++] =
+			QLCNIC_FILL_STATS(mac_stats->mac_rx_lt_1518b_pkts);
+		data[ind++] =
+			QLCNIC_FILL_STATS(mac_stats->mac_rx_gt_1518b_pkts);
+		data[ind++] = QLCNIC_FILL_STATS(mac_stats->mac_rx_length_error);
+		data[ind++] = QLCNIC_FILL_STATS(mac_stats->mac_rx_length_small);
+		data[ind++] = QLCNIC_FILL_STATS(mac_stats->mac_rx_length_large);
+		data[ind++] = QLCNIC_FILL_STATS(mac_stats->mac_rx_jabber);
+		data[ind++] = QLCNIC_FILL_STATS(mac_stats->mac_rx_dropped);
+		data[ind++] = QLCNIC_FILL_STATS(mac_stats->mac_rx_crc_error);
+		data[ind++] = QLCNIC_FILL_STATS(mac_stats->mac_align_error);
+	} else if (type == QLCNIC_ESW_STATS) {
+		struct __qlcnic_esw_statistics *esw_stats =
+				(struct __qlcnic_esw_statistics *)stats;
+		data[ind++] = QLCNIC_FILL_STATS(esw_stats->unicast_frames);
+		data[ind++] = QLCNIC_FILL_STATS(esw_stats->multicast_frames);
+		data[ind++] = QLCNIC_FILL_STATS(esw_stats->broadcast_frames);
+		data[ind++] = QLCNIC_FILL_STATS(esw_stats->dropped_frames);
+		data[ind++] = QLCNIC_FILL_STATS(esw_stats->errors);
+		data[ind++] = QLCNIC_FILL_STATS(esw_stats->local_frames);
+		data[ind++] = QLCNIC_FILL_STATS(esw_stats->numbytes);
+	}
 
 	*index = ind;
 }
@@ -900,6 +985,7 @@ qlcnic_get_ethtool_stats(struct net_device *dev,
 {
 	struct qlcnic_adapter *adapter = netdev_priv(dev);
 	struct qlcnic_esw_statistics port_stats;
+	struct qlcnic_mac_statistics mac_stats;
 	int index, ret;
 
 	for (index = 0; index < QLCNIC_STATS_LEN; index++) {
@@ -911,6 +997,11 @@ qlcnic_get_ethtool_stats(struct net_device *dev,
 		     sizeof(u64)) ? *(u64 *)p:(*(u32 *)p);
 	}
 
+	/* Retrieve MAC statistics from firmware */
+	memset(&mac_stats, 0, sizeof(struct qlcnic_mac_statistics));
+	qlcnic_get_mac_stats(adapter, &mac_stats);
+	qlcnic_fill_stats(&index, data, &mac_stats, QLCNIC_MAC_STATS);
+
 	if (!(adapter->flags & QLCNIC_ESWITCH_ENABLED))
 		return;
 
@@ -920,14 +1011,14 @@ qlcnic_get_ethtool_stats(struct net_device *dev,
 	if (ret)
 		return;
 
-	qlcnic_fill_device_stats(&index, data, &port_stats.rx);
+	qlcnic_fill_stats(&index, 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)
 		return;
 
-	qlcnic_fill_device_stats(&index, data, &port_stats.tx);
+	qlcnic_fill_stats(&index, data, &port_stats.tx, QLCNIC_ESW_STATS);
 }
 
 static int qlcnic_set_led(struct net_device *dev,
@@ -1132,11 +1223,21 @@ qlcnic_get_dump_flag(struct net_device *netdev, struct ethtool_dump *dump)
 	struct qlcnic_adapter *adapter = netdev_priv(netdev);
 	struct qlcnic_fw_dump *fw_dump = &adapter->ahw->fw_dump;
 
+	if (!fw_dump->tmpl_hdr) {
+		netdev_err(adapter->netdev, "FW Dump not supported\n");
+		return -ENOTSUPP;
+	}
+
 	if (fw_dump->clr)
 		dump->len = fw_dump->tmpl_hdr->size + fw_dump->size;
 	else
 		dump->len = 0;
-	dump->flag = fw_dump->tmpl_hdr->drv_cap_mask;
+
+	if (!fw_dump->enable)
+		dump->flag = ETH_FW_DUMP_DISABLE;
+	else
+		dump->flag = fw_dump->tmpl_hdr->drv_cap_mask;
+
 	dump->version = adapter->fw_version;
 	return 0;
 }
@@ -1150,6 +1251,11 @@ qlcnic_get_dump_data(struct net_device *netdev, struct ethtool_dump *dump,
 	struct qlcnic_adapter *adapter = netdev_priv(netdev);
 	struct qlcnic_fw_dump *fw_dump = &adapter->ahw->fw_dump;
 
+	if (!fw_dump->tmpl_hdr) {
+		netdev_err(netdev, "FW Dump not supported\n");
+		return -ENOTSUPP;
+	}
+
 	if (!fw_dump->clr) {
 		netdev_info(netdev, "Dump not available\n");
 		return -EINVAL;
@@ -1177,55 +1283,74 @@ qlcnic_get_dump_data(struct net_device *netdev, struct ethtool_dump *dump,
 static int
 qlcnic_set_dump(struct net_device *netdev, struct ethtool_dump *val)
 {
-	int ret = 0;
+	int i;
 	struct qlcnic_adapter *adapter = netdev_priv(netdev);
 	struct qlcnic_fw_dump *fw_dump = &adapter->ahw->fw_dump;
+	u32 state;
 
 	switch (val->flag) {
 	case QLCNIC_FORCE_FW_DUMP_KEY:
+		if (!fw_dump->tmpl_hdr) {
+			netdev_err(netdev, "FW dump not supported\n");
+			return -ENOTSUPP;
+		}
 		if (!fw_dump->enable) {
 			netdev_info(netdev, "FW dump not enabled\n");
-			return ret;
+			return 0;
 		}
 		if (fw_dump->clr) {
 			netdev_info(netdev,
 			"Previous dump not cleared, not forcing dump\n");
-			return ret;
+			return 0;
 		}
 		netdev_info(netdev, "Forcing a FW dump\n");
 		qlcnic_dev_request_reset(adapter);
 		break;
 	case QLCNIC_DISABLE_FW_DUMP:
-		if (fw_dump->enable) {
+		if (fw_dump->enable && fw_dump->tmpl_hdr) {
 			netdev_info(netdev, "Disabling FW dump\n");
 			fw_dump->enable = 0;
 		}
-		break;
+		return 0;
 	case QLCNIC_ENABLE_FW_DUMP:
-		if (!fw_dump->enable && fw_dump->tmpl_hdr) {
+		if (!fw_dump->tmpl_hdr) {
+			netdev_err(netdev, "FW dump not supported\n");
+			return -ENOTSUPP;
+		}
+		if (!fw_dump->enable) {
 			netdev_info(netdev, "Enabling FW dump\n");
 			fw_dump->enable = 1;
 		}
-		break;
+		return 0;
 	case QLCNIC_FORCE_FW_RESET:
 		netdev_info(netdev, "Forcing a FW reset\n");
 		qlcnic_dev_request_reset(adapter);
 		adapter->flags &= ~QLCNIC_FW_RESET_OWNER;
-		break;
+		return 0;
+	case QLCNIC_SET_QUIESCENT:
+	case QLCNIC_RESET_QUIESCENT:
+		state = QLCRD32(adapter, QLCNIC_CRB_DEV_STATE);
+		if (state == QLCNIC_DEV_FAILED || (state == QLCNIC_DEV_BADBAD))
+			netdev_info(netdev, "Device in FAILED state\n");
+		return 0;
 	default:
-		if (val->flag > QLCNIC_DUMP_MASK_MAX ||
-			val->flag < QLCNIC_DUMP_MASK_MIN) {
-				netdev_info(netdev,
-				"Invalid dump level: 0x%x\n", val->flag);
-				ret = -EINVAL;
-				goto out;
+		if (!fw_dump->tmpl_hdr) {
+			netdev_err(netdev, "FW dump not supported\n");
+			return -ENOTSUPP;
+		}
+		for (i = 0; i < ARRAY_SIZE(FW_DUMP_LEVELS); i++) {
+			if (val->flag == FW_DUMP_LEVELS[i]) {
+				fw_dump->tmpl_hdr->drv_cap_mask =
+							val->flag;
+				netdev_info(netdev, "Driver mask changed to: 0x%x\n",
+					fw_dump->tmpl_hdr->drv_cap_mask);
+				return 0;
+			}
 		}
-		fw_dump->tmpl_hdr->drv_cap_mask = val->flag & 0xff;
-		netdev_info(netdev, "Driver mask changed to: 0x%x\n",
-			fw_dump->tmpl_hdr->drv_cap_mask);
+		netdev_info(netdev, "Invalid dump level: 0x%x\n", val->flag);
+		return -EINVAL;
 	}
-out:
-	return ret;
+	return 0;
 }
 
 const struct ethtool_ops qlcnic_ethtool_ops = {
@@ -1258,3 +1383,10 @@ const struct ethtool_ops qlcnic_ethtool_ops = {
 	.get_dump_data = qlcnic_get_dump_data,
 	.set_dump = qlcnic_set_dump,
 };
+
+const struct ethtool_ops qlcnic_ethtool_failed_ops = {
+	.get_settings = qlcnic_get_settings,
+	.get_drvinfo = qlcnic_get_drvinfo,
+	.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 a52819303d1..6ced3195aad 100644
--- a/drivers/net/ethernet/qlogic/qlcnic/qlcnic_hdr.h
+++ b/drivers/net/ethernet/qlogic/qlcnic/qlcnic_hdr.h
@@ -704,6 +704,8 @@ enum {
 #define QLCNIC_DEV_FAILED		0x6
 #define QLCNIC_DEV_QUISCENT		0x7
 
+#define QLCNIC_DEV_BADBAD		0xbad0bad0
+
 #define QLCNIC_DEV_NPAR_NON_OPER	0 /* NON Operational */
 #define QLCNIC_DEV_NPAR_OPER		1 /* NPAR Operational */
 #define QLCNIC_DEV_NPAR_OPER_TIMEO	30 /* Operational time out */
@@ -776,6 +778,10 @@ struct qlcnic_legacy_intr_set {
 #define FLASH_ROM_WINDOW	0x42110030
 #define FLASH_ROM_DATA		0x42150000
 
+
+static const u32 FW_DUMP_LEVELS[] = {
+	0x3, 0x7, 0xf, 0x1f, 0x3f, 0x7f, 0xff };
+
 static const u32 MIU_TEST_READ_DATA[] = {
 	0x410000A8, 0x410000AC, 0x410000B8, 0x410000BC, };
 
diff --git a/drivers/net/ethernet/qlogic/qlcnic/qlcnic_init.c b/drivers/net/ethernet/qlogic/qlcnic/qlcnic_init.c
index d32cf0ddf1b..799fd40ed03 100644
--- a/drivers/net/ethernet/qlogic/qlcnic/qlcnic_init.c
+++ b/drivers/net/ethernet/qlogic/qlcnic/qlcnic_init.c
@@ -1321,8 +1321,7 @@ next:
 void
 qlcnic_release_firmware(struct qlcnic_adapter *adapter)
 {
-	if (adapter->fw)
-		release_firmware(adapter->fw);
+	release_firmware(adapter->fw);
 	adapter->fw = NULL;
 }
 
diff --git a/drivers/net/ethernet/qlogic/qlcnic/qlcnic_main.c b/drivers/net/ethernet/qlogic/qlcnic/qlcnic_main.c
index 75c32e875fe..46e77a2c512 100644
--- a/drivers/net/ethernet/qlogic/qlcnic/qlcnic_main.c
+++ b/drivers/net/ethernet/qlogic/qlcnic/qlcnic_main.c
@@ -338,6 +338,10 @@ static const struct net_device_ops qlcnic_netdev_ops = {
 #endif
 };
 
+static const struct net_device_ops qlcnic_netdev_failed_ops = {
+	.ndo_open	   = qlcnic_open,
+};
+
 static struct qlcnic_nic_template qlcnic_ops = {
 	.config_bridged_mode = qlcnic_config_bridged_mode,
 	.config_led = qlcnic_config_led,
@@ -1623,8 +1627,9 @@ qlcnic_probe(struct pci_dev *pdev, const struct pci_device_id *ent)
 
 	err = adapter->nic_ops->start_firmware(adapter);
 	if (err) {
-		dev_err(&pdev->dev, "Loading fw failed.Please Reboot\n");
-		goto err_out_decr_ref;
+		dev_err(&pdev->dev, "Loading fw failed. Please Reboot\n"
+			"\t\tIf reboot doesn't help, try flashing the card\n");
+		goto err_out_maintenance_mode;
 	}
 
 	if (qlcnic_read_mac_addr(adapter))
@@ -1695,6 +1700,18 @@ err_out_disable_pdev:
 	pci_set_drvdata(pdev, NULL);
 	pci_disable_device(pdev);
 	return err;
+
+err_out_maintenance_mode:
+	netdev->netdev_ops = &qlcnic_netdev_failed_ops;
+	SET_ETHTOOL_OPS(netdev, &qlcnic_ethtool_failed_ops);
+	err = register_netdev(netdev);
+	if (err) {
+		dev_err(&pdev->dev, "failed to register net device\n");
+		goto err_out_decr_ref;
+	}
+	pci_set_drvdata(pdev, adapter);
+	qlcnic_create_diag_entries(adapter);
+	return 0;
 }
 
 static void __devexit qlcnic_remove(struct pci_dev *pdev)
@@ -1831,8 +1848,14 @@ done:
 static int qlcnic_open(struct net_device *netdev)
 {
 	struct qlcnic_adapter *adapter = netdev_priv(netdev);
+	u32 state = QLCRD32(adapter, QLCNIC_CRB_DEV_STATE);
 	int err;
 
+	if (state == QLCNIC_DEV_FAILED || (state == QLCNIC_DEV_BADBAD)) {
+		netdev_err(netdev, "Device in FAILED state\n");
+		return -EIO;
+	}
+
 	netif_carrier_off(netdev);
 
 	err = qlcnic_attach(adapter);
@@ -1942,7 +1965,7 @@ qlcnic_send_filter(struct qlcnic_adapter *adapter,
 	__le16 vlan_id = 0;
 	u8 hindex;
 
-	if (!compare_ether_addr(phdr->h_source, adapter->mac_addr))
+	if (ether_addr_equal(phdr->h_source, adapter->mac_addr))
 		return;
 
 	if (adapter->fhash.fnum >= adapter->fhash.fmax)
@@ -2212,8 +2235,7 @@ qlcnic_xmit_frame(struct sk_buff *skb, struct net_device *netdev)
 
 	if (adapter->flags & QLCNIC_MACSPOOF) {
 		phdr = (struct ethhdr *)skb->data;
-		if (compare_ether_addr(phdr->h_source,
-					adapter->mac_addr))
+		if (!ether_addr_equal(phdr->h_source, adapter->mac_addr))
 			goto drop_packet;
 	}
 
@@ -3018,6 +3040,12 @@ qlcnic_dev_request_reset(struct qlcnic_adapter *adapter)
 		return;
 
 	state = QLCRD32(adapter, QLCNIC_CRB_DEV_STATE);
+	if (state  == QLCNIC_DEV_FAILED || (state == QLCNIC_DEV_BADBAD)) {
+		netdev_err(adapter->netdev,
+				"Device is in FAILED state, Please Reboot\n");
+		qlcnic_api_unlock(adapter);
+		return;
+	}
 
 	if (state == QLCNIC_DEV_READY) {
 		QLCWR32(adapter, QLCNIC_CRB_DEV_STATE, QLCNIC_DEV_NEED_RESET);
@@ -3061,6 +3089,9 @@ qlcnic_cancel_fw_work(struct qlcnic_adapter *adapter)
 	while (test_and_set_bit(__QLCNIC_RESETTING, &adapter->state))
 		msleep(10);
 
+	if (!adapter->fw_work.work.func)
+		return;
+
 	cancel_delayed_work_sync(&adapter->fw_work);
 }
 
@@ -4280,6 +4311,7 @@ static void
 qlcnic_create_diag_entries(struct qlcnic_adapter *adapter)
 {
 	struct device *dev = &adapter->pdev->dev;
+	u32 state = QLCRD32(adapter, QLCNIC_CRB_DEV_STATE);
 
 	if (device_create_bin_file(dev, &bin_attr_port_stats))
 		dev_info(dev, "failed to create port stats sysfs entry");
@@ -4288,14 +4320,19 @@ qlcnic_create_diag_entries(struct qlcnic_adapter *adapter)
 		return;
 	if (device_create_file(dev, &dev_attr_diag_mode))
 		dev_info(dev, "failed to create diag_mode sysfs entry\n");
-	if (device_create_file(dev, &dev_attr_beacon))
-		dev_info(dev, "failed to create beacon sysfs entry");
 	if (device_create_bin_file(dev, &bin_attr_crb))
 		dev_info(dev, "failed to create crb sysfs entry\n");
 	if (device_create_bin_file(dev, &bin_attr_mem))
 		dev_info(dev, "failed to create mem sysfs entry\n");
+
+	if (state == QLCNIC_DEV_FAILED || (state == QLCNIC_DEV_BADBAD))
+		return;
+
 	if (device_create_bin_file(dev, &bin_attr_pci_config))
 		dev_info(dev, "failed to create pci config sysfs entry");
+	if (device_create_file(dev, &dev_attr_beacon))
+		dev_info(dev, "failed to create beacon sysfs entry");
+
 	if (!(adapter->flags & QLCNIC_ESWITCH_ENABLED))
 		return;
 	if (device_create_bin_file(dev, &bin_attr_esw_config))
@@ -4314,16 +4351,19 @@ static void
 qlcnic_remove_diag_entries(struct qlcnic_adapter *adapter)
 {
 	struct device *dev = &adapter->pdev->dev;
+	u32 state = QLCRD32(adapter, QLCNIC_CRB_DEV_STATE);
 
 	device_remove_bin_file(dev, &bin_attr_port_stats);
 
 	if (adapter->op_mode == QLCNIC_NON_PRIV_FUNC)
 		return;
 	device_remove_file(dev, &dev_attr_diag_mode);
-	device_remove_file(dev, &dev_attr_beacon);
 	device_remove_bin_file(dev, &bin_attr_crb);
 	device_remove_bin_file(dev, &bin_attr_mem);
+	if (state == QLCNIC_DEV_FAILED || (state == QLCNIC_DEV_BADBAD))
+		return;
 	device_remove_bin_file(dev, &bin_attr_pci_config);
+	device_remove_file(dev, &dev_attr_beacon);
 	if (!(adapter->flags & QLCNIC_ESWITCH_ENABLED))
 		return;
 	device_remove_bin_file(dev, &bin_attr_esw_config);
diff --git a/drivers/net/ethernet/qlogic/qlge/qlge_main.c b/drivers/net/ethernet/qlogic/qlge/qlge_main.c
index 49343ec21c8..09d8d33171d 100644
--- a/drivers/net/ethernet/qlogic/qlge/qlge_main.c
+++ b/drivers/net/ethernet/qlogic/qlge/qlge_main.c
@@ -3845,7 +3845,7 @@ static int ql_wol(struct ql_adapter *qdev)
 	if (qdev->wol & (WAKE_ARP | WAKE_MAGICSECURE | WAKE_PHY | WAKE_UCAST |
 			WAKE_MCAST | WAKE_BCAST)) {
 		netif_err(qdev, ifdown, qdev->ndev,
-			  "Unsupported WOL paramter. qdev->wol = 0x%x.\n",
+			  "Unsupported WOL parameter. qdev->wol = 0x%x.\n",
 			  qdev->wol);
 		return -EINVAL;
 	}
diff --git a/drivers/net/ethernet/rdc/r6040.c b/drivers/net/ethernet/rdc/r6040.c
index b96e1920e04..4de73643fec 100644
--- a/drivers/net/ethernet/rdc/r6040.c
+++ b/drivers/net/ethernet/rdc/r6040.c
@@ -4,7 +4,7 @@
  * Copyright (C) 2004 Sten Wang <sten.wang@rdc.com.tw>
  * Copyright (C) 2007
  *	Daniel Gimpelevich <daniel@gimpelevich.san-francisco.ca.us>
- *	Florian Fainelli <florian@openwrt.org>
+ * Copyright (C) 2007-2012 Florian Fainelli <florian@openwrt.org>
  *
  * This program is free software; you can redistribute it and/or
  * modify it under the terms of the GNU General Public License
@@ -74,9 +74,13 @@
 #define MT_ICR		0x0C	/* TX interrupt control */
 #define MR_ICR		0x10	/* RX interrupt control */
 #define MTPR		0x14	/* TX poll command register */
+#define  TM2TX		0x0001	/* Trigger MAC to transmit */
 #define MR_BSR		0x18	/* RX buffer size */
 #define MR_DCR		0x1A	/* RX descriptor control */
 #define MLSR		0x1C	/* Last status */
+#define  TX_FIFO_UNDR	0x0200	/* TX FIFO under-run */
+#define	 TX_EXCEEDC	0x2000	/* Transmit exceed collision */
+#define  TX_LATEC	0x4000	/* Transmit late collision */
 #define MMDIO		0x20	/* MDIO control register */
 #define  MDIO_WRITE	0x4000	/* MDIO write */
 #define  MDIO_READ	0x2000	/* MDIO read */
@@ -124,6 +128,9 @@
 #define MID_3M		0x82	/* MID3 Medium */
 #define MID_3H		0x84	/* MID3 High */
 #define PHY_CC		0x88	/* PHY status change configuration register */
+#define  SCEN		0x8000	/* PHY status change enable */
+#define  PHYAD_SHIFT	8	/* PHY address shift */
+#define  TMRDIV_SHIFT	0	/* Timer divider shift */
 #define PHY_ST		0x8A	/* PHY status register */
 #define MAC_SM		0xAC	/* MAC status machine */
 #define  MAC_SM_RST	0x0002	/* MAC status machine reset */
@@ -137,6 +144,8 @@
 #define MBCR_DEFAULT	0x012A	/* MAC Bus Control Register */
 #define MCAST_MAX	3	/* Max number multicast addresses to filter */
 
+#define MAC_DEF_TIMEOUT	2048	/* Default MAC read/write operation timeout */
+
 /* Descriptor status */
 #define DSC_OWNER_MAC	0x8000	/* MAC is the owner of this descriptor */
 #define DSC_RX_OK	0x4000	/* RX was successful */
@@ -187,7 +196,7 @@ struct r6040_private {
 	dma_addr_t rx_ring_dma;
 	dma_addr_t tx_ring_dma;
 	u16	tx_free_desc;
-	u16	mcr0, mcr1;
+	u16	mcr0;
 	struct net_device *dev;
 	struct mii_bus *mii_bus;
 	struct napi_struct napi;
@@ -204,7 +213,7 @@ static char version[] __devinitdata = DRV_NAME
 /* Read a word data from PHY Chip */
 static int r6040_phy_read(void __iomem *ioaddr, int phy_addr, int reg)
 {
-	int limit = 2048;
+	int limit = MAC_DEF_TIMEOUT;
 	u16 cmd;
 
 	iowrite16(MDIO_READ + reg + (phy_addr << 8), ioaddr + MMDIO);
@@ -222,7 +231,7 @@ static int r6040_phy_read(void __iomem *ioaddr, int phy_addr, int reg)
 static void r6040_phy_write(void __iomem *ioaddr,
 					int phy_addr, int reg, u16 val)
 {
-	int limit = 2048;
+	int limit = MAC_DEF_TIMEOUT;
 	u16 cmd;
 
 	iowrite16(val, ioaddr + MMWD);
@@ -358,27 +367,35 @@ err_exit:
 	return rc;
 }
 
-static void r6040_init_mac_regs(struct net_device *dev)
+static void r6040_reset_mac(struct r6040_private *lp)
 {
-	struct r6040_private *lp = netdev_priv(dev);
 	void __iomem *ioaddr = lp->base;
-	int limit = 2048;
+	int limit = MAC_DEF_TIMEOUT;
 	u16 cmd;
 
-	/* Mask Off Interrupt */
-	iowrite16(MSK_INT, ioaddr + MIER);
-
-	/* Reset RDC MAC */
 	iowrite16(MAC_RST, ioaddr + MCR1);
 	while (limit--) {
 		cmd = ioread16(ioaddr + MCR1);
 		if (cmd & MAC_RST)
 			break;
 	}
+
 	/* Reset internal state machine */
 	iowrite16(MAC_SM_RST, ioaddr + MAC_SM);
 	iowrite16(0, ioaddr + MAC_SM);
 	mdelay(5);
+}
+
+static void r6040_init_mac_regs(struct net_device *dev)
+{
+	struct r6040_private *lp = netdev_priv(dev);
+	void __iomem *ioaddr = lp->base;
+
+	/* Mask Off Interrupt */
+	iowrite16(MSK_INT, ioaddr + MIER);
+
+	/* Reset RDC MAC */
+	r6040_reset_mac(lp);
 
 	/* MAC Bus Control Register */
 	iowrite16(MBCR_DEFAULT, ioaddr + MBCR);
@@ -407,7 +424,7 @@ static void r6040_init_mac_regs(struct net_device *dev)
 	/* Let TX poll the descriptors
 	 * we may got called by r6040_tx_timeout which has left
 	 * some unsent tx buffers */
-	iowrite16(0x01, ioaddr + MTPR);
+	iowrite16(TM2TX, ioaddr + MTPR);
 }
 
 static void r6040_tx_timeout(struct net_device *dev)
@@ -445,18 +462,13 @@ static void r6040_down(struct net_device *dev)
 {
 	struct r6040_private *lp = netdev_priv(dev);
 	void __iomem *ioaddr = lp->base;
-	int limit = 2048;
 	u16 *adrp;
-	u16 cmd;
 
 	/* Stop MAC */
 	iowrite16(MSK_INT, ioaddr + MIER);	/* Mask Off Interrupt */
-	iowrite16(MAC_RST, ioaddr + MCR1);	/* Reset RDC MAC */
-	while (limit--) {
-		cmd = ioread16(ioaddr + MCR1);
-		if (cmd & MAC_RST)
-			break;
-	}
+
+	/* Reset RDC MAC */
+	r6040_reset_mac(lp);
 
 	/* Restore MAC Address to MIDx */
 	adrp = (u16 *) dev->dev_addr;
@@ -599,9 +611,9 @@ static void r6040_tx(struct net_device *dev)
 		/* Check for errors */
 		err = ioread16(ioaddr + MLSR);
 
-		if (err & 0x0200)
-			dev->stats.rx_fifo_errors++;
-		if (err & (0x2000 | 0x4000))
+		if (err & TX_FIFO_UNDR)
+			dev->stats.tx_fifo_errors++;
+		if (err & (TX_EXCEEDC | TX_LATEC))
 			dev->stats.tx_carrier_errors++;
 
 		if (descptr->status & DSC_OWNER_MAC)
@@ -736,11 +748,7 @@ static void r6040_mac_address(struct net_device *dev)
 	u16 *adrp;
 
 	/* Reset MAC */
-	iowrite16(MAC_RST, ioaddr + MCR1);
-	/* Reset internal state machine */
-	iowrite16(MAC_SM_RST, ioaddr + MAC_SM);
-	iowrite16(0, ioaddr + MAC_SM);
-	mdelay(5);
+	r6040_reset_mac(lp);
 
 	/* Restore MAC Address */
 	adrp = (u16 *) dev->dev_addr;
@@ -840,7 +848,7 @@ static netdev_tx_t r6040_start_xmit(struct sk_buff *skb,
 	skb_tx_timestamp(skb);
 
 	/* Trigger the MAC to check the TX descriptor */
-	iowrite16(0x01, ioaddr + MTPR);
+	iowrite16(TM2TX, ioaddr + MTPR);
 	lp->tx_insert_ptr = descptr->vndescp;
 
 	/* If no tx resource, stop */
@@ -973,6 +981,7 @@ static const struct ethtool_ops netdev_ethtool_ops = {
 	.get_settings		= netdev_get_settings,
 	.set_settings		= netdev_set_settings,
 	.get_link		= ethtool_op_get_link,
+	.get_ts_info		= ethtool_op_get_ts_info,
 };
 
 static const struct net_device_ops r6040_netdev_ops = {
@@ -1126,10 +1135,15 @@ static int __devinit r6040_init_one(struct pci_dev *pdev,
 		err = -EIO;
 		goto err_out_free_res;
 	}
+
 	/* If PHY status change register is still set to zero it means the
-	 * bootloader didn't initialize it */
+	 * bootloader didn't initialize it, so we set it to:
+	 * - enable phy status change
+	 * - enable all phy addresses
+	 * - set to lowest timer divider */
 	if (ioread16(ioaddr + PHY_CC) == 0)
-		iowrite16(0x9f07, ioaddr + PHY_CC);
+		iowrite16(SCEN | PHY_MAX_ADDR << PHYAD_SHIFT |
+				7 << TMRDIV_SHIFT, ioaddr + PHY_CC);
 
 	/* Init system & device */
 	lp->base = ioaddr;
diff --git a/drivers/net/ethernet/realtek/8139cp.c b/drivers/net/ethernet/realtek/8139cp.c
index b3287c0fe27..5eef290997f 100644
--- a/drivers/net/ethernet/realtek/8139cp.c
+++ b/drivers/net/ethernet/realtek/8139cp.c
@@ -635,9 +635,12 @@ static irqreturn_t cp_interrupt (int irq, void *dev_instance)
  */
 static void cp_poll_controller(struct net_device *dev)
 {
-	disable_irq(dev->irq);
-	cp_interrupt(dev->irq, dev);
-	enable_irq(dev->irq);
+	struct cp_private *cp = netdev_priv(dev);
+	const int irq = cp->pdev->irq;
+
+	disable_irq(irq);
+	cp_interrupt(irq, dev);
+	enable_irq(irq);
 }
 #endif
 
@@ -1117,6 +1120,7 @@ static void cp_free_rings (struct cp_private *cp)
 static int cp_open (struct net_device *dev)
 {
 	struct cp_private *cp = netdev_priv(dev);
+	const int irq = cp->pdev->irq;
 	int rc;
 
 	netif_dbg(cp, ifup, dev, "enabling interface\n");
@@ -1129,7 +1133,7 @@ static int cp_open (struct net_device *dev)
 
 	cp_init_hw(cp);
 
-	rc = request_irq(dev->irq, cp_interrupt, IRQF_SHARED, dev->name, dev);
+	rc = request_irq(irq, cp_interrupt, IRQF_SHARED, dev->name, dev);
 	if (rc)
 		goto err_out_hw;
 
@@ -1166,7 +1170,7 @@ static int cp_close (struct net_device *dev)
 
 	spin_unlock_irqrestore(&cp->lock, flags);
 
-	free_irq(dev->irq, dev);
+	free_irq(cp->pdev->irq, dev);
 
 	cp_free_rings(cp);
 	return 0;
@@ -1914,7 +1918,6 @@ static int cp_init_one (struct pci_dev *pdev, const struct pci_device_id *ent)
 		       (unsigned long long)pciaddr);
 		goto err_out_res;
 	}
-	dev->base_addr = (unsigned long) regs;
 	cp->regs = regs;
 
 	cp_stop_hw(cp);
@@ -1942,14 +1945,12 @@ static int cp_init_one (struct pci_dev *pdev, const struct pci_device_id *ent)
 	dev->vlan_features = NETIF_F_SG | NETIF_F_IP_CSUM | NETIF_F_TSO |
 		NETIF_F_HIGHDMA;
 
-	dev->irq = pdev->irq;
-
 	rc = register_netdev(dev);
 	if (rc)
 		goto err_out_iomap;
 
-	netdev_info(dev, "RTL-8139C+ at 0x%lx, %pM, IRQ %d\n",
-		    dev->base_addr, dev->dev_addr, dev->irq);
+	netdev_info(dev, "RTL-8139C+ at 0x%p, %pM, IRQ %d\n",
+		    regs, dev->dev_addr, pdev->irq);
 
 	pci_set_drvdata(pdev, dev);
 
diff --git a/drivers/net/ethernet/realtek/8139too.c b/drivers/net/ethernet/realtek/8139too.c
index df7fd8d083d..03df076ed59 100644
--- a/drivers/net/ethernet/realtek/8139too.c
+++ b/drivers/net/ethernet/realtek/8139too.c
@@ -148,9 +148,9 @@ static int full_duplex[MAX_UNITS] = {-1, -1, -1, -1, -1, -1, -1, -1};
 
 /* Whether to use MMIO or PIO. Default to MMIO. */
 #ifdef CONFIG_8139TOO_PIO
-static int use_io = 1;
+static bool use_io = true;
 #else
-static int use_io = 0;
+static bool use_io = false;
 #endif
 
 /* Maximum number of multicast addresses to filter (vs. Rx-all-multicast).
@@ -620,7 +620,7 @@ MODULE_DESCRIPTION ("RealTek RTL-8139 Fast Ethernet driver");
 MODULE_LICENSE("GPL");
 MODULE_VERSION(DRV_VERSION);
 
-module_param(use_io, int, 0);
+module_param(use_io, bool, 0);
 MODULE_PARM_DESC(use_io, "Force use of I/O access mode. 0=MMIO 1=PIO");
 module_param(multicast_filter_limit, int, 0);
 module_param_array(media, int, NULL, 0);
@@ -750,15 +750,22 @@ static void rtl8139_chip_reset (void __iomem *ioaddr)
 
 static __devinit struct net_device * rtl8139_init_board (struct pci_dev *pdev)
 {
+	struct device *d = &pdev->dev;
 	void __iomem *ioaddr;
 	struct net_device *dev;
 	struct rtl8139_private *tp;
 	u8 tmp8;
 	int rc, disable_dev_on_err = 0;
-	unsigned int i;
-	unsigned long pio_start, pio_end, pio_flags, pio_len;
-	unsigned long mmio_start, mmio_end, mmio_flags, mmio_len;
+	unsigned int i, bar;
+	unsigned long io_len;
 	u32 version;
+	static const struct {
+		unsigned long mask;
+		char *type;
+	} res[] = {
+		{ IORESOURCE_IO,  "PIO" },
+		{ IORESOURCE_MEM, "MMIO" }
+	};
 
 	assert (pdev != NULL);
 
@@ -777,78 +784,45 @@ static __devinit struct net_device * rtl8139_init_board (struct pci_dev *pdev)
 	if (rc)
 		goto err_out;
 
-	pio_start = pci_resource_start (pdev, 0);
-	pio_end = pci_resource_end (pdev, 0);
-	pio_flags = pci_resource_flags (pdev, 0);
-	pio_len = pci_resource_len (pdev, 0);
-
-	mmio_start = pci_resource_start (pdev, 1);
-	mmio_end = pci_resource_end (pdev, 1);
-	mmio_flags = pci_resource_flags (pdev, 1);
-	mmio_len = pci_resource_len (pdev, 1);
-
-	/* set this immediately, we need to know before
-	 * we talk to the chip directly */
-	pr_debug("PIO region size == 0x%02lX\n", pio_len);
-	pr_debug("MMIO region size == 0x%02lX\n", mmio_len);
-
-retry:
-	if (use_io) {
-		/* make sure PCI base addr 0 is PIO */
-		if (!(pio_flags & IORESOURCE_IO)) {
-			dev_err(&pdev->dev, "region #0 not a PIO resource, aborting\n");
-			rc = -ENODEV;
-			goto err_out;
-		}
-		/* check for weird/broken PCI region reporting */
-		if (pio_len < RTL_MIN_IO_SIZE) {
-			dev_err(&pdev->dev, "Invalid PCI I/O region size(s), aborting\n");
-			rc = -ENODEV;
-			goto err_out;
-		}
-	} else {
-		/* make sure PCI base addr 1 is MMIO */
-		if (!(mmio_flags & IORESOURCE_MEM)) {
-			dev_err(&pdev->dev, "region #1 not an MMIO resource, aborting\n");
-			rc = -ENODEV;
-			goto err_out;
-		}
-		if (mmio_len < RTL_MIN_IO_SIZE) {
-			dev_err(&pdev->dev, "Invalid PCI mem region size(s), aborting\n");
-			rc = -ENODEV;
-			goto err_out;
-		}
-	}
-
 	rc = pci_request_regions (pdev, DRV_NAME);
 	if (rc)
 		goto err_out;
 	disable_dev_on_err = 1;
 
-	/* enable PCI bus-mastering */
 	pci_set_master (pdev);
 
-	if (use_io) {
-		ioaddr = pci_iomap(pdev, 0, 0);
-		if (!ioaddr) {
-			dev_err(&pdev->dev, "cannot map PIO, aborting\n");
-			rc = -EIO;
-			goto err_out;
-		}
-		dev->base_addr = pio_start;
-		tp->regs_len = pio_len;
-	} else {
-		/* ioremap MMIO region */
-		ioaddr = pci_iomap(pdev, 1, 0);
-		if (ioaddr == NULL) {
-			dev_err(&pdev->dev, "cannot remap MMIO, trying PIO\n");
-			pci_release_regions(pdev);
-			use_io = 1;
+retry:
+	/* PIO bar register comes first. */
+	bar = !use_io;
+
+	io_len = pci_resource_len(pdev, bar);
+
+	dev_dbg(d, "%s region size = 0x%02lX\n", res[bar].type, io_len);
+
+	if (!(pci_resource_flags(pdev, bar) & res[bar].mask)) {
+		dev_err(d, "region #%d not a %s resource, aborting\n", bar,
+			res[bar].type);
+		rc = -ENODEV;
+		goto err_out;
+	}
+	if (io_len < RTL_MIN_IO_SIZE) {
+		dev_err(d, "Invalid PCI %s region size(s), aborting\n",
+			res[bar].type);
+		rc = -ENODEV;
+		goto err_out;
+	}
+
+	ioaddr = pci_iomap(pdev, bar, 0);
+	if (!ioaddr) {
+		dev_err(d, "cannot map %s\n", res[bar].type);
+		if (!use_io) {
+			use_io = true;
 			goto retry;
 		}
-		dev->base_addr = (long) ioaddr;
-		tp->regs_len = mmio_len;
+		rc = -ENODEV;
+		goto err_out;
 	}
+	tp->regs_len = io_len;
 	tp->mmio_addr = ioaddr;
 
 	/* Bring old chips out of low-power mode. */
@@ -1035,8 +1009,6 @@ static int __devinit rtl8139_init_one (struct pci_dev *pdev,
 	dev->hw_features |= NETIF_F_RXALL;
 	dev->hw_features |= NETIF_F_RXFCS;
 
-	dev->irq = pdev->irq;
-
 	/* tp zeroed and aligned in alloc_etherdev */
 	tp = netdev_priv(dev);
 
@@ -1062,9 +1034,9 @@ static int __devinit rtl8139_init_one (struct pci_dev *pdev,
 
 	pci_set_drvdata (pdev, dev);
 
-	netdev_info(dev, "%s at 0x%lx, %pM, IRQ %d\n",
+	netdev_info(dev, "%s at 0x%p, %pM, IRQ %d\n",
 		    board_info[ent->driver_data].name,
-		    dev->base_addr, dev->dev_addr, dev->irq);
+		    ioaddr, dev->dev_addr, pdev->irq);
 
 	netdev_dbg(dev, "Identified 8139 chip type '%s'\n",
 		   rtl_chip_info[tp->chipset].name);
@@ -1339,10 +1311,11 @@ static void mdio_write (struct net_device *dev, int phy_id, int location,
 static int rtl8139_open (struct net_device *dev)
 {
 	struct rtl8139_private *tp = netdev_priv(dev);
-	int retval;
 	void __iomem *ioaddr = tp->mmio_addr;
+	const int irq = tp->pci_dev->irq;
+	int retval;
 
-	retval = request_irq (dev->irq, rtl8139_interrupt, IRQF_SHARED, dev->name, dev);
+	retval = request_irq(irq, rtl8139_interrupt, IRQF_SHARED, dev->name, dev);
 	if (retval)
 		return retval;
 
@@ -1351,7 +1324,7 @@ static int rtl8139_open (struct net_device *dev)
 	tp->rx_ring = dma_alloc_coherent(&tp->pci_dev->dev, RX_BUF_TOT_LEN,
 					   &tp->rx_ring_dma, GFP_KERNEL);
 	if (tp->tx_bufs == NULL || tp->rx_ring == NULL) {
-		free_irq(dev->irq, dev);
+		free_irq(irq, dev);
 
 		if (tp->tx_bufs)
 			dma_free_coherent(&tp->pci_dev->dev, TX_BUF_TOT_LEN,
@@ -1377,7 +1350,7 @@ static int rtl8139_open (struct net_device *dev)
 		  "%s() ioaddr %#llx IRQ %d GP Pins %02x %s-duplex\n",
 		  __func__,
 		  (unsigned long long)pci_resource_start (tp->pci_dev, 1),
-		  dev->irq, RTL_R8 (MediaStatus),
+		  irq, RTL_R8 (MediaStatus),
 		  tp->mii.full_duplex ? "full" : "half");
 
 	rtl8139_start_thread(tp);
@@ -2240,9 +2213,12 @@ static irqreturn_t rtl8139_interrupt (int irq, void *dev_instance)
  */
 static void rtl8139_poll_controller(struct net_device *dev)
 {
-	disable_irq(dev->irq);
-	rtl8139_interrupt(dev->irq, dev);
-	enable_irq(dev->irq);
+	struct rtl8139_private *tp = netdev_priv(dev);
+	const int irq = tp->pci_dev->irq;
+
+	disable_irq(irq);
+	rtl8139_interrupt(irq, dev);
+	enable_irq(irq);
 }
 #endif
 
@@ -2295,7 +2271,7 @@ static int rtl8139_close (struct net_device *dev)
 
 	spin_unlock_irqrestore (&tp->lock, flags);
 
-	free_irq (dev->irq, dev);
+	free_irq(tp->pci_dev->irq, dev);
 
 	rtl8139_tx_clear (tp);
 
diff --git a/drivers/net/ethernet/realtek/r8169.c b/drivers/net/ethernet/realtek/r8169.c
index f54509377ef..00b4f56a671 100644
--- a/drivers/net/ethernet/realtek/r8169.c
+++ b/drivers/net/ethernet/realtek/r8169.c
@@ -44,6 +44,8 @@
 #define FIRMWARE_8168F_1	"rtl_nic/rtl8168f-1.fw"
 #define FIRMWARE_8168F_2	"rtl_nic/rtl8168f-2.fw"
 #define FIRMWARE_8105E_1	"rtl_nic/rtl8105e-1.fw"
+#define FIRMWARE_8402_1		"rtl_nic/rtl8402-1.fw"
+#define FIRMWARE_8411_1		"rtl_nic/rtl8411-1.fw"
 
 #ifdef RTL8169_DEBUG
 #define assert(expr) \
@@ -61,8 +63,12 @@
 #define R8169_MSG_DEFAULT \
 	(NETIF_MSG_DRV | NETIF_MSG_PROBE | NETIF_MSG_IFUP | NETIF_MSG_IFDOWN)
 
-#define TX_BUFFS_AVAIL(tp) \
-	(tp->dirty_tx + NUM_TX_DESC - tp->cur_tx - 1)
+#define TX_SLOTS_AVAIL(tp) \
+	(tp->dirty_tx + NUM_TX_DESC - tp->cur_tx)
+
+/* A skbuff with nr_frags needs nr_frags+1 entries in the tx queue */
+#define TX_FRAGS_READY_FOR(tp,nr_frags) \
+	(TX_SLOTS_AVAIL(tp) >= (nr_frags + 1))
 
 /* 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. */
@@ -133,6 +139,8 @@ enum mac_version {
 	RTL_GIGA_MAC_VER_34,
 	RTL_GIGA_MAC_VER_35,
 	RTL_GIGA_MAC_VER_36,
+	RTL_GIGA_MAC_VER_37,
+	RTL_GIGA_MAC_VER_38,
 	RTL_GIGA_MAC_NONE   = 0xff,
 };
 
@@ -245,6 +253,12 @@ static const struct {
 	[RTL_GIGA_MAC_VER_36] =
 		_R("RTL8168f/8111f",	RTL_TD_1, FIRMWARE_8168F_2,
 							JUMBO_9K, false),
+	[RTL_GIGA_MAC_VER_37] =
+		_R("RTL8402",		RTL_TD_1, FIRMWARE_8402_1,
+							JUMBO_1K, true),
+	[RTL_GIGA_MAC_VER_38] =
+		_R("RTL8411",		RTL_TD_1, FIRMWARE_8411_1,
+							JUMBO_9K, false),
 };
 #undef _R
 
@@ -315,6 +329,8 @@ enum rtl_registers {
 	Config0		= 0x51,
 	Config1		= 0x52,
 	Config2		= 0x53,
+#define PME_SIGNAL			(1 << 5)	/* 8168c and later */
+
 	Config3		= 0x54,
 	Config4		= 0x55,
 	Config5		= 0x56,
@@ -355,6 +371,9 @@ enum rtl8168_8101_registers {
 #define	CSIAR_BYTE_ENABLE		0x0f
 #define	CSIAR_BYTE_ENABLE_SHIFT		12
 #define	CSIAR_ADDR_MASK			0x0fff
+#define CSIAR_FUNC_CARD			0x00000000
+#define CSIAR_FUNC_SDIO			0x00010000
+#define CSIAR_FUNC_NIC			0x00020000
 	PMCH			= 0x6f,
 	EPHYAR			= 0x80,
 #define	EPHYAR_FLAG			0x80000000
@@ -716,6 +735,11 @@ struct rtl8169_private {
 		void (*disable)(struct rtl8169_private *);
 	} jumbo_ops;
 
+	struct csi_ops {
+		void (*write)(void __iomem *, int, int);
+		u32 (*read)(void __iomem *, int);
+	} csi_ops;
+
 	int (*set_speed)(struct net_device *, u8 aneg, u16 sp, u8 dpx, u32 adv);
 	int (*get_settings)(struct net_device *, struct ethtool_cmd *);
 	void (*phy_reset_enable)(struct rtl8169_private *tp);
@@ -768,6 +792,8 @@ MODULE_FIRMWARE(FIRMWARE_8168E_3);
 MODULE_FIRMWARE(FIRMWARE_8105E_1);
 MODULE_FIRMWARE(FIRMWARE_8168F_1);
 MODULE_FIRMWARE(FIRMWARE_8168F_2);
+MODULE_FIRMWARE(FIRMWARE_8402_1);
+MODULE_FIRMWARE(FIRMWARE_8411_1);
 
 static void rtl_lock_work(struct rtl8169_private *tp)
 {
@@ -1078,40 +1104,6 @@ static u16 rtl_ephy_read(void __iomem *ioaddr, int reg_addr)
 	return value;
 }
 
-static void rtl_csi_write(void __iomem *ioaddr, int addr, int value)
-{
-	unsigned int i;
-
-	RTL_W32(CSIDR, value);
-	RTL_W32(CSIAR, CSIAR_WRITE_CMD | (addr & CSIAR_ADDR_MASK) |
-		CSIAR_BYTE_ENABLE << CSIAR_BYTE_ENABLE_SHIFT);
-
-	for (i = 0; i < 100; i++) {
-		if (!(RTL_R32(CSIAR) & CSIAR_FLAG))
-			break;
-		udelay(10);
-	}
-}
-
-static u32 rtl_csi_read(void __iomem *ioaddr, int addr)
-{
-	u32 value = ~0x00;
-	unsigned int i;
-
-	RTL_W32(CSIAR, (addr & CSIAR_ADDR_MASK) |
-		CSIAR_BYTE_ENABLE << CSIAR_BYTE_ENABLE_SHIFT);
-
-	for (i = 0; i < 100; i++) {
-		if (RTL_R32(CSIAR) & CSIAR_FLAG) {
-			value = RTL_R32(CSIDR);
-			break;
-		}
-		udelay(10);
-	}
-
-	return value;
-}
-
 static
 void rtl_eri_write(void __iomem *ioaddr, int addr, u32 mask, u32 val, int type)
 {
@@ -1281,7 +1273,8 @@ static void rtl_link_chg_patch(struct rtl8169_private *tp)
 	if (!netif_running(dev))
 		return;
 
-	if (tp->mac_version == RTL_GIGA_MAC_VER_34) {
+	if (tp->mac_version == RTL_GIGA_MAC_VER_34 ||
+	    tp->mac_version == RTL_GIGA_MAC_VER_38) {
 		if (RTL_R8(PHYstatus) & _1000bpsF) {
 			rtl_eri_write(ioaddr, 0x1bc, ERIAR_MASK_1111,
 				      0x00000011, ERIAR_EXGMAC);
@@ -1316,6 +1309,16 @@ static void rtl_link_chg_patch(struct rtl8169_private *tp)
 			rtl_eri_write(ioaddr, 0x1dc, ERIAR_MASK_1111,
 				      0x0000003f, ERIAR_EXGMAC);
 		}
+	} else if (tp->mac_version == RTL_GIGA_MAC_VER_37) {
+		if (RTL_R8(PHYstatus) & _10bps) {
+			rtl_eri_write(ioaddr, 0x1d0, ERIAR_MASK_0011,
+				      0x4d02, ERIAR_EXGMAC);
+			rtl_eri_write(ioaddr, 0x1dc, ERIAR_MASK_0011,
+				      0x0060, ERIAR_EXGMAC);
+		} else {
+			rtl_eri_write(ioaddr, 0x1d0, ERIAR_MASK_0011,
+				      0x0000, ERIAR_EXGMAC);
+		}
 	}
 }
 
@@ -1396,7 +1399,6 @@ static void __rtl8169_set_wol(struct rtl8169_private *tp, u32 wolopts)
 		u16 reg;
 		u8  mask;
 	} cfg[] = {
-		{ WAKE_ANY,   Config1, PMEnable },
 		{ WAKE_PHY,   Config3, LinkUp },
 		{ WAKE_MAGIC, Config3, MagicPacket },
 		{ WAKE_UCAST, Config5, UWF },
@@ -1404,16 +1406,32 @@ static void __rtl8169_set_wol(struct rtl8169_private *tp, u32 wolopts)
 		{ WAKE_MCAST, Config5, MWF },
 		{ WAKE_ANY,   Config5, LanWake }
 	};
+	u8 options;
 
 	RTL_W8(Cfg9346, Cfg9346_Unlock);
 
 	for (i = 0; i < ARRAY_SIZE(cfg); i++) {
-		u8 options = RTL_R8(cfg[i].reg) & ~cfg[i].mask;
+		options = RTL_R8(cfg[i].reg) & ~cfg[i].mask;
 		if (wolopts & cfg[i].opt)
 			options |= cfg[i].mask;
 		RTL_W8(cfg[i].reg, options);
 	}
 
+	switch (tp->mac_version) {
+	case RTL_GIGA_MAC_VER_01 ... RTL_GIGA_MAC_VER_17:
+		options = RTL_R8(Config1) & ~PMEnable;
+		if (wolopts)
+			options |= PMEnable;
+		RTL_W8(Config1, options);
+		break;
+	default:
+		options = RTL_R8(Config2) & ~PME_SIGNAL;
+		if (wolopts)
+			options |= PME_SIGNAL;
+		RTL_W8(Config2, options);
+		break;
+	}
+
 	RTL_W8(Cfg9346, Cfg9346_Lock);
 }
 
@@ -1853,6 +1871,7 @@ static const struct ethtool_ops rtl8169_ethtool_ops = {
 	.get_strings		= rtl8169_get_strings,
 	.get_sset_count		= rtl8169_get_sset_count,
 	.get_ethtool_stats	= rtl8169_get_ethtool_stats,
+	.get_ts_info		= ethtool_op_get_ts_info,
 };
 
 static void rtl8169_get_mac_version(struct rtl8169_private *tp,
@@ -1876,6 +1895,7 @@ static void rtl8169_get_mac_version(struct rtl8169_private *tp,
 		int mac_version;
 	} mac_info[] = {
 		/* 8168F family. */
+		{ 0x7c800000, 0x48800000,	RTL_GIGA_MAC_VER_38 },
 		{ 0x7cf00000, 0x48100000,	RTL_GIGA_MAC_VER_36 },
 		{ 0x7cf00000, 0x48000000,	RTL_GIGA_MAC_VER_35 },
 
@@ -1913,6 +1933,7 @@ static void rtl8169_get_mac_version(struct rtl8169_private *tp,
 		{ 0x7c800000, 0x30000000,	RTL_GIGA_MAC_VER_11 },
 
 		/* 8101 family. */
+		{ 0x7c800000, 0x44000000,	RTL_GIGA_MAC_VER_37 },
 		{ 0x7cf00000, 0x40b00000,	RTL_GIGA_MAC_VER_30 },
 		{ 0x7cf00000, 0x40a00000,	RTL_GIGA_MAC_VER_30 },
 		{ 0x7cf00000, 0x40900000,	RTL_GIGA_MAC_VER_29 },
@@ -3013,6 +3034,28 @@ static void rtl8168e_2_hw_phy_config(struct rtl8169_private *tp)
 	rtl_writephy(tp, 0x1f, 0x0000);
 }
 
+static void rtl8168f_hw_phy_config(struct rtl8169_private *tp)
+{
+	/* For 4-corner performance improve */
+	rtl_writephy(tp, 0x1f, 0x0005);
+	rtl_writephy(tp, 0x05, 0x8b80);
+	rtl_w1w0_phy(tp, 0x06, 0x0006, 0x0000);
+	rtl_writephy(tp, 0x1f, 0x0000);
+
+	/* PHY auto speed down */
+	rtl_writephy(tp, 0x1f, 0x0007);
+	rtl_writephy(tp, 0x1e, 0x002d);
+	rtl_w1w0_phy(tp, 0x18, 0x0010, 0x0000);
+	rtl_writephy(tp, 0x1f, 0x0000);
+	rtl_w1w0_phy(tp, 0x14, 0x8000, 0x0000);
+
+	/* Improve 10M EEE waveform */
+	rtl_writephy(tp, 0x1f, 0x0005);
+	rtl_writephy(tp, 0x05, 0x8b86);
+	rtl_w1w0_phy(tp, 0x06, 0x0001, 0x0000);
+	rtl_writephy(tp, 0x1f, 0x0000);
+}
+
 static void rtl8168f_1_hw_phy_config(struct rtl8169_private *tp)
 {
 	static const struct phy_reg phy_reg_init[] = {
@@ -3054,24 +3097,7 @@ static void rtl8168f_1_hw_phy_config(struct rtl8169_private *tp)
 
 	rtl_writephy_batch(tp, phy_reg_init, ARRAY_SIZE(phy_reg_init));
 
-	/* For 4-corner performance improve */
-	rtl_writephy(tp, 0x1f, 0x0005);
-	rtl_writephy(tp, 0x05, 0x8b80);
-	rtl_w1w0_phy(tp, 0x06, 0x0006, 0x0000);
-	rtl_writephy(tp, 0x1f, 0x0000);
-
-	/* PHY auto speed down */
-	rtl_writephy(tp, 0x1f, 0x0007);
-	rtl_writephy(tp, 0x1e, 0x002d);
-	rtl_w1w0_phy(tp, 0x18, 0x0010, 0x0000);
-	rtl_writephy(tp, 0x1f, 0x0000);
-	rtl_w1w0_phy(tp, 0x14, 0x8000, 0x0000);
-
-	/* Improve 10M EEE waveform */
-	rtl_writephy(tp, 0x1f, 0x0005);
-	rtl_writephy(tp, 0x05, 0x8b86);
-	rtl_w1w0_phy(tp, 0x06, 0x0001, 0x0000);
-	rtl_writephy(tp, 0x1f, 0x0000);
+	rtl8168f_hw_phy_config(tp);
 
 	/* Improve 2-pair detection performance */
 	rtl_writephy(tp, 0x1f, 0x0005);
@@ -3084,23 +3110,104 @@ static void rtl8168f_2_hw_phy_config(struct rtl8169_private *tp)
 {
 	rtl_apply_firmware(tp);
 
-	/* For 4-corner performance improve */
+	rtl8168f_hw_phy_config(tp);
+}
+
+static void rtl8411_hw_phy_config(struct rtl8169_private *tp)
+{
+	void __iomem *ioaddr = tp->mmio_addr;
+	static const struct phy_reg phy_reg_init[] = {
+		/* Channel estimation fine tune */
+		{ 0x1f, 0x0003 },
+		{ 0x09, 0xa20f },
+		{ 0x1f, 0x0000 },
+
+		/* Modify green table for giga & fnet */
+		{ 0x1f, 0x0005 },
+		{ 0x05, 0x8b55 },
+		{ 0x06, 0x0000 },
+		{ 0x05, 0x8b5e },
+		{ 0x06, 0x0000 },
+		{ 0x05, 0x8b67 },
+		{ 0x06, 0x0000 },
+		{ 0x05, 0x8b70 },
+		{ 0x06, 0x0000 },
+		{ 0x1f, 0x0000 },
+		{ 0x1f, 0x0007 },
+		{ 0x1e, 0x0078 },
+		{ 0x17, 0x0000 },
+		{ 0x19, 0x00aa },
+		{ 0x1f, 0x0000 },
+
+		/* Modify green table for 10M */
+		{ 0x1f, 0x0005 },
+		{ 0x05, 0x8b79 },
+		{ 0x06, 0xaa00 },
+		{ 0x1f, 0x0000 },
+
+		/* Disable hiimpedance detection (RTCT) */
+		{ 0x1f, 0x0003 },
+		{ 0x01, 0x328a },
+		{ 0x1f, 0x0000 }
+	};
+
+
+	rtl_apply_firmware(tp);
+
+	rtl8168f_hw_phy_config(tp);
+
+	/* Improve 2-pair detection performance */
 	rtl_writephy(tp, 0x1f, 0x0005);
-	rtl_writephy(tp, 0x05, 0x8b80);
-	rtl_w1w0_phy(tp, 0x06, 0x0006, 0x0000);
+	rtl_writephy(tp, 0x05, 0x8b85);
+	rtl_w1w0_phy(tp, 0x06, 0x4000, 0x0000);
 	rtl_writephy(tp, 0x1f, 0x0000);
 
-	/* PHY auto speed down */
-	rtl_writephy(tp, 0x1f, 0x0007);
-	rtl_writephy(tp, 0x1e, 0x002d);
-	rtl_w1w0_phy(tp, 0x18, 0x0010, 0x0000);
+	rtl_writephy_batch(tp, phy_reg_init, ARRAY_SIZE(phy_reg_init));
+
+	/* Modify green table for giga */
+	rtl_writephy(tp, 0x1f, 0x0005);
+	rtl_writephy(tp, 0x05, 0x8b54);
+	rtl_w1w0_phy(tp, 0x06, 0x0000, 0x0800);
+	rtl_writephy(tp, 0x05, 0x8b5d);
+	rtl_w1w0_phy(tp, 0x06, 0x0000, 0x0800);
+	rtl_writephy(tp, 0x05, 0x8a7c);
+	rtl_w1w0_phy(tp, 0x06, 0x0000, 0x0100);
+	rtl_writephy(tp, 0x05, 0x8a7f);
+	rtl_w1w0_phy(tp, 0x06, 0x0100, 0x0000);
+	rtl_writephy(tp, 0x05, 0x8a82);
+	rtl_w1w0_phy(tp, 0x06, 0x0000, 0x0100);
+	rtl_writephy(tp, 0x05, 0x8a85);
+	rtl_w1w0_phy(tp, 0x06, 0x0000, 0x0100);
+	rtl_writephy(tp, 0x05, 0x8a88);
+	rtl_w1w0_phy(tp, 0x06, 0x0000, 0x0100);
 	rtl_writephy(tp, 0x1f, 0x0000);
-	rtl_w1w0_phy(tp, 0x14, 0x8000, 0x0000);
 
-	/* Improve 10M EEE waveform */
+	/* uc same-seed solution */
 	rtl_writephy(tp, 0x1f, 0x0005);
-	rtl_writephy(tp, 0x05, 0x8b86);
-	rtl_w1w0_phy(tp, 0x06, 0x0001, 0x0000);
+	rtl_writephy(tp, 0x05, 0x8b85);
+	rtl_w1w0_phy(tp, 0x06, 0x8000, 0x0000);
+	rtl_writephy(tp, 0x1f, 0x0000);
+
+	/* eee setting */
+	rtl_w1w0_eri(ioaddr, 0x1b0, ERIAR_MASK_0001, 0x00, 0x03, ERIAR_EXGMAC);
+	rtl_writephy(tp, 0x1f, 0x0005);
+	rtl_writephy(tp, 0x05, 0x8b85);
+	rtl_w1w0_phy(tp, 0x06, 0x0000, 0x2000);
+	rtl_writephy(tp, 0x1f, 0x0004);
+	rtl_writephy(tp, 0x1f, 0x0007);
+	rtl_writephy(tp, 0x1e, 0x0020);
+	rtl_w1w0_phy(tp, 0x15, 0x0000, 0x0100);
+	rtl_writephy(tp, 0x1f, 0x0000);
+	rtl_writephy(tp, 0x0d, 0x0007);
+	rtl_writephy(tp, 0x0e, 0x003c);
+	rtl_writephy(tp, 0x0d, 0x4007);
+	rtl_writephy(tp, 0x0e, 0x0000);
+	rtl_writephy(tp, 0x0d, 0x0000);
+
+	/* Green feature */
+	rtl_writephy(tp, 0x1f, 0x0003);
+	rtl_w1w0_phy(tp, 0x19, 0x0000, 0x0001);
+	rtl_w1w0_phy(tp, 0x10, 0x0000, 0x0400);
 	rtl_writephy(tp, 0x1f, 0x0000);
 }
 
@@ -3147,6 +3254,25 @@ static void rtl8105e_hw_phy_config(struct rtl8169_private *tp)
 	rtl_writephy_batch(tp, phy_reg_init, ARRAY_SIZE(phy_reg_init));
 }
 
+static void rtl8402_hw_phy_config(struct rtl8169_private *tp)
+{
+	void __iomem *ioaddr = tp->mmio_addr;
+
+	/* Disable ALDPS before setting firmware */
+	rtl_writephy(tp, 0x1f, 0x0000);
+	rtl_writephy(tp, 0x18, 0x0310);
+	msleep(20);
+
+	rtl_apply_firmware(tp);
+
+	/* EEE setting */
+	rtl_eri_write(ioaddr, 0x1b0, ERIAR_MASK_0011, 0x0000, ERIAR_EXGMAC);
+	rtl_writephy(tp, 0x1f, 0x0004);
+	rtl_writephy(tp, 0x10, 0x401f);
+	rtl_writephy(tp, 0x19, 0x7030);
+	rtl_writephy(tp, 0x1f, 0x0000);
+}
+
 static void rtl_hw_phy_config(struct net_device *dev)
 {
 	struct rtl8169_private *tp = netdev_priv(dev);
@@ -3235,6 +3361,14 @@ static void rtl_hw_phy_config(struct net_device *dev)
 		rtl8168f_2_hw_phy_config(tp);
 		break;
 
+	case RTL_GIGA_MAC_VER_37:
+		rtl8402_hw_phy_config(tp);
+		break;
+
+	case RTL_GIGA_MAC_VER_38:
+		rtl8411_hw_phy_config(tp);
+		break;
+
 	default:
 		break;
 	}
@@ -3472,6 +3606,8 @@ static void rtl_wol_suspend_quirk(struct rtl8169_private *tp)
 	case RTL_GIGA_MAC_VER_32:
 	case RTL_GIGA_MAC_VER_33:
 	case RTL_GIGA_MAC_VER_34:
+	case RTL_GIGA_MAC_VER_37:
+	case RTL_GIGA_MAC_VER_38:
 		RTL_W32(RxConfig, RTL_R32(RxConfig) |
 			AcceptBroadcast | AcceptMulticast | AcceptMyPhys);
 		break;
@@ -3507,15 +3643,45 @@ static void r810x_phy_power_up(struct rtl8169_private *tp)
 
 static void r810x_pll_power_down(struct rtl8169_private *tp)
 {
+	void __iomem *ioaddr = tp->mmio_addr;
+
 	if (rtl_wol_pll_power_down(tp))
 		return;
 
 	r810x_phy_power_down(tp);
+
+	switch (tp->mac_version) {
+	case RTL_GIGA_MAC_VER_07:
+	case RTL_GIGA_MAC_VER_08:
+	case RTL_GIGA_MAC_VER_09:
+	case RTL_GIGA_MAC_VER_10:
+	case RTL_GIGA_MAC_VER_13:
+	case RTL_GIGA_MAC_VER_16:
+		break;
+	default:
+		RTL_W8(PMCH, RTL_R8(PMCH) & ~0x80);
+		break;
+	}
 }
 
 static void r810x_pll_power_up(struct rtl8169_private *tp)
 {
+	void __iomem *ioaddr = tp->mmio_addr;
+
 	r810x_phy_power_up(tp);
+
+	switch (tp->mac_version) {
+	case RTL_GIGA_MAC_VER_07:
+	case RTL_GIGA_MAC_VER_08:
+	case RTL_GIGA_MAC_VER_09:
+	case RTL_GIGA_MAC_VER_10:
+	case RTL_GIGA_MAC_VER_13:
+	case RTL_GIGA_MAC_VER_16:
+		break;
+	default:
+		RTL_W8(PMCH, RTL_R8(PMCH) | 0x80);
+		break;
+	}
 }
 
 static void r8168_phy_power_up(struct rtl8169_private *tp)
@@ -3619,13 +3785,6 @@ static void r8168_pll_power_up(struct rtl8169_private *tp)
 {
 	void __iomem *ioaddr = tp->mmio_addr;
 
-	if ((tp->mac_version == RTL_GIGA_MAC_VER_27 ||
-	     tp->mac_version == RTL_GIGA_MAC_VER_28 ||
-	     tp->mac_version == RTL_GIGA_MAC_VER_31) &&
-	    r8168dp_check_dash(tp)) {
-		return;
-	}
-
 	switch (tp->mac_version) {
 	case RTL_GIGA_MAC_VER_25:
 	case RTL_GIGA_MAC_VER_26:
@@ -3670,6 +3829,7 @@ static void __devinit rtl_init_pll_power_ops(struct rtl8169_private *tp)
 	case RTL_GIGA_MAC_VER_16:
 	case RTL_GIGA_MAC_VER_29:
 	case RTL_GIGA_MAC_VER_30:
+	case RTL_GIGA_MAC_VER_37:
 		ops->down	= r810x_pll_power_down;
 		ops->up		= r810x_pll_power_up;
 		break;
@@ -3694,6 +3854,7 @@ static void __devinit rtl_init_pll_power_ops(struct rtl8169_private *tp)
 	case RTL_GIGA_MAC_VER_34:
 	case RTL_GIGA_MAC_VER_35:
 	case RTL_GIGA_MAC_VER_36:
+	case RTL_GIGA_MAC_VER_38:
 		ops->down	= r8168_pll_power_down;
 		ops->up		= r8168_pll_power_up;
 		break;
@@ -3979,7 +4140,9 @@ static void rtl8169_hw_reset(struct rtl8169_private *tp)
 			udelay(20);
 	} else if (tp->mac_version == RTL_GIGA_MAC_VER_34 ||
 	           tp->mac_version == RTL_GIGA_MAC_VER_35 ||
-	           tp->mac_version == RTL_GIGA_MAC_VER_36) {
+	           tp->mac_version == RTL_GIGA_MAC_VER_36 ||
+	           tp->mac_version == RTL_GIGA_MAC_VER_37 ||
+	           tp->mac_version == RTL_GIGA_MAC_VER_38) {
 		RTL_W8(ChipCmd, RTL_R8(ChipCmd) | StopReq);
 		while (!(RTL_R32(TxConfig) & TXCFG_EMPTY))
 			udelay(100);
@@ -4185,22 +4348,141 @@ static void rtl_hw_start_8169(struct net_device *dev)
 	RTL_W16(MultiIntr, RTL_R16(MultiIntr) & 0xF000);
 }
 
-static void rtl_csi_access_enable(void __iomem *ioaddr, u32 bits)
+static void rtl_csi_write(struct rtl8169_private *tp, int addr, int value)
+{
+	if (tp->csi_ops.write)
+		tp->csi_ops.write(tp->mmio_addr, addr, value);
+}
+
+static u32 rtl_csi_read(struct rtl8169_private *tp, int addr)
+{
+	if (tp->csi_ops.read)
+		return tp->csi_ops.read(tp->mmio_addr, addr);
+	else
+		return ~0;
+}
+
+static void rtl_csi_access_enable(struct rtl8169_private *tp, u32 bits)
 {
 	u32 csi;
 
-	csi = rtl_csi_read(ioaddr, 0x070c) & 0x00ffffff;
-	rtl_csi_write(ioaddr, 0x070c, csi | bits);
+	csi = rtl_csi_read(tp, 0x070c) & 0x00ffffff;
+	rtl_csi_write(tp, 0x070c, csi | bits);
+}
+
+static void rtl_csi_access_enable_1(struct rtl8169_private *tp)
+{
+	rtl_csi_access_enable(tp, 0x17000000);
+}
+
+static void rtl_csi_access_enable_2(struct rtl8169_private *tp)
+{
+	rtl_csi_access_enable(tp, 0x27000000);
+}
+
+static void r8169_csi_write(void __iomem *ioaddr, int addr, int value)
+{
+	unsigned int i;
+
+	RTL_W32(CSIDR, value);
+	RTL_W32(CSIAR, CSIAR_WRITE_CMD | (addr & CSIAR_ADDR_MASK) |
+		CSIAR_BYTE_ENABLE << CSIAR_BYTE_ENABLE_SHIFT);
+
+	for (i = 0; i < 100; i++) {
+		if (!(RTL_R32(CSIAR) & CSIAR_FLAG))
+			break;
+		udelay(10);
+	}
+}
+
+static u32 r8169_csi_read(void __iomem *ioaddr, int addr)
+{
+	u32 value = ~0x00;
+	unsigned int i;
+
+	RTL_W32(CSIAR, (addr & CSIAR_ADDR_MASK) |
+		CSIAR_BYTE_ENABLE << CSIAR_BYTE_ENABLE_SHIFT);
+
+	for (i = 0; i < 100; i++) {
+		if (RTL_R32(CSIAR) & CSIAR_FLAG) {
+			value = RTL_R32(CSIDR);
+			break;
+		}
+		udelay(10);
+	}
+
+	return value;
+}
+
+static void r8402_csi_write(void __iomem *ioaddr, int addr, int value)
+{
+	unsigned int i;
+
+	RTL_W32(CSIDR, value);
+	RTL_W32(CSIAR, CSIAR_WRITE_CMD | (addr & CSIAR_ADDR_MASK) |
+		CSIAR_BYTE_ENABLE << CSIAR_BYTE_ENABLE_SHIFT |
+		CSIAR_FUNC_NIC);
+
+	for (i = 0; i < 100; i++) {
+		if (!(RTL_R32(CSIAR) & CSIAR_FLAG))
+			break;
+		udelay(10);
+	}
 }
 
-static void rtl_csi_access_enable_1(void __iomem *ioaddr)
+static u32 r8402_csi_read(void __iomem *ioaddr, int addr)
 {
-	rtl_csi_access_enable(ioaddr, 0x17000000);
+	u32 value = ~0x00;
+	unsigned int i;
+
+	RTL_W32(CSIAR, (addr & CSIAR_ADDR_MASK) | CSIAR_FUNC_NIC |
+		CSIAR_BYTE_ENABLE << CSIAR_BYTE_ENABLE_SHIFT);
+
+	for (i = 0; i < 100; i++) {
+		if (RTL_R32(CSIAR) & CSIAR_FLAG) {
+			value = RTL_R32(CSIDR);
+			break;
+		}
+		udelay(10);
+	}
+
+	return value;
 }
 
-static void rtl_csi_access_enable_2(void __iomem *ioaddr)
+static void __devinit rtl_init_csi_ops(struct rtl8169_private *tp)
 {
-	rtl_csi_access_enable(ioaddr, 0x27000000);
+	struct csi_ops *ops = &tp->csi_ops;
+
+	switch (tp->mac_version) {
+	case RTL_GIGA_MAC_VER_01:
+	case RTL_GIGA_MAC_VER_02:
+	case RTL_GIGA_MAC_VER_03:
+	case RTL_GIGA_MAC_VER_04:
+	case RTL_GIGA_MAC_VER_05:
+	case RTL_GIGA_MAC_VER_06:
+	case RTL_GIGA_MAC_VER_10:
+	case RTL_GIGA_MAC_VER_11:
+	case RTL_GIGA_MAC_VER_12:
+	case RTL_GIGA_MAC_VER_13:
+	case RTL_GIGA_MAC_VER_14:
+	case RTL_GIGA_MAC_VER_15:
+	case RTL_GIGA_MAC_VER_16:
+	case RTL_GIGA_MAC_VER_17:
+		ops->write	= NULL;
+		ops->read	= NULL;
+		break;
+
+	case RTL_GIGA_MAC_VER_37:
+	case RTL_GIGA_MAC_VER_38:
+		ops->write	= r8402_csi_write;
+		ops->read	= r8402_csi_read;
+		break;
+
+	default:
+		ops->write	= r8169_csi_write;
+		ops->read	= r8169_csi_read;
+		break;
+	}
 }
 
 struct ephy_info {
@@ -4257,8 +4539,11 @@ static void rtl_enable_clock_request(struct pci_dev *pdev)
 	PktCntrDisable | \
 	Mac_dbgo_sel)
 
-static void rtl_hw_start_8168bb(void __iomem *ioaddr, struct pci_dev *pdev)
+static void rtl_hw_start_8168bb(struct rtl8169_private *tp)
 {
+	void __iomem *ioaddr = tp->mmio_addr;
+	struct pci_dev *pdev = tp->pci_dev;
+
 	RTL_W8(Config3, RTL_R8(Config3) & ~Beacon_en);
 
 	RTL_W16(CPlusCmd, RTL_R16(CPlusCmd) & ~R8168_CPCMD_QUIRK_MASK);
@@ -4267,17 +4552,22 @@ static void rtl_hw_start_8168bb(void __iomem *ioaddr, struct pci_dev *pdev)
 		(0x5 << MAX_READ_REQUEST_SHIFT) | PCI_EXP_DEVCTL_NOSNOOP_EN);
 }
 
-static void rtl_hw_start_8168bef(void __iomem *ioaddr, struct pci_dev *pdev)
+static void rtl_hw_start_8168bef(struct rtl8169_private *tp)
 {
-	rtl_hw_start_8168bb(ioaddr, pdev);
+	void __iomem *ioaddr = tp->mmio_addr;
+
+	rtl_hw_start_8168bb(tp);
 
 	RTL_W8(MaxTxPacketSize, TxPacketMax);
 
 	RTL_W8(Config4, RTL_R8(Config4) & ~(1 << 0));
 }
 
-static void __rtl_hw_start_8168cp(void __iomem *ioaddr, struct pci_dev *pdev)
+static void __rtl_hw_start_8168cp(struct rtl8169_private *tp)
 {
+	void __iomem *ioaddr = tp->mmio_addr;
+	struct pci_dev *pdev = tp->pci_dev;
+
 	RTL_W8(Config1, RTL_R8(Config1) | Speed_down);
 
 	RTL_W8(Config3, RTL_R8(Config3) & ~Beacon_en);
@@ -4289,8 +4579,9 @@ static void __rtl_hw_start_8168cp(void __iomem *ioaddr, struct pci_dev *pdev)
 	RTL_W16(CPlusCmd, RTL_R16(CPlusCmd) & ~R8168_CPCMD_QUIRK_MASK);
 }
 
-static void rtl_hw_start_8168cp_1(void __iomem *ioaddr, struct pci_dev *pdev)
+static void rtl_hw_start_8168cp_1(struct rtl8169_private *tp)
 {
+	void __iomem *ioaddr = tp->mmio_addr;
 	static const struct ephy_info e_info_8168cp[] = {
 		{ 0x01, 0,	0x0001 },
 		{ 0x02, 0x0800,	0x1000 },
@@ -4299,16 +4590,19 @@ static void rtl_hw_start_8168cp_1(void __iomem *ioaddr, struct pci_dev *pdev)
 		{ 0x07, 0,	0x2000 }
 	};
 
-	rtl_csi_access_enable_2(ioaddr);
+	rtl_csi_access_enable_2(tp);
 
 	rtl_ephy_init(ioaddr, e_info_8168cp, ARRAY_SIZE(e_info_8168cp));
 
-	__rtl_hw_start_8168cp(ioaddr, pdev);
+	__rtl_hw_start_8168cp(tp);
 }
 
-static void rtl_hw_start_8168cp_2(void __iomem *ioaddr, struct pci_dev *pdev)
+static void rtl_hw_start_8168cp_2(struct rtl8169_private *tp)
 {
-	rtl_csi_access_enable_2(ioaddr);
+	void __iomem *ioaddr = tp->mmio_addr;
+	struct pci_dev *pdev = tp->pci_dev;
+
+	rtl_csi_access_enable_2(tp);
 
 	RTL_W8(Config3, RTL_R8(Config3) & ~Beacon_en);
 
@@ -4317,9 +4611,12 @@ static void rtl_hw_start_8168cp_2(void __iomem *ioaddr, struct pci_dev *pdev)
 	RTL_W16(CPlusCmd, RTL_R16(CPlusCmd) & ~R8168_CPCMD_QUIRK_MASK);
 }
 
-static void rtl_hw_start_8168cp_3(void __iomem *ioaddr, struct pci_dev *pdev)
+static void rtl_hw_start_8168cp_3(struct rtl8169_private *tp)
 {
-	rtl_csi_access_enable_2(ioaddr);
+	void __iomem *ioaddr = tp->mmio_addr;
+	struct pci_dev *pdev = tp->pci_dev;
+
+	rtl_csi_access_enable_2(tp);
 
 	RTL_W8(Config3, RTL_R8(Config3) & ~Beacon_en);
 
@@ -4333,52 +4630,57 @@ static void rtl_hw_start_8168cp_3(void __iomem *ioaddr, struct pci_dev *pdev)
 	RTL_W16(CPlusCmd, RTL_R16(CPlusCmd) & ~R8168_CPCMD_QUIRK_MASK);
 }
 
-static void rtl_hw_start_8168c_1(void __iomem *ioaddr, struct pci_dev *pdev)
+static void rtl_hw_start_8168c_1(struct rtl8169_private *tp)
 {
+	void __iomem *ioaddr = tp->mmio_addr;
 	static const struct ephy_info e_info_8168c_1[] = {
 		{ 0x02, 0x0800,	0x1000 },
 		{ 0x03, 0,	0x0002 },
 		{ 0x06, 0x0080,	0x0000 }
 	};
 
-	rtl_csi_access_enable_2(ioaddr);
+	rtl_csi_access_enable_2(tp);
 
 	RTL_W8(DBG_REG, 0x06 | FIX_NAK_1 | FIX_NAK_2);
 
 	rtl_ephy_init(ioaddr, e_info_8168c_1, ARRAY_SIZE(e_info_8168c_1));
 
-	__rtl_hw_start_8168cp(ioaddr, pdev);
+	__rtl_hw_start_8168cp(tp);
 }
 
-static void rtl_hw_start_8168c_2(void __iomem *ioaddr, struct pci_dev *pdev)
+static void rtl_hw_start_8168c_2(struct rtl8169_private *tp)
 {
+	void __iomem *ioaddr = tp->mmio_addr;
 	static const struct ephy_info e_info_8168c_2[] = {
 		{ 0x01, 0,	0x0001 },
 		{ 0x03, 0x0400,	0x0220 }
 	};
 
-	rtl_csi_access_enable_2(ioaddr);
+	rtl_csi_access_enable_2(tp);
 
 	rtl_ephy_init(ioaddr, e_info_8168c_2, ARRAY_SIZE(e_info_8168c_2));
 
-	__rtl_hw_start_8168cp(ioaddr, pdev);
+	__rtl_hw_start_8168cp(tp);
 }
 
-static void rtl_hw_start_8168c_3(void __iomem *ioaddr, struct pci_dev *pdev)
+static void rtl_hw_start_8168c_3(struct rtl8169_private *tp)
 {
-	rtl_hw_start_8168c_2(ioaddr, pdev);
+	rtl_hw_start_8168c_2(tp);
 }
 
-static void rtl_hw_start_8168c_4(void __iomem *ioaddr, struct pci_dev *pdev)
+static void rtl_hw_start_8168c_4(struct rtl8169_private *tp)
 {
-	rtl_csi_access_enable_2(ioaddr);
+	rtl_csi_access_enable_2(tp);
 
-	__rtl_hw_start_8168cp(ioaddr, pdev);
+	__rtl_hw_start_8168cp(tp);
 }
 
-static void rtl_hw_start_8168d(void __iomem *ioaddr, struct pci_dev *pdev)
+static void rtl_hw_start_8168d(struct rtl8169_private *tp)
 {
-	rtl_csi_access_enable_2(ioaddr);
+	void __iomem *ioaddr = tp->mmio_addr;
+	struct pci_dev *pdev = tp->pci_dev;
+
+	rtl_csi_access_enable_2(tp);
 
 	rtl_disable_clock_request(pdev);
 
@@ -4389,9 +4691,12 @@ static void rtl_hw_start_8168d(void __iomem *ioaddr, struct pci_dev *pdev)
 	RTL_W16(CPlusCmd, RTL_R16(CPlusCmd) & ~R8168_CPCMD_QUIRK_MASK);
 }
 
-static void rtl_hw_start_8168dp(void __iomem *ioaddr, struct pci_dev *pdev)
+static void rtl_hw_start_8168dp(struct rtl8169_private *tp)
 {
-	rtl_csi_access_enable_1(ioaddr);
+	void __iomem *ioaddr = tp->mmio_addr;
+	struct pci_dev *pdev = tp->pci_dev;
+
+	rtl_csi_access_enable_1(tp);
 
 	rtl_tx_performance_tweak(pdev, 0x5 << MAX_READ_REQUEST_SHIFT);
 
@@ -4400,8 +4705,10 @@ static void rtl_hw_start_8168dp(void __iomem *ioaddr, struct pci_dev *pdev)
 	rtl_disable_clock_request(pdev);
 }
 
-static void rtl_hw_start_8168d_4(void __iomem *ioaddr, struct pci_dev *pdev)
+static void rtl_hw_start_8168d_4(struct rtl8169_private *tp)
 {
+	void __iomem *ioaddr = tp->mmio_addr;
+	struct pci_dev *pdev = tp->pci_dev;
 	static const struct ephy_info e_info_8168d_4[] = {
 		{ 0x0b, ~0,	0x48 },
 		{ 0x19, 0x20,	0x50 },
@@ -4409,7 +4716,7 @@ static void rtl_hw_start_8168d_4(void __iomem *ioaddr, struct pci_dev *pdev)
 	};
 	int i;
 
-	rtl_csi_access_enable_1(ioaddr);
+	rtl_csi_access_enable_1(tp);
 
 	rtl_tx_performance_tweak(pdev, 0x5 << MAX_READ_REQUEST_SHIFT);
 
@@ -4426,8 +4733,10 @@ static void rtl_hw_start_8168d_4(void __iomem *ioaddr, struct pci_dev *pdev)
 	rtl_enable_clock_request(pdev);
 }
 
-static void rtl_hw_start_8168e_1(void __iomem *ioaddr, struct pci_dev *pdev)
+static void rtl_hw_start_8168e_1(struct rtl8169_private *tp)
 {
+	void __iomem *ioaddr = tp->mmio_addr;
+	struct pci_dev *pdev = tp->pci_dev;
 	static const struct ephy_info e_info_8168e_1[] = {
 		{ 0x00, 0x0200,	0x0100 },
 		{ 0x00, 0x0000,	0x0004 },
@@ -4444,7 +4753,7 @@ static void rtl_hw_start_8168e_1(void __iomem *ioaddr, struct pci_dev *pdev)
 		{ 0x0a, 0x0000,	0x0040 }
 	};
 
-	rtl_csi_access_enable_2(ioaddr);
+	rtl_csi_access_enable_2(tp);
 
 	rtl_ephy_init(ioaddr, e_info_8168e_1, ARRAY_SIZE(e_info_8168e_1));
 
@@ -4461,14 +4770,16 @@ static void rtl_hw_start_8168e_1(void __iomem *ioaddr, struct pci_dev *pdev)
 	RTL_W8(Config5, RTL_R8(Config5) & ~Spi_en);
 }
 
-static void rtl_hw_start_8168e_2(void __iomem *ioaddr, struct pci_dev *pdev)
+static void rtl_hw_start_8168e_2(struct rtl8169_private *tp)
 {
+	void __iomem *ioaddr = tp->mmio_addr;
+	struct pci_dev *pdev = tp->pci_dev;
 	static const struct ephy_info e_info_8168e_2[] = {
 		{ 0x09, 0x0000,	0x0080 },
 		{ 0x19, 0x0000,	0x0224 }
 	};
 
-	rtl_csi_access_enable_1(ioaddr);
+	rtl_csi_access_enable_1(tp);
 
 	rtl_ephy_init(ioaddr, e_info_8168e_2, ARRAY_SIZE(e_info_8168e_2));
 
@@ -4499,18 +4810,12 @@ static void rtl_hw_start_8168e_2(void __iomem *ioaddr, struct pci_dev *pdev)
 	RTL_W8(Config5, RTL_R8(Config5) & ~Spi_en);
 }
 
-static void rtl_hw_start_8168f_1(void __iomem *ioaddr, struct pci_dev *pdev)
+static void rtl_hw_start_8168f(struct rtl8169_private *tp)
 {
-	static const struct ephy_info e_info_8168f_1[] = {
-		{ 0x06, 0x00c0,	0x0020 },
-		{ 0x08, 0x0001,	0x0002 },
-		{ 0x09, 0x0000,	0x0080 },
-		{ 0x19, 0x0000,	0x0224 }
-	};
-
-	rtl_csi_access_enable_1(ioaddr);
+	void __iomem *ioaddr = tp->mmio_addr;
+	struct pci_dev *pdev = tp->pci_dev;
 
-	rtl_ephy_init(ioaddr, e_info_8168f_1, ARRAY_SIZE(e_info_8168f_1));
+	rtl_csi_access_enable_2(tp);
 
 	rtl_tx_performance_tweak(pdev, 0x5 << MAX_READ_REQUEST_SHIFT);
 
@@ -4524,8 +4829,6 @@ static void rtl_hw_start_8168f_1(void __iomem *ioaddr, struct pci_dev *pdev)
 	rtl_w1w0_eri(ioaddr, 0x1d0, ERIAR_MASK_0001, 0x10, 0x00, ERIAR_EXGMAC);
 	rtl_eri_write(ioaddr, 0xcc, ERIAR_MASK_1111, 0x00000050, ERIAR_EXGMAC);
 	rtl_eri_write(ioaddr, 0xd0, ERIAR_MASK_1111, 0x00000060, ERIAR_EXGMAC);
-	rtl_w1w0_eri(ioaddr, 0x0d4, ERIAR_MASK_0011, 0x0c00, 0xff00,
-		     ERIAR_EXGMAC);
 
 	RTL_W8(MaxTxPacketSize, EarlySize);
 
@@ -4533,20 +4836,54 @@ static void rtl_hw_start_8168f_1(void __iomem *ioaddr, struct pci_dev *pdev)
 
 	RTL_W32(TxConfig, RTL_R32(TxConfig) | TXCFG_AUTO_FIFO);
 	RTL_W8(MCU, RTL_R8(MCU) & ~NOW_IS_OOB);
+	RTL_W8(DLLPR, RTL_R8(DLLPR) | PFM_EN);
+	RTL_W32(MISC, RTL_R32(MISC) | PWM_EN);
+	RTL_W8(Config5, RTL_R8(Config5) & ~Spi_en);
+}
+
+static void rtl_hw_start_8168f_1(struct rtl8169_private *tp)
+{
+	void __iomem *ioaddr = tp->mmio_addr;
+	static const struct ephy_info e_info_8168f_1[] = {
+		{ 0x06, 0x00c0,	0x0020 },
+		{ 0x08, 0x0001,	0x0002 },
+		{ 0x09, 0x0000,	0x0080 },
+		{ 0x19, 0x0000,	0x0224 }
+	};
+
+	rtl_hw_start_8168f(tp);
+
+	rtl_ephy_init(ioaddr, e_info_8168f_1, ARRAY_SIZE(e_info_8168f_1));
+
+	rtl_w1w0_eri(ioaddr, 0x0d4, ERIAR_MASK_0011, 0x0c00, 0xff00,
+		     ERIAR_EXGMAC);
 
 	/* Adjust EEE LED frequency */
 	RTL_W8(EEE_LED, RTL_R8(EEE_LED) & ~0x07);
+}
 
-	RTL_W8(DLLPR, RTL_R8(DLLPR) | PFM_EN);
-	RTL_W32(MISC, RTL_R32(MISC) | PWM_EN);
-	RTL_W8(Config5, RTL_R8(Config5) & ~Spi_en);
+static void rtl_hw_start_8411(struct rtl8169_private *tp)
+{
+	void __iomem *ioaddr = tp->mmio_addr;
+	static const struct ephy_info e_info_8168f_1[] = {
+		{ 0x06, 0x00c0,	0x0020 },
+		{ 0x0f, 0xffff,	0x5200 },
+		{ 0x1e, 0x0000,	0x4000 },
+		{ 0x19, 0x0000,	0x0224 }
+	};
+
+	rtl_hw_start_8168f(tp);
+
+	rtl_ephy_init(ioaddr, e_info_8168f_1, ARRAY_SIZE(e_info_8168f_1));
+
+	rtl_w1w0_eri(ioaddr, 0x0d4, ERIAR_MASK_0011, 0x0c00, 0x0000,
+		     ERIAR_EXGMAC);
 }
 
 static void rtl_hw_start_8168(struct net_device *dev)
 {
 	struct rtl8169_private *tp = netdev_priv(dev);
 	void __iomem *ioaddr = tp->mmio_addr;
-	struct pci_dev *pdev = tp->pci_dev;
 
 	RTL_W8(Cfg9346, Cfg9346_Unlock);
 
@@ -4577,67 +4914,71 @@ static void rtl_hw_start_8168(struct net_device *dev)
 
 	switch (tp->mac_version) {
 	case RTL_GIGA_MAC_VER_11:
-		rtl_hw_start_8168bb(ioaddr, pdev);
+		rtl_hw_start_8168bb(tp);
 		break;
 
 	case RTL_GIGA_MAC_VER_12:
 	case RTL_GIGA_MAC_VER_17:
-		rtl_hw_start_8168bef(ioaddr, pdev);
+		rtl_hw_start_8168bef(tp);
 		break;
 
 	case RTL_GIGA_MAC_VER_18:
-		rtl_hw_start_8168cp_1(ioaddr, pdev);
+		rtl_hw_start_8168cp_1(tp);
 		break;
 
 	case RTL_GIGA_MAC_VER_19:
-		rtl_hw_start_8168c_1(ioaddr, pdev);
+		rtl_hw_start_8168c_1(tp);
 		break;
 
 	case RTL_GIGA_MAC_VER_20:
-		rtl_hw_start_8168c_2(ioaddr, pdev);
+		rtl_hw_start_8168c_2(tp);
 		break;
 
 	case RTL_GIGA_MAC_VER_21:
-		rtl_hw_start_8168c_3(ioaddr, pdev);
+		rtl_hw_start_8168c_3(tp);
 		break;
 
 	case RTL_GIGA_MAC_VER_22:
-		rtl_hw_start_8168c_4(ioaddr, pdev);
+		rtl_hw_start_8168c_4(tp);
 		break;
 
 	case RTL_GIGA_MAC_VER_23:
-		rtl_hw_start_8168cp_2(ioaddr, pdev);
+		rtl_hw_start_8168cp_2(tp);
 		break;
 
 	case RTL_GIGA_MAC_VER_24:
-		rtl_hw_start_8168cp_3(ioaddr, pdev);
+		rtl_hw_start_8168cp_3(tp);
 		break;
 
 	case RTL_GIGA_MAC_VER_25:
 	case RTL_GIGA_MAC_VER_26:
 	case RTL_GIGA_MAC_VER_27:
-		rtl_hw_start_8168d(ioaddr, pdev);
+		rtl_hw_start_8168d(tp);
 		break;
 
 	case RTL_GIGA_MAC_VER_28:
-		rtl_hw_start_8168d_4(ioaddr, pdev);
+		rtl_hw_start_8168d_4(tp);
 		break;
 
 	case RTL_GIGA_MAC_VER_31:
-		rtl_hw_start_8168dp(ioaddr, pdev);
+		rtl_hw_start_8168dp(tp);
 		break;
 
 	case RTL_GIGA_MAC_VER_32:
 	case RTL_GIGA_MAC_VER_33:
-		rtl_hw_start_8168e_1(ioaddr, pdev);
+		rtl_hw_start_8168e_1(tp);
 		break;
 	case RTL_GIGA_MAC_VER_34:
-		rtl_hw_start_8168e_2(ioaddr, pdev);
+		rtl_hw_start_8168e_2(tp);
 		break;
 
 	case RTL_GIGA_MAC_VER_35:
 	case RTL_GIGA_MAC_VER_36:
-		rtl_hw_start_8168f_1(ioaddr, pdev);
+		rtl_hw_start_8168f_1(tp);
+		break;
+
+	case RTL_GIGA_MAC_VER_38:
+		rtl_hw_start_8411(tp);
 		break;
 
 	default:
@@ -4664,8 +5005,10 @@ static void rtl_hw_start_8168(struct net_device *dev)
 	PktCntrDisable | \
 	Mac_dbgo_sel)
 
-static void rtl_hw_start_8102e_1(void __iomem *ioaddr, struct pci_dev *pdev)
+static void rtl_hw_start_8102e_1(struct rtl8169_private *tp)
 {
+	void __iomem *ioaddr = tp->mmio_addr;
+	struct pci_dev *pdev = tp->pci_dev;
 	static const struct ephy_info e_info_8102e_1[] = {
 		{ 0x01,	0, 0x6e65 },
 		{ 0x02,	0, 0x091f },
@@ -4678,7 +5021,7 @@ static void rtl_hw_start_8102e_1(void __iomem *ioaddr, struct pci_dev *pdev)
 	};
 	u8 cfg1;
 
-	rtl_csi_access_enable_2(ioaddr);
+	rtl_csi_access_enable_2(tp);
 
 	RTL_W8(DBG_REG, FIX_NAK_1);
 
@@ -4695,9 +5038,12 @@ static void rtl_hw_start_8102e_1(void __iomem *ioaddr, struct pci_dev *pdev)
 	rtl_ephy_init(ioaddr, e_info_8102e_1, ARRAY_SIZE(e_info_8102e_1));
 }
 
-static void rtl_hw_start_8102e_2(void __iomem *ioaddr, struct pci_dev *pdev)
+static void rtl_hw_start_8102e_2(struct rtl8169_private *tp)
 {
-	rtl_csi_access_enable_2(ioaddr);
+	void __iomem *ioaddr = tp->mmio_addr;
+	struct pci_dev *pdev = tp->pci_dev;
+
+	rtl_csi_access_enable_2(tp);
 
 	rtl_tx_performance_tweak(pdev, 0x5 << MAX_READ_REQUEST_SHIFT);
 
@@ -4705,15 +5051,16 @@ static void rtl_hw_start_8102e_2(void __iomem *ioaddr, struct pci_dev *pdev)
 	RTL_W8(Config3, RTL_R8(Config3) & ~Beacon_en);
 }
 
-static void rtl_hw_start_8102e_3(void __iomem *ioaddr, struct pci_dev *pdev)
+static void rtl_hw_start_8102e_3(struct rtl8169_private *tp)
 {
-	rtl_hw_start_8102e_2(ioaddr, pdev);
+	rtl_hw_start_8102e_2(tp);
 
-	rtl_ephy_write(ioaddr, 0x03, 0xc2f9);
+	rtl_ephy_write(tp->mmio_addr, 0x03, 0xc2f9);
 }
 
-static void rtl_hw_start_8105e_1(void __iomem *ioaddr, struct pci_dev *pdev)
+static void rtl_hw_start_8105e_1(struct rtl8169_private *tp)
 {
+	void __iomem *ioaddr = tp->mmio_addr;
 	static const struct ephy_info e_info_8105e_1[] = {
 		{ 0x07,	0, 0x4000 },
 		{ 0x19,	0, 0x0200 },
@@ -4737,12 +5084,44 @@ static void rtl_hw_start_8105e_1(void __iomem *ioaddr, struct pci_dev *pdev)
 	rtl_ephy_init(ioaddr, e_info_8105e_1, ARRAY_SIZE(e_info_8105e_1));
 }
 
-static void rtl_hw_start_8105e_2(void __iomem *ioaddr, struct pci_dev *pdev)
+static void rtl_hw_start_8105e_2(struct rtl8169_private *tp)
 {
-	rtl_hw_start_8105e_1(ioaddr, pdev);
+	void __iomem *ioaddr = tp->mmio_addr;
+
+	rtl_hw_start_8105e_1(tp);
 	rtl_ephy_write(ioaddr, 0x1e, rtl_ephy_read(ioaddr, 0x1e) | 0x8000);
 }
 
+static void rtl_hw_start_8402(struct rtl8169_private *tp)
+{
+	void __iomem *ioaddr = tp->mmio_addr;
+	static const struct ephy_info e_info_8402[] = {
+		{ 0x19,	0xffff, 0xff64 },
+		{ 0x1e,	0, 0x4000 }
+	};
+
+	rtl_csi_access_enable_2(tp);
+
+	/* Force LAN exit from ASPM if Rx/Tx are not idle */
+	RTL_W32(FuncEvent, RTL_R32(FuncEvent) | 0x002800);
+
+	RTL_W32(TxConfig, RTL_R32(TxConfig) | TXCFG_AUTO_FIFO);
+	RTL_W8(MCU, RTL_R8(MCU) & ~NOW_IS_OOB);
+
+	rtl_ephy_init(ioaddr, e_info_8402, ARRAY_SIZE(e_info_8402));
+
+	rtl_tx_performance_tweak(tp->pci_dev, 0x5 << MAX_READ_REQUEST_SHIFT);
+
+	rtl_eri_write(ioaddr, 0xc8, ERIAR_MASK_1111, 0x00000002, ERIAR_EXGMAC);
+	rtl_eri_write(ioaddr, 0xe8, ERIAR_MASK_1111, 0x00000006, ERIAR_EXGMAC);
+	rtl_w1w0_eri(ioaddr, 0xdc, ERIAR_MASK_0001, 0x00, 0x01, ERIAR_EXGMAC);
+	rtl_w1w0_eri(ioaddr, 0xdc, ERIAR_MASK_0001, 0x01, 0x00, ERIAR_EXGMAC);
+	rtl_eri_write(ioaddr, 0xc0, ERIAR_MASK_0011, 0x0000, ERIAR_EXGMAC);
+	rtl_eri_write(ioaddr, 0xb8, ERIAR_MASK_0011, 0x0000, ERIAR_EXGMAC);
+	rtl_w1w0_eri(ioaddr, 0x0d4, ERIAR_MASK_0011, 0x0e00, 0xff00,
+		     ERIAR_EXGMAC);
+}
+
 static void rtl_hw_start_8101(struct net_device *dev)
 {
 	struct rtl8169_private *tp = netdev_priv(dev);
@@ -4766,22 +5145,26 @@ static void rtl_hw_start_8101(struct net_device *dev)
 
 	switch (tp->mac_version) {
 	case RTL_GIGA_MAC_VER_07:
-		rtl_hw_start_8102e_1(ioaddr, pdev);
+		rtl_hw_start_8102e_1(tp);
 		break;
 
 	case RTL_GIGA_MAC_VER_08:
-		rtl_hw_start_8102e_3(ioaddr, pdev);
+		rtl_hw_start_8102e_3(tp);
 		break;
 
 	case RTL_GIGA_MAC_VER_09:
-		rtl_hw_start_8102e_2(ioaddr, pdev);
+		rtl_hw_start_8102e_2(tp);
 		break;
 
 	case RTL_GIGA_MAC_VER_29:
-		rtl_hw_start_8105e_1(ioaddr, pdev);
+		rtl_hw_start_8105e_1(tp);
 		break;
 	case RTL_GIGA_MAC_VER_30:
-		rtl_hw_start_8105e_2(ioaddr, pdev);
+		rtl_hw_start_8105e_2(tp);
+		break;
+
+	case RTL_GIGA_MAC_VER_37:
+		rtl_hw_start_8402(tp);
 		break;
 	}
 
@@ -5115,7 +5498,7 @@ static netdev_tx_t rtl8169_start_xmit(struct sk_buff *skb,
 	u32 opts[2];
 	int frags;
 
-	if (unlikely(TX_BUFFS_AVAIL(tp) < skb_shinfo(skb)->nr_frags)) {
+	if (unlikely(!TX_FRAGS_READY_FOR(tp, skb_shinfo(skb)->nr_frags))) {
 		netif_err(tp, drv, dev, "BUG! Tx Ring full when queue awake!\n");
 		goto err_stop_0;
 	}
@@ -5169,7 +5552,7 @@ static netdev_tx_t rtl8169_start_xmit(struct sk_buff *skb,
 
 	mmiowb();
 
-	if (TX_BUFFS_AVAIL(tp) < MAX_SKB_FRAGS) {
+	if (!TX_FRAGS_READY_FOR(tp, MAX_SKB_FRAGS)) {
 		/* Avoid wrongly optimistic queue wake-up: rtl_tx thread must
 		 * not miss a ring update when it notices a stopped queue.
 		 */
@@ -5183,7 +5566,7 @@ static netdev_tx_t rtl8169_start_xmit(struct sk_buff *skb,
 		 * can't.
 		 */
 		smp_mb();
-		if (TX_BUFFS_AVAIL(tp) >= MAX_SKB_FRAGS)
+		if (TX_FRAGS_READY_FOR(tp, MAX_SKB_FRAGS))
 			netif_wake_queue(dev);
 	}
 
@@ -5306,7 +5689,7 @@ static void rtl_tx(struct net_device *dev, struct rtl8169_private *tp)
 		 */
 		smp_mb();
 		if (netif_queue_stopped(dev) &&
-		    (TX_BUFFS_AVAIL(tp) >= MAX_SKB_FRAGS)) {
+		    TX_FRAGS_READY_FOR(tp, MAX_SKB_FRAGS)) {
 			netif_wake_queue(dev);
 		}
 		/*
@@ -5668,7 +6051,7 @@ static int rtl_open(struct net_device *dev)
 	pm_runtime_get_sync(&pdev->dev);
 
 	/*
-	 * Rx and Tx desscriptors needs 256 bytes alignment.
+	 * Rx and Tx descriptors needs 256 bytes alignment.
 	 * dma_alloc_coherent provides more.
 	 */
 	tp->TxDescArray = dma_alloc_coherent(&pdev->dev, R8169_TX_RING_BYTES,
@@ -6178,6 +6561,7 @@ rtl_init_one(struct pci_dev *pdev, const struct pci_device_id *ent)
 	rtl_init_mdio_ops(tp);
 	rtl_init_pll_power_ops(tp);
 	rtl_init_jumbo_ops(tp);
+	rtl_init_csi_ops(tp);
 
 	rtl8169_print_mac_version(tp);
 
diff --git a/drivers/net/ethernet/renesas/Kconfig b/drivers/net/ethernet/renesas/Kconfig
index 3fb2355af37..46df3a04030 100644
--- a/drivers/net/ethernet/renesas/Kconfig
+++ b/drivers/net/ethernet/renesas/Kconfig
@@ -4,11 +4,11 @@
 
 config SH_ETH
 	tristate "Renesas SuperH Ethernet support"
-	depends on SUPERH && \
+	depends on (SUPERH || ARCH_SHMOBILE) && \
 		(CPU_SUBTYPE_SH7710 || CPU_SUBTYPE_SH7712 || \
 		 CPU_SUBTYPE_SH7763 || CPU_SUBTYPE_SH7619 || \
 		 CPU_SUBTYPE_SH7724 || CPU_SUBTYPE_SH7734 || \
-		 CPU_SUBTYPE_SH7757)
+		 CPU_SUBTYPE_SH7757 || ARCH_R8A7740)
 	select CRC32
 	select NET_CORE
 	select MII
@@ -17,4 +17,5 @@ config SH_ETH
 	---help---
 	  Renesas SuperH Ethernet device driver.
 	  This driver supporting CPUs are:
-		- SH7619, SH7710, SH7712, SH7724, SH7734, SH7763 and SH7757.
+		- SH7619, SH7710, SH7712, SH7724, SH7734, SH7763, SH7757,
+		  and R8A7740.
diff --git a/drivers/net/ethernet/renesas/sh_eth.c b/drivers/net/ethernet/renesas/sh_eth.c
index d63e09b29a9..be3c2217916 100644
--- a/drivers/net/ethernet/renesas/sh_eth.c
+++ b/drivers/net/ethernet/renesas/sh_eth.c
@@ -386,6 +386,114 @@ static void sh_eth_reset_hw_crc(struct net_device *ndev)
 		sh_eth_write(ndev, 0x0, CSMR);
 }
 
+#elif defined(CONFIG_ARCH_R8A7740)
+#define SH_ETH_HAS_TSU	1
+static void sh_eth_chip_reset(struct net_device *ndev)
+{
+	struct sh_eth_private *mdp = netdev_priv(ndev);
+	unsigned long mii;
+
+	/* reset device */
+	sh_eth_tsu_write(mdp, ARSTR_ARSTR, ARSTR);
+	mdelay(1);
+
+	switch (mdp->phy_interface) {
+	case PHY_INTERFACE_MODE_GMII:
+		mii = 2;
+		break;
+	case PHY_INTERFACE_MODE_MII:
+		mii = 1;
+		break;
+	case PHY_INTERFACE_MODE_RMII:
+	default:
+		mii = 0;
+		break;
+	}
+	sh_eth_write(ndev, mii, RMII_MII);
+}
+
+static void sh_eth_reset(struct net_device *ndev)
+{
+	int cnt = 100;
+
+	sh_eth_write(ndev, EDSR_ENALL, EDSR);
+	sh_eth_write(ndev, sh_eth_read(ndev, EDMR) | EDMR_SRST_GETHER, EDMR);
+	while (cnt > 0) {
+		if (!(sh_eth_read(ndev, EDMR) & 0x3))
+			break;
+		mdelay(1);
+		cnt--;
+	}
+	if (cnt == 0)
+		printk(KERN_ERR "Device reset fail\n");
+
+	/* Table Init */
+	sh_eth_write(ndev, 0x0, TDLAR);
+	sh_eth_write(ndev, 0x0, TDFAR);
+	sh_eth_write(ndev, 0x0, TDFXR);
+	sh_eth_write(ndev, 0x0, TDFFR);
+	sh_eth_write(ndev, 0x0, RDLAR);
+	sh_eth_write(ndev, 0x0, RDFAR);
+	sh_eth_write(ndev, 0x0, RDFXR);
+	sh_eth_write(ndev, 0x0, RDFFR);
+}
+
+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, GECMR_10, GECMR);
+		break;
+	case 100:/* 100BASE */
+		sh_eth_write(ndev, GECMR_100, GECMR);
+		break;
+	case 1000: /* 1000BASE */
+		sh_eth_write(ndev, GECMR_1000, GECMR);
+		break;
+	default:
+		break;
+	}
+}
+
+/* R8A7740 */
+static struct sh_eth_cpu_data sh_eth_my_cpu_data = {
+	.chip_reset	= sh_eth_chip_reset,
+	.set_duplex	= sh_eth_set_duplex,
+	.set_rate	= sh_eth_set_rate,
+
+	.ecsr_value	= ECSR_ICD | ECSR_MPD,
+	.ecsipr_value	= ECSIPR_LCHNGIP | ECSIPR_ICDIP | ECSIPR_MPDIP,
+	.eesipr_value	= DMAC_M_RFRMER | DMAC_M_ECI | 0x003fffff,
+
+	.tx_check	= EESR_TC1 | EESR_FTC,
+	.eesr_err_check	= EESR_TWB1 | EESR_TWB | EESR_TABT | EESR_RABT | \
+			  EESR_RDE | EESR_RFRMER | EESR_TFE | EESR_TDE | \
+			  EESR_ECI,
+	.tx_error_check	= EESR_TWB1 | EESR_TWB | EESR_TABT | EESR_TDE | \
+			  EESR_TFE,
+
+	.apr		= 1,
+	.mpr		= 1,
+	.tpauser	= 1,
+	.bculr		= 1,
+	.hw_swap	= 1,
+	.no_trimd	= 1,
+	.no_ade		= 1,
+	.tsu		= 1,
+};
+
 #elif defined(CONFIG_CPU_SUBTYPE_SH7619)
 #define SH_ETH_RESET_DEFAULT	1
 static struct sh_eth_cpu_data sh_eth_my_cpu_data = {
@@ -443,7 +551,7 @@ static void sh_eth_reset(struct net_device *ndev)
 }
 #endif
 
-#if defined(CONFIG_CPU_SH4)
+#if defined(CONFIG_CPU_SH4) || defined(CONFIG_ARCH_SHMOBILE)
 static void sh_eth_set_receive_align(struct sk_buff *skb)
 {
 	int reserve;
@@ -919,6 +1027,10 @@ static int sh_eth_rx(struct net_device *ndev)
 		desc_status = edmac_to_cpu(mdp, rxdesc->status);
 		pkt_len = rxdesc->frame_length;
 
+#if defined(CONFIG_ARCH_R8A7740)
+		desc_status >>= 16;
+#endif
+
 		if (--boguscnt < 0)
 			break;
 
diff --git a/drivers/net/ethernet/renesas/sh_eth.h b/drivers/net/ethernet/renesas/sh_eth.h
index 0fa14afce23..57b8e1fc5d1 100644
--- a/drivers/net/ethernet/renesas/sh_eth.h
+++ b/drivers/net/ethernet/renesas/sh_eth.h
@@ -372,7 +372,7 @@ static const u16 sh_eth_offset_fast_sh3_sh2[SH_ETH_MAX_REGISTER_OFFSET] = {
 };
 
 /* Driver's parameters */
-#if defined(CONFIG_CPU_SH4)
+#if defined(CONFIG_CPU_SH4) || defined(CONFIG_ARCH_SHMOBILE)
 #define SH4_SKB_RX_ALIGN	32
 #else
 #define SH2_SH3_SKB_RX_ALIGN	2
@@ -381,7 +381,8 @@ static const u16 sh_eth_offset_fast_sh3_sh2[SH_ETH_MAX_REGISTER_OFFSET] = {
 /*
  * Register's bits
  */
-#if defined(CONFIG_CPU_SUBTYPE_SH7734) || defined(CONFIG_CPU_SUBTYPE_SH7763)
+#if defined(CONFIG_CPU_SUBTYPE_SH7734) || defined(CONFIG_CPU_SUBTYPE_SH7763) ||\
+    defined(CONFIG_ARCH_R8A7740)
 /* EDSR */
 enum EDSR_BIT {
 	EDSR_ENT = 0x01, EDSR_ENR = 0x02,
diff --git a/drivers/net/ethernet/s6gmac.c b/drivers/net/ethernet/s6gmac.c
index 1895605abb3..2ed3ab4b3c2 100644
--- a/drivers/net/ethernet/s6gmac.c
+++ b/drivers/net/ethernet/s6gmac.c
@@ -1,7 +1,7 @@
 /*
  * Ethernet driver for S6105 on chip network device
  * (c)2008 emlix GmbH http://www.emlix.com
- * Authors:	Oskar Schirmer <os@emlix.com>
+ * Authors:	Oskar Schirmer <oskar@scara.com>
  *		Daniel Gloeckner <dg@emlix.com>
  *
  * This program is free software; you can redistribute it and/or
@@ -937,7 +937,7 @@ static struct net_device_stats *s6gmac_stats(struct net_device *dev)
 	do {
 		unsigned long flags;
 		spin_lock_irqsave(&pd->lock, flags);
-		for (i = 0; i < sizeof(pd->stats) / sizeof(unsigned long); i++)
+		for (i = 0; i < ARRAY_SIZE(pd->stats); i++)
 			pd->stats[i] =
 				pd->carry[i] << (S6_GMAC_STAT_SIZE_MIN - 1);
 		s6gmac_stats_collect(pd, &statinf[0][0]);
@@ -1070,4 +1070,4 @@ module_exit(s6gmac_exit);
 
 MODULE_LICENSE("GPL");
 MODULE_DESCRIPTION("S6105 on chip Ethernet driver");
-MODULE_AUTHOR("Oskar Schirmer <os@emlix.com>");
+MODULE_AUTHOR("Oskar Schirmer <oskar@scara.com>");
diff --git a/drivers/net/ethernet/sfc/efx.c b/drivers/net/ethernet/sfc/efx.c
index 3cbfbffe3f0..b95f2e1b33f 100644
--- a/drivers/net/ethernet/sfc/efx.c
+++ b/drivers/net/ethernet/sfc/efx.c
@@ -656,25 +656,30 @@ static void efx_stop_datapath(struct efx_nic *efx)
 	struct efx_channel *channel;
 	struct efx_tx_queue *tx_queue;
 	struct efx_rx_queue *rx_queue;
+	struct pci_dev *dev = efx->pci_dev;
 	int rc;
 
 	EFX_ASSERT_RESET_SERIALISED(efx);
 	BUG_ON(efx->port_enabled);
 
-	rc = efx_nic_flush_queues(efx);
-	if (rc && EFX_WORKAROUND_7803(efx)) {
-		/* Schedule a reset to recover from the flush failure. The
-		 * descriptor caches reference memory we're about to free,
-		 * but falcon_reconfigure_mac_wrapper() won't reconnect
-		 * the MACs because of the pending reset. */
-		netif_err(efx, drv, efx->net_dev,
-			  "Resetting to recover from flush failure\n");
-		efx_schedule_reset(efx, RESET_TYPE_ALL);
-	} else if (rc) {
-		netif_err(efx, drv, efx->net_dev, "failed to flush queues\n");
-	} else {
-		netif_dbg(efx, drv, efx->net_dev,
-			  "successfully flushed all queues\n");
+	/* Only perform flush if dma is enabled */
+	if (dev->is_busmaster) {
+		rc = efx_nic_flush_queues(efx);
+
+		if (rc && EFX_WORKAROUND_7803(efx)) {
+			/* Schedule a reset to recover from the flush failure. The
+			 * descriptor caches reference memory we're about to free,
+			 * but falcon_reconfigure_mac_wrapper() won't reconnect
+			 * the MACs because of the pending reset. */
+			netif_err(efx, drv, efx->net_dev,
+				  "Resetting to recover from flush failure\n");
+			efx_schedule_reset(efx, RESET_TYPE_ALL);
+		} else if (rc) {
+			netif_err(efx, drv, efx->net_dev, "failed to flush queues\n");
+		} else {
+			netif_dbg(efx, drv, efx->net_dev,
+				  "successfully flushed all queues\n");
+		}
 	}
 
 	efx_for_each_channel(channel, efx) {
@@ -1349,7 +1354,7 @@ static int efx_probe_interrupts(struct efx_nic *efx)
 	}
 
 	/* RSS might be usable on VFs even if it is disabled on the PF */
-	efx->rss_spread = (efx->n_rx_channels > 1 ?
+	efx->rss_spread = ((efx->n_rx_channels > 1 || !efx_sriov_wanted(efx)) ?
 			   efx->n_rx_channels : efx_vf_size(efx));
 
 	return 0;
@@ -2492,8 +2497,8 @@ static void efx_pci_remove(struct pci_dev *pci_dev)
 	efx_fini_io(efx);
 	netif_dbg(efx, drv, efx->net_dev, "shutdown successful\n");
 
-	pci_set_drvdata(pci_dev, NULL);
 	efx_fini_struct(efx);
+	pci_set_drvdata(pci_dev, NULL);
 	free_netdev(efx->net_dev);
 };
 
@@ -2695,6 +2700,7 @@ static int __devinit efx_pci_probe(struct pci_dev *pci_dev,
  fail2:
 	efx_fini_struct(efx);
  fail1:
+	pci_set_drvdata(pci_dev, NULL);
 	WARN_ON(rc > 0);
 	netif_dbg(efx, drv, efx->net_dev, "initialisation failed. rc=%d\n", rc);
 	free_netdev(net_dev);
diff --git a/drivers/net/ethernet/sfc/ethtool.c b/drivers/net/ethernet/sfc/ethtool.c
index f22f45f515a..03ded364c8d 100644
--- a/drivers/net/ethernet/sfc/ethtool.c
+++ b/drivers/net/ethernet/sfc/ethtool.c
@@ -1023,7 +1023,7 @@ static int efx_ethtool_set_class_rule(struct efx_nic *efx,
 			return -EINVAL;
 
 		/* Is it a default UC or MC filter? */
-		if (!compare_ether_addr(mac_mask->h_dest, mac_addr_mc_mask) &&
+		if (ether_addr_equal(mac_mask->h_dest, mac_addr_mc_mask) &&
 		    vlan_tag_mask == 0) {
 			if (is_multicast_ether_addr(mac_entry->h_dest))
 				rc = efx_filter_set_mc_def(&spec);
@@ -1108,6 +1108,39 @@ static int efx_ethtool_set_rxfh_indir(struct net_device *net_dev,
 	return 0;
 }
 
+static int efx_ethtool_get_module_eeprom(struct net_device *net_dev,
+					 struct ethtool_eeprom *ee,
+					 u8 *data)
+{
+	struct efx_nic *efx = netdev_priv(net_dev);
+	int ret;
+
+	if (!efx->phy_op || !efx->phy_op->get_module_eeprom)
+		return -EOPNOTSUPP;
+
+	mutex_lock(&efx->mac_lock);
+	ret = efx->phy_op->get_module_eeprom(efx, ee, data);
+	mutex_unlock(&efx->mac_lock);
+
+	return ret;
+}
+
+static int efx_ethtool_get_module_info(struct net_device *net_dev,
+				       struct ethtool_modinfo *modinfo)
+{
+	struct efx_nic *efx = netdev_priv(net_dev);
+	int ret;
+
+	if (!efx->phy_op || !efx->phy_op->get_module_info)
+		return -EOPNOTSUPP;
+
+	mutex_lock(&efx->mac_lock);
+	ret = efx->phy_op->get_module_info(efx, modinfo);
+	mutex_unlock(&efx->mac_lock);
+
+	return ret;
+}
+
 const struct ethtool_ops efx_ethtool_ops = {
 	.get_settings		= efx_ethtool_get_settings,
 	.set_settings		= efx_ethtool_set_settings,
@@ -1137,4 +1170,6 @@ const struct ethtool_ops efx_ethtool_ops = {
 	.get_rxfh_indir_size	= efx_ethtool_get_rxfh_indir_size,
 	.get_rxfh_indir		= efx_ethtool_get_rxfh_indir,
 	.set_rxfh_indir		= efx_ethtool_set_rxfh_indir,
+	.get_module_info	= efx_ethtool_get_module_info,
+	.get_module_eeprom	= efx_ethtool_get_module_eeprom,
 };
diff --git a/drivers/net/ethernet/sfc/mcdi_phy.c b/drivers/net/ethernet/sfc/mcdi_phy.c
index 7bcad899a93..13cb40fe90c 100644
--- a/drivers/net/ethernet/sfc/mcdi_phy.c
+++ b/drivers/net/ethernet/sfc/mcdi_phy.c
@@ -739,6 +739,80 @@ static const char *efx_mcdi_phy_test_name(struct efx_nic *efx,
 	return NULL;
 }
 
+#define SFP_PAGE_SIZE	128
+#define SFP_NUM_PAGES	2
+static int efx_mcdi_phy_get_module_eeprom(struct efx_nic *efx,
+					  struct ethtool_eeprom *ee, u8 *data)
+{
+	u8 outbuf[MC_CMD_GET_PHY_MEDIA_INFO_OUT_LENMAX];
+	u8 inbuf[MC_CMD_GET_PHY_MEDIA_INFO_IN_LEN];
+	size_t outlen;
+	int rc;
+	unsigned int payload_len;
+	unsigned int space_remaining = ee->len;
+	unsigned int page;
+	unsigned int page_off;
+	unsigned int to_copy;
+	u8 *user_data = data;
+
+	BUILD_BUG_ON(SFP_PAGE_SIZE * SFP_NUM_PAGES != ETH_MODULE_SFF_8079_LEN);
+
+	page_off = ee->offset % SFP_PAGE_SIZE;
+	page = ee->offset / SFP_PAGE_SIZE;
+
+	while (space_remaining && (page < SFP_NUM_PAGES)) {
+		MCDI_SET_DWORD(inbuf, GET_PHY_MEDIA_INFO_IN_PAGE, page);
+
+		rc = efx_mcdi_rpc(efx, MC_CMD_GET_PHY_MEDIA_INFO,
+				  inbuf, sizeof(inbuf),
+				  outbuf, sizeof(outbuf),
+				  &outlen);
+		if (rc)
+			return rc;
+
+		if (outlen < (MC_CMD_GET_PHY_MEDIA_INFO_OUT_DATA_OFST +
+			      SFP_PAGE_SIZE))
+			return -EIO;
+
+		payload_len = MCDI_DWORD(outbuf,
+					 GET_PHY_MEDIA_INFO_OUT_DATALEN);
+		if (payload_len != SFP_PAGE_SIZE)
+			return -EIO;
+
+		/* Copy as much as we can into data */
+		payload_len -= page_off;
+		to_copy = (space_remaining < payload_len) ?
+			space_remaining : payload_len;
+
+		memcpy(user_data,
+		       outbuf + page_off +
+		       MC_CMD_GET_PHY_MEDIA_INFO_OUT_DATA_OFST,
+		       to_copy);
+
+		space_remaining -= to_copy;
+		user_data += to_copy;
+		page_off = 0;
+		page++;
+	}
+
+	return 0;
+}
+
+static int efx_mcdi_phy_get_module_info(struct efx_nic *efx,
+					struct ethtool_modinfo *modinfo)
+{
+	struct efx_mcdi_phy_data *phy_cfg = efx->phy_data;
+
+	switch (phy_cfg->media) {
+	case MC_CMD_MEDIA_SFP_PLUS:
+		modinfo->type = ETH_MODULE_SFF_8079;
+		modinfo->eeprom_len = ETH_MODULE_SFF_8079_LEN;
+		return 0;
+	default:
+		return -EOPNOTSUPP;
+	}
+}
+
 const struct efx_phy_operations efx_mcdi_phy_ops = {
 	.probe		= efx_mcdi_phy_probe,
 	.init		= efx_port_dummy_op_int,
@@ -751,4 +825,6 @@ const struct efx_phy_operations efx_mcdi_phy_ops = {
 	.test_alive	= efx_mcdi_phy_test_alive,
 	.run_tests	= efx_mcdi_phy_run_tests,
 	.test_name	= efx_mcdi_phy_test_name,
+	.get_module_eeprom = efx_mcdi_phy_get_module_eeprom,
+	.get_module_info = efx_mcdi_phy_get_module_info,
 };
diff --git a/drivers/net/ethernet/sfc/net_driver.h b/drivers/net/ethernet/sfc/net_driver.h
index f0385e1fb2d..0e575359af1 100644
--- a/drivers/net/ethernet/sfc/net_driver.h
+++ b/drivers/net/ethernet/sfc/net_driver.h
@@ -252,8 +252,6 @@ struct efx_rx_page_state {
  * @max_fill: RX descriptor maximum fill level (<= ring size)
  * @fast_fill_trigger: RX descriptor fill level that will trigger a fast fill
  *	(<= @max_fill)
- * @fast_fill_limit: The level to which a fast fill will fill
- *	(@fast_fill_trigger <= @fast_fill_limit <= @max_fill)
  * @min_fill: RX descriptor minimum non-zero fill level.
  *	This records the minimum fill level observed when a ring
  *	refill was triggered.
@@ -274,7 +272,6 @@ struct efx_rx_queue {
 	int removed_count;
 	unsigned int max_fill;
 	unsigned int fast_fill_trigger;
-	unsigned int fast_fill_limit;
 	unsigned int min_fill;
 	unsigned int min_overfill;
 	unsigned int alloc_page_count;
@@ -522,6 +519,11 @@ struct efx_phy_operations {
 	int (*test_alive) (struct efx_nic *efx);
 	const char *(*test_name) (struct efx_nic *efx, unsigned int index);
 	int (*run_tests) (struct efx_nic *efx, int *results, unsigned flags);
+	int (*get_module_eeprom) (struct efx_nic *efx,
+			       struct ethtool_eeprom *ee,
+			       u8 *data);
+	int (*get_module_info) (struct efx_nic *efx,
+				struct ethtool_modinfo *modinfo);
 };
 
 /**
diff --git a/drivers/net/ethernet/sfc/qt202x_phy.c b/drivers/net/ethernet/sfc/qt202x_phy.c
index 8a7caf88ffb..326a28637f3 100644
--- a/drivers/net/ethernet/sfc/qt202x_phy.c
+++ b/drivers/net/ethernet/sfc/qt202x_phy.c
@@ -449,6 +449,37 @@ static void qt202x_phy_remove(struct efx_nic *efx)
 	efx->phy_data = NULL;
 }
 
+static int qt202x_phy_get_module_info(struct efx_nic *efx,
+				      struct ethtool_modinfo *modinfo)
+{
+	modinfo->type = ETH_MODULE_SFF_8079;
+	modinfo->eeprom_len = ETH_MODULE_SFF_8079_LEN;
+	return 0;
+}
+
+static int qt202x_phy_get_module_eeprom(struct efx_nic *efx,
+					struct ethtool_eeprom *ee, u8 *data)
+{
+	int mmd, reg_base, rc, i;		
+
+	if (efx->phy_type == PHY_TYPE_QT2025C) {
+		mmd = MDIO_MMD_PCS;
+		reg_base = 0xd000;
+	} else {
+		mmd = MDIO_MMD_PMAPMD;
+		reg_base = 0x8007;
+	}
+
+	for (i = 0; i < ee->len; i++) {
+		rc = efx_mdio_read(efx, mmd, reg_base + ee->offset + i);
+		if (rc < 0)
+			return rc;
+		data[i] = rc;
+	}
+
+	return 0;
+}
+
 const struct efx_phy_operations falcon_qt202x_phy_ops = {
 	.probe		 = qt202x_phy_probe,
 	.init		 = qt202x_phy_init,
@@ -459,4 +490,6 @@ const struct efx_phy_operations falcon_qt202x_phy_ops = {
 	.get_settings	 = qt202x_phy_get_settings,
 	.set_settings	 = efx_mdio_set_settings,
 	.test_alive	 = efx_mdio_test_alive,
+	.get_module_eeprom = qt202x_phy_get_module_eeprom,
+	.get_module_info = qt202x_phy_get_module_info,
 };
diff --git a/drivers/net/ethernet/sfc/rx.c b/drivers/net/ethernet/sfc/rx.c
index 763fa2fe1a3..243e91f3dff 100644
--- a/drivers/net/ethernet/sfc/rx.c
+++ b/drivers/net/ethernet/sfc/rx.c
@@ -76,12 +76,7 @@ static int rx_alloc_method = RX_ALLOC_METHOD_AUTO;
 /* 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 = 90;
-
-/* This is the percentage fill level to which an RX queue will be refilled
- * when the "RX refill threshold" is reached.
- */
-static unsigned int rx_refill_limit = 95;
+static unsigned int rx_refill_threshold;
 
 /*
  * RX maximum head room required.
@@ -342,7 +337,7 @@ static void efx_recycle_rx_buffer(struct efx_channel *channel,
  * efx_fast_push_rx_descriptors - push new RX descriptors quickly
  * @rx_queue:		RX descriptor queue
  * This will aim to fill the RX descriptor queue up to
- * @rx_queue->@fast_fill_limit. If there is insufficient atomic
+ * @rx_queue->@max_fill. If there is insufficient atomic
  * memory to do so, a slow fill will be scheduled.
  *
  * The caller must provide serialisation (none is used here). In practise,
@@ -367,15 +362,14 @@ void efx_fast_push_rx_descriptors(struct efx_rx_queue *rx_queue)
 			rx_queue->min_fill = fill_level;
 	}
 
-	space = rx_queue->fast_fill_limit - fill_level;
-	if (space < EFX_RX_BATCH)
-		goto out;
+	space = rx_queue->max_fill - fill_level;
+	EFX_BUG_ON_PARANOID(space < EFX_RX_BATCH);
 
 	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",
 		   efx_rx_queue_index(rx_queue), fill_level,
-		   rx_queue->fast_fill_limit,
+		   rx_queue->max_fill,
 		   channel->rx_alloc_push_pages ? "page" : "skb");
 
 	do {
@@ -681,7 +675,7 @@ int efx_probe_rx_queue(struct efx_rx_queue *rx_queue)
 void efx_init_rx_queue(struct efx_rx_queue *rx_queue)
 {
 	struct efx_nic *efx = rx_queue->efx;
-	unsigned int max_fill, trigger, limit;
+	unsigned int max_fill, trigger, max_trigger;
 
 	netif_dbg(rx_queue->efx, drv, rx_queue->efx->net_dev,
 		  "initialising RX queue %d\n", efx_rx_queue_index(rx_queue));
@@ -694,12 +688,17 @@ void efx_init_rx_queue(struct efx_rx_queue *rx_queue)
 
 	/* Initialise limit fields */
 	max_fill = efx->rxq_entries - EFX_RXD_HEAD_ROOM;
-	trigger = max_fill * min(rx_refill_threshold, 100U) / 100U;
-	limit = max_fill * min(rx_refill_limit, 100U) / 100U;
+	max_trigger = max_fill - EFX_RX_BATCH;
+	if (rx_refill_threshold != 0) {
+		trigger = max_fill * min(rx_refill_threshold, 100U) / 100U;
+		if (trigger > max_trigger)
+			trigger = max_trigger;
+	} else {
+		trigger = max_trigger;
+	}
 
 	rx_queue->max_fill = max_fill;
 	rx_queue->fast_fill_trigger = trigger;
-	rx_queue->fast_fill_limit = limit;
 
 	/* Set up RX descriptor ring */
 	rx_queue->enabled = true;
@@ -746,5 +745,5 @@ 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 fast/slow fill threshold (%)");
+		 "RX descriptor ring refill threshold (%)");
 
diff --git a/drivers/net/ethernet/silan/sc92031.c b/drivers/net/ethernet/silan/sc92031.c
index a284d644053..32e55664df6 100644
--- a/drivers/net/ethernet/silan/sc92031.c
+++ b/drivers/net/ethernet/silan/sc92031.c
@@ -39,9 +39,7 @@
 #define SC92031_NAME "sc92031"
 
 /* BAR 0 is MMIO, BAR 1 is PIO */
-#ifndef SC92031_USE_BAR
-#define SC92031_USE_BAR 0
-#endif
+#define SC92031_USE_PIO	0
 
 /* Maximum number of multicast addresses to filter (vs. Rx-all-multicast). */
 static int multicast_filter_limit = 64;
@@ -366,7 +364,7 @@ static void sc92031_disable_interrupts(struct net_device *dev)
 	mmiowb();
 
 	/* wait for any concurrent interrupt/tasklet to finish */
-	synchronize_irq(dev->irq);
+	synchronize_irq(priv->pdev->irq);
 	tasklet_disable(&priv->tasklet);
 }
 
@@ -1114,10 +1112,13 @@ static void sc92031_tx_timeout(struct net_device *dev)
 #ifdef CONFIG_NET_POLL_CONTROLLER
 static void sc92031_poll_controller(struct net_device *dev)
 {
-	disable_irq(dev->irq);
-	if (sc92031_interrupt(dev->irq, dev) != IRQ_NONE)
+	struct sc92031_priv *priv = netdev_priv(dev);
+	const int irq = priv->pdev->irq;
+
+	disable_irq(irq);
+	if (sc92031_interrupt(irq, dev) != IRQ_NONE)
 		sc92031_tasklet((unsigned long)dev);
-	enable_irq(dev->irq);
+	enable_irq(irq);
 }
 #endif
 
@@ -1402,7 +1403,6 @@ static int __devinit sc92031_probe(struct pci_dev *pdev,
 	struct net_device *dev;
 	struct sc92031_priv *priv;
 	u32 mac0, mac1;
-	unsigned long base_addr;
 
 	err = pci_enable_device(pdev);
 	if (unlikely(err < 0))
@@ -1422,7 +1422,7 @@ static int __devinit sc92031_probe(struct pci_dev *pdev,
 	if (unlikely(err < 0))
 		goto out_request_regions;
 
-	port_base = pci_iomap(pdev, SC92031_USE_BAR, 0);
+	port_base = pci_iomap(pdev, SC92031_USE_PIO, 0);
 	if (unlikely(!port_base)) {
 		err = -EIO;
 		goto out_iomap;
@@ -1437,14 +1437,6 @@ static int __devinit sc92031_probe(struct pci_dev *pdev,
 	pci_set_drvdata(pdev, dev);
 	SET_NETDEV_DEV(dev, &pdev->dev);
 
-#if SC92031_USE_BAR == 0
-	dev->mem_start = pci_resource_start(pdev, SC92031_USE_BAR);
-	dev->mem_end = pci_resource_end(pdev, SC92031_USE_BAR);
-#elif SC92031_USE_BAR == 1
-	dev->base_addr = pci_resource_start(pdev, SC92031_USE_BAR);
-#endif
-	dev->irq = pdev->irq;
-
 	/* faked with skb_copy_and_csum_dev */
 	dev->features = NETIF_F_SG | NETIF_F_HIGHDMA |
 		NETIF_F_IP_CSUM | NETIF_F_IPV6_CSUM;
@@ -1478,13 +1470,9 @@ static int __devinit sc92031_probe(struct pci_dev *pdev,
 	if (err < 0)
 		goto out_register_netdev;
 
-#if SC92031_USE_BAR == 0
-	base_addr = dev->mem_start;
-#elif SC92031_USE_BAR == 1
-	base_addr = dev->base_addr;
-#endif
 	printk(KERN_INFO "%s: SC92031 at 0x%lx, %pM, IRQ %d\n", dev->name,
-			base_addr, dev->dev_addr, dev->irq);
+	       (long)pci_resource_start(pdev, SC92031_USE_PIO), dev->dev_addr,
+	       pdev->irq);
 
 	return 0;
 
diff --git a/drivers/net/ethernet/sis/sis190.c b/drivers/net/ethernet/sis/sis190.c
index a9deda8eaf6..4613591b43e 100644
--- a/drivers/net/ethernet/sis/sis190.c
+++ b/drivers/net/ethernet/sis/sis190.c
@@ -729,7 +729,7 @@ static void sis190_tx_interrupt(struct net_device *dev,
  * The interrupt handler does all of the Rx thread work and cleans up after
  * the Tx thread.
  */
-static irqreturn_t sis190_interrupt(int irq, void *__dev)
+static irqreturn_t sis190_irq(int irq, void *__dev)
 {
 	struct net_device *dev = __dev;
 	struct sis190_private *tp = netdev_priv(dev);
@@ -772,11 +772,11 @@ out:
 static void sis190_netpoll(struct net_device *dev)
 {
 	struct sis190_private *tp = netdev_priv(dev);
-	struct pci_dev *pdev = tp->pci_dev;
+	const int irq = tp->pci_dev->irq;
 
-	disable_irq(pdev->irq);
-	sis190_interrupt(pdev->irq, dev);
-	enable_irq(pdev->irq);
+	disable_irq(irq);
+	sis190_irq(irq, dev);
+	enable_irq(irq);
 }
 #endif
 
@@ -1085,7 +1085,7 @@ static int sis190_open(struct net_device *dev)
 
 	sis190_request_timer(dev);
 
-	rc = request_irq(dev->irq, sis190_interrupt, IRQF_SHARED, dev->name, dev);
+	rc = request_irq(pdev->irq, sis190_irq, IRQF_SHARED, dev->name, dev);
 	if (rc < 0)
 		goto err_release_timer_2;
 
@@ -1097,11 +1097,9 @@ err_release_timer_2:
 	sis190_delete_timer(dev);
 	sis190_rx_clear(tp);
 err_free_rx_1:
-	pci_free_consistent(tp->pci_dev, RX_RING_BYTES, tp->RxDescRing,
-		tp->rx_dma);
+	pci_free_consistent(pdev, RX_RING_BYTES, tp->RxDescRing, tp->rx_dma);
 err_free_tx_0:
-	pci_free_consistent(tp->pci_dev, TX_RING_BYTES, tp->TxDescRing,
-		tp->tx_dma);
+	pci_free_consistent(pdev, TX_RING_BYTES, tp->TxDescRing, tp->tx_dma);
 	goto out;
 }
 
@@ -1141,7 +1139,7 @@ static void sis190_down(struct net_device *dev)
 
 		spin_unlock_irq(&tp->lock);
 
-		synchronize_irq(dev->irq);
+		synchronize_irq(tp->pci_dev->irq);
 
 		if (!poll_locked)
 			poll_locked++;
@@ -1161,7 +1159,7 @@ static int sis190_close(struct net_device *dev)
 
 	sis190_down(dev);
 
-	free_irq(dev->irq, dev);
+	free_irq(pdev->irq, dev);
 
 	pci_free_consistent(pdev, TX_RING_BYTES, tp->TxDescRing, tp->tx_dma);
 	pci_free_consistent(pdev, RX_RING_BYTES, tp->RxDescRing, tp->rx_dma);
@@ -1884,8 +1882,6 @@ static int __devinit sis190_init_one(struct pci_dev *pdev,
 	dev->netdev_ops = &sis190_netdev_ops;
 
 	SET_ETHTOOL_OPS(dev, &sis190_ethtool_ops);
-	dev->irq = pdev->irq;
-	dev->base_addr = (unsigned long) 0xdead;
 	dev->watchdog_timeo = SIS190_TX_TIMEOUT;
 
 	spin_lock_init(&tp->lock);
@@ -1902,7 +1898,7 @@ static int __devinit sis190_init_one(struct pci_dev *pdev,
 		netdev_info(dev, "%s: %s at %p (IRQ: %d), %pM\n",
 			    pci_name(pdev),
 			    sis_chip_info[ent->driver_data].name,
-			    ioaddr, dev->irq, dev->dev_addr);
+			    ioaddr, pdev->irq, dev->dev_addr);
 		netdev_info(dev, "%s mode.\n",
 			    (tp->features & F_HAS_RGMII) ? "RGMII" : "GMII");
 	}
diff --git a/drivers/net/ethernet/sis/sis900.c b/drivers/net/ethernet/sis/sis900.c
index 5ccf02e7e3a..203d9c6ec23 100644
--- a/drivers/net/ethernet/sis/sis900.c
+++ b/drivers/net/ethernet/sis/sis900.c
@@ -168,6 +168,8 @@ struct sis900_private {
 	unsigned int cur_phy;
 	struct mii_if_info mii_info;
 
+	void __iomem	*ioaddr;
+
 	struct timer_list timer; /* Link status detection timer. */
 	u8 autong_complete; /* 1: auto-negotiate complete  */
 
@@ -201,13 +203,18 @@ MODULE_PARM_DESC(multicast_filter_limit, "SiS 900/7016 maximum number of filtere
 MODULE_PARM_DESC(max_interrupt_work, "SiS 900/7016 maximum events handled per interrupt");
 MODULE_PARM_DESC(sis900_debug, "SiS 900/7016 bitmapped debugging message level");
 
+#define sw32(reg, val)	iowrite32(val, ioaddr + (reg))
+#define sw8(reg, val)	iowrite8(val, ioaddr + (reg))
+#define sr32(reg)	ioread32(ioaddr + (reg))
+#define sr16(reg)	ioread16(ioaddr + (reg))
+
 #ifdef CONFIG_NET_POLL_CONTROLLER
 static void sis900_poll(struct net_device *dev);
 #endif
 static int sis900_open(struct net_device *net_dev);
 static int sis900_mii_probe (struct net_device * net_dev);
 static void sis900_init_rxfilter (struct net_device * net_dev);
-static u16 read_eeprom(long ioaddr, int location);
+static u16 read_eeprom(void __iomem *ioaddr, int location);
 static int mdio_read(struct net_device *net_dev, int phy_id, int location);
 static void mdio_write(struct net_device *net_dev, int phy_id, int location, int val);
 static void sis900_timer(unsigned long data);
@@ -231,7 +238,7 @@ static u16 sis900_default_phy(struct net_device * net_dev);
 static void sis900_set_capability( struct net_device *net_dev ,struct mii_phy *phy);
 static u16 sis900_reset_phy(struct net_device *net_dev, int phy_addr);
 static void sis900_auto_negotiate(struct net_device *net_dev, int phy_addr);
-static void sis900_set_mode (long ioaddr, int speed, int duplex);
+static void sis900_set_mode(struct sis900_private *, int speed, int duplex);
 static const struct ethtool_ops sis900_ethtool_ops;
 
 /**
@@ -246,7 +253,8 @@ static const struct ethtool_ops sis900_ethtool_ops;
 
 static int __devinit sis900_get_mac_addr(struct pci_dev * pci_dev, struct net_device *net_dev)
 {
-	long ioaddr = pci_resource_start(pci_dev, 0);
+	struct sis900_private *sis_priv = netdev_priv(net_dev);
+	void __iomem *ioaddr = sis_priv->ioaddr;
 	u16 signature;
 	int i;
 
@@ -325,29 +333,30 @@ static int __devinit sis630e_get_mac_addr(struct pci_dev * pci_dev,
 static int __devinit sis635_get_mac_addr(struct pci_dev * pci_dev,
 					struct net_device *net_dev)
 {
-	long ioaddr = net_dev->base_addr;
+	struct sis900_private *sis_priv = netdev_priv(net_dev);
+	void __iomem *ioaddr = sis_priv->ioaddr;
 	u32 rfcrSave;
 	u32 i;
 
-	rfcrSave = inl(rfcr + ioaddr);
+	rfcrSave = sr32(rfcr);
 
-	outl(rfcrSave | RELOAD, ioaddr + cr);
-	outl(0, ioaddr + cr);
+	sw32(cr, rfcrSave | RELOAD);
+	sw32(cr, 0);
 
 	/* disable packet filtering before setting filter */
-	outl(rfcrSave & ~RFEN, rfcr + ioaddr);
+	sw32(rfcr, rfcrSave & ~RFEN);
 
 	/* load MAC addr to filter data register */
 	for (i = 0 ; i < 3 ; i++) {
-		outl((i << RFADDR_shift), ioaddr + rfcr);
-		*( ((u16 *)net_dev->dev_addr) + i) = inw(ioaddr + rfdr);
+		sw32(rfcr, (i << RFADDR_shift));
+		*( ((u16 *)net_dev->dev_addr) + i) = sr16(rfdr);
 	}
 
 	/* Store MAC Address in perm_addr */
 	memcpy(net_dev->perm_addr, net_dev->dev_addr, ETH_ALEN);
 
 	/* enable packet filtering */
-	outl(rfcrSave | RFEN, rfcr + ioaddr);
+	sw32(rfcr, rfcrSave | RFEN);
 
 	return 1;
 }
@@ -371,31 +380,30 @@ static int __devinit sis635_get_mac_addr(struct pci_dev * pci_dev,
 static int __devinit sis96x_get_mac_addr(struct pci_dev * pci_dev,
 					struct net_device *net_dev)
 {
-	long ioaddr = net_dev->base_addr;
-	long ee_addr = ioaddr + mear;
-	u32 waittime = 0;
-	int i;
+	struct sis900_private *sis_priv = netdev_priv(net_dev);
+	void __iomem *ioaddr = sis_priv->ioaddr;
+	int wait, rc = 0;
 
-	outl(EEREQ, ee_addr);
-	while(waittime < 2000) {
-		if(inl(ee_addr) & EEGNT) {
+	sw32(mear, EEREQ);
+	for (wait = 0; wait < 2000; wait++) {
+		if (sr32(mear) & EEGNT) {
+			u16 *mac = (u16 *)net_dev->dev_addr;
+			int i;
 
 			/* get MAC address from EEPROM */
 			for (i = 0; i < 3; i++)
-			        ((u16 *)(net_dev->dev_addr))[i] = read_eeprom(ioaddr, i+EEPROMMACAddr);
+			        mac[i] = read_eeprom(ioaddr, i + EEPROMMACAddr);
 
 			/* Store MAC Address in perm_addr */
 			memcpy(net_dev->perm_addr, net_dev->dev_addr, ETH_ALEN);
 
-			outl(EEDONE, ee_addr);
-			return 1;
-		} else {
-			udelay(1);
-			waittime ++;
+			rc = 1;
+			break;
 		}
+		udelay(1);
 	}
-	outl(EEDONE, ee_addr);
-	return 0;
+	sw32(mear, EEDONE);
+	return rc;
 }
 
 static const struct net_device_ops sis900_netdev_ops = {
@@ -433,7 +441,7 @@ static int __devinit sis900_probe(struct pci_dev *pci_dev,
 	struct pci_dev *dev;
 	dma_addr_t ring_dma;
 	void *ring_space;
-	long ioaddr;
+	void __iomem *ioaddr;
 	int i, ret;
 	const char *card_name = card_names[pci_id->driver_data];
 	const char *dev_name = pci_name(pci_dev);
@@ -464,14 +472,17 @@ static int __devinit sis900_probe(struct pci_dev *pci_dev,
 	SET_NETDEV_DEV(net_dev, &pci_dev->dev);
 
 	/* We do a request_region() to register /proc/ioports info. */
-	ioaddr = pci_resource_start(pci_dev, 0);
 	ret = pci_request_regions(pci_dev, "sis900");
 	if (ret)
 		goto err_out;
 
+	/* IO region. */
+	ioaddr = pci_iomap(pci_dev, 0, 0);
+	if (!ioaddr)
+		goto err_out_cleardev;
+
 	sis_priv = netdev_priv(net_dev);
-	net_dev->base_addr = ioaddr;
-	net_dev->irq = pci_dev->irq;
+	sis_priv->ioaddr = ioaddr;
 	sis_priv->pci_dev = pci_dev;
 	spin_lock_init(&sis_priv->lock);
 
@@ -480,7 +491,7 @@ static int __devinit sis900_probe(struct pci_dev *pci_dev,
 	ring_space = pci_alloc_consistent(pci_dev, TX_TOTAL_SIZE, &ring_dma);
 	if (!ring_space) {
 		ret = -ENOMEM;
-		goto err_out_cleardev;
+		goto err_out_unmap;
 	}
 	sis_priv->tx_ring = ring_space;
 	sis_priv->tx_ring_dma = ring_dma;
@@ -534,7 +545,7 @@ static int __devinit sis900_probe(struct pci_dev *pci_dev,
 
 	/* 630ET : set the mii access mode as software-mode */
 	if (sis_priv->chipset_rev == SIS630ET_900_REV)
-		outl(ACCESSMODE | inl(ioaddr + cr), ioaddr + cr);
+		sw32(cr, ACCESSMODE | sr32(cr));
 
 	/* probe for mii transceiver */
 	if (sis900_mii_probe(net_dev) == 0) {
@@ -556,25 +567,27 @@ static int __devinit sis900_probe(struct pci_dev *pci_dev,
 		goto err_unmap_rx;
 
 	/* print some information about our NIC */
-	printk(KERN_INFO "%s: %s at %#lx, IRQ %d, %pM\n",
-	       net_dev->name, card_name, ioaddr, net_dev->irq,
+	printk(KERN_INFO "%s: %s at 0x%p, IRQ %d, %pM\n",
+	       net_dev->name, card_name, ioaddr, pci_dev->irq,
 	       net_dev->dev_addr);
 
 	/* Detect Wake on Lan support */
-	ret = (inl(net_dev->base_addr + CFGPMC) & PMESP) >> 27;
+	ret = (sr32(CFGPMC) & PMESP) >> 27;
 	if (netif_msg_probe(sis_priv) && (ret & PME_D3C) == 0)
 		printk(KERN_INFO "%s: Wake on LAN only available from suspend to RAM.", net_dev->name);
 
 	return 0;
 
- err_unmap_rx:
+err_unmap_rx:
 	pci_free_consistent(pci_dev, RX_TOTAL_SIZE, sis_priv->rx_ring,
 		sis_priv->rx_ring_dma);
- err_unmap_tx:
+err_unmap_tx:
 	pci_free_consistent(pci_dev, TX_TOTAL_SIZE, sis_priv->tx_ring,
 		sis_priv->tx_ring_dma);
- err_out_cleardev:
- 	pci_set_drvdata(pci_dev, NULL);
+err_out_unmap:
+	pci_iounmap(pci_dev, ioaddr);
+err_out_cleardev:
+	pci_set_drvdata(pci_dev, NULL);
 	pci_release_regions(pci_dev);
  err_out:
 	free_netdev(net_dev);
@@ -798,7 +811,7 @@ static void sis900_set_capability(struct net_device *net_dev, struct mii_phy *ph
 
 
 /* Delay between EEPROM clock transitions. */
-#define eeprom_delay()  inl(ee_addr)
+#define eeprom_delay()	sr32(mear)
 
 /**
  *	read_eeprom - Read Serial EEPROM
@@ -809,41 +822,41 @@ static void sis900_set_capability(struct net_device *net_dev, struct mii_phy *ph
  *	Note that location is in word (16 bits) unit
  */
 
-static u16 __devinit read_eeprom(long ioaddr, int location)
+static u16 __devinit read_eeprom(void __iomem *ioaddr, int location)
 {
+	u32 read_cmd = location | EEread;
 	int i;
 	u16 retval = 0;
-	long ee_addr = ioaddr + mear;
-	u32 read_cmd = location | EEread;
 
-	outl(0, ee_addr);
+	sw32(mear, 0);
 	eeprom_delay();
-	outl(EECS, ee_addr);
+	sw32(mear, EECS);
 	eeprom_delay();
 
 	/* Shift the read command (9) bits out. */
 	for (i = 8; i >= 0; i--) {
 		u32 dataval = (read_cmd & (1 << i)) ? EEDI | EECS : EECS;
-		outl(dataval, ee_addr);
+
+		sw32(mear, dataval);
 		eeprom_delay();
-		outl(dataval | EECLK, ee_addr);
+		sw32(mear, dataval | EECLK);
 		eeprom_delay();
 	}
-	outl(EECS, ee_addr);
+	sw32(mear, EECS);
 	eeprom_delay();
 
 	/* read the 16-bits data in */
 	for (i = 16; i > 0; i--) {
-		outl(EECS, ee_addr);
+		sw32(mear, EECS);
 		eeprom_delay();
-		outl(EECS | EECLK, ee_addr);
+		sw32(mear, EECS | EECLK);
 		eeprom_delay();
-		retval = (retval << 1) | ((inl(ee_addr) & EEDO) ? 1 : 0);
+		retval = (retval << 1) | ((sr32(mear) & EEDO) ? 1 : 0);
 		eeprom_delay();
 	}
 
 	/* Terminate the EEPROM access. */
-	outl(0, ee_addr);
+	sw32(mear, 0);
 	eeprom_delay();
 
 	return retval;
@@ -852,24 +865,27 @@ static u16 __devinit read_eeprom(long ioaddr, int location)
 /* Read and write the MII management registers using software-generated
    serial MDIO protocol. Note that the command bits and data bits are
    send out separately */
-#define mdio_delay()    inl(mdio_addr)
+#define mdio_delay()	sr32(mear)
 
-static void mdio_idle(long mdio_addr)
+static void mdio_idle(struct sis900_private *sp)
 {
-	outl(MDIO | MDDIR, mdio_addr);
+	void __iomem *ioaddr = sp->ioaddr;
+
+	sw32(mear, MDIO | MDDIR);
 	mdio_delay();
-	outl(MDIO | MDDIR | MDC, mdio_addr);
+	sw32(mear, MDIO | MDDIR | MDC);
 }
 
-/* Syncronize the MII management interface by shifting 32 one bits out. */
-static void mdio_reset(long mdio_addr)
+/* Synchronize the MII management interface by shifting 32 one bits out. */
+static void mdio_reset(struct sis900_private *sp)
 {
+	void __iomem *ioaddr = sp->ioaddr;
 	int i;
 
 	for (i = 31; i >= 0; i--) {
-		outl(MDDIR | MDIO, mdio_addr);
+		sw32(mear, MDDIR | MDIO);
 		mdio_delay();
-		outl(MDDIR | MDIO | MDC, mdio_addr);
+		sw32(mear, MDDIR | MDIO | MDC);
 		mdio_delay();
 	}
 }
@@ -887,31 +903,33 @@ static void mdio_reset(long mdio_addr)
 
 static int mdio_read(struct net_device *net_dev, int phy_id, int location)
 {
-	long mdio_addr = net_dev->base_addr + mear;
 	int mii_cmd = MIIread|(phy_id<<MIIpmdShift)|(location<<MIIregShift);
+	struct sis900_private *sp = netdev_priv(net_dev);
+	void __iomem *ioaddr = sp->ioaddr;
 	u16 retval = 0;
 	int i;
 
-	mdio_reset(mdio_addr);
-	mdio_idle(mdio_addr);
+	mdio_reset(sp);
+	mdio_idle(sp);
 
 	for (i = 15; i >= 0; i--) {
 		int dataval = (mii_cmd & (1 << i)) ? MDDIR | MDIO : MDDIR;
-		outl(dataval, mdio_addr);
+
+		sw32(mear, dataval);
 		mdio_delay();
-		outl(dataval | MDC, mdio_addr);
+		sw32(mear, dataval | MDC);
 		mdio_delay();
 	}
 
 	/* Read the 16 data bits. */
 	for (i = 16; i > 0; i--) {
-		outl(0, mdio_addr);
+		sw32(mear, 0);
 		mdio_delay();
-		retval = (retval << 1) | ((inl(mdio_addr) & MDIO) ? 1 : 0);
-		outl(MDC, mdio_addr);
+		retval = (retval << 1) | ((sr32(mear) & MDIO) ? 1 : 0);
+		sw32(mear, MDC);
 		mdio_delay();
 	}
-	outl(0x00, mdio_addr);
+	sw32(mear, 0x00);
 
 	return retval;
 }
@@ -931,19 +949,21 @@ static int mdio_read(struct net_device *net_dev, int phy_id, int location)
 static void mdio_write(struct net_device *net_dev, int phy_id, int location,
 			int value)
 {
-	long mdio_addr = net_dev->base_addr + mear;
 	int mii_cmd = MIIwrite|(phy_id<<MIIpmdShift)|(location<<MIIregShift);
+	struct sis900_private *sp = netdev_priv(net_dev);
+	void __iomem *ioaddr = sp->ioaddr;
 	int i;
 
-	mdio_reset(mdio_addr);
-	mdio_idle(mdio_addr);
+	mdio_reset(sp);
+	mdio_idle(sp);
 
 	/* Shift the command bits out. */
 	for (i = 15; i >= 0; i--) {
 		int dataval = (mii_cmd & (1 << i)) ? MDDIR | MDIO : MDDIR;
-		outb(dataval, mdio_addr);
+
+		sw8(mear, dataval);
 		mdio_delay();
-		outb(dataval | MDC, mdio_addr);
+		sw8(mear, dataval | MDC);
 		mdio_delay();
 	}
 	mdio_delay();
@@ -951,21 +971,22 @@ static void mdio_write(struct net_device *net_dev, int phy_id, int location,
 	/* Shift the value bits out. */
 	for (i = 15; i >= 0; i--) {
 		int dataval = (value & (1 << i)) ? MDDIR | MDIO : MDDIR;
-		outl(dataval, mdio_addr);
+
+		sw32(mear, dataval);
 		mdio_delay();
-		outl(dataval | MDC, mdio_addr);
+		sw32(mear, dataval | MDC);
 		mdio_delay();
 	}
 	mdio_delay();
 
 	/* Clear out extra bits. */
 	for (i = 2; i > 0; i--) {
-		outb(0, mdio_addr);
+		sw8(mear, 0);
 		mdio_delay();
-		outb(MDC, mdio_addr);
+		sw8(mear, MDC);
 		mdio_delay();
 	}
-	outl(0x00, mdio_addr);
+	sw32(mear, 0x00);
 }
 
 
@@ -1000,9 +1021,12 @@ static u16 sis900_reset_phy(struct net_device *net_dev, int phy_addr)
 */
 static void sis900_poll(struct net_device *dev)
 {
-	disable_irq(dev->irq);
-	sis900_interrupt(dev->irq, dev);
-	enable_irq(dev->irq);
+	struct sis900_private *sp = netdev_priv(dev);
+	const int irq = sp->pci_dev->irq;
+
+	disable_irq(irq);
+	sis900_interrupt(irq, dev);
+	enable_irq(irq);
 }
 #endif
 
@@ -1018,7 +1042,7 @@ static int
 sis900_open(struct net_device *net_dev)
 {
 	struct sis900_private *sis_priv = netdev_priv(net_dev);
-	long ioaddr = net_dev->base_addr;
+	void __iomem *ioaddr = sis_priv->ioaddr;
 	int ret;
 
 	/* Soft reset the chip. */
@@ -1027,8 +1051,8 @@ sis900_open(struct net_device *net_dev)
 	/* Equalizer workaround Rule */
 	sis630_set_eq(net_dev, sis_priv->chipset_rev);
 
-	ret = request_irq(net_dev->irq, sis900_interrupt, IRQF_SHARED,
-						net_dev->name, net_dev);
+	ret = request_irq(sis_priv->pci_dev->irq, sis900_interrupt, IRQF_SHARED,
+			  net_dev->name, net_dev);
 	if (ret)
 		return ret;
 
@@ -1042,12 +1066,12 @@ sis900_open(struct net_device *net_dev)
 	netif_start_queue(net_dev);
 
 	/* Workaround for EDB */
-	sis900_set_mode(ioaddr, HW_SPEED_10_MBPS, FDX_CAPABLE_HALF_SELECTED);
+	sis900_set_mode(sis_priv, HW_SPEED_10_MBPS, FDX_CAPABLE_HALF_SELECTED);
 
 	/* Enable all known interrupts by setting the interrupt mask. */
-	outl((RxSOVR|RxORN|RxERR|RxOK|TxURN|TxERR|TxIDLE), ioaddr + imr);
-	outl(RxENA | inl(ioaddr + cr), ioaddr + cr);
-	outl(IE, ioaddr + ier);
+	sw32(imr, RxSOVR | RxORN | RxERR | RxOK | TxURN | TxERR | TxIDLE);
+	sw32(cr, RxENA | sr32(cr));
+	sw32(ier, IE);
 
 	sis900_check_mode(net_dev, sis_priv->mii);
 
@@ -1074,31 +1098,30 @@ static void
 sis900_init_rxfilter (struct net_device * net_dev)
 {
 	struct sis900_private *sis_priv = netdev_priv(net_dev);
-	long ioaddr = net_dev->base_addr;
+	void __iomem *ioaddr = sis_priv->ioaddr;
 	u32 rfcrSave;
 	u32 i;
 
-	rfcrSave = inl(rfcr + ioaddr);
+	rfcrSave = sr32(rfcr);
 
 	/* disable packet filtering before setting filter */
-	outl(rfcrSave & ~RFEN, rfcr + ioaddr);
+	sw32(rfcr, rfcrSave & ~RFEN);
 
 	/* load MAC addr to filter data register */
 	for (i = 0 ; i < 3 ; i++) {
-		u32 w;
+		u32 w = (u32) *((u16 *)(net_dev->dev_addr)+i);
 
-		w = (u32) *((u16 *)(net_dev->dev_addr)+i);
-		outl((i << RFADDR_shift), ioaddr + rfcr);
-		outl(w, ioaddr + rfdr);
+		sw32(rfcr, i << RFADDR_shift);
+		sw32(rfdr, w);
 
 		if (netif_msg_hw(sis_priv)) {
 			printk(KERN_DEBUG "%s: Receive Filter Addrss[%d]=%x\n",
-			       net_dev->name, i, inl(ioaddr + rfdr));
+			       net_dev->name, i, sr32(rfdr));
 		}
 	}
 
 	/* enable packet filtering */
-	outl(rfcrSave | RFEN, rfcr + ioaddr);
+	sw32(rfcr, rfcrSave | RFEN);
 }
 
 /**
@@ -1112,7 +1135,7 @@ static void
 sis900_init_tx_ring(struct net_device *net_dev)
 {
 	struct sis900_private *sis_priv = netdev_priv(net_dev);
-	long ioaddr = net_dev->base_addr;
+	void __iomem *ioaddr = sis_priv->ioaddr;
 	int i;
 
 	sis_priv->tx_full = 0;
@@ -1128,10 +1151,10 @@ sis900_init_tx_ring(struct net_device *net_dev)
 	}
 
 	/* load Transmit Descriptor Register */
-	outl(sis_priv->tx_ring_dma, ioaddr + txdp);
+	sw32(txdp, sis_priv->tx_ring_dma);
 	if (netif_msg_hw(sis_priv))
 		printk(KERN_DEBUG "%s: TX descriptor register loaded with: %8.8x\n",
-		       net_dev->name, inl(ioaddr + txdp));
+		       net_dev->name, sr32(txdp));
 }
 
 /**
@@ -1146,7 +1169,7 @@ static void
 sis900_init_rx_ring(struct net_device *net_dev)
 {
 	struct sis900_private *sis_priv = netdev_priv(net_dev);
-	long ioaddr = net_dev->base_addr;
+	void __iomem *ioaddr = sis_priv->ioaddr;
 	int i;
 
 	sis_priv->cur_rx = 0;
@@ -1181,10 +1204,10 @@ sis900_init_rx_ring(struct net_device *net_dev)
 	sis_priv->dirty_rx = (unsigned int) (i - NUM_RX_DESC);
 
 	/* load Receive Descriptor Register */
-	outl(sis_priv->rx_ring_dma, ioaddr + rxdp);
+	sw32(rxdp, sis_priv->rx_ring_dma);
 	if (netif_msg_hw(sis_priv))
 		printk(KERN_DEBUG "%s: RX descriptor register loaded with: %8.8x\n",
-		       net_dev->name, inl(ioaddr + rxdp));
+		       net_dev->name, sr32(rxdp));
 }
 
 /**
@@ -1298,7 +1321,7 @@ static void sis900_timer(unsigned long data)
 
 		sis900_read_mode(net_dev, &speed, &duplex);
 		if (duplex){
-			sis900_set_mode(net_dev->base_addr, speed, duplex);
+			sis900_set_mode(sis_priv, speed, duplex);
 			sis630_set_eq(net_dev, sis_priv->chipset_rev);
 			netif_start_queue(net_dev);
 		}
@@ -1359,25 +1382,25 @@ static void sis900_timer(unsigned long data)
 static void sis900_check_mode(struct net_device *net_dev, struct mii_phy *mii_phy)
 {
 	struct sis900_private *sis_priv = netdev_priv(net_dev);
-	long ioaddr = net_dev->base_addr;
+	void __iomem *ioaddr = sis_priv->ioaddr;
 	int speed, duplex;
 
 	if (mii_phy->phy_types == LAN) {
-		outl(~EXD & inl(ioaddr + cfg), ioaddr + cfg);
+		sw32(cfg, ~EXD & sr32(cfg));
 		sis900_set_capability(net_dev , mii_phy);
 		sis900_auto_negotiate(net_dev, sis_priv->cur_phy);
 	} else {
-		outl(EXD | inl(ioaddr + cfg), ioaddr + cfg);
+		sw32(cfg, EXD | sr32(cfg));
 		speed = HW_SPEED_HOME;
 		duplex = FDX_CAPABLE_HALF_SELECTED;
-		sis900_set_mode(ioaddr, speed, duplex);
+		sis900_set_mode(sis_priv, speed, duplex);
 		sis_priv->autong_complete = 1;
 	}
 }
 
 /**
  *	sis900_set_mode - Set the media mode of mac register.
- *	@ioaddr: the address of the device
+ *	@sp:     the device private data
  *	@speed : the transmit speed to be determined
  *	@duplex: the duplex mode to be determined
  *
@@ -1388,11 +1411,12 @@ static void sis900_check_mode(struct net_device *net_dev, struct mii_phy *mii_ph
  *	double words.
  */
 
-static void sis900_set_mode (long ioaddr, int speed, int duplex)
+static void sis900_set_mode(struct sis900_private *sp, int speed, int duplex)
 {
+	void __iomem *ioaddr = sp->ioaddr;
 	u32 tx_flags = 0, rx_flags = 0;
 
-	if (inl(ioaddr + cfg) & EDB_MASTER_EN) {
+	if (sr32( cfg) & EDB_MASTER_EN) {
 		tx_flags = TxATP | (DMA_BURST_64 << TxMXDMA_shift) |
 					(TX_FILL_THRESH << TxFILLT_shift);
 		rx_flags = DMA_BURST_64 << RxMXDMA_shift;
@@ -1420,8 +1444,8 @@ static void sis900_set_mode (long ioaddr, int speed, int duplex)
 	rx_flags |= RxAJAB;
 #endif
 
-	outl (tx_flags, ioaddr + txcfg);
-	outl (rx_flags, ioaddr + rxcfg);
+	sw32(txcfg, tx_flags);
+	sw32(rxcfg, rx_flags);
 }
 
 /**
@@ -1528,16 +1552,17 @@ static void sis900_read_mode(struct net_device *net_dev, int *speed, int *duplex
 static void sis900_tx_timeout(struct net_device *net_dev)
 {
 	struct sis900_private *sis_priv = netdev_priv(net_dev);
-	long ioaddr = net_dev->base_addr;
+	void __iomem *ioaddr = sis_priv->ioaddr;
 	unsigned long flags;
 	int i;
 
-	if(netif_msg_tx_err(sis_priv))
+	if (netif_msg_tx_err(sis_priv)) {
 		printk(KERN_INFO "%s: Transmit timeout, status %8.8x %8.8x\n",
-	       		net_dev->name, inl(ioaddr + cr), inl(ioaddr + isr));
+			net_dev->name, sr32(cr), sr32(isr));
+	}
 
 	/* Disable interrupts by clearing the interrupt mask. */
-	outl(0x0000, ioaddr + imr);
+	sw32(imr, 0x0000);
 
 	/* use spinlock to prevent interrupt handler accessing buffer ring */
 	spin_lock_irqsave(&sis_priv->lock, flags);
@@ -1566,10 +1591,10 @@ static void sis900_tx_timeout(struct net_device *net_dev)
 	net_dev->trans_start = jiffies; /* prevent tx timeout */
 
 	/* load Transmit Descriptor Register */
-	outl(sis_priv->tx_ring_dma, ioaddr + txdp);
+	sw32(txdp, sis_priv->tx_ring_dma);
 
 	/* Enable all known interrupts by setting the interrupt mask. */
-	outl((RxSOVR|RxORN|RxERR|RxOK|TxURN|TxERR|TxIDLE), ioaddr + imr);
+	sw32(imr, RxSOVR | RxORN | RxERR | RxOK | TxURN | TxERR | TxIDLE);
 }
 
 /**
@@ -1586,7 +1611,7 @@ static netdev_tx_t
 sis900_start_xmit(struct sk_buff *skb, struct net_device *net_dev)
 {
 	struct sis900_private *sis_priv = netdev_priv(net_dev);
-	long ioaddr = net_dev->base_addr;
+	void __iomem *ioaddr = sis_priv->ioaddr;
 	unsigned int  entry;
 	unsigned long flags;
 	unsigned int  index_cur_tx, index_dirty_tx;
@@ -1608,7 +1633,7 @@ sis900_start_xmit(struct sk_buff *skb, struct net_device *net_dev)
 	sis_priv->tx_ring[entry].bufptr = pci_map_single(sis_priv->pci_dev,
 		skb->data, skb->len, PCI_DMA_TODEVICE);
 	sis_priv->tx_ring[entry].cmdsts = (OWN | skb->len);
-	outl(TxENA | inl(ioaddr + cr), ioaddr + cr);
+	sw32(cr, TxENA | sr32(cr));
 
 	sis_priv->cur_tx ++;
 	index_cur_tx = sis_priv->cur_tx;
@@ -1654,14 +1679,14 @@ static irqreturn_t sis900_interrupt(int irq, void *dev_instance)
 	struct net_device *net_dev = dev_instance;
 	struct sis900_private *sis_priv = netdev_priv(net_dev);
 	int boguscnt = max_interrupt_work;
-	long ioaddr = net_dev->base_addr;
+	void __iomem *ioaddr = sis_priv->ioaddr;
 	u32 status;
 	unsigned int handled = 0;
 
 	spin_lock (&sis_priv->lock);
 
 	do {
-		status = inl(ioaddr + isr);
+		status = sr32(isr);
 
 		if ((status & (HIBERR|TxURN|TxERR|TxIDLE|RxORN|RxERR|RxOK)) == 0)
 			/* nothing intresting happened */
@@ -1696,7 +1721,7 @@ static irqreturn_t sis900_interrupt(int irq, void *dev_instance)
 	if(netif_msg_intr(sis_priv))
 		printk(KERN_DEBUG "%s: exiting interrupt, "
 		       "interrupt status = 0x%#8.8x.\n",
-		       net_dev->name, inl(ioaddr + isr));
+		       net_dev->name, sr32(isr));
 
 	spin_unlock (&sis_priv->lock);
 	return IRQ_RETVAL(handled);
@@ -1715,7 +1740,7 @@ static irqreturn_t sis900_interrupt(int irq, void *dev_instance)
 static int sis900_rx(struct net_device *net_dev)
 {
 	struct sis900_private *sis_priv = netdev_priv(net_dev);
-	long ioaddr = net_dev->base_addr;
+	void __iomem *ioaddr = sis_priv->ioaddr;
 	unsigned int entry = sis_priv->cur_rx % NUM_RX_DESC;
 	u32 rx_status = sis_priv->rx_ring[entry].cmdsts;
 	int rx_work_limit;
@@ -1847,7 +1872,7 @@ refill_rx_ring:
 		}
 	}
 	/* re-enable the potentially idle receive state matchine */
-	outl(RxENA | inl(ioaddr + cr), ioaddr + cr );
+	sw32(cr , RxENA | sr32(cr));
 
 	return 0;
 }
@@ -1932,31 +1957,31 @@ static void sis900_finish_xmit (struct net_device *net_dev)
 
 static int sis900_close(struct net_device *net_dev)
 {
-	long ioaddr = net_dev->base_addr;
 	struct sis900_private *sis_priv = netdev_priv(net_dev);
+	struct pci_dev *pdev = sis_priv->pci_dev;
+	void __iomem *ioaddr = sis_priv->ioaddr;
 	struct sk_buff *skb;
 	int i;
 
 	netif_stop_queue(net_dev);
 
 	/* Disable interrupts by clearing the interrupt mask. */
-	outl(0x0000, ioaddr + imr);
-	outl(0x0000, ioaddr + ier);
+	sw32(imr, 0x0000);
+	sw32(ier, 0x0000);
 
 	/* Stop the chip's Tx and Rx Status Machine */
-	outl(RxDIS | TxDIS | inl(ioaddr + cr), ioaddr + cr);
+	sw32(cr, RxDIS | TxDIS | sr32(cr));
 
 	del_timer(&sis_priv->timer);
 
-	free_irq(net_dev->irq, net_dev);
+	free_irq(pdev->irq, net_dev);
 
 	/* Free Tx and RX skbuff */
 	for (i = 0; i < NUM_RX_DESC; i++) {
 		skb = sis_priv->rx_skbuff[i];
 		if (skb) {
-			pci_unmap_single(sis_priv->pci_dev,
-				sis_priv->rx_ring[i].bufptr,
-				RX_BUF_SIZE, PCI_DMA_FROMDEVICE);
+			pci_unmap_single(pdev, sis_priv->rx_ring[i].bufptr,
+					 RX_BUF_SIZE, PCI_DMA_FROMDEVICE);
 			dev_kfree_skb(skb);
 			sis_priv->rx_skbuff[i] = NULL;
 		}
@@ -1964,9 +1989,8 @@ static int sis900_close(struct net_device *net_dev)
 	for (i = 0; i < NUM_TX_DESC; i++) {
 		skb = sis_priv->tx_skbuff[i];
 		if (skb) {
-			pci_unmap_single(sis_priv->pci_dev,
-				sis_priv->tx_ring[i].bufptr, skb->len,
-				PCI_DMA_TODEVICE);
+			pci_unmap_single(pdev, sis_priv->tx_ring[i].bufptr,
+					 skb->len, PCI_DMA_TODEVICE);
 			dev_kfree_skb(skb);
 			sis_priv->tx_skbuff[i] = NULL;
 		}
@@ -2055,14 +2079,14 @@ static int sis900_nway_reset(struct net_device *net_dev)
 static int sis900_set_wol(struct net_device *net_dev, struct ethtool_wolinfo *wol)
 {
 	struct sis900_private *sis_priv = netdev_priv(net_dev);
-	long pmctrl_addr = net_dev->base_addr + pmctrl;
+	void __iomem *ioaddr = sis_priv->ioaddr;
 	u32 cfgpmcsr = 0, pmctrl_bits = 0;
 
 	if (wol->wolopts == 0) {
 		pci_read_config_dword(sis_priv->pci_dev, CFGPMCSR, &cfgpmcsr);
 		cfgpmcsr &= ~PME_EN;
 		pci_write_config_dword(sis_priv->pci_dev, CFGPMCSR, cfgpmcsr);
-		outl(pmctrl_bits, pmctrl_addr);
+		sw32(pmctrl, pmctrl_bits);
 		if (netif_msg_wol(sis_priv))
 			printk(KERN_DEBUG "%s: Wake on LAN disabled\n", net_dev->name);
 		return 0;
@@ -2077,7 +2101,7 @@ static int sis900_set_wol(struct net_device *net_dev, struct ethtool_wolinfo *wo
 	if (wol->wolopts & WAKE_PHY)
 		pmctrl_bits |= LINKON;
 
-	outl(pmctrl_bits, pmctrl_addr);
+	sw32(pmctrl, pmctrl_bits);
 
 	pci_read_config_dword(sis_priv->pci_dev, CFGPMCSR, &cfgpmcsr);
 	cfgpmcsr |= PME_EN;
@@ -2090,10 +2114,11 @@ static int sis900_set_wol(struct net_device *net_dev, struct ethtool_wolinfo *wo
 
 static void sis900_get_wol(struct net_device *net_dev, struct ethtool_wolinfo *wol)
 {
-	long pmctrl_addr = net_dev->base_addr + pmctrl;
+	struct sis900_private *sp = netdev_priv(net_dev);
+	void __iomem *ioaddr = sp->ioaddr;
 	u32 pmctrl_bits;
 
-	pmctrl_bits = inl(pmctrl_addr);
+	pmctrl_bits = sr32(pmctrl);
 	if (pmctrl_bits & MAGICPKT)
 		wol->wolopts |= WAKE_MAGIC;
 	if (pmctrl_bits & LINKON)
@@ -2279,8 +2304,8 @@ static inline u16 sis900_mcast_bitnr(u8 *addr, u8 revision)
 
 static void set_rx_mode(struct net_device *net_dev)
 {
-	long ioaddr = net_dev->base_addr;
 	struct sis900_private *sis_priv = netdev_priv(net_dev);
+	void __iomem *ioaddr = sis_priv->ioaddr;
 	u16 mc_filter[16] = {0};	/* 256/128 bits multicast hash table */
 	int i, table_entries;
 	u32 rx_mode;
@@ -2322,24 +2347,24 @@ static void set_rx_mode(struct net_device *net_dev)
 	/* update Multicast Hash Table in Receive Filter */
 	for (i = 0; i < table_entries; i++) {
                 /* why plus 0x04 ??, That makes the correct value for hash table. */
-		outl((u32)(0x00000004+i) << RFADDR_shift, ioaddr + rfcr);
-		outl(mc_filter[i], ioaddr + rfdr);
+		sw32(rfcr, (u32)(0x00000004 + i) << RFADDR_shift);
+		sw32(rfdr, mc_filter[i]);
 	}
 
-	outl(RFEN | rx_mode, ioaddr + rfcr);
+	sw32(rfcr, RFEN | rx_mode);
 
 	/* sis900 is capable of looping back packets at MAC level for
 	 * debugging purpose */
 	if (net_dev->flags & IFF_LOOPBACK) {
 		u32 cr_saved;
 		/* We must disable Tx/Rx before setting loopback mode */
-		cr_saved = inl(ioaddr + cr);
-		outl(cr_saved | TxDIS | RxDIS, ioaddr + cr);
+		cr_saved = sr32(cr);
+		sw32(cr, cr_saved | TxDIS | RxDIS);
 		/* enable loopback */
-		outl(inl(ioaddr + txcfg) | TxMLB, ioaddr + txcfg);
-		outl(inl(ioaddr + rxcfg) | RxATX, ioaddr + rxcfg);
+		sw32(txcfg, sr32(txcfg) | TxMLB);
+		sw32(rxcfg, sr32(rxcfg) | RxATX);
 		/* restore cr */
-		outl(cr_saved, ioaddr + cr);
+		sw32(cr, cr_saved);
 	}
 }
 
@@ -2355,26 +2380,25 @@ static void set_rx_mode(struct net_device *net_dev)
 static void sis900_reset(struct net_device *net_dev)
 {
 	struct sis900_private *sis_priv = netdev_priv(net_dev);
-	long ioaddr = net_dev->base_addr;
-	int i = 0;
+	void __iomem *ioaddr = sis_priv->ioaddr;
 	u32 status = TxRCMP | RxRCMP;
+	int i;
 
-	outl(0, ioaddr + ier);
-	outl(0, ioaddr + imr);
-	outl(0, ioaddr + rfcr);
+	sw32(ier, 0);
+	sw32(imr, 0);
+	sw32(rfcr, 0);
 
-	outl(RxRESET | TxRESET | RESET | inl(ioaddr + cr), ioaddr + cr);
+	sw32(cr, RxRESET | TxRESET | RESET | sr32(cr));
 
 	/* Check that the chip has finished the reset. */
-	while (status && (i++ < 1000)) {
-		status ^= (inl(isr + ioaddr) & status);
-	}
+	for (i = 0; status && (i < 1000); i++)
+		status ^= sr32(isr) & status;
 
-	if( (sis_priv->chipset_rev >= SIS635A_900_REV) ||
-			(sis_priv->chipset_rev == SIS900B_900_REV) )
-		outl(PESEL | RND_CNT, ioaddr + cfg);
+	if (sis_priv->chipset_rev >= SIS635A_900_REV ||
+	    sis_priv->chipset_rev == SIS900B_900_REV)
+		sw32(cfg, PESEL | RND_CNT);
 	else
-		outl(PESEL, ioaddr + cfg);
+		sw32(cfg, PESEL);
 }
 
 /**
@@ -2388,10 +2412,12 @@ static void __devexit sis900_remove(struct pci_dev *pci_dev)
 {
 	struct net_device *net_dev = pci_get_drvdata(pci_dev);
 	struct sis900_private *sis_priv = netdev_priv(net_dev);
-	struct mii_phy *phy = NULL;
+
+	unregister_netdev(net_dev);
 
 	while (sis_priv->first_mii) {
-		phy = sis_priv->first_mii;
+		struct mii_phy *phy = sis_priv->first_mii;
+
 		sis_priv->first_mii = phy->next;
 		kfree(phy);
 	}
@@ -2400,7 +2426,7 @@ static void __devexit sis900_remove(struct pci_dev *pci_dev)
 		sis_priv->rx_ring_dma);
 	pci_free_consistent(pci_dev, TX_TOTAL_SIZE, sis_priv->tx_ring,
 		sis_priv->tx_ring_dma);
-	unregister_netdev(net_dev);
+	pci_iounmap(pci_dev, sis_priv->ioaddr);
 	free_netdev(net_dev);
 	pci_release_regions(pci_dev);
 	pci_set_drvdata(pci_dev, NULL);
@@ -2411,7 +2437,8 @@ static void __devexit sis900_remove(struct pci_dev *pci_dev)
 static int sis900_suspend(struct pci_dev *pci_dev, pm_message_t state)
 {
 	struct net_device *net_dev = pci_get_drvdata(pci_dev);
-	long ioaddr = net_dev->base_addr;
+	struct sis900_private *sis_priv = netdev_priv(net_dev);
+	void __iomem *ioaddr = sis_priv->ioaddr;
 
 	if(!netif_running(net_dev))
 		return 0;
@@ -2420,7 +2447,7 @@ static int sis900_suspend(struct pci_dev *pci_dev, pm_message_t state)
 	netif_device_detach(net_dev);
 
 	/* Stop the chip's Tx and Rx Status Machine */
-	outl(RxDIS | TxDIS | inl(ioaddr + cr), ioaddr + cr);
+	sw32(cr, RxDIS | TxDIS | sr32(cr));
 
 	pci_set_power_state(pci_dev, PCI_D3hot);
 	pci_save_state(pci_dev);
@@ -2432,7 +2459,7 @@ static int sis900_resume(struct pci_dev *pci_dev)
 {
 	struct net_device *net_dev = pci_get_drvdata(pci_dev);
 	struct sis900_private *sis_priv = netdev_priv(net_dev);
-	long ioaddr = net_dev->base_addr;
+	void __iomem *ioaddr = sis_priv->ioaddr;
 
 	if(!netif_running(net_dev))
 		return 0;
@@ -2453,9 +2480,9 @@ static int sis900_resume(struct pci_dev *pci_dev)
 	sis900_set_mode(ioaddr, HW_SPEED_10_MBPS, FDX_CAPABLE_HALF_SELECTED);
 
 	/* Enable all known interrupts by setting the interrupt mask. */
-	outl((RxSOVR|RxORN|RxERR|RxOK|TxURN|TxERR|TxIDLE), ioaddr + imr);
-	outl(RxENA | inl(ioaddr + cr), ioaddr + cr);
-	outl(IE, ioaddr + ier);
+	sw32(imr, RxSOVR | RxORN | RxERR | RxOK | TxURN | TxERR | TxIDLE);
+	sw32(cr, RxENA | sr32(cr));
+	sw32(ier, IE);
 
 	sis900_check_mode(net_dev, sis_priv->mii);
 
diff --git a/drivers/net/ethernet/smsc/epic100.c b/drivers/net/ethernet/smsc/epic100.c
index 2a662e6112e..d01e59c348a 100644
--- a/drivers/net/ethernet/smsc/epic100.c
+++ b/drivers/net/ethernet/smsc/epic100.c
@@ -146,6 +146,12 @@ enum chip_capability_flags { MII_PWRDWN=1, TYPE2_INTR=2, NO_MII=4 };
 #define EPIC_TOTAL_SIZE 0x100
 #define USE_IO_OPS 1
 
+#ifdef USE_IO_OPS
+#define EPIC_BAR	0
+#else
+#define EPIC_BAR	1
+#endif
+
 typedef enum {
 	SMSC_83C170_0,
 	SMSC_83C170,
@@ -176,21 +182,11 @@ static DEFINE_PCI_DEVICE_TABLE(epic_pci_tbl) = {
 };
 MODULE_DEVICE_TABLE (pci, epic_pci_tbl);
 
-
-#ifndef USE_IO_OPS
-#undef inb
-#undef inw
-#undef inl
-#undef outb
-#undef outw
-#undef outl
-#define inb readb
-#define inw readw
-#define inl readl
-#define outb writeb
-#define outw writew
-#define outl writel
-#endif
+#define ew16(reg, val)	iowrite16(val, ioaddr + (reg))
+#define ew32(reg, val)	iowrite32(val, ioaddr + (reg))
+#define er8(reg)	ioread8(ioaddr + (reg))
+#define er16(reg)	ioread16(ioaddr + (reg))
+#define er32(reg)	ioread32(ioaddr + (reg))
 
 /* Offsets to registers, using the (ugh) SMC names. */
 enum epic_registers {
@@ -275,6 +271,7 @@ struct epic_private {
 	u32 irq_mask;
 	unsigned int rx_buf_sz;				/* Based on MTU+slack. */
 
+	void __iomem *ioaddr;
 	struct pci_dev *pci_dev;			/* PCI bus location. */
 	int chip_id, chip_flags;
 
@@ -290,7 +287,7 @@ struct epic_private {
 };
 
 static int epic_open(struct net_device *dev);
-static int read_eeprom(long ioaddr, int location);
+static int read_eeprom(struct epic_private *, int);
 static int mdio_read(struct net_device *dev, int phy_id, int location);
 static void mdio_write(struct net_device *dev, int phy_id, int loc, int val);
 static void epic_restart(struct net_device *dev);
@@ -321,11 +318,11 @@ static const struct net_device_ops epic_netdev_ops = {
 	.ndo_validate_addr	= eth_validate_addr,
 };
 
-static int __devinit epic_init_one (struct pci_dev *pdev,
-				    const struct pci_device_id *ent)
+static int __devinit epic_init_one(struct pci_dev *pdev,
+				   const struct pci_device_id *ent)
 {
 	static int card_idx = -1;
-	long ioaddr;
+	void __iomem *ioaddr;
 	int chip_idx = (int) ent->driver_data;
 	int irq;
 	struct net_device *dev;
@@ -368,19 +365,15 @@ static int __devinit epic_init_one (struct pci_dev *pdev,
 
 	SET_NETDEV_DEV(dev, &pdev->dev);
 
-#ifdef USE_IO_OPS
-	ioaddr = pci_resource_start (pdev, 0);
-#else
-	ioaddr = pci_resource_start (pdev, 1);
-	ioaddr = (long) pci_ioremap_bar(pdev, 1);
+	ioaddr = pci_iomap(pdev, EPIC_BAR, 0);
 	if (!ioaddr) {
 		dev_err(&pdev->dev, "ioremap failed\n");
 		goto err_out_free_netdev;
 	}
-#endif
 
 	pci_set_drvdata(pdev, dev);
 	ep = netdev_priv(dev);
+	ep->ioaddr = ioaddr;
 	ep->mii.dev = dev;
 	ep->mii.mdio_read = mdio_read;
 	ep->mii.mdio_write = mdio_write;
@@ -409,34 +402,31 @@ static int __devinit epic_init_one (struct pci_dev *pdev,
 			duplex = full_duplex[card_idx];
 	}
 
-	dev->base_addr = ioaddr;
-	dev->irq = irq;
-
 	spin_lock_init(&ep->lock);
 	spin_lock_init(&ep->napi_lock);
 	ep->reschedule_in_poll = 0;
 
 	/* Bring the chip out of low-power mode. */
-	outl(0x4200, ioaddr + GENCTL);
+	ew32(GENCTL, 0x4200);
 	/* Magic?!  If we don't set this bit the MII interface won't work. */
 	/* This magic is documented in SMSC app note 7.15 */
 	for (i = 16; i > 0; i--)
-		outl(0x0008, ioaddr + TEST1);
+		ew32(TEST1, 0x0008);
 
 	/* Turn on the MII transceiver. */
-	outl(0x12, ioaddr + MIICfg);
+	ew32(MIICfg, 0x12);
 	if (chip_idx == 1)
-		outl((inl(ioaddr + NVCTL) & ~0x003C) | 0x4800, ioaddr + NVCTL);
-	outl(0x0200, ioaddr + GENCTL);
+		ew32(NVCTL, (er32(NVCTL) & ~0x003c) | 0x4800);
+	ew32(GENCTL, 0x0200);
 
 	/* Note: the '175 does not have a serial EEPROM. */
 	for (i = 0; i < 3; i++)
-		((__le16 *)dev->dev_addr)[i] = cpu_to_le16(inw(ioaddr + LAN0 + i*4));
+		((__le16 *)dev->dev_addr)[i] = cpu_to_le16(er16(LAN0 + i*4));
 
 	if (debug > 2) {
 		dev_printk(KERN_DEBUG, &pdev->dev, "EEPROM contents:\n");
 		for (i = 0; i < 64; i++)
-			printk(" %4.4x%s", read_eeprom(ioaddr, i),
+			printk(" %4.4x%s", read_eeprom(ep, i),
 				   i % 16 == 15 ? "\n" : "");
 	}
 
@@ -481,8 +471,8 @@ static int __devinit epic_init_one (struct pci_dev *pdev,
 
 	/* Turn off the MII xcvr (175 only!), leave the chip in low-power mode. */
 	if (ep->chip_flags & MII_PWRDWN)
-		outl(inl(ioaddr + NVCTL) & ~0x483C, ioaddr + NVCTL);
-	outl(0x0008, ioaddr + GENCTL);
+		ew32(NVCTL, er32(NVCTL) & ~0x483c);
+	ew32(GENCTL, 0x0008);
 
 	/* The lower four bits are the media type. */
 	if (duplex) {
@@ -501,8 +491,9 @@ static int __devinit epic_init_one (struct pci_dev *pdev,
 	if (ret < 0)
 		goto err_out_unmap_rx;
 
-	printk(KERN_INFO "%s: %s at %#lx, IRQ %d, %pM\n",
-	       dev->name, pci_id_tbl[chip_idx].name, ioaddr, dev->irq,
+	printk(KERN_INFO "%s: %s at %lx, IRQ %d, %pM\n",
+	       dev->name, pci_id_tbl[chip_idx].name,
+	       (long)pci_resource_start(pdev, EPIC_BAR), pdev->irq,
 	       dev->dev_addr);
 
 out:
@@ -513,10 +504,8 @@ err_out_unmap_rx:
 err_out_unmap_tx:
 	pci_free_consistent(pdev, TX_TOTAL_SIZE, ep->tx_ring, ep->tx_ring_dma);
 err_out_iounmap:
-#ifndef USE_IO_OPS
-	iounmap(ioaddr);
+	pci_iounmap(pdev, ioaddr);
 err_out_free_netdev:
-#endif
 	free_netdev(dev);
 err_out_free_res:
 	pci_release_regions(pdev);
@@ -540,7 +529,7 @@ err_out_disable:
    This serves to flush the operation to the PCI bus.
  */
 
-#define eeprom_delay()	inl(ee_addr)
+#define eeprom_delay()	er32(EECTL)
 
 /* The EEPROM commands include the alway-set leading bit. */
 #define EE_WRITE_CMD	(5 << 6)
@@ -550,67 +539,67 @@ err_out_disable:
 
 static void epic_disable_int(struct net_device *dev, struct epic_private *ep)
 {
-	long ioaddr = dev->base_addr;
+	void __iomem *ioaddr = ep->ioaddr;
 
-	outl(0x00000000, ioaddr + INTMASK);
+	ew32(INTMASK, 0x00000000);
 }
 
-static inline void __epic_pci_commit(long ioaddr)
+static inline void __epic_pci_commit(void __iomem *ioaddr)
 {
 #ifndef USE_IO_OPS
-	inl(ioaddr + INTMASK);
+	er32(INTMASK);
 #endif
 }
 
 static inline void epic_napi_irq_off(struct net_device *dev,
 				     struct epic_private *ep)
 {
-	long ioaddr = dev->base_addr;
+	void __iomem *ioaddr = ep->ioaddr;
 
-	outl(ep->irq_mask & ~EpicNapiEvent, ioaddr + INTMASK);
+	ew32(INTMASK, ep->irq_mask & ~EpicNapiEvent);
 	__epic_pci_commit(ioaddr);
 }
 
 static inline void epic_napi_irq_on(struct net_device *dev,
 				    struct epic_private *ep)
 {
-	long ioaddr = dev->base_addr;
+	void __iomem *ioaddr = ep->ioaddr;
 
 	/* No need to commit possible posted write */
-	outl(ep->irq_mask | EpicNapiEvent, ioaddr + INTMASK);
+	ew32(INTMASK, ep->irq_mask | EpicNapiEvent);
 }
 
-static int __devinit read_eeprom(long ioaddr, int location)
+static int __devinit read_eeprom(struct epic_private *ep, int location)
 {
+	void __iomem *ioaddr = ep->ioaddr;
 	int i;
 	int retval = 0;
-	long ee_addr = ioaddr + EECTL;
 	int read_cmd = location |
-		(inl(ee_addr) & 0x40 ? EE_READ64_CMD : EE_READ256_CMD);
+		(er32(EECTL) & 0x40 ? EE_READ64_CMD : EE_READ256_CMD);
 
-	outl(EE_ENB & ~EE_CS, ee_addr);
-	outl(EE_ENB, ee_addr);
+	ew32(EECTL, EE_ENB & ~EE_CS);
+	ew32(EECTL, EE_ENB);
 
 	/* Shift the read command bits out. */
 	for (i = 12; i >= 0; i--) {
 		short dataval = (read_cmd & (1 << i)) ? EE_WRITE_1 : EE_WRITE_0;
-		outl(EE_ENB | dataval, ee_addr);
+		ew32(EECTL, EE_ENB | dataval);
 		eeprom_delay();
-		outl(EE_ENB | dataval | EE_SHIFT_CLK, ee_addr);
+		ew32(EECTL, EE_ENB | dataval | EE_SHIFT_CLK);
 		eeprom_delay();
 	}
-	outl(EE_ENB, ee_addr);
+	ew32(EECTL, EE_ENB);
 
 	for (i = 16; i > 0; i--) {
-		outl(EE_ENB | EE_SHIFT_CLK, ee_addr);
+		ew32(EECTL, EE_ENB | EE_SHIFT_CLK);
 		eeprom_delay();
-		retval = (retval << 1) | ((inl(ee_addr) & EE_DATA_READ) ? 1 : 0);
-		outl(EE_ENB, ee_addr);
+		retval = (retval << 1) | ((er32(EECTL) & EE_DATA_READ) ? 1 : 0);
+		ew32(EECTL, EE_ENB);
 		eeprom_delay();
 	}
 
 	/* Terminate the EEPROM access. */
-	outl(EE_ENB & ~EE_CS, ee_addr);
+	ew32(EECTL, EE_ENB & ~EE_CS);
 	return retval;
 }
 
@@ -618,22 +607,23 @@ static int __devinit read_eeprom(long ioaddr, int location)
 #define MII_WRITEOP		2
 static int mdio_read(struct net_device *dev, int phy_id, int location)
 {
-	long ioaddr = dev->base_addr;
+	struct epic_private *ep = netdev_priv(dev);
+	void __iomem *ioaddr = ep->ioaddr;
 	int read_cmd = (phy_id << 9) | (location << 4) | MII_READOP;
 	int i;
 
-	outl(read_cmd, ioaddr + MIICtrl);
+	ew32(MIICtrl, read_cmd);
 	/* Typical operation takes 25 loops. */
 	for (i = 400; i > 0; i--) {
 		barrier();
-		if ((inl(ioaddr + MIICtrl) & MII_READOP) == 0) {
+		if ((er32(MIICtrl) & MII_READOP) == 0) {
 			/* Work around read failure bug. */
 			if (phy_id == 1 && location < 6 &&
-			    inw(ioaddr + MIIData) == 0xffff) {
-				outl(read_cmd, ioaddr + MIICtrl);
+			    er16(MIIData) == 0xffff) {
+				ew32(MIICtrl, read_cmd);
 				continue;
 			}
-			return inw(ioaddr + MIIData);
+			return er16(MIIData);
 		}
 	}
 	return 0xffff;
@@ -641,14 +631,15 @@ static int mdio_read(struct net_device *dev, int phy_id, int location)
 
 static void mdio_write(struct net_device *dev, int phy_id, int loc, int value)
 {
-	long ioaddr = dev->base_addr;
+	struct epic_private *ep = netdev_priv(dev);
+	void __iomem *ioaddr = ep->ioaddr;
 	int i;
 
-	outw(value, ioaddr + MIIData);
-	outl((phy_id << 9) | (loc << 4) | MII_WRITEOP, ioaddr + MIICtrl);
+	ew16(MIIData, value);
+	ew32(MIICtrl, (phy_id << 9) | (loc << 4) | MII_WRITEOP);
 	for (i = 10000; i > 0; i--) {
 		barrier();
-		if ((inl(ioaddr + MIICtrl) & MII_WRITEOP) == 0)
+		if ((er32(MIICtrl) & MII_WRITEOP) == 0)
 			break;
 	}
 }
@@ -657,25 +648,26 @@ static void mdio_write(struct net_device *dev, int phy_id, int loc, int value)
 static int epic_open(struct net_device *dev)
 {
 	struct epic_private *ep = netdev_priv(dev);
-	long ioaddr = dev->base_addr;
-	int i;
-	int retval;
+	void __iomem *ioaddr = ep->ioaddr;
+	const int irq = ep->pci_dev->irq;
+	int rc, i;
 
 	/* Soft reset the chip. */
-	outl(0x4001, ioaddr + GENCTL);
+	ew32(GENCTL, 0x4001);
 
 	napi_enable(&ep->napi);
-	if ((retval = request_irq(dev->irq, epic_interrupt, IRQF_SHARED, dev->name, dev))) {
+	rc = request_irq(irq, epic_interrupt, IRQF_SHARED, dev->name, dev);
+	if (rc) {
 		napi_disable(&ep->napi);
-		return retval;
+		return rc;
 	}
 
 	epic_init_ring(dev);
 
-	outl(0x4000, ioaddr + GENCTL);
+	ew32(GENCTL, 0x4000);
 	/* This magic is documented in SMSC app note 7.15 */
 	for (i = 16; i > 0; i--)
-		outl(0x0008, ioaddr + TEST1);
+		ew32(TEST1, 0x0008);
 
 	/* Pull the chip out of low-power mode, enable interrupts, and set for
 	   PCI read multiple.  The MIIcfg setting and strange write order are
@@ -683,29 +675,29 @@ static int epic_open(struct net_device *dev)
 	   wiring on the Ositech CardBus card.
 	*/
 #if 0
-	outl(dev->if_port == 1 ? 0x13 : 0x12, ioaddr + MIICfg);
+	ew32(MIICfg, dev->if_port == 1 ? 0x13 : 0x12);
 #endif
 	if (ep->chip_flags & MII_PWRDWN)
-		outl((inl(ioaddr + NVCTL) & ~0x003C) | 0x4800, ioaddr + NVCTL);
+		ew32(NVCTL, (er32(NVCTL) & ~0x003c) | 0x4800);
 
 	/* Tell the chip to byteswap descriptors on big-endian hosts */
 #ifdef __BIG_ENDIAN
-	outl(0x4432 | (RX_FIFO_THRESH<<8), ioaddr + GENCTL);
-	inl(ioaddr + GENCTL);
-	outl(0x0432 | (RX_FIFO_THRESH<<8), ioaddr + GENCTL);
+	ew32(GENCTL, 0x4432 | (RX_FIFO_THRESH << 8));
+	er32(GENCTL);
+	ew32(GENCTL, 0x0432 | (RX_FIFO_THRESH << 8));
 #else
-	outl(0x4412 | (RX_FIFO_THRESH<<8), ioaddr + GENCTL);
-	inl(ioaddr + GENCTL);
-	outl(0x0412 | (RX_FIFO_THRESH<<8), ioaddr + GENCTL);
+	ew32(GENCTL, 0x4412 | (RX_FIFO_THRESH << 8));
+	er32(GENCTL);
+	ew32(GENCTL, 0x0412 | (RX_FIFO_THRESH << 8));
 #endif
 
 	udelay(20); /* Looks like EPII needs that if you want reliable RX init. FIXME: pci posting bug? */
 
 	for (i = 0; i < 3; i++)
-		outl(le16_to_cpu(((__le16*)dev->dev_addr)[i]), ioaddr + LAN0 + i*4);
+		ew32(LAN0 + i*4, le16_to_cpu(((__le16*)dev->dev_addr)[i]));
 
 	ep->tx_threshold = TX_FIFO_THRESH;
-	outl(ep->tx_threshold, ioaddr + TxThresh);
+	ew32(TxThresh, ep->tx_threshold);
 
 	if (media2miictl[dev->if_port & 15]) {
 		if (ep->mii_phy_cnt)
@@ -731,26 +723,27 @@ static int epic_open(struct net_device *dev)
 		}
 	}
 
-	outl(ep->mii.full_duplex ? 0x7F : 0x79, ioaddr + TxCtrl);
-	outl(ep->rx_ring_dma, ioaddr + PRxCDAR);
-	outl(ep->tx_ring_dma, ioaddr + PTxCDAR);
+	ew32(TxCtrl, ep->mii.full_duplex ? 0x7f : 0x79);
+	ew32(PRxCDAR, ep->rx_ring_dma);
+	ew32(PTxCDAR, ep->tx_ring_dma);
 
 	/* Start the chip's Rx process. */
 	set_rx_mode(dev);
-	outl(StartRx | RxQueued, ioaddr + COMMAND);
+	ew32(COMMAND, StartRx | RxQueued);
 
 	netif_start_queue(dev);
 
 	/* Enable interrupts by setting the interrupt mask. */
-	outl((ep->chip_flags & TYPE2_INTR ? PCIBusErr175 : PCIBusErr170)
-		 | CntFull | TxUnderrun
-		 | RxError | RxHeader | EpicNapiEvent, ioaddr + INTMASK);
-
-	if (debug > 1)
-		printk(KERN_DEBUG "%s: epic_open() ioaddr %lx IRQ %d status %4.4x "
-			   "%s-duplex.\n",
-			   dev->name, ioaddr, dev->irq, (int)inl(ioaddr + GENCTL),
-			   ep->mii.full_duplex ? "full" : "half");
+	ew32(INTMASK, RxError | RxHeader | EpicNapiEvent | CntFull |
+	     ((ep->chip_flags & TYPE2_INTR) ? PCIBusErr175 : PCIBusErr170) |
+	     TxUnderrun);
+
+	if (debug > 1) {
+		printk(KERN_DEBUG "%s: epic_open() ioaddr %p IRQ %d "
+		       "status %4.4x %s-duplex.\n",
+		       dev->name, ioaddr, irq, er32(GENCTL),
+		       ep->mii.full_duplex ? "full" : "half");
+	}
 
 	/* Set the timer to switch to check for link beat and perhaps switch
 	   to an alternate media type. */
@@ -760,27 +753,29 @@ static int epic_open(struct net_device *dev)
 	ep->timer.function = epic_timer;				/* timer handler */
 	add_timer(&ep->timer);
 
-	return 0;
+	return rc;
 }
 
 /* Reset the chip to recover from a PCI transaction error.
    This may occur at interrupt time. */
 static void epic_pause(struct net_device *dev)
 {
-	long ioaddr = dev->base_addr;
+	struct net_device_stats *stats = &dev->stats;
+	struct epic_private *ep = netdev_priv(dev);
+	void __iomem *ioaddr = ep->ioaddr;
 
 	netif_stop_queue (dev);
 
 	/* Disable interrupts by clearing the interrupt mask. */
-	outl(0x00000000, ioaddr + INTMASK);
+	ew32(INTMASK, 0x00000000);
 	/* Stop the chip's Tx and Rx DMA processes. */
-	outw(StopRx | StopTxDMA | StopRxDMA, ioaddr + COMMAND);
+	ew16(COMMAND, StopRx | StopTxDMA | StopRxDMA);
 
 	/* Update the error counts. */
-	if (inw(ioaddr + COMMAND) != 0xffff) {
-		dev->stats.rx_missed_errors += inb(ioaddr + MPCNT);
-		dev->stats.rx_frame_errors += inb(ioaddr + ALICNT);
-		dev->stats.rx_crc_errors += inb(ioaddr + CRCCNT);
+	if (er16(COMMAND) != 0xffff) {
+		stats->rx_missed_errors	+= er8(MPCNT);
+		stats->rx_frame_errors	+= er8(ALICNT);
+		stats->rx_crc_errors	+= er8(CRCCNT);
 	}
 
 	/* Remove the packets on the Rx queue. */
@@ -789,12 +784,12 @@ static void epic_pause(struct net_device *dev)
 
 static void epic_restart(struct net_device *dev)
 {
-	long ioaddr = dev->base_addr;
 	struct epic_private *ep = netdev_priv(dev);
+	void __iomem *ioaddr = ep->ioaddr;
 	int i;
 
 	/* Soft reset the chip. */
-	outl(0x4001, ioaddr + GENCTL);
+	ew32(GENCTL, 0x4001);
 
 	printk(KERN_DEBUG "%s: Restarting the EPIC chip, Rx %d/%d Tx %d/%d.\n",
 		   dev->name, ep->cur_rx, ep->dirty_rx, ep->dirty_tx, ep->cur_tx);
@@ -802,47 +797,46 @@ static void epic_restart(struct net_device *dev)
 
 	/* This magic is documented in SMSC app note 7.15 */
 	for (i = 16; i > 0; i--)
-		outl(0x0008, ioaddr + TEST1);
+		ew32(TEST1, 0x0008);
 
 #ifdef __BIG_ENDIAN
-	outl(0x0432 | (RX_FIFO_THRESH<<8), ioaddr + GENCTL);
+	ew32(GENCTL, 0x0432 | (RX_FIFO_THRESH << 8));
 #else
-	outl(0x0412 | (RX_FIFO_THRESH<<8), ioaddr + GENCTL);
+	ew32(GENCTL, 0x0412 | (RX_FIFO_THRESH << 8));
 #endif
-	outl(dev->if_port == 1 ? 0x13 : 0x12, ioaddr + MIICfg);
+	ew32(MIICfg, dev->if_port == 1 ? 0x13 : 0x12);
 	if (ep->chip_flags & MII_PWRDWN)
-		outl((inl(ioaddr + NVCTL) & ~0x003C) | 0x4800, ioaddr + NVCTL);
+		ew32(NVCTL, (er32(NVCTL) & ~0x003c) | 0x4800);
 
 	for (i = 0; i < 3; i++)
-		outl(le16_to_cpu(((__le16*)dev->dev_addr)[i]), ioaddr + LAN0 + i*4);
+		ew32(LAN0 + i*4, le16_to_cpu(((__le16*)dev->dev_addr)[i]));
 
 	ep->tx_threshold = TX_FIFO_THRESH;
-	outl(ep->tx_threshold, ioaddr + TxThresh);
-	outl(ep->mii.full_duplex ? 0x7F : 0x79, ioaddr + TxCtrl);
-	outl(ep->rx_ring_dma + (ep->cur_rx%RX_RING_SIZE)*
-		sizeof(struct epic_rx_desc), ioaddr + PRxCDAR);
-	outl(ep->tx_ring_dma + (ep->dirty_tx%TX_RING_SIZE)*
-		 sizeof(struct epic_tx_desc), ioaddr + PTxCDAR);
+	ew32(TxThresh, ep->tx_threshold);
+	ew32(TxCtrl, ep->mii.full_duplex ? 0x7f : 0x79);
+	ew32(PRxCDAR, ep->rx_ring_dma +
+	     (ep->cur_rx % RX_RING_SIZE) * sizeof(struct epic_rx_desc));
+	ew32(PTxCDAR, ep->tx_ring_dma +
+	     (ep->dirty_tx % TX_RING_SIZE) * sizeof(struct epic_tx_desc));
 
 	/* Start the chip's Rx process. */
 	set_rx_mode(dev);
-	outl(StartRx | RxQueued, ioaddr + COMMAND);
+	ew32(COMMAND, StartRx | RxQueued);
 
 	/* Enable interrupts by setting the interrupt mask. */
-	outl((ep->chip_flags & TYPE2_INTR ? PCIBusErr175 : PCIBusErr170)
-		 | CntFull | TxUnderrun
-		 | RxError | RxHeader | EpicNapiEvent, ioaddr + INTMASK);
+	ew32(INTMASK, RxError | RxHeader | EpicNapiEvent | CntFull |
+	     ((ep->chip_flags & TYPE2_INTR) ? PCIBusErr175 : PCIBusErr170) |
+	     TxUnderrun);
 
 	printk(KERN_DEBUG "%s: epic_restart() done, cmd status %4.4x, ctl %4.4x"
 		   " interrupt %4.4x.\n",
-		   dev->name, (int)inl(ioaddr + COMMAND), (int)inl(ioaddr + GENCTL),
-		   (int)inl(ioaddr + INTSTAT));
+		   dev->name, er32(COMMAND), er32(GENCTL), er32(INTSTAT));
 }
 
 static void check_media(struct net_device *dev)
 {
 	struct epic_private *ep = netdev_priv(dev);
-	long ioaddr = dev->base_addr;
+	void __iomem *ioaddr = ep->ioaddr;
 	int mii_lpa = ep->mii_phy_cnt ? mdio_read(dev, ep->phys[0], MII_LPA) : 0;
 	int negotiated = mii_lpa & ep->mii.advertising;
 	int duplex = (negotiated & 0x0100) || (negotiated & 0x01C0) == 0x0040;
@@ -856,7 +850,7 @@ static void check_media(struct net_device *dev)
 		printk(KERN_INFO "%s: Setting %s-duplex based on MII #%d link"
 			   " partner capability of %4.4x.\n", dev->name,
 			   ep->mii.full_duplex ? "full" : "half", ep->phys[0], mii_lpa);
-		outl(ep->mii.full_duplex ? 0x7F : 0x79, ioaddr + TxCtrl);
+		ew32(TxCtrl, ep->mii.full_duplex ? 0x7F : 0x79);
 	}
 }
 
@@ -864,16 +858,15 @@ static void epic_timer(unsigned long data)
 {
 	struct net_device *dev = (struct net_device *)data;
 	struct epic_private *ep = netdev_priv(dev);
-	long ioaddr = dev->base_addr;
+	void __iomem *ioaddr = ep->ioaddr;
 	int next_tick = 5*HZ;
 
 	if (debug > 3) {
 		printk(KERN_DEBUG "%s: Media monitor tick, Tx status %8.8x.\n",
-			   dev->name, (int)inl(ioaddr + TxSTAT));
+		       dev->name, er32(TxSTAT));
 		printk(KERN_DEBUG "%s: Other registers are IntMask %4.4x "
-			   "IntStatus %4.4x RxStatus %4.4x.\n",
-			   dev->name, (int)inl(ioaddr + INTMASK),
-			   (int)inl(ioaddr + INTSTAT), (int)inl(ioaddr + RxSTAT));
+		       "IntStatus %4.4x RxStatus %4.4x.\n", dev->name,
+		       er32(INTMASK), er32(INTSTAT), er32(RxSTAT));
 	}
 
 	check_media(dev);
@@ -885,23 +878,22 @@ static void epic_timer(unsigned long data)
 static void epic_tx_timeout(struct net_device *dev)
 {
 	struct epic_private *ep = netdev_priv(dev);
-	long ioaddr = dev->base_addr;
+	void __iomem *ioaddr = ep->ioaddr;
 
 	if (debug > 0) {
 		printk(KERN_WARNING "%s: Transmit timeout using MII device, "
-			   "Tx status %4.4x.\n",
-			   dev->name, (int)inw(ioaddr + TxSTAT));
+		       "Tx status %4.4x.\n", dev->name, er16(TxSTAT));
 		if (debug > 1) {
 			printk(KERN_DEBUG "%s: Tx indices: dirty_tx %d, cur_tx %d.\n",
 				   dev->name, ep->dirty_tx, ep->cur_tx);
 		}
 	}
-	if (inw(ioaddr + TxSTAT) & 0x10) {		/* Tx FIFO underflow. */
+	if (er16(TxSTAT) & 0x10) {		/* Tx FIFO underflow. */
 		dev->stats.tx_fifo_errors++;
-		outl(RestartTx, ioaddr + COMMAND);
+		ew32(COMMAND, RestartTx);
 	} else {
 		epic_restart(dev);
-		outl(TxQueued, dev->base_addr + COMMAND);
+		ew32(COMMAND, TxQueued);
 	}
 
 	dev->trans_start = jiffies; /* prevent tx timeout */
@@ -959,6 +951,7 @@ static void epic_init_ring(struct net_device *dev)
 static netdev_tx_t epic_start_xmit(struct sk_buff *skb, struct net_device *dev)
 {
 	struct epic_private *ep = netdev_priv(dev);
+	void __iomem *ioaddr = ep->ioaddr;
 	int entry, free_count;
 	u32 ctrl_word;
 	unsigned long flags;
@@ -999,13 +992,12 @@ static netdev_tx_t epic_start_xmit(struct sk_buff *skb, struct net_device *dev)
 
 	spin_unlock_irqrestore(&ep->lock, flags);
 	/* Trigger an immediate transmit demand. */
-	outl(TxQueued, dev->base_addr + COMMAND);
+	ew32(COMMAND, TxQueued);
 
 	if (debug > 4)
 		printk(KERN_DEBUG "%s: Queued Tx packet size %d to slot %d, "
-			   "flag %2.2x Tx status %8.8x.\n",
-			   dev->name, (int)skb->len, entry, ctrl_word,
-			   (int)inl(dev->base_addr + TxSTAT));
+		       "flag %2.2x Tx status %8.8x.\n", dev->name, skb->len,
+		       entry, ctrl_word, er32(TxSTAT));
 
 	return NETDEV_TX_OK;
 }
@@ -1086,18 +1078,17 @@ static irqreturn_t epic_interrupt(int irq, void *dev_instance)
 {
 	struct net_device *dev = dev_instance;
 	struct epic_private *ep = netdev_priv(dev);
-	long ioaddr = dev->base_addr;
+	void __iomem *ioaddr = ep->ioaddr;
 	unsigned int handled = 0;
 	int status;
 
-	status = inl(ioaddr + INTSTAT);
+	status = er32(INTSTAT);
 	/* Acknowledge all of the current interrupt sources ASAP. */
-	outl(status & EpicNormalEvent, ioaddr + INTSTAT);
+	ew32(INTSTAT, status & EpicNormalEvent);
 
 	if (debug > 4) {
 		printk(KERN_DEBUG "%s: Interrupt, status=%#8.8x new "
-				   "intstat=%#8.8x.\n", dev->name, status,
-				   (int)inl(ioaddr + INTSTAT));
+		       "intstat=%#8.8x.\n", dev->name, status, er32(INTSTAT));
 	}
 
 	if ((status & IntrSummary) == 0)
@@ -1118,19 +1109,21 @@ static irqreturn_t epic_interrupt(int irq, void *dev_instance)
 
 	/* Check uncommon events all at once. */
 	if (status & (CntFull | TxUnderrun | PCIBusErr170 | PCIBusErr175)) {
+		struct net_device_stats *stats = &dev->stats;
+
 		if (status == EpicRemoved)
 			goto out;
 
 		/* Always update the error counts to avoid overhead later. */
-		dev->stats.rx_missed_errors += inb(ioaddr + MPCNT);
-		dev->stats.rx_frame_errors += inb(ioaddr + ALICNT);
-		dev->stats.rx_crc_errors += inb(ioaddr + CRCCNT);
+		stats->rx_missed_errors	+= er8(MPCNT);
+		stats->rx_frame_errors	+= er8(ALICNT);
+		stats->rx_crc_errors	+= er8(CRCCNT);
 
 		if (status & TxUnderrun) { /* Tx FIFO underflow. */
-			dev->stats.tx_fifo_errors++;
-			outl(ep->tx_threshold += 128, ioaddr + TxThresh);
+			stats->tx_fifo_errors++;
+			ew32(TxThresh, ep->tx_threshold += 128);
 			/* Restart the transmit process. */
-			outl(RestartTx, ioaddr + COMMAND);
+			ew32(COMMAND, RestartTx);
 		}
 		if (status & PCIBusErr170) {
 			printk(KERN_ERR "%s: PCI Bus Error! status %4.4x.\n",
@@ -1139,7 +1132,7 @@ static irqreturn_t epic_interrupt(int irq, void *dev_instance)
 			epic_restart(dev);
 		}
 		/* Clear all error sources. */
-		outl(status & 0x7f18, ioaddr + INTSTAT);
+		ew32(INTSTAT, status & 0x7f18);
 	}
 
 out:
@@ -1248,17 +1241,17 @@ static int epic_rx(struct net_device *dev, int budget)
 
 static void epic_rx_err(struct net_device *dev, struct epic_private *ep)
 {
-	long ioaddr = dev->base_addr;
+	void __iomem *ioaddr = ep->ioaddr;
 	int status;
 
-	status = inl(ioaddr + INTSTAT);
+	status = er32(INTSTAT);
 
 	if (status == EpicRemoved)
 		return;
 	if (status & RxOverflow) 	/* Missed a Rx frame. */
 		dev->stats.rx_errors++;
 	if (status & (RxOverflow | RxFull))
-		outw(RxQueued, ioaddr + COMMAND);
+		ew16(COMMAND, RxQueued);
 }
 
 static int epic_poll(struct napi_struct *napi, int budget)
@@ -1266,7 +1259,7 @@ static int epic_poll(struct napi_struct *napi, int budget)
 	struct epic_private *ep = container_of(napi, struct epic_private, napi);
 	struct net_device *dev = ep->mii.dev;
 	int work_done = 0;
-	long ioaddr = dev->base_addr;
+	void __iomem *ioaddr = ep->ioaddr;
 
 rx_action:
 
@@ -1287,7 +1280,7 @@ rx_action:
 		more = ep->reschedule_in_poll;
 		if (!more) {
 			__napi_complete(napi);
-			outl(EpicNapiEvent, ioaddr + INTSTAT);
+			ew32(INTSTAT, EpicNapiEvent);
 			epic_napi_irq_on(dev, ep);
 		} else
 			ep->reschedule_in_poll--;
@@ -1303,8 +1296,9 @@ rx_action:
 
 static int epic_close(struct net_device *dev)
 {
-	long ioaddr = dev->base_addr;
 	struct epic_private *ep = netdev_priv(dev);
+	struct pci_dev *pdev = ep->pci_dev;
+	void __iomem *ioaddr = ep->ioaddr;
 	struct sk_buff *skb;
 	int i;
 
@@ -1313,13 +1307,13 @@ static int epic_close(struct net_device *dev)
 
 	if (debug > 1)
 		printk(KERN_DEBUG "%s: Shutting down ethercard, status was %2.2x.\n",
-			   dev->name, (int)inl(ioaddr + INTSTAT));
+		       dev->name, er32(INTSTAT));
 
 	del_timer_sync(&ep->timer);
 
 	epic_disable_int(dev, ep);
 
-	free_irq(dev->irq, dev);
+	free_irq(pdev->irq, dev);
 
 	epic_pause(dev);
 
@@ -1330,7 +1324,7 @@ static int epic_close(struct net_device *dev)
 		ep->rx_ring[i].rxstatus = 0;		/* Not owned by Epic chip. */
 		ep->rx_ring[i].buflength = 0;
 		if (skb) {
-			pci_unmap_single(ep->pci_dev, ep->rx_ring[i].bufaddr,
+			pci_unmap_single(pdev, ep->rx_ring[i].bufaddr,
 				 	 ep->rx_buf_sz, PCI_DMA_FROMDEVICE);
 			dev_kfree_skb(skb);
 		}
@@ -1341,26 +1335,28 @@ static int epic_close(struct net_device *dev)
 		ep->tx_skbuff[i] = NULL;
 		if (!skb)
 			continue;
-		pci_unmap_single(ep->pci_dev, ep->tx_ring[i].bufaddr,
-				 skb->len, PCI_DMA_TODEVICE);
+		pci_unmap_single(pdev, ep->tx_ring[i].bufaddr, skb->len,
+				 PCI_DMA_TODEVICE);
 		dev_kfree_skb(skb);
 	}
 
 	/* Green! Leave the chip in low-power mode. */
-	outl(0x0008, ioaddr + GENCTL);
+	ew32(GENCTL, 0x0008);
 
 	return 0;
 }
 
 static struct net_device_stats *epic_get_stats(struct net_device *dev)
 {
-	long ioaddr = dev->base_addr;
+	struct epic_private *ep = netdev_priv(dev);
+	void __iomem *ioaddr = ep->ioaddr;
 
 	if (netif_running(dev)) {
-		/* Update the error counts. */
-		dev->stats.rx_missed_errors += inb(ioaddr + MPCNT);
-		dev->stats.rx_frame_errors += inb(ioaddr + ALICNT);
-		dev->stats.rx_crc_errors += inb(ioaddr + CRCCNT);
+		struct net_device_stats *stats = &dev->stats;
+
+		stats->rx_missed_errors	+= er8(MPCNT);
+		stats->rx_frame_errors	+= er8(ALICNT);
+		stats->rx_crc_errors	+= er8(CRCCNT);
 	}
 
 	return &dev->stats;
@@ -1373,13 +1369,13 @@ static struct net_device_stats *epic_get_stats(struct net_device *dev)
 
 static void set_rx_mode(struct net_device *dev)
 {
-	long ioaddr = dev->base_addr;
 	struct epic_private *ep = netdev_priv(dev);
+	void __iomem *ioaddr = ep->ioaddr;
 	unsigned char mc_filter[8];		 /* Multicast hash filter */
 	int i;
 
 	if (dev->flags & IFF_PROMISC) {			/* Set promiscuous. */
-		outl(0x002C, ioaddr + RxCtrl);
+		ew32(RxCtrl, 0x002c);
 		/* Unconditionally log net taps. */
 		memset(mc_filter, 0xff, sizeof(mc_filter));
 	} else if ((!netdev_mc_empty(dev)) || (dev->flags & IFF_ALLMULTI)) {
@@ -1387,9 +1383,9 @@ static void set_rx_mode(struct net_device *dev)
 		   is never enabled. */
 		/* Too many to filter perfectly -- accept all multicasts. */
 		memset(mc_filter, 0xff, sizeof(mc_filter));
-		outl(0x000C, ioaddr + RxCtrl);
+		ew32(RxCtrl, 0x000c);
 	} else if (netdev_mc_empty(dev)) {
-		outl(0x0004, ioaddr + RxCtrl);
+		ew32(RxCtrl, 0x0004);
 		return;
 	} else {					/* Never executed, for now. */
 		struct netdev_hw_addr *ha;
@@ -1404,7 +1400,7 @@ static void set_rx_mode(struct net_device *dev)
 	/* ToDo: perhaps we need to stop the Tx and Rx process here? */
 	if (memcmp(mc_filter, ep->mc_filter, sizeof(mc_filter))) {
 		for (i = 0; i < 4; i++)
-			outw(((u16 *)mc_filter)[i], ioaddr + MC0 + i*4);
+			ew16(MC0 + i*4, ((u16 *)mc_filter)[i]);
 		memcpy(ep->mc_filter, mc_filter, sizeof(mc_filter));
 	}
 }
@@ -1466,22 +1462,26 @@ static void netdev_set_msglevel(struct net_device *dev, u32 value)
 
 static int ethtool_begin(struct net_device *dev)
 {
-	unsigned long ioaddr = dev->base_addr;
+	struct epic_private *ep = netdev_priv(dev);
+	void __iomem *ioaddr = ep->ioaddr;
+
 	/* power-up, if interface is down */
-	if (! netif_running(dev)) {
-		outl(0x0200, ioaddr + GENCTL);
-		outl((inl(ioaddr + NVCTL) & ~0x003C) | 0x4800, ioaddr + NVCTL);
+	if (!netif_running(dev)) {
+		ew32(GENCTL, 0x0200);
+		ew32(NVCTL, (er32(NVCTL) & ~0x003c) | 0x4800);
 	}
 	return 0;
 }
 
 static void ethtool_complete(struct net_device *dev)
 {
-	unsigned long ioaddr = dev->base_addr;
+	struct epic_private *ep = netdev_priv(dev);
+	void __iomem *ioaddr = ep->ioaddr;
+
 	/* power-down, if interface is down */
-	if (! netif_running(dev)) {
-		outl(0x0008, ioaddr + GENCTL);
-		outl((inl(ioaddr + NVCTL) & ~0x483C) | 0x0000, ioaddr + NVCTL);
+	if (!netif_running(dev)) {
+		ew32(GENCTL, 0x0008);
+		ew32(NVCTL, (er32(NVCTL) & ~0x483c) | 0x0000);
 	}
 }
 
@@ -1500,14 +1500,14 @@ static const struct ethtool_ops netdev_ethtool_ops = {
 static int netdev_ioctl(struct net_device *dev, struct ifreq *rq, int cmd)
 {
 	struct epic_private *np = netdev_priv(dev);
-	long ioaddr = dev->base_addr;
+	void __iomem *ioaddr = np->ioaddr;
 	struct mii_ioctl_data *data = if_mii(rq);
 	int rc;
 
 	/* power-up, if interface is down */
 	if (! netif_running(dev)) {
-		outl(0x0200, ioaddr + GENCTL);
-		outl((inl(ioaddr + NVCTL) & ~0x003C) | 0x4800, ioaddr + NVCTL);
+		ew32(GENCTL, 0x0200);
+		ew32(NVCTL, (er32(NVCTL) & ~0x003c) | 0x4800);
 	}
 
 	/* all non-ethtool ioctls (the SIOC[GS]MIIxxx ioctls) */
@@ -1517,14 +1517,14 @@ static int netdev_ioctl(struct net_device *dev, struct ifreq *rq, int cmd)
 
 	/* power-down, if interface is down */
 	if (! netif_running(dev)) {
-		outl(0x0008, ioaddr + GENCTL);
-		outl((inl(ioaddr + NVCTL) & ~0x483C) | 0x0000, ioaddr + NVCTL);
+		ew32(GENCTL, 0x0008);
+		ew32(NVCTL, (er32(NVCTL) & ~0x483c) | 0x0000);
 	}
 	return rc;
 }
 
 
-static void __devexit epic_remove_one (struct pci_dev *pdev)
+static void __devexit epic_remove_one(struct pci_dev *pdev)
 {
 	struct net_device *dev = pci_get_drvdata(pdev);
 	struct epic_private *ep = netdev_priv(dev);
@@ -1532,9 +1532,7 @@ static void __devexit epic_remove_one (struct pci_dev *pdev)
 	pci_free_consistent(pdev, TX_TOTAL_SIZE, ep->tx_ring, ep->tx_ring_dma);
 	pci_free_consistent(pdev, RX_TOTAL_SIZE, ep->rx_ring, ep->rx_ring_dma);
 	unregister_netdev(dev);
-#ifndef USE_IO_OPS
-	iounmap((void*) dev->base_addr);
-#endif
+	pci_iounmap(pdev, ep->ioaddr);
 	pci_release_regions(pdev);
 	free_netdev(dev);
 	pci_disable_device(pdev);
@@ -1548,13 +1546,14 @@ static void __devexit epic_remove_one (struct pci_dev *pdev)
 static int epic_suspend (struct pci_dev *pdev, pm_message_t state)
 {
 	struct net_device *dev = pci_get_drvdata(pdev);
-	long ioaddr = dev->base_addr;
+	struct epic_private *ep = netdev_priv(dev);
+	void __iomem *ioaddr = ep->ioaddr;
 
 	if (!netif_running(dev))
 		return 0;
 	epic_pause(dev);
 	/* Put the chip into low-power mode. */
-	outl(0x0008, ioaddr + GENCTL);
+	ew32(GENCTL, 0x0008);
 	/* pci_power_off(pdev, -1); */
 	return 0;
 }
diff --git a/drivers/net/ethernet/smsc/smsc911x.c b/drivers/net/ethernet/smsc/smsc911x.c
index cd3defb11ff..dab9c6f671e 100644
--- a/drivers/net/ethernet/smsc/smsc911x.c
+++ b/drivers/net/ethernet/smsc/smsc911x.c
@@ -2066,6 +2066,7 @@ static const struct ethtool_ops smsc911x_ethtool_ops = {
 	.get_eeprom_len = smsc911x_ethtool_get_eeprom_len,
 	.get_eeprom = smsc911x_ethtool_get_eeprom,
 	.set_eeprom = smsc911x_ethtool_set_eeprom,
+	.get_ts_info = ethtool_op_get_ts_info,
 };
 
 static const struct net_device_ops smsc911x_netdev_ops = {
diff --git a/drivers/net/ethernet/smsc/smsc9420.c b/drivers/net/ethernet/smsc/smsc9420.c
index 38386478532..fd33b21f6c9 100644
--- a/drivers/net/ethernet/smsc/smsc9420.c
+++ b/drivers/net/ethernet/smsc/smsc9420.c
@@ -54,7 +54,7 @@ struct smsc9420_ring_info {
 };
 
 struct smsc9420_pdata {
-	void __iomem *base_addr;
+	void __iomem *ioaddr;
 	struct pci_dev *pdev;
 	struct net_device *dev;
 
@@ -114,13 +114,13 @@ do {	if ((pd)->msg_enable & NETIF_MSG_##TYPE) \
 
 static inline u32 smsc9420_reg_read(struct smsc9420_pdata *pd, u32 offset)
 {
-	return ioread32(pd->base_addr + offset);
+	return ioread32(pd->ioaddr + offset);
 }
 
 static inline void
 smsc9420_reg_write(struct smsc9420_pdata *pd, u32 offset, u32 value)
 {
-	iowrite32(value, pd->base_addr + offset);
+	iowrite32(value, pd->ioaddr + offset);
 }
 
 static inline void smsc9420_pci_flush_write(struct smsc9420_pdata *pd)
@@ -469,6 +469,7 @@ static const struct ethtool_ops smsc9420_ethtool_ops = {
 	.set_eeprom = smsc9420_ethtool_set_eeprom,
 	.get_regs_len = smsc9420_ethtool_getregslen,
 	.get_regs = smsc9420_ethtool_getregs,
+	.get_ts_info = ethtool_op_get_ts_info,
 };
 
 /* Sets the device MAC address to dev_addr */
@@ -659,7 +660,7 @@ static irqreturn_t smsc9420_isr(int irq, void *dev_id)
 	ulong flags;
 
 	BUG_ON(!pd);
-	BUG_ON(!pd->base_addr);
+	BUG_ON(!pd->ioaddr);
 
 	int_cfg = smsc9420_reg_read(pd, INT_CFG);
 
@@ -720,9 +721,12 @@ static irqreturn_t smsc9420_isr(int irq, void *dev_id)
 #ifdef CONFIG_NET_POLL_CONTROLLER
 static void smsc9420_poll_controller(struct net_device *dev)
 {
-	disable_irq(dev->irq);
+	struct smsc9420_pdata *pd = netdev_priv(dev);
+	const int irq = pd->pdev->irq;
+
+	disable_irq(irq);
 	smsc9420_isr(0, dev);
-	enable_irq(dev->irq);
+	enable_irq(irq);
 }
 #endif /* CONFIG_NET_POLL_CONTROLLER */
 
@@ -759,7 +763,7 @@ static int smsc9420_stop(struct net_device *dev)
 	smsc9420_stop_rx(pd);
 	smsc9420_free_rx_ring(pd);
 
-	free_irq(dev->irq, pd);
+	free_irq(pd->pdev->irq, pd);
 
 	smsc9420_dmac_soft_reset(pd);
 
@@ -1331,15 +1335,12 @@ out:
 
 static int smsc9420_open(struct net_device *dev)
 {
-	struct smsc9420_pdata *pd;
+	struct smsc9420_pdata *pd = netdev_priv(dev);
 	u32 bus_mode, mac_cr, dmac_control, int_cfg, dma_intr_ena, int_ctl;
+	const int irq = pd->pdev->irq;
 	unsigned long flags;
 	int result = 0, timeout;
 
-	BUG_ON(!dev);
-	pd = netdev_priv(dev);
-	BUG_ON(!pd);
-
 	if (!is_valid_ether_addr(dev->dev_addr)) {
 		smsc_warn(IFUP, "dev_addr is not a valid MAC address");
 		result = -EADDRNOTAVAIL;
@@ -1358,9 +1359,10 @@ static int smsc9420_open(struct net_device *dev)
 	smsc9420_reg_write(pd, INT_STAT, 0xFFFFFFFF);
 	smsc9420_pci_flush_write(pd);
 
-	if (request_irq(dev->irq, smsc9420_isr, IRQF_SHARED | IRQF_DISABLED,
-			DRV_NAME, pd)) {
-		smsc_warn(IFUP, "Unable to use IRQ = %d", dev->irq);
+	result = request_irq(irq, smsc9420_isr, IRQF_SHARED | IRQF_DISABLED,
+			     DRV_NAME, pd);
+	if (result) {
+		smsc_warn(IFUP, "Unable to use IRQ = %d", irq);
 		result = -ENODEV;
 		goto out_0;
 	}
@@ -1395,7 +1397,7 @@ static int smsc9420_open(struct net_device *dev)
 	smsc9420_pci_flush_write(pd);
 
 	/* test the IRQ connection to the ISR */
-	smsc_dbg(IFUP, "Testing ISR using IRQ %d", dev->irq);
+	smsc_dbg(IFUP, "Testing ISR using IRQ %d", irq);
 	pd->software_irq_signal = false;
 
 	spin_lock_irqsave(&pd->int_lock, flags);
@@ -1430,7 +1432,7 @@ static int smsc9420_open(struct net_device *dev)
 		goto out_free_irq_1;
 	}
 
-	smsc_dbg(IFUP, "ISR passed test using IRQ %d", dev->irq);
+	smsc_dbg(IFUP, "ISR passed test using IRQ %d", irq);
 
 	result = smsc9420_alloc_tx_ring(pd);
 	if (result) {
@@ -1490,7 +1492,7 @@ out_free_rx_ring_3:
 out_free_tx_ring_2:
 	smsc9420_free_tx_ring(pd);
 out_free_irq_1:
-	free_irq(dev->irq, pd);
+	free_irq(irq, pd);
 out_0:
 	return result;
 }
@@ -1519,7 +1521,7 @@ static int smsc9420_suspend(struct pci_dev *pdev, pm_message_t state)
 		smsc9420_stop_rx(pd);
 		smsc9420_free_rx_ring(pd);
 
-		free_irq(dev->irq, pd);
+		free_irq(pd->pdev->irq, pd);
 
 		netif_device_detach(dev);
 	}
@@ -1552,6 +1554,7 @@ static int smsc9420_resume(struct pci_dev *pdev)
 		smsc_warn(IFUP, "pci_enable_wake failed: %d", err);
 
 	if (netif_running(dev)) {
+		/* FIXME: gross. It looks like ancient PM relic.*/
 		err = smsc9420_open(dev);
 		netif_device_attach(dev);
 	}
@@ -1625,8 +1628,6 @@ smsc9420_probe(struct pci_dev *pdev, const struct pci_device_id *id)
 	/* registers are double mapped with 0 offset for LE and 0x200 for BE */
 	virt_addr += LAN9420_CPSR_ENDIAN_OFFSET;
 
-	dev->base_addr = (ulong)virt_addr;
-
 	pd = netdev_priv(dev);
 
 	/* pci descriptors are created in the PCI consistent area */
@@ -1646,7 +1647,7 @@ smsc9420_probe(struct pci_dev *pdev, const struct pci_device_id *id)
 
 	pd->pdev = pdev;
 	pd->dev = dev;
-	pd->base_addr = virt_addr;
+	pd->ioaddr = virt_addr;
 	pd->msg_enable = smsc_debug;
 	pd->rx_csum = true;
 
@@ -1669,7 +1670,6 @@ smsc9420_probe(struct pci_dev *pdev, const struct pci_device_id *id)
 
 	dev->netdev_ops = &smsc9420_netdev_ops;
 	dev->ethtool_ops = &smsc9420_ethtool_ops;
-	dev->irq = pdev->irq;
 
 	netif_napi_add(dev, &pd->napi, smsc9420_rx_poll, NAPI_WEIGHT);
 
@@ -1727,7 +1727,7 @@ static void __devexit smsc9420_remove(struct pci_dev *pdev)
 	pci_free_consistent(pdev, sizeof(struct smsc9420_dma_desc) *
 		(RX_RING_SIZE + TX_RING_SIZE), pd->rx_ring, pd->rx_dma_addr);
 
-	iounmap(pd->base_addr - LAN9420_CPSR_ENDIAN_OFFSET);
+	iounmap(pd->ioaddr - LAN9420_CPSR_ENDIAN_OFFSET);
 	pci_release_regions(pdev);
 	free_netdev(dev);
 	pci_disable_device(pdev);
diff --git a/drivers/net/ethernet/stmicro/stmmac/common.h b/drivers/net/ethernet/stmicro/stmmac/common.h
index 0319d640f72..bcd54d6e94f 100644
--- a/drivers/net/ethernet/stmicro/stmmac/common.h
+++ b/drivers/net/ethernet/stmicro/stmmac/common.h
@@ -97,6 +97,16 @@ struct stmmac_extra_stats {
 	unsigned long normal_irq_n;
 };
 
+/* CSR Frequency Access Defines*/
+#define CSR_F_35M	35000000
+#define CSR_F_60M	60000000
+#define CSR_F_100M	100000000
+#define CSR_F_150M	150000000
+#define CSR_F_250M	250000000
+#define CSR_F_300M	300000000
+
+#define	MAC_CSR_H_FRQ_MASK	0x20
+
 #define HASH_TABLE_SIZE 64
 #define PAUSE_TIME 0x200
 
@@ -137,6 +147,7 @@ struct stmmac_extra_stats {
 #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 DEFAULT_DMA_PBL		8
 
 enum rx_frame_status { /* IPC status */
 	good_frame = 0,
@@ -228,7 +239,7 @@ struct stmmac_desc_ops {
 	int (*get_rx_owner) (struct dma_desc *p);
 	void (*set_rx_owner) (struct dma_desc *p);
 	/* Get the receive frame size */
-	int (*get_rx_frame_len) (struct dma_desc *p);
+	int (*get_rx_frame_len) (struct dma_desc *p, int rx_coe_type);
 	/* Return the reception status looking at the RDES1 */
 	int (*rx_status) (void *data, struct stmmac_extra_stats *x,
 			  struct dma_desc *p);
@@ -236,7 +247,8 @@ struct stmmac_desc_ops {
 
 struct stmmac_dma_ops {
 	/* DMA core initialization */
-	int (*init) (void __iomem *ioaddr, int pbl, u32 dma_tx, u32 dma_rx);
+	int (*init) (void __iomem *ioaddr, int pbl, int fb, int mb,
+		     int burst_len, u32 dma_tx, u32 dma_rx);
 	/* Dump DMA registers */
 	void (*dump_regs) (void __iomem *ioaddr);
 	/* Set tx/rx threshold in the csr6 register
@@ -261,14 +273,14 @@ struct stmmac_dma_ops {
 struct stmmac_ops {
 	/* MAC core initialization */
 	void (*core_init) (void __iomem *ioaddr) ____cacheline_aligned;
-	/* Support checksum offload engine */
-	int  (*rx_coe) (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 */
 	void (*host_irq_status) (void __iomem *ioaddr);
 	/* Multicast filter setting */
-	void (*set_filter) (struct net_device *dev);
+	void (*set_filter) (struct net_device *dev, int id);
 	/* Flow control setting */
 	void (*flow_ctrl) (void __iomem *ioaddr, unsigned int duplex,
 			   unsigned int fc, unsigned int pause_time);
diff --git a/drivers/net/ethernet/stmicro/stmmac/dwmac1000.h b/drivers/net/ethernet/stmicro/stmmac/dwmac1000.h
index cfcef0ea0fa..23478bf4ed7 100644
--- a/drivers/net/ethernet/stmicro/stmmac/dwmac1000.h
+++ b/drivers/net/ethernet/stmicro/stmmac/dwmac1000.h
@@ -61,9 +61,11 @@ enum power_event {
 };
 
 /* GMAC HW ADDR regs */
-#define GMAC_ADDR_HIGH(reg)		(0x00000040+(reg * 8))
-#define GMAC_ADDR_LOW(reg)		(0x00000044+(reg * 8))
-#define GMAC_MAX_UNICAST_ADDRESSES	16
+#define GMAC_ADDR_HIGH(reg)	(((reg > 15) ? 0x00000800 : 0x00000040) + \
+				(reg * 8))
+#define GMAC_ADDR_LOW(reg)	(((reg > 15) ? 0x00000804 : 0x00000044) + \
+				(reg * 8))
+#define GMAC_MAX_PERFECT_ADDRESSES	32
 
 #define GMAC_AN_CTRL	0x000000c0	/* AN control */
 #define GMAC_AN_STATUS	0x000000c4	/* AN status */
@@ -139,10 +141,11 @@ enum rx_tx_priority_ratio {
 };
 
 #define DMA_BUS_MODE_FB		0x00010000	/* Fixed burst */
+#define DMA_BUS_MODE_MB		0x04000000	/* Mixed burst */
 #define DMA_BUS_MODE_RPBL_MASK	0x003e0000	/* Rx-Programmable Burst Len */
 #define DMA_BUS_MODE_RPBL_SHIFT	17
 #define DMA_BUS_MODE_USP	0x00800000
-#define DMA_BUS_MODE_4PBL	0x01000000
+#define DMA_BUS_MODE_PBL	0x01000000
 #define DMA_BUS_MODE_AAL	0x02000000
 
 /* DMA CRS Control and Status Register Mapping */
@@ -205,4 +208,7 @@ enum rtc_control {
 #define GMAC_MMC_TX_INTR   0x108
 #define GMAC_MMC_RX_CSUM_OFFLOAD   0x208
 
+/* Synopsys Core versions */
+#define	DWMAC_CORE_3_40	34
+
 extern const struct stmmac_dma_ops dwmac1000_dma_ops;
diff --git a/drivers/net/ethernet/stmicro/stmmac/dwmac1000_core.c b/drivers/net/ethernet/stmicro/stmmac/dwmac1000_core.c
index b1c48b97594..b5e4d02f15c 100644
--- a/drivers/net/ethernet/stmicro/stmmac/dwmac1000_core.c
+++ b/drivers/net/ethernet/stmicro/stmmac/dwmac1000_core.c
@@ -46,7 +46,7 @@ static void dwmac1000_core_init(void __iomem *ioaddr)
 #endif
 }
 
-static int dwmac1000_rx_coe_supported(void __iomem *ioaddr)
+static int dwmac1000_rx_ipc_enable(void __iomem *ioaddr)
 {
 	u32 value = readl(ioaddr + GMAC_CONTROL);
 
@@ -84,10 +84,11 @@ static void dwmac1000_get_umac_addr(void __iomem *ioaddr, unsigned char *addr,
 				GMAC_ADDR_LOW(reg_n));
 }
 
-static void dwmac1000_set_filter(struct net_device *dev)
+static void dwmac1000_set_filter(struct net_device *dev, int id)
 {
 	void __iomem *ioaddr = (void __iomem *) dev->base_addr;
 	unsigned int value = 0;
+	unsigned int perfect_addr_number;
 
 	CHIP_DBG(KERN_INFO "%s: # mcasts %d, # unicast %d\n",
 		 __func__, netdev_mc_count(dev), netdev_uc_count(dev));
@@ -121,8 +122,14 @@ static void dwmac1000_set_filter(struct net_device *dev)
 		writel(mc_filter[1], ioaddr + GMAC_HASH_HIGH);
 	}
 
+	/* Extra 16 regs are available in cores newer than the 3.40. */
+	if (id > DWMAC_CORE_3_40)
+		perfect_addr_number = GMAC_MAX_PERFECT_ADDRESSES;
+	else
+		perfect_addr_number = GMAC_MAX_PERFECT_ADDRESSES / 2;
+
 	/* Handle multiple unicast addresses (perfect filtering)*/
-	if (netdev_uc_count(dev) > GMAC_MAX_UNICAST_ADDRESSES)
+	if (netdev_uc_count(dev) > perfect_addr_number)
 		/* Switch to promiscuous mode is more than 16 addrs
 		   are required */
 		value |= GMAC_FRAME_FILTER_PR;
@@ -211,7 +218,7 @@ static void dwmac1000_irq_status(void __iomem *ioaddr)
 
 static const struct stmmac_ops dwmac1000_ops = {
 	.core_init = dwmac1000_core_init,
-	.rx_coe = dwmac1000_rx_coe_supported,
+	.rx_ipc = dwmac1000_rx_ipc_enable,
 	.dump_regs = dwmac1000_dump_regs,
 	.host_irq_status = dwmac1000_irq_status,
 	.set_filter = dwmac1000_set_filter,
diff --git a/drivers/net/ethernet/stmicro/stmmac/dwmac1000_dma.c b/drivers/net/ethernet/stmicro/stmmac/dwmac1000_dma.c
index 4d5402a1d26..033500090f5 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, 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)
 {
 	u32 value = readl(ioaddr + DMA_BUS_MODE);
 	int limit;
@@ -48,15 +48,51 @@ static int dwmac1000_dma_init(void __iomem *ioaddr, int pbl, u32 dma_tx,
 	if (limit < 0)
 		return -EBUSY;
 
-	value = /* DMA_BUS_MODE_FB | */ DMA_BUS_MODE_4PBL |
-	    ((pbl << DMA_BUS_MODE_PBL_SHIFT) |
-	     (pbl << DMA_BUS_MODE_RPBL_SHIFT));
+	/*
+	 * Set the DMA PBL (Programmable Burst Length) mode
+	 * Before stmmac core 3.50 this mode bit was 4xPBL, and
+	 * post 3.5 mode bit acts as 8*PBL.
+	 * For core rev < 3.5, when the core is set for 4xPBL mode, the
+	 * DMA transfers the data in 4, 8, 16, 32, 64 & 128 beats
+	 * depending on pbl value.
+	 * For core rev > 3.5, when the core is set for 8xPBL mode, the
+	 * DMA transfers the data in 8, 16, 32, 64, 128 & 256 beats
+	 * depending on pbl value.
+	 */
+	value = DMA_BUS_MODE_PBL | ((pbl << DMA_BUS_MODE_PBL_SHIFT) |
+		(pbl << DMA_BUS_MODE_RPBL_SHIFT));
+
+	/* Set the Fixed burst mode */
+	if (fb)
+		value |= DMA_BUS_MODE_FB;
+
+	/* Mixed Burst has no effect when fb is set */
+	if (mb)
+		value |= DMA_BUS_MODE_MB;
 
 #ifdef CONFIG_STMMAC_DA
 	value |= DMA_BUS_MODE_DA;	/* Rx has priority over tx */
 #endif
 	writel(value, ioaddr + DMA_BUS_MODE);
 
+	/* In case of GMAC AXI configuration, program the DMA_AXI_BUS_MODE
+	 * for supported bursts.
+	 *
+	 * Note: This is applicable only for revision GMACv3.61a. For
+	 * older version this register is reserved and shall have no
+	 * effect.
+	 *
+	 * Note:
+	 *  For Fixed Burst Mode: if we directly write 0xFF to this
+	 *  register using the configurations pass from platform code,
+	 *  this would ensure that all bursts supported by core are set
+	 *  and those which are not supported would remain ineffective.
+	 *
+	 *  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. */
+	writel(burst_len, ioaddr + DMA_AXI_BUS_MODE);
+
 	/* Mask interrupts by writing to CSR7 */
 	writel(DMA_INTR_DEFAULT_MASK, ioaddr + DMA_INTR_ENA);
 
diff --git a/drivers/net/ethernet/stmicro/stmmac/dwmac100_core.c b/drivers/net/ethernet/stmicro/stmmac/dwmac100_core.c
index 138fb8dd1e8..19e0f4eed2b 100644
--- a/drivers/net/ethernet/stmicro/stmmac/dwmac100_core.c
+++ b/drivers/net/ethernet/stmicro/stmmac/dwmac100_core.c
@@ -43,11 +43,6 @@ static void dwmac100_core_init(void __iomem *ioaddr)
 #endif
 }
 
-static int dwmac100_rx_coe_supported(void __iomem *ioaddr)
-{
-	return 0;
-}
-
 static void dwmac100_dump_mac_regs(void __iomem *ioaddr)
 {
 	pr_info("\t----------------------------------------------\n"
@@ -72,6 +67,11 @@ static void dwmac100_dump_mac_regs(void __iomem *ioaddr)
 		readl(ioaddr + MAC_VLAN2));
 }
 
+static int dwmac100_rx_ipc_enable(void __iomem *ioaddr)
+{
+	return 0;
+}
+
 static void dwmac100_irq_status(void __iomem *ioaddr)
 {
 	return;
@@ -89,7 +89,7 @@ static void dwmac100_get_umac_addr(void __iomem *ioaddr, unsigned char *addr,
 	stmmac_get_mac_addr(ioaddr, addr, MAC_ADDR_HIGH, MAC_ADDR_LOW);
 }
 
-static void dwmac100_set_filter(struct net_device *dev)
+static void dwmac100_set_filter(struct net_device *dev, int id)
 {
 	void __iomem *ioaddr = (void __iomem *) dev->base_addr;
 	u32 value = readl(ioaddr + MAC_CONTROL);
@@ -160,7 +160,7 @@ static void dwmac100_pmt(void __iomem *ioaddr, unsigned long mode)
 
 static const struct stmmac_ops dwmac100_ops = {
 	.core_init = dwmac100_core_init,
-	.rx_coe = dwmac100_rx_coe_supported,
+	.rx_ipc = dwmac100_rx_ipc_enable,
 	.dump_regs = dwmac100_dump_mac_regs,
 	.host_irq_status = dwmac100_irq_status,
 	.set_filter = dwmac100_set_filter,
diff --git a/drivers/net/ethernet/stmicro/stmmac/dwmac100_dma.c b/drivers/net/ethernet/stmicro/stmmac/dwmac100_dma.c
index bc17fd08b55..c2b4d55a79b 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, 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)
 {
 	u32 value = readl(ioaddr + DMA_BUS_MODE);
 	int limit;
@@ -52,7 +52,7 @@ static int dwmac100_dma_init(void __iomem *ioaddr, int pbl, u32 dma_tx,
 
 	/* 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);
diff --git a/drivers/net/ethernet/stmicro/stmmac/dwmac_dma.h b/drivers/net/ethernet/stmicro/stmmac/dwmac_dma.h
index 437edacd602..6e0360f9cfd 100644
--- a/drivers/net/ethernet/stmicro/stmmac/dwmac_dma.h
+++ b/drivers/net/ethernet/stmicro/stmmac/dwmac_dma.h
@@ -32,6 +32,7 @@
 #define DMA_CONTROL		0x00001018	/* Ctrl (Operational Mode) */
 #define DMA_INTR_ENA		0x0000101c	/* Interrupt Enable */
 #define DMA_MISSED_FRAME_CTR	0x00001020	/* Missed Frame Counter */
+#define DMA_AXI_BUS_MODE       0x00001028      /* AXI Bus Mode */
 #define DMA_CUR_TX_BUF_ADDR	0x00001050	/* Current Host Tx Buffer */
 #define DMA_CUR_RX_BUF_ADDR	0x00001054	/* Current Host Rx Buffer */
 #define DMA_HW_FEATURE		0x00001058	/* HW Feature Register */
diff --git a/drivers/net/ethernet/stmicro/stmmac/dwmac_lib.c b/drivers/net/ethernet/stmicro/stmmac/dwmac_lib.c
index f20aa12931d..4e0e18a44fc 100644
--- a/drivers/net/ethernet/stmicro/stmmac/dwmac_lib.c
+++ b/drivers/net/ethernet/stmicro/stmmac/dwmac_lib.c
@@ -31,6 +31,8 @@
 #define DWMAC_LIB_DBG(fmt, args...)  do { } while (0)
 #endif
 
+#define GMAC_HI_REG_AE		0x80000000
+
 /* CSR1 enables the transmit DMA to check for new descriptor */
 void dwmac_enable_dma_transmission(void __iomem *ioaddr)
 {
@@ -233,7 +235,11 @@ void stmmac_set_mac_addr(void __iomem *ioaddr, u8 addr[6],
 	unsigned long data;
 
 	data = (addr[5] << 8) | addr[4];
-	writel(data, ioaddr + high);
+	/* For MAC Addr registers se have to set the Address Enable (AE)
+	 * bit that has no effect on the High Reg 0 where the bit 31 (MO)
+	 * is RO.
+	 */
+	writel(data | GMAC_HI_REG_AE, ioaddr + high);
 	data = (addr[3] << 24) | (addr[2] << 16) | (addr[1] << 8) | addr[0];
 	writel(data, ioaddr + low);
 }
diff --git a/drivers/net/ethernet/stmicro/stmmac/enh_desc.c b/drivers/net/ethernet/stmicro/stmmac/enh_desc.c
index ad1b627f8ec..2fc8ef95f97 100644
--- a/drivers/net/ethernet/stmicro/stmmac/enh_desc.c
+++ b/drivers/net/ethernet/stmicro/stmmac/enh_desc.c
@@ -22,6 +22,7 @@
   Author: Giuseppe Cavallaro <peppe.cavallaro@st.com>
 *******************************************************************************/
 
+#include <linux/stmmac.h>
 #include "common.h"
 #include "descs_com.h"
 
@@ -309,9 +310,17 @@ static void enh_desc_close_tx_desc(struct dma_desc *p)
 	p->des01.etx.interrupt = 1;
 }
 
-static int enh_desc_get_rx_frame_len(struct dma_desc *p)
+static int enh_desc_get_rx_frame_len(struct dma_desc *p, int rx_coe_type)
 {
-	return p->des01.erx.frame_length;
+	/* The type-1 checksum offload engines append the checksum at
+	 * the end of frame and the two bytes of checksum are added in
+	 * the length.
+	 * Adjust for that in the framelen for type-1 checksum offload
+	 * engines. */
+	if (rx_coe_type == STMMAC_RX_COE_TYPE1)
+		return p->des01.erx.frame_length - 2;
+	else
+		return p->des01.erx.frame_length;
 }
 
 const struct stmmac_desc_ops enh_desc_ops = {
diff --git a/drivers/net/ethernet/stmicro/stmmac/norm_desc.c b/drivers/net/ethernet/stmicro/stmmac/norm_desc.c
index 25953bb45a7..68962c549a2 100644
--- a/drivers/net/ethernet/stmicro/stmmac/norm_desc.c
+++ b/drivers/net/ethernet/stmicro/stmmac/norm_desc.c
@@ -22,6 +22,7 @@
   Author: Giuseppe Cavallaro <peppe.cavallaro@st.com>
 *******************************************************************************/
 
+#include <linux/stmmac.h>
 #include "common.h"
 #include "descs_com.h"
 
@@ -201,9 +202,17 @@ static void ndesc_close_tx_desc(struct dma_desc *p)
 	p->des01.tx.interrupt = 1;
 }
 
-static int ndesc_get_rx_frame_len(struct dma_desc *p)
+static int ndesc_get_rx_frame_len(struct dma_desc *p, int rx_coe_type)
 {
-	return p->des01.rx.frame_length;
+	/* The type-1 checksum offload engines append the checksum at
+	 * the end of frame and the two bytes of checksum are added in
+	 * the length.
+	 * Adjust for that in the framelen for type-1 checksum offload
+	 * engines. */
+	if (rx_coe_type == STMMAC_RX_COE_TYPE1)
+		return p->des01.rx.frame_length - 2;
+	else
+		return p->des01.rx.frame_length;
 }
 
 const struct stmmac_desc_ops ndesc_ops = {
diff --git a/drivers/net/ethernet/stmicro/stmmac/stmmac.h b/drivers/net/ethernet/stmicro/stmmac/stmmac.h
index b4b095fdcf2..6b5d060ee9d 100644
--- a/drivers/net/ethernet/stmicro/stmmac/stmmac.h
+++ b/drivers/net/ethernet/stmicro/stmmac/stmmac.h
@@ -21,7 +21,9 @@
 *******************************************************************************/
 
 #define STMMAC_RESOURCE_NAME   "stmmaceth"
-#define DRV_MODULE_VERSION	"Feb_2012"
+#define DRV_MODULE_VERSION	"March_2012"
+
+#include <linux/clk.h>
 #include <linux/stmmac.h>
 #include <linux/phy.h>
 #include "common.h"
@@ -56,8 +58,6 @@ struct stmmac_priv {
 
 	struct stmmac_extra_stats xstats;
 	struct napi_struct napi;
-
-	int rx_coe;
 	int no_csum_insertion;
 
 	struct phy_device *phydev;
@@ -81,6 +81,11 @@ struct stmmac_priv {
 	struct stmmac_counters mmc;
 	struct dma_features dma_cap;
 	int hw_cap_support;
+#ifdef CONFIG_HAVE_CLK
+	struct clk *stmmac_clk;
+#endif
+	int clk_csr;
+	int synopsys_id;
 };
 
 extern int phyaddr;
@@ -99,3 +104,42 @@ int stmmac_dvr_remove(struct net_device *ndev);
 struct stmmac_priv *stmmac_dvr_probe(struct device *device,
 				     struct plat_stmmacenet_data *plat_dat,
 				     void __iomem *addr);
+
+#ifdef CONFIG_HAVE_CLK
+static inline int stmmac_clk_enable(struct stmmac_priv *priv)
+{
+	if (!IS_ERR(priv->stmmac_clk))
+		return clk_enable(priv->stmmac_clk);
+
+	return 0;
+}
+
+static inline void stmmac_clk_disable(struct stmmac_priv *priv)
+{
+	if (IS_ERR(priv->stmmac_clk))
+		return;
+
+	clk_disable(priv->stmmac_clk);
+}
+static inline int stmmac_clk_get(struct stmmac_priv *priv)
+{
+	priv->stmmac_clk = clk_get(priv->device, NULL);
+
+	if (IS_ERR(priv->stmmac_clk))
+		return PTR_ERR(priv->stmmac_clk);
+
+	return 0;
+}
+#else
+static inline int stmmac_clk_enable(struct stmmac_priv *priv)
+{
+	return 0;
+}
+static inline void stmmac_clk_disable(struct stmmac_priv *priv)
+{
+}
+static inline int stmmac_clk_get(struct stmmac_priv *priv)
+{
+	return 0;
+}
+#endif /* CONFIG_HAVE_CLK */
diff --git a/drivers/net/ethernet/stmicro/stmmac/stmmac_ethtool.c b/drivers/net/ethernet/stmicro/stmmac/stmmac_ethtool.c
index f98e1511660..ce431846fc6 100644
--- a/drivers/net/ethernet/stmicro/stmmac/stmmac_ethtool.c
+++ b/drivers/net/ethernet/stmicro/stmmac/stmmac_ethtool.c
@@ -481,6 +481,7 @@ static const struct ethtool_ops stmmac_ethtool_ops = {
 	.get_wol = stmmac_get_wol,
 	.set_wol = stmmac_set_wol,
 	.get_sset_count	= stmmac_get_sset_count,
+	.get_ts_info = ethtool_op_get_ts_info,
 };
 
 void stmmac_set_ethtool_ops(struct net_device *netdev)
diff --git a/drivers/net/ethernet/stmicro/stmmac/stmmac_main.c b/drivers/net/ethernet/stmicro/stmmac/stmmac_main.c
index 48d56da62f0..70966330f44 100644
--- a/drivers/net/ethernet/stmicro/stmmac/stmmac_main.c
+++ b/drivers/net/ethernet/stmicro/stmmac/stmmac_main.c
@@ -163,6 +163,38 @@ static void stmmac_verify_args(void)
 		pause = PAUSE_TIME;
 }
 
+static void stmmac_clk_csr_set(struct stmmac_priv *priv)
+{
+#ifdef CONFIG_HAVE_CLK
+	u32 clk_rate;
+
+	if (IS_ERR(priv->stmmac_clk))
+		return;
+
+	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. */
+	if (!(priv->clk_csr & MAC_CSR_H_FRQ_MASK)) {
+		if (clk_rate < CSR_F_35M)
+			priv->clk_csr = STMMAC_CSR_20_35M;
+		else if ((clk_rate >= CSR_F_35M) && (clk_rate < CSR_F_60M))
+			priv->clk_csr = STMMAC_CSR_35_60M;
+		else if ((clk_rate >= CSR_F_60M) && (clk_rate < CSR_F_100M))
+			priv->clk_csr = STMMAC_CSR_60_100M;
+		else if ((clk_rate >= CSR_F_100M) && (clk_rate < CSR_F_150M))
+			priv->clk_csr = STMMAC_CSR_100_150M;
+		else if ((clk_rate >= CSR_F_150M) && (clk_rate < CSR_F_250M))
+			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. */
+#endif
+}
+
 #if defined(STMMAC_XMIT_DEBUG) || defined(STMMAC_RX_DEBUG)
 static void print_pkt(unsigned char *buf, int len)
 {
@@ -307,7 +339,13 @@ static int stmmac_init_phy(struct net_device *dev)
 	priv->speed = 0;
 	priv->oldduplex = -1;
 
-	snprintf(bus_id, MII_BUS_ID_SIZE, "stmmac-%x", priv->plat->bus_id);
+	if (priv->plat->phy_bus_name)
+		snprintf(bus_id, MII_BUS_ID_SIZE, "%s-%x",
+				priv->plat->phy_bus_name, priv->plat->bus_id);
+	else
+		snprintf(bus_id, MII_BUS_ID_SIZE, "stmmac-%x",
+				priv->plat->bus_id);
+
 	snprintf(phy_id, MII_BUS_ID_SIZE + 3, PHY_ID_FMT, bus_id,
 		 priv->plat->phy_addr);
 	pr_debug("stmmac_init_phy:  trying to attach to %s\n", phy_id);
@@ -884,6 +922,26 @@ static void stmmac_check_ether_addr(struct stmmac_priv *priv)
 						   priv->dev->dev_addr);
 }
 
+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;
+
+	/* 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;
+		mixed_burst = priv->plat->dma_cfg->mixed_burst;
+		burst_len = priv->plat->dma_cfg->burst_len;
+	}
+
+	return priv->hw->dma->init(priv->ioaddr, pbl, fixed_burst, mixed_burst,
+				   burst_len, priv->dma_tx_phy,
+				   priv->dma_rx_phy);
+}
+
 /**
  *  stmmac_open - open entry point of the driver
  *  @dev : pointer to the device structure.
@@ -898,16 +956,6 @@ static int stmmac_open(struct net_device *dev)
 	struct stmmac_priv *priv = netdev_priv(dev);
 	int ret;
 
-	stmmac_check_ether_addr(priv);
-
-	/* MDIO bus Registration */
-	ret = stmmac_mdio_register(dev);
-	if (ret < 0) {
-		pr_debug("%s: MDIO bus (id: %d) registration failed",
-			 __func__, priv->plat->bus_id);
-		return ret;
-	}
-
 #ifdef CONFIG_STMMAC_TIMER
 	priv->tm = kzalloc(sizeof(struct stmmac_timer *), GFP_KERNEL);
 	if (unlikely(priv->tm == NULL))
@@ -925,6 +973,10 @@ static int stmmac_open(struct net_device *dev)
 	} else
 		priv->tm->enable = 1;
 #endif
+	stmmac_clk_enable(priv);
+
+	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);
@@ -938,8 +990,7 @@ static int stmmac_open(struct net_device *dev)
 	init_dma_desc_rings(dev);
 
 	/* DMA initialization and SW reset */
-	ret = priv->hw->dma->init(priv->ioaddr, priv->plat->pbl,
-				  priv->dma_tx_phy, priv->dma_rx_phy);
+	ret = stmmac_init_dma_engine(priv);
 	if (ret < 0) {
 		pr_err("%s: DMA initialization failed\n", __func__);
 		goto open_error;
@@ -1026,6 +1077,8 @@ open_error:
 	if (priv->phydev)
 		phy_disconnect(priv->phydev);
 
+	stmmac_clk_disable(priv);
+
 	return ret;
 }
 
@@ -1077,7 +1130,7 @@ static int stmmac_release(struct net_device *dev)
 #ifdef CONFIG_STMMAC_DEBUG_FS
 	stmmac_exit_fs();
 #endif
-	stmmac_mdio_unregister(dev);
+	stmmac_clk_disable(priv);
 
 	return 0;
 }
@@ -1276,7 +1329,8 @@ static int stmmac_rx(struct stmmac_priv *priv, int limit)
 			struct sk_buff *skb;
 			int frame_len;
 
-			frame_len = priv->hw->desc->get_rx_frame_len(p);
+			frame_len = priv->hw->desc->get_rx_frame_len(p,
+					priv->plat->rx_coe);
 			/* ACS is set; GMAC core strips PAD/FCS for IEEE 802.3
 			 * Type frames (LLC/LLC-SNAP) */
 			if (unlikely(status != llc_snap))
@@ -1312,7 +1366,7 @@ static int stmmac_rx(struct stmmac_priv *priv, int limit)
 #endif
 			skb->protocol = eth_type_trans(skb, priv->dev);
 
-			if (unlikely(!priv->rx_coe)) {
+			if (unlikely(!priv->plat->rx_coe)) {
 				/* No RX COE for old mac10/100 devices */
 				skb_checksum_none_assert(skb);
 				netif_receive_skb(skb);
@@ -1413,7 +1467,7 @@ static void stmmac_set_rx_mode(struct net_device *dev)
 	struct stmmac_priv *priv = netdev_priv(dev);
 
 	spin_lock(&priv->lock);
-	priv->hw->mac->set_filter(dev);
+	priv->hw->mac->set_filter(dev, priv->synopsys_id);
 	spin_unlock(&priv->lock);
 }
 
@@ -1459,8 +1513,10 @@ static netdev_features_t stmmac_fix_features(struct net_device *dev,
 {
 	struct stmmac_priv *priv = netdev_priv(dev);
 
-	if (!priv->rx_coe)
+	if (priv->plat->rx_coe == STMMAC_RX_COE_NONE)
 		features &= ~NETIF_F_RXCSUM;
+	else if (priv->plat->rx_coe == STMMAC_RX_COE_TYPE1)
+		features &= ~NETIF_F_IPV6_CSUM;
 	if (!priv->plat->tx_coe)
 		features &= ~NETIF_F_ALL_CSUM;
 
@@ -1584,7 +1640,7 @@ static const struct file_operations stmmac_rings_status_fops = {
 	.open = stmmac_sysfs_ring_open,
 	.read = seq_read,
 	.llseek = seq_lseek,
-	.release = seq_release,
+	.release = single_release,
 };
 
 static int stmmac_sysfs_dma_cap_read(struct seq_file *seq, void *v)
@@ -1656,7 +1712,7 @@ static const struct file_operations stmmac_dma_cap_fops = {
 	.open = stmmac_sysfs_dma_cap_open,
 	.read = seq_read,
 	.llseek = seq_lseek,
-	.release = seq_release,
+	.release = single_release,
 };
 
 static int stmmac_init_fs(struct net_device *dev)
@@ -1752,7 +1808,7 @@ static int stmmac_hw_init(struct stmmac_priv *priv)
 	priv->hw->ring = &ring_mode_ops;
 
 	/* Get and dump the chip ID */
-	stmmac_get_synopsys_id(priv);
+	priv->synopsys_id = stmmac_get_synopsys_id(priv);
 
 	/* Get the HW capability (new GMAC newer than 3.50a) */
 	priv->hw_cap_support = stmmac_get_hw_features(priv);
@@ -1765,17 +1821,32 @@ static int stmmac_hw_init(struct stmmac_priv *priv)
 		 * register (if supported).
 		 */
 		priv->plat->enh_desc = priv->dma_cap.enh_desc;
-		priv->plat->tx_coe = priv->dma_cap.tx_coe;
 		priv->plat->pmt = priv->dma_cap.pmt_remote_wake_up;
+
+		priv->plat->tx_coe = priv->dma_cap.tx_coe;
+
+		if (priv->dma_cap.rx_coe_type2)
+			priv->plat->rx_coe = STMMAC_RX_COE_TYPE2;
+		else if (priv->dma_cap.rx_coe_type1)
+			priv->plat->rx_coe = STMMAC_RX_COE_TYPE1;
+
 	} else
 		pr_info(" No HW DMA feature register supported");
 
 	/* Select the enhnaced/normal descriptor structures */
 	stmmac_selec_desc_mode(priv);
 
-	priv->rx_coe = priv->hw->mac->rx_coe(priv->ioaddr);
-	if (priv->rx_coe)
-		pr_info(" RX Checksum Offload Engine supported\n");
+	/* 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");
+		priv->plat->rx_coe = STMMAC_RX_COE_NONE;
+	}
+
+	if (priv->plat->rx_coe)
+		pr_info(" RX Checksum Offload Engine supported (type %d)\n",
+			priv->plat->rx_coe);
 	if (priv->plat->tx_coe)
 		pr_info(" TX Checksum insertion supported\n");
 
@@ -1856,6 +1927,28 @@ struct stmmac_priv *stmmac_dvr_probe(struct device *device,
 		goto error;
 	}
 
+	if (stmmac_clk_get(priv))
+		pr_warning("%s: warning: cannot get CSR clock\n", __func__);
+
+	/* 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. Viceversa the driver'll try to
+	 * set the MDC clock dynamically according to the csr actual
+	 * clock input.
+	 */
+	if (!priv->plat->clk_csr)
+		stmmac_clk_csr_set(priv);
+	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;
+	}
+
 	return priv;
 
 error:
@@ -1883,6 +1976,7 @@ 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);
 	netif_carrier_off(ndev);
 	unregister_netdev(ndev);
 	free_netdev(ndev);
@@ -1895,6 +1989,7 @@ 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))
 		return 0;
@@ -1902,7 +1997,7 @@ int stmmac_suspend(struct net_device *ndev)
 	if (priv->phydev)
 		phy_stop(priv->phydev);
 
-	spin_lock(&priv->lock);
+	spin_lock_irqsave(&priv->lock, flags);
 
 	netif_device_detach(ndev);
 	netif_stop_queue(ndev);
@@ -1925,21 +2020,24 @@ int stmmac_suspend(struct net_device *ndev)
 	/* Enable Power down mode by programming the PMT regs */
 	if (device_may_wakeup(priv->device))
 		priv->hw->mac->pmt(priv->ioaddr, priv->wolopts);
-	else
+	else {
 		stmmac_set_mac(priv->ioaddr, false);
-
-	spin_unlock(&priv->lock);
+		/* Disable clock in case of PWM is off */
+		stmmac_clk_disable(priv);
+	}
+	spin_unlock_irqrestore(&priv->lock, flags);
 	return 0;
 }
 
 int stmmac_resume(struct net_device *ndev)
 {
 	struct stmmac_priv *priv = netdev_priv(ndev);
+	unsigned long flags;
 
 	if (!netif_running(ndev))
 		return 0;
 
-	spin_lock(&priv->lock);
+	spin_lock_irqsave(&priv->lock, flags);
 
 	/* Power Down bit, into the PM register, is cleared
 	 * automatically as soon as a magic packet or a Wake-up frame
@@ -1948,6 +2046,9 @@ int stmmac_resume(struct net_device *ndev)
 	 * from another devices (e.g. serial console). */
 	if (device_may_wakeup(priv->device))
 		priv->hw->mac->pmt(priv->ioaddr, 0);
+	else
+		/* enable the clk prevously disabled */
+		stmmac_clk_enable(priv);
 
 	netif_device_attach(ndev);
 
@@ -1964,7 +2065,7 @@ int stmmac_resume(struct net_device *ndev)
 
 	netif_start_queue(ndev);
 
-	spin_unlock(&priv->lock);
+	spin_unlock_irqrestore(&priv->lock, flags);
 
 	if (priv->phydev)
 		phy_start(priv->phydev);
diff --git a/drivers/net/ethernet/stmicro/stmmac/stmmac_mdio.c b/drivers/net/ethernet/stmicro/stmmac/stmmac_mdio.c
index 73195329aa4..ade10823204 100644
--- a/drivers/net/ethernet/stmicro/stmmac/stmmac_mdio.c
+++ b/drivers/net/ethernet/stmicro/stmmac/stmmac_mdio.c
@@ -34,6 +34,22 @@
 #define MII_BUSY 0x00000001
 #define MII_WRITE 0x00000002
 
+static int stmmac_mdio_busy_wait(void __iomem *ioaddr, unsigned int mii_addr)
+{
+	unsigned long curr;
+	unsigned long finish = jiffies + 3 * HZ;
+
+	do {
+		curr = jiffies;
+		if (readl(ioaddr + mii_addr) & MII_BUSY)
+			cpu_relax();
+		else
+			return 0;
+	} while (!time_after_eq(curr, finish));
+
+	return -EBUSY;
+}
+
 /**
  * stmmac_mdio_read
  * @bus: points to the mii_bus structure
@@ -54,11 +70,15 @@ static int stmmac_mdio_read(struct mii_bus *bus, int phyaddr, int phyreg)
 	int data;
 	u16 regValue = (((phyaddr << 11) & (0x0000F800)) |
 			((phyreg << 6) & (0x000007C0)));
-	regValue |= MII_BUSY | ((priv->plat->clk_csr & 7) << 2);
+	regValue |= MII_BUSY | ((priv->clk_csr & 0xF) << 2);
+
+	if (stmmac_mdio_busy_wait(priv->ioaddr, mii_address))
+		return -EBUSY;
 
-	do {} while (((readl(priv->ioaddr + mii_address)) & MII_BUSY) == 1);
 	writel(regValue, priv->ioaddr + mii_address);
-	do {} while (((readl(priv->ioaddr + mii_address)) & MII_BUSY) == 1);
+
+	if (stmmac_mdio_busy_wait(priv->ioaddr, mii_address))
+		return -EBUSY;
 
 	/* Read the data from the MII data register */
 	data = (int)readl(priv->ioaddr + mii_data);
@@ -86,20 +106,18 @@ static int stmmac_mdio_write(struct mii_bus *bus, int phyaddr, int phyreg,
 	    (((phyaddr << 11) & (0x0000F800)) | ((phyreg << 6) & (0x000007C0)))
 	    | MII_WRITE;
 
-	value |= MII_BUSY | ((priv->plat->clk_csr & 7) << 2);
-
+	value |= MII_BUSY | ((priv->clk_csr & 0xF) << 2);
 
 	/* Wait until any existing MII operation is complete */
-	do {} while (((readl(priv->ioaddr + mii_address)) & MII_BUSY) == 1);
+	if (stmmac_mdio_busy_wait(priv->ioaddr, mii_address))
+		return -EBUSY;
 
 	/* Set the MII address register to write */
 	writel(phydata, priv->ioaddr + mii_data);
 	writel(value, priv->ioaddr + mii_address);
 
 	/* Wait until any existing MII operation is complete */
-	do {} while (((readl(priv->ioaddr + mii_address)) & MII_BUSY) == 1);
-
-	return 0;
+	return stmmac_mdio_busy_wait(priv->ioaddr, mii_address);
 }
 
 /**
diff --git a/drivers/net/ethernet/stmicro/stmmac/stmmac_pci.c b/drivers/net/ethernet/stmicro/stmmac/stmmac_pci.c
index da66ed7c3c5..58fab5303e9 100644
--- a/drivers/net/ethernet/stmicro/stmmac/stmmac_pci.c
+++ b/drivers/net/ethernet/stmicro/stmmac/stmmac_pci.c
@@ -28,6 +28,7 @@
 
 struct plat_stmmacenet_data plat_dat;
 struct stmmac_mdio_bus_data mdio_data;
+struct stmmac_dma_cfg dma_cfg;
 
 static void stmmac_default_data(void)
 {
@@ -35,7 +36,6 @@ static void stmmac_default_data(void)
 	plat_dat.bus_id = 1;
 	plat_dat.phy_addr = 0;
 	plat_dat.interface = PHY_INTERFACE_MODE_GMII;
-	plat_dat.pbl = 32;
 	plat_dat.clk_csr = 2;	/* clk_csr_i = 20-35MHz & MDC = clk_csr_i/16 */
 	plat_dat.has_gmac = 1;
 	plat_dat.force_sf_dma_mode = 1;
@@ -44,6 +44,10 @@ static void stmmac_default_data(void)
 	mdio_data.phy_reset = NULL;
 	mdio_data.phy_mask = 0;
 	plat_dat.mdio_bus_data = &mdio_data;
+
+	dma_cfg.pbl = 32;
+	dma_cfg.burst_len = DMA_AXI_BLEN_256;
+	plat_dat.dma_cfg = &dma_cfg;
 }
 
 /**
diff --git a/drivers/net/ethernet/stmicro/stmmac/stmmac_platform.c b/drivers/net/ethernet/stmicro/stmmac/stmmac_platform.c
index 116529a366b..3dd8f080380 100644
--- a/drivers/net/ethernet/stmicro/stmmac/stmmac_platform.c
+++ b/drivers/net/ethernet/stmicro/stmmac/stmmac_platform.c
@@ -50,7 +50,6 @@ static int __devinit stmmac_probe_config_dt(struct platform_device *pdev,
 	 * once needed on other platforms.
 	 */
 	if (of_device_is_compatible(np, "st,spear600-gmac")) {
-		plat->pbl = 8;
 		plat->has_gmac = 1;
 		plat->pmt = 1;
 	}
@@ -189,9 +188,6 @@ static int stmmac_pltfr_remove(struct platform_device *pdev)
 	if (priv->plat->exit)
 		priv->plat->exit(pdev);
 
-	if (priv->plat->exit)
-		priv->plat->exit(pdev);
-
 	platform_set_drvdata(pdev, NULL);
 
 	iounmap((void *)priv->ioaddr);
@@ -218,14 +214,26 @@ static int stmmac_pltfr_resume(struct device *dev)
 
 int stmmac_pltfr_freeze(struct device *dev)
 {
+	int ret;
+	struct plat_stmmacenet_data *plat_dat = dev_get_platdata(dev);
 	struct net_device *ndev = dev_get_drvdata(dev);
+	struct platform_device *pdev = to_platform_device(dev);
 
-	return stmmac_freeze(ndev);
+	ret = stmmac_freeze(ndev);
+	if (plat_dat->exit)
+		plat_dat->exit(pdev);
+
+	return ret;
 }
 
 int stmmac_pltfr_restore(struct device *dev)
 {
+	struct plat_stmmacenet_data *plat_dat = dev_get_platdata(dev);
 	struct net_device *ndev = dev_get_drvdata(dev);
+	struct platform_device *pdev = to_platform_device(dev);
+
+	if (plat_dat->init)
+		plat_dat->init(pdev);
 
 	return stmmac_restore(ndev);
 }
diff --git a/drivers/net/ethernet/sun/niu.c b/drivers/net/ethernet/sun/niu.c
index c99b3b0e2ea..703c8cce2a2 100644
--- a/drivers/net/ethernet/sun/niu.c
+++ b/drivers/net/ethernet/sun/niu.c
@@ -9838,7 +9838,7 @@ static int __devinit niu_pci_init_one(struct pci_dev *pdev,
 			goto err_out_release_parent;
 		}
 	}
-	if (err || dma_mask == DMA_BIT_MASK(32)) {
+	if (err) {
 		err = pci_set_dma_mask(pdev, DMA_BIT_MASK(32));
 		if (err) {
 			dev_err(&pdev->dev, "No usable DMA configuration, aborting\n");
diff --git a/drivers/net/ethernet/sun/sungem.c b/drivers/net/ethernet/sun/sungem.c
index 4ba96909671..3cf4ab75583 100644
--- a/drivers/net/ethernet/sun/sungem.c
+++ b/drivers/net/ethernet/sun/sungem.c
@@ -401,7 +401,7 @@ static int gem_rxmac_reset(struct gem *gp)
 		return 1;
 	}
 
-	udelay(5000);
+	mdelay(5);
 
 	/* Execute RX reset command. */
 	writel(gp->swrst_base | GREG_SWRST_RXRST,
@@ -2898,7 +2898,6 @@ static int __devinit gem_init_one(struct pci_dev *pdev,
 	}
 
 	gp->pdev = pdev;
-	dev->base_addr = (long) pdev;
 	gp->dev = dev;
 
 	gp->msg_enable = DEFAULT_MSG;
@@ -2972,7 +2971,6 @@ static int __devinit gem_init_one(struct pci_dev *pdev,
 	netif_napi_add(dev, &gp->napi, gem_poll, 64);
 	dev->ethtool_ops = &gem_ethtool_ops;
 	dev->watchdog_timeo = 5 * HZ;
-	dev->irq = pdev->irq;
 	dev->dma = 0;
 
 	/* Set that now, in case PM kicks in now */
diff --git a/drivers/net/ethernet/sun/sunhme.c b/drivers/net/ethernet/sun/sunhme.c
index b95e7e681b3..dfc00c4683e 100644
--- a/drivers/net/ethernet/sun/sunhme.c
+++ b/drivers/net/ethernet/sun/sunhme.c
@@ -2182,11 +2182,12 @@ static int happy_meal_open(struct net_device *dev)
 	 * into a single source which we register handling at probe time.
 	 */
 	if ((hp->happy_flags & (HFLAG_QUATTRO|HFLAG_PCI)) != HFLAG_QUATTRO) {
-		if (request_irq(dev->irq, happy_meal_interrupt,
-				IRQF_SHARED, dev->name, (void *)dev)) {
+		res = request_irq(hp->irq, happy_meal_interrupt, IRQF_SHARED,
+				  dev->name, dev);
+		if (res) {
 			HMD(("EAGAIN\n"));
 			printk(KERN_ERR "happy_meal(SBUS): Can't order irq %d to go.\n",
-			       dev->irq);
+			       hp->irq);
 
 			return -EAGAIN;
 		}
@@ -2199,7 +2200,7 @@ static int happy_meal_open(struct net_device *dev)
 	spin_unlock_irq(&hp->happy_lock);
 
 	if (res && ((hp->happy_flags & (HFLAG_QUATTRO|HFLAG_PCI)) != HFLAG_QUATTRO))
-		free_irq(dev->irq, dev);
+		free_irq(hp->irq, dev);
 	return res;
 }
 
@@ -2221,7 +2222,7 @@ static int happy_meal_close(struct net_device *dev)
 	 * time and never unregister.
 	 */
 	if ((hp->happy_flags & (HFLAG_QUATTRO|HFLAG_PCI)) != HFLAG_QUATTRO)
-		free_irq(dev->irq, dev);
+		free_irq(hp->irq, dev);
 
 	return 0;
 }
@@ -2777,7 +2778,7 @@ static int __devinit happy_meal_sbus_probe_one(struct platform_device *op, int i
 	dev->hw_features = NETIF_F_SG | NETIF_F_HW_CSUM;
 	dev->features |= dev->hw_features | NETIF_F_RXCSUM;
 
-	dev->irq = op->archdata.irqs[0];
+	hp->irq = op->archdata.irqs[0];
 
 #if defined(CONFIG_SBUS) && defined(CONFIG_PCI)
 	/* Hook up SBUS register/descriptor accessors. */
@@ -2981,8 +2982,6 @@ static int __devinit happy_meal_pci_probe(struct pci_dev *pdev,
 	if (hme_version_printed++ == 0)
 		printk(KERN_INFO "%s", version);
 
-	dev->base_addr = (long) pdev;
-
 	hp = netdev_priv(dev);
 
 	hp->happy_dev = pdev;
@@ -3087,12 +3086,11 @@ static int __devinit happy_meal_pci_probe(struct pci_dev *pdev,
 
 	init_timer(&hp->happy_timer);
 
+	hp->irq = pdev->irq;
 	hp->dev = dev;
 	dev->netdev_ops = &hme_netdev_ops;
 	dev->watchdog_timeo = 5*HZ;
 	dev->ethtool_ops = &hme_ethtool_ops;
-	dev->irq = pdev->irq;
-	dev->dma = 0;
 
 	/* Happy Meal can do it all... */
 	dev->hw_features = NETIF_F_SG | NETIF_F_HW_CSUM;
diff --git a/drivers/net/ethernet/sun/sunhme.h b/drivers/net/ethernet/sun/sunhme.h
index 64f278360d8..f4307654e4a 100644
--- a/drivers/net/ethernet/sun/sunhme.h
+++ b/drivers/net/ethernet/sun/sunhme.h
@@ -432,6 +432,7 @@ struct happy_meal {
 
 	dma_addr_t                hblock_dvma;    /* DVMA visible address happy block  */
 	unsigned int              happy_flags;    /* Driver state flags                */
+	int                       irq;
 	enum happy_transceiver    tcvr_type;      /* Kind of transceiver in use        */
 	unsigned int              happy_bursts;   /* Get your mind out of the gutter   */
 	unsigned int              paddr;          /* PHY address for transceiver       */
diff --git a/drivers/net/ethernet/sun/sunvnet.c b/drivers/net/ethernet/sun/sunvnet.c
index 38e3ae9155b..a108db35924 100644
--- a/drivers/net/ethernet/sun/sunvnet.c
+++ b/drivers/net/ethernet/sun/sunvnet.c
@@ -618,7 +618,7 @@ struct vnet_port *__tx_port_find(struct vnet *vp, struct sk_buff *skb)
 	struct vnet_port *port;
 
 	hlist_for_each_entry(port, n, hp, hash) {
-		if (!compare_ether_addr(port->raddr, skb->data))
+		if (ether_addr_equal(port->raddr, skb->data))
 			return port;
 	}
 	port = NULL;
diff --git a/drivers/net/ethernet/tehuti/tehuti.c b/drivers/net/ethernet/tehuti/tehuti.c
index ad973ffc9ff..447a6932cab 100644
--- a/drivers/net/ethernet/tehuti/tehuti.c
+++ b/drivers/net/ethernet/tehuti/tehuti.c
@@ -341,8 +341,8 @@ static int bdx_fw_load(struct bdx_priv *priv)
 out:
 	if (master)
 		WRITE_REG(priv, regINIT_SEMAPHORE, 1);
-	if (fw)
-		release_firmware(fw);
+
+	release_firmware(fw);
 
 	if (rc) {
 		netdev_err(priv->ndev, "firmware loading failed\n");
@@ -1317,7 +1317,7 @@ static void print_rxdd(struct rxd_desc *rxdd, u32 rxd_val1, u16 len,
 
 static void print_rxfd(struct rxf_desc *rxfd)
 {
-	DBG("=== RxF desc CHIP ORDER/ENDIANESS =============\n"
+	DBG("=== RxF desc CHIP ORDER/ENDIANNESS =============\n"
 	    "info 0x%x va_lo %u pa_lo 0x%x pa_hi 0x%x len 0x%x\n",
 	    rxfd->info, rxfd->va_lo, rxfd->pa_lo, rxfd->pa_hi, rxfd->len);
 }
@@ -1988,10 +1988,6 @@ bdx_probe(struct pci_dev *pdev, const struct pci_device_id *ent)
 		/* these fields are used for info purposes only
 		 * so we can have them same for all ports of the board */
 		ndev->if_port = port;
-		ndev->base_addr = pciaddr;
-		ndev->mem_start = pciaddr;
-		ndev->mem_end = pciaddr + regionSize;
-		ndev->irq = pdev->irq;
 		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
diff --git a/drivers/net/ethernet/ti/Kconfig b/drivers/net/ethernet/ti/Kconfig
index b42252c4bec..1b173a6145d 100644
--- a/drivers/net/ethernet/ti/Kconfig
+++ b/drivers/net/ethernet/ti/Kconfig
@@ -51,7 +51,7 @@ config TI_DAVINCI_CPDMA
 
 config TI_CPSW
 	tristate "TI CPSW Switch Support"
-	depends on ARM && (ARCH_DAVINCI || SOC_OMAPAM33XX)
+	depends on ARM && (ARCH_DAVINCI || SOC_AM33XX)
 	select TI_DAVINCI_CPDMA
 	select TI_DAVINCI_MDIO
 	---help---
diff --git a/drivers/net/ethernet/ti/davinci_cpdma.c b/drivers/net/ethernet/ti/davinci_cpdma.c
index 34558766cbf..d614c374ed9 100644
--- a/drivers/net/ethernet/ti/davinci_cpdma.c
+++ b/drivers/net/ethernet/ti/davinci_cpdma.c
@@ -92,7 +92,7 @@ enum cpdma_state {
 	CPDMA_STATE_TEARDOWN,
 };
 
-const char *cpdma_state_str[] = { "idle", "active", "teardown" };
+static const char *cpdma_state_str[] = { "idle", "active", "teardown" };
 
 struct cpdma_ctlr {
 	enum cpdma_state	state;
@@ -276,6 +276,7 @@ struct cpdma_ctlr *cpdma_ctlr_create(struct cpdma_params *params)
 		ctlr->num_chan = CPDMA_MAX_CHANNELS;
 	return ctlr;
 }
+EXPORT_SYMBOL_GPL(cpdma_ctlr_create);
 
 int cpdma_ctlr_start(struct cpdma_ctlr *ctlr)
 {
@@ -321,6 +322,7 @@ int cpdma_ctlr_start(struct cpdma_ctlr *ctlr)
 	spin_unlock_irqrestore(&ctlr->lock, flags);
 	return 0;
 }
+EXPORT_SYMBOL_GPL(cpdma_ctlr_start);
 
 int cpdma_ctlr_stop(struct cpdma_ctlr *ctlr)
 {
@@ -351,6 +353,7 @@ int cpdma_ctlr_stop(struct cpdma_ctlr *ctlr)
 	spin_unlock_irqrestore(&ctlr->lock, flags);
 	return 0;
 }
+EXPORT_SYMBOL_GPL(cpdma_ctlr_stop);
 
 int cpdma_ctlr_dump(struct cpdma_ctlr *ctlr)
 {
@@ -421,6 +424,7 @@ int cpdma_ctlr_dump(struct cpdma_ctlr *ctlr)
 	spin_unlock_irqrestore(&ctlr->lock, flags);
 	return 0;
 }
+EXPORT_SYMBOL_GPL(cpdma_ctlr_dump);
 
 int cpdma_ctlr_destroy(struct cpdma_ctlr *ctlr)
 {
@@ -444,6 +448,7 @@ int cpdma_ctlr_destroy(struct cpdma_ctlr *ctlr)
 	kfree(ctlr);
 	return ret;
 }
+EXPORT_SYMBOL_GPL(cpdma_ctlr_destroy);
 
 int cpdma_ctlr_int_ctrl(struct cpdma_ctlr *ctlr, bool enable)
 {
@@ -528,6 +533,7 @@ err_chan_busy:
 err_chan_alloc:
 	return ERR_PTR(ret);
 }
+EXPORT_SYMBOL_GPL(cpdma_chan_create);
 
 int cpdma_chan_destroy(struct cpdma_chan *chan)
 {
@@ -545,6 +551,7 @@ int cpdma_chan_destroy(struct cpdma_chan *chan)
 	kfree(chan);
 	return 0;
 }
+EXPORT_SYMBOL_GPL(cpdma_chan_destroy);
 
 int cpdma_chan_get_stats(struct cpdma_chan *chan,
 			 struct cpdma_chan_stats *stats)
@@ -693,6 +700,7 @@ unlock_ret:
 	spin_unlock_irqrestore(&chan->lock, flags);
 	return ret;
 }
+EXPORT_SYMBOL_GPL(cpdma_chan_submit);
 
 static void __cpdma_chan_free(struct cpdma_chan *chan,
 			      struct cpdma_desc __iomem *desc,
@@ -776,6 +784,7 @@ int cpdma_chan_process(struct cpdma_chan *chan, int quota)
 	}
 	return used;
 }
+EXPORT_SYMBOL_GPL(cpdma_chan_process);
 
 int cpdma_chan_start(struct cpdma_chan *chan)
 {
@@ -803,6 +812,7 @@ int cpdma_chan_start(struct cpdma_chan *chan)
 	spin_unlock_irqrestore(&chan->lock, flags);
 	return 0;
 }
+EXPORT_SYMBOL_GPL(cpdma_chan_start);
 
 int cpdma_chan_stop(struct cpdma_chan *chan)
 {
@@ -863,6 +873,7 @@ int cpdma_chan_stop(struct cpdma_chan *chan)
 	spin_unlock_irqrestore(&chan->lock, flags);
 	return 0;
 }
+EXPORT_SYMBOL_GPL(cpdma_chan_stop);
 
 int cpdma_chan_int_ctrl(struct cpdma_chan *chan, bool enable)
 {
diff --git a/drivers/net/ethernet/ti/davinci_emac.c b/drivers/net/ethernet/ti/davinci_emac.c
index 08aff1a2087..4da93a5d7ec 100644
--- a/drivers/net/ethernet/ti/davinci_emac.c
+++ b/drivers/net/ethernet/ti/davinci_emac.c
@@ -627,6 +627,7 @@ static const struct ethtool_ops ethtool_ops = {
 	.get_link = ethtool_op_get_link,
 	.get_coalesce = emac_get_coalesce,
 	.set_coalesce =  emac_set_coalesce,
+	.get_ts_info = ethtool_op_get_ts_info,
 };
 
 /**
diff --git a/drivers/net/ethernet/ti/tlan.c b/drivers/net/ethernet/ti/tlan.c
index efd36691ce5..3e6abf0f277 100644
--- a/drivers/net/ethernet/ti/tlan.c
+++ b/drivers/net/ethernet/ti/tlan.c
@@ -2545,7 +2545,7 @@ static void tlan_phy_reset(struct net_device *dev)
 
 	phy = priv->phy[priv->phy_num];
 
-	TLAN_DBG(TLAN_DEBUG_GNRL, "%s: Reseting PHY.\n", dev->name);
+	TLAN_DBG(TLAN_DEBUG_GNRL, "%s: Resetting PHY.\n", dev->name);
 	tlan_mii_sync(dev->base_addr);
 	value = MII_GC_LOOPBK | MII_GC_RESET;
 	tlan_mii_write_reg(dev, phy, MII_GEN_CTL, value);
diff --git a/drivers/net/ethernet/tile/tilepro.c b/drivers/net/ethernet/tile/tilepro.c
index 3d501ec7fad..96070e9b50d 100644
--- a/drivers/net/ethernet/tile/tilepro.c
+++ b/drivers/net/ethernet/tile/tilepro.c
@@ -843,7 +843,7 @@ static bool tile_net_poll_aux(struct tile_net_cpu *info, int index)
 		if (!is_multicast_ether_addr(buf)) {
 			/* Filter packets not for our address. */
 			const u8 *mine = dev->dev_addr;
-			filter = compare_ether_addr(mine, buf);
+			filter = !ether_addr_equal(mine, buf);
 		}
 	}
 
diff --git a/drivers/net/ethernet/toshiba/ps3_gelic_wireless.c b/drivers/net/ethernet/toshiba/ps3_gelic_wireless.c
index 5c14f82c495..961c8321451 100644
--- a/drivers/net/ethernet/toshiba/ps3_gelic_wireless.c
+++ b/drivers/net/ethernet/toshiba/ps3_gelic_wireless.c
@@ -1590,8 +1590,8 @@ static void gelic_wl_scan_complete_event(struct gelic_wl_info *wl)
 		found = 0;
 		oldest = NULL;
 		list_for_each_entry(target, &wl->network_list, list) {
-			if (!compare_ether_addr(&target->hwinfo->bssid[2],
-						&scan_info->bssid[2])) {
+			if (ether_addr_equal(&target->hwinfo->bssid[2],
+					     &scan_info->bssid[2])) {
 				found = 1;
 				pr_debug("%s: same BBS found scanned list\n",
 					 __func__);
@@ -1691,8 +1691,8 @@ struct gelic_wl_scan_info *gelic_wl_find_best_bss(struct gelic_wl_info *wl)
 
 		/* If bss specified, check it only */
 		if (test_bit(GELIC_WL_STAT_BSSID_SET, &wl->stat)) {
-			if (!compare_ether_addr(&scan_info->hwinfo->bssid[2],
-						wl->bssid)) {
+			if (ether_addr_equal(&scan_info->hwinfo->bssid[2],
+					     wl->bssid)) {
 				best_bss = scan_info;
 				pr_debug("%s: bssid matched\n", __func__);
 				break;
diff --git a/drivers/net/ethernet/via/via-rhine.c b/drivers/net/ethernet/via/via-rhine.c
index fcfa01f7ceb..0459c096629 100644
--- a/drivers/net/ethernet/via/via-rhine.c
+++ b/drivers/net/ethernet/via/via-rhine.c
@@ -689,9 +689,12 @@ static void __devinit rhine_reload_eeprom(long pioaddr, struct net_device *dev)
 #ifdef CONFIG_NET_POLL_CONTROLLER
 static void rhine_poll(struct net_device *dev)
 {
-	disable_irq(dev->irq);
-	rhine_interrupt(dev->irq, (void *)dev);
-	enable_irq(dev->irq);
+	struct rhine_private *rp = netdev_priv(dev);
+	const int irq = rp->pdev->irq;
+
+	disable_irq(irq);
+	rhine_interrupt(irq, dev);
+	enable_irq(irq);
 }
 #endif
 
@@ -972,7 +975,6 @@ static int __devinit rhine_init_one(struct pci_dev *pdev,
 	}
 #endif /* USE_MMIO */
 
-	dev->base_addr = (unsigned long)ioaddr;
 	rp->base = ioaddr;
 
 	/* Get chip registers into a sane state */
@@ -995,8 +997,6 @@ static int __devinit rhine_init_one(struct pci_dev *pdev,
 	if (!phy_id)
 		phy_id = ioread8(ioaddr + 0x6C);
 
-	dev->irq = pdev->irq;
-
 	spin_lock_init(&rp->lock);
 	mutex_init(&rp->task_lock);
 	INIT_WORK(&rp->reset_task, rhine_reset_task);
diff --git a/drivers/net/ethernet/via/via-velocity.c b/drivers/net/ethernet/via/via-velocity.c
index 8a5d7c100a5..ea3e0a21ba7 100644
--- a/drivers/net/ethernet/via/via-velocity.c
+++ b/drivers/net/ethernet/via/via-velocity.c
@@ -2488,8 +2488,8 @@ static int velocity_close(struct net_device *dev)
 
 	if (vptr->flags & VELOCITY_FLAGS_WOL_ENABLED)
 		velocity_get_ip(vptr);
-	if (dev->irq != 0)
-		free_irq(dev->irq, dev);
+
+	free_irq(vptr->pdev->irq, dev);
 
 	velocity_free_rings(vptr);
 
@@ -2755,8 +2755,6 @@ static int __devinit velocity_found1(struct pci_dev *pdev, const struct pci_devi
 	if (ret < 0)
 		goto err_free_dev;
 
-	dev->irq = pdev->irq;
-
 	ret = velocity_get_pci_info(vptr, pdev);
 	if (ret < 0) {
 		/* error message already printed */
@@ -2779,8 +2777,6 @@ static int __devinit velocity_found1(struct pci_dev *pdev, const struct pci_devi
 
 	mac_wol_reset(regs);
 
-	dev->base_addr = vptr->ioaddr;
-
 	for (i = 0; i < 6; i++)
 		dev->dev_addr[i] = readb(&regs->PAR[i]);
 
@@ -2806,7 +2802,6 @@ static int __devinit velocity_found1(struct pci_dev *pdev, const struct pci_devi
 
 	vptr->phy_id = MII_GET_PHY_ID(vptr->mac_regs);
 
-	dev->irq = pdev->irq;
 	dev->netdev_ops = &velocity_netdev_ops;
 	dev->ethtool_ops = &velocity_ethtool_ops;
 	netif_napi_add(dev, &vptr->napi, velocity_poll, VELOCITY_NAPI_WEIGHT);
diff --git a/drivers/net/ethernet/wiznet/Kconfig b/drivers/net/ethernet/wiznet/Kconfig
new file mode 100644
index 00000000000..cb18043f583
--- /dev/null
+++ b/drivers/net/ethernet/wiznet/Kconfig
@@ -0,0 +1,73 @@
+#
+# WIZnet devices configuration
+#
+
+config NET_VENDOR_WIZNET
+	bool "WIZnet devices"
+	default y
+	---help---
+	  If you have a network (Ethernet) card belonging to this class, say Y
+	  and read the Ethernet-HOWTO, available from
+	  <http://www.tldp.org/docs.html#howto>.
+
+	  Note that the answer to this question doesn't directly affect the
+	  kernel: saying N will just cause the configurator to skip all
+	  the questions about WIZnet devices. If you say Y, you will be asked
+	  for your specific card in the following questions.
+
+if NET_VENDOR_WIZNET
+
+config WIZNET_W5100
+	tristate "WIZnet W5100 Ethernet support"
+	depends on HAS_IOMEM
+	---help---
+	  Support for WIZnet W5100 chips.
+
+	  W5100 is a single chip with integrated 10/100 Ethernet MAC,
+	  PHY and hardware TCP/IP stack, but this driver is limited to
+	  the MAC and PHY functions only, onchip TCP/IP is unused.
+
+	  To compile this driver as a module, choose M here: the module
+	  will be called w5100.
+
+config WIZNET_W5300
+	tristate "WIZnet W5300 Ethernet support"
+	depends on HAS_IOMEM
+	---help---
+	  Support for WIZnet W5300 chips.
+
+	  W5300 is a single chip with integrated 10/100 Ethernet MAC,
+	  PHY and hardware TCP/IP stack, but this driver is limited to
+	  the MAC and PHY functions only, onchip TCP/IP is unused.
+
+	  To compile this driver as a module, choose M here: the module
+	  will be called w5300.
+
+choice
+	prompt "WIZnet interface mode"
+	depends on WIZNET_W5100 || WIZNET_W5300
+	default WIZNET_BUS_ANY
+
+config WIZNET_BUS_DIRECT
+	bool "Direct address bus mode"
+	---help---
+	  In direct address mode host system can directly access all registers
+	  after mapping to Memory-Mapped I/O space.
+
+config WIZNET_BUS_INDIRECT
+	bool "Indirect address bus mode"
+	---help---
+	  In indirect address mode host system indirectly accesses registers
+	  using Indirect Mode Address Register and Indirect Mode Data Register,
+	  which are directly mapped to Memory-Mapped I/O space.
+
+config WIZNET_BUS_ANY
+	bool "Select interface mode in runtime"
+	---help---
+	  If interface mode is unknown in compile time, it can be selected
+	  in runtime from board/platform resources configuration.
+
+	  Performance may decrease compared to explicitly selected bus mode.
+endchoice
+
+endif # NET_VENDOR_WIZNET
diff --git a/drivers/net/ethernet/wiznet/Makefile b/drivers/net/ethernet/wiznet/Makefile
new file mode 100644
index 00000000000..c614535227e
--- /dev/null
+++ b/drivers/net/ethernet/wiznet/Makefile
@@ -0,0 +1,2 @@
+obj-$(CONFIG_WIZNET_W5100) += w5100.o
+obj-$(CONFIG_WIZNET_W5300) += w5300.o
diff --git a/drivers/net/ethernet/wiznet/w5100.c b/drivers/net/ethernet/wiznet/w5100.c
new file mode 100644
index 00000000000..a75e9ef5a4c
--- /dev/null
+++ b/drivers/net/ethernet/wiznet/w5100.c
@@ -0,0 +1,808 @@
+/*
+ * Ethernet driver for the WIZnet W5100 chip.
+ *
+ * Copyright (C) 2006-2008 WIZnet Co.,Ltd.
+ * Copyright (C) 2012 Mike Sinkovsky <msink@permonline.ru>
+ *
+ * Licensed under the GPL-2 or later.
+ */
+
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/kconfig.h>
+#include <linux/netdevice.h>
+#include <linux/etherdevice.h>
+#include <linux/platform_device.h>
+#include <linux/platform_data/wiznet.h>
+#include <linux/ethtool.h>
+#include <linux/skbuff.h>
+#include <linux/types.h>
+#include <linux/errno.h>
+#include <linux/delay.h>
+#include <linux/slab.h>
+#include <linux/spinlock.h>
+#include <linux/io.h>
+#include <linux/ioport.h>
+#include <linux/interrupt.h>
+#include <linux/irq.h>
+#include <linux/gpio.h>
+
+#define DRV_NAME	"w5100"
+#define DRV_VERSION	"2012-04-04"
+
+MODULE_DESCRIPTION("WIZnet W5100 Ethernet driver v"DRV_VERSION);
+MODULE_AUTHOR("Mike Sinkovsky <msink@permonline.ru>");
+MODULE_ALIAS("platform:"DRV_NAME);
+MODULE_LICENSE("GPL");
+
+/*
+ * Registers
+ */
+#define W5100_COMMON_REGS	0x0000
+#define W5100_MR		0x0000 /* Mode Register */
+#define   MR_RST		  0x80 /* S/W reset */
+#define   MR_PB			  0x10 /* Ping block */
+#define   MR_AI			  0x02 /* Address Auto-Increment */
+#define   MR_IND		  0x01 /* Indirect mode */
+#define W5100_SHAR		0x0009 /* Source MAC address */
+#define W5100_IR		0x0015 /* Interrupt Register */
+#define W5100_IMR		0x0016 /* Interrupt Mask Register */
+#define   IR_S0			  0x01 /* S0 interrupt */
+#define W5100_RTR		0x0017 /* Retry Time-value Register */
+#define   RTR_DEFAULT		  2000 /* =0x07d0 (2000) */
+#define W5100_RMSR		0x001a /* Receive Memory Size */
+#define W5100_TMSR		0x001b /* Transmit Memory Size */
+#define W5100_COMMON_REGS_LEN	0x0040
+
+#define W5100_S0_REGS		0x0400
+#define W5100_S0_MR		0x0400 /* S0 Mode Register */
+#define   S0_MR_MACRAW		  0x04 /* MAC RAW mode (promiscous) */
+#define   S0_MR_MACRAW_MF	  0x44 /* MAC RAW mode (filtered) */
+#define W5100_S0_CR		0x0401 /* S0 Command Register */
+#define   S0_CR_OPEN		  0x01 /* OPEN command */
+#define   S0_CR_CLOSE		  0x10 /* CLOSE command */
+#define   S0_CR_SEND		  0x20 /* SEND command */
+#define   S0_CR_RECV		  0x40 /* RECV command */
+#define W5100_S0_IR		0x0402 /* S0 Interrupt Register */
+#define   S0_IR_SENDOK		  0x10 /* complete sending */
+#define   S0_IR_RECV		  0x04 /* receiving data */
+#define W5100_S0_SR		0x0403 /* S0 Status Register */
+#define   S0_SR_MACRAW		  0x42 /* mac raw mode */
+#define W5100_S0_TX_FSR		0x0420 /* S0 Transmit free memory size */
+#define W5100_S0_TX_RD		0x0422 /* S0 Transmit memory read pointer */
+#define W5100_S0_TX_WR		0x0424 /* S0 Transmit memory write pointer */
+#define W5100_S0_RX_RSR		0x0426 /* S0 Receive free memory size */
+#define W5100_S0_RX_RD		0x0428 /* S0 Receive memory read pointer */
+#define W5100_S0_REGS_LEN	0x0040
+
+#define W5100_TX_MEM_START	0x4000
+#define W5100_TX_MEM_END	0x5fff
+#define W5100_TX_MEM_MASK	0x1fff
+#define W5100_RX_MEM_START	0x6000
+#define W5100_RX_MEM_END	0x7fff
+#define W5100_RX_MEM_MASK	0x1fff
+
+/*
+ * Device driver private data structure
+ */
+struct w5100_priv {
+	void __iomem *base;
+	spinlock_t reg_lock;
+	bool indirect;
+	u8   (*read)(struct w5100_priv *priv, u16 addr);
+	void (*write)(struct w5100_priv *priv, u16 addr, u8 data);
+	u16  (*read16)(struct w5100_priv *priv, u16 addr);
+	void (*write16)(struct w5100_priv *priv, u16 addr, u16 data);
+	void (*readbuf)(struct w5100_priv *priv, u16 addr, u8 *buf, int len);
+	void (*writebuf)(struct w5100_priv *priv, u16 addr, u8 *buf, int len);
+	int irq;
+	int link_irq;
+	int link_gpio;
+
+	struct napi_struct napi;
+	struct net_device *ndev;
+	bool promisc;
+	u32 msg_enable;
+};
+
+/************************************************************************
+ *
+ *  Lowlevel I/O functions
+ *
+ ***********************************************************************/
+
+/*
+ * In direct address mode host system can directly access W5100 registers
+ * after mapping to Memory-Mapped I/O space.
+ *
+ * 0x8000 bytes are required for memory space.
+ */
+static inline u8 w5100_read_direct(struct w5100_priv *priv, u16 addr)
+{
+	return ioread8(priv->base + (addr << CONFIG_WIZNET_BUS_SHIFT));
+}
+
+static inline void w5100_write_direct(struct w5100_priv *priv,
+				      u16 addr, u8 data)
+{
+	iowrite8(data, priv->base + (addr << CONFIG_WIZNET_BUS_SHIFT));
+}
+
+static u16 w5100_read16_direct(struct w5100_priv *priv, u16 addr)
+{
+	u16 data;
+	data  = w5100_read_direct(priv, addr) << 8;
+	data |= w5100_read_direct(priv, addr + 1);
+	return data;
+}
+
+static void w5100_write16_direct(struct w5100_priv *priv, u16 addr, u16 data)
+{
+	w5100_write_direct(priv, addr, data >> 8);
+	w5100_write_direct(priv, addr + 1, data);
+}
+
+static void w5100_readbuf_direct(struct w5100_priv *priv,
+				 u16 offset, u8 *buf, int len)
+{
+	u16 addr = W5100_RX_MEM_START + (offset & W5100_RX_MEM_MASK);
+	int i;
+
+	for (i = 0; i < len; i++, addr++) {
+		if (unlikely(addr > W5100_RX_MEM_END))
+			addr = W5100_RX_MEM_START;
+		*buf++ = w5100_read_direct(priv, addr);
+	}
+}
+
+static void w5100_writebuf_direct(struct w5100_priv *priv,
+				  u16 offset, u8 *buf, int len)
+{
+	u16 addr = W5100_TX_MEM_START + (offset & W5100_TX_MEM_MASK);
+	int i;
+
+	for (i = 0; i < len; i++, addr++) {
+		if (unlikely(addr > W5100_TX_MEM_END))
+			addr = W5100_TX_MEM_START;
+		w5100_write_direct(priv, addr, *buf++);
+	}
+}
+
+/*
+ * In indirect address mode host system indirectly accesses registers by
+ * using Indirect Mode Address Register (IDM_AR) and Indirect Mode Data
+ * Register (IDM_DR), which are directly mapped to Memory-Mapped I/O space.
+ * Mode Register (MR) is directly accessible.
+ *
+ * Only 0x04 bytes are required for memory space.
+ */
+#define W5100_IDM_AR		0x01   /* Indirect Mode Address Register */
+#define W5100_IDM_DR		0x03   /* Indirect Mode Data Register */
+
+static u8 w5100_read_indirect(struct w5100_priv *priv, u16 addr)
+{
+	unsigned long flags;
+	u8 data;
+
+	spin_lock_irqsave(&priv->reg_lock, flags);
+	w5100_write16_direct(priv, W5100_IDM_AR, addr);
+	mmiowb();
+	data = w5100_read_direct(priv, W5100_IDM_DR);
+	spin_unlock_irqrestore(&priv->reg_lock, flags);
+
+	return data;
+}
+
+static void w5100_write_indirect(struct w5100_priv *priv, u16 addr, u8 data)
+{
+	unsigned long flags;
+
+	spin_lock_irqsave(&priv->reg_lock, flags);
+	w5100_write16_direct(priv, W5100_IDM_AR, addr);
+	mmiowb();
+	w5100_write_direct(priv, W5100_IDM_DR, data);
+	mmiowb();
+	spin_unlock_irqrestore(&priv->reg_lock, flags);
+}
+
+static u16 w5100_read16_indirect(struct w5100_priv *priv, u16 addr)
+{
+	unsigned long flags;
+	u16 data;
+
+	spin_lock_irqsave(&priv->reg_lock, flags);
+	w5100_write16_direct(priv, W5100_IDM_AR, addr);
+	mmiowb();
+	data  = w5100_read_direct(priv, W5100_IDM_DR) << 8;
+	data |= w5100_read_direct(priv, W5100_IDM_DR);
+	spin_unlock_irqrestore(&priv->reg_lock, flags);
+
+	return data;
+}
+
+static void w5100_write16_indirect(struct w5100_priv *priv, u16 addr, u16 data)
+{
+	unsigned long flags;
+
+	spin_lock_irqsave(&priv->reg_lock, flags);
+	w5100_write16_direct(priv, W5100_IDM_AR, addr);
+	mmiowb();
+	w5100_write_direct(priv, W5100_IDM_DR, data >> 8);
+	w5100_write_direct(priv, W5100_IDM_DR, data);
+	mmiowb();
+	spin_unlock_irqrestore(&priv->reg_lock, flags);
+}
+
+static void w5100_readbuf_indirect(struct w5100_priv *priv,
+				   u16 offset, u8 *buf, int len)
+{
+	u16 addr = W5100_RX_MEM_START + (offset & W5100_RX_MEM_MASK);
+	unsigned long flags;
+	int i;
+
+	spin_lock_irqsave(&priv->reg_lock, flags);
+	w5100_write16_direct(priv, W5100_IDM_AR, addr);
+	mmiowb();
+
+	for (i = 0; i < len; i++, addr++) {
+		if (unlikely(addr > W5100_RX_MEM_END)) {
+			addr = W5100_RX_MEM_START;
+			w5100_write16_direct(priv, W5100_IDM_AR, addr);
+			mmiowb();
+		}
+		*buf++ = w5100_read_direct(priv, W5100_IDM_DR);
+	}
+	mmiowb();
+	spin_unlock_irqrestore(&priv->reg_lock, flags);
+}
+
+static void w5100_writebuf_indirect(struct w5100_priv *priv,
+				    u16 offset, u8 *buf, int len)
+{
+	u16 addr = W5100_TX_MEM_START + (offset & W5100_TX_MEM_MASK);
+	unsigned long flags;
+	int i;
+
+	spin_lock_irqsave(&priv->reg_lock, flags);
+	w5100_write16_direct(priv, W5100_IDM_AR, addr);
+	mmiowb();
+
+	for (i = 0; i < len; i++, addr++) {
+		if (unlikely(addr > W5100_TX_MEM_END)) {
+			addr = W5100_TX_MEM_START;
+			w5100_write16_direct(priv, W5100_IDM_AR, addr);
+			mmiowb();
+		}
+		w5100_write_direct(priv, W5100_IDM_DR, *buf++);
+	}
+	mmiowb();
+	spin_unlock_irqrestore(&priv->reg_lock, flags);
+}
+
+#if defined(CONFIG_WIZNET_BUS_DIRECT)
+#define w5100_read	w5100_read_direct
+#define w5100_write	w5100_write_direct
+#define w5100_read16	w5100_read16_direct
+#define w5100_write16	w5100_write16_direct
+#define w5100_readbuf	w5100_readbuf_direct
+#define w5100_writebuf	w5100_writebuf_direct
+
+#elif defined(CONFIG_WIZNET_BUS_INDIRECT)
+#define w5100_read	w5100_read_indirect
+#define w5100_write	w5100_write_indirect
+#define w5100_read16	w5100_read16_indirect
+#define w5100_write16	w5100_write16_indirect
+#define w5100_readbuf	w5100_readbuf_indirect
+#define w5100_writebuf	w5100_writebuf_indirect
+
+#else /* CONFIG_WIZNET_BUS_ANY */
+#define w5100_read	priv->read
+#define w5100_write	priv->write
+#define w5100_read16	priv->read16
+#define w5100_write16	priv->write16
+#define w5100_readbuf	priv->readbuf
+#define w5100_writebuf	priv->writebuf
+#endif
+
+static int w5100_command(struct w5100_priv *priv, u16 cmd)
+{
+	unsigned long timeout = jiffies + msecs_to_jiffies(100);
+
+	w5100_write(priv, W5100_S0_CR, cmd);
+	mmiowb();
+
+	while (w5100_read(priv, W5100_S0_CR) != 0) {
+		if (time_after(jiffies, timeout))
+			return -EIO;
+		cpu_relax();
+	}
+
+	return 0;
+}
+
+static void w5100_write_macaddr(struct w5100_priv *priv)
+{
+	struct net_device *ndev = priv->ndev;
+	int i;
+
+	for (i = 0; i < ETH_ALEN; i++)
+		w5100_write(priv, W5100_SHAR + i, ndev->dev_addr[i]);
+	mmiowb();
+}
+
+static void w5100_hw_reset(struct w5100_priv *priv)
+{
+	w5100_write_direct(priv, W5100_MR, MR_RST);
+	mmiowb();
+	mdelay(5);
+	w5100_write_direct(priv, W5100_MR, priv->indirect ?
+				  MR_PB | MR_AI | MR_IND :
+				  MR_PB);
+	mmiowb();
+	w5100_write(priv, W5100_IMR, 0);
+	w5100_write_macaddr(priv);
+
+	/* Configure 16K of internal memory
+	 * as 8K RX buffer and 8K TX buffer
+	 */
+	w5100_write(priv, W5100_RMSR, 0x03);
+	w5100_write(priv, W5100_TMSR, 0x03);
+	mmiowb();
+}
+
+static void w5100_hw_start(struct w5100_priv *priv)
+{
+	w5100_write(priv, W5100_S0_MR, priv->promisc ?
+			  S0_MR_MACRAW : S0_MR_MACRAW_MF);
+	mmiowb();
+	w5100_command(priv, S0_CR_OPEN);
+	w5100_write(priv, W5100_IMR, IR_S0);
+	mmiowb();
+}
+
+static void w5100_hw_close(struct w5100_priv *priv)
+{
+	w5100_write(priv, W5100_IMR, 0);
+	mmiowb();
+	w5100_command(priv, S0_CR_CLOSE);
+}
+
+/***********************************************************************
+ *
+ *   Device driver functions / callbacks
+ *
+ ***********************************************************************/
+
+static void w5100_get_drvinfo(struct net_device *ndev,
+			      struct ethtool_drvinfo *info)
+{
+	strlcpy(info->driver, DRV_NAME, sizeof(info->driver));
+	strlcpy(info->version, DRV_VERSION, sizeof(info->version));
+	strlcpy(info->bus_info, dev_name(ndev->dev.parent),
+		sizeof(info->bus_info));
+}
+
+static u32 w5100_get_link(struct net_device *ndev)
+{
+	struct w5100_priv *priv = netdev_priv(ndev);
+
+	if (gpio_is_valid(priv->link_gpio))
+		return !!gpio_get_value(priv->link_gpio);
+
+	return 1;
+}
+
+static u32 w5100_get_msglevel(struct net_device *ndev)
+{
+	struct w5100_priv *priv = netdev_priv(ndev);
+
+	return priv->msg_enable;
+}
+
+static void w5100_set_msglevel(struct net_device *ndev, u32 value)
+{
+	struct w5100_priv *priv = netdev_priv(ndev);
+
+	priv->msg_enable = value;
+}
+
+static int w5100_get_regs_len(struct net_device *ndev)
+{
+	return W5100_COMMON_REGS_LEN + W5100_S0_REGS_LEN;
+}
+
+static void w5100_get_regs(struct net_device *ndev,
+			   struct ethtool_regs *regs, void *_buf)
+{
+	struct w5100_priv *priv = netdev_priv(ndev);
+	u8 *buf = _buf;
+	u16 i;
+
+	regs->version = 1;
+	for (i = 0; i < W5100_COMMON_REGS_LEN; i++)
+		*buf++ = w5100_read(priv, W5100_COMMON_REGS + i);
+	for (i = 0; i < W5100_S0_REGS_LEN; i++)
+		*buf++ = w5100_read(priv, W5100_S0_REGS + i);
+}
+
+static void w5100_tx_timeout(struct net_device *ndev)
+{
+	struct w5100_priv *priv = netdev_priv(ndev);
+
+	netif_stop_queue(ndev);
+	w5100_hw_reset(priv);
+	w5100_hw_start(priv);
+	ndev->stats.tx_errors++;
+	ndev->trans_start = jiffies;
+	netif_wake_queue(ndev);
+}
+
+static int w5100_start_tx(struct sk_buff *skb, struct net_device *ndev)
+{
+	struct w5100_priv *priv = netdev_priv(ndev);
+	u16 offset;
+
+	netif_stop_queue(ndev);
+
+	offset = w5100_read16(priv, W5100_S0_TX_WR);
+	w5100_writebuf(priv, offset, skb->data, skb->len);
+	w5100_write16(priv, W5100_S0_TX_WR, offset + skb->len);
+	mmiowb();
+	ndev->stats.tx_bytes += skb->len;
+	ndev->stats.tx_packets++;
+	dev_kfree_skb(skb);
+
+	w5100_command(priv, S0_CR_SEND);
+
+	return NETDEV_TX_OK;
+}
+
+static int w5100_napi_poll(struct napi_struct *napi, int budget)
+{
+	struct w5100_priv *priv = container_of(napi, struct w5100_priv, napi);
+	struct net_device *ndev = priv->ndev;
+	struct sk_buff *skb;
+	int rx_count;
+	u16 rx_len;
+	u16 offset;
+	u8 header[2];
+
+	for (rx_count = 0; rx_count < budget; rx_count++) {
+		u16 rx_buf_len = w5100_read16(priv, W5100_S0_RX_RSR);
+		if (rx_buf_len == 0)
+			break;
+
+		offset = w5100_read16(priv, W5100_S0_RX_RD);
+		w5100_readbuf(priv, offset, header, 2);
+		rx_len = get_unaligned_be16(header) - 2;
+
+		skb = netdev_alloc_skb_ip_align(ndev, rx_len);
+		if (unlikely(!skb)) {
+			w5100_write16(priv, W5100_S0_RX_RD,
+					    offset + rx_buf_len);
+			w5100_command(priv, S0_CR_RECV);
+			ndev->stats.rx_dropped++;
+			return -ENOMEM;
+		}
+
+		skb_put(skb, rx_len);
+		w5100_readbuf(priv, offset + 2, skb->data, rx_len);
+		w5100_write16(priv, W5100_S0_RX_RD, offset + 2 + rx_len);
+		mmiowb();
+		w5100_command(priv, S0_CR_RECV);
+		skb->protocol = eth_type_trans(skb, ndev);
+
+		netif_receive_skb(skb);
+		ndev->stats.rx_packets++;
+		ndev->stats.rx_bytes += rx_len;
+	}
+
+	if (rx_count < budget) {
+		w5100_write(priv, W5100_IMR, IR_S0);
+		mmiowb();
+		napi_complete(napi);
+	}
+
+	return rx_count;
+}
+
+static irqreturn_t w5100_interrupt(int irq, void *ndev_instance)
+{
+	struct net_device *ndev = ndev_instance;
+	struct w5100_priv *priv = netdev_priv(ndev);
+
+	int ir = w5100_read(priv, W5100_S0_IR);
+	if (!ir)
+		return IRQ_NONE;
+	w5100_write(priv, W5100_S0_IR, ir);
+	mmiowb();
+
+	if (ir & S0_IR_SENDOK) {
+		netif_dbg(priv, tx_done, ndev, "tx done\n");
+		netif_wake_queue(ndev);
+	}
+
+	if (ir & S0_IR_RECV) {
+		if (napi_schedule_prep(&priv->napi)) {
+			w5100_write(priv, W5100_IMR, 0);
+			mmiowb();
+			__napi_schedule(&priv->napi);
+		}
+	}
+
+	return IRQ_HANDLED;
+}
+
+static irqreturn_t w5100_detect_link(int irq, void *ndev_instance)
+{
+	struct net_device *ndev = ndev_instance;
+	struct w5100_priv *priv = netdev_priv(ndev);
+
+	if (netif_running(ndev)) {
+		if (gpio_get_value(priv->link_gpio) != 0) {
+			netif_info(priv, link, ndev, "link is up\n");
+			netif_carrier_on(ndev);
+		} else {
+			netif_info(priv, link, ndev, "link is down\n");
+			netif_carrier_off(ndev);
+		}
+	}
+
+	return IRQ_HANDLED;
+}
+
+static void w5100_set_rx_mode(struct net_device *ndev)
+{
+	struct w5100_priv *priv = netdev_priv(ndev);
+	bool set_promisc = (ndev->flags & IFF_PROMISC) != 0;
+
+	if (priv->promisc != set_promisc) {
+		priv->promisc = set_promisc;
+		w5100_hw_start(priv);
+	}
+}
+
+static int w5100_set_macaddr(struct net_device *ndev, void *addr)
+{
+	struct w5100_priv *priv = netdev_priv(ndev);
+	struct sockaddr *sock_addr = addr;
+
+	if (!is_valid_ether_addr(sock_addr->sa_data))
+		return -EADDRNOTAVAIL;
+	memcpy(ndev->dev_addr, sock_addr->sa_data, ETH_ALEN);
+	ndev->addr_assign_type &= ~NET_ADDR_RANDOM;
+	w5100_write_macaddr(priv);
+	return 0;
+}
+
+static int w5100_open(struct net_device *ndev)
+{
+	struct w5100_priv *priv = netdev_priv(ndev);
+
+	netif_info(priv, ifup, ndev, "enabling\n");
+	if (!is_valid_ether_addr(ndev->dev_addr))
+		return -EINVAL;
+	w5100_hw_start(priv);
+	napi_enable(&priv->napi);
+	netif_start_queue(ndev);
+	if (!gpio_is_valid(priv->link_gpio) ||
+	    gpio_get_value(priv->link_gpio) != 0)
+		netif_carrier_on(ndev);
+	return 0;
+}
+
+static int w5100_stop(struct net_device *ndev)
+{
+	struct w5100_priv *priv = netdev_priv(ndev);
+
+	netif_info(priv, ifdown, ndev, "shutting down\n");
+	w5100_hw_close(priv);
+	netif_carrier_off(ndev);
+	netif_stop_queue(ndev);
+	napi_disable(&priv->napi);
+	return 0;
+}
+
+static const struct ethtool_ops w5100_ethtool_ops = {
+	.get_drvinfo		= w5100_get_drvinfo,
+	.get_msglevel		= w5100_get_msglevel,
+	.set_msglevel		= w5100_set_msglevel,
+	.get_link		= w5100_get_link,
+	.get_regs_len		= w5100_get_regs_len,
+	.get_regs		= w5100_get_regs,
+};
+
+static const struct net_device_ops w5100_netdev_ops = {
+	.ndo_open		= w5100_open,
+	.ndo_stop		= w5100_stop,
+	.ndo_start_xmit		= w5100_start_tx,
+	.ndo_tx_timeout		= w5100_tx_timeout,
+	.ndo_set_rx_mode	= w5100_set_rx_mode,
+	.ndo_set_mac_address	= w5100_set_macaddr,
+	.ndo_validate_addr	= eth_validate_addr,
+	.ndo_change_mtu		= eth_change_mtu,
+};
+
+static int __devinit w5100_hw_probe(struct platform_device *pdev)
+{
+	struct wiznet_platform_data *data = pdev->dev.platform_data;
+	struct net_device *ndev = platform_get_drvdata(pdev);
+	struct w5100_priv *priv = netdev_priv(ndev);
+	const char *name = netdev_name(ndev);
+	struct resource *mem;
+	int mem_size;
+	int irq;
+	int ret;
+
+	if (data && is_valid_ether_addr(data->mac_addr)) {
+		memcpy(ndev->dev_addr, data->mac_addr, ETH_ALEN);
+	} else {
+		random_ether_addr(ndev->dev_addr);
+		ndev->addr_assign_type |= NET_ADDR_RANDOM;
+	}
+
+	mem = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	if (!mem)
+		return -ENXIO;
+	mem_size = resource_size(mem);
+	if (!devm_request_mem_region(&pdev->dev, mem->start, mem_size, name))
+		return -EBUSY;
+	priv->base = devm_ioremap(&pdev->dev, mem->start, mem_size);
+	if (!priv->base)
+		return -EBUSY;
+
+	spin_lock_init(&priv->reg_lock);
+	priv->indirect = mem_size < W5100_BUS_DIRECT_SIZE;
+	if (priv->indirect) {
+		priv->read     = w5100_read_indirect;
+		priv->write    = w5100_write_indirect;
+		priv->read16   = w5100_read16_indirect;
+		priv->write16  = w5100_write16_indirect;
+		priv->readbuf  = w5100_readbuf_indirect;
+		priv->writebuf = w5100_writebuf_indirect;
+	} else {
+		priv->read     = w5100_read_direct;
+		priv->write    = w5100_write_direct;
+		priv->read16   = w5100_read16_direct;
+		priv->write16  = w5100_write16_direct;
+		priv->readbuf  = w5100_readbuf_direct;
+		priv->writebuf = w5100_writebuf_direct;
+	}
+
+	w5100_hw_reset(priv);
+	if (w5100_read16(priv, W5100_RTR) != RTR_DEFAULT)
+		return -ENODEV;
+
+	irq = platform_get_irq(pdev, 0);
+	if (irq < 0)
+		return irq;
+	ret = request_irq(irq, w5100_interrupt,
+			  IRQ_TYPE_LEVEL_LOW, name, ndev);
+	if (ret < 0)
+		return ret;
+	priv->irq = irq;
+
+	priv->link_gpio = data ? data->link_gpio : -EINVAL;
+	if (gpio_is_valid(priv->link_gpio)) {
+		char *link_name = devm_kzalloc(&pdev->dev, 16, GFP_KERNEL);
+		if (!link_name)
+			return -ENOMEM;
+		snprintf(link_name, 16, "%s-link", name);
+		priv->link_irq = gpio_to_irq(priv->link_gpio);
+		if (request_any_context_irq(priv->link_irq, w5100_detect_link,
+				IRQF_TRIGGER_RISING | IRQF_TRIGGER_FALLING,
+				link_name, priv->ndev) < 0)
+			priv->link_gpio = -EINVAL;
+	}
+
+	netdev_info(ndev, "at 0x%llx irq %d\n", (u64)mem->start, irq);
+	return 0;
+}
+
+static int __devinit w5100_probe(struct platform_device *pdev)
+{
+	struct w5100_priv *priv;
+	struct net_device *ndev;
+	int err;
+
+	ndev = alloc_etherdev(sizeof(*priv));
+	if (!ndev)
+		return -ENOMEM;
+	SET_NETDEV_DEV(ndev, &pdev->dev);
+	platform_set_drvdata(pdev, ndev);
+	priv = netdev_priv(ndev);
+	priv->ndev = ndev;
+
+	ether_setup(ndev);
+	ndev->netdev_ops = &w5100_netdev_ops;
+	ndev->ethtool_ops = &w5100_ethtool_ops;
+	ndev->watchdog_timeo = HZ;
+	netif_napi_add(ndev, &priv->napi, w5100_napi_poll, 16);
+
+	/* This chip doesn't support VLAN packets with normal MTU,
+	 * so disable VLAN for this device.
+	 */
+	ndev->features |= NETIF_F_VLAN_CHALLENGED;
+
+	err = register_netdev(ndev);
+	if (err < 0)
+		goto err_register;
+
+	err = w5100_hw_probe(pdev);
+	if (err < 0)
+		goto err_hw_probe;
+
+	return 0;
+
+err_hw_probe:
+	unregister_netdev(ndev);
+err_register:
+	free_netdev(ndev);
+	platform_set_drvdata(pdev, NULL);
+	return err;
+}
+
+static int __devexit w5100_remove(struct platform_device *pdev)
+{
+	struct net_device *ndev = platform_get_drvdata(pdev);
+	struct w5100_priv *priv = netdev_priv(ndev);
+
+	w5100_hw_reset(priv);
+	free_irq(priv->irq, ndev);
+	if (gpio_is_valid(priv->link_gpio))
+		free_irq(priv->link_irq, ndev);
+
+	unregister_netdev(ndev);
+	free_netdev(ndev);
+	platform_set_drvdata(pdev, NULL);
+	return 0;
+}
+
+#ifdef CONFIG_PM
+static int w5100_suspend(struct device *dev)
+{
+	struct platform_device *pdev = to_platform_device(dev);
+	struct net_device *ndev = platform_get_drvdata(pdev);
+	struct w5100_priv *priv = netdev_priv(ndev);
+
+	if (netif_running(ndev)) {
+		netif_carrier_off(ndev);
+		netif_device_detach(ndev);
+
+		w5100_hw_close(priv);
+	}
+	return 0;
+}
+
+static int w5100_resume(struct device *dev)
+{
+	struct platform_device *pdev = to_platform_device(dev);
+	struct net_device *ndev = platform_get_drvdata(pdev);
+	struct w5100_priv *priv = netdev_priv(ndev);
+
+	if (netif_running(ndev)) {
+		w5100_hw_reset(priv);
+		w5100_hw_start(priv);
+
+		netif_device_attach(ndev);
+		if (!gpio_is_valid(priv->link_gpio) ||
+		    gpio_get_value(priv->link_gpio) != 0)
+			netif_carrier_on(ndev);
+	}
+	return 0;
+}
+#endif /* CONFIG_PM */
+
+static SIMPLE_DEV_PM_OPS(w5100_pm_ops, w5100_suspend, w5100_resume);
+
+static struct platform_driver w5100_driver = {
+	.driver		= {
+		.name	= DRV_NAME,
+		.owner	= THIS_MODULE,
+		.pm	= &w5100_pm_ops,
+	},
+	.probe		= w5100_probe,
+	.remove		= __devexit_p(w5100_remove),
+};
+
+module_platform_driver(w5100_driver);
diff --git a/drivers/net/ethernet/wiznet/w5300.c b/drivers/net/ethernet/wiznet/w5300.c
new file mode 100644
index 00000000000..3306a20ec21
--- /dev/null
+++ b/drivers/net/ethernet/wiznet/w5300.c
@@ -0,0 +1,720 @@
+/*
+ * Ethernet driver for the WIZnet W5300 chip.
+ *
+ * Copyright (C) 2008-2009 WIZnet Co.,Ltd.
+ * Copyright (C) 2011 Taehun Kim <kth3321 <at> gmail.com>
+ * Copyright (C) 2012 Mike Sinkovsky <msink@permonline.ru>
+ *
+ * Licensed under the GPL-2 or later.
+ */
+
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/kconfig.h>
+#include <linux/netdevice.h>
+#include <linux/etherdevice.h>
+#include <linux/platform_device.h>
+#include <linux/platform_data/wiznet.h>
+#include <linux/ethtool.h>
+#include <linux/skbuff.h>
+#include <linux/types.h>
+#include <linux/errno.h>
+#include <linux/delay.h>
+#include <linux/slab.h>
+#include <linux/spinlock.h>
+#include <linux/io.h>
+#include <linux/ioport.h>
+#include <linux/interrupt.h>
+#include <linux/irq.h>
+#include <linux/gpio.h>
+
+#define DRV_NAME	"w5300"
+#define DRV_VERSION	"2012-04-04"
+
+MODULE_DESCRIPTION("WIZnet W5300 Ethernet driver v"DRV_VERSION);
+MODULE_AUTHOR("Mike Sinkovsky <msink@permonline.ru>");
+MODULE_ALIAS("platform:"DRV_NAME);
+MODULE_LICENSE("GPL");
+
+/*
+ * Registers
+ */
+#define W5300_MR		0x0000	/* Mode Register */
+#define   MR_DBW		  (1 << 15) /* Data bus width */
+#define   MR_MPF		  (1 << 14) /* Mac layer pause frame */
+#define   MR_WDF(n)		  (((n)&7)<<11) /* Write data fetch time */
+#define   MR_RDH		  (1 << 10) /* Read data hold time */
+#define   MR_FS			  (1 << 8)  /* FIFO swap */
+#define   MR_RST		  (1 << 7)  /* S/W reset */
+#define   MR_PB			  (1 << 4)  /* Ping block */
+#define   MR_DBS		  (1 << 2)  /* Data bus swap */
+#define   MR_IND		  (1 << 0)  /* Indirect mode */
+#define W5300_IR		0x0002	/* Interrupt Register */
+#define W5300_IMR		0x0004	/* Interrupt Mask Register */
+#define   IR_S0			  0x0001  /* S0 interrupt */
+#define W5300_SHARL		0x0008	/* Source MAC address (0123) */
+#define W5300_SHARH		0x000c	/* Source MAC address (45) */
+#define W5300_TMSRL		0x0020	/* Transmit Memory Size (0123) */
+#define W5300_TMSRH		0x0024	/* Transmit Memory Size (4567) */
+#define W5300_RMSRL		0x0028	/* Receive Memory Size (0123) */
+#define W5300_RMSRH		0x002c	/* Receive Memory Size (4567) */
+#define W5300_MTYPE		0x0030	/* Memory Type */
+#define W5300_IDR		0x00fe	/* Chip ID register */
+#define   IDR_W5300		  0x5300  /* =0x5300 for WIZnet W5300 */
+#define W5300_S0_MR		0x0200	/* S0 Mode Register */
+#define   S0_MR_CLOSED		  0x0000  /* Close mode */
+#define   S0_MR_MACRAW		  0x0004  /* MAC RAW mode (promiscous) */
+#define   S0_MR_MACRAW_MF	  0x0044  /* MAC RAW mode (filtered) */
+#define W5300_S0_CR		0x0202	/* S0 Command Register */
+#define   S0_CR_OPEN		  0x0001  /* OPEN command */
+#define   S0_CR_CLOSE		  0x0010  /* CLOSE command */
+#define   S0_CR_SEND		  0x0020  /* SEND command */
+#define   S0_CR_RECV		  0x0040  /* RECV command */
+#define W5300_S0_IMR		0x0204	/* S0 Interrupt Mask Register */
+#define W5300_S0_IR		0x0206	/* S0 Interrupt Register */
+#define   S0_IR_RECV		  0x0004  /* Receive interrupt */
+#define   S0_IR_SENDOK		  0x0010  /* Send OK interrupt */
+#define W5300_S0_SSR		0x0208	/* S0 Socket Status Register */
+#define W5300_S0_TX_WRSR	0x0220	/* S0 TX Write Size Register */
+#define W5300_S0_TX_FSR		0x0224	/* S0 TX Free Size Register */
+#define W5300_S0_RX_RSR		0x0228	/* S0 Received data Size */
+#define W5300_S0_TX_FIFO	0x022e	/* S0 Transmit FIFO */
+#define W5300_S0_RX_FIFO	0x0230	/* S0 Receive FIFO */
+#define W5300_REGS_LEN		0x0400
+
+/*
+ * Device driver private data structure
+ */
+struct w5300_priv {
+	void __iomem *base;
+	spinlock_t reg_lock;
+	bool indirect;
+	u16  (*read) (struct w5300_priv *priv, u16 addr);
+	void (*write)(struct w5300_priv *priv, u16 addr, u16 data);
+	int irq;
+	int link_irq;
+	int link_gpio;
+
+	struct napi_struct napi;
+	struct net_device *ndev;
+	bool promisc;
+	u32 msg_enable;
+};
+
+/************************************************************************
+ *
+ *  Lowlevel I/O functions
+ *
+ ***********************************************************************/
+
+/*
+ * In direct address mode host system can directly access W5300 registers
+ * after mapping to Memory-Mapped I/O space.
+ *
+ * 0x400 bytes are required for memory space.
+ */
+static inline u16 w5300_read_direct(struct w5300_priv *priv, u16 addr)
+{
+	return ioread16(priv->base + (addr << CONFIG_WIZNET_BUS_SHIFT));
+}
+
+static inline void w5300_write_direct(struct w5300_priv *priv,
+				      u16 addr, u16 data)
+{
+	iowrite16(data, priv->base + (addr << CONFIG_WIZNET_BUS_SHIFT));
+}
+
+/*
+ * In indirect address mode host system indirectly accesses registers by
+ * using Indirect Mode Address Register (IDM_AR) and Indirect Mode Data
+ * Register (IDM_DR), which are directly mapped to Memory-Mapped I/O space.
+ * Mode Register (MR) is directly accessible.
+ *
+ * Only 0x06 bytes are required for memory space.
+ */
+#define W5300_IDM_AR		0x0002	 /* Indirect Mode Address */
+#define W5300_IDM_DR		0x0004	 /* Indirect Mode Data */
+
+static u16 w5300_read_indirect(struct w5300_priv *priv, u16 addr)
+{
+	unsigned long flags;
+	u16 data;
+
+	spin_lock_irqsave(&priv->reg_lock, flags);
+	w5300_write_direct(priv, W5300_IDM_AR, addr);
+	mmiowb();
+	data = w5300_read_direct(priv, W5300_IDM_DR);
+	spin_unlock_irqrestore(&priv->reg_lock, flags);
+
+	return data;
+}
+
+static void w5300_write_indirect(struct w5300_priv *priv, u16 addr, u16 data)
+{
+	unsigned long flags;
+
+	spin_lock_irqsave(&priv->reg_lock, flags);
+	w5300_write_direct(priv, W5300_IDM_AR, addr);
+	mmiowb();
+	w5300_write_direct(priv, W5300_IDM_DR, data);
+	mmiowb();
+	spin_unlock_irqrestore(&priv->reg_lock, flags);
+}
+
+#if defined(CONFIG_WIZNET_BUS_DIRECT)
+#define w5300_read	w5300_read_direct
+#define w5300_write	w5300_write_direct
+
+#elif defined(CONFIG_WIZNET_BUS_INDIRECT)
+#define w5300_read	w5300_read_indirect
+#define w5300_write	w5300_write_indirect
+
+#else /* CONFIG_WIZNET_BUS_ANY */
+#define w5300_read	priv->read
+#define w5300_write	priv->write
+#endif
+
+static u32 w5300_read32(struct w5300_priv *priv, u16 addr)
+{
+	u32 data;
+	data  = w5300_read(priv, addr) << 16;
+	data |= w5300_read(priv, addr + 2);
+	return data;
+}
+
+static void w5300_write32(struct w5300_priv *priv, u16 addr, u32 data)
+{
+	w5300_write(priv, addr, data >> 16);
+	w5300_write(priv, addr + 2, data);
+}
+
+static int w5300_command(struct w5300_priv *priv, u16 cmd)
+{
+	unsigned long timeout = jiffies + msecs_to_jiffies(100);
+
+	w5300_write(priv, W5300_S0_CR, cmd);
+	mmiowb();
+
+	while (w5300_read(priv, W5300_S0_CR) != 0) {
+		if (time_after(jiffies, timeout))
+			return -EIO;
+		cpu_relax();
+	}
+
+	return 0;
+}
+
+static void w5300_read_frame(struct w5300_priv *priv, u8 *buf, int len)
+{
+	u16 fifo;
+	int i;
+
+	for (i = 0; i < len; i += 2) {
+		fifo = w5300_read(priv, W5300_S0_RX_FIFO);
+		*buf++ = fifo >> 8;
+		*buf++ = fifo;
+	}
+	fifo = w5300_read(priv, W5300_S0_RX_FIFO);
+	fifo = w5300_read(priv, W5300_S0_RX_FIFO);
+}
+
+static void w5300_write_frame(struct w5300_priv *priv, u8 *buf, int len)
+{
+	u16 fifo;
+	int i;
+
+	for (i = 0; i < len; i += 2) {
+		fifo  = *buf++ << 8;
+		fifo |= *buf++;
+		w5300_write(priv, W5300_S0_TX_FIFO, fifo);
+	}
+	w5300_write32(priv, W5300_S0_TX_WRSR, len);
+}
+
+static void w5300_write_macaddr(struct w5300_priv *priv)
+{
+	struct net_device *ndev = priv->ndev;
+	w5300_write32(priv, W5300_SHARL,
+		      ndev->dev_addr[0] << 24 |
+		      ndev->dev_addr[1] << 16 |
+		      ndev->dev_addr[2] << 8 |
+		      ndev->dev_addr[3]);
+	w5300_write(priv, W5300_SHARH,
+		      ndev->dev_addr[4] << 8 |
+		      ndev->dev_addr[5]);
+	mmiowb();
+}
+
+static void w5300_hw_reset(struct w5300_priv *priv)
+{
+	w5300_write_direct(priv, W5300_MR, MR_RST);
+	mmiowb();
+	mdelay(5);
+	w5300_write_direct(priv, W5300_MR, priv->indirect ?
+				 MR_WDF(7) | MR_PB | MR_IND :
+				 MR_WDF(7) | MR_PB);
+	mmiowb();
+	w5300_write(priv, W5300_IMR, 0);
+	w5300_write_macaddr(priv);
+
+	/* Configure 128K of internal memory
+	 * as 64K RX fifo and 64K TX fifo
+	 */
+	w5300_write32(priv, W5300_RMSRL, 64 << 24);
+	w5300_write32(priv, W5300_RMSRH, 0);
+	w5300_write32(priv, W5300_TMSRL, 64 << 24);
+	w5300_write32(priv, W5300_TMSRH, 0);
+	w5300_write(priv, W5300_MTYPE, 0x00ff);
+	mmiowb();
+}
+
+static void w5300_hw_start(struct w5300_priv *priv)
+{
+	w5300_write(priv, W5300_S0_MR, priv->promisc ?
+			  S0_MR_MACRAW : S0_MR_MACRAW_MF);
+	mmiowb();
+	w5300_command(priv, S0_CR_OPEN);
+	w5300_write(priv, W5300_S0_IMR, S0_IR_RECV | S0_IR_SENDOK);
+	w5300_write(priv, W5300_IMR, IR_S0);
+	mmiowb();
+}
+
+static void w5300_hw_close(struct w5300_priv *priv)
+{
+	w5300_write(priv, W5300_IMR, 0);
+	mmiowb();
+	w5300_command(priv, S0_CR_CLOSE);
+}
+
+/***********************************************************************
+ *
+ *   Device driver functions / callbacks
+ *
+ ***********************************************************************/
+
+static void w5300_get_drvinfo(struct net_device *ndev,
+			      struct ethtool_drvinfo *info)
+{
+	strlcpy(info->driver, DRV_NAME, sizeof(info->driver));
+	strlcpy(info->version, DRV_VERSION, sizeof(info->version));
+	strlcpy(info->bus_info, dev_name(ndev->dev.parent),
+		sizeof(info->bus_info));
+}
+
+static u32 w5300_get_link(struct net_device *ndev)
+{
+	struct w5300_priv *priv = netdev_priv(ndev);
+
+	if (gpio_is_valid(priv->link_gpio))
+		return !!gpio_get_value(priv->link_gpio);
+
+	return 1;
+}
+
+static u32 w5300_get_msglevel(struct net_device *ndev)
+{
+	struct w5300_priv *priv = netdev_priv(ndev);
+
+	return priv->msg_enable;
+}
+
+static void w5300_set_msglevel(struct net_device *ndev, u32 value)
+{
+	struct w5300_priv *priv = netdev_priv(ndev);
+
+	priv->msg_enable = value;
+}
+
+static int w5300_get_regs_len(struct net_device *ndev)
+{
+	return W5300_REGS_LEN;
+}
+
+static void w5300_get_regs(struct net_device *ndev,
+			   struct ethtool_regs *regs, void *_buf)
+{
+	struct w5300_priv *priv = netdev_priv(ndev);
+	u8 *buf = _buf;
+	u16 addr;
+	u16 data;
+
+	regs->version = 1;
+	for (addr = 0; addr < W5300_REGS_LEN; addr += 2) {
+		switch (addr & 0x23f) {
+		case W5300_S0_TX_FIFO: /* cannot read TX_FIFO */
+		case W5300_S0_RX_FIFO: /* cannot read RX_FIFO */
+			data = 0xffff;
+			break;
+		default:
+			data = w5300_read(priv, addr);
+			break;
+		}
+		*buf++ = data >> 8;
+		*buf++ = data;
+	}
+}
+
+static void w5300_tx_timeout(struct net_device *ndev)
+{
+	struct w5300_priv *priv = netdev_priv(ndev);
+
+	netif_stop_queue(ndev);
+	w5300_hw_reset(priv);
+	w5300_hw_start(priv);
+	ndev->stats.tx_errors++;
+	ndev->trans_start = jiffies;
+	netif_wake_queue(ndev);
+}
+
+static int w5300_start_tx(struct sk_buff *skb, struct net_device *ndev)
+{
+	struct w5300_priv *priv = netdev_priv(ndev);
+
+	netif_stop_queue(ndev);
+
+	w5300_write_frame(priv, skb->data, skb->len);
+	mmiowb();
+	ndev->stats.tx_packets++;
+	ndev->stats.tx_bytes += skb->len;
+	dev_kfree_skb(skb);
+	netif_dbg(priv, tx_queued, ndev, "tx queued\n");
+
+	w5300_command(priv, S0_CR_SEND);
+
+	return NETDEV_TX_OK;
+}
+
+static int w5300_napi_poll(struct napi_struct *napi, int budget)
+{
+	struct w5300_priv *priv = container_of(napi, struct w5300_priv, napi);
+	struct net_device *ndev = priv->ndev;
+	struct sk_buff *skb;
+	int rx_count;
+	u16 rx_len;
+
+	for (rx_count = 0; rx_count < budget; rx_count++) {
+		u32 rx_fifo_len = w5300_read32(priv, W5300_S0_RX_RSR);
+		if (rx_fifo_len == 0)
+			break;
+
+		rx_len = w5300_read(priv, W5300_S0_RX_FIFO);
+
+		skb = netdev_alloc_skb_ip_align(ndev, roundup(rx_len, 2));
+		if (unlikely(!skb)) {
+			u32 i;
+			for (i = 0; i < rx_fifo_len; i += 2)
+				w5300_read(priv, W5300_S0_RX_FIFO);
+			ndev->stats.rx_dropped++;
+			return -ENOMEM;
+		}
+
+		skb_put(skb, rx_len);
+		w5300_read_frame(priv, skb->data, rx_len);
+		skb->protocol = eth_type_trans(skb, ndev);
+
+		netif_receive_skb(skb);
+		ndev->stats.rx_packets++;
+		ndev->stats.rx_bytes += rx_len;
+	}
+
+	if (rx_count < budget) {
+		w5300_write(priv, W5300_IMR, IR_S0);
+		mmiowb();
+		napi_complete(napi);
+	}
+
+	return rx_count;
+}
+
+static irqreturn_t w5300_interrupt(int irq, void *ndev_instance)
+{
+	struct net_device *ndev = ndev_instance;
+	struct w5300_priv *priv = netdev_priv(ndev);
+
+	int ir = w5300_read(priv, W5300_S0_IR);
+	if (!ir)
+		return IRQ_NONE;
+	w5300_write(priv, W5300_S0_IR, ir);
+	mmiowb();
+
+	if (ir & S0_IR_SENDOK) {
+		netif_dbg(priv, tx_done, ndev, "tx done\n");
+		netif_wake_queue(ndev);
+	}
+
+	if (ir & S0_IR_RECV) {
+		if (napi_schedule_prep(&priv->napi)) {
+			w5300_write(priv, W5300_IMR, 0);
+			mmiowb();
+			__napi_schedule(&priv->napi);
+		}
+	}
+
+	return IRQ_HANDLED;
+}
+
+static irqreturn_t w5300_detect_link(int irq, void *ndev_instance)
+{
+	struct net_device *ndev = ndev_instance;
+	struct w5300_priv *priv = netdev_priv(ndev);
+
+	if (netif_running(ndev)) {
+		if (gpio_get_value(priv->link_gpio) != 0) {
+			netif_info(priv, link, ndev, "link is up\n");
+			netif_carrier_on(ndev);
+		} else {
+			netif_info(priv, link, ndev, "link is down\n");
+			netif_carrier_off(ndev);
+		}
+	}
+
+	return IRQ_HANDLED;
+}
+
+static void w5300_set_rx_mode(struct net_device *ndev)
+{
+	struct w5300_priv *priv = netdev_priv(ndev);
+	bool set_promisc = (ndev->flags & IFF_PROMISC) != 0;
+
+	if (priv->promisc != set_promisc) {
+		priv->promisc = set_promisc;
+		w5300_hw_start(priv);
+	}
+}
+
+static int w5300_set_macaddr(struct net_device *ndev, void *addr)
+{
+	struct w5300_priv *priv = netdev_priv(ndev);
+	struct sockaddr *sock_addr = addr;
+
+	if (!is_valid_ether_addr(sock_addr->sa_data))
+		return -EADDRNOTAVAIL;
+	memcpy(ndev->dev_addr, sock_addr->sa_data, ETH_ALEN);
+	ndev->addr_assign_type &= ~NET_ADDR_RANDOM;
+	w5300_write_macaddr(priv);
+	return 0;
+}
+
+static int w5300_open(struct net_device *ndev)
+{
+	struct w5300_priv *priv = netdev_priv(ndev);
+
+	netif_info(priv, ifup, ndev, "enabling\n");
+	if (!is_valid_ether_addr(ndev->dev_addr))
+		return -EINVAL;
+	w5300_hw_start(priv);
+	napi_enable(&priv->napi);
+	netif_start_queue(ndev);
+	if (!gpio_is_valid(priv->link_gpio) ||
+	    gpio_get_value(priv->link_gpio) != 0)
+		netif_carrier_on(ndev);
+	return 0;
+}
+
+static int w5300_stop(struct net_device *ndev)
+{
+	struct w5300_priv *priv = netdev_priv(ndev);
+
+	netif_info(priv, ifdown, ndev, "shutting down\n");
+	w5300_hw_close(priv);
+	netif_carrier_off(ndev);
+	netif_stop_queue(ndev);
+	napi_disable(&priv->napi);
+	return 0;
+}
+
+static const struct ethtool_ops w5300_ethtool_ops = {
+	.get_drvinfo		= w5300_get_drvinfo,
+	.get_msglevel		= w5300_get_msglevel,
+	.set_msglevel		= w5300_set_msglevel,
+	.get_link		= w5300_get_link,
+	.get_regs_len		= w5300_get_regs_len,
+	.get_regs		= w5300_get_regs,
+};
+
+static const struct net_device_ops w5300_netdev_ops = {
+	.ndo_open		= w5300_open,
+	.ndo_stop		= w5300_stop,
+	.ndo_start_xmit		= w5300_start_tx,
+	.ndo_tx_timeout		= w5300_tx_timeout,
+	.ndo_set_rx_mode	= w5300_set_rx_mode,
+	.ndo_set_mac_address	= w5300_set_macaddr,
+	.ndo_validate_addr	= eth_validate_addr,
+	.ndo_change_mtu		= eth_change_mtu,
+};
+
+static int __devinit w5300_hw_probe(struct platform_device *pdev)
+{
+	struct wiznet_platform_data *data = pdev->dev.platform_data;
+	struct net_device *ndev = platform_get_drvdata(pdev);
+	struct w5300_priv *priv = netdev_priv(ndev);
+	const char *name = netdev_name(ndev);
+	struct resource *mem;
+	int mem_size;
+	int irq;
+	int ret;
+
+	if (data && is_valid_ether_addr(data->mac_addr)) {
+		memcpy(ndev->dev_addr, data->mac_addr, ETH_ALEN);
+	} else {
+		random_ether_addr(ndev->dev_addr);
+		ndev->addr_assign_type |= NET_ADDR_RANDOM;
+	}
+
+	mem = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	if (!mem)
+		return -ENXIO;
+	mem_size = resource_size(mem);
+	if (!devm_request_mem_region(&pdev->dev, mem->start, mem_size, name))
+		return -EBUSY;
+	priv->base = devm_ioremap(&pdev->dev, mem->start, mem_size);
+	if (!priv->base)
+		return -EBUSY;
+
+	spin_lock_init(&priv->reg_lock);
+	priv->indirect = mem_size < W5300_BUS_DIRECT_SIZE;
+	if (priv->indirect) {
+		priv->read  = w5300_read_indirect;
+		priv->write = w5300_write_indirect;
+	} else {
+		priv->read  = w5300_read_direct;
+		priv->write = w5300_write_direct;
+	}
+
+	w5300_hw_reset(priv);
+	if (w5300_read(priv, W5300_IDR) != IDR_W5300)
+		return -ENODEV;
+
+	irq = platform_get_irq(pdev, 0);
+	if (irq < 0)
+		return irq;
+	ret = request_irq(irq, w5300_interrupt,
+			  IRQ_TYPE_LEVEL_LOW, name, ndev);
+	if (ret < 0)
+		return ret;
+	priv->irq = irq;
+
+	priv->link_gpio = data ? data->link_gpio : -EINVAL;
+	if (gpio_is_valid(priv->link_gpio)) {
+		char *link_name = devm_kzalloc(&pdev->dev, 16, GFP_KERNEL);
+		if (!link_name)
+			return -ENOMEM;
+		snprintf(link_name, 16, "%s-link", name);
+		priv->link_irq = gpio_to_irq(priv->link_gpio);
+		if (request_any_context_irq(priv->link_irq, w5300_detect_link,
+				IRQF_TRIGGER_RISING | IRQF_TRIGGER_FALLING,
+				link_name, priv->ndev) < 0)
+			priv->link_gpio = -EINVAL;
+	}
+
+	netdev_info(ndev, "at 0x%llx irq %d\n", (u64)mem->start, irq);
+	return 0;
+}
+
+static int __devinit w5300_probe(struct platform_device *pdev)
+{
+	struct w5300_priv *priv;
+	struct net_device *ndev;
+	int err;
+
+	ndev = alloc_etherdev(sizeof(*priv));
+	if (!ndev)
+		return -ENOMEM;
+	SET_NETDEV_DEV(ndev, &pdev->dev);
+	platform_set_drvdata(pdev, ndev);
+	priv = netdev_priv(ndev);
+	priv->ndev = ndev;
+
+	ether_setup(ndev);
+	ndev->netdev_ops = &w5300_netdev_ops;
+	ndev->ethtool_ops = &w5300_ethtool_ops;
+	ndev->watchdog_timeo = HZ;
+	netif_napi_add(ndev, &priv->napi, w5300_napi_poll, 16);
+
+	/* This chip doesn't support VLAN packets with normal MTU,
+	 * so disable VLAN for this device.
+	 */
+	ndev->features |= NETIF_F_VLAN_CHALLENGED;
+
+	err = register_netdev(ndev);
+	if (err < 0)
+		goto err_register;
+
+	err = w5300_hw_probe(pdev);
+	if (err < 0)
+		goto err_hw_probe;
+
+	return 0;
+
+err_hw_probe:
+	unregister_netdev(ndev);
+err_register:
+	free_netdev(ndev);
+	platform_set_drvdata(pdev, NULL);
+	return err;
+}
+
+static int __devexit w5300_remove(struct platform_device *pdev)
+{
+	struct net_device *ndev = platform_get_drvdata(pdev);
+	struct w5300_priv *priv = netdev_priv(ndev);
+
+	w5300_hw_reset(priv);
+	free_irq(priv->irq, ndev);
+	if (gpio_is_valid(priv->link_gpio))
+		free_irq(priv->link_irq, ndev);
+
+	unregister_netdev(ndev);
+	free_netdev(ndev);
+	platform_set_drvdata(pdev, NULL);
+	return 0;
+}
+
+#ifdef CONFIG_PM
+static int w5300_suspend(struct device *dev)
+{
+	struct platform_device *pdev = to_platform_device(dev);
+	struct net_device *ndev = platform_get_drvdata(pdev);
+	struct w5300_priv *priv = netdev_priv(ndev);
+
+	if (netif_running(ndev)) {
+		netif_carrier_off(ndev);
+		netif_device_detach(ndev);
+
+		w5300_hw_close(priv);
+	}
+	return 0;
+}
+
+static int w5300_resume(struct device *dev)
+{
+	struct platform_device *pdev = to_platform_device(dev);
+	struct net_device *ndev = platform_get_drvdata(pdev);
+	struct w5300_priv *priv = netdev_priv(ndev);
+
+	if (!netif_running(ndev)) {
+		w5300_hw_reset(priv);
+		w5300_hw_start(priv);
+
+		netif_device_attach(ndev);
+		if (!gpio_is_valid(priv->link_gpio) ||
+		    gpio_get_value(priv->link_gpio) != 0)
+			netif_carrier_on(ndev);
+	}
+	return 0;
+}
+#endif /* CONFIG_PM */
+
+static SIMPLE_DEV_PM_OPS(w5300_pm_ops, w5300_suspend, w5300_resume);
+
+static struct platform_driver w5300_driver = {
+	.driver		= {
+		.name	= DRV_NAME,
+		.owner	= THIS_MODULE,
+		.pm	= &w5300_pm_ops,
+	},
+	.probe		= w5300_probe,
+	.remove		= __devexit_p(w5300_remove),
+};
+
+module_platform_driver(w5300_driver);
diff --git a/drivers/net/ethernet/xilinx/ll_temac_main.c b/drivers/net/ethernet/xilinx/ll_temac_main.c
index d21591a2c59..1eaf7128afe 100644
--- a/drivers/net/ethernet/xilinx/ll_temac_main.c
+++ b/drivers/net/ethernet/xilinx/ll_temac_main.c
@@ -1000,6 +1000,7 @@ static const struct ethtool_ops temac_ethtool_ops = {
 	.set_settings = temac_set_settings,
 	.nway_reset = temac_nway_reset,
 	.get_link = ethtool_op_get_link,
+	.get_ts_info = ethtool_op_get_ts_info,
 };
 
 static int __devinit temac_of_probe(struct platform_device *op)
diff --git a/drivers/net/ethernet/xscale/Kconfig b/drivers/net/ethernet/xscale/Kconfig
index cf67352cea1..3f431019e61 100644
--- a/drivers/net/ethernet/xscale/Kconfig
+++ b/drivers/net/ethernet/xscale/Kconfig
@@ -5,8 +5,8 @@
 config NET_VENDOR_XSCALE
 	bool "Intel XScale IXP devices"
 	default y
-	depends on NET_VENDOR_INTEL && ((ARM && ARCH_IXP4XX && \
-		   IXP4XX_NPE && IXP4XX_QMGR) || ARCH_ENP2611)
+	depends on NET_VENDOR_INTEL && (ARM && ARCH_IXP4XX && \
+		   IXP4XX_NPE && IXP4XX_QMGR)
 	---help---
 	  If you have a network (Ethernet) card belonging to this class, say Y
 	  and read the Ethernet-HOWTO, available from
@@ -27,6 +27,4 @@ config IXP4XX_ETH
 	  Say Y here if you want to use built-in Ethernet ports
 	  on IXP4xx processor.
 
-source "drivers/net/ethernet/xscale/ixp2000/Kconfig"
-
 endif # NET_VENDOR_XSCALE
diff --git a/drivers/net/ethernet/xscale/Makefile b/drivers/net/ethernet/xscale/Makefile
index b195b9d7fe8..abc3b031fba 100644
--- a/drivers/net/ethernet/xscale/Makefile
+++ b/drivers/net/ethernet/xscale/Makefile
@@ -2,5 +2,4 @@
 # Makefile for the Intel XScale IXP device drivers.
 #
 
-obj-$(CONFIG_ENP2611_MSF_NET) += ixp2000/
 obj-$(CONFIG_IXP4XX_ETH) += ixp4xx_eth.o
diff --git a/drivers/net/ethernet/xscale/ixp2000/Kconfig b/drivers/net/ethernet/xscale/ixp2000/Kconfig
deleted file mode 100644
index 58dbc5b876b..00000000000
--- a/drivers/net/ethernet/xscale/ixp2000/Kconfig
+++ /dev/null
@@ -1,6 +0,0 @@
-config ENP2611_MSF_NET
-	tristate "Radisys ENP2611 MSF network interface support"
-	depends on ARCH_ENP2611
-	---help---
-	  This is a driver for the MSF network interface unit in
-	  the IXP2400 on the Radisys ENP2611 platform.
diff --git a/drivers/net/ethernet/xscale/ixp2000/Makefile b/drivers/net/ethernet/xscale/ixp2000/Makefile
deleted file mode 100644
index fd38351ceaa..00000000000
--- a/drivers/net/ethernet/xscale/ixp2000/Makefile
+++ /dev/null
@@ -1,3 +0,0 @@
-obj-$(CONFIG_ENP2611_MSF_NET) += enp2611_mod.o
-
-enp2611_mod-objs := caleb.o enp2611.o ixp2400-msf.o ixpdev.o pm3386.o
diff --git a/drivers/net/ethernet/xscale/ixp2000/caleb.c b/drivers/net/ethernet/xscale/ixp2000/caleb.c
deleted file mode 100644
index 7dea5b95012..00000000000
--- a/drivers/net/ethernet/xscale/ixp2000/caleb.c
+++ /dev/null
@@ -1,136 +0,0 @@
-/*
- * Helper functions for the SPI-3 bridge FPGA on the Radisys ENP2611
- * Copyright (C) 2004, 2005 Lennert Buytenhek <buytenh@wantstofly.org>
- * Dedicated to Marija Kulikova.
- *
- * 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/delay.h>
-#include <asm/io.h>
-#include "caleb.h"
-
-#define CALEB_IDLO		0x00
-#define CALEB_IDHI		0x01
-#define CALEB_RID		0x02
-#define CALEB_RESET		0x03
-#define CALEB_INTREN0		0x04
-#define CALEB_INTREN1		0x05
-#define CALEB_INTRSTAT0		0x06
-#define CALEB_INTRSTAT1		0x07
-#define CALEB_PORTEN		0x08
-#define CALEB_BURST		0x09
-#define CALEB_PORTPAUS		0x0A
-#define CALEB_PORTPAUSD		0x0B
-#define CALEB_PHY0RX		0x10
-#define CALEB_PHY1RX		0x11
-#define CALEB_PHY0TX		0x12
-#define CALEB_PHY1TX		0x13
-#define CALEB_IXPRX_HI_CNTR	0x15
-#define CALEB_PHY0RX_HI_CNTR	0x16
-#define CALEB_PHY1RX_HI_CNTR	0x17
-#define CALEB_IXPRX_CNTR	0x18
-#define CALEB_PHY0RX_CNTR	0x19
-#define CALEB_PHY1RX_CNTR	0x1A
-#define CALEB_IXPTX_CNTR	0x1B
-#define CALEB_PHY0TX_CNTR	0x1C
-#define CALEB_PHY1TX_CNTR	0x1D
-#define CALEB_DEBUG0		0x1E
-#define CALEB_DEBUG1		0x1F
-
-
-static u8 caleb_reg_read(int reg)
-{
-	u8 value;
-
-	value = *((volatile u8 *)(ENP2611_CALEB_VIRT_BASE + reg));
-
-//	printk(KERN_INFO "caleb_reg_read(%d) = %.2x\n", reg, value);
-
-	return value;
-}
-
-static void caleb_reg_write(int reg, u8 value)
-{
-	u8 dummy;
-
-//	printk(KERN_INFO "caleb_reg_write(%d, %.2x)\n", reg, value);
-
-	*((volatile u8 *)(ENP2611_CALEB_VIRT_BASE + reg)) = value;
-
-	dummy = *((volatile u8 *)ENP2611_CALEB_VIRT_BASE);
-	__asm__ __volatile__("mov %0, %0" : "+r" (dummy));
-}
-
-
-void caleb_reset(void)
-{
-	/*
-	 * Perform a chip reset.
-	 */
-	caleb_reg_write(CALEB_RESET, 0x02);
-	udelay(1);
-
-	/*
-	 * Enable all interrupt sources.  This is needed to get
-	 * meaningful results out of the status bits (register 6
-	 * and 7.)
-	 */
-	caleb_reg_write(CALEB_INTREN0, 0xff);
-	caleb_reg_write(CALEB_INTREN1, 0x07);
-
-	/*
-	 * Set RX and TX FIFO thresholds to 1.5kb.
-	 */
-	caleb_reg_write(CALEB_PHY0RX, 0x11);
-	caleb_reg_write(CALEB_PHY1RX, 0x11);
-	caleb_reg_write(CALEB_PHY0TX, 0x11);
-	caleb_reg_write(CALEB_PHY1TX, 0x11);
-
-	/*
-	 * Program SPI-3 burst size.
-	 */
-	caleb_reg_write(CALEB_BURST, 0);	// 64-byte RBUF mpackets
-//	caleb_reg_write(CALEB_BURST, 1);	// 128-byte RBUF mpackets
-//	caleb_reg_write(CALEB_BURST, 2);	// 256-byte RBUF mpackets
-}
-
-void caleb_enable_rx(int port)
-{
-	u8 temp;
-
-	temp = caleb_reg_read(CALEB_PORTEN);
-	temp |= 1 << port;
-	caleb_reg_write(CALEB_PORTEN, temp);
-}
-
-void caleb_disable_rx(int port)
-{
-	u8 temp;
-
-	temp = caleb_reg_read(CALEB_PORTEN);
-	temp &= ~(1 << port);
-	caleb_reg_write(CALEB_PORTEN, temp);
-}
-
-void caleb_enable_tx(int port)
-{
-	u8 temp;
-
-	temp = caleb_reg_read(CALEB_PORTEN);
-	temp |= 1 << (port + 4);
-	caleb_reg_write(CALEB_PORTEN, temp);
-}
-
-void caleb_disable_tx(int port)
-{
-	u8 temp;
-
-	temp = caleb_reg_read(CALEB_PORTEN);
-	temp &= ~(1 << (port + 4));
-	caleb_reg_write(CALEB_PORTEN, temp);
-}
diff --git a/drivers/net/ethernet/xscale/ixp2000/caleb.h b/drivers/net/ethernet/xscale/ixp2000/caleb.h
deleted file mode 100644
index e93a1ef5b8a..00000000000
--- a/drivers/net/ethernet/xscale/ixp2000/caleb.h
+++ /dev/null
@@ -1,22 +0,0 @@
-/*
- * Helper functions for the SPI-3 bridge FPGA on the Radisys ENP2611
- * Copyright (C) 2004, 2005 Lennert Buytenhek <buytenh@wantstofly.org>
- * Dedicated to Marija Kulikova.
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation; either version 2 of the License, or
- * (at your option) any later version.
- */
-
-#ifndef __CALEB_H
-#define __CALEB_H
-
-void caleb_reset(void);
-void caleb_enable_rx(int port);
-void caleb_disable_rx(int port);
-void caleb_enable_tx(int port);
-void caleb_disable_tx(int port);
-
-
-#endif
diff --git a/drivers/net/ethernet/xscale/ixp2000/enp2611.c b/drivers/net/ethernet/xscale/ixp2000/enp2611.c
deleted file mode 100644
index 34a6cfd1793..00000000000
--- a/drivers/net/ethernet/xscale/ixp2000/enp2611.c
+++ /dev/null
@@ -1,232 +0,0 @@
-/*
- * IXP2400 MSF network device driver for the Radisys ENP2611
- * Copyright (C) 2004, 2005 Lennert Buytenhek <buytenh@wantstofly.org>
- * Dedicated to Marija Kulikova.
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation; either version 2 of the License, or
- * (at your option) any later version.
- */
-
-#include <linux/module.h>
-#include <linux/kernel.h>
-#include <linux/netdevice.h>
-#include <linux/etherdevice.h>
-#include <linux/init.h>
-#include <linux/moduleparam.h>
-#include <asm/hardware/uengine.h>
-#include <asm/mach-types.h>
-#include <asm/io.h>
-#include "ixpdev.h"
-#include "caleb.h"
-#include "ixp2400-msf.h"
-#include "pm3386.h"
-
-/***********************************************************************
- * The Radisys ENP2611 is a PCI form factor board with three SFP GBIC
- * slots, connected via two PMC/Sierra 3386s and an SPI-3 bridge FPGA
- * to the IXP2400.
- *
- *                +-------------+
- * SFP GBIC #0 ---+             |       +---------+
- *                |  PM3386 #0  +-------+         |
- * SFP GBIC #1 ---+             |       | "Caleb" |         +---------+
- *                +-------------+       |         |         |         |
- *                                      | SPI-3   +---------+ IXP2400 |
- *                +-------------+       | bridge  |         |         |
- * SFP GBIC #2 ---+             |       | FPGA    |         +---------+
- *                |  PM3386 #1  +-------+         |
- *                |             |       +---------+
- *                +-------------+
- *              ^                   ^                  ^
- *              | 1.25Gbaud         | 104MHz           | 104MHz
- *              | SERDES ea.        | SPI-3 ea.        | SPI-3
- *
- ***********************************************************************/
-static struct ixp2400_msf_parameters enp2611_msf_parameters =
-{
-	.rx_mode =		IXP2400_RX_MODE_UTOPIA_POS |
-				IXP2400_RX_MODE_1x32 |
-				IXP2400_RX_MODE_MPHY |
-				IXP2400_RX_MODE_MPHY_32 |
-				IXP2400_RX_MODE_MPHY_POLLED_STATUS |
-				IXP2400_RX_MODE_MPHY_LEVEL3 |
-				IXP2400_RX_MODE_RBUF_SIZE_64,
-
-	.rxclk01_multiplier =	IXP2400_PLL_MULTIPLIER_16,
-
-	.rx_poll_ports =	3,
-
-	.rx_channel_mode = {
-		IXP2400_PORT_RX_MODE_MASTER |
-		IXP2400_PORT_RX_MODE_POS_PHY |
-		IXP2400_PORT_RX_MODE_POS_PHY_L3 |
-		IXP2400_PORT_RX_MODE_ODD_PARITY |
-		IXP2400_PORT_RX_MODE_2_CYCLE_DECODE,
-
-		IXP2400_PORT_RX_MODE_MASTER |
-		IXP2400_PORT_RX_MODE_POS_PHY |
-		IXP2400_PORT_RX_MODE_POS_PHY_L3 |
-		IXP2400_PORT_RX_MODE_ODD_PARITY |
-		IXP2400_PORT_RX_MODE_2_CYCLE_DECODE,
-
-		IXP2400_PORT_RX_MODE_MASTER |
-		IXP2400_PORT_RX_MODE_POS_PHY |
-		IXP2400_PORT_RX_MODE_POS_PHY_L3 |
-		IXP2400_PORT_RX_MODE_ODD_PARITY |
-		IXP2400_PORT_RX_MODE_2_CYCLE_DECODE,
-
-		IXP2400_PORT_RX_MODE_MASTER |
-		IXP2400_PORT_RX_MODE_POS_PHY |
-		IXP2400_PORT_RX_MODE_POS_PHY_L3 |
-		IXP2400_PORT_RX_MODE_ODD_PARITY |
-		IXP2400_PORT_RX_MODE_2_CYCLE_DECODE
-	},
-
-	.tx_mode =		IXP2400_TX_MODE_UTOPIA_POS |
-				IXP2400_TX_MODE_1x32 |
-				IXP2400_TX_MODE_MPHY |
-				IXP2400_TX_MODE_MPHY_32 |
-				IXP2400_TX_MODE_MPHY_POLLED_STATUS |
-				IXP2400_TX_MODE_MPHY_LEVEL3 |
-				IXP2400_TX_MODE_TBUF_SIZE_64,
-
-	.txclk01_multiplier =	IXP2400_PLL_MULTIPLIER_16,
-
-	.tx_poll_ports =	3,
-
-	.tx_channel_mode = {
-		IXP2400_PORT_TX_MODE_MASTER |
-		IXP2400_PORT_TX_MODE_POS_PHY |
-		IXP2400_PORT_TX_MODE_ODD_PARITY |
-		IXP2400_PORT_TX_MODE_2_CYCLE_DECODE,
-
-		IXP2400_PORT_TX_MODE_MASTER |
-		IXP2400_PORT_TX_MODE_POS_PHY |
-		IXP2400_PORT_TX_MODE_ODD_PARITY |
-		IXP2400_PORT_TX_MODE_2_CYCLE_DECODE,
-
-		IXP2400_PORT_TX_MODE_MASTER |
-		IXP2400_PORT_TX_MODE_POS_PHY |
-		IXP2400_PORT_TX_MODE_ODD_PARITY |
-		IXP2400_PORT_TX_MODE_2_CYCLE_DECODE,
-
-		IXP2400_PORT_TX_MODE_MASTER |
-		IXP2400_PORT_TX_MODE_POS_PHY |
-		IXP2400_PORT_TX_MODE_ODD_PARITY |
-		IXP2400_PORT_TX_MODE_2_CYCLE_DECODE
-	}
-};
-
-static struct net_device *nds[3];
-static struct timer_list link_check_timer;
-
-/* @@@ Poll the SFP moddef0 line too.  */
-/* @@@ Try to use the pm3386 DOOL interrupt as well.  */
-static void enp2611_check_link_status(unsigned long __dummy)
-{
-	int i;
-
-	for (i = 0; i < 3; i++) {
-		struct net_device *dev;
-		int status;
-
-		dev = nds[i];
-		if (dev == NULL)
-			continue;
-
-		status = pm3386_is_link_up(i);
-		if (status && !netif_carrier_ok(dev)) {
-			/* @@@ Should report autonegotiation status.  */
-			printk(KERN_INFO "%s: NIC Link is Up\n", dev->name);
-
-			pm3386_enable_tx(i);
-			caleb_enable_tx(i);
-			netif_carrier_on(dev);
-		} else if (!status && netif_carrier_ok(dev)) {
-			printk(KERN_INFO "%s: NIC Link is Down\n", dev->name);
-
-			netif_carrier_off(dev);
-			caleb_disable_tx(i);
-			pm3386_disable_tx(i);
-		}
-	}
-
-	link_check_timer.expires = jiffies + HZ / 10;
-	add_timer(&link_check_timer);
-}
-
-static void enp2611_set_port_admin_status(int port, int up)
-{
-	if (up) {
-		caleb_enable_rx(port);
-
-		pm3386_set_carrier(port, 1);
-		pm3386_enable_rx(port);
-	} else {
-		caleb_disable_tx(port);
-		pm3386_disable_tx(port);
-		/* @@@ Flush out pending packets.  */
-		pm3386_set_carrier(port, 0);
-
-		pm3386_disable_rx(port);
-		caleb_disable_rx(port);
-	}
-}
-
-static int __init enp2611_init_module(void)
-{ 
-	int ports;
-	int i;
-
-	if (!machine_is_enp2611())
-		return -ENODEV;
-
-	caleb_reset();
-	pm3386_reset();
-
-	ports = pm3386_port_count();
-	for (i = 0; i < ports; i++) {
-		nds[i] = ixpdev_alloc(i, sizeof(struct ixpdev_priv));
-		if (nds[i] == NULL) {
-			while (--i >= 0)
-				free_netdev(nds[i]);
-			return -ENOMEM;
-		}
-
-		pm3386_init_port(i);
-		pm3386_get_mac(i, nds[i]->dev_addr);
-	}
-
-	ixp2400_msf_init(&enp2611_msf_parameters);
-
-	if (ixpdev_init(ports, nds, enp2611_set_port_admin_status)) {
-		for (i = 0; i < ports; i++)
-			if (nds[i])
-				free_netdev(nds[i]);
-		return -EINVAL;
-	}
-
-	init_timer(&link_check_timer);
-	link_check_timer.function = enp2611_check_link_status;
-	link_check_timer.expires = jiffies;
-	add_timer(&link_check_timer);
-
-	return 0;
-}
-
-static void __exit enp2611_cleanup_module(void)
-{
-	int i;
-
-	del_timer_sync(&link_check_timer);
-
-	ixpdev_deinit();
-	for (i = 0; i < 3; i++)
-		free_netdev(nds[i]);
-}
-
-module_init(enp2611_init_module);
-module_exit(enp2611_cleanup_module);
-MODULE_LICENSE("GPL");
diff --git a/drivers/net/ethernet/xscale/ixp2000/ixp2400-msf.c b/drivers/net/ethernet/xscale/ixp2000/ixp2400-msf.c
deleted file mode 100644
index f5ffd7e05d2..00000000000
--- a/drivers/net/ethernet/xscale/ixp2000/ixp2400-msf.c
+++ /dev/null
@@ -1,212 +0,0 @@
-/*
- * Generic library functions for the MSF (Media and Switch Fabric) unit
- * found on the Intel IXP2400 network processor.
- *
- * Copyright (C) 2004, 2005 Lennert Buytenhek <buytenh@wantstofly.org>
- * Dedicated to Marija Kulikova.
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU Lesser General Public License as
- * published by the Free Software Foundation; either version 2.1 of the
- * License, or (at your option) any later version.
- */
-
-#include <linux/kernel.h>
-#include <linux/init.h>
-#include <mach/hardware.h>
-#include <mach/ixp2000-regs.h>
-#include <asm/delay.h>
-#include <asm/io.h>
-#include "ixp2400-msf.h"
-
-/*
- * This is the Intel recommended PLL init procedure as described on
- * page 340 of the IXP2400/IXP2800 Programmer's Reference Manual.
- */
-static void ixp2400_pll_init(struct ixp2400_msf_parameters *mp)
-{
-	int rx_dual_clock;
-	int tx_dual_clock;
-	u32 value;
-
-	/*
-	 * If the RX mode is not 1x32, we have to enable both RX PLLs
-	 * (#0 and #1.)  The same thing for the TX direction.
-	 */
-	rx_dual_clock = !!(mp->rx_mode & IXP2400_RX_MODE_WIDTH_MASK);
-	tx_dual_clock = !!(mp->tx_mode & IXP2400_TX_MODE_WIDTH_MASK);
-
-	/*
-	 * Read initial value.
-	 */
-	value = ixp2000_reg_read(IXP2000_MSF_CLK_CNTRL);
-
-	/*
-	 * Put PLLs in powerdown and bypass mode.
-	 */
-	value |= 0x0000f0f0;
-	ixp2000_reg_write(IXP2000_MSF_CLK_CNTRL, value);
-
-	/*
-	 * Set single or dual clock mode bits.
-	 */
-	value &= ~0x03000000;
-	value |= (rx_dual_clock << 24) | (tx_dual_clock << 25);
-
-	/*
-	 * Set multipliers.
-	 */
-	value &= ~0x00ff0000;
-	value |= mp->rxclk01_multiplier << 16;
-	value |= mp->rxclk23_multiplier << 18;
-	value |= mp->txclk01_multiplier << 20;
-	value |= mp->txclk23_multiplier << 22;
-
-	/*
-	 * And write value.
-	 */
-	ixp2000_reg_write(IXP2000_MSF_CLK_CNTRL, value);
-
-	/*
-	 * Disable PLL bypass mode.
-	 */
-	value &= ~(0x00005000 | rx_dual_clock << 13 | tx_dual_clock << 15);
-	ixp2000_reg_write(IXP2000_MSF_CLK_CNTRL, value);
-
-	/*
-	 * Turn on PLLs.
-	 */
-	value &= ~(0x00000050 | rx_dual_clock << 5 | tx_dual_clock << 7);
-	ixp2000_reg_write(IXP2000_MSF_CLK_CNTRL, value);
-
-	/*
-	 * Wait for PLLs to lock.  There are lock status bits, but IXP2400
-	 * erratum #65 says that these lock bits should not be relied upon
-	 * as they might not accurately reflect the true state of the PLLs.
-	 */
-	udelay(100);
-}
-
-/*
- * Needed according to p480 of Programmer's Reference Manual.
- */
-static void ixp2400_msf_free_rbuf_entries(struct ixp2400_msf_parameters *mp)
-{
-	int size_bits;
-	int i;
-
-	/*
-	 * Work around IXP2400 erratum #69 (silent RBUF-to-DRAM transfer
-	 * corruption) in the Intel-recommended way: do not add the RBUF
-	 * elements susceptible to corruption to the freelist.
-	 */
-	size_bits = mp->rx_mode & IXP2400_RX_MODE_RBUF_SIZE_MASK;
-	if (size_bits == IXP2400_RX_MODE_RBUF_SIZE_64) {
-		for (i = 1; i < 128; i++) {
-			if (i == 9 || i == 18 || i == 27)
-				continue;
-			ixp2000_reg_write(IXP2000_MSF_RBUF_ELEMENT_DONE, i);
-		}
-	} else if (size_bits == IXP2400_RX_MODE_RBUF_SIZE_128) {
-		for (i = 1; i < 64; i++) {
-			if (i == 4 || i == 9 || i == 13)
-				continue;
-			ixp2000_reg_write(IXP2000_MSF_RBUF_ELEMENT_DONE, i);
-		}
-	} else if (size_bits == IXP2400_RX_MODE_RBUF_SIZE_256) {
-		for (i = 1; i < 32; i++) {
-			if (i == 2 || i == 4 || i == 6)
-				continue;
-			ixp2000_reg_write(IXP2000_MSF_RBUF_ELEMENT_DONE, i);
-		}
-	}
-}
-
-static u32 ixp2400_msf_valid_channels(u32 reg)
-{
-	u32 channels;
-
-	channels = 0;
-	switch (reg & IXP2400_RX_MODE_WIDTH_MASK) {
-	case IXP2400_RX_MODE_1x32:
-		channels = 0x1;
-		if (reg & IXP2400_RX_MODE_MPHY &&
-		    !(reg & IXP2400_RX_MODE_MPHY_32))
-			channels = 0xf;
-		break;
-
-	case IXP2400_RX_MODE_2x16:
-		channels = 0x5;
-		break;
-
-	case IXP2400_RX_MODE_4x8:
-		channels = 0xf;
-		break;
-
-	case IXP2400_RX_MODE_1x16_2x8:
-		channels = 0xd;
-		break;
-	}
-
-	return channels;
-}
-
-static void ixp2400_msf_enable_rx(struct ixp2400_msf_parameters *mp)
-{
-	u32 value;
-
-	value = ixp2000_reg_read(IXP2000_MSF_RX_CONTROL) & 0x0fffffff;
-	value |= ixp2400_msf_valid_channels(mp->rx_mode) << 28;
-	ixp2000_reg_write(IXP2000_MSF_RX_CONTROL, value);
-}
-
-static void ixp2400_msf_enable_tx(struct ixp2400_msf_parameters *mp)
-{
-	u32 value;
-
-	value = ixp2000_reg_read(IXP2000_MSF_TX_CONTROL) & 0x0fffffff;
-	value |= ixp2400_msf_valid_channels(mp->tx_mode) << 28;
-	ixp2000_reg_write(IXP2000_MSF_TX_CONTROL, value);
-}
-
-
-void ixp2400_msf_init(struct ixp2400_msf_parameters *mp)
-{
-	u32 value;
-	int i;
-
-	/*
-	 * Init the RX/TX PLLs based on the passed parameter block.
-	 */
-	ixp2400_pll_init(mp);
-
-	/*
-	 * Reset MSF.  Bit 7 in IXP_RESET_0 resets the MSF.
-	 */
-	value = ixp2000_reg_read(IXP2000_RESET0);
-	ixp2000_reg_write(IXP2000_RESET0, value | 0x80);
-	ixp2000_reg_write(IXP2000_RESET0, value & ~0x80);
-
-	/*
-	 * Initialise the RX section.
-	 */
-	ixp2000_reg_write(IXP2000_MSF_RX_MPHY_POLL_LIMIT, mp->rx_poll_ports - 1);
-	ixp2000_reg_write(IXP2000_MSF_RX_CONTROL, mp->rx_mode);
-	for (i = 0; i < 4; i++) {
-		ixp2000_reg_write(IXP2000_MSF_RX_UP_CONTROL_0 + i,
-						mp->rx_channel_mode[i]);
-	}
-	ixp2400_msf_free_rbuf_entries(mp);
-	ixp2400_msf_enable_rx(mp);
-
-	/*
-	 * Initialise the TX section.
-	 */
-	ixp2000_reg_write(IXP2000_MSF_TX_MPHY_POLL_LIMIT, mp->tx_poll_ports - 1);
-	ixp2000_reg_write(IXP2000_MSF_TX_CONTROL, mp->tx_mode);
-	for (i = 0; i < 4; i++) {
-		ixp2000_reg_write(IXP2000_MSF_TX_UP_CONTROL_0 + i,
-						mp->tx_channel_mode[i]);
-	}
-	ixp2400_msf_enable_tx(mp);
-}
diff --git a/drivers/net/ethernet/xscale/ixp2000/ixp2400-msf.h b/drivers/net/ethernet/xscale/ixp2000/ixp2400-msf.h
deleted file mode 100644
index 3ac1af2771d..00000000000
--- a/drivers/net/ethernet/xscale/ixp2000/ixp2400-msf.h
+++ /dev/null
@@ -1,115 +0,0 @@
-/*
- * Generic library functions for the MSF (Media and Switch Fabric) unit
- * found on the Intel IXP2400 network processor.
- *
- * Copyright (C) 2004, 2005 Lennert Buytenhek <buytenh@wantstofly.org>
- * Dedicated to Marija Kulikova.
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU Lesser General Public License as
- * published by the Free Software Foundation; either version 2.1 of the
- * License, or (at your option) any later version.
- */
-
-#ifndef __IXP2400_MSF_H
-#define __IXP2400_MSF_H
-
-struct ixp2400_msf_parameters
-{
-	u32				rx_mode;
-	unsigned			rxclk01_multiplier:2;
-	unsigned			rxclk23_multiplier:2;
-	unsigned			rx_poll_ports:6;
-	u32				rx_channel_mode[4];
-
-	u32				tx_mode;
-	unsigned			txclk01_multiplier:2;
-	unsigned			txclk23_multiplier:2;
-	unsigned			tx_poll_ports:6;
-	u32				tx_channel_mode[4];
-};
-
-void ixp2400_msf_init(struct ixp2400_msf_parameters *mp);
-
-#define IXP2400_PLL_MULTIPLIER_48		0x00
-#define IXP2400_PLL_MULTIPLIER_24		0x01
-#define IXP2400_PLL_MULTIPLIER_16		0x02
-#define IXP2400_PLL_MULTIPLIER_12		0x03
-
-#define IXP2400_RX_MODE_CSIX			0x00400000
-#define IXP2400_RX_MODE_UTOPIA_POS		0x00000000
-#define IXP2400_RX_MODE_WIDTH_MASK		0x00300000
-#define IXP2400_RX_MODE_1x16_2x8		0x00300000
-#define IXP2400_RX_MODE_4x8			0x00200000
-#define IXP2400_RX_MODE_2x16			0x00100000
-#define IXP2400_RX_MODE_1x32			0x00000000
-#define IXP2400_RX_MODE_MPHY			0x00080000
-#define IXP2400_RX_MODE_SPHY			0x00000000
-#define IXP2400_RX_MODE_MPHY_32			0x00040000
-#define IXP2400_RX_MODE_MPHY_4			0x00000000
-#define IXP2400_RX_MODE_MPHY_POLLED_STATUS	0x00020000
-#define IXP2400_RX_MODE_MPHY_DIRECT_STATUS	0x00000000
-#define IXP2400_RX_MODE_CBUS_FULL_DUPLEX	0x00010000
-#define IXP2400_RX_MODE_CBUS_SIMPLEX		0x00000000
-#define IXP2400_RX_MODE_MPHY_LEVEL2		0x00004000
-#define IXP2400_RX_MODE_MPHY_LEVEL3		0x00000000
-#define IXP2400_RX_MODE_CBUS_8BIT		0x00002000
-#define IXP2400_RX_MODE_CBUS_4BIT		0x00000000
-#define IXP2400_RX_MODE_CSIX_SINGLE_FREELIST	0x00000200
-#define IXP2400_RX_MODE_CSIX_SPLIT_FREELISTS	0x00000000
-#define IXP2400_RX_MODE_RBUF_SIZE_MASK		0x0000000c
-#define IXP2400_RX_MODE_RBUF_SIZE_256		0x00000008
-#define IXP2400_RX_MODE_RBUF_SIZE_128		0x00000004
-#define IXP2400_RX_MODE_RBUF_SIZE_64		0x00000000
-
-#define IXP2400_PORT_RX_MODE_SLAVE		0x00000040
-#define IXP2400_PORT_RX_MODE_MASTER		0x00000000
-#define IXP2400_PORT_RX_MODE_POS_PHY_L3		0x00000020
-#define IXP2400_PORT_RX_MODE_POS_PHY_L2		0x00000000
-#define IXP2400_PORT_RX_MODE_POS_PHY		0x00000010
-#define IXP2400_PORT_RX_MODE_UTOPIA		0x00000000
-#define IXP2400_PORT_RX_MODE_EVEN_PARITY	0x0000000c
-#define IXP2400_PORT_RX_MODE_ODD_PARITY		0x00000008
-#define IXP2400_PORT_RX_MODE_NO_PARITY		0x00000000
-#define IXP2400_PORT_RX_MODE_UTOPIA_BIG_CELLS	0x00000002
-#define IXP2400_PORT_RX_MODE_UTOPIA_NORMAL_CELLS	0x00000000
-#define IXP2400_PORT_RX_MODE_2_CYCLE_DECODE	0x00000001
-#define IXP2400_PORT_RX_MODE_1_CYCLE_DECODE	0x00000000
-
-#define IXP2400_TX_MODE_CSIX			0x00400000
-#define IXP2400_TX_MODE_UTOPIA_POS		0x00000000
-#define IXP2400_TX_MODE_WIDTH_MASK		0x00300000
-#define IXP2400_TX_MODE_1x16_2x8		0x00300000
-#define IXP2400_TX_MODE_4x8			0x00200000
-#define IXP2400_TX_MODE_2x16			0x00100000
-#define IXP2400_TX_MODE_1x32			0x00000000
-#define IXP2400_TX_MODE_MPHY			0x00080000
-#define IXP2400_TX_MODE_SPHY			0x00000000
-#define IXP2400_TX_MODE_MPHY_32			0x00040000
-#define IXP2400_TX_MODE_MPHY_4			0x00000000
-#define IXP2400_TX_MODE_MPHY_POLLED_STATUS	0x00020000
-#define IXP2400_TX_MODE_MPHY_DIRECT_STATUS	0x00000000
-#define IXP2400_TX_MODE_CBUS_FULL_DUPLEX	0x00010000
-#define IXP2400_TX_MODE_CBUS_SIMPLEX		0x00000000
-#define IXP2400_TX_MODE_MPHY_LEVEL2		0x00004000
-#define IXP2400_TX_MODE_MPHY_LEVEL3		0x00000000
-#define IXP2400_TX_MODE_CBUS_8BIT		0x00002000
-#define IXP2400_TX_MODE_CBUS_4BIT		0x00000000
-#define IXP2400_TX_MODE_TBUF_SIZE_MASK		0x0000000c
-#define IXP2400_TX_MODE_TBUF_SIZE_256		0x00000008
-#define IXP2400_TX_MODE_TBUF_SIZE_128		0x00000004
-#define IXP2400_TX_MODE_TBUF_SIZE_64		0x00000000
-
-#define IXP2400_PORT_TX_MODE_SLAVE		0x00000040
-#define IXP2400_PORT_TX_MODE_MASTER		0x00000000
-#define IXP2400_PORT_TX_MODE_POS_PHY		0x00000010
-#define IXP2400_PORT_TX_MODE_UTOPIA		0x00000000
-#define IXP2400_PORT_TX_MODE_EVEN_PARITY	0x0000000c
-#define IXP2400_PORT_TX_MODE_ODD_PARITY		0x00000008
-#define IXP2400_PORT_TX_MODE_NO_PARITY		0x00000000
-#define IXP2400_PORT_TX_MODE_UTOPIA_BIG_CELLS	0x00000002
-#define IXP2400_PORT_TX_MODE_2_CYCLE_DECODE	0x00000001
-#define IXP2400_PORT_TX_MODE_1_CYCLE_DECODE	0x00000000
-
-
-#endif
diff --git a/drivers/net/ethernet/xscale/ixp2000/ixp2400_rx.uc b/drivers/net/ethernet/xscale/ixp2000/ixp2400_rx.uc
deleted file mode 100644
index 42a73e357af..00000000000
--- a/drivers/net/ethernet/xscale/ixp2000/ixp2400_rx.uc
+++ /dev/null
@@ -1,408 +0,0 @@
-/*
- * RX ucode for the Intel IXP2400 in POS-PHY mode.
- * Copyright (C) 2004, 2005 Lennert Buytenhek
- * Dedicated to Marija Kulikova.
- *
- * 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.
- *
- * Assumptions made in this code:
- * - The IXP2400 MSF is configured for POS-PHY mode, in a mode where
- *   only one full element list is used.  This includes, for example,
- *   1x32 SPHY and 1x32 MPHY32, but not 4x8 SPHY or 1x32 MPHY4.  (This
- *   is not an exhaustive list.)
- * - The RBUF uses 64-byte mpackets.
- * - RX descriptors reside in SRAM, and have the following format:
- *	struct rx_desc
- *	{
- *	// to uengine
- *		u32	buf_phys_addr;
- *		u32	buf_length;
- *
- *	// from uengine
- *		u32	channel;
- *		u32	pkt_length;
- *	};
- * - Packet data resides in DRAM.
- * - Packet buffer addresses are 8-byte aligned.
- * - Scratch ring 0 is rx_pending.
- * - Scratch ring 1 is rx_done, and has status condition 'full'.
- * - The host triggers rx_done flush and rx_pending refill on seeing INTA.
- * - This code is run on all eight threads of the microengine it runs on.
- *
- * Local memory is used for per-channel RX state.
- */
-
-#define RX_THREAD_FREELIST_0		0x0030
-#define RBUF_ELEMENT_DONE		0x0044
-
-#define CHANNEL_FLAGS			*l$index0[0]
-#define CHANNEL_FLAG_RECEIVING		1
-#define PACKET_LENGTH			*l$index0[1]
-#define PACKET_CHECKSUM			*l$index0[2]
-#define BUFFER_HANDLE			*l$index0[3]
-#define BUFFER_START			*l$index0[4]
-#define BUFFER_LENGTH			*l$index0[5]
-
-#define CHANNEL_STATE_SIZE		24	// in bytes
-#define CHANNEL_STATE_SHIFT		5	// ceil(log2(state size))
-
-
-	.sig volatile sig1
-	.sig volatile sig2
-	.sig volatile sig3
-
-	.sig mpacket_arrived
-	.reg add_to_rx_freelist
-	.reg read $rsw0, $rsw1
-	.xfer_order $rsw0 $rsw1
-
-	.reg zero
-
-	/*
-	 * Initialise add_to_rx_freelist.
-	 */
-	.begin
-		.reg temp
-		.reg temp2
-
-		immed[add_to_rx_freelist, RX_THREAD_FREELIST_0]
-		immed_w1[add_to_rx_freelist, (&$rsw0 | (&mpacket_arrived << 12))]
-
-		local_csr_rd[ACTIVE_CTX_STS]
-		immed[temp, 0]
-		alu[temp2, temp, and, 0x1f]
-		alu_shf[add_to_rx_freelist, add_to_rx_freelist, or, temp2, <<20]
-		alu[temp2, temp, and, 0x80]
-		alu_shf[add_to_rx_freelist, add_to_rx_freelist, or, temp2, <<18]
-	.end
-
-	immed[zero, 0]
-
-	/*
-	 * Skip context 0 initialisation?
-	 */
-	.begin
-		br!=ctx[0, mpacket_receive_loop#]
-	.end
-
-	/*
-	 * Initialise local memory.
-	 */
-	.begin
-		.reg addr
-		.reg temp
-
-		immed[temp, 0]
-	init_local_mem_loop#:
-		alu_shf[addr, --, b, temp, <<CHANNEL_STATE_SHIFT]
-		local_csr_wr[ACTIVE_LM_ADDR_0, addr]
-		nop
-		nop
-		nop
-
-		immed[CHANNEL_FLAGS, 0]
-
-		alu[temp, temp, +, 1]
-		alu[--, temp, and, 0x20]
-		beq[init_local_mem_loop#]
-	.end
-
-	/*
-	 * Initialise signal pipeline.
-	 */
-	.begin
-		local_csr_wr[SAME_ME_SIGNAL, (&sig1 << 3)]
-		.set_sig sig1
-
-		local_csr_wr[SAME_ME_SIGNAL, (&sig2 << 3)]
-		.set_sig sig2
-
-		local_csr_wr[SAME_ME_SIGNAL, (&sig3 << 3)]
-		.set_sig sig3
-	.end
-
-mpacket_receive_loop#:
-	/*
-	 * Synchronise and wait for mpacket.
-	 */
-	.begin
-		ctx_arb[sig1]
-		local_csr_wr[SAME_ME_SIGNAL, (0x80 | (&sig1 << 3))]
-
-		msf[fast_wr, --, add_to_rx_freelist, 0]
-		.set_sig mpacket_arrived
-		ctx_arb[mpacket_arrived]
-		.set $rsw0 $rsw1
-	.end
-
-	/*
-	 * We halt if we see {inbparerr,parerr,null,soperror}.
-	 */
-	.begin
-		alu_shf[--, 0x1b, and, $rsw0, >>8]
-		bne[abort_rswerr#]
-	.end
-
-	/*
-	 * Point local memory pointer to this channel's state area.
-	 */
-	.begin
-		.reg chanaddr
-
-		alu[chanaddr, $rsw0, and, 0x1f]
-		alu_shf[chanaddr, --, b, chanaddr, <<CHANNEL_STATE_SHIFT]
-		local_csr_wr[ACTIVE_LM_ADDR_0, chanaddr]
-		nop
-		nop
-		nop
-	.end
-
-	/*
-	 * Check whether we received a SOP mpacket while we were already
-	 * working on a packet, or a non-SOP mpacket while there was no
-	 * packet pending.  (SOP == RECEIVING -> abort)  If everything's
-	 * okay, update the RECEIVING flag to reflect our new state.
-	 */
-	.begin
-		.reg temp
-		.reg eop
-
-		#if CHANNEL_FLAG_RECEIVING != 1
-		#error CHANNEL_FLAG_RECEIVING is not 1
-		#endif
-
-		alu_shf[temp, 1, and, $rsw0, >>15]
-		alu[temp, temp, xor, CHANNEL_FLAGS]
-		alu[--, temp, and, CHANNEL_FLAG_RECEIVING]
-		beq[abort_proterr#]
-
-		alu_shf[eop, 1, and, $rsw0, >>14]
-		alu[CHANNEL_FLAGS, temp, xor, eop]
-	.end
-
-	/*
-	 * Copy the mpacket into the right spot, and in case of EOP,
-	 * write back the descriptor and pass the packet on.
-	 */
-	.begin
-		.reg buffer_offset
-		.reg _packet_length
-		.reg _packet_checksum
-		.reg _buffer_handle
-		.reg _buffer_start
-		.reg _buffer_length
-
-		/*
-		 * Determine buffer_offset, _packet_length and
-		 * _packet_checksum.
-		 */
-		.begin
-			.reg temp
-
-			alu[--, 1, and, $rsw0, >>15]
-			beq[not_sop#]
-
-			immed[PACKET_LENGTH, 0]
-			immed[PACKET_CHECKSUM, 0]
-
-		not_sop#:
-			alu[buffer_offset, --, b, PACKET_LENGTH]
-			alu_shf[temp, 0xff, and, $rsw0, >>16]
-			alu[_packet_length, buffer_offset, +, temp]
-			alu[PACKET_LENGTH, --, b, _packet_length]
-
-			immed[temp, 0xffff]
-			alu[temp, $rsw1, and, temp]
-			alu[_packet_checksum, PACKET_CHECKSUM, +, temp]
-			alu[PACKET_CHECKSUM, --, b, _packet_checksum]
-		.end
-
-		/*
-		 * Allocate buffer in case of SOP.
-		 */
-		.begin
-			.reg temp
-
-			alu[temp, 1, and, $rsw0, >>15]
-			beq[skip_buffer_alloc#]
-
-			.begin
-				.sig zzz
-				.reg read $stemp $stemp2
-				.xfer_order $stemp $stemp2
-
-			rx_nobufs#:
-				scratch[get, $stemp, zero, 0, 1], ctx_swap[zzz]
-				alu[_buffer_handle, --, b, $stemp]
-				beq[rx_nobufs#]
-
-				sram[read, $stemp, _buffer_handle, 0, 2],
-								ctx_swap[zzz]
-				alu[_buffer_start, --, b, $stemp]
-				alu[_buffer_length, --, b, $stemp2]
-			.end
-
-		skip_buffer_alloc#:
-		.end
-
-		/*
-		 * Resynchronise.
-		 */
-		.begin
-			ctx_arb[sig2]
-			local_csr_wr[SAME_ME_SIGNAL, (0x80 | (&sig2 << 3))]
-		.end
-
-		/*
-		 * Synchronise buffer state.
-		 */
-		.begin
-			.reg temp
-
-			alu[temp, 1, and, $rsw0, >>15]
-			beq[copy_from_local_mem#]
-
-			alu[BUFFER_HANDLE, --, b, _buffer_handle]
-			alu[BUFFER_START, --, b, _buffer_start]
-			alu[BUFFER_LENGTH, --, b, _buffer_length]
-			br[sync_state_done#]
-
-		copy_from_local_mem#:
-			alu[_buffer_handle, --, b, BUFFER_HANDLE]
-			alu[_buffer_start, --, b, BUFFER_START]
-			alu[_buffer_length, --, b, BUFFER_LENGTH]
-
-		sync_state_done#:
-		.end
-
-#if 0
-		/*
-		 * Debug buffer state management.
-		 */
-		.begin
-			.reg temp
-
-			alu[temp, 1, and, $rsw0, >>14]
-			beq[no_poison#]
-			immed[BUFFER_HANDLE, 0xdead]
-			immed[BUFFER_START, 0xdead]
-			immed[BUFFER_LENGTH, 0xdead]
-		no_poison#:
-
-			immed[temp, 0xdead]
-			alu[--, _buffer_handle, -, temp]
-			beq[state_corrupted#]
-			alu[--, _buffer_start, -, temp]
-			beq[state_corrupted#]
-			alu[--, _buffer_length, -, temp]
-			beq[state_corrupted#]
-		.end
-#endif
-
-		/*
-		 * Check buffer length.
-		 */
-		.begin
-			alu[--, _buffer_length, -, _packet_length]
-			blo[buffer_overflow#]
-		.end
-
-		/*
-		 * Copy the mpacket and give back the RBUF element.
-		 */
-		.begin
-			.reg element
-			.reg xfer_size
-			.reg temp
-			.sig copy_sig
-
-			alu_shf[element, 0x7f, and, $rsw0, >>24]
-			alu_shf[xfer_size, 0xff, and, $rsw0, >>16]
-
-			alu[xfer_size, xfer_size, -, 1]
-			alu_shf[xfer_size, 0x10, or, xfer_size, >>3]
-			alu_shf[temp, 0x10, or, xfer_size, <<21]
-			alu_shf[temp, temp, or, element, <<11]
-			alu_shf[--, temp, or, 1, <<18]
-
-			dram[rbuf_rd, --, _buffer_start, buffer_offset, max_8],
-						indirect_ref, sig_done[copy_sig]
-			ctx_arb[copy_sig]
-
-			alu[temp, RBUF_ELEMENT_DONE, or, element, <<16]
-			msf[fast_wr, --, temp, 0]
-		.end
-
-		/*
-		 * If EOP, write back the packet descriptor.
-		 */
-		.begin
-			.reg write $stemp $stemp2
-			.xfer_order $stemp $stemp2
-			.sig zzz
-
-			alu_shf[--, 1, and, $rsw0, >>14]
-			beq[no_writeback#]
-
-			alu[$stemp, $rsw0, and, 0x1f]
-			alu[$stemp2, --, b, _packet_length]
-			sram[write, $stemp, _buffer_handle, 8, 2], ctx_swap[zzz]
-
-		no_writeback#:
-		.end
-
-		/*
-		 * Resynchronise.
-		 */
-		.begin
-			ctx_arb[sig3]
-			local_csr_wr[SAME_ME_SIGNAL, (0x80 | (&sig3 << 3))]
-		.end
-
-		/*
-		 * If EOP, put the buffer back onto the scratch ring.
-		 */
-		.begin
-			.reg write $stemp
-			.sig zzz
-
-			br_inp_state[SCR_Ring1_Status, rx_done_ring_overflow#]
-
-			alu_shf[--, 1, and, $rsw0, >>14]
-			beq[mpacket_receive_loop#]
-
-			alu[--, 1, and, $rsw0, >>10]
-			bne[rxerr#]
-
-			alu[$stemp, --, b, _buffer_handle]
-			scratch[put, $stemp, zero, 4, 1], ctx_swap[zzz]
-			cap[fast_wr, 0, XSCALE_INT_A]
-			br[mpacket_receive_loop#]
-
-		rxerr#:
-			alu[$stemp, --, b, _buffer_handle]
-			scratch[put, $stemp, zero, 0, 1], ctx_swap[zzz]
-			br[mpacket_receive_loop#]
-		.end
-	.end
-
-
-abort_rswerr#:
-	halt
-
-abort_proterr#:
-	halt
-
-state_corrupted#:
-	halt
-
-buffer_overflow#:
-	halt
-
-rx_done_ring_overflow#:
-	halt
-
-
diff --git a/drivers/net/ethernet/xscale/ixp2000/ixp2400_rx.ucode b/drivers/net/ethernet/xscale/ixp2000/ixp2400_rx.ucode
deleted file mode 100644
index e8aee2f81aa..00000000000
--- a/drivers/net/ethernet/xscale/ixp2000/ixp2400_rx.ucode
+++ /dev/null
@@ -1,130 +0,0 @@
-static struct ixp2000_uengine_code ixp2400_rx =
-{
-	.cpu_model_bitmask	= 0x000003fe,
-	.cpu_min_revision	= 0,
-	.cpu_max_revision	= 255,
-
-	.uengine_parameters	= IXP2000_UENGINE_8_CONTEXTS |
-				  IXP2000_UENGINE_PRN_UPDATE_EVERY |
-				  IXP2000_UENGINE_NN_FROM_PREVIOUS |
-				  IXP2000_UENGINE_ASSERT_EMPTY_AT_0 |
-				  IXP2000_UENGINE_LM_ADDR1_PER_CONTEXT |
-				  IXP2000_UENGINE_LM_ADDR0_PER_CONTEXT,
-
-	.initial_reg_values	= (struct ixp2000_reg_value []) {
-		{ -1, -1 }
-	},
-
-	.num_insns		= 109,
-	.insns			= (u8 []) {
-		0xf0, 0x00, 0x0c, 0xc0, 0x05,
-		0xf4, 0x44, 0x0c, 0x00, 0x05,
-		0xfc, 0x04, 0x4c, 0x00, 0x00,
-		0xf0, 0x00, 0x00, 0x3b, 0x00,
-		0xb4, 0x40, 0xf0, 0x3b, 0x1f,
-		0x8a, 0xc0, 0x50, 0x3e, 0x05,
-		0xb4, 0x40, 0xf0, 0x3b, 0x80,
-		0x9a, 0xe0, 0x00, 0x3e, 0x05,
-		0xf0, 0x00, 0x00, 0x07, 0x00,
-		0xd8, 0x05, 0xc0, 0x00, 0x11,
-		0xf0, 0x00, 0x00, 0x0f, 0x00,
-		0x91, 0xb0, 0x20, 0x0e, 0x00,
-		0xfc, 0x06, 0x60, 0x0b, 0x00,
-		0xf0, 0x00, 0x0c, 0x03, 0x00,
-		0xf0, 0x00, 0x0c, 0x03, 0x00,
-		0xf0, 0x00, 0x0c, 0x03, 0x00,
-		0xf0, 0x00, 0x0c, 0x02, 0x00,
-		0xb0, 0xc0, 0x30, 0x0f, 0x01,
-		0xa4, 0x70, 0x00, 0x0f, 0x20,
-		0xd8, 0x02, 0xc0, 0x01, 0x00,
-		0xfc, 0x10, 0xac, 0x23, 0x08,
-		0xfc, 0x10, 0xac, 0x43, 0x10,
-		0xfc, 0x10, 0xac, 0x63, 0x18,
-		0xe0, 0x00, 0x00, 0x00, 0x02,
-		0xfc, 0x10, 0xae, 0x23, 0x88,
-		0x3d, 0x00, 0x04, 0x03, 0x20,
-		0xe0, 0x00, 0x00, 0x00, 0x10,
-		0x84, 0x82, 0x02, 0x01, 0x3b,
-		0xd8, 0x1a, 0x00, 0x01, 0x01,
-		0xb4, 0x00, 0x8c, 0x7d, 0x80,
-		0x91, 0xb0, 0x80, 0x22, 0x00,
-		0xfc, 0x06, 0x60, 0x23, 0x00,
-		0xf0, 0x00, 0x0c, 0x03, 0x00,
-		0xf0, 0x00, 0x0c, 0x03, 0x00,
-		0xf0, 0x00, 0x0c, 0x03, 0x00,
-		0x94, 0xf0, 0x92, 0x01, 0x21,
-		0xac, 0x40, 0x60, 0x26, 0x00,
-		0xa4, 0x30, 0x0c, 0x04, 0x06,
-		0xd8, 0x1a, 0x40, 0x01, 0x00,
-		0x94, 0xe0, 0xa2, 0x01, 0x21,
-		0xac, 0x20, 0x00, 0x28, 0x06,
-		0x84, 0xf2, 0x02, 0x01, 0x21,
-		0xd8, 0x0b, 0x40, 0x01, 0x00,
-		0xf0, 0x00, 0x0c, 0x02, 0x01,
-		0xf0, 0x00, 0x0c, 0x02, 0x02,
-		0xa0, 0x00, 0x08, 0x04, 0x00,
-		0x95, 0x00, 0xc6, 0x01, 0xff,
-		0xa0, 0x80, 0x10, 0x30, 0x00,
-		0xa0, 0x60, 0x1c, 0x00, 0x01,
-		0xf0, 0x0f, 0xf0, 0x33, 0xff,
-		0xb4, 0x00, 0xc0, 0x31, 0x81,
-		0xb0, 0x80, 0xb0, 0x32, 0x02,
-		0xa0, 0x20, 0x20, 0x2c, 0x00,
-		0x94, 0xf0, 0xd2, 0x01, 0x21,
-		0xd8, 0x0f, 0x40, 0x01, 0x00,
-		0x19, 0x40, 0x10, 0x04, 0x20,
-		0xa0, 0x00, 0x26, 0x04, 0x00,
-		0xd8, 0x0d, 0xc0, 0x01, 0x00,
-		0x00, 0x42, 0x10, 0x80, 0x02,
-		0xb0, 0x00, 0x46, 0x04, 0x00,
-		0xb0, 0x00, 0x56, 0x08, 0x00,
-		0xe0, 0x00, 0x00, 0x00, 0x04,
-		0xfc, 0x10, 0xae, 0x43, 0x90,
-		0x84, 0xf0, 0x32, 0x01, 0x21,
-		0xd8, 0x11, 0x40, 0x01, 0x00,
-		0xa0, 0x60, 0x3c, 0x00, 0x02,
-		0xa0, 0x20, 0x40, 0x10, 0x00,
-		0xa0, 0x20, 0x50, 0x14, 0x00,
-		0xd8, 0x12, 0x00, 0x00, 0x18,
-		0xa0, 0x00, 0x28, 0x0c, 0x00,
-		0xb0, 0x00, 0x48, 0x10, 0x00,
-		0xb0, 0x00, 0x58, 0x14, 0x00,
-		0xaa, 0xf0, 0x00, 0x14, 0x01,
-		0xd8, 0x1a, 0xc0, 0x01, 0x05,
-		0x85, 0x80, 0x42, 0x01, 0xff,
-		0x95, 0x00, 0x66, 0x01, 0xff,
-		0xba, 0xc0, 0x60, 0x1b, 0x01,
-		0x9a, 0x30, 0x60, 0x19, 0x30,
-		0x9a, 0xb0, 0x70, 0x1a, 0x30,
-		0x9b, 0x50, 0x78, 0x1e, 0x04,
-		0x8a, 0xe2, 0x08, 0x1e, 0x21,
-		0x6a, 0x4e, 0x00, 0x13, 0x00,
-		0xe0, 0x00, 0x00, 0x00, 0x30,
-		0x9b, 0x00, 0x7a, 0x92, 0x04,
-		0x3d, 0x00, 0x04, 0x1f, 0x20,
-		0x84, 0xe2, 0x02, 0x01, 0x21,
-		0xd8, 0x16, 0x80, 0x01, 0x00,
-		0xa4, 0x18, 0x0c, 0x7d, 0x80,
-		0xa0, 0x58, 0x1c, 0x00, 0x01,
-		0x01, 0x42, 0x00, 0xa0, 0x02,
-		0xe0, 0x00, 0x00, 0x00, 0x08,
-		0xfc, 0x10, 0xae, 0x63, 0x98,
-		0xd8, 0x1b, 0x00, 0xc2, 0x14,
-		0x84, 0xe2, 0x02, 0x01, 0x21,
-		0xd8, 0x05, 0xc0, 0x01, 0x00,
-		0x84, 0xa2, 0x02, 0x01, 0x21,
-		0xd8, 0x19, 0x40, 0x01, 0x01,
-		0xa0, 0x58, 0x0c, 0x00, 0x02,
-		0x1a, 0x40, 0x00, 0x04, 0x24,
-		0x33, 0x00, 0x01, 0x2f, 0x20,
-		0xd8, 0x05, 0xc0, 0x00, 0x18,
-		0xa0, 0x58, 0x0c, 0x00, 0x02,
-		0x1a, 0x40, 0x00, 0x04, 0x20,
-		0xd8, 0x05, 0xc0, 0x00, 0x18,
-		0xe0, 0x00, 0x02, 0x00, 0x00,
-		0xe0, 0x00, 0x02, 0x00, 0x00,
-		0xe0, 0x00, 0x02, 0x00, 0x00,
-		0xe0, 0x00, 0x02, 0x00, 0x00,
-		0xe0, 0x00, 0x02, 0x00, 0x00,
-	}
-};
diff --git a/drivers/net/ethernet/xscale/ixp2000/ixp2400_tx.uc b/drivers/net/ethernet/xscale/ixp2000/ixp2400_tx.uc
deleted file mode 100644
index d090d1884fb..00000000000
--- a/drivers/net/ethernet/xscale/ixp2000/ixp2400_tx.uc
+++ /dev/null
@@ -1,272 +0,0 @@
-/*
- * TX ucode for the Intel IXP2400 in POS-PHY mode.
- * Copyright (C) 2004, 2005 Lennert Buytenhek
- * Dedicated to Marija Kulikova.
- *
- * 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.
- *
- * Assumptions made in this code:
- * - The IXP2400 MSF is configured for POS-PHY mode, in a mode where
- *   only one TBUF partition is used.  This includes, for example,
- *   1x32 SPHY and 1x32 MPHY32, but not 4x8 SPHY or 1x32 MPHY4. (This
- *   is not an exhaustive list.)
- * - The TBUF uses 64-byte mpackets.
- * - TX descriptors reside in SRAM, and have the following format:
- *	struct tx_desc
- *	{
- *	// to uengine
- *		u32	buf_phys_addr;
- *		u32	pkt_length;
- *		u32	channel;
- *	};
- * - Packet data resides in DRAM.
- * - Packet buffer addresses are 8-byte aligned.
- * - Scratch ring 2 is tx_pending.
- * - Scratch ring 3 is tx_done, and has status condition 'full'.
- * - This code is run on all eight threads of the microengine it runs on.
- */
-
-#define TX_SEQUENCE_0		0x0060
-#define TBUF_CTRL		0x1800
-
-#define PARTITION_SIZE		128
-#define PARTITION_THRESH	96
-
-
-	.sig volatile sig1
-	.sig volatile sig2
-	.sig volatile sig3
-
-	.reg @old_tx_seq_0
-	.reg @mpkts_in_flight
-	.reg @next_tbuf_mpacket
-
-	.reg @buffer_handle
-	.reg @buffer_start
-	.reg @packet_length
-	.reg @channel
-	.reg @packet_offset
-
-	.reg zero
-
-	immed[zero, 0]
-
-	/*
-	 * Skip context 0 initialisation?
-	 */
-	.begin
-		br!=ctx[0, mpacket_tx_loop#]
-	.end
-
-	/*
-	 * Wait until all pending TBUF elements have been transmitted.
-	 */
-	.begin
-		.reg read $tx
-		.sig zzz
-
-	loop_empty#:
-		msf[read, $tx, zero, TX_SEQUENCE_0, 1], ctx_swap[zzz]
-		alu_shf[--, --, b, $tx, >>31]
-		beq[loop_empty#]
-
-		alu[@old_tx_seq_0, --, b, $tx]
-	.end
-
-	immed[@mpkts_in_flight, 0]
-	alu[@next_tbuf_mpacket, @old_tx_seq_0, and, (PARTITION_SIZE - 1)]
-
-	immed[@buffer_handle, 0]
-
-	/*
-	 * Initialise signal pipeline.
-	 */
-	.begin
-		local_csr_wr[SAME_ME_SIGNAL, (&sig1 << 3)]
-		.set_sig sig1
-
-		local_csr_wr[SAME_ME_SIGNAL, (&sig2 << 3)]
-		.set_sig sig2
-
-		local_csr_wr[SAME_ME_SIGNAL, (&sig3 << 3)]
-		.set_sig sig3
-	.end
-
-mpacket_tx_loop#:
-	.begin
-		.reg tbuf_element_index
-		.reg buffer_handle
-		.reg sop_eop
-		.reg packet_data
-		.reg channel
-		.reg mpacket_size
-
-		/*
-		 * If there is no packet currently being transmitted,
-		 * dequeue the next TX descriptor, and fetch the buffer
-		 * address, packet length and destination channel number.
-		 */
-		.begin
-			.reg read $stemp $stemp2 $stemp3
-			.xfer_order $stemp $stemp2 $stemp3
-			.sig zzz
-
-			ctx_arb[sig1]
-
-			alu[--, --, b, @buffer_handle]
-			bne[already_got_packet#]
-
-		tx_nobufs#:
-			scratch[get, $stemp, zero, 8, 1], ctx_swap[zzz]
-			alu[@buffer_handle, --, b, $stemp]
-			beq[tx_nobufs#]
-
-			sram[read, $stemp, $stemp, 0, 3], ctx_swap[zzz]
-			alu[@buffer_start, --, b, $stemp]
-			alu[@packet_length, --, b, $stemp2]
-			beq[zero_byte_packet#]
-			alu[@channel, --, b, $stemp3]
-			immed[@packet_offset, 0]
-
-		already_got_packet#:
-			local_csr_wr[SAME_ME_SIGNAL, (0x80 | (&sig1 << 3))]
-		.end
-
-		/*
-		 * Determine tbuf element index, SOP/EOP flags, mpacket
-		 * offset and mpacket size and cache buffer_handle and
-		 * channel number.
-		 */
-		.begin
-			alu[tbuf_element_index, --, b, @next_tbuf_mpacket]
-			alu[@next_tbuf_mpacket, @next_tbuf_mpacket, +, 1]
-			alu[@next_tbuf_mpacket, @next_tbuf_mpacket, and,
-							(PARTITION_SIZE - 1)]
-
-			alu[buffer_handle, --, b, @buffer_handle]
-			immed[@buffer_handle, 0]
-
-			immed[sop_eop, 1]
-
-			alu[packet_data, --, b, @packet_offset]
-			bne[no_sop#]
-			alu[sop_eop, sop_eop, or, 2]
-		no_sop#:
-			alu[packet_data, packet_data, +, @buffer_start]
-
-			alu[channel, --, b, @channel]
-
-			alu[mpacket_size, @packet_length, -, @packet_offset]
-			alu[--, 64, -, mpacket_size]
-			bhs[eop#]
-			alu[@buffer_handle, --, b, buffer_handle]
-			immed[mpacket_size, 64]
-			alu[sop_eop, sop_eop, and, 2]
-		eop#:
-
-			alu[@packet_offset, @packet_offset, +, mpacket_size]
-		.end
-
-		/*
-		 * Wait until there's enough space in the TBUF.
-		 */
-		.begin
-			.reg read $tx
-			.reg temp
-			.sig zzz
-
-			ctx_arb[sig2]
-
-			br[test_space#]
-
-		loop_space#:
-			msf[read, $tx, zero, TX_SEQUENCE_0, 1], ctx_swap[zzz]
-
-			alu[temp, $tx, -, @old_tx_seq_0]
-			alu[temp, temp, and, 0xff]
-			alu[@mpkts_in_flight, @mpkts_in_flight, -, temp]
-
-			alu[@old_tx_seq_0, --, b, $tx]
-
-		test_space#:
-			alu[--, PARTITION_THRESH, -, @mpkts_in_flight]
-			blo[loop_space#]
-
-			alu[@mpkts_in_flight, @mpkts_in_flight, +, 1]
-
-			local_csr_wr[SAME_ME_SIGNAL, (0x80 | (&sig2 << 3))]
-		.end
-
-		/*
-		 * Copy the packet data to the TBUF.
-		 */
-		.begin
-			.reg temp
-			.sig copy_sig
-
-			alu[temp, mpacket_size, -, 1]
-			alu_shf[temp, 0x10, or, temp, >>3]
-			alu_shf[temp, 0x10, or, temp, <<21]
-			alu_shf[temp, temp, or, tbuf_element_index, <<11]
-			alu_shf[--, temp, or, 1, <<18]
-
-			dram[tbuf_wr, --, packet_data, 0, max_8],
-					indirect_ref, sig_done[copy_sig]
-			ctx_arb[copy_sig]
-		.end
-
-		/*
-		 * Mark TBUF element as ready-to-be-transmitted.
-		 */
-		.begin
-			.reg write $tsw $tsw2
-			.xfer_order $tsw $tsw2
-			.reg temp
-			.sig zzz
-
-			alu_shf[temp, channel, or, mpacket_size, <<24]
-			alu_shf[$tsw, temp, or, sop_eop, <<8]
-			immed[$tsw2, 0]
-
-			immed[temp, TBUF_CTRL]
-			alu_shf[temp, temp, or, tbuf_element_index, <<3]
-			msf[write, $tsw, temp, 0, 2], ctx_swap[zzz]
-		.end
-
-		/*
-		 * Resynchronise.
-		 */
-		.begin
-			ctx_arb[sig3]
-			local_csr_wr[SAME_ME_SIGNAL, (0x80 | (&sig3 << 3))]
-		.end
-
-		/*
-		 * If this was an EOP mpacket, recycle the TX buffer
-	 	 * and signal the host.
-		 */
-		.begin
-			.reg write $stemp
-			.sig zzz
-
-			alu[--, sop_eop, and, 1]
-			beq[mpacket_tx_loop#]
-
-		tx_done_ring_full#:
-			br_inp_state[SCR_Ring3_Status, tx_done_ring_full#]
-
-			alu[$stemp, --, b, buffer_handle]
-			scratch[put, $stemp, zero, 12, 1], ctx_swap[zzz]
-			cap[fast_wr, 0, XSCALE_INT_A]
-			br[mpacket_tx_loop#]
-		.end
-	.end
-
-
-zero_byte_packet#:
-	halt
-
-
diff --git a/drivers/net/ethernet/xscale/ixp2000/ixp2400_tx.ucode b/drivers/net/ethernet/xscale/ixp2000/ixp2400_tx.ucode
deleted file mode 100644
index a433e24b0a5..00000000000
--- a/drivers/net/ethernet/xscale/ixp2000/ixp2400_tx.ucode
+++ /dev/null
@@ -1,98 +0,0 @@
-static struct ixp2000_uengine_code ixp2400_tx =
-{
-	.cpu_model_bitmask	= 0x000003fe,
-	.cpu_min_revision	= 0,
-	.cpu_max_revision	= 255,
-
-	.uengine_parameters	= IXP2000_UENGINE_8_CONTEXTS |
-				  IXP2000_UENGINE_PRN_UPDATE_EVERY |
-				  IXP2000_UENGINE_NN_FROM_PREVIOUS |
-				  IXP2000_UENGINE_ASSERT_EMPTY_AT_0 |
-				  IXP2000_UENGINE_LM_ADDR1_PER_CONTEXT |
-				  IXP2000_UENGINE_LM_ADDR0_PER_CONTEXT,
-
-	.initial_reg_values	= (struct ixp2000_reg_value []) {
-		{ -1, -1 }
-	},
-
-	.num_insns		= 77,
-	.insns			= (u8 []) {
-		0xf0, 0x00, 0x00, 0x07, 0x00,
-		0xd8, 0x03, 0x00, 0x00, 0x11,
-		0x3c, 0x40, 0x00, 0x04, 0xe0,
-		0x81, 0xf2, 0x02, 0x01, 0x00,
-		0xd8, 0x00, 0x80, 0x01, 0x00,
-		0xb0, 0x08, 0x06, 0x00, 0x00,
-		0xf0, 0x00, 0x0c, 0x00, 0x80,
-		0xb4, 0x49, 0x02, 0x03, 0x7f,
-		0xf0, 0x00, 0x02, 0x83, 0x00,
-		0xfc, 0x10, 0xac, 0x23, 0x08,
-		0xfc, 0x10, 0xac, 0x43, 0x10,
-		0xfc, 0x10, 0xac, 0x63, 0x18,
-		0xe0, 0x00, 0x00, 0x00, 0x02,
-		0xa0, 0x30, 0x02, 0x80, 0x00,
-		0xd8, 0x06, 0x00, 0x01, 0x01,
-		0x19, 0x40, 0x00, 0x04, 0x28,
-		0xb0, 0x0a, 0x06, 0x00, 0x00,
-		0xd8, 0x03, 0xc0, 0x01, 0x00,
-		0x00, 0x44, 0x00, 0x80, 0x80,
-		0xa0, 0x09, 0x06, 0x00, 0x00,
-		0xb0, 0x0b, 0x06, 0x04, 0x00,
-		0xd8, 0x13, 0x00, 0x01, 0x00,
-		0xb0, 0x0c, 0x06, 0x08, 0x00,
-		0xf0, 0x00, 0x0c, 0x00, 0xa0,
-		0xfc, 0x10, 0xae, 0x23, 0x88,
-		0xa0, 0x00, 0x12, 0x40, 0x00,
-		0xb0, 0xc9, 0x02, 0x43, 0x01,
-		0xb4, 0x49, 0x02, 0x43, 0x7f,
-		0xb0, 0x00, 0x22, 0x80, 0x00,
-		0xf0, 0x00, 0x02, 0x83, 0x00,
-		0xf0, 0x00, 0x0c, 0x04, 0x02,
-		0xb0, 0x40, 0x6c, 0x00, 0xa0,
-		0xd8, 0x08, 0x80, 0x01, 0x01,
-		0xaa, 0x00, 0x2c, 0x08, 0x02,
-		0xa0, 0xc0, 0x30, 0x18, 0x90,
-		0xa0, 0x00, 0x43, 0x00, 0x00,
-		0xba, 0xc0, 0x32, 0xc0, 0xa0,
-		0xaa, 0xb0, 0x00, 0x0f, 0x40,
-		0xd8, 0x0a, 0x80, 0x01, 0x04,
-		0xb0, 0x0a, 0x00, 0x08, 0x00,
-		0xf0, 0x00, 0x00, 0x0f, 0x40,
-		0xa4, 0x00, 0x2c, 0x08, 0x02,
-		0xa0, 0x8a, 0x00, 0x0c, 0xa0,
-		0xe0, 0x00, 0x00, 0x00, 0x04,
-		0xd8, 0x0c, 0x80, 0x00, 0x18,
-		0x3c, 0x40, 0x00, 0x04, 0xe0,
-		0xba, 0x80, 0x42, 0x01, 0x80,
-		0xb4, 0x40, 0x40, 0x13, 0xff,
-		0xaa, 0x88, 0x00, 0x10, 0x80,
-		0xb0, 0x08, 0x06, 0x00, 0x00,
-		0xaa, 0xf0, 0x0d, 0x80, 0x80,
-		0xd8, 0x0b, 0x40, 0x01, 0x05,
-		0xa0, 0x88, 0x0c, 0x04, 0x80,
-		0xfc, 0x10, 0xae, 0x43, 0x90,
-		0xba, 0xc0, 0x50, 0x0f, 0x01,
-		0x9a, 0x30, 0x50, 0x15, 0x30,
-		0x9a, 0xb0, 0x50, 0x16, 0x30,
-		0x9b, 0x50, 0x58, 0x16, 0x01,
-		0x8a, 0xe2, 0x08, 0x16, 0x21,
-		0x6b, 0x4e, 0x00, 0x83, 0x03,
-		0xe0, 0x00, 0x00, 0x00, 0x30,
-		0x9a, 0x80, 0x70, 0x0e, 0x04,
-		0x8b, 0x88, 0x08, 0x1e, 0x02,
-		0xf0, 0x00, 0x0c, 0x01, 0x81,
-		0xf0, 0x01, 0x80, 0x1f, 0x00,
-		0x9b, 0xd0, 0x78, 0x1e, 0x01,
-		0x3d, 0x42, 0x00, 0x1c, 0x20,
-		0xe0, 0x00, 0x00, 0x00, 0x08,
-		0xfc, 0x10, 0xae, 0x63, 0x98,
-		0xa4, 0x30, 0x0c, 0x04, 0x02,
-		0xd8, 0x03, 0x00, 0x01, 0x00,
-		0xd8, 0x11, 0xc1, 0x42, 0x14,
-		0xa0, 0x18, 0x00, 0x08, 0x00,
-		0x1a, 0x40, 0x00, 0x04, 0x2c,
-		0x33, 0x00, 0x01, 0x2f, 0x20,
-		0xd8, 0x03, 0x00, 0x00, 0x18,
-		0xe0, 0x00, 0x02, 0x00, 0x00,
-	}
-};
diff --git a/drivers/net/ethernet/xscale/ixp2000/ixpdev.c b/drivers/net/ethernet/xscale/ixp2000/ixpdev.c
deleted file mode 100644
index 45008377c8b..00000000000
--- a/drivers/net/ethernet/xscale/ixp2000/ixpdev.c
+++ /dev/null
@@ -1,437 +0,0 @@
-/*
- * IXP2000 MSF network device driver
- * Copyright (C) 2004, 2005 Lennert Buytenhek <buytenh@wantstofly.org>
- * Dedicated to Marija Kulikova.
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation; either version 2 of the License, or
- * (at your option) any later version.
- */
-
-#include <linux/module.h>
-#include <linux/kernel.h>
-#include <linux/netdevice.h>
-#include <linux/etherdevice.h>
-#include <linux/init.h>
-#include <linux/interrupt.h>
-#include <linux/moduleparam.h>
-#include <linux/gfp.h>
-#include <asm/hardware/uengine.h>
-#include <asm/io.h>
-#include "ixp2400_rx.ucode"
-#include "ixp2400_tx.ucode"
-#include "ixpdev_priv.h"
-#include "ixpdev.h"
-#include "pm3386.h"
-
-#define DRV_MODULE_VERSION	"0.2"
-
-static int nds_count;
-static struct net_device **nds;
-static int nds_open;
-static void (*set_port_admin_status)(int port, int up);
-
-static struct ixpdev_rx_desc * const rx_desc =
-	(struct ixpdev_rx_desc *)(IXP2000_SRAM0_VIRT_BASE + RX_BUF_DESC_BASE);
-static struct ixpdev_tx_desc * const tx_desc =
-	(struct ixpdev_tx_desc *)(IXP2000_SRAM0_VIRT_BASE + TX_BUF_DESC_BASE);
-static int tx_pointer;
-
-
-static int ixpdev_xmit(struct sk_buff *skb, struct net_device *dev)
-{
-	struct ixpdev_priv *ip = netdev_priv(dev);
-	struct ixpdev_tx_desc *desc;
-	int entry;
-	unsigned long flags;
-
-	if (unlikely(skb->len > PAGE_SIZE)) {
-		/* @@@ Count drops.  */
-		dev_kfree_skb(skb);
-		return NETDEV_TX_OK;
-	}
-
-	entry = tx_pointer;
-	tx_pointer = (tx_pointer + 1) % TX_BUF_COUNT;
-
-	desc = tx_desc + entry;
-	desc->pkt_length = skb->len;
-	desc->channel = ip->channel;
-
-	skb_copy_and_csum_dev(skb, phys_to_virt(desc->buf_addr));
-	dev_kfree_skb(skb);
-
-	ixp2000_reg_write(RING_TX_PENDING,
-		TX_BUF_DESC_BASE + (entry * sizeof(struct ixpdev_tx_desc)));
-
-	local_irq_save(flags);
-	ip->tx_queue_entries++;
-	if (ip->tx_queue_entries == TX_BUF_COUNT_PER_CHAN)
-		netif_stop_queue(dev);
-	local_irq_restore(flags);
-
-	return NETDEV_TX_OK;
-}
-
-
-static int ixpdev_rx(struct net_device *dev, int processed, int budget)
-{
-	while (processed < budget) {
-		struct ixpdev_rx_desc *desc;
-		struct sk_buff *skb;
-		void *buf;
-		u32 _desc;
-
-		_desc = ixp2000_reg_read(RING_RX_DONE);
-		if (_desc == 0)
-			return 0;
-
-		desc = rx_desc +
-			((_desc - RX_BUF_DESC_BASE) / sizeof(struct ixpdev_rx_desc));
-		buf = phys_to_virt(desc->buf_addr);
-
-		if (desc->pkt_length < 4 || desc->pkt_length > PAGE_SIZE) {
-			printk(KERN_ERR "ixp2000: rx err, length %d\n",
-					desc->pkt_length);
-			goto err;
-		}
-
-		if (desc->channel < 0 || desc->channel >= nds_count) {
-			printk(KERN_ERR "ixp2000: rx err, channel %d\n",
-					desc->channel);
-			goto err;
-		}
-
-		/* @@@ Make FCS stripping configurable.  */
-		desc->pkt_length -= 4;
-
-		if (unlikely(!netif_running(nds[desc->channel])))
-			goto err;
-
-		skb = netdev_alloc_skb_ip_align(dev, desc->pkt_length);
-		if (likely(skb != NULL)) {
-			skb_copy_to_linear_data(skb, buf, desc->pkt_length);
-			skb_put(skb, desc->pkt_length);
-			skb->protocol = eth_type_trans(skb, nds[desc->channel]);
-
-			netif_receive_skb(skb);
-		}
-
-err:
-		ixp2000_reg_write(RING_RX_PENDING, _desc);
-		processed++;
-	}
-
-	return processed;
-}
-
-/* dev always points to nds[0].  */
-static int ixpdev_poll(struct napi_struct *napi, int budget)
-{
-	struct ixpdev_priv *ip = container_of(napi, struct ixpdev_priv, napi);
-	struct net_device *dev = ip->dev;
-	int rx;
-
-	rx = 0;
-	do {
-		ixp2000_reg_write(IXP2000_IRQ_THD_RAW_STATUS_A_0, 0x00ff);
-
-		rx = ixpdev_rx(dev, rx, budget);
-		if (rx >= budget)
-			break;
-	} while (ixp2000_reg_read(IXP2000_IRQ_THD_RAW_STATUS_A_0) & 0x00ff);
-
-	napi_complete(napi);
-	ixp2000_reg_write(IXP2000_IRQ_THD_ENABLE_SET_A_0, 0x00ff);
-
-	return rx;
-}
-
-static void ixpdev_tx_complete(void)
-{
-	int channel;
-	u32 wake;
-
-	wake = 0;
-	while (1) {
-		struct ixpdev_priv *ip;
-		u32 desc;
-		int entry;
-
-		desc = ixp2000_reg_read(RING_TX_DONE);
-		if (desc == 0)
-			break;
-
-		/* @@@ Check whether entries come back in order.  */
-		entry = (desc - TX_BUF_DESC_BASE) / sizeof(struct ixpdev_tx_desc);
-		channel = tx_desc[entry].channel;
-
-		if (channel < 0 || channel >= nds_count) {
-			printk(KERN_ERR "ixp2000: txcomp channel index "
-					"out of bounds (%d, %.8i, %d)\n",
-					channel, (unsigned int)desc, entry);
-			continue;
-		}
-
-		ip = netdev_priv(nds[channel]);
-		if (ip->tx_queue_entries == TX_BUF_COUNT_PER_CHAN)
-			wake |= 1 << channel;
-		ip->tx_queue_entries--;
-	}
-
-	for (channel = 0; wake != 0; channel++) {
-		if (wake & (1 << channel)) {
-			netif_wake_queue(nds[channel]);
-			wake &= ~(1 << channel);
-		}
-	}
-}
-
-static irqreturn_t ixpdev_interrupt(int irq, void *dev_id)
-{
-	u32 status;
-
-	status = ixp2000_reg_read(IXP2000_IRQ_THD_STATUS_A_0);
-	if (status == 0)
-		return IRQ_NONE;
-
-	/*
-	 * Any of the eight receive units signaled RX?
-	 */
-	if (status & 0x00ff) {
-		struct net_device *dev = nds[0];
-		struct ixpdev_priv *ip = netdev_priv(dev);
-
-		ixp2000_reg_wrb(IXP2000_IRQ_THD_ENABLE_CLEAR_A_0, 0x00ff);
-		if (likely(napi_schedule_prep(&ip->napi))) {
-			__napi_schedule(&ip->napi);
-		} else {
-			printk(KERN_CRIT "ixp2000: irq while polling!!\n");
-		}
-	}
-
-	/*
-	 * Any of the eight transmit units signaled TXdone?
-	 */
-	if (status & 0xff00) {
-		ixp2000_reg_wrb(IXP2000_IRQ_THD_RAW_STATUS_A_0, 0xff00);
-		ixpdev_tx_complete();
-	}
-
-	return IRQ_HANDLED;
-}
-
-#ifdef CONFIG_NET_POLL_CONTROLLER
-static void ixpdev_poll_controller(struct net_device *dev)
-{
-	disable_irq(IRQ_IXP2000_THDA0);
-	ixpdev_interrupt(IRQ_IXP2000_THDA0, dev);
-	enable_irq(IRQ_IXP2000_THDA0);
-}
-#endif
-
-static int ixpdev_open(struct net_device *dev)
-{
-	struct ixpdev_priv *ip = netdev_priv(dev);
-	int err;
-
-	napi_enable(&ip->napi);
-	if (!nds_open++) {
-		err = request_irq(IRQ_IXP2000_THDA0, ixpdev_interrupt,
-					IRQF_SHARED, "ixp2000_eth", nds);
-		if (err) {
-			nds_open--;
-			napi_disable(&ip->napi);
-			return err;
-		}
-
-		ixp2000_reg_write(IXP2000_IRQ_THD_ENABLE_SET_A_0, 0xffff);
-	}
-
-	set_port_admin_status(ip->channel, 1);
-	netif_start_queue(dev);
-
-	return 0;
-}
-
-static int ixpdev_close(struct net_device *dev)
-{
-	struct ixpdev_priv *ip = netdev_priv(dev);
-
-	netif_stop_queue(dev);
-	napi_disable(&ip->napi);
-	set_port_admin_status(ip->channel, 0);
-
-	if (!--nds_open) {
-		ixp2000_reg_write(IXP2000_IRQ_THD_ENABLE_CLEAR_A_0, 0xffff);
-		free_irq(IRQ_IXP2000_THDA0, nds);
-	}
-
-	return 0;
-}
-
-static struct net_device_stats *ixpdev_get_stats(struct net_device *dev)
-{
-	struct ixpdev_priv *ip = netdev_priv(dev);
-
-	pm3386_get_stats(ip->channel, &(dev->stats));
-
-	return &(dev->stats);
-}
-
-static const struct net_device_ops ixpdev_netdev_ops = {
-	.ndo_open		= ixpdev_open,
-	.ndo_stop		= ixpdev_close,
-	.ndo_start_xmit		= ixpdev_xmit,
-	.ndo_change_mtu		= eth_change_mtu,
-	.ndo_validate_addr	= eth_validate_addr,
-	.ndo_set_mac_address	= eth_mac_addr,
-	.ndo_get_stats		= ixpdev_get_stats,
-#ifdef CONFIG_NET_POLL_CONTROLLER
-	.ndo_poll_controller	= ixpdev_poll_controller,
-#endif
-};
-
-struct net_device *ixpdev_alloc(int channel, int sizeof_priv)
-{
-	struct net_device *dev;
-	struct ixpdev_priv *ip;
-
-	dev = alloc_etherdev(sizeof_priv);
-	if (dev == NULL)
-		return NULL;
-
-	dev->netdev_ops = &ixpdev_netdev_ops;
-
-	dev->features |= NETIF_F_SG | NETIF_F_HW_CSUM;
-
-	ip = netdev_priv(dev);
-	ip->dev = dev;
-	netif_napi_add(dev, &ip->napi, ixpdev_poll, 64);
-	ip->channel = channel;
-	ip->tx_queue_entries = 0;
-
-	return dev;
-}
-
-int ixpdev_init(int __nds_count, struct net_device **__nds,
-		void (*__set_port_admin_status)(int port, int up))
-{
-	int i;
-	int err;
-
-	BUILD_BUG_ON(RX_BUF_COUNT > 192 || TX_BUF_COUNT > 192);
-
-	printk(KERN_INFO "IXP2000 MSF ethernet driver %s\n", DRV_MODULE_VERSION);
-
-	nds_count = __nds_count;
-	nds = __nds;
-	set_port_admin_status = __set_port_admin_status;
-
-	for (i = 0; i < RX_BUF_COUNT; i++) {
-		void *buf;
-
-		buf = (void *)get_zeroed_page(GFP_KERNEL);
-		if (buf == NULL) {
-			err = -ENOMEM;
-			while (--i >= 0)
-				free_page((unsigned long)phys_to_virt(rx_desc[i].buf_addr));
-			goto err_out;
-		}
-		rx_desc[i].buf_addr = virt_to_phys(buf);
-		rx_desc[i].buf_length = PAGE_SIZE;
-	}
-
-	/* @@@ Maybe we shouldn't be preallocating TX buffers.  */
-	for (i = 0; i < TX_BUF_COUNT; i++) {
-		void *buf;
-
-		buf = (void *)get_zeroed_page(GFP_KERNEL);
-		if (buf == NULL) {
-			err = -ENOMEM;
-			while (--i >= 0)
-				free_page((unsigned long)phys_to_virt(tx_desc[i].buf_addr));
-			goto err_free_rx;
-		}
-		tx_desc[i].buf_addr = virt_to_phys(buf);
-	}
-
-	/* 256 entries, ring status set means 'empty', base address 0x0000.  */
-	ixp2000_reg_write(RING_RX_PENDING_BASE, 0x44000000);
-	ixp2000_reg_write(RING_RX_PENDING_HEAD, 0x00000000);
-	ixp2000_reg_write(RING_RX_PENDING_TAIL, 0x00000000);
-
-	/* 256 entries, ring status set means 'full', base address 0x0400.  */
-	ixp2000_reg_write(RING_RX_DONE_BASE, 0x40000400);
-	ixp2000_reg_write(RING_RX_DONE_HEAD, 0x00000000);
-	ixp2000_reg_write(RING_RX_DONE_TAIL, 0x00000000);
-
-	for (i = 0; i < RX_BUF_COUNT; i++) {
-		ixp2000_reg_write(RING_RX_PENDING,
-			RX_BUF_DESC_BASE + (i * sizeof(struct ixpdev_rx_desc)));
-	}
-
-	ixp2000_uengine_load(0, &ixp2400_rx);
-	ixp2000_uengine_start_contexts(0, 0xff);
-
-	/* 256 entries, ring status set means 'empty', base address 0x0800.  */
-	ixp2000_reg_write(RING_TX_PENDING_BASE, 0x44000800);
-	ixp2000_reg_write(RING_TX_PENDING_HEAD, 0x00000000);
-	ixp2000_reg_write(RING_TX_PENDING_TAIL, 0x00000000);
-
-	/* 256 entries, ring status set means 'full', base address 0x0c00.  */
-	ixp2000_reg_write(RING_TX_DONE_BASE, 0x40000c00);
-	ixp2000_reg_write(RING_TX_DONE_HEAD, 0x00000000);
-	ixp2000_reg_write(RING_TX_DONE_TAIL, 0x00000000);
-
-	ixp2000_uengine_load(1, &ixp2400_tx);
-	ixp2000_uengine_start_contexts(1, 0xff);
-
-	for (i = 0; i < nds_count; i++) {
-		err = register_netdev(nds[i]);
-		if (err) {
-			while (--i >= 0)
-				unregister_netdev(nds[i]);
-			goto err_free_tx;
-		}
-	}
-
-	for (i = 0; i < nds_count; i++) {
-		printk(KERN_INFO "%s: IXP2000 MSF ethernet (port %d), %pM.\n",
-				 nds[i]->name, i, nds[i]->dev_addr);
-	}
-
-	return 0;
-
-err_free_tx:
-	for (i = 0; i < TX_BUF_COUNT; i++)
-		free_page((unsigned long)phys_to_virt(tx_desc[i].buf_addr));
-
-err_free_rx:
-	for (i = 0; i < RX_BUF_COUNT; i++)
-		free_page((unsigned long)phys_to_virt(rx_desc[i].buf_addr));
-
-err_out:
-	return err;
-} 
-
-void ixpdev_deinit(void)
-{
-	int i;
-
-	/* @@@ Flush out pending packets.  */
-
-	for (i = 0; i < nds_count; i++)
-		unregister_netdev(nds[i]);
-
-	ixp2000_uengine_stop_contexts(1, 0xff);
-	ixp2000_uengine_stop_contexts(0, 0xff);
-	ixp2000_uengine_reset(0x3);
-
-	for (i = 0; i < TX_BUF_COUNT; i++)
-		free_page((unsigned long)phys_to_virt(tx_desc[i].buf_addr));
-
-	for (i = 0; i < RX_BUF_COUNT; i++)
-		free_page((unsigned long)phys_to_virt(rx_desc[i].buf_addr));
-}
diff --git a/drivers/net/ethernet/xscale/ixp2000/ixpdev.h b/drivers/net/ethernet/xscale/ixp2000/ixpdev.h
deleted file mode 100644
index 391ece62324..00000000000
--- a/drivers/net/ethernet/xscale/ixp2000/ixpdev.h
+++ /dev/null
@@ -1,29 +0,0 @@
-/*
- * IXP2000 MSF network device driver
- * Copyright (C) 2004, 2005 Lennert Buytenhek <buytenh@wantstofly.org>
- * Dedicated to Marija Kulikova.
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation; either version 2 of the License, or
- * (at your option) any later version.
- */
-
-#ifndef __IXPDEV_H
-#define __IXPDEV_H
-
-struct ixpdev_priv
-{
-	struct net_device *dev;
-	struct napi_struct napi;
-	int	channel;
-	int	tx_queue_entries;
-};
-
-struct net_device *ixpdev_alloc(int channel, int sizeof_priv);
-int ixpdev_init(int num_ports, struct net_device **nds,
-		void (*set_port_admin_status)(int port, int up));
-void ixpdev_deinit(void);
-
-
-#endif
diff --git a/drivers/net/ethernet/xscale/ixp2000/ixpdev_priv.h b/drivers/net/ethernet/xscale/ixp2000/ixpdev_priv.h
deleted file mode 100644
index 86aa08ea0c3..00000000000
--- a/drivers/net/ethernet/xscale/ixp2000/ixpdev_priv.h
+++ /dev/null
@@ -1,57 +0,0 @@
-/*
- * IXP2000 MSF network device driver
- * Copyright (C) 2004, 2005 Lennert Buytenhek <buytenh@wantstofly.org>
- * Dedicated to Marija Kulikova.
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation; either version 2 of the License, or
- * (at your option) any later version.
- */
-
-#ifndef __IXPDEV_PRIV_H
-#define __IXPDEV_PRIV_H
-
-#define RX_BUF_DESC_BASE	0x00001000
-#define RX_BUF_COUNT		((3 * PAGE_SIZE) / (4 * sizeof(struct ixpdev_rx_desc)))
-#define TX_BUF_DESC_BASE	0x00002000
-#define TX_BUF_COUNT		((3 * PAGE_SIZE) / (4 * sizeof(struct ixpdev_tx_desc)))
-#define TX_BUF_COUNT_PER_CHAN	(TX_BUF_COUNT / 4)
-
-#define RING_RX_PENDING		((u32 *)IXP2000_SCRATCH_RING_VIRT_BASE)
-#define RING_RX_DONE		((u32 *)(IXP2000_SCRATCH_RING_VIRT_BASE + 4))
-#define RING_TX_PENDING		((u32 *)(IXP2000_SCRATCH_RING_VIRT_BASE + 8))
-#define RING_TX_DONE		((u32 *)(IXP2000_SCRATCH_RING_VIRT_BASE + 12))
-
-#define SCRATCH_REG(x)		((u32 *)(IXP2000_GLOBAL_REG_VIRT_BASE | 0x0800 | (x)))
-#define RING_RX_PENDING_BASE	SCRATCH_REG(0x00)
-#define RING_RX_PENDING_HEAD	SCRATCH_REG(0x04)
-#define RING_RX_PENDING_TAIL	SCRATCH_REG(0x08)
-#define RING_RX_DONE_BASE	SCRATCH_REG(0x10)
-#define RING_RX_DONE_HEAD	SCRATCH_REG(0x14)
-#define RING_RX_DONE_TAIL	SCRATCH_REG(0x18)
-#define RING_TX_PENDING_BASE	SCRATCH_REG(0x20)
-#define RING_TX_PENDING_HEAD	SCRATCH_REG(0x24)
-#define RING_TX_PENDING_TAIL	SCRATCH_REG(0x28)
-#define RING_TX_DONE_BASE	SCRATCH_REG(0x30)
-#define RING_TX_DONE_HEAD	SCRATCH_REG(0x34)
-#define RING_TX_DONE_TAIL	SCRATCH_REG(0x38)
-
-struct ixpdev_rx_desc
-{
-	u32	buf_addr;
-	u32	buf_length;
-	u32	channel;
-	u32	pkt_length;
-};
-
-struct ixpdev_tx_desc
-{
-	u32	buf_addr;
-	u32	pkt_length;
-	u32	channel;
-	u32	unused;
-};
-
-
-#endif
diff --git a/drivers/net/ethernet/xscale/ixp2000/pm3386.c b/drivers/net/ethernet/xscale/ixp2000/pm3386.c
deleted file mode 100644
index e08d3f9863b..00000000000
--- a/drivers/net/ethernet/xscale/ixp2000/pm3386.c
+++ /dev/null
@@ -1,351 +0,0 @@
-/*
- * Helper functions for the PM3386s on the Radisys ENP2611
- * Copyright (C) 2004, 2005 Lennert Buytenhek <buytenh@wantstofly.org>
- * Dedicated to Marija Kulikova.
- *
- * 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/delay.h>
-#include <linux/netdevice.h>
-#include <asm/io.h>
-#include "pm3386.h"
-
-/*
- * Read from register 'reg' of PM3386 device 'pm'.
- */
-static u16 pm3386_reg_read(int pm, int reg)
-{
-	void *_reg;
-	u16 value;
-
-	_reg = (void *)ENP2611_PM3386_0_VIRT_BASE;
-	if (pm == 1)
-		_reg = (void *)ENP2611_PM3386_1_VIRT_BASE;
-
-	value = *((volatile u16 *)(_reg + (reg << 1)));
-
-//	printk(KERN_INFO "pm3386_reg_read(%d, %.3x) = %.8x\n", pm, reg, value);
-
-	return value;
-}
-
-/*
- * Write to register 'reg' of PM3386 device 'pm', and perform
- * a readback from the identification register.
- */
-static void pm3386_reg_write(int pm, int reg, u16 value)
-{
-	void *_reg;
-	u16 dummy;
-
-//	printk(KERN_INFO "pm3386_reg_write(%d, %.3x, %.8x)\n", pm, reg, value);
-
-	_reg = (void *)ENP2611_PM3386_0_VIRT_BASE;
-	if (pm == 1)
-		_reg = (void *)ENP2611_PM3386_1_VIRT_BASE;
-
-	*((volatile u16 *)(_reg + (reg << 1))) = value;
-
-	dummy = *((volatile u16 *)_reg);
-	__asm__ __volatile__("mov %0, %0" : "+r" (dummy));
-}
-
-/*
- * Read from port 'port' register 'reg', where the registers
- * for the different ports are 'spacing' registers apart.
- */
-static u16 pm3386_port_reg_read(int port, int _reg, int spacing)
-{
-	int reg;
-
-	reg = _reg;
-	if (port & 1)
-		reg += spacing;
-
-	return pm3386_reg_read(port >> 1, reg);
-}
-
-/*
- * Write to port 'port' register 'reg', where the registers
- * for the different ports are 'spacing' registers apart.
- */
-static void pm3386_port_reg_write(int port, int _reg, int spacing, u16 value)
-{
-	int reg;
-
-	reg = _reg;
-	if (port & 1)
-		reg += spacing;
-
-	pm3386_reg_write(port >> 1, reg, value);
-}
-
-int pm3386_secondary_present(void)
-{
-	return pm3386_reg_read(1, 0) == 0x3386;
-}
-
-void pm3386_reset(void)
-{
-	u8 mac[3][6];
-	int secondary;
-
-	secondary = pm3386_secondary_present();
-
-	/* Save programmed MAC addresses.  */
-	pm3386_get_mac(0, mac[0]);
-	pm3386_get_mac(1, mac[1]);
-	if (secondary)
-		pm3386_get_mac(2, mac[2]);
-
-	/* Assert analog and digital reset.  */
-	pm3386_reg_write(0, 0x002, 0x0060);
-	if (secondary)
-		pm3386_reg_write(1, 0x002, 0x0060);
-	mdelay(1);
-
-	/* Deassert analog reset.  */
-	pm3386_reg_write(0, 0x002, 0x0062);
-	if (secondary)
-		pm3386_reg_write(1, 0x002, 0x0062);
-	mdelay(10);
-
-	/* Deassert digital reset.  */
-	pm3386_reg_write(0, 0x002, 0x0063);
-	if (secondary)
-		pm3386_reg_write(1, 0x002, 0x0063);
-	mdelay(10);
-
-	/* Restore programmed MAC addresses.  */
-	pm3386_set_mac(0, mac[0]);
-	pm3386_set_mac(1, mac[1]);
-	if (secondary)
-		pm3386_set_mac(2, mac[2]);
-
-	/* Disable carrier on all ports.  */
-	pm3386_set_carrier(0, 0);
-	pm3386_set_carrier(1, 0);
-	if (secondary)
-		pm3386_set_carrier(2, 0);
-}
-
-static u16 swaph(u16 x)
-{
-	return ((x << 8) | (x >> 8)) & 0xffff;
-}
-
-int pm3386_port_count(void)
-{
-	return 2 + pm3386_secondary_present();
-}
-
-void pm3386_init_port(int port)
-{
-	int pm = port >> 1;
-
-	/*
-	 * Work around ENP2611 bootloader programming MAC address
-	 * in reverse.
-	 */
-	if (pm3386_port_reg_read(port, 0x30a, 0x100) == 0x0000 &&
-	    (pm3386_port_reg_read(port, 0x309, 0x100) & 0xff00) == 0x5000) {
-		u16 temp[3];
-
-		temp[0] = pm3386_port_reg_read(port, 0x308, 0x100);
-		temp[1] = pm3386_port_reg_read(port, 0x309, 0x100);
-		temp[2] = pm3386_port_reg_read(port, 0x30a, 0x100);
-		pm3386_port_reg_write(port, 0x308, 0x100, swaph(temp[2]));
-		pm3386_port_reg_write(port, 0x309, 0x100, swaph(temp[1]));
-		pm3386_port_reg_write(port, 0x30a, 0x100, swaph(temp[0]));
-	}
-
-	/*
-	 * Initialise narrowbanding mode.  See application note 2010486
-	 * for more information.  (@@@ We also need to issue a reset
-	 * when ROOL or DOOL are detected.)
-	 */
-	pm3386_port_reg_write(port, 0x708, 0x10, 0xd055);
-	udelay(500);
-	pm3386_port_reg_write(port, 0x708, 0x10, 0x5055);
-
-	/*
-	 * SPI-3 ingress block.  Set 64 bytes SPI-3 burst size
-	 * towards SPI-3 bridge.
-	 */
-	pm3386_port_reg_write(port, 0x122, 0x20, 0x0002);
-
-	/*
-	 * Enable ingress protocol checking, and soft reset the
-	 * SPI-3 ingress block.
-	 */
-	pm3386_reg_write(pm, 0x103, 0x0003);
-	while (!(pm3386_reg_read(pm, 0x103) & 0x80))
-		;
-
-	/*
-	 * SPI-3 egress block.  Gather 12288 bytes of the current
-	 * packet in the TX fifo before initiating transmit on the
-	 * SERDES interface.  (Prevents TX underflows.)
-	 */
-	pm3386_port_reg_write(port, 0x221, 0x20, 0x0007);
-
-	/*
-	 * Enforce odd parity from the SPI-3 bridge, and soft reset
-	 * the SPI-3 egress block.
-	 */
-	pm3386_reg_write(pm, 0x203, 0x000d & ~(4 << (port & 1)));
-	while ((pm3386_reg_read(pm, 0x203) & 0x000c) != 0x000c)
-		;
-
-	/*
-	 * EGMAC block.  Set this channels to reject long preambles,
-	 * not send or transmit PAUSE frames, enable preamble checking,
-	 * disable frame length checking, enable FCS appending, enable
-	 * TX frame padding.
-	 */
-	pm3386_port_reg_write(port, 0x302, 0x100, 0x0113);
-
-	/*
-	 * Soft reset the EGMAC block.
-	 */
-	pm3386_port_reg_write(port, 0x301, 0x100, 0x8000);
-	pm3386_port_reg_write(port, 0x301, 0x100, 0x0000);
-
-	/*
-	 * Auto-sense autonegotiation status.
-	 */
-	pm3386_port_reg_write(port, 0x306, 0x100, 0x0100);
-
-	/*
-	 * Allow reception of jumbo frames.
-	 */
-	pm3386_port_reg_write(port, 0x310, 0x100, 9018);
-
-	/*
-	 * Allow transmission of jumbo frames.
-	 */
-	pm3386_port_reg_write(port, 0x336, 0x100, 9018);
-
-	/* @@@ Should set 0x337/0x437 (RX forwarding threshold.)  */
-
-	/*
-	 * Set autonegotiation parameters to 'no PAUSE, full duplex.'
-	 */
-	pm3386_port_reg_write(port, 0x31c, 0x100, 0x0020);
-
-	/*
-	 * Enable and restart autonegotiation.
-	 */
-	pm3386_port_reg_write(port, 0x318, 0x100, 0x0003);
-	pm3386_port_reg_write(port, 0x318, 0x100, 0x0002);
-}
-
-void pm3386_get_mac(int port, u8 *mac)
-{
-	u16 temp;
-
-	temp = pm3386_port_reg_read(port, 0x308, 0x100);
-	mac[0] = temp & 0xff;
-	mac[1] = (temp >> 8) & 0xff;
-
-	temp = pm3386_port_reg_read(port, 0x309, 0x100);
-	mac[2] = temp & 0xff;
-	mac[3] = (temp >> 8) & 0xff;
-
-	temp = pm3386_port_reg_read(port, 0x30a, 0x100);
-	mac[4] = temp & 0xff;
-	mac[5] = (temp >> 8) & 0xff;
-}
-
-void pm3386_set_mac(int port, u8 *mac)
-{
-	pm3386_port_reg_write(port, 0x308, 0x100, (mac[1] << 8) | mac[0]);
-	pm3386_port_reg_write(port, 0x309, 0x100, (mac[3] << 8) | mac[2]);
-	pm3386_port_reg_write(port, 0x30a, 0x100, (mac[5] << 8) | mac[4]);
-}
-
-static u32 pm3386_get_stat(int port, u16 base)
-{
-	u32 value;
-
-	value = pm3386_port_reg_read(port, base, 0x100);
-	value |= pm3386_port_reg_read(port, base + 1, 0x100) << 16;
-
-	return value;
-}
-
-void pm3386_get_stats(int port, struct net_device_stats *stats)
-{
-	/*
-	 * Snapshot statistics counters.
-	 */
-	pm3386_port_reg_write(port, 0x500, 0x100, 0x0001);
-	while (pm3386_port_reg_read(port, 0x500, 0x100) & 0x0001)
-		;
-
-	memset(stats, 0, sizeof(*stats));
-
-	stats->rx_packets = pm3386_get_stat(port, 0x510);
-	stats->tx_packets = pm3386_get_stat(port, 0x590);
-	stats->rx_bytes = pm3386_get_stat(port, 0x514);
-	stats->tx_bytes = pm3386_get_stat(port, 0x594);
-	/* @@@ Add other stats.  */
-}
-
-void pm3386_set_carrier(int port, int state)
-{
-	pm3386_port_reg_write(port, 0x703, 0x10, state ? 0x1001 : 0x0000);
-}
-
-int pm3386_is_link_up(int port)
-{
-	u16 temp;
-
-	temp = pm3386_port_reg_read(port, 0x31a, 0x100);
-	temp = pm3386_port_reg_read(port, 0x31a, 0x100);
-
-	return !!(temp & 0x0002);
-}
-
-void pm3386_enable_rx(int port)
-{
-	u16 temp;
-
-	temp = pm3386_port_reg_read(port, 0x303, 0x100);
-	temp |= 0x1000;
-	pm3386_port_reg_write(port, 0x303, 0x100, temp);
-}
-
-void pm3386_disable_rx(int port)
-{
-	u16 temp;
-
-	temp = pm3386_port_reg_read(port, 0x303, 0x100);
-	temp &= 0xefff;
-	pm3386_port_reg_write(port, 0x303, 0x100, temp);
-}
-
-void pm3386_enable_tx(int port)
-{
-	u16 temp;
-
-	temp = pm3386_port_reg_read(port, 0x303, 0x100);
-	temp |= 0x4000;
-	pm3386_port_reg_write(port, 0x303, 0x100, temp);
-}
-
-void pm3386_disable_tx(int port)
-{
-	u16 temp;
-
-	temp = pm3386_port_reg_read(port, 0x303, 0x100);
-	temp &= 0xbfff;
-	pm3386_port_reg_write(port, 0x303, 0x100, temp);
-}
-
-MODULE_LICENSE("GPL");
diff --git a/drivers/net/ethernet/xscale/ixp2000/pm3386.h b/drivers/net/ethernet/xscale/ixp2000/pm3386.h
deleted file mode 100644
index cc4183dca91..00000000000
--- a/drivers/net/ethernet/xscale/ixp2000/pm3386.h
+++ /dev/null
@@ -1,29 +0,0 @@
-/*
- * Helper functions for the PM3386s on the Radisys ENP2611
- * Copyright (C) 2004, 2005 Lennert Buytenhek <buytenh@wantstofly.org>
- * Dedicated to Marija Kulikova.
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation; either version 2 of the License, or
- * (at your option) any later version.
- */
-
-#ifndef __PM3386_H
-#define __PM3386_H
-
-void pm3386_reset(void);
-int pm3386_port_count(void);
-void pm3386_init_port(int port);
-void pm3386_get_mac(int port, u8 *mac);
-void pm3386_set_mac(int port, u8 *mac);
-void pm3386_get_stats(int port, struct net_device_stats *stats);
-void pm3386_set_carrier(int port, int state);
-int pm3386_is_link_up(int port);
-void pm3386_enable_rx(int port);
-void pm3386_disable_rx(int port);
-void pm3386_enable_tx(int port);
-void pm3386_disable_tx(int port);
-
-
-#endif
diff --git a/drivers/net/ethernet/xscale/ixp4xx_eth.c b/drivers/net/ethernet/xscale/ixp4xx_eth.c
index 41a8b5a9849..482648fcf0b 100644
--- a/drivers/net/ethernet/xscale/ixp4xx_eth.c
+++ b/drivers/net/ethernet/xscale/ixp4xx_eth.c
@@ -1002,12 +1002,41 @@ static int ixp4xx_nway_reset(struct net_device *dev)
 	return phy_start_aneg(port->phydev);
 }
 
+int ixp46x_phc_index = -1;
+
+static int ixp4xx_get_ts_info(struct net_device *dev,
+			      struct ethtool_ts_info *info)
+{
+	if (!cpu_is_ixp46x()) {
+		info->so_timestamping =
+			SOF_TIMESTAMPING_TX_SOFTWARE |
+			SOF_TIMESTAMPING_RX_SOFTWARE |
+			SOF_TIMESTAMPING_SOFTWARE;
+		info->phc_index = -1;
+		return 0;
+	}
+	info->so_timestamping =
+		SOF_TIMESTAMPING_TX_HARDWARE |
+		SOF_TIMESTAMPING_RX_HARDWARE |
+		SOF_TIMESTAMPING_RAW_HARDWARE;
+	info->phc_index = ixp46x_phc_index;
+	info->tx_types =
+		(1 << HWTSTAMP_TX_OFF) |
+		(1 << HWTSTAMP_TX_ON);
+	info->rx_filters =
+		(1 << HWTSTAMP_FILTER_NONE) |
+		(1 << HWTSTAMP_FILTER_PTP_V1_L4_SYNC) |
+		(1 << HWTSTAMP_FILTER_PTP_V1_L4_DELAY_REQ);
+	return 0;
+}
+
 static const struct ethtool_ops ixp4xx_ethtool_ops = {
 	.get_drvinfo = ixp4xx_get_drvinfo,
 	.get_settings = ixp4xx_get_settings,
 	.set_settings = ixp4xx_set_settings,
 	.nway_reset = ixp4xx_nway_reset,
 	.get_link = ethtool_op_get_link,
+	.get_ts_info = ixp4xx_get_ts_info,
 };
 
 
diff --git a/drivers/net/hippi/rrunner.c b/drivers/net/hippi/rrunner.c
index 168c8f41d09..d4719632ffc 100644
--- a/drivers/net/hippi/rrunner.c
+++ b/drivers/net/hippi/rrunner.c
@@ -113,10 +113,9 @@ static int __devinit rr_init_one(struct pci_dev *pdev,
 
 	SET_NETDEV_DEV(dev, &pdev->dev);
 
-	if (pci_request_regions(pdev, "rrunner")) {
-		ret = -EIO;
+	ret = pci_request_regions(pdev, "rrunner");
+	if (ret < 0)
 		goto out;
-	}
 
 	pci_set_drvdata(pdev, dev);
 
@@ -124,11 +123,8 @@ static int __devinit rr_init_one(struct pci_dev *pdev,
 
 	spin_lock_init(&rrpriv->lock);
 
-	dev->irq = pdev->irq;
 	dev->netdev_ops = &rr_netdev_ops;
 
-	dev->base_addr = pci_resource_start(pdev, 0);
-
 	/* display version info if adapter is found */
 	if (!version_disp) {
 		/* set display flag to TRUE so that */
@@ -146,16 +142,15 @@ static int __devinit rr_init_one(struct pci_dev *pdev,
 	pci_set_master(pdev);
 
 	printk(KERN_INFO "%s: Essential RoadRunner serial HIPPI "
-	       "at 0x%08lx, irq %i, PCI latency %i\n", dev->name,
-	       dev->base_addr, dev->irq, pci_latency);
+	       "at 0x%llx, irq %i, PCI latency %i\n", dev->name,
+	       (unsigned long long)pci_resource_start(pdev, 0),
+	       pdev->irq, pci_latency);
 
 	/*
-	 * Remap the regs into kernel space.
+	 * Remap the MMIO regs into kernel space.
 	 */
-
-	rrpriv->regs = ioremap(dev->base_addr, 0x1000);
-
-	if (!rrpriv->regs){
+	rrpriv->regs = pci_iomap(pdev, 0, 0x1000);
+	if (!rrpriv->regs) {
 		printk(KERN_ERR "%s:  Unable to map I/O register, "
 			"RoadRunner will be disabled.\n", dev->name);
 		ret = -EIO;
@@ -202,8 +197,6 @@ static int __devinit rr_init_one(struct pci_dev *pdev,
 
 	rr_init(dev);
 
-	dev->base_addr = 0;
-
 	ret = register_netdev(dev);
 	if (ret)
 		goto out;
@@ -217,7 +210,7 @@ static int __devinit rr_init_one(struct pci_dev *pdev,
 		pci_free_consistent(pdev, TX_TOTAL_SIZE, rrpriv->tx_ring,
 				    rrpriv->tx_ring_dma);
 	if (rrpriv->regs)
-		iounmap(rrpriv->regs);
+		pci_iounmap(pdev, rrpriv->regs);
 	if (pdev) {
 		pci_release_regions(pdev);
 		pci_set_drvdata(pdev, NULL);
@@ -231,29 +224,26 @@ static int __devinit rr_init_one(struct pci_dev *pdev,
 static void __devexit rr_remove_one (struct pci_dev *pdev)
 {
 	struct net_device *dev = pci_get_drvdata(pdev);
+	struct rr_private *rr = netdev_priv(dev);
 
-	if (dev) {
-		struct rr_private *rr = netdev_priv(dev);
-
-		if (!(readl(&rr->regs->HostCtrl) & NIC_HALTED)){
-			printk(KERN_ERR "%s: trying to unload running NIC\n",
-			       dev->name);
-			writel(HALT_NIC, &rr->regs->HostCtrl);
-		}
-
-		pci_free_consistent(pdev, EVT_RING_SIZE, rr->evt_ring,
-				    rr->evt_ring_dma);
-		pci_free_consistent(pdev, RX_TOTAL_SIZE, rr->rx_ring,
-				    rr->rx_ring_dma);
-		pci_free_consistent(pdev, TX_TOTAL_SIZE, rr->tx_ring,
-				    rr->tx_ring_dma);
-		unregister_netdev(dev);
-		iounmap(rr->regs);
-		free_netdev(dev);
-		pci_release_regions(pdev);
-		pci_disable_device(pdev);
-		pci_set_drvdata(pdev, NULL);
+	if (!(readl(&rr->regs->HostCtrl) & NIC_HALTED)) {
+		printk(KERN_ERR "%s: trying to unload running NIC\n",
+		       dev->name);
+		writel(HALT_NIC, &rr->regs->HostCtrl);
 	}
+
+	unregister_netdev(dev);
+	pci_free_consistent(pdev, EVT_RING_SIZE, rr->evt_ring,
+			    rr->evt_ring_dma);
+	pci_free_consistent(pdev, RX_TOTAL_SIZE, rr->rx_ring,
+			    rr->rx_ring_dma);
+	pci_free_consistent(pdev, TX_TOTAL_SIZE, rr->tx_ring,
+			    rr->tx_ring_dma);
+	pci_iounmap(pdev, rr->regs);
+	pci_release_regions(pdev);
+	pci_disable_device(pdev);
+	pci_set_drvdata(pdev, NULL);
+	free_netdev(dev);
 }
 
 
@@ -1229,9 +1219,9 @@ static int rr_open(struct net_device *dev)
 	readl(&regs->HostCtrl);
 	spin_unlock_irqrestore(&rrpriv->lock, flags);
 
-	if (request_irq(dev->irq, rr_interrupt, IRQF_SHARED, dev->name, dev)) {
+	if (request_irq(pdev->irq, rr_interrupt, IRQF_SHARED, dev->name, dev)) {
 		printk(KERN_WARNING "%s: Requested IRQ %d is busy\n",
-		       dev->name, dev->irq);
+		       dev->name, pdev->irq);
 		ecode = -EAGAIN;
 		goto error;
 	}
@@ -1338,16 +1328,15 @@ static void rr_dump(struct net_device *dev)
 
 static int rr_close(struct net_device *dev)
 {
-	struct rr_private *rrpriv;
-	struct rr_regs __iomem *regs;
+	struct rr_private *rrpriv = netdev_priv(dev);
+	struct rr_regs __iomem *regs = rrpriv->regs;
+	struct pci_dev *pdev = rrpriv->pci_dev;
 	unsigned long flags;
 	u32 tmp;
 	short i;
 
 	netif_stop_queue(dev);
 
-	rrpriv = netdev_priv(dev);
-	regs = rrpriv->regs;
 
 	/*
 	 * Lock to make sure we are not cleaning up while another CPU
@@ -1386,15 +1375,15 @@ static int rr_close(struct net_device *dev)
 	rr_raz_tx(rrpriv, dev);
 	rr_raz_rx(rrpriv, dev);
 
-	pci_free_consistent(rrpriv->pci_dev, 256 * sizeof(struct ring_ctrl),
+	pci_free_consistent(pdev, 256 * sizeof(struct ring_ctrl),
 			    rrpriv->rx_ctrl, rrpriv->rx_ctrl_dma);
 	rrpriv->rx_ctrl = NULL;
 
-	pci_free_consistent(rrpriv->pci_dev, sizeof(struct rr_info),
-			    rrpriv->info, rrpriv->info_dma);
+	pci_free_consistent(pdev, sizeof(struct rr_info), rrpriv->info,
+			    rrpriv->info_dma);
 	rrpriv->info = NULL;
 
-	free_irq(dev->irq, dev);
+	free_irq(pdev->irq, dev);
 	spin_unlock_irqrestore(&rrpriv->lock, flags);
 
 	return 0;
diff --git a/drivers/net/hyperv/hyperv_net.h b/drivers/net/hyperv/hyperv_net.h
index c3582455279..4ffcd57b011 100644
--- a/drivers/net/hyperv/hyperv_net.h
+++ b/drivers/net/hyperv/hyperv_net.h
@@ -27,6 +27,7 @@
 
 #include <linux/list.h>
 #include <linux/hyperv.h>
+#include <linux/rndis.h>
 
 /* Fwd declaration */
 struct hv_netvsc_packet;
@@ -506,295 +507,6 @@ struct netvsc_device {
 	void *extension;
 };
 
-
-/*  Status codes */
-
-
-#ifndef STATUS_SUCCESS
-#define STATUS_SUCCESS				(0x00000000L)
-#endif
-
-#ifndef STATUS_UNSUCCESSFUL
-#define STATUS_UNSUCCESSFUL			(0xC0000001L)
-#endif
-
-#ifndef STATUS_PENDING
-#define STATUS_PENDING				(0x00000103L)
-#endif
-
-#ifndef STATUS_INSUFFICIENT_RESOURCES
-#define STATUS_INSUFFICIENT_RESOURCES		(0xC000009AL)
-#endif
-
-#ifndef STATUS_BUFFER_OVERFLOW
-#define STATUS_BUFFER_OVERFLOW			(0x80000005L)
-#endif
-
-#ifndef STATUS_NOT_SUPPORTED
-#define STATUS_NOT_SUPPORTED			(0xC00000BBL)
-#endif
-
-#define RNDIS_STATUS_SUCCESS			(STATUS_SUCCESS)
-#define RNDIS_STATUS_PENDING			(STATUS_PENDING)
-#define RNDIS_STATUS_NOT_RECOGNIZED		(0x00010001L)
-#define RNDIS_STATUS_NOT_COPIED			(0x00010002L)
-#define RNDIS_STATUS_NOT_ACCEPTED		(0x00010003L)
-#define RNDIS_STATUS_CALL_ACTIVE		(0x00010007L)
-
-#define RNDIS_STATUS_ONLINE			(0x40010003L)
-#define RNDIS_STATUS_RESET_START		(0x40010004L)
-#define RNDIS_STATUS_RESET_END			(0x40010005L)
-#define RNDIS_STATUS_RING_STATUS		(0x40010006L)
-#define RNDIS_STATUS_CLOSED			(0x40010007L)
-#define RNDIS_STATUS_WAN_LINE_UP		(0x40010008L)
-#define RNDIS_STATUS_WAN_LINE_DOWN		(0x40010009L)
-#define RNDIS_STATUS_WAN_FRAGMENT		(0x4001000AL)
-#define RNDIS_STATUS_MEDIA_CONNECT		(0x4001000BL)
-#define RNDIS_STATUS_MEDIA_DISCONNECT		(0x4001000CL)
-#define RNDIS_STATUS_HARDWARE_LINE_UP		(0x4001000DL)
-#define RNDIS_STATUS_HARDWARE_LINE_DOWN		(0x4001000EL)
-#define RNDIS_STATUS_INTERFACE_UP		(0x4001000FL)
-#define RNDIS_STATUS_INTERFACE_DOWN		(0x40010010L)
-#define RNDIS_STATUS_MEDIA_BUSY			(0x40010011L)
-#define RNDIS_STATUS_MEDIA_SPECIFIC_INDICATION	(0x40010012L)
-#define RNDIS_STATUS_WW_INDICATION		RDIA_SPECIFIC_INDICATION
-#define RNDIS_STATUS_LINK_SPEED_CHANGE		(0x40010013L)
-
-#define RNDIS_STATUS_NOT_RESETTABLE		(0x80010001L)
-#define RNDIS_STATUS_SOFT_ERRORS		(0x80010003L)
-#define RNDIS_STATUS_HARD_ERRORS		(0x80010004L)
-#define RNDIS_STATUS_BUFFER_OVERFLOW		(STATUS_BUFFER_OVERFLOW)
-
-#define RNDIS_STATUS_FAILURE			(STATUS_UNSUCCESSFUL)
-#define RNDIS_STATUS_RESOURCES			(STATUS_INSUFFICIENT_RESOURCES)
-#define RNDIS_STATUS_CLOSING			(0xC0010002L)
-#define RNDIS_STATUS_BAD_VERSION		(0xC0010004L)
-#define RNDIS_STATUS_BAD_CHARACTERISTICS	(0xC0010005L)
-#define RNDIS_STATUS_ADAPTER_NOT_FOUND		(0xC0010006L)
-#define RNDIS_STATUS_OPEN_FAILED		(0xC0010007L)
-#define RNDIS_STATUS_DEVICE_FAILED		(0xC0010008L)
-#define RNDIS_STATUS_MULTICAST_FULL		(0xC0010009L)
-#define RNDIS_STATUS_MULTICAST_EXISTS		(0xC001000AL)
-#define RNDIS_STATUS_MULTICAST_NOT_FOUND	(0xC001000BL)
-#define RNDIS_STATUS_REQUEST_ABORTED		(0xC001000CL)
-#define RNDIS_STATUS_RESET_IN_PROGRESS		(0xC001000DL)
-#define RNDIS_STATUS_CLOSING_INDICATING		(0xC001000EL)
-#define RNDIS_STATUS_NOT_SUPPORTED		(STATUS_NOT_SUPPORTED)
-#define RNDIS_STATUS_INVALID_PACKET		(0xC001000FL)
-#define RNDIS_STATUS_OPEN_LIST_FULL		(0xC0010010L)
-#define RNDIS_STATUS_ADAPTER_NOT_READY		(0xC0010011L)
-#define RNDIS_STATUS_ADAPTER_NOT_OPEN		(0xC0010012L)
-#define RNDIS_STATUS_NOT_INDICATING		(0xC0010013L)
-#define RNDIS_STATUS_INVALID_LENGTH		(0xC0010014L)
-#define RNDIS_STATUS_INVALID_DATA		(0xC0010015L)
-#define RNDIS_STATUS_BUFFER_TOO_SHORT		(0xC0010016L)
-#define RNDIS_STATUS_INVALID_OID		(0xC0010017L)
-#define RNDIS_STATUS_ADAPTER_REMOVED		(0xC0010018L)
-#define RNDIS_STATUS_UNSUPPORTED_MEDIA		(0xC0010019L)
-#define RNDIS_STATUS_GROUP_ADDRESS_IN_USE	(0xC001001AL)
-#define RNDIS_STATUS_FILE_NOT_FOUND		(0xC001001BL)
-#define RNDIS_STATUS_ERROR_READING_FILE		(0xC001001CL)
-#define RNDIS_STATUS_ALREADY_MAPPED		(0xC001001DL)
-#define RNDIS_STATUS_RESOURCE_CONFLICT		(0xC001001EL)
-#define RNDIS_STATUS_NO_CABLE			(0xC001001FL)
-
-#define RNDIS_STATUS_INVALID_SAP		(0xC0010020L)
-#define RNDIS_STATUS_SAP_IN_USE			(0xC0010021L)
-#define RNDIS_STATUS_INVALID_ADDRESS		(0xC0010022L)
-#define RNDIS_STATUS_VC_NOT_ACTIVATED		(0xC0010023L)
-#define RNDIS_STATUS_DEST_OUT_OF_ORDER		(0xC0010024L)
-#define RNDIS_STATUS_VC_NOT_AVAILABLE		(0xC0010025L)
-#define RNDIS_STATUS_CELLRATE_NOT_AVAILABLE	(0xC0010026L)
-#define RNDIS_STATUS_INCOMPATABLE_QOS		(0xC0010027L)
-#define RNDIS_STATUS_AAL_PARAMS_UNSUPPORTED	(0xC0010028L)
-#define RNDIS_STATUS_NO_ROUTE_TO_DESTINATION	(0xC0010029L)
-
-#define RNDIS_STATUS_TOKEN_RING_OPEN_ERROR	(0xC0011000L)
-
-/* Object Identifiers used by NdisRequest Query/Set Information */
-/* General Objects */
-#define RNDIS_OID_GEN_SUPPORTED_LIST		0x00010101
-#define RNDIS_OID_GEN_HARDWARE_STATUS		0x00010102
-#define RNDIS_OID_GEN_MEDIA_SUPPORTED		0x00010103
-#define RNDIS_OID_GEN_MEDIA_IN_USE		0x00010104
-#define RNDIS_OID_GEN_MAXIMUM_LOOKAHEAD		0x00010105
-#define RNDIS_OID_GEN_MAXIMUM_FRAME_SIZE	0x00010106
-#define RNDIS_OID_GEN_LINK_SPEED		0x00010107
-#define RNDIS_OID_GEN_TRANSMIT_BUFFER_SPACE	0x00010108
-#define RNDIS_OID_GEN_RECEIVE_BUFFER_SPACE	0x00010109
-#define RNDIS_OID_GEN_TRANSMIT_BLOCK_SIZE	0x0001010A
-#define RNDIS_OID_GEN_RECEIVE_BLOCK_SIZE	0x0001010B
-#define RNDIS_OID_GEN_VENDOR_ID			0x0001010C
-#define RNDIS_OID_GEN_VENDOR_DESCRIPTION	0x0001010D
-#define RNDIS_OID_GEN_CURRENT_PACKET_FILTER	0x0001010E
-#define RNDIS_OID_GEN_CURRENT_LOOKAHEAD		0x0001010F
-#define RNDIS_OID_GEN_DRIVER_VERSION		0x00010110
-#define RNDIS_OID_GEN_MAXIMUM_TOTAL_SIZE	0x00010111
-#define RNDIS_OID_GEN_PROTOCOL_OPTIONS		0x00010112
-#define RNDIS_OID_GEN_MAC_OPTIONS		0x00010113
-#define RNDIS_OID_GEN_MEDIA_CONNECT_STATUS	0x00010114
-#define RNDIS_OID_GEN_MAXIMUM_SEND_PACKETS	0x00010115
-#define RNDIS_OID_GEN_VENDOR_DRIVER_VERSION	0x00010116
-#define RNDIS_OID_GEN_NETWORK_LAYER_ADDRESSES	0x00010118
-#define RNDIS_OID_GEN_TRANSPORT_HEADER_OFFSET	0x00010119
-#define RNDIS_OID_GEN_MACHINE_NAME		0x0001021A
-#define RNDIS_OID_GEN_RNDIS_CONFIG_PARAMETER	0x0001021B
-
-#define RNDIS_OID_GEN_XMIT_OK			0x00020101
-#define RNDIS_OID_GEN_RCV_OK			0x00020102
-#define RNDIS_OID_GEN_XMIT_ERROR		0x00020103
-#define RNDIS_OID_GEN_RCV_ERROR			0x00020104
-#define RNDIS_OID_GEN_RCV_NO_BUFFER		0x00020105
-
-#define RNDIS_OID_GEN_DIRECTED_BYTES_XMIT	0x00020201
-#define RNDIS_OID_GEN_DIRECTED_FRAMES_XMIT	0x00020202
-#define RNDIS_OID_GEN_MULTICAST_BYTES_XMIT	0x00020203
-#define RNDIS_OID_GEN_MULTICAST_FRAMES_XMIT	0x00020204
-#define RNDIS_OID_GEN_BROADCAST_BYTES_XMIT	0x00020205
-#define RNDIS_OID_GEN_BROADCAST_FRAMES_XMIT	0x00020206
-#define RNDIS_OID_GEN_DIRECTED_BYTES_RCV	0x00020207
-#define RNDIS_OID_GEN_DIRECTED_FRAMES_RCV	0x00020208
-#define RNDIS_OID_GEN_MULTICAST_BYTES_RCV	0x00020209
-#define RNDIS_OID_GEN_MULTICAST_FRAMES_RCV	0x0002020A
-#define RNDIS_OID_GEN_BROADCAST_BYTES_RCV	0x0002020B
-#define RNDIS_OID_GEN_BROADCAST_FRAMES_RCV	0x0002020C
-
-#define RNDIS_OID_GEN_RCV_CRC_ERROR		0x0002020D
-#define RNDIS_OID_GEN_TRANSMIT_QUEUE_LENGTH	0x0002020E
-
-#define RNDIS_OID_GEN_GET_TIME_CAPS		0x0002020F
-#define RNDIS_OID_GEN_GET_NETCARD_TIME		0x00020210
-
-/* These are connection-oriented general OIDs. */
-/* These replace the above OIDs for connection-oriented media. */
-#define RNDIS_OID_GEN_CO_SUPPORTED_LIST		0x00010101
-#define RNDIS_OID_GEN_CO_HARDWARE_STATUS	0x00010102
-#define RNDIS_OID_GEN_CO_MEDIA_SUPPORTED	0x00010103
-#define RNDIS_OID_GEN_CO_MEDIA_IN_USE		0x00010104
-#define RNDIS_OID_GEN_CO_LINK_SPEED		0x00010105
-#define RNDIS_OID_GEN_CO_VENDOR_ID		0x00010106
-#define RNDIS_OID_GEN_CO_VENDOR_DESCRIPTION	0x00010107
-#define RNDIS_OID_GEN_CO_DRIVER_VERSION		0x00010108
-#define RNDIS_OID_GEN_CO_PROTOCOL_OPTIONS	0x00010109
-#define RNDIS_OID_GEN_CO_MAC_OPTIONS		0x0001010A
-#define RNDIS_OID_GEN_CO_MEDIA_CONNECT_STATUS	0x0001010B
-#define RNDIS_OID_GEN_CO_VENDOR_DRIVER_VERSION	0x0001010C
-#define RNDIS_OID_GEN_CO_MINIMUM_LINK_SPEED	0x0001010D
-
-#define RNDIS_OID_GEN_CO_GET_TIME_CAPS		0x00010201
-#define RNDIS_OID_GEN_CO_GET_NETCARD_TIME	0x00010202
-
-/* These are connection-oriented statistics OIDs. */
-#define RNDIS_OID_GEN_CO_XMIT_PDUS_OK		0x00020101
-#define RNDIS_OID_GEN_CO_RCV_PDUS_OK		0x00020102
-#define RNDIS_OID_GEN_CO_XMIT_PDUS_ERROR	0x00020103
-#define RNDIS_OID_GEN_CO_RCV_PDUS_ERROR		0x00020104
-#define RNDIS_OID_GEN_CO_RCV_PDUS_NO_BUFFER	0x00020105
-
-
-#define RNDIS_OID_GEN_CO_RCV_CRC_ERROR		0x00020201
-#define RNDIS_OID_GEN_CO_TRANSMIT_QUEUE_LENGTH	0x00020202
-#define RNDIS_OID_GEN_CO_BYTES_XMIT		0x00020203
-#define RNDIS_OID_GEN_CO_BYTES_RCV		0x00020204
-#define RNDIS_OID_GEN_CO_BYTES_XMIT_OUTSTANDING	0x00020205
-#define RNDIS_OID_GEN_CO_NETCARD_LOAD		0x00020206
-
-/* These are objects for Connection-oriented media call-managers. */
-#define RNDIS_OID_CO_ADD_PVC			0xFF000001
-#define RNDIS_OID_CO_DELETE_PVC			0xFF000002
-#define RNDIS_OID_CO_GET_CALL_INFORMATION	0xFF000003
-#define RNDIS_OID_CO_ADD_ADDRESS		0xFF000004
-#define RNDIS_OID_CO_DELETE_ADDRESS		0xFF000005
-#define RNDIS_OID_CO_GET_ADDRESSES		0xFF000006
-#define RNDIS_OID_CO_ADDRESS_CHANGE		0xFF000007
-#define RNDIS_OID_CO_SIGNALING_ENABLED		0xFF000008
-#define RNDIS_OID_CO_SIGNALING_DISABLED		0xFF000009
-
-/* 802.3 Objects (Ethernet) */
-#define RNDIS_OID_802_3_PERMANENT_ADDRESS	0x01010101
-#define RNDIS_OID_802_3_CURRENT_ADDRESS		0x01010102
-#define RNDIS_OID_802_3_MULTICAST_LIST		0x01010103
-#define RNDIS_OID_802_3_MAXIMUM_LIST_SIZE	0x01010104
-#define RNDIS_OID_802_3_MAC_OPTIONS		0x01010105
-
-#define NDIS_802_3_MAC_OPTION_PRIORITY		0x00000001
-
-#define RNDIS_OID_802_3_RCV_ERROR_ALIGNMENT	0x01020101
-#define RNDIS_OID_802_3_XMIT_ONE_COLLISION	0x01020102
-#define RNDIS_OID_802_3_XMIT_MORE_COLLISIONS	0x01020103
-
-#define RNDIS_OID_802_3_XMIT_DEFERRED		0x01020201
-#define RNDIS_OID_802_3_XMIT_MAX_COLLISIONS	0x01020202
-#define RNDIS_OID_802_3_RCV_OVERRUN		0x01020203
-#define RNDIS_OID_802_3_XMIT_UNDERRUN		0x01020204
-#define RNDIS_OID_802_3_XMIT_HEARTBEAT_FAILURE	0x01020205
-#define RNDIS_OID_802_3_XMIT_TIMES_CRS_LOST	0x01020206
-#define RNDIS_OID_802_3_XMIT_LATE_COLLISIONS	0x01020207
-
-/* Remote NDIS message types */
-#define REMOTE_NDIS_PACKET_MSG			0x00000001
-#define REMOTE_NDIS_INITIALIZE_MSG		0x00000002
-#define REMOTE_NDIS_HALT_MSG			0x00000003
-#define REMOTE_NDIS_QUERY_MSG			0x00000004
-#define REMOTE_NDIS_SET_MSG			0x00000005
-#define REMOTE_NDIS_RESET_MSG			0x00000006
-#define REMOTE_NDIS_INDICATE_STATUS_MSG		0x00000007
-#define REMOTE_NDIS_KEEPALIVE_MSG		0x00000008
-
-#define REMOTE_CONDIS_MP_CREATE_VC_MSG		0x00008001
-#define REMOTE_CONDIS_MP_DELETE_VC_MSG		0x00008002
-#define REMOTE_CONDIS_MP_ACTIVATE_VC_MSG	0x00008005
-#define REMOTE_CONDIS_MP_DEACTIVATE_VC_MSG	0x00008006
-#define REMOTE_CONDIS_INDICATE_STATUS_MSG	0x00008007
-
-/* Remote NDIS message completion types */
-#define REMOTE_NDIS_INITIALIZE_CMPLT		0x80000002
-#define REMOTE_NDIS_QUERY_CMPLT			0x80000004
-#define REMOTE_NDIS_SET_CMPLT			0x80000005
-#define REMOTE_NDIS_RESET_CMPLT			0x80000006
-#define REMOTE_NDIS_KEEPALIVE_CMPLT		0x80000008
-
-#define REMOTE_CONDIS_MP_CREATE_VC_CMPLT	0x80008001
-#define REMOTE_CONDIS_MP_DELETE_VC_CMPLT	0x80008002
-#define REMOTE_CONDIS_MP_ACTIVATE_VC_CMPLT	0x80008005
-#define REMOTE_CONDIS_MP_DEACTIVATE_VC_CMPLT	0x80008006
-
-/*
- * Reserved message type for private communication between lower-layer host
- * driver and remote device, if necessary.
- */
-#define REMOTE_NDIS_BUS_MSG			0xff000001
-
-/*  Defines for DeviceFlags in struct rndis_initialize_complete */
-#define RNDIS_DF_CONNECTIONLESS			0x00000001
-#define RNDIS_DF_CONNECTION_ORIENTED		0x00000002
-#define RNDIS_DF_RAW_DATA			0x00000004
-
-/*  Remote NDIS medium types. */
-#define RNDIS_MEDIUM_802_3			0x00000000
-#define RNDIS_MEDIUM_802_5			0x00000001
-#define RNDIS_MEDIUM_FDDI				0x00000002
-#define RNDIS_MEDIUM_WAN				0x00000003
-#define RNDIS_MEDIUM_LOCAL_TALK			0x00000004
-#define RNDIS_MEDIUM_ARCNET_RAW			0x00000006
-#define RNDIS_MEDIUM_ARCNET_878_2			0x00000007
-#define RNDIS_MEDIUM_ATM				0x00000008
-#define RNDIS_MEDIUM_WIRELESS_WAN			0x00000009
-#define RNDIS_MEDIUM_IRDA				0x0000000a
-#define RNDIS_MEDIUM_CO_WAN			0x0000000b
-/* Not a real medium, defined as an upper-bound */
-#define RNDIS_MEDIUM_MAX				0x0000000d
-
-
-/* Remote NDIS medium connection states. */
-#define RNDIS_MEDIA_STATE_CONNECTED		0x00000000
-#define RNDIS_MEDIA_STATE_DISCONNECTED		0x00000001
-
-/*  Remote NDIS version numbers */
-#define RNDIS_MAJOR_VERSION			0x00000001
-#define RNDIS_MINOR_VERSION			0x00000000
-
-
 /* NdisInitialize message */
 struct rndis_initialize_request {
 	u32 req_id;
diff --git a/drivers/net/hyperv/netvsc.c b/drivers/net/hyperv/netvsc.c
index d025c83cd12..8b919471472 100644
--- a/drivers/net/hyperv/netvsc.c
+++ b/drivers/net/hyperv/netvsc.c
@@ -428,6 +428,24 @@ int netvsc_device_remove(struct hv_device *device)
 	return 0;
 }
 
+
+#define RING_AVAIL_PERCENT_HIWATER 20
+#define RING_AVAIL_PERCENT_LOWATER 10
+
+/*
+ * Get the percentage of available bytes to write in the ring.
+ * The return value is in range from 0 to 100.
+ */
+static inline u32 hv_ringbuf_avail_percent(
+		struct hv_ring_buffer_info *ring_info)
+{
+	u32 avail_read, avail_write;
+
+	hv_get_ringbuffer_availbytes(ring_info, &avail_read, &avail_write);
+
+	return avail_write * 100 / ring_info->ring_datasize;
+}
+
 static void netvsc_send_completion(struct hv_device *device,
 				   struct vmpacket_descriptor *packet)
 {
@@ -455,6 +473,8 @@ static void netvsc_send_completion(struct hv_device *device,
 		complete(&net_device->channel_init_wait);
 	} else if (nvsp_packet->hdr.msg_type ==
 		   NVSP_MSG1_TYPE_SEND_RNDIS_PKT_COMPLETE) {
+		int num_outstanding_sends;
+
 		/* Get the send context */
 		nvsc_packet = (struct hv_netvsc_packet *)(unsigned long)
 			packet->trans_id;
@@ -463,10 +483,14 @@ static void netvsc_send_completion(struct hv_device *device,
 		nvsc_packet->completion.send.send_completion(
 			nvsc_packet->completion.send.send_completion_ctx);
 
-		atomic_dec(&net_device->num_outstanding_sends);
+		num_outstanding_sends =
+			atomic_dec_return(&net_device->num_outstanding_sends);
 
-		if (netif_queue_stopped(ndev) && !net_device->start_remove)
-			netif_wake_queue(ndev);
+		if (netif_queue_stopped(ndev) && !net_device->start_remove &&
+			(hv_ringbuf_avail_percent(&device->channel->outbound)
+			> RING_AVAIL_PERCENT_HIWATER ||
+			num_outstanding_sends < 1))
+				netif_wake_queue(ndev);
 	} else {
 		netdev_err(ndev, "Unknown send completion packet type- "
 			   "%d received!!\n", nvsp_packet->hdr.msg_type);
@@ -519,10 +543,19 @@ int netvsc_send(struct hv_device *device,
 
 	if (ret == 0) {
 		atomic_inc(&net_device->num_outstanding_sends);
+		if (hv_ringbuf_avail_percent(&device->channel->outbound) <
+			RING_AVAIL_PERCENT_LOWATER) {
+			netif_stop_queue(ndev);
+			if (atomic_read(&net_device->
+				num_outstanding_sends) < 1)
+				netif_wake_queue(ndev);
+		}
 	} else if (ret == -EAGAIN) {
 		netif_stop_queue(ndev);
-		if (atomic_read(&net_device->num_outstanding_sends) < 1)
+		if (atomic_read(&net_device->num_outstanding_sends) < 1) {
 			netif_wake_queue(ndev);
+			ret = -ENOSPC;
+		}
 	} else {
 		netdev_err(ndev, "Unable to send packet %p ret %d\n",
 			   packet, ret);
diff --git a/drivers/net/hyperv/netvsc_drv.c b/drivers/net/hyperv/netvsc_drv.c
index 2d59138db7f..8f8ed332042 100644
--- a/drivers/net/hyperv/netvsc_drv.c
+++ b/drivers/net/hyperv/netvsc_drv.c
@@ -211,9 +211,13 @@ static int netvsc_start_xmit(struct sk_buff *skb, struct net_device *net)
 		net->stats.tx_packets++;
 	} else {
 		kfree(packet);
+		if (ret != -EAGAIN) {
+			dev_kfree_skb_any(skb);
+			net->stats.tx_dropped++;
+		}
 	}
 
-	return ret ? NETDEV_TX_BUSY : NETDEV_TX_OK;
+	return (ret == -EAGAIN) ? NETDEV_TX_BUSY : NETDEV_TX_OK;
 }
 
 /*
diff --git a/drivers/net/hyperv/rndis_filter.c b/drivers/net/hyperv/rndis_filter.c
index d6be64bcefd..981ebb11563 100644
--- a/drivers/net/hyperv/rndis_filter.c
+++ b/drivers/net/hyperv/rndis_filter.c
@@ -129,8 +129,8 @@ static void dump_rndis_message(struct hv_device *hv_dev,
 	netdev = net_device->ndev;
 
 	switch (rndis_msg->ndis_msg_type) {
-	case REMOTE_NDIS_PACKET_MSG:
-		netdev_dbg(netdev, "REMOTE_NDIS_PACKET_MSG (len %u, "
+	case RNDIS_MSG_PACKET:
+		netdev_dbg(netdev, "RNDIS_MSG_PACKET (len %u, "
 			   "data offset %u data len %u, # oob %u, "
 			   "oob offset %u, oob len %u, pkt offset %u, "
 			   "pkt len %u\n",
@@ -144,8 +144,8 @@ static void dump_rndis_message(struct hv_device *hv_dev,
 			   rndis_msg->msg.pkt.per_pkt_info_len);
 		break;
 
-	case REMOTE_NDIS_INITIALIZE_CMPLT:
-		netdev_dbg(netdev, "REMOTE_NDIS_INITIALIZE_CMPLT "
+	case RNDIS_MSG_INIT_C:
+		netdev_dbg(netdev, "RNDIS_MSG_INIT_C "
 			"(len %u, id 0x%x, status 0x%x, major %d, minor %d, "
 			"device flags %d, max xfer size 0x%x, max pkts %u, "
 			"pkt aligned %u)\n",
@@ -162,8 +162,8 @@ static void dump_rndis_message(struct hv_device *hv_dev,
 			   pkt_alignment_factor);
 		break;
 
-	case REMOTE_NDIS_QUERY_CMPLT:
-		netdev_dbg(netdev, "REMOTE_NDIS_QUERY_CMPLT "
+	case RNDIS_MSG_QUERY_C:
+		netdev_dbg(netdev, "RNDIS_MSG_QUERY_C "
 			"(len %u, id 0x%x, status 0x%x, buf len %u, "
 			"buf offset %u)\n",
 			rndis_msg->msg_len,
@@ -175,16 +175,16 @@ static void dump_rndis_message(struct hv_device *hv_dev,
 			   info_buf_offset);
 		break;
 
-	case REMOTE_NDIS_SET_CMPLT:
+	case RNDIS_MSG_SET_C:
 		netdev_dbg(netdev,
-			"REMOTE_NDIS_SET_CMPLT (len %u, id 0x%x, status 0x%x)\n",
+			"RNDIS_MSG_SET_C (len %u, id 0x%x, status 0x%x)\n",
 			rndis_msg->msg_len,
 			rndis_msg->msg.set_complete.req_id,
 			rndis_msg->msg.set_complete.status);
 		break;
 
-	case REMOTE_NDIS_INDICATE_STATUS_MSG:
-		netdev_dbg(netdev, "REMOTE_NDIS_INDICATE_STATUS_MSG "
+	case RNDIS_MSG_INDICATE:
+		netdev_dbg(netdev, "RNDIS_MSG_INDICATE "
 			"(len %u, status 0x%x, buf len %u, buf offset %u)\n",
 			rndis_msg->msg_len,
 			rndis_msg->msg.indicate_status.status,
@@ -264,14 +264,14 @@ static void rndis_filter_receive_response(struct rndis_device *dev,
 				sizeof(struct rndis_filter_packet));
 
 			if (resp->ndis_msg_type ==
-			    REMOTE_NDIS_RESET_CMPLT) {
+			    RNDIS_MSG_RESET_C) {
 				/* does not have a request id field */
 				request->response_msg.msg.reset_complete.
-					status = STATUS_BUFFER_OVERFLOW;
+					status = RNDIS_STATUS_BUFFER_OVERFLOW;
 			} else {
 				request->response_msg.msg.
 				init_complete.status =
-					STATUS_BUFFER_OVERFLOW;
+					RNDIS_STATUS_BUFFER_OVERFLOW;
 			}
 		}
 
@@ -415,19 +415,19 @@ int rndis_filter_receive(struct hv_device *dev,
 	dump_rndis_message(dev, rndis_msg);
 
 	switch (rndis_msg->ndis_msg_type) {
-	case REMOTE_NDIS_PACKET_MSG:
+	case RNDIS_MSG_PACKET:
 		/* data msg */
 		rndis_filter_receive_data(rndis_dev, rndis_msg, pkt);
 		break;
 
-	case REMOTE_NDIS_INITIALIZE_CMPLT:
-	case REMOTE_NDIS_QUERY_CMPLT:
-	case REMOTE_NDIS_SET_CMPLT:
+	case RNDIS_MSG_INIT_C:
+	case RNDIS_MSG_QUERY_C:
+	case RNDIS_MSG_SET_C:
 		/* completion msgs */
 		rndis_filter_receive_response(rndis_dev, rndis_msg);
 		break;
 
-	case REMOTE_NDIS_INDICATE_STATUS_MSG:
+	case RNDIS_MSG_INDICATE:
 		/* notification msgs */
 		rndis_filter_receive_indicate_status(rndis_dev, rndis_msg);
 		break;
@@ -456,7 +456,7 @@ static int rndis_filter_query_device(struct rndis_device *dev, u32 oid,
 		return -EINVAL;
 
 	*result_size = 0;
-	request = get_rndis_request(dev, REMOTE_NDIS_QUERY_MSG,
+	request = get_rndis_request(dev, RNDIS_MSG_QUERY,
 			RNDIS_MESSAGE_SIZE(struct rndis_query_request));
 	if (!request) {
 		ret = -ENOMEM;
@@ -536,7 +536,7 @@ int rndis_filter_set_packet_filter(struct rndis_device *dev, u32 new_filter)
 
 	ndev = dev->net_dev->ndev;
 
-	request = get_rndis_request(dev, REMOTE_NDIS_SET_MSG,
+	request = get_rndis_request(dev, RNDIS_MSG_SET,
 			RNDIS_MESSAGE_SIZE(struct rndis_set_request) +
 			sizeof(u32));
 	if (!request) {
@@ -588,7 +588,7 @@ static int rndis_filter_init_device(struct rndis_device *dev)
 	u32 status;
 	int ret, t;
 
-	request = get_rndis_request(dev, REMOTE_NDIS_INITIALIZE_MSG,
+	request = get_rndis_request(dev, RNDIS_MSG_INIT,
 			RNDIS_MESSAGE_SIZE(struct rndis_initialize_request));
 	if (!request) {
 		ret = -ENOMEM;
@@ -641,7 +641,7 @@ static void rndis_filter_halt_device(struct rndis_device *dev)
 	struct rndis_halt_request *halt;
 
 	/* Attempt to do a rndis device halt */
-	request = get_rndis_request(dev, REMOTE_NDIS_HALT_MSG,
+	request = get_rndis_request(dev, RNDIS_MSG_HALT,
 				RNDIS_MESSAGE_SIZE(struct rndis_halt_request));
 	if (!request)
 		goto cleanup;
@@ -805,7 +805,7 @@ int rndis_filter_send(struct hv_device *dev,
 	if (isvlan)
 		rndis_msg_size += NDIS_VLAN_PPI_SIZE;
 
-	rndis_msg->ndis_msg_type = REMOTE_NDIS_PACKET_MSG;
+	rndis_msg->ndis_msg_type = RNDIS_MSG_PACKET;
 	rndis_msg->msg_len = pkt->total_data_buflen +
 				      rndis_msg_size;
 
diff --git a/drivers/net/irda/Kconfig b/drivers/net/irda/Kconfig
index 468047866c8..595205406d7 100644
--- a/drivers/net/irda/Kconfig
+++ b/drivers/net/irda/Kconfig
@@ -211,8 +211,8 @@ config KINGSUN_DONGLE
 	  kingsun-sir.
 
 config EP7211_DONGLE
-	tristate "EP7211 I/R support"
-	depends on IRTTY_SIR && ARCH_EP7211 && IRDA && EXPERIMENTAL
+	tristate "Cirrus Logic clps711x I/R support"
+	depends on IRTTY_SIR && ARCH_CLPS711X && IRDA && EXPERIMENTAL
 	help
 	  Say Y here if you want to build support for the Cirrus logic
 	  EP7211 chipset's infrared module.
@@ -316,13 +316,13 @@ config AU1000_FIR
 	tristate "Alchemy IrDA SIR/FIR"
 	depends on IRDA && MIPS_ALCHEMY
 	help
-	  Say Y/M here to build suppor the the IrDA peripheral on the
+	  Say Y/M here to build support the IrDA peripheral on the
 	  Alchemy Au1000 and Au1100 SoCs.
 	  Say M to build a module; it will be called au1k_ir.ko
 
 config SMC_IRCC_FIR
-	tristate "SMSC IrCC (EXPERIMENTAL)"
-	depends on EXPERIMENTAL && IRDA && ISA_DMA_API
+	tristate "SMSC IrCC"
+	depends on IRDA && ISA_DMA_API
 	help
 	  Say Y here if you want to build support for the SMC Infrared
 	  Communications Controller.  It is used in a wide variety of
diff --git a/drivers/net/irda/donauboe.c b/drivers/net/irda/donauboe.c
index 4351296dde3..510b9c8d23a 100644
--- a/drivers/net/irda/donauboe.c
+++ b/drivers/net/irda/donauboe.c
@@ -1710,7 +1710,7 @@ toshoboe_gotosleep (struct pci_dev *pci_dev, pm_message_t crap)
 
 /* Flush all packets */
   while ((i--) && (self->txpending))
-    udelay (10000);
+    msleep(10);
 
   spin_lock_irqsave(&self->spinlock, flags);
 
diff --git a/drivers/net/irda/irda-usb.c b/drivers/net/irda/irda-usb.c
index 72f687b40d6..f9a86bdb12f 100644
--- a/drivers/net/irda/irda-usb.c
+++ b/drivers/net/irda/irda-usb.c
@@ -1671,7 +1671,7 @@ static int irda_usb_probe(struct usb_interface *intf,
 
 	/* Is this really necessary? (no, except maybe for broken devices) */
 	if (usb_reset_configuration (dev) < 0) {
-		err("reset_configuration failed");
+		dev_err(&intf->dev, "reset_configuration failed\n");
 		ret = -EIO;
 		goto err_out_3;
 	}
diff --git a/drivers/net/irda/kingsun-sir.c b/drivers/net/irda/kingsun-sir.c
index 79aebeee928..7b4833874ef 100644
--- a/drivers/net/irda/kingsun-sir.c
+++ b/drivers/net/irda/kingsun-sir.c
@@ -134,14 +134,16 @@ static void kingsun_send_irq(struct urb *urb)
 
 	/* in process of stopping, just drop data */
 	if (!netif_running(kingsun->netdev)) {
-		err("kingsun_send_irq: Network not running!");
+		dev_err(&kingsun->usbdev->dev,
+			"kingsun_send_irq: Network not running!\n");
 		return;
 	}
 
 	/* unlink, shutdown, unplug, other nasties */
 	if (urb->status != 0) {
-		err("kingsun_send_irq: urb asynchronously failed - %d",
-		    urb->status);
+		dev_err(&kingsun->usbdev->dev,
+			"kingsun_send_irq: urb asynchronously failed - %d\n",
+			urb->status);
 	}
 	netif_wake_queue(netdev);
 }
@@ -177,7 +179,8 @@ static netdev_tx_t kingsun_hard_xmit(struct sk_buff *skb,
 		kingsun, 1);
 
 	if ((ret = usb_submit_urb(kingsun->tx_urb, GFP_ATOMIC))) {
-		err("kingsun_hard_xmit: failed tx_urb submit: %d", ret);
+		dev_err(&kingsun->usbdev->dev,
+			"kingsun_hard_xmit: failed tx_urb submit: %d\n", ret);
 		switch (ret) {
 		case -ENODEV:
 		case -EPIPE:
@@ -211,8 +214,9 @@ static void kingsun_rcv_irq(struct urb *urb)
 
 	/* unlink, shutdown, unplug, other nasties */
 	if (urb->status != 0) {
-		err("kingsun_rcv_irq: urb asynchronously failed - %d",
-		    urb->status);
+		dev_err(&kingsun->usbdev->dev,
+			"kingsun_rcv_irq: urb asynchronously failed - %d\n",
+			urb->status);
 		kingsun->receiving = 0;
 		return;
 	}
@@ -238,8 +242,9 @@ static void kingsun_rcv_irq(struct urb *urb)
 				? 1 : 0;
 		}
 	} else if (urb->actual_length > 0) {
-		err("%s(): Unexpected response length, expected %d got %d",
-		    __func__, kingsun->max_rx, urb->actual_length);
+		dev_err(&kingsun->usbdev->dev,
+			"%s(): Unexpected response length, expected %d got %d\n",
+			__func__, kingsun->max_rx, urb->actual_length);
 	}
 	/* This urb has already been filled in kingsun_net_open */
 	ret = usb_submit_urb(urb, GFP_ATOMIC);
@@ -286,7 +291,7 @@ static int kingsun_net_open(struct net_device *netdev)
 	sprintf(hwname, "usb#%d", kingsun->usbdev->devnum);
 	kingsun->irlap = irlap_open(netdev, &kingsun->qos, hwname);
 	if (!kingsun->irlap) {
-		err("kingsun-sir: irlap_open failed");
+		dev_err(&kingsun->usbdev->dev, "irlap_open failed\n");
 		goto free_mem;
 	}
 
@@ -298,7 +303,8 @@ static int kingsun_net_open(struct net_device *netdev)
 	kingsun->rx_urb->status = 0;
 	err = usb_submit_urb(kingsun->rx_urb, GFP_KERNEL);
 	if (err) {
-		err("kingsun-sir: first urb-submit failed: %d", err);
+		dev_err(&kingsun->usbdev->dev,
+			"first urb-submit failed: %d\n", err);
 		goto close_irlap;
 	}
 
@@ -446,13 +452,15 @@ static int kingsun_probe(struct usb_interface *intf,
 	 */
 	interface = intf->cur_altsetting;
 	if (interface->desc.bNumEndpoints != 2) {
-		err("kingsun-sir: expected 2 endpoints, found %d",
-		    interface->desc.bNumEndpoints);
+		dev_err(&intf->dev,
+			"kingsun-sir: expected 2 endpoints, found %d\n",
+			interface->desc.bNumEndpoints);
 		return -ENODEV;
 	}
 	endpoint = &interface->endpoint[KINGSUN_EP_IN].desc;
 	if (!usb_endpoint_is_int_in(endpoint)) {
-		err("kingsun-sir: endpoint 0 is not interrupt IN");
+		dev_err(&intf->dev,
+			"kingsun-sir: endpoint 0 is not interrupt IN\n");
 		return -ENODEV;
 	}
 
@@ -460,14 +468,16 @@ static int kingsun_probe(struct usb_interface *intf,
 	pipe = usb_rcvintpipe(dev, ep_in);
 	maxp_in = usb_maxpacket(dev, pipe, usb_pipeout(pipe));
 	if (maxp_in > 255 || maxp_in <= 1) {
-		err("%s: endpoint 0 has max packet size %d not in range",
-		    __FILE__, maxp_in);
+		dev_err(&intf->dev,
+			"endpoint 0 has max packet size %d not in range\n",
+			maxp_in);
 		return -ENODEV;
 	}
 
 	endpoint = &interface->endpoint[KINGSUN_EP_OUT].desc;
 	if (!usb_endpoint_is_int_out(endpoint)) {
-		err("kingsun-sir: endpoint 1 is not interrupt OUT");
+		dev_err(&intf->dev,
+			"kingsun-sir: endpoint 1 is not interrupt OUT\n");
 		return -ENODEV;
 	}
 
diff --git a/drivers/net/irda/ks959-sir.c b/drivers/net/irda/ks959-sir.c
index abe689dffc7..824e2a93fe8 100644
--- a/drivers/net/irda/ks959-sir.c
+++ b/drivers/net/irda/ks959-sir.c
@@ -247,8 +247,9 @@ static void ks959_speed_irq(struct urb *urb)
 {
 	/* unlink, shutdown, unplug, other nasties */
 	if (urb->status != 0) {
-		err("ks959_speed_irq: urb asynchronously failed - %d",
-		    urb->status);
+		dev_err(&urb->dev->dev,
+			"ks959_speed_irq: urb asynchronously failed - %d\n",
+			urb->status);
 	}
 }
 
@@ -332,14 +333,16 @@ static void ks959_send_irq(struct urb *urb)
 
 	/* in process of stopping, just drop data */
 	if (!netif_running(kingsun->netdev)) {
-		err("ks959_send_irq: Network not running!");
+		dev_err(&kingsun->usbdev->dev,
+			"ks959_send_irq: Network not running!\n");
 		return;
 	}
 
 	/* unlink, shutdown, unplug, other nasties */
 	if (urb->status != 0) {
-		err("ks959_send_irq: urb asynchronously failed - %d",
-		    urb->status);
+		dev_err(&kingsun->usbdev->dev,
+			"ks959_send_irq: urb asynchronously failed - %d\n",
+			urb->status);
 		return;
 	}
 
@@ -358,8 +361,9 @@ static void ks959_send_irq(struct urb *urb)
 		if (kingsun->tx_buf_clear_used > 0) {
 			/* There is more data to be sent */
 			if ((ret = ks959_submit_tx_fragment(kingsun)) != 0) {
-				err("ks959_send_irq: failed tx_urb submit: %d",
-				    ret);
+				dev_err(&kingsun->usbdev->dev,
+					"ks959_send_irq: failed tx_urb submit: %d\n",
+					ret);
 				switch (ret) {
 				case -ENODEV:
 				case -EPIPE:
@@ -407,7 +411,8 @@ static netdev_tx_t ks959_hard_xmit(struct sk_buff *skb,
 	kingsun->tx_buf_clear_used = wraplen;
 
 	if ((ret = ks959_submit_tx_fragment(kingsun)) != 0) {
-		err("ks959_hard_xmit: failed tx_urb submit: %d", ret);
+		dev_err(&kingsun->usbdev->dev,
+			"ks959_hard_xmit: failed tx_urb submit: %d\n", ret);
 		switch (ret) {
 		case -ENODEV:
 		case -EPIPE:
@@ -442,8 +447,9 @@ static void ks959_rcv_irq(struct urb *urb)
 
 	/* unlink, shutdown, unplug, other nasties */
 	if (urb->status != 0) {
-		err("kingsun_rcv_irq: urb asynchronously failed - %d",
-		    urb->status);
+		dev_err(&kingsun->usbdev->dev,
+			"kingsun_rcv_irq: urb asynchronously failed - %d\n",
+			urb->status);
 		kingsun->receiving = 0;
 		return;
 	}
@@ -536,7 +542,7 @@ static int ks959_net_open(struct net_device *netdev)
 	sprintf(hwname, "usb#%d", kingsun->usbdev->devnum);
 	kingsun->irlap = irlap_open(netdev, &kingsun->qos, hwname);
 	if (!kingsun->irlap) {
-		err("ks959-sir: irlap_open failed");
+		dev_err(&kingsun->usbdev->dev, "irlap_open failed\n");
 		goto free_mem;
 	}
 
@@ -549,7 +555,8 @@ static int ks959_net_open(struct net_device *netdev)
 	kingsun->rx_urb->status = 0;
 	err = usb_submit_urb(kingsun->rx_urb, GFP_KERNEL);
 	if (err) {
-		err("ks959-sir: first urb-submit failed: %d", err);
+		dev_err(&kingsun->usbdev->dev,
+			"first urb-submit failed: %d\n", err);
 		goto close_irlap;
 	}
 
diff --git a/drivers/net/irda/ksdazzle-sir.c b/drivers/net/irda/ksdazzle-sir.c
index f8c01088eeb..5a278ab83c2 100644
--- a/drivers/net/irda/ksdazzle-sir.c
+++ b/drivers/net/irda/ksdazzle-sir.c
@@ -168,10 +168,10 @@ struct ksdazzle_cb {
 static void ksdazzle_speed_irq(struct urb *urb)
 {
 	/* unlink, shutdown, unplug, other nasties */
-	if (urb->status != 0) {
-		err("ksdazzle_speed_irq: urb asynchronously failed - %d",
-		    urb->status);
-	}
+	if (urb->status != 0)
+		dev_err(&urb->dev->dev,
+			"ksdazzle_speed_irq: urb asynchronously failed - %d\n",
+			urb->status);
 }
 
 /* Send a control request to change speed of the dongle */
@@ -245,14 +245,16 @@ static void ksdazzle_send_irq(struct urb *urb)
 
 	/* in process of stopping, just drop data */
 	if (!netif_running(kingsun->netdev)) {
-		err("ksdazzle_send_irq: Network not running!");
+		dev_err(&kingsun->usbdev->dev,
+			"ksdazzle_send_irq: Network not running!\n");
 		return;
 	}
 
 	/* unlink, shutdown, unplug, other nasties */
 	if (urb->status != 0) {
-		err("ksdazzle_send_irq: urb asynchronously failed - %d",
-		    urb->status);
+		dev_err(&kingsun->usbdev->dev,
+			"ksdazzle_send_irq: urb asynchronously failed - %d\n",
+			urb->status);
 		return;
 	}
 
@@ -271,7 +273,9 @@ static void ksdazzle_send_irq(struct urb *urb)
 		if (kingsun->tx_buf_clear_used > 0) {
 			/* There is more data to be sent */
 			if ((ret = ksdazzle_submit_tx_fragment(kingsun)) != 0) {
-				err("ksdazzle_send_irq: failed tx_urb submit: %d", ret);
+				dev_err(&kingsun->usbdev->dev,
+					"ksdazzle_send_irq: failed tx_urb submit: %d\n",
+					ret);
 				switch (ret) {
 				case -ENODEV:
 				case -EPIPE:
@@ -320,7 +324,8 @@ static netdev_tx_t ksdazzle_hard_xmit(struct sk_buff *skb,
 	kingsun->tx_buf_clear_used = wraplen;
 
 	if ((ret = ksdazzle_submit_tx_fragment(kingsun)) != 0) {
-		err("ksdazzle_hard_xmit: failed tx_urb submit: %d", ret);
+		dev_err(&kingsun->usbdev->dev,
+			"ksdazzle_hard_xmit: failed tx_urb submit: %d\n", ret);
 		switch (ret) {
 		case -ENODEV:
 		case -EPIPE:
@@ -355,8 +360,9 @@ static void ksdazzle_rcv_irq(struct urb *urb)
 
 	/* unlink, shutdown, unplug, other nasties */
 	if (urb->status != 0) {
-		err("ksdazzle_rcv_irq: urb asynchronously failed - %d",
-		    urb->status);
+		dev_err(&kingsun->usbdev->dev,
+			"ksdazzle_rcv_irq: urb asynchronously failed - %d\n",
+			urb->status);
 		kingsun->receiving = 0;
 		return;
 	}
@@ -430,7 +436,7 @@ static int ksdazzle_net_open(struct net_device *netdev)
 	sprintf(hwname, "usb#%d", kingsun->usbdev->devnum);
 	kingsun->irlap = irlap_open(netdev, &kingsun->qos, hwname);
 	if (!kingsun->irlap) {
-		err("ksdazzle-sir: irlap_open failed");
+		dev_err(&kingsun->usbdev->dev, "irlap_open failed\n");
 		goto free_mem;
 	}
 
@@ -442,7 +448,7 @@ static int ksdazzle_net_open(struct net_device *netdev)
 	kingsun->rx_urb->status = 0;
 	err = usb_submit_urb(kingsun->rx_urb, GFP_KERNEL);
 	if (err) {
-		err("ksdazzle-sir: first urb-submit failed: %d", err);
+		dev_err(&kingsun->usbdev->dev, "first urb-submit failed: %d\n", err);
 		goto close_irlap;
 	}
 
@@ -590,13 +596,14 @@ static int ksdazzle_probe(struct usb_interface *intf,
 	 */
 	interface = intf->cur_altsetting;
 	if (interface->desc.bNumEndpoints != 2) {
-		err("ksdazzle: expected 2 endpoints, found %d",
-		    interface->desc.bNumEndpoints);
+		dev_err(&intf->dev, "ksdazzle: expected 2 endpoints, found %d\n",
+			interface->desc.bNumEndpoints);
 		return -ENODEV;
 	}
 	endpoint = &interface->endpoint[KINGSUN_EP_IN].desc;
 	if (!usb_endpoint_is_int_in(endpoint)) {
-		err("ksdazzle: endpoint 0 is not interrupt IN");
+		dev_err(&intf->dev,
+			"ksdazzle: endpoint 0 is not interrupt IN\n");
 		return -ENODEV;
 	}
 
@@ -604,13 +611,16 @@ static int ksdazzle_probe(struct usb_interface *intf,
 	pipe = usb_rcvintpipe(dev, ep_in);
 	maxp_in = usb_maxpacket(dev, pipe, usb_pipeout(pipe));
 	if (maxp_in > 255 || maxp_in <= 1) {
-		err("ksdazzle: endpoint 0 has max packet size %d not in range [2..255]", maxp_in);
+		dev_err(&intf->dev,
+			"ksdazzle: endpoint 0 has max packet size %d not in range [2..255]\n",
+			maxp_in);
 		return -ENODEV;
 	}
 
 	endpoint = &interface->endpoint[KINGSUN_EP_OUT].desc;
 	if (!usb_endpoint_is_int_out(endpoint)) {
-		err("ksdazzle: endpoint 1 is not interrupt OUT");
+		dev_err(&intf->dev,
+			"ksdazzle: endpoint 1 is not interrupt OUT\n");
 		return -ENODEV;
 	}
 
diff --git a/drivers/net/irda/sh_irda.c b/drivers/net/irda/sh_irda.c
index 725d6b36782..eb315b8d07a 100644
--- a/drivers/net/irda/sh_irda.c
+++ b/drivers/net/irda/sh_irda.c
@@ -737,7 +737,7 @@ static int sh_irda_stop(struct net_device *ndev)
 	netif_stop_queue(ndev);
 	pm_runtime_put_sync(&self->pdev->dev);
 
-	dev_info(&ndev->dev, "stoped\n");
+	dev_info(&ndev->dev, "stopped\n");
 
 	return 0;
 }
diff --git a/drivers/net/irda/sh_sir.c b/drivers/net/irda/sh_sir.c
index e6661b5c1f8..256eddf1f75 100644
--- a/drivers/net/irda/sh_sir.c
+++ b/drivers/net/irda/sh_sir.c
@@ -685,7 +685,7 @@ static int sh_sir_stop(struct net_device *ndev)
 
 	netif_stop_queue(ndev);
 
-	dev_info(&ndev->dev, "stoped\n");
+	dev_info(&ndev->dev, "stopped\n");
 
 	return 0;
 }
diff --git a/drivers/net/irda/smsc-ircc2.c b/drivers/net/irda/smsc-ircc2.c
index 6c95d4087b2..a926813ee91 100644
--- a/drivers/net/irda/smsc-ircc2.c
+++ b/drivers/net/irda/smsc-ircc2.c
@@ -1,7 +1,6 @@
 /*********************************************************************
  *
  * Description:   Driver for the SMC Infrared Communications Controller
- * Status:        Experimental.
  * Author:        Daniele Peri (peri@csai.unipa.it)
  * Created at:
  * Modified at:
diff --git a/drivers/net/irda/stir4200.c b/drivers/net/irda/stir4200.c
index e6e59a078ef..876e709b65b 100644
--- a/drivers/net/irda/stir4200.c
+++ b/drivers/net/irda/stir4200.c
@@ -904,7 +904,7 @@ static int stir_net_open(struct net_device *netdev)
 	sprintf(hwname, "usb#%d", stir->usbdev->devnum);
 	stir->irlap = irlap_open(netdev, &stir->qos, hwname);
 	if (!stir->irlap) {
-		err("stir4200: irlap_open failed");
+		dev_err(&stir->usbdev->dev, "irlap_open failed\n");
 		goto err_out5;
 	}
 
@@ -913,7 +913,7 @@ static int stir_net_open(struct net_device *netdev)
 				   "%s", stir->netdev->name);
         if (IS_ERR(stir->thread)) {
                 err = PTR_ERR(stir->thread);
-		err("stir4200: unable to start kernel thread");
+		dev_err(&stir->usbdev->dev, "unable to start kernel thread\n");
 		goto err_out6;
 	}
 
@@ -1042,7 +1042,7 @@ static int stir_probe(struct usb_interface *intf,
 
 	ret = usb_reset_configuration(dev);
 	if (ret != 0) {
-		err("stir4200: usb reset configuration failed");
+		dev_err(&intf->dev, "usb reset configuration failed\n");
 		goto err_out2;
 	}
 
diff --git a/drivers/net/macvlan.c b/drivers/net/macvlan.c
index f975afdc315..66a9bfe7b1c 100644
--- a/drivers/net/macvlan.c
+++ b/drivers/net/macvlan.c
@@ -57,7 +57,7 @@ static struct macvlan_dev *macvlan_hash_lookup(const struct macvlan_port *port,
 	struct hlist_node *n;
 
 	hlist_for_each_entry_rcu(vlan, n, &port->vlan_hash[addr[5]], hlist) {
-		if (!compare_ether_addr_64bits(vlan->dev->dev_addr, addr))
+		if (ether_addr_equal_64bits(vlan->dev->dev_addr, addr))
 			return vlan;
 	}
 	return NULL;
@@ -96,7 +96,7 @@ static int macvlan_addr_busy(const struct macvlan_port *port,
 	 * currently in use by the underlying device or
 	 * another macvlan.
 	 */
-	if (!compare_ether_addr_64bits(port->dev->dev_addr, addr))
+	if (ether_addr_equal_64bits(port->dev->dev_addr, addr))
 		return 1;
 
 	if (macvlan_hash_lookup(port, addr))
@@ -118,8 +118,7 @@ static int macvlan_broadcast_one(struct sk_buff *skb,
 		return vlan->forward(dev, skb);
 
 	skb->dev = dev;
-	if (!compare_ether_addr_64bits(eth->h_dest,
-				       dev->broadcast))
+	if (ether_addr_equal_64bits(eth->h_dest, dev->broadcast))
 		skb->pkt_type = PACKET_BROADCAST;
 	else
 		skb->pkt_type = PACKET_MULTICAST;
@@ -259,7 +258,7 @@ static int macvlan_queue_xmit(struct sk_buff *skb, struct net_device *dev)
 
 xmit_world:
 	skb->ip_summed = ip_summed;
-	skb_set_dev(skb, vlan->lowerdev);
+	skb->dev = vlan->lowerdev;
 	return dev_queue_xmit(skb);
 }
 
@@ -312,7 +311,8 @@ static int macvlan_open(struct net_device *dev)
 	int err;
 
 	if (vlan->port->passthru) {
-		dev_set_promiscuity(lowerdev, 1);
+		if (!(vlan->flags & MACVLAN_FLAG_NOPROMISC))
+			dev_set_promiscuity(lowerdev, 1);
 		goto hash_add;
 	}
 
@@ -344,12 +344,15 @@ static int macvlan_stop(struct net_device *dev)
 	struct macvlan_dev *vlan = netdev_priv(dev);
 	struct net_device *lowerdev = vlan->lowerdev;
 
+	dev_uc_unsync(lowerdev, dev);
+	dev_mc_unsync(lowerdev, dev);
+
 	if (vlan->port->passthru) {
-		dev_set_promiscuity(lowerdev, -1);
+		if (!(vlan->flags & MACVLAN_FLAG_NOPROMISC))
+			dev_set_promiscuity(lowerdev, -1);
 		goto hash_del;
 	}
 
-	dev_mc_unsync(lowerdev, dev);
 	if (dev->flags & IFF_ALLMULTI)
 		dev_set_allmulti(lowerdev, -1);
 
@@ -399,10 +402,11 @@ static void macvlan_change_rx_flags(struct net_device *dev, int change)
 		dev_set_allmulti(lowerdev, dev->flags & IFF_ALLMULTI ? 1 : -1);
 }
 
-static void macvlan_set_multicast_list(struct net_device *dev)
+static void macvlan_set_mac_lists(struct net_device *dev)
 {
 	struct macvlan_dev *vlan = netdev_priv(dev);
 
+	dev_uc_sync(vlan->lowerdev, dev);
 	dev_mc_sync(vlan->lowerdev, dev);
 }
 
@@ -542,6 +546,43 @@ static int macvlan_vlan_rx_kill_vid(struct net_device *dev,
 	return 0;
 }
 
+static int macvlan_fdb_add(struct ndmsg *ndm,
+			   struct net_device *dev,
+			   unsigned char *addr,
+			   u16 flags)
+{
+	struct macvlan_dev *vlan = netdev_priv(dev);
+	int err = -EINVAL;
+
+	if (!vlan->port->passthru)
+		return -EOPNOTSUPP;
+
+	if (is_unicast_ether_addr(addr))
+		err = dev_uc_add_excl(dev, addr);
+	else if (is_multicast_ether_addr(addr))
+		err = dev_mc_add_excl(dev, addr);
+
+	return err;
+}
+
+static int macvlan_fdb_del(struct ndmsg *ndm,
+			   struct net_device *dev,
+			   unsigned char *addr)
+{
+	struct macvlan_dev *vlan = netdev_priv(dev);
+	int err = -EINVAL;
+
+	if (!vlan->port->passthru)
+		return -EOPNOTSUPP;
+
+	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);
+
+	return err;
+}
+
 static void macvlan_ethtool_get_drvinfo(struct net_device *dev,
 					struct ethtool_drvinfo *drvinfo)
 {
@@ -572,11 +613,14 @@ static const struct net_device_ops macvlan_netdev_ops = {
 	.ndo_change_mtu		= macvlan_change_mtu,
 	.ndo_change_rx_flags	= macvlan_change_rx_flags,
 	.ndo_set_mac_address	= macvlan_set_mac_address,
-	.ndo_set_rx_mode	= macvlan_set_multicast_list,
+	.ndo_set_rx_mode	= macvlan_set_mac_lists,
 	.ndo_get_stats64	= macvlan_dev_get_stats64,
 	.ndo_validate_addr	= eth_validate_addr,
 	.ndo_vlan_rx_add_vid	= macvlan_vlan_rx_add_vid,
 	.ndo_vlan_rx_kill_vid	= macvlan_vlan_rx_kill_vid,
+	.ndo_fdb_add		= macvlan_fdb_add,
+	.ndo_fdb_del		= macvlan_fdb_del,
+	.ndo_fdb_dump		= ndo_dflt_fdb_dump,
 };
 
 void macvlan_common_setup(struct net_device *dev)
@@ -711,6 +755,9 @@ int macvlan_common_newlink(struct net *src_net, struct net_device *dev,
 	if (data && data[IFLA_MACVLAN_MODE])
 		vlan->mode = nla_get_u32(data[IFLA_MACVLAN_MODE]);
 
+	if (data && data[IFLA_MACVLAN_FLAGS])
+		vlan->flags = nla_get_u16(data[IFLA_MACVLAN_FLAGS]);
+
 	if (vlan->mode == MACVLAN_MODE_PASSTHRU) {
 		if (port->count)
 			return -EINVAL;
@@ -760,6 +807,16 @@ static int macvlan_changelink(struct net_device *dev,
 	struct macvlan_dev *vlan = netdev_priv(dev);
 	if (data && data[IFLA_MACVLAN_MODE])
 		vlan->mode = nla_get_u32(data[IFLA_MACVLAN_MODE]);
+	if (data && data[IFLA_MACVLAN_FLAGS]) {
+		__u16 flags = nla_get_u16(data[IFLA_MACVLAN_FLAGS]);
+		bool promisc = (flags ^ vlan->flags) & MACVLAN_FLAG_NOPROMISC;
+
+		if (promisc && (flags & MACVLAN_FLAG_NOPROMISC))
+			dev_set_promiscuity(vlan->lowerdev, -1);
+		else if (promisc && !(flags & MACVLAN_FLAG_NOPROMISC))
+			dev_set_promiscuity(vlan->lowerdev, 1);
+		vlan->flags = flags;
+	}
 	return 0;
 }
 
@@ -773,7 +830,10 @@ static int macvlan_fill_info(struct sk_buff *skb,
 {
 	struct macvlan_dev *vlan = netdev_priv(dev);
 
-	NLA_PUT_U32(skb, IFLA_MACVLAN_MODE, vlan->mode);
+	if (nla_put_u32(skb, IFLA_MACVLAN_MODE, vlan->mode))
+		goto nla_put_failure;
+	if (nla_put_u16(skb, IFLA_MACVLAN_FLAGS, vlan->flags))
+		goto nla_put_failure;
 	return 0;
 
 nla_put_failure:
@@ -781,7 +841,8 @@ nla_put_failure:
 }
 
 static const struct nla_policy macvlan_policy[IFLA_MACVLAN_MAX + 1] = {
-	[IFLA_MACVLAN_MODE] = { .type = NLA_U32 },
+	[IFLA_MACVLAN_MODE]  = { .type = NLA_U32 },
+	[IFLA_MACVLAN_FLAGS] = { .type = NLA_U16 },
 };
 
 int macvlan_link_register(struct rtnl_link_ops *ops)
diff --git a/drivers/net/macvtap.c b/drivers/net/macvtap.c
index 0427c6561c8..2ee56de7b0c 100644
--- a/drivers/net/macvtap.c
+++ b/drivers/net/macvtap.c
@@ -1,5 +1,6 @@
 #include <linux/etherdevice.h>
 #include <linux/if_macvlan.h>
+#include <linux/if_vlan.h>
 #include <linux/interrupt.h>
 #include <linux/nsproxy.h>
 #include <linux/compat.h>
@@ -505,10 +506,11 @@ static int zerocopy_sg_from_iovec(struct sk_buff *skb, const struct iovec *from,
 		if (copy > size) {
 			++from;
 			--count;
-		}
+			offset = 0;
+		} else
+			offset += size;
 		copy -= size;
 		offset1 += size;
-		offset = 0;
 	}
 
 	if (len == offset1)
@@ -518,24 +520,29 @@ static int zerocopy_sg_from_iovec(struct sk_buff *skb, const struct iovec *from,
 		struct page *page[MAX_SKB_FRAGS];
 		int num_pages;
 		unsigned long base;
+		unsigned long truesize;
 
-		len = from->iov_len - offset1;
+		len = from->iov_len - offset;
 		if (!len) {
-			offset1 = 0;
+			offset = 0;
 			++from;
 			continue;
 		}
-		base = (unsigned long)from->iov_base + offset1;
+		base = (unsigned long)from->iov_base + offset;
 		size = ((base & ~PAGE_MASK) + len + ~PAGE_MASK) >> PAGE_SHIFT;
+		if (i + size > MAX_SKB_FRAGS)
+			return -EMSGSIZE;
 		num_pages = get_user_pages_fast(base, size, 0, &page[i]);
-		if ((num_pages != size) ||
-		    (num_pages > MAX_SKB_FRAGS - skb_shinfo(skb)->nr_frags))
-			/* put_page is in skb free */
+		if (num_pages != size) {
+			for (i = 0; i < num_pages; i++)
+				put_page(page[i]);
 			return -EFAULT;
+		}
+		truesize = size * PAGE_SIZE;
 		skb->data_len += len;
 		skb->len += len;
-		skb->truesize += len;
-		atomic_add(len, &skb->sk->sk_wmem_alloc);
+		skb->truesize += truesize;
+		atomic_add(truesize, &skb->sk->sk_wmem_alloc);
 		while (len) {
 			int off = base & ~PAGE_MASK;
 			int size = min_t(int, len, PAGE_SIZE - off);
@@ -546,7 +553,7 @@ static int zerocopy_sg_from_iovec(struct sk_buff *skb, const struct iovec *from,
 			len -= size;
 			i++;
 		}
-		offset1 = 0;
+		offset = 0;
 		++from;
 	}
 	return 0;
@@ -646,7 +653,7 @@ static ssize_t macvtap_get_user(struct macvtap_queue *q, struct msghdr *m,
 	int err;
 	struct virtio_net_hdr vnet_hdr = { 0 };
 	int vnet_hdr_len = 0;
-	int copylen;
+	int copylen = 0;
 	bool zerocopy = false;
 
 	if (q->flags & IFF_VNET_HDR) {
@@ -675,15 +682,31 @@ static ssize_t macvtap_get_user(struct macvtap_queue *q, struct msghdr *m,
 	if (unlikely(len < ETH_HLEN))
 		goto err;
 
+	err = -EMSGSIZE;
+	if (unlikely(count > UIO_MAXIOV))
+		goto err;
+
 	if (m && m->msg_control && sock_flag(&q->sk, SOCK_ZEROCOPY))
 		zerocopy = true;
 
 	if (zerocopy) {
+		/* Userspace may produce vectors with count greater than
+		 * MAX_SKB_FRAGS, so we need to linearize parts of the skb
+		 * to let the rest of data to be fit in the frags.
+		 */
+		if (count > MAX_SKB_FRAGS) {
+			copylen = iov_length(iv, count - MAX_SKB_FRAGS);
+			if (copylen < vnet_hdr_len)
+				copylen = 0;
+			else
+				copylen -= vnet_hdr_len;
+		}
 		/* There are 256 bytes to be copied in skb, so there is enough
 		 * room for skb expand head in case it is used.
 		 * The rest buffer is mapped from userspace.
 		 */
-		copylen = vnet_hdr.hdr_len;
+		if (copylen < vnet_hdr.hdr_len)
+			copylen = vnet_hdr.hdr_len;
 		if (!copylen)
 			copylen = GOODCOPY_LEN;
 	} else
@@ -694,10 +717,9 @@ static ssize_t macvtap_get_user(struct macvtap_queue *q, struct msghdr *m,
 	if (!skb)
 		goto err;
 
-	if (zerocopy) {
+	if (zerocopy)
 		err = zerocopy_sg_from_iovec(skb, iv, vnet_hdr_len, count);
-		skb_shinfo(skb)->tx_flags |= SKBTX_DEV_ZEROCOPY;
-	} else
+	else
 		err = skb_copy_datagram_from_iovec(skb, 0, iv, vnet_hdr_len,
 						   len);
 	if (err)
@@ -716,8 +738,10 @@ static ssize_t macvtap_get_user(struct macvtap_queue *q, struct msghdr *m,
 	rcu_read_lock_bh();
 	vlan = rcu_dereference_bh(q->vlan);
 	/* copy skb_ubuf_info for callback when skb has no error */
-	if (zerocopy)
+	if (zerocopy) {
 		skb_shinfo(skb)->destructor_arg = m->msg_control;
+		skb_shinfo(skb)->tx_flags |= SKBTX_DEV_ZEROCOPY;
+	}
 	if (vlan)
 		macvlan_start_xmit(skb, vlan->dev);
 	else
@@ -759,6 +783,8 @@ static ssize_t macvtap_put_user(struct macvtap_queue *q,
 	struct macvlan_dev *vlan;
 	int ret;
 	int vnet_hdr_len = 0;
+	int vlan_offset = 0;
+	int copied;
 
 	if (q->flags & IFF_VNET_HDR) {
 		struct virtio_net_hdr vnet_hdr;
@@ -773,18 +799,48 @@ static ssize_t macvtap_put_user(struct macvtap_queue *q,
 		if (memcpy_toiovecend(iv, (void *)&vnet_hdr, 0, sizeof(vnet_hdr)))
 			return -EFAULT;
 	}
+	copied = vnet_hdr_len;
+
+	if (!vlan_tx_tag_present(skb))
+		len = min_t(int, skb->len, len);
+	else {
+		int copy;
+		struct {
+			__be16 h_vlan_proto;
+			__be16 h_vlan_TCI;
+		} veth;
+		veth.h_vlan_proto = htons(ETH_P_8021Q);
+		veth.h_vlan_TCI = htons(vlan_tx_tag_get(skb));
+
+		vlan_offset = offsetof(struct vlan_ethhdr, h_vlan_proto);
+		len = min_t(int, skb->len + VLAN_HLEN, len);
+
+		copy = min_t(int, vlan_offset, len);
+		ret = skb_copy_datagram_const_iovec(skb, 0, iv, copied, copy);
+		len -= copy;
+		copied += copy;
+		if (ret || !len)
+			goto done;
+
+		copy = min_t(int, sizeof(veth), len);
+		ret = memcpy_toiovecend(iv, (void *)&veth, copied, copy);
+		len -= copy;
+		copied += copy;
+		if (ret || !len)
+			goto done;
+	}
 
-	len = min_t(int, skb->len, len);
-
-	ret = skb_copy_datagram_const_iovec(skb, 0, iv, vnet_hdr_len, len);
+	ret = skb_copy_datagram_const_iovec(skb, vlan_offset, iv, copied, len);
+	copied += len;
 
+done:
 	rcu_read_lock_bh();
 	vlan = rcu_dereference_bh(q->vlan);
 	if (vlan)
-		macvlan_count_rx(vlan, len, ret == 0, 0);
+		macvlan_count_rx(vlan, copied - vnet_hdr_len, ret == 0, 0);
 	rcu_read_unlock_bh();
 
-	return ret ? ret : (len + vnet_hdr_len);
+	return ret ? ret : copied;
 }
 
 static ssize_t macvtap_do_read(struct macvtap_queue *q, struct kiocb *iocb,
diff --git a/drivers/net/phy/Kconfig b/drivers/net/phy/Kconfig
index 0e01f4e5cd6..944cdfb80fe 100644
--- a/drivers/net/phy/Kconfig
+++ b/drivers/net/phy/Kconfig
@@ -135,6 +135,25 @@ config MDIO_OCTEON
 
 	  If in doubt, say Y.
 
+config MDIO_BUS_MUX
+	tristate
+	depends on OF_MDIO
+	help
+	  This module provides a driver framework for MDIO bus
+	  multiplexers which connect one of several child MDIO busses
+	  to a parent bus.  Switching between child busses is done by
+	  device specific drivers.
+
+config MDIO_BUS_MUX_GPIO
+	tristate "Support for GPIO controlled MDIO bus multiplexers"
+	depends on OF_GPIO && OF_MDIO
+	select MDIO_BUS_MUX
+	help
+	  This module provides a driver for MDIO bus multiplexers that
+	  are controlled via GPIO lines.  The multiplexer connects one of
+	  several child MDIO busses to a parent bus.  Child bus
+	  selection is under the control of GPIO lines.
+
 endif # PHYLIB
 
 config MICREL_KS8995MA
diff --git a/drivers/net/phy/Makefile b/drivers/net/phy/Makefile
index b7438b1b94b..f51af688ef8 100644
--- a/drivers/net/phy/Makefile
+++ b/drivers/net/phy/Makefile
@@ -25,3 +25,5 @@ obj-$(CONFIG_MICREL_PHY)	+= micrel.o
 obj-$(CONFIG_MDIO_OCTEON)	+= mdio-octeon.o
 obj-$(CONFIG_MICREL_KS8995MA)	+= spi_ks8995.o
 obj-$(CONFIG_AMD_PHY)		+= amd.o
+obj-$(CONFIG_MDIO_BUS_MUX)	+= mdio-mux.o
+obj-$(CONFIG_MDIO_BUS_MUX_GPIO)	+= mdio-mux-gpio.o
diff --git a/drivers/net/phy/bcm63xx.c b/drivers/net/phy/bcm63xx.c
index e16f98cb4f0..cd802eb25fd 100644
--- a/drivers/net/phy/bcm63xx.c
+++ b/drivers/net/phy/bcm63xx.c
@@ -39,10 +39,7 @@ static int bcm63xx_config_init(struct phy_device *phydev)
 		MII_BCM63XX_IR_SPEED |
 		MII_BCM63XX_IR_LINK) |
 		MII_BCM63XX_IR_EN;
-	err = phy_write(phydev, MII_BCM63XX_IR, reg);
-	if (err < 0)
-		return err;
-	return 0;
+	return phy_write(phydev, MII_BCM63XX_IR, reg);
 }
 
 static int bcm63xx_ack_interrupt(struct phy_device *phydev)
diff --git a/drivers/net/phy/davicom.c b/drivers/net/phy/davicom.c
index 2f774acdb55..5f59cc06477 100644
--- a/drivers/net/phy/davicom.c
+++ b/drivers/net/phy/davicom.c
@@ -134,12 +134,7 @@ static int dm9161_config_init(struct phy_device *phydev)
 		return err;
 
 	/* Reconnect the PHY, and enable Autonegotiation */
-	err = phy_write(phydev, MII_BMCR, BMCR_ANENABLE);
-
-	if (err < 0)
-		return err;
-
-	return 0;
+	return phy_write(phydev, MII_BMCR, BMCR_ANENABLE);
 }
 
 static int dm9161_ack_interrupt(struct phy_device *phydev)
diff --git a/drivers/net/phy/dp83640.c b/drivers/net/phy/dp83640.c
index dd7ae19579d..940b29022d0 100644
--- a/drivers/net/phy/dp83640.c
+++ b/drivers/net/phy/dp83640.c
@@ -1215,6 +1215,36 @@ static void dp83640_txtstamp(struct phy_device *phydev,
 	}
 }
 
+static int dp83640_ts_info(struct phy_device *dev, struct ethtool_ts_info *info)
+{
+	struct dp83640_private *dp83640 = dev->priv;
+
+	info->so_timestamping =
+		SOF_TIMESTAMPING_TX_HARDWARE |
+		SOF_TIMESTAMPING_RX_HARDWARE |
+		SOF_TIMESTAMPING_RAW_HARDWARE;
+	info->phc_index = ptp_clock_index(dp83640->clock->ptp_clock);
+	info->tx_types =
+		(1 << HWTSTAMP_TX_OFF) |
+		(1 << HWTSTAMP_TX_ON) |
+		(1 << HWTSTAMP_TX_ONESTEP_SYNC);
+	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_L2_EVENT) |
+		(1 << HWTSTAMP_FILTER_PTP_V2_L2_SYNC) |
+		(1 << HWTSTAMP_FILTER_PTP_V2_L2_DELAY_REQ) |
+		(1 << HWTSTAMP_FILTER_PTP_V2_EVENT) |
+		(1 << HWTSTAMP_FILTER_PTP_V2_SYNC) |
+		(1 << HWTSTAMP_FILTER_PTP_V2_DELAY_REQ);
+	return 0;
+}
+
 static struct phy_driver dp83640_driver = {
 	.phy_id		= DP83640_PHY_ID,
 	.phy_id_mask	= 0xfffffff0,
@@ -1225,6 +1255,7 @@ static struct phy_driver dp83640_driver = {
 	.remove		= dp83640_remove,
 	.config_aneg	= genphy_config_aneg,
 	.read_status	= genphy_read_status,
+	.ts_info	= dp83640_ts_info,
 	.hwtstamp	= dp83640_hwtstamp,
 	.rxtstamp	= dp83640_rxtstamp,
 	.txtstamp	= dp83640_txtstamp,
diff --git a/drivers/net/phy/marvell.c b/drivers/net/phy/marvell.c
index e8b9c53c304..418928d644b 100644
--- a/drivers/net/phy/marvell.c
+++ b/drivers/net/phy/marvell.c
@@ -455,11 +455,7 @@ static int m88e1111_config_init(struct phy_device *phydev)
 	if (err < 0)
 		return err;
 
-	err = phy_write(phydev, MII_BMCR, BMCR_RESET);
-	if (err < 0)
-		return err;
-
-	return 0;
+	return phy_write(phydev, MII_BMCR, BMCR_RESET);
 }
 
 static int m88e1118_config_aneg(struct phy_device *phydev)
@@ -515,11 +511,7 @@ static int m88e1118_config_init(struct phy_device *phydev)
 	if (err < 0)
 		return err;
 
-	err = phy_write(phydev, MII_BMCR, BMCR_RESET);
-	if (err < 0)
-		return err;
-
-	return 0;
+	return phy_write(phydev, MII_BMCR, BMCR_RESET);
 }
 
 static int m88e1149_config_init(struct phy_device *phydev)
@@ -545,11 +537,7 @@ static int m88e1149_config_init(struct phy_device *phydev)
 	if (err < 0)
 		return err;
 
-	err = phy_write(phydev, MII_BMCR, BMCR_RESET);
-	if (err < 0)
-		return err;
-
-	return 0;
+	return phy_write(phydev, MII_BMCR, BMCR_RESET);
 }
 
 static int m88e1145_config_init(struct phy_device *phydev)
diff --git a/drivers/net/phy/mdio-mux-gpio.c b/drivers/net/phy/mdio-mux-gpio.c
new file mode 100644
index 00000000000..e0cc4ef33de
--- /dev/null
+++ b/drivers/net/phy/mdio-mux-gpio.c
@@ -0,0 +1,142 @@
+/*
+ * 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.
+ *
+ * Copyright (C) 2011, 2012 Cavium, Inc.
+ */
+
+#include <linux/platform_device.h>
+#include <linux/device.h>
+#include <linux/of_mdio.h>
+#include <linux/module.h>
+#include <linux/init.h>
+#include <linux/phy.h>
+#include <linux/mdio-mux.h>
+#include <linux/of_gpio.h>
+
+#define DRV_VERSION "1.0"
+#define DRV_DESCRIPTION "GPIO controlled MDIO bus multiplexer driver"
+
+#define MDIO_MUX_GPIO_MAX_BITS 8
+
+struct mdio_mux_gpio_state {
+	int gpio[MDIO_MUX_GPIO_MAX_BITS];
+	unsigned int num_gpios;
+	void *mux_handle;
+};
+
+static int mdio_mux_gpio_switch_fn(int current_child, int desired_child,
+				   void *data)
+{
+	int change;
+	unsigned int n;
+	struct mdio_mux_gpio_state *s = data;
+
+	if (current_child == desired_child)
+		return 0;
+
+	change = current_child == -1 ? -1 : current_child ^ desired_child;
+
+	for (n = 0; n < s->num_gpios; n++) {
+		if (change & 1)
+			gpio_set_value_cansleep(s->gpio[n],
+						(desired_child & 1) != 0);
+		change >>= 1;
+		desired_child >>= 1;
+	}
+
+	return 0;
+}
+
+static int __devinit mdio_mux_gpio_probe(struct platform_device *pdev)
+{
+	enum of_gpio_flags f;
+	struct mdio_mux_gpio_state *s;
+	unsigned int num_gpios;
+	unsigned int n;
+	int r;
+
+	if (!pdev->dev.of_node)
+		return -ENODEV;
+
+	num_gpios = of_gpio_count(pdev->dev.of_node);
+	if (num_gpios == 0 || num_gpios > MDIO_MUX_GPIO_MAX_BITS)
+		return -ENODEV;
+
+	s = devm_kzalloc(&pdev->dev, sizeof(*s), GFP_KERNEL);
+	if (!s)
+		return -ENOMEM;
+
+	s->num_gpios = num_gpios;
+
+	for (n = 0; n < num_gpios; ) {
+		int gpio = of_get_gpio_flags(pdev->dev.of_node, n, &f);
+		if (gpio < 0) {
+			r = (gpio == -ENODEV) ? -EPROBE_DEFER : gpio;
+			goto err;
+		}
+		s->gpio[n] = gpio;
+
+		n++;
+
+		r = gpio_request(gpio, "mdio_mux_gpio");
+		if (r)
+			goto err;
+
+		r = gpio_direction_output(gpio, 0);
+		if (r)
+			goto err;
+	}
+
+	r = mdio_mux_init(&pdev->dev,
+			  mdio_mux_gpio_switch_fn, &s->mux_handle, s);
+
+	if (r == 0) {
+		pdev->dev.platform_data = s;
+		return 0;
+	}
+err:
+	while (n) {
+		n--;
+		gpio_free(s->gpio[n]);
+	}
+	devm_kfree(&pdev->dev, s);
+	return r;
+}
+
+static int __devexit mdio_mux_gpio_remove(struct platform_device *pdev)
+{
+	struct mdio_mux_gpio_state *s = pdev->dev.platform_data;
+	mdio_mux_uninit(s->mux_handle);
+	return 0;
+}
+
+static struct of_device_id mdio_mux_gpio_match[] = {
+	{
+		.compatible = "mdio-mux-gpio",
+	},
+	{
+		/* Legacy compatible property. */
+		.compatible = "cavium,mdio-mux-sn74cbtlv3253",
+	},
+	{},
+};
+MODULE_DEVICE_TABLE(of, mdio_mux_gpio_match);
+
+static struct platform_driver mdio_mux_gpio_driver = {
+	.driver = {
+		.name		= "mdio-mux-gpio",
+		.owner		= THIS_MODULE,
+		.of_match_table = mdio_mux_gpio_match,
+	},
+	.probe		= mdio_mux_gpio_probe,
+	.remove		= __devexit_p(mdio_mux_gpio_remove),
+};
+
+module_platform_driver(mdio_mux_gpio_driver);
+
+MODULE_DESCRIPTION(DRV_DESCRIPTION);
+MODULE_VERSION(DRV_VERSION);
+MODULE_AUTHOR("David Daney");
+MODULE_LICENSE("GPL");
diff --git a/drivers/net/phy/mdio-mux.c b/drivers/net/phy/mdio-mux.c
new file mode 100644
index 00000000000..39ea0674dcd
--- /dev/null
+++ b/drivers/net/phy/mdio-mux.c
@@ -0,0 +1,192 @@
+/*
+ * 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.
+ *
+ * Copyright (C) 2011, 2012 Cavium, Inc.
+ */
+
+#include <linux/platform_device.h>
+#include <linux/mdio-mux.h>
+#include <linux/of_mdio.h>
+#include <linux/device.h>
+#include <linux/module.h>
+#include <linux/phy.h>
+
+#define DRV_VERSION "1.0"
+#define DRV_DESCRIPTION "MDIO bus multiplexer driver"
+
+struct mdio_mux_child_bus;
+
+struct mdio_mux_parent_bus {
+	struct mii_bus *mii_bus;
+	int current_child;
+	int parent_id;
+	void *switch_data;
+	int (*switch_fn)(int current_child, int desired_child, void *data);
+
+	/* List of our children linked through their next fields. */
+	struct mdio_mux_child_bus *children;
+};
+
+struct mdio_mux_child_bus {
+	struct mii_bus *mii_bus;
+	struct mdio_mux_parent_bus *parent;
+	struct mdio_mux_child_bus *next;
+	int bus_number;
+	int phy_irq[PHY_MAX_ADDR];
+};
+
+/*
+ * The parent bus' lock is used to order access to the switch_fn.
+ */
+static int mdio_mux_read(struct mii_bus *bus, int phy_id, int regnum)
+{
+	struct mdio_mux_child_bus *cb = bus->priv;
+	struct mdio_mux_parent_bus *pb = cb->parent;
+	int r;
+
+	mutex_lock(&pb->mii_bus->mdio_lock);
+	r = pb->switch_fn(pb->current_child, cb->bus_number, pb->switch_data);
+	if (r)
+		goto out;
+
+	pb->current_child = cb->bus_number;
+
+	r = pb->mii_bus->read(pb->mii_bus, phy_id, regnum);
+out:
+	mutex_unlock(&pb->mii_bus->mdio_lock);
+
+	return r;
+}
+
+/*
+ * The parent bus' lock is used to order access to the switch_fn.
+ */
+static int mdio_mux_write(struct mii_bus *bus, int phy_id,
+			  int regnum, u16 val)
+{
+	struct mdio_mux_child_bus *cb = bus->priv;
+	struct mdio_mux_parent_bus *pb = cb->parent;
+
+	int r;
+
+	mutex_lock(&pb->mii_bus->mdio_lock);
+	r = pb->switch_fn(pb->current_child, cb->bus_number, pb->switch_data);
+	if (r)
+		goto out;
+
+	pb->current_child = cb->bus_number;
+
+	r = pb->mii_bus->write(pb->mii_bus, phy_id, regnum, val);
+out:
+	mutex_unlock(&pb->mii_bus->mdio_lock);
+
+	return r;
+}
+
+static int parent_count;
+
+int mdio_mux_init(struct device *dev,
+		  int (*switch_fn)(int cur, int desired, void *data),
+		  void **mux_handle,
+		  void *data)
+{
+	struct device_node *parent_bus_node;
+	struct device_node *child_bus_node;
+	int r, ret_val;
+	struct mii_bus *parent_bus;
+	struct mdio_mux_parent_bus *pb;
+	struct mdio_mux_child_bus *cb;
+
+	if (!dev->of_node)
+		return -ENODEV;
+
+	parent_bus_node = of_parse_phandle(dev->of_node, "mdio-parent-bus", 0);
+
+	if (!parent_bus_node)
+		return -ENODEV;
+
+	parent_bus = of_mdio_find_bus(parent_bus_node);
+	if (parent_bus == NULL) {
+		ret_val = -EPROBE_DEFER;
+		goto err_parent_bus;
+	}
+
+	pb = devm_kzalloc(dev, sizeof(*pb), GFP_KERNEL);
+	if (pb == NULL) {
+		ret_val = -ENOMEM;
+		goto err_parent_bus;
+	}
+
+	pb->switch_data = data;
+	pb->switch_fn = switch_fn;
+	pb->current_child = -1;
+	pb->parent_id = parent_count++;
+	pb->mii_bus = parent_bus;
+
+	ret_val = -ENODEV;
+	for_each_child_of_node(dev->of_node, child_bus_node) {
+		u32 v;
+
+		r = of_property_read_u32(child_bus_node, "reg", &v);
+		if (r)
+			continue;
+
+		cb = devm_kzalloc(dev, sizeof(*cb), GFP_KERNEL);
+		if (cb == NULL) {
+			dev_err(dev,
+				"Error: Failed to allocate memory for child\n");
+			ret_val = -ENOMEM;
+			break;
+		}
+		cb->bus_number = v;
+		cb->parent = pb;
+		cb->mii_bus = mdiobus_alloc();
+		cb->mii_bus->priv = cb;
+
+		cb->mii_bus->irq = cb->phy_irq;
+		cb->mii_bus->name = "mdio_mux";
+		snprintf(cb->mii_bus->id, MII_BUS_ID_SIZE, "%x.%x",
+			 pb->parent_id, v);
+		cb->mii_bus->parent = dev;
+		cb->mii_bus->read = mdio_mux_read;
+		cb->mii_bus->write = mdio_mux_write;
+		r = of_mdiobus_register(cb->mii_bus, child_bus_node);
+		if (r) {
+			mdiobus_free(cb->mii_bus);
+			devm_kfree(dev, cb);
+		} else {
+			of_node_get(child_bus_node);
+			cb->next = pb->children;
+			pb->children = cb;
+		}
+	}
+	if (pb->children) {
+		*mux_handle = pb;
+		dev_info(dev, "Version " DRV_VERSION "\n");
+		return 0;
+	}
+err_parent_bus:
+	of_node_put(parent_bus_node);
+	return ret_val;
+}
+EXPORT_SYMBOL_GPL(mdio_mux_init);
+
+void mdio_mux_uninit(void *mux_handle)
+{
+	struct mdio_mux_parent_bus *pb = mux_handle;
+	struct mdio_mux_child_bus *cb = pb->children;
+
+	while (cb) {
+		mdiobus_unregister(cb->mii_bus);
+		mdiobus_free(cb->mii_bus);
+		cb = cb->next;
+	}
+}
+EXPORT_SYMBOL_GPL(mdio_mux_uninit);
+
+MODULE_DESCRIPTION(DRV_DESCRIPTION);
+MODULE_VERSION(DRV_VERSION);
+MODULE_AUTHOR("David Daney");
+MODULE_LICENSE("GPL");
diff --git a/drivers/net/phy/mdio_bus.c b/drivers/net/phy/mdio_bus.c
index 8985cc62cf4..683ef1ce551 100644
--- a/drivers/net/phy/mdio_bus.c
+++ b/drivers/net/phy/mdio_bus.c
@@ -88,6 +88,38 @@ static struct class mdio_bus_class = {
 	.dev_release	= mdiobus_release,
 };
 
+#if IS_ENABLED(CONFIG_OF_MDIO)
+/* Helper function for of_mdio_find_bus */
+static int of_mdio_bus_match(struct device *dev, void *mdio_bus_np)
+{
+	return dev->of_node == mdio_bus_np;
+}
+/**
+ * of_mdio_find_bus - Given an mii_bus node, find the mii_bus.
+ * @mdio_np: Pointer to the mii_bus.
+ *
+ * Returns a pointer to the mii_bus, or NULL if none found.
+ *
+ * Because the association of a device_node and mii_bus is made via
+ * of_mdiobus_register(), the mii_bus cannot be found before it is
+ * registered with of_mdiobus_register().
+ *
+ */
+struct mii_bus *of_mdio_find_bus(struct device_node *mdio_bus_np)
+{
+	struct device *d;
+
+	if (!mdio_bus_np)
+		return NULL;
+
+	d = class_find_device(&mdio_bus_class, NULL,  mdio_bus_np,
+			      of_mdio_bus_match);
+
+	return d ? to_mii_bus(d) : NULL;
+}
+EXPORT_SYMBOL(of_mdio_find_bus);
+#endif
+
 /**
  * mdiobus_register - bring up all the PHYs on a given bus and attach them to bus
  * @bus: target mii_bus
diff --git a/drivers/net/phy/phy_device.c b/drivers/net/phy/phy_device.c
index e8c42d6a7d1..de86a558222 100644
--- a/drivers/net/phy/phy_device.c
+++ b/drivers/net/phy/phy_device.c
@@ -207,7 +207,7 @@ static struct phy_device* phy_device_create(struct mii_bus *bus,
  * Description: Reads the ID registers of the PHY at @addr on the
  *   @bus, stores it in @phy_id and returns zero on success.
  */
-int get_phy_id(struct mii_bus *bus, int addr, u32 *phy_id)
+static int get_phy_id(struct mii_bus *bus, int addr, u32 *phy_id)
 {
 	int phy_reg;
 
@@ -230,7 +230,6 @@ int get_phy_id(struct mii_bus *bus, int addr, u32 *phy_id)
 
 	return 0;
 }
-EXPORT_SYMBOL(get_phy_id);
 
 /**
  * get_phy_device - reads the specified PHY device and returns its @phy_device struct
diff --git a/drivers/net/phy/spi_ks8995.c b/drivers/net/phy/spi_ks8995.c
index 116a2dd7c87..4eb98bc52a0 100644
--- a/drivers/net/phy/spi_ks8995.c
+++ b/drivers/net/phy/spi_ks8995.c
@@ -348,7 +348,6 @@ static int __devexit ks8995_remove(struct spi_device *spi)
 static struct spi_driver ks8995_driver = {
 	.driver = {
 		.name	    = "spi-ks8995",
-		.bus	     = &spi_bus_type,
 		.owner	   = THIS_MODULE,
 	},
 	.probe	  = ks8995_probe,
diff --git a/drivers/net/ppp/ppp_async.c b/drivers/net/ppp/ppp_async.c
index af95a98fd86..a031f6b456b 100644
--- a/drivers/net/ppp/ppp_async.c
+++ b/drivers/net/ppp/ppp_async.c
@@ -613,7 +613,7 @@ ppp_async_encode(struct asyncppp *ap)
 	*buf++ = PPP_FLAG;
 	ap->olim = buf;
 
-	kfree_skb(ap->tpkt);
+	consume_skb(ap->tpkt);
 	ap->tpkt = NULL;
 	return 1;
 }
diff --git a/drivers/net/ppp/ppp_generic.c b/drivers/net/ppp/ppp_generic.c
index 21d7151fb0a..5c0557222f2 100644
--- a/drivers/net/ppp/ppp_generic.c
+++ b/drivers/net/ppp/ppp_generic.c
@@ -1092,13 +1092,13 @@ pad_compress_skb(struct ppp *ppp, struct sk_buff *skb)
 				   new_skb->data, skb->len + 2,
 				   compressor_skb_size);
 	if (len > 0 && (ppp->flags & SC_CCP_UP)) {
-		kfree_skb(skb);
+		consume_skb(skb);
 		skb = new_skb;
 		skb_put(skb, len);
 		skb_pull(skb, 2);	/* pull off A/C bytes */
 	} else if (len == 0) {
 		/* didn't compress, or CCP not up yet */
-		kfree_skb(new_skb);
+		consume_skb(new_skb);
 		new_skb = skb;
 	} else {
 		/*
@@ -1112,7 +1112,7 @@ pad_compress_skb(struct ppp *ppp, struct sk_buff *skb)
 		if (net_ratelimit())
 			netdev_err(ppp->dev, "ppp: compressor dropped pkt\n");
 		kfree_skb(skb);
-		kfree_skb(new_skb);
+		consume_skb(new_skb);
 		new_skb = NULL;
 	}
 	return new_skb;
@@ -1178,7 +1178,7 @@ ppp_send_frame(struct ppp *ppp, struct sk_buff *skb)
 				    !(ppp->flags & SC_NO_TCP_CCID));
 		if (cp == skb->data + 2) {
 			/* didn't compress */
-			kfree_skb(new_skb);
+			consume_skb(new_skb);
 		} else {
 			if (cp[0] & SL_TYPE_COMPRESSED_TCP) {
 				proto = PPP_VJC_COMP;
@@ -1187,7 +1187,7 @@ ppp_send_frame(struct ppp *ppp, struct sk_buff *skb)
 				proto = PPP_VJC_UNCOMP;
 				cp[0] = skb->data[2];
 			}
-			kfree_skb(skb);
+			consume_skb(skb);
 			skb = new_skb;
 			cp = skb_put(skb, len + 2);
 			cp[0] = 0;
@@ -1703,7 +1703,7 @@ ppp_receive_nonmp_frame(struct ppp *ppp, struct sk_buff *skb)
 			}
 			skb_reserve(ns, 2);
 			skb_copy_bits(skb, 0, skb_put(ns, skb->len), skb->len);
-			kfree_skb(skb);
+			consume_skb(skb);
 			skb = ns;
 		}
 		else
@@ -1851,7 +1851,7 @@ ppp_decompress_frame(struct ppp *ppp, struct sk_buff *skb)
 			goto err;
 		}
 
-		kfree_skb(skb);
+		consume_skb(skb);
 		skb = ns;
 		skb_put(skb, len);
 		skb_pull(skb, 2);	/* pull off the A/C bytes */
diff --git a/drivers/net/ppp/ppp_synctty.c b/drivers/net/ppp/ppp_synctty.c
index 55e466c511d..1a12033d2ef 100644
--- a/drivers/net/ppp/ppp_synctty.c
+++ b/drivers/net/ppp/ppp_synctty.c
@@ -588,7 +588,7 @@ ppp_sync_txmunge(struct syncppp *ap, struct sk_buff *skb)
 			skb_reserve(npkt,2);
 			skb_copy_from_linear_data(skb,
 				      skb_put(npkt, skb->len), skb->len);
-			kfree_skb(skb);
+			consume_skb(skb);
 			skb = npkt;
 		}
 		skb_push(skb,2);
@@ -656,7 +656,7 @@ ppp_sync_push(struct syncppp *ap)
 			if (sent < ap->tpkt->len) {
 				tty_stuffed = 1;
 			} else {
-				kfree_skb(ap->tpkt);
+				consume_skb(ap->tpkt);
 				ap->tpkt = NULL;
 				clear_bit(XMIT_FULL, &ap->xmit_flags);
 				done = 1;
diff --git a/drivers/net/ppp/pppoe.c b/drivers/net/ppp/pppoe.c
index 2fa1a9b6f49..cbf7047decc 100644
--- a/drivers/net/ppp/pppoe.c
+++ b/drivers/net/ppp/pppoe.c
@@ -201,7 +201,7 @@ static int __set_item(struct pppoe_net *pn, struct pppox_sock *po)
 	return 0;
 }
 
-static struct pppox_sock *__delete_item(struct pppoe_net *pn, __be16 sid,
+static void __delete_item(struct pppoe_net *pn, __be16 sid,
 					char *addr, int ifindex)
 {
 	int hash = hash_item(sid, addr);
@@ -220,8 +220,6 @@ static struct pppox_sock *__delete_item(struct pppoe_net *pn, __be16 sid,
 		src = &ret->next;
 		ret = ret->next;
 	}
-
-	return ret;
 }
 
 /**********************************************************************
@@ -264,16 +262,12 @@ static inline struct pppox_sock *get_item_by_addr(struct net *net,
 	return pppox_sock;
 }
 
-static inline struct pppox_sock *delete_item(struct pppoe_net *pn, __be16 sid,
+static inline void delete_item(struct pppoe_net *pn, __be16 sid,
 					char *addr, int ifindex)
 {
-	struct pppox_sock *ret;
-
 	write_lock_bh(&pn->hash_lock);
-	ret = __delete_item(pn, sid, addr, ifindex);
+	__delete_item(pn, sid, addr, ifindex);
 	write_unlock_bh(&pn->hash_lock);
-
-	return ret;
 }
 
 /***************************************************************************
@@ -990,8 +984,10 @@ static int pppoe_recvmsg(struct kiocb *iocb, struct socket *sock,
 	if (skb) {
 		total_len = min_t(size_t, total_len, skb->len);
 		error = skb_copy_datagram_iovec(skb, 0, m->msg_iov, total_len);
-		if (error == 0)
-			error = total_len;
+		if (error == 0) {
+			consume_skb(skb);
+			return total_len;
+		}
 	}
 
 	kfree_skb(skb);
diff --git a/drivers/net/ppp/pptp.c b/drivers/net/ppp/pptp.c
index 885dbdd9c39..1c98321b56c 100644
--- a/drivers/net/ppp/pptp.c
+++ b/drivers/net/ppp/pptp.c
@@ -116,8 +116,8 @@ static int lookup_chan_dst(u16 call_id, __be32 d_addr)
 	int i;
 
 	rcu_read_lock();
-	for (i = find_next_bit(callid_bitmap, MAX_CALLID, 1); i < MAX_CALLID;
-	     i = find_next_bit(callid_bitmap, MAX_CALLID, i + 1)) {
+	i = 1;
+	for_each_set_bit_from(i, callid_bitmap, MAX_CALLID) {
 		sock = rcu_dereference(callid_sock[i]);
 		if (!sock)
 			continue;
@@ -209,7 +209,7 @@ static int pptp_xmit(struct ppp_channel *chan, struct sk_buff *skb)
 		}
 		if (skb->sk)
 			skb_set_owner_w(new_skb, skb->sk);
-		kfree_skb(skb);
+		consume_skb(skb);
 		skb = new_skb;
 	}
 
diff --git a/drivers/net/team/Kconfig b/drivers/net/team/Kconfig
index 248a144033c..89024d5fc33 100644
--- a/drivers/net/team/Kconfig
+++ b/drivers/net/team/Kconfig
@@ -40,4 +40,15 @@ config NET_TEAM_MODE_ACTIVEBACKUP
 	  To compile this team mode as a module, choose M here: the module
 	  will be called team_mode_activebackup.
 
+config NET_TEAM_MODE_LOADBALANCE
+	tristate "Load-balance mode support"
+	depends on NET_TEAM
+	---help---
+	  This mode provides load balancing functionality. Tx port selection
+	  is done using BPF function set up from userspace (bpf_hash_func
+	  option)
+
+	  To compile this team mode as a module, choose M here: the module
+	  will be called team_mode_loadbalance.
+
 endif # NET_TEAM
diff --git a/drivers/net/team/Makefile b/drivers/net/team/Makefile
index 85f2028a87a..fb9f4c1c51f 100644
--- a/drivers/net/team/Makefile
+++ b/drivers/net/team/Makefile
@@ -5,3 +5,4 @@
 obj-$(CONFIG_NET_TEAM) += team.o
 obj-$(CONFIG_NET_TEAM_MODE_ROUNDROBIN) += team_mode_roundrobin.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 8f81805c682..c61ae35a53c 100644
--- a/drivers/net/team/team.c
+++ b/drivers/net/team/team.c
@@ -65,7 +65,7 @@ static int __set_port_mac(struct net_device *port_dev,
 	return dev_set_mac_address(port_dev, &addr);
 }
 
-int team_port_set_orig_mac(struct team_port *port)
+static int team_port_set_orig_mac(struct team_port *port)
 {
 	return __set_port_mac(port->dev, port->orig.dev_addr);
 }
@@ -76,12 +76,26 @@ int team_port_set_team_mac(struct team_port *port)
 }
 EXPORT_SYMBOL(team_port_set_team_mac);
 
+static void team_refresh_port_linkup(struct team_port *port)
+{
+	port->linkup = port->user.linkup_enabled ? port->user.linkup :
+						   port->state.linkup;
+}
 
 /*******************
  * Options handling
  *******************/
 
-struct team_option *__team_find_option(struct team *team, const char *opt_name)
+struct team_option_inst { /* One for each option instance */
+	struct list_head list;
+	struct team_option *option;
+	struct team_port *port; /* != NULL if per-port */
+	bool changed;
+	bool removed;
+};
+
+static struct team_option *__team_find_option(struct team *team,
+					      const char *opt_name)
 {
 	struct team_option *option;
 
@@ -92,9 +106,121 @@ struct team_option *__team_find_option(struct team *team, const char *opt_name)
 	return NULL;
 }
 
-int __team_options_register(struct team *team,
-			    const struct team_option *option,
-			    size_t option_count)
+static int __team_option_inst_add(struct team *team, struct team_option *option,
+				  struct team_port *port)
+{
+	struct team_option_inst *opt_inst;
+
+	opt_inst = kmalloc(sizeof(*opt_inst), GFP_KERNEL);
+	if (!opt_inst)
+		return -ENOMEM;
+	opt_inst->option = option;
+	opt_inst->port = port;
+	opt_inst->changed = true;
+	opt_inst->removed = false;
+	list_add_tail(&opt_inst->list, &team->option_inst_list);
+	return 0;
+}
+
+static void __team_option_inst_del(struct team_option_inst *opt_inst)
+{
+	list_del(&opt_inst->list);
+	kfree(opt_inst);
+}
+
+static void __team_option_inst_del_option(struct team *team,
+					  struct team_option *option)
+{
+	struct team_option_inst *opt_inst, *tmp;
+
+	list_for_each_entry_safe(opt_inst, tmp, &team->option_inst_list, list) {
+		if (opt_inst->option == option)
+			__team_option_inst_del(opt_inst);
+	}
+}
+
+static int __team_option_inst_add_option(struct team *team,
+					 struct team_option *option)
+{
+	struct team_port *port;
+	int err;
+
+	if (!option->per_port)
+		return __team_option_inst_add(team, option, 0);
+
+	list_for_each_entry(port, &team->port_list, list) {
+		err = __team_option_inst_add(team, option, port);
+		if (err)
+			goto inst_del_option;
+	}
+	return 0;
+
+inst_del_option:
+	__team_option_inst_del_option(team, option);
+	return err;
+}
+
+static void __team_option_inst_mark_removed_option(struct team *team,
+						   struct team_option *option)
+{
+	struct team_option_inst *opt_inst;
+
+	list_for_each_entry(opt_inst, &team->option_inst_list, list) {
+		if (opt_inst->option == option) {
+			opt_inst->changed = true;
+			opt_inst->removed = true;
+		}
+	}
+}
+
+static void __team_option_inst_del_port(struct team *team,
+					struct team_port *port)
+{
+	struct team_option_inst *opt_inst, *tmp;
+
+	list_for_each_entry_safe(opt_inst, tmp, &team->option_inst_list, list) {
+		if (opt_inst->option->per_port &&
+		    opt_inst->port == port)
+			__team_option_inst_del(opt_inst);
+	}
+}
+
+static int __team_option_inst_add_port(struct team *team,
+				       struct team_port *port)
+{
+	struct team_option *option;
+	int err;
+
+	list_for_each_entry(option, &team->option_list, list) {
+		if (!option->per_port)
+			continue;
+		err = __team_option_inst_add(team, option, port);
+		if (err)
+			goto inst_del_port;
+	}
+	return 0;
+
+inst_del_port:
+	__team_option_inst_del_port(team, port);
+	return err;
+}
+
+static void __team_option_inst_mark_removed_port(struct team *team,
+						 struct team_port *port)
+{
+	struct team_option_inst *opt_inst;
+
+	list_for_each_entry(opt_inst, &team->option_inst_list, list) {
+		if (opt_inst->port == port) {
+			opt_inst->changed = true;
+			opt_inst->removed = true;
+		}
+	}
+}
+
+static int __team_options_register(struct team *team,
+				   const struct team_option *option,
+				   size_t option_count)
 {
 	int i;
 	struct team_option **dst_opts;
@@ -107,26 +233,32 @@ int __team_options_register(struct team *team,
 	for (i = 0; i < option_count; i++, option++) {
 		if (__team_find_option(team, option->name)) {
 			err = -EEXIST;
-			goto rollback;
+			goto alloc_rollback;
 		}
 		dst_opts[i] = kmemdup(option, sizeof(*option), GFP_KERNEL);
 		if (!dst_opts[i]) {
 			err = -ENOMEM;
-			goto rollback;
+			goto alloc_rollback;
 		}
 	}
 
 	for (i = 0; i < option_count; i++) {
-		dst_opts[i]->changed = true;
-		dst_opts[i]->removed = false;
+		err = __team_option_inst_add_option(team, dst_opts[i]);
+		if (err)
+			goto inst_rollback;
 		list_add_tail(&dst_opts[i]->list, &team->option_list);
 	}
 
 	kfree(dst_opts);
 	return 0;
 
-rollback:
-	for (i = 0; i < option_count; i++)
+inst_rollback:
+	for (i--; i >= 0; i--)
+		__team_option_inst_del_option(team, dst_opts[i]);
+
+	i = option_count - 1;
+alloc_rollback:
+	for (i--; i >= 0; i--)
 		kfree(dst_opts[i]);
 
 	kfree(dst_opts);
@@ -143,10 +275,8 @@ static void __team_options_mark_removed(struct team *team,
 		struct team_option *del_opt;
 
 		del_opt = __team_find_option(team, option->name);
-		if (del_opt) {
-			del_opt->changed = true;
-			del_opt->removed = true;
-		}
+		if (del_opt)
+			__team_option_inst_mark_removed_option(team, del_opt);
 	}
 }
 
@@ -161,6 +291,7 @@ static void __team_options_unregister(struct team *team,
 
 		del_opt = __team_find_option(team, option->name);
 		if (del_opt) {
+			__team_option_inst_del_option(team, del_opt);
 			list_del(&del_opt->list);
 			kfree(del_opt);
 		}
@@ -193,22 +324,42 @@ void team_options_unregister(struct team *team,
 }
 EXPORT_SYMBOL(team_options_unregister);
 
-static int team_option_get(struct team *team, struct team_option *option,
-			   void *arg)
+static int team_option_port_add(struct team *team, struct team_port *port)
+{
+	int err;
+
+	err = __team_option_inst_add_port(team, port);
+	if (err)
+		return err;
+	__team_options_change_check(team);
+	return 0;
+}
+
+static void team_option_port_del(struct team *team, struct team_port *port)
+{
+	__team_option_inst_mark_removed_port(team, port);
+	__team_options_change_check(team);
+	__team_option_inst_del_port(team, port);
+}
+
+static int team_option_get(struct team *team,
+			   struct team_option_inst *opt_inst,
+			   struct team_gsetter_ctx *ctx)
 {
-	return option->getter(team, arg);
+	return opt_inst->option->getter(team, ctx);
 }
 
-static int team_option_set(struct team *team, struct team_option *option,
-			   void *arg)
+static int team_option_set(struct team *team,
+			   struct team_option_inst *opt_inst,
+			   struct team_gsetter_ctx *ctx)
 {
 	int err;
 
-	err = option->setter(team, arg);
+	err = opt_inst->option->setter(team, ctx);
 	if (err)
 		return err;
 
-	option->changed = true;
+	opt_inst->changed = true;
 	__team_options_change_check(team);
 	return err;
 }
@@ -408,6 +559,8 @@ static int team_change_mode(struct team *team, const char *kind)
  * Rx path frame handler
  ************************/
 
+static bool team_port_enabled(struct team_port *port);
+
 /* note: already called with rcu_read_lock */
 static rx_handler_result_t team_handle_frame(struct sk_buff **pskb)
 {
@@ -424,8 +577,12 @@ static rx_handler_result_t team_handle_frame(struct sk_buff **pskb)
 
 	port = team_port_get_rcu(skb->dev);
 	team = port->team;
-
-	res = team->ops.receive(team, port, skb);
+	if (!team_port_enabled(port)) {
+		/* allow exact match delivery for disabled ports */
+		res = RX_HANDLER_EXACT;
+	} else {
+		res = team->ops.receive(team, port, skb);
+	}
 	if (res == RX_HANDLER_ANOTHER) {
 		struct team_pcpu_stats *pcpu_stats;
 
@@ -461,17 +618,25 @@ static bool team_port_find(const struct team *team,
 	return false;
 }
 
+static bool team_port_enabled(struct team_port *port)
+{
+	return port->index != -1;
+}
+
 /*
- * Add/delete port to the team port list. Write guarded by rtnl_lock.
- * Takes care of correct port->index setup (might be racy).
+ * Enable/disable port by adding to enabled port hashlist and setting
+ * port->index (Might be racy so reader could see incorrect ifindex when
+ * processing a flying packet, but that is not a problem). Write guarded
+ * by team->lock.
  */
-static void team_port_list_add_port(struct team *team,
-				    struct team_port *port)
+static void team_port_enable(struct team *team,
+			     struct team_port *port)
 {
-	port->index = team->port_count++;
+	if (team_port_enabled(port))
+		return;
+	port->index = team->en_port_count++;
 	hlist_add_head_rcu(&port->hlist,
 			   team_port_index_hash(team, port->index));
-	list_add_tail_rcu(&port->list, &team->port_list);
 }
 
 static void __reconstruct_port_hlist(struct team *team, int rm_index)
@@ -479,7 +644,7 @@ static void __reconstruct_port_hlist(struct team *team, int rm_index)
 	int i;
 	struct team_port *port;
 
-	for (i = rm_index + 1; i < team->port_count; i++) {
+	for (i = rm_index + 1; i < team->en_port_count; i++) {
 		port = team_get_port_by_index(team, i);
 		hlist_del_rcu(&port->hlist);
 		port->index--;
@@ -488,15 +653,17 @@ static void __reconstruct_port_hlist(struct team *team, int rm_index)
 	}
 }
 
-static void team_port_list_del_port(struct team *team,
-				   struct team_port *port)
+static void team_port_disable(struct team *team,
+			      struct team_port *port)
 {
 	int rm_index = port->index;
 
+	if (!team_port_enabled(port))
+		return;
 	hlist_del_rcu(&port->hlist);
-	list_del_rcu(&port->list);
 	__reconstruct_port_hlist(team, rm_index);
-	team->port_count--;
+	team->en_port_count--;
+	port->index = -1;
 }
 
 #define TEAM_VLAN_FEATURES (NETIF_F_ALL_CSUM | NETIF_F_SG | \
@@ -642,7 +809,16 @@ static int team_port_add(struct team *team, struct net_device *port_dev)
 		goto err_handler_register;
 	}
 
-	team_port_list_add_port(team, port);
+	err = team_option_port_add(team, port);
+	if (err) {
+		netdev_err(dev, "Device %s failed to add per-port options\n",
+			   portname);
+		goto err_option_port_add;
+	}
+
+	port->index = -1;
+	team_port_enable(team, port);
+	list_add_tail_rcu(&port->list, &team->port_list);
 	team_adjust_ops(team);
 	__team_compute_features(team);
 	__team_port_change_check(port, !!netif_carrier_ok(port_dev));
@@ -651,6 +827,9 @@ static int team_port_add(struct team *team, struct net_device *port_dev)
 
 	return 0;
 
+err_option_port_add:
+	netdev_rx_handler_unregister(port_dev);
+
 err_handler_register:
 	netdev_set_master(port_dev, NULL);
 
@@ -688,8 +867,10 @@ static int team_port_del(struct team *team, struct net_device *port_dev)
 
 	port->removed = true;
 	__team_port_change_check(port, false);
-	team_port_list_del_port(team, port);
+	team_port_disable(team, port);
+	list_del_rcu(&port->list);
 	team_adjust_ops(team);
+	team_option_port_del(team, port);
 	netdev_rx_handler_unregister(port_dev);
 	netdev_set_master(port_dev, NULL);
 	vlan_vids_del_by_dev(port_dev, dev);
@@ -712,19 +893,66 @@ static int team_port_del(struct team *team, struct net_device *port_dev)
 
 static const char team_no_mode_kind[] = "*NOMODE*";
 
-static int team_mode_option_get(struct team *team, void *arg)
+static int team_mode_option_get(struct team *team, struct team_gsetter_ctx *ctx)
+{
+	ctx->data.str_val = team->mode ? team->mode->kind : team_no_mode_kind;
+	return 0;
+}
+
+static int team_mode_option_set(struct team *team, struct team_gsetter_ctx *ctx)
+{
+	return team_change_mode(team, ctx->data.str_val);
+}
+
+static int team_port_en_option_get(struct team *team,
+				   struct team_gsetter_ctx *ctx)
+{
+	ctx->data.bool_val = team_port_enabled(ctx->port);
+	return 0;
+}
+
+static int team_port_en_option_set(struct team *team,
+				   struct team_gsetter_ctx *ctx)
+{
+	if (ctx->data.bool_val)
+		team_port_enable(team, ctx->port);
+	else
+		team_port_disable(team, ctx->port);
+	return 0;
+}
+
+static int team_user_linkup_option_get(struct team *team,
+				       struct team_gsetter_ctx *ctx)
+{
+	ctx->data.bool_val = ctx->port->user.linkup;
+	return 0;
+}
+
+static int team_user_linkup_option_set(struct team *team,
+				       struct team_gsetter_ctx *ctx)
+{
+	ctx->port->user.linkup = ctx->data.bool_val;
+	team_refresh_port_linkup(ctx->port);
+	return 0;
+}
+
+static int team_user_linkup_en_option_get(struct team *team,
+					  struct team_gsetter_ctx *ctx)
 {
-	const char **str = arg;
+	struct team_port *port = ctx->port;
 
-	*str = team->mode ? team->mode->kind : team_no_mode_kind;
+	ctx->data.bool_val = port->user.linkup_enabled;
 	return 0;
 }
 
-static int team_mode_option_set(struct team *team, void *arg)
+static int team_user_linkup_en_option_set(struct team *team,
+					  struct team_gsetter_ctx *ctx)
 {
-	const char **str = arg;
+	struct team_port *port = ctx->port;
 
-	return team_change_mode(team, *str);
+	port->user.linkup_enabled = ctx->data.bool_val;
+	team_refresh_port_linkup(ctx->port);
+	return 0;
 }
 
 static const struct team_option team_options[] = {
@@ -734,6 +962,27 @@ static const struct team_option team_options[] = {
 		.getter = team_mode_option_get,
 		.setter = team_mode_option_set,
 	},
+	{
+		.name = "enabled",
+		.type = TEAM_OPTION_TYPE_BOOL,
+		.per_port = true,
+		.getter = team_port_en_option_get,
+		.setter = team_port_en_option_set,
+	},
+	{
+		.name = "user_linkup",
+		.type = TEAM_OPTION_TYPE_BOOL,
+		.per_port = true,
+		.getter = team_user_linkup_option_get,
+		.setter = team_user_linkup_option_set,
+	},
+	{
+		.name = "user_linkup_enabled",
+		.type = TEAM_OPTION_TYPE_BOOL,
+		.per_port = true,
+		.getter = team_user_linkup_en_option_get,
+		.setter = team_user_linkup_en_option_set,
+	},
 };
 
 static int team_init(struct net_device *dev)
@@ -750,12 +999,13 @@ static int team_init(struct net_device *dev)
 		return -ENOMEM;
 
 	for (i = 0; i < TEAM_PORT_HASHENTRIES; i++)
-		INIT_HLIST_HEAD(&team->port_hlist[i]);
+		INIT_HLIST_HEAD(&team->en_port_hlist[i]);
 	INIT_LIST_HEAD(&team->port_list);
 
 	team_adjust_ops(team);
 
 	INIT_LIST_HEAD(&team->option_list);
+	INIT_LIST_HEAD(&team->option_inst_list);
 	err = team_options_register(team, team_options, ARRAY_SIZE(team_options));
 	if (err)
 		goto err_options_register;
@@ -1145,10 +1395,7 @@ team_nl_option_policy[TEAM_ATTR_OPTION_MAX + 1] = {
 	},
 	[TEAM_ATTR_OPTION_CHANGED]		= { .type = NLA_FLAG },
 	[TEAM_ATTR_OPTION_TYPE]			= { .type = NLA_U8 },
-	[TEAM_ATTR_OPTION_DATA] = {
-		.type = NLA_BINARY,
-		.len = TEAM_STRING_MAX_LEN,
-	},
+	[TEAM_ATTR_OPTION_DATA]			= { .type = NLA_BINARY },
 };
 
 static int team_nl_cmd_noop(struct sk_buff *skb, struct genl_info *info)
@@ -1241,46 +1488,86 @@ static int team_nl_fill_options_get(struct sk_buff *skb,
 {
 	struct nlattr *option_list;
 	void *hdr;
-	struct team_option *option;
+	struct team_option_inst *opt_inst;
+	int err;
 
 	hdr = genlmsg_put(skb, pid, seq, &team_nl_family, flags,
 			  TEAM_CMD_OPTIONS_GET);
 	if (IS_ERR(hdr))
 		return PTR_ERR(hdr);
 
-	NLA_PUT_U32(skb, TEAM_ATTR_TEAM_IFINDEX, team->dev->ifindex);
+	if (nla_put_u32(skb, TEAM_ATTR_TEAM_IFINDEX, team->dev->ifindex))
+		goto nla_put_failure;
 	option_list = nla_nest_start(skb, TEAM_ATTR_LIST_OPTION);
 	if (!option_list)
 		return -EMSGSIZE;
 
-	list_for_each_entry(option, &team->option_list, list) {
+	list_for_each_entry(opt_inst, &team->option_inst_list, list) {
 		struct nlattr *option_item;
-		long arg;
+		struct team_option *option = opt_inst->option;
+		struct team_gsetter_ctx ctx;
 
 		/* Include only changed options if fill all mode is not on */
-		if (!fillall && !option->changed)
+		if (!fillall && !opt_inst->changed)
 			continue;
 		option_item = nla_nest_start(skb, TEAM_ATTR_ITEM_OPTION);
 		if (!option_item)
 			goto nla_put_failure;
-		NLA_PUT_STRING(skb, TEAM_ATTR_OPTION_NAME, option->name);
-		if (option->changed) {
-			NLA_PUT_FLAG(skb, TEAM_ATTR_OPTION_CHANGED);
-			option->changed = false;
+		if (nla_put_string(skb, TEAM_ATTR_OPTION_NAME, option->name))
+			goto nla_put_failure;
+		if (opt_inst->changed) {
+			if (nla_put_flag(skb, TEAM_ATTR_OPTION_CHANGED))
+				goto nla_put_failure;
+			opt_inst->changed = false;
 		}
-		if (option->removed)
-			NLA_PUT_FLAG(skb, TEAM_ATTR_OPTION_REMOVED);
+		if (opt_inst->removed &&
+		    nla_put_flag(skb, TEAM_ATTR_OPTION_REMOVED))
+			goto nla_put_failure;
+		if (opt_inst->port &&
+		    nla_put_u32(skb, TEAM_ATTR_OPTION_PORT_IFINDEX,
+				opt_inst->port->dev->ifindex))
+			goto nla_put_failure;
+		ctx.port = opt_inst->port;
 		switch (option->type) {
 		case TEAM_OPTION_TYPE_U32:
-			NLA_PUT_U8(skb, TEAM_ATTR_OPTION_TYPE, NLA_U32);
-			team_option_get(team, option, &arg);
-			NLA_PUT_U32(skb, TEAM_ATTR_OPTION_DATA, arg);
+			if (nla_put_u8(skb, TEAM_ATTR_OPTION_TYPE, NLA_U32))
+				goto nla_put_failure;
+			err = team_option_get(team, opt_inst, &ctx);
+			if (err)
+				goto errout;
+			if (nla_put_u32(skb, TEAM_ATTR_OPTION_DATA,
+					ctx.data.u32_val))
+				goto nla_put_failure;
 			break;
 		case TEAM_OPTION_TYPE_STRING:
-			NLA_PUT_U8(skb, TEAM_ATTR_OPTION_TYPE, NLA_STRING);
-			team_option_get(team, option, &arg);
-			NLA_PUT_STRING(skb, TEAM_ATTR_OPTION_DATA,
-				       (char *) arg);
+			if (nla_put_u8(skb, TEAM_ATTR_OPTION_TYPE, NLA_STRING))
+				goto nla_put_failure;
+			err = team_option_get(team, opt_inst, &ctx);
+			if (err)
+				goto errout;
+			if (nla_put_string(skb, TEAM_ATTR_OPTION_DATA,
+					   ctx.data.str_val))
+				goto nla_put_failure;
+			break;
+		case TEAM_OPTION_TYPE_BINARY:
+			if (nla_put_u8(skb, TEAM_ATTR_OPTION_TYPE, NLA_BINARY))
+				goto nla_put_failure;
+			err = team_option_get(team, opt_inst, &ctx);
+			if (err)
+				goto errout;
+			if (nla_put(skb, TEAM_ATTR_OPTION_DATA,
+				    ctx.data.bin_val.len, ctx.data.bin_val.ptr))
+				goto nla_put_failure;
+			break;
+		case TEAM_OPTION_TYPE_BOOL:
+			if (nla_put_u8(skb, TEAM_ATTR_OPTION_TYPE, NLA_FLAG))
+				goto nla_put_failure;
+			err = team_option_get(team, opt_inst, &ctx);
+			if (err)
+				goto errout;
+			if (ctx.data.bool_val &&
+			    nla_put_flag(skb, TEAM_ATTR_OPTION_DATA))
+				goto nla_put_failure;
 			break;
 		default:
 			BUG();
@@ -1292,8 +1579,10 @@ static int team_nl_fill_options_get(struct sk_buff *skb,
 	return genlmsg_end(skb, hdr);
 
 nla_put_failure:
+	err = -EMSGSIZE;
+errout:
 	genlmsg_cancel(skb, hdr);
-	return -EMSGSIZE;
+	return err;
 }
 
 static int team_nl_fill_options_get_all(struct sk_buff *skb,
@@ -1339,9 +1628,12 @@ static int team_nl_cmd_options_set(struct sk_buff *skb, struct genl_info *info)
 	}
 
 	nla_for_each_nested(nl_option, info->attrs[TEAM_ATTR_LIST_OPTION], i) {
-		struct nlattr *mode_attrs[TEAM_ATTR_OPTION_MAX + 1];
+		struct nlattr *opt_attrs[TEAM_ATTR_OPTION_MAX + 1];
+		struct nlattr *attr_port_ifindex;
+		struct nlattr *attr_data;
 		enum team_option_type opt_type;
-		struct team_option *option;
+		int opt_port_ifindex = 0; /* != 0 for per-port options */
+		struct team_option_inst *opt_inst;
 		char *opt_name;
 		bool opt_found = false;
 
@@ -1349,48 +1641,78 @@ static int team_nl_cmd_options_set(struct sk_buff *skb, struct genl_info *info)
 			err = -EINVAL;
 			goto team_put;
 		}
-		err = nla_parse_nested(mode_attrs, TEAM_ATTR_OPTION_MAX,
+		err = nla_parse_nested(opt_attrs, TEAM_ATTR_OPTION_MAX,
 				       nl_option, team_nl_option_policy);
 		if (err)
 			goto team_put;
-		if (!mode_attrs[TEAM_ATTR_OPTION_NAME] ||
-		    !mode_attrs[TEAM_ATTR_OPTION_TYPE] ||
-		    !mode_attrs[TEAM_ATTR_OPTION_DATA]) {
+		if (!opt_attrs[TEAM_ATTR_OPTION_NAME] ||
+		    !opt_attrs[TEAM_ATTR_OPTION_TYPE]) {
 			err = -EINVAL;
 			goto team_put;
 		}
-		switch (nla_get_u8(mode_attrs[TEAM_ATTR_OPTION_TYPE])) {
+		switch (nla_get_u8(opt_attrs[TEAM_ATTR_OPTION_TYPE])) {
 		case NLA_U32:
 			opt_type = TEAM_OPTION_TYPE_U32;
 			break;
 		case NLA_STRING:
 			opt_type = TEAM_OPTION_TYPE_STRING;
 			break;
+		case NLA_BINARY:
+			opt_type = TEAM_OPTION_TYPE_BINARY;
+			break;
+		case NLA_FLAG:
+			opt_type = TEAM_OPTION_TYPE_BOOL;
+			break;
 		default:
 			goto team_put;
 		}
 
-		opt_name = nla_data(mode_attrs[TEAM_ATTR_OPTION_NAME]);
-		list_for_each_entry(option, &team->option_list, list) {
-			long arg;
-			struct nlattr *opt_data_attr;
+		attr_data = opt_attrs[TEAM_ATTR_OPTION_DATA];
+		if (opt_type != TEAM_OPTION_TYPE_BOOL && !attr_data) {
+			err = -EINVAL;
+			goto team_put;
+		}
+
+		opt_name = nla_data(opt_attrs[TEAM_ATTR_OPTION_NAME]);
+		attr_port_ifindex = opt_attrs[TEAM_ATTR_OPTION_PORT_IFINDEX];
+		if (attr_port_ifindex)
+			opt_port_ifindex = nla_get_u32(attr_port_ifindex);
+
+		list_for_each_entry(opt_inst, &team->option_inst_list, list) {
+			struct team_option *option = opt_inst->option;
+			struct team_gsetter_ctx ctx;
+			int tmp_ifindex;
 
+			tmp_ifindex = opt_inst->port ?
+				      opt_inst->port->dev->ifindex : 0;
 			if (option->type != opt_type ||
-			    strcmp(option->name, opt_name))
+			    strcmp(option->name, opt_name) ||
+			    tmp_ifindex != opt_port_ifindex)
 				continue;
 			opt_found = true;
-			opt_data_attr = mode_attrs[TEAM_ATTR_OPTION_DATA];
+			ctx.port = opt_inst->port;
 			switch (opt_type) {
 			case TEAM_OPTION_TYPE_U32:
-				arg = nla_get_u32(opt_data_attr);
+				ctx.data.u32_val = nla_get_u32(attr_data);
 				break;
 			case TEAM_OPTION_TYPE_STRING:
-				arg = (long) nla_data(opt_data_attr);
+				if (nla_len(attr_data) > TEAM_STRING_MAX_LEN) {
+					err = -EINVAL;
+					goto team_put;
+				}
+				ctx.data.str_val = nla_data(attr_data);
+				break;
+			case TEAM_OPTION_TYPE_BINARY:
+				ctx.data.bin_val.len = nla_len(attr_data);
+				ctx.data.bin_val.ptr = nla_data(attr_data);
+				break;
+			case TEAM_OPTION_TYPE_BOOL:
+				ctx.data.bool_val = attr_data ? true : false;
 				break;
 			default:
 				BUG();
 			}
-			err = team_option_set(team, option, &arg);
+			err = team_option_set(team, opt_inst, &ctx);
 			if (err)
 				goto team_put;
 		}
@@ -1420,7 +1742,8 @@ static int team_nl_fill_port_list_get(struct sk_buff *skb,
 	if (IS_ERR(hdr))
 		return PTR_ERR(hdr);
 
-	NLA_PUT_U32(skb, TEAM_ATTR_TEAM_IFINDEX, team->dev->ifindex);
+	if (nla_put_u32(skb, TEAM_ATTR_TEAM_IFINDEX, team->dev->ifindex))
+		goto nla_put_failure;
 	port_list = nla_nest_start(skb, TEAM_ATTR_LIST_PORT);
 	if (!port_list)
 		return -EMSGSIZE;
@@ -1434,17 +1757,20 @@ static int team_nl_fill_port_list_get(struct sk_buff *skb,
 		port_item = nla_nest_start(skb, TEAM_ATTR_ITEM_PORT);
 		if (!port_item)
 			goto nla_put_failure;
-		NLA_PUT_U32(skb, TEAM_ATTR_PORT_IFINDEX, port->dev->ifindex);
+		if (nla_put_u32(skb, TEAM_ATTR_PORT_IFINDEX, port->dev->ifindex))
+			goto nla_put_failure;
 		if (port->changed) {
-			NLA_PUT_FLAG(skb, TEAM_ATTR_PORT_CHANGED);
+			if (nla_put_flag(skb, TEAM_ATTR_PORT_CHANGED))
+				goto nla_put_failure;
 			port->changed = false;
 		}
-		if (port->removed)
-			NLA_PUT_FLAG(skb, TEAM_ATTR_PORT_REMOVED);
-		if (port->linkup)
-			NLA_PUT_FLAG(skb, TEAM_ATTR_PORT_LINKUP);
-		NLA_PUT_U32(skb, TEAM_ATTR_PORT_SPEED, port->speed);
-		NLA_PUT_U8(skb, TEAM_ATTR_PORT_DUPLEX, port->duplex);
+		if ((port->removed &&
+		     nla_put_flag(skb, TEAM_ATTR_PORT_REMOVED)) ||
+		    (port->state.linkup &&
+		     nla_put_flag(skb, TEAM_ATTR_PORT_LINKUP)) ||
+		    nla_put_u32(skb, TEAM_ATTR_PORT_SPEED, port->state.speed) ||
+		    nla_put_u8(skb, TEAM_ATTR_PORT_DUPLEX, port->state.duplex))
+			goto nla_put_failure;
 		nla_nest_end(skb, port_item);
 	}
 
@@ -1603,23 +1929,24 @@ static void __team_port_change_check(struct team_port *port, bool linkup)
 {
 	int err;
 
-	if (!port->removed && port->linkup == linkup)
+	if (!port->removed && port->state.linkup == linkup)
 		return;
 
 	port->changed = true;
-	port->linkup = linkup;
+	port->state.linkup = linkup;
+	team_refresh_port_linkup(port);
 	if (linkup) {
 		struct ethtool_cmd ecmd;
 
 		err = __ethtool_get_settings(port->dev, &ecmd);
 		if (!err) {
-			port->speed = ethtool_cmd_speed(&ecmd);
-			port->duplex = ecmd.duplex;
+			port->state.speed = ethtool_cmd_speed(&ecmd);
+			port->state.duplex = ecmd.duplex;
 			goto send_event;
 		}
 	}
-	port->speed = 0;
-	port->duplex = 0;
+	port->state.speed = 0;
+	port->state.duplex = 0;
 
 send_event:
 	err = team_nl_send_event_port_list_get(port->team);
diff --git a/drivers/net/team/team_mode_activebackup.c b/drivers/net/team/team_mode_activebackup.c
index f4d960e82e2..fd6bd03aaa8 100644
--- a/drivers/net/team/team_mode_activebackup.c
+++ b/drivers/net/team/team_mode_activebackup.c
@@ -59,23 +59,21 @@ static void ab_port_leave(struct team *team, struct team_port *port)
 		RCU_INIT_POINTER(ab_priv(team)->active_port, NULL);
 }
 
-static int ab_active_port_get(struct team *team, void *arg)
+static int ab_active_port_get(struct team *team, struct team_gsetter_ctx *ctx)
 {
-	u32 *ifindex = arg;
-
-	*ifindex = 0;
 	if (ab_priv(team)->active_port)
-		*ifindex = ab_priv(team)->active_port->dev->ifindex;
+		ctx->data.u32_val = ab_priv(team)->active_port->dev->ifindex;
+	else
+		ctx->data.u32_val = 0;
 	return 0;
 }
 
-static int ab_active_port_set(struct team *team, void *arg)
+static int ab_active_port_set(struct team *team, struct team_gsetter_ctx *ctx)
 {
-	u32 *ifindex = arg;
 	struct team_port *port;
 
-	list_for_each_entry_rcu(port, &team->port_list, list) {
-		if (port->dev->ifindex == *ifindex) {
+	list_for_each_entry(port, &team->port_list, list) {
+		if (port->dev->ifindex == ctx->data.u32_val) {
 			rcu_assign_pointer(ab_priv(team)->active_port, port);
 			return 0;
 		}
@@ -92,12 +90,12 @@ static const struct team_option ab_options[] = {
 	},
 };
 
-int ab_init(struct team *team)
+static int ab_init(struct team *team)
 {
 	return team_options_register(team, ab_options, ARRAY_SIZE(ab_options));
 }
 
-void ab_exit(struct team *team)
+static void ab_exit(struct team *team)
 {
 	team_options_unregister(team, ab_options, ARRAY_SIZE(ab_options));
 }
diff --git a/drivers/net/team/team_mode_loadbalance.c b/drivers/net/team/team_mode_loadbalance.c
new file mode 100644
index 00000000000..86e8183c8e3
--- /dev/null
+++ b/drivers/net/team/team_mode_loadbalance.c
@@ -0,0 +1,174 @@
+/*
+ * drivers/net/team/team_mode_loadbalance.c - Load-balancing mode for team
+ * Copyright (c) 2012 Jiri Pirko <jpirko@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.
+ */
+
+#include <linux/kernel.h>
+#include <linux/types.h>
+#include <linux/module.h>
+#include <linux/init.h>
+#include <linux/errno.h>
+#include <linux/netdevice.h>
+#include <linux/filter.h>
+#include <linux/if_team.h>
+
+struct lb_priv {
+	struct sk_filter __rcu *fp;
+	struct sock_fprog *orig_fprog;
+};
+
+static struct lb_priv *lb_priv(struct team *team)
+{
+	return (struct lb_priv *) &team->mode_priv;
+}
+
+static bool lb_transmit(struct team *team, struct sk_buff *skb)
+{
+	struct sk_filter *fp;
+	struct team_port *port;
+	unsigned int hash;
+	int port_index;
+
+	fp = rcu_dereference(lb_priv(team)->fp);
+	if (unlikely(!fp))
+		goto drop;
+	hash = SK_RUN_FILTER(fp, skb);
+	port_index = hash % team->en_port_count;
+	port = team_get_port_by_index_rcu(team, port_index);
+	if (unlikely(!port))
+		goto drop;
+	skb->dev = port->dev;
+	if (dev_queue_xmit(skb))
+		return false;
+	return true;
+
+drop:
+	dev_kfree_skb_any(skb);
+	return false;
+}
+
+static int lb_bpf_func_get(struct team *team, struct team_gsetter_ctx *ctx)
+{
+	if (!lb_priv(team)->orig_fprog) {
+		ctx->data.bin_val.len = 0;
+		ctx->data.bin_val.ptr = NULL;
+		return 0;
+	}
+	ctx->data.bin_val.len = lb_priv(team)->orig_fprog->len *
+				sizeof(struct sock_filter);
+	ctx->data.bin_val.ptr = lb_priv(team)->orig_fprog->filter;
+	return 0;
+}
+
+static int __fprog_create(struct sock_fprog **pfprog, u32 data_len,
+			  const void *data)
+{
+	struct sock_fprog *fprog;
+	struct sock_filter *filter = (struct sock_filter *) data;
+
+	if (data_len % sizeof(struct sock_filter))
+		return -EINVAL;
+	fprog = kmalloc(sizeof(struct sock_fprog), GFP_KERNEL);
+	if (!fprog)
+		return -ENOMEM;
+	fprog->filter = kmemdup(filter, data_len, GFP_KERNEL);
+	if (!fprog->filter) {
+		kfree(fprog);
+		return -ENOMEM;
+	}
+	fprog->len = data_len / sizeof(struct sock_filter);
+	*pfprog = fprog;
+	return 0;
+}
+
+static void __fprog_destroy(struct sock_fprog *fprog)
+{
+	kfree(fprog->filter);
+	kfree(fprog);
+}
+
+static int lb_bpf_func_set(struct team *team, struct team_gsetter_ctx *ctx)
+{
+	struct sk_filter *fp = NULL;
+	struct sock_fprog *fprog = NULL;
+	int err;
+
+	if (ctx->data.bin_val.len) {
+		err = __fprog_create(&fprog, ctx->data.bin_val.len,
+				     ctx->data.bin_val.ptr);
+		if (err)
+			return err;
+		err = sk_unattached_filter_create(&fp, fprog);
+		if (err) {
+			__fprog_destroy(fprog);
+			return err;
+		}
+	}
+
+	if (lb_priv(team)->orig_fprog) {
+		/* Clear old filter data */
+		__fprog_destroy(lb_priv(team)->orig_fprog);
+		sk_unattached_filter_destroy(lb_priv(team)->fp);
+	}
+
+	rcu_assign_pointer(lb_priv(team)->fp, fp);
+	lb_priv(team)->orig_fprog = fprog;
+	return 0;
+}
+
+static const struct team_option lb_options[] = {
+	{
+		.name = "bpf_hash_func",
+		.type = TEAM_OPTION_TYPE_BINARY,
+		.getter = lb_bpf_func_get,
+		.setter = lb_bpf_func_set,
+	},
+};
+
+static int lb_init(struct team *team)
+{
+	return team_options_register(team, lb_options,
+				     ARRAY_SIZE(lb_options));
+}
+
+static void lb_exit(struct team *team)
+{
+	team_options_unregister(team, lb_options,
+				ARRAY_SIZE(lb_options));
+}
+
+static const struct team_mode_ops lb_mode_ops = {
+	.init			= lb_init,
+	.exit			= lb_exit,
+	.transmit		= lb_transmit,
+};
+
+static struct team_mode lb_mode = {
+	.kind		= "loadbalance",
+	.owner		= THIS_MODULE,
+	.priv_size	= sizeof(struct lb_priv),
+	.ops		= &lb_mode_ops,
+};
+
+static int __init lb_init_module(void)
+{
+	return team_mode_register(&lb_mode);
+}
+
+static void __exit lb_cleanup_module(void)
+{
+	team_mode_unregister(&lb_mode);
+}
+
+module_init(lb_init_module);
+module_exit(lb_cleanup_module);
+
+MODULE_LICENSE("GPL v2");
+MODULE_AUTHOR("Jiri Pirko <jpirko@redhat.com>");
+MODULE_DESCRIPTION("Load-balancing mode for team");
+MODULE_ALIAS("team-mode-loadbalance");
diff --git a/drivers/net/team/team_mode_roundrobin.c b/drivers/net/team/team_mode_roundrobin.c
index a0e8f806331..6abfbdc96be 100644
--- a/drivers/net/team/team_mode_roundrobin.c
+++ b/drivers/net/team/team_mode_roundrobin.c
@@ -50,7 +50,7 @@ static bool rr_transmit(struct team *team, struct sk_buff *skb)
 	struct team_port *port;
 	int port_index;
 
-	port_index = rr_priv(team)->sent_packets++ % team->port_count;
+	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);
 	if (unlikely(!port))
diff --git a/drivers/net/tokenring/3c359.c b/drivers/net/tokenring/3c359.c
deleted file mode 100644
index b15ac81d46f..00000000000
--- a/drivers/net/tokenring/3c359.c
+++ /dev/null
@@ -1,1843 +0,0 @@
-/*
- *   3c359.c (c) 2000 Mike Phillips (mikep@linuxtr.net) All Rights Reserved
- *
- *  Linux driver for 3Com 3c359 Tokenlink Velocity XL PCI NIC
- *
- *  Base Driver Olympic:
- *	Written 1999 Peter De Schrijver & Mike Phillips
- *
- *  This software may be used and distributed according to the terms
- *  of the GNU General Public License, incorporated herein by reference.
- * 
- *  7/17/00 - Clean up, version number 0.9.0. Ready to release to the world.
- *
- *  2/16/01 - Port up to kernel 2.4.2 ready for submission into the kernel.
- *  3/05/01 - Last clean up stuff before submission.
- *  2/15/01 - Finally, update to new pci api. 
- *
- *  To Do:
- */
-
-/* 
- *	Technical Card Details
- *
- *  All access to data is done with 16/8 bit transfers.  The transfer
- *  method really sucks. You can only read or write one location at a time.
- *
- *  Also, the microcode for the card must be uploaded if the card does not have
- *  the flashrom on board.  This is a 28K bloat in the driver when compiled
- *  as a module.
- *
- *  Rx is very simple, status into a ring of descriptors, dma data transfer,
- *  interrupts to tell us when a packet is received.
- *
- *  Tx is a little more interesting. Similar scenario, descriptor and dma data
- *  transfers, but we don't have to interrupt the card to tell it another packet
- *  is ready for transmission, we are just doing simple memory writes, not io or mmio
- *  writes.  The card can be set up to simply poll on the next
- *  descriptor pointer and when this value is non-zero will automatically download
- *  the next packet.  The card then interrupts us when the packet is done.
- *
- */
-
-#define XL_DEBUG 0
-
-#include <linux/jiffies.h>
-#include <linux/module.h>
-#include <linux/kernel.h>
-#include <linux/errno.h>
-#include <linux/timer.h>
-#include <linux/in.h>
-#include <linux/ioport.h>
-#include <linux/string.h>
-#include <linux/proc_fs.h>
-#include <linux/ptrace.h>
-#include <linux/skbuff.h>
-#include <linux/interrupt.h>
-#include <linux/delay.h>
-#include <linux/netdevice.h>
-#include <linux/trdevice.h>
-#include <linux/stddef.h>
-#include <linux/init.h>
-#include <linux/pci.h>
-#include <linux/spinlock.h>
-#include <linux/bitops.h>
-#include <linux/firmware.h>
-#include <linux/slab.h>
-
-#include <net/checksum.h>
-
-#include <asm/io.h>
-
-#include "3c359.h"
-
-static char version[] __devinitdata  = 
-"3c359.c v1.2.0 2/17/01 - Mike Phillips (mikep@linuxtr.net)" ; 
-
-#define FW_NAME		"3com/3C359.bin"
-MODULE_AUTHOR("Mike Phillips <mikep@linuxtr.net>") ; 
-MODULE_DESCRIPTION("3Com 3C359 Velocity XL Token Ring Adapter Driver\n") ;
-MODULE_FIRMWARE(FW_NAME);
-
-/* Module parameters */
-
-/* Ring Speed 0,4,16 
- * 0 = Autosense   
- * 4,16 = Selected speed only, no autosense
- * This allows the card to be the first on the ring
- * and become the active monitor.
- *
- * WARNING: Some hubs will allow you to insert
- * at the wrong speed.
- * 
- * The adapter will _not_ fail to open if there are no
- * active monitors on the ring, it will simply open up in 
- * its last known ringspeed if no ringspeed is specified.
- */
-
-static int ringspeed[XL_MAX_ADAPTERS] = {0,} ;
-
-module_param_array(ringspeed, int, NULL, 0);
-MODULE_PARM_DESC(ringspeed,"3c359: Ringspeed selection - 4,16 or 0") ;
-
-/* Packet buffer size */
-
-static int pkt_buf_sz[XL_MAX_ADAPTERS] = {0,} ;
- 
-module_param_array(pkt_buf_sz, int, NULL, 0) ;
-MODULE_PARM_DESC(pkt_buf_sz,"3c359: Initial buffer size") ;
-/* Message Level */
-
-static int message_level[XL_MAX_ADAPTERS] = {0,} ;
-
-module_param_array(message_level, int, NULL, 0) ;
-MODULE_PARM_DESC(message_level, "3c359: Level of reported messages") ;
-/* 
- *	This is a real nasty way of doing this, but otherwise you
- *	will be stuck with 1555 lines of hex #'s in the code.
- */
-
-static DEFINE_PCI_DEVICE_TABLE(xl_pci_tbl) =
-{
-	{PCI_VENDOR_ID_3COM,PCI_DEVICE_ID_3COM_3C359, PCI_ANY_ID, PCI_ANY_ID, },
-	{ }			/* terminate list */
-};
-MODULE_DEVICE_TABLE(pci,xl_pci_tbl) ; 
-
-static int xl_init(struct net_device *dev);
-static int xl_open(struct net_device *dev);
-static int xl_open_hw(struct net_device *dev) ;  
-static int xl_hw_reset(struct net_device *dev); 
-static netdev_tx_t xl_xmit(struct sk_buff *skb, struct net_device *dev);
-static void xl_dn_comp(struct net_device *dev); 
-static int xl_close(struct net_device *dev);
-static void xl_set_rx_mode(struct net_device *dev);
-static irqreturn_t xl_interrupt(int irq, void *dev_id);
-static int xl_set_mac_address(struct net_device *dev, void *addr) ; 
-static void xl_arb_cmd(struct net_device *dev);
-static void xl_asb_cmd(struct net_device *dev) ; 
-static void xl_srb_cmd(struct net_device *dev, int srb_cmd) ; 
-static void xl_wait_misr_flags(struct net_device *dev) ; 
-static int xl_change_mtu(struct net_device *dev, int mtu);
-static void xl_srb_bh(struct net_device *dev) ; 
-static void xl_asb_bh(struct net_device *dev) ; 
-static void xl_reset(struct net_device *dev) ;  
-static void xl_freemem(struct net_device *dev) ;  
-
-
-/* EEProm Access Functions */
-static u16  xl_ee_read(struct net_device *dev, int ee_addr) ; 
-static void  xl_ee_write(struct net_device *dev, int ee_addr, u16 ee_value) ; 
-
-/* Debugging functions */
-#if XL_DEBUG
-static void print_tx_state(struct net_device *dev) ; 
-static void print_rx_state(struct net_device *dev) ; 
-
-static void print_tx_state(struct net_device *dev)
-{
-
-	struct xl_private *xl_priv = netdev_priv(dev);
-	struct xl_tx_desc *txd ; 
-	u8 __iomem *xl_mmio = xl_priv->xl_mmio ; 
-	int i ; 
-
-	printk("tx_ring_head: %d, tx_ring_tail: %d, free_ent: %d\n",xl_priv->tx_ring_head,
-		xl_priv->tx_ring_tail, xl_priv->free_ring_entries) ; 
-	printk("Ring    , Address ,   FSH  , DnNextPtr, Buffer, Buffer_Len\n");
-	for (i = 0; i < 16; i++) {
-		txd = &(xl_priv->xl_tx_ring[i]) ; 
-		printk("%d, %08lx, %08x, %08x, %08x, %08x\n", i, virt_to_bus(txd),
-			txd->framestartheader, txd->dnnextptr, txd->buffer, txd->buffer_length ) ; 
-	}
-
-	printk("DNLISTPTR = %04x\n", readl(xl_mmio + MMIO_DNLISTPTR) );
-	
-	printk("DmaCtl = %04x\n", readl(xl_mmio + MMIO_DMA_CTRL) );
-	printk("Queue status = %0x\n",netif_running(dev) ) ;
-}
-
-static void print_rx_state(struct net_device *dev)
-{
-
-	struct xl_private *xl_priv = netdev_priv(dev);
-	struct xl_rx_desc *rxd ; 
-	u8 __iomem *xl_mmio = xl_priv->xl_mmio ; 
-	int i ; 
-
-	printk("rx_ring_tail: %d\n", xl_priv->rx_ring_tail);
-	printk("Ring    , Address ,   FrameState  , UPNextPtr, FragAddr, Frag_Len\n");
-	for (i = 0; i < 16; i++) { 
-		/* rxd = (struct xl_rx_desc *)xl_priv->rx_ring_dma_addr + (i * sizeof(struct xl_rx_desc)) ; */
-		rxd = &(xl_priv->xl_rx_ring[i]) ; 
-		printk("%d, %08lx, %08x, %08x, %08x, %08x\n", i, virt_to_bus(rxd),
-			rxd->framestatus, rxd->upnextptr, rxd->upfragaddr, rxd->upfraglen ) ; 
-	}
-
-	printk("UPLISTPTR = %04x\n", readl(xl_mmio + MMIO_UPLISTPTR));
-	
-	printk("DmaCtl = %04x\n", readl(xl_mmio + MMIO_DMA_CTRL));
-	printk("Queue status = %0x\n",netif_running(dev));
-} 
-#endif
-
-/*
- *	Read values from the on-board EEProm.  This looks very strange
- *	but you have to wait for the EEProm to get/set the value before 
- *	passing/getting the next value from the nic. As with all requests
- *	on this nic it has to be done in two stages, a) tell the nic which
- *	memory address you want to access and b) pass/get the value from the nic.
- *	With the EEProm, you have to wait before and between access a) and b).
- *	As this is only read at initialization time and the wait period is very 
- *	small we shouldn't have to worry about scheduling issues.
- */
-
-static u16 xl_ee_read(struct net_device *dev, int ee_addr)
-{ 
-	struct xl_private *xl_priv = netdev_priv(dev);
-	u8 __iomem *xl_mmio = xl_priv->xl_mmio ; 
-
-	/* Wait for EEProm to not be busy */
-	writel(IO_WORD_READ | EECONTROL, xl_mmio + MMIO_MAC_ACCESS_CMD) ; 
-	while ( readw(xl_mmio + MMIO_MACDATA) & EEBUSY ) ;
-
-	/* Tell EEProm what we want to do and where */
-	writel(IO_WORD_WRITE | EECONTROL, xl_mmio + MMIO_MAC_ACCESS_CMD) ; 
-	writew(EEREAD + ee_addr, xl_mmio + MMIO_MACDATA) ; 
-
-	/* Wait for EEProm to not be busy */
-	writel(IO_WORD_READ | EECONTROL, xl_mmio + MMIO_MAC_ACCESS_CMD) ; 
-	while ( readw(xl_mmio + MMIO_MACDATA) & EEBUSY ) ; 
-	
-	/* Tell EEProm what we want to do and where */
-	writel(IO_WORD_WRITE | EECONTROL , xl_mmio + MMIO_MAC_ACCESS_CMD) ; 
-	writew(EEREAD + ee_addr, xl_mmio + MMIO_MACDATA) ; 
-
-	/* Finally read the value from the EEProm */
-	writel(IO_WORD_READ | EEDATA , xl_mmio + MMIO_MAC_ACCESS_CMD) ; 
-	return readw(xl_mmio + MMIO_MACDATA) ; 
-}
-
-/* 
- *	Write values to the onboard eeprom. As with eeprom read you need to 
- *	set which location to write, wait, value to write, wait, with the 
- *	added twist of having to enable eeprom writes as well.
- */
-
-static void  xl_ee_write(struct net_device *dev, int ee_addr, u16 ee_value) 
-{
-	struct xl_private *xl_priv = netdev_priv(dev);
-	u8 __iomem *xl_mmio = xl_priv->xl_mmio ; 
-
-	/* Wait for EEProm to not be busy */
-	writel(IO_WORD_READ | EECONTROL, xl_mmio + MMIO_MAC_ACCESS_CMD) ; 
-	while ( readw(xl_mmio + MMIO_MACDATA) & EEBUSY ) ;
-	
-	/* Enable write/erase */
-	writel(IO_WORD_WRITE | EECONTROL, xl_mmio + MMIO_MAC_ACCESS_CMD) ; 
-	writew(EE_ENABLE_WRITE, xl_mmio + MMIO_MACDATA) ; 
-
-	/* Wait for EEProm to not be busy */
-	writel(IO_WORD_READ | EECONTROL, xl_mmio + MMIO_MAC_ACCESS_CMD) ; 
-	while ( readw(xl_mmio + MMIO_MACDATA) & EEBUSY ) ;
-
-	/* Put the value we want to write into EEDATA */ 
-	writel(IO_WORD_WRITE | EEDATA, xl_mmio + MMIO_MAC_ACCESS_CMD) ; 
-	writew(ee_value, xl_mmio + MMIO_MACDATA) ;
-
-	/* Tell EEProm to write eevalue into ee_addr */
-	writel(IO_WORD_WRITE | EECONTROL, xl_mmio + MMIO_MAC_ACCESS_CMD) ; 
-	writew(EEWRITE + ee_addr, xl_mmio + MMIO_MACDATA) ; 
-
-	/* Wait for EEProm to not be busy, to ensure write gets done */
-	writel(IO_WORD_READ | EECONTROL, xl_mmio + MMIO_MAC_ACCESS_CMD) ; 
-	while ( readw(xl_mmio + MMIO_MACDATA) & EEBUSY ) ;
-	
-	return ; 
-}
-
-static const struct net_device_ops xl_netdev_ops = {
-	.ndo_open		= xl_open,
-	.ndo_stop		= xl_close,
-	.ndo_start_xmit		= xl_xmit,
-	.ndo_change_mtu		= xl_change_mtu,
-	.ndo_set_rx_mode	= xl_set_rx_mode,
-	.ndo_set_mac_address	= xl_set_mac_address,
-};
- 
-static int __devinit xl_probe(struct pci_dev *pdev,
-			      const struct pci_device_id *ent) 
-{
-	struct net_device *dev ; 
-	struct xl_private *xl_priv ; 
-	static int card_no = -1 ;
-	int i ; 
-
-	card_no++ ; 
-
-	if (pci_enable_device(pdev)) { 
-		return -ENODEV ; 
-	} 
-
-	pci_set_master(pdev);
-
-	if ((i = pci_request_regions(pdev,"3c359"))) { 
-		return i ; 
-	}
-
-	/* 
-	 * Allowing init_trdev to allocate the private data will align
-	 * xl_private on a 32 bytes boundary which we need for the rx/tx
-	 * descriptors
-	 */
-
-	dev = alloc_trdev(sizeof(struct xl_private)) ; 
-	if (!dev) { 
-		pci_release_regions(pdev) ; 
-		return -ENOMEM ; 
-	} 
-	xl_priv = netdev_priv(dev);
-
-#if XL_DEBUG  
-	printk("pci_device: %p, dev:%p, dev->priv: %p, ba[0]: %10x, ba[1]:%10x\n", 
-		pdev, dev, netdev_priv(dev), (unsigned int)pdev->resource[0].start, (unsigned int)pdev->resource[1].start);
-#endif 
-
-	dev->irq=pdev->irq;
-	dev->base_addr=pci_resource_start(pdev,0) ; 
-	xl_priv->xl_card_name = pci_name(pdev);
-	xl_priv->xl_mmio=ioremap(pci_resource_start(pdev,1), XL_IO_SPACE);
-	xl_priv->pdev = pdev ; 
-		
-	if ((pkt_buf_sz[card_no] < 100) || (pkt_buf_sz[card_no] > 18000) )
-		xl_priv->pkt_buf_sz = PKT_BUF_SZ ; 
-	else
-		xl_priv->pkt_buf_sz = pkt_buf_sz[card_no] ; 
-
-	dev->mtu = xl_priv->pkt_buf_sz - TR_HLEN ; 
-	xl_priv->xl_ring_speed = ringspeed[card_no] ; 
-	xl_priv->xl_message_level = message_level[card_no] ; 
-	xl_priv->xl_functional_addr[0] = xl_priv->xl_functional_addr[1] = xl_priv->xl_functional_addr[2] = xl_priv->xl_functional_addr[3] = 0 ; 
-	xl_priv->xl_copy_all_options = 0 ; 
-		
-	if((i = xl_init(dev))) {
-		iounmap(xl_priv->xl_mmio) ; 
-		free_netdev(dev) ; 
-		pci_release_regions(pdev) ; 
-		return i ; 
-	}				
-
-	dev->netdev_ops = &xl_netdev_ops;
-	SET_NETDEV_DEV(dev, &pdev->dev);
-
-	pci_set_drvdata(pdev,dev) ; 
-	if ((i = register_netdev(dev))) { 
-		printk(KERN_ERR "3C359, register netdev failed\n") ;  
-		pci_set_drvdata(pdev,NULL) ; 
-		iounmap(xl_priv->xl_mmio) ; 
-		free_netdev(dev) ; 
-		pci_release_regions(pdev) ; 
-		return i ; 
-	}
-   
-	printk(KERN_INFO "3C359: %s registered as: %s\n",xl_priv->xl_card_name,dev->name) ; 
-
-	return 0; 
-}
-
-static int xl_init_firmware(struct xl_private *xl_priv)
-{
-	int err;
-
-	err = request_firmware(&xl_priv->fw, FW_NAME, &xl_priv->pdev->dev);
-	if (err) {
-		printk(KERN_ERR "Failed to load firmware \"%s\"\n", FW_NAME);
-		return err;
-	}
-
-	if (xl_priv->fw->size < 16) {
-		printk(KERN_ERR "Bogus length %zu in \"%s\"\n",
-		       xl_priv->fw->size, FW_NAME);
-		release_firmware(xl_priv->fw);
-		err = -EINVAL;
-	}
-
-	return err;
-}
-
-static int __devinit xl_init(struct net_device *dev) 
-{
-	struct xl_private *xl_priv = netdev_priv(dev);
-	int err;
-
-	printk(KERN_INFO "%s\n", version);
-	printk(KERN_INFO "%s: I/O at %hx, MMIO at %p, using irq %d\n",
-		xl_priv->xl_card_name, (unsigned int)dev->base_addr ,xl_priv->xl_mmio, dev->irq);
-
-	spin_lock_init(&xl_priv->xl_lock) ; 
-
-	err = xl_init_firmware(xl_priv);
-	if (err == 0)
-		err = xl_hw_reset(dev);
-
-	return err;
-}
-
-
-/* 
- *	Hardware reset.  This needs to be a separate entity as we need to reset the card
- *	when we change the EEProm settings.
- */
-
-static int xl_hw_reset(struct net_device *dev) 
-{
-	struct xl_private *xl_priv = netdev_priv(dev);
-	u8 __iomem *xl_mmio = xl_priv->xl_mmio ; 
-	unsigned long t ; 
-	u16 i ; 
-    	u16 result_16 ; 
-	u8 result_8 ;
-	u16 start ; 
-	int j ;
-
-	if (xl_priv->fw == NULL)
-		return -EINVAL;
-
-	/*
-	 *  Reset the card.  If the card has got the microcode on board, we have 
-         *  missed the initialization interrupt, so we must always do this.
-	 */
-
-	writew( GLOBAL_RESET, xl_mmio + MMIO_COMMAND ) ; 
-
-	/* 
-	 * Must wait for cmdInProgress bit (12) to clear before continuing with
-	 * card configuration.
-	 */
-
-	t=jiffies;
-	while (readw(xl_mmio + MMIO_INTSTATUS) & INTSTAT_CMD_IN_PROGRESS) { 
-		schedule();		
-		if (time_after(jiffies, t + 40 * HZ)) {
-			printk(KERN_ERR "%s: 3COM 3C359 Velocity XL  card not responding to global reset.\n", dev->name);
-			return -ENODEV;
-		}
-	}
-
-	/*
-	 *  Enable pmbar by setting bit in CPAttention
-	 */
-
-	writel( (IO_BYTE_READ | CPATTENTION), xl_mmio + MMIO_MAC_ACCESS_CMD) ;
-	result_8 = readb(xl_mmio + MMIO_MACDATA) ; 
-	result_8 = result_8 | CPA_PMBARVIS ; 
-	writel( (IO_BYTE_WRITE | CPATTENTION), xl_mmio + MMIO_MAC_ACCESS_CMD) ; 
-	writeb(result_8, xl_mmio + MMIO_MACDATA) ; 
-	
-	/*
-	 * Read cpHold bit in pmbar, if cleared we have got Flashrom on board.
- 	 * If not, we need to upload the microcode to the card
-	 */
-
-	writel( (IO_WORD_READ | PMBAR),xl_mmio + MMIO_MAC_ACCESS_CMD);  
-
-#if XL_DEBUG
-	printk(KERN_INFO "Read from PMBAR = %04x\n", readw(xl_mmio + MMIO_MACDATA));
-#endif
-
-	if ( readw( (xl_mmio + MMIO_MACDATA))  & PMB_CPHOLD ) { 
-
-		/* Set PmBar, privateMemoryBase bits (8:2) to 0 */
-
-		writel( (IO_WORD_READ | PMBAR),xl_mmio + MMIO_MAC_ACCESS_CMD);  
-		result_16 = readw(xl_mmio + MMIO_MACDATA) ; 
-		result_16 = result_16 & ~((0x7F) << 2) ; 
-		writel( (IO_WORD_WRITE | PMBAR), xl_mmio + MMIO_MAC_ACCESS_CMD) ; 
-		writew(result_16,xl_mmio + MMIO_MACDATA) ; 
-	
-		/* Set CPAttention, memWrEn bit */
-
-		writel( (IO_BYTE_READ | CPATTENTION), xl_mmio + MMIO_MAC_ACCESS_CMD) ; 
-		result_8 = readb(xl_mmio + MMIO_MACDATA) ; 
-		result_8 = result_8 | CPA_MEMWREN  ; 
-		writel( (IO_BYTE_WRITE | CPATTENTION), xl_mmio + MMIO_MAC_ACCESS_CMD) ; 
-		writeb(result_8, xl_mmio + MMIO_MACDATA) ; 
-
-		/* 
-		 * Now to write the microcode into the shared ram 
-	 	 * The microcode must finish at position 0xFFFF,
-	 	 * so we must subtract to get the start position for the code
-	 	 *
-		 * Looks strange but ensures compiler only uses
-		 * 16 bit unsigned int
-		 */
-		start = (0xFFFF - (xl_priv->fw->size) + 1) ;
-
-		printk(KERN_INFO "3C359: Uploading Microcode: "); 
-
-		for (i = start, j = 0; j < xl_priv->fw->size; i++, j++) {
-			writel(MEM_BYTE_WRITE | 0XD0000 | i,
-			       xl_mmio + MMIO_MAC_ACCESS_CMD);
-			writeb(xl_priv->fw->data[j], xl_mmio + MMIO_MACDATA);
-			if (j % 1024 == 0)
-				printk(".");
-		}
-		printk("\n") ; 
-
-		for (i = 0; i < 16; i++) {
-			writel((MEM_BYTE_WRITE | 0xDFFF0) + i,
-			       xl_mmio + MMIO_MAC_ACCESS_CMD);
-			writeb(xl_priv->fw->data[xl_priv->fw->size - 16 + i],
-			       xl_mmio + MMIO_MACDATA);
-		}
-
-		/*
-		 * Have to write the start address of the upload to FFF4, but
-                 * the address must be >> 4. You do not want to know how long
-                 * it took me to discover this.
-		 */
-
-		writel(MEM_WORD_WRITE | 0xDFFF4, xl_mmio + MMIO_MAC_ACCESS_CMD) ; 
-		writew(start >> 4, xl_mmio + MMIO_MACDATA);
-
-		/* Clear the CPAttention, memWrEn Bit */
-	
-		writel( (IO_BYTE_READ | CPATTENTION), xl_mmio + MMIO_MAC_ACCESS_CMD) ; 
-		result_8 = readb(xl_mmio + MMIO_MACDATA) ; 
-		result_8 = result_8 & ~CPA_MEMWREN ; 
-		writel( (IO_BYTE_WRITE | CPATTENTION), xl_mmio + MMIO_MAC_ACCESS_CMD) ; 
-		writeb(result_8, xl_mmio + MMIO_MACDATA) ; 
-
-		/* Clear the cpHold bit in pmbar */
-
-		writel( (IO_WORD_READ | PMBAR),xl_mmio + MMIO_MAC_ACCESS_CMD);  
-		result_16 = readw(xl_mmio + MMIO_MACDATA) ; 
-		result_16 = result_16 & ~PMB_CPHOLD ; 
-		writel( (IO_WORD_WRITE | PMBAR), xl_mmio + MMIO_MAC_ACCESS_CMD) ; 
-		writew(result_16,xl_mmio + MMIO_MACDATA) ; 
-
-
-	} /* If microcode upload required */
-
-	/* 
-	 * The card should now go though a self test procedure and get itself ready
-         * to be opened, we must wait for an srb response with the initialization
-         * information. 
-	 */
-
-#if XL_DEBUG
-	printk(KERN_INFO "%s: Microcode uploaded, must wait for the self test to complete\n", dev->name);
-#endif
-
-	writew(SETINDENABLE | 0xFFF, xl_mmio + MMIO_COMMAND) ; 
-
-	t=jiffies;
-	while ( !(readw(xl_mmio + MMIO_INTSTATUS_AUTO) & INTSTAT_SRB) ) { 
-		schedule();		
-		if (time_after(jiffies, t + 15 * HZ)) {
-			printk(KERN_ERR "3COM 3C359 Velocity XL  card not responding.\n");
-			return -ENODEV; 
-		}
-	}
-
-	/*
-	 * Write the RxBufArea with D000, RxEarlyThresh, TxStartThresh, 
- 	 * DnPriReqThresh, read the tech docs if you want to know what
-	 * values they need to be.
-	 */
-
-	writel(MMIO_WORD_WRITE | RXBUFAREA, xl_mmio + MMIO_MAC_ACCESS_CMD) ; 
-	writew(0xD000, xl_mmio + MMIO_MACDATA) ; 
-	
-	writel(MMIO_WORD_WRITE | RXEARLYTHRESH, xl_mmio + MMIO_MAC_ACCESS_CMD) ; 
-	writew(0X0020, xl_mmio + MMIO_MACDATA) ; 
-	
-	writew( SETTXSTARTTHRESH | 0x40 , xl_mmio + MMIO_COMMAND) ; 
-
-	writeb(0x04, xl_mmio + MMIO_DNBURSTTHRESH) ; 
-	writeb(0x04, xl_mmio + DNPRIREQTHRESH) ;
-
-	/*
-	 * Read WRBR to provide the location of the srb block, have to use byte reads not word reads. 
-	 * Tech docs have this wrong !!!!
-	 */
-
-	writel(MMIO_BYTE_READ | WRBR, xl_mmio + MMIO_MAC_ACCESS_CMD) ; 
-	xl_priv->srb = readb(xl_mmio + MMIO_MACDATA) << 8 ; 
-	writel( (MMIO_BYTE_READ | WRBR) + 1, xl_mmio + MMIO_MAC_ACCESS_CMD) ; 
-	xl_priv->srb = xl_priv->srb | readb(xl_mmio + MMIO_MACDATA) ;
-
-#if XL_DEBUG
-	writel(IO_WORD_READ | SWITCHSETTINGS, xl_mmio + MMIO_MAC_ACCESS_CMD) ; 
-	if ( readw(xl_mmio + MMIO_MACDATA) & 2) { 
-		printk(KERN_INFO "Default ring speed 4 mbps\n");
-	} else {
-		printk(KERN_INFO "Default ring speed 16 mbps\n");
-	} 
-	printk(KERN_INFO "%s: xl_priv->srb = %04x\n",xl_priv->xl_card_name, xl_priv->srb);
-#endif
-
-	return 0;
-}
-
-static int xl_open(struct net_device *dev) 
-{
-	struct xl_private *xl_priv=netdev_priv(dev);
-	u8 __iomem *xl_mmio = xl_priv->xl_mmio ; 
-	u8 i ; 
-	__le16 hwaddr[3] ; /* Should be u8[6] but we get word return values */
-	int open_err ;
-
-	u16 switchsettings, switchsettings_eeprom  ;
- 
-	if (request_irq(dev->irq, xl_interrupt, IRQF_SHARED , "3c359", dev))
-		return -EAGAIN;
-
-	/* 
-	 * Read the information from the EEPROM that we need.
-	 */
-	
-	hwaddr[0] = cpu_to_le16(xl_ee_read(dev,0x10));
-	hwaddr[1] = cpu_to_le16(xl_ee_read(dev,0x11));
-	hwaddr[2] = cpu_to_le16(xl_ee_read(dev,0x12));
-
-	/* Ring speed */
-
-	switchsettings_eeprom = xl_ee_read(dev,0x08) ;
-	switchsettings = switchsettings_eeprom ;  
-
-	if (xl_priv->xl_ring_speed != 0) { 
-		if (xl_priv->xl_ring_speed == 4)  
-			switchsettings = switchsettings | 0x02 ; 
-		else 
-			switchsettings = switchsettings & ~0x02 ; 
-	}
-
-	/* Only write EEProm if there has been a change */
-	if (switchsettings != switchsettings_eeprom) { 
-		xl_ee_write(dev,0x08,switchsettings) ; 
-		/* Hardware reset after changing EEProm */
-		xl_hw_reset(dev) ; 
-	}
-
-	memcpy(dev->dev_addr,hwaddr,dev->addr_len) ; 
-	
-	open_err = xl_open_hw(dev) ; 
-
-	/* 
-	 * This really needs to be cleaned up with better error reporting.
-	 */
-
-	if (open_err != 0) { /* Something went wrong with the open command */
-		if (open_err & 0x07) { /* Wrong speed, retry at different speed */
-			printk(KERN_WARNING "%s: Open Error, retrying at different ringspeed\n", dev->name);
-			switchsettings = switchsettings ^ 2 ; 
-			xl_ee_write(dev,0x08,switchsettings) ; 
-			xl_hw_reset(dev) ; 
-			open_err = xl_open_hw(dev) ; 
-			if (open_err != 0) { 
-				printk(KERN_WARNING "%s: Open error returned a second time, we're bombing out now\n", dev->name); 
-				free_irq(dev->irq,dev) ; 						
-				return -ENODEV ;
-			}  
-		} else { 
-			printk(KERN_WARNING "%s: Open Error = %04x\n", dev->name, open_err) ; 
-			free_irq(dev->irq,dev) ; 
-			return -ENODEV ; 
-		}
-	}
-
-	/*
-	 * Now to set up the Rx and Tx buffer structures
-	 */
-	/* These MUST be on 8 byte boundaries */
-	xl_priv->xl_tx_ring = kzalloc((sizeof(struct xl_tx_desc) * XL_TX_RING_SIZE) + 7, GFP_DMA | GFP_KERNEL);
-	if (xl_priv->xl_tx_ring == NULL) {
-		free_irq(dev->irq,dev);
-		return -ENOMEM;
-	}
-	xl_priv->xl_rx_ring = kzalloc((sizeof(struct xl_rx_desc) * XL_RX_RING_SIZE) +7, GFP_DMA | GFP_KERNEL);
-	if (xl_priv->xl_rx_ring == NULL) {
-		free_irq(dev->irq,dev);
-		kfree(xl_priv->xl_tx_ring);
-		return -ENOMEM;
-	}
-
-	 /* Setup Rx Ring */
-	 for (i=0 ; i < XL_RX_RING_SIZE ; i++) { 
-		struct sk_buff *skb ; 
-
-		skb = dev_alloc_skb(xl_priv->pkt_buf_sz) ; 
-		if (skb==NULL) 
-			break ; 
-
-		skb->dev = dev ; 
-		xl_priv->xl_rx_ring[i].upfragaddr = cpu_to_le32(pci_map_single(xl_priv->pdev, skb->data,xl_priv->pkt_buf_sz, PCI_DMA_FROMDEVICE));
-		xl_priv->xl_rx_ring[i].upfraglen = cpu_to_le32(xl_priv->pkt_buf_sz) | RXUPLASTFRAG;
-		xl_priv->rx_ring_skb[i] = skb ; 	
-	}
-
-	if (i==0) { 
-		printk(KERN_WARNING "%s: Not enough memory to allocate rx buffers. Adapter disabled\n",dev->name);
-		free_irq(dev->irq,dev) ; 
-		kfree(xl_priv->xl_tx_ring);
-		kfree(xl_priv->xl_rx_ring);
-		return -EIO ; 
-	} 
-
-	xl_priv->rx_ring_no = i ; 
-	xl_priv->rx_ring_tail = 0 ; 
-	xl_priv->rx_ring_dma_addr = pci_map_single(xl_priv->pdev,xl_priv->xl_rx_ring, sizeof(struct xl_rx_desc) * XL_RX_RING_SIZE, PCI_DMA_TODEVICE) ; 
-	for (i=0;i<(xl_priv->rx_ring_no-1);i++) { 
-		xl_priv->xl_rx_ring[i].upnextptr = cpu_to_le32(xl_priv->rx_ring_dma_addr + (sizeof (struct xl_rx_desc) * (i+1)));
-	} 
-	xl_priv->xl_rx_ring[i].upnextptr = 0 ; 
-
-	writel(xl_priv->rx_ring_dma_addr, xl_mmio + MMIO_UPLISTPTR) ; 
-	
-	/* Setup Tx Ring */
-	
-	xl_priv->tx_ring_dma_addr = pci_map_single(xl_priv->pdev,xl_priv->xl_tx_ring, sizeof(struct xl_tx_desc) * XL_TX_RING_SIZE,PCI_DMA_TODEVICE) ; 
-	
-	xl_priv->tx_ring_head = 1 ; 
-	xl_priv->tx_ring_tail = 255 ; /* Special marker for first packet */
-	xl_priv->free_ring_entries = XL_TX_RING_SIZE ; 
-
-	/*
- 	 * Setup the first dummy DPD entry for polling to start working.
-	 */
-
-	xl_priv->xl_tx_ring[0].framestartheader = TXDPDEMPTY;
-	xl_priv->xl_tx_ring[0].buffer = 0 ; 
-	xl_priv->xl_tx_ring[0].buffer_length = 0 ; 
-	xl_priv->xl_tx_ring[0].dnnextptr = 0 ; 
-
-	writel(xl_priv->tx_ring_dma_addr, xl_mmio + MMIO_DNLISTPTR) ; 
-	writel(DNUNSTALL, xl_mmio + MMIO_COMMAND) ; 
-	writel(UPUNSTALL, xl_mmio + MMIO_COMMAND) ; 
-	writel(DNENABLE, xl_mmio + MMIO_COMMAND) ; 
-	writeb(0x40, xl_mmio + MMIO_DNPOLL) ;	
-
-	/*
-	 * Enable interrupts on the card
-	 */
-
-	writel(SETINTENABLE | INT_MASK, xl_mmio + MMIO_COMMAND) ; 
-	writel(SETINDENABLE | INT_MASK, xl_mmio + MMIO_COMMAND) ; 
-
-	netif_start_queue(dev) ; 	
-	return 0;
-	
-}	
-
-static int xl_open_hw(struct net_device *dev) 
-{ 
-	struct xl_private *xl_priv=netdev_priv(dev);
-	u8 __iomem *xl_mmio = xl_priv->xl_mmio ; 
-	u16 vsoff ;
-	char ver_str[33];  
-	int open_err ; 
-	int i ; 
-	unsigned long t ; 
-
-	/*
-	 * Okay, let's build up the Open.NIC srb command
-	 *
-	 */
-		
-	writel( (MEM_BYTE_WRITE | 0xD0000 | xl_priv->srb), xl_mmio + MMIO_MAC_ACCESS_CMD) ; 
-	writeb(OPEN_NIC, xl_mmio + MMIO_MACDATA) ; 
-	
-	/*
-	 * Use this as a test byte, if it comes back with the same value, the command didn't work
-	 */
-
-	writel( (MEM_BYTE_WRITE | 0xD0000 | xl_priv->srb)+ 2, xl_mmio + MMIO_MAC_ACCESS_CMD) ; 
-	writeb(0xff,xl_mmio + MMIO_MACDATA) ; 
-
-	/* Open options */
-	writel( (MEM_BYTE_WRITE | 0xD0000 | xl_priv->srb) + 8, xl_mmio + MMIO_MAC_ACCESS_CMD) ; 
-	writeb(0x00, xl_mmio + MMIO_MACDATA) ; 
-	writel( (MEM_BYTE_WRITE | 0xD0000 | xl_priv->srb) + 9, xl_mmio + MMIO_MAC_ACCESS_CMD) ; 
-	writeb(0x00, xl_mmio + MMIO_MACDATA) ; 
-
-	/* 
-	 * Node address, be careful here, the docs say you can just put zeros here and it will use
-	 * the hardware address, it doesn't, you must include the node address in the open command.
-	 */
-
-	if (xl_priv->xl_laa[0]) {  /* If using a LAA address */
-		for (i=10;i<16;i++) { 
-			writel( (MEM_BYTE_WRITE | 0xD0000 | xl_priv->srb) + i, xl_mmio + MMIO_MAC_ACCESS_CMD) ; 
-			writeb(xl_priv->xl_laa[i-10],xl_mmio + MMIO_MACDATA) ;
-		}
-		memcpy(dev->dev_addr,xl_priv->xl_laa,dev->addr_len) ; 
-	} else { /* Regular hardware address */ 
-		for (i=10;i<16;i++) { 
-			writel( (MEM_BYTE_WRITE | 0xD0000 | xl_priv->srb) + i, xl_mmio + MMIO_MAC_ACCESS_CMD) ; 
-			writeb(dev->dev_addr[i-10], xl_mmio + MMIO_MACDATA) ; 
-		}
-	}
-
-	/* Default everything else to 0 */
-	for (i = 16; i < 34; i++) {
-		writel( (MEM_BYTE_WRITE | 0xD0000 | xl_priv->srb) + i, xl_mmio + MMIO_MAC_ACCESS_CMD) ; 
-		writeb(0x00,xl_mmio + MMIO_MACDATA) ; 
-	}
-	
-	/*
-	 *  Set the csrb bit in the MISR register
-	 */
-
-	xl_wait_misr_flags(dev) ; 
-	writel(MEM_BYTE_WRITE | MF_CSRB, xl_mmio + MMIO_MAC_ACCESS_CMD) ; 
-	writeb(0xFF, xl_mmio + MMIO_MACDATA) ; 
-	writel(MMIO_BYTE_WRITE | MISR_SET, xl_mmio + MMIO_MAC_ACCESS_CMD) ; 
-	writeb(MISR_CSRB , xl_mmio + MMIO_MACDATA) ; 
-
-	/*
-	 * Now wait for the command to run
-	 */
-
-	t=jiffies;
-	while (! (readw(xl_mmio + MMIO_INTSTATUS) & INTSTAT_SRB)) { 
-		schedule();		
-		if (time_after(jiffies, t + 40 * HZ)) {
-			printk(KERN_ERR "3COM 3C359 Velocity XL  card not responding.\n");
-			break ; 
-		}
-	}
-
-	/*
-	 * Let's interpret the open response
-	 */
-
-	writel( (MEM_BYTE_READ | 0xD0000 | xl_priv->srb)+2, xl_mmio + MMIO_MAC_ACCESS_CMD) ; 
-	if (readb(xl_mmio + MMIO_MACDATA)!=0) {
-		open_err = readb(xl_mmio + MMIO_MACDATA) << 8 ; 
-		writel( (MEM_BYTE_READ | 0xD0000 | xl_priv->srb) + 7, xl_mmio + MMIO_MAC_ACCESS_CMD) ; 
-		open_err |= readb(xl_mmio + MMIO_MACDATA) ; 
-		return open_err ; 
-	} else { 
-		writel( (MEM_WORD_READ | 0xD0000 | xl_priv->srb) + 8, xl_mmio + MMIO_MAC_ACCESS_CMD) ; 
-		xl_priv->asb = swab16(readw(xl_mmio + MMIO_MACDATA)) ;
-		printk(KERN_INFO "%s: Adapter Opened Details: ",dev->name) ; 
-		printk("ASB: %04x",xl_priv->asb ) ; 
-		writel( (MEM_WORD_READ | 0xD0000 | xl_priv->srb) + 10, xl_mmio + MMIO_MAC_ACCESS_CMD) ; 
-		printk(", SRB: %04x",swab16(readw(xl_mmio + MMIO_MACDATA)) ) ;
- 
-		writel( (MEM_WORD_READ | 0xD0000 | xl_priv->srb) + 12, xl_mmio + MMIO_MAC_ACCESS_CMD) ; 
-		xl_priv->arb = swab16(readw(xl_mmio + MMIO_MACDATA)) ;
-		printk(", ARB: %04x\n",xl_priv->arb );
-		writel( (MEM_WORD_READ | 0xD0000 | xl_priv->srb) + 14, xl_mmio + MMIO_MAC_ACCESS_CMD) ; 
-		vsoff = swab16(readw(xl_mmio + MMIO_MACDATA)) ;
-
-		/* 
-		 * Interesting, sending the individual characters directly to printk was causing klogd to use
-		 * use 100% of processor time, so we build up the string and print that instead.
-	   	 */
-
-		for (i=0;i<0x20;i++) { 
-			writel( (MEM_BYTE_READ | 0xD0000 | vsoff) + i, xl_mmio + MMIO_MAC_ACCESS_CMD) ; 
-			ver_str[i] = readb(xl_mmio + MMIO_MACDATA) ; 
-		}
-		ver_str[i] = '\0' ; 
-		printk(KERN_INFO "%s: Microcode version String: %s\n",dev->name,ver_str);
-	} 	
-	
-	/*
-	 * Issue the AckInterrupt
-	 */
-	writew(ACK_INTERRUPT | SRBRACK | LATCH_ACK, xl_mmio + MMIO_COMMAND) ; 
-
-	return 0 ; 
-}
-
-/*
- *	There are two ways of implementing rx on the 359 NIC, either
- * 	interrupt driven or polling.  We are going to uses interrupts,
- *	it is the easier way of doing things.
- *	
- *	The Rx works with a ring of Rx descriptors.  At initialise time the ring
- *	entries point to the next entry except for the last entry in the ring 
- *	which points to 0.  The card is programmed with the location of the first
- *	available descriptor and keeps reading the next_ptr until next_ptr is set
- *	to 0.  Hopefully with a ring size of 16 the card will never get to read a next_ptr
- *	of 0.  As the Rx interrupt is received we copy the frame up to the protocol layers
- *	and then point the end of the ring to our current position and point our current
- *	position to 0, therefore making the current position the last position on the ring.
- *	The last position on the ring therefore loops continually loops around the rx ring.
- *	
- *	rx_ring_tail is the position on the ring to process next. (Think of a snake, the head 
- *	expands as the card adds new packets and we go around eating the tail processing the
- *	packets.)
- *
- *	Undoubtably it could be streamlined and improved upon, but at the moment it works 
- *	and the fast path through the routine is fine. 
- *	
- *	adv_rx_ring could be inlined to increase performance, but its called a *lot* of times
- *	in xl_rx so would increase the size of the function significantly. 
- */
-
-static void adv_rx_ring(struct net_device *dev) /* Advance rx_ring, cut down on bloat in xl_rx */ 
-{
-	struct xl_private *xl_priv=netdev_priv(dev);
-	int n = xl_priv->rx_ring_tail;
-	int prev_ring_loc;
-
-	prev_ring_loc = (n + XL_RX_RING_SIZE - 1) & (XL_RX_RING_SIZE - 1);
-	xl_priv->xl_rx_ring[prev_ring_loc].upnextptr = cpu_to_le32(xl_priv->rx_ring_dma_addr + (sizeof (struct xl_rx_desc) * n));
-	xl_priv->xl_rx_ring[n].framestatus = 0;
-	xl_priv->xl_rx_ring[n].upnextptr = 0;
-	xl_priv->rx_ring_tail++;
-	xl_priv->rx_ring_tail &= (XL_RX_RING_SIZE-1);
-}
-
-static void xl_rx(struct net_device *dev)
-{
-	struct xl_private *xl_priv=netdev_priv(dev);
-	u8 __iomem * xl_mmio = xl_priv->xl_mmio ; 
-	struct sk_buff *skb, *skb2 ; 
-	int frame_length = 0, copy_len = 0  ; 	
-	int temp_ring_loc ;  
-
-	/*
-	 * Receive the next frame, loop around the ring until all frames
-  	 * have been received.
-	 */ 	 
-	
-	while (xl_priv->xl_rx_ring[xl_priv->rx_ring_tail].framestatus & (RXUPDCOMPLETE | RXUPDFULL) ) { /* Descriptor to process */
-
-		if (xl_priv->xl_rx_ring[xl_priv->rx_ring_tail].framestatus & RXUPDFULL ) { /* UpdFull, Multiple Descriptors used for the frame */
-
-			/* 
-			 * This is a pain, you need to go through all the descriptors until the last one 
-			 * for this frame to find the framelength
-			 */
-
-			temp_ring_loc = xl_priv->rx_ring_tail ; 
-
-			while (xl_priv->xl_rx_ring[temp_ring_loc].framestatus & RXUPDFULL ) {
-				temp_ring_loc++ ; 
-				temp_ring_loc &= (XL_RX_RING_SIZE-1) ; 
-			}
-
-			frame_length = le32_to_cpu(xl_priv->xl_rx_ring[temp_ring_loc].framestatus) & 0x7FFF;
-
-			skb = dev_alloc_skb(frame_length) ;
- 
-			if (skb==NULL) { /* No memory for frame, still need to roll forward the rx ring */
-				printk(KERN_WARNING "%s: dev_alloc_skb failed - multi buffer !\n", dev->name) ; 
-				while (xl_priv->rx_ring_tail != temp_ring_loc)  
-					adv_rx_ring(dev) ; 
-				
-				adv_rx_ring(dev) ; /* One more time just for luck :) */ 
-				dev->stats.rx_dropped++ ; 
-
-				writel(ACK_INTERRUPT | UPCOMPACK | LATCH_ACK , xl_mmio + MMIO_COMMAND) ; 
-				return ; 				
-			}
-	
-			while (xl_priv->rx_ring_tail != temp_ring_loc) { 
-				copy_len = le32_to_cpu(xl_priv->xl_rx_ring[xl_priv->rx_ring_tail].upfraglen) & 0x7FFF;
-				frame_length -= copy_len ;  
-				pci_dma_sync_single_for_cpu(xl_priv->pdev,le32_to_cpu(xl_priv->xl_rx_ring[xl_priv->rx_ring_tail].upfragaddr),xl_priv->pkt_buf_sz,PCI_DMA_FROMDEVICE);
-				skb_copy_from_linear_data(xl_priv->rx_ring_skb[xl_priv->rx_ring_tail],
-							  skb_put(skb, copy_len),
-							  copy_len);
-				pci_dma_sync_single_for_device(xl_priv->pdev,le32_to_cpu(xl_priv->xl_rx_ring[xl_priv->rx_ring_tail].upfragaddr),xl_priv->pkt_buf_sz,PCI_DMA_FROMDEVICE);
-				adv_rx_ring(dev) ; 
-			} 
-
-			/* Now we have found the last fragment */
-			pci_dma_sync_single_for_cpu(xl_priv->pdev,le32_to_cpu(xl_priv->xl_rx_ring[xl_priv->rx_ring_tail].upfragaddr),xl_priv->pkt_buf_sz,PCI_DMA_FROMDEVICE);
-			skb_copy_from_linear_data(xl_priv->rx_ring_skb[xl_priv->rx_ring_tail],
-				      skb_put(skb,copy_len), frame_length);
-/*			memcpy(skb_put(skb,frame_length), bus_to_virt(xl_priv->xl_rx_ring[xl_priv->rx_ring_tail].upfragaddr), frame_length) ; */
-			pci_dma_sync_single_for_device(xl_priv->pdev,le32_to_cpu(xl_priv->xl_rx_ring[xl_priv->rx_ring_tail].upfragaddr),xl_priv->pkt_buf_sz,PCI_DMA_FROMDEVICE);
-			adv_rx_ring(dev) ; 
-			skb->protocol = tr_type_trans(skb,dev) ; 
-			netif_rx(skb) ; 
-
-		} else { /* Single Descriptor Used, simply swap buffers over, fast path  */
-
-			frame_length = le32_to_cpu(xl_priv->xl_rx_ring[xl_priv->rx_ring_tail].framestatus) & 0x7FFF;
-			
-			skb = dev_alloc_skb(xl_priv->pkt_buf_sz) ; 
-
-			if (skb==NULL) { /* Still need to fix the rx ring */
-				printk(KERN_WARNING "%s: dev_alloc_skb failed in rx, single buffer\n",dev->name);
-				adv_rx_ring(dev) ; 
-				dev->stats.rx_dropped++ ; 
-				writel(ACK_INTERRUPT | UPCOMPACK | LATCH_ACK , xl_mmio + MMIO_COMMAND) ; 
-				return ; 
-			}
-
-			skb2 = xl_priv->rx_ring_skb[xl_priv->rx_ring_tail] ; 
-			pci_unmap_single(xl_priv->pdev, le32_to_cpu(xl_priv->xl_rx_ring[xl_priv->rx_ring_tail].upfragaddr), xl_priv->pkt_buf_sz,PCI_DMA_FROMDEVICE) ;
-			skb_put(skb2, frame_length) ; 
-			skb2->protocol = tr_type_trans(skb2,dev) ; 
-
-			xl_priv->rx_ring_skb[xl_priv->rx_ring_tail] = skb ; 	
-			xl_priv->xl_rx_ring[xl_priv->rx_ring_tail].upfragaddr = cpu_to_le32(pci_map_single(xl_priv->pdev,skb->data,xl_priv->pkt_buf_sz, PCI_DMA_FROMDEVICE));
-			xl_priv->xl_rx_ring[xl_priv->rx_ring_tail].upfraglen = cpu_to_le32(xl_priv->pkt_buf_sz) | RXUPLASTFRAG;
-			adv_rx_ring(dev) ; 
-			dev->stats.rx_packets++ ; 
-			dev->stats.rx_bytes += frame_length ; 	
-
-			netif_rx(skb2) ; 		
-		 } /* if multiple buffers */
-	} /* while packet to do */
-
-	/* Clear the updComplete interrupt */
-	writel(ACK_INTERRUPT | UPCOMPACK | LATCH_ACK , xl_mmio + MMIO_COMMAND) ; 
-	return ; 	
-}
-
-/*
- * This is ruthless, it doesn't care what state the card is in it will 
- * completely reset the adapter.
- */
-
-static void xl_reset(struct net_device *dev) 
-{
-	struct xl_private *xl_priv=netdev_priv(dev);
-	u8 __iomem * xl_mmio = xl_priv->xl_mmio ; 
-	unsigned long t; 
-
-	writew( GLOBAL_RESET, xl_mmio + MMIO_COMMAND ) ; 
-
-	/* 
-	 * Must wait for cmdInProgress bit (12) to clear before continuing with
-	 * card configuration.
-	 */
-
-	t=jiffies;
-	while (readw(xl_mmio + MMIO_INTSTATUS) & INTSTAT_CMD_IN_PROGRESS) { 
-		if (time_after(jiffies, t + 40 * HZ)) {
-			printk(KERN_ERR "3COM 3C359 Velocity XL  card not responding.\n");
-			break ; 
-		}
-	}
-	
-}
-
-static void xl_freemem(struct net_device *dev) 
-{
-	struct xl_private *xl_priv=netdev_priv(dev);
-	int i ; 
-
-	for (i=0;i<XL_RX_RING_SIZE;i++) {
-		dev_kfree_skb_irq(xl_priv->rx_ring_skb[xl_priv->rx_ring_tail]) ; 
-		pci_unmap_single(xl_priv->pdev,le32_to_cpu(xl_priv->xl_rx_ring[xl_priv->rx_ring_tail].upfragaddr),xl_priv->pkt_buf_sz, PCI_DMA_FROMDEVICE);
-		xl_priv->rx_ring_tail++ ; 
-		xl_priv->rx_ring_tail &= XL_RX_RING_SIZE-1; 
-	} 
-
-	/* unmap ring */
-	pci_unmap_single(xl_priv->pdev,xl_priv->rx_ring_dma_addr, sizeof(struct xl_rx_desc) * XL_RX_RING_SIZE, PCI_DMA_FROMDEVICE) ; 
-	
-	pci_unmap_single(xl_priv->pdev,xl_priv->tx_ring_dma_addr, sizeof(struct xl_tx_desc) * XL_TX_RING_SIZE, PCI_DMA_TODEVICE) ; 
-
-	kfree(xl_priv->xl_rx_ring) ; 
-	kfree(xl_priv->xl_tx_ring) ; 
-
-	return  ; 
-}
-
-static irqreturn_t xl_interrupt(int irq, void *dev_id) 
-{
-	struct net_device *dev = (struct net_device *)dev_id;
-	struct xl_private *xl_priv =netdev_priv(dev);
-	u8 __iomem * xl_mmio = xl_priv->xl_mmio ; 
-	u16 intstatus, macstatus  ;
-
-	intstatus = readw(xl_mmio + MMIO_INTSTATUS) ;  
-
-	if (!(intstatus & 1)) /* We didn't generate the interrupt */
-		return IRQ_NONE;
-
-	spin_lock(&xl_priv->xl_lock) ; 
-
-	/*
-	 * Process the interrupt
-	 */
-	/*
-	 * Something fishy going on here, we shouldn't get 0001 ints, not fatal though.
-	 */
-	if (intstatus == 0x0001) {  
-		writel(ACK_INTERRUPT | LATCH_ACK, xl_mmio + MMIO_COMMAND) ;
-		printk(KERN_INFO "%s: 00001 int received\n",dev->name);
-	} else {  
-		if (intstatus &	(HOSTERRINT | SRBRINT | ARBCINT | UPCOMPINT | DNCOMPINT | HARDERRINT | (1<<8) | TXUNDERRUN | ASBFINT)) { 
-			
-			/* 
-			 * Host Error.
-			 * It may be possible to recover from this, but usually it means something
-			 * is seriously fubar, so we just close the adapter.
-			 */
-
-			if (intstatus & HOSTERRINT) {
-				printk(KERN_WARNING "%s: Host Error, performing global reset, intstatus = %04x\n",dev->name,intstatus);
-				writew( GLOBAL_RESET, xl_mmio + MMIO_COMMAND ) ;
-				printk(KERN_WARNING "%s: Resetting hardware:\n", dev->name);
-				netif_stop_queue(dev) ;
-				xl_freemem(dev) ; 
-				free_irq(dev->irq,dev); 	
-				xl_reset(dev) ; 
-				writel(ACK_INTERRUPT | LATCH_ACK, xl_mmio + MMIO_COMMAND) ; 
-				spin_unlock(&xl_priv->xl_lock) ; 
-				return IRQ_HANDLED;
-			} /* Host Error */
-
-			if (intstatus & SRBRINT ) {  /* Srbc interrupt */
-				writel(ACK_INTERRUPT | SRBRACK | LATCH_ACK, xl_mmio + MMIO_COMMAND) ;
-				if (xl_priv->srb_queued)
-					xl_srb_bh(dev) ; 
-			} /* SRBR Interrupt */
-
-			if (intstatus & TXUNDERRUN) { /* Issue DnReset command */
-				writel(DNRESET, xl_mmio + MMIO_MAC_ACCESS_CMD) ; 
-				while (readw(xl_mmio + MMIO_INTSTATUS) & INTSTAT_CMD_IN_PROGRESS) { /* Wait for command to run */
-					/* !!! FIX-ME !!!! 
-					Must put a timeout check here ! */
-					/* Empty Loop */
-				} 
-				printk(KERN_WARNING "%s: TX Underrun received\n",dev->name);
-				writel(ACK_INTERRUPT | LATCH_ACK, xl_mmio + MMIO_COMMAND) ; 
-			} /* TxUnderRun */
-	
-			if (intstatus & ARBCINT ) { /* Arbc interrupt */
-				xl_arb_cmd(dev) ; 
-			} /* Arbc */
-
-			if (intstatus & ASBFINT) { 
-				if (xl_priv->asb_queued == 1) {
-					xl_asb_cmd(dev) ; 
-				} else if (xl_priv->asb_queued == 2) {
-					xl_asb_bh(dev) ; 
-				} else { 
-					writel(ACK_INTERRUPT | LATCH_ACK | ASBFACK, xl_mmio + MMIO_COMMAND) ; 
-				}  
-			} /* Asbf */
-
-			if (intstatus & UPCOMPINT ) /* UpComplete */
-				xl_rx(dev) ; 
-
-			if (intstatus & DNCOMPINT )  /* DnComplete */
-				xl_dn_comp(dev) ; 
-
-			if (intstatus & HARDERRINT ) { /* Hardware error */
-				writel(MMIO_WORD_READ | MACSTATUS, xl_mmio + MMIO_MAC_ACCESS_CMD) ; 
-				macstatus = readw(xl_mmio + MMIO_MACDATA) ; 
-				printk(KERN_WARNING "%s: MacStatusError, details: ", dev->name);
-				if (macstatus & (1<<14)) 
-					printk(KERN_WARNING "tchk error: Unrecoverable error\n");
-				if (macstatus & (1<<3))
-					printk(KERN_WARNING "eint error: Internal watchdog timer expired\n");
-				if (macstatus & (1<<2))
-					printk(KERN_WARNING "aint error: Host tried to perform invalid operation\n");
-				printk(KERN_WARNING "Instatus = %02x, macstatus = %02x\n",intstatus,macstatus) ; 
-				printk(KERN_WARNING "%s: Resetting hardware:\n", dev->name);
-				netif_stop_queue(dev) ;
-				xl_freemem(dev) ; 
-				free_irq(dev->irq,dev); 
-				unregister_netdev(dev) ; 
-				free_netdev(dev) ;  
-				xl_reset(dev) ; 
-				writel(ACK_INTERRUPT | LATCH_ACK, xl_mmio + MMIO_COMMAND) ; 
-				spin_unlock(&xl_priv->xl_lock) ; 
-				return IRQ_HANDLED;
-			}
-		} else { 
-			printk(KERN_WARNING "%s: Received Unknown interrupt : %04x\n", dev->name, intstatus);
-			writel(ACK_INTERRUPT | LATCH_ACK, xl_mmio + MMIO_COMMAND) ; 	
-		}
-	} 
-
-	/* Turn interrupts back on */
-
-	writel( SETINDENABLE | INT_MASK, xl_mmio + MMIO_COMMAND) ; 
-	writel( SETINTENABLE | INT_MASK, xl_mmio + MMIO_COMMAND) ; 
-
-	spin_unlock(&xl_priv->xl_lock) ;
-	return IRQ_HANDLED;
-}	
-
-/*
- *	Tx - Polling configuration
- */
-	
-static netdev_tx_t xl_xmit(struct sk_buff *skb, struct net_device *dev)
-{
-	struct xl_private *xl_priv=netdev_priv(dev);
-	struct xl_tx_desc *txd ; 
-	int tx_head, tx_tail, tx_prev ; 
-	unsigned long flags ; 	
-
-	spin_lock_irqsave(&xl_priv->xl_lock,flags) ; 
-
-	netif_stop_queue(dev) ; 
-
-	if (xl_priv->free_ring_entries > 1 ) { 	
-		/*
-		 * Set up the descriptor for the packet 
-		 */
-		tx_head = xl_priv->tx_ring_head ; 
-		tx_tail = xl_priv->tx_ring_tail ; 
-
-		txd = &(xl_priv->xl_tx_ring[tx_head]) ; 
-		txd->dnnextptr = 0 ; 
-		txd->framestartheader = cpu_to_le32(skb->len) | TXDNINDICATE;
-		txd->buffer = cpu_to_le32(pci_map_single(xl_priv->pdev, skb->data, skb->len, PCI_DMA_TODEVICE));
-		txd->buffer_length = cpu_to_le32(skb->len) | TXDNFRAGLAST;
-		xl_priv->tx_ring_skb[tx_head] = skb ; 
-		dev->stats.tx_packets++ ; 
-		dev->stats.tx_bytes += skb->len ;
-
-		/* 
-		 * Set the nextptr of the previous descriptor equal to this descriptor, add XL_TX_RING_SIZE -1 
-		 * to ensure no negative numbers in unsigned locations.
-		 */ 
-	
-		tx_prev = (xl_priv->tx_ring_head + XL_TX_RING_SIZE - 1) & (XL_TX_RING_SIZE - 1) ; 
-
-		xl_priv->tx_ring_head++ ; 
-		xl_priv->tx_ring_head &= (XL_TX_RING_SIZE - 1) ;
-		xl_priv->free_ring_entries-- ; 
-
-		xl_priv->xl_tx_ring[tx_prev].dnnextptr = cpu_to_le32(xl_priv->tx_ring_dma_addr + (sizeof (struct xl_tx_desc) * tx_head));
-
-		/* Sneaky, by doing a read on DnListPtr we can force the card to poll on the DnNextPtr */
-		/* readl(xl_mmio + MMIO_DNLISTPTR) ; */
-
-		netif_wake_queue(dev) ; 
-
-		spin_unlock_irqrestore(&xl_priv->xl_lock,flags) ; 
- 
-		return NETDEV_TX_OK;
-	} else {
-		spin_unlock_irqrestore(&xl_priv->xl_lock,flags) ; 
-		return NETDEV_TX_BUSY;
-	}
-
-}
-	
-/* 
- * The NIC has told us that a packet has been downloaded onto the card, we must
- * find out which packet it has done, clear the skb and information for the packet
- * then advance around the ring for all transmitted packets
- */
-
-static void xl_dn_comp(struct net_device *dev) 
-{
-	struct xl_private *xl_priv=netdev_priv(dev);
-	u8 __iomem * xl_mmio = xl_priv->xl_mmio ; 
-	struct xl_tx_desc *txd ; 
-
-
-	if (xl_priv->tx_ring_tail == 255) {/* First time */
-		xl_priv->xl_tx_ring[0].framestartheader = 0 ; 
-		xl_priv->xl_tx_ring[0].dnnextptr = 0 ;  
-		xl_priv->tx_ring_tail = 1 ; 
-	}
-
-	while (xl_priv->xl_tx_ring[xl_priv->tx_ring_tail].framestartheader & TXDNCOMPLETE ) { 
-		txd = &(xl_priv->xl_tx_ring[xl_priv->tx_ring_tail]) ;
-		pci_unmap_single(xl_priv->pdev, le32_to_cpu(txd->buffer), xl_priv->tx_ring_skb[xl_priv->tx_ring_tail]->len, PCI_DMA_TODEVICE);
-		txd->framestartheader = 0 ; 
-		txd->buffer = cpu_to_le32(0xdeadbeef);
-		txd->buffer_length  = 0 ;  
-		dev_kfree_skb_irq(xl_priv->tx_ring_skb[xl_priv->tx_ring_tail]) ;
-		xl_priv->tx_ring_tail++ ; 
-		xl_priv->tx_ring_tail &= (XL_TX_RING_SIZE - 1) ; 
-		xl_priv->free_ring_entries++ ; 
-	}
-
-	netif_wake_queue(dev) ; 
-
-	writel(ACK_INTERRUPT | DNCOMPACK | LATCH_ACK , xl_mmio + MMIO_COMMAND) ; 
-}
-
-/*
- * Close the adapter properly.
- * This srb reply cannot be handled from interrupt context as we have
- * to free the interrupt from the driver. 
- */
-
-static int xl_close(struct net_device *dev) 
-{
-	struct xl_private *xl_priv = netdev_priv(dev);
-	u8 __iomem * xl_mmio = xl_priv->xl_mmio ; 
-	unsigned long t ; 
-
-	netif_stop_queue(dev) ; 
-
-	/*
-	 * Close the adapter, need to stall the rx and tx queues.
-	 */
-
-    	writew(DNSTALL, xl_mmio + MMIO_COMMAND) ; 
-	t=jiffies;
-	while (readw(xl_mmio + MMIO_INTSTATUS) & INTSTAT_CMD_IN_PROGRESS) { 
-		schedule();		
-		if (time_after(jiffies, t + 10 * HZ)) {
-			printk(KERN_ERR "%s: 3COM 3C359 Velocity XL-DNSTALL not responding.\n", dev->name);
-			break ; 
-		}
-	}
-    	writew(DNDISABLE, xl_mmio + MMIO_COMMAND) ; 
-	t=jiffies;
-	while (readw(xl_mmio + MMIO_INTSTATUS) & INTSTAT_CMD_IN_PROGRESS) { 
-		schedule();		
-		if (time_after(jiffies, t + 10 * HZ)) {
-			printk(KERN_ERR "%s: 3COM 3C359 Velocity XL-DNDISABLE not responding.\n", dev->name);
-			break ;
-		}
-	}
-    	writew(UPSTALL, xl_mmio + MMIO_COMMAND) ; 
-	t=jiffies;
-	while (readw(xl_mmio + MMIO_INTSTATUS) & INTSTAT_CMD_IN_PROGRESS) { 
-		schedule();		
-		if (time_after(jiffies, t + 10 * HZ)) {
-			printk(KERN_ERR "%s: 3COM 3C359 Velocity XL-UPSTALL not responding.\n", dev->name);
-			break ; 
-		}
-	}
-
-	/* Turn off interrupts, we will still get the indication though
- 	 * so we can trap it
-	 */
-
-	writel(SETINTENABLE, xl_mmio + MMIO_COMMAND) ; 
-
-	xl_srb_cmd(dev,CLOSE_NIC) ; 
-
-	t=jiffies;
-	while (!(readw(xl_mmio + MMIO_INTSTATUS) & INTSTAT_SRB)) { 
-		schedule();		
-		if (time_after(jiffies, t + 10 * HZ)) {
-			printk(KERN_ERR "%s: 3COM 3C359 Velocity XL-CLOSENIC not responding.\n", dev->name);
-			break ; 
-		}
-	}
-	/* Read the srb response from the adapter */
-
-	writel(MEM_BYTE_READ | 0xd0000 | xl_priv->srb, xl_mmio + MMIO_MAC_ACCESS_CMD);
-	if (readb(xl_mmio + MMIO_MACDATA) != CLOSE_NIC) { 
-		printk(KERN_INFO "%s: CLOSE_NIC did not get a CLOSE_NIC response\n",dev->name);
-	} else { 
-		writel((MEM_BYTE_READ | 0xd0000 | xl_priv->srb) +2, xl_mmio + MMIO_MAC_ACCESS_CMD) ;
-		if (readb(xl_mmio + MMIO_MACDATA)==0) { 
-			printk(KERN_INFO "%s: Adapter has been closed\n",dev->name);
-			writew(ACK_INTERRUPT | SRBRACK | LATCH_ACK, xl_mmio + MMIO_COMMAND) ; 
-
-			xl_freemem(dev) ; 
-			free_irq(dev->irq,dev) ; 
-		} else { 
-			printk(KERN_INFO "%s: Close nic command returned error code %02x\n",dev->name, readb(xl_mmio + MMIO_MACDATA)) ;
-		} 
-	}
-
-	/* Reset the upload and download logic */
- 
-    	writew(UPRESET, xl_mmio + MMIO_COMMAND) ; 
-	t=jiffies;
-	while (readw(xl_mmio + MMIO_INTSTATUS) & INTSTAT_CMD_IN_PROGRESS) { 
-		schedule();		
-		if (time_after(jiffies, t + 10 * HZ)) {
-			printk(KERN_ERR "%s: 3COM 3C359 Velocity XL-UPRESET not responding.\n", dev->name);
-			break ; 
-		}
-	}
-    	writew(DNRESET, xl_mmio + MMIO_COMMAND) ; 
-	t=jiffies;
-	while (readw(xl_mmio + MMIO_INTSTATUS) & INTSTAT_CMD_IN_PROGRESS) { 
-		schedule();		
-		if (time_after(jiffies, t + 10 * HZ)) {
-			printk(KERN_ERR "%s: 3COM 3C359 Velocity XL-DNRESET not responding.\n", dev->name);
-			break ; 
-		}
-	}
-	xl_hw_reset(dev) ; 
-	return 0 ;
-}
-
-static void xl_set_rx_mode(struct net_device *dev) 
-{
-	struct xl_private *xl_priv = netdev_priv(dev);
-	struct netdev_hw_addr *ha;
-	unsigned char dev_mc_address[4] ; 
-	u16 options ; 
-
-	if (dev->flags & IFF_PROMISC)
-		options = 0x0004 ; 
-	else
-		options = 0x0000 ; 
-
-	if (options ^ xl_priv->xl_copy_all_options) { /* Changed, must send command */
-		xl_priv->xl_copy_all_options = options ; 
-		xl_srb_cmd(dev, SET_RECEIVE_MODE) ;
-		return ;  
-	}
-
-	dev_mc_address[0] = dev_mc_address[1] = dev_mc_address[2] = dev_mc_address[3] = 0 ;
-
-	netdev_for_each_mc_addr(ha, dev) {
-		dev_mc_address[0] |= ha->addr[2];
-		dev_mc_address[1] |= ha->addr[3];
-		dev_mc_address[2] |= ha->addr[4];
-		dev_mc_address[3] |= ha->addr[5];
-        }
-
-	if (memcmp(xl_priv->xl_functional_addr,dev_mc_address,4) != 0) { /* Options have changed, run the command */
-		memcpy(xl_priv->xl_functional_addr, dev_mc_address,4) ; 
-		xl_srb_cmd(dev, SET_FUNC_ADDRESS) ; 
-	}
-	return ; 
-}
-
-
-/*
- *	We issued an srb command and now we must read
- *	the response from the completed command.
- */
-
-static void xl_srb_bh(struct net_device *dev) 
-{ 
-	struct xl_private *xl_priv = netdev_priv(dev);
-	u8 __iomem * xl_mmio = xl_priv->xl_mmio ; 
-	u8 srb_cmd, ret_code ; 
-	int i ; 
-
-	writel(MEM_BYTE_READ | 0xd0000 | xl_priv->srb, xl_mmio + MMIO_MAC_ACCESS_CMD) ;
-	srb_cmd = readb(xl_mmio + MMIO_MACDATA) ; 
-	writel((MEM_BYTE_READ | 0xd0000 | xl_priv->srb) +2, xl_mmio + MMIO_MAC_ACCESS_CMD) ;
-	ret_code = readb(xl_mmio + MMIO_MACDATA) ; 
-
-	/* Ret_code is standard across all commands */
-
-	switch (ret_code) { 
-	case 1:
-		printk(KERN_INFO "%s: Command: %d - Invalid Command code\n",dev->name,srb_cmd) ; 
-		break ; 
-	case 4:
-		printk(KERN_INFO "%s: Command: %d - Adapter is closed, must be open for this command\n",dev->name,srb_cmd);
-		break ;
-	
-	case 6:
-		printk(KERN_INFO "%s: Command: %d - Options Invalid for command\n",dev->name,srb_cmd);
-		break ;
-
-	case 0: /* Successful command execution */ 
-		switch (srb_cmd) { 
-		case READ_LOG: /* Returns 14 bytes of data from the NIC */
-			if(xl_priv->xl_message_level)
-				printk(KERN_INFO "%s: READ.LOG 14 bytes of data ",dev->name) ; 
-			/* 
-			 * We still have to read the log even if message_level = 0 and we don't want
-			 * to see it
-			 */
-			for (i=0;i<14;i++) { 
-				writel(MEM_BYTE_READ | 0xd0000 | xl_priv->srb | i, xl_mmio + MMIO_MAC_ACCESS_CMD) ; 
-				if(xl_priv->xl_message_level) 
-					printk("%02x:",readb(xl_mmio + MMIO_MACDATA)) ; 	
-			} 
-			printk("\n") ; 
-			break ; 
-		case SET_FUNC_ADDRESS:
-			if(xl_priv->xl_message_level) 
-				printk(KERN_INFO "%s: Functional Address Set\n",dev->name);
-			break ; 
-		case CLOSE_NIC:
-			if(xl_priv->xl_message_level)
-				printk(KERN_INFO "%s: Received CLOSE_NIC interrupt in interrupt handler\n",dev->name);
-			break ; 
-		case SET_MULTICAST_MODE:
-			if(xl_priv->xl_message_level)
-				printk(KERN_INFO "%s: Multicast options successfully changed\n",dev->name) ; 
-			break ;
-		case SET_RECEIVE_MODE:
-			if(xl_priv->xl_message_level) {  
-				if (xl_priv->xl_copy_all_options == 0x0004) 
-					printk(KERN_INFO "%s: Entering promiscuous mode\n", dev->name);
-				else
-					printk(KERN_INFO "%s: Entering normal receive mode\n",dev->name);
-			}
-			break ; 
- 
-		} /* switch */
-		break ; 
-	} /* switch */
-	return ; 	
-} 
-
-static int xl_set_mac_address (struct net_device *dev, void *addr) 
-{
-	struct sockaddr *saddr = addr ; 
-	struct xl_private *xl_priv = netdev_priv(dev);
-
-	if (netif_running(dev)) { 
-		printk(KERN_WARNING "%s: Cannot set mac/laa address while card is open\n", dev->name) ; 
-		return -EIO ; 
-	}
-
-	memcpy(xl_priv->xl_laa, saddr->sa_data,dev->addr_len) ; 
-	
-	if (xl_priv->xl_message_level) { 
- 		printk(KERN_INFO "%s: MAC/LAA Set to  = %x.%x.%x.%x.%x.%x\n",dev->name, xl_priv->xl_laa[0],
-		xl_priv->xl_laa[1], xl_priv->xl_laa[2],
-		xl_priv->xl_laa[3], xl_priv->xl_laa[4],
-		xl_priv->xl_laa[5]);
-	} 
-
-	return 0 ; 
-}
-
-static void xl_arb_cmd(struct net_device *dev)
-{
-	struct xl_private *xl_priv = netdev_priv(dev);
-	u8 __iomem * xl_mmio = xl_priv->xl_mmio ; 
-	u8 arb_cmd ; 
-	u16 lan_status, lan_status_diff ; 
-
-	writel( ( MEM_BYTE_READ | 0xD0000 | xl_priv->arb), xl_mmio + MMIO_MAC_ACCESS_CMD) ; 
-	arb_cmd = readb(xl_mmio + MMIO_MACDATA) ; 
-	
-	if (arb_cmd == RING_STATUS_CHANGE) { /* Ring.Status.Change */
-		writel( ( (MEM_WORD_READ | 0xD0000 | xl_priv->arb) + 6), xl_mmio + MMIO_MAC_ACCESS_CMD) ;
-		 
-		printk(KERN_INFO "%s: Ring Status Change: New Status = %04x\n", dev->name, swab16(readw(xl_mmio + MMIO_MACDATA) )) ;
-
-		lan_status = swab16(readw(xl_mmio + MMIO_MACDATA));
-	
-		/* Acknowledge interrupt, this tells nic we are done with the arb */
-		writel(ACK_INTERRUPT | ARBCACK | LATCH_ACK, xl_mmio + MMIO_COMMAND) ; 
-			
-		lan_status_diff = xl_priv->xl_lan_status ^ lan_status ; 
-
-		if (lan_status_diff & (LSC_LWF | LSC_ARW | LSC_FPE | LSC_RR) ) { 
-			if (lan_status_diff & LSC_LWF) 
-				printk(KERN_WARNING "%s: Short circuit detected on the lobe\n",dev->name);
-			if (lan_status_diff & LSC_ARW) 
-				printk(KERN_WARNING "%s: Auto removal error\n",dev->name);
-			if (lan_status_diff & LSC_FPE)
-				printk(KERN_WARNING "%s: FDX Protocol Error\n",dev->name);
-			if (lan_status_diff & LSC_RR) 
-				printk(KERN_WARNING "%s: Force remove MAC frame received\n",dev->name);
-		
-			/* Adapter has been closed by the hardware */
-
-			netif_stop_queue(dev);
-			xl_freemem(dev) ; 
-			free_irq(dev->irq,dev);
-			
-			printk(KERN_WARNING "%s: Adapter has been closed\n", dev->name);
-		} /* If serious error */
-		
-		if (xl_priv->xl_message_level) { 
-			if (lan_status_diff & LSC_SIG_LOSS) 
-					printk(KERN_WARNING "%s: No receive signal detected\n", dev->name);
-			if (lan_status_diff & LSC_HARD_ERR)
-					printk(KERN_INFO "%s: Beaconing\n",dev->name);
-			if (lan_status_diff & LSC_SOFT_ERR)
-					printk(KERN_WARNING "%s: Adapter transmitted Soft Error Report Mac Frame\n",dev->name);
-			if (lan_status_diff & LSC_TRAN_BCN) 
-					printk(KERN_INFO "%s: We are transmitting the beacon, aaah\n",dev->name);
-			if (lan_status_diff & LSC_SS) 
-					printk(KERN_INFO "%s: Single Station on the ring\n", dev->name);
-			if (lan_status_diff & LSC_RING_REC)
-					printk(KERN_INFO "%s: Ring recovery ongoing\n",dev->name);
-			if (lan_status_diff & LSC_FDX_MODE)
-					printk(KERN_INFO "%s: Operating in FDX mode\n",dev->name);
-		} 	
-		
-		if (lan_status_diff & LSC_CO) { 
-				if (xl_priv->xl_message_level) 
-					printk(KERN_INFO "%s: Counter Overflow\n", dev->name);
-				/* Issue READ.LOG command */
-				xl_srb_cmd(dev, READ_LOG) ; 	
-		}
-
-		/* There is no command in the tech docs to issue the read_sr_counters */
-		if (lan_status_diff & LSC_SR_CO) { 
-			if (xl_priv->xl_message_level)
-				printk(KERN_INFO "%s: Source routing counters overflow\n", dev->name);
-		}
-
-		xl_priv->xl_lan_status = lan_status ; 
-	
-	}  /* Lan.change.status */
-	else if ( arb_cmd == RECEIVE_DATA) { /* Received.Data */
-#if XL_DEBUG
-		printk(KERN_INFO "Received.Data\n");
-#endif 		
-		writel( ((MEM_WORD_READ | 0xD0000 | xl_priv->arb) + 6), xl_mmio + MMIO_MAC_ACCESS_CMD) ;
-		xl_priv->mac_buffer = swab16(readw(xl_mmio + MMIO_MACDATA)) ;
-		
-		/* Now we are going to be really basic here and not do anything
-		 * with the data at all. The tech docs do not give me enough
-		 * information to calculate the buffers properly so we're
-		 * just going to tell the nic that we've dealt with the frame
-		 * anyway.
-		 */
-
-		/* Acknowledge interrupt, this tells nic we are done with the arb */
-		writel(ACK_INTERRUPT | ARBCACK | LATCH_ACK, xl_mmio + MMIO_COMMAND) ; 
-
-		/* Is the ASB free ? */ 	
-			
-		xl_priv->asb_queued = 0 ; 			
-		writel( ((MEM_BYTE_READ | 0xD0000 | xl_priv->asb) + 2), xl_mmio + MMIO_MAC_ACCESS_CMD) ;
-		if (readb(xl_mmio + MMIO_MACDATA) != 0xff) { 
-			xl_priv->asb_queued = 1 ;
-
-			xl_wait_misr_flags(dev) ;  
-
-			writel(MEM_BYTE_WRITE | MF_ASBFR, xl_mmio + MMIO_MAC_ACCESS_CMD); 
-			writeb(0xff, xl_mmio + MMIO_MACDATA) ;
-			writel(MMIO_BYTE_WRITE | MISR_SET, xl_mmio + MMIO_MAC_ACCESS_CMD) ; 
-			writeb(MISR_ASBFR, xl_mmio + MMIO_MACDATA) ; 
-			return ; 	
-			/* Drop out and wait for the bottom half to be run */
-		}
-	
-		xl_asb_cmd(dev) ; 
-		
-	} else {
-		printk(KERN_WARNING "%s: Received unknown arb (xl_priv) command: %02x\n",dev->name,arb_cmd);
-	}
-
-	/* Acknowledge the arb interrupt */
-
-	writel(ACK_INTERRUPT | ARBCACK | LATCH_ACK , xl_mmio + MMIO_COMMAND) ; 
-
-	return ; 
-}
-
-
-/*
- *	There is only one asb command, but we can get called from different
- *	places.
- */
-
-static void xl_asb_cmd(struct net_device *dev)
-{
-	struct xl_private *xl_priv = netdev_priv(dev);
-	u8 __iomem * xl_mmio = xl_priv->xl_mmio ; 
-
-	if (xl_priv->asb_queued == 1) 
-		writel(ACK_INTERRUPT | LATCH_ACK | ASBFACK, xl_mmio + MMIO_COMMAND) ; 
-		
-	writel(MEM_BYTE_WRITE | 0xd0000 | xl_priv->asb, xl_mmio + MMIO_MAC_ACCESS_CMD) ; 
-	writeb(0x81, xl_mmio + MMIO_MACDATA) ; 
-
-	writel(MEM_WORD_WRITE | 0xd0000 | xl_priv->asb | 6, xl_mmio + MMIO_MAC_ACCESS_CMD) ; 
-	writew(swab16(xl_priv->mac_buffer), xl_mmio + MMIO_MACDATA) ;
-
-	xl_wait_misr_flags(dev) ; 	
-
-	writel(MEM_BYTE_WRITE | MF_RASB, xl_mmio + MMIO_MAC_ACCESS_CMD); 
-	writeb(0xff, xl_mmio + MMIO_MACDATA) ;
-
-	writel(MMIO_BYTE_WRITE | MISR_SET, xl_mmio + MMIO_MAC_ACCESS_CMD) ; 
-	writeb(MISR_RASB, xl_mmio + MMIO_MACDATA) ; 
-
-	xl_priv->asb_queued = 2 ; 
-
-	return ; 
-}
-
-/*
- * 	This will only get called if there was an error
- *	from the asb cmd.
- */
-static void xl_asb_bh(struct net_device *dev) 
-{
-	struct xl_private *xl_priv = netdev_priv(dev);
-	u8 __iomem * xl_mmio = xl_priv->xl_mmio ; 
-	u8 ret_code ; 
-
-	writel(MMIO_BYTE_READ | 0xd0000 | xl_priv->asb | 2, xl_mmio + MMIO_MAC_ACCESS_CMD) ; 
-	ret_code = readb(xl_mmio + MMIO_MACDATA) ; 
-	switch (ret_code) { 
-		case 0x01:
-			printk(KERN_INFO "%s: ASB Command, unrecognized command code\n",dev->name);
-			break ;
-		case 0x26:
-			printk(KERN_INFO "%s: ASB Command, unexpected receive buffer\n", dev->name);
-			break ; 
-		case 0x40:
-			printk(KERN_INFO "%s: ASB Command, Invalid Station ID\n", dev->name);
-			break ;  
-	}
-	xl_priv->asb_queued = 0 ; 
-	writel(ACK_INTERRUPT | LATCH_ACK | ASBFACK, xl_mmio + MMIO_COMMAND) ;
-	return ;  
-}
-
-/* 	
- *	Issue srb commands to the nic 
- */
-
-static void xl_srb_cmd(struct net_device *dev, int srb_cmd) 
-{
-	struct xl_private *xl_priv = netdev_priv(dev);
-	u8 __iomem * xl_mmio = xl_priv->xl_mmio ; 
-
-	switch (srb_cmd) { 
-	case READ_LOG:
-		writel(MEM_BYTE_WRITE | 0xD0000 | xl_priv->srb, xl_mmio + MMIO_MAC_ACCESS_CMD) ; 
-		writeb(READ_LOG, xl_mmio + MMIO_MACDATA) ; 
-		break; 
-
-	case CLOSE_NIC:
-		writel(MEM_BYTE_WRITE | 0xD0000 | xl_priv->srb, xl_mmio + MMIO_MAC_ACCESS_CMD) ; 
-		writeb(CLOSE_NIC, xl_mmio + MMIO_MACDATA) ; 
-		break ;
-
-	case SET_RECEIVE_MODE:
-		writel(MEM_BYTE_WRITE | 0xD0000 | xl_priv->srb, xl_mmio + MMIO_MAC_ACCESS_CMD) ; 
-		writeb(SET_RECEIVE_MODE, xl_mmio + MMIO_MACDATA) ; 
-		writel(MEM_WORD_WRITE | 0xD0000 | xl_priv->srb | 4, xl_mmio + MMIO_MAC_ACCESS_CMD) ; 
-		writew(xl_priv->xl_copy_all_options, xl_mmio + MMIO_MACDATA) ; 
-		break ;
-
-	case SET_FUNC_ADDRESS:
-		writel(MEM_BYTE_WRITE | 0xD0000 | xl_priv->srb, xl_mmio + MMIO_MAC_ACCESS_CMD) ; 
-		writeb(SET_FUNC_ADDRESS, xl_mmio + MMIO_MACDATA) ; 
-		writel(MEM_BYTE_WRITE | 0xD0000 | xl_priv->srb | 6 , xl_mmio + MMIO_MAC_ACCESS_CMD) ; 
-		writeb(xl_priv->xl_functional_addr[0], xl_mmio + MMIO_MACDATA) ; 
-		writel(MEM_BYTE_WRITE | 0xD0000 | xl_priv->srb | 7 , xl_mmio + MMIO_MAC_ACCESS_CMD) ; 
-		writeb(xl_priv->xl_functional_addr[1], xl_mmio + MMIO_MACDATA) ; 
-		writel(MEM_BYTE_WRITE | 0xD0000 | xl_priv->srb | 8 , xl_mmio + MMIO_MAC_ACCESS_CMD) ; 
-		writeb(xl_priv->xl_functional_addr[2], xl_mmio + MMIO_MACDATA) ; 
-		writel(MEM_BYTE_WRITE | 0xD0000 | xl_priv->srb | 9 , xl_mmio + MMIO_MAC_ACCESS_CMD) ; 
-		writeb(xl_priv->xl_functional_addr[3], xl_mmio + MMIO_MACDATA) ;
-		break ;  
-	} /* switch */
-
-
-	xl_wait_misr_flags(dev)  ; 
-
-	/* Write 0xff to the CSRB flag */
-	writel(MEM_BYTE_WRITE | MF_CSRB , xl_mmio + MMIO_MAC_ACCESS_CMD) ; 
-	writeb(0xFF, xl_mmio + MMIO_MACDATA) ; 
-	/* Set csrb bit in MISR register to process command */
-	writel(MMIO_BYTE_WRITE | MISR_SET, xl_mmio + MMIO_MAC_ACCESS_CMD) ; 
-	writeb(MISR_CSRB, xl_mmio + MMIO_MACDATA) ; 
-	xl_priv->srb_queued = 1 ; 
-
-	return ; 
-}
-
-/*
- * This is nasty, to use the MISR command you have to wait for 6 memory locations
- * to be zero. This is the way the driver does on other OS'es so we should be ok with 
- * the empty loop.
- */
-
-static void xl_wait_misr_flags(struct net_device *dev) 
-{
-	struct xl_private *xl_priv = netdev_priv(dev);
-	u8 __iomem * xl_mmio = xl_priv->xl_mmio ; 
-	
-	int i  ; 
-	
-	writel(MMIO_BYTE_READ | MISR_RW, xl_mmio + MMIO_MAC_ACCESS_CMD) ; 
-	if (readb(xl_mmio + MMIO_MACDATA) != 0) {  /* Misr not clear */
-		for (i=0; i<6; i++) { 
-			writel(MEM_BYTE_READ | 0xDFFE0 | i, xl_mmio + MMIO_MAC_ACCESS_CMD) ; 
-			while (readb(xl_mmio + MMIO_MACDATA) != 0) {
-				;	/* Empty Loop */
-			}
-		} 
-	}
-
-	writel(MMIO_BYTE_WRITE | MISR_AND, xl_mmio + MMIO_MAC_ACCESS_CMD) ; 
-	writeb(0x80, xl_mmio + MMIO_MACDATA) ; 
-
-	return ; 
-} 
-
-/*
- *	Change mtu size, this should work the same as olympic
- */
-
-static int xl_change_mtu(struct net_device *dev, int mtu) 
-{
-	struct xl_private *xl_priv = netdev_priv(dev);
-	u16 max_mtu ; 
-
-	if (xl_priv->xl_ring_speed == 4)
-		max_mtu = 4500 ; 
-	else
-		max_mtu = 18000 ; 
-	
-	if (mtu > max_mtu)
-		return -EINVAL ; 
-	if (mtu < 100) 
-		return -EINVAL ; 
-
-	dev->mtu = mtu ; 
-	xl_priv->pkt_buf_sz = mtu + TR_HLEN ; 
-
-	return 0 ; 
-}
-
-static void __devexit xl_remove_one (struct pci_dev *pdev)
-{
-	struct net_device *dev = pci_get_drvdata(pdev);
-	struct xl_private *xl_priv=netdev_priv(dev);
-	
-	release_firmware(xl_priv->fw);
-	unregister_netdev(dev);
-	iounmap(xl_priv->xl_mmio) ; 
-	pci_release_regions(pdev) ; 
-	pci_set_drvdata(pdev,NULL) ; 
-	free_netdev(dev);
-	return ; 
-}
-
-static struct pci_driver xl_3c359_driver = {
-	.name		= "3c359",
-	.id_table	= xl_pci_tbl,
-	.probe		= xl_probe,
-	.remove		= __devexit_p(xl_remove_one),
-};
-
-static int __init xl_pci_init (void)
-{
-	return pci_register_driver(&xl_3c359_driver);
-}
-
-
-static void __exit xl_pci_cleanup (void)
-{
-	pci_unregister_driver (&xl_3c359_driver);
-}
-
-module_init(xl_pci_init);
-module_exit(xl_pci_cleanup);
-
-MODULE_LICENSE("GPL") ; 
diff --git a/drivers/net/tokenring/3c359.h b/drivers/net/tokenring/3c359.h
deleted file mode 100644
index bcb1a6b4a4c..00000000000
--- a/drivers/net/tokenring/3c359.h
+++ /dev/null
@@ -1,291 +0,0 @@
-/*
- *  3c359.h (c) 2000 Mike Phillips (mikep@linuxtr.net) All Rights Reserved
- *
- *  Linux driver for 3Com 3C359 Token Link PCI XL cards.
- *
- *  This software may be used and distributed according to the terms
- *  of the GNU General Public License Version 2 or (at your option) 
- *  any later verion, incorporated herein by reference.
- */
-
-/* Memory Access Commands */
-#define IO_BYTE_READ 0x28 << 24
-#define IO_BYTE_WRITE 0x18 << 24 
-#define IO_WORD_READ 0x20 << 24
-#define IO_WORD_WRITE 0x10 << 24
-#define MMIO_BYTE_READ 0x88 << 24
-#define MMIO_BYTE_WRITE 0x48 << 24
-#define MMIO_WORD_READ 0x80 << 24
-#define MMIO_WORD_WRITE 0x40 << 24
-#define MEM_BYTE_READ 0x8C << 24
-#define MEM_BYTE_WRITE 0x4C << 24
-#define MEM_WORD_READ 0x84 << 24
-#define MEM_WORD_WRITE 0x44 << 24
-
-#define PMBAR 0x1C80
-#define PMB_CPHOLD (1<<10)
-
-#define CPATTENTION 0x180D
-#define CPA_PMBARVIS (1<<7)
-#define CPA_MEMWREN (1<<6)
-
-#define SWITCHSETTINGS 0x1C88
-#define EECONTROL 0x1C8A
-#define EEDATA 0x1C8C
-#define EEREAD 0x0080 
-#define EEWRITE 0x0040
-#define EEERASE 0x0060
-#define EE_ENABLE_WRITE 0x0030
-#define EEBUSY (1<<15)
-
-#define WRBR 0xCDE02
-#define WWOR 0xCDE04
-#define WWCR 0xCDE06
-#define MACSTATUS 0xCDE08 
-#define MISR_RW 0xCDE0B
-#define MISR_AND 0xCDE2B
-#define MISR_SET 0xCDE4B
-#define RXBUFAREA 0xCDE10
-#define RXEARLYTHRESH 0xCDE12
-#define TXSTARTTHRESH 0x58
-#define DNPRIREQTHRESH 0x2C
-
-#define MISR_CSRB (1<<5)
-#define MISR_RASB (1<<4)
-#define MISR_SRBFR (1<<3)
-#define MISR_ASBFR (1<<2)
-#define MISR_ARBF (1<<1) 
-
-/* MISR Flags memory locations */
-#define MF_SSBF 0xDFFE0 
-#define MF_ARBF 0xDFFE1
-#define MF_ASBFR 0xDFFE2
-#define MF_SRBFR 0xDFFE3
-#define MF_RASB 0xDFFE4
-#define MF_CSRB 0xDFFE5
-
-#define MMIO_MACDATA 0x10 
-#define MMIO_MAC_ACCESS_CMD 0x14
-#define MMIO_TIMER 0x1A
-#define MMIO_DMA_CTRL 0x20
-#define MMIO_DNLISTPTR 0x24
-#define MMIO_HASHFILTER 0x28
-#define MMIO_CONFIG 0x29
-#define MMIO_DNPRIREQTHRESH 0x2C
-#define MMIO_DNPOLL 0x2D
-#define MMIO_UPPKTSTATUS 0x30
-#define MMIO_FREETIMER 0x34
-#define MMIO_COUNTDOWN 0x36
-#define MMIO_UPLISTPTR 0x38
-#define MMIO_UPPOLL 0x3C
-#define MMIO_UPBURSTTHRESH 0x40
-#define MMIO_DNBURSTTHRESH 0x41
-#define MMIO_INTSTATUS_AUTO 0x56
-#define MMIO_TXSTARTTHRESH 0x58
-#define MMIO_INTERRUPTENABLE 0x5A
-#define MMIO_INDICATIONENABLE 0x5C
-#define MMIO_COMMAND 0x5E  /* These two are meant to be the same */
-#define MMIO_INTSTATUS 0x5E /* Makes the code more readable this way */
-#define INTSTAT_CMD_IN_PROGRESS (1<<12) 
-#define INTSTAT_SRB (1<<14)
-#define INTSTAT_INTLATCH (1<<0)
-
-/* Indication / Interrupt Mask 
- * Annoyingly the bits to be set in the indication and interrupt enable
- * do not match with the actual bits received in the interrupt, although
- * they are in the same order. 
- * The mapping for the indication / interrupt are:
- * Bit	Indication / Interrupt
- *   0	HostError
- *   1	txcomplete
- *   2	updneeded
- *   3	rxcomplete
- *   4	intrequested
- *   5	macerror
- *   6  dncomplete
- *   7	upcomplete
- *   8	txunderrun
- *   9	asbf
- *  10	srbr
- *  11	arbc
- *
- *  The only ones we don't want to receive are txcomplete and rxcomplete
- *  we use dncomplete and upcomplete instead.
- */
-
-#define INT_MASK 0xFF5
-
-/* Note the subtle difference here, IND and INT */
-
-#define SETINDENABLE (8<<12)
-#define SETINTENABLE (7<<12)
-#define SRBBIT (1<<10)
-#define ASBBIT (1<<9)
-#define ARBBIT (1<<11)
-
-#define SRB 0xDFE90
-#define ASB 0xDFED0
-#define ARB 0xD0000
-#define SCRATCH 0xDFEF0
-
-#define INT_REQUEST 0x6000 /* (6 << 12) */
-#define ACK_INTERRUPT 0x6800 /* (13 <<11) */
-#define GLOBAL_RESET 0x00 
-#define DNDISABLE 0x5000 
-#define DNENABLE 0x4800 
-#define DNSTALL 0x3002
-#define DNRESET 0x5800
-#define DNUNSTALL 0x3003
-#define UPRESET 0x2800
-#define UPSTALL 0x3000
-#define UPUNSTALL 0x3001
-#define SETCONFIG 0x4000
-#define SETTXSTARTTHRESH 0x9800 
-
-/* Received Interrupts */
-#define ASBFINT (1<<13)
-#define SRBRINT (1<<14)
-#define ARBCINT (1<<15)
-#define TXUNDERRUN (1<<11)
-
-#define UPCOMPINT (1<<10)
-#define DNCOMPINT (1<<9)
-#define HARDERRINT (1<<7)
-#define RXCOMPLETE (1<<4)
-#define TXCOMPINT (1<<2)
-#define HOSTERRINT (1<<1)
-
-/* Receive descriptor bits */
-#define RXOVERRUN cpu_to_le32(1<<19)
-#define RXFC cpu_to_le32(1<<21)
-#define RXAR cpu_to_le32(1<<22)
-#define RXUPDCOMPLETE cpu_to_le32(1<<23)
-#define RXUPDFULL cpu_to_le32(1<<24)
-#define RXUPLASTFRAG cpu_to_le32(1<<31)
-
-/* Transmit descriptor bits */
-#define TXDNCOMPLETE cpu_to_le32(1<<16)
-#define TXTXINDICATE cpu_to_le32(1<<27)
-#define TXDPDEMPTY cpu_to_le32(1<<29)
-#define TXDNINDICATE cpu_to_le32(1<<31)
-#define TXDNFRAGLAST cpu_to_le32(1<<31)
-
-/* Interrupts to Acknowledge */
-#define LATCH_ACK 1 
-#define TXCOMPACK (1<<1)
-#define INTREQACK (1<<2)
-#define DNCOMPACK (1<<3)
-#define UPCOMPACK (1<<4)
-#define ASBFACK (1<<5)
-#define SRBRACK (1<<6)
-#define ARBCACK (1<<7)
-
-#define XL_IO_SPACE 128
-#define SRB_COMMAND_SIZE 50
-
-/* Adapter Commands */
-#define REQUEST_INT 0x00
-#define MODIFY_OPEN_PARMS 0x01
-#define RESTORE_OPEN_PARMS 0x02
-#define OPEN_NIC 0x03
-#define CLOSE_NIC 0x04
-#define SET_SLEEP_MODE 0x05
-#define SET_GROUP_ADDRESS 0x06
-#define SET_FUNC_ADDRESS 0x07
-#define READ_LOG 0x08
-#define SET_MULTICAST_MODE 0x0C
-#define CHANGE_WAKEUP_PATTERN 0x0D
-#define GET_STATISTICS 0x13
-#define SET_RECEIVE_MODE 0x1F
-
-/* ARB Commands */
-#define RECEIVE_DATA 0x81
-#define RING_STATUS_CHANGE 0x84
-
-/* ASB Commands */
-#define ASB_RECEIVE_DATE 0x81 
-
-/* Defines for LAN STATUS CHANGE reports */
-#define LSC_SIG_LOSS 0x8000
-#define LSC_HARD_ERR 0x4000
-#define LSC_SOFT_ERR 0x2000
-#define LSC_TRAN_BCN 0x1000
-#define LSC_LWF      0x0800
-#define LSC_ARW      0x0400
-#define LSC_FPE      0x0200
-#define LSC_RR       0x0100
-#define LSC_CO       0x0080
-#define LSC_SS       0x0040
-#define LSC_RING_REC 0x0020
-#define LSC_SR_CO    0x0010
-#define LSC_FDX_MODE 0x0004
-
-#define XL_MAX_ADAPTERS 8 /* 0x08 __MODULE_STRING can't hand 0xnn */
-
-/* 3c359 defaults for buffers */
- 
-#define XL_RX_RING_SIZE 16 /* must be a power of 2 */
-#define XL_TX_RING_SIZE 16 /* must be a power of 2 */
-
-#define PKT_BUF_SZ 4096 /* Default packet size */
-
-/* 3c359 data structures */
-
-struct xl_tx_desc {
-	__le32 dnnextptr;
-	__le32 framestartheader;
-	__le32 buffer;
-	__le32 buffer_length;
-};
-
-struct xl_rx_desc {
-	__le32 upnextptr;
-	__le32 framestatus;
-	__le32 upfragaddr;
-	__le32 upfraglen;
-};
-
-struct xl_private {
-	
-
-	/* These two structures must be aligned on 8 byte boundaries */
-
-	/* struct xl_rx_desc xl_rx_ring[XL_RX_RING_SIZE]; */
-	/* struct xl_tx_desc xl_tx_ring[XL_TX_RING_SIZE]; */
-	struct xl_rx_desc *xl_rx_ring ; 
-	struct xl_tx_desc *xl_tx_ring ; 
-	struct sk_buff *tx_ring_skb[XL_TX_RING_SIZE], *rx_ring_skb[XL_RX_RING_SIZE];	
-	int tx_ring_head, tx_ring_tail ;  
-	int rx_ring_tail, rx_ring_no ; 
-	int free_ring_entries ; 
-
-	u16 srb;
-	u16 arb;
-	u16 asb;
-
-	u8 __iomem *xl_mmio;
-	const char *xl_card_name;
-	struct pci_dev *pdev ; 
-	
-	spinlock_t xl_lock ; 
-
-	volatile int srb_queued;    
-	struct wait_queue *srb_wait;
-	volatile int asb_queued;   
-
-	u16 mac_buffer ; 	
-	u16 xl_lan_status ;
-	u8 xl_ring_speed ;
-	u16 pkt_buf_sz ; 
-	u8 xl_message_level; 
-	u16 xl_copy_all_options ;  
-	unsigned char xl_functional_addr[4] ; 
-	u16 xl_addr_table_addr, xl_parms_addr ; 
-	u8 xl_laa[6] ; 
-	u32 rx_ring_dma_addr ; 
-	u32 tx_ring_dma_addr ; 
-
-	/* firmware section */
-	const struct firmware *fw;
-};
-
diff --git a/drivers/net/tokenring/Kconfig b/drivers/net/tokenring/Kconfig
deleted file mode 100644
index 45550d42b36..00000000000
--- a/drivers/net/tokenring/Kconfig
+++ /dev/null
@@ -1,199 +0,0 @@
-#
-# Token Ring driver configuration
-#
-
-# So far, we only have PCI, ISA, and MCA token ring devices
-menuconfig TR
-	bool "Token Ring driver support"
-	depends on NETDEVICES && !UML
-	depends on (PCI || ISA || MCA || CCW || PCMCIA)
-	help
-	  Token Ring is IBM's way of communication on a local network; the
-	  rest of the world uses Ethernet. To participate on a Token Ring
-	  network, you need a special Token ring network card. If you are
-	  connected to such a Token Ring network and want to use your Token
-	  Ring card under Linux, say Y here and to the driver for your
-	  particular card below and read the Token-Ring mini-HOWTO, available
-	  from <http://www.tldp.org/docs.html#howto>. Most people can
-	  say N here.
-
-if TR
-
-config WANT_LLC
-	def_bool y
-	select LLC
-
-config PCMCIA_IBMTR
-	tristate "IBM PCMCIA tokenring adapter support"
-	depends on IBMTR!=y && PCMCIA
-	---help---
-	  Say Y here if you intend to attach this type of Token Ring PCMCIA
-	  card to your computer. You then also need to say Y to "Token Ring
-	  driver support".
-
-	  To compile this driver as a module, choose M here: the module will be
-	  called ibmtr_cs.
-
-config IBMTR
-	tristate "IBM Tropic chipset based adapter support"
-	depends on ISA || MCA
-	---help---
-	  This is support for all IBM Token Ring cards that don't use DMA. If
-	  you have such a beast, say Y and read the Token-Ring mini-HOWTO,
-	  available from <http://www.tldp.org/docs.html#howto>.
-
-	  Warning: this driver will almost definitely fail if more than one
-	  active Token Ring card is present.
-
-	  To compile this driver as a module, choose M here: the module will be
-	  called ibmtr.
-
-config IBMOL
-	tristate "IBM Olympic chipset PCI adapter support"
-	depends on PCI
-	---help---
-	  This is support for all non-Lanstreamer IBM PCI Token Ring Cards.
-	  Specifically this is all IBM PCI, PCI Wake On Lan, PCI II, PCI II
-	  Wake On Lan, and PCI 100/16/4 adapters.
-
-	  If you have such an adapter, say Y and read the Token-Ring
-	  mini-HOWTO, available from <http://www.tldp.org/docs.html#howto>.
-
-	  To compile this driver as a module, choose M here: the module will be
-	  called olympic.
-
-	  Also read <file:Documentation/networking/olympic.txt> or check the
-	  Linux Token Ring Project site for the latest information at
-	  <http://www.linuxtr.net/>.
-
-config IBMLS
-	tristate "IBM Lanstreamer chipset PCI adapter support"
-	depends on PCI && !64BIT
-	help
-	  This is support for IBM Lanstreamer PCI Token Ring Cards.
-
-	  If you have such an adapter, say Y and read the Token-Ring
-	  mini-HOWTO, available from <http://www.tldp.org/docs.html#howto>.
-
-	  To compile this driver as a module, choose M here: the module will be
-	  called lanstreamer.
-
-config 3C359
-	tristate "3Com 3C359 Token Link Velocity XL adapter support"
-	depends on PCI
-	---help---
-	  This is support for the 3Com PCI Velocity XL cards, specifically
-	  the 3Com 3C359, please note this is not for the 3C339 cards, you
-	  should use the tms380 driver instead.
-
-	  If you have such an adapter, say Y and read the Token-Ring
-	  mini-HOWTO, available from <http://www.tldp.org/docs.html#howto>.
-
-	  To compile this driver as a module, choose M here: the module will be
-	  called 3c359.
-
-	  Also read the file <file:Documentation/networking/3c359.txt> or check the 
-	  Linux Token Ring Project site for the latest information at
-	  <http://www.linuxtr.net>
-
-config TMS380TR
-	tristate "Generic TMS380 Token Ring ISA/PCI adapter support"
-	depends on PCI || ISA && ISA_DMA_API || MCA
-	select FW_LOADER
-	---help---
-	  This driver provides generic support for token ring adapters
-	  based on the Texas Instruments TMS380 series chipsets.  This
-	  includes the SysKonnect TR4/16(+) ISA (SK-4190), SysKonnect
-	  TR4/16(+) PCI (SK-4590), SysKonnect TR4/16 PCI (SK-4591),
-	  Compaq 4/16 PCI, Thomas-Conrad TC4048 4/16 PCI, and several
-	  Madge adapters.  If you say Y here, you will be asked to select
-	  which cards to support below.  If you're using modules, each
-	  class of card will be supported by a separate module.
-
-	  If you have such an adapter and would like to use it, say Y and
-	  read the Token-Ring mini-HOWTO, available from
-	  <http://www.tldp.org/docs.html#howto>.
-
-	  Also read the file <file:Documentation/networking/tms380tr.txt> or
-	  check <http://www.auk.cx/tms380tr/>.
-
-	  To compile this driver as a module, choose M here: the module will be
-	  called tms380tr.
-
-config TMSPCI
-	tristate "Generic TMS380 PCI support"
-	depends on TMS380TR && PCI
-	---help---
-	  This tms380 module supports generic TMS380-based PCI cards.
-
-	  These cards are known to work:
-	  - Compaq 4/16 TR PCI
-	  - SysKonnect TR4/16 PCI (SK-4590/SK-4591)
-	  - Thomas-Conrad TC4048 PCI 4/16
-	  - 3Com Token Link Velocity
-
-	  To compile this driver as a module, choose M here: the module will be
-	  called tmspci.
-
-config SKISA
-	tristate "SysKonnect TR4/16 ISA support"
-	depends on TMS380TR && ISA
-	help
-	  This tms380 module supports SysKonnect TR4/16 ISA cards.
-
-	  These cards are known to work:
-	  - SysKonnect TR4/16 ISA (SK-4190)
-
-	  To compile this driver as a module, choose M here: the module will be
-	  called skisa.
-
-config PROTEON
-	tristate "Proteon ISA support"
-	depends on TMS380TR && ISA
-	help
-	  This tms380 module supports Proteon ISA cards.
-
-	  These cards are known to work:
-	  - Proteon 1392
-	  - Proteon 1392 plus
-
-	  To compile this driver as a module, choose M here: the module will be
-	  called proteon.
-
-config ABYSS
-	tristate "Madge Smart 16/4 PCI Mk2 support"
-	depends on TMS380TR && PCI
-	help
-	  This tms380 module supports the Madge Smart 16/4 PCI Mk2
-	  cards (51-02).
-
-	  To compile this driver as a module, choose M here: the module will be
-	  called abyss.
-
-config MADGEMC
-	tristate "Madge Smart 16/4 Ringnode MicroChannel"
-	depends on TMS380TR && MCA
-	help
-	  This tms380 module supports the Madge Smart 16/4 MC16 and MC32
-	  MicroChannel adapters.
-
-	  To compile this driver as a module, choose M here: the module will be
-	  called madgemc.
-
-config SMCTR
-	tristate "SMC ISA/MCA adapter support"
-	depends on (ISA || MCA_LEGACY) && (BROKEN || !64BIT)
-	---help---
-	  This is support for the ISA and MCA SMC Token Ring cards,
-	  specifically SMC TokenCard Elite (8115T) and SMC TokenCard Elite/A
-	  (8115T/A) adapters.
-
-	  If you have such an adapter and would like to use it, say Y or M and
-	  read the Token-Ring mini-HOWTO, available from
-	  <http://www.tldp.org/docs.html#howto> and the file
-	  <file:Documentation/networking/smctr.txt>.
-
-	  To compile this driver as a module, choose M here: the module will be
-	  called smctr.
-
-endif # TR
diff --git a/drivers/net/tokenring/Makefile b/drivers/net/tokenring/Makefile
deleted file mode 100644
index f1be8d97b7a..00000000000
--- a/drivers/net/tokenring/Makefile
+++ /dev/null
@@ -1,16 +0,0 @@
-#
-# Makefile for drivers/net/tokenring
-#
-
-obj-$(CONFIG_PCMCIA_IBMTR)	+= ibmtr_cs.o
-obj-$(CONFIG_IBMTR)	+= ibmtr.o
-obj-$(CONFIG_IBMOL)	+= olympic.o
-obj-$(CONFIG_IBMLS)	+= lanstreamer.o
-obj-$(CONFIG_TMS380TR)	+= tms380tr.o
-obj-$(CONFIG_ABYSS)	+= abyss.o
-obj-$(CONFIG_MADGEMC)	+= madgemc.o
-obj-$(CONFIG_PROTEON)	+= proteon.o
-obj-$(CONFIG_TMSPCI)	+= tmspci.o
-obj-$(CONFIG_SKISA)	+= skisa.o
-obj-$(CONFIG_SMCTR)	+= smctr.o
-obj-$(CONFIG_3C359)	+= 3c359.o
diff --git a/drivers/net/tokenring/abyss.c b/drivers/net/tokenring/abyss.c
deleted file mode 100644
index b715e6b444d..00000000000
--- a/drivers/net/tokenring/abyss.c
+++ /dev/null
@@ -1,468 +0,0 @@
-/*
- *  abyss.c: Network driver for the Madge Smart 16/4 PCI Mk2 token ring card.
- *
- *  Written 1999-2000 by Adam Fritzler
- *
- *  This software may be used and distributed according to the terms
- *  of the GNU General Public License, incorporated herein by reference.
- *
- *  This driver module supports the following cards:
- *      - Madge Smart 16/4 PCI Mk2
- *
- *  Maintainer(s):
- *    AF	Adam Fritzler
- *
- *  Modification History:
- *	30-Dec-99	AF	Split off from the tms380tr driver.
- *	22-Jan-00	AF	Updated to use indirect read/writes 
- *	23-Nov-00	JG	New PCI API, cleanups
- *
- *
- *  TODO:
- *	1. See if we can use MMIO instead of inb/outb/inw/outw
- *	2. Add support for Mk1 (has AT24 attached to the PCI
- *		config registers)
- *
- */
-
-#include <linux/module.h>
-#include <linux/kernel.h>
-#include <linux/errno.h>
-#include <linux/pci.h>
-#include <linux/init.h>
-#include <linux/netdevice.h>
-#include <linux/trdevice.h>
-
-#include <asm/io.h>
-#include <asm/irq.h>
-
-#include "tms380tr.h"
-#include "abyss.h"            /* Madge-specific constants */
-
-static char version[] __devinitdata =
-"abyss.c: v1.02 23/11/2000 by Adam Fritzler\n";
-
-#define ABYSS_IO_EXTENT 64
-
-static DEFINE_PCI_DEVICE_TABLE(abyss_pci_tbl) = {
-	{ PCI_VENDOR_ID_MADGE, PCI_DEVICE_ID_MADGE_MK2,
-	  PCI_ANY_ID, PCI_ANY_ID, PCI_CLASS_NETWORK_TOKEN_RING << 8, 0x00ffffff, },
-	{ }			/* Terminating entry */
-};
-MODULE_DEVICE_TABLE(pci, abyss_pci_tbl);
-
-MODULE_LICENSE("GPL");
-
-static int abyss_open(struct net_device *dev);
-static int abyss_close(struct net_device *dev);
-static void abyss_enable(struct net_device *dev);
-static int abyss_chipset_init(struct net_device *dev);
-static void abyss_read_eeprom(struct net_device *dev);
-static unsigned short abyss_setnselout_pins(struct net_device *dev);
-
-static void at24_writedatabyte(unsigned long regaddr, unsigned char byte);
-static int at24_sendfullcmd(unsigned long regaddr, unsigned char cmd, unsigned char addr);
-static int at24_sendcmd(unsigned long regaddr, unsigned char cmd);
-static unsigned char at24_readdatabit(unsigned long regaddr);
-static unsigned char at24_readdatabyte(unsigned long regaddr);
-static int at24_waitforack(unsigned long regaddr);
-static int at24_waitfornack(unsigned long regaddr);
-static void at24_setlines(unsigned long regaddr, unsigned char clock, unsigned char data);
-static void at24_start(unsigned long regaddr);
-static unsigned char at24_readb(unsigned long regaddr, unsigned char addr);
-
-static unsigned short abyss_sifreadb(struct net_device *dev, unsigned short reg)
-{
-	return inb(dev->base_addr + reg);
-}
-
-static unsigned short abyss_sifreadw(struct net_device *dev, unsigned short reg)
-{
-	return inw(dev->base_addr + reg);
-}
-
-static void abyss_sifwriteb(struct net_device *dev, unsigned short val, unsigned short reg)
-{
-	outb(val, dev->base_addr + reg);
-}
-
-static void abyss_sifwritew(struct net_device *dev, unsigned short val, unsigned short reg)
-{
-	outw(val, dev->base_addr + reg);
-}
-
-static struct net_device_ops abyss_netdev_ops;
-
-static int __devinit abyss_attach(struct pci_dev *pdev, const struct pci_device_id *ent)
-{	
-	static int versionprinted;
-	struct net_device *dev;
-	struct net_local *tp;
-	int ret, pci_irq_line;
-	unsigned long pci_ioaddr;
-	
-	if (versionprinted++ == 0)
-		printk("%s", version);
-
-	if (pci_enable_device(pdev))
-		return -EIO;
-
-	/* Remove I/O space marker in bit 0. */
-	pci_irq_line = pdev->irq;
-	pci_ioaddr = pci_resource_start (pdev, 0);
-		
-	/* At this point we have found a valid card. */
-		
-	dev = alloc_trdev(sizeof(struct net_local));
-	if (!dev)
-		return -ENOMEM;
-
-	if (!request_region(pci_ioaddr, ABYSS_IO_EXTENT, dev->name)) {
-		ret = -EBUSY;
-		goto err_out_trdev;
-	}
-		
-	ret = request_irq(pdev->irq, tms380tr_interrupt, IRQF_SHARED,
-			  dev->name, dev);
-	if (ret)
-		goto err_out_region;
-		
-	dev->base_addr	= pci_ioaddr;
-	dev->irq	= pci_irq_line;
-		
-	printk("%s: Madge Smart 16/4 PCI Mk2 (Abyss)\n", dev->name);
-	printk("%s:    IO: %#4lx  IRQ: %d\n",
-	       dev->name, pci_ioaddr, dev->irq);
-	/*
-	 * The TMS SIF registers lay 0x10 above the card base address.
-	 */
-	dev->base_addr += 0x10;
-		
-	ret = tmsdev_init(dev, &pdev->dev);
-	if (ret) {
-		printk("%s: unable to get memory for dev->priv.\n", 
-		       dev->name);
-		goto err_out_irq;
-	}
-
-	abyss_read_eeprom(dev);
-
-	printk("%s:    Ring Station Address: %pM\n", dev->name, dev->dev_addr);
-
-	tp = netdev_priv(dev);
-	tp->setnselout = abyss_setnselout_pins;
-	tp->sifreadb = abyss_sifreadb;
-	tp->sifreadw = abyss_sifreadw;
-	tp->sifwriteb = abyss_sifwriteb;
-	tp->sifwritew = abyss_sifwritew;
-
-	memcpy(tp->ProductID, "Madge PCI 16/4 Mk2", PROD_ID_SIZE + 1);
-		
-	dev->netdev_ops = &abyss_netdev_ops;
-
-	pci_set_drvdata(pdev, dev);
-	SET_NETDEV_DEV(dev, &pdev->dev);
-
-	ret = register_netdev(dev);
-	if (ret)
-		goto err_out_tmsdev;
-	return 0;
-
-err_out_tmsdev:
-	pci_set_drvdata(pdev, NULL);
-	tmsdev_term(dev);
-err_out_irq:
-	free_irq(pdev->irq, dev);
-err_out_region:
-	release_region(pci_ioaddr, ABYSS_IO_EXTENT);
-err_out_trdev:
-	free_netdev(dev);
-	return ret;
-}
-
-static unsigned short abyss_setnselout_pins(struct net_device *dev)
-{
-	unsigned short val = 0;
-	struct net_local *tp = netdev_priv(dev);
-	
-	if(tp->DataRate == SPEED_4)
-		val |= 0x01;  /* Set 4Mbps */
-	else
-		val |= 0x00;  /* Set 16Mbps */
-	
-	return val;
-}
-
-/*
- * The following Madge boards should use this code:
- *   - Smart 16/4 PCI Mk2 (Abyss)
- *   - Smart 16/4 PCI Mk1 (PCI T)
- *   - Smart 16/4 Client Plus PnP (Big Apple)
- *   - Smart 16/4 Cardbus Mk2
- *
- * These access an Atmel AT24 SEEPROM using their glue chip registers. 
- *
- */
-static void at24_writedatabyte(unsigned long regaddr, unsigned char byte)
-{
-	int i;
-	
-	for (i = 0; i < 8; i++) {
-		at24_setlines(regaddr, 0, (byte >> (7-i))&0x01);
-		at24_setlines(regaddr, 1, (byte >> (7-i))&0x01);
-		at24_setlines(regaddr, 0, (byte >> (7-i))&0x01);
-	}
-}
-
-static int at24_sendfullcmd(unsigned long regaddr, unsigned char cmd, unsigned char addr)
-{
-	if (at24_sendcmd(regaddr, cmd)) {
-		at24_writedatabyte(regaddr, addr);
-		return at24_waitforack(regaddr);
-	}
-	return 0;
-}
-
-static int at24_sendcmd(unsigned long regaddr, unsigned char cmd)
-{
-	int i;
-	
-	for (i = 0; i < 10; i++) {
-		at24_start(regaddr);
-		at24_writedatabyte(regaddr, cmd);
-		if (at24_waitforack(regaddr))
-			return 1;
-	}
-	return 0;
-}
-
-static unsigned char at24_readdatabit(unsigned long regaddr)
-{
-	unsigned char val;
-
-	at24_setlines(regaddr, 0, 1);
-	at24_setlines(regaddr, 1, 1);
-	val = (inb(regaddr) & AT24_DATA)?1:0;
-	at24_setlines(regaddr, 1, 1);
-	at24_setlines(regaddr, 0, 1);
-	return val;
-}
-
-static unsigned char at24_readdatabyte(unsigned long regaddr)
-{
-	unsigned char data = 0;
-	int i;
-	
-	for (i = 0; i < 8; i++) {
-		data <<= 1;
-		data |= at24_readdatabit(regaddr);
-	}
-
-	return data;
-}
-
-static int at24_waitforack(unsigned long regaddr)
-{
-	int i;
-	
-	for (i = 0; i < 10; i++) {
-		if ((at24_readdatabit(regaddr) & 0x01) == 0x00)
-			return 1;
-	}
-	return 0;
-}
-
-static int at24_waitfornack(unsigned long regaddr)
-{
-	int i;
-	for (i = 0; i < 10; i++) {
-		if ((at24_readdatabit(regaddr) & 0x01) == 0x01)
-			return 1;
-	}
-	return 0;
-}
-
-static void at24_setlines(unsigned long regaddr, unsigned char clock, unsigned char data)
-{
-	unsigned char val = AT24_ENABLE;
-	if (clock)
-		val |= AT24_CLOCK;
-	if (data)
-		val |= AT24_DATA;
-
-	outb(val, regaddr); 
-	tms380tr_wait(20); /* Very necessary. */
-}
-
-static void at24_start(unsigned long regaddr)
-{
-	at24_setlines(regaddr, 0, 1);
-	at24_setlines(regaddr, 1, 1);
-	at24_setlines(regaddr, 1, 0);
-	at24_setlines(regaddr, 0, 1);
-}
-
-static unsigned char at24_readb(unsigned long regaddr, unsigned char addr)
-{
-	unsigned char data = 0xff;
-	
-	if (at24_sendfullcmd(regaddr, AT24_WRITE, addr)) {
-		if (at24_sendcmd(regaddr, AT24_READ)) {
-			data = at24_readdatabyte(regaddr);
-			if (!at24_waitfornack(regaddr))
-				data = 0xff;
-		}
-	}
-	return data;
-}
-
-
-/*
- * Enable basic functions of the Madge chipset needed
- * for initialization.
- */
-static void abyss_enable(struct net_device *dev)
-{
-	unsigned char reset_reg;
-	unsigned long ioaddr;
-	
-	ioaddr = dev->base_addr;
-	reset_reg = inb(ioaddr + PCIBM2_RESET_REG);
-	reset_reg |= PCIBM2_RESET_REG_CHIP_NRES;
-	outb(reset_reg, ioaddr + PCIBM2_RESET_REG);
-	tms380tr_wait(100);
-}
-
-/*
- * Enable the functions of the Madge chipset needed for
- * full working order. 
- */
-static int abyss_chipset_init(struct net_device *dev)
-{
-	unsigned char reset_reg;
-	unsigned long ioaddr;
-	
-	ioaddr = dev->base_addr;
-	
-	reset_reg = inb(ioaddr + PCIBM2_RESET_REG);
-	
-	reset_reg |= PCIBM2_RESET_REG_CHIP_NRES;
-	outb(reset_reg, ioaddr + PCIBM2_RESET_REG);
-	
-	reset_reg &= ~(PCIBM2_RESET_REG_CHIP_NRES |
-		       PCIBM2_RESET_REG_FIFO_NRES | 
-		       PCIBM2_RESET_REG_SIF_NRES);
-	outb(reset_reg, ioaddr + PCIBM2_RESET_REG);
-	
-	tms380tr_wait(100);
-	
-	reset_reg |= PCIBM2_RESET_REG_CHIP_NRES;
-	outb(reset_reg, ioaddr + PCIBM2_RESET_REG);
-	
-	reset_reg |= PCIBM2_RESET_REG_SIF_NRES;
-	outb(reset_reg, ioaddr + PCIBM2_RESET_REG);
-
-	reset_reg |= PCIBM2_RESET_REG_FIFO_NRES;
-	outb(reset_reg, ioaddr + PCIBM2_RESET_REG);
-
-	outb(PCIBM2_INT_CONTROL_REG_SINTEN | 
-	     PCIBM2_INT_CONTROL_REG_PCI_ERR_ENABLE, 
-	     ioaddr + PCIBM2_INT_CONTROL_REG);
-  
-	outb(30, ioaddr + PCIBM2_FIFO_THRESHOLD);
-	
-	return 0;
-}
-
-static inline void abyss_chipset_close(struct net_device *dev)
-{
-	unsigned long ioaddr;
-	
-	ioaddr = dev->base_addr;
-	outb(0, ioaddr + PCIBM2_RESET_REG);
-}
-
-/*
- * Read configuration data from the AT24 SEEPROM on Madge cards.
- *
- */
-static void abyss_read_eeprom(struct net_device *dev)
-{
-	struct net_local *tp;
-	unsigned long ioaddr;
-	unsigned short val;
-	int i;
-	
-	tp = netdev_priv(dev);
-	ioaddr = dev->base_addr;
-	
-	/* Must enable glue chip first */
-	abyss_enable(dev);
-	
-	val = at24_readb(ioaddr + PCIBM2_SEEPROM_REG, 
-			 PCIBM2_SEEPROM_RING_SPEED);
-	tp->DataRate = val?SPEED_4:SPEED_16; /* set open speed */
-	printk("%s:    SEEPROM: ring speed: %dMb/sec\n", dev->name, tp->DataRate);
-	
-	val = at24_readb(ioaddr + PCIBM2_SEEPROM_REG,
-			 PCIBM2_SEEPROM_RAM_SIZE) * 128;
-	printk("%s:    SEEPROM: adapter RAM: %dkb\n", dev->name, val);
-	
-	dev->addr_len = 6;
-	for (i = 0; i < 6; i++) 
-		dev->dev_addr[i] = at24_readb(ioaddr + PCIBM2_SEEPROM_REG, 
-					      PCIBM2_SEEPROM_BIA+i);
-}
-
-static int abyss_open(struct net_device *dev)
-{  
-	abyss_chipset_init(dev);
-	tms380tr_open(dev);
-	return 0;
-}
-
-static int abyss_close(struct net_device *dev)
-{
-	tms380tr_close(dev);
-	abyss_chipset_close(dev);
-	return 0;
-}
-
-static void __devexit abyss_detach (struct pci_dev *pdev)
-{
-	struct net_device *dev = pci_get_drvdata(pdev);
-	
-	BUG_ON(!dev);
-	unregister_netdev(dev);
-	release_region(dev->base_addr-0x10, ABYSS_IO_EXTENT);
-	free_irq(dev->irq, dev);
-	tmsdev_term(dev);
-	free_netdev(dev);
-	pci_set_drvdata(pdev, NULL);
-}
-
-static struct pci_driver abyss_driver = {
-	.name		= "abyss",
-	.id_table	= abyss_pci_tbl,
-	.probe		= abyss_attach,
-	.remove		= __devexit_p(abyss_detach),
-};
-
-static int __init abyss_init (void)
-{
-	abyss_netdev_ops = tms380tr_netdev_ops;
-
-	abyss_netdev_ops.ndo_open = abyss_open;
-	abyss_netdev_ops.ndo_stop = abyss_close;
-
-	return pci_register_driver(&abyss_driver);
-}
-
-static void __exit abyss_rmmod (void)
-{
-	pci_unregister_driver (&abyss_driver);
-}
-
-module_init(abyss_init);
-module_exit(abyss_rmmod);
-
diff --git a/drivers/net/tokenring/abyss.h b/drivers/net/tokenring/abyss.h
deleted file mode 100644
index b0a473b8913..00000000000
--- a/drivers/net/tokenring/abyss.h
+++ /dev/null
@@ -1,58 +0,0 @@
-/* 
- * abyss.h: Header for the abyss tms380tr module
- *
- * Authors:
- * - Adam Fritzler
- */
-
-#ifndef __LINUX_MADGETR_H
-#define __LINUX_MADGETR_H
-
-#ifdef __KERNEL__
-
-/*
- * For Madge Smart 16/4 PCI Mk2.  Since we increment the base address
- * to get everything correct for the TMS SIF, we do these as negatives
- * as they fall below the SIF in addressing.
- */
-#define PCIBM2_INT_STATUS_REG          ((short)-15)/* 0x01 */
-#define PCIBM2_INT_CONTROL_REG         ((short)-14)/* 0x02 */
-#define PCIBM2_RESET_REG               ((short)-12)/* 0x04 */
-#define PCIBM2_SEEPROM_REG             ((short)-9) /* 0x07 */
-
-#define PCIBM2_INT_CONTROL_REG_SINTEN           0x02
-#define PCIBM2_INT_CONTROL_REG_PCI_ERR_ENABLE   0x80
-#define PCIBM2_INT_STATUS_REG_PCI_ERR           0x80
-
-#define PCIBM2_RESET_REG_CHIP_NRES              0x01
-#define PCIBM2_RESET_REG_FIFO_NRES              0x02
-#define PCIBM2_RESET_REG_SIF_NRES               0x04
-
-#define PCIBM2_FIFO_THRESHOLD   0x21
-#define PCIBM2_BURST_LENGTH     0x22
-
-/*
- * Bits in PCIBM2_SEEPROM_REG.
- */
-#define AT24_ENABLE             0x04
-#define AT24_DATA               0x02
-#define AT24_CLOCK              0x01
-
-/*
- * AT24 Commands.
- */
-#define AT24_WRITE              0xA0
-#define AT24_READ               0xA1
-
-/*
- * Addresses in AT24 SEEPROM.
- */
-#define PCIBM2_SEEPROM_BIA          0x12
-#define PCIBM2_SEEPROM_RING_SPEED   0x18
-#define PCIBM2_SEEPROM_RAM_SIZE     0x1A
-#define PCIBM2_SEEPROM_HWF1         0x1C
-#define PCIBM2_SEEPROM_HWF2         0x1E
-
-
-#endif /* __KERNEL__ */
-#endif /* __LINUX_MADGETR_H */
diff --git a/drivers/net/tokenring/ibmtr.c b/drivers/net/tokenring/ibmtr.c
deleted file mode 100644
index b5c8c18f504..00000000000
--- a/drivers/net/tokenring/ibmtr.c
+++ /dev/null
@@ -1,1964 +0,0 @@
-/* ibmtr.c:  A shared-memory IBM Token Ring 16/4 driver for linux
- *
- *	Written 1993 by Mark Swanson and Peter De Schrijver.
- *	This software may be used and distributed according to the terms
- *	of the GNU General Public License, incorporated herein by reference.
- *
- *	This device driver should work with Any IBM Token Ring Card that does
- *	not use DMA.
- *
- *	I used Donald Becker's (becker@scyld.com) device driver work
- *	as a base for most of my initial work.
- *
- *	Changes by Peter De Schrijver
- *		(Peter.Deschrijver@linux.cc.kuleuven.ac.be) :
- *
- *	+ changed name to ibmtr.c in anticipation of other tr boards.
- *	+ changed reset code and adapter open code.
- *	+ added SAP open code.
- *	+ a first attempt to write interrupt, transmit and receive routines.
- *
- *	Changes by David W. Morris (dwm@shell.portal.com) :
- *	941003 dwm: - Restructure tok_probe for multiple adapters, devices.
- *	+ Add comments, misc reorg for clarity.
- *	+ Flatten interrupt handler levels.
- *
- *	Changes by Farzad Farid (farzy@zen.via.ecp.fr)
- *	and Pascal Andre (andre@chimay.via.ecp.fr) (March 9 1995) :
- *	+ multi ring support clean up.
- *	+ RFC1042 compliance enhanced.
- *
- *	Changes by Pascal Andre (andre@chimay.via.ecp.fr) (September 7 1995) :
- *	+ bug correction in tr_tx
- *	+ removed redundant information display
- *	+ some code reworking
- *
- *	Changes by Michel Lespinasse (walken@via.ecp.fr),
- *	Yann Doussot (doussot@via.ecp.fr) and Pascal Andre (andre@via.ecp.fr)
- *	(February 18, 1996) :
- *	+ modified shared memory and mmio access port the driver to
- *	  alpha platform (structure access -> readb/writeb)
- *
- *	Changes by Steve Kipisz (bungy@ibm.net or kipisz@vnet.ibm.com)
- *	(January 18 1996):
- *	+ swapped WWOR and WWCR in ibmtr.h
- *	+ moved some init code from tok_probe into trdev_init.  The
- *	  PCMCIA code can call trdev_init to complete initializing
- *	  the driver.
- *	+ added -DPCMCIA to support PCMCIA
- *	+ detecting PCMCIA Card Removal in interrupt handler.  If
- *	  ISRP is FF, then a PCMCIA card has been removed
- *        10/2000 Burt needed a new method to avoid crashing the OS
- *
- *	Changes by Paul Norton (pnorton@cts.com) :
- *	+ restructured the READ.LOG logic to prevent the transmit SRB
- *	  from being rudely overwritten before the transmit cycle is
- *	  complete. (August 15 1996)
- *	+ completed multiple adapter support. (November 20 1996)
- *	+ implemented csum_partial_copy in tr_rx and increased receive 
- *        buffer size and count. Minor fixes. (March 15, 1997)
- *
- *	Changes by Christopher Turcksin <wabbit@rtfc.demon.co.uk>
- *	+ Now compiles ok as a module again.
- *
- *	Changes by Paul Norton (pnorton@ieee.org) :
- *      + moved the header manipulation code in tr_tx and tr_rx to
- *        net/802/tr.c. (July 12 1997)
- *      + add retry and timeout on open if cable disconnected. (May 5 1998)
- *      + lifted 2000 byte mtu limit. now depends on shared-RAM size.
- *        May 25 1998)
- *      + can't allocate 2k recv buff at 8k shared-RAM. (20 October 1998)
- *
- *      Changes by Joel Sloan (jjs@c-me.com) :
- *      + disable verbose debug messages by default - to enable verbose
- *	  debugging, edit the IBMTR_DEBUG_MESSAGES define below 
- *	
- *	Changes by Mike Phillips <phillim@amtrak.com> :
- *	+ Added extra #ifdef's to work with new PCMCIA Token Ring Code.
- *	  The PCMCIA code now just sets up the card so it can be recognized
- *        by ibmtr_probe. Also checks allocated memory vs. on-board memory
- *	  for correct figure to use.
- *
- *	Changes by Tim Hockin (thockin@isunix.it.ilstu.edu) :
- *	+ added spinlocks for SMP sanity (10 March 1999)
- *
- *      Changes by Jochen Friedrich to enable RFC1469 Option 2 multicasting
- *      i.e. using functional address C0 00 00 04 00 00 to transmit and 
- *      receive multicast packets.
- *
- *      Changes by Mike Sullivan (based on original sram patch by Dave Grothe
- *      to support windowing into on adapter shared ram.
- *      i.e. Use LANAID to setup a PnP configuration with 16K RAM. Paging
- *      will shift this 16K window over the entire available shared RAM.
- *
- *      Changes by Peter De Schrijver (p2@mind.be) :
- *      + fixed a problem with PCMCIA card removal
- *
- *      Change by Mike Sullivan et al.:
- *      + added turbo card support. No need to use lanaid to configure
- *      the adapter into isa compatibility mode.
- *
- *      Changes by Burt Silverman to allow the computer to behave nicely when
- *	a cable is pulled or not in place, or a PCMCIA card is removed hot.
- */
-
-/* change the define of IBMTR_DEBUG_MESSAGES to a nonzero value 
-in the event that chatty debug messages are desired - jjs 12/30/98 */
-
-#define IBMTR_DEBUG_MESSAGES 0
-
-#include <linux/module.h>
-#include <linux/sched.h>
-
-#ifdef PCMCIA		/* required for ibmtr_cs.c to build */
-#undef MODULE		/* yes, really */
-#undef ENABLE_PAGING
-#else
-#define ENABLE_PAGING 1		
-#endif
-
-/* changes the output format of driver initialization */
-#define TR_VERBOSE	0
-
-/* some 95 OS send many non UI frame; this allow removing the warning */
-#define TR_FILTERNONUI	1
-
-#include <linux/interrupt.h>
-#include <linux/ioport.h>
-#include <linux/netdevice.h>
-#include <linux/ip.h>
-#include <linux/trdevice.h>
-#include <linux/ibmtr.h>
-
-#include <net/checksum.h>
-
-#include <asm/io.h>
-
-#define DPRINTK(format, args...) printk("%s: " format, dev->name , ## args)
-#define DPRINTD(format, args...) DummyCall("%s: " format, dev->name , ## args)
-
-/* version and credits */
-#ifndef PCMCIA
-static char version[] __devinitdata =
-    "\nibmtr.c: v1.3.57   8/ 7/94 Peter De Schrijver and Mark Swanson\n"
-    "         v2.1.125 10/20/98 Paul Norton    <pnorton@ieee.org>\n"
-    "         v2.2.0   12/30/98 Joel Sloan     <jjs@c-me.com>\n"
-    "         v2.2.1   02/08/00 Mike Sullivan  <sullivam@us.ibm.com>\n" 
-    "         v2.2.2   07/27/00 Burt Silverman <burts@us.ibm.com>\n" 
-    "         v2.4.0   03/01/01 Mike Sullivan <sullivan@us.ibm.com>\n";
-#endif
-
-/* this allows displaying full adapter information */
-
-static char *channel_def[] __devinitdata = { "ISA", "MCA", "ISA P&P" };
-
-static char pcchannelid[] __devinitdata = {
-	0x05, 0x00, 0x04, 0x09,
-	0x04, 0x03, 0x04, 0x0f,
-	0x03, 0x06, 0x03, 0x01,
-	0x03, 0x01, 0x03, 0x00,
-	0x03, 0x09, 0x03, 0x09,
-	0x03, 0x00, 0x02, 0x00
-};
-
-static char mcchannelid[] __devinitdata =  {
-	0x04, 0x0d, 0x04, 0x01,
-	0x05, 0x02, 0x05, 0x03,
-	0x03, 0x06, 0x03, 0x03,
-	0x05, 0x08, 0x03, 0x04,
-	0x03, 0x05, 0x03, 0x01,
-	0x03, 0x08, 0x02, 0x00
-};
-
-static char __devinit *adapter_def(char type)
-{
-	switch (type) {
-	case 0xF: return "PC Adapter | PC Adapter II | Adapter/A";
-	case 0xE: return "16/4 Adapter | 16/4 Adapter/A (long)";
-	case 0xD: return "16/4 Adapter/A (short) | 16/4 ISA-16 Adapter";
-	case 0xC: return "Auto 16/4 Adapter";
-	default: return "adapter (unknown type)";
-	}
-};
-
-#define TRC_INIT 0x01		/*  Trace initialization & PROBEs */
-#define TRC_INITV 0x02		/*  verbose init trace points     */
-static unsigned char ibmtr_debug_trace = 0;
-
-static int	ibmtr_probe1(struct net_device *dev, int ioaddr);
-static unsigned char get_sram_size(struct tok_info *adapt_info);
-static int 	trdev_init(struct net_device *dev);
-static int 	tok_open(struct net_device *dev);
-static int 	tok_init_card(struct net_device *dev);
-static void	tok_open_adapter(unsigned long dev_addr);
-static void 	open_sap(unsigned char type, struct net_device *dev);
-static void 	tok_set_multicast_list(struct net_device *dev);
-static netdev_tx_t tok_send_packet(struct sk_buff *skb,
-					 struct net_device *dev);
-static int 	tok_close(struct net_device *dev);
-static irqreturn_t tok_interrupt(int irq, void *dev_id);
-static void 	initial_tok_int(struct net_device *dev);
-static void 	tr_tx(struct net_device *dev);
-static void 	tr_rx(struct net_device *dev);
-static void	ibmtr_reset_timer(struct timer_list*tmr,struct net_device *dev);
-static void	tok_rerun(unsigned long dev_addr);
-static void	ibmtr_readlog(struct net_device *dev);
-static int	ibmtr_change_mtu(struct net_device *dev, int mtu);
-static void	find_turbo_adapters(int *iolist);
-
-static int ibmtr_portlist[IBMTR_MAX_ADAPTERS+1] __devinitdata = {
-	0xa20, 0xa24, 0, 0, 0
-};
-static int __devinitdata turbo_io[IBMTR_MAX_ADAPTERS] = {0};
-static int __devinitdata turbo_irq[IBMTR_MAX_ADAPTERS] = {0};
-static int __devinitdata turbo_searched = 0;
-
-#ifndef PCMCIA
-static __u32 ibmtr_mem_base __devinitdata = 0xd0000;
-#endif
-
-static void __devinit PrtChanID(char *pcid, short stride)
-{
-	short i, j;
-	for (i = 0, j = 0; i < 24; i++, j += stride)
-		printk("%1x", ((int) pcid[j]) & 0x0f);
-	printk("\n");
-}
-
-static void __devinit HWPrtChanID(void __iomem *pcid, short stride)
-{
-	short i, j;
-	for (i = 0, j = 0; i < 24; i++, j += stride)
-		printk("%1x", ((int) readb(pcid + j)) & 0x0f);
-	printk("\n");
-}
-
-/* We have to ioremap every checked address, because isa_readb is 
- * going away. 
- */
-
-static void __devinit find_turbo_adapters(int *iolist)
-{
-	int ram_addr;
-	int index=0;
-	void __iomem *chanid;
-	int found_turbo=0;
-	unsigned char *tchanid, ctemp;
-	int i, j;
-	unsigned long jif;
-	void __iomem *ram_mapped ;   
-
-	if (turbo_searched == 1) return;
-	turbo_searched=1;
-	for (ram_addr=0xC0000; ram_addr < 0xE0000; ram_addr+=0x2000) {
-
-		__u32 intf_tbl=0;
-
-		found_turbo=1;
-		ram_mapped = ioremap((u32)ram_addr,0x1fff) ; 
-		if (ram_mapped==NULL) 
- 			continue ; 
-		chanid=(CHANNEL_ID + ram_mapped);
-		tchanid=pcchannelid;
-		ctemp=readb(chanid) & 0x0f;
-		if (ctemp != *tchanid) continue;
-		for (i=2,j=1; i<=46; i=i+2,j++) {
-			if ((readb(chanid+i) & 0x0f) != tchanid[j]){
-				found_turbo=0;
-				break;
-			}
-		}
-		if (!found_turbo) continue;
-
-		writeb(0x90, ram_mapped+0x1E01);
-		for(i=2; i<0x0f; i++) {
-			writeb(0x00, ram_mapped+0x1E01+i);
-		}
-		writeb(0x00, ram_mapped+0x1E01);
-		for(jif=jiffies+TR_BUSY_INTERVAL; time_before_eq(jiffies,jif););
-		intf_tbl=ntohs(readw(ram_mapped+ACA_OFFSET+ACA_RW+WRBR_EVEN));
-		if (intf_tbl) {
-#if IBMTR_DEBUG_MESSAGES
-			printk("ibmtr::find_turbo_adapters, Turbo found at "
-				"ram_addr %x\n",ram_addr);
-			printk("ibmtr::find_turbo_adapters, interface_table ");
-			for(i=0; i<6; i++) {
-				printk("%x:",readb(ram_addr+intf_tbl+i));
-			}
-			printk("\n");
-#endif
-			turbo_io[index]=ntohs(readw(ram_mapped+intf_tbl+4));
-			turbo_irq[index]=readb(ram_mapped+intf_tbl+3);
-			outb(0, turbo_io[index] + ADAPTRESET);
-			for(jif=jiffies+TR_RST_TIME;time_before_eq(jiffies,jif););
-			outb(0, turbo_io[index] + ADAPTRESETREL);
-			index++;
-			continue;
-		}
-#if IBMTR_DEBUG_MESSAGES 
-		printk("ibmtr::find_turbo_adapters, ibmtr card found at"
-			" %x but not a Turbo model\n",ram_addr);
-#endif
-	iounmap(ram_mapped) ; 	
-	} /* for */
-	for(i=0; i<IBMTR_MAX_ADAPTERS; i++) {
-		if(!turbo_io[i]) break;
-		for (j=0; j<IBMTR_MAX_ADAPTERS; j++) {
-			if ( iolist[j] && iolist[j] != turbo_io[i]) continue;
-			iolist[j]=turbo_io[i];
-			break;
-		}
-	}
-}
-
-static void ibmtr_cleanup_card(struct net_device *dev)
-{
-	if (dev->base_addr) {
-		outb(0,dev->base_addr+ADAPTRESET);
-		
-		schedule_timeout_uninterruptible(TR_RST_TIME); /* wait 50ms */
-
-		outb(0,dev->base_addr+ADAPTRESETREL);
-	}
-
-#ifndef PCMCIA
-	free_irq(dev->irq, dev);
-	release_region(dev->base_addr, IBMTR_IO_EXTENT);
-
-	{ 
-		struct tok_info *ti = netdev_priv(dev);
-		iounmap(ti->mmio);
-		iounmap(ti->sram_virt);
-	}
-#endif		
-}
-
-/****************************************************************************
- *	ibmtr_probe():  Routine specified in the network device structure
- *	to probe for an IBM Token Ring Adapter.  Routine outline:
- *	I.    Interrogate hardware to determine if an adapter exists
- *	      and what the speeds and feeds are
- *	II.   Setup data structures to control execution based upon
- *	      adapter characteristics.
- *
- *	We expect ibmtr_probe to be called once for each device entry
- *	which references it.
- ****************************************************************************/
-
-static int __devinit ibmtr_probe(struct net_device *dev)
-{
-	int i;
-	int base_addr = dev->base_addr;
-
-	if (base_addr && base_addr <= 0x1ff) /* Don't probe at all. */
-		return -ENXIO;
-	if (base_addr > 0x1ff) { /* Check a single specified location.  */
-		if (!ibmtr_probe1(dev, base_addr)) return 0;
-		return -ENODEV;
-	}
-	find_turbo_adapters(ibmtr_portlist);
-	for (i = 0; ibmtr_portlist[i]; i++) {
-		int ioaddr = ibmtr_portlist[i];
-
-		if (!ibmtr_probe1(dev, ioaddr)) return 0;
-	}
-	return -ENODEV;
-}
-
-int __devinit ibmtr_probe_card(struct net_device *dev)
-{
-	int err = ibmtr_probe(dev);
-	if (!err) {
-		err = register_netdev(dev);
-		if (err)
-			ibmtr_cleanup_card(dev);
-	}
-	return err;
-}
-
-/*****************************************************************************/
-
-static int __devinit ibmtr_probe1(struct net_device *dev, int PIOaddr)
-{
-
-	unsigned char segment, intr=0, irq=0, i, j, cardpresent=NOTOK, temp=0;
-	void __iomem * t_mmio = NULL;
-	struct tok_info *ti = netdev_priv(dev);
-	void __iomem *cd_chanid;
-	unsigned char *tchanid, ctemp;
-#ifndef PCMCIA
-	unsigned char t_irq=0;
-        unsigned long timeout;
-	static int version_printed;
-#endif
-
-	/*    Query the adapter PIO base port which will return
-	 *    indication of where MMIO was placed. We also have a
-	 *    coded interrupt number.
-	 */
-	segment = inb(PIOaddr);
-	if (segment < 0x40 || segment > 0xe0) {
-		/* Out of range values so we'll assume non-existent IO device
-		 * but this is not necessarily a problem, esp if a turbo
-		 * adapter is being used.  */
-#if IBMTR_DEBUG_MESSAGES
-		DPRINTK("ibmtr_probe1(): unhappy that inb(0x%X) == 0x%X, "
-			"Hardware Problem?\n",PIOaddr,segment);
-#endif
-		return -ENODEV;
-	}
-	/*
-	 *    Compute the linear base address of the MMIO area
-	 *    as LINUX doesn't care about segments
-	 */
-	t_mmio = ioremap(((__u32) (segment & 0xfc) << 11) + 0x80000,2048);
-	if (!t_mmio) { 
-		DPRINTK("Cannot remap mmiobase memory area") ; 
-		return -ENODEV ; 
-	} 
-	intr = segment & 0x03;	/* low bits is coded interrupt # */
-	if (ibmtr_debug_trace & TRC_INIT)
-		DPRINTK("PIOaddr: %4hx seg/intr: %2x mmio base: %p intr: %d\n"
-				, PIOaddr, (int) segment, t_mmio, (int) intr);
-
-	/*
-	 *    Now we will compare expected 'channelid' strings with
-	 *    what we is there to learn of ISA/MCA or not TR card
-	 */
-#ifdef PCMCIA
-	iounmap(t_mmio);
-	t_mmio = ti->mmio;	/*BMS to get virtual address */
-	irq = ti->irq;		/*BMS to display the irq!   */
-#endif
-	cd_chanid = (CHANNEL_ID + t_mmio);	/* for efficiency */
-	tchanid = pcchannelid;
-	cardpresent = TR_ISA;	/* try ISA */
-
-	/*    Suboptimize knowing first byte different */
-	ctemp = readb(cd_chanid) & 0x0f;
-	if (ctemp != *tchanid) {	/* NOT ISA card, try MCA */
-		tchanid = mcchannelid;
-		cardpresent = TR_MCA;
-		if (ctemp != *tchanid)	/* Neither ISA nor MCA */
-			cardpresent = NOTOK;
-	}
-	if (cardpresent != NOTOK) {
-		/*       Know presumed type, try rest of ID */
-		for (i = 2, j = 1; i <= 46; i = i + 2, j++) {
-			if( (readb(cd_chanid+i)&0x0f) == tchanid[j]) continue;
-			/* match failed, not TR card */
-			cardpresent = NOTOK;
-			break;
-		}
-	}
-	/* 
-	 *    If we have an ISA board check for the ISA P&P version,
-	 *    as it has different IRQ settings 
-	 */
-	if (cardpresent == TR_ISA && (readb(AIPFID + t_mmio) == 0x0e))
-		cardpresent = TR_ISAPNP;
-	if (cardpresent == NOTOK) {	/* "channel_id" did not match, report */
-		if (!(ibmtr_debug_trace & TRC_INIT)) {
-#ifndef PCMCIA
-			iounmap(t_mmio);
-#endif
-			return -ENODEV;
-		}
-		DPRINTK( "Channel ID string not found for PIOaddr: %4hx\n",
-								PIOaddr);
-		DPRINTK("Expected for ISA: ");
-		PrtChanID(pcchannelid, 1);
-		DPRINTK("           found: ");
-/* BMS Note that this can be misleading, when hardware is flaky, because you
-   are reading it a second time here. So with my flaky hardware, I'll see my-
-   self in this block, with the HW ID matching the ISA ID exactly! */
-		HWPrtChanID(cd_chanid, 2);
-		DPRINTK("Expected for MCA: ");
-		PrtChanID(mcchannelid, 1);
-	}
-	/* Now, setup some of the pl0 buffers for this driver.. */
-	/* If called from PCMCIA, it is already set up, so no need to 
-	   waste the memory, just use the existing structure */
-#ifndef PCMCIA
-	ti->mmio = t_mmio;
-        for (i = 0; i < IBMTR_MAX_ADAPTERS; i++) {
-                if (turbo_io[i] != PIOaddr)
-			continue;
-#if IBMTR_DEBUG_MESSAGES 
-		printk("ibmtr::tr_probe1, setting PIOaddr %x to Turbo\n",
-		       PIOaddr);
-#endif
-		ti->turbo = 1;
-		t_irq = turbo_irq[i];
-        }
-#endif /* !PCMCIA */
-	ti->readlog_pending = 0;
-	init_waitqueue_head(&ti->wait_for_reset);
-
-	/* if PCMCIA, the card can be recognized as either TR_ISA or TR_ISAPNP
-	 * depending which card is inserted.	*/
-	
-#ifndef PCMCIA
-	switch (cardpresent) {
-	case TR_ISA:
-		if (intr == 0) irq = 9;	/* irq2 really is irq9 */
-		if (intr == 1) irq = 3;
-		if (intr == 2) irq = 6;
-		if (intr == 3) irq = 7;
-		ti->adapter_int_enable = PIOaddr + ADAPTINTREL;
-		break;
-	case TR_MCA:
-		if (intr == 0) irq = 9;
-		if (intr == 1) irq = 3;
-		if (intr == 2) irq = 10;
-		if (intr == 3) irq = 11;
-		ti->global_int_enable = 0;
-		ti->adapter_int_enable = 0;
-		ti->sram_phys=(__u32)(inb(PIOaddr+ADAPTRESETREL) & 0xfe) << 12;
-		break;
-	case TR_ISAPNP:
-		if (!t_irq) {
-			if (intr == 0) irq = 9;
-			if (intr == 1) irq = 3;
-			if (intr == 2) irq = 10;
-			if (intr == 3) irq = 11;
-		} else
-			irq=t_irq;
-		timeout = jiffies + TR_SPIN_INTERVAL;
-		while (!readb(ti->mmio + ACA_OFFSET + ACA_RW + RRR_EVEN)){
-			if (!time_after(jiffies, timeout)) continue;
-			DPRINTK( "Hardware timeout during initialization.\n");
-			iounmap(t_mmio);
-			return -ENODEV;
-		}
-		ti->sram_phys =
-		     ((__u32)readb(ti->mmio+ACA_OFFSET+ACA_RW+RRR_EVEN)<<12);
-		ti->adapter_int_enable = PIOaddr + ADAPTINTREL;
-		break;
-	} /*end switch (cardpresent) */
-#endif	/*not PCMCIA */
-
-	if (ibmtr_debug_trace & TRC_INIT) {	/* just report int */
-		DPRINTK("irq=%d", irq);
-		printk(", sram_phys=0x%x", ti->sram_phys);
-		if(ibmtr_debug_trace&TRC_INITV){ /* full chat in verbose only */
-			DPRINTK(", ti->mmio=%p", ti->mmio);
-			printk(", segment=%02X", segment);
-		}
-		printk(".\n");
-	}
-
-	/* Get hw address of token ring card */
-	j = 0;
-	for (i = 0; i < 0x18; i = i + 2) {
-		/* technical reference states to do this */
-		temp = readb(ti->mmio + AIP + i) & 0x0f;
-		ti->hw_address[j] = temp;
-		if (j & 1)
-			dev->dev_addr[(j / 2)] =
-				ti->hw_address[j]+ (ti->hw_address[j - 1] << 4);
-		++j;
-	}
-	/* get Adapter type:  'F' = Adapter/A, 'E' = 16/4 Adapter II,... */
-	ti->adapter_type = readb(ti->mmio + AIPADAPTYPE);
-
-	/* get Data Rate:  F=4Mb, E=16Mb, D=4Mb & 16Mb ?? */
-	ti->data_rate = readb(ti->mmio + AIPDATARATE);
-
-	/* Get Early Token Release support?: F=no, E=4Mb, D=16Mb, C=4&16Mb */
-	ti->token_release = readb(ti->mmio + AIPEARLYTOKEN);
-
-	/* How much shared RAM is on adapter ? */
-	if (ti->turbo) {
-		ti->avail_shared_ram=127;
-	} else {
-		ti->avail_shared_ram = get_sram_size(ti);/*in 512 byte units */
-	}
-	/* We need to set or do a bunch of work here based on previous results*/
-	/* Support paging?  What sizes?:  F=no, E=16k, D=32k, C=16 & 32k */
-	ti->shared_ram_paging = readb(ti->mmio + AIPSHRAMPAGE);
-
-	/* Available DHB  4Mb size:   F=2048, E=4096, D=4464 */
-	switch (readb(ti->mmio + AIP4MBDHB)) {
-	case 0xe: ti->dhb_size4mb = 4096; break;
-	case 0xd: ti->dhb_size4mb = 4464; break;
-	default:  ti->dhb_size4mb = 2048; break;
-	}
-
-	/* Available DHB 16Mb size:  F=2048, E=4096, D=8192, C=16384, B=17960 */
-	switch (readb(ti->mmio + AIP16MBDHB)) {
-	case 0xe: ti->dhb_size16mb = 4096; break;
-	case 0xd: ti->dhb_size16mb = 8192; break;
-	case 0xc: ti->dhb_size16mb = 16384; break;
-	case 0xb: ti->dhb_size16mb = 17960; break;
-	default:  ti->dhb_size16mb = 2048; break;
-	}
-
-	/*    We must figure out how much shared memory space this adapter
-	 *    will occupy so that if there are two adapters we can fit both
-	 *    in.  Given a choice, we will limit this adapter to 32K.  The
-	 *    maximum space will will use for two adapters is 64K so if the
-	 *    adapter we are working on demands 64K (it also doesn't support
-	 *    paging), then only one adapter can be supported.  
-	 */
-
-	/*
-	 *    determine how much of total RAM is mapped into PC space 
-	 */
-	ti->mapped_ram_size= /*sixteen to onehundredtwentyeight 512byte blocks*/
-	    1<< ((readb(ti->mmio+ACA_OFFSET+ACA_RW+RRR_ODD) >> 2 & 0x03) + 4);
-	ti->page_mask = 0;
-	if (ti->turbo)  ti->page_mask=0xf0;
-	else if (ti->shared_ram_paging == 0xf);  /* No paging in adapter */
-	else {
-#ifdef ENABLE_PAGING
-		unsigned char pg_size = 0;
-		/* BMS:   page size: PCMCIA, use configuration register;
-		   ISAPNP, use LANAIDC config tool from www.ibm.com  */
-		switch (ti->shared_ram_paging) {
-		case 0xf:
-			break;
-		case 0xe:
-			ti->page_mask = (ti->mapped_ram_size == 32) ? 0xc0 : 0;
-			pg_size = 32;	/* 16KB page size */
-			break;
-		case 0xd:
-			ti->page_mask = (ti->mapped_ram_size == 64) ? 0x80 : 0;
-			pg_size = 64;	/* 32KB page size */
-			break;
-		case 0xc:
-			switch (ti->mapped_ram_size) {
-			case 32:
-				ti->page_mask = 0xc0;
-				pg_size = 32;
-				break;
-			case 64:
-				ti->page_mask = 0x80;
-				pg_size = 64;
-				break;
-			}
-			break;
-		default:
-			DPRINTK("Unknown shared ram paging info %01X\n",
-							ti->shared_ram_paging);
-			iounmap(t_mmio); 
-			return -ENODEV;
-			break;
-		} /*end switch shared_ram_paging */
-
-		if (ibmtr_debug_trace & TRC_INIT)
-			DPRINTK("Shared RAM paging code: %02X, "
-				"mapped RAM size: %dK, shared RAM size: %dK, "
-				"page mask: %02X\n:",
-				ti->shared_ram_paging, ti->mapped_ram_size / 2,
-				ti->avail_shared_ram / 2, ti->page_mask);
-#endif	/*ENABLE_PAGING */
-	}
-
-#ifndef PCMCIA
-	/* finish figuring the shared RAM address */
-	if (cardpresent == TR_ISA) {
-		static const __u32 ram_bndry_mask[] = {
-			0xffffe000, 0xffffc000, 0xffff8000, 0xffff0000
-		};
-		__u32 new_base, rrr_32, chk_base, rbm;
-
-		rrr_32=readb(ti->mmio+ACA_OFFSET+ACA_RW+RRR_ODD) >> 2 & 0x03;
-		rbm = ram_bndry_mask[rrr_32];
-		new_base = (ibmtr_mem_base + (~rbm)) & rbm;/* up to boundary */
-		chk_base = new_base + (ti->mapped_ram_size << 9);
-		if (chk_base > (ibmtr_mem_base + IBMTR_SHARED_RAM_SIZE)) {
-			DPRINTK("Shared RAM for this adapter (%05x) exceeds "
-			"driver limit (%05x), adapter not started.\n",
-			chk_base, ibmtr_mem_base + IBMTR_SHARED_RAM_SIZE);
-			iounmap(t_mmio);
-			return -ENODEV;
-		} else { /* seems cool, record what we have figured out */
-			ti->sram_base = new_base >> 12;
-			ibmtr_mem_base = chk_base;
-		}
-	}
-	else  ti->sram_base = ti->sram_phys >> 12;
-
-	/* The PCMCIA has already got the interrupt line and the io port, 
-	   so no chance of anybody else getting it - MLP */
-	if (request_irq(dev->irq = irq, tok_interrupt, 0, "ibmtr", dev) != 0) {
-		DPRINTK("Could not grab irq %d.  Halting Token Ring driver.\n",
-					irq);
-		iounmap(t_mmio);
-		return -ENODEV;
-	}
-	/*?? Now, allocate some of the PIO PORTs for this driver.. */
-	/* record PIOaddr range as busy */
-	if (!request_region(PIOaddr, IBMTR_IO_EXTENT, "ibmtr")) {
-		DPRINTK("Could not grab PIO range. Halting driver.\n");
-		free_irq(dev->irq, dev);
-		iounmap(t_mmio);
-		return -EBUSY;
-	}
-
-	if (!version_printed++) {
-		printk(version);
-	}
-#endif /* !PCMCIA */
-	DPRINTK("%s %s found\n",
-		channel_def[cardpresent - 1], adapter_def(ti->adapter_type));
-	DPRINTK("using irq %d, PIOaddr %hx, %dK shared RAM.\n",
-			irq, PIOaddr, ti->mapped_ram_size / 2);
-	DPRINTK("Hardware address : %pM\n", dev->dev_addr);
-	if (ti->page_mask)
-		DPRINTK("Shared RAM paging enabled. "
-			"Page size: %uK Shared Ram size %dK\n",
-			((ti->page_mask^0xff)+1) >>2, ti->avail_shared_ram / 2);
-	else
-		DPRINTK("Shared RAM paging disabled. ti->page_mask %x\n",
-								ti->page_mask);
-
-	/* Calculate the maximum DHB we can use */
-	/* two cases where avail_shared_ram doesn't equal mapped_ram_size:
-	    1. avail_shared_ram is 127 but mapped_ram_size is 128 (typical)
-	    2. user has configured adapter for less than avail_shared_ram
-	       but is not using paging (she should use paging, I believe)
-	*/
-	if (!ti->page_mask) {
-		ti->avail_shared_ram=
-				min(ti->mapped_ram_size,ti->avail_shared_ram);
-	}
-
-	switch (ti->avail_shared_ram) {
-	case 16:		/* 8KB shared RAM */
-		ti->dhb_size4mb = min(ti->dhb_size4mb, (unsigned short)2048);
-		ti->rbuf_len4 = 1032;
-		ti->rbuf_cnt4=2;
-		ti->dhb_size16mb = min(ti->dhb_size16mb, (unsigned short)2048);
-		ti->rbuf_len16 = 1032;
-		ti->rbuf_cnt16=2;
-		break;
-	case 32:		/* 16KB shared RAM */
-		ti->dhb_size4mb = min(ti->dhb_size4mb, (unsigned short)4464);
-		ti->rbuf_len4 = 1032;
-		ti->rbuf_cnt4=4;
-		ti->dhb_size16mb = min(ti->dhb_size16mb, (unsigned short)4096);
-		ti->rbuf_len16 = 1032;	/*1024 usable */
-		ti->rbuf_cnt16=4;
-		break;
-	case 64:		/* 32KB shared RAM */
-		ti->dhb_size4mb = min(ti->dhb_size4mb, (unsigned short)4464);
-		ti->rbuf_len4 = 1032;
-		ti->rbuf_cnt4=6;
-		ti->dhb_size16mb = min(ti->dhb_size16mb, (unsigned short)10240);
-		ti->rbuf_len16 = 1032;
-		ti->rbuf_cnt16=6;
-		break;
-	case 127:		/* 63.5KB shared RAM */
-		ti->dhb_size4mb = min(ti->dhb_size4mb, (unsigned short)4464);
-		ti->rbuf_len4 = 1032;
-		ti->rbuf_cnt4=6;
-		ti->dhb_size16mb = min(ti->dhb_size16mb, (unsigned short)16384);
-		ti->rbuf_len16 = 1032;
-		ti->rbuf_cnt16=16;
-		break;
-	case 128:		/* 64KB   shared RAM */
-		ti->dhb_size4mb = min(ti->dhb_size4mb, (unsigned short)4464);
-		ti->rbuf_len4 = 1032;
-		ti->rbuf_cnt4=6;
-		ti->dhb_size16mb = min(ti->dhb_size16mb, (unsigned short)17960);
-		ti->rbuf_len16 = 1032;
-		ti->rbuf_cnt16=16;
-		break;
-	default:
-		ti->dhb_size4mb = 2048;
-		ti->rbuf_len4 = 1032;
-		ti->rbuf_cnt4=2;
-		ti->dhb_size16mb = 2048;
-		ti->rbuf_len16 = 1032;
-		ti->rbuf_cnt16=2;
-		break;
-	}
-	/* this formula is not smart enough for the paging case
-	ti->rbuf_cnt<x> = (ti->avail_shared_ram * BLOCKSZ - ADAPT_PRIVATE -
-			ARBLENGTH - SSBLENGTH - DLC_MAX_SAP * SAPLENGTH -
-			DLC_MAX_STA * STALENGTH - ti->dhb_size<x>mb * NUM_DHB -
-			SRBLENGTH - ASBLENGTH) / ti->rbuf_len<x>;
-	*/
-	ti->maxmtu16 = (ti->rbuf_len16 - 8) * ti->rbuf_cnt16  - TR_HLEN;
-	ti->maxmtu4 = (ti->rbuf_len4 - 8) * ti->rbuf_cnt4 - TR_HLEN;
-	/*BMS assuming 18 bytes of Routing Information (usually works) */
-	DPRINTK("Maximum Receive Internet Protocol MTU 16Mbps: %d, 4Mbps: %d\n",
-						     ti->maxmtu16, ti->maxmtu4);
-
-	dev->base_addr = PIOaddr;	/* set the value for device */
-	dev->mem_start = ti->sram_base << 12;
-	dev->mem_end = dev->mem_start + (ti->mapped_ram_size << 9) - 1;
-	trdev_init(dev);
-	return 0;   /* Return 0 to indicate we have found a Token Ring card. */
-}				/*ibmtr_probe1() */
-
-/*****************************************************************************/
-
-/* query the adapter for the size of shared RAM  */
-/* the function returns the RAM size in units of 512 bytes */
-
-static unsigned char __devinit get_sram_size(struct tok_info *adapt_info)
-{
-	unsigned char avail_sram_code;
-	static unsigned char size_code[] = { 0, 16, 32, 64, 127, 128 };
-	/* Adapter gives
-	   'F' -- use RRR bits 3,2
-	   'E' -- 8kb   'D' -- 16kb
-	   'C' -- 32kb  'A' -- 64KB
-	   'B' - 64KB less 512 bytes at top
-	   (WARNING ... must zero top bytes in INIT */
-
-	avail_sram_code = 0xf - readb(adapt_info->mmio + AIPAVAILSHRAM);
-	if (avail_sram_code) return size_code[avail_sram_code];
-	else		/* for code 'F', must compute size from RRR(3,2) bits */
-		return 1 <<
-		 ((readb(adapt_info->mmio+ACA_OFFSET+ACA_RW+RRR_ODD)>>2&3)+4);
-}
-
-/*****************************************************************************/
-
-static const struct net_device_ops trdev_netdev_ops = {
-	.ndo_open		= tok_open,
-	.ndo_stop		= tok_close,
-	.ndo_start_xmit		= tok_send_packet,
-	.ndo_set_rx_mode	= tok_set_multicast_list,
-	.ndo_change_mtu		= ibmtr_change_mtu,
-};
-
-static int __devinit trdev_init(struct net_device *dev)
-{
-	struct tok_info *ti = netdev_priv(dev);
-
-	SET_PAGE(ti->srb_page);
-        ti->open_failure = NO    ;
-	dev->netdev_ops = &trdev_netdev_ops;
-
-	return 0;
-}
-
-/*****************************************************************************/
-
-static int tok_init_card(struct net_device *dev)
-{
-	struct tok_info *ti;
-	short PIOaddr;
-	unsigned long i;
-
-	PIOaddr = dev->base_addr;
-	ti = netdev_priv(dev);
-	/* Special processing for first interrupt after reset */
-	ti->do_tok_int = FIRST_INT;
-	/* Reset adapter */
-	writeb(~INT_ENABLE, ti->mmio + ACA_OFFSET + ACA_RESET + ISRP_EVEN);
-	outb(0, PIOaddr + ADAPTRESET);
-
-	schedule_timeout_uninterruptible(TR_RST_TIME); /* wait 50ms */
-
-	outb(0, PIOaddr + ADAPTRESETREL);
-#ifdef ENABLE_PAGING
-	if (ti->page_mask)
-		writeb(SRPR_ENABLE_PAGING,ti->mmio+ACA_OFFSET+ACA_RW+SRPR_EVEN);
-#endif
-	writeb(INT_ENABLE, ti->mmio + ACA_OFFSET + ACA_SET + ISRP_EVEN);
-	i = sleep_on_timeout(&ti->wait_for_reset, 4 * HZ);
-	return i? 0 : -EAGAIN;
-}
-
-/*****************************************************************************/
-static int tok_open(struct net_device *dev)
-{
-	struct tok_info *ti = netdev_priv(dev);
-	int i;
-
-	/*the case we were left in a failure state during a previous open */
-	if (ti->open_failure == YES) {
-		DPRINTK("Last time you were disconnected, how about now?\n");
-		printk("You can't insert with an ICS connector half-cocked.\n");
-	}
-
-	ti->open_status  = CLOSED; /* CLOSED or OPEN      */
-	ti->sap_status   = CLOSED; /* CLOSED or OPEN      */
-	ti->open_failure =     NO; /* NO     or YES       */
-	ti->open_mode    = MANUAL; /* MANUAL or AUTOMATIC */
-
-	ti->sram_phys &= ~1; /* to reverse what we do in tok_close */
-	/* init the spinlock */
-	spin_lock_init(&ti->lock);
-	init_timer(&ti->tr_timer);
-	
-	i = tok_init_card(dev);
-	if (i) return i;
-
-	while (1){
-		tok_open_adapter((unsigned long) dev);
-		i= interruptible_sleep_on_timeout(&ti->wait_for_reset, 25 * HZ);
-		/* sig catch: estimate opening adapter takes more than .5 sec*/
-		if (i>(245*HZ)/10) break; /* fancier than if (i==25*HZ) */
-		if (i==0) break;
-		if (ti->open_status == OPEN && ti->sap_status==OPEN) {
-			netif_start_queue(dev);
-			DPRINTK("Adapter is up and running\n");
-			return 0;
-		}
-		i=schedule_timeout_interruptible(TR_RETRY_INTERVAL);
-							/* wait 30 seconds */
-		if(i!=0) break; /*prob. a signal, like the i>24*HZ case above */
-	}
-	outb(0, dev->base_addr + ADAPTRESET);/* kill pending interrupts*/
-	DPRINTK("TERMINATED via signal\n");	/*BMS useful */
-	return -EAGAIN;
-}
-
-/*****************************************************************************/
-
-#define COMMAND_OFST             0
-#define OPEN_OPTIONS_OFST        8
-#define NUM_RCV_BUF_OFST        24
-#define RCV_BUF_LEN_OFST        26
-#define DHB_LENGTH_OFST         28
-#define NUM_DHB_OFST            30
-#define DLC_MAX_SAP_OFST        32
-#define DLC_MAX_STA_OFST        33
-
-static void tok_open_adapter(unsigned long dev_addr)
-{
-	struct net_device *dev = (struct net_device *) dev_addr;
-	struct tok_info *ti;
-	int i;
-
-	ti = netdev_priv(dev);
-	SET_PAGE(ti->init_srb_page); 
-	writeb(~SRB_RESP_INT, ti->mmio + ACA_OFFSET + ACA_RESET + ISRP_ODD);
-	for (i = 0; i < sizeof(struct dir_open_adapter); i++)
-		writeb(0, ti->init_srb + i);
-	writeb(DIR_OPEN_ADAPTER, ti->init_srb + COMMAND_OFST);
-	writew(htons(OPEN_PASS_BCON_MAC), ti->init_srb + OPEN_OPTIONS_OFST);
-	if (ti->ring_speed == 16) {
-		writew(htons(ti->dhb_size16mb), ti->init_srb + DHB_LENGTH_OFST);
-		writew(htons(ti->rbuf_cnt16), ti->init_srb + NUM_RCV_BUF_OFST);
-		writew(htons(ti->rbuf_len16), ti->init_srb + RCV_BUF_LEN_OFST);
-	} else {
-		writew(htons(ti->dhb_size4mb), ti->init_srb + DHB_LENGTH_OFST);
-		writew(htons(ti->rbuf_cnt4), ti->init_srb + NUM_RCV_BUF_OFST);
-		writew(htons(ti->rbuf_len4), ti->init_srb + RCV_BUF_LEN_OFST);
-	}
-	writeb(NUM_DHB,		/* always 2 */ ti->init_srb + NUM_DHB_OFST);
-	writeb(DLC_MAX_SAP, ti->init_srb + DLC_MAX_SAP_OFST);
-	writeb(DLC_MAX_STA, ti->init_srb + DLC_MAX_STA_OFST);
-	ti->srb = ti->init_srb;	/* We use this one in the interrupt handler */
-	ti->srb_page = ti->init_srb_page;
-	DPRINTK("Opening adapter: Xmit bfrs: %d X %d, Rcv bfrs: %d X %d\n",
-		readb(ti->init_srb + NUM_DHB_OFST),
-		ntohs(readw(ti->init_srb + DHB_LENGTH_OFST)),
-		ntohs(readw(ti->init_srb + NUM_RCV_BUF_OFST)),
-		ntohs(readw(ti->init_srb + RCV_BUF_LEN_OFST)));
-	writeb(INT_ENABLE, ti->mmio + ACA_OFFSET + ACA_SET + ISRP_EVEN);
-	writeb(CMD_IN_SRB, ti->mmio + ACA_OFFSET + ACA_SET + ISRA_ODD);
-}
-
-/*****************************************************************************/
-
-static void open_sap(unsigned char type, struct net_device *dev)
-{
-	int i;
-	struct tok_info *ti = netdev_priv(dev);
-
-	SET_PAGE(ti->srb_page);
-	for (i = 0; i < sizeof(struct dlc_open_sap); i++)
-		writeb(0, ti->srb + i);
-
-#define MAX_I_FIELD_OFST        14
-#define SAP_VALUE_OFST          16
-#define SAP_OPTIONS_OFST        17
-#define STATION_COUNT_OFST      18
-
-	writeb(DLC_OPEN_SAP, ti->srb + COMMAND_OFST);
-	writew(htons(MAX_I_FIELD), ti->srb + MAX_I_FIELD_OFST);
-	writeb(SAP_OPEN_IND_SAP | SAP_OPEN_PRIORITY, ti->srb+ SAP_OPTIONS_OFST);
-	writeb(SAP_OPEN_STATION_CNT, ti->srb + STATION_COUNT_OFST);
-	writeb(type, ti->srb + SAP_VALUE_OFST);
-	writeb(CMD_IN_SRB, ti->mmio + ACA_OFFSET + ACA_SET + ISRA_ODD);
-}
-
-
-/*****************************************************************************/
-
-static void tok_set_multicast_list(struct net_device *dev)
-{
-	struct tok_info *ti = netdev_priv(dev);
-	struct netdev_hw_addr *ha;
-	unsigned char address[4];
-
-	int i;
-
-	/*BMS the next line is CRUCIAL or you may be sad when you */
-	/*BMS ifconfig tr down or hot unplug a PCMCIA card ??hownowbrowncow*/
-	if (/*BMSHELPdev->start == 0 ||*/ ti->open_status != OPEN) return;
-	address[0] = address[1] = address[2] = address[3] = 0;
-	netdev_for_each_mc_addr(ha, dev) {
-		address[0] |= ha->addr[2];
-		address[1] |= ha->addr[3];
-		address[2] |= ha->addr[4];
-		address[3] |= ha->addr[5];
-	}
-	SET_PAGE(ti->srb_page);
-	for (i = 0; i < sizeof(struct srb_set_funct_addr); i++)
-		writeb(0, ti->srb + i);
-
-#define FUNCT_ADDRESS_OFST 6
-
-	writeb(DIR_SET_FUNC_ADDR, ti->srb + COMMAND_OFST);
-	for (i = 0; i < 4; i++) 
-		writeb(address[i], ti->srb + FUNCT_ADDRESS_OFST + i);
-	writeb(CMD_IN_SRB, ti->mmio + ACA_OFFSET + ACA_SET + ISRA_ODD);
-#if TR_VERBOSE
-	DPRINTK("Setting functional address: ");
-	for (i=0;i<4;i++)  printk("%02X ", address[i]);
-	printk("\n");
-#endif
-}
-
-/*****************************************************************************/
-
-#define STATION_ID_OFST 4
-
-static netdev_tx_t tok_send_packet(struct sk_buff *skb,
-					 struct net_device *dev)
-{
-	struct tok_info *ti;
-	unsigned long flags;
-	ti = netdev_priv(dev);
-
-        netif_stop_queue(dev);
-
-	/* lock against other CPUs */
-	spin_lock_irqsave(&(ti->lock), flags);
-
-	/* Save skb; we'll need it when the adapter asks for the data */
-	ti->current_skb = skb;
-	SET_PAGE(ti->srb_page);
-	writeb(XMIT_UI_FRAME, ti->srb + COMMAND_OFST);
-	writew(ti->exsap_station_id, ti->srb + STATION_ID_OFST);
-	writeb(CMD_IN_SRB, ti->mmio + ACA_OFFSET + ACA_SET + ISRA_ODD);
-	spin_unlock_irqrestore(&(ti->lock), flags);
-	return NETDEV_TX_OK;
-}
-
-/*****************************************************************************/
-
-static int tok_close(struct net_device *dev)
-{
-	struct tok_info *ti = netdev_priv(dev);
-
-	/* Important for PCMCIA hot unplug, otherwise, we'll pull the card, */
-	/* unloading the module from memory, and then if a timer pops, ouch */
-	del_timer_sync(&ti->tr_timer);
-	outb(0, dev->base_addr + ADAPTRESET);
-	ti->sram_phys |= 1;
-	ti->open_status = CLOSED;
-
-	netif_stop_queue(dev);
-	DPRINTK("Adapter is closed.\n");
-	return 0;
-}
-
-/*****************************************************************************/
-
-#define RETCODE_OFST		2
-#define OPEN_ERROR_CODE_OFST	6
-#define ASB_ADDRESS_OFST        8
-#define SRB_ADDRESS_OFST        10
-#define ARB_ADDRESS_OFST        12
-#define SSB_ADDRESS_OFST        14
-
-static char *printphase[]= {"Lobe media test","Physical insertion",
-	      "Address verification","Roll call poll","Request Parameters"};
-static char *printerror[]={"Function failure","Signal loss","Reserved",
-		"Frequency error","Timeout","Ring failure","Ring beaconing",
-		"Duplicate node address",
-		"Parameter request-retry count exceeded","Remove received",
-		"IMPL force received","Duplicate modifier",
-		"No monitor detected","Monitor contention failed for RPL"};
-
-static void __iomem *map_address(struct tok_info *ti, unsigned index, __u8 *page)
-{
-	if (ti->page_mask) {
-		*page = (index >> 8) & ti->page_mask;
-		index &= ~(ti->page_mask << 8);
-	}
-	return ti->sram_virt + index;
-}
-
-static void dir_open_adapter (struct net_device *dev)
-{
-        struct tok_info *ti = netdev_priv(dev);
-        unsigned char ret_code;
-        __u16 err;
-
-        ti->srb = map_address(ti,
-		ntohs(readw(ti->init_srb + SRB_ADDRESS_OFST)),
-		&ti->srb_page);
-        ti->ssb = map_address(ti,
-		ntohs(readw(ti->init_srb + SSB_ADDRESS_OFST)),
-		&ti->ssb_page);
-        ti->arb = map_address(ti,
-		ntohs(readw(ti->init_srb + ARB_ADDRESS_OFST)),
-		&ti->arb_page);
-        ti->asb = map_address(ti,
-		ntohs(readw(ti->init_srb + ASB_ADDRESS_OFST)),
-		&ti->asb_page);
-        ti->current_skb = NULL;
-        ret_code = readb(ti->init_srb + RETCODE_OFST);
-        err = ntohs(readw(ti->init_srb + OPEN_ERROR_CODE_OFST));
-        if (!ret_code) {
-		ti->open_status = OPEN; /* TR adapter is now available */
-                if (ti->open_mode == AUTOMATIC) {
-			DPRINTK("Adapter reopened.\n");
-                }
-                writeb(~SRB_RESP_INT, ti->mmio+ACA_OFFSET+ACA_RESET+ISRP_ODD);
-                open_sap(EXTENDED_SAP, dev);
-		return;
-	}
-	ti->open_failure = YES;
-	if (ret_code == 7){
-               if (err == 0x24) {
-			if (!ti->auto_speedsave) {
-				DPRINTK("Open failed: Adapter speed must match "
-                                 "ring speed if Automatic Ring Speed Save is "
-				 "disabled.\n");
-				ti->open_action = FAIL;
-			}else
-				DPRINTK("Retrying open to adjust to "
-					"ring speed, ");
-                } else if (err == 0x2d) {
-			DPRINTK("Physical Insertion: No Monitor Detected, ");
-			printk("retrying after %ds delay...\n",
-					TR_RETRY_INTERVAL/HZ);
-                } else if (err == 0x11) {
-			DPRINTK("Lobe Media Function Failure (0x11), ");
-			printk(" retrying after %ds delay...\n",
-					TR_RETRY_INTERVAL/HZ);
-                } else {
-			char **prphase = printphase;
-			char **prerror = printerror;
-			int pnr = err / 16 - 1;
-			int enr = err % 16 - 1;
-			DPRINTK("TR Adapter misc open failure, error code = ");
-			if (pnr < 0 || pnr >= ARRAY_SIZE(printphase) ||
-					enr < 0 ||
-					enr >= ARRAY_SIZE(printerror))
-				printk("0x%x, invalid Phase/Error.", err);
-			else
-				printk("0x%x, Phase: %s, Error: %s\n", err,
-						prphase[pnr], prerror[enr]);
-			printk(" retrying after %ds delay...\n",
-					TR_RETRY_INTERVAL/HZ);
-                }
-        } else DPRINTK("open failed: ret_code = %02X..., ", ret_code);
-	if (ti->open_action != FAIL) {
-		if (ti->open_mode==AUTOMATIC){
-			ti->open_action = REOPEN;
-			ibmtr_reset_timer(&(ti->tr_timer), dev);
-			return;
-		}
-		wake_up(&ti->wait_for_reset);
-		return;
-	}
-	DPRINTK("FAILURE, CAPUT\n");
-}
-
-/******************************************************************************/
-
-static irqreturn_t tok_interrupt(int irq, void *dev_id)
-{
-	unsigned char status;
-	/*  unsigned char status_even ; */
-	struct tok_info *ti;
-	struct net_device *dev;
-#ifdef ENABLE_PAGING
-	unsigned char save_srpr;
-#endif
-
-	dev = dev_id;
-#if TR_VERBOSE
-	DPRINTK("Int from tok_driver, dev : %p irq%d\n", dev,irq);
-#endif
-	ti = netdev_priv(dev);
-	if (ti->sram_phys & 1)
-		return IRQ_NONE;         /* PCMCIA card extraction flag */
-	spin_lock(&(ti->lock));
-#ifdef ENABLE_PAGING
-	save_srpr = readb(ti->mmio + ACA_OFFSET + ACA_RW + SRPR_EVEN);
-#endif
-
-	/* Disable interrupts till processing is finished */
-	writeb((~INT_ENABLE), ti->mmio + ACA_OFFSET + ACA_RESET + ISRP_EVEN);
-
-	/* Reset interrupt for ISA boards */
-	if (ti->adapter_int_enable)
-		outb(0, ti->adapter_int_enable);
-	else /* used for PCMCIA cards */
-		outb(0, ti->global_int_enable);
-        if (ti->do_tok_int == FIRST_INT){
-                initial_tok_int(dev);
-#ifdef ENABLE_PAGING
-                writeb(save_srpr, ti->mmio + ACA_OFFSET + ACA_RW + SRPR_EVEN);
-#endif
-                spin_unlock(&(ti->lock));
-                return IRQ_HANDLED;
-        }
-	/*  Begin interrupt handler HERE inline to avoid the extra
-	    levels of logic and call depth for the original solution. */
-	status = readb(ti->mmio + ACA_OFFSET + ACA_RW + ISRP_ODD);
-	/*BMSstatus_even = readb (ti->mmio + ACA_OFFSET + ACA_RW + ISRP_EVEN) */
-	/*BMSdebugprintk("tok_interrupt: ISRP_ODD = 0x%x ISRP_EVEN = 0x%x\n", */
-	/*BMS                                       status,status_even);      */
-
-	if (status & ADAP_CHK_INT) {
-		int i;
-		void __iomem *check_reason;
-		__u8 check_reason_page = 0;
-		check_reason = map_address(ti,
-			ntohs(readw(ti->mmio+ ACA_OFFSET+ACA_RW + WWCR_EVEN)),
-			&check_reason_page);
-		SET_PAGE(check_reason_page);
-
-		DPRINTK("Adapter check interrupt\n");
-		DPRINTK("8 reason bytes follow: ");
-		for (i = 0; i < 8; i++, check_reason++)
-			printk("%02X ", (int) readb(check_reason));
-		printk("\n");
-		writeb(~ADAP_CHK_INT, ti->mmio+ ACA_OFFSET+ACA_RESET+ ISRP_ODD);
-		status = readb(ti->mmio + ACA_OFFSET + ACA_RW + ISRA_EVEN);
-		DPRINTK("ISRA_EVEN == 0x02%x\n",status);
-		ti->open_status = CLOSED;
-		ti->sap_status  = CLOSED;
-		ti->open_mode   = AUTOMATIC;
-		netif_carrier_off(dev);
-		netif_stop_queue(dev);
-		ti->open_action = RESTART;
-		outb(0, dev->base_addr + ADAPTRESET);
-		ibmtr_reset_timer(&(ti->tr_timer), dev);/*BMS try to reopen*/
-		spin_unlock(&(ti->lock));
-		return IRQ_HANDLED;
-	}
-	if (readb(ti->mmio + ACA_OFFSET + ACA_RW + ISRP_EVEN)
-		& (TCR_INT | ERR_INT | ACCESS_INT)) {
-		DPRINTK("adapter error: ISRP_EVEN : %02x\n",
-			(int)readb(ti->mmio+ ACA_OFFSET + ACA_RW + ISRP_EVEN));
-		writeb(~(TCR_INT | ERR_INT | ACCESS_INT),
-			ti->mmio + ACA_OFFSET + ACA_RESET + ISRP_EVEN);
-		status= readb(ti->mmio+ ACA_OFFSET + ACA_RW + ISRA_EVEN);/*BMS*/
-		DPRINTK("ISRA_EVEN == 0x02%x\n",status);/*BMS*/
-                writeb(INT_ENABLE, ti->mmio + ACA_OFFSET + ACA_SET + ISRP_EVEN);
-#ifdef ENABLE_PAGING
-                writeb(save_srpr, ti->mmio + ACA_OFFSET + ACA_RW + SRPR_EVEN);
-#endif
-                spin_unlock(&(ti->lock));
-                return IRQ_HANDLED;
-        }
-	if (status & SRB_RESP_INT) {	/* SRB response */
-		SET_PAGE(ti->srb_page);
-#if TR_VERBOSE
-		DPRINTK("SRB resp: cmd=%02X rsp=%02X\n",
-				readb(ti->srb), readb(ti->srb + RETCODE_OFST));
-#endif
-		switch (readb(ti->srb)) {	/* SRB command check */
-		case XMIT_DIR_FRAME:{
-			unsigned char xmit_ret_code;
-			xmit_ret_code = readb(ti->srb + RETCODE_OFST);
-			if (xmit_ret_code == 0xff) break;
-			DPRINTK("error on xmit_dir_frame request: %02X\n",
-								xmit_ret_code);
-			if (ti->current_skb) {
-				dev_kfree_skb_irq(ti->current_skb);
-				ti->current_skb = NULL;
-			}
-			/*dev->tbusy = 0;*/
-			netif_wake_queue(dev);
-			if (ti->readlog_pending)
-				ibmtr_readlog(dev);
-			break;
-		}
-		case XMIT_UI_FRAME:{
-			unsigned char xmit_ret_code;
-
-			xmit_ret_code = readb(ti->srb + RETCODE_OFST);
-			if (xmit_ret_code == 0xff) break;
-			DPRINTK("error on xmit_ui_frame request: %02X\n",
-								xmit_ret_code);
-			if (ti->current_skb) {
-				dev_kfree_skb_irq(ti->current_skb);
-				ti->current_skb = NULL;
-			}
-			netif_wake_queue(dev);
-			if (ti->readlog_pending)
-				ibmtr_readlog(dev);
-			break;
-		}
-		case DIR_OPEN_ADAPTER:
-			dir_open_adapter(dev);
-			break;
-		case DLC_OPEN_SAP:
-			if (readb(ti->srb + RETCODE_OFST)) {
-				DPRINTK("open_sap failed: ret_code = %02X, "
-					"retrying\n",
-					(int) readb(ti->srb + RETCODE_OFST));
-				ti->open_action = REOPEN;
-				ibmtr_reset_timer(&(ti->tr_timer), dev);
-				break;
-			}
-			ti->exsap_station_id = readw(ti->srb + STATION_ID_OFST);
-			ti->sap_status = OPEN;/* TR adapter is now available */
-			if (ti->open_mode==MANUAL){
-				wake_up(&ti->wait_for_reset);
-				break;
-			}
-			netif_wake_queue(dev);
-			netif_carrier_on(dev);
-			break;
-		case DIR_INTERRUPT:
-		case DIR_MOD_OPEN_PARAMS:
-		case DIR_SET_GRP_ADDR:
-		case DIR_SET_FUNC_ADDR:
-		case DLC_CLOSE_SAP:
-			if (readb(ti->srb + RETCODE_OFST))
-				DPRINTK("error on %02X: %02X\n",
-					(int) readb(ti->srb + COMMAND_OFST),
-					(int) readb(ti->srb + RETCODE_OFST));
-			break;
-		case DIR_READ_LOG:
-			if (readb(ti->srb + RETCODE_OFST)){
-				DPRINTK("error on dir_read_log: %02X\n",
-					(int) readb(ti->srb + RETCODE_OFST));
-				netif_wake_queue(dev);
-				break;
-			}
-#if IBMTR_DEBUG_MESSAGES
-
-#define LINE_ERRORS_OFST                 0
-#define INTERNAL_ERRORS_OFST             1
-#define BURST_ERRORS_OFST                2
-#define AC_ERRORS_OFST                   3
-#define ABORT_DELIMITERS_OFST            4
-#define LOST_FRAMES_OFST                 6
-#define RECV_CONGEST_COUNT_OFST          7
-#define FRAME_COPIED_ERRORS_OFST         8
-#define FREQUENCY_ERRORS_OFST            9
-#define TOKEN_ERRORS_OFST               10
-
-			DPRINTK("Line errors %02X, Internal errors %02X, "
-			"Burst errors %02X\n" "A/C errors %02X, "
-			"Abort delimiters %02X, Lost frames %02X\n"
-			"Receive congestion count %02X, "
-			"Frame copied errors %02X\nFrequency errors %02X, "
-			"Token errors %02X\n",
-			(int) readb(ti->srb + LINE_ERRORS_OFST),
-			(int) readb(ti->srb + INTERNAL_ERRORS_OFST),
-			(int) readb(ti->srb + BURST_ERRORS_OFST),
-			(int) readb(ti->srb + AC_ERRORS_OFST),
-			(int) readb(ti->srb + ABORT_DELIMITERS_OFST),
-			(int) readb(ti->srb + LOST_FRAMES_OFST),
-			(int) readb(ti->srb + RECV_CONGEST_COUNT_OFST),
-			(int) readb(ti->srb + FRAME_COPIED_ERRORS_OFST),
-			(int) readb(ti->srb + FREQUENCY_ERRORS_OFST),
-			(int) readb(ti->srb + TOKEN_ERRORS_OFST));
-#endif
-			netif_wake_queue(dev);
-			break;
-		default:
-			DPRINTK("Unknown command %02X encountered\n",
-						(int) readb(ti->srb));
-        	}	/* end switch SRB command check */
-		writeb(~SRB_RESP_INT, ti->mmio+ ACA_OFFSET+ACA_RESET+ ISRP_ODD);
-	}	/* if SRB response */
-	if (status & ASB_FREE_INT) {	/* ASB response */
-		SET_PAGE(ti->asb_page);
-#if TR_VERBOSE
-		DPRINTK("ASB resp: cmd=%02X\n", readb(ti->asb));
-#endif
-
-		switch (readb(ti->asb)) {	/* ASB command check */
-		case REC_DATA:
-		case XMIT_UI_FRAME:
-		case XMIT_DIR_FRAME:
-			break;
-		default:
-			DPRINTK("unknown command in asb %02X\n",
-						(int) readb(ti->asb));
-		}	/* switch ASB command check */
-		if (readb(ti->asb + 2) != 0xff)	/* checks ret_code */
-			DPRINTK("ASB error %02X in cmd %02X\n",
-				(int) readb(ti->asb + 2), (int) readb(ti->asb));
-		writeb(~ASB_FREE_INT, ti->mmio+ ACA_OFFSET+ACA_RESET+ ISRP_ODD);
-	}	/* if ASB response */
-
-#define STATUS_OFST             6
-#define NETW_STATUS_OFST        6
-
-	if (status & ARB_CMD_INT) {	/* ARB response */
-		SET_PAGE(ti->arb_page);
-#if TR_VERBOSE
-		DPRINTK("ARB resp: cmd=%02X\n", readb(ti->arb));
-#endif
-
-		switch (readb(ti->arb)) {	/* ARB command check */
-		case DLC_STATUS:
-			DPRINTK("DLC_STATUS new status: %02X on station %02X\n",
-				ntohs(readw(ti->arb + STATUS_OFST)),
-				ntohs(readw(ti->arb+ STATION_ID_OFST)));
-			break;
-		case REC_DATA:
-			tr_rx(dev);
-			break;
-		case RING_STAT_CHANGE:{
-			unsigned short ring_status;
-			ring_status= ntohs(readw(ti->arb + NETW_STATUS_OFST));
-			if (ibmtr_debug_trace & TRC_INIT)
-				DPRINTK("Ring Status Change...(0x%x)\n",
-								ring_status);
-			if(ring_status& (REMOVE_RECV|AUTO_REMOVAL|LOBE_FAULT)){
-				netif_stop_queue(dev);
-				netif_carrier_off(dev);
-				DPRINTK("Remove received, or Auto-removal error"
-					", or Lobe fault\n");
-				DPRINTK("We'll try to reopen the closed adapter"
-					" after a %d second delay.\n",
-						TR_RETRY_INTERVAL/HZ);
-				/*I was confused: I saw the TR reopening but */
-				/*forgot:with an RJ45 in an RJ45/ICS adapter */
-				/*but adapter not in the ring, the TR will   */
-				/* open, and then soon close and come here.  */
-				ti->open_mode = AUTOMATIC;
-				ti->open_status = CLOSED; /*12/2000 BMS*/
-				ti->open_action = REOPEN;
-				ibmtr_reset_timer(&(ti->tr_timer), dev);
-			} else if (ring_status & LOG_OVERFLOW) {
-				if(netif_queue_stopped(dev))
-					ti->readlog_pending = 1;
-				else
-					ibmtr_readlog(dev);
-			}
-			break;
-          	}
-		case XMIT_DATA_REQ:
-			tr_tx(dev);
-			break;
-		default:
-			DPRINTK("Unknown command %02X in arb\n",
-						(int) readb(ti->arb));
-			break;
-		}	/* switch ARB command check */
-		writeb(~ARB_CMD_INT, ti->mmio+ ACA_OFFSET+ACA_RESET + ISRP_ODD);
-		writeb(ARB_FREE, ti->mmio + ACA_OFFSET + ACA_SET + ISRA_ODD);
-	}	/* if ARB response */
-	if (status & SSB_RESP_INT) {	/* SSB response */
-		unsigned char retcode;
-		SET_PAGE(ti->ssb_page);
-#if TR_VERBOSE
-		DPRINTK("SSB resp: cmd=%02X rsp=%02X\n",
-				readb(ti->ssb), readb(ti->ssb + 2));
-#endif
-
-		switch (readb(ti->ssb)) {	/* SSB command check */
-		case XMIT_DIR_FRAME:
-		case XMIT_UI_FRAME:
-			retcode = readb(ti->ssb + 2);
-			if (retcode && (retcode != 0x22))/* checks ret_code */
-				DPRINTK("xmit ret_code: %02X xmit error code: "
-					"%02X\n",
-					(int)retcode, (int)readb(ti->ssb + 6));
-			else
-				dev->stats.tx_packets++;
-			break;
-		case XMIT_XID_CMD:
-			DPRINTK("xmit xid ret_code: %02X\n",
-						(int) readb(ti->ssb + 2));
-		default:
-			DPRINTK("Unknown command %02X in ssb\n",
-						(int) readb(ti->ssb));
-		}	/* SSB command check */
-		writeb(~SSB_RESP_INT, ti->mmio+ ACA_OFFSET+ACA_RESET+ ISRP_ODD);
-		writeb(SSB_FREE, ti->mmio + ACA_OFFSET + ACA_SET + ISRA_ODD);
-	}	/* if SSB response */
-#ifdef ENABLE_PAGING
-	writeb(save_srpr, ti->mmio + ACA_OFFSET + ACA_RW + SRPR_EVEN);
-#endif
-	writeb(INT_ENABLE, ti->mmio + ACA_OFFSET + ACA_SET + ISRP_EVEN);
-	spin_unlock(&(ti->lock));
-	return IRQ_HANDLED;
-}				/*tok_interrupt */
-
-/*****************************************************************************/
-
-#define INIT_STATUS_OFST        1
-#define INIT_STATUS_2_OFST      2
-#define ENCODED_ADDRESS_OFST    8
-
-static void initial_tok_int(struct net_device *dev)
-{
-
-	__u32 encoded_addr, hw_encoded_addr;
-	struct tok_info *ti;
-        unsigned char init_status; /*BMS 12/2000*/
-
-	ti = netdev_priv(dev);
-
-	ti->do_tok_int = NOT_FIRST;
-
-	/* we assign the shared-ram address for ISA devices */
-	writeb(ti->sram_base, ti->mmio + ACA_OFFSET + ACA_RW + RRR_EVEN);
-#ifndef PCMCIA
-        ti->sram_virt = ioremap(((__u32)ti->sram_base << 12), ti->avail_shared_ram);
-#endif
-	ti->init_srb = map_address(ti,
-		ntohs(readw(ti->mmio + ACA_OFFSET + WRBR_EVEN)),
-		&ti->init_srb_page);
-	if (ti->page_mask && ti->avail_shared_ram == 127) {
-		void __iomem *last_512;
-		__u8 last_512_page=0;
-		int i;
-		last_512 = map_address(ti, 0xfe00, &last_512_page);
-		/* initialize high section of ram (if necessary) */
-		SET_PAGE(last_512_page);
-		for (i = 0; i < 512; i++)
-			writeb(0, last_512 + i);
-	}
-	SET_PAGE(ti->init_srb_page);
-
-#if TR_VERBOSE
-	{
-	int i;
-
-	DPRINTK("ti->init_srb_page=0x%x\n", ti->init_srb_page);
-	DPRINTK("init_srb(%p):", ti->init_srb );
-	for (i = 0; i < 20; i++)
-		printk("%02X ", (int) readb(ti->init_srb + i));
-	printk("\n");
-	}
-#endif
-
-	hw_encoded_addr = readw(ti->init_srb + ENCODED_ADDRESS_OFST);
-	encoded_addr = ntohs(hw_encoded_addr);
-        init_status= /*BMS 12/2000 check for shallow mode possibility (Turbo)*/
-	readb(ti->init_srb+offsetof(struct srb_init_response,init_status));
-	/*printk("Initial interrupt: init_status= 0x%02x\n",init_status);*/
-	ti->ring_speed = init_status & 0x01 ? 16 : 4;
-	DPRINTK("Initial interrupt : %d Mbps, shared RAM base %08x.\n",
-				ti->ring_speed, (unsigned int)dev->mem_start);
-	ti->auto_speedsave = (readb(ti->init_srb+INIT_STATUS_2_OFST) & 4) != 0;
-
-        if (ti->open_mode == MANUAL)	wake_up(&ti->wait_for_reset);
-	else				tok_open_adapter((unsigned long)dev);
-        
-} /*initial_tok_int() */
-
-/*****************************************************************************/
-
-#define CMD_CORRELATE_OFST      1
-#define DHB_ADDRESS_OFST        6
-
-#define FRAME_LENGTH_OFST       6
-#define HEADER_LENGTH_OFST      8
-#define RSAP_VALUE_OFST         9
-
-static void tr_tx(struct net_device *dev)
-{
-	struct tok_info *ti = netdev_priv(dev);
-	struct trh_hdr *trhdr = (struct trh_hdr *) ti->current_skb->data;
-	unsigned int hdr_len;
-	__u32 dhb=0,dhb_base;
-	void __iomem *dhbuf = NULL;
-	unsigned char xmit_command;
-	int i,dhb_len=0x4000,src_len,src_offset;
-	struct trllc *llc;
-	struct srb_xmit xsrb;
-	__u8 dhb_page = 0;
-	__u8 llc_ssap;
-
-	SET_PAGE(ti->asb_page);
-
-	if (readb(ti->asb+RETCODE_OFST) != 0xFF) DPRINTK("ASB not free !!!\n");
-
-	/* in providing the transmit interrupts, is telling us it is ready for
-	   data and providing a shared memory address for us to stuff with data.
-	   Here we compute the effective address where we will place data.
-	*/
-	SET_PAGE(ti->arb_page);
-	dhb=dhb_base=ntohs(readw(ti->arb + DHB_ADDRESS_OFST));
-	if (ti->page_mask) {
-		dhb_page = (dhb_base >> 8) & ti->page_mask;
-		dhb=dhb_base & ~(ti->page_mask << 8);
-	}
-	dhbuf = ti->sram_virt + dhb;
-
-	/* Figure out the size of the 802.5 header */
-	if (!(trhdr->saddr[0] & 0x80))	/* RIF present? */
-		hdr_len = sizeof(struct trh_hdr) - TR_MAXRIFLEN;
-	else
-		hdr_len = ((ntohs(trhdr->rcf) & TR_RCF_LEN_MASK) >> 8)
-		    + sizeof(struct trh_hdr) - TR_MAXRIFLEN;
-
-	llc = (struct trllc *) (ti->current_skb->data + hdr_len);
-
-	llc_ssap = llc->ssap;
-	SET_PAGE(ti->srb_page);
-	memcpy_fromio(&xsrb, ti->srb, sizeof(xsrb));
-	SET_PAGE(ti->asb_page);
-	xmit_command = xsrb.command;
-
-	writeb(xmit_command, ti->asb + COMMAND_OFST);
-	writew(xsrb.station_id, ti->asb + STATION_ID_OFST);
-	writeb(llc_ssap, ti->asb + RSAP_VALUE_OFST);
-	writeb(xsrb.cmd_corr, ti->asb + CMD_CORRELATE_OFST);
-	writeb(0, ti->asb + RETCODE_OFST);
-	if ((xmit_command == XMIT_XID_CMD) || (xmit_command == XMIT_TEST_CMD)) {
-		writew(htons(0x11), ti->asb + FRAME_LENGTH_OFST);
-		writeb(0x0e, ti->asb + HEADER_LENGTH_OFST);
-		SET_PAGE(dhb_page);
-		writeb(AC, dhbuf);
-		writeb(LLC_FRAME, dhbuf + 1);
-		for (i = 0; i < TR_ALEN; i++)
-			writeb((int) 0x0FF, dhbuf + i + 2);
-		for (i = 0; i < TR_ALEN; i++)
-			writeb(0, dhbuf + i + TR_ALEN + 2);
-		writeb(RESP_IN_ASB, ti->mmio + ACA_OFFSET + ACA_SET + ISRA_ODD);
-		return;
-	}
-	/*
-	 *    the token ring packet is copied from sk_buff to the adapter
-	 *    buffer identified in the command data received with the interrupt.
-	 */
-	writeb(hdr_len, ti->asb + HEADER_LENGTH_OFST);
-	writew(htons(ti->current_skb->len), ti->asb + FRAME_LENGTH_OFST);
-	src_len=ti->current_skb->len;
-	src_offset=0;
-	dhb=dhb_base;
-	while(1) {
-		if (ti->page_mask) {
-			dhb_page=(dhb >> 8) & ti->page_mask;
-			dhb=dhb & ~(ti->page_mask << 8);
-			dhb_len=0x4000-dhb; /* remaining size of this page */
-		}
-		dhbuf = ti->sram_virt + dhb;
-		SET_PAGE(dhb_page);
-		if (src_len > dhb_len) {
-			memcpy_toio(dhbuf,&ti->current_skb->data[src_offset],
-					dhb_len);
-			src_len -= dhb_len;
-			src_offset += dhb_len;
-			dhb_base+=dhb_len;
-			dhb=dhb_base;
-			continue;
-		}
-		memcpy_toio(dhbuf, &ti->current_skb->data[src_offset], src_len);
-		break;
-	}
-	writeb(RESP_IN_ASB, ti->mmio + ACA_OFFSET + ACA_SET + ISRA_ODD);
-	dev->stats.tx_bytes += ti->current_skb->len;
-	dev_kfree_skb_irq(ti->current_skb);
-	ti->current_skb = NULL;
-	netif_wake_queue(dev);
-	if (ti->readlog_pending)
-		ibmtr_readlog(dev);
-}				/*tr_tx */
-
-/*****************************************************************************/
-
-
-#define RECEIVE_BUFFER_OFST     6
-#define LAN_HDR_LENGTH_OFST     8
-#define DLC_HDR_LENGTH_OFST     9
-
-#define DSAP_OFST               0
-#define SSAP_OFST               1
-#define LLC_OFST                2
-#define PROTID_OFST             3
-#define ETHERTYPE_OFST          6
-
-static void tr_rx(struct net_device *dev)
-{
-	struct tok_info *ti = netdev_priv(dev);
-	__u32 rbuffer;
-	void __iomem *rbuf, *rbufdata, *llc;
-	__u8 rbuffer_page = 0;
-	unsigned char *data;
-	unsigned int rbuffer_len, lan_hdr_len, hdr_len, ip_len, length;
-	unsigned char dlc_hdr_len;
-	struct sk_buff *skb;
-	unsigned int skb_size = 0;
-	int IPv4_p = 0;
-	unsigned int chksum = 0;
-	struct iphdr *iph;
-	struct arb_rec_req rarb;
-
-	SET_PAGE(ti->arb_page);
-	memcpy_fromio(&rarb, ti->arb, sizeof(rarb));
-	rbuffer = ntohs(rarb.rec_buf_addr) ;
-	rbuf = map_address(ti, rbuffer, &rbuffer_page);
-
-	SET_PAGE(ti->asb_page);
-
-	if (readb(ti->asb + RETCODE_OFST) !=0xFF) DPRINTK("ASB not free !!!\n");
-
-	writeb(REC_DATA, ti->asb + COMMAND_OFST);
-	writew(rarb.station_id, ti->asb + STATION_ID_OFST);
-	writew(rarb.rec_buf_addr, ti->asb + RECEIVE_BUFFER_OFST);
-
-	lan_hdr_len = rarb.lan_hdr_len;
-	if (lan_hdr_len > sizeof(struct trh_hdr)) {
-		DPRINTK("Linux cannot handle greater than 18 bytes RIF\n");
-		return;
-	}			/*BMS I added this above just to be very safe */
-	dlc_hdr_len = readb(ti->arb + DLC_HDR_LENGTH_OFST);
-	hdr_len = lan_hdr_len + sizeof(struct trllc) + sizeof(struct iphdr);
-
-	SET_PAGE(rbuffer_page);
-	llc = rbuf + offsetof(struct rec_buf, data) + lan_hdr_len;
-
-#if TR_VERBOSE
-	DPRINTK("offsetof data: %02X lan_hdr_len: %02X\n",
-	(__u32) offsetof(struct rec_buf, data), (unsigned int) lan_hdr_len);
-	DPRINTK("llc: %08X rec_buf_addr: %04X dev->mem_start: %lX\n",
-		llc, ntohs(rarb.rec_buf_addr), dev->mem_start);
-	DPRINTK("dsap: %02X, ssap: %02X, llc: %02X, protid: %02X%02X%02X, "
-		"ethertype: %04X\n",
-		(int) readb(llc + DSAP_OFST), (int) readb(llc + SSAP_OFST),
-		(int) readb(llc + LLC_OFST), (int) readb(llc + PROTID_OFST),
-		(int) readb(llc+PROTID_OFST+1),(int)readb(llc+PROTID_OFST + 2),
-		(int) ntohs(readw(llc + ETHERTYPE_OFST)));
-#endif
-	if (readb(llc + offsetof(struct trllc, llc)) != UI_CMD) {
-		SET_PAGE(ti->asb_page);
-		writeb(DATA_LOST, ti->asb + RETCODE_OFST);
-		dev->stats.rx_dropped++;
-		writeb(RESP_IN_ASB, ti->mmio + ACA_OFFSET + ACA_SET + ISRA_ODD);
-		return;
-	}
-	length = ntohs(rarb.frame_len);
-	if (readb(llc + DSAP_OFST) == EXTENDED_SAP &&
-	   readb(llc + SSAP_OFST) == EXTENDED_SAP &&
-		length >= hdr_len)	IPv4_p = 1;
-#if TR_VERBOSE
-#define SADDR_OFST	8
-#define DADDR_OFST	2
-
-	if (!IPv4_p) {
-
-		void __iomem *trhhdr = rbuf + offsetof(struct rec_buf, data);
-		u8 saddr[6];
-		u8 daddr[6];
-		int i;
-		for (i = 0 ; i < 6 ; i++)
-			saddr[i] = readb(trhhdr + SADDR_OFST + i);
-		for (i = 0 ; i < 6 ; i++)
-			daddr[i] = readb(trhhdr + DADDR_OFST + i);
-		DPRINTK("Probably non-IP frame received.\n");
-		DPRINTK("ssap: %02X dsap: %02X "
-			"saddr: %pM daddr: %pM\n",
-			readb(llc + SSAP_OFST), readb(llc + DSAP_OFST),
-			saddr, daddr);
-	}
-#endif
-
-	/*BMS handle the case she comes in with few hops but leaves with many */
-        skb_size=length-lan_hdr_len+sizeof(struct trh_hdr)+sizeof(struct trllc);
-
-	if (!(skb = dev_alloc_skb(skb_size))) {
-		DPRINTK("out of memory. frame dropped.\n");
-		dev->stats.rx_dropped++;
-		SET_PAGE(ti->asb_page);
-		writeb(DATA_LOST, ti->asb + offsetof(struct asb_rec, ret_code));
-		writeb(RESP_IN_ASB, ti->mmio + ACA_OFFSET + ACA_SET + ISRA_ODD);
-		return;
-	}
-	/*BMS again, if she comes in with few but leaves with many */
-	skb_reserve(skb, sizeof(struct trh_hdr) - lan_hdr_len);
-	skb_put(skb, length);
-	data = skb->data;
-	rbuffer_len = ntohs(readw(rbuf + offsetof(struct rec_buf, buf_len)));
-	rbufdata = rbuf + offsetof(struct rec_buf, data);
-
-	if (IPv4_p) {
-		/* Copy the headers without checksumming */
-		memcpy_fromio(data, rbufdata, hdr_len);
-
-		/* Watch for padded packets and bogons */
-		iph= (struct iphdr *)(data+ lan_hdr_len + sizeof(struct trllc));
-		ip_len = ntohs(iph->tot_len) - sizeof(struct iphdr);
-		length -= hdr_len;
-		if ((ip_len <= length) && (ip_len > 7))
-			length = ip_len;
-		data += hdr_len;
-		rbuffer_len -= hdr_len;
-		rbufdata += hdr_len;
-	}
-	/* Copy the payload... */
-#define BUFFER_POINTER_OFST	2
-#define BUFFER_LENGTH_OFST      6
-	for (;;) {
-		if (ibmtr_debug_trace&TRC_INITV && length < rbuffer_len)
-			DPRINTK("CURIOUS, length=%d < rbuffer_len=%d\n",
-						length,rbuffer_len);
-		if (IPv4_p)
-			chksum=csum_partial_copy_nocheck((void*)rbufdata,
-			    data,length<rbuffer_len?length:rbuffer_len,chksum);
-		else
-			memcpy_fromio(data, rbufdata, rbuffer_len);
-		rbuffer = ntohs(readw(rbuf+BUFFER_POINTER_OFST)) ;
-		if (!rbuffer)
-			break;
-		rbuffer -= 2;
-		length -= rbuffer_len;
-		data += rbuffer_len;
-		rbuf = map_address(ti, rbuffer, &rbuffer_page);
-		SET_PAGE(rbuffer_page);
-		rbuffer_len = ntohs(readw(rbuf + BUFFER_LENGTH_OFST));
-		rbufdata = rbuf + offsetof(struct rec_buf, data);
-	}
-
-	SET_PAGE(ti->asb_page);
-	writeb(0, ti->asb + offsetof(struct asb_rec, ret_code));
-
-	writeb(RESP_IN_ASB, ti->mmio + ACA_OFFSET + ACA_SET + ISRA_ODD);
-
-	dev->stats.rx_bytes += skb->len;
-	dev->stats.rx_packets++;
-
-	skb->protocol = tr_type_trans(skb, dev);
-	if (IPv4_p) {
-		skb->csum = chksum;
-		skb->ip_summed = CHECKSUM_COMPLETE;
-	}
-	netif_rx(skb);
-}				/*tr_rx */
-
-/*****************************************************************************/
-
-static void ibmtr_reset_timer(struct timer_list *tmr, struct net_device *dev)
-{
-	tmr->expires = jiffies + TR_RETRY_INTERVAL;
-	tmr->data = (unsigned long) dev;
-	tmr->function = tok_rerun;
-	init_timer(tmr);
-	add_timer(tmr);
-}
-
-/*****************************************************************************/
-
-static void tok_rerun(unsigned long dev_addr)
-{
-	struct net_device *dev = (struct net_device *)dev_addr;
-	struct tok_info *ti = netdev_priv(dev);
-
-	if ( ti->open_action == RESTART){
-		ti->do_tok_int = FIRST_INT;
-		outb(0, dev->base_addr + ADAPTRESETREL);
-#ifdef ENABLE_PAGING
-		if (ti->page_mask)
-			writeb(SRPR_ENABLE_PAGING,
-				ti->mmio + ACA_OFFSET + ACA_RW + SRPR_EVEN);
-#endif
-
-		writeb(INT_ENABLE, ti->mmio + ACA_OFFSET + ACA_SET + ISRP_EVEN);
-	} else
-		tok_open_adapter(dev_addr);
-}
-
-/*****************************************************************************/
-
-static void ibmtr_readlog(struct net_device *dev)
-{
-	struct tok_info *ti;
-
-	ti = netdev_priv(dev);
-
-	ti->readlog_pending = 0;
-	SET_PAGE(ti->srb_page);
-	writeb(DIR_READ_LOG, ti->srb);
-	writeb(INT_ENABLE, ti->mmio + ACA_OFFSET + ACA_SET + ISRP_EVEN);
-	writeb(CMD_IN_SRB, ti->mmio + ACA_OFFSET + ACA_SET + ISRA_ODD);
-
-	netif_stop_queue(dev);
-
-}
-
-/*****************************************************************************/
-
-static int ibmtr_change_mtu(struct net_device *dev, int mtu)
-{
-	struct tok_info *ti = netdev_priv(dev);
-
-	if (ti->ring_speed == 16 && mtu > ti->maxmtu16)
-		return -EINVAL;
-	if (ti->ring_speed == 4 && mtu > ti->maxmtu4)
-		return -EINVAL;
-	dev->mtu = mtu;
-	return 0;
-}
-
-/*****************************************************************************/
-#ifdef MODULE
-
-/* 3COM 3C619C supports 8 interrupts, 32 I/O ports */
-static struct net_device *dev_ibmtr[IBMTR_MAX_ADAPTERS];
-static int io[IBMTR_MAX_ADAPTERS] = { 0xa20, 0xa24 };
-static int irq[IBMTR_MAX_ADAPTERS];
-static int mem[IBMTR_MAX_ADAPTERS];
-
-MODULE_LICENSE("GPL");
-
-module_param_array(io, int, NULL, 0);
-module_param_array(irq, int, NULL, 0);
-module_param_array(mem, int, NULL, 0);
-
-static int __init ibmtr_init(void)
-{
-	int i;
-	int count=0;
-
-	find_turbo_adapters(io);
-
-	for (i = 0; i < IBMTR_MAX_ADAPTERS && io[i]; i++) {
-		struct net_device *dev;
-		irq[i] = 0;
-		mem[i] = 0;
-		dev = alloc_trdev(sizeof(struct tok_info));
-		if (dev == NULL) { 
-			if (i == 0)
-				return -ENOMEM;
-			break;
-		}
-		dev->base_addr = io[i];
-		dev->irq = irq[i];
-		dev->mem_start = mem[i];
-
-		if (ibmtr_probe_card(dev)) {
-			free_netdev(dev);
-			continue;
-		}
-		dev_ibmtr[i] = dev;
-		count++;
-	}
-	if (count) return 0;
-	printk("ibmtr: register_netdev() returned non-zero.\n");
-	return -EIO;
-}
-module_init(ibmtr_init);
-
-static void __exit ibmtr_cleanup(void)
-{
-	int i;
-
-	for (i = 0; i < IBMTR_MAX_ADAPTERS; i++){
-		if (!dev_ibmtr[i])
-			continue;
-		unregister_netdev(dev_ibmtr[i]);
-		ibmtr_cleanup_card(dev_ibmtr[i]);
-		free_netdev(dev_ibmtr[i]);
-	}
-}
-module_exit(ibmtr_cleanup);
-#endif
diff --git a/drivers/net/tokenring/ibmtr_cs.c b/drivers/net/tokenring/ibmtr_cs.c
deleted file mode 100644
index 356e28e4881..00000000000
--- a/drivers/net/tokenring/ibmtr_cs.c
+++ /dev/null
@@ -1,370 +0,0 @@
-/*======================================================================
-
-    A PCMCIA token-ring driver for IBM-based cards
-
-    This driver supports the IBM PCMCIA Token-Ring Card.
-    Written by Steve Kipisz, kipisz@vnet.ibm.com or
-                             bungy@ibm.net
-
-    Written 1995,1996.
-
-    This code is based on pcnet_cs.c from David Hinds.
-    
-    V2.2.0 February 1999 - Mike Phillips phillim@amtrak.com
-
-    Linux V2.2.x presented significant changes to the underlying
-    ibmtr.c code.  Mainly the code became a lot more organized and
-    modular.
-
-    This caused the old PCMCIA Token Ring driver to give up and go 
-    home early. Instead of just patching the old code to make it 
-    work, the PCMCIA code has been streamlined, updated and possibly
-    improved.
-
-    This code now only contains code required for the Card Services.
-    All we do here is set the card up enough so that the real ibmtr.c
-    driver can find it and work with it properly.
-
-    i.e. We set up the io port, irq, mmio memory and shared ram
-    memory.  This enables ibmtr_probe in ibmtr.c to find the card and
-    configure it as though it was a normal ISA and/or PnP card.
-
-    CHANGES
-
-    v2.2.5 April 1999 Mike Phillips (phillim@amtrak.com)
-    Obscure bug fix, required changed to ibmtr.c not ibmtr_cs.c
-    
-    v2.2.7 May 1999 Mike Phillips (phillim@amtrak.com)
-    Updated to version 2.2.7 to match the first version of the kernel
-    that the modification to ibmtr.c were incorporated into.
-    
-    v2.2.17 July 2000 Burt Silverman (burts@us.ibm.com)
-    Address translation feature of PCMCIA controller is usable so
-    memory windows can be placed in High memory (meaning above
-    0xFFFFF.)
-
-======================================================================*/
-
-#define pr_fmt(fmt) KBUILD_MODNAME ": " fmt
-
-#include <linux/kernel.h>
-#include <linux/init.h>
-#include <linux/ptrace.h>
-#include <linux/slab.h>
-#include <linux/string.h>
-#include <linux/timer.h>
-#include <linux/module.h>
-#include <linux/netdevice.h>
-#include <linux/trdevice.h>
-#include <linux/ibmtr.h>
-
-#include <pcmcia/cistpl.h>
-#include <pcmcia/ds.h>
-
-#include <asm/uaccess.h>
-#include <asm/io.h>
-
-#define PCMCIA
-#include "ibmtr.c"
-
-
-/*====================================================================*/
-
-/* Parameters that can be set with 'insmod' */
-
-/* MMIO base address */
-static u_long mmiobase = 0xce000;
-
-/* SRAM base address */
-static u_long srambase = 0xd0000;
-
-/* SRAM size 8,16,32,64 */
-static u_long sramsize = 64;
-
-/* Ringspeed 4,16 */
-static int ringspeed = 16;
-
-module_param(mmiobase, ulong, 0);
-module_param(srambase, ulong, 0);
-module_param(sramsize, ulong, 0);
-module_param(ringspeed, int, 0);
-MODULE_LICENSE("GPL");
-
-/*====================================================================*/
-
-static int ibmtr_config(struct pcmcia_device *link);
-static void ibmtr_hw_setup(struct net_device *dev, u_int mmiobase);
-static void ibmtr_release(struct pcmcia_device *link);
-static void ibmtr_detach(struct pcmcia_device *p_dev);
-
-/*====================================================================*/
-
-typedef struct ibmtr_dev_t {
-	struct pcmcia_device	*p_dev;
-	struct net_device	*dev;
-	struct tok_info		*ti;
-} ibmtr_dev_t;
-
-static irqreturn_t ibmtr_interrupt(int irq, void *dev_id) {
-	ibmtr_dev_t *info = dev_id;
-	struct net_device *dev = info->dev;
-	return tok_interrupt(irq, dev);
-};
-
-static int __devinit ibmtr_attach(struct pcmcia_device *link)
-{
-    ibmtr_dev_t *info;
-    struct net_device *dev;
-
-    dev_dbg(&link->dev, "ibmtr_attach()\n");
-
-    /* Create new token-ring device */
-    info = kzalloc(sizeof(*info), GFP_KERNEL);
-    if (!info) return -ENOMEM;
-    dev = alloc_trdev(sizeof(struct tok_info));
-    if (!dev) {
-	kfree(info);
-	return -ENOMEM;
-    }
-
-    info->p_dev = link;
-    link->priv = info;
-    info->ti = netdev_priv(dev);
-
-    link->resource[0]->flags |= IO_DATA_PATH_WIDTH_8;
-    link->resource[0]->end = 4;
-    link->config_flags |= CONF_ENABLE_IRQ;
-    link->config_regs = PRESENT_OPTION;
-
-    info->dev = dev;
-
-    return ibmtr_config(link);
-} /* ibmtr_attach */
-
-static void ibmtr_detach(struct pcmcia_device *link)
-{
-    struct ibmtr_dev_t *info = link->priv;
-    struct net_device *dev = info->dev;
-     struct tok_info *ti = netdev_priv(dev);
-
-    dev_dbg(&link->dev, "ibmtr_detach\n");
-    
-    /* 
-     * When the card removal interrupt hits tok_interrupt(), 
-     * bail out early, so we don't crash the machine 
-     */
-    ti->sram_phys |= 1;
-
-    unregister_netdev(dev);
-    
-    del_timer_sync(&(ti->tr_timer));
-
-    ibmtr_release(link);
-
-    free_netdev(dev);
-    kfree(info);
-} /* ibmtr_detach */
-
-static int __devinit ibmtr_config(struct pcmcia_device *link)
-{
-    ibmtr_dev_t *info = link->priv;
-    struct net_device *dev = info->dev;
-    struct tok_info *ti = netdev_priv(dev);
-    int i, ret;
-
-    dev_dbg(&link->dev, "ibmtr_config\n");
-
-    link->io_lines = 16;
-    link->config_index = 0x61;
-
-    /* Determine if this is PRIMARY or ALTERNATE. */
-
-    /* Try PRIMARY card at 0xA20-0xA23 */
-    link->resource[0]->start = 0xA20;
-    i = pcmcia_request_io(link);
-    if (i != 0) {
-	/* Couldn't get 0xA20-0xA23.  Try ALTERNATE at 0xA24-0xA27. */
-	link->resource[0]->start = 0xA24;
-	ret = pcmcia_request_io(link);
-	if (ret)
-		goto failed;
-    }
-    dev->base_addr = link->resource[0]->start;
-
-    ret = pcmcia_request_exclusive_irq(link, ibmtr_interrupt);
-    if (ret)
-	    goto failed;
-    dev->irq = link->irq;
-    ti->irq = link->irq;
-    ti->global_int_enable=GLOBAL_INT_ENABLE+((dev->irq==9) ? 2 : dev->irq);
-
-    /* Allocate the MMIO memory window */
-    link->resource[2]->flags |= WIN_DATA_WIDTH_16|WIN_MEMORY_TYPE_CM|WIN_ENABLE;
-    link->resource[2]->flags |= WIN_USE_WAIT;
-    link->resource[2]->start = 0;
-    link->resource[2]->end = 0x2000;
-    ret = pcmcia_request_window(link, link->resource[2], 250);
-    if (ret)
-	    goto failed;
-
-    ret = pcmcia_map_mem_page(link, link->resource[2], mmiobase);
-    if (ret)
-	    goto failed;
-    ti->mmio = ioremap(link->resource[2]->start,
-		    resource_size(link->resource[2]));
-
-    /* Allocate the SRAM memory window */
-    link->resource[3]->flags = WIN_DATA_WIDTH_16|WIN_MEMORY_TYPE_CM|WIN_ENABLE;
-    link->resource[3]->flags |= WIN_USE_WAIT;
-    link->resource[3]->start = 0;
-    link->resource[3]->end = sramsize * 1024;
-    ret = pcmcia_request_window(link, link->resource[3], 250);
-    if (ret)
-	    goto failed;
-
-    ret = pcmcia_map_mem_page(link, link->resource[3], srambase);
-    if (ret)
-	    goto failed;
-
-    ti->sram_base = srambase >> 12;
-    ti->sram_virt = ioremap(link->resource[3]->start,
-		    resource_size(link->resource[3]));
-    ti->sram_phys = link->resource[3]->start;
-
-    ret = pcmcia_enable_device(link);
-    if (ret)
-	    goto failed;
-
-    /*  Set up the Token-Ring Controller Configuration Register and
-        turn on the card.  Check the "Local Area Network Credit Card
-        Adapters Technical Reference"  SC30-3585 for this info.  */
-    ibmtr_hw_setup(dev, mmiobase);
-
-    SET_NETDEV_DEV(dev, &link->dev);
-
-    i = ibmtr_probe_card(dev);
-    if (i != 0) {
-	pr_notice("register_netdev() failed\n");
-	goto failed;
-    }
-
-    netdev_info(dev, "port %#3lx, irq %d, mmio %#5lx, sram %#5lx, hwaddr=%pM\n",
-		dev->base_addr, dev->irq,
-		(u_long)ti->mmio, (u_long)(ti->sram_base << 12),
-		dev->dev_addr);
-    return 0;
-
-failed:
-    ibmtr_release(link);
-    return -ENODEV;
-} /* ibmtr_config */
-
-static void ibmtr_release(struct pcmcia_device *link)
-{
-	ibmtr_dev_t *info = link->priv;
-	struct net_device *dev = info->dev;
-
-	dev_dbg(&link->dev, "ibmtr_release\n");
-
-	if (link->resource[2]->end) {
-		struct tok_info *ti = netdev_priv(dev);
-		iounmap(ti->mmio);
-	}
-	pcmcia_disable_device(link);
-}
-
-static int ibmtr_suspend(struct pcmcia_device *link)
-{
-	ibmtr_dev_t *info = link->priv;
-	struct net_device *dev = info->dev;
-
-	if (link->open)
-		netif_device_detach(dev);
-
-	return 0;
-}
-
-static int __devinit ibmtr_resume(struct pcmcia_device *link)
-{
-	ibmtr_dev_t *info = link->priv;
-	struct net_device *dev = info->dev;
-
-	if (link->open) {
-		ibmtr_probe(dev);	/* really? */
-		netif_device_attach(dev);
-	}
-
-	return 0;
-}
-
-
-/*====================================================================*/
-
-static void ibmtr_hw_setup(struct net_device *dev, u_int mmiobase)
-{
-    int i;
-
-    /* Bizarre IBM behavior, there are 16 bits of information we
-       need to set, but the card only allows us to send 4 bits at a 
-       time.  For each byte sent to base_addr, bits 7-4 tell the
-       card which part of the 16 bits we are setting, bits 3-0 contain 
-       the actual information */
-
-    /* First nibble provides 4 bits of mmio */
-    i = (mmiobase >> 16) & 0x0F;
-    outb(i, dev->base_addr);
-
-    /* Second nibble provides 3 bits of mmio */
-    i = 0x10 | ((mmiobase >> 12) & 0x0E);
-    outb(i, dev->base_addr);
-
-    /* Third nibble, hard-coded values */
-    i = 0x26;
-    outb(i, dev->base_addr);
-
-    /* Fourth nibble sets shared ram page size */
-
-    /* 8 = 00, 16 = 01, 32 = 10, 64 = 11 */          
-    i = (sramsize >> 4) & 0x07;
-    i = ((i == 4) ? 3 : i) << 2;
-    i |= 0x30;
-
-    if (ringspeed == 16)
-	i |= 2;
-    if (dev->base_addr == 0xA24)
-	i |= 1;
-    outb(i, dev->base_addr);
-
-    /* 0x40 will release the card for use */
-    outb(0x40, dev->base_addr);
-}
-
-static const struct pcmcia_device_id ibmtr_ids[] = {
-	PCMCIA_DEVICE_PROD_ID12("3Com", "TokenLink Velocity PC Card", 0x41240e5b, 0x82c3734e),
-	PCMCIA_DEVICE_PROD_ID12("IBM", "TOKEN RING", 0xb569a6e5, 0xbf8eed47),
-	PCMCIA_DEVICE_NULL,
-};
-MODULE_DEVICE_TABLE(pcmcia, ibmtr_ids);
-
-static struct pcmcia_driver ibmtr_cs_driver = {
-	.owner		= THIS_MODULE,
-	.name		= "ibmtr_cs",
-	.probe		= ibmtr_attach,
-	.remove		= ibmtr_detach,
-	.id_table       = ibmtr_ids,
-	.suspend	= ibmtr_suspend,
-	.resume		= ibmtr_resume,
-};
-
-static int __init init_ibmtr_cs(void)
-{
-	return pcmcia_register_driver(&ibmtr_cs_driver);
-}
-
-static void __exit exit_ibmtr_cs(void)
-{
-	pcmcia_unregister_driver(&ibmtr_cs_driver);
-}
-
-module_init(init_ibmtr_cs);
-module_exit(exit_ibmtr_cs);
diff --git a/drivers/net/tokenring/lanstreamer.c b/drivers/net/tokenring/lanstreamer.c
deleted file mode 100644
index 3e4b4f09111..00000000000
--- a/drivers/net/tokenring/lanstreamer.c
+++ /dev/null
@@ -1,1917 +0,0 @@
-/*
- *   lanstreamer.c -- driver for the IBM Auto LANStreamer PCI Adapter
- *
- *  Written By: Mike Sullivan, IBM Corporation
- *
- *  Copyright (C) 1999 IBM Corporation
- *
- *  Linux driver for IBM PCI tokenring cards based on the LanStreamer MPC
- *  chipset. 
- *
- *  This driver is based on the olympic driver for IBM PCI TokenRing cards (Pit/Pit-Phy/Olympic
- *  chipsets) written  by:
- *      1999 Peter De Schrijver All Rights Reserved
- *	1999 Mike Phillips (phillim@amtrak.com)
- *
- *  Base Driver Skeleton:
- *      Written 1993-94 by Donald Becker.
- *
- *      Copyright 1993 United States Government as represented by the
- *      Director, National Security Agency.
- *
- * 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.                              
- *                                                                           
- * NO WARRANTY                                                               
- * THE PROGRAM IS PROVIDED ON AN "AS IS" BASIS, WITHOUT WARRANTIES OR        
- * CONDITIONS OF ANY KIND, EITHER EXPRESS OR IMPLIED INCLUDING, WITHOUT      
- * LIMITATION, ANY WARRANTIES OR CONDITIONS OF TITLE, NON-INFRINGEMENT,      
- * MERCHANTABILITY OR FITNESS FOR A PARTICULAR PURPOSE. Each Recipient is    
- * solely responsible for determining the appropriateness of using and       
- * distributing the Program and assumes all risks associated with its        
- * exercise of rights under this Agreement, including but not limited to     
- * the risks and costs of program errors, damage to or loss of data,         
- * programs or equipment, and unavailability or interruption of operations.  
- *                                                                           
- * DISCLAIMER OF LIABILITY                                                   
- * NEITHER RECIPIENT NOR ANY CONTRIBUTORS SHALL HAVE ANY LIABILITY FOR ANY   
- * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL        
- * DAMAGES (INCLUDING WITHOUT LIMITATION LOST PROFITS), 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 OR DISTRIBUTION OF THE PROGRAM OR THE EXERCISE OF ANY RIGHTS GRANTED  
- * HEREUNDER, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGES             
- *                                                                           
- * 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 
- *                                                                           
- * 
- *  12/10/99 - Alpha Release 0.1.0
- *            First release to the public
- *  03/03/00 - Merged to kernel, indented -kr -i8 -bri0, fixed some missing
- *		malloc free checks, reviewed code. <alan@redhat.com>
- *  03/13/00 - Added spinlocks for smp
- *  03/08/01 - Added support for module_init() and module_exit()
- *  08/15/01 - Added ioctl() functionality for debugging, changed netif_*_queue
- *             calls and other incorrectness - Kent Yoder <yoder1@us.ibm.com>
- *  11/05/01 - Restructured the interrupt function, added delays, reduced the 
- *             the number of TX descriptors to 1, which together can prevent 
- *             the card from locking up the box - <yoder1@us.ibm.com>
- *  09/27/02 - New PCI interface + bug fix. - <yoder1@us.ibm.com>
- *  11/13/02 - Removed free_irq calls which could cause a hang, added
- *	       netif_carrier_{on|off} - <yoder1@us.ibm.com>
- *  
- *  To Do:
- *
- *
- *  If Problems do Occur
- *  Most problems can be rectified by either closing and opening the interface
- *  (ifconfig down and up) or rmmod and insmod'ing the driver (a bit difficult
- *  if compiled into the kernel).
- */
-
-/* Change STREAMER_DEBUG to 1 to get verbose, and I mean really verbose, messages */
-
-#define STREAMER_DEBUG 0
-#define STREAMER_DEBUG_PACKETS 0
-
-/* Change STREAMER_NETWORK_MONITOR to receive mac frames through the arb channel.
- * Will also create a /proc/net/streamer_tr entry if proc_fs is compiled into the
- * kernel.
- * Intended to be used to create a ring-error reporting network module 
- * i.e. it will give you the source address of beaconers on the ring 
- */
-
-#define STREAMER_NETWORK_MONITOR 0
-
-/* #define CONFIG_PROC_FS */
-
-/*
- *  Allow or disallow ioctl's for debugging
- */
-
-#define STREAMER_IOCTL 0
-
-#include <linux/module.h>
-#include <linux/kernel.h>
-#include <linux/errno.h>
-#include <linux/timer.h>
-#include <linux/in.h>
-#include <linux/ioport.h>
-#include <linux/string.h>
-#include <linux/proc_fs.h>
-#include <linux/ptrace.h>
-#include <linux/skbuff.h>
-#include <linux/interrupt.h>
-#include <linux/delay.h>
-#include <linux/netdevice.h>
-#include <linux/trdevice.h>
-#include <linux/stddef.h>
-#include <linux/init.h>
-#include <linux/pci.h>
-#include <linux/dma-mapping.h>
-#include <linux/spinlock.h>
-#include <linux/bitops.h>
-#include <linux/jiffies.h>
-#include <linux/slab.h>
-
-#include <net/net_namespace.h>
-#include <net/checksum.h>
-
-#include <asm/io.h>
-
-#include "lanstreamer.h"
-
-#if (BITS_PER_LONG == 64)
-#error broken on 64-bit: stores pointer to rx_ring->buffer in 32-bit int
-#endif
-
-
-/* I've got to put some intelligence into the version number so that Peter and I know
- * which version of the code somebody has got. 
- * Version Number = a.b.c.d  where a.b.c is the level of code and d is the latest author.
- * So 0.0.1.pds = Peter, 0.0.1.mlp = Mike
- * 
- * Official releases will only have an a.b.c version number format.
- */
-
-static char version[] = "LanStreamer.c v0.4.0 03/08/01 - Mike Sullivan\n"
-                        "              v0.5.3 11/13/02 - Kent Yoder";
-
-static DEFINE_PCI_DEVICE_TABLE(streamer_pci_tbl) = {
-	{ PCI_VENDOR_ID_IBM, PCI_DEVICE_ID_IBM_TR, PCI_ANY_ID, PCI_ANY_ID,},
-	{}	/* terminating entry */
-};
-MODULE_DEVICE_TABLE(pci,streamer_pci_tbl);
-
-
-static char *open_maj_error[] = {
-	"No error", "Lobe Media Test", "Physical Insertion",
-	"Address Verification", "Neighbor Notification (Ring Poll)",
-	"Request Parameters", "FDX Registration Request",
-	"FDX Lobe Media Test", "FDX Duplicate Address Check",
-	"Unknown stage"
-};
-
-static char *open_min_error[] = {
-	"No error", "Function Failure", "Signal Lost", "Wire Fault",
-	"Ring Speed Mismatch", "Timeout", "Ring Failure", "Ring Beaconing",
-	"Duplicate Node Address", "Request Parameters", "Remove Received",
-	"Reserved", "Reserved", "No Monitor Detected for RPL",
-	"Monitor Contention failer for RPL", "FDX Protocol Error"
-};
-
-/* Module parameters */
-
-/* Ring Speed 0,4,16
- * 0 = Autosense         
- * 4,16 = Selected speed only, no autosense
- * This allows the card to be the first on the ring
- * and become the active monitor.
- *
- * WARNING: Some hubs will allow you to insert
- * at the wrong speed
- */
-
-static int ringspeed[STREAMER_MAX_ADAPTERS] = { 0, };
-
-module_param_array(ringspeed, int, NULL, 0);
-
-/* Packet buffer size */
-
-static int pkt_buf_sz[STREAMER_MAX_ADAPTERS] = { 0, };
-
-module_param_array(pkt_buf_sz, int, NULL, 0);
-
-/* Message Level */
-
-static int message_level[STREAMER_MAX_ADAPTERS] = { 1, };
-
-module_param_array(message_level, int, NULL, 0);
-
-#if STREAMER_IOCTL
-static int streamer_ioctl(struct net_device *, struct ifreq *, int);
-#endif
-
-static int streamer_reset(struct net_device *dev);
-static int streamer_open(struct net_device *dev);
-static netdev_tx_t streamer_xmit(struct sk_buff *skb,
-				       struct net_device *dev);
-static int streamer_close(struct net_device *dev);
-static void streamer_set_rx_mode(struct net_device *dev);
-static irqreturn_t streamer_interrupt(int irq, void *dev_id);
-static int streamer_set_mac_address(struct net_device *dev, void *addr);
-static void streamer_arb_cmd(struct net_device *dev);
-static int streamer_change_mtu(struct net_device *dev, int mtu);
-static void streamer_srb_bh(struct net_device *dev);
-static void streamer_asb_bh(struct net_device *dev);
-#if STREAMER_NETWORK_MONITOR
-#ifdef CONFIG_PROC_FS
-static int streamer_proc_info(char *buffer, char **start, off_t offset,
-			      int length, int *eof, void *data);
-static int sprintf_info(char *buffer, struct net_device *dev);
-struct streamer_private *dev_streamer=NULL;
-#endif
-#endif
-
-static const struct net_device_ops streamer_netdev_ops = {
-	.ndo_open		= streamer_open,
-	.ndo_stop		= streamer_close,
-	.ndo_start_xmit		= streamer_xmit,
-	.ndo_change_mtu		= streamer_change_mtu,
-#if STREAMER_IOCTL
-	.ndo_do_ioctl		= streamer_ioctl,
-#endif
-	.ndo_set_rx_mode	= streamer_set_rx_mode,
-	.ndo_set_mac_address	= streamer_set_mac_address,
-};
-
-static int __devinit streamer_init_one(struct pci_dev *pdev,
-				       const struct pci_device_id *ent)
-{
-	struct net_device *dev;
-	struct streamer_private *streamer_priv;
-	unsigned long pio_start, pio_end, pio_flags, pio_len;
-	unsigned long mmio_start, mmio_end, mmio_flags, mmio_len;
-	int rc = 0;
-	static int card_no=-1;
-	u16 pcr;
-
-#if STREAMER_DEBUG
-	printk("lanstreamer::streamer_init_one, entry pdev %p\n",pdev);
-#endif
-
-	card_no++;
-	dev = alloc_trdev(sizeof(*streamer_priv));
-	if (dev==NULL) {
-		printk(KERN_ERR "lanstreamer: out of memory.\n");
-		return -ENOMEM;
-	}
-
-	streamer_priv = netdev_priv(dev);
-
-#if STREAMER_NETWORK_MONITOR
-#ifdef CONFIG_PROC_FS
-	if (!dev_streamer)
-		create_proc_read_entry("streamer_tr", 0, init_net.proc_net,
-					streamer_proc_info, NULL); 
-	streamer_priv->next = dev_streamer;
-	dev_streamer = streamer_priv;
-#endif
-#endif
-
-	rc = pci_set_dma_mask(pdev, DMA_BIT_MASK(32));
-	if (rc) {
-		printk(KERN_ERR "%s: No suitable PCI mapping available.\n",
-				dev->name);
-		rc = -ENODEV;
-		goto err_out;
-	}
-
-	rc = pci_enable_device(pdev);
-	if (rc) {
-		printk(KERN_ERR "lanstreamer: unable to enable pci device\n");
-		rc=-EIO;
-		goto err_out;
-	}
-
-	pci_set_master(pdev);
-
-	rc = pci_set_mwi(pdev);
-	if (rc) {
-		printk(KERN_ERR "lanstreamer: unable to enable MWI on pci device\n");
-		goto err_out_disable;
-	}
-
-	pio_start = pci_resource_start(pdev, 0);
-	pio_end = pci_resource_end(pdev, 0);
-	pio_flags = pci_resource_flags(pdev, 0);
-	pio_len = pci_resource_len(pdev, 0);
-
-	mmio_start = pci_resource_start(pdev, 1);
-	mmio_end = pci_resource_end(pdev, 1);
-	mmio_flags = pci_resource_flags(pdev, 1);
-	mmio_len = pci_resource_len(pdev, 1);
-
-#if STREAMER_DEBUG
-	printk("lanstreamer: pio_start %x pio_end %x pio_len %x pio_flags %x\n",
-		pio_start, pio_end, pio_len, pio_flags);
-	printk("lanstreamer: mmio_start %x mmio_end %x mmio_len %x mmio_flags %x\n",
-		mmio_start, mmio_end, mmio_flags, mmio_len);
-#endif
-
-	if (!request_region(pio_start, pio_len, "lanstreamer")) {
-		printk(KERN_ERR "lanstreamer: unable to get pci io addr %lx\n",
-			pio_start);
-		rc= -EBUSY;
-		goto err_out_mwi;
-	}
-
-	if (!request_mem_region(mmio_start, mmio_len, "lanstreamer")) {
-		printk(KERN_ERR "lanstreamer: unable to get pci mmio addr %lx\n",
-			mmio_start);
-		rc= -EBUSY;
-		goto err_out_free_pio;
-	}
-
-	streamer_priv->streamer_mmio=ioremap(mmio_start, mmio_len);
-	if (streamer_priv->streamer_mmio == NULL) {
-		printk(KERN_ERR "lanstreamer: unable to remap MMIO %lx\n",
-			mmio_start);
-		rc= -EIO;
-		goto err_out_free_mmio;
-	}
-
-	init_waitqueue_head(&streamer_priv->srb_wait);
-	init_waitqueue_head(&streamer_priv->trb_wait);
-
-	dev->netdev_ops = &streamer_netdev_ops;
-	dev->irq = pdev->irq;
-	dev->base_addr=pio_start;
-	SET_NETDEV_DEV(dev, &pdev->dev);
-
-	streamer_priv->streamer_card_name = (char *)pdev->resource[0].name;
-	streamer_priv->pci_dev = pdev;
-
-	if ((pkt_buf_sz[card_no] < 100) || (pkt_buf_sz[card_no] > 18000))
-		streamer_priv->pkt_buf_sz = PKT_BUF_SZ;
-	else
-		streamer_priv->pkt_buf_sz = pkt_buf_sz[card_no];
-
-	streamer_priv->streamer_ring_speed = ringspeed[card_no];
-	streamer_priv->streamer_message_level = message_level[card_no];
-
-	pci_set_drvdata(pdev, dev);
-
-	spin_lock_init(&streamer_priv->streamer_lock);
-
-	pci_read_config_word (pdev, PCI_COMMAND, &pcr);
-	pcr |= PCI_COMMAND_SERR;
-	pci_write_config_word (pdev, PCI_COMMAND, pcr);
-
-	printk("%s\n", version);
-	printk("%s: %s. I/O at %hx, MMIO at %p, using irq %d\n",dev->name,
-		streamer_priv->streamer_card_name,
-		(unsigned int) dev->base_addr,
-		streamer_priv->streamer_mmio, 
-		dev->irq);
-
-	if (streamer_reset(dev))
-		goto err_out_unmap;
-
-	rc = register_netdev(dev);
-	if (rc)
-		goto err_out_unmap;
-	return 0;
-
-err_out_unmap:
-	iounmap(streamer_priv->streamer_mmio);
-err_out_free_mmio:
-	release_mem_region(mmio_start, mmio_len);
-err_out_free_pio:
-	release_region(pio_start, pio_len);
-err_out_mwi:
-	pci_clear_mwi(pdev);
-err_out_disable:
-	pci_disable_device(pdev);
-err_out:
-	free_netdev(dev);
-#if STREAMER_DEBUG
-	printk("lanstreamer: Exit error %x\n",rc);
-#endif
-	return rc;
-}
-
-static void __devexit streamer_remove_one(struct pci_dev *pdev)
-{
-	struct net_device *dev=pci_get_drvdata(pdev);
-	struct streamer_private *streamer_priv;
-
-#if STREAMER_DEBUG
-	printk("lanstreamer::streamer_remove_one entry pdev %p\n",pdev);
-#endif
-
-	if (dev == NULL) {
-		printk(KERN_ERR "lanstreamer::streamer_remove_one, ERROR dev is NULL\n");
-		return;
-	}
-
-	streamer_priv=netdev_priv(dev);
-	if (streamer_priv == NULL) {
-		printk(KERN_ERR "lanstreamer::streamer_remove_one, ERROR dev->priv is NULL\n");
-		return;
-	}
-
-#if STREAMER_NETWORK_MONITOR
-#ifdef CONFIG_PROC_FS
-	{
-		struct streamer_private **p, **next;
-
-		for (p = &dev_streamer; *p; p = next) {
-			next = &(*p)->next;
-			if (*p == streamer_priv) {
-				*p = *next;
-				break;
-			}
-		}
-		if (!dev_streamer)
-			remove_proc_entry("streamer_tr", init_net.proc_net);
-	}
-#endif
-#endif
-
-	unregister_netdev(dev);
-	iounmap(streamer_priv->streamer_mmio);
-	release_mem_region(pci_resource_start(pdev, 1), pci_resource_len(pdev,1));
-	release_region(pci_resource_start(pdev, 0), pci_resource_len(pdev,0));
-	pci_clear_mwi(pdev);
-	pci_disable_device(pdev);
-	free_netdev(dev);
-	pci_set_drvdata(pdev, NULL);
-}
-
-
-static int streamer_reset(struct net_device *dev)
-{
-	struct streamer_private *streamer_priv;
-	__u8 __iomem *streamer_mmio;
-	unsigned long t;
-	unsigned int uaa_addr;
-	struct sk_buff *skb = NULL;
-	__u16 misr;
-
-	streamer_priv = netdev_priv(dev);
-	streamer_mmio = streamer_priv->streamer_mmio;
-
-	writew(readw(streamer_mmio + BCTL) | BCTL_SOFTRESET, streamer_mmio + BCTL);
-	t = jiffies;
-	/* Hold soft reset bit for a while */
-	ssleep(1);
-	
-	writew(readw(streamer_mmio + BCTL) & ~BCTL_SOFTRESET,
-	       streamer_mmio + BCTL);
-
-#if STREAMER_DEBUG
-	printk("BCTL: %x\n", readw(streamer_mmio + BCTL));
-	printk("GPR: %x\n", readw(streamer_mmio + GPR));
-	printk("SISRMASK: %x\n", readw(streamer_mmio + SISR_MASK));
-#endif
-	writew(readw(streamer_mmio + BCTL) | (BCTL_RX_FIFO_8 | BCTL_TX_FIFO_8), streamer_mmio + BCTL );
-
-	if (streamer_priv->streamer_ring_speed == 0) {	/* Autosense */
-		writew(readw(streamer_mmio + GPR) | GPR_AUTOSENSE,
-		       streamer_mmio + GPR);
-		if (streamer_priv->streamer_message_level)
-			printk(KERN_INFO "%s: Ringspeed autosense mode on\n",
-			       dev->name);
-	} else if (streamer_priv->streamer_ring_speed == 16) {
-		if (streamer_priv->streamer_message_level)
-			printk(KERN_INFO "%s: Trying to open at 16 Mbps as requested\n",
-			       dev->name);
-		writew(GPR_16MBPS, streamer_mmio + GPR);
-	} else if (streamer_priv->streamer_ring_speed == 4) {
-		if (streamer_priv->streamer_message_level)
-			printk(KERN_INFO "%s: Trying to open at 4 Mbps as requested\n",
-			       dev->name);
-		writew(0, streamer_mmio + GPR);
-	}
-
-	skb = dev_alloc_skb(streamer_priv->pkt_buf_sz);
-	if (!skb) {
-		printk(KERN_INFO "%s: skb allocation for diagnostics failed...proceeding\n",
-		       dev->name);
-	} else {
-	        struct streamer_rx_desc *rx_ring;
-                u8 *data;
-
-		rx_ring=(struct streamer_rx_desc *)skb->data;
-		data=((u8 *)skb->data)+sizeof(struct streamer_rx_desc);
-		rx_ring->forward=0;
-		rx_ring->status=0;
-		rx_ring->buffer=cpu_to_le32(pci_map_single(streamer_priv->pci_dev, data, 
-							512, PCI_DMA_FROMDEVICE));
-		rx_ring->framelen_buflen=512; 
-		writel(cpu_to_le32(pci_map_single(streamer_priv->pci_dev, rx_ring, 512, PCI_DMA_FROMDEVICE)),
-			streamer_mmio+RXBDA);
-	}
-
-#if STREAMER_DEBUG
-	printk("GPR = %x\n", readw(streamer_mmio + GPR));
-#endif
-	/* start solo init */
-	writew(SISR_MI, streamer_mmio + SISR_MASK_SUM);
-
-	while (!((readw(streamer_mmio + SISR)) & SISR_SRB_REPLY)) {
-		msleep_interruptible(100);
-		if (time_after(jiffies, t + 40 * HZ)) {
-			printk(KERN_ERR
-			       "IBM PCI tokenring card not responding\n");
-			release_region(dev->base_addr, STREAMER_IO_SPACE);
-			if (skb)
-				dev_kfree_skb(skb);
-			return -1;
-		}
-	}
-	writew(~SISR_SRB_REPLY, streamer_mmio + SISR_RUM);
-	misr = readw(streamer_mmio + MISR_RUM);
-	writew(~misr, streamer_mmio + MISR_RUM);
-
-	if (skb)
-		dev_kfree_skb(skb);	/* release skb used for diagnostics */
-
-#if STREAMER_DEBUG
-	printk("LAPWWO: %x, LAPA: %x LAPE:  %x\n",
-	       readw(streamer_mmio + LAPWWO), readw(streamer_mmio + LAPA),
-	       readw(streamer_mmio + LAPE));
-#endif
-
-#if STREAMER_DEBUG
-	{
-		int i;
-		writew(readw(streamer_mmio + LAPWWO),
-		       streamer_mmio + LAPA);
-		printk("initialization response srb dump: ");
-		for (i = 0; i < 10; i++)
-			printk("%x:",
-			       ntohs(readw(streamer_mmio + LAPDINC)));
-		printk("\n");
-	}
-#endif
-
-	writew(readw(streamer_mmio + LAPWWO) + 6, streamer_mmio + LAPA);
-	if (readw(streamer_mmio + LAPD)) {
-		printk(KERN_INFO "tokenring card initialization failed. errorcode : %x\n",
-		       ntohs(readw(streamer_mmio + LAPD)));
-		release_region(dev->base_addr, STREAMER_IO_SPACE);
-		return -1;
-	}
-
-	writew(readw(streamer_mmio + LAPWWO) + 8, streamer_mmio + LAPA);
-	uaa_addr = ntohs(readw(streamer_mmio + LAPDINC));
-	readw(streamer_mmio + LAPDINC);	/* skip over Level.Addr field */
-	streamer_priv->streamer_addr_table_addr = ntohs(readw(streamer_mmio + LAPDINC));
-	streamer_priv->streamer_parms_addr = ntohs(readw(streamer_mmio + LAPDINC));
-
-#if STREAMER_DEBUG
-	printk("UAA resides at %x\n", uaa_addr);
-#endif
-
-	/* setup uaa area for access with LAPD */
-	{
-		int i;
-		__u16 addr;
-		writew(uaa_addr, streamer_mmio + LAPA);
-		for (i = 0; i < 6; i += 2) {
-		        addr=ntohs(readw(streamer_mmio+LAPDINC));
-			dev->dev_addr[i]= (addr >> 8) & 0xff;
-			dev->dev_addr[i+1]= addr & 0xff;
-		}
-#if STREAMER_DEBUG
-		printk("Adapter address: %pM\n", dev->dev_addr);
-#endif
-	}
-	return 0;
-}
-
-static int streamer_open(struct net_device *dev)
-{
-	struct streamer_private *streamer_priv = netdev_priv(dev);
-	__u8 __iomem *streamer_mmio = streamer_priv->streamer_mmio;
-	unsigned long flags;
-	char open_error[255];
-	int i, open_finished = 1;
-	__u16 srb_word;
-	__u16 srb_open;
-	int rc;
-
-	if (readw(streamer_mmio+BMCTL_SUM) & BMCTL_RX_ENABLED) {
-	        rc=streamer_reset(dev);
-	}
-
-	if (request_irq(dev->irq, streamer_interrupt, IRQF_SHARED, "lanstreamer", dev)) {
-		return -EAGAIN;
-	}
-#if STREAMER_DEBUG
-	printk("BMCTL: %x\n", readw(streamer_mmio + BMCTL_SUM));
-	printk("pending ints: %x\n", readw(streamer_mmio + SISR));
-#endif
-
-	writew(SISR_MI | SISR_SRB_REPLY, streamer_mmio + SISR_MASK);	/* more ints later, doesn't stop arb cmd interrupt */
-	writew(LISR_LIE, streamer_mmio + LISR);	/* more ints later */
-
-	/* adapter is closed, so SRB is pointed to by LAPWWO */
-	writew(readw(streamer_mmio + LAPWWO), streamer_mmio + LAPA);
-
-#if STREAMER_DEBUG
-	printk("LAPWWO: %x, LAPA: %x\n", readw(streamer_mmio + LAPWWO),
-	       readw(streamer_mmio + LAPA));
-	printk("LAPE: %x\n", readw(streamer_mmio + LAPE));
-	printk("SISR Mask = %04x\n", readw(streamer_mmio + SISR_MASK));
-#endif
-	do {
-		for (i = 0; i < SRB_COMMAND_SIZE; i += 2) {
-			writew(0, streamer_mmio + LAPDINC);
-		}
-
-		writew(readw(streamer_mmio+LAPWWO),streamer_mmio+LAPA);
-		writew(htons(SRB_OPEN_ADAPTER<<8),streamer_mmio+LAPDINC) ; 	/* open */
-		writew(htons(STREAMER_CLEAR_RET_CODE<<8),streamer_mmio+LAPDINC);
-		writew(STREAMER_CLEAR_RET_CODE, streamer_mmio + LAPDINC);
-
-		writew(readw(streamer_mmio + LAPWWO) + 8, streamer_mmio + LAPA);
-#if STREAMER_NETWORK_MONITOR
-		/* If Network Monitor, instruct card to copy MAC frames through the ARB */
-		writew(htons(OPEN_ADAPTER_ENABLE_FDX | OPEN_ADAPTER_PASS_ADC_MAC | OPEN_ADAPTER_PASS_ATT_MAC | OPEN_ADAPTER_PASS_BEACON), streamer_mmio + LAPDINC);	/* offset 8 word contains open options */
-#else
-		writew(htons(OPEN_ADAPTER_ENABLE_FDX), streamer_mmio + LAPDINC);	/* Offset 8 word contains Open.Options */
-#endif
-
-		if (streamer_priv->streamer_laa[0]) {
-			writew(readw(streamer_mmio + LAPWWO) + 12, streamer_mmio + LAPA);
-			writew(htons((streamer_priv->streamer_laa[0] << 8) | 
-				     streamer_priv->streamer_laa[1]),streamer_mmio+LAPDINC);
-			writew(htons((streamer_priv->streamer_laa[2] << 8) | 
-				     streamer_priv->streamer_laa[3]),streamer_mmio+LAPDINC);
-			writew(htons((streamer_priv->streamer_laa[4] << 8) | 
-				     streamer_priv->streamer_laa[5]),streamer_mmio+LAPDINC);
-			memcpy(dev->dev_addr, streamer_priv->streamer_laa, dev->addr_len);
-		}
-
-		/* save off srb open offset */
-		srb_open = readw(streamer_mmio + LAPWWO);
-#if STREAMER_DEBUG
-		writew(readw(streamer_mmio + LAPWWO),
-		       streamer_mmio + LAPA);
-		printk("srb open request:\n");
-		for (i = 0; i < 16; i++) {
-			printk("%x:", ntohs(readw(streamer_mmio + LAPDINC)));
-		}
-		printk("\n");
-#endif
-		spin_lock_irqsave(&streamer_priv->streamer_lock, flags);
-		streamer_priv->srb_queued = 1;
-
-		/* signal solo that SRB command has been issued */
-		writew(LISR_SRB_CMD, streamer_mmio + LISR_SUM);
-		spin_unlock_irqrestore(&streamer_priv->streamer_lock, flags);
-
-		while (streamer_priv->srb_queued) {
-			interruptible_sleep_on_timeout(&streamer_priv->srb_wait, 5 * HZ);
-			if (signal_pending(current)) {
-				printk(KERN_WARNING "%s: SRB timed out.\n", dev->name);
-				printk(KERN_WARNING "SISR=%x MISR=%x, LISR=%x\n",
-				       readw(streamer_mmio + SISR),
-				       readw(streamer_mmio + MISR_RUM),
-				       readw(streamer_mmio + LISR));
-				streamer_priv->srb_queued = 0;
-				break;
-			}
-		}
-
-#if STREAMER_DEBUG
-		printk("SISR_MASK: %x\n", readw(streamer_mmio + SISR_MASK));
-		printk("srb open response:\n");
-		writew(srb_open, streamer_mmio + LAPA);
-		for (i = 0; i < 10; i++) {
-			printk("%x:",
-			       ntohs(readw(streamer_mmio + LAPDINC)));
-		}
-#endif
-
-		/* If we get the same return response as we set, the interrupt wasn't raised and the open
-		 * timed out.
-		 */
-		writew(srb_open + 2, streamer_mmio + LAPA);
-		srb_word = ntohs(readw(streamer_mmio + LAPD)) >> 8;
-		if (srb_word == STREAMER_CLEAR_RET_CODE) {
-			printk(KERN_WARNING "%s: Adapter Open time out or error.\n",
-			       dev->name);
-			return -EIO;
-		}
-
-		if (srb_word != 0) {
-			if (srb_word == 0x07) {
-				if (!streamer_priv->streamer_ring_speed && open_finished) {	/* Autosense , first time around */
-					printk(KERN_WARNING "%s: Retrying at different ring speed\n",
-					       dev->name);
-					open_finished = 0;
-				} else {
-					__u16 error_code;
-
-					writew(srb_open + 6, streamer_mmio + LAPA);
-					error_code = ntohs(readw(streamer_mmio + LAPD));
-					strcpy(open_error, open_maj_error[(error_code & 0xf0) >> 4]);
-					strcat(open_error, " - ");
-					strcat(open_error, open_min_error[(error_code & 0x0f)]);
-
-					if (!streamer_priv->streamer_ring_speed &&
-					    ((error_code & 0x0f) == 0x0d))
-					{
-						printk(KERN_WARNING "%s: Tried to autosense ring speed with no monitors present\n", dev->name);
-						printk(KERN_WARNING "%s: Please try again with a specified ring speed\n", dev->name);
-						free_irq(dev->irq, dev);
-						return -EIO;
-					}
-
-					printk(KERN_WARNING "%s: %s\n",
-					       dev->name, open_error);
-					free_irq(dev->irq, dev);
-					return -EIO;
-
-				}	/* if autosense && open_finished */
-			} else {
-				printk(KERN_WARNING "%s: Bad OPEN response: %x\n",
-				       dev->name, srb_word);
-				free_irq(dev->irq, dev);
-				return -EIO;
-			}
-		} else
-			open_finished = 1;
-	} while (!(open_finished));	/* Will only loop if ring speed mismatch re-open attempted && autosense is on */
-
-	writew(srb_open + 18, streamer_mmio + LAPA);
-	srb_word=ntohs(readw(streamer_mmio+LAPD)) >> 8;
-	if (srb_word & (1 << 3))
-		if (streamer_priv->streamer_message_level)
-			printk(KERN_INFO "%s: Opened in FDX Mode\n", dev->name);
-
-	if (srb_word & 1)
-		streamer_priv->streamer_ring_speed = 16;
-	else
-		streamer_priv->streamer_ring_speed = 4;
-
-	if (streamer_priv->streamer_message_level)
-		printk(KERN_INFO "%s: Opened in %d Mbps mode\n", 
-			dev->name,
-			streamer_priv->streamer_ring_speed);
-
-	writew(srb_open + 8, streamer_mmio + LAPA);
-	streamer_priv->asb = ntohs(readw(streamer_mmio + LAPDINC));
-	streamer_priv->srb = ntohs(readw(streamer_mmio + LAPDINC));
-	streamer_priv->arb = ntohs(readw(streamer_mmio + LAPDINC));
-	readw(streamer_mmio + LAPDINC);	/* offset 14 word is rsvd */
-	streamer_priv->trb = ntohs(readw(streamer_mmio + LAPDINC));
-
-	streamer_priv->streamer_receive_options = 0x00;
-	streamer_priv->streamer_copy_all_options = 0;
-
-	/* setup rx ring */
-	/* enable rx channel */
-	writew(~BMCTL_RX_DIS, streamer_mmio + BMCTL_RUM);
-
-	/* setup rx descriptors */
-	streamer_priv->streamer_rx_ring=
-	    kmalloc( sizeof(struct streamer_rx_desc)*
-		     STREAMER_RX_RING_SIZE,GFP_KERNEL);
-	if (!streamer_priv->streamer_rx_ring) {
-	    printk(KERN_WARNING "%s ALLOC of streamer rx ring FAILED!!\n",dev->name);
-	    return -EIO;
-	}
-
-	for (i = 0; i < STREAMER_RX_RING_SIZE; i++) {
-		struct sk_buff *skb;
-
-		skb = dev_alloc_skb(streamer_priv->pkt_buf_sz);
-		if (skb == NULL)
-			break;
-
-		skb->dev = dev;
-
-		streamer_priv->streamer_rx_ring[i].forward = 
-			cpu_to_le32(pci_map_single(streamer_priv->pci_dev, &streamer_priv->streamer_rx_ring[i + 1],
-					sizeof(struct streamer_rx_desc), PCI_DMA_FROMDEVICE));
-		streamer_priv->streamer_rx_ring[i].status = 0;
-		streamer_priv->streamer_rx_ring[i].buffer = 
-			cpu_to_le32(pci_map_single(streamer_priv->pci_dev, skb->data,
-					      streamer_priv->pkt_buf_sz, PCI_DMA_FROMDEVICE));
-		streamer_priv->streamer_rx_ring[i].framelen_buflen = streamer_priv->pkt_buf_sz;
-		streamer_priv->rx_ring_skb[i] = skb;
-	}
-	streamer_priv->streamer_rx_ring[STREAMER_RX_RING_SIZE - 1].forward =
-				cpu_to_le32(pci_map_single(streamer_priv->pci_dev, &streamer_priv->streamer_rx_ring[0],
-						sizeof(struct streamer_rx_desc), PCI_DMA_FROMDEVICE));
-
-	if (i == 0) {
-		printk(KERN_WARNING "%s: Not enough memory to allocate rx buffers. Adapter disabled\n", dev->name);
-		free_irq(dev->irq, dev);
-		return -EIO;
-	}
-
-	streamer_priv->rx_ring_last_received = STREAMER_RX_RING_SIZE - 1;	/* last processed rx status */
-
-	writel(cpu_to_le32(pci_map_single(streamer_priv->pci_dev, &streamer_priv->streamer_rx_ring[0],
-				sizeof(struct streamer_rx_desc), PCI_DMA_TODEVICE)), 
-		streamer_mmio + RXBDA);
-	writel(cpu_to_le32(pci_map_single(streamer_priv->pci_dev, &streamer_priv->streamer_rx_ring[STREAMER_RX_RING_SIZE - 1],
-				sizeof(struct streamer_rx_desc), PCI_DMA_TODEVICE)), 
-		streamer_mmio + RXLBDA);
-
-	/* set bus master interrupt event mask */
-	writew(MISR_RX_NOBUF | MISR_RX_EOF, streamer_mmio + MISR_MASK);
-
-
-	/* setup tx ring */
-	streamer_priv->streamer_tx_ring=kmalloc(sizeof(struct streamer_tx_desc)*
-						STREAMER_TX_RING_SIZE,GFP_KERNEL);
-	if (!streamer_priv->streamer_tx_ring) {
-	    printk(KERN_WARNING "%s ALLOC of streamer_tx_ring FAILED\n",dev->name);
-	    return -EIO;
-	}
-
-	writew(~BMCTL_TX2_DIS, streamer_mmio + BMCTL_RUM);	/* Enables TX channel 2 */
-	for (i = 0; i < STREAMER_TX_RING_SIZE; i++) {
-		streamer_priv->streamer_tx_ring[i].forward = cpu_to_le32(pci_map_single(streamer_priv->pci_dev, 
-										&streamer_priv->streamer_tx_ring[i + 1],
-										sizeof(struct streamer_tx_desc),
-										PCI_DMA_TODEVICE));
-		streamer_priv->streamer_tx_ring[i].status = 0;
-		streamer_priv->streamer_tx_ring[i].bufcnt_framelen = 0;
-		streamer_priv->streamer_tx_ring[i].buffer = 0;
-		streamer_priv->streamer_tx_ring[i].buflen = 0;
-		streamer_priv->streamer_tx_ring[i].rsvd1 = 0;
-		streamer_priv->streamer_tx_ring[i].rsvd2 = 0;
-		streamer_priv->streamer_tx_ring[i].rsvd3 = 0;
-	}
-	streamer_priv->streamer_tx_ring[STREAMER_TX_RING_SIZE - 1].forward =
-					cpu_to_le32(pci_map_single(streamer_priv->pci_dev, &streamer_priv->streamer_tx_ring[0],
-							sizeof(struct streamer_tx_desc), PCI_DMA_TODEVICE));
-
-	streamer_priv->free_tx_ring_entries = STREAMER_TX_RING_SIZE;
-	streamer_priv->tx_ring_free = 0;	/* next entry in tx ring to use */
-	streamer_priv->tx_ring_last_status = STREAMER_TX_RING_SIZE - 1;
-
-	/* set Busmaster interrupt event mask (handle receives on interrupt only */
-	writew(MISR_TX2_EOF | MISR_RX_NOBUF | MISR_RX_EOF, streamer_mmio + MISR_MASK);
-	/* set system event interrupt mask */
-	writew(SISR_ADAPTER_CHECK | SISR_ARB_CMD | SISR_TRB_REPLY | SISR_ASB_FREE, streamer_mmio + SISR_MASK_SUM);
-
-#if STREAMER_DEBUG
-	printk("BMCTL: %x\n", readw(streamer_mmio + BMCTL_SUM));
-	printk("SISR MASK: %x\n", readw(streamer_mmio + SISR_MASK));
-#endif
-
-#if STREAMER_NETWORK_MONITOR
-
-	writew(streamer_priv->streamer_addr_table_addr, streamer_mmio + LAPA);
-	printk("%s: Node Address: %04x:%04x:%04x\n", dev->name,
-		ntohs(readw(streamer_mmio + LAPDINC)),
-		ntohs(readw(streamer_mmio + LAPDINC)),
-		ntohs(readw(streamer_mmio + LAPDINC)));
-	readw(streamer_mmio + LAPDINC);
-	readw(streamer_mmio + LAPDINC);
-	printk("%s: Functional Address: %04x:%04x\n", dev->name,
-		ntohs(readw(streamer_mmio + LAPDINC)),
-		ntohs(readw(streamer_mmio + LAPDINC)));
-
-	writew(streamer_priv->streamer_parms_addr + 4,
-		streamer_mmio + LAPA);
-	printk("%s: NAUN Address: %04x:%04x:%04x\n", dev->name,
-		ntohs(readw(streamer_mmio + LAPDINC)),
-		ntohs(readw(streamer_mmio + LAPDINC)),
-		ntohs(readw(streamer_mmio + LAPDINC)));
-#endif
-
-	netif_start_queue(dev);
-	netif_carrier_on(dev);
-	return 0;
-}
-
-/*
- *	When we enter the rx routine we do not know how many frames have been 
- *	queued on the rx channel.  Therefore we start at the next rx status
- *	position and travel around the receive ring until we have completed
- *	all the frames.
- *
- *	This means that we may process the frame before we receive the end
- *	of frame interrupt. This is why we always test the status instead
- *	of blindly processing the next frame.
- *	
- */
-static void streamer_rx(struct net_device *dev)
-{
-	struct streamer_private *streamer_priv =
-	    netdev_priv(dev);
-	__u8 __iomem *streamer_mmio = streamer_priv->streamer_mmio;
-	struct streamer_rx_desc *rx_desc;
-	int rx_ring_last_received, length, frame_length, buffer_cnt = 0;
-	struct sk_buff *skb, *skb2;
-
-	/* setup the next rx descriptor to be received */
-	rx_desc = &streamer_priv->streamer_rx_ring[(streamer_priv->rx_ring_last_received + 1) & (STREAMER_RX_RING_SIZE - 1)];
-	rx_ring_last_received = streamer_priv->rx_ring_last_received;
-
-	while (rx_desc->status & 0x01000000) {	/* While processed descriptors are available */
-		if (rx_ring_last_received != streamer_priv->rx_ring_last_received) 
-		{
-			printk(KERN_WARNING "RX Error 1 rx_ring_last_received not the same %x %x\n",
-				rx_ring_last_received, streamer_priv->rx_ring_last_received);
-		}
-		streamer_priv->rx_ring_last_received = (streamer_priv->rx_ring_last_received + 1) & (STREAMER_RX_RING_SIZE - 1);
-		rx_ring_last_received = streamer_priv->rx_ring_last_received;
-
-		length = rx_desc->framelen_buflen & 0xffff;	/* buffer length */
-		frame_length = (rx_desc->framelen_buflen >> 16) & 0xffff;
-
-		if (rx_desc->status & 0x7E830000) {	/* errors */
-			if (streamer_priv->streamer_message_level) {
-				printk(KERN_WARNING "%s: Rx Error %x\n",
-				       dev->name, rx_desc->status);
-			}
-		} else {	/* received without errors */
-			if (rx_desc->status & 0x80000000) {	/* frame complete */
-				buffer_cnt = 1;
-				skb = dev_alloc_skb(streamer_priv->pkt_buf_sz);
-			} else {
-				skb = dev_alloc_skb(frame_length);
-			}
-
-			if (skb == NULL) 
-			{
-				printk(KERN_WARNING "%s: Not enough memory to copy packet to upper layers.\n",	dev->name);
-				dev->stats.rx_dropped++;
-			} else {	/* we allocated an skb OK */
-				if (buffer_cnt == 1) {
-					/* release the DMA mapping */
-					pci_unmap_single(streamer_priv->pci_dev, 
-						le32_to_cpu(streamer_priv->streamer_rx_ring[rx_ring_last_received].buffer),
-						streamer_priv->pkt_buf_sz, 
-						PCI_DMA_FROMDEVICE);
-					skb2 = streamer_priv->rx_ring_skb[rx_ring_last_received];
-#if STREAMER_DEBUG_PACKETS
-					{
-						int i;
-						printk("streamer_rx packet print: skb->data2 %p  skb->head %p\n", skb2->data, skb2->head);
-						for (i = 0; i < frame_length; i++) 
-						{
-							printk("%x:", skb2->data[i]);
-							if (((i + 1) % 16) == 0)
-								printk("\n");
-						}
-						printk("\n");
-					}
-#endif
-					skb_put(skb2, length);
-					skb2->protocol = tr_type_trans(skb2, dev);
-					/* recycle this descriptor */
-					streamer_priv->streamer_rx_ring[rx_ring_last_received].status = 0;
-					streamer_priv->streamer_rx_ring[rx_ring_last_received].framelen_buflen = streamer_priv->pkt_buf_sz;
-					streamer_priv->streamer_rx_ring[rx_ring_last_received].buffer = 
-						cpu_to_le32(pci_map_single(streamer_priv->pci_dev, skb->data, streamer_priv->pkt_buf_sz,
-								PCI_DMA_FROMDEVICE));
-					streamer_priv->rx_ring_skb[rx_ring_last_received] = skb;
-					/* place recycled descriptor back on the adapter */
-					writel(cpu_to_le32(pci_map_single(streamer_priv->pci_dev, 
-									&streamer_priv->streamer_rx_ring[rx_ring_last_received],
-									sizeof(struct streamer_rx_desc), PCI_DMA_FROMDEVICE)),
-						streamer_mmio + RXLBDA);
-					/* pass the received skb up to the protocol */
-					netif_rx(skb2);
-				} else {
-					do {	/* Walk the buffers */
-						pci_unmap_single(streamer_priv->pci_dev, le32_to_cpu(rx_desc->buffer), length, PCI_DMA_FROMDEVICE), 
-						memcpy(skb_put(skb, length), (void *)rx_desc->buffer, length);	/* copy this fragment */
-						streamer_priv->streamer_rx_ring[rx_ring_last_received].status = 0;
-						streamer_priv->streamer_rx_ring[rx_ring_last_received].framelen_buflen = streamer_priv->pkt_buf_sz;
-						
-						/* give descriptor back to the adapter */
-						writel(cpu_to_le32(pci_map_single(streamer_priv->pci_dev, 
-									&streamer_priv->streamer_rx_ring[rx_ring_last_received],
-									length, PCI_DMA_FROMDEVICE)), 
-							streamer_mmio + RXLBDA);
-
-						if (rx_desc->status & 0x80000000)
-							break;	/* this descriptor completes the frame */
-
-						/* else get the next pending descriptor */
-						if (rx_ring_last_received!= streamer_priv->rx_ring_last_received)
-						{
-							printk("RX Error rx_ring_last_received not the same %x %x\n",
-								rx_ring_last_received,
-								streamer_priv->rx_ring_last_received);
-						}
-						rx_desc = &streamer_priv->streamer_rx_ring[(streamer_priv->rx_ring_last_received+1) & (STREAMER_RX_RING_SIZE-1)];
-
-						length = rx_desc->framelen_buflen & 0xffff;	/* buffer length */
-						streamer_priv->rx_ring_last_received =	(streamer_priv->rx_ring_last_received+1) & (STREAMER_RX_RING_SIZE - 1);
-						rx_ring_last_received = streamer_priv->rx_ring_last_received;
-					} while (1);
-
-					skb->protocol = tr_type_trans(skb, dev);
-					/* send up to the protocol */
-					netif_rx(skb);
-				}
-				dev->stats.rx_packets++;
-				dev->stats.rx_bytes += length;
-			}	/* if skb == null */
-		}		/* end received without errors */
-
-		/* try the next one */
-		rx_desc = &streamer_priv->streamer_rx_ring[(rx_ring_last_received + 1) & (STREAMER_RX_RING_SIZE - 1)];
-	}			/* end for all completed rx descriptors */
-}
-
-static irqreturn_t streamer_interrupt(int irq, void *dev_id)
-{
-	struct net_device *dev = (struct net_device *) dev_id;
-	struct streamer_private *streamer_priv =
-	    netdev_priv(dev);
-	__u8 __iomem *streamer_mmio = streamer_priv->streamer_mmio;
-	__u16 sisr;
-	__u16 misr;
-	u8 max_intr = MAX_INTR;
-
-	spin_lock(&streamer_priv->streamer_lock);
-	sisr = readw(streamer_mmio + SISR);
-
-	while((sisr & (SISR_MI | SISR_SRB_REPLY | SISR_ADAPTER_CHECK | SISR_ASB_FREE | 
-		       SISR_ARB_CMD | SISR_TRB_REPLY | SISR_PAR_ERR | SISR_SERR_ERR)) &&
-	      (max_intr > 0)) {
-
-		if(sisr & SISR_PAR_ERR) {
-			writew(~SISR_PAR_ERR, streamer_mmio + SISR_RUM);
-			(void)readw(streamer_mmio + SISR_RUM);
-		}
-
-		else if(sisr & SISR_SERR_ERR) {
-			writew(~SISR_SERR_ERR, streamer_mmio + SISR_RUM);
-			(void)readw(streamer_mmio + SISR_RUM);
-		}
-
-		else if(sisr & SISR_MI) {
-			misr = readw(streamer_mmio + MISR_RUM);
-
-		if (misr & MISR_TX2_EOF) {
-				while(streamer_priv->streamer_tx_ring[(streamer_priv->tx_ring_last_status + 1) & (STREAMER_TX_RING_SIZE - 1)].status) {
-				streamer_priv->tx_ring_last_status = (streamer_priv->tx_ring_last_status + 1) & (STREAMER_TX_RING_SIZE - 1);
-				streamer_priv->free_tx_ring_entries++;
-				dev->stats.tx_bytes += streamer_priv->tx_ring_skb[streamer_priv->tx_ring_last_status]->len;
-				dev->stats.tx_packets++;
-				dev_kfree_skb_irq(streamer_priv->tx_ring_skb[streamer_priv->tx_ring_last_status]);
-				streamer_priv->streamer_tx_ring[streamer_priv->tx_ring_last_status].buffer = 0xdeadbeef;
-				streamer_priv->streamer_tx_ring[streamer_priv->tx_ring_last_status].status = 0;
-				streamer_priv->streamer_tx_ring[streamer_priv->tx_ring_last_status].bufcnt_framelen = 0;
-				streamer_priv->streamer_tx_ring[streamer_priv->tx_ring_last_status].buflen = 0;
-				streamer_priv->streamer_tx_ring[streamer_priv->tx_ring_last_status].rsvd1 = 0;
-				streamer_priv->streamer_tx_ring[streamer_priv->tx_ring_last_status].rsvd2 = 0;
-				streamer_priv->streamer_tx_ring[streamer_priv->tx_ring_last_status].rsvd3 = 0;
-			}
-			netif_wake_queue(dev);
-		}
-
-		if (misr & MISR_RX_EOF) {
-			streamer_rx(dev);
-		}
-		/* MISR_RX_EOF */
-
-			if (misr & MISR_RX_NOBUF) {
-				/* According to the documentation, we don't have to do anything,  
-                                 * but trapping it keeps it out of /var/log/messages.  
-                                 */
-			}		/* SISR_RX_NOBUF */
-
-			writew(~misr, streamer_mmio + MISR_RUM);
-			(void)readw(streamer_mmio + MISR_RUM);
-		}
-
-		else if (sisr & SISR_SRB_REPLY) {
-			if (streamer_priv->srb_queued == 1) {
-				wake_up_interruptible(&streamer_priv->srb_wait);
-			} else if (streamer_priv->srb_queued == 2) {
-				streamer_srb_bh(dev);
-			}
-			streamer_priv->srb_queued = 0;
-
-			writew(~SISR_SRB_REPLY, streamer_mmio + SISR_RUM);
-			(void)readw(streamer_mmio + SISR_RUM);
-		}
-
-		else if (sisr & SISR_ADAPTER_CHECK) {
-			printk(KERN_WARNING "%s: Adapter Check Interrupt Raised, 8 bytes of information follow:\n", dev->name);
-			writel(readl(streamer_mmio + LAPWWO), streamer_mmio + LAPA);
-			printk(KERN_WARNING "%s: Words %x:%x:%x:%x:\n",
-			       dev->name, readw(streamer_mmio + LAPDINC),
-			       ntohs(readw(streamer_mmio + LAPDINC)),
-			       ntohs(readw(streamer_mmio + LAPDINC)),
-			       ntohs(readw(streamer_mmio + LAPDINC)));
-			netif_stop_queue(dev);
-			netif_carrier_off(dev);
-			printk(KERN_WARNING "%s: Adapter must be manually reset.\n", dev->name);
-		}
-
-		/* SISR_ADAPTER_CHECK */
-		else if (sisr & SISR_ASB_FREE) {
-			/* Wake up anything that is waiting for the asb response */
-			if (streamer_priv->asb_queued) {
-				streamer_asb_bh(dev);
-			}
-			writew(~SISR_ASB_FREE, streamer_mmio + SISR_RUM);
-			(void)readw(streamer_mmio + SISR_RUM);
-		}
-		/* SISR_ASB_FREE */
-		else if (sisr & SISR_ARB_CMD) {
-			streamer_arb_cmd(dev);
-			writew(~SISR_ARB_CMD, streamer_mmio + SISR_RUM);
-			(void)readw(streamer_mmio + SISR_RUM);
-		}
-		/* SISR_ARB_CMD */
-		else if (sisr & SISR_TRB_REPLY) {
-			/* Wake up anything that is waiting for the trb response */
-			if (streamer_priv->trb_queued) {
-				wake_up_interruptible(&streamer_priv->
-						      trb_wait);
-			}
-			streamer_priv->trb_queued = 0;
-			writew(~SISR_TRB_REPLY, streamer_mmio + SISR_RUM);
-			(void)readw(streamer_mmio + SISR_RUM);
-		}
-		/* SISR_TRB_REPLY */
-
-		sisr = readw(streamer_mmio + SISR);
-		max_intr--;
-	} /* while() */		
-
-	spin_unlock(&streamer_priv->streamer_lock) ; 
-	return IRQ_HANDLED;
-}
-
-static netdev_tx_t streamer_xmit(struct sk_buff *skb,
-				       struct net_device *dev)
-{
-	struct streamer_private *streamer_priv =
-	    netdev_priv(dev);
-	__u8 __iomem *streamer_mmio = streamer_priv->streamer_mmio;
-	unsigned long flags ;
-
-	spin_lock_irqsave(&streamer_priv->streamer_lock, flags);
-
-	if (streamer_priv->free_tx_ring_entries) {
-		streamer_priv->streamer_tx_ring[streamer_priv->tx_ring_free].status = 0;
-		streamer_priv->streamer_tx_ring[streamer_priv->tx_ring_free].bufcnt_framelen = 0x00020000 | skb->len;
-		streamer_priv->streamer_tx_ring[streamer_priv->tx_ring_free].buffer = 
-			cpu_to_le32(pci_map_single(streamer_priv->pci_dev, skb->data, skb->len, PCI_DMA_TODEVICE));
-		streamer_priv->streamer_tx_ring[streamer_priv->tx_ring_free].rsvd1 = skb->len;
-		streamer_priv->streamer_tx_ring[streamer_priv->tx_ring_free].rsvd2 = 0;
-		streamer_priv->streamer_tx_ring[streamer_priv->tx_ring_free].rsvd3 = 0;
-		streamer_priv->streamer_tx_ring[streamer_priv->tx_ring_free].buflen = skb->len;
-
-		streamer_priv->tx_ring_skb[streamer_priv->tx_ring_free] = skb;
-		streamer_priv->free_tx_ring_entries--;
-#if STREAMER_DEBUG_PACKETS
-		{
-			int i;
-			printk("streamer_xmit packet print:\n");
-			for (i = 0; i < skb->len; i++) {
-				printk("%x:", skb->data[i]);
-				if (((i + 1) % 16) == 0)
-					printk("\n");
-			}
-			printk("\n");
-		}
-#endif
-
-		writel(cpu_to_le32(pci_map_single(streamer_priv->pci_dev, 
-					&streamer_priv->streamer_tx_ring[streamer_priv->tx_ring_free],
-					sizeof(struct streamer_tx_desc), PCI_DMA_TODEVICE)),
-			streamer_mmio + TX2LFDA);
-		(void)readl(streamer_mmio + TX2LFDA);
-
-		streamer_priv->tx_ring_free = (streamer_priv->tx_ring_free + 1) & (STREAMER_TX_RING_SIZE - 1);
-		spin_unlock_irqrestore(&streamer_priv->streamer_lock,flags);
-		return NETDEV_TX_OK;
-	} else {
-	        netif_stop_queue(dev);
-	        spin_unlock_irqrestore(&streamer_priv->streamer_lock,flags);
-		return NETDEV_TX_BUSY;
-	}
-}
-
-
-static int streamer_close(struct net_device *dev)
-{
-	struct streamer_private *streamer_priv =
-	    netdev_priv(dev);
-	__u8 __iomem *streamer_mmio = streamer_priv->streamer_mmio;
-	unsigned long flags;
-	int i;
-
-	netif_stop_queue(dev);
-	netif_carrier_off(dev);
-	writew(streamer_priv->srb, streamer_mmio + LAPA);
-	writew(htons(SRB_CLOSE_ADAPTER << 8),streamer_mmio+LAPDINC);
-	writew(htons(STREAMER_CLEAR_RET_CODE << 8), streamer_mmio+LAPDINC);
-
-	spin_lock_irqsave(&streamer_priv->streamer_lock, flags);
-
-	streamer_priv->srb_queued = 1;
-	writew(LISR_SRB_CMD, streamer_mmio + LISR_SUM);
-
-	spin_unlock_irqrestore(&streamer_priv->streamer_lock, flags);
-
-	while (streamer_priv->srb_queued) 
-	{
-		interruptible_sleep_on_timeout(&streamer_priv->srb_wait,
-					       jiffies + 60 * HZ);
-		if (signal_pending(current)) 
-		{
-			printk(KERN_WARNING "%s: SRB timed out.\n", dev->name);
-			printk(KERN_WARNING "SISR=%x MISR=%x LISR=%x\n",
-			       readw(streamer_mmio + SISR),
-			       readw(streamer_mmio + MISR_RUM),
-			       readw(streamer_mmio + LISR));
-			streamer_priv->srb_queued = 0;
-			break;
-		}
-	}
-
-	streamer_priv->rx_ring_last_received = (streamer_priv->rx_ring_last_received + 1) & (STREAMER_RX_RING_SIZE - 1);
-
-	for (i = 0; i < STREAMER_RX_RING_SIZE; i++) {
-	        if (streamer_priv->rx_ring_skb[streamer_priv->rx_ring_last_received]) {
-		        dev_kfree_skb(streamer_priv->rx_ring_skb[streamer_priv->rx_ring_last_received]);
-		} 
-		streamer_priv->rx_ring_last_received = (streamer_priv->rx_ring_last_received + 1) & (STREAMER_RX_RING_SIZE - 1);
-	}
-
-	/* reset tx/rx fifo's and busmaster logic */
-
-	/* TBD. Add graceful way to reset the LLC channel without doing a soft reset. 
-	   writel(readl(streamer_mmio+BCTL)|(3<<13),streamer_mmio+BCTL);
-	   udelay(1);
-	   writel(readl(streamer_mmio+BCTL)&~(3<<13),streamer_mmio+BCTL);
-	 */
-
-#if STREAMER_DEBUG
-	writew(streamer_priv->srb, streamer_mmio + LAPA);
-	printk("srb): ");
-	for (i = 0; i < 2; i++) {
-		printk("%x ", ntohs(readw(streamer_mmio + LAPDINC)));
-	}
-	printk("\n");
-#endif
-	free_irq(dev->irq, dev);
-	return 0;
-}
-
-static void streamer_set_rx_mode(struct net_device *dev)
-{
-	struct streamer_private *streamer_priv =
-	    netdev_priv(dev);
-	__u8 __iomem *streamer_mmio = streamer_priv->streamer_mmio;
-	__u8 options = 0;
-	struct netdev_hw_addr *ha;
-	unsigned char dev_mc_address[5];
-
-	writel(streamer_priv->srb, streamer_mmio + LAPA);
-	options = streamer_priv->streamer_copy_all_options;
-
-	if (dev->flags & IFF_PROMISC)
-		options |= (3 << 5);	/* All LLC and MAC frames, all through the main rx channel */
-	else
-		options &= ~(3 << 5);
-
-	/* Only issue the srb if there is a change in options */
-
-	if ((options ^ streamer_priv->streamer_copy_all_options)) 
-	{
-		/* Now to issue the srb command to alter the copy.all.options */
-		writew(htons(SRB_MODIFY_RECEIVE_OPTIONS << 8), streamer_mmio+LAPDINC);
-		writew(htons(STREAMER_CLEAR_RET_CODE << 8), streamer_mmio+LAPDINC);
-		writew(htons((streamer_priv->streamer_receive_options << 8) | options),streamer_mmio+LAPDINC);
-		writew(htons(0x4a41),streamer_mmio+LAPDINC);
-		writew(htons(0x4d45),streamer_mmio+LAPDINC);
-		writew(htons(0x5320),streamer_mmio+LAPDINC);
-		writew(0x2020, streamer_mmio + LAPDINC);
-
-		streamer_priv->srb_queued = 2;	/* Can't sleep, use srb_bh */
-
-		writel(LISR_SRB_CMD, streamer_mmio + LISR_SUM);
-
-		streamer_priv->streamer_copy_all_options = options;
-		return;
-	}
-
-	/* Set the functional addresses we need for multicast */
-	writel(streamer_priv->srb,streamer_mmio+LAPA);
-	dev_mc_address[0] = dev_mc_address[1] = dev_mc_address[2] = dev_mc_address[3] = 0 ; 
-  
-	netdev_for_each_mc_addr(ha, dev) {
-		dev_mc_address[0] |= ha->addr[2];
-		dev_mc_address[1] |= ha->addr[3];
-		dev_mc_address[2] |= ha->addr[4];
-		dev_mc_address[3] |= ha->addr[5];
-	}
-  
-	writew(htons(SRB_SET_FUNC_ADDRESS << 8),streamer_mmio+LAPDINC);
-	writew(htons(STREAMER_CLEAR_RET_CODE << 8), streamer_mmio+LAPDINC);
-	writew(0,streamer_mmio+LAPDINC);
-	writew(htons( (dev_mc_address[0] << 8) | dev_mc_address[1]),streamer_mmio+LAPDINC);
-	writew(htons( (dev_mc_address[2] << 8) | dev_mc_address[3]),streamer_mmio+LAPDINC);
-	streamer_priv->srb_queued = 2 ; 
-	writel(LISR_SRB_CMD,streamer_mmio+LISR_SUM);
-}
-
-static void streamer_srb_bh(struct net_device *dev)
-{
-	struct streamer_private *streamer_priv = netdev_priv(dev);
-	__u8 __iomem *streamer_mmio = streamer_priv->streamer_mmio;
-	__u16 srb_word;
-
-	writew(streamer_priv->srb, streamer_mmio + LAPA);
-	srb_word=ntohs(readw(streamer_mmio+LAPDINC)) >> 8;
-
-	switch (srb_word) {
-
-		/* SRB_MODIFY_RECEIVE_OPTIONS i.e. set_multicast_list options (promiscuous) 
-		 * At some point we should do something if we get an error, such as
-		 * resetting the IFF_PROMISC flag in dev
-		 */
-
-	case SRB_MODIFY_RECEIVE_OPTIONS:
-	        srb_word=ntohs(readw(streamer_mmio+LAPDINC)) >> 8;
-
-		switch (srb_word) {
-		case 0x01:
-			printk(KERN_WARNING "%s: Unrecognized srb command\n", dev->name);
-			break;
-		case 0x04:
-			printk(KERN_WARNING "%s: Adapter must be open for this operation, doh!!\n", dev->name);
-			break;
-		default:
-			if (streamer_priv->streamer_message_level)
-				printk(KERN_WARNING "%s: Receive Options Modified to %x,%x\n",
-				       dev->name,
-				       streamer_priv->streamer_copy_all_options,
-				       streamer_priv->streamer_receive_options);
-			break;
-		}		/* switch srb[2] */
-		break;
-
-
-		/* SRB_SET_GROUP_ADDRESS - Multicast group setting 
-		 */
-	case SRB_SET_GROUP_ADDRESS:
-	        srb_word=ntohs(readw(streamer_mmio+LAPDINC)) >> 8;
-		switch (srb_word) {
-		case 0x00:
-		        break;
-		case 0x01:
-			printk(KERN_WARNING "%s: Unrecognized srb command\n",dev->name);
-			break;
-		case 0x04:
-			printk(KERN_WARNING "%s: Adapter must be open for this operation, doh!!\n", dev->name);
-			break;
-		case 0x3c:
-			printk(KERN_WARNING "%s: Group/Functional address indicator bits not set correctly\n", dev->name);
-			break;
-		case 0x3e:	/* If we ever implement individual multicast addresses, will need to deal with this */
-			printk(KERN_WARNING "%s: Group address registers full\n", dev->name);
-			break;
-		case 0x55:
-			printk(KERN_INFO "%s: Group Address already set.\n", dev->name);
-			break;
-		default:
-			break;
-		}		/* switch srb[2] */
-		break;
-
-
-		/* SRB_RESET_GROUP_ADDRESS - Remove a multicast address from group list
-		 */
-	case SRB_RESET_GROUP_ADDRESS:
-	        srb_word=ntohs(readw(streamer_mmio+LAPDINC)) >> 8;
-		switch (srb_word) {
-		case 0x00:
-		        break;
-		case 0x01:
-			printk(KERN_WARNING "%s: Unrecognized srb command\n", dev->name);
-			break;
-		case 0x04:
-			printk(KERN_WARNING "%s: Adapter must be open for this operation, doh!!\n", dev->name);
-			break;
-		case 0x39:	/* Must deal with this if individual multicast addresses used */
-			printk(KERN_INFO "%s: Group address not found\n", dev->name);
-			break;
-		default:
-			break;
-		}		/* switch srb[2] */
-		break;
-
-
-		/* SRB_SET_FUNC_ADDRESS - Called by the set_rx_mode 
-		 */
-
-	case SRB_SET_FUNC_ADDRESS:
-	        srb_word=ntohs(readw(streamer_mmio+LAPDINC)) >> 8;
-		switch (srb_word) {
-		case 0x00:
-			if (streamer_priv->streamer_message_level)
-				printk(KERN_INFO "%s: Functional Address Mask Set\n", dev->name);
-			break;
-		case 0x01:
-			printk(KERN_WARNING "%s: Unrecognized srb command\n", dev->name);
-			break;
-		case 0x04:
-			printk(KERN_WARNING "%s: Adapter must be open for this operation, doh!!\n", dev->name);
-			break;
-		default:
-			break;
-		}		/* switch srb[2] */
-		break;
-
-		/* SRB_READ_LOG - Read and reset the adapter error counters
-		 */
-
-	case SRB_READ_LOG:
-	        srb_word=ntohs(readw(streamer_mmio+LAPDINC)) >> 8;
-		switch (srb_word) {
-		case 0x00:
-			{
-				int i;
-				if (streamer_priv->streamer_message_level)
-					printk(KERN_INFO "%s: Read Log command complete\n", dev->name);
-				printk("Read Log statistics: ");
-				writew(streamer_priv->srb + 6,
-				       streamer_mmio + LAPA);
-				for (i = 0; i < 5; i++) {
-					printk("%x:", ntohs(readw(streamer_mmio + LAPDINC)));
-				}
-				printk("\n");
-			}
-			break;
-		case 0x01:
-			printk(KERN_WARNING "%s: Unrecognized srb command\n", dev->name);
-			break;
-		case 0x04:
-			printk(KERN_WARNING "%s: Adapter must be open for this operation, doh!!\n", dev->name);
-			break;
-
-		}		/* switch srb[2] */
-		break;
-
-		/* SRB_READ_SR_COUNTERS - Read and reset the source routing bridge related counters */
-
-	case SRB_READ_SR_COUNTERS:
-	        srb_word=ntohs(readw(streamer_mmio+LAPDINC)) >> 8;
-		switch (srb_word) {
-		case 0x00:
-			if (streamer_priv->streamer_message_level)
-				printk(KERN_INFO "%s: Read Source Routing Counters issued\n", dev->name);
-			break;
-		case 0x01:
-			printk(KERN_WARNING "%s: Unrecognized srb command\n", dev->name);
-			break;
-		case 0x04:
-			printk(KERN_WARNING "%s: Adapter must be open for this operation, doh!!\n", dev->name);
-			break;
-		default:
-			break;
-		}		/* switch srb[2] */
-		break;
-
-	default:
-		printk(KERN_WARNING "%s: Unrecognized srb bh return value.\n", dev->name);
-		break;
-	}			/* switch srb[0] */
-}
-
-static int streamer_set_mac_address(struct net_device *dev, void *addr)
-{
-	struct sockaddr *saddr = addr;
-	struct streamer_private *streamer_priv = netdev_priv(dev);
-
-	if (netif_running(dev)) 
-	{
-		printk(KERN_WARNING "%s: Cannot set mac/laa address while card is open\n", dev->name);
-		return -EIO;
-	}
-
-	memcpy(streamer_priv->streamer_laa, saddr->sa_data, dev->addr_len);
-
-	if (streamer_priv->streamer_message_level) {
-		printk(KERN_INFO "%s: MAC/LAA Set to  = %x.%x.%x.%x.%x.%x\n",
-		       dev->name, streamer_priv->streamer_laa[0],
-		       streamer_priv->streamer_laa[1],
-		       streamer_priv->streamer_laa[2],
-		       streamer_priv->streamer_laa[3],
-		       streamer_priv->streamer_laa[4],
-		       streamer_priv->streamer_laa[5]);
-	}
-	return 0;
-}
-
-static void streamer_arb_cmd(struct net_device *dev)
-{
-	struct streamer_private *streamer_priv =
-	    netdev_priv(dev);
-	__u8 __iomem *streamer_mmio = streamer_priv->streamer_mmio;
-	__u8 header_len;
-	__u16 frame_len, buffer_len;
-	struct sk_buff *mac_frame;
-	__u8 frame_data[256];
-	__u16 buff_off;
-	__u16 lan_status = 0, lan_status_diff;	/* Initialize to stop compiler warning */
-	__u8 fdx_prot_error;
-	__u16 next_ptr;
-	__u16 arb_word;
-
-#if STREAMER_NETWORK_MONITOR
-	struct trh_hdr *mac_hdr;
-#endif
-
-	writew(streamer_priv->arb, streamer_mmio + LAPA);
-	arb_word=ntohs(readw(streamer_mmio+LAPD)) >> 8;
-	
-	if (arb_word == ARB_RECEIVE_DATA) {	/* Receive.data, MAC frames */
-		writew(streamer_priv->arb + 6, streamer_mmio + LAPA);
-		streamer_priv->mac_rx_buffer = buff_off = ntohs(readw(streamer_mmio + LAPDINC));
-		header_len=ntohs(readw(streamer_mmio+LAPDINC)) >> 8; /* 802.5 Token-Ring Header Length */
-		frame_len = ntohs(readw(streamer_mmio + LAPDINC));
-
-#if STREAMER_DEBUG
-		{
-			int i;
-			__u16 next;
-			__u8 status;
-			__u16 len;
-
-			writew(ntohs(buff_off), streamer_mmio + LAPA);	/*setup window to frame data */
-			next = htons(readw(streamer_mmio + LAPDINC));
-			status =
-			    ntohs(readw(streamer_mmio + LAPDINC)) & 0xff;
-			len = ntohs(readw(streamer_mmio + LAPDINC));
-
-			/* print out 1st 14 bytes of frame data */
-			for (i = 0; i < 7; i++) {
-				printk("Loc %d = %04x\n", i,
-				       ntohs(readw
-					     (streamer_mmio + LAPDINC)));
-			}
-
-			printk("next %04x, fs %02x, len %04x\n", next,
-			       status, len);
-		}
-#endif
-		if (!(mac_frame = dev_alloc_skb(frame_len))) {
-			printk(KERN_WARNING "%s: Memory squeeze, dropping frame.\n",
-			       dev->name);
-			goto drop_frame;
-		}
-		/* Walk the buffer chain, creating the frame */
-
-		do {
-			int i;
-			__u16 rx_word;
-
-			writew(htons(buff_off), streamer_mmio + LAPA);	/* setup window to frame data */
-			next_ptr = ntohs(readw(streamer_mmio + LAPDINC));
-			readw(streamer_mmio + LAPDINC);	/* read thru status word */
-			buffer_len = ntohs(readw(streamer_mmio + LAPDINC));
-
-			if (buffer_len > 256)
-				break;
-
-			i = 0;
-			while (i < buffer_len) {
-				rx_word=ntohs(readw(streamer_mmio+LAPDINC));
-				frame_data[i]=rx_word >> 8;
-				frame_data[i+1]=rx_word & 0xff;
-				i += 2;
-			}
-
-			memcpy(skb_put(mac_frame, buffer_len),
-				      frame_data, buffer_len);
-		} while (next_ptr && (buff_off = next_ptr));
-
-		mac_frame->protocol = tr_type_trans(mac_frame, dev);
-#if STREAMER_NETWORK_MONITOR
-		printk(KERN_WARNING "%s: Received MAC Frame, details:\n",
-		       dev->name);
-		mac_hdr = tr_hdr(mac_frame);
-		printk(KERN_WARNING
-		       "%s: MAC Frame Dest. Addr: %pM\n",
-		       dev->name, mac_hdr->daddr);
-		printk(KERN_WARNING
-		       "%s: MAC Frame Srce. Addr: %pM\n",
-		       dev->name, mac_hdr->saddr);
-#endif
-		netif_rx(mac_frame);
-
-		/* Now tell the card we have dealt with the received frame */
-drop_frame:
-		/* Set LISR Bit 1 */
-		writel(LISR_ARB_FREE, streamer_priv->streamer_mmio + LISR_SUM);
-
-		/* Is the ASB free ? */
-
-		if (!(readl(streamer_priv->streamer_mmio + SISR) & SISR_ASB_FREE)) 
-		{
-			streamer_priv->asb_queued = 1;
-			writel(LISR_ASB_FREE_REQ, streamer_priv->streamer_mmio + LISR_SUM);
-			return;
-			/* Drop out and wait for the bottom half to be run */
-		}
-
-
-		writew(streamer_priv->asb, streamer_mmio + LAPA);
-		writew(htons(ASB_RECEIVE_DATA << 8), streamer_mmio+LAPDINC);
-		writew(htons(STREAMER_CLEAR_RET_CODE << 8), streamer_mmio+LAPDINC);
-		writew(0, streamer_mmio + LAPDINC);
-		writew(htons(streamer_priv->mac_rx_buffer), streamer_mmio + LAPD);
-
-		writel(LISR_ASB_REPLY | LISR_ASB_FREE_REQ, streamer_priv->streamer_mmio + LISR_SUM);
-
-		streamer_priv->asb_queued = 2;
-		return;
-
-	} else if (arb_word == ARB_LAN_CHANGE_STATUS) {	/* Lan.change.status */
-		writew(streamer_priv->arb + 6, streamer_mmio + LAPA);
-		lan_status = ntohs(readw(streamer_mmio + LAPDINC));
-		fdx_prot_error = ntohs(readw(streamer_mmio+LAPD)) >> 8;
-		
-		/* Issue ARB Free */
-		writew(LISR_ARB_FREE, streamer_priv->streamer_mmio + LISR_SUM);
-
-		lan_status_diff = (streamer_priv->streamer_lan_status ^ lan_status) & 
-		    lan_status; 
-
-		if (lan_status_diff & (LSC_LWF | LSC_ARW | LSC_FPE | LSC_RR)) 
-		{
-			if (lan_status_diff & LSC_LWF)
-				printk(KERN_WARNING "%s: Short circuit detected on the lobe\n", dev->name);
-			if (lan_status_diff & LSC_ARW)
-				printk(KERN_WARNING "%s: Auto removal error\n", dev->name);
-			if (lan_status_diff & LSC_FPE)
-				printk(KERN_WARNING "%s: FDX Protocol Error\n", dev->name);
-			if (lan_status_diff & LSC_RR)
-				printk(KERN_WARNING "%s: Force remove MAC frame received\n", dev->name);
-
-			/* Adapter has been closed by the hardware */
-
-			/* reset tx/rx fifo's and busmaster logic */
-
-			/* @TBD. no llc reset on autostreamer writel(readl(streamer_mmio+BCTL)|(3<<13),streamer_mmio+BCTL);
-			   udelay(1);
-			   writel(readl(streamer_mmio+BCTL)&~(3<<13),streamer_mmio+BCTL); */
-
-			netif_stop_queue(dev);
-			netif_carrier_off(dev);
-			printk(KERN_WARNING "%s: Adapter must be manually reset.\n", dev->name);
-		}
-		/* If serious error */
-		if (streamer_priv->streamer_message_level) {
-			if (lan_status_diff & LSC_SIG_LOSS)
-				printk(KERN_WARNING "%s: No receive signal detected\n", dev->name);
-			if (lan_status_diff & LSC_HARD_ERR) 
-				printk(KERN_INFO "%s: Beaconing\n", dev->name);
-			if (lan_status_diff & LSC_SOFT_ERR)
-				printk(KERN_WARNING "%s: Adapter transmitted Soft Error Report Mac Frame\n", dev->name);
-			if (lan_status_diff & LSC_TRAN_BCN)
-				printk(KERN_INFO "%s: We are transmitting the beacon, aaah\n", dev->name);
-			if (lan_status_diff & LSC_SS)
-				printk(KERN_INFO "%s: Single Station on the ring\n", dev->name);
-			if (lan_status_diff & LSC_RING_REC)
-				printk(KERN_INFO "%s: Ring recovery ongoing\n", dev->name);
-			if (lan_status_diff & LSC_FDX_MODE)
-				printk(KERN_INFO "%s: Operating in FDX mode\n", dev->name);
-		}
-
-		if (lan_status_diff & LSC_CO) {
-			if (streamer_priv->streamer_message_level)
-				printk(KERN_INFO "%s: Counter Overflow\n", dev->name);
-
-			/* Issue READ.LOG command */
-
-			writew(streamer_priv->srb, streamer_mmio + LAPA);
-			writew(htons(SRB_READ_LOG << 8),streamer_mmio+LAPDINC);
-			writew(htons(STREAMER_CLEAR_RET_CODE << 8), streamer_mmio+LAPDINC);
-			writew(0, streamer_mmio + LAPDINC);
-			streamer_priv->srb_queued = 2;	/* Can't sleep, use srb_bh */
-
-			writew(LISR_SRB_CMD, streamer_mmio + LISR_SUM);
-		}
-
-		if (lan_status_diff & LSC_SR_CO) {
-			if (streamer_priv->streamer_message_level)
-				printk(KERN_INFO "%s: Source routing counters overflow\n", dev->name);
-
-			/* Issue a READ.SR.COUNTERS */
-			writew(streamer_priv->srb, streamer_mmio + LAPA);
-			writew(htons(SRB_READ_SR_COUNTERS << 8),
-			       streamer_mmio+LAPDINC);
-			writew(htons(STREAMER_CLEAR_RET_CODE << 8),
-			       streamer_mmio+LAPDINC);
-			streamer_priv->srb_queued = 2;	/* Can't sleep, use srb_bh */
-			writew(LISR_SRB_CMD, streamer_mmio + LISR_SUM);
-
-		}
-		streamer_priv->streamer_lan_status = lan_status;
-	} /* Lan.change.status */
-	else
-		printk(KERN_WARNING "%s: Unknown arb command\n", dev->name);
-}
-
-static void streamer_asb_bh(struct net_device *dev)
-{
-	struct streamer_private *streamer_priv =
-	    netdev_priv(dev);
-	__u8 __iomem *streamer_mmio = streamer_priv->streamer_mmio;
-
-	if (streamer_priv->asb_queued == 1) 
-	{
-		/* Dropped through the first time */
-
-		writew(streamer_priv->asb, streamer_mmio + LAPA);
-		writew(htons(ASB_RECEIVE_DATA << 8),streamer_mmio+LAPDINC);
-		writew(htons(STREAMER_CLEAR_RET_CODE << 8), streamer_mmio+LAPDINC);
-		writew(0, streamer_mmio + LAPDINC);
-		writew(htons(streamer_priv->mac_rx_buffer), streamer_mmio + LAPD);
-
-		writel(LISR_ASB_REPLY | LISR_ASB_FREE_REQ, streamer_priv->streamer_mmio + LISR_SUM);
-		streamer_priv->asb_queued = 2;
-
-		return;
-	}
-
-	if (streamer_priv->asb_queued == 2) {
-		__u8 rc;
-		writew(streamer_priv->asb + 2, streamer_mmio + LAPA);
-		rc=ntohs(readw(streamer_mmio+LAPD)) >> 8;
-		switch (rc) {
-		case 0x01:
-			printk(KERN_WARNING "%s: Unrecognized command code\n", dev->name);
-			break;
-		case 0x26:
-			printk(KERN_WARNING "%s: Unrecognized buffer address\n", dev->name);
-			break;
-		case 0xFF:
-			/* Valid response, everything should be ok again */
-			break;
-		default:
-			printk(KERN_WARNING "%s: Invalid return code in asb\n", dev->name);
-			break;
-		}
-	}
-	streamer_priv->asb_queued = 0;
-}
-
-static int streamer_change_mtu(struct net_device *dev, int mtu)
-{
-	struct streamer_private *streamer_priv =
-	    netdev_priv(dev);
-	__u16 max_mtu;
-
-	if (streamer_priv->streamer_ring_speed == 4)
-		max_mtu = 4500;
-	else
-		max_mtu = 18000;
-
-	if (mtu > max_mtu)
-		return -EINVAL;
-	if (mtu < 100)
-		return -EINVAL;
-
-	dev->mtu = mtu;
-	streamer_priv->pkt_buf_sz = mtu + TR_HLEN;
-
-	return 0;
-}
-
-#if STREAMER_NETWORK_MONITOR
-#ifdef CONFIG_PROC_FS
-static int streamer_proc_info(char *buffer, char **start, off_t offset,
-			      int length, int *eof, void *data)
-{
-  struct streamer_private *sdev=NULL;
-	struct pci_dev *pci_device = NULL;
-	int len = 0;
-	off_t begin = 0;
-	off_t pos = 0;
-	int size;
-
-  struct net_device *dev;
-
-	size = sprintf(buffer, "IBM LanStreamer/MPC Chipset Token Ring Adapters\n");
-
-	pos += size;
-	len += size;
-
-  for(sdev=dev_streamer; sdev; sdev=sdev->next) {
-    pci_device=sdev->pci_dev;
-    dev=pci_get_drvdata(pci_device);
-
-				size = sprintf_info(buffer + len, dev);
-				len += size;
-				pos = begin + len;
-
-				if (pos < offset) {
-					len = 0;
-					begin = pos;
-				}
-				if (pos > offset + length)
-					break;
-		}		/* for */
-
-	*start = buffer + (offset - begin);	/* Start of wanted data */
-	len -= (offset - begin);	/* Start slop */
-	if (len > length)
-		len = length;	/* Ending slop */
-	return len;
-}
-
-static int sprintf_info(char *buffer, struct net_device *dev)
-{
-	struct streamer_private *streamer_priv =
-	    netdev_priv(dev);
-	__u8 __iomem *streamer_mmio = streamer_priv->streamer_mmio;
-	struct streamer_adapter_addr_table sat;
-	struct streamer_parameters_table spt;
-	int size = 0;
-	int i;
-
-	writew(streamer_priv->streamer_addr_table_addr, streamer_mmio + LAPA);
-	for (i = 0; i < 14; i += 2) {
-		__u16 io_word;
-		__u8 *datap = (__u8 *) & sat;
-		io_word=ntohs(readw(streamer_mmio+LAPDINC));
-		datap[size]=io_word >> 8;
-		datap[size+1]=io_word & 0xff;
-	}
-	writew(streamer_priv->streamer_parms_addr, streamer_mmio + LAPA);
-	for (i = 0; i < 68; i += 2) {
-		__u16 io_word;
-		__u8 *datap = (__u8 *) & spt;
-		io_word=ntohs(readw(streamer_mmio+LAPDINC));
-		datap[size]=io_word >> 8;
-		datap[size+1]=io_word & 0xff;
-	}
-
-	size = sprintf(buffer, "\n%6s: Adapter Address   : Node Address      : Functional Addr\n", dev->name);
-
-	size += sprintf(buffer + size,
-			"%6s: %pM : %pM : %02x:%02x:%02x:%02x\n",
-			dev->name, dev->dev_addr, sat.node_addr,
-			sat.func_addr[0], sat.func_addr[1],
-			sat.func_addr[2], sat.func_addr[3]);
-
-	size += sprintf(buffer + size, "\n%6s: Token Ring Parameters Table:\n", dev->name);
-
-	size += sprintf(buffer + size, "%6s: Physical Addr : Up Node Address   : Poll Address      : AccPri : Auth Src : Att Code :\n", dev->name);
-
-	size += sprintf(buffer + size,
-		    "%6s: %02x:%02x:%02x:%02x   : %pM : %pM : %04x   : %04x     :  %04x    :\n",
-		    dev->name, spt.phys_addr[0], spt.phys_addr[1],
-		    spt.phys_addr[2], spt.phys_addr[3],
-		    spt.up_node_addr, spt.poll_addr,
-		    ntohs(spt.acc_priority), ntohs(spt.auth_source_class),
-		    ntohs(spt.att_code));
-
-	size += sprintf(buffer + size, "%6s: Source Address    : Bcn T : Maj. V : Lan St : Lcl Rg : Mon Err : Frame Correl : \n", dev->name);
-
-	size += sprintf(buffer + size,
-		    "%6s: %pM : %04x  : %04x   : %04x   : %04x   : %04x    :     %04x     : \n",
-		    dev->name, spt.source_addr,
-		    ntohs(spt.beacon_type), ntohs(spt.major_vector),
-		    ntohs(spt.lan_status), ntohs(spt.local_ring),
-		    ntohs(spt.mon_error), ntohs(spt.frame_correl));
-
-	size += sprintf(buffer + size, "%6s: Beacon Details :  Tx  :  Rx  : NAUN Node Address : NAUN Node Phys : \n",
-		    dev->name);
-
-	size += sprintf(buffer + size,
-		    "%6s:                :  %02x  :  %02x  : %pM : %02x:%02x:%02x:%02x    : \n",
-		    dev->name, ntohs(spt.beacon_transmit),
-		    ntohs(spt.beacon_receive),
-		    spt.beacon_naun,
-		    spt.beacon_phys[0], spt.beacon_phys[1],
-		    spt.beacon_phys[2], spt.beacon_phys[3]);
-	return size;
-}
-#endif
-#endif
-
-static struct pci_driver streamer_pci_driver = {
-  .name     = "lanstreamer",
-  .id_table = streamer_pci_tbl,
-  .probe    = streamer_init_one,
-  .remove   = __devexit_p(streamer_remove_one),
-};
-
-static int __init streamer_init_module(void) {
-  return pci_register_driver(&streamer_pci_driver);
-}
-
-static void __exit streamer_cleanup_module(void) {
-  pci_unregister_driver(&streamer_pci_driver);
-}
-
-module_init(streamer_init_module);
-module_exit(streamer_cleanup_module);
-MODULE_LICENSE("GPL");
diff --git a/drivers/net/tokenring/lanstreamer.h b/drivers/net/tokenring/lanstreamer.h
deleted file mode 100644
index 3c58d6a3fbc..00000000000
--- a/drivers/net/tokenring/lanstreamer.h
+++ /dev/null
@@ -1,343 +0,0 @@
-/*
- *   lanstreamer.h -- driver for the IBM Auto LANStreamer PCI Adapter
- *
- *  Written By: Mike Sullivan, IBM Corporation
- *
- *  Copyright (C) 1999 IBM Corporation
- *
- *  Linux driver for IBM PCI tokenring cards based on the LanStreamer MPC
- *  chipset. 
- *
- *  This driver is based on the olympic driver for IBM PCI TokenRing cards (Pit/Pit-Phy/Olympic
- *  chipsets) written  by:
- *      1999 Peter De Schrijver All Rights Reserved
- *	1999 Mike Phillips (phillim@amtrak.com)
- *
- *  Base Driver Skeleton:
- *      Written 1993-94 by Donald Becker.
- *
- *      Copyright 1993 United States Government as represented by the
- *      Director, National Security Agency.
- *
- * 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.                              
- *                                                                           
- * NO WARRANTY                                                               
- * THE PROGRAM IS PROVIDED ON AN "AS IS" BASIS, WITHOUT WARRANTIES OR        
- * CONDITIONS OF ANY KIND, EITHER EXPRESS OR IMPLIED INCLUDING, WITHOUT      
- * LIMITATION, ANY WARRANTIES OR CONDITIONS OF TITLE, NON-INFRINGEMENT,      
- * MERCHANTABILITY OR FITNESS FOR A PARTICULAR PURPOSE. Each Recipient is    
- * solely responsible for determining the appropriateness of using and       
- * distributing the Program and assumes all risks associated with its        
- * exercise of rights under this Agreement, including but not limited to     
- * the risks and costs of program errors, damage to or loss of data,         
- * programs or equipment, and unavailability or interruption of operations.  
- *                                                                           
- * DISCLAIMER OF LIABILITY                                                   
- * NEITHER RECIPIENT NOR ANY CONTRIBUTORS SHALL HAVE ANY LIABILITY FOR ANY   
- * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL        
- * DAMAGES (INCLUDING WITHOUT LIMITATION LOST PROFITS), 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 OR DISTRIBUTION OF THE PROGRAM OR THE EXERCISE OF ANY RIGHTS GRANTED  
- * HEREUNDER, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGES             
- *                                                                           
- * 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 
- *                                                                           
- * 
- *  12/10/99 - Alpha Release 0.1.0
- *            First release to the public
- *  08/15/01 - Added ioctl() definitions and others - Kent Yoder <yoder1@us.ibm.com>
- *
- */
-
-/* MAX_INTR - the maximum number of times we can loop
- * inside the interrupt function before returning
- * control to the OS (maximum value is 256)
- */
-#define MAX_INTR 5
-
-#define CLS 0x0C
-#define MLR 0x86
-#define LTR 0x0D
-
-#define BCTL 0x60
-#define BCTL_SOFTRESET (1<<15)
-#define BCTL_RX_FIFO_8 (1<<1)
-#define BCTL_TX_FIFO_8 (1<<3)
-
-#define GPR 0x4a
-#define GPR_AUTOSENSE (1<<2)
-#define GPR_16MBPS (1<<3)
-
-#define LISR 0x10
-#define LISR_SUM 0x12
-#define LISR_RUM 0x14
-
-#define LISR_LIE (1<<15)
-#define LISR_SLIM (1<<13)
-#define LISR_SLI (1<<12)
-#define LISR_BPEI (1<<9)
-#define LISR_BPE (1<<8)
-#define LISR_SRB_CMD (1<<5)
-#define LISR_ASB_REPLY (1<<4)
-#define LISR_ASB_FREE_REQ (1<<2)
-#define LISR_ARB_FREE (1<<1)
-#define LISR_TRB_FRAME (1<<0)
-
-#define SISR 0x16
-#define SISR_SUM 0x18
-#define SISR_RUM 0x1A
-#define SISR_MASK 0x54
-#define SISR_MASK_SUM 0x56
-#define SISR_MASK_RUM 0x58
-
-#define SISR_MI (1<<15)
-#define SISR_SERR_ERR (1<<14)
-#define SISR_TIMER (1<<11)
-#define SISR_LAP_PAR_ERR (1<<10)
-#define SISR_LAP_ACC_ERR (1<<9)
-#define SISR_PAR_ERR (1<<8)
-#define SISR_ADAPTER_CHECK (1<<6)
-#define SISR_SRB_REPLY (1<<5)
-#define SISR_ASB_FREE (1<<4)
-#define SISR_ARB_CMD (1<<3)
-#define SISR_TRB_REPLY (1<<2)
-
-#define MISR_RUM 0x5A
-#define MISR_MASK 0x5C
-#define MISR_MASK_RUM 0x5E
-
-#define MISR_TX2_IDLE (1<<15)
-#define MISR_TX2_NO_STATUS (1<<14)
-#define MISR_TX2_HALT (1<<13)
-#define MISR_TX2_EOF (1<<12)
-#define MISR_TX1_IDLE (1<<11)
-#define MISR_TX1_NO_STATUS (1<<10)
-#define MISR_TX1_HALT (1<<9)
-#define MISR_TX1_EOF (1<<8)
-#define MISR_RX_NOBUF (1<<5)
-#define MISR_RX_EOB (1<<4)
-#define MISR_RX_NO_STATUS (1<<2)
-#define MISR_RX_HALT (1<<1)
-#define MISR_RX_EOF (1<<0)
-
-#define LAPA 0x62
-#define LAPE 0x64
-#define LAPD 0x66
-#define LAPDINC 0x68
-#define LAPWWO 0x6A
-#define LAPWWC 0x6C
-#define LAPCTL 0x6E
-
-#define TIMER 0x4E4
-
-#define BMCTL_SUM 0x50
-#define BMCTL_RUM 0x52
-#define BMCTL_TX1_DIS (1<<14)
-#define BMCTL_TX2_DIS (1<<10)
-#define BMCTL_RX_DIS (1<<6)
-#define BMCTL_RX_ENABLED  (1<<5)
-
-#define RXLBDA  0x90
-#define RXBDA   0x94
-#define RXSTAT  0x98
-#define RXDBA   0x9C
-
-#define TX1LFDA 0xA0
-#define TX1FDA  0xA4
-#define TX1STAT 0xA8
-#define TX1DBA  0xAC
-#define TX2LFDA 0xB0
-#define TX2FDA  0xB4
-#define TX2STAT 0xB8
-#define TX2DBA  0xBC
-
-#define STREAMER_IO_SPACE 256
-
-#define SRB_COMMAND_SIZE 50
-
-#define STREAMER_MAX_ADAPTERS 8	/* 0x08 __MODULE_STRING can't hand 0xnn */
-
-/* Defines for LAN STATUS CHANGE reports */
-#define LSC_SIG_LOSS 0x8000
-#define LSC_HARD_ERR 0x4000
-#define LSC_SOFT_ERR 0x2000
-#define LSC_TRAN_BCN 0x1000
-#define LSC_LWF      0x0800
-#define LSC_ARW      0x0400
-#define LSC_FPE      0x0200
-#define LSC_RR       0x0100
-#define LSC_CO       0x0080
-#define LSC_SS       0x0040
-#define LSC_RING_REC 0x0020
-#define LSC_SR_CO    0x0010
-#define LSC_FDX_MODE 0x0004
-
-/* Defines for OPEN ADAPTER command */
-
-#define OPEN_ADAPTER_EXT_WRAP (1<<15)
-#define OPEN_ADAPTER_DIS_HARDEE (1<<14)
-#define OPEN_ADAPTER_DIS_SOFTERR (1<<13)
-#define OPEN_ADAPTER_PASS_ADC_MAC (1<<12)
-#define OPEN_ADAPTER_PASS_ATT_MAC (1<<11)
-#define OPEN_ADAPTER_ENABLE_EC (1<<10)
-#define OPEN_ADAPTER_CONTENDER (1<<8)
-#define OPEN_ADAPTER_PASS_BEACON (1<<7)
-#define OPEN_ADAPTER_ENABLE_FDX (1<<6)
-#define OPEN_ADAPTER_ENABLE_RPL (1<<5)
-#define OPEN_ADAPTER_INHIBIT_ETR (1<<4)
-#define OPEN_ADAPTER_INTERNAL_WRAP (1<<3)
-
-
-/* Defines for SRB Commands */
-#define SRB_CLOSE_ADAPTER 0x04
-#define SRB_CONFIGURE_BRIDGE 0x0c
-#define SRB_CONFIGURE_HP_CHANNEL 0x13
-#define SRB_MODIFY_BRIDGE_PARMS 0x15
-#define SRB_MODIFY_OPEN_OPTIONS 0x01
-#define SRB_MODIFY_RECEIVE_OPTIONS 0x17
-#define SRB_NO_OPERATION 0x00
-#define SRB_OPEN_ADAPTER 0x03
-#define SRB_READ_LOG 0x08
-#define SRB_READ_SR_COUNTERS 0x16
-#define SRB_RESET_GROUP_ADDRESS 0x02
-#define SRB_RESET_TARGET_SEGMETN 0x14
-#define SRB_SAVE_CONFIGURATION 0x1b
-#define SRB_SET_BRIDGE_PARMS 0x09
-#define SRB_SET_FUNC_ADDRESS 0x07
-#define SRB_SET_GROUP_ADDRESS 0x06
-#define SRB_SET_TARGET_SEGMENT 0x05
-
-/* Clear return code */
-#define STREAMER_CLEAR_RET_CODE 0xfe
-
-/* ARB Commands */
-#define ARB_RECEIVE_DATA 0x81
-#define ARB_LAN_CHANGE_STATUS 0x84
-
-/* ASB Response commands */
-#define ASB_RECEIVE_DATA 0x81
-
-
-/* Streamer defaults for buffers */
-
-#define STREAMER_RX_RING_SIZE 16	/* should be a power of 2 */
-/* Setting the number of TX descriptors to 1 is a workaround for an
- * undocumented hardware problem with the lanstreamer board. Setting
- * this to something higher may slightly increase the throughput you
- * can get from the card, but at the risk of locking up the box. - 
- * <yoder1@us.ibm.com>
- */
-#define STREAMER_TX_RING_SIZE 1	/* should be a power of 2 */
-
-#define PKT_BUF_SZ 4096		/* Default packet size */
-
-/* Streamer data structures */
-
-struct streamer_tx_desc {
-	__u32 forward;
-	__u32 status;
-	__u32 bufcnt_framelen;
-	__u32 buffer;
-	__u32 buflen;
-	__u32 rsvd1;
-	__u32 rsvd2;
-	__u32 rsvd3;
-};
-
-struct streamer_rx_desc {
-	__u32 forward;
-	__u32 status;
-	__u32 buffer;
-	__u32 framelen_buflen;
-};
-
-struct mac_receive_buffer {
-	__u16 next;
-	__u8 padding;
-	__u8 frame_status;
-	__u16 buffer_length;
-	__u8 frame_data;
-};
-
-struct streamer_private {
-
-	__u16 srb;
-	__u16 trb;
-	__u16 arb;
-	__u16 asb;
-
-        struct streamer_private *next;
-        struct pci_dev *pci_dev;
-	__u8 __iomem *streamer_mmio;
-        char *streamer_card_name;
- 
-        spinlock_t streamer_lock;
-
-	volatile int srb_queued;	/* True if an SRB is still posted */
-	wait_queue_head_t srb_wait;
-
-	volatile int asb_queued;	/* True if an ASB is posted */
-
-	volatile int trb_queued;	/* True if a TRB is posted */
-	wait_queue_head_t trb_wait;
-
-	struct streamer_rx_desc *streamer_rx_ring;
-	struct streamer_tx_desc *streamer_tx_ring;
-	struct sk_buff *tx_ring_skb[STREAMER_TX_RING_SIZE],
-	    *rx_ring_skb[STREAMER_RX_RING_SIZE];
-	int tx_ring_free, tx_ring_last_status, rx_ring_last_received,
-	    free_tx_ring_entries;
-
-	__u16 streamer_lan_status;
-	__u8 streamer_ring_speed;
-	__u16 pkt_buf_sz;
-	__u8 streamer_receive_options, streamer_copy_all_options,
-	    streamer_message_level;
-	__u16 streamer_addr_table_addr, streamer_parms_addr;
-	__u16 mac_rx_buffer;
-	__u8 streamer_laa[6];
-};
-
-struct streamer_adapter_addr_table {
-
-	__u8 node_addr[6];
-	__u8 reserved[4];
-	__u8 func_addr[4];
-};
-
-struct streamer_parameters_table {
-
-	__u8 phys_addr[4];
-	__u8 up_node_addr[6];
-	__u8 up_phys_addr[4];
-	__u8 poll_addr[6];
-	__u16 reserved;
-	__u16 acc_priority;
-	__u16 auth_source_class;
-	__u16 att_code;
-	__u8 source_addr[6];
-	__u16 beacon_type;
-	__u16 major_vector;
-	__u16 lan_status;
-	__u16 soft_error_time;
-	__u16 reserved1;
-	__u16 local_ring;
-	__u16 mon_error;
-	__u16 beacon_transmit;
-	__u16 beacon_receive;
-	__u16 frame_correl;
-	__u8 beacon_naun[6];
-	__u32 reserved2;
-	__u8 beacon_phys[4];
-};
diff --git a/drivers/net/tokenring/madgemc.c b/drivers/net/tokenring/madgemc.c
deleted file mode 100644
index 28adcdf3b14..00000000000
--- a/drivers/net/tokenring/madgemc.c
+++ /dev/null
@@ -1,761 +0,0 @@
-/*
- *  madgemc.c: Driver for the Madge Smart 16/4 MC16 MCA token ring card.
- *
- *  Written 2000 by Adam Fritzler
- *
- *  This software may be used and distributed according to the terms
- *  of the GNU General Public License, incorporated herein by reference.
- *
- *  This driver module supports the following cards:
- *      - Madge Smart 16/4 Ringnode MC16
- *	- Madge Smart 16/4 Ringnode MC32 (??)
- *
- *  Maintainer(s):
- *    AF	Adam Fritzler
- *
- *  Modification History:
- *	16-Jan-00	AF	Created
- *
- */
-static const char version[] = "madgemc.c: v0.91 23/01/2000 by Adam Fritzler\n";
-
-#include <linux/module.h>
-#include <linux/mca.h>
-#include <linux/slab.h>
-#include <linux/kernel.h>
-#include <linux/errno.h>
-#include <linux/init.h>
-#include <linux/netdevice.h>
-#include <linux/trdevice.h>
-
-#include <asm/io.h>
-#include <asm/irq.h>
-
-#include "tms380tr.h"
-#include "madgemc.h"            /* Madge-specific constants */
-
-#define MADGEMC_IO_EXTENT 32
-#define MADGEMC_SIF_OFFSET 0x08
-
-struct card_info {
-	/*
-	 * These are read from the BIA ROM.
-	 */
-	unsigned int manid;
-	unsigned int cardtype;
-	unsigned int cardrev;
-	unsigned int ramsize;
-	
-	/*
-	 * These are read from the MCA POS registers.  
-	 */
-	unsigned int burstmode:2;
-	unsigned int fairness:1; /* 0 = Fair, 1 = Unfair */
-	unsigned int arblevel:4;
-	unsigned int ringspeed:2; /* 0 = 4mb, 1 = 16, 2 = Auto/none */
-	unsigned int cabletype:1; /* 0 = RJ45, 1 = DB9 */
-};
-
-static int madgemc_open(struct net_device *dev);
-static int madgemc_close(struct net_device *dev);
-static int madgemc_chipset_init(struct net_device *dev);
-static void madgemc_read_rom(struct net_device *dev, struct card_info *card);
-static unsigned short madgemc_setnselout_pins(struct net_device *dev);
-static void madgemc_setcabletype(struct net_device *dev, int type);
-
-static int madgemc_mcaproc(char *buf, int slot, void *d);
-
-static void madgemc_setregpage(struct net_device *dev, int page);
-static void madgemc_setsifsel(struct net_device *dev, int val);
-static void madgemc_setint(struct net_device *dev, int val);
-
-static irqreturn_t madgemc_interrupt(int irq, void *dev_id);
-
-/*
- * These work around paging, however they don't guarantee you're on the
- * right page.
- */
-#define SIFREADB(reg) (inb(dev->base_addr + ((reg<0x8)?reg:reg-0x8)))
-#define SIFWRITEB(val, reg) (outb(val, dev->base_addr + ((reg<0x8)?reg:reg-0x8)))
-#define SIFREADW(reg) (inw(dev->base_addr + ((reg<0x8)?reg:reg-0x8)))
-#define SIFWRITEW(val, reg) (outw(val, dev->base_addr + ((reg<0x8)?reg:reg-0x8)))
-
-/*
- * Read a byte-length value from the register.
- */
-static unsigned short madgemc_sifreadb(struct net_device *dev, unsigned short reg)
-{
-	unsigned short ret;
-	if (reg<0x8)	
-		ret = SIFREADB(reg);
-	else {
-		madgemc_setregpage(dev, 1);	
-		ret = SIFREADB(reg);
-		madgemc_setregpage(dev, 0);
-	}
-	return ret;
-}
-
-/*
- * Write a byte-length value to a register.
- */
-static void madgemc_sifwriteb(struct net_device *dev, unsigned short val, unsigned short reg)
-{
-	if (reg<0x8)
-		SIFWRITEB(val, reg);
-	else {
-		madgemc_setregpage(dev, 1);
-		SIFWRITEB(val, reg);
-		madgemc_setregpage(dev, 0);
-	}
-}
-
-/*
- * Read a word-length value from a register
- */
-static unsigned short madgemc_sifreadw(struct net_device *dev, unsigned short reg)
-{
-	unsigned short ret;
-	if (reg<0x8)	
-		ret = SIFREADW(reg);
-	else {
-		madgemc_setregpage(dev, 1);	
-		ret = SIFREADW(reg);
-		madgemc_setregpage(dev, 0);
-	}
-	return ret;
-}
-
-/*
- * Write a word-length value to a register.
- */
-static void madgemc_sifwritew(struct net_device *dev, unsigned short val, unsigned short reg)
-{
-	if (reg<0x8)
-		SIFWRITEW(val, reg);
-	else {
-		madgemc_setregpage(dev, 1);
-		SIFWRITEW(val, reg);
-		madgemc_setregpage(dev, 0);
-	}
-}
-
-static struct net_device_ops madgemc_netdev_ops __read_mostly;
-
-static int __devinit madgemc_probe(struct device *device)
-{	
-	static int versionprinted;
-	struct net_device *dev;
-	struct net_local *tp;
-	struct card_info *card;
-	struct mca_device *mdev = to_mca_device(device);
-	int ret = 0;
-
-	if (versionprinted++ == 0)
-		printk("%s", version);
-
-	if(mca_device_claimed(mdev))
-		return -EBUSY;
-	mca_device_set_claim(mdev, 1);
-
-	dev = alloc_trdev(sizeof(struct net_local));
-	if (!dev) {
-		printk("madgemc: unable to allocate dev space\n");
-		mca_device_set_claim(mdev, 0);
-		ret = -ENOMEM;
-		goto getout;
-	}
-
-	dev->netdev_ops = &madgemc_netdev_ops;
-
-	card = kmalloc(sizeof(struct card_info), GFP_KERNEL);
-	if (card==NULL) {
-		ret = -ENOMEM;
-		goto getout1;
-	}
-
-	/*
-	 * Parse configuration information.  This all comes
-	 * directly from the publicly available @002d.ADF.
-	 * Get it from Madge or your local ADF library.
-	 */
-
-	/*
-	 * Base address 
-	 */
-	dev->base_addr = 0x0a20 + 
-		((mdev->pos[2] & MC16_POS2_ADDR2)?0x0400:0) +
-		((mdev->pos[0] & MC16_POS0_ADDR1)?0x1000:0) +
-		((mdev->pos[3] & MC16_POS3_ADDR3)?0x2000:0);
-
-	/*
-	 * Interrupt line
-	 */
-	switch(mdev->pos[0] >> 6) { /* upper two bits */
-		case 0x1: dev->irq = 3; break;
-		case 0x2: dev->irq = 9; break; /* IRQ 2 = IRQ 9 */
-		case 0x3: dev->irq = 10; break;
-		default: dev->irq = 0; break;
-	}
-
-	if (dev->irq == 0) {
-		printk("%s: invalid IRQ\n", dev->name);
-		ret = -EBUSY;
-		goto getout2;
-	}
-
-	if (!request_region(dev->base_addr, MADGEMC_IO_EXTENT, 
-			   "madgemc")) {
-		printk(KERN_INFO "madgemc: unable to setup Smart MC in slot %d because of I/O base conflict at 0x%04lx\n", mdev->slot, dev->base_addr);
-		dev->base_addr += MADGEMC_SIF_OFFSET;
-		ret = -EBUSY;
-		goto getout2;
-	}
-	dev->base_addr += MADGEMC_SIF_OFFSET;
-	
-	/*
-	 * Arbitration Level
-	 */
-	card->arblevel = ((mdev->pos[0] >> 1) & 0x7) + 8;
-
-	/*
-	 * Burst mode and Fairness
-	 */
-	card->burstmode = ((mdev->pos[2] >> 6) & 0x3);
-	card->fairness = ((mdev->pos[2] >> 4) & 0x1);
-
-	/*
-	 * Ring Speed
-	 */
-	if ((mdev->pos[1] >> 2)&0x1)
-		card->ringspeed = 2; /* not selected */
-	else if ((mdev->pos[2] >> 5) & 0x1)
-		card->ringspeed = 1; /* 16Mb */
-	else
-		card->ringspeed = 0; /* 4Mb */
-
-	/* 
-	 * Cable type
-	 */
-	if ((mdev->pos[1] >> 6)&0x1)
-		card->cabletype = 1; /* STP/DB9 */
-	else
-		card->cabletype = 0; /* UTP/RJ-45 */
-
-
-	/* 
-	 * ROM Info. This requires us to actually twiddle
-	 * bits on the card, so we must ensure above that 
-	 * the base address is free of conflict (request_region above).
-	 */
-	madgemc_read_rom(dev, card);
-		
-	if (card->manid != 0x4d) { /* something went wrong */
-		printk(KERN_INFO "%s: Madge MC ROM read failed (unknown manufacturer ID %02x)\n", dev->name, card->manid);
-		goto getout3;
-	}
-		
-	if ((card->cardtype != 0x08) && (card->cardtype != 0x0d)) {
-		printk(KERN_INFO "%s: Madge MC ROM read failed (unknown card ID %02x)\n", dev->name, card->cardtype);
-		ret = -EIO;
-		goto getout3;
-	}
-	       
-	/* All cards except Rev 0 and 1 MC16's have 256kb of RAM */
-	if ((card->cardtype == 0x08) && (card->cardrev <= 0x01))
-		card->ramsize = 128;
-	else
-		card->ramsize = 256;
-
-	printk("%s: %s Rev %d at 0x%04lx IRQ %d\n", 
-	       dev->name, 
-	       (card->cardtype == 0x08)?MADGEMC16_CARDNAME:
-	       MADGEMC32_CARDNAME, card->cardrev, 
-	       dev->base_addr, dev->irq);
-
-	if (card->cardtype == 0x0d)
-		printk("%s:     Warning: MC32 support is experimental and highly untested\n", dev->name);
-	
-	if (card->ringspeed==2) { /* Unknown */
-		printk("%s:     Warning: Ring speed not set in POS -- Please run the reference disk and set it!\n", dev->name);
-		card->ringspeed = 1; /* default to 16mb */
-	}
-		
-	printk("%s:     RAM Size: %dKB\n", dev->name, card->ramsize);
-
-	printk("%s:     Ring Speed: %dMb/sec on %s\n", dev->name, 
-	       (card->ringspeed)?16:4, 
-	       card->cabletype?"STP/DB9":"UTP/RJ-45");
-	printk("%s:     Arbitration Level: %d\n", dev->name, 
-	       card->arblevel);
-
-	printk("%s:     Burst Mode: ", dev->name);
-	switch(card->burstmode) {
-		case 0: printk("Cycle steal"); break;
-		case 1: printk("Limited burst"); break;
-		case 2: printk("Delayed release"); break;
-		case 3: printk("Immediate release"); break;
-	}
-	printk(" (%s)\n", (card->fairness)?"Unfair":"Fair");
-
-
-	/* 
-	 * Enable SIF before we assign the interrupt handler,
-	 * just in case we get spurious interrupts that need
-	 * handling.
-	 */ 
-	outb(0, dev->base_addr + MC_CONTROL_REG0); /* sanity */
-	madgemc_setsifsel(dev, 1);
-	if (request_irq(dev->irq, madgemc_interrupt, IRQF_SHARED,
-		       "madgemc", dev)) {
-		ret = -EBUSY;
-		goto getout3;
-	}
-
-	madgemc_chipset_init(dev); /* enables interrupts! */
-	madgemc_setcabletype(dev, card->cabletype);
-
-	/* Setup MCA structures */
-	mca_device_set_name(mdev, (card->cardtype == 0x08)?MADGEMC16_CARDNAME:MADGEMC32_CARDNAME);
-	mca_set_adapter_procfn(mdev->slot, madgemc_mcaproc, dev);
-
-	printk("%s:     Ring Station Address: %pM\n",
-	       dev->name, dev->dev_addr);
-
-	if (tmsdev_init(dev, device)) {
-		printk("%s: unable to get memory for dev->priv.\n", 
-		       dev->name);
-		ret = -ENOMEM;
-		goto getout4;
-	}
-	tp = netdev_priv(dev);
-
-	/* 
-	 * The MC16 is physically a 32bit card.  However, Madge
-	 * insists on calling it 16bit, so I'll assume here that
-	 * they know what they're talking about.  Cut off DMA
-	 * at 16mb.
-	 */
-	tp->setnselout = madgemc_setnselout_pins;
-	tp->sifwriteb = madgemc_sifwriteb;
-	tp->sifreadb = madgemc_sifreadb;
-	tp->sifwritew = madgemc_sifwritew;
-	tp->sifreadw = madgemc_sifreadw;
-	tp->DataRate = (card->ringspeed)?SPEED_16:SPEED_4;
-
-	memcpy(tp->ProductID, "Madge MCA 16/4    ", PROD_ID_SIZE + 1);
-
-	tp->tmspriv = card;
-	dev_set_drvdata(device, dev);
-
-	if (register_netdev(dev) == 0)
-		return 0;
-
-	dev_set_drvdata(device, NULL);
-	ret = -ENOMEM;
-getout4:
-	free_irq(dev->irq, dev);
-getout3:
-	release_region(dev->base_addr-MADGEMC_SIF_OFFSET, 
-		       MADGEMC_IO_EXTENT); 
-getout2:
-	kfree(card);
-getout1:
-	free_netdev(dev);
-getout:
-	mca_device_set_claim(mdev, 0);
-	return ret;
-}
-
-/*
- * Handle interrupts generated by the card
- *
- * The MicroChannel Madge cards need slightly more handling
- * after an interrupt than other TMS380 cards do.
- *
- * First we must make sure it was this card that generated the
- * interrupt (since interrupt sharing is allowed).  Then,
- * because we're using level-triggered interrupts (as is
- * standard on MCA), we must toggle the interrupt line
- * on the card in order to claim and acknowledge the interrupt.
- * Once that is done, the interrupt should be handlable in
- * the normal tms380tr_interrupt() routine.
- *
- * There's two ways we can check to see if the interrupt is ours,
- * both with their own disadvantages...
- *
- * 1)  	Read in the SIFSTS register from the TMS controller.  This
- *	is guaranteed to be accurate, however, there's a fairly
- *	large performance penalty for doing so: the Madge chips
- *	must request the register from the Eagle, the Eagle must
- *	read them from its internal bus, and then take the route
- *	back out again, for a 16bit read.  
- *
- * 2)	Use the MC_CONTROL_REG0_SINTR bit from the Madge ASICs.
- *	The major disadvantage here is that the accuracy of the
- *	bit is in question.  However, it cuts out the extra read
- *	cycles it takes to read the Eagle's SIF, as its only an
- *	8bit read, and theoretically the Madge bit is directly
- *	connected to the interrupt latch coming out of the Eagle
- *	hardware (that statement is not verified).  
- *
- * I can't determine which of these methods has the best win.  For now,
- * we make a compromise.  Use the Madge way for the first interrupt,
- * which should be the fast-path, and then once we hit the first 
- * interrupt, keep on trying using the SIF method until we've
- * exhausted all contiguous interrupts.
- *
- */
-static irqreturn_t madgemc_interrupt(int irq, void *dev_id)
-{
-	int pending,reg1;
-	struct net_device *dev;
-
-	if (!dev_id) {
-		printk("madgemc_interrupt: was not passed a dev_id!\n");
-		return IRQ_NONE;
-	}
-
-	dev = dev_id;
-
-	/* Make sure its really us. -- the Madge way */
-	pending = inb(dev->base_addr + MC_CONTROL_REG0);
-	if (!(pending & MC_CONTROL_REG0_SINTR))
-		return IRQ_NONE; /* not our interrupt */
-
-	/*
-	 * Since we're level-triggered, we may miss the rising edge
-	 * of the next interrupt while we're off handling this one,
-	 * so keep checking until the SIF verifies that it has nothing
-	 * left for us to do.
-	 */
-	pending = STS_SYSTEM_IRQ;
-	do {
-		if (pending & STS_SYSTEM_IRQ) {
-
-			/* Toggle the interrupt to reset the latch on card */
-			reg1 = inb(dev->base_addr + MC_CONTROL_REG1);
-			outb(reg1 ^ MC_CONTROL_REG1_SINTEN, 
-			     dev->base_addr + MC_CONTROL_REG1);
-			outb(reg1, dev->base_addr + MC_CONTROL_REG1);
-
-			/* Continue handling as normal */
-			tms380tr_interrupt(irq, dev_id);
-
-			pending = SIFREADW(SIFSTS); /* restart - the SIF way */
-
-		} else
-			return IRQ_HANDLED; 
-	} while (1);
-
-	return IRQ_HANDLED; /* not reachable */
-}
-
-/*
- * Set the card to the preferred ring speed.
- *
- * Unlike newer cards, the MC16/32 have their speed selection
- * circuit connected to the Madge ASICs and not to the TMS380
- * NSELOUT pins. Set the ASIC bits correctly here, and return 
- * zero to leave the TMS NSELOUT bits unaffected.
- *
- */
-static unsigned short madgemc_setnselout_pins(struct net_device *dev)
-{
-	unsigned char reg1;
-	struct net_local *tp = netdev_priv(dev);
-	
-	reg1 = inb(dev->base_addr + MC_CONTROL_REG1);
-
-	if(tp->DataRate == SPEED_16)
-		reg1 |= MC_CONTROL_REG1_SPEED_SEL; /* add for 16mb */
-	else if (reg1 & MC_CONTROL_REG1_SPEED_SEL)
-		reg1 ^= MC_CONTROL_REG1_SPEED_SEL; /* remove for 4mb */
-	outb(reg1, dev->base_addr + MC_CONTROL_REG1);
-
-	return 0; /* no change */
-}
-
-/*
- * Set the register page.  This equates to the SRSX line
- * on the TMS380Cx6.
- *
- * Register selection is normally done via three contiguous
- * bits.  However, some boards (such as the MC16/32) use only
- * two bits, plus a separate bit in the glue chip.  This
- * sets the SRSX bit (the top bit).  See page 4-17 in the
- * Yellow Book for which registers are affected.
- *
- */
-static void madgemc_setregpage(struct net_device *dev, int page)
-{	
-	static int reg1;
-
-	reg1 = inb(dev->base_addr + MC_CONTROL_REG1);
-	if ((page == 0) && (reg1 & MC_CONTROL_REG1_SRSX)) {
-		outb(reg1 ^ MC_CONTROL_REG1_SRSX, 
-		     dev->base_addr + MC_CONTROL_REG1);
-	}
-	else if (page == 1) {
-		outb(reg1 | MC_CONTROL_REG1_SRSX, 
-		     dev->base_addr + MC_CONTROL_REG1);
-	}
-	reg1 = inb(dev->base_addr + MC_CONTROL_REG1);
-}
-
-/*
- * The SIF registers are not mapped into register space by default
- * Set this to 1 to map them, 0 to map the BIA ROM.
- *
- */
-static void madgemc_setsifsel(struct net_device *dev, int val)
-{
-	unsigned int reg0;
-
-	reg0 = inb(dev->base_addr + MC_CONTROL_REG0);
-	if ((val == 0) && (reg0 & MC_CONTROL_REG0_SIFSEL)) {
-		outb(reg0 ^ MC_CONTROL_REG0_SIFSEL, 
-		     dev->base_addr + MC_CONTROL_REG0);
-	} else if (val == 1) {
-		outb(reg0 | MC_CONTROL_REG0_SIFSEL, 
-		     dev->base_addr + MC_CONTROL_REG0);
-	}	
-	reg0 = inb(dev->base_addr + MC_CONTROL_REG0);
-}
-
-/*
- * Enable SIF interrupts
- *
- * This does not enable interrupts in the SIF, but rather
- * enables SIF interrupts to be passed onto the host.
- *
- */
-static void madgemc_setint(struct net_device *dev, int val)
-{
-	unsigned int reg1;
-
-	reg1 = inb(dev->base_addr + MC_CONTROL_REG1);
-	if ((val == 0) && (reg1 & MC_CONTROL_REG1_SINTEN)) {
-		outb(reg1 ^ MC_CONTROL_REG1_SINTEN, 
-		     dev->base_addr + MC_CONTROL_REG1);
-	} else if (val == 1) {
-		outb(reg1 | MC_CONTROL_REG1_SINTEN, 
-		     dev->base_addr + MC_CONTROL_REG1);
-	}
-}
-
-/*
- * Cable type is set via control register 7. Bit zero high
- * for UTP, low for STP.
- */
-static void madgemc_setcabletype(struct net_device *dev, int type)
-{
-	outb((type==0)?MC_CONTROL_REG7_CABLEUTP:MC_CONTROL_REG7_CABLESTP,
-	     dev->base_addr + MC_CONTROL_REG7);
-}
-
-/*
- * Enable the functions of the Madge chipset needed for
- * full working order. 
- */
-static int madgemc_chipset_init(struct net_device *dev)
-{
-	outb(0, dev->base_addr + MC_CONTROL_REG1); /* pull SRESET low */
-	tms380tr_wait(100); /* wait for card to reset */
-
-	/* bring back into normal operating mode */
-	outb(MC_CONTROL_REG1_NSRESET, dev->base_addr + MC_CONTROL_REG1);
-
-	/* map SIF registers */
-	madgemc_setsifsel(dev, 1);
-
-	/* enable SIF interrupts */
-	madgemc_setint(dev, 1); 
-
-	return 0;
-}
-
-/*
- * Disable the board, and put back into power-up state.
- */
-static void madgemc_chipset_close(struct net_device *dev)
-{
-	/* disable interrupts */
-	madgemc_setint(dev, 0);
-	/* unmap SIF registers */
-	madgemc_setsifsel(dev, 0);
-}
-
-/*
- * Read the card type (MC16 or MC32) from the card.
- *
- * The configuration registers are stored in two separate
- * pages.  Pages are flipped by clearing bit 3 of CONTROL_REG0 (PAGE)
- * for page zero, or setting bit 3 for page one.
- *
- * Page zero contains the following data:
- *	Byte 0: Manufacturer ID (0x4D -- ASCII "M")
- *	Byte 1: Card type:
- *			0x08 for MC16
- *			0x0D for MC32
- *	Byte 2: Card revision
- *	Byte 3: Mirror of POS config register 0
- *	Byte 4: Mirror of POS 1
- *	Byte 5: Mirror of POS 2
- *
- * Page one contains the following data:
- *	Byte 0: Unused
- *	Byte 1-6: BIA, MSB to LSB.
- *
- * Note that to read the BIA, we must unmap the SIF registers
- * by clearing bit 2 of CONTROL_REG0 (SIFSEL), as the data
- * will reside in the same logical location.  For this reason,
- * _never_ read the BIA while the Eagle processor is running!
- * The SIF will be completely inaccessible until the BIA operation
- * is complete.
- *
- */
-static void madgemc_read_rom(struct net_device *dev, struct card_info *card)
-{
-	unsigned long ioaddr;
-	unsigned char reg0, reg1, tmpreg0, i;
-
-	ioaddr = dev->base_addr;
-
-	reg0 = inb(ioaddr + MC_CONTROL_REG0);
-	reg1 = inb(ioaddr + MC_CONTROL_REG1);
-
-	/* Switch to page zero and unmap SIF */
-	tmpreg0 = reg0 & ~(MC_CONTROL_REG0_PAGE + MC_CONTROL_REG0_SIFSEL);
-	outb(tmpreg0, ioaddr + MC_CONTROL_REG0);
-	
-	card->manid = inb(ioaddr + MC_ROM_MANUFACTURERID);
-	card->cardtype = inb(ioaddr + MC_ROM_ADAPTERID);
-	card->cardrev = inb(ioaddr + MC_ROM_REVISION);
-
-	/* Switch to rom page one */
-	outb(tmpreg0 | MC_CONTROL_REG0_PAGE, ioaddr + MC_CONTROL_REG0);
-
-	/* Read BIA */
-	dev->addr_len = 6;
-	for (i = 0; i < 6; i++)
-		dev->dev_addr[i] = inb(ioaddr + MC_ROM_BIA_START + i);
-	
-	/* Restore original register values */
-	outb(reg0, ioaddr + MC_CONTROL_REG0);
-	outb(reg1, ioaddr + MC_CONTROL_REG1);
-}
-
-static int madgemc_open(struct net_device *dev)
-{  
-	/*
-	 * Go ahead and reinitialize the chipset again, just to 
-	 * make sure we didn't get left in a bad state.
-	 */
-	madgemc_chipset_init(dev);
-	tms380tr_open(dev);
-	return 0;
-}
-
-static int madgemc_close(struct net_device *dev)
-{
-	tms380tr_close(dev);
-	madgemc_chipset_close(dev);
-	return 0;
-}
-
-/*
- * Give some details available from /proc/mca/slotX
- */
-static int madgemc_mcaproc(char *buf, int slot, void *d) 
-{	
-	struct net_device *dev = (struct net_device *)d;
-	struct net_local *tp = netdev_priv(dev);
-	struct card_info *curcard = tp->tmspriv;
-	int len = 0;
-	
-	len += sprintf(buf+len, "-------\n");
-	if (curcard) {
-		len += sprintf(buf+len, "Card Revision: %d\n", curcard->cardrev);
-		len += sprintf(buf+len, "RAM Size: %dkb\n", curcard->ramsize);
-		len += sprintf(buf+len, "Cable type: %s\n", (curcard->cabletype)?"STP/DB9":"UTP/RJ-45");
-		len += sprintf(buf+len, "Configured ring speed: %dMb/sec\n", (curcard->ringspeed)?16:4);
-		len += sprintf(buf+len, "Running ring speed: %dMb/sec\n", (tp->DataRate==SPEED_16)?16:4);
-		len += sprintf(buf+len, "Device: %s\n", dev->name);
-		len += sprintf(buf+len, "IO Port: 0x%04lx\n", dev->base_addr);
-		len += sprintf(buf+len, "IRQ: %d\n", dev->irq);
-		len += sprintf(buf+len, "Arbitration Level: %d\n", curcard->arblevel);
-		len += sprintf(buf+len, "Burst Mode: ");
-		switch(curcard->burstmode) {
-		case 0: len += sprintf(buf+len, "Cycle steal"); break;
-		case 1: len += sprintf(buf+len, "Limited burst"); break;
-		case 2: len += sprintf(buf+len, "Delayed release"); break;
-		case 3: len += sprintf(buf+len, "Immediate release"); break;
-		}
-		len += sprintf(buf+len, " (%s)\n", (curcard->fairness)?"Unfair":"Fair");
-		
-		len += sprintf(buf+len, "Ring Station Address: %pM\n",
-			       dev->dev_addr);
-	} else 
-		len += sprintf(buf+len, "Card not configured\n");
-
-	return len;
-}
-
-static int __devexit madgemc_remove(struct device *device)
-{
-	struct net_device *dev = dev_get_drvdata(device);
-	struct net_local *tp;
-        struct card_info *card;
-
-	BUG_ON(!dev);
-
-	tp = netdev_priv(dev);
-	card = tp->tmspriv;
-	kfree(card);
-	tp->tmspriv = NULL;
-
-	unregister_netdev(dev);
-	release_region(dev->base_addr-MADGEMC_SIF_OFFSET, MADGEMC_IO_EXTENT);
-	free_irq(dev->irq, dev);
-	tmsdev_term(dev);
-	free_netdev(dev);
-	dev_set_drvdata(device, NULL);
-
-	return 0;
-}
-
-static short madgemc_adapter_ids[] __initdata = {
-	0x002d,
-	0x0000
-};
-
-static struct mca_driver madgemc_driver = {
-	.id_table = madgemc_adapter_ids,
-	.driver = {
-		.name = "madgemc",
-		.bus = &mca_bus_type,
-		.probe = madgemc_probe,
-		.remove = __devexit_p(madgemc_remove),
-	},
-};
-
-static int __init madgemc_init (void)
-{
-	madgemc_netdev_ops = tms380tr_netdev_ops;
-	madgemc_netdev_ops.ndo_open = madgemc_open;
-	madgemc_netdev_ops.ndo_stop = madgemc_close;
-
-	return mca_register_driver (&madgemc_driver);
-}
-
-static void __exit madgemc_exit (void)
-{
-	mca_unregister_driver (&madgemc_driver);
-}
-
-module_init(madgemc_init);
-module_exit(madgemc_exit);
-
-MODULE_LICENSE("GPL");
-
diff --git a/drivers/net/tokenring/madgemc.h b/drivers/net/tokenring/madgemc.h
deleted file mode 100644
index fe88e272c53..00000000000
--- a/drivers/net/tokenring/madgemc.h
+++ /dev/null
@@ -1,70 +0,0 @@
-/* 
- * madgemc.h: Header for the madgemc tms380tr module
- *
- * Authors:
- * - Adam Fritzler
- */
-
-#ifndef __LINUX_MADGEMC_H
-#define __LINUX_MADGEMC_H
-
-#ifdef __KERNEL__
-
-#define MADGEMC16_CARDNAME "Madge Smart 16/4 MC16 Ringnode"
-#define MADGEMC32_CARDNAME "Madge Smart 16/4 MC32 Ringnode"
-
-/* 
- * Bit definitions for the POS config registers
- */
-#define MC16_POS0_ADDR1 0x20
-#define MC16_POS2_ADDR2 0x04
-#define MC16_POS3_ADDR3 0x20
-
-#define MC_CONTROL_REG0		((long)-8) /* 0x00 */
-#define MC_CONTROL_REG1		((long)-7) /* 0x01 */
-#define MC_ADAPTER_POS_REG0	((long)-6) /* 0x02 */
-#define MC_ADAPTER_POS_REG1	((long)-5) /* 0x03 */
-#define MC_ADAPTER_POS_REG2	((long)-4) /* 0x04 */
-#define MC_ADAPTER_REG5_UNUSED	((long)-3) /* 0x05 */
-#define MC_ADAPTER_REG6_UNUSED	((long)-2) /* 0x06 */
-#define MC_CONTROL_REG7		((long)-1) /* 0x07 */
-
-#define MC_CONTROL_REG0_UNKNOWN1	0x01
-#define MC_CONTROL_REG0_UNKNOWN2	0x02
-#define MC_CONTROL_REG0_SIFSEL		0x04
-#define MC_CONTROL_REG0_PAGE		0x08
-#define MC_CONTROL_REG0_TESTINTERRUPT	0x10
-#define MC_CONTROL_REG0_UNKNOWN20	0x20
-#define MC_CONTROL_REG0_SINTR		0x40
-#define MC_CONTROL_REG0_UNKNOWN80	0x80
-
-#define MC_CONTROL_REG1_SINTEN		0x01
-#define MC_CONTROL_REG1_BITOFDEATH	0x02
-#define MC_CONTROL_REG1_NSRESET		0x04
-#define MC_CONTROL_REG1_UNKNOWN8	0x08
-#define MC_CONTROL_REG1_UNKNOWN10	0x10
-#define MC_CONTROL_REG1_UNKNOWN20	0x20
-#define MC_CONTROL_REG1_SRSX		0x40
-#define MC_CONTROL_REG1_SPEED_SEL	0x80
-
-#define MC_CONTROL_REG7_CABLESTP	0x00
-#define MC_CONTROL_REG7_CABLEUTP	0x01
-
-/*
- * ROM Page Zero
- */
-#define MC_ROM_MANUFACTURERID		0x00
-#define MC_ROM_ADAPTERID		0x01
-#define MC_ROM_REVISION			0x02
-#define MC_ROM_CONFIG0			0x03
-#define MC_ROM_CONFIG1			0x04
-#define MC_ROM_CONFIG2			0x05
-
-/*
- * ROM Page One
- */
-#define MC_ROM_UNUSED_BYTE		0x00
-#define MC_ROM_BIA_START		0x01
-
-#endif /* __KERNEL__ */
-#endif /* __LINUX_MADGEMC_H */
diff --git a/drivers/net/tokenring/olympic.c b/drivers/net/tokenring/olympic.c
deleted file mode 100644
index 0e234741cc7..00000000000
--- a/drivers/net/tokenring/olympic.c
+++ /dev/null
@@ -1,1749 +0,0 @@
-/*
- *   olympic.c (c) 1999 Peter De Schrijver All Rights Reserved
- *		   1999/2000 Mike Phillips (mikep@linuxtr.net)
- *
- *  Linux driver for IBM PCI tokenring cards based on the Pit/Pit-Phy/Olympic
- *  chipset. 
- *
- *  Base Driver Skeleton:
- *      Written 1993-94 by Donald Becker.
- *
- *      Copyright 1993 United States Government as represented by the
- *      Director, National Security Agency.
- *
- *  Thanks to Erik De Cock, Adrian Bridgett and Frank Fiene for their 
- *  assistance and perserverance with the testing of this driver.
- *
- *  This software may be used and distributed according to the terms
- *  of the GNU General Public License, incorporated herein by reference.
- * 
- *  4/27/99 - Alpha Release 0.1.0
- *            First release to the public
- *
- *  6/8/99  - Official Release 0.2.0   
- *            Merged into the kernel code 
- *  8/18/99 - Updated driver for 2.3.13 kernel to use new pci
- *	      resource. Driver also reports the card name returned by
- *            the pci resource.
- *  1/11/00 - Added spinlocks for smp
- *  2/23/00 - Updated to dev_kfree_irq 
- *  3/10/00 - Fixed FDX enable which triggered other bugs also 
- *            squashed.
- *  5/20/00 - Changes to handle Olympic on LinuxPPC. Endian changes.
- *            The odd thing about the changes is that the fix for
- *            endian issues with the big-endian data in the arb, asb...
- *            was to always swab() the bytes, no matter what CPU.
- *            That's because the read[wl]() functions always swap the
- *            bytes on the way in on PPC.
- *            Fixing the hardware descriptors was another matter,
- *            because they weren't going through read[wl](), there all
- *            the results had to be in memory in le32 values. kdaaker
- *
- * 12/23/00 - Added minimal Cardbus support (Thanks Donald).
- *
- * 03/09/01 - Add new pci api, dev_base_lock, general clean up. 
- *
- * 03/27/01 - Add new dma pci (Thanks to Kyle Lucke) and alloc_trdev
- *	      Change proc_fs behaviour, now one entry per adapter.
- *
- * 04/09/01 - Couple of bug fixes to the dma unmaps and ejecting the
- *	      adapter when live does not take the system down with it.
- * 
- * 06/02/01 - Clean up, copy skb for small packets
- * 
- * 06/22/01 - Add EISR error handling routines 
- *
- * 07/19/01 - Improve bad LAA reporting, strip out freemem
- *	      into a separate function, its called from 3 
- *	      different places now. 
- * 02/09/02 - Replaced sleep_on. 
- * 03/01/02 - Replace access to several registers from 32 bit to 
- * 	      16 bit. Fixes alignment errors on PPC 64 bit machines.
- * 	      Thanks to Al Trautman for this one.
- * 03/10/02 - Fix BUG in arb_cmd. Bug was there all along but was
- * 	      silently ignored until the error checking code 
- * 	      went into version 1.0.0 
- * 06/04/02 - Add correct start up sequence for the cardbus adapters.
- * 	      Required for strict compliance with pci power mgmt specs.
- *  To Do:
- *
- *	     Wake on lan	
- * 
- *  If Problems do Occur
- *  Most problems can be rectified by either closing and opening the interface
- *  (ifconfig down and up) or rmmod and insmod'ing the driver (a bit difficult
- *  if compiled into the kernel).
- */
-
-/* Change OLYMPIC_DEBUG to 1 to get verbose, and I mean really verbose, messages */
-
-#define OLYMPIC_DEBUG 0
-
-
-#include <linux/module.h>
-#include <linux/kernel.h>
-#include <linux/errno.h>
-#include <linux/timer.h>
-#include <linux/in.h>
-#include <linux/ioport.h>
-#include <linux/seq_file.h>
-#include <linux/string.h>
-#include <linux/proc_fs.h>
-#include <linux/ptrace.h>
-#include <linux/skbuff.h>
-#include <linux/interrupt.h>
-#include <linux/delay.h>
-#include <linux/netdevice.h>
-#include <linux/trdevice.h>
-#include <linux/stddef.h>
-#include <linux/init.h>
-#include <linux/pci.h>
-#include <linux/spinlock.h>
-#include <linux/bitops.h>
-#include <linux/jiffies.h>
-
-#include <net/checksum.h>
-#include <net/net_namespace.h>
-
-#include <asm/io.h>
-
-#include "olympic.h"
-
-/* I've got to put some intelligence into the version number so that Peter and I know
- * which version of the code somebody has got. 
- * Version Number = a.b.c.d  where a.b.c is the level of code and d is the latest author.
- * So 0.0.1.pds = Peter, 0.0.1.mlp = Mike
- * 
- * Official releases will only have an a.b.c version number format. 
- */
-
-static char version[] =
-"Olympic.c v1.0.5 6/04/02 - Peter De Schrijver & Mike Phillips" ; 
-
-static char *open_maj_error[]  = {"No error", "Lobe Media Test", "Physical Insertion",
-				   "Address Verification", "Neighbor Notification (Ring Poll)",
-				   "Request Parameters","FDX Registration Request",
-				   "FDX Duplicate Address Check", "Station registration Query Wait",
-				   "Unknown stage"};
-
-static char *open_min_error[] = {"No error", "Function Failure", "Signal Lost", "Wire Fault",
-				   "Ring Speed Mismatch", "Timeout","Ring Failure","Ring Beaconing",
-				   "Duplicate Node Address","Request Parameters","Remove Received",
-				   "Reserved", "Reserved", "No Monitor Detected for RPL", 
-				   "Monitor Contention failer for RPL", "FDX Protocol Error"};
-
-/* Module parameters */
-
-MODULE_AUTHOR("Mike Phillips <mikep@linuxtr.net>") ; 
-MODULE_DESCRIPTION("Olympic PCI/Cardbus Chipset Driver") ; 
-
-/* Ring Speed 0,4,16,100 
- * 0 = Autosense         
- * 4,16 = Selected speed only, no autosense
- * This allows the card to be the first on the ring
- * and become the active monitor.
- * 100 = Nothing at present, 100mbps is autodetected
- * if FDX is turned on. May be implemented in the future to 
- * fail if 100mpbs is not detected.
- *
- * WARNING: Some hubs will allow you to insert
- * at the wrong speed
- */
-
-static int ringspeed[OLYMPIC_MAX_ADAPTERS] = {0,} ;
-module_param_array(ringspeed, int, NULL, 0);
-
-/* Packet buffer size */
-
-static int pkt_buf_sz[OLYMPIC_MAX_ADAPTERS] = {0,} ;
-module_param_array(pkt_buf_sz, int, NULL, 0) ;
-
-/* Message Level */
-
-static int message_level[OLYMPIC_MAX_ADAPTERS] = {0,} ; 
-module_param_array(message_level, int, NULL, 0) ;
-
-/* Change network_monitor to receive mac frames through the arb channel.
- * Will also create a /proc/net/olympic_tr%d entry, where %d is the tr
- * device, i.e. tr0, tr1 etc. 
- * Intended to be used to create a ring-error reporting network module 
- * i.e. it will give you the source address of beaconers on the ring 
- */
-static int network_monitor[OLYMPIC_MAX_ADAPTERS] = {0,};
-module_param_array(network_monitor, int, NULL, 0);
-
-static DEFINE_PCI_DEVICE_TABLE(olympic_pci_tbl) = {
-	{PCI_VENDOR_ID_IBM,PCI_DEVICE_ID_IBM_TR_WAKE,PCI_ANY_ID,PCI_ANY_ID,},
-	{ } 	/* Terminating Entry */
-};
-MODULE_DEVICE_TABLE(pci,olympic_pci_tbl) ; 
-
-
-static int olympic_probe(struct pci_dev *pdev, const struct pci_device_id *ent); 
-static int olympic_init(struct net_device *dev);
-static int olympic_open(struct net_device *dev);
-static netdev_tx_t olympic_xmit(struct sk_buff *skb,
-				      struct net_device *dev);
-static int olympic_close(struct net_device *dev);
-static void olympic_set_rx_mode(struct net_device *dev);
-static void olympic_freemem(struct net_device *dev) ;  
-static irqreturn_t olympic_interrupt(int irq, void *dev_id);
-static int olympic_set_mac_address(struct net_device *dev, void *addr) ; 
-static void olympic_arb_cmd(struct net_device *dev);
-static int olympic_change_mtu(struct net_device *dev, int mtu);
-static void olympic_srb_bh(struct net_device *dev) ; 
-static void olympic_asb_bh(struct net_device *dev) ; 
-static const struct file_operations olympic_proc_ops;
-
-static const struct net_device_ops olympic_netdev_ops = {
-	.ndo_open		= olympic_open,
-	.ndo_stop		= olympic_close,
-	.ndo_start_xmit		= olympic_xmit,
-	.ndo_change_mtu		= olympic_change_mtu,
-	.ndo_set_rx_mode	= olympic_set_rx_mode,
-	.ndo_set_mac_address	= olympic_set_mac_address,
-};
-
-static int __devinit olympic_probe(struct pci_dev *pdev, const struct pci_device_id *ent)
-{
-	struct net_device *dev ; 
-	struct olympic_private *olympic_priv;
-	static int card_no = -1 ;
-	int i ; 
-
-	card_no++ ; 
-
-	if ((i = pci_enable_device(pdev))) {
-		return i ; 
-	}
-
-	pci_set_master(pdev);
-
-	if ((i = pci_request_regions(pdev,"olympic"))) { 
-		goto op_disable_dev;
-	}
- 
-	dev = alloc_trdev(sizeof(struct olympic_private)) ; 
-	if (!dev) {
-		i = -ENOMEM; 
-		goto op_release_dev;
-	}
-
-	olympic_priv = netdev_priv(dev) ;
-	
-	spin_lock_init(&olympic_priv->olympic_lock) ; 
-
-	init_waitqueue_head(&olympic_priv->srb_wait);
-	init_waitqueue_head(&olympic_priv->trb_wait);
-#if OLYMPIC_DEBUG  
-	printk(KERN_INFO "pci_device: %p, dev:%p, dev->priv: %p\n", pdev, dev, netdev_priv(dev));
-#endif
-	dev->irq=pdev->irq;
-	dev->base_addr=pci_resource_start(pdev, 0);
-	olympic_priv->olympic_card_name = pci_name(pdev);
-	olympic_priv->pdev = pdev; 
-	olympic_priv->olympic_mmio = ioremap(pci_resource_start(pdev,1),256);
-	olympic_priv->olympic_lap = ioremap(pci_resource_start(pdev,2),2048);
-	if (!olympic_priv->olympic_mmio || !olympic_priv->olympic_lap) {
-		goto op_free_iomap;
-	}
-				
-	if ((pkt_buf_sz[card_no] < 100) || (pkt_buf_sz[card_no] > 18000) )
-		olympic_priv->pkt_buf_sz = PKT_BUF_SZ ; 
-	else
-		olympic_priv->pkt_buf_sz = pkt_buf_sz[card_no] ; 
-
-	dev->mtu = olympic_priv->pkt_buf_sz - TR_HLEN ; 
-	olympic_priv->olympic_ring_speed = ringspeed[card_no] ; 
-	olympic_priv->olympic_message_level = message_level[card_no] ; 
-	olympic_priv->olympic_network_monitor = network_monitor[card_no];
-	
-	if ((i = olympic_init(dev))) {
-		goto op_free_iomap;
-	}				
-
-	dev->netdev_ops = &olympic_netdev_ops;
-	SET_NETDEV_DEV(dev, &pdev->dev);
-
-	pci_set_drvdata(pdev,dev) ; 
-	register_netdev(dev) ; 
-	printk("Olympic: %s registered as: %s\n",olympic_priv->olympic_card_name,dev->name);
-	if (olympic_priv->olympic_network_monitor) { /* Must go after register_netdev as we need the device name */ 
-		char proc_name[20] ; 
-		strcpy(proc_name,"olympic_") ;
-		strcat(proc_name,dev->name) ; 
-		proc_create_data(proc_name, 0, init_net.proc_net, &olympic_proc_ops, dev);
-		printk("Olympic: Network Monitor information: /proc/%s\n",proc_name); 
-	}
-	return  0 ;
-
-op_free_iomap:
-	if (olympic_priv->olympic_mmio)
-		iounmap(olympic_priv->olympic_mmio); 
-	if (olympic_priv->olympic_lap)
-		iounmap(olympic_priv->olympic_lap);
-
-	free_netdev(dev);
-op_release_dev:
-	pci_release_regions(pdev); 
-
-op_disable_dev:
-	pci_disable_device(pdev);
-	return i;
-}
-
-static int olympic_init(struct net_device *dev)
-{
-    	struct olympic_private *olympic_priv;
-	u8 __iomem *olympic_mmio, *init_srb,*adapter_addr;
-	unsigned long t; 
-	unsigned int uaa_addr;
-
-	olympic_priv=netdev_priv(dev);
-	olympic_mmio=olympic_priv->olympic_mmio;
-
-	printk("%s\n", version);
-	printk("%s. I/O at %hx, MMIO at %p, LAP at %p, using irq %d\n", olympic_priv->olympic_card_name, (unsigned int) dev->base_addr,olympic_priv->olympic_mmio, olympic_priv->olympic_lap, dev->irq);
-
-	writel(readl(olympic_mmio+BCTL) | BCTL_SOFTRESET,olympic_mmio+BCTL);
-	t=jiffies;
-	while((readl(olympic_mmio+BCTL)) & BCTL_SOFTRESET) {
-		schedule();		
-		if(time_after(jiffies, t + 40*HZ)) {
-			printk(KERN_ERR "IBM PCI tokenring card not responding.\n");
-			return -ENODEV;
-		}
-	}
-
-
-	/* Needed for cardbus */
-	if(!(readl(olympic_mmio+BCTL) & BCTL_MODE_INDICATOR)) {
-		writel(readl(olympic_priv->olympic_mmio+FERMASK)|FERMASK_INT_BIT, olympic_mmio+FERMASK);
-	}
-	
-#if OLYMPIC_DEBUG
-	printk("BCTL: %x\n",readl(olympic_mmio+BCTL));
-	printk("GPR: %x\n",readw(olympic_mmio+GPR));
-	printk("SISRMASK: %x\n",readl(olympic_mmio+SISR_MASK));
-#endif
-	/* Aaaahhh, You have got to be real careful setting GPR, the card
-	   holds the previous values from flash memory, including autosense 
-           and ring speed */
-
-	writel(readl(olympic_mmio+BCTL)|BCTL_MIMREB,olympic_mmio+BCTL);
-	
-	if (olympic_priv->olympic_ring_speed  == 0) { /* Autosense */
-		writew(readw(olympic_mmio+GPR)|GPR_AUTOSENSE,olympic_mmio+GPR);
-		if (olympic_priv->olympic_message_level) 
-			printk(KERN_INFO "%s: Ringspeed autosense mode on\n",olympic_priv->olympic_card_name);
-	} else if (olympic_priv->olympic_ring_speed == 16) {
-		if (olympic_priv->olympic_message_level) 
-			printk(KERN_INFO "%s: Trying to open at 16 Mbps as requested\n", olympic_priv->olympic_card_name);
-		writew(GPR_16MBPS, olympic_mmio+GPR);
-	} else if (olympic_priv->olympic_ring_speed == 4) {
-		if (olympic_priv->olympic_message_level) 
-			printk(KERN_INFO "%s: Trying to open at 4 Mbps as requested\n", olympic_priv->olympic_card_name) ; 
-		writew(0, olympic_mmio+GPR);
-	} 
-	
-	writew(readw(olympic_mmio+GPR)|GPR_NEPTUNE_BF,olympic_mmio+GPR);
-
-#if OLYMPIC_DEBUG
-	printk("GPR = %x\n",readw(olympic_mmio + GPR) ) ; 
-#endif
-	/* Solo has been paused to meet the Cardbus power
-	 * specs if the adapter is cardbus. Check to 
-	 * see its been paused and then restart solo. The
-	 * adapter should set the pause bit within 1 second.
-	 */
-
-	if(!(readl(olympic_mmio+BCTL) & BCTL_MODE_INDICATOR)) { 
-		t=jiffies;
-		while (!(readl(olympic_mmio+CLKCTL) & CLKCTL_PAUSE)) {
-			schedule() ; 
-			if(time_after(jiffies, t + 2*HZ)) {
-				printk(KERN_ERR "IBM Cardbus tokenring adapter not responsing.\n") ; 
-				return -ENODEV;
-			}
-		}
-		writel(readl(olympic_mmio+CLKCTL) & ~CLKCTL_PAUSE, olympic_mmio+CLKCTL) ; 
-	}
-	
-	/* start solo init */
-	writel((1<<15),olympic_mmio+SISR_MASK_SUM);
-
-	t=jiffies;
-	while(!((readl(olympic_mmio+SISR_RR)) & SISR_SRB_REPLY)) {
-		schedule();		
-		if(time_after(jiffies, t + 15*HZ)) {
-			printk(KERN_ERR "IBM PCI tokenring card not responding.\n");
-			return -ENODEV;
-		}
-	}
-	
-	writel(readw(olympic_mmio+LAPWWO),olympic_mmio+LAPA);
-
-#if OLYMPIC_DEBUG
-	printk("LAPWWO: %x, LAPA: %x\n",readl(olympic_mmio+LAPWWO), readl(olympic_mmio+LAPA));
-#endif
-
-	init_srb=olympic_priv->olympic_lap + ((readw(olympic_mmio+LAPWWO)) & (~0xf800));
-
-#if OLYMPIC_DEBUG		
-{
-	int i;
-	printk("init_srb(%p): ",init_srb);
-	for(i=0;i<20;i++)
-		printk("%x ",readb(init_srb+i));
-	printk("\n");
-}
-#endif	
-	if(readw(init_srb+6)) {
-		printk(KERN_INFO "tokenring card initialization failed. errorcode : %x\n",readw(init_srb+6));
-		return -ENODEV;
-	}
-
-	if (olympic_priv->olympic_message_level) {
-		if ( readb(init_srb +2) & 0x40) { 
-			printk(KERN_INFO "Olympic: Adapter is FDX capable.\n") ;
-		} else { 
-			printk(KERN_INFO "Olympic: Adapter cannot do FDX.\n");
-		}
-	}
-  
-	uaa_addr=swab16(readw(init_srb+8));
-
-#if OLYMPIC_DEBUG
-	printk("UAA resides at %x\n",uaa_addr);
-#endif
-
-	writel(uaa_addr,olympic_mmio+LAPA);
-	adapter_addr=olympic_priv->olympic_lap + (uaa_addr & (~0xf800));
-
-	memcpy_fromio(&dev->dev_addr[0], adapter_addr,6);
-
-#if OLYMPIC_DEBUG
-	printk("adapter address: %pM\n", dev->dev_addr);
-#endif
-
-	olympic_priv->olympic_addr_table_addr = swab16(readw(init_srb + 12)); 
-	olympic_priv->olympic_parms_addr = swab16(readw(init_srb + 14)); 
-
-	return 0;
-
-}
-
-static int olympic_open(struct net_device *dev)
-{
-	struct olympic_private *olympic_priv=netdev_priv(dev);
-	u8 __iomem *olympic_mmio=olympic_priv->olympic_mmio,*init_srb;
-	unsigned long flags, t;
-	int i, open_finished = 1 ;
-	u8 resp, err;
-
-	DECLARE_WAITQUEUE(wait,current) ; 
-
-	olympic_init(dev);
-
-	if (request_irq(dev->irq, olympic_interrupt, IRQF_SHARED , "olympic",
-			dev))
-		return -EAGAIN;
-
-#if OLYMPIC_DEBUG
-	printk("BMCTL: %x\n",readl(olympic_mmio+BMCTL_SUM));
-	printk("pending ints: %x\n",readl(olympic_mmio+SISR_RR));
-#endif
-
-	writel(SISR_MI,olympic_mmio+SISR_MASK_SUM);
-
-	writel(SISR_MI | SISR_SRB_REPLY, olympic_mmio+SISR_MASK); /* more ints later, doesn't stop arb cmd interrupt */
-
-	writel(LISR_LIE,olympic_mmio+LISR); /* more ints later */
-
-	/* adapter is closed, so SRB is pointed to by LAPWWO */
-
-	writel(readw(olympic_mmio+LAPWWO),olympic_mmio+LAPA);
-	init_srb=olympic_priv->olympic_lap + ((readw(olympic_mmio+LAPWWO)) & (~0xf800));
-	
-#if OLYMPIC_DEBUG
-	printk("LAPWWO: %x, LAPA: %x\n",readw(olympic_mmio+LAPWWO), readl(olympic_mmio+LAPA));
-	printk("SISR Mask = %04x\n", readl(olympic_mmio+SISR_MASK));
-	printk("Before the open command\n");
-#endif	
-	do {
-		memset_io(init_srb,0,SRB_COMMAND_SIZE);
-
-		writeb(SRB_OPEN_ADAPTER,init_srb) ; 	/* open */
-		writeb(OLYMPIC_CLEAR_RET_CODE,init_srb+2);
-
-		/* If Network Monitor, instruct card to copy MAC frames through the ARB */
-		if (olympic_priv->olympic_network_monitor) 
-			writew(swab16(OPEN_ADAPTER_ENABLE_FDX | OPEN_ADAPTER_PASS_ADC_MAC | OPEN_ADAPTER_PASS_ATT_MAC | OPEN_ADAPTER_PASS_BEACON), init_srb+8);
-		else
-			writew(swab16(OPEN_ADAPTER_ENABLE_FDX), init_srb+8);
-	
-		/* Test OR of first 3 bytes as its totally possible for 
-		 * someone to set the first 2 bytes to be zero, although this 
-		 * is an error, the first byte must have bit 6 set to 1  */
-
-		if (olympic_priv->olympic_laa[0] | olympic_priv->olympic_laa[1] | olympic_priv->olympic_laa[2]) {
-			writeb(olympic_priv->olympic_laa[0],init_srb+12);
-			writeb(olympic_priv->olympic_laa[1],init_srb+13);
-			writeb(olympic_priv->olympic_laa[2],init_srb+14);
-			writeb(olympic_priv->olympic_laa[3],init_srb+15);
-			writeb(olympic_priv->olympic_laa[4],init_srb+16);
-			writeb(olympic_priv->olympic_laa[5],init_srb+17);
-			memcpy(dev->dev_addr,olympic_priv->olympic_laa,dev->addr_len) ;  
-		} 	
-		writeb(1,init_srb+30);
-
-		spin_lock_irqsave(&olympic_priv->olympic_lock,flags);	
-		olympic_priv->srb_queued=1;
-
-		writel(LISR_SRB_CMD,olympic_mmio+LISR_SUM);
-		spin_unlock_irqrestore(&olympic_priv->olympic_lock,flags);
-
-		t = jiffies ; 
-	
-		add_wait_queue(&olympic_priv->srb_wait,&wait) ;
-		set_current_state(TASK_INTERRUPTIBLE) ; 
- 
- 		while(olympic_priv->srb_queued) {        
-			schedule() ; 
-        		if(signal_pending(current))	{            
-				printk(KERN_WARNING "%s: Signal received in open.\n",
-                			dev->name);
-            			printk(KERN_WARNING "SISR=%x LISR=%x\n",
-                			readl(olympic_mmio+SISR),
-                			readl(olympic_mmio+LISR));
-            			olympic_priv->srb_queued=0;
-            			break;
-        		}
-			if (time_after(jiffies, t + 10*HZ)) {
-				printk(KERN_WARNING "%s: SRB timed out.\n",dev->name);
-				olympic_priv->srb_queued=0;
-				break ; 
-			} 
-			set_current_state(TASK_INTERRUPTIBLE) ; 
-    		}
-		remove_wait_queue(&olympic_priv->srb_wait,&wait) ; 
-		set_current_state(TASK_RUNNING) ; 
-		olympic_priv->srb_queued = 0 ; 
-#if OLYMPIC_DEBUG
-		printk("init_srb(%p): ",init_srb);
-		for(i=0;i<20;i++)
-			printk("%02x ",readb(init_srb+i));
-		printk("\n");
-#endif
-		
-		/* If we get the same return response as we set, the interrupt wasn't raised and the open
-                 * timed out.
-		 */
-
-		switch (resp = readb(init_srb+2)) {
-		case OLYMPIC_CLEAR_RET_CODE:
-			printk(KERN_WARNING "%s: Adapter Open time out or error.\n", dev->name) ; 
-			goto out;
-		case 0:
-			open_finished = 1;
-			break;
-		case 0x07:
-			if (!olympic_priv->olympic_ring_speed && open_finished) { /* Autosense , first time around */
-				printk(KERN_WARNING "%s: Retrying at different ring speed\n", dev->name);
-				open_finished = 0 ;  
-				continue;
-			}
-
-			err = readb(init_srb+7);
-
-			if (!olympic_priv->olympic_ring_speed && ((err & 0x0f) == 0x0d)) { 
-				printk(KERN_WARNING "%s: Tried to autosense ring speed with no monitors present\n",dev->name);
-				printk(KERN_WARNING "%s: Please try again with a specified ring speed\n",dev->name);
-			} else {
-				printk(KERN_WARNING "%s: %s - %s\n", dev->name,
-					open_maj_error[(err & 0xf0) >> 4],
-					open_min_error[(err & 0x0f)]);
-			}
-			goto out;
-
-		case 0x32:
-			printk(KERN_WARNING "%s: Invalid LAA: %pM\n",
-			       dev->name, olympic_priv->olympic_laa);
-			goto out;
-
-		default:
-			printk(KERN_WARNING "%s: Bad OPEN response: %x\n", dev->name, resp);
-			goto out;
-
-		}
-	} while (!(open_finished)) ; /* Will only loop if ring speed mismatch re-open attempted && autosense is on */	
-
-	if (readb(init_srb+18) & (1<<3)) 
-		if (olympic_priv->olympic_message_level) 
-			printk(KERN_INFO "%s: Opened in FDX Mode\n",dev->name);
-
-	if (readb(init_srb+18) & (1<<1))
-		olympic_priv->olympic_ring_speed = 100 ; 
-	else if (readb(init_srb+18) & 1)
-		olympic_priv->olympic_ring_speed = 16 ; 
-	else
-		olympic_priv->olympic_ring_speed = 4 ; 
-
-	if (olympic_priv->olympic_message_level) 
-		printk(KERN_INFO "%s: Opened in %d Mbps mode\n",dev->name, olympic_priv->olympic_ring_speed);
-
-	olympic_priv->asb = swab16(readw(init_srb+8));
-	olympic_priv->srb = swab16(readw(init_srb+10));
-	olympic_priv->arb = swab16(readw(init_srb+12));
-	olympic_priv->trb = swab16(readw(init_srb+16));
-
-	olympic_priv->olympic_receive_options = 0x01 ; 
-	olympic_priv->olympic_copy_all_options = 0 ; 
-	
-	/* setup rx ring */
-	
-	writel((3<<16),olympic_mmio+BMCTL_RWM); /* Ensure end of frame generated interrupts */ 
-
-	writel(BMCTL_RX_DIS|3,olympic_mmio+BMCTL_RWM); /* Yes, this the enables RX channel */
-
-	for(i=0;i<OLYMPIC_RX_RING_SIZE;i++) {
-
-		struct sk_buff *skb;
-		
-		skb=dev_alloc_skb(olympic_priv->pkt_buf_sz);
-		if(skb == NULL)
-			break;
-
-		skb->dev = dev;
-
-		olympic_priv->olympic_rx_ring[i].buffer = cpu_to_le32(pci_map_single(olympic_priv->pdev, 
-							  skb->data,olympic_priv->pkt_buf_sz, PCI_DMA_FROMDEVICE)) ; 
-		olympic_priv->olympic_rx_ring[i].res_length = cpu_to_le32(olympic_priv->pkt_buf_sz); 
-		olympic_priv->rx_ring_skb[i]=skb;
-	}
-
-	if (i==0) {
-		printk(KERN_WARNING "%s: Not enough memory to allocate rx buffers. Adapter disabled\n",dev->name);
-		goto out;
-	}
-
-	olympic_priv->rx_ring_dma_addr = pci_map_single(olympic_priv->pdev,olympic_priv->olympic_rx_ring, 
-					 sizeof(struct olympic_rx_desc) * OLYMPIC_RX_RING_SIZE, PCI_DMA_TODEVICE);
-	writel(olympic_priv->rx_ring_dma_addr, olympic_mmio+RXDESCQ);
-	writel(olympic_priv->rx_ring_dma_addr, olympic_mmio+RXCDA);
-	writew(i, olympic_mmio+RXDESCQCNT);
-		
-	olympic_priv->rx_status_ring_dma_addr = pci_map_single(olympic_priv->pdev, olympic_priv->olympic_rx_status_ring, 
-						sizeof(struct olympic_rx_status) * OLYMPIC_RX_RING_SIZE, PCI_DMA_FROMDEVICE);
-	writel(olympic_priv->rx_status_ring_dma_addr, olympic_mmio+RXSTATQ);
-	writel(olympic_priv->rx_status_ring_dma_addr, olympic_mmio+RXCSA);
-	
- 	olympic_priv->rx_ring_last_received = OLYMPIC_RX_RING_SIZE - 1;	/* last processed rx status */
-	olympic_priv->rx_status_last_received = OLYMPIC_RX_RING_SIZE - 1;  
-
-	writew(i, olympic_mmio+RXSTATQCNT);
-
-#if OLYMPIC_DEBUG 
-	printk("# of rx buffers: %d, RXENQ: %x\n",i, readw(olympic_mmio+RXENQ));
-	printk("RXCSA: %x, rx_status_ring[0]: %p\n",readl(olympic_mmio+RXCSA),&olympic_priv->olympic_rx_status_ring[0]);
-	printk(" stat_ring[1]: %p, stat_ring[2]: %p, stat_ring[3]: %p\n", &(olympic_priv->olympic_rx_status_ring[1]), &(olympic_priv->olympic_rx_status_ring[2]), &(olympic_priv->olympic_rx_status_ring[3]) );
-	printk(" stat_ring[4]: %p, stat_ring[5]: %p, stat_ring[6]: %p\n", &(olympic_priv->olympic_rx_status_ring[4]), &(olympic_priv->olympic_rx_status_ring[5]), &(olympic_priv->olympic_rx_status_ring[6]) );
-	printk(" stat_ring[7]: %p\n", &(olympic_priv->olympic_rx_status_ring[7])  );
-
-	printk("RXCDA: %x, rx_ring[0]: %p\n",readl(olympic_mmio+RXCDA),&olympic_priv->olympic_rx_ring[0]);
-	printk("Rx_ring_dma_addr = %08x, rx_status_dma_addr = %08x\n",
-		olympic_priv->rx_ring_dma_addr,olympic_priv->rx_status_ring_dma_addr) ; 
-#endif
-
-	writew((((readw(olympic_mmio+RXENQ)) & 0x8000) ^ 0x8000) | i,olympic_mmio+RXENQ);
-
-#if OLYMPIC_DEBUG 
-	printk("# of rx buffers: %d, RXENQ: %x\n",i, readw(olympic_mmio+RXENQ));
-	printk("RXCSA: %x, rx_ring[0]: %p\n",readl(olympic_mmio+RXCSA),&olympic_priv->olympic_rx_status_ring[0]);
-	printk("RXCDA: %x, rx_ring[0]: %p\n",readl(olympic_mmio+RXCDA),&olympic_priv->olympic_rx_ring[0]);
-#endif 
-
-	writel(SISR_RX_STATUS | SISR_RX_NOBUF,olympic_mmio+SISR_MASK_SUM);
-
-	/* setup tx ring */
-
-	writel(BMCTL_TX1_DIS,olympic_mmio+BMCTL_RWM); /* Yes, this enables TX channel 1 */
-	for(i=0;i<OLYMPIC_TX_RING_SIZE;i++) 
-		olympic_priv->olympic_tx_ring[i].buffer=cpu_to_le32(0xdeadbeef);
-
-	olympic_priv->free_tx_ring_entries=OLYMPIC_TX_RING_SIZE;
-	olympic_priv->tx_ring_dma_addr = pci_map_single(olympic_priv->pdev,olympic_priv->olympic_tx_ring,
-					 sizeof(struct olympic_tx_desc) * OLYMPIC_TX_RING_SIZE,PCI_DMA_TODEVICE) ; 
-	writel(olympic_priv->tx_ring_dma_addr, olympic_mmio+TXDESCQ_1);
-	writel(olympic_priv->tx_ring_dma_addr, olympic_mmio+TXCDA_1);
-	writew(OLYMPIC_TX_RING_SIZE, olympic_mmio+TXDESCQCNT_1);
-	
-	olympic_priv->tx_status_ring_dma_addr = pci_map_single(olympic_priv->pdev, olympic_priv->olympic_tx_status_ring,
-						sizeof(struct olympic_tx_status) * OLYMPIC_TX_RING_SIZE, PCI_DMA_FROMDEVICE);
-	writel(olympic_priv->tx_status_ring_dma_addr,olympic_mmio+TXSTATQ_1);
-	writel(olympic_priv->tx_status_ring_dma_addr,olympic_mmio+TXCSA_1);
-	writew(OLYMPIC_TX_RING_SIZE,olympic_mmio+TXSTATQCNT_1);
-		
-	olympic_priv->tx_ring_free=0; /* next entry in tx ring to use */
-	olympic_priv->tx_ring_last_status=OLYMPIC_TX_RING_SIZE-1; /* last processed tx status */
-
-	writel(0xffffffff, olympic_mmio+EISR_RWM) ; /* clean the eisr */
-	writel(0,olympic_mmio+EISR) ; 
-	writel(EISR_MASK_OPTIONS,olympic_mmio+EISR_MASK) ; /* enables most of the TX error interrupts */
-	writel(SISR_TX1_EOF | SISR_ADAPTER_CHECK | SISR_ARB_CMD | SISR_TRB_REPLY | SISR_ASB_FREE | SISR_ERR,olympic_mmio+SISR_MASK_SUM);
-
-#if OLYMPIC_DEBUG 
-	printk("BMCTL: %x\n",readl(olympic_mmio+BMCTL_SUM));
-	printk("SISR MASK: %x\n",readl(olympic_mmio+SISR_MASK));
-#endif
-
-	if (olympic_priv->olympic_network_monitor) { 
-		u8 __iomem *oat;
-		u8 __iomem *opt;
-		u8 addr[6];
-		oat = (olympic_priv->olympic_lap + olympic_priv->olympic_addr_table_addr);
-		opt = (olympic_priv->olympic_lap + olympic_priv->olympic_parms_addr);
-
-		for (i = 0; i < 6; i++)
-			addr[i] = readb(oat+offsetof(struct olympic_adapter_addr_table,node_addr)+i);
-		printk("%s: Node Address: %pM\n", dev->name, addr);
-		printk("%s: Functional Address: %02x:%02x:%02x:%02x\n",dev->name, 
-			readb(oat+offsetof(struct olympic_adapter_addr_table,func_addr)), 
-			readb(oat+offsetof(struct olympic_adapter_addr_table,func_addr)+1),
-			readb(oat+offsetof(struct olympic_adapter_addr_table,func_addr)+2),
-			readb(oat+offsetof(struct olympic_adapter_addr_table,func_addr)+3));
-
-		for (i = 0; i < 6; i++)
-			addr[i] = readb(opt+offsetof(struct olympic_parameters_table, up_node_addr)+i);
-		printk("%s: NAUN Address: %pM\n", dev->name, addr);
-	}
-	
-	netif_start_queue(dev);
-	return 0;
-
-out:
-	free_irq(dev->irq, dev);
-	return -EIO;
-}	
-
-/*
- *	When we enter the rx routine we do not know how many frames have been 
- *	queued on the rx channel.  Therefore we start at the next rx status
- *	position and travel around the receive ring until we have completed
- *	all the frames.
- *
- *	This means that we may process the frame before we receive the end
- *	of frame interrupt. This is why we always test the status instead
- *	of blindly processing the next frame.
- *
- *	We also remove the last 4 bytes from the packet as well, these are
- *	just token ring trailer info and upset protocols that don't check 
- *	their own length, i.e. SNA. 
- *	
- */
-static void olympic_rx(struct net_device *dev)
-{
-	struct olympic_private *olympic_priv=netdev_priv(dev);
-	u8 __iomem *olympic_mmio=olympic_priv->olympic_mmio;
-	struct olympic_rx_status *rx_status;
-	struct olympic_rx_desc *rx_desc ; 
-	int rx_ring_last_received,length, buffer_cnt, cpy_length, frag_len;
-	struct sk_buff *skb, *skb2;
-	int i;
-
-	rx_status=&(olympic_priv->olympic_rx_status_ring[(olympic_priv->rx_status_last_received + 1) & (OLYMPIC_RX_RING_SIZE - 1)]) ; 
- 
-	while (rx_status->status_buffercnt) { 
-                u32 l_status_buffercnt;
-
-		olympic_priv->rx_status_last_received++ ;
-		olympic_priv->rx_status_last_received &= (OLYMPIC_RX_RING_SIZE -1);
-#if OLYMPIC_DEBUG
-		printk("rx status: %x rx len: %x\n", le32_to_cpu(rx_status->status_buffercnt), le32_to_cpu(rx_status->fragmentcnt_framelen));
-#endif
-		length = le32_to_cpu(rx_status->fragmentcnt_framelen) & 0xffff;
-		buffer_cnt = le32_to_cpu(rx_status->status_buffercnt) & 0xffff; 
-		i = buffer_cnt ; /* Need buffer_cnt later for rxenq update */ 
-		frag_len = le32_to_cpu(rx_status->fragmentcnt_framelen) >> 16; 
-
-#if OLYMPIC_DEBUG 
-		printk("length: %x, frag_len: %x, buffer_cnt: %x\n", length, frag_len, buffer_cnt);
-#endif
-                l_status_buffercnt = le32_to_cpu(rx_status->status_buffercnt);
-		if(l_status_buffercnt & 0xC0000000) {
-			if (l_status_buffercnt & 0x3B000000) {
-				if (olympic_priv->olympic_message_level) {
-					if (l_status_buffercnt & (1<<29))  /* Rx Frame Truncated */
-						printk(KERN_WARNING "%s: Rx Frame Truncated\n",dev->name);
-					if (l_status_buffercnt & (1<<28)) /*Rx receive overrun */
-						printk(KERN_WARNING "%s: Rx Frame Receive overrun\n",dev->name);
-					if (l_status_buffercnt & (1<<27)) /* No receive buffers */
-						printk(KERN_WARNING "%s: No receive buffers\n",dev->name);
-					if (l_status_buffercnt & (1<<25)) /* Receive frame error detect */
-						printk(KERN_WARNING "%s: Receive frame error detect\n",dev->name);
-					if (l_status_buffercnt & (1<<24)) /* Received Error Detect */
-						printk(KERN_WARNING "%s: Received Error Detect\n",dev->name);
-				} 
-				olympic_priv->rx_ring_last_received += i ; 
-				olympic_priv->rx_ring_last_received &= (OLYMPIC_RX_RING_SIZE -1) ; 
-				dev->stats.rx_errors++;
-			} else {	
-			
-				if (buffer_cnt == 1) {
-					skb = dev_alloc_skb(max_t(int, olympic_priv->pkt_buf_sz,length)) ; 
-				} else {
-					skb = dev_alloc_skb(length) ; 
-				}
-
-				if (skb == NULL) {
-					printk(KERN_WARNING "%s: Not enough memory to copy packet to upper layers.\n",dev->name) ;
-					dev->stats.rx_dropped++;
-					/* Update counters even though we don't transfer the frame */
-					olympic_priv->rx_ring_last_received += i ; 
-					olympic_priv->rx_ring_last_received &= (OLYMPIC_RX_RING_SIZE -1) ;  
-				} else  {
-					/* Optimise based upon number of buffers used. 
-			   	   	   If only one buffer is used we can simply swap the buffers around.
-			   	   	   If more than one then we must use the new buffer and copy the information
-			   	   	   first. Ideally all frames would be in a single buffer, this can be tuned by
-                               	   	   altering the buffer size. If the length of the packet is less than
-					   1500 bytes we're going to copy it over anyway to stop packets getting
-					   dropped from sockets with buffers smaller than our pkt_buf_sz. */
-				
- 					if (buffer_cnt==1) {
-						olympic_priv->rx_ring_last_received++ ; 
-						olympic_priv->rx_ring_last_received &= (OLYMPIC_RX_RING_SIZE -1);
-						rx_ring_last_received = olympic_priv->rx_ring_last_received ;
-						if (length > 1500) { 
-							skb2=olympic_priv->rx_ring_skb[rx_ring_last_received] ; 
-							/* unmap buffer */
-							pci_unmap_single(olympic_priv->pdev,
-								le32_to_cpu(olympic_priv->olympic_rx_ring[rx_ring_last_received].buffer), 
-								olympic_priv->pkt_buf_sz,PCI_DMA_FROMDEVICE) ; 
-							skb_put(skb2,length-4);
-							skb2->protocol = tr_type_trans(skb2,dev);
-							olympic_priv->olympic_rx_ring[rx_ring_last_received].buffer = 
-								cpu_to_le32(pci_map_single(olympic_priv->pdev, skb->data, 
-								olympic_priv->pkt_buf_sz, PCI_DMA_FROMDEVICE));
-							olympic_priv->olympic_rx_ring[rx_ring_last_received].res_length = 
-								cpu_to_le32(olympic_priv->pkt_buf_sz); 
-							olympic_priv->rx_ring_skb[rx_ring_last_received] = skb ; 
-							netif_rx(skb2) ; 
-						} else { 
-							pci_dma_sync_single_for_cpu(olympic_priv->pdev,
-								le32_to_cpu(olympic_priv->olympic_rx_ring[rx_ring_last_received].buffer),
-								olympic_priv->pkt_buf_sz,PCI_DMA_FROMDEVICE) ; 
-							skb_copy_from_linear_data(olympic_priv->rx_ring_skb[rx_ring_last_received],
-								      skb_put(skb,length - 4),
-								      length - 4);
-							pci_dma_sync_single_for_device(olympic_priv->pdev,
-								le32_to_cpu(olympic_priv->olympic_rx_ring[rx_ring_last_received].buffer),
-								olympic_priv->pkt_buf_sz,PCI_DMA_FROMDEVICE) ;
-							skb->protocol = tr_type_trans(skb,dev) ; 
-							netif_rx(skb) ; 
-						} 
-					} else {
-						do { /* Walk the buffers */ 
-							olympic_priv->rx_ring_last_received++ ; 
-							olympic_priv->rx_ring_last_received &= (OLYMPIC_RX_RING_SIZE -1);
-							rx_ring_last_received = olympic_priv->rx_ring_last_received ; 
-							pci_dma_sync_single_for_cpu(olympic_priv->pdev,
-								le32_to_cpu(olympic_priv->olympic_rx_ring[rx_ring_last_received].buffer),
-								olympic_priv->pkt_buf_sz,PCI_DMA_FROMDEVICE) ; 
-							rx_desc = &(olympic_priv->olympic_rx_ring[rx_ring_last_received]);
-							cpy_length = (i == 1 ? frag_len : le32_to_cpu(rx_desc->res_length)); 
-							skb_copy_from_linear_data(olympic_priv->rx_ring_skb[rx_ring_last_received],
-								      skb_put(skb, cpy_length),
-								      cpy_length);
-							pci_dma_sync_single_for_device(olympic_priv->pdev,
-								le32_to_cpu(olympic_priv->olympic_rx_ring[rx_ring_last_received].buffer),
-								olympic_priv->pkt_buf_sz,PCI_DMA_FROMDEVICE) ;
-						} while (--i) ; 
-						skb_trim(skb,skb->len-4) ; 
-						skb->protocol = tr_type_trans(skb,dev);
-						netif_rx(skb) ; 
-					} 
-					dev->stats.rx_packets++ ;
-					dev->stats.rx_bytes += length ;
-				} /* if skb == null */
-			} /* If status & 0x3b */
-
-		} else { /*if buffercnt & 0xC */
-			olympic_priv->rx_ring_last_received += i ; 
-			olympic_priv->rx_ring_last_received &= (OLYMPIC_RX_RING_SIZE - 1) ; 
-		} 
-
-		rx_status->fragmentcnt_framelen = 0 ; 
-		rx_status->status_buffercnt = 0 ; 
-		rx_status = &(olympic_priv->olympic_rx_status_ring[(olympic_priv->rx_status_last_received+1) & (OLYMPIC_RX_RING_SIZE -1) ]);
-
-		writew((((readw(olympic_mmio+RXENQ)) & 0x8000) ^ 0x8000) |  buffer_cnt , olympic_mmio+RXENQ); 
-	} /* while */
-
-}
-
-static void olympic_freemem(struct net_device *dev) 
-{ 
-	struct olympic_private *olympic_priv=netdev_priv(dev);
-	int i;
-			
-	for(i=0;i<OLYMPIC_RX_RING_SIZE;i++) {
-		if (olympic_priv->rx_ring_skb[olympic_priv->rx_status_last_received] != NULL) {
-			dev_kfree_skb_irq(olympic_priv->rx_ring_skb[olympic_priv->rx_status_last_received]);
-			olympic_priv->rx_ring_skb[olympic_priv->rx_status_last_received] = NULL;
-		}
-		if (olympic_priv->olympic_rx_ring[olympic_priv->rx_status_last_received].buffer != cpu_to_le32(0xdeadbeef)) {
-			pci_unmap_single(olympic_priv->pdev, 
-			le32_to_cpu(olympic_priv->olympic_rx_ring[olympic_priv->rx_status_last_received].buffer),
-			olympic_priv->pkt_buf_sz, PCI_DMA_FROMDEVICE);
-		}
-		olympic_priv->rx_status_last_received++;
-		olympic_priv->rx_status_last_received&=OLYMPIC_RX_RING_SIZE-1;
-	}
-	/* unmap rings */
-	pci_unmap_single(olympic_priv->pdev, olympic_priv->rx_status_ring_dma_addr, 
-		sizeof(struct olympic_rx_status) * OLYMPIC_RX_RING_SIZE, PCI_DMA_FROMDEVICE);
-	pci_unmap_single(olympic_priv->pdev, olympic_priv->rx_ring_dma_addr,
-		sizeof(struct olympic_rx_desc) * OLYMPIC_RX_RING_SIZE, PCI_DMA_TODEVICE);
-
-	pci_unmap_single(olympic_priv->pdev, olympic_priv->tx_status_ring_dma_addr, 
-		sizeof(struct olympic_tx_status) * OLYMPIC_TX_RING_SIZE, PCI_DMA_FROMDEVICE);
-	pci_unmap_single(olympic_priv->pdev, olympic_priv->tx_ring_dma_addr, 
-		sizeof(struct olympic_tx_desc) * OLYMPIC_TX_RING_SIZE, PCI_DMA_TODEVICE);
-
-	return ; 
-}
- 
-static irqreturn_t olympic_interrupt(int irq, void *dev_id) 
-{
-	struct net_device *dev= (struct net_device *)dev_id;
-	struct olympic_private *olympic_priv=netdev_priv(dev);
-	u8 __iomem *olympic_mmio=olympic_priv->olympic_mmio;
-	u32 sisr;
-	u8 __iomem *adapter_check_area ; 
-	
-	/* 
-	 *  Read sisr but don't reset it yet. 
-	 *  The indication bit may have been set but the interrupt latch
-	 *  bit may not be set, so we'd lose the interrupt later. 
-	 */ 
-	sisr=readl(olympic_mmio+SISR) ; 
-	if (!(sisr & SISR_MI)) /* Interrupt isn't for us */ 
-		return IRQ_NONE;
-	sisr=readl(olympic_mmio+SISR_RR) ;  /* Read & Reset sisr */ 
-
-	spin_lock(&olympic_priv->olympic_lock);
-
-	/* Hotswap gives us this on removal */
-	if (sisr == 0xffffffff) { 
-		printk(KERN_WARNING "%s: Hotswap adapter removal.\n",dev->name) ; 
-		spin_unlock(&olympic_priv->olympic_lock) ; 
-		return IRQ_NONE;
-	} 
-		
-	if (sisr & (SISR_SRB_REPLY | SISR_TX1_EOF | SISR_RX_STATUS | SISR_ADAPTER_CHECK |  
-			SISR_ASB_FREE | SISR_ARB_CMD | SISR_TRB_REPLY | SISR_RX_NOBUF | SISR_ERR)) {  
-	
-		/* If we ever get this the adapter is seriously dead. Only a reset is going to 
-		 * bring it back to life. We're talking pci bus errors and such like :( */ 
-		if((sisr & SISR_ERR) && (readl(olympic_mmio+EISR) & EISR_MASK_OPTIONS)) {
-			printk(KERN_ERR "Olympic: EISR Error, EISR=%08x\n",readl(olympic_mmio+EISR)) ; 
-			printk(KERN_ERR "The adapter must be reset to clear this condition.\n") ; 
-			printk(KERN_ERR "Please report this error to the driver maintainer and/\n") ; 
-			printk(KERN_ERR "or the linux-tr mailing list.\n") ; 
-			wake_up_interruptible(&olympic_priv->srb_wait);
-			spin_unlock(&olympic_priv->olympic_lock) ; 
-			return IRQ_HANDLED;
-		} /* SISR_ERR */
-
-		if(sisr & SISR_SRB_REPLY) {
-			if(olympic_priv->srb_queued==1) {
-				wake_up_interruptible(&olympic_priv->srb_wait);
-			} else if (olympic_priv->srb_queued==2) { 
-				olympic_srb_bh(dev) ; 
-			}
-			olympic_priv->srb_queued=0;
-		} /* SISR_SRB_REPLY */
-
-		/* We shouldn't ever miss the Tx interrupt, but the you never know, hence the loop to ensure
-		   we get all tx completions. */
-		if (sisr & SISR_TX1_EOF) {
-			while(olympic_priv->olympic_tx_status_ring[(olympic_priv->tx_ring_last_status + 1) & (OLYMPIC_TX_RING_SIZE-1)].status) { 
-				olympic_priv->tx_ring_last_status++;
-				olympic_priv->tx_ring_last_status &= (OLYMPIC_TX_RING_SIZE-1);
-				olympic_priv->free_tx_ring_entries++;
-				dev->stats.tx_bytes += olympic_priv->tx_ring_skb[olympic_priv->tx_ring_last_status]->len;
-				dev->stats.tx_packets++ ;
-				pci_unmap_single(olympic_priv->pdev, 
-					le32_to_cpu(olympic_priv->olympic_tx_ring[olympic_priv->tx_ring_last_status].buffer), 
-					olympic_priv->tx_ring_skb[olympic_priv->tx_ring_last_status]->len,PCI_DMA_TODEVICE);
-				dev_kfree_skb_irq(olympic_priv->tx_ring_skb[olympic_priv->tx_ring_last_status]);
-				olympic_priv->olympic_tx_ring[olympic_priv->tx_ring_last_status].buffer=cpu_to_le32(0xdeadbeef);
-				olympic_priv->olympic_tx_status_ring[olympic_priv->tx_ring_last_status].status=0;
-			}
-			netif_wake_queue(dev);
-		} /* SISR_TX1_EOF */
-	
-		if (sisr & SISR_RX_STATUS) {
-			olympic_rx(dev);
-		} /* SISR_RX_STATUS */
-	
-		if (sisr & SISR_ADAPTER_CHECK) {
-			netif_stop_queue(dev);
-			printk(KERN_WARNING "%s: Adapter Check Interrupt Raised, 8 bytes of information follow:\n", dev->name);
-			writel(readl(olympic_mmio+LAPWWC),olympic_mmio+LAPA);
-			adapter_check_area = olympic_priv->olympic_lap + ((readl(olympic_mmio+LAPWWC)) & (~0xf800)) ;
-			printk(KERN_WARNING "%s: Bytes %02x:%02x:%02x:%02x:%02x:%02x:%02x:%02x\n",dev->name, readb(adapter_check_area+0), readb(adapter_check_area+1), readb(adapter_check_area+2), readb(adapter_check_area+3), readb(adapter_check_area+4), readb(adapter_check_area+5), readb(adapter_check_area+6), readb(adapter_check_area+7)) ; 
-			spin_unlock(&olympic_priv->olympic_lock) ; 
-			return IRQ_HANDLED; 
-		} /* SISR_ADAPTER_CHECK */
-	
-		if (sisr & SISR_ASB_FREE) {
-			/* Wake up anything that is waiting for the asb response */  
-			if (olympic_priv->asb_queued) {
-				olympic_asb_bh(dev) ; 
-			}
-		} /* SISR_ASB_FREE */
-	
-		if (sisr & SISR_ARB_CMD) {
-			olympic_arb_cmd(dev) ; 
-		} /* SISR_ARB_CMD */
-	
-		if (sisr & SISR_TRB_REPLY) {
-			/* Wake up anything that is waiting for the trb response */
-			if (olympic_priv->trb_queued) {
-				wake_up_interruptible(&olympic_priv->trb_wait);
-			}
-			olympic_priv->trb_queued = 0 ; 
-		} /* SISR_TRB_REPLY */	
-	
-		if (sisr & SISR_RX_NOBUF) {
-			/* According to the documentation, we don't have to do anything, but trapping it keeps it out of
-                  	   	   /var/log/messages.  */
-		} /* SISR_RX_NOBUF */
-	} else { 
-		printk(KERN_WARNING "%s: Unexpected interrupt: %x\n",dev->name, sisr);
-		printk(KERN_WARNING "%s: SISR_MASK: %x\n",dev->name, readl(olympic_mmio+SISR_MASK)) ;
-	} /* One if the interrupts we want */
-	writel(SISR_MI,olympic_mmio+SISR_MASK_SUM);
-	
-	spin_unlock(&olympic_priv->olympic_lock) ; 
-	return IRQ_HANDLED;
-}	
-
-static netdev_tx_t olympic_xmit(struct sk_buff *skb,
-				      struct net_device *dev)
-{
-	struct olympic_private *olympic_priv=netdev_priv(dev);
-	u8 __iomem *olympic_mmio=olympic_priv->olympic_mmio;
-	unsigned long flags ; 
-
-	spin_lock_irqsave(&olympic_priv->olympic_lock, flags);
-
-	netif_stop_queue(dev);
-	
-	if(olympic_priv->free_tx_ring_entries) {
-		olympic_priv->olympic_tx_ring[olympic_priv->tx_ring_free].buffer = 
-			cpu_to_le32(pci_map_single(olympic_priv->pdev, skb->data, skb->len,PCI_DMA_TODEVICE));
-		olympic_priv->olympic_tx_ring[olympic_priv->tx_ring_free].status_length = cpu_to_le32(skb->len | (0x80000000));
-		olympic_priv->tx_ring_skb[olympic_priv->tx_ring_free]=skb;
-		olympic_priv->free_tx_ring_entries--;
-
-        	olympic_priv->tx_ring_free++;
-        	olympic_priv->tx_ring_free &= (OLYMPIC_TX_RING_SIZE-1);
-		writew((((readw(olympic_mmio+TXENQ_1)) & 0x8000) ^ 0x8000) | 1,olympic_mmio+TXENQ_1);
-		netif_wake_queue(dev);
-		spin_unlock_irqrestore(&olympic_priv->olympic_lock,flags);
-		return NETDEV_TX_OK;
-	} else {
-		spin_unlock_irqrestore(&olympic_priv->olympic_lock,flags);
-		return NETDEV_TX_BUSY;
-	} 
-
-}
-	
-
-static int olympic_close(struct net_device *dev) 
-{
-	struct olympic_private *olympic_priv=netdev_priv(dev);
-	u8 __iomem *olympic_mmio=olympic_priv->olympic_mmio,*srb;
-	unsigned long t,flags;
-
-	DECLARE_WAITQUEUE(wait,current) ; 
-
-	netif_stop_queue(dev);
-	
-	writel(olympic_priv->srb,olympic_mmio+LAPA);
-	srb=olympic_priv->olympic_lap + (olympic_priv->srb & (~0xf800));
-	
-    	writeb(SRB_CLOSE_ADAPTER,srb+0);
-	writeb(0,srb+1);
-	writeb(OLYMPIC_CLEAR_RET_CODE,srb+2);
-
-	add_wait_queue(&olympic_priv->srb_wait,&wait) ;
-	set_current_state(TASK_INTERRUPTIBLE) ; 
-
-	spin_lock_irqsave(&olympic_priv->olympic_lock,flags);
-	olympic_priv->srb_queued=1;
-
-	writel(LISR_SRB_CMD,olympic_mmio+LISR_SUM);
-	spin_unlock_irqrestore(&olympic_priv->olympic_lock,flags);
-
-	while(olympic_priv->srb_queued) {
-
-		t = schedule_timeout_interruptible(60*HZ);
-
-        	if(signal_pending(current))	{            
-			printk(KERN_WARNING "%s: SRB timed out.\n",dev->name);
-            		printk(KERN_WARNING "SISR=%x MISR=%x\n",readl(olympic_mmio+SISR),readl(olympic_mmio+LISR));
-            		olympic_priv->srb_queued=0;
-            		break;
-        	}
-
-		if (t == 0) { 
-			printk(KERN_WARNING "%s: SRB timed out. May not be fatal.\n",dev->name);
-		} 
-		olympic_priv->srb_queued=0;
-    	}
-	remove_wait_queue(&olympic_priv->srb_wait,&wait) ; 
-
-	olympic_priv->rx_status_last_received++;
-	olympic_priv->rx_status_last_received&=OLYMPIC_RX_RING_SIZE-1;
-
-	olympic_freemem(dev) ; 	
-
-	/* reset tx/rx fifo's and busmaster logic */
-
-	writel(readl(olympic_mmio+BCTL)|(3<<13),olympic_mmio+BCTL);
-	udelay(1);
-	writel(readl(olympic_mmio+BCTL)&~(3<<13),olympic_mmio+BCTL);
-
-#if OLYMPIC_DEBUG
-	{
-	int i ; 
-	printk("srb(%p): ",srb);
-	for(i=0;i<4;i++)
-		printk("%x ",readb(srb+i));
-	printk("\n");
-	}
-#endif
-	free_irq(dev->irq,dev);
-
-	return 0;
-	
-}
-
-static void olympic_set_rx_mode(struct net_device *dev) 
-{
-	struct olympic_private *olympic_priv = netdev_priv(dev);
-   	u8 __iomem *olympic_mmio = olympic_priv->olympic_mmio ; 
-	u8 options = 0; 
-	u8 __iomem *srb;
-	struct netdev_hw_addr *ha;
-	unsigned char dev_mc_address[4] ; 
-
-	writel(olympic_priv->srb,olympic_mmio+LAPA);
-	srb=olympic_priv->olympic_lap + (olympic_priv->srb & (~0xf800));
-	options = olympic_priv->olympic_copy_all_options; 
-
-	if (dev->flags&IFF_PROMISC)  
-		options |= 0x61 ;
-	else
-		options &= ~0x61 ; 
-
-	/* Only issue the srb if there is a change in options */
-
-	if ((options ^ olympic_priv->olympic_copy_all_options)) { 
-	
-		/* Now to issue the srb command to alter the copy.all.options */
-	
-		writeb(SRB_MODIFY_RECEIVE_OPTIONS,srb);
-		writeb(0,srb+1);
-		writeb(OLYMPIC_CLEAR_RET_CODE,srb+2);
-		writeb(0,srb+3);
-		writeb(olympic_priv->olympic_receive_options,srb+4);
-		writeb(options,srb+5);
-
-		olympic_priv->srb_queued=2; /* Can't sleep, use srb_bh */
-
-		writel(LISR_SRB_CMD,olympic_mmio+LISR_SUM);
-
-		olympic_priv->olympic_copy_all_options = options ;
-		
-		return ;  
-	} 
-
-	/* Set the functional addresses we need for multicast */
-
-	dev_mc_address[0] = dev_mc_address[1] = dev_mc_address[2] = dev_mc_address[3] = 0 ; 
-
-	netdev_for_each_mc_addr(ha, dev) {
-		dev_mc_address[0] |= ha->addr[2];
-		dev_mc_address[1] |= ha->addr[3];
-		dev_mc_address[2] |= ha->addr[4];
-		dev_mc_address[3] |= ha->addr[5];
-	}
-
-	writeb(SRB_SET_FUNC_ADDRESS,srb+0);
-	writeb(0,srb+1);
-	writeb(OLYMPIC_CLEAR_RET_CODE,srb+2);
-	writeb(0,srb+3);
-	writeb(0,srb+4);
-	writeb(0,srb+5);
-	writeb(dev_mc_address[0],srb+6);
-	writeb(dev_mc_address[1],srb+7);
-	writeb(dev_mc_address[2],srb+8);
-	writeb(dev_mc_address[3],srb+9);
-
-	olympic_priv->srb_queued = 2 ;
-	writel(LISR_SRB_CMD,olympic_mmio+LISR_SUM);
-
-}
-
-static void olympic_srb_bh(struct net_device *dev) 
-{ 
-	struct olympic_private *olympic_priv = netdev_priv(dev);
-   	u8 __iomem *olympic_mmio = olympic_priv->olympic_mmio ; 
-	u8 __iomem *srb;
-
-	writel(olympic_priv->srb,olympic_mmio+LAPA);
-	srb=olympic_priv->olympic_lap + (olympic_priv->srb & (~0xf800));
-
-	switch (readb(srb)) { 
-
-		/* SRB_MODIFY_RECEIVE_OPTIONS i.e. set_multicast_list options (promiscuous) 
-                 * At some point we should do something if we get an error, such as
-                 * resetting the IFF_PROMISC flag in dev
-		 */
-
-		case SRB_MODIFY_RECEIVE_OPTIONS:
-			switch (readb(srb+2)) { 
-				case 0x01:
-					printk(KERN_WARNING "%s: Unrecognized srb command\n",dev->name) ; 
-					break ; 
-				case 0x04:
-					printk(KERN_WARNING "%s: Adapter must be open for this operation, doh!!\n",dev->name);
-					break ; 
-				default:
-					if (olympic_priv->olympic_message_level) 
-						printk(KERN_WARNING "%s: Receive Options Modified to %x,%x\n",dev->name,olympic_priv->olympic_copy_all_options, olympic_priv->olympic_receive_options) ; 
-					break ; 	
-			} /* switch srb[2] */ 
-			break ;
-		
-		/* SRB_SET_GROUP_ADDRESS - Multicast group setting 
-                 */
-
-		case SRB_SET_GROUP_ADDRESS:
-			switch (readb(srb+2)) { 
-				case 0x00:
-					break ; 
-				case 0x01:
-					printk(KERN_WARNING "%s: Unrecognized srb command\n",dev->name);
-					break ;
-				case 0x04:
-					printk(KERN_WARNING "%s: Adapter must be open for this operation, doh!!\n",dev->name); 
-					break ;
-				case 0x3c:
-					printk(KERN_WARNING "%s: Group/Functional address indicator bits not set correctly\n",dev->name) ; 
-					break ;
-				case 0x3e: /* If we ever implement individual multicast addresses, will need to deal with this */
-					printk(KERN_WARNING "%s: Group address registers full\n",dev->name) ; 
-					break ;  
-				case 0x55:
-					printk(KERN_INFO "%s: Group Address already set.\n",dev->name) ; 
-					break ;
-				default:
-					break ; 
-			} /* switch srb[2] */ 
-			break ; 
-
-		/* SRB_RESET_GROUP_ADDRESS - Remove a multicast address from group list
- 		 */
-
-		case SRB_RESET_GROUP_ADDRESS:
-			switch (readb(srb+2)) { 
-				case 0x00:
-					break ; 
-				case 0x01:
-					printk(KERN_WARNING "%s: Unrecognized srb command\n",dev->name);
-					break ; 
-				case 0x04:
-					printk(KERN_WARNING "%s: Adapter must be open for this operation, doh!!\n",dev->name) ; 
-					break ; 
-				case 0x39: /* Must deal with this if individual multicast addresses used */
-					printk(KERN_INFO "%s: Group address not found\n",dev->name);
-					break ;
-				default:
-					break ; 
-			} /* switch srb[2] */
-			break ; 
-
-		
-		/* SRB_SET_FUNC_ADDRESS - Called by the set_rx_mode 
-		 */
-
-		case SRB_SET_FUNC_ADDRESS:
-			switch (readb(srb+2)) { 
-				case 0x00:
-					if (olympic_priv->olympic_message_level)
-						printk(KERN_INFO "%s: Functional Address Mask Set\n",dev->name);
-					break ;
-				case 0x01:
-					printk(KERN_WARNING "%s: Unrecognized srb command\n",dev->name);
-					break ; 
-				case 0x04:
-					printk(KERN_WARNING "%s: Adapter must be open for this operation, doh!!\n",dev->name) ; 
-					break ; 
-				default:
-					break ; 
-			} /* switch srb[2] */
-			break ; 
-	
-		/* SRB_READ_LOG - Read and reset the adapter error counters
- 		 */
-
-		case SRB_READ_LOG:
-			switch (readb(srb+2)) { 
-				case 0x00: 
-					if (olympic_priv->olympic_message_level) 
-						printk(KERN_INFO "%s: Read Log issued\n",dev->name) ; 
-					break ; 
-				case 0x01:
-					printk(KERN_WARNING "%s: Unrecognized srb command\n",dev->name);
-					break ; 
-				case 0x04:
-					printk(KERN_WARNING "%s: Adapter must be open for this operation, doh!!\n",dev->name) ; 
-					break ; 
-			
-			} /* switch srb[2] */
-			break ; 
-		
-		/* SRB_READ_SR_COUNTERS - Read and reset the source routing bridge related counters */
-
-		case SRB_READ_SR_COUNTERS:
-			switch (readb(srb+2)) { 
-				case 0x00: 
-					if (olympic_priv->olympic_message_level) 
-						printk(KERN_INFO "%s: Read Source Routing Counters issued\n",dev->name) ; 
-					break ; 
-				case 0x01:
-					printk(KERN_WARNING "%s: Unrecognized srb command\n",dev->name);
-					break ; 
-				case 0x04:
-					printk(KERN_WARNING "%s: Adapter must be open for this operation, doh!!\n",dev->name) ; 
-					break ; 
-				default:
-					break ; 
-			} /* switch srb[2] */
-			break ;
- 
-		default:
-			printk(KERN_WARNING "%s: Unrecognized srb bh return value.\n",dev->name);
-			break ; 
-	} /* switch srb[0] */
-
-} 
-
-static int olympic_set_mac_address (struct net_device *dev, void *addr) 
-{
-	struct sockaddr *saddr = addr ; 
-	struct olympic_private *olympic_priv = netdev_priv(dev);
-
-	if (netif_running(dev)) { 
-		printk(KERN_WARNING "%s: Cannot set mac/laa address while card is open\n", dev->name) ; 
-		return -EIO ; 
-	}
-
-	memcpy(olympic_priv->olympic_laa, saddr->sa_data,dev->addr_len) ; 
-	
-	if (olympic_priv->olympic_message_level) { 
- 		printk(KERN_INFO "%s: MAC/LAA Set to  = %x.%x.%x.%x.%x.%x\n",dev->name, olympic_priv->olympic_laa[0],
-		olympic_priv->olympic_laa[1], olympic_priv->olympic_laa[2],
-		olympic_priv->olympic_laa[3], olympic_priv->olympic_laa[4],
-		olympic_priv->olympic_laa[5]);
-	} 
-
-	return 0 ; 
-}
-
-static void olympic_arb_cmd(struct net_device *dev)
-{
-	struct olympic_private *olympic_priv = netdev_priv(dev);
-	u8 __iomem *olympic_mmio=olympic_priv->olympic_mmio;
-	u8 __iomem *arb_block, *asb_block, *srb  ; 
-	u8 header_len ; 
-	u16 frame_len, buffer_len ;
-	struct sk_buff *mac_frame ;  
-	u8 __iomem *buf_ptr ;
-	u8 __iomem *frame_data ;  
-	u16 buff_off ; 
-	u16 lan_status = 0, lan_status_diff  ; /* Initialize to stop compiler warning */
-	u8 fdx_prot_error ; 
-	u16 next_ptr;
-
-	arb_block = (olympic_priv->olympic_lap + olympic_priv->arb) ; 
-	asb_block = (olympic_priv->olympic_lap + olympic_priv->asb) ; 
-	srb = (olympic_priv->olympic_lap + olympic_priv->srb) ; 
-	
-	if (readb(arb_block+0) == ARB_RECEIVE_DATA) { /* Receive.data, MAC frames */
-
-		header_len = readb(arb_block+8) ; /* 802.5 Token-Ring Header Length */	
-		frame_len = swab16(readw(arb_block + 10)) ; 
-
-		buff_off = swab16(readw(arb_block + 6)) ;
-		
-		buf_ptr = olympic_priv->olympic_lap + buff_off ; 
-
-#if OLYMPIC_DEBUG
-{
-		int i;
-		frame_data = buf_ptr+offsetof(struct mac_receive_buffer,frame_data) ; 
-
-		for (i=0 ;  i < 14 ; i++) { 
-			printk("Loc %d = %02x\n",i,readb(frame_data + i)); 
-		}
-
-		printk("next %04x, fs %02x, len %04x\n",readw(buf_ptr+offsetof(struct mac_receive_buffer,next)), readb(buf_ptr+offsetof(struct mac_receive_buffer,frame_status)), readw(buf_ptr+offsetof(struct mac_receive_buffer,buffer_length)));
-}
-#endif 
-		mac_frame = dev_alloc_skb(frame_len) ; 
-		if (!mac_frame) {
-			printk(KERN_WARNING "%s: Memory squeeze, dropping frame.\n", dev->name);
-			goto drop_frame;
-		}
-
-		/* Walk the buffer chain, creating the frame */
-
-		do {
-			frame_data = buf_ptr+offsetof(struct mac_receive_buffer,frame_data) ; 
-			buffer_len = swab16(readw(buf_ptr+offsetof(struct mac_receive_buffer,buffer_length))); 
-			memcpy_fromio(skb_put(mac_frame, buffer_len), frame_data , buffer_len ) ;
-			next_ptr=readw(buf_ptr+offsetof(struct mac_receive_buffer,next)); 
-		} while (next_ptr && (buf_ptr=olympic_priv->olympic_lap + swab16(next_ptr)));
-
-		mac_frame->protocol = tr_type_trans(mac_frame, dev);
-
-		if (olympic_priv->olympic_network_monitor) { 
-			struct trh_hdr *mac_hdr;
-			printk(KERN_WARNING "%s: Received MAC Frame, details:\n",dev->name);
-			mac_hdr = tr_hdr(mac_frame);
-			printk(KERN_WARNING "%s: MAC Frame Dest. Addr: %pM\n",
-			       dev->name, mac_hdr->daddr);
-			printk(KERN_WARNING "%s: MAC Frame Srce. Addr: %pM\n",
-			       dev->name, mac_hdr->saddr);
-		}
-		netif_rx(mac_frame);
-
-drop_frame:
-		/* Now tell the card we have dealt with the received frame */
-
-		/* Set LISR Bit 1 */
-		writel(LISR_ARB_FREE,olympic_priv->olympic_mmio + LISR_SUM);
-
-		/* Is the ASB free ? */ 	
-		
-		if (readb(asb_block + 2) != 0xff) { 
-			olympic_priv->asb_queued = 1 ; 
-			writel(LISR_ASB_FREE_REQ,olympic_priv->olympic_mmio+LISR_SUM); 
-			return ; 	
-			/* Drop out and wait for the bottom half to be run */
-		}
-		
-		writeb(ASB_RECEIVE_DATA,asb_block); /* Receive data */
-		writeb(OLYMPIC_CLEAR_RET_CODE,asb_block+2); /* Necessary ?? */
-		writeb(readb(arb_block+6),asb_block+6); /* Must send the address back to the adapter */
-		writeb(readb(arb_block+7),asb_block+7); /* To let it know we have dealt with the data */		
-
-		writel(LISR_ASB_REPLY | LISR_ASB_FREE_REQ,olympic_priv->olympic_mmio+LISR_SUM);
-		
-		olympic_priv->asb_queued = 2 ; 
-	
-		return ; 	
-		
-	} else if (readb(arb_block) == ARB_LAN_CHANGE_STATUS) { /* Lan.change.status */
-		lan_status = swab16(readw(arb_block+6));
-		fdx_prot_error = readb(arb_block+8) ; 
-		
-		/* Issue ARB Free */
-		writel(LISR_ARB_FREE,olympic_priv->olympic_mmio+LISR_SUM);
-
-		lan_status_diff = olympic_priv->olympic_lan_status ^ lan_status ; 
-
-		if (lan_status_diff & (LSC_LWF | LSC_ARW | LSC_FPE | LSC_RR) ) { 
-			if (lan_status_diff & LSC_LWF) 
-					printk(KERN_WARNING "%s: Short circuit detected on the lobe\n",dev->name);
-			if (lan_status_diff & LSC_ARW) 
-					printk(KERN_WARNING "%s: Auto removal error\n",dev->name);
-			if (lan_status_diff & LSC_FPE)
-					printk(KERN_WARNING "%s: FDX Protocol Error\n",dev->name);
-			if (lan_status_diff & LSC_RR) 
-					printk(KERN_WARNING "%s: Force remove MAC frame received\n",dev->name);
-		
-			/* Adapter has been closed by the hardware */
-		
-			/* reset tx/rx fifo's and busmaster logic */
-
-			writel(readl(olympic_mmio+BCTL)|(3<<13),olympic_mmio+BCTL);
-			udelay(1);
-			writel(readl(olympic_mmio+BCTL)&~(3<<13),olympic_mmio+BCTL);
-			netif_stop_queue(dev);
-			olympic_priv->srb = readw(olympic_priv->olympic_lap + LAPWWO) ; 
-			printk(KERN_WARNING "%s: Adapter has been closed\n", dev->name);
-		} /* If serious error */
-		
-		if (olympic_priv->olympic_message_level) { 
-			if (lan_status_diff & LSC_SIG_LOSS) 
-					printk(KERN_WARNING "%s: No receive signal detected\n", dev->name);
-			if (lan_status_diff & LSC_HARD_ERR)
-					printk(KERN_INFO "%s: Beaconing\n",dev->name);
-			if (lan_status_diff & LSC_SOFT_ERR)
-					printk(KERN_WARNING "%s: Adapter transmitted Soft Error Report Mac Frame\n",dev->name);
-			if (lan_status_diff & LSC_TRAN_BCN) 
-					printk(KERN_INFO "%s: We are transmitting the beacon, aaah\n",dev->name);
-			if (lan_status_diff & LSC_SS) 
-					printk(KERN_INFO "%s: Single Station on the ring\n", dev->name);
-			if (lan_status_diff & LSC_RING_REC)
-					printk(KERN_INFO "%s: Ring recovery ongoing\n",dev->name);
-			if (lan_status_diff & LSC_FDX_MODE)
-					printk(KERN_INFO "%s: Operating in FDX mode\n",dev->name);
-		} 	
-		
-		if (lan_status_diff & LSC_CO) { 
-					
-				if (olympic_priv->olympic_message_level) 
-					printk(KERN_INFO "%s: Counter Overflow\n", dev->name);
-					
-				/* Issue READ.LOG command */
-
-				writeb(SRB_READ_LOG, srb);
-				writeb(0,srb+1);
-				writeb(OLYMPIC_CLEAR_RET_CODE,srb+2);
-				writeb(0,srb+3);
-				writeb(0,srb+4);
-				writeb(0,srb+5);
-					
-				olympic_priv->srb_queued=2; /* Can't sleep, use srb_bh */
-
-				writel(LISR_SRB_CMD,olympic_mmio+LISR_SUM);
-					
-		}
-
-		if (lan_status_diff & LSC_SR_CO) { 
-
-				if (olympic_priv->olympic_message_level)
-					printk(KERN_INFO "%s: Source routing counters overflow\n", dev->name);
-
-				/* Issue a READ.SR.COUNTERS */
-				
-				writeb(SRB_READ_SR_COUNTERS,srb);
-				writeb(0,srb+1);
-				writeb(OLYMPIC_CLEAR_RET_CODE,srb+2);
-				writeb(0,srb+3);
-				
-				olympic_priv->srb_queued=2; /* Can't sleep, use srb_bh */
-
-				writel(LISR_SRB_CMD,olympic_mmio+LISR_SUM);
-
-		}
-
-		olympic_priv->olympic_lan_status = lan_status ; 
-	
-	}  /* Lan.change.status */
-	else
-		printk(KERN_WARNING "%s: Unknown arb command\n", dev->name);
-}
-
-static void olympic_asb_bh(struct net_device *dev) 
-{
-	struct olympic_private *olympic_priv = netdev_priv(dev);
-	u8 __iomem *arb_block, *asb_block ; 
-
-	arb_block = (olympic_priv->olympic_lap + olympic_priv->arb) ; 
-	asb_block = (olympic_priv->olympic_lap + olympic_priv->asb) ; 
-
-	if (olympic_priv->asb_queued == 1) {   /* Dropped through the first time */
-
-		writeb(ASB_RECEIVE_DATA,asb_block); /* Receive data */
-		writeb(OLYMPIC_CLEAR_RET_CODE,asb_block+2); /* Necessary ?? */
-		writeb(readb(arb_block+6),asb_block+6); /* Must send the address back to the adapter */
-		writeb(readb(arb_block+7),asb_block+7); /* To let it know we have dealt with the data */		
-
-		writel(LISR_ASB_REPLY | LISR_ASB_FREE_REQ,olympic_priv->olympic_mmio+LISR_SUM);
-		olympic_priv->asb_queued = 2 ; 
-
-		return ; 
-	}
-
-	if (olympic_priv->asb_queued == 2) { 
-		switch (readb(asb_block+2)) {
-			case 0x01:
-				printk(KERN_WARNING "%s: Unrecognized command code\n", dev->name);
-				break ;
-			case 0x26:
-				printk(KERN_WARNING "%s: Unrecognized buffer address\n", dev->name);
-				break ;
-			case 0xFF:
-				/* Valid response, everything should be ok again */
-				break ;
-			default:
-				printk(KERN_WARNING "%s: Invalid return code in asb\n",dev->name);
-				break ;
-		}
-	}
-	olympic_priv->asb_queued = 0 ; 
-}
- 
-static int olympic_change_mtu(struct net_device *dev, int mtu) 
-{
-	struct olympic_private *olympic_priv = netdev_priv(dev);
-	u16 max_mtu ; 
-
-	if (olympic_priv->olympic_ring_speed == 4)
-		max_mtu = 4500 ; 
-	else
-		max_mtu = 18000 ; 
-	
-	if (mtu > max_mtu)
-		return -EINVAL ; 
-	if (mtu < 100) 
-		return -EINVAL ; 
-
-	dev->mtu = mtu ; 
-	olympic_priv->pkt_buf_sz = mtu + TR_HLEN ; 
-
-	return 0 ; 
-}
-
-static int olympic_proc_show(struct seq_file *m, void *v)
-{
-	struct net_device *dev = m->private;
-	struct olympic_private *olympic_priv=netdev_priv(dev);
-	u8 __iomem *oat = (olympic_priv->olympic_lap + olympic_priv->olympic_addr_table_addr) ; 
-	u8 __iomem *opt = (olympic_priv->olympic_lap + olympic_priv->olympic_parms_addr) ; 
-	u8 addr[6];
-	u8 addr2[6];
-	int i;
-
-	seq_printf(m,
-		"IBM Pit/Pit-Phy/Olympic Chipset Token Ring Adapter %s\n",dev->name);
-	seq_printf(m, "\n%6s: Adapter Address   : Node Address      : Functional Addr\n",
- 	   dev->name); 
-
-	for (i = 0 ; i < 6 ; i++)
-		addr[i] = readb(oat+offsetof(struct olympic_adapter_addr_table,node_addr) + i);
-
-	seq_printf(m, "%6s: %pM : %pM : %02x:%02x:%02x:%02x\n",
-	   dev->name,
-	   dev->dev_addr, addr,
-	   readb(oat+offsetof(struct olympic_adapter_addr_table,func_addr)), 
-	   readb(oat+offsetof(struct olympic_adapter_addr_table,func_addr)+1),
-	   readb(oat+offsetof(struct olympic_adapter_addr_table,func_addr)+2),
-	   readb(oat+offsetof(struct olympic_adapter_addr_table,func_addr)+3));
-	 
-	seq_printf(m, "\n%6s: Token Ring Parameters Table:\n", dev->name);
-
-	seq_printf(m, "%6s: Physical Addr : Up Node Address   : Poll Address      : AccPri : Auth Src : Att Code :\n",
-	  dev->name) ; 
-
-	for (i = 0 ; i < 6 ; i++)
-		addr[i] = readb(opt+offsetof(struct olympic_parameters_table, up_node_addr) + i);
-	for (i = 0 ; i < 6 ; i++)
-		addr2[i] =  readb(opt+offsetof(struct olympic_parameters_table, poll_addr) + i);
-
-	seq_printf(m, "%6s: %02x:%02x:%02x:%02x   : %pM : %pM : %04x   : %04x     :  %04x    :\n",
-	  dev->name,
-	  readb(opt+offsetof(struct olympic_parameters_table, phys_addr)),
-	  readb(opt+offsetof(struct olympic_parameters_table, phys_addr)+1),
-	  readb(opt+offsetof(struct olympic_parameters_table, phys_addr)+2),
-	  readb(opt+offsetof(struct olympic_parameters_table, phys_addr)+3),
-	  addr, addr2,
-	  swab16(readw(opt+offsetof(struct olympic_parameters_table, acc_priority))),
-	  swab16(readw(opt+offsetof(struct olympic_parameters_table, auth_source_class))),
-	  swab16(readw(opt+offsetof(struct olympic_parameters_table, att_code))));
-
-	seq_printf(m, "%6s: Source Address    : Bcn T : Maj. V : Lan St : Lcl Rg : Mon Err : Frame Correl : \n",
-	  dev->name) ; 
-	
-	for (i = 0 ; i < 6 ; i++)
-		addr[i] = readb(opt+offsetof(struct olympic_parameters_table, source_addr) + i);
-	seq_printf(m, "%6s: %pM : %04x  : %04x   : %04x   : %04x   : %04x    :     %04x     : \n",
-	  dev->name, addr,
-	  swab16(readw(opt+offsetof(struct olympic_parameters_table, beacon_type))),
-	  swab16(readw(opt+offsetof(struct olympic_parameters_table, major_vector))),
-	  swab16(readw(opt+offsetof(struct olympic_parameters_table, lan_status))),
-	  swab16(readw(opt+offsetof(struct olympic_parameters_table, local_ring))),
-	  swab16(readw(opt+offsetof(struct olympic_parameters_table, mon_error))),
-	  swab16(readw(opt+offsetof(struct olympic_parameters_table, frame_correl))));
-
-	seq_printf(m, "%6s: Beacon Details :  Tx  :  Rx  : NAUN Node Address : NAUN Node Phys : \n",
-	  dev->name) ; 
-
-	for (i = 0 ; i < 6 ; i++)
-		addr[i] = readb(opt+offsetof(struct olympic_parameters_table, beacon_naun) + i);
-	seq_printf(m, "%6s:                :  %02x  :  %02x  : %pM : %02x:%02x:%02x:%02x    : \n",
-	  dev->name,
-	  swab16(readw(opt+offsetof(struct olympic_parameters_table, beacon_transmit))),
-	  swab16(readw(opt+offsetof(struct olympic_parameters_table, beacon_receive))),
-	  addr,
-	  readb(opt+offsetof(struct olympic_parameters_table, beacon_phys)),
-	  readb(opt+offsetof(struct olympic_parameters_table, beacon_phys)+1),
-	  readb(opt+offsetof(struct olympic_parameters_table, beacon_phys)+2),
-	  readb(opt+offsetof(struct olympic_parameters_table, beacon_phys)+3));
-
-	return 0;
-}
-
-static int olympic_proc_open(struct inode *inode, struct file *file)
-{
-	return single_open(file, olympic_proc_show, PDE(inode)->data);
-}
-
-static const struct file_operations olympic_proc_ops = {
-	.open		= olympic_proc_open,
-	.read		= seq_read,
-	.llseek		= seq_lseek,
-	.release	= single_release,
-};
-
-static void __devexit olympic_remove_one(struct pci_dev *pdev) 
-{
-	struct net_device *dev = pci_get_drvdata(pdev) ; 
-	struct olympic_private *olympic_priv=netdev_priv(dev);
-
-	if (olympic_priv->olympic_network_monitor) { 
-		char proc_name[20] ; 
-		strcpy(proc_name,"olympic_") ;
-		strcat(proc_name,dev->name) ;
-		remove_proc_entry(proc_name,init_net.proc_net);
-	}
-	unregister_netdev(dev) ; 
-	iounmap(olympic_priv->olympic_mmio) ; 
-	iounmap(olympic_priv->olympic_lap) ; 
-	pci_release_regions(pdev) ;
-	pci_set_drvdata(pdev,NULL) ;  	
-	free_netdev(dev) ; 
-}
-
-static struct pci_driver olympic_driver = { 
-	.name		= "olympic",
-	.id_table	= olympic_pci_tbl,
-	.probe		= olympic_probe,
-	.remove		= __devexit_p(olympic_remove_one),
-};
-
-static int __init olympic_pci_init(void) 
-{
-	return pci_register_driver(&olympic_driver) ;
-}
-
-static void __exit olympic_pci_cleanup(void)
-{
-	pci_unregister_driver(&olympic_driver) ; 
-}	
-
-
-module_init(olympic_pci_init) ; 
-module_exit(olympic_pci_cleanup) ; 
-
-MODULE_LICENSE("GPL");
diff --git a/drivers/net/tokenring/olympic.h b/drivers/net/tokenring/olympic.h
deleted file mode 100644
index 30631bae4c9..00000000000
--- a/drivers/net/tokenring/olympic.h
+++ /dev/null
@@ -1,321 +0,0 @@
-/*
- *  olympic.h (c) 1999 Peter De Schrijver All Rights Reserved
- *                1999,2000 Mike Phillips (mikep@linuxtr.net)
- *
- *  Linux driver for IBM PCI tokenring cards based on the olympic and the PIT/PHY chipset.
- *
- *  Base Driver Skeleton:
- *      Written 1993-94 by Donald Becker.
- *
- *      Copyright 1993 United States Government as represented by the
- *      Director, National Security Agency.
- *
- *  This software may be used and distributed according to the terms
- *  of the GNU General Public License, incorporated herein by reference.
- */
-
-#define CID 0x4e
-
-#define BCTL 0x70
-#define BCTL_SOFTRESET (1<<15)
-#define BCTL_MIMREB (1<<6)
-#define BCTL_MODE_INDICATOR (1<<5)
-
-#define GPR 0x4a
-#define GPR_OPTI_BF (1<<6)
-#define GPR_NEPTUNE_BF (1<<4) 
-#define GPR_AUTOSENSE (1<<2)
-#define GPR_16MBPS (1<<3) 
-
-#define PAG 0x85
-#define LBC 0x8e
-
-#define LISR 0x10
-#define LISR_SUM 0x14
-#define LISR_RWM 0x18
-
-#define LISR_LIE (1<<15)
-#define LISR_SLIM (1<<13)
-#define LISR_SLI (1<<12)
-#define LISR_PCMSRMASK (1<<11)
-#define LISR_PCMSRINT (1<<10)
-#define LISR_WOLMASK (1<<9)
-#define LISR_WOL (1<<8)
-#define LISR_SRB_CMD (1<<5)
-#define LISR_ASB_REPLY (1<<4)
-#define LISR_ASB_FREE_REQ (1<<2)
-#define LISR_ARB_FREE (1<<1)
-#define LISR_TRB_FRAME (1<<0)
-
-#define SISR 0x20
-#define SISR_SUM 0x24
-#define SISR_RWM 0x28
-#define SISR_RR 0x2C
-#define SISR_RESMASK 0x30
-#define SISR_MASK 0x54
-#define SISR_MASK_SUM 0x58
-#define SISR_MASK_RWM 0x5C
-
-#define SISR_TX2_IDLE (1<<31)
-#define SISR_TX2_HALT (1<<29)
-#define SISR_TX2_EOF (1<<28)
-#define SISR_TX1_IDLE (1<<27)
-#define SISR_TX1_HALT (1<<25)
-#define SISR_TX1_EOF (1<<24)
-#define SISR_TIMEOUT (1<<23)
-#define SISR_RX_NOBUF (1<<22)
-#define SISR_RX_STATUS (1<<21)
-#define SISR_RX_HALT (1<<18)
-#define SISR_RX_EOF_EARLY (1<<16)
-#define SISR_MI (1<<15)
-#define SISR_PI (1<<13)
-#define SISR_ERR (1<<9)
-#define SISR_ADAPTER_CHECK (1<<6)
-#define SISR_SRB_REPLY (1<<5)
-#define SISR_ASB_FREE (1<<4)
-#define SISR_ARB_CMD (1<<3)
-#define SISR_TRB_REPLY (1<<2)
-
-#define EISR 0x34
-#define EISR_RWM 0x38
-#define EISR_MASK 0x3c
-#define EISR_MASK_OPTIONS 0x001FFF7F
-
-#define LAPA 0x60
-#define LAPWWO 0x64
-#define LAPWWC 0x68
-#define LAPCTL 0x6C
-#define LAIPD 0x78
-#define LAIPDDINC 0x7C
-
-#define TIMER 0x50
-
-#define CLKCTL 0x74
-#define CLKCTL_PAUSE (1<<15) 
-
-#define PM_CON 0x4
-
-#define BMCTL_SUM 0x40
-#define BMCTL_RWM 0x44
-#define BMCTL_TX2_DIS (1<<30) 
-#define BMCTL_TX1_DIS (1<<26) 
-#define BMCTL_RX_DIS (1<<22) 
-
-#define BMASR 0xcc
-
-#define RXDESCQ 0x90
-#define RXDESCQCNT 0x94
-#define RXCDA 0x98
-#define RXENQ 0x9C
-#define RXSTATQ 0xA0
-#define RXSTATQCNT 0xA4
-#define RXCSA 0xA8
-#define RXCLEN 0xAC
-#define RXHLEN 0xAE
-
-#define TXDESCQ_1 0xb0
-#define TXDESCQ_2 0xd0
-#define TXDESCQCNT_1 0xb4
-#define TXDESCQCNT_2 0xd4
-#define TXCDA_1 0xb8
-#define TXCDA_2 0xd8
-#define TXENQ_1 0xbc
-#define TXENQ_2 0xdc
-#define TXSTATQ_1 0xc0
-#define TXSTATQ_2 0xe0
-#define TXSTATQCNT_1 0xc4
-#define TXSTATQCNT_2 0xe4
-#define TXCSA_1 0xc8
-#define TXCSA_2 0xe8
-/* Cardbus */
-#define FERMASK 0xf4
-#define FERMASK_INT_BIT (1<<15)
-
-#define OLYMPIC_IO_SPACE 256
-
-#define SRB_COMMAND_SIZE 50
-
-#define OLYMPIC_MAX_ADAPTERS 8 /* 0x08 __MODULE_STRING can't hand 0xnn */
-
-/* Defines for LAN STATUS CHANGE reports */
-#define LSC_SIG_LOSS 0x8000
-#define LSC_HARD_ERR 0x4000
-#define LSC_SOFT_ERR 0x2000
-#define LSC_TRAN_BCN 0x1000
-#define LSC_LWF      0x0800
-#define LSC_ARW      0x0400
-#define LSC_FPE      0x0200
-#define LSC_RR       0x0100
-#define LSC_CO       0x0080
-#define LSC_SS       0x0040
-#define LSC_RING_REC 0x0020
-#define LSC_SR_CO    0x0010
-#define LSC_FDX_MODE 0x0004
-
-/* Defines for OPEN ADAPTER command */
-
-#define OPEN_ADAPTER_EXT_WRAP (1<<15)
-#define OPEN_ADAPTER_DIS_HARDEE (1<<14)
-#define OPEN_ADAPTER_DIS_SOFTERR (1<<13)
-#define OPEN_ADAPTER_PASS_ADC_MAC (1<<12)
-#define OPEN_ADAPTER_PASS_ATT_MAC (1<<11)
-#define OPEN_ADAPTER_ENABLE_EC (1<<10)
-#define OPEN_ADAPTER_CONTENDER (1<<8)
-#define OPEN_ADAPTER_PASS_BEACON (1<<7)
-#define OPEN_ADAPTER_ENABLE_FDX (1<<6)
-#define OPEN_ADAPTER_ENABLE_RPL (1<<5)
-#define OPEN_ADAPTER_INHIBIT_ETR (1<<4)
-#define OPEN_ADAPTER_INTERNAL_WRAP (1<<3)
-#define OPEN_ADAPTER_USE_OPTS2 (1<<0)
-
-#define OPEN_ADAPTER_2_ENABLE_ONNOW (1<<15)
-
-/* Defines for SRB Commands */
-
-#define SRB_ACCESS_REGISTER 0x1f
-#define SRB_CLOSE_ADAPTER 0x04
-#define SRB_CONFIGURE_BRIDGE 0x0c
-#define SRB_CONFIGURE_WAKEUP_EVENT 0x1a
-#define SRB_MODIFY_BRIDGE_PARMS 0x15
-#define SRB_MODIFY_OPEN_OPTIONS 0x01
-#define SRB_MODIFY_RECEIVE_OPTIONS 0x17
-#define SRB_NO_OPERATION 0x00
-#define SRB_OPEN_ADAPTER 0x03
-#define SRB_READ_LOG 0x08
-#define SRB_READ_SR_COUNTERS 0x16
-#define SRB_RESET_GROUP_ADDRESS 0x02
-#define SRB_SAVE_CONFIGURATION 0x1b
-#define SRB_SET_BRIDGE_PARMS 0x09
-#define SRB_SET_BRIDGE_TARGETS 0x10
-#define SRB_SET_FUNC_ADDRESS 0x07
-#define SRB_SET_GROUP_ADDRESS 0x06
-#define SRB_SET_GROUP_ADDR_OPTIONS 0x11
-#define SRB_UPDATE_WAKEUP_PATTERN 0x19
-
-/* Clear return code */
-
-#define OLYMPIC_CLEAR_RET_CODE 0xfe 
-
-/* ARB Commands */
-#define ARB_RECEIVE_DATA 0x81
-#define ARB_LAN_CHANGE_STATUS 0x84
-/* ASB Response commands */
-
-#define ASB_RECEIVE_DATA 0x81
-
-
-/* Olympic defaults for buffers */
- 
-#define OLYMPIC_RX_RING_SIZE 16 /* should be a power of 2 */
-#define OLYMPIC_TX_RING_SIZE 8 /* should be a power of 2 */
-
-#define PKT_BUF_SZ 4096 /* Default packet size */
-
-/* Olympic data structures */
-
-/* xxxx These structures are all little endian in hardware. */
-
-struct olympic_tx_desc {
-	__le32 buffer;
-	__le32 status_length;
-};
-
-struct olympic_tx_status {
-	__le32 status;
-};
-
-struct olympic_rx_desc {
-	__le32 buffer;
-	__le32 res_length; 
-};
-
-struct olympic_rx_status {
-	__le32 fragmentcnt_framelen;
-	__le32 status_buffercnt;
-};
-/* xxxx END These structures are all little endian in hardware. */
-/* xxxx There may be more, but I'm pretty sure about these */
-
-struct mac_receive_buffer {
-	__le16 next ; 
-	u8 padding ; 
-	u8 frame_status ;
-	__le16 buffer_length ; 
-	u8 frame_data ; 
-};
-
-struct olympic_private {
-	
-	u16 srb;      /* be16 */
-	u16 trb;      /* be16 */
-	u16 arb;      /* be16 */
-	u16 asb;      /* be16 */
-
-	u8 __iomem *olympic_mmio;
-	u8 __iomem *olympic_lap;
-	struct pci_dev *pdev ; 
-	const char *olympic_card_name;
-
-	spinlock_t olympic_lock ; 
-
-	volatile int srb_queued;    /* True if an SRB is still posted */	
-	wait_queue_head_t srb_wait;
-
-	volatile int asb_queued;    /* True if an ASB is posted */
-
-	volatile int trb_queued;   /* True if a TRB is posted */
-	wait_queue_head_t trb_wait ; 
-
-	/* These must be on a 4 byte boundary. */
-	struct olympic_rx_desc olympic_rx_ring[OLYMPIC_RX_RING_SIZE];
-	struct olympic_tx_desc olympic_tx_ring[OLYMPIC_TX_RING_SIZE];
-	struct olympic_rx_status olympic_rx_status_ring[OLYMPIC_RX_RING_SIZE];	
-	struct olympic_tx_status olympic_tx_status_ring[OLYMPIC_TX_RING_SIZE];	
-
-	struct sk_buff *tx_ring_skb[OLYMPIC_TX_RING_SIZE], *rx_ring_skb[OLYMPIC_RX_RING_SIZE];	
-	int tx_ring_free, tx_ring_last_status, rx_ring_last_received,rx_status_last_received, free_tx_ring_entries;
-
-	u16 olympic_lan_status ;
-	u8 olympic_ring_speed ;
-	u16 pkt_buf_sz ; 
-	u8 olympic_receive_options, olympic_copy_all_options,olympic_message_level, olympic_network_monitor;  
-	u16 olympic_addr_table_addr, olympic_parms_addr ; 
-	u8 olympic_laa[6] ; 
-	u32 rx_ring_dma_addr;
-	u32 rx_status_ring_dma_addr;
-	u32 tx_ring_dma_addr;
-	u32 tx_status_ring_dma_addr;
-};
-
-struct olympic_adapter_addr_table {
-
-	u8 node_addr[6] ; 
-	u8 reserved[4] ; 
-	u8 func_addr[4] ; 
-} ; 
-
-struct olympic_parameters_table { 
-	
-	u8  phys_addr[4] ; 
-	u8  up_node_addr[6] ; 
-	u8  up_phys_addr[4] ; 
-	u8  poll_addr[6] ; 
-	u16 reserved ; 
-	u16 acc_priority ; 
-	u16 auth_source_class ; 
-	u16 att_code ; 
-	u8  source_addr[6] ; 
-	u16 beacon_type ; 
-	u16 major_vector ; 
-	u16 lan_status ; 
-	u16 soft_error_time ; 
- 	u16 reserved1 ; 
-	u16 local_ring ; 
-	u16 mon_error ; 
-	u16 beacon_transmit ; 
-	u16 beacon_receive ; 
-	u16 frame_correl ; 
-	u8  beacon_naun[6] ; 
-	u32 reserved2 ; 
-	u8  beacon_phys[4] ; 	
-}; 
diff --git a/drivers/net/tokenring/proteon.c b/drivers/net/tokenring/proteon.c
deleted file mode 100644
index 62d90e40f9e..00000000000
--- a/drivers/net/tokenring/proteon.c
+++ /dev/null
@@ -1,422 +0,0 @@
-/*
- *  proteon.c: A network driver for Proteon ISA token ring cards.
- *
- *  Based on tmspci written 1999 by Adam Fritzler
- *  
- *  Written 2003 by Jochen Friedrich
- *
- *  This software may be used and distributed according to the terms
- *  of the GNU General Public License, incorporated herein by reference.
- *
- *  This driver module supports the following cards:
- *	- Proteon 1392, 1392+
- *
- *  Maintainer(s):
- *    AF        Adam Fritzler
- *    JF	Jochen Friedrich	jochen@scram.de
- *
- *  Modification History:
- *	02-Jan-03	JF	Created
- *
- */
-static const char version[] = "proteon.c: v1.00 02/01/2003 by Jochen Friedrich\n";
-
-#include <linux/module.h>
-#include <linux/kernel.h>
-#include <linux/delay.h>
-#include <linux/errno.h>
-#include <linux/pci.h>
-#include <linux/init.h>
-#include <linux/netdevice.h>
-#include <linux/trdevice.h>
-#include <linux/platform_device.h>
-
-#include <asm/io.h>
-#include <asm/irq.h>
-#include <asm/pci.h>
-#include <asm/dma.h>
-
-#include "tms380tr.h"
-
-#define PROTEON_IO_EXTENT 32
-
-/* A zero-terminated list of I/O addresses to be probed. */
-static unsigned int portlist[] __initdata = {
-	0x0A20, 0x0E20, 0x1A20, 0x1E20, 0x2A20, 0x2E20, 0x3A20, 0x3E20,// Prot.
-	0x4A20, 0x4E20, 0x5A20, 0x5E20, 0x6A20, 0x6E20, 0x7A20, 0x7E20,// Prot.
-	0x8A20, 0x8E20, 0x9A20, 0x9E20, 0xAA20, 0xAE20, 0xBA20, 0xBE20,// Prot.
-	0xCA20, 0xCE20, 0xDA20, 0xDE20, 0xEA20, 0xEE20, 0xFA20, 0xFE20,// Prot.
-	0
-};
-
-/* A zero-terminated list of IRQs to be probed. */
-static unsigned short irqlist[] = {
-	7, 6, 5, 4, 3, 12, 11, 10, 9,
-	0
-};
-
-/* A zero-terminated list of DMAs to be probed. */
-static int dmalist[] __initdata = {
-	5, 6, 7,
-	0
-};
-
-static char cardname[] = "Proteon 1392\0";
-static u64 dma_mask = ISA_MAX_ADDRESS;
-static int proteon_open(struct net_device *dev);
-static void proteon_read_eeprom(struct net_device *dev);
-static unsigned short proteon_setnselout_pins(struct net_device *dev);
-
-static unsigned short proteon_sifreadb(struct net_device *dev, unsigned short reg)
-{
-	return inb(dev->base_addr + reg);
-}
-
-static unsigned short proteon_sifreadw(struct net_device *dev, unsigned short reg)
-{
-	return inw(dev->base_addr + reg);
-}
-
-static void proteon_sifwriteb(struct net_device *dev, unsigned short val, unsigned short reg)
-{
-	outb(val, dev->base_addr + reg);
-}
-
-static void proteon_sifwritew(struct net_device *dev, unsigned short val, unsigned short reg)
-{
-	outw(val, dev->base_addr + reg);
-}
-
-static int __init proteon_probe1(struct net_device *dev, int ioaddr)
-{
-	unsigned char chk1, chk2;
-	int i;
-
-	if (!request_region(ioaddr, PROTEON_IO_EXTENT, cardname))
-		return -ENODEV;
-		
-
-	chk1 = inb(ioaddr + 0x1f);      /* Get Proteon ID reg 1 */
-	if (chk1 != 0x1f) 
-		goto nodev;
-
-	chk1 = inb(ioaddr + 0x1e) & 0x07;       /* Get Proteon ID reg 0 */
-	for (i=0; i<16; i++) {
-		chk2 = inb(ioaddr + 0x1e) & 0x07;
-		if (((chk1 + 1) & 0x07) != chk2)
-			goto nodev;
-		chk1 = chk2;
-	}
-
-	dev->base_addr = ioaddr;
-	return 0;
-nodev:
-	release_region(ioaddr, PROTEON_IO_EXTENT); 
-	return -ENODEV;
-}
-
-static struct net_device_ops proteon_netdev_ops __read_mostly;
-
-static int __init setup_card(struct net_device *dev, struct device *pdev)
-{
-	struct net_local *tp;
-        static int versionprinted;
-	const unsigned *port;
-	int j,err = 0;
-
-	if (!dev)
-		return -ENOMEM;
-
-	if (dev->base_addr)	/* probe specific location */
-		err = proteon_probe1(dev, dev->base_addr);
-	else {
-		for (port = portlist; *port; port++) {
-			err = proteon_probe1(dev, *port);
-			if (!err)
-				break;
-		}
-	}
-	if (err)
-		goto out5;
-
-	/* At this point we have found a valid card. */
-
-	if (versionprinted++ == 0)
-		printk(KERN_DEBUG "%s", version);
-
-	err = -EIO;
-	pdev->dma_mask = &dma_mask;
-	if (tmsdev_init(dev, pdev))
-		goto out4;
-
-	dev->base_addr &= ~3; 
-		
-	proteon_read_eeprom(dev);
-
-	printk(KERN_DEBUG "proteon.c:    Ring Station Address: %pM\n",
-	       dev->dev_addr);
-		
-	tp = netdev_priv(dev);
-	tp->setnselout = proteon_setnselout_pins;
-		
-	tp->sifreadb = proteon_sifreadb;
-	tp->sifreadw = proteon_sifreadw;
-	tp->sifwriteb = proteon_sifwriteb;
-	tp->sifwritew = proteon_sifwritew;
-	
-	memcpy(tp->ProductID, cardname, PROD_ID_SIZE + 1);
-
-	tp->tmspriv = NULL;
-
-	dev->netdev_ops = &proteon_netdev_ops;
-
-	if (dev->irq == 0)
-	{
-		for(j = 0; irqlist[j] != 0; j++)
-		{
-			dev->irq = irqlist[j];
-			if (!request_irq(dev->irq, tms380tr_interrupt, 0, 
-				cardname, dev))
-				break;
-                }
-		
-                if(irqlist[j] == 0)
-                {
-                        printk(KERN_INFO "proteon.c: AutoSelect no IRQ available\n");
-			goto out3;
-		}
-	}
-	else
-	{
-		for(j = 0; irqlist[j] != 0; j++)
-			if (irqlist[j] == dev->irq)
-				break;
-		if (irqlist[j] == 0)
-		{
-			printk(KERN_INFO "proteon.c: Illegal IRQ %d specified\n",
-				dev->irq);
-			goto out3;
-		}
-		if (request_irq(dev->irq, tms380tr_interrupt, 0, 
-			cardname, dev))
-		{
-                        printk(KERN_INFO "proteon.c: Selected IRQ %d not available\n",
-				dev->irq);
-			goto out3;
-		}
-	}
-
-	if (dev->dma == 0)
-	{
-		for(j = 0; dmalist[j] != 0; j++)
-		{
-			dev->dma = dmalist[j];
-                        if (!request_dma(dev->dma, cardname))
-				break;
-		}
-
-		if(dmalist[j] == 0)
-		{
-			printk(KERN_INFO "proteon.c: AutoSelect no DMA available\n");
-			goto out2;
-		}
-	}
-	else
-	{
-		for(j = 0; dmalist[j] != 0; j++)
-			if (dmalist[j] == dev->dma)
-				break;
-		if (dmalist[j] == 0)
-		{
-                        printk(KERN_INFO "proteon.c: Illegal DMA %d specified\n",
-				dev->dma);
-			goto out2;
-		}
-		if (request_dma(dev->dma, cardname))
-		{
-                        printk(KERN_INFO "proteon.c: Selected DMA %d not available\n",
-				dev->dma);
-			goto out2;
-		}
-	}
-
-	err = register_netdev(dev);
-	if (err)
-		goto out;
-
-	printk(KERN_DEBUG "%s:    IO: %#4lx  IRQ: %d  DMA: %d\n",
-	       dev->name, dev->base_addr, dev->irq, dev->dma);
-
-	return 0;
-out:
-	free_dma(dev->dma);
-out2:
-	free_irq(dev->irq, dev);
-out3:
-	tmsdev_term(dev);
-out4:
-	release_region(dev->base_addr, PROTEON_IO_EXTENT);
-out5:
-	return err;
-}
-
-/*
- * Reads MAC address from adapter RAM, which should've read it from
- * the onboard ROM.  
- *
- * Calling this on a board that does not support it can be a very
- * dangerous thing.  The Madge board, for instance, will lock your
- * machine hard when this is called.  Luckily, its supported in a
- * separate driver.  --ASF
- */
-static void proteon_read_eeprom(struct net_device *dev)
-{
-	int i;
-	
-	/* Address: 0000:0000 */
-	proteon_sifwritew(dev, 0, SIFADX);
-	proteon_sifwritew(dev, 0, SIFADR);	
-	
-	/* Read six byte MAC address data */
-	dev->addr_len = 6;
-	for(i = 0; i < 6; i++)
-		dev->dev_addr[i] = proteon_sifreadw(dev, SIFINC) >> 8;
-}
-
-static unsigned short proteon_setnselout_pins(struct net_device *dev)
-{
-	return 0;
-}
-
-static int proteon_open(struct net_device *dev)
-{  
-	struct net_local *tp = netdev_priv(dev);
-	unsigned short val = 0;
-	int i;
-
-	/* Proteon reset sequence */
-	outb(0, dev->base_addr + 0x11);
-	mdelay(20);
-	outb(0x04, dev->base_addr + 0x11);
-	mdelay(20);
-	outb(0, dev->base_addr + 0x11);
-	mdelay(100);
-
-	/* set control/status reg */
-	val = inb(dev->base_addr + 0x11);
-	val |= 0x78;
-	val &= 0xf9;
-	if(tp->DataRate == SPEED_4)
-		val |= 0x20;
-	else
-		val &= ~0x20;
-
-	outb(val, dev->base_addr + 0x11);
-	outb(0xff, dev->base_addr + 0x12);
-	for(i = 0; irqlist[i] != 0; i++)
-	{
-		if(irqlist[i] == dev->irq)
-			break;
-	}
-	val = i;
-	i = (7 - dev->dma) << 4;
-	val |= i;
-	outb(val, dev->base_addr + 0x13);
-
-	return tms380tr_open(dev);
-}
-
-#define ISATR_MAX_ADAPTERS 3
-
-static int io[ISATR_MAX_ADAPTERS];
-static int irq[ISATR_MAX_ADAPTERS];
-static int dma[ISATR_MAX_ADAPTERS];
-
-MODULE_LICENSE("GPL");
-
-module_param_array(io, int, NULL, 0);
-module_param_array(irq, int, NULL, 0);
-module_param_array(dma, int, NULL, 0);
-
-static struct platform_device *proteon_dev[ISATR_MAX_ADAPTERS];
-
-static struct platform_driver proteon_driver = {
-	.driver		= {
-		.name	= "proteon",
-	},
-};
-
-static int __init proteon_init(void)
-{
-	struct net_device *dev;
-	struct platform_device *pdev;
-	int i, num = 0, err = 0;
-
-	proteon_netdev_ops = tms380tr_netdev_ops;
-	proteon_netdev_ops.ndo_open = proteon_open;
-	proteon_netdev_ops.ndo_stop = tms380tr_close;
-
-	err = platform_driver_register(&proteon_driver);
-	if (err)
-		return err;
-
-	for (i = 0; i < ISATR_MAX_ADAPTERS ; i++) {
-		dev = alloc_trdev(sizeof(struct net_local));
-		if (!dev)
-			continue;
-
-		dev->base_addr = io[i];
-		dev->irq = irq[i];
-		dev->dma = dma[i];
-		pdev = platform_device_register_simple("proteon",
-			i, NULL, 0);
-		if (IS_ERR(pdev)) {
-			free_netdev(dev);
-			continue;
-		}
-		err = setup_card(dev, &pdev->dev);
-		if (!err) {
-			proteon_dev[i] = pdev;
-			platform_set_drvdata(pdev, dev);
-			++num;
-		} else {
-			platform_device_unregister(pdev);
-			free_netdev(dev);
-		}
-	}
-
-	printk(KERN_NOTICE "proteon.c: %d cards found.\n", num);
-	/* Probe for cards. */
-	if (num == 0) {
-		printk(KERN_NOTICE "proteon.c: No cards found.\n");
-		platform_driver_unregister(&proteon_driver);
-		return -ENODEV;
-	}
-	return 0;
-}
-
-static void __exit proteon_cleanup(void)
-{
-	struct net_device *dev;
-	int i;
-
-	for (i = 0; i < ISATR_MAX_ADAPTERS ; i++) {
-		struct platform_device *pdev = proteon_dev[i];
-		
-		if (!pdev)
-			continue;
-		dev = platform_get_drvdata(pdev);
-		unregister_netdev(dev);
-		release_region(dev->base_addr, PROTEON_IO_EXTENT);
-		free_irq(dev->irq, dev);
-		free_dma(dev->dma);
-		tmsdev_term(dev);
-		free_netdev(dev);
-		platform_set_drvdata(pdev, NULL);
-		platform_device_unregister(pdev);
-	}
-	platform_driver_unregister(&proteon_driver);
-}
-
-module_init(proteon_init);
-module_exit(proteon_cleanup);
diff --git a/drivers/net/tokenring/skisa.c b/drivers/net/tokenring/skisa.c
deleted file mode 100644
index ee11e93dc30..00000000000
--- a/drivers/net/tokenring/skisa.c
+++ /dev/null
@@ -1,432 +0,0 @@
-/*
- *  skisa.c: A network driver for SK-NET TMS380-based ISA token ring cards.
- *
- *  Based on tmspci written 1999 by Adam Fritzler
- *  
- *  Written 2000 by Jochen Friedrich
- *  Dedicated to my girlfriend Steffi Bopp
- *
- *  This software may be used and distributed according to the terms
- *  of the GNU General Public License, incorporated herein by reference.
- *
- *  This driver module supports the following cards:
- *	- SysKonnect TR4/16(+) ISA	(SK-4190)
- *
- *  Maintainer(s):
- *    AF        Adam Fritzler
- *    JF	Jochen Friedrich	jochen@scram.de
- *
- *  Modification History:
- *	14-Jan-01	JF	Created
- *	28-Oct-02	JF	Fixed probe of card for static compilation.
- *				Fixed module init to not make hotplug go wild.
- *	09-Nov-02	JF	Fixed early bail out on out of memory
- *				situations if multiple cards are found.
- *				Cleaned up some unnecessary console SPAM.
- *	09-Dec-02	JF	Fixed module reference counting.
- *	02-Jan-03	JF	Renamed to skisa.c
- *
- */
-static const char version[] = "skisa.c: v1.03 09/12/2002 by Jochen Friedrich\n";
-
-#include <linux/module.h>
-#include <linux/kernel.h>
-#include <linux/errno.h>
-#include <linux/pci.h>
-#include <linux/init.h>
-#include <linux/netdevice.h>
-#include <linux/trdevice.h>
-#include <linux/platform_device.h>
-
-#include <asm/io.h>
-#include <asm/irq.h>
-#include <asm/pci.h>
-#include <asm/dma.h>
-
-#include "tms380tr.h"
-
-#define SK_ISA_IO_EXTENT 32
-
-/* A zero-terminated list of I/O addresses to be probed. */
-static unsigned int portlist[] __initdata = {
-	0x0A20, 0x1A20, 0x0B20, 0x1B20, 0x0980, 0x1980, 0x0900, 0x1900,// SK
-	0
-};
-
-/* A zero-terminated list of IRQs to be probed. 
- * Used again after initial probe for sktr_chipset_init, called from sktr_open.
- */
-static const unsigned short irqlist[] = {
-	3, 5, 9, 10, 11, 12, 15,
-	0
-};
-
-/* A zero-terminated list of DMAs to be probed. */
-static int dmalist[] __initdata = {
-	5, 6, 7,
-	0
-};
-
-static char isa_cardname[] = "SK NET TR 4/16 ISA\0";
-static u64 dma_mask = ISA_MAX_ADDRESS;
-static int sk_isa_open(struct net_device *dev);
-static void sk_isa_read_eeprom(struct net_device *dev);
-static unsigned short sk_isa_setnselout_pins(struct net_device *dev);
-
-static unsigned short sk_isa_sifreadb(struct net_device *dev, unsigned short reg)
-{
-	return inb(dev->base_addr + reg);
-}
-
-static unsigned short sk_isa_sifreadw(struct net_device *dev, unsigned short reg)
-{
-	return inw(dev->base_addr + reg);
-}
-
-static void sk_isa_sifwriteb(struct net_device *dev, unsigned short val, unsigned short reg)
-{
-	outb(val, dev->base_addr + reg);
-}
-
-static void sk_isa_sifwritew(struct net_device *dev, unsigned short val, unsigned short reg)
-{
-	outw(val, dev->base_addr + reg);
-}
-
-
-static int __init sk_isa_probe1(struct net_device *dev, int ioaddr)
-{
-	unsigned char old, chk1, chk2;
-
-	if (!request_region(ioaddr, SK_ISA_IO_EXTENT, isa_cardname))
-		return -ENODEV;
-
-	old = inb(ioaddr + SIFADR);	/* Get the old SIFADR value */
-
-	chk1 = 0;	/* Begin with check value 0 */
-	do {
-		/* Write new SIFADR value */
-		outb(chk1, ioaddr + SIFADR);
-
-		/* Read, invert and write */
-		chk2 = inb(ioaddr + SIFADD);
-		chk2 ^= 0x0FE;
-		outb(chk2, ioaddr + SIFADR);
-
-		/* Read, invert and compare */
-		chk2 = inb(ioaddr + SIFADD);
-		chk2 ^= 0x0FE;
-
-		if(chk1 != chk2) {
-			release_region(ioaddr, SK_ISA_IO_EXTENT);
-			return -ENODEV;
-		}
-
-		chk1 -= 2;
-	} while(chk1 != 0);	/* Repeat 128 times (all byte values) */
-
-    	/* Restore the SIFADR value */
-	outb(old, ioaddr + SIFADR);
-
-	dev->base_addr = ioaddr;
-	return 0;
-}
-
-static struct net_device_ops sk_isa_netdev_ops __read_mostly;
-
-static int __init setup_card(struct net_device *dev, struct device *pdev)
-{
-	struct net_local *tp;
-        static int versionprinted;
-	const unsigned *port;
-	int j, err = 0;
-
-	if (!dev)
-		return -ENOMEM;
-
-	if (dev->base_addr)	/* probe specific location */
-		err = sk_isa_probe1(dev, dev->base_addr);
-	else {
-		for (port = portlist; *port; port++) {
-			err = sk_isa_probe1(dev, *port);
-			if (!err)
-				break;
-		}
-	}
-	if (err)
-		goto out5;
-
-	/* At this point we have found a valid card. */
-
-	if (versionprinted++ == 0)
-		printk(KERN_DEBUG "%s", version);
-
-	err = -EIO;
-	pdev->dma_mask = &dma_mask;
-	if (tmsdev_init(dev, pdev))
-		goto out4;
-
-	dev->base_addr &= ~3; 
-		
-	sk_isa_read_eeprom(dev);
-
-	printk(KERN_DEBUG "skisa.c:    Ring Station Address: %pM\n",
-	       dev->dev_addr);
-		
-	tp = netdev_priv(dev);
-	tp->setnselout = sk_isa_setnselout_pins;
-		
-	tp->sifreadb = sk_isa_sifreadb;
-	tp->sifreadw = sk_isa_sifreadw;
-	tp->sifwriteb = sk_isa_sifwriteb;
-	tp->sifwritew = sk_isa_sifwritew;
-	
-	memcpy(tp->ProductID, isa_cardname, PROD_ID_SIZE + 1);
-
-	tp->tmspriv = NULL;
-
-	dev->netdev_ops = &sk_isa_netdev_ops;
-
-	if (dev->irq == 0)
-	{
-		for(j = 0; irqlist[j] != 0; j++)
-		{
-			dev->irq = irqlist[j];
-			if (!request_irq(dev->irq, tms380tr_interrupt, 0, 
-				isa_cardname, dev))
-				break;
-                }
-		
-                if(irqlist[j] == 0)
-                {
-                        printk(KERN_INFO "skisa.c: AutoSelect no IRQ available\n");
-			goto out3;
-		}
-	}
-	else
-	{
-		for(j = 0; irqlist[j] != 0; j++)
-			if (irqlist[j] == dev->irq)
-				break;
-		if (irqlist[j] == 0)
-		{
-			printk(KERN_INFO "skisa.c: Illegal IRQ %d specified\n",
-				dev->irq);
-			goto out3;
-		}
-		if (request_irq(dev->irq, tms380tr_interrupt, 0, 
-			isa_cardname, dev))
-		{
-                        printk(KERN_INFO "skisa.c: Selected IRQ %d not available\n",
-				dev->irq);
-			goto out3;
-		}
-	}
-
-	if (dev->dma == 0)
-	{
-		for(j = 0; dmalist[j] != 0; j++)
-		{
-			dev->dma = dmalist[j];
-                        if (!request_dma(dev->dma, isa_cardname))
-				break;
-		}
-
-		if(dmalist[j] == 0)
-		{
-			printk(KERN_INFO "skisa.c: AutoSelect no DMA available\n");
-			goto out2;
-		}
-	}
-	else
-	{
-		for(j = 0; dmalist[j] != 0; j++)
-			if (dmalist[j] == dev->dma)
-				break;
-		if (dmalist[j] == 0)
-		{
-                        printk(KERN_INFO "skisa.c: Illegal DMA %d specified\n",
-				dev->dma);
-			goto out2;
-		}
-		if (request_dma(dev->dma, isa_cardname))
-		{
-                        printk(KERN_INFO "skisa.c: Selected DMA %d not available\n",
-				dev->dma);
-			goto out2;
-		}
-	}
-
-	err = register_netdev(dev);
-	if (err)
-		goto out;
-
-	printk(KERN_DEBUG "%s:    IO: %#4lx  IRQ: %d  DMA: %d\n",
-	       dev->name, dev->base_addr, dev->irq, dev->dma);
-
-	return 0;
-out:
-	free_dma(dev->dma);
-out2:
-	free_irq(dev->irq, dev);
-out3:
-	tmsdev_term(dev);
-out4:
-	release_region(dev->base_addr, SK_ISA_IO_EXTENT);
-out5:
-	return err;
-}
-
-/*
- * Reads MAC address from adapter RAM, which should've read it from
- * the onboard ROM.  
- *
- * Calling this on a board that does not support it can be a very
- * dangerous thing.  The Madge board, for instance, will lock your
- * machine hard when this is called.  Luckily, its supported in a
- * separate driver.  --ASF
- */
-static void sk_isa_read_eeprom(struct net_device *dev)
-{
-	int i;
-	
-	/* Address: 0000:0000 */
-	sk_isa_sifwritew(dev, 0, SIFADX);
-	sk_isa_sifwritew(dev, 0, SIFADR);	
-	
-	/* Read six byte MAC address data */
-	dev->addr_len = 6;
-	for(i = 0; i < 6; i++)
-		dev->dev_addr[i] = sk_isa_sifreadw(dev, SIFINC) >> 8;
-}
-
-static unsigned short sk_isa_setnselout_pins(struct net_device *dev)
-{
-	return 0;
-}
-
-static int sk_isa_open(struct net_device *dev)
-{  
-	struct net_local *tp = netdev_priv(dev);
-	unsigned short val = 0;
-	unsigned short oldval;
-	int i;
-
-	val = 0;
-	for(i = 0; irqlist[i] != 0; i++)
-	{
-		if(irqlist[i] == dev->irq)
-			break;
-	}
-
-	val |= CYCLE_TIME << 2;
-	val |= i << 4;
-	i = dev->dma - 5;
-	val |= i;
-	if(tp->DataRate == SPEED_4)
-		val |= LINE_SPEED_BIT;
-	else
-		val &= ~LINE_SPEED_BIT;
-	oldval = sk_isa_sifreadb(dev, POSREG);
-	/* Leave cycle bits alone */
-	oldval |= 0xf3;
-	val &= oldval;
-	sk_isa_sifwriteb(dev, val, POSREG);
-
-	return tms380tr_open(dev);
-}
-
-#define ISATR_MAX_ADAPTERS 3
-
-static int io[ISATR_MAX_ADAPTERS];
-static int irq[ISATR_MAX_ADAPTERS];
-static int dma[ISATR_MAX_ADAPTERS];
-
-MODULE_LICENSE("GPL");
-
-module_param_array(io, int, NULL, 0);
-module_param_array(irq, int, NULL, 0);
-module_param_array(dma, int, NULL, 0);
-
-static struct platform_device *sk_isa_dev[ISATR_MAX_ADAPTERS];
-
-static struct platform_driver sk_isa_driver = {
-	.driver		= {
-		.name	= "skisa",
-	},
-};
-
-static int __init sk_isa_init(void)
-{
-	struct net_device *dev;
-	struct platform_device *pdev;
-	int i, num = 0, err = 0;
-
-	sk_isa_netdev_ops = tms380tr_netdev_ops;
-	sk_isa_netdev_ops.ndo_open = sk_isa_open;
-	sk_isa_netdev_ops.ndo_stop = tms380tr_close;
-
-	err = platform_driver_register(&sk_isa_driver);
-	if (err)
-		return err;
-
-	for (i = 0; i < ISATR_MAX_ADAPTERS ; i++) {
-		dev = alloc_trdev(sizeof(struct net_local));
-		if (!dev)
-			continue;
-
-		dev->base_addr = io[i];
-		dev->irq = irq[i];
-		dev->dma = dma[i];
-		pdev = platform_device_register_simple("skisa",
-			i, NULL, 0);
-		if (IS_ERR(pdev)) {
-			free_netdev(dev);
-			continue;
-		}
-		err = setup_card(dev, &pdev->dev);
-		if (!err) {
-			sk_isa_dev[i] = pdev;
-			platform_set_drvdata(sk_isa_dev[i], dev);
-			++num;
-		} else {
-			platform_device_unregister(pdev);
-			free_netdev(dev);
-		}
-	}
-
-	printk(KERN_NOTICE "skisa.c: %d cards found.\n", num);
-	/* Probe for cards. */
-	if (num == 0) {
-		printk(KERN_NOTICE "skisa.c: No cards found.\n");
-		platform_driver_unregister(&sk_isa_driver);
-		return -ENODEV;
-	}
-	return 0;
-}
-
-static void __exit sk_isa_cleanup(void)
-{
-	struct net_device *dev;
-	int i;
-
-	for (i = 0; i < ISATR_MAX_ADAPTERS ; i++) {
-		struct platform_device *pdev = sk_isa_dev[i];
-
-		if (!pdev)
-			continue;
-		dev = platform_get_drvdata(pdev);
-		unregister_netdev(dev);
-		release_region(dev->base_addr, SK_ISA_IO_EXTENT);
-		free_irq(dev->irq, dev);
-		free_dma(dev->dma);
-		tmsdev_term(dev);
-		free_netdev(dev);
-		platform_set_drvdata(pdev, NULL);
-		platform_device_unregister(pdev);
-	}
-	platform_driver_unregister(&sk_isa_driver);
-}
-
-module_init(sk_isa_init);
-module_exit(sk_isa_cleanup);
diff --git a/drivers/net/tokenring/smctr.c b/drivers/net/tokenring/smctr.c
deleted file mode 100644
index cb35fb79e01..00000000000
--- a/drivers/net/tokenring/smctr.c
+++ /dev/null
@@ -1,5717 +0,0 @@
-/*
- *  smctr.c: A network driver for the SMC Token Ring Adapters.
- *
- *  Written by Jay Schulist <jschlst@samba.org>
- *
- *  This software may be used and distributed according to the terms
- *  of the GNU General Public License, incorporated herein by reference.
- *
- *  This device driver works with the following SMC adapters:
- *      - SMC TokenCard Elite   (8115T, chips 825/584)
- *      - SMC TokenCard Elite/A MCA (8115T/A, chips 825/594)
- *
- *  Source(s):
- *  	- SMC TokenCard SDK.
- *
- *  Maintainer(s):
- *    JS        Jay Schulist <jschlst@samba.org>
- *
- * Changes:
- *    07102000          JS      Fixed a timing problem in smctr_wait_cmd();
- *                              Also added a bit more discriptive error msgs.
- *    07122000          JS      Fixed problem with detecting a card with
- *				module io/irq/mem specified.
- *
- *  To do:
- *    1. Multicast support.
- *
- *  Initial 2.5 cleanup Alan Cox <alan@lxorguk.ukuu.org.uk>  2002/10/28
- */
-
-#include <linux/module.h>
-#include <linux/kernel.h>
-#include <linux/types.h>
-#include <linux/fcntl.h>
-#include <linux/interrupt.h>
-#include <linux/ptrace.h>
-#include <linux/ioport.h>
-#include <linux/in.h>
-#include <linux/string.h>
-#include <linux/time.h>
-#include <linux/errno.h>
-#include <linux/init.h>
-#include <linux/mca-legacy.h>
-#include <linux/delay.h>
-#include <linux/netdevice.h>
-#include <linux/etherdevice.h>
-#include <linux/skbuff.h>
-#include <linux/trdevice.h>
-#include <linux/bitops.h>
-#include <linux/firmware.h>
-
-#include <asm/io.h>
-#include <asm/dma.h>
-#include <asm/irq.h>
-
-#if BITS_PER_LONG == 64
-#error FIXME: driver does not support 64-bit platforms
-#endif
-
-#include "smctr.h"               /* Our Stuff */
-
-static const char version[] __initdata =
-	KERN_INFO "smctr.c: v1.4 7/12/00 by jschlst@samba.org\n";
-static const char cardname[] = "smctr";
-
-
-#define SMCTR_IO_EXTENT   20
-
-#ifdef CONFIG_MCA_LEGACY
-static unsigned int smctr_posid = 0x6ec6;
-#endif
-
-static int ringspeed;
-
-/* SMC Name of the Adapter. */
-static char smctr_name[] = "SMC TokenCard";
-static char *smctr_model = "Unknown";
-
-/* Use 0 for production, 1 for verification, 2 for debug, and
- * 3 for very verbose debug.
- */
-#ifndef SMCTR_DEBUG
-#define SMCTR_DEBUG 1
-#endif
-static unsigned int smctr_debug = SMCTR_DEBUG;
-
-/* smctr.c prototypes and functions are arranged alphabeticly 
- * for clearity, maintainability and pure old fashion fun. 
- */
-/* A */
-static int smctr_alloc_shared_memory(struct net_device *dev);
-
-/* B */
-static int smctr_bypass_state(struct net_device *dev);
-
-/* C */
-static int smctr_checksum_firmware(struct net_device *dev);
-static int __init smctr_chk_isa(struct net_device *dev);
-static int smctr_chg_rx_mask(struct net_device *dev);
-static int smctr_clear_int(struct net_device *dev);
-static int smctr_clear_trc_reset(int ioaddr);
-static int smctr_close(struct net_device *dev);
-
-/* D */
-static int smctr_decode_firmware(struct net_device *dev,
-				 const struct firmware *fw);
-static int smctr_disable_16bit(struct net_device *dev);
-static int smctr_disable_adapter_ctrl_store(struct net_device *dev);
-static int smctr_disable_bic_int(struct net_device *dev);
-
-/* E */
-static int smctr_enable_16bit(struct net_device *dev);
-static int smctr_enable_adapter_ctrl_store(struct net_device *dev);
-static int smctr_enable_adapter_ram(struct net_device *dev);
-static int smctr_enable_bic_int(struct net_device *dev);
-
-/* G */
-static int __init smctr_get_boardid(struct net_device *dev, int mca);
-static int smctr_get_group_address(struct net_device *dev);
-static int smctr_get_functional_address(struct net_device *dev);
-static unsigned int smctr_get_num_rx_bdbs(struct net_device *dev);
-static int smctr_get_physical_drop_number(struct net_device *dev);
-static __u8 *smctr_get_rx_pointer(struct net_device *dev, short queue);
-static int smctr_get_station_id(struct net_device *dev);
-static FCBlock *smctr_get_tx_fcb(struct net_device *dev, __u16 queue,
-        __u16 bytes_count);
-static int smctr_get_upstream_neighbor_addr(struct net_device *dev);
-
-/* H */
-static int smctr_hardware_send_packet(struct net_device *dev,
-        struct net_local *tp);
-/* I */
-static int smctr_init_acbs(struct net_device *dev);
-static int smctr_init_adapter(struct net_device *dev);
-static int smctr_init_card_real(struct net_device *dev);
-static int smctr_init_rx_bdbs(struct net_device *dev);
-static int smctr_init_rx_fcbs(struct net_device *dev);
-static int smctr_init_shared_memory(struct net_device *dev);
-static int smctr_init_tx_bdbs(struct net_device *dev);
-static int smctr_init_tx_fcbs(struct net_device *dev);
-static int smctr_internal_self_test(struct net_device *dev);
-static irqreturn_t smctr_interrupt(int irq, void *dev_id);
-static int smctr_issue_enable_int_cmd(struct net_device *dev,
-        __u16 interrupt_enable_mask);
-static int smctr_issue_int_ack(struct net_device *dev, __u16 iack_code,
-        __u16 ibits);
-static int smctr_issue_init_timers_cmd(struct net_device *dev);
-static int smctr_issue_init_txrx_cmd(struct net_device *dev);
-static int smctr_issue_insert_cmd(struct net_device *dev);
-static int smctr_issue_read_ring_status_cmd(struct net_device *dev);
-static int smctr_issue_read_word_cmd(struct net_device *dev, __u16 aword_cnt);
-static int smctr_issue_remove_cmd(struct net_device *dev);
-static int smctr_issue_resume_acb_cmd(struct net_device *dev);
-static int smctr_issue_resume_rx_bdb_cmd(struct net_device *dev, __u16 queue);
-static int smctr_issue_resume_rx_fcb_cmd(struct net_device *dev, __u16 queue);
-static int smctr_issue_resume_tx_fcb_cmd(struct net_device *dev, __u16 queue);
-static int smctr_issue_test_internal_rom_cmd(struct net_device *dev);
-static int smctr_issue_test_hic_cmd(struct net_device *dev);
-static int smctr_issue_test_mac_reg_cmd(struct net_device *dev);
-static int smctr_issue_trc_loopback_cmd(struct net_device *dev);
-static int smctr_issue_tri_loopback_cmd(struct net_device *dev);
-static int smctr_issue_write_byte_cmd(struct net_device *dev,
-        short aword_cnt, void *byte);
-static int smctr_issue_write_word_cmd(struct net_device *dev,
-        short aword_cnt, void *word);
-
-/* J */
-static int smctr_join_complete_state(struct net_device *dev);
-
-/* L */
-static int smctr_link_tx_fcbs_to_bdbs(struct net_device *dev);
-static int smctr_load_firmware(struct net_device *dev);
-static int smctr_load_node_addr(struct net_device *dev);
-static int smctr_lobe_media_test(struct net_device *dev);
-static int smctr_lobe_media_test_cmd(struct net_device *dev);
-static int smctr_lobe_media_test_state(struct net_device *dev);
-
-/* M */
-static int smctr_make_8025_hdr(struct net_device *dev,
-        MAC_HEADER *rmf, MAC_HEADER *tmf, __u16 ac_fc);
-static int smctr_make_access_pri(struct net_device *dev,
-        MAC_SUB_VECTOR *tsv);
-static int smctr_make_addr_mod(struct net_device *dev, MAC_SUB_VECTOR *tsv);
-static int smctr_make_auth_funct_class(struct net_device *dev,
-        MAC_SUB_VECTOR *tsv);
-static int smctr_make_corr(struct net_device *dev,
-        MAC_SUB_VECTOR *tsv, __u16 correlator);
-static int smctr_make_funct_addr(struct net_device *dev,
-        MAC_SUB_VECTOR *tsv);
-static int smctr_make_group_addr(struct net_device *dev,
-        MAC_SUB_VECTOR *tsv);
-static int smctr_make_phy_drop_num(struct net_device *dev,
-        MAC_SUB_VECTOR *tsv);
-static int smctr_make_product_id(struct net_device *dev, MAC_SUB_VECTOR *tsv);
-static int smctr_make_station_id(struct net_device *dev, MAC_SUB_VECTOR *tsv);
-static int smctr_make_ring_station_status(struct net_device *dev,
-        MAC_SUB_VECTOR *tsv);
-static int smctr_make_ring_station_version(struct net_device *dev,
-        MAC_SUB_VECTOR *tsv);
-static int smctr_make_tx_status_code(struct net_device *dev,
-        MAC_SUB_VECTOR *tsv, __u16 tx_fstatus);
-static int smctr_make_upstream_neighbor_addr(struct net_device *dev,
-        MAC_SUB_VECTOR *tsv);
-static int smctr_make_wrap_data(struct net_device *dev,
-        MAC_SUB_VECTOR *tsv);
-
-/* O */
-static int smctr_open(struct net_device *dev);
-static int smctr_open_tr(struct net_device *dev);
-
-/* P */
-struct net_device *smctr_probe(int unit);
-static int __init smctr_probe1(struct net_device *dev, int ioaddr);
-static int smctr_process_rx_packet(MAC_HEADER *rmf, __u16 size,
-        struct net_device *dev, __u16 rx_status);
-
-/* R */
-static int smctr_ram_memory_test(struct net_device *dev);
-static int smctr_rcv_chg_param(struct net_device *dev, MAC_HEADER *rmf,
-        __u16 *correlator);
-static int smctr_rcv_init(struct net_device *dev, MAC_HEADER *rmf,
-        __u16 *correlator);
-static int smctr_rcv_tx_forward(struct net_device *dev, MAC_HEADER *rmf);
-static int smctr_rcv_rq_addr_state_attch(struct net_device *dev,
-        MAC_HEADER *rmf, __u16 *correlator);
-static int smctr_rcv_unknown(struct net_device *dev, MAC_HEADER *rmf,
-        __u16 *correlator);
-static int smctr_reset_adapter(struct net_device *dev);
-static int smctr_restart_tx_chain(struct net_device *dev, short queue);
-static int smctr_ring_status_chg(struct net_device *dev);
-static int smctr_rx_frame(struct net_device *dev);
-
-/* S */
-static int smctr_send_dat(struct net_device *dev);
-static netdev_tx_t smctr_send_packet(struct sk_buff *skb,
-					   struct net_device *dev);
-static int smctr_send_lobe_media_test(struct net_device *dev);
-static int smctr_send_rpt_addr(struct net_device *dev, MAC_HEADER *rmf,
-        __u16 correlator);
-static int smctr_send_rpt_attch(struct net_device *dev, MAC_HEADER *rmf,
-        __u16 correlator);
-static int smctr_send_rpt_state(struct net_device *dev, MAC_HEADER *rmf,
-        __u16 correlator);
-static int smctr_send_rpt_tx_forward(struct net_device *dev,
-        MAC_HEADER *rmf, __u16 tx_fstatus);
-static int smctr_send_rsp(struct net_device *dev, MAC_HEADER *rmf,
-        __u16 rcode, __u16 correlator);
-static int smctr_send_rq_init(struct net_device *dev);
-static int smctr_send_tx_forward(struct net_device *dev, MAC_HEADER *rmf,
-        __u16 *tx_fstatus);
-static int smctr_set_auth_access_pri(struct net_device *dev,
-        MAC_SUB_VECTOR *rsv);
-static int smctr_set_auth_funct_class(struct net_device *dev,
-        MAC_SUB_VECTOR *rsv);
-static int smctr_set_corr(struct net_device *dev, MAC_SUB_VECTOR *rsv,
-	__u16 *correlator);
-static int smctr_set_error_timer_value(struct net_device *dev,
-        MAC_SUB_VECTOR *rsv);
-static int smctr_set_frame_forward(struct net_device *dev,
-        MAC_SUB_VECTOR *rsv, __u8 dc_sc);
-static int smctr_set_local_ring_num(struct net_device *dev,
-        MAC_SUB_VECTOR *rsv);
-static unsigned short smctr_set_ctrl_attention(struct net_device *dev);
-static void smctr_set_multicast_list(struct net_device *dev);
-static int smctr_set_page(struct net_device *dev, __u8 *buf);
-static int smctr_set_phy_drop(struct net_device *dev,
-        MAC_SUB_VECTOR *rsv);
-static int smctr_set_ring_speed(struct net_device *dev);
-static int smctr_set_rx_look_ahead(struct net_device *dev);
-static int smctr_set_trc_reset(int ioaddr);
-static int smctr_setup_single_cmd(struct net_device *dev,
-        __u16 command, __u16 subcommand);
-static int smctr_setup_single_cmd_w_data(struct net_device *dev,
-        __u16 command, __u16 subcommand);
-static char *smctr_malloc(struct net_device *dev, __u16 size);
-static int smctr_status_chg(struct net_device *dev);
-
-/* T */
-static void smctr_timeout(struct net_device *dev);
-static int smctr_trc_send_packet(struct net_device *dev, FCBlock *fcb,
-        __u16 queue);
-static __u16 smctr_tx_complete(struct net_device *dev, __u16 queue);
-static unsigned short smctr_tx_move_frame(struct net_device *dev,
-        struct sk_buff *skb, __u8 *pbuff, unsigned int bytes);
-
-/* U */
-static int smctr_update_err_stats(struct net_device *dev);
-static int smctr_update_rx_chain(struct net_device *dev, __u16 queue);
-static int smctr_update_tx_chain(struct net_device *dev, FCBlock *fcb,
-        __u16 queue);
-
-/* W */
-static int smctr_wait_cmd(struct net_device *dev);
-static int smctr_wait_while_cbusy(struct net_device *dev);
-
-#define TO_256_BYTE_BOUNDRY(X)  (((X + 0xff) & 0xff00) - X)
-#define TO_PARAGRAPH_BOUNDRY(X) (((X + 0x0f) & 0xfff0) - X)
-#define PARAGRAPH_BOUNDRY(X)    smctr_malloc(dev, TO_PARAGRAPH_BOUNDRY(X))
-
-/* Allocate Adapter Shared Memory.
- * IMPORTANT NOTE: Any changes to this function MUST be mirrored in the
- * function "get_num_rx_bdbs" below!!!
- *
- * Order of memory allocation:
- *
- *       0. Initial System Configuration Block Pointer
- *       1. System Configuration Block
- *       2. System Control Block
- *       3. Action Command Block
- *       4. Interrupt Status Block
- *
- *       5. MAC TX FCB'S
- *       6. NON-MAC TX FCB'S
- *       7. MAC TX BDB'S
- *       8. NON-MAC TX BDB'S
- *       9. MAC RX FCB'S
- *      10. NON-MAC RX FCB'S
- *      11. MAC RX BDB'S
- *      12. NON-MAC RX BDB'S
- *      13. MAC TX Data Buffer( 1, 256 byte buffer)
- *      14. MAC RX Data Buffer( 1, 256 byte buffer)
- *
- *      15. NON-MAC TX Data Buffer
- *      16. NON-MAC RX Data Buffer
- */
-static int smctr_alloc_shared_memory(struct net_device *dev)
-{
-        struct net_local *tp = netdev_priv(dev);
-
-        if(smctr_debug > 10)
-                printk(KERN_DEBUG "%s: smctr_alloc_shared_memory\n", dev->name);
-
-        /* Allocate initial System Control Block pointer.
-         * This pointer is located in the last page, last offset - 4.
-         */
-        tp->iscpb_ptr = (ISCPBlock *)(tp->ram_access + ((__u32)64 * 0x400)
-                - (long)ISCP_BLOCK_SIZE);
-
-        /* Allocate System Control Blocks. */
-        tp->scgb_ptr = (SCGBlock *)smctr_malloc(dev, sizeof(SCGBlock));
-        PARAGRAPH_BOUNDRY(tp->sh_mem_used);
-
-        tp->sclb_ptr = (SCLBlock *)smctr_malloc(dev, sizeof(SCLBlock));
-        PARAGRAPH_BOUNDRY(tp->sh_mem_used);
-
-        tp->acb_head = (ACBlock *)smctr_malloc(dev,
-                sizeof(ACBlock)*tp->num_acbs);
-        PARAGRAPH_BOUNDRY(tp->sh_mem_used);
-
-        tp->isb_ptr = (ISBlock *)smctr_malloc(dev, sizeof(ISBlock));
-        PARAGRAPH_BOUNDRY(tp->sh_mem_used);
-
-        tp->misc_command_data = (__u16 *)smctr_malloc(dev, MISC_DATA_SIZE);
-        PARAGRAPH_BOUNDRY(tp->sh_mem_used);
-
-        /* Allocate transmit FCBs. */
-        tp->tx_fcb_head[MAC_QUEUE] = (FCBlock *)smctr_malloc(dev,
-                sizeof(FCBlock) * tp->num_tx_fcbs[MAC_QUEUE]);
-
-        tp->tx_fcb_head[NON_MAC_QUEUE] = (FCBlock *)smctr_malloc(dev,
-                sizeof(FCBlock) * tp->num_tx_fcbs[NON_MAC_QUEUE]);
-
-        tp->tx_fcb_head[BUG_QUEUE] = (FCBlock *)smctr_malloc(dev,
-                sizeof(FCBlock) * tp->num_tx_fcbs[BUG_QUEUE]);
-
-        /* Allocate transmit BDBs. */
-        tp->tx_bdb_head[MAC_QUEUE] = (BDBlock *)smctr_malloc(dev,
-                sizeof(BDBlock) * tp->num_tx_bdbs[MAC_QUEUE]);
-
-        tp->tx_bdb_head[NON_MAC_QUEUE] = (BDBlock *)smctr_malloc(dev,
-                sizeof(BDBlock) * tp->num_tx_bdbs[NON_MAC_QUEUE]);
-
-        tp->tx_bdb_head[BUG_QUEUE] = (BDBlock *)smctr_malloc(dev,
-                sizeof(BDBlock) * tp->num_tx_bdbs[BUG_QUEUE]);
-
-        /* Allocate receive FCBs. */
-        tp->rx_fcb_head[MAC_QUEUE] = (FCBlock *)smctr_malloc(dev,
-                sizeof(FCBlock) * tp->num_rx_fcbs[MAC_QUEUE]);
-
-        tp->rx_fcb_head[NON_MAC_QUEUE] = (FCBlock *)smctr_malloc(dev,
-                sizeof(FCBlock) * tp->num_rx_fcbs[NON_MAC_QUEUE]);
-
-        /* Allocate receive BDBs. */
-        tp->rx_bdb_head[MAC_QUEUE] = (BDBlock *)smctr_malloc(dev,
-                sizeof(BDBlock) * tp->num_rx_bdbs[MAC_QUEUE]);
-
-        tp->rx_bdb_end[MAC_QUEUE] = (BDBlock *)smctr_malloc(dev, 0);
-
-        tp->rx_bdb_head[NON_MAC_QUEUE] = (BDBlock *)smctr_malloc(dev,
-                sizeof(BDBlock) * tp->num_rx_bdbs[NON_MAC_QUEUE]);
-
-        tp->rx_bdb_end[NON_MAC_QUEUE] = (BDBlock *)smctr_malloc(dev, 0);
-
-        /* Allocate MAC transmit buffers.
-         * MAC Tx Buffers doen't have to be on an ODD Boundary.
-         */
-        tp->tx_buff_head[MAC_QUEUE]
-                = (__u16 *)smctr_malloc(dev, tp->tx_buff_size[MAC_QUEUE]);
-        tp->tx_buff_curr[MAC_QUEUE] = tp->tx_buff_head[MAC_QUEUE];
-        tp->tx_buff_end [MAC_QUEUE] = (__u16 *)smctr_malloc(dev, 0);
-
-        /* Allocate BUG transmit buffers. */
-        tp->tx_buff_head[BUG_QUEUE]
-                = (__u16 *)smctr_malloc(dev, tp->tx_buff_size[BUG_QUEUE]);
-        tp->tx_buff_curr[BUG_QUEUE] = tp->tx_buff_head[BUG_QUEUE];
-        tp->tx_buff_end[BUG_QUEUE] = (__u16 *)smctr_malloc(dev, 0);
-
-        /* Allocate MAC receive data buffers.
-         * MAC Rx buffer doesn't have to be on a 256 byte boundary.
-         */
-        tp->rx_buff_head[MAC_QUEUE] = (__u16 *)smctr_malloc(dev,
-                RX_DATA_BUFFER_SIZE * tp->num_rx_bdbs[MAC_QUEUE]);
-        tp->rx_buff_end[MAC_QUEUE] = (__u16 *)smctr_malloc(dev, 0);
-
-        /* Allocate Non-MAC transmit buffers.
-         * ?? For maximum Netware performance, put Tx Buffers on
-         * ODD Boundary and then restore malloc to Even Boundrys.
-         */
-        smctr_malloc(dev, 1L);
-        tp->tx_buff_head[NON_MAC_QUEUE]
-                = (__u16 *)smctr_malloc(dev, tp->tx_buff_size[NON_MAC_QUEUE]);
-        tp->tx_buff_curr[NON_MAC_QUEUE] = tp->tx_buff_head[NON_MAC_QUEUE];
-        tp->tx_buff_end [NON_MAC_QUEUE] = (__u16 *)smctr_malloc(dev, 0);
-        smctr_malloc(dev, 1L);
-
-        /* Allocate Non-MAC receive data buffers.
-         * To guarantee a minimum of 256 contiguous memory to
-         * UM_Receive_Packet's lookahead pointer, before a page
-         * change or ring end is encountered, place each rx buffer on
-         * a 256 byte boundary.
-         */
-        smctr_malloc(dev, TO_256_BYTE_BOUNDRY(tp->sh_mem_used));
-        tp->rx_buff_head[NON_MAC_QUEUE] = (__u16 *)smctr_malloc(dev,
-                RX_DATA_BUFFER_SIZE * tp->num_rx_bdbs[NON_MAC_QUEUE]);
-        tp->rx_buff_end[NON_MAC_QUEUE] = (__u16 *)smctr_malloc(dev, 0);
-
-        return 0;
-}
-
-/* Enter Bypass state. */
-static int smctr_bypass_state(struct net_device *dev)
-{
-        int err;
-
-	if(smctr_debug > 10)
-        	printk(KERN_DEBUG "%s: smctr_bypass_state\n", dev->name);
-
-        err = smctr_setup_single_cmd(dev, ACB_CMD_CHANGE_JOIN_STATE, JS_BYPASS_STATE);
-
-        return err;
-}
-
-static int smctr_checksum_firmware(struct net_device *dev)
-{
-        struct net_local *tp = netdev_priv(dev);
-        __u16 i, checksum = 0;
-
-        if(smctr_debug > 10)
-                printk(KERN_DEBUG "%s: smctr_checksum_firmware\n", dev->name);
-
-        smctr_enable_adapter_ctrl_store(dev);
-
-        for(i = 0; i < CS_RAM_SIZE; i += 2)
-                checksum += *((__u16 *)(tp->ram_access + i));
-
-        tp->microcode_version = *(__u16 *)(tp->ram_access
-                + CS_RAM_VERSION_OFFSET);
-        tp->microcode_version >>= 8;
-
-        smctr_disable_adapter_ctrl_store(dev);
-
-        if(checksum)
-                return checksum;
-
-        return 0;
-}
-
-static int __init smctr_chk_mca(struct net_device *dev)
-{
-#ifdef CONFIG_MCA_LEGACY
-	struct net_local *tp = netdev_priv(dev);
-	int current_slot;
-	__u8 r1, r2, r3, r4, r5;
-
-	current_slot = mca_find_unused_adapter(smctr_posid, 0);
-	if(current_slot == MCA_NOTFOUND)
-		return -ENODEV;
-
-	mca_set_adapter_name(current_slot, smctr_name);
-	mca_mark_as_used(current_slot);
-	tp->slot_num = current_slot;
-
-	r1 = mca_read_stored_pos(tp->slot_num, 2);
-	r2 = mca_read_stored_pos(tp->slot_num, 3);
-
-	if(tp->slot_num)
-		outb(CNFG_POS_CONTROL_REG, (__u8)((tp->slot_num - 1) | CNFG_SLOT_ENABLE_BIT));
-	else
-		outb(CNFG_POS_CONTROL_REG, (__u8)((tp->slot_num) | CNFG_SLOT_ENABLE_BIT));
-
-	r1 = inb(CNFG_POS_REG1);
-	r2 = inb(CNFG_POS_REG0);
-
-	tp->bic_type = BIC_594_CHIP;
-
-	/* IO */
-	r2 = mca_read_stored_pos(tp->slot_num, 2);
-	r2 &= 0xF0;
-	dev->base_addr = ((__u16)r2 << 8) + (__u16)0x800;
-	request_region(dev->base_addr, SMCTR_IO_EXTENT, smctr_name);
-
-	/* IRQ */
-	r5 = mca_read_stored_pos(tp->slot_num, 5);
-	r5 &= 0xC;
-        switch(r5)
-	{
-            	case 0:
-			dev->irq = 3;
-               		break;
-
-            	case 0x4:
-			dev->irq = 4;
-               		break;
-
-            	case 0x8:
-			dev->irq = 10;
-               		break;
-
-            	default:
-			dev->irq = 15;
-               		break;
-	}
-	if (request_irq(dev->irq, smctr_interrupt, IRQF_SHARED, smctr_name, dev)) {
-		release_region(dev->base_addr, SMCTR_IO_EXTENT);
-		return -ENODEV;
-	}
-
-	/* Get RAM base */
-	r3 = mca_read_stored_pos(tp->slot_num, 3);
-	tp->ram_base = ((__u32)(r3 & 0x7) << 13) + 0x0C0000;
-	if (r3 & 0x8)
-		tp->ram_base += 0x010000;
-	if (r3 & 0x80)
-		tp->ram_base += 0xF00000;
-
-	/* Get Ram Size */
-	r3 &= 0x30;
-	r3 >>= 4;
-
-	tp->ram_usable = (__u16)CNFG_SIZE_8KB << r3;
-	tp->ram_size = (__u16)CNFG_SIZE_64KB;
-	tp->board_id |= TOKEN_MEDIA;
-
-	r4 = mca_read_stored_pos(tp->slot_num, 4);
-	tp->rom_base = ((__u32)(r4 & 0x7) << 13) + 0x0C0000;
-	if (r4 & 0x8)
-		tp->rom_base += 0x010000;
-
-	/* Get ROM size. */
-	r4 >>= 4;
-	switch (r4) {
-		case 0:
-			tp->rom_size = CNFG_SIZE_8KB;
-			break;
-		case 1:
-			tp->rom_size = CNFG_SIZE_16KB;
-			break;
-		case 2:
-			tp->rom_size = CNFG_SIZE_32KB;
-			break;
-		default:
-			tp->rom_size = ROM_DISABLE;
-	}
-
-	/* Get Media Type. */
-	r5 = mca_read_stored_pos(tp->slot_num, 5);
-	r5 &= CNFG_MEDIA_TYPE_MASK;
-	switch(r5)
-	{
-		case (0):
-			tp->media_type = MEDIA_STP_4;
-			break;
-
-		case (1):
-			tp->media_type = MEDIA_STP_16;
-			break;
-
-		case (3):
-			tp->media_type = MEDIA_UTP_16;
-			break;
-
-		default:
-			tp->media_type = MEDIA_UTP_4;
-			break;
-	}
-	tp->media_menu = 14;
-
-	r2 = mca_read_stored_pos(tp->slot_num, 2);
-	if(!(r2 & 0x02))
-		tp->mode_bits |= EARLY_TOKEN_REL;
-
-	/* Disable slot */
-	outb(CNFG_POS_CONTROL_REG, 0);
-
-	tp->board_id = smctr_get_boardid(dev, 1);
-	switch(tp->board_id & 0xffff)
-        {
-                case WD8115TA:
-                        smctr_model = "8115T/A";
-                        break;
-
-                case WD8115T:
-			if(tp->extra_info & CHIP_REV_MASK)
-                                smctr_model = "8115T rev XE";
-                        else
-                                smctr_model = "8115T rev XD";
-                        break;
-
-                default:
-                        smctr_model = "Unknown";
-                        break;
-        }
-
-	return 0;
-#else
-	return -1;
-#endif /* CONFIG_MCA_LEGACY */
-}
-
-static int smctr_chg_rx_mask(struct net_device *dev)
-{
-        struct net_local *tp = netdev_priv(dev);
-        int err = 0;
-
-        if(smctr_debug > 10)
-		printk(KERN_DEBUG "%s: smctr_chg_rx_mask\n", dev->name);
-
-        smctr_enable_16bit(dev);
-        smctr_set_page(dev, (__u8 *)tp->ram_access);
-
-        if(tp->mode_bits & LOOPING_MODE_MASK)
-                tp->config_word0 |= RX_OWN_BIT;
-        else
-                tp->config_word0 &= ~RX_OWN_BIT;
-
-        if(tp->receive_mask & PROMISCUOUS_MODE)
-                tp->config_word0 |= PROMISCUOUS_BIT;
-        else
-                tp->config_word0 &= ~PROMISCUOUS_BIT;
-
-        if(tp->receive_mask & ACCEPT_ERR_PACKETS)
-                tp->config_word0 |= SAVBAD_BIT;
-        else
-                tp->config_word0 &= ~SAVBAD_BIT;
-
-        if(tp->receive_mask & ACCEPT_ATT_MAC_FRAMES)
-                tp->config_word0 |= RXATMAC;
-        else
-                tp->config_word0 &= ~RXATMAC;
-
-        if(tp->receive_mask & ACCEPT_MULTI_PROM)
-                tp->config_word1 |= MULTICAST_ADDRESS_BIT;
-        else
-                tp->config_word1 &= ~MULTICAST_ADDRESS_BIT;
-
-        if(tp->receive_mask & ACCEPT_SOURCE_ROUTING_SPANNING)
-                tp->config_word1 |= SOURCE_ROUTING_SPANNING_BITS;
-        else
-        {
-                if(tp->receive_mask & ACCEPT_SOURCE_ROUTING)
-                        tp->config_word1 |= SOURCE_ROUTING_EXPLORER_BIT;
-                else
-                        tp->config_word1 &= ~SOURCE_ROUTING_SPANNING_BITS;
-        }
-
-        if((err = smctr_issue_write_word_cmd(dev, RW_CONFIG_REGISTER_0,
-                &tp->config_word0)))
-        {
-                return err;
-        }
-
-        if((err = smctr_issue_write_word_cmd(dev, RW_CONFIG_REGISTER_1,
-                &tp->config_word1)))
-        {
-                return err;
-        }
-
-        smctr_disable_16bit(dev);
-
-        return 0;
-}
-
-static int smctr_clear_int(struct net_device *dev)
-{
-        struct net_local *tp = netdev_priv(dev);
-
-        outb((tp->trc_mask | CSR_CLRTINT), dev->base_addr + CSR);
-
-        return 0;
-}
-
-static int smctr_clear_trc_reset(int ioaddr)
-{
-        __u8 r;
-
-        r = inb(ioaddr + MSR);
-        outb(~MSR_RST & r, ioaddr + MSR);
-
-        return 0;
-}
-
-/*
- * The inverse routine to smctr_open().
- */
-static int smctr_close(struct net_device *dev)
-{
-        struct net_local *tp = netdev_priv(dev);
-        struct sk_buff *skb;
-        int err;
-
-	netif_stop_queue(dev);
-	
-	tp->cleanup = 1;
-
-        /* Check to see if adapter is already in a closed state. */
-        if(tp->status != OPEN)
-                return 0;
-
-        smctr_enable_16bit(dev);
-        smctr_set_page(dev, (__u8 *)tp->ram_access);
-
-        if((err = smctr_issue_remove_cmd(dev)))
-        {
-                smctr_disable_16bit(dev);
-                return err;
-        }
-
-        for(;;)
-        {
-                skb = skb_dequeue(&tp->SendSkbQueue);
-                if(skb == NULL)
-                        break;
-                tp->QueueSkb++;
-                dev_kfree_skb(skb);
-        }
-
-
-        return 0;
-}
-
-static int smctr_decode_firmware(struct net_device *dev,
-				 const struct firmware *fw)
-{
-        struct net_local *tp = netdev_priv(dev);
-        short bit = 0x80, shift = 12;
-        DECODE_TREE_NODE *tree;
-        short branch, tsize;
-        __u16 buff = 0;
-        long weight;
-        __u8 *ucode;
-        __u16 *mem;
-
-        if(smctr_debug > 10)
-                printk(KERN_DEBUG "%s: smctr_decode_firmware\n", dev->name);
-
-        weight  = *(long *)(fw->data + WEIGHT_OFFSET);
-        tsize   = *(__u8 *)(fw->data + TREE_SIZE_OFFSET);
-        tree    = (DECODE_TREE_NODE *)(fw->data + TREE_OFFSET);
-        ucode   = (__u8 *)(fw->data + TREE_OFFSET
-                        + (tsize * sizeof(DECODE_TREE_NODE)));
-        mem     = (__u16 *)(tp->ram_access);
-
-        while(weight)
-        {
-                branch = ROOT;
-                while((tree + branch)->tag != LEAF && weight)
-                {
-                        branch = *ucode & bit ? (tree + branch)->llink
-                                : (tree + branch)->rlink;
-
-                        bit >>= 1;
-                        weight--;
-
-                        if(bit == 0)
-                        {
-                                bit = 0x80;
-                                ucode++;
-                        }
-                }
-
-                buff |= (tree + branch)->info << shift;
-                shift -= 4;
-
-                if(shift < 0)
-                {
-                        *(mem++) = SWAP_BYTES(buff);
-                        buff    = 0;
-                        shift   = 12;
-                }
-        }
-
-        /* The following assumes the Control Store Memory has
-         * been initialized to zero. If the last partial word
-         * is zero, it will not be written.
-         */
-        if(buff)
-                *(mem++) = SWAP_BYTES(buff);
-
-        return 0;
-}
-
-static int smctr_disable_16bit(struct net_device *dev)
-{
-        return 0;
-}
-
-/*
- * On Exit, Adapter is:
- * 1. TRC is in a reset state and un-initialized.
- * 2. Adapter memory is enabled.
- * 3. Control Store memory is out of context (-WCSS is 1).
- */
-static int smctr_disable_adapter_ctrl_store(struct net_device *dev)
-{
-        struct net_local *tp = netdev_priv(dev);
-        int ioaddr = dev->base_addr;
-
-        if(smctr_debug > 10)
-                printk(KERN_DEBUG "%s: smctr_disable_adapter_ctrl_store\n", dev->name);
-
-        tp->trc_mask |= CSR_WCSS;
-        outb(tp->trc_mask, ioaddr + CSR);
-
-        return 0;
-}
-
-static int smctr_disable_bic_int(struct net_device *dev)
-{
-        struct net_local *tp = netdev_priv(dev);
-        int ioaddr = dev->base_addr;
-
-        tp->trc_mask = CSR_MSK_ALL | CSR_MSKCBUSY
-	        | CSR_MSKTINT | CSR_WCSS;
-        outb(tp->trc_mask, ioaddr + CSR);
-
-        return 0;
-}
-
-static int smctr_enable_16bit(struct net_device *dev)
-{
-        struct net_local *tp = netdev_priv(dev);
-        __u8    r;
-
-        if(tp->adapter_bus == BUS_ISA16_TYPE)
-        {
-                r = inb(dev->base_addr + LAAR);
-                outb((r | LAAR_MEM16ENB), dev->base_addr + LAAR);
-        }
-
-        return 0;
-}
-
-/*
- * To enable the adapter control store memory:
- * 1. Adapter must be in a RESET state.
- * 2. Adapter memory must be enabled.
- * 3. Control Store Memory is in context (-WCSS is 0).
- */
-static int smctr_enable_adapter_ctrl_store(struct net_device *dev)
-{
-        struct net_local *tp = netdev_priv(dev);
-        int ioaddr = dev->base_addr;
-
-        if(smctr_debug > 10)
-                printk(KERN_DEBUG "%s: smctr_enable_adapter_ctrl_store\n", dev->name);
-
-        smctr_set_trc_reset(ioaddr);
-        smctr_enable_adapter_ram(dev);
-
-        tp->trc_mask &= ~CSR_WCSS;
-        outb(tp->trc_mask, ioaddr + CSR);
-
-        return 0;
-}
-
-static int smctr_enable_adapter_ram(struct net_device *dev)
-{
-        int ioaddr = dev->base_addr;
-        __u8 r;
-
-        if(smctr_debug > 10)
-                printk(KERN_DEBUG "%s: smctr_enable_adapter_ram\n", dev->name);
-
-        r = inb(ioaddr + MSR);
-        outb(MSR_MEMB | r, ioaddr + MSR);
-
-        return 0;
-}
-
-static int smctr_enable_bic_int(struct net_device *dev)
-{
-        struct net_local *tp = netdev_priv(dev);
-        int ioaddr = dev->base_addr;
-        __u8 r;
-
-        switch(tp->bic_type)
-        {
-                case (BIC_584_CHIP):
-                        tp->trc_mask = CSR_MSKCBUSY | CSR_WCSS;
-                        outb(tp->trc_mask, ioaddr + CSR);
-                        r = inb(ioaddr + IRR);
-                        outb(r | IRR_IEN, ioaddr + IRR);
-                        break;
-
-                case (BIC_594_CHIP):
-                        tp->trc_mask = CSR_MSKCBUSY | CSR_WCSS;
-                        outb(tp->trc_mask, ioaddr + CSR);
-                        r = inb(ioaddr + IMCCR);
-                        outb(r | IMCCR_EIL, ioaddr + IMCCR);
-                        break;
-        }
-
-        return 0;
-}
-
-static int __init smctr_chk_isa(struct net_device *dev)
-{
-        struct net_local *tp = netdev_priv(dev);
-        int ioaddr = dev->base_addr;
-        __u8 r1, r2, b, chksum = 0;
-        __u16 r;
-	int i;
-	int err = -ENODEV;
-
-        if(smctr_debug > 10)
-                printk(KERN_DEBUG "%s: smctr_chk_isa %#4x\n", dev->name, ioaddr);
-
-	if((ioaddr & 0x1F) != 0)
-                goto out;
-
-        /* Grab the region so that no one else tries to probe our ioports. */
-	if (!request_region(ioaddr, SMCTR_IO_EXTENT, smctr_name)) {
-		err = -EBUSY;
-		goto out;
-	}
-
-        /* Checksum SMC node address */
-        for(i = 0; i < 8; i++)
-        {
-                b = inb(ioaddr + LAR0 + i);
-                chksum += b;
-        }
-
-        if (chksum != NODE_ADDR_CKSUM)
-                goto out2;
-
-        b = inb(ioaddr + BDID);
-	if(b != BRD_ID_8115T)
-        {
-                printk(KERN_ERR "%s: The adapter found is not supported\n", dev->name);
-                goto out2;
-        }
-
-        /* Check for 8115T Board ID */
-        r2 = 0;
-        for(r = 0; r < 8; r++)
-        {
-            r1 = inb(ioaddr + 0x8 + r);
-            r2 += r1;
-        }
-
-        /* value of RegF adds up the sum to 0xFF */
-        if((r2 != 0xFF) && (r2 != 0xEE))
-                goto out2;
-
-        /* Get adapter ID */
-        tp->board_id = smctr_get_boardid(dev, 0);
-        switch(tp->board_id & 0xffff)
-        {
-                case WD8115TA:
-                        smctr_model = "8115T/A";
-                        break;
-
-                case WD8115T:
-			if(tp->extra_info & CHIP_REV_MASK)
-                                smctr_model = "8115T rev XE";
-                        else
-                                smctr_model = "8115T rev XD";
-                        break;
-
-                default:
-                        smctr_model = "Unknown";
-                        break;
-        }
-
-        /* Store BIC type. */
-        tp->bic_type = BIC_584_CHIP;
-        tp->nic_type = NIC_825_CHIP;
-
-        /* Copy Ram Size */
-        tp->ram_usable  = CNFG_SIZE_16KB;
-        tp->ram_size    = CNFG_SIZE_64KB;
-
-        /* Get 58x Ram Base */
-        r1 = inb(ioaddr);
-        r1 &= 0x3F;
-
-        r2 = inb(ioaddr + CNFG_LAAR_584);
-        r2 &= CNFG_LAAR_MASK;
-        r2 <<= 3;
-        r2 |= ((r1 & 0x38) >> 3);
-
-        tp->ram_base = ((__u32)r2 << 16) + (((__u32)(r1 & 0x7)) << 13);
-
-        /* Get 584 Irq */
-        r1 = 0;
-        r1 = inb(ioaddr + CNFG_ICR_583);
-        r1 &= CNFG_ICR_IR2_584;
-
-        r2 = inb(ioaddr + CNFG_IRR_583);
-        r2 &= CNFG_IRR_IRQS;     /* 0x60 */
-        r2 >>= 5;
-
-        switch(r2)
-        {
-                case 0:
-                        if(r1 == 0)
-                                dev->irq = 2;
-                        else
-                                dev->irq = 10;
-                        break;
-
-                case 1:
-                        if(r1 == 0)
-                                dev->irq = 3;
-                        else
-                                dev->irq = 11;
-                        break;
-
-                case 2:
-                        if(r1 == 0)
-                        {
-                                if(tp->extra_info & ALTERNATE_IRQ_BIT)
-                                        dev->irq = 5;
-                                else
-                                        dev->irq = 4;
-                        }
-                        else
-                                dev->irq = 15;
-                        break;
-
-                case 3:
-                        if(r1 == 0)
-                                dev->irq = 7;
-                        else
-                                dev->irq = 4;
-                        break;
-
-                default:
-                        printk(KERN_ERR "%s: No IRQ found aborting\n", dev->name);
-                        goto out2;
-         }
-
-        if (request_irq(dev->irq, smctr_interrupt, IRQF_SHARED, smctr_name, dev))
-                goto out2;
-
-        /* Get 58x Rom Base */
-        r1 = inb(ioaddr + CNFG_BIO_583);
-        r1 &= 0x3E;
-        r1 |= 0x40;
-
-        tp->rom_base = (__u32)r1 << 13;
-
-        /* Get 58x Rom Size */
-        r1 = inb(ioaddr + CNFG_BIO_583);
-        r1 &= 0xC0;
-        if(r1 == 0)
-                tp->rom_size = ROM_DISABLE;
-        else
-        {
-                r1 >>= 6;
-                tp->rom_size = (__u16)CNFG_SIZE_8KB << r1;
-        }
-
-        /* Get 58x Boot Status */
-        r1 = inb(ioaddr + CNFG_GP2);
-
-        tp->mode_bits &= (~BOOT_STATUS_MASK);
-
-        if(r1 & CNFG_GP2_BOOT_NIBBLE)
-                tp->mode_bits |= BOOT_TYPE_1;
-
-        /* Get 58x Zero Wait State */
-        tp->mode_bits &= (~ZERO_WAIT_STATE_MASK);
-
-        r1 = inb(ioaddr + CNFG_IRR_583);
-
-        if(r1 & CNFG_IRR_ZWS)
-                 tp->mode_bits |= ZERO_WAIT_STATE_8_BIT;
-
-        if(tp->board_id & BOARD_16BIT)
-        {
-                r1 = inb(ioaddr + CNFG_LAAR_584);
-
-                if(r1 & CNFG_LAAR_ZWS)
-                        tp->mode_bits |= ZERO_WAIT_STATE_16_BIT;
-        }
-
-        /* Get 584 Media Menu */
-        tp->media_menu = 14;
-        r1 = inb(ioaddr + CNFG_IRR_583);
-
-        tp->mode_bits &= 0xf8ff;       /* (~CNFG_INTERFACE_TYPE_MASK) */
-        if((tp->board_id & TOKEN_MEDIA) == TOKEN_MEDIA)
-        {
-                /* Get Advanced Features */
-                if(((r1 & 0x6) >> 1) == 0x3)
-                        tp->media_type |= MEDIA_UTP_16;
-                else
-                {
-                        if(((r1 & 0x6) >> 1) == 0x2)
-                                tp->media_type |= MEDIA_STP_16;
-                        else
-                        {
-                                if(((r1 & 0x6) >> 1) == 0x1)
-                                        tp->media_type |= MEDIA_UTP_4;
-
-                                else
-                                        tp->media_type |= MEDIA_STP_4;
-                        }
-                }
-
-                r1 = inb(ioaddr + CNFG_GP2);
-                if(!(r1 & 0x2) )           /* GP2_ETRD */
-                        tp->mode_bits |= EARLY_TOKEN_REL;
-
-                /* see if the chip is corrupted
-                if(smctr_read_584_chksum(ioaddr))
-                {
-                        printk(KERN_ERR "%s: EEPROM Checksum Failure\n", dev->name);
-			free_irq(dev->irq, dev);
-                        goto out2;
-                }
-		*/
-        }
-
-        return 0;
-
-out2:
-	release_region(ioaddr, SMCTR_IO_EXTENT);
-out:
-	return err;
-}
-
-static int __init smctr_get_boardid(struct net_device *dev, int mca)
-{
-        struct net_local *tp = netdev_priv(dev);
-        int ioaddr = dev->base_addr;
-        __u8 r, r1, IdByte;
-        __u16 BoardIdMask;
-
-        tp->board_id = BoardIdMask = 0;
-
-	if(mca)
-	{
-		BoardIdMask |= (MICROCHANNEL+INTERFACE_CHIP+TOKEN_MEDIA+PAGED_RAM+BOARD_16BIT);
-		tp->extra_info |= (INTERFACE_594_CHIP+RAM_SIZE_64K+NIC_825_BIT+ALTERNATE_IRQ_BIT+SLOT_16BIT);
-	}
-	else
-	{
-        	BoardIdMask|=(INTERFACE_CHIP+TOKEN_MEDIA+PAGED_RAM+BOARD_16BIT);
-        	tp->extra_info |= (INTERFACE_584_CHIP + RAM_SIZE_64K
-        	        + NIC_825_BIT + ALTERNATE_IRQ_BIT);
-	}
-
-	if(!mca)
-	{
-        	r = inb(ioaddr + BID_REG_1);
-        	r &= 0x0c;
-       		outb(r, ioaddr + BID_REG_1);
-        	r = inb(ioaddr + BID_REG_1);
-
-        	if(r & BID_SIXTEEN_BIT_BIT)
-        	{
-        	        tp->extra_info |= SLOT_16BIT;
-        	        tp->adapter_bus = BUS_ISA16_TYPE;
-        	}
-        	else
-        	        tp->adapter_bus = BUS_ISA8_TYPE;
-	}
-	else
-		tp->adapter_bus = BUS_MCA_TYPE;
-
-        /* Get Board Id Byte */
-        IdByte = inb(ioaddr + BID_BOARD_ID_BYTE);
-
-        /* if Major version > 1.0 then
-         *      return;
-         */
-        if(IdByte & 0xF8)
-                return -1;
-
-        r1 = inb(ioaddr + BID_REG_1);
-        r1 &= BID_ICR_MASK;
-        r1 |= BID_OTHER_BIT;
-
-        outb(r1, ioaddr + BID_REG_1);
-        r1 = inb(ioaddr + BID_REG_3);
-
-        r1 &= BID_EAR_MASK;
-        r1 |= BID_ENGR_PAGE;
-
-        outb(r1, ioaddr + BID_REG_3);
-        r1 = inb(ioaddr + BID_REG_1);
-        r1 &= BID_ICR_MASK;
-        r1 |= (BID_RLA | BID_OTHER_BIT);
-
-        outb(r1, ioaddr + BID_REG_1);
-
-        r1 = inb(ioaddr + BID_REG_1);
-        while(r1 & BID_RECALL_DONE_MASK)
-                r1 = inb(ioaddr + BID_REG_1);
-
-        r = inb(ioaddr + BID_LAR_0 + BID_REG_6);
-
-        /* clear chip rev bits */
-        tp->extra_info &= ~CHIP_REV_MASK;
-        tp->extra_info |= ((r & BID_EEPROM_CHIP_REV_MASK) << 6);
-
-        r1 = inb(ioaddr + BID_REG_1);
-        r1 &= BID_ICR_MASK;
-        r1 |= BID_OTHER_BIT;
-
-        outb(r1, ioaddr + BID_REG_1);
-        r1 = inb(ioaddr + BID_REG_3);
-
-        r1 &= BID_EAR_MASK;
-        r1 |= BID_EA6;
-
-        outb(r1, ioaddr + BID_REG_3);
-        r1 = inb(ioaddr + BID_REG_1);
-
-        r1 &= BID_ICR_MASK;
-        r1 |= BID_RLA;
-
-        outb(r1, ioaddr + BID_REG_1);
-        r1 = inb(ioaddr + BID_REG_1);
-
-        while(r1 & BID_RECALL_DONE_MASK)
-                r1 = inb(ioaddr + BID_REG_1);
-
-        return BoardIdMask;
-}
-
-static int smctr_get_group_address(struct net_device *dev)
-{
-        smctr_issue_read_word_cmd(dev, RW_INDIVIDUAL_GROUP_ADDR);
-
-        return smctr_wait_cmd(dev);
-}
-
-static int smctr_get_functional_address(struct net_device *dev)
-{
-        smctr_issue_read_word_cmd(dev, RW_FUNCTIONAL_ADDR);
-
-        return smctr_wait_cmd(dev);
-}
-
-/* Calculate number of Non-MAC receive BDB's and data buffers.
- * This function must simulate allocateing shared memory exactly
- * as the allocate_shared_memory function above.
- */
-static unsigned int smctr_get_num_rx_bdbs(struct net_device *dev)
-{
-        struct net_local *tp = netdev_priv(dev);
-        unsigned int mem_used = 0;
-
-        /* Allocate System Control Blocks. */
-        mem_used += sizeof(SCGBlock);
-
-        mem_used += TO_PARAGRAPH_BOUNDRY(mem_used);
-        mem_used += sizeof(SCLBlock);
-
-        mem_used += TO_PARAGRAPH_BOUNDRY(mem_used);
-        mem_used += sizeof(ACBlock) * tp->num_acbs;
-
-        mem_used += TO_PARAGRAPH_BOUNDRY(mem_used);
-        mem_used += sizeof(ISBlock);
-
-        mem_used += TO_PARAGRAPH_BOUNDRY(mem_used);
-        mem_used += MISC_DATA_SIZE;
-
-        /* Allocate transmit FCB's. */
-        mem_used += TO_PARAGRAPH_BOUNDRY(mem_used);
-
-        mem_used += sizeof(FCBlock) * tp->num_tx_fcbs[MAC_QUEUE];
-        mem_used += sizeof(FCBlock) * tp->num_tx_fcbs[NON_MAC_QUEUE];
-        mem_used += sizeof(FCBlock) * tp->num_tx_fcbs[BUG_QUEUE];
-
-        /* Allocate transmit BDBs. */
-        mem_used += sizeof(BDBlock) * tp->num_tx_bdbs[MAC_QUEUE];
-        mem_used += sizeof(BDBlock) * tp->num_tx_bdbs[NON_MAC_QUEUE];
-        mem_used += sizeof(BDBlock) * tp->num_tx_bdbs[BUG_QUEUE];
-
-        /* Allocate receive FCBs. */
-        mem_used += sizeof(FCBlock) * tp->num_rx_fcbs[MAC_QUEUE];
-        mem_used += sizeof(FCBlock) * tp->num_rx_fcbs[NON_MAC_QUEUE];
-
-        /* Allocate receive BDBs. */
-        mem_used += sizeof(BDBlock) * tp->num_rx_bdbs[MAC_QUEUE];
-
-        /* Allocate MAC transmit buffers.
-         * MAC transmit buffers don't have to be on an ODD Boundary.
-         */
-        mem_used += tp->tx_buff_size[MAC_QUEUE];
-
-        /* Allocate BUG transmit buffers. */
-        mem_used += tp->tx_buff_size[BUG_QUEUE];
-
-        /* Allocate MAC receive data buffers.
-         * MAC receive buffers don't have to be on a 256 byte boundary.
-         */
-        mem_used += RX_DATA_BUFFER_SIZE * tp->num_rx_bdbs[MAC_QUEUE];
-
-        /* Allocate Non-MAC transmit buffers.
-         * For maximum Netware performance, put Tx Buffers on
-         * ODD Boundary,and then restore malloc to Even Boundrys.
-         */
-        mem_used += 1L;
-        mem_used += tp->tx_buff_size[NON_MAC_QUEUE];
-        mem_used += 1L;
-
-        /* CALCULATE NUMBER OF NON-MAC RX BDB'S
-         * AND NON-MAC RX DATA BUFFERS
-         *
-         * Make sure the mem_used offset at this point is the
-         * same as in allocate_shared memory or the following
-         * boundary adjustment will be incorrect (i.e. not allocating
-         * the non-mac receive buffers above cannot change the 256
-         * byte offset).
-         *
-         * Since this cannot be guaranteed, adding the full 256 bytes
-         * to the amount of shared memory used at this point will guaranteed
-         * that the rx data buffers do not overflow shared memory.
-         */
-        mem_used += 0x100;
-
-        return (0xffff - mem_used) / (RX_DATA_BUFFER_SIZE + sizeof(BDBlock));
-}
-
-static int smctr_get_physical_drop_number(struct net_device *dev)
-{
-        smctr_issue_read_word_cmd(dev, RW_PHYSICAL_DROP_NUMBER);
-
-        return smctr_wait_cmd(dev);
-}
-
-static __u8 * smctr_get_rx_pointer(struct net_device *dev, short queue)
-{
-        struct net_local *tp = netdev_priv(dev);
-        BDBlock *bdb;
-
-        bdb = (BDBlock *)((__u32)tp->ram_access
-                + (__u32)(tp->rx_fcb_curr[queue]->trc_bdb_ptr));
-
-        tp->rx_fcb_curr[queue]->bdb_ptr = bdb;
-
-        return (__u8 *)bdb->data_block_ptr;
-}
-
-static int smctr_get_station_id(struct net_device *dev)
-{
-        smctr_issue_read_word_cmd(dev, RW_INDIVIDUAL_MAC_ADDRESS);
-
-        return smctr_wait_cmd(dev);
-}
-
-/*
- * Get the current statistics. This may be called with the card open
- * or closed.
- */
-static struct net_device_stats *smctr_get_stats(struct net_device *dev)
-{
-        struct net_local *tp = netdev_priv(dev);
-
-        return (struct net_device_stats *)&tp->MacStat;
-}
-
-static FCBlock *smctr_get_tx_fcb(struct net_device *dev, __u16 queue,
-        __u16 bytes_count)
-{
-        struct net_local *tp = netdev_priv(dev);
-        FCBlock *pFCB;
-        BDBlock *pbdb;
-        unsigned short alloc_size;
-        unsigned short *temp;
-
-        if(smctr_debug > 20)
-                printk(KERN_DEBUG "smctr_get_tx_fcb\n");
-
-        /* check if there is enough FCB blocks */
-        if(tp->num_tx_fcbs_used[queue] >= tp->num_tx_fcbs[queue])
-                return (FCBlock *)(-1L);
-
-        /* round off the input pkt size to the nearest even number */
-        alloc_size = (bytes_count + 1) & 0xfffe;
-
-        /* check if enough mem */
-        if((tp->tx_buff_used[queue] + alloc_size) > tp->tx_buff_size[queue])
-                return (FCBlock *)(-1L);
-
-        /* check if past the end ;
-         * if exactly enough mem to end of ring, alloc from front.
-         * this avoids update of curr when curr = end
-         */
-        if(((unsigned long)(tp->tx_buff_curr[queue]) + alloc_size)
-                >= (unsigned long)(tp->tx_buff_end[queue]))
-        {
-                /* check if enough memory from ring head */
-                alloc_size = alloc_size +
-                        (__u16)((__u32)tp->tx_buff_end[queue]
-                        - (__u32)tp->tx_buff_curr[queue]);
-
-                if((tp->tx_buff_used[queue] + alloc_size)
-                        > tp->tx_buff_size[queue])
-                {
-                        return (FCBlock *)(-1L);
-                }
-
-                /* ring wrap */
-                tp->tx_buff_curr[queue] = tp->tx_buff_head[queue];
-        }
-
-        tp->tx_buff_used[queue] += alloc_size;
-        tp->num_tx_fcbs_used[queue]++;
-        tp->tx_fcb_curr[queue]->frame_length = bytes_count;
-        tp->tx_fcb_curr[queue]->memory_alloc = alloc_size;
-        temp = tp->tx_buff_curr[queue];
-        tp->tx_buff_curr[queue]
-                = (__u16 *)((__u32)temp + (__u32)((bytes_count + 1) & 0xfffe));
-
-        pbdb = tp->tx_fcb_curr[queue]->bdb_ptr;
-        pbdb->buffer_length = bytes_count;
-        pbdb->data_block_ptr = temp;
-        pbdb->trc_data_block_ptr = TRC_POINTER(temp);
-
-        pFCB = tp->tx_fcb_curr[queue];
-        tp->tx_fcb_curr[queue] = tp->tx_fcb_curr[queue]->next_ptr;
-
-        return pFCB;
-}
-
-static int smctr_get_upstream_neighbor_addr(struct net_device *dev)
-{
-        smctr_issue_read_word_cmd(dev, RW_UPSTREAM_NEIGHBOR_ADDRESS);
-
-        return smctr_wait_cmd(dev);
-}
-
-static int smctr_hardware_send_packet(struct net_device *dev,
-        struct net_local *tp)
-{
-        struct tr_statistics *tstat = &tp->MacStat;
-        struct sk_buff *skb;
-        FCBlock *fcb;
-
-        if(smctr_debug > 10)
-                printk(KERN_DEBUG"%s: smctr_hardware_send_packet\n", dev->name);
-
-        if(tp->status != OPEN)
-                return -1;
-
-        if(tp->monitor_state_ready != 1)
-                return -1;
-
-        for(;;)
-        {
-                /* Send first buffer from queue */
-                skb = skb_dequeue(&tp->SendSkbQueue);
-                if(skb == NULL)
-                        return -1;
-
-                tp->QueueSkb++;
-
-                if(skb->len < SMC_HEADER_SIZE || skb->len > tp->max_packet_size)
-			return -1;
-
-                smctr_enable_16bit(dev);
-                smctr_set_page(dev, (__u8 *)tp->ram_access);
-
-                if((fcb = smctr_get_tx_fcb(dev, NON_MAC_QUEUE, skb->len))
-                        == (FCBlock *)(-1L))
-                {
-                        smctr_disable_16bit(dev);
-                        return -1;
-                }
-
-                smctr_tx_move_frame(dev, skb,
-                        (__u8 *)fcb->bdb_ptr->data_block_ptr, skb->len);
-
-                smctr_set_page(dev, (__u8 *)fcb);
-
-                smctr_trc_send_packet(dev, fcb, NON_MAC_QUEUE);
-                dev_kfree_skb(skb);
-
-                tstat->tx_packets++;
-
-                smctr_disable_16bit(dev);
-        }
-
-        return 0;
-}
-
-static int smctr_init_acbs(struct net_device *dev)
-{
-        struct net_local *tp = netdev_priv(dev);
-        unsigned int i;
-        ACBlock *acb;
-
-        if(smctr_debug > 10)
-                printk(KERN_DEBUG "%s: smctr_init_acbs\n", dev->name);
-
-        acb                     = tp->acb_head;
-        acb->cmd_done_status    = (ACB_COMMAND_DONE | ACB_COMMAND_SUCCESSFUL);
-        acb->cmd_info           = ACB_CHAIN_END;
-        acb->cmd                = 0;
-        acb->subcmd             = 0;
-        acb->data_offset_lo     = 0;
-        acb->data_offset_hi     = 0;
-        acb->next_ptr
-                = (ACBlock *)(((char *)acb) + sizeof(ACBlock));
-        acb->trc_next_ptr       = TRC_POINTER(acb->next_ptr);
-
-        for(i = 1; i < tp->num_acbs; i++)
-        {
-                acb             = acb->next_ptr;
-                acb->cmd_done_status
-                        = (ACB_COMMAND_DONE | ACB_COMMAND_SUCCESSFUL);
-                acb->cmd_info = ACB_CHAIN_END;
-                acb->cmd        = 0;
-                acb->subcmd     = 0;
-                acb->data_offset_lo = 0;
-                acb->data_offset_hi = 0;
-                acb->next_ptr
-                        = (ACBlock *)(((char *)acb) + sizeof(ACBlock));
-                acb->trc_next_ptr = TRC_POINTER(acb->next_ptr);
-        }
-
-        acb->next_ptr           = tp->acb_head;
-        acb->trc_next_ptr       = TRC_POINTER(tp->acb_head);
-        tp->acb_next            = tp->acb_head->next_ptr;
-        tp->acb_curr            = tp->acb_head->next_ptr;
-        tp->num_acbs_used       = 0;
-
-        return 0;
-}
-
-static int smctr_init_adapter(struct net_device *dev)
-{
-        struct net_local *tp = netdev_priv(dev);
-        int err;
-
-        if(smctr_debug > 10)
-                printk(KERN_DEBUG "%s: smctr_init_adapter\n", dev->name);
-
-        tp->status              = CLOSED;
-        tp->page_offset_mask    = (tp->ram_usable * 1024) - 1;
-        skb_queue_head_init(&tp->SendSkbQueue);
-        tp->QueueSkb = MAX_TX_QUEUE;
-
-        if(!(tp->group_address_0 & 0x0080))
-                tp->group_address_0 |= 0x00C0;
-
-        if(!(tp->functional_address_0 & 0x00C0))
-                tp->functional_address_0 |= 0x00C0;
-
-        tp->functional_address[0] &= 0xFF7F;
-
-        if(tp->authorized_function_classes == 0)
-                tp->authorized_function_classes = 0x7FFF;
-
-        if(tp->authorized_access_priority == 0)
-                tp->authorized_access_priority = 0x06;
-
-        smctr_disable_bic_int(dev);
-        smctr_set_trc_reset(dev->base_addr);
-
-        smctr_enable_16bit(dev);
-        smctr_set_page(dev, (__u8 *)tp->ram_access);
-
-        if(smctr_checksum_firmware(dev))
-	{
-                printk(KERN_ERR "%s: Previously loaded firmware is missing\n",dev->name);
-		return -ENOENT;
-        }
-
-        if((err = smctr_ram_memory_test(dev)))
-	{
-                printk(KERN_ERR "%s: RAM memory test failed.\n", dev->name);
-                return -EIO;
-        }
-
-	smctr_set_rx_look_ahead(dev);
-        smctr_load_node_addr(dev);
-
-        /* Initialize adapter for Internal Self Test. */
-        smctr_reset_adapter(dev);
-        if((err = smctr_init_card_real(dev)))
-	{
-                printk(KERN_ERR "%s: Initialization of card failed (%d)\n",
-                        dev->name, err);
-                return -EINVAL;
-        }
-
-        /* This routine clobbers the TRC's internal registers. */
-        if((err = smctr_internal_self_test(dev)))
-	{
-                printk(KERN_ERR "%s: Card failed internal self test (%d)\n",
-                        dev->name, err);
-                return -EINVAL;
-        }
-
-        /* Re-Initialize adapter's internal registers */
-        smctr_reset_adapter(dev);
-        if((err = smctr_init_card_real(dev)))
-	{
-                printk(KERN_ERR "%s: Initialization of card failed (%d)\n",
-                        dev->name, err);
-                return -EINVAL;
-        }
-
-        smctr_enable_bic_int(dev);
-
-        if((err = smctr_issue_enable_int_cmd(dev, TRC_INTERRUPT_ENABLE_MASK)))
-                return err;
-
-        smctr_disable_16bit(dev);
-
-        return 0;
-}
-
-static int smctr_init_card_real(struct net_device *dev)
-{
-        struct net_local *tp = netdev_priv(dev);
-        int err = 0;
-
-        if(smctr_debug > 10)
-                printk(KERN_DEBUG "%s: smctr_init_card_real\n", dev->name);
-
-        tp->sh_mem_used = 0;
-        tp->num_acbs    = NUM_OF_ACBS;
-
-        /* Range Check Max Packet Size */
-        if(tp->max_packet_size < 256)
-                tp->max_packet_size = 256;
-        else
-        {
-                if(tp->max_packet_size > NON_MAC_TX_BUFFER_MEMORY)
-                        tp->max_packet_size = NON_MAC_TX_BUFFER_MEMORY;
-        }
-
-        tp->num_of_tx_buffs = (NON_MAC_TX_BUFFER_MEMORY
-                / tp->max_packet_size) - 1;
-
-        if(tp->num_of_tx_buffs > NUM_NON_MAC_TX_FCBS)
-                tp->num_of_tx_buffs = NUM_NON_MAC_TX_FCBS;
-        else
-        {
-                if(tp->num_of_tx_buffs == 0)
-                        tp->num_of_tx_buffs = 1;
-        }
-
-        /* Tx queue constants */
-        tp->num_tx_fcbs        [BUG_QUEUE]     = NUM_BUG_TX_FCBS;
-        tp->num_tx_bdbs        [BUG_QUEUE]     = NUM_BUG_TX_BDBS;
-        tp->tx_buff_size       [BUG_QUEUE]     = BUG_TX_BUFFER_MEMORY;
-        tp->tx_buff_used       [BUG_QUEUE]     = 0;
-        tp->tx_queue_status    [BUG_QUEUE]     = NOT_TRANSMITING;
-
-        tp->num_tx_fcbs        [MAC_QUEUE]     = NUM_MAC_TX_FCBS;
-        tp->num_tx_bdbs        [MAC_QUEUE]     = NUM_MAC_TX_BDBS;
-        tp->tx_buff_size       [MAC_QUEUE]     = MAC_TX_BUFFER_MEMORY;
-        tp->tx_buff_used       [MAC_QUEUE]     = 0;
-        tp->tx_queue_status    [MAC_QUEUE]     = NOT_TRANSMITING;
-
-        tp->num_tx_fcbs        [NON_MAC_QUEUE] = NUM_NON_MAC_TX_FCBS;
-        tp->num_tx_bdbs        [NON_MAC_QUEUE] = NUM_NON_MAC_TX_BDBS;
-        tp->tx_buff_size       [NON_MAC_QUEUE] = NON_MAC_TX_BUFFER_MEMORY;
-        tp->tx_buff_used       [NON_MAC_QUEUE] = 0;
-        tp->tx_queue_status    [NON_MAC_QUEUE] = NOT_TRANSMITING;
-
-        /* Receive Queue Constants */
-        tp->num_rx_fcbs[MAC_QUEUE] = NUM_MAC_RX_FCBS;
-        tp->num_rx_bdbs[MAC_QUEUE] = NUM_MAC_RX_BDBS;
-
-        if(tp->extra_info & CHIP_REV_MASK)
-                tp->num_rx_fcbs[NON_MAC_QUEUE] = 78;    /* 825 Rev. XE */
-        else
-                tp->num_rx_fcbs[NON_MAC_QUEUE] = 7;     /* 825 Rev. XD */
-
-        tp->num_rx_bdbs[NON_MAC_QUEUE] = smctr_get_num_rx_bdbs(dev);
-
-        smctr_alloc_shared_memory(dev);
-        smctr_init_shared_memory(dev);
-
-        if((err = smctr_issue_init_timers_cmd(dev)))
-                return err;
-
-        if((err = smctr_issue_init_txrx_cmd(dev)))
-	{
-                printk(KERN_ERR "%s: Hardware failure\n", dev->name);
-                return err;
-        }
-
-        return 0;
-}
-
-static int smctr_init_rx_bdbs(struct net_device *dev)
-{
-        struct net_local *tp = netdev_priv(dev);
-        unsigned int i, j;
-        BDBlock *bdb;
-        __u16 *buf;
-
-        if(smctr_debug > 10)
-                printk(KERN_DEBUG "%s: smctr_init_rx_bdbs\n", dev->name);
-
-        for(i = 0; i < NUM_RX_QS_USED; i++)
-        {
-                bdb = tp->rx_bdb_head[i];
-                buf = tp->rx_buff_head[i];
-                bdb->info = (BDB_CHAIN_END | BDB_NO_WARNING);
-                bdb->buffer_length = RX_DATA_BUFFER_SIZE;
-                bdb->next_ptr = (BDBlock *)(((char *)bdb) + sizeof(BDBlock));
-                bdb->data_block_ptr = buf;
-                bdb->trc_next_ptr = TRC_POINTER(bdb->next_ptr);
-
-                if(i == NON_MAC_QUEUE)
-                        bdb->trc_data_block_ptr = RX_BUFF_TRC_POINTER(buf);
-                else
-                        bdb->trc_data_block_ptr = TRC_POINTER(buf);
-
-                for(j = 1; j < tp->num_rx_bdbs[i]; j++)
-                {
-                        bdb->next_ptr->back_ptr = bdb;
-                        bdb = bdb->next_ptr;
-                        buf = (__u16 *)((char *)buf + RX_DATA_BUFFER_SIZE);
-                        bdb->info = (BDB_NOT_CHAIN_END | BDB_NO_WARNING);
-                        bdb->buffer_length = RX_DATA_BUFFER_SIZE;
-                        bdb->next_ptr = (BDBlock *)(((char *)bdb) + sizeof(BDBlock));
-                        bdb->data_block_ptr = buf;
-                        bdb->trc_next_ptr = TRC_POINTER(bdb->next_ptr);
-
-                        if(i == NON_MAC_QUEUE)
-                                bdb->trc_data_block_ptr = RX_BUFF_TRC_POINTER(buf);
-                        else
-                                bdb->trc_data_block_ptr = TRC_POINTER(buf);
-                }
-
-                bdb->next_ptr           = tp->rx_bdb_head[i];
-                bdb->trc_next_ptr       = TRC_POINTER(tp->rx_bdb_head[i]);
-
-                tp->rx_bdb_head[i]->back_ptr    = bdb;
-                tp->rx_bdb_curr[i]              = tp->rx_bdb_head[i]->next_ptr;
-        }
-
-        return 0;
-}
-
-static int smctr_init_rx_fcbs(struct net_device *dev)
-{
-        struct net_local *tp = netdev_priv(dev);
-        unsigned int i, j;
-        FCBlock *fcb;
-
-        for(i = 0; i < NUM_RX_QS_USED; i++)
-        {
-                fcb               = tp->rx_fcb_head[i];
-                fcb->frame_status = 0;
-                fcb->frame_length = 0;
-                fcb->info         = FCB_CHAIN_END;
-                fcb->next_ptr     = (FCBlock *)(((char*)fcb) + sizeof(FCBlock));
-                if(i == NON_MAC_QUEUE)
-                        fcb->trc_next_ptr = RX_FCB_TRC_POINTER(fcb->next_ptr);
-                else
-                        fcb->trc_next_ptr = TRC_POINTER(fcb->next_ptr);
-
-                for(j = 1; j < tp->num_rx_fcbs[i]; j++)
-                {
-                        fcb->next_ptr->back_ptr = fcb;
-                        fcb                     = fcb->next_ptr;
-                        fcb->frame_status       = 0;
-                        fcb->frame_length       = 0;
-                        fcb->info               = FCB_WARNING;
-                        fcb->next_ptr
-                                = (FCBlock *)(((char *)fcb) + sizeof(FCBlock));
-
-                        if(i == NON_MAC_QUEUE)
-                                fcb->trc_next_ptr
-                                        = RX_FCB_TRC_POINTER(fcb->next_ptr);
-                        else
-                                fcb->trc_next_ptr
-                                        = TRC_POINTER(fcb->next_ptr);
-                }
-
-                fcb->next_ptr = tp->rx_fcb_head[i];
-
-                if(i == NON_MAC_QUEUE)
-                        fcb->trc_next_ptr = RX_FCB_TRC_POINTER(fcb->next_ptr);
-                else
-                        fcb->trc_next_ptr = TRC_POINTER(fcb->next_ptr);
-
-                tp->rx_fcb_head[i]->back_ptr    = fcb;
-                tp->rx_fcb_curr[i]              = tp->rx_fcb_head[i]->next_ptr;
-        }
-
-        return 0;
-}
-
-static int smctr_init_shared_memory(struct net_device *dev)
-{
-        struct net_local *tp = netdev_priv(dev);
-        unsigned int i;
-        __u32 *iscpb;
-
-        if(smctr_debug > 10)
-                printk(KERN_DEBUG "%s: smctr_init_shared_memory\n", dev->name);
-
-        smctr_set_page(dev, (__u8 *)(unsigned int)tp->iscpb_ptr);
-
-        /* Initialize Initial System Configuration Point. (ISCP) */
-        iscpb = (__u32 *)PAGE_POINTER(&tp->iscpb_ptr->trc_scgb_ptr);
-        *iscpb = (__u32)(SWAP_WORDS(TRC_POINTER(tp->scgb_ptr)));
-
-        smctr_set_page(dev, (__u8 *)tp->ram_access);
-
-        /* Initialize System Configuration Pointers. (SCP) */
-        tp->scgb_ptr->config = (SCGB_ADDRESS_POINTER_FORMAT
-                | SCGB_MULTI_WORD_CONTROL | SCGB_DATA_FORMAT
-                | SCGB_BURST_LENGTH);
-
-        tp->scgb_ptr->trc_sclb_ptr      = TRC_POINTER(tp->sclb_ptr);
-        tp->scgb_ptr->trc_acb_ptr       = TRC_POINTER(tp->acb_head);
-        tp->scgb_ptr->trc_isb_ptr       = TRC_POINTER(tp->isb_ptr);
-        tp->scgb_ptr->isbsiz            = (sizeof(ISBlock)) - 2;
-
-        /* Initialize System Control Block. (SCB) */
-        tp->sclb_ptr->valid_command    = SCLB_VALID | SCLB_CMD_NOP;
-        tp->sclb_ptr->iack_code        = 0;
-        tp->sclb_ptr->resume_control   = 0;
-        tp->sclb_ptr->int_mask_control = 0;
-        tp->sclb_ptr->int_mask_state   = 0;
-
-        /* Initialize Interrupt Status Block. (ISB) */
-        for(i = 0; i < NUM_OF_INTERRUPTS; i++)
-        {
-                tp->isb_ptr->IStatus[i].IType = 0xf0;
-                tp->isb_ptr->IStatus[i].ISubtype = 0;
-        }
-
-        tp->current_isb_index = 0;
-
-        /* Initialize Action Command Block. (ACB) */
-        smctr_init_acbs(dev);
-
-        /* Initialize transmit FCB's and BDB's. */
-        smctr_link_tx_fcbs_to_bdbs(dev);
-        smctr_init_tx_bdbs(dev);
-        smctr_init_tx_fcbs(dev);
-
-        /* Initialize receive FCB's and BDB's. */
-        smctr_init_rx_bdbs(dev);
-        smctr_init_rx_fcbs(dev);
-
-        return 0;
-}
-
-static int smctr_init_tx_bdbs(struct net_device *dev)
-{
-        struct net_local *tp = netdev_priv(dev);
-        unsigned int i, j;
-        BDBlock *bdb;
-
-        for(i = 0; i < NUM_TX_QS_USED; i++)
-        {
-                bdb = tp->tx_bdb_head[i];
-                bdb->info = (BDB_NOT_CHAIN_END | BDB_NO_WARNING);
-                bdb->next_ptr = (BDBlock *)(((char *)bdb) + sizeof(BDBlock));
-                bdb->trc_next_ptr = TRC_POINTER(bdb->next_ptr);
-
-                for(j = 1; j < tp->num_tx_bdbs[i]; j++)
-                {
-                        bdb->next_ptr->back_ptr = bdb;
-                        bdb = bdb->next_ptr;
-                        bdb->info = (BDB_NOT_CHAIN_END | BDB_NO_WARNING);
-                        bdb->next_ptr
-                                = (BDBlock *)(((char *)bdb) + sizeof( BDBlock));                        bdb->trc_next_ptr = TRC_POINTER(bdb->next_ptr);
-                }
-
-                bdb->next_ptr = tp->tx_bdb_head[i];
-                bdb->trc_next_ptr = TRC_POINTER(tp->tx_bdb_head[i]);
-                tp->tx_bdb_head[i]->back_ptr = bdb;
-        }
-
-        return 0;
-}
-
-static int smctr_init_tx_fcbs(struct net_device *dev)
-{
-        struct net_local *tp = netdev_priv(dev);
-        unsigned int i, j;
-        FCBlock *fcb;
-
-        for(i = 0; i < NUM_TX_QS_USED; i++)
-        {
-                fcb               = tp->tx_fcb_head[i];
-                fcb->frame_status = 0;
-                fcb->frame_length = 0;
-                fcb->info         = FCB_CHAIN_END;
-                fcb->next_ptr = (FCBlock *)(((char *)fcb) + sizeof(FCBlock));
-                fcb->trc_next_ptr = TRC_POINTER(fcb->next_ptr);
-
-                for(j = 1; j < tp->num_tx_fcbs[i]; j++)
-                {
-                        fcb->next_ptr->back_ptr = fcb;
-                        fcb                     = fcb->next_ptr;
-                        fcb->frame_status       = 0;
-                        fcb->frame_length       = 0;
-                        fcb->info               = FCB_CHAIN_END;
-                        fcb->next_ptr
-                                = (FCBlock *)(((char *)fcb) + sizeof(FCBlock));
-                        fcb->trc_next_ptr = TRC_POINTER(fcb->next_ptr);
-                }
-
-                fcb->next_ptr           = tp->tx_fcb_head[i];
-                fcb->trc_next_ptr       = TRC_POINTER(tp->tx_fcb_head[i]);
-
-                tp->tx_fcb_head[i]->back_ptr    = fcb;
-                tp->tx_fcb_end[i]               = tp->tx_fcb_head[i]->next_ptr;
-                tp->tx_fcb_curr[i]              = tp->tx_fcb_head[i]->next_ptr;
-                tp->num_tx_fcbs_used[i]         = 0;
-        }
-
-        return 0;
-}
-
-static int smctr_internal_self_test(struct net_device *dev)
-{
-        struct net_local *tp = netdev_priv(dev);
-        int err;
-
-        if((err = smctr_issue_test_internal_rom_cmd(dev)))
-                return err;
-
-        if((err = smctr_wait_cmd(dev)))
-                return err;
-
-        if(tp->acb_head->cmd_done_status & 0xff)
-                return -1;
-
-        if((err = smctr_issue_test_hic_cmd(dev)))
-                return err;
-
-        if((err = smctr_wait_cmd(dev)))
-                return err;
-
-        if(tp->acb_head->cmd_done_status & 0xff)
-                return -1;
-
-        if((err = smctr_issue_test_mac_reg_cmd(dev)))
-                return err;
-
-        if((err = smctr_wait_cmd(dev)))
-                return err;
-
-        if(tp->acb_head->cmd_done_status & 0xff)
-                return -1;
-
-        return 0;
-}
-
-/*
- * The typical workload of the driver: Handle the network interface interrupts.
- */
-static irqreturn_t smctr_interrupt(int irq, void *dev_id)
-{
-        struct net_device *dev = dev_id;
-        struct net_local *tp;
-        int ioaddr;
-        __u16 interrupt_unmask_bits = 0, interrupt_ack_code = 0xff00;
-        __u16 err1, err = NOT_MY_INTERRUPT;
-        __u8 isb_type, isb_subtype;
-        __u16 isb_index;
-
-        ioaddr = dev->base_addr;
-        tp = netdev_priv(dev);
-
-        if(tp->status == NOT_INITIALIZED)
-                return IRQ_NONE;
-
-        spin_lock(&tp->lock);
-        
-        smctr_disable_bic_int(dev);
-        smctr_enable_16bit(dev);
-
-        smctr_clear_int(dev);
-
-        /* First read the LSB */
-        while((tp->isb_ptr->IStatus[tp->current_isb_index].IType & 0xf0) == 0)
-        {
-                isb_index       = tp->current_isb_index;
-                isb_type        = tp->isb_ptr->IStatus[isb_index].IType;
-                isb_subtype     = tp->isb_ptr->IStatus[isb_index].ISubtype;
-
-                (tp->current_isb_index)++;
-                if(tp->current_isb_index == NUM_OF_INTERRUPTS)
-                        tp->current_isb_index = 0;
-
-                if(isb_type >= 0x10)
-                {
-                        smctr_disable_16bit(dev);
-		        spin_unlock(&tp->lock);
-                        return IRQ_HANDLED;
-                }
-
-                err = HARDWARE_FAILED;
-                interrupt_ack_code = isb_index;
-                tp->isb_ptr->IStatus[isb_index].IType |= 0xf0;
-
-                interrupt_unmask_bits |= (1 << (__u16)isb_type);
-
-                switch(isb_type)
-                {
-                        case ISB_IMC_MAC_TYPE_3:
-                                smctr_disable_16bit(dev);
-
-                                switch(isb_subtype)
-                                {
-                                        case 0:
-                                                tp->monitor_state = MS_MONITOR_FSM_INACTIVE;
-                                               break;
-
-                                        case 1:
-                                                tp->monitor_state = MS_REPEAT_BEACON_STATE;
-                                                break;
-
-                                        case 2:
-                                                tp->monitor_state = MS_REPEAT_CLAIM_TOKEN_STATE;
-                                                break;
-
-                                        case 3:
-                                                tp->monitor_state = MS_TRANSMIT_CLAIM_TOKEN_STATE;                                                break;
-
-                                        case 4:
-                                                tp->monitor_state = MS_STANDBY_MONITOR_STATE;
-                                                break;
-
-                                        case 5:
-                                                tp->monitor_state = MS_TRANSMIT_BEACON_STATE;
-                                                break;
-
-                                        case 6:
-                                                tp->monitor_state = MS_ACTIVE_MONITOR_STATE;
-                                                break;
-
-                                        case 7:
-                                                tp->monitor_state = MS_TRANSMIT_RING_PURGE_STATE;
-                                                break;
-
-                                        case 8:   /* diagnostic state */
-                                                break;
-
-                                        case 9:
-                                                tp->monitor_state = MS_BEACON_TEST_STATE;
-                                                if(smctr_lobe_media_test(dev))
-                                                {
-                                                        tp->ring_status_flags = RING_STATUS_CHANGED;
-                                                        tp->ring_status = AUTO_REMOVAL_ERROR;
-                                                        smctr_ring_status_chg(dev);
-                                                        smctr_bypass_state(dev);
-                                                }
-                                                else
-                                                        smctr_issue_insert_cmd(dev);
-                                                break;
-
-                                        /* case 0x0a-0xff, illegal states */
-                                        default:
-                                                break;
-                                }
-
-                                tp->ring_status_flags = MONITOR_STATE_CHANGED;
-                                err = smctr_ring_status_chg(dev);
-
-                                smctr_enable_16bit(dev);
-                                break;
-
-                        /* Type 0x02 - MAC Error Counters Interrupt
-                         * One or more MAC Error Counter is half full
-                         *      MAC Error Counters
-                         *      Lost_FR_Error_Counter
-                         *      RCV_Congestion_Counter
-                         *      FR_copied_Error_Counter
-                         *      FREQ_Error_Counter
-                         *      Token_Error_Counter
-                         *      Line_Error_Counter
-                         *      Internal_Error_Count
-                         */
-                        case ISB_IMC_MAC_ERROR_COUNTERS:
-                                /* Read 802.5 Error Counters */
-                                err = smctr_issue_read_ring_status_cmd(dev);
-                                break;
-
-                        /* Type 0x04 - MAC Type 2 Interrupt
-                         * HOST needs to enqueue MAC Frame for transmission
-                         * SubType Bit 15 - RQ_INIT_PDU( Request Initialization)                         * Changed from RQ_INIT_PDU to
-                         * TRC_Status_Changed_Indicate
-                         */
-                        case ISB_IMC_MAC_TYPE_2:
-                                err = smctr_issue_read_ring_status_cmd(dev);
-                                break;
-
-
-                        /* Type 0x05 - TX Frame Interrupt (FI). */
-                        case ISB_IMC_TX_FRAME:
-                                /* BUG QUEUE for TRC stuck receive BUG */
-                                if(isb_subtype & TX_PENDING_PRIORITY_2)
-                                {
-                                        if((err = smctr_tx_complete(dev, BUG_QUEUE)) != SUCCESS)
-                                                break;
-                                }
-
-                                /* NON-MAC frames only */
-                                if(isb_subtype & TX_PENDING_PRIORITY_1)
-                                {
-                                        if((err = smctr_tx_complete(dev, NON_MAC_QUEUE)) != SUCCESS)
-                                                break;
-                                }
-
-                                /* MAC frames only */
-                                if(isb_subtype & TX_PENDING_PRIORITY_0)
-                                        err = smctr_tx_complete(dev, MAC_QUEUE);                                break;
-
-                        /* Type 0x06 - TX END OF QUEUE (FE) */
-                        case ISB_IMC_END_OF_TX_QUEUE:
-                                /* BUG queue */
-                                if(isb_subtype & TX_PENDING_PRIORITY_2)
-                                {
-                                        /* ok to clear Receive FIFO overrun
-                                         * imask send_BUG now completes.
-                                         */
-                                        interrupt_unmask_bits |= 0x800;
-
-                                        tp->tx_queue_status[BUG_QUEUE] = NOT_TRANSMITING;
-                                        if((err = smctr_tx_complete(dev, BUG_QUEUE)) != SUCCESS)
-                                                break;
-                                        if((err = smctr_restart_tx_chain(dev, BUG_QUEUE)) != SUCCESS)
-                                                break;
-                                }
-
-                                /* NON-MAC queue only */
-                                if(isb_subtype & TX_PENDING_PRIORITY_1)
-                                {
-                                        tp->tx_queue_status[NON_MAC_QUEUE] = NOT_TRANSMITING;
-                                        if((err = smctr_tx_complete(dev, NON_MAC_QUEUE)) != SUCCESS)
-                                                break;
-                                        if((err = smctr_restart_tx_chain(dev, NON_MAC_QUEUE)) != SUCCESS)
-                                                break;
-                                }
-
-                                /* MAC queue only */
-                                if(isb_subtype & TX_PENDING_PRIORITY_0)
-                                {
-                                        tp->tx_queue_status[MAC_QUEUE] = NOT_TRANSMITING;
-                                        if((err = smctr_tx_complete(dev, MAC_QUEUE)) != SUCCESS)
-                                                break;
-
-                                        err = smctr_restart_tx_chain(dev, MAC_QUEUE);
-                                }
-                                break;
-
-                        /* Type 0x07 - NON-MAC RX Resource Interrupt
-                         *   Subtype bit 12 - (BW) BDB warning
-                         *   Subtype bit 13 - (FW) FCB warning
-                         *   Subtype bit 14 - (BE) BDB End of chain
-                         *   Subtype bit 15 - (FE) FCB End of chain
-                         */
-                        case ISB_IMC_NON_MAC_RX_RESOURCE:
-                                tp->rx_fifo_overrun_count = 0;
-                                tp->receive_queue_number = NON_MAC_QUEUE;
-                                err1 = smctr_rx_frame(dev);
-
-                                if(isb_subtype & NON_MAC_RX_RESOURCE_FE)
-                                {
-                                        if((err = smctr_issue_resume_rx_fcb_cmd(                                                dev, NON_MAC_QUEUE)) != SUCCESS)                                                break;
-
-                                        if(tp->ptr_rx_fcb_overruns)
-                                                (*tp->ptr_rx_fcb_overruns)++;
-                                }
-
-                                if(isb_subtype & NON_MAC_RX_RESOURCE_BE)
-                                {
-                                        if((err = smctr_issue_resume_rx_bdb_cmd(                                                dev, NON_MAC_QUEUE)) != SUCCESS)                                                break;
-
-                                        if(tp->ptr_rx_bdb_overruns)
-                                                (*tp->ptr_rx_bdb_overruns)++;
-                                }
-                                err = err1;
-                                break;
-
-                        /* Type 0x08 - MAC RX Resource Interrupt
-                         *   Subtype bit 12 - (BW) BDB warning
-                         *   Subtype bit 13 - (FW) FCB warning
-                         *   Subtype bit 14 - (BE) BDB End of chain
-                         *   Subtype bit 15 - (FE) FCB End of chain
-                         */
-                        case ISB_IMC_MAC_RX_RESOURCE:
-                                tp->receive_queue_number = MAC_QUEUE;
-                                err1 = smctr_rx_frame(dev);
-
-                                if(isb_subtype & MAC_RX_RESOURCE_FE)
-                                {
-                                        if((err = smctr_issue_resume_rx_fcb_cmd(                                                dev, MAC_QUEUE)) != SUCCESS)
-                                                break;
-
-                                        if(tp->ptr_rx_fcb_overruns)
-                                                (*tp->ptr_rx_fcb_overruns)++;
-                                }
-
-                                if(isb_subtype & MAC_RX_RESOURCE_BE)
-                                {
-                                        if((err = smctr_issue_resume_rx_bdb_cmd(                                                dev, MAC_QUEUE)) != SUCCESS)
-                                                break;
-
-                                        if(tp->ptr_rx_bdb_overruns)
-                                                (*tp->ptr_rx_bdb_overruns)++;
-                                }
-                                err = err1;
-                                break;
-
-                        /* Type 0x09 - NON_MAC RX Frame Interrupt */
-                        case ISB_IMC_NON_MAC_RX_FRAME:
-                                tp->rx_fifo_overrun_count = 0;
-                                tp->receive_queue_number = NON_MAC_QUEUE;
-                                err = smctr_rx_frame(dev);
-                                break;
-
-                        /* Type 0x0A - MAC RX Frame Interrupt */
-                        case ISB_IMC_MAC_RX_FRAME:
-                                tp->receive_queue_number = MAC_QUEUE;
-                                err = smctr_rx_frame(dev);
-                                break;
-
-                        /* Type 0x0B - TRC status
-                         * TRC has encountered an error condition
-                         * subtype bit 14 - transmit FIFO underrun
-                         * subtype bit 15 - receive FIFO overrun
-                         */
-                        case ISB_IMC_TRC_FIFO_STATUS:
-                                if(isb_subtype & TRC_FIFO_STATUS_TX_UNDERRUN)
-                                {
-                                        if(tp->ptr_tx_fifo_underruns)
-                                                (*tp->ptr_tx_fifo_underruns)++;
-                                }
-
-                                if(isb_subtype & TRC_FIFO_STATUS_RX_OVERRUN)
-                                {
-                                        /* update overrun stuck receive counter
-                                         * if >= 3, has to clear it by sending
-                                         * back to back frames. We pick
-                                         * DAT(duplicate address MAC frame)
-                                         */
-                                        tp->rx_fifo_overrun_count++;
-
-                                        if(tp->rx_fifo_overrun_count >= 3)
-                                        {
-                                                tp->rx_fifo_overrun_count = 0;
-
-                                                /* delay clearing fifo overrun
-                                                 * imask till send_BUG tx
-                                                 * complete posted
-                                                 */
-                                                interrupt_unmask_bits &= (~0x800);
-                                                printk(KERN_CRIT "Jay please send bug\n");//                                              smctr_send_bug(dev);
-                                        }
-
-                                        if(tp->ptr_rx_fifo_overruns)
-                                                (*tp->ptr_rx_fifo_overruns)++;
-                                }
-
-                                err = SUCCESS;
-                                break;
-
-                        /* Type 0x0C - Action Command Status Interrupt
-                         * Subtype bit 14 - CB end of command chain (CE)
-                         * Subtype bit 15 - CB command interrupt (CI)
-                         */
-                        case ISB_IMC_COMMAND_STATUS:
-                                err = SUCCESS;
-                                if(tp->acb_head->cmd == ACB_CMD_HIC_NOP)
-                                {
-                                        printk(KERN_ERR "i1\n");
-                                        smctr_disable_16bit(dev);
-
-                                        /* XXXXXXXXXXXXXXXXX */
-                                /*      err = UM_Interrupt(dev); */
-
-                                        smctr_enable_16bit(dev);
-                                }
-                                else
-                                {
-                                        if((tp->acb_head->cmd
-					    == ACB_CMD_READ_TRC_STATUS) &&
-					   (tp->acb_head->subcmd
-					    == RW_TRC_STATUS_BLOCK))
-                                        {
-                                                if(tp->ptr_bcn_type)
-                                                {
-                                                        *(tp->ptr_bcn_type)
-                                                                = (__u32)((SBlock *)tp->misc_command_data)->BCN_Type;
-                                                }
-
-                                                if(((SBlock *)tp->misc_command_data)->Status_CHG_Indicate & ERROR_COUNTERS_CHANGED)
-                                                {
-                                                        smctr_update_err_stats(dev);
-                                                }
-
-                                                if(((SBlock *)tp->misc_command_data)->Status_CHG_Indicate & TI_NDIS_RING_STATUS_CHANGED)
-                                                {
-                                                        tp->ring_status
-                                                                = ((SBlock*)tp->misc_command_data)->TI_NDIS_Ring_Status;
-                                                        smctr_disable_16bit(dev);
-                                                        err = smctr_ring_status_chg(dev);
-                                                        smctr_enable_16bit(dev);
-                                                        if((tp->ring_status & REMOVE_RECEIVED) &&
-							   (tp->config_word0 & NO_AUTOREMOVE))
-                                                        {
-                                                                smctr_issue_remove_cmd(dev);
-                                                        }
-
-                                                        if(err != SUCCESS)
-                                                        {
-                                                                tp->acb_pending = 0;
-                                                                break;
-                                                        }
-                                                }
-
-                                                if(((SBlock *)tp->misc_command_data)->Status_CHG_Indicate & UNA_CHANGED)
-                                                {
-                                                        if(tp->ptr_una)
-                                                        {
-                                                                tp->ptr_una[0] = SWAP_BYTES(((SBlock *)tp->misc_command_data)->UNA[0]);
-                                                                tp->ptr_una[1] = SWAP_BYTES(((SBlock *)tp->misc_command_data)->UNA[1]);
-                                                                tp->ptr_una[2] = SWAP_BYTES(((SBlock *)tp->misc_command_data)->UNA[2]);
-                                                        }
-
-                                                }
-
-                                                if(((SBlock *)tp->misc_command_data)->Status_CHG_Indicate & READY_TO_SEND_RQ_INIT)                                                {
-                                                        err = smctr_send_rq_init(dev);
-                                                }
-                                        }
-                                }
-
-                                tp->acb_pending = 0;
-                                break;
-
-                        /* Type 0x0D - MAC Type 1 interrupt
-                         * Subtype -- 00 FR_BCN received at S12
-                         *            01 FR_BCN received at S21
-                         *            02 FR_DAT(DA=MA, A<>0) received at S21
-                         *            03 TSM_EXP at S21
-                         *            04 FR_REMOVE received at S42
-                         *            05 TBR_EXP, BR_FLAG_SET at S42
-                         *            06 TBT_EXP at S53
-                         */
-                        case ISB_IMC_MAC_TYPE_1:
-                                if(isb_subtype > 8)
-                                {
-                                        err = HARDWARE_FAILED;
-                                        break;
-                                }
-
-                                err = SUCCESS;
-                                switch(isb_subtype)
-                                {
-                                        case 0:
-                                                tp->join_state = JS_BYPASS_STATE;
-                                                if(tp->status != CLOSED)
-                                                {
-                                                        tp->status = CLOSED;
-                                                        err = smctr_status_chg(dev);
-                                                }
-                                                break;
-
-                                        case 1:
-                                                tp->join_state = JS_LOBE_TEST_STATE;
-                                                break;
-
-                                        case 2:
-                                                tp->join_state = JS_DETECT_MONITOR_PRESENT_STATE;
-                                                break;
-
-                                        case 3:
-                                                tp->join_state = JS_AWAIT_NEW_MONITOR_STATE;
-                                                break;
-
-                                        case 4:
-                                                tp->join_state = JS_DUPLICATE_ADDRESS_TEST_STATE;
-                                                break;
-
-                                        case 5:
-                                                tp->join_state = JS_NEIGHBOR_NOTIFICATION_STATE;
-                                                break;
-
-                                        case 6:
-                                                tp->join_state = JS_REQUEST_INITIALIZATION_STATE;
-                                                break;
-
-                                        case 7:
-                                                tp->join_state = JS_JOIN_COMPLETE_STATE;
-                                                tp->status = OPEN;
-                                                err = smctr_status_chg(dev);
-                                                break;
-
-                                        case 8:
-                                                tp->join_state = JS_BYPASS_WAIT_STATE;
-                                                break;
-                                }
-                                break ;
-
-                        /* Type 0x0E - TRC Initialization Sequence Interrupt
-                         * Subtype -- 00-FF Initializatin sequence complete
-                         */
-                        case ISB_IMC_TRC_INTRNL_TST_STATUS:
-                                tp->status = INITIALIZED;
-                                smctr_disable_16bit(dev);
-                                err = smctr_status_chg(dev);
-                                smctr_enable_16bit(dev);
-                                break;
-
-                        /* other interrupt types, illegal */
-                        default:
-                                break;
-                }
-
-                if(err != SUCCESS)
-                        break;
-        }
-
-        /* Checking the ack code instead of the unmask bits here is because :
-         * while fixing the stuck receive, DAT frame are sent and mask off
-         * FIFO overrun interrupt temporarily (interrupt_unmask_bits = 0)
-         * but we still want to issue ack to ISB
-         */
-        if(!(interrupt_ack_code & 0xff00))
-                smctr_issue_int_ack(dev, interrupt_ack_code, interrupt_unmask_bits);
-
-        smctr_disable_16bit(dev);
-        smctr_enable_bic_int(dev);
-        spin_unlock(&tp->lock);
-
-        return IRQ_HANDLED;
-}
-
-static int smctr_issue_enable_int_cmd(struct net_device *dev,
-        __u16 interrupt_enable_mask)
-{
-        struct net_local *tp = netdev_priv(dev);
-        int err;
-
-        if((err = smctr_wait_while_cbusy(dev)))
-                return err;
-
-        tp->sclb_ptr->int_mask_control  = interrupt_enable_mask;
-        tp->sclb_ptr->valid_command     = SCLB_VALID | SCLB_CMD_CLEAR_INTERRUPT_MASK;
-
-        smctr_set_ctrl_attention(dev);
-
-        return 0;
-}
-
-static int smctr_issue_int_ack(struct net_device *dev, __u16 iack_code, __u16 ibits)
-{
-        struct net_local *tp = netdev_priv(dev);
-
-        if(smctr_wait_while_cbusy(dev))
-                return -1;
-
-        tp->sclb_ptr->int_mask_control = ibits;
-        tp->sclb_ptr->iack_code = iack_code << 1; /* use the offset from base */        tp->sclb_ptr->resume_control = 0;
-        tp->sclb_ptr->valid_command = SCLB_VALID | SCLB_IACK_CODE_VALID | SCLB_CMD_CLEAR_INTERRUPT_MASK;
-
-        smctr_set_ctrl_attention(dev);
-
-        return 0;
-}
-
-static int smctr_issue_init_timers_cmd(struct net_device *dev)
-{
-        struct net_local *tp = netdev_priv(dev);
-        unsigned int i;
-        int err;
-        __u16 *pTimer_Struc = (__u16 *)tp->misc_command_data;
-
-        if((err = smctr_wait_while_cbusy(dev)))
-                return err;
-
-        if((err = smctr_wait_cmd(dev)))
-                return err;
-
-        tp->config_word0 = THDREN | DMA_TRIGGER | USETPT | NO_AUTOREMOVE;
-        tp->config_word1 = 0;
-
-        if((tp->media_type == MEDIA_STP_16) ||
-	   (tp->media_type == MEDIA_UTP_16) ||
-	   (tp->media_type == MEDIA_STP_16_UTP_16))
-        {
-                tp->config_word0 |= FREQ_16MB_BIT;
-        }
-
-        if(tp->mode_bits & EARLY_TOKEN_REL)
-                tp->config_word0 |= ETREN;
-
-        if(tp->mode_bits & LOOPING_MODE_MASK)
-                tp->config_word0 |= RX_OWN_BIT;
-        else
-                tp->config_word0 &= ~RX_OWN_BIT;
-
-        if(tp->receive_mask & PROMISCUOUS_MODE)
-                tp->config_word0 |= PROMISCUOUS_BIT;
-        else
-                tp->config_word0 &= ~PROMISCUOUS_BIT;
-
-        if(tp->receive_mask & ACCEPT_ERR_PACKETS)
-                tp->config_word0 |= SAVBAD_BIT;
-        else
-                tp->config_word0 &= ~SAVBAD_BIT;
-
-        if(tp->receive_mask & ACCEPT_ATT_MAC_FRAMES)
-                tp->config_word0 |= RXATMAC;
-        else
-                tp->config_word0 &= ~RXATMAC;
-
-        if(tp->receive_mask & ACCEPT_MULTI_PROM)
-                tp->config_word1 |= MULTICAST_ADDRESS_BIT;
-        else
-                tp->config_word1 &= ~MULTICAST_ADDRESS_BIT;
-
-        if(tp->receive_mask & ACCEPT_SOURCE_ROUTING_SPANNING)
-                tp->config_word1 |= SOURCE_ROUTING_SPANNING_BITS;
-        else
-        {
-                if(tp->receive_mask & ACCEPT_SOURCE_ROUTING)
-                        tp->config_word1 |= SOURCE_ROUTING_EXPLORER_BIT;
-                else
-                        tp->config_word1 &= ~SOURCE_ROUTING_SPANNING_BITS;
-        }
-
-        if((tp->media_type == MEDIA_STP_16) ||
-	   (tp->media_type == MEDIA_UTP_16) ||
-	   (tp->media_type == MEDIA_STP_16_UTP_16))
-        {
-                tp->config_word1 |= INTERFRAME_SPACING_16;
-        }
-        else
-                tp->config_word1 |= INTERFRAME_SPACING_4;
-
-        *pTimer_Struc++ = tp->config_word0;
-        *pTimer_Struc++ = tp->config_word1;
-
-        if((tp->media_type == MEDIA_STP_4) ||
-	   (tp->media_type == MEDIA_UTP_4) ||
-	   (tp->media_type == MEDIA_STP_4_UTP_4))
-        {
-                *pTimer_Struc++ = 0x00FA;       /* prescale */
-                *pTimer_Struc++ = 0x2710;       /* TPT_limit */
-                *pTimer_Struc++ = 0x2710;       /* TQP_limit */
-                *pTimer_Struc++ = 0x0A28;       /* TNT_limit */
-                *pTimer_Struc++ = 0x3E80;       /* TBT_limit */
-                *pTimer_Struc++ = 0x3A98;       /* TSM_limit */
-                *pTimer_Struc++ = 0x1B58;       /* TAM_limit */
-                *pTimer_Struc++ = 0x00C8;       /* TBR_limit */
-                *pTimer_Struc++ = 0x07D0;       /* TER_limit */
-                *pTimer_Struc++ = 0x000A;       /* TGT_limit */
-                *pTimer_Struc++ = 0x1162;       /* THT_limit */
-                *pTimer_Struc++ = 0x07D0;       /* TRR_limit */
-                *pTimer_Struc++ = 0x1388;       /* TVX_limit */
-                *pTimer_Struc++ = 0x0000;       /* reserved */
-        }
-        else
-        {
-                *pTimer_Struc++ = 0x03E8;       /* prescale */
-                *pTimer_Struc++ = 0x9C40;       /* TPT_limit */
-                *pTimer_Struc++ = 0x9C40;       /* TQP_limit */
-                *pTimer_Struc++ = 0x0A28;       /* TNT_limit */
-                *pTimer_Struc++ = 0x3E80;       /* TBT_limit */
-                *pTimer_Struc++ = 0x3A98;       /* TSM_limit */
-                *pTimer_Struc++ = 0x1B58;       /* TAM_limit */
-                *pTimer_Struc++ = 0x00C8;       /* TBR_limit */
-                *pTimer_Struc++ = 0x07D0;       /* TER_limit */
-                *pTimer_Struc++ = 0x000A;       /* TGT_limit */
-                *pTimer_Struc++ = 0x4588;       /* THT_limit */
-                *pTimer_Struc++ = 0x1F40;       /* TRR_limit */
-                *pTimer_Struc++ = 0x4E20;       /* TVX_limit */
-                *pTimer_Struc++ = 0x0000;       /* reserved */
-        }
-
-        /* Set node address. */
-        *pTimer_Struc++ = dev->dev_addr[0] << 8
-                | (dev->dev_addr[1] & 0xFF);
-        *pTimer_Struc++ = dev->dev_addr[2] << 8
-                | (dev->dev_addr[3] & 0xFF);
-        *pTimer_Struc++ = dev->dev_addr[4] << 8
-                | (dev->dev_addr[5] & 0xFF);
-
-        /* Set group address. */
-        *pTimer_Struc++ = tp->group_address_0 << 8
-                | tp->group_address_0 >> 8;
-        *pTimer_Struc++ = tp->group_address[0] << 8
-                | tp->group_address[0] >> 8;
-        *pTimer_Struc++ = tp->group_address[1] << 8
-                | tp->group_address[1] >> 8;
-
-        /* Set functional address. */
-        *pTimer_Struc++ = tp->functional_address_0 << 8
-                | tp->functional_address_0 >> 8;
-        *pTimer_Struc++ = tp->functional_address[0] << 8
-                | tp->functional_address[0] >> 8;
-        *pTimer_Struc++ = tp->functional_address[1] << 8
-                | tp->functional_address[1] >> 8;
-
-        /* Set Bit-Wise group address. */
-        *pTimer_Struc++ = tp->bitwise_group_address[0] << 8
-                | tp->bitwise_group_address[0] >> 8;
-        *pTimer_Struc++ = tp->bitwise_group_address[1] << 8
-                | tp->bitwise_group_address[1] >> 8;
-
-        /* Set ring number address. */
-        *pTimer_Struc++ = tp->source_ring_number;
-        *pTimer_Struc++ = tp->target_ring_number;
-
-        /* Physical drop number. */
-        *pTimer_Struc++ = (unsigned short)0;
-        *pTimer_Struc++ = (unsigned short)0;
-
-        /* Product instance ID. */
-        for(i = 0; i < 9; i++)
-                *pTimer_Struc++ = (unsigned short)0;
-
-        err = smctr_setup_single_cmd_w_data(dev, ACB_CMD_INIT_TRC_TIMERS, 0);
-
-        return err;
-}
-
-static int smctr_issue_init_txrx_cmd(struct net_device *dev)
-{
-        struct net_local *tp = netdev_priv(dev);
-        unsigned int i;
-        int err;
-        void **txrx_ptrs = (void *)tp->misc_command_data;
-
-        if((err = smctr_wait_while_cbusy(dev)))
-                return err;
-
-        if((err = smctr_wait_cmd(dev)))
-	{
-                printk(KERN_ERR "%s: Hardware failure\n", dev->name);
-                return err;
-        }
-
-        /* Initialize Transmit Queue Pointers that are used, to point to
-         * a single FCB.
-         */
-        for(i = 0; i < NUM_TX_QS_USED; i++)
-                *txrx_ptrs++ = (void *)TRC_POINTER(tp->tx_fcb_head[i]);
-
-        /* Initialize Transmit Queue Pointers that are NOT used to ZERO. */
-        for(; i < MAX_TX_QS; i++)
-                *txrx_ptrs++ = (void *)0;
-
-        /* Initialize Receive Queue Pointers (MAC and Non-MAC) that are
-         * used, to point to a single FCB and a BDB chain of buffers.
-         */
-        for(i = 0; i < NUM_RX_QS_USED; i++)
-        {
-                *txrx_ptrs++ = (void *)TRC_POINTER(tp->rx_fcb_head[i]);
-                *txrx_ptrs++ = (void *)TRC_POINTER(tp->rx_bdb_head[i]);
-        }
-
-        /* Initialize Receive Queue Pointers that are NOT used to ZERO. */
-        for(; i < MAX_RX_QS; i++)
-        {
-                *txrx_ptrs++ = (void *)0;
-                *txrx_ptrs++ = (void *)0;
-        }
-
-        err = smctr_setup_single_cmd_w_data(dev, ACB_CMD_INIT_TX_RX, 0);
-
-        return err;
-}
-
-static int smctr_issue_insert_cmd(struct net_device *dev)
-{
-        int err;
-
-        err = smctr_setup_single_cmd(dev, ACB_CMD_INSERT, ACB_SUB_CMD_NOP);
-
-        return err;
-}
-
-static int smctr_issue_read_ring_status_cmd(struct net_device *dev)
-{
-        int err;
-
-        if((err = smctr_wait_while_cbusy(dev)))
-                return err;
-
-        if((err = smctr_wait_cmd(dev)))
-                return err;
-
-        err = smctr_setup_single_cmd_w_data(dev, ACB_CMD_READ_TRC_STATUS,
-                RW_TRC_STATUS_BLOCK);
-
-        return err;
-}
-
-static int smctr_issue_read_word_cmd(struct net_device *dev, __u16 aword_cnt)
-{
-        int err;
-
-        if((err = smctr_wait_while_cbusy(dev)))
-                return err;
-
-        if((err = smctr_wait_cmd(dev)))
-                return err;
-
-        err = smctr_setup_single_cmd_w_data(dev, ACB_CMD_MCT_READ_VALUE,
-                aword_cnt);
-
-        return err;
-}
-
-static int smctr_issue_remove_cmd(struct net_device *dev)
-{
-        struct net_local *tp = netdev_priv(dev);
-        int err;
-
-        if((err = smctr_wait_while_cbusy(dev)))
-                return err;
-
-        tp->sclb_ptr->resume_control    = 0;
-        tp->sclb_ptr->valid_command     = SCLB_VALID | SCLB_CMD_REMOVE;
-
-        smctr_set_ctrl_attention(dev);
-
-        return 0;
-}
-
-static int smctr_issue_resume_acb_cmd(struct net_device *dev)
-{
-        struct net_local *tp = netdev_priv(dev);
-        int err;
-
-        if((err = smctr_wait_while_cbusy(dev)))
-                return err;
-
-        tp->sclb_ptr->resume_control = SCLB_RC_ACB;
-        tp->sclb_ptr->valid_command  = SCLB_VALID | SCLB_RESUME_CONTROL_VALID;
-
-        tp->acb_pending = 1;
-
-        smctr_set_ctrl_attention(dev);
-
-        return 0;
-}
-
-static int smctr_issue_resume_rx_bdb_cmd(struct net_device *dev, __u16 queue)
-{
-        struct net_local *tp = netdev_priv(dev);
-        int err;
-
-        if((err = smctr_wait_while_cbusy(dev)))
-                return err;
-
-        if(queue == MAC_QUEUE)
-                tp->sclb_ptr->resume_control = SCLB_RC_RX_MAC_BDB;
-        else
-                tp->sclb_ptr->resume_control = SCLB_RC_RX_NON_MAC_BDB;
-
-        tp->sclb_ptr->valid_command = SCLB_VALID | SCLB_RESUME_CONTROL_VALID;
-
-        smctr_set_ctrl_attention(dev);
-
-        return 0;
-}
-
-static int smctr_issue_resume_rx_fcb_cmd(struct net_device *dev, __u16 queue)
-{
-        struct net_local *tp = netdev_priv(dev);
-
-        if(smctr_debug > 10)
-                printk(KERN_DEBUG "%s: smctr_issue_resume_rx_fcb_cmd\n", dev->name);
-
-        if(smctr_wait_while_cbusy(dev))
-                return -1;
-
-        if(queue == MAC_QUEUE)
-                tp->sclb_ptr->resume_control = SCLB_RC_RX_MAC_FCB;
-        else
-                tp->sclb_ptr->resume_control = SCLB_RC_RX_NON_MAC_FCB;
-
-        tp->sclb_ptr->valid_command = SCLB_VALID | SCLB_RESUME_CONTROL_VALID;
-
-        smctr_set_ctrl_attention(dev);
-
-        return 0;
-}
-
-static int smctr_issue_resume_tx_fcb_cmd(struct net_device *dev, __u16 queue)
-{
-        struct net_local *tp = netdev_priv(dev);
-
-        if(smctr_debug > 10)
-                printk(KERN_DEBUG "%s: smctr_issue_resume_tx_fcb_cmd\n", dev->name);
-
-        if(smctr_wait_while_cbusy(dev))
-                return -1;
-
-        tp->sclb_ptr->resume_control = (SCLB_RC_TFCB0 << queue);
-        tp->sclb_ptr->valid_command = SCLB_RESUME_CONTROL_VALID | SCLB_VALID;
-
-        smctr_set_ctrl_attention(dev);
-
-        return 0;
-}
-
-static int smctr_issue_test_internal_rom_cmd(struct net_device *dev)
-{
-        int err;
-
-        err = smctr_setup_single_cmd(dev, ACB_CMD_MCT_TEST,
-                TRC_INTERNAL_ROM_TEST);
-
-        return err;
-}
-
-static int smctr_issue_test_hic_cmd(struct net_device *dev)
-{
-        int err;
-
-        err = smctr_setup_single_cmd(dev, ACB_CMD_HIC_TEST,
-                TRC_HOST_INTERFACE_REG_TEST);
-
-        return err;
-}
-
-static int smctr_issue_test_mac_reg_cmd(struct net_device *dev)
-{
-        int err;
-
-        err = smctr_setup_single_cmd(dev, ACB_CMD_MCT_TEST,
-                TRC_MAC_REGISTERS_TEST);
-
-        return err;
-}
-
-static int smctr_issue_trc_loopback_cmd(struct net_device *dev)
-{
-        int err;
-
-        err = smctr_setup_single_cmd(dev, ACB_CMD_MCT_TEST,
-                TRC_INTERNAL_LOOPBACK);
-
-        return err;
-}
-
-static int smctr_issue_tri_loopback_cmd(struct net_device *dev)
-{
-        int err;
-
-        err = smctr_setup_single_cmd(dev, ACB_CMD_MCT_TEST,
-                TRC_TRI_LOOPBACK);
-
-        return err;
-}
-
-static int smctr_issue_write_byte_cmd(struct net_device *dev,
-        short aword_cnt, void *byte)
-{
-	struct net_local *tp = netdev_priv(dev);
-        unsigned int iword, ibyte;
-	int err;
-
-        if((err = smctr_wait_while_cbusy(dev)))
-                return err;
-
-        if((err = smctr_wait_cmd(dev)))
-                return err;
-
-        for(iword = 0, ibyte = 0; iword < (unsigned int)(aword_cnt & 0xff);
-        	iword++, ibyte += 2)
-        {
-                tp->misc_command_data[iword] = (*((__u8 *)byte + ibyte) << 8)
-			| (*((__u8 *)byte + ibyte + 1));
-        }
-
-        return smctr_setup_single_cmd_w_data(dev, ACB_CMD_MCT_WRITE_VALUE,
-		aword_cnt);
-}
-
-static int smctr_issue_write_word_cmd(struct net_device *dev,
-        short aword_cnt, void *word)
-{
-        struct net_local *tp = netdev_priv(dev);
-        unsigned int i, err;
-
-        if((err = smctr_wait_while_cbusy(dev)))
-                return err;
-
-        if((err = smctr_wait_cmd(dev)))
-                return err;
-
-        for(i = 0; i < (unsigned int)(aword_cnt & 0xff); i++)
-                tp->misc_command_data[i] = *((__u16 *)word + i);
-
-        err = smctr_setup_single_cmd_w_data(dev, ACB_CMD_MCT_WRITE_VALUE,
-                aword_cnt);
-
-        return err;
-}
-
-static int smctr_join_complete_state(struct net_device *dev)
-{
-        int err;
-
-        err = smctr_setup_single_cmd(dev, ACB_CMD_CHANGE_JOIN_STATE,
-                JS_JOIN_COMPLETE_STATE);
-
-        return err;
-}
-
-static int smctr_link_tx_fcbs_to_bdbs(struct net_device *dev)
-{
-        struct net_local *tp = netdev_priv(dev);
-        unsigned int i, j;
-        FCBlock *fcb;
-        BDBlock *bdb;
-
-        for(i = 0; i < NUM_TX_QS_USED; i++)
-        {
-                fcb = tp->tx_fcb_head[i];
-                bdb = tp->tx_bdb_head[i];
-
-                for(j = 0; j < tp->num_tx_fcbs[i]; j++)
-                {
-                        fcb->bdb_ptr            = bdb;
-                        fcb->trc_bdb_ptr        = TRC_POINTER(bdb);
-                        fcb = (FCBlock *)((char *)fcb + sizeof(FCBlock));
-                        bdb = (BDBlock *)((char *)bdb + sizeof(BDBlock));
-                }
-        }
-
-        return 0;
-}
-
-static int smctr_load_firmware(struct net_device *dev)
-{
-        struct net_local *tp = netdev_priv(dev);
-	const struct firmware *fw;
-        __u16 i, checksum = 0;
-        int err = 0;
-
-        if(smctr_debug > 10)
-                printk(KERN_DEBUG "%s: smctr_load_firmware\n", dev->name);
-
-	if (request_firmware(&fw, "tr_smctr.bin", &dev->dev)) {
-		printk(KERN_ERR "%s: firmware not found\n", dev->name);
-		return UCODE_NOT_PRESENT;
-	}
-
-        tp->num_of_tx_buffs     = 4;
-        tp->mode_bits          |= UMAC;
-        tp->receive_mask        = 0;
-        tp->max_packet_size     = 4177;
-
-        /* Can only upload the firmware once per adapter reset. */
-        if (tp->microcode_version != 0) {
-		err = (UCODE_PRESENT);
-		goto out;
-	}
-
-        /* Verify the firmware exists and is there in the right amount. */
-        if (!fw->data ||
-	    (*(fw->data + UCODE_VERSION_OFFSET) < UCODE_VERSION))
-        {
-                err = (UCODE_NOT_PRESENT);
-		goto out;
-        }
-
-        /* UCODE_SIZE is not included in Checksum. */
-        for(i = 0; i < *((__u16 *)(fw->data + UCODE_SIZE_OFFSET)); i += 2)
-                checksum += *((__u16 *)(fw->data + 2 + i));
-        if (checksum) {
-		err = (UCODE_NOT_PRESENT);
-		goto out;
-	}
-
-        /* At this point we have a valid firmware image, lets kick it on up. */
-        smctr_enable_adapter_ram(dev);
-        smctr_enable_16bit(dev);
-        smctr_set_page(dev, (__u8 *)tp->ram_access);
-
-        if((smctr_checksum_firmware(dev)) ||
-	   (*(fw->data + UCODE_VERSION_OFFSET) > tp->microcode_version))
-        {
-                smctr_enable_adapter_ctrl_store(dev);
-
-                /* Zero out ram space for firmware. */
-                for(i = 0; i < CS_RAM_SIZE; i += 2)
-                        *((__u16 *)(tp->ram_access + i)) = 0;
-
-                smctr_decode_firmware(dev, fw);
-
-                tp->microcode_version = *(fw->data + UCODE_VERSION_OFFSET);                *((__u16 *)(tp->ram_access + CS_RAM_VERSION_OFFSET))
-                        = (tp->microcode_version << 8);
-                *((__u16 *)(tp->ram_access + CS_RAM_CHECKSUM_OFFSET))
-                        = ~(tp->microcode_version << 8) + 1;
-
-                smctr_disable_adapter_ctrl_store(dev);
-
-                if(smctr_checksum_firmware(dev))
-                        err = HARDWARE_FAILED;
-        }
-        else
-                err = UCODE_PRESENT;
-
-        smctr_disable_16bit(dev);
- out:
-	release_firmware(fw);
-        return err;
-}
-
-static int smctr_load_node_addr(struct net_device *dev)
-{
-        int ioaddr = dev->base_addr;
-        unsigned int i;
-        __u8 r;
-
-        for(i = 0; i < 6; i++)
-        {
-                r = inb(ioaddr + LAR0 + i);
-                dev->dev_addr[i] = (char)r;
-        }
-        dev->addr_len = 6;
-
-        return 0;
-}
-
-/* Lobe Media Test.
- * During the transmission of the initial 1500 lobe media MAC frames,
- * the phase lock loop in the 805 chip may lock, and then un-lock, causing
- * the 825 to go into a PURGE state. When performing a PURGE, the MCT
- * microcode will not transmit any frames given to it by the host, and
- * will consequently cause a timeout.
- *
- * NOTE 1: If the monitor_state is MS_BEACON_TEST_STATE, all transmit
- * queues other than the one used for the lobe_media_test should be
- * disabled.!?
- *
- * NOTE 2: If the monitor_state is MS_BEACON_TEST_STATE and the receive_mask
- * has any multi-cast or promiscuous bits set, the receive_mask needs to
- * be changed to clear the multi-cast or promiscuous mode bits, the lobe_test
- * run, and then the receive mask set back to its original value if the test
- * is successful.
- */
-static int smctr_lobe_media_test(struct net_device *dev)
-{
-        struct net_local *tp = netdev_priv(dev);
-        unsigned int i, perror = 0;
-        unsigned short saved_rcv_mask;
-
-        if(smctr_debug > 10)
-                printk(KERN_DEBUG "%s: smctr_lobe_media_test\n", dev->name);
-
-        /* Clear receive mask for lobe test. */
-        saved_rcv_mask          = tp->receive_mask;
-        tp->receive_mask        = 0;
-
-        smctr_chg_rx_mask(dev);
-
-        /* Setup the lobe media test. */
-        smctr_lobe_media_test_cmd(dev);
-        if(smctr_wait_cmd(dev))
-		goto err;
-
-        /* Tx lobe media test frames. */
-        for(i = 0; i < 1500; ++i)
-        {
-                if(smctr_send_lobe_media_test(dev))
-                {
-                        if(perror)
-				goto err;
-                        else
-                        {
-                                perror = 1;
-                                if(smctr_lobe_media_test_cmd(dev))
-					goto err;
-                        }
-                }
-        }
-
-        if(smctr_send_dat(dev))
-        {
-                if(smctr_send_dat(dev))
-			goto err;
-        }
-
-        /* Check if any frames received during test. */
-        if((tp->rx_fcb_curr[MAC_QUEUE]->frame_status) ||
-	   (tp->rx_fcb_curr[NON_MAC_QUEUE]->frame_status))
-		goto err;
-
-        /* Set receive mask to "Promisc" mode. */
-        tp->receive_mask = saved_rcv_mask;
-
-        smctr_chg_rx_mask(dev);
-
-	 return 0;
-err:
-	smctr_reset_adapter(dev);
-	tp->status = CLOSED;
-	return LOBE_MEDIA_TEST_FAILED;
-}
-
-static int smctr_lobe_media_test_cmd(struct net_device *dev)
-{
-        struct net_local *tp = netdev_priv(dev);
-        int err;
-
-        if(smctr_debug > 10)
-                printk(KERN_DEBUG "%s: smctr_lobe_media_test_cmd\n", dev->name);
-
-        /* Change to lobe media test state. */
-        if(tp->monitor_state != MS_BEACON_TEST_STATE)
-        {
-                smctr_lobe_media_test_state(dev);
-                if(smctr_wait_cmd(dev))
-                {
-                        printk(KERN_ERR "Lobe Failed test state\n");
-                        return LOBE_MEDIA_TEST_FAILED;
-                }
-        }
-
-        err = smctr_setup_single_cmd(dev, ACB_CMD_MCT_TEST,
-                TRC_LOBE_MEDIA_TEST);
-
-        return err;
-}
-
-static int smctr_lobe_media_test_state(struct net_device *dev)
-{
-        int err;
-
-        err = smctr_setup_single_cmd(dev, ACB_CMD_CHANGE_JOIN_STATE,
-                JS_LOBE_TEST_STATE);
-
-        return err;
-}
-
-static int smctr_make_8025_hdr(struct net_device *dev,
-        MAC_HEADER *rmf, MAC_HEADER *tmf, __u16 ac_fc)
-{
-        tmf->ac = MSB(ac_fc);                 /* msb is access control */
-        tmf->fc = LSB(ac_fc);                 /* lsb is frame control */
-
-        tmf->sa[0] = dev->dev_addr[0];
-        tmf->sa[1] = dev->dev_addr[1];
-        tmf->sa[2] = dev->dev_addr[2];
-        tmf->sa[3] = dev->dev_addr[3];
-        tmf->sa[4] = dev->dev_addr[4];
-        tmf->sa[5] = dev->dev_addr[5];
-
-        switch(tmf->vc)
-        {
-		/* Send RQ_INIT to RPS */
-                case RQ_INIT:
-                        tmf->da[0] = 0xc0;
-                        tmf->da[1] = 0x00;
-                        tmf->da[2] = 0x00;
-                        tmf->da[3] = 0x00;
-                        tmf->da[4] = 0x00;
-                        tmf->da[5] = 0x02;
-                        break;
-
-		/* Send RPT_TX_FORWARD to CRS */
-                case RPT_TX_FORWARD:
-                        tmf->da[0] = 0xc0;
-                        tmf->da[1] = 0x00;
-                        tmf->da[2] = 0x00;
-                        tmf->da[3] = 0x00;
-                        tmf->da[4] = 0x00;
-                        tmf->da[5] = 0x10;
-                        break;
-
-		/* Everything else goes to sender */
-                default:
-                        tmf->da[0] = rmf->sa[0];
-                        tmf->da[1] = rmf->sa[1];
-                        tmf->da[2] = rmf->sa[2];
-                        tmf->da[3] = rmf->sa[3];
-                        tmf->da[4] = rmf->sa[4];
-                        tmf->da[5] = rmf->sa[5];
-                        break;
-        }
-
-        return 0;
-}
-
-static int smctr_make_access_pri(struct net_device *dev, MAC_SUB_VECTOR *tsv)
-{
-        struct net_local *tp = netdev_priv(dev);
-
-        tsv->svi = AUTHORIZED_ACCESS_PRIORITY;
-        tsv->svl = S_AUTHORIZED_ACCESS_PRIORITY;
-
-        tsv->svv[0] = MSB(tp->authorized_access_priority);
-        tsv->svv[1] = LSB(tp->authorized_access_priority);
-
-	return 0;
-}
-
-static int smctr_make_addr_mod(struct net_device *dev, MAC_SUB_VECTOR *tsv)
-{
-        tsv->svi = ADDRESS_MODIFER;
-        tsv->svl = S_ADDRESS_MODIFER;
-
-        tsv->svv[0] = 0;
-        tsv->svv[1] = 0;
-
-        return 0;
-}
-
-static int smctr_make_auth_funct_class(struct net_device *dev,
-        MAC_SUB_VECTOR *tsv)
-{
-        struct net_local *tp = netdev_priv(dev);
-
-        tsv->svi = AUTHORIZED_FUNCTION_CLASS;
-        tsv->svl = S_AUTHORIZED_FUNCTION_CLASS;
-
-        tsv->svv[0] = MSB(tp->authorized_function_classes);
-        tsv->svv[1] = LSB(tp->authorized_function_classes);
-
-        return 0;
-}
-
-static int smctr_make_corr(struct net_device *dev,
-        MAC_SUB_VECTOR *tsv, __u16 correlator)
-{
-        tsv->svi = CORRELATOR;
-        tsv->svl = S_CORRELATOR;
-
-        tsv->svv[0] = MSB(correlator);
-        tsv->svv[1] = LSB(correlator);
-
-        return 0;
-}
-
-static int smctr_make_funct_addr(struct net_device *dev, MAC_SUB_VECTOR *tsv)
-{
-        struct net_local *tp = netdev_priv(dev);
-
-        smctr_get_functional_address(dev);
-
-        tsv->svi = FUNCTIONAL_ADDRESS;
-        tsv->svl = S_FUNCTIONAL_ADDRESS;
-
-        tsv->svv[0] = MSB(tp->misc_command_data[0]);
-        tsv->svv[1] = LSB(tp->misc_command_data[0]);
-
-        tsv->svv[2] = MSB(tp->misc_command_data[1]);
-        tsv->svv[3] = LSB(tp->misc_command_data[1]);
-
-        return 0;
-}
-
-static int smctr_make_group_addr(struct net_device *dev, MAC_SUB_VECTOR *tsv)
-{
-        struct net_local *tp = netdev_priv(dev);
-
-        smctr_get_group_address(dev);
-
-        tsv->svi = GROUP_ADDRESS;
-        tsv->svl = S_GROUP_ADDRESS;
-
-        tsv->svv[0] = MSB(tp->misc_command_data[0]);
-        tsv->svv[1] = LSB(tp->misc_command_data[0]);
-
-        tsv->svv[2] = MSB(tp->misc_command_data[1]);
-        tsv->svv[3] = LSB(tp->misc_command_data[1]);
-
-        /* Set Group Address Sub-vector to all zeros if only the
-         * Group Address/Functional Address Indicator is set.
-         */
-        if(tsv->svv[0] == 0x80 && tsv->svv[1] == 0x00 &&
-	   tsv->svv[2] == 0x00 && tsv->svv[3] == 0x00)
-                tsv->svv[0] = 0x00;
-
-        return 0;
-}
-
-static int smctr_make_phy_drop_num(struct net_device *dev,
-        MAC_SUB_VECTOR *tsv)
-{
-        struct net_local *tp = netdev_priv(dev);
-
-        smctr_get_physical_drop_number(dev);
-
-        tsv->svi = PHYSICAL_DROP;
-        tsv->svl = S_PHYSICAL_DROP;
-
-        tsv->svv[0] = MSB(tp->misc_command_data[0]);
-        tsv->svv[1] = LSB(tp->misc_command_data[0]);
-
-        tsv->svv[2] = MSB(tp->misc_command_data[1]);
-        tsv->svv[3] = LSB(tp->misc_command_data[1]);
-
-        return 0;
-}
-
-static int smctr_make_product_id(struct net_device *dev, MAC_SUB_VECTOR *tsv)
-{
-        int i;
-
-        tsv->svi = PRODUCT_INSTANCE_ID;
-        tsv->svl = S_PRODUCT_INSTANCE_ID;
-
-        for(i = 0; i < 18; i++)
-                tsv->svv[i] = 0xF0;
-
-        return 0;
-}
-
-static int smctr_make_station_id(struct net_device *dev, MAC_SUB_VECTOR *tsv)
-{
-        struct net_local *tp = netdev_priv(dev);
-
-        smctr_get_station_id(dev);
-
-        tsv->svi = STATION_IDENTIFER;
-        tsv->svl = S_STATION_IDENTIFER;
-
-        tsv->svv[0] = MSB(tp->misc_command_data[0]);
-        tsv->svv[1] = LSB(tp->misc_command_data[0]);
-
-        tsv->svv[2] = MSB(tp->misc_command_data[1]);
-        tsv->svv[3] = LSB(tp->misc_command_data[1]);
-
-        tsv->svv[4] = MSB(tp->misc_command_data[2]);
-        tsv->svv[5] = LSB(tp->misc_command_data[2]);
-
-        return 0;
-}
-
-static int smctr_make_ring_station_status(struct net_device *dev,
-        MAC_SUB_VECTOR * tsv)
-{
-        tsv->svi = RING_STATION_STATUS;
-        tsv->svl = S_RING_STATION_STATUS;
-
-        tsv->svv[0] = 0;
-        tsv->svv[1] = 0;
-        tsv->svv[2] = 0;
-        tsv->svv[3] = 0;
-        tsv->svv[4] = 0;
-        tsv->svv[5] = 0;
-
-        return 0;
-}
-
-static int smctr_make_ring_station_version(struct net_device *dev,
-        MAC_SUB_VECTOR *tsv)
-{
-        struct net_local *tp = netdev_priv(dev);
-
-        tsv->svi = RING_STATION_VERSION_NUMBER;
-        tsv->svl = S_RING_STATION_VERSION_NUMBER;
-
-        tsv->svv[0] = 0xe2;            /* EBCDIC - S */
-        tsv->svv[1] = 0xd4;            /* EBCDIC - M */
-        tsv->svv[2] = 0xc3;            /* EBCDIC - C */
-        tsv->svv[3] = 0x40;            /* EBCDIC -   */
-        tsv->svv[4] = 0xe5;            /* EBCDIC - V */
-        tsv->svv[5] = 0xF0 + (tp->microcode_version >> 4);
-        tsv->svv[6] = 0xF0 + (tp->microcode_version & 0x0f);
-        tsv->svv[7] = 0x40;            /* EBCDIC -   */
-        tsv->svv[8] = 0xe7;            /* EBCDIC - X */
-
-        if(tp->extra_info & CHIP_REV_MASK)
-                tsv->svv[9] = 0xc5;    /* EBCDIC - E */
-        else
-                tsv->svv[9] = 0xc4;    /* EBCDIC - D */
-
-        return 0;
-}
-
-static int smctr_make_tx_status_code(struct net_device *dev,
-        MAC_SUB_VECTOR *tsv, __u16 tx_fstatus)
-{
-        tsv->svi = TRANSMIT_STATUS_CODE;
-        tsv->svl = S_TRANSMIT_STATUS_CODE;
-
-	tsv->svv[0] = ((tx_fstatus & 0x0100 >> 6) | IBM_PASS_SOURCE_ADDR);
-
-        /* Stripped frame status of Transmitted Frame */
-        tsv->svv[1] = tx_fstatus & 0xff;
-
-        return 0;
-}
-
-static int smctr_make_upstream_neighbor_addr(struct net_device *dev,
-        MAC_SUB_VECTOR *tsv)
-{
-        struct net_local *tp = netdev_priv(dev);
-
-        smctr_get_upstream_neighbor_addr(dev);
-
-        tsv->svi = UPSTREAM_NEIGHBOR_ADDRESS;
-        tsv->svl = S_UPSTREAM_NEIGHBOR_ADDRESS;
-
-        tsv->svv[0] = MSB(tp->misc_command_data[0]);
-        tsv->svv[1] = LSB(tp->misc_command_data[0]);
-
-        tsv->svv[2] = MSB(tp->misc_command_data[1]);
-        tsv->svv[3] = LSB(tp->misc_command_data[1]);
-
-        tsv->svv[4] = MSB(tp->misc_command_data[2]);
-        tsv->svv[5] = LSB(tp->misc_command_data[2]);
-
-        return 0;
-}
-
-static int smctr_make_wrap_data(struct net_device *dev, MAC_SUB_VECTOR *tsv)
-{
-        tsv->svi = WRAP_DATA;
-        tsv->svl = S_WRAP_DATA;
-
-        return 0;
-}
-
-/*
- * Open/initialize the board. This is called sometime after
- * booting when the 'ifconfig' program is run.
- *
- * This routine should set everything up anew at each open, even
- * registers that "should" only need to be set once at boot, so that
- * there is non-reboot way to recover if something goes wrong.
- */
-static int smctr_open(struct net_device *dev)
-{
-        int err;
-
-        if(smctr_debug > 10)
-                printk(KERN_DEBUG "%s: smctr_open\n", dev->name);
-
-        err = smctr_init_adapter(dev);
-        if(err < 0)
-                return err;
-
-        return err;
-}
-
-/* Interrupt driven open of Token card. */
-static int smctr_open_tr(struct net_device *dev)
-{
-        struct net_local *tp = netdev_priv(dev);
-        unsigned long flags;
-        int err;
-
-        if(smctr_debug > 10)
-                printk(KERN_DEBUG "%s: smctr_open_tr\n", dev->name);
-
-        /* Now we can actually open the adapter. */
-        if(tp->status == OPEN)
-                return 0;
-        if(tp->status != INITIALIZED)
-                return -1;
-
-	/* FIXME: it would work a lot better if we masked the irq sources
-	   on the card here, then we could skip the locking and poll nicely */
-	spin_lock_irqsave(&tp->lock, flags);
-	
-        smctr_set_page(dev, (__u8 *)tp->ram_access);
-
-        if((err = smctr_issue_resume_rx_fcb_cmd(dev, (short)MAC_QUEUE)))
-                goto out;
-
-        if((err = smctr_issue_resume_rx_bdb_cmd(dev, (short)MAC_QUEUE)))
-                goto out;
-
-        if((err = smctr_issue_resume_rx_fcb_cmd(dev, (short)NON_MAC_QUEUE)))
-                goto out;
-
-        if((err = smctr_issue_resume_rx_bdb_cmd(dev, (short)NON_MAC_QUEUE)))
-                goto out;
-
-        tp->status = CLOSED;
-
-        /* Insert into the Ring or Enter Loopback Mode. */
-        if((tp->mode_bits & LOOPING_MODE_MASK) == LOOPBACK_MODE_1)
-        {
-                tp->status = CLOSED;
-
-                if(!(err = smctr_issue_trc_loopback_cmd(dev)))
-                {
-                        if(!(err = smctr_wait_cmd(dev)))
-                                tp->status = OPEN;
-                }
-
-                smctr_status_chg(dev);
-        }
-        else
-        {
-                if((tp->mode_bits & LOOPING_MODE_MASK) == LOOPBACK_MODE_2)
-                {
-                        tp->status = CLOSED;
-                        if(!(err = smctr_issue_tri_loopback_cmd(dev)))
-                        {
-                                if(!(err = smctr_wait_cmd(dev)))
-                                        tp->status = OPEN;
-                        }
-
-                        smctr_status_chg(dev);
-                }
-                else
-                {
-                        if((tp->mode_bits & LOOPING_MODE_MASK)
-                                == LOOPBACK_MODE_3)
-                        {
-                                tp->status = CLOSED;
-                                if(!(err = smctr_lobe_media_test_cmd(dev)))
-                                {
-                                        if(!(err = smctr_wait_cmd(dev)))
-                                                tp->status = OPEN;
-                                }
-                                smctr_status_chg(dev);
-                        }
-                        else
-                        {
-                                if(!(err = smctr_lobe_media_test(dev)))
-                                        err = smctr_issue_insert_cmd(dev);
-				else
-                                {
-                                        if(err == LOBE_MEDIA_TEST_FAILED)
-                                                printk(KERN_WARNING "%s: Lobe Media Test Failure - Check cable?\n", dev->name);
-                                }
-                        }
-                }
-        }
-
-out:
-        spin_unlock_irqrestore(&tp->lock, flags);
-
-        return err;
-}
-
-/* Check for a network adapter of this type, 
- * and return device structure if one exists.
- */
-struct net_device __init *smctr_probe(int unit)
-{
-	struct net_device *dev = alloc_trdev(sizeof(struct net_local));
-	static const unsigned ports[] = {
-		0x200, 0x220, 0x240, 0x260, 0x280, 0x2A0, 0x2C0, 0x2E0, 0x300,
-		0x320, 0x340, 0x360, 0x380, 0
-	};
-	const unsigned *port;
-        int err = 0;
-
-	if (!dev)
-		return ERR_PTR(-ENOMEM);
-
-	if (unit >= 0) {
-		sprintf(dev->name, "tr%d", unit);
-		netdev_boot_setup_check(dev);
-	}
-
-        if (dev->base_addr > 0x1ff)    /* Check a single specified location. */
-		err = smctr_probe1(dev, dev->base_addr);
-        else if(dev->base_addr != 0)  /* Don't probe at all. */
-                err =-ENXIO;
-	else {
-		for (port = ports; *port; port++) {
-			err = smctr_probe1(dev, *port);
-			if (!err)
-				break;
-		}
-	}
-	if (err)
-		goto out;
-	err = register_netdev(dev);
-	if (err)
-		goto out1;
-	return dev;
-out1:
-#ifdef CONFIG_MCA_LEGACY
-	{ struct net_local *tp = netdev_priv(dev);
-	  if (tp->slot_num)
-		mca_mark_as_unused(tp->slot_num);
-	}
-#endif
-	release_region(dev->base_addr, SMCTR_IO_EXTENT);
-	free_irq(dev->irq, dev);
-out:
-	free_netdev(dev);
-	return ERR_PTR(err);
-}
-
-static const struct net_device_ops smctr_netdev_ops = {
-	.ndo_open          = smctr_open,
-	.ndo_stop          = smctr_close,
-	.ndo_start_xmit    = smctr_send_packet,
-	.ndo_tx_timeout	   = smctr_timeout,
-	.ndo_get_stats     = smctr_get_stats,
-	.ndo_set_rx_mode   = smctr_set_multicast_list,
-};
-
-static int __init smctr_probe1(struct net_device *dev, int ioaddr)
-{
-        static unsigned version_printed;
-        struct net_local *tp = netdev_priv(dev);
-        int err;
-        __u32 *ram;
-
-        if(smctr_debug && version_printed++ == 0)
-                printk(version);
-
-        spin_lock_init(&tp->lock);
-        dev->base_addr = ioaddr;
-
-	/* Actually detect an adapter now. */
-        err = smctr_chk_isa(dev);
-        if(err < 0)
-        {
-		if ((err = smctr_chk_mca(dev)) < 0) {
-			err = -ENODEV;
-			goto out;
-		}
-        }
-
-        tp = netdev_priv(dev);
-        dev->mem_start = tp->ram_base;
-        dev->mem_end = dev->mem_start + 0x10000;
-        ram = (__u32 *)phys_to_virt(dev->mem_start);
-        tp->ram_access = *(__u32 *)&ram;
-	tp->status = NOT_INITIALIZED;
-
-        err = smctr_load_firmware(dev);
-        if(err != UCODE_PRESENT && err != SUCCESS)
-        {
-                printk(KERN_ERR "%s: Firmware load failed (%d)\n", dev->name, err);
-		err = -EIO;
-		goto out;
-        }
-
-	/* Allow user to specify ring speed on module insert. */
-	if(ringspeed == 4)
-		tp->media_type = MEDIA_UTP_4;
-	else
-		tp->media_type = MEDIA_UTP_16;
-
-        printk(KERN_INFO "%s: %s %s at Io %#4x, Irq %d, Rom %#4x, Ram %#4x.\n",
-                dev->name, smctr_name, smctr_model,
-                (unsigned int)dev->base_addr,
-                dev->irq, tp->rom_base, tp->ram_base);
-
-	dev->netdev_ops = &smctr_netdev_ops;
-        dev->watchdog_timeo	= HZ;
-        return 0;
-
-out:
-	return err;
-}
-
-static int smctr_process_rx_packet(MAC_HEADER *rmf, __u16 size,
-        struct net_device *dev, __u16 rx_status)
-{
-        struct net_local *tp = netdev_priv(dev);
-        struct sk_buff *skb;
-        __u16 rcode, correlator;
-        int err = 0;
-        __u8 xframe = 1;
-
-        rmf->vl = SWAP_BYTES(rmf->vl);
-        if(rx_status & FCB_RX_STATUS_DA_MATCHED)
-        {
-                switch(rmf->vc)
-                {
-                        /* Received MAC Frames Processed by RS. */
-                        case INIT:
-                                if((rcode = smctr_rcv_init(dev, rmf, &correlator)) == HARDWARE_FAILED)
-                                {
-                                        return rcode;
-                                }
-
-                                if((err = smctr_send_rsp(dev, rmf, rcode,
-                                        correlator)))
-                                {
-                                        return err;
-                                }
-                                break;
-
-                        case CHG_PARM:
-                                if((rcode = smctr_rcv_chg_param(dev, rmf,
-                                        &correlator)) ==HARDWARE_FAILED)
-                                {
-                                        return rcode;
-                                }
-
-                                if((err = smctr_send_rsp(dev, rmf, rcode,
-                                        correlator)))
-                                {
-                                        return err;
-                                }
-                                break;
-
-                        case RQ_ADDR:
-                                if((rcode = smctr_rcv_rq_addr_state_attch(dev,
-                                        rmf, &correlator)) != POSITIVE_ACK)
-                                {
-                                        if(rcode == HARDWARE_FAILED)
-                                                return rcode;
-                                        else
-                                                return smctr_send_rsp(dev, rmf,
-                                                        rcode, correlator);
-                                }
-
-                                if((err = smctr_send_rpt_addr(dev, rmf,
-                                        correlator)))
-                                {
-                                        return err;
-                                }
-                                break;
-
-                        case RQ_ATTCH:
-                                if((rcode = smctr_rcv_rq_addr_state_attch(dev,
-                                        rmf, &correlator)) != POSITIVE_ACK)
-                                {
-                                        if(rcode == HARDWARE_FAILED)
-                                                return rcode;
-                                        else
-                                                return smctr_send_rsp(dev, rmf,
-                                                        rcode,
-                                                        correlator);
-                                }
-
-                                if((err = smctr_send_rpt_attch(dev, rmf,
-                                        correlator)))
-                                {
-                                        return err;
-                                }
-                                break;
-
-                        case RQ_STATE:
-                                if((rcode = smctr_rcv_rq_addr_state_attch(dev,
-                                        rmf, &correlator)) != POSITIVE_ACK)
-                                {
-                                        if(rcode == HARDWARE_FAILED)
-                                                return rcode;
-                                        else
-                                                return smctr_send_rsp(dev, rmf,
-                                                        rcode,
-                                                        correlator);
-                                }
-
-                                if((err = smctr_send_rpt_state(dev, rmf,
-                                        correlator)))
-                                {
-                                        return err;
-                                }
-                                break;
-
-                        case TX_FORWARD: {
-        			__u16 uninitialized_var(tx_fstatus);
-
-                                if((rcode = smctr_rcv_tx_forward(dev, rmf))
-                                        != POSITIVE_ACK)
-                                {
-                                        if(rcode == HARDWARE_FAILED)
-                                                return rcode;
-                                        else
-                                                return smctr_send_rsp(dev, rmf,
-                                                        rcode,
-                                                        correlator);
-                                }
-
-                                if((err = smctr_send_tx_forward(dev, rmf,
-                                        &tx_fstatus)) == HARDWARE_FAILED)
-                                {
-                                        return err;
-                                }
-
-                                if(err == A_FRAME_WAS_FORWARDED)
-                                {
-                                        if((err = smctr_send_rpt_tx_forward(dev,
-						rmf, tx_fstatus))
-                                                == HARDWARE_FAILED)
-                                        {
-                                                return err;
-                                        }
-                                }
-                                break;
-			}
-
-                        /* Received MAC Frames Processed by CRS/REM/RPS. */
-                        case RSP:
-                        case RQ_INIT:
-                        case RPT_NEW_MON:
-                        case RPT_SUA_CHG:
-                        case RPT_ACTIVE_ERR:
-                        case RPT_NN_INCMP:
-                        case RPT_ERROR:
-                        case RPT_ATTCH:
-                        case RPT_STATE:
-                        case RPT_ADDR:
-                                break;
-
-                        /* Rcvd Att. MAC Frame (if RXATMAC set) or UNKNOWN */
-                        default:
-                                xframe = 0;
-                                if(!(tp->receive_mask & ACCEPT_ATT_MAC_FRAMES))
-                                {
-                                        rcode = smctr_rcv_unknown(dev, rmf,
-                                                &correlator);
-                                        if((err = smctr_send_rsp(dev, rmf,rcode,
-                                                correlator)))
-                                        {
-                                                return err;
-                                        }
-                                }
-
-                                break;
-                }
-        }
-        else
-        {
-                /* 1. DA doesn't match (Promiscuous Mode).
-                 * 2. Parse for Extended MAC Frame Type.
-                 */
-                switch(rmf->vc)
-                {
-                        case RSP:
-                        case INIT:
-                        case RQ_INIT:
-                        case RQ_ADDR:
-                        case RQ_ATTCH:
-                        case RQ_STATE:
-                        case CHG_PARM:
-                        case RPT_ADDR:
-                        case RPT_ERROR:
-                        case RPT_ATTCH:
-                        case RPT_STATE:
-                        case RPT_NEW_MON:
-                        case RPT_SUA_CHG:
-                        case RPT_NN_INCMP:
-                        case RPT_ACTIVE_ERR:
-                                break;
-
-                        default:
-                                xframe = 0;
-                                break;
-                }
-        }
-
-        /* NOTE: UNKNOWN MAC frames will NOT be passed up unless
-         * ACCEPT_ATT_MAC_FRAMES is set.
-         */
-        if(((tp->receive_mask & ACCEPT_ATT_MAC_FRAMES) &&
-	    (xframe == (__u8)0)) ||
-	   ((tp->receive_mask & ACCEPT_EXT_MAC_FRAMES) &&
-	    (xframe == (__u8)1)))
-        {
-                rmf->vl = SWAP_BYTES(rmf->vl);
-
-                if (!(skb = dev_alloc_skb(size)))
-			return -ENOMEM;
-                skb->len = size;
-
-                /* Slide data into a sleek skb. */
-                skb_put(skb, skb->len);
-                skb_copy_to_linear_data(skb, rmf, skb->len);
-
-                /* Update Counters */
-                tp->MacStat.rx_packets++;
-                tp->MacStat.rx_bytes += skb->len;
-
-                /* Kick the packet on up. */
-                skb->protocol = tr_type_trans(skb, dev);
-                netif_rx(skb);
-                err = 0;
-        }
-
-        return err;
-}
-
-/* Adapter RAM test. Incremental word ODD boundary data test. */
-static int smctr_ram_memory_test(struct net_device *dev)
-{
-        struct net_local *tp = netdev_priv(dev);
-        __u16 page, pages_of_ram, start_pattern = 0, word_pattern = 0,
-                word_read = 0, err_word = 0, err_pattern = 0;
-        unsigned int err_offset;
-        __u32 j, pword;
-        __u8 err = 0;
-
-        if(smctr_debug > 10)
-                printk(KERN_DEBUG "%s: smctr_ram_memory_test\n", dev->name);
-
-        start_pattern   = 0x0001;
-        pages_of_ram    = tp->ram_size / tp->ram_usable;
-        pword           = tp->ram_access;
-
-        /* Incremental word ODD boundary test. */
-        for(page = 0; (page < pages_of_ram) && (~err);
-                page++, start_pattern += 0x8000)
-        {
-                smctr_set_page(dev, (__u8 *)(tp->ram_access
-                        + (page * tp->ram_usable * 1024) + 1));
-                word_pattern = start_pattern;
-
-                for(j = 1; j < (__u32)(tp->ram_usable * 1024) - 1; j += 2)
-                        *(__u16 *)(pword + j) = word_pattern++;
-
-                word_pattern = start_pattern;
-
-                for(j = 1; j < (__u32)(tp->ram_usable * 1024) - 1 && (~err);
-		    j += 2, word_pattern++)
-                {
-                        word_read = *(__u16 *)(pword + j);
-                        if(word_read != word_pattern)
-                        {
-                                err             = (__u8)1;
-                                err_offset      = j;
-                                err_word        = word_read;
-                                err_pattern     = word_pattern;
-                                return RAM_TEST_FAILED;
-                        }
-                }
-        }
-
-        /* Zero out memory. */
-        for(page = 0; page < pages_of_ram && (~err); page++)
-        {
-                smctr_set_page(dev, (__u8 *)(tp->ram_access
-                        + (page * tp->ram_usable * 1024)));
-                word_pattern = 0;
-
-                for(j = 0; j < (__u32)tp->ram_usable * 1024; j +=2)
-                        *(__u16 *)(pword + j) = word_pattern;
-
-                for(j =0; j < (__u32)tp->ram_usable * 1024 && (~err); j += 2)
-                {
-                        word_read = *(__u16 *)(pword + j);
-                        if(word_read != word_pattern)
-                        {
-                                err             = (__u8)1;
-                                err_offset      = j;
-                                err_word        = word_read;
-                                err_pattern     = word_pattern;
-                                return RAM_TEST_FAILED;
-                        }
-                }
-        }
-
-        smctr_set_page(dev, (__u8 *)tp->ram_access);
-
-        return 0;
-}
-
-static int smctr_rcv_chg_param(struct net_device *dev, MAC_HEADER *rmf,
-        __u16 *correlator)
-{
-        MAC_SUB_VECTOR *rsv;
-        signed short vlen;
-        __u16 rcode = POSITIVE_ACK;
-        unsigned int svectors = F_NO_SUB_VECTORS_FOUND;
-
-        /* This Frame can only come from a CRS */
-        if((rmf->dc_sc & SC_MASK) != SC_CRS)
-                return E_INAPPROPRIATE_SOURCE_CLASS;
-
-        /* Remove MVID Length from total length. */
-        vlen = (signed short)rmf->vl - 4;
-
-        /* Point to First SVID */
-        rsv = (MAC_SUB_VECTOR *)((__u32)rmf + sizeof(MAC_HEADER));
-
-        /* Search for Appropriate SVID's. */
-        while((vlen > 0) && (rcode == POSITIVE_ACK))
-        {
-                switch(rsv->svi)
-                {
-                        case CORRELATOR:
-                                svectors |= F_CORRELATOR;
-                                rcode = smctr_set_corr(dev, rsv, correlator);
-                                break;
-
-                        case LOCAL_RING_NUMBER:
-                                svectors |= F_LOCAL_RING_NUMBER;
-                                rcode = smctr_set_local_ring_num(dev, rsv);
-                                break;
-
-                        case ASSIGN_PHYSICAL_DROP:
-                                svectors |= F_ASSIGN_PHYSICAL_DROP;
-                                rcode = smctr_set_phy_drop(dev, rsv);
-                                break;
-
-                        case ERROR_TIMER_VALUE:
-                                svectors |= F_ERROR_TIMER_VALUE;
-                                rcode = smctr_set_error_timer_value(dev, rsv);
-                                break;
-
-                        case AUTHORIZED_FUNCTION_CLASS:
-                                svectors |= F_AUTHORIZED_FUNCTION_CLASS;
-                                rcode = smctr_set_auth_funct_class(dev, rsv);
-                                break;
-
-                        case AUTHORIZED_ACCESS_PRIORITY:
-                                svectors |= F_AUTHORIZED_ACCESS_PRIORITY;
-                                rcode = smctr_set_auth_access_pri(dev, rsv);
-                                break;
-
-                        default:
-                                rcode = E_SUB_VECTOR_UNKNOWN;
-                                break;
-                }
-
-                /* Let Sender Know if SUM of SV length's is
-                 * larger then length in MVID length field
-                 */
-                if((vlen -= rsv->svl) < 0)
-                        rcode = E_VECTOR_LENGTH_ERROR;
-
-                rsv = (MAC_SUB_VECTOR *)((__u32)rsv + rsv->svl);
-        }
-
-        if(rcode == POSITIVE_ACK)
-        {
-                /* Let Sender Know if MVID length field
-                 * is larger then SUM of SV length's
-                 */
-                if(vlen != 0)
-                        rcode = E_VECTOR_LENGTH_ERROR;
-                else
-		{
-                	/* Let Sender Know if Expected SVID Missing */
-                	if((svectors & R_CHG_PARM) ^ R_CHG_PARM)
-                        	rcode = E_MISSING_SUB_VECTOR;
-		}
-        }
-
-        return rcode;
-}
-
-static int smctr_rcv_init(struct net_device *dev, MAC_HEADER *rmf,
-        __u16 *correlator)
-{
-        MAC_SUB_VECTOR *rsv;
-        signed short vlen;
-        __u16 rcode = POSITIVE_ACK;
-        unsigned int svectors = F_NO_SUB_VECTORS_FOUND;
-
-        /* This Frame can only come from a RPS */
-        if((rmf->dc_sc & SC_MASK) != SC_RPS)
-                return E_INAPPROPRIATE_SOURCE_CLASS;
-
-        /* Remove MVID Length from total length. */
-        vlen = (signed short)rmf->vl - 4;
-
-        /* Point to First SVID */
-        rsv = (MAC_SUB_VECTOR *)((__u32)rmf + sizeof(MAC_HEADER));
-
-        /* Search for Appropriate SVID's */
-        while((vlen > 0) && (rcode == POSITIVE_ACK))
-        {
-                switch(rsv->svi)
-                {
-                        case CORRELATOR:
-                                svectors |= F_CORRELATOR;
-                                rcode = smctr_set_corr(dev, rsv, correlator);
-                                break;
-
-                        case LOCAL_RING_NUMBER:
-                                svectors |= F_LOCAL_RING_NUMBER;
-                                rcode = smctr_set_local_ring_num(dev, rsv);
-                                break;
-
-                        case ASSIGN_PHYSICAL_DROP:
-                                svectors |= F_ASSIGN_PHYSICAL_DROP;
-                                rcode = smctr_set_phy_drop(dev, rsv);
-                                break;
-
-                        case ERROR_TIMER_VALUE:
-                                svectors |= F_ERROR_TIMER_VALUE;
-                                rcode = smctr_set_error_timer_value(dev, rsv);
-                                break;
-
-                        default:
-                                rcode = E_SUB_VECTOR_UNKNOWN;
-                                break;
-                }
-
-                /* Let Sender Know if SUM of SV length's is
-                 * larger then length in MVID length field
-		 */
-                if((vlen -= rsv->svl) < 0)
-                        rcode = E_VECTOR_LENGTH_ERROR;
-
-                rsv = (MAC_SUB_VECTOR *)((__u32)rsv + rsv->svl);
-        }
-
-        if(rcode == POSITIVE_ACK)
-        {
-                /* Let Sender Know if MVID length field
-                 * is larger then SUM of SV length's
-                 */
-                if(vlen != 0)
-                        rcode = E_VECTOR_LENGTH_ERROR;
-                else
-		{
-                	/* Let Sender Know if Expected SV Missing */
-                	if((svectors & R_INIT) ^ R_INIT)
-                        	rcode = E_MISSING_SUB_VECTOR;
-		}
-        }
-
-        return rcode;
-}
-
-static int smctr_rcv_tx_forward(struct net_device *dev, MAC_HEADER *rmf)
-{
-        MAC_SUB_VECTOR *rsv;
-        signed short vlen;
-        __u16 rcode = POSITIVE_ACK;
-        unsigned int svectors = F_NO_SUB_VECTORS_FOUND;
-
-        /* This Frame can only come from a CRS */
-        if((rmf->dc_sc & SC_MASK) != SC_CRS)
-                return E_INAPPROPRIATE_SOURCE_CLASS;
-
-        /* Remove MVID Length from total length */
-        vlen = (signed short)rmf->vl - 4;
-
-        /* Point to First SVID */
-        rsv = (MAC_SUB_VECTOR *)((__u32)rmf + sizeof(MAC_HEADER));
-
-        /* Search for Appropriate SVID's */
-        while((vlen > 0) && (rcode == POSITIVE_ACK))
-        {
-                switch(rsv->svi)
-                {
-                        case FRAME_FORWARD:
-                                svectors |= F_FRAME_FORWARD;
-                                rcode = smctr_set_frame_forward(dev, rsv, 
-					rmf->dc_sc);
-                                break;
-
-                        default:
-                                rcode = E_SUB_VECTOR_UNKNOWN;
-                                break;
-                }
-
-                /* Let Sender Know if SUM of SV length's is
-                 * larger then length in MVID length field
-		 */
-                if((vlen -= rsv->svl) < 0)
-                        rcode = E_VECTOR_LENGTH_ERROR;
-
-                rsv = (MAC_SUB_VECTOR *)((__u32)rsv + rsv->svl);
-        }
-
-        if(rcode == POSITIVE_ACK)
-        {
-                /* Let Sender Know if MVID length field
-                 * is larger then SUM of SV length's
-                 */
-                if(vlen != 0)
-                        rcode = E_VECTOR_LENGTH_ERROR;
-                else
-		{
-                	/* Let Sender Know if Expected SV Missing */
-                	if((svectors & R_TX_FORWARD) ^ R_TX_FORWARD)
-                        	rcode = E_MISSING_SUB_VECTOR;
-		}
-        }
-
-        return rcode;
-}
-
-static int smctr_rcv_rq_addr_state_attch(struct net_device *dev,
-        MAC_HEADER *rmf, __u16 *correlator)
-{
-        MAC_SUB_VECTOR *rsv;
-        signed short vlen;
-        __u16 rcode = POSITIVE_ACK;
-        unsigned int svectors = F_NO_SUB_VECTORS_FOUND;
-
-        /* Remove MVID Length from total length */
-        vlen = (signed short)rmf->vl - 4;
-
-        /* Point to First SVID */
-        rsv = (MAC_SUB_VECTOR *)((__u32)rmf + sizeof(MAC_HEADER));
-
-        /* Search for Appropriate SVID's */
-        while((vlen > 0) && (rcode == POSITIVE_ACK))
-        {
-                switch(rsv->svi)
-                {
-                        case CORRELATOR:
-                                svectors |= F_CORRELATOR;
-                                rcode = smctr_set_corr(dev, rsv, correlator);
-                                break;
-
-                        default:
-                                rcode = E_SUB_VECTOR_UNKNOWN;
-                                break;
-                }
-
-                /* Let Sender Know if SUM of SV length's is
-                 * larger then length in MVID length field
-                 */
-                if((vlen -= rsv->svl) < 0)
-                        rcode = E_VECTOR_LENGTH_ERROR;
-
-                rsv = (MAC_SUB_VECTOR *)((__u32)rsv + rsv->svl);
-        }
-
-        if(rcode == POSITIVE_ACK)
-        {
-                /* Let Sender Know if MVID length field
-                 * is larger then SUM of SV length's
-                 */
-                if(vlen != 0)
-                        rcode = E_VECTOR_LENGTH_ERROR;
-                else
-		{
-                	/* Let Sender Know if Expected SVID Missing */
-                	if((svectors & R_RQ_ATTCH_STATE_ADDR) 
-				^ R_RQ_ATTCH_STATE_ADDR)
-                        	rcode = E_MISSING_SUB_VECTOR;
-			}
-        }
-
-        return rcode;
-}
-
-static int smctr_rcv_unknown(struct net_device *dev, MAC_HEADER *rmf,
-        __u16 *correlator)
-{
-        MAC_SUB_VECTOR *rsv;
-        signed short vlen;
-
-        *correlator = 0;
-
-        /* Remove MVID Length from total length */
-        vlen = (signed short)rmf->vl - 4;
-
-        /* Point to First SVID */
-        rsv = (MAC_SUB_VECTOR *)((__u32)rmf + sizeof(MAC_HEADER));
-
-        /* Search for CORRELATOR for RSP to UNKNOWN */
-        while((vlen > 0) && (*correlator == 0))
-        {
-                switch(rsv->svi)
-                {
-                        case CORRELATOR:
-                                smctr_set_corr(dev, rsv, correlator);
-                                break;
-
-                        default:
-                                break;
-                }
-
-                vlen -= rsv->svl;
-                rsv = (MAC_SUB_VECTOR *)((__u32)rsv + rsv->svl);
-        }
-
-        return E_UNRECOGNIZED_VECTOR_ID;
-}
-
-/*
- * Reset the 825 NIC and exit w:
- * 1. The NIC reset cleared (non-reset state), halted and un-initialized.
- * 2. TINT masked.
- * 3. CBUSY masked.
- * 4. TINT clear.
- * 5. CBUSY clear.
- */
-static int smctr_reset_adapter(struct net_device *dev)
-{
-        struct net_local *tp = netdev_priv(dev);
-        int ioaddr = dev->base_addr;
-
-        /* Reseting the NIC will put it in a halted and un-initialized state. */        smctr_set_trc_reset(ioaddr);
-        mdelay(200); /* ~2 ms */
-
-        smctr_clear_trc_reset(ioaddr);
-        mdelay(200); /* ~2 ms */
-
-        /* Remove any latched interrupts that occurred prior to reseting the
-         * adapter or possibily caused by line glitches due to the reset.
-         */
-        outb(tp->trc_mask | CSR_CLRTINT | CSR_CLRCBUSY, ioaddr + CSR);
-
-        return 0;
-}
-
-static int smctr_restart_tx_chain(struct net_device *dev, short queue)
-{
-        struct net_local *tp = netdev_priv(dev);
-        int err = 0;
-
-        if(smctr_debug > 10)
-                printk(KERN_DEBUG "%s: smctr_restart_tx_chain\n", dev->name);
-
-        if(tp->num_tx_fcbs_used[queue] != 0 &&
-	   tp->tx_queue_status[queue] == NOT_TRANSMITING)
-        {
-                tp->tx_queue_status[queue] = TRANSMITING;
-                err = smctr_issue_resume_tx_fcb_cmd(dev, queue);
-        }
-
-        return err;
-}
-
-static int smctr_ring_status_chg(struct net_device *dev)
-{
-        struct net_local *tp = netdev_priv(dev);
-
-        if(smctr_debug > 10)
-                printk(KERN_DEBUG "%s: smctr_ring_status_chg\n", dev->name);
-
-        /* Check for ring_status_flag: whenever MONITOR_STATE_BIT
-         * Bit is set, check value of monitor_state, only then we
-         * enable and start transmit/receive timeout (if and only
-         * if it is MS_ACTIVE_MONITOR_STATE or MS_STANDBY_MONITOR_STATE)
-         */
-        if(tp->ring_status_flags == MONITOR_STATE_CHANGED)
-        {
-                if((tp->monitor_state == MS_ACTIVE_MONITOR_STATE) ||
-		   (tp->monitor_state == MS_STANDBY_MONITOR_STATE))
-                {
-                        tp->monitor_state_ready = 1;
-                }
-                else
-                {
-                        /* if adapter is NOT in either active monitor
-                         * or standby monitor state => Disable
-                         * transmit/receive timeout.
-                         */
-                        tp->monitor_state_ready = 0;
-
-			/* Ring speed problem, switching to auto mode. */
-			if(tp->monitor_state == MS_MONITOR_FSM_INACTIVE &&
-			   !tp->cleanup)
-			{
-				printk(KERN_INFO "%s: Incorrect ring speed switching.\n",
-					dev->name);
-				smctr_set_ring_speed(dev);
-			}
-                }
-        }
-
-        if(!(tp->ring_status_flags & RING_STATUS_CHANGED))
-                return 0;
-
-        switch(tp->ring_status)
-        {
-                case RING_RECOVERY:
-                        printk(KERN_INFO "%s: Ring Recovery\n", dev->name);
-                        break;
-
-                case SINGLE_STATION:
-                        printk(KERN_INFO "%s: Single Statinon\n", dev->name);
-                        break;
-
-                case COUNTER_OVERFLOW:
-                        printk(KERN_INFO "%s: Counter Overflow\n", dev->name);
-                        break;
-
-                case REMOVE_RECEIVED:
-                        printk(KERN_INFO "%s: Remove Received\n", dev->name);
-                        break;
-
-                case AUTO_REMOVAL_ERROR:
-                        printk(KERN_INFO "%s: Auto Remove Error\n", dev->name);
-                        break;
-
-                case LOBE_WIRE_FAULT:
-                        printk(KERN_INFO "%s: Lobe Wire Fault\n", dev->name);
-                        break;
-
-                case TRANSMIT_BEACON:
-                        printk(KERN_INFO "%s: Transmit Beacon\n", dev->name);
-                        break;
-
-                case SOFT_ERROR:
-                        printk(KERN_INFO "%s: Soft Error\n", dev->name);
-                        break;
-
-                case HARD_ERROR:
-                        printk(KERN_INFO "%s: Hard Error\n", dev->name);
-                        break;
-
-                case SIGNAL_LOSS:
-                        printk(KERN_INFO "%s: Signal Loss\n", dev->name);
-                        break;
-
-                default:
-			printk(KERN_INFO "%s: Unknown ring status change\n",
-				dev->name);
-                        break;
-        }
-
-        return 0;
-}
-
-static int smctr_rx_frame(struct net_device *dev)
-{
-        struct net_local *tp = netdev_priv(dev);
-        __u16 queue, status, rx_size, err = 0;
-        __u8 *pbuff;
-
-        if(smctr_debug > 10)
-                printk(KERN_DEBUG "%s: smctr_rx_frame\n", dev->name);
-
-        queue = tp->receive_queue_number;
-
-        while((status = tp->rx_fcb_curr[queue]->frame_status) != SUCCESS)
-        {
-                err = HARDWARE_FAILED;
-
-                if(((status & 0x007f) == 0) ||
-		   ((tp->receive_mask & ACCEPT_ERR_PACKETS) != 0))
-                {
-                        /* frame length less the CRC (4 bytes) + FS (1 byte) */
-                        rx_size = tp->rx_fcb_curr[queue]->frame_length - 5;
-
-                        pbuff = smctr_get_rx_pointer(dev, queue);
-
-                        smctr_set_page(dev, pbuff);
-                        smctr_disable_16bit(dev);
-
-                        /* pbuff points to addr within one page */
-                        pbuff = (__u8 *)PAGE_POINTER(pbuff);
-
-                        if(queue == NON_MAC_QUEUE)
-                        {
-                                struct sk_buff *skb;
-
-                                skb = dev_alloc_skb(rx_size);
-				if (skb) {
-                                	skb_put(skb, rx_size);
-
-					skb_copy_to_linear_data(skb, pbuff, rx_size);
-
-                                	/* Update Counters */
-                                	tp->MacStat.rx_packets++;
-                                	tp->MacStat.rx_bytes += skb->len;
-
-                                	/* Kick the packet on up. */
-                                	skb->protocol = tr_type_trans(skb, dev);
-                                	netif_rx(skb);
-				} else {
-				}
-                        }
-                        else
-                                smctr_process_rx_packet((MAC_HEADER *)pbuff,
-                                        rx_size, dev, status);
-                }
-
-                smctr_enable_16bit(dev);
-                smctr_set_page(dev, (__u8 *)tp->ram_access);
-                smctr_update_rx_chain(dev, queue);
-
-                if(err != SUCCESS)
-                        break;
-        }
-
-        return err;
-}
-
-static int smctr_send_dat(struct net_device *dev)
-{
-        struct net_local *tp = netdev_priv(dev);
-        unsigned int i, err;
-        MAC_HEADER *tmf;
-        FCBlock *fcb;
-
-        if(smctr_debug > 10)
-                printk(KERN_DEBUG "%s: smctr_send_dat\n", dev->name);
-
-        if((fcb = smctr_get_tx_fcb(dev, MAC_QUEUE,
-                sizeof(MAC_HEADER))) == (FCBlock *)(-1L))
-        {
-                return OUT_OF_RESOURCES;
-        }
-
-        /* Initialize DAT Data Fields. */
-        tmf = (MAC_HEADER *)fcb->bdb_ptr->data_block_ptr;
-        tmf->ac = MSB(AC_FC_DAT);
-        tmf->fc = LSB(AC_FC_DAT);
-
-        for(i = 0; i < 6; i++)
-        {
-                tmf->sa[i] = dev->dev_addr[i];
-                tmf->da[i] = dev->dev_addr[i];
-
-        }
-
-        tmf->vc        = DAT;
-        tmf->dc_sc     = DC_RS | SC_RS;
-        tmf->vl        = 4;
-        tmf->vl        = SWAP_BYTES(tmf->vl);
-
-        /* Start Transmit. */
-        if((err = smctr_trc_send_packet(dev, fcb, MAC_QUEUE)))
-                return err;
-
-        /* Wait for Transmit to Complete */
-        for(i = 0; i < 10000; i++)
-        {
-                if(fcb->frame_status & FCB_COMMAND_DONE)
-                        break;
-                mdelay(1);
-        }
-
-        /* Check if GOOD frame Tx'ed. */
-        if(!(fcb->frame_status &  FCB_COMMAND_DONE) ||
-	   fcb->frame_status & (FCB_TX_STATUS_E | FCB_TX_AC_BITS))
-        {
-                return INITIALIZE_FAILED;
-        }
-
-        /* De-allocated Tx FCB and Frame Buffer
-         * The FCB must be de-allocated manually if executing with
-         * interrupts disabled, other wise the ISR (LM_Service_Events)
-         * will de-allocate it when the interrupt occurs.
-         */
-        tp->tx_queue_status[MAC_QUEUE] = NOT_TRANSMITING;
-        smctr_update_tx_chain(dev, fcb, MAC_QUEUE);
-
-        return 0;
-}
-
-static void smctr_timeout(struct net_device *dev)
-{
-	/*
-         * If we get here, some higher level has decided we are broken.
-         * There should really be a "kick me" function call instead.
-         *
-         * Resetting the token ring adapter takes a long time so just
-         * fake transmission time and go on trying. Our own timeout
-         * routine is in sktr_timer_chk()
-         */
-        dev->trans_start = jiffies; /* prevent tx timeout */
-        netif_wake_queue(dev);
-}
-
-/*
- * Gets skb from system, queues it and checks if it can be sent
- */
-static netdev_tx_t smctr_send_packet(struct sk_buff *skb,
-					   struct net_device *dev)
-{
-        struct net_local *tp = netdev_priv(dev);
-
-        if(smctr_debug > 10)
-                printk(KERN_DEBUG "%s: smctr_send_packet\n", dev->name);
-
-        /*
-         * Block a transmit overlap
-         */
-         
-        netif_stop_queue(dev);
-
-        if(tp->QueueSkb == 0)
-                return NETDEV_TX_BUSY;     /* Return with tbusy set: queue full */
-
-        tp->QueueSkb--;
-        skb_queue_tail(&tp->SendSkbQueue, skb);
-        smctr_hardware_send_packet(dev, tp);
-        if(tp->QueueSkb > 0)
-		netif_wake_queue(dev);
-		
-        return NETDEV_TX_OK;
-}
-
-static int smctr_send_lobe_media_test(struct net_device *dev)
-{
-        struct net_local *tp = netdev_priv(dev);
-	MAC_SUB_VECTOR *tsv;
-	MAC_HEADER *tmf;
-        FCBlock *fcb;
-	__u32 i;
-	int err;
-
-        if(smctr_debug > 15)
-                printk(KERN_DEBUG "%s: smctr_send_lobe_media_test\n", dev->name);
-
-        if((fcb = smctr_get_tx_fcb(dev, MAC_QUEUE, sizeof(struct trh_hdr)
-                + S_WRAP_DATA + S_WRAP_DATA)) == (FCBlock *)(-1L))
-        {
-                return OUT_OF_RESOURCES;
-        }
-
-        /* Initialize DAT Data Fields. */
-        tmf = (MAC_HEADER *)fcb->bdb_ptr->data_block_ptr;
-        tmf->ac = MSB(AC_FC_LOBE_MEDIA_TEST);
-        tmf->fc = LSB(AC_FC_LOBE_MEDIA_TEST);
-
-        for(i = 0; i < 6; i++)
-        {
-                tmf->da[i] = 0;
-                tmf->sa[i] = dev->dev_addr[i];
-        }
-
-        tmf->vc        = LOBE_MEDIA_TEST;
-        tmf->dc_sc     = DC_RS | SC_RS;
-        tmf->vl        = 4;
-
-        tsv = (MAC_SUB_VECTOR *)((__u32)tmf + sizeof(MAC_HEADER));
-        smctr_make_wrap_data(dev, tsv);
-        tmf->vl += tsv->svl;
-
-        tsv = (MAC_SUB_VECTOR *)((__u32)tsv + tsv->svl);
-        smctr_make_wrap_data(dev, tsv);
-        tmf->vl += tsv->svl;
-
-        /* Start Transmit. */
-        tmf->vl = SWAP_BYTES(tmf->vl);
-        if((err = smctr_trc_send_packet(dev, fcb, MAC_QUEUE)))
-                return err;
-
-        /* Wait for Transmit to Complete. (10 ms). */
-        for(i=0; i < 10000; i++)
-        {
-                if(fcb->frame_status & FCB_COMMAND_DONE)
-                        break;
-                mdelay(1);
-        }
-
-        /* Check if GOOD frame Tx'ed */
-        if(!(fcb->frame_status & FCB_COMMAND_DONE) ||
-	   fcb->frame_status & (FCB_TX_STATUS_E | FCB_TX_AC_BITS))
-        {
-                return LOBE_MEDIA_TEST_FAILED;
-        }
-
-        /* De-allocated Tx FCB and Frame Buffer
-         * The FCB must be de-allocated manually if executing with
-         * interrupts disabled, other wise the ISR (LM_Service_Events)
-         * will de-allocate it when the interrupt occurs.
-         */
-        tp->tx_queue_status[MAC_QUEUE] = NOT_TRANSMITING;
-        smctr_update_tx_chain(dev, fcb, MAC_QUEUE);
-
-        return 0;
-}
-
-static int smctr_send_rpt_addr(struct net_device *dev, MAC_HEADER *rmf,
-        __u16 correlator)
-{
-        MAC_HEADER *tmf;
-        MAC_SUB_VECTOR *tsv;
-        FCBlock *fcb;
-
-        if((fcb = smctr_get_tx_fcb(dev, MAC_QUEUE, sizeof(MAC_HEADER)
-		+ S_CORRELATOR + S_PHYSICAL_DROP + S_UPSTREAM_NEIGHBOR_ADDRESS
-		+ S_ADDRESS_MODIFER + S_GROUP_ADDRESS + S_FUNCTIONAL_ADDRESS))
-		== (FCBlock *)(-1L))
-        {
-                return 0;
-        }
-
-        tmf 		= (MAC_HEADER *)fcb->bdb_ptr->data_block_ptr;
-        tmf->vc    	= RPT_ADDR;
-        tmf->dc_sc 	= (rmf->dc_sc & SC_MASK) << 4;
-        tmf->vl    	= 4;
-
-        smctr_make_8025_hdr(dev, rmf, tmf, AC_FC_RPT_ADDR);
-
-        tsv = (MAC_SUB_VECTOR *)((__u32)tmf + sizeof(MAC_HEADER));
-        smctr_make_corr(dev, tsv, correlator);
-
-        tmf->vl += tsv->svl;
-        tsv = (MAC_SUB_VECTOR *)((__u32)tsv + tsv->svl);
-        smctr_make_phy_drop_num(dev, tsv);
-
-        tmf->vl += tsv->svl;
-        tsv = (MAC_SUB_VECTOR *)((__u32)tsv + tsv->svl);
-        smctr_make_upstream_neighbor_addr(dev, tsv);
-
-        tmf->vl += tsv->svl;
-        tsv = (MAC_SUB_VECTOR *)((__u32)tsv + tsv->svl);
-        smctr_make_addr_mod(dev, tsv);
-
-        tmf->vl += tsv->svl;
-        tsv = (MAC_SUB_VECTOR *)((__u32)tsv + tsv->svl);
-        smctr_make_group_addr(dev, tsv);
-
-        tmf->vl += tsv->svl;
-        tsv = (MAC_SUB_VECTOR *)((__u32)tsv + tsv->svl);
-        smctr_make_funct_addr(dev, tsv);
-
-        tmf->vl += tsv->svl;
-
-        /* Subtract out MVID and MVL which is
-         * include in both vl and MAC_HEADER
-         */
-/*      fcb->frame_length           = tmf->vl + sizeof(MAC_HEADER) - 4;
-        fcb->bdb_ptr->buffer_length = tmf->vl + sizeof(MAC_HEADER) - 4;
-*/
-        tmf->vl = SWAP_BYTES(tmf->vl);
-
-        return smctr_trc_send_packet(dev, fcb, MAC_QUEUE);
-}
-
-static int smctr_send_rpt_attch(struct net_device *dev, MAC_HEADER *rmf,
-        __u16 correlator)
-{
-        MAC_HEADER *tmf;
-        MAC_SUB_VECTOR *tsv;
-        FCBlock *fcb;
-
-        if((fcb = smctr_get_tx_fcb(dev, MAC_QUEUE, sizeof(MAC_HEADER)
-		+ S_CORRELATOR + S_PRODUCT_INSTANCE_ID + S_FUNCTIONAL_ADDRESS
-		+ S_AUTHORIZED_FUNCTION_CLASS + S_AUTHORIZED_ACCESS_PRIORITY))
-		== (FCBlock *)(-1L))
-        {
-                return 0;
-        }
-
-        tmf 	   = (MAC_HEADER *)fcb->bdb_ptr->data_block_ptr;
-        tmf->vc    = RPT_ATTCH;
-        tmf->dc_sc = (rmf->dc_sc & SC_MASK) << 4;
-        tmf->vl    = 4;
-
-        smctr_make_8025_hdr(dev, rmf, tmf, AC_FC_RPT_ATTCH);
-
-        tsv = (MAC_SUB_VECTOR *)((__u32)tmf + sizeof(MAC_HEADER));
-        smctr_make_corr(dev, tsv, correlator);
-
-        tmf->vl += tsv->svl;
-        tsv = (MAC_SUB_VECTOR *)((__u32)tsv + tsv->svl);
-        smctr_make_product_id(dev, tsv);
-
-        tmf->vl += tsv->svl;
-        tsv = (MAC_SUB_VECTOR *)((__u32)tsv + tsv->svl);
-        smctr_make_funct_addr(dev, tsv);
-
-        tmf->vl += tsv->svl;
-        tsv = (MAC_SUB_VECTOR *)((__u32)tsv + tsv->svl);
-        smctr_make_auth_funct_class(dev, tsv);
-
-        tmf->vl += tsv->svl;
-        tsv = (MAC_SUB_VECTOR *)((__u32)tsv + tsv->svl);
-        smctr_make_access_pri(dev, tsv);
-
-        tmf->vl += tsv->svl;
-
-        /* Subtract out MVID and MVL which is
-         * include in both vl and MAC_HEADER
-         */
-/*      fcb->frame_length           = tmf->vl + sizeof(MAC_HEADER) - 4;
-        fcb->bdb_ptr->buffer_length = tmf->vl + sizeof(MAC_HEADER) - 4;
-*/
-        tmf->vl = SWAP_BYTES(tmf->vl);
-
-        return smctr_trc_send_packet(dev, fcb, MAC_QUEUE);
-}
-
-static int smctr_send_rpt_state(struct net_device *dev, MAC_HEADER *rmf,
-        __u16 correlator)
-{
-        MAC_HEADER *tmf;
-        MAC_SUB_VECTOR *tsv;
-        FCBlock *fcb;
-
-        if((fcb = smctr_get_tx_fcb(dev, MAC_QUEUE, sizeof(MAC_HEADER)
-		+ S_CORRELATOR + S_RING_STATION_VERSION_NUMBER
-		+ S_RING_STATION_STATUS + S_STATION_IDENTIFER))
-		== (FCBlock *)(-1L))
-        {
-                return 0;
-        }
-
-        tmf 	   = (MAC_HEADER *)fcb->bdb_ptr->data_block_ptr;
-        tmf->vc    = RPT_STATE;
-        tmf->dc_sc = (rmf->dc_sc & SC_MASK) << 4;
-        tmf->vl    = 4;
-
-        smctr_make_8025_hdr(dev, rmf, tmf, AC_FC_RPT_STATE);
-
-        tsv = (MAC_SUB_VECTOR *)((__u32)tmf + sizeof(MAC_HEADER));
-        smctr_make_corr(dev, tsv, correlator);
-
-        tmf->vl += tsv->svl;
-        tsv = (MAC_SUB_VECTOR *)((__u32)tsv + tsv->svl);
-        smctr_make_ring_station_version(dev, tsv);
-
-        tmf->vl += tsv->svl;
-        tsv = (MAC_SUB_VECTOR *)((__u32)tsv + tsv->svl);
-        smctr_make_ring_station_status(dev, tsv);
-
-        tmf->vl += tsv->svl;
-        tsv = (MAC_SUB_VECTOR *)((__u32)tsv + tsv->svl);
-        smctr_make_station_id(dev, tsv);
-
-        tmf->vl += tsv->svl;
-
-        /* Subtract out MVID and MVL which is
-         * include in both vl and MAC_HEADER
-         */
-/*      fcb->frame_length           = tmf->vl + sizeof(MAC_HEADER) - 4;
-        fcb->bdb_ptr->buffer_length = tmf->vl + sizeof(MAC_HEADER) - 4;
-*/
-        tmf->vl = SWAP_BYTES(tmf->vl);
-
-        return smctr_trc_send_packet(dev, fcb, MAC_QUEUE);
-}
-
-static int smctr_send_rpt_tx_forward(struct net_device *dev,
-        MAC_HEADER *rmf, __u16 tx_fstatus)
-{
-        MAC_HEADER *tmf;
-        MAC_SUB_VECTOR *tsv;
-        FCBlock *fcb;
-
-        if((fcb = smctr_get_tx_fcb(dev, MAC_QUEUE, sizeof(MAC_HEADER)
-		+ S_TRANSMIT_STATUS_CODE)) == (FCBlock *)(-1L))
-        {
-                return 0;
-        }
-
-        tmf 	   = (MAC_HEADER *)fcb->bdb_ptr->data_block_ptr;
-        tmf->vc    = RPT_TX_FORWARD;
-        tmf->dc_sc = (rmf->dc_sc & SC_MASK) << 4;
-        tmf->vl    = 4;
-
-        smctr_make_8025_hdr(dev, rmf, tmf, AC_FC_RPT_TX_FORWARD);
-
-        tsv = (MAC_SUB_VECTOR *)((__u32)tmf + sizeof(MAC_HEADER));
-        smctr_make_tx_status_code(dev, tsv, tx_fstatus);
-
-        tmf->vl += tsv->svl;
-
-        /* Subtract out MVID and MVL which is
-         * include in both vl and MAC_HEADER
-         */
-/*      fcb->frame_length           = tmf->vl + sizeof(MAC_HEADER) - 4;
-        fcb->bdb_ptr->buffer_length = tmf->vl + sizeof(MAC_HEADER) - 4;
-*/
-        tmf->vl = SWAP_BYTES(tmf->vl);
-
-        return smctr_trc_send_packet(dev, fcb, MAC_QUEUE);
-}
-
-static int smctr_send_rsp(struct net_device *dev, MAC_HEADER *rmf,
-        __u16 rcode, __u16 correlator)
-{
-        MAC_HEADER *tmf;
-        MAC_SUB_VECTOR *tsv;
-        FCBlock *fcb;
-
-        if((fcb = smctr_get_tx_fcb(dev, MAC_QUEUE, sizeof(MAC_HEADER)
-		+ S_CORRELATOR + S_RESPONSE_CODE)) == (FCBlock *)(-1L))
-        {
-                return 0;
-        }
-
-        tmf 	   = (MAC_HEADER *)fcb->bdb_ptr->data_block_ptr;
-        tmf->vc    = RSP;
-        tmf->dc_sc = (rmf->dc_sc & SC_MASK) << 4;
-        tmf->vl    = 4;
-
-        smctr_make_8025_hdr(dev, rmf, tmf, AC_FC_RSP);
-
-        tsv = (MAC_SUB_VECTOR *)((__u32)tmf + sizeof(MAC_HEADER));
-        smctr_make_corr(dev, tsv, correlator);
-
-        return 0;
-}
-
-static int smctr_send_rq_init(struct net_device *dev)
-{
-        struct net_local *tp = netdev_priv(dev);
-        MAC_HEADER *tmf;
-        MAC_SUB_VECTOR *tsv;
-        FCBlock *fcb;
-	unsigned int i, count = 0;
-	__u16 fstatus;
-	int err;
-
-        do {
-        	if(((fcb = smctr_get_tx_fcb(dev, MAC_QUEUE, sizeof(MAC_HEADER)
-			+ S_PRODUCT_INSTANCE_ID + S_UPSTREAM_NEIGHBOR_ADDRESS
-			+ S_RING_STATION_VERSION_NUMBER + S_ADDRESS_MODIFER))
-			== (FCBlock *)(-1L)))
-                {
-                        return 0;
-                }
-
-                tmf 	   = (MAC_HEADER *)fcb->bdb_ptr->data_block_ptr;
-                tmf->vc    = RQ_INIT;
-                tmf->dc_sc = DC_RPS | SC_RS;
-                tmf->vl    = 4;
-
-                smctr_make_8025_hdr(dev, NULL, tmf, AC_FC_RQ_INIT);
-
-                tsv = (MAC_SUB_VECTOR *)((__u32)tmf + sizeof(MAC_HEADER));
-                smctr_make_product_id(dev, tsv);
-
-                tmf->vl += tsv->svl;
-                tsv = (MAC_SUB_VECTOR *)((__u32)tsv + tsv->svl);
-                smctr_make_upstream_neighbor_addr(dev, tsv);
-
-                tmf->vl += tsv->svl;
-                tsv = (MAC_SUB_VECTOR *)((__u32)tsv + tsv->svl);
-                smctr_make_ring_station_version(dev, tsv);
-
-                tmf->vl += tsv->svl;
-                tsv = (MAC_SUB_VECTOR *)((__u32)tsv + tsv->svl);
-                smctr_make_addr_mod(dev, tsv);
-
-                tmf->vl += tsv->svl;
-
-                /* Subtract out MVID and MVL which is
-                 * include in both vl and MAC_HEADER
-                 */
-/*              fcb->frame_length           = tmf->vl + sizeof(MAC_HEADER) - 4;
-                fcb->bdb_ptr->buffer_length = tmf->vl + sizeof(MAC_HEADER) - 4;
-*/
-                tmf->vl = SWAP_BYTES(tmf->vl);
-
-                if((err = smctr_trc_send_packet(dev, fcb, MAC_QUEUE)))
-                        return err;
-
-                /* Wait for Transmit to Complete */
-      		for(i = 0; i < 10000; i++) 
-		{
-          		if(fcb->frame_status & FCB_COMMAND_DONE)
-              			break;
-          		mdelay(1);
-      		}
-
-                /* Check if GOOD frame Tx'ed */
-                fstatus = fcb->frame_status;
-
-                if(!(fstatus & FCB_COMMAND_DONE))
-                        return HARDWARE_FAILED;
-
-                if(!(fstatus & FCB_TX_STATUS_E))
-                        count++;
-
-                /* De-allocated Tx FCB and Frame Buffer
-                 * The FCB must be de-allocated manually if executing with
-                 * interrupts disabled, other wise the ISR (LM_Service_Events)
-                 * will de-allocate it when the interrupt occurs.
-                 */
-                tp->tx_queue_status[MAC_QUEUE] = NOT_TRANSMITING;
-                smctr_update_tx_chain(dev, fcb, MAC_QUEUE);
-        } while(count < 4 && ((fstatus & FCB_TX_AC_BITS) ^ FCB_TX_AC_BITS));
-
-	return smctr_join_complete_state(dev);
-}
-
-static int smctr_send_tx_forward(struct net_device *dev, MAC_HEADER *rmf,
-        __u16 *tx_fstatus)
-{
-        struct net_local *tp = netdev_priv(dev);
-        FCBlock *fcb;
-        unsigned int i;
-	int err;
-
-        /* Check if this is the END POINT of the Transmit Forward Chain. */
-        if(rmf->vl <= 18)
-                return 0;
-
-        /* Allocate Transmit FCB only by requesting 0 bytes
-         * of data buffer.
-         */
-        if((fcb = smctr_get_tx_fcb(dev, MAC_QUEUE, 0)) == (FCBlock *)(-1L))
-                return 0;
-
-        /* Set pointer to Transmit Frame Buffer to the data
-         * portion of the received TX Forward frame, making
-         * sure to skip over the Vector Code (vc) and Vector
-         * length (vl).
-         */
-        fcb->bdb_ptr->trc_data_block_ptr = TRC_POINTER((__u32)rmf 
-		+ sizeof(MAC_HEADER) + 2);
-        fcb->bdb_ptr->data_block_ptr     = (__u16 *)((__u32)rmf 
-		+ sizeof(MAC_HEADER) + 2);
-
-        fcb->frame_length                = rmf->vl - 4 - 2;
-        fcb->bdb_ptr->buffer_length      = rmf->vl - 4 - 2;
-
-        if((err = smctr_trc_send_packet(dev, fcb, MAC_QUEUE)))
-                return err;
-
-        /* Wait for Transmit to Complete */
-   	for(i = 0; i < 10000; i++) 
-	{
-       		if(fcb->frame_status & FCB_COMMAND_DONE)
-           		break;
-        	mdelay(1);
-   	}
-
-        /* Check if GOOD frame Tx'ed */
-        if(!(fcb->frame_status & FCB_COMMAND_DONE))
-        {
-                if((err = smctr_issue_resume_tx_fcb_cmd(dev, MAC_QUEUE)))
-                        return err;
-
-      		for(i = 0; i < 10000; i++) 
-		{
-          		if(fcb->frame_status & FCB_COMMAND_DONE)
-              			break;
-        		mdelay(1);
-      		}
-
-                if(!(fcb->frame_status & FCB_COMMAND_DONE))
-                        return HARDWARE_FAILED;
-        }
-
-        *tx_fstatus = fcb->frame_status;
-
-        return A_FRAME_WAS_FORWARDED;
-}
-
-static int smctr_set_auth_access_pri(struct net_device *dev,
-        MAC_SUB_VECTOR *rsv)
-{
-        struct net_local *tp = netdev_priv(dev);
-
-        if(rsv->svl != S_AUTHORIZED_ACCESS_PRIORITY)
-                return E_SUB_VECTOR_LENGTH_ERROR;
-
-        tp->authorized_access_priority = (rsv->svv[0] << 8 | rsv->svv[1]);
-
-        return POSITIVE_ACK;
-}
-
-static int smctr_set_auth_funct_class(struct net_device *dev,
-        MAC_SUB_VECTOR *rsv)
-{
-        struct net_local *tp = netdev_priv(dev);
-
-        if(rsv->svl != S_AUTHORIZED_FUNCTION_CLASS)
-                return E_SUB_VECTOR_LENGTH_ERROR;
-
-        tp->authorized_function_classes = (rsv->svv[0] << 8 | rsv->svv[1]);
-
-        return POSITIVE_ACK;
-}
-
-static int smctr_set_corr(struct net_device *dev, MAC_SUB_VECTOR *rsv,
-        __u16 *correlator)
-{
-        if(rsv->svl != S_CORRELATOR)
-                return E_SUB_VECTOR_LENGTH_ERROR;
-
-        *correlator = (rsv->svv[0] << 8 | rsv->svv[1]);
-
-        return POSITIVE_ACK;
-}
-
-static int smctr_set_error_timer_value(struct net_device *dev,
-        MAC_SUB_VECTOR *rsv)
-{
-	__u16 err_tval;
-	int err;
-
-        if(rsv->svl != S_ERROR_TIMER_VALUE)
-                return E_SUB_VECTOR_LENGTH_ERROR;
-
-        err_tval = (rsv->svv[0] << 8 | rsv->svv[1])*10;
-
-        smctr_issue_write_word_cmd(dev, RW_TER_THRESHOLD, &err_tval);
-
-        if((err = smctr_wait_cmd(dev)))
-                return err;
-
-        return POSITIVE_ACK;
-}
-
-static int smctr_set_frame_forward(struct net_device *dev,
-        MAC_SUB_VECTOR *rsv, __u8 dc_sc)
-{
-        if((rsv->svl < 2) || (rsv->svl > S_FRAME_FORWARD))
-                return E_SUB_VECTOR_LENGTH_ERROR;
-
-        if((dc_sc & DC_MASK) != DC_CRS)
-        {
-                if(rsv->svl >= 2 && rsv->svl < 20)
-			return E_TRANSMIT_FORWARD_INVALID;
-
-                if((rsv->svv[0] != 0) || (rsv->svv[1] != 0))
-                        return E_TRANSMIT_FORWARD_INVALID;
-        }
-
-        return POSITIVE_ACK;
-}
-
-static int smctr_set_local_ring_num(struct net_device *dev,
-        MAC_SUB_VECTOR *rsv)
-{
-        struct net_local *tp = netdev_priv(dev);
-
-        if(rsv->svl != S_LOCAL_RING_NUMBER)
-                return E_SUB_VECTOR_LENGTH_ERROR;
-
-        if(tp->ptr_local_ring_num)
-                *(__u16 *)(tp->ptr_local_ring_num) 
-			= (rsv->svv[0] << 8 | rsv->svv[1]);
-
-        return POSITIVE_ACK;
-}
-
-static unsigned short smctr_set_ctrl_attention(struct net_device *dev)
-{
-        struct net_local *tp = netdev_priv(dev);
-        int ioaddr = dev->base_addr;
-
-        if(tp->bic_type == BIC_585_CHIP)
-                outb((tp->trc_mask | HWR_CA), ioaddr + HWR);
-        else
-        {
-                outb((tp->trc_mask | CSR_CA), ioaddr + CSR);
-                outb(tp->trc_mask, ioaddr + CSR);
-        }
-
-        return 0;
-}
-
-static void smctr_set_multicast_list(struct net_device *dev)
-{
-        if(smctr_debug > 10)
-                printk(KERN_DEBUG "%s: smctr_set_multicast_list\n", dev->name);
-}
-
-static int smctr_set_page(struct net_device *dev, __u8 *buf)
-{
-        struct net_local *tp = netdev_priv(dev);
-        __u8 amask;
-        __u32 tptr;
-
-        tptr = (__u32)buf - (__u32)tp->ram_access;
-        amask = (__u8)((tptr & PR_PAGE_MASK) >> 8);
-        outb(amask, dev->base_addr + PR);
-
-        return 0;
-}
-
-static int smctr_set_phy_drop(struct net_device *dev, MAC_SUB_VECTOR *rsv)
-{
-	int err;
-
-        if(rsv->svl != S_PHYSICAL_DROP)
-                return E_SUB_VECTOR_LENGTH_ERROR;
-
-        smctr_issue_write_byte_cmd(dev, RW_PHYSICAL_DROP_NUMBER, &rsv->svv[0]);
-        if((err = smctr_wait_cmd(dev)))
-                return err;
-
-        return POSITIVE_ACK;
-}
-
-/* Reset the ring speed to the opposite of what it was. This auto-pilot
- * mode requires a complete reset and re-init of the adapter.
- */
-static int smctr_set_ring_speed(struct net_device *dev)
-{
-        struct net_local *tp = netdev_priv(dev);
-	int err;
-
-        if(tp->media_type == MEDIA_UTP_16)
-                tp->media_type = MEDIA_UTP_4;
-        else
-                tp->media_type = MEDIA_UTP_16;
-
-        smctr_enable_16bit(dev);
-
-        /* Re-Initialize adapter's internal registers */
-        smctr_reset_adapter(dev);
-
-        if((err = smctr_init_card_real(dev)))
-                return err;
-
-        smctr_enable_bic_int(dev);
-
-        if((err = smctr_issue_enable_int_cmd(dev, TRC_INTERRUPT_ENABLE_MASK)))
-                return err;
-
-        smctr_disable_16bit(dev);
-
-	return 0;
-}
-
-static int smctr_set_rx_look_ahead(struct net_device *dev)
-{
-        struct net_local *tp = netdev_priv(dev);
-        __u16 sword, rword;
-
-        if(smctr_debug > 10)
-                printk(KERN_DEBUG "%s: smctr_set_rx_look_ahead_flag\n", dev->name);
-
-        tp->adapter_flags &= ~(FORCED_16BIT_MODE);
-        tp->adapter_flags |= RX_VALID_LOOKAHEAD;
-
-        if(tp->adapter_bus == BUS_ISA16_TYPE)
-        {
-                sword = *((__u16 *)(tp->ram_access));
-                *((__u16 *)(tp->ram_access)) = 0x1234;
-
-                smctr_disable_16bit(dev);
-                rword = *((__u16 *)(tp->ram_access));
-                smctr_enable_16bit(dev);
-
-                if(rword != 0x1234)
-                        tp->adapter_flags |= FORCED_16BIT_MODE;
-
-                *((__u16 *)(tp->ram_access)) = sword;
-        }
-
-        return 0;
-}
-
-static int smctr_set_trc_reset(int ioaddr)
-{
-        __u8 r;
-
-        r = inb(ioaddr + MSR);
-        outb(MSR_RST | r, ioaddr + MSR);
-
-        return 0;
-}
-
-/*
- * This function can be called if the adapter is busy or not.
- */
-static int smctr_setup_single_cmd(struct net_device *dev,
-        __u16 command, __u16 subcommand)
-{
-        struct net_local *tp = netdev_priv(dev);
-        unsigned int err;
-
-        if(smctr_debug > 10)
-                printk(KERN_DEBUG "%s: smctr_setup_single_cmd\n", dev->name);
-
-        if((err = smctr_wait_while_cbusy(dev)))
-                return err;
-
-        if((err = (unsigned int)smctr_wait_cmd(dev)))
-                return err;
-
-        tp->acb_head->cmd_done_status   = 0;
-        tp->acb_head->cmd               = command;
-        tp->acb_head->subcmd            = subcommand;
-
-        err = smctr_issue_resume_acb_cmd(dev);
-
-        return err;
-}
-
-/*
- * This function can not be called with the adapter busy.
- */
-static int smctr_setup_single_cmd_w_data(struct net_device *dev,
-        __u16 command, __u16 subcommand)
-{
-        struct net_local *tp = netdev_priv(dev);
-
-        tp->acb_head->cmd_done_status   = ACB_COMMAND_NOT_DONE;
-        tp->acb_head->cmd               = command;
-        tp->acb_head->subcmd            = subcommand;
-        tp->acb_head->data_offset_lo
-                = (__u16)TRC_POINTER(tp->misc_command_data);
-
-        return smctr_issue_resume_acb_cmd(dev);
-}
-
-static char *smctr_malloc(struct net_device *dev, __u16 size)
-{
-        struct net_local *tp = netdev_priv(dev);
-        char *m;
-
-        m = (char *)(tp->ram_access + tp->sh_mem_used);
-        tp->sh_mem_used += (__u32)size;
-
-        return m;
-}
-
-static int smctr_status_chg(struct net_device *dev)
-{
-        struct net_local *tp = netdev_priv(dev);
-
-        if(smctr_debug > 10)
-                printk(KERN_DEBUG "%s: smctr_status_chg\n", dev->name);
-
-        switch(tp->status)
-        {
-                case OPEN:
-                        break;
-
-                case CLOSED:
-                        break;
-
-                /* Interrupt driven open() completion. XXX */
-                case INITIALIZED:
-                        tp->group_address_0 = 0;
-                        tp->group_address[0] = 0;
-                        tp->group_address[1] = 0;
-                        tp->functional_address_0 = 0;
-                        tp->functional_address[0] = 0;
-                        tp->functional_address[1] = 0;
-                        smctr_open_tr(dev);
-                        break;
-
-                default:
-                        printk(KERN_INFO "%s: status change unknown %x\n",
-                                dev->name, tp->status);
-                        break;
-        }
-
-        return 0;
-}
-
-static int smctr_trc_send_packet(struct net_device *dev, FCBlock *fcb,
-        __u16 queue)
-{
-        struct net_local *tp = netdev_priv(dev);
-        int err = 0;
-
-        if(smctr_debug > 10)
-                printk(KERN_DEBUG "%s: smctr_trc_send_packet\n", dev->name);
-
-        fcb->info = FCB_CHAIN_END | FCB_ENABLE_TFS;
-        if(tp->num_tx_fcbs[queue] != 1)
-                fcb->back_ptr->info = FCB_INTERRUPT_ENABLE | FCB_ENABLE_TFS;
-
-        if(tp->tx_queue_status[queue] == NOT_TRANSMITING)
-        {
-                tp->tx_queue_status[queue] = TRANSMITING;
-                err = smctr_issue_resume_tx_fcb_cmd(dev, queue);
-        }
-
-        return err;
-}
-
-static __u16 smctr_tx_complete(struct net_device *dev, __u16 queue)
-{
-        struct net_local *tp = netdev_priv(dev);
-        __u16 status, err = 0;
-        int cstatus;
-
-        if(smctr_debug > 10)
-                printk(KERN_DEBUG "%s: smctr_tx_complete\n", dev->name);
-
-        while((status = tp->tx_fcb_end[queue]->frame_status) != SUCCESS)
-        {
-                if(status & 0x7e00 )
-                {
-                        err = HARDWARE_FAILED;
-                        break;
-                }
-
-                if((err = smctr_update_tx_chain(dev, tp->tx_fcb_end[queue],
-                        queue)) != SUCCESS)
-                        break;
-
-                smctr_disable_16bit(dev);
-
-                if(tp->mode_bits & UMAC)
-                {
-                        if(!(status & (FCB_TX_STATUS_AR1 | FCB_TX_STATUS_AR2)))
-                                cstatus = NO_SUCH_DESTINATION;
-                        else
-                        {
-                                if(!(status & (FCB_TX_STATUS_CR1 | FCB_TX_STATUS_CR2)))
-                                        cstatus = DEST_OUT_OF_RESOURCES;
-                                else
-                                {
-                                        if(status & FCB_TX_STATUS_E)
-                                                cstatus = MAX_COLLISIONS;
-                                        else
-                                                cstatus = SUCCESS;
-                                }
-                        }
-                }
-                else
-                        cstatus = SUCCESS;
-
-                if(queue == BUG_QUEUE)
-                        err = SUCCESS;
-
-                smctr_enable_16bit(dev);
-                if(err != SUCCESS)
-                        break;
-        }
-
-        return err;
-}
-
-static unsigned short smctr_tx_move_frame(struct net_device *dev,
-        struct sk_buff *skb, __u8 *pbuff, unsigned int bytes)
-{
-        struct net_local *tp = netdev_priv(dev);
-        unsigned int ram_usable;
-        __u32 flen, len, offset = 0;
-        __u8 *frag, *page;
-
-        if(smctr_debug > 10)
-                printk(KERN_DEBUG "%s: smctr_tx_move_frame\n", dev->name);
-
-        ram_usable = ((unsigned int)tp->ram_usable) << 10;
-        frag       = skb->data;
-        flen       = skb->len;
-
-        while(flen > 0 && bytes > 0)
-        {
-                smctr_set_page(dev, pbuff);
-
-                offset = SMC_PAGE_OFFSET(pbuff);
-
-                if(offset + flen > ram_usable)
-                        len = ram_usable - offset;
-                else
-                        len = flen;
-
-                if(len > bytes)
-                        len = bytes;
-
-                page = (char *) (offset + tp->ram_access);
-                memcpy(page, frag, len);
-
-                flen -=len;
-                bytes -= len;
-                frag += len;
-                pbuff += len;
-        }
-
-        return 0;
-}
-
-/* Update the error statistic counters for this adapter. */
-static int smctr_update_err_stats(struct net_device *dev)
-{
-        struct net_local *tp = netdev_priv(dev);
-        struct tr_statistics *tstat = &tp->MacStat;
-
-        if(tstat->internal_errors)
-                tstat->internal_errors
-                        += *(tp->misc_command_data + 0) & 0x00ff;
-
-        if(tstat->line_errors)
-                tstat->line_errors += *(tp->misc_command_data + 0) >> 8;
-
-        if(tstat->A_C_errors)
-                tstat->A_C_errors += *(tp->misc_command_data + 1) & 0x00ff;
-
-        if(tstat->burst_errors)
-                tstat->burst_errors += *(tp->misc_command_data + 1) >> 8;
-
-        if(tstat->abort_delimiters)
-                tstat->abort_delimiters += *(tp->misc_command_data + 2) >> 8;
-
-        if(tstat->recv_congest_count)
-                tstat->recv_congest_count
-                        += *(tp->misc_command_data + 3) & 0x00ff;
-
-        if(tstat->lost_frames)
-                tstat->lost_frames
-                        += *(tp->misc_command_data + 3) >> 8;
-
-        if(tstat->frequency_errors)
-                tstat->frequency_errors += *(tp->misc_command_data + 4) & 0x00ff;
-
-        if(tstat->frame_copied_errors)
-                 tstat->frame_copied_errors
-                        += *(tp->misc_command_data + 4) >> 8;
-
-        if(tstat->token_errors)
-                tstat->token_errors += *(tp->misc_command_data + 5) >> 8;
-
-        return 0;
-}
-
-static int smctr_update_rx_chain(struct net_device *dev, __u16 queue)
-{
-        struct net_local *tp = netdev_priv(dev);
-        FCBlock *fcb;
-        BDBlock *bdb;
-        __u16 size, len;
-
-        fcb = tp->rx_fcb_curr[queue];
-        len = fcb->frame_length;
-
-        fcb->frame_status = 0;
-        fcb->info = FCB_CHAIN_END;
-        fcb->back_ptr->info = FCB_WARNING;
-
-        tp->rx_fcb_curr[queue] = tp->rx_fcb_curr[queue]->next_ptr;
-
-        /* update RX BDBs */
-        size = (len >> RX_BDB_SIZE_SHIFT);
-        if(len & RX_DATA_BUFFER_SIZE_MASK)
-                size += sizeof(BDBlock);
-        size &= (~RX_BDB_SIZE_MASK);
-
-        /* check if wrap around */
-        bdb = (BDBlock *)((__u32)(tp->rx_bdb_curr[queue]) + (__u32)(size));
-        if((__u32)bdb >= (__u32)tp->rx_bdb_end[queue])
-        {
-                bdb = (BDBlock *)((__u32)(tp->rx_bdb_head[queue])
-                        + (__u32)(bdb) - (__u32)(tp->rx_bdb_end[queue]));
-        }
-
-        bdb->back_ptr->info = BDB_CHAIN_END;
-        tp->rx_bdb_curr[queue]->back_ptr->info = BDB_NOT_CHAIN_END;
-        tp->rx_bdb_curr[queue] = bdb;
-
-        return 0;
-}
-
-static int smctr_update_tx_chain(struct net_device *dev, FCBlock *fcb,
-        __u16 queue)
-{
-        struct net_local *tp = netdev_priv(dev);
-
-        if(smctr_debug > 20)
-                printk(KERN_DEBUG "smctr_update_tx_chain\n");
-
-        if(tp->num_tx_fcbs_used[queue] <= 0)
-                return HARDWARE_FAILED;
-        else
-        {
-                if(tp->tx_buff_used[queue] < fcb->memory_alloc)
-                {
-                        tp->tx_buff_used[queue] = 0;
-                        return HARDWARE_FAILED;
-                }
-
-                tp->tx_buff_used[queue] -= fcb->memory_alloc;
-
-                /* if all transmit buffer are cleared
-                 * need to set the tx_buff_curr[] to tx_buff_head[]
-                 * otherwise, tx buffer will be segregate and cannot
-                 * accommodate and buffer greater than (curr - head) and
-                 * (end - curr) since we do not allow wrap around allocation.
-                 */
-                if(tp->tx_buff_used[queue] == 0)
-                        tp->tx_buff_curr[queue] = tp->tx_buff_head[queue];
-
-                tp->num_tx_fcbs_used[queue]--;
-                fcb->frame_status = 0;
-                tp->tx_fcb_end[queue] = fcb->next_ptr;
-		netif_wake_queue(dev);
-                return 0;
-        }
-}
-
-static int smctr_wait_cmd(struct net_device *dev)
-{
-        struct net_local *tp = netdev_priv(dev);
-        unsigned int loop_count = 0x20000;
-
-        if(smctr_debug > 10)
-                printk(KERN_DEBUG "%s: smctr_wait_cmd\n", dev->name);
-
-        while(loop_count)
-        {
-                if(tp->acb_head->cmd_done_status & ACB_COMMAND_DONE)
-                        break;
-		udelay(1);
-                loop_count--;
-        }
-
-        if(loop_count == 0)
-                return HARDWARE_FAILED;
-
-        if(tp->acb_head->cmd_done_status & 0xff)
-                return HARDWARE_FAILED;
-
-        return 0;
-}
-
-static int smctr_wait_while_cbusy(struct net_device *dev)
-{
-        struct net_local *tp = netdev_priv(dev);
-        unsigned int timeout = 0x20000;
-        int ioaddr = dev->base_addr;
-        __u8 r;
-
-        if(tp->bic_type == BIC_585_CHIP)
-        {
-                while(timeout)
-                {
-                        r = inb(ioaddr + HWR);
-                        if((r & HWR_CBUSY) == 0)
-                                break;
-                        timeout--;
-                }
-        }
-        else
-        {
-                while(timeout)
-                {
-                        r = inb(ioaddr + CSR);
-                        if((r & CSR_CBUSY) == 0)
-                                break;
-                        timeout--;
-                }
-        }
-
-        if(timeout)
-                return 0;
-        else
-                return HARDWARE_FAILED;
-}
-
-#ifdef MODULE
-
-static struct net_device* dev_smctr[SMCTR_MAX_ADAPTERS];
-static int io[SMCTR_MAX_ADAPTERS];
-static int irq[SMCTR_MAX_ADAPTERS];
-
-MODULE_LICENSE("GPL");
-MODULE_FIRMWARE("tr_smctr.bin");
-
-module_param_array(io, int, NULL, 0);
-module_param_array(irq, int, NULL, 0);
-module_param(ringspeed, int, 0);
-
-static struct net_device * __init setup_card(int n)
-{
-	struct net_device *dev = alloc_trdev(sizeof(struct net_local));
-	int err;
-	
-	if (!dev)
-		return ERR_PTR(-ENOMEM);
-
-	dev->irq = irq[n];
-	err = smctr_probe1(dev, io[n]);
-	if (err) 
-		goto out;
-		
-	err = register_netdev(dev);
-	if (err)
-		goto out1;
-	return dev;
- out1:
-#ifdef CONFIG_MCA_LEGACY
-	{ struct net_local *tp = netdev_priv(dev);
-	  if (tp->slot_num)
-		mca_mark_as_unused(tp->slot_num);
-	}
-#endif
-	release_region(dev->base_addr, SMCTR_IO_EXTENT);
-	free_irq(dev->irq, dev);
-out:
-	free_netdev(dev);
-	return ERR_PTR(err);
-}
-
-int __init init_module(void)
-{
-        int i, found = 0;
-	struct net_device *dev;
-
-        for(i = 0; i < SMCTR_MAX_ADAPTERS; i++) {
-		dev = io[0]? setup_card(i) : smctr_probe(-1);
-		if (!IS_ERR(dev)) {
-			++found;
-			dev_smctr[i] = dev;
-		}
-        }
-
-        return found ? 0 : -ENODEV;
-}
-
-void __exit cleanup_module(void)
-{
-        int i;
-
-        for(i = 0; i < SMCTR_MAX_ADAPTERS; i++) {
-		struct net_device *dev = dev_smctr[i];
-
-		if (dev) {
-
-			unregister_netdev(dev);
-#ifdef CONFIG_MCA_LEGACY
-			{ struct net_local *tp = netdev_priv(dev);
-			if (tp->slot_num)
-				mca_mark_as_unused(tp->slot_num);
-			}
-#endif
-			release_region(dev->base_addr, SMCTR_IO_EXTENT);
-			if (dev->irq)
-				free_irq(dev->irq, dev);
-
-			free_netdev(dev);
-		}
-        }
-}
-#endif /* MODULE */
diff --git a/drivers/net/tokenring/smctr.h b/drivers/net/tokenring/smctr.h
deleted file mode 100644
index 6e5700ab4fc..00000000000
--- a/drivers/net/tokenring/smctr.h
+++ /dev/null
@@ -1,1585 +0,0 @@
-/* smctr.h: SMC Token Ring driver header for Linux
- *
- * Authors:
- *  - Jay Schulist <jschlst@samba.org>
- */
-
-#ifndef __LINUX_SMCTR_H
-#define __LINUX_SMCTR_H
-
-#ifdef __KERNEL__
-
-#define MAX_TX_QUEUE 10
-
-#define SMC_HEADER_SIZE 14
-
-#define SMC_PAGE_OFFSET(X)          (((unsigned long)(X) - tp->ram_access) & tp->page_offset_mask)
-
-#define INIT            0x0D
-#define RQ_ATTCH        0x10
-#define RQ_STATE        0x0F
-#define RQ_ADDR         0x0E
-#define CHG_PARM        0x0C
-#define RSP             0x00
-#define TX_FORWARD      0x09
-
-#define AC_FC_DAT	((3<<13) | 1)
-#define      DAT             0x07
-
-#define RPT_NEW_MON     0x25
-#define RPT_SUA_CHG     0x26
-#define RPT_ACTIVE_ERR  0x28
-#define RPT_NN_INCMP    0x27
-#define RPT_ERROR       0x29
-
-#define RQ_INIT         0x20
-#define RPT_ATTCH       0x24
-#define RPT_STATE       0x23
-#define RPT_ADDR        0x22
-
-#define POSITIVE_ACK                    0x0001
-#define A_FRAME_WAS_FORWARDED           0x8888
-
-#define      GROUP_ADDRESS                   0x2B
-#define      PHYSICAL_DROP                   0x0B
-#define      AUTHORIZED_ACCESS_PRIORITY      0x07
-#define      AUTHORIZED_FUNCTION_CLASS       0x06
-#define      FUNCTIONAL_ADDRESS              0x2C
-#define      RING_STATION_STATUS             0x29
-#define      TRANSMIT_STATUS_CODE            0x2A
-#define      IBM_PASS_SOURCE_ADDR    0x01
-#define      AC_FC_RPT_TX_FORWARD            ((0<<13) | 0)
-#define      AC_FC_RPT_STATE                 ((0<<13) | 0)
-#define      AC_FC_RPT_ADDR                  ((0<<13) | 0)
-#define      CORRELATOR                      0x09
-
-#define POSITIVE_ACK                    0x0001          /*             */
-#define E_MAC_DATA_INCOMPLETE           0x8001          /* not used    */
-#define E_VECTOR_LENGTH_ERROR           0x8002          /*             */
-#define E_UNRECOGNIZED_VECTOR_ID        0x8003          /*             */
-#define E_INAPPROPRIATE_SOURCE_CLASS    0x8004          /*             */
-#define E_SUB_VECTOR_LENGTH_ERROR       0x8005          /*             */
-#define E_TRANSMIT_FORWARD_INVALID      0x8006          /* def. by IBM */
-#define E_MISSING_SUB_VECTOR            0x8007          /*             */
-#define E_SUB_VECTOR_UNKNOWN            0x8008          /*             */
-#define E_MAC_HEADER_TOO_LONG           0x8009          /*             */
-#define E_FUNCTION_DISABLED             0x800A          /* not used    */
-
-#define A_FRAME_WAS_FORWARDED           0x8888          /* used by send_TX_FORWARD */
-
-#define UPSTREAM_NEIGHBOR_ADDRESS       0x02
-#define LOCAL_RING_NUMBER               0x03
-#define ASSIGN_PHYSICAL_DROP            0x04
-#define ERROR_TIMER_VALUE               0x05
-#define AUTHORIZED_FUNCTION_CLASS       0x06
-#define AUTHORIZED_ACCESS_PRIORITY      0x07
-#define CORRELATOR                      0x09
-#define PHYSICAL_DROP                   0x0B
-#define RESPONSE_CODE                   0x20
-#define ADDRESS_MODIFER                 0x21
-#define PRODUCT_INSTANCE_ID             0x22
-#define RING_STATION_VERSION_NUMBER     0x23
-#define WRAP_DATA                       0x26
-#define FRAME_FORWARD                   0x27
-#define STATION_IDENTIFER               0x28
-#define RING_STATION_STATUS             0x29
-#define TRANSMIT_STATUS_CODE            0x2A
-#define GROUP_ADDRESS                   0x2B
-#define FUNCTIONAL_ADDRESS              0x2C
-
-#define F_NO_SUB_VECTORS_FOUND                  0x0000
-#define F_UPSTREAM_NEIGHBOR_ADDRESS             0x0001
-#define F_LOCAL_RING_NUMBER                     0x0002
-#define F_ASSIGN_PHYSICAL_DROP                  0x0004
-#define F_ERROR_TIMER_VALUE                     0x0008
-#define F_AUTHORIZED_FUNCTION_CLASS             0x0010
-#define F_AUTHORIZED_ACCESS_PRIORITY            0x0020
-#define F_CORRELATOR                            0x0040
-#define F_PHYSICAL_DROP                         0x0080
-#define F_RESPONSE_CODE                         0x0100
-#define F_PRODUCT_INSTANCE_ID                   0x0200
-#define F_RING_STATION_VERSION_NUMBER           0x0400
-#define F_STATION_IDENTIFER                     0x0800
-#define F_RING_STATION_STATUS                   0x1000
-#define F_GROUP_ADDRESS                         0x2000
-#define F_FUNCTIONAL_ADDRESS                    0x4000
-#define F_FRAME_FORWARD                         0x8000
-
-#define R_INIT                                  0x00
-#define R_RQ_ATTCH_STATE_ADDR                   0x00
-#define R_CHG_PARM                              0x00
-#define R_TX_FORWARD                            F_FRAME_FORWARD
-
-
-#define      UPSTREAM_NEIGHBOR_ADDRESS       0x02
-#define      ADDRESS_MODIFER                 0x21
-#define      RING_STATION_VERSION_NUMBER     0x23
-#define      PRODUCT_INSTANCE_ID             0x22
-
-#define      RPT_TX_FORWARD  0x2A
-
-#define AC_FC_INIT                      (3<<13) | 0 /*                     */
-#define AC_FC_RQ_INIT                   ((3<<13) | 0) /*                     */
-#define AC_FC_RQ_ATTCH                  (3<<13) | 0 /* DC = SC of rx frame */
-#define AC_FC_RQ_STATE                  (3<<13) | 0 /* DC = SC of rx frame */
-#define AC_FC_RQ_ADDR                   (3<<13) | 0 /* DC = SC of rx frame */
-#define AC_FC_CHG_PARM                  (3<<13) | 0 /*                     */
-#define AC_FC_RSP                       (0<<13) | 0 /* DC = SC of rx frame */
-#define AC_FC_RPT_ATTCH                 (0<<13) | 0
-
-#define S_UPSTREAM_NEIGHBOR_ADDRESS               6 + 2
-#define S_LOCAL_RING_NUMBER                       2 + 2
-#define S_ASSIGN_PHYSICAL_DROP                    4 + 2
-#define S_ERROR_TIMER_VALUE                       2 + 2
-#define S_AUTHORIZED_FUNCTION_CLASS               2 + 2
-#define S_AUTHORIZED_ACCESS_PRIORITY              2 + 2
-#define S_CORRELATOR                              2 + 2
-#define S_PHYSICAL_DROP                           4 + 2
-#define S_RESPONSE_CODE                           4 + 2
-#define S_ADDRESS_MODIFER                         2 + 2
-#define S_PRODUCT_INSTANCE_ID                    18 + 2
-#define S_RING_STATION_VERSION_NUMBER            10 + 2
-#define S_STATION_IDENTIFER                       6 + 2
-#define S_RING_STATION_STATUS                     6 + 2
-#define S_GROUP_ADDRESS                           4 + 2
-#define S_FUNCTIONAL_ADDRESS                      4 + 2
-#define S_FRAME_FORWARD                         252 + 2
-#define S_TRANSMIT_STATUS_CODE                    2 + 2
-
-#define ISB_IMC_RES0                    0x0000  /* */
-#define ISB_IMC_MAC_TYPE_3              0x0001  /* MAC_ARC_INDICATE */
-#define ISB_IMC_MAC_ERROR_COUNTERS      0x0002  /* */
-#define ISB_IMC_RES1                    0x0003  /* */
-#define ISB_IMC_MAC_TYPE_2              0x0004  /* QUE_MAC_INDICATE */
-#define ISB_IMC_TX_FRAME                0x0005  /* */
-#define ISB_IMC_END_OF_TX_QUEUE         0x0006  /* */
-#define ISB_IMC_NON_MAC_RX_RESOURCE     0x0007  /* */
-#define ISB_IMC_MAC_RX_RESOURCE         0x0008  /* */
-#define ISB_IMC_NON_MAC_RX_FRAME        0x0009  /* */
-#define ISB_IMC_MAC_RX_FRAME            0x000A  /* */
-#define ISB_IMC_TRC_FIFO_STATUS         0x000B  /* */
-#define ISB_IMC_COMMAND_STATUS          0x000C  /* */
-#define ISB_IMC_MAC_TYPE_1              0x000D  /* Self Removed */
-#define ISB_IMC_TRC_INTRNL_TST_STATUS   0x000E  /* */
-#define ISB_IMC_RES2                    0x000F  /* */
-
-#define NON_MAC_RX_RESOURCE_BW          0x10    /* shifted right 8 bits */
-#define NON_MAC_RX_RESOURCE_FW          0x20    /* shifted right 8 bits */
-#define NON_MAC_RX_RESOURCE_BE          0x40    /* shifted right 8 bits */
-#define NON_MAC_RX_RESOURCE_FE          0x80    /* shifted right 8 bits */
-#define RAW_NON_MAC_RX_RESOURCE_BW      0x1000  /* */
-#define RAW_NON_MAC_RX_RESOURCE_FW      0x2000  /* */
-#define RAW_NON_MAC_RX_RESOURCE_BE      0x4000  /* */
-#define RAW_NON_MAC_RX_RESOURCE_FE      0x8000  /* */
-
-#define MAC_RX_RESOURCE_BW              0x10    /* shifted right 8 bits */
-#define MAC_RX_RESOURCE_FW              0x20    /* shifted right 8 bits */
-#define MAC_RX_RESOURCE_BE              0x40    /* shifted right 8 bits */
-#define MAC_RX_RESOURCE_FE              0x80    /* shifted right 8 bits */
-#define RAW_MAC_RX_RESOURCE_BW          0x1000  /* */
-#define RAW_MAC_RX_RESOURCE_FW          0x2000  /* */
-#define RAW_MAC_RX_RESOURCE_BE          0x4000  /* */
-#define RAW_MAC_RX_RESOURCE_FE          0x8000  /* */
-
-#define TRC_FIFO_STATUS_TX_UNDERRUN     0x40    /* shifted right 8 bits */
-#define TRC_FIFO_STATUS_RX_OVERRUN      0x80    /* shifted right 8 bits */
-#define RAW_TRC_FIFO_STATUS_TX_UNDERRUN 0x4000  /* */
-#define RAW_TRC_FIFO_STATUS_RX_OVERRUN  0x8000  /* */
-
-#define       CSR_CLRTINT             0x08
-
-#define MSB(X)                  ((__u8)((__u16) X >> 8))
-#define LSB(X)                  ((__u8)((__u16) X &  0xff))
-
-#define AC_FC_LOBE_MEDIA_TEST           ((3<<13) | 0)
-#define S_WRAP_DATA                             248 + 2 /* 500 + 2 */
-#define      WRAP_DATA                       0x26
-#define LOBE_MEDIA_TEST 0x08
-
-/* Destination Class (dc) */
-
-#define DC_MASK         0xF0
-#define DC_RS           0x00
-#define DC_CRS          0x40
-#define DC_RPS          0x50
-#define DC_REM          0x60
-
-/* Source Classes (sc) */
-
-#define SC_MASK         0x0F
-#define SC_RS           0x00
-#define SC_CRS          0x04
-#define SC_RPS          0x05
-#define SC_REM          0x06
-
-#define PR		0x11
-#define PR_PAGE_MASK	0x0C000
-
-#define MICROCHANNEL	0x0008
-#define INTERFACE_CHIP	0x0010
-#define BOARD_16BIT	0x0040
-#define PAGED_RAM	0x0080
-#define WD8115TA	(TOKEN_MEDIA | MICROCHANNEL | INTERFACE_CHIP | PAGED_RAM)
-#define WD8115T		(TOKEN_MEDIA | INTERFACE_CHIP | BOARD_16BIT | PAGED_RAM)
-
-#define BRD_ID_8316	0x50
-
-#define r587_SER	0x001
-#define SER_DIN		0x80
-#define SER_DOUT	0x40
-#define SER_CLK		0x20
-#define SER_ECS		0x10
-#define SER_E806	0x08
-#define SER_PNP		0x04
-#define SER_BIO		0x02
-#define SER_16B		0x01
-
-#define r587_IDR	0x004
-#define IDR_IRQ_MASK	0x0F0
-#define IDR_DCS_MASK	0x007
-#define IDR_RWS		0x008
-
-
-#define r587_BIO	0x003
-#define BIO_ENB		0x080
-#define BIO_MASK	0x03F
-
-#define r587_PCR	0x005
-#define PCR_RAMS	0x040
-
-
-
-#define NUM_ADDR_BITS	8
-
-#define ISA_MAX_ADDRESS		0x00ffffff
-
-#define SMCTR_MAX_ADAPTERS	7
-
-#define MC_TABLE_ENTRIES      16
-
-#define MAXFRAGMENTS          32
-
-#define CHIP_REV_MASK         0x3000
-
-#define MAX_TX_QS             8
-#define NUM_TX_QS_USED        3
-
-#define MAX_RX_QS             2
-#define NUM_RX_QS_USED        2
-
-#define INTEL_DATA_FORMAT	0x4000
-#define INTEL_ADDRESS_POINTER_FORMAT	0x8000
-#define PAGE_POINTER(X)		((((unsigned long)(X) - tp->ram_access) & tp->page_offset_mask) + tp->ram_access)
-#define SWAP_WORDS(X)		(((X & 0xFFFF) << 16) | (X >> 16))
-
-#define INTERFACE_CHIP          0x0010          /* Soft Config Adapter */
-#define ADVANCED_FEATURES       0x0020          /* Adv. netw. interface features */
-#define BOARD_16BIT             0x0040          /* 16 bit capability */
-#define PAGED_RAM               0x0080          /* Adapter has paged RAM */
-
-#define PAGED_ROM               0x0100          /* Adapter has paged ROM */
-
-#define RAM_SIZE_UNKNOWN        0x0000          /* Unknown RAM size */
-#define RAM_SIZE_0K             0x0001          /* 0K  RAM */
-#define RAM_SIZE_8K             0x0002          /* 8k  RAM */
-#define RAM_SIZE_16K            0x0003          /* 16k RAM */
-#define RAM_SIZE_32K            0x0004          /* 32k RAM */
-#define RAM_SIZE_64K            0x0005          /* 64k RAM */
-#define RAM_SIZE_RESERVED_6     0x0006          /* Reserved RAM size */
-#define RAM_SIZE_RESERVED_7     0x0007          /* Reserved RAM size */
-#define RAM_SIZE_MASK           0x0007          /* Isolates RAM Size */
-
-#define TOKEN_MEDIA           0x0005
-
-#define BID_REG_0       0x00
-#define BID_REG_1       0x01
-#define BID_REG_2       0x02
-#define BID_REG_3       0x03
-#define BID_REG_4       0x04
-#define BID_REG_5       0x05
-#define BID_REG_6       0x06
-#define BID_REG_7       0x07
-#define BID_LAR_0       0x08
-#define BID_LAR_1       0x09
-#define BID_LAR_2       0x0A
-#define BID_LAR_3       0x0B
-#define BID_LAR_4       0x0C
-#define BID_LAR_5       0x0D
-
-#define BID_BOARD_ID_BYTE       0x0E
-#define BID_CHCKSM_BYTE         0x0F
-#define BID_LAR_OFFSET          0x08  
-
-#define BID_MSZ_583_BIT         0x08
-#define BID_SIXTEEN_BIT_BIT     0x01
-
-#define BID_BOARD_REV_MASK      0x1E
-
-#define BID_MEDIA_TYPE_BIT      0x01
-#define BID_SOFT_CONFIG_BIT     0x20
-#define BID_RAM_SIZE_BIT        0x40
-#define BID_BUS_TYPE_BIT        0x80
-
-#define BID_CR          0x10
-
-#define BID_TXP         0x04            /* Transmit Packet Command */
-
-#define BID_TCR_DIFF    0x0D    /* Transmit Configuration Register */
-
-#define BID_TCR_VAL     0x18            /* Value to Test 8390 or 690 */
-#define BID_PS0         0x00            /* Register Page Select 0 */
-#define BID_PS1         0x40            /* Register Page Select 1 */
-#define BID_PS2         0x80            /* Register Page Select 2 */
-#define BID_PS_MASK     0x3F            /* For Masking Off Page Select Bits */
-
-#define BID_EEPROM_0                    0x08
-#define BID_EEPROM_1                    0x09
-#define BID_EEPROM_2                    0x0A
-#define BID_EEPROM_3                    0x0B
-#define BID_EEPROM_4                    0x0C
-#define BID_EEPROM_5                    0x0D
-#define BID_EEPROM_6                    0x0E
-#define BID_EEPROM_7                    0x0F
-
-#define BID_OTHER_BIT                   0x02
-#define BID_ICR_MASK                    0x0C
-#define BID_EAR_MASK                    0x0F
-#define BID_ENGR_PAGE                   0x0A0
-#define BID_RLA                         0x10
-#define BID_EA6                         0x80
-#define BID_RECALL_DONE_MASK            0x10
-#define BID_BID_EEPROM_OVERRIDE         0xFFB0
-#define BID_EXTRA_EEPROM_OVERRIDE       0xFFD0
-#define BID_EEPROM_MEDIA_MASK           0x07
-#define BID_STARLAN_TYPE                0x00
-#define BID_ETHERNET_TYPE               0x01
-#define BID_TP_TYPE                     0x02
-#define BID_EW_TYPE                     0x03
-#define BID_TOKEN_RING_TYPE             0x04
-#define BID_UTP2_TYPE                   0x05
-#define BID_EEPROM_IRQ_MASK             0x18
-#define BID_PRIMARY_IRQ                 0x00
-#define BID_ALTERNATE_IRQ_1             0x08
-#define BID_ALTERNATE_IRQ_2             0x10
-#define BID_ALTERNATE_IRQ_3             0x18
-#define BID_EEPROM_RAM_SIZE_MASK        0xE0
-#define BID_EEPROM_RAM_SIZE_RES1        0x00
-#define BID_EEPROM_RAM_SIZE_RES2        0x20
-#define BID_EEPROM_RAM_SIZE_8K          0x40
-#define BID_EEPROM_RAM_SIZE_16K         0x60
-#define BID_EEPROM_RAM_SIZE_32K         0x80
-#define BID_EEPROM_RAM_SIZE_64K         0xA0
-#define BID_EEPROM_RAM_SIZE_RES3        0xC0
-#define BID_EEPROM_RAM_SIZE_RES4        0xE0
-#define BID_EEPROM_BUS_TYPE_MASK        0x07
-#define BID_EEPROM_BUS_TYPE_AT          0x00
-#define BID_EEPROM_BUS_TYPE_MCA         0x01
-#define BID_EEPROM_BUS_TYPE_EISA        0x02
-#define BID_EEPROM_BUS_TYPE_NEC         0x03
-#define BID_EEPROM_BUS_SIZE_MASK        0x18
-#define BID_EEPROM_BUS_SIZE_8BIT        0x00
-#define BID_EEPROM_BUS_SIZE_16BIT       0x08
-#define BID_EEPROM_BUS_SIZE_32BIT       0x10
-#define BID_EEPROM_BUS_SIZE_64BIT       0x18
-#define BID_EEPROM_BUS_MASTER           0x20
-#define BID_EEPROM_RAM_PAGING           0x40
-#define BID_EEPROM_ROM_PAGING           0x80
-#define BID_EEPROM_PAGING_MASK          0xC0
-#define BID_EEPROM_LOW_COST             0x08
-#define BID_EEPROM_IO_MAPPED            0x10
-#define BID_EEPROM_HMI                  0x01
-#define BID_EEPROM_AUTO_MEDIA_DETECT    0x01
-#define BID_EEPROM_CHIP_REV_MASK        0x0C
-
-#define BID_EEPROM_LAN_ADDR             0x30
-
-#define BID_EEPROM_MEDIA_OPTION         0x54
-#define BID_EEPROM_MEDIA_UTP            0x01
-#define BID_EEPROM_4MB_RING             0x08
-#define BID_EEPROM_16MB_RING            0x10
-#define BID_EEPROM_MEDIA_STP            0x40
-
-#define BID_EEPROM_MISC_DATA            0x56
-#define BID_EEPROM_EARLY_TOKEN_RELEASE  0x02
-
-#define CNFG_ID_8003E           0x6fc0
-#define CNFG_ID_8003S           0x6fc1
-#define CNFG_ID_8003W           0x6fc2
-#define CNFG_ID_8115TRA         0x6ec6
-#define CNFG_ID_8013E           0x61C8
-#define CNFG_ID_8013W           0x61C9
-#define CNFG_ID_BISTRO03E       0xEFE5
-#define CNFG_ID_BISTRO13E       0xEFD5
-#define CNFG_ID_BISTRO13W       0xEFD4
-#define CNFG_MSR_583    0x0
-#define CNFG_ICR_583    0x1
-#define CNFG_IAR_583    0x2
-#define CNFG_BIO_583    0x3
-#define CNFG_EAR_583    0x3
-#define CNFG_IRR_583    0x4
-#define CNFG_LAAR_584   0x5
-#define CNFG_GP2                0x7
-#define CNFG_LAAR_MASK          0x1F
-#define CNFG_LAAR_ZWS           0x20
-#define CNFG_LAAR_L16E          0x40
-#define CNFG_ICR_IR2_584        0x04
-#define CNFG_ICR_MASK       0x08
-#define CNFG_ICR_MSZ        0x08
-#define CNFG_ICR_RLA        0x10
-#define CNFG_ICR_STO        0x80
-#define CNFG_IRR_IRQS           0x60
-#define CNFG_IRR_IEN            0x80
-#define CNFG_IRR_ZWS            0x01
-#define CNFG_GP2_BOOT_NIBBLE    0x0F
-#define CNFG_IRR_OUT2       0x04
-#define CNFG_IRR_OUT1       0x02
-
-#define CNFG_SIZE_8KB           8
-#define CNFG_SIZE_16KB          16
-#define CNFG_SIZE_32KB          32
-#define CNFG_SIZE_64KB          64
-#define CNFG_SIZE_128KB     128
-#define CNFG_SIZE_256KB     256
-#define ROM_DISABLE             0x0
-
-#define CNFG_SLOT_ENABLE_BIT    0x08
-
-#define CNFG_POS_CONTROL_REG    0x096
-#define CNFG_POS_REG0           0x100
-#define CNFG_POS_REG1           0x101
-#define CNFG_POS_REG2           0x102
-#define CNFG_POS_REG3           0x103
-#define CNFG_POS_REG4           0x104
-#define CNFG_POS_REG5           0x105
-
-#define CNFG_ADAPTER_TYPE_MASK  0x0e
-
-#define SLOT_16BIT              0x0008
-#define INTERFACE_5X3_CHIP      0x0000          /* 0000 = 583 or 593 chips */
-#define NIC_690_BIT                     0x0010          /* NIC is 690 */
-#define ALTERNATE_IRQ_BIT       0x0020          /* Alternate IRQ is used */
-#define INTERFACE_584_CHIP      0x0040          /* 0001 = 584 chip */
-#define INTERFACE_594_CHIP      0x0080          /* 0010 = 594 chip */
-#define INTERFACE_585_CHIP      0x0100          /* 0100 = 585/790 chip */
-#define INTERFACE_CHIP_MASK     0x03C0          /* Isolates Intfc Chip Type */
-
-#define BOARD_16BIT             0x0040
-#define NODE_ADDR_CKSUM 	0xEE
-#define BRD_ID_8115T    	0x04
-
-#define NIC_825_BIT             0x0400          /* TRC 83C825 NIC */
-#define NIC_790_BIT             0x0800          /* NIC is 83C790 Ethernet */
-
-#define CHIP_REV_MASK           0x3000
-
-#define HWR_CBUSY			0x02
-#define HWR_CA				0x01
-
-#define MAC_QUEUE                       0
-#define NON_MAC_QUEUE                   1
-#define BUG_QUEUE                       2       /* NO RECEIVE QUEUE, ONLY TX */
-
-#define NUM_MAC_TX_FCBS                 8
-#define NUM_MAC_TX_BDBS                 NUM_MAC_TX_FCBS
-#define NUM_MAC_RX_FCBS                 7
-#define NUM_MAC_RX_BDBS                 8
-
-#define NUM_NON_MAC_TX_FCBS             6
-#define NUM_NON_MAC_TX_BDBS             NUM_NON_MAC_TX_FCBS
-
-#define NUM_NON_MAC_RX_BDBS             0       /* CALCULATED DYNAMICALLY */
-
-#define NUM_BUG_TX_FCBS                 8
-#define NUM_BUG_TX_BDBS                 NUM_BUG_TX_FCBS
-
-#define MAC_TX_BUFFER_MEMORY            1024
-#define NON_MAC_TX_BUFFER_MEMORY        (20 * 1024)
-#define BUG_TX_BUFFER_MEMORY            (NUM_BUG_TX_FCBS * 32)
-
-#define RX_BUFFER_MEMORY                0       /* CALCULATED DYNAMICALLY */
-#define RX_DATA_BUFFER_SIZE             256
-#define RX_BDB_SIZE_SHIFT               3       /* log2(RX_DATA_BUFFER_SIZE)-log2(sizeof(BDBlock)) */
-#define RX_BDB_SIZE_MASK                (sizeof(BDBlock) - 1)
-#define RX_DATA_BUFFER_SIZE_MASK        (RX_DATA_BUFFER_SIZE-1)
-
-#define NUM_OF_INTERRUPTS               0x20
-
-#define NOT_TRANSMITING                 0
-#define TRANSMITING			1
-
-#define TRC_INTERRUPT_ENABLE_MASK       0x7FF6
-
-#define UCODE_VERSION                   0x58
-
-#define UCODE_SIZE_OFFSET               0x0000  /* WORD */
-#define UCODE_CHECKSUM_OFFSET           0x0002  /* WORD */
-#define UCODE_VERSION_OFFSET            0x0004  /* BYTE */
-
-#define CS_RAM_SIZE                     0X2000
-#define CS_RAM_CHECKSUM_OFFSET          0x1FFE  /* WORD 1FFE(MSB)-1FFF(LSB)*/
-#define CS_RAM_VERSION_OFFSET           0x1FFC  /* WORD 1FFC(MSB)-1FFD(LSB)*/
-
-#define MISC_DATA_SIZE                  128
-#define NUM_OF_ACBS                     1
-
-#define ACB_COMMAND_NOT_DONE            0x0000  /* Init, command not done */
-#define ACB_COMMAND_DONE                0x8000  /* TRC says command done */
-#define ACB_COMMAND_STATUS_MASK         0x00FF  /* low byte is status */
-#define ACB_COMMAND_SUCCESSFUL          0x0000  /* means cmd was successful */
-#define ACB_NOT_CHAIN_END               0x0000  /* tell TRC more CBs in chain */
-#define ACB_CHAIN_END                   0x8000  /* tell TRC last CB in chain */
-#define ACB_COMMAND_NO_INTERRUPT        0x0000  /* tell TRC no INT after CB */
-#define ACB_COMMAND_INTERRUPT           0x2000  /* tell TRC to INT after CB */
-#define ACB_SUB_CMD_NOP                 0x0000
-#define ACB_CMD_HIC_NOP                 0x0080
-#define ACB_CMD_MCT_NOP                 0x0000
-#define ACB_CMD_MCT_TEST                0x0001
-#define ACB_CMD_HIC_TEST                0x0081
-#define ACB_CMD_INSERT                  0x0002
-#define ACB_CMD_REMOVE                  0x0003
-#define ACB_CMD_MCT_WRITE_VALUE         0x0004
-#define ACB_CMD_HIC_WRITE_VALUE         0x0084
-#define ACB_CMD_MCT_READ_VALUE          0x0005
-#define ACB_CMD_HIC_READ_VALUE          0x0085
-#define ACB_CMD_INIT_TX_RX              0x0086
-#define ACB_CMD_INIT_TRC_TIMERS         0x0006
-#define ACB_CMD_READ_TRC_STATUS         0x0007
-#define ACB_CMD_CHANGE_JOIN_STATE       0x0008
-#define ACB_CMD_RESERVED_9              0x0009
-#define ACB_CMD_RESERVED_A              0x000A
-#define ACB_CMD_RESERVED_B              0x000B
-#define ACB_CMD_RESERVED_C              0x000C
-#define ACB_CMD_RESERVED_D              0x000D
-#define ACB_CMD_RESERVED_E              0x000E
-#define ACB_CMD_RESERVED_F              0x000F
-
-#define TRC_MAC_REGISTERS_TEST          0x0000
-#define TRC_INTERNAL_LOOPBACK           0x0001
-#define TRC_TRI_LOOPBACK                0x0002
-#define TRC_INTERNAL_ROM_TEST           0x0003
-#define TRC_LOBE_MEDIA_TEST             0x0004
-#define TRC_ANALOG_TEST                 0x0005
-#define TRC_HOST_INTERFACE_REG_TEST     0x0003
-
-#define TEST_DMA_1                      0x0000
-#define TEST_DMA_2                      0x0001
-#define TEST_MCT_ROM                    0x0002
-#define HIC_INTERNAL_DIAG               0x0003
-
-#define ABORT_TRANSMIT_PRIORITY_0       0x0001
-#define ABORT_TRANSMIT_PRIORITY_1       0x0002
-#define ABORT_TRANSMIT_PRIORITY_2       0x0004
-#define ABORT_TRANSMIT_PRIORITY_3       0x0008
-#define ABORT_TRANSMIT_PRIORITY_4       0x0010
-#define ABORT_TRANSMIT_PRIORITY_5       0x0020
-#define ABORT_TRANSMIT_PRIORITY_6       0x0040
-#define ABORT_TRANSMIT_PRIORITY_7       0x0080
-
-#define TX_PENDING_PRIORITY_0           0x0001
-#define TX_PENDING_PRIORITY_1           0x0002
-#define TX_PENDING_PRIORITY_2           0x0004
-#define TX_PENDING_PRIORITY_3           0x0008
-#define TX_PENDING_PRIORITY_4           0x0010
-#define TX_PENDING_PRIORITY_5           0x0020
-#define TX_PENDING_PRIORITY_6           0x0040
-#define TX_PENDING_PRIORITY_7           0x0080
-
-#define FCB_FRAME_LENGTH                0x100
-#define FCB_COMMAND_DONE                0x8000  /* FCB Word 0 */
-#define FCB_NOT_CHAIN_END               0x0000  /* FCB Word 1 */
-#define FCB_CHAIN_END                   0x8000
-#define FCB_NO_WARNING                  0x0000
-#define FCB_WARNING                     0x4000
-#define FCB_INTERRUPT_DISABLE           0x0000
-#define FCB_INTERRUPT_ENABLE            0x2000
-
-#define FCB_ENABLE_IMA                  0x0008
-#define FCB_ENABLE_TES                  0x0004  /* Guarantee Tx before Int */
-#define FCB_ENABLE_TFS                  0x0002  /* Post Tx Frame Status */
-#define FCB_ENABLE_NTC                  0x0001  /* No Tx CRC */
-
-#define FCB_TX_STATUS_CR2               0x0004
-#define FCB_TX_STATUS_AR2               0x0008
-#define FCB_TX_STATUS_CR1               0x0040
-#define FCB_TX_STATUS_AR1               0x0080
-#define FCB_TX_AC_BITS                  (FCB_TX_STATUS_AR1+FCB_TX_STATUS_AR2+FCB_TX_STATUS_CR1+FCB_TX_STATUS_CR2)
-#define FCB_TX_STATUS_E                 0x0100
-
-#define FCB_RX_STATUS_ANY_ERROR         0x0001
-#define FCB_RX_STATUS_FCS_ERROR         0x0002
-
-#define FCB_RX_STATUS_IA_MATCHED        0x0400
-#define FCB_RX_STATUS_IGA_BSGA_MATCHED  0x0500
-#define FCB_RX_STATUS_FA_MATCHED        0x0600
-#define FCB_RX_STATUS_BA_MATCHED        0x0700
-#define FCB_RX_STATUS_DA_MATCHED        0x0400
-#define FCB_RX_STATUS_SOURCE_ROUTING    0x0800
-
-#define BDB_BUFFER_SIZE                 0x100
-#define BDB_NOT_CHAIN_END               0x0000
-#define BDB_CHAIN_END                   0x8000
-#define BDB_NO_WARNING                  0x0000
-#define BDB_WARNING                     0x4000
-
-#define ERROR_COUNTERS_CHANGED          0x0001
-#define TI_NDIS_RING_STATUS_CHANGED     0x0002
-#define UNA_CHANGED                     0x0004
-#define READY_TO_SEND_RQ_INIT           0x0008
-
-#define SCGB_ADDRESS_POINTER_FORMAT     INTEL_ADDRESS_POINTER_FORMAT
-#define SCGB_DATA_FORMAT                INTEL_DATA_FORMAT
-#define SCGB_MULTI_WORD_CONTROL         0
-#define SCGB_BURST_LENGTH               0x000E  /* DMA Burst Length */
-
-#define SCGB_CONFIG                     (INTEL_ADDRESS_POINTER_FORMAT+INTEL_DATA_FORMAT+SCGB_BURST_LENGTH)
-
-#define ISCP_BLOCK_SIZE                 0x0A
-#define RAM_SIZE                        0x10000
-#define INIT_SYS_CONFIG_PTR_OFFSET      (RAM_SIZE-ISCP_BLOCK_SIZE)
-#define SCGP_BLOCK_OFFSET               0
-
-#define SCLB_NOT_VALID                  0x0000  /* Initially, SCLB not valid */
-#define SCLB_VALID                      0x8000  /* Host tells TRC SCLB valid */
-#define SCLB_PROCESSED                  0x0000  /* TRC says SCLB processed */
-#define SCLB_RESUME_CONTROL_NOT_VALID   0x0000  /* Initially, RC not valid */
-#define SCLB_RESUME_CONTROL_VALID       0x4000  /* Host tells TRC RC valid */
-#define SCLB_IACK_CODE_NOT_VALID        0x0000  /* Initially, IACK not valid */
-#define SCLB_IACK_CODE_VALID            0x2000  /* Host tells TRC IACK valid */
-#define SCLB_CMD_NOP                    0x0000
-#define SCLB_CMD_REMOVE                 0x0001
-#define SCLB_CMD_SUSPEND_ACB_CHAIN      0x0002
-#define SCLB_CMD_SET_INTERRUPT_MASK     0x0003
-#define SCLB_CMD_CLEAR_INTERRUPT_MASK   0x0004
-#define SCLB_CMD_RESERVED_5             0x0005
-#define SCLB_CMD_RESERVED_6             0x0006
-#define SCLB_CMD_RESERVED_7             0x0007
-#define SCLB_CMD_RESERVED_8             0x0008
-#define SCLB_CMD_RESERVED_9             0x0009
-#define SCLB_CMD_RESERVED_A             0x000A
-#define SCLB_CMD_RESERVED_B             0x000B
-#define SCLB_CMD_RESERVED_C             0x000C
-#define SCLB_CMD_RESERVED_D             0x000D
-#define SCLB_CMD_RESERVED_E             0x000E
-#define SCLB_CMD_RESERVED_F             0x000F
-
-#define SCLB_RC_ACB                     0x0001  /* Action Command Block Chain */
-#define SCLB_RC_RES0                    0x0002  /* Always Zero */
-#define SCLB_RC_RES1                    0x0004  /* Always Zero */
-#define SCLB_RC_RES2                    0x0008  /* Always Zero */
-#define SCLB_RC_RX_MAC_FCB              0x0010  /* RX_MAC_FCB Chain */
-#define SCLB_RC_RX_MAC_BDB              0x0020  /* RX_MAC_BDB Chain */
-#define SCLB_RC_RX_NON_MAC_FCB          0x0040  /* RX_NON_MAC_FCB Chain */
-#define SCLB_RC_RX_NON_MAC_BDB          0x0080  /* RX_NON_MAC_BDB Chain */
-#define SCLB_RC_TFCB0                   0x0100  /* TX Priority 0 FCB Chain */
-#define SCLB_RC_TFCB1                   0x0200  /* TX Priority 1 FCB Chain */
-#define SCLB_RC_TFCB2                   0x0400  /* TX Priority 2 FCB Chain */
-#define SCLB_RC_TFCB3                   0x0800  /* TX Priority 3 FCB Chain */
-#define SCLB_RC_TFCB4                   0x1000  /* TX Priority 4 FCB Chain */
-#define SCLB_RC_TFCB5                   0x2000  /* TX Priority 5 FCB Chain */
-#define SCLB_RC_TFCB6                   0x4000  /* TX Priority 6 FCB Chain */
-#define SCLB_RC_TFCB7                   0x8000  /* TX Priority 7 FCB Chain */
-
-#define SCLB_IMC_RES0                   0x0001  /* */
-#define SCLB_IMC_MAC_TYPE_3             0x0002  /* MAC_ARC_INDICATE */
-#define SCLB_IMC_MAC_ERROR_COUNTERS     0x0004  /* */
-#define SCLB_IMC_RES1                   0x0008  /* */
-#define SCLB_IMC_MAC_TYPE_2             0x0010  /* QUE_MAC_INDICATE */
-#define SCLB_IMC_TX_FRAME               0x0020  /* */
-#define SCLB_IMC_END_OF_TX_QUEUE        0x0040  /* */
-#define SCLB_IMC_NON_MAC_RX_RESOURCE    0x0080  /* */
-#define SCLB_IMC_MAC_RX_RESOURCE        0x0100  /* */
-#define SCLB_IMC_NON_MAC_RX_FRAME       0x0200  /* */
-#define SCLB_IMC_MAC_RX_FRAME           0x0400  /* */
-#define SCLB_IMC_TRC_FIFO_STATUS        0x0800  /* */
-#define SCLB_IMC_COMMAND_STATUS         0x1000  /* */
-#define SCLB_IMC_MAC_TYPE_1             0x2000  /* Self Removed */
-#define SCLB_IMC_TRC_INTRNL_TST_STATUS  0x4000  /* */
-#define SCLB_IMC_RES2                   0x8000  /* */
-
-#define DMA_TRIGGER                     0x0004
-#define FREQ_16MB_BIT                   0x0010
-#define THDREN                          0x0020
-#define CFG0_RSV1                       0x0040
-#define CFG0_RSV2                       0x0080
-#define ETREN                           0x0100
-#define RX_OWN_BIT                      0x0200
-#define RXATMAC                         0x0400
-#define PROMISCUOUS_BIT                 0x0800
-#define USETPT                          0x1000
-#define SAVBAD_BIT                      0x2000
-#define ONEQUE                          0x4000
-#define NO_AUTOREMOVE                   0x8000
-
-#define RX_FCB_AREA_8316        0x00000000
-#define RX_BUFF_AREA_8316       0x00000000
-
-#define TRC_POINTER(X)          ((unsigned long)(X) - tp->ram_access)
-#define RX_FCB_TRC_POINTER(X)   ((unsigned long)(X) - tp->ram_access + RX_FCB_AREA_8316)
-#define RX_BUFF_TRC_POINTER(X) ((unsigned long)(X) - tp->ram_access + RX_BUFF_AREA_8316)
-
-// Offset 0: MSR - Memory Select Register
-//
-#define r587_MSR        0x000   // Register Offset
-//#define       MSR_RST         0x080   // LAN Controller Reset
-#define MSR_MENB        0x040   // Shared Memory Enable
-#define MSR_RA18        0x020   // Ram Address bit 18   (583, 584, 587)
-#define MSR_RA17        0x010   // Ram Address bit 17   (583, 584, 585/790)
-#define MSR_RA16        0x008   // Ram Address bit 16   (583, 584, 585/790)
-#define MSR_RA15        0x004   // Ram Address bit 15   (583, 584, 585/790)
-#define MSR_RA14        0x002   // Ram Address bit 14   (583, 584, 585/790)
-#define MSR_RA13        0x001   // Ram Address bit 13   (583, 584, 585/790)
-
-#define MSR_MASK        0x03F   // Mask for Address bits RA18-RA13 (583, 584, 587)
-
-#define MSR                     0x00
-#define IRR                     0x04
-#define HWR                     0x04
-#define LAAR                    0x05
-#define IMCCR                   0x05
-#define LAR0                    0x08
-#define BDID                    0x0E    // Adapter ID byte register offset
-#define CSR                     0x10
-#define PR                      0x11
-
-#define MSR_RST                 0x80
-#define MSR_MEMB                0x40
-#define MSR_0WS                 0x20
-
-#define FORCED_16BIT_MODE       0x0002
-
-#define INTERFRAME_SPACING_16           0x0003  /* 6 bytes */
-#define INTERFRAME_SPACING_4            0x0001  /* 2 bytes */
-#define MULTICAST_ADDRESS_BIT           0x0010
-#define NON_SRC_ROUTING_BIT             0x0020
-
-#define LOOPING_MODE_MASK       0x0007
-
-/*
- * Decode firmware defines.
- */
-#define SWAP_BYTES(X)		((X & 0xff) << 8) | (X >> 8)
-#define WEIGHT_OFFSET		5
-#define TREE_SIZE_OFFSET	9
-#define TREE_OFFSET		11
-
-/* The Huffman Encoding Tree is constructed of these nodes. */
-typedef struct {
-	__u8	llink;	/* Short version of above node. */
-	__u8	tag;
-	__u8	info;	/* This node is used on decodes. */
-	__u8	rlink;
-} DECODE_TREE_NODE;
-
-#define ROOT	0	/* Branch value. */
-#define LEAF	0	/* Tag field value. */
-#define BRANCH	1	/* Tag field value. */
-
-/*
- * Multicast Table Structure
- */
-typedef struct {
-        __u8    address[6];
-        __u8    instance_count;
-} McTable;
-
-/*
- * Fragment Descriptor Definition
- */
-typedef struct {
-        __u8  *fragment_ptr;
-        __u32   fragment_length;
-} FragmentStructure;
-
-/*
- * Data Buffer Structure Definition
- */
-typedef struct {
-        __u32 fragment_count;
-        FragmentStructure       fragment_list[MAXFRAGMENTS];
-} DataBufferStructure;
-
-#pragma pack(1)
-typedef struct {
-                __u8    IType;
-                __u8    ISubtype;
-} Interrupt_Status_Word;
-
-#pragma pack(1)
-typedef struct BDBlockType {
-                __u16                   info;                   /* 02 */
-                __u32                   trc_next_ptr;           /* 06 */
-                __u32                   trc_data_block_ptr;     /* 10 */
-                __u16                   buffer_length;          /* 12 */
-
-                __u16                   *data_block_ptr;        /* 16 */
-                struct  BDBlockType     *next_ptr;              /* 20 */
-                struct  BDBlockType     *back_ptr;              /* 24 */
-                __u8                    filler[8];              /* 32 */
-} BDBlock;
-
-#pragma pack(1)
-typedef struct FCBlockType {
-                __u16                   frame_status;           /* 02 */
-                __u16                   info;                   /* 04 */
-                __u32                   trc_next_ptr;           /* 08 */
-                __u32                   trc_bdb_ptr;            /* 12 */
-                __u16                   frame_length;           /* 14 */
-
-                BDBlock                 *bdb_ptr;               /* 18 */
-                struct  FCBlockType     *next_ptr;              /* 22 */
-                struct  FCBlockType     *back_ptr;              /* 26 */
-                __u16                   memory_alloc;           /* 28 */
-                __u8                    filler[4];              /* 32 */
-
-} FCBlock;
-
-#pragma pack(1)
-typedef struct SBlockType{
-                __u8                           Internal_Error_Count;
-                __u8                           Line_Error_Count;
-                __u8                           AC_Error_Count;
-                __u8                           Burst_Error_Count;
-                __u8                            RESERVED_COUNTER_0;
-                __u8                            AD_TRANS_Count;
-                __u8                            RCV_Congestion_Count;
-                __u8                            Lost_FR_Error_Count;
-                __u8                            FREQ_Error_Count;
-                __u8                            FR_Copied_Error_Count;
-                __u8                            RESERVED_COUNTER_1;
-                __u8                            Token_Error_Count;
-
-                __u16                           TI_NDIS_Ring_Status;
-                __u16                           BCN_Type;
-                __u16                           Error_Code;
-                __u16                           SA_of_Last_AMP_SMP[3];
-                __u16                           UNA[3];
-                __u16                           Ucode_Version_Number;
-                __u16                           Status_CHG_Indicate;
-                __u16                           RESERVED_STATUS_0;
-} SBlock;
-
-#pragma pack(1)
-typedef struct ACBlockType {
-                __u16                   cmd_done_status;    /* 02 */
-                __u16                   cmd_info;           /* 04 */
-                __u32                   trc_next_ptr;           /* 08 */
-                __u16                   cmd;                /* 10 */
-                __u16                   subcmd;             /* 12 */
-                __u16                   data_offset_lo;         /* 14 */
-                __u16                   data_offset_hi;         /* 16 */
-
-                struct  ACBlockType     *next_ptr;              /* 20 */
-
-                __u8                    filler[12];             /* 32 */
-} ACBlock;
-
-#define NUM_OF_INTERRUPTS               0x20
-
-#pragma pack(1)
-typedef struct {
-                Interrupt_Status_Word   IStatus[NUM_OF_INTERRUPTS];
-} ISBlock;
-
-#pragma pack(1)
-typedef struct {
-                __u16                   valid_command;          /* 02 */
-                __u16                   iack_code;              /* 04 */
-                __u16                   resume_control;         /* 06 */
-                __u16                   int_mask_control;       /* 08 */
-                __u16                   int_mask_state;         /* 10 */
-
-                __u8                    filler[6];              /* 16 */
-} SCLBlock;
-
-#pragma pack(1)
-typedef struct
-{
-                __u16                   config;                 /* 02 */
-                __u32                   trc_sclb_ptr;           /* 06 */
-                __u32                   trc_acb_ptr;            /* 10 */
-                __u32                   trc_isb_ptr;            /* 14 */
-                __u16                   isbsiz;                 /* 16 */
-
-                SCLBlock                *sclb_ptr;              /* 20 */
-                ACBlock                 *acb_ptr;               /* 24 */
-                ISBlock                 *isb_ptr;               /* 28 */
-
-                __u16                   Non_Mac_Rx_Bdbs;        /* 30 DEBUG */
-                __u8                    filler[2];              /* 32 */
-
-} SCGBlock;
-
-#pragma pack(1)
-typedef struct
-{
-	__u32		trc_scgb_ptr;
-	SCGBlock	*scgb_ptr;
-} ISCPBlock;
-#pragma pack()
-
-typedef struct net_local {
-	ISCPBlock       *iscpb_ptr;
-        SCGBlock        *scgb_ptr;
-        SCLBlock        *sclb_ptr;
-        ISBlock         *isb_ptr;
-
-	ACBlock         *acb_head;
-        ACBlock         *acb_curr;
-        ACBlock         *acb_next;
-
-	__u8		adapter_name[12];
-
-	__u16		num_rx_bdbs	[NUM_RX_QS_USED];
-	__u16		num_rx_fcbs	[NUM_RX_QS_USED];
-
-	__u16		num_tx_bdbs	[NUM_TX_QS_USED];
-	__u16		num_tx_fcbs	[NUM_TX_QS_USED];
-
-	__u16		num_of_tx_buffs;
-
-	__u16		tx_buff_size	[NUM_TX_QS_USED];
-	__u16		tx_buff_used	[NUM_TX_QS_USED];
-	__u16		tx_queue_status	[NUM_TX_QS_USED];
-
-	FCBlock		*tx_fcb_head[NUM_TX_QS_USED];
-	FCBlock		*tx_fcb_curr[NUM_TX_QS_USED];
-	FCBlock		*tx_fcb_end[NUM_TX_QS_USED];
-	BDBlock		*tx_bdb_head[NUM_TX_QS_USED];
-	__u16		*tx_buff_head[NUM_TX_QS_USED];
-	__u16		*tx_buff_end[NUM_TX_QS_USED];
-	__u16		*tx_buff_curr[NUM_TX_QS_USED];
-	__u16		num_tx_fcbs_used[NUM_TX_QS_USED];
-
-	FCBlock		*rx_fcb_head[NUM_RX_QS_USED];
-	FCBlock		*rx_fcb_curr[NUM_RX_QS_USED];
-	BDBlock		*rx_bdb_head[NUM_RX_QS_USED];
-	BDBlock		*rx_bdb_curr[NUM_RX_QS_USED];
-	BDBlock		*rx_bdb_end[NUM_RX_QS_USED];
-	__u16		*rx_buff_head[NUM_RX_QS_USED];
-	__u16		*rx_buff_end[NUM_RX_QS_USED];
-
-	__u32		*ptr_local_ring_num;
-
-	__u32		sh_mem_used;
-
-	__u16		page_offset_mask;
-
-	__u16		authorized_function_classes;
-	__u16		authorized_access_priority;
-
-        __u16            num_acbs;
-        __u16            num_acbs_used;
-        __u16            acb_pending;
-
-	__u16		current_isb_index;
-
-	__u8            monitor_state;
-	__u8		monitor_state_ready;
-	__u16		ring_status;
-	__u8		ring_status_flags;
-	__u8		state;
-
-	__u8		join_state;
-
-	__u8		slot_num;
-	__u16		pos_id;
-
-	__u32		*ptr_una;
-	__u32		*ptr_bcn_type;
-	__u32		*ptr_tx_fifo_underruns;
-	__u32		*ptr_rx_fifo_underruns;
-	__u32		*ptr_rx_fifo_overruns;
-	__u32		*ptr_tx_fifo_overruns;
-	__u32		*ptr_tx_fcb_overruns;
-	__u32		*ptr_rx_fcb_overruns;
-	__u32		*ptr_tx_bdb_overruns;
-	__u32		*ptr_rx_bdb_overruns;
-
-	__u16		receive_queue_number;
-
-	__u8		rx_fifo_overrun_count;
-	__u8		tx_fifo_overrun_count;
-
-	__u16            adapter_flags;
-	__u16		adapter_flags1;
-	__u16            *misc_command_data;
-	__u16            max_packet_size;
-
-	__u16            config_word0;
-        __u16            config_word1;
-
-	__u8            trc_mask;
-
-	__u16            source_ring_number;
-        __u16            target_ring_number;
-
-	__u16		microcode_version;
-
-	__u16            bic_type;
-        __u16            nic_type;
-        __u16            board_id;
-
-	__u16            rom_size;
-	__u32		rom_base;
-        __u16            ram_size;
-        __u16            ram_usable;
-	__u32		ram_base;
-	__u32		ram_access;
-
-	__u16            extra_info;
-        __u16            mode_bits;
-	__u16		media_menu;
-	__u16		media_type;
-	__u16		adapter_bus;
-
-	__u16		status;
-	__u16            receive_mask;
-
-	__u16            group_address_0;
-        __u16            group_address[2];
-        __u16            functional_address_0;
-        __u16            functional_address[2];
-        __u16            bitwise_group_address[2];
-
-	__u8		cleanup;
-
-	struct sk_buff_head SendSkbQueue;
-        __u16 QueueSkb;
-
-	struct tr_statistics MacStat;   /* MAC statistics structure */
-	
-	spinlock_t	lock;
-} NET_LOCAL;
-
-/************************************
- * SNMP-ON-BOARD Agent Link Structure
- ************************************/
-
-typedef struct {
-        __u8           LnkSigStr[12]; /* signature string "SmcLinkTable" */
-        __u8           LnkDrvTyp;     /* 1=Redbox ODI, 2=ODI DOS, 3=ODI OS/2, 4=NDIS DOS */
-        __u8           LnkFlg;        /* 0 if no agent linked, 1 if agent linked */
-        void           *LnkNfo;       /* routine which returns pointer to NIC info */
-        void           *LnkAgtRcv;    /* pointer to agent receive trap entry */
-        void           *LnkAgtXmt;            /* pointer to agent transmit trap
-entry  */
-void           *LnkGet;                  /* pointer to NIC receive data
-copy routine */
-        void           *LnkSnd;                  /* pointer to NIC send routine
-*/
-        void           *LnkRst;                  /* pointer to NIC driver reset
-routine */
-        void           *LnkMib;                  /* pointer to MIB data base */
-        void           *LnkMibAct;            /* pointer to MIB action routine list */
-        __u16           LnkCntOffset;  /* offset to error counters */
-        __u16           LnkCntNum;     /* number of error counters */
-        __u16           LnkCntSize;    /* size of error counters i.e. 32 = 32 bits */
-        void           *LnkISR;       /* pointer to interrupt vector */
-        __u8           LnkFrmTyp;     /* 1=Ethernet, 2=Token Ring */
-        __u8           LnkDrvVer1 ;   /* driver major version */
-        __u8           LnkDrvVer2 ;   /* driver minor version */
-} AgentLink;
-
-/*
- * Definitions for pcm_card_flags(bit_mapped)
- */
-#define REG_COMPLETE   0x0001
-#define INSERTED       0x0002
-#define PCC_INSERTED   0x0004         /* 1=currently inserted, 0=cur removed */
-
-/*
- * Adapter RAM test patterns
- */
-#define RAM_PATTERN_1  0x55AA
-#define RAM_PATTERN_2  0x9249
-#define RAM_PATTERN_3  0xDB6D
-
-/*
- * definitions for RAM test
- */
-#define ROM_SIGNATURE  0xAA55
-#define MIN_ROM_SIZE   0x2000
-
-/*
- * Return Codes
- */
-#define SUCCESS                 0x0000
-#define ADAPTER_AND_CONFIG      0x0001
-#define ADAPTER_NO_CONFIG       0x0002
-#define NOT_MY_INTERRUPT        0x0003
-#define FRAME_REJECTED          0x0004
-#define EVENTS_DISABLED         0x0005
-#define OUT_OF_RESOURCES        0x0006
-#define INVALID_PARAMETER       0x0007
-#define INVALID_FUNCTION        0x0008
-#define INITIALIZE_FAILED       0x0009
-#define CLOSE_FAILED            0x000A
-#define MAX_COLLISIONS          0x000B
-#define NO_SUCH_DESTINATION     0x000C
-#define BUFFER_TOO_SMALL_ERROR  0x000D
-#define ADAPTER_CLOSED          0x000E
-#define UCODE_NOT_PRESENT       0x000F
-#define FIFO_UNDERRUN           0x0010
-#define DEST_OUT_OF_RESOURCES   0x0011
-#define ADAPTER_NOT_INITIALIZED 0x0012
-#define PENDING                 0x0013
-#define UCODE_PRESENT           0x0014
-#define NOT_INIT_BY_BRIDGE      0x0015
-
-#define OPEN_FAILED             0x0080
-#define HARDWARE_FAILED         0x0081
-#define SELF_TEST_FAILED        0x0082
-#define RAM_TEST_FAILED         0x0083
-#define RAM_CONFLICT            0x0084
-#define ROM_CONFLICT            0x0085
-#define UNKNOWN_ADAPTER         0x0086
-#define CONFIG_ERROR            0x0087
-#define CONFIG_WARNING          0x0088
-#define NO_FIXED_CNFG           0x0089
-#define EEROM_CKSUM_ERROR       0x008A
-#define ROM_SIGNATURE_ERROR     0x008B
-#define ROM_CHECKSUM_ERROR      0x008C
-#define ROM_SIZE_ERROR          0x008D
-#define UNSUPPORTED_NIC_CHIP    0x008E
-#define NIC_REG_ERROR           0x008F
-#define BIC_REG_ERROR           0x0090
-#define MICROCODE_TEST_ERROR    0x0091
-#define LOBE_MEDIA_TEST_FAILED  0x0092
-
-#define ADAPTER_FOUND_LAN_CORRUPT 0x009B
-
-#define ADAPTER_NOT_FOUND       0xFFFF
-
-#define ILLEGAL_FUNCTION        INVALID_FUNCTION
-
-/* Errors */
-#define IO_BASE_INVALID         0x0001
-#define IO_BASE_RANGE           0x0002
-#define IRQ_INVALID             0x0004
-#define IRQ_RANGE               0x0008
-#define RAM_BASE_INVALID        0x0010
-#define RAM_BASE_RANGE          0x0020
-#define RAM_SIZE_RANGE          0x0040
-#define MEDIA_INVALID           0x0800
-
-/* Warnings */
-#define IRQ_MISMATCH            0x0080
-#define RAM_BASE_MISMATCH       0x0100
-#define RAM_SIZE_MISMATCH       0x0200
-#define BUS_MODE_MISMATCH       0x0400
-
-#define RX_CRC_ERROR                            0x01
-#define RX_ALIGNMENT_ERROR              0x02
-#define RX_HW_FAILED                            0x80
-
-/*
- * Definitions for the field RING_STATUS_FLAGS
- */
-#define RING_STATUS_CHANGED                     0X01
-#define MONITOR_STATE_CHANGED                   0X02
-#define JOIN_STATE_CHANGED                      0X04
-
-/*
- * Definitions for the field JOIN_STATE
- */
-#define JS_BYPASS_STATE                         0x00
-#define JS_LOBE_TEST_STATE                      0x01
-#define JS_DETECT_MONITOR_PRESENT_STATE         0x02
-#define JS_AWAIT_NEW_MONITOR_STATE              0x03
-#define JS_DUPLICATE_ADDRESS_TEST_STATE         0x04
-#define JS_NEIGHBOR_NOTIFICATION_STATE          0x05
-#define JS_REQUEST_INITIALIZATION_STATE         0x06
-#define JS_JOIN_COMPLETE_STATE                  0x07
-#define JS_BYPASS_WAIT_STATE                    0x08
-
-/*
- * Definitions for the field MONITOR_STATE
- */
-#define MS_MONITOR_FSM_INACTIVE                 0x00
-#define MS_REPEAT_BEACON_STATE                  0x01
-#define MS_REPEAT_CLAIM_TOKEN_STATE             0x02
-#define MS_TRANSMIT_CLAIM_TOKEN_STATE           0x03
-#define MS_STANDBY_MONITOR_STATE                0x04
-#define MS_TRANSMIT_BEACON_STATE                0x05
-#define MS_ACTIVE_MONITOR_STATE                 0x06
-#define MS_TRANSMIT_RING_PURGE_STATE            0x07
-#define MS_BEACON_TEST_STATE                    0x09
-
-/*
- * Definitions for the bit-field RING_STATUS
- */
-#define SIGNAL_LOSS                             0x8000
-#define HARD_ERROR                              0x4000
-#define SOFT_ERROR                              0x2000
-#define TRANSMIT_BEACON                         0x1000
-#define LOBE_WIRE_FAULT                         0x0800
-#define AUTO_REMOVAL_ERROR                      0x0400
-#define REMOVE_RECEIVED                         0x0100
-#define COUNTER_OVERFLOW                        0x0080
-#define SINGLE_STATION                          0x0040
-#define RING_RECOVERY                           0x0020
-
-/*
- * Definitions for the field BUS_TYPE
- */
-#define AT_BUS                  0x00
-#define MCA_BUS                 0x01
-#define EISA_BUS                0x02
-#define PCI_BUS                 0x03
-#define PCMCIA_BUS              0x04
-
-/*
- * Definitions for adapter_flags
- */
-#define RX_VALID_LOOKAHEAD      0x0001
-#define FORCED_16BIT_MODE       0x0002
-#define ADAPTER_DISABLED        0x0004
-#define TRANSMIT_CHAIN_INT      0x0008
-#define EARLY_RX_FRAME          0x0010
-#define EARLY_TX                0x0020
-#define EARLY_RX_COPY           0x0040
-#define USES_PHYSICAL_ADDR      0x0080		/* Rsvd for DEC PCI and 9232 */
-#define NEEDS_PHYSICAL_ADDR  	0x0100       	/* Reserved*/
-#define RX_STATUS_PENDING       0x0200
-#define ERX_DISABLED         	0x0400       	/* EARLY_RX_ENABLE rcv_mask */
-#define ENABLE_TX_PENDING       0x0800
-#define ENABLE_RX_PENDING       0x1000
-#define PERM_CLOSE              0x2000  
-#define IO_MAPPED               0x4000  	/* IOmapped bus interface 795 */
-#define ETX_DISABLED            0x8000
-
-
-/*
- * Definitions for adapter_flags1
- */
-#define TX_PHY_RX_VIRT          0x0001 
-#define NEEDS_HOST_RAM          0x0002
-#define NEEDS_MEDIA_TYPE        0x0004
-#define EARLY_RX_DONE           0x0008
-#define PNP_BOOT_BIT            0x0010  /* activates PnP & config on power-up */
-                                        /* clear => regular PnP operation */
-#define PNP_ENABLE              0x0020  /* regular PnP operation clear => */
-                                        /* no PnP, overrides PNP_BOOT_BIT */
-#define SATURN_ENABLE           0x0040
-
-#define ADAPTER_REMOVABLE       0x0080 	/* adapter is hot swappable */
-#define TX_PHY                  0x0100  /* Uses physical address for tx bufs */
-#define RX_PHY                  0x0200  /* Uses physical address for rx bufs */
-#define TX_VIRT                 0x0400  /* Uses virtual addr for tx bufs */
-#define RX_VIRT                 0x0800 
-#define NEEDS_SERVICE           0x1000 
-
-/*
- * Adapter Status Codes
- */
-#define OPEN                    0x0001
-#define INITIALIZED             0x0002
-#define CLOSED                  0x0003
-#define FAILED                  0x0005
-#define NOT_INITIALIZED         0x0006
-#define IO_CONFLICT             0x0007
-#define CARD_REMOVED            0x0008
-#define CARD_INSERTED           0x0009
-
-/*
- * Mode Bit Definitions
- */
-#define INTERRUPT_STATUS_BIT    0x8000  /* PC Interrupt Line: 0 = Not Enabled */
-#define BOOT_STATUS_MASK        0x6000  /* Mask to isolate BOOT_STATUS */
-#define BOOT_INHIBIT            0x0000  /* BOOT_STATUS is 'inhibited' */
-#define BOOT_TYPE_1             0x2000  /* Unused BOOT_STATUS value */
-#define BOOT_TYPE_2             0x4000  /* Unused BOOT_STATUS value */
-#define BOOT_TYPE_3             0x6000  /* Unused BOOT_STATUS value */
-#define ZERO_WAIT_STATE_MASK    0x1800  /* Mask to isolate Wait State flags */
-#define ZERO_WAIT_STATE_8_BIT   0x1000  /* 0 = Disabled (Inserts Wait States) */
-#define ZERO_WAIT_STATE_16_BIT  0x0800  /* 0 = Disabled (Inserts Wait States) */
-#define LOOPING_MODE_MASK       0x0007
-#define LOOPBACK_MODE_0         0x0000
-#define LOOPBACK_MODE_1         0x0001
-#define LOOPBACK_MODE_2         0x0002
-#define LOOPBACK_MODE_3         0x0003
-#define LOOPBACK_MODE_4         0x0004
-#define LOOPBACK_MODE_5         0x0005
-#define LOOPBACK_MODE_6         0x0006
-#define LOOPBACK_MODE_7         0x0007
-#define AUTO_MEDIA_DETECT       0x0008
-#define MANUAL_CRC              0x0010
-#define EARLY_TOKEN_REL         0x0020  /* Early Token Release for Token Ring */
-#define UMAC               0x0040 
-#define UTP2_PORT               0x0080  /* For 8216T2, 0=port A, 1=Port B. */
-#define BNC_10BT_INTERFACE      0x0600  /* BNC and UTP current media set */
-#define UTP_INTERFACE           0x0500  /* Ethernet UTP Only. */
-#define BNC_INTERFACE           0x0400
-#define AUI_INTERFACE           0x0300
-#define AUI_10BT_INTERFACE      0x0200
-#define STARLAN_10_INTERFACE    0x0100
-#define INTERFACE_TYPE_MASK     0x0700
-
-/*
- * Media Type Bit Definitions
- *
- * legend:      TP = Twisted Pair
- *              STP = Shielded twisted pair
- *              UTP = Unshielded twisted pair
- */
-
-#define CNFG_MEDIA_TYPE_MASK    0x001e  /* POS Register 3 Mask         */
-
-#define MEDIA_S10               0x0000  /* Ethernet adapter, TP.        */
-#define MEDIA_AUI_UTP           0x0001  /* Ethernet adapter, AUI/UTP media */
-#define MEDIA_BNC               0x0002  /* Ethernet adapter, BNC media. */
-#define MEDIA_AUI               0x0003  /* Ethernet Adapter, AUI media. */
-#define MEDIA_STP_16            0x0004  /* TokenRing adap, 16Mbit STP.  */
-#define MEDIA_STP_4             0x0005  /* TokenRing adap, 4Mbit STP.   */
-#define MEDIA_UTP_16            0x0006  /* TokenRing adap, 16Mbit UTP.  */
-#define MEDIA_UTP_4             0x0007  /* TokenRing adap, 4Mbit UTP.   */
-#define MEDIA_UTP               0x0008  /* Ethernet adapter, UTP media (no AUI)
-*/
-#define MEDIA_BNC_UTP           0x0010  /* Ethernet adapter, BNC/UTP media */
-#define MEDIA_UTPFD             0x0011  /* Ethernet adapter, TP full duplex */
-#define MEDIA_UTPNL             0x0012  /* Ethernet adapter, TP with link integrity test disabled */
-#define MEDIA_AUI_BNC           0x0013  /* Ethernet adapter, AUI/BNC media */
-#define MEDIA_AUI_BNC_UTP       0x0014  /* Ethernet adapter, AUI_BNC/UTP */
-#define MEDIA_UTPA              0x0015  /* Ethernet UTP-10Mbps Ports A */
-#define MEDIA_UTPB              0x0016  /* Ethernet UTP-10Mbps Ports B */
-#define MEDIA_STP_16_UTP_16     0x0017  /* Token Ring STP-16Mbps/UTP-16Mbps */
-#define MEDIA_STP_4_UTP_4       0x0018  /* Token Ring STP-4Mbps/UTP-4Mbps */
-
-#define MEDIA_STP100_UTP100     0x0020  /* Ethernet STP-100Mbps/UTP-100Mbps */
-#define MEDIA_UTP100FD          0x0021  /* Ethernet UTP-100Mbps, full duplex */
-#define MEDIA_UTP100            0x0022  /* Ethernet UTP-100Mbps */
-
-
-#define MEDIA_UNKNOWN           0xFFFF  /* Unknown adapter/media type   */
-
-/*
- * Definitions for the field:
- * media_type2
- */
-#define MEDIA_TYPE_MII              0x0001
-#define MEDIA_TYPE_UTP              0x0002
-#define MEDIA_TYPE_BNC              0x0004
-#define MEDIA_TYPE_AUI              0x0008
-#define MEDIA_TYPE_S10              0x0010
-#define MEDIA_TYPE_AUTO_SENSE       0x1000
-#define MEDIA_TYPE_AUTO_DETECT      0x4000
-#define MEDIA_TYPE_AUTO_NEGOTIATE   0x8000
-
-/*
- * Definitions for the field:
- * line_speed
- */
-#define LINE_SPEED_UNKNOWN          0x0000
-#define LINE_SPEED_4                0x0001
-#define LINE_SPEED_10               0x0002
-#define LINE_SPEED_16               0x0004
-#define LINE_SPEED_100              0x0008
-#define LINE_SPEED_T4               0x0008  /* 100BaseT4 aliased for 9332BVT */
-#define LINE_SPEED_FULL_DUPLEX      0x8000
-
-/*
- * Definitions for the field:
- * bic_type (Bus interface chip type)
- */
-#define BIC_NO_CHIP             0x0000  /* Bus interface chip not implemented */
-#define BIC_583_CHIP            0x0001  /* 83C583 bus interface chip */
-#define BIC_584_CHIP            0x0002  /* 83C584 bus interface chip */
-#define BIC_585_CHIP            0x0003  /* 83C585 bus interface chip */
-#define BIC_593_CHIP            0x0004  /* 83C593 bus interface chip */
-#define BIC_594_CHIP            0x0005  /* 83C594 bus interface chip */
-#define BIC_564_CHIP            0x0006  /* PCMCIA Bus interface chip */
-#define BIC_790_CHIP            0x0007  /* 83C790 bus i-face/Ethernet NIC chip */
-#define BIC_571_CHIP            0x0008  /* 83C571 EISA bus master i-face */
-#define BIC_587_CHIP            0x0009  /* Token Ring AT bus master i-face */
-#define BIC_574_CHIP            0x0010  /* FEAST bus interface chip */
-#define BIC_8432_CHIP           0x0011  /* 8432 bus i-face/Ethernet NIC(DEC PCI) */
-#define BIC_9332_CHIP           0x0012  /* 9332 bus i-face/100Mbps Ether NIC(DEC PCI) */
-#define BIC_8432E_CHIP          0x0013  /* 8432 Enhanced bus iface/Ethernet NIC(DEC) */
-#define BIC_EPIC100_CHIP        0x0014  /* EPIC/100 10/100 Mbps Ethernet BIC/NIC */
-#define BIC_C94_CHIP            0x0015  /* 91C94 bus i-face in PCMCIA mode */
-#define BIC_X8020_CHIP          0x0016  /* Xilinx PCMCIA multi-func i-face */
-
-/*
- * Definitions for the field:
- * nic_type (Bus interface chip type)
- */
-#define NIC_UNK_CHIP            0x0000  /* Unknown NIC chip      */
-#define NIC_8390_CHIP           0x0001  /* DP8390 Ethernet NIC   */
-#define NIC_690_CHIP            0x0002  /* 83C690 Ethernet NIC   */
-#define NIC_825_CHIP            0x0003  /* 83C825 Token Ring NIC */
-/*      #define NIC_???_CHIP    0x0004  */ /* Not used           */
-/*      #define NIC_???_CHIP    0x0005  */ /* Not used           */
-/*      #define NIC_???_CHIP    0x0006  */ /* Not used           */
-#define NIC_790_CHIP            0x0007  /* 83C790 bus i-face/Ethernet NIC chip */
-#define NIC_C100_CHIP           0x0010  /* FEAST 100Mbps Ethernet NIC */
-#define NIC_8432_CHIP           0x0011  /* 8432 bus i-face/Ethernet NIC(DEC PCI) */
-#define NIC_9332_CHIP           0x0012  /* 9332 bus i-face/100Mbps Ether NIC(DEC PCI) */
-#define NIC_8432E_CHIP          0x0013  /* 8432 enhanced bus iface/Ethernet NIC(DEC) */
-#define NIC_EPIC100_CHIP        0x0014   /* EPIC/100 10/100 Mbps Ethernet BIC/NIC */
-#define NIC_C94_CHIP            0x0015  /* 91C94 PC Card with multi func */
-
-/*
- * Definitions for the field:
- * adapter_type The adapter_type field describes the adapter/bus
- *              configuration.
- */
-#define BUS_ISA16_TYPE          0x0001  /* 16 bit adap in 16 bit (E)ISA slot  */
-#define BUS_ISA8_TYPE           0x0002  /* 8/16b adap in 8 bit XT/(E)ISA slot */
-#define BUS_MCA_TYPE            0x0003  /* Micro Channel adapter              */
-
-/*
- * Receive Mask definitions
- */
-#define ACCEPT_MULTICAST                0x0001
-#define ACCEPT_BROADCAST                0x0002
-#define PROMISCUOUS_MODE                0x0004
-#define ACCEPT_SOURCE_ROUTING           0x0008
-#define ACCEPT_ERR_PACKETS              0x0010
-#define ACCEPT_ATT_MAC_FRAMES           0x0020
-#define ACCEPT_MULTI_PROM               0x0040
-#define TRANSMIT_ONLY                   0x0080
-#define ACCEPT_EXT_MAC_FRAMES           0x0100
-#define EARLY_RX_ENABLE                 0x0200
-#define PKT_SIZE_NOT_NEEDED             0x0400
-#define ACCEPT_SOURCE_ROUTING_SPANNING  0x0808
-
-#define ACCEPT_ALL_MAC_FRAMES           0x0120
-
-/*
- * config_mode defs
- */
-#define STORE_EEROM             0x0001  /* Store config in EEROM. */
-#define STORE_REGS              0x0002  /* Store config in register set. */
-
-/*
- * equates for lmac_flags in adapter structure (Ethernet)
- */
-#define         MEM_DISABLE     0x0001
-#define         RX_STATUS_POLL  0x0002
-#define         USE_RE_BIT      0x0004
-/*#define       RESERVED        0x0008 */
-/*#define       RESERVED        0x0010 */
-/*#define       RESERVED        0x0020 */
-/*#define       RESERVED        0x0040 */
-/*#define       RESERVED        0x0080 */
-/*#define       RESERVED        0x0100 */
-/*#define       RESERVED        0x0200 */
-/*#define       RESERVED        0x0400 */
-/*#define       RESERVED        0x0800 */
-/*#define       RESERVED        0x1000 */
-/*#define       RESERVED        0x2000 */
-/*#define       RESERVED        0x4000 */
-/*#define       RESERVED        0x8000 */
-
-/* media_opts & media_set Fields bit defs for Ethernet ... */
-#define         MED_OPT_BNC     0x01
-#define         MED_OPT_UTP     0x02
-#define         MED_OPT_AUI     0x04
-#define         MED_OPT_10MB    0x08
-#define         MED_OPT_100MB   0x10
-#define         MED_OPT_S10     0x20
-
-/* media_opts & media_set Fields bit defs for Token Ring ... */
-#define         MED_OPT_4MB     0x08
-#define         MED_OPT_16MB    0x10
-#define         MED_OPT_STP     0x40
-
-#define MAX_8023_SIZE           1500    /* Max 802.3 size of frame. */
-#define DEFAULT_ERX_VALUE       4       /* Number of 16-byte blocks for 790B early Rx. */
-#define DEFAULT_ETX_VALUE       32      /* Number of bytes for 790B early Tx. */
-#define DEFAULT_TX_RETRIES      3       /* Number of transmit retries */
-#define LPBK_FRAME_SIZE         1024    /* Default loopback frame for Rx calibration test. */
-#define MAX_LOOKAHEAD_SIZE      252     /* Max lookahead size for ethernet. */
-
-#define RW_MAC_STATE                    0x1101
-#define RW_SA_OF_LAST_AMP_OR_SMP        0x2803
-#define RW_PHYSICAL_DROP_NUMBER         0x3B02
-#define RW_UPSTREAM_NEIGHBOR_ADDRESS    0x3E03
-#define RW_PRODUCT_INSTANCE_ID          0x4B09
-
-#define RW_TRC_STATUS_BLOCK             0x5412
-
-#define RW_MAC_ERROR_COUNTERS_NO_CLEAR  0x8006
-#define RW_MAC_ERROR_COUNTER_CLEAR      0x7A06
-#define RW_CONFIG_REGISTER_0            0xA001
-#define RW_CONFIG_REGISTER_1            0xA101
-#define RW_PRESCALE_TIMER_THRESHOLD     0xA201
-#define RW_TPT_THRESHOLD                0xA301
-#define RW_TQP_THRESHOLD                0xA401
-#define RW_TNT_THRESHOLD                0xA501
-#define RW_TBT_THRESHOLD                0xA601
-#define RW_TSM_THRESHOLD                0xA701
-#define RW_TAM_THRESHOLD                0xA801
-#define RW_TBR_THRESHOLD                0xA901
-#define RW_TER_THRESHOLD                0xAA01
-#define RW_TGT_THRESHOLD                0xAB01
-#define RW_THT_THRESHOLD                0xAC01
-#define RW_TRR_THRESHOLD                0xAD01
-#define RW_TVX_THRESHOLD                0xAE01
-#define RW_INDIVIDUAL_MAC_ADDRESS       0xB003
-
-#define RW_INDIVIDUAL_GROUP_ADDRESS     0xB303  /* all of group addr */
-#define RW_INDIVIDUAL_GROUP_ADDR_WORD_0 0xB301  /* 1st word of group addr */
-#define RW_INDIVIDUAL_GROUP_ADDR        0xB402  /* 2nd-3rd word of group addr */
-#define RW_FUNCTIONAL_ADDRESS           0xB603  /* all of functional addr */
-#define RW_FUNCTIONAL_ADDR_WORD_0       0xB601  /* 1st word of func  addr */
-#define RW_FUNCTIONAL_ADDR              0xB702  /* 2nd-3rd word func addr */
-
-#define RW_BIT_SIGNIFICANT_GROUP_ADDR   0xB902
-#define RW_SOURCE_RING_BRIDGE_NUMBER    0xBB01
-#define RW_TARGET_RING_NUMBER           0xBC01
-
-#define RW_HIC_INTERRUPT_MASK           0xC601
-
-#define SOURCE_ROUTING_SPANNING_BITS    0x00C0  /* Spanning Tree Frames */
-#define SOURCE_ROUTING_EXPLORER_BIT     0x0040  /* Explorer and Single Route */
-
-        /* write */
-
-#define CSR_MSK_ALL             0x80    // Bic 587 Only
-#define CSR_MSKTINT             0x20
-#define CSR_MSKCBUSY            0x10
-#define CSR_CLRTINT             0x08
-#define CSR_CLRCBUSY            0x04
-#define CSR_WCSS                0x02
-#define CSR_CA                  0x01
-
-        /* read */
-
-#define CSR_TINT                0x20
-#define CSR_CINT                0x10
-#define CSR_TSTAT               0x08
-#define CSR_CSTAT               0x04
-#define CSR_FAULT               0x02
-#define CSR_CBUSY               0x01
-
-#define LAAR_MEM16ENB           0x80
-#define Zws16                   0x20
-
-#define IRR_IEN                 0x80
-#define Zws8                    0x01
-
-#define IMCCR_EIL               0x04
-
-typedef struct {
-        __u8            ac;                             /* Access Control */
-        __u8            fc;                             /* Frame Control */
-        __u8            da[6];                          /* Dest Addr */
-        __u8            sa[6];                          /* Source Addr */
-
-        __u16            vl;                             /* Vector Length */
-        __u8            dc_sc;                          /* Dest/Source Class */
-        __u8            vc;                             /* Vector Code */
-        } MAC_HEADER;
-
-#define MAX_SUB_VECTOR_INFO     (RX_DATA_BUFFER_SIZE - sizeof(MAC_HEADER) - 2)
-
-typedef struct
-        {
-        __u8            svl;                            /* Sub-vector Length */
-        __u8            svi;                            /* Sub-vector Code */
-        __u8            svv[MAX_SUB_VECTOR_INFO];       /* Sub-vector Info */
-        } MAC_SUB_VECTOR;
-
-#endif	/* __KERNEL__ */
-#endif	/* __LINUX_SMCTR_H */
diff --git a/drivers/net/tokenring/tms380tr.c b/drivers/net/tokenring/tms380tr.c
deleted file mode 100644
index be4813e0366..00000000000
--- a/drivers/net/tokenring/tms380tr.c
+++ /dev/null
@@ -1,2306 +0,0 @@
-/*
- *  tms380tr.c: A network driver library for Texas Instruments TMS380-based
- *              Token Ring Adapters.
- *
- *  Originally sktr.c: Written 1997 by Christoph Goos
- *
- *  A fine result of the Linux Systems Network Architecture Project.
- *  http://www.vanheusden.com/sna/ 
- *
- *  This software may be used and distributed according to the terms
- *  of the GNU General Public License, incorporated herein by reference.
- *
- *  The following modules are currently available for card support:
- *	- tmspci (Generic PCI card support)
- *	- abyss (Madge PCI support)
- *      - tmsisa (SysKonnect TR4/16 ISA)
- *
- *  Sources:
- *  	- The hardware related parts of this driver are take from
- *  	  the SysKonnect Token Ring driver for Windows NT.
- *  	- I used the IBM Token Ring driver 'ibmtr.c' as a base for this
- *  	  driver, as well as the 'skeleton.c' driver by Donald Becker.
- *  	- Also various other drivers in the linux source tree were taken
- *  	  as samples for some tasks.
- *      - TI TMS380 Second-Generation Token Ring User's Guide
- *  	- TI datasheets for respective chips
- *  	- David Hein at Texas Instruments 
- *  	- Various Madge employees
- *
- *  Maintainer(s):
- *    JS	Jay Schulist		jschlst@samba.org
- *    CG	Christoph Goos		cgoos@syskonnect.de
- *    AF	Adam Fritzler
- *    MLP       Mike Phillips           phillim@amtrak.com
- *    JF	Jochen Friedrich	jochen@scram.de
- *     
- *  Modification History:
- *	29-Aug-97	CG	Created
- *	04-Apr-98	CG	Fixed problems caused by tok_timer_check
- *	10-Apr-98	CG	Fixed lockups at cable disconnection
- *	27-May-98	JS	Formated to Linux Kernel Format
- *	31-May-98	JS	Hacked in PCI support
- *	16-Jun-98	JS	Modulized for multiple cards with one driver
- *	   Sep-99	AF	Renamed to tms380tr (supports more than SK's)
- *      23-Sep-99	AF      Added Compaq and Thomas-Conrad PCI support
- *				Fixed a bug causing double copies on PCI
- *				Fixed for new multicast stuff (2.2/2.3)
- *	25-Sep-99	AF	Uped TPL_NUM from 3 to 9
- *				Removed extraneous 'No free TPL'
- *	22-Dec-99	AF	Added Madge PCI Mk2 support and generalized
- *				parts of the initilization procedure.
- *	30-Dec-99	AF	Turned tms380tr into a library ala 8390.
- *				Madge support is provided in the abyss module
- *				Generic PCI support is in the tmspci module.
- *	30-Nov-00	JF	Updated PCI code to support IO MMU via
- *				pci_map_static(). Alpha uses this MMU for ISA
- *				as well.
- *      14-Jan-01	JF	Fix DMA on ifdown/ifup sequences. Some 
- *      			cleanup.
- *	13-Jan-02	JF	Add spinlock to fix race condition.
- *	09-Nov-02	JF	Fixed printks to not SPAM the console during
- *				normal operation.
- *	30-Dec-02	JF	Removed incorrect __init from 
- *				tms380tr_init_card.
- *	22-Jul-05	JF	Converted to dma-mapping.
- *      			
- *  To do:
- *    1. Multi/Broadcast packet handling (this may have fixed itself)
- *    2. Write a sktrisa module that includes the old ISA support (done)
- *    3. Allow modules to load their own microcode
- *    4. Speed up the BUD process -- freezing the kernel for 3+sec is
- *         quite unacceptable.
- *    5. Still a few remaining stalls when the cable is unplugged.
- */
-
-#ifdef MODULE
-static const char version[] = "tms380tr.c: v1.10 30/12/2002 by Christoph Goos, Adam Fritzler\n";
-#endif
-
-#include <linux/module.h>
-#include <linux/kernel.h>
-#include <linux/types.h>
-#include <linux/fcntl.h>
-#include <linux/interrupt.h>
-#include <linux/ptrace.h>
-#include <linux/ioport.h>
-#include <linux/in.h>
-#include <linux/string.h>
-#include <linux/time.h>
-#include <linux/errno.h>
-#include <linux/init.h>
-#include <linux/dma-mapping.h>
-#include <linux/delay.h>
-#include <linux/netdevice.h>
-#include <linux/etherdevice.h>
-#include <linux/skbuff.h>
-#include <linux/trdevice.h>
-#include <linux/firmware.h>
-#include <linux/bitops.h>
-
-#include <asm/io.h>
-#include <asm/dma.h>
-#include <asm/irq.h>
-#include <asm/uaccess.h>
-
-#include "tms380tr.h"		/* Our Stuff */
-
-/* Use 0 for production, 1 for verification, 2 for debug, and
- * 3 for very verbose debug.
- */
-#ifndef TMS380TR_DEBUG
-#define TMS380TR_DEBUG 0
-#endif
-static unsigned int tms380tr_debug = TMS380TR_DEBUG;
-
-/* Index to functions, as function prototypes.
- * Alphabetical by function name.
- */
-
-/* "A" */
-/* "B" */
-static int      tms380tr_bringup_diags(struct net_device *dev);
-/* "C" */
-static void	tms380tr_cancel_tx_queue(struct net_local* tp);
-static int 	tms380tr_chipset_init(struct net_device *dev);
-static void 	tms380tr_chk_irq(struct net_device *dev);
-static void 	tms380tr_chk_outstanding_cmds(struct net_device *dev);
-static void 	tms380tr_chk_src_addr(unsigned char *frame, unsigned char *hw_addr);
-static unsigned char tms380tr_chk_ssb(struct net_local *tp, unsigned short IrqType);
-int	 	tms380tr_close(struct net_device *dev);
-static void 	tms380tr_cmd_status_irq(struct net_device *dev);
-/* "D" */
-static void 	tms380tr_disable_interrupts(struct net_device *dev);
-#if TMS380TR_DEBUG > 0
-static void 	tms380tr_dump(unsigned char *Data, int length);
-#endif
-/* "E" */
-static void 	tms380tr_enable_interrupts(struct net_device *dev);
-static void 	tms380tr_exec_cmd(struct net_device *dev, unsigned short Command);
-static void 	tms380tr_exec_sifcmd(struct net_device *dev, unsigned int WriteValue);
-/* "F" */
-/* "G" */
-static struct net_device_stats *tms380tr_get_stats(struct net_device *dev);
-/* "H" */
-static netdev_tx_t tms380tr_hardware_send_packet(struct sk_buff *skb,
-						       struct net_device *dev);
-/* "I" */
-static int 	tms380tr_init_adapter(struct net_device *dev);
-static void 	tms380tr_init_ipb(struct net_local *tp);
-static void 	tms380tr_init_net_local(struct net_device *dev);
-static void 	tms380tr_init_opb(struct net_device *dev);
-/* "M" */
-/* "O" */
-int		tms380tr_open(struct net_device *dev);
-static void	tms380tr_open_adapter(struct net_device *dev);
-/* "P" */
-/* "R" */
-static void 	tms380tr_rcv_status_irq(struct net_device *dev);
-static int 	tms380tr_read_ptr(struct net_device *dev);
-static void 	tms380tr_read_ram(struct net_device *dev, unsigned char *Data,
-			unsigned short Address, int Length);
-static int 	tms380tr_reset_adapter(struct net_device *dev);
-static void 	tms380tr_reset_interrupt(struct net_device *dev);
-static void 	tms380tr_ring_status_irq(struct net_device *dev);
-/* "S" */
-static netdev_tx_t tms380tr_send_packet(struct sk_buff *skb,
-					      struct net_device *dev);
-static void 	tms380tr_set_multicast_list(struct net_device *dev);
-static int	tms380tr_set_mac_address(struct net_device *dev, void *addr);
-/* "T" */
-static void 	tms380tr_timer_chk(unsigned long data);
-static void 	tms380tr_timer_end_wait(unsigned long data);
-static void 	tms380tr_tx_status_irq(struct net_device *dev);
-/* "U" */
-static void 	tms380tr_update_rcv_stats(struct net_local *tp,
-			unsigned char DataPtr[], unsigned int Length);
-/* "W" */
-void	 	tms380tr_wait(unsigned long time);
-static void 	tms380tr_write_rpl_status(RPL *rpl, unsigned int Status);
-static void 	tms380tr_write_tpl_status(TPL *tpl, unsigned int Status);
-
-#define SIFREADB(reg) \
-	(((struct net_local *)netdev_priv(dev))->sifreadb(dev, reg))
-#define SIFWRITEB(val, reg) \
-	(((struct net_local *)netdev_priv(dev))->sifwriteb(dev, val, reg))
-#define SIFREADW(reg) \
-	(((struct net_local *)netdev_priv(dev))->sifreadw(dev, reg))
-#define SIFWRITEW(val, reg) \
-	(((struct net_local *)netdev_priv(dev))->sifwritew(dev, val, reg))
-
-
-
-#if 0 /* TMS380TR_DEBUG > 0 */
-static int madgemc_sifprobe(struct net_device *dev)
-{
-        unsigned char old, chk1, chk2;
-	
-	old = SIFREADB(SIFADR);  /* Get the old SIFADR value */
-
-        chk1 = 0;       /* Begin with check value 0 */
-        do {
-		madgemc_setregpage(dev, 0);
-                /* Write new SIFADR value */
-		SIFWRITEB(chk1, SIFADR);
-		chk2 = SIFREADB(SIFADR);
-		if (chk2 != chk1)
-			return -1;
-		
-		madgemc_setregpage(dev, 1);
-                /* Read, invert and write */
-		chk2 = SIFREADB(SIFADD);
-		if (chk2 != chk1)
-			return -1;
-
-		madgemc_setregpage(dev, 0);
-                chk2 ^= 0x0FE;
-		SIFWRITEB(chk2, SIFADR);
-
-                /* Read, invert and compare */
-		madgemc_setregpage(dev, 1);
-		chk2 = SIFREADB(SIFADD);
-		madgemc_setregpage(dev, 0);
-                chk2 ^= 0x0FE;
-
-                if(chk1 != chk2)
-                        return -1;    /* No adapter */
-                chk1 -= 2;
-        } while(chk1 != 0);     /* Repeat 128 times (all byte values) */
-
-	madgemc_setregpage(dev, 0); /* sanity */
-        /* Restore the SIFADR value */
-	SIFWRITEB(old, SIFADR);
-
-        return 0;
-}
-#endif
-
-/*
- * Open/initialize the board. This is called sometime after
- * booting when the 'ifconfig' program is run.
- *
- * This routine should set everything up anew at each open, even
- * registers that "should" only need to be set once at boot, so that
- * there is non-reboot way to recover if something goes wrong.
- */
-int tms380tr_open(struct net_device *dev)
-{
-	struct net_local *tp = netdev_priv(dev);
-	int err;
-	
-	/* init the spinlock */
-	spin_lock_init(&tp->lock);
-	init_timer(&tp->timer);
-
-	/* Reset the hardware here. Don't forget to set the station address. */
-
-#ifdef CONFIG_ISA
-	if(dev->dma > 0) 
-	{
-		unsigned long flags=claim_dma_lock();
-		disable_dma(dev->dma);
-		set_dma_mode(dev->dma, DMA_MODE_CASCADE);
-		enable_dma(dev->dma);
-		release_dma_lock(flags);
-	}
-#endif
-	
-	err = tms380tr_chipset_init(dev);
-  	if(err)
-	{
-		printk(KERN_INFO "%s: Chipset initialization error\n", 
-			dev->name);
-		return -1;
-	}
-
-	tp->timer.expires	= jiffies + 30*HZ;
-	tp->timer.function	= tms380tr_timer_end_wait;
-	tp->timer.data		= (unsigned long)dev;
-	add_timer(&tp->timer);
-
-	printk(KERN_DEBUG "%s: Adapter RAM size: %dK\n", 
-	       dev->name, tms380tr_read_ptr(dev));
-
-	tms380tr_enable_interrupts(dev);
-	tms380tr_open_adapter(dev);
-
-	netif_start_queue(dev);
-	
-	/* Wait for interrupt from hardware. If interrupt does not come,
-	 * there will be a timeout from the timer.
-	 */
-	tp->Sleeping = 1;
-	interruptible_sleep_on(&tp->wait_for_tok_int);
-	del_timer(&tp->timer);
-
-	/* If AdapterVirtOpenFlag is 1, the adapter is now open for use */
-	if(tp->AdapterVirtOpenFlag == 0)
-	{
-		tms380tr_disable_interrupts(dev);
-		return -1;
-	}
-
-	tp->StartTime = jiffies;
-
-	/* Start function control timer */
-	tp->timer.expires	= jiffies + 2*HZ;
-	tp->timer.function	= tms380tr_timer_chk;
-	tp->timer.data		= (unsigned long)dev;
-	add_timer(&tp->timer);
-
-	return 0;
-}
-
-/*
- * Timeout function while waiting for event
- */
-static void tms380tr_timer_end_wait(unsigned long data)
-{
-	struct net_device *dev = (struct net_device*)data;
-	struct net_local *tp = netdev_priv(dev);
-
-	if(tp->Sleeping)
-	{
-		tp->Sleeping = 0;
-		wake_up_interruptible(&tp->wait_for_tok_int);
-	}
-}
-
-/*
- * Initialize the chipset
- */
-static int tms380tr_chipset_init(struct net_device *dev)
-{
-	struct net_local *tp = netdev_priv(dev);
-	int err;
-
-	tms380tr_init_ipb(tp);
-	tms380tr_init_opb(dev);
-	tms380tr_init_net_local(dev);
-
-	if(tms380tr_debug > 3)
-		printk(KERN_DEBUG "%s: Resetting adapter...\n", dev->name);
-	err = tms380tr_reset_adapter(dev);
-	if(err < 0)
-		return -1;
-
-	if(tms380tr_debug > 3)
-		printk(KERN_DEBUG "%s: Bringup diags...\n", dev->name);
-	err = tms380tr_bringup_diags(dev);
-	if(err < 0)
-		return -1;
-
-	if(tms380tr_debug > 3)
-		printk(KERN_DEBUG "%s: Init adapter...\n", dev->name);
-	err = tms380tr_init_adapter(dev);
-	if(err < 0)
-		return -1;
-
-	if(tms380tr_debug > 3)
-		printk(KERN_DEBUG "%s: Done!\n", dev->name);
-	return 0;
-}
-
-/*
- * Initializes the net_local structure.
- */
-static void tms380tr_init_net_local(struct net_device *dev)
-{
-	struct net_local *tp = netdev_priv(dev);
-	int i;
-	dma_addr_t dmabuf;
-
-	tp->scb.CMD	= 0;
-	tp->scb.Parm[0] = 0;
-	tp->scb.Parm[1] = 0;
-
-	tp->ssb.STS	= 0;
-	tp->ssb.Parm[0] = 0;
-	tp->ssb.Parm[1] = 0;
-	tp->ssb.Parm[2] = 0;
-
-	tp->CMDqueue	= 0;
-
-	tp->AdapterOpenFlag	= 0;
-	tp->AdapterVirtOpenFlag = 0;
-	tp->ScbInUse		= 0;
-	tp->OpenCommandIssued	= 0;
-	tp->ReOpenInProgress	= 0;
-	tp->HaltInProgress	= 0;
-	tp->TransmitHaltScheduled = 0;
-	tp->LobeWireFaultLogged	= 0;
-	tp->LastOpenStatus	= 0;
-	tp->MaxPacketSize	= DEFAULT_PACKET_SIZE;
-
-	/* Create circular chain of transmit lists */
-	for (i = 0; i < TPL_NUM; i++)
-	{
-		tp->Tpl[i].NextTPLAddr = htonl(((char *)(&tp->Tpl[(i+1) % TPL_NUM]) - (char *)tp) + tp->dmabuffer); /* DMA buffer may be MMU driven */
-		tp->Tpl[i].Status	= 0;
-		tp->Tpl[i].FrameSize	= 0;
-		tp->Tpl[i].FragList[0].DataCount	= 0;
-		tp->Tpl[i].FragList[0].DataAddr		= 0;
-		tp->Tpl[i].NextTPLPtr	= &tp->Tpl[(i+1) % TPL_NUM];
-		tp->Tpl[i].MData	= NULL;
-		tp->Tpl[i].TPLIndex	= i;
-		tp->Tpl[i].DMABuff	= 0;
-		tp->Tpl[i].BusyFlag	= 0;
-	}
-
-	tp->TplFree = tp->TplBusy = &tp->Tpl[0];
-
-	/* Create circular chain of receive lists */
-	for (i = 0; i < RPL_NUM; i++)
-	{
-		tp->Rpl[i].NextRPLAddr = htonl(((char *)(&tp->Rpl[(i+1) % RPL_NUM]) - (char *)tp) + tp->dmabuffer); /* DMA buffer may be MMU driven */
-		tp->Rpl[i].Status = (RX_VALID | RX_START_FRAME | RX_END_FRAME | RX_FRAME_IRQ);
-		tp->Rpl[i].FrameSize = 0;
-		tp->Rpl[i].FragList[0].DataCount = cpu_to_be16((unsigned short)tp->MaxPacketSize);
-
-		/* Alloc skb and point adapter to data area */
-		tp->Rpl[i].Skb = dev_alloc_skb(tp->MaxPacketSize);
-			tp->Rpl[i].DMABuff = 0;
-
-		/* skb == NULL ? then use local buffer */
-		if(tp->Rpl[i].Skb == NULL)
-		{
-			tp->Rpl[i].SkbStat = SKB_UNAVAILABLE;
-			tp->Rpl[i].FragList[0].DataAddr = htonl(((char *)tp->LocalRxBuffers[i] - (char *)tp) + tp->dmabuffer);
-			tp->Rpl[i].MData = tp->LocalRxBuffers[i];
-		}
-		else	/* SKB != NULL */
-		{
-			tp->Rpl[i].Skb->dev = dev;
-			skb_put(tp->Rpl[i].Skb, tp->MaxPacketSize);
-
-			/* data unreachable for DMA ? then use local buffer */
-			dmabuf = dma_map_single(tp->pdev, tp->Rpl[i].Skb->data, tp->MaxPacketSize, DMA_FROM_DEVICE);
-			if(tp->dmalimit && (dmabuf + tp->MaxPacketSize > tp->dmalimit))
-			{
-				tp->Rpl[i].SkbStat = SKB_DATA_COPY;
-				tp->Rpl[i].FragList[0].DataAddr = htonl(((char *)tp->LocalRxBuffers[i] - (char *)tp) + tp->dmabuffer);
-				tp->Rpl[i].MData = tp->LocalRxBuffers[i];
-			}
-			else	/* DMA directly in skb->data */
-			{
-				tp->Rpl[i].SkbStat = SKB_DMA_DIRECT;
-				tp->Rpl[i].FragList[0].DataAddr = htonl(dmabuf);
-				tp->Rpl[i].MData = tp->Rpl[i].Skb->data;
-				tp->Rpl[i].DMABuff = dmabuf;
-			}
-		}
-
-		tp->Rpl[i].NextRPLPtr = &tp->Rpl[(i+1) % RPL_NUM];
-		tp->Rpl[i].RPLIndex = i;
-	}
-
-	tp->RplHead = &tp->Rpl[0];
-	tp->RplTail = &tp->Rpl[RPL_NUM-1];
-	tp->RplTail->Status = (RX_START_FRAME | RX_END_FRAME | RX_FRAME_IRQ);
-}
-
-/*
- * Initializes the initialisation parameter block.
- */
-static void tms380tr_init_ipb(struct net_local *tp)
-{
-	tp->ipb.Init_Options	= BURST_MODE;
-	tp->ipb.CMD_Status_IV	= 0;
-	tp->ipb.TX_IV		= 0;
-	tp->ipb.RX_IV		= 0;
-	tp->ipb.Ring_Status_IV	= 0;
-	tp->ipb.SCB_Clear_IV	= 0;
-	tp->ipb.Adapter_CHK_IV	= 0;
-	tp->ipb.RX_Burst_Size	= BURST_SIZE;
-	tp->ipb.TX_Burst_Size	= BURST_SIZE;
-	tp->ipb.DMA_Abort_Thrhld = DMA_RETRIES;
-	tp->ipb.SCB_Addr	= 0;
-	tp->ipb.SSB_Addr	= 0;
-}
-
-/*
- * Initializes the open parameter block.
- */
-static void tms380tr_init_opb(struct net_device *dev)
-{
-	struct net_local *tp;
-	unsigned long Addr;
-	unsigned short RplSize    = RPL_SIZE;
-	unsigned short TplSize    = TPL_SIZE;
-	unsigned short BufferSize = BUFFER_SIZE;
-	int i;
-
-	tp = netdev_priv(dev);
-
-	tp->ocpl.OPENOptions 	 = 0;
-	tp->ocpl.OPENOptions 	|= ENABLE_FULL_DUPLEX_SELECTION;
-	tp->ocpl.FullDuplex 	 = 0;
-	tp->ocpl.FullDuplex 	|= OPEN_FULL_DUPLEX_OFF;
-
-        /* 
-	 * Set node address 
-	 *
-	 * We go ahead and put it in the OPB even though on
-	 * most of the generic adapters this isn't required.
-	 * Its simpler this way.  -- ASF
-	 */
-        for (i=0;i<6;i++)
-                tp->ocpl.NodeAddr[i] = ((unsigned char *)dev->dev_addr)[i];
-
-	tp->ocpl.GroupAddr	 = 0;
-	tp->ocpl.FunctAddr	 = 0;
-	tp->ocpl.RxListSize	 = cpu_to_be16((unsigned short)RplSize);
-	tp->ocpl.TxListSize	 = cpu_to_be16((unsigned short)TplSize);
-	tp->ocpl.BufSize	 = cpu_to_be16((unsigned short)BufferSize);
-	tp->ocpl.Reserved	 = 0;
-	tp->ocpl.TXBufMin	 = TX_BUF_MIN;
-	tp->ocpl.TXBufMax	 = TX_BUF_MAX;
-
-	Addr = htonl(((char *)tp->ProductID - (char *)tp) + tp->dmabuffer);
-
-	tp->ocpl.ProdIDAddr[0]	 = LOWORD(Addr);
-	tp->ocpl.ProdIDAddr[1]	 = HIWORD(Addr);
-}
-
-/*
- * Send OPEN command to adapter
- */
-static void tms380tr_open_adapter(struct net_device *dev)
-{
-	struct net_local *tp = netdev_priv(dev);
-
-	if(tp->OpenCommandIssued)
-		return;
-
-	tp->OpenCommandIssued = 1;
-	tms380tr_exec_cmd(dev, OC_OPEN);
-}
-
-/*
- * Clear the adapter's interrupt flag. Clear system interrupt enable
- * (SINTEN): disable adapter to system interrupts.
- */
-static void tms380tr_disable_interrupts(struct net_device *dev)
-{
-	SIFWRITEB(0, SIFACL);
-}
-
-/*
- * Set the adapter's interrupt flag. Set system interrupt enable
- * (SINTEN): enable adapter to system interrupts.
- */
-static void tms380tr_enable_interrupts(struct net_device *dev)
-{
-	SIFWRITEB(ACL_SINTEN, SIFACL);
-}
-
-/*
- * Put command in command queue, try to execute it.
- */
-static void tms380tr_exec_cmd(struct net_device *dev, unsigned short Command)
-{
-	struct net_local *tp = netdev_priv(dev);
-
-	tp->CMDqueue |= Command;
-	tms380tr_chk_outstanding_cmds(dev);
-}
-
-static void tms380tr_timeout(struct net_device *dev)
-{
-	/*
-	 * If we get here, some higher level has decided we are broken.
-	 * There should really be a "kick me" function call instead.
-	 *
-	 * Resetting the token ring adapter takes a long time so just
-	 * fake transmission time and go on trying. Our own timeout
-	 * routine is in tms380tr_timer_chk()
-	 */
-	dev->trans_start = jiffies; /* prevent tx timeout */
-	netif_wake_queue(dev);
-}
-
-/*
- * Gets skb from system, queues it and checks if it can be sent
- */
-static netdev_tx_t tms380tr_send_packet(struct sk_buff *skb,
-					      struct net_device *dev)
-{
-	struct net_local *tp = netdev_priv(dev);
-	netdev_tx_t rc;
-
-	rc = tms380tr_hardware_send_packet(skb, dev);
-	if(tp->TplFree->NextTPLPtr->BusyFlag)
-		netif_stop_queue(dev);
-	return rc;
-}
-
-/*
- * Move frames into adapter tx queue
- */
-static netdev_tx_t tms380tr_hardware_send_packet(struct sk_buff *skb,
-						       struct net_device *dev)
-{
-	TPL *tpl;
-	short length;
-	unsigned char *buf;
-	unsigned long flags;
-	int i;
-	dma_addr_t dmabuf, newbuf;
-	struct net_local *tp = netdev_priv(dev);
-   
-	/* Try to get a free TPL from the chain.
-	 *
-	 * NOTE: We *must* always leave one unused TPL in the chain,
-	 * because otherwise the adapter might send frames twice.
-	 */
-	spin_lock_irqsave(&tp->lock, flags);
-	if(tp->TplFree->NextTPLPtr->BusyFlag)  { /* No free TPL */
-		if (tms380tr_debug > 0)
-			printk(KERN_DEBUG "%s: No free TPL\n", dev->name);
-		spin_unlock_irqrestore(&tp->lock, flags);
-		return NETDEV_TX_BUSY;
-	}
-
-	dmabuf = 0;
-
-	/* Is buffer reachable for Busmaster-DMA? */
-
-	length	= skb->len;
-	dmabuf = dma_map_single(tp->pdev, skb->data, length, DMA_TO_DEVICE);
-	if(tp->dmalimit && (dmabuf + length > tp->dmalimit)) {
-		/* Copy frame to local buffer */
-		dma_unmap_single(tp->pdev, dmabuf, length, DMA_TO_DEVICE);
-		dmabuf  = 0;
-		i 	= tp->TplFree->TPLIndex;
-		buf 	= tp->LocalTxBuffers[i];
-		skb_copy_from_linear_data(skb, buf, length);
-		newbuf 	= ((char *)buf - (char *)tp) + tp->dmabuffer;
-	}
-	else {
-		/* Send direct from skb->data */
-		newbuf	= dmabuf;
-		buf	= skb->data;
-	}
-	/* Source address in packet? */
-	tms380tr_chk_src_addr(buf, dev->dev_addr);
-	tp->LastSendTime	= jiffies;
-	tpl 			= tp->TplFree;	/* Get the "free" TPL */
-	tpl->BusyFlag 		= 1;		/* Mark TPL as busy */
-	tp->TplFree 		= tpl->NextTPLPtr;
-    
-	/* Save the skb for delayed return of skb to system */
-	tpl->Skb = skb;
-	tpl->DMABuff = dmabuf;
-	tpl->FragList[0].DataCount = cpu_to_be16((unsigned short)length);
-	tpl->FragList[0].DataAddr  = htonl(newbuf);
-
-	/* Write the data length in the transmit list. */
-	tpl->FrameSize 	= cpu_to_be16((unsigned short)length);
-	tpl->MData 	= buf;
-
-	/* Transmit the frame and set the status values. */
-	tms380tr_write_tpl_status(tpl, TX_VALID | TX_START_FRAME
-				| TX_END_FRAME | TX_PASS_SRC_ADDR
-				| TX_FRAME_IRQ);
-
-	/* Let adapter send the frame. */
-	tms380tr_exec_sifcmd(dev, CMD_TX_VALID);
-	spin_unlock_irqrestore(&tp->lock, flags);
-
-	return NETDEV_TX_OK;
-}
-
-/*
- * Write the given value to the 'Status' field of the specified TPL.
- * NOTE: This function should be used whenever the status of any TPL must be
- * modified by the driver, because the compiler may otherwise change the
- * order of instructions such that writing the TPL status may be executed at
- * an undesirable time. When this function is used, the status is always
- * written when the function is called.
- */
-static void tms380tr_write_tpl_status(TPL *tpl, unsigned int Status)
-{
-	tpl->Status = Status;
-}
-
-static void tms380tr_chk_src_addr(unsigned char *frame, unsigned char *hw_addr)
-{
-	unsigned char SRBit;
-
-	if((((unsigned long)frame[8]) & ~0x80) != 0)	/* Compare 4 bytes */
-		return;
-	if((unsigned short)frame[12] != 0)		/* Compare 2 bytes */
-		return;
-
-	SRBit = frame[8] & 0x80;
-	memcpy(&frame[8], hw_addr, 6);
-	frame[8] |= SRBit;
-}
-
-/*
- * The timer routine: Check if adapter still open and working, reopen if not. 
- */
-static void tms380tr_timer_chk(unsigned long data)
-{
-	struct net_device *dev = (struct net_device*)data;
-	struct net_local *tp = netdev_priv(dev);
-
-	if(tp->HaltInProgress)
-		return;
-
-	tms380tr_chk_outstanding_cmds(dev);
-	if(time_before(tp->LastSendTime + SEND_TIMEOUT, jiffies) &&
-	   (tp->TplFree != tp->TplBusy))
-	{
-		/* Anything to send, but stalled too long */
-		tp->LastSendTime = jiffies;
-		tms380tr_exec_cmd(dev, OC_CLOSE);	/* Does reopen automatically */
-	}
-
-	tp->timer.expires = jiffies + 2*HZ;
-	add_timer(&tp->timer);
-
-	if(tp->AdapterOpenFlag || tp->ReOpenInProgress)
-		return;
-	tp->ReOpenInProgress = 1;
-	tms380tr_open_adapter(dev);
-}
-
-/*
- * The typical workload of the driver: Handle the network interface interrupts.
- */
-irqreturn_t tms380tr_interrupt(int irq, void *dev_id)
-{
-	struct net_device *dev = dev_id;
-	struct net_local *tp;
-	unsigned short irq_type;
-	int handled = 0;
-
-	tp = netdev_priv(dev);
-
-	irq_type = SIFREADW(SIFSTS);
-
-	while(irq_type & STS_SYSTEM_IRQ) {
-		handled = 1;
-		irq_type &= STS_IRQ_MASK;
-
-		if(!tms380tr_chk_ssb(tp, irq_type)) {
-			printk(KERN_DEBUG "%s: DATA LATE occurred\n", dev->name);
-			break;
-		}
-
-		switch(irq_type) {
-		case STS_IRQ_RECEIVE_STATUS:
-			tms380tr_reset_interrupt(dev);
-			tms380tr_rcv_status_irq(dev);
-			break;
-
-		case STS_IRQ_TRANSMIT_STATUS:
-			/* Check if TRANSMIT.HALT command is complete */
-			if(tp->ssb.Parm[0] & COMMAND_COMPLETE) {
-				tp->TransmitCommandActive = 0;
-					tp->TransmitHaltScheduled = 0;
-
-					/* Issue a new transmit command. */
-					tms380tr_exec_cmd(dev, OC_TRANSMIT);
-				}
-
-				tms380tr_reset_interrupt(dev);
-				tms380tr_tx_status_irq(dev);
-				break;
-
-		case STS_IRQ_COMMAND_STATUS:
-			/* The SSB contains status of last command
-			 * other than receive/transmit.
-			 */
-			tms380tr_cmd_status_irq(dev);
-			break;
-			
-		case STS_IRQ_SCB_CLEAR:
-			/* The SCB is free for another command. */
-			tp->ScbInUse = 0;
-			tms380tr_chk_outstanding_cmds(dev);
-			break;
-			
-		case STS_IRQ_RING_STATUS:
-			tms380tr_ring_status_irq(dev);
-			break;
-
-		case STS_IRQ_ADAPTER_CHECK:
-			tms380tr_chk_irq(dev);
-			break;
-
-		case STS_IRQ_LLC_STATUS:
-			printk(KERN_DEBUG "tms380tr: unexpected LLC status IRQ\n");
-			break;
-			
-		case STS_IRQ_TIMER:
-			printk(KERN_DEBUG "tms380tr: unexpected Timer IRQ\n");
-			break;
-			
-		case STS_IRQ_RECEIVE_PENDING:
-			printk(KERN_DEBUG "tms380tr: unexpected Receive Pending IRQ\n");
-			break;
-			
-		default:
-			printk(KERN_DEBUG "Unknown Token Ring IRQ (0x%04x)\n", irq_type);
-			break;
-		}
-
-		/* Reset system interrupt if not already done. */
-		if(irq_type != STS_IRQ_TRANSMIT_STATUS &&
-		   irq_type != STS_IRQ_RECEIVE_STATUS) {
-			tms380tr_reset_interrupt(dev);
-		}
-
-		irq_type = SIFREADW(SIFSTS);
-	}
-
-	return IRQ_RETVAL(handled);
-}
-
-/*
- *  Reset the INTERRUPT SYSTEM bit and issue SSB CLEAR command.
- */
-static void tms380tr_reset_interrupt(struct net_device *dev)
-{
-	struct net_local *tp = netdev_priv(dev);
-	SSB *ssb = &tp->ssb;
-
-	/*
-	 * [Workaround for "Data Late"]
-	 * Set all fields of the SSB to well-defined values so we can
-	 * check if the adapter has written the SSB.
-	 */
-
-	ssb->STS	= (unsigned short) -1;
-	ssb->Parm[0] 	= (unsigned short) -1;
-	ssb->Parm[1] 	= (unsigned short) -1;
-	ssb->Parm[2] 	= (unsigned short) -1;
-
-	/* Free SSB by issuing SSB_CLEAR command after reading IRQ code
-	 * and clear STS_SYSTEM_IRQ bit: enable adapter for further interrupts.
-	 */
-	tms380tr_exec_sifcmd(dev, CMD_SSB_CLEAR | CMD_CLEAR_SYSTEM_IRQ);
-}
-
-/*
- * Check if the SSB has actually been written by the adapter.
- */
-static unsigned char tms380tr_chk_ssb(struct net_local *tp, unsigned short IrqType)
-{
-	SSB *ssb = &tp->ssb;	/* The address of the SSB. */
-
-	/* C 0 1 2 INTERRUPT CODE
-	 * - - - - --------------
-	 * 1 1 1 1 TRANSMIT STATUS
-	 * 1 1 1 1 RECEIVE STATUS
-	 * 1 ? ? 0 COMMAND STATUS
-	 * 0 0 0 0 SCB CLEAR
-	 * 1 1 0 0 RING STATUS
-	 * 0 0 0 0 ADAPTER CHECK
-	 *
-	 * 0 = SSB field not affected by interrupt
-	 * 1 = SSB field is affected by interrupt
-	 *
-	 * C = SSB ADDRESS +0: COMMAND
-	 * 0 = SSB ADDRESS +2: STATUS 0
-	 * 1 = SSB ADDRESS +4: STATUS 1
-	 * 2 = SSB ADDRESS +6: STATUS 2
-	 */
-
-	/* Check if this interrupt does use the SSB. */
-
-	if(IrqType != STS_IRQ_TRANSMIT_STATUS &&
-	   IrqType != STS_IRQ_RECEIVE_STATUS &&
-	   IrqType != STS_IRQ_COMMAND_STATUS &&
-	   IrqType != STS_IRQ_RING_STATUS)
-	{
-		return 1;	/* SSB not involved. */
-	}
-
-	/* Note: All fields of the SSB have been set to all ones (-1) after it
-	 * has last been used by the software (see DriverIsr()).
-	 *
-	 * Check if the affected SSB fields are still unchanged.
-	 */
-
-	if(ssb->STS == (unsigned short) -1)
-		return 0;	/* Command field not yet available. */
-	if(IrqType == STS_IRQ_COMMAND_STATUS)
-		return 1;	/* Status fields not always affected. */
-	if(ssb->Parm[0] == (unsigned short) -1)
-		return 0;	/* Status 1 field not yet available. */
-	if(IrqType == STS_IRQ_RING_STATUS)
-		return 1;	/* Status 2 & 3 fields not affected. */
-
-	/* Note: At this point, the interrupt is either TRANSMIT or RECEIVE. */
-	if(ssb->Parm[1] == (unsigned short) -1)
-		return 0;	/* Status 2 field not yet available. */
-	if(ssb->Parm[2] == (unsigned short) -1)
-		return 0;	/* Status 3 field not yet available. */
-
-	return 1;	/* All SSB fields have been written by the adapter. */
-}
-
-/*
- * Evaluates the command results status in the SSB status field.
- */
-static void tms380tr_cmd_status_irq(struct net_device *dev)
-{
-	struct net_local *tp = netdev_priv(dev);
-	unsigned short ssb_cmd, ssb_parm_0;
-	unsigned short ssb_parm_1;
-	char *open_err = "Open error -";
-	char *code_err = "Open code -";
-
-	/* Copy the ssb values to local variables */
-	ssb_cmd    = tp->ssb.STS;
-	ssb_parm_0 = tp->ssb.Parm[0];
-	ssb_parm_1 = tp->ssb.Parm[1];
-
-	if(ssb_cmd == OPEN)
-	{
-		tp->Sleeping = 0;
-		if(!tp->ReOpenInProgress)
-	    		wake_up_interruptible(&tp->wait_for_tok_int);
-
-		tp->OpenCommandIssued = 0;
-		tp->ScbInUse = 0;
-
-		if((ssb_parm_0 & 0x00FF) == GOOD_COMPLETION)
-		{
-			/* Success, the adapter is open. */
-			tp->LobeWireFaultLogged	= 0;
-			tp->AdapterOpenFlag 	= 1;
-			tp->AdapterVirtOpenFlag = 1;
-			tp->TransmitCommandActive = 0;
-			tms380tr_exec_cmd(dev, OC_TRANSMIT);
-			tms380tr_exec_cmd(dev, OC_RECEIVE);
-
-			if(tp->ReOpenInProgress)
-				tp->ReOpenInProgress = 0;
-
-			return;
-		}
-		else 	/* The adapter did not open. */
-		{
-	    		if(ssb_parm_0 & NODE_ADDR_ERROR)
-				printk(KERN_INFO "%s: Node address error\n",
-					dev->name);
-	    		if(ssb_parm_0 & LIST_SIZE_ERROR)
-				printk(KERN_INFO "%s: List size error\n",
-					dev->name);
-	    		if(ssb_parm_0 & BUF_SIZE_ERROR)
-				printk(KERN_INFO "%s: Buffer size error\n",
-					dev->name);
-	    		if(ssb_parm_0 & TX_BUF_COUNT_ERROR)
-				printk(KERN_INFO "%s: Tx buffer count error\n",
-					dev->name);
-	    		if(ssb_parm_0 & INVALID_OPEN_OPTION)
-				printk(KERN_INFO "%s: Invalid open option\n",
-					dev->name);
-	    		if(ssb_parm_0 & OPEN_ERROR)
-			{
-				/* Show the open phase. */
-				switch(ssb_parm_0 & OPEN_PHASES_MASK)
-				{
-					case LOBE_MEDIA_TEST:
-						if(!tp->LobeWireFaultLogged)
-						{
-							tp->LobeWireFaultLogged = 1;
-							printk(KERN_INFO "%s: %s Lobe wire fault (check cable !).\n", dev->name, open_err);
-		    				}
-						tp->ReOpenInProgress	= 1;
-						tp->AdapterOpenFlag 	= 0;
-						tp->AdapterVirtOpenFlag = 1;
-						tms380tr_open_adapter(dev);
-						return;
-
-					case PHYSICAL_INSERTION:
-						printk(KERN_INFO "%s: %s Physical insertion.\n", dev->name, open_err);
-						break;
-
-					case ADDRESS_VERIFICATION:
-						printk(KERN_INFO "%s: %s Address verification.\n", dev->name, open_err);
-						break;
-
-					case PARTICIPATION_IN_RING_POLL:
-						printk(KERN_INFO "%s: %s Participation in ring poll.\n", dev->name, open_err);
-						break;
-
-					case REQUEST_INITIALISATION:
-						printk(KERN_INFO "%s: %s Request initialisation.\n", dev->name, open_err);
-						break;
-
-					case FULLDUPLEX_CHECK:
-						printk(KERN_INFO "%s: %s Full duplex check.\n", dev->name, open_err);
-						break;
-
-					default:
-						printk(KERN_INFO "%s: %s Unknown open phase\n", dev->name, open_err);
-						break;
-				}
-
-				/* Show the open errors. */
-				switch(ssb_parm_0 & OPEN_ERROR_CODES_MASK)
-				{
-					case OPEN_FUNCTION_FAILURE:
-						printk(KERN_INFO "%s: %s OPEN_FUNCTION_FAILURE", dev->name, code_err);
-						tp->LastOpenStatus =
-							OPEN_FUNCTION_FAILURE;
-						break;
-
-					case OPEN_SIGNAL_LOSS:
-						printk(KERN_INFO "%s: %s OPEN_SIGNAL_LOSS\n", dev->name, code_err);
-						tp->LastOpenStatus =
-							OPEN_SIGNAL_LOSS;
-						break;
-
-					case OPEN_TIMEOUT:
-						printk(KERN_INFO "%s: %s OPEN_TIMEOUT\n", dev->name, code_err);
-						tp->LastOpenStatus =
-							OPEN_TIMEOUT;
-						break;
-
-					case OPEN_RING_FAILURE:
-						printk(KERN_INFO "%s: %s OPEN_RING_FAILURE\n", dev->name, code_err);
-						tp->LastOpenStatus =
-							OPEN_RING_FAILURE;
-						break;
-
-					case OPEN_RING_BEACONING:
-						printk(KERN_INFO "%s: %s OPEN_RING_BEACONING\n", dev->name, code_err);
-						tp->LastOpenStatus =
-							OPEN_RING_BEACONING;
-						break;
-
-					case OPEN_DUPLICATE_NODEADDR:
-						printk(KERN_INFO "%s: %s OPEN_DUPLICATE_NODEADDR\n", dev->name, code_err);
-						tp->LastOpenStatus =
-							OPEN_DUPLICATE_NODEADDR;
-						break;
-
-					case OPEN_REQUEST_INIT:
-						printk(KERN_INFO "%s: %s OPEN_REQUEST_INIT\n", dev->name, code_err);
-						tp->LastOpenStatus =
-							OPEN_REQUEST_INIT;
-						break;
-
-					case OPEN_REMOVE_RECEIVED:
-						printk(KERN_INFO "%s: %s OPEN_REMOVE_RECEIVED", dev->name, code_err);
-						tp->LastOpenStatus =
-							OPEN_REMOVE_RECEIVED;
-						break;
-
-					case OPEN_FULLDUPLEX_SET:
-						printk(KERN_INFO "%s: %s OPEN_FULLDUPLEX_SET\n", dev->name, code_err);
-						tp->LastOpenStatus =
-							OPEN_FULLDUPLEX_SET;
-						break;
-
-					default:
-						printk(KERN_INFO "%s: %s Unknown open err code", dev->name, code_err);
-						tp->LastOpenStatus =
-							OPEN_FUNCTION_FAILURE;
-						break;
-				}
-			}
-
-			tp->AdapterOpenFlag 	= 0;
-			tp->AdapterVirtOpenFlag = 0;
-
-			return;
-		}
-	}
-	else
-	{
-		if(ssb_cmd != READ_ERROR_LOG)
-			return;
-
-		/* Add values from the error log table to the MAC
-		 * statistics counters and update the errorlogtable
-		 * memory.
-		 */
-		tp->MacStat.line_errors += tp->errorlogtable.Line_Error;
-		tp->MacStat.burst_errors += tp->errorlogtable.Burst_Error;
-		tp->MacStat.A_C_errors += tp->errorlogtable.ARI_FCI_Error;
-		tp->MacStat.lost_frames += tp->errorlogtable.Lost_Frame_Error;
-		tp->MacStat.recv_congest_count += tp->errorlogtable.Rx_Congest_Error;
-		tp->MacStat.rx_errors += tp->errorlogtable.Rx_Congest_Error;
-		tp->MacStat.frame_copied_errors += tp->errorlogtable.Frame_Copied_Error;
-		tp->MacStat.token_errors += tp->errorlogtable.Token_Error;
-		tp->MacStat.dummy1 += tp->errorlogtable.DMA_Bus_Error;
-		tp->MacStat.dummy1 += tp->errorlogtable.DMA_Parity_Error;
-		tp->MacStat.abort_delimiters += tp->errorlogtable.AbortDelimeters;
-		tp->MacStat.frequency_errors += tp->errorlogtable.Frequency_Error;
-		tp->MacStat.internal_errors += tp->errorlogtable.Internal_Error;
-	}
-}
-
-/*
- * The inverse routine to tms380tr_open().
- */
-int tms380tr_close(struct net_device *dev)
-{
-	struct net_local *tp = netdev_priv(dev);
-	netif_stop_queue(dev);
-	
-	del_timer(&tp->timer);
-
-	/* Flush the Tx and disable Rx here. */
-
-	tp->HaltInProgress 	= 1;
-	tms380tr_exec_cmd(dev, OC_CLOSE);
-	tp->timer.expires	= jiffies + 1*HZ;
-	tp->timer.function 	= tms380tr_timer_end_wait;
-	tp->timer.data 		= (unsigned long)dev;
-	add_timer(&tp->timer);
-
-	tms380tr_enable_interrupts(dev);
-
-	tp->Sleeping = 1;
-	interruptible_sleep_on(&tp->wait_for_tok_int);
-	tp->TransmitCommandActive = 0;
-    
-	del_timer(&tp->timer);
-	tms380tr_disable_interrupts(dev);
-   
-#ifdef CONFIG_ISA
-	if(dev->dma > 0) 
-	{
-		unsigned long flags=claim_dma_lock();
-		disable_dma(dev->dma);
-		release_dma_lock(flags);
-	}
-#endif
-	
-	SIFWRITEW(0xFF00, SIFCMD);
-#if 0
-	if(dev->dma > 0) /* what the? */
-		SIFWRITEB(0xff, POSREG);
-#endif
-	tms380tr_cancel_tx_queue(tp);
-
-	return 0;
-}
-
-/*
- * Get the current statistics. This may be called with the card open
- * or closed.
- */
-static struct net_device_stats *tms380tr_get_stats(struct net_device *dev)
-{
-	struct net_local *tp = netdev_priv(dev);
-
-	return (struct net_device_stats *)&tp->MacStat;
-}
-
-/*
- * Set or clear the multicast filter for this adapter.
- */
-static void tms380tr_set_multicast_list(struct net_device *dev)
-{
-	struct net_local *tp = netdev_priv(dev);
-	unsigned int OpenOptions;
-	
-	OpenOptions = tp->ocpl.OPENOptions &
-		~(PASS_ADAPTER_MAC_FRAMES
-		  | PASS_ATTENTION_FRAMES
-		  | PASS_BEACON_MAC_FRAMES
-		  | COPY_ALL_MAC_FRAMES
-		  | COPY_ALL_NON_MAC_FRAMES);
-	
-	tp->ocpl.FunctAddr = 0;
-	
-	if(dev->flags & IFF_PROMISC)
-		/* Enable promiscuous mode */
-		OpenOptions |= COPY_ALL_NON_MAC_FRAMES |
-			COPY_ALL_MAC_FRAMES;
-	else
-	{
-		if(dev->flags & IFF_ALLMULTI)
-		{
-			/* Disable promiscuous mode, use normal mode. */
-			tp->ocpl.FunctAddr = 0xFFFFFFFF;
-		}
-		else
-		{
-			struct netdev_hw_addr *ha;
-
-			netdev_for_each_mc_addr(ha, dev) {
-				((char *)(&tp->ocpl.FunctAddr))[0] |=
-					ha->addr[2];
-				((char *)(&tp->ocpl.FunctAddr))[1] |=
-					ha->addr[3];
-				((char *)(&tp->ocpl.FunctAddr))[2] |=
-					ha->addr[4];
-				((char *)(&tp->ocpl.FunctAddr))[3] |=
-					ha->addr[5];
-			}
-		}
-		tms380tr_exec_cmd(dev, OC_SET_FUNCT_ADDR);
-	}
-	
-	tp->ocpl.OPENOptions = OpenOptions;
-	tms380tr_exec_cmd(dev, OC_MODIFY_OPEN_PARMS);
-}
-
-/*
- * Wait for some time (microseconds)
- */
-void tms380tr_wait(unsigned long time)
-{
-#if 0
-	long tmp;
-	
-	tmp = jiffies + time/(1000000/HZ);
-	do {
-		tmp = schedule_timeout_interruptible(tmp);
-	} while(time_after(tmp, jiffies));
-#else
-	mdelay(time / 1000);
-#endif
-}
-
-/*
- * Write a command value to the SIFCMD register
- */
-static void tms380tr_exec_sifcmd(struct net_device *dev, unsigned int WriteValue)
-{
-	unsigned short cmd;
-	unsigned short SifStsValue;
-	unsigned long loop_counter;
-
-	WriteValue = ((WriteValue ^ CMD_SYSTEM_IRQ) | CMD_INTERRUPT_ADAPTER);
-	cmd = (unsigned short)WriteValue;
-	loop_counter = 0,5 * 800000;
-	do {
-		SifStsValue = SIFREADW(SIFSTS);
-	} while((SifStsValue & CMD_INTERRUPT_ADAPTER) && loop_counter--);
-	SIFWRITEW(cmd, SIFCMD);
-}
-
-/*
- * Processes adapter hardware reset, halts adapter and downloads firmware,
- * clears the halt bit.
- */
-static int tms380tr_reset_adapter(struct net_device *dev)
-{
-	struct net_local *tp = netdev_priv(dev);
-	unsigned short *fw_ptr;
-	unsigned short count, c, count2;
-	const struct firmware *fw_entry = NULL;
-
-	if (request_firmware(&fw_entry, "tms380tr.bin", tp->pdev) != 0) {
-		printk(KERN_ALERT "%s: firmware %s is missing, cannot start.\n",
-			dev->name, "tms380tr.bin");
-		return -1;
-	}
-
-	fw_ptr = (unsigned short *)fw_entry->data;
-	count2 = fw_entry->size / 2;
-
-	/* Hardware adapter reset */
-	SIFWRITEW(ACL_ARESET, SIFACL);
-	tms380tr_wait(40);
-	
-	c = SIFREADW(SIFACL);
-	tms380tr_wait(20);
-
-	if(dev->dma == 0)	/* For PCI adapters */
-	{
-		c &= ~(ACL_NSELOUT0 | ACL_NSELOUT1);	/* Clear bits */
-		if(tp->setnselout)
-		  c |= (*tp->setnselout)(dev);
-	}
-
-	/* In case a command is pending - forget it */
-	tp->ScbInUse = 0;
-
-	c &= ~ACL_ARESET;		/* Clear adapter reset bit */
-	c |=  ACL_CPHALT;		/* Halt adapter CPU, allow download */
-	c |= ACL_BOOT;
-	c |= ACL_SINTEN;
-	c &= ~ACL_PSDMAEN;		/* Clear pseudo dma bit */
-	SIFWRITEW(c, SIFACL);
-	tms380tr_wait(40);
-
-	count = 0;
-	/* Download firmware via DIO interface: */
-	do {
-		if (count2 < 3) continue;
-
-		/* Download first address part */
-		SIFWRITEW(*fw_ptr, SIFADX);
-		fw_ptr++;
-		count2--;
-		/* Download second address part */
-		SIFWRITEW(*fw_ptr, SIFADD);
-		fw_ptr++;
-		count2--;
-
-		if((count = *fw_ptr) != 0)	/* Load loop counter */
-		{
-			fw_ptr++;	/* Download block data */
-			count2--;
-			if (count > count2) continue;
-
-			for(; count > 0; count--)
-			{
-				SIFWRITEW(*fw_ptr, SIFINC);
-				fw_ptr++;
-				count2--;
-			}
-		}
-		else	/* Stop, if last block downloaded */
-		{
-			c = SIFREADW(SIFACL);
-			c &= (~ACL_CPHALT | ACL_SINTEN);
-
-			/* Clear CPHALT and start BUD */
-			SIFWRITEW(c, SIFACL);
-			release_firmware(fw_entry);
-			return 1;
-		}
-	} while(count == 0);
-
-	release_firmware(fw_entry);
-	printk(KERN_INFO "%s: Adapter Download Failed\n", dev->name);
-	return -1;
-}
-
-MODULE_FIRMWARE("tms380tr.bin");
-
-/*
- * Starts bring up diagnostics of token ring adapter and evaluates
- * diagnostic results.
- */
-static int tms380tr_bringup_diags(struct net_device *dev)
-{
-	int loop_cnt, retry_cnt;
-	unsigned short Status;
-
-	tms380tr_wait(HALF_SECOND);
-	tms380tr_exec_sifcmd(dev, EXEC_SOFT_RESET);
-	tms380tr_wait(HALF_SECOND);
-
-	retry_cnt = BUD_MAX_RETRIES;	/* maximal number of retrys */
-
-	do {
-		retry_cnt--;
-		if(tms380tr_debug > 3)
-			printk(KERN_DEBUG "BUD-Status: ");
-		loop_cnt = BUD_MAX_LOOPCNT;	/* maximum: three seconds*/
-		do {			/* Inspect BUD results */
-			loop_cnt--;
-			tms380tr_wait(HALF_SECOND);
-			Status = SIFREADW(SIFSTS);
-			Status &= STS_MASK;
-
-			if(tms380tr_debug > 3)
-				printk(KERN_DEBUG " %04X\n", Status);
-			/* BUD successfully completed */
-			if(Status == STS_INITIALIZE)
-				return 1;
-		/* Unrecoverable hardware error, BUD not completed? */
-		} while((loop_cnt > 0) && ((Status & (STS_ERROR | STS_TEST))
-			!= (STS_ERROR | STS_TEST)));
-
-		/* Error preventing completion of BUD */
-		if(retry_cnt > 0)
-		{
-			printk(KERN_INFO "%s: Adapter Software Reset.\n", 
-				dev->name);
-			tms380tr_exec_sifcmd(dev, EXEC_SOFT_RESET);
-			tms380tr_wait(HALF_SECOND);
-		}
-	} while(retry_cnt > 0);
-
-	Status = SIFREADW(SIFSTS);
-	
-	printk(KERN_INFO "%s: Hardware error\n", dev->name);
-	/* Hardware error occurred! */
-	Status &= 0x001f;
-	if (Status & 0x0010)
-		printk(KERN_INFO "%s: BUD Error: Timeout\n", dev->name);
-	else if ((Status & 0x000f) > 6)
-		printk(KERN_INFO "%s: BUD Error: Illegal Failure\n", dev->name);
-	else
-		printk(KERN_INFO "%s: Bring Up Diagnostics Error (%04X) occurred\n", dev->name, Status & 0x000f);
-
-	return -1;
-}
-
-/*
- * Copy initialisation data to adapter memory, beginning at address
- * 1:0A00; Starting DMA test and evaluating result bits.
- */
-static int tms380tr_init_adapter(struct net_device *dev)
-{
-	struct net_local *tp = netdev_priv(dev);
-
-	const unsigned char SCB_Test[6] = {0x00, 0x00, 0xC1, 0xE2, 0xD4, 0x8B};
-	const unsigned char SSB_Test[8] = {0xFF, 0xFF, 0xD1, 0xD7,
-						0xC5, 0xD9, 0xC3, 0xD4};
-	void *ptr = (void *)&tp->ipb;
-	unsigned short *ipb_ptr = (unsigned short *)ptr;
-	unsigned char *cb_ptr = (unsigned char *) &tp->scb;
-	unsigned char *sb_ptr = (unsigned char *) &tp->ssb;
-	unsigned short Status;
-	int i, loop_cnt, retry_cnt;
-
-	/* Normalize: byte order low/high, word order high/low! (only IPB!) */
-	tp->ipb.SCB_Addr = SWAPW(((char *)&tp->scb - (char *)tp) + tp->dmabuffer);
-	tp->ipb.SSB_Addr = SWAPW(((char *)&tp->ssb - (char *)tp) + tp->dmabuffer);
-
-	if(tms380tr_debug > 3)
-	{
-		printk(KERN_DEBUG "%s: buffer (real): %lx\n", dev->name, (long) &tp->scb);
-		printk(KERN_DEBUG "%s: buffer (virt): %lx\n", dev->name, (long) ((char *)&tp->scb - (char *)tp) + (long) tp->dmabuffer);
-		printk(KERN_DEBUG "%s: buffer (DMA) : %lx\n", dev->name, (long) tp->dmabuffer);
-		printk(KERN_DEBUG "%s: buffer (tp)  : %lx\n", dev->name, (long) tp);
-	}
-	/* Maximum: three initialization retries */
-	retry_cnt = INIT_MAX_RETRIES;
-
-	do {
-		retry_cnt--;
-
-		/* Transfer initialization block */
-		SIFWRITEW(0x0001, SIFADX);
-
-		/* To address 0001:0A00 of adapter RAM */
-		SIFWRITEW(0x0A00, SIFADD);
-
-		/* Write 11 words to adapter RAM */
-		for(i = 0; i < 11; i++)
-			SIFWRITEW(ipb_ptr[i], SIFINC);
-
-		/* Execute SCB adapter command */
-		tms380tr_exec_sifcmd(dev, CMD_EXECUTE);
-
-		loop_cnt = INIT_MAX_LOOPCNT;	/* Maximum: 11 seconds */
-
-		/* While remaining retries, no error and not completed */
-		do {
-			Status = 0;
-			loop_cnt--;
-			tms380tr_wait(HALF_SECOND);
-
-			/* Mask interesting status bits */
-			Status = SIFREADW(SIFSTS);
-			Status &= STS_MASK;
-		} while(((Status &(STS_INITIALIZE | STS_ERROR | STS_TEST)) != 0) &&
-			((Status & STS_ERROR) == 0) && (loop_cnt != 0));
-
-		if((Status & (STS_INITIALIZE | STS_ERROR | STS_TEST)) == 0)
-		{
-			/* Initialization completed without error */
-			i = 0;
-			do {	/* Test if contents of SCB is valid */
-				if(SCB_Test[i] != *(cb_ptr + i))
-				{
-					printk(KERN_INFO "%s: DMA failed\n", dev->name);
-					/* DMA data error: wrong data in SCB */
-					return -1;
-				}
-				i++;
-			} while(i < 6);
-
-			i = 0;
-			do {	/* Test if contents of SSB is valid */
-				if(SSB_Test[i] != *(sb_ptr + i))
-					/* DMA data error: wrong data in SSB */
-					return -1;
-				i++;
-			} while (i < 8);
-
-			return 1;	/* Adapter successfully initialized */
-		}
-		else
-		{
-			if((Status & STS_ERROR) != 0)
-			{
-				/* Initialization error occurred */
-				Status = SIFREADW(SIFSTS);
-				Status &= STS_ERROR_MASK;
-				/* ShowInitialisationErrorCode(Status); */
-				printk(KERN_INFO "%s: Status error: %d\n", dev->name, Status);
-				return -1; /* Unrecoverable error */
-			}
-			else
-			{
-				if(retry_cnt > 0)
-				{
-					/* Reset adapter and try init again */
-					tms380tr_exec_sifcmd(dev, EXEC_SOFT_RESET);
-					tms380tr_wait(HALF_SECOND);
-				}
-			}
-		}
-	} while(retry_cnt > 0);
-
-	printk(KERN_INFO "%s: Retry exceeded\n", dev->name);
-	return -1;
-}
-
-/*
- * Check for outstanding commands in command queue and tries to execute
- * command immediately. Corresponding command flag in command queue is cleared.
- */
-static void tms380tr_chk_outstanding_cmds(struct net_device *dev)
-{
-	struct net_local *tp = netdev_priv(dev);
-	unsigned long Addr = 0;
-
-	if(tp->CMDqueue == 0)
-		return;		/* No command execution */
-
-	/* If SCB in use: no command */
-	if(tp->ScbInUse == 1)
-		return;
-
-	/* Check if adapter is opened, avoiding COMMAND_REJECT
-	 * interrupt by the adapter!
-	 */
-	if (tp->AdapterOpenFlag == 0) {
-		if (tp->CMDqueue & OC_OPEN) {
-			/* Execute OPEN command	*/
-			tp->CMDqueue ^= OC_OPEN;
-
-			Addr = htonl(((char *)&tp->ocpl - (char *)tp) + tp->dmabuffer);
-			tp->scb.Parm[0] = LOWORD(Addr);
-			tp->scb.Parm[1] = HIWORD(Addr);
-			tp->scb.CMD = OPEN;
-		} else
-			/* No OPEN command queued, but adapter closed. Note:
-			 * We'll try to re-open the adapter in DriverPoll()
-			 */
-			return;		/* No adapter command issued */
-	} else {
-		/* Adapter is open; evaluate command queue: try to execute
-		 * outstanding commands (depending on priority!) CLOSE
-		 * command queued
-		 */
-		if (tp->CMDqueue & OC_CLOSE) {
-			tp->CMDqueue ^= OC_CLOSE;
-			tp->AdapterOpenFlag = 0;
-			tp->scb.Parm[0] = 0; /* Parm[0], Parm[1] are ignored */
-			tp->scb.Parm[1] = 0; /* but should be set to zero! */
-			tp->scb.CMD = CLOSE;
-			if(!tp->HaltInProgress)
-				tp->CMDqueue |= OC_OPEN; /* re-open adapter */
-			else
-				tp->CMDqueue = 0;	/* no more commands */
-		} else if (tp->CMDqueue & OC_RECEIVE) {
-			tp->CMDqueue ^= OC_RECEIVE;
-			Addr = htonl(((char *)tp->RplHead - (char *)tp) + tp->dmabuffer);
-			tp->scb.Parm[0] = LOWORD(Addr);
-			tp->scb.Parm[1] = HIWORD(Addr);
-			tp->scb.CMD = RECEIVE;
-		} else if (tp->CMDqueue & OC_TRANSMIT_HALT) {
-			/* NOTE: TRANSMIT.HALT must be checked
-			 * before TRANSMIT.
-			 */
-			tp->CMDqueue ^= OC_TRANSMIT_HALT;
-			tp->scb.CMD = TRANSMIT_HALT;
-
-			/* Parm[0] and Parm[1] are ignored
-			 * but should be set to zero!
-			 */
-			tp->scb.Parm[0] = 0;
-			tp->scb.Parm[1] = 0;
-		} else if (tp->CMDqueue & OC_TRANSMIT) {
-			/* NOTE: TRANSMIT must be
-			 * checked after TRANSMIT.HALT
-			 */
-			if (tp->TransmitCommandActive) {
-				if (!tp->TransmitHaltScheduled) {
-					tp->TransmitHaltScheduled = 1;
-					tms380tr_exec_cmd(dev, OC_TRANSMIT_HALT);
-				}
-				tp->TransmitCommandActive = 0;
-				return;
-			}
-
-			tp->CMDqueue ^= OC_TRANSMIT;
-			tms380tr_cancel_tx_queue(tp);
-			Addr = htonl(((char *)tp->TplBusy - (char *)tp) + tp->dmabuffer);
-			tp->scb.Parm[0] = LOWORD(Addr);
-			tp->scb.Parm[1] = HIWORD(Addr);
-			tp->scb.CMD = TRANSMIT;
-			tp->TransmitCommandActive = 1;
-		} else if (tp->CMDqueue & OC_MODIFY_OPEN_PARMS) {
-			tp->CMDqueue ^= OC_MODIFY_OPEN_PARMS;
-			tp->scb.Parm[0] = tp->ocpl.OPENOptions; /* new OPEN options*/
-			tp->scb.Parm[0] |= ENABLE_FULL_DUPLEX_SELECTION;
-			tp->scb.Parm[1] = 0; /* is ignored but should be zero */
-			tp->scb.CMD = MODIFY_OPEN_PARMS;
-		} else if (tp->CMDqueue & OC_SET_FUNCT_ADDR) {
-			tp->CMDqueue ^= OC_SET_FUNCT_ADDR;
-			tp->scb.Parm[0] = LOWORD(tp->ocpl.FunctAddr);
-			tp->scb.Parm[1] = HIWORD(tp->ocpl.FunctAddr);
-			tp->scb.CMD = SET_FUNCT_ADDR;
-		} else if (tp->CMDqueue & OC_SET_GROUP_ADDR) {
-			tp->CMDqueue ^= OC_SET_GROUP_ADDR;
-			tp->scb.Parm[0] = LOWORD(tp->ocpl.GroupAddr);
-			tp->scb.Parm[1] = HIWORD(tp->ocpl.GroupAddr);
-			tp->scb.CMD = SET_GROUP_ADDR;
-		} else if (tp->CMDqueue & OC_READ_ERROR_LOG) {
-			tp->CMDqueue ^= OC_READ_ERROR_LOG;
-			Addr = htonl(((char *)&tp->errorlogtable - (char *)tp) + tp->dmabuffer);
-			tp->scb.Parm[0] = LOWORD(Addr);
-			tp->scb.Parm[1] = HIWORD(Addr);
-			tp->scb.CMD = READ_ERROR_LOG;
-		} else {
-			printk(KERN_WARNING "CheckForOutstandingCommand: unknown Command\n");
-			tp->CMDqueue = 0;
-			return;
-		}
-	}
-
-	tp->ScbInUse = 1;	/* Set semaphore: SCB in use. */
-
-	/* Execute SCB and generate IRQ when done. */
-	tms380tr_exec_sifcmd(dev, CMD_EXECUTE | CMD_SCB_REQUEST);
-}
-
-/*
- * IRQ conditions: signal loss on the ring, transmit or receive of beacon
- * frames (disabled if bit 1 of OPEN option is set); report error MAC
- * frame transmit (disabled if bit 2 of OPEN option is set); open or short
- * circuit fault on the lobe is detected; remove MAC frame received;
- * error counter overflow (255); opened adapter is the only station in ring.
- * After some of the IRQs the adapter is closed!
- */
-static void tms380tr_ring_status_irq(struct net_device *dev)
-{
-	struct net_local *tp = netdev_priv(dev);
-
-	tp->CurrentRingStatus = be16_to_cpu((unsigned short)tp->ssb.Parm[0]);
-
-	/* First: fill up statistics */
-	if(tp->ssb.Parm[0] & SIGNAL_LOSS)
-	{
-		printk(KERN_INFO "%s: Signal Loss\n", dev->name);
-		tp->MacStat.line_errors++;
-	}
-
-	/* Adapter is closed, but initialized */
-	if(tp->ssb.Parm[0] & LOBE_WIRE_FAULT)
-	{
-		printk(KERN_INFO "%s: Lobe Wire Fault, Reopen Adapter\n", 
-			dev->name);
-		tp->MacStat.line_errors++;
-	}
-
-	if(tp->ssb.Parm[0] & RING_RECOVERY)
-		printk(KERN_INFO "%s: Ring Recovery\n", dev->name);
-
-	/* Counter overflow: read error log */
-	if(tp->ssb.Parm[0] & COUNTER_OVERFLOW)
-	{
-		printk(KERN_INFO "%s: Counter Overflow\n", dev->name);
-		tms380tr_exec_cmd(dev, OC_READ_ERROR_LOG);
-	}
-
-	/* Adapter is closed, but initialized */
-	if(tp->ssb.Parm[0] & REMOVE_RECEIVED)
-		printk(KERN_INFO "%s: Remove Received, Reopen Adapter\n", 
-			dev->name);
-
-	/* Adapter is closed, but initialized */
-	if(tp->ssb.Parm[0] & AUTO_REMOVAL_ERROR)
-		printk(KERN_INFO "%s: Auto Removal Error, Reopen Adapter\n", 
-			dev->name);
-
-	if(tp->ssb.Parm[0] & HARD_ERROR)
-		printk(KERN_INFO "%s: Hard Error\n", dev->name);
-
-	if(tp->ssb.Parm[0] & SOFT_ERROR)
-		printk(KERN_INFO "%s: Soft Error\n", dev->name);
-
-	if(tp->ssb.Parm[0] & TRANSMIT_BEACON)
-		printk(KERN_INFO "%s: Transmit Beacon\n", dev->name);
-
-	if(tp->ssb.Parm[0] & SINGLE_STATION)
-		printk(KERN_INFO "%s: Single Station\n", dev->name);
-
-	/* Check if adapter has been closed */
-	if(tp->ssb.Parm[0] & ADAPTER_CLOSED)
-	{
-		printk(KERN_INFO "%s: Adapter closed (Reopening)," 
-			"CurrentRingStat %x\n",
-			dev->name, tp->CurrentRingStatus);
-		tp->AdapterOpenFlag = 0;
-		tms380tr_open_adapter(dev);
-	}
-}
-
-/*
- * Issued if adapter has encountered an unrecoverable hardware
- * or software error.
- */
-static void tms380tr_chk_irq(struct net_device *dev)
-{
-	int i;
-	unsigned short AdapterCheckBlock[4];
-	struct net_local *tp = netdev_priv(dev);
-
-	tp->AdapterOpenFlag = 0;	/* Adapter closed now */
-
-	/* Page number of adapter memory */
-	SIFWRITEW(0x0001, SIFADX);
-	/* Address offset */
-	SIFWRITEW(CHECKADDR, SIFADR);
-
-	/* Reading 8 byte adapter check block. */
-	for(i = 0; i < 4; i++)
-		AdapterCheckBlock[i] = SIFREADW(SIFINC);
-
-	if(tms380tr_debug > 3)
-	{
-		printk(KERN_DEBUG "%s: AdapterCheckBlock: ", dev->name);
-		for (i = 0; i < 4; i++)
-			printk("%04X", AdapterCheckBlock[i]);
-		printk("\n");
-	}
-
-	switch(AdapterCheckBlock[0])
-	{
-		case DIO_PARITY:
-			printk(KERN_INFO "%s: DIO parity error\n", dev->name);
-			break;
-
-		case DMA_READ_ABORT:
-			printk(KERN_INFO "%s DMA read operation aborted:\n",
-				dev->name);
-			switch (AdapterCheckBlock[1])
-			{
-				case 0:
-					printk(KERN_INFO "Timeout\n");
-					printk(KERN_INFO "Address: %04X %04X\n",
-						AdapterCheckBlock[2],
-						AdapterCheckBlock[3]);
-					break;
-
-				case 1:
-					printk(KERN_INFO "Parity error\n");
-					printk(KERN_INFO "Address: %04X %04X\n",
-						AdapterCheckBlock[2], 
-						AdapterCheckBlock[3]);
-					break;
-
-				case 2: 
-					printk(KERN_INFO "Bus error\n");
-					printk(KERN_INFO "Address: %04X %04X\n",
-						AdapterCheckBlock[2], 
-						AdapterCheckBlock[3]);
-					break;
-
-				default:
-					printk(KERN_INFO "Unknown error.\n");
-					break;
-			}
-			break;
-
-		case DMA_WRITE_ABORT:
-			printk(KERN_INFO "%s: DMA write operation aborted:\n",
-				dev->name);
-			switch (AdapterCheckBlock[1])
-			{
-				case 0: 
-					printk(KERN_INFO "Timeout\n");
-					printk(KERN_INFO "Address: %04X %04X\n",
-						AdapterCheckBlock[2], 
-						AdapterCheckBlock[3]);
-					break;
-
-				case 1: 
-					printk(KERN_INFO "Parity error\n");
-					printk(KERN_INFO "Address: %04X %04X\n",
-						AdapterCheckBlock[2], 
-						AdapterCheckBlock[3]);
-					break;
-
-				case 2: 
-					printk(KERN_INFO "Bus error\n");
-					printk(KERN_INFO "Address: %04X %04X\n",
-						AdapterCheckBlock[2], 
-						AdapterCheckBlock[3]);
-					break;
-
-				default:
-					printk(KERN_INFO "Unknown error.\n");
-					break;
-			}
-			break;
-
-		case ILLEGAL_OP_CODE:
-			printk(KERN_INFO "%s: Illegal operation code in firmware\n",
-				dev->name);
-			/* Parm[0-3]: adapter internal register R13-R15 */
-			break;
-
-		case PARITY_ERRORS:
-			printk(KERN_INFO "%s: Adapter internal bus parity error\n",
-				dev->name);
-			/* Parm[0-3]: adapter internal register R13-R15 */
-			break;
-
-		case RAM_DATA_ERROR:
-			printk(KERN_INFO "%s: RAM data error\n", dev->name);
-			/* Parm[0-1]: MSW/LSW address of RAM location. */
-			break;
-
-		case RAM_PARITY_ERROR:
-			printk(KERN_INFO "%s: RAM parity error\n", dev->name);
-			/* Parm[0-1]: MSW/LSW address of RAM location. */
-			break;
-
-		case RING_UNDERRUN:
-			printk(KERN_INFO "%s: Internal DMA underrun detected\n",
-				dev->name);
-			break;
-
-		case INVALID_IRQ:
-			printk(KERN_INFO "%s: Unrecognized interrupt detected\n",
-				dev->name);
-			/* Parm[0-3]: adapter internal register R13-R15 */
-			break;
-
-		case INVALID_ERROR_IRQ:
-			printk(KERN_INFO "%s: Unrecognized error interrupt detected\n",
-				dev->name);
-			/* Parm[0-3]: adapter internal register R13-R15 */
-			break;
-
-		case INVALID_XOP:
-			printk(KERN_INFO "%s: Unrecognized XOP request detected\n",
-				dev->name);
-			/* Parm[0-3]: adapter internal register R13-R15 */
-			break;
-
-		default:
-			printk(KERN_INFO "%s: Unknown status", dev->name);
-			break;
-	}
-
-	if(tms380tr_chipset_init(dev) == 1)
-	{
-		/* Restart of firmware successful */
-		tp->AdapterOpenFlag = 1;
-	}
-}
-
-/*
- * Internal adapter pointer to RAM data are copied from adapter into
- * host system.
- */
-static int tms380tr_read_ptr(struct net_device *dev)
-{
-	struct net_local *tp = netdev_priv(dev);
-	unsigned short adapterram;
-
-	tms380tr_read_ram(dev, (unsigned char *)&tp->intptrs.BurnedInAddrPtr,
-			ADAPTER_INT_PTRS, 16);
-	tms380tr_read_ram(dev, (unsigned char *)&adapterram,
-			cpu_to_be16((unsigned short)tp->intptrs.AdapterRAMPtr), 2);
-	return be16_to_cpu(adapterram); 
-}
-
-/*
- * Reads a number of bytes from adapter to system memory.
- */
-static void tms380tr_read_ram(struct net_device *dev, unsigned char *Data,
-				unsigned short Address, int Length)
-{
-	int i;
-	unsigned short old_sifadx, old_sifadr, InWord;
-
-	/* Save the current values */
-	old_sifadx = SIFREADW(SIFADX);
-	old_sifadr = SIFREADW(SIFADR);
-
-	/* Page number of adapter memory */
-	SIFWRITEW(0x0001, SIFADX);
-	/* Address offset in adapter RAM */
-        SIFWRITEW(Address, SIFADR);
-
-	/* Copy len byte from adapter memory to system data area. */
-	i = 0;
-	for(;;)
-	{
-		InWord = SIFREADW(SIFINC);
-
-		*(Data + i) = HIBYTE(InWord);	/* Write first byte */
-		if(++i == Length)		/* All is done break */
-			break;
-
-		*(Data + i) = LOBYTE(InWord);	/* Write second byte */
-		if (++i == Length)		/* All is done break */
-			break;
-	}
-
-	/* Restore original values */
-	SIFWRITEW(old_sifadx, SIFADX);
-	SIFWRITEW(old_sifadr, SIFADR);
-}
-
-/*
- * Cancel all queued packets in the transmission queue.
- */
-static void tms380tr_cancel_tx_queue(struct net_local* tp)
-{
-	TPL *tpl;
-
-	/*
-	 * NOTE: There must not be an active TRANSMIT command pending, when
-	 * this function is called.
-	 */
-	if(tp->TransmitCommandActive)
-		return;
-
-	for(;;)
-	{
-		tpl = tp->TplBusy;
-		if(!tpl->BusyFlag)
-			break;
-		/* "Remove" TPL from busy list. */
-		tp->TplBusy = tpl->NextTPLPtr;
-		tms380tr_write_tpl_status(tpl, 0);	/* Clear VALID bit */
-		tpl->BusyFlag = 0;		/* "free" TPL */
-
-		printk(KERN_INFO "Cancel tx (%08lXh).\n", (unsigned long)tpl);
-		if (tpl->DMABuff)
-			dma_unmap_single(tp->pdev, tpl->DMABuff, tpl->Skb->len, DMA_TO_DEVICE);
-		dev_kfree_skb_any(tpl->Skb);
-	}
-}
-
-/*
- * This function is called whenever a transmit interrupt is generated by the
- * adapter. For a command complete interrupt, it is checked if we have to
- * issue a new transmit command or not.
- */
-static void tms380tr_tx_status_irq(struct net_device *dev)
-{
-	struct net_local *tp = netdev_priv(dev);
-	unsigned char HighByte, HighAc, LowAc;
-	TPL *tpl;
-
-	/* NOTE: At this point the SSB from TRANSMIT STATUS is no longer
-	 * available, because the CLEAR SSB command has already been issued.
-	 *
-	 * Process all complete transmissions.
-	 */
-
-	for(;;)
-	{
-		tpl = tp->TplBusy;
-		if(!tpl->BusyFlag || (tpl->Status
-			& (TX_VALID | TX_FRAME_COMPLETE))
-			!= TX_FRAME_COMPLETE)
-		{
-			break;
-		}
-
-		/* "Remove" TPL from busy list. */
-		tp->TplBusy = tpl->NextTPLPtr ;
-
-		/* Check the transmit status field only for directed frames*/
-		if(DIRECTED_FRAME(tpl) && (tpl->Status & TX_ERROR) == 0)
-		{
-			HighByte = GET_TRANSMIT_STATUS_HIGH_BYTE(tpl->Status);
-			HighAc   = GET_FRAME_STATUS_HIGH_AC(HighByte);
-			LowAc    = GET_FRAME_STATUS_LOW_AC(HighByte);
-
-			if((HighAc != LowAc) || (HighAc == AC_NOT_RECOGNIZED))
-			{
-				printk(KERN_DEBUG "%s: (DA=%08lX not recognized)\n",
-					dev->name,
-					*(unsigned long *)&tpl->MData[2+2]);
-			}
-			else
-			{
-				if(tms380tr_debug > 3)
-					printk(KERN_DEBUG "%s: Directed frame tx'd\n", 
-						dev->name);
-			}
-		}
-		else
-		{
-			if(!DIRECTED_FRAME(tpl))
-			{
-				if(tms380tr_debug > 3)
-					printk(KERN_DEBUG "%s: Broadcast frame tx'd\n",
-						dev->name);
-			}
-		}
-
-		tp->MacStat.tx_packets++;
-		if (tpl->DMABuff)
-			dma_unmap_single(tp->pdev, tpl->DMABuff, tpl->Skb->len, DMA_TO_DEVICE);
-		dev_kfree_skb_irq(tpl->Skb);
-		tpl->BusyFlag = 0;	/* "free" TPL */
-	}
-
-	if(!tp->TplFree->NextTPLPtr->BusyFlag)
-		netif_wake_queue(dev);
-}
-
-/*
- * Called if a frame receive interrupt is generated by the adapter.
- * Check if the frame is valid and indicate it to system.
- */
-static void tms380tr_rcv_status_irq(struct net_device *dev)
-{
-	struct net_local *tp = netdev_priv(dev);
-	unsigned char *ReceiveDataPtr;
-	struct sk_buff *skb;
-	unsigned int Length, Length2;
-	RPL *rpl;
-	RPL *SaveHead;
-	dma_addr_t dmabuf;
-
-	/* NOTE: At this point the SSB from RECEIVE STATUS is no longer
-	 * available, because the CLEAR SSB command has already been issued.
-	 *
-	 * Process all complete receives.
-	 */
-
-	for(;;)
-	{
-		rpl = tp->RplHead;
-		if(rpl->Status & RX_VALID)
-			break;		/* RPL still in use by adapter */
-
-		/* Forward RPLHead pointer to next list. */
-		SaveHead = tp->RplHead;
-		tp->RplHead = rpl->NextRPLPtr;
-
-		/* Get the frame size (Byte swap for Intel).
-		 * Do this early (see workaround comment below)
-		 */
-		Length = be16_to_cpu(rpl->FrameSize);
-
-		/* Check if the Frame_Start, Frame_End and
-		 * Frame_Complete bits are set.
-		 */
-		if((rpl->Status & VALID_SINGLE_BUFFER_FRAME)
-			== VALID_SINGLE_BUFFER_FRAME)
-		{
-			ReceiveDataPtr = rpl->MData;
-
-			/* Workaround for delayed write of FrameSize on ISA
-			 * (FrameSize is false but valid-bit is reset)
-			 * Frame size is set to zero when the RPL is freed.
-			 * Length2 is there because there have also been
-			 * cases where the FrameSize was partially written
-			 */
-			Length2 = be16_to_cpu(rpl->FrameSize);
-
-			if(Length == 0 || Length != Length2)
-			{
-				tp->RplHead = SaveHead;
-				break;	/* Return to tms380tr_interrupt */
-			}
-			tms380tr_update_rcv_stats(tp,ReceiveDataPtr,Length);
-			  
-			if(tms380tr_debug > 3)
-				printk(KERN_DEBUG "%s: Packet Length %04X (%d)\n",
-					dev->name, Length, Length);
-			  
-			/* Indicate the received frame to system the
-			 * adapter does the Source-Routing padding for 
-			 * us. See: OpenOptions in tms380tr_init_opb()
-			 */
-			skb = rpl->Skb;
-			if(rpl->SkbStat == SKB_UNAVAILABLE)
-			{
-				/* Try again to allocate skb */
-				skb = dev_alloc_skb(tp->MaxPacketSize);
-				if(skb == NULL)
-				{
-					/* Update Stats ?? */
-				}
-				else
-				{
-					skb_put(skb, tp->MaxPacketSize);
-					rpl->SkbStat 	= SKB_DATA_COPY;
-					ReceiveDataPtr 	= rpl->MData;
-				}
-			}
-
-			if(skb && (rpl->SkbStat == SKB_DATA_COPY ||
-				   rpl->SkbStat == SKB_DMA_DIRECT))
-			{
-				if(rpl->SkbStat == SKB_DATA_COPY)
-					skb_copy_to_linear_data(skb, ReceiveDataPtr,
-						       Length);
-
-				/* Deliver frame to system */
-				rpl->Skb = NULL;
-				skb_trim(skb,Length);
-				skb->protocol = tr_type_trans(skb,dev);
-				netif_rx(skb);
-			}
-		}
-		else	/* Invalid frame */
-		{
-			if(rpl->Skb != NULL)
-				dev_kfree_skb_irq(rpl->Skb);
-
-			/* Skip list. */
-			if(rpl->Status & RX_START_FRAME)
-				/* Frame start bit is set -> overflow. */
-				tp->MacStat.rx_errors++;
-		}
-		if (rpl->DMABuff)
-			dma_unmap_single(tp->pdev, rpl->DMABuff, tp->MaxPacketSize, DMA_TO_DEVICE);
-		rpl->DMABuff = 0;
-
-		/* Allocate new skb for rpl */
-		rpl->Skb = dev_alloc_skb(tp->MaxPacketSize);
-		/* skb == NULL ? then use local buffer */
-		if(rpl->Skb == NULL)
-		{
-			rpl->SkbStat = SKB_UNAVAILABLE;
-			rpl->FragList[0].DataAddr = htonl(((char *)tp->LocalRxBuffers[rpl->RPLIndex] - (char *)tp) + tp->dmabuffer);
-			rpl->MData = tp->LocalRxBuffers[rpl->RPLIndex];
-		}
-		else	/* skb != NULL */
-		{
-			rpl->Skb->dev = dev;
-			skb_put(rpl->Skb, tp->MaxPacketSize);
-
-			/* Data unreachable for DMA ? then use local buffer */
-			dmabuf = dma_map_single(tp->pdev, rpl->Skb->data, tp->MaxPacketSize, DMA_FROM_DEVICE);
-			if(tp->dmalimit && (dmabuf + tp->MaxPacketSize > tp->dmalimit))
-			{
-				rpl->SkbStat = SKB_DATA_COPY;
-				rpl->FragList[0].DataAddr = htonl(((char *)tp->LocalRxBuffers[rpl->RPLIndex] - (char *)tp) + tp->dmabuffer);
-				rpl->MData = tp->LocalRxBuffers[rpl->RPLIndex];
-			}
-			else
-			{
-				/* DMA directly in skb->data */
-				rpl->SkbStat = SKB_DMA_DIRECT;
-				rpl->FragList[0].DataAddr = htonl(dmabuf);
-				rpl->MData = rpl->Skb->data;
-				rpl->DMABuff = dmabuf;
-			}
-		}
-
-		rpl->FragList[0].DataCount = cpu_to_be16((unsigned short)tp->MaxPacketSize);
-		rpl->FrameSize = 0;
-
-		/* Pass the last RPL back to the adapter */
-		tp->RplTail->FrameSize = 0;
-
-		/* Reset the CSTAT field in the list. */
-		tms380tr_write_rpl_status(tp->RplTail, RX_VALID | RX_FRAME_IRQ);
-
-		/* Current RPL becomes last one in list. */
-		tp->RplTail = tp->RplTail->NextRPLPtr;
-
-		/* Inform adapter about RPL valid. */
-		tms380tr_exec_sifcmd(dev, CMD_RX_VALID);
-	}
-}
-
-/*
- * This function should be used whenever the status of any RPL must be
- * modified by the driver, because the compiler may otherwise change the
- * order of instructions such that writing the RPL status may be executed
- * at an undesirable time. When this function is used, the status is
- * always written when the function is called.
- */
-static void tms380tr_write_rpl_status(RPL *rpl, unsigned int Status)
-{
-	rpl->Status = Status;
-}
-
-/*
- * The function updates the statistic counters in mac->MacStat.
- * It differtiates between directed and broadcast/multicast ( ==functional)
- * frames.
- */
-static void tms380tr_update_rcv_stats(struct net_local *tp, unsigned char DataPtr[],
-					unsigned int Length)
-{
-	tp->MacStat.rx_packets++;
-	tp->MacStat.rx_bytes += Length;
-	
-	/* Test functional bit */
-	if(DataPtr[2] & GROUP_BIT)
-		tp->MacStat.multicast++;
-}
-
-static int tms380tr_set_mac_address(struct net_device *dev, void *addr)
-{
-	struct net_local *tp = netdev_priv(dev);
-	struct sockaddr *saddr = addr;
-	
-	if (tp->AdapterOpenFlag || tp->AdapterVirtOpenFlag) {
-		printk(KERN_WARNING "%s: Cannot set MAC/LAA address while card is open\n", dev->name);
-		return -EIO;
-	}
-	memcpy(dev->dev_addr, saddr->sa_data, dev->addr_len);
-	return 0;
-}
-
-#if TMS380TR_DEBUG > 0
-/*
- * Dump Packet (data)
- */
-static void tms380tr_dump(unsigned char *Data, int length)
-{
-	int i, j;
-
-	for (i = 0, j = 0; i < length / 8; i++, j += 8)
-	{
-		printk(KERN_DEBUG "%02x %02x %02x %02x %02x %02x %02x %02x\n",
-		       Data[j+0],Data[j+1],Data[j+2],Data[j+3],
-		       Data[j+4],Data[j+5],Data[j+6],Data[j+7]);
-	}
-}
-#endif
-
-void tmsdev_term(struct net_device *dev)
-{
-	struct net_local *tp;
-
-	tp = netdev_priv(dev);
-	dma_unmap_single(tp->pdev, tp->dmabuffer, sizeof(struct net_local),
-		DMA_BIDIRECTIONAL);
-}
-
-const struct net_device_ops tms380tr_netdev_ops = {
-	.ndo_open		= tms380tr_open,
-	.ndo_stop		= tms380tr_close,
-	.ndo_start_xmit		= tms380tr_send_packet,
-	.ndo_tx_timeout		= tms380tr_timeout,
-	.ndo_get_stats		= tms380tr_get_stats,
-	.ndo_set_rx_mode	= tms380tr_set_multicast_list,
-	.ndo_set_mac_address	= tms380tr_set_mac_address,
-};
-EXPORT_SYMBOL(tms380tr_netdev_ops);
-
-int tmsdev_init(struct net_device *dev, struct device *pdev)
-{
-	struct net_local *tms_local;
-
-	memset(netdev_priv(dev), 0, sizeof(struct net_local));
-	tms_local = netdev_priv(dev);
-	init_waitqueue_head(&tms_local->wait_for_tok_int);
-	if (pdev->dma_mask)
-		tms_local->dmalimit = *pdev->dma_mask;
-	else
-		return -ENOMEM;
-	tms_local->pdev = pdev;
-	tms_local->dmabuffer = dma_map_single(pdev, (void *)tms_local,
-	    sizeof(struct net_local), DMA_BIDIRECTIONAL);
-	if (tms_local->dmabuffer + sizeof(struct net_local) > 
-			tms_local->dmalimit)
-	{
-		printk(KERN_INFO "%s: Memory not accessible for DMA\n",
-			dev->name);
-		tmsdev_term(dev);
-		return -ENOMEM;
-	}
-	
-	dev->netdev_ops		= &tms380tr_netdev_ops;
-	dev->watchdog_timeo	= HZ;
-
-	return 0;
-}
-
-EXPORT_SYMBOL(tms380tr_open);
-EXPORT_SYMBOL(tms380tr_close);
-EXPORT_SYMBOL(tms380tr_interrupt);
-EXPORT_SYMBOL(tmsdev_init);
-EXPORT_SYMBOL(tmsdev_term);
-EXPORT_SYMBOL(tms380tr_wait);
-
-#ifdef MODULE
-
-static struct module *TMS380_module = NULL;
-
-int init_module(void)
-{
-	printk(KERN_DEBUG "%s", version);
-	
-	TMS380_module = &__this_module;
-	return 0;
-}
-
-void cleanup_module(void)
-{
-	TMS380_module = NULL;
-}
-#endif
-
-MODULE_LICENSE("GPL");
-
diff --git a/drivers/net/tokenring/tms380tr.h b/drivers/net/tokenring/tms380tr.h
deleted file mode 100644
index e5a617c586c..00000000000
--- a/drivers/net/tokenring/tms380tr.h
+++ /dev/null
@@ -1,1141 +0,0 @@
-/* 
- * tms380tr.h: TI TMS380 Token Ring driver for Linux
- *
- * Authors:
- * - Christoph Goos <cgoos@syskonnect.de>
- * - Adam Fritzler
- */
-
-#ifndef __LINUX_TMS380TR_H
-#define __LINUX_TMS380TR_H
-
-#ifdef __KERNEL__
-
-#include <linux/interrupt.h>
-
-/* module prototypes */
-extern const struct net_device_ops tms380tr_netdev_ops;
-int tms380tr_open(struct net_device *dev);
-int tms380tr_close(struct net_device *dev);
-irqreturn_t tms380tr_interrupt(int irq, void *dev_id);
-int tmsdev_init(struct net_device *dev, struct device *pdev);
-void tmsdev_term(struct net_device *dev);
-void tms380tr_wait(unsigned long time);
-
-#define TMS380TR_MAX_ADAPTERS 7
-
-#define SEND_TIMEOUT 10*HZ
-
-#define TR_RCF_LONGEST_FRAME_MASK 0x0070
-#define TR_RCF_FRAME4K 0x0030
-
-/*------------------------------------------------------------------*/
-/*  Bit order for adapter communication with DMA		    */
-/*  --------------------------------------------------------------  */
-/*  Bit  8 | 9| 10| 11|| 12| 13| 14| 15|| 0| 1| 2| 3|| 4| 5| 6| 7|  */
-/*  --------------------------------------------------------------  */
-/*  The bytes in a word must be byte swapped. Also, if a double	    */
-/*  word is used for storage, then the words, as well as the bytes, */
-/*  must be swapped. 						    */
-/*  Bit order for adapter communication with DIO 		    */
-/*  --------------------------------------------------------------  */
-/*  Bit  0 | 1| 2| 3|| 4| 5| 6| 7|| 8| 9| 10| 11|| 12| 13| 14| 15|  */
-/*  --------------------------------------------------------------  */
-/*------------------------------------------------------------------*/
-
-/* Swap words of a long.                        */
-#define SWAPW(x) (((x) << 16) | ((x) >> 16))
-
-/* Get the low byte of a word.                      */
-#define LOBYTE(w)       ((unsigned char)(w))
-
-/* Get the high byte of a word.                     */
-#define HIBYTE(w)       ((unsigned char)((unsigned short)(w) >> 8))
-
-/* Get the low word of a long.                      */
-#define LOWORD(l)       ((unsigned short)(l))
-
-/* Get the high word of a long.                     */
-#define HIWORD(l)       ((unsigned short)((unsigned long)(l) >> 16))
-
-
-
-/* Token ring adapter I/O addresses for normal mode. */
-
-/*
- * The SIF registers.  Common to all adapters.
- */
-/* Basic SIF (SRSX = 0) */
-#define SIFDAT      		0x00	/* SIF/DMA data. */
-#define SIFINC      		0x02  	/* IO Word data with auto increment. */
-#define SIFINH      		0x03  	/* IO Byte data with auto increment. */
-#define SIFADR      		0x04  	/* SIF/DMA Address. */
-#define SIFCMD      		0x06  	/* SIF Command. */
-#define SIFSTS      		0x06  	/* SIF Status. */
-
-/* "Extended" SIF (SRSX = 1) */
-#define SIFACL      		0x08  	/* SIF Adapter Control Register. */
-#define SIFADD      		0x0a 	/* SIF/DMA Address. -- 0x0a */
-#define SIFADX      		0x0c     /* 0x0c */
-#define DMALEN      		0x0e 	/* SIF DMA length. -- 0x0e */
-
-/*
- * POS Registers.  Only for ISA Adapters.
- */
-#define POSREG      		0x10 	/* Adapter Program Option Select (POS)
-			 		 * Register: base IO address + 16 byte.
-			 		 */
-#define POSREG_2    		24L 	/* only for TR4/16+ adapter
-			 		 * base IO address + 24 byte. -- 0x18
-			 		 */
-
-/* SIFCMD command codes (high-low) */
-#define CMD_INTERRUPT_ADAPTER   0x8000  /* Cause internal adapter interrupt */
-#define CMD_ADAPTER_RESET   	0x4000  /* Hardware reset of adapter */
-#define CMD_SSB_CLEAR		0x2000  /* Acknowledge to adapter to
-					 * system interrupts.
-					 */
-#define CMD_EXECUTE		0x1000	/* Execute SCB command */
-#define CMD_SCB_REQUEST		0x0800  /* Request adapter to interrupt
-					 * system when SCB is available for
-					 * another command.
-					 */
-#define CMD_RX_CONTINUE		0x0400  /* Continue receive after odd pointer
-					 * stop. (odd pointer receive method)
-					 */
-#define CMD_RX_VALID		0x0200  /* Now actual RPL is valid. */
-#define CMD_TX_VALID		0x0100  /* Now actual TPL is valid. (valid
-					 * bit receive/transmit method)
-					 */
-#define CMD_SYSTEM_IRQ		0x0080  /* Adapter-to-attached-system
-					 * interrupt is reset.
-					 */
-#define CMD_CLEAR_SYSTEM_IRQ	0x0080	/* Clear SYSTEM_INTERRUPT bit.
-					 * (write: 1=ignore, 0=reset)
-					 */
-#define EXEC_SOFT_RESET		0xFF00  /* adapter soft reset. (restart
-					 * adapter after hardware reset)
-					 */
-
-
-/* ACL commands (high-low) */
-#define ACL_SWHLDA		0x0800  /* Software hold acknowledge. */
-#define ACL_SWDDIR		0x0400  /* Data transfer direction. */
-#define ACL_SWHRQ		0x0200  /* Pseudo DMA operation. */
-#define ACL_PSDMAEN		0x0100  /* Enable pseudo system DMA. */
-#define ACL_ARESET		0x0080  /* Adapter hardware reset command.
-					 * (held in reset condition as
-					 * long as bit is set)
-					 */
-#define ACL_CPHALT		0x0040  /* Communication processor halt.
-					 * (can only be set while ACL_ARESET
-					 * bit is set; prevents adapter
-					 * processor from executing code while
-					 * downloading firmware)
-					 */
-#define ACL_BOOT		0x0020
-#define ACL_SINTEN		0x0008  /* System interrupt enable/disable
-					 * (1/0): can be written if ACL_ARESET
-					 * is zero.
-					 */
-#define ACL_PEN                 0x0004
-
-#define ACL_NSELOUT0            0x0002 
-#define ACL_NSELOUT1            0x0001	/* NSELOUTx have a card-specific
-					 * meaning for setting ring speed.
-					 */
-
-#define PS_DMA_MASK		(ACL_SWHRQ | ACL_PSDMAEN)
-
-
-/* SIFSTS register return codes (high-low) */
-#define STS_SYSTEM_IRQ		0x0080	/* Adapter-to-attached-system
-					 * interrupt is valid.
-					 */
-#define STS_INITIALIZE		0x0040  /* INITIALIZE status. (ready to
-					 * initialize)
-					 */
-#define STS_TEST		0x0020  /* TEST status. (BUD not completed) */
-#define STS_ERROR		0x0010  /* ERROR status. (unrecoverable
-					 * HW error occurred)
-					 */
-#define STS_MASK		0x00F0  /* Mask interesting status bits. */
-#define STS_ERROR_MASK		0x000F  /* Get Error Code by masking the
-					 * interrupt code bits.
-					 */
-#define ADAPTER_INT_PTRS	0x0A00  /* Address offset of adapter internal
-					 * pointers 01:0a00 (high-low) have to
-					 * be read after init and before open.
-					 */
-
-
-/* Interrupt Codes (only MAC IRQs) */
-#define STS_IRQ_ADAPTER_CHECK	0x0000	/* unrecoverable hardware or
-					 * software error.
-					 */ 
-#define STS_IRQ_RING_STATUS	0x0004  /* SSB is updated with ring status. */
-#define STS_IRQ_LLC_STATUS	0x0005	/* Not used in MAC-only microcode */
-#define STS_IRQ_SCB_CLEAR	0x0006	/* SCB clear, following an
-					 * SCB_REQUEST IRQ.
-					 */
-#define STS_IRQ_TIMER		0x0007	/* Not normally used in MAC ucode */
-#define STS_IRQ_COMMAND_STATUS	0x0008	/* SSB is updated with command 
-					 * status.
-					 */ 
-#define STS_IRQ_RECEIVE_STATUS	0x000A	/* SSB is updated with receive
-					 * status.
-					 */
-#define STS_IRQ_TRANSMIT_STATUS	0x000C	/* SSB is updated with transmit
-                                         * status
-					 */
-#define STS_IRQ_RECEIVE_PENDING	0x000E	/* Not used in MAC-only microcode */
-#define STS_IRQ_MASK		0x000F	/* = STS_ERROR_MASK. */
-
-
-/* TRANSMIT_STATUS completion code: (SSB.Parm[0]) */
-#define COMMAND_COMPLETE	0x0080	/* TRANSMIT command completed
-                                         * (avoid this!) issue another transmit
-					 * to send additional frames.
-					 */
-#define FRAME_COMPLETE		0x0040	/* Frame has been transmitted;
-					 * INTERRUPT_FRAME bit was set in the
-					 * CSTAT request; indication of possibly
-					 * more than one frame transmissions!
-					 * SSB.Parm[0-1]: 32 bit pointer to
-					 * TPL of last frame.
-					 */
-#define LIST_ERROR		0x0020	/* Error in one of the TPLs that
-					 * compose the frame; TRANSMIT
-					 * terminated; Parm[1-2]: 32bit pointer
-					 * to TPL which starts the error
-					 * frame; error details in bits 8-13.
-					 * (14?)
-					 */
-#define FRAME_SIZE_ERROR	0x8000	/* FRAME_SIZE does not equal the sum of
-					 * the valid DATA_COUNT fields;
-					 * FRAME_SIZE less than header plus
-					 * information field. (15 bytes +
-					 * routing field) Or if FRAME_SIZE
-					 * was specified as zero in one list.
-					 */
-#define TX_THRESHOLD		0x4000	/* FRAME_SIZE greater than (BUFFER_SIZE
-					 * - 9) * TX_BUF_MAX.
-					 */
-#define ODD_ADDRESS		0x2000	/* Odd forward pointer value is
-					 * read on a list without END_FRAME
-					 * indication.
-					 */
-#define FRAME_ERROR		0x1000	/* START_FRAME bit (not) anticipated,
-					 * but (not) set.
-					 */
-#define ACCESS_PRIORITY_ERROR	0x0800	/* Access priority requested has not
-					 * been allowed.
-					 */
-#define UNENABLED_MAC_FRAME	0x0400	/* MAC frame has source class of zero
-					 * or MAC frame PCF ATTN field is
-					 * greater than one.
-					 */
-#define ILLEGAL_FRAME_FORMAT	0x0200	/* Bit 0 or FC field was set to one. */
-
-
-/*
- * Since we need to support some functions even if the adapter is in a
- * CLOSED state, we have a (pseudo-) command queue which holds commands
- * that are outstandig to be executed.
- *
- * Each time a command completes, an interrupt occurs and the next
- * command is executed. The command queue is actually a simple word with 
- * a bit for each outstandig command. Therefore the commands will not be
- * executed in the order they have been queued.
- *
- * The following defines the command code bits and the command queue:
- */
-#define OC_OPEN			0x0001	/* OPEN command */
-#define OC_TRANSMIT		0x0002	/* TRANSMIT command */
-#define OC_TRANSMIT_HALT	0x0004	/* TRANSMIT_HALT command */
-#define OC_RECEIVE		0x0008	/* RECEIVE command */
-#define OC_CLOSE		0x0010	/* CLOSE command */
-#define OC_SET_GROUP_ADDR	0x0020	/* SET_GROUP_ADDR command */
-#define OC_SET_FUNCT_ADDR	0x0040	/* SET_FUNCT_ADDR command */
-#define OC_READ_ERROR_LOG	0x0080	/* READ_ERROR_LOG command */
-#define OC_READ_ADAPTER		0x0100	/* READ_ADAPTER command */
-#define OC_MODIFY_OPEN_PARMS	0x0400	/* MODIFY_OPEN_PARMS command */
-#define OC_RESTORE_OPEN_PARMS	0x0800	/* RESTORE_OPEN_PARMS command */
-#define OC_SET_FIRST_16_GROUP	0x1000	/* SET_FIRST_16_GROUP command */
-#define OC_SET_BRIDGE_PARMS	0x2000	/* SET_BRIDGE_PARMS command */
-#define OC_CONFIG_BRIDGE_PARMS	0x4000	/* CONFIG_BRIDGE_PARMS command */
-
-#define OPEN			0x0300	/* C: open command. S: completion. */
-#define TRANSMIT		0x0400	/* C: transmit command. S: completion
-					 * status. (reject: COMMAND_REJECT if
-					 * adapter not opened, TRANSMIT already
-					 * issued or address passed in the SCB
-					 * not word aligned)
-					 */
-#define TRANSMIT_HALT		0x0500	/* C: interrupt TX TPL chain; if no
-					 * TRANSMIT command issued, the command
-					 * is ignored (completion with TRANSMIT
-					 * status (0x0400)!)
-					 */
-#define RECEIVE			0x0600	/* C: receive command. S: completion
-					 * status. (reject: COMMAND_REJECT if
-					 * adapter not opened, RECEIVE already
-					 * issued or address passed in the SCB 
-					 * not word aligned)
-					 */
-#define CLOSE			0x0700	/* C: close adapter. S: completion.
-					 * (COMMAND_REJECT if adapter not open)
-					 */
-#define SET_GROUP_ADDR		0x0800	/* C: alter adapter group address after
-					 * OPEN. S: completion. (COMMAND_REJECT
-					 * if adapter not open)
-					 */
-#define SET_FUNCT_ADDR		0x0900	/* C: alter adapter functional address
-					 * after OPEN. S: completion.
-					 * (COMMAND_REJECT if adapter not open)
-					 */
-#define READ_ERROR_LOG		0x0A00	/* C: read adapter error counters.
-					 * S: completion. (command ignored
-					 * if adapter not open!)
-					 */
-#define READ_ADAPTER		0x0B00	/* C: read data from adapter memory.
-					 * (important: after init and before
-					 * open!) S: completion. (ADAPTER_CHECK
-					 * interrupt if undefined storage area
-					 * read)
-					 */
-#define MODIFY_OPEN_PARMS	0x0D00	/* C: modify some adapter operational
-					 * parameters. (bit correspondend to
-					 * WRAP_INTERFACE is ignored)
-					 * S: completion. (reject: 
-					 * COMMAND_REJECT)
-					 */
-#define RESTORE_OPEN_PARMS	0x0E00	/* C: modify some adapter operational
-					 * parameters. (bit correspondend
-					 * to WRAP_INTERFACE is ignored)
-					 * S: completion. (reject:
-					 * COMMAND_REJECT)
-					 */
-#define SET_FIRST_16_GROUP	0x0F00	/* C: alter the first two bytes in
-					 * adapter group address.
-					 * S: completion. (reject:
-					 * COMMAND_REJECT)
-					 */
-#define SET_BRIDGE_PARMS	0x1000	/* C: values and conditions for the
-					 * adapter hardware to use when frames
-					 * are copied for forwarding.
-					 * S: completion. (reject:
-					 * COMMAND_REJECT)
-					 */
-#define CONFIG_BRIDGE_PARMS	0x1100	/* C: ..
-					 * S: completion. (reject:
-					 * COMMAND_REJECT)
-					 */
-
-#define SPEED_4			4
-#define SPEED_16		16	/* Default transmission speed  */
-
-
-/* Initialization Parameter Block (IPB); word alignment necessary! */
-#define BURST_SIZE	0x0018	/* Default burst size */
-#define BURST_MODE	0x9F00	/* Burst mode enable */
-#define DMA_RETRIES	0x0505	/* Magic DMA retry number... */
-
-#define CYCLE_TIME	3	/* Default AT-bus cycle time: 500 ns
-				 * (later adapter version: fix  cycle time!)
-				 */
-#define LINE_SPEED_BIT	0x80
-
-/* Macro definition for the wait function. */
-#define ONE_SECOND_TICKS	1000000
-#define HALF_SECOND		(ONE_SECOND_TICKS / 2)
-#define ONE_SECOND		(ONE_SECOND_TICKS)
-#define TWO_SECONDS		(ONE_SECOND_TICKS * 2)
-#define THREE_SECONDS		(ONE_SECOND_TICKS * 3)
-#define FOUR_SECONDS		(ONE_SECOND_TICKS * 4)
-#define FIVE_SECONDS		(ONE_SECOND_TICKS * 5)
-
-#define BUFFER_SIZE 		2048	/* Buffers on Adapter */
-
-#pragma pack(1)
-typedef struct {
-	unsigned short Init_Options;	/* Initialize with burst mode;
-					 * LLC disabled. (MAC only)
-					 */
-
-	/* Interrupt vectors the adapter places on attached system bus. */
-	u_int8_t  CMD_Status_IV;    /* Interrupt vector: command status. */
-	u_int8_t  TX_IV;	    /* Interrupt vector: transmit. */
-	u_int8_t  RX_IV;	    /* Interrupt vector: receive. */
-	u_int8_t  Ring_Status_IV;   /* Interrupt vector: ring status. */
-	u_int8_t  SCB_Clear_IV;	    /* Interrupt vector: SCB clear. */
-	u_int8_t  Adapter_CHK_IV;   /* Interrupt vector: adapter check. */
-
-	u_int16_t RX_Burst_Size;    /* Max. number of transfer cycles. */
-	u_int16_t TX_Burst_Size;    /* During DMA burst; even value! */
-	u_int16_t DMA_Abort_Thrhld; /* Number of DMA retries. */
-
-	u_int32_t SCB_Addr;   /* SCB address: even, word aligned, high-low */
-	u_int32_t SSB_Addr;   /* SSB address: even, word aligned, high-low */
-} IPB, *IPB_Ptr;
-#pragma pack()
-
-/*
- * OPEN Command Parameter List (OCPL) (can be reused, if the adapter has to
- * be reopened)
- */
-#define BUFFER_SIZE	2048		/* Buffers on Adapter. */
-#define TPL_SIZE	8+6*TX_FRAG_NUM /* Depending on fragments per TPL. */
-#define RPL_SIZE	14		/* (with TI firmware v2.26 handling
-					 * up to nine fragments possible)
-					 */
-#define TX_BUF_MIN	20		/* ??? (Stephan: calculation with */
-#define TX_BUF_MAX	40		/* BUFFER_SIZE and MAX_FRAME_SIZE) ??? 
-					 */
-#define DISABLE_EARLY_TOKEN_RELEASE 	0x1000
-
-/* OPEN Options (high-low) */
-#define WRAP_INTERFACE		0x0080	/* Inserting omitted for test
-					 * purposes; transmit data appears
-					 * as receive data. (useful for
-					 * testing; change: CLOSE necessary)
-					 */
-#define DISABLE_HARD_ERROR	0x0040	/* On HARD_ERROR & TRANSMIT_BEACON
-					 * no RING.STATUS interrupt.
-					 */
-#define DISABLE_SOFT_ERROR	0x0020	/* On SOFT_ERROR, no RING.STATUS
-					 * interrupt.
-					 */
-#define PASS_ADAPTER_MAC_FRAMES	0x0010	/* Passing unsupported MAC frames
-					 * to system.
-					 */
-#define PASS_ATTENTION_FRAMES	0x0008	/* All changed attention MAC frames are
-					 * passed to the system.
-					 */
-#define PAD_ROUTING_FIELD	0x0004	/* Routing field is padded to 18
-					 * bytes.
-					 */
-#define FRAME_HOLD		0x0002	/*Adapter waits for entire frame before
-					 * initiating DMA transfer; otherwise:
-					 * DMA transfer initiation if internal
-					 * buffer filled.
-					 */
-#define CONTENDER		0x0001	/* Adapter participates in the monitor
-					 * contention process.
-					 */
-#define PASS_BEACON_MAC_FRAMES	0x8000	/* Adapter passes beacon MAC frames
-					 * to the system.
-					 */
-#define EARLY_TOKEN_RELEASE 	0x1000	/* Only valid in 16 Mbps operation;
-					 * 0 = ETR. (no effect in 4 Mbps
-					 * operation)
-					 */
-#define COPY_ALL_MAC_FRAMES	0x0400	/* All MAC frames are copied to
-					 * the system. (after OPEN: duplicate
-					 * address test (DAT) MAC frame is 
-					 * first received frame copied to the
-					 * system)
-					 */
-#define COPY_ALL_NON_MAC_FRAMES	0x0200	/* All non MAC frames are copied to
-					 * the system.
-					 */
-#define PASS_FIRST_BUF_ONLY	0x0100	/* Passes only first internal buffer
-					 * of each received frame; FrameSize
-					 * of RPLs must contain internal
-					 * BUFFER_SIZE bits for promiscuous mode.
-					 */
-#define ENABLE_FULL_DUPLEX_SELECTION	0x2000 
- 					/* Enable the use of full-duplex
-					 * settings with bits in byte 22 in
-					 * ocpl. (new feature in firmware
-					 * version 3.09)
-					 */
-
-/* Full-duplex settings */
-#define OPEN_FULL_DUPLEX_OFF	0x0000
-#define OPEN_FULL_DUPLEX_ON	0x00c0
-#define OPEN_FULL_DUPLEX_AUTO	0x0080
-
-#define PROD_ID_SIZE	18	/* Length of product ID. */
-
-#define TX_FRAG_NUM	3	 /* Number of fragments used in one TPL. */
-#define TX_MORE_FRAGMENTS 0x8000 /* Bit set in DataCount to indicate more
-				  * fragments following.
-				  */
-
-/* XXX is there some better way to do this? */
-#define ISA_MAX_ADDRESS 	0x00ffffff
-#define PCI_MAX_ADDRESS		0xffffffff
-
-#pragma pack(1)
-typedef struct {
-	u_int16_t OPENOptions;
-	u_int8_t  NodeAddr[6];	/* Adapter node address; use ROM 
-				 * address
-				 */
-	u_int32_t GroupAddr;	/* Multicast: high order
-				 * bytes = 0xC000
-				 */
-	u_int32_t FunctAddr;	/* High order bytes = 0xC000 */
-	__be16 RxListSize;	/* RPL size: 0 (=26), 14, 20 or
-				 * 26 bytes read by the adapter.
-				 * (Depending on the number of 
-				 * fragments/list)
-				 */
-	__be16 TxListSize;	/* TPL size */
-	__be16 BufSize;		/* Is automatically rounded up to the
-				 * nearest nK boundary.
-				 */
-	u_int16_t FullDuplex;
-	u_int16_t Reserved;
-	u_int8_t  TXBufMin;	/* Number of adapter buffers reserved
-				 * for transmission a minimum of 2
-				 * buffers must be allocated.
-				 */
-	u_int8_t  TXBufMax;	/* Maximum number of adapter buffers
-				 * for transmit; a minimum of 2 buffers
-				 * must be available for receive.
-				 * Default: 6
-				 */
-	u_int16_t ProdIDAddr[2];/* Pointer to product ID. */
-} OPB, *OPB_Ptr;
-#pragma pack()
-
-/*
- * SCB: adapter commands enabled by the host system started by writing
- * CMD_INTERRUPT_ADAPTER | CMD_EXECUTE (|SCB_REQUEST) to the SIFCMD IO
- * register. (special case: | CMD_SYSTEM_IRQ for initialization)
- */
-#pragma pack(1)
-typedef struct {
-	u_int16_t CMD;		/* Command code */
-	u_int16_t Parm[2];	/* Pointer to Command Parameter Block */
-} SCB;	/* System Command Block (32 bit physical address; big endian)*/
-#pragma pack()
-
-/*
- * SSB: adapter command return status can be evaluated after COMMAND_STATUS
- * adapter to system interrupt after reading SSB, the availability of the SSB
- * has to be told the adapter by writing CMD_INTERRUPT_ADAPTER | CMD_SSB_CLEAR
- * in the SIFCMD IO register.
- */
-#pragma pack(1)
-typedef struct {
-	u_int16_t STS;		/* Status code */
-	u_int16_t Parm[3];	/* Parameter or pointer to Status Parameter
-				 * Block.
-				 */
-} SSB;	/* System Status Block (big endian - physical address)  */
-#pragma pack()
-
-typedef struct {
-	unsigned short BurnedInAddrPtr;	/* Pointer to adapter burned in
-					 * address. (BIA)
-					 */
-	unsigned short SoftwareLevelPtr;/* Pointer to software level data. */
-	unsigned short AdapterAddrPtr;	/* Pointer to adapter addresses. */
-	unsigned short AdapterParmsPtr;	/* Pointer to adapter parameters. */
-	unsigned short MACBufferPtr;	/* Pointer to MAC buffer. (internal) */
-	unsigned short LLCCountersPtr;	/* Pointer to LLC counters.  */
-	unsigned short SpeedFlagPtr;	/* Pointer to data rate flag.
-					 * (4/16 Mbps)
-					 */
-	unsigned short AdapterRAMPtr;	/* Pointer to adapter RAM found. (KB) */
-} INTPTRS;	/* Adapter internal pointers */
-
-#pragma pack(1)
-typedef struct {
-	u_int8_t  Line_Error;		/* Line error: code violation in
-					 * frame or in a token, or FCS error.
-					 */
-	u_int8_t  Internal_Error;	/* IBM specific. (Reserved_1) */
-	u_int8_t  Burst_Error;
-	u_int8_t  ARI_FCI_Error;	/* ARI/FCI bit zero in AMP or
-					 * SMP MAC frame.
-					 */
-	u_int8_t  AbortDelimeters;	/* IBM specific. (Reserved_2) */
-	u_int8_t  Reserved_3;
-	u_int8_t  Lost_Frame_Error;	/* Receive of end of transmitted
-					 * frame failed.
-					 */
-	u_int8_t  Rx_Congest_Error;	/* Adapter in repeat mode has not
-					 * enough buffer space to copy incoming
-					 * frame.
-					 */
-	u_int8_t  Frame_Copied_Error;	/* ARI bit not zero in frame
-					 * addressed to adapter.
-					 */
-	u_int8_t  Frequency_Error;	/* IBM specific. (Reserved_4) */
-	u_int8_t  Token_Error;		/* (active only in monitor station) */
-	u_int8_t  Reserved_5;
-	u_int8_t  DMA_Bus_Error;	/* DMA bus errors not exceeding the
-					 * abort thresholds.
-					 */
-	u_int8_t  DMA_Parity_Error;	/* DMA parity errors not exceeding
-					 * the abort thresholds.
-					 */
-} ERRORTAB;	/* Adapter error counters */
-#pragma pack()
-
-
-/*--------------------- Send and Receive definitions -------------------*/
-#pragma pack(1)
-typedef struct {
-	__be16 DataCount;	/* Value 0, even and odd values are
-				 * permitted; value is unaltered most
-				 * significant bit set: following
-				 * fragments last fragment: most
-				 * significant bit is not evaluated.
-				 * (???)
-				 */
-	__be32 DataAddr;	/* Pointer to frame data fragment;
-				 * even or odd.
-				 */
-} Fragment;
-#pragma pack()
-
-#define MAX_FRAG_NUMBERS    9	/* Maximal number of fragments possible to use
-				 * in one RPL/TPL. (depending on TI firmware 
-				 * version)
-				 */
-
-/*
- * AC (1), FC (1), Dst (6), Src (6), RIF (18), Data (4472) = 4504
- * The packet size can be one of the follows: 548, 1502, 2084, 4504, 8176,
- * 11439, 17832. Refer to TMS380 Second Generation Token Ring User's Guide
- * Page 2-27.
- */
-#define HEADER_SIZE		(1 + 1 + 6 + 6)
-#define SRC_SIZE		18
-#define MIN_DATA_SIZE		516
-#define DEFAULT_DATA_SIZE	4472
-#define MAX_DATA_SIZE		17800
-
-#define DEFAULT_PACKET_SIZE (HEADER_SIZE + SRC_SIZE + DEFAULT_DATA_SIZE)
-#define MIN_PACKET_SIZE     (HEADER_SIZE + SRC_SIZE + MIN_DATA_SIZE)
-#define MAX_PACKET_SIZE     (HEADER_SIZE + SRC_SIZE + MAX_DATA_SIZE)
-
-/*
- * Macros to deal with the frame status field.
- */
-#define AC_NOT_RECOGNIZED	0x00
-#define GROUP_BIT		0x80
-#define GET_TRANSMIT_STATUS_HIGH_BYTE(Ts) ((unsigned char)((Ts) >> 8))
-#define GET_FRAME_STATUS_HIGH_AC(Fs)	  ((unsigned char)(((Fs) & 0xC0) >> 6))
-#define GET_FRAME_STATUS_LOW_AC(Fs)       ((unsigned char)(((Fs) & 0x0C) >> 2))
-#define DIRECTED_FRAME(Context)           (!((Context)->MData[2] & GROUP_BIT))
-
-
-/*--------------------- Send Functions ---------------------------------*/
-/* define TX_CSTAT _REQUEST (R) and _COMPLETE (C) values (high-low) */
-
-#define TX_VALID		0x0080	/* R: set via TRANSMIT.VALID interrupt.
-					 * C: always reset to zero!
-					 */
-#define TX_FRAME_COMPLETE	0x0040	/* R: must be reset to zero.
-					 * C: set to one.
-					 */
-#define TX_START_FRAME		0x0020  /* R: start of a frame: 1 
-					 * C: unchanged.
-					 */
-#define TX_END_FRAME		0x0010  /* R: end of a frame: 1
-					 * C: unchanged.
-					 */
-#define TX_FRAME_IRQ		0x0008  /* R: request interrupt generation
-					 * after transmission.
-					 * C: unchanged.
-					 */
-#define TX_ERROR		0x0004  /* R: reserved.
-					 * C: set to one if Error occurred.
-					 */
-#define TX_INTERFRAME_WAIT	0x0004
-#define TX_PASS_CRC		0x0002  /* R: set if CRC value is already
-					 * calculated. (valid only in
-					 * FRAME_START TPL)
-					 * C: unchanged.
-					 */
-#define TX_PASS_SRC_ADDR	0x0001  /* R: adapter uses explicit frame
-					 * source address and does not overwrite
-					 * with the adapter node address.
-					 * (valid only in FRAME_START TPL)
-					 *
-					 * C: unchanged.
-					 */
-#define TX_STRIP_FS		0xFF00  /* R: reserved.
-					 * C: if no Transmission Error,
-					 * field contains copy of FS byte after
-					 * stripping of frame.
-					 */
-
-/*
- * Structure of Transmit Parameter Lists (TPLs) (only one frame every TPL,
- * but possibly multiple TPLs for one frame) the length of the TPLs has to be
- * initialized in the OPL. (OPEN parameter list)
- */
-#define TPL_NUM		3	/* Number of Transmit Parameter Lists.
-				 * !! MUST BE >= 3 !!
-				 */
-
-#pragma pack(1)
-typedef struct s_TPL TPL;
-
-struct s_TPL {	/* Transmit Parameter List (align on even word boundaries) */
-	__be32 NextTPLAddr;		/* Pointer to next TPL in chain; if
-					 * pointer is odd: this is the last
-					 * TPL. Pointing to itself can cause
-					 * problems!
-					 */
-	volatile u_int16_t Status;	/* Initialized by the adapter:
-					 * CSTAT_REQUEST important: update least
-					 * significant bit first! Set by the
-					 * adapter: CSTAT_COMPLETE status.
-					 */
-	__be16 FrameSize;		/* Number of bytes to be transmitted
-					 * as a frame including AC/FC,
-					 * Destination, Source, Routing field
-					 * not including CRC, FS, End Delimiter
-					 * (valid only if START_FRAME bit in 
-					 * CSTAT nonzero) must not be zero in
-					 * any list; maximum value: (BUFFER_SIZE
-					 * - 8) * TX_BUF_MAX sum of DataCount
-					 * values in FragmentList must equal
-					 * Frame_Size value in START_FRAME TPL!
-					 * frame data fragment list.
-					 */
-
-	/* TPL/RPL size in OPEN parameter list depending on maximal
-	 * numbers of fragments used in one parameter list.
-	 */
-	Fragment FragList[TX_FRAG_NUM];	/* Maximum: nine frame fragments in one
-					 * TPL actual version of firmware: 9
-					 * fragments possible.
-					 */
-#pragma pack()
-
-	/* Special proprietary data and precalculations */
-
-	TPL *NextTPLPtr;		/* Pointer to next TPL in chain. */
-	unsigned char *MData;
-	struct sk_buff *Skb;
-	unsigned char TPLIndex;
-	volatile unsigned char BusyFlag;/* Flag: TPL busy? */
-	dma_addr_t DMABuff;		/* DMA IO bus address from dma_map */
-};
-
-/* ---------------------Receive Functions-------------------------------*
- * define RECEIVE_CSTAT_REQUEST (R) and RECEIVE_CSTAT_COMPLETE (C) values.
- * (high-low)
- */
-#define RX_VALID		0x0080	/* R: set; tell adapter with
-					 * RECEIVE.VALID interrupt.
-					 * C: reset to zero.
-					 */
-#define RX_FRAME_COMPLETE	0x0040  /* R: must be reset to zero,
-					 * C: set to one.
-					 */
-#define RX_START_FRAME		0x0020  /* R: must be reset to zero.
-					 * C: set to one on the list.
-					 */
-#define RX_END_FRAME		0x0010  /* R: must be reset to zero.
-					 * C: set to one on the list
-					 * that ends the frame.
-					 */
-#define RX_FRAME_IRQ		0x0008  /* R: request interrupt generation
-					 * after receive.
-					 * C: unchanged.
-					 */
-#define RX_INTERFRAME_WAIT	0x0004  /* R: after receiving a frame:
-					 * interrupt and wait for a
-					 * RECEIVE.CONTINUE.
-					 * C: unchanged.
-					 */
-#define RX_PASS_CRC		0x0002  /* R: if set, the adapter includes
-					 * the CRC in data passed. (last four 
-					 * bytes; valid only if FRAME_START is
-					 * set)
-					 * C: set, if CRC is included in
-					 * received data.
-					 */
-#define RX_PASS_SRC_ADDR	0x0001  /* R: adapter uses explicit frame
-					 * source address and does not
-					 * overwrite with the adapter node
-					 * address. (valid only if FRAME_START
-					 * is set)
-					 * C: unchanged.
-					 */
-#define RX_RECEIVE_FS		0xFC00  /* R: reserved; must be reset to zero.
-					 * C: on lists with START_FRAME, field
-					 * contains frame status field from
-					 * received frame; otherwise cleared.
-					 */
-#define RX_ADDR_MATCH		0x0300  /* R: reserved; must be reset to zero.
-					 * C: address match code mask.
-					 */ 
-#define RX_STATUS_MASK		0x00FF  /* Mask for receive status bits. */
-
-#define RX_INTERN_ADDR_MATCH    0x0100  /* C: internally address match. */
-#define RX_EXTERN_ADDR_MATCH    0x0200  /* C: externally matched via
-					 * XMATCH/XFAIL interface.
-					 */
-#define RX_INTEXT_ADDR_MATCH    0x0300  /* C: internally and externally
-					 * matched.
-					 */
-#define RX_READY (RX_VALID | RX_FRAME_IRQ) /* Ready for receive. */
-
-/* Constants for Command Status Interrupt.
- * COMMAND_REJECT status field bit functions (SSB.Parm[0])
- */
-#define ILLEGAL_COMMAND		0x0080	/* Set if an unknown command
-					 * is issued to the adapter
-					 */
-#define ADDRESS_ERROR		0x0040  /* Set if any address field in
-					 * the SCB is odd. (not word aligned)
-					 */
-#define ADAPTER_OPEN		0x0020  /* Command issued illegal with
-					 * open adapter.
-					 */
-#define ADAPTER_CLOSE		0x0010  /* Command issued illegal with
-					 * closed adapter.
-					 */
-#define SAME_COMMAND		0x0008  /* Command issued with same command
-					 * already executing.
-					 */
-
-/* OPEN_COMPLETION values (SSB.Parm[0], MSB) */
-#define NODE_ADDR_ERROR		0x0040  /* Wrong address or BIA read
-					 * zero address.
-					 */
-#define LIST_SIZE_ERROR		0x0020  /* If List_Size value not in 0,
-					 * 14, 20, 26.
-					 */
-#define BUF_SIZE_ERROR		0x0010  /* Not enough available memory for
-					 * two buffers.
-					 */
-#define TX_BUF_COUNT_ERROR	0x0004  /* Remaining receive buffers less than
-					 * two.
-					 */
-#define OPEN_ERROR		0x0002	/* Error during ring insertion; more
-					 * information in bits 8-15.
-					 */
-
-/* Standard return codes */
-#define GOOD_COMPLETION		0x0080  /* =OPEN_SUCCESSFULL */
-#define INVALID_OPEN_OPTION	0x0001  /* OPEN options are not supported by
-					 * the adapter.
-					 */
-
-/* OPEN phases; details of OPEN_ERROR (SSB.Parm[0], LSB)            */
-#define OPEN_PHASES_MASK            0xF000  /* Check only the bits 8-11. */
-#define LOBE_MEDIA_TEST             0x1000
-#define PHYSICAL_INSERTION          0x2000
-#define ADDRESS_VERIFICATION        0x3000
-#define PARTICIPATION_IN_RING_POLL  0x4000
-#define REQUEST_INITIALISATION      0x5000
-#define FULLDUPLEX_CHECK            0x6000
-
-/* OPEN error codes; details of OPEN_ERROR (SSB.Parm[0], LSB) */
-#define OPEN_ERROR_CODES_MASK	0x0F00  /* Check only the bits 12-15. */
-#define OPEN_FUNCTION_FAILURE   0x0100  /* Unable to transmit to itself or
-					 * frames received before insertion.
-					 */
-#define OPEN_SIGNAL_LOSS	0x0200	/* Signal loss condition detected at
-					 * receiver.
-					 */
-#define OPEN_TIMEOUT		0x0500	/* Insertion timer expired before
-					 * logical insertion.
-					 */
-#define OPEN_RING_FAILURE	0x0600	/* Unable to receive own ring purge
-					 * MAC frames.
-					 */
-#define OPEN_RING_BEACONING	0x0700	/* Beacon MAC frame received after
-					 * ring insertion.
-					 */
-#define OPEN_DUPLICATE_NODEADDR	0x0800  /* Other station in ring found
-					 * with the same address.
-					 */
-#define OPEN_REQUEST_INIT	0x0900	/* RPS present but does not respond. */
-#define OPEN_REMOVE_RECEIVED    0x0A00  /* Adapter received a remove adapter
-					 * MAC frame.
-					 */
-#define OPEN_FULLDUPLEX_SET	0x0D00	/* Got this with full duplex on when
-					 * trying to connect to a normal ring.
-					 */
-
-/* SET_BRIDGE_PARMS return codes: */
-#define BRIDGE_INVALID_MAX_LEN  0x4000  /* MAX_ROUTING_FIELD_LENGTH odd,
-					 * less than 6 or > 30.
-					 */
-#define BRIDGE_INVALID_SRC_RING 0x2000  /* SOURCE_RING number zero, too large
-					 * or = TARGET_RING.
-					 */
-#define BRIDGE_INVALID_TRG_RING 0x1000  /* TARGET_RING number zero, too large
-					 * or = SOURCE_RING.
-					 */
-#define BRIDGE_INVALID_BRDGE_NO 0x0800  /* BRIDGE_NUMBER too large. */
-#define BRIDGE_INVALID_OPTIONS  0x0400  /* Invalid bridge options. */
-#define BRIDGE_DIAGS_FAILED     0x0200  /* Diagnostics of TMS380SRA failed. */
-#define BRIDGE_NO_SRA           0x0100  /* The TMS380SRA does not exist in HW
-					 * configuration.
-					 */
-
-/*
- * Bring Up Diagnostics error codes.
- */
-#define BUD_INITIAL_ERROR       0x0
-#define BUD_CHECKSUM_ERROR      0x1
-#define BUD_ADAPTER_RAM_ERROR   0x2
-#define BUD_INSTRUCTION_ERROR   0x3
-#define BUD_CONTEXT_ERROR       0x4
-#define BUD_PROTOCOL_ERROR      0x5
-#define BUD_INTERFACE_ERROR	0x6
-
-/* BUD constants */
-#define BUD_MAX_RETRIES         3
-#define BUD_MAX_LOOPCNT         6
-#define BUD_TIMEOUT             3000
-
-/* Initialization constants */
-#define INIT_MAX_RETRIES        3	/* Maximum three retries. */
-#define INIT_MAX_LOOPCNT        22      /* Maximum loop counts. */
-
-/* RING STATUS field values (high/low) */
-#define SIGNAL_LOSS             0x0080  /* Loss of signal on the ring
-					 * detected.
-					 */
-#define HARD_ERROR              0x0040  /* Transmitting or receiving beacon
-					 * frames.
-					 */
-#define SOFT_ERROR              0x0020  /* Report error MAC frame
-					 * transmitted.
-					 */
-#define TRANSMIT_BEACON         0x0010  /* Transmitting beacon frames on the
-					 * ring.
-					 */
-#define LOBE_WIRE_FAULT         0x0008  /* Open or short circuit in the
-					 * cable to concentrator; adapter
-					 * closed.
-					 */
-#define AUTO_REMOVAL_ERROR      0x0004  /* Lobe wrap test failed, deinserted;
-					 * adapter closed.
-					 */
-#define REMOVE_RECEIVED         0x0001  /* Received a remove ring station MAC
-					 * MAC frame request; adapter closed.
-					 */
-#define COUNTER_OVERFLOW        0x8000  /* Overflow of one of the adapters
-					 * error counters; READ.ERROR.LOG.
-					 */
-#define SINGLE_STATION          0x4000  /* Adapter is the only station on the
-					 * ring.
-					 */
-#define RING_RECOVERY           0x2000  /* Claim token MAC frames on the ring;
-					 * reset after ring purge frame.
-					 */
-
-#define ADAPTER_CLOSED (LOBE_WIRE_FAULT | AUTO_REMOVAL_ERROR |\
-                        REMOVE_RECEIVED)
-
-/* Adapter_check_block.Status field bit assignments: */
-#define DIO_PARITY              0x8000  /* Adapter detects bad parity
-					 * through direct I/O access.
-					 */
-#define DMA_READ_ABORT          0x4000  /* Aborting DMA read operation
-					 * from system Parm[0]: 0=timeout,
-					 * 1=parity error, 2=bus error;
-					 * Parm[1]: 32 bit pointer to host
-					 * system address at failure.
-					 */
-#define DMA_WRITE_ABORT         0x2000  /* Aborting DMA write operation
-					 * to system. (parameters analogous to
-					 * DMA_READ_ABORT)
-					 */
-#define ILLEGAL_OP_CODE         0x1000  /* Illegal operation code in the
-					 * the adapters firmware Parm[0]-2:
-					 * communications processor registers
-					 * R13-R15.
-					 */
-#define PARITY_ERRORS           0x0800  /* Adapter detects internal bus
-					 * parity error.
-					 */
-#define RAM_DATA_ERROR          0x0080  /* Valid only during RAM testing;
-					 * RAM data error Parm[0-1]: 32 bit
-					 * pointer to RAM location.
-					 */
-#define RAM_PARITY_ERROR        0x0040  /* Valid only during RAM testing;
-					 * RAM parity error Parm[0-1]: 32 bit
-					 * pointer to RAM location.
-					 */
-#define RING_UNDERRUN           0x0020  /* Internal DMA underrun when
-					 * transmitting onto ring.
-					 */
-#define INVALID_IRQ             0x0008  /* Unrecognized interrupt generated
-					 * internal to adapter Parm[0-2]:
-					 * adapter register R13-R15.
-					 */
-#define INVALID_ERROR_IRQ       0x0004  /* Unrecognized error interrupt
-					 * generated Parm[0-2]: adapter register
-					 * R13-R15.
-					 */
-#define INVALID_XOP             0x0002  /* Unrecognized XOP request in
-					 * communication processor Parm[0-2]:
-					 * adapter register R13-R15.
-					 */
-#define CHECKADDR               0x05E0  /* Adapter check status information
-					 * address offset.
-					 */
-#define ROM_PAGE_0              0x0000  /* Adapter ROM page 0. */
-
-/*
- * RECEIVE.STATUS interrupt result SSB values: (high-low)
- * (RECEIVE_COMPLETE field bit definitions in SSB.Parm[0])
- */
-#define RX_COMPLETE             0x0080  /* SSB.Parm[0]; SSB.Parm[1]: 32
-					 * bit pointer to last RPL.
-					 */
-#define RX_SUSPENDED            0x0040  /* SSB.Parm[0]; SSB.Parm[1]: 32
-					 * bit pointer to RPL with odd
-					 * forward pointer.
-					 */
-
-/* Valid receive CSTAT: */
-#define RX_FRAME_CONTROL_BITS (RX_VALID | RX_START_FRAME | RX_END_FRAME | \
-			       RX_FRAME_COMPLETE)
-#define VALID_SINGLE_BUFFER_FRAME (RX_START_FRAME | RX_END_FRAME | \
-				   RX_FRAME_COMPLETE)
-
-typedef enum SKB_STAT SKB_STAT;
-enum SKB_STAT {
-	SKB_UNAVAILABLE,
-	SKB_DMA_DIRECT,
-	SKB_DATA_COPY
-};
-
-/* Receive Parameter List (RPL) The length of the RPLs has to be initialized 
- * in the OPL. (OPEN parameter list)
- */
-#define RPL_NUM		3
-
-#define RX_FRAG_NUM     1	/* Maximal number of used fragments in one RPL.
-				 * (up to firmware v2.24: 3, now: up to 9)
-				 */
-
-#pragma pack(1)
-typedef struct s_RPL RPL;
-struct s_RPL {	/* Receive Parameter List */
-	__be32 NextRPLAddr;		/* Pointer to next RPL in chain
-					 * (normalized = physical 32 bit
-					 * address) if pointer is odd: this
-					 * is last RPL. Pointing to itself can
-					 * cause problems!
-					 */
-	volatile u_int16_t Status;	/* Set by creation of Receive Parameter
-					 * List RECEIVE_CSTAT_COMPLETE set by
-					 * adapter in lists that start or end
-					 * a frame.
-					 */
-	volatile __be16 FrameSize;	 /* Number of bytes received as a
-					 * frame including AC/FC, Destination,
-					 * Source, Routing field not including 
-					 * CRC, FS (Frame Status), End Delimiter
-					 * (valid only if START_FRAME bit in 
-					 * CSTAT nonzero) must not be zero in
-					 * any list; maximum value: (BUFFER_SIZE
-					 * - 8) * TX_BUF_MAX sum of DataCount
-					 * values in FragmentList must equal
-					 * Frame_Size value in START_FRAME TPL!
-					 * frame data fragment list
-					 */
-
-	/* TPL/RPL size in OPEN parameter list depending on maximal numbers
-	 * of fragments used in one parameter list.
-	 */
-	Fragment FragList[RX_FRAG_NUM];	/* Maximum: nine frame fragments in
-					 * one TPL. Actual version of firmware:
-					 * 9 fragments possible.
-					 */
-#pragma pack()
-
-	/* Special proprietary data and precalculations. */
-	RPL *NextRPLPtr;	/* Logical pointer to next RPL in chain. */
-	unsigned char *MData;
-	struct sk_buff *Skb;
-	SKB_STAT SkbStat;
-	int RPLIndex;
-	dma_addr_t DMABuff;		/* DMA IO bus address from dma_map */
-};
-
-/* Information that need to be kept for each board. */
-typedef struct net_local {
-#pragma pack(1)
-	IPB ipb;	/* Initialization Parameter Block. */
-	SCB scb;	/* System Command Block: system to adapter 
-			 * communication.
-			 */
-	SSB ssb;	/* System Status Block: adapter to system 
-			 * communication.
-			 */
-	OPB ocpl;	/* Open Options Parameter Block. */
-
-	ERRORTAB errorlogtable;	/* Adapter statistic error counters.
-				 * (read from adapter memory)
-				 */
-	unsigned char ProductID[PROD_ID_SIZE + 1]; /* Product ID */
-#pragma pack()
-
-	TPL Tpl[TPL_NUM];
-	TPL *TplFree;
-	TPL *TplBusy;
-	unsigned char LocalTxBuffers[TPL_NUM][DEFAULT_PACKET_SIZE];
-
-	RPL Rpl[RPL_NUM];
-	RPL *RplHead;
-	RPL *RplTail;
-	unsigned char LocalRxBuffers[RPL_NUM][DEFAULT_PACKET_SIZE];
-
-	struct device *pdev;
-	int DataRate;
-	unsigned char ScbInUse;
-	unsigned short CMDqueue;
-
-	unsigned long AdapterOpenFlag:1;
-	unsigned long AdapterVirtOpenFlag:1;
-	unsigned long OpenCommandIssued:1;
-	unsigned long TransmitCommandActive:1;
-	unsigned long TransmitHaltScheduled:1;
-	unsigned long HaltInProgress:1;
-	unsigned long LobeWireFaultLogged:1;
-	unsigned long ReOpenInProgress:1;
-	unsigned long Sleeping:1;
-
-	unsigned long LastOpenStatus;
-	unsigned short CurrentRingStatus;
-	unsigned long MaxPacketSize;
-	
-	unsigned long StartTime;
-	unsigned long LastSendTime;
-
-	struct tr_statistics MacStat;	/* MAC statistics structure */
-
-	unsigned long dmalimit; /* the max DMA address (ie, ISA) */
-	dma_addr_t    dmabuffer; /* the DMA bus address corresponding to
-				    priv. Might be different from virt_to_bus()
-				    for architectures with IO MMU (Alpha) */
-
-	struct timer_list timer;
-
-	wait_queue_head_t  wait_for_tok_int;
-
-	INTPTRS intptrs;	/* Internal adapter pointer. Must be read
-				 * before OPEN command.
-				 */
-	unsigned short (*setnselout)(struct net_device *);
-	unsigned short (*sifreadb)(struct net_device *, unsigned short);
-	void (*sifwriteb)(struct net_device *, unsigned short, unsigned short);
-	unsigned short (*sifreadw)(struct net_device *, unsigned short);
-	void (*sifwritew)(struct net_device *, unsigned short, unsigned short);
-
-	spinlock_t lock;                /* SMP protection */
-	void *tmspriv;
-} NET_LOCAL;
-
-#endif	/* __KERNEL__ */
-#endif	/* __LINUX_TMS380TR_H */
diff --git a/drivers/net/tokenring/tmspci.c b/drivers/net/tokenring/tmspci.c
deleted file mode 100644
index fb9918da579..00000000000
--- a/drivers/net/tokenring/tmspci.c
+++ /dev/null
@@ -1,248 +0,0 @@
-/*
- *  tmspci.c: A generic network driver for TMS380-based PCI token ring cards.
- *
- *  Written 1999 by Adam Fritzler
- *
- *  This software may be used and distributed according to the terms
- *  of the GNU General Public License, incorporated herein by reference.
- *
- *  This driver module supports the following cards:
- *	- SysKonnect TR4/16(+) PCI	(SK-4590)
- *	- SysKonnect TR4/16 PCI		(SK-4591)
- *      - Compaq TR 4/16 PCI
- *      - Thomas-Conrad TC4048 4/16 PCI 
- *      - 3Com 3C339 Token Link Velocity
- *
- *  Maintainer(s):
- *    AF	Adam Fritzler
- *
- *  Modification History:
- *	30-Dec-99	AF	Split off from the tms380tr driver.
- *	22-Jan-00	AF	Updated to use indirect read/writes
- *	23-Nov-00	JG	New PCI API, cleanups
- *
- *  TODO:
- *	1. See if we can use MMIO instead of port accesses
- *
- */
-
-#include <linux/module.h>
-#include <linux/kernel.h>
-#include <linux/errno.h>
-#include <linux/pci.h>
-#include <linux/init.h>
-#include <linux/netdevice.h>
-#include <linux/trdevice.h>
-
-#include <asm/io.h>
-#include <asm/irq.h>
-
-#include "tms380tr.h"
-
-static char version[] __devinitdata =
-"tmspci.c: v1.02 23/11/2000 by Adam Fritzler\n";
-
-#define TMS_PCI_IO_EXTENT 32
-
-struct card_info {
-	unsigned char nselout[2]; /* NSELOUT vals for 4mb([0]) and 16mb([1]) */
-	char *name;
-};
-
-static struct card_info card_info_table[] = {
-	{ {0x03, 0x01}, "Compaq 4/16 TR PCI"},
-	{ {0x03, 0x01}, "SK NET TR 4/16 PCI"},
-	{ {0x03, 0x01}, "Thomas-Conrad TC4048 PCI 4/16"},
-	{ {0x03, 0x01}, "3Com Token Link Velocity"},
-};
-
-static DEFINE_PCI_DEVICE_TABLE(tmspci_pci_tbl) = {
-	{ PCI_VENDOR_ID_COMPAQ, PCI_DEVICE_ID_COMPAQ_TOKENRING, PCI_ANY_ID, PCI_ANY_ID, 0, 0, 0 },
-	{ PCI_VENDOR_ID_SYSKONNECT, PCI_DEVICE_ID_SYSKONNECT_TR, PCI_ANY_ID, PCI_ANY_ID, 0, 0, 1 },
-	{ PCI_VENDOR_ID_TCONRAD, PCI_DEVICE_ID_TCONRAD_TOKENRING, PCI_ANY_ID, PCI_ANY_ID, 0, 0, 2 },
-	{ PCI_VENDOR_ID_3COM, PCI_DEVICE_ID_3COM_3C339, PCI_ANY_ID, PCI_ANY_ID, 0, 0, 3 },
-	{ }			/* Terminating entry */
-};
-MODULE_DEVICE_TABLE(pci, tmspci_pci_tbl);
-
-MODULE_LICENSE("GPL");
-
-static void tms_pci_read_eeprom(struct net_device *dev);
-static unsigned short tms_pci_setnselout_pins(struct net_device *dev);
-
-static unsigned short tms_pci_sifreadb(struct net_device *dev, unsigned short reg)
-{
-	return inb(dev->base_addr + reg);
-}
-
-static unsigned short tms_pci_sifreadw(struct net_device *dev, unsigned short reg)
-{
-	return inw(dev->base_addr + reg);
-}
-
-static void tms_pci_sifwriteb(struct net_device *dev, unsigned short val, unsigned short reg)
-{
-	outb(val, dev->base_addr + reg);
-}
-
-static void tms_pci_sifwritew(struct net_device *dev, unsigned short val, unsigned short reg)
-{
-	outw(val, dev->base_addr + reg);
-}
-
-static int __devinit tms_pci_attach(struct pci_dev *pdev, const struct pci_device_id *ent)
-{	
-	static int versionprinted;
-	struct net_device *dev;
-	struct net_local *tp;
-	int ret;
-	unsigned int pci_irq_line;
-	unsigned long pci_ioaddr;
-	struct card_info *cardinfo = &card_info_table[ent->driver_data];
-
-	if (versionprinted++ == 0)
-		printk("%s", version);
-
-	if (pci_enable_device(pdev))
-		return -EIO;
-
-	/* Remove I/O space marker in bit 0. */
-	pci_irq_line = pdev->irq;
-	pci_ioaddr = pci_resource_start (pdev, 0);
-
-	/* At this point we have found a valid card. */
-	dev = alloc_trdev(sizeof(struct net_local));
-	if (!dev)
-		return -ENOMEM;
-
-	if (!request_region(pci_ioaddr, TMS_PCI_IO_EXTENT, dev->name)) {
-		ret = -EBUSY;
-		goto err_out_trdev;
-	}
-
-	dev->base_addr	= pci_ioaddr;
-	dev->irq 	= pci_irq_line;
-	dev->dma	= 0;
-
-	dev_info(&pdev->dev, "%s\n", cardinfo->name);
-	dev_info(&pdev->dev, "    IO: %#4lx  IRQ: %d\n", dev->base_addr, dev->irq);
-		
-	tms_pci_read_eeprom(dev);
-
-	dev_info(&pdev->dev, "    Ring Station Address: %pM\n", dev->dev_addr);
-		
-	ret = tmsdev_init(dev, &pdev->dev);
-	if (ret) {
-		dev_info(&pdev->dev, "unable to get memory for dev->priv.\n");
-		goto err_out_region;
-	}
-
-	tp = netdev_priv(dev);
-	tp->setnselout = tms_pci_setnselout_pins;
-		
-	tp->sifreadb = tms_pci_sifreadb;
-	tp->sifreadw = tms_pci_sifreadw;
-	tp->sifwriteb = tms_pci_sifwriteb;
-	tp->sifwritew = tms_pci_sifwritew;
-		
-	memcpy(tp->ProductID, cardinfo->name, PROD_ID_SIZE + 1);
-
-	tp->tmspriv = cardinfo;
-
-	dev->netdev_ops = &tms380tr_netdev_ops;
-
-	ret = request_irq(pdev->irq, tms380tr_interrupt, IRQF_SHARED,
-			  dev->name, dev);
-	if (ret)
-		goto err_out_tmsdev;
-
-	pci_set_drvdata(pdev, dev);
-	SET_NETDEV_DEV(dev, &pdev->dev);
-
-	ret = register_netdev(dev);
-	if (ret)
-		goto err_out_irq;
-	
-	return 0;
-
-err_out_irq:
-	free_irq(pdev->irq, dev);
-err_out_tmsdev:
-	pci_set_drvdata(pdev, NULL);
-	tmsdev_term(dev);
-err_out_region:
-	release_region(pci_ioaddr, TMS_PCI_IO_EXTENT);
-err_out_trdev:
-	free_netdev(dev);
-	return ret;
-}
-
-/*
- * Reads MAC address from adapter RAM, which should've read it from
- * the onboard ROM.  
- *
- * Calling this on a board that does not support it can be a very
- * dangerous thing.  The Madge board, for instance, will lock your
- * machine hard when this is called.  Luckily, its supported in a
- * separate driver.  --ASF
- */
-static void tms_pci_read_eeprom(struct net_device *dev)
-{
-	int i;
-	
-	/* Address: 0000:0000 */
-	tms_pci_sifwritew(dev, 0, SIFADX);
-	tms_pci_sifwritew(dev, 0, SIFADR);	
-	
-	/* Read six byte MAC address data */
-	dev->addr_len = 6;
-	for(i = 0; i < 6; i++)
-		dev->dev_addr[i] = tms_pci_sifreadw(dev, SIFINC) >> 8;
-}
-
-static unsigned short tms_pci_setnselout_pins(struct net_device *dev)
-{
-	unsigned short val = 0;
-	struct net_local *tp = netdev_priv(dev);
-	struct card_info *cardinfo = tp->tmspriv;
-  
-	if(tp->DataRate == SPEED_4)
-		val |= cardinfo->nselout[0];	/* Set 4Mbps */
-	else
-		val |= cardinfo->nselout[1];	/* Set 16Mbps */
-	return val;
-}
-
-static void __devexit tms_pci_detach (struct pci_dev *pdev)
-{
-	struct net_device *dev = pci_get_drvdata(pdev);
-
-	BUG_ON(!dev);
-	unregister_netdev(dev);
-	release_region(dev->base_addr, TMS_PCI_IO_EXTENT);
-	free_irq(dev->irq, dev);
-	tmsdev_term(dev);
-	free_netdev(dev);
-	pci_set_drvdata(pdev, NULL);
-}
-
-static struct pci_driver tms_pci_driver = {
-	.name		= "tmspci",
-	.id_table	= tmspci_pci_tbl,
-	.probe		= tms_pci_attach,
-	.remove		= __devexit_p(tms_pci_detach),
-};
-
-static int __init tms_pci_init (void)
-{
-	return pci_register_driver(&tms_pci_driver);
-}
-
-static void __exit tms_pci_rmmod (void)
-{
-	pci_unregister_driver (&tms_pci_driver);
-}
-
-module_init(tms_pci_init);
-module_exit(tms_pci_rmmod);
-
diff --git a/drivers/net/tun.c b/drivers/net/tun.c
index bb8c72c79c6..987aeefbc77 100644
--- a/drivers/net/tun.c
+++ b/drivers/net/tun.c
@@ -313,7 +313,7 @@ static int run_filter(struct tap_filter *filter, const struct sk_buff *skb)
 
 	/* Exact match */
 	for (i = 0; i < filter->count; i++)
-		if (!compare_ether_addr(eh->h_dest, filter->addr[i]))
+		if (ether_addr_equal(eh->h_dest, filter->addr[i]))
 			return 1;
 
 	/* Inexact match (multicast only) */
diff --git a/drivers/net/usb/asix.c b/drivers/net/usb/asix.c
index 42b5151aa78..71e2b0523bc 100644
--- a/drivers/net/usb/asix.c
+++ b/drivers/net/usb/asix.c
@@ -1647,6 +1647,7 @@ static struct usb_driver asix_driver = {
 	.resume =	usbnet_resume,
 	.disconnect =	usbnet_disconnect,
 	.supports_autosuspend = 1,
+	.disable_hub_initiated_lpm = 1,
 };
 
 module_usb_driver(asix_driver);
diff --git a/drivers/net/usb/catc.c b/drivers/net/usb/catc.c
index 182cfb4aeb1..26c5bebd9ec 100644
--- a/drivers/net/usb/catc.c
+++ b/drivers/net/usb/catc.c
@@ -338,16 +338,18 @@ static void catc_irq_done(struct urb *urb)
 		} else {
 			catc->rx_urb->dev = catc->usbdev;
 			if ((res = usb_submit_urb(catc->rx_urb, GFP_ATOMIC)) < 0) {
-				err("submit(rx_urb) status %d", res);
+				dev_err(&catc->usbdev->dev,
+					"submit(rx_urb) status %d\n", res);
 			}
 		} 
 	}
 resubmit:
 	res = usb_submit_urb (urb, GFP_ATOMIC);
 	if (res)
-		err ("can't resubmit intr, %s-%s, status %d",
-				catc->usbdev->bus->bus_name,
-				catc->usbdev->devpath, res);
+		dev_err(&catc->usbdev->dev,
+			"can't resubmit intr, %s-%s, status %d\n",
+			catc->usbdev->bus->bus_name,
+			catc->usbdev->devpath, res);
 }
 
 /*
@@ -366,7 +368,8 @@ static int catc_tx_run(struct catc *catc)
 	catc->tx_urb->dev = catc->usbdev;
 
 	if ((status = usb_submit_urb(catc->tx_urb, GFP_ATOMIC)) < 0)
-		err("submit(tx_urb), status %d", status);
+		dev_err(&catc->usbdev->dev, "submit(tx_urb), status %d\n",
+			status);
 
 	catc->tx_idx = !catc->tx_idx;
 	catc->tx_ptr = 0;
@@ -496,7 +499,8 @@ static void catc_ctrl_run(struct catc *catc)
 		memcpy(catc->ctrl_buf, q->buf, q->len);
 
 	if ((status = usb_submit_urb(catc->ctrl_urb, GFP_ATOMIC)))
-		err("submit(ctrl_urb) status %d", status);
+		dev_err(&catc->usbdev->dev, "submit(ctrl_urb) status %d\n",
+			status);
 }
 
 static void catc_ctrl_done(struct urb *urb)
@@ -555,7 +559,7 @@ static int catc_ctrl_async(struct catc *catc, u8 dir, u8 request, u16 value,
 	catc->ctrl_head = (catc->ctrl_head + 1) & (CTRL_QUEUE - 1);
 
 	if (catc->ctrl_head == catc->ctrl_tail) {
-		err("ctrl queue full");
+		dev_err(&catc->usbdev->dev, "ctrl queue full\n");
 		catc->ctrl_tail = (catc->ctrl_tail + 1) & (CTRL_QUEUE - 1);
 		retval = -1;
 	}
@@ -714,7 +718,8 @@ static int catc_open(struct net_device *netdev)
 
 	catc->irq_urb->dev = catc->usbdev;
 	if ((status = usb_submit_urb(catc->irq_urb, GFP_KERNEL)) < 0) {
-		err("submit(irq_urb) status %d", status);
+		dev_err(&catc->usbdev->dev, "submit(irq_urb) status %d\n",
+			status);
 		return -1;
 	}
 
@@ -769,7 +774,7 @@ static int catc_probe(struct usb_interface *intf, const struct usb_device_id *id
 
 	if (usb_set_interface(usbdev,
 			intf->altsetting->desc.bInterfaceNumber, 1)) {
-                err("Can't set altsetting 1.");
+                dev_err(&intf->dev, "Can't set altsetting 1.\n");
 		return -EIO;
 	}
 
@@ -799,7 +804,7 @@ static int catc_probe(struct usb_interface *intf, const struct usb_device_id *id
 	catc->irq_urb = usb_alloc_urb(0, GFP_KERNEL);
 	if ((!catc->ctrl_urb) || (!catc->tx_urb) || 
 	    (!catc->rx_urb) || (!catc->irq_urb)) {
-		err("No free urbs available.");
+		dev_err(&intf->dev, "No free urbs available.\n");
 		usb_free_urb(catc->ctrl_urb);
 		usb_free_urb(catc->tx_urb);
 		usb_free_urb(catc->rx_urb);
@@ -947,6 +952,7 @@ static struct usb_driver catc_driver = {
 	.probe =	catc_probe,
 	.disconnect =	catc_disconnect,
 	.id_table =	catc_id_table,
+	.disable_hub_initiated_lpm = 1,
 };
 
 module_usb_driver(catc_driver);
diff --git a/drivers/net/usb/cdc-phonet.c b/drivers/net/usb/cdc-phonet.c
index 3e41b00c680..d848d4dd575 100644
--- a/drivers/net/usb/cdc-phonet.c
+++ b/drivers/net/usb/cdc-phonet.c
@@ -457,6 +457,7 @@ static struct usb_driver usbpn_driver = {
 	.probe =	usbpn_probe,
 	.disconnect =	usbpn_disconnect,
 	.id_table =	usbpn_ids,
+	.disable_hub_initiated_lpm = 1,
 };
 
 module_usb_driver(usbpn_driver);
diff --git a/drivers/net/usb/cdc_eem.c b/drivers/net/usb/cdc_eem.c
index 685a4e22c76..434d5af8e6f 100644
--- a/drivers/net/usb/cdc_eem.c
+++ b/drivers/net/usb/cdc_eem.c
@@ -368,6 +368,7 @@ static struct usb_driver eem_driver = {
 	.disconnect =	usbnet_disconnect,
 	.suspend =	usbnet_suspend,
 	.resume =	usbnet_resume,
+	.disable_hub_initiated_lpm = 1,
 };
 
 module_usb_driver(eem_driver);
diff --git a/drivers/net/usb/cdc_ether.c b/drivers/net/usb/cdc_ether.c
index 90a30026a93..a03de719704 100644
--- a/drivers/net/usb/cdc_ether.c
+++ b/drivers/net/usb/cdc_ether.c
@@ -83,6 +83,7 @@ int usbnet_generic_cdc_bind(struct usbnet *dev, struct usb_interface *intf)
 	struct cdc_state		*info = (void *) &dev->data;
 	int				status;
 	int				rndis;
+	bool				android_rndis_quirk = false;
 	struct usb_driver		*driver = driver_of(intf);
 	struct usb_cdc_mdlm_desc	*desc = NULL;
 	struct usb_cdc_mdlm_detail_desc *detail = NULL;
@@ -195,6 +196,11 @@ int usbnet_generic_cdc_bind(struct usbnet *dev, struct usb_interface *intf)
 					info->control,
 					info->u->bSlaveInterface0,
 					info->data);
+				/* fall back to hard-wiring for RNDIS */
+				if (rndis) {
+					android_rndis_quirk = true;
+					goto next_desc;
+				}
 				goto bad_desc;
 			}
 			if (info->control != intf) {
@@ -271,11 +277,15 @@ next_desc:
 	/* Microsoft ActiveSync based and some regular RNDIS devices lack the
 	 * CDC descriptors, so we'll hard-wire the interfaces and not check
 	 * for descriptors.
+	 *
+	 * Some Android RNDIS devices have a CDC Union descriptor pointing
+	 * to non-existing interfaces.  Ignore that and attempt the same
+	 * hard-wired 0 and 1 interfaces.
 	 */
-	if (rndis && !info->u) {
+	if (rndis && (!info->u || android_rndis_quirk)) {
 		info->control = usb_ifnum_to_if(dev->udev, 0);
 		info->data = usb_ifnum_to_if(dev->udev, 1);
-		if (!info->control || !info->data) {
+		if (!info->control || !info->data || info->control != intf) {
 			dev_dbg(&intf->dev,
 				"rndis: master #0/%p slave #1/%p\n",
 				info->control,
@@ -475,6 +485,8 @@ static const struct driver_info wwan_info = {
 /*-------------------------------------------------------------------------*/
 
 #define HUAWEI_VENDOR_ID	0x12D1
+#define NOVATEL_VENDOR_ID	0x1410
+#define ZTE_VENDOR_ID		0x19D2
 
 static const struct usb_device_id	products [] = {
 /*
@@ -592,6 +604,76 @@ static const struct usb_device_id	products [] = {
  * because of bugs/quirks in a given product (like Zaurus, above).
  */
 {
+	/* Novatel USB551L */
+	/* This match must come *before* the generic CDC-ETHER match so that
+	 * we get FLAG_WWAN set on the device, since it's descriptors are
+	 * generic CDC-ETHER.
+	 */
+	.match_flags    =   USB_DEVICE_ID_MATCH_VENDOR
+		 | USB_DEVICE_ID_MATCH_PRODUCT
+		 | USB_DEVICE_ID_MATCH_INT_INFO,
+	.idVendor               = NOVATEL_VENDOR_ID,
+	.idProduct		= 0xB001,
+	.bInterfaceClass	= USB_CLASS_COMM,
+	.bInterfaceSubClass	= USB_CDC_SUBCLASS_ETHERNET,
+	.bInterfaceProtocol	= USB_CDC_PROTO_NONE,
+	.driver_info = (unsigned long)&wwan_info,
+}, {
+	/* ZTE (Vodafone) K3805-Z */
+	.match_flags    =   USB_DEVICE_ID_MATCH_VENDOR
+		 | USB_DEVICE_ID_MATCH_PRODUCT
+		 | USB_DEVICE_ID_MATCH_INT_INFO,
+	.idVendor               = ZTE_VENDOR_ID,
+	.idProduct		= 0x1003,
+	.bInterfaceClass	= USB_CLASS_COMM,
+	.bInterfaceSubClass	= USB_CDC_SUBCLASS_ETHERNET,
+	.bInterfaceProtocol	= USB_CDC_PROTO_NONE,
+	.driver_info = (unsigned long)&wwan_info,
+}, {
+	/* ZTE (Vodafone) K3806-Z */
+	.match_flags    =   USB_DEVICE_ID_MATCH_VENDOR
+		 | USB_DEVICE_ID_MATCH_PRODUCT
+		 | USB_DEVICE_ID_MATCH_INT_INFO,
+	.idVendor               = ZTE_VENDOR_ID,
+	.idProduct		= 0x1015,
+	.bInterfaceClass	= USB_CLASS_COMM,
+	.bInterfaceSubClass	= USB_CDC_SUBCLASS_ETHERNET,
+	.bInterfaceProtocol	= USB_CDC_PROTO_NONE,
+	.driver_info = (unsigned long)&wwan_info,
+}, {
+	/* ZTE (Vodafone) K4510-Z */
+	.match_flags    =   USB_DEVICE_ID_MATCH_VENDOR
+		 | USB_DEVICE_ID_MATCH_PRODUCT
+		 | USB_DEVICE_ID_MATCH_INT_INFO,
+	.idVendor               = ZTE_VENDOR_ID,
+	.idProduct		= 0x1173,
+	.bInterfaceClass	= USB_CLASS_COMM,
+	.bInterfaceSubClass	= USB_CDC_SUBCLASS_ETHERNET,
+	.bInterfaceProtocol	= USB_CDC_PROTO_NONE,
+	.driver_info = (unsigned long)&wwan_info,
+}, {
+	/* ZTE (Vodafone) K3770-Z */
+	.match_flags    =   USB_DEVICE_ID_MATCH_VENDOR
+		 | USB_DEVICE_ID_MATCH_PRODUCT
+		 | USB_DEVICE_ID_MATCH_INT_INFO,
+	.idVendor               = ZTE_VENDOR_ID,
+	.idProduct		= 0x1177,
+	.bInterfaceClass	= USB_CLASS_COMM,
+	.bInterfaceSubClass	= USB_CDC_SUBCLASS_ETHERNET,
+	.bInterfaceProtocol	= USB_CDC_PROTO_NONE,
+	.driver_info = (unsigned long)&wwan_info,
+}, {
+	/* ZTE (Vodafone) K3772-Z */
+	.match_flags    =   USB_DEVICE_ID_MATCH_VENDOR
+		 | USB_DEVICE_ID_MATCH_PRODUCT
+		 | USB_DEVICE_ID_MATCH_INT_INFO,
+	.idVendor               = ZTE_VENDOR_ID,
+	.idProduct		= 0x1181,
+	.bInterfaceClass	= USB_CLASS_COMM,
+	.bInterfaceSubClass	= USB_CDC_SUBCLASS_ETHERNET,
+	.bInterfaceProtocol	= USB_CDC_PROTO_NONE,
+	.driver_info = (unsigned long)&wwan_info,
+}, {
 	USB_INTERFACE_INFO(USB_CLASS_COMM, USB_CDC_SUBCLASS_ETHERNET,
 			USB_CDC_PROTO_NONE),
 	.driver_info = (unsigned long) &cdc_info,
@@ -623,6 +705,7 @@ static struct usb_driver cdc_driver = {
 	.resume =	usbnet_resume,
 	.reset_resume =	usbnet_resume,
 	.supports_autosuspend = 1,
+	.disable_hub_initiated_lpm = 1,
 };
 
 module_usb_driver(cdc_driver);
diff --git a/drivers/net/usb/cdc_ncm.c b/drivers/net/usb/cdc_ncm.c
index 7adc9f6b0ea..4b9513fcf27 100644
--- a/drivers/net/usb/cdc_ncm.c
+++ b/drivers/net/usb/cdc_ncm.c
@@ -1212,6 +1212,7 @@ static struct usb_driver cdc_ncm_driver = {
 	.resume = usbnet_resume,
 	.reset_resume =	usbnet_resume,
 	.supports_autosuspend = 1,
+	.disable_hub_initiated_lpm = 1,
 };
 
 static const struct ethtool_ops cdc_ncm_ethtool_ops = {
diff --git a/drivers/net/usb/cdc_subset.c b/drivers/net/usb/cdc_subset.c
index b403d934e4e..0d1fe89ae0b 100644
--- a/drivers/net/usb/cdc_subset.c
+++ b/drivers/net/usb/cdc_subset.c
@@ -336,6 +336,7 @@ static struct usb_driver cdc_subset_driver = {
 	.resume =	usbnet_resume,
 	.disconnect =	usbnet_disconnect,
 	.id_table =	products,
+	.disable_hub_initiated_lpm = 1,
 };
 
 module_usb_driver(cdc_subset_driver);
diff --git a/drivers/net/usb/cx82310_eth.c b/drivers/net/usb/cx82310_eth.c
index 0e0531356e6..49ab45e17fe 100644
--- a/drivers/net/usb/cx82310_eth.c
+++ b/drivers/net/usb/cx82310_eth.c
@@ -327,6 +327,7 @@ static struct usb_driver cx82310_driver = {
 	.disconnect	= usbnet_disconnect,
 	.suspend	= usbnet_suspend,
 	.resume		= usbnet_resume,
+	.disable_hub_initiated_lpm = 1,
 };
 
 module_usb_driver(cx82310_driver);
diff --git a/drivers/net/usb/dm9601.c b/drivers/net/usb/dm9601.c
index b97226318ea..e0433ce6ced 100644
--- a/drivers/net/usb/dm9601.c
+++ b/drivers/net/usb/dm9601.c
@@ -670,6 +670,7 @@ static struct usb_driver dm9601_driver = {
 	.disconnect = usbnet_disconnect,
 	.suspend = usbnet_suspend,
 	.resume = usbnet_resume,
+	.disable_hub_initiated_lpm = 1,
 };
 
 module_usb_driver(dm9601_driver);
diff --git a/drivers/net/usb/gl620a.c b/drivers/net/usb/gl620a.c
index 38266bdae26..db3c8021f2a 100644
--- a/drivers/net/usb/gl620a.c
+++ b/drivers/net/usb/gl620a.c
@@ -225,6 +225,7 @@ static struct usb_driver gl620a_driver = {
 	.disconnect =	usbnet_disconnect,
 	.suspend =	usbnet_suspend,
 	.resume =	usbnet_resume,
+	.disable_hub_initiated_lpm = 1,
 };
 
 module_usb_driver(gl620a_driver);
diff --git a/drivers/net/usb/hso.c b/drivers/net/usb/hso.c
index 2d2a6882ba3..62f30b46fa4 100644
--- a/drivers/net/usb/hso.c
+++ b/drivers/net/usb/hso.c
@@ -106,13 +106,6 @@
 
 #define MAX_RX_URBS			2
 
-static inline struct hso_serial *get_serial_by_tty(struct tty_struct *tty)
-{
-	if (tty)
-		return tty->driver_data;
-	return NULL;
-}
-
 /*****************************************************************************/
 /* Debugging functions                                                       */
 /*****************************************************************************/
@@ -255,9 +248,8 @@ struct hso_serial {
 	u8 dtr_state;
 	unsigned tx_urb_used:1;
 
+	struct tty_port port;
 	/* from usb_serial_port */
-	struct tty_struct *tty;
-	int open_count;
 	spinlock_t serial_lock;
 
 	int (*write_data) (struct hso_serial *serial);
@@ -1114,7 +1106,7 @@ static void hso_init_termios(struct ktermios *termios)
 static void _hso_serial_set_termios(struct tty_struct *tty,
 				    struct ktermios *old)
 {
-	struct hso_serial *serial = get_serial_by_tty(tty);
+	struct hso_serial *serial = tty->driver_data;
 	struct ktermios *termios;
 
 	if (!serial) {
@@ -1190,7 +1182,7 @@ static void put_rxbuf_data_and_resubmit_ctrl_urb(struct hso_serial *serial)
 	struct urb *urb;
 
 	urb = serial->rx_urb[0];
-	if (serial->open_count > 0) {
+	if (serial->port.count > 0) {
 		count = put_rxbuf_data(urb, serial);
 		if (count == -1)
 			return;
@@ -1226,7 +1218,7 @@ static void hso_std_serial_read_bulk_callback(struct urb *urb)
 	DUMP1(urb->transfer_buffer, urb->actual_length);
 
 	/* Anyone listening? */
-	if (serial->open_count == 0)
+	if (serial->port.count == 0)
 		return;
 
 	if (status == 0) {
@@ -1268,7 +1260,7 @@ static void hso_unthrottle_tasklet(struct hso_serial *serial)
 
 static	void hso_unthrottle(struct tty_struct *tty)
 {
-	struct hso_serial *serial = get_serial_by_tty(tty);
+	struct hso_serial *serial = tty->driver_data;
 
 	tasklet_hi_schedule(&serial->unthrottle_tasklet);
 }
@@ -1304,15 +1296,12 @@ static int hso_serial_open(struct tty_struct *tty, struct file *filp)
 	kref_get(&serial->parent->ref);
 
 	/* setup */
-	spin_lock_irq(&serial->serial_lock);
 	tty->driver_data = serial;
-	tty_kref_put(serial->tty);
-	serial->tty = tty_kref_get(tty);
-	spin_unlock_irq(&serial->serial_lock);
+	tty_port_tty_set(&serial->port, tty);
 
 	/* check for port already opened, if not set the termios */
-	serial->open_count++;
-	if (serial->open_count == 1) {
+	serial->port.count++;
+	if (serial->port.count == 1) {
 		serial->rx_state = RX_IDLE;
 		/* Force default termio settings */
 		_hso_serial_set_termios(tty, NULL);
@@ -1324,7 +1313,7 @@ static int hso_serial_open(struct tty_struct *tty, struct file *filp)
 		result = hso_start_serial_device(serial->parent, GFP_KERNEL);
 		if (result) {
 			hso_stop_serial_device(serial->parent);
-			serial->open_count--;
+			serial->port.count--;
 			kref_put(&serial->parent->ref, hso_serial_ref_free);
 		}
 	} else {
@@ -1361,17 +1350,11 @@ static void hso_serial_close(struct tty_struct *tty, struct file *filp)
 
 	/* reset the rts and dtr */
 	/* do the actual close */
-	serial->open_count--;
+	serial->port.count--;
 
-	if (serial->open_count <= 0) {
-		serial->open_count = 0;
-		spin_lock_irq(&serial->serial_lock);
-		if (serial->tty == tty) {
-			serial->tty->driver_data = NULL;
-			serial->tty = NULL;
-			tty_kref_put(tty);
-		}
-		spin_unlock_irq(&serial->serial_lock);
+	if (serial->port.count <= 0) {
+		serial->port.count = 0;
+		tty_port_tty_set(&serial->port, NULL);
 		if (!usb_gone)
 			hso_stop_serial_device(serial->parent);
 		tasklet_kill(&serial->unthrottle_tasklet);
@@ -1390,7 +1373,7 @@ static void hso_serial_close(struct tty_struct *tty, struct file *filp)
 static int hso_serial_write(struct tty_struct *tty, const unsigned char *buf,
 			    int count)
 {
-	struct hso_serial *serial = get_serial_by_tty(tty);
+	struct hso_serial *serial = tty->driver_data;
 	int space, tx_bytes;
 	unsigned long flags;
 
@@ -1422,7 +1405,7 @@ out:
 /* how much room is there for writing */
 static int hso_serial_write_room(struct tty_struct *tty)
 {
-	struct hso_serial *serial = get_serial_by_tty(tty);
+	struct hso_serial *serial = tty->driver_data;
 	int room;
 	unsigned long flags;
 
@@ -1437,7 +1420,7 @@ static int hso_serial_write_room(struct tty_struct *tty)
 /* setup the term */
 static void hso_serial_set_termios(struct tty_struct *tty, struct ktermios *old)
 {
-	struct hso_serial *serial = get_serial_by_tty(tty);
+	struct hso_serial *serial = tty->driver_data;
 	unsigned long flags;
 
 	if (old)
@@ -1446,7 +1429,7 @@ static void hso_serial_set_termios(struct tty_struct *tty, struct ktermios *old)
 
 	/* the actual setup */
 	spin_lock_irqsave(&serial->serial_lock, flags);
-	if (serial->open_count)
+	if (serial->port.count)
 		_hso_serial_set_termios(tty, old);
 	else
 		tty->termios = old;
@@ -1458,7 +1441,7 @@ static void hso_serial_set_termios(struct tty_struct *tty, struct ktermios *old)
 /* how many characters in the buffer */
 static int hso_serial_chars_in_buffer(struct tty_struct *tty)
 {
-	struct hso_serial *serial = get_serial_by_tty(tty);
+	struct hso_serial *serial = tty->driver_data;
 	int chars;
 	unsigned long flags;
 
@@ -1629,7 +1612,7 @@ static int hso_get_count(struct tty_struct *tty,
 		  struct serial_icounter_struct *icount)
 {
 	struct uart_icount cnow;
-	struct hso_serial *serial = get_serial_by_tty(tty);
+	struct hso_serial *serial = tty->driver_data;
 	struct hso_tiocmget  *tiocmget = serial->tiocmget;
 
 	memset(icount, 0, sizeof(struct serial_icounter_struct));
@@ -1659,7 +1642,7 @@ static int hso_get_count(struct tty_struct *tty,
 static int hso_serial_tiocmget(struct tty_struct *tty)
 {
 	int retval;
-	struct hso_serial *serial = get_serial_by_tty(tty);
+	struct hso_serial *serial = tty->driver_data;
 	struct hso_tiocmget  *tiocmget;
 	u16 UART_state_bitmap;
 
@@ -1693,7 +1676,7 @@ static int hso_serial_tiocmset(struct tty_struct *tty,
 	int val = 0;
 	unsigned long flags;
 	int if_num;
-	struct hso_serial *serial = get_serial_by_tty(tty);
+	struct hso_serial *serial = tty->driver_data;
 
 	/* sanity check */
 	if (!serial) {
@@ -1733,7 +1716,7 @@ static int hso_serial_tiocmset(struct tty_struct *tty,
 static int hso_serial_ioctl(struct tty_struct *tty,
 			    unsigned int cmd, unsigned long arg)
 {
-	struct hso_serial *serial =  get_serial_by_tty(tty);
+	struct hso_serial *serial = tty->driver_data;
 	int ret = 0;
 	D4("IOCTL cmd: %d, arg: %ld", cmd, arg);
 
@@ -1905,7 +1888,7 @@ static void intr_callback(struct urb *urb)
 				D1("Pending read interrupt on port %d\n", i);
 				spin_lock(&serial->serial_lock);
 				if (serial->rx_state == RX_IDLE &&
-					serial->open_count > 0) {
+					serial->port.count > 0) {
 					/* Setup and send a ctrl req read on
 					 * port i */
 					if (!serial->rx_urb_filled[0]) {
@@ -1954,14 +1937,13 @@ static void hso_std_serial_write_bulk_callback(struct urb *urb)
 
 	spin_lock(&serial->serial_lock);
 	serial->tx_urb_used = 0;
-	tty = tty_kref_get(serial->tty);
 	spin_unlock(&serial->serial_lock);
 	if (status) {
 		handle_usb_error(status, __func__, serial->parent);
-		tty_kref_put(tty);
 		return;
 	}
 	hso_put_activity(serial->parent);
+	tty = tty_port_tty_get(&serial->port);
 	if (tty) {
 		tty_wakeup(tty);
 		tty_kref_put(tty);
@@ -2001,7 +1983,6 @@ static void ctrl_callback(struct urb *urb)
 	struct hso_serial *serial = urb->context;
 	struct usb_ctrlrequest *req;
 	int status = urb->status;
-	struct tty_struct *tty;
 
 	/* sanity check */
 	if (!serial)
@@ -2009,11 +1990,9 @@ static void ctrl_callback(struct urb *urb)
 
 	spin_lock(&serial->serial_lock);
 	serial->tx_urb_used = 0;
-	tty = tty_kref_get(serial->tty);
 	spin_unlock(&serial->serial_lock);
 	if (status) {
 		handle_usb_error(status, __func__, serial->parent);
-		tty_kref_put(tty);
 		return;
 	}
 
@@ -2031,13 +2010,15 @@ static void ctrl_callback(struct urb *urb)
 		put_rxbuf_data_and_resubmit_ctrl_urb(serial);
 		spin_unlock(&serial->serial_lock);
 	} else {
+		struct tty_struct *tty = tty_port_tty_get(&serial->port);
 		hso_put_activity(serial->parent);
-		if (tty)
+		if (tty) {
 			tty_wakeup(tty);
+			tty_kref_put(tty);
+		}
 		/* response to a write command */
 		hso_kick_transmit(serial);
 	}
-	tty_kref_put(tty);
 }
 
 /* handle RX data for serial port */
@@ -2053,8 +2034,7 @@ static int put_rxbuf_data(struct urb *urb, struct hso_serial *serial)
 		return -2;
 	}
 
-	/* All callers to put_rxbuf_data hold serial_lock */
-	tty = tty_kref_get(serial->tty);
+	tty = tty_port_tty_get(&serial->port);
 
 	/* Push data to tty */
 	if (tty) {
@@ -2074,12 +2054,12 @@ static int put_rxbuf_data(struct urb *urb, struct hso_serial *serial)
 			write_length_remaining -= curr_write_len;
 			tty_flip_buffer_push(tty);
 		}
+		tty_kref_put(tty);
 	}
 	if (write_length_remaining == 0) {
 		serial->curr_rx_urb_offset = 0;
 		serial->rx_urb_filled[hso_urb_to_index(serial, urb)] = 0;
 	}
-	tty_kref_put(tty);
 	return write_length_remaining;
 }
 
@@ -2320,6 +2300,7 @@ static int hso_serial_common_create(struct hso_serial *serial, int num_urbs,
 	serial->minor = minor;
 	serial->magic = HSO_SERIAL_MAGIC;
 	spin_lock_init(&serial->serial_lock);
+	tty_port_init(&serial->port);
 	serial->num_rx_urbs = num_urbs;
 
 	/* RX, allocate urb and initialize */
@@ -3098,7 +3079,7 @@ static int hso_resume(struct usb_interface *iface)
 	/* Start all serial ports */
 	for (i = 0; i < HSO_SERIAL_TTY_MINORS; i++) {
 		if (serial_table[i] && (serial_table[i]->interface == iface)) {
-			if (dev2ser(serial_table[i])->open_count) {
+			if (dev2ser(serial_table[i])->port.count) {
 				result =
 				    hso_start_serial_device(serial_table[i], GFP_NOIO);
 				hso_kick_transmit(dev2ser(serial_table[i]));
@@ -3172,13 +3153,12 @@ static void hso_free_interface(struct usb_interface *interface)
 		if (serial_table[i] &&
 		    (serial_table[i]->interface == interface)) {
 			hso_dev = dev2ser(serial_table[i]);
-			spin_lock_irq(&hso_dev->serial_lock);
-			tty = tty_kref_get(hso_dev->tty);
-			spin_unlock_irq(&hso_dev->serial_lock);
-			if (tty)
+			tty = tty_port_tty_get(&hso_dev->port);
+			if (tty) {
 				tty_hangup(tty);
+				tty_kref_put(tty);
+			}
 			mutex_lock(&hso_dev->parent->mutex);
-			tty_kref_put(tty);
 			hso_dev->parent->usb_gone = 1;
 			mutex_unlock(&hso_dev->parent->mutex);
 			kref_put(&serial_table[i]->ref, hso_serial_ref_free);
@@ -3291,6 +3271,7 @@ static struct usb_driver hso_driver = {
 	.resume = hso_resume,
 	.reset_resume = hso_resume,
 	.supports_autosuspend = 1,
+	.disable_hub_initiated_lpm = 1,
 };
 
 static int __init hso_init(void)
@@ -3312,7 +3293,6 @@ static int __init hso_init(void)
 		return -ENOMEM;
 
 	/* fill in all needed values */
-	tty_drv->magic = TTY_DRIVER_MAGIC;
 	tty_drv->driver_name = driver_name;
 	tty_drv->name = tty_filename;
 
@@ -3333,7 +3313,7 @@ static int __init hso_init(void)
 	if (result) {
 		printk(KERN_ERR "%s - tty_register_driver failed(%d)\n",
 			__func__, result);
-		return result;
+		goto err_free_tty;
 	}
 
 	/* register this module as an usb driver */
@@ -3341,13 +3321,16 @@ static int __init hso_init(void)
 	if (result) {
 		printk(KERN_ERR "Could not register hso driver? error: %d\n",
 			result);
-		/* cleanup serial interface */
-		tty_unregister_driver(tty_drv);
-		return result;
+		goto err_unreg_tty;
 	}
 
 	/* done */
 	return 0;
+err_unreg_tty:
+	tty_unregister_driver(tty_drv);
+err_free_tty:
+	put_tty_driver(tty_drv);
+	return result;
 }
 
 static void __exit hso_exit(void)
@@ -3355,6 +3338,7 @@ static void __exit hso_exit(void)
 	printk(KERN_INFO "hso: unloaded\n");
 
 	tty_unregister_driver(tty_drv);
+	put_tty_driver(tty_drv);
 	/* deregister the usb driver */
 	usb_deregister(&hso_driver);
 }
diff --git a/drivers/net/usb/int51x1.c b/drivers/net/usb/int51x1.c
index 12a22a453ff..8de641713d5 100644
--- a/drivers/net/usb/int51x1.c
+++ b/drivers/net/usb/int51x1.c
@@ -236,6 +236,7 @@ static struct usb_driver int51x1_driver = {
 	.disconnect = usbnet_disconnect,
 	.suspend    = usbnet_suspend,
 	.resume     = usbnet_resume,
+	.disable_hub_initiated_lpm = 1,
 };
 
 module_usb_driver(int51x1_driver);
diff --git a/drivers/net/usb/ipheth.c b/drivers/net/usb/ipheth.c
index dd78c4cbd45..964031e3da8 100644
--- a/drivers/net/usb/ipheth.c
+++ b/drivers/net/usb/ipheth.c
@@ -209,7 +209,8 @@ static void ipheth_rcvbulk_callback(struct urb *urb)
 	case 0:
 		break;
 	default:
-		err("%s: urb status: %d", __func__, status);
+		dev_err(&dev->intf->dev, "%s: urb status: %d\n",
+			__func__, status);
 		return;
 	}
 
@@ -222,7 +223,8 @@ static void ipheth_rcvbulk_callback(struct urb *urb)
 
 	skb = dev_alloc_skb(len);
 	if (!skb) {
-		err("%s: dev_alloc_skb: -ENOMEM", __func__);
+		dev_err(&dev->intf->dev, "%s: dev_alloc_skb: -ENOMEM\n",
+			__func__);
 		dev->net->stats.rx_dropped++;
 		return;
 	}
@@ -251,7 +253,8 @@ static void ipheth_sndbulk_callback(struct urb *urb)
 	    status != -ENOENT &&
 	    status != -ECONNRESET &&
 	    status != -ESHUTDOWN)
-		err("%s: urb status: %d", __func__, status);
+		dev_err(&dev->intf->dev, "%s: urb status: %d\n",
+		__func__, status);
 
 	dev_kfree_skb_irq(dev->tx_skb);
 	netif_wake_queue(dev->net);
@@ -271,7 +274,8 @@ static int ipheth_carrier_set(struct ipheth_device *dev)
 			dev->ctrl_buf, IPHETH_CTRL_BUF_SIZE,
 			IPHETH_CTRL_TIMEOUT);
 	if (retval < 0) {
-		err("%s: usb_control_msg: %d", __func__, retval);
+		dev_err(&dev->intf->dev, "%s: usb_control_msg: %d\n",
+			__func__, retval);
 		return retval;
 	}
 
@@ -308,9 +312,11 @@ static int ipheth_get_macaddr(struct ipheth_device *dev)
 				 IPHETH_CTRL_BUF_SIZE,
 				 IPHETH_CTRL_TIMEOUT);
 	if (retval < 0) {
-		err("%s: usb_control_msg: %d", __func__, retval);
+		dev_err(&dev->intf->dev, "%s: usb_control_msg: %d\n",
+			__func__, retval);
 	} else if (retval < ETH_ALEN) {
-		err("%s: usb_control_msg: short packet: %d bytes",
+		dev_err(&dev->intf->dev,
+			"%s: usb_control_msg: short packet: %d bytes\n",
 			__func__, retval);
 		retval = -EINVAL;
 	} else {
@@ -335,7 +341,8 @@ static int ipheth_rx_submit(struct ipheth_device *dev, gfp_t mem_flags)
 
 	retval = usb_submit_urb(dev->rx_urb, mem_flags);
 	if (retval)
-		err("%s: usb_submit_urb: %d", __func__, retval);
+		dev_err(&dev->intf->dev, "%s: usb_submit_urb: %d\n",
+			__func__, retval);
 	return retval;
 }
 
@@ -396,7 +403,8 @@ static int ipheth_tx(struct sk_buff *skb, struct net_device *net)
 
 	retval = usb_submit_urb(dev->tx_urb, GFP_ATOMIC);
 	if (retval) {
-		err("%s: usb_submit_urb: %d", __func__, retval);
+		dev_err(&dev->intf->dev, "%s: usb_submit_urb: %d\n",
+			__func__, retval);
 		dev->net->stats.tx_errors++;
 		dev_kfree_skb_irq(skb);
 	} else {
@@ -414,7 +422,7 @@ static void ipheth_tx_timeout(struct net_device *net)
 {
 	struct ipheth_device *dev = netdev_priv(net);
 
-	err("%s: TX timeout", __func__);
+	dev_err(&dev->intf->dev, "%s: TX timeout\n", __func__);
 	dev->net->stats.tx_errors++;
 	usb_unlink_urb(dev->tx_urb);
 }
@@ -464,7 +472,7 @@ static int ipheth_probe(struct usb_interface *intf,
 	hintf = usb_altnum_to_altsetting(intf, IPHETH_ALT_INTFNUM);
 	if (hintf == NULL) {
 		retval = -ENODEV;
-		err("Unable to find alternate settings interface");
+		dev_err(&intf->dev, "Unable to find alternate settings interface\n");
 		goto err_endpoints;
 	}
 
@@ -477,7 +485,7 @@ static int ipheth_probe(struct usb_interface *intf,
 	}
 	if (!(dev->bulk_in && dev->bulk_out)) {
 		retval = -ENODEV;
-		err("Unable to find endpoints");
+		dev_err(&intf->dev, "Unable to find endpoints\n");
 		goto err_endpoints;
 	}
 
@@ -495,7 +503,7 @@ static int ipheth_probe(struct usb_interface *intf,
 
 	retval = ipheth_alloc_urbs(dev);
 	if (retval) {
-		err("error allocating urbs: %d", retval);
+		dev_err(&intf->dev, "error allocating urbs: %d\n", retval);
 		goto err_alloc_urbs;
 	}
 
@@ -506,7 +514,7 @@ static int ipheth_probe(struct usb_interface *intf,
 
 	retval = register_netdev(netdev);
 	if (retval) {
-		err("error registering netdev: %d", retval);
+		dev_err(&intf->dev, "error registering netdev: %d\n", retval);
 		retval = -EIO;
 		goto err_register_netdev;
 	}
@@ -546,6 +554,7 @@ static struct usb_driver ipheth_driver = {
 	.probe =	ipheth_probe,
 	.disconnect =	ipheth_disconnect,
 	.id_table =	ipheth_table,
+	.disable_hub_initiated_lpm = 1,
 };
 
 module_usb_driver(ipheth_driver);
diff --git a/drivers/net/usb/kalmia.c b/drivers/net/usb/kalmia.c
index 7562649b3d6..92c49e0a59e 100644
--- a/drivers/net/usb/kalmia.c
+++ b/drivers/net/usb/kalmia.c
@@ -372,7 +372,8 @@ static struct usb_driver kalmia_driver = {
 	.probe = usbnet_probe,
 	.disconnect = usbnet_disconnect,
 	.suspend = usbnet_suspend,
-	.resume = usbnet_resume
+	.resume = usbnet_resume,
+	.disable_hub_initiated_lpm = 1,
 };
 
 module_usb_driver(kalmia_driver);
diff --git a/drivers/net/usb/kaweth.c b/drivers/net/usb/kaweth.c
index df2a2cf35a9..d8ad5528438 100644
--- a/drivers/net/usb/kaweth.c
+++ b/drivers/net/usb/kaweth.c
@@ -179,6 +179,7 @@ static struct usb_driver kaweth_driver = {
 	.resume =	kaweth_resume,
 	.id_table =     usb_klsi_table,
 	.supports_autosuspend =	1,
+	.disable_hub_initiated_lpm = 1,
 };
 
 typedef __u8 eth_addr_t[6];
@@ -400,12 +401,13 @@ static int kaweth_download_firmware(struct kaweth_device *kaweth,
 
 	ret = request_firmware(&fw, fwname, &kaweth->dev->dev);
 	if (ret) {
-		err("Firmware request failed\n");
+		dev_err(&kaweth->intf->dev, "Firmware request failed\n");
 		return ret;
 	}
 
 	if (fw->size > KAWETH_FIRMWARE_BUF_SIZE) {
-		err("Firmware too big: %zu", fw->size);
+		dev_err(&kaweth->intf->dev, "Firmware too big: %zu\n",
+			fw->size);
 		release_firmware(fw);
 		return -ENOSPC;
 	}
@@ -501,9 +503,10 @@ static void kaweth_resubmit_int_urb(struct kaweth_device *kaweth, gfp_t mf)
 	}
 
 	if (status)
-		err ("can't resubmit intr, %s-%s, status %d",
-				kaweth->dev->bus->bus_name,
-				kaweth->dev->devpath, status);
+		dev_err(&kaweth->intf->dev,
+			"can't resubmit intr, %s-%s, status %d\n",
+			kaweth->dev->bus->bus_name,
+			kaweth->dev->devpath, status);
 }
 
 static void int_callback(struct urb *u)
@@ -576,7 +579,8 @@ static int kaweth_resubmit_rx_urb(struct kaweth_device *kaweth,
 			kaweth->suspend_lowmem_rx = 1;
 			schedule_delayed_work(&kaweth->lowmem_work, HZ/4);
 		}
-		err("resubmitting rx_urb %d failed", result);
+		dev_err(&kaweth->intf->dev, "resubmitting rx_urb %d failed\n",
+			result);
 	} else {
 		kaweth->suspend_lowmem_rx = 0;
 	}
@@ -634,20 +638,21 @@ static void kaweth_usb_receive(struct urb *urb)
 	spin_unlock(&kaweth->device_lock);
 
 	if(status && status != -EREMOTEIO && count != 1) {
-		err("%s RX status: %d count: %d packet_len: %d",
-                           net->name,
-			   status,
-			   count,
-			   (int)pkt_len);
+		dev_err(&kaweth->intf->dev,
+			"%s RX status: %d count: %d packet_len: %d\n",
+			net->name, status, count, (int)pkt_len);
 		kaweth_resubmit_rx_urb(kaweth, GFP_ATOMIC);
                 return;
 	}
 
 	if(kaweth->net && (count > 2)) {
 		if(pkt_len > (count - 2)) {
-			err("Packet length too long for USB frame (pkt_len: %x, count: %x)",pkt_len, count);
-			err("Packet len & 2047: %x", pkt_len & 2047);
-			err("Count 2: %x", count2);
+			dev_err(&kaweth->intf->dev,
+				"Packet length too long for USB frame (pkt_len: %x, count: %x)\n",
+				pkt_len, count);
+			dev_err(&kaweth->intf->dev, "Packet len & 2047: %x\n",
+				pkt_len & 2047);
+			dev_err(&kaweth->intf->dev, "Count 2: %x\n", count2);
 		        kaweth_resubmit_rx_urb(kaweth, GFP_ATOMIC);
                         return;
                 }
@@ -686,7 +691,7 @@ static int kaweth_open(struct net_device *net)
 
 	res = usb_autopm_get_interface(kaweth->intf);
 	if (res) {
-		err("Interface cannot be resumed.");
+		dev_err(&kaweth->intf->dev, "Interface cannot be resumed.\n");
 		return -EIO;
 	}
 	res = kaweth_resubmit_rx_urb(kaweth, GFP_KERNEL);
@@ -907,7 +912,8 @@ static void kaweth_async_set_rx_mode(struct kaweth_device *kaweth)
 				KAWETH_CONTROL_TIMEOUT);
 
 	if(result < 0) {
-		err("Failed to set Rx mode: %d", result);
+		dev_err(&kaweth->intf->dev, "Failed to set Rx mode: %d\n",
+			result);
 	}
 	else {
 		dbg("Set Rx mode to %d", packet_filter_bitmap);
@@ -1045,7 +1051,8 @@ static int kaweth_probe(
 						      "kaweth/new_code.bin",
 						      100,
 						      2)) < 0) {
-			err("Error downloading firmware (%d)", result);
+			dev_err(&intf->dev, "Error downloading firmware (%d)\n",
+				result);
 			goto err_fw;
 		}
 
@@ -1053,7 +1060,9 @@ static int kaweth_probe(
 						      "kaweth/new_code_fix.bin",
 						      100,
 						      3)) < 0) {
-			err("Error downloading firmware fix (%d)", result);
+			dev_err(&intf->dev,
+				"Error downloading firmware fix (%d)\n",
+				result);
 			goto err_fw;
 		}
 
@@ -1061,7 +1070,9 @@ static int kaweth_probe(
 						      "kaweth/trigger_code.bin",
 						      126,
 						      2)) < 0) {
-			err("Error downloading trigger code (%d)", result);
+			dev_err(&intf->dev,
+				"Error downloading trigger code (%d)\n",
+				result);
 			goto err_fw;
 
 		}
@@ -1070,13 +1081,14 @@ static int kaweth_probe(
 						      "kaweth/trigger_code_fix.bin",
 						      126,
 						      3)) < 0) {
-			err("Error downloading trigger code fix (%d)", result);
+			dev_err(&intf->dev, "Error downloading trigger code fix (%d)\n", result);
 			goto err_fw;
 		}
 
 
 		if ((result = kaweth_trigger_firmware(kaweth, 126)) < 0) {
-			err("Error triggering firmware (%d)", result);
+			dev_err(&intf->dev, "Error triggering firmware (%d)\n",
+				result);
 			goto err_fw;
 		}
 
@@ -1091,7 +1103,7 @@ err_fw:
 	result = kaweth_read_configuration(kaweth);
 
 	if(result < 0) {
-		err("Error reading configuration (%d), no net device created", result);
+		dev_err(&intf->dev, "Error reading configuration (%d), no net device created\n", result);
 		goto err_free_netdev;
 	}
 
@@ -1103,7 +1115,7 @@ err_fw:
 	if(!memcmp(&kaweth->configuration.hw_addr,
                    &bcast_addr,
 		   sizeof(bcast_addr))) {
-		err("Firmware not functioning properly, no net device created");
+		dev_err(&intf->dev, "Firmware not functioning properly, no net device created\n");
 		goto err_free_netdev;
 	}
 
@@ -1113,7 +1125,7 @@ err_fw:
 	}
 
 	if(kaweth_set_sofs_wait(kaweth, KAWETH_SOFS_TO_WAIT) < 0) {
-		err("Error setting SOFS wait");
+		dev_err(&intf->dev, "Error setting SOFS wait\n");
 		goto err_free_netdev;
 	}
 
@@ -1123,7 +1135,7 @@ err_fw:
                                            KAWETH_PACKET_FILTER_MULTICAST);
 
 	if(result < 0) {
-		err("Error setting receive filter");
+		dev_err(&intf->dev, "Error setting receive filter\n");
 		goto err_free_netdev;
 	}
 
@@ -1175,7 +1187,7 @@ err_fw:
 
 	SET_NETDEV_DEV(netdev, &intf->dev);
 	if (register_netdev(netdev) != 0) {
-		err("Error registering netdev.");
+		dev_err(&intf->dev, "Error registering netdev.\n");
 		goto err_intfdata;
 	}
 
diff --git a/drivers/net/usb/lg-vl600.c b/drivers/net/usb/lg-vl600.c
index 45a981fde43..808d6506da4 100644
--- a/drivers/net/usb/lg-vl600.c
+++ b/drivers/net/usb/lg-vl600.c
@@ -344,6 +344,7 @@ static struct usb_driver lg_vl600_driver = {
 	.disconnect	= usbnet_disconnect,
 	.suspend	= usbnet_suspend,
 	.resume		= usbnet_resume,
+	.disable_hub_initiated_lpm = 1,
 };
 
 module_usb_driver(lg_vl600_driver);
diff --git a/drivers/net/usb/mcs7830.c b/drivers/net/usb/mcs7830.c
index c434b6ba033..add1064f755 100644
--- a/drivers/net/usb/mcs7830.c
+++ b/drivers/net/usb/mcs7830.c
@@ -690,6 +690,7 @@ static struct usb_driver mcs7830_driver = {
 	.suspend = usbnet_suspend,
 	.resume = usbnet_resume,
 	.reset_resume = mcs7830_reset_resume,
+	.disable_hub_initiated_lpm = 1,
 };
 
 module_usb_driver(mcs7830_driver);
diff --git a/drivers/net/usb/net1080.c b/drivers/net/usb/net1080.c
index 83f965cb69e..28c4d513ba8 100644
--- a/drivers/net/usb/net1080.c
+++ b/drivers/net/usb/net1080.c
@@ -587,6 +587,7 @@ static struct usb_driver net1080_driver = {
 	.disconnect =	usbnet_disconnect,
 	.suspend =	usbnet_suspend,
 	.resume =	usbnet_resume,
+	.disable_hub_initiated_lpm = 1,
 };
 
 module_usb_driver(net1080_driver);
diff --git a/drivers/net/usb/pegasus.c b/drivers/net/usb/pegasus.c
index 75239309232..7023220456c 100644
--- a/drivers/net/usb/pegasus.c
+++ b/drivers/net/usb/pegasus.c
@@ -1489,6 +1489,7 @@ static struct usb_driver pegasus_driver = {
 	.id_table = pegasus_ids,
 	.suspend = pegasus_suspend,
 	.resume = pegasus_resume,
+	.disable_hub_initiated_lpm = 1,
 };
 
 static void __init parse_id(char *id)
diff --git a/drivers/net/usb/plusb.c b/drivers/net/usb/plusb.c
index b2b035e2997..4584b9a805b 100644
--- a/drivers/net/usb/plusb.c
+++ b/drivers/net/usb/plusb.c
@@ -152,6 +152,7 @@ static struct usb_driver plusb_driver = {
 	.disconnect =	usbnet_disconnect,
 	.suspend =	usbnet_suspend,
 	.resume =	usbnet_resume,
+	.disable_hub_initiated_lpm = 1,
 };
 
 module_usb_driver(plusb_driver);
diff --git a/drivers/net/usb/qmi_wwan.c b/drivers/net/usb/qmi_wwan.c
index d316503b35d..380dbea6109 100644
--- a/drivers/net/usb/qmi_wwan.c
+++ b/drivers/net/usb/qmi_wwan.c
@@ -356,10 +356,19 @@ static const struct driver_info	qmi_wwan_gobi = {
 };
 
 /* ZTE suck at making USB descriptors */
+static const struct driver_info	qmi_wwan_force_int1 = {
+	.description	= "Qualcomm WWAN/QMI device",
+	.flags		= FLAG_WWAN,
+	.bind		= qmi_wwan_bind_shared,
+	.unbind		= qmi_wwan_unbind_shared,
+	.manage_power	= qmi_wwan_manage_power,
+	.data		= BIT(1), /* interface whitelist bitmap */
+};
+
 static const struct driver_info	qmi_wwan_force_int4 = {
-	.description	= "Qualcomm Gobi wwan/QMI device",
+	.description	= "Qualcomm WWAN/QMI device",
 	.flags		= FLAG_WWAN,
-	.bind		= qmi_wwan_bind_gobi,
+	.bind		= qmi_wwan_bind_shared,
 	.unbind		= qmi_wwan_unbind_shared,
 	.manage_power	= qmi_wwan_manage_power,
 	.data		= BIT(4), /* interface whitelist bitmap */
@@ -401,6 +410,14 @@ static const struct usb_device_id products[] = {
 		.bInterfaceProtocol = 8, /* NOTE: This is the *slave* interface of the CDC Union! */
 		.driver_info        = (unsigned long)&qmi_wwan_info,
 	},
+	{	/* Vodafone/Huawei K5005 (12d1:14c8) and similar modems */
+		.match_flags        = USB_DEVICE_ID_MATCH_VENDOR | USB_DEVICE_ID_MATCH_INT_INFO,
+		.idVendor           = HUAWEI_VENDOR_ID,
+		.bInterfaceClass    = USB_CLASS_VENDOR_SPEC,
+		.bInterfaceSubClass = 1,
+		.bInterfaceProtocol = 56, /* NOTE: This is the *slave* interface of the CDC Union! */
+		.driver_info        = (unsigned long)&qmi_wwan_info,
+	},
 	{	/* Huawei E392, E398 and possibly others in "Windows mode"
 		 * using a combined control and data interface without any CDC
 		 * functional descriptors
@@ -430,6 +447,15 @@ static const struct usb_device_id products[] = {
 		.bInterfaceProtocol = 0xff,
 		.driver_info        = (unsigned long)&qmi_wwan_force_int4,
 	},
+	{	/* ZTE (Vodafone) K3520-Z */
+		.match_flags	    = USB_DEVICE_ID_MATCH_DEVICE | USB_DEVICE_ID_MATCH_INT_INFO,
+		.idVendor           = 0x19d2,
+		.idProduct          = 0x0055,
+		.bInterfaceClass    = 0xff,
+		.bInterfaceSubClass = 0xff,
+		.bInterfaceProtocol = 0xff,
+		.driver_info        = (unsigned long)&qmi_wwan_force_int1,
+	},
 	{	/* ZTE (Vodafone) K3565-Z */
 		.match_flags	    = USB_DEVICE_ID_MATCH_DEVICE | USB_DEVICE_ID_MATCH_INT_INFO,
 		.idVendor           = 0x19d2,
@@ -457,6 +483,15 @@ static const struct usb_device_id products[] = {
 		.bInterfaceProtocol = 0xff,
 		.driver_info        = (unsigned long)&qmi_wwan_force_int4,
 	},
+	{	/* ZTE (Vodafone) K3765-Z */
+		.match_flags	    = USB_DEVICE_ID_MATCH_DEVICE | USB_DEVICE_ID_MATCH_INT_INFO,
+		.idVendor           = 0x19d2,
+		.idProduct          = 0x2002,
+		.bInterfaceClass    = 0xff,
+		.bInterfaceSubClass = 0xff,
+		.bInterfaceProtocol = 0xff,
+		.driver_info        = (unsigned long)&qmi_wwan_force_int4,
+	},
 	{	/* ZTE (Vodafone) K4505-Z */
 		.match_flags	    = USB_DEVICE_ID_MATCH_DEVICE | USB_DEVICE_ID_MATCH_INT_INFO,
 		.idVendor           = 0x19d2,
@@ -525,6 +560,7 @@ static struct usb_driver qmi_wwan_driver = {
 	.resume		      =	qmi_wwan_resume,
 	.reset_resume         = qmi_wwan_resume,
 	.supports_autosuspend = 1,
+	.disable_hub_initiated_lpm = 1,
 };
 
 static int __init qmi_wwan_init(void)
diff --git a/drivers/net/usb/rndis_host.c b/drivers/net/usb/rndis_host.c
index c8f1b5b3aff..4a4335833c3 100644
--- a/drivers/net/usb/rndis_host.c
+++ b/drivers/net/usb/rndis_host.c
@@ -77,7 +77,9 @@ static void rndis_msg_indicate(struct usbnet *dev, struct rndis_indicate *msg,
 	if (dev->driver_info->indication) {
 		dev->driver_info->indication(dev, msg, buflen);
 	} else {
-		switch (msg->status) {
+		u32 status = le32_to_cpu(msg->status);
+
+		switch (status) {
 		case RNDIS_STATUS_MEDIA_CONNECT:
 			dev_info(udev, "rndis media connect\n");
 			break;
@@ -85,8 +87,7 @@ static void rndis_msg_indicate(struct usbnet *dev, struct rndis_indicate *msg,
 			dev_info(udev, "rndis media disconnect\n");
 			break;
 		default:
-			dev_info(udev, "rndis indication: 0x%08x\n",
-					le32_to_cpu(msg->status));
+			dev_info(udev, "rndis indication: 0x%08x\n", status);
 		}
 	}
 }
@@ -109,16 +110,17 @@ int rndis_command(struct usbnet *dev, struct rndis_msg_hdr *buf, int buflen)
 	int			retval;
 	int			partial;
 	unsigned		count;
-	__le32			rsp;
-	u32			xid = 0, msg_len, request_id;
+	u32			xid = 0, msg_len, request_id, msg_type, rsp,
+				status;
 
 	/* REVISIT when this gets called from contexts other than probe() or
 	 * disconnect(): either serialize, or dispatch responses on xid
 	 */
 
+	msg_type = le32_to_cpu(buf->msg_type);
+
 	/* Issue the request; xid is unique, don't bother byteswapping it */
-	if (likely(buf->msg_type != RNDIS_MSG_HALT &&
-		   buf->msg_type != RNDIS_MSG_RESET)) {
+	if (likely(msg_type != RNDIS_MSG_HALT && msg_type != RNDIS_MSG_RESET)) {
 		xid = dev->xid++;
 		if (!xid)
 			xid = dev->xid++;
@@ -149,7 +151,7 @@ int rndis_command(struct usbnet *dev, struct rndis_msg_hdr *buf, int buflen)
 	}
 
 	/* Poll the control channel; the request probably completed immediately */
-	rsp = buf->msg_type | RNDIS_MSG_COMPLETION;
+	rsp = le32_to_cpu(buf->msg_type) | RNDIS_MSG_COMPLETION;
 	for (count = 0; count < 10; count++) {
 		memset(buf, 0, CONTROL_BUFFER_SIZE);
 		retval = usb_control_msg(dev->udev,
@@ -160,35 +162,36 @@ int rndis_command(struct usbnet *dev, struct rndis_msg_hdr *buf, int buflen)
 			buf, buflen,
 			RNDIS_CONTROL_TIMEOUT_MS);
 		if (likely(retval >= 8)) {
+			msg_type = le32_to_cpu(buf->msg_type);
 			msg_len = le32_to_cpu(buf->msg_len);
+			status = le32_to_cpu(buf->status);
 			request_id = (__force u32) buf->request_id;
-			if (likely(buf->msg_type == rsp)) {
+			if (likely(msg_type == rsp)) {
 				if (likely(request_id == xid)) {
 					if (unlikely(rsp == RNDIS_MSG_RESET_C))
 						return 0;
-					if (likely(RNDIS_STATUS_SUCCESS
-							== buf->status))
+					if (likely(RNDIS_STATUS_SUCCESS ==
+							status))
 						return 0;
 					dev_dbg(&info->control->dev,
 						"rndis reply status %08x\n",
-						le32_to_cpu(buf->status));
+						status);
 					return -EL3RST;
 				}
 				dev_dbg(&info->control->dev,
 					"rndis reply id %d expected %d\n",
 					request_id, xid);
 				/* then likely retry */
-			} else switch (buf->msg_type) {
-			case RNDIS_MSG_INDICATE:	/* fault/event */
+			} else switch (msg_type) {
+			case RNDIS_MSG_INDICATE: /* fault/event */
 				rndis_msg_indicate(dev, (void *)buf, buflen);
-
 				break;
-			case RNDIS_MSG_KEEPALIVE: {	/* ping */
+			case RNDIS_MSG_KEEPALIVE: { /* ping */
 				struct rndis_keepalive_c *msg = (void *)buf;
 
-				msg->msg_type = RNDIS_MSG_KEEPALIVE_C;
+				msg->msg_type = cpu_to_le32(RNDIS_MSG_KEEPALIVE_C);
 				msg->msg_len = cpu_to_le32(sizeof *msg);
-				msg->status = RNDIS_STATUS_SUCCESS;
+				msg->status = cpu_to_le32(RNDIS_STATUS_SUCCESS);
 				retval = usb_control_msg(dev->udev,
 					usb_sndctrlpipe(dev->udev, 0),
 					USB_CDC_SEND_ENCAPSULATED_COMMAND,
@@ -236,7 +239,7 @@ EXPORT_SYMBOL_GPL(rndis_command);
  * ActiveSync 4.1 Windows driver.
  */
 static int rndis_query(struct usbnet *dev, struct usb_interface *intf,
-		void *buf, __le32 oid, u32 in_len,
+		void *buf, u32 oid, u32 in_len,
 		void **reply, int *reply_len)
 {
 	int retval;
@@ -251,9 +254,9 @@ static int rndis_query(struct usbnet *dev, struct usb_interface *intf,
 	u.buf = buf;
 
 	memset(u.get, 0, sizeof *u.get + in_len);
-	u.get->msg_type = RNDIS_MSG_QUERY;
+	u.get->msg_type = cpu_to_le32(RNDIS_MSG_QUERY);
 	u.get->msg_len = cpu_to_le32(sizeof *u.get + in_len);
-	u.get->oid = oid;
+	u.get->oid = cpu_to_le32(oid);
 	u.get->len = cpu_to_le32(in_len);
 	u.get->offset = cpu_to_le32(20);
 
@@ -324,7 +327,7 @@ generic_rndis_bind(struct usbnet *dev, struct usb_interface *intf, int flags)
 	if (retval < 0)
 		goto fail;
 
-	u.init->msg_type = RNDIS_MSG_INIT;
+	u.init->msg_type = cpu_to_le32(RNDIS_MSG_INIT);
 	u.init->msg_len = cpu_to_le32(sizeof *u.init);
 	u.init->major_version = cpu_to_le32(1);
 	u.init->minor_version = cpu_to_le32(0);
@@ -395,22 +398,23 @@ generic_rndis_bind(struct usbnet *dev, struct usb_interface *intf, int flags)
 	/* Check physical medium */
 	phym = NULL;
 	reply_len = sizeof *phym;
-	retval = rndis_query(dev, intf, u.buf, OID_GEN_PHYSICAL_MEDIUM,
-			0, (void **) &phym, &reply_len);
+	retval = rndis_query(dev, intf, u.buf,
+			     RNDIS_OID_GEN_PHYSICAL_MEDIUM,
+			     0, (void **) &phym, &reply_len);
 	if (retval != 0 || !phym) {
 		/* OID is optional so don't fail here. */
-		phym_unspec = RNDIS_PHYSICAL_MEDIUM_UNSPECIFIED;
+		phym_unspec = cpu_to_le32(RNDIS_PHYSICAL_MEDIUM_UNSPECIFIED);
 		phym = &phym_unspec;
 	}
 	if ((flags & FLAG_RNDIS_PHYM_WIRELESS) &&
-			*phym != RNDIS_PHYSICAL_MEDIUM_WIRELESS_LAN) {
+	    le32_to_cpup(phym) != RNDIS_PHYSICAL_MEDIUM_WIRELESS_LAN) {
 		netif_dbg(dev, probe, dev->net,
 			  "driver requires wireless physical medium, but device is not\n");
 		retval = -ENODEV;
 		goto halt_fail_and_release;
 	}
 	if ((flags & FLAG_RNDIS_PHYM_NOT_WIRELESS) &&
-			*phym == RNDIS_PHYSICAL_MEDIUM_WIRELESS_LAN) {
+	    le32_to_cpup(phym) == RNDIS_PHYSICAL_MEDIUM_WIRELESS_LAN) {
 		netif_dbg(dev, probe, dev->net,
 			  "driver requires non-wireless physical medium, but device is wireless.\n");
 		retval = -ENODEV;
@@ -419,8 +423,9 @@ generic_rndis_bind(struct usbnet *dev, struct usb_interface *intf, int flags)
 
 	/* Get designated host ethernet address */
 	reply_len = ETH_ALEN;
-	retval = rndis_query(dev, intf, u.buf, OID_802_3_PERMANENT_ADDRESS,
-			48, (void **) &bp, &reply_len);
+	retval = rndis_query(dev, intf, u.buf,
+			     RNDIS_OID_802_3_PERMANENT_ADDRESS,
+			     48, (void **) &bp, &reply_len);
 	if (unlikely(retval< 0)) {
 		dev_err(&intf->dev, "rndis get ethaddr, %d\n", retval);
 		goto halt_fail_and_release;
@@ -430,12 +435,12 @@ generic_rndis_bind(struct usbnet *dev, struct usb_interface *intf, int flags)
 
 	/* set a nonzero filter to enable data transfers */
 	memset(u.set, 0, sizeof *u.set);
-	u.set->msg_type = RNDIS_MSG_SET;
+	u.set->msg_type = cpu_to_le32(RNDIS_MSG_SET);
 	u.set->msg_len = cpu_to_le32(4 + sizeof *u.set);
-	u.set->oid = OID_GEN_CURRENT_PACKET_FILTER;
+	u.set->oid = cpu_to_le32(RNDIS_OID_GEN_CURRENT_PACKET_FILTER);
 	u.set->len = cpu_to_le32(4);
 	u.set->offset = cpu_to_le32((sizeof *u.set) - 8);
-	*(__le32 *)(u.buf + sizeof *u.set) = RNDIS_DEFAULT_FILTER;
+	*(__le32 *)(u.buf + sizeof *u.set) = cpu_to_le32(RNDIS_DEFAULT_FILTER);
 
 	retval = rndis_command(dev, u.header, CONTROL_BUFFER_SIZE);
 	if (unlikely(retval < 0)) {
@@ -450,7 +455,7 @@ generic_rndis_bind(struct usbnet *dev, struct usb_interface *intf, int flags)
 
 halt_fail_and_release:
 	memset(u.halt, 0, sizeof *u.halt);
-	u.halt->msg_type = RNDIS_MSG_HALT;
+	u.halt->msg_type = cpu_to_le32(RNDIS_MSG_HALT);
 	u.halt->msg_len = cpu_to_le32(sizeof *u.halt);
 	(void) rndis_command(dev, (void *)u.halt, CONTROL_BUFFER_SIZE);
 fail_and_release:
@@ -475,7 +480,7 @@ void rndis_unbind(struct usbnet *dev, struct usb_interface *intf)
 	/* try to clear any rndis state/activity (no i/o from stack!) */
 	halt = kzalloc(CONTROL_BUFFER_SIZE, GFP_KERNEL);
 	if (halt) {
-		halt->msg_type = RNDIS_MSG_HALT;
+		halt->msg_type = cpu_to_le32(RNDIS_MSG_HALT);
 		halt->msg_len = cpu_to_le32(sizeof *halt);
 		(void) rndis_command(dev, (void *)halt, CONTROL_BUFFER_SIZE);
 		kfree(halt);
@@ -494,16 +499,16 @@ int rndis_rx_fixup(struct usbnet *dev, struct sk_buff *skb)
 	while (likely(skb->len)) {
 		struct rndis_data_hdr	*hdr = (void *)skb->data;
 		struct sk_buff		*skb2;
-		u32			msg_len, data_offset, data_len;
+		u32			msg_type, msg_len, data_offset, data_len;
 
+		msg_type = le32_to_cpu(hdr->msg_type);
 		msg_len = le32_to_cpu(hdr->msg_len);
 		data_offset = le32_to_cpu(hdr->data_offset);
 		data_len = le32_to_cpu(hdr->data_len);
 
 		/* don't choke if we see oob, per-packet data, etc */
-		if (unlikely(hdr->msg_type != RNDIS_MSG_PACKET ||
-			     skb->len < msg_len ||
-			     (data_offset + data_len + 8) > msg_len)) {
+		if (unlikely(msg_type != RNDIS_MSG_PACKET || skb->len < msg_len
+				|| (data_offset + data_len + 8) > msg_len)) {
 			dev->net->stats.rx_frame_errors++;
 			netdev_dbg(dev->net, "bad rndis message %d/%d/%d/%d, len %d\n",
 				   le32_to_cpu(hdr->msg_type),
@@ -569,7 +574,7 @@ rndis_tx_fixup(struct usbnet *dev, struct sk_buff *skb, gfp_t flags)
 fill:
 	hdr = (void *) __skb_push(skb, sizeof *hdr);
 	memset(hdr, 0, sizeof *hdr);
-	hdr->msg_type = RNDIS_MSG_PACKET;
+	hdr->msg_type = cpu_to_le32(RNDIS_MSG_PACKET);
 	hdr->msg_len = cpu_to_le32(skb->len);
 	hdr->data_offset = cpu_to_le32(sizeof(*hdr) - 8);
 	hdr->data_len = cpu_to_le32(len);
@@ -633,6 +638,7 @@ static struct usb_driver rndis_driver = {
 	.disconnect =	usbnet_disconnect,
 	.suspend =	usbnet_suspend,
 	.resume =	usbnet_resume,
+	.disable_hub_initiated_lpm = 1,
 };
 
 module_usb_driver(rndis_driver);
diff --git a/drivers/net/usb/rtl8150.c b/drivers/net/usb/rtl8150.c
index d363b31053d..0e2c92e0e53 100644
--- a/drivers/net/usb/rtl8150.c
+++ b/drivers/net/usb/rtl8150.c
@@ -203,7 +203,8 @@ static int async_set_registers(rtl8150_t * dev, u16 indx, u16 size)
 	if ((ret = usb_submit_urb(dev->ctrl_urb, GFP_ATOMIC))) {
 		if (ret == -ENODEV)
 			netif_device_detach(dev->netdev);
-		err("control request submission failed: %d", ret);
+		dev_err(&dev->udev->dev,
+			"control request submission failed: %d\n", ret);
 	} else
 		set_bit(RX_REG_SET, &dev->flags);
 
@@ -516,9 +517,9 @@ resubmit:
 	if (res == -ENODEV)
 		netif_device_detach(dev->netdev);
 	else if (res)
-		err ("can't resubmit intr, %s-%s/input0, status %d",
-				dev->udev->bus->bus_name,
-				dev->udev->devpath, res);
+		dev_err(&dev->udev->dev,
+			"can't resubmit intr, %s-%s/input0, status %d\n",
+			dev->udev->bus->bus_name, dev->udev->devpath, res);
 }
 
 static int rtl8150_suspend(struct usb_interface *intf, pm_message_t message)
@@ -890,11 +891,11 @@ static int rtl8150_probe(struct usb_interface *intf,
 	dev->intr_interval = 100;	/* 100ms */
 
 	if (!alloc_all_urbs(dev)) {
-		err("out of memory");
+		dev_err(&intf->dev, "out of memory\n");
 		goto out;
 	}
 	if (!rtl8150_reset(dev)) {
-		err("couldn't reset the device");
+		dev_err(&intf->dev, "couldn't reset the device\n");
 		goto out1;
 	}
 	fill_skb_pool(dev);
@@ -903,7 +904,7 @@ static int rtl8150_probe(struct usb_interface *intf,
 	usb_set_intfdata(intf, dev);
 	SET_NETDEV_DEV(netdev, &intf->dev);
 	if (register_netdev(netdev) != 0) {
-		err("couldn't register the device");
+		dev_err(&intf->dev, "couldn't register the device\n");
 		goto out2;
 	}
 
@@ -947,7 +948,8 @@ static struct usb_driver rtl8150_driver = {
 	.disconnect	= rtl8150_disconnect,
 	.id_table	= rtl8150_table,
 	.suspend	= rtl8150_suspend,
-	.resume		= rtl8150_resume
+	.resume		= rtl8150_resume,
+	.disable_hub_initiated_lpm = 1,
 };
 
 module_usb_driver(rtl8150_driver);
diff --git a/drivers/net/usb/sierra_net.c b/drivers/net/usb/sierra_net.c
index b59cf20c781..3faef5670d1 100644
--- a/drivers/net/usb/sierra_net.c
+++ b/drivers/net/usb/sierra_net.c
@@ -982,6 +982,7 @@ static struct usb_driver sierra_net_driver = {
 	.suspend = usbnet_suspend,
 	.resume = usbnet_resume,
 	.no_dynamic_id = 1,
+	.disable_hub_initiated_lpm = 1,
 };
 
 module_usb_driver(sierra_net_driver);
diff --git a/drivers/net/usb/smsc75xx.c b/drivers/net/usb/smsc75xx.c
index 00103a8c5e0..1c6e51588da 100644
--- a/drivers/net/usb/smsc75xx.c
+++ b/drivers/net/usb/smsc75xx.c
@@ -508,10 +508,9 @@ static int smsc75xx_link_reset(struct usbnet *dev)
 	u16 lcladv, rmtadv;
 	int ret;
 
-	/* read and write to clear phy interrupt status */
-	ret = smsc75xx_mdio_read(dev->net, mii->phy_id, PHY_INT_SRC);
-	check_warn_return(ret, "Error reading PHY_INT_SRC");
-	smsc75xx_mdio_write(dev->net, mii->phy_id, PHY_INT_SRC, 0xffff);
+	/* write to clear phy interrupt status */
+	smsc75xx_mdio_write(dev->net, mii->phy_id, PHY_INT_SRC,
+		PHY_INT_SRC_CLEAR_ALL);
 
 	ret = smsc75xx_write_reg(dev, INT_STS, INT_STS_CLEAR_ALL);
 	check_warn_return(ret, "Error writing INT_STS");
@@ -904,15 +903,20 @@ static int smsc75xx_reset(struct usbnet *dev)
 
 	netif_dbg(dev, ifup, dev->net, "ID_REV = 0x%08x", buf);
 
-	/* Configure GPIO pins as LED outputs */
-	ret = smsc75xx_read_reg(dev, LED_GPIO_CFG, &buf);
-	check_warn_return(ret, "Failed to read LED_GPIO_CFG: %d", ret);
+	ret = smsc75xx_read_reg(dev, E2P_CMD, &buf);
+	check_warn_return(ret, "Failed to read E2P_CMD: %d", ret);
 
-	buf &= ~(LED_GPIO_CFG_LED2_FUN_SEL | LED_GPIO_CFG_LED10_FUN_SEL);
-	buf |= LED_GPIO_CFG_LEDGPIO_EN | LED_GPIO_CFG_LED2_FUN_SEL;
+	/* only set default GPIO/LED settings if no EEPROM is detected */
+	if (!(buf & E2P_CMD_LOADED)) {
+		ret = smsc75xx_read_reg(dev, LED_GPIO_CFG, &buf);
+		check_warn_return(ret, "Failed to read LED_GPIO_CFG: %d", ret);
 
-	ret = smsc75xx_write_reg(dev, LED_GPIO_CFG, buf);
-	check_warn_return(ret, "Failed to write LED_GPIO_CFG: %d", ret);
+		buf &= ~(LED_GPIO_CFG_LED2_FUN_SEL | LED_GPIO_CFG_LED10_FUN_SEL);
+		buf |= LED_GPIO_CFG_LEDGPIO_EN | LED_GPIO_CFG_LED2_FUN_SEL;
+
+		ret = smsc75xx_write_reg(dev, LED_GPIO_CFG, buf);
+		check_warn_return(ret, "Failed to write LED_GPIO_CFG: %d", ret);
+	}
 
 	ret = smsc75xx_write_reg(dev, FLOW, 0);
 	check_warn_return(ret, "Failed to write FLOW: %d", ret);
@@ -1250,6 +1254,7 @@ static struct usb_driver smsc75xx_driver = {
 	.suspend	= usbnet_suspend,
 	.resume		= usbnet_resume,
 	.disconnect	= usbnet_disconnect,
+	.disable_hub_initiated_lpm = 1,
 };
 
 module_usb_driver(smsc75xx_driver);
diff --git a/drivers/net/usb/smsc75xx.h b/drivers/net/usb/smsc75xx.h
index 16e98c77834..67eba39e6ee 100644
--- a/drivers/net/usb/smsc75xx.h
+++ b/drivers/net/usb/smsc75xx.h
@@ -388,6 +388,7 @@
 #define PHY_INT_SRC_ANEG_COMP		((u16)0x0040)
 #define PHY_INT_SRC_REMOTE_FAULT	((u16)0x0020)
 #define PHY_INT_SRC_LINK_DOWN		((u16)0x0010)
+#define PHY_INT_SRC_CLEAR_ALL		((u16)0xffff)
 
 #define PHY_INT_MASK			(30)
 #define PHY_INT_MASK_ENERGY_ON		((u16)0x0080)
diff --git a/drivers/net/usb/smsc95xx.c b/drivers/net/usb/smsc95xx.c
index 94ae66999f5..b1112e75385 100644
--- a/drivers/net/usb/smsc95xx.c
+++ b/drivers/net/usb/smsc95xx.c
@@ -1297,6 +1297,7 @@ static struct usb_driver smsc95xx_driver = {
 	.suspend	= usbnet_suspend,
 	.resume		= usbnet_resume,
 	.disconnect	= usbnet_disconnect,
+	.disable_hub_initiated_lpm = 1,
 };
 
 module_usb_driver(smsc95xx_driver);
diff --git a/drivers/net/usb/usbnet.c b/drivers/net/usb/usbnet.c
index 2d927fb4adf..9f58330f131 100644
--- a/drivers/net/usb/usbnet.c
+++ b/drivers/net/usb/usbnet.c
@@ -282,17 +282,32 @@ int usbnet_change_mtu (struct net_device *net, int new_mtu)
 }
 EXPORT_SYMBOL_GPL(usbnet_change_mtu);
 
+/* The caller must hold list->lock */
+static void __usbnet_queue_skb(struct sk_buff_head *list,
+			struct sk_buff *newsk, enum skb_state state)
+{
+	struct skb_data *entry = (struct skb_data *) newsk->cb;
+
+	__skb_queue_tail(list, newsk);
+	entry->state = state;
+}
+
 /*-------------------------------------------------------------------------*/
 
 /* some LK 2.4 HCDs oopsed if we freed or resubmitted urbs from
  * completion callbacks.  2.5 should have fixed those bugs...
  */
 
-static void defer_bh(struct usbnet *dev, struct sk_buff *skb, struct sk_buff_head *list)
+static enum skb_state defer_bh(struct usbnet *dev, struct sk_buff *skb,
+		struct sk_buff_head *list, enum skb_state state)
 {
 	unsigned long		flags;
+	enum skb_state 		old_state;
+	struct skb_data *entry = (struct skb_data *) skb->cb;
 
 	spin_lock_irqsave(&list->lock, flags);
+	old_state = entry->state;
+	entry->state = state;
 	__skb_unlink(skb, list);
 	spin_unlock(&list->lock);
 	spin_lock(&dev->done.lock);
@@ -300,6 +315,7 @@ static void defer_bh(struct usbnet *dev, struct sk_buff *skb, struct sk_buff_hea
 	if (dev->done.qlen == 1)
 		tasklet_schedule(&dev->bh);
 	spin_unlock_irqrestore(&dev->done.lock, flags);
+	return old_state;
 }
 
 /* some work can't be done in tasklets, so we use keventd
@@ -340,7 +356,6 @@ static int rx_submit (struct usbnet *dev, struct urb *urb, gfp_t flags)
 	entry = (struct skb_data *) skb->cb;
 	entry->urb = urb;
 	entry->dev = dev;
-	entry->state = rx_start;
 	entry->length = 0;
 
 	usb_fill_bulk_urb (urb, dev->udev, dev->in,
@@ -372,7 +387,7 @@ static int rx_submit (struct usbnet *dev, struct urb *urb, gfp_t flags)
 			tasklet_schedule (&dev->bh);
 			break;
 		case 0:
-			__skb_queue_tail (&dev->rxq, skb);
+			__usbnet_queue_skb(&dev->rxq, skb, rx_start);
 		}
 	} else {
 		netif_dbg(dev, ifdown, dev->net, "rx: stopped\n");
@@ -423,16 +438,17 @@ static void rx_complete (struct urb *urb)
 	struct skb_data		*entry = (struct skb_data *) skb->cb;
 	struct usbnet		*dev = entry->dev;
 	int			urb_status = urb->status;
+	enum skb_state		state;
 
 	skb_put (skb, urb->actual_length);
-	entry->state = rx_done;
+	state = rx_done;
 	entry->urb = NULL;
 
 	switch (urb_status) {
 	/* success */
 	case 0:
 		if (skb->len < dev->net->hard_header_len) {
-			entry->state = rx_cleanup;
+			state = rx_cleanup;
 			dev->net->stats.rx_errors++;
 			dev->net->stats.rx_length_errors++;
 			netif_dbg(dev, rx_err, dev->net,
@@ -471,7 +487,7 @@ static void rx_complete (struct urb *urb)
 				  "rx throttle %d\n", urb_status);
 		}
 block:
-		entry->state = rx_cleanup;
+		state = rx_cleanup;
 		entry->urb = urb;
 		urb = NULL;
 		break;
@@ -482,17 +498,18 @@ block:
 		// FALLTHROUGH
 
 	default:
-		entry->state = rx_cleanup;
+		state = rx_cleanup;
 		dev->net->stats.rx_errors++;
 		netif_dbg(dev, rx_err, dev->net, "rx status %d\n", urb_status);
 		break;
 	}
 
-	defer_bh(dev, skb, &dev->rxq);
+	state = defer_bh(dev, skb, &dev->rxq, state);
 
 	if (urb) {
 		if (netif_running (dev->net) &&
-		    !test_bit (EVENT_RX_HALT, &dev->flags)) {
+		    !test_bit (EVENT_RX_HALT, &dev->flags) &&
+		    state != unlink_start) {
 			rx_submit (dev, urb, GFP_ATOMIC);
 			usb_mark_last_busy(dev->udev);
 			return;
@@ -579,16 +596,23 @@ EXPORT_SYMBOL_GPL(usbnet_purge_paused_rxq);
 static int unlink_urbs (struct usbnet *dev, struct sk_buff_head *q)
 {
 	unsigned long		flags;
-	struct sk_buff		*skb, *skbnext;
+	struct sk_buff		*skb;
 	int			count = 0;
 
 	spin_lock_irqsave (&q->lock, flags);
-	skb_queue_walk_safe(q, skb, skbnext) {
+	while (!skb_queue_empty(q)) {
 		struct skb_data		*entry;
 		struct urb		*urb;
 		int			retval;
 
-		entry = (struct skb_data *) skb->cb;
+		skb_queue_walk(q, skb) {
+			entry = (struct skb_data *) skb->cb;
+			if (entry->state != unlink_start)
+				goto found;
+		}
+		break;
+found:
+		entry->state = unlink_start;
 		urb = entry->urb;
 
 		/*
@@ -885,6 +909,7 @@ static const struct ethtool_ops usbnet_ethtool_ops = {
 	.get_drvinfo		= usbnet_get_drvinfo,
 	.get_msglevel		= usbnet_get_msglevel,
 	.set_msglevel		= usbnet_set_msglevel,
+	.get_ts_info		= ethtool_op_get_ts_info,
 };
 
 /*-------------------------------------------------------------------------*/
@@ -1039,8 +1064,7 @@ static void tx_complete (struct urb *urb)
 	}
 
 	usb_autopm_put_interface_async(dev->intf);
-	entry->state = tx_done;
-	defer_bh(dev, skb, &dev->txq);
+	(void) defer_bh(dev, skb, &dev->txq, tx_done);
 }
 
 /*-------------------------------------------------------------------------*/
@@ -1096,7 +1120,6 @@ netdev_tx_t usbnet_start_xmit (struct sk_buff *skb,
 	entry = (struct skb_data *) skb->cb;
 	entry->urb = urb;
 	entry->dev = dev;
-	entry->state = tx_start;
 	entry->length = length;
 
 	usb_fill_bulk_urb (urb, dev->udev, dev->out,
@@ -1155,7 +1178,7 @@ netdev_tx_t usbnet_start_xmit (struct sk_buff *skb,
 		break;
 	case 0:
 		net->trans_start = jiffies;
-		__skb_queue_tail (&dev->txq, skb);
+		__usbnet_queue_skb(&dev->txq, skb, tx_start);
 		if (dev->txq.qlen >= TX_QLEN (dev))
 			netif_stop_queue (net);
 	}
diff --git a/drivers/net/usb/zaurus.c b/drivers/net/usb/zaurus.c
index 34db195fb8b..35c90307d47 100644
--- a/drivers/net/usb/zaurus.c
+++ b/drivers/net/usb/zaurus.c
@@ -377,6 +377,7 @@ static struct usb_driver zaurus_driver = {
 	.disconnect =	usbnet_disconnect,
 	.suspend =	usbnet_suspend,
 	.resume =	usbnet_resume,
+	.disable_hub_initiated_lpm = 1,
 };
 
 module_usb_driver(zaurus_driver);
diff --git a/drivers/net/virtio_net.c b/drivers/net/virtio_net.c
index af8acc85f4b..9ce6995e8d0 100644
--- a/drivers/net/virtio_net.c
+++ b/drivers/net/virtio_net.c
@@ -66,12 +66,21 @@ struct virtnet_info {
 	/* Host will merge rx buffers for big packets (shake it! shake it!) */
 	bool mergeable_rx_bufs;
 
+	/* enable config space updates */
+	bool config_enable;
+
 	/* Active statistics */
 	struct virtnet_stats __percpu *stats;
 
 	/* Work struct for refilling if we run low on memory. */
 	struct delayed_work refill;
 
+	/* Work struct for config space updates */
+	struct work_struct config_work;
+
+	/* Lock for config space updates */
+	struct mutex config_lock;
+
 	/* Chain pages by the private ptr. */
 	struct page *pages;
 
@@ -492,7 +501,9 @@ static void virtnet_napi_enable(struct virtnet_info *vi)
 	 * We synchronize against interrupts via NAPI_STATE_SCHED */
 	if (napi_schedule_prep(&vi->napi)) {
 		virtqueue_disable_cb(vi->rvq);
+		local_bh_disable();
 		__napi_schedule(&vi->napi);
+		local_bh_enable();
 	}
 }
 
@@ -780,6 +791,16 @@ static bool virtnet_send_command(struct virtnet_info *vi, u8 class, u8 cmd,
 	return status == VIRTIO_NET_OK;
 }
 
+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))
+		dev_warn(&vi->dev->dev, "Failed to ack link announce.\n");
+	rtnl_unlock();
+}
+
 static int virtnet_close(struct net_device *dev)
 {
 	struct virtnet_info *vi = netdev_priv(dev);
@@ -951,20 +972,31 @@ static const struct net_device_ops virtnet_netdev = {
 #endif
 };
 
-static void virtnet_update_status(struct virtnet_info *vi)
+static void virtnet_config_changed_work(struct work_struct *work)
 {
+	struct virtnet_info *vi =
+		container_of(work, struct virtnet_info, config_work);
 	u16 v;
 
+	mutex_lock(&vi->config_lock);
+	if (!vi->config_enable)
+		goto done;
+
 	if (virtio_config_val(vi->vdev, VIRTIO_NET_F_STATUS,
 			      offsetof(struct virtio_net_config, status),
 			      &v) < 0)
-		return;
+		goto done;
+
+	if (v & VIRTIO_NET_S_ANNOUNCE) {
+		netif_notify_peers(vi->dev);
+		virtnet_ack_link_announce(vi);
+	}
 
 	/* Ignore unknown (future) status bits */
 	v &= VIRTIO_NET_S_LINK_UP;
 
 	if (vi->status == v)
-		return;
+		goto done;
 
 	vi->status = v;
 
@@ -975,13 +1007,15 @@ static void virtnet_update_status(struct virtnet_info *vi)
 		netif_carrier_off(vi->dev);
 		netif_stop_queue(vi->dev);
 	}
+done:
+	mutex_unlock(&vi->config_lock);
 }
 
 static void virtnet_config_changed(struct virtio_device *vdev)
 {
 	struct virtnet_info *vi = vdev->priv;
 
-	virtnet_update_status(vi);
+	queue_work(system_nrt_wq, &vi->config_work);
 }
 
 static int init_vqs(struct virtnet_info *vi)
@@ -1075,6 +1109,9 @@ static int virtnet_probe(struct virtio_device *vdev)
 		goto free;
 
 	INIT_DELAYED_WORK(&vi->refill, refill_work);
+	mutex_init(&vi->config_lock);
+	vi->config_enable = true;
+	INIT_WORK(&vi->config_work, virtnet_config_changed_work);
 	sg_init_table(vi->rx_sg, ARRAY_SIZE(vi->rx_sg));
 	sg_init_table(vi->tx_sg, ARRAY_SIZE(vi->tx_sg));
 
@@ -1110,7 +1147,7 @@ static int virtnet_probe(struct virtio_device *vdev)
 	   otherwise get link status from config. */
 	if (virtio_has_feature(vi->vdev, VIRTIO_NET_F_STATUS)) {
 		netif_carrier_off(dev);
-		virtnet_update_status(vi);
+		queue_work(system_nrt_wq, &vi->config_work);
 	} else {
 		vi->status = VIRTIO_NET_S_LINK_UP;
 		netif_carrier_on(dev);
@@ -1169,10 +1206,17 @@ static void __devexit virtnet_remove(struct virtio_device *vdev)
 {
 	struct virtnet_info *vi = vdev->priv;
 
+	/* Prevent config work handler from accessing the device. */
+	mutex_lock(&vi->config_lock);
+	vi->config_enable = false;
+	mutex_unlock(&vi->config_lock);
+
 	unregister_netdev(vi->dev);
 
 	remove_vq_common(vi);
 
+	flush_work(&vi->config_work);
+
 	free_percpu(vi->stats);
 	free_netdev(vi->dev);
 }
@@ -1182,6 +1226,11 @@ static int virtnet_freeze(struct virtio_device *vdev)
 {
 	struct virtnet_info *vi = vdev->priv;
 
+	/* Prevent config work handler from accessing the device */
+	mutex_lock(&vi->config_lock);
+	vi->config_enable = false;
+	mutex_unlock(&vi->config_lock);
+
 	virtqueue_disable_cb(vi->rvq);
 	virtqueue_disable_cb(vi->svq);
 	if (virtio_has_feature(vi->vdev, VIRTIO_NET_F_CTRL_VQ))
@@ -1195,6 +1244,8 @@ static int virtnet_freeze(struct virtio_device *vdev)
 
 	remove_vq_common(vi);
 
+	flush_work(&vi->config_work);
+
 	return 0;
 }
 
@@ -1215,6 +1266,10 @@ static int virtnet_restore(struct virtio_device *vdev)
 	if (!try_fill_recv(vi, GFP_KERNEL))
 		queue_delayed_work(system_nrt_wq, &vi->refill, 0);
 
+	mutex_lock(&vi->config_lock);
+	vi->config_enable = true;
+	mutex_unlock(&vi->config_lock);
+
 	return 0;
 }
 #endif
@@ -1232,6 +1287,7 @@ static unsigned int features[] = {
 	VIRTIO_NET_F_GUEST_ECN, VIRTIO_NET_F_GUEST_UFO,
 	VIRTIO_NET_F_MRG_RXBUF, VIRTIO_NET_F_STATUS, VIRTIO_NET_F_CTRL_VQ,
 	VIRTIO_NET_F_CTRL_RX, VIRTIO_NET_F_CTRL_VLAN,
+	VIRTIO_NET_F_GUEST_ANNOUNCE,
 };
 
 static struct virtio_driver virtio_net_driver = {
diff --git a/drivers/net/wan/Kconfig b/drivers/net/wan/Kconfig
index d70ede7a7f9..d58431e99f7 100644
--- a/drivers/net/wan/Kconfig
+++ b/drivers/net/wan/Kconfig
@@ -203,37 +203,6 @@ config WANXL_BUILD_FIRMWARE
 
 	  You should never need this option, say N.
 
-config PC300
-	tristate "Cyclades-PC300 support (RS-232/V.35, X.21, T1/E1 boards)"
-	depends on HDLC && PCI && BROKEN
-	---help---
-	  This driver is broken because of struct tty_driver change.
-
-	  Driver for the Cyclades-PC300 synchronous communication boards.
-
-	  These boards provide synchronous serial interfaces to your
-	  Linux box (interfaces currently available are RS-232/V.35, X.21 and
-	  T1/E1). If you wish to support Multilink PPP, please select the
-	  option later and read the file README.mlppp provided by PC300
-	  package.
-
-	  To compile this as a module, choose M here: the module
-	  will be called pc300.
-
-	  If unsure, say N.
-
-config PC300_MLPPP
-	bool "Cyclades-PC300 MLPPP support"
-	depends on PC300 && PPP_MULTILINK && PPP_SYNC_TTY && HDLC_PPP
-	help
-	  Multilink PPP over the PC300 synchronous communication boards.
-
-comment "Cyclades-PC300 MLPPP support is disabled."
-	depends on HDLC && PC300 && (PPP=n || !PPP_MULTILINK || PPP_SYNC_TTY=n || !HDLC_PPP)
-
-comment "Refer to the file README.mlppp, provided by PC300 package."
-	depends on HDLC && PC300 && (PPP=n || !PPP_MULTILINK || PPP_SYNC_TTY=n || !HDLC_PPP)
-
 config PC300TOO
 	tristate "Cyclades PC300 RSV/X21 alternative support"
 	depends on HDLC && PCI
diff --git a/drivers/net/wan/Makefile b/drivers/net/wan/Makefile
index 19d14bc2835..eac709bed7a 100644
--- a/drivers/net/wan/Makefile
+++ b/drivers/net/wan/Makefile
@@ -17,10 +17,6 @@ obj-$(CONFIG_HDLC_FR)		+= hdlc_fr.o
 obj-$(CONFIG_HDLC_PPP)		+= hdlc_ppp.o
 obj-$(CONFIG_HDLC_X25)		+= hdlc_x25.o
 
-pc300-y				:= pc300_drv.o
-pc300-$(CONFIG_PC300_MLPPP)	+= pc300_tty.o
-pc300-objs			:= $(pc300-y)
-
 obj-$(CONFIG_HOSTESS_SV11)	+= z85230.o	hostess_sv11.o
 obj-$(CONFIG_SEALEVEL_4021)	+= z85230.o	sealevel.o
 obj-$(CONFIG_COSA)		+= cosa.o
@@ -35,7 +31,6 @@ obj-$(CONFIG_SDLA)		+= sdla.o
 obj-$(CONFIG_CYCLADES_SYNC)	+= cycx_drv.o cyclomx.o
 obj-$(CONFIG_LAPBETHER)		+= lapbether.o
 obj-$(CONFIG_SBNI)		+= sbni.o
-obj-$(CONFIG_PC300)		+= pc300.o
 obj-$(CONFIG_N2)		+= n2.o
 obj-$(CONFIG_C101)		+= c101.o
 obj-$(CONFIG_WANXL)		+= wanxl.o
diff --git a/drivers/net/wan/dscc4.c b/drivers/net/wan/dscc4.c
index c676de7de02..9eb6479306d 100644
--- a/drivers/net/wan/dscc4.c
+++ b/drivers/net/wan/dscc4.c
@@ -2055,15 +2055,4 @@ static struct pci_driver dscc4_driver = {
 	.remove		= __devexit_p(dscc4_remove_one),
 };
 
-static int __init dscc4_init_module(void)
-{
-	return pci_register_driver(&dscc4_driver);
-}
-
-static void __exit dscc4_cleanup_module(void)
-{
-	pci_unregister_driver(&dscc4_driver);
-}
-
-module_init(dscc4_init_module);
-module_exit(dscc4_cleanup_module);
+module_pci_driver(dscc4_driver);
diff --git a/drivers/net/wan/lmc/lmc_main.c b/drivers/net/wan/lmc/lmc_main.c
index 76a8a4a522e..f5d533a706e 100644
--- a/drivers/net/wan/lmc/lmc_main.c
+++ b/drivers/net/wan/lmc/lmc_main.c
@@ -1120,7 +1120,7 @@ static void lmc_running_reset (struct net_device *dev) /*fold00*/
 {
     lmc_softc_t *sc = dev_to_sc(dev);
 
-    lmc_trace(dev, "lmc_runnig_reset in");
+    lmc_trace(dev, "lmc_running_reset in");
 
     /* stop interrupts */
     /* Clear the interrupt mask */
@@ -1736,18 +1736,7 @@ static struct pci_driver lmc_driver = {
 	.remove		= __devexit_p(lmc_remove_one),
 };
 
-static int __init init_lmc(void)
-{
-    return pci_register_driver(&lmc_driver);
-}
-
-static void __exit exit_lmc(void)
-{
-    pci_unregister_driver(&lmc_driver);
-}
-
-module_init(init_lmc);
-module_exit(exit_lmc);
+module_pci_driver(lmc_driver);
 
 unsigned lmc_mii_readreg (lmc_softc_t * const sc, unsigned devaddr, unsigned regno) /*fold00*/
 {
diff --git a/drivers/net/wimax/i2400m/Kconfig b/drivers/net/wimax/i2400m/Kconfig
index 3f703384295..672de18a776 100644
--- a/drivers/net/wimax/i2400m/Kconfig
+++ b/drivers/net/wimax/i2400m/Kconfig
@@ -32,8 +32,9 @@ config WIMAX_I2400M_SDIO
 	  If unsure, it is safe to select M (module).
 
 config WIMAX_IWMC3200_SDIO
-	bool "Intel Wireless Multicom WiMAX Connection 3200 over SDIO"
+	bool "Intel Wireless Multicom WiMAX Connection 3200 over SDIO (EXPERIMENTAL)"
 	depends on WIMAX_I2400M_SDIO
+	depends on EXPERIMENTAL
 	select IWMC3200TOP
 	help
 	  Select if you have a device based on the Intel Multicom WiMAX
diff --git a/drivers/net/wimax/i2400m/usb-rx.c b/drivers/net/wimax/i2400m/usb-rx.c
index e3257681e36..b78ee676e10 100644
--- a/drivers/net/wimax/i2400m/usb-rx.c
+++ b/drivers/net/wimax/i2400m/usb-rx.c
@@ -277,7 +277,7 @@ retry:
 		d_printf(1, dev, "RX: size changed to %d, received %d, "
 			 "copied %d, capacity %ld\n",
 			 rx_size, read_size, rx_skb->len,
-			 (long) (skb_end_pointer(new_skb) - new_skb->head));
+			 (long) skb_end_offset(new_skb));
 		goto retry;
 	}
 		/* In most cases, it happens due to the hardware scheduling a
diff --git a/drivers/net/wimax/i2400m/usb.c b/drivers/net/wimax/i2400m/usb.c
index 29b1e033a10..713d033891e 100644
--- a/drivers/net/wimax/i2400m/usb.c
+++ b/drivers/net/wimax/i2400m/usb.c
@@ -695,7 +695,7 @@ int i2400mu_resume(struct usb_interface *iface)
 	d_fnstart(3, dev, "(iface %p)\n", iface);
 	rmb();		/* see i2400m->updown's documentation  */
 	if (i2400m->updown == 0) {
-		d_printf(1, dev, "fw was down, no resume neeed\n");
+		d_printf(1, dev, "fw was down, no resume needed\n");
 		goto out;
 	}
 	d_printf(1, dev, "fw was up, resuming\n");
diff --git a/drivers/net/wireless/Kconfig b/drivers/net/wireless/Kconfig
index abd3b71cd4a..5f58fa53238 100644
--- a/drivers/net/wireless/Kconfig
+++ b/drivers/net/wireless/Kconfig
@@ -282,8 +282,7 @@ source "drivers/net/wireless/orinoco/Kconfig"
 source "drivers/net/wireless/p54/Kconfig"
 source "drivers/net/wireless/rt2x00/Kconfig"
 source "drivers/net/wireless/rtlwifi/Kconfig"
-source "drivers/net/wireless/wl1251/Kconfig"
-source "drivers/net/wireless/wl12xx/Kconfig"
+source "drivers/net/wireless/ti/Kconfig"
 source "drivers/net/wireless/zd1211rw/Kconfig"
 source "drivers/net/wireless/mwifiex/Kconfig"
 
diff --git a/drivers/net/wireless/Makefile b/drivers/net/wireless/Makefile
index 98db76196b5..0ce218b931d 100644
--- a/drivers/net/wireless/Makefile
+++ b/drivers/net/wireless/Makefile
@@ -51,9 +51,7 @@ obj-$(CONFIG_ATH_COMMON)	+= ath/
 
 obj-$(CONFIG_MAC80211_HWSIM)	+= mac80211_hwsim.o
 
-obj-$(CONFIG_WL1251)	+= wl1251/
-obj-$(CONFIG_WL12XX)	+= wl12xx/
-obj-$(CONFIG_WL12XX_PLATFORM_DATA)	+= wl12xx/
+obj-$(CONFIG_WL_TI)	+= ti/
 
 obj-$(CONFIG_IWM)	+= iwmc3200wifi/
 
diff --git a/drivers/net/wireless/adm8211.c b/drivers/net/wireless/adm8211.c
index f5ce5623da9..0ac09a2bd14 100644
--- a/drivers/net/wireless/adm8211.c
+++ b/drivers/net/wireless/adm8211.c
@@ -1991,19 +1991,4 @@ static struct pci_driver adm8211_driver = {
 #endif /* CONFIG_PM */
 };
 
-
-
-static int __init adm8211_init(void)
-{
-	return pci_register_driver(&adm8211_driver);
-}
-
-
-static void __exit adm8211_exit(void)
-{
-	pci_unregister_driver(&adm8211_driver);
-}
-
-
-module_init(adm8211_init);
-module_exit(adm8211_exit);
+module_pci_driver(adm8211_driver);
diff --git a/drivers/net/wireless/at76c50x-usb.c b/drivers/net/wireless/at76c50x-usb.c
index 4045e5ab055..efc162e0b51 100644
--- a/drivers/net/wireless/at76c50x-usb.c
+++ b/drivers/net/wireless/at76c50x-usb.c
@@ -1122,12 +1122,12 @@ exit:
 static void at76_dump_mib_local(struct at76_priv *priv)
 {
 	int ret;
-	struct mib_local *m = kmalloc(sizeof(struct mib_phy), GFP_KERNEL);
+	struct mib_local *m = kmalloc(sizeof(*m), GFP_KERNEL);
 
 	if (!m)
 		return;
 
-	ret = at76_get_mib(priv->udev, MIB_LOCAL, m, sizeof(struct mib_local));
+	ret = at76_get_mib(priv->udev, MIB_LOCAL, m, sizeof(*m));
 	if (ret < 0) {
 		wiphy_err(priv->hw->wiphy,
 			  "at76_get_mib (LOCAL) failed: %d\n", ret);
@@ -1751,7 +1751,7 @@ static void at76_mac80211_tx(struct ieee80211_hw *hw, struct sk_buff *skb)
 	 * following workaround is necessary. If the TX frame is an
 	 * authentication frame extract the bssid and send the CMD_JOIN. */
 	if (mgmt->frame_control & cpu_to_le16(IEEE80211_STYPE_AUTH)) {
-		if (compare_ether_addr(priv->bssid, mgmt->bssid)) {
+		if (!ether_addr_equal(priv->bssid, mgmt->bssid)) {
 			memcpy(priv->bssid, mgmt->bssid, ETH_ALEN);
 			ieee80211_queue_work(hw, &priv->work_join_bssid);
 			dev_kfree_skb_any(skb);
@@ -1955,7 +1955,7 @@ static int at76_hw_scan(struct ieee80211_hw *hw,
 	ret = at76_set_card_command(priv->udev, CMD_SCAN, &scan, sizeof(scan));
 
 	if (ret < 0) {
-		err("CMD_SCAN failed: %d", ret);
+		wiphy_err(priv->hw->wiphy, "CMD_SCAN failed: %d\n", ret);
 		goto exit;
 	}
 
@@ -2486,6 +2486,7 @@ static struct usb_driver at76_driver = {
 	.probe = at76_probe,
 	.disconnect = at76_disconnect,
 	.id_table = dev_table,
+	.disable_hub_initiated_lpm = 1,
 };
 
 static int __init at76_mod_init(void)
@@ -2512,10 +2513,8 @@ static void __exit at76_mod_exit(void)
 
 	printk(KERN_INFO DRIVER_DESC " " DRIVER_VERSION " unloading\n");
 	usb_deregister(&at76_driver);
-	for (i = 0; i < ARRAY_SIZE(firmwares); i++) {
-		if (firmwares[i].fw)
-			release_firmware(firmwares[i].fw);
-	}
+	for (i = 0; i < ARRAY_SIZE(firmwares); i++)
+		release_firmware(firmwares[i].fw);
 	led_trigger_unregister_simple(ledtrig_tx);
 }
 
diff --git a/drivers/net/wireless/ath/ath5k/ani.c b/drivers/net/wireless/ath/ath5k/ani.c
index 35e93704c4e..5c008757662 100644
--- a/drivers/net/wireless/ath/ath5k/ani.c
+++ b/drivers/net/wireless/ath/ath5k/ani.c
@@ -14,6 +14,8 @@
  * OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
  */
 
+#define pr_fmt(fmt) KBUILD_MODNAME ": " fmt
+
 #include "ath5k.h"
 #include "reg.h"
 #include "debug.h"
@@ -728,33 +730,25 @@ void
 ath5k_ani_print_counters(struct ath5k_hw *ah)
 {
 	/* clears too */
-	printk(KERN_NOTICE "ACK fail\t%d\n",
-		ath5k_hw_reg_read(ah, AR5K_ACK_FAIL));
-	printk(KERN_NOTICE "RTS fail\t%d\n",
-		ath5k_hw_reg_read(ah, AR5K_RTS_FAIL));
-	printk(KERN_NOTICE "RTS success\t%d\n",
-		ath5k_hw_reg_read(ah, AR5K_RTS_OK));
-	printk(KERN_NOTICE "FCS error\t%d\n",
-		ath5k_hw_reg_read(ah, AR5K_FCS_FAIL));
+	pr_notice("ACK fail\t%d\n", ath5k_hw_reg_read(ah, AR5K_ACK_FAIL));
+	pr_notice("RTS fail\t%d\n", ath5k_hw_reg_read(ah, AR5K_RTS_FAIL));
+	pr_notice("RTS success\t%d\n", ath5k_hw_reg_read(ah, AR5K_RTS_OK));
+	pr_notice("FCS error\t%d\n", ath5k_hw_reg_read(ah, AR5K_FCS_FAIL));
 
 	/* no clear */
-	printk(KERN_NOTICE "tx\t%d\n",
-		ath5k_hw_reg_read(ah, AR5K_PROFCNT_TX));
-	printk(KERN_NOTICE "rx\t%d\n",
-		ath5k_hw_reg_read(ah, AR5K_PROFCNT_RX));
-	printk(KERN_NOTICE "busy\t%d\n",
-		ath5k_hw_reg_read(ah, AR5K_PROFCNT_RXCLR));
-	printk(KERN_NOTICE "cycles\t%d\n",
-		ath5k_hw_reg_read(ah, AR5K_PROFCNT_CYCLE));
-
-	printk(KERN_NOTICE "AR5K_PHYERR_CNT1\t%d\n",
-		ath5k_hw_reg_read(ah, AR5K_PHYERR_CNT1));
-	printk(KERN_NOTICE "AR5K_PHYERR_CNT2\t%d\n",
-		ath5k_hw_reg_read(ah, AR5K_PHYERR_CNT2));
-	printk(KERN_NOTICE "AR5K_OFDM_FIL_CNT\t%d\n",
-		ath5k_hw_reg_read(ah, AR5K_OFDM_FIL_CNT));
-	printk(KERN_NOTICE "AR5K_CCK_FIL_CNT\t%d\n",
-		ath5k_hw_reg_read(ah, AR5K_CCK_FIL_CNT));
+	pr_notice("tx\t%d\n", ath5k_hw_reg_read(ah, AR5K_PROFCNT_TX));
+	pr_notice("rx\t%d\n", ath5k_hw_reg_read(ah, AR5K_PROFCNT_RX));
+	pr_notice("busy\t%d\n", ath5k_hw_reg_read(ah, AR5K_PROFCNT_RXCLR));
+	pr_notice("cycles\t%d\n", ath5k_hw_reg_read(ah, AR5K_PROFCNT_CYCLE));
+
+	pr_notice("AR5K_PHYERR_CNT1\t%d\n",
+		  ath5k_hw_reg_read(ah, AR5K_PHYERR_CNT1));
+	pr_notice("AR5K_PHYERR_CNT2\t%d\n",
+		  ath5k_hw_reg_read(ah, AR5K_PHYERR_CNT2));
+	pr_notice("AR5K_OFDM_FIL_CNT\t%d\n",
+		  ath5k_hw_reg_read(ah, AR5K_OFDM_FIL_CNT));
+	pr_notice("AR5K_CCK_FIL_CNT\t%d\n",
+		  ath5k_hw_reg_read(ah, AR5K_CCK_FIL_CNT));
 }
 
 #endif
diff --git a/drivers/net/wireless/ath/ath5k/ath5k.h b/drivers/net/wireless/ath/ath5k/ath5k.h
index 8d434b8f585..64a453a6dfe 100644
--- a/drivers/net/wireless/ath/ath5k/ath5k.h
+++ b/drivers/net/wireless/ath/ath5k/ath5k.h
@@ -76,26 +76,29 @@
   GENERIC DRIVER DEFINITIONS
 \****************************/
 
-#define ATH5K_PRINTF(fmt, ...) \
-	printk(KERN_WARNING "%s: " fmt, __func__, ##__VA_ARGS__)
+#define ATH5K_PRINTF(fmt, ...)						\
+	pr_warn("%s: " fmt, __func__, ##__VA_ARGS__)
 
-#define ATH5K_PRINTK(_sc, _level, _fmt, ...) \
-	printk(_level "ath5k %s: " _fmt, \
-		((_sc) && (_sc)->hw) ? wiphy_name((_sc)->hw->wiphy) : "", \
-		##__VA_ARGS__)
+void __printf(3, 4)
+_ath5k_printk(const struct ath5k_hw *ah, const char *level,
+	      const char *fmt, ...);
 
-#define ATH5K_PRINTK_LIMIT(_sc, _level, _fmt, ...) do { \
-	if (net_ratelimit()) \
-		ATH5K_PRINTK(_sc, _level, _fmt, ##__VA_ARGS__); \
-	} while (0)
+#define ATH5K_PRINTK(_sc, _level, _fmt, ...)				\
+	_ath5k_printk(_sc, _level, _fmt, ##__VA_ARGS__)
 
-#define ATH5K_INFO(_sc, _fmt, ...) \
+#define ATH5K_PRINTK_LIMIT(_sc, _level, _fmt, ...)			\
+do {									\
+	if (net_ratelimit())						\
+		ATH5K_PRINTK(_sc, _level, _fmt, ##__VA_ARGS__); 	\
+} while (0)
+
+#define ATH5K_INFO(_sc, _fmt, ...)					\
 	ATH5K_PRINTK(_sc, KERN_INFO, _fmt, ##__VA_ARGS__)
 
-#define ATH5K_WARN(_sc, _fmt, ...) \
+#define ATH5K_WARN(_sc, _fmt, ...)					\
 	ATH5K_PRINTK_LIMIT(_sc, KERN_WARNING, _fmt, ##__VA_ARGS__)
 
-#define ATH5K_ERR(_sc, _fmt, ...) \
+#define ATH5K_ERR(_sc, _fmt, ...)					\
 	ATH5K_PRINTK_LIMIT(_sc, KERN_ERR, _fmt, ##__VA_ARGS__)
 
 /*
@@ -1524,7 +1527,7 @@ void ath5k_eeprom_detach(struct ath5k_hw *ah);
 
 /* Protocol Control Unit Functions */
 /* Helpers */
-int ath5k_hw_get_frame_duration(struct ath5k_hw *ah,
+int ath5k_hw_get_frame_duration(struct ath5k_hw *ah, enum ieee80211_band band,
 		int len, struct ieee80211_rate *rate, bool shortpre);
 unsigned int ath5k_hw_get_default_slottime(struct ath5k_hw *ah);
 unsigned int ath5k_hw_get_default_sifs(struct ath5k_hw *ah);
diff --git a/drivers/net/wireless/ath/ath5k/attach.c b/drivers/net/wireless/ath/ath5k/attach.c
index d7114c75fe9..7106547a14d 100644
--- a/drivers/net/wireless/ath/ath5k/attach.c
+++ b/drivers/net/wireless/ath/ath5k/attach.c
@@ -20,6 +20,8 @@
 * Attach/Detach Functions and helpers *
 \*************************************/
 
+#define pr_fmt(fmt) KBUILD_MODNAME ": " fmt
+
 #include <linux/pci.h>
 #include <linux/slab.h>
 #include "ath5k.h"
diff --git a/drivers/net/wireless/ath/ath5k/base.c b/drivers/net/wireless/ath/ath5k/base.c
index 0e643b016b3..0ba81a66061 100644
--- a/drivers/net/wireless/ath/ath5k/base.c
+++ b/drivers/net/wireless/ath/ath5k/base.c
@@ -40,6 +40,8 @@
  *
  */
 
+#define pr_fmt(fmt) KBUILD_MODNAME ": " fmt
+
 #include <linux/module.h>
 #include <linux/delay.h>
 #include <linux/dma-mapping.h>
@@ -460,7 +462,7 @@ void ath5k_vif_iter(void *data, u8 *mac, struct ieee80211_vif *vif)
 	}
 
 	if (iter_data->need_set_hw_addr && iter_data->hw_macaddr)
-		if (compare_ether_addr(iter_data->hw_macaddr, mac) == 0)
+		if (ether_addr_equal(iter_data->hw_macaddr, mac))
 			iter_data->need_set_hw_addr = false;
 
 	if (!iter_data->any_assoc) {
@@ -1168,7 +1170,7 @@ ath5k_check_ibss_tsf(struct ath5k_hw *ah, struct sk_buff *skb,
 
 	if (ieee80211_is_beacon(mgmt->frame_control) &&
 	    le16_to_cpu(mgmt->u.beacon.capab_info) & WLAN_CAPABILITY_IBSS &&
-	    memcmp(mgmt->bssid, common->curbssid, ETH_ALEN) == 0) {
+	    ether_addr_equal(mgmt->bssid, common->curbssid)) {
 		/*
 		 * Received an IBSS beacon with the same BSSID. Hardware *must*
 		 * have updated the local TSF. We have to work around various
@@ -1232,7 +1234,7 @@ ath5k_update_beacon_rssi(struct ath5k_hw *ah, struct sk_buff *skb, int rssi)
 
 	/* only beacons from our BSSID */
 	if (!ieee80211_is_beacon(mgmt->frame_control) ||
-	    memcmp(mgmt->bssid, common->curbssid, ETH_ALEN) != 0)
+	    !ether_addr_equal(mgmt->bssid, common->curbssid))
 		return;
 
 	ewma_add(&ah->ah_beacon_rssi_avg, rssi);
@@ -3038,3 +3040,23 @@ ath5k_set_beacon_filter(struct ieee80211_hw *hw, bool enable)
 	ath5k_hw_set_rx_filter(ah, rfilt);
 	ah->filter_flags = rfilt;
 }
+
+void _ath5k_printk(const struct ath5k_hw *ah, const char *level,
+		   const char *fmt, ...)
+{
+	struct va_format vaf;
+	va_list args;
+
+	va_start(args, fmt);
+
+	vaf.fmt = fmt;
+	vaf.va = &args;
+
+	if (ah && ah->hw)
+		printk("%s" pr_fmt("%s: %pV"),
+		       level, wiphy_name(ah->hw->wiphy), &vaf);
+	else
+		printk("%s" pr_fmt("%pV"), level, &vaf);
+
+	va_end(args);
+}
diff --git a/drivers/net/wireless/ath/ath5k/debug.c b/drivers/net/wireless/ath/ath5k/debug.c
index e5e8f45d86a..9d00dab666a 100644
--- a/drivers/net/wireless/ath/ath5k/debug.c
+++ b/drivers/net/wireless/ath/ath5k/debug.c
@@ -57,6 +57,9 @@
  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF
  * THE POSSIBILITY OF SUCH DAMAGES.
  */
+
+#define pr_fmt(fmt) KBUILD_MODNAME ": " fmt
+
 #include <linux/export.h>
 #include <linux/moduleparam.h>
 
@@ -247,10 +250,10 @@ static ssize_t write_file_beacon(struct file *file,
 
 	if (strncmp(buf, "disable", 7) == 0) {
 		AR5K_REG_DISABLE_BITS(ah, AR5K_BEACON, AR5K_BEACON_ENABLE);
-		printk(KERN_INFO "debugfs disable beacons\n");
+		pr_info("debugfs disable beacons\n");
 	} else if (strncmp(buf, "enable", 6) == 0) {
 		AR5K_REG_ENABLE_BITS(ah, AR5K_BEACON, AR5K_BEACON_ENABLE);
-		printk(KERN_INFO "debugfs enable beacons\n");
+		pr_info("debugfs enable beacons\n");
 	}
 	return count;
 }
@@ -450,19 +453,19 @@ static ssize_t write_file_antenna(struct file *file,
 
 	if (strncmp(buf, "diversity", 9) == 0) {
 		ath5k_hw_set_antenna_mode(ah, AR5K_ANTMODE_DEFAULT);
-		printk(KERN_INFO "ath5k debug: enable diversity\n");
+		pr_info("debug: enable diversity\n");
 	} else if (strncmp(buf, "fixed-a", 7) == 0) {
 		ath5k_hw_set_antenna_mode(ah, AR5K_ANTMODE_FIXED_A);
-		printk(KERN_INFO "ath5k debugfs: fixed antenna A\n");
+		pr_info("debug: fixed antenna A\n");
 	} else if (strncmp(buf, "fixed-b", 7) == 0) {
 		ath5k_hw_set_antenna_mode(ah, AR5K_ANTMODE_FIXED_B);
-		printk(KERN_INFO "ath5k debug: fixed antenna B\n");
+		pr_info("debug: fixed antenna B\n");
 	} else if (strncmp(buf, "clear", 5) == 0) {
 		for (i = 0; i < ARRAY_SIZE(ah->stats.antenna_rx); i++) {
 			ah->stats.antenna_rx[i] = 0;
 			ah->stats.antenna_tx[i] = 0;
 		}
-		printk(KERN_INFO "ath5k debug: cleared antenna stats\n");
+		pr_info("debug: cleared antenna stats\n");
 	}
 	return count;
 }
@@ -632,7 +635,7 @@ static ssize_t write_file_frameerrors(struct file *file,
 		st->txerr_fifo = 0;
 		st->txerr_filt = 0;
 		st->tx_all_count = 0;
-		printk(KERN_INFO "ath5k debug: cleared frameerrors stats\n");
+		pr_info("debug: cleared frameerrors stats\n");
 	}
 	return count;
 }
diff --git a/drivers/net/wireless/ath/ath5k/desc.c b/drivers/net/wireless/ath/ath5k/desc.c
index f8bfa3ac2af..bd8d4392d68 100644
--- a/drivers/net/wireless/ath/ath5k/desc.c
+++ b/drivers/net/wireless/ath/ath5k/desc.c
@@ -21,6 +21,8 @@
  Hardware Descriptor Functions
 \******************************/
 
+#define pr_fmt(fmt) KBUILD_MODNAME ": " fmt
+
 #include "ath5k.h"
 #include "reg.h"
 #include "debug.h"
@@ -441,10 +443,8 @@ ath5k_hw_proc_2word_tx_status(struct ath5k_hw *ah,
 				struct ath5k_desc *desc,
 				struct ath5k_tx_status *ts)
 {
-	struct ath5k_hw_2w_tx_ctl *tx_ctl;
 	struct ath5k_hw_tx_status *tx_status;
 
-	tx_ctl = &desc->ud.ds_tx5210.tx_ctl;
 	tx_status = &desc->ud.ds_tx5210.tx_stat;
 
 	/* No frame has been send or error */
@@ -495,11 +495,9 @@ ath5k_hw_proc_4word_tx_status(struct ath5k_hw *ah,
 				struct ath5k_desc *desc,
 				struct ath5k_tx_status *ts)
 {
-	struct ath5k_hw_4w_tx_ctl *tx_ctl;
 	struct ath5k_hw_tx_status *tx_status;
 	u32 txstat0, txstat1;
 
-	tx_ctl = &desc->ud.ds_tx5212.tx_ctl;
 	tx_status = &desc->ud.ds_tx5212.tx_stat;
 
 	txstat1 = ACCESS_ONCE(tx_status->tx_status_1);
diff --git a/drivers/net/wireless/ath/ath5k/dma.c b/drivers/net/wireless/ath/ath5k/dma.c
index 5cc9aa81469..ce86f158423 100644
--- a/drivers/net/wireless/ath/ath5k/dma.c
+++ b/drivers/net/wireless/ath/ath5k/dma.c
@@ -29,6 +29,8 @@
  * status registers (ISR).
  */
 
+#define pr_fmt(fmt) KBUILD_MODNAME ": " fmt
+
 #include "ath5k.h"
 #include "reg.h"
 #include "debug.h"
diff --git a/drivers/net/wireless/ath/ath5k/eeprom.c b/drivers/net/wireless/ath/ath5k/eeprom.c
index cd708c15b77..4026c906cc7 100644
--- a/drivers/net/wireless/ath/ath5k/eeprom.c
+++ b/drivers/net/wireless/ath/ath5k/eeprom.c
@@ -21,6 +21,8 @@
 * EEPROM access functions and helpers *
 \*************************************/
 
+#define pr_fmt(fmt) KBUILD_MODNAME ": " fmt
+
 #include <linux/slab.h>
 
 #include "ath5k.h"
diff --git a/drivers/net/wireless/ath/ath5k/initvals.c b/drivers/net/wireless/ath/ath5k/initvals.c
index a1ea78e05b4..ee1c2fa8b59 100644
--- a/drivers/net/wireless/ath/ath5k/initvals.c
+++ b/drivers/net/wireless/ath/ath5k/initvals.c
@@ -19,6 +19,8 @@
  *
  */
 
+#define pr_fmt(fmt) KBUILD_MODNAME ": " fmt
+
 #include "ath5k.h"
 #include "reg.h"
 #include "debug.h"
@@ -1574,8 +1576,7 @@ ath5k_hw_write_initvals(struct ath5k_hw *ah, u8 mode, bool skip_pcu)
 
 		/* AR5K_MODE_11B */
 		if (mode > 2) {
-			ATH5K_ERR(ah,
-				"unsupported channel mode: %d\n", mode);
+			ATH5K_ERR(ah, "unsupported channel mode: %d\n", mode);
 			return -EINVAL;
 		}
 
diff --git a/drivers/net/wireless/ath/ath5k/led.c b/drivers/net/wireless/ath/ath5k/led.c
index c1151c72371..b9f708a45f4 100644
--- a/drivers/net/wireless/ath/ath5k/led.c
+++ b/drivers/net/wireless/ath/ath5k/led.c
@@ -39,6 +39,8 @@
  *
  */
 
+#define pr_fmt(fmt) KBUILD_MODNAME ": " fmt
+
 #include <linux/pci.h>
 #include "ath5k.h"
 
diff --git a/drivers/net/wireless/ath/ath5k/mac80211-ops.c b/drivers/net/wireless/ath/ath5k/mac80211-ops.c
index 5c532995541..22b80af0f47 100644
--- a/drivers/net/wireless/ath/ath5k/mac80211-ops.c
+++ b/drivers/net/wireless/ath/ath5k/mac80211-ops.c
@@ -41,6 +41,8 @@
  *
  */
 
+#define pr_fmt(fmt) KBUILD_MODNAME ": " fmt
+
 #include <net/mac80211.h>
 #include <asm/unaligned.h>
 
diff --git a/drivers/net/wireless/ath/ath5k/pci.c b/drivers/net/wireless/ath/ath5k/pci.c
index 849fa060ebc..dff48fbc63b 100644
--- a/drivers/net/wireless/ath/ath5k/pci.c
+++ b/drivers/net/wireless/ath/ath5k/pci.c
@@ -14,6 +14,8 @@
  * OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
  */
 
+#define pr_fmt(fmt) KBUILD_MODNAME ": " fmt
+
 #include <linux/nl80211.h>
 #include <linux/pci.h>
 #include <linux/pci-aspm.h>
@@ -45,6 +47,7 @@ static DEFINE_PCI_DEVICE_TABLE(ath5k_pci_id_table) = {
 	{ PCI_VDEVICE(ATHEROS, 0x001b) }, /* 5413 Eagle */
 	{ PCI_VDEVICE(ATHEROS, 0x001c) }, /* PCI-E cards */
 	{ PCI_VDEVICE(ATHEROS, 0x001d) }, /* 2417 Nala */
+	{ PCI_VDEVICE(ATHEROS, 0xff1b) }, /* AR5BXB63 */
 	{ 0 }
 };
 MODULE_DEVICE_TABLE(pci, ath5k_pci_id_table);
@@ -337,28 +340,4 @@ static struct pci_driver ath5k_pci_driver = {
 	.driver.pm	= ATH5K_PM_OPS,
 };
 
-/*
- * Module init/exit functions
- */
-static int __init
-init_ath5k_pci(void)
-{
-	int ret;
-
-	ret = pci_register_driver(&ath5k_pci_driver);
-	if (ret) {
-		printk(KERN_ERR "ath5k_pci: can't register pci driver\n");
-		return ret;
-	}
-
-	return 0;
-}
-
-static void __exit
-exit_ath5k_pci(void)
-{
-	pci_unregister_driver(&ath5k_pci_driver);
-}
-
-module_init(init_ath5k_pci);
-module_exit(exit_ath5k_pci);
+module_pci_driver(ath5k_pci_driver);
diff --git a/drivers/net/wireless/ath/ath5k/pcu.c b/drivers/net/wireless/ath/ath5k/pcu.c
index cebfd6fd31d..1f16b4227d8 100644
--- a/drivers/net/wireless/ath/ath5k/pcu.c
+++ b/drivers/net/wireless/ath/ath5k/pcu.c
@@ -110,7 +110,7 @@ static const unsigned int ack_rates_high[] =
  * bwmodes.
  */
 int
-ath5k_hw_get_frame_duration(struct ath5k_hw *ah,
+ath5k_hw_get_frame_duration(struct ath5k_hw *ah, enum ieee80211_band band,
 		int len, struct ieee80211_rate *rate, bool shortpre)
 {
 	int sifs, preamble, plcp_bits, sym_time;
@@ -120,7 +120,7 @@ ath5k_hw_get_frame_duration(struct ath5k_hw *ah,
 	/* Fallback */
 	if (!ah->ah_bwmode) {
 		__le16 raw_dur = ieee80211_generic_frame_duration(ah->hw,
-					NULL, len, rate);
+					NULL, band, len, rate);
 
 		/* subtract difference between long and short preamble */
 		dur = le16_to_cpu(raw_dur);
@@ -302,14 +302,15 @@ ath5k_hw_write_rate_duration(struct ath5k_hw *ah)
 		 * actual rate for this rate. See mac80211 tx.c
 		 * ieee80211_duration() for a brief description of
 		 * what rate we should choose to TX ACKs. */
-		tx_time = ath5k_hw_get_frame_duration(ah, 10, rate, false);
+		tx_time = ath5k_hw_get_frame_duration(ah, band, 10,
+					rate, false);
 
 		ath5k_hw_reg_write(ah, tx_time, reg);
 
 		if (!(rate->flags & IEEE80211_RATE_SHORT_PREAMBLE))
 			continue;
 
-		tx_time = ath5k_hw_get_frame_duration(ah, 10, rate, true);
+		tx_time = ath5k_hw_get_frame_duration(ah, band, 10, rate, true);
 		ath5k_hw_reg_write(ah, tx_time,
 			reg + (AR5K_SET_SHORT_PREAMBLE << 2));
 	}
diff --git a/drivers/net/wireless/ath/ath5k/phy.c b/drivers/net/wireless/ath/ath5k/phy.c
index 3a2845489a1..8b71a2d947e 100644
--- a/drivers/net/wireless/ath/ath5k/phy.c
+++ b/drivers/net/wireless/ath/ath5k/phy.c
@@ -22,6 +22,8 @@
 * PHY related functions *
 \***********************/
 
+#define pr_fmt(fmt) KBUILD_MODNAME ": " fmt
+
 #include <linux/delay.h>
 #include <linux/slab.h>
 #include <asm/unaligned.h>
diff --git a/drivers/net/wireless/ath/ath5k/qcu.c b/drivers/net/wireless/ath/ath5k/qcu.c
index 30b50f93417..65fe929529a 100644
--- a/drivers/net/wireless/ath/ath5k/qcu.c
+++ b/drivers/net/wireless/ath/ath5k/qcu.c
@@ -20,6 +20,8 @@
 Queue Control Unit, DCF Control Unit Functions
 \********************************************/
 
+#define pr_fmt(fmt) KBUILD_MODNAME ": " fmt
+
 #include "ath5k.h"
 #include "reg.h"
 #include "debug.h"
@@ -563,6 +565,7 @@ ath5k_hw_reset_tx_queue(struct ath5k_hw *ah, unsigned int queue)
 int ath5k_hw_set_ifs_intervals(struct ath5k_hw *ah, unsigned int slot_time)
 {
 	struct ieee80211_channel *channel = ah->ah_current_channel;
+	enum ieee80211_band band;
 	struct ieee80211_rate *rate;
 	u32 ack_tx_time, eifs, eifs_clock, sifs, sifs_clock;
 	u32 slot_time_clock = ath5k_hw_htoclock(ah, slot_time);
@@ -598,11 +601,12 @@ int ath5k_hw_set_ifs_intervals(struct ath5k_hw *ah, unsigned int slot_time)
 	 * Also we have different lowest rate for 802.11a
 	 */
 	if (channel->band == IEEE80211_BAND_5GHZ)
-		rate = &ah->sbands[IEEE80211_BAND_5GHZ].bitrates[0];
+		band = IEEE80211_BAND_5GHZ;
 	else
-		rate = &ah->sbands[IEEE80211_BAND_2GHZ].bitrates[0];
+		band = IEEE80211_BAND_2GHZ;
 
-	ack_tx_time = ath5k_hw_get_frame_duration(ah, 10, rate, false);
+	rate = &ah->sbands[band].bitrates[0];
+	ack_tx_time = ath5k_hw_get_frame_duration(ah, band, 10, rate, false);
 
 	/* ack_tx_time includes an SIFS already */
 	eifs = ack_tx_time + sifs + 2 * slot_time;
diff --git a/drivers/net/wireless/ath/ath5k/reset.c b/drivers/net/wireless/ath/ath5k/reset.c
index 200f165c0c6..0c2dd4771c3 100644
--- a/drivers/net/wireless/ath/ath5k/reset.c
+++ b/drivers/net/wireless/ath/ath5k/reset.c
@@ -23,6 +23,8 @@
   Reset function and helpers
 \****************************/
 
+#define pr_fmt(fmt) KBUILD_MODNAME ": " fmt
+
 #include <asm/unaligned.h>
 
 #include <linux/pci.h>		/* To determine if a card is pci-e */
diff --git a/drivers/net/wireless/ath/ath5k/sysfs.c b/drivers/net/wireless/ath/ath5k/sysfs.c
index 9364da7bd13..04cf0ca7261 100644
--- a/drivers/net/wireless/ath/ath5k/sysfs.c
+++ b/drivers/net/wireless/ath/ath5k/sysfs.c
@@ -1,3 +1,5 @@
+#define pr_fmt(fmt) KBUILD_MODNAME ": " fmt
+
 #include <linux/device.h>
 #include <linux/pci.h>
 
diff --git a/drivers/net/wireless/ath/ath6kl/Makefile b/drivers/net/wireless/ath/ath6kl/Makefile
index 85746c3eb02..8cae8886f17 100644
--- a/drivers/net/wireless/ath/ath6kl/Makefile
+++ b/drivers/net/wireless/ath/ath6kl/Makefile
@@ -25,7 +25,8 @@
 obj-$(CONFIG_ATH6KL) += ath6kl_core.o
 ath6kl_core-y += debug.o
 ath6kl_core-y += hif.o
-ath6kl_core-y += htc.o
+ath6kl_core-y += htc_mbox.o
+ath6kl_core-y += htc_pipe.o
 ath6kl_core-y += bmi.o
 ath6kl_core-y += cfg80211.o
 ath6kl_core-y += init.o
diff --git a/drivers/net/wireless/ath/ath6kl/cfg80211.c b/drivers/net/wireless/ath/ath6kl/cfg80211.c
index 00d38952b5f..b869a358ce4 100644
--- a/drivers/net/wireless/ath/ath6kl/cfg80211.c
+++ b/drivers/net/wireless/ath/ath6kl/cfg80211.c
@@ -15,6 +15,8 @@
  * OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
  */
 
+#define pr_fmt(fmt) KBUILD_MODNAME ": " fmt
+
 #include <linux/moduleparam.h>
 #include <linux/inetdevice.h>
 #include <linux/export.h>
@@ -49,6 +51,8 @@
 	.max_power      = 30,                       \
 }
 
+#define DEFAULT_BG_SCAN_PERIOD 60
+
 static struct ieee80211_rate ath6kl_rates[] = {
 	RATETAB_ENT(10, 0x1, 0),
 	RATETAB_ENT(20, 0x2, 0),
@@ -69,7 +73,8 @@ static struct ieee80211_rate ath6kl_rates[] = {
 #define ath6kl_g_rates     (ath6kl_rates + 0)
 #define ath6kl_g_rates_size    12
 
-#define ath6kl_g_htcap (IEEE80211_HT_CAP_SUP_WIDTH_20_40 | \
+#define ath6kl_g_htcap IEEE80211_HT_CAP_SGI_20
+#define ath6kl_a_htcap (IEEE80211_HT_CAP_SUP_WIDTH_20_40 | \
 			IEEE80211_HT_CAP_SGI_20		 | \
 			IEEE80211_HT_CAP_SGI_40)
 
@@ -126,7 +131,7 @@ static struct ieee80211_supported_band ath6kl_band_5ghz = {
 	.channels = ath6kl_5ghz_a_channels,
 	.n_bitrates = ath6kl_a_rates_size,
 	.bitrates = ath6kl_a_rates,
-	.ht_cap.cap = ath6kl_g_htcap,
+	.ht_cap.cap = ath6kl_a_htcap,
 	.ht_cap.ht_supported = true,
 };
 
@@ -607,6 +612,17 @@ static int ath6kl_cfg80211_connect(struct wiphy *wiphy, struct net_device *dev,
 					vif->req_bssid, vif->ch_hint,
 					ar->connect_ctrl_flags, nw_subtype);
 
+	/* disable background scan if period is 0 */
+	if (sme->bg_scan_period == 0)
+		sme->bg_scan_period = 0xffff;
+
+	/* configure default value if not specified */
+	if (sme->bg_scan_period == -1)
+		sme->bg_scan_period = DEFAULT_BG_SCAN_PERIOD;
+
+	ath6kl_wmi_scanparams_cmd(ar->wmi, vif->fw_vif_idx, 0, 0,
+				  sme->bg_scan_period, 0, 0, 0, 3, 0, 0, 0);
+
 	up(&ar->sem);
 
 	if (status == -EINVAL) {
@@ -677,8 +693,8 @@ ath6kl_add_bss_if_needed(struct ath6kl_vif *vif,
 					  ie, 2 + vif->ssid_len + beacon_ie_len,
 					  0, GFP_KERNEL);
 		if (bss)
-			ath6kl_dbg(ATH6KL_DBG_WLAN_CFG, "added bss %pM to "
-				   "cfg80211\n", bssid);
+			ath6kl_dbg(ATH6KL_DBG_WLAN_CFG,
+				   "added bss %pM to cfg80211\n", bssid);
 		kfree(ie);
 	} else
 		ath6kl_dbg(ATH6KL_DBG_WLAN_CFG, "cfg80211 already has a bss\n");
@@ -866,6 +882,32 @@ void ath6kl_cfg80211_disconnect_event(struct ath6kl_vif *vif, u8 reason,
 	vif->sme_state = SME_DISCONNECTED;
 }
 
+static int ath6kl_set_probed_ssids(struct ath6kl *ar,
+				   struct ath6kl_vif *vif,
+				   struct cfg80211_ssid *ssids, int n_ssids)
+{
+	u8 i;
+
+	if (n_ssids > MAX_PROBED_SSID_INDEX)
+		return -EINVAL;
+
+	for (i = 0; i < n_ssids; i++) {
+		ath6kl_wmi_probedssid_cmd(ar->wmi, vif->fw_vif_idx, i,
+					  ssids[i].ssid_len ?
+					  SPECIFIC_SSID_FLAG : ANY_SSID_FLAG,
+					  ssids[i].ssid_len,
+					  ssids[i].ssid);
+	}
+
+	/* Make sure no old entries are left behind */
+	for (i = n_ssids; i < MAX_PROBED_SSID_INDEX; i++) {
+		ath6kl_wmi_probedssid_cmd(ar->wmi, vif->fw_vif_idx, i,
+					  DISABLE_SSID_FLAG, 0, NULL);
+	}
+
+	return 0;
+}
+
 static int ath6kl_cfg80211_scan(struct wiphy *wiphy, struct net_device *ndev,
 				struct cfg80211_scan_request *request)
 {
@@ -883,36 +925,25 @@ static int ath6kl_cfg80211_scan(struct wiphy *wiphy, struct net_device *ndev,
 
 	if (!ar->usr_bss_filter) {
 		clear_bit(CLEAR_BSSFILTER_ON_BEACON, &vif->flags);
-		ret = ath6kl_wmi_bssfilter_cmd(
-			ar->wmi, vif->fw_vif_idx,
-			(test_bit(CONNECTED, &vif->flags) ?
-			 ALL_BUT_BSS_FILTER : ALL_BSS_FILTER), 0);
+		ret = ath6kl_wmi_bssfilter_cmd(ar->wmi, vif->fw_vif_idx,
+					       ALL_BSS_FILTER, 0);
 		if (ret) {
 			ath6kl_err("couldn't set bss filtering\n");
 			return ret;
 		}
 	}
 
-	if (request->n_ssids && request->ssids[0].ssid_len) {
-		u8 i;
-
-		if (request->n_ssids > (MAX_PROBED_SSID_INDEX - 1))
-			request->n_ssids = MAX_PROBED_SSID_INDEX - 1;
-
-		for (i = 0; i < request->n_ssids; i++)
-			ath6kl_wmi_probedssid_cmd(ar->wmi, vif->fw_vif_idx,
-						  i + 1, SPECIFIC_SSID_FLAG,
-						  request->ssids[i].ssid_len,
-						  request->ssids[i].ssid);
-	}
+	ret = ath6kl_set_probed_ssids(ar, vif, request->ssids,
+				      request->n_ssids);
+	if (ret < 0)
+		return ret;
 
 	/* this also clears IE in fw if it's not set */
 	ret = ath6kl_wmi_set_appie_cmd(ar->wmi, vif->fw_vif_idx,
 				       WMI_FRAME_PROBE_REQ,
 				       request->ie, request->ie_len);
 	if (ret) {
-		ath6kl_err("failed to set Probe Request appie for "
-			   "scan");
+		ath6kl_err("failed to set Probe Request appie for scan");
 		return ret;
 	}
 
@@ -929,8 +960,7 @@ static int ath6kl_cfg80211_scan(struct wiphy *wiphy, struct net_device *ndev,
 
 		channels = kzalloc(n_channels * sizeof(u16), GFP_KERNEL);
 		if (channels == NULL) {
-			ath6kl_warn("failed to set scan channels, "
-				    "scan all channels");
+			ath6kl_warn("failed to set scan channels, scan all channels");
 			n_channels = 0;
 		}
 
@@ -941,6 +971,8 @@ static int ath6kl_cfg80211_scan(struct wiphy *wiphy, struct net_device *ndev,
 	if (test_bit(CONNECTED, &vif->flags))
 		force_fg_scan = 1;
 
+	vif->scan_req = request;
+
 	if (test_bit(ATH6KL_FW_CAPABILITY_STA_P2PDEV_DUPLEX,
 		     ar->fw_capabilities)) {
 		/*
@@ -963,10 +995,10 @@ static int ath6kl_cfg80211_scan(struct wiphy *wiphy, struct net_device *ndev,
 						ATH6KL_FG_SCAN_INTERVAL,
 						n_channels, channels);
 	}
-	if (ret)
+	if (ret) {
 		ath6kl_err("wmi_startscan_cmd failed\n");
-	else
-		vif->scan_req = request;
+		vif->scan_req = NULL;
+	}
 
 	kfree(channels);
 
@@ -1000,6 +1032,20 @@ out:
 	vif->scan_req = NULL;
 }
 
+void ath6kl_cfg80211_ch_switch_notify(struct ath6kl_vif *vif, int freq,
+				      enum wmi_phy_mode mode)
+{
+	enum nl80211_channel_type type;
+
+	ath6kl_dbg(ATH6KL_DBG_WLAN_CFG,
+		   "channel switch notify nw_type %d freq %d mode %d\n",
+		   vif->nw_type, freq, mode);
+
+	type = (mode == WMI_11G_HT20) ? NL80211_CHAN_HT20 : NL80211_CHAN_NO_HT;
+
+	cfg80211_ch_switch_notify(vif->ndev, freq, type);
+}
+
 static int ath6kl_cfg80211_add_key(struct wiphy *wiphy, struct net_device *ndev,
 				   u8 key_index, bool pairwise,
 				   const u8 *mac_addr,
@@ -1093,9 +1139,8 @@ static int ath6kl_cfg80211_add_key(struct wiphy *wiphy, struct net_device *ndev,
 		ar->ap_mode_bkey.key_len = key->key_len;
 		memcpy(ar->ap_mode_bkey.key, key->key, key->key_len);
 		if (!test_bit(CONNECTED, &vif->flags)) {
-			ath6kl_dbg(ATH6KL_DBG_WLAN_CFG, "Delay initial group "
-				   "key configuration until AP mode has been "
-				   "started\n");
+			ath6kl_dbg(ATH6KL_DBG_WLAN_CFG,
+				   "Delay initial group key configuration until AP mode has been started\n");
 			/*
 			 * The key will be set in ath6kl_connect_ap_mode() once
 			 * the connected event is received from the target.
@@ -1111,8 +1156,8 @@ static int ath6kl_cfg80211_add_key(struct wiphy *wiphy, struct net_device *ndev,
 		 * the AP mode has properly started
 		 * (ath6kl_install_statioc_wep_keys).
 		 */
-		ath6kl_dbg(ATH6KL_DBG_WLAN_CFG, "Delay WEP key configuration "
-			   "until AP mode has been started\n");
+		ath6kl_dbg(ATH6KL_DBG_WLAN_CFG,
+			   "Delay WEP key configuration until AP mode has been started\n");
 		vif->wep_key_list[key_index].key_len = key->key_len;
 		memcpy(vif->wep_key_list[key_index].key, key->key,
 		       key->key_len);
@@ -1436,9 +1481,38 @@ static int ath6kl_cfg80211_change_iface(struct wiphy *wiphy,
 					struct vif_params *params)
 {
 	struct ath6kl_vif *vif = netdev_priv(ndev);
+	int i;
 
 	ath6kl_dbg(ATH6KL_DBG_WLAN_CFG, "%s: type %u\n", __func__, type);
 
+	/*
+	 * Don't bring up p2p on an interface which is not initialized
+	 * for p2p operation where fw does not have capability to switch
+	 * dynamically between non-p2p and p2p type interface.
+	 */
+	if (!test_bit(ATH6KL_FW_CAPABILITY_STA_P2PDEV_DUPLEX,
+		      vif->ar->fw_capabilities) &&
+	    (type == NL80211_IFTYPE_P2P_CLIENT ||
+	     type == NL80211_IFTYPE_P2P_GO)) {
+		if (vif->ar->vif_max == 1) {
+			if (vif->fw_vif_idx != 0)
+				return -EINVAL;
+			else
+				goto set_iface_type;
+		}
+
+		for (i = vif->ar->max_norm_iface; i < vif->ar->vif_max; i++) {
+			if (i == vif->fw_vif_idx)
+				break;
+		}
+
+		if (i == vif->ar->vif_max) {
+			ath6kl_err("Invalid interface to bring up P2P\n");
+			return -EINVAL;
+		}
+	}
+
+set_iface_type:
 	switch (type) {
 	case NL80211_IFTYPE_STATION:
 		vif->next_mode = INFRA_NETWORK;
@@ -1915,8 +1989,7 @@ static int ath6kl_wow_sta(struct ath6kl *ar, struct ath6kl_vif *vif)
 				sizeof(discvr_pattern), discvr_offset,
 				discvr_pattern, discvr_mask);
 		if (ret) {
-			ath6kl_err("failed to add WOW mDNS/SSDP/LLMNR "
-				   "pattern\n");
+			ath6kl_err("failed to add WOW mDNS/SSDP/LLMNR pattern\n");
 			return ret;
 		}
 	}
@@ -1924,17 +1997,70 @@ static int ath6kl_wow_sta(struct ath6kl *ar, struct ath6kl_vif *vif)
 	return 0;
 }
 
+static int is_hsleep_mode_procsed(struct ath6kl_vif *vif)
+{
+	return test_bit(HOST_SLEEP_MODE_CMD_PROCESSED, &vif->flags);
+}
+
+static bool is_ctrl_ep_empty(struct ath6kl *ar)
+{
+	return !ar->tx_pending[ar->ctrl_ep];
+}
+
+static int ath6kl_cfg80211_host_sleep(struct ath6kl *ar, struct ath6kl_vif *vif)
+{
+	int ret, left;
+
+	clear_bit(HOST_SLEEP_MODE_CMD_PROCESSED, &vif->flags);
+
+	ret = ath6kl_wmi_set_host_sleep_mode_cmd(ar->wmi, vif->fw_vif_idx,
+						 ATH6KL_HOST_MODE_ASLEEP);
+	if (ret)
+		return ret;
+
+	left = wait_event_interruptible_timeout(ar->event_wq,
+						is_hsleep_mode_procsed(vif),
+						WMI_TIMEOUT);
+	if (left == 0) {
+		ath6kl_warn("timeout, didn't get host sleep cmd processed event\n");
+		ret = -ETIMEDOUT;
+	} else if (left < 0) {
+		ath6kl_warn("error while waiting for host sleep cmd processed event %d\n",
+			    left);
+		ret = left;
+	}
+
+	if (ar->tx_pending[ar->ctrl_ep]) {
+		left = wait_event_interruptible_timeout(ar->event_wq,
+							is_ctrl_ep_empty(ar),
+							WMI_TIMEOUT);
+		if (left == 0) {
+			ath6kl_warn("clear wmi ctrl data timeout\n");
+			ret = -ETIMEDOUT;
+		} else if (left < 0) {
+			ath6kl_warn("clear wmi ctrl data failed: %d\n", left);
+			ret = left;
+		}
+	}
+
+	return ret;
+}
+
 static int ath6kl_wow_suspend(struct ath6kl *ar, struct cfg80211_wowlan *wow)
 {
 	struct in_device *in_dev;
 	struct in_ifaddr *ifa;
 	struct ath6kl_vif *vif;
-	int ret, left;
+	int ret;
 	u32 filter = 0;
 	u16 i, bmiss_time;
 	u8 index = 0;
 	__be32 ips[MAX_IP_ADDRS];
 
+	/* The FW currently can't support multi-vif WoW properly. */
+	if (ar->num_vif > 1)
+		return -EIO;
+
 	vif = ath6kl_vif_first(ar);
 	if (!vif)
 		return -EIO;
@@ -1948,6 +2074,13 @@ static int ath6kl_wow_suspend(struct ath6kl *ar, struct cfg80211_wowlan *wow)
 	if (wow && (wow->n_patterns > WOW_MAX_FILTERS_PER_LIST))
 		return -EINVAL;
 
+	if (!test_bit(NETDEV_MCAST_ALL_ON, &vif->flags)) {
+		ret = ath6kl_wmi_mcast_filter_cmd(vif->ar->wmi,
+						vif->fw_vif_idx, false);
+		if (ret)
+			return ret;
+	}
+
 	/* Clear existing WOW patterns */
 	for (i = 0; i < WOW_MAX_FILTERS_PER_LIST; i++)
 		ath6kl_wmi_del_wow_pattern_cmd(ar->wmi, vif->fw_vif_idx,
@@ -2030,39 +2163,11 @@ skip_arp:
 	if (ret)
 		return ret;
 
-	clear_bit(HOST_SLEEP_MODE_CMD_PROCESSED, &vif->flags);
-
-	ret = ath6kl_wmi_set_host_sleep_mode_cmd(ar->wmi, vif->fw_vif_idx,
-						 ATH6KL_HOST_MODE_ASLEEP);
+	ret = ath6kl_cfg80211_host_sleep(ar, vif);
 	if (ret)
 		return ret;
 
-	left = wait_event_interruptible_timeout(ar->event_wq,
-			test_bit(HOST_SLEEP_MODE_CMD_PROCESSED, &vif->flags),
-			WMI_TIMEOUT);
-	if (left == 0) {
-		ath6kl_warn("timeout, didn't get host sleep cmd "
-			    "processed event\n");
-		ret = -ETIMEDOUT;
-	} else if (left < 0) {
-		ath6kl_warn("error while waiting for host sleep cmd "
-			    "processed event %d\n", left);
-		ret = left;
-	}
-
-	if (ar->tx_pending[ar->ctrl_ep]) {
-		left = wait_event_interruptible_timeout(ar->event_wq,
-				ar->tx_pending[ar->ctrl_ep] == 0, WMI_TIMEOUT);
-		if (left == 0) {
-			ath6kl_warn("clear wmi ctrl data timeout\n");
-			ret = -ETIMEDOUT;
-		} else if (left < 0) {
-			ath6kl_warn("clear wmi ctrl data failed: %d\n", left);
-			ret = left;
-		}
-	}
-
-	return ret;
+	return 0;
 }
 
 static int ath6kl_wow_resume(struct ath6kl *ar)
@@ -2079,8 +2184,8 @@ static int ath6kl_wow_resume(struct ath6kl *ar)
 	ret = ath6kl_wmi_set_host_sleep_mode_cmd(ar->wmi, vif->fw_vif_idx,
 						 ATH6KL_HOST_MODE_AWAKE);
 	if (ret) {
-		ath6kl_warn("Failed to configure host sleep mode for "
-			    "wow resume: %d\n", ret);
+		ath6kl_warn("Failed to configure host sleep mode for wow resume: %d\n",
+			    ret);
 		ar->state = ATH6KL_STATE_WOW;
 		return ret;
 	}
@@ -2104,15 +2209,96 @@ static int ath6kl_wow_resume(struct ath6kl *ar)
 
 	ar->state = ATH6KL_STATE_ON;
 
+	if (!test_bit(NETDEV_MCAST_ALL_OFF, &vif->flags)) {
+		ret = ath6kl_wmi_mcast_filter_cmd(vif->ar->wmi,
+					vif->fw_vif_idx, true);
+		if (ret)
+			return ret;
+	}
+
 	netif_wake_queue(vif->ndev);
 
 	return 0;
 }
 
+static int ath6kl_cfg80211_deepsleep_suspend(struct ath6kl *ar)
+{
+	struct ath6kl_vif *vif;
+	int ret;
+
+	vif = ath6kl_vif_first(ar);
+	if (!vif)
+		return -EIO;
+
+	if (!test_bit(WMI_READY, &ar->flag)) {
+		ath6kl_err("deepsleep failed as wmi is not ready\n");
+		return -EIO;
+	}
+
+	ath6kl_cfg80211_stop_all(ar);
+
+	/* Save the current power mode before enabling power save */
+	ar->wmi->saved_pwr_mode = ar->wmi->pwr_mode;
+
+	ret = ath6kl_wmi_powermode_cmd(ar->wmi, 0, REC_POWER);
+	if (ret)
+		return ret;
+
+	/* Disable WOW mode */
+	ret = ath6kl_wmi_set_wow_mode_cmd(ar->wmi, vif->fw_vif_idx,
+					  ATH6KL_WOW_MODE_DISABLE,
+					  0, 0);
+	if (ret)
+		return ret;
+
+	/* Flush all non control pkts in TX path */
+	ath6kl_tx_data_cleanup(ar);
+
+	ret = ath6kl_cfg80211_host_sleep(ar, vif);
+	if (ret)
+		return ret;
+
+	return 0;
+}
+
+static int ath6kl_cfg80211_deepsleep_resume(struct ath6kl *ar)
+{
+	struct ath6kl_vif *vif;
+	int ret;
+
+	vif = ath6kl_vif_first(ar);
+
+	if (!vif)
+		return -EIO;
+
+	if (ar->wmi->pwr_mode != ar->wmi->saved_pwr_mode) {
+		ret = ath6kl_wmi_powermode_cmd(ar->wmi, 0,
+					       ar->wmi->saved_pwr_mode);
+		if (ret)
+			return ret;
+	}
+
+	ret = ath6kl_wmi_set_host_sleep_mode_cmd(ar->wmi, vif->fw_vif_idx,
+						 ATH6KL_HOST_MODE_AWAKE);
+	if (ret)
+		return ret;
+
+	ar->state = ATH6KL_STATE_ON;
+
+	/* Reset scan parameter to default values */
+	ret = ath6kl_wmi_scanparams_cmd(ar->wmi, vif->fw_vif_idx,
+					0, 0, 0, 0, 0, 0, 3, 0, 0, 0);
+	if (ret)
+		return ret;
+
+	return 0;
+}
+
 int ath6kl_cfg80211_suspend(struct ath6kl *ar,
 			    enum ath6kl_cfg_suspend_mode mode,
 			    struct cfg80211_wowlan *wow)
 {
+	struct ath6kl_vif *vif;
 	enum ath6kl_state prev_state;
 	int ret;
 
@@ -2137,15 +2323,12 @@ int ath6kl_cfg80211_suspend(struct ath6kl *ar,
 
 	case ATH6KL_CFG_SUSPEND_DEEPSLEEP:
 
-		ath6kl_cfg80211_stop_all(ar);
-
-		/* save the current power mode before enabling power save */
-		ar->wmi->saved_pwr_mode = ar->wmi->pwr_mode;
+		ath6kl_dbg(ATH6KL_DBG_SUSPEND, "deep sleep suspend\n");
 
-		ret = ath6kl_wmi_powermode_cmd(ar->wmi, 0, REC_POWER);
+		ret = ath6kl_cfg80211_deepsleep_suspend(ar);
 		if (ret) {
-			ath6kl_warn("wmi powermode command failed during suspend: %d\n",
-				    ret);
+			ath6kl_err("deepsleep suspend failed: %d\n", ret);
+			return ret;
 		}
 
 		ar->state = ATH6KL_STATE_DEEPSLEEP;
@@ -2185,6 +2368,9 @@ int ath6kl_cfg80211_suspend(struct ath6kl *ar,
 		break;
 	}
 
+	list_for_each_entry(vif, &ar->vif_list, list)
+		ath6kl_cfg80211_scan_complete_event(vif, true);
+
 	return 0;
 }
 EXPORT_SYMBOL(ath6kl_cfg80211_suspend);
@@ -2206,17 +2392,13 @@ int ath6kl_cfg80211_resume(struct ath6kl *ar)
 		break;
 
 	case ATH6KL_STATE_DEEPSLEEP:
-		if (ar->wmi->pwr_mode != ar->wmi->saved_pwr_mode) {
-			ret = ath6kl_wmi_powermode_cmd(ar->wmi, 0,
-						       ar->wmi->saved_pwr_mode);
-			if (ret) {
-				ath6kl_warn("wmi powermode command failed during resume: %d\n",
-					    ret);
-			}
-		}
-
-		ar->state = ATH6KL_STATE_ON;
+		ath6kl_dbg(ATH6KL_DBG_SUSPEND, "deep sleep resume\n");
 
+		ret = ath6kl_cfg80211_deepsleep_resume(ar);
+		if (ret) {
+			ath6kl_warn("deep sleep resume failed: %d\n", ret);
+			return ret;
+		}
 		break;
 
 	case ATH6KL_STATE_CUTPOWER:
@@ -2290,31 +2472,43 @@ void ath6kl_check_wow_status(struct ath6kl *ar)
 }
 #endif
 
-static int ath6kl_set_channel(struct wiphy *wiphy, struct net_device *dev,
-			      struct ieee80211_channel *chan,
-			      enum nl80211_channel_type channel_type)
+static int ath6kl_set_htcap(struct ath6kl_vif *vif, enum ieee80211_band band,
+			    bool ht_enable)
 {
-	struct ath6kl_vif *vif;
+	struct ath6kl_htcap *htcap = &vif->htcap;
 
-	/*
-	 * 'dev' could be NULL if a channel change is required for the hardware
-	 * device itself, instead of a particular VIF.
-	 *
-	 * FIXME: To be handled properly when monitor mode is supported.
-	 */
-	if (!dev)
-		return -EBUSY;
+	if (htcap->ht_enable == ht_enable)
+		return 0;
 
-	vif = netdev_priv(dev);
+	if (ht_enable) {
+		/* Set default ht capabilities */
+		htcap->ht_enable = true;
+		htcap->cap_info = (band == IEEE80211_BAND_2GHZ) ?
+				   ath6kl_g_htcap : ath6kl_a_htcap;
+		htcap->ampdu_factor = IEEE80211_HT_MAX_AMPDU_16K;
+	} else /* Disable ht */
+		memset(htcap, 0, sizeof(*htcap));
 
-	if (!ath6kl_cfg80211_ready(vif))
-		return -EIO;
+	return ath6kl_wmi_set_htcap_cmd(vif->ar->wmi, vif->fw_vif_idx,
+					band, htcap);
+}
 
-	ath6kl_dbg(ATH6KL_DBG_WLAN_CFG, "%s: center_freq=%u hw_value=%u\n",
-		   __func__, chan->center_freq, chan->hw_value);
-	vif->next_chan = chan->center_freq;
+static int ath6kl_restore_htcap(struct ath6kl_vif *vif)
+{
+	struct wiphy *wiphy = vif->ar->wiphy;
+	int band, ret = 0;
 
-	return 0;
+	for (band = 0; band < IEEE80211_NUM_BANDS; band++) {
+		if (!wiphy->bands[band])
+			continue;
+
+		ret = ath6kl_set_htcap(vif, band,
+				wiphy->bands[band]->ht_cap.ht_supported);
+		if (ret)
+			return ret;
+	}
+
+	return ret;
 }
 
 static bool ath6kl_is_p2p_ie(const u8 *pos)
@@ -2391,6 +2585,87 @@ static int ath6kl_set_ies(struct ath6kl_vif *vif,
 	return 0;
 }
 
+static int ath6kl_set_channel(struct wiphy *wiphy, struct net_device *dev,
+			      struct ieee80211_channel *chan,
+			      enum nl80211_channel_type channel_type)
+{
+	struct ath6kl_vif *vif;
+
+	/*
+	 * 'dev' could be NULL if a channel change is required for the hardware
+	 * device itself, instead of a particular VIF.
+	 *
+	 * FIXME: To be handled properly when monitor mode is supported.
+	 */
+	if (!dev)
+		return -EBUSY;
+
+	vif = netdev_priv(dev);
+
+	if (!ath6kl_cfg80211_ready(vif))
+		return -EIO;
+
+	ath6kl_dbg(ATH6KL_DBG_WLAN_CFG, "%s: center_freq=%u hw_value=%u\n",
+		   __func__, chan->center_freq, chan->hw_value);
+	vif->next_chan = chan->center_freq;
+	vif->next_ch_type = channel_type;
+	vif->next_ch_band = chan->band;
+
+	return 0;
+}
+
+static int ath6kl_get_rsn_capab(struct cfg80211_beacon_data *beacon,
+				u8 *rsn_capab)
+{
+	const u8 *rsn_ie;
+	size_t rsn_ie_len;
+	u16 cnt;
+
+	if (!beacon->tail)
+		return -EINVAL;
+
+	rsn_ie = cfg80211_find_ie(WLAN_EID_RSN, beacon->tail, beacon->tail_len);
+	if (!rsn_ie)
+		return -EINVAL;
+
+	rsn_ie_len = *(rsn_ie + 1);
+	/* skip element id and length */
+	rsn_ie += 2;
+
+	/* skip version */
+	if (rsn_ie_len < 2)
+		return -EINVAL;
+	rsn_ie +=  2;
+	rsn_ie_len -= 2;
+
+	/* skip group cipher suite */
+	if (rsn_ie_len < 4)
+		return 0;
+	rsn_ie +=  4;
+	rsn_ie_len -= 4;
+
+	/* skip pairwise cipher suite */
+	if (rsn_ie_len < 2)
+		return 0;
+	cnt = get_unaligned_le16(rsn_ie);
+	rsn_ie += (2 + cnt * 4);
+	rsn_ie_len -= (2 + cnt * 4);
+
+	/* skip akm suite */
+	if (rsn_ie_len < 2)
+		return 0;
+	cnt = get_unaligned_le16(rsn_ie);
+	rsn_ie += (2 + cnt * 4);
+	rsn_ie_len -= (2 + cnt * 4);
+
+	if (rsn_ie_len < 2)
+		return 0;
+
+	memcpy(rsn_capab, rsn_ie, 2);
+
+	return 0;
+}
+
 static int ath6kl_start_ap(struct wiphy *wiphy, struct net_device *dev,
 			   struct cfg80211_ap_settings *info)
 {
@@ -2403,6 +2678,7 @@ static int ath6kl_start_ap(struct wiphy *wiphy, struct net_device *dev,
 	struct wmi_connect_cmd p;
 	int res;
 	int i, ret;
+	u16 rsn_capab = 0;
 
 	ath6kl_dbg(ATH6KL_DBG_WLAN_CFG, "%s:\n", __func__);
 
@@ -2532,6 +2808,35 @@ static int ath6kl_start_ap(struct wiphy *wiphy, struct net_device *dev,
 		p.nw_subtype = SUBTYPE_NONE;
 	}
 
+	if (info->inactivity_timeout) {
+		res = ath6kl_wmi_set_inact_period(ar->wmi, vif->fw_vif_idx,
+						  info->inactivity_timeout);
+		if (res < 0)
+			return res;
+	}
+
+	if (ath6kl_set_htcap(vif, vif->next_ch_band,
+			     vif->next_ch_type != NL80211_CHAN_NO_HT))
+		return -EIO;
+
+	/*
+	 * Get the PTKSA replay counter in the RSN IE. Supplicant
+	 * will use the RSN IE in M3 message and firmware has to
+	 * advertise the same in beacon/probe response. Send
+	 * the complete RSN IE capability field to firmware
+	 */
+	if (!ath6kl_get_rsn_capab(&info->beacon, (u8 *) &rsn_capab) &&
+	    test_bit(ATH6KL_FW_CAPABILITY_RSN_CAP_OVERRIDE,
+		     ar->fw_capabilities)) {
+		res = ath6kl_wmi_set_ie_cmd(ar->wmi, vif->fw_vif_idx,
+					    WLAN_EID_RSN, WMI_RSN_IE_CAPB,
+					    (const u8 *) &rsn_capab,
+					    sizeof(rsn_capab));
+		if (res < 0)
+			return res;
+	}
+
+	memcpy(&vif->profile, &p, sizeof(p));
 	res = ath6kl_wmi_ap_profile_commit(ar->wmi, vif->fw_vif_idx, &p);
 	if (res < 0)
 		return res;
@@ -2566,7 +2871,8 @@ static int ath6kl_stop_ap(struct wiphy *wiphy, struct net_device *dev)
 	ath6kl_wmi_disconnect_cmd(ar->wmi, vif->fw_vif_idx);
 	clear_bit(CONNECTED, &vif->flags);
 
-	return 0;
+	/* Restore ht setting in firmware */
+	return ath6kl_restore_htcap(vif);
 }
 
 static const u8 bcast_addr[ETH_ALEN] = { 0xff, 0xff, 0xff, 0xff, 0xff, 0xff };
@@ -2747,6 +3053,21 @@ static bool ath6kl_mgmt_powersave_ap(struct ath6kl_vif *vif,
 	return false;
 }
 
+/* Check if SSID length is greater than DIRECT- */
+static bool ath6kl_is_p2p_go_ssid(const u8 *buf, size_t len)
+{
+	const struct ieee80211_mgmt *mgmt;
+	mgmt = (const struct ieee80211_mgmt *) buf;
+
+	/* variable[1] contains the SSID tag length */
+	if (buf + len >= &mgmt->u.probe_resp.variable[1] &&
+	    (mgmt->u.probe_resp.variable[1] > P2P_WILDCARD_SSID_LEN)) {
+		return true;
+	}
+
+	return false;
+}
+
 static int ath6kl_mgmt_tx(struct wiphy *wiphy, struct net_device *dev,
 			  struct ieee80211_channel *chan, bool offchan,
 			  enum nl80211_channel_type channel_type,
@@ -2761,11 +3082,11 @@ static int ath6kl_mgmt_tx(struct wiphy *wiphy, struct net_device *dev,
 	bool more_data, queued;
 
 	mgmt = (const struct ieee80211_mgmt *) buf;
-	if (buf + len >= mgmt->u.probe_resp.variable &&
-	    vif->nw_type == AP_NETWORK && test_bit(CONNECTED, &vif->flags) &&
-	    ieee80211_is_probe_resp(mgmt->frame_control)) {
+	if (vif->nw_type == AP_NETWORK && test_bit(CONNECTED, &vif->flags) &&
+	    ieee80211_is_probe_resp(mgmt->frame_control) &&
+	    ath6kl_is_p2p_go_ssid(buf, len)) {
 		/*
-		 * Send Probe Response frame in AP mode using a separate WMI
+		 * Send Probe Response frame in GO mode using a separate WMI
 		 * command to allow the target to fill in the generic IEs.
 		 */
 		*cookie = 0; /* TX status not supported */
@@ -2825,7 +3146,6 @@ static int ath6kl_cfg80211_sscan_start(struct wiphy *wiphy,
 	struct ath6kl_vif *vif = netdev_priv(dev);
 	u16 interval;
 	int ret;
-	u8 i;
 
 	if (ar->state != ATH6KL_STATE_ON)
 		return -EIO;
@@ -2833,27 +3153,23 @@ static int ath6kl_cfg80211_sscan_start(struct wiphy *wiphy,
 	if (vif->sme_state != SME_DISCONNECTED)
 		return -EBUSY;
 
-	for (i = 0; i < ar->wiphy->max_sched_scan_ssids; i++) {
-		ath6kl_wmi_probedssid_cmd(ar->wmi, vif->fw_vif_idx,
-					  i, DISABLE_SSID_FLAG,
-					  0, NULL);
-	}
+	/* The FW currently can't support multi-vif WoW properly. */
+	if (ar->num_vif > 1)
+		return -EIO;
+
+	ath6kl_cfg80211_scan_complete_event(vif, true);
+
+	ret = ath6kl_set_probed_ssids(ar, vif, request->ssids,
+				      request->n_ssids);
+	if (ret < 0)
+		return ret;
 
 	/* fw uses seconds, also make sure that it's >0 */
 	interval = max_t(u16, 1, request->interval / 1000);
 
 	ath6kl_wmi_scanparams_cmd(ar->wmi, vif->fw_vif_idx,
 				  interval, interval,
-				  10, 0, 0, 0, 3, 0, 0, 0);
-
-	if (request->n_ssids && request->ssids[0].ssid_len) {
-		for (i = 0; i < request->n_ssids; i++) {
-			ath6kl_wmi_probedssid_cmd(ar->wmi, vif->fw_vif_idx,
-						  i, SPECIFIC_SSID_FLAG,
-						  request->ssids[i].ssid_len,
-						  request->ssids[i].ssid);
-		}
-	}
+				  vif->bg_scan_period, 0, 0, 0, 3, 0, 0, 0);
 
 	ret = ath6kl_wmi_set_wow_mode_cmd(ar->wmi, vif->fw_vif_idx,
 					  ATH6KL_WOW_MODE_ENABLE,
@@ -3013,8 +3329,7 @@ void ath6kl_cfg80211_stop_all(struct ath6kl *ar)
 		ar->wmi->saved_pwr_mode = ar->wmi->pwr_mode;
 
 		if (ath6kl_wmi_powermode_cmd(ar->wmi, 0, REC_POWER) != 0)
-			ath6kl_warn("ath6kl_deep_sleep_enable: "
-				    "wmi_powermode_cmd failed\n");
+			ath6kl_warn("ath6kl_deep_sleep_enable: wmi_powermode_cmd failed\n");
 		return;
 	}
 
@@ -3094,6 +3409,8 @@ struct net_device *ath6kl_interface_add(struct ath6kl *ar, char *name,
 	vif->next_mode = nw_type;
 	vif->listen_intvl_t = ATH6KL_DEFAULT_LISTEN_INTVAL;
 	vif->bmiss_time_t = ATH6KL_DEFAULT_BMISS_TIME;
+	vif->bg_scan_period = 0;
+	vif->htcap.ht_enable = true;
 
 	memcpy(ndev->dev_addr, ar->mac_addr, ETH_ALEN);
 	if (fw_vif_idx != 0)
@@ -3134,6 +3451,7 @@ err:
 int ath6kl_cfg80211_init(struct ath6kl *ar)
 {
 	struct wiphy *wiphy = ar->wiphy;
+	bool band_2gig = false, band_5gig = false, ht = false;
 	int ret;
 
 	wiphy->mgmt_stypes = ath6kl_mgmt_stypes;
@@ -3154,8 +3472,46 @@ int ath6kl_cfg80211_init(struct ath6kl *ar)
 	/* max num of ssids that can be probed during scanning */
 	wiphy->max_scan_ssids = MAX_PROBED_SSID_INDEX;
 	wiphy->max_scan_ie_len = 1000; /* FIX: what is correct limit? */
-	wiphy->bands[IEEE80211_BAND_2GHZ] = &ath6kl_band_2ghz;
-	wiphy->bands[IEEE80211_BAND_5GHZ] = &ath6kl_band_5ghz;
+	switch (ar->hw.cap) {
+	case WMI_11AN_CAP:
+		ht = true;
+	case WMI_11A_CAP:
+		band_5gig = true;
+		break;
+	case WMI_11GN_CAP:
+		ht = true;
+	case WMI_11G_CAP:
+		band_2gig = true;
+		break;
+	case WMI_11AGN_CAP:
+		ht = true;
+	case WMI_11AG_CAP:
+		band_2gig = true;
+		band_5gig = true;
+		break;
+	default:
+		ath6kl_err("invalid phy capability!\n");
+		return -EINVAL;
+	}
+
+	/*
+	 * Even if the fw has HT support, advertise HT cap only when
+	 * the firmware has support to override RSN capability, otherwise
+	 * 4-way handshake would fail.
+	 */
+	if (!(ht &&
+	      test_bit(ATH6KL_FW_CAPABILITY_RSN_CAP_OVERRIDE,
+		       ar->fw_capabilities))) {
+		ath6kl_band_2ghz.ht_cap.cap = 0;
+		ath6kl_band_2ghz.ht_cap.ht_supported = false;
+		ath6kl_band_5ghz.ht_cap.cap = 0;
+		ath6kl_band_5ghz.ht_cap.ht_supported = false;
+	}
+	if (band_2gig)
+		wiphy->bands[IEEE80211_BAND_2GHZ] = &ath6kl_band_2ghz;
+	if (band_5gig)
+		wiphy->bands[IEEE80211_BAND_5GHZ] = &ath6kl_band_5ghz;
+
 	wiphy->signal_type = CFG80211_SIGNAL_TYPE_MBM;
 
 	wiphy->cipher_suites = cipher_suites;
@@ -3171,7 +3527,7 @@ int ath6kl_cfg80211_init(struct ath6kl *ar)
 	wiphy->wowlan.pattern_min_len = 1;
 	wiphy->wowlan.pattern_max_len = WOW_PATTERN_SIZE;
 
-	wiphy->max_sched_scan_ssids = 10;
+	wiphy->max_sched_scan_ssids = MAX_PROBED_SSID_INDEX;
 
 	ar->wiphy->flags |= WIPHY_FLAG_SUPPORTS_FW_ROAM |
 			    WIPHY_FLAG_HAVE_AP_SME |
@@ -3181,11 +3537,14 @@ int ath6kl_cfg80211_init(struct ath6kl *ar)
 	if (test_bit(ATH6KL_FW_CAPABILITY_SCHED_SCAN, ar->fw_capabilities))
 		ar->wiphy->flags |= WIPHY_FLAG_SUPPORTS_SCHED_SCAN;
 
+	if (test_bit(ATH6KL_FW_CAPABILITY_INACTIVITY_TIMEOUT,
+		     ar->fw_capabilities))
+		ar->wiphy->features = NL80211_FEATURE_INACTIVITY_TIMER;
+
 	ar->wiphy->probe_resp_offload =
 		NL80211_PROBE_RESP_OFFLOAD_SUPPORT_WPS |
 		NL80211_PROBE_RESP_OFFLOAD_SUPPORT_WPS2 |
-		NL80211_PROBE_RESP_OFFLOAD_SUPPORT_P2P |
-		NL80211_PROBE_RESP_OFFLOAD_SUPPORT_80211U;
+		NL80211_PROBE_RESP_OFFLOAD_SUPPORT_P2P;
 
 	ret = wiphy_register(wiphy);
 	if (ret < 0) {
diff --git a/drivers/net/wireless/ath/ath6kl/cfg80211.h b/drivers/net/wireless/ath/ath6kl/cfg80211.h
index c5def436417..5ea8cbb79f4 100644
--- a/drivers/net/wireless/ath/ath6kl/cfg80211.h
+++ b/drivers/net/wireless/ath/ath6kl/cfg80211.h
@@ -28,6 +28,8 @@ enum ath6kl_cfg_suspend_mode {
 struct net_device *ath6kl_interface_add(struct ath6kl *ar, char *name,
 					enum nl80211_iftype type,
 					u8 fw_vif_idx, u8 nw_type);
+void ath6kl_cfg80211_ch_switch_notify(struct ath6kl_vif *vif, int freq,
+				      enum wmi_phy_mode mode);
 void ath6kl_cfg80211_scan_complete_event(struct ath6kl_vif *vif, bool aborted);
 
 void ath6kl_cfg80211_connect_event(struct ath6kl_vif *vif, u16 channel,
diff --git a/drivers/net/wireless/ath/ath6kl/common.h b/drivers/net/wireless/ath/ath6kl/common.h
index a60e78c0472..98a886154d9 100644
--- a/drivers/net/wireless/ath/ath6kl/common.h
+++ b/drivers/net/wireless/ath/ath6kl/common.h
@@ -22,7 +22,8 @@
 
 #define ATH6KL_MAX_IE			256
 
-extern int ath6kl_printk(const char *level, const char *fmt, ...);
+extern __printf(2, 3)
+int ath6kl_printk(const char *level, const char *fmt, ...);
 
 /*
  * Reflects the version of binary interface exposed by ATH6KL target
@@ -77,6 +78,7 @@ enum crypto_type {
 
 struct htc_endpoint_credit_dist;
 struct ath6kl;
+struct ath6kl_htcap;
 enum htc_credit_dist_reason;
 struct ath6kl_htc_credit_info;
 
diff --git a/drivers/net/wireless/ath/ath6kl/core.c b/drivers/net/wireless/ath/ath6kl/core.c
index 45e641f3a41..fdb3b1decc7 100644
--- a/drivers/net/wireless/ath/ath6kl/core.c
+++ b/drivers/net/wireless/ath/ath6kl/core.c
@@ -20,9 +20,11 @@
 #include <linux/module.h>
 #include <linux/moduleparam.h>
 #include <linux/export.h>
+#include <linux/vmalloc.h>
 
 #include "debug.h"
 #include "hif-ops.h"
+#include "htc-ops.h"
 #include "cfg80211.h"
 
 unsigned int debug_mask;
@@ -39,12 +41,36 @@ module_param(uart_debug, uint, 0644);
 module_param(ath6kl_p2p, uint, 0644);
 module_param(testmode, uint, 0644);
 
-int ath6kl_core_init(struct ath6kl *ar)
+void ath6kl_core_tx_complete(struct ath6kl *ar, struct sk_buff *skb)
+{
+	ath6kl_htc_tx_complete(ar, skb);
+}
+EXPORT_SYMBOL(ath6kl_core_tx_complete);
+
+void ath6kl_core_rx_complete(struct ath6kl *ar, struct sk_buff *skb, u8 pipe)
+{
+	ath6kl_htc_rx_complete(ar, skb, pipe);
+}
+EXPORT_SYMBOL(ath6kl_core_rx_complete);
+
+int ath6kl_core_init(struct ath6kl *ar, enum ath6kl_htc_type htc_type)
 {
 	struct ath6kl_bmi_target_info targ_info;
 	struct net_device *ndev;
 	int ret = 0, i;
 
+	switch (htc_type) {
+	case ATH6KL_HTC_TYPE_MBOX:
+		ath6kl_htc_mbox_attach(ar);
+		break;
+	case ATH6KL_HTC_TYPE_PIPE:
+		ath6kl_htc_pipe_attach(ar);
+		break;
+	default:
+		WARN_ON(1);
+		return -ENOMEM;
+	}
+
 	ar->ath6kl_wq = create_singlethread_workqueue("ath6kl");
 	if (!ar->ath6kl_wq)
 		return -ENOMEM;
@@ -280,7 +306,7 @@ void ath6kl_core_cleanup(struct ath6kl *ar)
 
 	kfree(ar->fw_board);
 	kfree(ar->fw_otp);
-	kfree(ar->fw);
+	vfree(ar->fw);
 	kfree(ar->fw_patch);
 	kfree(ar->fw_testscript);
 
diff --git a/drivers/net/wireless/ath/ath6kl/core.h b/drivers/net/wireless/ath/ath6kl/core.h
index f1dd8906be4..4d9c6f14269 100644
--- a/drivers/net/wireless/ath/ath6kl/core.h
+++ b/drivers/net/wireless/ath/ath6kl/core.h
@@ -91,6 +91,15 @@ enum ath6kl_fw_capability {
 	 */
 	ATH6KL_FW_CAPABILITY_STA_P2PDEV_DUPLEX,
 
+	/*
+	 * Firmware has support to cleanup inactive stations
+	 * in AP mode.
+	 */
+	ATH6KL_FW_CAPABILITY_INACTIVITY_TIMEOUT,
+
+	/* Firmware has support to override rsn cap of rsn ie */
+	ATH6KL_FW_CAPABILITY_RSN_CAP_OVERRIDE,
+
 	/* this needs to be last */
 	ATH6KL_FW_CAPABILITY_MAX,
 };
@@ -117,9 +126,9 @@ struct ath6kl_fw_ie {
 #define AR6003_HW_2_0_FIRMWARE_FILE		"athwlan.bin.z77"
 #define AR6003_HW_2_0_TCMD_FIRMWARE_FILE	"athtcmd_ram.bin"
 #define AR6003_HW_2_0_PATCH_FILE		"data.patch.bin"
-#define AR6003_HW_2_0_BOARD_DATA_FILE "ath6k/AR6003/hw2.0/bdata.bin"
+#define AR6003_HW_2_0_BOARD_DATA_FILE AR6003_HW_2_0_FW_DIR "/bdata.bin"
 #define AR6003_HW_2_0_DEFAULT_BOARD_DATA_FILE \
-			"ath6k/AR6003/hw2.0/bdata.SD31.bin"
+			AR6003_HW_2_0_FW_DIR "/bdata.SD31.bin"
 
 /* AR6003 3.0 definitions */
 #define AR6003_HW_2_1_1_VERSION                 0x30000582
@@ -130,25 +139,33 @@ struct ath6kl_fw_ie {
 #define AR6003_HW_2_1_1_UTF_FIRMWARE_FILE	"utf.bin"
 #define AR6003_HW_2_1_1_TESTSCRIPT_FILE	"nullTestFlow.bin"
 #define AR6003_HW_2_1_1_PATCH_FILE		"data.patch.bin"
-#define AR6003_HW_2_1_1_BOARD_DATA_FILE "ath6k/AR6003/hw2.1.1/bdata.bin"
+#define AR6003_HW_2_1_1_BOARD_DATA_FILE AR6003_HW_2_1_1_FW_DIR "/bdata.bin"
 #define AR6003_HW_2_1_1_DEFAULT_BOARD_DATA_FILE	\
-			"ath6k/AR6003/hw2.1.1/bdata.SD31.bin"
+			AR6003_HW_2_1_1_FW_DIR "/bdata.SD31.bin"
 
 /* AR6004 1.0 definitions */
 #define AR6004_HW_1_0_VERSION                 0x30000623
 #define AR6004_HW_1_0_FW_DIR			"ath6k/AR6004/hw1.0"
 #define AR6004_HW_1_0_FIRMWARE_FILE		"fw.ram.bin"
-#define AR6004_HW_1_0_BOARD_DATA_FILE         "ath6k/AR6004/hw1.0/bdata.bin"
+#define AR6004_HW_1_0_BOARD_DATA_FILE         AR6004_HW_1_0_FW_DIR "/bdata.bin"
 #define AR6004_HW_1_0_DEFAULT_BOARD_DATA_FILE \
-	"ath6k/AR6004/hw1.0/bdata.DB132.bin"
+	AR6004_HW_1_0_FW_DIR "/bdata.DB132.bin"
 
 /* AR6004 1.1 definitions */
 #define AR6004_HW_1_1_VERSION                 0x30000001
 #define AR6004_HW_1_1_FW_DIR			"ath6k/AR6004/hw1.1"
 #define AR6004_HW_1_1_FIRMWARE_FILE		"fw.ram.bin"
-#define AR6004_HW_1_1_BOARD_DATA_FILE         "ath6k/AR6004/hw1.1/bdata.bin"
+#define AR6004_HW_1_1_BOARD_DATA_FILE         AR6004_HW_1_1_FW_DIR "/bdata.bin"
 #define AR6004_HW_1_1_DEFAULT_BOARD_DATA_FILE \
-	"ath6k/AR6004/hw1.1/bdata.DB132.bin"
+	AR6004_HW_1_1_FW_DIR "/bdata.DB132.bin"
+
+/* AR6004 1.2 definitions */
+#define AR6004_HW_1_2_VERSION                 0x300007e8
+#define AR6004_HW_1_2_FW_DIR			"ath6k/AR6004/hw1.2"
+#define AR6004_HW_1_2_FIRMWARE_FILE           "fw.ram.bin"
+#define AR6004_HW_1_2_BOARD_DATA_FILE         AR6004_HW_1_2_FW_DIR "/bdata.bin"
+#define AR6004_HW_1_2_DEFAULT_BOARD_DATA_FILE \
+	AR6004_HW_1_2_FW_DIR "/bdata.bin"
 
 /* Per STA data, used in AP mode */
 #define STA_PS_AWAKE		BIT(0)
@@ -205,6 +222,8 @@ struct ath6kl_fw_ie {
 #define ATH6KL_CONF_ENABLE_TX_BURST		BIT(3)
 #define ATH6KL_CONF_UART_DEBUG			BIT(4)
 
+#define P2P_WILDCARD_SSID_LEN			7 /* DIRECT- */
+
 enum wlan_low_pwr_state {
 	WLAN_POWER_STATE_ON,
 	WLAN_POWER_STATE_CUT_PWR,
@@ -454,6 +473,11 @@ enum ath6kl_hif_type {
 	ATH6KL_HIF_TYPE_USB,
 };
 
+enum ath6kl_htc_type {
+	ATH6KL_HTC_TYPE_MBOX,
+	ATH6KL_HTC_TYPE_PIPE,
+};
+
 /* Max number of filters that hw supports */
 #define ATH6K_MAX_MC_FILTERS_PER_LIST 7
 struct ath6kl_mc_filter {
@@ -461,6 +485,12 @@ struct ath6kl_mc_filter {
 	char hw_addr[ATH6KL_MCAST_FILTER_MAC_ADDR_SIZE];
 };
 
+struct ath6kl_htcap {
+	bool ht_enable;
+	u8 ampdu_factor;
+	unsigned short cap_info;
+};
+
 /*
  * Driver's maximum limit, note that some firmwares support only one vif
  * and the runtime (current) limit must be checked from ar->vif_max.
@@ -480,6 +510,8 @@ enum ath6kl_vif_state {
 	WLAN_ENABLED,
 	STATS_UPDATE_PEND,
 	HOST_SLEEP_MODE_CMD_PROCESSED,
+	NETDEV_MCAST_ALL_ON,
+	NETDEV_MCAST_ALL_OFF,
 };
 
 struct ath6kl_vif {
@@ -509,6 +541,7 @@ struct ath6kl_vif {
 	struct ath6kl_wep_key wep_key_list[WMI_MAX_KEY_INDEX + 1];
 	struct ath6kl_key keys[WMI_MAX_KEY_INDEX + 1];
 	struct aggr_info *aggr_cntxt;
+	struct ath6kl_htcap htcap;
 
 	struct timer_list disconnect_timer;
 	struct timer_list sched_scan_timer;
@@ -521,12 +554,16 @@ struct ath6kl_vif {
 	u32 send_action_id;
 	bool probe_req_report;
 	u16 next_chan;
+	enum nl80211_channel_type next_ch_type;
+	enum ieee80211_band next_ch_band;
 	u16 assoc_bss_beacon_int;
 	u16 listen_intvl_t;
 	u16 bmiss_time_t;
+	u16 bg_scan_period;
 	u8 assoc_bss_dtim_period;
 	struct net_device_stats net_stats;
 	struct target_stats target_stats;
+	struct wmi_connect_cmd profile;
 
 	struct list_head mc_filter;
 };
@@ -568,6 +605,7 @@ struct ath6kl {
 
 	struct ath6kl_bmi bmi;
 	const struct ath6kl_hif_ops *hif_ops;
+	const struct ath6kl_htc_ops *htc_ops;
 	struct wmi *wmi;
 	int tx_pending[ENDPOINT_MAX];
 	int total_tx_data_pend;
@@ -614,6 +652,7 @@ struct ath6kl {
 	u8 sta_list_index;
 	struct ath6kl_req_key ap_mode_bkey;
 	struct sk_buff_head mcastpsq;
+	u32 want_ch_switch;
 
 	/*
 	 * FIXME: protects access to mcastpsq but is actually useless as
@@ -646,6 +685,7 @@ struct ath6kl {
 		u32 refclk_hz;
 		u32 uarttx_pin;
 		u32 testscript_addr;
+		enum wmi_phy_cap cap;
 
 		struct ath6kl_hw_fw {
 			const char *dir;
@@ -746,7 +786,8 @@ void init_netdev(struct net_device *dev);
 void ath6kl_cookie_init(struct ath6kl *ar);
 void ath6kl_cookie_cleanup(struct ath6kl *ar);
 void ath6kl_rx(struct htc_target *target, struct htc_packet *packet);
-void ath6kl_tx_complete(void *context, struct list_head *packet_queue);
+void ath6kl_tx_complete(struct htc_target *context,
+			struct list_head *packet_queue);
 enum htc_send_full_action ath6kl_tx_queue_full(struct htc_target *target,
 					       struct htc_packet *packet);
 void ath6kl_stop_txrx(struct ath6kl *ar);
@@ -778,7 +819,8 @@ void aggr_reset_state(struct aggr_info_conn *aggr_conn);
 struct ath6kl_sta *ath6kl_find_sta(struct ath6kl_vif *vif, u8 *node_addr);
 struct ath6kl_sta *ath6kl_find_sta_by_aid(struct ath6kl *ar, u8 aid);
 
-void ath6kl_ready_event(void *devt, u8 *datap, u32 sw_ver, u32 abi_ver);
+void ath6kl_ready_event(void *devt, u8 *datap, u32 sw_ver, u32 abi_ver,
+			enum wmi_phy_cap cap);
 int ath6kl_control_tx(void *devt, struct sk_buff *skb,
 		      enum htc_endpoint_id eid);
 void ath6kl_connect_event(struct ath6kl_vif *vif, u16 channel,
@@ -821,8 +863,11 @@ int ath6kl_init_hw_params(struct ath6kl *ar);
 
 void ath6kl_check_wow_status(struct ath6kl *ar);
 
+void ath6kl_core_tx_complete(struct ath6kl *ar, struct sk_buff *skb);
+void ath6kl_core_rx_complete(struct ath6kl *ar, struct sk_buff *skb, u8 pipe);
+
 struct ath6kl *ath6kl_core_create(struct device *dev);
-int ath6kl_core_init(struct ath6kl *ar);
+int ath6kl_core_init(struct ath6kl *ar, enum ath6kl_htc_type htc_type);
 void ath6kl_core_cleanup(struct ath6kl *ar);
 void ath6kl_core_destroy(struct ath6kl *ar);
 
diff --git a/drivers/net/wireless/ath/ath6kl/debug.c b/drivers/net/wireless/ath/ath6kl/debug.c
index d01403a263f..15cfe30e54f 100644
--- a/drivers/net/wireless/ath/ath6kl/debug.c
+++ b/drivers/net/wireless/ath/ath6kl/debug.c
@@ -401,8 +401,10 @@ static ssize_t ath6kl_fwlog_block_read(struct file *file,
 
 		ret = wait_for_completion_interruptible(
 			&ar->debug.fwlog_completion);
-		if (ret == -ERESTARTSYS)
+		if (ret == -ERESTARTSYS) {
+			vfree(buf);
 			return ret;
+		}
 
 		spin_lock(&ar->debug.fwlog_queue.lock);
 	}
@@ -616,6 +618,12 @@ static ssize_t read_file_tgt_stats(struct file *file, char __user *user_buf,
 			 "Num disconnects", tgt_stats->cs_discon_cnt);
 	len += scnprintf(buf + len, buf_len - len, "%20s %10d\n",
 			 "Beacon avg rssi", tgt_stats->cs_ave_beacon_rssi);
+	len += scnprintf(buf + len, buf_len - len, "%20s %10d\n",
+			 "ARP pkt received", tgt_stats->arp_received);
+	len += scnprintf(buf + len, buf_len - len, "%20s %10d\n",
+			 "ARP pkt matched", tgt_stats->arp_matched);
+	len += scnprintf(buf + len, buf_len - len, "%20s %10d\n",
+			 "ARP pkt replied", tgt_stats->arp_replied);
 
 	if (len > buf_len)
 		len = buf_len;
@@ -1564,10 +1572,15 @@ static ssize_t ath6kl_bgscan_int_write(struct file *file,
 				size_t count, loff_t *ppos)
 {
 	struct ath6kl *ar = file->private_data;
+	struct ath6kl_vif *vif;
 	u16 bgscan_int;
 	char buf[32];
 	ssize_t len;
 
+	vif = ath6kl_vif_first(ar);
+	if (!vif)
+		return -EIO;
+
 	len = min(count, sizeof(buf) - 1);
 	if (copy_from_user(buf, user_buf, len))
 		return -EFAULT;
@@ -1579,6 +1592,8 @@ static ssize_t ath6kl_bgscan_int_write(struct file *file,
 	if (bgscan_int == 0)
 		bgscan_int = 0xffff;
 
+	vif->bg_scan_period = bgscan_int;
+
 	ath6kl_wmi_scanparams_cmd(ar->wmi, 0, 0, 0, bgscan_int, 0, 0, 0, 3,
 				  0, 0, 0);
 
@@ -1803,6 +1818,7 @@ int ath6kl_debug_init_fs(struct ath6kl *ar)
 void ath6kl_debug_cleanup(struct ath6kl *ar)
 {
 	skb_queue_purge(&ar->debug.fwlog_queue);
+	complete(&ar->debug.fwlog_completion);
 	kfree(ar->debug.roam_tbl);
 }
 
diff --git a/drivers/net/wireless/ath/ath6kl/debug.h b/drivers/net/wireless/ath/ath6kl/debug.h
index 1803a0baae8..49639d8266c 100644
--- a/drivers/net/wireless/ath/ath6kl/debug.h
+++ b/drivers/net/wireless/ath/ath6kl/debug.h
@@ -43,6 +43,7 @@ enum ATH6K_DEBUG_MASK {
 	ATH6KL_DBG_WMI_DUMP	= BIT(19),
 	ATH6KL_DBG_SUSPEND	= BIT(20),
 	ATH6KL_DBG_USB		= BIT(21),
+	ATH6KL_DBG_USB_BULK	= BIT(22),
 	ATH6KL_DBG_ANY	        = 0xffffffff  /* enable all logs */
 };
 
diff --git a/drivers/net/wireless/ath/ath6kl/hif-ops.h b/drivers/net/wireless/ath/ath6kl/hif-ops.h
index fd84086638e..8c9e72d5250 100644
--- a/drivers/net/wireless/ath/ath6kl/hif-ops.h
+++ b/drivers/net/wireless/ath/ath6kl/hif-ops.h
@@ -150,4 +150,38 @@ static inline void ath6kl_hif_stop(struct ath6kl *ar)
 	ar->hif_ops->stop(ar);
 }
 
+static inline int ath6kl_hif_pipe_send(struct ath6kl *ar,
+				       u8 pipe, struct sk_buff *hdr_buf,
+				       struct sk_buff *buf)
+{
+	ath6kl_dbg(ATH6KL_DBG_HIF, "hif pipe send\n");
+
+	return ar->hif_ops->pipe_send(ar, pipe, hdr_buf, buf);
+}
+
+static inline void ath6kl_hif_pipe_get_default(struct ath6kl *ar,
+					       u8 *ul_pipe, u8 *dl_pipe)
+{
+	ath6kl_dbg(ATH6KL_DBG_HIF, "hif pipe get default\n");
+
+	ar->hif_ops->pipe_get_default(ar, ul_pipe, dl_pipe);
+}
+
+static inline int ath6kl_hif_pipe_map_service(struct ath6kl *ar,
+					      u16 service_id, u8 *ul_pipe,
+					      u8 *dl_pipe)
+{
+	ath6kl_dbg(ATH6KL_DBG_HIF, "hif pipe get default\n");
+
+	return ar->hif_ops->pipe_map_service(ar, service_id, ul_pipe, dl_pipe);
+}
+
+static inline u16 ath6kl_hif_pipe_get_free_queue_number(struct ath6kl *ar,
+							u8 pipe)
+{
+	ath6kl_dbg(ATH6KL_DBG_HIF, "hif pipe get free queue number\n");
+
+	return ar->hif_ops->pipe_get_free_queue_number(ar, pipe);
+}
+
 #endif
diff --git a/drivers/net/wireless/ath/ath6kl/hif.h b/drivers/net/wireless/ath/ath6kl/hif.h
index 20ed6b73517..61f6b21fb0a 100644
--- a/drivers/net/wireless/ath/ath6kl/hif.h
+++ b/drivers/net/wireless/ath/ath6kl/hif.h
@@ -256,6 +256,12 @@ struct ath6kl_hif_ops {
 	int (*power_on)(struct ath6kl *ar);
 	int (*power_off)(struct ath6kl *ar);
 	void (*stop)(struct ath6kl *ar);
+	int (*pipe_send)(struct ath6kl *ar, u8 pipe, struct sk_buff *hdr_buf,
+			 struct sk_buff *buf);
+	void (*pipe_get_default)(struct ath6kl *ar, u8 *pipe_ul, u8 *pipe_dl);
+	int (*pipe_map_service)(struct ath6kl *ar, u16 service_id, u8 *pipe_ul,
+				u8 *pipe_dl);
+	u16 (*pipe_get_free_queue_number)(struct ath6kl *ar, u8 pipe);
 };
 
 int ath6kl_hif_setup(struct ath6kl_device *dev);
diff --git a/drivers/net/wireless/ath/ath6kl/htc-ops.h b/drivers/net/wireless/ath/ath6kl/htc-ops.h
new file mode 100644
index 00000000000..2d4eed55cfd
--- /dev/null
+++ b/drivers/net/wireless/ath/ath6kl/htc-ops.h
@@ -0,0 +1,113 @@
+/*
+ * Copyright (c) 2004-2011 Atheros Communications 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.
+ */
+
+#ifndef HTC_OPS_H
+#define HTC_OPS_H
+
+#include "htc.h"
+#include "debug.h"
+
+static inline void *ath6kl_htc_create(struct ath6kl *ar)
+{
+	return ar->htc_ops->create(ar);
+}
+
+static inline int ath6kl_htc_wait_target(struct htc_target *target)
+{
+	return target->dev->ar->htc_ops->wait_target(target);
+}
+
+static inline int ath6kl_htc_start(struct htc_target *target)
+{
+	return target->dev->ar->htc_ops->start(target);
+}
+
+static inline int ath6kl_htc_conn_service(struct htc_target *target,
+					  struct htc_service_connect_req *req,
+					  struct htc_service_connect_resp *resp)
+{
+	return target->dev->ar->htc_ops->conn_service(target, req, resp);
+}
+
+static inline int ath6kl_htc_tx(struct htc_target *target,
+				struct htc_packet *packet)
+{
+	return target->dev->ar->htc_ops->tx(target, packet);
+}
+
+static inline void ath6kl_htc_stop(struct htc_target *target)
+{
+	return target->dev->ar->htc_ops->stop(target);
+}
+
+static inline void ath6kl_htc_cleanup(struct htc_target *target)
+{
+	return target->dev->ar->htc_ops->cleanup(target);
+}
+
+static inline void ath6kl_htc_flush_txep(struct htc_target *target,
+					 enum htc_endpoint_id endpoint,
+					 u16 tag)
+{
+	return target->dev->ar->htc_ops->flush_txep(target, endpoint, tag);
+}
+
+static inline void ath6kl_htc_flush_rx_buf(struct htc_target *target)
+{
+	return target->dev->ar->htc_ops->flush_rx_buf(target);
+}
+
+static inline void ath6kl_htc_activity_changed(struct htc_target *target,
+					       enum htc_endpoint_id endpoint,
+					       bool active)
+{
+	return target->dev->ar->htc_ops->activity_changed(target, endpoint,
+							  active);
+}
+
+static inline int ath6kl_htc_get_rxbuf_num(struct htc_target *target,
+					   enum htc_endpoint_id endpoint)
+{
+	return target->dev->ar->htc_ops->get_rxbuf_num(target, endpoint);
+}
+
+static inline int ath6kl_htc_add_rxbuf_multiple(struct htc_target *target,
+						struct list_head *pktq)
+{
+	return target->dev->ar->htc_ops->add_rxbuf_multiple(target, pktq);
+}
+
+static inline int ath6kl_htc_credit_setup(struct htc_target *target,
+					  struct ath6kl_htc_credit_info *info)
+{
+	return target->dev->ar->htc_ops->credit_setup(target, info);
+}
+
+static inline void ath6kl_htc_tx_complete(struct ath6kl *ar,
+					  struct sk_buff *skb)
+{
+	ar->htc_ops->tx_complete(ar, skb);
+}
+
+
+static inline void ath6kl_htc_rx_complete(struct ath6kl *ar,
+					  struct sk_buff *skb, u8 pipe)
+{
+	ar->htc_ops->rx_complete(ar, skb, pipe);
+}
+
+
+#endif
diff --git a/drivers/net/wireless/ath/ath6kl/htc.h b/drivers/net/wireless/ath/ath6kl/htc.h
index 5027ccc36b6..a2c8ff80979 100644
--- a/drivers/net/wireless/ath/ath6kl/htc.h
+++ b/drivers/net/wireless/ath/ath6kl/htc.h
@@ -25,6 +25,7 @@
 /* send direction */
 #define HTC_FLAGS_NEED_CREDIT_UPDATE (1 << 0)
 #define HTC_FLAGS_SEND_BUNDLE        (1 << 1)
+#define HTC_FLAGS_TX_FIXUP_NETBUF    (1 << 2)
 
 /* receive direction */
 #define HTC_FLG_RX_UNUSED        (1 << 0)
@@ -56,6 +57,10 @@
 #define HTC_CONN_FLGS_THRESH_LVL_THREE_QUAT	0x2
 #define HTC_CONN_FLGS_REDUCE_CRED_DRIB		0x4
 #define HTC_CONN_FLGS_THRESH_MASK		0x3
+/* disable credit flow control on a specific service */
+#define HTC_CONN_FLGS_DISABLE_CRED_FLOW_CTRL          (1 << 3)
+#define HTC_CONN_FLGS_SET_RECV_ALLOC_SHIFT    8
+#define HTC_CONN_FLGS_SET_RECV_ALLOC_MASK     0xFF00
 
 /* connect response status codes */
 #define HTC_SERVICE_SUCCESS      0
@@ -75,6 +80,7 @@
 #define HTC_RECORD_LOOKAHEAD_BUNDLE 3
 
 #define HTC_SETUP_COMP_FLG_RX_BNDL_EN     (1 << 0)
+#define HTC_SETUP_COMP_FLG_DISABLE_TX_CREDIT_FLOW (1 << 1)
 
 #define MAKE_SERVICE_ID(group, index) \
 	(int)(((int)group << 8) | (int)(index))
@@ -109,6 +115,8 @@
 
 /* HTC operational parameters */
 #define HTC_TARGET_RESPONSE_TIMEOUT        2000	/* in ms */
+#define HTC_TARGET_RESPONSE_POLL_WAIT      10
+#define HTC_TARGET_RESPONSE_POLL_COUNT     200
 #define HTC_TARGET_DEBUG_INTR_MASK         0x01
 #define HTC_TARGET_CREDIT_INTR_MASK        0xF0
 
@@ -128,6 +136,7 @@
 
 #define HTC_RECV_WAIT_BUFFERS        (1 << 0)
 #define HTC_OP_STATE_STOPPING        (1 << 0)
+#define HTC_OP_STATE_SETUP_COMPLETE  (1 << 1)
 
 /*
  * The frame header length and message formats defined herein were selected
@@ -311,6 +320,14 @@ struct htc_packet {
 
 	void (*completion) (struct htc_target *, struct htc_packet *);
 	struct htc_target *context;
+
+	/*
+	 * optimization for network-oriented data, the HTC packet
+	 * can pass the network buffer corresponding to the HTC packet
+	 * lower layers may optimized the transfer knowing this is
+	 * a network buffer
+	 */
+	struct sk_buff *skb;
 };
 
 enum htc_send_full_action {
@@ -319,12 +336,14 @@ enum htc_send_full_action {
 };
 
 struct htc_ep_callbacks {
+	void (*tx_complete) (struct htc_target *, struct htc_packet *);
 	void (*rx) (struct htc_target *, struct htc_packet *);
 	void (*rx_refill) (struct htc_target *, enum htc_endpoint_id endpoint);
 	enum htc_send_full_action (*tx_full) (struct htc_target *,
 					      struct htc_packet *);
 	struct htc_packet *(*rx_allocthresh) (struct htc_target *,
 					      enum htc_endpoint_id, int);
+	void (*tx_comp_multi) (struct htc_target *, struct list_head *);
 	int rx_alloc_thresh;
 	int rx_refill_thresh;
 };
@@ -502,6 +521,13 @@ struct htc_endpoint {
 	u32 conn_flags;
 	struct htc_endpoint_stats ep_st;
 	u16 tx_drop_packet_threshold;
+
+	struct {
+		u8 pipeid_ul;
+		u8 pipeid_dl;
+		struct list_head tx_lookup_queue;
+		bool tx_credit_flow_enabled;
+	} pipe;
 };
 
 struct htc_control_buffer {
@@ -509,6 +535,42 @@ struct htc_control_buffer {
 	u8 *buf;
 };
 
+struct htc_pipe_txcredit_alloc {
+	u16 service_id;
+	u8 credit_alloc;
+};
+
+enum htc_send_queue_result {
+	HTC_SEND_QUEUE_OK = 0,	/* packet was queued */
+	HTC_SEND_QUEUE_DROP = 1,	/* this packet should be dropped */
+};
+
+struct ath6kl_htc_ops {
+	void* (*create)(struct ath6kl *ar);
+	int (*wait_target)(struct htc_target *target);
+	int (*start)(struct htc_target *target);
+	int (*conn_service)(struct htc_target *target,
+			    struct htc_service_connect_req *req,
+			    struct htc_service_connect_resp *resp);
+	int  (*tx)(struct htc_target *target, struct htc_packet *packet);
+	void (*stop)(struct htc_target *target);
+	void (*cleanup)(struct htc_target *target);
+	void (*flush_txep)(struct htc_target *target,
+			   enum htc_endpoint_id endpoint, u16 tag);
+	void (*flush_rx_buf)(struct htc_target *target);
+	void (*activity_changed)(struct htc_target *target,
+				 enum htc_endpoint_id endpoint,
+				 bool active);
+	int (*get_rxbuf_num)(struct htc_target *target,
+			     enum htc_endpoint_id endpoint);
+	int (*add_rxbuf_multiple)(struct htc_target *target,
+				  struct list_head *pktq);
+	int (*credit_setup)(struct htc_target *target,
+			    struct ath6kl_htc_credit_info *cred_info);
+	int (*tx_complete)(struct ath6kl *ar, struct sk_buff *skb);
+	int (*rx_complete)(struct ath6kl *ar, struct sk_buff *skb, u8 pipe);
+};
+
 struct ath6kl_device;
 
 /* our HTC target state */
@@ -557,36 +619,19 @@ struct htc_target {
 
 	/* counts the number of Tx without bundling continously per AC */
 	u32 ac_tx_count[WMM_NUM_AC];
+
+	struct {
+		struct htc_packet *htc_packet_pool;
+		u8 ctrl_response_buf[HTC_MAX_CTRL_MSG_LEN];
+		int ctrl_response_len;
+		bool ctrl_response_valid;
+		struct htc_pipe_txcredit_alloc txcredit_alloc[ENDPOINT_MAX];
+	} pipe;
 };
 
-void *ath6kl_htc_create(struct ath6kl *ar);
-void ath6kl_htc_set_credit_dist(struct htc_target *target,
-				struct ath6kl_htc_credit_info *cred_info,
-				u16 svc_pri_order[], int len);
-int ath6kl_htc_wait_target(struct htc_target *target);
-int ath6kl_htc_start(struct htc_target *target);
-int ath6kl_htc_conn_service(struct htc_target *target,
-			    struct htc_service_connect_req *req,
-			    struct htc_service_connect_resp *resp);
-int ath6kl_htc_tx(struct htc_target *target, struct htc_packet *packet);
-void ath6kl_htc_stop(struct htc_target *target);
-void ath6kl_htc_cleanup(struct htc_target *target);
-void ath6kl_htc_flush_txep(struct htc_target *target,
-			   enum htc_endpoint_id endpoint, u16 tag);
-void ath6kl_htc_flush_rx_buf(struct htc_target *target);
-void ath6kl_htc_indicate_activity_change(struct htc_target *target,
-					 enum htc_endpoint_id endpoint,
-					 bool active);
-int ath6kl_htc_get_rxbuf_num(struct htc_target *target,
-			     enum htc_endpoint_id endpoint);
-int ath6kl_htc_add_rxbuf_multiple(struct htc_target *target,
-				  struct list_head *pktq);
 int ath6kl_htc_rxmsg_pending_handler(struct htc_target *target,
 				     u32 msg_look_ahead, int *n_pkts);
 
-int ath6kl_credit_setup(void *htc_handle,
-			struct ath6kl_htc_credit_info *cred_info);
-
 static inline void set_htc_pkt_info(struct htc_packet *packet, void *context,
 				    u8 *buf, unsigned int len,
 				    enum htc_endpoint_id eid, u16 tag)
@@ -626,4 +671,7 @@ static inline int get_queue_depth(struct list_head *queue)
 	return depth;
 }
 
+void ath6kl_htc_pipe_attach(struct ath6kl *ar);
+void ath6kl_htc_mbox_attach(struct ath6kl *ar);
+
 #endif
diff --git a/drivers/net/wireless/ath/ath6kl/htc.c b/drivers/net/wireless/ath/ath6kl/htc_mbox.c
index 4849d99cce7..2798624d3a9 100644
--- a/drivers/net/wireless/ath/ath6kl/htc.c
+++ b/drivers/net/wireless/ath/ath6kl/htc_mbox.c
@@ -23,6 +23,14 @@
 
 #define CALC_TXRX_PADDED_LEN(dev, len)  (__ALIGN_MASK((len), (dev)->block_mask))
 
+static void ath6kl_htc_mbox_cleanup(struct htc_target *target);
+static void ath6kl_htc_mbox_stop(struct htc_target *target);
+static int ath6kl_htc_mbox_add_rxbuf_multiple(struct htc_target *target,
+					      struct list_head *pkt_queue);
+static void ath6kl_htc_set_credit_dist(struct htc_target *target,
+				       struct ath6kl_htc_credit_info *cred_info,
+				       u16 svc_pri_order[], int len);
+
 /* threshold to re-enable Tx bundling for an AC*/
 #define TX_RESUME_BUNDLE_THRESHOLD	1500
 
@@ -75,10 +83,7 @@ static void ath6kl_credit_init(struct ath6kl_htc_credit_info *cred_info,
 			 * never goes inactive EVER.
 			 */
 			cur_ep_dist->dist_flags |= HTC_EP_ACTIVE;
-		} else if (cur_ep_dist->svc_id == WMI_DATA_BK_SVC)
-			/* this is the lowest priority data endpoint */
-			/* FIXME: this looks fishy, check */
-			cred_info->lowestpri_ep_dist = cur_ep_dist->list;
+		}
 
 		/*
 		 * Streams have to be created (explicit | implicit) for all
@@ -92,6 +97,13 @@ static void ath6kl_credit_init(struct ath6kl_htc_credit_info *cred_info,
 		 */
 	}
 
+	/*
+	 * For ath6kl_credit_seek function,
+	 * it use list_for_each_entry_reverse to walk around the whole ep list.
+	 * Therefore assign this lowestpri_ep_dist after walk around the ep_list
+	 */
+	cred_info->lowestpri_ep_dist = cur_ep_dist->list;
+
 	WARN_ON(cred_info->cur_free_credits <= 0);
 
 	list_for_each_entry(cur_ep_dist, ep_list, list) {
@@ -130,8 +142,8 @@ static void ath6kl_credit_init(struct ath6kl_htc_credit_info *cred_info,
 }
 
 /* initialize and setup credit distribution */
-int ath6kl_credit_setup(void *htc_handle,
-			struct ath6kl_htc_credit_info *cred_info)
+static int ath6kl_htc_mbox_credit_setup(struct htc_target *htc_target,
+			       struct ath6kl_htc_credit_info *cred_info)
 {
 	u16 servicepriority[5];
 
@@ -144,7 +156,7 @@ int ath6kl_credit_setup(void *htc_handle,
 	servicepriority[4] = WMI_DATA_BK_SVC; /* lowest */
 
 	/* set priority list */
-	ath6kl_htc_set_credit_dist(htc_handle, cred_info, servicepriority, 5);
+	ath6kl_htc_set_credit_dist(htc_target, cred_info, servicepriority, 5);
 
 	return 0;
 }
@@ -432,7 +444,7 @@ static void htc_tx_complete(struct htc_endpoint *endpoint,
 		   "htc tx complete ep %d pkts %d\n",
 		   endpoint->eid, get_queue_depth(txq));
 
-	ath6kl_tx_complete(endpoint->target->dev->ar, txq);
+	ath6kl_tx_complete(endpoint->target, txq);
 }
 
 static void htc_tx_comp_handler(struct htc_target *target,
@@ -750,7 +762,7 @@ static void ath6kl_htc_tx_bundle(struct htc_endpoint *endpoint,
 	u32 txb_mask;
 	u8 ac = WMM_NUM_AC;
 
-	if ((HTC_CTRL_RSVD_SVC != endpoint->svc_id) ||
+	if ((HTC_CTRL_RSVD_SVC != endpoint->svc_id) &&
 	    (WMI_CONTROL_SVC != endpoint->svc_id))
 		ac = target->dev->ar->ep2ac_map[endpoint->eid];
 
@@ -785,16 +797,17 @@ static void ath6kl_htc_tx_bundle(struct htc_endpoint *endpoint,
 				 * itself
 				 */
 				txb_mask = ((1 << ac) - 1);
-		/*
-		 * when the scatter request resources drop below a
-		 * certain threshold, disable Tx bundling for all
-		 * AC's with priority lower than the current requesting
-		 * AC. Otherwise re-enable Tx bundling for them
-		 */
-		if (scat_req->scat_q_depth < ATH6KL_SCATTER_REQS)
-			target->tx_bndl_mask &= ~txb_mask;
-		else
-			target->tx_bndl_mask |= txb_mask;
+
+			/*
+			 * when the scatter request resources drop below a
+			 * certain threshold, disable Tx bundling for all
+			 * AC's with priority lower than the current requesting
+			 * AC. Otherwise re-enable Tx bundling for them
+			 */
+			if (scat_req->scat_q_depth < ATH6KL_SCATTER_REQS)
+				target->tx_bndl_mask &= ~txb_mask;
+			else
+				target->tx_bndl_mask |= txb_mask;
 		}
 
 		ath6kl_dbg(ATH6KL_DBG_HTC, "htc tx pkts to scatter: %d\n",
@@ -841,6 +854,7 @@ static void ath6kl_htc_tx_from_queue(struct htc_target *target,
 	int bundle_sent;
 	int n_pkts_bundle;
 	u8 ac = WMM_NUM_AC;
+	int status;
 
 	spin_lock_bh(&target->tx_lock);
 
@@ -858,7 +872,7 @@ static void ath6kl_htc_tx_from_queue(struct htc_target *target,
 	 */
 	INIT_LIST_HEAD(&txq);
 
-	if ((HTC_CTRL_RSVD_SVC != endpoint->svc_id) ||
+	if ((HTC_CTRL_RSVD_SVC != endpoint->svc_id) &&
 	    (WMI_CONTROL_SVC != endpoint->svc_id))
 		ac = target->dev->ar->ep2ac_map[endpoint->eid];
 
@@ -902,7 +916,12 @@ static void ath6kl_htc_tx_from_queue(struct htc_target *target,
 
 			ath6kl_htc_tx_prep_pkt(packet, packet->info.tx.flags,
 					       0, packet->info.tx.seqno);
-			ath6kl_htc_tx_issue(target, packet);
+			status = ath6kl_htc_tx_issue(target, packet);
+
+			if (status) {
+				packet->status = status;
+				packet->completion(packet->context, packet);
+			}
 		}
 
 		spin_lock_bh(&target->tx_lock);
@@ -1065,7 +1084,7 @@ static int htc_setup_tx_complete(struct htc_target *target)
 	return status;
 }
 
-void ath6kl_htc_set_credit_dist(struct htc_target *target,
+static void ath6kl_htc_set_credit_dist(struct htc_target *target,
 				struct ath6kl_htc_credit_info *credit_info,
 				u16 srvc_pri_order[], int list_len)
 {
@@ -1093,7 +1112,8 @@ void ath6kl_htc_set_credit_dist(struct htc_target *target,
 	}
 }
 
-int ath6kl_htc_tx(struct htc_target *target, struct htc_packet *packet)
+static int ath6kl_htc_mbox_tx(struct htc_target *target,
+			      struct htc_packet *packet)
 {
 	struct htc_endpoint *endpoint;
 	struct list_head queue;
@@ -1121,7 +1141,7 @@ int ath6kl_htc_tx(struct htc_target *target, struct htc_packet *packet)
 }
 
 /* flush endpoint TX queue */
-void ath6kl_htc_flush_txep(struct htc_target *target,
+static void ath6kl_htc_mbox_flush_txep(struct htc_target *target,
 			   enum htc_endpoint_id eid, u16 tag)
 {
 	struct htc_packet *packet, *tmp_pkt;
@@ -1173,12 +1193,13 @@ static void ath6kl_htc_flush_txep_all(struct htc_target *target)
 		if (endpoint->svc_id == 0)
 			/* not in use.. */
 			continue;
-		ath6kl_htc_flush_txep(target, i, HTC_TX_PACKET_TAG_ALL);
+		ath6kl_htc_mbox_flush_txep(target, i, HTC_TX_PACKET_TAG_ALL);
 	}
 }
 
-void ath6kl_htc_indicate_activity_change(struct htc_target *target,
-					 enum htc_endpoint_id eid, bool active)
+static void ath6kl_htc_mbox_activity_changed(struct htc_target *target,
+					     enum htc_endpoint_id eid,
+					     bool active)
 {
 	struct htc_endpoint *endpoint = &target->endpoint[eid];
 	bool dist = false;
@@ -1246,7 +1267,7 @@ static int htc_add_rxbuf(struct htc_target *target, struct htc_packet *packet)
 
 	INIT_LIST_HEAD(&queue);
 	list_add_tail(&packet->list, &queue);
-	return ath6kl_htc_add_rxbuf_multiple(target, &queue);
+	return ath6kl_htc_mbox_add_rxbuf_multiple(target, &queue);
 }
 
 static void htc_reclaim_rxbuf(struct htc_target *target,
@@ -1353,7 +1374,9 @@ static int ath6kl_htc_rx_setup(struct htc_target *target,
 					sizeof(*htc_hdr));
 
 	if (!htc_valid_rx_frame_len(target, ep->eid, full_len)) {
-		ath6kl_warn("Rx buffer requested with invalid length\n");
+		ath6kl_warn("Rx buffer requested with invalid length htc_hdr:eid %d, flags 0x%x, len %d\n",
+			    htc_hdr->eid, htc_hdr->flags,
+			    le16_to_cpu(htc_hdr->payld_len));
 		return -EINVAL;
 	}
 
@@ -2288,7 +2311,7 @@ fail_ctrl_rx:
 	return NULL;
 }
 
-int ath6kl_htc_add_rxbuf_multiple(struct htc_target *target,
+static int ath6kl_htc_mbox_add_rxbuf_multiple(struct htc_target *target,
 				  struct list_head *pkt_queue)
 {
 	struct htc_endpoint *endpoint;
@@ -2350,7 +2373,7 @@ int ath6kl_htc_add_rxbuf_multiple(struct htc_target *target,
 	return status;
 }
 
-void ath6kl_htc_flush_rx_buf(struct htc_target *target)
+static void ath6kl_htc_mbox_flush_rx_buf(struct htc_target *target)
 {
 	struct htc_endpoint *endpoint;
 	struct htc_packet *packet, *tmp_pkt;
@@ -2392,7 +2415,7 @@ void ath6kl_htc_flush_rx_buf(struct htc_target *target)
 	}
 }
 
-int ath6kl_htc_conn_service(struct htc_target *target,
+static int ath6kl_htc_mbox_conn_service(struct htc_target *target,
 			    struct htc_service_connect_req *conn_req,
 			    struct htc_service_connect_resp *conn_resp)
 {
@@ -2564,7 +2587,7 @@ static void reset_ep_state(struct htc_target *target)
 	INIT_LIST_HEAD(&target->cred_dist_list);
 }
 
-int ath6kl_htc_get_rxbuf_num(struct htc_target *target,
+static int ath6kl_htc_mbox_get_rxbuf_num(struct htc_target *target,
 			     enum htc_endpoint_id endpoint)
 {
 	int num;
@@ -2624,7 +2647,7 @@ static void htc_setup_msg_bndl(struct htc_target *target)
 	}
 }
 
-int ath6kl_htc_wait_target(struct htc_target *target)
+static int ath6kl_htc_mbox_wait_target(struct htc_target *target)
 {
 	struct htc_packet *packet = NULL;
 	struct htc_ready_ext_msg *rdy_msg;
@@ -2693,12 +2716,12 @@ int ath6kl_htc_wait_target(struct htc_target *target)
 	connect.svc_id = HTC_CTRL_RSVD_SVC;
 
 	/* connect fake service */
-	status = ath6kl_htc_conn_service((void *)target, &connect, &resp);
+	status = ath6kl_htc_mbox_conn_service((void *)target, &connect, &resp);
 
 	if (status)
 		/*
 		 * FIXME: this call doesn't make sense, the caller should
-		 * call ath6kl_htc_cleanup() when it wants remove htc
+		 * call ath6kl_htc_mbox_cleanup() when it wants remove htc
 		 */
 		ath6kl_hif_cleanup_scatter(target->dev->ar);
 
@@ -2715,7 +2738,7 @@ fail_wait_target:
  * Start HTC, enable interrupts and let the target know
  * host has finished setup.
  */
-int ath6kl_htc_start(struct htc_target *target)
+static int ath6kl_htc_mbox_start(struct htc_target *target)
 {
 	struct htc_packet *packet;
 	int status;
@@ -2752,7 +2775,7 @@ int ath6kl_htc_start(struct htc_target *target)
 	status = ath6kl_hif_unmask_intrs(target->dev);
 
 	if (status)
-		ath6kl_htc_stop(target);
+		ath6kl_htc_mbox_stop(target);
 
 	return status;
 }
@@ -2796,7 +2819,7 @@ static int ath6kl_htc_reset(struct htc_target *target)
 }
 
 /* htc_stop: stop interrupt reception, and flush all queued buffers */
-void ath6kl_htc_stop(struct htc_target *target)
+static void ath6kl_htc_mbox_stop(struct htc_target *target)
 {
 	spin_lock_bh(&target->htc_lock);
 	target->htc_flags |= HTC_OP_STATE_STOPPING;
@@ -2811,12 +2834,12 @@ void ath6kl_htc_stop(struct htc_target *target)
 
 	ath6kl_htc_flush_txep_all(target);
 
-	ath6kl_htc_flush_rx_buf(target);
+	ath6kl_htc_mbox_flush_rx_buf(target);
 
 	ath6kl_htc_reset(target);
 }
 
-void *ath6kl_htc_create(struct ath6kl *ar)
+static void *ath6kl_htc_mbox_create(struct ath6kl *ar)
 {
 	struct htc_target *target = NULL;
 	int status = 0;
@@ -2857,13 +2880,13 @@ void *ath6kl_htc_create(struct ath6kl *ar)
 	return target;
 
 err_htc_cleanup:
-	ath6kl_htc_cleanup(target);
+	ath6kl_htc_mbox_cleanup(target);
 
 	return NULL;
 }
 
 /* cleanup the HTC instance */
-void ath6kl_htc_cleanup(struct htc_target *target)
+static void ath6kl_htc_mbox_cleanup(struct htc_target *target)
 {
 	struct htc_packet *packet, *tmp_packet;
 
@@ -2888,3 +2911,24 @@ void ath6kl_htc_cleanup(struct htc_target *target)
 	kfree(target->dev);
 	kfree(target);
 }
+
+static const struct ath6kl_htc_ops ath6kl_htc_mbox_ops = {
+	.create = ath6kl_htc_mbox_create,
+	.wait_target = ath6kl_htc_mbox_wait_target,
+	.start = ath6kl_htc_mbox_start,
+	.conn_service = ath6kl_htc_mbox_conn_service,
+	.tx = ath6kl_htc_mbox_tx,
+	.stop = ath6kl_htc_mbox_stop,
+	.cleanup = ath6kl_htc_mbox_cleanup,
+	.flush_txep = ath6kl_htc_mbox_flush_txep,
+	.flush_rx_buf = ath6kl_htc_mbox_flush_rx_buf,
+	.activity_changed = ath6kl_htc_mbox_activity_changed,
+	.get_rxbuf_num = ath6kl_htc_mbox_get_rxbuf_num,
+	.add_rxbuf_multiple = ath6kl_htc_mbox_add_rxbuf_multiple,
+	.credit_setup = ath6kl_htc_mbox_credit_setup,
+};
+
+void ath6kl_htc_mbox_attach(struct ath6kl *ar)
+{
+	ar->htc_ops = &ath6kl_htc_mbox_ops;
+}
diff --git a/drivers/net/wireless/ath/ath6kl/htc_pipe.c b/drivers/net/wireless/ath/ath6kl/htc_pipe.c
new file mode 100644
index 00000000000..f9626c72369
--- /dev/null
+++ b/drivers/net/wireless/ath/ath6kl/htc_pipe.c
@@ -0,0 +1,1708 @@
+/*
+ * Copyright (c) 2007-2011 Atheros Communications 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 "core.h"
+#include "debug.h"
+#include "hif-ops.h"
+
+#define HTC_PACKET_CONTAINER_ALLOCATION 32
+#define HTC_CONTROL_BUFFER_SIZE (HTC_MAX_CTRL_MSG_LEN + HTC_HDR_LENGTH)
+
+static int ath6kl_htc_pipe_tx(struct htc_target *handle,
+			      struct htc_packet *packet);
+static void ath6kl_htc_pipe_cleanup(struct htc_target *handle);
+
+/* htc pipe tx path */
+static inline void restore_tx_packet(struct htc_packet *packet)
+{
+	if (packet->info.tx.flags & HTC_FLAGS_TX_FIXUP_NETBUF) {
+		skb_pull(packet->skb, sizeof(struct htc_frame_hdr));
+		packet->info.tx.flags &= ~HTC_FLAGS_TX_FIXUP_NETBUF;
+	}
+}
+
+static void do_send_completion(struct htc_endpoint *ep,
+			       struct list_head *queue_to_indicate)
+{
+	struct htc_packet *packet;
+
+	if (list_empty(queue_to_indicate)) {
+		/* nothing to indicate */
+		return;
+	}
+
+	if (ep->ep_cb.tx_comp_multi != NULL) {
+		ath6kl_dbg(ATH6KL_DBG_HTC,
+			   "%s: calling ep %d, send complete multiple callback (%d pkts)\n",
+			   __func__, ep->eid,
+			   get_queue_depth(queue_to_indicate));
+		/*
+		 * a multiple send complete handler is being used,
+		 * pass the queue to the handler
+		 */
+		ep->ep_cb.tx_comp_multi(ep->target, queue_to_indicate);
+		/*
+		 * all packets are now owned by the callback,
+		 * reset queue to be safe
+		 */
+		INIT_LIST_HEAD(queue_to_indicate);
+	} else {
+		/* using legacy EpTxComplete */
+		do {
+			packet = list_first_entry(queue_to_indicate,
+						  struct htc_packet, list);
+
+			list_del(&packet->list);
+			ath6kl_dbg(ATH6KL_DBG_HTC,
+				   "%s: calling ep %d send complete callback on packet 0x%p\n",
+				   __func__, ep->eid, packet);
+			ep->ep_cb.tx_complete(ep->target, packet);
+		} while (!list_empty(queue_to_indicate));
+	}
+}
+
+static void send_packet_completion(struct htc_target *target,
+				   struct htc_packet *packet)
+{
+	struct htc_endpoint *ep = &target->endpoint[packet->endpoint];
+	struct list_head container;
+
+	restore_tx_packet(packet);
+	INIT_LIST_HEAD(&container);
+	list_add_tail(&packet->list, &container);
+
+	/* do completion */
+	do_send_completion(ep, &container);
+}
+
+static void get_htc_packet_credit_based(struct htc_target *target,
+					struct htc_endpoint *ep,
+					struct list_head *queue)
+{
+	int credits_required;
+	int remainder;
+	u8 send_flags;
+	struct htc_packet *packet;
+	unsigned int transfer_len;
+
+	/* NOTE : the TX lock is held when this function is called */
+
+	/* loop until we can grab as many packets out of the queue as we can */
+	while (true) {
+		send_flags = 0;
+		if (list_empty(&ep->txq))
+			break;
+
+		/* get packet at head, but don't remove it */
+		packet = list_first_entry(&ep->txq, struct htc_packet, list);
+
+		ath6kl_dbg(ATH6KL_DBG_HTC,
+			   "%s: got head packet:0x%p , queue depth: %d\n",
+			   __func__, packet, get_queue_depth(&ep->txq));
+
+		transfer_len = packet->act_len + HTC_HDR_LENGTH;
+
+		if (transfer_len <= target->tgt_cred_sz) {
+			credits_required = 1;
+		} else {
+			/* figure out how many credits this message requires */
+			credits_required = transfer_len / target->tgt_cred_sz;
+			remainder = transfer_len % target->tgt_cred_sz;
+
+			if (remainder)
+				credits_required++;
+		}
+
+		ath6kl_dbg(ATH6KL_DBG_HTC, "%s: creds required:%d got:%d\n",
+			   __func__, credits_required, ep->cred_dist.credits);
+
+		if (ep->eid == ENDPOINT_0) {
+			/*
+			 * endpoint 0 is special, it always has a credit and
+			 * does not require credit based flow control
+			 */
+			credits_required = 0;
+
+		} else {
+
+			if (ep->cred_dist.credits < credits_required)
+				break;
+
+			ep->cred_dist.credits -= credits_required;
+			ep->ep_st.cred_cosumd += credits_required;
+
+			/* check if we need credits back from the target */
+			if (ep->cred_dist.credits <
+					ep->cred_dist.cred_per_msg) {
+				/* tell the target we need credits ASAP! */
+				send_flags |= HTC_FLAGS_NEED_CREDIT_UPDATE;
+				ep->ep_st.cred_low_indicate += 1;
+				ath6kl_dbg(ATH6KL_DBG_HTC,
+					   "%s: host needs credits\n",
+					   __func__);
+			}
+		}
+
+		/* now we can fully dequeue */
+		packet = list_first_entry(&ep->txq, struct htc_packet, list);
+
+		list_del(&packet->list);
+		/* save the number of credits this packet consumed */
+		packet->info.tx.cred_used = credits_required;
+		/* save send flags */
+		packet->info.tx.flags = send_flags;
+		packet->info.tx.seqno = ep->seqno;
+		ep->seqno++;
+		/* queue this packet into the caller's queue */
+		list_add_tail(&packet->list, queue);
+	}
+
+}
+
+static void get_htc_packet(struct htc_target *target,
+			   struct htc_endpoint *ep,
+			   struct list_head *queue, int resources)
+{
+	struct htc_packet *packet;
+
+	/* NOTE : the TX lock is held when this function is called */
+
+	/* loop until we can grab as many packets out of the queue as we can */
+	while (resources) {
+		if (list_empty(&ep->txq))
+			break;
+
+		packet = list_first_entry(&ep->txq, struct htc_packet, list);
+		list_del(&packet->list);
+
+		ath6kl_dbg(ATH6KL_DBG_HTC,
+			   "%s: got packet:0x%p , new queue depth: %d\n",
+			   __func__, packet, get_queue_depth(&ep->txq));
+		packet->info.tx.seqno = ep->seqno;
+		packet->info.tx.flags = 0;
+		packet->info.tx.cred_used = 0;
+		ep->seqno++;
+
+		/* queue this packet into the caller's queue */
+		list_add_tail(&packet->list, queue);
+		resources--;
+	}
+}
+
+static int htc_issue_packets(struct htc_target *target,
+			     struct htc_endpoint *ep,
+			     struct list_head *pkt_queue)
+{
+	int status = 0;
+	u16 payload_len;
+	struct sk_buff *skb;
+	struct htc_frame_hdr *htc_hdr;
+	struct htc_packet *packet;
+
+	ath6kl_dbg(ATH6KL_DBG_HTC,
+		   "%s: queue: 0x%p, pkts %d\n", __func__,
+		   pkt_queue, get_queue_depth(pkt_queue));
+
+	while (!list_empty(pkt_queue)) {
+		packet = list_first_entry(pkt_queue, struct htc_packet, list);
+		list_del(&packet->list);
+
+		skb = packet->skb;
+		if (!skb) {
+			WARN_ON_ONCE(1);
+			status = -EINVAL;
+			break;
+		}
+
+		payload_len = packet->act_len;
+
+		/* setup HTC frame header */
+		htc_hdr = (struct htc_frame_hdr *) skb_push(skb,
+							    sizeof(*htc_hdr));
+		if (!htc_hdr) {
+			WARN_ON_ONCE(1);
+			status = -EINVAL;
+			break;
+		}
+
+		packet->info.tx.flags |= HTC_FLAGS_TX_FIXUP_NETBUF;
+
+		/* Endianess? */
+		put_unaligned((u16) payload_len, &htc_hdr->payld_len);
+		htc_hdr->flags = packet->info.tx.flags;
+		htc_hdr->eid = (u8) packet->endpoint;
+		htc_hdr->ctrl[0] = 0;
+		htc_hdr->ctrl[1] = (u8) packet->info.tx.seqno;
+
+		spin_lock_bh(&target->tx_lock);
+
+		/* store in look up queue to match completions */
+		list_add_tail(&packet->list, &ep->pipe.tx_lookup_queue);
+		ep->ep_st.tx_issued += 1;
+		spin_unlock_bh(&target->tx_lock);
+
+		status = ath6kl_hif_pipe_send(target->dev->ar,
+					      ep->pipe.pipeid_ul, NULL, skb);
+
+		if (status != 0) {
+			if (status != -ENOMEM) {
+				/* TODO: if more than 1 endpoint maps to the
+				 * same PipeID, it is possible to run out of
+				 * resources in the HIF layer.
+				 * Don't emit the error
+				 */
+				ath6kl_dbg(ATH6KL_DBG_HTC,
+					   "%s: failed status:%d\n",
+					   __func__, status);
+			}
+			spin_lock_bh(&target->tx_lock);
+			list_del(&packet->list);
+
+			/* reclaim credits */
+			ep->cred_dist.credits += packet->info.tx.cred_used;
+			spin_unlock_bh(&target->tx_lock);
+
+			/* put it back into the callers queue */
+			list_add(&packet->list, pkt_queue);
+			break;
+		}
+
+	}
+
+	if (status != 0) {
+		while (!list_empty(pkt_queue)) {
+			if (status != -ENOMEM) {
+				ath6kl_dbg(ATH6KL_DBG_HTC,
+					   "%s: failed pkt:0x%p status:%d\n",
+					   __func__, packet, status);
+			}
+
+			packet = list_first_entry(pkt_queue,
+						  struct htc_packet, list);
+			list_del(&packet->list);
+			packet->status = status;
+			send_packet_completion(target, packet);
+		}
+	}
+
+	return status;
+}
+
+static enum htc_send_queue_result htc_try_send(struct htc_target *target,
+					       struct htc_endpoint *ep,
+					       struct list_head *txq)
+{
+	struct list_head send_queue;	/* temp queue to hold packets */
+	struct htc_packet *packet, *tmp_pkt;
+	struct ath6kl *ar = target->dev->ar;
+	enum htc_send_full_action action;
+	int tx_resources, overflow, txqueue_depth, i, good_pkts;
+	u8 pipeid;
+
+	ath6kl_dbg(ATH6KL_DBG_HTC, "%s: (queue:0x%p depth:%d)\n",
+		   __func__, txq,
+		   (txq == NULL) ? 0 : get_queue_depth(txq));
+
+	/* init the local send queue */
+	INIT_LIST_HEAD(&send_queue);
+
+	/*
+	 * txq equals to NULL means
+	 * caller didn't provide a queue, just wants us to
+	 * check queues and send
+	 */
+	if (txq != NULL) {
+		if (list_empty(txq)) {
+			/* empty queue */
+			return HTC_SEND_QUEUE_DROP;
+		}
+
+		spin_lock_bh(&target->tx_lock);
+		txqueue_depth = get_queue_depth(&ep->txq);
+		spin_unlock_bh(&target->tx_lock);
+
+		if (txqueue_depth >= ep->max_txq_depth) {
+			/* we've already overflowed */
+			overflow = get_queue_depth(txq);
+		} else {
+			/* get how much we will overflow by */
+			overflow = txqueue_depth;
+			overflow += get_queue_depth(txq);
+			/* get how much we will overflow the TX queue by */
+			overflow -= ep->max_txq_depth;
+		}
+
+		/* if overflow is negative or zero, we are okay */
+		if (overflow > 0) {
+			ath6kl_dbg(ATH6KL_DBG_HTC,
+				   "%s: Endpoint %d, TX queue will overflow :%d, Tx Depth:%d, Max:%d\n",
+				   __func__, ep->eid, overflow, txqueue_depth,
+				   ep->max_txq_depth);
+		}
+		if ((overflow <= 0) ||
+		    (ep->ep_cb.tx_full == NULL)) {
+			/*
+			 * all packets will fit or caller did not provide send
+			 * full indication handler -- just move all of them
+			 * to the local send_queue object
+			 */
+			list_splice_tail_init(txq, &send_queue);
+		} else {
+			good_pkts = get_queue_depth(txq) - overflow;
+			if (good_pkts < 0) {
+				WARN_ON_ONCE(1);
+				return HTC_SEND_QUEUE_DROP;
+			}
+
+			/* we have overflowed, and a callback is provided */
+			/* dequeue all non-overflow packets to the sendqueue */
+			for (i = 0; i < good_pkts; i++) {
+				/* pop off caller's queue */
+				packet = list_first_entry(txq,
+							  struct htc_packet,
+							  list);
+				list_del(&packet->list);
+				/* insert into local queue */
+				list_add_tail(&packet->list, &send_queue);
+			}
+
+			/*
+			 * the caller's queue has all the packets that won't fit
+			 * walk through the caller's queue and indicate each to
+			 * the send full handler
+			 */
+			list_for_each_entry_safe(packet, tmp_pkt,
+						 txq, list) {
+
+				ath6kl_dbg(ATH6KL_DBG_HTC,
+					   "%s: Indicat overflowed TX pkts: %p\n",
+					   __func__, packet);
+				action = ep->ep_cb.tx_full(ep->target, packet);
+				if (action == HTC_SEND_FULL_DROP) {
+					/* callback wants the packet dropped */
+					ep->ep_st.tx_dropped += 1;
+
+					/* leave this one in the caller's queue
+					 * for cleanup */
+				} else {
+					/* callback wants to keep this packet,
+					 * remove from caller's queue */
+					list_del(&packet->list);
+					/* put it in the send queue */
+					list_add_tail(&packet->list,
+						      &send_queue);
+				}
+
+			}
+
+			if (list_empty(&send_queue)) {
+				/* no packets made it in, caller will cleanup */
+				return HTC_SEND_QUEUE_DROP;
+			}
+		}
+	}
+
+	if (!ep->pipe.tx_credit_flow_enabled) {
+		tx_resources =
+		    ath6kl_hif_pipe_get_free_queue_number(ar,
+							  ep->pipe.pipeid_ul);
+	} else {
+		tx_resources = 0;
+	}
+
+	spin_lock_bh(&target->tx_lock);
+	if (!list_empty(&send_queue)) {
+		/* transfer packets to tail */
+		list_splice_tail_init(&send_queue, &ep->txq);
+		if (!list_empty(&send_queue)) {
+			WARN_ON_ONCE(1);
+			spin_unlock_bh(&target->tx_lock);
+			return HTC_SEND_QUEUE_DROP;
+		}
+		INIT_LIST_HEAD(&send_queue);
+	}
+
+	/* increment tx processing count on entry */
+	ep->tx_proc_cnt++;
+
+	if (ep->tx_proc_cnt > 1) {
+		/*
+		 * Another thread or task is draining the TX queues on this
+		 * endpoint that thread will reset the tx processing count
+		 * when the queue is drained.
+		 */
+		ep->tx_proc_cnt--;
+		spin_unlock_bh(&target->tx_lock);
+		return HTC_SEND_QUEUE_OK;
+	}
+
+	/***** beyond this point only 1 thread may enter ******/
+
+	/*
+	 * Now drain the endpoint TX queue for transmission as long as we have
+	 * enough transmit resources.
+	 */
+	while (true) {
+
+		if (get_queue_depth(&ep->txq) == 0)
+			break;
+
+		if (ep->pipe.tx_credit_flow_enabled) {
+			/*
+			 * Credit based mechanism provides flow control
+			 * based on target transmit resource availability,
+			 * we assume that the HIF layer will always have
+			 * bus resources greater than target transmit
+			 * resources.
+			 */
+			get_htc_packet_credit_based(target, ep, &send_queue);
+		} else {
+			/*
+			 * Get all packets for this endpoint that we can
+			 * for this pass.
+			 */
+			get_htc_packet(target, ep, &send_queue, tx_resources);
+		}
+
+		if (get_queue_depth(&send_queue) == 0) {
+			/*
+			 * Didn't get packets due to out of resources or TX
+			 * queue was drained.
+			 */
+			break;
+		}
+
+		spin_unlock_bh(&target->tx_lock);
+
+		/* send what we can */
+		htc_issue_packets(target, ep, &send_queue);
+
+		if (!ep->pipe.tx_credit_flow_enabled) {
+			pipeid = ep->pipe.pipeid_ul;
+			tx_resources =
+			    ath6kl_hif_pipe_get_free_queue_number(ar, pipeid);
+		}
+
+		spin_lock_bh(&target->tx_lock);
+
+	}
+	/* done with this endpoint, we can clear the count */
+	ep->tx_proc_cnt = 0;
+	spin_unlock_bh(&target->tx_lock);
+
+	return HTC_SEND_QUEUE_OK;
+}
+
+/* htc control packet manipulation */
+static void destroy_htc_txctrl_packet(struct htc_packet *packet)
+{
+	struct sk_buff *skb;
+	skb = packet->skb;
+	if (skb != NULL)
+		dev_kfree_skb(skb);
+
+	kfree(packet);
+}
+
+static struct htc_packet *build_htc_txctrl_packet(void)
+{
+	struct htc_packet *packet = NULL;
+	struct sk_buff *skb;
+
+	packet = kzalloc(sizeof(struct htc_packet), GFP_KERNEL);
+	if (packet == NULL)
+		return NULL;
+
+	skb = __dev_alloc_skb(HTC_CONTROL_BUFFER_SIZE, GFP_KERNEL);
+
+	if (skb == NULL) {
+		kfree(packet);
+		return NULL;
+	}
+	packet->skb = skb;
+
+	return packet;
+}
+
+static void htc_free_txctrl_packet(struct htc_target *target,
+				   struct htc_packet *packet)
+{
+	destroy_htc_txctrl_packet(packet);
+}
+
+static struct htc_packet *htc_alloc_txctrl_packet(struct htc_target *target)
+{
+	return build_htc_txctrl_packet();
+}
+
+static void htc_txctrl_complete(struct htc_target *target,
+				struct htc_packet *packet)
+{
+	htc_free_txctrl_packet(target, packet);
+}
+
+#define MAX_MESSAGE_SIZE 1536
+
+static int htc_setup_target_buffer_assignments(struct htc_target *target)
+{
+	int status, credits, credit_per_maxmsg, i;
+	struct htc_pipe_txcredit_alloc *entry;
+	unsigned int hif_usbaudioclass = 0;
+
+	credit_per_maxmsg = MAX_MESSAGE_SIZE / target->tgt_cred_sz;
+	if (MAX_MESSAGE_SIZE % target->tgt_cred_sz)
+		credit_per_maxmsg++;
+
+	/* TODO, this should be configured by the caller! */
+
+	credits = target->tgt_creds;
+	entry = &target->pipe.txcredit_alloc[0];
+
+	status = -ENOMEM;
+
+	/* FIXME: hif_usbaudioclass is always zero */
+	if (hif_usbaudioclass) {
+		ath6kl_dbg(ATH6KL_DBG_HTC,
+			   "%s: For USB Audio Class- Total:%d\n",
+			   __func__, credits);
+		entry++;
+		entry++;
+		/* Setup VO Service To have Max Credits */
+		entry->service_id = WMI_DATA_VO_SVC;
+		entry->credit_alloc = (credits - 6);
+		if (entry->credit_alloc == 0)
+			entry->credit_alloc++;
+
+		credits -= (int) entry->credit_alloc;
+		if (credits <= 0)
+			return status;
+
+		entry++;
+		entry->service_id = WMI_CONTROL_SVC;
+		entry->credit_alloc = credit_per_maxmsg;
+		credits -= (int) entry->credit_alloc;
+		if (credits <= 0)
+			return status;
+
+		/* leftovers go to best effort */
+		entry++;
+		entry++;
+		entry->service_id = WMI_DATA_BE_SVC;
+		entry->credit_alloc = (u8) credits;
+		status = 0;
+	} else {
+		entry++;
+		entry->service_id = WMI_DATA_VI_SVC;
+		entry->credit_alloc = credits / 4;
+		if (entry->credit_alloc == 0)
+			entry->credit_alloc++;
+
+		credits -= (int) entry->credit_alloc;
+		if (credits <= 0)
+			return status;
+
+		entry++;
+		entry->service_id = WMI_DATA_VO_SVC;
+		entry->credit_alloc = credits / 4;
+		if (entry->credit_alloc == 0)
+			entry->credit_alloc++;
+
+		credits -= (int) entry->credit_alloc;
+		if (credits <= 0)
+			return status;
+
+		entry++;
+		entry->service_id = WMI_CONTROL_SVC;
+		entry->credit_alloc = credit_per_maxmsg;
+		credits -= (int) entry->credit_alloc;
+		if (credits <= 0)
+			return status;
+
+		entry++;
+		entry->service_id = WMI_DATA_BK_SVC;
+		entry->credit_alloc = credit_per_maxmsg;
+		credits -= (int) entry->credit_alloc;
+		if (credits <= 0)
+			return status;
+
+		/* leftovers go to best effort */
+		entry++;
+		entry->service_id = WMI_DATA_BE_SVC;
+		entry->credit_alloc = (u8) credits;
+		status = 0;
+	}
+
+	if (status == 0) {
+		for (i = 0; i < ENDPOINT_MAX; i++) {
+			if (target->pipe.txcredit_alloc[i].service_id != 0) {
+				ath6kl_dbg(ATH6KL_DBG_HTC,
+					   "HTC Service Index : %d TX : 0x%2.2X : alloc:%d\n",
+					   i,
+					   target->pipe.txcredit_alloc[i].
+					   service_id,
+					   target->pipe.txcredit_alloc[i].
+					   credit_alloc);
+			}
+		}
+	}
+	return status;
+}
+
+/* process credit reports and call distribution function */
+static void htc_process_credit_report(struct htc_target *target,
+				      struct htc_credit_report *rpt,
+				      int num_entries,
+				      enum htc_endpoint_id from_ep)
+{
+	int total_credits = 0, i;
+	struct htc_endpoint *ep;
+
+	/* lock out TX while we update credits */
+	spin_lock_bh(&target->tx_lock);
+
+	for (i = 0; i < num_entries; i++, rpt++) {
+		if (rpt->eid >= ENDPOINT_MAX) {
+			WARN_ON_ONCE(1);
+			spin_unlock_bh(&target->tx_lock);
+			return;
+		}
+
+		ep = &target->endpoint[rpt->eid];
+		ep->cred_dist.credits += rpt->credits;
+
+		if (ep->cred_dist.credits && get_queue_depth(&ep->txq)) {
+			spin_unlock_bh(&target->tx_lock);
+			htc_try_send(target, ep, NULL);
+			spin_lock_bh(&target->tx_lock);
+		}
+
+		total_credits += rpt->credits;
+	}
+	ath6kl_dbg(ATH6KL_DBG_HTC,
+		   "Report indicated %d credits to distribute\n",
+		   total_credits);
+
+	spin_unlock_bh(&target->tx_lock);
+}
+
+/* flush endpoint TX queue */
+static void htc_flush_tx_endpoint(struct htc_target *target,
+				  struct htc_endpoint *ep, u16 tag)
+{
+	struct htc_packet *packet;
+
+	spin_lock_bh(&target->tx_lock);
+	while (get_queue_depth(&ep->txq)) {
+		packet = list_first_entry(&ep->txq, struct htc_packet, list);
+		list_del(&packet->list);
+		packet->status = 0;
+		send_packet_completion(target, packet);
+	}
+	spin_unlock_bh(&target->tx_lock);
+}
+
+/*
+ * In the adapted HIF layer, struct sk_buff * are passed between HIF and HTC,
+ * since upper layers expects struct htc_packet containers we use the completed
+ * skb and lookup it's corresponding HTC packet buffer from a lookup list.
+ * This is extra overhead that can be fixed by re-aligning HIF interfaces with
+ * HTC.
+ */
+static struct htc_packet *htc_lookup_tx_packet(struct htc_target *target,
+					       struct htc_endpoint *ep,
+					       struct sk_buff *skb)
+{
+	struct htc_packet *packet, *tmp_pkt, *found_packet = NULL;
+
+	spin_lock_bh(&target->tx_lock);
+
+	/*
+	 * interate from the front of tx lookup queue
+	 * this lookup should be fast since lower layers completes in-order and
+	 * so the completed packet should be at the head of the list generally
+	 */
+	list_for_each_entry_safe(packet, tmp_pkt, &ep->pipe.tx_lookup_queue,
+				 list) {
+		/* check for removal */
+		if (skb == packet->skb) {
+			/* found it */
+			list_del(&packet->list);
+			found_packet = packet;
+			break;
+		}
+	}
+
+	spin_unlock_bh(&target->tx_lock);
+
+	return found_packet;
+}
+
+static int ath6kl_htc_pipe_tx_complete(struct ath6kl *ar, struct sk_buff *skb)
+{
+	struct htc_target *target = ar->htc_target;
+	struct htc_frame_hdr *htc_hdr;
+	struct htc_endpoint *ep;
+	struct htc_packet *packet;
+	u8 ep_id, *netdata;
+	u32 netlen;
+
+	netdata = skb->data;
+	netlen = skb->len;
+
+	htc_hdr = (struct htc_frame_hdr *) netdata;
+
+	ep_id = htc_hdr->eid;
+	ep = &target->endpoint[ep_id];
+
+	packet = htc_lookup_tx_packet(target, ep, skb);
+	if (packet == NULL) {
+		/* may have already been flushed and freed */
+		ath6kl_err("HTC TX lookup failed!\n");
+	} else {
+		/* will be giving this buffer back to upper layers */
+		packet->status = 0;
+		send_packet_completion(target, packet);
+	}
+	skb = NULL;
+
+	if (!ep->pipe.tx_credit_flow_enabled) {
+		/*
+		 * note: when using TX credit flow, the re-checking of queues
+		 * happens when credits flow back from the target. in the
+		 * non-TX credit case, we recheck after the packet completes
+		 */
+		htc_try_send(target, ep, NULL);
+	}
+
+	return 0;
+}
+
+static int htc_send_packets_multiple(struct htc_target *target,
+				     struct list_head *pkt_queue)
+{
+	struct htc_endpoint *ep;
+	struct htc_packet *packet, *tmp_pkt;
+
+	if (list_empty(pkt_queue))
+		return -EINVAL;
+
+	/* get first packet to find out which ep the packets will go into */
+	packet = list_first_entry(pkt_queue, struct htc_packet, list);
+
+	if (packet->endpoint >= ENDPOINT_MAX) {
+		WARN_ON_ONCE(1);
+		return -EINVAL;
+	}
+	ep = &target->endpoint[packet->endpoint];
+
+	htc_try_send(target, ep, pkt_queue);
+
+	/* do completion on any packets that couldn't get in */
+	if (!list_empty(pkt_queue)) {
+		list_for_each_entry_safe(packet, tmp_pkt, pkt_queue, list) {
+			packet->status = -ENOMEM;
+		}
+
+		do_send_completion(ep, pkt_queue);
+	}
+
+	return 0;
+}
+
+/* htc pipe rx path */
+static struct htc_packet *alloc_htc_packet_container(struct htc_target *target)
+{
+	struct htc_packet *packet;
+	spin_lock_bh(&target->rx_lock);
+
+	if (target->pipe.htc_packet_pool == NULL) {
+		spin_unlock_bh(&target->rx_lock);
+		return NULL;
+	}
+
+	packet = target->pipe.htc_packet_pool;
+	target->pipe.htc_packet_pool = (struct htc_packet *) packet->list.next;
+
+	spin_unlock_bh(&target->rx_lock);
+
+	packet->list.next = NULL;
+	return packet;
+}
+
+static void free_htc_packet_container(struct htc_target *target,
+				      struct htc_packet *packet)
+{
+	struct list_head *lh;
+
+	spin_lock_bh(&target->rx_lock);
+
+	if (target->pipe.htc_packet_pool == NULL) {
+		target->pipe.htc_packet_pool = packet;
+		packet->list.next = NULL;
+	} else {
+		lh = (struct list_head *) target->pipe.htc_packet_pool;
+		packet->list.next = lh;
+		target->pipe.htc_packet_pool = packet;
+	}
+
+	spin_unlock_bh(&target->rx_lock);
+}
+
+static int htc_process_trailer(struct htc_target *target, u8 *buffer,
+			       int len, enum htc_endpoint_id from_ep)
+{
+	struct htc_credit_report *report;
+	struct htc_record_hdr *record;
+	u8 *record_buf, *orig_buf;
+	int orig_len, status;
+
+	orig_buf = buffer;
+	orig_len = len;
+	status = 0;
+
+	while (len > 0) {
+		if (len < sizeof(struct htc_record_hdr)) {
+			status = -EINVAL;
+			break;
+		}
+
+		/* these are byte aligned structs */
+		record = (struct htc_record_hdr *) buffer;
+		len -= sizeof(struct htc_record_hdr);
+		buffer += sizeof(struct htc_record_hdr);
+
+		if (record->len > len) {
+			/* no room left in buffer for record */
+			ath6kl_dbg(ATH6KL_DBG_HTC,
+				   "invalid length: %d (id:%d) buffer has: %d bytes left\n",
+				   record->len, record->rec_id, len);
+			status = -EINVAL;
+			break;
+		}
+
+		/* start of record follows the header */
+		record_buf = buffer;
+
+		switch (record->rec_id) {
+		case HTC_RECORD_CREDITS:
+			if (record->len < sizeof(struct htc_credit_report)) {
+				WARN_ON_ONCE(1);
+				return -EINVAL;
+			}
+
+			report = (struct htc_credit_report *) record_buf;
+			htc_process_credit_report(target, report,
+						  record->len / sizeof(*report),
+						  from_ep);
+			break;
+		default:
+			ath6kl_dbg(ATH6KL_DBG_HTC,
+				   "unhandled record: id:%d length:%d\n",
+				   record->rec_id, record->len);
+			break;
+		}
+
+		if (status != 0)
+			break;
+
+		/* advance buffer past this record for next time around */
+		buffer += record->len;
+		len -= record->len;
+	}
+
+	return status;
+}
+
+static void do_recv_completion(struct htc_endpoint *ep,
+			       struct list_head *queue_to_indicate)
+{
+	struct htc_packet *packet;
+
+	if (list_empty(queue_to_indicate)) {
+		/* nothing to indicate */
+		return;
+	}
+
+	/* using legacy EpRecv */
+	while (!list_empty(queue_to_indicate)) {
+		packet = list_first_entry(queue_to_indicate,
+					  struct htc_packet, list);
+		list_del(&packet->list);
+		ep->ep_cb.rx(ep->target, packet);
+	}
+
+	return;
+}
+
+static void recv_packet_completion(struct htc_target *target,
+				   struct htc_endpoint *ep,
+				   struct htc_packet *packet)
+{
+	struct list_head container;
+	INIT_LIST_HEAD(&container);
+	list_add_tail(&packet->list, &container);
+
+	/* do completion */
+	do_recv_completion(ep, &container);
+}
+
+static int ath6kl_htc_pipe_rx_complete(struct ath6kl *ar, struct sk_buff *skb,
+				       u8 pipeid)
+{
+	struct htc_target *target = ar->htc_target;
+	u8 *netdata, *trailer, hdr_info;
+	struct htc_frame_hdr *htc_hdr;
+	u32 netlen, trailerlen = 0;
+	struct htc_packet *packet;
+	struct htc_endpoint *ep;
+	u16 payload_len;
+	int status = 0;
+
+	netdata = skb->data;
+	netlen = skb->len;
+
+	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",
+			   htc_hdr->eid);
+		status = -EINVAL;
+		goto free_skb;
+	}
+
+	payload_len = le16_to_cpu(get_unaligned(&htc_hdr->payld_len));
+
+	if (netlen < (payload_len + HTC_HDR_LENGTH)) {
+		ath6kl_dbg(ATH6KL_DBG_HTC,
+			   "HTC Rx: insufficient length, got:%d expected =%u\n",
+			   netlen, payload_len + HTC_HDR_LENGTH);
+		status = -EINVAL;
+		goto free_skb;
+	}
+
+	/* get flags to check for trailer */
+	hdr_info = htc_hdr->flags;
+	if (hdr_info & HTC_FLG_RX_TRAILER) {
+		/* extract the trailer length */
+		hdr_info = htc_hdr->ctrl[0];
+		if ((hdr_info < sizeof(struct htc_record_hdr)) ||
+		    (hdr_info > payload_len)) {
+			ath6kl_dbg(ATH6KL_DBG_HTC,
+				   "invalid header: payloadlen should be %d, CB[0]: %d\n",
+				   payload_len, hdr_info);
+			status = -EINVAL;
+			goto free_skb;
+		}
+
+		trailerlen = hdr_info;
+		/* process trailer after hdr/apps payload */
+		trailer = (u8 *) htc_hdr + HTC_HDR_LENGTH +
+			payload_len - hdr_info;
+		status = htc_process_trailer(target, trailer, hdr_info,
+					     htc_hdr->eid);
+		if (status != 0)
+			goto free_skb;
+	}
+
+	if (((int) payload_len - (int) trailerlen) <= 0) {
+		/* zero length packet with trailer, just drop these */
+		goto free_skb;
+	}
+
+	if (htc_hdr->eid == ENDPOINT_0) {
+		/* handle HTC control message */
+		if (target->htc_flags & HTC_OP_STATE_SETUP_COMPLETE) {
+			/*
+			 * fatal: target should not send unsolicited
+			 * messageson the endpoint 0
+			 */
+			ath6kl_dbg(ATH6KL_DBG_HTC,
+				   "HTC ignores Rx Ctrl after setup complete\n");
+			status = -EINVAL;
+			goto free_skb;
+		}
+
+		/* remove HTC header */
+		skb_pull(skb, HTC_HDR_LENGTH);
+
+		netdata = skb->data;
+		netlen = skb->len;
+
+		spin_lock_bh(&target->rx_lock);
+
+		target->pipe.ctrl_response_valid = true;
+		target->pipe.ctrl_response_len = min_t(int, netlen,
+						       HTC_MAX_CTRL_MSG_LEN);
+		memcpy(target->pipe.ctrl_response_buf, netdata,
+		       target->pipe.ctrl_response_len);
+
+		spin_unlock_bh(&target->rx_lock);
+
+		dev_kfree_skb(skb);
+		skb = NULL;
+		goto free_skb;
+	}
+
+	/*
+	 * TODO: the message based HIF architecture allocates net bufs
+	 * for recv packets since it bridges that HIF to upper layers,
+	 * which expects HTC packets, we form the packets here
+	 */
+	packet = alloc_htc_packet_container(target);
+	if (packet == NULL) {
+		status = -ENOMEM;
+		goto free_skb;
+	}
+
+	packet->status = 0;
+	packet->endpoint = htc_hdr->eid;
+	packet->pkt_cntxt = skb;
+
+	/* TODO: for backwards compatibility */
+	packet->buf = skb_push(skb, 0) + HTC_HDR_LENGTH;
+	packet->act_len = netlen - HTC_HDR_LENGTH - trailerlen;
+
+	/*
+	 * TODO: this is a hack because the driver layer will set the
+	 * actual len of the skb again which will just double the len
+	 */
+	skb_trim(skb, 0);
+
+	recv_packet_completion(target, ep, packet);
+
+	/* recover the packet container */
+	free_htc_packet_container(target, packet);
+	skb = NULL;
+
+free_skb:
+	if (skb != NULL)
+		dev_kfree_skb(skb);
+
+	return status;
+
+}
+
+static void htc_flush_rx_queue(struct htc_target *target,
+			       struct htc_endpoint *ep)
+{
+	struct list_head container;
+	struct htc_packet *packet;
+
+	spin_lock_bh(&target->rx_lock);
+
+	while (1) {
+		if (list_empty(&ep->rx_bufq))
+			break;
+
+		packet = list_first_entry(&ep->rx_bufq,
+					  struct htc_packet, list);
+		list_del(&packet->list);
+
+		spin_unlock_bh(&target->rx_lock);
+		packet->status = -ECANCELED;
+		packet->act_len = 0;
+
+		ath6kl_dbg(ATH6KL_DBG_HTC,
+			   "Flushing RX packet:0x%p, length:%d, ep:%d\n",
+			   packet, packet->buf_len,
+			   packet->endpoint);
+
+		INIT_LIST_HEAD(&container);
+		list_add_tail(&packet->list, &container);
+
+		/* give the packet back */
+		do_recv_completion(ep, &container);
+		spin_lock_bh(&target->rx_lock);
+	}
+
+	spin_unlock_bh(&target->rx_lock);
+}
+
+/* polling routine to wait for a control packet to be received */
+static int htc_wait_recv_ctrl_message(struct htc_target *target)
+{
+	int count = HTC_TARGET_RESPONSE_POLL_COUNT;
+
+	while (count > 0) {
+		spin_lock_bh(&target->rx_lock);
+
+		if (target->pipe.ctrl_response_valid) {
+			target->pipe.ctrl_response_valid = false;
+			spin_unlock_bh(&target->rx_lock);
+			break;
+		}
+
+		spin_unlock_bh(&target->rx_lock);
+
+		count--;
+
+		msleep_interruptible(HTC_TARGET_RESPONSE_POLL_WAIT);
+	}
+
+	if (count <= 0) {
+		ath6kl_dbg(ATH6KL_DBG_HTC, "%s: Timeout!\n", __func__);
+		return -ECOMM;
+	}
+
+	return 0;
+}
+
+static void htc_rxctrl_complete(struct htc_target *context,
+				struct htc_packet *packet)
+{
+	/* TODO, can't really receive HTC control messages yet.... */
+	ath6kl_dbg(ATH6KL_DBG_HTC, "%s: invalid call function\n", __func__);
+}
+
+/* htc pipe initialization */
+static void reset_endpoint_states(struct htc_target *target)
+{
+	struct htc_endpoint *ep;
+	int i;
+
+	for (i = ENDPOINT_0; i < ENDPOINT_MAX; i++) {
+		ep = &target->endpoint[i];
+		ep->svc_id = 0;
+		ep->len_max = 0;
+		ep->max_txq_depth = 0;
+		ep->eid = i;
+		INIT_LIST_HEAD(&ep->txq);
+		INIT_LIST_HEAD(&ep->pipe.tx_lookup_queue);
+		INIT_LIST_HEAD(&ep->rx_bufq);
+		ep->target = target;
+		ep->pipe.tx_credit_flow_enabled = (bool) 1; /* FIXME */
+	}
+}
+
+/* start HTC, this is called after all services are connected */
+static int htc_config_target_hif_pipe(struct htc_target *target)
+{
+	return 0;
+}
+
+/* htc service functions */
+static u8 htc_get_credit_alloc(struct htc_target *target, u16 service_id)
+{
+	u8 allocation = 0;
+	int i;
+
+	for (i = 0; i < ENDPOINT_MAX; i++) {
+		if (target->pipe.txcredit_alloc[i].service_id == service_id)
+			allocation =
+				target->pipe.txcredit_alloc[i].credit_alloc;
+	}
+
+	if (allocation == 0) {
+		ath6kl_dbg(ATH6KL_DBG_HTC,
+			   "HTC Service TX : 0x%2.2X : allocation is zero!\n",
+			   service_id);
+	}
+
+	return allocation;
+}
+
+static int ath6kl_htc_pipe_conn_service(struct htc_target *target,
+		     struct htc_service_connect_req *conn_req,
+		     struct htc_service_connect_resp *conn_resp)
+{
+	struct ath6kl *ar = target->dev->ar;
+	struct htc_packet *packet = NULL;
+	struct htc_conn_service_resp *resp_msg;
+	struct htc_conn_service_msg *conn_msg;
+	enum htc_endpoint_id assigned_epid = ENDPOINT_MAX;
+	bool disable_credit_flowctrl = false;
+	unsigned int max_msg_size = 0;
+	struct htc_endpoint *ep;
+	int length, status = 0;
+	struct sk_buff *skb;
+	u8 tx_alloc;
+	u16 flags;
+
+	if (conn_req->svc_id == 0) {
+		WARN_ON_ONCE(1);
+		status = -EINVAL;
+		goto free_packet;
+	}
+
+	if (conn_req->svc_id == HTC_CTRL_RSVD_SVC) {
+		/* special case for pseudo control service */
+		assigned_epid = ENDPOINT_0;
+		max_msg_size = HTC_MAX_CTRL_MSG_LEN;
+		tx_alloc = 0;
+
+	} else {
+
+		tx_alloc = htc_get_credit_alloc(target, conn_req->svc_id);
+		if (tx_alloc == 0) {
+			status = -ENOMEM;
+			goto free_packet;
+		}
+
+		/* allocate a packet to send to the target */
+		packet = htc_alloc_txctrl_packet(target);
+
+		if (packet == NULL) {
+			WARN_ON_ONCE(1);
+			status = -ENOMEM;
+			goto free_packet;
+		}
+
+		skb = packet->skb;
+		length = sizeof(struct htc_conn_service_msg);
+
+		/* assemble connect service message */
+		conn_msg = (struct htc_conn_service_msg *) skb_put(skb,
+								   length);
+		if (conn_msg == NULL) {
+			WARN_ON_ONCE(1);
+			status = -EINVAL;
+			goto free_packet;
+		}
+
+		memset(conn_msg, 0,
+		       sizeof(struct htc_conn_service_msg));
+		conn_msg->msg_id = cpu_to_le16(HTC_MSG_CONN_SVC_ID);
+		conn_msg->svc_id = cpu_to_le16(conn_req->svc_id);
+		conn_msg->conn_flags = cpu_to_le16(conn_req->conn_flags &
+					~HTC_CONN_FLGS_SET_RECV_ALLOC_MASK);
+
+		/* tell target desired recv alloc for this ep */
+		flags = tx_alloc << HTC_CONN_FLGS_SET_RECV_ALLOC_SHIFT;
+		conn_msg->conn_flags |= cpu_to_le16(flags);
+
+		if (conn_req->conn_flags &
+		    HTC_CONN_FLGS_DISABLE_CRED_FLOW_CTRL) {
+			disable_credit_flowctrl = true;
+		}
+
+		set_htc_pkt_info(packet, NULL, (u8 *) conn_msg,
+				 length,
+				 ENDPOINT_0, HTC_SERVICE_TX_PACKET_TAG);
+
+		status = ath6kl_htc_pipe_tx(target, packet);
+
+		/* we don't own it anymore */
+		packet = NULL;
+		if (status != 0)
+			goto free_packet;
+
+		/* wait for response */
+		status = htc_wait_recv_ctrl_message(target);
+		if (status != 0)
+			goto free_packet;
+
+		/* we controlled the buffer creation so it has to be
+		 * properly aligned
+		 */
+		resp_msg = (struct htc_conn_service_resp *)
+		    target->pipe.ctrl_response_buf;
+
+		if (resp_msg->msg_id != cpu_to_le16(HTC_MSG_CONN_SVC_RESP_ID) ||
+		    (target->pipe.ctrl_response_len < sizeof(*resp_msg))) {
+			/* this message is not valid */
+			WARN_ON_ONCE(1);
+			status = -EINVAL;
+			goto free_packet;
+		}
+
+		ath6kl_dbg(ATH6KL_DBG_TRC,
+			   "%s: service 0x%X conn resp: status: %d ep: %d\n",
+			   __func__, resp_msg->svc_id, resp_msg->status,
+			   resp_msg->eid);
+
+		conn_resp->resp_code = resp_msg->status;
+		/* check response status */
+		if (resp_msg->status != HTC_SERVICE_SUCCESS) {
+			ath6kl_dbg(ATH6KL_DBG_HTC,
+				   "Target failed service 0x%X connect request (status:%d)\n",
+				   resp_msg->svc_id, resp_msg->status);
+			status = -EINVAL;
+			goto free_packet;
+		}
+
+		assigned_epid = (enum htc_endpoint_id) resp_msg->eid;
+		max_msg_size = le16_to_cpu(resp_msg->max_msg_sz);
+	}
+
+	/* the rest are parameter checks so set the error status */
+	status = -EINVAL;
+
+	if (assigned_epid >= ENDPOINT_MAX) {
+		WARN_ON_ONCE(1);
+		goto free_packet;
+	}
+
+	if (max_msg_size == 0) {
+		WARN_ON_ONCE(1);
+		goto free_packet;
+	}
+
+	ep = &target->endpoint[assigned_epid];
+	ep->eid = assigned_epid;
+	if (ep->svc_id != 0) {
+		/* endpoint already in use! */
+		WARN_ON_ONCE(1);
+		goto free_packet;
+	}
+
+	/* return assigned endpoint to caller */
+	conn_resp->endpoint = assigned_epid;
+	conn_resp->len_max = max_msg_size;
+
+	/* setup the endpoint */
+	ep->svc_id = conn_req->svc_id; /* this marks ep in use */
+	ep->max_txq_depth = conn_req->max_txq_depth;
+	ep->len_max = max_msg_size;
+	ep->cred_dist.credits = tx_alloc;
+	ep->cred_dist.cred_sz = target->tgt_cred_sz;
+	ep->cred_dist.cred_per_msg = max_msg_size / target->tgt_cred_sz;
+	if (max_msg_size % target->tgt_cred_sz)
+		ep->cred_dist.cred_per_msg++;
+
+	/* copy all the callbacks */
+	ep->ep_cb = conn_req->ep_cb;
+
+	/* initialize tx_drop_packet_threshold */
+	ep->tx_drop_packet_threshold = MAX_HI_COOKIE_NUM;
+
+	status = ath6kl_hif_pipe_map_service(ar, ep->svc_id,
+					     &ep->pipe.pipeid_ul,
+					     &ep->pipe.pipeid_dl);
+	if (status != 0)
+		goto free_packet;
+
+	ath6kl_dbg(ATH6KL_DBG_HTC,
+		   "SVC Ready: 0x%4.4X: ULpipe:%d DLpipe:%d id:%d\n",
+		   ep->svc_id, ep->pipe.pipeid_ul,
+		   ep->pipe.pipeid_dl, ep->eid);
+
+	if (disable_credit_flowctrl && ep->pipe.tx_credit_flow_enabled) {
+		ep->pipe.tx_credit_flow_enabled = false;
+		ath6kl_dbg(ATH6KL_DBG_HTC,
+			   "SVC: 0x%4.4X ep:%d TX flow control off\n",
+			   ep->svc_id, assigned_epid);
+	}
+
+free_packet:
+	if (packet != NULL)
+		htc_free_txctrl_packet(target, packet);
+	return status;
+}
+
+/* htc export functions */
+static void *ath6kl_htc_pipe_create(struct ath6kl *ar)
+{
+	int status = 0;
+	struct htc_endpoint *ep = NULL;
+	struct htc_target *target = NULL;
+	struct htc_packet *packet;
+	int i;
+
+	target = kzalloc(sizeof(struct htc_target), GFP_KERNEL);
+	if (target == NULL) {
+		ath6kl_err("htc create unable to allocate memory\n");
+		status = -ENOMEM;
+		goto fail_htc_create;
+	}
+
+	spin_lock_init(&target->htc_lock);
+	spin_lock_init(&target->rx_lock);
+	spin_lock_init(&target->tx_lock);
+
+	reset_endpoint_states(target);
+
+	for (i = 0; i < HTC_PACKET_CONTAINER_ALLOCATION; i++) {
+		packet = kzalloc(sizeof(struct htc_packet), GFP_KERNEL);
+
+		if (packet != NULL)
+			free_htc_packet_container(target, packet);
+	}
+
+	target->dev = kzalloc(sizeof(*target->dev), GFP_KERNEL);
+	if (!target->dev) {
+		ath6kl_err("unable to allocate memory\n");
+		status = -ENOMEM;
+		goto fail_htc_create;
+	}
+	target->dev->ar = ar;
+	target->dev->htc_cnxt = target;
+
+	/* Get HIF default pipe for HTC message exchange */
+	ep = &target->endpoint[ENDPOINT_0];
+
+	ath6kl_hif_pipe_get_default(ar, &ep->pipe.pipeid_ul,
+				    &ep->pipe.pipeid_dl);
+
+	return target;
+
+fail_htc_create:
+	if (status != 0) {
+		if (target != NULL)
+			ath6kl_htc_pipe_cleanup(target);
+
+		target = NULL;
+	}
+	return target;
+}
+
+/* cleanup the HTC instance */
+static void ath6kl_htc_pipe_cleanup(struct htc_target *target)
+{
+	struct htc_packet *packet;
+
+	while (true) {
+		packet = alloc_htc_packet_container(target);
+		if (packet == NULL)
+			break;
+		kfree(packet);
+	}
+
+	kfree(target->dev);
+
+	/* kfree our instance */
+	kfree(target);
+}
+
+static int ath6kl_htc_pipe_start(struct htc_target *target)
+{
+	struct sk_buff *skb;
+	struct htc_setup_comp_ext_msg *setup;
+	struct htc_packet *packet;
+
+	htc_config_target_hif_pipe(target);
+
+	/* allocate a buffer to send */
+	packet = htc_alloc_txctrl_packet(target);
+	if (packet == NULL) {
+		WARN_ON_ONCE(1);
+		return -ENOMEM;
+	}
+
+	skb = packet->skb;
+
+	/* assemble setup complete message */
+	setup = (struct htc_setup_comp_ext_msg *) skb_put(skb,
+							  sizeof(*setup));
+	memset(setup, 0, sizeof(struct htc_setup_comp_ext_msg));
+	setup->msg_id = cpu_to_le16(HTC_MSG_SETUP_COMPLETE_EX_ID);
+
+	ath6kl_dbg(ATH6KL_DBG_HTC, "HTC using TX credit flow control\n");
+
+	set_htc_pkt_info(packet, NULL, (u8 *) setup,
+			 sizeof(struct htc_setup_comp_ext_msg),
+			 ENDPOINT_0, HTC_SERVICE_TX_PACKET_TAG);
+
+	target->htc_flags |= HTC_OP_STATE_SETUP_COMPLETE;
+
+	return ath6kl_htc_pipe_tx(target, packet);
+}
+
+static void ath6kl_htc_pipe_stop(struct htc_target *target)
+{
+	int i;
+	struct htc_endpoint *ep;
+
+	/* cleanup endpoints */
+	for (i = 0; i < ENDPOINT_MAX; i++) {
+		ep = &target->endpoint[i];
+		htc_flush_rx_queue(target, ep);
+		htc_flush_tx_endpoint(target, ep, HTC_TX_PACKET_TAG_ALL);
+	}
+
+	reset_endpoint_states(target);
+	target->htc_flags &= ~HTC_OP_STATE_SETUP_COMPLETE;
+}
+
+static int ath6kl_htc_pipe_get_rxbuf_num(struct htc_target *target,
+					 enum htc_endpoint_id endpoint)
+{
+	int num;
+
+	spin_lock_bh(&target->rx_lock);
+	num = get_queue_depth(&(target->endpoint[endpoint].rx_bufq));
+	spin_unlock_bh(&target->rx_lock);
+
+	return num;
+}
+
+static int ath6kl_htc_pipe_tx(struct htc_target *target,
+			      struct htc_packet *packet)
+{
+	struct list_head queue;
+
+	ath6kl_dbg(ATH6KL_DBG_HTC,
+		   "%s: endPointId: %d, buffer: 0x%p, length: %d\n",
+		   __func__, packet->endpoint, packet->buf,
+		   packet->act_len);
+
+	INIT_LIST_HEAD(&queue);
+	list_add_tail(&packet->list, &queue);
+
+	return htc_send_packets_multiple(target, &queue);
+}
+
+static int ath6kl_htc_pipe_wait_target(struct htc_target *target)
+{
+	struct htc_ready_ext_msg *ready_msg;
+	struct htc_service_connect_req connect;
+	struct htc_service_connect_resp resp;
+	int status = 0;
+
+	status = htc_wait_recv_ctrl_message(target);
+
+	if (status != 0)
+		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);
+		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);
+		return -ECOMM;
+	}
+
+	ath6kl_dbg(ATH6KL_DBG_HTC,
+		   "Target Ready! : transmit resources : %d size:%d\n",
+		   ready_msg->ver2_0_info.cred_cnt,
+		   ready_msg->ver2_0_info.cred_sz);
+
+	target->tgt_creds = le16_to_cpu(ready_msg->ver2_0_info.cred_cnt);
+	target->tgt_cred_sz = le16_to_cpu(ready_msg->ver2_0_info.cred_sz);
+
+	if ((target->tgt_creds == 0) || (target->tgt_cred_sz == 0))
+		return -ECOMM;
+
+	htc_setup_target_buffer_assignments(target);
+
+	/* setup our pseudo HTC control endpoint connection */
+	memset(&connect, 0, sizeof(connect));
+	memset(&resp, 0, sizeof(resp));
+	connect.ep_cb.tx_complete = htc_txctrl_complete;
+	connect.ep_cb.rx = htc_rxctrl_complete;
+	connect.max_txq_depth = NUM_CONTROL_TX_BUFFERS;
+	connect.svc_id = HTC_CTRL_RSVD_SVC;
+
+	/* connect fake service */
+	status = ath6kl_htc_pipe_conn_service(target, &connect, &resp);
+
+	return status;
+}
+
+static void ath6kl_htc_pipe_flush_txep(struct htc_target *target,
+				       enum htc_endpoint_id endpoint, u16 tag)
+{
+	struct htc_endpoint *ep = &target->endpoint[endpoint];
+
+	if (ep->svc_id == 0) {
+		WARN_ON_ONCE(1);
+		/* not in use.. */
+		return;
+	}
+
+	htc_flush_tx_endpoint(target, ep, tag);
+}
+
+static int ath6kl_htc_pipe_add_rxbuf_multiple(struct htc_target *target,
+					      struct list_head *pkt_queue)
+{
+	struct htc_packet *packet, *tmp_pkt, *first;
+	struct htc_endpoint *ep;
+	int status = 0;
+
+	if (list_empty(pkt_queue))
+		return -EINVAL;
+
+	first = list_first_entry(pkt_queue, struct htc_packet, list);
+
+	if (first->endpoint >= ENDPOINT_MAX) {
+		WARN_ON_ONCE(1);
+		return -EINVAL;
+	}
+
+	ath6kl_dbg(ATH6KL_DBG_HTC, "%s: epid: %d, cnt:%d, len: %d\n",
+		   __func__, first->endpoint, get_queue_depth(pkt_queue),
+		   first->buf_len);
+
+	ep = &target->endpoint[first->endpoint];
+
+	spin_lock_bh(&target->rx_lock);
+
+	/* store receive packets */
+	list_splice_tail_init(pkt_queue, &ep->rx_bufq);
+
+	spin_unlock_bh(&target->rx_lock);
+
+	if (status != 0) {
+		/* walk through queue and mark each one canceled */
+		list_for_each_entry_safe(packet, tmp_pkt, pkt_queue, list) {
+			packet->status = -ECANCELED;
+		}
+
+		do_recv_completion(ep, pkt_queue);
+	}
+
+	return status;
+}
+
+static void ath6kl_htc_pipe_activity_changed(struct htc_target *target,
+					     enum htc_endpoint_id ep,
+					     bool active)
+{
+	/* TODO */
+}
+
+static void ath6kl_htc_pipe_flush_rx_buf(struct htc_target *target)
+{
+	/* TODO */
+}
+
+static int ath6kl_htc_pipe_credit_setup(struct htc_target *target,
+					struct ath6kl_htc_credit_info *info)
+{
+	return 0;
+}
+
+static const struct ath6kl_htc_ops ath6kl_htc_pipe_ops = {
+	.create = ath6kl_htc_pipe_create,
+	.wait_target = ath6kl_htc_pipe_wait_target,
+	.start = ath6kl_htc_pipe_start,
+	.conn_service = ath6kl_htc_pipe_conn_service,
+	.tx = ath6kl_htc_pipe_tx,
+	.stop = ath6kl_htc_pipe_stop,
+	.cleanup = ath6kl_htc_pipe_cleanup,
+	.flush_txep = ath6kl_htc_pipe_flush_txep,
+	.flush_rx_buf = ath6kl_htc_pipe_flush_rx_buf,
+	.activity_changed = ath6kl_htc_pipe_activity_changed,
+	.get_rxbuf_num = ath6kl_htc_pipe_get_rxbuf_num,
+	.add_rxbuf_multiple = ath6kl_htc_pipe_add_rxbuf_multiple,
+	.credit_setup = ath6kl_htc_pipe_credit_setup,
+	.tx_complete = ath6kl_htc_pipe_tx_complete,
+	.rx_complete = ath6kl_htc_pipe_rx_complete,
+};
+
+void ath6kl_htc_pipe_attach(struct ath6kl *ar)
+{
+	ar->htc_ops = &ath6kl_htc_pipe_ops;
+}
diff --git a/drivers/net/wireless/ath/ath6kl/init.c b/drivers/net/wireless/ath/ath6kl/init.c
index 03cae142f17..7eb0515f458 100644
--- a/drivers/net/wireless/ath/ath6kl/init.c
+++ b/drivers/net/wireless/ath/ath6kl/init.c
@@ -16,17 +16,21 @@
  * OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
  */
 
+#define pr_fmt(fmt) KBUILD_MODNAME ": " fmt
+
 #include <linux/moduleparam.h>
 #include <linux/errno.h>
 #include <linux/export.h>
 #include <linux/of.h>
 #include <linux/mmc/sdio_func.h>
+#include <linux/vmalloc.h>
 
 #include "core.h"
 #include "cfg80211.h"
 #include "target.h"
 #include "debug.h"
 #include "hif-ops.h"
+#include "htc-ops.h"
 
 static const struct ath6kl_hw hw_list[] = {
 	{
@@ -115,6 +119,24 @@ static const struct ath6kl_hw hw_list[] = {
 		.fw_board		= AR6004_HW_1_1_BOARD_DATA_FILE,
 		.fw_default_board	= AR6004_HW_1_1_DEFAULT_BOARD_DATA_FILE,
 	},
+	{
+		.id				= AR6004_HW_1_2_VERSION,
+		.name				= "ar6004 hw 1.2",
+		.dataset_patch_addr		= 0x436ecc,
+		.app_load_addr			= 0x1234,
+		.board_ext_data_addr		= 0x437000,
+		.reserved_ram_size		= 9216,
+		.board_addr			= 0x435c00,
+		.refclk_hz			= 40000000,
+		.uarttx_pin			= 11,
+
+		.fw = {
+			.dir		= AR6004_HW_1_2_FW_DIR,
+			.fw		= AR6004_HW_1_2_FIRMWARE_FILE,
+		},
+		.fw_board		= AR6004_HW_1_2_BOARD_DATA_FILE,
+		.fw_default_board	= AR6004_HW_1_2_DEFAULT_BOARD_DATA_FILE,
+	},
 };
 
 /*
@@ -256,6 +278,7 @@ static int ath6kl_init_service_ep(struct ath6kl *ar)
 	memset(&connect, 0, sizeof(connect));
 
 	/* these fields are the same for all service endpoints */
+	connect.ep_cb.tx_comp_multi = ath6kl_tx_complete;
 	connect.ep_cb.rx = ath6kl_rx;
 	connect.ep_cb.rx_refill = ath6kl_rx_refill;
 	connect.ep_cb.tx_full = ath6kl_tx_queue_full;
@@ -440,9 +463,9 @@ static int ath6kl_target_config_wlan_params(struct ath6kl *ar, int idx)
 					      P2P_FLAG_MACADDR_REQ |
 					      P2P_FLAG_HMODEL_REQ);
 		if (ret) {
-			ath6kl_dbg(ATH6KL_DBG_TRC, "failed to request P2P "
-				   "capabilities (%d) - assuming P2P not "
-				   "supported\n", ret);
+			ath6kl_dbg(ATH6KL_DBG_TRC,
+				   "failed to request P2P capabilities (%d) - assuming P2P not supported\n",
+				   ret);
 			ar->p2p = false;
 		}
 	}
@@ -451,8 +474,9 @@ static int ath6kl_target_config_wlan_params(struct ath6kl *ar, int idx)
 		/* Enable Probe Request reporting for P2P */
 		ret = ath6kl_wmi_probe_report_req_cmd(ar->wmi, idx, true);
 		if (ret) {
-			ath6kl_dbg(ATH6KL_DBG_TRC, "failed to enable Probe "
-				   "Request reporting (%d)\n", ret);
+			ath6kl_dbg(ATH6KL_DBG_TRC,
+				   "failed to enable Probe Request reporting (%d)\n",
+				   ret);
 		}
 	}
 
@@ -485,22 +509,31 @@ int ath6kl_configure_target(struct ath6kl *ar)
 		fw_mode |= fw_iftype << (i * HI_OPTION_FW_MODE_BITS);
 
 	/*
-	 * By default, submodes :
+	 * Submodes when fw does not support dynamic interface
+	 * switching:
 	 *		vif[0] - AP/STA/IBSS
 	 *		vif[1] - "P2P dev"/"P2P GO"/"P2P Client"
 	 *		vif[2] - "P2P dev"/"P2P GO"/"P2P Client"
+	 * Otherwise, All the interface are initialized to p2p dev.
 	 */
 
-	for (i = 0; i < ar->max_norm_iface; i++)
-		fw_submode |= HI_OPTION_FW_SUBMODE_NONE <<
-			      (i * HI_OPTION_FW_SUBMODE_BITS);
+	if (test_bit(ATH6KL_FW_CAPABILITY_STA_P2PDEV_DUPLEX,
+		     ar->fw_capabilities)) {
+		for (i = 0; i < ar->vif_max; i++)
+			fw_submode |= HI_OPTION_FW_SUBMODE_P2PDEV <<
+				(i * HI_OPTION_FW_SUBMODE_BITS);
+	} else {
+		for (i = 0; i < ar->max_norm_iface; i++)
+			fw_submode |= HI_OPTION_FW_SUBMODE_NONE <<
+				(i * HI_OPTION_FW_SUBMODE_BITS);
 
-	for (i = ar->max_norm_iface; i < ar->vif_max; i++)
-		fw_submode |= HI_OPTION_FW_SUBMODE_P2PDEV <<
-			      (i * HI_OPTION_FW_SUBMODE_BITS);
+		for (i = ar->max_norm_iface; i < ar->vif_max; i++)
+			fw_submode |= HI_OPTION_FW_SUBMODE_P2PDEV <<
+				(i * HI_OPTION_FW_SUBMODE_BITS);
 
-	if (ar->p2p && ar->vif_max == 1)
-		fw_submode = HI_OPTION_FW_SUBMODE_P2PDEV;
+		if (ar->p2p && ar->vif_max == 1)
+			fw_submode = HI_OPTION_FW_SUBMODE_P2PDEV;
+	}
 
 	if (ath6kl_bmi_write_hi32(ar, hi_app_host_interest,
 				  HTC_PROTOCOL_VERSION) != 0) {
@@ -539,18 +572,20 @@ int ath6kl_configure_target(struct ath6kl *ar)
 	 * but possible in theory.
 	 */
 
-	param = ar->hw.board_ext_data_addr;
-	ram_reserved_size = ar->hw.reserved_ram_size;
+	if (ar->target_type == TARGET_TYPE_AR6003) {
+		param = ar->hw.board_ext_data_addr;
+		ram_reserved_size = ar->hw.reserved_ram_size;
 
-	if (ath6kl_bmi_write_hi32(ar, hi_board_ext_data, param) != 0) {
-		ath6kl_err("bmi_write_memory for hi_board_ext_data failed\n");
-		return -EIO;
-	}
+		if (ath6kl_bmi_write_hi32(ar, hi_board_ext_data, param) != 0) {
+			ath6kl_err("bmi_write_memory for hi_board_ext_data failed\n");
+			return -EIO;
+		}
 
-	if (ath6kl_bmi_write_hi32(ar, hi_end_ram_reserve_sz,
-				  ram_reserved_size) != 0) {
-		ath6kl_err("bmi_write_memory for hi_end_ram_reserve_sz failed\n");
-		return -EIO;
+		if (ath6kl_bmi_write_hi32(ar, hi_end_ram_reserve_sz,
+					  ram_reserved_size) != 0) {
+			ath6kl_err("bmi_write_memory for hi_end_ram_reserve_sz failed\n");
+			return -EIO;
+		}
 	}
 
 	/* set the block size for the target */
@@ -924,13 +959,14 @@ static int ath6kl_fetch_fw_apin(struct ath6kl *ar, const char *name)
 			if (ar->fw != NULL)
 				break;
 
-			ar->fw = kmemdup(data, ie_len, GFP_KERNEL);
+			ar->fw = vmalloc(ie_len);
 
 			if (ar->fw == NULL) {
 				ret = -ENOMEM;
 				goto out;
 			}
 
+			memcpy(ar->fw, data, ie_len);
 			ar->fw_len = ie_len;
 			break;
 		case ATH6KL_FW_IE_PATCH_IMAGE:
@@ -1507,7 +1543,7 @@ int ath6kl_init_hw_start(struct ath6kl *ar)
 	}
 
 	/* setup credit distribution */
-	ath6kl_credit_setup(ar->htc_target, &ar->credit_state_info);
+	ath6kl_htc_credit_setup(ar->htc_target, &ar->credit_state_info);
 
 	/* start HTC */
 	ret = ath6kl_htc_start(ar->htc_target);
diff --git a/drivers/net/wireless/ath/ath6kl/main.c b/drivers/net/wireless/ath/ath6kl/main.c
index 229e1922ebe..e5524470529 100644
--- a/drivers/net/wireless/ath/ath6kl/main.c
+++ b/drivers/net/wireless/ath/ath6kl/main.c
@@ -15,6 +15,8 @@
  * OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
  */
 
+#define pr_fmt(fmt) KBUILD_MODNAME ": " fmt
+
 #include "core.h"
 #include "hif-ops.h"
 #include "cfg80211.h"
@@ -419,8 +421,8 @@ void ath6kl_connect_ap_mode_bss(struct ath6kl_vif *vif, u16 channel)
 		if (!ik->valid)
 			break;
 
-		ath6kl_dbg(ATH6KL_DBG_WLAN_CFG, "Delayed addkey for "
-			   "the initial group key for AP mode\n");
+		ath6kl_dbg(ATH6KL_DBG_WLAN_CFG,
+			   "Delayed addkey for the initial group key for AP mode\n");
 		memset(key_rsc, 0, sizeof(key_rsc));
 		res = ath6kl_wmi_addkey_cmd(
 			ar->wmi, vif->fw_vif_idx, ik->key_index, ik->key_type,
@@ -428,12 +430,19 @@ void ath6kl_connect_ap_mode_bss(struct ath6kl_vif *vif, u16 channel)
 			ik->key,
 			KEY_OP_INIT_VAL, NULL, SYNC_BOTH_WMIFLAG);
 		if (res) {
-			ath6kl_dbg(ATH6KL_DBG_WLAN_CFG, "Delayed "
-				   "addkey failed: %d\n", res);
+			ath6kl_dbg(ATH6KL_DBG_WLAN_CFG,
+				   "Delayed addkey failed: %d\n", res);
 		}
 		break;
 	}
 
+	if (ar->want_ch_switch & (1 << vif->fw_vif_idx)) {
+		ar->want_ch_switch &= ~(1 << vif->fw_vif_idx);
+		/* we actually don't know the phymode, default to HT20 */
+		ath6kl_cfg80211_ch_switch_notify(vif, channel,
+						 WMI_11G_HT20);
+	}
+
 	ath6kl_wmi_bssfilter_cmd(ar->wmi, vif->fw_vif_idx, NONE_BSS_FILTER, 0);
 	set_bit(CONNECTED, &vif->flags);
 	netif_carrier_on(vif->ndev);
@@ -539,7 +548,8 @@ void ath6kl_disconnect(struct ath6kl_vif *vif)
 
 /* WMI Event handlers */
 
-void ath6kl_ready_event(void *devt, u8 *datap, u32 sw_ver, u32 abi_ver)
+void ath6kl_ready_event(void *devt, u8 *datap, u32 sw_ver, u32 abi_ver,
+			enum wmi_phy_cap cap)
 {
 	struct ath6kl *ar = devt;
 
@@ -549,6 +559,7 @@ void ath6kl_ready_event(void *devt, u8 *datap, u32 sw_ver, u32 abi_ver)
 
 	ar->version.wlan_ver = sw_ver;
 	ar->version.abi_ver = abi_ver;
+	ar->hw.cap = cap;
 
 	snprintf(ar->wiphy->fw_version,
 		 sizeof(ar->wiphy->fw_version),
@@ -582,6 +593,45 @@ void ath6kl_scan_complete_evt(struct ath6kl_vif *vif, int status)
 	ath6kl_dbg(ATH6KL_DBG_WLAN_CFG, "scan complete: %d\n", status);
 }
 
+static int ath6kl_commit_ch_switch(struct ath6kl_vif *vif, u16 channel)
+{
+
+	struct ath6kl *ar = vif->ar;
+
+	vif->next_chan = channel;
+	vif->profile.ch = cpu_to_le16(channel);
+
+	switch (vif->nw_type) {
+	case AP_NETWORK:
+		return ath6kl_wmi_ap_profile_commit(ar->wmi, vif->fw_vif_idx,
+						    &vif->profile);
+	default:
+		ath6kl_err("won't switch channels nw_type=%d\n", vif->nw_type);
+		return -ENOTSUPP;
+	}
+}
+
+static void ath6kl_check_ch_switch(struct ath6kl *ar, u16 channel)
+{
+
+	struct ath6kl_vif *vif;
+	int res = 0;
+
+	if (!ar->want_ch_switch)
+		return;
+
+	spin_lock_bh(&ar->list_lock);
+	list_for_each_entry(vif, &ar->vif_list, list) {
+		if (ar->want_ch_switch & (1 << vif->fw_vif_idx))
+			res = ath6kl_commit_ch_switch(vif, channel);
+
+		if (res)
+			ath6kl_err("channel switch failed nw_type %d res %d\n",
+				   vif->nw_type, res);
+	}
+	spin_unlock_bh(&ar->list_lock);
+}
+
 void ath6kl_connect_event(struct ath6kl_vif *vif, u16 channel, u8 *bssid,
 			  u16 listen_int, u16 beacon_int,
 			  enum network_type net_type, u8 beacon_ie_len,
@@ -599,9 +649,11 @@ void ath6kl_connect_event(struct ath6kl_vif *vif, u16 channel, u8 *bssid,
 	memcpy(vif->bssid, bssid, sizeof(vif->bssid));
 	vif->bss_ch = channel;
 
-	if ((vif->nw_type == INFRA_NETWORK))
+	if ((vif->nw_type == INFRA_NETWORK)) {
 		ath6kl_wmi_listeninterval_cmd(ar->wmi, vif->fw_vif_idx,
 					      vif->listen_intvl_t, 0);
+		ath6kl_check_ch_switch(ar, channel);
+	}
 
 	netif_wake_queue(vif->ndev);
 
@@ -756,6 +808,10 @@ static void ath6kl_update_target_stats(struct ath6kl_vif *vif, u8 *ptr, u32 len)
 	stats->wow_evt_discarded +=
 		le16_to_cpu(tgt_stats->wow_stats.wow_evt_discarded);
 
+	stats->arp_received = le32_to_cpu(tgt_stats->arp_stats.arp_received);
+	stats->arp_replied = le32_to_cpu(tgt_stats->arp_stats.arp_replied);
+	stats->arp_matched = le32_to_cpu(tgt_stats->arp_stats.arp_matched);
+
 	if (test_bit(STATS_UPDATE_PEND, &vif->flags)) {
 		clear_bit(STATS_UPDATE_PEND, &vif->flags);
 		wake_up(&ar->event_wq);
@@ -920,6 +976,11 @@ void ath6kl_disconnect_event(struct ath6kl_vif *vif, u8 reason, u8 *bssid,
 	struct ath6kl *ar = vif->ar;
 
 	if (vif->nw_type == AP_NETWORK) {
+		/* disconnect due to other STA vif switching channels */
+		if (reason == BSS_DISCONNECTED &&
+		    prot_reason_status == WMI_AP_REASON_STA_ROAM)
+			ar->want_ch_switch |= 1 << vif->fw_vif_idx;
+
 		if (!ath6kl_remove_sta(ar, bssid, prot_reason_status))
 			return;
 
@@ -1084,7 +1145,7 @@ static int ath6kl_set_features(struct net_device *dev,
 static void ath6kl_set_multicast_list(struct net_device *ndev)
 {
 	struct ath6kl_vif *vif = netdev_priv(ndev);
-	bool mc_all_on = false, mc_all_off = false;
+	bool mc_all_on = false;
 	int mc_count = netdev_mc_count(ndev);
 	struct netdev_hw_addr *ha;
 	bool found;
@@ -1096,24 +1157,41 @@ static void ath6kl_set_multicast_list(struct net_device *ndev)
 	    !test_bit(WLAN_ENABLED, &vif->flags))
 		return;
 
+	/* Enable multicast-all filter. */
 	mc_all_on = !!(ndev->flags & IFF_PROMISC) ||
 		    !!(ndev->flags & IFF_ALLMULTI) ||
 		    !!(mc_count > ATH6K_MAX_MC_FILTERS_PER_LIST);
 
-	mc_all_off = !(ndev->flags & IFF_MULTICAST) || mc_count == 0;
+	if (mc_all_on)
+		set_bit(NETDEV_MCAST_ALL_ON, &vif->flags);
+	else
+		clear_bit(NETDEV_MCAST_ALL_ON, &vif->flags);
+
+	mc_all_on = mc_all_on || (vif->ar->state == ATH6KL_STATE_ON);
 
-	if (mc_all_on || mc_all_off) {
-		/* Enable/disable all multicast */
-		ath6kl_dbg(ATH6KL_DBG_TRC, "%s multicast filter\n",
-			   mc_all_on ? "enabling" : "disabling");
-		ret = ath6kl_wmi_mcast_filter_cmd(vif->ar->wmi, vif->fw_vif_idx,
+	if (!(ndev->flags & IFF_MULTICAST)) {
+		mc_all_on = false;
+		set_bit(NETDEV_MCAST_ALL_OFF, &vif->flags);
+	} else {
+		clear_bit(NETDEV_MCAST_ALL_OFF, &vif->flags);
+	}
+
+	/* Enable/disable "multicast-all" filter*/
+	ath6kl_dbg(ATH6KL_DBG_TRC, "%s multicast-all filter\n",
+		   mc_all_on ? "enabling" : "disabling");
+
+	ret = ath6kl_wmi_mcast_filter_cmd(vif->ar->wmi, vif->fw_vif_idx,
 						  mc_all_on);
-		if (ret)
-			ath6kl_warn("Failed to %s multicast receive\n",
-				    mc_all_on ? "enable" : "disable");
+	if (ret) {
+		ath6kl_warn("Failed to %s multicast-all receive\n",
+			    mc_all_on ? "enable" : "disable");
 		return;
 	}
 
+	if (test_bit(NETDEV_MCAST_ALL_ON, &vif->flags))
+		return;
+
+	/* Keep the driver and firmware mcast list in sync. */
 	list_for_each_entry_safe(mc_filter, tmp, &vif->mc_filter, list) {
 		found = false;
 		netdev_for_each_mc_addr(ha, ndev) {
diff --git a/drivers/net/wireless/ath/ath6kl/sdio.c b/drivers/net/wireless/ath/ath6kl/sdio.c
index 53528648b42..05b95405f7b 100644
--- a/drivers/net/wireless/ath/ath6kl/sdio.c
+++ b/drivers/net/wireless/ath/ath6kl/sdio.c
@@ -552,7 +552,7 @@ static int ath6kl_sdio_write_async(struct ath6kl *ar, u32 address, u8 *buffer,
 
 	bus_req = ath6kl_sdio_alloc_busreq(ar_sdio);
 
-	if (!bus_req)
+	if (WARN_ON_ONCE(!bus_req))
 		return -ENOMEM;
 
 	bus_req->address = address;
@@ -915,6 +915,9 @@ static int ath6kl_sdio_suspend(struct ath6kl *ar, struct cfg80211_wowlan *wow)
 	}
 
 cut_pwr:
+	if (func->card && func->card->host)
+		func->card->host->pm_flags &= ~MMC_PM_KEEP_POWER;
+
 	return ath6kl_cfg80211_suspend(ar, ATH6KL_CFG_SUSPEND_CUTPOWER, NULL);
 }
 
@@ -985,9 +988,8 @@ static int ath6kl_set_addrwin_reg(struct ath6kl *ar, u32 reg_addr, u32 addr)
 	}
 
 	if (status) {
-		ath6kl_err("%s: failed to write initial bytes of 0x%x "
-			   "to window reg: 0x%X\n", __func__,
-			   addr, reg_addr);
+		ath6kl_err("%s: failed to write initial bytes of 0x%x to window reg: 0x%X\n",
+			   __func__, addr, reg_addr);
 		return status;
 	}
 
@@ -1076,8 +1078,8 @@ static int ath6kl_sdio_bmi_credits(struct ath6kl *ar)
 					 (u8 *)&ar->bmi.cmd_credits, 4,
 					 HIF_RD_SYNC_BYTE_INC);
 		if (ret) {
-			ath6kl_err("Unable to decrement the command credit "
-						"count register: %d\n", ret);
+			ath6kl_err("Unable to decrement the command credit count register: %d\n",
+				   ret);
 			return ret;
 		}
 
@@ -1362,7 +1364,7 @@ static int ath6kl_sdio_probe(struct sdio_func *func,
 		goto err_core_alloc;
 	}
 
-	ret = ath6kl_core_init(ar);
+	ret = ath6kl_core_init(ar, ATH6KL_HTC_TYPE_MBOX);
 	if (ret) {
 		ath6kl_err("Failed to init ath6kl core\n");
 		goto err_core_alloc;
@@ -1457,3 +1459,6 @@ MODULE_FIRMWARE(AR6004_HW_1_0_DEFAULT_BOARD_DATA_FILE);
 MODULE_FIRMWARE(AR6004_HW_1_1_FW_DIR "/" AR6004_HW_1_1_FIRMWARE_FILE);
 MODULE_FIRMWARE(AR6004_HW_1_1_BOARD_DATA_FILE);
 MODULE_FIRMWARE(AR6004_HW_1_1_DEFAULT_BOARD_DATA_FILE);
+MODULE_FIRMWARE(AR6004_HW_1_2_FW_DIR "/" AR6004_HW_1_2_FIRMWARE_FILE);
+MODULE_FIRMWARE(AR6004_HW_1_2_BOARD_DATA_FILE);
+MODULE_FIRMWARE(AR6004_HW_1_2_DEFAULT_BOARD_DATA_FILE);
diff --git a/drivers/net/wireless/ath/ath6kl/testmode.c b/drivers/net/wireless/ath/ath6kl/testmode.c
index 6675c92b542..acc9aa832f7 100644
--- a/drivers/net/wireless/ath/ath6kl/testmode.c
+++ b/drivers/net/wireless/ath/ath6kl/testmode.c
@@ -55,8 +55,9 @@ void ath6kl_tm_rx_event(struct ath6kl *ar, void *buf, size_t buf_len)
 		ath6kl_warn("failed to allocate testmode rx skb!\n");
 		return;
 	}
-	NLA_PUT_U32(skb, ATH6KL_TM_ATTR_CMD, ATH6KL_TM_CMD_TCMD);
-	NLA_PUT(skb, ATH6KL_TM_ATTR_DATA, buf_len, buf);
+	if (nla_put_u32(skb, ATH6KL_TM_ATTR_CMD, ATH6KL_TM_CMD_TCMD) ||
+	    nla_put(skb, ATH6KL_TM_ATTR_DATA, buf_len, buf))
+		goto nla_put_failure;
 	cfg80211_testmode_event(skb, GFP_KERNEL);
 	return;
 
diff --git a/drivers/net/wireless/ath/ath6kl/txrx.c b/drivers/net/wireless/ath/ath6kl/txrx.c
index f85353fd179..67206aedea6 100644
--- a/drivers/net/wireless/ath/ath6kl/txrx.c
+++ b/drivers/net/wireless/ath/ath6kl/txrx.c
@@ -15,8 +15,11 @@
  * OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
  */
 
+#define pr_fmt(fmt) KBUILD_MODNAME ": " fmt
+
 #include "core.h"
 #include "debug.h"
+#include "htc-ops.h"
 
 /*
  * tid - tid_mux0..tid_mux3
@@ -322,6 +325,7 @@ int ath6kl_control_tx(void *devt, struct sk_buff *skb,
 	cookie->map_no = 0;
 	set_htc_pkt_info(&cookie->htc_pkt, cookie, skb->data, skb->len,
 			 eid, ATH6KL_CONTROL_PKT_TAG);
+	cookie->htc_pkt.skb = skb;
 
 	/*
 	 * This interface is asynchronous, if there is an error, cleanup
@@ -358,15 +362,11 @@ int ath6kl_data_tx(struct sk_buff *skb, struct net_device *dev)
 		   skb, skb->data, skb->len);
 
 	/* If target is not associated */
-	if (!test_bit(CONNECTED, &vif->flags)) {
-		dev_kfree_skb(skb);
-		return 0;
-	}
+	if (!test_bit(CONNECTED, &vif->flags))
+		goto fail_tx;
 
-	if (WARN_ON_ONCE(ar->state != ATH6KL_STATE_ON)) {
-		dev_kfree_skb(skb);
-		return 0;
-	}
+	if (WARN_ON_ONCE(ar->state != ATH6KL_STATE_ON))
+		goto fail_tx;
 
 	if (!test_bit(WMI_READY, &ar->flag))
 		goto fail_tx;
@@ -490,6 +490,7 @@ int ath6kl_data_tx(struct sk_buff *skb, struct net_device *dev)
 	cookie->map_no = map_no;
 	set_htc_pkt_info(&cookie->htc_pkt, cookie, skb->data, skb->len,
 			 eid, htc_tag);
+	cookie->htc_pkt.skb = skb;
 
 	ath6kl_dbg_dump(ATH6KL_DBG_RAW_BYTES, __func__, "tx ",
 			skb->data, skb->len);
@@ -570,7 +571,7 @@ void ath6kl_indicate_tx_activity(void *devt, u8 traffic_class, bool active)
 
 notify_htc:
 	/* notify HTC, this may cause credit distribution changes */
-	ath6kl_htc_indicate_activity_change(ar->htc_target, eid, active);
+	ath6kl_htc_activity_changed(ar->htc_target, eid, active);
 }
 
 enum htc_send_full_action ath6kl_tx_queue_full(struct htc_target *target,
@@ -666,9 +667,10 @@ static void ath6kl_tx_clear_node_map(struct ath6kl_vif *vif,
 	}
 }
 
-void ath6kl_tx_complete(void *context, struct list_head *packet_queue)
+void ath6kl_tx_complete(struct htc_target *target,
+			struct list_head *packet_queue)
 {
-	struct ath6kl *ar = context;
+	struct ath6kl *ar = target->dev->ar;
 	struct sk_buff_head skb_queue;
 	struct htc_packet *packet;
 	struct sk_buff *skb;
@@ -887,6 +889,7 @@ void ath6kl_rx_refill(struct htc_target *target, enum htc_endpoint_id endpoint)
 			skb->data = PTR_ALIGN(skb->data - 4, 4);
 		set_htc_rxpkt_info(packet, skb, skb->data,
 				   ATH6KL_BUFFER_SIZE, endpoint);
+		packet->skb = skb;
 		list_add_tail(&packet->list, &queue);
 	}
 
@@ -909,6 +912,8 @@ void ath6kl_refill_amsdu_rxbufs(struct ath6kl *ar, int count)
 			skb->data = PTR_ALIGN(skb->data - 4, 4);
 		set_htc_rxpkt_info(packet, skb, skb->data,
 				   ATH6KL_AMSDU_BUFFER_SIZE, 0);
+		packet->skb = skb;
+
 		spin_lock_bh(&ar->lock);
 		list_add_tail(&packet->list, &ar->amsdu_rx_buffer_queue);
 		spin_unlock_bh(&ar->lock);
@@ -1281,6 +1286,7 @@ void ath6kl_rx(struct htc_target *target, struct htc_packet *packet)
 	struct wmi_data_hdr *dhdr;
 	int min_hdr_len;
 	u8 meta_type, dot11_hdr = 0;
+	u8 pad_before_data_start;
 	int status = packet->status;
 	enum htc_endpoint_id ept = packet->endpoint;
 	bool is_amsdu, prev_ps, ps_state = false;
@@ -1492,6 +1498,10 @@ void ath6kl_rx(struct htc_target *target, struct htc_packet *packet)
 	seq_no = wmi_data_hdr_get_seqno(dhdr);
 	meta_type = wmi_data_hdr_get_meta(dhdr);
 	dot11_hdr = wmi_data_hdr_get_dot11(dhdr);
+	pad_before_data_start =
+		(le16_to_cpu(dhdr->info3) >> WMI_DATA_HDR_PAD_BEFORE_DATA_SHIFT)
+			& WMI_DATA_HDR_PAD_BEFORE_DATA_MASK;
+
 	skb_pull(skb, sizeof(struct wmi_data_hdr));
 
 	switch (meta_type) {
@@ -1510,6 +1520,8 @@ void ath6kl_rx(struct htc_target *target, struct htc_packet *packet)
 		break;
 	}
 
+	skb_pull(skb, pad_before_data_start);
+
 	if (dot11_hdr)
 		status = ath6kl_wmi_dot11_hdr_remove(ar->wmi, skb);
 	else if (!is_amsdu)
@@ -1579,7 +1591,8 @@ void ath6kl_rx(struct htc_target *target, struct htc_packet *packet)
 			/* aggregation code will handle the skb */
 			return;
 		}
-	}
+	} else if (!is_broadcast_ether_addr(datap->h_dest))
+		vif->net_stats.multicast++;
 
 	ath6kl_deliver_frames_to_nw_stack(vif->ndev, skb);
 }
diff --git a/drivers/net/wireless/ath/ath6kl/usb.c b/drivers/net/wireless/ath/ath6kl/usb.c
index 325b1224c2b..3740c3d6ab8 100644
--- a/drivers/net/wireless/ath/ath6kl/usb.c
+++ b/drivers/net/wireless/ath/ath6kl/usb.c
@@ -21,15 +21,77 @@
 #include "debug.h"
 #include "core.h"
 
+/* constants */
+#define TX_URB_COUNT            32
+#define RX_URB_COUNT            32
+#define ATH6KL_USB_RX_BUFFER_SIZE  1700
+
+/* tx/rx pipes for usb */
+enum ATH6KL_USB_PIPE_ID {
+	ATH6KL_USB_PIPE_TX_CTRL = 0,
+	ATH6KL_USB_PIPE_TX_DATA_LP,
+	ATH6KL_USB_PIPE_TX_DATA_MP,
+	ATH6KL_USB_PIPE_TX_DATA_HP,
+	ATH6KL_USB_PIPE_RX_CTRL,
+	ATH6KL_USB_PIPE_RX_DATA,
+	ATH6KL_USB_PIPE_RX_DATA2,
+	ATH6KL_USB_PIPE_RX_INT,
+	ATH6KL_USB_PIPE_MAX
+};
+
+#define ATH6KL_USB_PIPE_INVALID ATH6KL_USB_PIPE_MAX
+
+struct ath6kl_usb_pipe {
+	struct list_head urb_list_head;
+	struct usb_anchor urb_submitted;
+	u32 urb_alloc;
+	u32 urb_cnt;
+	u32 urb_cnt_thresh;
+	unsigned int usb_pipe_handle;
+	u32 flags;
+	u8 ep_address;
+	u8 logical_pipe_num;
+	struct ath6kl_usb *ar_usb;
+	u16 max_packet_size;
+	struct work_struct io_complete_work;
+	struct sk_buff_head io_comp_queue;
+	struct usb_endpoint_descriptor *ep_desc;
+};
+
+#define ATH6KL_USB_PIPE_FLAG_TX    (1 << 0)
+
 /* usb device object */
 struct ath6kl_usb {
+	/* protects pipe->urb_list_head and  pipe->urb_cnt */
+	spinlock_t cs_lock;
+
 	struct usb_device *udev;
 	struct usb_interface *interface;
+	struct ath6kl_usb_pipe pipes[ATH6KL_USB_PIPE_MAX];
 	u8 *diag_cmd_buffer;
 	u8 *diag_resp_buffer;
 	struct ath6kl *ar;
 };
 
+/* usb urb object */
+struct ath6kl_urb_context {
+	struct list_head link;
+	struct ath6kl_usb_pipe *pipe;
+	struct sk_buff *skb;
+	struct ath6kl *ar;
+};
+
+/* USB endpoint definitions */
+#define ATH6KL_USB_EP_ADDR_APP_CTRL_IN          0x81
+#define ATH6KL_USB_EP_ADDR_APP_DATA_IN          0x82
+#define ATH6KL_USB_EP_ADDR_APP_DATA2_IN         0x83
+#define ATH6KL_USB_EP_ADDR_APP_INT_IN           0x84
+
+#define ATH6KL_USB_EP_ADDR_APP_CTRL_OUT         0x01
+#define ATH6KL_USB_EP_ADDR_APP_DATA_LP_OUT      0x02
+#define ATH6KL_USB_EP_ADDR_APP_DATA_MP_OUT      0x03
+#define ATH6KL_USB_EP_ADDR_APP_DATA_HP_OUT      0x04
+
 /* diagnostic command defnitions */
 #define ATH6KL_USB_CONTROL_REQ_SEND_BMI_CMD        1
 #define ATH6KL_USB_CONTROL_REQ_RECV_BMI_RESP       2
@@ -55,11 +117,493 @@ struct ath6kl_usb_ctrl_diag_resp_read {
 	__le32 value;
 } __packed;
 
+/* function declarations */
+static void ath6kl_usb_recv_complete(struct urb *urb);
+
+#define ATH6KL_USB_IS_BULK_EP(attr) (((attr) & 3) == 0x02)
+#define ATH6KL_USB_IS_INT_EP(attr)  (((attr) & 3) == 0x03)
+#define ATH6KL_USB_IS_ISOC_EP(attr)  (((attr) & 3) == 0x01)
+#define ATH6KL_USB_IS_DIR_IN(addr)  ((addr) & 0x80)
+
+/* pipe/urb operations */
+static struct ath6kl_urb_context *
+ath6kl_usb_alloc_urb_from_pipe(struct ath6kl_usb_pipe *pipe)
+{
+	struct ath6kl_urb_context *urb_context = NULL;
+	unsigned long flags;
+
+	spin_lock_irqsave(&pipe->ar_usb->cs_lock, flags);
+	if (!list_empty(&pipe->urb_list_head)) {
+		urb_context =
+		    list_first_entry(&pipe->urb_list_head,
+				     struct ath6kl_urb_context, link);
+		list_del(&urb_context->link);
+		pipe->urb_cnt--;
+	}
+	spin_unlock_irqrestore(&pipe->ar_usb->cs_lock, flags);
+
+	return urb_context;
+}
+
+static void ath6kl_usb_free_urb_to_pipe(struct ath6kl_usb_pipe *pipe,
+					struct ath6kl_urb_context *urb_context)
+{
+	unsigned long flags;
+
+	spin_lock_irqsave(&pipe->ar_usb->cs_lock, flags);
+	pipe->urb_cnt++;
+
+	list_add(&urb_context->link, &pipe->urb_list_head);
+	spin_unlock_irqrestore(&pipe->ar_usb->cs_lock, flags);
+}
+
+static void ath6kl_usb_cleanup_recv_urb(struct ath6kl_urb_context *urb_context)
+{
+	if (urb_context->skb != NULL) {
+		dev_kfree_skb(urb_context->skb);
+		urb_context->skb = NULL;
+	}
+
+	ath6kl_usb_free_urb_to_pipe(urb_context->pipe, urb_context);
+}
+
+static inline struct ath6kl_usb *ath6kl_usb_priv(struct ath6kl *ar)
+{
+	return ar->hif_priv;
+}
+
+/* pipe resource allocation/cleanup */
+static int ath6kl_usb_alloc_pipe_resources(struct ath6kl_usb_pipe *pipe,
+					   int urb_cnt)
+{
+	struct ath6kl_urb_context *urb_context;
+	int status = 0, i;
+
+	INIT_LIST_HEAD(&pipe->urb_list_head);
+	init_usb_anchor(&pipe->urb_submitted);
+
+	for (i = 0; i < urb_cnt; i++) {
+		urb_context = kzalloc(sizeof(struct ath6kl_urb_context),
+				      GFP_KERNEL);
+		if (urb_context == NULL)
+			/* FIXME: set status to -ENOMEM */
+			break;
+
+		urb_context->pipe = pipe;
+
+		/*
+		 * we are only allocate the urb contexts here, the actual URB
+		 * is allocated from the kernel as needed to do a transaction
+		 */
+		pipe->urb_alloc++;
+		ath6kl_usb_free_urb_to_pipe(pipe, urb_context);
+	}
+
+	ath6kl_dbg(ATH6KL_DBG_USB,
+		   "ath6kl usb: alloc resources lpipe:%d hpipe:0x%X urbs:%d\n",
+		   pipe->logical_pipe_num, pipe->usb_pipe_handle,
+		   pipe->urb_alloc);
+
+	return status;
+}
+
+static void ath6kl_usb_free_pipe_resources(struct ath6kl_usb_pipe *pipe)
+{
+	struct ath6kl_urb_context *urb_context;
+
+	if (pipe->ar_usb == NULL) {
+		/* nothing allocated for this pipe */
+		return;
+	}
+
+	ath6kl_dbg(ATH6KL_DBG_USB,
+		   "ath6kl usb: free resources lpipe:%d"
+		   "hpipe:0x%X urbs:%d avail:%d\n",
+		   pipe->logical_pipe_num, pipe->usb_pipe_handle,
+		   pipe->urb_alloc, pipe->urb_cnt);
+
+	if (pipe->urb_alloc != pipe->urb_cnt) {
+		ath6kl_dbg(ATH6KL_DBG_USB,
+			   "ath6kl usb: urb leak! lpipe:%d"
+			   "hpipe:0x%X urbs:%d avail:%d\n",
+			   pipe->logical_pipe_num, pipe->usb_pipe_handle,
+			   pipe->urb_alloc, pipe->urb_cnt);
+	}
+
+	while (true) {
+		urb_context = ath6kl_usb_alloc_urb_from_pipe(pipe);
+		if (urb_context == NULL)
+			break;
+		kfree(urb_context);
+	}
+
+}
+
+static void ath6kl_usb_cleanup_pipe_resources(struct ath6kl_usb *ar_usb)
+{
+	int i;
+
+	for (i = 0; i < ATH6KL_USB_PIPE_MAX; i++)
+		ath6kl_usb_free_pipe_resources(&ar_usb->pipes[i]);
+
+}
+
+static u8 ath6kl_usb_get_logical_pipe_num(struct ath6kl_usb *ar_usb,
+					  u8 ep_address, int *urb_count)
+{
+	u8 pipe_num = ATH6KL_USB_PIPE_INVALID;
+
+	switch (ep_address) {
+	case ATH6KL_USB_EP_ADDR_APP_CTRL_IN:
+		pipe_num = ATH6KL_USB_PIPE_RX_CTRL;
+		*urb_count = RX_URB_COUNT;
+		break;
+	case ATH6KL_USB_EP_ADDR_APP_DATA_IN:
+		pipe_num = ATH6KL_USB_PIPE_RX_DATA;
+		*urb_count = RX_URB_COUNT;
+		break;
+	case ATH6KL_USB_EP_ADDR_APP_INT_IN:
+		pipe_num = ATH6KL_USB_PIPE_RX_INT;
+		*urb_count = RX_URB_COUNT;
+		break;
+	case ATH6KL_USB_EP_ADDR_APP_DATA2_IN:
+		pipe_num = ATH6KL_USB_PIPE_RX_DATA2;
+		*urb_count = RX_URB_COUNT;
+		break;
+	case ATH6KL_USB_EP_ADDR_APP_CTRL_OUT:
+		pipe_num = ATH6KL_USB_PIPE_TX_CTRL;
+		*urb_count = TX_URB_COUNT;
+		break;
+	case ATH6KL_USB_EP_ADDR_APP_DATA_LP_OUT:
+		pipe_num = ATH6KL_USB_PIPE_TX_DATA_LP;
+		*urb_count = TX_URB_COUNT;
+		break;
+	case ATH6KL_USB_EP_ADDR_APP_DATA_MP_OUT:
+		pipe_num = ATH6KL_USB_PIPE_TX_DATA_MP;
+		*urb_count = TX_URB_COUNT;
+		break;
+	case ATH6KL_USB_EP_ADDR_APP_DATA_HP_OUT:
+		pipe_num = ATH6KL_USB_PIPE_TX_DATA_HP;
+		*urb_count = TX_URB_COUNT;
+		break;
+	default:
+		/* note: there may be endpoints not currently used */
+		break;
+	}
+
+	return pipe_num;
+}
+
+static int ath6kl_usb_setup_pipe_resources(struct ath6kl_usb *ar_usb)
+{
+	struct usb_interface *interface = ar_usb->interface;
+	struct usb_host_interface *iface_desc = interface->cur_altsetting;
+	struct usb_endpoint_descriptor *endpoint;
+	struct ath6kl_usb_pipe *pipe;
+	int i, urbcount, status = 0;
+	u8 pipe_num;
+
+	ath6kl_dbg(ATH6KL_DBG_USB, "setting up USB Pipes using interface\n");
+
+	/* walk decriptors and setup pipes */
+	for (i = 0; i < iface_desc->desc.bNumEndpoints; ++i) {
+		endpoint = &iface_desc->endpoint[i].desc;
+
+		if (ATH6KL_USB_IS_BULK_EP(endpoint->bmAttributes)) {
+			ath6kl_dbg(ATH6KL_DBG_USB,
+				   "%s Bulk Ep:0x%2.2X maxpktsz:%d\n",
+				   ATH6KL_USB_IS_DIR_IN
+				   (endpoint->bEndpointAddress) ?
+				   "RX" : "TX", endpoint->bEndpointAddress,
+				   le16_to_cpu(endpoint->wMaxPacketSize));
+		} else if (ATH6KL_USB_IS_INT_EP(endpoint->bmAttributes)) {
+			ath6kl_dbg(ATH6KL_DBG_USB,
+				   "%s Int Ep:0x%2.2X maxpktsz:%d interval:%d\n",
+				   ATH6KL_USB_IS_DIR_IN
+				   (endpoint->bEndpointAddress) ?
+				   "RX" : "TX", endpoint->bEndpointAddress,
+				   le16_to_cpu(endpoint->wMaxPacketSize),
+				   endpoint->bInterval);
+		} else if (ATH6KL_USB_IS_ISOC_EP(endpoint->bmAttributes)) {
+			/* TODO for ISO */
+			ath6kl_dbg(ATH6KL_DBG_USB,
+				   "%s ISOC Ep:0x%2.2X maxpktsz:%d interval:%d\n",
+				   ATH6KL_USB_IS_DIR_IN
+				   (endpoint->bEndpointAddress) ?
+				   "RX" : "TX", endpoint->bEndpointAddress,
+				   le16_to_cpu(endpoint->wMaxPacketSize),
+				   endpoint->bInterval);
+		}
+		urbcount = 0;
+
+		pipe_num =
+		    ath6kl_usb_get_logical_pipe_num(ar_usb,
+						    endpoint->bEndpointAddress,
+						    &urbcount);
+		if (pipe_num == ATH6KL_USB_PIPE_INVALID)
+			continue;
+
+		pipe = &ar_usb->pipes[pipe_num];
+		if (pipe->ar_usb != NULL) {
+			/* hmmm..pipe was already setup */
+			continue;
+		}
+
+		pipe->ar_usb = ar_usb;
+		pipe->logical_pipe_num = pipe_num;
+		pipe->ep_address = endpoint->bEndpointAddress;
+		pipe->max_packet_size = le16_to_cpu(endpoint->wMaxPacketSize);
+
+		if (ATH6KL_USB_IS_BULK_EP(endpoint->bmAttributes)) {
+			if (ATH6KL_USB_IS_DIR_IN(pipe->ep_address)) {
+				pipe->usb_pipe_handle =
+				    usb_rcvbulkpipe(ar_usb->udev,
+						    pipe->ep_address);
+			} else {
+				pipe->usb_pipe_handle =
+				    usb_sndbulkpipe(ar_usb->udev,
+						    pipe->ep_address);
+			}
+		} else if (ATH6KL_USB_IS_INT_EP(endpoint->bmAttributes)) {
+			if (ATH6KL_USB_IS_DIR_IN(pipe->ep_address)) {
+				pipe->usb_pipe_handle =
+				    usb_rcvintpipe(ar_usb->udev,
+						   pipe->ep_address);
+			} else {
+				pipe->usb_pipe_handle =
+				    usb_sndintpipe(ar_usb->udev,
+						   pipe->ep_address);
+			}
+		} else if (ATH6KL_USB_IS_ISOC_EP(endpoint->bmAttributes)) {
+			/* TODO for ISO */
+			if (ATH6KL_USB_IS_DIR_IN(pipe->ep_address)) {
+				pipe->usb_pipe_handle =
+				    usb_rcvisocpipe(ar_usb->udev,
+						    pipe->ep_address);
+			} else {
+				pipe->usb_pipe_handle =
+				    usb_sndisocpipe(ar_usb->udev,
+						    pipe->ep_address);
+			}
+		}
+
+		pipe->ep_desc = endpoint;
+
+		if (!ATH6KL_USB_IS_DIR_IN(pipe->ep_address))
+			pipe->flags |= ATH6KL_USB_PIPE_FLAG_TX;
+
+		status = ath6kl_usb_alloc_pipe_resources(pipe, urbcount);
+		if (status != 0)
+			break;
+	}
+
+	return status;
+}
+
+/* pipe operations */
+static void ath6kl_usb_post_recv_transfers(struct ath6kl_usb_pipe *recv_pipe,
+					   int buffer_length)
+{
+	struct ath6kl_urb_context *urb_context;
+	struct urb *urb;
+	int usb_status;
+
+	while (true) {
+		urb_context = ath6kl_usb_alloc_urb_from_pipe(recv_pipe);
+		if (urb_context == NULL)
+			break;
+
+		urb_context->skb = dev_alloc_skb(buffer_length);
+		if (urb_context->skb == NULL)
+			goto err_cleanup_urb;
+
+		urb = usb_alloc_urb(0, GFP_ATOMIC);
+		if (urb == NULL)
+			goto err_cleanup_urb;
+
+		usb_fill_bulk_urb(urb,
+				  recv_pipe->ar_usb->udev,
+				  recv_pipe->usb_pipe_handle,
+				  urb_context->skb->data,
+				  buffer_length,
+				  ath6kl_usb_recv_complete, urb_context);
+
+		ath6kl_dbg(ATH6KL_DBG_USB_BULK,
+			   "ath6kl usb: bulk recv submit:%d, 0x%X (ep:0x%2.2X), %d bytes buf:0x%p\n",
+			   recv_pipe->logical_pipe_num,
+			   recv_pipe->usb_pipe_handle, recv_pipe->ep_address,
+			   buffer_length, urb_context->skb);
+
+		usb_anchor_urb(urb, &recv_pipe->urb_submitted);
+		usb_status = usb_submit_urb(urb, GFP_ATOMIC);
+
+		if (usb_status) {
+			ath6kl_dbg(ATH6KL_DBG_USB_BULK,
+				   "ath6kl usb : usb bulk recv failed %d\n",
+				   usb_status);
+			usb_unanchor_urb(urb);
+			usb_free_urb(urb);
+			goto err_cleanup_urb;
+		}
+		usb_free_urb(urb);
+	}
+	return;
+
+err_cleanup_urb:
+	ath6kl_usb_cleanup_recv_urb(urb_context);
+	return;
+}
+
+static void ath6kl_usb_flush_all(struct ath6kl_usb *ar_usb)
+{
+	int i;
+
+	for (i = 0; i < ATH6KL_USB_PIPE_MAX; i++) {
+		if (ar_usb->pipes[i].ar_usb != NULL)
+			usb_kill_anchored_urbs(&ar_usb->pipes[i].urb_submitted);
+	}
+
+	/*
+	 * Flushing any pending I/O may schedule work this call will block
+	 * until all scheduled work runs to completion.
+	 */
+	flush_scheduled_work();
+}
+
+static void ath6kl_usb_start_recv_pipes(struct ath6kl_usb *ar_usb)
+{
+	/*
+	 * note: control pipe is no longer used
+	 * ar_usb->pipes[ATH6KL_USB_PIPE_RX_CTRL].urb_cnt_thresh =
+	 *      ar_usb->pipes[ATH6KL_USB_PIPE_RX_CTRL].urb_alloc/2;
+	 * ath6kl_usb_post_recv_transfers(&ar_usb->
+	 *		pipes[ATH6KL_USB_PIPE_RX_CTRL],
+	 *		ATH6KL_USB_RX_BUFFER_SIZE);
+	 */
+
+	ar_usb->pipes[ATH6KL_USB_PIPE_RX_DATA].urb_cnt_thresh =
+	    ar_usb->pipes[ATH6KL_USB_PIPE_RX_DATA].urb_alloc / 2;
+	ath6kl_usb_post_recv_transfers(&ar_usb->pipes[ATH6KL_USB_PIPE_RX_DATA],
+				       ATH6KL_USB_RX_BUFFER_SIZE);
+}
+
+/* hif usb rx/tx completion functions */
+static void ath6kl_usb_recv_complete(struct urb *urb)
+{
+	struct ath6kl_urb_context *urb_context = urb->context;
+	struct ath6kl_usb_pipe *pipe = urb_context->pipe;
+	struct sk_buff *skb = NULL;
+	int status = 0;
+
+	ath6kl_dbg(ATH6KL_DBG_USB_BULK,
+		   "%s: recv pipe: %d, stat:%d, len:%d urb:0x%p\n", __func__,
+		   pipe->logical_pipe_num, urb->status, urb->actual_length,
+		   urb);
+
+	if (urb->status != 0) {
+		status = -EIO;
+		switch (urb->status) {
+		case -ECONNRESET:
+		case -ENOENT:
+		case -ESHUTDOWN:
+			/*
+			 * no need to spew these errors when device
+			 * removed or urb killed due to driver shutdown
+			 */
+			status = -ECANCELED;
+			break;
+		default:
+			ath6kl_dbg(ATH6KL_DBG_USB_BULK,
+				   "%s recv pipe: %d (ep:0x%2.2X), failed:%d\n",
+				   __func__, pipe->logical_pipe_num,
+				   pipe->ep_address, urb->status);
+			break;
+		}
+		goto cleanup_recv_urb;
+	}
+
+	if (urb->actual_length == 0)
+		goto cleanup_recv_urb;
+
+	skb = urb_context->skb;
+
+	/* we are going to pass it up */
+	urb_context->skb = NULL;
+	skb_put(skb, urb->actual_length);
+
+	/* note: queue implements a lock */
+	skb_queue_tail(&pipe->io_comp_queue, skb);
+	schedule_work(&pipe->io_complete_work);
+
+cleanup_recv_urb:
+	ath6kl_usb_cleanup_recv_urb(urb_context);
+
+	if (status == 0 &&
+	    pipe->urb_cnt >= pipe->urb_cnt_thresh) {
+		/* our free urbs are piling up, post more transfers */
+		ath6kl_usb_post_recv_transfers(pipe, ATH6KL_USB_RX_BUFFER_SIZE);
+	}
+}
+
+static void ath6kl_usb_usb_transmit_complete(struct urb *urb)
+{
+	struct ath6kl_urb_context *urb_context = urb->context;
+	struct ath6kl_usb_pipe *pipe = urb_context->pipe;
+	struct sk_buff *skb;
+
+	ath6kl_dbg(ATH6KL_DBG_USB_BULK,
+		   "%s: pipe: %d, stat:%d, len:%d\n",
+		   __func__, pipe->logical_pipe_num, urb->status,
+		   urb->actual_length);
+
+	if (urb->status != 0) {
+		ath6kl_dbg(ATH6KL_DBG_USB_BULK,
+			   "%s:  pipe: %d, failed:%d\n",
+			   __func__, pipe->logical_pipe_num, urb->status);
+	}
+
+	skb = urb_context->skb;
+	urb_context->skb = NULL;
+	ath6kl_usb_free_urb_to_pipe(urb_context->pipe, urb_context);
+
+	/* note: queue implements a lock */
+	skb_queue_tail(&pipe->io_comp_queue, skb);
+	schedule_work(&pipe->io_complete_work);
+}
+
+static void ath6kl_usb_io_comp_work(struct work_struct *work)
+{
+	struct ath6kl_usb_pipe *pipe = container_of(work,
+						    struct ath6kl_usb_pipe,
+						    io_complete_work);
+	struct ath6kl_usb *ar_usb;
+	struct sk_buff *skb;
+
+	ar_usb = pipe->ar_usb;
+
+	while ((skb = skb_dequeue(&pipe->io_comp_queue))) {
+		if (pipe->flags & ATH6KL_USB_PIPE_FLAG_TX) {
+			ath6kl_dbg(ATH6KL_DBG_USB_BULK,
+				   "ath6kl usb xmit callback buf:0x%p\n", skb);
+			ath6kl_core_tx_complete(ar_usb->ar, skb);
+		} else {
+			ath6kl_dbg(ATH6KL_DBG_USB_BULK,
+				   "ath6kl usb recv callback buf:0x%p\n", skb);
+			ath6kl_core_rx_complete(ar_usb->ar, skb,
+						pipe->logical_pipe_num);
+		}
+	}
+}
+
 #define ATH6KL_USB_MAX_DIAG_CMD (sizeof(struct ath6kl_usb_ctrl_diag_cmd_write))
 #define ATH6KL_USB_MAX_DIAG_RESP (sizeof(struct ath6kl_usb_ctrl_diag_resp_read))
 
 static void ath6kl_usb_destroy(struct ath6kl_usb *ar_usb)
 {
+	ath6kl_usb_flush_all(ar_usb);
+
+	ath6kl_usb_cleanup_pipe_resources(ar_usb);
+
 	usb_set_intfdata(ar_usb->interface, NULL);
 
 	kfree(ar_usb->diag_cmd_buffer);
@@ -70,19 +614,28 @@ static void ath6kl_usb_destroy(struct ath6kl_usb *ar_usb)
 
 static struct ath6kl_usb *ath6kl_usb_create(struct usb_interface *interface)
 {
-	struct ath6kl_usb *ar_usb = NULL;
 	struct usb_device *dev = interface_to_usbdev(interface);
+	struct ath6kl_usb *ar_usb;
+	struct ath6kl_usb_pipe *pipe;
 	int status = 0;
+	int i;
 
 	ar_usb = kzalloc(sizeof(struct ath6kl_usb), GFP_KERNEL);
 	if (ar_usb == NULL)
 		goto fail_ath6kl_usb_create;
 
-	memset(ar_usb, 0, sizeof(struct ath6kl_usb));
 	usb_set_intfdata(interface, ar_usb);
+	spin_lock_init(&(ar_usb->cs_lock));
 	ar_usb->udev = dev;
 	ar_usb->interface = interface;
 
+	for (i = 0; i < ATH6KL_USB_PIPE_MAX; i++) {
+		pipe = &ar_usb->pipes[i];
+		INIT_WORK(&pipe->io_complete_work,
+			  ath6kl_usb_io_comp_work);
+		skb_queue_head_init(&pipe->io_comp_queue);
+	}
+
 	ar_usb->diag_cmd_buffer = kzalloc(ATH6KL_USB_MAX_DIAG_CMD, GFP_KERNEL);
 	if (ar_usb->diag_cmd_buffer == NULL) {
 		status = -ENOMEM;
@@ -96,6 +649,8 @@ static struct ath6kl_usb *ath6kl_usb_create(struct usb_interface *interface)
 		goto fail_ath6kl_usb_create;
 	}
 
+	status = ath6kl_usb_setup_pipe_resources(ar_usb);
+
 fail_ath6kl_usb_create:
 	if (status != 0) {
 		ath6kl_usb_destroy(ar_usb);
@@ -114,11 +669,177 @@ static void ath6kl_usb_device_detached(struct usb_interface *interface)
 
 	ath6kl_stop_txrx(ar_usb->ar);
 
+	/* Delay to wait for the target to reboot */
+	mdelay(20);
 	ath6kl_core_cleanup(ar_usb->ar);
-
 	ath6kl_usb_destroy(ar_usb);
 }
 
+/* exported hif usb APIs for htc pipe */
+static void hif_start(struct ath6kl *ar)
+{
+	struct ath6kl_usb *device = ath6kl_usb_priv(ar);
+	int i;
+
+	ath6kl_usb_start_recv_pipes(device);
+
+	/* set the TX resource avail threshold for each TX pipe */
+	for (i = ATH6KL_USB_PIPE_TX_CTRL;
+	     i <= ATH6KL_USB_PIPE_TX_DATA_HP; i++) {
+		device->pipes[i].urb_cnt_thresh =
+		    device->pipes[i].urb_alloc / 2;
+	}
+}
+
+static int ath6kl_usb_send(struct ath6kl *ar, u8 PipeID,
+			   struct sk_buff *hdr_skb, struct sk_buff *skb)
+{
+	struct ath6kl_usb *device = ath6kl_usb_priv(ar);
+	struct ath6kl_usb_pipe *pipe = &device->pipes[PipeID];
+	struct ath6kl_urb_context *urb_context;
+	int usb_status, status = 0;
+	struct urb *urb;
+	u8 *data;
+	u32 len;
+
+	ath6kl_dbg(ATH6KL_DBG_USB_BULK, "+%s pipe : %d, buf:0x%p\n",
+		   __func__, PipeID, skb);
+
+	urb_context = ath6kl_usb_alloc_urb_from_pipe(pipe);
+
+	if (urb_context == NULL) {
+		/*
+		 * TODO: it is possible to run out of urbs if
+		 * 2 endpoints map to the same pipe ID
+		 */
+		ath6kl_dbg(ATH6KL_DBG_USB_BULK,
+			   "%s pipe:%d no urbs left. URB Cnt : %d\n",
+			   __func__, PipeID, pipe->urb_cnt);
+		status = -ENOMEM;
+		goto fail_hif_send;
+	}
+
+	urb_context->skb = skb;
+
+	data = skb->data;
+	len = skb->len;
+
+	urb = usb_alloc_urb(0, GFP_ATOMIC);
+	if (urb == NULL) {
+		status = -ENOMEM;
+		ath6kl_usb_free_urb_to_pipe(urb_context->pipe,
+					    urb_context);
+		goto fail_hif_send;
+	}
+
+	usb_fill_bulk_urb(urb,
+			  device->udev,
+			  pipe->usb_pipe_handle,
+			  data,
+			  len,
+			  ath6kl_usb_usb_transmit_complete, urb_context);
+
+	if ((len % pipe->max_packet_size) == 0) {
+		/* hit a max packet boundary on this pipe */
+		urb->transfer_flags |= URB_ZERO_PACKET;
+	}
+
+	ath6kl_dbg(ATH6KL_DBG_USB_BULK,
+		   "athusb bulk send submit:%d, 0x%X (ep:0x%2.2X), %d bytes\n",
+		   pipe->logical_pipe_num, pipe->usb_pipe_handle,
+		   pipe->ep_address, len);
+
+	usb_anchor_urb(urb, &pipe->urb_submitted);
+	usb_status = usb_submit_urb(urb, GFP_ATOMIC);
+
+	if (usb_status) {
+		ath6kl_dbg(ATH6KL_DBG_USB_BULK,
+			   "ath6kl usb : usb bulk transmit failed %d\n",
+			   usb_status);
+		usb_unanchor_urb(urb);
+		ath6kl_usb_free_urb_to_pipe(urb_context->pipe,
+					    urb_context);
+		status = -EINVAL;
+	}
+	usb_free_urb(urb);
+
+fail_hif_send:
+	return status;
+}
+
+static void hif_stop(struct ath6kl *ar)
+{
+	struct ath6kl_usb *device = ath6kl_usb_priv(ar);
+
+	ath6kl_usb_flush_all(device);
+}
+
+static void ath6kl_usb_get_default_pipe(struct ath6kl *ar,
+					u8 *ul_pipe, u8 *dl_pipe)
+{
+	*ul_pipe = ATH6KL_USB_PIPE_TX_CTRL;
+	*dl_pipe = ATH6KL_USB_PIPE_RX_CTRL;
+}
+
+static int ath6kl_usb_map_service_pipe(struct ath6kl *ar, u16 svc_id,
+				       u8 *ul_pipe, u8 *dl_pipe)
+{
+	int status = 0;
+
+	switch (svc_id) {
+	case HTC_CTRL_RSVD_SVC:
+	case WMI_CONTROL_SVC:
+		*ul_pipe = ATH6KL_USB_PIPE_TX_CTRL;
+		/* due to large control packets, shift to data pipe */
+		*dl_pipe = ATH6KL_USB_PIPE_RX_DATA;
+		break;
+	case WMI_DATA_BE_SVC:
+	case WMI_DATA_BK_SVC:
+		*ul_pipe = ATH6KL_USB_PIPE_TX_DATA_LP;
+		/*
+		* Disable rxdata2 directly, it will be enabled
+		* if FW enable rxdata2
+		*/
+		*dl_pipe = ATH6KL_USB_PIPE_RX_DATA;
+		break;
+	case WMI_DATA_VI_SVC:
+		*ul_pipe = ATH6KL_USB_PIPE_TX_DATA_MP;
+		/*
+		* Disable rxdata2 directly, it will be enabled
+		* if FW enable rxdata2
+		*/
+		*dl_pipe = ATH6KL_USB_PIPE_RX_DATA;
+		break;
+	case WMI_DATA_VO_SVC:
+		*ul_pipe = ATH6KL_USB_PIPE_TX_DATA_HP;
+		/*
+		* Disable rxdata2 directly, it will be enabled
+		* if FW enable rxdata2
+		*/
+		*dl_pipe = ATH6KL_USB_PIPE_RX_DATA;
+		break;
+	default:
+		status = -EPERM;
+		break;
+	}
+
+	return status;
+}
+
+static u16 ath6kl_usb_get_free_queue_number(struct ath6kl *ar, u8 pipe_id)
+{
+	struct ath6kl_usb *device = ath6kl_usb_priv(ar);
+
+	return device->pipes[pipe_id].urb_cnt;
+}
+
+static void hif_detach_htc(struct ath6kl *ar)
+{
+	struct ath6kl_usb *device = ath6kl_usb_priv(ar);
+
+	ath6kl_usb_flush_all(device);
+}
+
 static int ath6kl_usb_submit_ctrl_out(struct ath6kl_usb *ar_usb,
 				   u8 req, u16 value, u16 index, void *data,
 				   u32 size)
@@ -301,14 +1022,29 @@ static int ath6kl_usb_bmi_write(struct ath6kl *ar, u8 *buf, u32 len)
 
 static int ath6kl_usb_power_on(struct ath6kl *ar)
 {
+	hif_start(ar);
 	return 0;
 }
 
 static int ath6kl_usb_power_off(struct ath6kl *ar)
 {
+	hif_detach_htc(ar);
 	return 0;
 }
 
+static void ath6kl_usb_stop(struct ath6kl *ar)
+{
+	hif_stop(ar);
+}
+
+static void ath6kl_usb_cleanup_scatter(struct ath6kl *ar)
+{
+	/*
+	 * USB doesn't support it. Just return.
+	 */
+	return;
+}
+
 static const struct ath6kl_hif_ops ath6kl_usb_ops = {
 	.diag_read32 = ath6kl_usb_diag_read32,
 	.diag_write32 = ath6kl_usb_diag_write32,
@@ -316,6 +1052,12 @@ static const struct ath6kl_hif_ops ath6kl_usb_ops = {
 	.bmi_write = ath6kl_usb_bmi_write,
 	.power_on = ath6kl_usb_power_on,
 	.power_off = ath6kl_usb_power_off,
+	.stop = ath6kl_usb_stop,
+	.pipe_send = ath6kl_usb_send,
+	.pipe_get_default = ath6kl_usb_get_default_pipe,
+	.pipe_map_service = ath6kl_usb_map_service_pipe,
+	.pipe_get_free_queue_number = ath6kl_usb_get_free_queue_number,
+	.cleanup_scatter = ath6kl_usb_cleanup_scatter,
 };
 
 /* ath6kl usb driver registered functions */
@@ -368,7 +1110,7 @@ static int ath6kl_usb_probe(struct usb_interface *interface,
 
 	ar_usb->ar = ar;
 
-	ret = ath6kl_core_init(ar);
+	ret = ath6kl_core_init(ar, ATH6KL_HTC_TYPE_PIPE);
 	if (ret) {
 		ath6kl_err("Failed to init ath6kl core: %d\n", ret);
 		goto err_core_free;
@@ -392,6 +1134,46 @@ static void ath6kl_usb_remove(struct usb_interface *interface)
 	ath6kl_usb_device_detached(interface);
 }
 
+#ifdef CONFIG_PM
+
+static int ath6kl_usb_suspend(struct usb_interface *interface,
+			      pm_message_t message)
+{
+	struct ath6kl_usb *device;
+	device = usb_get_intfdata(interface);
+
+	ath6kl_usb_flush_all(device);
+	return 0;
+}
+
+static int ath6kl_usb_resume(struct usb_interface *interface)
+{
+	struct ath6kl_usb *device;
+	device = usb_get_intfdata(interface);
+
+	ath6kl_usb_post_recv_transfers(&device->pipes[ATH6KL_USB_PIPE_RX_DATA],
+				       ATH6KL_USB_RX_BUFFER_SIZE);
+	ath6kl_usb_post_recv_transfers(&device->pipes[ATH6KL_USB_PIPE_RX_DATA2],
+				       ATH6KL_USB_RX_BUFFER_SIZE);
+
+	return 0;
+}
+
+static int ath6kl_usb_reset_resume(struct usb_interface *intf)
+{
+	if (usb_get_intfdata(intf))
+		ath6kl_usb_remove(intf);
+	return 0;
+}
+
+#else
+
+#define ath6kl_usb_suspend NULL
+#define ath6kl_usb_resume NULL
+#define ath6kl_usb_reset_resume NULL
+
+#endif
+
 /* table of devices that work with this driver */
 static struct usb_device_id ath6kl_usb_ids[] = {
 	{USB_DEVICE(0x0cf3, 0x9374)},
@@ -403,8 +1185,13 @@ MODULE_DEVICE_TABLE(usb, ath6kl_usb_ids);
 static struct usb_driver ath6kl_usb_driver = {
 	.name = "ath6kl_usb",
 	.probe = ath6kl_usb_probe,
+	.suspend = ath6kl_usb_suspend,
+	.resume = ath6kl_usb_resume,
+	.reset_resume = ath6kl_usb_reset_resume,
 	.disconnect = ath6kl_usb_remove,
 	.id_table = ath6kl_usb_ids,
+	.supports_autosuspend = true,
+	.disable_hub_initiated_lpm = 1,
 };
 
 static int ath6kl_usb_init(void)
@@ -430,3 +1217,6 @@ MODULE_FIRMWARE(AR6004_HW_1_0_DEFAULT_BOARD_DATA_FILE);
 MODULE_FIRMWARE(AR6004_HW_1_1_FIRMWARE_FILE);
 MODULE_FIRMWARE(AR6004_HW_1_1_BOARD_DATA_FILE);
 MODULE_FIRMWARE(AR6004_HW_1_1_DEFAULT_BOARD_DATA_FILE);
+MODULE_FIRMWARE(AR6004_HW_1_2_FIRMWARE_FILE);
+MODULE_FIRMWARE(AR6004_HW_1_2_BOARD_DATA_FILE);
+MODULE_FIRMWARE(AR6004_HW_1_2_DEFAULT_BOARD_DATA_FILE);
diff --git a/drivers/net/wireless/ath/ath6kl/wmi.c b/drivers/net/wireless/ath/ath6kl/wmi.c
index 2b442332cd0..ee8ec2394c2 100644
--- a/drivers/net/wireless/ath/ath6kl/wmi.c
+++ b/drivers/net/wireless/ath/ath6kl/wmi.c
@@ -16,6 +16,7 @@
  */
 
 #include <linux/ip.h>
+#include <linux/in.h>
 #include "core.h"
 #include "debug.h"
 #include "testmode.h"
@@ -289,6 +290,13 @@ int ath6kl_wmi_implicit_create_pstream(struct wmi *wmi, u8 if_idx,
 					layer2_priority);
 		} else
 			usr_pri = layer2_priority & 0x7;
+
+		/*
+		 * Queue the EAPOL frames in the same WMM_AC_VO queue
+		 * as that of management frames.
+		 */
+		if (skb->protocol == cpu_to_be16(ETH_P_PAE))
+			usr_pri = WMI_VOICE_USER_PRIORITY;
 	}
 
 	/*
@@ -460,8 +468,9 @@ static int ath6kl_wmi_remain_on_chnl_event_rx(struct wmi *wmi, u8 *datap,
 		   freq, dur);
 	chan = ieee80211_get_channel(ar->wiphy, freq);
 	if (!chan) {
-		ath6kl_dbg(ATH6KL_DBG_WMI, "remain_on_chnl: Unknown channel "
-			   "(freq=%u)\n", freq);
+		ath6kl_dbg(ATH6KL_DBG_WMI,
+			   "remain_on_chnl: Unknown channel (freq=%u)\n",
+			   freq);
 		return -EINVAL;
 	}
 	id = vif->last_roc_id;
@@ -488,12 +497,14 @@ static int ath6kl_wmi_cancel_remain_on_chnl_event_rx(struct wmi *wmi,
 	ev = (struct wmi_cancel_remain_on_chnl_event *) datap;
 	freq = le32_to_cpu(ev->freq);
 	dur = le32_to_cpu(ev->duration);
-	ath6kl_dbg(ATH6KL_DBG_WMI, "cancel_remain_on_chnl: freq=%u dur=%u "
-		   "status=%u\n", freq, dur, ev->status);
+	ath6kl_dbg(ATH6KL_DBG_WMI,
+		   "cancel_remain_on_chnl: freq=%u dur=%u status=%u\n",
+		   freq, dur, ev->status);
 	chan = ieee80211_get_channel(ar->wiphy, freq);
 	if (!chan) {
-		ath6kl_dbg(ATH6KL_DBG_WMI, "cancel_remain_on_chnl: Unknown "
-			   "channel (freq=%u)\n", freq);
+		ath6kl_dbg(ATH6KL_DBG_WMI,
+			   "cancel_remain_on_chnl: Unknown channel (freq=%u)\n",
+			   freq);
 		return -EINVAL;
 	}
 	if (vif->last_cancel_roc_id &&
@@ -548,12 +559,12 @@ static int ath6kl_wmi_rx_probe_req_event_rx(struct wmi *wmi, u8 *datap, int len,
 	freq = le32_to_cpu(ev->freq);
 	dlen = le16_to_cpu(ev->len);
 	if (datap + len < ev->data + dlen) {
-		ath6kl_err("invalid wmi_p2p_rx_probe_req_event: "
-			   "len=%d dlen=%u\n", len, dlen);
+		ath6kl_err("invalid wmi_p2p_rx_probe_req_event: len=%d dlen=%u\n",
+			   len, dlen);
 		return -EINVAL;
 	}
-	ath6kl_dbg(ATH6KL_DBG_WMI, "rx_probe_req: len=%u freq=%u "
-		   "probe_req_report=%d\n",
+	ath6kl_dbg(ATH6KL_DBG_WMI,
+		   "rx_probe_req: len=%u freq=%u probe_req_report=%d\n",
 		   dlen, freq, vif->probe_req_report);
 
 	if (vif->probe_req_report || vif->nw_type == AP_NETWORK)
@@ -592,8 +603,8 @@ static int ath6kl_wmi_rx_action_event_rx(struct wmi *wmi, u8 *datap, int len,
 	freq = le32_to_cpu(ev->freq);
 	dlen = le16_to_cpu(ev->len);
 	if (datap + len < ev->data + dlen) {
-		ath6kl_err("invalid wmi_rx_action_event: "
-			   "len=%d dlen=%u\n", len, dlen);
+		ath6kl_err("invalid wmi_rx_action_event: len=%d dlen=%u\n",
+			   len, dlen);
 		return -EINVAL;
 	}
 	ath6kl_dbg(ATH6KL_DBG_WMI, "rx_action: len=%u freq=%u\n", dlen, freq);
@@ -687,7 +698,7 @@ static int ath6kl_wmi_ready_event_rx(struct wmi *wmi, u8 *datap, int len)
 
 	ath6kl_ready_event(wmi->parent_dev, ev->mac_addr,
 			   le32_to_cpu(ev->sw_version),
-			   le32_to_cpu(ev->abi_version));
+			   le32_to_cpu(ev->abi_version), ev->phy_cap);
 
 	return 0;
 }
@@ -777,16 +788,15 @@ static int ath6kl_wmi_connect_event_rx(struct wmi *wmi, u8 *datap, int len,
 		/* AP mode start/STA connected event */
 		struct net_device *dev = vif->ndev;
 		if (memcmp(dev->dev_addr, ev->u.ap_bss.bssid, ETH_ALEN) == 0) {
-			ath6kl_dbg(ATH6KL_DBG_WMI, "%s: freq %d bssid %pM "
-				   "(AP started)\n",
+			ath6kl_dbg(ATH6KL_DBG_WMI,
+				   "%s: freq %d bssid %pM (AP started)\n",
 				   __func__, le16_to_cpu(ev->u.ap_bss.ch),
 				   ev->u.ap_bss.bssid);
 			ath6kl_connect_ap_mode_bss(
 				vif, le16_to_cpu(ev->u.ap_bss.ch));
 		} else {
-			ath6kl_dbg(ATH6KL_DBG_WMI, "%s: aid %u mac_addr %pM "
-				   "auth=%u keymgmt=%u cipher=%u apsd_info=%u "
-				   "(STA connected)\n",
+			ath6kl_dbg(ATH6KL_DBG_WMI,
+				   "%s: aid %u mac_addr %pM auth=%u keymgmt=%u cipher=%u apsd_info=%u (STA connected)\n",
 				   __func__, ev->u.ap_sta.aid,
 				   ev->u.ap_sta.mac_addr,
 				   ev->u.ap_sta.auth,
@@ -1229,8 +1239,9 @@ static int ath6kl_wmi_neighbor_report_event_rx(struct wmi *wmi, u8 *datap,
 	ev = (struct wmi_neighbor_report_event *) datap;
 	if (sizeof(*ev) + ev->num_neighbors * sizeof(struct wmi_neighbor_info)
 	    > len) {
-		ath6kl_dbg(ATH6KL_DBG_WMI, "truncated neighbor event "
-			   "(num=%d len=%d)\n", ev->num_neighbors, len);
+		ath6kl_dbg(ATH6KL_DBG_WMI,
+			   "truncated neighbor event (num=%d len=%d)\n",
+			   ev->num_neighbors, len);
 		return -EINVAL;
 	}
 	for (i = 0; i < ev->num_neighbors; i++) {
@@ -1814,12 +1825,14 @@ int ath6kl_wmi_beginscan_cmd(struct wmi *wmi, u8 if_idx,
 			     u32 home_dwell_time, u32 force_scan_interval,
 			     s8 num_chan, u16 *ch_list, u32 no_cck, u32 *rates)
 {
+	struct ieee80211_supported_band *sband;
 	struct sk_buff *skb;
 	struct wmi_begin_scan_cmd *sc;
-	s8 size;
+	s8 size, *supp_rates;
 	int i, band, ret;
 	struct ath6kl *ar = wmi->parent_dev;
 	int num_rates;
+	u32 ratemask;
 
 	size = sizeof(struct wmi_begin_scan_cmd);
 
@@ -1846,10 +1859,13 @@ int ath6kl_wmi_beginscan_cmd(struct wmi *wmi, u8 if_idx,
 	sc->num_ch = num_chan;
 
 	for (band = 0; band < IEEE80211_NUM_BANDS; band++) {
-		struct ieee80211_supported_band *sband =
-		    ar->wiphy->bands[band];
-		u32 ratemask = rates[band];
-		u8 *supp_rates = sc->supp_rates[band].rates;
+		sband = ar->wiphy->bands[band];
+
+		if (!sband)
+			continue;
+
+		ratemask = rates[band];
+		supp_rates = sc->supp_rates[band].rates;
 		num_rates = 0;
 
 		for (i = 0; i < sband->n_bitrates; i++) {
@@ -2129,8 +2145,8 @@ int ath6kl_wmi_addkey_cmd(struct wmi *wmi, u8 if_idx, u8 key_index,
 	struct wmi_add_cipher_key_cmd *cmd;
 	int ret;
 
-	ath6kl_dbg(ATH6KL_DBG_WMI, "addkey cmd: key_index=%u key_type=%d "
-		   "key_usage=%d key_len=%d key_op_ctrl=%d\n",
+	ath6kl_dbg(ATH6KL_DBG_WMI,
+		   "addkey cmd: key_index=%u key_type=%d key_usage=%d key_len=%d key_op_ctrl=%d\n",
 		   key_index, key_type, key_usage, key_len, key_op_ctrl);
 
 	if ((key_index > WMI_MAX_KEY_INDEX) || (key_len > WMI_MAX_KEY_LEN) ||
@@ -2882,6 +2898,43 @@ int ath6kl_wmi_set_keepalive_cmd(struct wmi *wmi, u8 if_idx,
 	return ret;
 }
 
+int ath6kl_wmi_set_htcap_cmd(struct wmi *wmi, u8 if_idx,
+			     enum ieee80211_band band,
+			     struct ath6kl_htcap *htcap)
+{
+	struct sk_buff *skb;
+	struct wmi_set_htcap_cmd *cmd;
+
+	skb = ath6kl_wmi_get_new_buf(sizeof(*cmd));
+	if (!skb)
+		return -ENOMEM;
+
+	cmd = (struct wmi_set_htcap_cmd *) skb->data;
+
+	/*
+	 * NOTE: Band in firmware matches enum ieee80211_band, it is unlikely
+	 * this will be changed in firmware. If at all there is any change in
+	 * band value, the host needs to be fixed.
+	 */
+	cmd->band = band;
+	cmd->ht_enable = !!htcap->ht_enable;
+	cmd->ht20_sgi = !!(htcap->cap_info & IEEE80211_HT_CAP_SGI_20);
+	cmd->ht40_supported =
+		!!(htcap->cap_info & IEEE80211_HT_CAP_SUP_WIDTH_20_40);
+	cmd->ht40_sgi = !!(htcap->cap_info & IEEE80211_HT_CAP_SGI_40);
+	cmd->intolerant_40mhz =
+		!!(htcap->cap_info & IEEE80211_HT_CAP_40MHZ_INTOLERANT);
+	cmd->max_ampdu_len_exp = htcap->ampdu_factor;
+
+	ath6kl_dbg(ATH6KL_DBG_WMI,
+		   "Set htcap: band:%d ht_enable:%d 40mhz:%d sgi_20mhz:%d sgi_40mhz:%d 40mhz_intolerant:%d ampdu_len_exp:%d\n",
+		   cmd->band, cmd->ht_enable, cmd->ht40_supported,
+		   cmd->ht20_sgi, cmd->ht40_sgi, cmd->intolerant_40mhz,
+		   cmd->max_ampdu_len_exp);
+	return ath6kl_wmi_cmd_send(wmi, if_idx, skb, WMI_SET_HT_CAP_CMDID,
+				   NO_SYNC_WMIFLAG);
+}
+
 int ath6kl_wmi_test_cmd(struct wmi *wmi, void *buf, size_t len)
 {
 	struct sk_buff *skb;
@@ -3010,8 +3063,8 @@ int ath6kl_wmi_ap_profile_commit(struct wmi *wmip, u8 if_idx,
 
 	res = ath6kl_wmi_cmd_send(wmip, if_idx, skb, WMI_AP_CONFIG_COMMIT_CMDID,
 				  NO_SYNC_WMIFLAG);
-	ath6kl_dbg(ATH6KL_DBG_WMI, "%s: nw_type=%u auth_mode=%u ch=%u "
-		   "ctrl_flags=0x%x-> res=%d\n",
+	ath6kl_dbg(ATH6KL_DBG_WMI,
+		   "%s: nw_type=%u auth_mode=%u ch=%u ctrl_flags=0x%x-> res=%d\n",
 		   __func__, p->nw_type, p->auth_mode, le16_to_cpu(p->ch),
 		   le32_to_cpu(p->ctrl_flags), res);
 	return res;
@@ -3032,6 +3085,9 @@ int ath6kl_wmi_ap_set_mlme(struct wmi *wmip, u8 if_idx, u8 cmd, const u8 *mac,
 	cm->reason = cpu_to_le16(reason);
 	cm->cmd = cmd;
 
+	ath6kl_dbg(ATH6KL_DBG_WMI, "ap_set_mlme: cmd=%d reason=%d\n", cm->cmd,
+		   cm->reason);
+
 	return ath6kl_wmi_cmd_send(wmip, if_idx, skb, WMI_AP_SET_MLME_CMDID,
 				   NO_SYNC_WMIFLAG);
 }
@@ -3168,8 +3224,9 @@ int ath6kl_wmi_set_appie_cmd(struct wmi *wmi, u8 if_idx, u8 mgmt_frm_type,
 	if (!skb)
 		return -ENOMEM;
 
-	ath6kl_dbg(ATH6KL_DBG_WMI, "set_appie_cmd: mgmt_frm_type=%u "
-		   "ie_len=%u\n", mgmt_frm_type, ie_len);
+	ath6kl_dbg(ATH6KL_DBG_WMI,
+		   "set_appie_cmd: mgmt_frm_type=%u ie_len=%u\n",
+		   mgmt_frm_type, ie_len);
 	p = (struct wmi_set_appie_cmd *) skb->data;
 	p->mgmt_frm_type = mgmt_frm_type;
 	p->ie_len = ie_len;
@@ -3181,6 +3238,29 @@ int ath6kl_wmi_set_appie_cmd(struct wmi *wmi, u8 if_idx, u8 mgmt_frm_type,
 				   NO_SYNC_WMIFLAG);
 }
 
+int ath6kl_wmi_set_ie_cmd(struct wmi *wmi, u8 if_idx, u8 ie_id, u8 ie_field,
+			  const u8 *ie_info, u8 ie_len)
+{
+	struct sk_buff *skb;
+	struct wmi_set_ie_cmd *p;
+
+	skb = ath6kl_wmi_get_new_buf(sizeof(*p) + ie_len);
+	if (!skb)
+		return -ENOMEM;
+
+	ath6kl_dbg(ATH6KL_DBG_WMI, "set_ie_cmd: ie_id=%u ie_ie_field=%u ie_len=%u\n",
+		   ie_id, ie_field, ie_len);
+	p = (struct wmi_set_ie_cmd *) skb->data;
+	p->ie_id = ie_id;
+	p->ie_field = ie_field;
+	p->ie_len = ie_len;
+	if (ie_info && ie_len > 0)
+		memcpy(p->ie_info, ie_info, ie_len);
+
+	return ath6kl_wmi_cmd_send(wmi, if_idx, skb, WMI_SET_IE_CMDID,
+				   NO_SYNC_WMIFLAG);
+}
+
 int ath6kl_wmi_disable_11b_rates_cmd(struct wmi *wmi, bool disable)
 {
 	struct sk_buff *skb;
@@ -3247,8 +3327,9 @@ static int ath6kl_wmi_send_action_cmd(struct wmi *wmi, u8 if_idx, u32 id,
 	wmi->last_mgmt_tx_frame = buf;
 	wmi->last_mgmt_tx_frame_len = data_len;
 
-	ath6kl_dbg(ATH6KL_DBG_WMI, "send_action_cmd: id=%u freq=%u wait=%u "
-		   "len=%u\n", id, freq, wait, data_len);
+	ath6kl_dbg(ATH6KL_DBG_WMI,
+		   "send_action_cmd: id=%u freq=%u wait=%u len=%u\n",
+		   id, freq, wait, data_len);
 	p = (struct wmi_send_action_cmd *) skb->data;
 	p->id = cpu_to_le32(id);
 	p->freq = cpu_to_le32(freq);
@@ -3285,8 +3366,9 @@ static int __ath6kl_wmi_send_mgmt_cmd(struct wmi *wmi, u8 if_idx, u32 id,
 	wmi->last_mgmt_tx_frame = buf;
 	wmi->last_mgmt_tx_frame_len = data_len;
 
-	ath6kl_dbg(ATH6KL_DBG_WMI, "send_action_cmd: id=%u freq=%u wait=%u "
-		   "len=%u\n", id, freq, wait, data_len);
+	ath6kl_dbg(ATH6KL_DBG_WMI,
+		   "send_action_cmd: id=%u freq=%u wait=%u len=%u\n",
+		   id, freq, wait, data_len);
 	p = (struct wmi_send_mgmt_cmd *) skb->data;
 	p->id = cpu_to_le32(id);
 	p->freq = cpu_to_le32(freq);
@@ -3339,8 +3421,9 @@ int ath6kl_wmi_send_probe_response_cmd(struct wmi *wmi, u8 if_idx, u32 freq,
 	if (!skb)
 		return -ENOMEM;
 
-	ath6kl_dbg(ATH6KL_DBG_WMI, "send_probe_response_cmd: freq=%u dst=%pM "
-		   "len=%u\n", freq, dst, data_len);
+	ath6kl_dbg(ATH6KL_DBG_WMI,
+		   "send_probe_response_cmd: freq=%u dst=%pM len=%u\n",
+		   freq, dst, data_len);
 	p = (struct wmi_p2p_probe_response_cmd *) skb->data;
 	p->freq = cpu_to_le32(freq);
 	memcpy(p->destination_addr, dst, ETH_ALEN);
@@ -3392,6 +3475,23 @@ int ath6kl_wmi_cancel_remain_on_chnl_cmd(struct wmi *wmi, u8 if_idx)
 				     WMI_CANCEL_REMAIN_ON_CHNL_CMDID);
 }
 
+int ath6kl_wmi_set_inact_period(struct wmi *wmi, u8 if_idx, int inact_timeout)
+{
+	struct sk_buff *skb;
+	struct wmi_set_inact_period_cmd *cmd;
+
+	skb = ath6kl_wmi_get_new_buf(sizeof(*cmd));
+	if (!skb)
+		return -ENOMEM;
+
+	cmd = (struct wmi_set_inact_period_cmd *) skb->data;
+	cmd->inact_period = cpu_to_le32(inact_timeout);
+	cmd->num_null_func = 0;
+
+	return ath6kl_wmi_cmd_send(wmi, if_idx, skb, WMI_AP_CONN_INACT_CMDID,
+				   NO_SYNC_WMIFLAG);
+}
+
 static int ath6kl_wmi_control_rx_xtnd(struct wmi *wmi, struct sk_buff *skb)
 {
 	struct wmix_cmd_hdr *cmd;
diff --git a/drivers/net/wireless/ath/ath6kl/wmi.h b/drivers/net/wireless/ath/ath6kl/wmi.h
index 4092e3e8079..9076bec3a2b 100644
--- a/drivers/net/wireless/ath/ath6kl/wmi.h
+++ b/drivers/net/wireless/ath/ath6kl/wmi.h
@@ -106,6 +106,8 @@ struct wmi_data_sync_bufs {
 #define WMM_AC_VI   2		/* video */
 #define WMM_AC_VO   3		/* voice */
 
+#define WMI_VOICE_USER_PRIORITY		0x7
+
 struct wmi {
 	u16 stream_exist_for_ac[WMM_NUM_AC];
 	u8 fat_pipe_exist;
@@ -182,6 +184,9 @@ enum wmi_data_hdr_flags {
 #define WMI_DATA_HDR_META_MASK      0x7
 #define WMI_DATA_HDR_META_SHIFT     13
 
+#define WMI_DATA_HDR_PAD_BEFORE_DATA_MASK               0xFF
+#define WMI_DATA_HDR_PAD_BEFORE_DATA_SHIFT              0x8
+
 /* Macros for operating on WMI_DATA_HDR (info3) field */
 #define WMI_DATA_HDR_IF_IDX_MASK    0xF
 
@@ -423,6 +428,7 @@ enum wmi_cmd_id {
 	WMI_SET_FRAMERATES_CMDID,
 	WMI_SET_AP_PS_CMDID,
 	WMI_SET_QOS_SUPP_CMDID,
+	WMI_SET_IE_CMDID,
 
 	/* WMI_THIN_RESERVED_... mark the start and end
 	 * values for WMI_THIN_RESERVED command IDs. These
@@ -629,6 +635,11 @@ enum wmi_mgmt_frame_type {
 	WMI_NUM_MGMT_FRAME
 };
 
+enum wmi_ie_field_type {
+	WMI_RSN_IE_CAPB	= 0x1,
+	WMI_IE_FULL	= 0xFF,  /* indicats full IE */
+};
+
 /* WMI_CONNECT_CMDID  */
 enum network_type {
 	INFRA_NETWORK = 0x01,
@@ -1142,6 +1153,7 @@ enum wmi_phy_mode {
 	WMI_11AG_MODE = 0x3,
 	WMI_11B_MODE = 0x4,
 	WMI_11GONLY_MODE = 0x5,
+	WMI_11G_HT20	= 0x6,
 };
 
 #define WMI_MAX_CHANNELS        32
@@ -1268,6 +1280,16 @@ struct wmi_mcast_filter_add_del_cmd {
 	u8 mcast_mac[ATH6KL_MCAST_FILTER_MAC_ADDR_SIZE];
 } __packed;
 
+struct wmi_set_htcap_cmd {
+	u8 band;
+	u8 ht_enable;
+	u8 ht40_supported;
+	u8 ht20_sgi;
+	u8 ht40_sgi;
+	u8 intolerant_40mhz;
+	u8 max_ampdu_len_exp;
+} __packed;
+
 /* Command Replies */
 
 /* WMI_GET_CHANNEL_LIST_CMDID reply */
@@ -1397,6 +1419,16 @@ struct wmi_ready_event_2 {
 	u8 phy_cap;
 } __packed;
 
+/* WMI_PHY_CAPABILITY */
+enum wmi_phy_cap {
+	WMI_11A_CAP = 0x01,
+	WMI_11G_CAP = 0x02,
+	WMI_11AG_CAP = 0x03,
+	WMI_11AN_CAP = 0x04,
+	WMI_11GN_CAP = 0x05,
+	WMI_11AGN_CAP = 0x06,
+};
+
 /* Connect Event */
 struct wmi_connect_event {
 	union {
@@ -1449,6 +1481,17 @@ enum wmi_disconnect_reason {
 	IBSS_MERGE = 0xe,
 };
 
+/* AP mode disconnect proto_reasons */
+enum ap_disconnect_reason {
+	WMI_AP_REASON_STA_LEFT		= 101,
+	WMI_AP_REASON_FROM_HOST		= 102,
+	WMI_AP_REASON_COMM_TIMEOUT	= 103,
+	WMI_AP_REASON_MAX_STA		= 104,
+	WMI_AP_REASON_ACL		= 105,
+	WMI_AP_REASON_STA_ROAM		= 106,
+	WMI_AP_REASON_DFS_CHANNEL	= 107,
+};
+
 #define ATH6KL_COUNTRY_RD_SHIFT        16
 
 struct ath6kl_wmi_regdomain {
@@ -1913,6 +1956,14 @@ struct wmi_set_appie_cmd {
 	u8 ie_info[0];
 } __packed;
 
+struct wmi_set_ie_cmd {
+	u8 ie_id;
+	u8 ie_field;	/* enum wmi_ie_field_type */
+	u8 ie_len;
+	u8 reserved;
+	u8 ie_info[0];
+} __packed;
+
 /* Notify the WSC registration status to the target */
 #define WSC_REG_ACTIVE     1
 #define WSC_REG_INACTIVE   0
@@ -2141,6 +2192,11 @@ struct wmi_ap_hidden_ssid_cmd {
 	u8 hidden_ssid;
 } __packed;
 
+struct wmi_set_inact_period_cmd {
+	__le32 inact_period;
+	u8 num_null_func;
+} __packed;
+
 /* AP mode events */
 struct wmi_ap_set_apsd_cmd {
 	u8 enable;
@@ -2465,6 +2521,9 @@ int ath6kl_wmi_get_roam_tbl_cmd(struct wmi *wmi);
 int ath6kl_wmi_set_wmm_txop(struct wmi *wmi, u8 if_idx, enum wmi_txop_cfg cfg);
 int ath6kl_wmi_set_keepalive_cmd(struct wmi *wmi, u8 if_idx,
 				 u8 keep_alive_intvl);
+int ath6kl_wmi_set_htcap_cmd(struct wmi *wmi, u8 if_idx,
+			     enum ieee80211_band band,
+			     struct ath6kl_htcap *htcap);
 int ath6kl_wmi_test_cmd(struct wmi *wmi, void *buf, size_t len);
 
 s32 ath6kl_wmi_get_rate(s8 rate_index);
@@ -2515,6 +2574,9 @@ int ath6kl_wmi_set_rx_frame_format_cmd(struct wmi *wmi, u8 if_idx,
 int ath6kl_wmi_set_appie_cmd(struct wmi *wmi, u8 if_idx, u8 mgmt_frm_type,
 			     const u8 *ie, u8 ie_len);
 
+int ath6kl_wmi_set_ie_cmd(struct wmi *wmi, u8 if_idx, u8 ie_id, u8 ie_field,
+			  const u8 *ie_info, u8 ie_len);
+
 /* P2P */
 int ath6kl_wmi_disable_11b_rates_cmd(struct wmi *wmi, bool disable);
 
@@ -2538,6 +2600,8 @@ int ath6kl_wmi_cancel_remain_on_chnl_cmd(struct wmi *wmi, u8 if_idx);
 int ath6kl_wmi_set_appie_cmd(struct wmi *wmi, u8 if_idx, u8 mgmt_frm_type,
 			     const u8 *ie, u8 ie_len);
 
+int ath6kl_wmi_set_inact_period(struct wmi *wmi, u8 if_idx, int inact_timeout);
+
 void ath6kl_wmi_sscan_timer(unsigned long ptr);
 
 struct ath6kl_vif *ath6kl_get_vif_by_index(struct ath6kl *ar, u8 if_idx);
diff --git a/drivers/net/wireless/ath/ath9k/Makefile b/drivers/net/wireless/ath/ath9k/Makefile
index 27d95fe5ade..3f0b8472378 100644
--- a/drivers/net/wireless/ath/ath9k/Makefile
+++ b/drivers/net/wireless/ath/ath9k/Makefile
@@ -11,7 +11,10 @@ ath9k-$(CONFIG_ATH9K_PCI) += pci.o
 ath9k-$(CONFIG_ATH9K_AHB) += ahb.o
 ath9k-$(CONFIG_ATH9K_DEBUGFS) += debug.o
 ath9k-$(CONFIG_ATH9K_DFS_DEBUGFS) += dfs_debug.o
-ath9k-$(CONFIG_ATH9K_DFS_CERTIFIED) += dfs.o
+ath9k-$(CONFIG_ATH9K_DFS_CERTIFIED) += \
+		dfs.o \
+		dfs_pattern_detector.o \
+		dfs_pri_detector.o
 
 obj-$(CONFIG_ATH9K) += ath9k.o
 
diff --git a/drivers/net/wireless/ath/ath9k/ani.c b/drivers/net/wireless/ath/ath9k/ani.c
index 7e0ea4e9833..b4c77f9d747 100644
--- a/drivers/net/wireless/ath/ath9k/ani.c
+++ b/drivers/net/wireless/ath/ath9k/ani.c
@@ -46,8 +46,8 @@ static const struct ani_ofdm_level_entry ofdm_level_table[] = {
 	{  5,  4,  1  }, /* lvl 5 */
 	{  6,  5,  1  }, /* lvl 6 */
 	{  7,  6,  1  }, /* lvl 7 */
-	{  7,  7,  1  }, /* lvl 8 */
-	{  7,  8,  0  }  /* lvl 9 */
+	{  7,  6,  0  }, /* lvl 8 */
+	{  7,  7,  0  }  /* lvl 9 */
 };
 #define ATH9K_ANI_OFDM_NUM_LEVEL \
 	ARRAY_SIZE(ofdm_level_table)
@@ -91,8 +91,8 @@ static const struct ani_cck_level_entry cck_level_table[] = {
 	{  4,  0  }, /* lvl 4 */
 	{  5,  0  }, /* lvl 5 */
 	{  6,  0  }, /* lvl 6 */
-	{  7,  0  }, /* lvl 7 (only for high rssi) */
-	{  8,  0  }  /* lvl 8 (only for high rssi) */
+	{  6,  0  }, /* lvl 7 (only for high rssi) */
+	{  7,  0  }  /* lvl 8 (only for high rssi) */
 };
 
 #define ATH9K_ANI_CCK_NUM_LEVEL \
@@ -274,7 +274,9 @@ static void ath9k_hw_set_ofdm_nil(struct ath_hw *ah, u8 immunityLevel)
 		aniState->rssiThrLow, aniState->rssiThrHigh);
 
 	if (aniState->update_ani)
-		aniState->ofdmNoiseImmunityLevel = immunityLevel;
+		aniState->ofdmNoiseImmunityLevel =
+			(immunityLevel > ATH9K_ANI_OFDM_DEF_LEVEL) ?
+			immunityLevel : ATH9K_ANI_OFDM_DEF_LEVEL;
 
 	entry_ofdm = &ofdm_level_table[aniState->ofdmNoiseImmunityLevel];
 	entry_cck = &cck_level_table[aniState->cckNoiseImmunityLevel];
@@ -290,16 +292,9 @@ static void ath9k_hw_set_ofdm_nil(struct ath_hw *ah, u8 immunityLevel)
 				     ATH9K_ANI_FIRSTEP_LEVEL,
 				     entry_ofdm->fir_step_level);
 
-	if ((ah->opmode != NL80211_IFTYPE_STATION &&
-	     ah->opmode != NL80211_IFTYPE_ADHOC) ||
-	    aniState->noiseFloor <= aniState->rssiThrHigh) {
-		if (aniState->ofdmWeakSigDetectOff)
-			/* force on ofdm weak sig detect */
-			ath9k_hw_ani_control(ah,
-				ATH9K_ANI_OFDM_WEAK_SIGNAL_DETECTION,
-					     true);
-		else if (aniState->ofdmWeakSigDetectOff ==
-			 entry_ofdm->ofdm_weak_signal_on)
+	if ((aniState->noiseFloor >= aniState->rssiThrHigh) &&
+	    (!aniState->ofdmWeakSigDetectOff !=
+	     entry_ofdm->ofdm_weak_signal_on)) {
 			ath9k_hw_ani_control(ah,
 				ATH9K_ANI_OFDM_WEAK_SIGNAL_DETECTION,
 				entry_ofdm->ofdm_weak_signal_on);
@@ -347,7 +342,9 @@ static void ath9k_hw_set_cck_nil(struct ath_hw *ah, u_int8_t immunityLevel)
 		immunityLevel = ATH9K_ANI_CCK_MAX_LEVEL_LOW_RSSI;
 
 	if (aniState->update_ani)
-		aniState->cckNoiseImmunityLevel = immunityLevel;
+		aniState->cckNoiseImmunityLevel =
+			(immunityLevel > ATH9K_ANI_CCK_DEF_LEVEL) ?
+			immunityLevel : ATH9K_ANI_CCK_DEF_LEVEL;
 
 	entry_ofdm = &ofdm_level_table[aniState->ofdmNoiseImmunityLevel];
 	entry_cck = &cck_level_table[aniState->cckNoiseImmunityLevel];
@@ -717,26 +714,30 @@ void ath9k_hw_ani_monitor(struct ath_hw *ah, struct ath9k_channel *chan)
 		ofdmPhyErrRate, aniState->cckNoiseImmunityLevel,
 		cckPhyErrRate, aniState->ofdmsTurn);
 
-	if (aniState->listenTime > 5 * ah->aniperiod) {
-		if (ofdmPhyErrRate <= ah->config.ofdm_trig_low &&
-		    cckPhyErrRate <= ah->config.cck_trig_low) {
+	if (aniState->listenTime > ah->aniperiod) {
+		if (cckPhyErrRate < ah->config.cck_trig_low &&
+		    ((ofdmPhyErrRate < ah->config.ofdm_trig_low &&
+		      aniState->ofdmNoiseImmunityLevel <
+		      ATH9K_ANI_OFDM_DEF_LEVEL) ||
+		     (ofdmPhyErrRate < ATH9K_ANI_OFDM_TRIG_LOW_ABOVE_INI &&
+		      aniState->ofdmNoiseImmunityLevel >=
+		      ATH9K_ANI_OFDM_DEF_LEVEL))) {
 			ath9k_hw_ani_lower_immunity(ah);
 			aniState->ofdmsTurn = !aniState->ofdmsTurn;
-		}
-		ath9k_ani_restart(ah);
-	} else if (aniState->listenTime > ah->aniperiod) {
-		/* check to see if need to raise immunity */
-		if (ofdmPhyErrRate > ah->config.ofdm_trig_high &&
-		    (cckPhyErrRate <= ah->config.cck_trig_high ||
-		     aniState->ofdmsTurn)) {
+		} else if ((ofdmPhyErrRate > ah->config.ofdm_trig_high &&
+			    aniState->ofdmNoiseImmunityLevel >=
+			    ATH9K_ANI_OFDM_DEF_LEVEL) ||
+			   (ofdmPhyErrRate >
+			    ATH9K_ANI_OFDM_TRIG_HIGH_BELOW_INI &&
+			    aniState->ofdmNoiseImmunityLevel <
+			    ATH9K_ANI_OFDM_DEF_LEVEL)) {
 			ath9k_hw_ani_ofdm_err_trigger(ah);
-			ath9k_ani_restart(ah);
 			aniState->ofdmsTurn = false;
 		} else if (cckPhyErrRate > ah->config.cck_trig_high) {
 			ath9k_hw_ani_cck_err_trigger(ah);
-			ath9k_ani_restart(ah);
 			aniState->ofdmsTurn = true;
 		}
+		ath9k_ani_restart(ah);
 	}
 }
 EXPORT_SYMBOL(ath9k_hw_ani_monitor);
diff --git a/drivers/net/wireless/ath/ath9k/ani.h b/drivers/net/wireless/ath/ath9k/ani.h
index 83029d6c7b2..72e2b874e17 100644
--- a/drivers/net/wireless/ath/ath9k/ani.h
+++ b/drivers/net/wireless/ath/ath9k/ani.h
@@ -25,11 +25,13 @@
 
 /* units are errors per second */
 #define ATH9K_ANI_OFDM_TRIG_HIGH_OLD      500
-#define ATH9K_ANI_OFDM_TRIG_HIGH_NEW      1000
+#define ATH9K_ANI_OFDM_TRIG_HIGH_NEW      3500
+#define ATH9K_ANI_OFDM_TRIG_HIGH_BELOW_INI 1000
 
 /* units are errors per second */
 #define ATH9K_ANI_OFDM_TRIG_LOW_OLD       200
 #define ATH9K_ANI_OFDM_TRIG_LOW_NEW       400
+#define ATH9K_ANI_OFDM_TRIG_LOW_ABOVE_INI 900
 
 /* units are errors per second */
 #define ATH9K_ANI_CCK_TRIG_HIGH_OLD       200
@@ -53,7 +55,7 @@
 #define ATH9K_ANI_RSSI_THR_LOW            7
 
 #define ATH9K_ANI_PERIOD_OLD              100
-#define ATH9K_ANI_PERIOD_NEW              1000
+#define ATH9K_ANI_PERIOD_NEW              300
 
 /* in ms */
 #define ATH9K_ANI_POLLINTERVAL_OLD        100
diff --git a/drivers/net/wireless/ath/ath9k/ar5008_phy.c b/drivers/net/wireless/ath/ath9k/ar5008_phy.c
index aba088005b2..c7492c6a251 100644
--- a/drivers/net/wireless/ath/ath9k/ar5008_phy.c
+++ b/drivers/net/wireless/ath/ath9k/ar5008_phy.c
@@ -245,7 +245,6 @@ static int ar5008_hw_set_channel(struct ath_hw *ah, struct ath9k_channel *chan)
 	REG_WRITE(ah, AR_PHY(0x37), reg32);
 
 	ah->curchan = chan;
-	ah->curchan_rad_index = -1;
 
 	return 0;
 }
@@ -619,19 +618,10 @@ static void ar5008_hw_init_bb(struct ath_hw *ah,
 	u32 synthDelay;
 
 	synthDelay = REG_READ(ah, AR_PHY_RX_DELAY) & AR_PHY_RX_DELAY_DELAY;
-	if (IS_CHAN_B(chan))
-		synthDelay = (4 * synthDelay) / 22;
-	else
-		synthDelay /= 10;
-
-	if (IS_CHAN_HALF_RATE(chan))
-		synthDelay *= 2;
-	else if (IS_CHAN_QUARTER_RATE(chan))
-		synthDelay *= 4;
 
 	REG_WRITE(ah, AR_PHY_ACTIVE, AR_PHY_ACTIVE_EN);
 
-	udelay(synthDelay + BASE_ACTIVATE_DELAY);
+	ath9k_hw_synth_delay(ah, chan, synthDelay);
 }
 
 static void ar5008_hw_init_chain_masks(struct ath_hw *ah)
@@ -949,12 +939,8 @@ static bool ar5008_hw_rfbus_req(struct ath_hw *ah)
 static void ar5008_hw_rfbus_done(struct ath_hw *ah)
 {
 	u32 synthDelay = REG_READ(ah, AR_PHY_RX_DELAY) & AR_PHY_RX_DELAY_DELAY;
-	if (IS_CHAN_B(ah->curchan))
-		synthDelay = (4 * synthDelay) / 22;
-	else
-		synthDelay /= 10;
 
-	udelay(synthDelay + BASE_ACTIVATE_DELAY);
+	ath9k_hw_synth_delay(ah, ah->curchan, synthDelay);
 
 	REG_WRITE(ah, AR_PHY_RFBUS_REQ, 0);
 }
@@ -1047,46 +1033,8 @@ static bool ar5008_hw_ani_control_old(struct ath_hw *ah,
 		break;
 	}
 	case ATH9K_ANI_OFDM_WEAK_SIGNAL_DETECTION:{
-		static const int m1ThreshLow[] = { 127, 50 };
-		static const int m2ThreshLow[] = { 127, 40 };
-		static const int m1Thresh[] = { 127, 0x4d };
-		static const int m2Thresh[] = { 127, 0x40 };
-		static const int m2CountThr[] = { 31, 16 };
-		static const int m2CountThrLow[] = { 63, 48 };
 		u32 on = param ? 1 : 0;
 
-		REG_RMW_FIELD(ah, AR_PHY_SFCORR_LOW,
-			      AR_PHY_SFCORR_LOW_M1_THRESH_LOW,
-			      m1ThreshLow[on]);
-		REG_RMW_FIELD(ah, AR_PHY_SFCORR_LOW,
-			      AR_PHY_SFCORR_LOW_M2_THRESH_LOW,
-			      m2ThreshLow[on]);
-		REG_RMW_FIELD(ah, AR_PHY_SFCORR,
-			      AR_PHY_SFCORR_M1_THRESH,
-			      m1Thresh[on]);
-		REG_RMW_FIELD(ah, AR_PHY_SFCORR,
-			      AR_PHY_SFCORR_M2_THRESH,
-			      m2Thresh[on]);
-		REG_RMW_FIELD(ah, AR_PHY_SFCORR,
-			      AR_PHY_SFCORR_M2COUNT_THR,
-			      m2CountThr[on]);
-		REG_RMW_FIELD(ah, AR_PHY_SFCORR_LOW,
-			      AR_PHY_SFCORR_LOW_M2COUNT_THR_LOW,
-			      m2CountThrLow[on]);
-
-		REG_RMW_FIELD(ah, AR_PHY_SFCORR_EXT,
-			      AR_PHY_SFCORR_EXT_M1_THRESH_LOW,
-			      m1ThreshLow[on]);
-		REG_RMW_FIELD(ah, AR_PHY_SFCORR_EXT,
-			      AR_PHY_SFCORR_EXT_M2_THRESH_LOW,
-			      m2ThreshLow[on]);
-		REG_RMW_FIELD(ah, AR_PHY_SFCORR_EXT,
-			      AR_PHY_SFCORR_EXT_M1_THRESH,
-			      m1Thresh[on]);
-		REG_RMW_FIELD(ah, AR_PHY_SFCORR_EXT,
-			      AR_PHY_SFCORR_EXT_M2_THRESH,
-			      m2Thresh[on]);
-
 		if (on)
 			REG_SET_BIT(ah, AR_PHY_SFCORR_LOW,
 				    AR_PHY_SFCORR_LOW_USE_SELF_CORR_LOW);
diff --git a/drivers/net/wireless/ath/ath9k/ar9002_mac.c b/drivers/net/wireless/ath/ath9k/ar9002_mac.c
index aa2abaf31cb..8d78253c26c 100644
--- a/drivers/net/wireless/ath/ath9k/ar9002_mac.c
+++ b/drivers/net/wireless/ath/ath9k/ar9002_mac.c
@@ -136,6 +136,7 @@ static bool ar9002_hw_get_isr(struct ath_hw *ah, enum ath9k_int *masked)
 	}
 
 	if (sync_cause) {
+		ath9k_debug_sync_cause(common, sync_cause);
 		fatal_int =
 			(sync_cause &
 			 (AR_INTR_SYNC_HOST1_FATAL | AR_INTR_SYNC_HOST1_PERR))
diff --git a/drivers/net/wireless/ath/ath9k/ar9002_phy.c b/drivers/net/wireless/ath/ath9k/ar9002_phy.c
index 3cbbb033fce..846dd7974eb 100644
--- a/drivers/net/wireless/ath/ath9k/ar9002_phy.c
+++ b/drivers/net/wireless/ath/ath9k/ar9002_phy.c
@@ -152,7 +152,6 @@ static int ar9002_hw_set_channel(struct ath_hw *ah, struct ath9k_channel *chan)
 	REG_WRITE(ah, AR_PHY_SYNTH_CONTROL, reg32);
 
 	ah->curchan = chan;
-	ah->curchan_rad_index = -1;
 
 	return 0;
 }
diff --git a/drivers/net/wireless/ath/ath9k/ar9003_2p2_initvals.h b/drivers/net/wireless/ath/ath9k/ar9003_2p2_initvals.h
index 46c79a3d473..952cb2b4656 100644
--- a/drivers/net/wireless/ath/ath9k/ar9003_2p2_initvals.h
+++ b/drivers/net/wireless/ath/ath9k/ar9003_2p2_initvals.h
@@ -777,11 +777,11 @@ static const u32 ar9300Common_rx_gain_table_2p2[][2] = {
 	{0x0000a074, 0x00000000},
 	{0x0000a078, 0x00000000},
 	{0x0000a07c, 0x00000000},
-	{0x0000a080, 0x22222229},
-	{0x0000a084, 0x1d1d1d1d},
-	{0x0000a088, 0x1d1d1d1d},
-	{0x0000a08c, 0x1d1d1d1d},
-	{0x0000a090, 0x171d1d1d},
+	{0x0000a080, 0x1a1a1a1a},
+	{0x0000a084, 0x1a1a1a1a},
+	{0x0000a088, 0x1a1a1a1a},
+	{0x0000a08c, 0x1a1a1a1a},
+	{0x0000a090, 0x171a1a1a},
 	{0x0000a094, 0x11111717},
 	{0x0000a098, 0x00030311},
 	{0x0000a09c, 0x00000000},
diff --git a/drivers/net/wireless/ath/ath9k/ar9003_calib.c b/drivers/net/wireless/ath/ath9k/ar9003_calib.c
index 63089cc1faf..9fdd70fcaf5 100644
--- a/drivers/net/wireless/ath/ath9k/ar9003_calib.c
+++ b/drivers/net/wireless/ath/ath9k/ar9003_calib.c
@@ -892,34 +892,6 @@ static void ar9003_hw_tx_iq_cal_reload(struct ath_hw *ah)
 		      AR_PHY_RX_IQCAL_CORR_B0_LOOPBACK_IQCORR_EN, 0x1);
 }
 
-static bool ar9003_hw_rtt_restore(struct ath_hw *ah, struct ath9k_channel *chan)
-{
-	struct ath9k_rtt_hist *hist;
-	u32 *table;
-	int i;
-	bool restore;
-
-	if (!ah->caldata)
-		return false;
-
-	hist = &ah->caldata->rtt_hist;
-	if (!hist->num_readings)
-		return false;
-
-	ar9003_hw_rtt_enable(ah);
-	ar9003_hw_rtt_set_mask(ah, 0x00);
-	for (i = 0; i < AR9300_MAX_CHAINS; i++) {
-		if (!(ah->rxchainmask & (1 << i)))
-			continue;
-		table = &hist->table[i][hist->num_readings][0];
-		ar9003_hw_rtt_load_hist(ah, i, table);
-	}
-	restore = ar9003_hw_rtt_force_restore(ah);
-	ar9003_hw_rtt_disable(ah);
-
-	return restore;
-}
-
 static bool ar9003_hw_init_cal(struct ath_hw *ah,
 			       struct ath9k_channel *chan)
 {
@@ -942,9 +914,10 @@ static bool ar9003_hw_init_cal(struct ath_hw *ah,
 		if (!ar9003_hw_rtt_restore(ah, chan))
 			run_rtt_cal = true;
 
-		ath_dbg(common, CALIBRATE, "RTT restore %s\n",
-			run_rtt_cal ? "failed" : "succeed");
+		if (run_rtt_cal)
+			ath_dbg(common, CALIBRATE, "RTT calibration to be done\n");
 	}
+
 	run_agc_cal = run_rtt_cal;
 
 	if (run_rtt_cal) {
@@ -1000,10 +973,12 @@ static bool ar9003_hw_init_cal(struct ath_hw *ah,
 	if (mci && IS_CHAN_2GHZ(chan) && run_agc_cal)
 		ar9003_mci_init_cal_req(ah, &is_reusable);
 
-	txiqcal_done = ar9003_hw_tx_iq_cal_run(ah);
-	REG_WRITE(ah, AR_PHY_ACTIVE, AR_PHY_ACTIVE_DIS);
-	udelay(5);
-	REG_WRITE(ah, AR_PHY_ACTIVE, AR_PHY_ACTIVE_EN);
+	if (!(IS_CHAN_HALF_RATE(chan) || IS_CHAN_QUARTER_RATE(chan))) {
+		txiqcal_done = ar9003_hw_tx_iq_cal_run(ah);
+		REG_WRITE(ah, AR_PHY_ACTIVE, AR_PHY_ACTIVE_DIS);
+		udelay(5);
+		REG_WRITE(ah, AR_PHY_ACTIVE, AR_PHY_ACTIVE_EN);
+	}
 
 skip_tx_iqcal:
 	if (run_agc_cal || !(ah->ah_flags & AH_FASTCC)) {
@@ -1067,17 +1042,14 @@ skip_tx_iqcal:
 #undef CL_TAB_ENTRY
 
 	if (run_rtt_cal && caldata) {
-		struct ath9k_rtt_hist *hist = &caldata->rtt_hist;
-		if (is_reusable && (hist->num_readings < RTT_HIST_MAX)) {
-			u32 *table;
+		if (is_reusable) {
+			if (!ath9k_hw_rfbus_req(ah))
+				ath_err(ath9k_hw_common(ah),
+					"Could not stop baseband\n");
+			else
+				ar9003_hw_rtt_fill_hist(ah);
 
-			hist->num_readings++;
-			for (i = 0; i < AR9300_MAX_CHAINS; i++) {
-				if (!(ah->rxchainmask & (1 << i)))
-					continue;
-				table = &hist->table[i][hist->num_readings][0];
-				ar9003_hw_rtt_fill_hist(ah, i, table);
-			}
+			ath9k_hw_rfbus_done(ah);
 		}
 
 		ar9003_hw_rtt_disable(ah);
diff --git a/drivers/net/wireless/ath/ath9k/ar9003_eeprom.c b/drivers/net/wireless/ath/ath9k/ar9003_eeprom.c
index 6bb4db052bb..ac53d901801 100644
--- a/drivers/net/wireless/ath/ath9k/ar9003_eeprom.c
+++ b/drivers/net/wireless/ath/ath9k/ar9003_eeprom.c
@@ -30,11 +30,6 @@
 #define CTL_11A_EXT (CTL_11A | EXT_ADDITIVE)
 #define CTL_11G_EXT (CTL_11G | EXT_ADDITIVE)
 #define CTL_11B_EXT (CTL_11B | EXT_ADDITIVE)
-#define REDUCE_SCALED_POWER_BY_TWO_CHAIN     6  /* 10*log10(2)*2 */
-#define REDUCE_SCALED_POWER_BY_THREE_CHAIN   9  /* 10*log10(3)*2 */
-#define PWRINCR_3_TO_1_CHAIN      9             /* 10*log(3)*2 */
-#define PWRINCR_3_TO_2_CHAIN      3             /* floor(10*log(3/2)*2) */
-#define PWRINCR_2_TO_1_CHAIN      6             /* 10*log(2)*2 */
 
 #define SUB_NUM_CTL_MODES_AT_5G_40 2    /* excluding HT40, EXT-OFDM */
 #define SUB_NUM_CTL_MODES_AT_2G_40 3    /* excluding HT40, EXT-OFDM, EXT-CCK */
@@ -2936,15 +2931,6 @@ static const struct ar9300_eeprom *ar9003_eeprom_struct_find_by_id(int id)
 #undef N_LOOP
 }
 
-
-static u16 ath9k_hw_fbin2freq(u8 fbin, bool is2GHz)
-{
-	if (fbin == AR5416_BCHAN_UNUSED)
-		return fbin;
-
-	return (u16) ((is2GHz) ? (2300 + fbin) : (4800 + 5 * fbin));
-}
-
 static int ath9k_hw_ar9300_check_eeprom(struct ath_hw *ah)
 {
 	return 0;
@@ -4070,7 +4056,7 @@ static u8 ar9003_hw_eeprom_get_tgt_pwr(struct ath_hw *ah,
 	 * targetpower piers stored on eeprom
 	 */
 	for (i = 0; i < numPiers; i++) {
-		freqArray[i] = FBIN2FREQ(pFreqBin[i], is2GHz);
+		freqArray[i] = ath9k_hw_fbin2freq(pFreqBin[i], is2GHz);
 		targetPowerArray[i] = pEepromTargetPwr[i].tPow2x[rateIndex];
 	}
 
@@ -4106,7 +4092,7 @@ static u8 ar9003_hw_eeprom_get_ht20_tgt_pwr(struct ath_hw *ah,
 	 * from targetpower piers stored on eeprom
 	 */
 	for (i = 0; i < numPiers; i++) {
-		freqArray[i] = FBIN2FREQ(pFreqBin[i], is2GHz);
+		freqArray[i] = ath9k_hw_fbin2freq(pFreqBin[i], is2GHz);
 		targetPowerArray[i] = pEepromTargetPwr[i].tPow2x[rateIndex];
 	}
 
@@ -4142,7 +4128,7 @@ static u8 ar9003_hw_eeprom_get_ht40_tgt_pwr(struct ath_hw *ah,
 	 * targetpower piers stored on eeprom
 	 */
 	for (i = 0; i < numPiers; i++) {
-		freqArray[i] = FBIN2FREQ(pFreqBin[i], is2GHz);
+		freqArray[i] = ath9k_hw_fbin2freq(pFreqBin[i], is2GHz);
 		targetPowerArray[i] = pEepromTargetPwr[i].tPow2x[rateIndex];
 	}
 
@@ -4167,7 +4153,7 @@ static u8 ar9003_hw_eeprom_get_cck_tgt_pwr(struct ath_hw *ah,
 	 * targetpower piers stored on eeprom
 	 */
 	for (i = 0; i < numPiers; i++) {
-		freqArray[i] = FBIN2FREQ(pFreqBin[i], 1);
+		freqArray[i] = ath9k_hw_fbin2freq(pFreqBin[i], 1);
 		targetPowerArray[i] = pEepromTargetPwr[i].tPow2x[rateIndex];
 	}
 
@@ -4295,18 +4281,10 @@ static int ar9003_hw_tx_power_regwrite(struct ath_hw *ah, u8 * pPwrArray)
 #undef POW_SM
 }
 
-static void ar9003_hw_set_target_power_eeprom(struct ath_hw *ah, u16 freq,
-					      u8 *targetPowerValT2)
+static void ar9003_hw_get_legacy_target_powers(struct ath_hw *ah, u16 freq,
+					       u8 *targetPowerValT2,
+					       bool is2GHz)
 {
-	/* XXX: hard code for now, need to get from eeprom struct */
-	u8 ht40PowerIncForPdadc = 0;
-	bool is2GHz = false;
-	unsigned int i = 0;
-	struct ath_common *common = ath9k_hw_common(ah);
-
-	if (freq < 4000)
-		is2GHz = true;
-
 	targetPowerValT2[ALL_TARGET_LEGACY_6_24] =
 	    ar9003_hw_eeprom_get_tgt_pwr(ah, LEGACY_TARGET_RATE_6_24, freq,
 					 is2GHz);
@@ -4319,6 +4297,11 @@ static void ar9003_hw_set_target_power_eeprom(struct ath_hw *ah, u16 freq,
 	targetPowerValT2[ALL_TARGET_LEGACY_54] =
 	    ar9003_hw_eeprom_get_tgt_pwr(ah, LEGACY_TARGET_RATE_54, freq,
 					 is2GHz);
+}
+
+static void ar9003_hw_get_cck_target_powers(struct ath_hw *ah, u16 freq,
+					    u8 *targetPowerValT2)
+{
 	targetPowerValT2[ALL_TARGET_LEGACY_1L_5L] =
 	    ar9003_hw_eeprom_get_cck_tgt_pwr(ah, LEGACY_TARGET_RATE_1L_5L,
 					     freq);
@@ -4328,6 +4311,11 @@ static void ar9003_hw_set_target_power_eeprom(struct ath_hw *ah, u16 freq,
 	    ar9003_hw_eeprom_get_cck_tgt_pwr(ah, LEGACY_TARGET_RATE_11L, freq);
 	targetPowerValT2[ALL_TARGET_LEGACY_11S] =
 	    ar9003_hw_eeprom_get_cck_tgt_pwr(ah, LEGACY_TARGET_RATE_11S, freq);
+}
+
+static void ar9003_hw_get_ht20_target_powers(struct ath_hw *ah, u16 freq,
+					     u8 *targetPowerValT2, bool is2GHz)
+{
 	targetPowerValT2[ALL_TARGET_HT20_0_8_16] =
 	    ar9003_hw_eeprom_get_ht20_tgt_pwr(ah, HT_TARGET_RATE_0_8_16, freq,
 					      is2GHz);
@@ -4370,6 +4358,16 @@ static void ar9003_hw_set_target_power_eeprom(struct ath_hw *ah, u16 freq,
 	targetPowerValT2[ALL_TARGET_HT20_23] =
 	    ar9003_hw_eeprom_get_ht20_tgt_pwr(ah, HT_TARGET_RATE_23, freq,
 					      is2GHz);
+}
+
+static void ar9003_hw_get_ht40_target_powers(struct ath_hw *ah,
+						   u16 freq,
+						   u8 *targetPowerValT2,
+						   bool is2GHz)
+{
+	/* XXX: hard code for now, need to get from eeprom struct */
+	u8 ht40PowerIncForPdadc = 0;
+
 	targetPowerValT2[ALL_TARGET_HT40_0_8_16] =
 	    ar9003_hw_eeprom_get_ht40_tgt_pwr(ah, HT_TARGET_RATE_0_8_16, freq,
 					      is2GHz) + ht40PowerIncForPdadc;
@@ -4413,6 +4411,26 @@ static void ar9003_hw_set_target_power_eeprom(struct ath_hw *ah, u16 freq,
 	targetPowerValT2[ALL_TARGET_HT40_23] =
 	    ar9003_hw_eeprom_get_ht40_tgt_pwr(ah, HT_TARGET_RATE_23, freq,
 					      is2GHz) + ht40PowerIncForPdadc;
+}
+
+static void ar9003_hw_get_target_power_eeprom(struct ath_hw *ah,
+					      struct ath9k_channel *chan,
+					      u8 *targetPowerValT2)
+{
+	bool is2GHz = IS_CHAN_2GHZ(chan);
+	unsigned int i = 0;
+	struct ath_common *common = ath9k_hw_common(ah);
+	u16 freq = chan->channel;
+
+	if (is2GHz)
+		ar9003_hw_get_cck_target_powers(ah, freq, targetPowerValT2);
+
+	ar9003_hw_get_legacy_target_powers(ah, freq, targetPowerValT2, is2GHz);
+	ar9003_hw_get_ht20_target_powers(ah, freq, targetPowerValT2, is2GHz);
+
+	if (IS_CHAN_HT40(chan))
+		ar9003_hw_get_ht40_target_powers(ah, freq, targetPowerValT2,
+						 is2GHz);
 
 	for (i = 0; i < ar9300RateSize; i++) {
 		ath_dbg(common, EEPROM, "TPC[%02d] 0x%08x\n",
@@ -4464,7 +4482,7 @@ static int ar9003_hw_cal_pier_get(struct ath_hw *ah,
 		is2GHz = 1;
 	}
 
-	*pfrequency = FBIN2FREQ(*pCalPier, is2GHz);
+	*pfrequency = ath9k_hw_fbin2freq(*pCalPier, is2GHz);
 	*pcorrection = pCalPierStruct->refPower;
 	*ptemperature = pCalPierStruct->tempMeas;
 	*pvoltage = pCalPierStruct->voltMeas;
@@ -4789,34 +4807,9 @@ static void ar9003_hw_set_power_per_rate_table(struct ath_hw *ah,
 	bool is2ghz = IS_CHAN_2GHZ(chan);
 
 	ath9k_hw_get_channel_centers(ah, chan, &centers);
-	scaledPower = powerLimit - antenna_reduction;
-
-	/*
-	 * Reduce scaled Power by number of chains active to get
-	 * to per chain tx power level
-	 */
-	switch (ar5416_get_ntxchains(ah->txchainmask)) {
-	case 1:
-		break;
-	case 2:
-		if (scaledPower > REDUCE_SCALED_POWER_BY_TWO_CHAIN)
-			scaledPower -= REDUCE_SCALED_POWER_BY_TWO_CHAIN;
-		else
-			scaledPower = 0;
-		break;
-	case 3:
-		if (scaledPower > REDUCE_SCALED_POWER_BY_THREE_CHAIN)
-			scaledPower -= REDUCE_SCALED_POWER_BY_THREE_CHAIN;
-		else
-			scaledPower = 0;
-		break;
-	}
+	scaledPower = ath9k_hw_get_scaled_power(ah, powerLimit,
+						antenna_reduction);
 
-	scaledPower = max((u16)0, scaledPower);
-
-	/*
-	 * Get target powers from EEPROM - our baseline for TX Power
-	 */
 	if (is2ghz) {
 		/* Setup for CTL modes */
 		/* CTL_11B, CTL_11G, CTL_2GHT20 */
@@ -4988,7 +4981,12 @@ static void ath9k_hw_ar9300_set_txpower(struct ath_hw *ah,
 	unsigned int i = 0, paprd_scale_factor = 0;
 	u8 pwr_idx, min_pwridx = 0;
 
-	ar9003_hw_set_target_power_eeprom(ah, chan->channel, targetPowerValT2);
+	memset(targetPowerValT2, 0 , sizeof(targetPowerValT2));
+
+	/*
+	 * Get target powers from EEPROM - our baseline for TX Power
+	 */
+	ar9003_hw_get_target_power_eeprom(ah, chan, targetPowerValT2);
 
 	if (ah->eep_ops->get_eeprom(ah, EEP_PAPRD)) {
 		if (IS_CHAN_2GHZ(chan))
@@ -5060,8 +5058,6 @@ static void ath9k_hw_ar9300_set_txpower(struct ath_hw *ah,
 			i, targetPowerValT2[i]);
 	}
 
-	ah->txpower_limit = regulatory->max_power_level;
-
 	/* Write target power array to registers */
 	ar9003_hw_tx_power_regwrite(ah, targetPowerValT2);
 	ar9003_hw_calibration_apply(ah, chan->channel);
diff --git a/drivers/net/wireless/ath/ath9k/ar9003_eeprom.h b/drivers/net/wireless/ath/ath9k/ar9003_eeprom.h
index bb223fe8281..2505ac44f0c 100644
--- a/drivers/net/wireless/ath/ath9k/ar9003_eeprom.h
+++ b/drivers/net/wireless/ath/ath9k/ar9003_eeprom.h
@@ -42,7 +42,6 @@
 #define AR9300_EEPMISC_WOW           0x02
 #define AR9300_CUSTOMER_DATA_SIZE    20
 
-#define FBIN2FREQ(x, y) ((y) ? (2300 + x) : (4800 + 5 * x))
 #define AR9300_MAX_CHAINS            3
 #define AR9300_ANT_16S               25
 #define AR9300_FUTURE_MODAL_SZ       6
diff --git a/drivers/net/wireless/ath/ath9k/ar9003_hw.c b/drivers/net/wireless/ath/ath9k/ar9003_hw.c
index 0f56e322dd3..a0e3394b10d 100644
--- a/drivers/net/wireless/ath/ath9k/ar9003_hw.c
+++ b/drivers/net/wireless/ath/ath9k/ar9003_hw.c
@@ -305,11 +305,6 @@ static void ar9003_hw_init_mode_regs(struct ath_hw *ah)
 				ar9462_common_rx_gain_table_2p0,
 				ARRAY_SIZE(ar9462_common_rx_gain_table_2p0), 2);
 
-		INIT_INI_ARRAY(&ah->ini_BTCOEX_MAX_TXPWR,
-				ar9462_2p0_BTCOEX_MAX_TXPWR_table,
-				ARRAY_SIZE(ar9462_2p0_BTCOEX_MAX_TXPWR_table),
-				2);
-
 		/* Awake -> Sleep Setting */
 		INIT_INI_ARRAY(&ah->iniPcieSerdes,
 				PCIE_PLL_ON_CREQ_DIS_L1_2P0,
diff --git a/drivers/net/wireless/ath/ath9k/ar9003_mac.c b/drivers/net/wireless/ath/ath9k/ar9003_mac.c
index a66a13b7684..d9e0824af09 100644
--- a/drivers/net/wireless/ath/ath9k/ar9003_mac.c
+++ b/drivers/net/wireless/ath/ath9k/ar9003_mac.c
@@ -306,6 +306,8 @@ static bool ar9003_hw_get_isr(struct ath_hw *ah, enum ath9k_int *masked)
 		ar9003_mci_get_isr(ah, masked);
 
 	if (sync_cause) {
+		ath9k_debug_sync_cause(common, sync_cause);
+
 		if (sync_cause & AR_INTR_SYNC_RADM_CPL_TIMEOUT) {
 			REG_WRITE(ah, AR_RC, AR_RC_HOSTIF);
 			REG_WRITE(ah, AR_RC, 0);
diff --git a/drivers/net/wireless/ath/ath9k/ar9003_mci.c b/drivers/net/wireless/ath/ath9k/ar9003_mci.c
index 3cac293a284..ffbb180f91e 100644
--- a/drivers/net/wireless/ath/ath9k/ar9003_mci.c
+++ b/drivers/net/wireless/ath/ath9k/ar9003_mci.c
@@ -756,7 +756,7 @@ int ar9003_mci_end_reset(struct ath_hw *ah, struct ath9k_channel *chan,
 		if (caldata) {
 			caldata->done_txiqcal_once = false;
 			caldata->done_txclcal_once = false;
-			caldata->rtt_hist.num_readings = 0;
+			caldata->rtt_done = false;
 		}
 
 		if (!ath9k_hw_init_cal(ah, chan))
diff --git a/drivers/net/wireless/ath/ath9k/ar9003_phy.c b/drivers/net/wireless/ath/ath9k/ar9003_phy.c
index deb6cfb2959..11abb972be1 100644
--- a/drivers/net/wireless/ath/ath9k/ar9003_phy.c
+++ b/drivers/net/wireless/ath/ath9k/ar9003_phy.c
@@ -152,7 +152,6 @@ static int ar9003_hw_set_channel(struct ath_hw *ah, struct ath9k_channel *chan)
 	REG_WRITE(ah, AR_PHY_65NM_CH0_SYNTH7, reg32);
 
 	ah->curchan = chan;
-	ah->curchan_rad_index = -1;
 
 	return 0;
 }
@@ -209,11 +208,12 @@ static void ar9003_hw_spur_mitigate_mrc_cck(struct ath_hw *ah,
 			continue;
 		negative = 0;
 		if (AR_SREV_9485(ah) || AR_SREV_9340(ah) || AR_SREV_9330(ah))
-			cur_bb_spur = FBIN2FREQ(spur_fbin_ptr[i],
-					IS_CHAN_2GHZ(chan)) - synth_freq;
+			cur_bb_spur = ath9k_hw_fbin2freq(spur_fbin_ptr[i],
+							 IS_CHAN_2GHZ(chan));
 		else
-			cur_bb_spur = spur_freq[i] - synth_freq;
+			cur_bb_spur = spur_freq[i];
 
+		cur_bb_spur -= synth_freq;
 		if (cur_bb_spur < 0) {
 			negative = 1;
 			cur_bb_spur = -cur_bb_spur;
@@ -373,7 +373,7 @@ static void ar9003_hw_spur_ofdm_work(struct ath_hw *ah,
 			else
 				spur_subchannel_sd = 0;
 
-			spur_freq_sd = (freq_offset << 9) / 11;
+			spur_freq_sd = ((freq_offset + 10) << 9) / 11;
 
 		} else {
 			if (REG_READ_FIELD(ah, AR_PHY_GEN_CTRL,
@@ -382,7 +382,7 @@ static void ar9003_hw_spur_ofdm_work(struct ath_hw *ah,
 			else
 				spur_subchannel_sd = 1;
 
-			spur_freq_sd = (freq_offset << 9) / 11;
+			spur_freq_sd = ((freq_offset - 10) << 9) / 11;
 
 		}
 
@@ -443,7 +443,8 @@ static void ar9003_hw_spur_mitigate_ofdm(struct ath_hw *ah,
 	ar9003_hw_spur_ofdm_clear(ah);
 
 	for (i = 0; i < AR_EEPROM_MODAL_SPURS && spurChansPtr[i]; i++) {
-		freq_offset = FBIN2FREQ(spurChansPtr[i], mode) - synth_freq;
+		freq_offset = ath9k_hw_fbin2freq(spurChansPtr[i], mode);
+		freq_offset -= synth_freq;
 		if (abs(freq_offset) < range) {
 			ar9003_hw_spur_ofdm_work(ah, chan, freq_offset);
 			break;
@@ -525,22 +526,10 @@ static void ar9003_hw_init_bb(struct ath_hw *ah,
 	 * Value is in 100ns increments.
 	 */
 	synthDelay = REG_READ(ah, AR_PHY_RX_DELAY) & AR_PHY_RX_DELAY_DELAY;
-	if (IS_CHAN_B(chan))
-		synthDelay = (4 * synthDelay) / 22;
-	else
-		synthDelay /= 10;
 
 	/* Activate the PHY (includes baseband activate + synthesizer on) */
 	REG_WRITE(ah, AR_PHY_ACTIVE, AR_PHY_ACTIVE_EN);
-
-	/*
-	 * There is an issue if the AP starts the calibration before
-	 * the base band timeout completes.  This could result in the
-	 * rx_clear false triggering.  As a workaround we add delay an
-	 * extra BASE_ACTIVATE_DELAY usecs to ensure this condition
-	 * does not happen.
-	 */
-	udelay(synthDelay + BASE_ACTIVATE_DELAY);
+	ath9k_hw_synth_delay(ah, chan, synthDelay);
 }
 
 static void ar9003_hw_set_chain_masks(struct ath_hw *ah, u8 rx, u8 tx)
@@ -684,9 +673,6 @@ static int ar9003_hw_process_ini(struct ath_hw *ah,
 
 	REG_WRITE_ARRAY(&ah->iniAdditional, 1, regWrites);
 
-	if (AR_SREV_9462(ah))
-		ar9003_hw_prog_ini(ah, &ah->ini_BTCOEX_MAX_TXPWR, 1);
-
 	if (chan->channel == 2484)
 		ar9003_hw_prog_ini(ah, &ah->ini_japan2484, 1);
 
@@ -725,6 +711,14 @@ static void ar9003_hw_set_rfmode(struct ath_hw *ah,
 
 	if (IS_CHAN_A_FAST_CLOCK(ah, chan))
 		rfMode |= (AR_PHY_MODE_DYNAMIC | AR_PHY_MODE_DYN_CCK_DISABLE);
+	if (IS_CHAN_QUARTER_RATE(chan))
+		rfMode |= AR_PHY_MODE_QUARTER;
+	if (IS_CHAN_HALF_RATE(chan))
+		rfMode |= AR_PHY_MODE_HALF;
+
+	if (rfMode & (AR_PHY_MODE_QUARTER | AR_PHY_MODE_HALF))
+		REG_RMW_FIELD(ah, AR_PHY_FRAME_CTL,
+			      AR_PHY_FRAME_CTL_CF_OVERLAP_WINDOW, 3);
 
 	REG_WRITE(ah, AR_PHY_MODE, rfMode);
 }
@@ -795,12 +789,8 @@ static bool ar9003_hw_rfbus_req(struct ath_hw *ah)
 static void ar9003_hw_rfbus_done(struct ath_hw *ah)
 {
 	u32 synthDelay = REG_READ(ah, AR_PHY_RX_DELAY) & AR_PHY_RX_DELAY_DELAY;
-	if (IS_CHAN_B(ah->curchan))
-		synthDelay = (4 * synthDelay) / 22;
-	else
-		synthDelay /= 10;
 
-	udelay(synthDelay + BASE_ACTIVATE_DELAY);
+	ath9k_hw_synth_delay(ah, ah->curchan, synthDelay);
 
 	REG_WRITE(ah, AR_PHY_RFBUS_REQ, 0);
 }
@@ -823,55 +813,6 @@ static bool ar9003_hw_ani_control(struct ath_hw *ah,
 		 * on == 0 means more noise imm
 		 */
 		u32 on = param ? 1 : 0;
-		/*
-		 * make register setting for default
-		 * (weak sig detect ON) come from INI file
-		 */
-		int m1ThreshLow = on ?
-			aniState->iniDef.m1ThreshLow : m1ThreshLow_off;
-		int m2ThreshLow = on ?
-			aniState->iniDef.m2ThreshLow : m2ThreshLow_off;
-		int m1Thresh = on ?
-			aniState->iniDef.m1Thresh : m1Thresh_off;
-		int m2Thresh = on ?
-			aniState->iniDef.m2Thresh : m2Thresh_off;
-		int m2CountThr = on ?
-			aniState->iniDef.m2CountThr : m2CountThr_off;
-		int m2CountThrLow = on ?
-			aniState->iniDef.m2CountThrLow : m2CountThrLow_off;
-		int m1ThreshLowExt = on ?
-			aniState->iniDef.m1ThreshLowExt : m1ThreshLowExt_off;
-		int m2ThreshLowExt = on ?
-			aniState->iniDef.m2ThreshLowExt : m2ThreshLowExt_off;
-		int m1ThreshExt = on ?
-			aniState->iniDef.m1ThreshExt : m1ThreshExt_off;
-		int m2ThreshExt = on ?
-			aniState->iniDef.m2ThreshExt : m2ThreshExt_off;
-
-		REG_RMW_FIELD(ah, AR_PHY_SFCORR_LOW,
-			      AR_PHY_SFCORR_LOW_M1_THRESH_LOW,
-			      m1ThreshLow);
-		REG_RMW_FIELD(ah, AR_PHY_SFCORR_LOW,
-			      AR_PHY_SFCORR_LOW_M2_THRESH_LOW,
-			      m2ThreshLow);
-		REG_RMW_FIELD(ah, AR_PHY_SFCORR,
-			      AR_PHY_SFCORR_M1_THRESH, m1Thresh);
-		REG_RMW_FIELD(ah, AR_PHY_SFCORR,
-			      AR_PHY_SFCORR_M2_THRESH, m2Thresh);
-		REG_RMW_FIELD(ah, AR_PHY_SFCORR,
-			      AR_PHY_SFCORR_M2COUNT_THR, m2CountThr);
-		REG_RMW_FIELD(ah, AR_PHY_SFCORR_LOW,
-			      AR_PHY_SFCORR_LOW_M2COUNT_THR_LOW,
-			      m2CountThrLow);
-
-		REG_RMW_FIELD(ah, AR_PHY_SFCORR_EXT,
-			      AR_PHY_SFCORR_EXT_M1_THRESH_LOW, m1ThreshLowExt);
-		REG_RMW_FIELD(ah, AR_PHY_SFCORR_EXT,
-			      AR_PHY_SFCORR_EXT_M2_THRESH_LOW, m2ThreshLowExt);
-		REG_RMW_FIELD(ah, AR_PHY_SFCORR_EXT,
-			      AR_PHY_SFCORR_EXT_M1_THRESH, m1ThreshExt);
-		REG_RMW_FIELD(ah, AR_PHY_SFCORR_EXT,
-			      AR_PHY_SFCORR_EXT_M2_THRESH, m2ThreshExt);
 
 		if (on)
 			REG_SET_BIT(ah, AR_PHY_SFCORR_LOW,
diff --git a/drivers/net/wireless/ath/ath9k/ar9003_phy.h b/drivers/net/wireless/ath/ath9k/ar9003_phy.h
index d834d97fe72..7268a48a92a 100644
--- a/drivers/net/wireless/ath/ath9k/ar9003_phy.h
+++ b/drivers/net/wireless/ath/ath9k/ar9003_phy.h
@@ -468,6 +468,9 @@
 #define AR_PHY_ADDAC_PARA_CTL    (AR_SM_BASE + 0x150)
 #define AR_PHY_XPA_CFG           (AR_SM_BASE + 0x158)
 
+#define AR_PHY_FRAME_CTL_CF_OVERLAP_WINDOW  3
+#define AR_PHY_FRAME_CTL_CF_OVERLAP_WINDOW_S    0
+
 #define AR_PHY_SPUR_MASK_A_CF_PUNC_MASK_IDX_A           0x0001FC00
 #define AR_PHY_SPUR_MASK_A_CF_PUNC_MASK_IDX_A_S         10
 #define AR_PHY_SPUR_MASK_A_CF_PUNC_MASK_A                       0x3FF
diff --git a/drivers/net/wireless/ath/ath9k/ar9003_rtt.c b/drivers/net/wireless/ath/ath9k/ar9003_rtt.c
index 458bedf0b0a..74de3539c2c 100644
--- a/drivers/net/wireless/ath/ath9k/ar9003_rtt.c
+++ b/drivers/net/wireless/ath/ath9k/ar9003_rtt.c
@@ -15,6 +15,7 @@
  */
 
 #include "hw.h"
+#include "hw-ops.h"
 #include "ar9003_phy.h"
 #include "ar9003_rtt.h"
 
@@ -69,7 +70,7 @@ bool ar9003_hw_rtt_force_restore(struct ath_hw *ah)
 }
 
 static void ar9003_hw_rtt_load_hist_entry(struct ath_hw *ah, u8 chain,
-		u32 index, u32 data28)
+					  u32 index, u32 data28)
 {
 	u32 val;
 
@@ -100,12 +101,21 @@ static void ar9003_hw_rtt_load_hist_entry(struct ath_hw *ah, u8 chain,
 		      RTT_ACCESS_TIMEOUT);
 }
 
-void ar9003_hw_rtt_load_hist(struct ath_hw *ah, u8 chain, u32 *table)
+void ar9003_hw_rtt_load_hist(struct ath_hw *ah)
 {
-	int i;
+	int chain, i;
 
-	for (i = 0; i < MAX_RTT_TABLE_ENTRY; i++)
-		ar9003_hw_rtt_load_hist_entry(ah, chain, i, table[i]);
+	for (chain = 0; chain < AR9300_MAX_CHAINS; chain++) {
+		if (!(ah->rxchainmask & (1 << chain)))
+			continue;
+		for (i = 0; i < MAX_RTT_TABLE_ENTRY; i++) {
+			ar9003_hw_rtt_load_hist_entry(ah, chain, i,
+					      ah->caldata->rtt_table[chain][i]);
+			ath_dbg(ath9k_hw_common(ah), CALIBRATE,
+				"Load RTT value at idx %d, chain %d: 0x%x\n",
+				i, chain, ah->caldata->rtt_table[chain][i]);
+		}
+	}
 }
 
 static int ar9003_hw_rtt_fill_hist_entry(struct ath_hw *ah, u8 chain, u32 index)
@@ -128,27 +138,71 @@ static int ar9003_hw_rtt_fill_hist_entry(struct ath_hw *ah, u8 chain, u32 index)
 			   RTT_ACCESS_TIMEOUT))
 		return RTT_BAD_VALUE;
 
-	val = REG_READ(ah, AR_PHY_RTT_TABLE_SW_INTF_1_B(chain));
+	val = MS(REG_READ(ah, AR_PHY_RTT_TABLE_SW_INTF_1_B(chain)),
+		 AR_PHY_RTT_SW_RTT_TABLE_DATA);
+
 
 	return val;
 }
 
-void ar9003_hw_rtt_fill_hist(struct ath_hw *ah, u8 chain, u32 *table)
+void ar9003_hw_rtt_fill_hist(struct ath_hw *ah)
 {
-	int i;
+	int chain, i;
+
+	for (chain = 0; chain < AR9300_MAX_CHAINS; chain++) {
+		if (!(ah->rxchainmask & (1 << chain)))
+			continue;
+		for (i = 0; i < MAX_RTT_TABLE_ENTRY; i++) {
+			ah->caldata->rtt_table[chain][i] =
+				ar9003_hw_rtt_fill_hist_entry(ah, chain, i);
+			ath_dbg(ath9k_hw_common(ah), CALIBRATE,
+				"RTT value at idx %d, chain %d is: 0x%x\n",
+				i, chain, ah->caldata->rtt_table[chain][i]);
+		}
+	}
 
-	for (i = 0; i < MAX_RTT_TABLE_ENTRY; i++)
-		table[i] = ar9003_hw_rtt_fill_hist_entry(ah, chain, i);
+	ah->caldata->rtt_done = true;
 }
 
 void ar9003_hw_rtt_clear_hist(struct ath_hw *ah)
 {
-	int i, j;
+	int chain, i;
 
-	for (i = 0; i < AR9300_MAX_CHAINS; i++) {
-		if (!(ah->rxchainmask & (1 << i)))
+	for (chain = 0; chain < AR9300_MAX_CHAINS; chain++) {
+		if (!(ah->rxchainmask & (1 << chain)))
 			continue;
-		for (j = 0; j < MAX_RTT_TABLE_ENTRY; j++)
-			ar9003_hw_rtt_load_hist_entry(ah, i, j, 0);
+		for (i = 0; i < MAX_RTT_TABLE_ENTRY; i++)
+			ar9003_hw_rtt_load_hist_entry(ah, chain, i, 0);
 	}
+
+	if (ah->caldata)
+		ah->caldata->rtt_done = false;
+}
+
+bool ar9003_hw_rtt_restore(struct ath_hw *ah, struct ath9k_channel *chan)
+{
+	bool restore;
+
+	if (!ah->caldata)
+		return false;
+
+	if (!ah->caldata->rtt_done)
+		return false;
+
+	ar9003_hw_rtt_enable(ah);
+	ar9003_hw_rtt_set_mask(ah, 0x10);
+
+	if (!ath9k_hw_rfbus_req(ah)) {
+		ath_err(ath9k_hw_common(ah), "Could not stop baseband\n");
+		restore = false;
+		goto fail;
+	}
+
+	ar9003_hw_rtt_load_hist(ah);
+	restore = ar9003_hw_rtt_force_restore(ah);
+
+fail:
+	ath9k_hw_rfbus_done(ah);
+	ar9003_hw_rtt_disable(ah);
+	return restore;
 }
diff --git a/drivers/net/wireless/ath/ath9k/ar9003_rtt.h b/drivers/net/wireless/ath/ath9k/ar9003_rtt.h
index 030758d087d..a43b30d723a 100644
--- a/drivers/net/wireless/ath/ath9k/ar9003_rtt.h
+++ b/drivers/net/wireless/ath/ath9k/ar9003_rtt.h
@@ -21,8 +21,9 @@ void ar9003_hw_rtt_enable(struct ath_hw *ah);
 void ar9003_hw_rtt_disable(struct ath_hw *ah);
 void ar9003_hw_rtt_set_mask(struct ath_hw *ah, u32 rtt_mask);
 bool ar9003_hw_rtt_force_restore(struct ath_hw *ah);
-void ar9003_hw_rtt_load_hist(struct ath_hw *ah, u8 chain, u32 *table);
-void ar9003_hw_rtt_fill_hist(struct ath_hw *ah, u8 chain, u32 *table);
+void ar9003_hw_rtt_load_hist(struct ath_hw *ah);
+void ar9003_hw_rtt_fill_hist(struct ath_hw *ah);
 void ar9003_hw_rtt_clear_hist(struct ath_hw *ah);
+bool ar9003_hw_rtt_restore(struct ath_hw *ah, struct ath9k_channel *chan);
 
 #endif
diff --git a/drivers/net/wireless/ath/ath9k/ar9462_2p0_initvals.h b/drivers/net/wireless/ath/ath9k/ar9462_2p0_initvals.h
index b6ba1e8149b..1d6658e139b 100644
--- a/drivers/net/wireless/ath/ath9k/ar9462_2p0_initvals.h
+++ b/drivers/net/wireless/ath/ath9k/ar9462_2p0_initvals.h
@@ -1115,9 +1115,9 @@ static const u32 ar9462_2p0_mac_core[][2] = {
 	{0x000081f8, 0x00000000},
 	{0x000081fc, 0x00000000},
 	{0x00008240, 0x00100000},
-	{0x00008244, 0x0010f400},
+	{0x00008244, 0x0010f424},
 	{0x00008248, 0x00000800},
-	{0x0000824c, 0x0001e800},
+	{0x0000824c, 0x0001e848},
 	{0x00008250, 0x00000000},
 	{0x00008254, 0x00000000},
 	{0x00008258, 0x00000000},
@@ -1448,16 +1448,4 @@ static const u32 ar9462_common_mixed_rx_gain_table_2p0[][2] = {
 	{0x0000b1fc, 0x00000196},
 };
 
-static const u32 ar9462_2p0_BTCOEX_MAX_TXPWR_table[][2] = {
-	/* Addr      allmodes  */
-	{0x000018c0, 0x10101010},
-	{0x000018c4, 0x10101010},
-	{0x000018c8, 0x10101010},
-	{0x000018cc, 0x10101010},
-	{0x000018d0, 0x10101010},
-	{0x000018d4, 0x10101010},
-	{0x000018d8, 0x10101010},
-	{0x000018dc, 0x10101010},
-};
-
 #endif /* INITVALS_9462_2P0_H */
diff --git a/drivers/net/wireless/ath/ath9k/ath9k.h b/drivers/net/wireless/ath/ath9k/ath9k.h
index 8c84049682a..a277cf6f339 100644
--- a/drivers/net/wireless/ath/ath9k/ath9k.h
+++ b/drivers/net/wireless/ath/ath9k/ath9k.h
@@ -26,6 +26,7 @@
 #include "debug.h"
 #include "common.h"
 #include "mci.h"
+#include "dfs.h"
 
 /*
  * Header for the ath9k.ko driver core *only* -- hw code nor any other driver
@@ -369,7 +370,7 @@ struct ath_vif {
  * number of beacon intervals, the game's up.
  */
 #define BSTUCK_THRESH           	9
-#define	ATH_BCBUF               	4
+#define	ATH_BCBUF               	8
 #define ATH_DEFAULT_BINTVAL     	100 /* TU */
 #define ATH_DEFAULT_BMISS_LIMIT 	10
 #define IEEE80211_MS_TO_TU(x)           (((x) * 1000) / 1024)
@@ -430,6 +431,8 @@ void ath9k_set_beaconing_status(struct ath_softc *sc, bool status);
 void ath_reset_work(struct work_struct *work);
 void ath_hw_check(struct work_struct *work);
 void ath_hw_pll_work(struct work_struct *work);
+void ath_rx_poll(unsigned long data);
+void ath_start_rx_poll(struct ath_softc *sc, u8 nbeacon);
 void ath_paprd_calibrate(struct work_struct *work);
 void ath_ani_calibrate(unsigned long data);
 void ath_start_ani(struct ath_common *common);
@@ -670,6 +673,7 @@ struct ath_softc {
 	struct ath_beacon_config cur_beacon_conf;
 	struct delayed_work tx_complete_work;
 	struct delayed_work hw_pll_work;
+	struct timer_list rx_poll_timer;
 
 #ifdef CONFIG_ATH9K_BTCOEX_SUPPORT
 	struct ath_btcoex btcoex;
@@ -680,6 +684,7 @@ struct ath_softc {
 
 	struct ath_ant_comb ant_comb;
 	u8 ant_tx, ant_rx;
+	struct dfs_pattern_detector *dfs_detector;
 };
 
 void ath9k_tasklet(unsigned long data);
diff --git a/drivers/net/wireless/ath/ath9k/beacon.c b/drivers/net/wireless/ath/ath9k/beacon.c
index 626418222c8..11bc55e3d69 100644
--- a/drivers/net/wireless/ath/ath9k/beacon.c
+++ b/drivers/net/wireless/ath/ath9k/beacon.c
@@ -91,7 +91,7 @@ static void ath_beacon_setup(struct ath_softc *sc, struct ieee80211_vif *vif,
 	info.txpower = MAX_RATE_POWER;
 	info.keyix = ATH9K_TXKEYIX_INVALID;
 	info.keytype = ATH9K_KEY_TYPE_CLEAR;
-	info.flags = ATH9K_TXDESC_NOACK | ATH9K_TXDESC_INTREQ;
+	info.flags = ATH9K_TXDESC_NOACK | ATH9K_TXDESC_CLRDMASK;
 
 	info.buf_addr[0] = bf->bf_buf_addr;
 	info.buf_len[0] = roundup(skb->len, 4);
@@ -359,6 +359,11 @@ void ath_beacon_tasklet(unsigned long data)
 	int slot;
 	u32 bfaddr, bc = 0;
 
+	if (work_pending(&sc->hw_reset_work)) {
+		ath_dbg(common, RESET,
+			"reset work is pending, skip beaconing now\n");
+		return;
+	}
 	/*
 	 * Check if the previous beacon has gone out.  If
 	 * not don't try to post another, skip this period
@@ -369,6 +374,9 @@ void ath_beacon_tasklet(unsigned long data)
 	if (ath9k_hw_numtxpending(ah, sc->beacon.beaconq) != 0) {
 		sc->beacon.bmisscnt++;
 
+		if (!ath9k_hw_check_alive(ah))
+			ieee80211_queue_work(sc->hw, &sc->hw_check_work);
+
 		if (sc->beacon.bmisscnt < BSTUCK_THRESH * sc->nbcnvifs) {
 			ath_dbg(common, BSTUCK,
 				"missed %u consecutive beacons\n",
@@ -378,6 +386,7 @@ void ath_beacon_tasklet(unsigned long data)
 				ath9k_hw_bstuck_nfcal(ah);
 		} else if (sc->beacon.bmisscnt >= BSTUCK_THRESH) {
 			ath_dbg(common, BSTUCK, "beacon is officially stuck\n");
+			sc->beacon.bmisscnt = 0;
 			sc->sc_flags |= SC_OP_TSF_RESET;
 			ieee80211_queue_work(sc->hw, &sc->hw_reset_work);
 		}
@@ -650,6 +659,8 @@ static void ath_beacon_config_adhoc(struct ath_softc *sc,
 	u32 tsf, intval, nexttbtt;
 
 	ath9k_reset_beacon_status(sc);
+	if (!(sc->sc_flags & SC_OP_BEACONS))
+		ath9k_hw_settsf64(ah, sc->beacon.bc_tstamp);
 
 	intval = TU_TO_USEC(conf->beacon_interval);
 	tsf = roundup(ath9k_hw_gettsf32(ah) + TU_TO_USEC(FUDGE), intval);
@@ -806,8 +817,10 @@ void ath9k_set_beaconing_status(struct ath_softc *sc, bool status)
 {
 	struct ath_hw *ah = sc->sc_ah;
 
-	if (!ath_has_valid_bslot(sc))
+	if (!ath_has_valid_bslot(sc)) {
+		sc->sc_flags &= ~SC_OP_BEACONS;
 		return;
+	}
 
 	ath9k_ps_wakeup(sc);
 	if (status) {
diff --git a/drivers/net/wireless/ath/ath9k/btcoex.c b/drivers/net/wireless/ath/ath9k/btcoex.c
index ec327199341..1ca6da80d4a 100644
--- a/drivers/net/wireless/ath/ath9k/btcoex.c
+++ b/drivers/net/wireless/ath/ath9k/btcoex.c
@@ -108,9 +108,7 @@ void ath9k_hw_btcoex_init_scheme(struct ath_hw *ah)
 		return;
 	}
 
-	if (AR_SREV_9462(ah)) {
-		btcoex_hw->scheme = ATH_BTCOEX_CFG_MCI;
-	} else if (AR_SREV_9300_20_OR_LATER(ah)) {
+	if (AR_SREV_9300_20_OR_LATER(ah)) {
 		btcoex_hw->scheme = ATH_BTCOEX_CFG_3WIRE;
 		btcoex_hw->btactive_gpio = ATH_BTACTIVE_GPIO_9300;
 		btcoex_hw->wlanactive_gpio = ATH_WLANACTIVE_GPIO_9300;
@@ -284,11 +282,12 @@ void ath9k_hw_btcoex_enable(struct ath_hw *ah)
 		ath9k_hw_btcoex_enable_2wire(ah);
 		break;
 	case ATH_BTCOEX_CFG_3WIRE:
+		if (AR_SREV_9462(ah)) {
+			ath9k_hw_btcoex_enable_mci(ah);
+			return;
+		}
 		ath9k_hw_btcoex_enable_3wire(ah);
 		break;
-	case ATH_BTCOEX_CFG_MCI:
-		ath9k_hw_btcoex_enable_mci(ah);
-		return;
 	}
 
 	REG_RMW(ah, AR_GPIO_PDPU,
@@ -305,11 +304,12 @@ void ath9k_hw_btcoex_disable(struct ath_hw *ah)
 	int i;
 
 	btcoex_hw->enabled = false;
-	if (btcoex_hw->scheme == ATH_BTCOEX_CFG_MCI) {
+	if (AR_SREV_9462(ah)) {
 		ath9k_hw_btcoex_bt_stomp(ah, ATH_BTCOEX_STOMP_NONE);
 		for (i = 0; i < AR9300_NUM_BT_WEIGHTS; i++)
 			REG_WRITE(ah, AR_MCI_COEX_WL_WEIGHTS(i),
 				  btcoex_hw->wlan_weight[i]);
+		return;
 	}
 	ath9k_hw_set_gpio(ah, btcoex_hw->wlanactive_gpio, 0);
 
diff --git a/drivers/net/wireless/ath/ath9k/btcoex.h b/drivers/net/wireless/ath/ath9k/btcoex.h
index 8f93aef4414..3a1e1cfabd5 100644
--- a/drivers/net/wireless/ath/ath9k/btcoex.h
+++ b/drivers/net/wireless/ath/ath9k/btcoex.h
@@ -51,7 +51,6 @@ enum ath_btcoex_scheme {
 	ATH_BTCOEX_CFG_NONE,
 	ATH_BTCOEX_CFG_2WIRE,
 	ATH_BTCOEX_CFG_3WIRE,
-	ATH_BTCOEX_CFG_MCI,
 };
 
 struct ath9k_hw_mci {
diff --git a/drivers/net/wireless/ath/ath9k/debug.c b/drivers/net/wireless/ath/ath9k/debug.c
index ff47b32ecaf..fde700c4e49 100644
--- a/drivers/net/wireless/ath/ath9k/debug.c
+++ b/drivers/net/wireless/ath/ath9k/debug.c
@@ -380,63 +380,75 @@ static ssize_t read_file_interrupt(struct file *file, char __user *user_buf,
 				   size_t count, loff_t *ppos)
 {
 	struct ath_softc *sc = file->private_data;
-	char buf[512];
 	unsigned int len = 0;
+	int rv;
+	int mxlen = 4000;
+	char *buf = kmalloc(mxlen, GFP_KERNEL);
+	if (!buf)
+		return -ENOMEM;
+
+#define PR_IS(a, s)						\
+	do {							\
+		len += snprintf(buf + len, mxlen - len,		\
+				"%21s: %10u\n", a,		\
+				sc->debug.stats.istats.s);	\
+	} while (0)
 
 	if (sc->sc_ah->caps.hw_caps & ATH9K_HW_CAP_EDMA) {
-		len += snprintf(buf + len, sizeof(buf) - len,
-			"%8s: %10u\n", "RXLP", sc->debug.stats.istats.rxlp);
-		len += snprintf(buf + len, sizeof(buf) - len,
-			"%8s: %10u\n", "RXHP", sc->debug.stats.istats.rxhp);
-		len += snprintf(buf + len, sizeof(buf) - len,
-			"%8s: %10u\n", "WATCHDOG",
-			sc->debug.stats.istats.bb_watchdog);
+		PR_IS("RXLP", rxlp);
+		PR_IS("RXHP", rxhp);
+		PR_IS("WATHDOG", bb_watchdog);
 	} else {
-		len += snprintf(buf + len, sizeof(buf) - len,
-			"%8s: %10u\n", "RX", sc->debug.stats.istats.rxok);
+		PR_IS("RX", rxok);
 	}
-	len += snprintf(buf + len, sizeof(buf) - len,
-		"%8s: %10u\n", "RXEOL", sc->debug.stats.istats.rxeol);
-	len += snprintf(buf + len, sizeof(buf) - len,
-		"%8s: %10u\n", "RXORN", sc->debug.stats.istats.rxorn);
-	len += snprintf(buf + len, sizeof(buf) - len,
-		"%8s: %10u\n", "TX", sc->debug.stats.istats.txok);
-	len += snprintf(buf + len, sizeof(buf) - len,
-		"%8s: %10u\n", "TXURN", sc->debug.stats.istats.txurn);
-	len += snprintf(buf + len, sizeof(buf) - len,
-		"%8s: %10u\n", "MIB", sc->debug.stats.istats.mib);
-	len += snprintf(buf + len, sizeof(buf) - len,
-		"%8s: %10u\n", "RXPHY", sc->debug.stats.istats.rxphyerr);
-	len += snprintf(buf + len, sizeof(buf) - len,
-		"%8s: %10u\n", "RXKCM", sc->debug.stats.istats.rx_keycache_miss);
-	len += snprintf(buf + len, sizeof(buf) - len,
-		"%8s: %10u\n", "SWBA", sc->debug.stats.istats.swba);
-	len += snprintf(buf + len, sizeof(buf) - len,
-		"%8s: %10u\n", "BMISS", sc->debug.stats.istats.bmiss);
-	len += snprintf(buf + len, sizeof(buf) - len,
-		"%8s: %10u\n", "BNR", sc->debug.stats.istats.bnr);
-	len += snprintf(buf + len, sizeof(buf) - len,
-		"%8s: %10u\n", "CST", sc->debug.stats.istats.cst);
-	len += snprintf(buf + len, sizeof(buf) - len,
-		"%8s: %10u\n", "GTT", sc->debug.stats.istats.gtt);
-	len += snprintf(buf + len, sizeof(buf) - len,
-		"%8s: %10u\n", "TIM", sc->debug.stats.istats.tim);
-	len += snprintf(buf + len, sizeof(buf) - len,
-		"%8s: %10u\n", "CABEND", sc->debug.stats.istats.cabend);
-	len += snprintf(buf + len, sizeof(buf) - len,
-		"%8s: %10u\n", "DTIMSYNC", sc->debug.stats.istats.dtimsync);
-	len += snprintf(buf + len, sizeof(buf) - len,
-		"%8s: %10u\n", "DTIM", sc->debug.stats.istats.dtim);
-	len += snprintf(buf + len, sizeof(buf) - len,
-		"%8s: %10u\n", "TSFOOR", sc->debug.stats.istats.tsfoor);
-	len += snprintf(buf + len, sizeof(buf) - len,
-		"%8s: %10u\n", "TOTAL", sc->debug.stats.istats.total);
-
-
-	if (len > sizeof(buf))
-		len = sizeof(buf);
-
-	return simple_read_from_buffer(user_buf, count, ppos, buf, len);
+	PR_IS("RXEOL", rxeol);
+	PR_IS("RXORN", rxorn);
+	PR_IS("TX", txok);
+	PR_IS("TXURN", txurn);
+	PR_IS("MIB", mib);
+	PR_IS("RXPHY", rxphyerr);
+	PR_IS("RXKCM", rx_keycache_miss);
+	PR_IS("SWBA", swba);
+	PR_IS("BMISS", bmiss);
+	PR_IS("BNR", bnr);
+	PR_IS("CST", cst);
+	PR_IS("GTT", gtt);
+	PR_IS("TIM", tim);
+	PR_IS("CABEND", cabend);
+	PR_IS("DTIMSYNC", dtimsync);
+	PR_IS("DTIM", dtim);
+	PR_IS("TSFOOR", tsfoor);
+	PR_IS("TOTAL", total);
+
+	len += snprintf(buf + len, mxlen - len,
+			"SYNC_CAUSE stats:\n");
+
+	PR_IS("Sync-All", sync_cause_all);
+	PR_IS("RTC-IRQ", sync_rtc_irq);
+	PR_IS("MAC-IRQ", sync_mac_irq);
+	PR_IS("EEPROM-Illegal-Access", eeprom_illegal_access);
+	PR_IS("APB-Timeout", apb_timeout);
+	PR_IS("PCI-Mode-Conflict", pci_mode_conflict);
+	PR_IS("HOST1-Fatal", host1_fatal);
+	PR_IS("HOST1-Perr", host1_perr);
+	PR_IS("TRCV-FIFO-Perr", trcv_fifo_perr);
+	PR_IS("RADM-CPL-EP", radm_cpl_ep);
+	PR_IS("RADM-CPL-DLLP-Abort", radm_cpl_dllp_abort);
+	PR_IS("RADM-CPL-TLP-Abort", radm_cpl_tlp_abort);
+	PR_IS("RADM-CPL-ECRC-Err", radm_cpl_ecrc_err);
+	PR_IS("RADM-CPL-Timeout", radm_cpl_timeout);
+	PR_IS("Local-Bus-Timeout", local_timeout);
+	PR_IS("PM-Access", pm_access);
+	PR_IS("MAC-Awake", mac_awake);
+	PR_IS("MAC-Asleep", mac_asleep);
+	PR_IS("MAC-Sleep-Access", mac_sleep_access);
+
+	if (len > mxlen)
+		len = mxlen;
+
+	rv = simple_read_from_buffer(user_buf, count, ppos, buf, len);
+	kfree(buf);
+	return rv;
 }
 
 static const struct file_operations fops_interrupt = {
@@ -524,6 +536,7 @@ static ssize_t read_file_xmit(struct file *file, char __user *user_buf,
 	PR("hw-put-tx-buf:   ", puttxbuf);
 	PR("hw-tx-start:     ", txstart);
 	PR("hw-tx-proc-desc: ", txprocdesc);
+	PR("TX-Failed:       ", txfailed);
 	len += snprintf(buf + len, size - len,
 			"%s%11p%11p%10p%10p\n", "txq-memory-address:",
 			sc->tx.txq_map[WME_AC_BE],
@@ -880,6 +893,13 @@ static ssize_t read_file_recv(struct file *file, char __user *user_buf,
 	len += snprintf(buf + len, size - len, "%22s : %10u\n", s, \
 			sc->debug.stats.rxstats.phy_err_stats[p]);
 
+#define RXS_ERR(s, e)					    \
+	do {						    \
+		len += snprintf(buf + len, size - len,	    \
+				"%22s : %10u\n", s,	    \
+				sc->debug.stats.rxstats.e); \
+	} while (0)
+
 	struct ath_softc *sc = file->private_data;
 	char *buf;
 	unsigned int len = 0, size = 1600;
@@ -889,27 +909,18 @@ static ssize_t read_file_recv(struct file *file, char __user *user_buf,
 	if (buf == NULL)
 		return -ENOMEM;
 
-	len += snprintf(buf + len, size - len,
-			"%22s : %10u\n", "CRC ERR",
-			sc->debug.stats.rxstats.crc_err);
-	len += snprintf(buf + len, size - len,
-			"%22s : %10u\n", "DECRYPT CRC ERR",
-			sc->debug.stats.rxstats.decrypt_crc_err);
-	len += snprintf(buf + len, size - len,
-			"%22s : %10u\n", "PHY ERR",
-			sc->debug.stats.rxstats.phy_err);
-	len += snprintf(buf + len, size - len,
-			"%22s : %10u\n", "MIC ERR",
-			sc->debug.stats.rxstats.mic_err);
-	len += snprintf(buf + len, size - len,
-			"%22s : %10u\n", "PRE-DELIM CRC ERR",
-			sc->debug.stats.rxstats.pre_delim_crc_err);
-	len += snprintf(buf + len, size - len,
-			"%22s : %10u\n", "POST-DELIM CRC ERR",
-			sc->debug.stats.rxstats.post_delim_crc_err);
-	len += snprintf(buf + len, size - len,
-			"%22s : %10u\n", "DECRYPT BUSY ERR",
-			sc->debug.stats.rxstats.decrypt_busy_err);
+	RXS_ERR("CRC ERR", crc_err);
+	RXS_ERR("DECRYPT CRC ERR", decrypt_crc_err);
+	RXS_ERR("PHY ERR", phy_err);
+	RXS_ERR("MIC ERR", mic_err);
+	RXS_ERR("PRE-DELIM CRC ERR", pre_delim_crc_err);
+	RXS_ERR("POST-DELIM CRC ERR", post_delim_crc_err);
+	RXS_ERR("DECRYPT BUSY ERR", decrypt_busy_err);
+	RXS_ERR("RX-LENGTH-ERR", rx_len_err);
+	RXS_ERR("RX-OOM-ERR", rx_oom_err);
+	RXS_ERR("RX-RATE-ERR", rx_rate_err);
+	RXS_ERR("RX-DROP-RXFLUSH", rx_drop_rxflush);
+	RXS_ERR("RX-TOO-MANY-FRAGS", rx_too_many_frags_err);
 
 	PHY_ERR("UNDERRUN ERR", ATH9K_PHYERR_UNDERRUN);
 	PHY_ERR("TIMING ERR", ATH9K_PHYERR_TIMING);
@@ -938,12 +949,10 @@ static ssize_t read_file_recv(struct file *file, char __user *user_buf,
 	PHY_ERR("HT-LENGTH ERR", ATH9K_PHYERR_HT_LENGTH_ILLEGAL);
 	PHY_ERR("HT-RATE ERR", ATH9K_PHYERR_HT_RATE_ILLEGAL);
 
-	len += snprintf(buf + len, size - len,
-			"%22s : %10u\n", "RX-Pkts-All",
-			sc->debug.stats.rxstats.rx_pkts_all);
-	len += snprintf(buf + len, size - len,
-			"%22s : %10u\n", "RX-Bytes-All",
-			sc->debug.stats.rxstats.rx_bytes_all);
+	RXS_ERR("RX-Pkts-All", rx_pkts_all);
+	RXS_ERR("RX-Bytes-All", rx_bytes_all);
+	RXS_ERR("RX-Beacons", rx_beacons);
+	RXS_ERR("RX-Frags", rx_frags);
 
 	if (len > size)
 		len = size;
@@ -953,12 +962,12 @@ static ssize_t read_file_recv(struct file *file, char __user *user_buf,
 
 	return retval;
 
+#undef RXS_ERR
 #undef PHY_ERR
 }
 
 void ath_debug_stat_rx(struct ath_softc *sc, struct ath_rx_status *rs)
 {
-#define RX_STAT_INC(c) sc->debug.stats.rxstats.c++
 #define RX_PHY_ERR_INC(c) sc->debug.stats.rxstats.phy_err_stats[c]++
 #define RX_SAMP_DBG(c) (sc->debug.bb_mac_samp[sc->debug.sampidx].rs\
 			[sc->debug.rsidx].c)
@@ -1004,7 +1013,6 @@ void ath_debug_stat_rx(struct ath_softc *sc, struct ath_rx_status *rs)
 
 #endif
 
-#undef RX_STAT_INC
 #undef RX_PHY_ERR_INC
 #undef RX_SAMP_DBG
 }
diff --git a/drivers/net/wireless/ath/ath9k/debug.h b/drivers/net/wireless/ath/ath9k/debug.h
index 64fcfad467b..c34da09d910 100644
--- a/drivers/net/wireless/ath/ath9k/debug.h
+++ b/drivers/net/wireless/ath/ath9k/debug.h
@@ -60,6 +60,7 @@ struct ath_buf;
  * @tsfoor: TSF out of range, indicates that the corrected TSF received
  * from a beacon differs from the PCU's internal TSF by more than a
  * (programmable) threshold
+ * @local_timeout: Internal bus timeout.
  */
 struct ath_interrupt_stats {
 	u32 total;
@@ -85,8 +86,30 @@ struct ath_interrupt_stats {
 	u32 dtim;
 	u32 bb_watchdog;
 	u32 tsfoor;
+
+	/* Sync-cause stats */
+	u32 sync_cause_all;
+	u32 sync_rtc_irq;
+	u32 sync_mac_irq;
+	u32 eeprom_illegal_access;
+	u32 apb_timeout;
+	u32 pci_mode_conflict;
+	u32 host1_fatal;
+	u32 host1_perr;
+	u32 trcv_fifo_perr;
+	u32 radm_cpl_ep;
+	u32 radm_cpl_dllp_abort;
+	u32 radm_cpl_tlp_abort;
+	u32 radm_cpl_ecrc_err;
+	u32 radm_cpl_timeout;
+	u32 local_timeout;
+	u32 pm_access;
+	u32 mac_awake;
+	u32 mac_asleep;
+	u32 mac_sleep_access;
 };
 
+
 /**
  * struct ath_tx_stats - Statistics about TX
  * @tx_pkts_all:  No. of total frames transmitted, including ones that
@@ -113,6 +136,7 @@ struct ath_interrupt_stats {
  * @puttxbuf: Number of times hardware was given txbuf to write.
  * @txstart:  Number of times hardware was told to start tx.
  * @txprocdesc:  Number of times tx descriptor was processed
+ * @txfailed:  Out-of-memory or other errors in xmit path.
  */
 struct ath_tx_stats {
 	u32 tx_pkts_all;
@@ -135,8 +159,11 @@ struct ath_tx_stats {
 	u32 puttxbuf;
 	u32 txstart;
 	u32 txprocdesc;
+	u32 txfailed;
 };
 
+#define RX_STAT_INC(c) (sc->debug.stats.rxstats.c++)
+
 /**
  * struct ath_rx_stats - RX Statistics
  * @rx_pkts_all:  No. of total frames received, including ones that
@@ -153,6 +180,13 @@ struct ath_tx_stats {
  * @post_delim_crc_err: Post-Frame delimiter CRC error detections
  * @decrypt_busy_err: Decryption interruptions counter
  * @phy_err_stats: Individual PHY error statistics
+ * @rx_len_err:  No. of frames discarded due to bad length.
+ * @rx_oom_err:  No. of frames dropped due to OOM issues.
+ * @rx_rate_err:  No. of frames dropped due to rate errors.
+ * @rx_too_many_frags_err:  Frames dropped due to too-many-frags received.
+ * @rx_drop_rxflush: No. of frames dropped due to RX-FLUSH.
+ * @rx_beacons:  No. of beacons received.
+ * @rx_frags:  No. of rx-fragements received.
  */
 struct ath_rx_stats {
 	u32 rx_pkts_all;
@@ -165,6 +199,13 @@ struct ath_rx_stats {
 	u32 post_delim_crc_err;
 	u32 decrypt_busy_err;
 	u32 phy_err_stats[ATH9K_PHYERR_MAX];
+	u32 rx_len_err;
+	u32 rx_oom_err;
+	u32 rx_rate_err;
+	u32 rx_too_many_frags_err;
+	u32 rx_drop_rxflush;
+	u32 rx_beacons;
+	u32 rx_frags;
 };
 
 enum ath_reset_type {
@@ -174,6 +215,7 @@ enum ath_reset_type {
 	RESET_TYPE_TX_ERROR,
 	RESET_TYPE_TX_HANG,
 	RESET_TYPE_PLL_HANG,
+	RESET_TYPE_MAC_HANG,
 	__RESET_TYPE_MAX
 };
 
@@ -247,6 +289,8 @@ void ath_debug_stat_rx(struct ath_softc *sc, struct ath_rx_status *rs);
 
 #else
 
+#define RX_STAT_INC(c) /* NOP */
+
 static inline int ath9k_init_debug(struct ath_hw *ah)
 {
 	return 0;
diff --git a/drivers/net/wireless/ath/ath9k/dfs.c b/drivers/net/wireless/ath/ath9k/dfs.c
index f4f56aff1e9..ecc81792f2d 100644
--- a/drivers/net/wireless/ath/ath9k/dfs.c
+++ b/drivers/net/wireless/ath/ath9k/dfs.c
@@ -21,17 +21,6 @@
 #include "dfs.h"
 #include "dfs_debug.h"
 
-/*
- * TODO: move into or synchronize this with generic header
- *	 as soon as IF is defined
- */
-struct dfs_radar_pulse {
-	u16 freq;
-	u64 ts;
-	u32 width;
-	u8 rssi;
-};
-
 /* internal struct to pass radar data */
 struct ath_radar_data {
 	u8 pulse_bw_info;
@@ -60,44 +49,44 @@ static u32 dur_to_usecs(struct ath_hw *ah, u32 dur)
 #define EXT_CH_RADAR_FOUND 0x02
 static bool
 ath9k_postprocess_radar_event(struct ath_softc *sc,
-			      struct ath_radar_data *are,
-			      struct dfs_radar_pulse *drp)
+			      struct ath_radar_data *ard,
+			      struct pulse_event *pe)
 {
 	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",
-		are->pulse_bw_info,
-		are->pulse_length_pri, are->rssi,
-		are->pulse_length_ext, are->ext_rssi);
+		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.
 	 */
-	are->pulse_bw_info &= 0x03;
+	ard->pulse_bw_info &= 0x03;
 
-	switch (are->pulse_bw_info) {
+	switch (ard->pulse_bw_info) {
 	case PRI_CH_RADAR_FOUND:
 		/* radar in ctrl channel */
-		dur = are->pulse_length_pri;
+		dur = ard->pulse_length_pri;
 		DFS_STAT_INC(sc, pri_phy_errors);
 		/*
 		 * cannot use ctrl channel RSSI
 		 * if extension channel is stronger
 		 */
-		rssi = (are->ext_rssi >= (are->rssi + 3)) ? 0 : are->rssi;
+		rssi = (ard->ext_rssi >= (ard->rssi + 3)) ? 0 : ard->rssi;
 		break;
 	case EXT_CH_RADAR_FOUND:
 		/* radar in extension channel */
-		dur = are->pulse_length_ext;
+		dur = ard->pulse_length_ext;
 		DFS_STAT_INC(sc, ext_phy_errors);
 		/*
 		 * cannot use extension channel RSSI
 		 * if control channel is stronger
 		 */
-		rssi = (are->rssi >= (are->ext_rssi + 12)) ? 0 : are->ext_rssi;
+		rssi = (ard->rssi >= (ard->ext_rssi + 12)) ? 0 : ard->ext_rssi;
 		break;
 	case (PRI_CH_RADAR_FOUND | EXT_CH_RADAR_FOUND):
 		/*
@@ -107,14 +96,14 @@ ath9k_postprocess_radar_event(struct ath_softc *sc,
 		 * Radiated testing, when pulse is on DC, different pri and
 		 * ext durations are reported, so take the larger of the two
 		 */
-		if (are->pulse_length_ext >= are->pulse_length_pri)
-			dur = are->pulse_length_ext;
+		if (ard->pulse_length_ext >= ard->pulse_length_pri)
+			dur = ard->pulse_length_ext;
 		else
-			dur = are->pulse_length_pri;
+			dur = ard->pulse_length_pri;
 		DFS_STAT_INC(sc, dc_phy_errors);
 
 		/* when both are present use stronger one */
-		rssi = (are->rssi < are->ext_rssi) ? are->ext_rssi : are->rssi;
+		rssi = (ard->rssi < ard->ext_rssi) ? ard->ext_rssi : ard->rssi;
 		break;
 	default:
 		/*
@@ -137,8 +126,8 @@ ath9k_postprocess_radar_event(struct ath_softc *sc,
 	 */
 
 	/* convert duration to usecs */
-	drp->width = dur_to_usecs(sc->sc_ah, dur);
-	drp->rssi = rssi;
+	pe->width = dur_to_usecs(sc->sc_ah, dur);
+	pe->rssi = rssi;
 
 	DFS_STAT_INC(sc, pulses_detected);
 	return true;
@@ -155,15 +144,17 @@ void ath9k_dfs_process_phyerr(struct ath_softc *sc, void *data,
 	struct ath_radar_data ard;
 	u16 datalen;
 	char *vdata_end;
-	struct dfs_radar_pulse drp;
+	struct pulse_event pe;
 	struct ath_hw *ah = sc->sc_ah;
 	struct ath_common *common = ath9k_hw_common(ah);
 
-	if ((!(rs->rs_phyerr != ATH9K_PHYERR_RADAR)) &&
-	    (!(rs->rs_phyerr != ATH9K_PHYERR_FALSE_RADAR_EXT))) {
+	DFS_STAT_INC(sc, pulses_total);
+	if ((rs->rs_phyerr != ATH9K_PHYERR_RADAR) &&
+	    (rs->rs_phyerr != ATH9K_PHYERR_FALSE_RADAR_EXT)) {
 		ath_dbg(common, DFS,
 			"Error: rs_phyer=0x%x not a radar error\n",
 			rs->rs_phyerr);
+		DFS_STAT_INC(sc, pulses_no_dfs);
 		return;
 	}
 
@@ -189,27 +180,22 @@ void ath9k_dfs_process_phyerr(struct ath_softc *sc, void *data,
 	ard.pulse_bw_info = vdata_end[-1];
 	ard.pulse_length_ext = vdata_end[-2];
 	ard.pulse_length_pri = vdata_end[-3];
-
-	ath_dbg(common, DFS,
-		"bw_info=%d, length_pri=%d, length_ext=%d, "
-		"rssi_pri=%d, rssi_ext=%d\n",
-		ard.pulse_bw_info, ard.pulse_length_pri, ard.pulse_length_ext,
-		ard.rssi, ard.ext_rssi);
-
-	drp.freq = ah->curchan->channel;
-	drp.ts = mactime;
-	if (ath9k_postprocess_radar_event(sc, &ard, &drp)) {
+	pe.freq = ah->curchan->channel;
+	pe.ts = mactime;
+	if (ath9k_postprocess_radar_event(sc, &ard, &pe)) {
+		struct dfs_pattern_detector *pd = sc->dfs_detector;
 		static u64 last_ts;
 		ath_dbg(common, DFS,
 			"ath9k_dfs_process_phyerr: channel=%d, ts=%llu, "
 			"width=%d, rssi=%d, delta_ts=%llu\n",
-			drp.freq, drp.ts, drp.width, drp.rssi, drp.ts-last_ts);
-		last_ts = drp.ts;
-		/*
-		 * TODO: forward pulse to pattern detector
-		 *
-		 * ieee80211_add_radar_pulse(drp.freq, drp.ts,
-		 *                           drp.width, drp.rssi);
-		 */
+			pe.freq, pe.ts, pe.width, pe.rssi, pe.ts-last_ts);
+		last_ts = pe.ts;
+		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
+			 */
+		}
 	}
 }
diff --git a/drivers/net/wireless/ath/ath9k/dfs.h b/drivers/net/wireless/ath/ath9k/dfs.h
index c2412857f12..3c839f06a06 100644
--- a/drivers/net/wireless/ath/ath9k/dfs.h
+++ b/drivers/net/wireless/ath/ath9k/dfs.h
@@ -17,6 +17,7 @@
 
 #ifndef ATH9K_DFS_H
 #define ATH9K_DFS_H
+#include "dfs_pattern_detector.h"
 
 #if defined(CONFIG_ATH9K_DFS_CERTIFIED)
 /**
@@ -31,13 +32,14 @@
  *
  * The radar information provided as raw payload data is validated and
  * filtered for false pulses. Events passing all tests are forwarded to
- * the upper layer for pattern detection.
+ * the DFS detector for pattern detection.
  */
 void ath9k_dfs_process_phyerr(struct ath_softc *sc, void *data,
 			      struct ath_rx_status *rs, u64 mactime);
 #else
-static inline void ath9k_dfs_process_phyerr(struct ath_softc *sc, void *data,
-					    struct ath_rx_status *rs, u64 mactime) { }
+static inline void
+ath9k_dfs_process_phyerr(struct ath_softc *sc, void *data,
+			 struct ath_rx_status *rs, u64 mactime) { }
 #endif
 
 #endif /* ATH9K_DFS_H */
diff --git a/drivers/net/wireless/ath/ath9k/dfs_debug.c b/drivers/net/wireless/ath/ath9k/dfs_debug.c
index 4364c103ed3..55d28072ade 100644
--- a/drivers/net/wireless/ath/ath9k/dfs_debug.c
+++ b/drivers/net/wireless/ath/ath9k/dfs_debug.c
@@ -21,9 +21,15 @@
 #include "ath9k.h"
 #include "dfs_debug.h"
 
+
+struct ath_dfs_pool_stats global_dfs_pool_stats = { 0 };
+
 #define ATH9K_DFS_STAT(s, p) \
 	len += snprintf(buf + len, size - len, "%28s : %10u\n", s, \
 			sc->debug.stats.dfs_stats.p);
+#define ATH9K_DFS_POOL_STAT(s, p) \
+	len += snprintf(buf + len, size - len, "%28s : %10u\n", s, \
+			global_dfs_pool_stats.p);
 
 static ssize_t read_file_dfs(struct file *file, char __user *user_buf,
 			     size_t count, loff_t *ppos)
@@ -43,6 +49,9 @@ static ssize_t read_file_dfs(struct file *file, char __user *user_buf,
 			hw_ver->macVersion, hw_ver->macRev,
 			(sc->sc_ah->caps.hw_caps & ATH9K_HW_CAP_DFS) ?
 					"enabled" : "disabled");
+	len += snprintf(buf + len, size - len, "Pulse detector statistics:\n");
+	ATH9K_DFS_STAT("pulse events reported   ", pulses_total);
+	ATH9K_DFS_STAT("invalid pulse events    ", pulses_no_dfs);
 	ATH9K_DFS_STAT("DFS pulses detected     ", pulses_detected);
 	ATH9K_DFS_STAT("Datalen discards        ", datalen_discards);
 	ATH9K_DFS_STAT("RSSI discards           ", rssi_discards);
@@ -50,6 +59,18 @@ static ssize_t read_file_dfs(struct file *file, char __user *user_buf,
 	ATH9K_DFS_STAT("Primary channel pulses  ", pri_phy_errors);
 	ATH9K_DFS_STAT("Secondary channel pulses", ext_phy_errors);
 	ATH9K_DFS_STAT("Dual channel pulses     ", dc_phy_errors);
+	len += snprintf(buf + len, size - len, "Radar detector statistics "
+			"(current DFS region: %d)\n", sc->dfs_detector->region);
+	ATH9K_DFS_STAT("Pulse events processed  ", pulses_processed);
+	ATH9K_DFS_STAT("Radars detected         ", radar_detected);
+	len += snprintf(buf + len, size - len, "Global Pool statistics:\n");
+	ATH9K_DFS_POOL_STAT("Pool references         ", pool_reference);
+	ATH9K_DFS_POOL_STAT("Pulses allocated        ", pulse_allocated);
+	ATH9K_DFS_POOL_STAT("Pulses alloc error      ", pulse_alloc_error);
+	ATH9K_DFS_POOL_STAT("Pulses in use           ", pulse_used);
+	ATH9K_DFS_POOL_STAT("Seqs. allocated         ", pseq_allocated);
+	ATH9K_DFS_POOL_STAT("Seqs. alloc error       ", pseq_alloc_error);
+	ATH9K_DFS_POOL_STAT("Seqs. in use            ", pseq_used);
 
 	if (len > size)
 		len = size;
@@ -60,8 +81,33 @@ static ssize_t read_file_dfs(struct file *file, char __user *user_buf,
 	return retval;
 }
 
+/* magic number to prevent accidental reset of DFS statistics */
+#define DFS_STATS_RESET_MAGIC	0x80000000
+static ssize_t write_file_dfs(struct file *file, const char __user *user_buf,
+			      size_t count, loff_t *ppos)
+{
+	struct ath_softc *sc = file->private_data;
+	unsigned long val;
+	char buf[32];
+	ssize_t len;
+
+	len = min(count, sizeof(buf) - 1);
+	if (copy_from_user(buf, user_buf, len))
+		return -EFAULT;
+
+	buf[len] = '\0';
+	if (strict_strtoul(buf, 0, &val))
+		return -EINVAL;
+
+	if (val == DFS_STATS_RESET_MAGIC)
+		memset(&sc->debug.stats.dfs_stats, 0,
+		       sizeof(sc->debug.stats.dfs_stats));
+	return count;
+}
+
 static const struct file_operations fops_dfs_stats = {
 	.read = read_file_dfs,
+	.write = write_file_dfs,
 	.open = simple_open,
 	.owner = THIS_MODULE,
 	.llseek = default_llseek,
diff --git a/drivers/net/wireless/ath/ath9k/dfs_debug.h b/drivers/net/wireless/ath/ath9k/dfs_debug.h
index 4911724cb44..e36810a4b58 100644
--- a/drivers/net/wireless/ath/ath9k/dfs_debug.h
+++ b/drivers/net/wireless/ath/ath9k/dfs_debug.h
@@ -22,17 +22,23 @@
 #include "hw.h"
 
 /**
- * struct ath_dfs_stats - DFS Statistics
- *
- * @pulses_detected:  No. of pulses detected so far
- * @datalen_discards: No. of pulses discarded due to invalid datalen
- * @rssi_discards:    No. of pulses discarded due to invalid RSSI
- * @bwinfo_discards:  No. of pulses discarded due to invalid BW info
- * @pri_phy_errors:   No. of pulses reported for primary channel
- * @ext_phy_errors:   No. of pulses reported for extension channel
- * @dc_phy_errors:    No. of pulses reported for primary + extension channel
+ * struct ath_dfs_stats - DFS Statistics per wiphy
+ * @pulses_total:     pulses reported by HW
+ * @pulses_no_dfs:    pulses wrongly reported as DFS
+ * @pulses_detected:  pulses detected so far
+ * @datalen_discards: pulses discarded due to invalid datalen
+ * @rssi_discards:    pulses discarded due to invalid RSSI
+ * @bwinfo_discards:  pulses discarded due to invalid BW info
+ * @pri_phy_errors:   pulses reported for primary channel
+ * @ext_phy_errors:   pulses reported for extension channel
+ * @dc_phy_errors:    pulses reported for primary + extension channel
+ * @pulses_processed: pulses forwarded to detector
+ * @radar_detected:   radars detected
  */
 struct ath_dfs_stats {
+	/* pulse stats */
+	u32 pulses_total;
+	u32 pulses_no_dfs;
 	u32 pulses_detected;
 	u32 datalen_discards;
 	u32 rssi_discards;
@@ -40,18 +46,39 @@ struct ath_dfs_stats {
 	u32 pri_phy_errors;
 	u32 ext_phy_errors;
 	u32 dc_phy_errors;
+	/* pattern detection stats */
+	u32 pulses_processed;
+	u32 radar_detected;
 };
 
+/**
+ * struct ath_dfs_pool_stats - DFS Statistics for global pools
+ */
+struct ath_dfs_pool_stats {
+	u32 pool_reference;
+	u32 pulse_allocated;
+	u32 pulse_alloc_error;
+	u32 pulse_used;
+	u32 pseq_allocated;
+	u32 pseq_alloc_error;
+	u32 pseq_used;
+};
 #if defined(CONFIG_ATH9K_DFS_DEBUGFS)
 
 #define DFS_STAT_INC(sc, c) (sc->debug.stats.dfs_stats.c++)
 void ath9k_dfs_init_debug(struct ath_softc *sc);
 
+#define DFS_POOL_STAT_INC(c) (global_dfs_pool_stats.c++)
+#define DFS_POOL_STAT_DEC(c) (global_dfs_pool_stats.c--)
+extern struct ath_dfs_pool_stats global_dfs_pool_stats;
+
 #else
 
 #define DFS_STAT_INC(sc, c) do { } while (0)
 static inline void ath9k_dfs_init_debug(struct ath_softc *sc) { }
 
+#define DFS_POOL_STAT_INC(c) do { } while (0)
+#define DFS_POOL_STAT_DEC(c) do { } while (0)
 #endif /* CONFIG_ATH9K_DFS_DEBUGFS */
 
 #endif /* ATH9K_DFS_DEBUG_H */
diff --git a/drivers/net/wireless/ath/ath9k/dfs_pattern_detector.c b/drivers/net/wireless/ath/ath9k/dfs_pattern_detector.c
new file mode 100644
index 00000000000..ea2a6cf7ef2
--- /dev/null
+++ b/drivers/net/wireless/ath/ath9k/dfs_pattern_detector.c
@@ -0,0 +1,300 @@
+/*
+ * Copyright (c) 2012 Neratec Solutions AG
+ *
+ * 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/export.h>
+
+#include "dfs_pattern_detector.h"
+#include "dfs_pri_detector.h"
+
+/*
+ * tolerated deviation of radar time stamp in usecs on both sides
+ * TODO: this might need to be HW-dependent
+ */
+#define PRI_TOLERANCE	16
+
+/**
+ * struct radar_types - contains array of patterns defined for one DFS domain
+ * @domain: DFS regulatory domain
+ * @num_radar_types: number of radar types to follow
+ * @radar_types: radar types array
+ */
+struct radar_types {
+	enum nl80211_dfs_regions region;
+	u32 num_radar_types;
+	const struct radar_detector_specs *radar_types;
+};
+
+/* percentage on ppb threshold to trigger detection */
+#define MIN_PPB_THRESH	50
+#define PPB_THRESH(PPB) ((PPB * MIN_PPB_THRESH + 50) / 100)
+#define PRF2PRI(PRF) ((1000000 + PRF / 2) / PRF)
+
+#define ETSI_PATTERN(ID, WMIN, WMAX, PMIN, PMAX, PRF, PPB)	\
+{								\
+	ID, WMIN, WMAX, (PRF2PRI(PMAX) - PRI_TOLERANCE),	\
+	(PRF2PRI(PMIN) * PRF + PRI_TOLERANCE), PRF, PPB * PRF,	\
+	PPB_THRESH(PPB), PRI_TOLERANCE,				\
+}
+
+/* radar types as defined by ETSI EN-301-893 v1.5.1 */
+static const struct radar_detector_specs etsi_radar_ref_types_v15[] = {
+	ETSI_PATTERN(0,  0,  1,  700,  700, 1, 18),
+	ETSI_PATTERN(1,  0,  5,  200, 1000, 1, 10),
+	ETSI_PATTERN(2,  0, 15,  200, 1600, 1, 15),
+	ETSI_PATTERN(3,  0, 15, 2300, 4000, 1, 25),
+	ETSI_PATTERN(4, 20, 30, 2000, 4000, 1, 20),
+	ETSI_PATTERN(5,  0,  2,  300,  400, 3, 10),
+	ETSI_PATTERN(6,  0,  2,  400, 1200, 3, 15),
+};
+
+static const struct radar_types etsi_radar_types_v15 = {
+	.region			= NL80211_DFS_ETSI,
+	.num_radar_types	= ARRAY_SIZE(etsi_radar_ref_types_v15),
+	.radar_types		= etsi_radar_ref_types_v15,
+};
+
+/* for now, we support ETSI radar types, FCC and JP are TODO */
+static const struct radar_types *dfs_domains[] = {
+	&etsi_radar_types_v15,
+};
+
+/**
+ * get_dfs_domain_radar_types() - get radar types for a given DFS domain
+ * @param domain DFS domain
+ * @return radar_types ptr on success, NULL if DFS domain is not supported
+ */
+static const struct radar_types *
+get_dfs_domain_radar_types(enum nl80211_dfs_regions region)
+{
+	u32 i;
+	for (i = 0; i < ARRAY_SIZE(dfs_domains); i++) {
+		if (dfs_domains[i]->region == region)
+			return dfs_domains[i];
+	}
+	return NULL;
+}
+
+/**
+ * struct channel_detector - detector elements for a DFS channel
+ * @head: list_head
+ * @freq: frequency for this channel detector in MHz
+ * @detectors: array of dynamically created detector elements for this freq
+ *
+ * Channel detectors are required to provide multi-channel DFS detection, e.g.
+ * to support off-channel scanning. A pattern detector has a list of channels
+ * radar pulses have been reported for in the past.
+ */
+struct channel_detector {
+	struct list_head head;
+	u16 freq;
+	struct pri_detector **detectors;
+};
+
+/* channel_detector_reset() - reset detector lines for a given channel */
+static void channel_detector_reset(struct dfs_pattern_detector *dpd,
+				   struct channel_detector *cd)
+{
+	u32 i;
+	if (cd == NULL)
+		return;
+	for (i = 0; i < dpd->num_radar_types; i++)
+		cd->detectors[i]->reset(cd->detectors[i], dpd->last_pulse_ts);
+}
+
+/* channel_detector_exit() - destructor */
+static void channel_detector_exit(struct dfs_pattern_detector *dpd,
+				  struct channel_detector *cd)
+{
+	u32 i;
+	if (cd == NULL)
+		return;
+	list_del(&cd->head);
+	for (i = 0; i < dpd->num_radar_types; i++) {
+		struct pri_detector *de = cd->detectors[i];
+		if (de != NULL)
+			de->exit(de);
+	}
+	kfree(cd->detectors);
+	kfree(cd);
+}
+
+static struct channel_detector *
+channel_detector_create(struct dfs_pattern_detector *dpd, u16 freq)
+{
+	u32 sz, i;
+	struct channel_detector *cd;
+
+	cd = kmalloc(sizeof(*cd), GFP_KERNEL);
+	if (cd == NULL)
+		goto fail;
+
+	INIT_LIST_HEAD(&cd->head);
+	cd->freq = freq;
+	sz = sizeof(cd->detectors) * dpd->num_radar_types;
+	cd->detectors = kzalloc(sz, GFP_KERNEL);
+	if (cd->detectors == NULL)
+		goto fail;
+
+	for (i = 0; i < dpd->num_radar_types; i++) {
+		const struct radar_detector_specs *rs = &dpd->radar_spec[i];
+		struct pri_detector *de = pri_detector_init(rs);
+		if (de == NULL)
+			goto fail;
+		cd->detectors[i] = de;
+	}
+	list_add(&cd->head, &dpd->channel_detectors);
+	return cd;
+
+fail:
+	pr_err("failed to allocate channel_detector for freq=%d\n", freq);
+	channel_detector_exit(dpd, cd);
+	return NULL;
+}
+
+/**
+ * channel_detector_get() - get channel detector for given frequency
+ * @param dpd instance pointer
+ * @param freq frequency in MHz
+ * @return pointer to channel detector on success, NULL otherwise
+ *
+ * Return existing channel detector for the given frequency or return a
+ * newly create one.
+ */
+static struct channel_detector *
+channel_detector_get(struct dfs_pattern_detector *dpd, u16 freq)
+{
+	struct channel_detector *cd;
+	list_for_each_entry(cd, &dpd->channel_detectors, head) {
+		if (cd->freq == freq)
+			return cd;
+	}
+	return channel_detector_create(dpd, freq);
+}
+
+/*
+ * DFS Pattern Detector
+ */
+
+/* dpd_reset(): reset all channel detectors */
+static void dpd_reset(struct dfs_pattern_detector *dpd)
+{
+	struct channel_detector *cd;
+	if (!list_empty(&dpd->channel_detectors))
+		list_for_each_entry(cd, &dpd->channel_detectors, head)
+			channel_detector_reset(dpd, cd);
+
+}
+static void dpd_exit(struct dfs_pattern_detector *dpd)
+{
+	struct channel_detector *cd, *cd0;
+	if (!list_empty(&dpd->channel_detectors))
+		list_for_each_entry_safe(cd, cd0, &dpd->channel_detectors, head)
+			channel_detector_exit(dpd, cd);
+	kfree(dpd);
+}
+
+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
+		 */
+		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");
+		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) {
+			channel_detector_reset(dpd, cd);
+			return true;
+		}
+	}
+	return false;
+}
+
+static bool dpd_set_domain(struct dfs_pattern_detector *dpd,
+			   enum nl80211_dfs_regions region)
+{
+	const struct radar_types *rt;
+	struct channel_detector *cd, *cd0;
+
+	if (dpd->region == region)
+		return true;
+
+	dpd->region = NL80211_DFS_UNSET;
+
+	rt = get_dfs_domain_radar_types(region);
+	if (rt == NULL)
+		return false;
+
+	/* delete all channel detectors for previous DFS domain */
+	if (!list_empty(&dpd->channel_detectors))
+		list_for_each_entry_safe(cd, cd0, &dpd->channel_detectors, head)
+			channel_detector_exit(dpd, cd);
+	dpd->radar_spec = rt->radar_types;
+	dpd->num_radar_types = rt->num_radar_types;
+
+	dpd->region = region;
+	return true;
+}
+
+static struct dfs_pattern_detector default_dpd = {
+	.exit		= dpd_exit,
+	.set_domain	= dpd_set_domain,
+	.add_pulse	= dpd_add_pulse,
+	.region		= NL80211_DFS_UNSET,
+};
+
+struct dfs_pattern_detector *
+dfs_pattern_detector_init(enum nl80211_dfs_regions region)
+{
+	struct dfs_pattern_detector *dpd;
+	dpd = kmalloc(sizeof(*dpd), GFP_KERNEL);
+	if (dpd == NULL) {
+		pr_err("allocation of dfs_pattern_detector failed\n");
+		return NULL;
+	}
+	*dpd = default_dpd;
+	INIT_LIST_HEAD(&dpd->channel_detectors);
+
+	if (dpd->set_domain(dpd, region))
+		return dpd;
+
+	pr_err("Could not set DFS domain to %d. ", region);
+	return NULL;
+}
+EXPORT_SYMBOL(dfs_pattern_detector_init);
diff --git a/drivers/net/wireless/ath/ath9k/dfs_pattern_detector.h b/drivers/net/wireless/ath/ath9k/dfs_pattern_detector.h
new file mode 100644
index 00000000000..fd0328a3099
--- /dev/null
+++ b/drivers/net/wireless/ath/ath9k/dfs_pattern_detector.h
@@ -0,0 +1,104 @@
+/*
+ * Copyright (c) 2012 Neratec Solutions AG
+ *
+ * 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 DFS_PATTERN_DETECTOR_H
+#define DFS_PATTERN_DETECTOR_H
+
+#include <linux/types.h>
+#include <linux/list.h>
+#include <linux/nl80211.h>
+
+/**
+ * struct pulse_event - describing pulses reported by PHY
+ * @ts: pulse time stamp in us
+ * @freq: channel frequency in MHz
+ * @width: pulse duration in us
+ * @rssi: rssi of radar event
+ */
+struct pulse_event {
+	u64 ts;
+	u16 freq;
+	u8 width;
+	u8 rssi;
+};
+
+/**
+ * struct radar_detector_specs - detector specs for a radar pattern type
+ * @type_id: pattern type, as defined by regulatory
+ * @width_min: minimum radar pulse width in [us]
+ * @width_max: maximum radar pulse width in [us]
+ * @pri_min: minimum pulse repetition interval in [us] (including tolerance)
+ * @pri_max: minimum pri in [us] (including tolerance)
+ * @num_pri: maximum number of different pri for this type
+ * @ppb: pulses per bursts for this type
+ * @ppb_thresh: number of pulses required to trigger detection
+ * @max_pri_tolerance: pulse time stamp tolerance on both sides [us]
+ */
+struct radar_detector_specs {
+	u8 type_id;
+	u8 width_min;
+	u8 width_max;
+	u16 pri_min;
+	u16 pri_max;
+	u8 num_pri;
+	u8 ppb;
+	u8 ppb_thresh;
+	u8 max_pri_tolerance;
+};
+
+/**
+ * struct dfs_pattern_detector - DFS pattern detector
+ * @exit(): destructor
+ * @set_domain(): set DFS domain, resets detector lines upon domain changes
+ * @add_pulse(): add radar pulse to detector, returns true on detection
+ * @region: active DFS region, NL80211_DFS_UNSET until set
+ * @num_radar_types: number of different radar types
+ * @last_pulse_ts: time stamp of last valid pulse in usecs
+ * @radar_detector_specs: array of radar detection specs
+ * @channel_detectors: list connecting channel_detector elements
+ */
+struct dfs_pattern_detector {
+	void (*exit)(struct dfs_pattern_detector *dpd);
+	bool (*set_domain)(struct dfs_pattern_detector *dpd,
+			   enum nl80211_dfs_regions region);
+	bool (*add_pulse)(struct dfs_pattern_detector *dpd,
+			  struct pulse_event *pe);
+
+	enum nl80211_dfs_regions region;
+	u8 num_radar_types;
+	u64 last_pulse_ts;
+
+	const struct radar_detector_specs *radar_spec;
+	struct list_head channel_detectors;
+};
+
+/**
+ * dfs_pattern_detector_init() - constructor for pattern detector class
+ * @param region: DFS domain to be used, can be NL80211_DFS_UNSET at creation
+ * @return instance pointer on success, NULL otherwise
+ */
+#if defined(CONFIG_ATH9K_DFS_CERTIFIED)
+extern struct dfs_pattern_detector *
+dfs_pattern_detector_init(enum nl80211_dfs_regions region);
+#else
+static inline struct dfs_pattern_detector *
+dfs_pattern_detector_init(enum nl80211_dfs_regions region)
+{
+	return NULL;
+}
+#endif /* CONFIG_ATH9K_DFS_CERTIFIED */
+
+#endif /* DFS_PATTERN_DETECTOR_H */
diff --git a/drivers/net/wireless/ath/ath9k/dfs_pri_detector.c b/drivers/net/wireless/ath/ath9k/dfs_pri_detector.c
new file mode 100644
index 00000000000..91b8dceeadb
--- /dev/null
+++ b/drivers/net/wireless/ath/ath9k/dfs_pri_detector.c
@@ -0,0 +1,452 @@
+/*
+ * Copyright (c) 2012 Neratec Solutions AG
+ *
+ * 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/spinlock.h>
+
+#include "ath9k.h"
+#include "dfs_pattern_detector.h"
+#include "dfs_pri_detector.h"
+#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
+ */
+struct pulse_elem {
+	struct list_head head;
+	u64 ts;
+};
+
+/**
+ * pde_get_multiple() - get number of multiples considering a given tolerance
+ * @return factor if abs(val - factor*fraction) <= tolerance, 0 otherwise
+ */
+static u32 pde_get_multiple(u32 val, u32 fraction, u32 tolerance)
+{
+	u32 remainder;
+	u32 factor;
+	u32 delta;
+
+	if (fraction == 0)
+		return 0;
+
+	delta = (val < fraction) ? (fraction - val) : (val - fraction);
+
+	if (delta <= tolerance)
+		/* val and fraction are within tolerance */
+		return 1;
+
+	factor = val / fraction;
+	remainder = val % fraction;
+	if (remainder > tolerance) {
+		/* no exact match */
+		if ((fraction - remainder) <= tolerance)
+			/* remainder is within tolerance */
+			factor++;
+		else
+			factor = 0;
+	}
+	return factor;
+}
+
+/**
+ * DOC: Singleton Pulse and Sequence Pools
+ *
+ * Instances of pri_sequence and pulse_elem are kept in singleton pools to
+ * reduce the number of dynamic allocations. They are shared between all
+ * instances and grow up to the peak number of simultaneously used objects.
+ *
+ * Memory is freed after all references to the pools are released.
+ */
+static u32 singleton_pool_references;
+static LIST_HEAD(pulse_pool);
+static LIST_HEAD(pseq_pool);
+static DEFINE_SPINLOCK(pool_lock);
+
+static void pool_register_ref(void)
+{
+	spin_lock_bh(&pool_lock);
+	singleton_pool_references++;
+	DFS_POOL_STAT_INC(pool_reference);
+	spin_unlock_bh(&pool_lock);
+}
+
+static void pool_deregister_ref(void)
+{
+	spin_lock_bh(&pool_lock);
+	singleton_pool_references--;
+	DFS_POOL_STAT_DEC(pool_reference);
+	if (singleton_pool_references == 0) {
+		/* free singleton pools with no references left */
+		struct pri_sequence *ps, *ps0;
+		struct pulse_elem *p, *p0;
+
+		list_for_each_entry_safe(p, p0, &pulse_pool, head) {
+			list_del(&p->head);
+			DFS_POOL_STAT_DEC(pulse_allocated);
+			kfree(p);
+		}
+		list_for_each_entry_safe(ps, ps0, &pseq_pool, head) {
+			list_del(&ps->head);
+			DFS_POOL_STAT_DEC(pseq_allocated);
+			kfree(ps);
+		}
+	}
+	spin_unlock_bh(&pool_lock);
+}
+
+static void pool_put_pulse_elem(struct pulse_elem *pe)
+{
+	spin_lock_bh(&pool_lock);
+	list_add(&pe->head, &pulse_pool);
+	DFS_POOL_STAT_DEC(pulse_used);
+	spin_unlock_bh(&pool_lock);
+}
+
+static void pool_put_pseq_elem(struct pri_sequence *pse)
+{
+	spin_lock_bh(&pool_lock);
+	list_add(&pse->head, &pseq_pool);
+	DFS_POOL_STAT_DEC(pseq_used);
+	spin_unlock_bh(&pool_lock);
+}
+
+static struct pri_sequence *pool_get_pseq_elem(void)
+{
+	struct pri_sequence *pse = NULL;
+	spin_lock_bh(&pool_lock);
+	if (!list_empty(&pseq_pool)) {
+		pse = list_first_entry(&pseq_pool, struct pri_sequence, head);
+		list_del(&pse->head);
+		DFS_POOL_STAT_INC(pseq_used);
+	}
+	spin_unlock_bh(&pool_lock);
+	return pse;
+}
+
+static struct pulse_elem *pool_get_pulse_elem(void)
+{
+	struct pulse_elem *pe = NULL;
+	spin_lock_bh(&pool_lock);
+	if (!list_empty(&pulse_pool)) {
+		pe = list_first_entry(&pulse_pool, struct pulse_elem, head);
+		list_del(&pe->head);
+		DFS_POOL_STAT_INC(pulse_used);
+	}
+	spin_unlock_bh(&pool_lock);
+	return pe;
+}
+
+static struct pulse_elem *pulse_queue_get_tail(struct pri_detector *pde)
+{
+	struct list_head *l = &pde->pulses;
+	if (list_empty(l))
+		return NULL;
+	return list_entry(l->prev, struct pulse_elem, head);
+}
+
+static bool pulse_queue_dequeue(struct pri_detector *pde)
+{
+	struct pulse_elem *p = pulse_queue_get_tail(pde);
+	if (p != NULL) {
+		list_del_init(&p->head);
+		pde->count--;
+		/* give it back to pool */
+		pool_put_pulse_elem(p);
+	}
+	return (pde->count > 0);
+}
+
+/* remove pulses older than window */
+static void pulse_queue_check_window(struct pri_detector *pde)
+{
+	u64 min_valid_ts;
+	struct pulse_elem *p;
+
+	/* there is no delta time with less than 2 pulses */
+	if (pde->count < 2)
+		return;
+
+	if (pde->last_ts <= pde->window_size)
+		return;
+
+	min_valid_ts = pde->last_ts - pde->window_size;
+	while ((p = pulse_queue_get_tail(pde)) != NULL) {
+		if (p->ts >= min_valid_ts)
+			return;
+		pulse_queue_dequeue(pde);
+	}
+}
+
+static bool pulse_queue_enqueue(struct pri_detector *pde, u64 ts)
+{
+	struct pulse_elem *p = pool_get_pulse_elem();
+	if (p == NULL) {
+		p = kmalloc(sizeof(*p), GFP_KERNEL);
+		if (p == NULL) {
+			DFS_POOL_STAT_INC(pulse_alloc_error);
+			return false;
+		}
+		DFS_POOL_STAT_INC(pulse_allocated);
+		DFS_POOL_STAT_INC(pulse_used);
+	}
+	INIT_LIST_HEAD(&p->head);
+	p->ts = ts;
+	list_add(&p->head, &pde->pulses);
+	pde->count++;
+	pde->last_ts = ts;
+	pulse_queue_check_window(pde);
+	if (pde->count >= pde->max_count)
+		pulse_queue_dequeue(pde);
+	return true;
+}
+
+static bool pseq_handler_create_sequences(struct pri_detector *pde,
+					  u64 ts, u32 min_count)
+{
+	struct pulse_elem *p;
+	list_for_each_entry(p, &pde->pulses, head) {
+		struct pri_sequence ps, *new_ps;
+		struct pulse_elem *p2;
+		u32 tmp_false_count;
+		u64 min_valid_ts;
+		u32 delta_ts = ts - p->ts;
+
+		if (delta_ts < pde->rs->pri_min)
+			/* ignore too small pri */
+			continue;
+
+		if (delta_ts > pde->rs->pri_max)
+			/* stop on too large pri (sorted list) */
+			break;
+
+		/* build a new sequence with new potential pri */
+		ps.count = 2;
+		ps.count_falses = 0;
+		ps.first_ts = p->ts;
+		ps.last_ts = ts;
+		ps.pri = ts - p->ts;
+		ps.dur = ps.pri * (pde->rs->ppb - 1)
+				+ 2 * pde->rs->max_pri_tolerance;
+
+		p2 = p;
+		tmp_false_count = 0;
+		min_valid_ts = ts - ps.dur;
+		/* check which past pulses are candidates for new sequence */
+		list_for_each_entry_continue(p2, &pde->pulses, head) {
+			u32 factor;
+			if (p2->ts < min_valid_ts)
+				/* stop on crossing window border */
+				break;
+			/* check if pulse match (multi)PRI */
+			factor = pde_get_multiple(ps.last_ts - p2->ts, ps.pri,
+						  pde->rs->max_pri_tolerance);
+			if (factor > 0) {
+				ps.count++;
+				ps.first_ts = p2->ts;
+				/*
+				 * on match, add the intermediate falses
+				 * and reset counter
+				 */
+				ps.count_falses += tmp_false_count;
+				tmp_false_count = 0;
+			} else {
+				/* this is a potential false one */
+				tmp_false_count++;
+			}
+		}
+		if (ps.count < min_count)
+			/* did not reach minimum count, drop sequence */
+			continue;
+
+		/* this is a valid one, add it */
+		ps.deadline_ts = ps.first_ts + ps.dur;
+		new_ps = pool_get_pseq_elem();
+		if (new_ps == NULL) {
+			new_ps = kmalloc(sizeof(*new_ps), GFP_KERNEL);
+			if (new_ps == NULL) {
+				DFS_POOL_STAT_INC(pseq_alloc_error);
+				return false;
+			}
+			DFS_POOL_STAT_INC(pseq_allocated);
+			DFS_POOL_STAT_INC(pseq_used);
+		}
+		memcpy(new_ps, &ps, sizeof(ps));
+		INIT_LIST_HEAD(&new_ps->head);
+		list_add(&new_ps->head, &pde->sequences);
+	}
+	return true;
+}
+
+/* check new ts and add to all matching existing sequences */
+static u32
+pseq_handler_add_to_existing_seqs(struct pri_detector *pde, u64 ts)
+{
+	u32 max_count = 0;
+	struct pri_sequence *ps, *ps2;
+	list_for_each_entry_safe(ps, ps2, &pde->sequences, head) {
+		u32 delta_ts;
+		u32 factor;
+
+		/* first ensure that sequence is within window */
+		if (ts > ps->deadline_ts) {
+			list_del_init(&ps->head);
+			pool_put_pseq_elem(ps);
+			continue;
+		}
+
+		delta_ts = ts - ps->last_ts;
+		factor = pde_get_multiple(delta_ts, ps->pri,
+					  pde->rs->max_pri_tolerance);
+		if (factor > 0) {
+			ps->last_ts = ts;
+			ps->count++;
+
+			if (max_count < ps->count)
+				max_count = ps->count;
+		} else {
+			ps->count_falses++;
+		}
+	}
+	return max_count;
+}
+
+static struct pri_sequence *
+pseq_handler_check_detection(struct pri_detector *pde)
+{
+	struct pri_sequence *ps;
+
+	if (list_empty(&pde->sequences))
+		return NULL;
+
+	list_for_each_entry(ps, &pde->sequences, head) {
+		/*
+		 * we assume to have enough matching confidence if we
+		 * 1) have enough pulses
+		 * 2) have more matching than false pulses
+		 */
+		if ((ps->count >= pde->rs->ppb_thresh) &&
+		    (ps->count * pde->rs->num_pri >= ps->count_falses))
+			return ps;
+	}
+	return NULL;
+}
+
+
+/* free pulse queue and sequences list and give objects back to pools */
+static void pri_detector_reset(struct pri_detector *pde, u64 ts)
+{
+	struct pri_sequence *ps, *ps0;
+	struct pulse_elem *p, *p0;
+	list_for_each_entry_safe(ps, ps0, &pde->sequences, head) {
+		list_del_init(&ps->head);
+		pool_put_pseq_elem(ps);
+	}
+	list_for_each_entry_safe(p, p0, &pde->pulses, head) {
+		list_del_init(&p->head);
+		pool_put_pulse_elem(p);
+	}
+	pde->count = 0;
+	pde->last_ts = ts;
+}
+
+static void pri_detector_exit(struct pri_detector *de)
+{
+	pri_detector_reset(de, 0);
+	pool_deregister_ref();
+	kfree(de);
+}
+
+static bool pri_detector_add_pulse(struct pri_detector *de,
+				   struct pulse_event *event)
+{
+	u32 max_updated_seq;
+	struct pri_sequence *ps;
+	u64 ts = event->ts;
+	const struct radar_detector_specs *rs = de->rs;
+
+	/* ignore pulses not within width range */
+	if ((rs->width_min > event->width) || (rs->width_max < event->width))
+		return false;
+
+	if ((ts - de->last_ts) < rs->max_pri_tolerance)
+		/* if delta to last pulse is too short, don't use this pulse */
+		return false;
+	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;
+}
+
+struct pri_detector *
+pri_detector_init(const struct radar_detector_specs *rs)
+{
+	struct pri_detector *de;
+	de = kzalloc(sizeof(*de), GFP_KERNEL);
+	if (de == NULL)
+		return NULL;
+	de->exit = pri_detector_exit;
+	de->add_pulse = pri_detector_add_pulse;
+	de->reset = pri_detector_reset;
+
+	INIT_LIST_HEAD(&de->sequences);
+	INIT_LIST_HEAD(&de->pulses);
+	de->window_size = rs->pri_max * rs->ppb * rs->num_pri;
+	de->max_count = rs->ppb * 2;
+	de->rs = rs;
+
+	pool_register_ref();
+	return de;
+}
diff --git a/drivers/net/wireless/ath/ath9k/dfs_pri_detector.h b/drivers/net/wireless/ath/ath9k/dfs_pri_detector.h
new file mode 100644
index 00000000000..81cde9f28e4
--- /dev/null
+++ b/drivers/net/wireless/ath/ath9k/dfs_pri_detector.h
@@ -0,0 +1,52 @@
+/*
+ * Copyright (c) 2012 Neratec Solutions AG
+ *
+ * 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 DFS_PRI_DETECTOR_H
+#define DFS_PRI_DETECTOR_H
+
+#include <linux/list.h>
+
+/**
+ * struct pri_detector - PRI detector element for a dedicated radar type
+ * @exit(): destructor
+ * @add_pulse(): add pulse event, returns true 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
+ * @sequences: list_head holding potential pulse sequences
+ * @pulses: list connecting pulse_elem objects
+ * @count: number of pulses in queue
+ * @max_count: maximum number of pulses to be queued
+ * @window_size: window size back from newest pulse time stamp in usecs
+ */
+struct pri_detector {
+	void (*exit)     (struct pri_detector *de);
+	bool (*add_pulse)(struct pri_detector *de, struct pulse_event *e);
+	void (*reset)    (struct pri_detector *de, u64 ts);
+
+/* private: internal use only */
+	const struct radar_detector_specs *rs;
+	u64 last_ts;
+	struct list_head sequences;
+	struct list_head pulses;
+	u32 count;
+	u32 max_count;
+	u32 window_size;
+};
+
+struct pri_detector *pri_detector_init(const struct radar_detector_specs *rs);
+
+#endif /* DFS_PRI_DETECTOR_H */
diff --git a/drivers/net/wireless/ath/ath9k/eeprom.c b/drivers/net/wireless/ath/ath9k/eeprom.c
index c4352323331..0512397a293 100644
--- a/drivers/net/wireless/ath/ath9k/eeprom.c
+++ b/drivers/net/wireless/ath/ath9k/eeprom.c
@@ -16,14 +16,6 @@
 
 #include "hw.h"
 
-static inline u16 ath9k_hw_fbin2freq(u8 fbin, bool is2GHz)
-{
-	if (fbin == AR5416_BCHAN_UNUSED)
-		return fbin;
-
-	return (u16) ((is2GHz) ? (2300 + fbin) : (4800 + 5 * fbin));
-}
-
 void ath9k_hw_analog_shift_regwrite(struct ath_hw *ah, u32 reg, u32 val)
 {
         REG_WRITE(ah, reg, val);
@@ -290,6 +282,34 @@ u16 ath9k_hw_get_max_edge_power(u16 freq, struct cal_ctl_edges *pRdEdgesPower,
 	return twiceMaxEdgePower;
 }
 
+u16 ath9k_hw_get_scaled_power(struct ath_hw *ah, u16 power_limit,
+			      u8 antenna_reduction)
+{
+	u16 reduction = antenna_reduction;
+
+	/*
+	 * Reduce scaled Power by number of chains active
+	 * to get the per chain tx power level.
+	 */
+	switch (ar5416_get_ntxchains(ah->txchainmask)) {
+	case 1:
+		break;
+	case 2:
+		reduction += POWER_CORRECTION_FOR_TWO_CHAIN;
+		break;
+	case 3:
+		reduction += POWER_CORRECTION_FOR_THREE_CHAIN;
+		break;
+	}
+
+	if (power_limit > reduction)
+		power_limit -= reduction;
+	else
+		power_limit = 0;
+
+	return power_limit;
+}
+
 void ath9k_hw_update_regulatory_maxpower(struct ath_hw *ah)
 {
 	struct ath_common *common = ath9k_hw_common(ah);
@@ -299,10 +319,10 @@ void ath9k_hw_update_regulatory_maxpower(struct ath_hw *ah)
 	case 1:
 		break;
 	case 2:
-		regulatory->max_power_level += INCREASE_MAXPOW_BY_TWO_CHAIN;
+		regulatory->max_power_level += POWER_CORRECTION_FOR_TWO_CHAIN;
 		break;
 	case 3:
-		regulatory->max_power_level += INCREASE_MAXPOW_BY_THREE_CHAIN;
+		regulatory->max_power_level += POWER_CORRECTION_FOR_THREE_CHAIN;
 		break;
 	default:
 		ath_dbg(common, EEPROM, "Invalid chainmask configuration\n");
diff --git a/drivers/net/wireless/ath/ath9k/eeprom.h b/drivers/net/wireless/ath/ath9k/eeprom.h
index 5ff7ab96512..33acb920ed3 100644
--- a/drivers/net/wireless/ath/ath9k/eeprom.h
+++ b/drivers/net/wireless/ath/ath9k/eeprom.h
@@ -79,8 +79,8 @@
 #define SUB_NUM_CTL_MODES_AT_5G_40 2
 #define SUB_NUM_CTL_MODES_AT_2G_40 3
 
-#define INCREASE_MAXPOW_BY_TWO_CHAIN     6  /* 10*log10(2)*2 */
-#define INCREASE_MAXPOW_BY_THREE_CHAIN   10 /* 10*log10(3)*2 */
+#define POWER_CORRECTION_FOR_TWO_CHAIN		6  /* 10*log10(2)*2 */
+#define POWER_CORRECTION_FOR_THREE_CHAIN	10 /* 10*log10(3)*2 */
 
 /*
  * For AR9285 and later chipsets, the following bits are not being programmed
@@ -686,6 +686,8 @@ void ath9k_hw_get_target_powers(struct ath_hw *ah,
 				u16 numRates, bool isHt40Target);
 u16 ath9k_hw_get_max_edge_power(u16 freq, struct cal_ctl_edges *pRdEdgesPower,
 				bool is2GHz, int num_band_edges);
+u16 ath9k_hw_get_scaled_power(struct ath_hw *ah, u16 power_limit,
+			      u8 antenna_reduction);
 void ath9k_hw_update_regulatory_maxpower(struct ath_hw *ah);
 int ath9k_hw_eeprom_init(struct ath_hw *ah);
 
@@ -697,6 +699,14 @@ void ath9k_hw_get_gain_boundaries_pdadcs(struct ath_hw *ah,
 				u16 *pPdGainBoundaries, u8 *pPDADCValues,
 				u16 numXpdGains);
 
+static inline u16 ath9k_hw_fbin2freq(u8 fbin, bool is2GHz)
+{
+	if (fbin == AR5416_BCHAN_UNUSED)
+		return fbin;
+
+	return (u16) ((is2GHz) ? (2300 + fbin) : (4800 + 5 * fbin));
+}
+
 #define ar5416_get_ntxchains(_txchainmask)			\
 	(((_txchainmask >> 2) & 1) +                            \
 	 ((_txchainmask >> 1) & 1) + (_txchainmask & 1))
diff --git a/drivers/net/wireless/ath/ath9k/eeprom_9287.c b/drivers/net/wireless/ath/ath9k/eeprom_9287.c
index b34e8b2990b..aa614767adf 100644
--- a/drivers/net/wireless/ath/ath9k/eeprom_9287.c
+++ b/drivers/net/wireless/ath/ath9k/eeprom_9287.c
@@ -564,9 +564,6 @@ static void ath9k_hw_set_ar9287_power_per_rate_table(struct ath_hw *ah,
 	(((cfgCtl & ~CTL_MODE_M) | (pCtlMode[ctlMode] & CTL_MODE_M)) == \
 	 ((pEepData->ctlIndex[i] & CTL_MODE_M) | SD_NO_CTL))
 
-#define REDUCE_SCALED_POWER_BY_TWO_CHAIN     6
-#define REDUCE_SCALED_POWER_BY_THREE_CHAIN   10
-
 	u16 twiceMaxEdgePower;
 	int i;
 	struct cal_ctl_data_ar9287 *rep;
@@ -591,29 +588,8 @@ static void ath9k_hw_set_ar9287_power_per_rate_table(struct ath_hw *ah,
 	tx_chainmask = ah->txchainmask;
 
 	ath9k_hw_get_channel_centers(ah, chan, &centers);
-	scaledPower = powerLimit - antenna_reduction;
-
-	/*
-	 * Reduce scaled Power by number of chains active
-	 * to get the per chain tx power level.
-	 */
-	switch (ar5416_get_ntxchains(tx_chainmask)) {
-	case 1:
-		break;
-	case 2:
-		if (scaledPower > REDUCE_SCALED_POWER_BY_TWO_CHAIN)
-			scaledPower -= REDUCE_SCALED_POWER_BY_TWO_CHAIN;
-		else
-			scaledPower = 0;
-		break;
-	case 3:
-		if (scaledPower > REDUCE_SCALED_POWER_BY_THREE_CHAIN)
-			scaledPower -= REDUCE_SCALED_POWER_BY_THREE_CHAIN;
-		else
-			scaledPower = 0;
-		break;
-	}
-	scaledPower = max((u16)0, scaledPower);
+	scaledPower = ath9k_hw_get_scaled_power(ah, powerLimit,
+						antenna_reduction);
 
 	/*
 	 * Get TX power from EEPROM.
@@ -786,8 +762,6 @@ static void ath9k_hw_set_ar9287_power_per_rate_table(struct ath_hw *ah,
 
 #undef CMP_CTL
 #undef CMP_NO_CTL
-#undef REDUCE_SCALED_POWER_BY_TWO_CHAIN
-#undef REDUCE_SCALED_POWER_BY_THREE_CHAIN
 }
 
 static void ath9k_hw_ar9287_set_txpower(struct ath_hw *ah,
diff --git a/drivers/net/wireless/ath/ath9k/eeprom_def.c b/drivers/net/wireless/ath/ath9k/eeprom_def.c
index 619b95d764f..b5fba8b18b8 100644
--- a/drivers/net/wireless/ath/ath9k/eeprom_def.c
+++ b/drivers/net/wireless/ath/ath9k/eeprom_def.c
@@ -991,9 +991,6 @@ static void ath9k_hw_set_def_power_per_rate_table(struct ath_hw *ah,
 						  u16 antenna_reduction,
 						  u16 powerLimit)
 {
-#define REDUCE_SCALED_POWER_BY_TWO_CHAIN     6  /* 10*log10(2)*2 */
-#define REDUCE_SCALED_POWER_BY_THREE_CHAIN   9 /* 10*log10(3)*2 */
-
 	struct ar5416_eeprom_def *pEepData = &ah->eeprom.def;
 	u16 twiceMaxEdgePower;
 	int i;
@@ -1027,24 +1024,8 @@ static void ath9k_hw_set_def_power_per_rate_table(struct ath_hw *ah,
 
 	ath9k_hw_get_channel_centers(ah, chan, &centers);
 
-	scaledPower = powerLimit - antenna_reduction;
-
-	switch (ar5416_get_ntxchains(tx_chainmask)) {
-	case 1:
-		break;
-	case 2:
-		if (scaledPower > REDUCE_SCALED_POWER_BY_TWO_CHAIN)
-			scaledPower -= REDUCE_SCALED_POWER_BY_TWO_CHAIN;
-		else
-			scaledPower = 0;
-		break;
-	case 3:
-		if (scaledPower > REDUCE_SCALED_POWER_BY_THREE_CHAIN)
-			scaledPower -= REDUCE_SCALED_POWER_BY_THREE_CHAIN;
-		else
-			scaledPower = 0;
-		break;
-	}
+	scaledPower = ath9k_hw_get_scaled_power(ah, powerLimit,
+						antenna_reduction);
 
 	if (IS_CHAN_2GHZ(chan)) {
 		numCtlModes = ARRAY_SIZE(ctlModesFor11g) -
@@ -1263,20 +1244,7 @@ static void ath9k_hw_def_set_txpower(struct ath_hw *ah,
 			regulatory->max_power_level = ratesArray[i];
 	}
 
-	switch(ar5416_get_ntxchains(ah->txchainmask)) {
-	case 1:
-		break;
-	case 2:
-		regulatory->max_power_level += INCREASE_MAXPOW_BY_TWO_CHAIN;
-		break;
-	case 3:
-		regulatory->max_power_level += INCREASE_MAXPOW_BY_THREE_CHAIN;
-		break;
-	default:
-		ath_dbg(ath9k_hw_common(ah), EEPROM,
-			"Invalid chainmask configuration\n");
-		break;
-	}
+	ath9k_hw_update_regulatory_maxpower(ah);
 
 	if (test)
 		return;
diff --git a/drivers/net/wireless/ath/ath9k/gpio.c b/drivers/net/wireless/ath/ath9k/gpio.c
index fbe23de1297..281a9af0f1b 100644
--- a/drivers/net/wireless/ath/ath9k/gpio.c
+++ b/drivers/net/wireless/ath/ath9k/gpio.c
@@ -41,6 +41,9 @@ void ath_init_leds(struct ath_softc *sc)
 {
 	int ret;
 
+	if (AR_SREV_9100(sc->sc_ah))
+		return;
+
 	if (sc->sc_ah->led_pin < 0) {
 		if (AR_SREV_9287(sc->sc_ah))
 			sc->sc_ah->led_pin = ATH_LED_PIN_9287;
@@ -362,7 +365,7 @@ void ath9k_stop_btcoex(struct ath_softc *sc)
 		ath9k_hw_btcoex_disable(ah);
 		if (ath9k_hw_get_btcoex_scheme(ah) == ATH_BTCOEX_CFG_3WIRE)
 			ath9k_btcoex_timer_pause(sc);
-		if (ath9k_hw_get_btcoex_scheme(ah) == ATH_BTCOEX_CFG_MCI)
+		if (AR_SREV_9462(ah))
 			ath_mci_flush_profile(&sc->btcoex.mci);
 	}
 }
@@ -373,7 +376,7 @@ void ath9k_deinit_btcoex(struct ath_softc *sc)
 	    ath9k_hw_get_btcoex_scheme(sc->sc_ah) == ATH_BTCOEX_CFG_3WIRE)
 		ath_gen_timer_free(sc->sc_ah, sc->btcoex.no_stomp_timer);
 
-	if (ath9k_hw_get_btcoex_scheme(sc->sc_ah) == ATH_BTCOEX_CFG_MCI)
+	if (AR_SREV_9462(sc->sc_ah))
 		ath_mci_cleanup(sc);
 }
 
@@ -399,17 +402,16 @@ int ath9k_init_btcoex(struct ath_softc *sc)
 		txq = sc->tx.txq_map[WME_AC_BE];
 		ath9k_hw_init_btcoex_hw(sc->sc_ah, txq->axq_qnum);
 		sc->btcoex.bt_stomp_type = ATH_BTCOEX_STOMP_LOW;
-		break;
-	case ATH_BTCOEX_CFG_MCI:
-		sc->btcoex.bt_stomp_type = ATH_BTCOEX_STOMP_LOW;
-		sc->btcoex.duty_cycle = ATH_BTCOEX_DEF_DUTY_CYCLE;
-		INIT_LIST_HEAD(&sc->btcoex.mci.info);
+		if (AR_SREV_9462(ah)) {
+			sc->btcoex.duty_cycle = ATH_BTCOEX_DEF_DUTY_CYCLE;
+			INIT_LIST_HEAD(&sc->btcoex.mci.info);
 
-		r = ath_mci_setup(sc);
-		if (r)
-			return r;
+			r = ath_mci_setup(sc);
+			if (r)
+				return r;
 
-		ath9k_hw_btcoex_init_mci(ah);
+			ath9k_hw_btcoex_init_mci(ah);
+		}
 
 		break;
 	default:
diff --git a/drivers/net/wireless/ath/ath9k/hif_usb.c b/drivers/net/wireless/ath/ath9k/hif_usb.c
index 424aabb2c73..aa327adcc3d 100644
--- a/drivers/net/wireless/ath/ath9k/hif_usb.c
+++ b/drivers/net/wireless/ath/ath9k/hif_usb.c
@@ -53,6 +53,8 @@ static struct usb_device_id ath9k_hif_usb_ids[] = {
 	  .driver_info = AR9280_USB },  /* SMC Networks */
 	{ USB_DEVICE(0x0411, 0x017f),
 	  .driver_info = AR9280_USB },  /* Sony UWA-BR100 */
+	{ USB_DEVICE(0x04da, 0x3904),
+	  .driver_info = AR9280_USB },
 
 	{ USB_DEVICE(0x0cf3, 0x20ff),
 	  .driver_info = STORAGE_DEVICE },
@@ -1356,6 +1358,7 @@ static struct usb_driver ath9k_hif_usb_driver = {
 #endif
 	.id_table = ath9k_hif_usb_ids,
 	.soft_unbind = 1,
+	.disable_hub_initiated_lpm = 1,
 };
 
 int ath9k_hif_usb_init(void)
diff --git a/drivers/net/wireless/ath/ath9k/htc_drv_init.c b/drivers/net/wireless/ath/ath9k/htc_drv_init.c
index de5ee15ee63..25213d521bc 100644
--- a/drivers/net/wireless/ath/ath9k/htc_drv_init.c
+++ b/drivers/net/wireless/ath/ath9k/htc_drv_init.c
@@ -14,6 +14,8 @@
  * OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
  */
 
+#define pr_fmt(fmt) KBUILD_MODNAME ": " fmt
+
 #include "htc.h"
 
 MODULE_AUTHOR("Atheros Communications");
@@ -711,7 +713,8 @@ static void ath9k_set_hw_capab(struct ath9k_htc_priv *priv,
 
 	hw->wiphy->flags &= ~WIPHY_FLAG_PS_ON_BY_DEFAULT;
 
-	hw->wiphy->flags |= WIPHY_FLAG_IBSS_RSN;
+	hw->wiphy->flags |= WIPHY_FLAG_IBSS_RSN |
+			    WIPHY_FLAG_HAS_REMAIN_ON_CHANNEL;
 
 	hw->queues = 4;
 	hw->channel_change_time = 5000;
@@ -966,9 +969,7 @@ int ath9k_htc_resume(struct htc_target *htc_handle)
 static int __init ath9k_htc_init(void)
 {
 	if (ath9k_hif_usb_init() < 0) {
-		printk(KERN_ERR
-			"ath9k_htc: No USB devices found,"
-			" driver not installed.\n");
+		pr_err("No USB devices found, driver not installed\n");
 		return -ENODEV;
 	}
 
@@ -979,6 +980,6 @@ module_init(ath9k_htc_init);
 static void __exit ath9k_htc_exit(void)
 {
 	ath9k_hif_usb_exit();
-	printk(KERN_INFO "ath9k_htc: Driver unloaded\n");
+	pr_info("Driver unloaded\n");
 }
 module_exit(ath9k_htc_exit);
diff --git a/drivers/net/wireless/ath/ath9k/htc_hst.c b/drivers/net/wireless/ath/ath9k/htc_hst.c
index c25226a32dd..4a9570dfba7 100644
--- a/drivers/net/wireless/ath/ath9k/htc_hst.c
+++ b/drivers/net/wireless/ath/ath9k/htc_hst.c
@@ -14,6 +14,8 @@
  * OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
  */
 
+#define pr_fmt(fmt) KBUILD_MODNAME ": " fmt
+
 #include "htc.h"
 
 static int htc_issue_send(struct htc_target *target, struct sk_buff* skb,
@@ -461,7 +463,7 @@ int ath9k_htc_hw_init(struct htc_target *target,
 		      char *product, u32 drv_info)
 {
 	if (ath9k_htc_probe_device(target, dev, devid, product, drv_info)) {
-		printk(KERN_ERR "Failed to initialize the device\n");
+		pr_err("Failed to initialize the device\n");
 		return -ENODEV;
 	}
 
diff --git a/drivers/net/wireless/ath/ath9k/hw.c b/drivers/net/wireless/ath/ath9k/hw.c
index fa84e37bf09..abe05ec85d5 100644
--- a/drivers/net/wireless/ath/ath9k/hw.c
+++ b/drivers/net/wireless/ath/ath9k/hw.c
@@ -24,6 +24,8 @@
 #include "rc.h"
 #include "ar9003_mac.h"
 #include "ar9003_mci.h"
+#include "debug.h"
+#include "ath9k.h"
 
 static bool ath9k_hw_set_reset_reg(struct ath_hw *ah, u32 type);
 
@@ -83,6 +85,53 @@ static void ath9k_hw_ani_cache_ini_regs(struct ath_hw *ah)
 /* Helper Functions */
 /********************/
 
+#ifdef CONFIG_ATH9K_DEBUGFS
+
+void ath9k_debug_sync_cause(struct ath_common *common, u32 sync_cause)
+{
+	struct ath_softc *sc = common->priv;
+	if (sync_cause)
+		sc->debug.stats.istats.sync_cause_all++;
+	if (sync_cause & AR_INTR_SYNC_RTC_IRQ)
+		sc->debug.stats.istats.sync_rtc_irq++;
+	if (sync_cause & AR_INTR_SYNC_MAC_IRQ)
+		sc->debug.stats.istats.sync_mac_irq++;
+	if (sync_cause & AR_INTR_SYNC_EEPROM_ILLEGAL_ACCESS)
+		sc->debug.stats.istats.eeprom_illegal_access++;
+	if (sync_cause & AR_INTR_SYNC_APB_TIMEOUT)
+		sc->debug.stats.istats.apb_timeout++;
+	if (sync_cause & AR_INTR_SYNC_PCI_MODE_CONFLICT)
+		sc->debug.stats.istats.pci_mode_conflict++;
+	if (sync_cause & AR_INTR_SYNC_HOST1_FATAL)
+		sc->debug.stats.istats.host1_fatal++;
+	if (sync_cause & AR_INTR_SYNC_HOST1_PERR)
+		sc->debug.stats.istats.host1_perr++;
+	if (sync_cause & AR_INTR_SYNC_TRCV_FIFO_PERR)
+		sc->debug.stats.istats.trcv_fifo_perr++;
+	if (sync_cause & AR_INTR_SYNC_RADM_CPL_EP)
+		sc->debug.stats.istats.radm_cpl_ep++;
+	if (sync_cause & AR_INTR_SYNC_RADM_CPL_DLLP_ABORT)
+		sc->debug.stats.istats.radm_cpl_dllp_abort++;
+	if (sync_cause & AR_INTR_SYNC_RADM_CPL_TLP_ABORT)
+		sc->debug.stats.istats.radm_cpl_tlp_abort++;
+	if (sync_cause & AR_INTR_SYNC_RADM_CPL_ECRC_ERR)
+		sc->debug.stats.istats.radm_cpl_ecrc_err++;
+	if (sync_cause & AR_INTR_SYNC_RADM_CPL_TIMEOUT)
+		sc->debug.stats.istats.radm_cpl_timeout++;
+	if (sync_cause & AR_INTR_SYNC_LOCAL_TIMEOUT)
+		sc->debug.stats.istats.local_timeout++;
+	if (sync_cause & AR_INTR_SYNC_PM_ACCESS)
+		sc->debug.stats.istats.pm_access++;
+	if (sync_cause & AR_INTR_SYNC_MAC_AWAKE)
+		sc->debug.stats.istats.mac_awake++;
+	if (sync_cause & AR_INTR_SYNC_MAC_ASLEEP)
+		sc->debug.stats.istats.mac_asleep++;
+	if (sync_cause & AR_INTR_SYNC_MAC_SLEEP_ACCESS)
+		sc->debug.stats.istats.mac_sleep_access++;
+}
+#endif
+
+
 static void ath9k_hw_set_clockrate(struct ath_hw *ah)
 {
 	struct ieee80211_conf *conf = &ath9k_hw_common(ah)->hw->conf;
@@ -142,6 +191,22 @@ bool ath9k_hw_wait(struct ath_hw *ah, u32 reg, u32 mask, u32 val, u32 timeout)
 }
 EXPORT_SYMBOL(ath9k_hw_wait);
 
+void ath9k_hw_synth_delay(struct ath_hw *ah, struct ath9k_channel *chan,
+			  int hw_delay)
+{
+	if (IS_CHAN_B(chan))
+		hw_delay = (4 * hw_delay) / 22;
+	else
+		hw_delay /= 10;
+
+	if (IS_CHAN_HALF_RATE(chan))
+		hw_delay *= 2;
+	else if (IS_CHAN_QUARTER_RATE(chan))
+		hw_delay *= 4;
+
+	udelay(hw_delay + BASE_ACTIVATE_DELAY);
+}
+
 void ath9k_hw_write_array(struct ath_hw *ah, struct ar5416IniArray *array,
 			  int column, unsigned int *writecnt)
 {
@@ -388,8 +453,8 @@ static void ath9k_hw_init_config(struct ath_hw *ah)
 {
 	int i;
 
-	ah->config.dma_beacon_response_time = 2;
-	ah->config.sw_beacon_response_time = 10;
+	ah->config.dma_beacon_response_time = 1;
+	ah->config.sw_beacon_response_time = 6;
 	ah->config.additional_swba_backoff = 0;
 	ah->config.ack_6mb = 0x0;
 	ah->config.cwm_ignore_extcca = 0;
@@ -445,7 +510,6 @@ static void ath9k_hw_init_defaults(struct ath_hw *ah)
 		AR_STA_ID1_MCAST_KSRCH;
 	if (AR_SREV_9100(ah))
 		ah->sta_id1_defaults |= AR_STA_ID1_AR9100_BA_FIX;
-	ah->enable_32kHz_clock = DONT_USE_32KHZ;
 	ah->slottime = ATH9K_SLOT_TIME_9;
 	ah->globaltxtimeout = (u32) -1;
 	ah->power_mode = ATH9K_PM_UNDEFINED;
@@ -972,7 +1036,7 @@ void ath9k_hw_init_global_settings(struct ath_hw *ah)
 	struct ath_common *common = ath9k_hw_common(ah);
 	struct ieee80211_conf *conf = &common->hw->conf;
 	const struct ath9k_channel *chan = ah->curchan;
-	int acktimeout, ctstimeout;
+	int acktimeout, ctstimeout, ack_offset = 0;
 	int slottime;
 	int sifstime;
 	int rx_lat = 0, tx_lat = 0, eifs = 0;
@@ -993,6 +1057,11 @@ void ath9k_hw_init_global_settings(struct ath_hw *ah)
 		rx_lat = 37;
 	tx_lat = 54;
 
+	if (IS_CHAN_5GHZ(chan))
+		sifstime = 16;
+	else
+		sifstime = 10;
+
 	if (IS_CHAN_HALF_RATE(chan)) {
 		eifs = 175;
 		rx_lat *= 2;
@@ -1000,8 +1069,9 @@ void ath9k_hw_init_global_settings(struct ath_hw *ah)
 		if (IS_CHAN_A_FAST_CLOCK(ah, chan))
 		    tx_lat += 11;
 
+		sifstime *= 2;
+		ack_offset = 16;
 		slottime = 13;
-		sifstime = 32;
 	} else if (IS_CHAN_QUARTER_RATE(chan)) {
 		eifs = 340;
 		rx_lat = (rx_lat * 4) - 1;
@@ -1009,8 +1079,9 @@ void ath9k_hw_init_global_settings(struct ath_hw *ah)
 		if (IS_CHAN_A_FAST_CLOCK(ah, chan))
 		    tx_lat += 22;
 
+		sifstime *= 4;
+		ack_offset = 32;
 		slottime = 21;
-		sifstime = 64;
 	} else {
 		if (AR_SREV_9287(ah) && AR_SREV_9287_13_OR_LATER(ah)) {
 			eifs = AR_D_GBL_IFS_EIFS_ASYNC_FIFO;
@@ -1024,14 +1095,10 @@ void ath9k_hw_init_global_settings(struct ath_hw *ah)
 		tx_lat = MS(reg, AR_USEC_TX_LAT);
 
 		slottime = ah->slottime;
-		if (IS_CHAN_5GHZ(chan))
-			sifstime = 16;
-		else
-			sifstime = 10;
 	}
 
 	/* As defined by IEEE 802.11-2007 17.3.8.6 */
-	acktimeout = slottime + sifstime + 3 * ah->coverage_class;
+	acktimeout = slottime + sifstime + 3 * ah->coverage_class + ack_offset;
 	ctstimeout = acktimeout;
 
 	/*
@@ -1041,7 +1108,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->channel && conf->channel->band == IEEE80211_BAND_2GHZ &&
+	    !IS_CHAN_HALF_RATE(chan) && !IS_CHAN_QUARTER_RATE(chan)) {
 		acktimeout += 64 - sifstime - ah->slottime;
 		ctstimeout += 48 - sifstime - ah->slottime;
 	}
@@ -1491,11 +1559,84 @@ static void ath9k_hw_apply_gpio_override(struct ath_hw *ah)
 	}
 }
 
+static bool ath9k_hw_check_dcs(u32 dma_dbg, u32 num_dcu_states,
+			       int *hang_state, int *hang_pos)
+{
+	static u32 dcu_chain_state[] = {5, 6, 9}; /* DCU chain stuck states */
+	u32 chain_state, dcs_pos, i;
+
+	for (dcs_pos = 0; dcs_pos < num_dcu_states; dcs_pos++) {
+		chain_state = (dma_dbg >> (5 * dcs_pos)) & 0x1f;
+		for (i = 0; i < 3; i++) {
+			if (chain_state == dcu_chain_state[i]) {
+				*hang_state = chain_state;
+				*hang_pos = dcs_pos;
+				return true;
+			}
+		}
+	}
+	return false;
+}
+
+#define DCU_COMPLETE_STATE        1
+#define DCU_COMPLETE_STATE_MASK 0x3
+#define NUM_STATUS_READS         50
+static bool ath9k_hw_detect_mac_hang(struct ath_hw *ah)
+{
+	u32 chain_state, comp_state, dcs_reg = AR_DMADBG_4;
+	u32 i, hang_pos, hang_state, num_state = 6;
+
+	comp_state = REG_READ(ah, AR_DMADBG_6);
+
+	if ((comp_state & DCU_COMPLETE_STATE_MASK) != DCU_COMPLETE_STATE) {
+		ath_dbg(ath9k_hw_common(ah), RESET,
+			"MAC Hang signature not found at DCU complete\n");
+		return false;
+	}
+
+	chain_state = REG_READ(ah, dcs_reg);
+	if (ath9k_hw_check_dcs(chain_state, num_state, &hang_state, &hang_pos))
+		goto hang_check_iter;
+
+	dcs_reg = AR_DMADBG_5;
+	num_state = 4;
+	chain_state = REG_READ(ah, dcs_reg);
+	if (ath9k_hw_check_dcs(chain_state, num_state, &hang_state, &hang_pos))
+		goto hang_check_iter;
+
+	ath_dbg(ath9k_hw_common(ah), RESET,
+		"MAC Hang signature 1 not found\n");
+	return false;
+
+hang_check_iter:
+	ath_dbg(ath9k_hw_common(ah), RESET,
+		"DCU registers: chain %08x complete %08x Hang: state %d pos %d\n",
+		chain_state, comp_state, hang_state, hang_pos);
+
+	for (i = 0; i < NUM_STATUS_READS; i++) {
+		chain_state = REG_READ(ah, dcs_reg);
+		chain_state = (chain_state >> (5 * hang_pos)) & 0x1f;
+		comp_state = REG_READ(ah, AR_DMADBG_6);
+
+		if (((comp_state & DCU_COMPLETE_STATE_MASK) !=
+					DCU_COMPLETE_STATE) ||
+		    (chain_state != hang_state))
+			return false;
+	}
+
+	ath_dbg(ath9k_hw_common(ah), RESET, "MAC Hang signature 1 found\n");
+
+	return true;
+}
+
 bool ath9k_hw_check_alive(struct ath_hw *ah)
 {
 	int count = 50;
 	u32 reg;
 
+	if (AR_SREV_9300(ah))
+		return !ath9k_hw_detect_mac_hang(ah);
+
 	if (AR_SREV_9285_12_OR_LATER(ah))
 		return true;
 
@@ -1546,6 +1687,10 @@ static int ath9k_hw_do_fastcc(struct ath_hw *ah, struct ath9k_channel *chan)
 	if (chan->channel == ah->curchan->channel)
 		goto fail;
 
+	if ((ah->curchan->channelFlags | chan->channelFlags) &
+	    (CHANNEL_HALF | CHANNEL_QUARTER))
+		goto fail;
+
 	if ((chan->channelFlags & CHANNEL_ALL) !=
 	    (ah->curchan->channelFlags & CHANNEL_ALL))
 		goto fail;
@@ -1557,10 +1702,10 @@ static int ath9k_hw_do_fastcc(struct ath_hw *ah, struct ath9k_channel *chan)
 	 * For AR9462, make sure that calibration data for
 	 * re-using are present.
 	 */
-	if (AR_SREV_9462(ah) && (!ah->caldata ||
-				 !ah->caldata->done_txiqcal_once ||
-				 !ah->caldata->done_txclcal_once ||
-				 !ah->caldata->rtt_hist.num_readings))
+	if (AR_SREV_9462(ah) && (ah->caldata &&
+				 (!ah->caldata->done_txiqcal_once ||
+				  !ah->caldata->done_txclcal_once ||
+				  !ah->caldata->rtt_done)))
 		goto fail;
 
 	ath_dbg(common, RESET, "FastChannelChange for %d -> %d\n",
@@ -1796,7 +1941,6 @@ int ath9k_hw_reset(struct ath_hw *ah, struct ath9k_channel *chan,
 	if (caldata) {
 		caldata->done_txiqcal_once = false;
 		caldata->done_txclcal_once = false;
-		caldata->rtt_hist.num_readings = 0;
 	}
 	if (!ath9k_hw_init_cal(ah, chan))
 		return -EIO;
diff --git a/drivers/net/wireless/ath/ath9k/hw.h b/drivers/net/wireless/ath/ath9k/hw.h
index e88f182ff45..b620c557c2a 100644
--- a/drivers/net/wireless/ath/ath9k/hw.h
+++ b/drivers/net/wireless/ath/ath9k/hw.h
@@ -348,12 +348,6 @@ enum ath9k_int {
 	 CHANNEL_HT40MINUS)
 
 #define MAX_RTT_TABLE_ENTRY     6
-#define RTT_HIST_MAX            3
-struct ath9k_rtt_hist {
-	u32 table[AR9300_MAX_CHAINS][RTT_HIST_MAX][MAX_RTT_TABLE_ENTRY];
-	u8 num_readings;
-};
-
 #define MAX_IQCAL_MEASUREMENT	8
 #define MAX_CL_TAB_ENTRY	16
 
@@ -363,6 +357,7 @@ struct ath9k_hw_cal_data {
 	int32_t CalValid;
 	int8_t iCoff;
 	int8_t qCoff;
+	bool rtt_done;
 	bool paprd_done;
 	bool nfcal_pending;
 	bool nfcal_interference;
@@ -373,8 +368,8 @@ struct ath9k_hw_cal_data {
 	u32 num_measures[AR9300_MAX_CHAINS];
 	int tx_corr_coeff[MAX_IQCAL_MEASUREMENT][AR9300_MAX_CHAINS];
 	u32 tx_clcal[AR9300_MAX_CHAINS][MAX_CL_TAB_ENTRY];
+	u32 rtt_table[AR9300_MAX_CHAINS][MAX_RTT_TABLE_ENTRY];
 	struct ath9k_nfcal_hist nfCalHist[NUM_NF_READINGS];
-	struct ath9k_rtt_hist rtt_hist;
 };
 
 struct ath9k_channel {
@@ -708,7 +703,6 @@ struct ath_hw {
 	struct ar5416Stats stats;
 	struct ath9k_tx_queue_info txq[ATH9K_NUM_TX_QUEUES];
 
-	int16_t curchan_rad_index;
 	enum ath9k_int imask;
 	u32 imrs2_reg;
 	u32 txok_interrupt_mask;
@@ -762,11 +756,6 @@ struct ath_hw {
 
 	u32 sta_id1_defaults;
 	u32 misc_mode;
-	enum {
-		AUTO_32KHZ,
-		USE_32KHZ,
-		DONT_USE_32KHZ,
-	} enable_32kHz_clock;
 
 	/* Private to hardware code */
 	struct ath_hw_private_ops private_ops;
@@ -783,7 +772,6 @@ struct ath_hw {
 	u32 *analogBank7Data;
 	u32 *bank6Temp;
 
-	u8 txpower_limit;
 	int coverage_class;
 	u32 slottime;
 	u32 globaltxtimeout;
@@ -848,7 +836,6 @@ struct ath_hw {
 	struct ath_gen_timer_table hw_gen_timers;
 
 	struct ar9003_txs *ts_ring;
-	void *ts_start;
 	u32 ts_paddr_start;
 	u32 ts_paddr_end;
 	u16 ts_tail;
@@ -915,7 +902,6 @@ static inline u8 get_streams(int mask)
 }
 
 /* Initialization, Detach, Reset */
-const char *ath9k_hw_probe(u16 vendorid, u16 devid);
 void ath9k_hw_deinit(struct ath_hw *ah);
 int ath9k_hw_init(struct ath_hw *ah);
 int ath9k_hw_reset(struct ath_hw *ah, struct ath9k_channel *chan,
@@ -932,6 +918,8 @@ void ath9k_hw_set_gpio(struct ath_hw *ah, u32 gpio, u32 val);
 void ath9k_hw_setantenna(struct ath_hw *ah, u32 antenna);
 
 /* General Operation */
+void ath9k_hw_synth_delay(struct ath_hw *ah, struct ath9k_channel *chan,
+			  int hw_delay);
 bool ath9k_hw_wait(struct ath_hw *ah, u32 reg, u32 mask, u32 val, u32 timeout);
 void ath9k_hw_write_array(struct ath_hw *ah, struct ar5416IniArray *array,
 			  int column, unsigned int *writecnt);
@@ -965,6 +953,13 @@ bool ath9k_hw_check_alive(struct ath_hw *ah);
 
 bool ath9k_hw_setpower(struct ath_hw *ah, enum ath9k_power_mode mode);
 
+#ifdef CONFIG_ATH9K_DEBUGFS
+void ath9k_debug_sync_cause(struct ath_common *common, u32 sync_cause);
+#else
+static inline void ath9k_debug_sync_cause(struct ath_common *common,
+					  u32 sync_cause) {}
+#endif
+
 /* Generic hw timer primitives */
 struct ath_gen_timer *ath_gen_timer_alloc(struct ath_hw *ah,
 					  void (*trigger)(void *),
@@ -1012,7 +1007,6 @@ int ar9003_paprd_create_curve(struct ath_hw *ah,
 int ar9003_paprd_setup_gain_table(struct ath_hw *ah, int chain);
 int ar9003_paprd_init_table(struct ath_hw *ah);
 bool ar9003_paprd_is_done(struct ath_hw *ah);
-void ar9003_hw_set_paprd_txdesc(struct ath_hw *ah, void *ds, u8 chains);
 
 /* Hardware family op attach helpers */
 void ar5008_hw_attach_phy_ops(struct ath_hw *ah);
diff --git a/drivers/net/wireless/ath/ath9k/init.c b/drivers/net/wireless/ath/ath9k/init.c
index cb006458fc4..dee9e092449 100644
--- a/drivers/net/wireless/ath/ath9k/init.c
+++ b/drivers/net/wireless/ath/ath9k/init.c
@@ -14,6 +14,8 @@
  * OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
  */
 
+#define pr_fmt(fmt) KBUILD_MODNAME ": " fmt
+
 #include <linux/dma-mapping.h>
 #include <linux/slab.h>
 #include <linux/ath9k_platform.h>
@@ -519,6 +521,8 @@ 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);
+
 	if (!pdata) {
 		ah->ah_flags |= AH_USE_EEPROM;
 		sc->sc_ah->led_pin = -1;
@@ -642,6 +646,24 @@ void ath9k_reload_chainmask_settings(struct ath_softc *sc)
 		setup_ht_cap(sc, &sc->sbands[IEEE80211_BAND_5GHZ].ht_cap);
 }
 
+static const struct ieee80211_iface_limit if_limits[] = {
+	{ .max = 2048,	.types = BIT(NL80211_IFTYPE_STATION) |
+				 BIT(NL80211_IFTYPE_P2P_CLIENT) |
+				 BIT(NL80211_IFTYPE_WDS) },
+	{ .max = 8,	.types =
+#ifdef CONFIG_MAC80211_MESH
+				 BIT(NL80211_IFTYPE_MESH_POINT) |
+#endif
+				 BIT(NL80211_IFTYPE_AP) |
+				 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,
+};
 
 void ath9k_set_hw_capab(struct ath_softc *sc, struct ieee80211_hw *hw)
 {
@@ -671,11 +693,15 @@ 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;
+
 	if (AR_SREV_5416(sc->sc_ah))
 		hw->wiphy->flags &= ~WIPHY_FLAG_PS_ON_BY_DEFAULT;
 
 	hw->wiphy->flags |= WIPHY_FLAG_IBSS_RSN;
 	hw->wiphy->flags |= WIPHY_FLAG_SUPPORTS_TDLS;
+	hw->wiphy->flags |= WIPHY_FLAG_HAS_REMAIN_ON_CHANNEL;
 
 	hw->queues = 4;
 	hw->max_rates = 4;
@@ -779,6 +805,7 @@ int ath9k_init_device(u16 devid, struct ath_softc *sc,
 			goto error_world;
 	}
 
+	setup_timer(&sc->rx_poll_timer, ath_rx_poll, (unsigned long)sc);
 	sc->last_rssi = ATH_RSSI_DUMMY_MARKER;
 
 	ath_init_leds(sc);
@@ -821,6 +848,8 @@ static void ath9k_deinit_softc(struct ath_softc *sc)
 			ath_tx_cleanupq(sc, &sc->tx.txq[i]);
 
 	ath9k_hw_deinit(sc->sc_ah);
+	if (sc->dfs_detector != NULL)
+		sc->dfs_detector->exit(sc->dfs_detector);
 
 	kfree(sc->sc_ah);
 	sc->sc_ah = NULL;
@@ -866,17 +895,14 @@ static int __init ath9k_init(void)
 	/* Register rate control algorithm */
 	error = ath_rate_control_register();
 	if (error != 0) {
-		printk(KERN_ERR
-			"ath9k: Unable to register rate control "
-			"algorithm: %d\n",
-			error);
+		pr_err("Unable to register rate control algorithm: %d\n",
+		       error);
 		goto err_out;
 	}
 
 	error = ath_pci_init();
 	if (error < 0) {
-		printk(KERN_ERR
-			"ath9k: No PCI devices found, driver not installed.\n");
+		pr_err("No PCI devices found, driver not installed\n");
 		error = -ENODEV;
 		goto err_rate_unregister;
 	}
@@ -905,6 +931,6 @@ static void __exit ath9k_exit(void)
 	ath_ahb_exit();
 	ath_pci_exit();
 	ath_rate_control_unregister();
-	printk(KERN_INFO "%s: Driver unloaded\n", dev_info);
+	pr_info("%s: Driver unloaded\n", dev_info);
 }
 module_exit(ath9k_exit);
diff --git a/drivers/net/wireless/ath/ath9k/mac.c b/drivers/net/wireless/ath/ath9k/mac.c
index f7bd2532269..04ef775ccee 100644
--- a/drivers/net/wireless/ath/ath9k/mac.c
+++ b/drivers/net/wireless/ath/ath9k/mac.c
@@ -133,8 +133,16 @@ EXPORT_SYMBOL(ath9k_hw_updatetxtriglevel);
 
 void ath9k_hw_abort_tx_dma(struct ath_hw *ah)
 {
+	int maxdelay = 1000;
 	int i, q;
 
+	if (ah->curchan) {
+		if (IS_CHAN_HALF_RATE(ah->curchan))
+			maxdelay *= 2;
+		else if (IS_CHAN_QUARTER_RATE(ah->curchan))
+			maxdelay *= 4;
+	}
+
 	REG_WRITE(ah, AR_Q_TXD, AR_Q_TXD_M);
 
 	REG_SET_BIT(ah, AR_PCU_MISC, AR_PCU_FORCE_QUIET_COLL | AR_PCU_CLEAR_VMF);
@@ -142,7 +150,7 @@ void ath9k_hw_abort_tx_dma(struct ath_hw *ah)
 	REG_SET_BIT(ah, AR_D_GBL_IFS_MISC, AR_D_GBL_IFS_MISC_IGNORE_BACKOFF);
 
 	for (q = 0; q < AR_NUM_QCU; q++) {
-		for (i = 0; i < 1000; i++) {
+		for (i = 0; i < maxdelay; i++) {
 			if (i)
 				udelay(5);
 
diff --git a/drivers/net/wireless/ath/ath9k/main.c b/drivers/net/wireless/ath/ath9k/main.c
index 798ea57252b..dfa78e8b647 100644
--- a/drivers/net/wireless/ath/ath9k/main.c
+++ b/drivers/net/wireless/ath/ath9k/main.c
@@ -113,21 +113,25 @@ void ath9k_ps_restore(struct ath_softc *sc)
 	struct ath_common *common = ath9k_hw_common(sc->sc_ah);
 	enum ath9k_power_mode mode;
 	unsigned long flags;
+	bool reset;
 
 	spin_lock_irqsave(&sc->sc_pm_lock, flags);
 	if (--sc->ps_usecount != 0)
 		goto unlock;
 
-	if (sc->ps_idle && (sc->ps_flags & PS_WAIT_FOR_TX_ACK))
+	if (sc->ps_idle) {
+		ath9k_hw_setrxabort(sc->sc_ah, 1);
+		ath9k_hw_stopdmarecv(sc->sc_ah, &reset);
 		mode = ATH9K_PM_FULL_SLEEP;
-	else if (sc->ps_enabled &&
-		 !(sc->ps_flags & (PS_WAIT_FOR_BEACON |
-			      PS_WAIT_FOR_CAB |
-			      PS_WAIT_FOR_PSPOLL_DATA |
-			      PS_WAIT_FOR_TX_ACK)))
+	} else if (sc->ps_enabled &&
+		   !(sc->ps_flags & (PS_WAIT_FOR_BEACON |
+				     PS_WAIT_FOR_CAB |
+				     PS_WAIT_FOR_PSPOLL_DATA |
+				     PS_WAIT_FOR_TX_ACK))) {
 		mode = ATH9K_PM_NETWORK_SLEEP;
-	else
+	} else {
 		goto unlock;
+	}
 
 	spin_lock(&common->cc_lock);
 	ath_hw_cycle_counters_update(common);
@@ -241,6 +245,7 @@ static bool ath_prepare_reset(struct ath_softc *sc, bool retry_tx, bool flush)
 
 	sc->hw_busy_count = 0;
 	del_timer_sync(&common->ani.timer);
+	del_timer_sync(&sc->rx_poll_timer);
 
 	ath9k_debug_samp_bb_mac(sc);
 	ath9k_hw_disable_interrupts(ah);
@@ -282,6 +287,7 @@ static bool ath_complete_reset(struct ath_softc *sc, bool start)
 
 		ieee80211_queue_delayed_work(sc->hw, &sc->tx_complete_work, 0);
 		ieee80211_queue_delayed_work(sc->hw, &sc->hw_pll_work, HZ/2);
+		ath_start_rx_poll(sc, 3);
 		if (!common->disable_ani)
 			ath_start_ani(common);
 	}
@@ -690,17 +696,6 @@ void ath9k_tasklet(unsigned long data)
 		goto out;
 	}
 
-	/*
-	 * Only run the baseband hang check if beacons stop working in AP or
-	 * IBSS mode, because it has a high false positive rate. For station
-	 * mode it should not be necessary, since the upper layers will detect
-	 * this through a beacon miss automatically and the following channel
-	 * change will trigger a hardware reset anyway
-	 */
-	if (ath9k_hw_numtxpending(ah, sc->beacon.beaconq) != 0 &&
-	    !ath9k_hw_check_alive(ah))
-		ieee80211_queue_work(sc->hw, &sc->hw_check_work);
-
 	if ((status & ATH9K_INT_TSFOOR) && sc->ps_enabled) {
 		/*
 		 * TSF sync does not look correct; remain awake to sync with
@@ -912,10 +907,19 @@ void ath_hw_check(struct work_struct *work)
 	struct ath_common *common = ath9k_hw_common(sc->sc_ah);
 	unsigned long flags;
 	int busy;
+	u8 is_alive, nbeacon = 1;
 
 	ath9k_ps_wakeup(sc);
-	if (ath9k_hw_check_alive(sc->sc_ah))
+	is_alive = ath9k_hw_check_alive(sc->sc_ah);
+
+	if (is_alive && !AR_SREV_9300(sc->sc_ah))
 		goto out;
+	else if (!is_alive && AR_SREV_9300(sc->sc_ah)) {
+		ath_dbg(common, RESET,
+			"DCU stuck is detected. Schedule chip reset\n");
+		RESET_STAT_INC(sc, RESET_TYPE_MAC_HANG);
+		goto sched_reset;
+	}
 
 	spin_lock_irqsave(&common->cc_lock, flags);
 	busy = ath_update_survey_stats(sc);
@@ -926,12 +930,18 @@ void ath_hw_check(struct work_struct *work)
 	if (busy >= 99) {
 		if (++sc->hw_busy_count >= 3) {
 			RESET_STAT_INC(sc, RESET_TYPE_BB_HANG);
-			ieee80211_queue_work(sc->hw, &sc->hw_reset_work);
+			goto sched_reset;
 		}
-
-	} else if (busy >= 0)
+	} else if (busy >= 0) {
 		sc->hw_busy_count = 0;
+		nbeacon = 3;
+	}
 
+	ath_start_rx_poll(sc, nbeacon);
+	goto out;
+
+sched_reset:
+	ieee80211_queue_work(sc->hw, &sc->hw_reset_work);
 out:
 	ath9k_ps_restore(sc);
 }
@@ -1094,14 +1104,7 @@ static void ath9k_tx(struct ieee80211_hw *hw, struct sk_buff *skb)
 		}
 	}
 
-	/*
-	 * Cannot tx while the hardware is in full sleep, it first needs a full
-	 * chip reset to recover from that
-	 */
-	if (unlikely(sc->sc_ah->power_mode == ATH9K_PM_FULL_SLEEP))
-		goto exit;
-
-	if (unlikely(sc->sc_ah->power_mode != ATH9K_PM_AWAKE)) {
+	if (unlikely(sc->sc_ah->power_mode == ATH9K_PM_NETWORK_SLEEP)) {
 		/*
 		 * We are using PS-Poll and mac80211 can request TX while in
 		 * power save mode. Need to wake up hardware for the TX to be
@@ -1120,12 +1123,21 @@ static void ath9k_tx(struct ieee80211_hw *hw, struct sk_buff *skb)
 		}
 		/*
 		 * The actual restore operation will happen only after
-		 * the sc_flags bit is cleared. We are just dropping
+		 * the ps_flags bit is cleared. We are just dropping
 		 * the ps_usecount here.
 		 */
 		ath9k_ps_restore(sc);
 	}
 
+	/*
+	 * Cannot tx while the hardware is in full sleep, it first needs a full
+	 * chip reset to recover from that
+	 */
+	if (unlikely(sc->sc_ah->power_mode == ATH9K_PM_FULL_SLEEP)) {
+		ath_err(common, "TX while HW is in FULL_SLEEP mode\n");
+		goto exit;
+	}
+
 	memset(&txctl, 0, sizeof(struct ath_tx_control));
 	txctl.txq = sc->tx.txq_map[skb_get_queue_mapping(skb)];
 
@@ -1133,6 +1145,7 @@ static void ath9k_tx(struct ieee80211_hw *hw, struct sk_buff *skb)
 
 	if (ath_tx_start(hw, skb, &txctl) != 0) {
 		ath_dbg(common, XMIT, "TX failed\n");
+		TX_STAT_INC(txctl.txq->axq_qnum, txfailed);
 		goto exit;
 	}
 
@@ -1151,6 +1164,7 @@ static void ath9k_stop(struct ieee80211_hw *hw)
 	mutex_lock(&sc->mutex);
 
 	ath_cancel_work(sc);
+	del_timer_sync(&sc->rx_poll_timer);
 
 	if (sc->sc_flags & SC_OP_INVALID) {
 		ath_dbg(common, ANY, "Device not present\n");
@@ -1237,7 +1251,6 @@ static void ath9k_reclaim_beacon(struct ath_softc *sc,
 	ath9k_set_beaconing_status(sc, false);
 	ath_beacon_return(sc, avp);
 	ath9k_set_beaconing_status(sc, true);
-	sc->sc_flags &= ~SC_OP_BEACONS;
 }
 
 static void ath9k_vif_iter(void *data, u8 *mac, struct ieee80211_vif *vif)
@@ -1368,21 +1381,31 @@ static void ath9k_do_vif_add_setup(struct ieee80211_hw *hw,
 	ath9k_calculate_summary_state(hw, vif);
 
 	if (ath9k_uses_beacons(vif->type)) {
-		int error;
-		/* This may fail because upper levels do not have beacons
-		 * properly configured yet.  That's OK, we assume it
-		 * will be properly configured and then we will be notified
-		 * in the info_changed method and set up beacons properly
-		 * there.
-		 */
+		/* Reserve a beacon slot for the vif */
 		ath9k_set_beaconing_status(sc, false);
-		error = ath_beacon_alloc(sc, vif);
-		if (!error)
-			ath_beacon_config(sc, vif);
+		ath_beacon_alloc(sc, vif);
 		ath9k_set_beaconing_status(sc, true);
 	}
 }
 
+void ath_start_rx_poll(struct ath_softc *sc, u8 nbeacon)
+{
+	if (!AR_SREV_9300(sc->sc_ah))
+		return;
+
+	if (!(sc->sc_flags & SC_OP_PRIM_STA_VIF))
+		return;
+
+	mod_timer(&sc->rx_poll_timer, jiffies + msecs_to_jiffies
+			(nbeacon * sc->cur_beacon_conf.beacon_interval));
+}
+
+void ath_rx_poll(unsigned long data)
+{
+	struct ath_softc *sc = (struct ath_softc *)data;
+
+	ieee80211_queue_work(sc->hw, &sc->hw_check_work);
+}
 
 static int ath9k_add_interface(struct ieee80211_hw *hw,
 			       struct ieee80211_vif *vif)
@@ -1511,6 +1534,7 @@ static void ath9k_remove_interface(struct ieee80211_hw *hw,
 static void ath9k_enable_ps(struct ath_softc *sc)
 {
 	struct ath_hw *ah = sc->sc_ah;
+	struct ath_common *common = ath9k_hw_common(ah);
 
 	sc->ps_enabled = true;
 	if (!(ah->caps.hw_caps & ATH9K_HW_CAP_AUTOSLEEP)) {
@@ -1520,11 +1544,13 @@ static void ath9k_enable_ps(struct ath_softc *sc)
 		}
 		ath9k_hw_setrxabort(ah, 1);
 	}
+	ath_dbg(common, PS, "PowerSave enabled\n");
 }
 
 static void ath9k_disable_ps(struct ath_softc *sc)
 {
 	struct ath_hw *ah = sc->sc_ah;
+	struct ath_common *common = ath9k_hw_common(ah);
 
 	sc->ps_enabled = false;
 	ath9k_hw_setpower(ah, ATH9K_PM_AWAKE);
@@ -1539,7 +1565,7 @@ static void ath9k_disable_ps(struct ath_softc *sc)
 			ath9k_hw_set_interrupts(ah);
 		}
 	}
-
+	ath_dbg(common, PS, "PowerSave disabled\n");
 }
 
 static int ath9k_config(struct ieee80211_hw *hw, u32 changed)
@@ -1911,6 +1937,8 @@ static void ath9k_bss_iter(void *data, u8 *mac, struct ieee80211_vif *vif)
 		sc->last_rssi = ATH_RSSI_DUMMY_MARKER;
 		sc->sc_ah->stats.avgbrssi = ATH_RSSI_DUMMY_MARKER;
 
+		ath_start_rx_poll(sc, 3);
+
 		if (!common->disable_ani) {
 			sc->sc_flags |= SC_OP_ANI_RUN;
 			ath_start_ani(common);
@@ -1950,6 +1978,7 @@ static void ath9k_config_bss(struct ath_softc *sc, struct ieee80211_vif *vif)
 		/* Stop ANI */
 		sc->sc_flags &= ~SC_OP_ANI_RUN;
 		del_timer_sync(&common->ani.timer);
+		del_timer_sync(&sc->rx_poll_timer);
 		memset(&sc->caldata, 0, sizeof(sc->caldata));
 	}
 }
@@ -1964,7 +1993,6 @@ static void ath9k_bss_info_changed(struct ieee80211_hw *hw,
 	struct ath_common *common = ath9k_hw_common(ah);
 	struct ath_vif *avp = (void *)vif->drv_priv;
 	int slottime;
-	int error;
 
 	ath9k_ps_wakeup(sc);
 	mutex_lock(&sc->mutex);
@@ -1993,16 +2021,29 @@ static void ath9k_bss_info_changed(struct ieee80211_hw *hw,
 		} else {
 			sc->sc_flags &= ~SC_OP_ANI_RUN;
 			del_timer_sync(&common->ani.timer);
+			del_timer_sync(&sc->rx_poll_timer);
 		}
 	}
 
-	/* Enable transmission of beacons (AP, IBSS, MESH) */
-	if ((changed & BSS_CHANGED_BEACON) ||
-	    ((changed & BSS_CHANGED_BEACON_ENABLED) && bss_conf->enable_beacon)) {
+	/*
+	 * In case of AP mode, the HW TSF has to be reset
+	 * when the beacon interval changes.
+	 */
+	if ((changed & BSS_CHANGED_BEACON_INT) &&
+	    (vif->type == NL80211_IFTYPE_AP))
+		sc->sc_flags |= SC_OP_TSF_RESET;
+
+	/* Configure beaconing (AP, IBSS, MESH) */
+	if (ath9k_uses_beacons(vif->type) &&
+	    ((changed & BSS_CHANGED_BEACON) ||
+	     (changed & BSS_CHANGED_BEACON_ENABLED) ||
+	     (changed & BSS_CHANGED_BEACON_INT))) {
 		ath9k_set_beaconing_status(sc, false);
-		error = ath_beacon_alloc(sc, vif);
-		if (!error)
-			ath_beacon_config(sc, vif);
+		if (bss_conf->enable_beacon)
+			ath_beacon_alloc(sc, vif);
+		else
+			avp->is_bslot_active = false;
+		ath_beacon_config(sc, vif);
 		ath9k_set_beaconing_status(sc, true);
 	}
 
@@ -2025,30 +2066,6 @@ static void ath9k_bss_info_changed(struct ieee80211_hw *hw,
 		}
 	}
 
-	/* Disable transmission of beacons */
-	if ((changed & BSS_CHANGED_BEACON_ENABLED) &&
-	    !bss_conf->enable_beacon) {
-		ath9k_set_beaconing_status(sc, false);
-		avp->is_bslot_active = false;
-		ath9k_set_beaconing_status(sc, true);
-	}
-
-	if (changed & BSS_CHANGED_BEACON_INT) {
-		/*
-		 * In case of AP mode, the HW TSF has to be reset
-		 * when the beacon interval changes.
-		 */
-		if (vif->type == NL80211_IFTYPE_AP) {
-			sc->sc_flags |= SC_OP_TSF_RESET;
-			ath9k_set_beaconing_status(sc, false);
-			error = ath_beacon_alloc(sc, vif);
-			if (!error)
-				ath_beacon_config(sc, vif);
-			ath9k_set_beaconing_status(sc, true);
-		} else
-			ath_beacon_config(sc, vif);
-	}
-
 	mutex_unlock(&sc->mutex);
 	ath9k_ps_restore(sc);
 }
diff --git a/drivers/net/wireless/ath/ath9k/pci.c b/drivers/net/wireless/ath/ath9k/pci.c
index 77dc327def8..a856b51255f 100644
--- a/drivers/net/wireless/ath/ath9k/pci.c
+++ b/drivers/net/wireless/ath/ath9k/pci.c
@@ -14,6 +14,8 @@
  * OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
  */
 
+#define pr_fmt(fmt) KBUILD_MODNAME ": " fmt
+
 #include <linux/nl80211.h>
 #include <linux/pci.h>
 #include <linux/pci-aspm.h>
@@ -171,14 +173,13 @@ static int ath_pci_probe(struct pci_dev *pdev, const struct pci_device_id *id)
 
 	ret =  pci_set_dma_mask(pdev, DMA_BIT_MASK(32));
 	if (ret) {
-		printk(KERN_ERR "ath9k: 32-bit DMA not available\n");
+		pr_err("32-bit DMA not available\n");
 		goto err_dma;
 	}
 
 	ret = pci_set_consistent_dma_mask(pdev, DMA_BIT_MASK(32));
 	if (ret) {
-		printk(KERN_ERR "ath9k: 32-bit DMA consistent "
-			"DMA enable failed\n");
+		pr_err("32-bit DMA consistent DMA enable failed\n");
 		goto err_dma;
 	}
 
@@ -224,7 +225,7 @@ static int ath_pci_probe(struct pci_dev *pdev, const struct pci_device_id *id)
 
 	mem = pci_iomap(pdev, 0, 0);
 	if (!mem) {
-		printk(KERN_ERR "PCI memory map error\n") ;
+		pr_err("PCI memory map error\n") ;
 		ret = -EIO;
 		goto err_iomap;
 	}
diff --git a/drivers/net/wireless/ath/ath9k/rc.c b/drivers/net/wireless/ath/ath9k/rc.c
index 08bb4553270..92a6c0a87f8 100644
--- a/drivers/net/wireless/ath/ath9k/rc.c
+++ b/drivers/net/wireless/ath/ath9k/rc.c
@@ -1436,7 +1436,7 @@ static void ath_rate_init(void *priv, struct ieee80211_supported_band *sband,
 
 static void ath_rate_update(void *priv, struct ieee80211_supported_band *sband,
 			    struct ieee80211_sta *sta, void *priv_sta,
-			    u32 changed, enum nl80211_channel_type oper_chan_type)
+			    u32 changed)
 {
 	struct ath_softc *sc = priv;
 	struct ath_rate_priv *ath_rc_priv = priv_sta;
@@ -1447,12 +1447,11 @@ static void ath_rate_update(void *priv, struct ieee80211_supported_band *sband,
 
 	/* FIXME: Handle AP mode later when we support CWM */
 
-	if (changed & IEEE80211_RC_HT_CHANGED) {
+	if (changed & IEEE80211_RC_BW_CHANGED) {
 		if (sc->sc_ah->opmode != NL80211_IFTYPE_STATION)
 			return;
 
-		if (oper_chan_type == NL80211_CHAN_HT40MINUS ||
-		    oper_chan_type == NL80211_CHAN_HT40PLUS)
+		if (sta->ht_cap.cap & IEEE80211_HT_CAP_SUP_WIDTH_20_40)
 			oper_cw40 = true;
 
 		if (oper_cw40)
diff --git a/drivers/net/wireless/ath/ath9k/recv.c b/drivers/net/wireless/ath/ath9k/recv.c
index 1c4583c7ff7..e1fcc68124d 100644
--- a/drivers/net/wireless/ath/ath9k/recv.c
+++ b/drivers/net/wireless/ath/ath9k/recv.c
@@ -812,6 +812,7 @@ static bool ath9k_rx_accept(struct ath_common *common,
 	is_valid_tkip = rx_stats->rs_keyix != ATH9K_RXKEYIX_INVALID &&
 		test_bit(rx_stats->rs_keyix, common->tkip_keymap);
 	strip_mic = is_valid_tkip && ieee80211_is_data(fc) &&
+		ieee80211_has_protected(fc) &&
 		!(rx_stats->rs_status &
 		(ATH9K_RXERR_DECRYPT | ATH9K_RXERR_CRC | ATH9K_RXERR_MIC |
 		 ATH9K_RXERR_KEYMISS));
@@ -824,15 +825,20 @@ static bool ath9k_rx_accept(struct ath_common *common,
 	if (rx_stats->rs_keyix == ATH9K_RXKEYIX_INVALID)
 		rx_stats->rs_status &= ~ATH9K_RXERR_KEYMISS;
 
-	if (!rx_stats->rs_datalen)
+	if (!rx_stats->rs_datalen) {
+		RX_STAT_INC(rx_len_err);
 		return false;
+	}
+
         /*
          * rs_status follows rs_datalen so if rs_datalen is too large
          * we can take a hint that hardware corrupted it, so ignore
          * those frames.
          */
-	if (rx_stats->rs_datalen > (common->rx_bufsize - rx_status_len))
+	if (rx_stats->rs_datalen > (common->rx_bufsize - rx_status_len)) {
+		RX_STAT_INC(rx_len_err);
 		return false;
+	}
 
 	/* Only use error bits from the last fragment */
 	if (rx_stats->rs_more)
@@ -902,6 +908,7 @@ static int ath9k_process_rate(struct ath_common *common,
 	struct ieee80211_supported_band *sband;
 	enum ieee80211_band band;
 	unsigned int i = 0;
+	struct ath_softc __maybe_unused *sc = common->priv;
 
 	band = hw->conf.channel->band;
 	sband = hw->wiphy->bands[band];
@@ -936,7 +943,7 @@ static int ath9k_process_rate(struct ath_common *common,
 	ath_dbg(common, ANY,
 		"unsupported hw bitrate detected 0x%02x using 1 Mbit\n",
 		rx_stats->rs_rate);
-
+	RX_STAT_INC(rx_rate_err);
 	return -EINVAL;
 }
 
@@ -1823,10 +1830,14 @@ int ath_rx_tasklet(struct ath_softc *sc, int flush, bool hp)
 
 		hdr = (struct ieee80211_hdr *) (hdr_skb->data + rx_status_len);
 		rxs = IEEE80211_SKB_RXCB(hdr_skb);
-		if (ieee80211_is_beacon(hdr->frame_control) &&
-		    !is_zero_ether_addr(common->curbssid) &&
-		    !compare_ether_addr(hdr->addr3, common->curbssid))
-			rs.is_mybeacon = true;
+		if (ieee80211_is_beacon(hdr->frame_control)) {
+			RX_STAT_INC(rx_beacons);
+			if (!is_zero_ether_addr(common->curbssid) &&
+			    ether_addr_equal(hdr->addr3, common->curbssid))
+				rs.is_mybeacon = true;
+			else
+				rs.is_mybeacon = false;
+		}
 		else
 			rs.is_mybeacon = false;
 
@@ -1836,8 +1847,10 @@ int ath_rx_tasklet(struct ath_softc *sc, int flush, bool hp)
 		 * If we're asked to flush receive queue, directly
 		 * chain it back at the queue without processing it.
 		 */
-		if (sc->sc_flags & SC_OP_RXFLUSH)
+		if (sc->sc_flags & SC_OP_RXFLUSH) {
+			RX_STAT_INC(rx_drop_rxflush);
 			goto requeue_drop_frag;
+		}
 
 		memset(rxs, 0, sizeof(struct ieee80211_rx_status));
 
@@ -1855,6 +1868,10 @@ int ath_rx_tasklet(struct ath_softc *sc, int flush, bool hp)
 		if (retval)
 			goto requeue_drop_frag;
 
+		if (rs.is_mybeacon) {
+			sc->hw_busy_count = 0;
+			ath_start_rx_poll(sc, 3);
+		}
 		/* Ensure we always have an skb to requeue once we are done
 		 * processing the current buffer's skb */
 		requeue_skb = ath_rxbuf_alloc(common, common->rx_bufsize, GFP_ATOMIC);
@@ -1863,8 +1880,10 @@ int ath_rx_tasklet(struct ath_softc *sc, int flush, bool hp)
 		 * tell hardware it can give us a new frame using the old
 		 * skb and put it at the tail of the sc->rx.rxbuf list for
 		 * processing. */
-		if (!requeue_skb)
+		if (!requeue_skb) {
+			RX_STAT_INC(rx_oom_err);
 			goto requeue_drop_frag;
+		}
 
 		/* Unmap the frame */
 		dma_unmap_single(sc->dev, bf->bf_buf_addr,
@@ -1895,6 +1914,7 @@ int ath_rx_tasklet(struct ath_softc *sc, int flush, bool hp)
 		}
 
 		if (rs.rs_more) {
+			RX_STAT_INC(rx_frags);
 			/*
 			 * rs_more indicates chained descriptors which can be
 			 * used to link buffers together for a sort of
@@ -1904,6 +1924,7 @@ int ath_rx_tasklet(struct ath_softc *sc, int flush, bool hp)
 				/* too many fragments - cannot handle frame */
 				dev_kfree_skb_any(sc->rx.frag);
 				dev_kfree_skb_any(skb);
+				RX_STAT_INC(rx_too_many_frags_err);
 				skb = NULL;
 			}
 			sc->rx.frag = skb;
@@ -1915,6 +1936,7 @@ int ath_rx_tasklet(struct ath_softc *sc, int flush, bool hp)
 
 			if (pskb_expand_head(hdr_skb, 0, space, GFP_ATOMIC) < 0) {
 				dev_kfree_skb(skb);
+				RX_STAT_INC(rx_oom_err);
 				goto requeue_drop_frag;
 			}
 
diff --git a/drivers/net/wireless/ath/carl9170/cmd.h b/drivers/net/wireless/ath/carl9170/cmd.h
index 885c42778b8..65919c902f5 100644
--- a/drivers/net/wireless/ath/carl9170/cmd.h
+++ b/drivers/net/wireless/ath/carl9170/cmd.h
@@ -114,7 +114,7 @@ __regwrite_out :							\
 
 #define carl9170_regwrite_result()					\
 	__err;								\
-} while (0);
+} while (0)
 
 
 #define carl9170_async_regwrite_get_buf()				\
@@ -126,7 +126,7 @@ do {									\
 		__err = -ENOMEM;					\
 		goto __async_regwrite_out;				\
 	}								\
-} while (0);
+} while (0)
 
 #define carl9170_async_regwrite_begin(carl)				\
 do {									\
@@ -169,6 +169,6 @@ __async_regwrite_out:							\
 
 #define carl9170_async_regwrite_result()				\
 	__err;								\
-} while (0);
+} while (0)
 
 #endif /* __CMD_H */
diff --git a/drivers/net/wireless/ath/carl9170/fw.c b/drivers/net/wireless/ath/carl9170/fw.c
index cffde8d9a52..5c73c03872f 100644
--- a/drivers/net/wireless/ath/carl9170/fw.c
+++ b/drivers/net/wireless/ath/carl9170/fw.c
@@ -355,6 +355,8 @@ static int carl9170_fw(struct ar9170 *ar, const __u8 *data, size_t len)
 
 	ar->hw->wiphy->interface_modes |= if_comb_types;
 
+	ar->hw->wiphy->flags |= WIPHY_FLAG_HAS_REMAIN_ON_CHANNEL;
+
 #undef SUPPORTED
 	return carl9170_fw_tx_sequence(ar);
 }
diff --git a/drivers/net/wireless/ath/carl9170/rx.c b/drivers/net/wireless/ath/carl9170/rx.c
index dc99030ea8b..84b22eec7ab 100644
--- a/drivers/net/wireless/ath/carl9170/rx.c
+++ b/drivers/net/wireless/ath/carl9170/rx.c
@@ -538,7 +538,7 @@ static void carl9170_ps_beacon(struct ar9170 *ar, void *data, unsigned int len)
 		return;
 
 	/* and only beacons from the associated BSSID, please */
-	if (compare_ether_addr(hdr->addr3, ar->common.curbssid) ||
+	if (!ether_addr_equal(hdr->addr3, ar->common.curbssid) ||
 	    !ar->common.curaid)
 		return;
 
diff --git a/drivers/net/wireless/ath/carl9170/usb.c b/drivers/net/wireless/ath/carl9170/usb.c
index 89821e4835c..888152ce3ec 100644
--- a/drivers/net/wireless/ath/carl9170/usb.c
+++ b/drivers/net/wireless/ath/carl9170/usb.c
@@ -1159,6 +1159,7 @@ static struct usb_driver carl9170_driver = {
 	.resume = carl9170_usb_resume,
 	.reset_resume = carl9170_usb_resume,
 #endif /* CONFIG_PM */
+	.disable_hub_initiated_lpm = 1,
 };
 
 module_usb_driver(carl9170_driver);
diff --git a/drivers/net/wireless/ath/main.c b/drivers/net/wireless/ath/main.c
index ea2c737138d..8e99540cd90 100644
--- a/drivers/net/wireless/ath/main.c
+++ b/drivers/net/wireless/ath/main.c
@@ -14,6 +14,8 @@
  * OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
  */
 
+#define pr_fmt(fmt) KBUILD_MODNAME ": " fmt
+
 #include <linux/kernel.h>
 #include <linux/module.h>
 
@@ -49,7 +51,7 @@ struct sk_buff *ath_rxbuf_alloc(struct ath_common *common,
 		if (off != 0)
 			skb_reserve(skb, common->cachelsz - off);
 	} else {
-		printk(KERN_ERR "skbuff alloc of size %u failed\n", len);
+		pr_err("skbuff alloc of size %u failed\n", len);
 		return NULL;
 	}
 
diff --git a/drivers/net/wireless/ath/regd.c b/drivers/net/wireless/ath/regd.c
index 10dea37431b..d81698015bf 100644
--- a/drivers/net/wireless/ath/regd.c
+++ b/drivers/net/wireless/ath/regd.c
@@ -14,6 +14,8 @@
  * OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
  */
 
+#define pr_fmt(fmt) KBUILD_MODNAME ": " fmt
+
 #include <linux/kernel.h>
 #include <linux/export.h>
 #include <net/cfg80211.h>
@@ -562,7 +564,7 @@ static int __ath_regd_init(struct ath_regulatory *reg)
 	printk(KERN_DEBUG "ath: EEPROM regdomain: 0x%0x\n", reg->current_rd);
 
 	if (!ath_regd_is_eeprom_valid(reg)) {
-		printk(KERN_ERR "ath: Invalid EEPROM contents\n");
+		pr_err("Invalid EEPROM contents\n");
 		return -EINVAL;
 	}
 
diff --git a/drivers/net/wireless/atmel.c b/drivers/net/wireless/atmel.c
index 6c87a823f5a..d07c0301da6 100644
--- a/drivers/net/wireless/atmel.c
+++ b/drivers/net/wireless/atmel.c
@@ -3989,8 +3989,7 @@ static int reset_atmel_card(struct net_device *dev)
 			atmel_copy_to_card(priv->dev, 0x8000, &fw[0x6000], len - 0x6000);
 		}
 
-		if (fw_entry)
-			release_firmware(fw_entry);
+		release_firmware(fw_entry);
 	}
 
 	err = atmel_wakeup_firmware(priv);
diff --git a/drivers/net/wireless/atmel_pci.c b/drivers/net/wireless/atmel_pci.c
index 9ab1192004c..51e33b53386 100644
--- a/drivers/net/wireless/atmel_pci.c
+++ b/drivers/net/wireless/atmel_pci.c
@@ -74,15 +74,4 @@ static void __devexit atmel_pci_remove(struct pci_dev *pdev)
 	stop_atmel_card(pci_get_drvdata(pdev));
 }
 
-static int __init atmel_init_module(void)
-{
-	return pci_register_driver(&atmel_driver);
-}
-
-static void __exit atmel_cleanup_module(void)
-{
-	pci_unregister_driver(&atmel_driver);
-}
-
-module_init(atmel_init_module);
-module_exit(atmel_cleanup_module);
+module_pci_driver(atmel_driver);
diff --git a/drivers/net/wireless/b43/bus.c b/drivers/net/wireless/b43/bus.c
index 424692df239..565fdbdd691 100644
--- a/drivers/net/wireless/b43/bus.c
+++ b/drivers/net/wireless/b43/bus.c
@@ -107,11 +107,9 @@ struct b43_bus_dev *b43_bus_dev_bcma_init(struct bcma_device *core)
 	dev->dma_dev = core->dma_dev;
 	dev->irq = core->irq;
 
-	/*
 	dev->board_vendor = core->bus->boardinfo.vendor;
 	dev->board_type = core->bus->boardinfo.type;
-	dev->board_rev = core->bus->boardinfo.rev;
-	*/
+	dev->board_rev = core->bus->sprom.board_rev;
 
 	dev->chip_id = core->bus->chipinfo.id;
 	dev->chip_rev = core->bus->chipinfo.rev;
@@ -210,7 +208,7 @@ struct b43_bus_dev *b43_bus_dev_ssb_init(struct ssb_device *sdev)
 
 	dev->board_vendor = sdev->bus->boardinfo.vendor;
 	dev->board_type = sdev->bus->boardinfo.type;
-	dev->board_rev = sdev->bus->boardinfo.rev;
+	dev->board_rev = sdev->bus->sprom.board_rev;
 
 	dev->chip_id = sdev->bus->chip_id;
 	dev->chip_rev = sdev->bus->chip_rev;
diff --git a/drivers/net/wireless/b43/dma.c b/drivers/net/wireless/b43/dma.c
index b5f1b91002b..777cd74921d 100644
--- a/drivers/net/wireless/b43/dma.c
+++ b/drivers/net/wireless/b43/dma.c
@@ -1109,7 +1109,7 @@ static bool b43_dma_translation_in_low_word(struct b43_wldev *dev,
 #ifdef CONFIG_B43_SSB
 	if (dev->dev->bus_type == B43_BUS_SSB &&
 	    dev->dev->sdev->bus->bustype == SSB_BUSTYPE_PCI &&
-	    !(dev->dev->sdev->bus->host_pci->is_pcie &&
+	    !(pci_is_pcie(dev->dev->sdev->bus->host_pci) &&
 	      ssb_read32(dev->dev->sdev, SSB_TMSHIGH) & SSB_TMSHIGH_DMA64))
 			return 1;
 #endif
diff --git a/drivers/net/wireless/b43/main.c b/drivers/net/wireless/b43/main.c
index e4d6dc2e37d..5a39b226b2e 100644
--- a/drivers/net/wireless/b43/main.c
+++ b/drivers/net/wireless/b43/main.c
@@ -4010,6 +4010,20 @@ static int b43_op_set_key(struct ieee80211_hw *hw, enum set_key_cmd cmd,
 	if (modparam_nohwcrypt)
 		return -ENOSPC; /* User disabled HW-crypto */
 
+	if ((vif->type == NL80211_IFTYPE_ADHOC ||
+	     vif->type == NL80211_IFTYPE_MESH_POINT) &&
+	    (key->cipher == WLAN_CIPHER_SUITE_TKIP ||
+	     key->cipher == WLAN_CIPHER_SUITE_CCMP) &&
+	    !(key->flags & IEEE80211_KEY_FLAG_PAIRWISE)) {
+		/*
+		 * For now, disable hw crypto for the RSN IBSS group keys. This
+		 * could be optimized in the future, but until that gets
+		 * implemented, use of software crypto for group addressed
+		 * frames is a acceptable to allow RSN IBSS to be used.
+		 */
+		return -EOPNOTSUPP;
+	}
+
 	mutex_lock(&wl->mutex);
 
 	dev = wl->current_dev;
@@ -5229,10 +5243,10 @@ static void b43_sprom_fixup(struct ssb_bus *bus)
 
 	/* boardflags workarounds */
 	if (bus->boardinfo.vendor == SSB_BOARDVENDOR_DELL &&
-	    bus->chip_id == 0x4301 && bus->boardinfo.rev == 0x74)
+	    bus->chip_id == 0x4301 && bus->sprom.board_rev == 0x74)
 		bus->sprom.boardflags_lo |= B43_BFL_BTCOEXIST;
 	if (bus->boardinfo.vendor == PCI_VENDOR_ID_APPLE &&
-	    bus->boardinfo.type == 0x4E && bus->boardinfo.rev > 0x40)
+	    bus->boardinfo.type == 0x4E && bus->sprom.board_rev > 0x40)
 		bus->sprom.boardflags_lo |= B43_BFL_PACTRL;
 	if (bus->bustype == SSB_BUSTYPE_PCI) {
 		pdev = bus->host_pci;
@@ -5281,6 +5295,8 @@ static struct b43_wl *b43_wireless_init(struct b43_bus_dev *dev)
 		BIT(NL80211_IFTYPE_WDS) |
 		BIT(NL80211_IFTYPE_ADHOC);
 
+	hw->wiphy->flags |= WIPHY_FLAG_IBSS_RSN;
+
 	hw->queues = modparam_qos ? B43_QOS_QUEUE_NUM : 1;
 	wl->mac80211_initially_registered_queues = hw->queues;
 	hw->max_rates = 2;
diff --git a/drivers/net/wireless/b43/sdio.c b/drivers/net/wireless/b43/sdio.c
index 80b0755ed3a..a54fb2d2908 100644
--- a/drivers/net/wireless/b43/sdio.c
+++ b/drivers/net/wireless/b43/sdio.c
@@ -193,7 +193,7 @@ static struct sdio_driver b43_sdio_driver = {
 	.name		= "b43-sdio",
 	.id_table	= b43_sdio_ids,
 	.probe		= b43_sdio_probe,
-	.remove		= b43_sdio_remove,
+	.remove		= __devexit_p(b43_sdio_remove),
 };
 
 int b43_sdio_init(void)
diff --git a/drivers/net/wireless/b43/xmit.c b/drivers/net/wireless/b43/xmit.c
index 2c5367884b3..b31ccc02fa2 100644
--- a/drivers/net/wireless/b43/xmit.c
+++ b/drivers/net/wireless/b43/xmit.c
@@ -290,7 +290,8 @@ int b43_generate_txhdr(struct b43_wldev *dev,
 		txhdr->dur_fb = wlhdr->duration_id;
 	} else {
 		txhdr->dur_fb = ieee80211_generic_frame_duration(
-			dev->wl->hw, info->control.vif, fragment_len, fbrate);
+			dev->wl->hw, info->control.vif, info->band,
+			fragment_len, fbrate);
 	}
 
 	plcp_fragment_len = fragment_len + FCS_LEN;
@@ -378,7 +379,7 @@ int b43_generate_txhdr(struct b43_wldev *dev,
 	if (info->control.rates[0].flags & IEEE80211_TX_RC_USE_SHORT_PREAMBLE)
 		phy_ctl |= B43_TXH_PHY_SHORTPRMBL;
 
-	switch (b43_ieee80211_antenna_sanitize(dev, info->antenna_sel_tx)) {
+	switch (b43_ieee80211_antenna_sanitize(dev, 0)) {
 	case 0: /* Default */
 		phy_ctl |= B43_TXH_PHY_ANT01AUTO;
 		break;
diff --git a/drivers/net/wireless/b43legacy/main.c b/drivers/net/wireless/b43legacy/main.c
index df7e16dfb36..cd9c9bc186d 100644
--- a/drivers/net/wireless/b43legacy/main.c
+++ b/drivers/net/wireless/b43legacy/main.c
@@ -1056,6 +1056,7 @@ static void b43legacy_write_probe_resp_plcp(struct b43legacy_wldev *dev,
 	b43legacy_generate_plcp_hdr(&plcp, size + FCS_LEN, rate->hw_value);
 	dur = ieee80211_generic_frame_duration(dev->wl->hw,
 					       dev->wl->vif,
+					       IEEE80211_BAND_2GHZ,
 					       size,
 					       rate);
 	/* Write PLCP in two parts and timing for packet transfer */
@@ -1121,6 +1122,7 @@ static const u8 *b43legacy_generate_probe_resp(struct b43legacy_wldev *dev,
 					 IEEE80211_STYPE_PROBE_RESP);
 	dur = ieee80211_generic_frame_duration(dev->wl->hw,
 					       dev->wl->vif,
+					       IEEE80211_BAND_2GHZ,
 					       *dest_size,
 					       rate);
 	hdr->duration_id = dur;
@@ -1571,8 +1573,6 @@ static void b43legacy_request_firmware(struct work_struct *work)
 	const char *filename;
 	int err;
 
-	/* do dummy read */
-	ssb_read32(dev->dev, SSB_TMSHIGH);
 	if (!fw->ucode) {
 		if (rev == 2)
 			filename = "ucode2";
@@ -3779,7 +3779,7 @@ static void b43legacy_sprom_fixup(struct ssb_bus *bus)
 	/* boardflags workarounds */
 	if (bus->boardinfo.vendor == PCI_VENDOR_ID_APPLE &&
 	    bus->boardinfo.type == 0x4E &&
-	    bus->boardinfo.rev > 0x40)
+	    bus->sprom.board_rev > 0x40)
 		bus->sprom.boardflags_lo |= B43legacy_BFL_PACTRL;
 }
 
diff --git a/drivers/net/wireless/b43legacy/phy.c b/drivers/net/wireless/b43legacy/phy.c
index 950334197f4..995c7d0c212 100644
--- a/drivers/net/wireless/b43legacy/phy.c
+++ b/drivers/net/wireless/b43legacy/phy.c
@@ -408,7 +408,7 @@ static void b43legacy_phy_setupg(struct b43legacy_wldev *dev)
 
 		if (is_bcm_board_vendor(dev) &&
 		    (dev->dev->bus->boardinfo.type == 0x0416) &&
-		    (dev->dev->bus->boardinfo.rev == 0x0017))
+		    (dev->dev->bus->sprom.board_rev == 0x0017))
 			return;
 
 		b43legacy_ilt_write(dev, 0x5001, 0x0002);
@@ -424,7 +424,7 @@ static void b43legacy_phy_setupg(struct b43legacy_wldev *dev)
 
 		if (is_bcm_board_vendor(dev) &&
 		    (dev->dev->bus->boardinfo.type == 0x0416) &&
-		    (dev->dev->bus->boardinfo.rev == 0x0017))
+		    (dev->dev->bus->sprom.board_rev == 0x0017))
 			return;
 
 		b43legacy_ilt_write(dev, 0x0401, 0x0002);
diff --git a/drivers/net/wireless/b43legacy/radio.c b/drivers/net/wireless/b43legacy/radio.c
index fcbafcd603c..89617769039 100644
--- a/drivers/net/wireless/b43legacy/radio.c
+++ b/drivers/net/wireless/b43legacy/radio.c
@@ -1998,7 +1998,7 @@ u16 b43legacy_default_radio_attenuation(struct b43legacy_wldev *dev)
 			if (phy->type == B43legacy_PHYTYPE_G) {
 				if (is_bcm_board_vendor(dev) &&
 				    dev->dev->bus->boardinfo.type == 0x421 &&
-				    dev->dev->bus->boardinfo.rev >= 30)
+				    dev->dev->bus->sprom.board_rev >= 30)
 					att = 3;
 				else if (is_bcm_board_vendor(dev) &&
 					 dev->dev->bus->boardinfo.type == 0x416)
@@ -2008,7 +2008,7 @@ u16 b43legacy_default_radio_attenuation(struct b43legacy_wldev *dev)
 			} else {
 				if (is_bcm_board_vendor(dev) &&
 				    dev->dev->bus->boardinfo.type == 0x421 &&
-				    dev->dev->bus->boardinfo.rev >= 30)
+				    dev->dev->bus->sprom.board_rev >= 30)
 					att = 7;
 				else
 					att = 6;
@@ -2018,7 +2018,7 @@ u16 b43legacy_default_radio_attenuation(struct b43legacy_wldev *dev)
 			if (phy->type == B43legacy_PHYTYPE_G) {
 				if (is_bcm_board_vendor(dev) &&
 				    dev->dev->bus->boardinfo.type == 0x421 &&
-				    dev->dev->bus->boardinfo.rev >= 30)
+				    dev->dev->bus->sprom.board_rev >= 30)
 					att = 3;
 				else if (is_bcm_board_vendor(dev) &&
 					 dev->dev->bus->boardinfo.type ==
@@ -2052,9 +2052,9 @@ u16 b43legacy_default_radio_attenuation(struct b43legacy_wldev *dev)
 	}
 	if (is_bcm_board_vendor(dev) &&
 	    dev->dev->bus->boardinfo.type == 0x421) {
-		if (dev->dev->bus->boardinfo.rev < 0x43)
+		if (dev->dev->bus->sprom.board_rev < 0x43)
 			att = 2;
-		else if (dev->dev->bus->boardinfo.rev < 0x51)
+		else if (dev->dev->bus->sprom.board_rev < 0x51)
 			att = 3;
 	}
 	if (att == 0xFFFF)
diff --git a/drivers/net/wireless/b43legacy/xmit.c b/drivers/net/wireless/b43legacy/xmit.c
index 5188fab0b37..a8012f2749e 100644
--- a/drivers/net/wireless/b43legacy/xmit.c
+++ b/drivers/net/wireless/b43legacy/xmit.c
@@ -228,6 +228,7 @@ static int generate_txhdr_fw3(struct b43legacy_wldev *dev,
 	} else {
 		txhdr->dur_fb = ieee80211_generic_frame_duration(dev->wl->hw,
 							 info->control.vif,
+							 info->band,
 							 fragment_len,
 							 rate_fb);
 	}
@@ -277,19 +278,7 @@ static int generate_txhdr_fw3(struct b43legacy_wldev *dev,
 		phy_ctl |= B43legacy_TX4_PHY_ENC_OFDM;
 	if (info->control.rates[0].flags & IEEE80211_TX_RC_USE_SHORT_PREAMBLE)
 		phy_ctl |= B43legacy_TX4_PHY_SHORTPRMBL;
-	switch (info->antenna_sel_tx) {
-	case 0:
-		phy_ctl |= B43legacy_TX4_PHY_ANTLAST;
-		break;
-	case 1:
-		phy_ctl |= B43legacy_TX4_PHY_ANT0;
-		break;
-	case 2:
-		phy_ctl |= B43legacy_TX4_PHY_ANT1;
-		break;
-	default:
-		B43legacy_BUG_ON(1);
-	}
+	phy_ctl |= B43legacy_TX4_PHY_ANTLAST;
 
 	/* MAC control */
 	rates = info->control.rates;
diff --git a/drivers/net/wireless/brcm80211/Kconfig b/drivers/net/wireless/brcm80211/Kconfig
index c5104533e24..b480088b3db 100644
--- a/drivers/net/wireless/brcm80211/Kconfig
+++ b/drivers/net/wireless/brcm80211/Kconfig
@@ -36,6 +36,15 @@ 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/bcmsdh.c b/drivers/net/wireless/brcm80211/brcmfmac/bcmsdh.c
index e925290b432..e2480d19627 100644
--- a/drivers/net/wireless/brcm80211/brcmfmac/bcmsdh.c
+++ b/drivers/net/wireless/brcm80211/brcmfmac/bcmsdh.c
@@ -39,195 +39,262 @@
 
 #define SDIOH_API_ACCESS_RETRY_LIMIT	2
 
-static void brcmf_sdioh_irqhandler(struct sdio_func *func)
+#ifdef CONFIG_BRCMFMAC_SDIO_OOB
+static irqreturn_t brcmf_sdio_irqhandler(int irq, void *dev_id)
 {
-	struct brcmf_sdio_dev *sdiodev = dev_get_drvdata(&func->card->dev);
+	struct brcmf_sdio_dev *sdiodev = dev_get_drvdata(dev_id);
 
-	brcmf_dbg(TRACE, "***IRQHandler\n");
+	brcmf_dbg(INTR, "oob intr triggered\n");
 
-	sdio_release_host(func);
+	/*
+	 * out-of-band interrupt is level-triggered which won't
+	 * be cleared until dpc
+	 */
+	if (sdiodev->irq_en) {
+		disable_irq_nosync(irq);
+		sdiodev->irq_en = false;
+	}
 
 	brcmf_sdbrcm_isr(sdiodev->bus);
 
-	sdio_claim_host(func);
+	return IRQ_HANDLED;
 }
 
-/* dummy handler for SDIO function 2 interrupt */
-static void brcmf_sdioh_dummy_irq_handler(struct sdio_func *func)
+int brcmf_sdio_intr_register(struct brcmf_sdio_dev *sdiodev)
 {
-}
+	int ret = 0;
+	u8 data;
+	unsigned long flags;
 
-int brcmf_sdcard_intr_reg(struct brcmf_sdio_dev *sdiodev)
-{
 	brcmf_dbg(TRACE, "Entering\n");
 
-	sdio_claim_host(sdiodev->func[1]);
-	sdio_claim_irq(sdiodev->func[1], brcmf_sdioh_irqhandler);
-	sdio_claim_irq(sdiodev->func[2], brcmf_sdioh_dummy_irq_handler);
-	sdio_release_host(sdiodev->func[1]);
+	brcmf_dbg(ERROR, "requesting irq %d\n", sdiodev->irq);
+	ret = request_irq(sdiodev->irq, brcmf_sdio_irqhandler,
+			  sdiodev->irq_flags, "brcmf_oob_intr",
+			  &sdiodev->func[1]->card->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;
+
+	/* 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 ane 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);
 
 	return 0;
 }
 
-int brcmf_sdcard_intr_dereg(struct brcmf_sdio_dev *sdiodev)
+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_sdio_regwb(sdiodev, SDIO_CCCR_BRCM_SEPINT, 0, NULL);
+	brcmf_sdio_regwb(sdiodev, SDIO_CCCR_IENx, 0, NULL);
+
+	if (sdiodev->irq_wake) {
+		disable_irq_wake(sdiodev->irq);
+		sdiodev->irq_wake = false;
+	}
+	free_irq(sdiodev->irq, &sdiodev->func[1]->card->dev);
+	sdiodev->irq_en = false;
 
 	return 0;
 }
+#else		/* CONFIG_BRCMFMAC_SDIO_OOB */
+static void brcmf_sdio_irqhandler(struct sdio_func *func)
+{
+	struct brcmf_sdio_dev *sdiodev = dev_get_drvdata(&func->card->dev);
+
+	brcmf_dbg(INTR, "ib intr triggered\n");
 
-u8 brcmf_sdcard_cfg_read(struct brcmf_sdio_dev *sdiodev, uint fnc_num, u32 addr,
-			 int *err)
+	brcmf_sdbrcm_isr(sdiodev->bus);
+}
+
+/* dummy handler for SDIO function 2 interrupt */
+static void brcmf_sdio_dummy_irqhandler(struct sdio_func *func)
 {
-	int status;
-	s32 retry = 0;
-	u8 data = 0;
+}
 
-	do {
-		if (retry)	/* wait for 1 ms till bus get settled down */
-			udelay(1000);
-		status = brcmf_sdioh_request_byte(sdiodev, SDIOH_READ, fnc_num,
-						  addr, (u8 *) &data);
-	} while (status != 0
-		 && (retry++ < SDIOH_API_ACCESS_RETRY_LIMIT));
-	if (err)
-		*err = status;
+int brcmf_sdio_intr_register(struct brcmf_sdio_dev *sdiodev)
+{
+	brcmf_dbg(TRACE, "Entering\n");
 
-	brcmf_dbg(INFO, "fun = %d, addr = 0x%x, u8data = 0x%x\n",
-		  fnc_num, addr, data);
+	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]);
 
-	return data;
+	return 0;
 }
 
-void
-brcmf_sdcard_cfg_write(struct brcmf_sdio_dev *sdiodev, uint fnc_num, u32 addr,
-		       u8 data, int *err)
+int brcmf_sdio_intr_unregister(struct brcmf_sdio_dev *sdiodev)
 {
-	int status;
-	s32 retry = 0;
+	brcmf_dbg(TRACE, "Entering\n");
 
-	do {
-		if (retry)	/* wait for 1 ms till bus get settled down */
-			udelay(1000);
-		status = brcmf_sdioh_request_byte(sdiodev, SDIOH_WRITE, fnc_num,
-						  addr, (u8 *) &data);
-	} while (status != 0
-		 && (retry++ < SDIOH_API_ACCESS_RETRY_LIMIT));
-	if (err)
-		*err = status;
+	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(INFO, "fun = %d, addr = 0x%x, u8data = 0x%x\n",
-		  fnc_num, addr, data);
+	return 0;
 }
+#endif		/* CONFIG_BRCMFMAC_SDIO_OOB */
 
 int
 brcmf_sdcard_set_sbaddr_window(struct brcmf_sdio_dev *sdiodev, u32 address)
 {
-	int err = 0;
-	brcmf_sdcard_cfg_write(sdiodev, SDIO_FUNC_1, SBSDIO_FUNC1_SBADDRLOW,
-			 (address >> 8) & SBSDIO_SBADDRLOW_MASK, &err);
-	if (!err)
-		brcmf_sdcard_cfg_write(sdiodev, SDIO_FUNC_1,
-				       SBSDIO_FUNC1_SBADDRMID,
-				       (address >> 16) & SBSDIO_SBADDRMID_MASK,
-				       &err);
-	if (!err)
-		brcmf_sdcard_cfg_write(sdiodev, SDIO_FUNC_1,
-				       SBSDIO_FUNC1_SBADDRHIGH,
-				       (address >> 24) & SBSDIO_SBADDRHIGH_MASK,
-				       &err);
+	int err = 0, i;
+	u8 addr[3];
+	s32 retry;
+
+	addr[0] = (address >> 8) & SBSDIO_SBADDRLOW_MASK;
+	addr[1] = (address >> 16) & SBSDIO_SBADDRMID_MASK;
+	addr[2] = (address >> 24) & SBSDIO_SBADDRHIGH_MASK;
+
+	for (i = 0; i < 3; i++) {
+		retry = 0;
+		do {
+			if (retry)
+				usleep_range(1000, 2000);
+			err = brcmf_sdioh_request_byte(sdiodev, SDIOH_WRITE,
+					SDIO_FUNC_1, SBSDIO_FUNC1_SBADDRLOW + i,
+					&addr[i]);
+		} while (err != 0 && retry++ < SDIOH_API_ACCESS_RETRY_LIMIT);
+
+		if (err) {
+			brcmf_dbg(ERROR, "failed at addr:0x%0x\n",
+				  SBSDIO_FUNC1_SBADDRLOW + i);
+			break;
+		}
+	}
 
 	return err;
 }
 
-u32 brcmf_sdcard_reg_read(struct brcmf_sdio_dev *sdiodev, u32 addr, uint size)
+static int
+brcmf_sdio_regrw_helper(struct brcmf_sdio_dev *sdiodev, u32 addr,
+			void *data, bool write)
 {
-	int status;
-	u32 word = 0;
-	uint bar0 = addr & ~SBSDIO_SB_OFT_ADDR_MASK;
-
-	brcmf_dbg(INFO, "fun = 1, addr = 0x%x\n", addr);
-
-	if (bar0 != sdiodev->sbwad) {
-		if (brcmf_sdcard_set_sbaddr_window(sdiodev, bar0))
-			return 0xFFFFFFFF;
-
-		sdiodev->sbwad = bar0;
-	}
-
-	addr &= SBSDIO_SB_OFT_ADDR_MASK;
-	if (size == 4)
+	u8 func_num, reg_size;
+	u32 bar;
+	s32 retry = 0;
+	int ret;
+
+	/*
+	 * figure out how to read the register based on address range
+	 * 0x00 ~ 0x7FF: function 0 CCCR and FBR
+	 * 0x10000 ~ 0x1FFFF: function 1 miscellaneous registers
+	 * The rest: function 1 silicon backplane core registers
+	 */
+	if ((addr & ~REG_F0_REG_MASK) == 0) {
+		func_num = SDIO_FUNC_0;
+		reg_size = 1;
+	} else if ((addr & ~REG_F1_MISC_MASK) == 0) {
+		func_num = SDIO_FUNC_1;
+		reg_size = 1;
+	} else {
+		func_num = SDIO_FUNC_1;
+		reg_size = 4;
+
+		/* Set the window for SB core register */
+		bar = addr & ~SBSDIO_SB_OFT_ADDR_MASK;
+		if (bar != sdiodev->sbwad) {
+			ret = brcmf_sdcard_set_sbaddr_window(sdiodev, bar);
+			if (ret != 0) {
+				memset(data, 0xFF, reg_size);
+				return ret;
+			}
+			sdiodev->sbwad = bar;
+		}
+		addr &= SBSDIO_SB_OFT_ADDR_MASK;
 		addr |= SBSDIO_SB_ACCESS_2_4B_FLAG;
+	}
 
-	status = brcmf_sdioh_request_word(sdiodev, SDIOH_READ, SDIO_FUNC_1,
-					  addr, &word, size);
+	do {
+		if (!write)
+			memset(data, 0, reg_size);
+		if (retry)	/* wait for 1 ms till bus get settled down */
+			usleep_range(1000, 2000);
+		if (reg_size == 1)
+			ret = brcmf_sdioh_request_byte(sdiodev, write,
+						       func_num, addr, data);
+		else
+			ret = brcmf_sdioh_request_word(sdiodev, write,
+						       func_num, addr, data, 4);
+	} while (ret != 0 && retry++ < SDIOH_API_ACCESS_RETRY_LIMIT);
+
+	if (ret != 0)
+		brcmf_dbg(ERROR, "failed with %d\n", ret);
 
-	sdiodev->regfail = (status != 0);
+	return ret;
+}
 
-	brcmf_dbg(INFO, "u32data = 0x%x\n", word);
+u8 brcmf_sdio_regrb(struct brcmf_sdio_dev *sdiodev, u32 addr, int *ret)
+{
+	u8 data;
+	int retval;
 
-	/* if ok, return appropriately masked word */
-	if (status == 0) {
-		switch (size) {
-		case sizeof(u8):
-			return word & 0xff;
-		case sizeof(u16):
-			return word & 0xffff;
-		case sizeof(u32):
-			return word;
-		default:
-			sdiodev->regfail = true;
+	brcmf_dbg(INFO, "addr:0x%08x\n", addr);
+	retval = brcmf_sdio_regrw_helper(sdiodev, addr, &data, false);
+	brcmf_dbg(INFO, "data:0x%02x\n", data);
 
-		}
-	}
+	if (ret)
+		*ret = retval;
 
-	/* otherwise, bad sdio access or invalid size */
-	brcmf_dbg(ERROR, "error reading addr 0x%04x size %d\n", addr, size);
-	return 0xFFFFFFFF;
+	return data;
 }
 
-u32 brcmf_sdcard_reg_write(struct brcmf_sdio_dev *sdiodev, u32 addr, uint size,
-			   u32 data)
+u32 brcmf_sdio_regrl(struct brcmf_sdio_dev *sdiodev, u32 addr, int *ret)
 {
-	int status;
-	uint bar0 = addr & ~SBSDIO_SB_OFT_ADDR_MASK;
-	int err = 0;
+	u32 data;
+	int retval;
 
-	brcmf_dbg(INFO, "fun = 1, addr = 0x%x, uint%ddata = 0x%x\n",
-		  addr, size * 8, data);
+	brcmf_dbg(INFO, "addr:0x%08x\n", addr);
+	retval = brcmf_sdio_regrw_helper(sdiodev, addr, &data, false);
+	brcmf_dbg(INFO, "data:0x%08x\n", data);
 
-	if (bar0 != sdiodev->sbwad) {
-		err = brcmf_sdcard_set_sbaddr_window(sdiodev, bar0);
-		if (err)
-			return err;
+	if (ret)
+		*ret = retval;
 
-		sdiodev->sbwad = bar0;
-	}
+	return data;
+}
 
-	addr &= SBSDIO_SB_OFT_ADDR_MASK;
-	if (size == 4)
-		addr |= SBSDIO_SB_ACCESS_2_4B_FLAG;
-	status =
-	    brcmf_sdioh_request_word(sdiodev, SDIOH_WRITE, SDIO_FUNC_1,
-				     addr, &data, size);
-	sdiodev->regfail = (status != 0);
+void brcmf_sdio_regwb(struct brcmf_sdio_dev *sdiodev, u32 addr,
+		      u8 data, int *ret)
+{
+	int retval;
 
-	if (status == 0)
-		return 0;
+	brcmf_dbg(INFO, "addr:0x%08x, data:0x%02x\n", addr, data);
+	retval = brcmf_sdio_regrw_helper(sdiodev, addr, &data, true);
 
-	brcmf_dbg(ERROR, "error writing 0x%08x to addr 0x%04x size %d\n",
-		  data, addr, size);
-	return 0xFFFFFFFF;
+	if (ret)
+		*ret = retval;
 }
 
-bool brcmf_sdcard_regfail(struct brcmf_sdio_dev *sdiodev)
+void brcmf_sdio_regwl(struct brcmf_sdio_dev *sdiodev, u32 addr,
+		      u32 data, int *ret)
 {
-	return sdiodev->regfail;
+	int retval;
+
+	brcmf_dbg(INFO, "addr:0x%08x, data:0x%08x\n", addr, data);
+	retval = brcmf_sdio_regrw_helper(sdiodev, addr, &data, true);
+
+	if (ret)
+		*ret = retval;
 }
 
 static int brcmf_sdcard_recv_prepare(struct brcmf_sdio_dev *sdiodev, uint fn,
diff --git a/drivers/net/wireless/brcm80211/brcmfmac/bcmsdh_sdmmc.c b/drivers/net/wireless/brcm80211/brcmfmac/bcmsdh_sdmmc.c
index 758c115b556..82f51dbd0d6 100644
--- a/drivers/net/wireless/brcm80211/brcmfmac/bcmsdh_sdmmc.c
+++ b/drivers/net/wireless/brcm80211/brcmfmac/bcmsdh_sdmmc.c
@@ -27,6 +27,7 @@
 #include <linux/errno.h>
 #include <linux/sched.h>	/* request_irq() */
 #include <linux/module.h>
+#include <linux/platform_device.h>
 #include <net/cfg80211.h>
 
 #include <defs.h>
@@ -55,6 +56,15 @@ static const struct sdio_device_id brcmf_sdmmc_ids[] = {
 };
 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 bool
 brcmf_pm_resume_error(struct brcmf_sdio_dev *sdiodev)
 {
@@ -107,7 +117,8 @@ static inline int brcmf_sdioh_f0_write_byte(struct brcmf_sdio_dev *sdiodev,
 			}
 			sdio_release_host(sdfunc);
 		}
-	} else if (regaddr == SDIO_CCCR_ABORT) {
+	} else if ((regaddr == SDIO_CCCR_ABORT) ||
+		   (regaddr == SDIO_CCCR_IENx)) {
 		sdfunc = kmemdup(sdiodev->func[0], sizeof(struct sdio_func),
 				 GFP_KERNEL);
 		if (!sdfunc)
@@ -335,43 +346,17 @@ int brcmf_sdioh_request_buffer(struct brcmf_sdio_dev *sdiodev,
 	return status;
 }
 
-/* Read client card reg */
-static int
-brcmf_sdioh_card_regread(struct brcmf_sdio_dev *sdiodev, int func, u32 regaddr,
-			 int regsize, u32 *data)
-{
-
-	if ((func == 0) || (regsize == 1)) {
-		u8 temp = 0;
-
-		brcmf_sdioh_request_byte(sdiodev, SDIOH_READ, func, regaddr,
-					 &temp);
-		*data = temp;
-		*data &= 0xff;
-		brcmf_dbg(DATA, "byte read data=0x%02x\n", *data);
-	} else {
-		brcmf_sdioh_request_word(sdiodev, SDIOH_READ, func, regaddr,
-					 data, regsize);
-		if (regsize == 2)
-			*data &= 0xffff;
-
-		brcmf_dbg(DATA, "word read data=0x%08x\n", *data);
-	}
-
-	return SUCCESS;
-}
-
 static int brcmf_sdioh_get_cisaddr(struct brcmf_sdio_dev *sdiodev, u32 regaddr)
 {
 	/* read 24 bits and return valid 17 bit addr */
-	int i;
+	int i, ret;
 	u32 scratch, regdata;
 	__le32 scratch_le;
 	u8 *ptr = (u8 *)&scratch_le;
 
 	for (i = 0; i < 3; i++) {
-		if ((brcmf_sdioh_card_regread(sdiodev, 0, regaddr, 1,
-				&regdata)) != SUCCESS)
+		regdata = brcmf_sdio_regrl(sdiodev, regaddr, &ret);
+		if (ret != 0)
 			brcmf_dbg(ERROR, "Can't read!\n");
 
 		*ptr++ = (u8) regdata;
@@ -467,12 +452,40 @@ 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_dbg(ERROR, "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)
 {
 	int ret = 0;
 	struct brcmf_sdio_dev *sdiodev;
 	struct brcmf_bus *bus_if;
+
 	brcmf_dbg(TRACE, "Enter\n");
 	brcmf_dbg(TRACE, "func->class=%x\n", func->class);
 	brcmf_dbg(TRACE, "sdio_vendor: 0x%04x\n", func->vendor);
@@ -511,6 +524,10 @@ static int brcmf_ops_sdio_probe(struct sdio_func *func,
 		sdiodev = dev_get_drvdata(&func->card->dev);
 		if ((!sdiodev) || (sdiodev->func[1]->card != func->card))
 			return -ENODEV;
+
+		ret = brcmf_sdio_getintrcfg(sdiodev);
+		if (ret)
+			return ret;
 		sdiodev->func[2] = func;
 
 		bus_if = sdiodev->bus_if;
@@ -603,6 +620,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;
+
+	INIT_LIST_HEAD(&oobirq_lh);
+
+	for (i = 0; ; i++) {
+		res = platform_get_resource(pdev, IORESOURCE_IRQ, i);
+		if (!res)
+			break;
+
+		oobirq_entry = kzalloc(sizeof(struct brcmf_sdio_oobirq),
+				       GFP_KERNEL);
+		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;
+
+	ret = sdio_register_driver(&brcmf_sdmmc_driver);
+
+	if (ret)
+		brcmf_dbg(ERROR, "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)
+{
+	brcmf_dbg(TRACE, "Enter\n");
+
+	sdio_unregister_driver(&brcmf_sdmmc_driver);
+
+	platform_driver_unregister(&brcmf_sdio_pd);
+}
+
+void brcmf_sdio_init(void)
+{
+	int ret;
+
+	brcmf_dbg(TRACE, "Enter\n");
+
+	ret = platform_driver_register(&brcmf_sdio_pd);
+
+	if (ret)
+		brcmf_dbg(ERROR, "platform_driver_register failed: %d\n", ret);
+}
+#else
 void brcmf_sdio_exit(void)
 {
 	brcmf_dbg(TRACE, "Enter\n");
@@ -621,3 +697,4 @@ void brcmf_sdio_init(void)
 	if (ret)
 		brcmf_dbg(ERROR, "sdio_register_driver failed: %d\n", ret);
 }
+#endif		/* CONFIG_BRCMFMAC_SDIO_OOB */
diff --git a/drivers/net/wireless/brcm80211/brcmfmac/dhd.h b/drivers/net/wireless/brcm80211/brcmfmac/dhd.h
index 07686a748d3..9f637014486 100644
--- a/drivers/net/wireless/brcm80211/brcmfmac/dhd.h
+++ b/drivers/net/wireless/brcm80211/brcmfmac/dhd.h
@@ -632,7 +632,6 @@ extern const struct bcmevent_name bcmevent_names[];
 extern uint brcmf_c_mkiovar(char *name, char *data, uint datalen,
 			  char *buf, uint len);
 
-extern int brcmf_net_attach(struct brcmf_pub *drvr, int idx);
 extern int brcmf_netdev_wait_pend8021x(struct net_device *ndev);
 
 extern s32 brcmf_exec_dcmd(struct net_device *dev, u32 cmd, void *arg, u32 len);
diff --git a/drivers/net/wireless/brcm80211/brcmfmac/dhd_cdc.c b/drivers/net/wireless/brcm80211/brcmfmac/dhd_cdc.c
index b3e3b7f25d8..a5c15cac5e7 100644
--- a/drivers/net/wireless/brcm80211/brcmfmac/dhd_cdc.c
+++ b/drivers/net/wireless/brcm80211/brcmfmac/dhd_cdc.c
@@ -421,6 +421,7 @@ int brcmf_proto_hdrpull(struct device *dev, int *ifidx,
 	pktbuf->priority = h->priority & BDC_PRIORITY_MASK;
 
 	skb_pull(pktbuf, BDC_HEADER_LEN);
+	skb_pull(pktbuf, h->data_offset << 2);
 
 	return 0;
 }
diff --git a/drivers/net/wireless/brcm80211/brcmfmac/dhd_common.c b/drivers/net/wireless/brcm80211/brcmfmac/dhd_common.c
index 4187435220f..236cb9fa460 100644
--- a/drivers/net/wireless/brcm80211/brcmfmac/dhd_common.c
+++ b/drivers/net/wireless/brcm80211/brcmfmac/dhd_common.c
@@ -799,7 +799,6 @@ int brcmf_c_preinit_dcmds(struct brcmf_pub *drvr)
 {
 	char iovbuf[BRCMF_EVENTING_MASK_LEN + 12];	/*  Room for
 				 "event_msgs" + '\0' + bitvec  */
-	uint up = 0;
 	char buf[128], *ptr;
 	u32 dongle_align = drvr->bus_if->align;
 	u32 glom = 0;
@@ -853,9 +852,6 @@ int brcmf_c_preinit_dcmds(struct brcmf_pub *drvr)
 	brcmf_proto_cdc_set_dcmd(drvr, 0, BRCMF_C_SET_VAR, iovbuf,
 				  sizeof(iovbuf));
 
-	/* Force STA UP */
-	brcmf_proto_cdc_set_dcmd(drvr, 0, BRCMF_C_UP, (char *)&up, sizeof(up));
-
 	/* Setup event_msgs */
 	brcmf_c_mkiovar("event_msgs", drvr->eventmask, BRCMF_EVENTING_MASK_LEN,
 		      iovbuf, sizeof(iovbuf));
diff --git a/drivers/net/wireless/brcm80211/brcmfmac/dhd_linux.c b/drivers/net/wireless/brcm80211/brcmfmac/dhd_linux.c
index 2a1e5ae0c40..8933f9b31a9 100644
--- a/drivers/net/wireless/brcm80211/brcmfmac/dhd_linux.c
+++ b/drivers/net/wireless/brcm80211/brcmfmac/dhd_linux.c
@@ -799,6 +799,7 @@ static int brcmf_netdev_open(struct net_device *ndev)
 	struct brcmf_bus *bus_if = drvr->bus_if;
 	u32 toe_ol;
 	s32 ret = 0;
+	uint up = 0;
 
 	brcmf_dbg(TRACE, "ifidx %d\n", ifp->idx);
 
@@ -822,6 +823,10 @@ static int brcmf_netdev_open(struct net_device *ndev)
 			drvr->iflist[ifp->idx]->ndev->features &=
 				~NETIF_F_IP_CSUM;
 	}
+
+	/* make sure RF is ready for work */
+	brcmf_proto_cdc_set_dcmd(drvr, 0, BRCMF_C_UP, (char *)&up, sizeof(up));
+
 	/* Allow transmit calls */
 	netif_start_queue(ndev);
 	drvr->bus_if->drvr_up = true;
@@ -843,6 +848,63 @@ static const struct net_device_ops brcmf_netdev_ops_pri = {
 	.ndo_set_rx_mode = brcmf_netdev_set_multicast_list
 };
 
+static int brcmf_net_attach(struct brcmf_if *ifp)
+{
+	struct brcmf_pub *drvr = ifp->drvr;
+	struct net_device *ndev;
+	u8 temp_addr[ETH_ALEN];
+
+	brcmf_dbg(TRACE, "ifidx %d\n", ifp->idx);
+
+	ndev = drvr->iflist[ifp->idx]->ndev;
+	ndev->netdev_ops = &brcmf_netdev_ops_pri;
+
+	/*
+	 * determine mac address to use
+	 */
+	if (is_valid_ether_addr(ifp->mac_addr))
+		memcpy(temp_addr, ifp->mac_addr, ETH_ALEN);
+	else
+		memcpy(temp_addr, drvr->mac, ETH_ALEN);
+
+	if (ifp->idx == 1) {
+		brcmf_dbg(TRACE, "ACCESS POINT MAC:\n");
+		/*  ACCESSPOINT INTERFACE CASE */
+		temp_addr[0] |= 0X02;	/* set bit 2 ,
+			 - Locally Administered address  */
+
+	}
+	ndev->hard_header_len = ETH_HLEN + drvr->hdrlen;
+	ndev->ethtool_ops = &brcmf_ethtool_ops;
+
+	drvr->rxsz = ndev->mtu + ndev->hard_header_len +
+			      drvr->hdrlen;
+
+	memcpy(ndev->dev_addr, temp_addr, ETH_ALEN);
+
+	/* attach to cfg80211 for primary interface */
+	if (!ifp->idx) {
+		drvr->config = brcmf_cfg80211_attach(ndev, drvr->dev, drvr);
+		if (drvr->config == NULL) {
+			brcmf_dbg(ERROR, "wl_cfg80211_attach failed\n");
+			goto fail;
+		}
+	}
+
+	if (register_netdev(ndev) != 0) {
+		brcmf_dbg(ERROR, "couldn't register the net device\n");
+		goto fail;
+	}
+
+	brcmf_dbg(INFO, "%s: Broadcom Dongle Host Driver\n", ndev->name);
+
+	return 0;
+
+fail:
+	ndev->netdev_ops = NULL;
+	return -EBADE;
+}
+
 int
 brcmf_add_if(struct device *dev, int ifidx, char *name, u8 *mac_addr)
 {
@@ -882,7 +944,7 @@ brcmf_add_if(struct device *dev, int ifidx, char *name, u8 *mac_addr)
 	if (mac_addr != NULL)
 		memcpy(&ifp->mac_addr, mac_addr, ETH_ALEN);
 
-	if (brcmf_net_attach(drvr, ifp->idx)) {
+	if (brcmf_net_attach(ifp)) {
 		brcmf_dbg(ERROR, "brcmf_net_attach failed");
 		free_netdev(ifp->ndev);
 		drvr->iflist[ifidx] = NULL;
@@ -1016,69 +1078,16 @@ int brcmf_bus_start(struct device *dev)
 	if (ret < 0)
 		return ret;
 
+	/* add primary networking interface */
+	ret = brcmf_add_if(dev, 0, "wlan%d", drvr->mac);
+	if (ret < 0)
+		return ret;
+
 	/* signal bus ready */
 	bus_if->state = BRCMF_BUS_DATA;
 	return 0;
 }
 
-int brcmf_net_attach(struct brcmf_pub *drvr, int ifidx)
-{
-	struct net_device *ndev;
-	u8 temp_addr[ETH_ALEN] = {
-		0x00, 0x90, 0x4c, 0x11, 0x22, 0x33};
-
-	brcmf_dbg(TRACE, "ifidx %d\n", ifidx);
-
-	ndev = drvr->iflist[ifidx]->ndev;
-	ndev->netdev_ops = &brcmf_netdev_ops_pri;
-
-	/*
-	 * We have to use the primary MAC for virtual interfaces
-	 */
-	if (ifidx != 0) {
-		/* for virtual interfaces use the primary MAC  */
-		memcpy(temp_addr, drvr->mac, ETH_ALEN);
-
-	}
-
-	if (ifidx == 1) {
-		brcmf_dbg(TRACE, "ACCESS POINT MAC:\n");
-		/*  ACCESSPOINT INTERFACE CASE */
-		temp_addr[0] |= 0X02;	/* set bit 2 ,
-			 - Locally Administered address  */
-
-	}
-	ndev->hard_header_len = ETH_HLEN + drvr->hdrlen;
-	ndev->ethtool_ops = &brcmf_ethtool_ops;
-
-	drvr->rxsz = ndev->mtu + ndev->hard_header_len +
-			      drvr->hdrlen;
-
-	memcpy(ndev->dev_addr, temp_addr, ETH_ALEN);
-
-	/* attach to cfg80211 for primary interface */
-	if (!ifidx) {
-		drvr->config = brcmf_cfg80211_attach(ndev, drvr->dev, drvr);
-		if (drvr->config == NULL) {
-			brcmf_dbg(ERROR, "wl_cfg80211_attach failed\n");
-			goto fail;
-		}
-	}
-
-	if (register_netdev(ndev) != 0) {
-		brcmf_dbg(ERROR, "couldn't register the net device\n");
-		goto fail;
-	}
-
-	brcmf_dbg(INFO, "%s: Broadcom Dongle Host Driver\n", ndev->name);
-
-	return 0;
-
-fail:
-	ndev->netdev_ops = NULL;
-	return -EBADE;
-}
-
 static void brcmf_bus_detach(struct brcmf_pub *drvr)
 {
 	brcmf_dbg(TRACE, "Enter\n");
diff --git a/drivers/net/wireless/brcm80211/brcmfmac/dhd_sdio.c b/drivers/net/wireless/brcm80211/brcmfmac/dhd_sdio.c
index eb3829b03cd..1dbf2be478c 100644
--- a/drivers/net/wireless/brcm80211/brcmfmac/dhd_sdio.c
+++ b/drivers/net/wireless/brcm80211/brcmfmac/dhd_sdio.c
@@ -629,43 +629,29 @@ static bool data_ok(struct brcmf_sdio *bus)
  * Reads a register in the SDIO hardware block. This block occupies a series of
  * adresses on the 32 bit backplane bus.
  */
-static void
-r_sdreg32(struct brcmf_sdio *bus, u32 *regvar, u32 reg_offset, u32 *retryvar)
+static int
+r_sdreg32(struct brcmf_sdio *bus, u32 *regvar, u32 offset)
 {
 	u8 idx = brcmf_sdio_chip_getinfidx(bus->ci, BCMA_CORE_SDIO_DEV);
-	*retryvar = 0;
-	do {
-		*regvar = brcmf_sdcard_reg_read(bus->sdiodev,
-				bus->ci->c_inf[idx].base + reg_offset,
-				sizeof(u32));
-	} while (brcmf_sdcard_regfail(bus->sdiodev) &&
-		 (++(*retryvar) <= retry_limit));
-	if (*retryvar) {
-		bus->regfails += (*retryvar-1);
-		if (*retryvar > retry_limit) {
-			brcmf_dbg(ERROR, "FAILED READ %Xh\n", reg_offset);
-			*regvar = 0;
-		}
-	}
+	int ret;
+
+	*regvar = brcmf_sdio_regrl(bus->sdiodev,
+				   bus->ci->c_inf[idx].base + offset, &ret);
+
+	return ret;
 }
 
-static void
-w_sdreg32(struct brcmf_sdio *bus, u32 regval, u32 reg_offset, u32 *retryvar)
+static int
+w_sdreg32(struct brcmf_sdio *bus, u32 regval, u32 reg_offset)
 {
 	u8 idx = brcmf_sdio_chip_getinfidx(bus->ci, BCMA_CORE_SDIO_DEV);
-	*retryvar = 0;
-	do {
-		brcmf_sdcard_reg_write(bus->sdiodev,
-				       bus->ci->c_inf[idx].base + reg_offset,
-				       sizeof(u32), regval);
-	} while (brcmf_sdcard_regfail(bus->sdiodev) &&
-		 (++(*retryvar) <= retry_limit));
-	if (*retryvar) {
-		bus->regfails += (*retryvar-1);
-		if (*retryvar > retry_limit)
-			brcmf_dbg(ERROR, "FAILED REGISTER WRITE %Xh\n",
-				  reg_offset);
-	}
+	int ret;
+
+	brcmf_sdio_regwl(bus->sdiodev,
+			 bus->ci->c_inf[idx].base + reg_offset,
+			 regval, &ret);
+
+	return ret;
 }
 
 #define PKT_AVAILABLE()		(intstatus & I_HMB_FRAME_IND)
@@ -697,16 +683,16 @@ static int brcmf_sdbrcm_htclk(struct brcmf_sdio *bus, bool on, bool pendok)
 		clkreq =
 		    bus->alp_only ? SBSDIO_ALP_AVAIL_REQ : SBSDIO_HT_AVAIL_REQ;
 
-		brcmf_sdcard_cfg_write(bus->sdiodev, SDIO_FUNC_1,
-				       SBSDIO_FUNC1_CHIPCLKCSR, clkreq, &err);
+		brcmf_sdio_regwb(bus->sdiodev, SBSDIO_FUNC1_CHIPCLKCSR,
+				 clkreq, &err);
 		if (err) {
 			brcmf_dbg(ERROR, "HT Avail request error: %d\n", err);
 			return -EBADE;
 		}
 
 		/* Check current status */
-		clkctl = brcmf_sdcard_cfg_read(bus->sdiodev, SDIO_FUNC_1,
-					       SBSDIO_FUNC1_CHIPCLKCSR, &err);
+		clkctl = brcmf_sdio_regrb(bus->sdiodev,
+					  SBSDIO_FUNC1_CHIPCLKCSR, &err);
 		if (err) {
 			brcmf_dbg(ERROR, "HT Avail read error: %d\n", err);
 			return -EBADE;
@@ -715,9 +701,8 @@ static int brcmf_sdbrcm_htclk(struct brcmf_sdio *bus, bool on, bool pendok)
 		/* Go to pending and await interrupt if appropriate */
 		if (!SBSDIO_CLKAV(clkctl, bus->alp_only) && pendok) {
 			/* Allow only clock-available interrupt */
-			devctl = brcmf_sdcard_cfg_read(bus->sdiodev,
-					SDIO_FUNC_1,
-					SBSDIO_DEVICE_CTL, &err);
+			devctl = brcmf_sdio_regrb(bus->sdiodev,
+						  SBSDIO_DEVICE_CTL, &err);
 			if (err) {
 				brcmf_dbg(ERROR, "Devctl error setting CA: %d\n",
 					  err);
@@ -725,30 +710,28 @@ static int brcmf_sdbrcm_htclk(struct brcmf_sdio *bus, bool on, bool pendok)
 			}
 
 			devctl |= SBSDIO_DEVCTL_CA_INT_ONLY;
-			brcmf_sdcard_cfg_write(bus->sdiodev, SDIO_FUNC_1,
-					       SBSDIO_DEVICE_CTL, devctl, &err);
+			brcmf_sdio_regwb(bus->sdiodev, SBSDIO_DEVICE_CTL,
+					 devctl, &err);
 			brcmf_dbg(INFO, "CLKCTL: set PENDING\n");
 			bus->clkstate = CLK_PENDING;
 
 			return 0;
 		} else if (bus->clkstate == CLK_PENDING) {
 			/* Cancel CA-only interrupt filter */
-			devctl =
-			    brcmf_sdcard_cfg_read(bus->sdiodev, SDIO_FUNC_1,
+			devctl = brcmf_sdio_regrb(bus->sdiodev,
 						  SBSDIO_DEVICE_CTL, &err);
 			devctl &= ~SBSDIO_DEVCTL_CA_INT_ONLY;
-			brcmf_sdcard_cfg_write(bus->sdiodev, SDIO_FUNC_1,
-				SBSDIO_DEVICE_CTL, devctl, &err);
+			brcmf_sdio_regwb(bus->sdiodev, SBSDIO_DEVICE_CTL,
+					 devctl, &err);
 		}
 
 		/* Otherwise, wait here (polling) for HT Avail */
 		timeout = jiffies +
 			  msecs_to_jiffies(PMU_MAX_TRANSITION_DLY/1000);
 		while (!SBSDIO_CLKAV(clkctl, bus->alp_only)) {
-			clkctl = brcmf_sdcard_cfg_read(bus->sdiodev,
-						       SDIO_FUNC_1,
-						       SBSDIO_FUNC1_CHIPCLKCSR,
-						       &err);
+			clkctl = brcmf_sdio_regrb(bus->sdiodev,
+						  SBSDIO_FUNC1_CHIPCLKCSR,
+						  &err);
 			if (time_after(jiffies, timeout))
 				break;
 			else
@@ -781,17 +764,16 @@ static int brcmf_sdbrcm_htclk(struct brcmf_sdio *bus, bool on, bool pendok)
 
 		if (bus->clkstate == CLK_PENDING) {
 			/* Cancel CA-only interrupt filter */
-			devctl = brcmf_sdcard_cfg_read(bus->sdiodev,
-					SDIO_FUNC_1,
-					SBSDIO_DEVICE_CTL, &err);
+			devctl = brcmf_sdio_regrb(bus->sdiodev,
+						  SBSDIO_DEVICE_CTL, &err);
 			devctl &= ~SBSDIO_DEVCTL_CA_INT_ONLY;
-			brcmf_sdcard_cfg_write(bus->sdiodev, SDIO_FUNC_1,
-				SBSDIO_DEVICE_CTL, devctl, &err);
+			brcmf_sdio_regwb(bus->sdiodev, SBSDIO_DEVICE_CTL,
+					 devctl, &err);
 		}
 
 		bus->clkstate = CLK_SDONLY;
-		brcmf_sdcard_cfg_write(bus->sdiodev, SDIO_FUNC_1,
-			SBSDIO_FUNC1_CHIPCLKCSR, clkreq, &err);
+		brcmf_sdio_regwb(bus->sdiodev, SBSDIO_FUNC1_CHIPCLKCSR,
+				 clkreq, &err);
 		brcmf_dbg(INFO, "CLKCTL: turned OFF\n");
 		if (err) {
 			brcmf_dbg(ERROR, "Failed access turning clock off: %d\n",
@@ -874,7 +856,7 @@ static int brcmf_sdbrcm_clkctl(struct brcmf_sdio *bus, uint target, bool pendok)
 
 static int brcmf_sdbrcm_bussleep(struct brcmf_sdio *bus, bool sleep)
 {
-	uint retries = 0;
+	int ret;
 
 	brcmf_dbg(INFO, "request %s (currently %s)\n",
 		  sleep ? "SLEEP" : "WAKE",
@@ -894,22 +876,20 @@ static int brcmf_sdbrcm_bussleep(struct brcmf_sdio *bus, bool sleep)
 		brcmf_sdbrcm_clkctl(bus, CLK_AVAIL, false);
 
 		/* Tell device to start using OOB wakeup */
-		w_sdreg32(bus, SMB_USE_OOB,
-			  offsetof(struct sdpcmd_regs, tosbmailbox), &retries);
-		if (retries > retry_limit)
+		ret = w_sdreg32(bus, SMB_USE_OOB,
+				offsetof(struct sdpcmd_regs, tosbmailbox));
+		if (ret != 0)
 			brcmf_dbg(ERROR, "CANNOT SIGNAL CHIP, WILL NOT WAKE UP!!\n");
 
 		/* Turn off our contribution to the HT clock request */
 		brcmf_sdbrcm_clkctl(bus, CLK_SDONLY, false);
 
-		brcmf_sdcard_cfg_write(bus->sdiodev, SDIO_FUNC_1,
-			SBSDIO_FUNC1_CHIPCLKCSR,
-			SBSDIO_FORCE_HW_CLKREQ_OFF, NULL);
+		brcmf_sdio_regwb(bus->sdiodev, SBSDIO_FUNC1_CHIPCLKCSR,
+				 SBSDIO_FORCE_HW_CLKREQ_OFF, NULL);
 
 		/* Isolate the bus */
-		brcmf_sdcard_cfg_write(bus->sdiodev, SDIO_FUNC_1,
-			SBSDIO_DEVICE_CTL,
-			SBSDIO_DEVCTL_PADS_ISO, NULL);
+		brcmf_sdio_regwb(bus->sdiodev, SBSDIO_DEVICE_CTL,
+				 SBSDIO_DEVCTL_PADS_ISO, NULL);
 
 		/* Change state */
 		bus->sleeping = true;
@@ -917,21 +897,20 @@ static int brcmf_sdbrcm_bussleep(struct brcmf_sdio *bus, bool sleep)
 	} else {
 		/* Waking up: bus power up is ok, set local state */
 
-		brcmf_sdcard_cfg_write(bus->sdiodev, SDIO_FUNC_1,
-			SBSDIO_FUNC1_CHIPCLKCSR, 0, NULL);
+		brcmf_sdio_regwb(bus->sdiodev, SBSDIO_FUNC1_CHIPCLKCSR,
+				 0, NULL);
 
 		/* Make sure the controller has the bus up */
 		brcmf_sdbrcm_clkctl(bus, CLK_AVAIL, false);
 
 		/* Send misc interrupt to indicate OOB not needed */
-		w_sdreg32(bus, 0, offsetof(struct sdpcmd_regs, tosbmailboxdata),
-			  &retries);
-		if (retries <= retry_limit)
-			w_sdreg32(bus, SMB_DEV_INT,
-				  offsetof(struct sdpcmd_regs, tosbmailbox),
-				  &retries);
-
-		if (retries > retry_limit)
+		ret = w_sdreg32(bus, 0,
+				offsetof(struct sdpcmd_regs, tosbmailboxdata));
+		if (ret == 0)
+			ret = w_sdreg32(bus, SMB_DEV_INT,
+				offsetof(struct sdpcmd_regs, tosbmailbox));
+
+		if (ret != 0)
 			brcmf_dbg(ERROR, "CANNOT SIGNAL CHIP TO CLEAR OOB!!\n");
 
 		/* Make sure we have SD bus access */
@@ -955,17 +934,17 @@ static u32 brcmf_sdbrcm_hostmail(struct brcmf_sdio *bus)
 	u32 intstatus = 0;
 	u32 hmb_data;
 	u8 fcbits;
-	uint retries = 0;
+	int ret;
 
 	brcmf_dbg(TRACE, "Enter\n");
 
 	/* Read mailbox data and ack that we did so */
-	r_sdreg32(bus, &hmb_data,
-		  offsetof(struct sdpcmd_regs, tohostmailboxdata), &retries);
+	ret = r_sdreg32(bus, &hmb_data,
+			offsetof(struct sdpcmd_regs, tohostmailboxdata));
 
-	if (retries <= retry_limit)
+	if (ret == 0)
 		w_sdreg32(bus, SMB_INT_ACK,
-			  offsetof(struct sdpcmd_regs, tosbmailbox), &retries);
+			  offsetof(struct sdpcmd_regs, tosbmailbox));
 	bus->f1regdata += 2;
 
 	/* Dongle recomposed rx frames, accept them again */
@@ -1040,17 +1019,16 @@ static void brcmf_sdbrcm_rxfail(struct brcmf_sdio *bus, bool abort, bool rtx)
 	if (abort)
 		brcmf_sdcard_abort(bus->sdiodev, SDIO_FUNC_2);
 
-	brcmf_sdcard_cfg_write(bus->sdiodev, SDIO_FUNC_1,
-			       SBSDIO_FUNC1_FRAMECTRL,
-			       SFC_RF_TERM, &err);
+	brcmf_sdio_regwb(bus->sdiodev, SBSDIO_FUNC1_FRAMECTRL,
+			 SFC_RF_TERM, &err);
 	bus->f1regdata++;
 
 	/* Wait until the packet has been flushed (device/FIFO stable) */
 	for (lastrbc = retries = 0xffff; retries > 0; retries--) {
-		hi = brcmf_sdcard_cfg_read(bus->sdiodev, SDIO_FUNC_1,
-					   SBSDIO_FUNC1_RFRAMEBCHI, NULL);
-		lo = brcmf_sdcard_cfg_read(bus->sdiodev, SDIO_FUNC_1,
-					   SBSDIO_FUNC1_RFRAMEBCLO, NULL);
+		hi = brcmf_sdio_regrb(bus->sdiodev,
+				      SBSDIO_FUNC1_RFRAMEBCHI, &err);
+		lo = brcmf_sdio_regrb(bus->sdiodev,
+				      SBSDIO_FUNC1_RFRAMEBCLO, &err);
 		bus->f1regdata += 2;
 
 		if ((hi == 0) && (lo == 0))
@@ -1070,11 +1048,11 @@ static void brcmf_sdbrcm_rxfail(struct brcmf_sdio *bus, bool abort, bool rtx)
 
 	if (rtx) {
 		bus->rxrtx++;
-		w_sdreg32(bus, SMB_NAK,
-			  offsetof(struct sdpcmd_regs, tosbmailbox), &retries);
+		err = w_sdreg32(bus, SMB_NAK,
+				offsetof(struct sdpcmd_regs, tosbmailbox));
 
 		bus->f1regdata++;
-		if (retries <= retry_limit)
+		if (err == 0)
 			bus->rxskip = true;
 	}
 
@@ -1082,7 +1060,7 @@ static void brcmf_sdbrcm_rxfail(struct brcmf_sdio *bus, bool abort, bool rtx)
 	bus->nextlen = 0;
 
 	/* If we can't reach the device, signal failure */
-	if (err || brcmf_sdcard_regfail(bus->sdiodev))
+	if (err)
 		bus->sdiodev->bus_if->state = BRCMF_BUS_DOWN;
 }
 
@@ -2178,21 +2156,16 @@ static int brcmf_sdbrcm_txpkt(struct brcmf_sdio *bus, struct sk_buff *pkt,
 		bus->tx_sderrs++;
 
 		brcmf_sdcard_abort(bus->sdiodev, SDIO_FUNC_2);
-		brcmf_sdcard_cfg_write(bus->sdiodev, SDIO_FUNC_1,
-				 SBSDIO_FUNC1_FRAMECTRL, SFC_WF_TERM,
-				 NULL);
+		brcmf_sdio_regwb(bus->sdiodev, SBSDIO_FUNC1_FRAMECTRL,
+				 SFC_WF_TERM, NULL);
 		bus->f1regdata++;
 
 		for (i = 0; i < 3; i++) {
 			u8 hi, lo;
-			hi = brcmf_sdcard_cfg_read(bus->sdiodev,
-					     SDIO_FUNC_1,
-					     SBSDIO_FUNC1_WFRAMEBCHI,
-					     NULL);
-			lo = brcmf_sdcard_cfg_read(bus->sdiodev,
-					     SDIO_FUNC_1,
-					     SBSDIO_FUNC1_WFRAMEBCLO,
-					     NULL);
+			hi = brcmf_sdio_regrb(bus->sdiodev,
+					      SBSDIO_FUNC1_WFRAMEBCHI, NULL);
+			lo = brcmf_sdio_regrb(bus->sdiodev,
+					      SBSDIO_FUNC1_WFRAMEBCLO, NULL);
 			bus->f1regdata += 2;
 			if ((hi == 0) && (lo == 0))
 				break;
@@ -2219,7 +2192,6 @@ static uint brcmf_sdbrcm_sendfromq(struct brcmf_sdio *bus, uint maxframes)
 {
 	struct sk_buff *pkt;
 	u32 intstatus = 0;
-	uint retries = 0;
 	int ret = 0, prec_out;
 	uint cnt = 0;
 	uint datalen;
@@ -2249,11 +2221,11 @@ static uint brcmf_sdbrcm_sendfromq(struct brcmf_sdio *bus, uint maxframes)
 		/* In poll mode, need to check for other events */
 		if (!bus->intr && cnt) {
 			/* Check device status, signal pending interrupt */
-			r_sdreg32(bus, &intstatus,
-				  offsetof(struct sdpcmd_regs, intstatus),
-				  &retries);
+			ret = r_sdreg32(bus, &intstatus,
+					offsetof(struct sdpcmd_regs,
+						 intstatus));
 			bus->f2txdata++;
-			if (brcmf_sdcard_regfail(bus->sdiodev))
+			if (ret != 0)
 				break;
 			if (intstatus & bus->hostintmask)
 				bus->ipend = true;
@@ -2275,7 +2247,6 @@ static void brcmf_sdbrcm_bus_stop(struct device *dev)
 {
 	u32 local_hostintmask;
 	u8 saveclk;
-	uint retries;
 	int err;
 	struct brcmf_bus *bus_if = dev_get_drvdata(dev);
 	struct brcmf_sdio_dev *sdiodev = bus_if->bus_priv.sdio;
@@ -2303,7 +2274,7 @@ static void brcmf_sdbrcm_bus_stop(struct device *dev)
 	brcmf_sdbrcm_clkctl(bus, CLK_AVAIL, false);
 
 	/* Disable and clear interrupts at the chip level also */
-	w_sdreg32(bus, 0, offsetof(struct sdpcmd_regs, hostintmask), &retries);
+	w_sdreg32(bus, 0, offsetof(struct sdpcmd_regs, hostintmask));
 	local_hostintmask = bus->hostintmask;
 	bus->hostintmask = 0;
 
@@ -2311,24 +2282,23 @@ static void brcmf_sdbrcm_bus_stop(struct device *dev)
 	bus->sdiodev->bus_if->state = BRCMF_BUS_DOWN;
 
 	/* Force clocks on backplane to be sure F2 interrupt propagates */
-	saveclk = brcmf_sdcard_cfg_read(bus->sdiodev, SDIO_FUNC_1,
-					SBSDIO_FUNC1_CHIPCLKCSR, &err);
+	saveclk = brcmf_sdio_regrb(bus->sdiodev,
+				   SBSDIO_FUNC1_CHIPCLKCSR, &err);
 	if (!err) {
-		brcmf_sdcard_cfg_write(bus->sdiodev, SDIO_FUNC_1,
-				       SBSDIO_FUNC1_CHIPCLKCSR,
-				       (saveclk | SBSDIO_FORCE_HT), &err);
+		brcmf_sdio_regwb(bus->sdiodev, SBSDIO_FUNC1_CHIPCLKCSR,
+				 (saveclk | SBSDIO_FORCE_HT), &err);
 	}
 	if (err)
 		brcmf_dbg(ERROR, "Failed to force clock for F2: err %d\n", err);
 
 	/* Turn off the bus (F2), free any pending packets */
 	brcmf_dbg(INTR, "disable SDIO interrupts\n");
-	brcmf_sdcard_cfg_write(bus->sdiodev, SDIO_FUNC_0, SDIO_CCCR_IOEx,
-			 SDIO_FUNC_ENABLE_1, NULL);
+	brcmf_sdio_regwb(bus->sdiodev, SDIO_CCCR_IOEx, SDIO_FUNC_ENABLE_1,
+			 NULL);
 
 	/* Clear any pending interrupts now that F2 is disabled */
 	w_sdreg32(bus, local_hostintmask,
-		  offsetof(struct sdpcmd_regs, intstatus), &retries);
+		  offsetof(struct sdpcmd_regs, intstatus));
 
 	/* Turn off the backplane clock (only) */
 	brcmf_sdbrcm_clkctl(bus, CLK_SDONLY, false);
@@ -2352,15 +2322,33 @@ static void brcmf_sdbrcm_bus_stop(struct device *dev)
 	up(&bus->sdsem);
 }
 
+#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 && !bus->ipend) {
+		enable_irq(bus->sdiodev->irq);
+		bus->sdiodev->irq_en = true;
+	}
+	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 bool brcmf_sdbrcm_dpc(struct brcmf_sdio *bus)
 {
 	u32 intstatus, newstatus = 0;
-	uint retries = 0;
 	uint rxlimit = bus->rxbound;	/* Rx frames to read before resched */
 	uint txlimit = bus->txbound;	/* Tx frames to send before resched */
 	uint framecnt = 0;	/* Temporary counter of tx/rx frames */
 	bool rxdone = true;	/* Flag for no more read data */
 	bool resched = false;	/* Flag indicating resched wanted */
+	int err;
 
 	brcmf_dbg(TRACE, "Enter\n");
 
@@ -2371,13 +2359,12 @@ static bool brcmf_sdbrcm_dpc(struct brcmf_sdio *bus)
 
 	/* If waiting for HTAVAIL, check status */
 	if (bus->clkstate == CLK_PENDING) {
-		int err;
 		u8 clkctl, devctl = 0;
 
 #ifdef DEBUG
 		/* Check for inconsistent device control */
-		devctl = brcmf_sdcard_cfg_read(bus->sdiodev, SDIO_FUNC_1,
-					       SBSDIO_DEVICE_CTL, &err);
+		devctl = brcmf_sdio_regrb(bus->sdiodev,
+					  SBSDIO_DEVICE_CTL, &err);
 		if (err) {
 			brcmf_dbg(ERROR, "error reading DEVCTL: %d\n", err);
 			bus->sdiodev->bus_if->state = BRCMF_BUS_DOWN;
@@ -2385,8 +2372,8 @@ static bool brcmf_sdbrcm_dpc(struct brcmf_sdio *bus)
 #endif				/* DEBUG */
 
 		/* Read CSR, if clock on switch to AVAIL, else ignore */
-		clkctl = brcmf_sdcard_cfg_read(bus->sdiodev, SDIO_FUNC_1,
-					       SBSDIO_FUNC1_CHIPCLKCSR, &err);
+		clkctl = brcmf_sdio_regrb(bus->sdiodev,
+					  SBSDIO_FUNC1_CHIPCLKCSR, &err);
 		if (err) {
 			brcmf_dbg(ERROR, "error reading CSR: %d\n",
 				  err);
@@ -2397,17 +2384,16 @@ static bool brcmf_sdbrcm_dpc(struct brcmf_sdio *bus)
 			  devctl, clkctl);
 
 		if (SBSDIO_HTAV(clkctl)) {
-			devctl = brcmf_sdcard_cfg_read(bus->sdiodev,
-						       SDIO_FUNC_1,
-						       SBSDIO_DEVICE_CTL, &err);
+			devctl = brcmf_sdio_regrb(bus->sdiodev,
+						  SBSDIO_DEVICE_CTL, &err);
 			if (err) {
 				brcmf_dbg(ERROR, "error reading DEVCTL: %d\n",
 					  err);
 				bus->sdiodev->bus_if->state = BRCMF_BUS_DOWN;
 			}
 			devctl &= ~SBSDIO_DEVCTL_CA_INT_ONLY;
-			brcmf_sdcard_cfg_write(bus->sdiodev, SDIO_FUNC_1,
-				SBSDIO_DEVICE_CTL, devctl, &err);
+			brcmf_sdio_regwb(bus->sdiodev, SBSDIO_DEVICE_CTL,
+					 devctl, &err);
 			if (err) {
 				brcmf_dbg(ERROR, "error writing DEVCTL: %d\n",
 					  err);
@@ -2429,17 +2415,17 @@ static bool brcmf_sdbrcm_dpc(struct brcmf_sdio *bus)
 	/* Pending interrupt indicates new device status */
 	if (bus->ipend) {
 		bus->ipend = false;
-		r_sdreg32(bus, &newstatus,
-			  offsetof(struct sdpcmd_regs, intstatus), &retries);
+		err = r_sdreg32(bus, &newstatus,
+				offsetof(struct sdpcmd_regs, intstatus));
 		bus->f1regdata++;
-		if (brcmf_sdcard_regfail(bus->sdiodev))
+		if (err != 0)
 			newstatus = 0;
 		newstatus &= bus->hostintmask;
 		bus->fcstate = !!(newstatus & I_HMB_FC_STATE);
 		if (newstatus) {
-			w_sdreg32(bus, newstatus,
-				  offsetof(struct sdpcmd_regs, intstatus),
-				  &retries);
+			err = w_sdreg32(bus, newstatus,
+					offsetof(struct sdpcmd_regs,
+						 intstatus));
 			bus->f1regdata++;
 		}
 	}
@@ -2454,11 +2440,11 @@ static bool brcmf_sdbrcm_dpc(struct brcmf_sdio *bus)
 	 */
 	if (intstatus & I_HMB_FC_CHANGE) {
 		intstatus &= ~I_HMB_FC_CHANGE;
-		w_sdreg32(bus, I_HMB_FC_CHANGE,
-			  offsetof(struct sdpcmd_regs, intstatus), &retries);
+		err = w_sdreg32(bus, I_HMB_FC_CHANGE,
+				offsetof(struct sdpcmd_regs, intstatus));
 
-		r_sdreg32(bus, &newstatus,
-			  offsetof(struct sdpcmd_regs, intstatus), &retries);
+		err = r_sdreg32(bus, &newstatus,
+				offsetof(struct sdpcmd_regs, intstatus));
 		bus->f1regdata += 2;
 		bus->fcstate =
 		    !!(newstatus & (I_HMB_FC_STATE | I_HMB_FC_CHANGE));
@@ -2509,6 +2495,8 @@ static bool brcmf_sdbrcm_dpc(struct brcmf_sdio *bus)
 	bus->intstatus = intstatus;
 
 clkwait:
+	brcmf_sdbrcm_clrintr(bus);
+
 	if (data_ok(bus) && bus->ctrl_frame_stat &&
 		(bus->clkstate == CLK_AVAIL)) {
 		int ret, i;
@@ -2526,21 +2514,18 @@ clkwait:
 
 			brcmf_sdcard_abort(bus->sdiodev, SDIO_FUNC_2);
 
-			brcmf_sdcard_cfg_write(bus->sdiodev, SDIO_FUNC_1,
-					 SBSDIO_FUNC1_FRAMECTRL, SFC_WF_TERM,
-					 NULL);
+			brcmf_sdio_regwb(bus->sdiodev, SBSDIO_FUNC1_FRAMECTRL,
+					 SFC_WF_TERM, &err);
 			bus->f1regdata++;
 
 			for (i = 0; i < 3; i++) {
 				u8 hi, lo;
-				hi = brcmf_sdcard_cfg_read(bus->sdiodev,
-						     SDIO_FUNC_1,
-						     SBSDIO_FUNC1_WFRAMEBCHI,
-						     NULL);
-				lo = brcmf_sdcard_cfg_read(bus->sdiodev,
-						     SDIO_FUNC_1,
-						     SBSDIO_FUNC1_WFRAMEBCLO,
-						     NULL);
+				hi = brcmf_sdio_regrb(bus->sdiodev,
+						      SBSDIO_FUNC1_WFRAMEBCHI,
+						      &err);
+				lo = brcmf_sdio_regrb(bus->sdiodev,
+						      SBSDIO_FUNC1_WFRAMEBCLO,
+						      &err);
 				bus->f1regdata += 2;
 				if ((hi == 0) && (lo == 0))
 					break;
@@ -2567,10 +2552,8 @@ clkwait:
 		 else await next interrupt */
 	/* On failed register access, all bets are off:
 		 no resched or interrupts */
-	if ((bus->sdiodev->bus_if->state == BRCMF_BUS_DOWN) ||
-	    brcmf_sdcard_regfail(bus->sdiodev)) {
-		brcmf_dbg(ERROR, "failed backplane access over SDIO, halting operation %d\n",
-			  brcmf_sdcard_regfail(bus->sdiodev));
+	if ((bus->sdiodev->bus_if->state == BRCMF_BUS_DOWN) || (err != 0)) {
+		brcmf_dbg(ERROR, "failed backplane access over SDIO, halting operation\n");
 		bus->sdiodev->bus_if->state = BRCMF_BUS_DOWN;
 		bus->intstatus = 0;
 	} else if (bus->clkstate == CLK_PENDING) {
@@ -2637,6 +2620,7 @@ static int brcmf_sdbrcm_dpc_thread(void *data)
 				/* after stopping the bus, exit thread */
 				brcmf_sdbrcm_bus_stop(bus->sdiodev->dev);
 				bus->dpc_tsk = NULL;
+				spin_lock_irqsave(&bus->dpc_tl_lock, flags);
 				break;
 			}
 
@@ -2865,19 +2849,16 @@ static int brcmf_tx_frame(struct brcmf_sdio *bus, u8 *frame, u16 len)
 
 		brcmf_sdcard_abort(bus->sdiodev, SDIO_FUNC_2);
 
-		brcmf_sdcard_cfg_write(bus->sdiodev, SDIO_FUNC_1,
-				       SBSDIO_FUNC1_FRAMECTRL,
-				       SFC_WF_TERM, NULL);
+		brcmf_sdio_regwb(bus->sdiodev, SBSDIO_FUNC1_FRAMECTRL,
+				 SFC_WF_TERM, NULL);
 		bus->f1regdata++;
 
 		for (i = 0; i < 3; i++) {
 			u8 hi, lo;
-			hi = brcmf_sdcard_cfg_read(bus->sdiodev, SDIO_FUNC_1,
-						   SBSDIO_FUNC1_WFRAMEBCHI,
-						   NULL);
-			lo = brcmf_sdcard_cfg_read(bus->sdiodev, SDIO_FUNC_1,
-						   SBSDIO_FUNC1_WFRAMEBCLO,
-						   NULL);
+			hi = brcmf_sdio_regrb(bus->sdiodev,
+					      SBSDIO_FUNC1_WFRAMEBCHI, NULL);
+			lo = brcmf_sdio_regrb(bus->sdiodev,
+					      SBSDIO_FUNC1_WFRAMEBCLO, NULL);
 			bus->f1regdata += 2;
 			if (hi == 0 && lo == 0)
 				break;
@@ -3167,7 +3148,6 @@ static int brcmf_sdbrcm_write_vars(struct brcmf_sdio *bus)
 
 static int brcmf_sdbrcm_download_state(struct brcmf_sdio *bus, bool enter)
 {
-	uint retries;
 	int bcmerror = 0;
 	struct chip_info *ci = bus->ci;
 
@@ -3201,7 +3181,7 @@ static int brcmf_sdbrcm_download_state(struct brcmf_sdio *bus, bool enter)
 		}
 
 		w_sdreg32(bus, 0xFFFFFFFF,
-			  offsetof(struct sdpcmd_regs, intstatus), &retries);
+			  offsetof(struct sdpcmd_regs, intstatus));
 
 		ci->resetcore(bus->sdiodev, ci, BCMA_CORE_ARM_CM3);
 
@@ -3423,7 +3403,6 @@ static int brcmf_sdbrcm_bus_init(struct device *dev)
 	struct brcmf_sdio_dev *sdiodev = bus_if->bus_priv.sdio;
 	struct brcmf_sdio *bus = sdiodev->bus;
 	unsigned long timeout;
-	uint retries = 0;
 	u8 ready, enable;
 	int err, ret = 0;
 	u8 saveclk;
@@ -3451,13 +3430,11 @@ static int brcmf_sdbrcm_bus_init(struct device *dev)
 		goto exit;
 
 	/* Force clocks on backplane to be sure F2 interrupt propagates */
-	saveclk =
-	    brcmf_sdcard_cfg_read(bus->sdiodev, SDIO_FUNC_1,
-				  SBSDIO_FUNC1_CHIPCLKCSR, &err);
+	saveclk = brcmf_sdio_regrb(bus->sdiodev,
+				   SBSDIO_FUNC1_CHIPCLKCSR, &err);
 	if (!err) {
-		brcmf_sdcard_cfg_write(bus->sdiodev, SDIO_FUNC_1,
-				       SBSDIO_FUNC1_CHIPCLKCSR,
-				       (saveclk | SBSDIO_FORCE_HT), &err);
+		brcmf_sdio_regwb(bus->sdiodev, SBSDIO_FUNC1_CHIPCLKCSR,
+				 (saveclk | SBSDIO_FORCE_HT), &err);
 	}
 	if (err) {
 		brcmf_dbg(ERROR, "Failed to force clock for F2: err %d\n", err);
@@ -3466,17 +3443,16 @@ static int brcmf_sdbrcm_bus_init(struct device *dev)
 
 	/* Enable function 2 (frame transfers) */
 	w_sdreg32(bus, SDPCM_PROT_VERSION << SMB_DATA_VERSION_SHIFT,
-		  offsetof(struct sdpcmd_regs, tosbmailboxdata), &retries);
+		  offsetof(struct sdpcmd_regs, tosbmailboxdata));
 	enable = (SDIO_FUNC_ENABLE_1 | SDIO_FUNC_ENABLE_2);
 
-	brcmf_sdcard_cfg_write(bus->sdiodev, SDIO_FUNC_0, SDIO_CCCR_IOEx,
-			       enable, NULL);
+	brcmf_sdio_regwb(bus->sdiodev, SDIO_CCCR_IOEx, enable, NULL);
 
 	timeout = jiffies + msecs_to_jiffies(BRCMF_WAIT_F2RDY);
 	ready = 0;
 	while (enable != ready) {
-		ready = brcmf_sdcard_cfg_read(bus->sdiodev, SDIO_FUNC_0,
-					      SDIO_CCCR_IORx, NULL);
+		ready = brcmf_sdio_regrb(bus->sdiodev,
+					 SDIO_CCCR_IORx, NULL);
 		if (time_after(jiffies, timeout))
 			break;
 		else if (time_after(jiffies, timeout - BRCMF_WAIT_F2RDY + 50))
@@ -3491,24 +3467,27 @@ static int brcmf_sdbrcm_bus_init(struct device *dev)
 		/* Set up the interrupt mask and enable interrupts */
 		bus->hostintmask = HOSTINTMASK;
 		w_sdreg32(bus, bus->hostintmask,
-			  offsetof(struct sdpcmd_regs, hostintmask), &retries);
+			  offsetof(struct sdpcmd_regs, hostintmask));
 
-		brcmf_sdcard_cfg_write(bus->sdiodev, SDIO_FUNC_1,
-				       SBSDIO_WATERMARK, 8, &err);
+		brcmf_sdio_regwb(bus->sdiodev, SBSDIO_WATERMARK, 8, &err);
 	} else {
 		/* Disable F2 again */
 		enable = SDIO_FUNC_ENABLE_1;
-		brcmf_sdcard_cfg_write(bus->sdiodev, SDIO_FUNC_0,
-				       SDIO_CCCR_IOEx, enable, NULL);
+		brcmf_sdio_regwb(bus->sdiodev, SDIO_CCCR_IOEx, enable, NULL);
 		ret = -ENODEV;
 	}
 
 	/* Restore previous clock setting */
-	brcmf_sdcard_cfg_write(bus->sdiodev, SDIO_FUNC_1,
-			       SBSDIO_FUNC1_CHIPCLKCSR, saveclk, &err);
+	brcmf_sdio_regwb(bus->sdiodev, SBSDIO_FUNC1_CHIPCLKCSR, saveclk, &err);
+
+	if (ret == 0) {
+		ret = brcmf_sdio_intr_register(bus->sdiodev);
+		if (ret != 0)
+			brcmf_dbg(ERROR, "intr register failed:%d\n", ret);
+	}
 
 	/* If we didn't come up, turn off backplane clock */
-	if (!ret)
+	if (bus_if->state != BRCMF_BUS_DATA)
 		brcmf_sdbrcm_clkctl(bus, CLK_NONE, false);
 
 exit:
@@ -3579,9 +3558,9 @@ static bool brcmf_sdbrcm_bus_watchdog(struct brcmf_sdio *bus)
 
 			if (!bus->dpc_sched) {
 				u8 devpend;
-				devpend = brcmf_sdcard_cfg_read(bus->sdiodev,
-						SDIO_FUNC_0, SDIO_CCCR_INTx,
-						NULL);
+				devpend = brcmf_sdio_regrb(bus->sdiodev,
+							   SDIO_CCCR_INTx,
+							   NULL);
 				intstatus =
 				    devpend & (INTR_STATUS_FUNC1 |
 					       INTR_STATUS_FUNC2);
@@ -3705,24 +3684,18 @@ brcmf_sdbrcm_probe_attach(struct brcmf_sdio *bus, u32 regsva)
 
 	bus->alp_only = true;
 
-	/* Return the window to backplane enumeration space for core access */
-	if (brcmf_sdcard_set_sbaddr_window(bus->sdiodev, SI_ENUM_BASE))
-		brcmf_dbg(ERROR, "FAILED to return to SI_ENUM_BASE\n");
-
 	pr_debug("F1 signature read @0x18000000=0x%4x\n",
-		 brcmf_sdcard_reg_read(bus->sdiodev, SI_ENUM_BASE, 4));
+		 brcmf_sdio_regrl(bus->sdiodev, SI_ENUM_BASE, NULL));
 
 	/*
 	 * Force PLL off until brcmf_sdio_chip_attach()
 	 * programs PLL control regs
 	 */
 
-	brcmf_sdcard_cfg_write(bus->sdiodev, SDIO_FUNC_1,
-			       SBSDIO_FUNC1_CHIPCLKCSR,
-			       BRCMF_INIT_CLKCTL1, &err);
+	brcmf_sdio_regwb(bus->sdiodev, SBSDIO_FUNC1_CHIPCLKCSR,
+			 BRCMF_INIT_CLKCTL1, &err);
 	if (!err)
-		clkctl =
-		    brcmf_sdcard_cfg_read(bus->sdiodev, SDIO_FUNC_1,
+		clkctl = brcmf_sdio_regrb(bus->sdiodev,
 					  SBSDIO_FUNC1_CHIPCLKCSR, &err);
 
 	if (err || ((clkctl & ~SBSDIO_AVBITS) != BRCMF_INIT_CLKCTL1)) {
@@ -3755,9 +3728,8 @@ brcmf_sdbrcm_probe_attach(struct brcmf_sdio *bus, u32 regsva)
 	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_sdcard_reg_read(bus->sdiodev, reg_addr, sizeof(u32));
-	brcmf_sdcard_reg_write(bus->sdiodev, reg_addr, sizeof(u32),
-			       reg_val | CC_BPRESEN);
+	reg_val = brcmf_sdio_regrl(bus->sdiodev, reg_addr, NULL);
+	brcmf_sdio_regwl(bus->sdiodev, reg_addr, reg_val | CC_BPRESEN, NULL);
 
 	brcmu_pktq_init(&bus->txq, (PRIOMASK + 1), TXQLEN);
 
@@ -3782,16 +3754,15 @@ static bool brcmf_sdbrcm_probe_init(struct brcmf_sdio *bus)
 	brcmf_dbg(TRACE, "Enter\n");
 
 	/* Disable F2 to clear any intermediate frame state on the dongle */
-	brcmf_sdcard_cfg_write(bus->sdiodev, SDIO_FUNC_0, SDIO_CCCR_IOEx,
-			       SDIO_FUNC_ENABLE_1, NULL);
+	brcmf_sdio_regwb(bus->sdiodev, SDIO_CCCR_IOEx,
+			 SDIO_FUNC_ENABLE_1, NULL);
 
 	bus->sdiodev->bus_if->state = BRCMF_BUS_DOWN;
 	bus->sleeping = false;
 	bus->rxflow = false;
 
 	/* Done with backplane-dependent accesses, can drop clock... */
-	brcmf_sdcard_cfg_write(bus->sdiodev, SDIO_FUNC_1,
-			       SBSDIO_FUNC1_CHIPCLKCSR, 0, NULL);
+	brcmf_sdio_regwb(bus->sdiodev, SBSDIO_FUNC1_CHIPCLKCSR, 0, NULL);
 
 	/* ...and initialize clock/power states */
 	bus->clkstate = CLK_SDONLY;
@@ -3866,7 +3837,7 @@ static void brcmf_sdbrcm_release(struct brcmf_sdio *bus)
 
 	if (bus) {
 		/* De-register interrupt handler */
-		brcmf_sdcard_intr_dereg(bus->sdiodev);
+		brcmf_sdio_intr_unregister(bus->sdiodev);
 
 		if (bus->sdiodev->bus_if->drvr) {
 			brcmf_detach(bus->sdiodev->dev);
@@ -3967,15 +3938,6 @@ void *brcmf_sdbrcm_probe(u32 regsva, struct brcmf_sdio_dev *sdiodev)
 		goto fail;
 	}
 
-	/* Register interrupt callback, but mask it (not operational yet). */
-	brcmf_dbg(INTR, "disable SDIO interrupts (not interested yet)\n");
-	ret = brcmf_sdcard_intr_reg(bus->sdiodev);
-	if (ret != 0) {
-		brcmf_dbg(ERROR, "FAILED: sdcard_intr_reg returned %d\n", ret);
-		goto fail;
-	}
-	brcmf_dbg(INTR, "registered SDIO interrupt function ok\n");
-
 	brcmf_dbg(INFO, "completed!!\n");
 
 	/* if firmware path present try to download and bring up bus */
@@ -3987,12 +3949,6 @@ void *brcmf_sdbrcm_probe(u32 regsva, struct brcmf_sdio_dev *sdiodev)
 		}
 	}
 
-	/* add interface and open for business */
-	if (brcmf_add_if(bus->sdiodev->dev, 0, "wlan%d", NULL)) {
-		brcmf_dbg(ERROR, "Add primary net device interface failed!!\n");
-		goto fail;
-	}
-
 	return bus;
 
 fail:
diff --git a/drivers/net/wireless/brcm80211/brcmfmac/sdio_chip.c b/drivers/net/wireless/brcm80211/brcmfmac/sdio_chip.c
index 1534efc2163..f8e1f1c84d0 100644
--- a/drivers/net/wireless/brcm80211/brcmfmac/sdio_chip.c
+++ b/drivers/net/wireless/brcm80211/brcmfmac/sdio_chip.c
@@ -93,8 +93,9 @@ brcmf_sdio_sb_corerev(struct brcmf_sdio_dev *sdiodev,
 
 	idx = brcmf_sdio_chip_getinfidx(ci, coreid);
 
-	regdata = brcmf_sdcard_reg_read(sdiodev,
-			CORE_SB(ci->c_inf[idx].base, sbidhigh), 4);
+	regdata = brcmf_sdio_regrl(sdiodev,
+				   CORE_SB(ci->c_inf[idx].base, sbidhigh),
+				   NULL);
 	return SBCOREREV(regdata);
 }
 
@@ -118,8 +119,9 @@ brcmf_sdio_sb_iscoreup(struct brcmf_sdio_dev *sdiodev,
 
 	idx = brcmf_sdio_chip_getinfidx(ci, coreid);
 
-	regdata = brcmf_sdcard_reg_read(sdiodev,
-			CORE_SB(ci->c_inf[idx].base, sbtmstatelow), 4);
+	regdata = brcmf_sdio_regrl(sdiodev,
+				   CORE_SB(ci->c_inf[idx].base, sbtmstatelow),
+				   NULL);
 	regdata &= (SSB_TMSLOW_RESET | SSB_TMSLOW_REJECT |
 		    SSB_IMSTATE_REJECT | SSB_TMSLOW_CLOCK);
 	return (SSB_TMSLOW_CLOCK == regdata);
@@ -135,13 +137,13 @@ brcmf_sdio_ai_iscoreup(struct brcmf_sdio_dev *sdiodev,
 
 	idx = brcmf_sdio_chip_getinfidx(ci, coreid);
 
-	regdata = brcmf_sdcard_reg_read(sdiodev,
-					ci->c_inf[idx].wrapbase+BCMA_IOCTL, 4);
+	regdata = brcmf_sdio_regrl(sdiodev, ci->c_inf[idx].wrapbase+BCMA_IOCTL,
+				   NULL);
 	ret = (regdata & (BCMA_IOCTL_FGC | BCMA_IOCTL_CLK)) == BCMA_IOCTL_CLK;
 
-	regdata = brcmf_sdcard_reg_read(sdiodev,
-					ci->c_inf[idx].wrapbase+BCMA_RESET_CTL,
-					4);
+	regdata = brcmf_sdio_regrl(sdiodev,
+				   ci->c_inf[idx].wrapbase+BCMA_RESET_CTL,
+				   NULL);
 	ret = ret && ((regdata & BCMA_RESET_CTL_RESET) == 0);
 
 	return ret;
@@ -151,84 +153,85 @@ static void
 brcmf_sdio_sb_coredisable(struct brcmf_sdio_dev *sdiodev,
 			  struct chip_info *ci, u16 coreid)
 {
-	u32 regdata;
+	u32 regdata, base;
 	u8 idx;
 
 	idx = brcmf_sdio_chip_getinfidx(ci, coreid);
+	base = ci->c_inf[idx].base;
 
-	regdata = brcmf_sdcard_reg_read(sdiodev,
-		CORE_SB(ci->c_inf[idx].base, sbtmstatelow), 4);
+	regdata = brcmf_sdio_regrl(sdiodev, CORE_SB(base, sbtmstatelow), NULL);
 	if (regdata & SSB_TMSLOW_RESET)
 		return;
 
-	regdata = brcmf_sdcard_reg_read(sdiodev,
-		CORE_SB(ci->c_inf[idx].base, sbtmstatelow), 4);
+	regdata = brcmf_sdio_regrl(sdiodev, CORE_SB(base, sbtmstatelow), NULL);
 	if ((regdata & SSB_TMSLOW_CLOCK) != 0) {
 		/*
 		 * set target reject and spin until busy is clear
 		 * (preserve core-specific bits)
 		 */
-		regdata = brcmf_sdcard_reg_read(sdiodev,
-			CORE_SB(ci->c_inf[idx].base, sbtmstatelow), 4);
-		brcmf_sdcard_reg_write(sdiodev,
-				CORE_SB(ci->c_inf[idx].base, sbtmstatelow),
-				4, regdata | SSB_TMSLOW_REJECT);
-
-		regdata = brcmf_sdcard_reg_read(sdiodev,
-			CORE_SB(ci->c_inf[idx].base, sbtmstatelow), 4);
+		regdata = brcmf_sdio_regrl(sdiodev, CORE_SB(base, sbtmstatelow),
+					   NULL);
+		brcmf_sdio_regwl(sdiodev, CORE_SB(base, sbtmstatelow),
+				 regdata | SSB_TMSLOW_REJECT, NULL);
+
+		regdata = brcmf_sdio_regrl(sdiodev, CORE_SB(base, sbtmstatelow),
+					   NULL);
 		udelay(1);
-		SPINWAIT((brcmf_sdcard_reg_read(sdiodev,
-			CORE_SB(ci->c_inf[idx].base, sbtmstatehigh), 4) &
+		SPINWAIT((brcmf_sdio_regrl(sdiodev,
+					   CORE_SB(base, sbtmstatehigh),
+					   NULL) &
 			SSB_TMSHIGH_BUSY), 100000);
 
-		regdata = brcmf_sdcard_reg_read(sdiodev,
-			CORE_SB(ci->c_inf[idx].base, sbtmstatehigh), 4);
+		regdata = brcmf_sdio_regrl(sdiodev,
+					   CORE_SB(base, sbtmstatehigh),
+					   NULL);
 		if (regdata & SSB_TMSHIGH_BUSY)
 			brcmf_dbg(ERROR, "core state still busy\n");
 
-		regdata = brcmf_sdcard_reg_read(sdiodev,
-			CORE_SB(ci->c_inf[idx].base, sbidlow), 4);
+		regdata = brcmf_sdio_regrl(sdiodev, CORE_SB(base, sbidlow),
+					   NULL);
 		if (regdata & SSB_IDLOW_INITIATOR) {
-			regdata = brcmf_sdcard_reg_read(sdiodev,
-				CORE_SB(ci->c_inf[idx].base, sbimstate), 4) |
-				SSB_IMSTATE_REJECT;
-			brcmf_sdcard_reg_write(sdiodev,
-				CORE_SB(ci->c_inf[idx].base, sbimstate), 4,
-				regdata);
-			regdata = brcmf_sdcard_reg_read(sdiodev,
-				CORE_SB(ci->c_inf[idx].base, sbimstate), 4);
+			regdata = brcmf_sdio_regrl(sdiodev,
+						   CORE_SB(base, sbimstate),
+						   NULL);
+			regdata |= SSB_IMSTATE_REJECT;
+			brcmf_sdio_regwl(sdiodev, CORE_SB(base, sbimstate),
+					 regdata, NULL);
+			regdata = brcmf_sdio_regrl(sdiodev,
+						   CORE_SB(base, sbimstate),
+						   NULL);
 			udelay(1);
-			SPINWAIT((brcmf_sdcard_reg_read(sdiodev,
-				CORE_SB(ci->c_inf[idx].base, sbimstate), 4) &
+			SPINWAIT((brcmf_sdio_regrl(sdiodev,
+						   CORE_SB(base, sbimstate),
+						   NULL) &
 				SSB_IMSTATE_BUSY), 100000);
 		}
 
 		/* set reset and reject while enabling the clocks */
-		brcmf_sdcard_reg_write(sdiodev,
-			CORE_SB(ci->c_inf[idx].base, sbtmstatelow), 4,
-			(SSB_TMSLOW_FGC | SSB_TMSLOW_CLOCK |
-			SSB_TMSLOW_REJECT | SSB_TMSLOW_RESET));
-		regdata = brcmf_sdcard_reg_read(sdiodev,
-			CORE_SB(ci->c_inf[idx].base, sbtmstatelow), 4);
+		regdata = SSB_TMSLOW_FGC | SSB_TMSLOW_CLOCK |
+			  SSB_TMSLOW_REJECT | SSB_TMSLOW_RESET;
+		brcmf_sdio_regwl(sdiodev, CORE_SB(base, sbtmstatelow),
+				 regdata, NULL);
+		regdata = brcmf_sdio_regrl(sdiodev, CORE_SB(base, sbtmstatelow),
+					   NULL);
 		udelay(10);
 
 		/* clear the initiator reject bit */
-		regdata = brcmf_sdcard_reg_read(sdiodev,
-			CORE_SB(ci->c_inf[idx].base, sbidlow), 4);
+		regdata = brcmf_sdio_regrl(sdiodev, CORE_SB(base, sbidlow),
+					   NULL);
 		if (regdata & SSB_IDLOW_INITIATOR) {
-			regdata = brcmf_sdcard_reg_read(sdiodev,
-				CORE_SB(ci->c_inf[idx].base, sbimstate), 4) &
-				~SSB_IMSTATE_REJECT;
-			brcmf_sdcard_reg_write(sdiodev,
-				CORE_SB(ci->c_inf[idx].base, sbimstate), 4,
-				regdata);
+			regdata = brcmf_sdio_regrl(sdiodev,
+						   CORE_SB(base, sbimstate),
+						   NULL);
+			regdata &= ~SSB_IMSTATE_REJECT;
+			brcmf_sdio_regwl(sdiodev, CORE_SB(base, sbimstate),
+					 regdata, NULL);
 		}
 	}
 
 	/* leave reset and reject asserted */
-	brcmf_sdcard_reg_write(sdiodev,
-		CORE_SB(ci->c_inf[idx].base, sbtmstatelow), 4,
-		(SSB_TMSLOW_REJECT | SSB_TMSLOW_RESET));
+	brcmf_sdio_regwl(sdiodev, CORE_SB(base, sbtmstatelow),
+			 (SSB_TMSLOW_REJECT | SSB_TMSLOW_RESET), NULL);
 	udelay(1);
 }
 
@@ -242,20 +245,19 @@ brcmf_sdio_ai_coredisable(struct brcmf_sdio_dev *sdiodev,
 	idx = brcmf_sdio_chip_getinfidx(ci, coreid);
 
 	/* if core is already in reset, just return */
-	regdata = brcmf_sdcard_reg_read(sdiodev,
-					ci->c_inf[idx].wrapbase+BCMA_RESET_CTL,
-					4);
+	regdata = brcmf_sdio_regrl(sdiodev,
+				   ci->c_inf[idx].wrapbase+BCMA_RESET_CTL,
+				   NULL);
 	if ((regdata & BCMA_RESET_CTL_RESET) != 0)
 		return;
 
-	brcmf_sdcard_reg_write(sdiodev, ci->c_inf[idx].wrapbase+BCMA_IOCTL,
-			       4, 0);
-	regdata = brcmf_sdcard_reg_read(sdiodev,
-					ci->c_inf[idx].wrapbase+BCMA_IOCTL, 4);
+	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,
+				   NULL);
 	udelay(10);
 
-	brcmf_sdcard_reg_write(sdiodev, ci->c_inf[idx].wrapbase+BCMA_RESET_CTL,
-			       4, BCMA_RESET_CTL_RESET);
+	brcmf_sdio_regwl(sdiodev, ci->c_inf[idx].wrapbase+BCMA_RESET_CTL,
+			 BCMA_RESET_CTL_RESET, NULL);
 	udelay(1);
 }
 
@@ -279,41 +281,47 @@ brcmf_sdio_sb_resetcore(struct brcmf_sdio_dev *sdiodev,
 	 * set reset while enabling the clock and
 	 * forcing them on throughout the core
 	 */
-	brcmf_sdcard_reg_write(sdiodev,
-			CORE_SB(ci->c_inf[idx].base, sbtmstatelow), 4,
-			SSB_TMSLOW_FGC | SSB_TMSLOW_CLOCK | SSB_TMSLOW_RESET);
-	regdata = brcmf_sdcard_reg_read(sdiodev,
-				CORE_SB(ci->c_inf[idx].base, sbtmstatelow), 4);
+	brcmf_sdio_regwl(sdiodev,
+			 CORE_SB(ci->c_inf[idx].base, sbtmstatelow),
+			 SSB_TMSLOW_FGC | SSB_TMSLOW_CLOCK | SSB_TMSLOW_RESET,
+			 NULL);
+	regdata = brcmf_sdio_regrl(sdiodev,
+				   CORE_SB(ci->c_inf[idx].base, sbtmstatelow),
+				   NULL);
 	udelay(1);
 
 	/* clear any serror */
-	regdata = brcmf_sdcard_reg_read(sdiodev,
-				CORE_SB(ci->c_inf[idx].base, sbtmstatehigh), 4);
+	regdata = brcmf_sdio_regrl(sdiodev,
+				   CORE_SB(ci->c_inf[idx].base, sbtmstatehigh),
+				   NULL);
 	if (regdata & SSB_TMSHIGH_SERR)
-		brcmf_sdcard_reg_write(sdiodev,
-			CORE_SB(ci->c_inf[idx].base, sbtmstatehigh), 4, 0);
+		brcmf_sdio_regwl(sdiodev,
+				 CORE_SB(ci->c_inf[idx].base, sbtmstatehigh),
+				 0, NULL);
 
-	regdata = brcmf_sdcard_reg_read(sdiodev,
-				CORE_SB(ci->c_inf[idx].base, sbimstate), 4);
+	regdata = brcmf_sdio_regrl(sdiodev,
+				   CORE_SB(ci->c_inf[idx].base, sbimstate),
+				   NULL);
 	if (regdata & (SSB_IMSTATE_IBE | SSB_IMSTATE_TO))
-		brcmf_sdcard_reg_write(sdiodev,
-			CORE_SB(ci->c_inf[idx].base, sbimstate), 4,
-			regdata & ~(SSB_IMSTATE_IBE | SSB_IMSTATE_TO));
+		brcmf_sdio_regwl(sdiodev,
+				 CORE_SB(ci->c_inf[idx].base, sbimstate),
+				 regdata & ~(SSB_IMSTATE_IBE | SSB_IMSTATE_TO),
+				 NULL);
 
 	/* clear reset and allow it to propagate throughout the core */
-	brcmf_sdcard_reg_write(sdiodev,
-		CORE_SB(ci->c_inf[idx].base, sbtmstatelow), 4,
-		SSB_TMSLOW_FGC | SSB_TMSLOW_CLOCK);
-	regdata = brcmf_sdcard_reg_read(sdiodev,
-				CORE_SB(ci->c_inf[idx].base, sbtmstatelow), 4);
+	brcmf_sdio_regwl(sdiodev, CORE_SB(ci->c_inf[idx].base, sbtmstatelow),
+			 SSB_TMSLOW_FGC | SSB_TMSLOW_CLOCK, NULL);
+	regdata = brcmf_sdio_regrl(sdiodev,
+				   CORE_SB(ci->c_inf[idx].base, sbtmstatelow),
+				   NULL);
 	udelay(1);
 
 	/* leave clock enabled */
-	brcmf_sdcard_reg_write(sdiodev,
-			       CORE_SB(ci->c_inf[idx].base, sbtmstatelow),
-			       4, SSB_TMSLOW_CLOCK);
-	regdata = brcmf_sdcard_reg_read(sdiodev,
-				CORE_SB(ci->c_inf[idx].base, sbtmstatelow), 4);
+	brcmf_sdio_regwl(sdiodev, CORE_SB(ci->c_inf[idx].base, sbtmstatelow),
+			 SSB_TMSLOW_CLOCK, NULL);
+	regdata = brcmf_sdio_regrl(sdiodev,
+				   CORE_SB(ci->c_inf[idx].base, sbtmstatelow),
+				   NULL);
 	udelay(1);
 }
 
@@ -330,18 +338,18 @@ brcmf_sdio_ai_resetcore(struct brcmf_sdio_dev *sdiodev,
 	brcmf_sdio_ai_coredisable(sdiodev, ci, coreid);
 
 	/* now do initialization sequence */
-	brcmf_sdcard_reg_write(sdiodev, ci->c_inf[idx].wrapbase+BCMA_IOCTL,
-			       4, BCMA_IOCTL_FGC | BCMA_IOCTL_CLK);
-	regdata = brcmf_sdcard_reg_read(sdiodev,
-					ci->c_inf[idx].wrapbase+BCMA_IOCTL, 4);
-	brcmf_sdcard_reg_write(sdiodev, ci->c_inf[idx].wrapbase+BCMA_RESET_CTL,
-			       4, 0);
+	brcmf_sdio_regwl(sdiodev, ci->c_inf[idx].wrapbase+BCMA_IOCTL,
+			 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);
 	udelay(1);
 
-	brcmf_sdcard_reg_write(sdiodev, ci->c_inf[idx].wrapbase+BCMA_IOCTL,
-			       4, BCMA_IOCTL_CLK);
-	regdata = brcmf_sdcard_reg_read(sdiodev,
-					ci->c_inf[idx].wrapbase+BCMA_IOCTL, 4);
+	brcmf_sdio_regwl(sdiodev, ci->c_inf[idx].wrapbase+BCMA_IOCTL,
+			 BCMA_IOCTL_CLK, NULL);
+	regdata = brcmf_sdio_regrl(sdiodev, ci->c_inf[idx].wrapbase+BCMA_IOCTL,
+				   NULL);
 	udelay(1);
 }
 
@@ -358,8 +366,9 @@ static int brcmf_sdio_chip_recognition(struct brcmf_sdio_dev *sdiodev,
 	 */
 	ci->c_inf[0].id = BCMA_CORE_CHIPCOMMON;
 	ci->c_inf[0].base = regs;
-	regdata = brcmf_sdcard_reg_read(sdiodev,
-			CORE_CC_REG(ci->c_inf[0].base, chipid), 4);
+	regdata = brcmf_sdio_regrl(sdiodev,
+				   CORE_CC_REG(ci->c_inf[0].base, chipid),
+				   NULL);
 	ci->chip = regdata & CID_ID_MASK;
 	ci->chiprev = (regdata & CID_REV_MASK) >> CID_REV_SHIFT;
 	ci->socitype = (regdata & CID_TYPE_MASK) >> CID_TYPE_SHIFT;
@@ -428,8 +437,7 @@ brcmf_sdio_chip_buscoreprep(struct brcmf_sdio_dev *sdiodev)
 
 	/* Try forcing SDIO core to do ALPAvail request only */
 	clkset = SBSDIO_FORCE_HW_CLKREQ_OFF | SBSDIO_ALP_AVAIL_REQ;
-	brcmf_sdcard_cfg_write(sdiodev, SDIO_FUNC_1,
-			       SBSDIO_FUNC1_CHIPCLKCSR,	clkset, &err);
+	brcmf_sdio_regwb(sdiodev, SBSDIO_FUNC1_CHIPCLKCSR, clkset, &err);
 	if (err) {
 		brcmf_dbg(ERROR, "error writing for HT off\n");
 		return err;
@@ -437,8 +445,8 @@ brcmf_sdio_chip_buscoreprep(struct brcmf_sdio_dev *sdiodev)
 
 	/* If register supported, wait for ALPAvail and then force ALP */
 	/* This may take up to 15 milliseconds */
-	clkval = brcmf_sdcard_cfg_read(sdiodev, SDIO_FUNC_1,
-				       SBSDIO_FUNC1_CHIPCLKCSR, NULL);
+	clkval = brcmf_sdio_regrb(sdiodev,
+				  SBSDIO_FUNC1_CHIPCLKCSR, NULL);
 
 	if ((clkval & ~SBSDIO_AVBITS) != clkset) {
 		brcmf_dbg(ERROR, "ChipClkCSR access: wrote 0x%02x read 0x%02x\n",
@@ -446,8 +454,8 @@ brcmf_sdio_chip_buscoreprep(struct brcmf_sdio_dev *sdiodev)
 		return -EACCES;
 	}
 
-	SPINWAIT(((clkval = brcmf_sdcard_cfg_read(sdiodev, SDIO_FUNC_1,
-				SBSDIO_FUNC1_CHIPCLKCSR, NULL)),
+	SPINWAIT(((clkval = brcmf_sdio_regrb(sdiodev,
+					     SBSDIO_FUNC1_CHIPCLKCSR, NULL)),
 			!SBSDIO_ALPAV(clkval)),
 			PMU_MAX_TRANSITION_DLY);
 	if (!SBSDIO_ALPAV(clkval)) {
@@ -457,13 +465,11 @@ brcmf_sdio_chip_buscoreprep(struct brcmf_sdio_dev *sdiodev)
 	}
 
 	clkset = SBSDIO_FORCE_HW_CLKREQ_OFF | SBSDIO_FORCE_ALP;
-	brcmf_sdcard_cfg_write(sdiodev, SDIO_FUNC_1,
-			       SBSDIO_FUNC1_CHIPCLKCSR, clkset, &err);
+	brcmf_sdio_regwb(sdiodev, SBSDIO_FUNC1_CHIPCLKCSR, clkset, &err);
 	udelay(65);
 
 	/* Also, disable the extra SDIO pull-ups */
-	brcmf_sdcard_cfg_write(sdiodev, SDIO_FUNC_1,
-			       SBSDIO_FUNC1_SDIOPULLUP, 0, NULL);
+	brcmf_sdio_regwb(sdiodev, SBSDIO_FUNC1_SDIOPULLUP, 0, NULL);
 
 	return 0;
 }
@@ -472,18 +478,22 @@ static void
 brcmf_sdio_chip_buscoresetup(struct brcmf_sdio_dev *sdiodev,
 			     struct chip_info *ci)
 {
+	u32 base = ci->c_inf[0].base;
+
 	/* get chipcommon rev */
 	ci->c_inf[0].rev = ci->corerev(sdiodev, ci, ci->c_inf[0].id);
 
 	/* get chipcommon capabilites */
-	ci->c_inf[0].caps =
-		brcmf_sdcard_reg_read(sdiodev,
-		CORE_CC_REG(ci->c_inf[0].base, capabilities), 4);
+	ci->c_inf[0].caps = brcmf_sdio_regrl(sdiodev,
+					     CORE_CC_REG(base, capabilities),
+					     NULL);
 
 	/* get pmu caps & rev */
 	if (ci->c_inf[0].caps & CC_CAP_PMU) {
-		ci->pmucaps = brcmf_sdcard_reg_read(sdiodev,
-			CORE_CC_REG(ci->c_inf[0].base, pmucapabilities), 4);
+		ci->pmucaps =
+			brcmf_sdio_regrl(sdiodev,
+					 CORE_CC_REG(base, pmucapabilities),
+					 NULL);
 		ci->pmurev = ci->pmucaps & PCAP_REV_MASK;
 	}
 
@@ -523,10 +533,10 @@ int brcmf_sdio_chip_attach(struct brcmf_sdio_dev *sdiodev,
 
 	brcmf_sdio_chip_buscoresetup(sdiodev, ci);
 
-	brcmf_sdcard_reg_write(sdiodev,
-		CORE_CC_REG(ci->c_inf[0].base, gpiopullup), 4, 0);
-	brcmf_sdcard_reg_write(sdiodev,
-		CORE_CC_REG(ci->c_inf[0].base, gpiopulldown), 4, 0);
+	brcmf_sdio_regwl(sdiodev, CORE_CC_REG(ci->c_inf[0].base, gpiopullup),
+			 0, NULL);
+	brcmf_sdio_regwl(sdiodev, CORE_CC_REG(ci->c_inf[0].base, gpiopulldown),
+			 0, NULL);
 
 	*ci_ptr = ci;
 	return 0;
@@ -562,6 +572,7 @@ brcmf_sdio_chip_drivestrengthinit(struct brcmf_sdio_dev *sdiodev,
 	u32 str_mask = 0;
 	u32 str_shift = 0;
 	char chn[8];
+	u32 base = ci->c_inf[0].base;
 
 	if (!(ci->c_inf[0].caps & CC_CAP_PMU))
 		return;
@@ -591,17 +602,17 @@ brcmf_sdio_chip_drivestrengthinit(struct brcmf_sdio_dev *sdiodev,
 			}
 		}
 
-		brcmf_sdcard_reg_write(sdiodev,
-			CORE_CC_REG(ci->c_inf[0].base, chipcontrol_addr),
-			4, 1);
-		cc_data_temp = brcmf_sdcard_reg_read(sdiodev,
-			CORE_CC_REG(ci->c_inf[0].base, chipcontrol_addr), 4);
+		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);
 		cc_data_temp &= ~str_mask;
 		drivestrength_sel <<= str_shift;
 		cc_data_temp |= drivestrength_sel;
-		brcmf_sdcard_reg_write(sdiodev,
-			CORE_CC_REG(ci->c_inf[0].base, chipcontrol_addr),
-			4, cc_data_temp);
+		brcmf_sdio_regwl(sdiodev, CORE_CC_REG(base, chipcontrol_addr),
+				 cc_data_temp, NULL);
 
 		brcmf_dbg(INFO, "SDIO: %dmA drive strength selected, set to 0x%08x\n",
 			  drivestrength, cc_data_temp);
diff --git a/drivers/net/wireless/brcm80211/brcmfmac/sdio_host.h b/drivers/net/wireless/brcm80211/brcmfmac/sdio_host.h
index 0281d207d99..29bf78d264e 100644
--- a/drivers/net/wireless/brcm80211/brcmfmac/sdio_host.h
+++ b/drivers/net/wireless/brcm80211/brcmfmac/sdio_host.h
@@ -40,9 +40,20 @@
 /* Maximum number of I/O funcs */
 #define SDIOD_MAX_IOFUNCS	7
 
+/* mask of register map */
+#define REG_F0_REG_MASK		0x7FF
+#define REG_F1_MISC_MASK	0x1FFFF
+
 /* as of sdiod rev 0, supports 3 functions */
 #define SBSDIO_NUM_FUNCTION		3
 
+/* function 0 vendor specific CCCR registers */
+#define SDIO_CCCR_BRCM_SEPINT		0xf2
+
+#define  SDIO_SEPINT_MASK		0x01
+#define  SDIO_SEPINT_OE			0x02
+#define  SDIO_SEPINT_ACT_HI		0x04
+
 /* function 1 miscellaneous registers */
 
 /* sprom command and status */
@@ -135,7 +146,6 @@ struct brcmf_sdio_dev {
 	u8 num_funcs;			/* Supported funcs on client */
 	u32 func_cis_ptr[SDIOD_MAX_IOFUNCS];
 	u32 sbwad;			/* Save backplane window address */
-	bool regfail;			/* status of last reg_r/w call */
 	void *bus;
 	atomic_t suspend;		/* suspend flag */
 	wait_queue_head_t request_byte_wait;
@@ -144,39 +154,26 @@ 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 */
+	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 device interrupt handler. */
-extern int
-brcmf_sdcard_intr_reg(struct brcmf_sdio_dev *sdiodev);
-
-extern int brcmf_sdcard_intr_dereg(struct brcmf_sdio_dev *sdiodev);
-
-/* Access SDIO address space (e.g. CCCR) using CMD52 (single-byte interface).
- *   fn:   function number
- *   addr: unmodified SDIO-space address
- *   data: data byte to write
- *   err:  pointer to error code (or NULL)
- */
-extern u8 brcmf_sdcard_cfg_read(struct brcmf_sdio_dev *sdiodev, uint func,
-				u32 addr, int *err);
-extern void brcmf_sdcard_cfg_write(struct brcmf_sdio_dev *sdiodev, uint func,
-				   u32 addr, u8 data, int *err);
-
-/* Synchronous access to device (client) core registers via CMD53 to F1.
- *   addr: backplane address (i.e. >= regsva from attach)
- *   size: register width in bytes (2 or 4)
- *   data: data for register write
- */
-extern u32
-brcmf_sdcard_reg_read(struct brcmf_sdio_dev *sdiodev, u32 addr, uint size);
-
-extern u32
-brcmf_sdcard_reg_write(struct brcmf_sdio_dev *sdiodev, u32 addr, uint size,
-		       u32 data);
-
-/* Indicate if last reg read/write failed */
-extern bool brcmf_sdcard_regfail(struct brcmf_sdio_dev *sdiodev);
+/* Register/deregister interrupt handler. */
+extern int brcmf_sdio_intr_register(struct brcmf_sdio_dev *sdiodev);
+extern int brcmf_sdio_intr_unregister(struct brcmf_sdio_dev *sdiodev);
+
+/* sdio device register access interface */
+extern u8 brcmf_sdio_regrb(struct brcmf_sdio_dev *sdiodev, u32 addr, int *ret);
+extern u32 brcmf_sdio_regrl(struct brcmf_sdio_dev *sdiodev, u32 addr, int *ret);
+extern void brcmf_sdio_regwb(struct brcmf_sdio_dev *sdiodev, u32 addr,
+			     u8 data, int *ret);
+extern void brcmf_sdio_regwl(struct brcmf_sdio_dev *sdiodev, u32 addr,
+			     u32 data, int *ret);
 
 /* Buffer transfer to/from device (client) core via cmd53.
  *   fn:       function number
diff --git a/drivers/net/wireless/brcm80211/brcmfmac/usb.c b/drivers/net/wireless/brcm80211/brcmfmac/usb.c
index 82364223e81..c5a34ffe645 100644
--- a/drivers/net/wireless/brcm80211/brcmfmac/usb.c
+++ b/drivers/net/wireless/brcm80211/brcmfmac/usb.c
@@ -1383,14 +1383,6 @@ static int brcmf_usb_probe_cb(struct device *dev, const char *desc,
 		goto fail;
 	}
 
-	/* add interface and open for business */
-	ret = brcmf_add_if(dev, 0, "wlan%d", NULL);
-	if (ret) {
-		brcmf_dbg(ERROR, "Add primary net device interface failed!!\n");
-		brcmf_detach(dev);
-		goto fail;
-	}
-
 	return 0;
 fail:
 	/* Release resources in reverse order */
@@ -1604,7 +1596,8 @@ static struct usb_driver brcmf_usbdrvr = {
 	.id_table = brcmf_usb_devid_table,
 	.suspend = brcmf_usb_suspend,
 	.resume = brcmf_usb_resume,
-	.supports_autosuspend = 1
+	.supports_autosuspend = 1,
+	.disable_hub_initiated_lpm = 1,
 };
 
 void brcmf_usb_exit(void)
diff --git a/drivers/net/wireless/brcm80211/brcmsmac/Makefile b/drivers/net/wireless/brcm80211/brcmsmac/Makefile
index c2eb2d0af38..e227c4c68ef 100644
--- a/drivers/net/wireless/brcm80211/brcmsmac/Makefile
+++ b/drivers/net/wireless/brcm80211/brcmsmac/Makefile
@@ -39,10 +39,7 @@ BRCMSMAC_OFILES := \
 	phy/phytbl_lcn.o \
 	phy/phytbl_n.o \
 	phy/phy_qmath.o \
-	otp.o \
-	srom.o \
 	dma.o \
-	nicpci.o \
 	brcms_trace_events.o
 
 MODULEPFX := brcmsmac
diff --git a/drivers/net/wireless/brcm80211/brcmsmac/aiutils.c b/drivers/net/wireless/brcm80211/brcmsmac/aiutils.c
index c93ea35bcee..6d8b7213643 100644
--- a/drivers/net/wireless/brcm80211/brcmsmac/aiutils.c
+++ b/drivers/net/wireless/brcm80211/brcmsmac/aiutils.c
@@ -19,7 +19,6 @@
 #define pr_fmt(fmt) KBUILD_MODNAME ": " fmt
 
 #include <linux/delay.h>
-#include <linux/pci.h>
 
 #include <defs.h>
 #include <chipcommon.h>
@@ -29,8 +28,6 @@
 #include "types.h"
 #include "pub.h"
 #include "pmu.h"
-#include "srom.h"
-#include "nicpci.h"
 #include "aiutils.h"
 
 /* slow_clk_ctl */
@@ -321,7 +318,6 @@
 #define	IS_SIM(chippkg)	\
 	((chippkg == HDLSIM_PKG_ID) || (chippkg == HWSIM_PKG_ID))
 
-#define PCI(sih)	(ai_get_buscoretype(sih) == PCI_CORE_ID)
 #define PCIE(sih)	(ai_get_buscoretype(sih) == PCIE_CORE_ID)
 
 #define PCI_FORCEHT(sih) (PCIE(sih) && (ai_get_chip_id(sih) == BCM4716_CHIP_ID))
@@ -454,36 +450,9 @@ struct aidmp {
 	u32 componentid3;	/* 0xffc */
 };
 
-/* return true if PCIE capability exists in the pci config space */
-static bool ai_ispcie(struct si_info *sii)
-{
-	u8 cap_ptr;
-
-	cap_ptr =
-	    pcicore_find_pci_capability(sii->pcibus, PCI_CAP_ID_EXP, NULL,
-					NULL);
-	if (!cap_ptr)
-		return false;
-
-	return true;
-}
-
-static bool ai_buscore_prep(struct si_info *sii)
-{
-	/* kludge to enable the clock on the 4306 which lacks a slowclock */
-	if (!ai_ispcie(sii))
-		ai_clkctl_xtal(&sii->pub, XTAL | PLL, ON);
-	return true;
-}
-
 static bool
 ai_buscore_setup(struct si_info *sii, struct bcma_device *cc)
 {
-	struct bcma_device *pci = NULL;
-	struct bcma_device *pcie = NULL;
-	struct bcma_device *core;
-
-
 	/* no cores found, bail out */
 	if (cc->bus->nr_cores == 0)
 		return false;
@@ -492,8 +461,7 @@ ai_buscore_setup(struct si_info *sii, struct bcma_device *cc)
 	sii->pub.ccrev = cc->id.rev;
 
 	/* get chipcommon chipstatus */
-	if (ai_get_ccrev(&sii->pub) >= 11)
-		sii->chipst = bcma_read32(cc, CHIPCREGOFFS(chipstatus));
+	sii->chipst = bcma_read32(cc, CHIPCREGOFFS(chipstatus));
 
 	/* get chipcommon capabilites */
 	sii->pub.cccaps = bcma_read32(cc, CHIPCREGOFFS(capabilities));
@@ -506,64 +474,18 @@ ai_buscore_setup(struct si_info *sii, struct bcma_device *cc)
 	}
 
 	/* figure out buscore */
-	list_for_each_entry(core, &cc->bus->cores, list) {
-		uint cid, crev;
-
-		cid = core->id.id;
-		crev = core->id.rev;
-
-		if (cid == PCI_CORE_ID) {
-			pci = core;
-		} else if (cid == PCIE_CORE_ID) {
-			pcie = core;
-		}
-	}
-
-	if (pci && pcie) {
-		if (ai_ispcie(sii))
-			pci = NULL;
-		else
-			pcie = NULL;
-	}
-	if (pci) {
-		sii->buscore = pci;
-	} else if (pcie) {
-		sii->buscore = pcie;
-	}
-
-	/* fixup necessary chip/core configurations */
-	if (!sii->pch) {
-		sii->pch = pcicore_init(&sii->pub, sii->icbus->drv_pci.core);
-		if (sii->pch == NULL)
-			return false;
-	}
-	if (ai_pci_fixcfg(&sii->pub))
-		return false;
+	sii->buscore = ai_findcore(&sii->pub, PCIE_CORE_ID, 0);
 
 	return true;
 }
 
-/*
- * get boardtype and boardrev
- */
-static __used void ai_nvram_process(struct si_info *sii)
-{
-	uint w = 0;
-
-	/* do a pci config read to get subsystem id and subvendor id */
-	pci_read_config_dword(sii->pcibus, PCI_SUBSYSTEM_VENDOR_ID, &w);
-
-	sii->pub.boardvendor = w & 0xffff;
-	sii->pub.boardtype = (w >> 16) & 0xffff;
-}
-
 static struct si_info *ai_doattach(struct si_info *sii,
 				   struct bcma_bus *pbus)
 {
 	struct si_pub *sih = &sii->pub;
 	u32 w, savewin;
 	struct bcma_device *cc;
-	uint socitype;
+	struct ssb_sprom *sprom = &pbus->sprom;
 
 	savewin = 0;
 
@@ -573,38 +495,15 @@ static struct si_info *ai_doattach(struct si_info *sii,
 	/* switch to Chipcommon core */
 	cc = pbus->drv_cc.core;
 
-	/* bus/core/clk setup for register access */
-	if (!ai_buscore_prep(sii))
-		return NULL;
+	sih->chip = pbus->chipinfo.id;
+	sih->chiprev = pbus->chipinfo.rev;
+	sih->chippkg = pbus->chipinfo.pkg;
+	sih->boardvendor = pbus->boardinfo.vendor;
+	sih->boardtype = pbus->boardinfo.type;
 
-	/*
-	 * ChipID recognition.
-	 *   We assume we can read chipid at offset 0 from the regs arg.
-	 *   If we add other chiptypes (or if we need to support old sdio
-	 *   hosts w/o chipcommon), some way of recognizing them needs to
-	 *   be added here.
-	 */
-	w = bcma_read32(cc, CHIPCREGOFFS(chipid));
-	socitype = (w & CID_TYPE_MASK) >> CID_TYPE_SHIFT;
-	/* Might as wll fill in chip id rev & pkg */
-	sih->chip = w & CID_ID_MASK;
-	sih->chiprev = (w & CID_REV_MASK) >> CID_REV_SHIFT;
-	sih->chippkg = (w & CID_PKG_MASK) >> CID_PKG_SHIFT;
-
-	/* scan for cores */
-	if (socitype != SOCI_AI)
-		return NULL;
-
-	SI_MSG("Found chip type AI (0x%08x)\n", w);
 	if (!ai_buscore_setup(sii, cc))
 		goto exit;
 
-	/* Init nvram from sprom/otp if they exist */
-	if (srom_var_init(&sii->pub))
-		goto exit;
-
-	ai_nvram_process(sii);
-
 	/* === NVRAM, clock is ready === */
 	bcma_write32(cc, CHIPCREGOFFS(gpiopullup), 0);
 	bcma_write32(cc, CHIPCREGOFFS(gpiopulldown), 0);
@@ -617,15 +516,13 @@ static struct si_info *ai_doattach(struct si_info *sii,
 	}
 
 	/* setup the GPIO based LED powersave register */
-	w = getintvar(sih, BRCMS_SROM_LEDDC);
+	w = (sprom->leddc_on_time << BCMA_CC_GPIOTIMER_ONTIME_SHIFT) |
+		 (sprom->leddc_off_time << BCMA_CC_GPIOTIMER_OFFTIME_SHIFT);
 	if (w == 0)
 		w = DEFAULT_GPIOTIMERVAL;
 	ai_cc_reg(sih, offsetof(struct chipcregs, gpiotimerval),
 		  ~0, w);
 
-	if (PCIE(sih))
-		pcicore_attach(sii->pch, SI_DOATTACH);
-
 	if (ai_get_chip_id(sih) == BCM43224_CHIP_ID) {
 		/*
 		 * enable 12 mA drive strenth for 43224 and
@@ -659,9 +556,6 @@ static struct si_info *ai_doattach(struct si_info *sii,
 	return sii;
 
  exit:
-	if (sii->pch)
-		pcicore_deinit(sii->pch);
-	sii->pch = NULL;
 
 	return NULL;
 }
@@ -700,11 +594,6 @@ void ai_detach(struct si_pub *sih)
 	if (sii == NULL)
 		return;
 
-	if (sii->pch)
-		pcicore_deinit(sii->pch);
-	sii->pch = NULL;
-
-	srom_free_vars(sih);
 	kfree(sii);
 }
 
@@ -755,21 +644,7 @@ uint ai_cc_reg(struct si_pub *sih, uint regoff, u32 mask, u32 val)
 /* return the slow clock source - LPO, XTAL, or PCI */
 static uint ai_slowclk_src(struct si_pub *sih, struct bcma_device *cc)
 {
-	struct si_info *sii;
-	u32 val;
-
-	sii = (struct si_info *)sih;
-	if (ai_get_ccrev(&sii->pub) < 6) {
-		pci_read_config_dword(sii->pcibus, PCI_GPIO_OUT,
-				      &val);
-		if (val & PCI_CFG_GPIO_SCS)
-			return SCC_SS_PCI;
-		return SCC_SS_XTAL;
-	} else if (ai_get_ccrev(&sii->pub) < 10) {
-		return bcma_read32(cc, CHIPCREGOFFS(slow_clk_ctl)) &
-		       SCC_SS_MASK;
-	} else			/* Insta-clock */
-		return SCC_SS_XTAL;
+	return SCC_SS_XTAL;
 }
 
 /*
@@ -779,36 +654,12 @@ static uint ai_slowclk_src(struct si_pub *sih, struct bcma_device *cc)
 static uint ai_slowclk_freq(struct si_pub *sih, bool max_freq,
 			    struct bcma_device *cc)
 {
-	u32 slowclk;
 	uint div;
 
-	slowclk = ai_slowclk_src(sih, cc);
-	if (ai_get_ccrev(sih) < 6) {
-		if (slowclk == SCC_SS_PCI)
-			return max_freq ? (PCIMAXFREQ / 64)
-				: (PCIMINFREQ / 64);
-		else
-			return max_freq ? (XTALMAXFREQ / 32)
-				: (XTALMINFREQ / 32);
-	} else if (ai_get_ccrev(sih) < 10) {
-		div = 4 *
-		    (((bcma_read32(cc, CHIPCREGOFFS(slow_clk_ctl)) &
-		      SCC_CD_MASK) >> SCC_CD_SHIFT) + 1);
-		if (slowclk == SCC_SS_LPO)
-			return max_freq ? LPOMAXFREQ : LPOMINFREQ;
-		else if (slowclk == SCC_SS_XTAL)
-			return max_freq ? (XTALMAXFREQ / div)
-				: (XTALMINFREQ / div);
-		else if (slowclk == SCC_SS_PCI)
-			return max_freq ? (PCIMAXFREQ / div)
-				: (PCIMINFREQ / div);
-	} else {
-		/* Chipc rev 10 is InstaClock */
-		div = bcma_read32(cc, CHIPCREGOFFS(system_clk_ctl));
-		div = 4 * ((div >> SYCC_CD_SHIFT) + 1);
-		return max_freq ? XTALMAXFREQ : (XTALMINFREQ / div);
-	}
-	return 0;
+	/* Chipc rev 10 is InstaClock */
+	div = bcma_read32(cc, CHIPCREGOFFS(system_clk_ctl));
+	div = 4 * ((div >> SYCC_CD_SHIFT) + 1);
+	return max_freq ? XTALMAXFREQ : (XTALMINFREQ / div);
 }
 
 static void
@@ -831,8 +682,7 @@ ai_clkctl_setdelay(struct si_pub *sih, struct bcma_device *cc)
 
 	/* Starting with 4318 it is ILP that is used for the delays */
 	slowmaxfreq =
-	    ai_slowclk_freq(sih,
-			    (ai_get_ccrev(sih) >= 10) ? false : true, cc);
+	    ai_slowclk_freq(sih, false, cc);
 
 	pll_on_delay = ((slowmaxfreq * pll_delay) + 999999) / 1000000;
 	fref_sel_delay = ((slowmaxfreq * FREF_DELAY) + 999999) / 1000000;
@@ -854,9 +704,8 @@ void ai_clkctl_init(struct si_pub *sih)
 		return;
 
 	/* set all Instaclk chip ILP to 1 MHz */
-	if (ai_get_ccrev(sih) >= 10)
-		bcma_maskset32(cc, CHIPCREGOFFS(system_clk_ctl), SYCC_CD_MASK,
-			       (ILP_DIV_1MHZ << SYCC_CD_SHIFT));
+	bcma_maskset32(cc, CHIPCREGOFFS(system_clk_ctl), SYCC_CD_MASK,
+		       (ILP_DIV_1MHZ << SYCC_CD_SHIFT));
 
 	ai_clkctl_setdelay(sih, cc);
 }
@@ -891,140 +740,6 @@ u16 ai_clkctl_fast_pwrup_delay(struct si_pub *sih)
 	return fpdelay;
 }
 
-/* turn primary xtal and/or pll off/on */
-int ai_clkctl_xtal(struct si_pub *sih, uint what, bool on)
-{
-	struct si_info *sii;
-	u32 in, out, outen;
-
-	sii = (struct si_info *)sih;
-
-	/* pcie core doesn't have any mapping to control the xtal pu */
-	if (PCIE(sih))
-		return -1;
-
-	pci_read_config_dword(sii->pcibus, PCI_GPIO_IN, &in);
-	pci_read_config_dword(sii->pcibus, PCI_GPIO_OUT, &out);
-	pci_read_config_dword(sii->pcibus, PCI_GPIO_OUTEN, &outen);
-
-	/*
-	 * Avoid glitching the clock if GPRS is already using it.
-	 * We can't actually read the state of the PLLPD so we infer it
-	 * by the value of XTAL_PU which *is* readable via gpioin.
-	 */
-	if (on && (in & PCI_CFG_GPIO_XTAL))
-		return 0;
-
-	if (what & XTAL)
-		outen |= PCI_CFG_GPIO_XTAL;
-	if (what & PLL)
-		outen |= PCI_CFG_GPIO_PLL;
-
-	if (on) {
-		/* turn primary xtal on */
-		if (what & XTAL) {
-			out |= PCI_CFG_GPIO_XTAL;
-			if (what & PLL)
-				out |= PCI_CFG_GPIO_PLL;
-			pci_write_config_dword(sii->pcibus,
-					       PCI_GPIO_OUT, out);
-			pci_write_config_dword(sii->pcibus,
-					       PCI_GPIO_OUTEN, outen);
-			udelay(XTAL_ON_DELAY);
-		}
-
-		/* turn pll on */
-		if (what & PLL) {
-			out &= ~PCI_CFG_GPIO_PLL;
-			pci_write_config_dword(sii->pcibus,
-					       PCI_GPIO_OUT, out);
-			mdelay(2);
-		}
-	} else {
-		if (what & XTAL)
-			out &= ~PCI_CFG_GPIO_XTAL;
-		if (what & PLL)
-			out |= PCI_CFG_GPIO_PLL;
-		pci_write_config_dword(sii->pcibus,
-				       PCI_GPIO_OUT, out);
-		pci_write_config_dword(sii->pcibus,
-				       PCI_GPIO_OUTEN, outen);
-	}
-
-	return 0;
-}
-
-/* clk control mechanism through chipcommon, no policy checking */
-static bool _ai_clkctl_cc(struct si_info *sii, uint mode)
-{
-	struct bcma_device *cc;
-	u32 scc;
-
-	/* chipcommon cores prior to rev6 don't support dynamic clock control */
-	if (ai_get_ccrev(&sii->pub) < 6)
-		return false;
-
-	cc = ai_findcore(&sii->pub, BCMA_CORE_CHIPCOMMON, 0);
-
-	if (!(ai_get_cccaps(&sii->pub) & CC_CAP_PWR_CTL) &&
-	    (ai_get_ccrev(&sii->pub) < 20))
-		return mode == CLK_FAST;
-
-	switch (mode) {
-	case CLK_FAST:		/* FORCEHT, fast (pll) clock */
-		if (ai_get_ccrev(&sii->pub) < 10) {
-			/*
-			 * don't forget to force xtal back
-			 * on before we clear SCC_DYN_XTAL..
-			 */
-			ai_clkctl_xtal(&sii->pub, XTAL, ON);
-			bcma_maskset32(cc, CHIPCREGOFFS(slow_clk_ctl),
-				       (SCC_XC | SCC_FS | SCC_IP), SCC_IP);
-		} else if (ai_get_ccrev(&sii->pub) < 20) {
-			bcma_set32(cc, CHIPCREGOFFS(system_clk_ctl), SYCC_HR);
-		} else {
-			bcma_set32(cc, CHIPCREGOFFS(clk_ctl_st), CCS_FORCEHT);
-		}
-
-		/* wait for the PLL */
-		if (ai_get_cccaps(&sii->pub) & CC_CAP_PMU) {
-			u32 htavail = CCS_HTAVAIL;
-			SPINWAIT(((bcma_read32(cc, CHIPCREGOFFS(clk_ctl_st)) &
-				   htavail) == 0), PMU_MAX_TRANSITION_DLY);
-		} else {
-			udelay(PLL_DELAY);
-		}
-		break;
-
-	case CLK_DYNAMIC:	/* enable dynamic clock control */
-		if (ai_get_ccrev(&sii->pub) < 10) {
-			scc = bcma_read32(cc, CHIPCREGOFFS(slow_clk_ctl));
-			scc &= ~(SCC_FS | SCC_IP | SCC_XC);
-			if ((scc & SCC_SS_MASK) != SCC_SS_XTAL)
-				scc |= SCC_XC;
-			bcma_write32(cc, CHIPCREGOFFS(slow_clk_ctl), scc);
-
-			/*
-			 * for dynamic control, we have to
-			 * release our xtal_pu "force on"
-			 */
-			if (scc & SCC_XC)
-				ai_clkctl_xtal(&sii->pub, XTAL, OFF);
-		} else if (ai_get_ccrev(&sii->pub) < 20) {
-			/* Instaclock */
-			bcma_mask32(cc, CHIPCREGOFFS(system_clk_ctl), ~SYCC_HR);
-		} else {
-			bcma_mask32(cc, CHIPCREGOFFS(clk_ctl_st), ~CCS_FORCEHT);
-		}
-		break;
-
-	default:
-		break;
-	}
-
-	return mode == CLK_FAST;
-}
-
 /*
  *  clock control policy function throught chipcommon
  *
@@ -1033,133 +748,53 @@ static bool _ai_clkctl_cc(struct si_info *sii, uint mode)
  *    this is a wrapper over the next internal function
  *      to allow flexible policy settings for outside caller
  */
-bool ai_clkctl_cc(struct si_pub *sih, uint mode)
+bool ai_clkctl_cc(struct si_pub *sih, enum bcma_clkmode mode)
 {
 	struct si_info *sii;
+	struct bcma_device *cc;
 
 	sii = (struct si_info *)sih;
 
-	/* chipcommon cores prior to rev6 don't support dynamic clock control */
-	if (ai_get_ccrev(sih) < 6)
-		return false;
-
 	if (PCI_FORCEHT(sih))
-		return mode == CLK_FAST;
+		return mode == BCMA_CLKMODE_FAST;
 
-	return _ai_clkctl_cc(sii, mode);
+	cc = ai_findcore(&sii->pub, BCMA_CORE_CHIPCOMMON, 0);
+	bcma_core_set_clockmode(cc, mode);
+	return mode == BCMA_CLKMODE_FAST;
 }
 
 void ai_pci_up(struct si_pub *sih)
 {
 	struct si_info *sii;
+	struct bcma_device *cc;
 
 	sii = (struct si_info *)sih;
 
-	if (PCI_FORCEHT(sih))
-		_ai_clkctl_cc(sii, CLK_FAST);
+	if (PCI_FORCEHT(sih)) {
+		cc = ai_findcore(&sii->pub, BCMA_CORE_CHIPCOMMON, 0);
+		bcma_core_set_clockmode(cc, BCMA_CLKMODE_FAST);
+	}
 
 	if (PCIE(sih))
-		pcicore_up(sii->pch, SI_PCIUP);
-
-}
-
-/* Unconfigure and/or apply various WARs when system is going to sleep mode */
-void ai_pci_sleep(struct si_pub *sih)
-{
-	struct si_info *sii;
-
-	sii = (struct si_info *)sih;
-
-	pcicore_sleep(sii->pch);
+		bcma_core_pci_extend_L1timer(&sii->icbus->drv_pci, true);
 }
 
 /* Unconfigure and/or apply various WARs when going down */
 void ai_pci_down(struct si_pub *sih)
 {
 	struct si_info *sii;
+	struct bcma_device *cc;
 
 	sii = (struct si_info *)sih;
 
 	/* release FORCEHT since chip is going to "down" state */
-	if (PCI_FORCEHT(sih))
-		_ai_clkctl_cc(sii, CLK_DYNAMIC);
-
-	pcicore_down(sii->pch, SI_PCIDOWN);
-}
-
-/*
- * Configure the pci core for pci client (NIC) action
- * coremask is the bitvec of cores by index to be enabled.
- */
-void ai_pci_setup(struct si_pub *sih, uint coremask)
-{
-	struct si_info *sii;
-	u32 w;
-
-	sii = (struct si_info *)sih;
-
-	/*
-	 * Enable sb->pci interrupts.  Assume
-	 * PCI rev 2.3 support was added in pci core rev 6 and things changed..
-	 */
-	if (PCIE(sih) || (PCI(sih) && (ai_get_buscorerev(sih) >= 6))) {
-		/* pci config write to set this core bit in PCIIntMask */
-		pci_read_config_dword(sii->pcibus, PCI_INT_MASK, &w);
-		w |= (coremask << PCI_SBIM_SHIFT);
-		pci_write_config_dword(sii->pcibus, PCI_INT_MASK, w);
-	}
-
-	if (PCI(sih)) {
-		pcicore_pci_setup(sii->pch);
+	if (PCI_FORCEHT(sih)) {
+		cc = ai_findcore(&sii->pub, BCMA_CORE_CHIPCOMMON, 0);
+		bcma_core_set_clockmode(cc, BCMA_CLKMODE_DYNAMIC);
 	}
-}
 
-/*
- * Fixup SROMless PCI device's configuration.
- * The current core may be changed upon return.
- */
-int ai_pci_fixcfg(struct si_pub *sih)
-{
-	struct si_info *sii = (struct si_info *)sih;
-
-	/* Fixup PI in SROM shadow area to enable the correct PCI core access */
-	/* check 'pi' is correct and fix it if not */
-	pcicore_fixcfg(sii->pch);
-	pcicore_hwup(sii->pch);
-	return 0;
-}
-
-/* mask&set gpiocontrol bits */
-u32 ai_gpiocontrol(struct si_pub *sih, u32 mask, u32 val, u8 priority)
-{
-	uint regoff;
-
-	regoff = offsetof(struct chipcregs, gpiocontrol);
-	return ai_cc_reg(sih, regoff, mask, val);
-}
-
-void ai_chipcontrl_epa4331(struct si_pub *sih, bool on)
-{
-	struct bcma_device *cc;
-	u32 val;
-
-	cc = ai_findcore(sih, CC_CORE_ID, 0);
-
-	if (on) {
-		if (ai_get_chippkg(sih) == 9 || ai_get_chippkg(sih) == 0xb)
-			/* Ext PA Controls for 4331 12x9 Package */
-			bcma_set32(cc, CHIPCREGOFFS(chipcontrol),
-				   CCTRL4331_EXTPA_EN |
-				   CCTRL4331_EXTPA_ON_GPIO2_5);
-		else
-			/* Ext PA Controls for 4331 12x12 Package */
-			bcma_set32(cc, CHIPCREGOFFS(chipcontrol),
-				   CCTRL4331_EXTPA_EN);
-	} else {
-		val &= ~(CCTRL4331_EXTPA_EN | CCTRL4331_EXTPA_ON_GPIO2_5);
-		bcma_mask32(cc, CHIPCREGOFFS(chipcontrol),
-			    ~(CCTRL4331_EXTPA_EN | CCTRL4331_EXTPA_ON_GPIO2_5));
-	}
+	if (PCIE(sih))
+		bcma_core_pci_extend_L1timer(&sii->icbus->drv_pci, false);
 }
 
 /* Enable BT-COEX & Ex-PA for 4313 */
@@ -1181,6 +816,9 @@ bool ai_deviceremoved(struct si_pub *sih)
 
 	sii = (struct si_info *)sih;
 
+	if (sii->icbus->hosttype != BCMA_HOSTTYPE_PCI)
+		return false;
+
 	pci_read_config_dword(sii->pcibus, PCI_VENDOR_ID, &w);
 	if ((w & 0xFFFF) != PCI_VENDOR_ID_BROADCOM)
 		return true;
@@ -1188,45 +826,6 @@ bool ai_deviceremoved(struct si_pub *sih)
 	return false;
 }
 
-bool ai_is_sprom_available(struct si_pub *sih)
-{
-	struct si_info *sii = (struct si_info *)sih;
-
-	if (ai_get_ccrev(sih) >= 31) {
-		struct bcma_device *cc;
-		u32 sromctrl;
-
-		if ((ai_get_cccaps(sih) & CC_CAP_SROM) == 0)
-			return false;
-
-		cc = ai_findcore(sih, BCMA_CORE_CHIPCOMMON, 0);
-		sromctrl = bcma_read32(cc, CHIPCREGOFFS(sromcontrol));
-		return sromctrl & SRC_PRESENT;
-	}
-
-	switch (ai_get_chip_id(sih)) {
-	case BCM4313_CHIP_ID:
-		return (sii->chipst & CST4313_SPROM_PRESENT) != 0;
-	default:
-		return true;
-	}
-}
-
-bool ai_is_otp_disabled(struct si_pub *sih)
-{
-	struct si_info *sii = (struct si_info *)sih;
-
-	switch (ai_get_chip_id(sih)) {
-	case BCM4313_CHIP_ID:
-		return (sii->chipst & CST4313_OTP_PRESENT) == 0;
-		/* These chips always have their OTP on */
-	case BCM43224_CHIP_ID:
-	case BCM43225_CHIP_ID:
-	default:
-		return false;
-	}
-}
-
 uint ai_get_buscoretype(struct si_pub *sih)
 {
 	struct si_info *sii = (struct si_info *)sih;
diff --git a/drivers/net/wireless/brcm80211/brcmsmac/aiutils.h b/drivers/net/wireless/brcm80211/brcmsmac/aiutils.h
index f84c6f78169..d9f04a683bd 100644
--- a/drivers/net/wireless/brcm80211/brcmsmac/aiutils.h
+++ b/drivers/net/wireless/brcm80211/brcmsmac/aiutils.h
@@ -113,10 +113,6 @@
 #define	XTAL			0x1	/* primary crystal oscillator (2050) */
 #define	PLL			0x2	/* main chip pll */
 
-/* clkctl clk mode */
-#define	CLK_FAST		0	/* force fast (pll) clock */
-#define	CLK_DYNAMIC		2	/* enable dynamic clock control */
-
 /* GPIO usage priorities */
 #define GPIO_DRV_PRIORITY	0	/* Driver */
 #define GPIO_APP_PRIORITY	1	/* Application */
@@ -172,9 +168,7 @@ struct si_info {
 	struct si_pub pub;	/* back plane public state (must be first) */
 	struct bcma_bus *icbus;	/* handle to soc interconnect bus */
 	struct pci_dev *pcibus;	/* handle to pci bus */
-	struct pcicore_info *pch; /* PCI/E core handle */
 	struct bcma_device *buscore;
-	struct list_head var_list; /* list of srom variables */
 
 	u32 chipst;		/* chip status */
 };
@@ -197,38 +191,20 @@ extern u32 ai_core_cflags(struct bcma_device *core, u32 mask, u32 val);
 extern struct si_pub *ai_attach(struct bcma_bus *pbus);
 extern void ai_detach(struct si_pub *sih);
 extern uint ai_cc_reg(struct si_pub *sih, uint regoff, u32 mask, u32 val);
-extern void ai_pci_setup(struct si_pub *sih, uint coremask);
 extern void ai_clkctl_init(struct si_pub *sih);
 extern u16 ai_clkctl_fast_pwrup_delay(struct si_pub *sih);
 extern bool ai_clkctl_cc(struct si_pub *sih, uint mode);
-extern int ai_clkctl_xtal(struct si_pub *sih, uint what, bool on);
 extern bool ai_deviceremoved(struct si_pub *sih);
-extern u32 ai_gpiocontrol(struct si_pub *sih, u32 mask, u32 val,
-			     u8 priority);
-
-/* OTP status */
-extern bool ai_is_otp_disabled(struct si_pub *sih);
-
-/* SPROM availability */
-extern bool ai_is_sprom_available(struct si_pub *sih);
 
-extern void ai_pci_sleep(struct si_pub *sih);
 extern void ai_pci_down(struct si_pub *sih);
 extern void ai_pci_up(struct si_pub *sih);
-extern int ai_pci_fixcfg(struct si_pub *sih);
 
-extern void ai_chipcontrl_epa4331(struct si_pub *sih, bool on);
 /* Enable Ex-PA for 4313 */
 extern void ai_epa_4313war(struct si_pub *sih);
 
 extern uint ai_get_buscoretype(struct si_pub *sih);
 extern uint ai_get_buscorerev(struct si_pub *sih);
 
-static inline int ai_get_ccrev(struct si_pub *sih)
-{
-	return sih->ccrev;
-}
-
 static inline u32 ai_get_cccaps(struct si_pub *sih)
 {
 	return sih->cccaps;
diff --git a/drivers/net/wireless/brcm80211/brcmsmac/antsel.c b/drivers/net/wireless/brcm80211/brcmsmac/antsel.c
index a47ce25cb9a..55e12c32791 100644
--- a/drivers/net/wireless/brcm80211/brcmsmac/antsel.c
+++ b/drivers/net/wireless/brcm80211/brcmsmac/antsel.c
@@ -108,7 +108,7 @@ brcms_c_antsel_init_cfg(struct antsel_info *asi, struct brcms_antselcfg *antsel,
 struct antsel_info *brcms_c_antsel_attach(struct brcms_c_info *wlc)
 {
 	struct antsel_info *asi;
-	struct si_pub *sih = wlc->hw->sih;
+	struct ssb_sprom *sprom = &wlc->hw->d11core->bus->sprom;
 
 	asi = kzalloc(sizeof(struct antsel_info), GFP_ATOMIC);
 	if (!asi)
@@ -118,7 +118,7 @@ struct antsel_info *brcms_c_antsel_attach(struct brcms_c_info *wlc)
 	asi->pub = wlc->pub;
 	asi->antsel_type = ANTSEL_NA;
 	asi->antsel_avail = false;
-	asi->antsel_antswitch = (u8) getintvar(sih, BRCMS_SROM_ANTSWITCH);
+	asi->antsel_antswitch = sprom->antswitch;
 
 	if ((asi->pub->sromrev >= 4) && (asi->antsel_antswitch != 0)) {
 		switch (asi->antsel_antswitch) {
@@ -128,12 +128,12 @@ struct antsel_info *brcms_c_antsel_attach(struct brcms_c_info *wlc)
 			/* 4321/2 board with 2x3 switch logic */
 			asi->antsel_type = ANTSEL_2x3;
 			/* Antenna selection availability */
-			if (((u16) getintvar(sih, BRCMS_SROM_AA2G) == 7) ||
-			    ((u16) getintvar(sih, BRCMS_SROM_AA5G) == 7)) {
+			if ((sprom->ant_available_bg == 7) ||
+			    (sprom->ant_available_a == 7)) {
 				asi->antsel_avail = true;
 			} else if (
-				(u16) getintvar(sih, BRCMS_SROM_AA2G) == 3 ||
-				(u16) getintvar(sih, BRCMS_SROM_AA5G) == 3) {
+				sprom->ant_available_bg == 3 ||
+				sprom->ant_available_a == 3) {
 				asi->antsel_avail = false;
 			} else {
 				asi->antsel_avail = false;
@@ -146,8 +146,8 @@ struct antsel_info *brcms_c_antsel_attach(struct brcms_c_info *wlc)
 			break;
 		}
 	} else if ((asi->pub->sromrev == 4) &&
-		   ((u16) getintvar(sih, BRCMS_SROM_AA2G) == 7) &&
-		   ((u16) getintvar(sih, BRCMS_SROM_AA5G) == 0)) {
+		   (sprom->ant_available_bg == 7) &&
+		   (sprom->ant_available_a == 0)) {
 		/* hack to match old 4321CB2 cards with 2of3 antenna switch */
 		asi->antsel_type = ANTSEL_2x3;
 		asi->antsel_avail = true;
diff --git a/drivers/net/wireless/brcm80211/brcmsmac/channel.c b/drivers/net/wireless/brcm80211/brcmsmac/channel.c
index 55e9f45fce2..eb77ac3cfb6 100644
--- a/drivers/net/wireless/brcm80211/brcmsmac/channel.c
+++ b/drivers/net/wireless/brcm80211/brcmsmac/channel.c
@@ -628,6 +628,40 @@ brcms_c_country_aggregate_map(struct brcms_cm_info *wlc_cm, const char *ccode,
 	return false;
 }
 
+/*
+ * Indicates whether the country provided is valid to pass
+ * to cfg80211 or not.
+ *
+ * returns true if valid; false if not.
+ */
+static bool brcms_c_country_valid(const char *ccode)
+{
+	/*
+	 * only allow ascii alpha uppercase for the first 2
+	 * chars.
+	 */
+	if (!((0x80 & ccode[0]) == 0 && ccode[0] >= 0x41 && ccode[0] <= 0x5A &&
+	      (0x80 & ccode[1]) == 0 && ccode[1] >= 0x41 && ccode[1] <= 0x5A &&
+	      ccode[2] == '\0'))
+		return false;
+
+	/*
+	 * do not match ISO 3166-1 user assigned country codes
+	 * that may be in the driver table
+	 */
+	if (!strcmp("AA", ccode) ||        /* AA */
+	    !strcmp("ZZ", ccode) ||        /* ZZ */
+	    ccode[0] == 'X' ||             /* XA - XZ */
+	    (ccode[0] == 'Q' &&            /* QM - QZ */
+	     (ccode[1] >= 'M' && ccode[1] <= 'Z')))
+		return false;
+
+	if (!strcmp("NA", ccode))
+		return false;
+
+	return true;
+}
+
 /* Lookup a country info structure from a null terminated country
  * abbreviation and regrev directly with no translation.
  */
@@ -1076,7 +1110,7 @@ struct brcms_cm_info *brcms_c_channel_mgr_attach(struct brcms_c_info *wlc)
 	char country_abbrev[BRCM_CNTRY_BUF_SZ];
 	const struct country_info *country;
 	struct brcms_pub *pub = wlc->pub;
-	char *ccode;
+	struct ssb_sprom *sprom = &wlc->hw->d11core->bus->sprom;
 
 	BCMMSG(wlc->wiphy, "wl%d\n", wlc->pub->unit);
 
@@ -1088,9 +1122,8 @@ struct brcms_cm_info *brcms_c_channel_mgr_attach(struct brcms_c_info *wlc)
 	wlc->cmi = wlc_cm;
 
 	/* store the country code for passing up as a regulatory hint */
-	ccode = getvar(wlc->hw->sih, BRCMS_SROM_CCODE);
-	if (ccode)
-		strncpy(wlc->pub->srom_ccode, ccode, BRCM_CNTRY_BUF_SZ - 1);
+	if (sprom->alpha2 && brcms_c_country_valid(sprom->alpha2))
+		strncpy(wlc->pub->srom_ccode, sprom->alpha2, sizeof(sprom->alpha2));
 
 	/*
 	 * internal country information which must match
diff --git a/drivers/net/wireless/brcm80211/brcmsmac/d11.h b/drivers/net/wireless/brcm80211/brcmsmac/d11.h
index 1948cb2771e..3f659e09f1c 100644
--- a/drivers/net/wireless/brcm80211/brcmsmac/d11.h
+++ b/drivers/net/wireless/brcm80211/brcmsmac/d11.h
@@ -733,7 +733,7 @@ struct cck_phy_hdr {
 	do { \
 		plcp[1] = len & 0xff; \
 		plcp[2] = ((len >> 8) & 0xff); \
-	} while (0);
+	} while (0)
 
 #define BRCMS_SET_MIMO_PLCP_AMPDU(plcp) (plcp[3] |= MIMO_PLCP_AMPDU)
 #define BRCMS_CLR_MIMO_PLCP_AMPDU(plcp) (plcp[3] &= ~MIMO_PLCP_AMPDU)
diff --git a/drivers/net/wireless/brcm80211/brcmsmac/mac80211_if.c b/drivers/net/wireless/brcm80211/brcmsmac/mac80211_if.c
index 569ab8abd2a..50f92a0b7c4 100644
--- a/drivers/net/wireless/brcm80211/brcmsmac/mac80211_if.c
+++ b/drivers/net/wireless/brcm80211/brcmsmac/mac80211_if.c
@@ -25,7 +25,6 @@
 #include <linux/bcma/bcma.h>
 #include <net/mac80211.h>
 #include <defs.h>
-#include "nicpci.h"
 #include "phy/phy_int.h"
 #include "d11.h"
 #include "channel.h"
@@ -770,7 +769,7 @@ void brcms_dpc(unsigned long data)
  * Precondition: Since this function is called in brcms_pci_probe() context,
  * no locking is required.
  */
-static int brcms_request_fw(struct brcms_info *wl, struct pci_dev *pdev)
+static int brcms_request_fw(struct brcms_info *wl, struct bcma_device *pdev)
 {
 	int status;
 	struct device *device = &pdev->dev;
@@ -1022,7 +1021,7 @@ static struct brcms_info *brcms_attach(struct bcma_device *pdev)
 	spin_lock_init(&wl->isr_lock);
 
 	/* prepare ucode */
-	if (brcms_request_fw(wl, pdev->bus->host_pci) < 0) {
+	if (brcms_request_fw(wl, pdev) < 0) {
 		wiphy_err(wl->wiphy, "%s: Failed to find firmware usually in "
 			  "%s\n", KBUILD_MODNAME, "/lib/firmware/brcm");
 		brcms_release_fw(wl);
@@ -1043,12 +1042,12 @@ static struct brcms_info *brcms_attach(struct bcma_device *pdev)
 	wl->pub->ieee_hw = hw;
 
 	/* register our interrupt handler */
-	if (request_irq(pdev->bus->host_pci->irq, brcms_isr,
+	if (request_irq(pdev->irq, brcms_isr,
 			IRQF_SHARED, KBUILD_MODNAME, wl)) {
 		wiphy_err(wl->wiphy, "wl%d: request_irq() failed\n", unit);
 		goto fail;
 	}
-	wl->irq = pdev->bus->host_pci->irq;
+	wl->irq = pdev->irq;
 
 	/* register module */
 	brcms_c_module_register(wl->pub, "linux", wl, NULL);
@@ -1069,11 +1068,7 @@ static struct brcms_info *brcms_attach(struct bcma_device *pdev)
 		wiphy_err(wl->wiphy, "%s: ieee80211_register_hw failed, status"
 			  "%d\n", __func__, err);
 
-	if (wl->pub->srom_ccode[0])
-		err = brcms_set_hint(wl, wl->pub->srom_ccode);
-	else
-		err = brcms_set_hint(wl, "US");
-	if (err)
+	if (wl->pub->srom_ccode[0] && brcms_set_hint(wl, wl->pub->srom_ccode))
 		wiphy_err(wl->wiphy, "%s: regulatory_hint failed, status %d\n",
 			  __func__, err);
 
@@ -1102,7 +1097,7 @@ static int __devinit brcms_bcma_probe(struct bcma_device *pdev)
 
 	dev_info(&pdev->dev, "mfg %x core %x rev %d class %d irq %d\n",
 		 pdev->id.manuf, pdev->id.id, pdev->id.rev, pdev->id.class,
-		 pdev->bus->host_pci->irq);
+		 pdev->irq);
 
 	if ((pdev->id.manuf != BCMA_MANUF_BCM) ||
 	    (pdev->id.id != BCMA_CORE_80211))
diff --git a/drivers/net/wireless/brcm80211/brcmsmac/main.c b/drivers/net/wireless/brcm80211/brcmsmac/main.c
index b4d92792c50..19db4052c44 100644
--- a/drivers/net/wireless/brcm80211/brcmsmac/main.c
+++ b/drivers/net/wireless/brcm80211/brcmsmac/main.c
@@ -1219,7 +1219,7 @@ static void brcms_b_wait_for_wake(struct brcms_hardware *wlc_hw)
 }
 
 /* control chip clock to save power, enable dynamic clock or force fast clock */
-static void brcms_b_clkctl_clk(struct brcms_hardware *wlc_hw, uint mode)
+static void brcms_b_clkctl_clk(struct brcms_hardware *wlc_hw, enum bcma_clkmode mode)
 {
 	if (ai_get_cccaps(wlc_hw->sih) & CC_CAP_PMU) {
 		/* new chips with PMU, CCS_FORCEHT will distribute the HT clock
@@ -1229,7 +1229,7 @@ static void brcms_b_clkctl_clk(struct brcms_hardware *wlc_hw, uint mode)
 		 */
 
 		if (wlc_hw->clk) {
-			if (mode == CLK_FAST) {
+			if (mode == BCMA_CLKMODE_FAST) {
 				bcma_set32(wlc_hw->d11core,
 					   D11REGOFFS(clk_ctl_st),
 					   CCS_FORCEHT);
@@ -1260,7 +1260,7 @@ static void brcms_b_clkctl_clk(struct brcms_hardware *wlc_hw, uint mode)
 					~CCS_FORCEHT);
 			}
 		}
-		wlc_hw->forcefastclk = (mode == CLK_FAST);
+		wlc_hw->forcefastclk = (mode == BCMA_CLKMODE_FAST);
 	} else {
 
 		/* old chips w/o PMU, force HT through cc,
@@ -1567,7 +1567,7 @@ void brcms_b_bw_set(struct brcms_hardware *wlc_hw, u16 bw)
 	/* request FAST clock if not on */
 	fastclk = wlc_hw->forcefastclk;
 	if (!fastclk)
-		brcms_b_clkctl_clk(wlc_hw, CLK_FAST);
+		brcms_b_clkctl_clk(wlc_hw, BCMA_CLKMODE_FAST);
 
 	wlc_phy_bw_state_set(wlc_hw->band->pi, bw);
 
@@ -1576,7 +1576,7 @@ void brcms_b_bw_set(struct brcms_hardware *wlc_hw, u16 bw)
 
 	/* restore the clk */
 	if (!fastclk)
-		brcms_b_clkctl_clk(wlc_hw, CLK_DYNAMIC);
+		brcms_b_clkctl_clk(wlc_hw, BCMA_CLKMODE_DYNAMIC);
 }
 
 static void brcms_b_upd_synthpu(struct brcms_hardware *wlc_hw)
@@ -1882,27 +1882,20 @@ static bool brcms_c_validboardtype(struct brcms_hardware *wlc_hw)
 	return true;
 }
 
-static char *brcms_c_get_macaddr(struct brcms_hardware *wlc_hw)
+static void brcms_c_get_macaddr(struct brcms_hardware *wlc_hw, u8 etheraddr[ETH_ALEN])
 {
-	enum brcms_srom_id var_id = BRCMS_SROM_MACADDR;
-	char *macaddr;
+	struct ssb_sprom *sprom = &wlc_hw->d11core->bus->sprom;
 
 	/* If macaddr exists, use it (Sromrev4, CIS, ...). */
-	macaddr = getvar(wlc_hw->sih, var_id);
-	if (macaddr != NULL)
-		return macaddr;
+	if (!is_zero_ether_addr(sprom->il0mac)) {
+		memcpy(etheraddr, sprom->il0mac, 6);
+		return;
+	}
 
 	if (wlc_hw->_nbands > 1)
-		var_id = BRCMS_SROM_ET1MACADDR;
+		memcpy(etheraddr, sprom->et1mac, 6);
 	else
-		var_id = BRCMS_SROM_IL0MACADDR;
-
-	macaddr = getvar(wlc_hw->sih, var_id);
-	if (macaddr == NULL)
-		wiphy_err(wlc_hw->wlc->wiphy, "wl%d: wlc_get_macaddr: macaddr "
-			  "getvar(%d) not found\n", wlc_hw->unit, var_id);
-
-	return macaddr;
+		memcpy(etheraddr, sprom->il0mac, 6);
 }
 
 /* power both the pll and external oscillator on/off */
@@ -1917,9 +1910,6 @@ static void brcms_b_xtal(struct brcms_hardware *wlc_hw, bool want)
 	if (!want && wlc_hw->pllreq)
 		return;
 
-	if (wlc_hw->sih)
-		ai_clkctl_xtal(wlc_hw->sih, XTAL | PLL, want);
-
 	wlc_hw->sbclk = want;
 	if (!wlc_hw->sbclk) {
 		wlc_hw->clk = false;
@@ -2004,7 +1994,7 @@ void brcms_b_corereset(struct brcms_hardware *wlc_hw, u32 flags)
 	/* request FAST clock if not on  */
 	fastclk = wlc_hw->forcefastclk;
 	if (!fastclk)
-		brcms_b_clkctl_clk(wlc_hw, CLK_FAST);
+		brcms_b_clkctl_clk(wlc_hw, BCMA_CLKMODE_FAST);
 
 	/* reset the dma engines except first time thru */
 	if (bcma_core_is_enabled(wlc_hw->d11core)) {
@@ -2053,7 +2043,7 @@ void brcms_b_corereset(struct brcms_hardware *wlc_hw, u32 flags)
 	brcms_c_mctrl_reset(wlc_hw);
 
 	if (ai_get_cccaps(wlc_hw->sih) & CC_CAP_PMU)
-		brcms_b_clkctl_clk(wlc_hw, CLK_FAST);
+		brcms_b_clkctl_clk(wlc_hw, BCMA_CLKMODE_FAST);
 
 	brcms_b_phy_reset(wlc_hw);
 
@@ -2065,7 +2055,7 @@ void brcms_b_corereset(struct brcms_hardware *wlc_hw, u32 flags)
 
 	/* restore the clk setting */
 	if (!fastclk)
-		brcms_b_clkctl_clk(wlc_hw, CLK_DYNAMIC);
+		brcms_b_clkctl_clk(wlc_hw, BCMA_CLKMODE_DYNAMIC);
 }
 
 /* txfifo sizes needs to be modified(increased) since the newer cores
@@ -2218,7 +2208,7 @@ static void brcms_c_gpio_init(struct brcms_c_info *wlc)
 		gm |= gc |= BOARD_GPIO_PACTRL;
 
 	/* apply to gpiocontrol register */
-	ai_gpiocontrol(wlc_hw->sih, gm, gc, GPIO_DRV_PRIORITY);
+	bcma_chipco_gpio_control(&wlc_hw->d11core->bus->drv_cc, gm, gc);
 }
 
 static void brcms_ucode_write(struct brcms_hardware *wlc_hw,
@@ -3371,7 +3361,7 @@ static brcms_b_init(struct brcms_hardware *wlc_hw, u16 chanspec) {
 	/* request FAST clock if not on */
 	fastclk = wlc_hw->forcefastclk;
 	if (!fastclk)
-		brcms_b_clkctl_clk(wlc_hw, CLK_FAST);
+		brcms_b_clkctl_clk(wlc_hw, BCMA_CLKMODE_FAST);
 
 	/* disable interrupts */
 	macintmask = brcms_intrsoff(wlc->wl);
@@ -3405,7 +3395,7 @@ static brcms_b_init(struct brcms_hardware *wlc_hw, u16 chanspec) {
 
 	/* restore the clk */
 	if (!fastclk)
-		brcms_b_clkctl_clk(wlc_hw, CLK_DYNAMIC);
+		brcms_b_clkctl_clk(wlc_hw, BCMA_CLKMODE_DYNAMIC);
 }
 
 static void brcms_c_set_phy_chanspec(struct brcms_c_info *wlc,
@@ -4436,17 +4426,22 @@ static int brcms_b_attach(struct brcms_c_info *wlc, struct bcma_device *core,
 			  uint unit, bool piomode)
 {
 	struct brcms_hardware *wlc_hw;
-	char *macaddr = NULL;
 	uint err = 0;
 	uint j;
 	bool wme = false;
 	struct shared_phy_params sha_params;
 	struct wiphy *wiphy = wlc->wiphy;
 	struct pci_dev *pcidev = core->bus->host_pci;
+	struct ssb_sprom *sprom = &core->bus->sprom;
 
-	BCMMSG(wlc->wiphy, "wl%d: vendor 0x%x device 0x%x\n", unit,
-	       pcidev->vendor,
-	       pcidev->device);
+	if (core->bus->hosttype == BCMA_HOSTTYPE_PCI)
+		BCMMSG(wlc->wiphy, "wl%d: vendor 0x%x device 0x%x\n", unit,
+		       pcidev->vendor,
+		       pcidev->device);
+	else
+		BCMMSG(wlc->wiphy, "wl%d: vendor 0x%x device 0x%x\n", unit,
+		       core->bus->boardinfo.vendor,
+		       core->bus->boardinfo.type);
 
 	wme = true;
 
@@ -4472,7 +4467,8 @@ static int brcms_b_attach(struct brcms_c_info *wlc, struct bcma_device *core,
 	}
 
 	/* verify again the device is supported */
-	if (!brcms_c_chipmatch(pcidev->vendor, pcidev->device)) {
+	if (core->bus->hosttype == BCMA_HOSTTYPE_PCI &&
+	    !brcms_c_chipmatch(pcidev->vendor, pcidev->device)) {
 		wiphy_err(wiphy, "wl%d: brcms_b_attach: Unsupported "
 			"vendor/device (0x%x/0x%x)\n",
 			 unit, pcidev->vendor, pcidev->device);
@@ -4480,8 +4476,13 @@ static int brcms_b_attach(struct brcms_c_info *wlc, struct bcma_device *core,
 		goto fail;
 	}
 
-	wlc_hw->vendorid = pcidev->vendor;
-	wlc_hw->deviceid = pcidev->device;
+	if (core->bus->hosttype == BCMA_HOSTTYPE_PCI) {
+		wlc_hw->vendorid = pcidev->vendor;
+		wlc_hw->deviceid = pcidev->device;
+	} else {
+		wlc_hw->vendorid = core->bus->boardinfo.vendor;
+		wlc_hw->deviceid = core->bus->boardinfo.type;
+	}
 
 	wlc_hw->d11core = core;
 	wlc_hw->corerev = core->id.rev;
@@ -4501,7 +4502,7 @@ static int brcms_b_attach(struct brcms_c_info *wlc, struct bcma_device *core,
 	 *   is still false; But it will be called again inside wlc_corereset,
 	 *   after d11 is out of reset.
 	 */
-	brcms_b_clkctl_clk(wlc_hw, CLK_FAST);
+	brcms_b_clkctl_clk(wlc_hw, BCMA_CLKMODE_FAST);
 	brcms_b_corereset(wlc_hw, BRCMS_USE_COREFLAGS);
 
 	if (!brcms_b_validate_chip_access(wlc_hw)) {
@@ -4512,7 +4513,7 @@ static int brcms_b_attach(struct brcms_c_info *wlc, struct bcma_device *core,
 	}
 
 	/* get the board rev, used just below */
-	j = getintvar(wlc_hw->sih, BRCMS_SROM_BOARDREV);
+	j = sprom->board_rev;
 	/* promote srom boardrev of 0xFF to 1 */
 	if (j == BOARDREV_PROMOTABLE)
 		j = BOARDREV_PROMOTED;
@@ -4525,11 +4526,9 @@ static int brcms_b_attach(struct brcms_c_info *wlc, struct bcma_device *core,
 		err = 15;
 		goto fail;
 	}
-	wlc_hw->sromrev = (u8) getintvar(wlc_hw->sih, BRCMS_SROM_REV);
-	wlc_hw->boardflags = (u32) getintvar(wlc_hw->sih,
-					     BRCMS_SROM_BOARDFLAGS);
-	wlc_hw->boardflags2 = (u32) getintvar(wlc_hw->sih,
-					      BRCMS_SROM_BOARDFLAGS2);
+	wlc_hw->sromrev = sprom->revision;
+	wlc_hw->boardflags = sprom->boardflags_lo + (sprom->boardflags_hi << 16);
+	wlc_hw->boardflags2 = sprom->boardflags2_lo + (sprom->boardflags2_hi << 16);
 
 	if (wlc_hw->boardflags & BFL_NOPLLDOWN)
 		brcms_b_pllreq(wlc_hw, true, BRCMS_PLLREQ_SHARED);
@@ -4702,25 +4701,18 @@ static int brcms_b_attach(struct brcms_c_info *wlc, struct bcma_device *core,
 	 */
 
 	/* init etheraddr state variables */
-	macaddr = brcms_c_get_macaddr(wlc_hw);
-	if (macaddr == NULL) {
-		wiphy_err(wiphy, "wl%d: brcms_b_attach: macaddr not found\n",
-			  unit);
-		err = 21;
-		goto fail;
-	}
-	if (!mac_pton(macaddr, wlc_hw->etheraddr) ||
-	    is_broadcast_ether_addr(wlc_hw->etheraddr) ||
+	brcms_c_get_macaddr(wlc_hw, wlc_hw->etheraddr);
+
+	if (is_broadcast_ether_addr(wlc_hw->etheraddr) ||
 	    is_zero_ether_addr(wlc_hw->etheraddr)) {
-		wiphy_err(wiphy, "wl%d: brcms_b_attach: bad macaddr %s\n",
-			  unit, macaddr);
+		wiphy_err(wiphy, "wl%d: brcms_b_attach: bad macaddr\n",
+			  unit);
 		err = 22;
 		goto fail;
 	}
 
-	BCMMSG(wlc->wiphy, "deviceid 0x%x nbands %d board 0x%x macaddr: %s\n",
-	       wlc_hw->deviceid, wlc_hw->_nbands, ai_get_boardtype(wlc_hw->sih),
-	       macaddr);
+	BCMMSG(wlc->wiphy, "deviceid 0x%x nbands %d board 0x%x\n",
+	       wlc_hw->deviceid, wlc_hw->_nbands, ai_get_boardtype(wlc_hw->sih));
 
 	return err;
 
@@ -4770,16 +4762,16 @@ static bool brcms_c_attach_stf_ant_init(struct brcms_c_info *wlc)
 	int aa;
 	uint unit;
 	int bandtype;
-	struct si_pub *sih = wlc->hw->sih;
+	struct ssb_sprom *sprom = &wlc->hw->d11core->bus->sprom;
 
 	unit = wlc->pub->unit;
 	bandtype = wlc->band->bandtype;
 
 	/* get antennas available */
 	if (bandtype == BRCM_BAND_5G)
-		aa = (s8) getintvar(sih, BRCMS_SROM_AA5G);
+		aa = sprom->ant_available_a;
 	else
-		aa = (s8) getintvar(sih, BRCMS_SROM_AA2G);
+		aa = sprom->ant_available_bg;
 
 	if ((aa < 1) || (aa > 15)) {
 		wiphy_err(wlc->wiphy, "wl%d: %s: Invalid antennas available in"
@@ -4799,9 +4791,9 @@ static bool brcms_c_attach_stf_ant_init(struct brcms_c_info *wlc)
 
 	/* Compute Antenna Gain */
 	if (bandtype == BRCM_BAND_5G)
-		wlc->band->antgain = (s8) getintvar(sih, BRCMS_SROM_AG1);
+		wlc->band->antgain = sprom->antenna_gain.a1;
 	else
-		wlc->band->antgain = (s8) getintvar(sih, BRCMS_SROM_AG0);
+		wlc->band->antgain = sprom->antenna_gain.a0;
 
 	brcms_c_attach_antgain_init(wlc);
 
@@ -4952,15 +4944,6 @@ static int brcms_b_detach(struct brcms_c_info *wlc)
 
 	callbacks = 0;
 
-	if (wlc_hw->sih) {
-		/*
-		 * detach interrupt sync mechanism since interrupt is disabled
-		 * and per-port interrupt object may has been freed. this must
-		 * be done before sb core switch
-		 */
-		ai_pci_sleep(wlc_hw->sih);
-	}
-
 	brcms_b_detach_dmapio(wlc_hw);
 
 	band = wlc_hw->band;
@@ -5047,9 +5030,7 @@ static void brcms_b_hw_up(struct brcms_hardware *wlc_hw)
 	 */
 	brcms_b_xtal(wlc_hw, ON);
 	ai_clkctl_init(wlc_hw->sih);
-	brcms_b_clkctl_clk(wlc_hw, CLK_FAST);
-
-	ai_pci_fixcfg(wlc_hw->sih);
+	brcms_b_clkctl_clk(wlc_hw, BCMA_CLKMODE_FAST);
 
 	/*
 	 * TODO: test suspend/resume
@@ -5078,8 +5059,6 @@ static void brcms_b_hw_up(struct brcms_hardware *wlc_hw)
 
 static int brcms_b_up_prep(struct brcms_hardware *wlc_hw)
 {
-	uint coremask;
-
 	BCMMSG(wlc_hw->wlc->wiphy, "wl%d\n", wlc_hw->unit);
 
 	/*
@@ -5088,15 +5067,14 @@ static int brcms_b_up_prep(struct brcms_hardware *wlc_hw)
 	 */
 	brcms_b_xtal(wlc_hw, ON);
 	ai_clkctl_init(wlc_hw->sih);
-	brcms_b_clkctl_clk(wlc_hw, CLK_FAST);
+	brcms_b_clkctl_clk(wlc_hw, BCMA_CLKMODE_FAST);
 
 	/*
 	 * Configure pci/pcmcia here instead of in brcms_c_attach()
 	 * to allow mfg hotswap:  down, hotswap (chip power cycle), up.
 	 */
-	coremask = (1 << wlc_hw->wlc->core->coreidx);
-
-	ai_pci_setup(wlc_hw->sih, coremask);
+	bcma_core_pci_irq_ctl(&wlc_hw->d11core->bus->drv_pci, wlc_hw->d11core,
+			      true);
 
 	/*
 	 * Need to read the hwradio status here to cover the case where the
@@ -5126,7 +5104,7 @@ static int brcms_b_up_finish(struct brcms_hardware *wlc_hw)
 	wlc_phy_hw_state_upd(wlc_hw->band->pi, true);
 
 	/* FULLY enable dynamic power control and d11 core interrupt */
-	brcms_b_clkctl_clk(wlc_hw, CLK_DYNAMIC);
+	brcms_b_clkctl_clk(wlc_hw, BCMA_CLKMODE_DYNAMIC);
 	brcms_intrson(wlc_hw->wlc->wl);
 	return 0;
 }
@@ -5267,7 +5245,7 @@ static int brcms_b_bmac_down_prep(struct brcms_hardware *wlc_hw)
 		brcms_intrsoff(wlc_hw->wlc->wl);
 
 		/* ensure we're running on the pll clock again */
-		brcms_b_clkctl_clk(wlc_hw, CLK_FAST);
+		brcms_b_clkctl_clk(wlc_hw, BCMA_CLKMODE_FAST);
 	}
 	/* down phy at the last of this stage */
 	callbacks += wlc_phy_down(wlc_hw->band->pi);
diff --git a/drivers/net/wireless/brcm80211/brcmsmac/nicpci.c b/drivers/net/wireless/brcm80211/brcmsmac/nicpci.c
deleted file mode 100644
index 7fad6dc1925..00000000000
--- a/drivers/net/wireless/brcm80211/brcmsmac/nicpci.c
+++ /dev/null
@@ -1,826 +0,0 @@
-/*
- * 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/slab.h>
-#include <linux/delay.h>
-#include <linux/pci.h>
-
-#include <defs.h>
-#include <soc.h>
-#include <chipcommon.h>
-#include "aiutils.h"
-#include "pub.h"
-#include "nicpci.h"
-
-/* SPROM offsets */
-#define SRSH_ASPM_OFFSET		4	/* word 4 */
-#define SRSH_ASPM_ENB			0x18	/* bit 3, 4 */
-#define SRSH_ASPM_L1_ENB		0x10	/* bit 4 */
-#define SRSH_ASPM_L0s_ENB		0x8	/* bit 3 */
-
-#define SRSH_PCIE_MISC_CONFIG		5	/* word 5 */
-#define SRSH_L23READY_EXIT_NOPERST	0x8000	/* bit 15 */
-#define SRSH_CLKREQ_OFFSET_REV5		20	/* word 20 for srom rev <= 5 */
-#define SRSH_CLKREQ_ENB			0x0800	/* bit 11 */
-#define SRSH_BD_OFFSET                  6	/* word 6 */
-
-/* chipcontrol */
-#define CHIPCTRL_4321_PLL_DOWN		0x800000/* serdes PLL down override */
-
-/* MDIO control */
-#define MDIOCTL_DIVISOR_MASK		0x7f	/* clock to be used on MDIO */
-#define MDIOCTL_DIVISOR_VAL		0x2
-#define MDIOCTL_PREAM_EN		0x80	/* Enable preamble sequnce */
-#define MDIOCTL_ACCESS_DONE		0x100	/* Transaction complete */
-
-/* MDIO Data */
-#define MDIODATA_MASK			0x0000ffff	/* data 2 bytes */
-#define MDIODATA_TA			0x00020000	/* Turnaround */
-
-#define MDIODATA_REGADDR_SHF		18		/* Regaddr shift */
-#define MDIODATA_REGADDR_MASK		0x007c0000	/* Regaddr Mask */
-#define MDIODATA_DEVADDR_SHF		23	/* Physmedia devaddr shift */
-#define MDIODATA_DEVADDR_MASK		0x0f800000
-						/* Physmedia devaddr Mask */
-
-/* MDIO Data for older revisions < 10 */
-#define MDIODATA_REGADDR_SHF_OLD	18	/* Regaddr shift */
-#define MDIODATA_REGADDR_MASK_OLD	0x003c0000
-						/* Regaddr Mask */
-#define MDIODATA_DEVADDR_SHF_OLD	22	/* Physmedia devaddr shift  */
-#define MDIODATA_DEVADDR_MASK_OLD	0x0fc00000
-						/* Physmedia devaddr Mask */
-
-/* Transactions flags */
-#define MDIODATA_WRITE			0x10000000
-#define MDIODATA_READ			0x20000000
-#define MDIODATA_START			0x40000000
-
-#define MDIODATA_DEV_ADDR		0x0	/* dev address for serdes */
-#define	MDIODATA_BLK_ADDR		0x1F	/* blk address for serdes */
-
-/* serdes regs (rev < 10) */
-#define MDIODATA_DEV_PLL		0x1d	/* SERDES PLL Dev */
-#define MDIODATA_DEV_TX			0x1e	/* SERDES TX Dev */
-#define MDIODATA_DEV_RX			0x1f	/* SERDES RX Dev */
-
-/* SERDES RX registers */
-#define SERDES_RX_CTRL			1	/* Rx cntrl */
-#define SERDES_RX_TIMER1		2	/* Rx Timer1 */
-#define SERDES_RX_CDR			6	/* CDR */
-#define SERDES_RX_CDRBW			7	/* CDR BW */
-/* SERDES RX control register */
-#define SERDES_RX_CTRL_FORCE		0x80	/* rxpolarity_force */
-#define SERDES_RX_CTRL_POLARITY		0x40	/* rxpolarity_value */
-
-/* SERDES PLL registers */
-#define SERDES_PLL_CTRL                 1	/* PLL control reg */
-#define PLL_CTRL_FREQDET_EN             0x4000	/* bit 14 is FREQDET on */
-
-/* Linkcontrol reg offset in PCIE Cap */
-#define PCIE_CAP_LINKCTRL_OFFSET	16	/* offset in pcie cap */
-#define PCIE_CAP_LCREG_ASPML0s		0x01	/* ASPM L0s in linkctrl */
-#define PCIE_CAP_LCREG_ASPML1		0x02	/* ASPM L1 in linkctrl */
-#define PCIE_CLKREQ_ENAB		0x100	/* CLKREQ Enab in linkctrl */
-
-#define PCIE_ASPM_ENAB			3	/* ASPM L0s & L1 in linkctrl */
-#define PCIE_ASPM_L1_ENAB		2	/* ASPM L0s & L1 in linkctrl */
-#define PCIE_ASPM_L0s_ENAB		1	/* ASPM L0s & L1 in linkctrl */
-#define PCIE_ASPM_DISAB			0	/* ASPM L0s & L1 in linkctrl */
-
-/* Power management threshold */
-#define PCIE_L1THRESHOLDTIME_MASK       0xFF00	/* bits 8 - 15 */
-#define PCIE_L1THRESHOLDTIME_SHIFT      8	/* PCIE_L1THRESHOLDTIME_SHIFT */
-#define PCIE_L1THRESHOLD_WARVAL         0x72	/* WAR value */
-#define PCIE_ASPMTIMER_EXTEND		0x01000000
-						/* > rev7:
-						 * enable extend ASPM timer
-						 */
-
-/* different register spaces to access thru pcie indirect access */
-#define PCIE_CONFIGREGS		1	/* Access to config space */
-#define PCIE_PCIEREGS		2	/* Access to pcie registers */
-
-/* PCIE protocol PHY diagnostic registers */
-#define	PCIE_PLP_STATUSREG		0x204	/* Status */
-
-/* Status reg PCIE_PLP_STATUSREG */
-#define PCIE_PLP_POLARITYINV_STAT	0x10
-
-/* PCIE protocol DLLP diagnostic registers */
-#define PCIE_DLLP_LCREG			0x100	/* Link Control */
-#define PCIE_DLLP_PMTHRESHREG		0x128	/* Power Management Threshold */
-
-/* PCIE protocol TLP diagnostic registers */
-#define PCIE_TLP_WORKAROUNDSREG		0x004	/* TLP Workarounds */
-
-/* Sonics to PCI translation types */
-#define	SBTOPCI_PREF	0x4		/* prefetch enable */
-#define	SBTOPCI_BURST	0x8		/* burst enable */
-#define	SBTOPCI_RC_READMULTI	0x20	/* memory read multiple */
-
-#define PCI_CLKRUN_DSBL	0x8000	/* Bit 15 forceClkrun */
-
-/* PCI core index in SROM shadow area */
-#define SRSH_PI_OFFSET	0	/* first word */
-#define SRSH_PI_MASK	0xf000	/* bit 15:12 */
-#define SRSH_PI_SHIFT	12	/* bit 15:12 */
-
-#define PCIREGOFFS(field)	offsetof(struct sbpciregs, field)
-#define PCIEREGOFFS(field)	offsetof(struct sbpcieregs, field)
-
-/* Sonics side: PCI core and host control registers */
-struct sbpciregs {
-	u32 control;		/* PCI control */
-	u32 PAD[3];
-	u32 arbcontrol;		/* PCI arbiter control */
-	u32 clkrun;		/* Clkrun Control (>=rev11) */
-	u32 PAD[2];
-	u32 intstatus;		/* Interrupt status */
-	u32 intmask;		/* Interrupt mask */
-	u32 sbtopcimailbox;	/* Sonics to PCI mailbox */
-	u32 PAD[9];
-	u32 bcastaddr;		/* Sonics broadcast address */
-	u32 bcastdata;		/* Sonics broadcast data */
-	u32 PAD[2];
-	u32 gpioin;		/* ro: gpio input (>=rev2) */
-	u32 gpioout;		/* rw: gpio output (>=rev2) */
-	u32 gpioouten;		/* rw: gpio output enable (>= rev2) */
-	u32 gpiocontrol;	/* rw: gpio control (>= rev2) */
-	u32 PAD[36];
-	u32 sbtopci0;		/* Sonics to PCI translation 0 */
-	u32 sbtopci1;		/* Sonics to PCI translation 1 */
-	u32 sbtopci2;		/* Sonics to PCI translation 2 */
-	u32 PAD[189];
-	u32 pcicfg[4][64];	/* 0x400 - 0x7FF, PCI Cfg Space (>=rev8) */
-	u16 sprom[36];		/* SPROM shadow Area */
-	u32 PAD[46];
-};
-
-/* SB side: PCIE core and host control registers */
-struct sbpcieregs {
-	u32 control;		/* host mode only */
-	u32 PAD[2];
-	u32 biststatus;		/* bist Status: 0x00C */
-	u32 gpiosel;		/* PCIE gpio sel: 0x010 */
-	u32 gpioouten;		/* PCIE gpio outen: 0x14 */
-	u32 PAD[2];
-	u32 intstatus;		/* Interrupt status: 0x20 */
-	u32 intmask;		/* Interrupt mask: 0x24 */
-	u32 sbtopcimailbox;	/* sb to pcie mailbox: 0x028 */
-	u32 PAD[53];
-	u32 sbtopcie0;		/* sb to pcie translation 0: 0x100 */
-	u32 sbtopcie1;		/* sb to pcie translation 1: 0x104 */
-	u32 sbtopcie2;		/* sb to pcie translation 2: 0x108 */
-	u32 PAD[5];
-
-	/* pcie core supports in direct access to config space */
-	u32 configaddr;	/* pcie config space access: Address field: 0x120 */
-	u32 configdata;	/* pcie config space access: Data field: 0x124 */
-
-	/* mdio access to serdes */
-	u32 mdiocontrol;	/* controls the mdio access: 0x128 */
-	u32 mdiodata;		/* Data to the mdio access: 0x12c */
-
-	/* pcie protocol phy/dllp/tlp register indirect access mechanism */
-	u32 pcieindaddr;	/* indirect access to
-				 * the internal register: 0x130
-				 */
-	u32 pcieinddata;	/* Data to/from the internal regsiter: 0x134 */
-
-	u32 clkreqenctrl;	/* >= rev 6, Clkreq rdma control : 0x138 */
-	u32 PAD[177];
-	u32 pciecfg[4][64];	/* 0x400 - 0x7FF, PCIE Cfg Space */
-	u16 sprom[64];		/* SPROM shadow Area */
-};
-
-struct pcicore_info {
-	struct bcma_device *core;
-	struct si_pub *sih;	/* System interconnect handle */
-	struct pci_dev *dev;
-	u8 pciecap_lcreg_offset;/* PCIE capability LCreg offset
-				 * in the config space
-				 */
-	bool pcie_pr42767;
-	u8 pcie_polarity;
-	u8 pcie_war_aspm_ovr;	/* Override ASPM/Clkreq settings */
-
-	u8 pmecap_offset;	/* PM Capability offset in the config space */
-	bool pmecap;		/* Capable of generating PME */
-};
-
-#define PCIE_ASPM(sih)							\
-	((ai_get_buscoretype(sih) == PCIE_CORE_ID) &&			\
-	 ((ai_get_buscorerev(sih) >= 3) &&				\
-	  (ai_get_buscorerev(sih) <= 5)))
-
-
-/* delay needed between the mdio control/ mdiodata register data access */
-static void pr28829_delay(void)
-{
-	udelay(10);
-}
-
-/* Initialize the PCI core.
- * It's caller's responsibility to make sure that this is done only once
- */
-struct pcicore_info *pcicore_init(struct si_pub *sih, struct bcma_device *core)
-{
-	struct pcicore_info *pi;
-
-	/* alloc struct pcicore_info */
-	pi = kzalloc(sizeof(struct pcicore_info), GFP_ATOMIC);
-	if (pi == NULL)
-		return NULL;
-
-	pi->sih = sih;
-	pi->dev = core->bus->host_pci;
-	pi->core = core;
-
-	if (core->id.id == PCIE_CORE_ID) {
-		u8 cap_ptr;
-		cap_ptr = pcicore_find_pci_capability(pi->dev, PCI_CAP_ID_EXP,
-						      NULL, NULL);
-		pi->pciecap_lcreg_offset = cap_ptr + PCIE_CAP_LINKCTRL_OFFSET;
-	}
-	return pi;
-}
-
-void pcicore_deinit(struct pcicore_info *pch)
-{
-	kfree(pch);
-}
-
-/* return cap_offset if requested capability exists in the PCI config space */
-/* Note that it's caller's responsibility to make sure it's a pci bus */
-u8
-pcicore_find_pci_capability(struct pci_dev *dev, u8 req_cap_id,
-			    unsigned char *buf, u32 *buflen)
-{
-	u8 cap_id;
-	u8 cap_ptr = 0;
-	u32 bufsize;
-	u8 byte_val;
-
-	/* check for Header type 0 */
-	pci_read_config_byte(dev, PCI_HEADER_TYPE, &byte_val);
-	if ((byte_val & 0x7f) != PCI_HEADER_TYPE_NORMAL)
-		goto end;
-
-	/* check if the capability pointer field exists */
-	pci_read_config_byte(dev, PCI_STATUS, &byte_val);
-	if (!(byte_val & PCI_STATUS_CAP_LIST))
-		goto end;
-
-	pci_read_config_byte(dev, PCI_CAPABILITY_LIST, &cap_ptr);
-	/* check if the capability pointer is 0x00 */
-	if (cap_ptr == 0x00)
-		goto end;
-
-	/* loop thru the capability list
-	 * and see if the pcie capability exists
-	 */
-
-	pci_read_config_byte(dev, cap_ptr, &cap_id);
-
-	while (cap_id != req_cap_id) {
-		pci_read_config_byte(dev, cap_ptr + 1, &cap_ptr);
-		if (cap_ptr == 0x00)
-			break;
-		pci_read_config_byte(dev, cap_ptr, &cap_id);
-	}
-	if (cap_id != req_cap_id)
-		goto end;
-
-	/* found the caller requested capability */
-	if (buf != NULL && buflen != NULL) {
-		u8 cap_data;
-
-		bufsize = *buflen;
-		if (!bufsize)
-			goto end;
-		*buflen = 0;
-		/* copy the capability data excluding cap ID and next ptr */
-		cap_data = cap_ptr + 2;
-		if ((bufsize + cap_data) > PCI_SZPCR)
-			bufsize = PCI_SZPCR - cap_data;
-		*buflen = bufsize;
-		while (bufsize--) {
-			pci_read_config_byte(dev, cap_data, buf);
-			cap_data++;
-			buf++;
-		}
-	}
-end:
-	return cap_ptr;
-}
-
-/* ***** Register Access API */
-static uint
-pcie_readreg(struct bcma_device *core, uint addrtype, uint offset)
-{
-	uint retval = 0xFFFFFFFF;
-
-	switch (addrtype) {
-	case PCIE_CONFIGREGS:
-		bcma_write32(core, PCIEREGOFFS(configaddr), offset);
-		(void)bcma_read32(core, PCIEREGOFFS(configaddr));
-		retval = bcma_read32(core, PCIEREGOFFS(configdata));
-		break;
-	case PCIE_PCIEREGS:
-		bcma_write32(core, PCIEREGOFFS(pcieindaddr), offset);
-		(void)bcma_read32(core, PCIEREGOFFS(pcieindaddr));
-		retval = bcma_read32(core, PCIEREGOFFS(pcieinddata));
-		break;
-	}
-
-	return retval;
-}
-
-static uint pcie_writereg(struct bcma_device *core, uint addrtype,
-			  uint offset, uint val)
-{
-	switch (addrtype) {
-	case PCIE_CONFIGREGS:
-		bcma_write32(core, PCIEREGOFFS(configaddr), offset);
-		bcma_write32(core, PCIEREGOFFS(configdata), val);
-		break;
-	case PCIE_PCIEREGS:
-		bcma_write32(core, PCIEREGOFFS(pcieindaddr), offset);
-		bcma_write32(core, PCIEREGOFFS(pcieinddata), val);
-		break;
-	default:
-		break;
-	}
-	return 0;
-}
-
-static bool pcie_mdiosetblock(struct pcicore_info *pi, uint blk)
-{
-	uint mdiodata, i = 0;
-	uint pcie_serdes_spinwait = 200;
-
-	mdiodata = (MDIODATA_START | MDIODATA_WRITE | MDIODATA_TA |
-		    (MDIODATA_DEV_ADDR << MDIODATA_DEVADDR_SHF) |
-		    (MDIODATA_BLK_ADDR << MDIODATA_REGADDR_SHF) |
-		    (blk << 4));
-	bcma_write32(pi->core, PCIEREGOFFS(mdiodata), mdiodata);
-
-	pr28829_delay();
-	/* retry till the transaction is complete */
-	while (i < pcie_serdes_spinwait) {
-		if (bcma_read32(pi->core, PCIEREGOFFS(mdiocontrol)) &
-		    MDIOCTL_ACCESS_DONE)
-			break;
-
-		udelay(1000);
-		i++;
-	}
-
-	if (i >= pcie_serdes_spinwait)
-		return false;
-
-	return true;
-}
-
-static int
-pcie_mdioop(struct pcicore_info *pi, uint physmedia, uint regaddr, bool write,
-	    uint *val)
-{
-	uint mdiodata;
-	uint i = 0;
-	uint pcie_serdes_spinwait = 10;
-
-	/* enable mdio access to SERDES */
-	bcma_write32(pi->core, PCIEREGOFFS(mdiocontrol),
-		     MDIOCTL_PREAM_EN | MDIOCTL_DIVISOR_VAL);
-
-	if (ai_get_buscorerev(pi->sih) >= 10) {
-		/* new serdes is slower in rw,
-		 * using two layers of reg address mapping
-		 */
-		if (!pcie_mdiosetblock(pi, physmedia))
-			return 1;
-		mdiodata = ((MDIODATA_DEV_ADDR << MDIODATA_DEVADDR_SHF) |
-			    (regaddr << MDIODATA_REGADDR_SHF));
-		pcie_serdes_spinwait *= 20;
-	} else {
-		mdiodata = ((physmedia << MDIODATA_DEVADDR_SHF_OLD) |
-			    (regaddr << MDIODATA_REGADDR_SHF_OLD));
-	}
-
-	if (!write)
-		mdiodata |= (MDIODATA_START | MDIODATA_READ | MDIODATA_TA);
-	else
-		mdiodata |= (MDIODATA_START | MDIODATA_WRITE | MDIODATA_TA |
-			     *val);
-
-	bcma_write32(pi->core, PCIEREGOFFS(mdiodata), mdiodata);
-
-	pr28829_delay();
-
-	/* retry till the transaction is complete */
-	while (i < pcie_serdes_spinwait) {
-		if (bcma_read32(pi->core, PCIEREGOFFS(mdiocontrol)) &
-		    MDIOCTL_ACCESS_DONE) {
-			if (!write) {
-				pr28829_delay();
-				*val = (bcma_read32(pi->core,
-						    PCIEREGOFFS(mdiodata)) &
-					MDIODATA_MASK);
-			}
-			/* Disable mdio access to SERDES */
-			bcma_write32(pi->core, PCIEREGOFFS(mdiocontrol), 0);
-			return 0;
-		}
-		udelay(1000);
-		i++;
-	}
-
-	/* Timed out. Disable mdio access to SERDES. */
-	bcma_write32(pi->core, PCIEREGOFFS(mdiocontrol), 0);
-	return 1;
-}
-
-/* use the mdio interface to read from mdio slaves */
-static int
-pcie_mdioread(struct pcicore_info *pi, uint physmedia, uint regaddr,
-	      uint *regval)
-{
-	return pcie_mdioop(pi, physmedia, regaddr, false, regval);
-}
-
-/* use the mdio interface to write to mdio slaves */
-static int
-pcie_mdiowrite(struct pcicore_info *pi, uint physmedia, uint regaddr, uint val)
-{
-	return pcie_mdioop(pi, physmedia, regaddr, true, &val);
-}
-
-/* ***** Support functions ***** */
-static u8 pcie_clkreq(struct pcicore_info *pi, u32 mask, u32 val)
-{
-	u32 reg_val;
-	u8 offset;
-
-	offset = pi->pciecap_lcreg_offset;
-	if (!offset)
-		return 0;
-
-	pci_read_config_dword(pi->dev, offset, &reg_val);
-	/* set operation */
-	if (mask) {
-		if (val)
-			reg_val |= PCIE_CLKREQ_ENAB;
-		else
-			reg_val &= ~PCIE_CLKREQ_ENAB;
-		pci_write_config_dword(pi->dev, offset, reg_val);
-		pci_read_config_dword(pi->dev, offset, &reg_val);
-	}
-	if (reg_val & PCIE_CLKREQ_ENAB)
-		return 1;
-	else
-		return 0;
-}
-
-static void pcie_extendL1timer(struct pcicore_info *pi, bool extend)
-{
-	u32 w;
-	struct si_pub *sih = pi->sih;
-
-	if (ai_get_buscoretype(sih) != PCIE_CORE_ID ||
-	    ai_get_buscorerev(sih) < 7)
-		return;
-
-	w = pcie_readreg(pi->core, PCIE_PCIEREGS, PCIE_DLLP_PMTHRESHREG);
-	if (extend)
-		w |= PCIE_ASPMTIMER_EXTEND;
-	else
-		w &= ~PCIE_ASPMTIMER_EXTEND;
-	pcie_writereg(pi->core, PCIE_PCIEREGS, PCIE_DLLP_PMTHRESHREG, w);
-	w = pcie_readreg(pi->core, PCIE_PCIEREGS, PCIE_DLLP_PMTHRESHREG);
-}
-
-/* centralized clkreq control policy */
-static void pcie_clkreq_upd(struct pcicore_info *pi, uint state)
-{
-	struct si_pub *sih = pi->sih;
-
-	switch (state) {
-	case SI_DOATTACH:
-		if (PCIE_ASPM(sih))
-			pcie_clkreq(pi, 1, 0);
-		break;
-	case SI_PCIDOWN:
-		/* turn on serdes PLL down */
-		if (ai_get_buscorerev(sih) == 6) {
-			ai_cc_reg(sih,
-				  offsetof(struct chipcregs, chipcontrol_addr),
-				  ~0, 0);
-			ai_cc_reg(sih,
-				  offsetof(struct chipcregs, chipcontrol_data),
-				  ~0x40, 0);
-		} else if (pi->pcie_pr42767) {
-			pcie_clkreq(pi, 1, 1);
-		}
-		break;
-	case SI_PCIUP:
-		/* turn off serdes PLL down */
-		if (ai_get_buscorerev(sih) == 6) {
-			ai_cc_reg(sih,
-				  offsetof(struct chipcregs, chipcontrol_addr),
-				  ~0, 0);
-			ai_cc_reg(sih,
-				  offsetof(struct chipcregs, chipcontrol_data),
-				  ~0x40, 0x40);
-		} else if (PCIE_ASPM(sih)) {	/* disable clkreq */
-			pcie_clkreq(pi, 1, 0);
-		}
-		break;
-	}
-}
-
-/* ***** PCI core WARs ***** */
-/* Done only once at attach time */
-static void pcie_war_polarity(struct pcicore_info *pi)
-{
-	u32 w;
-
-	if (pi->pcie_polarity != 0)
-		return;
-
-	w = pcie_readreg(pi->core, PCIE_PCIEREGS, PCIE_PLP_STATUSREG);
-
-	/* Detect the current polarity at attach and force that polarity and
-	 * disable changing the polarity
-	 */
-	if ((w & PCIE_PLP_POLARITYINV_STAT) == 0)
-		pi->pcie_polarity = SERDES_RX_CTRL_FORCE;
-	else
-		pi->pcie_polarity = (SERDES_RX_CTRL_FORCE |
-				     SERDES_RX_CTRL_POLARITY);
-}
-
-/* enable ASPM and CLKREQ if srom doesn't have it */
-/* Needs to happen when update to shadow SROM is needed
- *   : Coming out of 'standby'/'hibernate'
- *   : If pcie_war_aspm_ovr state changed
- */
-static void pcie_war_aspm_clkreq(struct pcicore_info *pi)
-{
-	struct si_pub *sih = pi->sih;
-	u16 val16;
-	u32 w;
-
-	if (!PCIE_ASPM(sih))
-		return;
-
-	/* bypass this on QT or VSIM */
-	val16 = bcma_read16(pi->core, PCIEREGOFFS(sprom[SRSH_ASPM_OFFSET]));
-
-	val16 &= ~SRSH_ASPM_ENB;
-	if (pi->pcie_war_aspm_ovr == PCIE_ASPM_ENAB)
-		val16 |= SRSH_ASPM_ENB;
-	else if (pi->pcie_war_aspm_ovr == PCIE_ASPM_L1_ENAB)
-		val16 |= SRSH_ASPM_L1_ENB;
-	else if (pi->pcie_war_aspm_ovr == PCIE_ASPM_L0s_ENAB)
-		val16 |= SRSH_ASPM_L0s_ENB;
-
-	bcma_write16(pi->core, PCIEREGOFFS(sprom[SRSH_ASPM_OFFSET]), val16);
-
-	pci_read_config_dword(pi->dev, pi->pciecap_lcreg_offset, &w);
-	w &= ~PCIE_ASPM_ENAB;
-	w |= pi->pcie_war_aspm_ovr;
-	pci_write_config_dword(pi->dev, pi->pciecap_lcreg_offset, w);
-
-	val16 = bcma_read16(pi->core,
-			    PCIEREGOFFS(sprom[SRSH_CLKREQ_OFFSET_REV5]));
-
-	if (pi->pcie_war_aspm_ovr != PCIE_ASPM_DISAB) {
-		val16 |= SRSH_CLKREQ_ENB;
-		pi->pcie_pr42767 = true;
-	} else
-		val16 &= ~SRSH_CLKREQ_ENB;
-
-	bcma_write16(pi->core, PCIEREGOFFS(sprom[SRSH_CLKREQ_OFFSET_REV5]),
-		     val16);
-}
-
-/* Apply the polarity determined at the start */
-/* Needs to happen when coming out of 'standby'/'hibernate' */
-static void pcie_war_serdes(struct pcicore_info *pi)
-{
-	u32 w = 0;
-
-	if (pi->pcie_polarity != 0)
-		pcie_mdiowrite(pi, MDIODATA_DEV_RX, SERDES_RX_CTRL,
-			       pi->pcie_polarity);
-
-	pcie_mdioread(pi, MDIODATA_DEV_PLL, SERDES_PLL_CTRL, &w);
-	if (w & PLL_CTRL_FREQDET_EN) {
-		w &= ~PLL_CTRL_FREQDET_EN;
-		pcie_mdiowrite(pi, MDIODATA_DEV_PLL, SERDES_PLL_CTRL, w);
-	}
-}
-
-/* Fix MISC config to allow coming out of L2/L3-Ready state w/o PRST */
-/* Needs to happen when coming out of 'standby'/'hibernate' */
-static void pcie_misc_config_fixup(struct pcicore_info *pi)
-{
-	u16 val16;
-
-	val16 = bcma_read16(pi->core,
-			    PCIEREGOFFS(sprom[SRSH_PCIE_MISC_CONFIG]));
-
-	if ((val16 & SRSH_L23READY_EXIT_NOPERST) == 0) {
-		val16 |= SRSH_L23READY_EXIT_NOPERST;
-		bcma_write16(pi->core,
-			     PCIEREGOFFS(sprom[SRSH_PCIE_MISC_CONFIG]), val16);
-	}
-}
-
-/* quick hack for testing */
-/* Needs to happen when coming out of 'standby'/'hibernate' */
-static void pcie_war_noplldown(struct pcicore_info *pi)
-{
-	/* turn off serdes PLL down */
-	ai_cc_reg(pi->sih, offsetof(struct chipcregs, chipcontrol),
-		  CHIPCTRL_4321_PLL_DOWN, CHIPCTRL_4321_PLL_DOWN);
-
-	/* clear srom shadow backdoor */
-	bcma_write16(pi->core, PCIEREGOFFS(sprom[SRSH_BD_OFFSET]), 0);
-}
-
-/* Needs to happen when coming out of 'standby'/'hibernate' */
-static void pcie_war_pci_setup(struct pcicore_info *pi)
-{
-	struct si_pub *sih = pi->sih;
-	u32 w;
-
-	if (ai_get_buscorerev(sih) == 0 || ai_get_buscorerev(sih) == 1) {
-		w = pcie_readreg(pi->core, PCIE_PCIEREGS,
-				 PCIE_TLP_WORKAROUNDSREG);
-		w |= 0x8;
-		pcie_writereg(pi->core, PCIE_PCIEREGS,
-			      PCIE_TLP_WORKAROUNDSREG, w);
-	}
-
-	if (ai_get_buscorerev(sih) == 1) {
-		w = pcie_readreg(pi->core, PCIE_PCIEREGS, PCIE_DLLP_LCREG);
-		w |= 0x40;
-		pcie_writereg(pi->core, PCIE_PCIEREGS, PCIE_DLLP_LCREG, w);
-	}
-
-	if (ai_get_buscorerev(sih) == 0) {
-		pcie_mdiowrite(pi, MDIODATA_DEV_RX, SERDES_RX_TIMER1, 0x8128);
-		pcie_mdiowrite(pi, MDIODATA_DEV_RX, SERDES_RX_CDR, 0x0100);
-		pcie_mdiowrite(pi, MDIODATA_DEV_RX, SERDES_RX_CDRBW, 0x1466);
-	} else if (PCIE_ASPM(sih)) {
-		/* Change the L1 threshold for better performance */
-		w = pcie_readreg(pi->core, PCIE_PCIEREGS,
-				 PCIE_DLLP_PMTHRESHREG);
-		w &= ~PCIE_L1THRESHOLDTIME_MASK;
-		w |= PCIE_L1THRESHOLD_WARVAL << PCIE_L1THRESHOLDTIME_SHIFT;
-		pcie_writereg(pi->core, PCIE_PCIEREGS,
-			      PCIE_DLLP_PMTHRESHREG, w);
-
-		pcie_war_serdes(pi);
-
-		pcie_war_aspm_clkreq(pi);
-	} else if (ai_get_buscorerev(pi->sih) == 7)
-		pcie_war_noplldown(pi);
-
-	/* Note that the fix is actually in the SROM,
-	 * that's why this is open-ended
-	 */
-	if (ai_get_buscorerev(pi->sih) >= 6)
-		pcie_misc_config_fixup(pi);
-}
-
-/* ***** Functions called during driver state changes ***** */
-void pcicore_attach(struct pcicore_info *pi, int state)
-{
-	struct si_pub *sih = pi->sih;
-	u32 bfl2 = (u32)getintvar(sih, BRCMS_SROM_BOARDFLAGS2);
-
-	/* Determine if this board needs override */
-	if (PCIE_ASPM(sih)) {
-		if (bfl2 & BFL2_PCIEWAR_OVR)
-			pi->pcie_war_aspm_ovr = PCIE_ASPM_DISAB;
-		else
-			pi->pcie_war_aspm_ovr = PCIE_ASPM_ENAB;
-	}
-
-	/* These need to happen in this order only */
-	pcie_war_polarity(pi);
-
-	pcie_war_serdes(pi);
-
-	pcie_war_aspm_clkreq(pi);
-
-	pcie_clkreq_upd(pi, state);
-
-}
-
-void pcicore_hwup(struct pcicore_info *pi)
-{
-	if (!pi || ai_get_buscoretype(pi->sih) != PCIE_CORE_ID)
-		return;
-
-	pcie_war_pci_setup(pi);
-}
-
-void pcicore_up(struct pcicore_info *pi, int state)
-{
-	if (!pi || ai_get_buscoretype(pi->sih) != PCIE_CORE_ID)
-		return;
-
-	/* Restore L1 timer for better performance */
-	pcie_extendL1timer(pi, true);
-
-	pcie_clkreq_upd(pi, state);
-}
-
-/* When the device is going to enter D3 state
- * (or the system is going to enter S3/S4 states)
- */
-void pcicore_sleep(struct pcicore_info *pi)
-{
-	u32 w;
-
-	if (!pi || !PCIE_ASPM(pi->sih))
-		return;
-
-	pci_read_config_dword(pi->dev, pi->pciecap_lcreg_offset, &w);
-	w &= ~PCIE_CAP_LCREG_ASPML1;
-	pci_write_config_dword(pi->dev, pi->pciecap_lcreg_offset, w);
-
-	pi->pcie_pr42767 = false;
-}
-
-void pcicore_down(struct pcicore_info *pi, int state)
-{
-	if (!pi || ai_get_buscoretype(pi->sih) != PCIE_CORE_ID)
-		return;
-
-	pcie_clkreq_upd(pi, state);
-
-	/* Reduce L1 timer for better power savings */
-	pcie_extendL1timer(pi, false);
-}
-
-void pcicore_fixcfg(struct pcicore_info *pi)
-{
-	struct bcma_device *core = pi->core;
-	u16 val16;
-	uint regoff;
-
-	switch (pi->core->id.id) {
-	case BCMA_CORE_PCI:
-		regoff = PCIREGOFFS(sprom[SRSH_PI_OFFSET]);
-		break;
-
-	case BCMA_CORE_PCIE:
-		regoff = PCIEREGOFFS(sprom[SRSH_PI_OFFSET]);
-		break;
-
-	default:
-		return;
-	}
-
-	val16 = bcma_read16(pi->core, regoff);
-	if (((val16 & SRSH_PI_MASK) >> SRSH_PI_SHIFT) !=
-	    (u16)core->core_index) {
-		val16 = ((u16)core->core_index << SRSH_PI_SHIFT) |
-			(val16 & ~SRSH_PI_MASK);
-		bcma_write16(pi->core, regoff, val16);
-	}
-}
-
-/* precondition: current core is pci core */
-void
-pcicore_pci_setup(struct pcicore_info *pi)
-{
-	bcma_set32(pi->core, PCIREGOFFS(sbtopci2),
-		   SBTOPCI_PREF | SBTOPCI_BURST);
-
-	if (pi->core->id.rev >= 11) {
-		bcma_set32(pi->core, PCIREGOFFS(sbtopci2),
-			   SBTOPCI_RC_READMULTI);
-		bcma_set32(pi->core, PCIREGOFFS(clkrun), PCI_CLKRUN_DSBL);
-		(void)bcma_read32(pi->core, PCIREGOFFS(clkrun));
-	}
-}
diff --git a/drivers/net/wireless/brcm80211/brcmsmac/nicpci.h b/drivers/net/wireless/brcm80211/brcmsmac/nicpci.h
deleted file mode 100644
index 9fc3ead540a..00000000000
--- a/drivers/net/wireless/brcm80211/brcmsmac/nicpci.h
+++ /dev/null
@@ -1,77 +0,0 @@
-/*
- * 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	_BRCM_NICPCI_H_
-#define	_BRCM_NICPCI_H_
-
-#include "types.h"
-
-/* PCI configuration address space size */
-#define PCI_SZPCR		256
-
-/* Brcm PCI configuration registers */
-/* backplane address space accessed by BAR0 */
-#define PCI_BAR0_WIN		0x80
-/* sprom property control */
-#define PCI_SPROM_CONTROL	0x88
-/* mask of PCI and other cores interrupts */
-#define PCI_INT_MASK		0x94
-/* backplane core interrupt mask bits offset */
-#define  PCI_SBIM_SHIFT		8
-/* backplane address space accessed by second 4KB of BAR0 */
-#define PCI_BAR0_WIN2		0xac
-/* pci config space gpio input (>=rev3) */
-#define PCI_GPIO_IN		0xb0
-/* pci config space gpio output (>=rev3) */
-#define PCI_GPIO_OUT		0xb4
-/* pci config space gpio output enable (>=rev3) */
-#define PCI_GPIO_OUTEN		0xb8
-
-/* bar0 + 4K accesses external sprom */
-#define PCI_BAR0_SPROM_OFFSET	(4 * 1024)
-/* bar0 + 6K accesses pci core registers */
-#define PCI_BAR0_PCIREGS_OFFSET	(6 * 1024)
-/*
- * pci core SB registers are at the end of the
- * 8KB window, so their address is the "regular"
- * address plus 4K
- */
-#define PCI_BAR0_PCISBR_OFFSET	(4 * 1024)
-/* bar0 window size Match with corerev 13 */
-#define PCI_BAR0_WINSZ		(16 * 1024)
-/* On pci corerev >= 13 and all pcie, the bar0 is now 16KB and it maps: */
-/* bar0 + 8K accesses pci/pcie core registers */
-#define PCI_16KB0_PCIREGS_OFFSET (8 * 1024)
-/* bar0 + 12K accesses chipc core registers */
-#define PCI_16KB0_CCREGS_OFFSET	(12 * 1024)
-
-struct sbpciregs;
-struct sbpcieregs;
-
-extern struct pcicore_info *pcicore_init(struct si_pub *sih,
-					 struct bcma_device *core);
-extern void pcicore_deinit(struct pcicore_info *pch);
-extern void pcicore_attach(struct pcicore_info *pch, int state);
-extern void pcicore_hwup(struct pcicore_info *pch);
-extern void pcicore_up(struct pcicore_info *pch, int state);
-extern void pcicore_sleep(struct pcicore_info *pch);
-extern void pcicore_down(struct pcicore_info *pch, int state);
-extern u8 pcicore_find_pci_capability(struct pci_dev *dev, u8 req_cap_id,
-				      unsigned char *buf, u32 *buflen);
-extern void pcicore_fixcfg(struct pcicore_info *pch);
-extern void pcicore_pci_setup(struct pcicore_info *pch);
-
-#endif /* _BRCM_NICPCI_H_ */
diff --git a/drivers/net/wireless/brcm80211/brcmsmac/otp.c b/drivers/net/wireless/brcm80211/brcmsmac/otp.c
deleted file mode 100644
index f1ca1262586..00000000000
--- a/drivers/net/wireless/brcm80211/brcmsmac/otp.c
+++ /dev/null
@@ -1,410 +0,0 @@
-/*
- * 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/io.h>
-#include <linux/errno.h>
-#include <linux/string.h>
-
-#include <brcm_hw_ids.h>
-#include <chipcommon.h>
-#include "aiutils.h"
-#include "otp.h"
-
-#define OTPS_GUP_MASK		0x00000f00
-#define OTPS_GUP_SHIFT		8
-/* h/w subregion is programmed */
-#define OTPS_GUP_HW		0x00000100
-/* s/w subregion is programmed */
-#define OTPS_GUP_SW		0x00000200
-/* chipid/pkgopt subregion is programmed */
-#define OTPS_GUP_CI		0x00000400
-/* fuse subregion is programmed */
-#define OTPS_GUP_FUSE		0x00000800
-
-/* Fields in otpprog in rev >= 21 */
-#define OTPP_COL_MASK		0x000000ff
-#define OTPP_COL_SHIFT		0
-#define OTPP_ROW_MASK		0x0000ff00
-#define OTPP_ROW_SHIFT		8
-#define OTPP_OC_MASK		0x0f000000
-#define OTPP_OC_SHIFT		24
-#define OTPP_READERR		0x10000000
-#define OTPP_VALUE_MASK		0x20000000
-#define OTPP_VALUE_SHIFT	29
-#define OTPP_START_BUSY		0x80000000
-#define	OTPP_READ		0x40000000
-
-/* Opcodes for OTPP_OC field */
-#define OTPPOC_READ		0
-#define OTPPOC_BIT_PROG		1
-#define OTPPOC_VERIFY		3
-#define OTPPOC_INIT		4
-#define OTPPOC_SET		5
-#define OTPPOC_RESET		6
-#define OTPPOC_OCST		7
-#define OTPPOC_ROW_LOCK		8
-#define OTPPOC_PRESCN_TEST	9
-
-#define OTPTYPE_IPX(ccrev)	((ccrev) == 21 || (ccrev) >= 23)
-
-#define OTPP_TRIES	10000000	/* # of tries for OTPP */
-
-#define MAXNUMRDES		9	/* Maximum OTP redundancy entries */
-
-/* Fixed size subregions sizes in words */
-#define OTPGU_CI_SZ		2
-
-struct otpinfo;
-
-/* OTP function struct */
-struct otp_fn_s {
-	int (*init)(struct si_pub *sih, struct otpinfo *oi);
-	int (*read_region)(struct otpinfo *oi, int region, u16 *data,
-			   uint *wlen);
-};
-
-struct otpinfo {
-	struct bcma_device *core; /* chipc core */
-	const struct otp_fn_s *fn;	/* OTP functions */
-	struct si_pub *sih;		/* Saved sb handle */
-
-	/* IPX OTP section */
-	u16 wsize;		/* Size of otp in words */
-	u16 rows;		/* Geometry */
-	u16 cols;		/* Geometry */
-	u32 status;		/* Flag bits (lock/prog/rv).
-				 * (Reflected only when OTP is power cycled)
-				 */
-	u16 hwbase;		/* hardware subregion offset */
-	u16 hwlim;		/* hardware subregion boundary */
-	u16 swbase;		/* software subregion offset */
-	u16 swlim;		/* software subregion boundary */
-	u16 fbase;		/* fuse subregion offset */
-	u16 flim;		/* fuse subregion boundary */
-	int otpgu_base;		/* offset to General Use Region */
-};
-
-/* OTP layout */
-/* CC revs 21, 24 and 27 OTP General Use Region word offset */
-#define REVA4_OTPGU_BASE	12
-
-/* CC revs 23, 25, 26, 28 and above OTP General Use Region word offset */
-#define REVB8_OTPGU_BASE	20
-
-/* CC rev 36 OTP General Use Region word offset */
-#define REV36_OTPGU_BASE	12
-
-/* Subregion word offsets in General Use region */
-#define OTPGU_HSB_OFF		0
-#define OTPGU_SFB_OFF		1
-#define OTPGU_CI_OFF		2
-#define OTPGU_P_OFF		3
-#define OTPGU_SROM_OFF		4
-
-/* Flag bit offsets in General Use region  */
-#define OTPGU_HWP_OFF		60
-#define OTPGU_SWP_OFF		61
-#define OTPGU_CIP_OFF		62
-#define OTPGU_FUSEP_OFF		63
-#define OTPGU_CIP_MSK		0x4000
-#define OTPGU_P_MSK		0xf000
-#define OTPGU_P_SHIFT		(OTPGU_HWP_OFF % 16)
-
-/* OTP Size */
-#define OTP_SZ_FU_324		((roundup(324, 8))/8)	/* 324 bits */
-#define OTP_SZ_FU_288		(288/8)	/* 288 bits */
-#define OTP_SZ_FU_216		(216/8)	/* 216 bits */
-#define OTP_SZ_FU_72		(72/8)	/* 72 bits */
-#define OTP_SZ_CHECKSUM		(16/8)	/* 16 bits */
-#define OTP4315_SWREG_SZ	178	/* 178 bytes */
-#define OTP_SZ_FU_144		(144/8)	/* 144 bits */
-
-static u16
-ipxotp_otpr(struct otpinfo *oi, uint wn)
-{
-	return bcma_read16(oi->core,
-			   CHIPCREGOFFS(sromotp[wn]));
-}
-
-/*
- * Calculate max HW/SW region byte size by subtracting fuse region
- * and checksum size, osizew is oi->wsize (OTP size - GU size) in words
- */
-static int ipxotp_max_rgnsz(struct si_pub *sih, int osizew)
-{
-	int ret = 0;
-
-	switch (ai_get_chip_id(sih)) {
-	case BCM43224_CHIP_ID:
-	case BCM43225_CHIP_ID:
-		ret = osizew * 2 - OTP_SZ_FU_72 - OTP_SZ_CHECKSUM;
-		break;
-	case BCM4313_CHIP_ID:
-		ret = osizew * 2 - OTP_SZ_FU_72 - OTP_SZ_CHECKSUM;
-		break;
-	default:
-		break;	/* Don't know about this chip */
-	}
-
-	return ret;
-}
-
-static void _ipxotp_init(struct otpinfo *oi)
-{
-	uint k;
-	u32 otpp, st;
-	int ccrev = ai_get_ccrev(oi->sih);
-
-
-	/*
-	 * record word offset of General Use Region
-	 * for various chipcommon revs
-	 */
-	if (ccrev == 21 || ccrev == 24
-	    || ccrev == 27) {
-		oi->otpgu_base = REVA4_OTPGU_BASE;
-	} else if (ccrev == 36) {
-		/*
-		 * OTP size greater than equal to 2KB (128 words),
-		 * otpgu_base is similar to rev23
-		 */
-		if (oi->wsize >= 128)
-			oi->otpgu_base = REVB8_OTPGU_BASE;
-		else
-			oi->otpgu_base = REV36_OTPGU_BASE;
-	} else if (ccrev == 23 || ccrev >= 25) {
-		oi->otpgu_base = REVB8_OTPGU_BASE;
-	}
-
-	/* First issue an init command so the status is up to date */
-	otpp =
-	    OTPP_START_BUSY | ((OTPPOC_INIT << OTPP_OC_SHIFT) & OTPP_OC_MASK);
-
-	bcma_write32(oi->core, CHIPCREGOFFS(otpprog), otpp);
-	st = bcma_read32(oi->core, CHIPCREGOFFS(otpprog));
-	for (k = 0; (st & OTPP_START_BUSY) && (k < OTPP_TRIES); k++)
-		st = bcma_read32(oi->core, CHIPCREGOFFS(otpprog));
-	if (k >= OTPP_TRIES)
-		return;
-
-	/* Read OTP lock bits and subregion programmed indication bits */
-	oi->status = bcma_read32(oi->core, CHIPCREGOFFS(otpstatus));
-
-	if ((ai_get_chip_id(oi->sih) == BCM43224_CHIP_ID)
-	    || (ai_get_chip_id(oi->sih) == BCM43225_CHIP_ID)) {
-		u32 p_bits;
-		p_bits = (ipxotp_otpr(oi, oi->otpgu_base + OTPGU_P_OFF) &
-			  OTPGU_P_MSK) >> OTPGU_P_SHIFT;
-		oi->status |= (p_bits << OTPS_GUP_SHIFT);
-	}
-
-	/*
-	 * h/w region base and fuse region limit are fixed to
-	 * the top and the bottom of the general use region.
-	 * Everything else can be flexible.
-	 */
-	oi->hwbase = oi->otpgu_base + OTPGU_SROM_OFF;
-	oi->hwlim = oi->wsize;
-	if (oi->status & OTPS_GUP_HW) {
-		oi->hwlim =
-		    ipxotp_otpr(oi, oi->otpgu_base + OTPGU_HSB_OFF) / 16;
-		oi->swbase = oi->hwlim;
-	} else
-		oi->swbase = oi->hwbase;
-
-	/* subtract fuse and checksum from beginning */
-	oi->swlim = ipxotp_max_rgnsz(oi->sih, oi->wsize) / 2;
-
-	if (oi->status & OTPS_GUP_SW) {
-		oi->swlim =
-		    ipxotp_otpr(oi, oi->otpgu_base + OTPGU_SFB_OFF) / 16;
-		oi->fbase = oi->swlim;
-	} else
-		oi->fbase = oi->swbase;
-
-	oi->flim = oi->wsize;
-}
-
-static int ipxotp_init(struct si_pub *sih, struct otpinfo *oi)
-{
-	/* Make sure we're running IPX OTP */
-	if (!OTPTYPE_IPX(ai_get_ccrev(sih)))
-		return -EBADE;
-
-	/* Make sure OTP is not disabled */
-	if (ai_is_otp_disabled(sih))
-		return -EBADE;
-
-	/* Check for otp size */
-	switch ((ai_get_cccaps(sih) & CC_CAP_OTPSIZE) >> CC_CAP_OTPSIZE_SHIFT) {
-	case 0:
-		/* Nothing there */
-		return -EBADE;
-	case 1:		/* 32x64 */
-		oi->rows = 32;
-		oi->cols = 64;
-		oi->wsize = 128;
-		break;
-	case 2:		/* 64x64 */
-		oi->rows = 64;
-		oi->cols = 64;
-		oi->wsize = 256;
-		break;
-	case 5:		/* 96x64 */
-		oi->rows = 96;
-		oi->cols = 64;
-		oi->wsize = 384;
-		break;
-	case 7:		/* 16x64 *//* 1024 bits */
-		oi->rows = 16;
-		oi->cols = 64;
-		oi->wsize = 64;
-		break;
-	default:
-		/* Don't know the geometry */
-		return -EBADE;
-	}
-
-	/* Retrieve OTP region info */
-	_ipxotp_init(oi);
-	return 0;
-}
-
-static int
-ipxotp_read_region(struct otpinfo *oi, int region, u16 *data, uint *wlen)
-{
-	uint base, i, sz;
-
-	/* Validate region selection */
-	switch (region) {
-	case OTP_HW_RGN:
-		sz = (uint) oi->hwlim - oi->hwbase;
-		if (!(oi->status & OTPS_GUP_HW)) {
-			*wlen = sz;
-			return -ENODATA;
-		}
-		if (*wlen < sz) {
-			*wlen = sz;
-			return -EOVERFLOW;
-		}
-		base = oi->hwbase;
-		break;
-	case OTP_SW_RGN:
-		sz = ((uint) oi->swlim - oi->swbase);
-		if (!(oi->status & OTPS_GUP_SW)) {
-			*wlen = sz;
-			return -ENODATA;
-		}
-		if (*wlen < sz) {
-			*wlen = sz;
-			return -EOVERFLOW;
-		}
-		base = oi->swbase;
-		break;
-	case OTP_CI_RGN:
-		sz = OTPGU_CI_SZ;
-		if (!(oi->status & OTPS_GUP_CI)) {
-			*wlen = sz;
-			return -ENODATA;
-		}
-		if (*wlen < sz) {
-			*wlen = sz;
-			return -EOVERFLOW;
-		}
-		base = oi->otpgu_base + OTPGU_CI_OFF;
-		break;
-	case OTP_FUSE_RGN:
-		sz = (uint) oi->flim - oi->fbase;
-		if (!(oi->status & OTPS_GUP_FUSE)) {
-			*wlen = sz;
-			return -ENODATA;
-		}
-		if (*wlen < sz) {
-			*wlen = sz;
-			return -EOVERFLOW;
-		}
-		base = oi->fbase;
-		break;
-	case OTP_ALL_RGN:
-		sz = ((uint) oi->flim - oi->hwbase);
-		if (!(oi->status & (OTPS_GUP_HW | OTPS_GUP_SW))) {
-			*wlen = sz;
-			return -ENODATA;
-		}
-		if (*wlen < sz) {
-			*wlen = sz;
-			return -EOVERFLOW;
-		}
-		base = oi->hwbase;
-		break;
-	default:
-		return -EINVAL;
-	}
-
-	/* Read the data */
-	for (i = 0; i < sz; i++)
-		data[i] = ipxotp_otpr(oi, base + i);
-
-	*wlen = sz;
-	return 0;
-}
-
-static const struct otp_fn_s ipxotp_fn = {
-	(int (*)(struct si_pub *, struct otpinfo *)) ipxotp_init,
-	(int (*)(struct otpinfo *, int, u16 *, uint *)) ipxotp_read_region,
-};
-
-static int otp_init(struct si_pub *sih, struct otpinfo *oi)
-{
-	int ret;
-
-	memset(oi, 0, sizeof(struct otpinfo));
-
-	oi->core = ai_findcore(sih, BCMA_CORE_CHIPCOMMON, 0);
-
-	if (OTPTYPE_IPX(ai_get_ccrev(sih)))
-		oi->fn = &ipxotp_fn;
-
-	if (oi->fn == NULL)
-		return -EBADE;
-
-	oi->sih = sih;
-
-	ret = (oi->fn->init)(sih, oi);
-
-	return ret;
-}
-
-int
-otp_read_region(struct si_pub *sih, int region, u16 *data, uint *wlen) {
-	struct otpinfo otpinfo;
-	struct otpinfo *oi = &otpinfo;
-	int err = 0;
-
-	if (ai_is_otp_disabled(sih)) {
-		err = -EPERM;
-		goto out;
-	}
-
-	err = otp_init(sih, oi);
-	if (err)
-		goto out;
-
-	err = ((oi)->fn->read_region)(oi, region, data, wlen);
-
- out:
-	return err;
-}
diff --git a/drivers/net/wireless/brcm80211/brcmsmac/otp.h b/drivers/net/wireless/brcm80211/brcmsmac/otp.h
deleted file mode 100644
index 6b6d31cf956..00000000000
--- a/drivers/net/wireless/brcm80211/brcmsmac/otp.h
+++ /dev/null
@@ -1,36 +0,0 @@
-/*
- * 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	_BRCM_OTP_H_
-#define	_BRCM_OTP_H_
-
-#include "types.h"
-
-/* OTP regions */
-#define OTP_HW_RGN	1
-#define OTP_SW_RGN	2
-#define OTP_CI_RGN	4
-#define OTP_FUSE_RGN	8
-/* From h/w region to end of OTP including checksum */
-#define OTP_ALL_RGN	0xf
-
-/* OTP Size */
-#define OTP_SZ_MAX		(6144/8)	/* maximum bytes in one CIS */
-
-extern int otp_read_region(struct si_pub *sih, int region, u16 *data,
-			   uint *wlen);
-
-#endif				/* _BRCM_OTP_H_ */
diff --git a/drivers/net/wireless/brcm80211/brcmsmac/phy/phy_lcn.c b/drivers/net/wireless/brcm80211/brcmsmac/phy/phy_lcn.c
index ce8562aa5db..abfd78822fb 100644
--- a/drivers/net/wireless/brcm80211/brcmsmac/phy/phy_lcn.c
+++ b/drivers/net/wireless/brcm80211/brcmsmac/phy/phy_lcn.c
@@ -207,8 +207,7 @@ static const iqcal_gain_params_lcnphy *tbl_iqcal_gainparams_lcnphy[1] = {
 };
 
 static const u16 iqcal_gainparams_numgains_lcnphy[1] = {
-	sizeof(tbl_iqcal_gainparams_lcnphy_2G) /
-	sizeof(*tbl_iqcal_gainparams_lcnphy_2G),
+	ARRAY_SIZE(tbl_iqcal_gainparams_lcnphy_2G),
 };
 
 static const struct lcnphy_sfo_cfg lcnphy_sfo_cfg[] = {
@@ -4818,28 +4817,23 @@ static bool wlc_phy_txpwr_srom_read_lcnphy(struct brcms_phy *pi)
 	s8 txpwr = 0;
 	int i;
 	struct brcms_phy_lcnphy *pi_lcn = pi->u.pi_lcnphy;
-	struct phy_shim_info *shim = pi->sh->physhim;
+	struct ssb_sprom *sprom = &pi->d11core->bus->sprom;
 
 	if (CHSPEC_IS2G(pi->radio_chanspec)) {
 		u16 cckpo = 0;
 		u32 offset_ofdm, offset_mcs;
 
-		pi_lcn->lcnphy_tr_isolation_mid =
-			(u8)wlapi_getintvar(shim, BRCMS_SROM_TRISO2G);
+		pi_lcn->lcnphy_tr_isolation_mid = sprom->fem.ghz2.tr_iso;
 
-		pi_lcn->lcnphy_rx_power_offset =
-			(u8)wlapi_getintvar(shim, BRCMS_SROM_RXPO2G);
+		pi_lcn->lcnphy_rx_power_offset = sprom->rxpo2g;
 
-		pi->txpa_2g[0] = (s16)wlapi_getintvar(shim, BRCMS_SROM_PA0B0);
-		pi->txpa_2g[1] = (s16)wlapi_getintvar(shim, BRCMS_SROM_PA0B1);
-		pi->txpa_2g[2] = (s16)wlapi_getintvar(shim, BRCMS_SROM_PA0B2);
+		pi->txpa_2g[0] = sprom->pa0b0;
+		pi->txpa_2g[1] = sprom->pa0b1;
+		pi->txpa_2g[2] = sprom->pa0b2;
 
-		pi_lcn->lcnphy_rssi_vf =
-				(u8)wlapi_getintvar(shim, BRCMS_SROM_RSSISMF2G);
-		pi_lcn->lcnphy_rssi_vc =
-				(u8)wlapi_getintvar(shim, BRCMS_SROM_RSSISMC2G);
-		pi_lcn->lcnphy_rssi_gs =
-				(u8)wlapi_getintvar(shim, BRCMS_SROM_RSSISAV2G);
+		pi_lcn->lcnphy_rssi_vf = sprom->rssismf2g;
+		pi_lcn->lcnphy_rssi_vc = sprom->rssismc2g;
+		pi_lcn->lcnphy_rssi_gs = sprom->rssisav2g;
 
 		pi_lcn->lcnphy_rssi_vf_lowtemp = pi_lcn->lcnphy_rssi_vf;
 		pi_lcn->lcnphy_rssi_vc_lowtemp = pi_lcn->lcnphy_rssi_vc;
@@ -4849,7 +4843,7 @@ static bool wlc_phy_txpwr_srom_read_lcnphy(struct brcms_phy *pi)
 		pi_lcn->lcnphy_rssi_vc_hightemp = pi_lcn->lcnphy_rssi_vc;
 		pi_lcn->lcnphy_rssi_gs_hightemp = pi_lcn->lcnphy_rssi_gs;
 
-		txpwr = (s8)wlapi_getintvar(shim, BRCMS_SROM_MAXP2GA0);
+		txpwr = sprom->core_pwr_info[0].maxpwr_2g;
 		pi->tx_srom_max_2g = txpwr;
 
 		for (i = 0; i < PWRTBL_NUM_COEFF; i++) {
@@ -4857,8 +4851,8 @@ static bool wlc_phy_txpwr_srom_read_lcnphy(struct brcms_phy *pi)
 			pi->txpa_2g_high_temp[i] = pi->txpa_2g[i];
 		}
 
-		cckpo = (u16)wlapi_getintvar(shim, BRCMS_SROM_CCK2GPO);
-		offset_ofdm = (u32)wlapi_getintvar(shim, BRCMS_SROM_OFDM2GPO);
+		cckpo = sprom->cck2gpo;
+		offset_ofdm = sprom->ofdm2gpo;
 		if (cckpo) {
 			uint max_pwr_chan = txpwr;
 
@@ -4877,7 +4871,7 @@ static bool wlc_phy_txpwr_srom_read_lcnphy(struct brcms_phy *pi)
 		} else {
 			u8 opo = 0;
 
-			opo = (u8)wlapi_getintvar(shim, BRCMS_SROM_OPO);
+			opo = sprom->opo;
 
 			for (i = TXP_FIRST_CCK; i <= TXP_LAST_CCK; i++)
 				pi->tx_srom_max_rate_2g[i] = txpwr;
@@ -4887,12 +4881,8 @@ static bool wlc_phy_txpwr_srom_read_lcnphy(struct brcms_phy *pi)
 						((offset_ofdm & 0xf) * 2);
 				offset_ofdm >>= 4;
 			}
-			offset_mcs =
-				wlapi_getintvar(shim,
-						BRCMS_SROM_MCS2GPO1) << 16;
-			offset_mcs |=
-				(u16) wlapi_getintvar(shim,
-						      BRCMS_SROM_MCS2GPO0);
+			offset_mcs = sprom->mcs2gpo[1] << 16;
+			offset_mcs |= sprom->mcs2gpo[0];
 			pi_lcn->lcnphy_mcs20_po = offset_mcs;
 			for (i = TXP_FIRST_SISO_MCS_20;
 			     i <= TXP_LAST_SISO_MCS_20; i++) {
@@ -4902,25 +4892,17 @@ static bool wlc_phy_txpwr_srom_read_lcnphy(struct brcms_phy *pi)
 			}
 		}
 
-		pi_lcn->lcnphy_rawtempsense =
-			(u16)wlapi_getintvar(shim, BRCMS_SROM_RAWTEMPSENSE);
-		pi_lcn->lcnphy_measPower =
-			(u8)wlapi_getintvar(shim, BRCMS_SROM_MEASPOWER);
-		pi_lcn->lcnphy_tempsense_slope =
-			(u8)wlapi_getintvar(shim, BRCMS_SROM_TEMPSENSE_SLOPE);
-		pi_lcn->lcnphy_hw_iqcal_en =
-			(bool)wlapi_getintvar(shim, BRCMS_SROM_HW_IQCAL_EN);
-		pi_lcn->lcnphy_iqcal_swp_dis =
-			(bool)wlapi_getintvar(shim, BRCMS_SROM_IQCAL_SWP_DIS);
-		pi_lcn->lcnphy_tempcorrx =
-			(u8)wlapi_getintvar(shim, BRCMS_SROM_TEMPCORRX);
-		pi_lcn->lcnphy_tempsense_option =
-			(u8)wlapi_getintvar(shim, BRCMS_SROM_TEMPSENSE_OPTION);
-		pi_lcn->lcnphy_freqoffset_corr =
-			(u8)wlapi_getintvar(shim, BRCMS_SROM_FREQOFFSET_CORR);
-		if ((u8)wlapi_getintvar(shim, BRCMS_SROM_AA2G) > 1)
+		pi_lcn->lcnphy_rawtempsense = sprom->rawtempsense;
+		pi_lcn->lcnphy_measPower = sprom->measpower;
+		pi_lcn->lcnphy_tempsense_slope = sprom->tempsense_slope;
+		pi_lcn->lcnphy_hw_iqcal_en = sprom->hw_iqcal_en;
+		pi_lcn->lcnphy_iqcal_swp_dis = sprom->iqcal_swp_dis;
+		pi_lcn->lcnphy_tempcorrx = sprom->tempcorrx;
+		pi_lcn->lcnphy_tempsense_option = sprom->tempsense_option;
+		pi_lcn->lcnphy_freqoffset_corr = sprom->freqoffset_corr;
+		if (sprom->ant_available_bg > 1)
 			wlc_phy_ant_rxdiv_set((struct brcms_phy_pub *) pi,
-				(u8) wlapi_getintvar(shim, BRCMS_SROM_AA2G));
+				sprom->ant_available_bg);
 	}
 	pi_lcn->lcnphy_cck_dig_filt_type = -1;
 
diff --git a/drivers/net/wireless/brcm80211/brcmsmac/phy/phy_n.c b/drivers/net/wireless/brcm80211/brcmsmac/phy/phy_n.c
index 39095741fd0..13b261517cc 100644
--- a/drivers/net/wireless/brcm80211/brcmsmac/phy/phy_n.c
+++ b/drivers/net/wireless/brcm80211/brcmsmac/phy/phy_n.c
@@ -14386,30 +14386,30 @@ static void wlc_phy_txpwr_srom_read_ppr_nphy(struct brcms_phy *pi)
 {
 	u16 bw40po, cddpo, stbcpo, bwduppo;
 	uint band_num;
-	struct phy_shim_info *shim = pi->sh->physhim;
+	struct ssb_sprom *sprom = &pi->d11core->bus->sprom;
 
 	if (pi->sh->sromrev >= 9)
 		return;
 
-	bw40po = (u16) wlapi_getintvar(shim, BRCMS_SROM_BW40PO);
+	bw40po = sprom->bw40po;
 	pi->bw402gpo = bw40po & 0xf;
 	pi->bw405gpo = (bw40po & 0xf0) >> 4;
 	pi->bw405glpo = (bw40po & 0xf00) >> 8;
 	pi->bw405ghpo = (bw40po & 0xf000) >> 12;
 
-	cddpo = (u16) wlapi_getintvar(shim, BRCMS_SROM_CDDPO);
+	cddpo = sprom->cddpo;
 	pi->cdd2gpo = cddpo & 0xf;
 	pi->cdd5gpo = (cddpo & 0xf0) >> 4;
 	pi->cdd5glpo = (cddpo & 0xf00) >> 8;
 	pi->cdd5ghpo = (cddpo & 0xf000) >> 12;
 
-	stbcpo = (u16) wlapi_getintvar(shim, BRCMS_SROM_STBCPO);
+	stbcpo = sprom->stbcpo;
 	pi->stbc2gpo = stbcpo & 0xf;
 	pi->stbc5gpo = (stbcpo & 0xf0) >> 4;
 	pi->stbc5glpo = (stbcpo & 0xf00) >> 8;
 	pi->stbc5ghpo = (stbcpo & 0xf000) >> 12;
 
-	bwduppo = (u16) wlapi_getintvar(shim, BRCMS_SROM_BWDUPPO);
+	bwduppo = sprom->bwduppo;
 	pi->bwdup2gpo = bwduppo & 0xf;
 	pi->bwdup5gpo = (bwduppo & 0xf0) >> 4;
 	pi->bwdup5glpo = (bwduppo & 0xf00) >> 8;
@@ -14419,242 +14419,137 @@ static void wlc_phy_txpwr_srom_read_ppr_nphy(struct brcms_phy *pi)
 	     band_num++) {
 		switch (band_num) {
 		case 0:
-
 			pi->nphy_pwrctrl_info[PHY_CORE_0].max_pwr_2g =
-				(s8) wlapi_getintvar(shim,
-						     BRCMS_SROM_MAXP2GA0);
+				sprom->core_pwr_info[0].maxpwr_2g;
 			pi->nphy_pwrctrl_info[PHY_CORE_1].max_pwr_2g =
-				(s8) wlapi_getintvar(shim,
-						     BRCMS_SROM_MAXP2GA1);
+				sprom->core_pwr_info[1].maxpwr_2g;
 			pi->nphy_pwrctrl_info[PHY_CORE_0].pwrdet_2g_a1 =
-				(s16) wlapi_getintvar(shim,
-						      BRCMS_SROM_PA2GW0A0);
+				sprom->core_pwr_info[0].pa_2g[0];
 			pi->nphy_pwrctrl_info[PHY_CORE_1].pwrdet_2g_a1 =
-				(s16) wlapi_getintvar(shim,
-						      BRCMS_SROM_PA2GW0A1);
+				sprom->core_pwr_info[1].pa_2g[0];
 			pi->nphy_pwrctrl_info[PHY_CORE_0].pwrdet_2g_b0 =
-				(s16) wlapi_getintvar(shim,
-						      BRCMS_SROM_PA2GW1A0);
+				sprom->core_pwr_info[0].pa_2g[1];
 			pi->nphy_pwrctrl_info[PHY_CORE_1].pwrdet_2g_b0 =
-				(s16) wlapi_getintvar(shim,
-						      BRCMS_SROM_PA2GW1A1);
+				sprom->core_pwr_info[1].pa_2g[1];
 			pi->nphy_pwrctrl_info[PHY_CORE_0].pwrdet_2g_b1 =
-				(s16) wlapi_getintvar(shim,
-						      BRCMS_SROM_PA2GW2A0);
+				sprom->core_pwr_info[0].pa_2g[2];
 			pi->nphy_pwrctrl_info[PHY_CORE_1].pwrdet_2g_b1 =
-				(s16) wlapi_getintvar(shim,
-						      BRCMS_SROM_PA2GW2A1);
+				sprom->core_pwr_info[1].pa_2g[2];
 			pi->nphy_pwrctrl_info[PHY_CORE_0].idle_targ_2g =
-				(s8) wlapi_getintvar(shim, BRCMS_SROM_ITT2GA0);
+				sprom->core_pwr_info[0].itssi_2g;
 			pi->nphy_pwrctrl_info[PHY_CORE_1].idle_targ_2g =
-				(s8) wlapi_getintvar(shim, BRCMS_SROM_ITT2GA1);
-
-			pi->cck2gpo = (u16) wlapi_getintvar(shim,
-							    BRCMS_SROM_CCK2GPO);
-
-			pi->ofdm2gpo =
-				(u32) wlapi_getintvar(shim,
-						      BRCMS_SROM_OFDM2GPO);
-
-			pi->mcs2gpo[0] =
-				(u16) wlapi_getintvar(shim,
-						      BRCMS_SROM_MCS2GPO0);
-			pi->mcs2gpo[1] =
-				(u16) wlapi_getintvar(shim,
-						      BRCMS_SROM_MCS2GPO1);
-			pi->mcs2gpo[2] =
-				(u16) wlapi_getintvar(shim,
-						      BRCMS_SROM_MCS2GPO2);
-			pi->mcs2gpo[3] =
-				(u16) wlapi_getintvar(shim,
-						      BRCMS_SROM_MCS2GPO3);
-			pi->mcs2gpo[4] =
-				(u16) wlapi_getintvar(shim,
-						      BRCMS_SROM_MCS2GPO4);
-			pi->mcs2gpo[5] =
-				(u16) wlapi_getintvar(shim,
-						      BRCMS_SROM_MCS2GPO5);
-			pi->mcs2gpo[6] =
-				(u16) wlapi_getintvar(shim,
-						      BRCMS_SROM_MCS2GPO6);
-			pi->mcs2gpo[7] =
-				(u16) wlapi_getintvar(shim,
-						      BRCMS_SROM_MCS2GPO7);
+				sprom->core_pwr_info[1].itssi_2g;
+
+			pi->cck2gpo = sprom->cck2gpo;
+
+			pi->ofdm2gpo = sprom->ofdm2gpo;
+
+			pi->mcs2gpo[0] = sprom->mcs2gpo[0];
+			pi->mcs2gpo[1] = sprom->mcs2gpo[1];
+			pi->mcs2gpo[2] = sprom->mcs2gpo[2];
+			pi->mcs2gpo[3] = sprom->mcs2gpo[3];
+			pi->mcs2gpo[4] = sprom->mcs2gpo[4];
+			pi->mcs2gpo[5] = sprom->mcs2gpo[5];
+			pi->mcs2gpo[6] = sprom->mcs2gpo[6];
+			pi->mcs2gpo[7] = sprom->mcs2gpo[7];
 			break;
 		case 1:
 
 			pi->nphy_pwrctrl_info[PHY_CORE_0].max_pwr_5gm =
-				(s8) wlapi_getintvar(shim, BRCMS_SROM_MAXP5GA0);
+				sprom->core_pwr_info[0].maxpwr_5g;
 			pi->nphy_pwrctrl_info[PHY_CORE_1].max_pwr_5gm =
-				(s8) wlapi_getintvar(shim,
-						     BRCMS_SROM_MAXP5GA1);
+				sprom->core_pwr_info[1].maxpwr_5g;
 			pi->nphy_pwrctrl_info[PHY_CORE_0].pwrdet_5gm_a1 =
-				(s16) wlapi_getintvar(shim,
-						      BRCMS_SROM_PA5GW0A0);
+				sprom->core_pwr_info[0].pa_5g[0];
 			pi->nphy_pwrctrl_info[PHY_CORE_1].pwrdet_5gm_a1 =
-				(s16) wlapi_getintvar(shim,
-						      BRCMS_SROM_PA5GW0A1);
+				sprom->core_pwr_info[1].pa_5g[0];
 			pi->nphy_pwrctrl_info[PHY_CORE_0].pwrdet_5gm_b0 =
-				(s16) wlapi_getintvar(shim,
-						      BRCMS_SROM_PA5GW1A0);
+				sprom->core_pwr_info[0].pa_5g[1];
 			pi->nphy_pwrctrl_info[PHY_CORE_1].pwrdet_5gm_b0 =
-				(s16) wlapi_getintvar(shim,
-						      BRCMS_SROM_PA5GW1A1);
+				sprom->core_pwr_info[1].pa_5g[1];
 			pi->nphy_pwrctrl_info[PHY_CORE_0].pwrdet_5gm_b1 =
-				(s16) wlapi_getintvar(shim,
-						      BRCMS_SROM_PA5GW2A0);
+				sprom->core_pwr_info[0].pa_5g[2];
 			pi->nphy_pwrctrl_info[PHY_CORE_1].pwrdet_5gm_b1 =
-				(s16) wlapi_getintvar(shim,
-						      BRCMS_SROM_PA5GW2A1);
+				sprom->core_pwr_info[1].pa_5g[2];
 			pi->nphy_pwrctrl_info[PHY_CORE_0].idle_targ_5gm =
-				(s8) wlapi_getintvar(shim, BRCMS_SROM_ITT5GA0);
+				sprom->core_pwr_info[0].itssi_5g;
 			pi->nphy_pwrctrl_info[PHY_CORE_1].idle_targ_5gm =
-				(s8) wlapi_getintvar(shim, BRCMS_SROM_ITT5GA1);
-
-			pi->ofdm5gpo =
-				(u32) wlapi_getintvar(shim,
-						      BRCMS_SROM_OFDM5GPO);
-
-			pi->mcs5gpo[0] =
-				(u16) wlapi_getintvar(shim,
-						      BRCMS_SROM_MCS5GPO0);
-			pi->mcs5gpo[1] =
-				(u16) wlapi_getintvar(shim,
-						      BRCMS_SROM_MCS5GPO1);
-			pi->mcs5gpo[2] =
-				(u16) wlapi_getintvar(shim,
-						      BRCMS_SROM_MCS5GPO2);
-			pi->mcs5gpo[3] =
-				(u16) wlapi_getintvar(shim,
-						      BRCMS_SROM_MCS5GPO3);
-			pi->mcs5gpo[4] =
-				(u16) wlapi_getintvar(shim,
-						      BRCMS_SROM_MCS5GPO4);
-			pi->mcs5gpo[5] =
-				(u16) wlapi_getintvar(shim,
-						      BRCMS_SROM_MCS5GPO5);
-			pi->mcs5gpo[6] =
-				(u16) wlapi_getintvar(shim,
-						      BRCMS_SROM_MCS5GPO6);
-			pi->mcs5gpo[7] =
-				(u16) wlapi_getintvar(shim,
-						      BRCMS_SROM_MCS5GPO7);
+				sprom->core_pwr_info[1].itssi_5g;
+
+			pi->ofdm5gpo = sprom->ofdm5gpo;
+
+			pi->mcs5gpo[0] = sprom->mcs5gpo[0];
+			pi->mcs5gpo[1] = sprom->mcs5gpo[1];
+			pi->mcs5gpo[2] = sprom->mcs5gpo[2];
+			pi->mcs5gpo[3] = sprom->mcs5gpo[3];
+			pi->mcs5gpo[4] = sprom->mcs5gpo[4];
+			pi->mcs5gpo[5] = sprom->mcs5gpo[5];
+			pi->mcs5gpo[6] = sprom->mcs5gpo[6];
+			pi->mcs5gpo[7] = sprom->mcs5gpo[7];
 			break;
 		case 2:
 
 			pi->nphy_pwrctrl_info[0].max_pwr_5gl =
-				(s8) wlapi_getintvar(shim,
-						     BRCMS_SROM_MAXP5GLA0);
+				sprom->core_pwr_info[0].maxpwr_5gl;
 			pi->nphy_pwrctrl_info[1].max_pwr_5gl =
-				(s8) wlapi_getintvar(shim,
-						     BRCMS_SROM_MAXP5GLA1);
+				sprom->core_pwr_info[1].maxpwr_5gl;
 			pi->nphy_pwrctrl_info[0].pwrdet_5gl_a1 =
-				(s16) wlapi_getintvar(shim,
-						      BRCMS_SROM_PA5GLW0A0);
+				sprom->core_pwr_info[0].pa_5gl[0];
 			pi->nphy_pwrctrl_info[1].pwrdet_5gl_a1 =
-				(s16) wlapi_getintvar(shim,
-						      BRCMS_SROM_PA5GLW0A1);
+				sprom->core_pwr_info[1].pa_5gl[0];
 			pi->nphy_pwrctrl_info[0].pwrdet_5gl_b0 =
-				(s16) wlapi_getintvar(shim,
-						      BRCMS_SROM_PA5GLW1A0);
+				sprom->core_pwr_info[0].pa_5gl[1];
 			pi->nphy_pwrctrl_info[1].pwrdet_5gl_b0 =
-				(s16) wlapi_getintvar(shim,
-						      BRCMS_SROM_PA5GLW1A1);
+				sprom->core_pwr_info[1].pa_5gl[1];
 			pi->nphy_pwrctrl_info[0].pwrdet_5gl_b1 =
-				(s16) wlapi_getintvar(shim,
-						      BRCMS_SROM_PA5GLW2A0);
+				sprom->core_pwr_info[0].pa_5gl[2];
 			pi->nphy_pwrctrl_info[1].pwrdet_5gl_b1 =
-				(s16) wlapi_getintvar(shim,
-						      BRCMS_SROM_PA5GLW2A1);
+				sprom->core_pwr_info[1].pa_5gl[2];
 			pi->nphy_pwrctrl_info[0].idle_targ_5gl = 0;
 			pi->nphy_pwrctrl_info[1].idle_targ_5gl = 0;
 
-			pi->ofdm5glpo =
-				(u32) wlapi_getintvar(shim,
-						      BRCMS_SROM_OFDM5GLPO);
-
-			pi->mcs5glpo[0] =
-				(u16) wlapi_getintvar(shim,
-						      BRCMS_SROM_MCS5GLPO0);
-			pi->mcs5glpo[1] =
-				(u16) wlapi_getintvar(shim,
-						      BRCMS_SROM_MCS5GLPO1);
-			pi->mcs5glpo[2] =
-				(u16) wlapi_getintvar(shim,
-						      BRCMS_SROM_MCS5GLPO2);
-			pi->mcs5glpo[3] =
-				(u16) wlapi_getintvar(shim,
-						      BRCMS_SROM_MCS5GLPO3);
-			pi->mcs5glpo[4] =
-				(u16) wlapi_getintvar(shim,
-						      BRCMS_SROM_MCS5GLPO4);
-			pi->mcs5glpo[5] =
-				(u16) wlapi_getintvar(shim,
-						      BRCMS_SROM_MCS5GLPO5);
-			pi->mcs5glpo[6] =
-				(u16) wlapi_getintvar(shim,
-						      BRCMS_SROM_MCS5GLPO6);
-			pi->mcs5glpo[7] =
-				(u16) wlapi_getintvar(shim,
-						      BRCMS_SROM_MCS5GLPO7);
+			pi->ofdm5glpo = sprom->ofdm5glpo;
+
+			pi->mcs5glpo[0] = sprom->mcs5glpo[0];
+			pi->mcs5glpo[1] = sprom->mcs5glpo[1];
+			pi->mcs5glpo[2] = sprom->mcs5glpo[2];
+			pi->mcs5glpo[3] = sprom->mcs5glpo[3];
+			pi->mcs5glpo[4] = sprom->mcs5glpo[4];
+			pi->mcs5glpo[5] = sprom->mcs5glpo[5];
+			pi->mcs5glpo[6] = sprom->mcs5glpo[6];
+			pi->mcs5glpo[7] = sprom->mcs5glpo[7];
 			break;
 		case 3:
 
 			pi->nphy_pwrctrl_info[0].max_pwr_5gh =
-				(s8) wlapi_getintvar(shim,
-						     BRCMS_SROM_MAXP5GHA0);
+				sprom->core_pwr_info[0].maxpwr_5gh;
 			pi->nphy_pwrctrl_info[1].max_pwr_5gh =
-				(s8) wlapi_getintvar(shim,
-						     BRCMS_SROM_MAXP5GHA1);
+				sprom->core_pwr_info[1].maxpwr_5gh;
 			pi->nphy_pwrctrl_info[0].pwrdet_5gh_a1 =
-				(s16) wlapi_getintvar(shim,
-						      BRCMS_SROM_PA5GHW0A0);
+				sprom->core_pwr_info[0].pa_5gh[0];
 			pi->nphy_pwrctrl_info[1].pwrdet_5gh_a1 =
-				(s16) wlapi_getintvar(shim,
-						      BRCMS_SROM_PA5GHW0A1);
+				sprom->core_pwr_info[1].pa_5gh[0];
 			pi->nphy_pwrctrl_info[0].pwrdet_5gh_b0 =
-				(s16) wlapi_getintvar(shim,
-						      BRCMS_SROM_PA5GHW1A0);
+				sprom->core_pwr_info[0].pa_5gh[1];
 			pi->nphy_pwrctrl_info[1].pwrdet_5gh_b0 =
-				(s16) wlapi_getintvar(shim,
-						      BRCMS_SROM_PA5GHW1A1);
+				sprom->core_pwr_info[1].pa_5gh[1];
 			pi->nphy_pwrctrl_info[0].pwrdet_5gh_b1 =
-				(s16) wlapi_getintvar(shim,
-						      BRCMS_SROM_PA5GHW2A0);
+				sprom->core_pwr_info[0].pa_5gh[2];
 			pi->nphy_pwrctrl_info[1].pwrdet_5gh_b1 =
-				(s16) wlapi_getintvar(shim,
-						      BRCMS_SROM_PA5GHW2A1);
+				sprom->core_pwr_info[1].pa_5gh[2];
 			pi->nphy_pwrctrl_info[0].idle_targ_5gh = 0;
 			pi->nphy_pwrctrl_info[1].idle_targ_5gh = 0;
 
-			pi->ofdm5ghpo =
-				(u32) wlapi_getintvar(shim,
-						      BRCMS_SROM_OFDM5GHPO);
-
-			pi->mcs5ghpo[0] =
-				(u16) wlapi_getintvar(shim,
-						      BRCMS_SROM_MCS5GHPO0);
-			pi->mcs5ghpo[1] =
-				(u16) wlapi_getintvar(shim,
-						      BRCMS_SROM_MCS5GHPO1);
-			pi->mcs5ghpo[2] =
-				(u16) wlapi_getintvar(shim,
-						      BRCMS_SROM_MCS5GHPO2);
-			pi->mcs5ghpo[3] =
-				(u16) wlapi_getintvar(shim,
-						      BRCMS_SROM_MCS5GHPO3);
-			pi->mcs5ghpo[4] =
-				(u16) wlapi_getintvar(shim,
-						      BRCMS_SROM_MCS5GHPO4);
-			pi->mcs5ghpo[5] =
-				(u16) wlapi_getintvar(shim,
-						      BRCMS_SROM_MCS5GHPO5);
-			pi->mcs5ghpo[6] =
-				(u16) wlapi_getintvar(shim,
-						      BRCMS_SROM_MCS5GHPO6);
-			pi->mcs5ghpo[7] =
-				(u16) wlapi_getintvar(shim,
-						      BRCMS_SROM_MCS5GHPO7);
+			pi->ofdm5ghpo = sprom->ofdm5ghpo;
+
+			pi->mcs5ghpo[0] = sprom->mcs5ghpo[0];
+			pi->mcs5ghpo[1] = sprom->mcs5ghpo[1];
+			pi->mcs5ghpo[2] = sprom->mcs5ghpo[2];
+			pi->mcs5ghpo[3] = sprom->mcs5ghpo[3];
+			pi->mcs5ghpo[4] = sprom->mcs5ghpo[4];
+			pi->mcs5ghpo[5] = sprom->mcs5ghpo[5];
+			pi->mcs5ghpo[6] = sprom->mcs5ghpo[6];
+			pi->mcs5ghpo[7] = sprom->mcs5ghpo[7];
 			break;
 		}
 	}
@@ -14664,45 +14559,34 @@ static void wlc_phy_txpwr_srom_read_ppr_nphy(struct brcms_phy *pi)
 
 static bool wlc_phy_txpwr_srom_read_nphy(struct brcms_phy *pi)
 {
-	struct phy_shim_info *shim = pi->sh->physhim;
-
-	pi->antswitch = (u8) wlapi_getintvar(shim, BRCMS_SROM_ANTSWITCH);
-	pi->aa2g = (u8) wlapi_getintvar(shim, BRCMS_SROM_AA2G);
-	pi->aa5g = (u8) wlapi_getintvar(shim, BRCMS_SROM_AA5G);
-
-	pi->srom_fem2g.tssipos = (u8) wlapi_getintvar(shim,
-						      BRCMS_SROM_TSSIPOS2G);
-	pi->srom_fem2g.extpagain = (u8) wlapi_getintvar(shim,
-							BRCMS_SROM_EXTPAGAIN2G);
-	pi->srom_fem2g.pdetrange = (u8) wlapi_getintvar(shim,
-							BRCMS_SROM_PDETRANGE2G);
-	pi->srom_fem2g.triso = (u8) wlapi_getintvar(shim, BRCMS_SROM_TRISO2G);
-	pi->srom_fem2g.antswctrllut =
-			(u8) wlapi_getintvar(shim, BRCMS_SROM_ANTSWCTL2G);
-
-	pi->srom_fem5g.tssipos = (u8) wlapi_getintvar(shim,
-						      BRCMS_SROM_TSSIPOS5G);
-	pi->srom_fem5g.extpagain = (u8) wlapi_getintvar(shim,
-							BRCMS_SROM_EXTPAGAIN5G);
-	pi->srom_fem5g.pdetrange = (u8) wlapi_getintvar(shim,
-							BRCMS_SROM_PDETRANGE5G);
-	pi->srom_fem5g.triso = (u8) wlapi_getintvar(shim, BRCMS_SROM_TRISO5G);
-	if (wlapi_getvar(shim, BRCMS_SROM_ANTSWCTL5G))
-		pi->srom_fem5g.antswctrllut =
-			(u8) wlapi_getintvar(shim, BRCMS_SROM_ANTSWCTL5G);
+	struct ssb_sprom *sprom = &pi->d11core->bus->sprom;
+
+	pi->antswitch = sprom->antswitch;
+	pi->aa2g = sprom->ant_available_bg;
+	pi->aa5g = sprom->ant_available_a;
+
+	pi->srom_fem2g.tssipos = sprom->fem.ghz2.tssipos;
+	pi->srom_fem2g.extpagain = sprom->fem.ghz2.extpa_gain;
+	pi->srom_fem2g.pdetrange = sprom->fem.ghz2.pdet_range;
+	pi->srom_fem2g.triso = sprom->fem.ghz2.tr_iso;
+	pi->srom_fem2g.antswctrllut = sprom->fem.ghz2.antswlut;
+
+	pi->srom_fem5g.tssipos = sprom->fem.ghz5.tssipos;
+	pi->srom_fem5g.extpagain = sprom->fem.ghz5.extpa_gain;
+	pi->srom_fem5g.pdetrange = sprom->fem.ghz5.pdet_range;
+	pi->srom_fem5g.triso = sprom->fem.ghz5.tr_iso;
+	if (sprom->fem.ghz5.antswlut)
+		pi->srom_fem5g.antswctrllut = sprom->fem.ghz5.antswlut;
 	else
-		pi->srom_fem5g.antswctrllut =
-			(u8) wlapi_getintvar(shim, BRCMS_SROM_ANTSWCTL2G);
+		pi->srom_fem5g.antswctrllut = sprom->fem.ghz2.antswlut;
 
 	wlc_phy_txpower_ipa_upd(pi);
 
-	pi->phy_txcore_disable_temp =
-			(s16) wlapi_getintvar(shim, BRCMS_SROM_TEMPTHRESH);
+	pi->phy_txcore_disable_temp = sprom->tempthresh;
 	if (pi->phy_txcore_disable_temp == 0)
 		pi->phy_txcore_disable_temp = PHY_CHAIN_TX_DISABLE_TEMP;
 
-	pi->phy_tempsense_offset = (s8) wlapi_getintvar(shim,
-							BRCMS_SROM_TEMPOFFSET);
+	pi->phy_tempsense_offset = sprom->tempoffset;
 	if (pi->phy_tempsense_offset != 0) {
 		if (pi->phy_tempsense_offset >
 		    (NPHY_SROM_TEMPSHIFT + NPHY_SROM_MAXTEMPOFFSET))
@@ -14717,8 +14601,7 @@ static bool wlc_phy_txpwr_srom_read_nphy(struct brcms_phy *pi)
 	pi->phy_txcore_enable_temp =
 		pi->phy_txcore_disable_temp - PHY_HYSTERESIS_DELTATEMP;
 
-	pi->phycal_tempdelta =
-			(u8) wlapi_getintvar(shim, BRCMS_SROM_PHYCAL_TEMPDELTA);
+	pi->phycal_tempdelta = sprom->phycal_tempdelta;
 	if (pi->phycal_tempdelta > NPHY_CAL_MAXTEMPDELTA)
 		pi->phycal_tempdelta = 0;
 
@@ -16353,11 +16236,7 @@ static void wlc_phy_workarounds_nphy(struct brcms_phy *pi)
 			wlc_phy_set_rfseq_nphy(pi, NPHY_RFSEQ_RX2TX,
 					       rfseq_rx2tx_events_rev3_ipa,
 					       rfseq_rx2tx_dlys_rev3_ipa,
-					       sizeof
-					       (rfseq_rx2tx_events_rev3_ipa) /
-					       sizeof
-					       (rfseq_rx2tx_events_rev3_ipa
-						[0]));
+					       ARRAY_SIZE(rfseq_rx2tx_events_rev3_ipa));
 
 		mod_phy_reg(pi, 0x299, (0x3 << 14), (0x1 << 14));
 		mod_phy_reg(pi, 0x29d, (0x3 << 14), (0x1 << 14));
@@ -16858,18 +16737,13 @@ static void wlc_phy_workarounds_nphy(struct brcms_phy *pi)
 		wlc_phy_set_rfseq_nphy(pi, NPHY_RFSEQ_TX2RX,
 				       rfseq_tx2rx_events_rev3,
 				       rfseq_tx2rx_dlys_rev3,
-				       sizeof(rfseq_tx2rx_events_rev3) /
-				       sizeof(rfseq_tx2rx_events_rev3[0]));
+				       ARRAY_SIZE(rfseq_tx2rx_events_rev3));
 
 		if (PHY_IPA(pi))
 			wlc_phy_set_rfseq_nphy(pi, NPHY_RFSEQ_RX2TX,
 					       rfseq_rx2tx_events_rev3_ipa,
 					       rfseq_rx2tx_dlys_rev3_ipa,
-					       sizeof
-					       (rfseq_rx2tx_events_rev3_ipa) /
-					       sizeof
-					       (rfseq_rx2tx_events_rev3_ipa
-						[0]));
+					       ARRAY_SIZE(rfseq_rx2tx_events_rev3_ipa));
 
 		if ((pi->sh->hw_phyrxchain != 0x3) &&
 		    (pi->sh->hw_phyrxchain != pi->sh->hw_phytxchain)) {
@@ -16885,8 +16759,7 @@ static void wlc_phy_workarounds_nphy(struct brcms_phy *pi)
 				pi, NPHY_RFSEQ_RX2TX,
 				rfseq_rx2tx_events_rev3,
 				rfseq_rx2tx_dlys_rev3,
-				sizeof(rfseq_rx2tx_events_rev3)	/
-				sizeof(rfseq_rx2tx_events_rev3[0]));
+				ARRAY_SIZE(rfseq_rx2tx_events_rev3));
 		}
 
 		if (CHSPEC_IS2G(pi->radio_chanspec))
@@ -17209,13 +17082,11 @@ static void wlc_phy_workarounds_nphy(struct brcms_phy *pi)
 
 		wlc_phy_set_rfseq_nphy(pi, NPHY_RFSEQ_RX2TX, rfseq_rx2tx_events,
 				       rfseq_rx2tx_dlys,
-				       sizeof(rfseq_rx2tx_events) /
-				       sizeof(rfseq_rx2tx_events[0]));
+				       ARRAY_SIZE(rfseq_rx2tx_events));
 
 		wlc_phy_set_rfseq_nphy(pi, NPHY_RFSEQ_TX2RX, rfseq_tx2rx_events,
 				       rfseq_tx2rx_dlys,
-				       sizeof(rfseq_tx2rx_events) /
-				       sizeof(rfseq_tx2rx_events[0]));
+				       ARRAY_SIZE(rfseq_tx2rx_events));
 
 		wlc_phy_workarounds_nphy_gainctrl(pi);
 
@@ -19357,8 +19228,7 @@ static void wlc_phy_spurwar_nphy(struct brcms_phy *pi)
 			}
 
 			if (isAdjustNoiseVar) {
-				numTonesAdjust = sizeof(nphy_adj_tone_id_buf) /
-						sizeof(nphy_adj_tone_id_buf[0]);
+				numTonesAdjust = ARRAY_SIZE(nphy_adj_tone_id_buf);
 
 				wlc_phy_adjust_min_noisevar_nphy(
 					pi,
@@ -21473,7 +21343,7 @@ void wlc_phy_antsel_init(struct brcms_phy_pub *ppi, bool lut_init)
 		write_phy_reg(pi, 0xc8, 0x0);
 		write_phy_reg(pi, 0xc9, 0x0);
 
-		ai_gpiocontrol(pi->sh->sih, mask, mask, GPIO_DRV_PRIORITY);
+		bcma_chipco_gpio_control(&pi->d11core->bus->drv_cc, mask, mask);
 
 		mc = bcma_read32(pi->d11core, D11REGOFFS(maccontrol));
 		mc &= ~MCTL_GPOUT_SEL_MASK;
@@ -25204,32 +25074,26 @@ static u8 wlc_phy_a3_nphy(struct brcms_phy *pi, u8 start_gain, u8 core)
 
 				phy_a15 = pad_gain_codes_used_2057rev5;
 				phy_a13 =
-					sizeof(pad_gain_codes_used_2057rev5) /
-					sizeof(pad_gain_codes_used_2057rev5
-						[0]) - 1;
+					ARRAY_SIZE(pad_gain_codes_used_2057rev5) - 1;
 
 			} else if ((pi->pubpi.radiorev == 7)
 				   || (pi->pubpi.radiorev == 8)) {
 
 				phy_a15 = pad_gain_codes_used_2057rev7;
 				phy_a13 =
-					sizeof(pad_gain_codes_used_2057rev7) /
-					sizeof(pad_gain_codes_used_2057rev7
-						[0]) - 1;
+					ARRAY_SIZE(pad_gain_codes_used_2057rev7) - 1;
 
 			} else {
 
 				phy_a15 = pad_all_gain_codes_2057;
-				phy_a13 = sizeof(pad_all_gain_codes_2057) /
-					  sizeof(pad_all_gain_codes_2057[0]) -
+				phy_a13 = ARRAY_SIZE(pad_all_gain_codes_2057) -
 					  1;
 			}
 
 		} else {
 
 			phy_a15 = pga_all_gain_codes_2057;
-			phy_a13 = sizeof(pga_all_gain_codes_2057) /
-				  sizeof(pga_all_gain_codes_2057[0]) - 1;
+			phy_a13 = ARRAY_SIZE(pga_all_gain_codes_2057) - 1;
 		}
 
 		phy_a14 = 0;
diff --git a/drivers/net/wireless/brcm80211/brcmsmac/phy_shim.c b/drivers/net/wireless/brcm80211/brcmsmac/phy_shim.c
index 5926854f62e..a0de5db0cd6 100644
--- a/drivers/net/wireless/brcm80211/brcmsmac/phy_shim.c
+++ b/drivers/net/wireless/brcm80211/brcmsmac/phy_shim.c
@@ -214,12 +214,3 @@ wlapi_copyto_objmem(struct phy_shim_info *physhim, uint offset, const void *buf,
 {
 	brcms_b_copyto_objmem(physhim->wlc_hw, offset, buf, l, sel);
 }
-
-char *wlapi_getvar(struct phy_shim_info *physhim, enum brcms_srom_id id)
-{
-	return getvar(physhim->wlc_hw->sih, id);
-}
-int wlapi_getintvar(struct phy_shim_info *physhim, enum brcms_srom_id id)
-{
-	return getintvar(physhim->wlc_hw->sih, id);
-}
diff --git a/drivers/net/wireless/brcm80211/brcmsmac/phy_shim.h b/drivers/net/wireless/brcm80211/brcmsmac/phy_shim.h
index 9168c459b18..2c5b66b7597 100644
--- a/drivers/net/wireless/brcm80211/brcmsmac/phy_shim.h
+++ b/drivers/net/wireless/brcm80211/brcmsmac/phy_shim.h
@@ -175,8 +175,5 @@ extern void wlapi_copyto_objmem(struct phy_shim_info *physhim, uint,
 extern void wlapi_high_update_phy_mode(struct phy_shim_info *physhim,
 				       u32 phy_mode);
 extern u16 wlapi_bmac_get_txant(struct phy_shim_info *physhim);
-extern char *wlapi_getvar(struct phy_shim_info *physhim, enum brcms_srom_id id);
-extern int wlapi_getintvar(struct phy_shim_info *physhim,
-			   enum brcms_srom_id id);
 
 #endif				/* _BRCM_PHY_SHIM_H_ */
diff --git a/drivers/net/wireless/brcm80211/brcmsmac/pub.h b/drivers/net/wireless/brcm80211/brcmsmac/pub.h
index f0038ad7d7b..aa5d67f8d87 100644
--- a/drivers/net/wireless/brcm80211/brcmsmac/pub.h
+++ b/drivers/net/wireless/brcm80211/brcmsmac/pub.h
@@ -22,232 +22,6 @@
 #include "types.h"
 #include "defs.h"
 
-enum brcms_srom_id {
-	BRCMS_SROM_NULL,
-	BRCMS_SROM_CONT,
-	BRCMS_SROM_AA2G,
-	BRCMS_SROM_AA5G,
-	BRCMS_SROM_AG0,
-	BRCMS_SROM_AG1,
-	BRCMS_SROM_AG2,
-	BRCMS_SROM_AG3,
-	BRCMS_SROM_ANTSWCTL2G,
-	BRCMS_SROM_ANTSWCTL5G,
-	BRCMS_SROM_ANTSWITCH,
-	BRCMS_SROM_BOARDFLAGS2,
-	BRCMS_SROM_BOARDFLAGS,
-	BRCMS_SROM_BOARDNUM,
-	BRCMS_SROM_BOARDREV,
-	BRCMS_SROM_BOARDTYPE,
-	BRCMS_SROM_BW40PO,
-	BRCMS_SROM_BWDUPPO,
-	BRCMS_SROM_BXA2G,
-	BRCMS_SROM_BXA5G,
-	BRCMS_SROM_CC,
-	BRCMS_SROM_CCK2GPO,
-	BRCMS_SROM_CCKBW202GPO,
-	BRCMS_SROM_CCKBW20UL2GPO,
-	BRCMS_SROM_CCODE,
-	BRCMS_SROM_CDDPO,
-	BRCMS_SROM_DEVID,
-	BRCMS_SROM_ET1MACADDR,
-	BRCMS_SROM_EXTPAGAIN2G,
-	BRCMS_SROM_EXTPAGAIN5G,
-	BRCMS_SROM_FREQOFFSET_CORR,
-	BRCMS_SROM_HW_IQCAL_EN,
-	BRCMS_SROM_IL0MACADDR,
-	BRCMS_SROM_IQCAL_SWP_DIS,
-	BRCMS_SROM_LEDBH0,
-	BRCMS_SROM_LEDBH1,
-	BRCMS_SROM_LEDBH2,
-	BRCMS_SROM_LEDBH3,
-	BRCMS_SROM_LEDDC,
-	BRCMS_SROM_LEGOFDM40DUPPO,
-	BRCMS_SROM_LEGOFDMBW202GPO,
-	BRCMS_SROM_LEGOFDMBW205GHPO,
-	BRCMS_SROM_LEGOFDMBW205GLPO,
-	BRCMS_SROM_LEGOFDMBW205GMPO,
-	BRCMS_SROM_LEGOFDMBW20UL2GPO,
-	BRCMS_SROM_LEGOFDMBW20UL5GHPO,
-	BRCMS_SROM_LEGOFDMBW20UL5GLPO,
-	BRCMS_SROM_LEGOFDMBW20UL5GMPO,
-	BRCMS_SROM_MACADDR,
-	BRCMS_SROM_MCS2GPO0,
-	BRCMS_SROM_MCS2GPO1,
-	BRCMS_SROM_MCS2GPO2,
-	BRCMS_SROM_MCS2GPO3,
-	BRCMS_SROM_MCS2GPO4,
-	BRCMS_SROM_MCS2GPO5,
-	BRCMS_SROM_MCS2GPO6,
-	BRCMS_SROM_MCS2GPO7,
-	BRCMS_SROM_MCS32PO,
-	BRCMS_SROM_MCS5GHPO0,
-	BRCMS_SROM_MCS5GHPO1,
-	BRCMS_SROM_MCS5GHPO2,
-	BRCMS_SROM_MCS5GHPO3,
-	BRCMS_SROM_MCS5GHPO4,
-	BRCMS_SROM_MCS5GHPO5,
-	BRCMS_SROM_MCS5GHPO6,
-	BRCMS_SROM_MCS5GHPO7,
-	BRCMS_SROM_MCS5GLPO0,
-	BRCMS_SROM_MCS5GLPO1,
-	BRCMS_SROM_MCS5GLPO2,
-	BRCMS_SROM_MCS5GLPO3,
-	BRCMS_SROM_MCS5GLPO4,
-	BRCMS_SROM_MCS5GLPO5,
-	BRCMS_SROM_MCS5GLPO6,
-	BRCMS_SROM_MCS5GLPO7,
-	BRCMS_SROM_MCS5GPO0,
-	BRCMS_SROM_MCS5GPO1,
-	BRCMS_SROM_MCS5GPO2,
-	BRCMS_SROM_MCS5GPO3,
-	BRCMS_SROM_MCS5GPO4,
-	BRCMS_SROM_MCS5GPO5,
-	BRCMS_SROM_MCS5GPO6,
-	BRCMS_SROM_MCS5GPO7,
-	BRCMS_SROM_MCSBW202GPO,
-	BRCMS_SROM_MCSBW205GHPO,
-	BRCMS_SROM_MCSBW205GLPO,
-	BRCMS_SROM_MCSBW205GMPO,
-	BRCMS_SROM_MCSBW20UL2GPO,
-	BRCMS_SROM_MCSBW20UL5GHPO,
-	BRCMS_SROM_MCSBW20UL5GLPO,
-	BRCMS_SROM_MCSBW20UL5GMPO,
-	BRCMS_SROM_MCSBW402GPO,
-	BRCMS_SROM_MCSBW405GHPO,
-	BRCMS_SROM_MCSBW405GLPO,
-	BRCMS_SROM_MCSBW405GMPO,
-	BRCMS_SROM_MEASPOWER,
-	BRCMS_SROM_OFDM2GPO,
-	BRCMS_SROM_OFDM5GHPO,
-	BRCMS_SROM_OFDM5GLPO,
-	BRCMS_SROM_OFDM5GPO,
-	BRCMS_SROM_OPO,
-	BRCMS_SROM_PA0B0,
-	BRCMS_SROM_PA0B1,
-	BRCMS_SROM_PA0B2,
-	BRCMS_SROM_PA0ITSSIT,
-	BRCMS_SROM_PA0MAXPWR,
-	BRCMS_SROM_PA1B0,
-	BRCMS_SROM_PA1B1,
-	BRCMS_SROM_PA1B2,
-	BRCMS_SROM_PA1HIB0,
-	BRCMS_SROM_PA1HIB1,
-	BRCMS_SROM_PA1HIB2,
-	BRCMS_SROM_PA1HIMAXPWR,
-	BRCMS_SROM_PA1ITSSIT,
-	BRCMS_SROM_PA1LOB0,
-	BRCMS_SROM_PA1LOB1,
-	BRCMS_SROM_PA1LOB2,
-	BRCMS_SROM_PA1LOMAXPWR,
-	BRCMS_SROM_PA1MAXPWR,
-	BRCMS_SROM_PDETRANGE2G,
-	BRCMS_SROM_PDETRANGE5G,
-	BRCMS_SROM_PHYCAL_TEMPDELTA,
-	BRCMS_SROM_RAWTEMPSENSE,
-	BRCMS_SROM_REGREV,
-	BRCMS_SROM_REV,
-	BRCMS_SROM_RSSISAV2G,
-	BRCMS_SROM_RSSISAV5G,
-	BRCMS_SROM_RSSISMC2G,
-	BRCMS_SROM_RSSISMC5G,
-	BRCMS_SROM_RSSISMF2G,
-	BRCMS_SROM_RSSISMF5G,
-	BRCMS_SROM_RXCHAIN,
-	BRCMS_SROM_RXPO2G,
-	BRCMS_SROM_RXPO5G,
-	BRCMS_SROM_STBCPO,
-	BRCMS_SROM_TEMPCORRX,
-	BRCMS_SROM_TEMPOFFSET,
-	BRCMS_SROM_TEMPSENSE_OPTION,
-	BRCMS_SROM_TEMPSENSE_SLOPE,
-	BRCMS_SROM_TEMPTHRESH,
-	BRCMS_SROM_TRI2G,
-	BRCMS_SROM_TRI5GH,
-	BRCMS_SROM_TRI5GL,
-	BRCMS_SROM_TRI5G,
-	BRCMS_SROM_TRISO2G,
-	BRCMS_SROM_TRISO5G,
-	BRCMS_SROM_TSSIPOS2G,
-	BRCMS_SROM_TSSIPOS5G,
-	BRCMS_SROM_TXCHAIN,
-	/*
-	 * per-path identifiers (see srom.c)
-	 */
-	BRCMS_SROM_ITT2GA0,
-	BRCMS_SROM_ITT2GA1,
-	BRCMS_SROM_ITT2GA2,
-	BRCMS_SROM_ITT2GA3,
-	BRCMS_SROM_ITT5GA0,
-	BRCMS_SROM_ITT5GA1,
-	BRCMS_SROM_ITT5GA2,
-	BRCMS_SROM_ITT5GA3,
-	BRCMS_SROM_MAXP2GA0,
-	BRCMS_SROM_MAXP2GA1,
-	BRCMS_SROM_MAXP2GA2,
-	BRCMS_SROM_MAXP2GA3,
-	BRCMS_SROM_MAXP5GA0,
-	BRCMS_SROM_MAXP5GA1,
-	BRCMS_SROM_MAXP5GA2,
-	BRCMS_SROM_MAXP5GA3,
-	BRCMS_SROM_MAXP5GHA0,
-	BRCMS_SROM_MAXP5GHA1,
-	BRCMS_SROM_MAXP5GHA2,
-	BRCMS_SROM_MAXP5GHA3,
-	BRCMS_SROM_MAXP5GLA0,
-	BRCMS_SROM_MAXP5GLA1,
-	BRCMS_SROM_MAXP5GLA2,
-	BRCMS_SROM_MAXP5GLA3,
-	BRCMS_SROM_PA2GW0A0,
-	BRCMS_SROM_PA2GW0A1,
-	BRCMS_SROM_PA2GW0A2,
-	BRCMS_SROM_PA2GW0A3,
-	BRCMS_SROM_PA2GW1A0,
-	BRCMS_SROM_PA2GW1A1,
-	BRCMS_SROM_PA2GW1A2,
-	BRCMS_SROM_PA2GW1A3,
-	BRCMS_SROM_PA2GW2A0,
-	BRCMS_SROM_PA2GW2A1,
-	BRCMS_SROM_PA2GW2A2,
-	BRCMS_SROM_PA2GW2A3,
-	BRCMS_SROM_PA5GHW0A0,
-	BRCMS_SROM_PA5GHW0A1,
-	BRCMS_SROM_PA5GHW0A2,
-	BRCMS_SROM_PA5GHW0A3,
-	BRCMS_SROM_PA5GHW1A0,
-	BRCMS_SROM_PA5GHW1A1,
-	BRCMS_SROM_PA5GHW1A2,
-	BRCMS_SROM_PA5GHW1A3,
-	BRCMS_SROM_PA5GHW2A0,
-	BRCMS_SROM_PA5GHW2A1,
-	BRCMS_SROM_PA5GHW2A2,
-	BRCMS_SROM_PA5GHW2A3,
-	BRCMS_SROM_PA5GLW0A0,
-	BRCMS_SROM_PA5GLW0A1,
-	BRCMS_SROM_PA5GLW0A2,
-	BRCMS_SROM_PA5GLW0A3,
-	BRCMS_SROM_PA5GLW1A0,
-	BRCMS_SROM_PA5GLW1A1,
-	BRCMS_SROM_PA5GLW1A2,
-	BRCMS_SROM_PA5GLW1A3,
-	BRCMS_SROM_PA5GLW2A0,
-	BRCMS_SROM_PA5GLW2A1,
-	BRCMS_SROM_PA5GLW2A2,
-	BRCMS_SROM_PA5GLW2A3,
-	BRCMS_SROM_PA5GW0A0,
-	BRCMS_SROM_PA5GW0A1,
-	BRCMS_SROM_PA5GW0A2,
-	BRCMS_SROM_PA5GW0A3,
-	BRCMS_SROM_PA5GW1A0,
-	BRCMS_SROM_PA5GW1A1,
-	BRCMS_SROM_PA5GW1A2,
-	BRCMS_SROM_PA5GW1A3,
-	BRCMS_SROM_PA5GW2A0,
-	BRCMS_SROM_PA5GW2A1,
-	BRCMS_SROM_PA5GW2A2,
-	BRCMS_SROM_PA5GW2A3,
-};
-
 #define	BRCMS_NUMRATES	16	/* max # of rates in a rateset */
 
 /* phy types */
@@ -565,8 +339,6 @@ extern void brcms_c_ampdu_flush(struct brcms_c_info *wlc,
 			    struct ieee80211_sta *sta, u16 tid);
 extern void brcms_c_ampdu_tx_operational(struct brcms_c_info *wlc, u8 tid,
 					 u8 ba_wsize, uint max_rx_ampdu_bytes);
-extern char *getvar(struct si_pub *sih, enum brcms_srom_id id);
-extern int getintvar(struct si_pub *sih, enum brcms_srom_id id);
 extern int brcms_c_module_register(struct brcms_pub *pub,
 				   const char *name, struct brcms_info *hdl,
 				   int (*down_fn)(void *handle));
diff --git a/drivers/net/wireless/brcm80211/brcmsmac/srom.c b/drivers/net/wireless/brcm80211/brcmsmac/srom.c
deleted file mode 100644
index b96f4b9d74b..00000000000
--- a/drivers/net/wireless/brcm80211/brcmsmac/srom.c
+++ /dev/null
@@ -1,980 +0,0 @@
-/*
- * 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/kernel.h>
-#include <linux/string.h>
-#include <linux/io.h>
-#include <linux/etherdevice.h>
-#include <linux/crc8.h>
-#include <stdarg.h>
-
-#include <chipcommon.h>
-#include <brcmu_utils.h>
-#include "pub.h"
-#include "nicpci.h"
-#include "aiutils.h"
-#include "otp.h"
-#include "srom.h"
-#include "soc.h"
-
-/*
- * SROM CRC8 polynomial value:
- *
- * x^8 + x^7 +x^6 + x^4 + x^2 + 1
- */
-#define SROM_CRC8_POLY		0xAB
-
-/* Maximum srom: 6 Kilobits == 768 bytes */
-#define	SROM_MAX		768
-
-/* PCI fields */
-#define PCI_F0DEVID		48
-
-#define	SROM_WORDS		64
-
-#define	SROM_SSID		2
-
-#define	SROM_WL1LHMAXP		29
-
-#define	SROM_WL1LPAB0		30
-#define	SROM_WL1LPAB1		31
-#define	SROM_WL1LPAB2		32
-
-#define	SROM_WL1HPAB0		33
-#define	SROM_WL1HPAB1		34
-#define	SROM_WL1HPAB2		35
-
-#define	SROM_MACHI_IL0		36
-#define	SROM_MACMID_IL0		37
-#define	SROM_MACLO_IL0		38
-#define	SROM_MACHI_ET1		42
-#define	SROM_MACMID_ET1		43
-#define	SROM_MACLO_ET1		44
-
-#define	SROM_BXARSSI2G		40
-#define	SROM_BXARSSI5G		41
-
-#define	SROM_TRI52G		42
-#define	SROM_TRI5GHL		43
-
-#define	SROM_RXPO52G		45
-
-#define	SROM_AABREV		46
-/* Fields in AABREV */
-#define	SROM_BR_MASK		0x00ff
-#define	SROM_CC_MASK		0x0f00
-#define	SROM_CC_SHIFT		8
-#define	SROM_AA0_MASK		0x3000
-#define	SROM_AA0_SHIFT		12
-#define	SROM_AA1_MASK		0xc000
-#define	SROM_AA1_SHIFT		14
-
-#define	SROM_WL0PAB0		47
-#define	SROM_WL0PAB1		48
-#define	SROM_WL0PAB2		49
-
-#define	SROM_LEDBH10		50
-#define	SROM_LEDBH32		51
-
-#define	SROM_WL10MAXP		52
-
-#define	SROM_WL1PAB0		53
-#define	SROM_WL1PAB1		54
-#define	SROM_WL1PAB2		55
-
-#define	SROM_ITT		56
-
-#define	SROM_BFL		57
-#define	SROM_BFL2		28
-
-#define	SROM_AG10		58
-
-#define	SROM_CCODE		59
-
-#define	SROM_OPO		60
-
-#define	SROM_CRCREV		63
-
-#define	SROM4_WORDS		220
-
-#define SROM4_TXCHAIN_MASK	0x000f
-#define SROM4_RXCHAIN_MASK	0x00f0
-#define SROM4_SWITCH_MASK	0xff00
-
-/* Per-path fields */
-#define	MAX_PATH_SROM		4
-
-#define	SROM4_CRCREV		219
-
-/* SROM Rev 8: Make space for a 48word hardware header for PCIe rev >= 6.
- * This is acombined srom for both MIMO and SISO boards, usable in
- * the .130 4Kilobit OTP with hardware redundancy.
- */
-#define	SROM8_BREV		65
-
-#define	SROM8_BFL0		66
-#define	SROM8_BFL1		67
-#define	SROM8_BFL2		68
-#define	SROM8_BFL3		69
-
-#define	SROM8_MACHI		70
-#define	SROM8_MACMID		71
-#define	SROM8_MACLO		72
-
-#define	SROM8_CCODE		73
-#define	SROM8_REGREV		74
-
-#define	SROM8_LEDBH10		75
-#define	SROM8_LEDBH32		76
-
-#define	SROM8_LEDDC		77
-
-#define	SROM8_AA		78
-
-#define	SROM8_AG10		79
-#define	SROM8_AG32		80
-
-#define	SROM8_TXRXC		81
-
-#define	SROM8_BXARSSI2G		82
-#define	SROM8_BXARSSI5G		83
-#define	SROM8_TRI52G		84
-#define	SROM8_TRI5GHL		85
-#define	SROM8_RXPO52G		86
-
-#define SROM8_FEM2G		87
-#define SROM8_FEM5G		88
-#define SROM8_FEM_ANTSWLUT_MASK		0xf800
-#define SROM8_FEM_ANTSWLUT_SHIFT	11
-#define SROM8_FEM_TR_ISO_MASK		0x0700
-#define SROM8_FEM_TR_ISO_SHIFT		8
-#define SROM8_FEM_PDET_RANGE_MASK	0x00f8
-#define SROM8_FEM_PDET_RANGE_SHIFT	3
-#define SROM8_FEM_EXTPA_GAIN_MASK	0x0006
-#define SROM8_FEM_EXTPA_GAIN_SHIFT	1
-#define SROM8_FEM_TSSIPOS_MASK		0x0001
-#define SROM8_FEM_TSSIPOS_SHIFT		0
-
-#define SROM8_THERMAL		89
-
-/* Temp sense related entries */
-#define SROM8_MPWR_RAWTS		90
-#define SROM8_TS_SLP_OPT_CORRX	91
-/* FOC: freiquency offset correction, HWIQ: H/W IOCAL enable,
- * IQSWP: IQ CAL swap disable */
-#define SROM8_FOC_HWIQ_IQSWP	92
-
-/* Temperature delta for PHY calibration */
-#define SROM8_PHYCAL_TEMPDELTA	93
-
-/* Per-path offsets & fields */
-#define	SROM8_PATH0		96
-#define	SROM8_PATH1		112
-#define	SROM8_PATH2		128
-#define	SROM8_PATH3		144
-
-#define	SROM8_2G_ITT_MAXP	0
-#define	SROM8_2G_PA		1
-#define	SROM8_5G_ITT_MAXP	4
-#define	SROM8_5GLH_MAXP		5
-#define	SROM8_5G_PA		6
-#define	SROM8_5GL_PA		9
-#define	SROM8_5GH_PA		12
-
-/* All the miriad power offsets */
-#define	SROM8_2G_CCKPO		160
-
-#define	SROM8_2G_OFDMPO		161
-#define	SROM8_5G_OFDMPO		163
-#define	SROM8_5GL_OFDMPO	165
-#define	SROM8_5GH_OFDMPO	167
-
-#define	SROM8_2G_MCSPO		169
-#define	SROM8_5G_MCSPO		177
-#define	SROM8_5GL_MCSPO		185
-#define	SROM8_5GH_MCSPO		193
-
-#define	SROM8_CDDPO		201
-#define	SROM8_STBCPO		202
-#define	SROM8_BW40PO		203
-#define	SROM8_BWDUPPO		204
-
-/* SISO PA parameters are in the path0 spaces */
-#define	SROM8_SISO		96
-
-/* Legacy names for SISO PA paramters */
-#define	SROM8_W0_ITTMAXP	(SROM8_SISO + SROM8_2G_ITT_MAXP)
-#define	SROM8_W0_PAB0		(SROM8_SISO + SROM8_2G_PA)
-#define	SROM8_W0_PAB1		(SROM8_SISO + SROM8_2G_PA + 1)
-#define	SROM8_W0_PAB2		(SROM8_SISO + SROM8_2G_PA + 2)
-#define	SROM8_W1_ITTMAXP	(SROM8_SISO + SROM8_5G_ITT_MAXP)
-#define	SROM8_W1_MAXP_LCHC	(SROM8_SISO + SROM8_5GLH_MAXP)
-#define	SROM8_W1_PAB0		(SROM8_SISO + SROM8_5G_PA)
-#define	SROM8_W1_PAB1		(SROM8_SISO + SROM8_5G_PA + 1)
-#define	SROM8_W1_PAB2		(SROM8_SISO + SROM8_5G_PA + 2)
-#define	SROM8_W1_PAB0_LC	(SROM8_SISO + SROM8_5GL_PA)
-#define	SROM8_W1_PAB1_LC	(SROM8_SISO + SROM8_5GL_PA + 1)
-#define	SROM8_W1_PAB2_LC	(SROM8_SISO + SROM8_5GL_PA + 2)
-#define	SROM8_W1_PAB0_HC	(SROM8_SISO + SROM8_5GH_PA)
-#define	SROM8_W1_PAB1_HC	(SROM8_SISO + SROM8_5GH_PA + 1)
-#define	SROM8_W1_PAB2_HC	(SROM8_SISO + SROM8_5GH_PA + 2)
-
-/* SROM REV 9 */
-#define SROM9_2GPO_CCKBW20	160
-#define SROM9_2GPO_CCKBW20UL	161
-#define SROM9_2GPO_LOFDMBW20	162
-#define SROM9_2GPO_LOFDMBW20UL	164
-
-#define SROM9_5GLPO_LOFDMBW20	166
-#define SROM9_5GLPO_LOFDMBW20UL	168
-#define SROM9_5GMPO_LOFDMBW20	170
-#define SROM9_5GMPO_LOFDMBW20UL	172
-#define SROM9_5GHPO_LOFDMBW20	174
-#define SROM9_5GHPO_LOFDMBW20UL	176
-
-#define SROM9_2GPO_MCSBW20	178
-#define SROM9_2GPO_MCSBW20UL	180
-#define SROM9_2GPO_MCSBW40	182
-
-#define SROM9_5GLPO_MCSBW20	184
-#define SROM9_5GLPO_MCSBW20UL	186
-#define SROM9_5GLPO_MCSBW40	188
-#define SROM9_5GMPO_MCSBW20	190
-#define SROM9_5GMPO_MCSBW20UL	192
-#define SROM9_5GMPO_MCSBW40	194
-#define SROM9_5GHPO_MCSBW20	196
-#define SROM9_5GHPO_MCSBW20UL	198
-#define SROM9_5GHPO_MCSBW40	200
-
-#define SROM9_PO_MCS32		202
-#define SROM9_PO_LOFDM40DUP	203
-
-/* SROM flags (see sromvar_t) */
-
-/* value continues as described by the next entry */
-#define SRFL_MORE	1
-#define	SRFL_NOFFS	2	/* value bits can't be all one's */
-#define	SRFL_PRHEX	4	/* value is in hexdecimal format */
-#define	SRFL_PRSIGN	8	/* value is in signed decimal format */
-#define	SRFL_CCODE	0x10	/* value is in country code format */
-#define	SRFL_ETHADDR	0x20	/* value is an Ethernet address */
-#define SRFL_LEDDC	0x40	/* value is an LED duty cycle */
-/* do not generate a nvram param, entry is for mfgc */
-#define SRFL_NOVAR	0x80
-
-/* Max. nvram variable table size */
-#define	MAXSZ_NVRAM_VARS	4096
-
-/*
- * indicates type of value.
- */
-enum brcms_srom_var_type {
-	BRCMS_SROM_STRING,
-	BRCMS_SROM_SNUMBER,
-	BRCMS_SROM_UNUMBER
-};
-
-/*
- * storage type for srom variable.
- *
- * var_list: for linked list operations.
- * varid: identifier of the variable.
- * var_type: type of variable.
- * buf: variable value when var_type == BRCMS_SROM_STRING.
- * uval: unsigned variable value when var_type == BRCMS_SROM_UNUMBER.
- * sval: signed variable value when var_type == BRCMS_SROM_SNUMBER.
- */
-struct brcms_srom_list_head {
-	struct list_head var_list;
-	enum brcms_srom_id varid;
-	enum brcms_srom_var_type var_type;
-	union {
-		char buf[0];
-		u32 uval;
-		s32 sval;
-	};
-};
-
-struct brcms_sromvar {
-	enum brcms_srom_id varid;
-	u32 revmask;
-	u32 flags;
-	u16 off;
-	u16 mask;
-};
-
-struct brcms_varbuf {
-	char *base;		/* pointer to buffer base */
-	char *buf;		/* pointer to current position */
-	unsigned int size;	/* current (residual) size in bytes */
-};
-
-/*
- * Assumptions:
- * - Ethernet address spans across 3 consecutive words
- *
- * Table rules:
- * - Add multiple entries next to each other if a value spans across multiple
- *   words (even multiple fields in the same word) with each entry except the
- *   last having it's SRFL_MORE bit set.
- * - Ethernet address entry does not follow above rule and must not have
- *   SRFL_MORE bit set. Its SRFL_ETHADDR bit implies it takes multiple words.
- * - The last entry's name field must be NULL to indicate the end of the table.
- *   Other entries must have non-NULL name.
- */
-static const struct brcms_sromvar pci_sromvars[] = {
-	{BRCMS_SROM_DEVID, 0xffffff00, SRFL_PRHEX | SRFL_NOVAR, PCI_F0DEVID,
-	 0xffff},
-	{BRCMS_SROM_BOARDREV, 0xffffff00, SRFL_PRHEX, SROM8_BREV, 0xffff},
-	{BRCMS_SROM_BOARDFLAGS, 0xffffff00, SRFL_PRHEX | SRFL_MORE, SROM8_BFL0,
-	 0xffff},
-	{BRCMS_SROM_CONT, 0, 0, SROM8_BFL1, 0xffff},
-	{BRCMS_SROM_BOARDFLAGS2, 0xffffff00, SRFL_PRHEX | SRFL_MORE, SROM8_BFL2,
-	 0xffff},
-	{BRCMS_SROM_CONT, 0, 0, SROM8_BFL3, 0xffff},
-	{BRCMS_SROM_BOARDTYPE, 0xfffffffc, SRFL_PRHEX, SROM_SSID, 0xffff},
-	{BRCMS_SROM_BOARDNUM, 0xffffff00, 0, SROM8_MACLO, 0xffff},
-	{BRCMS_SROM_REGREV, 0xffffff00, 0, SROM8_REGREV, 0x00ff},
-	{BRCMS_SROM_LEDBH0, 0xffffff00, SRFL_NOFFS, SROM8_LEDBH10, 0x00ff},
-	{BRCMS_SROM_LEDBH1, 0xffffff00, SRFL_NOFFS, SROM8_LEDBH10, 0xff00},
-	{BRCMS_SROM_LEDBH2, 0xffffff00, SRFL_NOFFS, SROM8_LEDBH32, 0x00ff},
-	{BRCMS_SROM_LEDBH3, 0xffffff00, SRFL_NOFFS, SROM8_LEDBH32, 0xff00},
-	{BRCMS_SROM_PA0B0, 0xffffff00, SRFL_PRHEX, SROM8_W0_PAB0, 0xffff},
-	{BRCMS_SROM_PA0B1, 0xffffff00, SRFL_PRHEX, SROM8_W0_PAB1, 0xffff},
-	{BRCMS_SROM_PA0B2, 0xffffff00, SRFL_PRHEX, SROM8_W0_PAB2, 0xffff},
-	{BRCMS_SROM_PA0ITSSIT, 0xffffff00, 0, SROM8_W0_ITTMAXP, 0xff00},
-	{BRCMS_SROM_PA0MAXPWR, 0xffffff00, 0, SROM8_W0_ITTMAXP, 0x00ff},
-	{BRCMS_SROM_OPO, 0xffffff00, 0, SROM8_2G_OFDMPO, 0x00ff},
-	{BRCMS_SROM_AA2G, 0xffffff00, 0, SROM8_AA, 0x00ff},
-	{BRCMS_SROM_AA5G, 0xffffff00, 0, SROM8_AA, 0xff00},
-	{BRCMS_SROM_AG0, 0xffffff00, 0, SROM8_AG10, 0x00ff},
-	{BRCMS_SROM_AG1, 0xffffff00, 0, SROM8_AG10, 0xff00},
-	{BRCMS_SROM_AG2, 0xffffff00, 0, SROM8_AG32, 0x00ff},
-	{BRCMS_SROM_AG3, 0xffffff00, 0, SROM8_AG32, 0xff00},
-	{BRCMS_SROM_PA1B0, 0xffffff00, SRFL_PRHEX, SROM8_W1_PAB0, 0xffff},
-	{BRCMS_SROM_PA1B1, 0xffffff00, SRFL_PRHEX, SROM8_W1_PAB1, 0xffff},
-	{BRCMS_SROM_PA1B2, 0xffffff00, SRFL_PRHEX, SROM8_W1_PAB2, 0xffff},
-	{BRCMS_SROM_PA1LOB0, 0xffffff00, SRFL_PRHEX, SROM8_W1_PAB0_LC, 0xffff},
-	{BRCMS_SROM_PA1LOB1, 0xffffff00, SRFL_PRHEX, SROM8_W1_PAB1_LC, 0xffff},
-	{BRCMS_SROM_PA1LOB2, 0xffffff00, SRFL_PRHEX, SROM8_W1_PAB2_LC, 0xffff},
-	{BRCMS_SROM_PA1HIB0, 0xffffff00, SRFL_PRHEX, SROM8_W1_PAB0_HC, 0xffff},
-	{BRCMS_SROM_PA1HIB1, 0xffffff00, SRFL_PRHEX, SROM8_W1_PAB1_HC, 0xffff},
-	{BRCMS_SROM_PA1HIB2, 0xffffff00, SRFL_PRHEX, SROM8_W1_PAB2_HC, 0xffff},
-	{BRCMS_SROM_PA1ITSSIT, 0xffffff00, 0, SROM8_W1_ITTMAXP, 0xff00},
-	{BRCMS_SROM_PA1MAXPWR, 0xffffff00, 0, SROM8_W1_ITTMAXP, 0x00ff},
-	{BRCMS_SROM_PA1LOMAXPWR, 0xffffff00, 0, SROM8_W1_MAXP_LCHC, 0xff00},
-	{BRCMS_SROM_PA1HIMAXPWR, 0xffffff00, 0, SROM8_W1_MAXP_LCHC, 0x00ff},
-	{BRCMS_SROM_BXA2G, 0xffffff00, 0, SROM8_BXARSSI2G, 0x1800},
-	{BRCMS_SROM_RSSISAV2G, 0xffffff00, 0, SROM8_BXARSSI2G, 0x0700},
-	{BRCMS_SROM_RSSISMC2G, 0xffffff00, 0, SROM8_BXARSSI2G, 0x00f0},
-	{BRCMS_SROM_RSSISMF2G, 0xffffff00, 0, SROM8_BXARSSI2G, 0x000f},
-	{BRCMS_SROM_BXA5G, 0xffffff00, 0, SROM8_BXARSSI5G, 0x1800},
-	{BRCMS_SROM_RSSISAV5G, 0xffffff00, 0, SROM8_BXARSSI5G, 0x0700},
-	{BRCMS_SROM_RSSISMC5G, 0xffffff00, 0, SROM8_BXARSSI5G, 0x00f0},
-	{BRCMS_SROM_RSSISMF5G, 0xffffff00, 0, SROM8_BXARSSI5G, 0x000f},
-	{BRCMS_SROM_TRI2G, 0xffffff00, 0, SROM8_TRI52G, 0x00ff},
-	{BRCMS_SROM_TRI5G, 0xffffff00, 0, SROM8_TRI52G, 0xff00},
-	{BRCMS_SROM_TRI5GL, 0xffffff00, 0, SROM8_TRI5GHL, 0x00ff},
-	{BRCMS_SROM_TRI5GH, 0xffffff00, 0, SROM8_TRI5GHL, 0xff00},
-	{BRCMS_SROM_RXPO2G, 0xffffff00, SRFL_PRSIGN, SROM8_RXPO52G, 0x00ff},
-	{BRCMS_SROM_RXPO5G, 0xffffff00, SRFL_PRSIGN, SROM8_RXPO52G, 0xff00},
-	{BRCMS_SROM_TXCHAIN, 0xffffff00, SRFL_NOFFS, SROM8_TXRXC,
-	 SROM4_TXCHAIN_MASK},
-	{BRCMS_SROM_RXCHAIN, 0xffffff00, SRFL_NOFFS, SROM8_TXRXC,
-	 SROM4_RXCHAIN_MASK},
-	{BRCMS_SROM_ANTSWITCH, 0xffffff00, SRFL_NOFFS, SROM8_TXRXC,
-	 SROM4_SWITCH_MASK},
-	{BRCMS_SROM_TSSIPOS2G, 0xffffff00, 0, SROM8_FEM2G,
-	 SROM8_FEM_TSSIPOS_MASK},
-	{BRCMS_SROM_EXTPAGAIN2G, 0xffffff00, 0, SROM8_FEM2G,
-	 SROM8_FEM_EXTPA_GAIN_MASK},
-	{BRCMS_SROM_PDETRANGE2G, 0xffffff00, 0, SROM8_FEM2G,
-	 SROM8_FEM_PDET_RANGE_MASK},
-	{BRCMS_SROM_TRISO2G, 0xffffff00, 0, SROM8_FEM2G, SROM8_FEM_TR_ISO_MASK},
-	{BRCMS_SROM_ANTSWCTL2G, 0xffffff00, 0, SROM8_FEM2G,
-	 SROM8_FEM_ANTSWLUT_MASK},
-	{BRCMS_SROM_TSSIPOS5G, 0xffffff00, 0, SROM8_FEM5G,
-	 SROM8_FEM_TSSIPOS_MASK},
-	{BRCMS_SROM_EXTPAGAIN5G, 0xffffff00, 0, SROM8_FEM5G,
-	 SROM8_FEM_EXTPA_GAIN_MASK},
-	{BRCMS_SROM_PDETRANGE5G, 0xffffff00, 0, SROM8_FEM5G,
-	 SROM8_FEM_PDET_RANGE_MASK},
-	{BRCMS_SROM_TRISO5G, 0xffffff00, 0, SROM8_FEM5G, SROM8_FEM_TR_ISO_MASK},
-	{BRCMS_SROM_ANTSWCTL5G, 0xffffff00, 0, SROM8_FEM5G,
-	 SROM8_FEM_ANTSWLUT_MASK},
-	{BRCMS_SROM_TEMPTHRESH, 0xffffff00, 0, SROM8_THERMAL, 0xff00},
-	{BRCMS_SROM_TEMPOFFSET, 0xffffff00, 0, SROM8_THERMAL, 0x00ff},
-
-	{BRCMS_SROM_CCODE, 0xffffff00, SRFL_CCODE, SROM8_CCODE, 0xffff},
-	{BRCMS_SROM_MACADDR, 0xffffff00, SRFL_ETHADDR, SROM8_MACHI, 0xffff},
-	{BRCMS_SROM_LEDDC, 0xffffff00, SRFL_NOFFS | SRFL_LEDDC, SROM8_LEDDC,
-	 0xffff},
-	{BRCMS_SROM_RAWTEMPSENSE, 0xffffff00, SRFL_PRHEX, SROM8_MPWR_RAWTS,
-	 0x01ff},
-	{BRCMS_SROM_MEASPOWER, 0xffffff00, SRFL_PRHEX, SROM8_MPWR_RAWTS,
-	 0xfe00},
-	{BRCMS_SROM_TEMPSENSE_SLOPE, 0xffffff00, SRFL_PRHEX,
-	 SROM8_TS_SLP_OPT_CORRX, 0x00ff},
-	{BRCMS_SROM_TEMPCORRX, 0xffffff00, SRFL_PRHEX, SROM8_TS_SLP_OPT_CORRX,
-	 0xfc00},
-	{BRCMS_SROM_TEMPSENSE_OPTION, 0xffffff00, SRFL_PRHEX,
-	 SROM8_TS_SLP_OPT_CORRX, 0x0300},
-	{BRCMS_SROM_FREQOFFSET_CORR, 0xffffff00, SRFL_PRHEX,
-	 SROM8_FOC_HWIQ_IQSWP, 0x000f},
-	{BRCMS_SROM_IQCAL_SWP_DIS, 0xffffff00, SRFL_PRHEX, SROM8_FOC_HWIQ_IQSWP,
-	 0x0010},
-	{BRCMS_SROM_HW_IQCAL_EN, 0xffffff00, SRFL_PRHEX, SROM8_FOC_HWIQ_IQSWP,
-	 0x0020},
-	{BRCMS_SROM_PHYCAL_TEMPDELTA, 0xffffff00, 0, SROM8_PHYCAL_TEMPDELTA,
-	 0x00ff},
-
-	{BRCMS_SROM_CCK2GPO, 0x00000100, 0, SROM8_2G_CCKPO, 0xffff},
-	{BRCMS_SROM_OFDM2GPO, 0x00000100, SRFL_MORE, SROM8_2G_OFDMPO, 0xffff},
-	{BRCMS_SROM_CONT, 0, 0, SROM8_2G_OFDMPO + 1, 0xffff},
-	{BRCMS_SROM_OFDM5GPO, 0x00000100, SRFL_MORE, SROM8_5G_OFDMPO, 0xffff},
-	{BRCMS_SROM_CONT, 0, 0, SROM8_5G_OFDMPO + 1, 0xffff},
-	{BRCMS_SROM_OFDM5GLPO, 0x00000100, SRFL_MORE, SROM8_5GL_OFDMPO, 0xffff},
-	{BRCMS_SROM_CONT, 0, 0, SROM8_5GL_OFDMPO + 1, 0xffff},
-	{BRCMS_SROM_OFDM5GHPO, 0x00000100, SRFL_MORE, SROM8_5GH_OFDMPO, 0xffff},
-	{BRCMS_SROM_CONT, 0, 0, SROM8_5GH_OFDMPO + 1, 0xffff},
-	{BRCMS_SROM_MCS2GPO0, 0x00000100, 0, SROM8_2G_MCSPO, 0xffff},
-	{BRCMS_SROM_MCS2GPO1, 0x00000100, 0, SROM8_2G_MCSPO + 1, 0xffff},
-	{BRCMS_SROM_MCS2GPO2, 0x00000100, 0, SROM8_2G_MCSPO + 2, 0xffff},
-	{BRCMS_SROM_MCS2GPO3, 0x00000100, 0, SROM8_2G_MCSPO + 3, 0xffff},
-	{BRCMS_SROM_MCS2GPO4, 0x00000100, 0, SROM8_2G_MCSPO + 4, 0xffff},
-	{BRCMS_SROM_MCS2GPO5, 0x00000100, 0, SROM8_2G_MCSPO + 5, 0xffff},
-	{BRCMS_SROM_MCS2GPO6, 0x00000100, 0, SROM8_2G_MCSPO + 6, 0xffff},
-	{BRCMS_SROM_MCS2GPO7, 0x00000100, 0, SROM8_2G_MCSPO + 7, 0xffff},
-	{BRCMS_SROM_MCS5GPO0, 0x00000100, 0, SROM8_5G_MCSPO, 0xffff},
-	{BRCMS_SROM_MCS5GPO1, 0x00000100, 0, SROM8_5G_MCSPO + 1, 0xffff},
-	{BRCMS_SROM_MCS5GPO2, 0x00000100, 0, SROM8_5G_MCSPO + 2, 0xffff},
-	{BRCMS_SROM_MCS5GPO3, 0x00000100, 0, SROM8_5G_MCSPO + 3, 0xffff},
-	{BRCMS_SROM_MCS5GPO4, 0x00000100, 0, SROM8_5G_MCSPO + 4, 0xffff},
-	{BRCMS_SROM_MCS5GPO5, 0x00000100, 0, SROM8_5G_MCSPO + 5, 0xffff},
-	{BRCMS_SROM_MCS5GPO6, 0x00000100, 0, SROM8_5G_MCSPO + 6, 0xffff},
-	{BRCMS_SROM_MCS5GPO7, 0x00000100, 0, SROM8_5G_MCSPO + 7, 0xffff},
-	{BRCMS_SROM_MCS5GLPO0, 0x00000100, 0, SROM8_5GL_MCSPO, 0xffff},
-	{BRCMS_SROM_MCS5GLPO1, 0x00000100, 0, SROM8_5GL_MCSPO + 1, 0xffff},
-	{BRCMS_SROM_MCS5GLPO2, 0x00000100, 0, SROM8_5GL_MCSPO + 2, 0xffff},
-	{BRCMS_SROM_MCS5GLPO3, 0x00000100, 0, SROM8_5GL_MCSPO + 3, 0xffff},
-	{BRCMS_SROM_MCS5GLPO4, 0x00000100, 0, SROM8_5GL_MCSPO + 4, 0xffff},
-	{BRCMS_SROM_MCS5GLPO5, 0x00000100, 0, SROM8_5GL_MCSPO + 5, 0xffff},
-	{BRCMS_SROM_MCS5GLPO6, 0x00000100, 0, SROM8_5GL_MCSPO + 6, 0xffff},
-	{BRCMS_SROM_MCS5GLPO7, 0x00000100, 0, SROM8_5GL_MCSPO + 7, 0xffff},
-	{BRCMS_SROM_MCS5GHPO0, 0x00000100, 0, SROM8_5GH_MCSPO, 0xffff},
-	{BRCMS_SROM_MCS5GHPO1, 0x00000100, 0, SROM8_5GH_MCSPO + 1, 0xffff},
-	{BRCMS_SROM_MCS5GHPO2, 0x00000100, 0, SROM8_5GH_MCSPO + 2, 0xffff},
-	{BRCMS_SROM_MCS5GHPO3, 0x00000100, 0, SROM8_5GH_MCSPO + 3, 0xffff},
-	{BRCMS_SROM_MCS5GHPO4, 0x00000100, 0, SROM8_5GH_MCSPO + 4, 0xffff},
-	{BRCMS_SROM_MCS5GHPO5, 0x00000100, 0, SROM8_5GH_MCSPO + 5, 0xffff},
-	{BRCMS_SROM_MCS5GHPO6, 0x00000100, 0, SROM8_5GH_MCSPO + 6, 0xffff},
-	{BRCMS_SROM_MCS5GHPO7, 0x00000100, 0, SROM8_5GH_MCSPO + 7, 0xffff},
-	{BRCMS_SROM_CDDPO, 0x00000100, 0, SROM8_CDDPO, 0xffff},
-	{BRCMS_SROM_STBCPO, 0x00000100, 0, SROM8_STBCPO, 0xffff},
-	{BRCMS_SROM_BW40PO, 0x00000100, 0, SROM8_BW40PO, 0xffff},
-	{BRCMS_SROM_BWDUPPO, 0x00000100, 0, SROM8_BWDUPPO, 0xffff},
-
-	/* power per rate from sromrev 9 */
-	{BRCMS_SROM_CCKBW202GPO, 0xfffffe00, 0, SROM9_2GPO_CCKBW20, 0xffff},
-	{BRCMS_SROM_CCKBW20UL2GPO, 0xfffffe00, 0, SROM9_2GPO_CCKBW20UL, 0xffff},
-	{BRCMS_SROM_LEGOFDMBW202GPO, 0xfffffe00, SRFL_MORE,
-	 SROM9_2GPO_LOFDMBW20, 0xffff},
-	{BRCMS_SROM_CONT, 0, 0, SROM9_2GPO_LOFDMBW20 + 1, 0xffff},
-	{BRCMS_SROM_LEGOFDMBW20UL2GPO, 0xfffffe00, SRFL_MORE,
-	 SROM9_2GPO_LOFDMBW20UL, 0xffff},
-	{BRCMS_SROM_CONT, 0, 0, SROM9_2GPO_LOFDMBW20UL + 1, 0xffff},
-	{BRCMS_SROM_LEGOFDMBW205GLPO, 0xfffffe00, SRFL_MORE,
-	 SROM9_5GLPO_LOFDMBW20, 0xffff},
-	{BRCMS_SROM_CONT, 0, 0, SROM9_5GLPO_LOFDMBW20 + 1, 0xffff},
-	{BRCMS_SROM_LEGOFDMBW20UL5GLPO, 0xfffffe00, SRFL_MORE,
-	 SROM9_5GLPO_LOFDMBW20UL, 0xffff},
-	{BRCMS_SROM_CONT, 0, 0, SROM9_5GLPO_LOFDMBW20UL + 1, 0xffff},
-	{BRCMS_SROM_LEGOFDMBW205GMPO, 0xfffffe00, SRFL_MORE,
-	 SROM9_5GMPO_LOFDMBW20, 0xffff},
-	{BRCMS_SROM_CONT, 0, 0, SROM9_5GMPO_LOFDMBW20 + 1, 0xffff},
-	{BRCMS_SROM_LEGOFDMBW20UL5GMPO, 0xfffffe00, SRFL_MORE,
-	 SROM9_5GMPO_LOFDMBW20UL, 0xffff},
-	{BRCMS_SROM_CONT, 0, 0, SROM9_5GMPO_LOFDMBW20UL + 1, 0xffff},
-	{BRCMS_SROM_LEGOFDMBW205GHPO, 0xfffffe00, SRFL_MORE,
-	 SROM9_5GHPO_LOFDMBW20, 0xffff},
-	{BRCMS_SROM_CONT, 0, 0, SROM9_5GHPO_LOFDMBW20 + 1, 0xffff},
-	{BRCMS_SROM_LEGOFDMBW20UL5GHPO, 0xfffffe00, SRFL_MORE,
-	 SROM9_5GHPO_LOFDMBW20UL, 0xffff},
-	{BRCMS_SROM_CONT, 0, 0, SROM9_5GHPO_LOFDMBW20UL + 1, 0xffff},
-	{BRCMS_SROM_MCSBW202GPO, 0xfffffe00, SRFL_MORE, SROM9_2GPO_MCSBW20,
-	 0xffff},
-	{BRCMS_SROM_CONT, 0, 0, SROM9_2GPO_MCSBW20 + 1, 0xffff},
-	{BRCMS_SROM_MCSBW20UL2GPO, 0xfffffe00, SRFL_MORE, SROM9_2GPO_MCSBW20UL,
-	 0xffff},
-	{BRCMS_SROM_CONT, 0, 0, SROM9_2GPO_MCSBW20UL + 1, 0xffff},
-	{BRCMS_SROM_MCSBW402GPO, 0xfffffe00, SRFL_MORE, SROM9_2GPO_MCSBW40,
-	 0xffff},
-	{BRCMS_SROM_CONT, 0, 0, SROM9_2GPO_MCSBW40 + 1, 0xffff},
-	{BRCMS_SROM_MCSBW205GLPO, 0xfffffe00, SRFL_MORE, SROM9_5GLPO_MCSBW20,
-	 0xffff},
-	{BRCMS_SROM_CONT, 0, 0, SROM9_5GLPO_MCSBW20 + 1, 0xffff},
-	{BRCMS_SROM_MCSBW20UL5GLPO, 0xfffffe00, SRFL_MORE,
-	 SROM9_5GLPO_MCSBW20UL, 0xffff},
-	{BRCMS_SROM_CONT, 0, 0, SROM9_5GLPO_MCSBW20UL + 1, 0xffff},
-	{BRCMS_SROM_MCSBW405GLPO, 0xfffffe00, SRFL_MORE, SROM9_5GLPO_MCSBW40,
-	 0xffff},
-	{BRCMS_SROM_CONT, 0, 0, SROM9_5GLPO_MCSBW40 + 1, 0xffff},
-	{BRCMS_SROM_MCSBW205GMPO, 0xfffffe00, SRFL_MORE, SROM9_5GMPO_MCSBW20,
-	 0xffff},
-	{BRCMS_SROM_CONT, 0, 0, SROM9_5GMPO_MCSBW20 + 1, 0xffff},
-	{BRCMS_SROM_MCSBW20UL5GMPO, 0xfffffe00, SRFL_MORE,
-	 SROM9_5GMPO_MCSBW20UL, 0xffff},
-	{BRCMS_SROM_CONT, 0, 0, SROM9_5GMPO_MCSBW20UL + 1, 0xffff},
-	{BRCMS_SROM_MCSBW405GMPO, 0xfffffe00, SRFL_MORE, SROM9_5GMPO_MCSBW40,
-	 0xffff},
-	{BRCMS_SROM_CONT, 0, 0, SROM9_5GMPO_MCSBW40 + 1, 0xffff},
-	{BRCMS_SROM_MCSBW205GHPO, 0xfffffe00, SRFL_MORE, SROM9_5GHPO_MCSBW20,
-	 0xffff},
-	{BRCMS_SROM_CONT, 0, 0, SROM9_5GHPO_MCSBW20 + 1, 0xffff},
-	{BRCMS_SROM_MCSBW20UL5GHPO, 0xfffffe00, SRFL_MORE,
-	 SROM9_5GHPO_MCSBW20UL, 0xffff},
-	{BRCMS_SROM_CONT, 0, 0, SROM9_5GHPO_MCSBW20UL + 1, 0xffff},
-	{BRCMS_SROM_MCSBW405GHPO, 0xfffffe00, SRFL_MORE, SROM9_5GHPO_MCSBW40,
-	 0xffff},
-	{BRCMS_SROM_CONT, 0, 0, SROM9_5GHPO_MCSBW40 + 1, 0xffff},
-	{BRCMS_SROM_MCS32PO, 0xfffffe00, 0, SROM9_PO_MCS32, 0xffff},
-	{BRCMS_SROM_LEGOFDM40DUPPO, 0xfffffe00, 0, SROM9_PO_LOFDM40DUP, 0xffff},
-
-	{BRCMS_SROM_NULL, 0, 0, 0, 0}
-};
-
-static const struct brcms_sromvar perpath_pci_sromvars[] = {
-	{BRCMS_SROM_MAXP2GA0, 0xffffff00, 0, SROM8_2G_ITT_MAXP, 0x00ff},
-	{BRCMS_SROM_ITT2GA0, 0xffffff00, 0, SROM8_2G_ITT_MAXP, 0xff00},
-	{BRCMS_SROM_ITT5GA0, 0xffffff00, 0, SROM8_5G_ITT_MAXP, 0xff00},
-	{BRCMS_SROM_PA2GW0A0, 0xffffff00, SRFL_PRHEX, SROM8_2G_PA, 0xffff},
-	{BRCMS_SROM_PA2GW1A0, 0xffffff00, SRFL_PRHEX, SROM8_2G_PA + 1, 0xffff},
-	{BRCMS_SROM_PA2GW2A0, 0xffffff00, SRFL_PRHEX, SROM8_2G_PA + 2, 0xffff},
-	{BRCMS_SROM_MAXP5GA0, 0xffffff00, 0, SROM8_5G_ITT_MAXP, 0x00ff},
-	{BRCMS_SROM_MAXP5GHA0, 0xffffff00, 0, SROM8_5GLH_MAXP, 0x00ff},
-	{BRCMS_SROM_MAXP5GLA0, 0xffffff00, 0, SROM8_5GLH_MAXP, 0xff00},
-	{BRCMS_SROM_PA5GW0A0, 0xffffff00, SRFL_PRHEX, SROM8_5G_PA, 0xffff},
-	{BRCMS_SROM_PA5GW1A0, 0xffffff00, SRFL_PRHEX, SROM8_5G_PA + 1, 0xffff},
-	{BRCMS_SROM_PA5GW2A0, 0xffffff00, SRFL_PRHEX, SROM8_5G_PA + 2, 0xffff},
-	{BRCMS_SROM_PA5GLW0A0, 0xffffff00, SRFL_PRHEX, SROM8_5GL_PA, 0xffff},
-	{BRCMS_SROM_PA5GLW1A0, 0xffffff00, SRFL_PRHEX, SROM8_5GL_PA + 1,
-	 0xffff},
-	{BRCMS_SROM_PA5GLW2A0, 0xffffff00, SRFL_PRHEX, SROM8_5GL_PA + 2,
-	 0xffff},
-	{BRCMS_SROM_PA5GHW0A0, 0xffffff00, SRFL_PRHEX, SROM8_5GH_PA, 0xffff},
-	{BRCMS_SROM_PA5GHW1A0, 0xffffff00, SRFL_PRHEX, SROM8_5GH_PA + 1,
-	 0xffff},
-	{BRCMS_SROM_PA5GHW2A0, 0xffffff00, SRFL_PRHEX, SROM8_5GH_PA + 2,
-	 0xffff},
-	{BRCMS_SROM_NULL, 0, 0, 0, 0}
-};
-
-/* crc table has the same contents for every device instance, so it can be
- * shared between devices. */
-static u8 brcms_srom_crc8_table[CRC8_TABLE_SIZE];
-
-static uint mask_shift(u16 mask)
-{
-	uint i;
-	for (i = 0; i < (sizeof(mask) << 3); i++) {
-		if (mask & (1 << i))
-			return i;
-	}
-	return 0;
-}
-
-static uint mask_width(u16 mask)
-{
-	int i;
-	for (i = (sizeof(mask) << 3) - 1; i >= 0; i--) {
-		if (mask & (1 << i))
-			return (uint) (i - mask_shift(mask) + 1);
-	}
-	return 0;
-}
-
-static inline void le16_to_cpu_buf(u16 *buf, uint nwords)
-{
-	while (nwords--)
-		*(buf + nwords) = le16_to_cpu(*(__le16 *)(buf + nwords));
-}
-
-static inline void cpu_to_le16_buf(u16 *buf, uint nwords)
-{
-	while (nwords--)
-		*(__le16 *)(buf + nwords) = cpu_to_le16(*(buf + nwords));
-}
-
-/*
- * convert binary srom data into linked list of srom variable items.
- */
-static int
-_initvars_srom_pci(u8 sromrev, u16 *srom, struct list_head *var_list)
-{
-	struct brcms_srom_list_head *entry;
-	enum brcms_srom_id id;
-	u16 w;
-	u32 val = 0;
-	const struct brcms_sromvar *srv;
-	uint width;
-	uint flags;
-	u32 sr = (1 << sromrev);
-	uint p;
-	uint pb =  SROM8_PATH0;
-	const uint psz = SROM8_PATH1 - SROM8_PATH0;
-
-	/* first store the srom revision */
-	entry = kzalloc(sizeof(struct brcms_srom_list_head), GFP_KERNEL);
-	if (!entry)
-		return -ENOMEM;
-
-	entry->varid = BRCMS_SROM_REV;
-	entry->var_type = BRCMS_SROM_UNUMBER;
-	entry->uval = sromrev;
-	list_add(&entry->var_list, var_list);
-
-	for (srv = pci_sromvars; srv->varid != BRCMS_SROM_NULL; srv++) {
-		enum brcms_srom_var_type type;
-		u8 ea[ETH_ALEN];
-		u8 extra_space = 0;
-
-		if ((srv->revmask & sr) == 0)
-			continue;
-
-		flags = srv->flags;
-		id = srv->varid;
-
-		/* This entry is for mfgc only. Don't generate param for it, */
-		if (flags & SRFL_NOVAR)
-			continue;
-
-		if (flags & SRFL_ETHADDR) {
-			/*
-			 * stored in string format XX:XX:XX:XX:XX:XX (17 chars)
-			 */
-			ea[0] = (srom[srv->off] >> 8) & 0xff;
-			ea[1] = srom[srv->off] & 0xff;
-			ea[2] = (srom[srv->off + 1] >> 8) & 0xff;
-			ea[3] = srom[srv->off + 1] & 0xff;
-			ea[4] = (srom[srv->off + 2] >> 8) & 0xff;
-			ea[5] = srom[srv->off + 2] & 0xff;
-			/* 17 characters + string terminator - union size */
-			extra_space = 18 - sizeof(s32);
-			type = BRCMS_SROM_STRING;
-		} else {
-			w = srom[srv->off];
-			val = (w & srv->mask) >> mask_shift(srv->mask);
-			width = mask_width(srv->mask);
-
-			while (srv->flags & SRFL_MORE) {
-				srv++;
-				if (srv->off == 0)
-					continue;
-
-				w = srom[srv->off];
-				val +=
-				    ((w & srv->mask) >> mask_shift(srv->
-								   mask)) <<
-				    width;
-				width += mask_width(srv->mask);
-			}
-
-			if ((flags & SRFL_NOFFS)
-			    && ((int)val == (1 << width) - 1))
-				continue;
-
-			if (flags & SRFL_CCODE) {
-				type = BRCMS_SROM_STRING;
-			} else if (flags & SRFL_LEDDC) {
-				/* LED Powersave duty cycle has to be scaled:
-				 *(oncount >> 24) (offcount >> 8)
-				 */
-				u32 w32 = /* oncount */
-					  (((val >> 8) & 0xff) << 24) |
-					  /* offcount */
-					  (((val & 0xff)) << 8);
-				type = BRCMS_SROM_UNUMBER;
-				val = w32;
-			} else if ((flags & SRFL_PRSIGN)
-				 && (val & (1 << (width - 1)))) {
-				type = BRCMS_SROM_SNUMBER;
-				val |= ~0 << width;
-			} else
-				type = BRCMS_SROM_UNUMBER;
-		}
-
-		entry = kzalloc(sizeof(struct brcms_srom_list_head) +
-				extra_space, GFP_KERNEL);
-		if (!entry)
-			return -ENOMEM;
-		entry->varid = id;
-		entry->var_type = type;
-		if (flags & SRFL_ETHADDR) {
-			snprintf(entry->buf, 18, "%pM", ea);
-		} else if (flags & SRFL_CCODE) {
-			if (val == 0)
-				entry->buf[0] = '\0';
-			else
-				snprintf(entry->buf, 3, "%c%c",
-					 (val >> 8), (val & 0xff));
-		} else {
-			entry->uval = val;
-		}
-
-		list_add(&entry->var_list, var_list);
-	}
-
-	for (p = 0; p < MAX_PATH_SROM; p++) {
-		for (srv = perpath_pci_sromvars;
-		     srv->varid != BRCMS_SROM_NULL; srv++) {
-			if ((srv->revmask & sr) == 0)
-				continue;
-
-			if (srv->flags & SRFL_NOVAR)
-				continue;
-
-			w = srom[pb + srv->off];
-			val = (w & srv->mask) >> mask_shift(srv->mask);
-			width = mask_width(srv->mask);
-
-			/* Cheating: no per-path var is more than
-			 * 1 word */
-			if ((srv->flags & SRFL_NOFFS)
-			    && ((int)val == (1 << width) - 1))
-				continue;
-
-			entry =
-			    kzalloc(sizeof(struct brcms_srom_list_head),
-				    GFP_KERNEL);
-			if (!entry)
-				return -ENOMEM;
-			entry->varid = srv->varid+p;
-			entry->var_type = BRCMS_SROM_UNUMBER;
-			entry->uval = val;
-			list_add(&entry->var_list, var_list);
-		}
-		pb += psz;
-	}
-	return 0;
-}
-
-/*
- * The crc check is done on a little-endian array, we need
- * to switch the bytes around before checking crc (and
- * then switch it back).
- */
-static int do_crc_check(u16 *buf, unsigned nwords)
-{
-	u8 crc;
-
-	cpu_to_le16_buf(buf, nwords);
-	crc = crc8(brcms_srom_crc8_table, (void *)buf, nwords << 1, CRC8_INIT_VALUE);
-	le16_to_cpu_buf(buf, nwords);
-
-	return crc == CRC8_GOOD_VALUE(brcms_srom_crc8_table);
-}
-
-/*
- * Read in and validate sprom.
- * Return 0 on success, nonzero on error.
- */
-static int
-sprom_read_pci(struct si_pub *sih, u16 *buf, uint nwords, bool check_crc)
-{
-	int err = 0;
-	uint i;
-	struct bcma_device *core;
-	uint sprom_offset;
-
-	/* determine core to read */
-	if (ai_get_ccrev(sih) < 32) {
-		core = ai_findcore(sih, BCMA_CORE_80211, 0);
-		sprom_offset = PCI_BAR0_SPROM_OFFSET;
-	} else {
-		core = ai_findcore(sih, BCMA_CORE_CHIPCOMMON, 0);
-		sprom_offset = CHIPCREGOFFS(sromotp);
-	}
-
-	/* read the sprom */
-	for (i = 0; i < nwords; i++)
-		buf[i] = bcma_read16(core, sprom_offset+i*2);
-
-	if (buf[0] == 0xffff)
-		/*
-		 * The hardware thinks that an srom that starts with
-		 * 0xffff is blank, regardless of the rest of the
-		 * content, so declare it bad.
-		 */
-		return -ENODATA;
-
-	if (check_crc && !do_crc_check(buf, nwords))
-		err = -EIO;
-
-	return err;
-}
-
-static int otp_read_pci(struct si_pub *sih, u16 *buf, uint nwords)
-{
-	u8 *otp;
-	uint sz = OTP_SZ_MAX / 2;	/* size in words */
-	int err = 0;
-
-	otp = kzalloc(OTP_SZ_MAX, GFP_ATOMIC);
-	if (otp == NULL)
-		return -ENOMEM;
-
-	err = otp_read_region(sih, OTP_HW_RGN, (u16 *) otp, &sz);
-
-	sz = min_t(uint, sz, nwords);
-	memcpy(buf, otp, sz * 2);
-
-	kfree(otp);
-
-	/* Check CRC */
-	if (buf[0] == 0xffff)
-		/* The hardware thinks that an srom that starts with 0xffff
-		 * is blank, regardless of the rest of the content, so declare
-		 * it bad.
-		 */
-		return -ENODATA;
-
-	/* fixup the endianness so crc8 will pass */
-	cpu_to_le16_buf(buf, sz);
-	if (crc8(brcms_srom_crc8_table, (u8 *) buf, sz * 2,
-		 CRC8_INIT_VALUE) != CRC8_GOOD_VALUE(brcms_srom_crc8_table))
-		err = -EIO;
-	else
-		/* now correct the endianness of the byte array */
-		le16_to_cpu_buf(buf, sz);
-
-	return err;
-}
-
-/*
- * Initialize nonvolatile variable table from sprom.
- * Return 0 on success, nonzero on error.
- */
-int srom_var_init(struct si_pub *sih)
-{
-	u16 *srom;
-	u8 sromrev = 0;
-	u32 sr;
-	int err = 0;
-
-	/*
-	 * Apply CRC over SROM content regardless SROM is present or not.
-	 */
-	srom = kmalloc(SROM_MAX, GFP_ATOMIC);
-	if (!srom)
-		return -ENOMEM;
-
-	crc8_populate_lsb(brcms_srom_crc8_table, SROM_CRC8_POLY);
-	if (ai_is_sprom_available(sih)) {
-		err = sprom_read_pci(sih, srom, SROM4_WORDS, true);
-
-		if (err == 0)
-			/* srom read and passed crc */
-			/* top word of sprom contains version and crc8 */
-			sromrev = srom[SROM4_CRCREV] & 0xff;
-	} else {
-		/* Use OTP if SPROM not available */
-		err = otp_read_pci(sih, srom, SROM4_WORDS);
-		if (err == 0)
-			/* OTP only contain SROM rev8/rev9 for now */
-			sromrev = srom[SROM4_CRCREV] & 0xff;
-	}
-
-	if (!err) {
-		struct si_info *sii = (struct si_info *)sih;
-
-		/* Bitmask for the sromrev */
-		sr = 1 << sromrev;
-
-		/*
-		 * srom version check: Current valid versions: 8, 9
-		 */
-		if ((sr & 0x300) == 0) {
-			err = -EINVAL;
-			goto errout;
-		}
-
-		INIT_LIST_HEAD(&sii->var_list);
-
-		/* parse SROM into name=value pairs. */
-		err = _initvars_srom_pci(sromrev, srom, &sii->var_list);
-		if (err)
-			srom_free_vars(sih);
-	}
-
-errout:
-	kfree(srom);
-	return err;
-}
-
-void srom_free_vars(struct si_pub *sih)
-{
-	struct si_info *sii;
-	struct brcms_srom_list_head *entry, *next;
-
-	sii = (struct si_info *)sih;
-	list_for_each_entry_safe(entry, next, &sii->var_list, var_list) {
-		list_del(&entry->var_list);
-		kfree(entry);
-	}
-}
-
-/*
- * Search the name=value vars for a specific one and return its value.
- * Returns NULL if not found.
- */
-char *getvar(struct si_pub *sih, enum brcms_srom_id id)
-{
-	struct si_info *sii;
-	struct brcms_srom_list_head *entry;
-
-	sii = (struct si_info *)sih;
-
-	list_for_each_entry(entry, &sii->var_list, var_list)
-		if (entry->varid == id)
-			return &entry->buf[0];
-
-	/* nothing found */
-	return NULL;
-}
-
-/*
- * Search the vars for a specific one and return its value as
- * an integer. Returns 0 if not found.-
- */
-int getintvar(struct si_pub *sih, enum brcms_srom_id id)
-{
-	struct si_info *sii;
-	struct brcms_srom_list_head *entry;
-	unsigned long res;
-
-	sii = (struct si_info *)sih;
-
-	list_for_each_entry(entry, &sii->var_list, var_list)
-		if (entry->varid == id) {
-			if (entry->var_type == BRCMS_SROM_SNUMBER ||
-			    entry->var_type == BRCMS_SROM_UNUMBER)
-				return (int)entry->sval;
-			else if (!kstrtoul(&entry->buf[0], 0, &res))
-				return (int)res;
-		}
-
-	return 0;
-}
diff --git a/drivers/net/wireless/brcm80211/brcmsmac/srom.h b/drivers/net/wireless/brcm80211/brcmsmac/srom.h
deleted file mode 100644
index f2a58f262c9..00000000000
--- a/drivers/net/wireless/brcm80211/brcmsmac/srom.h
+++ /dev/null
@@ -1,29 +0,0 @@
-/*
- * 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	_BRCM_SROM_H_
-#define	_BRCM_SROM_H_
-
-#include "types.h"
-
-/* Prototypes */
-extern int srom_var_init(struct si_pub *sih);
-extern void srom_free_vars(struct si_pub *sih);
-
-extern int srom_read(struct si_pub *sih, uint bus, void *curmap,
-		     uint byteoff, uint nbytes, u16 *buf, bool check_crc);
-
-#endif				/* _BRCM_SROM_H_ */
diff --git a/drivers/net/wireless/brcm80211/brcmsmac/stf.c b/drivers/net/wireless/brcm80211/brcmsmac/stf.c
index d8f528eb180..ed1d1aa71d2 100644
--- a/drivers/net/wireless/brcm80211/brcmsmac/stf.c
+++ b/drivers/net/wireless/brcm80211/brcmsmac/stf.c
@@ -370,9 +370,11 @@ void brcms_c_stf_phy_txant_upd(struct brcms_c_info *wlc)
 
 void brcms_c_stf_phy_chain_calc(struct brcms_c_info *wlc)
 {
+	struct ssb_sprom *sprom = &wlc->hw->d11core->bus->sprom;
+
 	/* get available rx/tx chains */
-	wlc->stf->hw_txchain = (u8) getintvar(wlc->hw->sih, BRCMS_SROM_TXCHAIN);
-	wlc->stf->hw_rxchain = (u8) getintvar(wlc->hw->sih, BRCMS_SROM_RXCHAIN);
+	wlc->stf->hw_txchain = sprom->txchain;
+	wlc->stf->hw_rxchain = sprom->rxchain;
 
 	/* these parameter are intended to be used for all PHY types */
 	if (wlc->stf->hw_txchain == 0 || wlc->stf->hw_txchain == 0xf) {
diff --git a/drivers/net/wireless/brcm80211/include/brcm_hw_ids.h b/drivers/net/wireless/brcm80211/include/brcm_hw_ids.h
index 5fb17d53c9b..333193f20e1 100644
--- a/drivers/net/wireless/brcm80211/include/brcm_hw_ids.h
+++ b/drivers/net/wireless/brcm80211/include/brcm_hw_ids.h
@@ -17,17 +17,7 @@
 #ifndef	_BRCM_HW_IDS_H_
 #define	_BRCM_HW_IDS_H_
 
-#define	BCM4325_D11DUAL_ID	0x431b
-#define	BCM4325_D11G_ID		0x431c
-#define	BCM4325_D11A_ID		0x431d
-
-#define BCM4329_D11N2G_ID	0x432f	/* 4329 802.11n 2.4G device */
-#define BCM4329_D11N5G_ID	0x4330	/* 4329 802.11n 5G device */
-#define BCM4329_D11NDUAL_ID	0x432e
-
-#define BCM4319_D11N_ID		0x4337	/* 4319 802.11n dualband device */
-#define BCM4319_D11N2G_ID	0x4338	/* 4319 802.11n 2.4G device */
-#define BCM4319_D11N5G_ID	0x4339	/* 4319 802.11n 5G device */
+#define BCM4313_D11N2G_ID	0x4727	/* 4313 802.11n 2.4G device */
 
 #define BCM43224_D11N_ID	0x4353	/* 43224 802.11n dualband device */
 #define BCM43224_D11N_ID_VEN1	0x0576	/* Vendor specific 43224 802.11n db */
@@ -37,23 +27,15 @@
 #define BCM43236_D11N_ID	0x4346	/* 43236 802.11n dualband device */
 #define BCM43236_D11N2G_ID	0x4347	/* 43236 802.11n 2.4GHz device */
 
-#define BCM4313_D11N2G_ID	0x4727	/* 4313 802.11n 2.4G device */
-
-/* Chip IDs */
-#define BCM4313_CHIP_ID		0x4313	/* 4313 chip id */
-#define	BCM4319_CHIP_ID		0x4319	/* 4319 chip id */
-
-#define	BCM43224_CHIP_ID	43224	/* 43224 chipcommon chipid */
-#define	BCM43225_CHIP_ID	43225	/* 43225 chipcommon chipid */
-#define	BCM43421_CHIP_ID	43421	/* 43421 chipcommon chipid */
-#define	BCM43235_CHIP_ID	43235	/* 43235 chipcommon chipid */
-#define	BCM43236_CHIP_ID	43236	/* 43236 chipcommon chipid */
-#define	BCM43238_CHIP_ID	43238	/* 43238 chipcommon chipid */
-#define	BCM4329_CHIP_ID		0x4329	/* 4329 chipcommon chipid */
-#define	BCM4325_CHIP_ID		0x4325	/* 4325 chipcommon chipid */
-#define	BCM4331_CHIP_ID		0x4331	/* 4331 chipcommon chipid */
-#define BCM4336_CHIP_ID		0x4336	/* 4336 chipcommon chipid */
-#define BCM4330_CHIP_ID		0x4330	/* 4330 chipcommon chipid */
-#define BCM6362_CHIP_ID		0x6362	/* 6362 chipcommon chipid */
+/* Chipcommon Core Chip IDs */
+#define BCM4313_CHIP_ID		0x4313
+#define BCM43224_CHIP_ID	43224
+#define BCM43225_CHIP_ID	43225
+#define BCM43235_CHIP_ID	43235
+#define BCM43236_CHIP_ID	43236
+#define BCM43238_CHIP_ID	43238
+#define BCM4329_CHIP_ID		0x4329
+#define BCM4330_CHIP_ID		0x4330
+#define BCM4331_CHIP_ID		0x4331
 
 #endif				/* _BRCM_HW_IDS_H_ */
diff --git a/drivers/net/wireless/hostap/hostap_main.c b/drivers/net/wireless/hostap/hostap_main.c
index bfa0d54221e..627bc12074c 100644
--- a/drivers/net/wireless/hostap/hostap_main.c
+++ b/drivers/net/wireless/hostap/hostap_main.c
@@ -244,8 +244,7 @@ u16 hostap_tx_callback_register(local_info_t *local,
 	unsigned long flags;
 	struct hostap_tx_callback_info *entry;
 
-	entry = kmalloc(sizeof(*entry),
-							   GFP_ATOMIC);
+	entry = kmalloc(sizeof(*entry), GFP_KERNEL);
 	if (entry == NULL)
 		return 0;
 
diff --git a/drivers/net/wireless/hostap/hostap_pci.c b/drivers/net/wireless/hostap/hostap_pci.c
index 972a9c3af39..05ca3402dca 100644
--- a/drivers/net/wireless/hostap/hostap_pci.c
+++ b/drivers/net/wireless/hostap/hostap_pci.c
@@ -457,18 +457,4 @@ static struct pci_driver prism2_pci_driver = {
 #endif /* CONFIG_PM */
 };
 
-
-static int __init init_prism2_pci(void)
-{
-	return pci_register_driver(&prism2_pci_driver);
-}
-
-
-static void __exit exit_prism2_pci(void)
-{
-	pci_unregister_driver(&prism2_pci_driver);
-}
-
-
-module_init(init_prism2_pci);
-module_exit(exit_prism2_pci);
+module_pci_driver(prism2_pci_driver);
diff --git a/drivers/net/wireless/hostap/hostap_plx.c b/drivers/net/wireless/hostap/hostap_plx.c
index 33e79037770..c3d067ee4db 100644
--- a/drivers/net/wireless/hostap/hostap_plx.c
+++ b/drivers/net/wireless/hostap/hostap_plx.c
@@ -616,18 +616,4 @@ static struct pci_driver prism2_plx_driver = {
 	.remove		= prism2_plx_remove,
 };
 
-
-static int __init init_prism2_plx(void)
-{
-	return pci_register_driver(&prism2_plx_driver);
-}
-
-
-static void __exit exit_prism2_plx(void)
-{
-	pci_unregister_driver(&prism2_plx_driver);
-}
-
-
-module_init(init_prism2_plx);
-module_exit(exit_prism2_plx);
+module_pci_driver(prism2_plx_driver);
diff --git a/drivers/net/wireless/ipw2x00/ipw.h b/drivers/net/wireless/ipw2x00/ipw.h
new file mode 100644
index 00000000000..4007bf5ed6f
--- /dev/null
+++ b/drivers/net/wireless/ipw2x00/ipw.h
@@ -0,0 +1,23 @@
+/*
+ * Intel Pro/Wireless 2100, 2200BG, 2915ABG network connection driver
+ *
+ * Copyright 2012 Stanislav Yakovlev <stas.yakovlev@gmail.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ */
+
+#ifndef __IPW_H__
+#define __IPW_H__
+
+#include <linux/ieee80211.h>
+
+static const u32 ipw_cipher_suites[] = {
+	WLAN_CIPHER_SUITE_WEP40,
+	WLAN_CIPHER_SUITE_WEP104,
+	WLAN_CIPHER_SUITE_TKIP,
+	WLAN_CIPHER_SUITE_CCMP,
+};
+
+#endif
diff --git a/drivers/net/wireless/ipw2x00/ipw2100.c b/drivers/net/wireless/ipw2x00/ipw2100.c
index f0551f807f6..9cfae0c0870 100644
--- a/drivers/net/wireless/ipw2x00/ipw2100.c
+++ b/drivers/net/wireless/ipw2x00/ipw2100.c
@@ -166,6 +166,7 @@ that only one external action is invoked at a time.
 #include <net/lib80211.h>
 
 #include "ipw2100.h"
+#include "ipw.h"
 
 #define IPW2100_VERSION "git-1.2.2"
 
@@ -343,38 +344,50 @@ static struct iw_handler_def ipw2100_wx_handler_def;
 
 static inline void read_register(struct net_device *dev, u32 reg, u32 * val)
 {
-	*val = readl((void __iomem *)(dev->base_addr + reg));
+	struct ipw2100_priv *priv = libipw_priv(dev);
+
+	*val = ioread32(priv->ioaddr + reg);
 	IPW_DEBUG_IO("r: 0x%08X => 0x%08X\n", reg, *val);
 }
 
 static inline void write_register(struct net_device *dev, u32 reg, u32 val)
 {
-	writel(val, (void __iomem *)(dev->base_addr + reg));
+	struct ipw2100_priv *priv = libipw_priv(dev);
+
+	iowrite32(val, priv->ioaddr + reg);
 	IPW_DEBUG_IO("w: 0x%08X <= 0x%08X\n", reg, val);
 }
 
 static inline void read_register_word(struct net_device *dev, u32 reg,
 				      u16 * val)
 {
-	*val = readw((void __iomem *)(dev->base_addr + reg));
+	struct ipw2100_priv *priv = libipw_priv(dev);
+
+	*val = ioread16(priv->ioaddr + reg);
 	IPW_DEBUG_IO("r: 0x%08X => %04X\n", reg, *val);
 }
 
 static inline void read_register_byte(struct net_device *dev, u32 reg, u8 * val)
 {
-	*val = readb((void __iomem *)(dev->base_addr + reg));
+	struct ipw2100_priv *priv = libipw_priv(dev);
+
+	*val = ioread8(priv->ioaddr + reg);
 	IPW_DEBUG_IO("r: 0x%08X => %02X\n", reg, *val);
 }
 
 static inline void write_register_word(struct net_device *dev, u32 reg, u16 val)
 {
-	writew(val, (void __iomem *)(dev->base_addr + reg));
+	struct ipw2100_priv *priv = libipw_priv(dev);
+
+	iowrite16(val, priv->ioaddr + reg);
 	IPW_DEBUG_IO("w: 0x%08X <= %04X\n", reg, val);
 }
 
 static inline void write_register_byte(struct net_device *dev, u32 reg, u8 val)
 {
-	writeb(val, (void __iomem *)(dev->base_addr + reg));
+	struct ipw2100_priv *priv = libipw_priv(dev);
+
+	iowrite8(val, priv->ioaddr + reg);
 	IPW_DEBUG_IO("w: 0x%08X =< %02X\n", reg, val);
 }
 
@@ -506,13 +519,13 @@ static void read_nic_memory(struct net_device *dev, u32 addr, u32 len,
 		read_register_byte(dev, IPW_REG_INDIRECT_ACCESS_DATA + i, buf);
 }
 
-static inline int ipw2100_hw_is_adapter_in_system(struct net_device *dev)
+static bool ipw2100_hw_is_adapter_in_system(struct net_device *dev)
 {
-	return (dev->base_addr &&
-		(readl
-		 ((void __iomem *)(dev->base_addr +
-				   IPW_REG_DOA_DEBUG_AREA_START))
-		 == IPW_DATA_DOA_DEBUG_VALUE));
+	u32 dbg;
+
+	read_register(dev, IPW_REG_DOA_DEBUG_AREA_START, &dbg);
+
+	return dbg == IPW_DATA_DOA_DEBUG_VALUE;
 }
 
 static int ipw2100_get_ordinal(struct ipw2100_priv *priv, u32 ord,
@@ -1946,11 +1959,12 @@ static int ipw2100_wdev_init(struct net_device *dev)
 		wdev->wiphy->bands[IEEE80211_BAND_2GHZ] = bg_band;
 	}
 
+	wdev->wiphy->cipher_suites = ipw_cipher_suites;
+	wdev->wiphy->n_cipher_suites = ARRAY_SIZE(ipw_cipher_suites);
+
 	set_wiphy_dev(wdev->wiphy, &priv->pci_dev->dev);
-	if (wiphy_register(wdev->wiphy)) {
-		ipw2100_down(priv);
+	if (wiphy_register(wdev->wiphy))
 		return -EIO;
-	}
 	return 0;
 }
 
@@ -3773,7 +3787,7 @@ IPW2100_ORD(STAT_TX_HOST_REQUESTS, "requested Host Tx's (MSDU)"),
 	    IPW2100_ORD(COUNTRY_CODE,
 				"IEEE country code as recv'd from beacon"),
 	    IPW2100_ORD(COUNTRY_CHANNELS,
-				"channels suported by country"),
+				"channels supported by country"),
 	    IPW2100_ORD(RESET_CNT, "adapter resets (warm)"),
 	    IPW2100_ORD(BEACON_INTERVAL, "Beacon interval"),
 	    IPW2100_ORD(ANTENNA_DIVERSITY,
@@ -4062,7 +4076,7 @@ static int ipw2100_switch_mode(struct ipw2100_priv *priv, u32 mode)
 	ipw2100_firmware.version = 0;
 #endif
 
-	printk(KERN_INFO "%s: Reseting on mode change.\n", priv->net_dev->name);
+	printk(KERN_INFO "%s: Resetting on mode change.\n", priv->net_dev->name);
 	priv->reset_backoff = 0;
 	schedule_reset(priv);
 
@@ -6082,9 +6096,7 @@ static const struct net_device_ops ipw2100_netdev_ops = {
 /* Look into using netdev destructor to shutdown libipw? */
 
 static struct net_device *ipw2100_alloc_device(struct pci_dev *pci_dev,
-					       void __iomem * base_addr,
-					       unsigned long mem_start,
-					       unsigned long mem_len)
+					       void __iomem * ioaddr)
 {
 	struct ipw2100_priv *priv;
 	struct net_device *dev;
@@ -6096,6 +6108,7 @@ static struct net_device *ipw2100_alloc_device(struct pci_dev *pci_dev,
 	priv->ieee = netdev_priv(dev);
 	priv->pci_dev = pci_dev;
 	priv->net_dev = dev;
+	priv->ioaddr = ioaddr;
 
 	priv->ieee->hard_start_xmit = ipw2100_tx;
 	priv->ieee->set_security = shim__set_security;
@@ -6111,10 +6124,6 @@ static struct net_device *ipw2100_alloc_device(struct pci_dev *pci_dev,
 	dev->watchdog_timeo = 3 * HZ;
 	dev->irq = 0;
 
-	dev->base_addr = (unsigned long)base_addr;
-	dev->mem_start = mem_start;
-	dev->mem_end = dev->mem_start + mem_len - 1;
-
 	/* NOTE: We don't use the wireless_handlers hook
 	 * in dev as the system will start throwing WX requests
 	 * to us before we're actually initialized and it just
@@ -6215,8 +6224,7 @@ static struct net_device *ipw2100_alloc_device(struct pci_dev *pci_dev,
 static int ipw2100_pci_init_one(struct pci_dev *pci_dev,
 				const struct pci_device_id *ent)
 {
-	unsigned long mem_start, mem_len, mem_flags;
-	void __iomem *base_addr = NULL;
+	void __iomem *ioaddr;
 	struct net_device *dev = NULL;
 	struct ipw2100_priv *priv = NULL;
 	int err = 0;
@@ -6225,18 +6233,14 @@ static int ipw2100_pci_init_one(struct pci_dev *pci_dev,
 
 	IPW_DEBUG_INFO("enter\n");
 
-	mem_start = pci_resource_start(pci_dev, 0);
-	mem_len = pci_resource_len(pci_dev, 0);
-	mem_flags = pci_resource_flags(pci_dev, 0);
-
-	if ((mem_flags & IORESOURCE_MEM) != IORESOURCE_MEM) {
+	if (!(pci_resource_flags(pci_dev, 0) & IORESOURCE_MEM)) {
 		IPW_DEBUG_INFO("weird - resource type is not memory\n");
 		err = -ENODEV;
-		goto fail;
+		goto out;
 	}
 
-	base_addr = ioremap_nocache(mem_start, mem_len);
-	if (!base_addr) {
+	ioaddr = pci_iomap(pci_dev, 0, 0);
+	if (!ioaddr) {
 		printk(KERN_WARNING DRV_NAME
 		       "Error calling ioremap_nocache.\n");
 		err = -EIO;
@@ -6244,7 +6248,7 @@ static int ipw2100_pci_init_one(struct pci_dev *pci_dev,
 	}
 
 	/* allocate and initialize our net_device */
-	dev = ipw2100_alloc_device(pci_dev, base_addr, mem_start, mem_len);
+	dev = ipw2100_alloc_device(pci_dev, ioaddr);
 	if (!dev) {
 		printk(KERN_WARNING DRV_NAME
 		       "Error calling ipw2100_alloc_device.\n");
@@ -6325,6 +6329,11 @@ static int ipw2100_pci_init_one(struct pci_dev *pci_dev,
 	printk(KERN_INFO DRV_NAME
 	       ": Detected Intel PRO/Wireless 2100 Network Connection\n");
 
+	err = ipw2100_wdev_init(dev);
+	if (err)
+		goto fail;
+	registered = 1;
+
 	/* Bring up the interface.  Pre 0.46, after we registered the
 	 * network device we would call ipw2100_up.  This introduced a race
 	 * condition with newer hotplug configurations (network was coming
@@ -6341,11 +6350,7 @@ static int ipw2100_pci_init_one(struct pci_dev *pci_dev,
 		       "Error calling register_netdev.\n");
 		goto fail;
 	}
-	registered = 1;
-
-	err = ipw2100_wdev_init(dev);
-	if (err)
-		goto fail;
+	registered = 2;
 
 	mutex_lock(&priv->action_mutex);
 
@@ -6379,18 +6384,21 @@ static int ipw2100_pci_init_one(struct pci_dev *pci_dev,
 	priv->status |= STATUS_INITIALIZED;
 
 	mutex_unlock(&priv->action_mutex);
-
-	return 0;
+out:
+	return err;
 
       fail_unlock:
 	mutex_unlock(&priv->action_mutex);
-	wiphy_unregister(priv->ieee->wdev.wiphy);
-	kfree(priv->ieee->bg_band.channels);
       fail:
 	if (dev) {
-		if (registered)
+		if (registered >= 2)
 			unregister_netdev(dev);
 
+		if (registered) {
+			wiphy_unregister(priv->ieee->wdev.wiphy);
+			kfree(priv->ieee->bg_band.channels);
+		}
+
 		ipw2100_hw_stop_adapter(priv);
 
 		ipw2100_disable_interrupts(priv);
@@ -6409,63 +6417,56 @@ static int ipw2100_pci_init_one(struct pci_dev *pci_dev,
 		pci_set_drvdata(pci_dev, NULL);
 	}
 
-	if (base_addr)
-		iounmap(base_addr);
+	pci_iounmap(pci_dev, ioaddr);
 
 	pci_release_regions(pci_dev);
 	pci_disable_device(pci_dev);
-
-	return err;
+	goto out;
 }
 
 static void __devexit ipw2100_pci_remove_one(struct pci_dev *pci_dev)
 {
 	struct ipw2100_priv *priv = pci_get_drvdata(pci_dev);
-	struct net_device *dev;
+	struct net_device *dev = priv->net_dev;
 
-	if (priv) {
-		mutex_lock(&priv->action_mutex);
+	mutex_lock(&priv->action_mutex);
 
-		priv->status &= ~STATUS_INITIALIZED;
+	priv->status &= ~STATUS_INITIALIZED;
 
-		dev = priv->net_dev;
-		sysfs_remove_group(&pci_dev->dev.kobj,
-				   &ipw2100_attribute_group);
+	sysfs_remove_group(&pci_dev->dev.kobj, &ipw2100_attribute_group);
 
 #ifdef CONFIG_PM
-		if (ipw2100_firmware.version)
-			ipw2100_release_firmware(priv, &ipw2100_firmware);
+	if (ipw2100_firmware.version)
+		ipw2100_release_firmware(priv, &ipw2100_firmware);
 #endif
-		/* Take down the hardware */
-		ipw2100_down(priv);
+	/* Take down the hardware */
+	ipw2100_down(priv);
 
-		/* Release the mutex so that the network subsystem can
-		 * complete any needed calls into the driver... */
-		mutex_unlock(&priv->action_mutex);
+	/* Release the mutex so that the network subsystem can
+	 * complete any needed calls into the driver... */
+	mutex_unlock(&priv->action_mutex);
 
-		/* Unregister the device first - this results in close()
-		 * being called if the device is open.  If we free storage
-		 * first, then close() will crash. */
-		unregister_netdev(dev);
+	/* Unregister the device first - this results in close()
+	 * being called if the device is open.  If we free storage
+	 * first, then close() will crash.
+	 * FIXME: remove the comment above. */
+	unregister_netdev(dev);
 
-		ipw2100_kill_works(priv);
+	ipw2100_kill_works(priv);
 
-		ipw2100_queues_free(priv);
+	ipw2100_queues_free(priv);
 
-		/* Free potential debugging firmware snapshot */
-		ipw2100_snapshot_free(priv);
+	/* Free potential debugging firmware snapshot */
+	ipw2100_snapshot_free(priv);
 
-		if (dev->irq)
-			free_irq(dev->irq, priv);
+	free_irq(dev->irq, priv);
 
-		if (dev->base_addr)
-			iounmap((void __iomem *)dev->base_addr);
+	pci_iounmap(pci_dev, priv->ioaddr);
 
-		/* wiphy_unregister needs to be here, before free_libipw */
-		wiphy_unregister(priv->ieee->wdev.wiphy);
-		kfree(priv->ieee->bg_band.channels);
-		free_libipw(dev, 0);
-	}
+	/* wiphy_unregister needs to be here, before free_libipw */
+	wiphy_unregister(priv->ieee->wdev.wiphy);
+	kfree(priv->ieee->bg_band.channels);
+	free_libipw(dev, 0);
 
 	pci_release_regions(pci_dev);
 	pci_disable_device(pci_dev);
@@ -8508,8 +8509,7 @@ static void ipw2100_release_firmware(struct ipw2100_priv *priv,
 				     struct ipw2100_fw *fw)
 {
 	fw->version = 0;
-	if (fw->fw_entry)
-		release_firmware(fw->fw_entry);
+	release_firmware(fw->fw_entry);
 	fw->fw_entry = NULL;
 }
 
@@ -8609,7 +8609,7 @@ static int ipw2100_ucode_download(struct ipw2100_priv *priv,
 	struct net_device *dev = priv->net_dev;
 	const unsigned char *microcode_data = fw->uc.data;
 	unsigned int microcode_data_left = fw->uc.size;
-	void __iomem *reg = (void __iomem *)dev->base_addr;
+	void __iomem *reg = priv->ioaddr;
 
 	struct symbol_alive_response response;
 	int i, j;
diff --git a/drivers/net/wireless/ipw2x00/ipw2100.h b/drivers/net/wireless/ipw2x00/ipw2100.h
index 99cba968aa5..97312524249 100644
--- a/drivers/net/wireless/ipw2x00/ipw2100.h
+++ b/drivers/net/wireless/ipw2x00/ipw2100.h
@@ -135,15 +135,6 @@ enum {
 	IPW_HW_STATE_ENABLED = 0
 };
 
-struct ssid_context {
-	char ssid[IW_ESSID_MAX_SIZE + 1];
-	int ssid_len;
-	unsigned char bssid[ETH_ALEN];
-	int port_type;
-	int channel;
-
-};
-
 extern const char *port_type_str[];
 extern const char *band_str[];
 
@@ -488,6 +479,7 @@ enum {
 #define CAP_PRIVACY_ON          (1<<1)	/* Off = No privacy */
 
 struct ipw2100_priv {
+	void __iomem *ioaddr;
 
 	int stop_hang_check;	/* Set 1 when shutting down to kill hang_check */
 	int stop_rf_kill;	/* Set 1 when shutting down to kill rf_kill */
diff --git a/drivers/net/wireless/ipw2x00/ipw2200.c b/drivers/net/wireless/ipw2x00/ipw2200.c
index 1779db3aa2b..0036737fe8e 100644
--- a/drivers/net/wireless/ipw2x00/ipw2200.c
+++ b/drivers/net/wireless/ipw2x00/ipw2200.c
@@ -34,6 +34,7 @@
 #include <linux/slab.h>
 #include <net/cfg80211-wext.h>
 #include "ipw2200.h"
+#include "ipw.h"
 
 
 #ifndef KBUILD_EXTMOD
@@ -3668,8 +3669,7 @@ static int ipw_load(struct ipw_priv *priv)
 		priv->rxq = NULL;
 	}
 	ipw_tx_queue_free(priv);
-	if (raw)
-		release_firmware(raw);
+	release_firmware(raw);
 #ifdef CONFIG_PM
 	fw_loaded = 0;
 	raw = NULL;
@@ -7035,7 +7035,7 @@ static int ipw_qos_activate(struct ipw_priv *priv,
 			    cpu_to_le16(burst_duration);
 	} else if (priv->ieee->iw_mode == IW_MODE_ADHOC) {
 		if (type == IEEE_B) {
-			IPW_DEBUG_QOS("QoS activate IBSS nework mode %d\n",
+			IPW_DEBUG_QOS("QoS activate IBSS network mode %d\n",
 				      type);
 			if (priv->qos_data.qos_enable == 0)
 				active_one = &def_parameters_CCK;
@@ -11443,20 +11443,6 @@ static void ipw_bg_down(struct work_struct *work)
 	mutex_unlock(&priv->mutex);
 }
 
-/* Called by register_netdev() */
-static int ipw_net_init(struct net_device *dev)
-{
-	int rc = 0;
-	struct ipw_priv *priv = libipw_priv(dev);
-
-	mutex_lock(&priv->mutex);
-	if (ipw_up(priv))
-		rc = -EIO;
-	mutex_unlock(&priv->mutex);
-
-	return rc;
-}
-
 static int ipw_wdev_init(struct net_device *dev)
 {
 	int i, rc = 0;
@@ -11544,6 +11530,9 @@ static int ipw_wdev_init(struct net_device *dev)
 		wdev->wiphy->bands[IEEE80211_BAND_5GHZ] = a_band;
 	}
 
+	wdev->wiphy->cipher_suites = ipw_cipher_suites;
+	wdev->wiphy->n_cipher_suites = ARRAY_SIZE(ipw_cipher_suites);
+
 	set_wiphy_dev(wdev->wiphy, &priv->pci_dev->dev);
 
 	/* With that information in place, we can now register the wiphy... */
@@ -11722,7 +11711,6 @@ static void ipw_prom_free(struct ipw_priv *priv)
 #endif
 
 static const struct net_device_ops ipw_netdev_ops = {
-	.ndo_init		= ipw_net_init,
 	.ndo_open		= ipw_net_open,
 	.ndo_stop		= ipw_net_stop,
 	.ndo_set_rx_mode	= ipw_net_set_multicast_list,
@@ -11837,10 +11825,6 @@ static int __devinit ipw_pci_probe(struct pci_dev *pdev,
 	net_dev->wireless_data = &priv->wireless_data;
 	net_dev->wireless_handlers = &ipw_wx_handler_def;
 	net_dev->ethtool_ops = &ipw_ethtool_ops;
-	net_dev->irq = pdev->irq;
-	net_dev->base_addr = (unsigned long)priv->hw_base;
-	net_dev->mem_start = pci_resource_start(pdev, 0);
-	net_dev->mem_end = net_dev->mem_start + pci_resource_len(pdev, 0) - 1;
 
 	err = sysfs_create_group(&pdev->dev.kobj, &ipw_attribute_group);
 	if (err) {
@@ -11849,17 +11833,24 @@ static int __devinit ipw_pci_probe(struct pci_dev *pdev,
 		goto out_release_irq;
 	}
 
-	mutex_unlock(&priv->mutex);
-	err = register_netdev(net_dev);
-	if (err) {
-		IPW_ERROR("failed to register network device\n");
+	if (ipw_up(priv)) {
+		mutex_unlock(&priv->mutex);
+		err = -EIO;
 		goto out_remove_sysfs;
 	}
 
+	mutex_unlock(&priv->mutex);
+
 	err = ipw_wdev_init(net_dev);
 	if (err) {
 		IPW_ERROR("failed to register wireless device\n");
-		goto out_unregister_netdev;
+		goto out_remove_sysfs;
+	}
+
+	err = register_netdev(net_dev);
+	if (err) {
+		IPW_ERROR("failed to register network device\n");
+		goto out_unregister_wiphy;
 	}
 
 #ifdef CONFIG_IPW2200_PROMISCUOUS
@@ -11868,10 +11859,8 @@ static int __devinit ipw_pci_probe(struct pci_dev *pdev,
 		if (err) {
 			IPW_ERROR("Failed to register promiscuous network "
 				  "device (error %d).\n", err);
-			wiphy_unregister(priv->ieee->wdev.wiphy);
-			kfree(priv->ieee->a_band.channels);
-			kfree(priv->ieee->bg_band.channels);
-			goto out_unregister_netdev;
+			unregister_netdev(priv->net_dev);
+			goto out_unregister_wiphy;
 		}
 	}
 #endif
@@ -11883,8 +11872,10 @@ static int __devinit ipw_pci_probe(struct pci_dev *pdev,
 
 	return 0;
 
-      out_unregister_netdev:
-	unregister_netdev(priv->net_dev);
+      out_unregister_wiphy:
+	wiphy_unregister(priv->ieee->wdev.wiphy);
+	kfree(priv->ieee->a_band.channels);
+	kfree(priv->ieee->bg_band.channels);
       out_remove_sysfs:
 	sysfs_remove_group(&pdev->dev.kobj, &ipw_attribute_group);
       out_release_irq:
diff --git a/drivers/net/wireless/ipw2x00/libipw.h b/drivers/net/wireless/ipw2x00/libipw.h
index 8874588fb92..0b22fb42173 100644
--- a/drivers/net/wireless/ipw2x00/libipw.h
+++ b/drivers/net/wireless/ipw2x00/libipw.h
@@ -584,61 +584,6 @@ struct libipw_tim_parameters {
 
 /*******************************************************/
 
-enum {				/* libipw_basic_report.map */
-	LIBIPW_BASIC_MAP_BSS = (1 << 0),
-	LIBIPW_BASIC_MAP_OFDM = (1 << 1),
-	LIBIPW_BASIC_MAP_UNIDENTIFIED = (1 << 2),
-	LIBIPW_BASIC_MAP_RADAR = (1 << 3),
-	LIBIPW_BASIC_MAP_UNMEASURED = (1 << 4),
-	/* Bits 5-7 are reserved */
-
-};
-struct libipw_basic_report {
-	u8 channel;
-	__le64 start_time;
-	__le16 duration;
-	u8 map;
-} __packed;
-
-enum {				/* libipw_measurement_request.mode */
-	/* Bit 0 is reserved */
-	LIBIPW_MEASUREMENT_ENABLE = (1 << 1),
-	LIBIPW_MEASUREMENT_REQUEST = (1 << 2),
-	LIBIPW_MEASUREMENT_REPORT = (1 << 3),
-	/* Bits 4-7 are reserved */
-};
-
-enum {
-	LIBIPW_REPORT_BASIC = 0,	/* required */
-	LIBIPW_REPORT_CCA = 1,	/* optional */
-	LIBIPW_REPORT_RPI = 2,	/* optional */
-	/* 3-255 reserved */
-};
-
-struct libipw_measurement_params {
-	u8 channel;
-	__le64 start_time;
-	__le16 duration;
-} __packed;
-
-struct libipw_measurement_request {
-	struct libipw_info_element ie;
-	u8 token;
-	u8 mode;
-	u8 type;
-	struct libipw_measurement_params params[0];
-} __packed;
-
-struct libipw_measurement_report {
-	struct libipw_info_element ie;
-	u8 token;
-	u8 mode;
-	u8 type;
-	union {
-		struct libipw_basic_report basic[0];
-	} u;
-} __packed;
-
 struct libipw_tpc_report {
 	u8 transmit_power;
 	u8 link_margin;
diff --git a/drivers/net/wireless/ipw2x00/libipw_rx.c b/drivers/net/wireless/ipw2x00/libipw_rx.c
index c4955d25a19..02e05792323 100644
--- a/drivers/net/wireless/ipw2x00/libipw_rx.c
+++ b/drivers/net/wireless/ipw2x00/libipw_rx.c
@@ -77,8 +77,8 @@ static struct libipw_frag_entry *libipw_frag_cache_find(struct
 
 		if (entry->skb != NULL && entry->seq == seq &&
 		    (entry->last_frag + 1 == frag || frag == -1) &&
-		    !compare_ether_addr(entry->src_addr, src) &&
-		    !compare_ether_addr(entry->dst_addr, dst))
+		    ether_addr_equal(entry->src_addr, src) &&
+		    ether_addr_equal(entry->dst_addr, dst))
 			return entry;
 	}
 
@@ -245,12 +245,12 @@ static int libipw_is_eapol_frame(struct libipw_device *ieee,
 	/* check that the frame is unicast frame to us */
 	if ((fc & (IEEE80211_FCTL_TODS | IEEE80211_FCTL_FROMDS)) ==
 	    IEEE80211_FCTL_TODS &&
-	    !compare_ether_addr(hdr->addr1, dev->dev_addr) &&
-	    !compare_ether_addr(hdr->addr3, dev->dev_addr)) {
+	    ether_addr_equal(hdr->addr1, dev->dev_addr) &&
+	    ether_addr_equal(hdr->addr3, dev->dev_addr)) {
 		/* ToDS frame with own addr BSSID and DA */
 	} else if ((fc & (IEEE80211_FCTL_TODS | IEEE80211_FCTL_FROMDS)) ==
 		   IEEE80211_FCTL_FROMDS &&
-		   !compare_ether_addr(hdr->addr1, dev->dev_addr)) {
+		   ether_addr_equal(hdr->addr1, dev->dev_addr)) {
 		/* FromDS frame with own addr as DA */
 	} else
 		return 0;
@@ -523,8 +523,8 @@ int libipw_rx(struct libipw_device *ieee, struct sk_buff *skb,
 
 	if (ieee->iw_mode == IW_MODE_MASTER && !wds &&
 	    (fc & (IEEE80211_FCTL_TODS | IEEE80211_FCTL_FROMDS)) ==
-	    IEEE80211_FCTL_FROMDS && ieee->stadev
-	    && !compare_ether_addr(hdr->addr2, ieee->assoc_ap_addr)) {
+	    IEEE80211_FCTL_FROMDS && ieee->stadev &&
+	    ether_addr_equal(hdr->addr2, ieee->assoc_ap_addr)) {
 		/* Frame from BSSID of the AP for which we are a client */
 		skb->dev = dev = ieee->stadev;
 		stats = hostap_get_stats(dev);
@@ -1468,7 +1468,7 @@ static inline int is_same_network(struct libipw_network *src,
 	 * as one network */
 	return ((src->ssid_len == dst->ssid_len) &&
 		(src->channel == dst->channel) &&
-		!compare_ether_addr(src->bssid, dst->bssid) &&
+		ether_addr_equal(src->bssid, dst->bssid) &&
 		!memcmp(src->ssid, dst->ssid, src->ssid_len));
 }
 
diff --git a/drivers/net/wireless/iwlegacy/3945.c b/drivers/net/wireless/iwlegacy/3945.c
index b25c01be0d9..87e53989433 100644
--- a/drivers/net/wireless/iwlegacy/3945.c
+++ b/drivers/net/wireless/iwlegacy/3945.c
@@ -453,10 +453,10 @@ il3945_is_network_packet(struct il_priv *il, struct ieee80211_hdr *header)
 	switch (il->iw_mode) {
 	case NL80211_IFTYPE_ADHOC:	/* Header: Dest. | Source    | BSSID */
 		/* packets to our IBSS update information */
-		return !compare_ether_addr(header->addr3, il->bssid);
+		return ether_addr_equal(header->addr3, il->bssid);
 	case NL80211_IFTYPE_STATION:	/* Header: Dest. | AP{BSSID} | Source */
 		/* packets to our IBSS update information */
-		return !compare_ether_addr(header->addr2, il->bssid);
+		return ether_addr_equal(header->addr2, il->bssid);
 	default:
 		return 1;
 	}
diff --git a/drivers/net/wireless/iwlegacy/4965-mac.c b/drivers/net/wireless/iwlegacy/4965-mac.c
index c46275a9256..509301a5e7e 100644
--- a/drivers/net/wireless/iwlegacy/4965-mac.c
+++ b/drivers/net/wireless/iwlegacy/4965-mac.c
@@ -2565,7 +2565,7 @@ il4965_find_station(struct il_priv *il, const u8 *addr)
 	spin_lock_irqsave(&il->sta_lock, flags);
 	for (i = start; i < il->hw_params.max_stations; i++)
 		if (il->stations[i].used &&
-		    (!compare_ether_addr(il->stations[i].sta.sta.addr, addr))) {
+		    ether_addr_equal(il->stations[i].sta.sta.addr, addr)) {
 			ret = i;
 			goto out;
 		}
@@ -2850,9 +2850,9 @@ void
 il4965_hwrate_to_tx_control(struct il_priv *il, u32 rate_n_flags,
 			    struct ieee80211_tx_info *info)
 {
-	struct ieee80211_tx_rate *r = &info->control.rates[0];
+	struct ieee80211_tx_rate *r = &info->status.rates[0];
 
-	info->antenna_sel_tx =
+	info->status.antenna =
 	    ((rate_n_flags & RATE_MCS_ANT_ABC_MSK) >> RATE_MCS_ANT_POS);
 	if (rate_n_flags & RATE_MCS_HT_MSK)
 		r->flags |= IEEE80211_TX_RC_MCS;
diff --git a/drivers/net/wireless/iwlegacy/4965-rs.c b/drivers/net/wireless/iwlegacy/4965-rs.c
index 11ab1247fae..f3b8e91aa3d 100644
--- a/drivers/net/wireless/iwlegacy/4965-rs.c
+++ b/drivers/net/wireless/iwlegacy/4965-rs.c
@@ -873,7 +873,7 @@ il4965_rs_tx_status(void *il_r, struct ieee80211_supported_band *sband,
 	    tbl_type.is_SGI != !!(mac_flags & IEEE80211_TX_RC_SHORT_GI) ||
 	    tbl_type.is_ht40 != !!(mac_flags & IEEE80211_TX_RC_40_MHZ_WIDTH) ||
 	    tbl_type.is_dup != !!(mac_flags & IEEE80211_TX_RC_DUP_DATA) ||
-	    tbl_type.ant_type != info->antenna_sel_tx ||
+	    tbl_type.ant_type != info->status.antenna ||
 	    !!(tx_rate & RATE_MCS_HT_MSK) != !!(mac_flags & IEEE80211_TX_RC_MCS)
 	    || !!(tx_rate & RATE_MCS_GF_MSK) !=
 	    !!(mac_flags & IEEE80211_TX_RC_GREEN_FIELD) || rs_idx != mac_idx) {
diff --git a/drivers/net/wireless/iwlegacy/common.c b/drivers/net/wireless/iwlegacy/common.c
index eaf249452e5..cbf2dc18341 100644
--- a/drivers/net/wireless/iwlegacy/common.c
+++ b/drivers/net/wireless/iwlegacy/common.c
@@ -1896,8 +1896,8 @@ il_prep_station(struct il_priv *il, const u8 *addr, bool is_ap,
 		sta_id = il->hw_params.bcast_id;
 	else
 		for (i = IL_STA_ID; i < il->hw_params.max_stations; i++) {
-			if (!compare_ether_addr
-			    (il->stations[i].sta.sta.addr, addr)) {
+			if (ether_addr_equal(il->stations[i].sta.sta.addr,
+					     addr)) {
 				sta_id = i;
 				break;
 			}
@@ -1926,7 +1926,7 @@ il_prep_station(struct il_priv *il, const u8 *addr, bool is_ap,
 
 	if ((il->stations[sta_id].used & IL_STA_DRIVER_ACTIVE) &&
 	    (il->stations[sta_id].used & IL_STA_UCODE_ACTIVE) &&
-	    !compare_ether_addr(il->stations[sta_id].sta.sta.addr, addr)) {
+	    ether_addr_equal(il->stations[sta_id].sta.sta.addr, addr)) {
 		D_ASSOC("STA %d (%pM) already added, not adding again.\n",
 			sta_id, addr);
 		return sta_id;
@@ -3744,10 +3744,10 @@ il_full_rxon_required(struct il_priv *il)
 
 	/* These items are only settable from the full RXON command */
 	CHK(!il_is_associated(il));
-	CHK(compare_ether_addr(staging->bssid_addr, active->bssid_addr));
-	CHK(compare_ether_addr(staging->node_addr, active->node_addr));
-	CHK(compare_ether_addr
-	    (staging->wlap_bssid_addr, active->wlap_bssid_addr));
+	CHK(!ether_addr_equal(staging->bssid_addr, active->bssid_addr));
+	CHK(!ether_addr_equal(staging->node_addr, active->node_addr));
+	CHK(!ether_addr_equal(staging->wlap_bssid_addr,
+			      active->wlap_bssid_addr));
 	CHK_NEQ(staging->dev_type, active->dev_type);
 	CHK_NEQ(staging->channel, active->channel);
 	CHK_NEQ(staging->air_propagation, active->air_propagation);
diff --git a/drivers/net/wireless/iwlwifi/Kconfig b/drivers/net/wireless/iwlwifi/Kconfig
index 2fe62730ddd..db6c6e52802 100644
--- a/drivers/net/wireless/iwlwifi/Kconfig
+++ b/drivers/net/wireless/iwlwifi/Kconfig
@@ -113,20 +113,21 @@ config IWLWIFI_DEVICE_TESTMODE
 	  generic netlink message via NL80211_TESTMODE channel.
 
 config IWLWIFI_P2P
-       bool "iwlwifi experimental P2P support"
-       depends on IWLWIFI
-       help
-         This option enables experimental P2P support for some devices
-         based on microcode support. Since P2P support is still under
-         development, this option may even enable it for some devices
-         now that turn out to not support it in the future due to
-         microcode restrictions.
+	def_bool y
+	bool "iwlwifi experimental P2P support"
+	depends on IWLWIFI
+	help
+	  This option enables experimental P2P support for some devices
+	  based on microcode support. Since P2P support is still under
+	  development, this option may even enable it for some devices
+	  now that turn out to not support it in the future due to
+	  microcode restrictions.
 
-         To determine if your microcode supports the experimental P2P
-         offered by this option, check if the driver advertises AP
-         support when it is loaded.
+	  To determine if your microcode supports the experimental P2P
+	  offered by this option, check if the driver advertises AP
+	  support when it is loaded.
 
-         Say Y only if you want to experiment with P2P.
+	  Say Y only if you want to experiment with P2P.
 
 config IWLWIFI_EXPERIMENTAL_MFP
 	bool "support MFP (802.11w) even if uCode doesn't advertise"
@@ -136,3 +137,11 @@ config IWLWIFI_EXPERIMENTAL_MFP
 	  even if the microcode doesn't advertise it.
 
 	  Say Y only if you want to experiment with MFP.
+
+config IWLWIFI_UCODE16
+	bool "support uCode 16.0"
+	depends on IWLWIFI
+	help
+	  This option enables support for uCode version 16.0.
+
+	  Say Y if you want to use 16.0 microcode.
diff --git a/drivers/net/wireless/iwlwifi/Makefile b/drivers/net/wireless/iwlwifi/Makefile
index 85d163ed3db..406f297a9a5 100644
--- a/drivers/net/wireless/iwlwifi/Makefile
+++ b/drivers/net/wireless/iwlwifi/Makefile
@@ -5,9 +5,9 @@ iwlwifi-objs		+= iwl-ucode.o iwl-agn-tx.o iwl-debug.o
 iwlwifi-objs		+= iwl-agn-lib.o iwl-agn-calib.o iwl-io.o
 iwlwifi-objs		+= iwl-agn-tt.o iwl-agn-sta.o iwl-agn-rx.o
 
-iwlwifi-objs		+= iwl-core.o iwl-eeprom.o iwl-power.o
+iwlwifi-objs		+= iwl-eeprom.o iwl-power.o
 iwlwifi-objs		+= iwl-scan.o iwl-led.o
-iwlwifi-objs		+= iwl-agn-rxon.o
+iwlwifi-objs		+= iwl-agn-rxon.o iwl-agn-devices.o
 iwlwifi-objs		+= iwl-5000.o
 iwlwifi-objs		+= iwl-6000.o
 iwlwifi-objs		+= iwl-1000.o
@@ -17,6 +17,8 @@ iwlwifi-objs		+= iwl-drv.o
 iwlwifi-objs		+= iwl-notif-wait.o
 iwlwifi-objs		+= iwl-trans-pcie.o iwl-trans-pcie-rx.o iwl-trans-pcie-tx.o
 
+
+iwlwifi-$(CONFIG_IWLWIFI_UCODE16) += iwl-phy-db.o
 iwlwifi-$(CONFIG_IWLWIFI_DEBUGFS) += iwl-debugfs.o
 iwlwifi-$(CONFIG_IWLWIFI_DEVICE_TRACING) += iwl-devtrace.o
 iwlwifi-$(CONFIG_IWLWIFI_DEVICE_TESTMODE) += iwl-testmode.o
diff --git a/drivers/net/wireless/iwlwifi/iwl-1000.c b/drivers/net/wireless/iwlwifi/iwl-1000.c
index 8d80e233bc7..2629a6602df 100644
--- a/drivers/net/wireless/iwlwifi/iwl-1000.c
+++ b/drivers/net/wireless/iwlwifi/iwl-1000.c
@@ -24,26 +24,12 @@
  *
  *****************************************************************************/
 
-#include <linux/kernel.h>
 #include <linux/module.h>
-#include <linux/init.h>
-#include <linux/delay.h>
-#include <linux/skbuff.h>
-#include <linux/netdevice.h>
-#include <net/mac80211.h>
-#include <linux/etherdevice.h>
-#include <asm/unaligned.h>
 #include <linux/stringify.h>
-
-#include "iwl-eeprom.h"
-#include "iwl-dev.h"
-#include "iwl-core.h"
-#include "iwl-io.h"
-#include "iwl-agn.h"
-#include "iwl-agn-hw.h"
-#include "iwl-shared.h"
+#include "iwl-config.h"
 #include "iwl-cfg.h"
-#include "iwl-prph.h"
+#include "iwl-csr.h"
+#include "iwl-agn-hw.h"
 
 /* Highest firmware API version supported */
 #define IWL1000_UCODE_API_MAX 5
@@ -57,6 +43,10 @@
 #define IWL1000_UCODE_API_MIN 1
 #define IWL100_UCODE_API_MIN 5
 
+/* EEPROM version */
+#define EEPROM_1000_TX_POWER_VERSION	(4)
+#define EEPROM_1000_EEPROM_VERSION	(0x15C)
+
 #define IWL1000_FW_PRE "iwlwifi-1000-"
 #define IWL1000_MODULE_FIRMWARE(api) IWL1000_FW_PRE __stringify(api) ".ucode"
 
@@ -64,100 +54,8 @@
 #define IWL100_MODULE_FIRMWARE(api) IWL100_FW_PRE __stringify(api) ".ucode"
 
 
-/*
- * For 1000, use advance thermal throttling critical temperature threshold,
- * but legacy thermal management implementation for now.
- * This is for the reason of 1000 uCode using advance thermal throttling API
- * but not implement ct_kill_exit based on ct_kill exit temperature
- * so the thermal throttling will still based on legacy thermal throttling
- * management.
- * The code here need to be modified once 1000 uCode has the advanced thermal
- * throttling algorithm in place
- */
-static void iwl1000_set_ct_threshold(struct iwl_priv *priv)
-{
-	/* want Celsius */
-	hw_params(priv).ct_kill_threshold = CT_KILL_THRESHOLD_LEGACY;
-	hw_params(priv).ct_kill_exit_threshold = CT_KILL_EXIT_THRESHOLD;
-}
-
-/* NIC configuration for 1000 series */
-static void iwl1000_nic_config(struct iwl_priv *priv)
-{
-	/* set CSR_HW_CONFIG_REG for uCode use */
-	iwl_set_bit(trans(priv), CSR_HW_IF_CONFIG_REG,
-		    CSR_HW_IF_CONFIG_REG_BIT_RADIO_SI |
-		    CSR_HW_IF_CONFIG_REG_BIT_MAC_SI);
-
-	/* Setting digital SVR for 1000 card to 1.32V */
-	/* locking is acquired in iwl_set_bits_mask_prph() function */
-	iwl_set_bits_mask_prph(trans(priv), APMG_DIGITAL_SVR_REG,
-				APMG_SVR_DIGITAL_VOLTAGE_1_32,
-				~APMG_SVR_VOLTAGE_CONFIG_BIT_MSK);
-}
-
-static const struct iwl_sensitivity_ranges iwl1000_sensitivity = {
-	.min_nrg_cck = 95,
-	.auto_corr_min_ofdm = 90,
-	.auto_corr_min_ofdm_mrc = 170,
-	.auto_corr_min_ofdm_x1 = 120,
-	.auto_corr_min_ofdm_mrc_x1 = 240,
-
-	.auto_corr_max_ofdm = 120,
-	.auto_corr_max_ofdm_mrc = 210,
-	.auto_corr_max_ofdm_x1 = 155,
-	.auto_corr_max_ofdm_mrc_x1 = 290,
-
-	.auto_corr_min_cck = 125,
-	.auto_corr_max_cck = 200,
-	.auto_corr_min_cck_mrc = 170,
-	.auto_corr_max_cck_mrc = 400,
-	.nrg_th_cck = 95,
-	.nrg_th_ofdm = 95,
-
-	.barker_corr_th_min = 190,
-	.barker_corr_th_min_mrc = 390,
-	.nrg_th_cca = 62,
-};
-
-static void iwl1000_hw_set_hw_params(struct iwl_priv *priv)
-{
-	hw_params(priv).ht40_channel =  BIT(IEEE80211_BAND_2GHZ);
-
-	hw_params(priv).tx_chains_num =
-		num_of_ant(hw_params(priv).valid_tx_ant);
-	if (cfg(priv)->rx_with_siso_diversity)
-		hw_params(priv).rx_chains_num = 1;
-	else
-		hw_params(priv).rx_chains_num =
-			num_of_ant(hw_params(priv).valid_rx_ant);
-
-	iwl1000_set_ct_threshold(priv);
-
-	/* Set initial sensitivity parameters */
-	hw_params(priv).sens = &iwl1000_sensitivity;
-}
-
-static struct iwl_lib_ops iwl1000_lib = {
-	.set_hw_params = iwl1000_hw_set_hw_params,
-	.nic_config = iwl1000_nic_config,
-	.eeprom_ops = {
-		.regulatory_bands = {
-			EEPROM_REG_BAND_1_CHANNELS,
-			EEPROM_REG_BAND_2_CHANNELS,
-			EEPROM_REG_BAND_3_CHANNELS,
-			EEPROM_REG_BAND_4_CHANNELS,
-			EEPROM_REG_BAND_5_CHANNELS,
-			EEPROM_REG_BAND_24_HT40_CHANNELS,
-			EEPROM_REGULATORY_BAND_NO_HT40,
-		},
-	},
-	.temperature = iwlagn_temperature,
-};
-
 static const struct iwl_base_params iwl1000_base_params = {
 	.num_of_queues = IWLAGN_NUM_QUEUES,
-	.num_of_ampdu_queues = IWLAGN_NUM_AMPDU_QUEUES,
 	.eeprom_size = OTP_LOW_IMAGE_SIZE,
 	.pll_cfg_val = CSR50_ANA_PLL_CFG_VAL,
 	.max_ll_items = OTP_MAX_LL_ITEMS_1000,
@@ -166,15 +64,13 @@ static const struct iwl_base_params iwl1000_base_params = {
 	.support_ct_kill_exit = true,
 	.plcp_delta_threshold = IWL_MAX_PLCP_ERR_EXT_LONG_THRESHOLD_DEF,
 	.chain_noise_scale = 1000,
-	.wd_timeout = IWL_DEF_WD_TIMEOUT,
+	.wd_timeout = IWL_WATCHHDOG_DISABLED,
 	.max_event_log_size = 128,
-	.wd_disable = true,
 };
 
 static const struct iwl_ht_params iwl1000_ht_params = {
 	.ht_greenfield_support = true,
 	.use_rts_for_aggregation = true, /* use rts/cts protection */
-	.smps_mode = IEEE80211_SMPS_DYNAMIC,
 };
 
 #define IWL_DEVICE_1000						\
@@ -182,11 +78,11 @@ static const struct iwl_ht_params iwl1000_ht_params = {
 	.ucode_api_max = IWL1000_UCODE_API_MAX,			\
 	.ucode_api_ok = IWL1000_UCODE_API_OK,			\
 	.ucode_api_min = IWL1000_UCODE_API_MIN,			\
+	.device_family = IWL_DEVICE_FAMILY_1000,		\
 	.max_inst_size = IWLAGN_RTC_INST_SIZE,			\
 	.max_data_size = IWLAGN_RTC_DATA_SIZE,			\
 	.eeprom_ver = EEPROM_1000_EEPROM_VERSION,		\
 	.eeprom_calib_ver = EEPROM_1000_TX_POWER_VERSION,	\
-	.lib = &iwl1000_lib,					\
 	.base_params = &iwl1000_base_params,			\
 	.led_mode = IWL_LED_BLINK
 
@@ -206,11 +102,11 @@ const struct iwl_cfg iwl1000_bg_cfg = {
 	.ucode_api_max = IWL100_UCODE_API_MAX,			\
 	.ucode_api_ok = IWL100_UCODE_API_OK,			\
 	.ucode_api_min = IWL100_UCODE_API_MIN,			\
+	.device_family = IWL_DEVICE_FAMILY_100,			\
 	.max_inst_size = IWLAGN_RTC_INST_SIZE,			\
 	.max_data_size = IWLAGN_RTC_DATA_SIZE,			\
 	.eeprom_ver = EEPROM_1000_EEPROM_VERSION,		\
 	.eeprom_calib_ver = EEPROM_1000_TX_POWER_VERSION,	\
-	.lib = &iwl1000_lib,					\
 	.base_params = &iwl1000_base_params,			\
 	.led_mode = IWL_LED_RF_STATE,				\
 	.rx_with_siso_diversity = true
diff --git a/drivers/net/wireless/iwlwifi/iwl-2000.c b/drivers/net/wireless/iwlwifi/iwl-2000.c
index ea108622e0b..7f793417c78 100644
--- a/drivers/net/wireless/iwlwifi/iwl-2000.c
+++ b/drivers/net/wireless/iwlwifi/iwl-2000.c
@@ -24,25 +24,12 @@
  *
  *****************************************************************************/
 
-#include <linux/kernel.h>
 #include <linux/module.h>
-#include <linux/init.h>
-#include <linux/delay.h>
-#include <linux/skbuff.h>
-#include <linux/netdevice.h>
-#include <net/mac80211.h>
-#include <linux/etherdevice.h>
-#include <asm/unaligned.h>
 #include <linux/stringify.h>
-
-#include "iwl-eeprom.h"
-#include "iwl-dev.h"
-#include "iwl-core.h"
-#include "iwl-io.h"
-#include "iwl-agn.h"
-#include "iwl-agn-hw.h"
-#include "iwl-shared.h"
+#include "iwl-config.h"
 #include "iwl-cfg.h"
+#include "iwl-agn-hw.h"
+#include "iwl-commands.h" /* needed for BT for now */
 
 /* Highest firmware API version supported */
 #define IWL2030_UCODE_API_MAX 6
@@ -62,6 +49,11 @@
 #define IWL105_UCODE_API_MIN 5
 #define IWL135_UCODE_API_MIN 5
 
+/* EEPROM version */
+#define EEPROM_2000_TX_POWER_VERSION	(6)
+#define EEPROM_2000_EEPROM_VERSION	(0x805)
+
+
 #define IWL2030_FW_PRE "iwlwifi-2030-"
 #define IWL2030_MODULE_FIRMWARE(api) IWL2030_FW_PRE __stringify(api) ".ucode"
 
@@ -74,105 +66,9 @@
 #define IWL135_FW_PRE "iwlwifi-135-"
 #define IWL135_MODULE_FIRMWARE(api) IWL135_FW_PRE __stringify(api) ".ucode"
 
-static void iwl2000_set_ct_threshold(struct iwl_priv *priv)
-{
-	/* want Celsius */
-	hw_params(priv).ct_kill_threshold = CT_KILL_THRESHOLD;
-	hw_params(priv).ct_kill_exit_threshold = CT_KILL_EXIT_THRESHOLD;
-}
-
-/* NIC configuration for 2000 series */
-static void iwl2000_nic_config(struct iwl_priv *priv)
-{
-	iwl_rf_config(priv);
-
-	if (cfg(priv)->iq_invert)
-		iwl_set_bit(trans(priv), CSR_GP_DRIVER_REG,
-			    CSR_GP_DRIVER_REG_BIT_RADIO_IQ_INVER);
-}
-
-static const struct iwl_sensitivity_ranges iwl2000_sensitivity = {
-	.min_nrg_cck = 97,
-	.auto_corr_min_ofdm = 80,
-	.auto_corr_min_ofdm_mrc = 128,
-	.auto_corr_min_ofdm_x1 = 105,
-	.auto_corr_min_ofdm_mrc_x1 = 192,
-
-	.auto_corr_max_ofdm = 145,
-	.auto_corr_max_ofdm_mrc = 232,
-	.auto_corr_max_ofdm_x1 = 110,
-	.auto_corr_max_ofdm_mrc_x1 = 232,
-
-	.auto_corr_min_cck = 125,
-	.auto_corr_max_cck = 175,
-	.auto_corr_min_cck_mrc = 160,
-	.auto_corr_max_cck_mrc = 310,
-	.nrg_th_cck = 97,
-	.nrg_th_ofdm = 100,
-
-	.barker_corr_th_min = 190,
-	.barker_corr_th_min_mrc = 390,
-	.nrg_th_cca = 62,
-};
-
-static void iwl2000_hw_set_hw_params(struct iwl_priv *priv)
-{
-	hw_params(priv).ht40_channel =  BIT(IEEE80211_BAND_2GHZ);
-
-	hw_params(priv).tx_chains_num =
-		num_of_ant(hw_params(priv).valid_tx_ant);
-	if (cfg(priv)->rx_with_siso_diversity)
-		hw_params(priv).rx_chains_num = 1;
-	else
-		hw_params(priv).rx_chains_num =
-			num_of_ant(hw_params(priv).valid_rx_ant);
-
-	iwl2000_set_ct_threshold(priv);
-
-	/* Set initial sensitivity parameters */
-	hw_params(priv).sens = &iwl2000_sensitivity;
-}
-
-static struct iwl_lib_ops iwl2000_lib = {
-	.set_hw_params = iwl2000_hw_set_hw_params,
-	.nic_config = iwl2000_nic_config,
-	.eeprom_ops = {
-		.regulatory_bands = {
-			EEPROM_REG_BAND_1_CHANNELS,
-			EEPROM_REG_BAND_2_CHANNELS,
-			EEPROM_REG_BAND_3_CHANNELS,
-			EEPROM_REG_BAND_4_CHANNELS,
-			EEPROM_REG_BAND_5_CHANNELS,
-			EEPROM_6000_REG_BAND_24_HT40_CHANNELS,
-			EEPROM_REGULATORY_BAND_NO_HT40,
-		},
-		.enhanced_txpower = true,
-	},
-	.temperature = iwlagn_temperature,
-};
-
-static struct iwl_lib_ops iwl2030_lib = {
-	.set_hw_params = iwl2000_hw_set_hw_params,
-	.nic_config = iwl2000_nic_config,
-	.eeprom_ops = {
-		.regulatory_bands = {
-			EEPROM_REG_BAND_1_CHANNELS,
-			EEPROM_REG_BAND_2_CHANNELS,
-			EEPROM_REG_BAND_3_CHANNELS,
-			EEPROM_REG_BAND_4_CHANNELS,
-			EEPROM_REG_BAND_5_CHANNELS,
-			EEPROM_6000_REG_BAND_24_HT40_CHANNELS,
-			EEPROM_REGULATORY_BAND_NO_HT40,
-		},
-		.enhanced_txpower = true,
-	},
-	.temperature = iwlagn_temperature,
-};
-
 static const struct iwl_base_params iwl2000_base_params = {
 	.eeprom_size = OTP_LOW_IMAGE_SIZE,
 	.num_of_queues = IWLAGN_NUM_QUEUES,
-	.num_of_ampdu_queues = IWLAGN_NUM_AMPDU_QUEUES,
 	.pll_cfg_val = 0,
 	.max_ll_items = OTP_MAX_LL_ITEMS_2x00,
 	.shadow_ram_support = true,
@@ -191,7 +87,6 @@ static const struct iwl_base_params iwl2000_base_params = {
 static const struct iwl_base_params iwl2030_base_params = {
 	.eeprom_size = OTP_LOW_IMAGE_SIZE,
 	.num_of_queues = IWLAGN_NUM_QUEUES,
-	.num_of_ampdu_queues = IWLAGN_NUM_AMPDU_QUEUES,
 	.pll_cfg_val = 0,
 	.max_ll_items = OTP_MAX_LL_ITEMS_2x00,
 	.shadow_ram_support = true,
@@ -226,16 +121,15 @@ static const struct iwl_bt_params iwl2030_bt_params = {
 	.ucode_api_max = IWL2000_UCODE_API_MAX,			\
 	.ucode_api_ok = IWL2000_UCODE_API_OK,			\
 	.ucode_api_min = IWL2000_UCODE_API_MIN,			\
+	.device_family = IWL_DEVICE_FAMILY_2000,		\
 	.max_inst_size = IWL60_RTC_INST_SIZE,			\
 	.max_data_size = IWL60_RTC_DATA_SIZE,			\
 	.eeprom_ver = EEPROM_2000_EEPROM_VERSION,		\
 	.eeprom_calib_ver = EEPROM_2000_TX_POWER_VERSION,	\
-	.lib = &iwl2000_lib,					\
 	.base_params = &iwl2000_base_params,			\
 	.need_temp_offset_calib = true,				\
 	.temp_offset_v2 = true,					\
-	.led_mode = IWL_LED_RF_STATE,				\
-	.iq_invert = true					\
+	.led_mode = IWL_LED_RF_STATE
 
 const struct iwl_cfg iwl2000_2bgn_cfg = {
 	.name = "Intel(R) Centrino(R) Wireless-N 2200 BGN",
@@ -254,18 +148,17 @@ const struct iwl_cfg iwl2000_2bgn_d_cfg = {
 	.ucode_api_max = IWL2030_UCODE_API_MAX,			\
 	.ucode_api_ok = IWL2030_UCODE_API_OK,			\
 	.ucode_api_min = IWL2030_UCODE_API_MIN,			\
+	.device_family = IWL_DEVICE_FAMILY_2030,		\
 	.max_inst_size = IWL60_RTC_INST_SIZE,			\
 	.max_data_size = IWL60_RTC_DATA_SIZE,			\
 	.eeprom_ver = EEPROM_2000_EEPROM_VERSION,		\
 	.eeprom_calib_ver = EEPROM_2000_TX_POWER_VERSION,	\
-	.lib = &iwl2030_lib,					\
 	.base_params = &iwl2030_base_params,			\
 	.bt_params = &iwl2030_bt_params,			\
 	.need_temp_offset_calib = true,				\
 	.temp_offset_v2 = true,					\
 	.led_mode = IWL_LED_RF_STATE,				\
-	.adv_pm = true,						\
-	.iq_invert = true					\
+	.adv_pm = true
 
 const struct iwl_cfg iwl2030_2bgn_cfg = {
 	.name = "Intel(R) Centrino(R) Wireless-N 2230 BGN",
@@ -278,18 +171,17 @@ const struct iwl_cfg iwl2030_2bgn_cfg = {
 	.ucode_api_max = IWL105_UCODE_API_MAX,			\
 	.ucode_api_ok = IWL105_UCODE_API_OK,			\
 	.ucode_api_min = IWL105_UCODE_API_MIN,			\
+	.device_family = IWL_DEVICE_FAMILY_105,			\
 	.max_inst_size = IWL60_RTC_INST_SIZE,			\
 	.max_data_size = IWL60_RTC_DATA_SIZE,			\
 	.eeprom_ver = EEPROM_2000_EEPROM_VERSION,		\
 	.eeprom_calib_ver = EEPROM_2000_TX_POWER_VERSION,	\
-	.lib = &iwl2000_lib,					\
 	.base_params = &iwl2000_base_params,			\
 	.need_temp_offset_calib = true,				\
 	.temp_offset_v2 = true,					\
 	.led_mode = IWL_LED_RF_STATE,				\
 	.adv_pm = true,						\
-	.rx_with_siso_diversity = true,				\
-	.iq_invert = true					\
+	.rx_with_siso_diversity = true
 
 const struct iwl_cfg iwl105_bgn_cfg = {
 	.name = "Intel(R) Centrino(R) Wireless-N 105 BGN",
@@ -308,19 +200,18 @@ const struct iwl_cfg iwl105_bgn_d_cfg = {
 	.ucode_api_max = IWL135_UCODE_API_MAX,			\
 	.ucode_api_ok = IWL135_UCODE_API_OK,			\
 	.ucode_api_min = IWL135_UCODE_API_MIN,			\
+	.device_family = IWL_DEVICE_FAMILY_135,			\
 	.max_inst_size = IWL60_RTC_INST_SIZE,			\
 	.max_data_size = IWL60_RTC_DATA_SIZE,			\
 	.eeprom_ver = EEPROM_2000_EEPROM_VERSION,		\
 	.eeprom_calib_ver = EEPROM_2000_TX_POWER_VERSION,	\
-	.lib = &iwl2030_lib,					\
 	.base_params = &iwl2030_base_params,			\
 	.bt_params = &iwl2030_bt_params,			\
 	.need_temp_offset_calib = true,				\
 	.temp_offset_v2 = true,					\
 	.led_mode = IWL_LED_RF_STATE,				\
 	.adv_pm = true,						\
-	.rx_with_siso_diversity = true,				\
-	.iq_invert = true					\
+	.rx_with_siso_diversity = true
 
 const struct iwl_cfg iwl135_bgn_cfg = {
 	.name = "Intel(R) Centrino(R) Wireless-N 135 BGN",
diff --git a/drivers/net/wireless/iwlwifi/iwl-5000.c b/drivers/net/wireless/iwlwifi/iwl-5000.c
index de0920c74cd..8e26bc825f2 100644
--- a/drivers/net/wireless/iwlwifi/iwl-5000.c
+++ b/drivers/net/wireless/iwlwifi/iwl-5000.c
@@ -24,28 +24,12 @@
  *
  *****************************************************************************/
 
-#include <linux/kernel.h>
 #include <linux/module.h>
-#include <linux/init.h>
-#include <linux/delay.h>
-#include <linux/sched.h>
-#include <linux/skbuff.h>
-#include <linux/netdevice.h>
-#include <net/mac80211.h>
-#include <linux/etherdevice.h>
-#include <asm/unaligned.h>
 #include <linux/stringify.h>
-
-#include "iwl-eeprom.h"
-#include "iwl-dev.h"
-#include "iwl-core.h"
-#include "iwl-io.h"
-#include "iwl-agn.h"
-#include "iwl-agn-hw.h"
-#include "iwl-trans.h"
-#include "iwl-shared.h"
+#include "iwl-config.h"
 #include "iwl-cfg.h"
-#include "iwl-prph.h"
+#include "iwl-agn-hw.h"
+#include "iwl-csr.h"
 
 /* Highest firmware API version supported */
 #define IWL5000_UCODE_API_MAX 5
@@ -59,268 +43,28 @@
 #define IWL5000_UCODE_API_MIN 1
 #define IWL5150_UCODE_API_MIN 1
 
+/* EEPROM versions */
+#define EEPROM_5000_TX_POWER_VERSION	(4)
+#define EEPROM_5000_EEPROM_VERSION	(0x11A)
+#define EEPROM_5050_TX_POWER_VERSION	(4)
+#define EEPROM_5050_EEPROM_VERSION	(0x21E)
+
 #define IWL5000_FW_PRE "iwlwifi-5000-"
 #define IWL5000_MODULE_FIRMWARE(api) IWL5000_FW_PRE __stringify(api) ".ucode"
 
 #define IWL5150_FW_PRE "iwlwifi-5150-"
 #define IWL5150_MODULE_FIRMWARE(api) IWL5150_FW_PRE __stringify(api) ".ucode"
 
-/* NIC configuration for 5000 series */
-static void iwl5000_nic_config(struct iwl_priv *priv)
-{
-	iwl_rf_config(priv);
-
-	/* W/A : NIC is stuck in a reset state after Early PCIe power off
-	 * (PCIe power is lost before PERST# is asserted),
-	 * causing ME FW to lose ownership and not being able to obtain it back.
-	 */
-	iwl_set_bits_mask_prph(trans(priv), APMG_PS_CTRL_REG,
-				APMG_PS_CTRL_EARLY_PWR_OFF_RESET_DIS,
-				~APMG_PS_CTRL_EARLY_PWR_OFF_RESET_DIS);
-}
-
-static const struct iwl_sensitivity_ranges iwl5000_sensitivity = {
-	.min_nrg_cck = 100,
-	.auto_corr_min_ofdm = 90,
-	.auto_corr_min_ofdm_mrc = 170,
-	.auto_corr_min_ofdm_x1 = 105,
-	.auto_corr_min_ofdm_mrc_x1 = 220,
-
-	.auto_corr_max_ofdm = 120,
-	.auto_corr_max_ofdm_mrc = 210,
-	.auto_corr_max_ofdm_x1 = 120,
-	.auto_corr_max_ofdm_mrc_x1 = 240,
-
-	.auto_corr_min_cck = 125,
-	.auto_corr_max_cck = 200,
-	.auto_corr_min_cck_mrc = 200,
-	.auto_corr_max_cck_mrc = 400,
-	.nrg_th_cck = 100,
-	.nrg_th_ofdm = 100,
-
-	.barker_corr_th_min = 190,
-	.barker_corr_th_min_mrc = 390,
-	.nrg_th_cca = 62,
-};
-
-static struct iwl_sensitivity_ranges iwl5150_sensitivity = {
-	.min_nrg_cck = 95,
-	.auto_corr_min_ofdm = 90,
-	.auto_corr_min_ofdm_mrc = 170,
-	.auto_corr_min_ofdm_x1 = 105,
-	.auto_corr_min_ofdm_mrc_x1 = 220,
-
-	.auto_corr_max_ofdm = 120,
-	.auto_corr_max_ofdm_mrc = 210,
-	/* max = min for performance bug in 5150 DSP */
-	.auto_corr_max_ofdm_x1 = 105,
-	.auto_corr_max_ofdm_mrc_x1 = 220,
-
-	.auto_corr_min_cck = 125,
-	.auto_corr_max_cck = 200,
-	.auto_corr_min_cck_mrc = 170,
-	.auto_corr_max_cck_mrc = 400,
-	.nrg_th_cck = 95,
-	.nrg_th_ofdm = 95,
-
-	.barker_corr_th_min = 190,
-	.barker_corr_th_min_mrc = 390,
-	.nrg_th_cca = 62,
-};
-
-#define IWL_5150_VOLTAGE_TO_TEMPERATURE_COEFF	(-5)
-
-static s32 iwl_temp_calib_to_offset(struct iwl_shared *shrd)
-{
-	u16 temperature, voltage;
-	__le16 *temp_calib = (__le16 *)iwl_eeprom_query_addr(shrd,
-				EEPROM_KELVIN_TEMPERATURE);
-
-	temperature = le16_to_cpu(temp_calib[0]);
-	voltage = le16_to_cpu(temp_calib[1]);
-
-	/* offset = temp - volt / coeff */
-	return (s32)(temperature - voltage / IWL_5150_VOLTAGE_TO_TEMPERATURE_COEFF);
-}
-
-static void iwl5150_set_ct_threshold(struct iwl_priv *priv)
-{
-	const s32 volt2temp_coef = IWL_5150_VOLTAGE_TO_TEMPERATURE_COEFF;
-	s32 threshold = (s32)CELSIUS_TO_KELVIN(CT_KILL_THRESHOLD_LEGACY) -
-			iwl_temp_calib_to_offset(priv->shrd);
-
-	hw_params(priv).ct_kill_threshold = threshold * volt2temp_coef;
-}
-
-static void iwl5000_set_ct_threshold(struct iwl_priv *priv)
-{
-	/* want Celsius */
-	hw_params(priv).ct_kill_threshold = CT_KILL_THRESHOLD_LEGACY;
-}
-
-static void iwl5000_hw_set_hw_params(struct iwl_priv *priv)
-{
-	hw_params(priv).ht40_channel =  BIT(IEEE80211_BAND_2GHZ) |
-					BIT(IEEE80211_BAND_5GHZ);
-
-	hw_params(priv).tx_chains_num =
-		num_of_ant(hw_params(priv).valid_tx_ant);
-	hw_params(priv).rx_chains_num =
-		num_of_ant(hw_params(priv).valid_rx_ant);
-
-	iwl5000_set_ct_threshold(priv);
-
-	/* Set initial sensitivity parameters */
-	hw_params(priv).sens = &iwl5000_sensitivity;
-}
-
-static void iwl5150_hw_set_hw_params(struct iwl_priv *priv)
-{
-	hw_params(priv).ht40_channel =  BIT(IEEE80211_BAND_2GHZ) |
-					BIT(IEEE80211_BAND_5GHZ);
-
-	hw_params(priv).tx_chains_num =
-		num_of_ant(hw_params(priv).valid_tx_ant);
-	hw_params(priv).rx_chains_num =
-		num_of_ant(hw_params(priv).valid_rx_ant);
-
-	iwl5150_set_ct_threshold(priv);
-
-	/* Set initial sensitivity parameters */
-	hw_params(priv).sens = &iwl5150_sensitivity;
-}
-
-static void iwl5150_temperature(struct iwl_priv *priv)
-{
-	u32 vt = 0;
-	s32 offset =  iwl_temp_calib_to_offset(priv->shrd);
-
-	vt = le32_to_cpu(priv->statistics.common.temperature);
-	vt = vt / IWL_5150_VOLTAGE_TO_TEMPERATURE_COEFF + offset;
-	/* now vt hold the temperature in Kelvin */
-	priv->temperature = KELVIN_TO_CELSIUS(vt);
-	iwl_tt_handler(priv);
-}
-
-static int iwl5000_hw_channel_switch(struct iwl_priv *priv,
-				     struct ieee80211_channel_switch *ch_switch)
-{
-	/*
-	 * MULTI-FIXME
-	 * See iwlagn_mac_channel_switch.
-	 */
-	struct iwl_rxon_context *ctx = &priv->contexts[IWL_RXON_CTX_BSS];
-	struct iwl5000_channel_switch_cmd cmd;
-	const struct iwl_channel_info *ch_info;
-	u32 switch_time_in_usec, ucode_switch_time;
-	u16 ch;
-	u32 tsf_low;
-	u8 switch_count;
-	u16 beacon_interval = le16_to_cpu(ctx->timing.beacon_interval);
-	struct ieee80211_vif *vif = ctx->vif;
-	struct iwl_host_cmd hcmd = {
-		.id = REPLY_CHANNEL_SWITCH,
-		.len = { sizeof(cmd), },
-		.flags = CMD_SYNC,
-		.data = { &cmd, },
-	};
-
-	cmd.band = priv->band == IEEE80211_BAND_2GHZ;
-	ch = ch_switch->channel->hw_value;
-	IWL_DEBUG_11H(priv, "channel switch from %d to %d\n",
-		      ctx->active.channel, ch);
-	cmd.channel = cpu_to_le16(ch);
-	cmd.rxon_flags = ctx->staging.flags;
-	cmd.rxon_filter_flags = ctx->staging.filter_flags;
-	switch_count = ch_switch->count;
-	tsf_low = ch_switch->timestamp & 0x0ffffffff;
-	/*
-	 * calculate the ucode channel switch time
-	 * adding TSF as one of the factor for when to switch
-	 */
-	if ((priv->ucode_beacon_time > tsf_low) && beacon_interval) {
-		if (switch_count > ((priv->ucode_beacon_time - tsf_low) /
-		    beacon_interval)) {
-			switch_count -= (priv->ucode_beacon_time -
-				tsf_low) / beacon_interval;
-		} else
-			switch_count = 0;
-	}
-	if (switch_count <= 1)
-		cmd.switch_time = cpu_to_le32(priv->ucode_beacon_time);
-	else {
-		switch_time_in_usec =
-			vif->bss_conf.beacon_int * switch_count * TIME_UNIT;
-		ucode_switch_time = iwl_usecs_to_beacons(priv,
-							 switch_time_in_usec,
-							 beacon_interval);
-		cmd.switch_time = iwl_add_beacon_time(priv,
-						      priv->ucode_beacon_time,
-						      ucode_switch_time,
-						      beacon_interval);
-	}
-	IWL_DEBUG_11H(priv, "uCode time for the switch is 0x%x\n",
-		      cmd.switch_time);
-	ch_info = iwl_get_channel_info(priv, priv->band, ch);
-	if (ch_info)
-		cmd.expect_beacon = is_channel_radar(ch_info);
-	else {
-		IWL_ERR(priv, "invalid channel switch from %u to %u\n",
-			ctx->active.channel, ch);
-		return -EFAULT;
-	}
-
-	return iwl_dvm_send_cmd(priv, &hcmd);
-}
-
-static struct iwl_lib_ops iwl5000_lib = {
-	.set_hw_params = iwl5000_hw_set_hw_params,
-	.set_channel_switch = iwl5000_hw_channel_switch,
-	.nic_config = iwl5000_nic_config,
-	.eeprom_ops = {
-		.regulatory_bands = {
-			EEPROM_REG_BAND_1_CHANNELS,
-			EEPROM_REG_BAND_2_CHANNELS,
-			EEPROM_REG_BAND_3_CHANNELS,
-			EEPROM_REG_BAND_4_CHANNELS,
-			EEPROM_REG_BAND_5_CHANNELS,
-			EEPROM_REG_BAND_24_HT40_CHANNELS,
-			EEPROM_REG_BAND_52_HT40_CHANNELS
-		},
-	},
-	.temperature = iwlagn_temperature,
-};
-
-static struct iwl_lib_ops iwl5150_lib = {
-	.set_hw_params = iwl5150_hw_set_hw_params,
-	.set_channel_switch = iwl5000_hw_channel_switch,
-	.nic_config = iwl5000_nic_config,
-	.eeprom_ops = {
-		.regulatory_bands = {
-			EEPROM_REG_BAND_1_CHANNELS,
-			EEPROM_REG_BAND_2_CHANNELS,
-			EEPROM_REG_BAND_3_CHANNELS,
-			EEPROM_REG_BAND_4_CHANNELS,
-			EEPROM_REG_BAND_5_CHANNELS,
-			EEPROM_REG_BAND_24_HT40_CHANNELS,
-			EEPROM_REG_BAND_52_HT40_CHANNELS
-		},
-	},
-	.temperature = iwl5150_temperature,
-};
-
 static const struct iwl_base_params iwl5000_base_params = {
 	.eeprom_size = IWLAGN_EEPROM_IMG_SIZE,
 	.num_of_queues = IWLAGN_NUM_QUEUES,
-	.num_of_ampdu_queues = IWLAGN_NUM_AMPDU_QUEUES,
 	.pll_cfg_val = CSR50_ANA_PLL_CFG_VAL,
 	.led_compensation = 51,
 	.plcp_delta_threshold = IWL_MAX_PLCP_ERR_LONG_THRESHOLD_DEF,
 	.chain_noise_scale = 1000,
-	.wd_timeout = IWL_LONG_WD_TIMEOUT,
+	.wd_timeout = IWL_WATCHHDOG_DISABLED,
 	.max_event_log_size = 512,
 	.no_idle_support = true,
-	.wd_disable = true,
 };
 
 static const struct iwl_ht_params iwl5000_ht_params = {
@@ -332,11 +76,11 @@ static const struct iwl_ht_params iwl5000_ht_params = {
 	.ucode_api_max = IWL5000_UCODE_API_MAX,			\
 	.ucode_api_ok = IWL5000_UCODE_API_OK,			\
 	.ucode_api_min = IWL5000_UCODE_API_MIN,			\
+	.device_family = IWL_DEVICE_FAMILY_5000,		\
 	.max_inst_size = IWLAGN_RTC_INST_SIZE,			\
 	.max_data_size = IWLAGN_RTC_DATA_SIZE,			\
 	.eeprom_ver = EEPROM_5000_EEPROM_VERSION,		\
 	.eeprom_calib_ver = EEPROM_5000_TX_POWER_VERSION,	\
-	.lib = &iwl5000_lib,					\
 	.base_params = &iwl5000_base_params,			\
 	.led_mode = IWL_LED_BLINK
 
@@ -378,11 +122,11 @@ const struct iwl_cfg iwl5350_agn_cfg = {
 	.ucode_api_max = IWL5000_UCODE_API_MAX,
 	.ucode_api_ok = IWL5000_UCODE_API_OK,
 	.ucode_api_min = IWL5000_UCODE_API_MIN,
+	.device_family = IWL_DEVICE_FAMILY_5000,
 	.max_inst_size = IWLAGN_RTC_INST_SIZE,
 	.max_data_size = IWLAGN_RTC_DATA_SIZE,
 	.eeprom_ver = EEPROM_5050_EEPROM_VERSION,
 	.eeprom_calib_ver = EEPROM_5050_TX_POWER_VERSION,
-	.lib = &iwl5000_lib,
 	.base_params = &iwl5000_base_params,
 	.ht_params = &iwl5000_ht_params,
 	.led_mode = IWL_LED_BLINK,
@@ -394,11 +138,11 @@ const struct iwl_cfg iwl5350_agn_cfg = {
 	.ucode_api_max = IWL5150_UCODE_API_MAX,			\
 	.ucode_api_ok = IWL5150_UCODE_API_OK,			\
 	.ucode_api_min = IWL5150_UCODE_API_MIN,			\
+	.device_family = IWL_DEVICE_FAMILY_5150,		\
 	.max_inst_size = IWLAGN_RTC_INST_SIZE,			\
 	.max_data_size = IWLAGN_RTC_DATA_SIZE,			\
 	.eeprom_ver = EEPROM_5050_EEPROM_VERSION,		\
 	.eeprom_calib_ver = EEPROM_5050_TX_POWER_VERSION,	\
-	.lib = &iwl5150_lib,					\
 	.base_params = &iwl5000_base_params,			\
 	.no_xtal_calib = true,					\
 	.led_mode = IWL_LED_BLINK,				\
diff --git a/drivers/net/wireless/iwlwifi/iwl-6000.c b/drivers/net/wireless/iwlwifi/iwl-6000.c
index f0c91505a7f..381b02cf339 100644
--- a/drivers/net/wireless/iwlwifi/iwl-6000.c
+++ b/drivers/net/wireless/iwlwifi/iwl-6000.c
@@ -24,26 +24,12 @@
  *
  *****************************************************************************/
 
-#include <linux/kernel.h>
 #include <linux/module.h>
-#include <linux/init.h>
-#include <linux/delay.h>
-#include <linux/skbuff.h>
-#include <linux/netdevice.h>
-#include <net/mac80211.h>
-#include <linux/etherdevice.h>
-#include <asm/unaligned.h>
 #include <linux/stringify.h>
-
-#include "iwl-eeprom.h"
-#include "iwl-dev.h"
-#include "iwl-core.h"
-#include "iwl-io.h"
-#include "iwl-agn.h"
-#include "iwl-agn-hw.h"
-#include "iwl-trans.h"
-#include "iwl-shared.h"
+#include "iwl-config.h"
 #include "iwl-cfg.h"
+#include "iwl-agn-hw.h"
+#include "iwl-commands.h" /* needed for BT for now */
 
 /* Highest firmware API version supported */
 #define IWL6000_UCODE_API_MAX 6
@@ -61,6 +47,20 @@
 #define IWL6050_UCODE_API_MIN 4
 #define IWL6000G2_UCODE_API_MIN 4
 
+/* EEPROM versions */
+#define EEPROM_6000_TX_POWER_VERSION	(4)
+#define EEPROM_6000_EEPROM_VERSION	(0x423)
+#define EEPROM_6050_TX_POWER_VERSION	(4)
+#define EEPROM_6050_EEPROM_VERSION	(0x532)
+#define EEPROM_6150_TX_POWER_VERSION	(6)
+#define EEPROM_6150_EEPROM_VERSION	(0x553)
+#define EEPROM_6005_TX_POWER_VERSION	(6)
+#define EEPROM_6005_EEPROM_VERSION	(0x709)
+#define EEPROM_6030_TX_POWER_VERSION	(6)
+#define EEPROM_6030_EEPROM_VERSION	(0x709)
+#define EEPROM_6035_TX_POWER_VERSION	(6)
+#define EEPROM_6035_EEPROM_VERSION	(0x753)
+
 #define IWL6000_FW_PRE "iwlwifi-6000-"
 #define IWL6000_MODULE_FIRMWARE(api) IWL6000_FW_PRE __stringify(api) ".ucode"
 
@@ -73,205 +73,9 @@
 #define IWL6030_FW_PRE "iwlwifi-6000g2b-"
 #define IWL6030_MODULE_FIRMWARE(api) IWL6030_FW_PRE __stringify(api) ".ucode"
 
-static void iwl6000_set_ct_threshold(struct iwl_priv *priv)
-{
-	/* want Celsius */
-	hw_params(priv).ct_kill_threshold = CT_KILL_THRESHOLD;
-	hw_params(priv).ct_kill_exit_threshold = CT_KILL_EXIT_THRESHOLD;
-}
-
-static void iwl6050_additional_nic_config(struct iwl_priv *priv)
-{
-	/* Indicate calibration version to uCode. */
-	if (iwl_eeprom_calib_version(priv->shrd) >= 6)
-		iwl_set_bit(trans(priv), CSR_GP_DRIVER_REG,
-				CSR_GP_DRIVER_REG_BIT_CALIB_VERSION6);
-}
-
-static void iwl6150_additional_nic_config(struct iwl_priv *priv)
-{
-	/* Indicate calibration version to uCode. */
-	if (iwl_eeprom_calib_version(priv->shrd) >= 6)
-		iwl_set_bit(trans(priv), CSR_GP_DRIVER_REG,
-				CSR_GP_DRIVER_REG_BIT_CALIB_VERSION6);
-	iwl_set_bit(trans(priv), CSR_GP_DRIVER_REG,
-		    CSR_GP_DRIVER_REG_BIT_6050_1x2);
-}
-
-static void iwl6000i_additional_nic_config(struct iwl_priv *priv)
-{
-	/* 2x2 IPA phy type */
-	iwl_write32(trans(priv), CSR_GP_DRIVER_REG,
-		     CSR_GP_DRIVER_REG_BIT_RADIO_SKU_2x2_IPA);
-}
-
-/* NIC configuration for 6000 series */
-static void iwl6000_nic_config(struct iwl_priv *priv)
-{
-	iwl_rf_config(priv);
-
-	/* do additional nic configuration if needed */
-	if (cfg(priv)->additional_nic_config)
-		cfg(priv)->additional_nic_config(priv);
-}
-
-static const struct iwl_sensitivity_ranges iwl6000_sensitivity = {
-	.min_nrg_cck = 110,
-	.auto_corr_min_ofdm = 80,
-	.auto_corr_min_ofdm_mrc = 128,
-	.auto_corr_min_ofdm_x1 = 105,
-	.auto_corr_min_ofdm_mrc_x1 = 192,
-
-	.auto_corr_max_ofdm = 145,
-	.auto_corr_max_ofdm_mrc = 232,
-	.auto_corr_max_ofdm_x1 = 110,
-	.auto_corr_max_ofdm_mrc_x1 = 232,
-
-	.auto_corr_min_cck = 125,
-	.auto_corr_max_cck = 175,
-	.auto_corr_min_cck_mrc = 160,
-	.auto_corr_max_cck_mrc = 310,
-	.nrg_th_cck = 110,
-	.nrg_th_ofdm = 110,
-
-	.barker_corr_th_min = 190,
-	.barker_corr_th_min_mrc = 336,
-	.nrg_th_cca = 62,
-};
-
-static void iwl6000_hw_set_hw_params(struct iwl_priv *priv)
-{
-	hw_params(priv).ht40_channel =  BIT(IEEE80211_BAND_2GHZ) |
-					BIT(IEEE80211_BAND_5GHZ);
-
-	hw_params(priv).tx_chains_num =
-		num_of_ant(hw_params(priv).valid_tx_ant);
-	if (cfg(priv)->rx_with_siso_diversity)
-		hw_params(priv).rx_chains_num = 1;
-	else
-		hw_params(priv).rx_chains_num =
-			num_of_ant(hw_params(priv).valid_rx_ant);
-
-	iwl6000_set_ct_threshold(priv);
-
-	/* Set initial sensitivity parameters */
-	hw_params(priv).sens = &iwl6000_sensitivity;
-
-}
-
-static int iwl6000_hw_channel_switch(struct iwl_priv *priv,
-				     struct ieee80211_channel_switch *ch_switch)
-{
-	/*
-	 * MULTI-FIXME
-	 * See iwlagn_mac_channel_switch.
-	 */
-	struct iwl_rxon_context *ctx = &priv->contexts[IWL_RXON_CTX_BSS];
-	struct iwl6000_channel_switch_cmd cmd;
-	const struct iwl_channel_info *ch_info;
-	u32 switch_time_in_usec, ucode_switch_time;
-	u16 ch;
-	u32 tsf_low;
-	u8 switch_count;
-	u16 beacon_interval = le16_to_cpu(ctx->timing.beacon_interval);
-	struct ieee80211_vif *vif = ctx->vif;
-	struct iwl_host_cmd hcmd = {
-		.id = REPLY_CHANNEL_SWITCH,
-		.len = { sizeof(cmd), },
-		.flags = CMD_SYNC,
-		.data = { &cmd, },
-	};
-
-	cmd.band = priv->band == IEEE80211_BAND_2GHZ;
-	ch = ch_switch->channel->hw_value;
-	IWL_DEBUG_11H(priv, "channel switch from %u to %u\n",
-		      ctx->active.channel, ch);
-	cmd.channel = cpu_to_le16(ch);
-	cmd.rxon_flags = ctx->staging.flags;
-	cmd.rxon_filter_flags = ctx->staging.filter_flags;
-	switch_count = ch_switch->count;
-	tsf_low = ch_switch->timestamp & 0x0ffffffff;
-	/*
-	 * calculate the ucode channel switch time
-	 * adding TSF as one of the factor for when to switch
-	 */
-	if ((priv->ucode_beacon_time > tsf_low) && beacon_interval) {
-		if (switch_count > ((priv->ucode_beacon_time - tsf_low) /
-		    beacon_interval)) {
-			switch_count -= (priv->ucode_beacon_time -
-				tsf_low) / beacon_interval;
-		} else
-			switch_count = 0;
-	}
-	if (switch_count <= 1)
-		cmd.switch_time = cpu_to_le32(priv->ucode_beacon_time);
-	else {
-		switch_time_in_usec =
-			vif->bss_conf.beacon_int * switch_count * TIME_UNIT;
-		ucode_switch_time = iwl_usecs_to_beacons(priv,
-							 switch_time_in_usec,
-							 beacon_interval);
-		cmd.switch_time = iwl_add_beacon_time(priv,
-						      priv->ucode_beacon_time,
-						      ucode_switch_time,
-						      beacon_interval);
-	}
-	IWL_DEBUG_11H(priv, "uCode time for the switch is 0x%x\n",
-		      cmd.switch_time);
-	ch_info = iwl_get_channel_info(priv, priv->band, ch);
-	if (ch_info)
-		cmd.expect_beacon = is_channel_radar(ch_info);
-	else {
-		IWL_ERR(priv, "invalid channel switch from %u to %u\n",
-			ctx->active.channel, ch);
-		return -EFAULT;
-	}
-
-	return iwl_dvm_send_cmd(priv, &hcmd);
-}
-
-static struct iwl_lib_ops iwl6000_lib = {
-	.set_hw_params = iwl6000_hw_set_hw_params,
-	.set_channel_switch = iwl6000_hw_channel_switch,
-	.nic_config = iwl6000_nic_config,
-	.eeprom_ops = {
-		.regulatory_bands = {
-			EEPROM_REG_BAND_1_CHANNELS,
-			EEPROM_REG_BAND_2_CHANNELS,
-			EEPROM_REG_BAND_3_CHANNELS,
-			EEPROM_REG_BAND_4_CHANNELS,
-			EEPROM_REG_BAND_5_CHANNELS,
-			EEPROM_6000_REG_BAND_24_HT40_CHANNELS,
-			EEPROM_REG_BAND_52_HT40_CHANNELS
-		},
-		.enhanced_txpower = true,
-	},
-	.temperature = iwlagn_temperature,
-};
-
-static struct iwl_lib_ops iwl6030_lib = {
-	.set_hw_params = iwl6000_hw_set_hw_params,
-	.set_channel_switch = iwl6000_hw_channel_switch,
-	.nic_config = iwl6000_nic_config,
-	.eeprom_ops = {
-		.regulatory_bands = {
-			EEPROM_REG_BAND_1_CHANNELS,
-			EEPROM_REG_BAND_2_CHANNELS,
-			EEPROM_REG_BAND_3_CHANNELS,
-			EEPROM_REG_BAND_4_CHANNELS,
-			EEPROM_REG_BAND_5_CHANNELS,
-			EEPROM_6000_REG_BAND_24_HT40_CHANNELS,
-			EEPROM_REG_BAND_52_HT40_CHANNELS
-		},
-		.enhanced_txpower = true,
-	},
-	.temperature = iwlagn_temperature,
-};
-
 static const struct iwl_base_params iwl6000_base_params = {
 	.eeprom_size = OTP_LOW_IMAGE_SIZE,
 	.num_of_queues = IWLAGN_NUM_QUEUES,
-	.num_of_ampdu_queues = IWLAGN_NUM_AMPDU_QUEUES,
 	.pll_cfg_val = 0,
 	.max_ll_items = OTP_MAX_LL_ITEMS_6x00,
 	.shadow_ram_support = true,
@@ -288,7 +92,6 @@ static const struct iwl_base_params iwl6000_base_params = {
 static const struct iwl_base_params iwl6050_base_params = {
 	.eeprom_size = OTP_LOW_IMAGE_SIZE,
 	.num_of_queues = IWLAGN_NUM_QUEUES,
-	.num_of_ampdu_queues = IWLAGN_NUM_AMPDU_QUEUES,
 	.pll_cfg_val = 0,
 	.max_ll_items = OTP_MAX_LL_ITEMS_6x50,
 	.shadow_ram_support = true,
@@ -305,7 +108,6 @@ static const struct iwl_base_params iwl6050_base_params = {
 static const struct iwl_base_params iwl6000_g2_base_params = {
 	.eeprom_size = OTP_LOW_IMAGE_SIZE,
 	.num_of_queues = IWLAGN_NUM_QUEUES,
-	.num_of_ampdu_queues = IWLAGN_NUM_AMPDU_QUEUES,
 	.pll_cfg_val = 0,
 	.max_ll_items = OTP_MAX_LL_ITEMS_6x00,
 	.shadow_ram_support = true,
@@ -338,11 +140,11 @@ static const struct iwl_bt_params iwl6000_bt_params = {
 	.ucode_api_max = IWL6000G2_UCODE_API_MAX,		\
 	.ucode_api_ok = IWL6000G2_UCODE_API_OK,			\
 	.ucode_api_min = IWL6000G2_UCODE_API_MIN,		\
+	.device_family = IWL_DEVICE_FAMILY_6005,		\
 	.max_inst_size = IWL60_RTC_INST_SIZE,			\
 	.max_data_size = IWL60_RTC_DATA_SIZE,			\
 	.eeprom_ver = EEPROM_6005_EEPROM_VERSION,		\
 	.eeprom_calib_ver = EEPROM_6005_TX_POWER_VERSION,	\
-	.lib = &iwl6000_lib,					\
 	.base_params = &iwl6000_g2_base_params,			\
 	.need_temp_offset_calib = true,				\
 	.led_mode = IWL_LED_RF_STATE
@@ -392,11 +194,11 @@ const struct iwl_cfg iwl6005_2agn_mow2_cfg = {
 	.ucode_api_max = IWL6000G2_UCODE_API_MAX,		\
 	.ucode_api_ok = IWL6000G2B_UCODE_API_OK,		\
 	.ucode_api_min = IWL6000G2_UCODE_API_MIN,		\
+	.device_family = IWL_DEVICE_FAMILY_6030,		\
 	.max_inst_size = IWL60_RTC_INST_SIZE,			\
 	.max_data_size = IWL60_RTC_DATA_SIZE,			\
 	.eeprom_ver = EEPROM_6030_EEPROM_VERSION,		\
 	.eeprom_calib_ver = EEPROM_6030_TX_POWER_VERSION,	\
-	.lib = &iwl6030_lib,					\
 	.base_params = &iwl6000_g2_base_params,			\
 	.bt_params = &iwl6000_bt_params,			\
 	.need_temp_offset_calib = true,				\
@@ -463,14 +265,13 @@ const struct iwl_cfg iwl130_bg_cfg = {
 	.ucode_api_max = IWL6000_UCODE_API_MAX,			\
 	.ucode_api_ok = IWL6000_UCODE_API_OK,			\
 	.ucode_api_min = IWL6000_UCODE_API_MIN,			\
+	.device_family = IWL_DEVICE_FAMILY_6000i,		\
 	.max_inst_size = IWL60_RTC_INST_SIZE,			\
 	.max_data_size = IWL60_RTC_DATA_SIZE,			\
 	.valid_tx_ant = ANT_BC,		/* .cfg overwrite */	\
 	.valid_rx_ant = ANT_BC,		/* .cfg overwrite */	\
 	.eeprom_ver = EEPROM_6000_EEPROM_VERSION,		\
 	.eeprom_calib_ver = EEPROM_6000_TX_POWER_VERSION,	\
-	.lib = &iwl6000_lib,					\
-	.additional_nic_config = iwl6000i_additional_nic_config,\
 	.base_params = &iwl6000_base_params,			\
 	.led_mode = IWL_LED_BLINK
 
@@ -494,12 +295,11 @@ const struct iwl_cfg iwl6000i_2bg_cfg = {
 	.fw_name_pre = IWL6050_FW_PRE,				\
 	.ucode_api_max = IWL6050_UCODE_API_MAX,			\
 	.ucode_api_min = IWL6050_UCODE_API_MIN,			\
+	.device_family = IWL_DEVICE_FAMILY_6050,		\
 	.max_inst_size = IWL60_RTC_INST_SIZE,			\
 	.max_data_size = IWL60_RTC_DATA_SIZE,			\
 	.valid_tx_ant = ANT_AB,		/* .cfg overwrite */	\
 	.valid_rx_ant = ANT_AB,		/* .cfg overwrite */	\
-	.lib = &iwl6000_lib,					\
-	.additional_nic_config = iwl6050_additional_nic_config,	\
 	.eeprom_ver = EEPROM_6050_EEPROM_VERSION,		\
 	.eeprom_calib_ver = EEPROM_6050_TX_POWER_VERSION,	\
 	.base_params = &iwl6050_base_params,			\
@@ -521,10 +321,9 @@ const struct iwl_cfg iwl6050_2abg_cfg = {
 	.fw_name_pre = IWL6050_FW_PRE,				\
 	.ucode_api_max = IWL6050_UCODE_API_MAX,			\
 	.ucode_api_min = IWL6050_UCODE_API_MIN,			\
+	.device_family = IWL_DEVICE_FAMILY_6150,		\
 	.max_inst_size = IWL60_RTC_INST_SIZE,			\
 	.max_data_size = IWL60_RTC_DATA_SIZE,			\
-	.lib = &iwl6000_lib,					\
-	.additional_nic_config = iwl6150_additional_nic_config,	\
 	.eeprom_ver = EEPROM_6150_EEPROM_VERSION,		\
 	.eeprom_calib_ver = EEPROM_6150_TX_POWER_VERSION,	\
 	.base_params = &iwl6050_base_params,			\
@@ -548,11 +347,11 @@ const struct iwl_cfg iwl6000_3agn_cfg = {
 	.ucode_api_max = IWL6000_UCODE_API_MAX,
 	.ucode_api_ok = IWL6000_UCODE_API_OK,
 	.ucode_api_min = IWL6000_UCODE_API_MIN,
+	.device_family = IWL_DEVICE_FAMILY_6000,
 	.max_inst_size = IWL60_RTC_INST_SIZE,
 	.max_data_size = IWL60_RTC_DATA_SIZE,
 	.eeprom_ver = EEPROM_6000_EEPROM_VERSION,
 	.eeprom_calib_ver = EEPROM_6000_TX_POWER_VERSION,
-	.lib = &iwl6000_lib,
 	.base_params = &iwl6000_base_params,
 	.ht_params = &iwl6000_ht_params,
 	.led_mode = IWL_LED_BLINK,
diff --git a/drivers/net/wireless/iwlwifi/iwl-agn-calib.c b/drivers/net/wireless/iwlwifi/iwl-agn-calib.c
index 84cbe7bb504..95f27f1a423 100644
--- a/drivers/net/wireless/iwlwifi/iwl-agn-calib.c
+++ b/drivers/net/wireless/iwlwifi/iwl-agn-calib.c
@@ -64,7 +64,6 @@
 #include <net/mac80211.h>
 
 #include "iwl-dev.h"
-#include "iwl-core.h"
 #include "iwl-agn-calib.h"
 #include "iwl-trans.h"
 #include "iwl-agn.h"
@@ -190,7 +189,7 @@ static int iwl_sens_energy_cck(struct iwl_priv *priv,
 	u32 max_false_alarms = MAX_FA_CCK * rx_enable_time;
 	u32 min_false_alarms = MIN_FA_CCK * rx_enable_time;
 	struct iwl_sensitivity_data *data = NULL;
-	const struct iwl_sensitivity_ranges *ranges = hw_params(priv).sens;
+	const struct iwl_sensitivity_ranges *ranges = priv->hw_params.sens;
 
 	data = &(priv->sensitivity_data);
 
@@ -373,7 +372,7 @@ static int iwl_sens_auto_corr_ofdm(struct iwl_priv *priv,
 	u32 max_false_alarms = MAX_FA_OFDM * rx_enable_time;
 	u32 min_false_alarms = MIN_FA_OFDM * rx_enable_time;
 	struct iwl_sensitivity_data *data = NULL;
-	const struct iwl_sensitivity_ranges *ranges = hw_params(priv).sens;
+	const struct iwl_sensitivity_ranges *ranges = priv->hw_params.sens;
 
 	data = &(priv->sensitivity_data);
 
@@ -521,7 +520,7 @@ static int iwl_enhance_sensitivity_write(struct iwl_priv *priv)
 
 	iwl_prepare_legacy_sensitivity_tbl(priv, data, &cmd.enhance_table[0]);
 
-	if (cfg(priv)->base_params->hd_v2) {
+	if (priv->cfg->base_params->hd_v2) {
 		cmd.enhance_table[HD_INA_NON_SQUARE_DET_OFDM_INDEX] =
 			HD_INA_NON_SQUARE_DET_OFDM_DATA_V2;
 		cmd.enhance_table[HD_INA_NON_SQUARE_DET_CCK_INDEX] =
@@ -597,9 +596,9 @@ void iwl_init_sensitivity(struct iwl_priv *priv)
 	int ret = 0;
 	int i;
 	struct iwl_sensitivity_data *data = NULL;
-	const struct iwl_sensitivity_ranges *ranges = hw_params(priv).sens;
+	const struct iwl_sensitivity_ranges *ranges = priv->hw_params.sens;
 
-	if (priv->disable_sens_cal)
+	if (priv->calib_disabled & IWL_SENSITIVITY_CALIB_DISABLED)
 		return;
 
 	IWL_DEBUG_CALIB(priv, "Start iwl_init_sensitivity\n");
@@ -663,7 +662,7 @@ void iwl_sensitivity_calibration(struct iwl_priv *priv)
 	struct statistics_rx_phy *ofdm, *cck;
 	struct statistics_general_data statis;
 
-	if (priv->disable_sens_cal)
+	if (priv->calib_disabled & IWL_SENSITIVITY_CALIB_DISABLED)
 		return;
 
 	data = &(priv->sensitivity_data);
@@ -833,28 +832,28 @@ static void iwl_find_disconn_antenna(struct iwl_priv *priv, u32* average_sig,
 	 * To be safe, simply mask out any chains that we know
 	 * are not on the device.
 	 */
-	active_chains &= hw_params(priv).valid_rx_ant;
+	active_chains &= priv->hw_params.valid_rx_ant;
 
 	num_tx_chains = 0;
 	for (i = 0; i < NUM_RX_CHAINS; i++) {
 		/* loops on all the bits of
 		 * priv->hw_setting.valid_tx_ant */
 		u8 ant_msk = (1 << i);
-		if (!(hw_params(priv).valid_tx_ant & ant_msk))
+		if (!(priv->hw_params.valid_tx_ant & ant_msk))
 			continue;
 
 		num_tx_chains++;
 		if (data->disconn_array[i] == 0)
 			/* there is a Tx antenna connected */
 			break;
-		if (num_tx_chains == hw_params(priv).tx_chains_num &&
+		if (num_tx_chains == priv->hw_params.tx_chains_num &&
 		    data->disconn_array[i]) {
 			/*
 			 * If all chains are disconnected
 			 * connect the first valid tx chain
 			 */
 			first_chain =
-				find_first_chain(hw_params(priv).valid_tx_ant);
+				find_first_chain(priv->hw_params.valid_tx_ant);
 			data->disconn_array[first_chain] = 0;
 			active_chains |= BIT(first_chain);
 			IWL_DEBUG_CALIB(priv,
@@ -864,13 +863,13 @@ static void iwl_find_disconn_antenna(struct iwl_priv *priv, u32* average_sig,
 		}
 	}
 
-	if (active_chains != hw_params(priv).valid_rx_ant &&
+	if (active_chains != priv->hw_params.valid_rx_ant &&
 	    active_chains != priv->chain_noise_data.active_chains)
 		IWL_DEBUG_CALIB(priv,
 				"Detected that not all antennas are connected! "
 				"Connected: %#x, valid: %#x.\n",
 				active_chains,
-				hw_params(priv).valid_rx_ant);
+				priv->hw_params.valid_rx_ant);
 
 	/* Save for use within RXON, TX, SCAN commands, etc. */
 	data->active_chains = active_chains;
@@ -895,7 +894,7 @@ static void iwlagn_gain_computation(struct iwl_priv *priv,
 			continue;
 		}
 
-		delta_g = (cfg(priv)->base_params->chain_noise_scale *
+		delta_g = (priv->cfg->base_params->chain_noise_scale *
 			((s32)average_noise[default_chain] -
 			(s32)average_noise[i])) / 1500;
 
@@ -970,7 +969,7 @@ void iwl_chain_noise_calibration(struct iwl_priv *priv)
 	 */
 	struct iwl_rxon_context *ctx = &priv->contexts[IWL_RXON_CTX_BSS];
 
-	if (priv->disable_chain_noise_cal)
+	if (priv->calib_disabled & IWL_CHAIN_NOISE_CALIB_DISABLED)
 		return;
 
 	data = &(priv->chain_noise_data);
@@ -1051,11 +1050,11 @@ void iwl_chain_noise_calibration(struct iwl_priv *priv)
 		return;
 
 	/* Analyze signal for disconnected antenna */
-	if (cfg(priv)->bt_params &&
-	    cfg(priv)->bt_params->advanced_bt_coexist) {
+	if (priv->cfg->bt_params &&
+	    priv->cfg->bt_params->advanced_bt_coexist) {
 		/* Disable disconnected antenna algorithm for advanced
 		   bt coex, assuming valid antennas are connected */
-		data->active_chains = hw_params(priv).valid_rx_ant;
+		data->active_chains = priv->hw_params.valid_rx_ant;
 		for (i = 0; i < NUM_RX_CHAINS; i++)
 			if (!(data->active_chains & (1<<i)))
 				data->disconn_array[i] = 1;
@@ -1085,7 +1084,7 @@ void iwl_chain_noise_calibration(struct iwl_priv *priv)
 			min_average_noise, min_average_noise_antenna_i);
 
 	iwlagn_gain_computation(priv, average_noise,
-				find_first_chain(hw_params(priv).valid_rx_ant));
+				find_first_chain(priv->hw_params.valid_rx_ant));
 
 	/* Some power changes may have been made during the calibration.
 	 * Update and commit the RXON
diff --git a/drivers/net/wireless/iwlwifi/iwl-agn-calib.h b/drivers/net/wireless/iwlwifi/iwl-agn-calib.h
index 9ed6683314a..dbe13787f27 100644
--- a/drivers/net/wireless/iwlwifi/iwl-agn-calib.h
+++ b/drivers/net/wireless/iwlwifi/iwl-agn-calib.h
@@ -63,7 +63,6 @@
 #define __iwl_calib_h__
 
 #include "iwl-dev.h"
-#include "iwl-core.h"
 #include "iwl-commands.h"
 
 void iwl_chain_noise_calibration(struct iwl_priv *priv);
diff --git a/drivers/net/wireless/iwlwifi/iwl-agn-devices.c b/drivers/net/wireless/iwlwifi/iwl-agn-devices.c
new file mode 100644
index 00000000000..48533b3a0f9
--- /dev/null
+++ b/drivers/net/wireless/iwlwifi/iwl-agn-devices.c
@@ -0,0 +1,755 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2008 - 2012 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.
+ *
+ * Contact Information:
+ *  Intel Linux Wireless <ilw@linux.intel.com>
+ * Intel Corporation, 5200 N.E. Elam Young Parkway, Hillsboro, OR 97124-6497
+ *
+ *****************************************************************************/
+
+/*
+ * DVM device-specific data & functions
+ */
+#include "iwl-agn.h"
+#include "iwl-dev.h"
+#include "iwl-commands.h"
+#include "iwl-io.h"
+#include "iwl-prph.h"
+
+/*
+ * 1000 series
+ * ===========
+ */
+
+/*
+ * For 1000, use advance thermal throttling critical temperature threshold,
+ * but legacy thermal management implementation for now.
+ * This is for the reason of 1000 uCode using advance thermal throttling API
+ * but not implement ct_kill_exit based on ct_kill exit temperature
+ * so the thermal throttling will still based on legacy thermal throttling
+ * management.
+ * The code here need to be modified once 1000 uCode has the advanced thermal
+ * throttling algorithm in place
+ */
+static void iwl1000_set_ct_threshold(struct iwl_priv *priv)
+{
+	/* want Celsius */
+	priv->hw_params.ct_kill_threshold = CT_KILL_THRESHOLD_LEGACY;
+	priv->hw_params.ct_kill_exit_threshold = CT_KILL_EXIT_THRESHOLD;
+}
+
+/* NIC configuration for 1000 series */
+static void iwl1000_nic_config(struct iwl_priv *priv)
+{
+	/* set CSR_HW_CONFIG_REG for uCode use */
+	iwl_set_bit(priv->trans, CSR_HW_IF_CONFIG_REG,
+		    CSR_HW_IF_CONFIG_REG_BIT_RADIO_SI |
+		    CSR_HW_IF_CONFIG_REG_BIT_MAC_SI);
+
+	/* Setting digital SVR for 1000 card to 1.32V */
+	/* locking is acquired in iwl_set_bits_mask_prph() function */
+	iwl_set_bits_mask_prph(priv->trans, APMG_DIGITAL_SVR_REG,
+				APMG_SVR_DIGITAL_VOLTAGE_1_32,
+				~APMG_SVR_VOLTAGE_CONFIG_BIT_MSK);
+}
+
+/**
+ * iwl_beacon_time_mask_low - mask of lower 32 bit of beacon time
+ * @priv -- pointer to iwl_priv data structure
+ * @tsf_bits -- number of bits need to shift for masking)
+ */
+static inline u32 iwl_beacon_time_mask_low(struct iwl_priv *priv,
+					   u16 tsf_bits)
+{
+	return (1 << tsf_bits) - 1;
+}
+
+/**
+ * iwl_beacon_time_mask_high - mask of higher 32 bit of beacon time
+ * @priv -- pointer to iwl_priv data structure
+ * @tsf_bits -- number of bits need to shift for masking)
+ */
+static inline u32 iwl_beacon_time_mask_high(struct iwl_priv *priv,
+					    u16 tsf_bits)
+{
+	return ((1 << (32 - tsf_bits)) - 1) << tsf_bits;
+}
+
+/*
+ * extended beacon time format
+ * time in usec will be changed into a 32-bit value in extended:internal format
+ * the extended part is the beacon counts
+ * the internal part is the time in usec within one beacon interval
+ */
+static u32 iwl_usecs_to_beacons(struct iwl_priv *priv, u32 usec,
+				u32 beacon_interval)
+{
+	u32 quot;
+	u32 rem;
+	u32 interval = beacon_interval * TIME_UNIT;
+
+	if (!interval || !usec)
+		return 0;
+
+	quot = (usec / interval) &
+		(iwl_beacon_time_mask_high(priv, IWLAGN_EXT_BEACON_TIME_POS) >>
+		IWLAGN_EXT_BEACON_TIME_POS);
+	rem = (usec % interval) & iwl_beacon_time_mask_low(priv,
+				   IWLAGN_EXT_BEACON_TIME_POS);
+
+	return (quot << IWLAGN_EXT_BEACON_TIME_POS) + rem;
+}
+
+/* base is usually what we get from ucode with each received frame,
+ * the same as HW timer counter counting down
+ */
+static __le32 iwl_add_beacon_time(struct iwl_priv *priv, u32 base,
+			   u32 addon, u32 beacon_interval)
+{
+	u32 base_low = base & iwl_beacon_time_mask_low(priv,
+				IWLAGN_EXT_BEACON_TIME_POS);
+	u32 addon_low = addon & iwl_beacon_time_mask_low(priv,
+				IWLAGN_EXT_BEACON_TIME_POS);
+	u32 interval = beacon_interval * TIME_UNIT;
+	u32 res = (base & iwl_beacon_time_mask_high(priv,
+				IWLAGN_EXT_BEACON_TIME_POS)) +
+				(addon & iwl_beacon_time_mask_high(priv,
+				IWLAGN_EXT_BEACON_TIME_POS));
+
+	if (base_low > addon_low)
+		res += base_low - addon_low;
+	else if (base_low < addon_low) {
+		res += interval + base_low - addon_low;
+		res += (1 << IWLAGN_EXT_BEACON_TIME_POS);
+	} else
+		res += (1 << IWLAGN_EXT_BEACON_TIME_POS);
+
+	return cpu_to_le32(res);
+}
+
+static const struct iwl_sensitivity_ranges iwl1000_sensitivity = {
+	.min_nrg_cck = 95,
+	.auto_corr_min_ofdm = 90,
+	.auto_corr_min_ofdm_mrc = 170,
+	.auto_corr_min_ofdm_x1 = 120,
+	.auto_corr_min_ofdm_mrc_x1 = 240,
+
+	.auto_corr_max_ofdm = 120,
+	.auto_corr_max_ofdm_mrc = 210,
+	.auto_corr_max_ofdm_x1 = 155,
+	.auto_corr_max_ofdm_mrc_x1 = 290,
+
+	.auto_corr_min_cck = 125,
+	.auto_corr_max_cck = 200,
+	.auto_corr_min_cck_mrc = 170,
+	.auto_corr_max_cck_mrc = 400,
+	.nrg_th_cck = 95,
+	.nrg_th_ofdm = 95,
+
+	.barker_corr_th_min = 190,
+	.barker_corr_th_min_mrc = 390,
+	.nrg_th_cca = 62,
+};
+
+static void iwl1000_hw_set_hw_params(struct iwl_priv *priv)
+{
+	priv->hw_params.ht40_channel =  BIT(IEEE80211_BAND_2GHZ);
+
+	priv->hw_params.tx_chains_num =
+		num_of_ant(priv->hw_params.valid_tx_ant);
+	if (priv->cfg->rx_with_siso_diversity)
+		priv->hw_params.rx_chains_num = 1;
+	else
+		priv->hw_params.rx_chains_num =
+			num_of_ant(priv->hw_params.valid_rx_ant);
+
+	iwl1000_set_ct_threshold(priv);
+
+	/* Set initial sensitivity parameters */
+	priv->hw_params.sens = &iwl1000_sensitivity;
+}
+
+struct iwl_lib_ops iwl1000_lib = {
+	.set_hw_params = iwl1000_hw_set_hw_params,
+	.nic_config = iwl1000_nic_config,
+	.eeprom_ops = {
+		.regulatory_bands = {
+			EEPROM_REG_BAND_1_CHANNELS,
+			EEPROM_REG_BAND_2_CHANNELS,
+			EEPROM_REG_BAND_3_CHANNELS,
+			EEPROM_REG_BAND_4_CHANNELS,
+			EEPROM_REG_BAND_5_CHANNELS,
+			EEPROM_REG_BAND_24_HT40_CHANNELS,
+			EEPROM_REGULATORY_BAND_NO_HT40,
+		},
+	},
+	.temperature = iwlagn_temperature,
+};
+
+
+/*
+ * 2000 series
+ * ===========
+ */
+
+static void iwl2000_set_ct_threshold(struct iwl_priv *priv)
+{
+	/* want Celsius */
+	priv->hw_params.ct_kill_threshold = CT_KILL_THRESHOLD;
+	priv->hw_params.ct_kill_exit_threshold = CT_KILL_EXIT_THRESHOLD;
+}
+
+/* NIC configuration for 2000 series */
+static void iwl2000_nic_config(struct iwl_priv *priv)
+{
+	iwl_rf_config(priv);
+
+	iwl_set_bit(priv->trans, CSR_GP_DRIVER_REG,
+		    CSR_GP_DRIVER_REG_BIT_RADIO_IQ_INVER);
+}
+
+static const struct iwl_sensitivity_ranges iwl2000_sensitivity = {
+	.min_nrg_cck = 97,
+	.auto_corr_min_ofdm = 80,
+	.auto_corr_min_ofdm_mrc = 128,
+	.auto_corr_min_ofdm_x1 = 105,
+	.auto_corr_min_ofdm_mrc_x1 = 192,
+
+	.auto_corr_max_ofdm = 145,
+	.auto_corr_max_ofdm_mrc = 232,
+	.auto_corr_max_ofdm_x1 = 110,
+	.auto_corr_max_ofdm_mrc_x1 = 232,
+
+	.auto_corr_min_cck = 125,
+	.auto_corr_max_cck = 175,
+	.auto_corr_min_cck_mrc = 160,
+	.auto_corr_max_cck_mrc = 310,
+	.nrg_th_cck = 97,
+	.nrg_th_ofdm = 100,
+
+	.barker_corr_th_min = 190,
+	.barker_corr_th_min_mrc = 390,
+	.nrg_th_cca = 62,
+};
+
+static void iwl2000_hw_set_hw_params(struct iwl_priv *priv)
+{
+	priv->hw_params.ht40_channel =  BIT(IEEE80211_BAND_2GHZ);
+
+	priv->hw_params.tx_chains_num =
+		num_of_ant(priv->hw_params.valid_tx_ant);
+	if (priv->cfg->rx_with_siso_diversity)
+		priv->hw_params.rx_chains_num = 1;
+	else
+		priv->hw_params.rx_chains_num =
+			num_of_ant(priv->hw_params.valid_rx_ant);
+
+	iwl2000_set_ct_threshold(priv);
+
+	/* Set initial sensitivity parameters */
+	priv->hw_params.sens = &iwl2000_sensitivity;
+}
+
+struct iwl_lib_ops iwl2000_lib = {
+	.set_hw_params = iwl2000_hw_set_hw_params,
+	.nic_config = iwl2000_nic_config,
+	.eeprom_ops = {
+		.regulatory_bands = {
+			EEPROM_REG_BAND_1_CHANNELS,
+			EEPROM_REG_BAND_2_CHANNELS,
+			EEPROM_REG_BAND_3_CHANNELS,
+			EEPROM_REG_BAND_4_CHANNELS,
+			EEPROM_REG_BAND_5_CHANNELS,
+			EEPROM_6000_REG_BAND_24_HT40_CHANNELS,
+			EEPROM_REGULATORY_BAND_NO_HT40,
+		},
+		.enhanced_txpower = true,
+	},
+	.temperature = iwlagn_temperature,
+};
+
+struct iwl_lib_ops iwl2030_lib = {
+	.set_hw_params = iwl2000_hw_set_hw_params,
+	.nic_config = iwl2000_nic_config,
+	.eeprom_ops = {
+		.regulatory_bands = {
+			EEPROM_REG_BAND_1_CHANNELS,
+			EEPROM_REG_BAND_2_CHANNELS,
+			EEPROM_REG_BAND_3_CHANNELS,
+			EEPROM_REG_BAND_4_CHANNELS,
+			EEPROM_REG_BAND_5_CHANNELS,
+			EEPROM_6000_REG_BAND_24_HT40_CHANNELS,
+			EEPROM_REGULATORY_BAND_NO_HT40,
+		},
+		.enhanced_txpower = true,
+	},
+	.temperature = iwlagn_temperature,
+};
+
+/*
+ * 5000 series
+ * ===========
+ */
+
+/* NIC configuration for 5000 series */
+static void iwl5000_nic_config(struct iwl_priv *priv)
+{
+	iwl_rf_config(priv);
+
+	/* W/A : NIC is stuck in a reset state after Early PCIe power off
+	 * (PCIe power is lost before PERST# is asserted),
+	 * causing ME FW to lose ownership and not being able to obtain it back.
+	 */
+	iwl_set_bits_mask_prph(priv->trans, APMG_PS_CTRL_REG,
+				APMG_PS_CTRL_EARLY_PWR_OFF_RESET_DIS,
+				~APMG_PS_CTRL_EARLY_PWR_OFF_RESET_DIS);
+}
+
+static const struct iwl_sensitivity_ranges iwl5000_sensitivity = {
+	.min_nrg_cck = 100,
+	.auto_corr_min_ofdm = 90,
+	.auto_corr_min_ofdm_mrc = 170,
+	.auto_corr_min_ofdm_x1 = 105,
+	.auto_corr_min_ofdm_mrc_x1 = 220,
+
+	.auto_corr_max_ofdm = 120,
+	.auto_corr_max_ofdm_mrc = 210,
+	.auto_corr_max_ofdm_x1 = 120,
+	.auto_corr_max_ofdm_mrc_x1 = 240,
+
+	.auto_corr_min_cck = 125,
+	.auto_corr_max_cck = 200,
+	.auto_corr_min_cck_mrc = 200,
+	.auto_corr_max_cck_mrc = 400,
+	.nrg_th_cck = 100,
+	.nrg_th_ofdm = 100,
+
+	.barker_corr_th_min = 190,
+	.barker_corr_th_min_mrc = 390,
+	.nrg_th_cca = 62,
+};
+
+static struct iwl_sensitivity_ranges iwl5150_sensitivity = {
+	.min_nrg_cck = 95,
+	.auto_corr_min_ofdm = 90,
+	.auto_corr_min_ofdm_mrc = 170,
+	.auto_corr_min_ofdm_x1 = 105,
+	.auto_corr_min_ofdm_mrc_x1 = 220,
+
+	.auto_corr_max_ofdm = 120,
+	.auto_corr_max_ofdm_mrc = 210,
+	/* max = min for performance bug in 5150 DSP */
+	.auto_corr_max_ofdm_x1 = 105,
+	.auto_corr_max_ofdm_mrc_x1 = 220,
+
+	.auto_corr_min_cck = 125,
+	.auto_corr_max_cck = 200,
+	.auto_corr_min_cck_mrc = 170,
+	.auto_corr_max_cck_mrc = 400,
+	.nrg_th_cck = 95,
+	.nrg_th_ofdm = 95,
+
+	.barker_corr_th_min = 190,
+	.barker_corr_th_min_mrc = 390,
+	.nrg_th_cca = 62,
+};
+
+#define IWL_5150_VOLTAGE_TO_TEMPERATURE_COEFF	(-5)
+
+static s32 iwl_temp_calib_to_offset(struct iwl_priv *priv)
+{
+	u16 temperature, voltage;
+	__le16 *temp_calib = (__le16 *)iwl_eeprom_query_addr(priv,
+				EEPROM_KELVIN_TEMPERATURE);
+
+	temperature = le16_to_cpu(temp_calib[0]);
+	voltage = le16_to_cpu(temp_calib[1]);
+
+	/* offset = temp - volt / coeff */
+	return (s32)(temperature -
+			voltage / IWL_5150_VOLTAGE_TO_TEMPERATURE_COEFF);
+}
+
+static void iwl5150_set_ct_threshold(struct iwl_priv *priv)
+{
+	const s32 volt2temp_coef = IWL_5150_VOLTAGE_TO_TEMPERATURE_COEFF;
+	s32 threshold = (s32)CELSIUS_TO_KELVIN(CT_KILL_THRESHOLD_LEGACY) -
+			iwl_temp_calib_to_offset(priv);
+
+	priv->hw_params.ct_kill_threshold = threshold * volt2temp_coef;
+}
+
+static void iwl5000_set_ct_threshold(struct iwl_priv *priv)
+{
+	/* want Celsius */
+	priv->hw_params.ct_kill_threshold = CT_KILL_THRESHOLD_LEGACY;
+}
+
+static void iwl5000_hw_set_hw_params(struct iwl_priv *priv)
+{
+	priv->hw_params.ht40_channel =  BIT(IEEE80211_BAND_2GHZ) |
+					BIT(IEEE80211_BAND_5GHZ);
+
+	priv->hw_params.tx_chains_num =
+		num_of_ant(priv->hw_params.valid_tx_ant);
+	priv->hw_params.rx_chains_num =
+		num_of_ant(priv->hw_params.valid_rx_ant);
+
+	iwl5000_set_ct_threshold(priv);
+
+	/* Set initial sensitivity parameters */
+	priv->hw_params.sens = &iwl5000_sensitivity;
+}
+
+static void iwl5150_hw_set_hw_params(struct iwl_priv *priv)
+{
+	priv->hw_params.ht40_channel =  BIT(IEEE80211_BAND_2GHZ) |
+					BIT(IEEE80211_BAND_5GHZ);
+
+	priv->hw_params.tx_chains_num =
+		num_of_ant(priv->hw_params.valid_tx_ant);
+	priv->hw_params.rx_chains_num =
+		num_of_ant(priv->hw_params.valid_rx_ant);
+
+	iwl5150_set_ct_threshold(priv);
+
+	/* Set initial sensitivity parameters */
+	priv->hw_params.sens = &iwl5150_sensitivity;
+}
+
+static void iwl5150_temperature(struct iwl_priv *priv)
+{
+	u32 vt = 0;
+	s32 offset =  iwl_temp_calib_to_offset(priv);
+
+	vt = le32_to_cpu(priv->statistics.common.temperature);
+	vt = vt / IWL_5150_VOLTAGE_TO_TEMPERATURE_COEFF + offset;
+	/* now vt hold the temperature in Kelvin */
+	priv->temperature = KELVIN_TO_CELSIUS(vt);
+	iwl_tt_handler(priv);
+}
+
+static int iwl5000_hw_channel_switch(struct iwl_priv *priv,
+				     struct ieee80211_channel_switch *ch_switch)
+{
+	/*
+	 * MULTI-FIXME
+	 * See iwlagn_mac_channel_switch.
+	 */
+	struct iwl_rxon_context *ctx = &priv->contexts[IWL_RXON_CTX_BSS];
+	struct iwl5000_channel_switch_cmd cmd;
+	const struct iwl_channel_info *ch_info;
+	u32 switch_time_in_usec, ucode_switch_time;
+	u16 ch;
+	u32 tsf_low;
+	u8 switch_count;
+	u16 beacon_interval = le16_to_cpu(ctx->timing.beacon_interval);
+	struct ieee80211_vif *vif = ctx->vif;
+	struct iwl_host_cmd hcmd = {
+		.id = REPLY_CHANNEL_SWITCH,
+		.len = { sizeof(cmd), },
+		.flags = CMD_SYNC,
+		.data = { &cmd, },
+	};
+
+	cmd.band = priv->band == IEEE80211_BAND_2GHZ;
+	ch = ch_switch->channel->hw_value;
+	IWL_DEBUG_11H(priv, "channel switch from %d to %d\n",
+		      ctx->active.channel, ch);
+	cmd.channel = cpu_to_le16(ch);
+	cmd.rxon_flags = ctx->staging.flags;
+	cmd.rxon_filter_flags = ctx->staging.filter_flags;
+	switch_count = ch_switch->count;
+	tsf_low = ch_switch->timestamp & 0x0ffffffff;
+	/*
+	 * calculate the ucode channel switch time
+	 * adding TSF as one of the factor for when to switch
+	 */
+	if ((priv->ucode_beacon_time > tsf_low) && beacon_interval) {
+		if (switch_count > ((priv->ucode_beacon_time - tsf_low) /
+		    beacon_interval)) {
+			switch_count -= (priv->ucode_beacon_time -
+				tsf_low) / beacon_interval;
+		} else
+			switch_count = 0;
+	}
+	if (switch_count <= 1)
+		cmd.switch_time = cpu_to_le32(priv->ucode_beacon_time);
+	else {
+		switch_time_in_usec =
+			vif->bss_conf.beacon_int * switch_count * TIME_UNIT;
+		ucode_switch_time = iwl_usecs_to_beacons(priv,
+							 switch_time_in_usec,
+							 beacon_interval);
+		cmd.switch_time = iwl_add_beacon_time(priv,
+						      priv->ucode_beacon_time,
+						      ucode_switch_time,
+						      beacon_interval);
+	}
+	IWL_DEBUG_11H(priv, "uCode time for the switch is 0x%x\n",
+		      cmd.switch_time);
+	ch_info = iwl_get_channel_info(priv, priv->band, ch);
+	if (ch_info)
+		cmd.expect_beacon = is_channel_radar(ch_info);
+	else {
+		IWL_ERR(priv, "invalid channel switch from %u to %u\n",
+			ctx->active.channel, ch);
+		return -EFAULT;
+	}
+
+	return iwl_dvm_send_cmd(priv, &hcmd);
+}
+
+struct iwl_lib_ops iwl5000_lib = {
+	.set_hw_params = iwl5000_hw_set_hw_params,
+	.set_channel_switch = iwl5000_hw_channel_switch,
+	.nic_config = iwl5000_nic_config,
+	.eeprom_ops = {
+		.regulatory_bands = {
+			EEPROM_REG_BAND_1_CHANNELS,
+			EEPROM_REG_BAND_2_CHANNELS,
+			EEPROM_REG_BAND_3_CHANNELS,
+			EEPROM_REG_BAND_4_CHANNELS,
+			EEPROM_REG_BAND_5_CHANNELS,
+			EEPROM_REG_BAND_24_HT40_CHANNELS,
+			EEPROM_REG_BAND_52_HT40_CHANNELS
+		},
+	},
+	.temperature = iwlagn_temperature,
+};
+
+struct iwl_lib_ops iwl5150_lib = {
+	.set_hw_params = iwl5150_hw_set_hw_params,
+	.set_channel_switch = iwl5000_hw_channel_switch,
+	.nic_config = iwl5000_nic_config,
+	.eeprom_ops = {
+		.regulatory_bands = {
+			EEPROM_REG_BAND_1_CHANNELS,
+			EEPROM_REG_BAND_2_CHANNELS,
+			EEPROM_REG_BAND_3_CHANNELS,
+			EEPROM_REG_BAND_4_CHANNELS,
+			EEPROM_REG_BAND_5_CHANNELS,
+			EEPROM_REG_BAND_24_HT40_CHANNELS,
+			EEPROM_REG_BAND_52_HT40_CHANNELS
+		},
+	},
+	.temperature = iwl5150_temperature,
+};
+
+
+
+/*
+ * 6000 series
+ * ===========
+ */
+
+static void iwl6000_set_ct_threshold(struct iwl_priv *priv)
+{
+	/* want Celsius */
+	priv->hw_params.ct_kill_threshold = CT_KILL_THRESHOLD;
+	priv->hw_params.ct_kill_exit_threshold = CT_KILL_EXIT_THRESHOLD;
+}
+
+/* NIC configuration for 6000 series */
+static void iwl6000_nic_config(struct iwl_priv *priv)
+{
+	iwl_rf_config(priv);
+
+	switch (priv->cfg->device_family) {
+	case IWL_DEVICE_FAMILY_6005:
+	case IWL_DEVICE_FAMILY_6030:
+	case IWL_DEVICE_FAMILY_6000:
+		break;
+	case IWL_DEVICE_FAMILY_6000i:
+		/* 2x2 IPA phy type */
+		iwl_write32(priv->trans, CSR_GP_DRIVER_REG,
+			     CSR_GP_DRIVER_REG_BIT_RADIO_SKU_2x2_IPA);
+		break;
+	case IWL_DEVICE_FAMILY_6050:
+		/* Indicate calibration version to uCode. */
+		if (iwl_eeprom_calib_version(priv) >= 6)
+			iwl_set_bit(priv->trans, CSR_GP_DRIVER_REG,
+					CSR_GP_DRIVER_REG_BIT_CALIB_VERSION6);
+		break;
+	case IWL_DEVICE_FAMILY_6150:
+		/* Indicate calibration version to uCode. */
+		if (iwl_eeprom_calib_version(priv) >= 6)
+			iwl_set_bit(priv->trans, CSR_GP_DRIVER_REG,
+					CSR_GP_DRIVER_REG_BIT_CALIB_VERSION6);
+		iwl_set_bit(priv->trans, CSR_GP_DRIVER_REG,
+			    CSR_GP_DRIVER_REG_BIT_6050_1x2);
+		break;
+	default:
+		WARN_ON(1);
+	}
+}
+
+static const struct iwl_sensitivity_ranges iwl6000_sensitivity = {
+	.min_nrg_cck = 110,
+	.auto_corr_min_ofdm = 80,
+	.auto_corr_min_ofdm_mrc = 128,
+	.auto_corr_min_ofdm_x1 = 105,
+	.auto_corr_min_ofdm_mrc_x1 = 192,
+
+	.auto_corr_max_ofdm = 145,
+	.auto_corr_max_ofdm_mrc = 232,
+	.auto_corr_max_ofdm_x1 = 110,
+	.auto_corr_max_ofdm_mrc_x1 = 232,
+
+	.auto_corr_min_cck = 125,
+	.auto_corr_max_cck = 175,
+	.auto_corr_min_cck_mrc = 160,
+	.auto_corr_max_cck_mrc = 310,
+	.nrg_th_cck = 110,
+	.nrg_th_ofdm = 110,
+
+	.barker_corr_th_min = 190,
+	.barker_corr_th_min_mrc = 336,
+	.nrg_th_cca = 62,
+};
+
+static void iwl6000_hw_set_hw_params(struct iwl_priv *priv)
+{
+	priv->hw_params.ht40_channel =  BIT(IEEE80211_BAND_2GHZ) |
+					BIT(IEEE80211_BAND_5GHZ);
+
+	priv->hw_params.tx_chains_num =
+		num_of_ant(priv->hw_params.valid_tx_ant);
+	if (priv->cfg->rx_with_siso_diversity)
+		priv->hw_params.rx_chains_num = 1;
+	else
+		priv->hw_params.rx_chains_num =
+			num_of_ant(priv->hw_params.valid_rx_ant);
+
+	iwl6000_set_ct_threshold(priv);
+
+	/* Set initial sensitivity parameters */
+	priv->hw_params.sens = &iwl6000_sensitivity;
+
+}
+
+static int iwl6000_hw_channel_switch(struct iwl_priv *priv,
+				     struct ieee80211_channel_switch *ch_switch)
+{
+	/*
+	 * MULTI-FIXME
+	 * See iwlagn_mac_channel_switch.
+	 */
+	struct iwl_rxon_context *ctx = &priv->contexts[IWL_RXON_CTX_BSS];
+	struct iwl6000_channel_switch_cmd cmd;
+	const struct iwl_channel_info *ch_info;
+	u32 switch_time_in_usec, ucode_switch_time;
+	u16 ch;
+	u32 tsf_low;
+	u8 switch_count;
+	u16 beacon_interval = le16_to_cpu(ctx->timing.beacon_interval);
+	struct ieee80211_vif *vif = ctx->vif;
+	struct iwl_host_cmd hcmd = {
+		.id = REPLY_CHANNEL_SWITCH,
+		.len = { sizeof(cmd), },
+		.flags = CMD_SYNC,
+		.data = { &cmd, },
+	};
+
+	cmd.band = priv->band == IEEE80211_BAND_2GHZ;
+	ch = ch_switch->channel->hw_value;
+	IWL_DEBUG_11H(priv, "channel switch from %u to %u\n",
+		      ctx->active.channel, ch);
+	cmd.channel = cpu_to_le16(ch);
+	cmd.rxon_flags = ctx->staging.flags;
+	cmd.rxon_filter_flags = ctx->staging.filter_flags;
+	switch_count = ch_switch->count;
+	tsf_low = ch_switch->timestamp & 0x0ffffffff;
+	/*
+	 * calculate the ucode channel switch time
+	 * adding TSF as one of the factor for when to switch
+	 */
+	if ((priv->ucode_beacon_time > tsf_low) && beacon_interval) {
+		if (switch_count > ((priv->ucode_beacon_time - tsf_low) /
+		    beacon_interval)) {
+			switch_count -= (priv->ucode_beacon_time -
+				tsf_low) / beacon_interval;
+		} else
+			switch_count = 0;
+	}
+	if (switch_count <= 1)
+		cmd.switch_time = cpu_to_le32(priv->ucode_beacon_time);
+	else {
+		switch_time_in_usec =
+			vif->bss_conf.beacon_int * switch_count * TIME_UNIT;
+		ucode_switch_time = iwl_usecs_to_beacons(priv,
+							 switch_time_in_usec,
+							 beacon_interval);
+		cmd.switch_time = iwl_add_beacon_time(priv,
+						      priv->ucode_beacon_time,
+						      ucode_switch_time,
+						      beacon_interval);
+	}
+	IWL_DEBUG_11H(priv, "uCode time for the switch is 0x%x\n",
+		      cmd.switch_time);
+	ch_info = iwl_get_channel_info(priv, priv->band, ch);
+	if (ch_info)
+		cmd.expect_beacon = is_channel_radar(ch_info);
+	else {
+		IWL_ERR(priv, "invalid channel switch from %u to %u\n",
+			ctx->active.channel, ch);
+		return -EFAULT;
+	}
+
+	return iwl_dvm_send_cmd(priv, &hcmd);
+}
+
+struct iwl_lib_ops iwl6000_lib = {
+	.set_hw_params = iwl6000_hw_set_hw_params,
+	.set_channel_switch = iwl6000_hw_channel_switch,
+	.nic_config = iwl6000_nic_config,
+	.eeprom_ops = {
+		.regulatory_bands = {
+			EEPROM_REG_BAND_1_CHANNELS,
+			EEPROM_REG_BAND_2_CHANNELS,
+			EEPROM_REG_BAND_3_CHANNELS,
+			EEPROM_REG_BAND_4_CHANNELS,
+			EEPROM_REG_BAND_5_CHANNELS,
+			EEPROM_6000_REG_BAND_24_HT40_CHANNELS,
+			EEPROM_REG_BAND_52_HT40_CHANNELS
+		},
+		.enhanced_txpower = true,
+	},
+	.temperature = iwlagn_temperature,
+};
+
+struct iwl_lib_ops iwl6030_lib = {
+	.set_hw_params = iwl6000_hw_set_hw_params,
+	.set_channel_switch = iwl6000_hw_channel_switch,
+	.nic_config = iwl6000_nic_config,
+	.eeprom_ops = {
+		.regulatory_bands = {
+			EEPROM_REG_BAND_1_CHANNELS,
+			EEPROM_REG_BAND_2_CHANNELS,
+			EEPROM_REG_BAND_3_CHANNELS,
+			EEPROM_REG_BAND_4_CHANNELS,
+			EEPROM_REG_BAND_5_CHANNELS,
+			EEPROM_6000_REG_BAND_24_HT40_CHANNELS,
+			EEPROM_REG_BAND_52_HT40_CHANNELS
+		},
+		.enhanced_txpower = true,
+	},
+	.temperature = iwlagn_temperature,
+};
diff --git a/drivers/net/wireless/iwlwifi/iwl-agn-hw.h b/drivers/net/wireless/iwlwifi/iwl-agn-hw.h
index d0ec0abd3c8..7960a52f6ad 100644
--- a/drivers/net/wireless/iwlwifi/iwl-agn-hw.h
+++ b/drivers/net/wireless/iwlwifi/iwl-agn-hw.h
@@ -102,10 +102,18 @@
 
 /* EEPROM */
 #define IWLAGN_EEPROM_IMG_SIZE		2048
+/* OTP */
+/* lower blocks contain EEPROM image and calibration data */
+#define OTP_LOW_IMAGE_SIZE		(2 * 512 * sizeof(u16)) /* 2 KB */
+/* high blocks contain PAPD data */
+#define OTP_HIGH_IMAGE_SIZE_6x00        (6 * 512 * sizeof(u16)) /* 6 KB */
+#define OTP_HIGH_IMAGE_SIZE_1000        (0x200 * sizeof(u16)) /* 1024 bytes */
+#define OTP_MAX_LL_ITEMS_1000		(3)	/* OTP blocks for 1000 */
+#define OTP_MAX_LL_ITEMS_6x00		(4)	/* OTP blocks for 6x00 */
+#define OTP_MAX_LL_ITEMS_6x50		(7)	/* OTP blocks for 6x50 */
+#define OTP_MAX_LL_ITEMS_2x00		(4)	/* OTP blocks for 2x00 */
+
 
-#define IWLAGN_CMD_FIFO_NUM		7
 #define IWLAGN_NUM_QUEUES		20
-#define IWLAGN_NUM_AMPDU_QUEUES		9
-#define IWLAGN_FIRST_AMPDU_QUEUE	11
 
 #endif /* __iwl_agn_hw_h__ */
diff --git a/drivers/net/wireless/iwlwifi/iwl-agn-lib.c b/drivers/net/wireless/iwlwifi/iwl-agn-lib.c
index 56f41c9409d..e55ec6c8a92 100644
--- a/drivers/net/wireless/iwlwifi/iwl-agn-lib.c
+++ b/drivers/net/wireless/iwlwifi/iwl-agn-lib.c
@@ -31,14 +31,14 @@
 #include <linux/module.h>
 #include <linux/init.h>
 #include <linux/sched.h>
+#include <net/mac80211.h>
 
 #include "iwl-dev.h"
-#include "iwl-core.h"
 #include "iwl-io.h"
 #include "iwl-agn-hw.h"
 #include "iwl-agn.h"
 #include "iwl-trans.h"
-#include "iwl-shared.h"
+#include "iwl-modparams.h"
 
 int iwlagn_hw_valid_rtc_data_addr(u32 addr)
 {
@@ -94,81 +94,6 @@ void iwlagn_temperature(struct iwl_priv *priv)
 	iwl_tt_handler(priv);
 }
 
-u16 iwl_eeprom_calib_version(struct iwl_shared *shrd)
-{
-	struct iwl_eeprom_calib_hdr *hdr;
-
-	hdr = (struct iwl_eeprom_calib_hdr *)iwl_eeprom_query_addr(shrd,
-							EEPROM_CALIB_ALL);
-	return hdr->version;
-
-}
-
-/*
- * EEPROM
- */
-static u32 eeprom_indirect_address(const struct iwl_shared *shrd, u32 address)
-{
-	u16 offset = 0;
-
-	if ((address & INDIRECT_ADDRESS) == 0)
-		return address;
-
-	switch (address & INDIRECT_TYPE_MSK) {
-	case INDIRECT_HOST:
-		offset = iwl_eeprom_query16(shrd, EEPROM_LINK_HOST);
-		break;
-	case INDIRECT_GENERAL:
-		offset = iwl_eeprom_query16(shrd, EEPROM_LINK_GENERAL);
-		break;
-	case INDIRECT_REGULATORY:
-		offset = iwl_eeprom_query16(shrd, EEPROM_LINK_REGULATORY);
-		break;
-	case INDIRECT_TXP_LIMIT:
-		offset = iwl_eeprom_query16(shrd, EEPROM_LINK_TXP_LIMIT);
-		break;
-	case INDIRECT_TXP_LIMIT_SIZE:
-		offset = iwl_eeprom_query16(shrd, EEPROM_LINK_TXP_LIMIT_SIZE);
-		break;
-	case INDIRECT_CALIBRATION:
-		offset = iwl_eeprom_query16(shrd, EEPROM_LINK_CALIBRATION);
-		break;
-	case INDIRECT_PROCESS_ADJST:
-		offset = iwl_eeprom_query16(shrd, EEPROM_LINK_PROCESS_ADJST);
-		break;
-	case INDIRECT_OTHERS:
-		offset = iwl_eeprom_query16(shrd, EEPROM_LINK_OTHERS);
-		break;
-	default:
-		IWL_ERR(shrd->trans, "illegal indirect type: 0x%X\n",
-		address & INDIRECT_TYPE_MSK);
-		break;
-	}
-
-	/* translate the offset from words to byte */
-	return (address & ADDRESS_MSK) + (offset << 1);
-}
-
-const u8 *iwl_eeprom_query_addr(const struct iwl_shared *shrd, size_t offset)
-{
-	u32 address = eeprom_indirect_address(shrd, offset);
-	BUG_ON(address >= shrd->cfg->base_params->eeprom_size);
-	return &shrd->eeprom[address];
-}
-
-struct iwl_mod_params iwlagn_mod_params = {
-	.amsdu_size_8K = 1,
-	.restart_fw = 1,
-	.plcp_check = true,
-	.bt_coex_active = true,
-	.no_sleep_autoadjust = true,
-	.power_level = IWL_POWER_INDEX_1,
-	.bt_ch_announce = true,
-	.wanted_ucode_alternative = 1,
-	.auto_agg = true,
-	/* the rest are 0 by default */
-};
-
 int iwlagn_hwrate_to_mac80211_idx(u32 rate_n_flags, enum ieee80211_band band)
 {
 	int idx = 0;
@@ -228,13 +153,13 @@ int iwlagn_txfifo_flush(struct iwl_priv *priv, u16 flush_control)
 				 IWL_SCD_BE_MSK | IWL_SCD_BK_MSK |
 				 IWL_SCD_MGMT_MSK;
 	if ((flush_control & BIT(IWL_RXON_CTX_PAN)) &&
-	    (priv->shrd->valid_contexts != BIT(IWL_RXON_CTX_BSS)))
+	    (priv->valid_contexts != BIT(IWL_RXON_CTX_BSS)))
 		flush_cmd.fifo_control |= IWL_PAN_SCD_VO_MSK |
 				IWL_PAN_SCD_VI_MSK | IWL_PAN_SCD_BE_MSK |
 				IWL_PAN_SCD_BK_MSK | IWL_PAN_SCD_MGMT_MSK |
 				IWL_PAN_SCD_MULTICAST_MSK;
 
-	if (hw_params(priv).sku & EEPROM_SKU_CAP_11N_ENABLE)
+	if (priv->hw_params.sku & EEPROM_SKU_CAP_11N_ENABLE)
 		flush_cmd.fifo_control |= IWL_AGG_TX_QUEUE_MSK;
 
 	IWL_DEBUG_INFO(priv, "fifo queue control: 0X%x\n",
@@ -253,7 +178,7 @@ void iwlagn_dev_txfifo_flush(struct iwl_priv *priv, u16 flush_control)
 		goto done;
 	}
 	IWL_DEBUG_INFO(priv, "wait transmit/flush all frames\n");
-	iwl_trans_wait_tx_queue_empty(trans(priv));
+	iwl_trans_wait_tx_queue_empty(priv->trans);
 done:
 	ieee80211_wake_queues(priv->hw);
 	mutex_unlock(&priv->mutex);
@@ -262,76 +187,8 @@ done:
 /*
  * BT coex
  */
-/*
- * Macros to access the lookup table.
- *
- * The lookup table has 7 inputs: bt3_prio, bt3_txrx, bt_rf_act, wifi_req,
-* wifi_prio, wifi_txrx and wifi_sh_ant_req.
- *
- * It has three outputs: WLAN_ACTIVE, WLAN_KILL and ANT_SWITCH
- *
- * The format is that "registers" 8 through 11 contain the WLAN_ACTIVE bits
- * one after another in 32-bit registers, and "registers" 0 through 7 contain
- * the WLAN_KILL and ANT_SWITCH bits interleaved (in that order).
- *
- * These macros encode that format.
- */
-#define LUT_VALUE(bt3_prio, bt3_txrx, bt_rf_act, wifi_req, wifi_prio, \
-		  wifi_txrx, wifi_sh_ant_req) \
-	(bt3_prio | (bt3_txrx << 1) | (bt_rf_act << 2) | (wifi_req << 3) | \
-	(wifi_prio << 4) | (wifi_txrx << 5) | (wifi_sh_ant_req << 6))
-
-#define LUT_PTA_WLAN_ACTIVE_OP(lut, op, val) \
-	lut[8 + ((val) >> 5)] op (cpu_to_le32(BIT((val) & 0x1f)))
-#define LUT_TEST_PTA_WLAN_ACTIVE(lut, bt3_prio, bt3_txrx, bt_rf_act, wifi_req, \
-				 wifi_prio, wifi_txrx, wifi_sh_ant_req) \
-	(!!(LUT_PTA_WLAN_ACTIVE_OP(lut, &, LUT_VALUE(bt3_prio, bt3_txrx, \
-				   bt_rf_act, wifi_req, wifi_prio, wifi_txrx, \
-				   wifi_sh_ant_req))))
-#define LUT_SET_PTA_WLAN_ACTIVE(lut, bt3_prio, bt3_txrx, bt_rf_act, wifi_req, \
-				wifi_prio, wifi_txrx, wifi_sh_ant_req) \
-	LUT_PTA_WLAN_ACTIVE_OP(lut, |=, LUT_VALUE(bt3_prio, bt3_txrx, \
-			       bt_rf_act, wifi_req, wifi_prio, wifi_txrx, \
-			       wifi_sh_ant_req))
-#define LUT_CLEAR_PTA_WLAN_ACTIVE(lut, bt3_prio, bt3_txrx, bt_rf_act, \
-				  wifi_req, wifi_prio, wifi_txrx, \
-				  wifi_sh_ant_req) \
-	LUT_PTA_WLAN_ACTIVE_OP(lut, &= ~, LUT_VALUE(bt3_prio, bt3_txrx, \
-			       bt_rf_act, wifi_req, wifi_prio, wifi_txrx, \
-			       wifi_sh_ant_req))
-
-#define LUT_WLAN_KILL_OP(lut, op, val) \
-	lut[(val) >> 4] op (cpu_to_le32(BIT(((val) << 1) & 0x1e)))
-#define LUT_TEST_WLAN_KILL(lut, bt3_prio, bt3_txrx, bt_rf_act, wifi_req, \
-			   wifi_prio, wifi_txrx, wifi_sh_ant_req) \
-	(!!(LUT_WLAN_KILL_OP(lut, &, LUT_VALUE(bt3_prio, bt3_txrx, bt_rf_act, \
-			     wifi_req, wifi_prio, wifi_txrx, wifi_sh_ant_req))))
-#define LUT_SET_WLAN_KILL(lut, bt3_prio, bt3_txrx, bt_rf_act, wifi_req, \
-			  wifi_prio, wifi_txrx, wifi_sh_ant_req) \
-	LUT_WLAN_KILL_OP(lut, |=, LUT_VALUE(bt3_prio, bt3_txrx, bt_rf_act, \
-			 wifi_req, wifi_prio, wifi_txrx, wifi_sh_ant_req))
-#define LUT_CLEAR_WLAN_KILL(lut, bt3_prio, bt3_txrx, bt_rf_act, wifi_req, \
-			    wifi_prio, wifi_txrx, wifi_sh_ant_req) \
-	LUT_WLAN_KILL_OP(lut, &= ~, LUT_VALUE(bt3_prio, bt3_txrx, bt_rf_act, \
-			 wifi_req, wifi_prio, wifi_txrx, wifi_sh_ant_req))
-
-#define LUT_ANT_SWITCH_OP(lut, op, val) \
-	lut[(val) >> 4] op (cpu_to_le32(BIT((((val) << 1) & 0x1e) + 1)))
-#define LUT_TEST_ANT_SWITCH(lut, bt3_prio, bt3_txrx, bt_rf_act, wifi_req, \
-			    wifi_prio, wifi_txrx, wifi_sh_ant_req) \
-	(!!(LUT_ANT_SWITCH_OP(lut, &, LUT_VALUE(bt3_prio, bt3_txrx, bt_rf_act, \
-			      wifi_req, wifi_prio, wifi_txrx, \
-			      wifi_sh_ant_req))))
-#define LUT_SET_ANT_SWITCH(lut, bt3_prio, bt3_txrx, bt_rf_act, wifi_req, \
-			   wifi_prio, wifi_txrx, wifi_sh_ant_req) \
-	LUT_ANT_SWITCH_OP(lut, |=, LUT_VALUE(bt3_prio, bt3_txrx, bt_rf_act, \
-			  wifi_req, wifi_prio, wifi_txrx, wifi_sh_ant_req))
-#define LUT_CLEAR_ANT_SWITCH(lut, bt3_prio, bt3_txrx, bt_rf_act, wifi_req, \
-			     wifi_prio, wifi_txrx, wifi_sh_ant_req) \
-	LUT_ANT_SWITCH_OP(lut, &= ~, LUT_VALUE(bt3_prio, bt3_txrx, bt_rf_act, \
-			  wifi_req, wifi_prio, wifi_txrx, wifi_sh_ant_req))
-
-static const __le32 iwlagn_def_3w_lookup[12] = {
+/* Notmal TDM */
+static const __le32 iwlagn_def_3w_lookup[IWLAGN_BT_DECISION_LUT_SIZE] = {
 	cpu_to_le32(0xaaaaaaaa),
 	cpu_to_le32(0xaaaaaaaa),
 	cpu_to_le32(0xaeaaaaaa),
@@ -346,7 +203,25 @@ static const __le32 iwlagn_def_3w_lookup[12] = {
 	cpu_to_le32(0xf0005000),
 };
 
-static const __le32 iwlagn_concurrent_lookup[12] = {
+
+/* Loose Coex */
+static const __le32 iwlagn_loose_lookup[IWLAGN_BT_DECISION_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 iwlagn_concurrent_lookup[IWLAGN_BT_DECISION_LUT_SIZE] = {
 	cpu_to_le32(0xaaaaaaaa),
 	cpu_to_le32(0xaaaaaaaa),
 	cpu_to_le32(0xaaaaaaaa),
@@ -369,32 +244,50 @@ void iwlagn_send_advance_bt_config(struct iwl_priv *priv)
 		.bt3_prio_sample_time = IWLAGN_BT3_PRIO_SAMPLE_DEFAULT,
 		.bt3_timer_t2_value = IWLAGN_BT3_T2_DEFAULT,
 	};
-	struct iwl6000_bt_cmd bt_cmd_6000;
-	struct iwl2000_bt_cmd bt_cmd_2000;
+	struct iwl_bt_cmd_v1 bt_cmd_v1;
+	struct iwl_bt_cmd_v2 bt_cmd_v2;
 	int ret;
 
 	BUILD_BUG_ON(sizeof(iwlagn_def_3w_lookup) !=
 			sizeof(basic.bt3_lookup_table));
 
-	if (cfg(priv)->bt_params) {
-		if (cfg(priv)->bt_params->bt_session_2) {
-			bt_cmd_2000.prio_boost = cpu_to_le32(
-				cfg(priv)->bt_params->bt_prio_boost);
-			bt_cmd_2000.tx_prio_boost = 0;
-			bt_cmd_2000.rx_prio_boost = 0;
+	if (priv->cfg->bt_params) {
+		/*
+		 * newer generation of devices (2000 series and newer)
+		 * use the version 2 of the bt command
+		 * we need to make sure sending the host command
+		 * with correct data structure to avoid uCode assert
+		 */
+		if (priv->cfg->bt_params->bt_session_2) {
+			bt_cmd_v2.prio_boost = cpu_to_le32(
+				priv->cfg->bt_params->bt_prio_boost);
+			bt_cmd_v2.tx_prio_boost = 0;
+			bt_cmd_v2.rx_prio_boost = 0;
 		} else {
-			bt_cmd_6000.prio_boost =
-				cfg(priv)->bt_params->bt_prio_boost;
-			bt_cmd_6000.tx_prio_boost = 0;
-			bt_cmd_6000.rx_prio_boost = 0;
+			bt_cmd_v1.prio_boost =
+				priv->cfg->bt_params->bt_prio_boost;
+			bt_cmd_v1.tx_prio_boost = 0;
+			bt_cmd_v1.rx_prio_boost = 0;
 		}
 	} else {
 		IWL_ERR(priv, "failed to construct BT Coex Config\n");
 		return;
 	}
 
+	/*
+	 * Possible situations when BT needs to take over for receive,
+	 * at the same time where STA needs to response to AP's frame(s),
+	 * reduce the tx power of the required response frames, by that,
+	 * allow the concurrent BT receive & WiFi transmit
+	 * (BT - ANT A, WiFi -ANT B), without interference to one another
+	 *
+	 * Reduced tx power apply to control frames only (ACK/Back/CTS)
+	 * when indicated by the BT config command
+	 */
 	basic.kill_ack_mask = priv->kill_ack_mask;
 	basic.kill_cts_mask = priv->kill_cts_mask;
+	if (priv->reduced_txpower)
+		basic.reduce_txpower = IWLAGN_BT_REDUCED_TX_PWR;
 	basic.valid = priv->bt_valid;
 
 	/*
@@ -403,7 +296,7 @@ void iwlagn_send_advance_bt_config(struct iwl_priv *priv)
 	 * (might be in monitor mode), or the interface is in
 	 * IBSS mode (no proper uCode support for coex then).
 	 */
-	if (!iwlagn_mod_params.bt_coex_active ||
+	if (!iwlwifi_mod_params.bt_coex_active ||
 	    priv->iw_mode == NL80211_IFTYPE_ADHOC) {
 		basic.flags = IWLAGN_BT_FLAG_COEX_MODE_DISABLED;
 	} else {
@@ -432,16 +325,16 @@ void iwlagn_send_advance_bt_config(struct iwl_priv *priv)
 		       priv->bt_full_concurrent ?
 		       "full concurrency" : "3-wire");
 
-	if (cfg(priv)->bt_params->bt_session_2) {
-		memcpy(&bt_cmd_2000.basic, &basic,
+	if (priv->cfg->bt_params->bt_session_2) {
+		memcpy(&bt_cmd_v2.basic, &basic,
 			sizeof(basic));
 		ret = iwl_dvm_send_cmd_pdu(priv, REPLY_BT_CONFIG,
-			CMD_SYNC, sizeof(bt_cmd_2000), &bt_cmd_2000);
+			CMD_SYNC, sizeof(bt_cmd_v2), &bt_cmd_v2);
 	} else {
-		memcpy(&bt_cmd_6000.basic, &basic,
+		memcpy(&bt_cmd_v1.basic, &basic,
 			sizeof(basic));
 		ret = iwl_dvm_send_cmd_pdu(priv, REPLY_BT_CONFIG,
-			CMD_SYNC, sizeof(bt_cmd_6000), &bt_cmd_6000);
+			CMD_SYNC, sizeof(bt_cmd_v1), &bt_cmd_v1);
 	}
 	if (ret)
 		IWL_ERR(priv, "failed to send BT Coex Config\n");
@@ -615,7 +508,7 @@ static void iwlagn_print_uartmsg(struct iwl_priv *priv,
 				struct iwl_bt_uart_msg *uart_msg)
 {
 	IWL_DEBUG_COEX(priv, "Message Type = 0x%X, SSN = 0x%X, "
-			"Update Req = 0x%X",
+			"Update Req = 0x%X\n",
 		(BT_UART_MSG_FRAME1MSGTYPE_MSK & uart_msg->frame1) >>
 			BT_UART_MSG_FRAME1MSGTYPE_POS,
 		(BT_UART_MSG_FRAME1SSN_MSK & uart_msg->frame1) >>
@@ -624,7 +517,7 @@ static void iwlagn_print_uartmsg(struct iwl_priv *priv,
 			BT_UART_MSG_FRAME1UPDATEREQ_POS);
 
 	IWL_DEBUG_COEX(priv, "Open connections = 0x%X, Traffic load = 0x%X, "
-			"Chl_SeqN = 0x%X, In band = 0x%X",
+			"Chl_SeqN = 0x%X, In band = 0x%X\n",
 		(BT_UART_MSG_FRAME2OPENCONNECTIONS_MSK & uart_msg->frame2) >>
 			BT_UART_MSG_FRAME2OPENCONNECTIONS_POS,
 		(BT_UART_MSG_FRAME2TRAFFICLOAD_MSK & uart_msg->frame2) >>
@@ -635,7 +528,7 @@ static void iwlagn_print_uartmsg(struct iwl_priv *priv,
 			BT_UART_MSG_FRAME2INBAND_POS);
 
 	IWL_DEBUG_COEX(priv, "SCO/eSCO = 0x%X, Sniff = 0x%X, A2DP = 0x%X, "
-			"ACL = 0x%X, Master = 0x%X, OBEX = 0x%X",
+			"ACL = 0x%X, Master = 0x%X, OBEX = 0x%X\n",
 		(BT_UART_MSG_FRAME3SCOESCO_MSK & uart_msg->frame3) >>
 			BT_UART_MSG_FRAME3SCOESCO_POS,
 		(BT_UART_MSG_FRAME3SNIFF_MSK & uart_msg->frame3) >>
@@ -649,12 +542,12 @@ static void iwlagn_print_uartmsg(struct iwl_priv *priv,
 		(BT_UART_MSG_FRAME3OBEX_MSK & uart_msg->frame3) >>
 			BT_UART_MSG_FRAME3OBEX_POS);
 
-	IWL_DEBUG_COEX(priv, "Idle duration = 0x%X",
+	IWL_DEBUG_COEX(priv, "Idle duration = 0x%X\n",
 		(BT_UART_MSG_FRAME4IDLEDURATION_MSK & uart_msg->frame4) >>
 			BT_UART_MSG_FRAME4IDLEDURATION_POS);
 
 	IWL_DEBUG_COEX(priv, "Tx Activity = 0x%X, Rx Activity = 0x%X, "
-			"eSCO Retransmissions = 0x%X",
+			"eSCO Retransmissions = 0x%X\n",
 		(BT_UART_MSG_FRAME5TXACTIVITY_MSK & uart_msg->frame5) >>
 			BT_UART_MSG_FRAME5TXACTIVITY_POS,
 		(BT_UART_MSG_FRAME5RXACTIVITY_MSK & uart_msg->frame5) >>
@@ -662,14 +555,14 @@ static void iwlagn_print_uartmsg(struct iwl_priv *priv,
 		(BT_UART_MSG_FRAME5ESCORETRANSMIT_MSK & uart_msg->frame5) >>
 			BT_UART_MSG_FRAME5ESCORETRANSMIT_POS);
 
-	IWL_DEBUG_COEX(priv, "Sniff Interval = 0x%X, Discoverable = 0x%X",
+	IWL_DEBUG_COEX(priv, "Sniff Interval = 0x%X, Discoverable = 0x%X\n",
 		(BT_UART_MSG_FRAME6SNIFFINTERVAL_MSK & uart_msg->frame6) >>
 			BT_UART_MSG_FRAME6SNIFFINTERVAL_POS,
 		(BT_UART_MSG_FRAME6DISCOVERABLE_MSK & uart_msg->frame6) >>
 			BT_UART_MSG_FRAME6DISCOVERABLE_POS);
 
 	IWL_DEBUG_COEX(priv, "Sniff Activity = 0x%X, Page = "
-			"0x%X, Inquiry = 0x%X, Connectable = 0x%X",
+			"0x%X, Inquiry = 0x%X, Connectable = 0x%X\n",
 		(BT_UART_MSG_FRAME7SNIFFACTIVITY_MSK & uart_msg->frame7) >>
 			BT_UART_MSG_FRAME7SNIFFACTIVITY_POS,
 		(BT_UART_MSG_FRAME7PAGE_MSK & uart_msg->frame7) >>
@@ -680,29 +573,81 @@ static void iwlagn_print_uartmsg(struct iwl_priv *priv,
 			BT_UART_MSG_FRAME7CONNECTABLE_POS);
 }
 
-static void iwlagn_set_kill_msk(struct iwl_priv *priv,
+static bool iwlagn_set_kill_msk(struct iwl_priv *priv,
 				struct iwl_bt_uart_msg *uart_msg)
 {
-	u8 kill_msk;
-	static const __le32 bt_kill_ack_msg[2] = {
+	bool need_update = false;
+	u8 kill_msk = IWL_BT_KILL_REDUCE;
+	static const __le32 bt_kill_ack_msg[3] = {
 		IWLAGN_BT_KILL_ACK_MASK_DEFAULT,
-		IWLAGN_BT_KILL_ACK_CTS_MASK_SCO };
-	static const __le32 bt_kill_cts_msg[2] = {
+		IWLAGN_BT_KILL_ACK_CTS_MASK_SCO,
+		IWLAGN_BT_KILL_ACK_CTS_MASK_REDUCE};
+	static const __le32 bt_kill_cts_msg[3] = {
 		IWLAGN_BT_KILL_CTS_MASK_DEFAULT,
-		IWLAGN_BT_KILL_ACK_CTS_MASK_SCO };
+		IWLAGN_BT_KILL_ACK_CTS_MASK_SCO,
+		IWLAGN_BT_KILL_ACK_CTS_MASK_REDUCE};
 
-	kill_msk = (BT_UART_MSG_FRAME3SCOESCO_MSK & uart_msg->frame3)
-		? 1 : 0;
+	if (!priv->reduced_txpower)
+		kill_msk = (BT_UART_MSG_FRAME3SCOESCO_MSK & uart_msg->frame3)
+			? IWL_BT_KILL_OVERRIDE : IWL_BT_KILL_DEFAULT;
 	if (priv->kill_ack_mask != bt_kill_ack_msg[kill_msk] ||
 	    priv->kill_cts_mask != bt_kill_cts_msg[kill_msk]) {
 		priv->bt_valid |= IWLAGN_BT_VALID_KILL_ACK_MASK;
 		priv->kill_ack_mask = bt_kill_ack_msg[kill_msk];
 		priv->bt_valid |= IWLAGN_BT_VALID_KILL_CTS_MASK;
 		priv->kill_cts_mask = bt_kill_cts_msg[kill_msk];
+		need_update = true;
+	}
+	return need_update;
+}
 
-		/* schedule to send runtime bt_config */
-		queue_work(priv->workqueue, &priv->bt_runtime_config);
+/*
+ * Upon RSSI changes, sends a bt config command with following changes
+ *  1. enable/disable "reduced control frames tx power
+ *  2. update the "kill)ack_mask" and "kill_cts_mask"
+ *
+ * If "reduced tx power" is enabled, uCode shall
+ *  1. ACK/Back/CTS rate shall reduced to 6Mbps
+ *  2. not use duplciate 20/40MHz mode
+ */
+static bool iwlagn_fill_txpower_mode(struct iwl_priv *priv,
+				struct iwl_bt_uart_msg *uart_msg)
+{
+	bool need_update = false;
+	struct iwl_rxon_context *ctx = &priv->contexts[IWL_RXON_CTX_BSS];
+	int ave_rssi;
+
+	ave_rssi = ieee80211_ave_rssi(ctx->vif);
+	if (!ave_rssi) {
+		/* no rssi data, no changes to reduce tx power */
+		IWL_DEBUG_COEX(priv, "no rssi data available\n");
+		return need_update;
+	}
+	if (!priv->reduced_txpower &&
+	    !iwl_is_associated(priv, IWL_RXON_CTX_PAN) &&
+	    (ave_rssi > BT_ENABLE_REDUCED_TXPOWER_THRESHOLD) &&
+	    (uart_msg->frame3 & (BT_UART_MSG_FRAME3ACL_MSK |
+	    BT_UART_MSG_FRAME3OBEX_MSK)) &&
+	    !(uart_msg->frame3 & (BT_UART_MSG_FRAME3SCOESCO_MSK |
+	    BT_UART_MSG_FRAME3SNIFF_MSK | BT_UART_MSG_FRAME3A2DP_MSK))) {
+		/* enabling reduced tx power */
+		priv->reduced_txpower = true;
+		priv->bt_valid |= IWLAGN_BT_VALID_REDUCED_TX_PWR;
+		need_update = true;
+	} else if (priv->reduced_txpower &&
+		   (iwl_is_associated(priv, IWL_RXON_CTX_PAN) ||
+		   (ave_rssi < BT_DISABLE_REDUCED_TXPOWER_THRESHOLD) ||
+		   (uart_msg->frame3 & (BT_UART_MSG_FRAME3SCOESCO_MSK |
+		   BT_UART_MSG_FRAME3SNIFF_MSK | BT_UART_MSG_FRAME3A2DP_MSK)) ||
+		   !(uart_msg->frame3 & (BT_UART_MSG_FRAME3ACL_MSK |
+		   BT_UART_MSG_FRAME3OBEX_MSK)))) {
+		/* disable reduced tx power */
+		priv->reduced_txpower = false;
+		priv->bt_valid |= IWLAGN_BT_VALID_REDUCED_TX_PWR;
+		need_update = true;
 	}
+
+	return need_update;
 }
 
 int iwlagn_bt_coex_profile_notif(struct iwl_priv *priv,
@@ -750,7 +695,12 @@ int iwlagn_bt_coex_profile_notif(struct iwl_priv *priv,
 		}
 	}
 
-	iwlagn_set_kill_msk(priv, uart_msg);
+	/* schedule to send runtime bt_config */
+	/* check reduce power before change ack/cts kill mask */
+	if (iwlagn_fill_txpower_mode(priv, uart_msg) ||
+	    iwlagn_set_kill_msk(priv, uart_msg))
+		queue_work(priv->workqueue, &priv->bt_runtime_config);
+
 
 	/* FIXME: based on notification, adjust the prio_boost */
 
@@ -798,8 +748,8 @@ static bool is_single_rx_stream(struct iwl_priv *priv)
  */
 static int iwl_get_active_rx_chain_count(struct iwl_priv *priv)
 {
-	if (cfg(priv)->bt_params &&
-	    cfg(priv)->bt_params->advanced_bt_coexist &&
+	if (priv->cfg->bt_params &&
+	    priv->cfg->bt_params->advanced_bt_coexist &&
 	    (priv->bt_full_concurrent ||
 	     priv->bt_traffic_load >= IWL_BT_COEX_TRAFFIC_LOAD_HIGH)) {
 		/*
@@ -856,7 +806,7 @@ static u8 iwl_count_chain_bitmap(u32 chain_bitmap)
 void iwlagn_set_rxon_chain(struct iwl_priv *priv, struct iwl_rxon_context *ctx)
 {
 	bool is_single = is_single_rx_stream(priv);
-	bool is_cam = !test_bit(STATUS_POWER_PMI, &priv->shrd->status);
+	bool is_cam = !test_bit(STATUS_POWER_PMI, &priv->status);
 	u8 idle_rx_cnt, active_rx_cnt, valid_rx_cnt;
 	u32 active_chains;
 	u16 rx_chain;
@@ -868,10 +818,10 @@ void iwlagn_set_rxon_chain(struct iwl_priv *priv, struct iwl_rxon_context *ctx)
 	if (priv->chain_noise_data.active_chains)
 		active_chains = priv->chain_noise_data.active_chains;
 	else
-		active_chains = hw_params(priv).valid_rx_ant;
+		active_chains = priv->hw_params.valid_rx_ant;
 
-	if (cfg(priv)->bt_params &&
-	    cfg(priv)->bt_params->advanced_bt_coexist &&
+	if (priv->cfg->bt_params &&
+	    priv->cfg->bt_params->advanced_bt_coexist &&
 	    (priv->bt_full_concurrent ||
 	     priv->bt_traffic_load >= IWL_BT_COEX_TRAFFIC_LOAD_HIGH)) {
 		/*
@@ -1190,7 +1140,7 @@ int iwlagn_suspend(struct iwl_priv *priv, struct cfg80211_wowlan *wowlan)
 	memcpy(&rxon, &ctx->active, sizeof(rxon));
 
 	priv->ucode_loaded = false;
-	iwl_trans_stop_device(trans(priv));
+	iwl_trans_stop_device(priv->trans);
 
 	priv->wowlan = true;
 
@@ -1212,7 +1162,7 @@ int iwlagn_suspend(struct iwl_priv *priv, struct cfg80211_wowlan *wowlan)
 	if (ret)
 		goto out;
 
-	if (!iwlagn_mod_params.sw_crypto) {
+	if (!iwlwifi_mod_params.sw_crypto) {
 		/* mark all keys clear */
 		priv->ucode_key_table = 0;
 		ctx->key_mapping_keys = 0;
@@ -1298,6 +1248,12 @@ int iwl_dvm_send_cmd(struct iwl_priv *priv, struct iwl_host_cmd *cmd)
 		return -EIO;
 	}
 
+	if (test_bit(STATUS_FW_ERROR, &priv->status)) {
+		IWL_ERR(priv, "Command %s failed: FW Error\n",
+			iwl_dvm_get_cmd_string(cmd->id));
+		return -EIO;
+	}
+
 	/*
 	 * Synchronous commands from this op-mode must hold
 	 * the mutex, this ensures we don't try to send two
@@ -1312,7 +1268,7 @@ int iwl_dvm_send_cmd(struct iwl_priv *priv, struct iwl_host_cmd *cmd)
 		return -EIO;
 	}
 
-	return iwl_trans_send_cmd(trans(priv), cmd);
+	return iwl_trans_send_cmd(priv->trans, cmd);
 }
 
 int iwl_dvm_send_cmd_pdu(struct iwl_priv *priv, u8 id,
diff --git a/drivers/net/wireless/iwlwifi/iwl-agn-rs.c b/drivers/net/wireless/iwlwifi/iwl-agn-rs.c
index 7e590b349dd..51e1a69ffdd 100644
--- a/drivers/net/wireless/iwlwifi/iwl-agn-rs.c
+++ b/drivers/net/wireless/iwlwifi/iwl-agn-rs.c
@@ -36,9 +36,9 @@
 #include <linux/workqueue.h>
 
 #include "iwl-dev.h"
-#include "iwl-core.h"
 #include "iwl-agn.h"
 #include "iwl-op-mode.h"
+#include "iwl-modparams.h"
 
 #define RS_NAME "iwl-agn-rs"
 
@@ -420,7 +420,7 @@ static int rs_tl_turn_on_agg_for_tid(struct iwl_priv *priv,
 
 	load = rs_tl_get_load(lq_data, tid);
 
-	if ((iwlagn_mod_params.auto_agg) || (load > IWL_AGG_LOAD_THRESHOLD)) {
+	if ((iwlwifi_mod_params.auto_agg) || (load > IWL_AGG_LOAD_THRESHOLD)) {
 		IWL_DEBUG_HT(priv, "Starting Tx agg: STA: %pM tid: %d\n",
 				sta->addr, tid);
 		ret = ieee80211_start_tx_ba_session(sta, tid, 5000);
@@ -819,7 +819,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(hw_params(priv).valid_tx_ant);
+			    first_antenna(priv->hw_params.valid_tx_ant);
 
 		tbl->is_ht40 = 0;
 		tbl->is_SGI = 0;
@@ -969,7 +969,7 @@ static void rs_tx_status(void *priv_r, struct ieee80211_supported_band *sband,
 	    (tbl_type.is_SGI != !!(mac_flags & IEEE80211_TX_RC_SHORT_GI)) ||
 	    (tbl_type.is_ht40 != !!(mac_flags & IEEE80211_TX_RC_40_MHZ_WIDTH)) ||
 	    (tbl_type.is_dup != !!(mac_flags & IEEE80211_TX_RC_DUP_DATA)) ||
-	    (tbl_type.ant_type != info->antenna_sel_tx) ||
+	    (tbl_type.ant_type != info->status.antenna) ||
 	    (!!(tx_rate & RATE_MCS_HT_MSK) != !!(mac_flags & IEEE80211_TX_RC_MCS)) ||
 	    (!!(tx_rate & RATE_MCS_GF_MSK) != !!(mac_flags & IEEE80211_TX_RC_GREEN_FIELD)) ||
 	    (rs_index != mac_index)) {
@@ -1085,7 +1085,7 @@ done:
 	    (priv->tm_fixed_rate != lq_sta->dbg_fixed_rate))
 		rs_program_fix_rate(priv, lq_sta);
 #endif
-	if (cfg(priv)->bt_params && cfg(priv)->bt_params->advanced_bt_coexist)
+	if (priv->cfg->bt_params && priv->cfg->bt_params->advanced_bt_coexist)
 		rs_bt_update_lq(priv, ctx, lq_sta);
 }
 
@@ -1291,7 +1291,7 @@ static int rs_switch_to_mimo2(struct iwl_priv *priv,
 		return -1;
 
 	/* Need both Tx chains/antennas to support MIMO */
-	if (hw_params(priv).tx_chains_num < 2)
+	if (priv->hw_params.tx_chains_num < 2)
 		return -1;
 
 	IWL_DEBUG_RATE(priv, "LQ: try to switch to MIMO2\n");
@@ -1347,7 +1347,7 @@ static int rs_switch_to_mimo3(struct iwl_priv *priv,
 		return -1;
 
 	/* Need both Tx chains/antennas to support MIMO */
-	if (hw_params(priv).tx_chains_num < 3)
+	if (priv->hw_params.tx_chains_num < 3)
 		return -1;
 
 	IWL_DEBUG_RATE(priv, "LQ: try to switch to MIMO3\n");
@@ -1446,8 +1446,8 @@ static int rs_move_legacy_other(struct iwl_priv *priv,
 	u32 sz = (sizeof(struct iwl_scale_tbl_info) -
 		  (sizeof(struct iwl_rate_scale_data) * IWL_RATE_COUNT));
 	u8 start_action;
-	u8 valid_tx_ant = hw_params(priv).valid_tx_ant;
-	u8 tx_chains_num = hw_params(priv).tx_chains_num;
+	u8 valid_tx_ant = priv->hw_params.valid_tx_ant;
+	u8 tx_chains_num = priv->hw_params.tx_chains_num;
 	int ret = 0;
 	u8 update_search_tbl_counter = 0;
 
@@ -1464,7 +1464,7 @@ static int rs_move_legacy_other(struct iwl_priv *priv,
 	case IWL_BT_COEX_TRAFFIC_LOAD_CONTINUOUS:
 		/* avoid antenna B and MIMO */
 		valid_tx_ant =
-			first_antenna(hw_params(priv).valid_tx_ant);
+			first_antenna(priv->hw_params.valid_tx_ant);
 		if (tbl->action >= IWL_LEGACY_SWITCH_ANTENNA2 &&
 		    tbl->action != IWL_LEGACY_SWITCH_SISO)
 			tbl->action = IWL_LEGACY_SWITCH_SISO;
@@ -1488,7 +1488,7 @@ static int rs_move_legacy_other(struct iwl_priv *priv,
 		else if (tbl->action >= IWL_LEGACY_SWITCH_ANTENNA2)
 			tbl->action = IWL_LEGACY_SWITCH_SISO;
 		valid_tx_ant =
-			first_antenna(hw_params(priv).valid_tx_ant);
+			first_antenna(priv->hw_params.valid_tx_ant);
 	}
 
 	start_action = tbl->action;
@@ -1622,8 +1622,8 @@ static int rs_move_siso_to_other(struct iwl_priv *priv,
 	u32 sz = (sizeof(struct iwl_scale_tbl_info) -
 		  (sizeof(struct iwl_rate_scale_data) * IWL_RATE_COUNT));
 	u8 start_action;
-	u8 valid_tx_ant = hw_params(priv).valid_tx_ant;
-	u8 tx_chains_num = hw_params(priv).tx_chains_num;
+	u8 valid_tx_ant = priv->hw_params.valid_tx_ant;
+	u8 tx_chains_num = priv->hw_params.tx_chains_num;
 	u8 update_search_tbl_counter = 0;
 	int ret;
 
@@ -1640,7 +1640,7 @@ static int rs_move_siso_to_other(struct iwl_priv *priv,
 	case IWL_BT_COEX_TRAFFIC_LOAD_CONTINUOUS:
 		/* avoid antenna B and MIMO */
 		valid_tx_ant =
-			first_antenna(hw_params(priv).valid_tx_ant);
+			first_antenna(priv->hw_params.valid_tx_ant);
 		if (tbl->action != IWL_SISO_SWITCH_ANTENNA1)
 			tbl->action = IWL_SISO_SWITCH_ANTENNA1;
 		break;
@@ -1658,7 +1658,7 @@ static int rs_move_siso_to_other(struct iwl_priv *priv,
 	/* configure as 1x1 if bt full concurrency */
 	if (priv->bt_full_concurrent) {
 		valid_tx_ant =
-			first_antenna(hw_params(priv).valid_tx_ant);
+			first_antenna(priv->hw_params.valid_tx_ant);
 		if (tbl->action >= IWL_LEGACY_SWITCH_ANTENNA2)
 			tbl->action = IWL_SISO_SWITCH_ANTENNA1;
 	}
@@ -1794,8 +1794,8 @@ static int rs_move_mimo2_to_other(struct iwl_priv *priv,
 	u32 sz = (sizeof(struct iwl_scale_tbl_info) -
 		  (sizeof(struct iwl_rate_scale_data) * IWL_RATE_COUNT));
 	u8 start_action;
-	u8 valid_tx_ant = hw_params(priv).valid_tx_ant;
-	u8 tx_chains_num = hw_params(priv).tx_chains_num;
+	u8 valid_tx_ant = priv->hw_params.valid_tx_ant;
+	u8 tx_chains_num = priv->hw_params.tx_chains_num;
 	u8 update_search_tbl_counter = 0;
 	int ret;
 
@@ -1964,8 +1964,8 @@ static int rs_move_mimo3_to_other(struct iwl_priv *priv,
 	u32 sz = (sizeof(struct iwl_scale_tbl_info) -
 		  (sizeof(struct iwl_rate_scale_data) * IWL_RATE_COUNT));
 	u8 start_action;
-	u8 valid_tx_ant = hw_params(priv).valid_tx_ant;
-	u8 tx_chains_num = hw_params(priv).tx_chains_num;
+	u8 valid_tx_ant = priv->hw_params.valid_tx_ant;
+	u8 tx_chains_num = priv->hw_params.tx_chains_num;
 	int ret;
 	u8 update_search_tbl_counter = 0;
 
@@ -2166,7 +2166,7 @@ static void rs_stay_in_table(struct iwl_lq_sta *lq_sta, bool force_search)
 		    (lq_sta->total_success > lq_sta->max_success_limit) ||
 		    ((!lq_sta->search_better_tbl) && (lq_sta->flush_timer)
 		     && (flush_interval_passed))) {
-			IWL_DEBUG_RATE(priv, "LQ: stay is expired %d %d %d\n:",
+			IWL_DEBUG_RATE(priv, "LQ: stay is expired %d %d %d\n",
 				     lq_sta->total_failed,
 				     lq_sta->total_success,
 				     flush_interval_passed);
@@ -2698,7 +2698,7 @@ static void rs_initialize_lq(struct iwl_priv *priv,
 
 	i = lq_sta->last_txrate_idx;
 
-	valid_tx_ant = hw_params(priv).valid_tx_ant;
+	valid_tx_ant = priv->hw_params.valid_tx_ant;
 
 	if (!lq_sta->search_better_tbl)
 		active_tbl = lq_sta->active_tbl;
@@ -2826,6 +2826,7 @@ void iwl_rs_rate_init(struct iwl_priv *priv, struct ieee80211_sta *sta, u8 sta_i
 	struct iwl_station_priv *sta_priv;
 	struct iwl_lq_sta *lq_sta;
 	struct ieee80211_supported_band *sband;
+	unsigned long supp; /* must be unsigned long for for_each_set_bit */
 
 	sta_priv = (struct iwl_station_priv *) sta->drv_priv;
 	lq_sta = &sta_priv->lq_sta;
@@ -2855,8 +2856,15 @@ void iwl_rs_rate_init(struct iwl_priv *priv, struct ieee80211_sta *sta, u8 sta_i
 	lq_sta->max_rate_idx = -1;
 	lq_sta->missed_rate_counter = IWL_MISSED_RATE_MAX;
 	lq_sta->is_green = rs_use_green(sta);
-	lq_sta->active_legacy_rate = priv->active_rate & ~(0x1000);
-	lq_sta->band = priv->band;
+	lq_sta->band = sband->band;
+	/*
+	 * active legacy rates as per supported rates bitmap
+	 */
+	supp = sta->supp_rates[sband->band];
+	lq_sta->active_legacy_rate = 0;
+	for_each_set_bit(i, &supp, BITS_PER_LONG)
+		lq_sta->active_legacy_rate |= BIT(sband->bitrates[i].hw_value);
+
 	/*
 	 * active_siso_rate mask includes 9 MBits (bit 5), and CCK (bits 0-3),
 	 * supp_rates[] does not; shift to convert format, force 9 MBits off.
@@ -2884,15 +2892,15 @@ void iwl_rs_rate_init(struct iwl_priv *priv, struct ieee80211_sta *sta, u8 sta_i
 
 	/* These values will be overridden later */
 	lq_sta->lq.general_params.single_stream_ant_msk =
-		first_antenna(hw_params(priv).valid_tx_ant);
+		first_antenna(priv->hw_params.valid_tx_ant);
 	lq_sta->lq.general_params.dual_stream_ant_msk =
-		hw_params(priv).valid_tx_ant &
-		~first_antenna(hw_params(priv).valid_tx_ant);
+		priv->hw_params.valid_tx_ant &
+		~first_antenna(priv->hw_params.valid_tx_ant);
 	if (!lq_sta->lq.general_params.dual_stream_ant_msk) {
 		lq_sta->lq.general_params.dual_stream_ant_msk = ANT_AB;
-	} else if (num_of_ant(hw_params(priv).valid_tx_ant) == 2) {
+	} else if (num_of_ant(priv->hw_params.valid_tx_ant) == 2) {
 		lq_sta->lq.general_params.dual_stream_ant_msk =
-			hw_params(priv).valid_tx_ant;
+			priv->hw_params.valid_tx_ant;
 	}
 
 	/* as default allow aggregation for all tids */
@@ -2938,7 +2946,7 @@ static void rs_fill_link_cmd(struct iwl_priv *priv,
 	if (priv && priv->bt_full_concurrent) {
 		/* 1x1 only */
 		tbl_type.ant_type =
-			first_antenna(hw_params(priv).valid_tx_ant);
+			first_antenna(priv->hw_params.valid_tx_ant);
 	}
 
 	/* How many times should we repeat the initial rate? */
@@ -2970,7 +2978,7 @@ static void rs_fill_link_cmd(struct iwl_priv *priv,
 		if (priv->bt_full_concurrent)
 			valid_tx_ant = ANT_A;
 		else
-			valid_tx_ant = hw_params(priv).valid_tx_ant;
+			valid_tx_ant = priv->hw_params.valid_tx_ant;
 	}
 
 	/* Fill rest of rate table */
@@ -3004,7 +3012,7 @@ static void rs_fill_link_cmd(struct iwl_priv *priv,
 		if (priv && priv->bt_full_concurrent) {
 			/* 1x1 only */
 			tbl_type.ant_type =
-			    first_antenna(hw_params(priv).valid_tx_ant);
+			    first_antenna(priv->hw_params.valid_tx_ant);
 		}
 
 		/* Indicate to uCode which entries might be MIMO.
@@ -3055,11 +3063,11 @@ static void rs_fill_link_cmd(struct iwl_priv *priv,
 	 * overwrite if needed, pass aggregation time limit
 	 * to uCode in uSec
 	 */
-	if (priv && cfg(priv)->bt_params &&
-	    cfg(priv)->bt_params->agg_time_limit &&
+	if (priv && priv->cfg->bt_params &&
+	    priv->cfg->bt_params->agg_time_limit &&
 	    priv->bt_traffic_load >= IWL_BT_COEX_TRAFFIC_LOAD_HIGH)
 		lq_cmd->agg_params.agg_time_limit =
-			cpu_to_le16(cfg(priv)->bt_params->agg_time_limit);
+			cpu_to_le16(priv->cfg->bt_params->agg_time_limit);
 }
 
 static void *rs_alloc(struct ieee80211_hw *hw, struct dentry *debugfsdir)
@@ -3091,7 +3099,7 @@ static void rs_dbgfs_set_mcs(struct iwl_lq_sta *lq_sta,
 	u8 ant_sel_tx;
 
 	priv = lq_sta->drv;
-	valid_tx_ant = hw_params(priv).valid_tx_ant;
+	valid_tx_ant = priv->hw_params.valid_tx_ant;
 	if (lq_sta->dbg_fixed_rate) {
 		ant_sel_tx =
 		  ((lq_sta->dbg_fixed_rate & RATE_MCS_ANT_ABC_MSK)
@@ -3162,9 +3170,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",
-	    (hw_params(priv).valid_tx_ant & ANT_A) ? "ANT_A," : "",
-	    (hw_params(priv).valid_tx_ant & ANT_B) ? "ANT_B," : "",
-	    (hw_params(priv).valid_tx_ant & ANT_C) ? "ANT_C" : "");
+	    (priv->hw_params.valid_tx_ant & ANT_A) ? "ANT_A," : "",
+	    (priv->hw_params.valid_tx_ant & ANT_B) ? "ANT_B," : "",
+	    (priv->hw_params.valid_tx_ant & 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/iwl-agn-rs.h b/drivers/net/wireless/iwlwifi/iwl-agn-rs.h
index 203b1c13c49..82d02e1ae89 100644
--- a/drivers/net/wireless/iwlwifi/iwl-agn-rs.h
+++ b/drivers/net/wireless/iwlwifi/iwl-agn-rs.h
@@ -30,6 +30,7 @@
 #include <net/mac80211.h>
 
 #include "iwl-commands.h"
+#include "iwl-config.h"
 
 struct iwl_rate_info {
 	u8 plcp;	/* uCode API:  IWL_RATE_6M_PLCP, etc. */
@@ -174,32 +175,6 @@ enum {
 	IWL_RATE_11M_IEEE = 22,
 };
 
-#define IWL_CCK_BASIC_RATES_MASK    \
-       (IWL_RATE_1M_MASK          | \
-	IWL_RATE_2M_MASK)
-
-#define IWL_CCK_RATES_MASK          \
-       (IWL_CCK_BASIC_RATES_MASK  | \
-	IWL_RATE_5M_MASK          | \
-	IWL_RATE_11M_MASK)
-
-#define IWL_OFDM_BASIC_RATES_MASK   \
-	(IWL_RATE_6M_MASK         | \
-	IWL_RATE_12M_MASK         | \
-	IWL_RATE_24M_MASK)
-
-#define IWL_OFDM_RATES_MASK         \
-       (IWL_OFDM_BASIC_RATES_MASK | \
-	IWL_RATE_9M_MASK          | \
-	IWL_RATE_18M_MASK         | \
-	IWL_RATE_36M_MASK         | \
-	IWL_RATE_48M_MASK         | \
-	IWL_RATE_54M_MASK)
-
-#define IWL_BASIC_RATES_MASK         \
-	(IWL_OFDM_BASIC_RATES_MASK | \
-	 IWL_CCK_BASIC_RATES_MASK)
-
 #define IWL_RATES_MASK ((1 << IWL_RATE_COUNT) - 1)
 
 #define IWL_INVALID_VALUE    -1
@@ -306,15 +281,6 @@ enum iwl_table_type {
 #define is_a_band(tbl) ((tbl) == LQ_A)
 #define is_g_and(tbl) ((tbl) == LQ_G)
 
-#define	ANT_NONE	0x0
-#define	ANT_A		BIT(0)
-#define	ANT_B		BIT(1)
-#define	ANT_AB		(ANT_A | ANT_B)
-#define ANT_C		BIT(2)
-#define	ANT_AC		(ANT_A | ANT_C)
-#define ANT_BC		(ANT_B | ANT_C)
-#define ANT_ABC		(ANT_AB | ANT_C)
-
 #define IWL_MAX_MCS_DISPLAY_SIZE	12
 
 struct iwl_rate_mcs_info {
diff --git a/drivers/net/wireless/iwlwifi/iwl-agn-rx.c b/drivers/net/wireless/iwlwifi/iwl-agn-rx.c
index f4b84d1596e..403de96f974 100644
--- a/drivers/net/wireless/iwlwifi/iwl-agn-rx.c
+++ b/drivers/net/wireless/iwlwifi/iwl-agn-rx.c
@@ -34,95 +34,91 @@
 #include <asm/unaligned.h>
 #include "iwl-eeprom.h"
 #include "iwl-dev.h"
-#include "iwl-core.h"
 #include "iwl-io.h"
 #include "iwl-agn-calib.h"
 #include "iwl-agn.h"
-#include "iwl-shared.h"
-
-const char *get_cmd_string(u8 cmd)
-{
-	switch (cmd) {
-		IWL_CMD(REPLY_ALIVE);
-		IWL_CMD(REPLY_ERROR);
-		IWL_CMD(REPLY_ECHO);
-		IWL_CMD(REPLY_RXON);
-		IWL_CMD(REPLY_RXON_ASSOC);
-		IWL_CMD(REPLY_QOS_PARAM);
-		IWL_CMD(REPLY_RXON_TIMING);
-		IWL_CMD(REPLY_ADD_STA);
-		IWL_CMD(REPLY_REMOVE_STA);
-		IWL_CMD(REPLY_REMOVE_ALL_STA);
-		IWL_CMD(REPLY_TXFIFO_FLUSH);
-		IWL_CMD(REPLY_WEPKEY);
-		IWL_CMD(REPLY_TX);
-		IWL_CMD(REPLY_LEDS_CMD);
-		IWL_CMD(REPLY_TX_LINK_QUALITY_CMD);
-		IWL_CMD(COEX_PRIORITY_TABLE_CMD);
-		IWL_CMD(COEX_MEDIUM_NOTIFICATION);
-		IWL_CMD(COEX_EVENT_CMD);
-		IWL_CMD(REPLY_QUIET_CMD);
-		IWL_CMD(REPLY_CHANNEL_SWITCH);
-		IWL_CMD(CHANNEL_SWITCH_NOTIFICATION);
-		IWL_CMD(REPLY_SPECTRUM_MEASUREMENT_CMD);
-		IWL_CMD(SPECTRUM_MEASURE_NOTIFICATION);
-		IWL_CMD(POWER_TABLE_CMD);
-		IWL_CMD(PM_SLEEP_NOTIFICATION);
-		IWL_CMD(PM_DEBUG_STATISTIC_NOTIFIC);
-		IWL_CMD(REPLY_SCAN_CMD);
-		IWL_CMD(REPLY_SCAN_ABORT_CMD);
-		IWL_CMD(SCAN_START_NOTIFICATION);
-		IWL_CMD(SCAN_RESULTS_NOTIFICATION);
-		IWL_CMD(SCAN_COMPLETE_NOTIFICATION);
-		IWL_CMD(BEACON_NOTIFICATION);
-		IWL_CMD(REPLY_TX_BEACON);
-		IWL_CMD(WHO_IS_AWAKE_NOTIFICATION);
-		IWL_CMD(QUIET_NOTIFICATION);
-		IWL_CMD(REPLY_TX_PWR_TABLE_CMD);
-		IWL_CMD(MEASURE_ABORT_NOTIFICATION);
-		IWL_CMD(REPLY_BT_CONFIG);
-		IWL_CMD(REPLY_STATISTICS_CMD);
-		IWL_CMD(STATISTICS_NOTIFICATION);
-		IWL_CMD(REPLY_CARD_STATE_CMD);
-		IWL_CMD(CARD_STATE_NOTIFICATION);
-		IWL_CMD(MISSED_BEACONS_NOTIFICATION);
-		IWL_CMD(REPLY_CT_KILL_CONFIG_CMD);
-		IWL_CMD(SENSITIVITY_CMD);
-		IWL_CMD(REPLY_PHY_CALIBRATION_CMD);
-		IWL_CMD(REPLY_RX_PHY_CMD);
-		IWL_CMD(REPLY_RX_MPDU_CMD);
-		IWL_CMD(REPLY_RX);
-		IWL_CMD(REPLY_COMPRESSED_BA);
-		IWL_CMD(CALIBRATION_CFG_CMD);
-		IWL_CMD(CALIBRATION_RES_NOTIFICATION);
-		IWL_CMD(CALIBRATION_COMPLETE_NOTIFICATION);
-		IWL_CMD(REPLY_TX_POWER_DBM_CMD);
-		IWL_CMD(TEMPERATURE_NOTIFICATION);
-		IWL_CMD(TX_ANT_CONFIGURATION_CMD);
-		IWL_CMD(REPLY_BT_COEX_PROFILE_NOTIF);
-		IWL_CMD(REPLY_BT_COEX_PRIO_TABLE);
-		IWL_CMD(REPLY_BT_COEX_PROT_ENV);
-		IWL_CMD(REPLY_WIPAN_PARAMS);
-		IWL_CMD(REPLY_WIPAN_RXON);
-		IWL_CMD(REPLY_WIPAN_RXON_TIMING);
-		IWL_CMD(REPLY_WIPAN_RXON_ASSOC);
-		IWL_CMD(REPLY_WIPAN_QOS_PARAM);
-		IWL_CMD(REPLY_WIPAN_WEPKEY);
-		IWL_CMD(REPLY_WIPAN_P2P_CHANNEL_SWITCH);
-		IWL_CMD(REPLY_WIPAN_NOA_NOTIFICATION);
-		IWL_CMD(REPLY_WIPAN_DEACTIVATION_COMPLETE);
-		IWL_CMD(REPLY_WOWLAN_PATTERNS);
-		IWL_CMD(REPLY_WOWLAN_WAKEUP_FILTER);
-		IWL_CMD(REPLY_WOWLAN_TSC_RSC_PARAMS);
-		IWL_CMD(REPLY_WOWLAN_TKIP_PARAMS);
-		IWL_CMD(REPLY_WOWLAN_KEK_KCK_MATERIAL);
-		IWL_CMD(REPLY_WOWLAN_GET_STATUS);
-		IWL_CMD(REPLY_D3_CONFIG);
-	default:
-		return "UNKNOWN";
-
-	}
-}
+#include "iwl-modparams.h"
+
+#define IWL_CMD_ENTRY(x) [x] = #x
+
+const char *iwl_dvm_cmd_strings[REPLY_MAX] = {
+	IWL_CMD_ENTRY(REPLY_ALIVE),
+	IWL_CMD_ENTRY(REPLY_ERROR),
+	IWL_CMD_ENTRY(REPLY_ECHO),
+	IWL_CMD_ENTRY(REPLY_RXON),
+	IWL_CMD_ENTRY(REPLY_RXON_ASSOC),
+	IWL_CMD_ENTRY(REPLY_QOS_PARAM),
+	IWL_CMD_ENTRY(REPLY_RXON_TIMING),
+	IWL_CMD_ENTRY(REPLY_ADD_STA),
+	IWL_CMD_ENTRY(REPLY_REMOVE_STA),
+	IWL_CMD_ENTRY(REPLY_REMOVE_ALL_STA),
+	IWL_CMD_ENTRY(REPLY_TXFIFO_FLUSH),
+	IWL_CMD_ENTRY(REPLY_WEPKEY),
+	IWL_CMD_ENTRY(REPLY_TX),
+	IWL_CMD_ENTRY(REPLY_LEDS_CMD),
+	IWL_CMD_ENTRY(REPLY_TX_LINK_QUALITY_CMD),
+	IWL_CMD_ENTRY(COEX_PRIORITY_TABLE_CMD),
+	IWL_CMD_ENTRY(COEX_MEDIUM_NOTIFICATION),
+	IWL_CMD_ENTRY(COEX_EVENT_CMD),
+	IWL_CMD_ENTRY(REPLY_QUIET_CMD),
+	IWL_CMD_ENTRY(REPLY_CHANNEL_SWITCH),
+	IWL_CMD_ENTRY(CHANNEL_SWITCH_NOTIFICATION),
+	IWL_CMD_ENTRY(REPLY_SPECTRUM_MEASUREMENT_CMD),
+	IWL_CMD_ENTRY(SPECTRUM_MEASURE_NOTIFICATION),
+	IWL_CMD_ENTRY(POWER_TABLE_CMD),
+	IWL_CMD_ENTRY(PM_SLEEP_NOTIFICATION),
+	IWL_CMD_ENTRY(PM_DEBUG_STATISTIC_NOTIFIC),
+	IWL_CMD_ENTRY(REPLY_SCAN_CMD),
+	IWL_CMD_ENTRY(REPLY_SCAN_ABORT_CMD),
+	IWL_CMD_ENTRY(SCAN_START_NOTIFICATION),
+	IWL_CMD_ENTRY(SCAN_RESULTS_NOTIFICATION),
+	IWL_CMD_ENTRY(SCAN_COMPLETE_NOTIFICATION),
+	IWL_CMD_ENTRY(BEACON_NOTIFICATION),
+	IWL_CMD_ENTRY(REPLY_TX_BEACON),
+	IWL_CMD_ENTRY(WHO_IS_AWAKE_NOTIFICATION),
+	IWL_CMD_ENTRY(QUIET_NOTIFICATION),
+	IWL_CMD_ENTRY(REPLY_TX_PWR_TABLE_CMD),
+	IWL_CMD_ENTRY(MEASURE_ABORT_NOTIFICATION),
+	IWL_CMD_ENTRY(REPLY_BT_CONFIG),
+	IWL_CMD_ENTRY(REPLY_STATISTICS_CMD),
+	IWL_CMD_ENTRY(STATISTICS_NOTIFICATION),
+	IWL_CMD_ENTRY(REPLY_CARD_STATE_CMD),
+	IWL_CMD_ENTRY(CARD_STATE_NOTIFICATION),
+	IWL_CMD_ENTRY(MISSED_BEACONS_NOTIFICATION),
+	IWL_CMD_ENTRY(REPLY_CT_KILL_CONFIG_CMD),
+	IWL_CMD_ENTRY(SENSITIVITY_CMD),
+	IWL_CMD_ENTRY(REPLY_PHY_CALIBRATION_CMD),
+	IWL_CMD_ENTRY(REPLY_RX_PHY_CMD),
+	IWL_CMD_ENTRY(REPLY_RX_MPDU_CMD),
+	IWL_CMD_ENTRY(REPLY_RX),
+	IWL_CMD_ENTRY(REPLY_COMPRESSED_BA),
+	IWL_CMD_ENTRY(CALIBRATION_CFG_CMD),
+	IWL_CMD_ENTRY(CALIBRATION_RES_NOTIFICATION),
+	IWL_CMD_ENTRY(CALIBRATION_COMPLETE_NOTIFICATION),
+	IWL_CMD_ENTRY(REPLY_TX_POWER_DBM_CMD),
+	IWL_CMD_ENTRY(TEMPERATURE_NOTIFICATION),
+	IWL_CMD_ENTRY(TX_ANT_CONFIGURATION_CMD),
+	IWL_CMD_ENTRY(REPLY_BT_COEX_PROFILE_NOTIF),
+	IWL_CMD_ENTRY(REPLY_BT_COEX_PRIO_TABLE),
+	IWL_CMD_ENTRY(REPLY_BT_COEX_PROT_ENV),
+	IWL_CMD_ENTRY(REPLY_WIPAN_PARAMS),
+	IWL_CMD_ENTRY(REPLY_WIPAN_RXON),
+	IWL_CMD_ENTRY(REPLY_WIPAN_RXON_TIMING),
+	IWL_CMD_ENTRY(REPLY_WIPAN_RXON_ASSOC),
+	IWL_CMD_ENTRY(REPLY_WIPAN_QOS_PARAM),
+	IWL_CMD_ENTRY(REPLY_WIPAN_WEPKEY),
+	IWL_CMD_ENTRY(REPLY_WIPAN_P2P_CHANNEL_SWITCH),
+	IWL_CMD_ENTRY(REPLY_WIPAN_NOA_NOTIFICATION),
+	IWL_CMD_ENTRY(REPLY_WIPAN_DEACTIVATION_COMPLETE),
+	IWL_CMD_ENTRY(REPLY_WOWLAN_PATTERNS),
+	IWL_CMD_ENTRY(REPLY_WOWLAN_WAKEUP_FILTER),
+	IWL_CMD_ENTRY(REPLY_WOWLAN_TSC_RSC_PARAMS),
+	IWL_CMD_ENTRY(REPLY_WOWLAN_TKIP_PARAMS),
+	IWL_CMD_ENTRY(REPLY_WOWLAN_KEK_KCK_MATERIAL),
+	IWL_CMD_ENTRY(REPLY_WOWLAN_GET_STATUS),
+	IWL_CMD_ENTRY(REPLY_D3_CONFIG),
+};
+#undef IWL_CMD_ENTRY
 
 /******************************************************************************
  *
@@ -137,10 +133,9 @@ static int iwlagn_rx_reply_error(struct iwl_priv *priv,
 	struct iwl_rx_packet *pkt = rxb_addr(rxb);
 	struct iwl_error_resp *err_resp = (void *)pkt->data;
 
-	IWL_ERR(priv, "Error Reply type 0x%08X cmd %s (0x%02X) "
+	IWL_ERR(priv, "Error Reply type 0x%08X cmd REPLY_ERROR (0x%02X) "
 		"seq 0x%04X ser 0x%08X\n",
 		le32_to_cpu(err_resp->error_type),
-		get_cmd_string(err_resp->cmd_id),
 		err_resp->cmd_id,
 		le16_to_cpu(err_resp->bad_cmd_seq_num),
 		le32_to_cpu(err_resp->error_info));
@@ -216,8 +211,7 @@ static int iwlagn_rx_pm_debug_statistics_notif(struct iwl_priv *priv,
 	u32 __maybe_unused len =
 		le32_to_cpu(pkt->len_n_flags) & FH_RSCSR_FRAME_SIZE_MSK;
 	IWL_DEBUG_RADIO(priv, "Dumping %d bytes of unhandled "
-			"notification for %s:\n", len,
-			get_cmd_string(pkt->hdr.cmd));
+			"notification for PM_DEBUG_STATISTIC_NOTIFIC:\n", len);
 	iwl_print_hex_dump(priv, IWL_DL_RADIO, pkt->data, len);
 	return 0;
 }
@@ -246,69 +240,6 @@ static int iwlagn_rx_beacon_notif(struct iwl_priv *priv,
 	return 0;
 }
 
-/* the threshold ratio of actual_ack_cnt to expected_ack_cnt in percent */
-#define ACK_CNT_RATIO (50)
-#define BA_TIMEOUT_CNT (5)
-#define BA_TIMEOUT_MAX (16)
-
-/**
- * iwl_good_ack_health - checks for ACK count ratios, BA timeout retries.
- *
- * When the ACK count ratio is low and aggregated BA timeout retries exceeding
- * the BA_TIMEOUT_MAX, reload firmware and bring system back to normal
- * operation state.
- */
-static bool iwlagn_good_ack_health(struct iwl_priv *priv,
-				struct statistics_tx *cur)
-{
-	int actual_delta, expected_delta, ba_timeout_delta;
-	struct statistics_tx *old;
-
-	if (priv->agg_tids_count)
-		return true;
-
-	lockdep_assert_held(&priv->statistics.lock);
-
-	old = &priv->statistics.tx;
-
-	actual_delta = le32_to_cpu(cur->actual_ack_cnt) -
-		       le32_to_cpu(old->actual_ack_cnt);
-	expected_delta = le32_to_cpu(cur->expected_ack_cnt) -
-			 le32_to_cpu(old->expected_ack_cnt);
-
-	/* Values should not be negative, but we do not trust the firmware */
-	if (actual_delta <= 0 || expected_delta <= 0)
-		return true;
-
-	ba_timeout_delta = le32_to_cpu(cur->agg.ba_timeout) -
-			   le32_to_cpu(old->agg.ba_timeout);
-
-	if ((actual_delta * 100 / expected_delta) < ACK_CNT_RATIO &&
-	    ba_timeout_delta > BA_TIMEOUT_CNT) {
-		IWL_DEBUG_RADIO(priv,
-			"deltas: actual %d expected %d ba_timeout %d\n",
-			actual_delta, expected_delta, ba_timeout_delta);
-
-#ifdef CONFIG_IWLWIFI_DEBUGFS
-		/*
-		 * This is ifdef'ed on DEBUGFS because otherwise the
-		 * statistics aren't available. If DEBUGFS is set but
-		 * DEBUG is not, these will just compile out.
-		 */
-		IWL_DEBUG_RADIO(priv, "rx_detected_cnt delta %d\n",
-				priv->delta_stats.tx.rx_detected_cnt);
-		IWL_DEBUG_RADIO(priv,
-				"ack_or_ba_timeout_collision delta %d\n",
-				priv->delta_stats.tx.ack_or_ba_timeout_collision);
-#endif
-
-		if (ba_timeout_delta >= BA_TIMEOUT_MAX)
-			return false;
-	}
-
-	return true;
-}
-
 /**
  * iwl_good_plcp_health - checks for plcp error.
  *
@@ -347,6 +278,45 @@ static bool iwlagn_good_plcp_health(struct iwl_priv *priv,
 	return true;
 }
 
+int iwl_force_rf_reset(struct iwl_priv *priv, bool external)
+{
+	struct iwl_rf_reset *rf_reset;
+
+	if (test_bit(STATUS_EXIT_PENDING, &priv->status))
+		return -EAGAIN;
+
+	if (!iwl_is_any_associated(priv)) {
+		IWL_DEBUG_SCAN(priv, "force reset rejected: not associated\n");
+		return -ENOLINK;
+	}
+
+	rf_reset = &priv->rf_reset;
+	rf_reset->reset_request_count++;
+	if (!external && rf_reset->last_reset_jiffies &&
+	    time_after(rf_reset->last_reset_jiffies +
+		       IWL_DELAY_NEXT_FORCE_RF_RESET, jiffies)) {
+		IWL_DEBUG_INFO(priv, "RF reset rejected\n");
+		rf_reset->reset_reject_count++;
+		return -EAGAIN;
+	}
+	rf_reset->reset_success_count++;
+	rf_reset->last_reset_jiffies = jiffies;
+
+	/*
+	 * There is no easy and better way to force reset the radio,
+	 * the only known method is switching channel which will force to
+	 * reset and tune the radio.
+	 * Use internal short scan (single channel) operation to should
+	 * achieve this objective.
+	 * Driver should reset the radio when number of consecutive missed
+	 * beacon, or any other uCode error condition detected.
+	 */
+	IWL_DEBUG_INFO(priv, "perform radio reset.\n");
+	iwl_internal_short_hw_scan(priv);
+	return 0;
+}
+
+
 static void iwlagn_recover_from_statistics(struct iwl_priv *priv,
 				struct statistics_rx_phy *cur_ofdm,
 				struct statistics_rx_ht_phy *cur_ofdm_ht,
@@ -368,15 +338,9 @@ static void iwlagn_recover_from_statistics(struct iwl_priv *priv,
 	if (msecs < 99)
 		return;
 
-	if (iwlagn_mod_params.ack_check && !iwlagn_good_ack_health(priv, tx)) {
-		IWL_ERR(priv, "low ack count detected, restart firmware\n");
-		if (!iwl_force_reset(priv, IWL_FW_RESET, false))
-			return;
-	}
-
-	if (iwlagn_mod_params.plcp_check &&
+	if (iwlwifi_mod_params.plcp_check &&
 	    !iwlagn_good_plcp_health(priv, cur_ofdm, cur_ofdm_ht, msecs))
-		iwl_force_reset(priv, IWL_RF_RESET, false);
+		iwl_force_rf_reset(priv, false);
 }
 
 /* Calculate noise level, based on measurements during network silence just
@@ -589,8 +553,8 @@ static int iwlagn_rx_statistics(struct iwl_priv *priv,
 		iwlagn_rx_calc_noise(priv);
 		queue_work(priv->workqueue, &priv->run_time_calib_work);
 	}
-	if (cfg(priv)->lib->temperature && change)
-		cfg(priv)->lib->temperature(priv);
+	if (priv->lib->temperature && change)
+		priv->lib->temperature(priv);
 
 	spin_unlock(&priv->statistics.lock);
 
@@ -639,16 +603,16 @@ static int iwlagn_rx_card_state_notif(struct iwl_priv *priv,
 	if (flags & (SW_CARD_DISABLED | HW_CARD_DISABLED |
 		     CT_CARD_DISABLED)) {
 
-		iwl_write32(trans(priv), CSR_UCODE_DRV_GP1_SET,
+		iwl_write32(priv->trans, CSR_UCODE_DRV_GP1_SET,
 			    CSR_UCODE_DRV_GP1_BIT_CMD_BLOCKED);
 
-		iwl_write_direct32(trans(priv), HBUS_TARG_MBX_C,
+		iwl_write_direct32(priv->trans, HBUS_TARG_MBX_C,
 					HBUS_TARG_MBX_C_REG_BIT_CMD_BLOCKED);
 
 		if (!(flags & RXON_CARD_DISABLED)) {
-			iwl_write32(trans(priv), CSR_UCODE_DRV_GP1_CLR,
+			iwl_write32(priv->trans, CSR_UCODE_DRV_GP1_CLR,
 				    CSR_UCODE_DRV_GP1_BIT_CMD_BLOCKED);
-			iwl_write_direct32(trans(priv), HBUS_TARG_MBX_C,
+			iwl_write_direct32(priv->trans, HBUS_TARG_MBX_C,
 					HBUS_TARG_MBX_C_REG_BIT_CMD_BLOCKED);
 		}
 		if (flags & CT_CARD_DISABLED)
@@ -671,7 +635,7 @@ static int iwlagn_rx_card_state_notif(struct iwl_priv *priv,
 		wiphy_rfkill_set_hw_state(priv->hw->wiphy,
 			test_bit(STATUS_RF_KILL_HW, &priv->status));
 	else
-		wake_up(&trans(priv)->wait_command_queue);
+		wake_up(&priv->trans->wait_command_queue);
 	return 0;
 }
 
@@ -773,8 +737,7 @@ static void iwlagn_pass_packet_to_mac80211(struct iwl_priv *priv,
 	struct sk_buff *skb;
 	__le16 fc = hdr->frame_control;
 	struct iwl_rxon_context *ctx;
-	struct page *p;
-	int offset;
+	unsigned int hdrlen, fraglen;
 
 	/* We only process data packets if the interface is open */
 	if (unlikely(!priv->is_open)) {
@@ -784,21 +747,34 @@ static void iwlagn_pass_packet_to_mac80211(struct iwl_priv *priv,
 	}
 
 	/* In case of HW accelerated crypto and bad decryption, drop */
-	if (!iwlagn_mod_params.sw_crypto &&
+	if (!iwlwifi_mod_params.sw_crypto &&
 	    iwlagn_set_decrypted_flag(priv, hdr, ampdu_status, stats))
 		return;
 
-	skb = dev_alloc_skb(128);
+	/* Dont use dev_alloc_skb(), we'll have enough headroom once
+	 * ieee80211_hdr pulled.
+	 */
+	skb = alloc_skb(128, GFP_ATOMIC);
 	if (!skb) {
-		IWL_ERR(priv, "dev_alloc_skb failed\n");
+		IWL_ERR(priv, "alloc_skb failed\n");
 		return;
 	}
+	/* If frame is small enough to fit in skb->head, pull it completely.
+	 * If not, only pull ieee80211_hdr so that splice() or TCP coalesce
+	 * are more efficient.
+	 */
+	hdrlen = (len <= skb_tailroom(skb)) ? len : sizeof(*hdr);
+
+	memcpy(skb_put(skb, hdrlen), hdr, hdrlen);
+	fraglen = len - hdrlen;
 
-	offset = (void *)hdr - rxb_addr(rxb);
-	p = rxb_steal_page(rxb);
-	skb_add_rx_frag(skb, 0, p, offset, len, len);
+	if (fraglen) {
+		int offset = (void *)hdr + hdrlen -
+			     rxb_addr(rxb) + rxb_offset(rxb);
 
-	iwl_update_stats(priv, false, fc, len);
+		skb_add_rx_frag(skb, 0, rxb_steal_page(rxb), offset,
+				fraglen, rxb->truesize);
+	}
 
 	/*
 	* Wake any queues that were stopped due to a passive channel tx
@@ -809,8 +785,8 @@ static void iwlagn_pass_packet_to_mac80211(struct iwl_priv *priv,
 	*/
 	if (unlikely(ieee80211_is_beacon(fc) && priv->passive_no_rx)) {
 		for_each_context(priv, ctx) {
-			if (compare_ether_addr(hdr->addr3,
-					       ctx->active.bssid_addr))
+			if (!ether_addr_equal(hdr->addr3,
+					      ctx->active.bssid_addr))
 				continue;
 			iwlagn_lift_passive_no_rx(priv);
 		}
@@ -970,7 +946,7 @@ static int iwlagn_rx_reply_rx(struct iwl_priv *priv,
 	}
 
 	if ((unlikely(phy_res->cfg_phy_cnt > 20))) {
-		IWL_DEBUG_DROP(priv, "dsp size out of range [0,20]: %d/n",
+		IWL_DEBUG_DROP(priv, "dsp size out of range [0,20]: %d\n",
 				phy_res->cfg_phy_cnt);
 		return 0;
 	}
@@ -1005,7 +981,6 @@ static int iwlagn_rx_reply_rx(struct iwl_priv *priv,
 	/* Find max signal strength (dBm) among 3 antenna/receiver chains */
 	rx_status.signal = iwlagn_calc_rssi(priv, phy_res);
 
-	iwl_dbg_log_rx_data_frame(priv, len, header);
 	IWL_DEBUG_STATS_LIMIT(priv, "Rssi %d, TSF %llu\n",
 		rx_status.signal, (unsigned long long)rx_status.mactime);
 
@@ -1134,16 +1109,13 @@ void iwl_setup_rx_handlers(struct iwl_priv *priv)
 	handlers[REPLY_COMPRESSED_BA]		=
 		iwlagn_rx_reply_compressed_ba;
 
-	/* init calibration handlers */
-	priv->rx_handlers[CALIBRATION_RES_NOTIFICATION] =
-					iwlagn_rx_calib_result;
 	priv->rx_handlers[REPLY_TX] = iwlagn_rx_reply_tx;
 
 	/* set up notification wait support */
 	iwl_notification_wait_init(&priv->notif_wait);
 
 	/* Set up BT Rx handlers */
-	if (cfg(priv)->bt_params)
+	if (priv->cfg->bt_params)
 		iwlagn_bt_rx_handler_setup(priv);
 }
 
@@ -1185,9 +1157,9 @@ int iwl_rx_dispatch(struct iwl_op_mode *op_mode, struct iwl_rx_cmd_buffer *rxb,
 			err = priv->rx_handlers[pkt->hdr.cmd] (priv, rxb, cmd);
 		} else {
 			/* No handling needed */
-			IWL_DEBUG_RX(priv,
-				"No handler needed for %s, 0x%02x\n",
-				get_cmd_string(pkt->hdr.cmd), pkt->hdr.cmd);
+			IWL_DEBUG_RX(priv, "No handler needed for %s, 0x%02x\n",
+				     iwl_dvm_get_cmd_string(pkt->hdr.cmd),
+				     pkt->hdr.cmd);
 		}
 	}
 	return err;
diff --git a/drivers/net/wireless/iwlwifi/iwl-agn-rxon.c b/drivers/net/wireless/iwlwifi/iwl-agn-rxon.c
index 2e1a31797a9..0a3aa7c8300 100644
--- a/drivers/net/wireless/iwlwifi/iwl-agn-rxon.c
+++ b/drivers/net/wireless/iwlwifi/iwl-agn-rxon.c
@@ -24,12 +24,83 @@
  *
  *****************************************************************************/
 
+#include <linux/etherdevice.h>
 #include "iwl-dev.h"
 #include "iwl-agn.h"
-#include "iwl-core.h"
 #include "iwl-agn-calib.h"
 #include "iwl-trans.h"
-#include "iwl-shared.h"
+#include "iwl-modparams.h"
+
+/*
+ * initialize rxon structure with default values from eeprom
+ */
+void iwl_connection_init_rx_config(struct iwl_priv *priv,
+				   struct iwl_rxon_context *ctx)
+{
+	const struct iwl_channel_info *ch_info;
+
+	memset(&ctx->staging, 0, sizeof(ctx->staging));
+
+	if (!ctx->vif) {
+		ctx->staging.dev_type = ctx->unused_devtype;
+	} else
+	switch (ctx->vif->type) {
+	case NL80211_IFTYPE_AP:
+		ctx->staging.dev_type = ctx->ap_devtype;
+		break;
+
+	case NL80211_IFTYPE_STATION:
+		ctx->staging.dev_type = ctx->station_devtype;
+		ctx->staging.filter_flags = RXON_FILTER_ACCEPT_GRP_MSK;
+		break;
+
+	case NL80211_IFTYPE_ADHOC:
+		ctx->staging.dev_type = ctx->ibss_devtype;
+		ctx->staging.flags = RXON_FLG_SHORT_PREAMBLE_MSK;
+		ctx->staging.filter_flags = RXON_FILTER_BCON_AWARE_MSK |
+						  RXON_FILTER_ACCEPT_GRP_MSK;
+		break;
+
+	case NL80211_IFTYPE_MONITOR:
+		ctx->staging.dev_type = RXON_DEV_TYPE_SNIFFER;
+		break;
+
+	default:
+		IWL_ERR(priv, "Unsupported interface type %d\n",
+			ctx->vif->type);
+		break;
+	}
+
+#if 0
+	/* TODO:  Figure out when short_preamble would be set and cache from
+	 * that */
+	if (!hw_to_local(priv->hw)->short_preamble)
+		ctx->staging.flags &= ~RXON_FLG_SHORT_PREAMBLE_MSK;
+	else
+		ctx->staging.flags |= RXON_FLG_SHORT_PREAMBLE_MSK;
+#endif
+
+	ch_info = iwl_get_channel_info(priv, priv->band,
+				       le16_to_cpu(ctx->active.channel));
+
+	if (!ch_info)
+		ch_info = &priv->channel_info[0];
+
+	ctx->staging.channel = cpu_to_le16(ch_info->channel);
+	priv->band = ch_info->band;
+
+	iwl_set_flags_for_band(priv, ctx, priv->band, ctx->vif);
+
+	/* clear both MIX and PURE40 mode flag */
+	ctx->staging.flags &= ~(RXON_FLG_CHANNEL_MODE_MIXED |
+					RXON_FLG_CHANNEL_MODE_PURE_40);
+	if (ctx->vif)
+		memcpy(ctx->staging.node_addr, ctx->vif->addr, ETH_ALEN);
+
+	ctx->staging.ofdm_ht_single_stream_basic_rates = 0xff;
+	ctx->staging.ofdm_ht_dual_stream_basic_rates = 0xff;
+	ctx->staging.ofdm_ht_triple_stream_basic_rates = 0xff;
+}
 
 static int iwlagn_disable_bss(struct iwl_priv *priv,
 			      struct iwl_rxon_context *ctx,
@@ -59,9 +130,12 @@ static int iwlagn_disable_pan(struct iwl_priv *priv,
 	__le32 old_filter = send->filter_flags;
 	u8 old_dev_type = send->dev_type;
 	int ret;
+	static const u8 deactivate_cmd[] = {
+		REPLY_WIPAN_DEACTIVATION_COMPLETE
+	};
 
 	iwl_init_notification_wait(&priv->notif_wait, &disable_wait,
-				   REPLY_WIPAN_DEACTIVATION_COMPLETE,
+				   deactivate_cmd, ARRAY_SIZE(deactivate_cmd),
 				   NULL, NULL);
 
 	send->filter_flags &= ~RXON_FILTER_ASSOC_MSK;
@@ -101,8 +175,7 @@ static int iwlagn_disconn_pan(struct iwl_priv *priv,
 	return ret;
 }
 
-static void iwlagn_update_qos(struct iwl_priv *priv,
-			      struct iwl_rxon_context *ctx)
+void iwlagn_update_qos(struct iwl_priv *priv, struct iwl_rxon_context *ctx)
 {
 	int ret;
 
@@ -129,8 +202,8 @@ static void iwlagn_update_qos(struct iwl_priv *priv,
 		IWL_DEBUG_QUIET_RFKILL(priv, "Failed to update QoS\n");
 }
 
-static int iwlagn_update_beacon(struct iwl_priv *priv,
-				struct ieee80211_vif *vif)
+int iwlagn_update_beacon(struct iwl_priv *priv,
+			 struct ieee80211_vif *vif)
 {
 	lockdep_assert_held(&priv->mutex);
 
@@ -186,6 +259,109 @@ static int iwlagn_send_rxon_assoc(struct iwl_priv *priv,
 	return ret;
 }
 
+static u16 iwl_adjust_beacon_interval(u16 beacon_val, u16 max_beacon_val)
+{
+	u16 new_val;
+	u16 beacon_factor;
+
+	/*
+	 * If mac80211 hasn't given us a beacon interval, program
+	 * the default into the device (not checking this here
+	 * would cause the adjustment below to return the maximum
+	 * value, which may break PAN.)
+	 */
+	if (!beacon_val)
+		return DEFAULT_BEACON_INTERVAL;
+
+	/*
+	 * If the beacon interval we obtained from the peer
+	 * is too large, we'll have to wake up more often
+	 * (and in IBSS case, we'll beacon too much)
+	 *
+	 * For example, if max_beacon_val is 4096, and the
+	 * requested beacon interval is 7000, we'll have to
+	 * use 3500 to be able to wake up on the beacons.
+	 *
+	 * This could badly influence beacon detection stats.
+	 */
+
+	beacon_factor = (beacon_val + max_beacon_val) / max_beacon_val;
+	new_val = beacon_val / beacon_factor;
+
+	if (!new_val)
+		new_val = max_beacon_val;
+
+	return new_val;
+}
+
+static int iwl_send_rxon_timing(struct iwl_priv *priv,
+				struct iwl_rxon_context *ctx)
+{
+	u64 tsf;
+	s32 interval_tm, rem;
+	struct ieee80211_conf *conf = NULL;
+	u16 beacon_int;
+	struct ieee80211_vif *vif = ctx->vif;
+
+	conf = &priv->hw->conf;
+
+	lockdep_assert_held(&priv->mutex);
+
+	memset(&ctx->timing, 0, sizeof(struct iwl_rxon_time_cmd));
+
+	ctx->timing.timestamp = cpu_to_le64(priv->timestamp);
+	ctx->timing.listen_interval = cpu_to_le16(conf->listen_interval);
+
+	beacon_int = vif ? vif->bss_conf.beacon_int : 0;
+
+	/*
+	 * TODO: For IBSS we need to get atim_window from mac80211,
+	 *	 for now just always use 0
+	 */
+	ctx->timing.atim_window = 0;
+
+	if (ctx->ctxid == IWL_RXON_CTX_PAN &&
+	    (!ctx->vif || ctx->vif->type != NL80211_IFTYPE_STATION) &&
+	    iwl_is_associated(priv, IWL_RXON_CTX_BSS) &&
+	    priv->contexts[IWL_RXON_CTX_BSS].vif &&
+	    priv->contexts[IWL_RXON_CTX_BSS].vif->bss_conf.beacon_int) {
+		ctx->timing.beacon_interval =
+			priv->contexts[IWL_RXON_CTX_BSS].timing.beacon_interval;
+		beacon_int = le16_to_cpu(ctx->timing.beacon_interval);
+	} else if (ctx->ctxid == IWL_RXON_CTX_BSS &&
+		   iwl_is_associated(priv, IWL_RXON_CTX_PAN) &&
+		   priv->contexts[IWL_RXON_CTX_PAN].vif &&
+		   priv->contexts[IWL_RXON_CTX_PAN].vif->bss_conf.beacon_int &&
+		   (!iwl_is_associated_ctx(ctx) || !ctx->vif ||
+		    !ctx->vif->bss_conf.beacon_int)) {
+		ctx->timing.beacon_interval =
+			priv->contexts[IWL_RXON_CTX_PAN].timing.beacon_interval;
+		beacon_int = le16_to_cpu(ctx->timing.beacon_interval);
+	} else {
+		beacon_int = iwl_adjust_beacon_interval(beacon_int,
+			IWL_MAX_UCODE_BEACON_INTERVAL * TIME_UNIT);
+		ctx->timing.beacon_interval = cpu_to_le16(beacon_int);
+	}
+
+	ctx->beacon_int = beacon_int;
+
+	tsf = priv->timestamp; /* tsf is modifed by do_div: copy it */
+	interval_tm = beacon_int * TIME_UNIT;
+	rem = do_div(tsf, interval_tm);
+	ctx->timing.beacon_init_val = cpu_to_le32(interval_tm - rem);
+
+	ctx->timing.dtim_period = vif ? (vif->bss_conf.dtim_period ?: 1) : 1;
+
+	IWL_DEBUG_ASSOC(priv,
+			"beacon interval %d beacon timer %d beacon tim %d\n",
+			le16_to_cpu(ctx->timing.beacon_interval),
+			le32_to_cpu(ctx->timing.beacon_init_val),
+			le16_to_cpu(ctx->timing.atim_window));
+
+	return iwl_dvm_send_cmd_pdu(priv, ctx->rxon_timing_cmd,
+				CMD_SYNC, sizeof(ctx->timing), &ctx->timing);
+}
+
 static int iwlagn_rxon_disconn(struct iwl_priv *priv,
 			       struct iwl_rxon_context *ctx)
 {
@@ -228,6 +404,64 @@ static int iwlagn_rxon_disconn(struct iwl_priv *priv,
 	return 0;
 }
 
+static int iwl_set_tx_power(struct iwl_priv *priv, s8 tx_power, bool force)
+{
+	int ret;
+	s8 prev_tx_power;
+	bool defer;
+	struct iwl_rxon_context *ctx = &priv->contexts[IWL_RXON_CTX_BSS];
+
+	if (priv->calib_disabled & IWL_TX_POWER_CALIB_DISABLED)
+		return 0;
+
+	lockdep_assert_held(&priv->mutex);
+
+	if (priv->tx_power_user_lmt == tx_power && !force)
+		return 0;
+
+	if (tx_power < IWLAGN_TX_POWER_TARGET_POWER_MIN) {
+		IWL_WARN(priv,
+			 "Requested user TXPOWER %d below lower limit %d.\n",
+			 tx_power,
+			 IWLAGN_TX_POWER_TARGET_POWER_MIN);
+		return -EINVAL;
+	}
+
+	if (tx_power > priv->tx_power_device_lmt) {
+		IWL_WARN(priv,
+			"Requested user TXPOWER %d above upper limit %d.\n",
+			 tx_power, priv->tx_power_device_lmt);
+		return -EINVAL;
+	}
+
+	if (!iwl_is_ready_rf(priv))
+		return -EIO;
+
+	/* scan complete and commit_rxon use tx_power_next value,
+	 * it always need to be updated for newest request */
+	priv->tx_power_next = tx_power;
+
+	/* do not set tx power when scanning or channel changing */
+	defer = test_bit(STATUS_SCANNING, &priv->status) ||
+		memcmp(&ctx->active, &ctx->staging, sizeof(ctx->staging));
+	if (defer && !force) {
+		IWL_DEBUG_INFO(priv, "Deferring tx power set\n");
+		return 0;
+	}
+
+	prev_tx_power = priv->tx_power_user_lmt;
+	priv->tx_power_user_lmt = tx_power;
+
+	ret = iwlagn_send_tx_power(priv);
+
+	/* if fail to set tx_power, restore the orig. tx power */
+	if (ret) {
+		priv->tx_power_user_lmt = prev_tx_power;
+		priv->tx_power_next = prev_tx_power;
+	}
+	return ret;
+}
+
 static int iwlagn_rxon_connect(struct iwl_priv *priv,
 			       struct iwl_rxon_context *ctx)
 {
@@ -295,9 +529,9 @@ static int iwlagn_rxon_connect(struct iwl_priv *priv,
 	}
 
 	if (ctx->vif && ctx->vif->type == NL80211_IFTYPE_STATION &&
-	    cfg(priv)->ht_params && cfg(priv)->ht_params->smps_mode)
+	    priv->cfg->ht_params && priv->cfg->ht_params->smps_mode)
 		ieee80211_request_smps(ctx->vif,
-				       cfg(priv)->ht_params->smps_mode);
+				       priv->cfg->ht_params->smps_mode);
 
 	return 0;
 }
@@ -309,7 +543,7 @@ int iwlagn_set_pan_params(struct iwl_priv *priv)
 	int slot0 = 300, slot1 = 0;
 	int ret;
 
-	if (priv->shrd->valid_contexts == BIT(IWL_RXON_CTX_BSS))
+	if (priv->valid_contexts == BIT(IWL_RXON_CTX_BSS))
 		return 0;
 
 	BUILD_BUG_ON(NUM_IWL_RXON_CTX != 2);
@@ -394,6 +628,414 @@ int iwlagn_set_pan_params(struct iwl_priv *priv)
 	return ret;
 }
 
+static void _iwl_set_rxon_ht(struct iwl_priv *priv,
+			     struct iwl_ht_config *ht_conf,
+			     struct iwl_rxon_context *ctx)
+{
+	struct iwl_rxon_cmd *rxon = &ctx->staging;
+
+	if (!ctx->ht.enabled) {
+		rxon->flags &= ~(RXON_FLG_CHANNEL_MODE_MSK |
+			RXON_FLG_CTRL_CHANNEL_LOC_HI_MSK |
+			RXON_FLG_HT40_PROT_MSK |
+			RXON_FLG_HT_PROT_MSK);
+		return;
+	}
+
+	/* FIXME: if the definition of ht.protection changed, the "translation"
+	 * will be needed for rxon->flags
+	 */
+	rxon->flags |= cpu_to_le32(ctx->ht.protection <<
+				   RXON_FLG_HT_OPERATING_MODE_POS);
+
+	/* Set up channel bandwidth:
+	 * 20 MHz only, 20/40 mixed or pure 40 if ht40 ok */
+	/* clear the HT channel mode before set the mode */
+	rxon->flags &= ~(RXON_FLG_CHANNEL_MODE_MSK |
+			 RXON_FLG_CTRL_CHANNEL_LOC_HI_MSK);
+	if (iwl_is_ht40_tx_allowed(priv, ctx, NULL)) {
+		/* pure ht40 */
+		if (ctx->ht.protection ==
+		    IEEE80211_HT_OP_MODE_PROTECTION_20MHZ) {
+			rxon->flags |= RXON_FLG_CHANNEL_MODE_PURE_40;
+			/*
+			 * Note: control channel is opposite of extension
+			 * channel
+			 */
+			switch (ctx->ht.extension_chan_offset) {
+			case IEEE80211_HT_PARAM_CHA_SEC_ABOVE:
+				rxon->flags &=
+					~RXON_FLG_CTRL_CHANNEL_LOC_HI_MSK;
+				break;
+			case IEEE80211_HT_PARAM_CHA_SEC_BELOW:
+				rxon->flags |=
+					RXON_FLG_CTRL_CHANNEL_LOC_HI_MSK;
+				break;
+			}
+		} else {
+			/*
+			 * Note: control channel is opposite of extension
+			 * channel
+			 */
+			switch (ctx->ht.extension_chan_offset) {
+			case IEEE80211_HT_PARAM_CHA_SEC_ABOVE:
+				rxon->flags &=
+					~(RXON_FLG_CTRL_CHANNEL_LOC_HI_MSK);
+				rxon->flags |= RXON_FLG_CHANNEL_MODE_MIXED;
+				break;
+			case IEEE80211_HT_PARAM_CHA_SEC_BELOW:
+				rxon->flags |= RXON_FLG_CTRL_CHANNEL_LOC_HI_MSK;
+				rxon->flags |= RXON_FLG_CHANNEL_MODE_MIXED;
+				break;
+			case IEEE80211_HT_PARAM_CHA_SEC_NONE:
+			default:
+				/*
+				 * channel location only valid if in Mixed
+				 * mode
+				 */
+				IWL_ERR(priv,
+					"invalid extension channel offset\n");
+				break;
+			}
+		}
+	} else {
+		rxon->flags |= RXON_FLG_CHANNEL_MODE_LEGACY;
+	}
+
+	iwlagn_set_rxon_chain(priv, ctx);
+
+	IWL_DEBUG_ASSOC(priv, "rxon flags 0x%X operation mode :0x%X "
+			"extension channel offset 0x%x\n",
+			le32_to_cpu(rxon->flags), ctx->ht.protection,
+			ctx->ht.extension_chan_offset);
+}
+
+void iwl_set_rxon_ht(struct iwl_priv *priv, struct iwl_ht_config *ht_conf)
+{
+	struct iwl_rxon_context *ctx;
+
+	for_each_context(priv, ctx)
+		_iwl_set_rxon_ht(priv, ht_conf, ctx);
+}
+
+/**
+ * iwl_set_rxon_channel - Set the band and channel values in staging RXON
+ * @ch: requested channel as a pointer to struct ieee80211_channel
+
+ * NOTE:  Does not commit to the hardware; it sets appropriate bit fields
+ * in the staging RXON flag structure based on the ch->band
+ */
+void iwl_set_rxon_channel(struct iwl_priv *priv, struct ieee80211_channel *ch,
+			 struct iwl_rxon_context *ctx)
+{
+	enum ieee80211_band band = ch->band;
+	u16 channel = ch->hw_value;
+
+	if ((le16_to_cpu(ctx->staging.channel) == channel) &&
+	    (priv->band == band))
+		return;
+
+	ctx->staging.channel = cpu_to_le16(channel);
+	if (band == IEEE80211_BAND_5GHZ)
+		ctx->staging.flags &= ~RXON_FLG_BAND_24G_MSK;
+	else
+		ctx->staging.flags |= RXON_FLG_BAND_24G_MSK;
+
+	priv->band = band;
+
+	IWL_DEBUG_INFO(priv, "Staging channel set to %d [%d]\n", channel, band);
+
+}
+
+void iwl_set_flags_for_band(struct iwl_priv *priv,
+			    struct iwl_rxon_context *ctx,
+			    enum ieee80211_band band,
+			    struct ieee80211_vif *vif)
+{
+	if (band == IEEE80211_BAND_5GHZ) {
+		ctx->staging.flags &=
+		    ~(RXON_FLG_BAND_24G_MSK | RXON_FLG_AUTO_DETECT_MSK
+		      | RXON_FLG_CCK_MSK);
+		ctx->staging.flags |= RXON_FLG_SHORT_SLOT_MSK;
+	} else {
+		/* Copied from iwl_post_associate() */
+		if (vif && vif->bss_conf.use_short_slot)
+			ctx->staging.flags |= RXON_FLG_SHORT_SLOT_MSK;
+		else
+			ctx->staging.flags &= ~RXON_FLG_SHORT_SLOT_MSK;
+
+		ctx->staging.flags |= RXON_FLG_BAND_24G_MSK;
+		ctx->staging.flags |= RXON_FLG_AUTO_DETECT_MSK;
+		ctx->staging.flags &= ~RXON_FLG_CCK_MSK;
+	}
+}
+
+static void iwl_set_rxon_hwcrypto(struct iwl_priv *priv,
+				  struct iwl_rxon_context *ctx, int hw_decrypt)
+{
+	struct iwl_rxon_cmd *rxon = &ctx->staging;
+
+	if (hw_decrypt)
+		rxon->filter_flags &= ~RXON_FILTER_DIS_DECRYPT_MSK;
+	else
+		rxon->filter_flags |= RXON_FILTER_DIS_DECRYPT_MSK;
+
+}
+
+/* validate RXON structure is valid */
+static int iwl_check_rxon_cmd(struct iwl_priv *priv,
+			      struct iwl_rxon_context *ctx)
+{
+	struct iwl_rxon_cmd *rxon = &ctx->staging;
+	u32 errors = 0;
+
+	if (rxon->flags & RXON_FLG_BAND_24G_MSK) {
+		if (rxon->flags & RXON_FLG_TGJ_NARROW_BAND_MSK) {
+			IWL_WARN(priv, "check 2.4G: wrong narrow\n");
+			errors |= BIT(0);
+		}
+		if (rxon->flags & RXON_FLG_RADAR_DETECT_MSK) {
+			IWL_WARN(priv, "check 2.4G: wrong radar\n");
+			errors |= BIT(1);
+		}
+	} else {
+		if (!(rxon->flags & RXON_FLG_SHORT_SLOT_MSK)) {
+			IWL_WARN(priv, "check 5.2G: not short slot!\n");
+			errors |= BIT(2);
+		}
+		if (rxon->flags & RXON_FLG_CCK_MSK) {
+			IWL_WARN(priv, "check 5.2G: CCK!\n");
+			errors |= BIT(3);
+		}
+	}
+	if ((rxon->node_addr[0] | rxon->bssid_addr[0]) & 0x1) {
+		IWL_WARN(priv, "mac/bssid mcast!\n");
+		errors |= BIT(4);
+	}
+
+	/* make sure basic rates 6Mbps and 1Mbps are supported */
+	if ((rxon->ofdm_basic_rates & IWL_RATE_6M_MASK) == 0 &&
+	    (rxon->cck_basic_rates & IWL_RATE_1M_MASK) == 0) {
+		IWL_WARN(priv, "neither 1 nor 6 are basic\n");
+		errors |= BIT(5);
+	}
+
+	if (le16_to_cpu(rxon->assoc_id) > 2007) {
+		IWL_WARN(priv, "aid > 2007\n");
+		errors |= BIT(6);
+	}
+
+	if ((rxon->flags & (RXON_FLG_CCK_MSK | RXON_FLG_SHORT_SLOT_MSK))
+			== (RXON_FLG_CCK_MSK | RXON_FLG_SHORT_SLOT_MSK)) {
+		IWL_WARN(priv, "CCK and short slot\n");
+		errors |= BIT(7);
+	}
+
+	if ((rxon->flags & (RXON_FLG_CCK_MSK | RXON_FLG_AUTO_DETECT_MSK))
+			== (RXON_FLG_CCK_MSK | RXON_FLG_AUTO_DETECT_MSK)) {
+		IWL_WARN(priv, "CCK and auto detect");
+		errors |= BIT(8);
+	}
+
+	if ((rxon->flags & (RXON_FLG_AUTO_DETECT_MSK |
+			    RXON_FLG_TGG_PROTECT_MSK)) ==
+			    RXON_FLG_TGG_PROTECT_MSK) {
+		IWL_WARN(priv, "TGg but no auto-detect\n");
+		errors |= BIT(9);
+	}
+
+	if (rxon->channel == 0) {
+		IWL_WARN(priv, "zero channel is invalid\n");
+		errors |= BIT(10);
+	}
+
+	WARN(errors, "Invalid RXON (%#x), channel %d",
+	     errors, le16_to_cpu(rxon->channel));
+
+	return errors ? -EINVAL : 0;
+}
+
+/**
+ * iwl_full_rxon_required - check if full RXON (vs RXON_ASSOC) cmd is needed
+ * @priv: staging_rxon is compared to active_rxon
+ *
+ * If the RXON structure is changing enough to require a new tune,
+ * or is clearing the RXON_FILTER_ASSOC_MSK, then return 1 to indicate that
+ * a new tune (full RXON command, rather than RXON_ASSOC cmd) is required.
+ */
+int iwl_full_rxon_required(struct iwl_priv *priv,
+			   struct iwl_rxon_context *ctx)
+{
+	const struct iwl_rxon_cmd *staging = &ctx->staging;
+	const struct iwl_rxon_cmd *active = &ctx->active;
+
+#define CHK(cond)							\
+	if ((cond)) {							\
+		IWL_DEBUG_INFO(priv, "need full RXON - " #cond "\n");	\
+		return 1;						\
+	}
+
+#define CHK_NEQ(c1, c2)						\
+	if ((c1) != (c2)) {					\
+		IWL_DEBUG_INFO(priv, "need full RXON - "	\
+			       #c1 " != " #c2 " - %d != %d\n",	\
+			       (c1), (c2));			\
+		return 1;					\
+	}
+
+	/* These items are only settable from the full RXON command */
+	CHK(!iwl_is_associated_ctx(ctx));
+	CHK(!ether_addr_equal(staging->bssid_addr, active->bssid_addr));
+	CHK(!ether_addr_equal(staging->node_addr, active->node_addr));
+	CHK(!ether_addr_equal(staging->wlap_bssid_addr,
+			      active->wlap_bssid_addr));
+	CHK_NEQ(staging->dev_type, active->dev_type);
+	CHK_NEQ(staging->channel, active->channel);
+	CHK_NEQ(staging->air_propagation, active->air_propagation);
+	CHK_NEQ(staging->ofdm_ht_single_stream_basic_rates,
+		active->ofdm_ht_single_stream_basic_rates);
+	CHK_NEQ(staging->ofdm_ht_dual_stream_basic_rates,
+		active->ofdm_ht_dual_stream_basic_rates);
+	CHK_NEQ(staging->ofdm_ht_triple_stream_basic_rates,
+		active->ofdm_ht_triple_stream_basic_rates);
+	CHK_NEQ(staging->assoc_id, active->assoc_id);
+
+	/* flags, filter_flags, ofdm_basic_rates, and cck_basic_rates can
+	 * be updated with the RXON_ASSOC command -- however only some
+	 * flag transitions are allowed using RXON_ASSOC */
+
+	/* Check if we are not switching bands */
+	CHK_NEQ(staging->flags & RXON_FLG_BAND_24G_MSK,
+		active->flags & RXON_FLG_BAND_24G_MSK);
+
+	/* Check if we are switching association toggle */
+	CHK_NEQ(staging->filter_flags & RXON_FILTER_ASSOC_MSK,
+		active->filter_flags & RXON_FILTER_ASSOC_MSK);
+
+#undef CHK
+#undef CHK_NEQ
+
+	return 0;
+}
+
+#ifdef CONFIG_IWLWIFI_DEBUG
+void iwl_print_rx_config_cmd(struct iwl_priv *priv,
+			     enum iwl_rxon_context_id ctxid)
+{
+	struct iwl_rxon_context *ctx = &priv->contexts[ctxid];
+	struct iwl_rxon_cmd *rxon = &ctx->staging;
+
+	IWL_DEBUG_RADIO(priv, "RX CONFIG:\n");
+	iwl_print_hex_dump(priv, IWL_DL_RADIO, (u8 *) rxon, sizeof(*rxon));
+	IWL_DEBUG_RADIO(priv, "u16 channel: 0x%x\n",
+			le16_to_cpu(rxon->channel));
+	IWL_DEBUG_RADIO(priv, "u32 flags: 0x%08X\n",
+			le32_to_cpu(rxon->flags));
+	IWL_DEBUG_RADIO(priv, "u32 filter_flags: 0x%08x\n",
+			le32_to_cpu(rxon->filter_flags));
+	IWL_DEBUG_RADIO(priv, "u8 dev_type: 0x%x\n", rxon->dev_type);
+	IWL_DEBUG_RADIO(priv, "u8 ofdm_basic_rates: 0x%02x\n",
+			rxon->ofdm_basic_rates);
+	IWL_DEBUG_RADIO(priv, "u8 cck_basic_rates: 0x%02x\n",
+			rxon->cck_basic_rates);
+	IWL_DEBUG_RADIO(priv, "u8[6] node_addr: %pM\n", rxon->node_addr);
+	IWL_DEBUG_RADIO(priv, "u8[6] bssid_addr: %pM\n", rxon->bssid_addr);
+	IWL_DEBUG_RADIO(priv, "u16 assoc_id: 0x%x\n",
+			le16_to_cpu(rxon->assoc_id));
+}
+#endif
+
+static void iwl_calc_basic_rates(struct iwl_priv *priv,
+				 struct iwl_rxon_context *ctx)
+{
+	int lowest_present_ofdm = 100;
+	int lowest_present_cck = 100;
+	u8 cck = 0;
+	u8 ofdm = 0;
+
+	if (ctx->vif) {
+		struct ieee80211_supported_band *sband;
+		unsigned long basic = ctx->vif->bss_conf.basic_rates;
+		int i;
+
+		sband = priv->hw->wiphy->bands[priv->hw->conf.channel->band];
+
+		for_each_set_bit(i, &basic, BITS_PER_LONG) {
+			int hw = sband->bitrates[i].hw_value;
+			if (hw >= IWL_FIRST_OFDM_RATE) {
+				ofdm |= BIT(hw - IWL_FIRST_OFDM_RATE);
+				if (lowest_present_ofdm > hw)
+					lowest_present_ofdm = hw;
+			} else {
+				BUILD_BUG_ON(IWL_FIRST_CCK_RATE != 0);
+
+				cck |= BIT(hw);
+				if (lowest_present_cck > hw)
+					lowest_present_cck = hw;
+			}
+		}
+	}
+
+	/*
+	 * Now we've got the basic rates as bitmaps in the ofdm and cck
+	 * variables. This isn't sufficient though, as there might not
+	 * be all the right rates in the bitmap. E.g. if the only basic
+	 * rates are 5.5 Mbps and 11 Mbps, we still need to add 1 Mbps
+	 * and 6 Mbps because the 802.11-2007 standard says in 9.6:
+	 *
+	 *    [...] a STA responding to a received frame shall transmit
+	 *    its Control Response frame [...] at the highest rate in the
+	 *    BSSBasicRateSet parameter that is less than or equal to the
+	 *    rate of the immediately previous frame in the frame exchange
+	 *    sequence ([...]) and that is of the same modulation class
+	 *    ([...]) as the received frame. If no rate contained in the
+	 *    BSSBasicRateSet parameter meets these conditions, then the
+	 *    control frame sent in response to a received frame shall be
+	 *    transmitted at the highest mandatory rate of the PHY that is
+	 *    less than or equal to the rate of the received frame, and
+	 *    that is of the same modulation class as the received frame.
+	 *
+	 * As a consequence, we need to add all mandatory rates that are
+	 * lower than all of the basic rates to these bitmaps.
+	 */
+
+	if (IWL_RATE_24M_INDEX < lowest_present_ofdm)
+		ofdm |= IWL_RATE_24M_MASK >> IWL_FIRST_OFDM_RATE;
+	if (IWL_RATE_12M_INDEX < lowest_present_ofdm)
+		ofdm |= IWL_RATE_12M_MASK >> IWL_FIRST_OFDM_RATE;
+	/* 6M already there or needed so always add */
+	ofdm |= IWL_RATE_6M_MASK >> IWL_FIRST_OFDM_RATE;
+
+	/*
+	 * CCK is a bit more complex with DSSS vs. HR/DSSS vs. ERP.
+	 * Note, however:
+	 *  - if no CCK rates are basic, it must be ERP since there must
+	 *    be some basic rates at all, so they're OFDM => ERP PHY
+	 *    (or we're in 5 GHz, and the cck bitmap will never be used)
+	 *  - if 11M is a basic rate, it must be ERP as well, so add 5.5M
+	 *  - if 5.5M is basic, 1M and 2M are mandatory
+	 *  - if 2M is basic, 1M is mandatory
+	 *  - if 1M is basic, that's the only valid ACK rate.
+	 * As a consequence, it's not as complicated as it sounds, just add
+	 * any lower rates to the ACK rate bitmap.
+	 */
+	if (IWL_RATE_11M_INDEX < lowest_present_ofdm)
+		ofdm |= IWL_RATE_11M_MASK >> IWL_FIRST_CCK_RATE;
+	if (IWL_RATE_5M_INDEX < lowest_present_ofdm)
+		ofdm |= IWL_RATE_5M_MASK >> IWL_FIRST_CCK_RATE;
+	if (IWL_RATE_2M_INDEX < lowest_present_ofdm)
+		ofdm |= IWL_RATE_2M_MASK >> IWL_FIRST_CCK_RATE;
+	/* 1M already there or needed so always add */
+	cck |= IWL_RATE_1M_MASK >> IWL_FIRST_CCK_RATE;
+
+	IWL_DEBUG_RATE(priv, "Set basic rates cck:0x%.2x ofdm:0x%.2x\n",
+		       cck, ofdm);
+
+	/* "basic_rates" is a misnomer here -- should be called ACK rates */
+	ctx->staging.cck_basic_rates = cck;
+	ctx->staging.ofdm_basic_rates = ofdm;
+}
+
 /**
  * iwlagn_commit_rxon - commit staging_rxon to hardware
  *
@@ -433,11 +1075,14 @@ int iwlagn_commit_rxon(struct iwl_priv *priv, struct iwl_rxon_context *ctx)
 	/* always get timestamp with Rx frame */
 	ctx->staging.flags |= RXON_FLG_TSF2HOST_MSK;
 
+	/* recalculate basic rates */
+	iwl_calc_basic_rates(priv, ctx);
+
 	/*
 	 * force CTS-to-self frames protection if RTS-CTS is not preferred
 	 * one aggregation protection method
 	 */
-	if (!hw_params(priv).use_rts_for_aggregation)
+	if (!priv->hw_params.use_rts_for_aggregation)
 		ctx->staging.flags |= RXON_FLG_SELF_CTS_EN;
 
 	if ((ctx->vif && ctx->vif->bss_conf.use_short_slot) ||
@@ -489,7 +1134,7 @@ int iwlagn_commit_rxon(struct iwl_priv *priv, struct iwl_rxon_context *ctx)
 		return 0;
 	}
 
-	iwl_set_rxon_hwcrypto(priv, ctx, !iwlagn_mod_params.sw_crypto);
+	iwl_set_rxon_hwcrypto(priv, ctx, !iwlwifi_mod_params.sw_crypto);
 
 	IWL_DEBUG_INFO(priv,
 		       "Going to commit RXON\n"
@@ -547,7 +1192,7 @@ int iwlagn_mac_config(struct ieee80211_hw *hw, u32 changed)
 	const struct iwl_channel_info *ch_info;
 	int ret = 0;
 
-	IWL_DEBUG_MAC80211(priv, "enter: changed %#x", changed);
+	IWL_DEBUG_MAC80211(priv, "enter: changed %#x\n", changed);
 
 	mutex_lock(&priv->mutex);
 
@@ -621,13 +1266,6 @@ int iwlagn_mac_config(struct ieee80211_hw *hw, u32 changed)
 		}
 
 		iwl_update_bcast_stations(priv);
-
-		/*
-		 * The list of supported rates and rate mask can be different
-		 * for each band; since the band may have changed, reset
-		 * the rate mask to what mac80211 lists.
-		 */
-		iwl_set_rate(priv);
 	}
 
 	if (changed & (IEEE80211_CONF_CHANGE_PS |
@@ -656,9 +1294,9 @@ int iwlagn_mac_config(struct ieee80211_hw *hw, u32 changed)
 	return ret;
 }
 
-static void iwlagn_check_needed_chains(struct iwl_priv *priv,
-				       struct iwl_rxon_context *ctx,
-				       struct ieee80211_bss_conf *bss_conf)
+void iwlagn_check_needed_chains(struct iwl_priv *priv,
+				struct iwl_rxon_context *ctx,
+				struct ieee80211_bss_conf *bss_conf)
 {
 	struct ieee80211_vif *vif = ctx->vif;
 	struct iwl_rxon_context *tmp;
@@ -750,11 +1388,14 @@ static void iwlagn_check_needed_chains(struct iwl_priv *priv,
 	ht_conf->single_chain_sufficient = !need_multiple;
 }
 
-static void iwlagn_chain_noise_reset(struct iwl_priv *priv)
+void iwlagn_chain_noise_reset(struct iwl_priv *priv)
 {
 	struct iwl_chain_noise_data *data = &priv->chain_noise_data;
 	int ret;
 
+	if (!(priv->calib_disabled & IWL_CHAIN_NOISE_CALIB_DISABLED))
+		return;
+
 	if ((data->state == IWL_CHAIN_NOISE_ALIVE) &&
 	    iwl_is_any_associated(priv)) {
 		struct iwl_calib_chain_noise_reset_cmd cmd;
@@ -907,8 +1548,7 @@ void iwlagn_bss_info_changed(struct ieee80211_hw *hw,
 			iwl_power_update_mode(priv, false);
 
 		/* Enable RX differential gain and sensitivity calibrations */
-		if (!priv->disable_chain_noise_cal)
-			iwlagn_chain_noise_reset(priv);
+		iwlagn_chain_noise_reset(priv);
 		priv->start_calib = 1;
 	}
 
diff --git a/drivers/net/wireless/iwlwifi/iwl-agn-sta.c b/drivers/net/wireless/iwlwifi/iwl-agn-sta.c
index c4175603864..b31584e87bc 100644
--- a/drivers/net/wireless/iwlwifi/iwl-agn-sta.c
+++ b/drivers/net/wireless/iwlwifi/iwl-agn-sta.c
@@ -30,10 +30,11 @@
 #include <net/mac80211.h>
 
 #include "iwl-dev.h"
-#include "iwl-core.h"
 #include "iwl-agn.h"
 #include "iwl-trans.h"
 
+const u8 iwl_bcast_addr[ETH_ALEN] = { 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF };
+
 static int iwl_sta_ucode_activate(struct iwl_priv *priv, u8 sta_id)
 {
 	lockdep_assert_held(&priv->sta_lock);
@@ -170,6 +171,50 @@ int iwl_send_add_sta(struct iwl_priv *priv,
 	return cmd.handler_status;
 }
 
+static bool iwl_is_channel_extension(struct iwl_priv *priv,
+				     enum ieee80211_band band,
+				     u16 channel, u8 extension_chan_offset)
+{
+	const struct iwl_channel_info *ch_info;
+
+	ch_info = iwl_get_channel_info(priv, band, channel);
+	if (!is_channel_valid(ch_info))
+		return false;
+
+	if (extension_chan_offset == IEEE80211_HT_PARAM_CHA_SEC_ABOVE)
+		return !(ch_info->ht40_extension_channel &
+					IEEE80211_CHAN_NO_HT40PLUS);
+	else if (extension_chan_offset == IEEE80211_HT_PARAM_CHA_SEC_BELOW)
+		return !(ch_info->ht40_extension_channel &
+					IEEE80211_CHAN_NO_HT40MINUS);
+
+	return false;
+}
+
+bool iwl_is_ht40_tx_allowed(struct iwl_priv *priv,
+			    struct iwl_rxon_context *ctx,
+			    struct ieee80211_sta_ht_cap *ht_cap)
+{
+	if (!ctx->ht.enabled || !ctx->ht.is_40mhz)
+		return false;
+
+	/*
+	 * We do not check for IEEE80211_HT_CAP_SUP_WIDTH_20_40
+	 * the bit will not set if it is pure 40MHz case
+	 */
+	if (ht_cap && !ht_cap->ht_supported)
+		return false;
+
+#ifdef CONFIG_IWLWIFI_DEBUGFS
+	if (priv->disable_ht40)
+		return false;
+#endif
+
+	return iwl_is_channel_extension(priv, priv->band,
+			le16_to_cpu(ctx->staging.channel),
+			ctx->ht.extension_chan_offset);
+}
+
 static void iwl_sta_calc_ht_flags(struct iwl_priv *priv,
 				  struct ieee80211_sta *sta,
 				  struct iwl_rxon_context *ctx,
@@ -277,8 +322,8 @@ u8 iwl_prep_station(struct iwl_priv *priv, struct iwl_rxon_context *ctx,
 		sta_id = ctx->bcast_sta_id;
 	else
 		for (i = IWL_STA_ID; i < IWLAGN_STATION_COUNT; i++) {
-			if (!compare_ether_addr(priv->stations[i].sta.sta.addr,
-						addr)) {
+			if (ether_addr_equal(priv->stations[i].sta.sta.addr,
+					     addr)) {
 				sta_id = i;
 				break;
 			}
@@ -308,7 +353,7 @@ u8 iwl_prep_station(struct iwl_priv *priv, struct iwl_rxon_context *ctx,
 
 	if ((priv->stations[sta_id].used & IWL_STA_DRIVER_ACTIVE) &&
 	    (priv->stations[sta_id].used & IWL_STA_UCODE_ACTIVE) &&
-	    !compare_ether_addr(priv->stations[sta_id].sta.sta.addr, addr)) {
+	    ether_addr_equal(priv->stations[sta_id].sta.sta.addr, addr)) {
 		IWL_DEBUG_ASSOC(priv, "STA %d (%pM) already added, not "
 				"adding again.\n", sta_id, addr);
 		return sta_id;
@@ -581,6 +626,56 @@ void iwl_deactivate_station(struct iwl_priv *priv, const u8 sta_id,
 	spin_unlock_bh(&priv->sta_lock);
 }
 
+static void iwl_sta_fill_lq(struct iwl_priv *priv, struct iwl_rxon_context *ctx,
+			    u8 sta_id, struct iwl_link_quality_cmd *link_cmd)
+{
+	int i, r;
+	u32 rate_flags = 0;
+	__le32 rate_n_flags;
+
+	lockdep_assert_held(&priv->mutex);
+
+	memset(link_cmd, 0, sizeof(*link_cmd));
+
+	/* Set up the rate scaling to start at selected rate, fall back
+	 * all the way down to 1M in IEEE order, and then spin on 1M */
+	if (priv->band == IEEE80211_BAND_5GHZ)
+		r = IWL_RATE_6M_INDEX;
+	else if (ctx && ctx->vif && ctx->vif->p2p)
+		r = IWL_RATE_6M_INDEX;
+	else
+		r = IWL_RATE_1M_INDEX;
+
+	if (r >= IWL_FIRST_CCK_RATE && r <= IWL_LAST_CCK_RATE)
+		rate_flags |= RATE_MCS_CCK_MSK;
+
+	rate_flags |= first_antenna(priv->hw_params.valid_tx_ant) <<
+				RATE_MCS_ANT_POS;
+	rate_n_flags = iwl_hw_set_rate_n_flags(iwl_rates[r].plcp, rate_flags);
+	for (i = 0; i < LINK_QUAL_MAX_RETRY_NUM; i++)
+		link_cmd->rs_table[i].rate_n_flags = rate_n_flags;
+
+	link_cmd->general_params.single_stream_ant_msk =
+			first_antenna(priv->hw_params.valid_tx_ant);
+
+	link_cmd->general_params.dual_stream_ant_msk =
+		priv->hw_params.valid_tx_ant &
+		~first_antenna(priv->hw_params.valid_tx_ant);
+	if (!link_cmd->general_params.dual_stream_ant_msk) {
+		link_cmd->general_params.dual_stream_ant_msk = ANT_AB;
+	} else if (num_of_ant(priv->hw_params.valid_tx_ant) == 2) {
+		link_cmd->general_params.dual_stream_ant_msk =
+			priv->hw_params.valid_tx_ant;
+	}
+
+	link_cmd->agg_params.agg_dis_start_th =
+		LINK_QUAL_AGG_DISABLE_START_DEF;
+	link_cmd->agg_params.agg_time_limit =
+		cpu_to_le16(LINK_QUAL_AGG_TIME_LIMIT_DEF);
+
+	link_cmd->sta_id = sta_id;
+}
+
 /**
  * iwl_clear_ucode_stations - clear ucode station table bits
  *
@@ -841,56 +936,6 @@ int iwl_send_lq_cmd(struct iwl_priv *priv, struct iwl_rxon_context *ctx,
 }
 
 
-void iwl_sta_fill_lq(struct iwl_priv *priv, struct iwl_rxon_context *ctx,
-		     u8 sta_id, struct iwl_link_quality_cmd *link_cmd)
-{
-	int i, r;
-	u32 rate_flags = 0;
-	__le32 rate_n_flags;
-
-	lockdep_assert_held(&priv->mutex);
-
-	memset(link_cmd, 0, sizeof(*link_cmd));
-
-	/* Set up the rate scaling to start at selected rate, fall back
-	 * all the way down to 1M in IEEE order, and then spin on 1M */
-	if (priv->band == IEEE80211_BAND_5GHZ)
-		r = IWL_RATE_6M_INDEX;
-	else if (ctx && ctx->vif && ctx->vif->p2p)
-		r = IWL_RATE_6M_INDEX;
-	else
-		r = IWL_RATE_1M_INDEX;
-
-	if (r >= IWL_FIRST_CCK_RATE && r <= IWL_LAST_CCK_RATE)
-		rate_flags |= RATE_MCS_CCK_MSK;
-
-	rate_flags |= first_antenna(hw_params(priv).valid_tx_ant) <<
-				RATE_MCS_ANT_POS;
-	rate_n_flags = iwl_hw_set_rate_n_flags(iwl_rates[r].plcp, rate_flags);
-	for (i = 0; i < LINK_QUAL_MAX_RETRY_NUM; i++)
-		link_cmd->rs_table[i].rate_n_flags = rate_n_flags;
-
-	link_cmd->general_params.single_stream_ant_msk =
-			first_antenna(hw_params(priv).valid_tx_ant);
-
-	link_cmd->general_params.dual_stream_ant_msk =
-		hw_params(priv).valid_tx_ant &
-		~first_antenna(hw_params(priv).valid_tx_ant);
-	if (!link_cmd->general_params.dual_stream_ant_msk) {
-		link_cmd->general_params.dual_stream_ant_msk = ANT_AB;
-	} else if (num_of_ant(hw_params(priv).valid_tx_ant) == 2) {
-		link_cmd->general_params.dual_stream_ant_msk =
-			hw_params(priv).valid_tx_ant;
-	}
-
-	link_cmd->agg_params.agg_dis_start_th =
-		LINK_QUAL_AGG_DISABLE_START_DEF;
-	link_cmd->agg_params.agg_time_limit =
-		cpu_to_le16(LINK_QUAL_AGG_TIME_LIMIT_DEF);
-
-	link_cmd->sta_id = sta_id;
-}
-
 static struct iwl_link_quality_cmd *
 iwl_sta_alloc_lq(struct iwl_priv *priv, struct iwl_rxon_context *ctx,
 		 u8 sta_id)
diff --git a/drivers/net/wireless/iwlwifi/iwl-agn-tt.c b/drivers/net/wireless/iwlwifi/iwl-agn-tt.c
index baaf5ba2fc3..a5cfe0aceed 100644
--- a/drivers/net/wireless/iwlwifi/iwl-agn-tt.c
+++ b/drivers/net/wireless/iwlwifi/iwl-agn-tt.c
@@ -37,11 +37,11 @@
 #include "iwl-agn.h"
 #include "iwl-eeprom.h"
 #include "iwl-dev.h"
-#include "iwl-core.h"
 #include "iwl-io.h"
 #include "iwl-commands.h"
 #include "iwl-debug.h"
 #include "iwl-agn-tt.h"
+#include "iwl-modparams.h"
 
 /* default Thermal Throttling transaction table
  * Current state   |         Throttling Down               |  Throttling Up
@@ -179,19 +179,19 @@ static void iwl_tt_check_exit_ct_kill(unsigned long data)
 
 	if (tt->state == IWL_TI_CT_KILL) {
 		if (priv->thermal_throttle.ct_kill_toggle) {
-			iwl_write32(trans(priv), CSR_UCODE_DRV_GP1_CLR,
+			iwl_write32(priv->trans, CSR_UCODE_DRV_GP1_CLR,
 				    CSR_UCODE_DRV_GP1_REG_BIT_CT_KILL_EXIT);
 			priv->thermal_throttle.ct_kill_toggle = false;
 		} else {
-			iwl_write32(trans(priv), CSR_UCODE_DRV_GP1_SET,
+			iwl_write32(priv->trans, CSR_UCODE_DRV_GP1_SET,
 				    CSR_UCODE_DRV_GP1_REG_BIT_CT_KILL_EXIT);
 			priv->thermal_throttle.ct_kill_toggle = true;
 		}
-		iwl_read32(trans(priv), CSR_UCODE_DRV_GP1);
-		spin_lock_irqsave(&trans(priv)->reg_lock, flags);
-		if (likely(iwl_grab_nic_access(trans(priv))))
-			iwl_release_nic_access(trans(priv));
-		spin_unlock_irqrestore(&trans(priv)->reg_lock, flags);
+		iwl_read32(priv->trans, CSR_UCODE_DRV_GP1);
+		spin_lock_irqsave(&priv->trans->reg_lock, flags);
+		if (likely(iwl_grab_nic_access(priv->trans)))
+			iwl_release_nic_access(priv->trans);
+		spin_unlock_irqrestore(&priv->trans->reg_lock, flags);
 
 		/* Reschedule the ct_kill timer to occur in
 		 * CT_KILL_EXIT_DURATION seconds to ensure we get a
@@ -632,7 +632,7 @@ void iwl_tt_initialize(struct iwl_priv *priv)
 	INIT_WORK(&priv->ct_enter, iwl_bg_ct_enter);
 	INIT_WORK(&priv->ct_exit, iwl_bg_ct_exit);
 
-	if (cfg(priv)->base_params->adv_thermal_throttle) {
+	if (priv->cfg->base_params->adv_thermal_throttle) {
 		IWL_DEBUG_TEMP(priv, "Advanced Thermal Throttling\n");
 		tt->restriction = kcalloc(IWL_TI_STATE_MAX,
 					  sizeof(struct iwl_tt_restriction),
diff --git a/drivers/net/wireless/iwlwifi/iwl-agn-tx.c b/drivers/net/wireless/iwlwifi/iwl-agn-tx.c
index 34adedc74d3..3366e2e2f00 100644
--- a/drivers/net/wireless/iwlwifi/iwl-agn-tx.c
+++ b/drivers/net/wireless/iwlwifi/iwl-agn-tx.c
@@ -34,12 +34,22 @@
 #include <linux/ieee80211.h>
 
 #include "iwl-dev.h"
-#include "iwl-core.h"
 #include "iwl-io.h"
 #include "iwl-agn-hw.h"
 #include "iwl-agn.h"
 #include "iwl-trans.h"
 
+static const u8 tid_to_ac[] = {
+	IEEE80211_AC_BE,
+	IEEE80211_AC_BK,
+	IEEE80211_AC_BK,
+	IEEE80211_AC_BE,
+	IEEE80211_AC_VI,
+	IEEE80211_AC_VI,
+	IEEE80211_AC_VO,
+	IEEE80211_AC_VO,
+};
+
 static void iwlagn_tx_cmd_protection(struct iwl_priv *priv,
 				     struct ieee80211_tx_info *info,
 				     __le16 fc, __le32 *tx_flags)
@@ -74,8 +84,8 @@ static void iwlagn_tx_cmd_build_basic(struct iwl_priv *priv,
 	else if (ieee80211_is_back_req(fc))
 		tx_flags |= TX_CMD_FLG_ACK_MSK | TX_CMD_FLG_IMM_BA_RSP_MASK;
 	else if (info->band == IEEE80211_BAND_2GHZ &&
-		 cfg(priv)->bt_params &&
-		 cfg(priv)->bt_params->advanced_bt_coexist &&
+		 priv->cfg->bt_params &&
+		 priv->cfg->bt_params->advanced_bt_coexist &&
 		 (ieee80211_is_auth(fc) || ieee80211_is_assoc_req(fc) ||
 		 ieee80211_is_reassoc_req(fc) ||
 		 skb->protocol == cpu_to_be16(ETH_P_PAE)))
@@ -192,15 +202,15 @@ static void iwlagn_tx_cmd_build_rate(struct iwl_priv *priv,
 		rate_flags |= RATE_MCS_CCK_MSK;
 
 	/* Set up antennas */
-	 if (cfg(priv)->bt_params &&
-	     cfg(priv)->bt_params->advanced_bt_coexist &&
+	 if (priv->cfg->bt_params &&
+	     priv->cfg->bt_params->advanced_bt_coexist &&
 	     priv->bt_full_concurrent) {
 		/* operated as 1x1 in full concurrency mode */
 		priv->mgmt_tx_ant = iwl_toggle_tx_ant(priv, priv->mgmt_tx_ant,
-				first_antenna(hw_params(priv).valid_tx_ant));
+				first_antenna(priv->hw_params.valid_tx_ant));
 	} else
 		priv->mgmt_tx_ant = iwl_toggle_tx_ant(priv, priv->mgmt_tx_ant,
-						hw_params(priv).valid_tx_ant);
+						priv->hw_params.valid_tx_ant);
 	rate_flags |= iwl_ant_idx_to_flags(priv->mgmt_tx_ant);
 
 	/* Set the rate in the TX cmd */
@@ -293,6 +303,7 @@ int iwlagn_tx_skb(struct iwl_priv *priv, struct sk_buff *skb)
 	u16 len, seq_number = 0;
 	u8 sta_id, tid = IWL_MAX_TID_COUNT;
 	bool is_agg = false;
+	int txq_id;
 
 	if (info->control.vif)
 		ctx = iwl_rxon_ctx_from_vif(info->control.vif);
@@ -384,12 +395,9 @@ int iwlagn_tx_skb(struct iwl_priv *priv, struct sk_buff *skb)
 
 	/* TODO need this for burst mode later on */
 	iwlagn_tx_cmd_build_basic(priv, skb, tx_cmd, info, hdr, sta_id);
-	iwl_dbg_log_tx_data_frame(priv, len, hdr);
 
 	iwlagn_tx_cmd_build_rate(priv, tx_cmd, info, fc);
 
-	iwl_update_stats(priv, true, fc, len);
-
 	memset(&info->status, 0, sizeof(info->status));
 
 	info->driver_data[0] = ctx;
@@ -435,7 +443,31 @@ int iwlagn_tx_skb(struct iwl_priv *priv, struct sk_buff *skb)
 	/* Copy MAC header from skb into command buffer */
 	memcpy(tx_cmd->hdr, hdr, hdr_len);
 
-	if (iwl_trans_tx(trans(priv), skb, dev_cmd, ctx->ctxid, sta_id, tid))
+	if (is_agg)
+		txq_id = priv->tid_data[sta_id][tid].agg.txq_id;
+	else if (info->flags & IEEE80211_TX_CTL_SEND_AFTER_DTIM) {
+		/*
+		 * Send this frame after DTIM -- there's a special queue
+		 * reserved for this for contexts that support AP mode.
+		 */
+		txq_id = ctx->mcast_queue;
+
+		/*
+		 * The microcode will clear the more data
+		 * bit in the last frame it transmits.
+		 */
+		hdr->frame_control |=
+			cpu_to_le16(IEEE80211_FCTL_MOREDATA);
+	} else if (info->flags & IEEE80211_TX_CTL_TX_OFFCHAN)
+		txq_id = IWL_AUX_QUEUE;
+	else
+		txq_id = ctx->ac_to_queue[skb_get_queue_mapping(skb)];
+
+	WARN_ON_ONCE(!is_agg && txq_id != info->hw_queue);
+	WARN_ON_ONCE(is_agg &&
+		     priv->queue_to_mac80211[txq_id] != info->hw_queue);
+
+	if (iwl_trans_tx(priv->trans, skb, dev_cmd, txq_id))
 		goto drop_unlock_sta;
 
 	if (ieee80211_is_data_qos(fc) && !ieee80211_is_qos_nullfunc(fc) &&
@@ -464,11 +496,33 @@ drop_unlock_priv:
 	return -1;
 }
 
+static int iwlagn_alloc_agg_txq(struct iwl_priv *priv, int mq)
+{
+	int q;
+
+	for (q = IWLAGN_FIRST_AMPDU_QUEUE;
+	     q < priv->cfg->base_params->num_of_queues; q++) {
+		if (!test_and_set_bit(q, priv->agg_q_alloc)) {
+			priv->queue_to_mac80211[q] = mq;
+			return q;
+		}
+	}
+
+	return -ENOSPC;
+}
+
+static void iwlagn_dealloc_agg_txq(struct iwl_priv *priv, int q)
+{
+	clear_bit(q, priv->agg_q_alloc);
+	priv->queue_to_mac80211[q] = IWL_INVALID_MAC80211_QUEUE;
+}
+
 int iwlagn_tx_agg_stop(struct iwl_priv *priv, struct ieee80211_vif *vif,
 			struct ieee80211_sta *sta, u16 tid)
 {
 	struct iwl_tid_data *tid_data;
-	int sta_id;
+	int sta_id, txq_id;
+	enum iwl_agg_state agg_state;
 
 	sta_id = iwl_sta_id(sta);
 
@@ -480,6 +534,7 @@ int iwlagn_tx_agg_stop(struct iwl_priv *priv, struct ieee80211_vif *vif,
 	spin_lock_bh(&priv->sta_lock);
 
 	tid_data = &priv->tid_data[sta_id][tid];
+	txq_id = priv->tid_data[sta_id][tid].agg.txq_id;
 
 	switch (priv->tid_data[sta_id][tid].agg.state) {
 	case IWL_EMPTYING_HW_QUEUE_ADDBA:
@@ -491,6 +546,13 @@ int iwlagn_tx_agg_stop(struct iwl_priv *priv, struct ieee80211_vif *vif,
 		*/
 		IWL_DEBUG_HT(priv, "AGG stop before setup done\n");
 		goto turn_off;
+	case IWL_AGG_STARTING:
+		/*
+		 * This can happen when the session is stopped before
+		 * we receive ADDBA response
+		 */
+		IWL_DEBUG_HT(priv, "AGG stop before AGG became operational\n");
+		goto turn_off;
 	case IWL_AGG_ON:
 		break;
 	default:
@@ -504,9 +566,13 @@ int iwlagn_tx_agg_stop(struct iwl_priv *priv, struct ieee80211_vif *vif,
 	tid_data->agg.ssn = SEQ_TO_SN(tid_data->seq_number);
 
 	/* There are still packets for this RA / TID in the HW */
-	if (tid_data->agg.ssn != tid_data->next_reclaimed) {
+	if (!test_bit(txq_id, priv->agg_q_alloc)) {
+		IWL_DEBUG_TX_QUEUES(priv,
+			"stopping AGG on STA/TID %d/%d but hwq %d not used\n",
+			sta_id, tid, txq_id);
+	} else if (tid_data->agg.ssn != tid_data->next_reclaimed) {
 		IWL_DEBUG_TX_QUEUES(priv, "Can't proceed: ssn %d, "
-				    "next_recl = %d",
+				    "next_recl = %d\n",
 				    tid_data->agg.ssn,
 				    tid_data->next_reclaimed);
 		priv->tid_data[sta_id][tid].agg.state =
@@ -515,14 +581,28 @@ int iwlagn_tx_agg_stop(struct iwl_priv *priv, struct ieee80211_vif *vif,
 		return 0;
 	}
 
-	IWL_DEBUG_TX_QUEUES(priv, "Can proceed: ssn = next_recl = %d",
+	IWL_DEBUG_TX_QUEUES(priv, "Can proceed: ssn = next_recl = %d\n",
 			    tid_data->agg.ssn);
 turn_off:
+	agg_state = priv->tid_data[sta_id][tid].agg.state;
 	priv->tid_data[sta_id][tid].agg.state = IWL_AGG_OFF;
 
 	spin_unlock_bh(&priv->sta_lock);
 
-	iwl_trans_tx_agg_disable(trans(priv), sta_id, tid);
+	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_tx_agg_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);
+	}
 
 	ieee80211_stop_tx_ba_cb_irqsafe(vif, sta->addr, tid);
 
@@ -532,9 +612,9 @@ turn_off:
 int iwlagn_tx_agg_start(struct iwl_priv *priv, struct ieee80211_vif *vif,
 			struct ieee80211_sta *sta, u16 tid, u16 *ssn)
 {
+	struct iwl_rxon_context *ctx = iwl_rxon_ctx_from_vif(vif);
 	struct iwl_tid_data *tid_data;
-	int sta_id;
-	int ret;
+	int sta_id, txq_id, ret;
 
 	IWL_DEBUG_HT(priv, "TX AGG request on ra = %pM tid = %d\n",
 		     sta->addr, tid);
@@ -552,36 +632,37 @@ int iwlagn_tx_agg_start(struct iwl_priv *priv, struct ieee80211_vif *vif,
 		return -ENXIO;
 	}
 
+	txq_id = iwlagn_alloc_agg_txq(priv, ctx->ac_to_queue[tid_to_ac[tid]]);
+	if (txq_id < 0) {
+		IWL_DEBUG_TX_QUEUES(priv,
+			"No free aggregation queue for %pM/%d\n",
+			sta->addr, tid);
+		return txq_id;
+	}
+
 	ret = iwl_sta_tx_modify_enable_tid(priv, sta_id, tid);
 	if (ret)
 		return ret;
 
 	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.txq_id = txq_id;
 
 	*ssn = tid_data->agg.ssn;
 
-	ret = iwl_trans_tx_agg_alloc(trans(priv), sta_id, tid);
-	if (ret) {
-		spin_unlock_bh(&priv->sta_lock);
-		return ret;
-	}
-
 	if (*ssn == tid_data->next_reclaimed) {
-		IWL_DEBUG_TX_QUEUES(priv, "Can proceed: ssn = next_recl = %d",
+		IWL_DEBUG_TX_QUEUES(priv, "Can proceed: ssn = next_recl = %d\n",
 				    tid_data->agg.ssn);
-		tid_data->agg.state = IWL_AGG_ON;
+		tid_data->agg.state = IWL_AGG_STARTING;
 		ieee80211_start_tx_ba_cb_irqsafe(vif, sta->addr, tid);
 	} else {
 		IWL_DEBUG_TX_QUEUES(priv, "Can't proceed: ssn %d, "
-				    "next_reclaimed = %d",
+				    "next_reclaimed = %d\n",
 				    tid_data->agg.ssn,
 				    tid_data->next_reclaimed);
 		tid_data->agg.state = IWL_EMPTYING_HW_QUEUE_ADDBA;
 	}
-
 	spin_unlock_bh(&priv->sta_lock);
 
 	return ret;
@@ -592,15 +673,21 @@ int iwlagn_tx_agg_oper(struct iwl_priv *priv, struct ieee80211_vif *vif,
 {
 	struct iwl_station_priv *sta_priv = (void *) sta->drv_priv;
 	struct iwl_rxon_context *ctx = iwl_rxon_ctx_from_vif(vif);
+	int q, fifo;
 	u16 ssn;
 
 	buf_size = min_t(int, buf_size, LINK_QUAL_AGG_FRAME_LIMIT_DEF);
 
 	spin_lock_bh(&priv->sta_lock);
 	ssn = priv->tid_data[sta_priv->sta_id][tid].agg.ssn;
+	q = priv->tid_data[sta_priv->sta_id][tid].agg.txq_id;
+	priv->tid_data[sta_priv->sta_id][tid].agg.state = IWL_AGG_ON;
 	spin_unlock_bh(&priv->sta_lock);
 
-	iwl_trans_tx_agg_setup(trans(priv), ctx->ctxid, sta_priv->sta_id, tid,
+	fifo = ctx->ac_to_fifo[tid_to_ac[tid]];
+
+	iwl_trans_tx_agg_setup(priv->trans, q, fifo,
+			       sta_priv->sta_id, tid,
 			       buf_size, ssn);
 
 	/*
@@ -623,7 +710,7 @@ int iwlagn_tx_agg_oper(struct iwl_priv *priv, struct ieee80211_vif *vif,
 	sta_priv->max_agg_bufsize =
 		min(sta_priv->max_agg_bufsize, buf_size);
 
-	if (hw_params(priv).use_rts_for_aggregation) {
+	if (priv->hw_params.use_rts_for_aggregation) {
 		/*
 		 * switch to RTS/CTS if it is the prefer protection
 		 * method for HT traffic
@@ -666,7 +753,9 @@ static void iwlagn_check_ratid_empty(struct iwl_priv *priv, int sta_id, u8 tid)
 			IWL_DEBUG_TX_QUEUES(priv,
 				"Can continue DELBA flow ssn = next_recl ="
 				" %d", tid_data->next_reclaimed);
-			iwl_trans_tx_agg_disable(trans(priv), sta_id, tid);
+			iwl_trans_tx_agg_disable(priv->trans,
+						 tid_data->agg.txq_id);
+			iwlagn_dealloc_agg_txq(priv, tid_data->agg.txq_id);
 			tid_data->agg.state = IWL_AGG_OFF;
 			ieee80211_stop_tx_ba_cb_irqsafe(vif, addr, tid);
 		}
@@ -677,7 +766,7 @@ static void iwlagn_check_ratid_empty(struct iwl_priv *priv, int sta_id, u8 tid)
 			IWL_DEBUG_TX_QUEUES(priv,
 				"Can continue ADDBA flow ssn = next_recl ="
 				" %d", tid_data->next_reclaimed);
-			tid_data->agg.state = IWL_AGG_ON;
+			tid_data->agg.state = IWL_AGG_STARTING;
 			ieee80211_start_tx_ba_cb_irqsafe(vif, addr, tid);
 		}
 		break;
@@ -711,9 +800,9 @@ static void iwlagn_non_agg_tx_status(struct iwl_priv *priv,
 static void iwlagn_hwrate_to_tx_control(struct iwl_priv *priv, u32 rate_n_flags,
 				  struct ieee80211_tx_info *info)
 {
-	struct ieee80211_tx_rate *r = &info->control.rates[0];
+	struct ieee80211_tx_rate *r = &info->status.rates[0];
 
-	info->antenna_sel_tx =
+	info->status.antenna =
 		((rate_n_flags & RATE_MCS_ANT_ABC_MSK) >> RATE_MCS_ANT_POS);
 	if (rate_n_flags & RATE_MCS_HT_MSK)
 		r->flags |= IEEE80211_TX_RC_MCS;
@@ -841,8 +930,8 @@ static void iwl_rx_reply_tx_agg(struct iwl_priv *priv,
 	 * notification again.
 	 */
 	if (tx_resp->bt_kill_count && tx_resp->frame_count == 1 &&
-	    cfg(priv)->bt_params &&
-	    cfg(priv)->bt_params->advanced_bt_coexist) {
+	    priv->cfg->bt_params &&
+	    priv->cfg->bt_params->advanced_bt_coexist) {
 		IWL_DEBUG_COEX(priv, "receive reply tx w/ bt_kill\n");
 	}
 
@@ -1005,6 +1094,29 @@ static void iwl_check_abort_status(struct iwl_priv *priv,
 	}
 }
 
+static int iwl_reclaim(struct iwl_priv *priv, int sta_id, int tid,
+		       int txq_id, int ssn, struct sk_buff_head *skbs)
+{
+	if (unlikely(txq_id >= IWLAGN_FIRST_AMPDU_QUEUE &&
+		     tid != IWL_TID_NON_QOS &&
+		     txq_id != priv->tid_data[sta_id][tid].agg.txq_id)) {
+		/*
+		 * FIXME: this is a uCode bug which need to be addressed,
+		 * log the information and return for now.
+		 * Since it is can possibly happen very often and in order
+		 * not to fill the syslog, don't use IWL_ERR or IWL_WARN
+		 */
+		IWL_DEBUG_TX_QUEUES(priv,
+			"Bad queue mapping txq_id=%d, agg_txq[sta:%d,tid:%d]=%d\n",
+			txq_id, sta_id, tid,
+			priv->tid_data[sta_id][tid].agg.txq_id);
+		return 1;
+	}
+
+	iwl_trans_reclaim(priv->trans, txq_id, ssn, skbs);
+	return 0;
+}
+
 int iwlagn_rx_reply_tx(struct iwl_priv *priv, struct iwl_rx_cmd_buffer *rxb,
 			       struct iwl_device_cmd *cmd)
 {
@@ -1059,13 +1171,12 @@ int iwlagn_rx_reply_tx(struct iwl_priv *priv, struct iwl_rx_cmd_buffer *rxb,
 		if (tid != IWL_TID_NON_QOS) {
 			priv->tid_data[sta_id][tid].next_reclaimed =
 				next_reclaimed;
-			IWL_DEBUG_TX_REPLY(priv, "Next reclaimed packet:%d",
+			IWL_DEBUG_TX_REPLY(priv, "Next reclaimed packet:%d\n",
 						  next_reclaimed);
 		}
 
 		/*we can free until ssn % q.n_bd not inclusive */
-		WARN_ON(iwl_trans_reclaim(trans(priv), sta_id, tid,
-					  txq_id, ssn, &skbs));
+		WARN_ON(iwl_reclaim(priv, sta_id, tid, txq_id, ssn, &skbs));
 		iwlagn_check_ratid_empty(priv, sta_id, tid);
 		freed = 0;
 
@@ -1159,7 +1270,7 @@ int iwlagn_rx_reply_compressed_ba(struct iwl_priv *priv,
 	 * (in Tx queue's circular buffer) of first TFD/frame in window */
 	u16 ba_resp_scd_ssn = le16_to_cpu(ba_resp->scd_ssn);
 
-	if (scd_flow >= cfg(priv)->base_params->num_of_queues) {
+	if (scd_flow >= priv->cfg->base_params->num_of_queues) {
 		IWL_ERR(priv,
 			"BUG_ON scd_flow is bigger than number of queues\n");
 		return 0;
@@ -1183,8 +1294,8 @@ int iwlagn_rx_reply_compressed_ba(struct iwl_priv *priv,
 	/* Release all TFDs before the SSN, i.e. all TFDs in front of
 	 * block-ack window (we assume that they've been successfully
 	 * transmitted ... if not, it's too late anyway). */
-	if (iwl_trans_reclaim(trans(priv), sta_id, tid, scd_flow,
-			      ba_resp_scd_ssn, &reclaimed_skbs)) {
+	if (iwl_reclaim(priv, sta_id, tid, scd_flow,
+			ba_resp_scd_ssn, &reclaimed_skbs)) {
 		spin_unlock(&priv->sta_lock);
 		return 0;
 	}
@@ -1195,10 +1306,11 @@ int iwlagn_rx_reply_compressed_ba(struct iwl_priv *priv,
 			   (u8 *) &ba_resp->sta_addr_lo32,
 			   ba_resp->sta_id);
 	IWL_DEBUG_TX_REPLY(priv, "TID = %d, SeqCtl = %d, bitmap = 0x%llx, "
-			   "scd_flow = %d, scd_ssn = %d\n",
+			   "scd_flow = %d, scd_ssn = %d sent:%d, acked:%d\n",
 			   ba_resp->tid, le16_to_cpu(ba_resp->seq_ctl),
 			   (unsigned long long)le64_to_cpu(ba_resp->bitmap),
-			   scd_flow, ba_resp_scd_ssn);
+			   scd_flow, ba_resp_scd_ssn, ba_resp->txed,
+			   ba_resp->txed_2_done);
 
 	/* Mark that the expected block-ack response arrived */
 	agg->wait_for_ba = false;
@@ -1214,8 +1326,6 @@ int iwlagn_rx_reply_compressed_ba(struct iwl_priv *priv,
 		 */
 		ba_resp->txed = ba_resp->txed_2_done;
 	}
-	IWL_DEBUG_HT(priv, "agg frames sent:%d, acked:%d\n",
-			ba_resp->txed, ba_resp->txed_2_done);
 
 	priv->tid_data[sta_id][tid].next_reclaimed = ba_resp_scd_ssn;
 
diff --git a/drivers/net/wireless/iwlwifi/iwl-agn.c b/drivers/net/wireless/iwlwifi/iwl-agn.c
index 2a9a16f901c..ec36e2b020b 100644
--- a/drivers/net/wireless/iwlwifi/iwl-agn.c
+++ b/drivers/net/wireless/iwlwifi/iwl-agn.c
@@ -26,6 +26,9 @@
  * Intel Corporation, 5200 N.E. Elam Young Parkway, Hillsboro, OR 97124-6497
  *
  *****************************************************************************/
+
+#define pr_fmt(fmt) KBUILD_MODNAME ": " fmt
+
 #include <linux/kernel.h>
 #include <linux/module.h>
 #include <linux/init.h>
@@ -43,13 +46,13 @@
 
 #include "iwl-eeprom.h"
 #include "iwl-dev.h"
-#include "iwl-core.h"
 #include "iwl-io.h"
 #include "iwl-agn-calib.h"
 #include "iwl-agn.h"
-#include "iwl-shared.h"
 #include "iwl-trans.h"
 #include "iwl-op-mode.h"
+#include "iwl-drv.h"
+#include "iwl-modparams.h"
 
 /******************************************************************************
  *
@@ -177,7 +180,7 @@ int iwlagn_send_beacon_cmd(struct iwl_priv *priv)
 		rate = info->control.rates[0].idx;
 
 	priv->mgmt_tx_ant = iwl_toggle_tx_ant(priv, priv->mgmt_tx_ant,
-					      hw_params(priv).valid_tx_ant);
+					      priv->hw_params.valid_tx_ant);
 	rate_flags = iwl_ant_idx_to_flags(priv->mgmt_tx_ant);
 
 	/* In mac80211, rates for 5 GHz start at 0 */
@@ -286,6 +289,25 @@ out:
 	mutex_unlock(&priv->mutex);
 }
 
+int iwl_send_statistics_request(struct iwl_priv *priv, u8 flags, bool clear)
+{
+	struct iwl_statistics_cmd statistics_cmd = {
+		.configuration_flags =
+			clear ? IWL_STATS_CONF_CLEAR_STATS : 0,
+	};
+
+	if (flags & CMD_ASYNC)
+		return iwl_dvm_send_cmd_pdu(priv, REPLY_STATISTICS_CMD,
+					CMD_ASYNC,
+					sizeof(struct iwl_statistics_cmd),
+					&statistics_cmd);
+	else
+		return iwl_dvm_send_cmd_pdu(priv, REPLY_STATISTICS_CMD,
+					CMD_SYNC,
+					sizeof(struct iwl_statistics_cmd),
+					&statistics_cmd);
+}
+
 /**
  * iwl_bg_statistics_periodic - Timer callback to queue statistics
  *
@@ -326,14 +348,14 @@ static void iwl_print_cont_event_trace(struct iwl_priv *priv, u32 base,
 		ptr = base + (4 * sizeof(u32)) + (start_idx * 3 * sizeof(u32));
 
 	/* Make sure device is powered up for SRAM reads */
-	spin_lock_irqsave(&trans(priv)->reg_lock, reg_flags);
-	if (unlikely(!iwl_grab_nic_access(trans(priv)))) {
-		spin_unlock_irqrestore(&trans(priv)->reg_lock, reg_flags);
+	spin_lock_irqsave(&priv->trans->reg_lock, reg_flags);
+	if (unlikely(!iwl_grab_nic_access(priv->trans))) {
+		spin_unlock_irqrestore(&priv->trans->reg_lock, reg_flags);
 		return;
 	}
 
 	/* Set starting address; reads will auto-increment */
-	iwl_write32(trans(priv), HBUS_TARG_MEM_RADDR, ptr);
+	iwl_write32(priv->trans, HBUS_TARG_MEM_RADDR, ptr);
 
 	/*
 	 * Refuse to read more than would have fit into the log from
@@ -349,20 +371,20 @@ static void iwl_print_cont_event_trace(struct iwl_priv *priv, u32 base,
 	 * place event id # at far right for easier visual parsing.
 	 */
 	for (i = 0; i < num_events; i++) {
-		ev = iwl_read32(trans(priv), HBUS_TARG_MEM_RDAT);
-		time = iwl_read32(trans(priv), HBUS_TARG_MEM_RDAT);
+		ev = iwl_read32(priv->trans, HBUS_TARG_MEM_RDAT);
+		time = iwl_read32(priv->trans, HBUS_TARG_MEM_RDAT);
 		if (mode == 0) {
 			trace_iwlwifi_dev_ucode_cont_event(
-					trans(priv)->dev, 0, time, ev);
+					priv->trans->dev, 0, time, ev);
 		} else {
-			data = iwl_read32(trans(priv), HBUS_TARG_MEM_RDAT);
+			data = iwl_read32(priv->trans, HBUS_TARG_MEM_RDAT);
 			trace_iwlwifi_dev_ucode_cont_event(
-					trans(priv)->dev, time, data, ev);
+					priv->trans->dev, time, data, ev);
 		}
 	}
 	/* Allow device to power down */
-	iwl_release_nic_access(trans(priv));
-	spin_unlock_irqrestore(&trans(priv)->reg_lock, reg_flags);
+	iwl_release_nic_access(priv->trans);
+	spin_unlock_irqrestore(&priv->trans->reg_lock, reg_flags);
 }
 
 static void iwl_continuous_event_trace(struct iwl_priv *priv)
@@ -379,10 +401,9 @@ static void iwl_continuous_event_trace(struct iwl_priv *priv)
 	u32 num_wraps;  /* # times uCode wrapped to top of log */
 	u32 next_entry; /* index of next entry to be written by uCode */
 
-	base = priv->shrd->device_pointers.log_event_table;
+	base = priv->device_pointers.log_event_table;
 	if (iwlagn_hw_valid_rtc_data_addr(base)) {
-		iwl_read_targ_mem_words(trans(priv), base, &read, sizeof(read));
-
+		iwl_read_targ_mem_words(priv->trans, base, &read, sizeof(read));
 		capacity = read.capacity;
 		mode = read.mode;
 		num_wraps = read.wrap_counter;
@@ -422,7 +443,7 @@ static void iwl_continuous_event_trace(struct iwl_priv *priv)
 		else
 			priv->event_log.wraps_once_count++;
 
-		trace_iwlwifi_dev_ucode_wrap_event(trans(priv)->dev,
+		trace_iwlwifi_dev_ucode_wrap_event(priv->trans->dev,
 				num_wraps - priv->event_log.num_wraps,
 				next_entry, priv->event_log.next_entry);
 
@@ -488,7 +509,76 @@ static void iwl_bg_tx_flush(struct work_struct *work)
 	iwlagn_dev_txfifo_flush(priv, IWL_DROP_ALL);
 }
 
-static void iwl_init_context(struct iwl_priv *priv, u32 ucode_flags)
+/*
+ * queue/FIFO/AC mapping definitions
+ */
+
+#define IWL_TX_FIFO_BK		0	/* shared */
+#define IWL_TX_FIFO_BE		1
+#define IWL_TX_FIFO_VI		2	/* shared */
+#define IWL_TX_FIFO_VO		3
+#define IWL_TX_FIFO_BK_IPAN	IWL_TX_FIFO_BK
+#define IWL_TX_FIFO_BE_IPAN	4
+#define IWL_TX_FIFO_VI_IPAN	IWL_TX_FIFO_VI
+#define IWL_TX_FIFO_VO_IPAN	5
+/* re-uses the VO FIFO, uCode will properly flush/schedule */
+#define IWL_TX_FIFO_AUX		5
+#define IWL_TX_FIFO_UNUSED	-1
+
+#define IWLAGN_CMD_FIFO_NUM	7
+
+/*
+ * This queue number is required for proper operation
+ * because the ucode will stop/start the scheduler as
+ * required.
+ */
+#define IWL_IPAN_MCAST_QUEUE	8
+
+static const u8 iwlagn_default_queue_to_tx_fifo[] = {
+	IWL_TX_FIFO_VO,
+	IWL_TX_FIFO_VI,
+	IWL_TX_FIFO_BE,
+	IWL_TX_FIFO_BK,
+	IWLAGN_CMD_FIFO_NUM,
+};
+
+static const u8 iwlagn_ipan_queue_to_tx_fifo[] = {
+	IWL_TX_FIFO_VO,
+	IWL_TX_FIFO_VI,
+	IWL_TX_FIFO_BE,
+	IWL_TX_FIFO_BK,
+	IWL_TX_FIFO_BK_IPAN,
+	IWL_TX_FIFO_BE_IPAN,
+	IWL_TX_FIFO_VI_IPAN,
+	IWL_TX_FIFO_VO_IPAN,
+	IWL_TX_FIFO_BE_IPAN,
+	IWLAGN_CMD_FIFO_NUM,
+	IWL_TX_FIFO_AUX,
+};
+
+static const u8 iwlagn_bss_ac_to_fifo[] = {
+	IWL_TX_FIFO_VO,
+	IWL_TX_FIFO_VI,
+	IWL_TX_FIFO_BE,
+	IWL_TX_FIFO_BK,
+};
+
+static const u8 iwlagn_bss_ac_to_queue[] = {
+	0, 1, 2, 3,
+};
+
+static const u8 iwlagn_pan_ac_to_fifo[] = {
+	IWL_TX_FIFO_VO_IPAN,
+	IWL_TX_FIFO_VI_IPAN,
+	IWL_TX_FIFO_BE_IPAN,
+	IWL_TX_FIFO_BK_IPAN,
+};
+
+static const u8 iwlagn_pan_ac_to_queue[] = {
+	7, 6, 5, 4,
+};
+
+void iwl_init_context(struct iwl_priv *priv, u32 ucode_flags)
 {
 	int i;
 
@@ -496,9 +586,9 @@ static void iwl_init_context(struct iwl_priv *priv, u32 ucode_flags)
 	 * The default context is always valid,
 	 * the PAN context depends on uCode.
 	 */
-	priv->shrd->valid_contexts = BIT(IWL_RXON_CTX_BSS);
+	priv->valid_contexts = BIT(IWL_RXON_CTX_BSS);
 	if (ucode_flags & IWL_UCODE_TLV_FLAGS_PAN)
-		priv->shrd->valid_contexts |= BIT(IWL_RXON_CTX_PAN);
+		priv->valid_contexts |= BIT(IWL_RXON_CTX_PAN);
 
 	for (i = 0; i < NUM_IWL_RXON_CTX; i++)
 		priv->contexts[i].ctxid = i;
@@ -513,13 +603,17 @@ static void iwl_init_context(struct iwl_priv *priv, u32 ucode_flags)
 	priv->contexts[IWL_RXON_CTX_BSS].wep_key_cmd = REPLY_WEPKEY;
 	priv->contexts[IWL_RXON_CTX_BSS].bcast_sta_id = IWLAGN_BROADCAST_ID;
 	priv->contexts[IWL_RXON_CTX_BSS].exclusive_interface_modes =
-		BIT(NL80211_IFTYPE_ADHOC);
+		BIT(NL80211_IFTYPE_ADHOC) | BIT(NL80211_IFTYPE_MONITOR);
 	priv->contexts[IWL_RXON_CTX_BSS].interface_modes =
 		BIT(NL80211_IFTYPE_STATION);
 	priv->contexts[IWL_RXON_CTX_BSS].ap_devtype = RXON_DEV_TYPE_AP;
 	priv->contexts[IWL_RXON_CTX_BSS].ibss_devtype = RXON_DEV_TYPE_IBSS;
 	priv->contexts[IWL_RXON_CTX_BSS].station_devtype = RXON_DEV_TYPE_ESS;
 	priv->contexts[IWL_RXON_CTX_BSS].unused_devtype = RXON_DEV_TYPE_ESS;
+	memcpy(priv->contexts[IWL_RXON_CTX_BSS].ac_to_queue,
+	       iwlagn_bss_ac_to_queue, sizeof(iwlagn_bss_ac_to_queue));
+	memcpy(priv->contexts[IWL_RXON_CTX_BSS].ac_to_fifo,
+	       iwlagn_bss_ac_to_fifo, sizeof(iwlagn_bss_ac_to_fifo));
 
 	priv->contexts[IWL_RXON_CTX_PAN].rxon_cmd = REPLY_WIPAN_RXON;
 	priv->contexts[IWL_RXON_CTX_PAN].rxon_timing_cmd =
@@ -542,26 +636,31 @@ static void iwl_init_context(struct iwl_priv *priv, u32 ucode_flags)
 	priv->contexts[IWL_RXON_CTX_PAN].ap_devtype = RXON_DEV_TYPE_CP;
 	priv->contexts[IWL_RXON_CTX_PAN].station_devtype = RXON_DEV_TYPE_2STA;
 	priv->contexts[IWL_RXON_CTX_PAN].unused_devtype = RXON_DEV_TYPE_P2P;
+	memcpy(priv->contexts[IWL_RXON_CTX_PAN].ac_to_queue,
+	       iwlagn_pan_ac_to_queue, sizeof(iwlagn_pan_ac_to_queue));
+	memcpy(priv->contexts[IWL_RXON_CTX_PAN].ac_to_fifo,
+	       iwlagn_pan_ac_to_fifo, sizeof(iwlagn_pan_ac_to_fifo));
+	priv->contexts[IWL_RXON_CTX_PAN].mcast_queue = IWL_IPAN_MCAST_QUEUE;
 
 	BUILD_BUG_ON(NUM_IWL_RXON_CTX != 2);
 }
 
-static void iwl_rf_kill_ct_config(struct iwl_priv *priv)
+void iwl_rf_kill_ct_config(struct iwl_priv *priv)
 {
 	struct iwl_ct_kill_config cmd;
 	struct iwl_ct_kill_throttling_config adv_cmd;
 	int ret = 0;
 
-	iwl_write32(trans(priv), CSR_UCODE_DRV_GP1_CLR,
+	iwl_write32(priv->trans, CSR_UCODE_DRV_GP1_CLR,
 		    CSR_UCODE_DRV_GP1_REG_BIT_CT_KILL_EXIT);
 
 	priv->thermal_throttle.ct_kill_toggle = false;
 
-	if (cfg(priv)->base_params->support_ct_kill_exit) {
+	if (priv->cfg->base_params->support_ct_kill_exit) {
 		adv_cmd.critical_temperature_enter =
-			cpu_to_le32(hw_params(priv).ct_kill_threshold);
+			cpu_to_le32(priv->hw_params.ct_kill_threshold);
 		adv_cmd.critical_temperature_exit =
-			cpu_to_le32(hw_params(priv).ct_kill_exit_threshold);
+			cpu_to_le32(priv->hw_params.ct_kill_exit_threshold);
 
 		ret = iwl_dvm_send_cmd_pdu(priv,
 				       REPLY_CT_KILL_CONFIG_CMD,
@@ -572,11 +671,11 @@ static void iwl_rf_kill_ct_config(struct iwl_priv *priv)
 			IWL_DEBUG_INFO(priv, "REPLY_CT_KILL_CONFIG_CMD "
 				"succeeded, critical temperature enter is %d,"
 				"exit is %d\n",
-				hw_params(priv).ct_kill_threshold,
-				hw_params(priv).ct_kill_exit_threshold);
+				priv->hw_params.ct_kill_threshold,
+				priv->hw_params.ct_kill_exit_threshold);
 	} else {
 		cmd.critical_temperature_R =
-			cpu_to_le32(hw_params(priv).ct_kill_threshold);
+			cpu_to_le32(priv->hw_params.ct_kill_threshold);
 
 		ret = iwl_dvm_send_cmd_pdu(priv,
 				       REPLY_CT_KILL_CONFIG_CMD,
@@ -587,7 +686,7 @@ static void iwl_rf_kill_ct_config(struct iwl_priv *priv)
 			IWL_DEBUG_INFO(priv, "REPLY_CT_KILL_CONFIG_CMD "
 				"succeeded, "
 				"critical temperature is %d\n",
-				hw_params(priv).ct_kill_threshold);
+				priv->hw_params.ct_kill_threshold);
 	}
 }
 
@@ -627,6 +726,29 @@ static int iwlagn_send_tx_ant_config(struct iwl_priv *priv, u8 valid_tx_ant)
 	}
 }
 
+void iwl_send_bt_config(struct iwl_priv *priv)
+{
+	struct iwl_bt_cmd bt_cmd = {
+		.lead_time = BT_LEAD_TIME_DEF,
+		.max_kill = BT_MAX_KILL_DEF,
+		.kill_ack_mask = 0,
+		.kill_cts_mask = 0,
+	};
+
+	if (!iwlwifi_mod_params.bt_coex_active)
+		bt_cmd.flags = BT_COEX_DISABLE;
+	else
+		bt_cmd.flags = BT_COEX_ENABLE;
+
+	priv->bt_enable_flag = bt_cmd.flags;
+	IWL_DEBUG_INFO(priv, "BT coex %s\n",
+		(bt_cmd.flags == BT_COEX_DISABLE) ? "disable" : "active");
+
+	if (iwl_dvm_send_cmd_pdu(priv, REPLY_BT_CONFIG,
+			     CMD_SYNC, sizeof(struct iwl_bt_cmd), &bt_cmd))
+		IWL_ERR(priv, "failed to send BT Coex Config\n");
+}
+
 /**
  * iwl_alive_start - called after REPLY_ALIVE notification received
  *                   from protocol/runtime uCode (initialization uCode's
@@ -642,9 +764,6 @@ int iwl_alive_start(struct iwl_priv *priv)
 	/* After the ALIVE response, we can send host commands to the uCode */
 	set_bit(STATUS_ALIVE, &priv->status);
 
-	/* Enable watchdog to monitor the driver tx queues */
-	iwl_setup_watchdog(priv);
-
 	if (iwl_is_rfkill(priv))
 		return -ERFKILL;
 
@@ -654,10 +773,10 @@ int iwl_alive_start(struct iwl_priv *priv)
 	}
 
 	/* download priority table before any calibration request */
-	if (cfg(priv)->bt_params &&
-	    cfg(priv)->bt_params->advanced_bt_coexist) {
+	if (priv->cfg->bt_params &&
+	    priv->cfg->bt_params->advanced_bt_coexist) {
 		/* Configure Bluetooth device coexistence support */
-		if (cfg(priv)->bt_params->bt_sco_disable)
+		if (priv->cfg->bt_params->bt_sco_disable)
 			priv->bt_enable_pspoll = false;
 		else
 			priv->bt_enable_pspoll = true;
@@ -694,10 +813,8 @@ int iwl_alive_start(struct iwl_priv *priv)
 
 	ieee80211_wake_queues(priv->hw);
 
-	priv->active_rate = IWL_RATES_MASK;
-
 	/* Configure Tx antenna selection based on H/W config */
-	iwlagn_send_tx_ant_config(priv, hw_params(priv).valid_tx_ant);
+	iwlagn_send_tx_ant_config(priv, priv->hw_params.valid_tx_ant);
 
 	if (iwl_is_associated_ctx(ctx) && !priv->wowlan) {
 		struct iwl_rxon_cmd *active_rxon =
@@ -788,10 +905,6 @@ void iwl_down(struct iwl_priv *priv)
 	exit_pending =
 		test_and_set_bit(STATUS_EXIT_PENDING, &priv->status);
 
-	/* Stop TX queues watchdog. We need to have STATUS_EXIT_PENDING bit set
-	 * to prevent rearm timer */
-	del_timer_sync(&priv->watchdog);
-
 	iwl_clear_ucode_stations(priv, NULL);
 	iwl_dealloc_bcast_stations(priv);
 	iwl_clear_driver_stations(priv);
@@ -800,9 +913,9 @@ void iwl_down(struct iwl_priv *priv)
 	priv->bt_status = 0;
 	priv->cur_rssi_ctx = NULL;
 	priv->bt_is_sco = 0;
-	if (cfg(priv)->bt_params)
+	if (priv->cfg->bt_params)
 		priv->bt_traffic_load =
-			 cfg(priv)->bt_params->bt_init_traffic_load;
+			 priv->cfg->bt_params->bt_init_traffic_load;
 	else
 		priv->bt_traffic_load = 0;
 	priv->bt_full_concurrent = false;
@@ -817,18 +930,17 @@ void iwl_down(struct iwl_priv *priv)
 		ieee80211_stop_queues(priv->hw);
 
 	priv->ucode_loaded = false;
-	iwl_trans_stop_device(trans(priv));
+	iwl_trans_stop_device(priv->trans);
 
 	/* Clear out all status bits but a few that are stable across reset */
 	priv->status &= test_bit(STATUS_RF_KILL_HW, &priv->status) <<
 				STATUS_RF_KILL_HW |
 			test_bit(STATUS_GEO_CONFIGURED, &priv->status) <<
 				STATUS_GEO_CONFIGURED |
+			test_bit(STATUS_FW_ERROR, &priv->status) <<
+				STATUS_FW_ERROR |
 			test_bit(STATUS_EXIT_PENDING, &priv->status) <<
 				STATUS_EXIT_PENDING;
-	priv->shrd->status &=
-			test_bit(STATUS_FW_ERROR, &priv->shrd->status) <<
-				STATUS_FW_ERROR;
 
 	dev_kfree_skb(priv->beacon_skb);
 	priv->beacon_skb = NULL;
@@ -868,6 +980,7 @@ void iwlagn_prepare_restart(struct iwl_priv *priv)
 	u8 bt_load;
 	u8 bt_status;
 	bool bt_is_sco;
+	int i;
 
 	lockdep_assert_held(&priv->mutex);
 
@@ -895,6 +1008,15 @@ void iwlagn_prepare_restart(struct iwl_priv *priv)
 	priv->bt_traffic_load = bt_load;
 	priv->bt_status = bt_status;
 	priv->bt_is_sco = bt_is_sco;
+
+	/* reset aggregation queues */
+	for (i = IWLAGN_FIRST_AMPDU_QUEUE; i < IWL_MAX_HW_QUEUES; i++)
+		priv->queue_to_mac80211[i] = IWL_INVALID_MAC80211_QUEUE;
+	/* and stop counts */
+	for (i = 0; i < IWL_MAX_HW_QUEUES; i++)
+		atomic_set(&priv->queue_stop_count[i], 0);
+
+	memset(priv->agg_q_alloc, 0, sizeof(priv->agg_q_alloc));
 }
 
 static void iwl_bg_restart(struct work_struct *data)
@@ -904,7 +1026,7 @@ static void iwl_bg_restart(struct work_struct *data)
 	if (test_bit(STATUS_EXIT_PENDING, &priv->status))
 		return;
 
-	if (test_and_clear_bit(STATUS_FW_ERROR, &priv->shrd->status)) {
+	if (test_and_clear_bit(STATUS_FW_ERROR, &priv->status)) {
 		mutex_lock(&priv->mutex);
 		iwlagn_prepare_restart(priv);
 		mutex_unlock(&priv->mutex);
@@ -956,7 +1078,7 @@ static void iwlagn_disable_roc_work(struct work_struct *work)
  *
  *****************************************************************************/
 
-static void iwl_setup_deferred_work(struct iwl_priv *priv)
+void iwl_setup_deferred_work(struct iwl_priv *priv)
 {
 	priv->workqueue = create_singlethread_workqueue(DRV_NAME);
 
@@ -971,7 +1093,7 @@ static void iwl_setup_deferred_work(struct iwl_priv *priv)
 
 	iwl_setup_scan_deferred_work(priv);
 
-	if (cfg(priv)->bt_params)
+	if (priv->cfg->bt_params)
 		iwlagn_bt_setup_deferred_work(priv);
 
 	init_timer(&priv->statistics_periodic);
@@ -981,15 +1103,11 @@ static void iwl_setup_deferred_work(struct iwl_priv *priv)
 	init_timer(&priv->ucode_trace);
 	priv->ucode_trace.data = (unsigned long)priv;
 	priv->ucode_trace.function = iwl_bg_ucode_trace;
-
-	init_timer(&priv->watchdog);
-	priv->watchdog.data = (unsigned long)priv;
-	priv->watchdog.function = iwl_bg_watchdog;
 }
 
 void iwl_cancel_deferred_work(struct iwl_priv *priv)
 {
-	if (cfg(priv)->bt_params)
+	if (priv->cfg->bt_params)
 		iwlagn_bt_cancel_deferred_work(priv);
 
 	cancel_work_sync(&priv->run_time_calib_work);
@@ -1025,7 +1143,193 @@ static void iwl_init_hw_rates(struct ieee80211_rate *rates)
 	}
 }
 
-static int iwl_init_drv(struct iwl_priv *priv)
+#define MAX_BIT_RATE_40_MHZ 150 /* Mbps */
+#define MAX_BIT_RATE_20_MHZ 72 /* Mbps */
+static void iwl_init_ht_hw_capab(const struct iwl_priv *priv,
+			      struct ieee80211_sta_ht_cap *ht_info,
+			      enum ieee80211_band band)
+{
+	u16 max_bit_rate = 0;
+	u8 rx_chains_num = priv->hw_params.rx_chains_num;
+	u8 tx_chains_num = priv->hw_params.tx_chains_num;
+
+	ht_info->cap = 0;
+	memset(&ht_info->mcs, 0, sizeof(ht_info->mcs));
+
+	ht_info->ht_supported = true;
+
+	if (priv->cfg->ht_params &&
+	    priv->cfg->ht_params->ht_greenfield_support)
+		ht_info->cap |= IEEE80211_HT_CAP_GRN_FLD;
+	ht_info->cap |= IEEE80211_HT_CAP_SGI_20;
+	max_bit_rate = MAX_BIT_RATE_20_MHZ;
+	if (priv->hw_params.ht40_channel & BIT(band)) {
+		ht_info->cap |= IEEE80211_HT_CAP_SUP_WIDTH_20_40;
+		ht_info->cap |= IEEE80211_HT_CAP_SGI_40;
+		ht_info->mcs.rx_mask[4] = 0x01;
+		max_bit_rate = MAX_BIT_RATE_40_MHZ;
+	}
+
+	if (iwlwifi_mod_params.amsdu_size_8K)
+		ht_info->cap |= IEEE80211_HT_CAP_MAX_AMSDU;
+
+	ht_info->ampdu_factor = CFG_HT_RX_AMPDU_FACTOR_DEF;
+	ht_info->ampdu_density = CFG_HT_MPDU_DENSITY_DEF;
+
+	ht_info->mcs.rx_mask[0] = 0xFF;
+	if (rx_chains_num >= 2)
+		ht_info->mcs.rx_mask[1] = 0xFF;
+	if (rx_chains_num >= 3)
+		ht_info->mcs.rx_mask[2] = 0xFF;
+
+	/* Highest supported Rx data rate */
+	max_bit_rate *= rx_chains_num;
+	WARN_ON(max_bit_rate & ~IEEE80211_HT_MCS_RX_HIGHEST_MASK);
+	ht_info->mcs.rx_highest = cpu_to_le16(max_bit_rate);
+
+	/* Tx MCS capabilities */
+	ht_info->mcs.tx_params = IEEE80211_HT_MCS_TX_DEFINED;
+	if (tx_chains_num != rx_chains_num) {
+		ht_info->mcs.tx_params |= IEEE80211_HT_MCS_TX_RX_DIFF;
+		ht_info->mcs.tx_params |= ((tx_chains_num - 1) <<
+				IEEE80211_HT_MCS_TX_MAX_STREAMS_SHIFT);
+	}
+}
+
+/**
+ * iwl_init_geos - Initialize mac80211's geo/channel info based from eeprom
+ */
+static int iwl_init_geos(struct iwl_priv *priv)
+{
+	struct iwl_channel_info *ch;
+	struct ieee80211_supported_band *sband;
+	struct ieee80211_channel *channels;
+	struct ieee80211_channel *geo_ch;
+	struct ieee80211_rate *rates;
+	int i = 0;
+	s8 max_tx_power = IWLAGN_TX_POWER_TARGET_POWER_MIN;
+
+	if (priv->bands[IEEE80211_BAND_2GHZ].n_bitrates ||
+	    priv->bands[IEEE80211_BAND_5GHZ].n_bitrates) {
+		IWL_DEBUG_INFO(priv, "Geography modes already initialized.\n");
+		set_bit(STATUS_GEO_CONFIGURED, &priv->status);
+		return 0;
+	}
+
+	channels = kcalloc(priv->channel_count,
+			   sizeof(struct ieee80211_channel), GFP_KERNEL);
+	if (!channels)
+		return -ENOMEM;
+
+	rates = kcalloc(IWL_RATE_COUNT_LEGACY, sizeof(struct ieee80211_rate),
+			GFP_KERNEL);
+	if (!rates) {
+		kfree(channels);
+		return -ENOMEM;
+	}
+
+	/* 5.2GHz channels start after the 2.4GHz channels */
+	sband = &priv->bands[IEEE80211_BAND_5GHZ];
+	sband->channels = &channels[ARRAY_SIZE(iwl_eeprom_band_1)];
+	/* just OFDM */
+	sband->bitrates = &rates[IWL_FIRST_OFDM_RATE];
+	sband->n_bitrates = IWL_RATE_COUNT_LEGACY - IWL_FIRST_OFDM_RATE;
+
+	if (priv->hw_params.sku & EEPROM_SKU_CAP_11N_ENABLE)
+		iwl_init_ht_hw_capab(priv, &sband->ht_cap,
+					 IEEE80211_BAND_5GHZ);
+
+	sband = &priv->bands[IEEE80211_BAND_2GHZ];
+	sband->channels = channels;
+	/* OFDM & CCK */
+	sband->bitrates = rates;
+	sband->n_bitrates = IWL_RATE_COUNT_LEGACY;
+
+	if (priv->hw_params.sku & EEPROM_SKU_CAP_11N_ENABLE)
+		iwl_init_ht_hw_capab(priv, &sband->ht_cap,
+					 IEEE80211_BAND_2GHZ);
+
+	priv->ieee_channels = channels;
+	priv->ieee_rates = rates;
+
+	for (i = 0;  i < priv->channel_count; i++) {
+		ch = &priv->channel_info[i];
+
+		/* FIXME: might be removed if scan is OK */
+		if (!is_channel_valid(ch))
+			continue;
+
+		sband =  &priv->bands[ch->band];
+
+		geo_ch = &sband->channels[sband->n_channels++];
+
+		geo_ch->center_freq =
+			ieee80211_channel_to_frequency(ch->channel, ch->band);
+		geo_ch->max_power = ch->max_power_avg;
+		geo_ch->max_antenna_gain = 0xff;
+		geo_ch->hw_value = ch->channel;
+
+		if (is_channel_valid(ch)) {
+			if (!(ch->flags & EEPROM_CHANNEL_IBSS))
+				geo_ch->flags |= IEEE80211_CHAN_NO_IBSS;
+
+			if (!(ch->flags & EEPROM_CHANNEL_ACTIVE))
+				geo_ch->flags |= IEEE80211_CHAN_PASSIVE_SCAN;
+
+			if (ch->flags & EEPROM_CHANNEL_RADAR)
+				geo_ch->flags |= IEEE80211_CHAN_RADAR;
+
+			geo_ch->flags |= ch->ht40_extension_channel;
+
+			if (ch->max_power_avg > max_tx_power)
+				max_tx_power = ch->max_power_avg;
+		} else {
+			geo_ch->flags |= IEEE80211_CHAN_DISABLED;
+		}
+
+		IWL_DEBUG_INFO(priv, "Channel %d Freq=%d[%sGHz] %s flag=0x%X\n",
+				ch->channel, geo_ch->center_freq,
+				is_channel_a_band(ch) ?  "5.2" : "2.4",
+				geo_ch->flags & IEEE80211_CHAN_DISABLED ?
+				"restricted" : "valid",
+				 geo_ch->flags);
+	}
+
+	priv->tx_power_device_lmt = max_tx_power;
+	priv->tx_power_user_lmt = max_tx_power;
+	priv->tx_power_next = max_tx_power;
+
+	if ((priv->bands[IEEE80211_BAND_5GHZ].n_channels == 0) &&
+	     priv->hw_params.sku & EEPROM_SKU_CAP_BAND_52GHZ) {
+		IWL_INFO(priv, "Incorrectly detected BG card as ABG. "
+			"Please send your %s to maintainer.\n",
+			priv->trans->hw_id_str);
+		priv->hw_params.sku &= ~EEPROM_SKU_CAP_BAND_52GHZ;
+	}
+
+	if (iwlwifi_mod_params.disable_5ghz)
+		priv->bands[IEEE80211_BAND_5GHZ].n_channels = 0;
+
+	IWL_INFO(priv, "Tunable channels: %d 802.11bg, %d 802.11a channels\n",
+		   priv->bands[IEEE80211_BAND_2GHZ].n_channels,
+		   priv->bands[IEEE80211_BAND_5GHZ].n_channels);
+
+	set_bit(STATUS_GEO_CONFIGURED, &priv->status);
+
+	return 0;
+}
+
+/*
+ * iwl_free_geos - undo allocations in iwl_init_geos
+ */
+static void iwl_free_geos(struct iwl_priv *priv)
+{
+	kfree(priv->ieee_channels);
+	kfree(priv->ieee_rates);
+	clear_bit(STATUS_GEO_CONFIGURED, &priv->status);
+}
+
+int iwl_init_drv(struct iwl_priv *priv)
 {
 	int ret;
 
@@ -1040,7 +1344,7 @@ static int iwl_init_drv(struct iwl_priv *priv)
 	priv->band = IEEE80211_BAND_2GHZ;
 
 	priv->plcp_delta_threshold =
-		cfg(priv)->base_params->plcp_delta_threshold;
+		priv->cfg->base_params->plcp_delta_threshold;
 
 	priv->iw_mode = NL80211_IFTYPE_STATION;
 	priv->current_ht_config.smps = IEEE80211_SMPS_STATIC;
@@ -1049,12 +1353,6 @@ static int iwl_init_drv(struct iwl_priv *priv)
 
 	priv->ucode_owner = IWL_OWNERSHIP_DRIVER;
 
-	/* initialize force reset */
-	priv->force_reset[IWL_RF_RESET].reset_duration =
-		IWL_DELAY_NEXT_FORCE_RF_RESET;
-	priv->force_reset[IWL_FW_RESET].reset_duration =
-		IWL_DELAY_NEXT_FORCE_FW_RELOAD;
-
 	priv->rx_statistics_jiffies = jiffies;
 
 	/* Choose which receivers/antennas to use */
@@ -1063,8 +1361,8 @@ static int iwl_init_drv(struct iwl_priv *priv)
 	iwl_init_scan_params(priv);
 
 	/* init bt coex */
-	if (cfg(priv)->bt_params &&
-	    cfg(priv)->bt_params->advanced_bt_coexist) {
+	if (priv->cfg->bt_params &&
+	    priv->cfg->bt_params->advanced_bt_coexist) {
 		priv->kill_ack_mask = IWLAGN_BT_KILL_ACK_MASK_DEFAULT;
 		priv->kill_cts_mask = IWLAGN_BT_KILL_CTS_MASK_DEFAULT;
 		priv->bt_valid = IWLAGN_BT_ALL_VALID_MSK;
@@ -1094,7 +1392,7 @@ err:
 	return ret;
 }
 
-static void iwl_uninit_drv(struct iwl_priv *priv)
+void iwl_uninit_drv(struct iwl_priv *priv)
 {
 	iwl_free_geos(priv);
 	iwl_free_channel_map(priv);
@@ -1107,75 +1405,59 @@ static void iwl_uninit_drv(struct iwl_priv *priv)
 #endif
 }
 
-/* Size of one Rx buffer in host DRAM */
-#define IWL_RX_BUF_SIZE_4K (4 * 1024)
-#define IWL_RX_BUF_SIZE_8K (8 * 1024)
-
-static void iwl_set_hw_params(struct iwl_priv *priv)
+void iwl_set_hw_params(struct iwl_priv *priv)
 {
-	if (cfg(priv)->ht_params)
-		hw_params(priv).use_rts_for_aggregation =
-			cfg(priv)->ht_params->use_rts_for_aggregation;
-
-	if (iwlagn_mod_params.amsdu_size_8K)
-		hw_params(priv).rx_page_order =
-			get_order(IWL_RX_BUF_SIZE_8K);
-	else
-		hw_params(priv).rx_page_order =
-			get_order(IWL_RX_BUF_SIZE_4K);
-
-	if (iwlagn_mod_params.disable_11n & IWL_DISABLE_HT_ALL)
-		hw_params(priv).sku &= ~EEPROM_SKU_CAP_11N_ENABLE;
+	if (priv->cfg->ht_params)
+		priv->hw_params.use_rts_for_aggregation =
+			priv->cfg->ht_params->use_rts_for_aggregation;
 
-	hw_params(priv).num_ampdu_queues =
-		cfg(priv)->base_params->num_of_ampdu_queues;
-	hw_params(priv).wd_timeout = cfg(priv)->base_params->wd_timeout;
+	if (iwlwifi_mod_params.disable_11n & IWL_DISABLE_HT_ALL)
+		priv->hw_params.sku &= ~EEPROM_SKU_CAP_11N_ENABLE;
 
 	/* Device-specific setup */
-	cfg(priv)->lib->set_hw_params(priv);
+	priv->lib->set_hw_params(priv);
 }
 
 
 
-static void iwl_debug_config(struct iwl_priv *priv)
+/* show what optional capabilities we have */
+void iwl_option_config(struct iwl_priv *priv)
 {
-	dev_printk(KERN_INFO, trans(priv)->dev, "CONFIG_IWLWIFI_DEBUG "
 #ifdef CONFIG_IWLWIFI_DEBUG
-		"enabled\n");
+	IWL_INFO(priv, "CONFIG_IWLWIFI_DEBUG enabled\n");
 #else
-		"disabled\n");
+	IWL_INFO(priv, "CONFIG_IWLWIFI_DEBUG disabled\n");
 #endif
-	dev_printk(KERN_INFO, trans(priv)->dev, "CONFIG_IWLWIFI_DEBUGFS "
+
 #ifdef CONFIG_IWLWIFI_DEBUGFS
-		"enabled\n");
+	IWL_INFO(priv, "CONFIG_IWLWIFI_DEBUGFS enabled\n");
 #else
-		"disabled\n");
+	IWL_INFO(priv, "CONFIG_IWLWIFI_DEBUGFS disabled\n");
 #endif
-	dev_printk(KERN_INFO, trans(priv)->dev, "CONFIG_IWLWIFI_DEVICE_TRACING "
+
 #ifdef CONFIG_IWLWIFI_DEVICE_TRACING
-		"enabled\n");
+	IWL_INFO(priv, "CONFIG_IWLWIFI_DEVICE_TRACING enabled\n");
 #else
-		"disabled\n");
+	IWL_INFO(priv, "CONFIG_IWLWIFI_DEVICE_TRACING disabled\n");
 #endif
 
-	dev_printk(KERN_INFO, trans(priv)->dev, "CONFIG_IWLWIFI_DEVICE_TESTMODE "
 #ifdef CONFIG_IWLWIFI_DEVICE_TESTMODE
-		"enabled\n");
+	IWL_INFO(priv, "CONFIG_IWLWIFI_DEVICE_TESTMODE enabled\n");
 #else
-		"disabled\n");
+	IWL_INFO(priv, "CONFIG_IWLWIFI_DEVICE_TESTMODE disabled\n");
 #endif
-	dev_printk(KERN_INFO, trans(priv)->dev, "CONFIG_IWLWIFI_P2P "
+
 #ifdef CONFIG_IWLWIFI_P2P
-		"enabled\n");
+	IWL_INFO(priv, "CONFIG_IWLWIFI_P2P enabled\n");
 #else
-		"disabled\n");
+	IWL_INFO(priv, "CONFIG_IWLWIFI_P2P disabled\n");
 #endif
 }
 
 static struct iwl_op_mode *iwl_op_mode_dvm_start(struct iwl_trans *trans,
+						 const struct iwl_cfg *cfg,
 						 const struct iwl_fw *fw)
 {
-	int err = 0;
 	struct iwl_priv *priv;
 	struct ieee80211_hw *hw;
 	struct iwl_op_mode *op_mode;
@@ -1190,25 +1472,60 @@ static struct iwl_op_mode *iwl_op_mode_dvm_start(struct iwl_trans *trans,
 		STATISTICS_NOTIFICATION,
 		REPLY_TX,
 	};
+	int i;
 
 	/************************
 	 * 1. Allocating HW data
 	 ************************/
 	hw = iwl_alloc_all();
 	if (!hw) {
-		pr_err("%s: Cannot allocate network device\n",
-				cfg(trans)->name);
-		err = -ENOMEM;
+		pr_err("%s: Cannot allocate network device\n", cfg->name);
 		goto out;
 	}
 
 	op_mode = hw->priv;
 	op_mode->ops = &iwl_dvm_ops;
 	priv = IWL_OP_MODE_GET_DVM(op_mode);
-	priv->shrd = trans->shrd;
+	priv->trans = trans;
+	priv->dev = trans->dev;
+	priv->cfg = cfg;
 	priv->fw = fw;
-	/* TODO: remove fw from shared data later */
-	priv->shrd->fw = fw;
+
+	switch (priv->cfg->device_family) {
+	case IWL_DEVICE_FAMILY_1000:
+	case IWL_DEVICE_FAMILY_100:
+		priv->lib = &iwl1000_lib;
+		break;
+	case IWL_DEVICE_FAMILY_2000:
+	case IWL_DEVICE_FAMILY_105:
+		priv->lib = &iwl2000_lib;
+		break;
+	case IWL_DEVICE_FAMILY_2030:
+	case IWL_DEVICE_FAMILY_135:
+		priv->lib = &iwl2030_lib;
+		break;
+	case IWL_DEVICE_FAMILY_5000:
+		priv->lib = &iwl5000_lib;
+		break;
+	case IWL_DEVICE_FAMILY_5150:
+		priv->lib = &iwl5150_lib;
+		break;
+	case IWL_DEVICE_FAMILY_6000:
+	case IWL_DEVICE_FAMILY_6005:
+	case IWL_DEVICE_FAMILY_6000i:
+	case IWL_DEVICE_FAMILY_6050:
+	case IWL_DEVICE_FAMILY_6150:
+		priv->lib = &iwl6000_lib;
+		break;
+	case IWL_DEVICE_FAMILY_6030:
+		priv->lib = &iwl6030_lib;
+		break;
+	default:
+		break;
+	}
+
+	if (WARN_ON(!priv->lib))
+		goto out_free_hw;
 
 	/*
 	 * Populate the state variables that the transport layer needs
@@ -1217,87 +1534,90 @@ static struct iwl_op_mode *iwl_op_mode_dvm_start(struct iwl_trans *trans,
 	trans_cfg.op_mode = op_mode;
 	trans_cfg.no_reclaim_cmds = no_reclaim_cmds;
 	trans_cfg.n_no_reclaim_cmds = ARRAY_SIZE(no_reclaim_cmds);
+	trans_cfg.rx_buf_size_8k = iwlwifi_mod_params.amsdu_size_8K;
+	if (!iwlwifi_mod_params.wd_disable)
+		trans_cfg.queue_watchdog_timeout =
+			priv->cfg->base_params->wd_timeout;
+	else
+		trans_cfg.queue_watchdog_timeout = IWL_WATCHHDOG_DISABLED;
+	trans_cfg.command_names = iwl_dvm_cmd_strings;
 
 	ucode_flags = fw->ucode_capa.flags;
 
 #ifndef CONFIG_IWLWIFI_P2P
-	ucode_flags &= ~IWL_UCODE_TLV_FLAGS_PAN;
+	ucode_flags &= ~IWL_UCODE_TLV_FLAGS_P2P;
 #endif
 
 	if (ucode_flags & IWL_UCODE_TLV_FLAGS_PAN) {
 		priv->sta_key_max_num = STA_KEY_MAX_NUM_PAN;
 		trans_cfg.cmd_queue = IWL_IPAN_CMD_QUEUE_NUM;
+		trans_cfg.queue_to_fifo = iwlagn_ipan_queue_to_tx_fifo;
+		trans_cfg.n_queue_to_fifo =
+			ARRAY_SIZE(iwlagn_ipan_queue_to_tx_fifo);
 	} else {
 		priv->sta_key_max_num = STA_KEY_MAX_NUM;
 		trans_cfg.cmd_queue = IWL_DEFAULT_CMD_QUEUE_NUM;
+		trans_cfg.queue_to_fifo = iwlagn_default_queue_to_tx_fifo;
+		trans_cfg.n_queue_to_fifo =
+			ARRAY_SIZE(iwlagn_default_queue_to_tx_fifo);
 	}
 
 	/* Configure transport layer */
-	iwl_trans_configure(trans(priv), &trans_cfg);
+	iwl_trans_configure(priv->trans, &trans_cfg);
 
 	/* At this point both hw and priv are allocated. */
 
-	SET_IEEE80211_DEV(priv->hw, trans(priv)->dev);
+	SET_IEEE80211_DEV(priv->hw, priv->trans->dev);
 
-	/* show what debugging capabilities we have */
-	iwl_debug_config(priv);
+	iwl_option_config(priv);
 
 	IWL_DEBUG_INFO(priv, "*** LOAD DRIVER ***\n");
 
 	/* is antenna coupling more than 35dB ? */
 	priv->bt_ant_couple_ok =
-		(iwlagn_mod_params.ant_coupling >
+		(iwlwifi_mod_params.ant_coupling >
 			IWL_BT_ANTENNA_COUPLING_THRESHOLD) ?
 			true : false;
 
 	/* enable/disable bt channel inhibition */
-	priv->bt_ch_announce = iwlagn_mod_params.bt_ch_announce;
+	priv->bt_ch_announce = iwlwifi_mod_params.bt_ch_announce;
 	IWL_DEBUG_INFO(priv, "BT channel inhibition is %s\n",
 		       (priv->bt_ch_announce) ? "On" : "Off");
 
-	if (iwl_alloc_traffic_mem(priv))
-		IWL_ERR(priv, "Not enough memory to generate traffic log\n");
-
 	/* these spin locks will be used in apm_ops.init and EEPROM access
 	 * we should init now
 	 */
-	spin_lock_init(&trans(priv)->reg_lock);
 	spin_lock_init(&priv->statistics.lock);
 
 	/***********************
 	 * 2. Read REV register
 	 ***********************/
 	IWL_INFO(priv, "Detected %s, REV=0x%X\n",
-		cfg(priv)->name, trans(priv)->hw_rev);
+		priv->cfg->name, priv->trans->hw_rev);
 
-	err = iwl_trans_start_hw(trans(priv));
-	if (err)
-		goto out_free_traffic_mem;
+	if (iwl_trans_start_hw(priv->trans))
+		goto out_free_hw;
 
-	/*****************
-	 * 3. Read EEPROM
-	 *****************/
-	err = iwl_eeprom_init(trans(priv), trans(priv)->hw_rev);
-	/* Reset chip to save power until we load uCode during "up". */
-	iwl_trans_stop_hw(trans(priv));
-	if (err) {
+	/* Read the EEPROM */
+	if (iwl_eeprom_init(priv, priv->trans->hw_rev)) {
 		IWL_ERR(priv, "Unable to init EEPROM\n");
-		goto out_free_traffic_mem;
+		goto out_free_hw;
 	}
-	err = iwl_eeprom_check_version(priv);
-	if (err)
+	/* Reset chip to save power until we load uCode during "up". */
+	iwl_trans_stop_hw(priv->trans, false);
+
+	if (iwl_eeprom_check_version(priv))
 		goto out_free_eeprom;
 
-	err = iwl_eeprom_init_hw_params(priv);
-	if (err)
+	if (iwl_eeprom_init_hw_params(priv))
 		goto out_free_eeprom;
 
 	/* extract MAC Address */
-	iwl_eeprom_get_mac(priv->shrd, priv->addresses[0].addr);
+	iwl_eeprom_get_mac(priv, priv->addresses[0].addr);
 	IWL_DEBUG_INFO(priv, "MAC address: %pM\n", priv->addresses[0].addr);
 	priv->hw->wiphy->addresses = priv->addresses;
 	priv->hw->wiphy->n_addresses = 1;
-	num_mac = iwl_eeprom_query16(priv->shrd, EEPROM_NUM_MAC_ADDRESS);
+	num_mac = iwl_eeprom_query16(priv, EEPROM_NUM_MAC_ADDRESS);
 	if (num_mac > 1) {
 		memcpy(priv->addresses[1].addr, priv->addresses[0].addr,
 		       ETH_ALEN);
@@ -1310,7 +1630,7 @@ static struct iwl_op_mode *iwl_op_mode_dvm_start(struct iwl_trans *trans,
 	 ************************/
 	iwl_set_hw_params(priv);
 
-	if (!(hw_params(priv).sku & EEPROM_SKU_CAP_IPAN_ENABLE)) {
+	if (!(priv->hw_params.sku & EEPROM_SKU_CAP_IPAN_ENABLE)) {
 		IWL_DEBUG_INFO(priv, "Your EEPROM disabled PAN");
 		ucode_flags &= ~IWL_UCODE_TLV_FLAGS_PAN;
 		/*
@@ -1320,18 +1640,32 @@ static struct iwl_op_mode *iwl_op_mode_dvm_start(struct iwl_trans *trans,
 		ucode_flags &= ~IWL_UCODE_TLV_FLAGS_P2P;
 		priv->sta_key_max_num = STA_KEY_MAX_NUM;
 		trans_cfg.cmd_queue = IWL_DEFAULT_CMD_QUEUE_NUM;
+		trans_cfg.queue_to_fifo = iwlagn_default_queue_to_tx_fifo;
+		trans_cfg.n_queue_to_fifo =
+			ARRAY_SIZE(iwlagn_default_queue_to_tx_fifo);
 
 		/* Configure transport layer again*/
-		iwl_trans_configure(trans(priv), &trans_cfg);
+		iwl_trans_configure(priv->trans, &trans_cfg);
 	}
 
 	/*******************
 	 * 5. Setup priv
 	 *******************/
+	for (i = 0; i < IWL_MAX_HW_QUEUES; i++) {
+		priv->queue_to_mac80211[i] = IWL_INVALID_MAC80211_QUEUE;
+		if (i < IWLAGN_FIRST_AMPDU_QUEUE &&
+		    i != IWL_DEFAULT_CMD_QUEUE_NUM &&
+		    i != IWL_IPAN_CMD_QUEUE_NUM)
+			priv->queue_to_mac80211[i] = i;
+		atomic_set(&priv->queue_stop_count[i], 0);
+	}
+
+	WARN_ON(trans_cfg.queue_to_fifo[trans_cfg.cmd_queue] !=
+						IWLAGN_CMD_FIFO_NUM);
 
-	err = iwl_init_drv(priv);
-	if (err)
+	if (iwl_init_drv(priv))
 		goto out_free_eeprom;
+
 	/* At this point both hw and priv are initialized. */
 
 	/********************
@@ -1364,15 +1698,12 @@ static struct iwl_op_mode *iwl_op_mode_dvm_start(struct iwl_trans *trans,
 	 *
 	 * 7. Setup and register with mac80211 and debugfs
 	 **************************************************/
-	err = iwlagn_mac_setup_register(priv, &fw->ucode_capa);
-	if (err)
+	if (iwlagn_mac_setup_register(priv, &fw->ucode_capa))
 		goto out_destroy_workqueue;
 
-	err = iwl_dbgfs_register(priv, DRV_NAME);
-	if (err)
+	if (iwl_dbgfs_register(priv, DRV_NAME))
 		IWL_ERR(priv,
-			"failed to create debugfs files. Ignoring error: %d\n",
-			err);
+			"failed to create debugfs files. Ignoring error\n");
 
 	return op_mode;
 
@@ -1381,16 +1712,15 @@ out_destroy_workqueue:
 	priv->workqueue = NULL;
 	iwl_uninit_drv(priv);
 out_free_eeprom:
-	iwl_eeprom_free(priv->shrd);
-out_free_traffic_mem:
-	iwl_free_traffic_mem(priv);
+	iwl_eeprom_free(priv);
+out_free_hw:
 	ieee80211_free_hw(priv->hw);
 out:
 	op_mode = NULL;
 	return op_mode;
 }
 
-static void iwl_op_mode_dvm_stop(struct iwl_op_mode *op_mode)
+void iwl_op_mode_dvm_stop(struct iwl_op_mode *op_mode)
 {
 	struct iwl_priv *priv = IWL_OP_MODE_GET_DVM(op_mode);
 
@@ -1405,9 +1735,9 @@ static void iwl_op_mode_dvm_stop(struct iwl_op_mode *op_mode)
 
 	/*This will stop the queues, move the device to low power state */
 	priv->ucode_loaded = false;
-	iwl_trans_stop_device(trans(priv));
+	iwl_trans_stop_device(priv->trans);
 
-	iwl_eeprom_free(priv->shrd);
+	iwl_eeprom_free(priv);
 
 	/*netif_stop_queue(dev); */
 	flush_workqueue(priv->workqueue);
@@ -1417,69 +1747,562 @@ static void iwl_op_mode_dvm_stop(struct iwl_op_mode *op_mode)
 	 * until now... */
 	destroy_workqueue(priv->workqueue);
 	priv->workqueue = NULL;
-	iwl_free_traffic_mem(priv);
 
 	iwl_uninit_drv(priv);
 
 	dev_kfree_skb(priv->beacon_skb);
 
+	iwl_trans_stop_hw(priv->trans, true);
 	ieee80211_free_hw(priv->hw);
 }
 
-static void iwl_cmd_queue_full(struct iwl_op_mode *op_mode)
+static const char * const desc_lookup_text[] = {
+	"OK",
+	"FAIL",
+	"BAD_PARAM",
+	"BAD_CHECKSUM",
+	"NMI_INTERRUPT_WDG",
+	"SYSASSERT",
+	"FATAL_ERROR",
+	"BAD_COMMAND",
+	"HW_ERROR_TUNE_LOCK",
+	"HW_ERROR_TEMPERATURE",
+	"ILLEGAL_CHAN_FREQ",
+	"VCC_NOT_STABLE",
+	"FH_ERROR",
+	"NMI_INTERRUPT_HOST",
+	"NMI_INTERRUPT_ACTION_PT",
+	"NMI_INTERRUPT_UNKNOWN",
+	"UCODE_VERSION_MISMATCH",
+	"HW_ERROR_ABS_LOCK",
+	"HW_ERROR_CAL_LOCK_FAIL",
+	"NMI_INTERRUPT_INST_ACTION_PT",
+	"NMI_INTERRUPT_DATA_ACTION_PT",
+	"NMI_TRM_HW_ER",
+	"NMI_INTERRUPT_TRM",
+	"NMI_INTERRUPT_BREAK_POINT",
+	"DEBUG_0",
+	"DEBUG_1",
+	"DEBUG_2",
+	"DEBUG_3",
+};
+
+static struct { char *name; u8 num; } advanced_lookup[] = {
+	{ "NMI_INTERRUPT_WDG", 0x34 },
+	{ "SYSASSERT", 0x35 },
+	{ "UCODE_VERSION_MISMATCH", 0x37 },
+	{ "BAD_COMMAND", 0x38 },
+	{ "NMI_INTERRUPT_DATA_ACTION_PT", 0x3C },
+	{ "FATAL_ERROR", 0x3D },
+	{ "NMI_TRM_HW_ERR", 0x46 },
+	{ "NMI_INTERRUPT_TRM", 0x4C },
+	{ "NMI_INTERRUPT_BREAK_POINT", 0x54 },
+	{ "NMI_INTERRUPT_WDG_RXF_FULL", 0x5C },
+	{ "NMI_INTERRUPT_WDG_NO_RBD_RXF_FULL", 0x64 },
+	{ "NMI_INTERRUPT_HOST", 0x66 },
+	{ "NMI_INTERRUPT_ACTION_PT", 0x7C },
+	{ "NMI_INTERRUPT_UNKNOWN", 0x84 },
+	{ "NMI_INTERRUPT_INST_ACTION_PT", 0x86 },
+	{ "ADVANCED_SYSASSERT", 0 },
+};
+
+static const char *desc_lookup(u32 num)
+{
+	int i;
+	int max = ARRAY_SIZE(desc_lookup_text);
+
+	if (num < max)
+		return desc_lookup_text[num];
+
+	max = ARRAY_SIZE(advanced_lookup) - 1;
+	for (i = 0; i < max; i++) {
+		if (advanced_lookup[i].num == num)
+			break;
+	}
+	return advanced_lookup[i].name;
+}
+
+#define ERROR_START_OFFSET  (1 * sizeof(u32))
+#define ERROR_ELEM_SIZE     (7 * sizeof(u32))
+
+static void iwl_dump_nic_error_log(struct iwl_priv *priv)
+{
+	struct iwl_trans *trans = priv->trans;
+	u32 base;
+	struct iwl_error_event_table table;
+
+	base = priv->device_pointers.error_event_table;
+	if (priv->cur_ucode == IWL_UCODE_INIT) {
+		if (!base)
+			base = priv->fw->init_errlog_ptr;
+	} else {
+		if (!base)
+			base = priv->fw->inst_errlog_ptr;
+	}
+
+	if (!iwlagn_hw_valid_rtc_data_addr(base)) {
+		IWL_ERR(priv,
+			"Not valid error log pointer 0x%08X for %s uCode\n",
+			base,
+			(priv->cur_ucode == IWL_UCODE_INIT)
+					? "Init" : "RT");
+		return;
+	}
+
+	/*TODO: Update dbgfs with ISR error stats obtained below */
+	iwl_read_targ_mem_words(trans, base, &table, sizeof(table));
+
+	if (ERROR_START_OFFSET <= table.valid * ERROR_ELEM_SIZE) {
+		IWL_ERR(trans, "Start IWL Error Log Dump:\n");
+		IWL_ERR(trans, "Status: 0x%08lX, count: %d\n",
+			priv->status, table.valid);
+	}
+
+	trace_iwlwifi_dev_ucode_error(trans->dev, table.error_id, table.tsf_low,
+				      table.data1, table.data2, table.line,
+				      table.blink1, table.blink2, table.ilink1,
+				      table.ilink2, table.bcon_time, table.gp1,
+				      table.gp2, table.gp3, table.ucode_ver,
+				      table.hw_ver, table.brd_ver);
+	IWL_ERR(priv, "0x%08X | %-28s\n", table.error_id,
+		desc_lookup(table.error_id));
+	IWL_ERR(priv, "0x%08X | uPc\n", table.pc);
+	IWL_ERR(priv, "0x%08X | branchlink1\n", table.blink1);
+	IWL_ERR(priv, "0x%08X | branchlink2\n", table.blink2);
+	IWL_ERR(priv, "0x%08X | interruptlink1\n", table.ilink1);
+	IWL_ERR(priv, "0x%08X | interruptlink2\n", table.ilink2);
+	IWL_ERR(priv, "0x%08X | data1\n", table.data1);
+	IWL_ERR(priv, "0x%08X | data2\n", table.data2);
+	IWL_ERR(priv, "0x%08X | line\n", table.line);
+	IWL_ERR(priv, "0x%08X | beacon time\n", table.bcon_time);
+	IWL_ERR(priv, "0x%08X | tsf low\n", table.tsf_low);
+	IWL_ERR(priv, "0x%08X | tsf hi\n", table.tsf_hi);
+	IWL_ERR(priv, "0x%08X | time gp1\n", table.gp1);
+	IWL_ERR(priv, "0x%08X | time gp2\n", table.gp2);
+	IWL_ERR(priv, "0x%08X | time gp3\n", table.gp3);
+	IWL_ERR(priv, "0x%08X | uCode version\n", table.ucode_ver);
+	IWL_ERR(priv, "0x%08X | hw version\n", table.hw_ver);
+	IWL_ERR(priv, "0x%08X | board version\n", table.brd_ver);
+	IWL_ERR(priv, "0x%08X | hcmd\n", table.hcmd);
+	IWL_ERR(priv, "0x%08X | isr0\n", table.isr0);
+	IWL_ERR(priv, "0x%08X | isr1\n", table.isr1);
+	IWL_ERR(priv, "0x%08X | isr2\n", table.isr2);
+	IWL_ERR(priv, "0x%08X | isr3\n", table.isr3);
+	IWL_ERR(priv, "0x%08X | isr4\n", table.isr4);
+	IWL_ERR(priv, "0x%08X | isr_pref\n", table.isr_pref);
+	IWL_ERR(priv, "0x%08X | wait_event\n", table.wait_event);
+	IWL_ERR(priv, "0x%08X | l2p_control\n", table.l2p_control);
+	IWL_ERR(priv, "0x%08X | l2p_duration\n", table.l2p_duration);
+	IWL_ERR(priv, "0x%08X | l2p_mhvalid\n", table.l2p_mhvalid);
+	IWL_ERR(priv, "0x%08X | l2p_addr_match\n", table.l2p_addr_match);
+	IWL_ERR(priv, "0x%08X | lmpm_pmg_sel\n", table.lmpm_pmg_sel);
+	IWL_ERR(priv, "0x%08X | timestamp\n", table.u_timestamp);
+	IWL_ERR(priv, "0x%08X | flow_handler\n", table.flow_handler);
+}
+
+#define EVENT_START_OFFSET  (4 * sizeof(u32))
+
+/**
+ * iwl_print_event_log - Dump error event log to syslog
+ *
+ */
+static int iwl_print_event_log(struct iwl_priv *priv, u32 start_idx,
+			       u32 num_events, u32 mode,
+			       int pos, char **buf, size_t bufsz)
+{
+	u32 i;
+	u32 base;       /* SRAM byte address of event log header */
+	u32 event_size; /* 2 u32s, or 3 u32s if timestamp recorded */
+	u32 ptr;        /* SRAM byte address of log data */
+	u32 ev, time, data; /* event log data */
+	unsigned long reg_flags;
+
+	struct iwl_trans *trans = priv->trans;
+
+	if (num_events == 0)
+		return pos;
+
+	base = priv->device_pointers.log_event_table;
+	if (priv->cur_ucode == IWL_UCODE_INIT) {
+		if (!base)
+			base = priv->fw->init_evtlog_ptr;
+	} else {
+		if (!base)
+			base = priv->fw->inst_evtlog_ptr;
+	}
+
+	if (mode == 0)
+		event_size = 2 * sizeof(u32);
+	else
+		event_size = 3 * sizeof(u32);
+
+	ptr = base + EVENT_START_OFFSET + (start_idx * event_size);
+
+	/* Make sure device is powered up for SRAM reads */
+	spin_lock_irqsave(&trans->reg_lock, reg_flags);
+	if (unlikely(!iwl_grab_nic_access(trans)))
+		goto out_unlock;
+
+	/* Set starting address; reads will auto-increment */
+	iwl_write32(trans, HBUS_TARG_MEM_RADDR, ptr);
+
+	/* "time" is actually "data" for mode 0 (no timestamp).
+	* place event id # at far right for easier visual parsing. */
+	for (i = 0; i < num_events; i++) {
+		ev = iwl_read32(trans, HBUS_TARG_MEM_RDAT);
+		time = iwl_read32(trans, HBUS_TARG_MEM_RDAT);
+		if (mode == 0) {
+			/* data, ev */
+			if (bufsz) {
+				pos += scnprintf(*buf + pos, bufsz - pos,
+						"EVT_LOG:0x%08x:%04u\n",
+						time, ev);
+			} else {
+				trace_iwlwifi_dev_ucode_event(trans->dev, 0,
+					time, ev);
+				IWL_ERR(priv, "EVT_LOG:0x%08x:%04u\n",
+					time, ev);
+			}
+		} else {
+			data = iwl_read32(trans, HBUS_TARG_MEM_RDAT);
+			if (bufsz) {
+				pos += scnprintf(*buf + pos, bufsz - pos,
+						"EVT_LOGT:%010u:0x%08x:%04u\n",
+						 time, data, ev);
+			} else {
+				IWL_ERR(priv, "EVT_LOGT:%010u:0x%08x:%04u\n",
+					time, data, ev);
+				trace_iwlwifi_dev_ucode_event(trans->dev, time,
+					data, ev);
+			}
+		}
+	}
+
+	/* Allow device to power down */
+	iwl_release_nic_access(trans);
+out_unlock:
+	spin_unlock_irqrestore(&trans->reg_lock, reg_flags);
+	return pos;
+}
+
+/**
+ * iwl_print_last_event_logs - Dump the newest # of event log to syslog
+ */
+static int iwl_print_last_event_logs(struct iwl_priv *priv, u32 capacity,
+				    u32 num_wraps, u32 next_entry,
+				    u32 size, u32 mode,
+				    int pos, char **buf, size_t bufsz)
+{
+	/*
+	 * display the newest DEFAULT_LOG_ENTRIES entries
+	 * i.e the entries just before the next ont that uCode would fill.
+	 */
+	if (num_wraps) {
+		if (next_entry < size) {
+			pos = iwl_print_event_log(priv,
+						capacity - (size - next_entry),
+						size - next_entry, mode,
+						pos, buf, bufsz);
+			pos = iwl_print_event_log(priv, 0,
+						  next_entry, mode,
+						  pos, buf, bufsz);
+		} else
+			pos = iwl_print_event_log(priv, next_entry - size,
+						  size, mode, pos, buf, bufsz);
+	} else {
+		if (next_entry < size) {
+			pos = iwl_print_event_log(priv, 0, next_entry,
+						  mode, pos, buf, bufsz);
+		} else {
+			pos = iwl_print_event_log(priv, next_entry - size,
+						  size, mode, pos, buf, bufsz);
+		}
+	}
+	return pos;
+}
+
+#define DEFAULT_DUMP_EVENT_LOG_ENTRIES (20)
+
+int iwl_dump_nic_event_log(struct iwl_priv *priv, bool full_log,
+			    char **buf, bool display)
+{
+	u32 base;       /* SRAM byte address of event log header */
+	u32 capacity;   /* event log capacity in # entries */
+	u32 mode;       /* 0 - no timestamp, 1 - timestamp recorded */
+	u32 num_wraps;  /* # times uCode wrapped to top of log */
+	u32 next_entry; /* index of next entry to be written by uCode */
+	u32 size;       /* # entries that we'll print */
+	u32 logsize;
+	int pos = 0;
+	size_t bufsz = 0;
+	struct iwl_trans *trans = priv->trans;
+
+	base = priv->device_pointers.log_event_table;
+	if (priv->cur_ucode == IWL_UCODE_INIT) {
+		logsize = priv->fw->init_evtlog_size;
+		if (!base)
+			base = priv->fw->init_evtlog_ptr;
+	} else {
+		logsize = priv->fw->inst_evtlog_size;
+		if (!base)
+			base = priv->fw->inst_evtlog_ptr;
+	}
+
+	if (!iwlagn_hw_valid_rtc_data_addr(base)) {
+		IWL_ERR(priv,
+			"Invalid event log pointer 0x%08X for %s uCode\n",
+			base,
+			(priv->cur_ucode == IWL_UCODE_INIT)
+					? "Init" : "RT");
+		return -EINVAL;
+	}
+
+	/* event log header */
+	capacity = iwl_read_targ_mem(trans, base);
+	mode = iwl_read_targ_mem(trans, base + (1 * sizeof(u32)));
+	num_wraps = iwl_read_targ_mem(trans, base + (2 * sizeof(u32)));
+	next_entry = iwl_read_targ_mem(trans, base + (3 * sizeof(u32)));
+
+	if (capacity > logsize) {
+		IWL_ERR(priv, "Log capacity %d is bogus, limit to %d "
+			"entries\n", capacity, logsize);
+		capacity = logsize;
+	}
+
+	if (next_entry > logsize) {
+		IWL_ERR(priv, "Log write index %d is bogus, limit to %d\n",
+			next_entry, logsize);
+		next_entry = logsize;
+	}
+
+	size = num_wraps ? capacity : next_entry;
+
+	/* bail out if nothing in log */
+	if (size == 0) {
+		IWL_ERR(trans, "Start IWL Event Log Dump: nothing in log\n");
+		return pos;
+	}
+
+#ifdef CONFIG_IWLWIFI_DEBUG
+	if (!(iwl_have_debug_level(IWL_DL_FW_ERRORS)) && !full_log)
+		size = (size > DEFAULT_DUMP_EVENT_LOG_ENTRIES)
+			? DEFAULT_DUMP_EVENT_LOG_ENTRIES : size;
+#else
+	size = (size > DEFAULT_DUMP_EVENT_LOG_ENTRIES)
+		? DEFAULT_DUMP_EVENT_LOG_ENTRIES : size;
+#endif
+	IWL_ERR(priv, "Start IWL Event Log Dump: display last %u entries\n",
+		size);
+
+#ifdef CONFIG_IWLWIFI_DEBUG
+	if (display) {
+		if (full_log)
+			bufsz = capacity * 48;
+		else
+			bufsz = size * 48;
+		*buf = kmalloc(bufsz, GFP_KERNEL);
+		if (!*buf)
+			return -ENOMEM;
+	}
+	if (iwl_have_debug_level(IWL_DL_FW_ERRORS) || full_log) {
+		/*
+		 * if uCode has wrapped back to top of log,
+		 * start at the oldest entry,
+		 * i.e the next one that uCode would fill.
+		 */
+		if (num_wraps)
+			pos = iwl_print_event_log(priv, next_entry,
+						capacity - next_entry, mode,
+						pos, buf, bufsz);
+		/* (then/else) start at top of log */
+		pos = iwl_print_event_log(priv, 0,
+					  next_entry, mode, pos, buf, bufsz);
+	} else
+		pos = iwl_print_last_event_logs(priv, capacity, num_wraps,
+						next_entry, size, mode,
+						pos, buf, bufsz);
+#else
+	pos = iwl_print_last_event_logs(priv, capacity, num_wraps,
+					next_entry, size, mode,
+					pos, buf, bufsz);
+#endif
+	return pos;
+}
+
+static void iwlagn_fw_error(struct iwl_priv *priv, bool ondemand)
+{
+	unsigned int reload_msec;
+	unsigned long reload_jiffies;
+
+#ifdef CONFIG_IWLWIFI_DEBUG
+	if (iwl_have_debug_level(IWL_DL_FW_ERRORS))
+		iwl_print_rx_config_cmd(priv, IWL_RXON_CTX_BSS);
+#endif
+
+	/* uCode is no longer loaded. */
+	priv->ucode_loaded = false;
+
+	/* Set the FW error flag -- cleared on iwl_down */
+	set_bit(STATUS_FW_ERROR, &priv->status);
+
+	iwl_abort_notification_waits(&priv->notif_wait);
+
+	/* Keep the restart process from trying to send host
+	 * commands by clearing the ready bit */
+	clear_bit(STATUS_READY, &priv->status);
+
+	wake_up(&priv->trans->wait_command_queue);
+
+	if (!ondemand) {
+		/*
+		 * If firmware keep reloading, then it indicate something
+		 * serious wrong and firmware having problem to recover
+		 * from it. Instead of keep trying which will fill the syslog
+		 * and hang the system, let's just stop it
+		 */
+		reload_jiffies = jiffies;
+		reload_msec = jiffies_to_msecs((long) reload_jiffies -
+					(long) priv->reload_jiffies);
+		priv->reload_jiffies = reload_jiffies;
+		if (reload_msec <= IWL_MIN_RELOAD_DURATION) {
+			priv->reload_count++;
+			if (priv->reload_count >= IWL_MAX_CONTINUE_RELOAD_CNT) {
+				IWL_ERR(priv, "BUG_ON, Stop restarting\n");
+				return;
+			}
+		} else
+			priv->reload_count = 0;
+	}
+
+	if (!test_bit(STATUS_EXIT_PENDING, &priv->status)) {
+		if (iwlwifi_mod_params.restart_fw) {
+			IWL_DEBUG_FW_ERRORS(priv,
+				  "Restarting adapter due to uCode error.\n");
+			queue_work(priv->workqueue, &priv->restart);
+		} else
+			IWL_DEBUG_FW_ERRORS(priv,
+				  "Detected FW error, but not restarting\n");
+	}
+}
+
+void iwl_nic_error(struct iwl_op_mode *op_mode)
+{
+	struct iwl_priv *priv = IWL_OP_MODE_GET_DVM(op_mode);
+
+	IWL_ERR(priv, "Loaded firmware version: %s\n",
+		priv->fw->fw_version);
+
+	iwl_dump_nic_error_log(priv);
+	iwl_dump_nic_event_log(priv, false, NULL, false);
+
+	iwlagn_fw_error(priv, false);
+}
+
+void iwl_cmd_queue_full(struct iwl_op_mode *op_mode)
 {
 	struct iwl_priv *priv = IWL_OP_MODE_GET_DVM(op_mode);
 
 	if (!iwl_check_for_ct_kill(priv)) {
 		IWL_ERR(priv, "Restarting adapter queue is full\n");
-		iwl_nic_error(op_mode);
+		iwlagn_fw_error(priv, false);
 	}
 }
 
-static void iwl_nic_config(struct iwl_op_mode *op_mode)
+void iwl_nic_config(struct iwl_op_mode *op_mode)
 {
 	struct iwl_priv *priv = IWL_OP_MODE_GET_DVM(op_mode);
 
-	cfg(priv)->lib->nic_config(priv);
+	priv->lib->nic_config(priv);
 }
 
-static void iwl_stop_sw_queue(struct iwl_op_mode *op_mode, u8 ac)
+static void iwl_wimax_active(struct iwl_op_mode *op_mode)
 {
 	struct iwl_priv *priv = IWL_OP_MODE_GET_DVM(op_mode);
 
-	set_bit(ac, &priv->transport_queue_stop);
-	ieee80211_stop_queue(priv->hw, ac);
+	clear_bit(STATUS_READY, &priv->status);
+	IWL_ERR(priv, "RF is used by WiMAX\n");
 }
 
-static void iwl_wake_sw_queue(struct iwl_op_mode *op_mode, u8 ac)
+void iwl_stop_sw_queue(struct iwl_op_mode *op_mode, int queue)
 {
 	struct iwl_priv *priv = IWL_OP_MODE_GET_DVM(op_mode);
+	int mq = priv->queue_to_mac80211[queue];
+
+	if (WARN_ON_ONCE(mq == IWL_INVALID_MAC80211_QUEUE))
+		return;
 
-	clear_bit(ac, &priv->transport_queue_stop);
+	if (atomic_inc_return(&priv->queue_stop_count[mq]) > 1) {
+		IWL_DEBUG_TX_QUEUES(priv,
+			"queue %d (mac80211 %d) already stopped\n",
+			queue, mq);
+		return;
+	}
+
+	set_bit(mq, &priv->transport_queue_stop);
+	ieee80211_stop_queue(priv->hw, mq);
+}
+
+void iwl_wake_sw_queue(struct iwl_op_mode *op_mode, int queue)
+{
+	struct iwl_priv *priv = IWL_OP_MODE_GET_DVM(op_mode);
+	int mq = priv->queue_to_mac80211[queue];
+
+	if (WARN_ON_ONCE(mq == IWL_INVALID_MAC80211_QUEUE))
+		return;
+
+	if (atomic_dec_return(&priv->queue_stop_count[mq]) > 0) {
+		IWL_DEBUG_TX_QUEUES(priv,
+			"queue %d (mac80211 %d) already awake\n",
+			queue, mq);
+		return;
+	}
+
+	clear_bit(mq, &priv->transport_queue_stop);
 
 	if (!priv->passive_no_rx)
-		ieee80211_wake_queue(priv->hw, ac);
+		ieee80211_wake_queue(priv->hw, mq);
 }
 
 void iwlagn_lift_passive_no_rx(struct iwl_priv *priv)
 {
-	int ac;
+	int mq;
 
 	if (!priv->passive_no_rx)
 		return;
 
-	for (ac = IEEE80211_AC_VO; ac < IEEE80211_NUM_ACS; ac++) {
-		if (!test_bit(ac, &priv->transport_queue_stop)) {
-			IWL_DEBUG_TX_QUEUES(priv, "Wake queue %d");
-			ieee80211_wake_queue(priv->hw, ac);
+	for (mq = 0; mq < IWLAGN_FIRST_AMPDU_QUEUE; mq++) {
+		if (!test_bit(mq, &priv->transport_queue_stop)) {
+			IWL_DEBUG_TX_QUEUES(priv, "Wake queue %d", mq);
+			ieee80211_wake_queue(priv->hw, mq);
 		} else {
-			IWL_DEBUG_TX_QUEUES(priv, "Don't wake queue %d");
+			IWL_DEBUG_TX_QUEUES(priv, "Don't wake queue %d", mq);
 		}
 	}
 
 	priv->passive_no_rx = false;
 }
 
+void iwl_free_skb(struct iwl_op_mode *op_mode, struct sk_buff *skb)
+{
+	struct ieee80211_tx_info *info;
+
+	info = IEEE80211_SKB_CB(skb);
+	kmem_cache_free(iwl_tx_cmd_pool, (info->driver_data[1]));
+	dev_kfree_skb_any(skb);
+}
+
+void iwl_set_hw_rfkill_state(struct iwl_op_mode *op_mode, bool state)
+{
+	struct iwl_priv *priv = IWL_OP_MODE_GET_DVM(op_mode);
+
+	if (state)
+		set_bit(STATUS_RF_KILL_HW, &priv->status);
+	else
+		clear_bit(STATUS_RF_KILL_HW, &priv->status);
+
+	wiphy_rfkill_set_hw_state(priv->hw->wiphy, state);
+}
+
 const struct iwl_op_mode_ops iwl_dvm_ops = {
 	.start = iwl_op_mode_dvm_start,
 	.stop = iwl_op_mode_dvm_stop,
@@ -1491,6 +2314,7 @@ const struct iwl_op_mode_ops iwl_dvm_ops = {
 	.nic_error = iwl_nic_error,
 	.cmd_queue_full = iwl_cmd_queue_full,
 	.nic_config = iwl_nic_config,
+	.wimax_active = iwl_wimax_active,
 };
 
 /*****************************************************************************
@@ -1541,96 +2365,3 @@ static void __exit iwl_exit(void)
 
 module_exit(iwl_exit);
 module_init(iwl_init);
-
-#ifdef CONFIG_IWLWIFI_DEBUG
-module_param_named(debug, iwlagn_mod_params.debug_level, uint,
-		   S_IRUGO | S_IWUSR);
-MODULE_PARM_DESC(debug, "debug output mask");
-#endif
-
-module_param_named(swcrypto, iwlagn_mod_params.sw_crypto, int, S_IRUGO);
-MODULE_PARM_DESC(swcrypto, "using crypto in software (default 0 [hardware])");
-module_param_named(11n_disable, iwlagn_mod_params.disable_11n, uint, S_IRUGO);
-MODULE_PARM_DESC(11n_disable,
-	"disable 11n functionality, bitmap: 1: full, 2: agg TX, 4: agg RX");
-module_param_named(amsdu_size_8K, iwlagn_mod_params.amsdu_size_8K,
-		   int, S_IRUGO);
-MODULE_PARM_DESC(amsdu_size_8K, "enable 8K amsdu size");
-module_param_named(fw_restart, iwlagn_mod_params.restart_fw, int, S_IRUGO);
-MODULE_PARM_DESC(fw_restart, "restart firmware in case of error");
-
-module_param_named(ucode_alternative,
-		   iwlagn_mod_params.wanted_ucode_alternative,
-		   int, S_IRUGO);
-MODULE_PARM_DESC(ucode_alternative,
-		 "specify ucode alternative to use from ucode file");
-
-module_param_named(antenna_coupling, iwlagn_mod_params.ant_coupling,
-		   int, S_IRUGO);
-MODULE_PARM_DESC(antenna_coupling,
-		 "specify antenna coupling in dB (defualt: 0 dB)");
-
-module_param_named(bt_ch_inhibition, iwlagn_mod_params.bt_ch_announce,
-		   bool, S_IRUGO);
-MODULE_PARM_DESC(bt_ch_inhibition,
-		 "Enable BT channel inhibition (default: enable)");
-
-module_param_named(plcp_check, iwlagn_mod_params.plcp_check, bool, S_IRUGO);
-MODULE_PARM_DESC(plcp_check, "Check plcp health (default: 1 [enabled])");
-
-module_param_named(ack_check, iwlagn_mod_params.ack_check, bool, S_IRUGO);
-MODULE_PARM_DESC(ack_check, "Check ack health (default: 0 [disabled])");
-
-module_param_named(wd_disable, iwlagn_mod_params.wd_disable, int, S_IRUGO);
-MODULE_PARM_DESC(wd_disable,
-		"Disable stuck queue watchdog timer 0=system default, "
-		"1=disable, 2=enable (default: 0)");
-
-/*
- * set bt_coex_active to true, uCode will do kill/defer
- * every time the priority line is asserted (BT is sending signals on the
- * priority line in the PCIx).
- * set bt_coex_active to false, uCode will ignore the BT activity and
- * perform the normal operation
- *
- * User might experience transmit issue on some platform due to WiFi/BT
- * co-exist problem. The possible behaviors are:
- *   Able to scan and finding all the available AP
- *   Not able to associate with any AP
- * On those platforms, WiFi communication can be restored by set
- * "bt_coex_active" module parameter to "false"
- *
- * default: bt_coex_active = true (BT_COEX_ENABLE)
- */
-module_param_named(bt_coex_active, iwlagn_mod_params.bt_coex_active,
-		bool, S_IRUGO);
-MODULE_PARM_DESC(bt_coex_active, "enable wifi/bt co-exist (default: enable)");
-
-module_param_named(led_mode, iwlagn_mod_params.led_mode, int, S_IRUGO);
-MODULE_PARM_DESC(led_mode, "0=system default, "
-		"1=On(RF On)/Off(RF Off), 2=blinking, 3=Off (default: 0)");
-
-module_param_named(power_save, iwlagn_mod_params.power_save,
-		bool, S_IRUGO);
-MODULE_PARM_DESC(power_save,
-		 "enable WiFi power management (default: disable)");
-
-module_param_named(power_level, iwlagn_mod_params.power_level,
-		int, S_IRUGO);
-MODULE_PARM_DESC(power_level,
-		 "default power save level (range from 1 - 5, default: 1)");
-
-module_param_named(auto_agg, iwlagn_mod_params.auto_agg,
-		bool, S_IRUGO);
-MODULE_PARM_DESC(auto_agg,
-		 "enable agg w/o check traffic load (default: enable)");
-
-/*
- * For now, keep using power level 1 instead of automatically
- * adjusting ...
- */
-module_param_named(no_sleep_autoadjust, iwlagn_mod_params.no_sleep_autoadjust,
-		bool, S_IRUGO);
-MODULE_PARM_DESC(no_sleep_autoadjust,
-		 "don't automatically adjust sleep level "
-		 "according to maximum network latency (default: true)");
diff --git a/drivers/net/wireless/iwlwifi/iwl-agn.h b/drivers/net/wireless/iwlwifi/iwl-agn.h
index 3780a03f271..79c0fe06f4d 100644
--- a/drivers/net/wireless/iwlwifi/iwl-agn.h
+++ b/drivers/net/wireless/iwlwifi/iwl-agn.h
@@ -64,6 +64,43 @@
 #define __iwl_agn_h__
 
 #include "iwl-dev.h"
+#include "iwl-config.h"
+
+/* The first 11 queues (0-10) are used otherwise */
+#define IWLAGN_FIRST_AMPDU_QUEUE	11
+
+/* AUX (TX during scan dwell) queue */
+#define IWL_AUX_QUEUE		10
+
+/* device operations */
+extern struct iwl_lib_ops iwl1000_lib;
+extern struct iwl_lib_ops iwl2000_lib;
+extern struct iwl_lib_ops iwl2030_lib;
+extern struct iwl_lib_ops iwl5000_lib;
+extern struct iwl_lib_ops iwl5150_lib;
+extern struct iwl_lib_ops iwl6000_lib;
+extern struct iwl_lib_ops iwl6030_lib;
+
+
+#define TIME_UNIT		1024
+
+/*****************************************************
+* DRIVER STATUS FUNCTIONS
+******************************************************/
+#define STATUS_RF_KILL_HW	0
+#define STATUS_CT_KILL		1
+#define STATUS_ALIVE		2
+#define STATUS_READY		3
+#define STATUS_GEO_CONFIGURED	4
+#define STATUS_EXIT_PENDING	5
+#define STATUS_STATISTICS	6
+#define STATUS_SCANNING		7
+#define STATUS_SCAN_ABORTING	8
+#define STATUS_SCAN_HW		9
+#define STATUS_FW_ERROR		10
+#define STATUS_CHANNEL_SWITCH_PENDING 11
+#define STATUS_SCAN_COMPLETE	12
+#define STATUS_POWER_PMI	13
 
 struct iwl_ucode_capabilities;
 
@@ -80,12 +117,9 @@ static inline void iwl_set_calib_hdr(struct iwl_calib_hdr *hdr, u8 cmd)
 void iwl_down(struct iwl_priv *priv);
 void iwl_cancel_deferred_work(struct iwl_priv *priv);
 void iwlagn_prepare_restart(struct iwl_priv *priv);
-void iwl_free_skb(struct iwl_op_mode *op_mode, struct sk_buff *skb);
 int __must_check iwl_rx_dispatch(struct iwl_op_mode *op_mode,
 				 struct iwl_rx_cmd_buffer *rxb,
 				 struct iwl_device_cmd *cmd);
-void iwl_set_hw_rfkill_state(struct iwl_op_mode *op_mode, bool state);
-void iwl_nic_error(struct iwl_op_mode *op_mode);
 
 bool iwl_check_for_ct_kill(struct iwl_priv *priv);
 
@@ -103,6 +137,8 @@ int iwl_dvm_send_cmd_pdu(struct iwl_priv *priv, u8 id,
 			 u32 flags, u16 len, const void *data);
 
 /* RXON */
+void iwl_connection_init_rx_config(struct iwl_priv *priv,
+				   struct iwl_rxon_context *ctx);
 int iwlagn_set_pan_params(struct iwl_priv *priv);
 int iwlagn_commit_rxon(struct iwl_priv *priv, struct iwl_rxon_context *ctx);
 void iwlagn_set_rxon_chain(struct iwl_priv *priv, struct iwl_rxon_context *ctx);
@@ -113,11 +149,15 @@ void iwlagn_bss_info_changed(struct ieee80211_hw *hw,
 			     u32 changes);
 void iwlagn_config_ht40(struct ieee80211_conf *conf,
 			struct iwl_rxon_context *ctx);
+void iwl_set_rxon_ht(struct iwl_priv *priv, struct iwl_ht_config *ht_conf);
+void iwl_set_rxon_channel(struct iwl_priv *priv, struct ieee80211_channel *ch,
+			 struct iwl_rxon_context *ctx);
+void iwl_set_flags_for_band(struct iwl_priv *priv,
+			    struct iwl_rxon_context *ctx,
+			    enum ieee80211_band band,
+			    struct ieee80211_vif *vif);
 
 /* uCode */
-int iwlagn_rx_calib_result(struct iwl_priv *priv,
-			    struct iwl_rx_cmd_buffer *rxb,
-			    struct iwl_device_cmd *cmd);
 int iwl_send_bt_env(struct iwl_priv *priv, u8 action, u8 type);
 void iwl_send_prio_tbl(struct iwl_priv *priv);
 int iwl_init_alive_start(struct iwl_priv *priv);
@@ -128,14 +168,25 @@ int iwl_send_calib_results(struct iwl_priv *priv);
 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);
+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);
-u16 iwl_eeprom_calib_version(struct iwl_shared *shrd);
 int iwlagn_txfifo_flush(struct iwl_priv *priv, u16 flush_control);
 void iwlagn_dev_txfifo_flush(struct iwl_priv *priv, u16 flush_control);
 int iwlagn_send_beacon_cmd(struct iwl_priv *priv);
+int iwl_send_statistics_request(struct iwl_priv *priv,
+				u8 flags, bool clear);
+
+static inline const struct ieee80211_supported_band *iwl_get_hw_mode(
+			struct iwl_priv *priv, enum ieee80211_band band)
+{
+	return priv->hw->wiphy->bands[band];
+}
+
 #ifdef CONFIG_PM_SLEEP
 int iwlagn_send_patterns(struct iwl_priv *priv,
 			 struct cfg80211_wowlan *wowlan);
@@ -145,6 +196,7 @@ int iwlagn_suspend(struct iwl_priv *priv, struct cfg80211_wowlan *wowlan);
 /* rx */
 int iwlagn_hwrate_to_mac80211_idx(u32 rate_n_flags, enum ieee80211_band band);
 void iwl_setup_rx_handlers(struct iwl_priv *priv);
+void iwl_chswitch_done(struct iwl_priv *priv, bool is_success);
 
 
 /* tx */
@@ -189,6 +241,31 @@ u8 iwl_toggle_tx_ant(struct iwl_priv *priv, u8 ant_idx, u8 valid);
 /* scan */
 void iwlagn_post_scan(struct iwl_priv *priv);
 void iwlagn_disable_roc(struct iwl_priv *priv);
+int iwl_force_rf_reset(struct iwl_priv *priv, bool external);
+void iwl_init_scan_params(struct iwl_priv *priv);
+int iwl_scan_cancel(struct iwl_priv *priv);
+void iwl_scan_cancel_timeout(struct iwl_priv *priv, unsigned long ms);
+void iwl_force_scan_end(struct iwl_priv *priv);
+void iwl_internal_short_hw_scan(struct iwl_priv *priv);
+void iwl_setup_rx_scan_handlers(struct iwl_priv *priv);
+void iwl_setup_scan_deferred_work(struct iwl_priv *priv);
+void iwl_cancel_scan_deferred_work(struct iwl_priv *priv);
+int __must_check iwl_scan_initiate(struct iwl_priv *priv,
+				   struct ieee80211_vif *vif,
+				   enum iwl_scan_type scan_type,
+				   enum ieee80211_band band);
+
+/* For faster active scanning, scan will move to the next channel if fewer than
+ * PLCP_QUIET_THRESH packets are heard on this channel within
+ * ACTIVE_QUIET_TIME after sending probe request.  This shortens the dwell
+ * time if it's a quiet channel (nothing responded to our probe, and there's
+ * no other traffic).
+ * Disable "quiet" feature by setting PLCP_QUIET_THRESH to 0. */
+#define IWL_ACTIVE_QUIET_TIME       cpu_to_le16(10)  /* msec */
+#define IWL_PLCP_QUIET_THRESH       cpu_to_le16(1)  /* packets */
+
+#define IWL_SCAN_CHECK_WATCHDOG		(HZ * 7)
+
 
 /* bt coex */
 void iwlagn_send_advance_bt_config(struct iwl_priv *priv);
@@ -201,6 +278,12 @@ void iwlagn_bt_cancel_deferred_work(struct iwl_priv *priv);
 void iwlagn_bt_coex_rssi_monitor(struct iwl_priv *priv);
 void iwlagn_bt_adjust_rssi_monitor(struct iwl_priv *priv, bool rssi_ena);
 
+static inline bool iwl_advanced_bt_coexist(struct iwl_priv *priv)
+{
+	return priv->cfg->bt_params &&
+	       priv->cfg->bt_params->advanced_bt_coexist;
+}
+
 #ifdef CONFIG_IWLWIFI_DEBUG
 const char *iwl_get_tx_fail_reason(u32 status);
 const char *iwl_get_agg_tx_fail_reason(u16 status);
@@ -239,8 +322,6 @@ void iwl_deactivate_station(struct iwl_priv *priv, const u8 sta_id,
 u8 iwl_prep_station(struct iwl_priv *priv, struct iwl_rxon_context *ctx,
 		    const u8 *addr, bool is_ap, struct ieee80211_sta *sta);
 
-void iwl_sta_fill_lq(struct iwl_priv *priv, struct iwl_rxon_context *ctx,
-		     u8 sta_id, struct iwl_link_quality_cmd *link_cmd);
 int iwl_send_lq_cmd(struct iwl_priv *priv, struct iwl_rxon_context *ctx,
 		    struct iwl_link_quality_cmd *lq, u8 flags, bool init);
 int iwl_add_sta_callback(struct iwl_priv *priv, struct iwl_rx_cmd_buffer *rxb,
@@ -248,6 +329,9 @@ int iwl_add_sta_callback(struct iwl_priv *priv, struct iwl_rx_cmd_buffer *rxb,
 int iwl_sta_update_ht(struct iwl_priv *priv, struct iwl_rxon_context *ctx,
 		      struct ieee80211_sta *sta);
 
+bool iwl_is_ht40_tx_allowed(struct iwl_priv *priv,
+			    struct iwl_rxon_context *ctx,
+			    struct ieee80211_sta_ht_cap *ht_cap);
 
 static inline int iwl_sta_id(struct ieee80211_sta *sta)
 {
@@ -305,9 +389,6 @@ static inline __le32 iwl_hw_set_rate_n_flags(u8 rate, u32 flags)
 	return cpu_to_le32(flags|(u32)rate);
 }
 
-/* eeprom */
-void iwl_eeprom_get_mac(const struct iwl_shared *shrd, u8 *mac);
-
 extern int iwl_alive_start(struct iwl_priv *priv);
 /* svtool */
 #ifdef CONFIG_IWLWIFI_DEVICE_TESTMODE
@@ -386,13 +467,35 @@ static inline int iwl_is_ready_rf(struct iwl_priv *priv)
 	return iwl_is_ready(priv);
 }
 
+static inline void iwl_dvm_set_pmi(struct iwl_priv *priv, bool state)
+{
+	if (state)
+		set_bit(STATUS_POWER_PMI, &priv->status);
+	else
+		clear_bit(STATUS_POWER_PMI, &priv->status);
+	iwl_trans_set_pmi(priv->trans, state);
+}
+
+#ifdef CONFIG_IWLWIFI_DEBUGFS
+int iwl_dbgfs_register(struct iwl_priv *priv, const char *name);
+void iwl_dbgfs_unregister(struct iwl_priv *priv);
+#else
+static inline int iwl_dbgfs_register(struct iwl_priv *priv, const char *name)
+{
+	return 0;
+}
+static inline void iwl_dbgfs_unregister(struct iwl_priv *priv)
+{
+}
+#endif /* CONFIG_IWLWIFI_DEBUGFS */
+
 #ifdef CONFIG_IWLWIFI_DEBUG
 #define IWL_DEBUG_QUIET_RFKILL(m, fmt, args...)	\
 do {									\
 	if (!iwl_is_rfkill((m)))					\
 		IWL_ERR(m, fmt, ##args);				\
 	else								\
-		__iwl_err(trans(m)->dev, true,				\
+		__iwl_err((m)->dev, true,				\
 			  !iwl_have_debug_level(IWL_DL_RADIO),		\
 			  fmt, ##args);					\
 } while (0)
@@ -402,8 +505,98 @@ do {									\
 	if (!iwl_is_rfkill((m)))					\
 		IWL_ERR(m, fmt, ##args);				\
 	else								\
-		__iwl_err(trans(m)->dev, true, true, fmt, ##args);	\
+		__iwl_err((m)->dev, true, true, fmt, ##args);	\
 } while (0)
 #endif				/* CONFIG_IWLWIFI_DEBUG */
 
+extern const char *iwl_dvm_cmd_strings[REPLY_MAX];
+
+static inline const char *iwl_dvm_get_cmd_string(u8 cmd)
+{
+	const char *s = iwl_dvm_cmd_strings[cmd];
+	if (s)
+		return s;
+	return "UNKNOWN";
+}
+
+/* API method exported for mvm hybrid state */
+void iwl_setup_deferred_work(struct iwl_priv *priv);
+int iwl_send_wimax_coex(struct iwl_priv *priv);
+int iwl_send_bt_env(struct iwl_priv *priv, u8 action, u8 type);
+void iwl_option_config(struct iwl_priv *priv);
+void iwl_set_hw_params(struct iwl_priv *priv);
+void iwl_init_context(struct iwl_priv *priv, u32 ucode_flags);
+int iwl_init_drv(struct iwl_priv *priv);
+void iwl_uninit_drv(struct iwl_priv *priv);
+void iwl_send_bt_config(struct iwl_priv *priv);
+void iwl_rf_kill_ct_config(struct iwl_priv *priv);
+int iwl_setup_interface(struct iwl_priv *priv, struct iwl_rxon_context *ctx);
+void iwl_teardown_interface(struct iwl_priv *priv,
+			    struct ieee80211_vif *vif,
+			    bool mode_change);
+int iwl_full_rxon_required(struct iwl_priv *priv, struct iwl_rxon_context *ctx);
+void iwlagn_update_qos(struct iwl_priv *priv, struct iwl_rxon_context *ctx);
+void iwlagn_check_needed_chains(struct iwl_priv *priv,
+				struct iwl_rxon_context *ctx,
+				struct ieee80211_bss_conf *bss_conf);
+void iwlagn_chain_noise_reset(struct iwl_priv *priv);
+int iwlagn_update_beacon(struct iwl_priv *priv,
+			 struct ieee80211_vif *vif);
+void iwl_tt_handler(struct iwl_priv *priv);
+void iwl_op_mode_dvm_stop(struct iwl_op_mode *op_mode);
+void iwl_stop_sw_queue(struct iwl_op_mode *op_mode, int queue);
+void iwl_set_hw_rfkill_state(struct iwl_op_mode *op_mode, bool state);
+void iwl_free_skb(struct iwl_op_mode *op_mode, struct sk_buff *skb);
+void iwl_nic_error(struct iwl_op_mode *op_mode);
+void iwl_cmd_queue_full(struct iwl_op_mode *op_mode);
+void iwl_nic_config(struct iwl_op_mode *op_mode);
+int iwlagn_mac_set_tim(struct ieee80211_hw *hw,
+		       struct ieee80211_sta *sta, bool set);
+void iwlagn_mac_rssi_callback(struct ieee80211_hw *hw,
+			      enum ieee80211_rssi_event rssi_event);
+int iwlagn_mac_cancel_remain_on_channel(struct ieee80211_hw *hw);
+int iwlagn_mac_tx_last_beacon(struct ieee80211_hw *hw);
+void iwlagn_mac_flush(struct ieee80211_hw *hw, bool drop);
+void iwl_wake_sw_queue(struct iwl_op_mode *op_mode, int queue);
+void iwlagn_mac_channel_switch(struct ieee80211_hw *hw,
+			       struct ieee80211_channel_switch *ch_switch);
+int iwlagn_mac_sta_state(struct ieee80211_hw *hw,
+			 struct ieee80211_vif *vif,
+			 struct ieee80211_sta *sta,
+			 enum ieee80211_sta_state old_state,
+			 enum ieee80211_sta_state new_state);
+int iwlagn_mac_ampdu_action(struct ieee80211_hw *hw,
+			    struct ieee80211_vif *vif,
+			    enum ieee80211_ampdu_mlme_action action,
+			    struct ieee80211_sta *sta, u16 tid, u16 *ssn,
+			    u8 buf_size);
+int iwlagn_mac_hw_scan(struct ieee80211_hw *hw,
+		       struct ieee80211_vif *vif,
+		       struct cfg80211_scan_request *req);
+void iwlagn_mac_sta_notify(struct ieee80211_hw *hw,
+			   struct ieee80211_vif *vif,
+			   enum sta_notify_cmd cmd,
+			   struct ieee80211_sta *sta);
+void iwlagn_configure_filter(struct ieee80211_hw *hw,
+			     unsigned int changed_flags,
+			     unsigned int *total_flags,
+			     u64 multicast);
+int iwlagn_mac_conf_tx(struct ieee80211_hw *hw,
+		       struct ieee80211_vif *vif, u16 queue,
+		       const struct ieee80211_tx_queue_params *params);
+void iwlagn_mac_set_rekey_data(struct ieee80211_hw *hw,
+			       struct ieee80211_vif *vif,
+			       struct cfg80211_gtk_rekey_data *data);
+void iwlagn_mac_update_tkip_key(struct ieee80211_hw *hw,
+				struct ieee80211_vif *vif,
+				struct ieee80211_key_conf *keyconf,
+				struct ieee80211_sta *sta,
+				u32 iv32, u16 *phase1key);
+int iwlagn_mac_set_key(struct ieee80211_hw *hw, enum set_key_cmd cmd,
+		       struct ieee80211_vif *vif,
+		       struct ieee80211_sta *sta,
+		       struct ieee80211_key_conf *key);
+void iwlagn_mac_stop(struct ieee80211_hw *hw);
+void iwlagn_mac_tx(struct ieee80211_hw *hw, struct sk_buff *skb);
+int iwlagn_mac_suspend(struct ieee80211_hw *hw, struct cfg80211_wowlan *wowlan);
 #endif /* __iwl_agn_h__ */
diff --git a/drivers/net/wireless/iwlwifi/iwl-commands.h b/drivers/net/wireless/iwlwifi/iwl-commands.h
index 9ed73e5154b..9af6a239b38 100644
--- a/drivers/net/wireless/iwlwifi/iwl-commands.h
+++ b/drivers/net/wireless/iwlwifi/iwl-commands.h
@@ -1877,9 +1877,16 @@ struct iwl_bt_cmd {
 
 #define IWLAGN_BT3_T7_DEFAULT		1
 
+enum iwl_bt_kill_idx {
+	IWL_BT_KILL_DEFAULT = 0,
+	IWL_BT_KILL_OVERRIDE = 1,
+	IWL_BT_KILL_REDUCE = 2,
+};
+
 #define IWLAGN_BT_KILL_ACK_MASK_DEFAULT	cpu_to_le32(0xffff0000)
 #define IWLAGN_BT_KILL_CTS_MASK_DEFAULT	cpu_to_le32(0xffff0000)
 #define IWLAGN_BT_KILL_ACK_CTS_MASK_SCO	cpu_to_le32(0xffffffff)
+#define IWLAGN_BT_KILL_ACK_CTS_MASK_REDUCE	cpu_to_le32(0)
 
 #define IWLAGN_BT3_PRIO_SAMPLE_DEFAULT	2
 
@@ -1891,7 +1898,7 @@ struct iwl_bt_cmd {
 #define IWLAGN_BT_VALID_3W_TIMERS	cpu_to_le16(BIT(3))
 #define IWLAGN_BT_VALID_KILL_ACK_MASK	cpu_to_le16(BIT(4))
 #define IWLAGN_BT_VALID_KILL_CTS_MASK	cpu_to_le16(BIT(5))
-#define IWLAGN_BT_VALID_BT4_TIMES	cpu_to_le16(BIT(6))
+#define IWLAGN_BT_VALID_REDUCED_TX_PWR	cpu_to_le16(BIT(6))
 #define IWLAGN_BT_VALID_3W_LUT		cpu_to_le16(BIT(7))
 
 #define IWLAGN_BT_ALL_VALID_MSK		(IWLAGN_BT_VALID_ENABLE_FLAGS | \
@@ -1900,9 +1907,13 @@ struct iwl_bt_cmd {
 					IWLAGN_BT_VALID_3W_TIMERS | \
 					IWLAGN_BT_VALID_KILL_ACK_MASK | \
 					IWLAGN_BT_VALID_KILL_CTS_MASK | \
-					IWLAGN_BT_VALID_BT4_TIMES | \
+					IWLAGN_BT_VALID_REDUCED_TX_PWR | \
 					IWLAGN_BT_VALID_3W_LUT)
 
+#define IWLAGN_BT_REDUCED_TX_PWR	BIT(0)
+
+#define IWLAGN_BT_DECISION_LUT_SIZE	12
+
 struct iwl_basic_bt_cmd {
 	u8 flags;
 	u8 ledtime; /* unused */
@@ -1913,12 +1924,17 @@ struct iwl_basic_bt_cmd {
 	u8 bt3_prio_sample_time;
 	u8 bt3_timer_t2_value;
 	__le16 bt4_reaction_time; /* unused */
-	__le32 bt3_lookup_table[12];
-	__le16 bt4_decision_time; /* unused */
+	__le32 bt3_lookup_table[IWLAGN_BT_DECISION_LUT_SIZE];
+	/*
+	 * bit 0: use reduced tx power for control frame
+	 * bit 1 - 7: reserved
+	 */
+	u8 reduce_txpower;
+	u8 reserved;
 	__le16 valid;
 };
 
-struct iwl6000_bt_cmd {
+struct iwl_bt_cmd_v1 {
 	struct iwl_basic_bt_cmd basic;
 	u8 prio_boost;
 	/*
@@ -1929,7 +1945,7 @@ struct iwl6000_bt_cmd {
 	__le16 rx_prio_boost;	/* SW boost of WiFi rx priority */
 };
 
-struct iwl2000_bt_cmd {
+struct iwl_bt_cmd_v2 {
 	struct iwl_basic_bt_cmd basic;
 	__le32 prio_boost;
 	/*
@@ -2262,7 +2278,6 @@ struct iwl_ssid_ie {
 #define IWL_GOOD_CRC_TH_DISABLED	0
 #define IWL_GOOD_CRC_TH_DEFAULT		cpu_to_le16(1)
 #define IWL_GOOD_CRC_TH_NEVER		cpu_to_le16(0xffff)
-#define IWL_MAX_SCAN_SIZE 1024
 #define IWL_MAX_CMD_SIZE 4096
 
 /*
@@ -3634,6 +3649,9 @@ enum iwl_bt_coex_profile_traffic_load {
 		(0x3<<BT_UART_MSG_2_FRAME7RESERVED_POS)
 
 
+#define BT_ENABLE_REDUCED_TXPOWER_THRESHOLD	(-62)
+#define BT_DISABLE_REDUCED_TXPOWER_THRESHOLD	(-65)
+
 struct iwl_bt_uart_msg {
 	u8 header;
 	u8 frame1;
diff --git a/drivers/net/wireless/iwlwifi/iwl-shared.h b/drivers/net/wireless/iwlwifi/iwl-config.h
index b515d657a0a..67b28aa7f9b 100644
--- a/drivers/net/wireless/iwlwifi/iwl-shared.h
+++ b/drivers/net/wireless/iwlwifi/iwl-config.h
@@ -60,136 +60,29 @@
  * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
  *
  *****************************************************************************/
-#ifndef __iwl_shared_h__
-#define __iwl_shared_h__
+#ifndef __IWL_CONFIG_H__
+#define __IWL_CONFIG_H__
 
 #include <linux/types.h>
-#include <linux/spinlock.h>
-#include <linux/gfp.h>
 #include <net/mac80211.h>
 
-#include "iwl-commands.h"
-#include "iwl-fw.h"
 
-/**
- * DOC: shared area - role and goal
- *
- * The shared area contains all the data exported by the upper layer to the
- * other layers. Since the bus and transport layer shouldn't dereference
- * iwl_priv, all the data needed by the upper layer and the transport / bus
- * layer must be here.
- * The shared area also holds pointer to all the other layers. This allows a
- * layer to call a function from another layer.
- *
- * NOTE: All the layers hold a pointer to the shared area which must be shrd.
- *	A few macros assume that (_m)->shrd points to the shared area no matter
- *	what _m is.
- *
- * gets notifications about enumeration, suspend, resume.
- * For the moment, the bus layer is not a linux kernel module as itself, and
- * the module_init function of the driver must call the bus specific
- * registration functions. These functions are listed at the end of this file.
- * For the moment, there is only one implementation of this interface: PCI-e.
- * This implementation is iwl-pci.c
- */
-
-struct iwl_priv;
-struct iwl_trans;
-struct iwl_sensitivity_ranges;
-struct iwl_trans_ops;
-
-#define DRV_NAME        "iwlwifi"
-#define IWLWIFI_VERSION "in-tree:"
-#define DRV_COPYRIGHT	"Copyright(c) 2003-2012 Intel Corporation"
-#define DRV_AUTHOR     "<ilw@linux.intel.com>"
-
-extern struct iwl_mod_params iwlagn_mod_params;
-
-#define IWL_DISABLE_HT_ALL	BIT(0)
-#define IWL_DISABLE_HT_TXAGG	BIT(1)
-#define IWL_DISABLE_HT_RXAGG	BIT(2)
-
-/**
- * struct iwl_mod_params
- *
- * Holds the module parameters
- *
- * @sw_crypto: using hardware encryption, default = 0
- * @disable_11n: disable 11n capabilities, default = 0,
- *	use IWL_DISABLE_HT_* constants
- * @amsdu_size_8K: enable 8K amsdu size, default = 1
- * @antenna: both antennas (use diversity), default = 0
- * @restart_fw: restart firmware, default = 1
- * @plcp_check: enable plcp health check, default = true
- * @ack_check: disable ack health check, default = false
- * @wd_disable: enable stuck queue check, default = 0
- * @bt_coex_active: enable bt coex, default = true
- * @led_mode: system default, default = 0
- * @no_sleep_autoadjust: disable autoadjust, default = true
- * @power_save: disable power save, default = false
- * @power_level: power level, default = 1
- * @debug_level: levels are IWL_DL_*
- * @ant_coupling: antenna coupling in dB, default = 0
- * @bt_ch_announce: BT channel inhibition, default = enable
- * @wanted_ucode_alternative: ucode alternative to use, default = 1
- * @auto_agg: enable agg. without check, default = true
- */
-struct iwl_mod_params {
-	int sw_crypto;
-	unsigned int disable_11n;
-	int amsdu_size_8K;
-	int antenna;
-	int restart_fw;
-	bool plcp_check;
-	bool ack_check;
-	int  wd_disable;
-	bool bt_coex_active;
-	int led_mode;
-	bool no_sleep_autoadjust;
-	bool power_save;
-	int power_level;
-	u32 debug_level;
-	int ant_coupling;
-	bool bt_ch_announce;
-	int wanted_ucode_alternative;
-	bool auto_agg;
-};
-
-/**
- * struct iwl_hw_params
- *
- * Holds the module parameters
- *
- * @num_ampdu_queues: num of ampdu queues
- * @tx_chains_num: Number of TX chains
- * @rx_chains_num: Number of RX chains
- * @valid_tx_ant: usable antennas for TX
- * @valid_rx_ant: usable antennas for RX
- * @ht40_channel: is 40MHz width possible: BIT(IEEE80211_BAND_XXX)
- * @sku: sku read from EEPROM
- * @rx_page_order: Rx buffer page order
- * @ct_kill_threshold: temperature threshold - in hw dependent unit
- * @ct_kill_exit_threshold: when to reeable the device - in hw dependent unit
- *	relevant for 1000, 6000 and up
- * @wd_timeout: TX queues watchdog timeout
- * @struct iwl_sensitivity_ranges: range of sensitivity values
- * @use_rts_for_aggregation: use rts/cts protection for HT traffic
- */
-struct iwl_hw_params {
-	u8  num_ampdu_queues;
-	u8  tx_chains_num;
-	u8  rx_chains_num;
-	u8  valid_tx_ant;
-	u8  valid_rx_ant;
-	u8  ht40_channel;
-	bool use_rts_for_aggregation;
-	u16 sku;
-	u32 rx_page_order;
-	u32 ct_kill_threshold;
-	u32 ct_kill_exit_threshold;
-	unsigned int wd_timeout;
-
-	const struct iwl_sensitivity_ranges *sens;
+enum iwl_device_family {
+	IWL_DEVICE_FAMILY_UNDEFINED,
+	IWL_DEVICE_FAMILY_1000,
+	IWL_DEVICE_FAMILY_100,
+	IWL_DEVICE_FAMILY_2000,
+	IWL_DEVICE_FAMILY_2030,
+	IWL_DEVICE_FAMILY_105,
+	IWL_DEVICE_FAMILY_135,
+	IWL_DEVICE_FAMILY_5000,
+	IWL_DEVICE_FAMILY_5150,
+	IWL_DEVICE_FAMILY_6000,
+	IWL_DEVICE_FAMILY_6000i,
+	IWL_DEVICE_FAMILY_6005,
+	IWL_DEVICE_FAMILY_6030,
+	IWL_DEVICE_FAMILY_6050,
+	IWL_DEVICE_FAMILY_6150,
 };
 
 /*
@@ -209,6 +102,34 @@ enum iwl_led_mode {
 };
 
 /*
+ * This is the threshold value of plcp error rate per 100mSecs.  It is
+ * used to set and check for the validity of plcp_delta.
+ */
+#define IWL_MAX_PLCP_ERR_THRESHOLD_MIN		1
+#define IWL_MAX_PLCP_ERR_THRESHOLD_DEF		50
+#define IWL_MAX_PLCP_ERR_LONG_THRESHOLD_DEF	100
+#define IWL_MAX_PLCP_ERR_EXT_LONG_THRESHOLD_DEF	200
+#define IWL_MAX_PLCP_ERR_THRESHOLD_MAX		255
+#define IWL_MAX_PLCP_ERR_THRESHOLD_DISABLE	0
+
+/* TX queue watchdog timeouts in mSecs */
+#define IWL_WATCHHDOG_DISABLED	0
+#define IWL_DEF_WD_TIMEOUT	2000
+#define IWL_LONG_WD_TIMEOUT	10000
+#define IWL_MAX_WD_TIMEOUT	120000
+
+/* Antenna presence definitions */
+#define	ANT_NONE	0x0
+#define	ANT_A		BIT(0)
+#define	ANT_B		BIT(1)
+#define ANT_C		BIT(2)
+#define	ANT_AB		(ANT_A | ANT_B)
+#define	ANT_AC		(ANT_A | ANT_C)
+#define ANT_BC		(ANT_B | ANT_C)
+#define ANT_ABC		(ANT_A | ANT_B | ANT_C)
+
+
+/*
  * @max_ll_items: max number of OTP blocks
  * @shadow_ram_support: shadow support for OTP memory
  * @led_compensation: compensate on the led on/off time per HW according
@@ -217,7 +138,6 @@ enum iwl_led_mode {
  * @chain_noise_num_beacons: number of beacons used to compute chain noise
  * @adv_thermal_throttle: support advance thermal throttle
  * @support_ct_kill_exit: support ct kill exit condition
- * @support_wimax_coexist: support wimax/wifi co-exist
  * @plcp_delta_threshold: plcp error rate threshold used to trigger
  *	radio tuning when there is a high receiving plcp error rate
  * @chain_noise_scale: default chain noise scale used for gain computation
@@ -226,12 +146,10 @@ enum iwl_led_mode {
  * @shadow_reg_enable: HW shadhow register bit
  * @hd_v2: v2 of enhanced sensitivity value, used for 2000 series and up
  * @no_idle_support: do not support idle mode
- * wd_disable: disable watchdog timer
  */
 struct iwl_base_params {
 	int eeprom_size;
 	int num_of_queues;	/* def: HW dependent */
-	int num_of_ampdu_queues;/* def: HW dependent */
 	/* for iwl_apm_init() */
 	u32 pll_cfg_val;
 
@@ -240,7 +158,6 @@ struct iwl_base_params {
 	u16 led_compensation;
 	bool adv_thermal_throttle;
 	bool support_ct_kill_exit;
-	const bool support_wimax_coexist;
 	u8 plcp_delta_threshold;
 	s32 chain_noise_scale;
 	unsigned int wd_timeout;
@@ -248,7 +165,6 @@ struct iwl_base_params {
 	const bool shadow_reg_enable;
 	const bool hd_v2;
 	const bool no_idle_support;
-	const bool wd_disable;
 };
 
 /*
@@ -292,28 +208,21 @@ struct iwl_ht_params {
  * @eeprom_ver: EEPROM version
  * @eeprom_calib_ver: EEPROM calibration version
  * @lib: pointer to the lib ops
- * @additional_nic_config: additional nic configuration
  * @base_params: pointer to basic parameters
  * @ht_params: point to ht patameters
  * @bt_params: pointer to bt parameters
  * @need_temp_offset_calib: need to perform temperature offset calibration
  * @no_xtal_calib: some devices do not need crystal calibration data,
  *	don't send it to those
- * @scan_rx_antennas: available antenna for scan operation
  * @led_mode: 0=blinking, 1=On(RF On)/Off(RF Off)
  * @adv_pm: advance power management
  * @rx_with_siso_diversity: 1x1 device with rx antenna diversity
  * @internal_wimax_coex: internal wifi/wimax combo device
- * @iq_invert: I/Q inversion
  * @temp_offset_v2: support v2 of temperature offset calibration
  *
- * We enable the driver to be backward compatible wrt API version. The
- * driver specifies which APIs it supports (with @ucode_api_max being the
- * highest and @ucode_api_min the lowest). Firmware will only be loaded if
- * it has a supported API version.
- *
- * The ideal usage of this infrastructure is to treat a new ucode API
- * release as a new hardware revision.
+ * We enable the driver to be backward compatible wrt. hardware features.
+ * API differences in uCode shouldn't be handled here but through TLVs
+ * and/or the uCode API version instead.
  */
 struct iwl_cfg {
 	/* params specific to an individual device within a device family */
@@ -322,14 +231,13 @@ struct iwl_cfg {
 	const unsigned int ucode_api_max;
 	const unsigned int ucode_api_ok;
 	const unsigned int ucode_api_min;
+	const enum iwl_device_family device_family;
 	const u32 max_data_size;
 	const u32 max_inst_size;
 	u8   valid_tx_ant;
 	u8   valid_rx_ant;
 	u16  eeprom_ver;
 	u16  eeprom_calib_ver;
-	const struct iwl_lib_ops *lib;
-	void (*additional_nic_config)(struct iwl_priv *priv);
 	/* params not likely to change within a device family */
 	const struct iwl_base_params *base_params;
 	/* params likely to change within a device family */
@@ -337,99 +245,11 @@ struct iwl_cfg {
 	const struct iwl_bt_params *bt_params;
 	const bool need_temp_offset_calib; /* if used set to true */
 	const bool no_xtal_calib;
-	u8 scan_rx_antennas[IEEE80211_NUM_BANDS];
 	enum iwl_led_mode led_mode;
 	const bool adv_pm;
 	const bool rx_with_siso_diversity;
 	const bool internal_wimax_coex;
-	const bool iq_invert;
 	const bool temp_offset_v2;
 };
 
-/**
- * struct iwl_shared - shared fields for all the layers of the driver
- *
- * @status: STATUS_*
- * @wowlan: are we running wowlan uCode
- * @valid_contexts: microcode/device supports multiple contexts
- * @bus: pointer to the bus layer data
- * @cfg: see struct iwl_cfg
- * @priv: pointer to the upper layer data
- * @trans: pointer to the transport layer data
- * @nic: pointer to the nic data
- * @hw_params: see struct iwl_hw_params
- * @lock: protect general shared data
- * @eeprom: pointer to the eeprom/OTP image
- * @ucode_type: indicator of loaded ucode image
- * @device_pointers: pointers to ucode event tables
- */
-struct iwl_shared {
-	unsigned long status;
-	u8 valid_contexts;
-
-	const struct iwl_cfg *cfg;
-	struct iwl_trans *trans;
-	void *drv;
-	struct iwl_hw_params hw_params;
-	const struct iwl_fw *fw;
-
-	/* eeprom -- this is in the card's little endian byte order */
-	u8 *eeprom;
-
-	/* ucode related variables */
-	enum iwl_ucode_type ucode_type;
-
-	struct {
-		u32 error_event_table;
-		u32 log_event_table;
-	} device_pointers;
-
-};
-
-/*Whatever _m is (iwl_trans, iwl_priv, these macros will work */
-#define cfg(_m)		((_m)->shrd->cfg)
-#define trans(_m)	((_m)->shrd->trans)
-#define hw_params(_m)	((_m)->shrd->hw_params)
-
-static inline bool iwl_have_debug_level(u32 level)
-{
-	return iwlagn_mod_params.debug_level & level;
-}
-
-enum iwl_rxon_context_id {
-	IWL_RXON_CTX_BSS,
-	IWL_RXON_CTX_PAN,
-
-	NUM_IWL_RXON_CTX
-};
-
-int iwlagn_hw_valid_rtc_data_addr(u32 addr);
-const char *get_cmd_string(u8 cmd);
-
-#define IWL_CMD(x) case x: return #x
-
-/*****************************************************
-* DRIVER STATUS FUNCTIONS
-******************************************************/
-#define STATUS_HCMD_ACTIVE	0	/* host command in progress */
-/* 1 is unused (used to be STATUS_HCMD_SYNC_ACTIVE) */
-#define STATUS_INT_ENABLED	2
-#define STATUS_RF_KILL_HW	3
-#define STATUS_CT_KILL		4
-#define STATUS_INIT		5
-#define STATUS_ALIVE		6
-#define STATUS_READY		7
-#define STATUS_TEMPERATURE	8
-#define STATUS_GEO_CONFIGURED	9
-#define STATUS_EXIT_PENDING	10
-#define STATUS_STATISTICS	12
-#define STATUS_SCANNING		13
-#define STATUS_SCAN_ABORTING	14
-#define STATUS_SCAN_HW		15
-#define STATUS_POWER_PMI	16
-#define STATUS_FW_ERROR		17
-#define STATUS_DEVICE_ENABLED	18
-#define STATUS_CHANNEL_SWITCH_PENDING 19
-#define STATUS_SCAN_COMPLETE	20
-
-#endif /* #__iwl_shared_h__ */
+#endif /* __IWL_CONFIG_H__ */
diff --git a/drivers/net/wireless/iwlwifi/iwl-core.c b/drivers/net/wireless/iwlwifi/iwl-core.c
deleted file mode 100644
index 46490d3b95b..00000000000
--- a/drivers/net/wireless/iwlwifi/iwl-core.c
+++ /dev/null
@@ -1,1480 +0,0 @@
-/******************************************************************************
- *
- * GPL LICENSE SUMMARY
- *
- * Copyright(c) 2008 - 2012 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
- *****************************************************************************/
-
-#include <linux/kernel.h>
-#include <linux/module.h>
-#include <linux/etherdevice.h>
-#include <linux/sched.h>
-#include <linux/slab.h>
-#include <net/mac80211.h>
-
-#include "iwl-eeprom.h"
-#include "iwl-debug.h"
-#include "iwl-core.h"
-#include "iwl-io.h"
-#include "iwl-power.h"
-#include "iwl-shared.h"
-#include "iwl-agn.h"
-#include "iwl-trans.h"
-
-const u8 iwl_bcast_addr[ETH_ALEN] = { 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF };
-
-#define MAX_BIT_RATE_40_MHZ 150 /* Mbps */
-#define MAX_BIT_RATE_20_MHZ 72 /* Mbps */
-static void iwl_init_ht_hw_capab(const struct iwl_priv *priv,
-			      struct ieee80211_sta_ht_cap *ht_info,
-			      enum ieee80211_band band)
-{
-	u16 max_bit_rate = 0;
-	u8 rx_chains_num = hw_params(priv).rx_chains_num;
-	u8 tx_chains_num = hw_params(priv).tx_chains_num;
-
-	ht_info->cap = 0;
-	memset(&ht_info->mcs, 0, sizeof(ht_info->mcs));
-
-	ht_info->ht_supported = true;
-
-	if (cfg(priv)->ht_params &&
-	    cfg(priv)->ht_params->ht_greenfield_support)
-		ht_info->cap |= IEEE80211_HT_CAP_GRN_FLD;
-	ht_info->cap |= IEEE80211_HT_CAP_SGI_20;
-	max_bit_rate = MAX_BIT_RATE_20_MHZ;
-	if (hw_params(priv).ht40_channel & BIT(band)) {
-		ht_info->cap |= IEEE80211_HT_CAP_SUP_WIDTH_20_40;
-		ht_info->cap |= IEEE80211_HT_CAP_SGI_40;
-		ht_info->mcs.rx_mask[4] = 0x01;
-		max_bit_rate = MAX_BIT_RATE_40_MHZ;
-	}
-
-	if (iwlagn_mod_params.amsdu_size_8K)
-		ht_info->cap |= IEEE80211_HT_CAP_MAX_AMSDU;
-
-	ht_info->ampdu_factor = CFG_HT_RX_AMPDU_FACTOR_DEF;
-	ht_info->ampdu_density = CFG_HT_MPDU_DENSITY_DEF;
-
-	ht_info->mcs.rx_mask[0] = 0xFF;
-	if (rx_chains_num >= 2)
-		ht_info->mcs.rx_mask[1] = 0xFF;
-	if (rx_chains_num >= 3)
-		ht_info->mcs.rx_mask[2] = 0xFF;
-
-	/* Highest supported Rx data rate */
-	max_bit_rate *= rx_chains_num;
-	WARN_ON(max_bit_rate & ~IEEE80211_HT_MCS_RX_HIGHEST_MASK);
-	ht_info->mcs.rx_highest = cpu_to_le16(max_bit_rate);
-
-	/* Tx MCS capabilities */
-	ht_info->mcs.tx_params = IEEE80211_HT_MCS_TX_DEFINED;
-	if (tx_chains_num != rx_chains_num) {
-		ht_info->mcs.tx_params |= IEEE80211_HT_MCS_TX_RX_DIFF;
-		ht_info->mcs.tx_params |= ((tx_chains_num - 1) <<
-				IEEE80211_HT_MCS_TX_MAX_STREAMS_SHIFT);
-	}
-}
-
-/**
- * iwl_init_geos - Initialize mac80211's geo/channel info based from eeprom
- */
-int iwl_init_geos(struct iwl_priv *priv)
-{
-	struct iwl_channel_info *ch;
-	struct ieee80211_supported_band *sband;
-	struct ieee80211_channel *channels;
-	struct ieee80211_channel *geo_ch;
-	struct ieee80211_rate *rates;
-	int i = 0;
-	s8 max_tx_power = IWLAGN_TX_POWER_TARGET_POWER_MIN;
-
-	if (priv->bands[IEEE80211_BAND_2GHZ].n_bitrates ||
-	    priv->bands[IEEE80211_BAND_5GHZ].n_bitrates) {
-		IWL_DEBUG_INFO(priv, "Geography modes already initialized.\n");
-		set_bit(STATUS_GEO_CONFIGURED, &priv->status);
-		return 0;
-	}
-
-	channels = kcalloc(priv->channel_count,
-			   sizeof(struct ieee80211_channel), GFP_KERNEL);
-	if (!channels)
-		return -ENOMEM;
-
-	rates = kcalloc(IWL_RATE_COUNT_LEGACY, sizeof(struct ieee80211_rate),
-			GFP_KERNEL);
-	if (!rates) {
-		kfree(channels);
-		return -ENOMEM;
-	}
-
-	/* 5.2GHz channels start after the 2.4GHz channels */
-	sband = &priv->bands[IEEE80211_BAND_5GHZ];
-	sband->channels = &channels[ARRAY_SIZE(iwl_eeprom_band_1)];
-	/* just OFDM */
-	sband->bitrates = &rates[IWL_FIRST_OFDM_RATE];
-	sband->n_bitrates = IWL_RATE_COUNT_LEGACY - IWL_FIRST_OFDM_RATE;
-
-	if (hw_params(priv).sku & EEPROM_SKU_CAP_11N_ENABLE)
-		iwl_init_ht_hw_capab(priv, &sband->ht_cap,
-					 IEEE80211_BAND_5GHZ);
-
-	sband = &priv->bands[IEEE80211_BAND_2GHZ];
-	sband->channels = channels;
-	/* OFDM & CCK */
-	sband->bitrates = rates;
-	sband->n_bitrates = IWL_RATE_COUNT_LEGACY;
-
-	if (hw_params(priv).sku & EEPROM_SKU_CAP_11N_ENABLE)
-		iwl_init_ht_hw_capab(priv, &sband->ht_cap,
-					 IEEE80211_BAND_2GHZ);
-
-	priv->ieee_channels = channels;
-	priv->ieee_rates = rates;
-
-	for (i = 0;  i < priv->channel_count; i++) {
-		ch = &priv->channel_info[i];
-
-		/* FIXME: might be removed if scan is OK */
-		if (!is_channel_valid(ch))
-			continue;
-
-		sband =  &priv->bands[ch->band];
-
-		geo_ch = &sband->channels[sband->n_channels++];
-
-		geo_ch->center_freq =
-			ieee80211_channel_to_frequency(ch->channel, ch->band);
-		geo_ch->max_power = ch->max_power_avg;
-		geo_ch->max_antenna_gain = 0xff;
-		geo_ch->hw_value = ch->channel;
-
-		if (is_channel_valid(ch)) {
-			if (!(ch->flags & EEPROM_CHANNEL_IBSS))
-				geo_ch->flags |= IEEE80211_CHAN_NO_IBSS;
-
-			if (!(ch->flags & EEPROM_CHANNEL_ACTIVE))
-				geo_ch->flags |= IEEE80211_CHAN_PASSIVE_SCAN;
-
-			if (ch->flags & EEPROM_CHANNEL_RADAR)
-				geo_ch->flags |= IEEE80211_CHAN_RADAR;
-
-			geo_ch->flags |= ch->ht40_extension_channel;
-
-			if (ch->max_power_avg > max_tx_power)
-				max_tx_power = ch->max_power_avg;
-		} else {
-			geo_ch->flags |= IEEE80211_CHAN_DISABLED;
-		}
-
-		IWL_DEBUG_INFO(priv, "Channel %d Freq=%d[%sGHz] %s flag=0x%X\n",
-				ch->channel, geo_ch->center_freq,
-				is_channel_a_band(ch) ?  "5.2" : "2.4",
-				geo_ch->flags & IEEE80211_CHAN_DISABLED ?
-				"restricted" : "valid",
-				 geo_ch->flags);
-	}
-
-	priv->tx_power_device_lmt = max_tx_power;
-	priv->tx_power_user_lmt = max_tx_power;
-	priv->tx_power_next = max_tx_power;
-
-	if ((priv->bands[IEEE80211_BAND_5GHZ].n_channels == 0) &&
-	     hw_params(priv).sku & EEPROM_SKU_CAP_BAND_52GHZ) {
-		IWL_INFO(priv, "Incorrectly detected BG card as ABG. "
-			"Please send your %s to maintainer.\n",
-			trans(priv)->hw_id_str);
-		hw_params(priv).sku &= ~EEPROM_SKU_CAP_BAND_52GHZ;
-	}
-
-	IWL_INFO(priv, "Tunable channels: %d 802.11bg, %d 802.11a channels\n",
-		   priv->bands[IEEE80211_BAND_2GHZ].n_channels,
-		   priv->bands[IEEE80211_BAND_5GHZ].n_channels);
-
-	set_bit(STATUS_GEO_CONFIGURED, &priv->status);
-
-	return 0;
-}
-
-/*
- * iwl_free_geos - undo allocations in iwl_init_geos
- */
-void iwl_free_geos(struct iwl_priv *priv)
-{
-	kfree(priv->ieee_channels);
-	kfree(priv->ieee_rates);
-	clear_bit(STATUS_GEO_CONFIGURED, &priv->status);
-}
-
-static bool iwl_is_channel_extension(struct iwl_priv *priv,
-				     enum ieee80211_band band,
-				     u16 channel, u8 extension_chan_offset)
-{
-	const struct iwl_channel_info *ch_info;
-
-	ch_info = iwl_get_channel_info(priv, band, channel);
-	if (!is_channel_valid(ch_info))
-		return false;
-
-	if (extension_chan_offset == IEEE80211_HT_PARAM_CHA_SEC_ABOVE)
-		return !(ch_info->ht40_extension_channel &
-					IEEE80211_CHAN_NO_HT40PLUS);
-	else if (extension_chan_offset == IEEE80211_HT_PARAM_CHA_SEC_BELOW)
-		return !(ch_info->ht40_extension_channel &
-					IEEE80211_CHAN_NO_HT40MINUS);
-
-	return false;
-}
-
-bool iwl_is_ht40_tx_allowed(struct iwl_priv *priv,
-			    struct iwl_rxon_context *ctx,
-			    struct ieee80211_sta_ht_cap *ht_cap)
-{
-	if (!ctx->ht.enabled || !ctx->ht.is_40mhz)
-		return false;
-
-	/*
-	 * We do not check for IEEE80211_HT_CAP_SUP_WIDTH_20_40
-	 * the bit will not set if it is pure 40MHz case
-	 */
-	if (ht_cap && !ht_cap->ht_supported)
-		return false;
-
-#ifdef CONFIG_IWLWIFI_DEBUGFS
-	if (priv->disable_ht40)
-		return false;
-#endif
-
-	return iwl_is_channel_extension(priv, priv->band,
-			le16_to_cpu(ctx->staging.channel),
-			ctx->ht.extension_chan_offset);
-}
-
-static u16 iwl_adjust_beacon_interval(u16 beacon_val, u16 max_beacon_val)
-{
-	u16 new_val;
-	u16 beacon_factor;
-
-	/*
-	 * If mac80211 hasn't given us a beacon interval, program
-	 * the default into the device (not checking this here
-	 * would cause the adjustment below to return the maximum
-	 * value, which may break PAN.)
-	 */
-	if (!beacon_val)
-		return DEFAULT_BEACON_INTERVAL;
-
-	/*
-	 * If the beacon interval we obtained from the peer
-	 * is too large, we'll have to wake up more often
-	 * (and in IBSS case, we'll beacon too much)
-	 *
-	 * For example, if max_beacon_val is 4096, and the
-	 * requested beacon interval is 7000, we'll have to
-	 * use 3500 to be able to wake up on the beacons.
-	 *
-	 * This could badly influence beacon detection stats.
-	 */
-
-	beacon_factor = (beacon_val + max_beacon_val) / max_beacon_val;
-	new_val = beacon_val / beacon_factor;
-
-	if (!new_val)
-		new_val = max_beacon_val;
-
-	return new_val;
-}
-
-int iwl_send_rxon_timing(struct iwl_priv *priv, struct iwl_rxon_context *ctx)
-{
-	u64 tsf;
-	s32 interval_tm, rem;
-	struct ieee80211_conf *conf = NULL;
-	u16 beacon_int;
-	struct ieee80211_vif *vif = ctx->vif;
-
-	conf = &priv->hw->conf;
-
-	lockdep_assert_held(&priv->mutex);
-
-	memset(&ctx->timing, 0, sizeof(struct iwl_rxon_time_cmd));
-
-	ctx->timing.timestamp = cpu_to_le64(priv->timestamp);
-	ctx->timing.listen_interval = cpu_to_le16(conf->listen_interval);
-
-	beacon_int = vif ? vif->bss_conf.beacon_int : 0;
-
-	/*
-	 * TODO: For IBSS we need to get atim_window from mac80211,
-	 *	 for now just always use 0
-	 */
-	ctx->timing.atim_window = 0;
-
-	if (ctx->ctxid == IWL_RXON_CTX_PAN &&
-	    (!ctx->vif || ctx->vif->type != NL80211_IFTYPE_STATION) &&
-	    iwl_is_associated(priv, IWL_RXON_CTX_BSS) &&
-	    priv->contexts[IWL_RXON_CTX_BSS].vif &&
-	    priv->contexts[IWL_RXON_CTX_BSS].vif->bss_conf.beacon_int) {
-		ctx->timing.beacon_interval =
-			priv->contexts[IWL_RXON_CTX_BSS].timing.beacon_interval;
-		beacon_int = le16_to_cpu(ctx->timing.beacon_interval);
-	} else if (ctx->ctxid == IWL_RXON_CTX_BSS &&
-		   iwl_is_associated(priv, IWL_RXON_CTX_PAN) &&
-		   priv->contexts[IWL_RXON_CTX_PAN].vif &&
-		   priv->contexts[IWL_RXON_CTX_PAN].vif->bss_conf.beacon_int &&
-		   (!iwl_is_associated_ctx(ctx) || !ctx->vif ||
-		    !ctx->vif->bss_conf.beacon_int)) {
-		ctx->timing.beacon_interval =
-			priv->contexts[IWL_RXON_CTX_PAN].timing.beacon_interval;
-		beacon_int = le16_to_cpu(ctx->timing.beacon_interval);
-	} else {
-		beacon_int = iwl_adjust_beacon_interval(beacon_int,
-			IWL_MAX_UCODE_BEACON_INTERVAL * TIME_UNIT);
-		ctx->timing.beacon_interval = cpu_to_le16(beacon_int);
-	}
-
-	ctx->beacon_int = beacon_int;
-
-	tsf = priv->timestamp; /* tsf is modifed by do_div: copy it */
-	interval_tm = beacon_int * TIME_UNIT;
-	rem = do_div(tsf, interval_tm);
-	ctx->timing.beacon_init_val = cpu_to_le32(interval_tm - rem);
-
-	ctx->timing.dtim_period = vif ? (vif->bss_conf.dtim_period ?: 1) : 1;
-
-	IWL_DEBUG_ASSOC(priv,
-			"beacon interval %d beacon timer %d beacon tim %d\n",
-			le16_to_cpu(ctx->timing.beacon_interval),
-			le32_to_cpu(ctx->timing.beacon_init_val),
-			le16_to_cpu(ctx->timing.atim_window));
-
-	return iwl_dvm_send_cmd_pdu(priv, ctx->rxon_timing_cmd,
-				CMD_SYNC, sizeof(ctx->timing), &ctx->timing);
-}
-
-void iwl_set_rxon_hwcrypto(struct iwl_priv *priv, struct iwl_rxon_context *ctx,
-			   int hw_decrypt)
-{
-	struct iwl_rxon_cmd *rxon = &ctx->staging;
-
-	if (hw_decrypt)
-		rxon->filter_flags &= ~RXON_FILTER_DIS_DECRYPT_MSK;
-	else
-		rxon->filter_flags |= RXON_FILTER_DIS_DECRYPT_MSK;
-
-}
-
-/* validate RXON structure is valid */
-int iwl_check_rxon_cmd(struct iwl_priv *priv, struct iwl_rxon_context *ctx)
-{
-	struct iwl_rxon_cmd *rxon = &ctx->staging;
-	u32 errors = 0;
-
-	if (rxon->flags & RXON_FLG_BAND_24G_MSK) {
-		if (rxon->flags & RXON_FLG_TGJ_NARROW_BAND_MSK) {
-			IWL_WARN(priv, "check 2.4G: wrong narrow\n");
-			errors |= BIT(0);
-		}
-		if (rxon->flags & RXON_FLG_RADAR_DETECT_MSK) {
-			IWL_WARN(priv, "check 2.4G: wrong radar\n");
-			errors |= BIT(1);
-		}
-	} else {
-		if (!(rxon->flags & RXON_FLG_SHORT_SLOT_MSK)) {
-			IWL_WARN(priv, "check 5.2G: not short slot!\n");
-			errors |= BIT(2);
-		}
-		if (rxon->flags & RXON_FLG_CCK_MSK) {
-			IWL_WARN(priv, "check 5.2G: CCK!\n");
-			errors |= BIT(3);
-		}
-	}
-	if ((rxon->node_addr[0] | rxon->bssid_addr[0]) & 0x1) {
-		IWL_WARN(priv, "mac/bssid mcast!\n");
-		errors |= BIT(4);
-	}
-
-	/* make sure basic rates 6Mbps and 1Mbps are supported */
-	if ((rxon->ofdm_basic_rates & IWL_RATE_6M_MASK) == 0 &&
-	    (rxon->cck_basic_rates & IWL_RATE_1M_MASK) == 0) {
-		IWL_WARN(priv, "neither 1 nor 6 are basic\n");
-		errors |= BIT(5);
-	}
-
-	if (le16_to_cpu(rxon->assoc_id) > 2007) {
-		IWL_WARN(priv, "aid > 2007\n");
-		errors |= BIT(6);
-	}
-
-	if ((rxon->flags & (RXON_FLG_CCK_MSK | RXON_FLG_SHORT_SLOT_MSK))
-			== (RXON_FLG_CCK_MSK | RXON_FLG_SHORT_SLOT_MSK)) {
-		IWL_WARN(priv, "CCK and short slot\n");
-		errors |= BIT(7);
-	}
-
-	if ((rxon->flags & (RXON_FLG_CCK_MSK | RXON_FLG_AUTO_DETECT_MSK))
-			== (RXON_FLG_CCK_MSK | RXON_FLG_AUTO_DETECT_MSK)) {
-		IWL_WARN(priv, "CCK and auto detect");
-		errors |= BIT(8);
-	}
-
-	if ((rxon->flags & (RXON_FLG_AUTO_DETECT_MSK |
-			    RXON_FLG_TGG_PROTECT_MSK)) ==
-			    RXON_FLG_TGG_PROTECT_MSK) {
-		IWL_WARN(priv, "TGg but no auto-detect\n");
-		errors |= BIT(9);
-	}
-
-	if (rxon->channel == 0) {
-		IWL_WARN(priv, "zero channel is invalid\n");
-		errors |= BIT(10);
-	}
-
-	WARN(errors, "Invalid RXON (%#x), channel %d",
-	     errors, le16_to_cpu(rxon->channel));
-
-	return errors ? -EINVAL : 0;
-}
-
-/**
- * iwl_full_rxon_required - check if full RXON (vs RXON_ASSOC) cmd is needed
- * @priv: staging_rxon is compared to active_rxon
- *
- * If the RXON structure is changing enough to require a new tune,
- * or is clearing the RXON_FILTER_ASSOC_MSK, then return 1 to indicate that
- * a new tune (full RXON command, rather than RXON_ASSOC cmd) is required.
- */
-int iwl_full_rxon_required(struct iwl_priv *priv,
-			   struct iwl_rxon_context *ctx)
-{
-	const struct iwl_rxon_cmd *staging = &ctx->staging;
-	const struct iwl_rxon_cmd *active = &ctx->active;
-
-#define CHK(cond)							\
-	if ((cond)) {							\
-		IWL_DEBUG_INFO(priv, "need full RXON - " #cond "\n");	\
-		return 1;						\
-	}
-
-#define CHK_NEQ(c1, c2)						\
-	if ((c1) != (c2)) {					\
-		IWL_DEBUG_INFO(priv, "need full RXON - "	\
-			       #c1 " != " #c2 " - %d != %d\n",	\
-			       (c1), (c2));			\
-		return 1;					\
-	}
-
-	/* These items are only settable from the full RXON command */
-	CHK(!iwl_is_associated_ctx(ctx));
-	CHK(compare_ether_addr(staging->bssid_addr, active->bssid_addr));
-	CHK(compare_ether_addr(staging->node_addr, active->node_addr));
-	CHK(compare_ether_addr(staging->wlap_bssid_addr,
-				active->wlap_bssid_addr));
-	CHK_NEQ(staging->dev_type, active->dev_type);
-	CHK_NEQ(staging->channel, active->channel);
-	CHK_NEQ(staging->air_propagation, active->air_propagation);
-	CHK_NEQ(staging->ofdm_ht_single_stream_basic_rates,
-		active->ofdm_ht_single_stream_basic_rates);
-	CHK_NEQ(staging->ofdm_ht_dual_stream_basic_rates,
-		active->ofdm_ht_dual_stream_basic_rates);
-	CHK_NEQ(staging->ofdm_ht_triple_stream_basic_rates,
-		active->ofdm_ht_triple_stream_basic_rates);
-	CHK_NEQ(staging->assoc_id, active->assoc_id);
-
-	/* flags, filter_flags, ofdm_basic_rates, and cck_basic_rates can
-	 * be updated with the RXON_ASSOC command -- however only some
-	 * flag transitions are allowed using RXON_ASSOC */
-
-	/* Check if we are not switching bands */
-	CHK_NEQ(staging->flags & RXON_FLG_BAND_24G_MSK,
-		active->flags & RXON_FLG_BAND_24G_MSK);
-
-	/* Check if we are switching association toggle */
-	CHK_NEQ(staging->filter_flags & RXON_FILTER_ASSOC_MSK,
-		active->filter_flags & RXON_FILTER_ASSOC_MSK);
-
-#undef CHK
-#undef CHK_NEQ
-
-	return 0;
-}
-
-static void _iwl_set_rxon_ht(struct iwl_priv *priv,
-			     struct iwl_ht_config *ht_conf,
-			     struct iwl_rxon_context *ctx)
-{
-	struct iwl_rxon_cmd *rxon = &ctx->staging;
-
-	if (!ctx->ht.enabled) {
-		rxon->flags &= ~(RXON_FLG_CHANNEL_MODE_MSK |
-			RXON_FLG_CTRL_CHANNEL_LOC_HI_MSK |
-			RXON_FLG_HT40_PROT_MSK |
-			RXON_FLG_HT_PROT_MSK);
-		return;
-	}
-
-	/* FIXME: if the definition of ht.protection changed, the "translation"
-	 * will be needed for rxon->flags
-	 */
-	rxon->flags |= cpu_to_le32(ctx->ht.protection << RXON_FLG_HT_OPERATING_MODE_POS);
-
-	/* Set up channel bandwidth:
-	 * 20 MHz only, 20/40 mixed or pure 40 if ht40 ok */
-	/* clear the HT channel mode before set the mode */
-	rxon->flags &= ~(RXON_FLG_CHANNEL_MODE_MSK |
-			 RXON_FLG_CTRL_CHANNEL_LOC_HI_MSK);
-	if (iwl_is_ht40_tx_allowed(priv, ctx, NULL)) {
-		/* pure ht40 */
-		if (ctx->ht.protection == IEEE80211_HT_OP_MODE_PROTECTION_20MHZ) {
-			rxon->flags |= RXON_FLG_CHANNEL_MODE_PURE_40;
-			/* Note: control channel is opposite of extension channel */
-			switch (ctx->ht.extension_chan_offset) {
-			case IEEE80211_HT_PARAM_CHA_SEC_ABOVE:
-				rxon->flags &= ~RXON_FLG_CTRL_CHANNEL_LOC_HI_MSK;
-				break;
-			case IEEE80211_HT_PARAM_CHA_SEC_BELOW:
-				rxon->flags |= RXON_FLG_CTRL_CHANNEL_LOC_HI_MSK;
-				break;
-			}
-		} else {
-			/* Note: control channel is opposite of extension channel */
-			switch (ctx->ht.extension_chan_offset) {
-			case IEEE80211_HT_PARAM_CHA_SEC_ABOVE:
-				rxon->flags &= ~(RXON_FLG_CTRL_CHANNEL_LOC_HI_MSK);
-				rxon->flags |= RXON_FLG_CHANNEL_MODE_MIXED;
-				break;
-			case IEEE80211_HT_PARAM_CHA_SEC_BELOW:
-				rxon->flags |= RXON_FLG_CTRL_CHANNEL_LOC_HI_MSK;
-				rxon->flags |= RXON_FLG_CHANNEL_MODE_MIXED;
-				break;
-			case IEEE80211_HT_PARAM_CHA_SEC_NONE:
-			default:
-				/* channel location only valid if in Mixed mode */
-				IWL_ERR(priv, "invalid extension channel offset\n");
-				break;
-			}
-		}
-	} else {
-		rxon->flags |= RXON_FLG_CHANNEL_MODE_LEGACY;
-	}
-
-	iwlagn_set_rxon_chain(priv, ctx);
-
-	IWL_DEBUG_ASSOC(priv, "rxon flags 0x%X operation mode :0x%X "
-			"extension channel offset 0x%x\n",
-			le32_to_cpu(rxon->flags), ctx->ht.protection,
-			ctx->ht.extension_chan_offset);
-}
-
-void iwl_set_rxon_ht(struct iwl_priv *priv, struct iwl_ht_config *ht_conf)
-{
-	struct iwl_rxon_context *ctx;
-
-	for_each_context(priv, ctx)
-		_iwl_set_rxon_ht(priv, ht_conf, ctx);
-}
-
-/* Return valid, unused, channel for a passive scan to reset the RF */
-u8 iwl_get_single_channel_number(struct iwl_priv *priv,
-				 enum ieee80211_band band)
-{
-	const struct iwl_channel_info *ch_info;
-	int i;
-	u8 channel = 0;
-	u8 min, max;
-	struct iwl_rxon_context *ctx;
-
-	if (band == IEEE80211_BAND_5GHZ) {
-		min = 14;
-		max = priv->channel_count;
-	} else {
-		min = 0;
-		max = 14;
-	}
-
-	for (i = min; i < max; i++) {
-		bool busy = false;
-
-		for_each_context(priv, ctx) {
-			busy = priv->channel_info[i].channel ==
-				le16_to_cpu(ctx->staging.channel);
-			if (busy)
-				break;
-		}
-
-		if (busy)
-			continue;
-
-		channel = priv->channel_info[i].channel;
-		ch_info = iwl_get_channel_info(priv, band, channel);
-		if (is_channel_valid(ch_info))
-			break;
-	}
-
-	return channel;
-}
-
-/**
- * iwl_set_rxon_channel - Set the band and channel values in staging RXON
- * @ch: requested channel as a pointer to struct ieee80211_channel
-
- * NOTE:  Does not commit to the hardware; it sets appropriate bit fields
- * in the staging RXON flag structure based on the ch->band
- */
-void iwl_set_rxon_channel(struct iwl_priv *priv, struct ieee80211_channel *ch,
-			 struct iwl_rxon_context *ctx)
-{
-	enum ieee80211_band band = ch->band;
-	u16 channel = ch->hw_value;
-
-	if ((le16_to_cpu(ctx->staging.channel) == channel) &&
-	    (priv->band == band))
-		return;
-
-	ctx->staging.channel = cpu_to_le16(channel);
-	if (band == IEEE80211_BAND_5GHZ)
-		ctx->staging.flags &= ~RXON_FLG_BAND_24G_MSK;
-	else
-		ctx->staging.flags |= RXON_FLG_BAND_24G_MSK;
-
-	priv->band = band;
-
-	IWL_DEBUG_INFO(priv, "Staging channel set to %d [%d]\n", channel, band);
-
-}
-
-void iwl_set_flags_for_band(struct iwl_priv *priv,
-			    struct iwl_rxon_context *ctx,
-			    enum ieee80211_band band,
-			    struct ieee80211_vif *vif)
-{
-	if (band == IEEE80211_BAND_5GHZ) {
-		ctx->staging.flags &=
-		    ~(RXON_FLG_BAND_24G_MSK | RXON_FLG_AUTO_DETECT_MSK
-		      | RXON_FLG_CCK_MSK);
-		ctx->staging.flags |= RXON_FLG_SHORT_SLOT_MSK;
-	} else {
-		/* Copied from iwl_post_associate() */
-		if (vif && vif->bss_conf.use_short_slot)
-			ctx->staging.flags |= RXON_FLG_SHORT_SLOT_MSK;
-		else
-			ctx->staging.flags &= ~RXON_FLG_SHORT_SLOT_MSK;
-
-		ctx->staging.flags |= RXON_FLG_BAND_24G_MSK;
-		ctx->staging.flags |= RXON_FLG_AUTO_DETECT_MSK;
-		ctx->staging.flags &= ~RXON_FLG_CCK_MSK;
-	}
-}
-
-/*
- * initialize rxon structure with default values from eeprom
- */
-void iwl_connection_init_rx_config(struct iwl_priv *priv,
-				   struct iwl_rxon_context *ctx)
-{
-	const struct iwl_channel_info *ch_info;
-
-	memset(&ctx->staging, 0, sizeof(ctx->staging));
-
-	if (!ctx->vif) {
-		ctx->staging.dev_type = ctx->unused_devtype;
-	} else switch (ctx->vif->type) {
-	case NL80211_IFTYPE_AP:
-		ctx->staging.dev_type = ctx->ap_devtype;
-		break;
-
-	case NL80211_IFTYPE_STATION:
-		ctx->staging.dev_type = ctx->station_devtype;
-		ctx->staging.filter_flags = RXON_FILTER_ACCEPT_GRP_MSK;
-		break;
-
-	case NL80211_IFTYPE_ADHOC:
-		ctx->staging.dev_type = ctx->ibss_devtype;
-		ctx->staging.flags = RXON_FLG_SHORT_PREAMBLE_MSK;
-		ctx->staging.filter_flags = RXON_FILTER_BCON_AWARE_MSK |
-						  RXON_FILTER_ACCEPT_GRP_MSK;
-		break;
-
-	default:
-		IWL_ERR(priv, "Unsupported interface type %d\n",
-			ctx->vif->type);
-		break;
-	}
-
-#if 0
-	/* TODO:  Figure out when short_preamble would be set and cache from
-	 * that */
-	if (!hw_to_local(priv->hw)->short_preamble)
-		ctx->staging.flags &= ~RXON_FLG_SHORT_PREAMBLE_MSK;
-	else
-		ctx->staging.flags |= RXON_FLG_SHORT_PREAMBLE_MSK;
-#endif
-
-	ch_info = iwl_get_channel_info(priv, priv->band,
-				       le16_to_cpu(ctx->active.channel));
-
-	if (!ch_info)
-		ch_info = &priv->channel_info[0];
-
-	ctx->staging.channel = cpu_to_le16(ch_info->channel);
-	priv->band = ch_info->band;
-
-	iwl_set_flags_for_band(priv, ctx, priv->band, ctx->vif);
-
-	ctx->staging.ofdm_basic_rates =
-	    (IWL_OFDM_RATES_MASK >> IWL_FIRST_OFDM_RATE) & 0xFF;
-	ctx->staging.cck_basic_rates =
-	    (IWL_CCK_RATES_MASK >> IWL_FIRST_CCK_RATE) & 0xF;
-
-	/* clear both MIX and PURE40 mode flag */
-	ctx->staging.flags &= ~(RXON_FLG_CHANNEL_MODE_MIXED |
-					RXON_FLG_CHANNEL_MODE_PURE_40);
-	if (ctx->vif)
-		memcpy(ctx->staging.node_addr, ctx->vif->addr, ETH_ALEN);
-
-	ctx->staging.ofdm_ht_single_stream_basic_rates = 0xff;
-	ctx->staging.ofdm_ht_dual_stream_basic_rates = 0xff;
-	ctx->staging.ofdm_ht_triple_stream_basic_rates = 0xff;
-}
-
-void iwl_set_rate(struct iwl_priv *priv)
-{
-	const struct ieee80211_supported_band *hw = NULL;
-	struct ieee80211_rate *rate;
-	struct iwl_rxon_context *ctx;
-	int i;
-
-	hw = iwl_get_hw_mode(priv, priv->band);
-	if (!hw) {
-		IWL_ERR(priv, "Failed to set rate: unable to get hw mode\n");
-		return;
-	}
-
-	priv->active_rate = 0;
-
-	for (i = 0; i < hw->n_bitrates; i++) {
-		rate = &(hw->bitrates[i]);
-		if (rate->hw_value < IWL_RATE_COUNT_LEGACY)
-			priv->active_rate |= (1 << rate->hw_value);
-	}
-
-	IWL_DEBUG_RATE(priv, "Set active_rate = %0x\n", priv->active_rate);
-
-	for_each_context(priv, ctx) {
-		ctx->staging.cck_basic_rates =
-		    (IWL_CCK_BASIC_RATES_MASK >> IWL_FIRST_CCK_RATE) & 0xF;
-
-		ctx->staging.ofdm_basic_rates =
-		   (IWL_OFDM_BASIC_RATES_MASK >> IWL_FIRST_OFDM_RATE) & 0xFF;
-	}
-}
-
-void iwl_chswitch_done(struct iwl_priv *priv, bool is_success)
-{
-	/*
-	 * MULTI-FIXME
-	 * See iwlagn_mac_channel_switch.
-	 */
-	struct iwl_rxon_context *ctx = &priv->contexts[IWL_RXON_CTX_BSS];
-
-	if (test_bit(STATUS_EXIT_PENDING, &priv->status))
-		return;
-
-	if (test_and_clear_bit(STATUS_CHANNEL_SWITCH_PENDING, &priv->status))
-		ieee80211_chswitch_done(ctx->vif, is_success);
-}
-
-#ifdef CONFIG_IWLWIFI_DEBUG
-void iwl_print_rx_config_cmd(struct iwl_priv *priv,
-			     enum iwl_rxon_context_id ctxid)
-{
-	struct iwl_rxon_context *ctx = &priv->contexts[ctxid];
-	struct iwl_rxon_cmd *rxon = &ctx->staging;
-
-	IWL_DEBUG_RADIO(priv, "RX CONFIG:\n");
-	iwl_print_hex_dump(priv, IWL_DL_RADIO, (u8 *) rxon, sizeof(*rxon));
-	IWL_DEBUG_RADIO(priv, "u16 channel: 0x%x\n", le16_to_cpu(rxon->channel));
-	IWL_DEBUG_RADIO(priv, "u32 flags: 0x%08X\n", le32_to_cpu(rxon->flags));
-	IWL_DEBUG_RADIO(priv, "u32 filter_flags: 0x%08x\n",
-			le32_to_cpu(rxon->filter_flags));
-	IWL_DEBUG_RADIO(priv, "u8 dev_type: 0x%x\n", rxon->dev_type);
-	IWL_DEBUG_RADIO(priv, "u8 ofdm_basic_rates: 0x%02x\n",
-			rxon->ofdm_basic_rates);
-	IWL_DEBUG_RADIO(priv, "u8 cck_basic_rates: 0x%02x\n", rxon->cck_basic_rates);
-	IWL_DEBUG_RADIO(priv, "u8[6] node_addr: %pM\n", rxon->node_addr);
-	IWL_DEBUG_RADIO(priv, "u8[6] bssid_addr: %pM\n", rxon->bssid_addr);
-	IWL_DEBUG_RADIO(priv, "u16 assoc_id: 0x%x\n", le16_to_cpu(rxon->assoc_id));
-}
-#endif
-
-static void iwlagn_fw_error(struct iwl_priv *priv, bool ondemand)
-{
-	unsigned int reload_msec;
-	unsigned long reload_jiffies;
-
-#ifdef CONFIG_IWLWIFI_DEBUG
-	if (iwl_have_debug_level(IWL_DL_FW_ERRORS))
-		iwl_print_rx_config_cmd(priv, IWL_RXON_CTX_BSS);
-#endif
-
-	/* uCode is no longer loaded. */
-	priv->ucode_loaded = false;
-
-	/* Set the FW error flag -- cleared on iwl_down */
-	set_bit(STATUS_FW_ERROR, &priv->shrd->status);
-
-	/* Cancel currently queued command. */
-	clear_bit(STATUS_HCMD_ACTIVE, &priv->shrd->status);
-
-	iwl_abort_notification_waits(&priv->notif_wait);
-
-	/* Keep the restart process from trying to send host
-	 * commands by clearing the ready bit */
-	clear_bit(STATUS_READY, &priv->status);
-
-	wake_up(&trans(priv)->wait_command_queue);
-
-	if (!ondemand) {
-		/*
-		 * If firmware keep reloading, then it indicate something
-		 * serious wrong and firmware having problem to recover
-		 * from it. Instead of keep trying which will fill the syslog
-		 * and hang the system, let's just stop it
-		 */
-		reload_jiffies = jiffies;
-		reload_msec = jiffies_to_msecs((long) reload_jiffies -
-					(long) priv->reload_jiffies);
-		priv->reload_jiffies = reload_jiffies;
-		if (reload_msec <= IWL_MIN_RELOAD_DURATION) {
-			priv->reload_count++;
-			if (priv->reload_count >= IWL_MAX_CONTINUE_RELOAD_CNT) {
-				IWL_ERR(priv, "BUG_ON, Stop restarting\n");
-				return;
-			}
-		} else
-			priv->reload_count = 0;
-	}
-
-	if (!test_bit(STATUS_EXIT_PENDING, &priv->status)) {
-		if (iwlagn_mod_params.restart_fw) {
-			IWL_DEBUG_FW_ERRORS(priv,
-				  "Restarting adapter due to uCode error.\n");
-			queue_work(priv->workqueue, &priv->restart);
-		} else
-			IWL_DEBUG_FW_ERRORS(priv,
-				  "Detected FW error, but not restarting\n");
-	}
-}
-
-int iwl_set_tx_power(struct iwl_priv *priv, s8 tx_power, bool force)
-{
-	int ret;
-	s8 prev_tx_power;
-	bool defer;
-	struct iwl_rxon_context *ctx = &priv->contexts[IWL_RXON_CTX_BSS];
-
-	lockdep_assert_held(&priv->mutex);
-
-	if (priv->tx_power_user_lmt == tx_power && !force)
-		return 0;
-
-	if (tx_power < IWLAGN_TX_POWER_TARGET_POWER_MIN) {
-		IWL_WARN(priv,
-			 "Requested user TXPOWER %d below lower limit %d.\n",
-			 tx_power,
-			 IWLAGN_TX_POWER_TARGET_POWER_MIN);
-		return -EINVAL;
-	}
-
-	if (tx_power > priv->tx_power_device_lmt) {
-		IWL_WARN(priv,
-			"Requested user TXPOWER %d above upper limit %d.\n",
-			 tx_power, priv->tx_power_device_lmt);
-		return -EINVAL;
-	}
-
-	if (!iwl_is_ready_rf(priv))
-		return -EIO;
-
-	/* scan complete and commit_rxon use tx_power_next value,
-	 * it always need to be updated for newest request */
-	priv->tx_power_next = tx_power;
-
-	/* do not set tx power when scanning or channel changing */
-	defer = test_bit(STATUS_SCANNING, &priv->status) ||
-		memcmp(&ctx->active, &ctx->staging, sizeof(ctx->staging));
-	if (defer && !force) {
-		IWL_DEBUG_INFO(priv, "Deferring tx power set\n");
-		return 0;
-	}
-
-	prev_tx_power = priv->tx_power_user_lmt;
-	priv->tx_power_user_lmt = tx_power;
-
-	ret = iwlagn_send_tx_power(priv);
-
-	/* if fail to set tx_power, restore the orig. tx power */
-	if (ret) {
-		priv->tx_power_user_lmt = prev_tx_power;
-		priv->tx_power_next = prev_tx_power;
-	}
-	return ret;
-}
-
-void iwl_send_bt_config(struct iwl_priv *priv)
-{
-	struct iwl_bt_cmd bt_cmd = {
-		.lead_time = BT_LEAD_TIME_DEF,
-		.max_kill = BT_MAX_KILL_DEF,
-		.kill_ack_mask = 0,
-		.kill_cts_mask = 0,
-	};
-
-	if (!iwlagn_mod_params.bt_coex_active)
-		bt_cmd.flags = BT_COEX_DISABLE;
-	else
-		bt_cmd.flags = BT_COEX_ENABLE;
-
-	priv->bt_enable_flag = bt_cmd.flags;
-	IWL_DEBUG_INFO(priv, "BT coex %s\n",
-		(bt_cmd.flags == BT_COEX_DISABLE) ? "disable" : "active");
-
-	if (iwl_dvm_send_cmd_pdu(priv, REPLY_BT_CONFIG,
-			     CMD_SYNC, sizeof(struct iwl_bt_cmd), &bt_cmd))
-		IWL_ERR(priv, "failed to send BT Coex Config\n");
-}
-
-int iwl_send_statistics_request(struct iwl_priv *priv, u8 flags, bool clear)
-{
-	struct iwl_statistics_cmd statistics_cmd = {
-		.configuration_flags =
-			clear ? IWL_STATS_CONF_CLEAR_STATS : 0,
-	};
-
-	if (flags & CMD_ASYNC)
-		return iwl_dvm_send_cmd_pdu(priv, REPLY_STATISTICS_CMD,
-					      CMD_ASYNC,
-					       sizeof(struct iwl_statistics_cmd),
-					       &statistics_cmd);
-	else
-		return iwl_dvm_send_cmd_pdu(priv, REPLY_STATISTICS_CMD,
-					CMD_SYNC,
-					sizeof(struct iwl_statistics_cmd),
-					&statistics_cmd);
-}
-
-
-
-
-#ifdef CONFIG_IWLWIFI_DEBUGFS
-
-#define IWL_TRAFFIC_DUMP_SIZE	(IWL_TRAFFIC_ENTRY_SIZE * IWL_TRAFFIC_ENTRIES)
-
-void iwl_reset_traffic_log(struct iwl_priv *priv)
-{
-	priv->tx_traffic_idx = 0;
-	priv->rx_traffic_idx = 0;
-	if (priv->tx_traffic)
-		memset(priv->tx_traffic, 0, IWL_TRAFFIC_DUMP_SIZE);
-	if (priv->rx_traffic)
-		memset(priv->rx_traffic, 0, IWL_TRAFFIC_DUMP_SIZE);
-}
-
-int iwl_alloc_traffic_mem(struct iwl_priv *priv)
-{
-	u32 traffic_size = IWL_TRAFFIC_DUMP_SIZE;
-
-	if (iwl_have_debug_level(IWL_DL_TX)) {
-		if (!priv->tx_traffic) {
-			priv->tx_traffic =
-				kzalloc(traffic_size, GFP_KERNEL);
-			if (!priv->tx_traffic)
-				return -ENOMEM;
-		}
-	}
-	if (iwl_have_debug_level(IWL_DL_RX)) {
-		if (!priv->rx_traffic) {
-			priv->rx_traffic =
-				kzalloc(traffic_size, GFP_KERNEL);
-			if (!priv->rx_traffic)
-				return -ENOMEM;
-		}
-	}
-	iwl_reset_traffic_log(priv);
-	return 0;
-}
-
-void iwl_free_traffic_mem(struct iwl_priv *priv)
-{
-	kfree(priv->tx_traffic);
-	priv->tx_traffic = NULL;
-
-	kfree(priv->rx_traffic);
-	priv->rx_traffic = NULL;
-}
-
-void iwl_dbg_log_tx_data_frame(struct iwl_priv *priv,
-		      u16 length, struct ieee80211_hdr *header)
-{
-	__le16 fc;
-	u16 len;
-
-	if (likely(!iwl_have_debug_level(IWL_DL_TX)))
-		return;
-
-	if (!priv->tx_traffic)
-		return;
-
-	fc = header->frame_control;
-	if (ieee80211_is_data(fc)) {
-		len = (length > IWL_TRAFFIC_ENTRY_SIZE)
-		       ? IWL_TRAFFIC_ENTRY_SIZE : length;
-		memcpy((priv->tx_traffic +
-		       (priv->tx_traffic_idx * IWL_TRAFFIC_ENTRY_SIZE)),
-		       header, len);
-		priv->tx_traffic_idx =
-			(priv->tx_traffic_idx + 1) % IWL_TRAFFIC_ENTRIES;
-	}
-}
-
-void iwl_dbg_log_rx_data_frame(struct iwl_priv *priv,
-		      u16 length, struct ieee80211_hdr *header)
-{
-	__le16 fc;
-	u16 len;
-
-	if (likely(!iwl_have_debug_level(IWL_DL_RX)))
-		return;
-
-	if (!priv->rx_traffic)
-		return;
-
-	fc = header->frame_control;
-	if (ieee80211_is_data(fc)) {
-		len = (length > IWL_TRAFFIC_ENTRY_SIZE)
-		       ? IWL_TRAFFIC_ENTRY_SIZE : length;
-		memcpy((priv->rx_traffic +
-		       (priv->rx_traffic_idx * IWL_TRAFFIC_ENTRY_SIZE)),
-		       header, len);
-		priv->rx_traffic_idx =
-			(priv->rx_traffic_idx + 1) % IWL_TRAFFIC_ENTRIES;
-	}
-}
-
-const char *get_mgmt_string(int cmd)
-{
-	switch (cmd) {
-		IWL_CMD(MANAGEMENT_ASSOC_REQ);
-		IWL_CMD(MANAGEMENT_ASSOC_RESP);
-		IWL_CMD(MANAGEMENT_REASSOC_REQ);
-		IWL_CMD(MANAGEMENT_REASSOC_RESP);
-		IWL_CMD(MANAGEMENT_PROBE_REQ);
-		IWL_CMD(MANAGEMENT_PROBE_RESP);
-		IWL_CMD(MANAGEMENT_BEACON);
-		IWL_CMD(MANAGEMENT_ATIM);
-		IWL_CMD(MANAGEMENT_DISASSOC);
-		IWL_CMD(MANAGEMENT_AUTH);
-		IWL_CMD(MANAGEMENT_DEAUTH);
-		IWL_CMD(MANAGEMENT_ACTION);
-	default:
-		return "UNKNOWN";
-
-	}
-}
-
-const char *get_ctrl_string(int cmd)
-{
-	switch (cmd) {
-		IWL_CMD(CONTROL_BACK_REQ);
-		IWL_CMD(CONTROL_BACK);
-		IWL_CMD(CONTROL_PSPOLL);
-		IWL_CMD(CONTROL_RTS);
-		IWL_CMD(CONTROL_CTS);
-		IWL_CMD(CONTROL_ACK);
-		IWL_CMD(CONTROL_CFEND);
-		IWL_CMD(CONTROL_CFENDACK);
-	default:
-		return "UNKNOWN";
-
-	}
-}
-
-void iwl_clear_traffic_stats(struct iwl_priv *priv)
-{
-	memset(&priv->tx_stats, 0, sizeof(struct traffic_stats));
-	memset(&priv->rx_stats, 0, sizeof(struct traffic_stats));
-}
-
-/*
- * if CONFIG_IWLWIFI_DEBUGFS defined, iwl_update_stats function will
- * record all the MGMT, CTRL and DATA pkt for both TX and Rx pass.
- * Use debugFs to display the rx/rx_statistics
- * if CONFIG_IWLWIFI_DEBUGFS not being defined, then no MGMT and CTRL
- * information will be recorded, but DATA pkt still will be recorded
- * for the reason of iwl_led.c need to control the led blinking based on
- * number of tx and rx data.
- *
- */
-void iwl_update_stats(struct iwl_priv *priv, bool is_tx, __le16 fc, u16 len)
-{
-	struct traffic_stats	*stats;
-
-	if (is_tx)
-		stats = &priv->tx_stats;
-	else
-		stats = &priv->rx_stats;
-
-	if (ieee80211_is_mgmt(fc)) {
-		switch (fc & cpu_to_le16(IEEE80211_FCTL_STYPE)) {
-		case cpu_to_le16(IEEE80211_STYPE_ASSOC_REQ):
-			stats->mgmt[MANAGEMENT_ASSOC_REQ]++;
-			break;
-		case cpu_to_le16(IEEE80211_STYPE_ASSOC_RESP):
-			stats->mgmt[MANAGEMENT_ASSOC_RESP]++;
-			break;
-		case cpu_to_le16(IEEE80211_STYPE_REASSOC_REQ):
-			stats->mgmt[MANAGEMENT_REASSOC_REQ]++;
-			break;
-		case cpu_to_le16(IEEE80211_STYPE_REASSOC_RESP):
-			stats->mgmt[MANAGEMENT_REASSOC_RESP]++;
-			break;
-		case cpu_to_le16(IEEE80211_STYPE_PROBE_REQ):
-			stats->mgmt[MANAGEMENT_PROBE_REQ]++;
-			break;
-		case cpu_to_le16(IEEE80211_STYPE_PROBE_RESP):
-			stats->mgmt[MANAGEMENT_PROBE_RESP]++;
-			break;
-		case cpu_to_le16(IEEE80211_STYPE_BEACON):
-			stats->mgmt[MANAGEMENT_BEACON]++;
-			break;
-		case cpu_to_le16(IEEE80211_STYPE_ATIM):
-			stats->mgmt[MANAGEMENT_ATIM]++;
-			break;
-		case cpu_to_le16(IEEE80211_STYPE_DISASSOC):
-			stats->mgmt[MANAGEMENT_DISASSOC]++;
-			break;
-		case cpu_to_le16(IEEE80211_STYPE_AUTH):
-			stats->mgmt[MANAGEMENT_AUTH]++;
-			break;
-		case cpu_to_le16(IEEE80211_STYPE_DEAUTH):
-			stats->mgmt[MANAGEMENT_DEAUTH]++;
-			break;
-		case cpu_to_le16(IEEE80211_STYPE_ACTION):
-			stats->mgmt[MANAGEMENT_ACTION]++;
-			break;
-		}
-	} else if (ieee80211_is_ctl(fc)) {
-		switch (fc & cpu_to_le16(IEEE80211_FCTL_STYPE)) {
-		case cpu_to_le16(IEEE80211_STYPE_BACK_REQ):
-			stats->ctrl[CONTROL_BACK_REQ]++;
-			break;
-		case cpu_to_le16(IEEE80211_STYPE_BACK):
-			stats->ctrl[CONTROL_BACK]++;
-			break;
-		case cpu_to_le16(IEEE80211_STYPE_PSPOLL):
-			stats->ctrl[CONTROL_PSPOLL]++;
-			break;
-		case cpu_to_le16(IEEE80211_STYPE_RTS):
-			stats->ctrl[CONTROL_RTS]++;
-			break;
-		case cpu_to_le16(IEEE80211_STYPE_CTS):
-			stats->ctrl[CONTROL_CTS]++;
-			break;
-		case cpu_to_le16(IEEE80211_STYPE_ACK):
-			stats->ctrl[CONTROL_ACK]++;
-			break;
-		case cpu_to_le16(IEEE80211_STYPE_CFEND):
-			stats->ctrl[CONTROL_CFEND]++;
-			break;
-		case cpu_to_le16(IEEE80211_STYPE_CFENDACK):
-			stats->ctrl[CONTROL_CFENDACK]++;
-			break;
-		}
-	} else {
-		/* data */
-		stats->data_cnt++;
-		stats->data_bytes += len;
-	}
-}
-#endif
-
-static void iwl_force_rf_reset(struct iwl_priv *priv)
-{
-	if (test_bit(STATUS_EXIT_PENDING, &priv->status))
-		return;
-
-	if (!iwl_is_any_associated(priv)) {
-		IWL_DEBUG_SCAN(priv, "force reset rejected: not associated\n");
-		return;
-	}
-	/*
-	 * There is no easy and better way to force reset the radio,
-	 * the only known method is switching channel which will force to
-	 * reset and tune the radio.
-	 * Use internal short scan (single channel) operation to should
-	 * achieve this objective.
-	 * Driver should reset the radio when number of consecutive missed
-	 * beacon, or any other uCode error condition detected.
-	 */
-	IWL_DEBUG_INFO(priv, "perform radio reset.\n");
-	iwl_internal_short_hw_scan(priv);
-}
-
-
-int iwl_force_reset(struct iwl_priv *priv, int mode, bool external)
-{
-	struct iwl_force_reset *force_reset;
-
-	if (test_bit(STATUS_EXIT_PENDING, &priv->status))
-		return -EINVAL;
-
-	if (mode >= IWL_MAX_FORCE_RESET) {
-		IWL_DEBUG_INFO(priv, "invalid reset request.\n");
-		return -EINVAL;
-	}
-	force_reset = &priv->force_reset[mode];
-	force_reset->reset_request_count++;
-	if (!external) {
-		if (force_reset->last_force_reset_jiffies &&
-		    time_after(force_reset->last_force_reset_jiffies +
-		    force_reset->reset_duration, jiffies)) {
-			IWL_DEBUG_INFO(priv, "force reset rejected\n");
-			force_reset->reset_reject_count++;
-			return -EAGAIN;
-		}
-	}
-	force_reset->reset_success_count++;
-	force_reset->last_force_reset_jiffies = jiffies;
-	IWL_DEBUG_INFO(priv, "perform force reset (%d)\n", mode);
-	switch (mode) {
-	case IWL_RF_RESET:
-		iwl_force_rf_reset(priv);
-		break;
-	case IWL_FW_RESET:
-		/*
-		 * if the request is from external(ex: debugfs),
-		 * then always perform the request in regardless the module
-		 * parameter setting
-		 * if the request is from internal (uCode error or driver
-		 * detect failure), then fw_restart module parameter
-		 * need to be check before performing firmware reload
-		 */
-		if (!external && !iwlagn_mod_params.restart_fw) {
-			IWL_DEBUG_INFO(priv, "Cancel firmware reload based on "
-				       "module parameter setting\n");
-			break;
-		}
-		IWL_ERR(priv, "On demand firmware reload\n");
-		iwlagn_fw_error(priv, true);
-		break;
-	}
-	return 0;
-}
-
-
-int iwl_cmd_echo_test(struct iwl_priv *priv)
-{
-	int ret;
-	struct iwl_host_cmd cmd = {
-		.id = REPLY_ECHO,
-		.len = { 0 },
-		.flags = CMD_SYNC,
-	};
-
-	ret = iwl_dvm_send_cmd(priv, &cmd);
-	if (ret)
-		IWL_ERR(priv, "echo testing fail: 0X%x\n", ret);
-	else
-		IWL_DEBUG_INFO(priv, "echo testing pass\n");
-	return ret;
-}
-
-static inline int iwl_check_stuck_queue(struct iwl_priv *priv, int txq)
-{
-	if (iwl_trans_check_stuck_queue(trans(priv), txq)) {
-		int ret;
-		ret = iwl_force_reset(priv, IWL_FW_RESET, false);
-		return (ret == -EAGAIN) ? 0 : 1;
-	}
-	return 0;
-}
-
-/*
- * Making watchdog tick be a quarter of timeout assure we will
- * discover the queue hung between timeout and 1.25*timeout
- */
-#define IWL_WD_TICK(timeout) ((timeout) / 4)
-
-/*
- * Watchdog timer callback, we check each tx queue for stuck, if if hung
- * we reset the firmware. If everything is fine just rearm the timer.
- */
-void iwl_bg_watchdog(unsigned long data)
-{
-	struct iwl_priv *priv = (struct iwl_priv *)data;
-	int cnt;
-	unsigned long timeout;
-
-	if (test_bit(STATUS_EXIT_PENDING, &priv->status))
-		return;
-
-	if (iwl_is_rfkill(priv))
-		return;
-
-	timeout = hw_params(priv).wd_timeout;
-	if (timeout == 0)
-		return;
-
-	/* monitor and check for stuck queues */
-	for (cnt = 0; cnt < cfg(priv)->base_params->num_of_queues; cnt++)
-		if (iwl_check_stuck_queue(priv, cnt))
-			return;
-
-	mod_timer(&priv->watchdog, jiffies +
-		  msecs_to_jiffies(IWL_WD_TICK(timeout)));
-}
-
-void iwl_setup_watchdog(struct iwl_priv *priv)
-{
-	unsigned int timeout = hw_params(priv).wd_timeout;
-
-	if (!iwlagn_mod_params.wd_disable) {
-		/* use system default */
-		if (timeout && !cfg(priv)->base_params->wd_disable)
-			mod_timer(&priv->watchdog,
-				jiffies +
-				msecs_to_jiffies(IWL_WD_TICK(timeout)));
-		else
-			del_timer(&priv->watchdog);
-	} else {
-		/* module parameter overwrite default configuration */
-		if (timeout && iwlagn_mod_params.wd_disable == 2)
-			mod_timer(&priv->watchdog,
-				jiffies +
-				msecs_to_jiffies(IWL_WD_TICK(timeout)));
-		else
-			del_timer(&priv->watchdog);
-	}
-}
-
-/**
- * iwl_beacon_time_mask_low - mask of lower 32 bit of beacon time
- * @priv -- pointer to iwl_priv data structure
- * @tsf_bits -- number of bits need to shift for masking)
- */
-static inline u32 iwl_beacon_time_mask_low(struct iwl_priv *priv,
-					   u16 tsf_bits)
-{
-	return (1 << tsf_bits) - 1;
-}
-
-/**
- * iwl_beacon_time_mask_high - mask of higher 32 bit of beacon time
- * @priv -- pointer to iwl_priv data structure
- * @tsf_bits -- number of bits need to shift for masking)
- */
-static inline u32 iwl_beacon_time_mask_high(struct iwl_priv *priv,
-					    u16 tsf_bits)
-{
-	return ((1 << (32 - tsf_bits)) - 1) << tsf_bits;
-}
-
-/*
- * extended beacon time format
- * time in usec will be changed into a 32-bit value in extended:internal format
- * the extended part is the beacon counts
- * the internal part is the time in usec within one beacon interval
- */
-u32 iwl_usecs_to_beacons(struct iwl_priv *priv, u32 usec, u32 beacon_interval)
-{
-	u32 quot;
-	u32 rem;
-	u32 interval = beacon_interval * TIME_UNIT;
-
-	if (!interval || !usec)
-		return 0;
-
-	quot = (usec / interval) &
-		(iwl_beacon_time_mask_high(priv, IWLAGN_EXT_BEACON_TIME_POS) >>
-		IWLAGN_EXT_BEACON_TIME_POS);
-	rem = (usec % interval) & iwl_beacon_time_mask_low(priv,
-				   IWLAGN_EXT_BEACON_TIME_POS);
-
-	return (quot << IWLAGN_EXT_BEACON_TIME_POS) + rem;
-}
-
-/* base is usually what we get from ucode with each received frame,
- * the same as HW timer counter counting down
- */
-__le32 iwl_add_beacon_time(struct iwl_priv *priv, u32 base,
-			   u32 addon, u32 beacon_interval)
-{
-	u32 base_low = base & iwl_beacon_time_mask_low(priv,
-				IWLAGN_EXT_BEACON_TIME_POS);
-	u32 addon_low = addon & iwl_beacon_time_mask_low(priv,
-				IWLAGN_EXT_BEACON_TIME_POS);
-	u32 interval = beacon_interval * TIME_UNIT;
-	u32 res = (base & iwl_beacon_time_mask_high(priv,
-				IWLAGN_EXT_BEACON_TIME_POS)) +
-				(addon & iwl_beacon_time_mask_high(priv,
-				IWLAGN_EXT_BEACON_TIME_POS));
-
-	if (base_low > addon_low)
-		res += base_low - addon_low;
-	else if (base_low < addon_low) {
-		res += interval + base_low - addon_low;
-		res += (1 << IWLAGN_EXT_BEACON_TIME_POS);
-	} else
-		res += (1 << IWLAGN_EXT_BEACON_TIME_POS);
-
-	return cpu_to_le32(res);
-}
-
-void iwl_nic_error(struct iwl_op_mode *op_mode)
-{
-	struct iwl_priv *priv = IWL_OP_MODE_GET_DVM(op_mode);
-
-	iwlagn_fw_error(priv, false);
-}
-
-void iwl_set_hw_rfkill_state(struct iwl_op_mode *op_mode, bool state)
-{
-	struct iwl_priv *priv = IWL_OP_MODE_GET_DVM(op_mode);
-
-	if (state)
-		set_bit(STATUS_RF_KILL_HW, &priv->status);
-	else
-		clear_bit(STATUS_RF_KILL_HW, &priv->status);
-
-	wiphy_rfkill_set_hw_state(priv->hw->wiphy, state);
-}
-
-void iwl_free_skb(struct iwl_op_mode *op_mode, struct sk_buff *skb)
-{
-	struct ieee80211_tx_info *info;
-
-	info = IEEE80211_SKB_CB(skb);
-	kmem_cache_free(iwl_tx_cmd_pool, (info->driver_data[1]));
-	dev_kfree_skb_any(skb);
-}
diff --git a/drivers/net/wireless/iwlwifi/iwl-core.h b/drivers/net/wireless/iwlwifi/iwl-core.h
deleted file mode 100644
index 635eb685ede..00000000000
--- a/drivers/net/wireless/iwlwifi/iwl-core.h
+++ /dev/null
@@ -1,234 +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) 2008 - 2012 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 - 2012 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_core_h__
-#define __iwl_core_h__
-
-#include "iwl-dev.h"
-#include "iwl-io.h"
-
-/************************
- * forward declarations *
- ************************/
-struct iwl_host_cmd;
-struct iwl_cmd;
-
-#define TIME_UNIT		1024
-
-struct iwl_lib_ops {
-	/* set hw dependent parameters */
-	void (*set_hw_params)(struct iwl_priv *priv);
-	int (*set_channel_switch)(struct iwl_priv *priv,
-				  struct ieee80211_channel_switch *ch_switch);
-	/* device specific configuration */
-	void (*nic_config)(struct iwl_priv *priv);
-
-	/* eeprom operations (as defined in iwl-eeprom.h) */
-	struct iwl_eeprom_ops eeprom_ops;
-
-	/* temperature */
-	void (*temperature)(struct iwl_priv *priv);
-};
-
-/***************************
- *   L i b                 *
- ***************************/
-
-void iwl_set_rxon_hwcrypto(struct iwl_priv *priv, struct iwl_rxon_context *ctx,
-			   int hw_decrypt);
-int iwl_check_rxon_cmd(struct iwl_priv *priv, struct iwl_rxon_context *ctx);
-int iwl_full_rxon_required(struct iwl_priv *priv, struct iwl_rxon_context *ctx);
-void iwl_set_rxon_channel(struct iwl_priv *priv, struct ieee80211_channel *ch,
-			 struct iwl_rxon_context *ctx);
-void iwl_set_flags_for_band(struct iwl_priv *priv,
-			    struct iwl_rxon_context *ctx,
-			    enum ieee80211_band band,
-			    struct ieee80211_vif *vif);
-u8 iwl_get_single_channel_number(struct iwl_priv *priv,
-				  enum ieee80211_band band);
-void iwl_set_rxon_ht(struct iwl_priv *priv, struct iwl_ht_config *ht_conf);
-bool iwl_is_ht40_tx_allowed(struct iwl_priv *priv,
-			    struct iwl_rxon_context *ctx,
-			    struct ieee80211_sta_ht_cap *ht_cap);
-void iwl_connection_init_rx_config(struct iwl_priv *priv,
-				   struct iwl_rxon_context *ctx);
-void iwl_set_rate(struct iwl_priv *priv);
-int iwl_cmd_echo_test(struct iwl_priv *priv);
-#ifdef CONFIG_IWLWIFI_DEBUGFS
-int iwl_alloc_traffic_mem(struct iwl_priv *priv);
-void iwl_free_traffic_mem(struct iwl_priv *priv);
-void iwl_dbg_log_tx_data_frame(struct iwl_priv *priv,
-				u16 length, struct ieee80211_hdr *header);
-void iwl_dbg_log_rx_data_frame(struct iwl_priv *priv,
-				u16 length, struct ieee80211_hdr *header);
-const char *get_mgmt_string(int cmd);
-const char *get_ctrl_string(int cmd);
-void iwl_clear_traffic_stats(struct iwl_priv *priv);
-void iwl_update_stats(struct iwl_priv *priv, bool is_tx, __le16 fc,
-		      u16 len);
-void iwl_reset_traffic_log(struct iwl_priv *priv);
-
-#else
-static inline int iwl_alloc_traffic_mem(struct iwl_priv *priv)
-{
-	return 0;
-}
-static inline void iwl_free_traffic_mem(struct iwl_priv *priv)
-{
-}
-static inline void iwl_reset_traffic_log(struct iwl_priv *priv)
-{
-}
-static inline void iwl_dbg_log_tx_data_frame(struct iwl_priv *priv,
-		      u16 length, struct ieee80211_hdr *header)
-{
-}
-static inline void iwl_dbg_log_rx_data_frame(struct iwl_priv *priv,
-		      u16 length, struct ieee80211_hdr *header)
-{
-}
-static inline void iwl_update_stats(struct iwl_priv *priv, bool is_tx,
-				    __le16 fc, u16 len)
-{
-}
-#endif
-
-/*****************************************************
-* RX
-******************************************************/
-void iwl_chswitch_done(struct iwl_priv *priv, bool is_success);
-
-void iwl_setup_watchdog(struct iwl_priv *priv);
-/*****************************************************
- * TX power
- ****************************************************/
-int iwl_set_tx_power(struct iwl_priv *priv, s8 tx_power, bool force);
-
-/*******************************************************************************
- * Scanning
- ******************************************************************************/
-void iwl_init_scan_params(struct iwl_priv *priv);
-int iwl_scan_cancel(struct iwl_priv *priv);
-void iwl_scan_cancel_timeout(struct iwl_priv *priv, unsigned long ms);
-void iwl_force_scan_end(struct iwl_priv *priv);
-void iwl_internal_short_hw_scan(struct iwl_priv *priv);
-int iwl_force_reset(struct iwl_priv *priv, int mode, bool external);
-void iwl_setup_rx_scan_handlers(struct iwl_priv *priv);
-void iwl_setup_scan_deferred_work(struct iwl_priv *priv);
-void iwl_cancel_scan_deferred_work(struct iwl_priv *priv);
-int __must_check iwl_scan_initiate(struct iwl_priv *priv,
-				   struct ieee80211_vif *vif,
-				   enum iwl_scan_type scan_type,
-				   enum ieee80211_band band);
-
-/* For faster active scanning, scan will move to the next channel if fewer than
- * PLCP_QUIET_THRESH packets are heard on this channel within
- * ACTIVE_QUIET_TIME after sending probe request.  This shortens the dwell
- * time if it's a quiet channel (nothing responded to our probe, and there's
- * no other traffic).
- * Disable "quiet" feature by setting PLCP_QUIET_THRESH to 0. */
-#define IWL_ACTIVE_QUIET_TIME       cpu_to_le16(10)  /* msec */
-#define IWL_PLCP_QUIET_THRESH       cpu_to_le16(1)  /* packets */
-
-#define IWL_SCAN_CHECK_WATCHDOG		(HZ * 7)
-
-/* traffic log definitions */
-#define IWL_TRAFFIC_ENTRIES	(256)
-#define IWL_TRAFFIC_ENTRY_SIZE  (64)
-
-/*****************************************************
- *   S e n d i n g     H o s t     C o m m a n d s   *
- *****************************************************/
-
-void iwl_bg_watchdog(unsigned long data);
-u32 iwl_usecs_to_beacons(struct iwl_priv *priv, u32 usec, u32 beacon_interval);
-__le32 iwl_add_beacon_time(struct iwl_priv *priv, u32 base,
-			   u32 addon, u32 beacon_interval);
-
-
-/*****************************************************
-*  GEOS
-******************************************************/
-int iwl_init_geos(struct iwl_priv *priv);
-void iwl_free_geos(struct iwl_priv *priv);
-
-extern void iwl_send_bt_config(struct iwl_priv *priv);
-extern int iwl_send_statistics_request(struct iwl_priv *priv,
-				       u8 flags, bool clear);
-
-int iwl_send_rxon_timing(struct iwl_priv *priv, struct iwl_rxon_context *ctx);
-
-static inline const struct ieee80211_supported_band *iwl_get_hw_mode(
-			struct iwl_priv *priv, enum ieee80211_band band)
-{
-	return priv->hw->wiphy->bands[band];
-}
-
-static inline bool iwl_advanced_bt_coexist(struct iwl_priv *priv)
-{
-	return cfg(priv)->bt_params &&
-	       cfg(priv)->bt_params->advanced_bt_coexist;
-}
-
-extern bool bt_siso_mode;
-
-#endif /* __iwl_core_h__ */
diff --git a/drivers/net/wireless/iwlwifi/iwl-csr.h b/drivers/net/wireless/iwlwifi/iwl-csr.h
index 5f96ce105f0..59750543fce 100644
--- a/drivers/net/wireless/iwlwifi/iwl-csr.h
+++ b/drivers/net/wireless/iwlwifi/iwl-csr.h
@@ -430,6 +430,9 @@
 #define HBUS_TARG_PRPH_WDAT     (HBUS_BASE+0x04c)
 #define HBUS_TARG_PRPH_RDAT     (HBUS_BASE+0x050)
 
+/* Used to enable DBGM */
+#define HBUS_TARG_TEST_REG	(HBUS_BASE+0x05c)
+
 /*
  * Per-Tx-queue write pointer (index, really!)
  * Indicates index to next TFD that driver will fill (1 past latest filled).
diff --git a/drivers/net/wireless/iwlwifi/iwl-debug.c b/drivers/net/wireless/iwlwifi/iwl-debug.c
index 059efabda18..2d1b42847b9 100644
--- a/drivers/net/wireless/iwlwifi/iwl-debug.c
+++ b/drivers/net/wireless/iwlwifi/iwl-debug.c
@@ -63,6 +63,7 @@
 
 #include <linux/interrupt.h>
 #include "iwl-debug.h"
+#include "iwl-devtrace.h"
 
 #define __iwl_fn(fn)						\
 void __iwl_ ##fn(struct device *dev, const char *fmt, ...)	\
diff --git a/drivers/net/wireless/iwlwifi/iwl-debug.h b/drivers/net/wireless/iwlwifi/iwl-debug.h
index a6b32a11e10..8376b842bdb 100644
--- a/drivers/net/wireless/iwlwifi/iwl-debug.h
+++ b/drivers/net/wireless/iwlwifi/iwl-debug.h
@@ -29,10 +29,13 @@
 #ifndef __iwl_debug_h__
 #define __iwl_debug_h__
 
-#include "iwl-shared.h"
-#include "iwl-devtrace.h"
+#include "iwl-modparams.h"
 
-struct iwl_priv;
+
+static inline bool iwl_have_debug_level(u32 level)
+{
+	return iwlwifi_mod_params.debug_level & level;
+}
 
 void __iwl_err(struct device *dev, bool rfkill_prefix, bool only_trace,
 		const char *fmt, ...);
@@ -41,10 +44,10 @@ void __iwl_info(struct device *dev, const char *fmt, ...);
 void __iwl_crit(struct device *dev, const char *fmt, ...);
 
 /* No matter what is m (priv, bus, trans), this will work */
-#define IWL_ERR(m, f, a...) __iwl_err(trans(m)->dev, false, false, f, ## a)
-#define IWL_WARN(m, f, a...) __iwl_warn(trans(m)->dev, f, ## a)
-#define IWL_INFO(m, f, a...) __iwl_info(trans(m)->dev, f, ## a)
-#define IWL_CRIT(m, f, a...) __iwl_crit(trans(m)->dev, f, ## a)
+#define IWL_ERR(m, f, a...) __iwl_err((m)->dev, false, false, f, ## a)
+#define IWL_WARN(m, f, a...) __iwl_warn((m)->dev, f, ## a)
+#define IWL_INFO(m, f, a...) __iwl_info((m)->dev, f, ## a)
+#define IWL_CRIT(m, f, a...) __iwl_crit((m)->dev, f, ## a)
 
 #if defined(CONFIG_IWLWIFI_DEBUG) || defined(CONFIG_IWLWIFI_DEVICE_TRACING)
 void __iwl_dbg(struct device *dev,
@@ -65,9 +68,9 @@ do {									\
 } while (0)
 
 #define IWL_DEBUG(m, level, fmt, args...)				\
-	__iwl_dbg(trans(m)->dev, level, false, __func__, fmt, ##args)
+	__iwl_dbg((m)->dev, level, false, __func__, fmt, ##args)
 #define IWL_DEBUG_LIMIT(m, level, fmt, args...)				\
-	__iwl_dbg(trans(m)->dev, level, true, __func__, fmt, ##args)
+	__iwl_dbg((m)->dev, level, true, __func__, fmt, ##args)
 
 #ifdef CONFIG_IWLWIFI_DEBUG
 #define iwl_print_hex_dump(m, level, p, len)				\
@@ -80,19 +83,6 @@ do {                                            			\
 #define iwl_print_hex_dump(m, level, p, len)
 #endif				/* CONFIG_IWLWIFI_DEBUG */
 
-#ifdef CONFIG_IWLWIFI_DEBUGFS
-int iwl_dbgfs_register(struct iwl_priv *priv, const char *name);
-void iwl_dbgfs_unregister(struct iwl_priv *priv);
-#else
-static inline int iwl_dbgfs_register(struct iwl_priv *priv, const char *name)
-{
-	return 0;
-}
-static inline void iwl_dbgfs_unregister(struct iwl_priv *priv)
-{
-}
-#endif				/* CONFIG_IWLWIFI_DEBUGFS */
-
 /*
  * To use the debug system:
  *
diff --git a/drivers/net/wireless/iwlwifi/iwl-debugfs.c b/drivers/net/wireless/iwlwifi/iwl-debugfs.c
index 2bbaebd99ad..e7c157e5ebe 100644
--- a/drivers/net/wireless/iwlwifi/iwl-debugfs.c
+++ b/drivers/net/wireless/iwlwifi/iwl-debugfs.c
@@ -37,9 +37,9 @@
 
 #include "iwl-dev.h"
 #include "iwl-debug.h"
-#include "iwl-core.h"
 #include "iwl-io.h"
 #include "iwl-agn.h"
+#include "iwl-modparams.h"
 
 /* create and remove of files */
 #define DEBUGFS_ADD_FILE(name, parent, mode) do {			\
@@ -111,105 +111,6 @@ static const struct file_operations iwl_dbgfs_##name##_ops = {          \
 	.llseek = generic_file_llseek,					\
 };
 
-static ssize_t iwl_dbgfs_tx_statistics_read(struct file *file,
-						char __user *user_buf,
-						size_t count, loff_t *ppos) {
-
-	struct iwl_priv *priv = file->private_data;
-	char *buf;
-	int pos = 0;
-
-	int cnt;
-	ssize_t ret;
-	const size_t bufsz = 100 +
-		sizeof(char) * 50 * (MANAGEMENT_MAX + CONTROL_MAX);
-	buf = kzalloc(bufsz, GFP_KERNEL);
-	if (!buf)
-		return -ENOMEM;
-	pos += scnprintf(buf + pos, bufsz - pos, "Management:\n");
-	for (cnt = 0; cnt < MANAGEMENT_MAX; cnt++) {
-		pos += scnprintf(buf + pos, bufsz - pos,
-				 "\t%25s\t\t: %u\n",
-				 get_mgmt_string(cnt),
-				 priv->tx_stats.mgmt[cnt]);
-	}
-	pos += scnprintf(buf + pos, bufsz - pos, "Control\n");
-	for (cnt = 0; cnt < CONTROL_MAX; cnt++) {
-		pos += scnprintf(buf + pos, bufsz - pos,
-				 "\t%25s\t\t: %u\n",
-				 get_ctrl_string(cnt),
-				 priv->tx_stats.ctrl[cnt]);
-	}
-	pos += scnprintf(buf + pos, bufsz - pos, "Data:\n");
-	pos += scnprintf(buf + pos, bufsz - pos, "\tcnt: %u\n",
-			 priv->tx_stats.data_cnt);
-	pos += scnprintf(buf + pos, bufsz - pos, "\tbytes: %llu\n",
-			 priv->tx_stats.data_bytes);
-	ret = simple_read_from_buffer(user_buf, count, ppos, buf, pos);
-	kfree(buf);
-	return ret;
-}
-
-static ssize_t iwl_dbgfs_clear_traffic_statistics_write(struct file *file,
-					const char __user *user_buf,
-					size_t count, loff_t *ppos)
-{
-	struct iwl_priv *priv = file->private_data;
-	u32 clear_flag;
-	char buf[8];
-	int buf_size;
-
-	memset(buf, 0, sizeof(buf));
-	buf_size = min(count, sizeof(buf) -  1);
-	if (copy_from_user(buf, user_buf, buf_size))
-		return -EFAULT;
-	if (sscanf(buf, "%x", &clear_flag) != 1)
-		return -EFAULT;
-	iwl_clear_traffic_stats(priv);
-
-	return count;
-}
-
-static ssize_t iwl_dbgfs_rx_statistics_read(struct file *file,
-						char __user *user_buf,
-						size_t count, loff_t *ppos) {
-
-	struct iwl_priv *priv = file->private_data;
-	char *buf;
-	int pos = 0;
-	int cnt;
-	ssize_t ret;
-	const size_t bufsz = 100 +
-		sizeof(char) * 50 * (MANAGEMENT_MAX + CONTROL_MAX);
-	buf = kzalloc(bufsz, GFP_KERNEL);
-	if (!buf)
-		return -ENOMEM;
-
-	pos += scnprintf(buf + pos, bufsz - pos, "Management:\n");
-	for (cnt = 0; cnt < MANAGEMENT_MAX; cnt++) {
-		pos += scnprintf(buf + pos, bufsz - pos,
-				 "\t%25s\t\t: %u\n",
-				 get_mgmt_string(cnt),
-				 priv->rx_stats.mgmt[cnt]);
-	}
-	pos += scnprintf(buf + pos, bufsz - pos, "Control:\n");
-	for (cnt = 0; cnt < CONTROL_MAX; cnt++) {
-		pos += scnprintf(buf + pos, bufsz - pos,
-				 "\t%25s\t\t: %u\n",
-				 get_ctrl_string(cnt),
-				 priv->rx_stats.ctrl[cnt]);
-	}
-	pos += scnprintf(buf + pos, bufsz - pos, "Data:\n");
-	pos += scnprintf(buf + pos, bufsz - pos, "\tcnt: %u\n",
-			 priv->rx_stats.data_cnt);
-	pos += scnprintf(buf + pos, bufsz - pos, "\tbytes: %llu\n",
-			 priv->rx_stats.data_bytes);
-
-	ret = simple_read_from_buffer(user_buf, count, ppos, buf, pos);
-	kfree(buf);
-	return ret;
-}
-
 static ssize_t iwl_dbgfs_sram_read(struct file *file,
 					char __user *user_buf,
 					size_t count, loff_t *ppos)
@@ -230,11 +131,9 @@ static ssize_t iwl_dbgfs_sram_read(struct file *file,
 	/* default is to dump the entire data segment */
 	if (!priv->dbgfs_sram_offset && !priv->dbgfs_sram_len) {
 		priv->dbgfs_sram_offset = 0x800000;
-		if (!priv->ucode_loaded) {
-			IWL_ERR(priv, "No uCode has been loadded.\n");
+		if (!priv->ucode_loaded)
 			return -EINVAL;
-		}
-		img = &priv->fw->img[priv->shrd->ucode_type];
+		img = &priv->fw->img[priv->cur_ucode];
 		priv->dbgfs_sram_len = img->sec[IWL_UCODE_SECTION_DATA].len;
 	}
 	len = priv->dbgfs_sram_len;
@@ -259,7 +158,7 @@ static ssize_t iwl_dbgfs_sram_read(struct file *file,
 	sram = priv->dbgfs_sram_offset & ~0x3;
 
 	/* read the first u32 from sram */
-	val = iwl_read_targ_mem(trans(priv), sram);
+	val = iwl_read_targ_mem(priv->trans, sram);
 
 	for (; len; len--) {
 		/* put the address at the start of every line */
@@ -278,7 +177,7 @@ static ssize_t iwl_dbgfs_sram_read(struct file *file,
 		if (++offset == 4) {
 			sram += 4;
 			offset = 0;
-			val = iwl_read_targ_mem(trans(priv), sram);
+			val = iwl_read_targ_mem(priv->trans, sram);
 		}
 
 		/* put in extra spaces and split lines for human readability */
@@ -369,14 +268,19 @@ static ssize_t iwl_dbgfs_stations_read(struct file *file, char __user *user_buf,
 				 i, station->sta.sta.addr,
 				 station->sta.station_flags_msk);
 		pos += scnprintf(buf + pos, bufsz - pos,
-				"TID\tseq_num\trate_n_flags\n");
+				"TID seqno  next_rclmd "
+				"rate_n_flags state txq\n");
 
 		for (j = 0; j < IWL_MAX_TID_COUNT; j++) {
 			tid_data = &priv->tid_data[i][j];
 			pos += scnprintf(buf + pos, bufsz - pos,
-				"%d:\t%#x\t%#x",
+				"%d:  0x%.4x 0x%.4x     0x%.8x   "
+				"%d     %.2d",
 				j, tid_data->seq_number,
-				tid_data->agg.rate_n_flags);
+				tid_data->next_reclaimed,
+				tid_data->agg.rate_n_flags,
+				tid_data->agg.state,
+				tid_data->agg.txq_id);
 
 			if (tid_data->agg.wait_for_ba)
 				pos += scnprintf(buf + pos, bufsz - pos,
@@ -403,30 +307,25 @@ static ssize_t iwl_dbgfs_nvm_read(struct file *file,
 	const u8 *ptr;
 	char *buf;
 	u16 eeprom_ver;
-	size_t eeprom_len = cfg(priv)->base_params->eeprom_size;
+	size_t eeprom_len = priv->cfg->base_params->eeprom_size;
 	buf_size = 4 * eeprom_len + 256;
 
-	if (eeprom_len % 16) {
-		IWL_ERR(priv, "NVM size is not multiple of 16.\n");
+	if (eeprom_len % 16)
 		return -ENODATA;
-	}
 
-	ptr = priv->shrd->eeprom;
-	if (!ptr) {
-		IWL_ERR(priv, "Invalid EEPROM/OTP memory\n");
+	ptr = priv->eeprom;
+	if (!ptr)
 		return -ENOMEM;
-	}
 
 	/* 4 characters for byte 0xYY */
 	buf = kzalloc(buf_size, GFP_KERNEL);
-	if (!buf) {
-		IWL_ERR(priv, "Can not allocate Buffer\n");
+	if (!buf)
 		return -ENOMEM;
-	}
-	eeprom_ver = iwl_eeprom_query16(priv->shrd, EEPROM_VERSION);
+
+	eeprom_ver = iwl_eeprom_query16(priv, EEPROM_VERSION);
 	pos += scnprintf(buf + pos, buf_size - pos, "NVM Type: %s, "
 			"version: 0x%x\n",
-			(trans(priv)->nvm_device_type == NVM_DEVICE_TYPE_OTP)
+			(priv->nvm_device_type == NVM_DEVICE_TYPE_OTP)
 			 ? "OTP" : "EEPROM", eeprom_ver);
 	for (ofs = 0 ; ofs < eeprom_len ; ofs += 16) {
 		pos += scnprintf(buf + pos, buf_size - pos, "0x%.4x ", ofs);
@@ -456,10 +355,8 @@ static ssize_t iwl_dbgfs_channels_read(struct file *file, char __user *user_buf,
 		return -EAGAIN;
 
 	buf = kzalloc(bufsz, GFP_KERNEL);
-	if (!buf) {
-		IWL_ERR(priv, "Can not allocate Buffer\n");
+	if (!buf)
 		return -ENOMEM;
-	}
 
 	supp_band = iwl_get_hw_mode(priv, IEEE80211_BAND_2GHZ);
 	if (supp_band) {
@@ -521,8 +418,6 @@ static ssize_t iwl_dbgfs_status_read(struct file *file,
 	int pos = 0;
 	const size_t bufsz = sizeof(buf);
 
-	pos += scnprintf(buf + pos, bufsz - pos, "STATUS_HCMD_ACTIVE:\t %d\n",
-		test_bit(STATUS_HCMD_ACTIVE, &priv->shrd->status));
 	pos += scnprintf(buf + pos, bufsz - pos, "STATUS_RF_KILL_HW:\t %d\n",
 		test_bit(STATUS_RF_KILL_HW, &priv->status));
 	pos += scnprintf(buf + pos, bufsz - pos, "STATUS_CT_KILL:\t\t %d\n",
@@ -544,9 +439,9 @@ static ssize_t iwl_dbgfs_status_read(struct file *file,
 	pos += scnprintf(buf + pos, bufsz - pos, "STATUS_SCAN_HW:\t\t %d\n",
 		test_bit(STATUS_SCAN_HW, &priv->status));
 	pos += scnprintf(buf + pos, bufsz - pos, "STATUS_POWER_PMI:\t %d\n",
-		test_bit(STATUS_POWER_PMI, &priv->shrd->status));
+		test_bit(STATUS_POWER_PMI, &priv->status));
 	pos += scnprintf(buf + pos, bufsz - pos, "STATUS_FW_ERROR:\t %d\n",
-		test_bit(STATUS_FW_ERROR, &priv->shrd->status));
+		test_bit(STATUS_FW_ERROR, &priv->status));
 	return simple_read_from_buffer(user_buf, count, ppos, buf, pos);
 }
 
@@ -563,16 +458,14 @@ static ssize_t iwl_dbgfs_rx_handlers_read(struct file *file,
 	ssize_t ret;
 
 	buf = kzalloc(bufsz, GFP_KERNEL);
-	if (!buf) {
-		IWL_ERR(priv, "Can not allocate Buffer\n");
+	if (!buf)
 		return -ENOMEM;
-	}
 
 	for (cnt = 0; cnt < REPLY_MAX; cnt++) {
 		if (priv->rx_handlers_stats[cnt] > 0)
 			pos += scnprintf(buf + pos, bufsz - pos,
 				"\tRx handler[%36s]:\t\t %u\n",
-				get_cmd_string(cnt),
+				iwl_dvm_get_cmd_string(cnt),
 				priv->rx_handlers_stats[cnt]);
 	}
 
@@ -680,11 +573,8 @@ static ssize_t iwl_dbgfs_disable_ht40_write(struct file *file,
 		return -EFAULT;
 	if (!iwl_is_any_associated(priv))
 		priv->disable_ht40 = ht40 ? true : false;
-	else {
-		IWL_ERR(priv, "Sta associated with AP - "
-			"Change to 40MHz channel support is not allowed\n");
+	else
 		return -EINVAL;
-	}
 
 	return count;
 }
@@ -816,87 +706,6 @@ DEBUGFS_READ_FILE_OPS(temperature);
 DEBUGFS_READ_WRITE_FILE_OPS(sleep_level_override);
 DEBUGFS_READ_FILE_OPS(current_sleep_command);
 
-static ssize_t iwl_dbgfs_traffic_log_read(struct file *file,
-					 char __user *user_buf,
-					 size_t count, loff_t *ppos)
-{
-	struct iwl_priv *priv = file->private_data;
-	int pos = 0, ofs = 0;
-	int cnt = 0, entry;
-
-	char *buf;
-	int bufsz = ((IWL_TRAFFIC_ENTRIES * IWL_TRAFFIC_ENTRY_SIZE * 64) * 2) +
-		(cfg(priv)->base_params->num_of_queues * 32 * 8) + 400;
-	const u8 *ptr;
-	ssize_t ret;
-
-	buf = kzalloc(bufsz, GFP_KERNEL);
-	if (!buf) {
-		IWL_ERR(priv, "Can not allocate buffer\n");
-		return -ENOMEM;
-	}
-	if (priv->tx_traffic && iwl_have_debug_level(IWL_DL_TX)) {
-		ptr = priv->tx_traffic;
-		pos += scnprintf(buf + pos, bufsz - pos,
-				"Tx Traffic idx: %u\n", priv->tx_traffic_idx);
-		for (cnt = 0, ofs = 0; cnt < IWL_TRAFFIC_ENTRIES; cnt++) {
-			for (entry = 0; entry < IWL_TRAFFIC_ENTRY_SIZE / 16;
-			     entry++,  ofs += 16) {
-				pos += scnprintf(buf + pos, bufsz - pos,
-						"0x%.4x ", ofs);
-				hex_dump_to_buffer(ptr + ofs, 16, 16, 2,
-						   buf + pos, bufsz - pos, 0);
-				pos += strlen(buf + pos);
-				if (bufsz - pos > 0)
-					buf[pos++] = '\n';
-			}
-		}
-	}
-
-	if (priv->rx_traffic && iwl_have_debug_level(IWL_DL_RX)) {
-		ptr = priv->rx_traffic;
-		pos += scnprintf(buf + pos, bufsz - pos,
-				"Rx Traffic idx: %u\n", priv->rx_traffic_idx);
-		for (cnt = 0, ofs = 0; cnt < IWL_TRAFFIC_ENTRIES; cnt++) {
-			for (entry = 0; entry < IWL_TRAFFIC_ENTRY_SIZE / 16;
-			     entry++,  ofs += 16) {
-				pos += scnprintf(buf + pos, bufsz - pos,
-						"0x%.4x ", ofs);
-				hex_dump_to_buffer(ptr + ofs, 16, 16, 2,
-						   buf + pos, bufsz - pos, 0);
-				pos += strlen(buf + pos);
-				if (bufsz - pos > 0)
-					buf[pos++] = '\n';
-			}
-		}
-	}
-
-	ret = simple_read_from_buffer(user_buf, count, ppos, buf, pos);
-	kfree(buf);
-	return ret;
-}
-
-static ssize_t iwl_dbgfs_traffic_log_write(struct file *file,
-					 const char __user *user_buf,
-					 size_t count, loff_t *ppos)
-{
-	struct iwl_priv *priv = file->private_data;
-	char buf[8];
-	int buf_size;
-	int traffic_log;
-
-	memset(buf, 0, sizeof(buf));
-	buf_size = min(count, sizeof(buf) -  1);
-	if (copy_from_user(buf, user_buf, buf_size))
-		return -EFAULT;
-	if (sscanf(buf, "%d", &traffic_log) != 1)
-		return -EFAULT;
-	if (traffic_log == 0)
-		iwl_reset_traffic_log(priv);
-
-	return count;
-}
-
 static const char *fmt_value = "  %-30s %10u\n";
 static const char *fmt_hex   = "  %-30s       0x%02X\n";
 static const char *fmt_table = "  %-30s %10u  %10u  %10u  %10u\n";
@@ -947,10 +756,8 @@ static ssize_t iwl_dbgfs_ucode_rx_stats_read(struct file *file,
 		return -EAGAIN;
 
 	buf = kzalloc(bufsz, GFP_KERNEL);
-	if (!buf) {
-		IWL_ERR(priv, "Can not allocate Buffer\n");
+	if (!buf)
 		return -ENOMEM;
-	}
 
 	/*
 	 * the statistic information display here is based on
@@ -1376,10 +1183,8 @@ static ssize_t iwl_dbgfs_ucode_tx_stats_read(struct file *file,
 		return -EAGAIN;
 
 	buf = kzalloc(bufsz, GFP_KERNEL);
-	if (!buf) {
-		IWL_ERR(priv, "Can not allocate Buffer\n");
+	if (!buf)
 		return -ENOMEM;
-	}
 
 	/* the statistic information display here is based on
 	 * the last statistics notification from uCode
@@ -1536,17 +1341,17 @@ static ssize_t iwl_dbgfs_ucode_tx_stats_read(struct file *file,
 	if (tx->tx_power.ant_a || tx->tx_power.ant_b || tx->tx_power.ant_c) {
 		pos += scnprintf(buf + pos, bufsz - pos,
 			"tx power: (1/2 dB step)\n");
-		if ((hw_params(priv).valid_tx_ant & ANT_A) &&
+		if ((priv->hw_params.valid_tx_ant & ANT_A) &&
 		    tx->tx_power.ant_a)
 			pos += scnprintf(buf + pos, bufsz - pos,
 					fmt_hex, "antenna A:",
 					tx->tx_power.ant_a);
-		if ((hw_params(priv).valid_tx_ant & ANT_B) &&
+		if ((priv->hw_params.valid_tx_ant & ANT_B) &&
 		    tx->tx_power.ant_b)
 			pos += scnprintf(buf + pos, bufsz - pos,
 					fmt_hex, "antenna B:",
 					tx->tx_power.ant_b);
-		if ((hw_params(priv).valid_tx_ant & ANT_C) &&
+		if ((priv->hw_params.valid_tx_ant & ANT_C) &&
 		    tx->tx_power.ant_c)
 			pos += scnprintf(buf + pos, bufsz - pos,
 					fmt_hex, "antenna C:",
@@ -1578,10 +1383,8 @@ static ssize_t iwl_dbgfs_ucode_general_stats_read(struct file *file,
 		return -EAGAIN;
 
 	buf = kzalloc(bufsz, GFP_KERNEL);
-	if (!buf) {
-		IWL_ERR(priv, "Can not allocate Buffer\n");
+	if (!buf)
 		return -ENOMEM;
-	}
 
 	/* the statistic information display here is based on
 	 * the last statistics notification from uCode
@@ -1704,16 +1507,11 @@ static ssize_t iwl_dbgfs_ucode_bt_stats_read(struct file *file,
 	ret = iwl_send_statistics_request(priv, CMD_SYNC, false);
 	mutex_unlock(&priv->mutex);
 
-	if (ret) {
-		IWL_ERR(priv,
-			"Error sending statistics request: %zd\n", ret);
+	if (ret)
 		return -EAGAIN;
-	}
 	buf = kzalloc(bufsz, GFP_KERNEL);
-	if (!buf) {
-		IWL_ERR(priv, "Can not allocate Buffer\n");
+	if (!buf)
 		return -ENOMEM;
-	}
 
 	/*
 	 * the statistic information display here is based on
@@ -1790,10 +1588,8 @@ static ssize_t iwl_dbgfs_reply_tx_error_read(struct file *file,
 		return -EAGAIN;
 
 	buf = kzalloc(bufsz, GFP_KERNEL);
-	if (!buf) {
-		IWL_ERR(priv, "Can not allocate Buffer\n");
+	if (!buf)
 		return -ENOMEM;
-	}
 
 	pos += scnprintf(buf + pos, bufsz - pos, "Statistics_TX_Error:\n");
 	pos += scnprintf(buf + pos, bufsz - pos, "%s:\t\t\t\t%u\n",
@@ -1933,10 +1729,8 @@ static ssize_t iwl_dbgfs_sensitivity_read(struct file *file,
 
 	data = &priv->sensitivity_data;
 	buf = kzalloc(bufsz, GFP_KERNEL);
-	if (!buf) {
-		IWL_ERR(priv, "Can not allocate Buffer\n");
+	if (!buf)
 		return -ENOMEM;
-	}
 
 	pos += scnprintf(buf + pos, bufsz - pos, "auto_corr_ofdm:\t\t\t %u\n",
 			data->auto_corr_ofdm);
@@ -2014,10 +1808,8 @@ static ssize_t iwl_dbgfs_chain_noise_read(struct file *file,
 
 	data = &priv->chain_noise_data;
 	buf = kzalloc(bufsz, GFP_KERNEL);
-	if (!buf) {
-		IWL_ERR(priv, "Can not allocate Buffer\n");
+	if (!buf)
 		return -ENOMEM;
-	}
 
 	pos += scnprintf(buf + pos, bufsz - pos, "active_chains:\t\t\t %u\n",
 			data->active_chains);
@@ -2068,7 +1860,7 @@ static ssize_t iwl_dbgfs_power_save_status_read(struct file *file,
 	const size_t bufsz = sizeof(buf);
 	u32 pwrsave_status;
 
-	pwrsave_status = iwl_read32(trans(priv), CSR_GP_CNTRL) &
+	pwrsave_status = iwl_read32(priv->trans, CSR_GP_CNTRL) &
 			CSR_GP_REG_POWER_SAVE_STATUS_MSK;
 
 	pos += scnprintf(buf + pos, bufsz - pos, "Power Save Status: ");
@@ -2262,59 +2054,39 @@ static ssize_t iwl_dbgfs_plcp_delta_write(struct file *file,
 	return count;
 }
 
-static ssize_t iwl_dbgfs_force_reset_read(struct file *file,
-					char __user *user_buf,
-					size_t count, loff_t *ppos)
+static ssize_t iwl_dbgfs_rf_reset_read(struct file *file,
+				       char __user *user_buf,
+				       size_t count, loff_t *ppos)
 {
 	struct iwl_priv *priv = file->private_data;
-	int i, pos = 0;
+	int pos = 0;
 	char buf[300];
 	const size_t bufsz = sizeof(buf);
-	struct iwl_force_reset *force_reset;
+	struct iwl_rf_reset *rf_reset = &priv->rf_reset;
+
+	pos += scnprintf(buf + pos, bufsz - pos,
+			"RF reset statistics\n");
+	pos += scnprintf(buf + pos, bufsz - pos,
+			"\tnumber of reset request: %d\n",
+			rf_reset->reset_request_count);
+	pos += scnprintf(buf + pos, bufsz - pos,
+			"\tnumber of reset request success: %d\n",
+			rf_reset->reset_success_count);
+	pos += scnprintf(buf + pos, bufsz - pos,
+			"\tnumber of reset request reject: %d\n",
+			rf_reset->reset_reject_count);
 
-	for (i = 0; i < IWL_MAX_FORCE_RESET; i++) {
-		force_reset = &priv->force_reset[i];
-		pos += scnprintf(buf + pos, bufsz - pos,
-				"Force reset method %d\n", i);
-		pos += scnprintf(buf + pos, bufsz - pos,
-				"\tnumber of reset request: %d\n",
-				force_reset->reset_request_count);
-		pos += scnprintf(buf + pos, bufsz - pos,
-				"\tnumber of reset request success: %d\n",
-				force_reset->reset_success_count);
-		pos += scnprintf(buf + pos, bufsz - pos,
-				"\tnumber of reset request reject: %d\n",
-				force_reset->reset_reject_count);
-		pos += scnprintf(buf + pos, bufsz - pos,
-				"\treset duration: %lu\n",
-				force_reset->reset_duration);
-	}
 	return simple_read_from_buffer(user_buf, count, ppos, buf, pos);
 }
 
-static ssize_t iwl_dbgfs_force_reset_write(struct file *file,
+static ssize_t iwl_dbgfs_rf_reset_write(struct file *file,
 					const char __user *user_buf,
 					size_t count, loff_t *ppos) {
 
 	struct iwl_priv *priv = file->private_data;
-	char buf[8];
-	int buf_size;
-	int reset, ret;
+	int ret;
 
-	memset(buf, 0, sizeof(buf));
-	buf_size = min(count, sizeof(buf) -  1);
-	if (copy_from_user(buf, user_buf, buf_size))
-		return -EFAULT;
-	if (sscanf(buf, "%d", &reset) != 1)
-		return -EINVAL;
-	switch (reset) {
-	case IWL_RF_RESET:
-	case IWL_FW_RESET:
-		ret = iwl_force_reset(priv, reset, true);
-		break;
-	default:
-		return -EINVAL;
-	}
+	ret = iwl_force_rf_reset(priv, true);
 	return ret ? ret : count;
 }
 
@@ -2342,29 +2114,6 @@ static ssize_t iwl_dbgfs_txfifo_flush_write(struct file *file,
 	return count;
 }
 
-static ssize_t iwl_dbgfs_wd_timeout_write(struct file *file,
-					const char __user *user_buf,
-					size_t count, loff_t *ppos)
-{
-	struct iwl_priv *priv = file->private_data;
-	char buf[8];
-	int buf_size;
-	int timeout;
-
-	memset(buf, 0, sizeof(buf));
-	buf_size = min(count, sizeof(buf) -  1);
-	if (copy_from_user(buf, user_buf, buf_size))
-		return -EFAULT;
-	if (sscanf(buf, "%d", &timeout) != 1)
-		return -EINVAL;
-	if (timeout < 0 || timeout > IWL_MAX_WD_TIMEOUT)
-		timeout = IWL_DEF_WD_TIMEOUT;
-
-	hw_params(priv).wd_timeout = timeout;
-	iwl_setup_watchdog(priv);
-	return count;
-}
-
 static ssize_t iwl_dbgfs_bt_traffic_read(struct file *file,
 					char __user *user_buf,
 					size_t count, loff_t *ppos) {
@@ -2420,10 +2169,10 @@ static ssize_t iwl_dbgfs_protection_mode_read(struct file *file,
 	char buf[40];
 	const size_t bufsz = sizeof(buf);
 
-	if (cfg(priv)->ht_params)
+	if (priv->cfg->ht_params)
 		pos += scnprintf(buf + pos, bufsz - pos,
 			 "use %s for aggregation\n",
-			 (hw_params(priv).use_rts_for_aggregation) ?
+			 (priv->hw_params.use_rts_for_aggregation) ?
 				"rts/cts" : "cts-to-self");
 	else
 		pos += scnprintf(buf + pos, bufsz - pos, "N/A");
@@ -2440,7 +2189,7 @@ static ssize_t iwl_dbgfs_protection_mode_write(struct file *file,
 	int buf_size;
 	int rts;
 
-	if (!cfg(priv)->ht_params)
+	if (!priv->cfg->ht_params)
 		return -EINVAL;
 
 	memset(buf, 0, sizeof(buf));
@@ -2450,12 +2199,29 @@ static ssize_t iwl_dbgfs_protection_mode_write(struct file *file,
 	if (sscanf(buf, "%d", &rts) != 1)
 		return -EINVAL;
 	if (rts)
-		hw_params(priv).use_rts_for_aggregation = true;
+		priv->hw_params.use_rts_for_aggregation = true;
 	else
-		hw_params(priv).use_rts_for_aggregation = false;
+		priv->hw_params.use_rts_for_aggregation = false;
 	return count;
 }
 
+static int iwl_cmd_echo_test(struct iwl_priv *priv)
+{
+	int ret;
+	struct iwl_host_cmd cmd = {
+		.id = REPLY_ECHO,
+		.len = { 0 },
+		.flags = CMD_SYNC,
+	};
+
+	ret = iwl_dvm_send_cmd(priv, &cmd);
+	if (ret)
+		IWL_ERR(priv, "echo testing fail: 0X%x\n", ret);
+	else
+		IWL_DEBUG_INFO(priv, "echo testing pass\n");
+	return ret;
+}
+
 static ssize_t iwl_dbgfs_echo_test_write(struct file *file,
 					const char __user *user_buf,
 					size_t count, loff_t *ppos)
@@ -2473,9 +2239,93 @@ static ssize_t iwl_dbgfs_echo_test_write(struct file *file,
 	return count;
 }
 
-DEBUGFS_READ_FILE_OPS(rx_statistics);
-DEBUGFS_READ_FILE_OPS(tx_statistics);
-DEBUGFS_READ_WRITE_FILE_OPS(traffic_log);
+static ssize_t iwl_dbgfs_log_event_read(struct file *file,
+					 char __user *user_buf,
+					 size_t count, loff_t *ppos)
+{
+	struct iwl_priv *priv = file->private_data;
+	char *buf;
+	int pos = 0;
+	ssize_t ret = -ENOMEM;
+
+	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);
+	}
+	return ret;
+}
+
+static ssize_t iwl_dbgfs_log_event_write(struct file *file,
+					const char __user *user_buf,
+					size_t count, loff_t *ppos)
+{
+	struct iwl_priv *priv = file->private_data;
+	u32 event_log_flag;
+	char buf[8];
+	int buf_size;
+
+	memset(buf, 0, sizeof(buf));
+	buf_size = min(count, sizeof(buf) -  1);
+	if (copy_from_user(buf, user_buf, buf_size))
+		return -EFAULT;
+	if (sscanf(buf, "%d", &event_log_flag) != 1)
+		return -EFAULT;
+	if (event_log_flag == 1)
+		iwl_dump_nic_event_log(priv, true, NULL, false);
+
+	return count;
+}
+
+static ssize_t iwl_dbgfs_calib_disabled_read(struct file *file,
+					 char __user *user_buf,
+					 size_t count, loff_t *ppos)
+{
+	struct iwl_priv *priv = file->private_data;
+	char buf[120];
+	int pos = 0;
+	const size_t bufsz = sizeof(buf);
+
+	pos += scnprintf(buf + pos, bufsz - pos,
+			 "Sensitivity calibrations %s\n",
+			 (priv->calib_disabled &
+					IWL_SENSITIVITY_CALIB_DISABLED) ?
+			 "DISABLED" : "ENABLED");
+	pos += scnprintf(buf + pos, bufsz - pos,
+			 "Chain noise calibrations %s\n",
+			 (priv->calib_disabled &
+					IWL_CHAIN_NOISE_CALIB_DISABLED) ?
+			 "DISABLED" : "ENABLED");
+	pos += scnprintf(buf + pos, bufsz - pos,
+			 "Tx power calibrations %s\n",
+			 (priv->calib_disabled &
+					IWL_TX_POWER_CALIB_DISABLED) ?
+			 "DISABLED" : "ENABLED");
+
+	return simple_read_from_buffer(user_buf, count, ppos, buf, pos);
+}
+
+static ssize_t iwl_dbgfs_calib_disabled_write(struct file *file,
+					      const char __user *user_buf,
+					      size_t count, loff_t *ppos)
+{
+	struct iwl_priv *priv = file->private_data;
+	char buf[8];
+	u32 calib_disabled;
+	int buf_size;
+
+	memset(buf, 0, sizeof(buf));
+	buf_size = min(count, sizeof(buf) - 1);
+	if (copy_from_user(buf, user_buf, buf_size))
+		return -EFAULT;
+	if (sscanf(buf, "%x", &calib_disabled) != 1)
+		return -EFAULT;
+
+	priv->calib_disabled = calib_disabled;
+
+	return count;
+}
+
 DEBUGFS_READ_FILE_OPS(ucode_rx_stats);
 DEBUGFS_READ_FILE_OPS(ucode_tx_stats);
 DEBUGFS_READ_FILE_OPS(ucode_general_stats);
@@ -2483,20 +2333,20 @@ DEBUGFS_READ_FILE_OPS(sensitivity);
 DEBUGFS_READ_FILE_OPS(chain_noise);
 DEBUGFS_READ_FILE_OPS(power_save_status);
 DEBUGFS_WRITE_FILE_OPS(clear_ucode_statistics);
-DEBUGFS_WRITE_FILE_OPS(clear_traffic_statistics);
 DEBUGFS_READ_WRITE_FILE_OPS(ucode_tracing);
 DEBUGFS_READ_WRITE_FILE_OPS(missed_beacon);
 DEBUGFS_READ_WRITE_FILE_OPS(plcp_delta);
-DEBUGFS_READ_WRITE_FILE_OPS(force_reset);
+DEBUGFS_READ_WRITE_FILE_OPS(rf_reset);
 DEBUGFS_READ_FILE_OPS(rxon_flags);
 DEBUGFS_READ_FILE_OPS(rxon_filter_flags);
 DEBUGFS_WRITE_FILE_OPS(txfifo_flush);
 DEBUGFS_READ_FILE_OPS(ucode_bt_stats);
-DEBUGFS_WRITE_FILE_OPS(wd_timeout);
 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_READ_WRITE_FILE_OPS(log_event);
+DEBUGFS_READ_WRITE_FILE_OPS(calib_disabled);
 
 /*
  * Create the debugfs files and directories
@@ -2537,15 +2387,11 @@ int iwl_dbgfs_register(struct iwl_priv *priv, const char *name)
 	DEBUGFS_ADD_FILE(disable_ht40, dir_data, S_IWUSR | S_IRUSR);
 	DEBUGFS_ADD_FILE(temperature, dir_data, S_IRUSR);
 
-	DEBUGFS_ADD_FILE(rx_statistics, dir_debug, S_IRUSR);
-	DEBUGFS_ADD_FILE(tx_statistics, dir_debug, S_IRUSR);
-	DEBUGFS_ADD_FILE(traffic_log, dir_debug, S_IWUSR | S_IRUSR);
 	DEBUGFS_ADD_FILE(power_save_status, dir_debug, S_IRUSR);
 	DEBUGFS_ADD_FILE(clear_ucode_statistics, dir_debug, S_IWUSR);
-	DEBUGFS_ADD_FILE(clear_traffic_statistics, dir_debug, S_IWUSR);
 	DEBUGFS_ADD_FILE(missed_beacon, dir_debug, S_IWUSR);
 	DEBUGFS_ADD_FILE(plcp_delta, dir_debug, S_IWUSR | S_IRUSR);
-	DEBUGFS_ADD_FILE(force_reset, dir_debug, S_IWUSR | S_IRUSR);
+	DEBUGFS_ADD_FILE(rf_reset, dir_debug, S_IWUSR | S_IRUSR);
 	DEBUGFS_ADD_FILE(ucode_rx_stats, dir_debug, S_IRUSR);
 	DEBUGFS_ADD_FILE(ucode_tx_stats, dir_debug, S_IRUSR);
 	DEBUGFS_ADD_FILE(ucode_general_stats, dir_debug, S_IRUSR);
@@ -2558,17 +2404,16 @@ int iwl_dbgfs_register(struct iwl_priv *priv, const char *name)
 	DEBUGFS_ADD_FILE(reply_tx_error, dir_debug, S_IRUSR);
 	DEBUGFS_ADD_FILE(rxon_flags, dir_debug, S_IWUSR);
 	DEBUGFS_ADD_FILE(rxon_filter_flags, dir_debug, S_IWUSR);
-	DEBUGFS_ADD_FILE(wd_timeout, dir_debug, S_IWUSR);
 	DEBUGFS_ADD_FILE(echo_test, dir_debug, S_IWUSR);
+	DEBUGFS_ADD_FILE(log_event, dir_debug, S_IWUSR | S_IRUSR);
+
 	if (iwl_advanced_bt_coexist(priv))
 		DEBUGFS_ADD_FILE(bt_traffic, dir_debug, S_IRUSR);
 
-	DEBUGFS_ADD_BOOL(disable_sensitivity, dir_rf,
-			 &priv->disable_sens_cal);
-	DEBUGFS_ADD_BOOL(disable_chain_noise, dir_rf,
-			 &priv->disable_chain_noise_cal);
+	/* Calibrations disabled/enabled status*/
+	DEBUGFS_ADD_FILE(calib_disabled, dir_rf, S_IWUSR | S_IRUSR);
 
-	if (iwl_trans_dbgfs_register(trans(priv), dir_debug))
+	if (iwl_trans_dbgfs_register(priv->trans, dir_debug))
 		goto err;
 	return 0;
 
diff --git a/drivers/net/wireless/iwlwifi/iwl-dev.h b/drivers/net/wireless/iwlwifi/iwl-dev.h
index 16956b777f9..70062379d0e 100644
--- a/drivers/net/wireless/iwlwifi/iwl-dev.h
+++ b/drivers/net/wireless/iwlwifi/iwl-dev.h
@@ -38,6 +38,7 @@
 #include <linux/slab.h>
 #include <linux/mutex.h>
 
+#include "iwl-fw.h"
 #include "iwl-eeprom.h"
 #include "iwl-csr.h"
 #include "iwl-debug.h"
@@ -47,12 +48,9 @@
 #include "iwl-agn-rs.h"
 #include "iwl-agn-tt.h"
 #include "iwl-trans.h"
-#include "iwl-shared.h"
 #include "iwl-op-mode.h"
 #include "iwl-notif-wait.h"
 
-struct iwl_tx_queue;
-
 /* CT-KILL constants */
 #define CT_KILL_THRESHOLD_LEGACY   110 /* in Celsius */
 #define CT_KILL_THRESHOLD	   114 /* in Celsius */
@@ -196,6 +194,7 @@ struct iwl_qos_info {
  * These states relate to a specific RA / TID.
  *
  * @IWL_AGG_OFF: aggregation is not used
+ * @IWL_AGG_STARTING: aggregation are starting (between start and oper)
  * @IWL_AGG_ON: aggregation session is up
  * @IWL_EMPTYING_HW_QUEUE_ADDBA: establishing a BA session - waiting for the
  *	HW queue to be empty from packets for this RA /TID.
@@ -204,6 +203,7 @@ struct iwl_qos_info {
  */
 enum iwl_agg_state {
 	IWL_AGG_OFF = 0,
+	IWL_AGG_STARTING,
 	IWL_AGG_ON,
 	IWL_EMPTYING_HW_QUEUE_ADDBA,
 	IWL_EMPTYING_HW_QUEUE_DELBA,
@@ -220,8 +220,7 @@ enum iwl_agg_state {
  *	Tx response (REPLY_TX), and the block ack notification
  *	(REPLY_COMPRESSED_BA).
  * @state: state of the BA agreement establishment / tear down.
- * @txq_id: Tx queue used by the BA session - used by the transport layer.
- *	Needed by the upper layer for debugfs only.
+ * @txq_id: Tx queue used by the BA session
  * @ssn: the first packet to be sent in AGG HW queue in Tx AGG start flow, or
  *	the first packet to be sent in legacy HW queue in Tx AGG stop flow.
  *	Basically when next_reclaimed reaches ssn, we can tell mac80211 that
@@ -507,44 +506,6 @@ struct reply_agg_tx_error_statistics {
 	u32 unknown;
 };
 
-/* management statistics */
-enum iwl_mgmt_stats {
-	MANAGEMENT_ASSOC_REQ = 0,
-	MANAGEMENT_ASSOC_RESP,
-	MANAGEMENT_REASSOC_REQ,
-	MANAGEMENT_REASSOC_RESP,
-	MANAGEMENT_PROBE_REQ,
-	MANAGEMENT_PROBE_RESP,
-	MANAGEMENT_BEACON,
-	MANAGEMENT_ATIM,
-	MANAGEMENT_DISASSOC,
-	MANAGEMENT_AUTH,
-	MANAGEMENT_DEAUTH,
-	MANAGEMENT_ACTION,
-	MANAGEMENT_MAX,
-};
-/* control statistics */
-enum iwl_ctrl_stats {
-	CONTROL_BACK_REQ =  0,
-	CONTROL_BACK,
-	CONTROL_PSPOLL,
-	CONTROL_RTS,
-	CONTROL_CTS,
-	CONTROL_ACK,
-	CONTROL_CFEND,
-	CONTROL_CFENDACK,
-	CONTROL_MAX,
-};
-
-struct traffic_stats {
-#ifdef CONFIG_IWLWIFI_DEBUGFS
-	u32 mgmt[MANAGEMENT_MAX];
-	u32 ctrl[CONTROL_MAX];
-	u32 data_cnt;
-	u64 data_bytes;
-#endif
-};
-
 /*
  * schedule the timer to wake up every UCODE_TRACE_PERIOD milliseconds
  * to perform continuous uCode event logging operation if enabled
@@ -571,24 +532,7 @@ struct iwl_event_log {
 	int wraps_more_count;
 };
 
-/*
- * This is the threshold value of plcp error rate per 100mSecs.  It is
- * used to set and check for the validity of plcp_delta.
- */
-#define IWL_MAX_PLCP_ERR_THRESHOLD_MIN	(1)
-#define IWL_MAX_PLCP_ERR_THRESHOLD_DEF	(50)
-#define IWL_MAX_PLCP_ERR_LONG_THRESHOLD_DEF	(100)
-#define IWL_MAX_PLCP_ERR_EXT_LONG_THRESHOLD_DEF	(200)
-#define IWL_MAX_PLCP_ERR_THRESHOLD_MAX	(255)
-#define IWL_MAX_PLCP_ERR_THRESHOLD_DISABLE	(0)
-
 #define IWL_DELAY_NEXT_FORCE_RF_RESET  (HZ*3)
-#define IWL_DELAY_NEXT_FORCE_FW_RELOAD (HZ*5)
-
-/* TX queue watchdog timeouts in mSecs */
-#define IWL_DEF_WD_TIMEOUT	(2000)
-#define IWL_LONG_WD_TIMEOUT	(10000)
-#define IWL_MAX_WD_TIMEOUT	(120000)
 
 /* BT Antenna Coupling Threshold (dB) */
 #define IWL_BT_ANTENNA_COUPLING_THRESHOLD	(35)
@@ -598,18 +542,18 @@ struct iwl_event_log {
 #define IWL_MAX_CONTINUE_RELOAD_CNT	4
 
 
-enum iwl_reset {
-	IWL_RF_RESET = 0,
-	IWL_FW_RESET,
-	IWL_MAX_FORCE_RESET,
-};
-
-struct iwl_force_reset {
+struct iwl_rf_reset {
 	int reset_request_count;
 	int reset_success_count;
 	int reset_reject_count;
-	unsigned long reset_duration;
-	unsigned long last_force_reset_jiffies;
+	unsigned long last_reset_jiffies;
+};
+
+enum iwl_rxon_context_id {
+	IWL_RXON_CTX_BSS,
+	IWL_RXON_CTX_PAN,
+
+	NUM_IWL_RXON_CTX
 };
 
 /* extend beacon time format bit shifting  */
@@ -623,6 +567,10 @@ struct iwl_force_reset {
 struct iwl_rxon_context {
 	struct ieee80211_vif *vif;
 
+	u8 mcast_queue;
+	u8 ac_to_queue[IEEE80211_NUM_ACS];
+	u8 ac_to_fifo[IEEE80211_NUM_ACS];
+
 	/*
 	 * We could use the vif to indicate active, but we
 	 * also need it to be active during disabling when
@@ -677,6 +625,52 @@ enum iwl_scan_type {
 	IWL_SCAN_ROC,
 };
 
+/**
+ * struct iwl_hw_params
+ *
+ * Holds the module parameters
+ *
+ * @tx_chains_num: Number of TX chains
+ * @rx_chains_num: Number of RX chains
+ * @valid_tx_ant: usable antennas for TX
+ * @valid_rx_ant: usable antennas for RX
+ * @ht40_channel: is 40MHz width possible: BIT(IEEE80211_BAND_XXX)
+ * @sku: sku read from EEPROM
+ * @ct_kill_threshold: temperature threshold - in hw dependent unit
+ * @ct_kill_exit_threshold: when to reeable the device - in hw dependent unit
+ *	relevant for 1000, 6000 and up
+ * @struct iwl_sensitivity_ranges: range of sensitivity values
+ * @use_rts_for_aggregation: use rts/cts protection for HT traffic
+ */
+struct iwl_hw_params {
+	u8  tx_chains_num;
+	u8  rx_chains_num;
+	u8  valid_tx_ant;
+	u8  valid_rx_ant;
+	u8  ht40_channel;
+	bool use_rts_for_aggregation;
+	u16 sku;
+	u32 ct_kill_threshold;
+	u32 ct_kill_exit_threshold;
+
+	const struct iwl_sensitivity_ranges *sens;
+};
+
+struct iwl_lib_ops {
+	/* set hw dependent parameters */
+	void (*set_hw_params)(struct iwl_priv *priv);
+	int (*set_channel_switch)(struct iwl_priv *priv,
+				  struct ieee80211_channel_switch *ch_switch);
+	/* device specific configuration */
+	void (*nic_config)(struct iwl_priv *priv);
+
+	/* eeprom operations (as defined in iwl-eeprom.h) */
+	struct iwl_eeprom_ops eeprom_ops;
+
+	/* temperature */
+	void (*temperature)(struct iwl_priv *priv);
+};
+
 #ifdef CONFIG_IWLWIFI_DEVICE_TESTMODE
 struct iwl_testmode_trace {
 	u32 buff_size;
@@ -701,6 +695,17 @@ struct iwl_wipan_noa_data {
 	u8 data[];
 };
 
+/* Calibration disabling bit mask */
+enum {
+	IWL_CALIB_ENABLE_ALL			= 0,
+
+	IWL_SENSITIVITY_CALIB_DISABLED		= BIT(0),
+	IWL_CHAIN_NOISE_CALIB_DISABLED		= BIT(1),
+	IWL_TX_POWER_CALIB_DISABLED		= BIT(2),
+
+	IWL_CALIB_DISABLE_ALL			= 0xFFFFFFFF,
+};
+
 #define IWL_OP_MODE_GET_DVM(_iwl_op_mode) \
 	((struct iwl_priv *) ((_iwl_op_mode)->op_mode_specific))
 
@@ -710,9 +715,11 @@ struct iwl_wipan_noa_data {
 
 struct iwl_priv {
 
-	/*data shared among all the driver's layers */
-	struct iwl_shared *shrd;
+	struct iwl_trans *trans;
+	struct device *dev;		/* for debug prints only */
+	const struct iwl_cfg *cfg;
 	const struct iwl_fw *fw;
+	const struct iwl_lib_ops *lib;
 	unsigned long status;
 
 	spinlock_t sta_lock;
@@ -720,6 +727,11 @@ struct iwl_priv {
 
 	unsigned long transport_queue_stop;
 	bool passive_no_rx;
+#define IWL_INVALID_MAC80211_QUEUE	0xff
+	u8 queue_to_mac80211[IWL_MAX_HW_QUEUES];
+	atomic_t queue_stop_count[IWL_MAX_HW_QUEUES];
+
+	unsigned long agg_q_alloc[BITS_TO_LONGS(IWL_MAX_HW_QUEUES)];
 
 	/* ieee device used by generic ieee processing code */
 	struct ieee80211_hw *hw;
@@ -730,7 +742,10 @@ struct iwl_priv {
 
 	struct workqueue_struct *workqueue;
 
+	struct iwl_hw_params hw_params;
+
 	enum ieee80211_band band;
+	u8 valid_contexts;
 
 	void (*pre_rx_handler)(struct iwl_priv *priv,
 			       struct iwl_rx_cmd_buffer *rxb);
@@ -763,8 +778,8 @@ struct iwl_priv {
 	/*counters */
 	u32 rx_handlers_stats[REPLY_MAX];
 
-	/* force reset */
-	struct iwl_force_reset force_reset[IWL_MAX_FORCE_RESET];
+	/* rf reset */
+	struct iwl_rf_reset rf_reset;
 
 	/* firmware reload counter and timestamp */
 	unsigned long reload_jiffies;
@@ -810,8 +825,6 @@ struct iwl_priv {
 
 	__le16 switch_channel;
 
-	u16 active_rate;
-
 	u8 start_calib;
 	struct iwl_sensitivity_data sensitivity_data;
 	struct iwl_chain_noise_data chain_noise_data;
@@ -825,10 +838,6 @@ struct iwl_priv {
 
 	int activity_timer_active;
 
-	/* counts mgmt, ctl, and data packets */
-	struct traffic_stats tx_stats;
-	struct traffic_stats rx_stats;
-
 	struct iwl_power_mgr power_data;
 	struct iwl_tt_mgmt thermal_throttle;
 
@@ -912,6 +921,7 @@ struct iwl_priv {
 	__le32 kill_ack_mask;
 	__le32 kill_cts_mask;
 	__le16 bt_valid;
+	bool reduced_txpower;
 	u16 bt_on_thresh;
 	u16 bt_duration;
 	u16 dynamic_frag_thresh;
@@ -948,23 +958,21 @@ struct iwl_priv {
 
 #ifdef CONFIG_IWLWIFI_DEBUGFS
 	/* debugfs */
-	u16 tx_traffic_idx;
-	u16 rx_traffic_idx;
-	u8 *tx_traffic;
-	u8 *rx_traffic;
 	struct dentry *debugfs_dir;
 	u32 dbgfs_sram_offset, dbgfs_sram_len;
 	bool disable_ht40;
 	void *wowlan_sram;
 #endif /* CONFIG_IWLWIFI_DEBUGFS */
 
+	/* eeprom -- this is in the card's little endian byte order */
+	u8 *eeprom;
+	enum iwl_nvm_type nvm_device_type;
+
 	struct work_struct txpower_work;
-	u32 disable_sens_cal;
-	u32 disable_chain_noise_cal;
+	u32 calib_disabled;
 	struct work_struct run_time_calib_work;
 	struct timer_list statistics_periodic;
 	struct timer_list ucode_trace;
-	struct timer_list watchdog;
 
 	struct iwl_event_log event_log;
 
@@ -982,10 +990,18 @@ struct iwl_priv {
 	__le64 replay_ctr;
 	__le16 last_seq_ctl;
 	bool have_rekey_data;
+
+	/* device_pointers: pointers to ucode event tables */
+	struct {
+		u32 error_event_table;
+		u32 log_event_table;
+	} device_pointers;
+
+	/* indicator of loaded ucode image */
+	enum iwl_ucode_type cur_ucode;
 }; /*iwl_priv */
 
 extern struct kmem_cache *iwl_tx_cmd_pool;
-extern struct iwl_mod_params iwlagn_mod_params;
 
 static inline struct iwl_rxon_context *
 iwl_rxon_ctx_from_vif(struct ieee80211_vif *vif)
@@ -998,7 +1014,7 @@ iwl_rxon_ctx_from_vif(struct ieee80211_vif *vif)
 #define for_each_context(priv, ctx)				\
 	for (ctx = &priv->contexts[IWL_RXON_CTX_BSS];		\
 	     ctx < &priv->contexts[NUM_IWL_RXON_CTX]; ctx++)	\
-		if (priv->shrd->valid_contexts & BIT(ctx->ctxid))
+		if (priv->valid_contexts & BIT(ctx->ctxid))
 
 static inline int iwl_is_associated_ctx(struct iwl_rxon_context *ctx)
 {
diff --git a/drivers/net/wireless/iwlwifi/iwl-drv.c b/drivers/net/wireless/iwlwifi/iwl-drv.c
index 6f312c77af5..3c72bad0ae5 100644
--- a/drivers/net/wireless/iwlwifi/iwl-drv.c
+++ b/drivers/net/wireless/iwlwifi/iwl-drv.c
@@ -66,10 +66,13 @@
 #include <linux/module.h>
 
 #include "iwl-drv.h"
+#include "iwl-debug.h"
 #include "iwl-trans.h"
-#include "iwl-shared.h"
 #include "iwl-op-mode.h"
 #include "iwl-agn-hw.h"
+#include "iwl-fw.h"
+#include "iwl-config.h"
+#include "iwl-modparams.h"
 
 /* private includes */
 #include "iwl-fw-file.h"
@@ -77,8 +80,10 @@
 /**
  * struct iwl_drv - drv common data
  * @fw: the iwl_fw structure
- * @shrd: pointer to common shared structure
  * @op_mode: the running op_mode
+ * @trans: transport layer
+ * @dev: for debug prints only
+ * @cfg: configuration struct
  * @fw_index: firmware revision to try loading
  * @firmware_name: composite filename of ucode file to load
  * @request_firmware_complete: the firmware has been obtained from user space
@@ -86,8 +91,10 @@
 struct iwl_drv {
 	struct iwl_fw fw;
 
-	struct iwl_shared *shrd;
 	struct iwl_op_mode *op_mode;
+	struct iwl_trans *trans;
+	struct device *dev;
+	const struct iwl_cfg *cfg;
 
 	int fw_index;                   /* firmware we're trying to load */
 	char firmware_name[25];         /* name of firmware file to load */
@@ -110,7 +117,7 @@ struct fw_sec {
 static void iwl_free_fw_desc(struct iwl_drv *drv, struct fw_desc *desc)
 {
 	if (desc->v_addr)
-		dma_free_coherent(trans(drv)->dev, desc->len,
+		dma_free_coherent(drv->trans->dev, desc->len,
 				  desc->v_addr, desc->p_addr);
 	desc->v_addr = NULL;
 	desc->len = 0;
@@ -138,7 +145,7 @@ static int iwl_alloc_fw_desc(struct iwl_drv *drv, struct fw_desc *desc,
 		return -EINVAL;
 	}
 
-	desc->v_addr = dma_alloc_coherent(trans(drv)->dev, sec->size,
+	desc->v_addr = dma_alloc_coherent(drv->trans->dev, sec->size,
 					  &desc->p_addr, GFP_KERNEL);
 	if (!desc->v_addr)
 		return -ENOMEM;
@@ -156,8 +163,7 @@ static void iwl_ucode_callback(const struct firmware *ucode_raw, void *context);
 
 static int iwl_request_firmware(struct iwl_drv *drv, bool first)
 {
-	const struct iwl_cfg *cfg = cfg(drv);
-	const char *name_pre = cfg->fw_name_pre;
+	const char *name_pre = drv->cfg->fw_name_pre;
 	char tag[8];
 
 	if (first) {
@@ -166,14 +172,14 @@ static int iwl_request_firmware(struct iwl_drv *drv, bool first)
 		strcpy(tag, UCODE_EXPERIMENTAL_TAG);
 	} else if (drv->fw_index == UCODE_EXPERIMENTAL_INDEX) {
 #endif
-		drv->fw_index = cfg->ucode_api_max;
+		drv->fw_index = drv->cfg->ucode_api_max;
 		sprintf(tag, "%d", drv->fw_index);
 	} else {
 		drv->fw_index--;
 		sprintf(tag, "%d", drv->fw_index);
 	}
 
-	if (drv->fw_index < cfg->ucode_api_min) {
+	if (drv->fw_index < drv->cfg->ucode_api_min) {
 		IWL_ERR(drv, "no suitable firmware found!\n");
 		return -ENOENT;
 	}
@@ -186,7 +192,7 @@ static int iwl_request_firmware(struct iwl_drv *drv, bool first)
 		       drv->firmware_name);
 
 	return request_firmware_nowait(THIS_MODULE, 1, drv->firmware_name,
-				       trans(drv)->dev,
+				       drv->trans->dev,
 				       GFP_KERNEL, drv, iwl_ucode_callback);
 }
 
@@ -284,6 +290,7 @@ static int iwl_store_ucode_sec(struct iwl_firmware_pieces *pieces,
 
 	sec->offset = le32_to_cpu(sec_parse->offset);
 	sec->data = sec_parse->data;
+	sec->size = size - sizeof(sec_parse->offset);
 
 	++img->sec_counter;
 
@@ -414,9 +421,6 @@ static int iwl_parse_tlv_firmware(struct iwl_drv *drv,
 	struct iwl_ucode_tlv *tlv;
 	size_t len = ucode_raw->size;
 	const u8 *data;
-	int wanted_alternative = iwlagn_mod_params.wanted_ucode_alternative;
-	int tmp;
-	u64 alternatives;
 	u32 tlv_len;
 	enum iwl_ucode_tlv_type tlv_type;
 	const u8 *tlv_data;
@@ -434,23 +438,6 @@ static int iwl_parse_tlv_firmware(struct iwl_drv *drv,
 		return -EINVAL;
 	}
 
-	/*
-	 * Check which alternatives are present, and "downgrade"
-	 * when the chosen alternative is not present, warning
-	 * the user when that happens. Some files may not have
-	 * any alternatives, so don't warn in that case.
-	 */
-	alternatives = le64_to_cpu(ucode->alternatives);
-	tmp = wanted_alternative;
-	if (wanted_alternative > 63)
-		wanted_alternative = 63;
-	while (wanted_alternative && !(alternatives & BIT(wanted_alternative)))
-		wanted_alternative--;
-	if (wanted_alternative && wanted_alternative != tmp)
-		IWL_WARN(drv,
-			 "uCode alternative %d not available, choosing %d\n",
-			 tmp, wanted_alternative);
-
 	drv->fw.ucode_ver = le32_to_cpu(ucode->ver);
 	build = le32_to_cpu(ucode->build);
 
@@ -475,14 +462,11 @@ static int iwl_parse_tlv_firmware(struct iwl_drv *drv,
 	len -= sizeof(*ucode);
 
 	while (len >= sizeof(*tlv)) {
-		u16 tlv_alt;
-
 		len -= sizeof(*tlv);
 		tlv = (void *)data;
 
 		tlv_len = le32_to_cpu(tlv->length);
-		tlv_type = le16_to_cpu(tlv->type);
-		tlv_alt = le16_to_cpu(tlv->alternative);
+		tlv_type = le32_to_cpu(tlv->type);
 		tlv_data = tlv->data;
 
 		if (len < tlv_len) {
@@ -493,14 +477,6 @@ static int iwl_parse_tlv_firmware(struct iwl_drv *drv,
 		len -= ALIGN(tlv_len, 4);
 		data += sizeof(*tlv) + ALIGN(tlv_len, 4);
 
-		/*
-		 * Alternative 0 is always valid.
-		 *
-		 * Skip alternative TLVs that are not selected.
-		 */
-		if (tlv_alt != 0 && tlv_alt != wanted_alternative)
-			continue;
-
 		switch (tlv_type) {
 		case IWL_UCODE_TLV_INST:
 			set_sec_data(pieces, IWL_UCODE_REGULAR,
@@ -755,14 +731,13 @@ static int validate_sec_sizes(struct iwl_drv *drv,
 static void iwl_ucode_callback(const struct firmware *ucode_raw, void *context)
 {
 	struct iwl_drv *drv = context;
-	const struct iwl_cfg *cfg = cfg(drv);
 	struct iwl_fw *fw = &drv->fw;
 	struct iwl_ucode_header *ucode;
 	int err;
 	struct iwl_firmware_pieces pieces;
-	const unsigned int api_max = cfg->ucode_api_max;
-	unsigned int api_ok = cfg->ucode_api_ok;
-	const unsigned int api_min = cfg->ucode_api_min;
+	const unsigned int api_max = drv->cfg->ucode_api_max;
+	unsigned int api_ok = drv->cfg->ucode_api_ok;
+	const unsigned int api_min = drv->cfg->ucode_api_min;
 	u32 api_ver;
 	int i;
 
@@ -838,46 +813,10 @@ static void iwl_ucode_callback(const struct firmware *ucode_raw, void *context)
 	IWL_INFO(drv, "loaded firmware version %s", drv->fw.fw_version);
 
 	/*
-	 * For any of the failures below (before allocating pci memory)
-	 * we will try to load a version with a smaller API -- maybe the
-	 * user just got a corrupted version of the latest API.
-	 */
-
-	IWL_DEBUG_INFO(drv, "f/w package hdr ucode version raw = 0x%x\n",
-		       drv->fw.ucode_ver);
-	IWL_DEBUG_INFO(drv, "f/w package hdr runtime inst size = %Zd\n",
-		get_sec_size(&pieces, IWL_UCODE_REGULAR,
-			     IWL_UCODE_SECTION_INST));
-	IWL_DEBUG_INFO(drv, "f/w package hdr runtime data size = %Zd\n",
-		get_sec_size(&pieces, IWL_UCODE_REGULAR,
-			     IWL_UCODE_SECTION_DATA));
-	IWL_DEBUG_INFO(drv, "f/w package hdr init inst size = %Zd\n",
-		get_sec_size(&pieces, IWL_UCODE_INIT, IWL_UCODE_SECTION_INST));
-	IWL_DEBUG_INFO(drv, "f/w package hdr init data size = %Zd\n",
-		get_sec_size(&pieces, IWL_UCODE_INIT, IWL_UCODE_SECTION_DATA));
-
-	/* Verify that uCode images will fit in card's SRAM */
-	if (get_sec_size(&pieces, IWL_UCODE_REGULAR, IWL_UCODE_SECTION_INST) >
-							cfg->max_inst_size) {
-		IWL_ERR(drv, "uCode instr len %Zd too large to fit in\n",
-			get_sec_size(&pieces, IWL_UCODE_REGULAR,
-				     IWL_UCODE_SECTION_INST));
-		goto try_again;
-	}
-
-	if (get_sec_size(&pieces, IWL_UCODE_REGULAR, IWL_UCODE_SECTION_DATA) >
-							cfg->max_data_size) {
-		IWL_ERR(drv, "uCode data len %Zd too large to fit in\n",
-			get_sec_size(&pieces, IWL_UCODE_REGULAR,
-				     IWL_UCODE_SECTION_DATA));
-		goto try_again;
-	}
-
-	/*
 	 * In mvm uCode there is no difference between data and instructions
 	 * sections.
 	 */
-	if (!fw->mvm_fw && validate_sec_sizes(drv, &pieces, cfg))
+	if (!fw->mvm_fw && validate_sec_sizes(drv, &pieces, drv->cfg))
 		goto try_again;
 
 	/* Allocate ucode buffers for card's bus-master loading ... */
@@ -901,14 +840,14 @@ static void iwl_ucode_callback(const struct firmware *ucode_raw, void *context)
 		fw->init_evtlog_size = (pieces.init_evtlog_size - 16)/12;
 	else
 		fw->init_evtlog_size =
-			cfg->base_params->max_event_log_size;
+			drv->cfg->base_params->max_event_log_size;
 	fw->init_errlog_ptr = pieces.init_errlog_ptr;
 	fw->inst_evtlog_ptr = pieces.inst_evtlog_ptr;
 	if (pieces.inst_evtlog_size)
 		fw->inst_evtlog_size = (pieces.inst_evtlog_size - 16)/12;
 	else
 		fw->inst_evtlog_size =
-			cfg->base_params->max_event_log_size;
+			drv->cfg->base_params->max_event_log_size;
 	fw->inst_errlog_ptr = pieces.inst_errlog_ptr;
 
 	/*
@@ -924,7 +863,7 @@ static void iwl_ucode_callback(const struct firmware *ucode_raw, void *context)
 	release_firmware(ucode_raw);
 	complete(&drv->request_firmware_complete);
 
-	drv->op_mode = iwl_dvm_ops.start(drv->shrd->trans, &drv->fw);
+	drv->op_mode = iwl_dvm_ops.start(drv->trans, drv->cfg, &drv->fw);
 
 	if (!drv->op_mode)
 		goto out_unbind;
@@ -944,42 +883,38 @@ static void iwl_ucode_callback(const struct firmware *ucode_raw, void *context)
 	release_firmware(ucode_raw);
  out_unbind:
 	complete(&drv->request_firmware_complete);
-	device_release_driver(trans(drv)->dev);
+	device_release_driver(drv->trans->dev);
 }
 
-int iwl_drv_start(struct iwl_shared *shrd,
-		  struct iwl_trans *trans, const struct iwl_cfg *cfg)
+struct iwl_drv *iwl_drv_start(struct iwl_trans *trans,
+			      const struct iwl_cfg *cfg)
 {
 	struct iwl_drv *drv;
 	int ret;
 
-	shrd->cfg = cfg;
-
 	drv = kzalloc(sizeof(*drv), GFP_KERNEL);
-	if (!drv) {
-		dev_printk(KERN_ERR, trans->dev, "Couldn't allocate iwl_drv");
-		return -ENOMEM;
-	}
-	drv->shrd = shrd;
-	shrd->drv = drv;
+	if (!drv)
+		return NULL;
+
+	drv->trans = trans;
+	drv->dev = trans->dev;
+	drv->cfg = cfg;
 
 	init_completion(&drv->request_firmware_complete);
 
 	ret = iwl_request_firmware(drv, true);
 
 	if (ret) {
-		dev_printk(KERN_ERR, trans->dev, "Couldn't request the fw");
+		IWL_ERR(trans, "Couldn't request the fw\n");
 		kfree(drv);
-		shrd->drv = NULL;
+		drv = NULL;
 	}
 
-	return ret;
+	return drv;
 }
 
-void iwl_drv_stop(struct iwl_shared *shrd)
+void iwl_drv_stop(struct iwl_drv *drv)
 {
-	struct iwl_drv *drv = shrd->drv;
-
 	wait_for_completion(&drv->request_firmware_complete);
 
 	/* op_mode can be NULL if its start failed */
@@ -989,5 +924,95 @@ void iwl_drv_stop(struct iwl_shared *shrd)
 	iwl_dealloc_ucode(drv);
 
 	kfree(drv);
-	shrd->drv = NULL;
 }
+
+
+/* shared module parameters */
+struct iwl_mod_params iwlwifi_mod_params = {
+	.amsdu_size_8K = 1,
+	.restart_fw = 1,
+	.plcp_check = true,
+	.bt_coex_active = true,
+	.power_level = IWL_POWER_INDEX_1,
+	.bt_ch_announce = true,
+	.auto_agg = true,
+	/* the rest are 0 by default */
+};
+
+#ifdef CONFIG_IWLWIFI_DEBUG
+module_param_named(debug, iwlwifi_mod_params.debug_level, uint,
+		   S_IRUGO | S_IWUSR);
+MODULE_PARM_DESC(debug, "debug output mask");
+#endif
+
+module_param_named(swcrypto, iwlwifi_mod_params.sw_crypto, int, S_IRUGO);
+MODULE_PARM_DESC(swcrypto, "using crypto in software (default 0 [hardware])");
+module_param_named(11n_disable, iwlwifi_mod_params.disable_11n, uint, S_IRUGO);
+MODULE_PARM_DESC(11n_disable,
+	"disable 11n functionality, bitmap: 1: full, 2: agg TX, 4: agg RX");
+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");
+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(antenna_coupling, iwlwifi_mod_params.ant_coupling,
+		   int, S_IRUGO);
+MODULE_PARM_DESC(antenna_coupling,
+		 "specify antenna coupling in dB (defualt: 0 dB)");
+
+module_param_named(bt_ch_inhibition, iwlwifi_mod_params.bt_ch_announce,
+		   bool, S_IRUGO);
+MODULE_PARM_DESC(bt_ch_inhibition,
+		 "Enable BT channel inhibition (default: enable)");
+
+module_param_named(plcp_check, iwlwifi_mod_params.plcp_check, bool, S_IRUGO);
+MODULE_PARM_DESC(plcp_check, "Check plcp health (default: 1 [enabled])");
+
+module_param_named(wd_disable, iwlwifi_mod_params.wd_disable, int, S_IRUGO);
+MODULE_PARM_DESC(wd_disable,
+		"Disable stuck queue watchdog timer 0=system default, "
+		"1=disable, 2=enable (default: 0)");
+
+/*
+ * set bt_coex_active to true, uCode will do kill/defer
+ * every time the priority line is asserted (BT is sending signals on the
+ * priority line in the PCIx).
+ * set bt_coex_active to false, uCode will ignore the BT activity and
+ * perform the normal operation
+ *
+ * User might experience transmit issue on some platform due to WiFi/BT
+ * co-exist problem. The possible behaviors are:
+ *   Able to scan and finding all the available AP
+ *   Not able to associate with any AP
+ * On those platforms, WiFi communication can be restored by set
+ * "bt_coex_active" module parameter to "false"
+ *
+ * default: bt_coex_active = true (BT_COEX_ENABLE)
+ */
+module_param_named(bt_coex_active, iwlwifi_mod_params.bt_coex_active,
+		bool, S_IRUGO);
+MODULE_PARM_DESC(bt_coex_active, "enable wifi/bt co-exist (default: enable)");
+
+module_param_named(led_mode, iwlwifi_mod_params.led_mode, int, S_IRUGO);
+MODULE_PARM_DESC(led_mode, "0=system default, "
+		"1=On(RF On)/Off(RF Off), 2=blinking, 3=Off (default: 0)");
+
+module_param_named(power_save, iwlwifi_mod_params.power_save,
+		bool, S_IRUGO);
+MODULE_PARM_DESC(power_save,
+		 "enable WiFi power management (default: disable)");
+
+module_param_named(power_level, iwlwifi_mod_params.power_level,
+		int, S_IRUGO);
+MODULE_PARM_DESC(power_level,
+		 "default power save level (range from 1 - 5, default: 1)");
+
+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 3b771c1d909..2cbf137b25b 100644
--- a/drivers/net/wireless/iwlwifi/iwl-drv.h
+++ b/drivers/net/wireless/iwlwifi/iwl-drv.h
@@ -63,7 +63,12 @@
 #ifndef __iwl_drv_h__
 #define __iwl_drv_h__
 
-#include "iwl-shared.h"
+/* for all modules */
+#define DRV_NAME        "iwlwifi"
+#define IWLWIFI_VERSION "in-tree:"
+#define DRV_COPYRIGHT	"Copyright(c) 2003-2012 Intel Corporation"
+#define DRV_AUTHOR     "<ilw@linux.intel.com>"
+
 
 /**
  * DOC: Driver system flows - drv component
@@ -90,34 +95,32 @@
  * 8) iwl_ucode_callback starts the wifi implementation to matches the fw
  */
 
+struct iwl_drv;
+struct iwl_trans;
+struct iwl_cfg;
 /**
  * iwl_drv_start - start the drv
  *
- * @shrd: the shrd area
  * @trans_ops: the ops of the transport
  * @cfg: device specific constants / virtual functions
  *
- * TODO: review the parameters given to this function
- *
  * starts the driver: fetches the firmware. This should be called by bus
  * specific system flows implementations. For example, the bus specific probe
  * function should do bus related operations only, and then call to this
- * function.
+ * function. It returns the driver object or %NULL if an error occured.
  */
-int iwl_drv_start(struct iwl_shared *shrd,
-		  struct iwl_trans *trans, const struct iwl_cfg *cfg);
+struct iwl_drv *iwl_drv_start(struct iwl_trans *trans,
+			      const struct iwl_cfg *cfg);
 
 /**
  * iwl_drv_stop - stop the drv
  *
- * @shrd: the shrd area
- *
- * TODO: review the parameters given to this function
+ * @drv:
  *
  * Stop the driver. This should be called by bus specific system flows
  * implementations. For example, the bus specific remove function should first
  * call this function and then do the bus related operations only.
  */
-void iwl_drv_stop(struct iwl_shared *shrd);
+void iwl_drv_stop(struct iwl_drv *drv);
 
 #endif /* __iwl_drv_h__ */
diff --git a/drivers/net/wireless/iwlwifi/iwl-eeprom.c b/drivers/net/wireless/iwlwifi/iwl-eeprom.c
index 23cea42b949..50c58911e71 100644
--- a/drivers/net/wireless/iwlwifi/iwl-eeprom.c
+++ b/drivers/net/wireless/iwlwifi/iwl-eeprom.c
@@ -68,9 +68,7 @@
 
 #include <net/mac80211.h>
 
-#include "iwl-commands.h"
 #include "iwl-dev.h"
-#include "iwl-core.h"
 #include "iwl-debug.h"
 #include "iwl-agn.h"
 #include "iwl-eeprom.h"
@@ -187,33 +185,33 @@ static void iwl_eeprom_release_semaphore(struct iwl_trans *trans)
 
 }
 
-static int iwl_eeprom_verify_signature(struct iwl_trans *trans)
+static int iwl_eeprom_verify_signature(struct iwl_priv *priv)
 {
-	u32 gp = iwl_read32(trans, CSR_EEPROM_GP) &
+	u32 gp = iwl_read32(priv->trans, CSR_EEPROM_GP) &
 			   CSR_EEPROM_GP_VALID_MSK;
 	int ret = 0;
 
-	IWL_DEBUG_EEPROM(trans, "EEPROM signature=0x%08x\n", gp);
+	IWL_DEBUG_EEPROM(priv, "EEPROM signature=0x%08x\n", gp);
 	switch (gp) {
 	case CSR_EEPROM_GP_BAD_SIG_EEP_GOOD_SIG_OTP:
-		if (trans->nvm_device_type != NVM_DEVICE_TYPE_OTP) {
-			IWL_ERR(trans, "EEPROM with bad signature: 0x%08x\n",
+		if (priv->nvm_device_type != NVM_DEVICE_TYPE_OTP) {
+			IWL_ERR(priv, "EEPROM with bad signature: 0x%08x\n",
 				gp);
 			ret = -ENOENT;
 		}
 		break;
 	case CSR_EEPROM_GP_GOOD_SIG_EEP_LESS_THAN_4K:
 	case CSR_EEPROM_GP_GOOD_SIG_EEP_MORE_THAN_4K:
-		if (trans->nvm_device_type != NVM_DEVICE_TYPE_EEPROM) {
-			IWL_ERR(trans, "OTP with bad signature: 0x%08x\n", gp);
+		if (priv->nvm_device_type != NVM_DEVICE_TYPE_EEPROM) {
+			IWL_ERR(priv, "OTP with bad signature: 0x%08x\n", gp);
 			ret = -ENOENT;
 		}
 		break;
 	case CSR_EEPROM_GP_BAD_SIGNATURE_BOTH_EEP_AND_OTP:
 	default:
-		IWL_ERR(trans, "bad EEPROM/OTP signature, type=%s, "
+		IWL_ERR(priv, "bad EEPROM/OTP signature, type=%s, "
 			"EEPROM_GP=0x%08x\n",
-			(trans->nvm_device_type == NVM_DEVICE_TYPE_OTP)
+			(priv->nvm_device_type == NVM_DEVICE_TYPE_OTP)
 			? "OTP" : "EEPROM", gp);
 		ret = -ENOENT;
 		break;
@@ -221,11 +219,11 @@ static int iwl_eeprom_verify_signature(struct iwl_trans *trans)
 	return ret;
 }
 
-u16 iwl_eeprom_query16(const struct iwl_shared *shrd, size_t offset)
+u16 iwl_eeprom_query16(struct iwl_priv *priv, size_t offset)
 {
-	if (!shrd->eeprom)
+	if (!priv->eeprom)
 		return 0;
-	return (u16)shrd->eeprom[offset] | ((u16)shrd->eeprom[offset + 1] << 8);
+	return (u16)priv->eeprom[offset] | ((u16)priv->eeprom[offset + 1] << 8);
 }
 
 int iwl_eeprom_check_version(struct iwl_priv *priv)
@@ -233,11 +231,11 @@ int iwl_eeprom_check_version(struct iwl_priv *priv)
 	u16 eeprom_ver;
 	u16 calib_ver;
 
-	eeprom_ver = iwl_eeprom_query16(priv->shrd, EEPROM_VERSION);
-	calib_ver = iwl_eeprom_calib_version(priv->shrd);
+	eeprom_ver = iwl_eeprom_query16(priv, EEPROM_VERSION);
+	calib_ver = iwl_eeprom_calib_version(priv);
 
-	if (eeprom_ver < cfg(priv)->eeprom_ver ||
-	    calib_ver < cfg(priv)->eeprom_calib_ver)
+	if (eeprom_ver < priv->cfg->eeprom_ver ||
+	    calib_ver < priv->cfg->eeprom_calib_ver)
 		goto err;
 
 	IWL_INFO(priv, "device EEPROM VER=0x%x, CALIB=0x%x\n",
@@ -247,58 +245,115 @@ int iwl_eeprom_check_version(struct iwl_priv *priv)
 err:
 	IWL_ERR(priv, "Unsupported (too old) EEPROM VER=0x%x < 0x%x "
 		  "CALIB=0x%x < 0x%x\n",
-		  eeprom_ver, cfg(priv)->eeprom_ver,
-		  calib_ver,  cfg(priv)->eeprom_calib_ver);
+		  eeprom_ver, priv->cfg->eeprom_ver,
+		  calib_ver,  priv->cfg->eeprom_calib_ver);
 	return -EINVAL;
 
 }
 
 int iwl_eeprom_init_hw_params(struct iwl_priv *priv)
 {
-	struct iwl_shared *shrd = priv->shrd;
 	u16 radio_cfg;
 
-	hw_params(priv).sku = iwl_eeprom_query16(shrd, EEPROM_SKU_CAP);
-	if (hw_params(priv).sku & EEPROM_SKU_CAP_11N_ENABLE &&
-	    !cfg(priv)->ht_params) {
+	priv->hw_params.sku = iwl_eeprom_query16(priv, EEPROM_SKU_CAP);
+	if (priv->hw_params.sku & EEPROM_SKU_CAP_11N_ENABLE &&
+	    !priv->cfg->ht_params) {
 		IWL_ERR(priv, "Invalid 11n configuration\n");
 		return -EINVAL;
 	}
 
-	if (!hw_params(priv).sku) {
+	if (!priv->hw_params.sku) {
 		IWL_ERR(priv, "Invalid device sku\n");
 		return -EINVAL;
 	}
 
-	IWL_INFO(priv, "Device SKU: 0x%X\n", hw_params(priv).sku);
+	IWL_INFO(priv, "Device SKU: 0x%X\n", priv->hw_params.sku);
 
-	radio_cfg = iwl_eeprom_query16(shrd, EEPROM_RADIO_CONFIG);
+	radio_cfg = iwl_eeprom_query16(priv, EEPROM_RADIO_CONFIG);
 
-	hw_params(priv).valid_tx_ant = EEPROM_RF_CFG_TX_ANT_MSK(radio_cfg);
-	hw_params(priv).valid_rx_ant = EEPROM_RF_CFG_RX_ANT_MSK(radio_cfg);
+	priv->hw_params.valid_tx_ant = EEPROM_RF_CFG_TX_ANT_MSK(radio_cfg);
+	priv->hw_params.valid_rx_ant = EEPROM_RF_CFG_RX_ANT_MSK(radio_cfg);
 
 	/* check overrides (some devices have wrong EEPROM) */
-	if (cfg(priv)->valid_tx_ant)
-		hw_params(priv).valid_tx_ant = cfg(priv)->valid_tx_ant;
-	if (cfg(priv)->valid_rx_ant)
-		hw_params(priv).valid_rx_ant = cfg(priv)->valid_rx_ant;
+	if (priv->cfg->valid_tx_ant)
+		priv->hw_params.valid_tx_ant = priv->cfg->valid_tx_ant;
+	if (priv->cfg->valid_rx_ant)
+		priv->hw_params.valid_rx_ant = priv->cfg->valid_rx_ant;
 
-	if (!hw_params(priv).valid_tx_ant || !hw_params(priv).valid_rx_ant) {
+	if (!priv->hw_params.valid_tx_ant || !priv->hw_params.valid_rx_ant) {
 		IWL_ERR(priv, "Invalid chain (0x%X, 0x%X)\n",
-			hw_params(priv).valid_tx_ant,
-			hw_params(priv).valid_rx_ant);
+			priv->hw_params.valid_tx_ant,
+			priv->hw_params.valid_rx_ant);
 		return -EINVAL;
 	}
 
 	IWL_INFO(priv, "Valid Tx ant: 0x%X, Valid Rx ant: 0x%X\n",
-		 hw_params(priv).valid_tx_ant, hw_params(priv).valid_rx_ant);
+		 priv->hw_params.valid_tx_ant, priv->hw_params.valid_rx_ant);
 
 	return 0;
 }
 
-void iwl_eeprom_get_mac(const struct iwl_shared *shrd, u8 *mac)
+u16 iwl_eeprom_calib_version(struct iwl_priv *priv)
 {
-	const u8 *addr = iwl_eeprom_query_addr(shrd,
+	struct iwl_eeprom_calib_hdr *hdr;
+
+	hdr = (struct iwl_eeprom_calib_hdr *)iwl_eeprom_query_addr(priv,
+							EEPROM_CALIB_ALL);
+	return hdr->version;
+}
+
+static u32 eeprom_indirect_address(struct iwl_priv *priv, u32 address)
+{
+	u16 offset = 0;
+
+	if ((address & INDIRECT_ADDRESS) == 0)
+		return address;
+
+	switch (address & INDIRECT_TYPE_MSK) {
+	case INDIRECT_HOST:
+		offset = iwl_eeprom_query16(priv, EEPROM_LINK_HOST);
+		break;
+	case INDIRECT_GENERAL:
+		offset = iwl_eeprom_query16(priv, EEPROM_LINK_GENERAL);
+		break;
+	case INDIRECT_REGULATORY:
+		offset = iwl_eeprom_query16(priv, EEPROM_LINK_REGULATORY);
+		break;
+	case INDIRECT_TXP_LIMIT:
+		offset = iwl_eeprom_query16(priv, EEPROM_LINK_TXP_LIMIT);
+		break;
+	case INDIRECT_TXP_LIMIT_SIZE:
+		offset = iwl_eeprom_query16(priv, EEPROM_LINK_TXP_LIMIT_SIZE);
+		break;
+	case INDIRECT_CALIBRATION:
+		offset = iwl_eeprom_query16(priv, EEPROM_LINK_CALIBRATION);
+		break;
+	case INDIRECT_PROCESS_ADJST:
+		offset = iwl_eeprom_query16(priv, EEPROM_LINK_PROCESS_ADJST);
+		break;
+	case INDIRECT_OTHERS:
+		offset = iwl_eeprom_query16(priv, EEPROM_LINK_OTHERS);
+		break;
+	default:
+		IWL_ERR(priv, "illegal indirect type: 0x%X\n",
+		address & INDIRECT_TYPE_MSK);
+		break;
+	}
+
+	/* translate the offset from words to byte */
+	return (address & ADDRESS_MSK) + (offset << 1);
+}
+
+const u8 *iwl_eeprom_query_addr(struct iwl_priv *priv, size_t offset)
+{
+	u32 address = eeprom_indirect_address(priv, offset);
+	BUG_ON(address >= priv->cfg->base_params->eeprom_size);
+	return &priv->eeprom[address];
+}
+
+void iwl_eeprom_get_mac(struct iwl_priv *priv, u8 *mac)
+{
+	const u8 *addr = iwl_eeprom_query_addr(priv,
 					EEPROM_MAC_ADDRESS);
 	memcpy(mac, addr, ETH_ALEN);
 }
@@ -376,7 +431,7 @@ static int iwl_init_otp_access(struct iwl_trans *trans)
 		 * CSR auto clock gate disable bit -
 		 * this is only applicable for HW with OTP shadow RAM
 		 */
-		if (cfg(trans)->base_params->shadow_ram_support)
+		if (trans->cfg->base_params->shadow_ram_support)
 			iwl_set_bit(trans, CSR_DBG_LINK_PWR_MGMT_REG,
 				CSR_RESET_LINK_PWR_MGMT_DISABLED);
 	}
@@ -497,7 +552,7 @@ static int iwl_find_otp_image(struct iwl_trans *trans,
 		}
 		/* more in the link list, continue */
 		usedblocks++;
-	} while (usedblocks <= cfg(trans)->base_params->max_ll_items);
+	} while (usedblocks <= trans->cfg->base_params->max_ll_items);
 
 	/* OTP has no valid blocks */
 	IWL_DEBUG_EEPROM(trans, "OTP has no valid blocks\n");
@@ -591,7 +646,6 @@ iwl_eeprom_enh_txp_read_element(struct iwl_priv *priv,
 
 static void iwl_eeprom_enhanced_txpower(struct iwl_priv *priv)
 {
-	struct iwl_shared *shrd = priv->shrd;
 	struct iwl_eeprom_enhanced_txpwr *txp_array, *txp;
 	int idx, entries;
 	__le16 *txp_len;
@@ -600,10 +654,10 @@ static void iwl_eeprom_enhanced_txpower(struct iwl_priv *priv)
 	BUILD_BUG_ON(sizeof(struct iwl_eeprom_enhanced_txpwr) != 8);
 
 	/* the length is in 16-bit words, but we want entries */
-	txp_len = (__le16 *) iwl_eeprom_query_addr(shrd, EEPROM_TXP_SZ_OFFS);
+	txp_len = (__le16 *) iwl_eeprom_query_addr(priv, EEPROM_TXP_SZ_OFFS);
 	entries = le16_to_cpup(txp_len) * 2 / EEPROM_TXP_ENTRY_LEN;
 
-	txp_array = (void *) iwl_eeprom_query_addr(shrd, EEPROM_TXP_OFFS);
+	txp_array = (void *) iwl_eeprom_query_addr(priv, EEPROM_TXP_OFFS);
 
 	for (idx = 0; idx < entries; idx++) {
 		txp = &txp_array[idx];
@@ -637,7 +691,7 @@ static void iwl_eeprom_enhanced_txpower(struct iwl_priv *priv)
 				 ((txp->delta_20_in_40 & 0xf0) >> 4),
 				 (txp->delta_20_in_40 & 0x0f));
 
-		max_txp_avg = iwl_get_max_txpower_avg(cfg(priv), txp_array, idx,
+		max_txp_avg = iwl_get_max_txpower_avg(priv->cfg, txp_array, idx,
 						      &max_txp_avg_halfdbm);
 
 		/*
@@ -656,66 +710,66 @@ static void iwl_eeprom_enhanced_txpower(struct iwl_priv *priv)
 /**
  * iwl_eeprom_init - read EEPROM contents
  *
- * Load the EEPROM contents from adapter into shrd->eeprom
+ * Load the EEPROM contents from adapter into priv->eeprom
  *
  * NOTE:  This routine uses the non-debug IO access functions.
  */
-int iwl_eeprom_init(struct iwl_trans *trans, u32 hw_rev)
+int iwl_eeprom_init(struct iwl_priv *priv, u32 hw_rev)
 {
 	__le16 *e;
-	u32 gp = iwl_read32(trans, CSR_EEPROM_GP);
+	u32 gp = iwl_read32(priv->trans, CSR_EEPROM_GP);
 	int sz;
 	int ret;
 	u16 addr;
 	u16 validblockaddr = 0;
 	u16 cache_addr = 0;
 
-	trans->nvm_device_type = iwl_get_nvm_type(trans, hw_rev);
-	if (trans->nvm_device_type == -ENOENT)
+	priv->nvm_device_type = iwl_get_nvm_type(priv->trans, hw_rev);
+	if (priv->nvm_device_type == -ENOENT)
 		return -ENOENT;
 	/* allocate eeprom */
-	sz = cfg(trans)->base_params->eeprom_size;
-	IWL_DEBUG_EEPROM(trans, "NVM size = %d\n", sz);
-	trans->shrd->eeprom = kzalloc(sz, GFP_KERNEL);
-	if (!trans->shrd->eeprom) {
+	sz = priv->cfg->base_params->eeprom_size;
+	IWL_DEBUG_EEPROM(priv, "NVM size = %d\n", sz);
+	priv->eeprom = kzalloc(sz, GFP_KERNEL);
+	if (!priv->eeprom) {
 		ret = -ENOMEM;
 		goto alloc_err;
 	}
-	e = (__le16 *)trans->shrd->eeprom;
+	e = (__le16 *)priv->eeprom;
 
-	ret = iwl_eeprom_verify_signature(trans);
+	ret = iwl_eeprom_verify_signature(priv);
 	if (ret < 0) {
-		IWL_ERR(trans, "EEPROM not found, EEPROM_GP=0x%08x\n", gp);
+		IWL_ERR(priv, "EEPROM not found, EEPROM_GP=0x%08x\n", gp);
 		ret = -ENOENT;
 		goto err;
 	}
 
 	/* Make sure driver (instead of uCode) is allowed to read EEPROM */
-	ret = iwl_eeprom_acquire_semaphore(trans);
+	ret = iwl_eeprom_acquire_semaphore(priv->trans);
 	if (ret < 0) {
-		IWL_ERR(trans, "Failed to acquire EEPROM semaphore.\n");
+		IWL_ERR(priv, "Failed to acquire EEPROM semaphore.\n");
 		ret = -ENOENT;
 		goto err;
 	}
 
-	if (trans->nvm_device_type == NVM_DEVICE_TYPE_OTP) {
+	if (priv->nvm_device_type == NVM_DEVICE_TYPE_OTP) {
 
-		ret = iwl_init_otp_access(trans);
+		ret = iwl_init_otp_access(priv->trans);
 		if (ret) {
-			IWL_ERR(trans, "Failed to initialize OTP access.\n");
+			IWL_ERR(priv, "Failed to initialize OTP access.\n");
 			ret = -ENOENT;
 			goto done;
 		}
-		iwl_write32(trans, CSR_EEPROM_GP,
-			    iwl_read32(trans, CSR_EEPROM_GP) &
+		iwl_write32(priv->trans, CSR_EEPROM_GP,
+			    iwl_read32(priv->trans, CSR_EEPROM_GP) &
 			    ~CSR_EEPROM_GP_IF_OWNER_MSK);
 
-		iwl_set_bit(trans, CSR_OTP_GP_REG,
+		iwl_set_bit(priv->trans, CSR_OTP_GP_REG,
 			     CSR_OTP_GP_REG_ECC_CORR_STATUS_MSK |
 			     CSR_OTP_GP_REG_ECC_UNCORR_STATUS_MSK);
 		/* traversing the linked list if no shadow ram supported */
-		if (!cfg(trans)->base_params->shadow_ram_support) {
-			if (iwl_find_otp_image(trans, &validblockaddr)) {
+		if (!priv->cfg->base_params->shadow_ram_support) {
+			if (iwl_find_otp_image(priv->trans, &validblockaddr)) {
 				ret = -ENOENT;
 				goto done;
 			}
@@ -724,7 +778,8 @@ int iwl_eeprom_init(struct iwl_trans *trans, u32 hw_rev)
 		     addr += sizeof(u16)) {
 			__le16 eeprom_data;
 
-			ret = iwl_read_otp_word(trans, addr, &eeprom_data);
+			ret = iwl_read_otp_word(priv->trans, addr,
+						&eeprom_data);
 			if (ret)
 				goto done;
 			e[cache_addr / 2] = eeprom_data;
@@ -735,94 +790,93 @@ int iwl_eeprom_init(struct iwl_trans *trans, u32 hw_rev)
 		for (addr = 0; addr < sz; addr += sizeof(u16)) {
 			u32 r;
 
-			iwl_write32(trans, CSR_EEPROM_REG,
+			iwl_write32(priv->trans, CSR_EEPROM_REG,
 				    CSR_EEPROM_REG_MSK_ADDR & (addr << 1));
 
-			ret = iwl_poll_bit(trans, CSR_EEPROM_REG,
+			ret = iwl_poll_bit(priv->trans, CSR_EEPROM_REG,
 						  CSR_EEPROM_REG_READ_VALID_MSK,
 						  CSR_EEPROM_REG_READ_VALID_MSK,
 						  IWL_EEPROM_ACCESS_TIMEOUT);
 			if (ret < 0) {
-				IWL_ERR(trans,
+				IWL_ERR(priv,
 					"Time out reading EEPROM[%d]\n", addr);
 				goto done;
 			}
-			r = iwl_read32(trans, CSR_EEPROM_REG);
+			r = iwl_read32(priv->trans, CSR_EEPROM_REG);
 			e[addr / 2] = cpu_to_le16(r >> 16);
 		}
 	}
 
-	IWL_DEBUG_EEPROM(trans, "NVM Type: %s, version: 0x%x\n",
-		       (trans->nvm_device_type == NVM_DEVICE_TYPE_OTP)
+	IWL_DEBUG_EEPROM(priv, "NVM Type: %s, version: 0x%x\n",
+		       (priv->nvm_device_type == NVM_DEVICE_TYPE_OTP)
 		       ? "OTP" : "EEPROM",
-		       iwl_eeprom_query16(trans->shrd, EEPROM_VERSION));
+		       iwl_eeprom_query16(priv, EEPROM_VERSION));
 
 	ret = 0;
 done:
-	iwl_eeprom_release_semaphore(trans);
+	iwl_eeprom_release_semaphore(priv->trans);
 
 err:
 	if (ret)
-		iwl_eeprom_free(trans->shrd);
+		iwl_eeprom_free(priv);
 alloc_err:
 	return ret;
 }
 
-void iwl_eeprom_free(struct iwl_shared *shrd)
+void iwl_eeprom_free(struct iwl_priv *priv)
 {
-	kfree(shrd->eeprom);
-	shrd->eeprom = NULL;
+	kfree(priv->eeprom);
+	priv->eeprom = NULL;
 }
 
-static void iwl_init_band_reference(const struct iwl_priv *priv,
+static void iwl_init_band_reference(struct iwl_priv *priv,
 			int eep_band, int *eeprom_ch_count,
 			const struct iwl_eeprom_channel **eeprom_ch_info,
 			const u8 **eeprom_ch_index)
 {
-	struct iwl_shared *shrd = priv->shrd;
-	u32 offset = cfg(priv)->lib->
+	u32 offset = priv->lib->
 			eeprom_ops.regulatory_bands[eep_band - 1];
 	switch (eep_band) {
 	case 1:		/* 2.4GHz band */
 		*eeprom_ch_count = ARRAY_SIZE(iwl_eeprom_band_1);
 		*eeprom_ch_info = (struct iwl_eeprom_channel *)
-				iwl_eeprom_query_addr(shrd, offset);
+				iwl_eeprom_query_addr(priv, offset);
 		*eeprom_ch_index = iwl_eeprom_band_1;
 		break;
 	case 2:		/* 4.9GHz band */
 		*eeprom_ch_count = ARRAY_SIZE(iwl_eeprom_band_2);
 		*eeprom_ch_info = (struct iwl_eeprom_channel *)
-				iwl_eeprom_query_addr(shrd, offset);
+				iwl_eeprom_query_addr(priv, offset);
 		*eeprom_ch_index = iwl_eeprom_band_2;
 		break;
 	case 3:		/* 5.2GHz band */
 		*eeprom_ch_count = ARRAY_SIZE(iwl_eeprom_band_3);
 		*eeprom_ch_info = (struct iwl_eeprom_channel *)
-				iwl_eeprom_query_addr(shrd, offset);
+				iwl_eeprom_query_addr(priv, offset);
 		*eeprom_ch_index = iwl_eeprom_band_3;
 		break;
 	case 4:		/* 5.5GHz band */
 		*eeprom_ch_count = ARRAY_SIZE(iwl_eeprom_band_4);
 		*eeprom_ch_info = (struct iwl_eeprom_channel *)
-				iwl_eeprom_query_addr(shrd, offset);
+				iwl_eeprom_query_addr(priv, offset);
 		*eeprom_ch_index = iwl_eeprom_band_4;
 		break;
 	case 5:		/* 5.7GHz band */
 		*eeprom_ch_count = ARRAY_SIZE(iwl_eeprom_band_5);
 		*eeprom_ch_info = (struct iwl_eeprom_channel *)
-				iwl_eeprom_query_addr(shrd, offset);
+				iwl_eeprom_query_addr(priv, offset);
 		*eeprom_ch_index = iwl_eeprom_band_5;
 		break;
 	case 6:		/* 2.4GHz ht40 channels */
 		*eeprom_ch_count = ARRAY_SIZE(iwl_eeprom_band_6);
 		*eeprom_ch_info = (struct iwl_eeprom_channel *)
-				iwl_eeprom_query_addr(shrd, offset);
+				iwl_eeprom_query_addr(priv, offset);
 		*eeprom_ch_index = iwl_eeprom_band_6;
 		break;
 	case 7:		/* 5 GHz ht40 channels */
 		*eeprom_ch_count = ARRAY_SIZE(iwl_eeprom_band_7);
 		*eeprom_ch_info = (struct iwl_eeprom_channel *)
-				iwl_eeprom_query_addr(shrd, offset);
+				iwl_eeprom_query_addr(priv, offset);
 		*eeprom_ch_index = iwl_eeprom_band_7;
 		break;
 	default:
@@ -987,9 +1041,9 @@ int iwl_init_channel_map(struct iwl_priv *priv)
 	}
 
 	/* Check if we do have HT40 channels */
-	if (cfg(priv)->lib->eeprom_ops.regulatory_bands[5] ==
+	if (priv->lib->eeprom_ops.regulatory_bands[5] ==
 	    EEPROM_REGULATORY_BAND_NO_HT40 &&
-	    cfg(priv)->lib->eeprom_ops.regulatory_bands[6] ==
+	    priv->lib->eeprom_ops.regulatory_bands[6] ==
 	    EEPROM_REGULATORY_BAND_NO_HT40)
 		return 0;
 
@@ -1025,7 +1079,7 @@ int iwl_init_channel_map(struct iwl_priv *priv)
 	 * driver need to process addition information
 	 * to determine the max channel tx power limits
 	 */
-	if (cfg(priv)->lib->eeprom_ops.enhanced_txpower)
+	if (priv->lib->eeprom_ops.enhanced_txpower)
 		iwl_eeprom_enhanced_txpower(priv);
 
 	return 0;
@@ -1072,11 +1126,11 @@ void iwl_rf_config(struct iwl_priv *priv)
 {
 	u16 radio_cfg;
 
-	radio_cfg = iwl_eeprom_query16(priv->shrd, EEPROM_RADIO_CONFIG);
+	radio_cfg = iwl_eeprom_query16(priv, EEPROM_RADIO_CONFIG);
 
 	/* write radio config values to register */
 	if (EEPROM_RF_CFG_TYPE_MSK(radio_cfg) <= EEPROM_RF_CONFIG_TYPE_MAX) {
-		iwl_set_bit(trans(priv), CSR_HW_IF_CONFIG_REG,
+		iwl_set_bit(priv->trans, CSR_HW_IF_CONFIG_REG,
 			    EEPROM_RF_CFG_TYPE_MSK(radio_cfg) |
 			    EEPROM_RF_CFG_STEP_MSK(radio_cfg) |
 			    EEPROM_RF_CFG_DASH_MSK(radio_cfg));
@@ -1088,7 +1142,7 @@ void iwl_rf_config(struct iwl_priv *priv)
 		WARN_ON(1);
 
 	/* set CSR_HW_CONFIG_REG for uCode use */
-	iwl_set_bit(trans(priv), CSR_HW_IF_CONFIG_REG,
+	iwl_set_bit(priv->trans, CSR_HW_IF_CONFIG_REG,
 		    CSR_HW_IF_CONFIG_REG_BIT_RADIO_SI |
 		    CSR_HW_IF_CONFIG_REG_BIT_MAC_SI);
 }
diff --git a/drivers/net/wireless/iwlwifi/iwl-eeprom.h b/drivers/net/wireless/iwlwifi/iwl-eeprom.h
index e4a75834099..64bfd947cae 100644
--- a/drivers/net/wireless/iwlwifi/iwl-eeprom.h
+++ b/drivers/net/wireless/iwlwifi/iwl-eeprom.h
@@ -66,8 +66,6 @@
 #include <net/mac80211.h>
 
 struct iwl_priv;
-struct iwl_shared;
-struct iwl_trans;
 
 /*
  * EEPROM access time values:
@@ -208,59 +206,6 @@ struct iwl_eeprom_calib_hdr {
 /* 6000 regulatory - indirect access */
 #define EEPROM_6000_REG_BAND_24_HT40_CHANNELS  ((0x80)\
 		| INDIRECT_ADDRESS | INDIRECT_REGULATORY)   /* 14  bytes */
-
-/* 5000 Specific */
-#define EEPROM_5000_TX_POWER_VERSION    (4)
-#define EEPROM_5000_EEPROM_VERSION	(0x11A)
-
-/* 5050 Specific */
-#define EEPROM_5050_TX_POWER_VERSION    (4)
-#define EEPROM_5050_EEPROM_VERSION	(0x21E)
-
-/* 1000 Specific */
-#define EEPROM_1000_TX_POWER_VERSION    (4)
-#define EEPROM_1000_EEPROM_VERSION	(0x15C)
-
-/* 6x00 Specific */
-#define EEPROM_6000_TX_POWER_VERSION    (4)
-#define EEPROM_6000_EEPROM_VERSION	(0x423)
-
-/* 6x50 Specific */
-#define EEPROM_6050_TX_POWER_VERSION    (4)
-#define EEPROM_6050_EEPROM_VERSION	(0x532)
-
-/* 6150 Specific */
-#define EEPROM_6150_TX_POWER_VERSION    (6)
-#define EEPROM_6150_EEPROM_VERSION	(0x553)
-
-/* 6x05 Specific */
-#define EEPROM_6005_TX_POWER_VERSION    (6)
-#define EEPROM_6005_EEPROM_VERSION	(0x709)
-
-/* 6x30 Specific */
-#define EEPROM_6030_TX_POWER_VERSION    (6)
-#define EEPROM_6030_EEPROM_VERSION	(0x709)
-
-/* 2x00 Specific */
-#define EEPROM_2000_TX_POWER_VERSION    (6)
-#define EEPROM_2000_EEPROM_VERSION	(0x805)
-
-/* 6x35 Specific */
-#define EEPROM_6035_TX_POWER_VERSION    (6)
-#define EEPROM_6035_EEPROM_VERSION	(0x753)
-
-
-/* OTP */
-/* lower blocks contain EEPROM image and calibration data */
-#define OTP_LOW_IMAGE_SIZE		(2 * 512 * sizeof(u16)) /* 2 KB */
-/* high blocks contain PAPD data */
-#define OTP_HIGH_IMAGE_SIZE_6x00        (6 * 512 * sizeof(u16)) /* 6 KB */
-#define OTP_HIGH_IMAGE_SIZE_1000        (0x200 * sizeof(u16)) /* 1024 bytes */
-#define OTP_MAX_LL_ITEMS_1000		(3)	/* OTP blocks for 1000 */
-#define OTP_MAX_LL_ITEMS_6x00		(4)	/* OTP blocks for 6x00 */
-#define OTP_MAX_LL_ITEMS_6x50		(7)	/* OTP blocks for 6x50 */
-#define OTP_MAX_LL_ITEMS_2x00		(4)	/* OTP blocks for 2x00 */
-
 /* 2.4 GHz */
 extern const u8 iwl_eeprom_band_1[14];
 
@@ -306,12 +251,14 @@ struct iwl_eeprom_ops {
 };
 
 
-int iwl_eeprom_init(struct iwl_trans *trans, u32 hw_rev);
-void iwl_eeprom_free(struct iwl_shared *shrd);
-int  iwl_eeprom_check_version(struct iwl_priv *priv);
+int iwl_eeprom_init(struct iwl_priv *priv, u32 hw_rev);
+void iwl_eeprom_free(struct iwl_priv *priv);
+int iwl_eeprom_check_version(struct iwl_priv *priv);
 int iwl_eeprom_init_hw_params(struct iwl_priv *priv);
-const u8 *iwl_eeprom_query_addr(const struct iwl_shared *shrd, size_t offset);
-u16 iwl_eeprom_query16(const struct iwl_shared *shrd, size_t offset);
+u16 iwl_eeprom_calib_version(struct iwl_priv *priv);
+const u8 *iwl_eeprom_query_addr(struct iwl_priv *priv, size_t offset);
+u16 iwl_eeprom_query16(struct iwl_priv *priv, size_t offset);
+void iwl_eeprom_get_mac(struct iwl_priv *priv, u8 *mac);
 int iwl_init_channel_map(struct iwl_priv *priv);
 void iwl_free_channel_map(struct iwl_priv *priv);
 const struct iwl_channel_info *iwl_get_channel_info(
diff --git a/drivers/net/wireless/iwlwifi/iwl-fw-file.h b/drivers/net/wireless/iwlwifi/iwl-fw-file.h
index c924ccb93c8..e71564053e7 100644
--- a/drivers/net/wireless/iwlwifi/iwl-fw-file.h
+++ b/drivers/net/wireless/iwlwifi/iwl-fw-file.h
@@ -93,15 +93,7 @@ struct iwl_ucode_header {
  * new TLV uCode file layout
  *
  * The new TLV file format contains TLVs, that each specify
- * some piece of data. To facilitate "groups", for example
- * different instruction image with different capabilities,
- * bundled with the same init image, an alternative mechanism
- * is provided:
- * When the alternative field is 0, that means that the item
- * is always valid. When it is non-zero, then it is only
- * valid in conjunction with items of the same alternative,
- * in which case the driver (user) selects one alternative
- * to use.
+ * some piece of data.
  */
 
 enum iwl_ucode_tlv_type {
@@ -132,8 +124,7 @@ enum iwl_ucode_tlv_type {
 };
 
 struct iwl_ucode_tlv {
-	__le16 type;		/* see above */
-	__le16 alternative;	/* see comment */
+	__le32 type;		/* see above */
 	__le32 length;		/* not including type/length fields */
 	u8 data[0];
 };
@@ -152,7 +143,7 @@ struct iwl_tlv_ucode_header {
 	u8 human_readable[64];
 	__le32 ver;		/* major/minor/API/serial */
 	__le32 build;
-	__le64 alternatives;	/* bitmask of valid alternatives */
+	__le64 ignore;
 	/*
 	 * The data contained herein has a TLV layout,
 	 * see above for the TLV header and types.
diff --git a/drivers/net/wireless/iwlwifi/iwl-fw.h b/drivers/net/wireless/iwlwifi/iwl-fw.h
index 8e36bdc1e52..2153e4cc557 100644
--- a/drivers/net/wireless/iwlwifi/iwl-fw.h
+++ b/drivers/net/wireless/iwlwifi/iwl-fw.h
@@ -63,6 +63,7 @@
 #ifndef __iwl_fw_h__
 #define __iwl_fw_h__
 #include <linux/types.h>
+#include <net/mac80211.h>
 
 /**
  * enum iwl_ucode_tlv_flag - ucode API flags
diff --git a/drivers/net/wireless/iwlwifi/iwl-io.h b/drivers/net/wireless/iwlwifi/iwl-io.h
index 09b856768f6..abb3250164b 100644
--- a/drivers/net/wireless/iwlwifi/iwl-io.h
+++ b/drivers/net/wireless/iwlwifi/iwl-io.h
@@ -30,7 +30,6 @@
 #define __iwl_io_h__
 
 #include "iwl-devtrace.h"
-#include "iwl-shared.h"
 #include "iwl-trans.h"
 
 static inline void iwl_write8(struct iwl_trans *trans, u32 ofs, u8 val)
diff --git a/drivers/net/wireless/iwlwifi/iwl-led.c b/drivers/net/wireless/iwlwifi/iwl-led.c
index 1993a2b7ae6..47000419f91 100644
--- a/drivers/net/wireless/iwlwifi/iwl-led.c
+++ b/drivers/net/wireless/iwlwifi/iwl-led.c
@@ -36,11 +36,10 @@
 #include <asm/unaligned.h>
 
 #include "iwl-dev.h"
-#include "iwl-core.h"
 #include "iwl-agn.h"
 #include "iwl-io.h"
 #include "iwl-trans.h"
-#include "iwl-shared.h"
+#include "iwl-modparams.h"
 
 /* Throughput		OFF time(ms)	ON time (ms)
  *	>300			25		25
@@ -71,7 +70,7 @@ static const struct ieee80211_tpt_blink iwl_blink[] = {
 /* Set led register off */
 void iwlagn_led_enable(struct iwl_priv *priv)
 {
-	iwl_write32(trans(priv), CSR_LED_REG, CSR_LED_REG_TRUN_ON);
+	iwl_write32(priv->trans, CSR_LED_REG, CSR_LED_REG_TRUN_ON);
 }
 
 /*
@@ -107,9 +106,9 @@ static int iwl_send_led_cmd(struct iwl_priv *priv, struct iwl_led_cmd *led_cmd)
 	};
 	u32 reg;
 
-	reg = iwl_read32(trans(priv), CSR_LED_REG);
+	reg = iwl_read32(priv->trans, CSR_LED_REG);
 	if (reg != (reg & CSR_LED_BSM_CTRL_MSK))
-		iwl_write32(trans(priv), CSR_LED_REG,
+		iwl_write32(priv->trans, CSR_LED_REG,
 			    reg & CSR_LED_BSM_CTRL_MSK);
 
 	return iwl_dvm_send_cmd(priv, &cmd);
@@ -138,11 +137,11 @@ static int iwl_led_cmd(struct iwl_priv *priv,
 	}
 
 	IWL_DEBUG_LED(priv, "Led blink time compensation=%u\n",
-			cfg(priv)->base_params->led_compensation);
+			priv->cfg->base_params->led_compensation);
 	led_cmd.on = iwl_blink_compensation(priv, on,
-				cfg(priv)->base_params->led_compensation);
+				priv->cfg->base_params->led_compensation);
 	led_cmd.off = iwl_blink_compensation(priv, off,
-				cfg(priv)->base_params->led_compensation);
+				priv->cfg->base_params->led_compensation);
 
 	ret = iwl_send_led_cmd(priv, &led_cmd);
 	if (!ret) {
@@ -175,7 +174,7 @@ static int iwl_led_blink_set(struct led_classdev *led_cdev,
 
 void iwl_leds_init(struct iwl_priv *priv)
 {
-	int mode = iwlagn_mod_params.led_mode;
+	int mode = iwlwifi_mod_params.led_mode;
 	int ret;
 
 	if (mode == IWL_LED_DISABLE) {
@@ -183,7 +182,7 @@ void iwl_leds_init(struct iwl_priv *priv)
 		return;
 	}
 	if (mode == IWL_LED_DEFAULT)
-		mode = cfg(priv)->led_mode;
+		mode = priv->cfg->led_mode;
 
 	priv->led.name = kasprintf(GFP_KERNEL, "%s-led",
 				   wiphy_name(priv->hw->wiphy));
@@ -207,7 +206,7 @@ void iwl_leds_init(struct iwl_priv *priv)
 		break;
 	}
 
-	ret = led_classdev_register(trans(priv)->dev, &priv->led);
+	ret = led_classdev_register(priv->trans->dev, &priv->led);
 	if (ret) {
 		kfree(priv->led.name);
 		return;
diff --git a/drivers/net/wireless/iwlwifi/iwl-mac80211.c b/drivers/net/wireless/iwlwifi/iwl-mac80211.c
index c24a7134a6f..ab2f4d7500a 100644
--- a/drivers/net/wireless/iwlwifi/iwl-mac80211.c
+++ b/drivers/net/wireless/iwlwifi/iwl-mac80211.c
@@ -44,13 +44,12 @@
 
 #include "iwl-eeprom.h"
 #include "iwl-dev.h"
-#include "iwl-core.h"
 #include "iwl-io.h"
 #include "iwl-agn-calib.h"
 #include "iwl-agn.h"
-#include "iwl-shared.h"
 #include "iwl-trans.h"
 #include "iwl-op-mode.h"
+#include "iwl-modparams.h"
 
 /*****************************************************************************
  *
@@ -147,7 +146,14 @@ int iwlagn_mac_setup_register(struct iwl_priv *priv,
 		    IEEE80211_HW_AMPDU_AGGREGATION |
 		    IEEE80211_HW_NEED_DTIM_PERIOD |
 		    IEEE80211_HW_SPECTRUM_MGMT |
-		    IEEE80211_HW_REPORTS_TX_ACK_STATUS;
+		    IEEE80211_HW_REPORTS_TX_ACK_STATUS |
+		    IEEE80211_HW_QUEUE_CONTROL |
+		    IEEE80211_HW_SUPPORTS_PS |
+		    IEEE80211_HW_SUPPORTS_DYNAMIC_PS |
+		    IEEE80211_HW_WANT_MONITOR_VIF |
+		    IEEE80211_HW_SCAN_WHILE_IDLE;
+
+	hw->offchannel_tx_hw_queue = IWL_AUX_QUEUE;
 
 	/*
 	 * Including the following line will crash some AP's.  This
@@ -156,10 +162,7 @@ int iwlagn_mac_setup_register(struct iwl_priv *priv,
 	hw->max_tx_aggregation_subframes = LINK_QUAL_AGG_FRAME_LIMIT_DEF;
 	 */
 
-	hw->flags |= IEEE80211_HW_SUPPORTS_PS |
-		     IEEE80211_HW_SUPPORTS_DYNAMIC_PS;
-
-	if (hw_params(priv).sku & EEPROM_SKU_CAP_11N_ENABLE)
+	if (priv->hw_params.sku & EEPROM_SKU_CAP_11N_ENABLE)
 		hw->flags |= IEEE80211_HW_SUPPORTS_DYNAMIC_SMPS |
 			     IEEE80211_HW_SUPPORTS_STATIC_SMPS;
 
@@ -197,13 +200,13 @@ int iwlagn_mac_setup_register(struct iwl_priv *priv,
 			    WIPHY_FLAG_IBSS_RSN;
 
 	if (priv->fw->img[IWL_UCODE_WOWLAN].sec[0].len &&
-	    trans(priv)->ops->wowlan_suspend &&
-	    device_can_wakeup(trans(priv)->dev)) {
+	    priv->trans->ops->wowlan_suspend &&
+	    device_can_wakeup(priv->trans->dev)) {
 		hw->wiphy->wowlan.flags = WIPHY_WOWLAN_MAGIC_PKT |
 					  WIPHY_WOWLAN_DISCONNECT |
 					  WIPHY_WOWLAN_EAP_IDENTITY_REQ |
 					  WIPHY_WOWLAN_RFKILL_RELEASE;
-		if (!iwlagn_mod_params.sw_crypto)
+		if (!iwlwifi_mod_params.sw_crypto)
 			hw->wiphy->wowlan.flags |=
 				WIPHY_WOWLAN_SUPPORTS_GTK_REKEY |
 				WIPHY_WOWLAN_GTK_REKEY_FAILURE;
@@ -215,17 +218,20 @@ int iwlagn_mac_setup_register(struct iwl_priv *priv,
 					IWLAGN_WOWLAN_MAX_PATTERN_LEN;
 	}
 
-	if (iwlagn_mod_params.power_save)
+	if (iwlwifi_mod_params.power_save)
 		hw->wiphy->flags |= WIPHY_FLAG_PS_ON_BY_DEFAULT;
 	else
 		hw->wiphy->flags &= ~WIPHY_FLAG_PS_ON_BY_DEFAULT;
 
 	hw->wiphy->max_scan_ssids = PROBE_OPTION_MAX;
-	/* we create the 802.11 header and a zero-length SSID element */
-	hw->wiphy->max_scan_ie_len = capa->max_probe_length - 24 - 2;
+	/* we create the 802.11 header and a max-length SSID element */
+	hw->wiphy->max_scan_ie_len = capa->max_probe_length - 24 - 34;
 
-	/* Default value; 4 EDCA QOS priorities */
-	hw->queues = 4;
+	/*
+	 * We don't use all queues: 4 and 9 are unused and any
+	 * aggregation queue gets mapped down to the AC queue.
+	 */
+	hw->queues = IWLAGN_FIRST_AMPDU_QUEUE;
 
 	hw->max_listen_interval = IWL_CONN_MAX_LISTEN_INTERVAL;
 
@@ -236,7 +242,7 @@ int iwlagn_mac_setup_register(struct iwl_priv *priv,
 		priv->hw->wiphy->bands[IEEE80211_BAND_5GHZ] =
 			&priv->bands[IEEE80211_BAND_5GHZ];
 
-	hw->wiphy->hw_version = trans(priv)->hw_id;
+	hw->wiphy->hw_version = priv->trans->hw_id;
 
 	iwl_leds_init(priv);
 
@@ -332,7 +338,7 @@ static int iwlagn_mac_start(struct ieee80211_hw *hw)
 	return 0;
 }
 
-static void iwlagn_mac_stop(struct ieee80211_hw *hw)
+void iwlagn_mac_stop(struct ieee80211_hw *hw)
 {
 	struct iwl_priv *priv = IWL_MAC80211_GET_DVM(hw);
 
@@ -355,18 +361,18 @@ static void iwlagn_mac_stop(struct ieee80211_hw *hw)
 	 * even if interface is down, trans->down will leave the RF
 	 * kill interrupt enabled
 	 */
-	iwl_trans_stop_hw(trans(priv));
+	iwl_trans_stop_hw(priv->trans, false);
 
 	IWL_DEBUG_MAC80211(priv, "leave\n");
 }
 
-static void iwlagn_mac_set_rekey_data(struct ieee80211_hw *hw,
-				      struct ieee80211_vif *vif,
-				      struct cfg80211_gtk_rekey_data *data)
+void iwlagn_mac_set_rekey_data(struct ieee80211_hw *hw,
+			       struct ieee80211_vif *vif,
+			       struct cfg80211_gtk_rekey_data *data)
 {
 	struct iwl_priv *priv = IWL_MAC80211_GET_DVM(hw);
 
-	if (iwlagn_mod_params.sw_crypto)
+	if (iwlwifi_mod_params.sw_crypto)
 		return;
 
 	IWL_DEBUG_MAC80211(priv, "enter\n");
@@ -388,8 +394,7 @@ static void iwlagn_mac_set_rekey_data(struct ieee80211_hw *hw,
 
 #ifdef CONFIG_PM_SLEEP
 
-static int iwlagn_mac_suspend(struct ieee80211_hw *hw,
-			      struct cfg80211_wowlan *wowlan)
+int iwlagn_mac_suspend(struct ieee80211_hw *hw, struct cfg80211_wowlan *wowlan)
 {
 	struct iwl_priv *priv = IWL_MAC80211_GET_DVM(hw);
 	struct iwl_rxon_context *ctx = &priv->contexts[IWL_RXON_CTX_BSS];
@@ -412,9 +417,9 @@ static int iwlagn_mac_suspend(struct ieee80211_hw *hw,
 	if (ret)
 		goto error;
 
-	device_set_wakeup_enable(trans(priv)->dev, true);
+	device_set_wakeup_enable(priv->trans->dev, true);
 
-	iwl_trans_wowlan_suspend(trans(priv));
+	iwl_trans_wowlan_suspend(priv->trans);
 
 	goto out;
 
@@ -437,27 +442,28 @@ static int iwlagn_mac_resume(struct ieee80211_hw *hw)
 	unsigned long flags;
 	u32 base, status = 0xffffffff;
 	int ret = -EIO;
-	const struct fw_img *img;
 
 	IWL_DEBUG_MAC80211(priv, "enter\n");
 	mutex_lock(&priv->mutex);
 
-	iwl_write32(trans(priv), CSR_UCODE_DRV_GP1_CLR,
+	iwl_write32(priv->trans, CSR_UCODE_DRV_GP1_CLR,
 			  CSR_UCODE_DRV_GP1_BIT_D3_CFG_COMPLETE);
 
-	base = priv->shrd->device_pointers.error_event_table;
+	base = priv->device_pointers.error_event_table;
 	if (iwlagn_hw_valid_rtc_data_addr(base)) {
-		spin_lock_irqsave(&trans(priv)->reg_lock, flags);
-		ret = iwl_grab_nic_access_silent(trans(priv));
+		spin_lock_irqsave(&priv->trans->reg_lock, flags);
+		ret = iwl_grab_nic_access_silent(priv->trans);
 		if (likely(ret == 0)) {
-			iwl_write32(trans(priv), HBUS_TARG_MEM_RADDR, base);
-			status = iwl_read32(trans(priv), HBUS_TARG_MEM_RDAT);
-			iwl_release_nic_access(trans(priv));
+			iwl_write32(priv->trans, HBUS_TARG_MEM_RADDR, base);
+			status = iwl_read32(priv->trans, HBUS_TARG_MEM_RDAT);
+			iwl_release_nic_access(priv->trans);
 		}
-		spin_unlock_irqrestore(&trans(priv)->reg_lock, flags);
+		spin_unlock_irqrestore(&priv->trans->reg_lock, flags);
 
 #ifdef CONFIG_IWLWIFI_DEBUGFS
 		if (ret == 0) {
+			const struct fw_img *img;
+
 			img = &(priv->fw->img[IWL_UCODE_WOWLAN]);
 			if (!priv->wowlan_sram) {
 				priv->wowlan_sram =
@@ -467,7 +473,7 @@ static int iwlagn_mac_resume(struct ieee80211_hw *hw)
 
 			if (priv->wowlan_sram)
 				_iwl_read_targ_mem_words(
-				      trans(priv), 0x800000,
+				      priv->trans, 0x800000,
 				      priv->wowlan_sram,
 				      img->sec[IWL_UCODE_SECTION_DATA].len / 4);
 		}
@@ -479,7 +485,7 @@ static int iwlagn_mac_resume(struct ieee80211_hw *hw)
 
 	priv->wowlan = false;
 
-	device_set_wakeup_enable(trans(priv)->dev, false);
+	device_set_wakeup_enable(priv->trans->dev, false);
 
 	iwlagn_prepare_restart(priv);
 
@@ -497,7 +503,7 @@ static int iwlagn_mac_resume(struct ieee80211_hw *hw)
 
 #endif
 
-static void iwlagn_mac_tx(struct ieee80211_hw *hw, struct sk_buff *skb)
+void iwlagn_mac_tx(struct ieee80211_hw *hw, struct sk_buff *skb)
 {
 	struct iwl_priv *priv = IWL_MAC80211_GET_DVM(hw);
 
@@ -508,21 +514,21 @@ static void iwlagn_mac_tx(struct ieee80211_hw *hw, struct sk_buff *skb)
 		dev_kfree_skb_any(skb);
 }
 
-static void iwlagn_mac_update_tkip_key(struct ieee80211_hw *hw,
-				       struct ieee80211_vif *vif,
-				       struct ieee80211_key_conf *keyconf,
-				       struct ieee80211_sta *sta,
-				       u32 iv32, u16 *phase1key)
+void iwlagn_mac_update_tkip_key(struct ieee80211_hw *hw,
+				struct ieee80211_vif *vif,
+				struct ieee80211_key_conf *keyconf,
+				struct ieee80211_sta *sta,
+				u32 iv32, u16 *phase1key)
 {
 	struct iwl_priv *priv = IWL_MAC80211_GET_DVM(hw);
 
 	iwl_update_tkip_key(priv, vif, keyconf, sta, iv32, phase1key);
 }
 
-static int iwlagn_mac_set_key(struct ieee80211_hw *hw, enum set_key_cmd cmd,
-			      struct ieee80211_vif *vif,
-			      struct ieee80211_sta *sta,
-			      struct ieee80211_key_conf *key)
+int iwlagn_mac_set_key(struct ieee80211_hw *hw, enum set_key_cmd cmd,
+		       struct ieee80211_vif *vif,
+		       struct ieee80211_sta *sta,
+		       struct ieee80211_key_conf *key)
 {
 	struct iwl_priv *priv = IWL_MAC80211_GET_DVM(hw);
 	struct iwl_vif_priv *vif_priv = (void *)vif->drv_priv;
@@ -532,7 +538,7 @@ static int iwlagn_mac_set_key(struct ieee80211_hw *hw, enum set_key_cmd cmd,
 
 	IWL_DEBUG_MAC80211(priv, "enter\n");
 
-	if (iwlagn_mod_params.sw_crypto) {
+	if (iwlwifi_mod_params.sw_crypto) {
 		IWL_DEBUG_MAC80211(priv, "leave - hwcrypto disabled\n");
 		return -EOPNOTSUPP;
 	}
@@ -622,11 +628,11 @@ static int iwlagn_mac_set_key(struct ieee80211_hw *hw, enum set_key_cmd cmd,
 	return ret;
 }
 
-static int iwlagn_mac_ampdu_action(struct ieee80211_hw *hw,
-				   struct ieee80211_vif *vif,
-				   enum ieee80211_ampdu_mlme_action action,
-				   struct ieee80211_sta *sta, u16 tid, u16 *ssn,
-				   u8 buf_size)
+int iwlagn_mac_ampdu_action(struct ieee80211_hw *hw,
+			    struct ieee80211_vif *vif,
+			    enum ieee80211_ampdu_mlme_action action,
+			    struct ieee80211_sta *sta, u16 tid, u16 *ssn,
+			    u8 buf_size)
 {
 	struct iwl_priv *priv = IWL_MAC80211_GET_DVM(hw);
 	int ret = -EINVAL;
@@ -635,7 +641,7 @@ static int iwlagn_mac_ampdu_action(struct ieee80211_hw *hw,
 	IWL_DEBUG_HT(priv, "A-MPDU action on addr %pM tid %d\n",
 		     sta->addr, tid);
 
-	if (!(hw_params(priv).sku & EEPROM_SKU_CAP_11N_ENABLE))
+	if (!(priv->hw_params.sku & EEPROM_SKU_CAP_11N_ENABLE))
 		return -EACCES;
 
 	IWL_DEBUG_MAC80211(priv, "enter\n");
@@ -643,7 +649,7 @@ static int iwlagn_mac_ampdu_action(struct ieee80211_hw *hw,
 
 	switch (action) {
 	case IEEE80211_AMPDU_RX_START:
-		if (iwlagn_mod_params.disable_11n & IWL_DISABLE_HT_RXAGG)
+		if (iwlwifi_mod_params.disable_11n & IWL_DISABLE_HT_RXAGG)
 			break;
 		IWL_DEBUG_HT(priv, "start Rx\n");
 		ret = iwl_sta_rx_agg_start(priv, sta, tid, *ssn);
@@ -653,7 +659,9 @@ static int iwlagn_mac_ampdu_action(struct ieee80211_hw *hw,
 		ret = iwl_sta_rx_agg_stop(priv, sta, tid);
 		break;
 	case IEEE80211_AMPDU_TX_START:
-		if (iwlagn_mod_params.disable_11n & IWL_DISABLE_HT_TXAGG)
+		if (!priv->trans->ops->tx_agg_setup)
+			break;
+		if (iwlwifi_mod_params.disable_11n & IWL_DISABLE_HT_TXAGG)
 			break;
 		IWL_DEBUG_HT(priv, "start Tx\n");
 		ret = iwlagn_tx_agg_start(priv, vif, sta, tid, ssn);
@@ -667,7 +675,7 @@ static int iwlagn_mac_ampdu_action(struct ieee80211_hw *hw,
 				     priv->agg_tids_count);
 		}
 		if (!priv->agg_tids_count &&
-		    hw_params(priv).use_rts_for_aggregation) {
+		    priv->hw_params.use_rts_for_aggregation) {
 			/*
 			 * switch off RTS/CTS if it was previously enabled
 			 */
@@ -746,11 +754,11 @@ static int iwlagn_mac_sta_remove(struct ieee80211_hw *hw,
 	return ret;
 }
 
-static int iwlagn_mac_sta_state(struct ieee80211_hw *hw,
-				struct ieee80211_vif *vif,
-				struct ieee80211_sta *sta,
-				enum ieee80211_sta_state old_state,
-				enum ieee80211_sta_state new_state)
+int iwlagn_mac_sta_state(struct ieee80211_hw *hw,
+			 struct ieee80211_vif *vif,
+			 struct ieee80211_sta *sta,
+			 enum ieee80211_sta_state old_state,
+			 enum ieee80211_sta_state new_state)
 {
 	struct iwl_priv *priv = IWL_MAC80211_GET_DVM(hw);
 	struct iwl_vif_priv *vif_priv = (void *)vif->drv_priv;
@@ -829,8 +837,8 @@ static int iwlagn_mac_sta_state(struct ieee80211_hw *hw,
 	return ret;
 }
 
-static void iwlagn_mac_channel_switch(struct ieee80211_hw *hw,
-				struct ieee80211_channel_switch *ch_switch)
+void iwlagn_mac_channel_switch(struct ieee80211_hw *hw,
+			       struct ieee80211_channel_switch *ch_switch)
 {
 	struct iwl_priv *priv = IWL_MAC80211_GET_DVM(hw);
 	const struct iwl_channel_info *ch_info;
@@ -863,7 +871,7 @@ static void iwlagn_mac_channel_switch(struct ieee80211_hw *hw,
 	if (!iwl_is_associated_ctx(ctx))
 		goto out;
 
-	if (!cfg(priv)->lib->set_channel_switch)
+	if (!priv->lib->set_channel_switch)
 		goto out;
 
 	ch = channel->hw_value;
@@ -892,14 +900,13 @@ static void iwlagn_mac_channel_switch(struct ieee80211_hw *hw,
 	iwl_set_rxon_ht(priv, ht_conf);
 	iwl_set_flags_for_band(priv, ctx, channel->band, ctx->vif);
 
-	iwl_set_rate(priv);
 	/*
 	 * at this point, staging_rxon has the
 	 * configuration for channel switch
 	 */
 	set_bit(STATUS_CHANNEL_SWITCH_PENDING, &priv->status);
 	priv->switch_channel = cpu_to_le16(ch);
-	if (cfg(priv)->lib->set_channel_switch(priv, ch_switch)) {
+	if (priv->lib->set_channel_switch(priv, ch_switch)) {
 		clear_bit(STATUS_CHANNEL_SWITCH_PENDING, &priv->status);
 		priv->switch_channel = 0;
 		ieee80211_chswitch_done(ctx->vif, false);
@@ -910,10 +917,25 @@ out:
 	IWL_DEBUG_MAC80211(priv, "leave\n");
 }
 
-static void iwlagn_configure_filter(struct ieee80211_hw *hw,
-				    unsigned int changed_flags,
-				    unsigned int *total_flags,
-				    u64 multicast)
+void iwl_chswitch_done(struct iwl_priv *priv, bool is_success)
+{
+	/*
+	 * MULTI-FIXME
+	 * See iwlagn_mac_channel_switch.
+	 */
+	struct iwl_rxon_context *ctx = &priv->contexts[IWL_RXON_CTX_BSS];
+
+	if (test_bit(STATUS_EXIT_PENDING, &priv->status))
+		return;
+
+	if (test_and_clear_bit(STATUS_CHANNEL_SWITCH_PENDING, &priv->status))
+		ieee80211_chswitch_done(ctx->vif, is_success);
+}
+
+void iwlagn_configure_filter(struct ieee80211_hw *hw,
+			     unsigned int changed_flags,
+			     unsigned int *total_flags,
+			     u64 multicast)
 {
 	struct iwl_priv *priv = IWL_MAC80211_GET_DVM(hw);
 	__le32 filter_or = 0, filter_nand = 0;
@@ -960,7 +982,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)
+void iwlagn_mac_flush(struct ieee80211_hw *hw, bool drop)
 {
 	struct iwl_priv *priv = IWL_MAC80211_GET_DVM(hw);
 
@@ -988,7 +1010,7 @@ static void iwlagn_mac_flush(struct ieee80211_hw *hw, bool drop)
 		}
 	}
 	IWL_DEBUG_MAC80211(priv, "wait transmit/flush all frames\n");
-	iwl_trans_wait_tx_queue_empty(trans(priv));
+	iwl_trans_wait_tx_queue_empty(priv->trans);
 done:
 	mutex_unlock(&priv->mutex);
 	IWL_DEBUG_MAC80211(priv, "leave\n");
@@ -1003,7 +1025,7 @@ static int iwlagn_mac_remain_on_channel(struct ieee80211_hw *hw,
 	struct iwl_rxon_context *ctx = &priv->contexts[IWL_RXON_CTX_PAN];
 	int err = 0;
 
-	if (!(priv->shrd->valid_contexts & BIT(IWL_RXON_CTX_PAN)))
+	if (!(priv->valid_contexts & BIT(IWL_RXON_CTX_PAN)))
 		return -EOPNOTSUPP;
 
 	if (!(ctx->interface_modes & BIT(NL80211_IFTYPE_P2P_CLIENT)))
@@ -1087,11 +1109,11 @@ static int iwlagn_mac_remain_on_channel(struct ieee80211_hw *hw,
 	return err;
 }
 
-static int iwlagn_mac_cancel_remain_on_channel(struct ieee80211_hw *hw)
+int iwlagn_mac_cancel_remain_on_channel(struct ieee80211_hw *hw)
 {
 	struct iwl_priv *priv = IWL_MAC80211_GET_DVM(hw);
 
-	if (!(priv->shrd->valid_contexts & BIT(IWL_RXON_CTX_PAN)))
+	if (!(priv->valid_contexts & BIT(IWL_RXON_CTX_PAN)))
 		return -EOPNOTSUPP;
 
 	IWL_DEBUG_MAC80211(priv, "enter\n");
@@ -1104,16 +1126,16 @@ static int iwlagn_mac_cancel_remain_on_channel(struct ieee80211_hw *hw)
 	return 0;
 }
 
-static void iwlagn_mac_rssi_callback(struct ieee80211_hw *hw,
-			   enum ieee80211_rssi_event rssi_event)
+void iwlagn_mac_rssi_callback(struct ieee80211_hw *hw,
+			      enum ieee80211_rssi_event rssi_event)
 {
 	struct iwl_priv *priv = IWL_MAC80211_GET_DVM(hw);
 
 	IWL_DEBUG_MAC80211(priv, "enter\n");
 	mutex_lock(&priv->mutex);
 
-	if (cfg(priv)->bt_params &&
-			cfg(priv)->bt_params->advanced_bt_coexist) {
+	if (priv->cfg->bt_params &&
+			priv->cfg->bt_params->advanced_bt_coexist) {
 		if (rssi_event == RSSI_EVENT_LOW)
 			priv->bt_enable_pspoll = true;
 		else if (rssi_event == RSSI_EVENT_HIGH)
@@ -1129,8 +1151,8 @@ static void iwlagn_mac_rssi_callback(struct ieee80211_hw *hw,
 	IWL_DEBUG_MAC80211(priv, "leave\n");
 }
 
-static int iwlagn_mac_set_tim(struct ieee80211_hw *hw,
-			   struct ieee80211_sta *sta, bool set)
+int iwlagn_mac_set_tim(struct ieee80211_hw *hw,
+		       struct ieee80211_sta *sta, bool set)
 {
 	struct iwl_priv *priv = IWL_MAC80211_GET_DVM(hw);
 
@@ -1139,9 +1161,9 @@ static int iwlagn_mac_set_tim(struct ieee80211_hw *hw,
 	return 0;
 }
 
-static int iwlagn_mac_conf_tx(struct ieee80211_hw *hw,
-		    struct ieee80211_vif *vif, u16 queue,
-		    const struct ieee80211_tx_queue_params *params)
+int iwlagn_mac_conf_tx(struct ieee80211_hw *hw,
+		       struct ieee80211_vif *vif, u16 queue,
+		       const struct ieee80211_tx_queue_params *params)
 {
 	struct iwl_priv *priv = IWL_MAC80211_GET_DVM(hw);
 	struct iwl_vif_priv *vif_priv = (void *)vif->drv_priv;
@@ -1183,7 +1205,7 @@ static int iwlagn_mac_conf_tx(struct ieee80211_hw *hw,
 	return 0;
 }
 
-static int iwlagn_mac_tx_last_beacon(struct ieee80211_hw *hw)
+int iwlagn_mac_tx_last_beacon(struct ieee80211_hw *hw)
 {
 	struct iwl_priv *priv = IWL_MAC80211_GET_DVM(hw);
 
@@ -1199,11 +1221,10 @@ static int iwl_set_mode(struct iwl_priv *priv, struct iwl_rxon_context *ctx)
 	return iwlagn_commit_rxon(priv, ctx);
 }
 
-static int iwl_setup_interface(struct iwl_priv *priv,
-			       struct iwl_rxon_context *ctx)
+int iwl_setup_interface(struct iwl_priv *priv, struct iwl_rxon_context *ctx)
 {
 	struct ieee80211_vif *vif = ctx->vif;
-	int err;
+	int err, ac;
 
 	lockdep_assert_held(&priv->mutex);
 
@@ -1223,7 +1244,7 @@ static int iwl_setup_interface(struct iwl_priv *priv,
 		return err;
 	}
 
-	if (cfg(priv)->bt_params && cfg(priv)->bt_params->advanced_bt_coexist &&
+	if (priv->cfg->bt_params && priv->cfg->bt_params->advanced_bt_coexist &&
 	    vif->type == NL80211_IFTYPE_ADHOC) {
 		/*
 		 * pretend to have high BT traffic as long as we
@@ -1233,11 +1254,20 @@ static int iwl_setup_interface(struct iwl_priv *priv,
 		priv->bt_traffic_load = IWL_BT_COEX_TRAFFIC_LOAD_HIGH;
 	}
 
+	/* set up queue mappings */
+	for (ac = 0; ac < IEEE80211_NUM_ACS; ac++)
+		vif->hw_queue[ac] = ctx->ac_to_queue[ac];
+
+	if (vif->type == NL80211_IFTYPE_AP)
+		vif->cab_queue = ctx->mcast_queue;
+	else
+		vif->cab_queue = IEEE80211_INVAL_HW_QUEUE;
+
 	return 0;
 }
 
 static int iwlagn_mac_add_interface(struct ieee80211_hw *hw,
-			     struct ieee80211_vif *vif)
+				    struct ieee80211_vif *vif)
 {
 	struct iwl_priv *priv = IWL_MAC80211_GET_DVM(hw);
 	struct iwl_vif_priv *vif_priv = (void *)vif->drv_priv;
@@ -1311,9 +1341,9 @@ static int iwlagn_mac_add_interface(struct ieee80211_hw *hw,
 	return err;
 }
 
-static void iwl_teardown_interface(struct iwl_priv *priv,
-				   struct ieee80211_vif *vif,
-				   bool mode_change)
+void iwl_teardown_interface(struct iwl_priv *priv,
+			    struct ieee80211_vif *vif,
+			    bool mode_change)
 {
 	struct iwl_rxon_context *ctx = iwl_rxon_ctx_from_vif(vif);
 
@@ -1454,9 +1484,9 @@ static int iwlagn_mac_change_interface(struct ieee80211_hw *hw,
 	return err;
 }
 
-static int iwlagn_mac_hw_scan(struct ieee80211_hw *hw,
-		    struct ieee80211_vif *vif,
-		    struct cfg80211_scan_request *req)
+int iwlagn_mac_hw_scan(struct ieee80211_hw *hw,
+		       struct ieee80211_vif *vif,
+		       struct cfg80211_scan_request *req)
 {
 	struct iwl_priv *priv = IWL_MAC80211_GET_DVM(hw);
 	int ret;
@@ -1511,7 +1541,7 @@ static void iwl_sta_modify_ps_wake(struct iwl_priv *priv, int sta_id)
 	iwl_send_add_sta(priv, &cmd, CMD_ASYNC);
 }
 
-static void iwlagn_mac_sta_notify(struct ieee80211_hw *hw,
+void iwlagn_mac_sta_notify(struct ieee80211_hw *hw,
 			   struct ieee80211_vif *vif,
 			   enum sta_notify_cmd cmd,
 			   struct ieee80211_sta *sta)
diff --git a/drivers/net/wireless/iwlwifi/iwl-modparams.h b/drivers/net/wireless/iwlwifi/iwl-modparams.h
new file mode 100644
index 00000000000..d9a86d6b2bd
--- /dev/null
+++ b/drivers/net/wireless/iwlwifi/iwl-modparams.h
@@ -0,0 +1,126 @@
+/******************************************************************************
+ *
+ * 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 - 2012 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 - 2012 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_modparams_h__
+#define __iwl_modparams_h__
+
+#include <linux/types.h>
+#include <linux/spinlock.h>
+#include <linux/gfp.h>
+#include <net/mac80211.h>
+
+extern struct iwl_mod_params iwlwifi_mod_params;
+
+enum iwl_power_level {
+	IWL_POWER_INDEX_1,
+	IWL_POWER_INDEX_2,
+	IWL_POWER_INDEX_3,
+	IWL_POWER_INDEX_4,
+	IWL_POWER_INDEX_5,
+	IWL_POWER_NUM
+};
+
+#define IWL_DISABLE_HT_ALL	BIT(0)
+#define IWL_DISABLE_HT_TXAGG	BIT(1)
+#define IWL_DISABLE_HT_RXAGG	BIT(2)
+
+/**
+ * struct iwl_mod_params
+ *
+ * Holds the module parameters
+ *
+ * @sw_crypto: using hardware encryption, default = 0
+ * @disable_11n: disable 11n capabilities, default = 0,
+ *	use IWL_DISABLE_HT_* constants
+ * @amsdu_size_8K: enable 8K amsdu size, default = 1
+ * @restart_fw: restart firmware, default = 1
+ * @plcp_check: enable plcp health check, default = true
+ * @wd_disable: enable stuck queue check, default = 0
+ * @bt_coex_active: enable bt coex, default = true
+ * @led_mode: system default, default = 0
+ * @power_save: disable power save, default = false
+ * @power_level: power level, default = 1
+ * @debug_level: levels are IWL_DL_*
+ * @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 plcp_check;
+	int  wd_disable;
+	bool bt_coex_active;
+	int led_mode;
+	bool power_save;
+	int power_level;
+	u32 debug_level;
+	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 88dc4a0f96b..0066b899fe5 100644
--- a/drivers/net/wireless/iwlwifi/iwl-notif-wait.c
+++ b/drivers/net/wireless/iwlwifi/iwl-notif-wait.c
@@ -75,21 +75,45 @@ void iwl_notification_wait_init(struct iwl_notif_wait_data *notif_wait)
 void iwl_notification_wait_notify(struct iwl_notif_wait_data *notif_wait,
 				  struct iwl_rx_packet *pkt)
 {
+	bool triggered = false;
+
 	if (!list_empty(&notif_wait->notif_waits)) {
 		struct iwl_notification_wait *w;
 
 		spin_lock(&notif_wait->notif_wait_lock);
 		list_for_each_entry(w, &notif_wait->notif_waits, list) {
-			if (w->cmd != pkt->hdr.cmd)
+			int i;
+			bool found = false;
+
+			/*
+			 * If it already finished (triggered) or has been
+			 * aborted then don't evaluate it again to avoid races,
+			 * Otherwise the function could be called again even
+			 * though it returned true before
+			 */
+			if (w->triggered || w->aborted)
+				continue;
+
+			for (i = 0; i < w->n_cmds; i++) {
+				if (w->cmds[i] == pkt->hdr.cmd) {
+					found = true;
+					break;
+				}
+			}
+			if (!found)
 				continue;
-			w->triggered = true;
-			if (w->fn)
-				w->fn(notif_wait, pkt, w->fn_data);
+
+			if (!w->fn || w->fn(notif_wait, pkt, w->fn_data)) {
+				w->triggered = true;
+				triggered = true;
+			}
 		}
 		spin_unlock(&notif_wait->notif_wait_lock);
 
-		wake_up_all(&notif_wait->notif_waitq);
 	}
+
+	if (triggered)
+		wake_up_all(&notif_wait->notif_waitq);
 }
 
 void iwl_abort_notification_waits(struct iwl_notif_wait_data *notif_wait)
@@ -109,14 +133,18 @@ void iwl_abort_notification_waits(struct iwl_notif_wait_data *notif_wait)
 void
 iwl_init_notification_wait(struct iwl_notif_wait_data *notif_wait,
 			   struct iwl_notification_wait *wait_entry,
-			   u8 cmd,
-			   void (*fn)(struct iwl_notif_wait_data *notif_wait,
+			   const u8 *cmds, int n_cmds,
+			   bool (*fn)(struct iwl_notif_wait_data *notif_wait,
 				      struct iwl_rx_packet *pkt, void *data),
 			   void *fn_data)
 {
+	if (WARN_ON(n_cmds > MAX_NOTIF_CMDS))
+		n_cmds = MAX_NOTIF_CMDS;
+
 	wait_entry->fn = fn;
 	wait_entry->fn_data = fn_data;
-	wait_entry->cmd = cmd;
+	wait_entry->n_cmds = n_cmds;
+	memcpy(wait_entry->cmds, cmds, n_cmds);
 	wait_entry->triggered = false;
 	wait_entry->aborted = false;
 
diff --git a/drivers/net/wireless/iwlwifi/iwl-notif-wait.h b/drivers/net/wireless/iwlwifi/iwl-notif-wait.h
index 5e8af957aa7..821523100cf 100644
--- a/drivers/net/wireless/iwlwifi/iwl-notif-wait.h
+++ b/drivers/net/wireless/iwlwifi/iwl-notif-wait.h
@@ -72,11 +72,19 @@ struct iwl_notif_wait_data {
 	wait_queue_head_t notif_waitq;
 };
 
+#define MAX_NOTIF_CMDS	5
+
 /**
  * struct iwl_notification_wait - notification wait entry
  * @list: list head for global list
- * @fn: function called with the notification
- * @cmd: command ID
+ * @fn: Function called with the notification. If the function
+ *	returns true, the wait is over, if it returns false then
+ *	the waiter stays blocked. If no function is given, any
+ *	of the listed commands will unblock the waiter.
+ * @cmds: command IDs
+ * @n_cmds: number of command IDs
+ * @triggered: waiter should be woken up
+ * @aborted: wait was aborted
  *
  * This structure is not used directly, to wait for a
  * notification declare it on the stack, and call
@@ -93,11 +101,12 @@ struct iwl_notif_wait_data {
 struct iwl_notification_wait {
 	struct list_head list;
 
-	void (*fn)(struct iwl_notif_wait_data *notif_data,
+	bool (*fn)(struct iwl_notif_wait_data *notif_data,
 		   struct iwl_rx_packet *pkt, void *data);
 	void *fn_data;
 
-	u8 cmd;
+	u8 cmds[MAX_NOTIF_CMDS];
+	u8 n_cmds;
 	bool triggered, aborted;
 };
 
@@ -112,8 +121,8 @@ void iwl_abort_notification_waits(struct iwl_notif_wait_data *notif_data);
 void __acquires(wait_entry)
 iwl_init_notification_wait(struct iwl_notif_wait_data *notif_data,
 			   struct iwl_notification_wait *wait_entry,
-			   u8 cmd,
-			   void (*fn)(struct iwl_notif_wait_data *notif_data,
+			   const u8 *cmds, int n_cmds,
+			   bool (*fn)(struct iwl_notif_wait_data *notif_data,
 				      struct iwl_rx_packet *pkt, void *data),
 			   void *fn_data);
 
diff --git a/drivers/net/wireless/iwlwifi/iwl-op-mode.h b/drivers/net/wireless/iwlwifi/iwl-op-mode.h
index 6ea4163ff56..4ef742b28e0 100644
--- a/drivers/net/wireless/iwlwifi/iwl-op-mode.h
+++ b/drivers/net/wireless/iwlwifi/iwl-op-mode.h
@@ -69,6 +69,7 @@ struct sk_buff;
 struct iwl_device_cmd;
 struct iwl_rx_cmd_buffer;
 struct iwl_fw;
+struct iwl_cfg;
 
 /**
  * DOC: Operational mode - what is it ?
@@ -111,10 +112,10 @@ struct iwl_fw;
  * @rx: Rx notification to the op_mode. rxb is the Rx buffer itself. Cmd is the
  *	HCMD the this Rx responds to.
  *	Must be atomic.
- * @queue_full: notifies that a HW queue is full. Ac is the ac of the queue
+ * @queue_full: notifies that a HW queue is full.
  *	Must be atomic
  * @queue_not_full: notifies that a HW queue is not full any more.
- *	Ac is the ac of the queue. Must be atomic
+ *	Must be atomic
  * @hw_rf_kill:notifies of a change in the HW rf kill switch. True means that
  *	the radio is killed. Must be atomic.
  * @free_skb: allows the transport layer to free skbs that haven't been
@@ -125,20 +126,23 @@ struct iwl_fw;
  * @cmd_queue_full: Called when the command queue gets full. Must be atomic.
  * @nic_config: configure NIC, called before firmware is started.
  *	May sleep
+ * @wimax_active: invoked when WiMax becomes active.  Must be atomic.
  */
 struct iwl_op_mode_ops {
 	struct iwl_op_mode *(*start)(struct iwl_trans *trans,
+				     const struct iwl_cfg *cfg,
 				     const struct iwl_fw *fw);
 	void (*stop)(struct iwl_op_mode *op_mode);
 	int (*rx)(struct iwl_op_mode *op_mode, struct iwl_rx_cmd_buffer *rxb,
 		  struct iwl_device_cmd *cmd);
-	void (*queue_full)(struct iwl_op_mode *op_mode, u8 ac);
-	void (*queue_not_full)(struct iwl_op_mode *op_mode, u8 ac);
+	void (*queue_full)(struct iwl_op_mode *op_mode, int queue);
+	void (*queue_not_full)(struct iwl_op_mode *op_mode, int queue);
 	void (*hw_rf_kill)(struct iwl_op_mode *op_mode, bool state);
 	void (*free_skb)(struct iwl_op_mode *op_mode, struct sk_buff *skb);
 	void (*nic_error)(struct iwl_op_mode *op_mode);
 	void (*cmd_queue_full)(struct iwl_op_mode *op_mode);
 	void (*nic_config)(struct iwl_op_mode *op_mode);
+	void (*wimax_active)(struct iwl_op_mode *op_mode);
 };
 
 /**
@@ -169,15 +173,16 @@ static inline int iwl_op_mode_rx(struct iwl_op_mode *op_mode,
 	return op_mode->ops->rx(op_mode, rxb, cmd);
 }
 
-static inline void iwl_op_mode_queue_full(struct iwl_op_mode *op_mode, u8 ac)
+static inline void iwl_op_mode_queue_full(struct iwl_op_mode *op_mode,
+					  int queue)
 {
-	op_mode->ops->queue_full(op_mode, ac);
+	op_mode->ops->queue_full(op_mode, queue);
 }
 
 static inline void iwl_op_mode_queue_not_full(struct iwl_op_mode *op_mode,
-					      u8 ac)
+					      int queue)
 {
-	op_mode->ops->queue_not_full(op_mode, ac);
+	op_mode->ops->queue_not_full(op_mode, queue);
 }
 
 static inline void iwl_op_mode_hw_rf_kill(struct iwl_op_mode *op_mode,
@@ -208,6 +213,11 @@ static inline void iwl_op_mode_nic_config(struct iwl_op_mode *op_mode)
 	op_mode->ops->nic_config(op_mode);
 }
 
+static inline void iwl_op_mode_wimax_active(struct iwl_op_mode *op_mode)
+{
+	op_mode->ops->wimax_active(op_mode);
+}
+
 /*****************************************************
 * Op mode layers implementations
 ******************************************************/
diff --git a/drivers/net/wireless/iwlwifi/iwl-pci.c b/drivers/net/wireless/iwlwifi/iwl-pci.c
index c5e339ee918..0c8a1c2d884 100644
--- a/drivers/net/wireless/iwlwifi/iwl-pci.c
+++ b/drivers/net/wireless/iwlwifi/iwl-pci.c
@@ -60,17 +60,18 @@
  * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
  *
  *****************************************************************************/
+
+#define pr_fmt(fmt) KBUILD_MODNAME ": " fmt
+
 #include <linux/module.h>
 #include <linux/pci.h>
 #include <linux/pci-aspm.h>
 
-#include "iwl-io.h"
-#include "iwl-shared.h"
 #include "iwl-trans.h"
-#include "iwl-csr.h"
 #include "iwl-cfg.h"
 #include "iwl-drv.h"
 #include "iwl-trans.h"
+#include "iwl-trans-pcie-int.h"
 
 #define IWL_PCI_DEVICE(dev, subdev, cfg) \
 	.vendor = PCI_VENDOR_ID_INTEL,  .device = (dev), \
@@ -261,61 +262,46 @@ MODULE_DEVICE_TABLE(pci, iwl_hw_card_ids);
 /* PCI registers */
 #define PCI_CFG_RETRY_TIMEOUT	0x041
 
+#ifndef CONFIG_IWLWIFI_IDI
+
 static int iwl_pci_probe(struct pci_dev *pdev, const struct pci_device_id *ent)
 {
 	const struct iwl_cfg *cfg = (struct iwl_cfg *)(ent->driver_data);
-	struct iwl_shared *shrd;
 	struct iwl_trans *iwl_trans;
-	int err;
-
-	shrd = kzalloc(sizeof(*iwl_trans->shrd), GFP_KERNEL);
-	if (!shrd) {
-		dev_printk(KERN_ERR, &pdev->dev,
-			   "Couldn't allocate iwl_shared");
-		err = -ENOMEM;
-		goto out_free_bus;
-	}
+	struct iwl_trans_pcie *trans_pcie;
 
-#ifdef CONFIG_IWLWIFI_IDI
-	iwl_trans = iwl_trans_idi_alloc(shrd, pdev, ent);
-#else
-	iwl_trans = iwl_trans_pcie_alloc(shrd, pdev, ent);
-#endif
-	if (iwl_trans == NULL) {
-		err = -ENOMEM;
-		goto out_free_bus;
-	}
+	iwl_trans = iwl_trans_pcie_alloc(pdev, ent, cfg);
+	if (iwl_trans == NULL)
+		return -ENOMEM;
 
-	shrd->trans = iwl_trans;
 	pci_set_drvdata(pdev, iwl_trans);
 
-	err = iwl_drv_start(shrd, iwl_trans, cfg);
-	if (err)
+	trans_pcie = IWL_TRANS_GET_PCIE_TRANS(iwl_trans);
+	trans_pcie->drv = iwl_drv_start(iwl_trans, cfg);
+	if (!trans_pcie->drv)
 		goto out_free_trans;
 
 	return 0;
 
 out_free_trans:
-	iwl_trans_free(iwl_trans);
+	iwl_trans_pcie_free(iwl_trans);
 	pci_set_drvdata(pdev, NULL);
-out_free_bus:
-	kfree(shrd);
-	return err;
+	return -EFAULT;
 }
 
 static void __devexit iwl_pci_remove(struct pci_dev *pdev)
 {
-	struct iwl_trans *iwl_trans = pci_get_drvdata(pdev);
-	struct iwl_shared *shrd = iwl_trans->shrd;
+	struct iwl_trans *trans = pci_get_drvdata(pdev);
+	struct iwl_trans_pcie *trans_pcie = IWL_TRANS_GET_PCIE_TRANS(trans);
 
-	iwl_drv_stop(shrd);
-	iwl_trans_free(shrd->trans);
+	iwl_drv_stop(trans_pcie->drv);
+	iwl_trans_pcie_free(trans);
 
 	pci_set_drvdata(pdev, NULL);
-
-	kfree(shrd);
 }
 
+#endif /* CONFIG_IWLWIFI_IDI */
+
 #ifdef CONFIG_PM_SLEEP
 
 static int iwl_pci_suspend(struct device *device)
@@ -360,6 +346,15 @@ static SIMPLE_DEV_PM_OPS(iwl_dev_pm_ops, iwl_pci_suspend, iwl_pci_resume);
 
 #endif
 
+#ifdef CONFIG_IWLWIFI_IDI
+/*
+ * Defined externally in iwl-idi.c
+ */
+int iwl_pci_probe(struct pci_dev *pdev, const struct pci_device_id *ent);
+void __devexit iwl_pci_remove(struct pci_dev *pdev);
+
+#endif /* CONFIG_IWLWIFI_IDI */
+
 static struct pci_driver iwl_pci_driver = {
 	.name = DRV_NAME,
 	.id_table = iwl_hw_card_ids,
diff --git a/drivers/net/wireless/iwlwifi/iwl-phy-db.c b/drivers/net/wireless/iwlwifi/iwl-phy-db.c
new file mode 100644
index 00000000000..f166955340f
--- /dev/null
+++ b/drivers/net/wireless/iwlwifi/iwl-phy-db.c
@@ -0,0 +1,288 @@
+/******************************************************************************
+ *
+ * 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 - 2012 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 - 2012 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/slab.h>
+#include <linux/string.h>
+
+#include "iwl-debug.h"
+#include "iwl-dev.h"
+
+#include "iwl-phy-db.h"
+
+#define CHANNEL_NUM_SIZE	4	/* num of channels in calib_ch size */
+
+struct iwl_phy_db *iwl_phy_db_init(struct device *dev)
+{
+	struct iwl_phy_db *phy_db = kzalloc(sizeof(struct iwl_phy_db),
+					    GFP_KERNEL);
+
+	if (!phy_db)
+		return phy_db;
+
+	phy_db->dev = dev;
+
+	/* TODO: add default values of the phy db. */
+	return phy_db;
+}
+
+/*
+ * get phy db section: returns a pointer to a phy db section specified by
+ * type and channel group id.
+ */
+static struct iwl_phy_db_entry *
+iwl_phy_db_get_section(struct iwl_phy_db *phy_db,
+		       enum iwl_phy_db_section_type type,
+		       u16 chg_id)
+{
+	if (!phy_db || type < 0 || type >= IWL_PHY_DB_MAX)
+		return NULL;
+
+	switch (type) {
+	case IWL_PHY_DB_CFG:
+		return &phy_db->cfg;
+	case IWL_PHY_DB_CALIB_NCH:
+		return &phy_db->calib_nch;
+	case IWL_PHY_DB_CALIB_CH:
+		return &phy_db->calib_ch;
+	case IWL_PHY_DB_CALIB_CHG_PAPD:
+		if (chg_id < 0 || chg_id >= IWL_NUM_PAPD_CH_GROUPS)
+			return NULL;
+		return &phy_db->calib_ch_group_papd[chg_id];
+	case IWL_PHY_DB_CALIB_CHG_TXP:
+		if (chg_id < 0 || chg_id >= IWL_NUM_TXP_CH_GROUPS)
+			return NULL;
+		return &phy_db->calib_ch_group_txp[chg_id];
+	default:
+		return NULL;
+	}
+	return NULL;
+}
+
+static void iwl_phy_db_free_section(struct iwl_phy_db *phy_db,
+				    enum iwl_phy_db_section_type type,
+				    u16 chg_id)
+{
+	struct iwl_phy_db_entry *entry =
+				iwl_phy_db_get_section(phy_db, type, chg_id);
+	if (!entry)
+		return;
+
+	kfree(entry->data);
+	entry->data = NULL;
+	entry->size = 0;
+}
+
+void iwl_phy_db_free(struct iwl_phy_db *phy_db)
+{
+	int i;
+
+	if (!phy_db)
+		return;
+
+	iwl_phy_db_free_section(phy_db, IWL_PHY_DB_CFG, 0);
+	iwl_phy_db_free_section(phy_db, IWL_PHY_DB_CALIB_NCH, 0);
+	iwl_phy_db_free_section(phy_db, IWL_PHY_DB_CALIB_CH, 0);
+	for (i = 0; i < IWL_NUM_PAPD_CH_GROUPS; i++)
+		iwl_phy_db_free_section(phy_db, IWL_PHY_DB_CALIB_CHG_PAPD, i);
+	for (i = 0; i < IWL_NUM_TXP_CH_GROUPS; i++)
+		iwl_phy_db_free_section(phy_db, IWL_PHY_DB_CALIB_CHG_TXP, i);
+
+	kfree(phy_db);
+}
+
+int iwl_phy_db_set_section(struct iwl_phy_db *phy_db,
+			   enum iwl_phy_db_section_type type, u8 *data,
+			   u16 size, gfp_t alloc_ctx)
+{
+	struct iwl_phy_db_entry *entry;
+	u16 chg_id = 0;
+
+	if (!phy_db)
+		return -EINVAL;
+
+	if (type == IWL_PHY_DB_CALIB_CHG_PAPD ||
+	    type == IWL_PHY_DB_CALIB_CHG_TXP)
+		chg_id = le16_to_cpup((__le16 *)data);
+
+	entry = iwl_phy_db_get_section(phy_db, type, chg_id);
+	if (!entry)
+		return -EINVAL;
+
+	kfree(entry->data);
+	entry->data = kmemdup(data, size, alloc_ctx);
+	if (!entry->data) {
+		entry->size = 0;
+		return -ENOMEM;
+	}
+
+	entry->size = size;
+
+	if (type == IWL_PHY_DB_CALIB_CH) {
+		phy_db->channel_num = le32_to_cpup((__le32 *)data);
+		phy_db->channel_size =
+		      (size - CHANNEL_NUM_SIZE) / phy_db->channel_num;
+	}
+
+	return 0;
+}
+
+static int is_valid_channel(u16 ch_id)
+{
+	if (ch_id <= 14 ||
+	    (36 <= ch_id && ch_id <= 64 && ch_id % 4 == 0) ||
+	    (100 <= ch_id && ch_id <= 140 && ch_id % 4 == 0) ||
+	    (145 <= ch_id && ch_id <= 165 && ch_id % 4 == 1))
+		return 1;
+	return 0;
+}
+
+static u8 ch_id_to_ch_index(u16 ch_id)
+{
+	if (WARN_ON(!is_valid_channel(ch_id)))
+		return 0xff;
+
+	if (ch_id <= 14)
+		return ch_id - 1;
+	if (ch_id <= 64)
+		return (ch_id + 20) / 4;
+	if (ch_id <= 140)
+		return (ch_id - 12) / 4;
+	return (ch_id - 13) / 4;
+}
+
+
+static u16 channel_id_to_papd(u16 ch_id)
+{
+	if (WARN_ON(!is_valid_channel(ch_id)))
+		return 0xff;
+
+	if (1 <= ch_id && ch_id <= 14)
+		return 0;
+	if (36 <= ch_id && ch_id <= 64)
+		return 1;
+	if (100 <= ch_id && ch_id <= 140)
+		return 2;
+	return 3;
+}
+
+static u16 channel_id_to_txp(struct iwl_phy_db *phy_db, u16 ch_id)
+{
+	struct iwl_phy_db_chg_txp *txp_chg;
+	int i;
+	u8 ch_index = ch_id_to_ch_index(ch_id);
+	if (ch_index == 0xff)
+		return 0xff;
+
+	for (i = 0; i < IWL_NUM_TXP_CH_GROUPS; i++) {
+		txp_chg = (void *)phy_db->calib_ch_group_txp[i].data;
+		if (!txp_chg)
+			return 0xff;
+		/*
+		 * Looking for the first channel group that its max channel is
+		 * higher then wanted channel.
+		 */
+		if (le16_to_cpu(txp_chg->max_channel_idx) >= ch_index)
+			return i;
+	}
+	return 0xff;
+}
+
+int iwl_phy_db_get_section_data(struct iwl_phy_db *phy_db,
+				enum iwl_phy_db_section_type type, u8 **data,
+				u16 *size, u16 ch_id)
+{
+	struct iwl_phy_db_entry *entry;
+	u32 channel_num;
+	u32 channel_size;
+	u16 ch_group_id = 0;
+	u16 index;
+
+	if (!phy_db)
+		return -EINVAL;
+
+	/* find wanted channel group */
+	if (type == IWL_PHY_DB_CALIB_CHG_PAPD)
+		ch_group_id = channel_id_to_papd(ch_id);
+	else if (type == IWL_PHY_DB_CALIB_CHG_TXP)
+		ch_group_id = channel_id_to_txp(phy_db, ch_id);
+
+	entry = iwl_phy_db_get_section(phy_db, type, ch_group_id);
+	if (!entry)
+		return -EINVAL;
+
+	if (type == IWL_PHY_DB_CALIB_CH) {
+		index = ch_id_to_ch_index(ch_id);
+		channel_num = phy_db->channel_num;
+		channel_size = phy_db->channel_size;
+		if (index >= channel_num) {
+			IWL_ERR(phy_db, "Wrong channel number %d", ch_id);
+			return -EINVAL;
+		}
+		*data = entry->data + CHANNEL_NUM_SIZE + index * channel_size;
+		*size = channel_size;
+	} else {
+		*data = entry->data;
+		*size = entry->size;
+	}
+	return 0;
+}
diff --git a/drivers/staging/mei/mei.h b/drivers/net/wireless/iwlwifi/iwl-phy-db.h
index bc0d8b69c49..c34c6a9303a 100644
--- a/drivers/staging/mei/mei.h
+++ b/drivers/net/wireless/iwlwifi/iwl-phy-db.h
@@ -1,13 +1,11 @@
 /******************************************************************************
- * Intel Management Engine Interface (Intel MEI) Linux driver
- * Intel MEI Interface Header
  *
  * 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) 2003 - 2012 Intel Corporation. All rights reserved.
+ * Copyright(c) 2007 - 2012 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
@@ -27,13 +25,12 @@
  * in the file called LICENSE.GPL.
  *
  * Contact Information:
- *	Intel Corporation.
- *	linux-mei@linux.intel.com
- *	http://www.intel.com
+ *  Intel Linux Wireless <ilw@linux.intel.com>
+ * Intel Corporation, 5200 N.E. Elam Young Parkway, Hillsboro, OR 97124-6497
  *
  * BSD LICENSE
  *
- * Copyright(c) 2003 - 2012 Intel Corporation. All rights reserved.
+ * Copyright(c) 2005 - 2012 Intel Corporation. All rights reserved.
  * All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
@@ -64,47 +61,69 @@
  *
  *****************************************************************************/
 
-#ifndef _LINUX_MEI_H
-#define _LINUX_MEI_H
+#ifndef __IWL_PHYDB_H__
+#define __IWL_PHYDB_H__
 
-#include <linux/uuid.h>
+#include <linux/types.h>
 
-/*
- * This IOCTL is used to associate the current file descriptor with a
- * FW Client (given by UUID). This opens a communication channel
- * between a host client and a FW client. From this point every read and write
- * will communicate with the associated FW client.
- * Only in close() (file_operation release()) the communication between
- * the clients is disconnected
- *
- * The IOCTL argument is a struct with a union that contains
- * the input parameter and the output parameter for this IOCTL.
- *
- * The input parameter is UUID of the FW Client.
- * The output parameter is the properties of the FW client
- * (FW protocol version and max message size).
- *
- */
-#define IOCTL_MEI_CONNECT_CLIENT \
-	_IOWR('H' , 0x01, struct mei_connect_client_data)
+#define IWL_NUM_PAPD_CH_GROUPS	4
+#define IWL_NUM_TXP_CH_GROUPS	8
 
-/*
- * Intel MEI client information struct
- */
-struct mei_client {
-	__u32 max_msg_length;
-	__u8 protocol_version;
-	__u8 reserved[3];
+struct iwl_phy_db_entry {
+	u16	size;
+	u8	*data;
 };
 
-/*
- * IOCTL Connect Client Data structure
+struct iwl_shared;
+
+/**
+ * struct iwl_phy_db - stores phy configuration and calibration data.
+ *
+ * @cfg: phy configuration.
+ * @calib_nch: non channel specific calibration data.
+ * @calib_ch: channel specific calibration data.
+ * @calib_ch_group_papd: calibration data related to papd channel group.
+ * @calib_ch_group_txp: calibration data related to tx power chanel group.
  */
-struct mei_connect_client_data {
-	union {
-		uuid_le in_client_uuid;
-		struct mei_client out_client_properties;
-	};
+struct iwl_phy_db {
+	struct iwl_phy_db_entry	cfg;
+	struct iwl_phy_db_entry	calib_nch;
+	struct iwl_phy_db_entry	calib_ch;
+	struct iwl_phy_db_entry	calib_ch_group_papd[IWL_NUM_PAPD_CH_GROUPS];
+	struct iwl_phy_db_entry	calib_ch_group_txp[IWL_NUM_TXP_CH_GROUPS];
+
+	u32 channel_num;
+	u32 channel_size;
+
+	/* for an access to the logger */
+	struct device *dev;
 };
 
-#endif /* _LINUX_MEI_H  */
+enum iwl_phy_db_section_type {
+	IWL_PHY_DB_CFG = 1,
+	IWL_PHY_DB_CALIB_NCH,
+	IWL_PHY_DB_CALIB_CH,
+	IWL_PHY_DB_CALIB_CHG_PAPD,
+	IWL_PHY_DB_CALIB_CHG_TXP,
+	IWL_PHY_DB_MAX
+};
+
+/* for parsing of tx power channel group data that comes from the firmware*/
+struct iwl_phy_db_chg_txp {
+	__le32 space;
+	__le16 max_channel_idx;
+} __packed;
+
+struct iwl_phy_db *iwl_phy_db_init(struct device *dev);
+
+void iwl_phy_db_free(struct iwl_phy_db *phy_db);
+
+int iwl_phy_db_set_section(struct iwl_phy_db *phy_db,
+			   enum iwl_phy_db_section_type type, u8 *data,
+			   u16 size, gfp_t alloc_ctx);
+
+int iwl_phy_db_get_section_data(struct iwl_phy_db *phy_db,
+				enum iwl_phy_db_section_type type, u8 **data,
+				u16 *size, u16 ch_id);
+
+#endif /* __IWL_PHYDB_H__ */
diff --git a/drivers/net/wireless/iwlwifi/iwl-power.c b/drivers/net/wireless/iwlwifi/iwl-power.c
index 958d9d09aee..544ddf17f5b 100644
--- a/drivers/net/wireless/iwlwifi/iwl-power.c
+++ b/drivers/net/wireless/iwlwifi/iwl-power.c
@@ -37,13 +37,12 @@
 #include "iwl-eeprom.h"
 #include "iwl-dev.h"
 #include "iwl-agn.h"
-#include "iwl-core.h"
 #include "iwl-io.h"
 #include "iwl-commands.h"
 #include "iwl-debug.h"
 #include "iwl-power.h"
 #include "iwl-trans.h"
-#include "iwl-shared.h"
+#include "iwl-modparams.h"
 
 /*
  * Setting power level allows the card to go to sleep when not busy.
@@ -167,7 +166,7 @@ static void iwl_static_sleep_cmd(struct iwl_priv *priv,
 	u8 skip;
 	u32 slp_itrvl;
 
-	if (cfg(priv)->adv_pm) {
+	if (priv->cfg->adv_pm) {
 		table = apm_range_2;
 		if (period <= IWL_DTIM_RANGE_1_MAX)
 			table = apm_range_1;
@@ -215,13 +214,13 @@ static void iwl_static_sleep_cmd(struct iwl_priv *priv,
 	else
 		cmd->flags &= ~IWL_POWER_SLEEP_OVER_DTIM_MSK;
 
-	if (cfg(priv)->base_params->shadow_reg_enable)
+	if (priv->cfg->base_params->shadow_reg_enable)
 		cmd->flags |= IWL_POWER_SHADOW_REG_ENA;
 	else
 		cmd->flags &= ~IWL_POWER_SHADOW_REG_ENA;
 
 	if (iwl_advanced_bt_coexist(priv)) {
-		if (!cfg(priv)->bt_params->bt_sco_disable)
+		if (!priv->cfg->bt_params->bt_sco_disable)
 			cmd->flags |= IWL_POWER_BT_SCO_ENA;
 		else
 			cmd->flags &= ~IWL_POWER_BT_SCO_ENA;
@@ -254,6 +253,8 @@ static void iwl_static_sleep_cmd(struct iwl_priv *priv,
 
 	IWL_DEBUG_POWER(priv, "numSkipDtim = %u, dtimPeriod = %d\n",
 			skip, period);
+	/* The power level here is 0-4 (used as array index), but user expects
+	to see 1-5 (according to spec). */
 	IWL_DEBUG_POWER(priv, "Sleep command for index %d\n", lvl + 1);
 }
 
@@ -268,61 +269,6 @@ static void iwl_power_sleep_cam_cmd(struct iwl_priv *priv,
 	IWL_DEBUG_POWER(priv, "Sleep command for CAM\n");
 }
 
-static void iwl_power_fill_sleep_cmd(struct iwl_priv *priv,
-				     struct iwl_powertable_cmd *cmd,
-				     int dynps_ms, int wakeup_period)
-{
-	/*
-	 * These are the original power level 3 sleep successions. The
-	 * device may behave better with such succession and was also
-	 * only tested with that. Just like the original sleep commands,
-	 * also adjust the succession here to the wakeup_period below.
-	 * The ranges are the same as for the sleep commands, 0-2, 3-9
-	 * and >10, which is selected based on the DTIM interval for
-	 * the sleep index but here we use the wakeup period since that
-	 * is what we need to do for the latency requirements.
-	 */
-	static const u8 slp_succ_r0[IWL_POWER_VEC_SIZE] = { 2, 2, 2, 2, 2 };
-	static const u8 slp_succ_r1[IWL_POWER_VEC_SIZE] = { 2, 4, 6, 7, 9 };
-	static const u8 slp_succ_r2[IWL_POWER_VEC_SIZE] = { 2, 7, 9, 9, 0xFF };
-	const u8 *slp_succ = slp_succ_r0;
-	int i;
-
-	if (wakeup_period > IWL_DTIM_RANGE_0_MAX)
-		slp_succ = slp_succ_r1;
-	if (wakeup_period > IWL_DTIM_RANGE_1_MAX)
-		slp_succ = slp_succ_r2;
-
-	memset(cmd, 0, sizeof(*cmd));
-
-	cmd->flags = IWL_POWER_DRIVER_ALLOW_SLEEP_MSK |
-		     IWL_POWER_FAST_PD; /* no use seeing frames for others */
-
-	if (priv->power_data.bus_pm)
-		cmd->flags |= IWL_POWER_PCI_PM_MSK;
-
-	if (cfg(priv)->base_params->shadow_reg_enable)
-		cmd->flags |= IWL_POWER_SHADOW_REG_ENA;
-	else
-		cmd->flags &= ~IWL_POWER_SHADOW_REG_ENA;
-
-	if (iwl_advanced_bt_coexist(priv)) {
-		if (!cfg(priv)->bt_params->bt_sco_disable)
-			cmd->flags |= IWL_POWER_BT_SCO_ENA;
-		else
-			cmd->flags &= ~IWL_POWER_BT_SCO_ENA;
-	}
-
-	cmd->rx_data_timeout = cpu_to_le32(1000 * dynps_ms);
-	cmd->tx_data_timeout = cpu_to_le32(1000 * dynps_ms);
-
-	for (i = 0; i < IWL_POWER_VEC_SIZE; i++)
-		cmd->sleep_interval[i] =
-			cpu_to_le32(min_t(int, slp_succ[i], wakeup_period));
-
-	IWL_DEBUG_POWER(priv, "Automatic sleep command\n");
-}
-
 static int iwl_set_power(struct iwl_priv *priv, struct iwl_powertable_cmd *cmd)
 {
 	IWL_DEBUG_POWER(priv, "Sending power/sleep command\n");
@@ -350,7 +296,7 @@ static void iwl_power_build_cmd(struct iwl_priv *priv,
 
 	if (priv->wowlan)
 		iwl_static_sleep_cmd(priv, cmd, IWL_POWER_INDEX_5, dtimper);
-	else if (!cfg(priv)->base_params->no_idle_support &&
+	else if (!priv->cfg->base_params->no_idle_support &&
 		 priv->hw->conf.flags & IEEE80211_CONF_IDLE)
 		iwl_static_sleep_cmd(priv, cmd, IWL_POWER_INDEX_5, 20);
 	else if (iwl_tt_is_low_power_state(priv)) {
@@ -363,18 +309,17 @@ static void iwl_power_build_cmd(struct iwl_priv *priv,
 		iwl_static_sleep_cmd(priv, cmd,
 				     priv->power_data.debug_sleep_level_override,
 				     dtimper);
-	else if (iwlagn_mod_params.no_sleep_autoadjust) {
-		if (iwlagn_mod_params.power_level > IWL_POWER_INDEX_1 &&
-		    iwlagn_mod_params.power_level <= IWL_POWER_INDEX_5)
+	else {
+		/* Note that the user parameter is 1-5 (according to spec),
+		but we pass 0-4 because it acts as an array index. */
+		if (iwlwifi_mod_params.power_level > IWL_POWER_INDEX_1 &&
+		    iwlwifi_mod_params.power_level <= IWL_POWER_NUM)
 			iwl_static_sleep_cmd(priv, cmd,
-				iwlagn_mod_params.power_level, dtimper);
+				iwlwifi_mod_params.power_level - 1, dtimper);
 		else
 			iwl_static_sleep_cmd(priv, cmd,
 				IWL_POWER_INDEX_1, dtimper);
-	} else
-		iwl_power_fill_sleep_cmd(priv, cmd,
-					 priv->hw->conf.dynamic_ps_timeout,
-					 priv->hw->conf.max_sleep_period);
+	}
 }
 
 int iwl_power_set_mode(struct iwl_priv *priv, struct iwl_powertable_cmd *cmd,
@@ -403,12 +348,12 @@ int iwl_power_set_mode(struct iwl_priv *priv, struct iwl_powertable_cmd *cmd,
 	}
 
 	if (cmd->flags & IWL_POWER_DRIVER_ALLOW_SLEEP_MSK)
-		set_bit(STATUS_POWER_PMI, &priv->shrd->status);
+		iwl_dvm_set_pmi(priv, true);
 
 	ret = iwl_set_power(priv, cmd);
 	if (!ret) {
 		if (!(cmd->flags & IWL_POWER_DRIVER_ALLOW_SLEEP_MSK))
-			clear_bit(STATUS_POWER_PMI, &priv->shrd->status);
+			iwl_dvm_set_pmi(priv, false);
 
 		if (update_chains)
 			iwl_update_chain_flags(priv);
@@ -436,7 +381,7 @@ int iwl_power_update_mode(struct iwl_priv *priv, bool force)
 /* initialize to default */
 void iwl_power_initialize(struct iwl_priv *priv)
 {
-	priv->power_data.bus_pm = trans(priv)->pm_support;
+	priv->power_data.bus_pm = priv->trans->pm_support;
 
 	priv->power_data.debug_sleep_level_override = -1;
 
diff --git a/drivers/net/wireless/iwlwifi/iwl-power.h b/drivers/net/wireless/iwlwifi/iwl-power.h
index 07a19fce5fd..21afc92efac 100644
--- a/drivers/net/wireless/iwlwifi/iwl-power.h
+++ b/drivers/net/wireless/iwlwifi/iwl-power.h
@@ -30,15 +30,6 @@
 
 #include "iwl-commands.h"
 
-enum iwl_power_level {
-	IWL_POWER_INDEX_1,
-	IWL_POWER_INDEX_2,
-	IWL_POWER_INDEX_3,
-	IWL_POWER_INDEX_4,
-	IWL_POWER_INDEX_5,
-	IWL_POWER_NUM
-};
-
 struct iwl_power_mgr {
 	struct iwl_powertable_cmd sleep_cmd;
 	struct iwl_powertable_cmd sleep_cmd_next;
diff --git a/drivers/net/wireless/iwlwifi/iwl-scan.c b/drivers/net/wireless/iwlwifi/iwl-scan.c
index 902efe4bc89..031d8e21f82 100644
--- a/drivers/net/wireless/iwlwifi/iwl-scan.c
+++ b/drivers/net/wireless/iwlwifi/iwl-scan.c
@@ -32,7 +32,6 @@
 
 #include "iwl-eeprom.h"
 #include "iwl-dev.h"
-#include "iwl-core.h"
 #include "iwl-io.h"
 #include "iwl-agn.h"
 #include "iwl-trans.h"
@@ -53,6 +52,7 @@
 #define IWL_PASSIVE_DWELL_TIME_52   (10)
 #define IWL_PASSIVE_DWELL_BASE      (100)
 #define IWL_CHANNEL_TUNE_TIME       5
+#define MAX_SCAN_CHANNEL	    50
 
 static int iwl_send_scan_abort(struct iwl_priv *priv)
 {
@@ -69,7 +69,7 @@ static int iwl_send_scan_abort(struct iwl_priv *priv)
 	if (!test_bit(STATUS_READY, &priv->status) ||
 	    !test_bit(STATUS_GEO_CONFIGURED, &priv->status) ||
 	    !test_bit(STATUS_SCAN_HW, &priv->status) ||
-	    test_bit(STATUS_FW_ERROR, &priv->shrd->status))
+	    test_bit(STATUS_FW_ERROR, &priv->status))
 		return -EIO;
 
 	ret = iwl_dvm_send_cmd(priv, &cmd);
@@ -451,6 +451,46 @@ static u16 iwl_get_passive_dwell_time(struct iwl_priv *priv,
 	return iwl_limit_dwell(priv, passive);
 }
 
+/* Return valid, unused, channel for a passive scan to reset the RF */
+static u8 iwl_get_single_channel_number(struct iwl_priv *priv,
+				 enum ieee80211_band band)
+{
+	const struct iwl_channel_info *ch_info;
+	int i;
+	u8 channel = 0;
+	u8 min, max;
+	struct iwl_rxon_context *ctx;
+
+	if (band == IEEE80211_BAND_5GHZ) {
+		min = 14;
+		max = priv->channel_count;
+	} else {
+		min = 0;
+		max = 14;
+	}
+
+	for (i = min; i < max; i++) {
+		bool busy = false;
+
+		for_each_context(priv, ctx) {
+			busy = priv->channel_info[i].channel ==
+				le16_to_cpu(ctx->staging.channel);
+			if (busy)
+				break;
+		}
+
+		if (busy)
+			continue;
+
+		channel = priv->channel_info[i].channel;
+		ch_info = iwl_get_channel_info(priv, band, channel);
+		if (is_channel_valid(ch_info))
+			break;
+	}
+
+	return channel;
+}
+
 static int iwl_get_single_channel_for_scan(struct iwl_priv *priv,
 					   struct ieee80211_vif *vif,
 					   enum ieee80211_band band,
@@ -577,7 +617,8 @@ static int iwl_get_channels_for_scan(struct iwl_priv *priv,
  */
 
 static u16 iwl_fill_probe_req(struct ieee80211_mgmt *frame, const u8 *ta,
-			      const u8 *ies, int ie_len, int left)
+			      const u8 *ies, int ie_len, const u8 *ssid,
+			      u8 ssid_len, int left)
 {
 	int len = 0;
 	u8 *pos = NULL;
@@ -599,14 +640,18 @@ static u16 iwl_fill_probe_req(struct ieee80211_mgmt *frame, const u8 *ta,
 	/* ...next IE... */
 	pos = &frame->u.probe_req.variable[0];
 
-	/* fill in our indirect SSID IE */
-	left -= 2;
+	/* fill in our SSID IE */
+	left -= ssid_len + 2;
 	if (left < 0)
 		return 0;
 	*pos++ = WLAN_EID_SSID;
-	*pos++ = 0;
+	*pos++ = ssid_len;
+	if (ssid && ssid_len) {
+		memcpy(pos, ssid, ssid_len);
+		pos += ssid_len;
+	}
 
-	len += 2;
+	len += ssid_len + 2;
 
 	if (WARN_ON(left < ie_len))
 		return len;
@@ -633,13 +678,22 @@ static int iwlagn_request_scan(struct iwl_priv *priv, struct ieee80211_vif *vif)
 	u16 rx_chain = 0;
 	enum ieee80211_band band;
 	u8 n_probes = 0;
-	u8 rx_ant = hw_params(priv).valid_rx_ant;
+	u8 rx_ant = priv->hw_params.valid_rx_ant;
 	u8 rate;
 	bool is_active = false;
 	int  chan_mod;
 	u8 active_chains;
-	u8 scan_tx_antennas = hw_params(priv).valid_tx_ant;
+	u8 scan_tx_antennas = priv->hw_params.valid_tx_ant;
 	int ret;
+	int scan_cmd_size = sizeof(struct iwl_scan_cmd) +
+			    MAX_SCAN_CHANNEL * sizeof(struct iwl_scan_channel) +
+			    priv->fw->ucode_capa.max_probe_length;
+	const u8 *ssid = NULL;
+	u8 ssid_len = 0;
+
+	if (WARN_ON_ONCE(priv->scan_request &&
+			 priv->scan_request->n_channels > MAX_SCAN_CHANNEL))
+		return -EINVAL;
 
 	lockdep_assert_held(&priv->mutex);
 
@@ -647,8 +701,7 @@ static int iwlagn_request_scan(struct iwl_priv *priv, struct ieee80211_vif *vif)
 		ctx = iwl_rxon_ctx_from_vif(vif);
 
 	if (!priv->scan_cmd) {
-		priv->scan_cmd = kmalloc(sizeof(struct iwl_scan_cmd) +
-					 IWL_MAX_SCAN_SIZE, GFP_KERNEL);
+		priv->scan_cmd = kmalloc(scan_cmd_size, GFP_KERNEL);
 		if (!priv->scan_cmd) {
 			IWL_DEBUG_SCAN(priv,
 				       "fail to allocate memory for scan\n");
@@ -656,7 +709,7 @@ static int iwlagn_request_scan(struct iwl_priv *priv, struct ieee80211_vif *vif)
 		}
 	}
 	scan = priv->scan_cmd;
-	memset(scan, 0, sizeof(struct iwl_scan_cmd) + IWL_MAX_SCAN_SIZE);
+	memset(scan, 0, scan_cmd_size);
 
 	scan->quiet_plcp_th = IWL_PLCP_QUIET_THRESH;
 	scan->quiet_time = IWL_ACTIVE_QUIET_TIME;
@@ -707,10 +760,18 @@ static int iwlagn_request_scan(struct iwl_priv *priv, struct ieee80211_vif *vif)
 		if (priv->scan_request->n_ssids) {
 			int i, p = 0;
 			IWL_DEBUG_SCAN(priv, "Kicking off active scan\n");
-			for (i = 0; i < priv->scan_request->n_ssids; i++) {
-				/* always does wildcard anyway */
-				if (!priv->scan_request->ssids[i].ssid_len)
-					continue;
+			/*
+			 * The highest priority SSID is inserted to the
+			 * probe request template.
+			 */
+			ssid_len = priv->scan_request->ssids[0].ssid_len;
+			ssid = priv->scan_request->ssids[0].ssid;
+
+			/*
+			 * Invert the order of ssids, the firmware will invert
+			 * it back.
+			 */
+			for (i = priv->scan_request->n_ssids - 1; i >= 1; i--) {
 				scan->direct_scan[p].id = WLAN_EID_SSID;
 				scan->direct_scan[p].len =
 					priv->scan_request->ssids[i].ssid_len;
@@ -751,8 +812,8 @@ static int iwlagn_request_scan(struct iwl_priv *priv, struct ieee80211_vif *vif)
 		 * Internal scans are passive, so we can indiscriminately set
 		 * the BT ignore flag on 2.4 GHz since it applies to TX only.
 		 */
-		if (cfg(priv)->bt_params &&
-		    cfg(priv)->bt_params->advanced_bt_coexist)
+		if (priv->cfg->bt_params &&
+		    priv->cfg->bt_params->advanced_bt_coexist)
 			scan->tx_cmd.tx_flags |= TX_CMD_FLG_IGNORE_BT;
 		break;
 	case IEEE80211_BAND_5GHZ:
@@ -793,12 +854,9 @@ static int iwlagn_request_scan(struct iwl_priv *priv, struct ieee80211_vif *vif)
 
 	band = priv->scan_band;
 
-	if (cfg(priv)->scan_rx_antennas[band])
-		rx_ant = cfg(priv)->scan_rx_antennas[band];
-
 	if (band == IEEE80211_BAND_2GHZ &&
-	    cfg(priv)->bt_params &&
-	    cfg(priv)->bt_params->advanced_bt_coexist) {
+	    priv->cfg->bt_params &&
+	    priv->cfg->bt_params->advanced_bt_coexist) {
 		/* transmit 2.4 GHz probes only on first antenna */
 		scan_tx_antennas = first_antenna(scan_tx_antennas);
 	}
@@ -809,8 +867,12 @@ static int iwlagn_request_scan(struct iwl_priv *priv, struct ieee80211_vif *vif)
 	rate_flags |= iwl_ant_idx_to_flags(priv->scan_tx_ant[band]);
 	scan->tx_cmd.rate_n_flags = iwl_hw_set_rate_n_flags(rate, rate_flags);
 
-	/* In power save mode use one chain, otherwise use all chains */
-	if (test_bit(STATUS_POWER_PMI, &priv->shrd->status)) {
+	/*
+	 * In power save mode while associated use one chain,
+	 * otherwise use all chains
+	 */
+	if (test_bit(STATUS_POWER_PMI, &priv->status) &&
+	    !(priv->hw->conf.flags & IEEE80211_CONF_IDLE)) {
 		/* rx_ant has been set to all valid chains previously */
 		active_chains = rx_ant &
 				((u8)(priv->chain_noise_data.active_chains));
@@ -822,8 +884,8 @@ static int iwlagn_request_scan(struct iwl_priv *priv, struct ieee80211_vif *vif)
 
 		rx_ant = first_antenna(active_chains);
 	}
-	if (cfg(priv)->bt_params &&
-	    cfg(priv)->bt_params->advanced_bt_coexist &&
+	if (priv->cfg->bt_params &&
+	    priv->cfg->bt_params->advanced_bt_coexist &&
 	    priv->bt_full_concurrent) {
 		/* operated as 1x1 in full concurrency mode */
 		rx_ant = first_antenna(rx_ant);
@@ -831,7 +893,7 @@ static int iwlagn_request_scan(struct iwl_priv *priv, struct ieee80211_vif *vif)
 
 	/* MIMO is not used here, but value is required */
 	rx_chain |=
-		hw_params(priv).valid_rx_ant << RXON_RX_CHAIN_VALID_POS;
+		priv->hw_params.valid_rx_ant << RXON_RX_CHAIN_VALID_POS;
 	rx_chain |= rx_ant << RXON_RX_CHAIN_FORCE_MIMO_SEL_POS;
 	rx_chain |= rx_ant << RXON_RX_CHAIN_FORCE_SEL_POS;
 	rx_chain |= 0x1 << RXON_RX_CHAIN_DRIVER_FORCE_POS;
@@ -843,7 +905,8 @@ static int iwlagn_request_scan(struct iwl_priv *priv, struct ieee80211_vif *vif)
 					vif->addr,
 					priv->scan_request->ie,
 					priv->scan_request->ie_len,
-					IWL_MAX_SCAN_SIZE - sizeof(*scan));
+					ssid, ssid_len,
+					scan_cmd_size - sizeof(*scan));
 		break;
 	case IWL_SCAN_RADIO_RESET:
 	case IWL_SCAN_ROC:
@@ -851,7 +914,8 @@ static int iwlagn_request_scan(struct iwl_priv *priv, struct ieee80211_vif *vif)
 		cmd_len = iwl_fill_probe_req(
 					(struct ieee80211_mgmt *)scan->data,
 					iwl_bcast_addr, NULL, 0,
-					IWL_MAX_SCAN_SIZE - sizeof(*scan));
+					NULL, 0,
+					scan_cmd_size - sizeof(*scan));
 		break;
 	default:
 		BUG();
@@ -944,7 +1008,7 @@ static int iwlagn_request_scan(struct iwl_priv *priv, struct ieee80211_vif *vif)
 
 void iwl_init_scan_params(struct iwl_priv *priv)
 {
-	u8 ant_idx = fls(hw_params(priv).valid_tx_ant) - 1;
+	u8 ant_idx = fls(priv->hw_params.valid_tx_ant) - 1;
 	if (!priv->scan_tx_ant[IEEE80211_BAND_5GHZ])
 		priv->scan_tx_ant[IEEE80211_BAND_5GHZ] = ant_idx;
 	if (!priv->scan_tx_ant[IEEE80211_BAND_2GHZ])
diff --git a/drivers/net/wireless/iwlwifi/iwl-testmode.c b/drivers/net/wireless/iwlwifi/iwl-testmode.c
index 76f7f925143..060aac3e22f 100644
--- a/drivers/net/wireless/iwlwifi/iwl-testmode.c
+++ b/drivers/net/wireless/iwlwifi/iwl-testmode.c
@@ -71,7 +71,6 @@
 #include <net/netlink.h>
 
 #include "iwl-dev.h"
-#include "iwl-core.h"
 #include "iwl-debug.h"
 #include "iwl-io.h"
 #include "iwl-agn.h"
@@ -184,9 +183,10 @@ static void iwl_testmode_ucode_rx_pkt(struct iwl_priv *priv,
 			 "Run out of memory for messages to user space ?\n");
 		return;
 	}
-	NLA_PUT_U32(skb, IWL_TM_ATTR_COMMAND, IWL_TM_CMD_DEV2APP_UCODE_RX_PKT);
-	/* the length doesn't include len_n_flags field, so add it manually */
-	NLA_PUT(skb, IWL_TM_ATTR_UCODE_RX_PKT, length + sizeof(__le32), data);
+	if (nla_put_u32(skb, IWL_TM_ATTR_COMMAND, IWL_TM_CMD_DEV2APP_UCODE_RX_PKT) ||
+	    /* the length doesn't include len_n_flags field, so add it manually */
+	    nla_put(skb, IWL_TM_ATTR_UCODE_RX_PKT, length + sizeof(__le32), data))
+		goto nla_put_failure;
 	cfg80211_testmode_event(skb, GFP_ATOMIC);
 	return;
 
@@ -218,7 +218,7 @@ static void iwl_trace_cleanup(struct iwl_priv *priv)
 	if (priv->testmode_trace.trace_enabled) {
 		if (priv->testmode_trace.cpu_addr &&
 		    priv->testmode_trace.dma_addr)
-			dma_free_coherent(trans(priv)->dev,
+			dma_free_coherent(priv->trans->dev,
 					priv->testmode_trace.total_size,
 					priv->testmode_trace.cpu_addr,
 					priv->testmode_trace.dma_addr);
@@ -314,8 +314,9 @@ static int iwl_testmode_ucode(struct ieee80211_hw *hw, struct nlattr **tb)
 	memcpy(reply_buf, &(pkt->hdr), reply_len);
 	iwl_free_resp(&cmd);
 
-	NLA_PUT_U32(skb, IWL_TM_ATTR_COMMAND, IWL_TM_CMD_DEV2APP_UCODE_RX_PKT);
-	NLA_PUT(skb, IWL_TM_ATTR_UCODE_RX_PKT, reply_len, reply_buf);
+	if (nla_put_u32(skb, IWL_TM_ATTR_COMMAND, IWL_TM_CMD_DEV2APP_UCODE_RX_PKT) ||
+	    nla_put(skb, IWL_TM_ATTR_UCODE_RX_PKT, reply_len, reply_buf))
+		goto nla_put_failure;
 	return cfg80211_testmode_reply(skb);
 
 nla_put_failure:
@@ -371,7 +372,7 @@ static int iwl_testmode_reg(struct ieee80211_hw *hw, struct nlattr **tb)
 
 	switch (cmd) {
 	case IWL_TM_CMD_APP2DEV_DIRECT_REG_READ32:
-		val32 = iwl_read_direct32(trans(priv), ofs);
+		val32 = iwl_read_direct32(priv->trans, ofs);
 		IWL_INFO(priv, "32bit value to read 0x%x\n", val32);
 
 		skb = cfg80211_testmode_alloc_reply_skb(hw->wiphy, 20);
@@ -379,7 +380,8 @@ static int iwl_testmode_reg(struct ieee80211_hw *hw, struct nlattr **tb)
 			IWL_ERR(priv, "Memory allocation fail\n");
 			return -ENOMEM;
 		}
-		NLA_PUT_U32(skb, IWL_TM_ATTR_REG_VALUE32, val32);
+		if (nla_put_u32(skb, IWL_TM_ATTR_REG_VALUE32, val32))
+			goto nla_put_failure;
 		status = cfg80211_testmode_reply(skb);
 		if (status < 0)
 			IWL_ERR(priv, "Error sending msg : %d\n", status);
@@ -391,7 +393,7 @@ static int iwl_testmode_reg(struct ieee80211_hw *hw, struct nlattr **tb)
 		} else {
 			val32 = nla_get_u32(tb[IWL_TM_ATTR_REG_VALUE32]);
 			IWL_INFO(priv, "32bit value to write 0x%x\n", val32);
-			iwl_write_direct32(trans(priv), ofs, val32);
+			iwl_write_direct32(priv->trans, ofs, val32);
 		}
 		break;
 	case IWL_TM_CMD_APP2DEV_DIRECT_REG_WRITE8:
@@ -401,7 +403,7 @@ static int iwl_testmode_reg(struct ieee80211_hw *hw, struct nlattr **tb)
 		} else {
 			val8 = nla_get_u8(tb[IWL_TM_ATTR_REG_VALUE8]);
 			IWL_INFO(priv, "8bit value to write 0x%x\n", val8);
-			iwl_write8(trans(priv), ofs, val8);
+			iwl_write8(priv->trans, ofs, val8);
 		}
 		break;
 	default:
@@ -420,10 +422,13 @@ nla_put_failure:
 static int iwl_testmode_cfg_init_calib(struct iwl_priv *priv)
 {
 	struct iwl_notification_wait calib_wait;
+	static const u8 calib_complete[] = {
+		CALIBRATION_COMPLETE_NOTIFICATION
+	};
 	int ret;
 
 	iwl_init_notification_wait(&priv->notif_wait, &calib_wait,
-				   CALIBRATION_COMPLETE_NOTIFICATION,
+				   calib_complete, ARRAY_SIZE(calib_complete),
 				   NULL, NULL);
 	ret = iwl_init_alive_start(priv);
 	if (ret) {
@@ -461,7 +466,7 @@ cfg_init_calib_error:
 static int iwl_testmode_driver(struct ieee80211_hw *hw, struct nlattr **tb)
 {
 	struct iwl_priv *priv = IWL_MAC80211_GET_DVM(hw);
-	struct iwl_trans *trans = trans(priv);
+	struct iwl_trans *trans = priv->trans;
 	struct sk_buff *skb;
 	unsigned char *rsp_data_ptr = NULL;
 	int status = 0, rsp_data_len = 0;
@@ -470,18 +475,19 @@ static int iwl_testmode_driver(struct ieee80211_hw *hw, struct nlattr **tb)
 
 	switch (nla_get_u32(tb[IWL_TM_ATTR_COMMAND])) {
 	case IWL_TM_CMD_APP2DEV_GET_DEVICENAME:
-		rsp_data_ptr = (unsigned char *)cfg(priv)->name;
-		rsp_data_len = strlen(cfg(priv)->name);
+		rsp_data_ptr = (unsigned char *)priv->cfg->name;
+		rsp_data_len = strlen(priv->cfg->name);
 		skb = cfg80211_testmode_alloc_reply_skb(hw->wiphy,
 							rsp_data_len + 20);
 		if (!skb) {
 			IWL_ERR(priv, "Memory allocation fail\n");
 			return -ENOMEM;
 		}
-		NLA_PUT_U32(skb, IWL_TM_ATTR_COMMAND,
-			    IWL_TM_CMD_DEV2APP_SYNC_RSP);
-		NLA_PUT(skb, IWL_TM_ATTR_SYNC_RSP,
-			rsp_data_len, rsp_data_ptr);
+		if (nla_put_u32(skb, IWL_TM_ATTR_COMMAND,
+				IWL_TM_CMD_DEV2APP_SYNC_RSP) ||
+		    nla_put(skb, IWL_TM_ATTR_SYNC_RSP,
+			    rsp_data_len, rsp_data_ptr))
+			goto nla_put_failure;
 		status = cfg80211_testmode_reply(skb);
 		if (status < 0)
 			IWL_ERR(priv, "Error sending msg : %d\n", status);
@@ -529,18 +535,19 @@ static int iwl_testmode_driver(struct ieee80211_hw *hw, struct nlattr **tb)
 		break;
 
 	case IWL_TM_CMD_APP2DEV_GET_EEPROM:
-		if (priv->shrd->eeprom) {
+		if (priv->eeprom) {
 			skb = cfg80211_testmode_alloc_reply_skb(hw->wiphy,
-				cfg(priv)->base_params->eeprom_size + 20);
+				priv->cfg->base_params->eeprom_size + 20);
 			if (!skb) {
 				IWL_ERR(priv, "Memory allocation fail\n");
 				return -ENOMEM;
 			}
-			NLA_PUT_U32(skb, IWL_TM_ATTR_COMMAND,
-				IWL_TM_CMD_DEV2APP_EEPROM_RSP);
-			NLA_PUT(skb, IWL_TM_ATTR_EEPROM,
-				cfg(priv)->base_params->eeprom_size,
-				priv->shrd->eeprom);
+			if (nla_put_u32(skb, IWL_TM_ATTR_COMMAND,
+					IWL_TM_CMD_DEV2APP_EEPROM_RSP) ||
+			    nla_put(skb, IWL_TM_ATTR_EEPROM,
+				    priv->cfg->base_params->eeprom_size,
+				    priv->eeprom))
+				goto nla_put_failure;
 			status = cfg80211_testmode_reply(skb);
 			if (status < 0)
 				IWL_ERR(priv, "Error sending msg : %d\n",
@@ -566,15 +573,16 @@ static int iwl_testmode_driver(struct ieee80211_hw *hw, struct nlattr **tb)
 			IWL_ERR(priv, "Memory allocation fail\n");
 			return -ENOMEM;
 		}
-		NLA_PUT_U32(skb, IWL_TM_ATTR_FW_VERSION,
-			    priv->fw->ucode_ver);
+		if (nla_put_u32(skb, IWL_TM_ATTR_FW_VERSION,
+				priv->fw->ucode_ver))
+			goto nla_put_failure;
 		status = cfg80211_testmode_reply(skb);
 		if (status < 0)
 			IWL_ERR(priv, "Error sending msg : %d\n", status);
 		break;
 
 	case IWL_TM_CMD_APP2DEV_GET_DEVICE_ID:
-		devid = trans(priv)->hw_id;
+		devid = priv->trans->hw_id;
 		IWL_INFO(priv, "hw version: 0x%x\n", devid);
 
 		skb = cfg80211_testmode_alloc_reply_skb(hw->wiphy, 20);
@@ -582,7 +590,8 @@ static int iwl_testmode_driver(struct ieee80211_hw *hw, struct nlattr **tb)
 			IWL_ERR(priv, "Memory allocation fail\n");
 			return -ENOMEM;
 		}
-		NLA_PUT_U32(skb, IWL_TM_ATTR_DEVICE_ID, devid);
+		if (nla_put_u32(skb, IWL_TM_ATTR_DEVICE_ID, devid))
+			goto nla_put_failure;
 		status = cfg80211_testmode_reply(skb);
 		if (status < 0)
 			IWL_ERR(priv, "Error sending msg : %d\n", status);
@@ -598,13 +607,14 @@ static int iwl_testmode_driver(struct ieee80211_hw *hw, struct nlattr **tb)
 			IWL_ERR(priv, "No uCode has not been loaded\n");
 			return -EINVAL;
 		} else {
-			img = &priv->fw->img[priv->shrd->ucode_type];
+			img = &priv->fw->img[priv->cur_ucode];
 			inst_size = img->sec[IWL_UCODE_SECTION_INST].len;
 			data_size = img->sec[IWL_UCODE_SECTION_DATA].len;
 		}
-		NLA_PUT_U32(skb, IWL_TM_ATTR_FW_TYPE, priv->shrd->ucode_type);
-		NLA_PUT_U32(skb, IWL_TM_ATTR_FW_INST_SIZE, inst_size);
-		NLA_PUT_U32(skb, IWL_TM_ATTR_FW_DATA_SIZE, data_size);
+		if (nla_put_u32(skb, IWL_TM_ATTR_FW_TYPE, priv->cur_ucode) ||
+		    nla_put_u32(skb, IWL_TM_ATTR_FW_INST_SIZE, inst_size) ||
+		    nla_put_u32(skb, IWL_TM_ATTR_FW_DATA_SIZE, data_size))
+			goto nla_put_failure;
 		status = cfg80211_testmode_reply(skb);
 		if (status < 0)
 			IWL_ERR(priv, "Error sending msg : %d\n", status);
@@ -639,7 +649,7 @@ static int iwl_testmode_trace(struct ieee80211_hw *hw, struct nlattr **tb)
 	struct iwl_priv *priv = IWL_MAC80211_GET_DVM(hw);
 	struct sk_buff *skb;
 	int status = 0;
-	struct device *dev = trans(priv)->dev;
+	struct device *dev = priv->trans->dev;
 
 	switch (nla_get_u32(tb[IWL_TM_ATTR_COMMAND])) {
 	case IWL_TM_CMD_APP2DEV_BEGIN_TRACE:
@@ -678,9 +688,10 @@ static int iwl_testmode_trace(struct ieee80211_hw *hw, struct nlattr **tb)
 			iwl_trace_cleanup(priv);
 			return -ENOMEM;
 		}
-		NLA_PUT(skb, IWL_TM_ATTR_TRACE_ADDR,
-			sizeof(priv->testmode_trace.dma_addr),
-			(u64 *)&priv->testmode_trace.dma_addr);
+		if (nla_put(skb, IWL_TM_ATTR_TRACE_ADDR,
+			    sizeof(priv->testmode_trace.dma_addr),
+			    (u64 *)&priv->testmode_trace.dma_addr))
+			goto nla_put_failure;
 		status = cfg80211_testmode_reply(skb);
 		if (status < 0) {
 			IWL_ERR(priv, "Error sending msg : %d\n", status);
@@ -725,9 +736,10 @@ static int iwl_testmode_trace_dump(struct ieee80211_hw *hw,
 			length = priv->testmode_trace.buff_size %
 				DUMP_CHUNK_SIZE;
 
-		NLA_PUT(skb, IWL_TM_ATTR_TRACE_DUMP, length,
-			priv->testmode_trace.trace_addr +
-			(DUMP_CHUNK_SIZE * idx));
+		if (nla_put(skb, IWL_TM_ATTR_TRACE_DUMP, length,
+			    priv->testmode_trace.trace_addr +
+			    (DUMP_CHUNK_SIZE * idx)))
+			goto nla_put_failure;
 		idx++;
 		cb->args[4] = idx;
 		return 0;
@@ -779,7 +791,7 @@ static int iwl_testmode_ownership(struct ieee80211_hw *hw, struct nlattr **tb)
 
 static int iwl_testmode_indirect_read(struct iwl_priv *priv, u32 addr, u32 size)
 {
-	struct iwl_trans *trans = trans(priv);
+	struct iwl_trans *trans = priv->trans;
 	unsigned long flags;
 	int i;
 
@@ -819,7 +831,7 @@ static int iwl_testmode_indirect_read(struct iwl_priv *priv, u32 addr, u32 size)
 static int iwl_testmode_indirect_write(struct iwl_priv *priv, u32 addr,
 	u32 size, unsigned char *buf)
 {
-	struct iwl_trans *trans = trans(priv);
+	struct iwl_trans *trans = priv->trans;
 	u32 val, i;
 	unsigned long flags;
 
@@ -922,9 +934,10 @@ static int iwl_testmode_buffer_dump(struct ieee80211_hw *hw,
 			length = priv->testmode_mem.buff_size %
 				DUMP_CHUNK_SIZE;
 
-		NLA_PUT(skb, IWL_TM_ATTR_BUFFER_DUMP, length,
-			priv->testmode_mem.buff_addr +
-			(DUMP_CHUNK_SIZE * idx));
+		if (nla_put(skb, IWL_TM_ATTR_BUFFER_DUMP, length,
+			    priv->testmode_mem.buff_addr +
+			    (DUMP_CHUNK_SIZE * idx)))
+			goto nla_put_failure;
 		idx++;
 		cb->args[4] = idx;
 		return 0;
diff --git a/drivers/net/wireless/iwlwifi/iwl-trans-pcie-int.h b/drivers/net/wireless/iwlwifi/iwl-trans-pcie-int.h
index 1c2fe87bd7e..6213c05a4b5 100644
--- a/drivers/net/wireless/iwlwifi/iwl-trans-pcie-int.h
+++ b/drivers/net/wireless/iwlwifi/iwl-trans-pcie-int.h
@@ -34,17 +34,15 @@
 #include <linux/skbuff.h>
 #include <linux/wait.h>
 #include <linux/pci.h>
+#include <linux/timer.h>
 
 #include "iwl-fh.h"
 #include "iwl-csr.h"
-#include "iwl-shared.h"
 #include "iwl-trans.h"
 #include "iwl-debug.h"
 #include "iwl-io.h"
 #include "iwl-op-mode.h"
 
-struct iwl_tx_queue;
-struct iwl_queue;
 struct iwl_host_cmd;
 
 /*This file includes the declaration that are internal to the
@@ -136,21 +134,14 @@ static inline int iwl_queue_dec_wrap(int index, int n_bd)
 	return --index & (n_bd - 1);
 }
 
-/*
- * This queue number is required for proper operation
- * because the ucode will stop/start the scheduler as
- * required.
- */
-#define IWL_IPAN_MCAST_QUEUE		8
-
 struct iwl_cmd_meta {
 	/* only for SYNC commands, iff the reply skb is wanted */
 	struct iwl_host_cmd *source;
 
-	u32 flags;
-
 	DEFINE_DMA_UNMAP_ADDR(mapping);
 	DEFINE_DMA_UNMAP_LEN(len);
+
+	u32 flags;
 };
 
 /*
@@ -188,72 +179,66 @@ struct iwl_queue {
 				* space less than this */
 };
 
+#define TFD_TX_CMD_SLOTS 256
+#define TFD_CMD_SLOTS 32
+
+struct iwl_pcie_tx_queue_entry {
+	struct iwl_device_cmd *cmd;
+	struct sk_buff *skb;
+	struct iwl_cmd_meta meta;
+};
+
 /**
  * struct iwl_tx_queue - Tx Queue for DMA
  * @q: generic Rx/Tx queue descriptor
- * @bd: base of circular buffer of TFDs
- * @cmd: array of command/TX buffer pointers
- * @meta: array of meta data for each command/tx buffer
- * @dma_addr_cmd: physical address of cmd/tx buffer array
- * @txb: array of per-TFD driver data
- * lock: queue lock
- * @time_stamp: time (in jiffies) of last read_ptr change
+ * @tfds: transmit frame descriptors (DMA memory)
+ * @entries: transmit entries (driver state)
+ * @lock: queue lock
+ * @stuck_timer: timer that fires if queue gets stuck
+ * @trans_pcie: pointer back to transport (for timer)
  * @need_update: indicates need to update read/write index
- * @sched_retry: indicates queue is high-throughput aggregation (HT AGG) enabled
- * @sta_id: valid if sched_retry is set
- * @tid: valid if sched_retry is set
+ * @active: stores if queue is active
  *
  * A Tx queue consists of circular buffer of BDs (a.k.a. TFDs, transmit frame
  * descriptors) and required locking structures.
  */
-#define TFD_TX_CMD_SLOTS 256
-#define TFD_CMD_SLOTS 32
-
 struct iwl_tx_queue {
 	struct iwl_queue q;
 	struct iwl_tfd *tfds;
-	struct iwl_device_cmd **cmd;
-	struct iwl_cmd_meta *meta;
-	struct sk_buff **skbs;
+	struct iwl_pcie_tx_queue_entry *entries;
 	spinlock_t lock;
-	unsigned long time_stamp;
+	struct timer_list stuck_timer;
+	struct iwl_trans_pcie *trans_pcie;
 	u8 need_update;
-	u8 sched_retry;
 	u8 active;
-	u8 swq_id;
-
-	u16 sta_id;
-	u16 tid;
 };
 
 /**
  * struct iwl_trans_pcie - PCIe transport specific data
  * @rxq: all the RX queue data
  * @rx_replenish: work that will be called when buffers need to be allocated
+ * @drv - pointer to iwl_drv
  * @trans: pointer to the generic transport area
  * @irq - the irq number for the device
  * @irq_requested: true when the irq has been requested
  * @scd_base_addr: scheduler sram base address in SRAM
  * @scd_bc_tbls: pointer to the byte count table of the scheduler
  * @kw: keep warm address
- * @ac_to_fifo: to what fifo is a specifc AC mapped ?
- * @ac_to_queue: to what tx queue  is a specifc AC mapped ?
- * @mcast_queue:
- * @txq: Tx DMA processing queues
- * @txq_ctx_active_msk: what queue is active
- * queue_stopped: tracks what queue is stopped
- * queue_stop_count: tracks what SW queue is stopped
  * @pci_dev: basic pci-network driver stuff
  * @hw_base: pci hardware address support
  * @ucode_write_complete: indicates that the ucode has been copied.
  * @ucode_write_waitq: wait queue for uCode load
  * @status - transport specific status flags
  * @cmd_queue - command queue number
+ * @rx_buf_size_8k: 8 kB RX buffer size
+ * @rx_page_order: page order for receive buffer size
+ * @wd_timeout: queue watchdog timeout (jiffies)
  */
 struct iwl_trans_pcie {
 	struct iwl_rx_queue rxq;
 	struct work_struct rx_replenish;
 	struct iwl_trans *trans;
+	struct iwl_drv *drv;
 
 	/* INT ICT Table */
 	__le32 *ict_tbl;
@@ -272,16 +257,9 @@ struct iwl_trans_pcie {
 	struct iwl_dma_ptr scd_bc_tbls;
 	struct iwl_dma_ptr kw;
 
-	const u8 *ac_to_fifo[NUM_IWL_RXON_CTX];
-	const u8 *ac_to_queue[NUM_IWL_RXON_CTX];
-	u8 mcast_queue[NUM_IWL_RXON_CTX];
-	u8 agg_txq[IWLAGN_STATION_COUNT][IWL_MAX_TID_COUNT];
-
 	struct iwl_tx_queue *txq;
-	unsigned long txq_ctx_active_msk;
-#define IWL_MAX_HW_QUEUES	32
+	unsigned long queue_used[BITS_TO_LONGS(IWL_MAX_HW_QUEUES)];
 	unsigned long queue_stopped[BITS_TO_LONGS(IWL_MAX_HW_QUEUES)];
-	atomic_t queue_stop_count[4];
 
 	/* PCI bus related data */
 	struct pci_dev *pci_dev;
@@ -293,11 +271,41 @@ struct iwl_trans_pcie {
 	u8 cmd_queue;
 	u8 n_no_reclaim_cmds;
 	u8 no_reclaim_cmds[MAX_NO_RECLAIM_CMDS];
+	u8 setup_q_to_fifo[IWL_MAX_HW_QUEUES];
+	u8 n_q_to_fifo;
+
+	bool rx_buf_size_8k;
+	u32 rx_page_order;
+
+	const char **command_names;
+
+	/* queue watchdog */
+	unsigned long wd_timeout;
 };
 
+/*****************************************************
+* DRIVER STATUS FUNCTIONS
+******************************************************/
+#define STATUS_HCMD_ACTIVE	0
+#define STATUS_DEVICE_ENABLED	1
+#define STATUS_TPOWER_PMI	2
+#define STATUS_INT_ENABLED	3
+
 #define IWL_TRANS_GET_PCIE_TRANS(_iwl_trans) \
 	((struct iwl_trans_pcie *) ((_iwl_trans)->trans_specific))
 
+static inline struct iwl_trans *
+iwl_trans_pcie_get_trans(struct iwl_trans_pcie *trans_pcie)
+{
+	return container_of((void *)trans_pcie, struct iwl_trans,
+			    trans_specific);
+}
+
+struct iwl_trans *iwl_trans_pcie_alloc(struct pci_dev *pdev,
+				       const struct pci_device_id *ent,
+				       const struct iwl_cfg *cfg);
+void iwl_trans_pcie_free(struct iwl_trans *trans);
+
 /*****************************************************
 * RX
 ******************************************************/
@@ -331,15 +339,12 @@ void iwl_tx_cmd_complete(struct iwl_trans *trans,
 void iwl_trans_txq_update_byte_cnt_tbl(struct iwl_trans *trans,
 					   struct iwl_tx_queue *txq,
 					   u16 byte_cnt);
-int iwl_trans_pcie_tx_agg_disable(struct iwl_trans *trans,
-				  int sta_id, int tid);
+void iwl_trans_pcie_tx_agg_disable(struct iwl_trans *trans, int queue);
 void iwl_trans_set_wr_ptrs(struct iwl_trans *trans, int txq_id, u32 index);
 void iwl_trans_tx_queue_set_status(struct iwl_trans *trans,
-			     struct iwl_tx_queue *txq,
-			     int tx_fifo_id, int scd_retry);
-int iwl_trans_pcie_tx_agg_alloc(struct iwl_trans *trans, int sta_id, int tid);
-void iwl_trans_pcie_tx_agg_setup(struct iwl_trans *trans,
-				 enum iwl_rxon_context_id ctx,
+				   struct iwl_tx_queue *txq,
+				   int tx_fifo_id, bool active);
+void iwl_trans_pcie_tx_agg_setup(struct iwl_trans *trans, int queue, int fifo,
 				 int sta_id, int tid, int frame_limit, u16 ssn);
 void iwlagn_txq_free_tfd(struct iwl_trans *trans, struct iwl_tx_queue *txq,
 	int index, enum dma_data_direction dma_dir);
@@ -350,8 +355,6 @@ int iwl_queue_space(const struct iwl_queue *q);
 /*****************************************************
 * Error handling
 ******************************************************/
-int iwl_dump_nic_event_log(struct iwl_trans *trans, bool full_log,
-			    char **buf, bool display);
 int iwl_dump_fh(struct iwl_trans *trans, char **buf, bool display);
 void iwl_dump_csr(struct iwl_trans *trans);
 
@@ -388,91 +391,28 @@ static inline void iwl_enable_rfkill_int(struct iwl_trans *trans)
 	iwl_write32(trans, CSR_INT_MASK, CSR_INT_BIT_RF_KILL);
 }
 
-/*
- * we have 8 bits used like this:
- *
- * 7 6 5 4 3 2 1 0
- * | | | | | | | |
- * | | | | | | +-+-------- AC queue (0-3)
- * | | | | | |
- * | +-+-+-+-+------------ HW queue ID
- * |
- * +---------------------- unused
- */
-static inline void iwl_set_swq_id(struct iwl_tx_queue *txq, u8 ac, u8 hwq)
-{
-	BUG_ON(ac > 3);   /* only have 2 bits */
-	BUG_ON(hwq > 31); /* only use 5 bits */
-
-	txq->swq_id = (hwq << 2) | ac;
-}
-
-static inline u8 iwl_get_queue_ac(struct iwl_tx_queue *txq)
-{
-	return txq->swq_id & 0x3;
-}
-
 static inline void iwl_wake_queue(struct iwl_trans *trans,
 				  struct iwl_tx_queue *txq)
 {
-	u8 queue = txq->swq_id;
-	u8 ac = queue & 3;
-	u8 hwq = (queue >> 2) & 0x1f;
-	struct iwl_trans_pcie *trans_pcie =
-		IWL_TRANS_GET_PCIE_TRANS(trans);
-
-	if (test_and_clear_bit(hwq, trans_pcie->queue_stopped)) {
-		if (atomic_dec_return(&trans_pcie->queue_stop_count[ac]) <= 0) {
-			iwl_op_mode_queue_not_full(trans->op_mode, ac);
-			IWL_DEBUG_TX_QUEUES(trans, "Wake hwq %d ac %d",
-					    hwq, ac);
-		} else {
-			IWL_DEBUG_TX_QUEUES(trans,
-				"Don't wake hwq %d ac %d stop count %d",
-				hwq, ac,
-				atomic_read(&trans_pcie->queue_stop_count[ac]));
-		}
+	struct iwl_trans_pcie *trans_pcie = IWL_TRANS_GET_PCIE_TRANS(trans);
+
+	if (test_and_clear_bit(txq->q.id, trans_pcie->queue_stopped)) {
+		IWL_DEBUG_TX_QUEUES(trans, "Wake hwq %d\n", txq->q.id);
+		iwl_op_mode_queue_not_full(trans->op_mode, txq->q.id);
 	}
 }
 
 static inline void iwl_stop_queue(struct iwl_trans *trans,
 				  struct iwl_tx_queue *txq)
 {
-	u8 queue = txq->swq_id;
-	u8 ac = queue & 3;
-	u8 hwq = (queue >> 2) & 0x1f;
-	struct iwl_trans_pcie *trans_pcie =
-		IWL_TRANS_GET_PCIE_TRANS(trans);
-
-	if (!test_and_set_bit(hwq, trans_pcie->queue_stopped)) {
-		if (atomic_inc_return(&trans_pcie->queue_stop_count[ac]) > 0) {
-			iwl_op_mode_queue_full(trans->op_mode, ac);
-			IWL_DEBUG_TX_QUEUES(trans,
-				"Stop hwq %d ac %d stop count %d",
-				hwq, ac,
-				atomic_read(&trans_pcie->queue_stop_count[ac]));
-		} else {
-			IWL_DEBUG_TX_QUEUES(trans,
-				"Don't stop hwq %d ac %d stop count %d",
-				hwq, ac,
-				atomic_read(&trans_pcie->queue_stop_count[ac]));
-		}
-	} else {
-		IWL_DEBUG_TX_QUEUES(trans, "stop hwq %d, but it is stopped",
-				    hwq);
-	}
-}
-
-static inline void iwl_txq_ctx_activate(struct iwl_trans_pcie *trans_pcie,
-					int txq_id)
-{
-	set_bit(txq_id, &trans_pcie->txq_ctx_active_msk);
-}
+	struct iwl_trans_pcie *trans_pcie = IWL_TRANS_GET_PCIE_TRANS(trans);
 
-static inline void iwl_txq_ctx_deactivate(struct iwl_trans_pcie *trans_pcie,
-					  int txq_id)
-{
-	clear_bit(txq_id, &trans_pcie->txq_ctx_active_msk);
+	if (!test_and_set_bit(txq->q.id, trans_pcie->queue_stopped)) {
+		iwl_op_mode_queue_full(trans->op_mode, txq->q.id);
+		IWL_DEBUG_TX_QUEUES(trans, "Stop hwq %d\n", txq->q.id);
+	} else
+		IWL_DEBUG_TX_QUEUES(trans, "hwq %d already stopped\n",
+				    txq->q.id);
 }
 
 static inline int iwl_queue_used(const struct iwl_queue *q, int i)
@@ -487,19 +427,18 @@ static inline u8 get_cmd_index(struct iwl_queue *q, u32 index)
 	return index & (q->n_window - 1);
 }
 
-#define IWL_TX_FIFO_BK		0	/* shared */
-#define IWL_TX_FIFO_BE		1
-#define IWL_TX_FIFO_VI		2	/* shared */
-#define IWL_TX_FIFO_VO		3
-#define IWL_TX_FIFO_BK_IPAN	IWL_TX_FIFO_BK
-#define IWL_TX_FIFO_BE_IPAN	4
-#define IWL_TX_FIFO_VI_IPAN	IWL_TX_FIFO_VI
-#define IWL_TX_FIFO_VO_IPAN	5
-/* re-uses the VO FIFO, uCode will properly flush/schedule */
-#define IWL_TX_FIFO_AUX		5
-#define IWL_TX_FIFO_UNUSED	-1
-
-/* AUX (TX during scan dwell) queue */
-#define IWL_AUX_QUEUE		10
+static inline const char *
+trans_pcie_get_cmd_string(struct iwl_trans_pcie *trans_pcie, u8 cmd)
+{
+	if (!trans_pcie->command_names || !trans_pcie->command_names[cmd])
+		return "UNKNOWN";
+	return trans_pcie->command_names[cmd];
+}
+
+static inline bool iwl_is_rfkill_set(struct iwl_trans *trans)
+{
+	return !(iwl_read32(trans, CSR_GP_CNTRL) &
+		CSR_GP_CNTRL_REG_FLAG_HW_RF_KILL_SW);
+}
 
 #endif /* __iwl_trans_int_pcie_h__ */
diff --git a/drivers/net/wireless/iwlwifi/iwl-trans-pcie-rx.c b/drivers/net/wireless/iwlwifi/iwl-trans-pcie-rx.c
index 8b1a7988e17..08517d3c80b 100644
--- a/drivers/net/wireless/iwlwifi/iwl-trans-pcie-rx.c
+++ b/drivers/net/wireless/iwlwifi/iwl-trans-pcie-rx.c
@@ -140,14 +140,17 @@ void iwl_rx_queue_update_write_ptr(struct iwl_trans *trans,
 	if (q->need_update == 0)
 		goto exit_unlock;
 
-	if (cfg(trans)->base_params->shadow_reg_enable) {
+	if (trans->cfg->base_params->shadow_reg_enable) {
 		/* shadow register enabled */
 		/* Device expects a multiple of 8 */
 		q->write_actual = (q->write & ~0x7);
 		iwl_write32(trans, FH_RSCSR_CHNL0_WPTR, q->write_actual);
 	} else {
+		struct iwl_trans_pcie *trans_pcie =
+			IWL_TRANS_GET_PCIE_TRANS(trans);
+
 		/* If power-saving is in use, make sure device is awake */
-		if (test_bit(STATUS_POWER_PMI, &trans->shrd->status)) {
+		if (test_bit(STATUS_TPOWER_PMI, &trans_pcie->status)) {
 			reg = iwl_read32(trans, CSR_UCODE_DRV_GP1);
 
 			if (reg & CSR_UCODE_DRV_GP1_BIT_MAC_SLEEP) {
@@ -271,17 +274,17 @@ static void iwlagn_rx_allocate(struct iwl_trans *trans, gfp_t priority)
 		if (rxq->free_count > RX_LOW_WATERMARK)
 			gfp_mask |= __GFP_NOWARN;
 
-		if (hw_params(trans).rx_page_order > 0)
+		if (trans_pcie->rx_page_order > 0)
 			gfp_mask |= __GFP_COMP;
 
 		/* Alloc a new receive buffer */
 		page = alloc_pages(gfp_mask,
-				  hw_params(trans).rx_page_order);
+				  trans_pcie->rx_page_order);
 		if (!page) {
 			if (net_ratelimit())
 				IWL_DEBUG_INFO(trans, "alloc_pages failed, "
 					   "order: %d\n",
-					   hw_params(trans).rx_page_order);
+					   trans_pcie->rx_page_order);
 
 			if ((rxq->free_count <= RX_LOW_WATERMARK) &&
 			    net_ratelimit())
@@ -300,7 +303,7 @@ static void iwlagn_rx_allocate(struct iwl_trans *trans, gfp_t priority)
 
 		if (list_empty(&rxq->rx_used)) {
 			spin_unlock_irqrestore(&rxq->lock, flags);
-			__free_pages(page, hw_params(trans).rx_page_order);
+			__free_pages(page, trans_pcie->rx_page_order);
 			return;
 		}
 		element = rxq->rx_used.next;
@@ -313,7 +316,7 @@ static void iwlagn_rx_allocate(struct iwl_trans *trans, gfp_t priority)
 		rxb->page = page;
 		/* Get physical address of the RB */
 		rxb->page_dma = dma_map_page(trans->dev, page, 0,
-				PAGE_SIZE << hw_params(trans).rx_page_order,
+				PAGE_SIZE << trans_pcie->rx_page_order,
 				DMA_FROM_DEVICE);
 		/* dma address must be no more than 36 bits */
 		BUG_ON(rxb->page_dma & ~DMA_BIT_MASK(36));
@@ -362,83 +365,98 @@ static void iwl_rx_handle_rxbuf(struct iwl_trans *trans,
 	struct iwl_trans_pcie *trans_pcie = IWL_TRANS_GET_PCIE_TRANS(trans);
 	struct iwl_rx_queue *rxq = &trans_pcie->rxq;
 	struct iwl_tx_queue *txq = &trans_pcie->txq[trans_pcie->cmd_queue];
-	struct iwl_device_cmd *cmd;
 	unsigned long flags;
-	int len, err;
-	u16 sequence;
-	struct iwl_rx_cmd_buffer rxcb;
-	struct iwl_rx_packet *pkt;
-	bool reclaim;
-	int index, cmd_index;
+	bool page_stolen = false;
+	int max_len = PAGE_SIZE << trans_pcie->rx_page_order;
+	u32 offset = 0;
 
 	if (WARN_ON(!rxb))
 		return;
 
-	dma_unmap_page(trans->dev, rxb->page_dma,
-		       PAGE_SIZE << hw_params(trans).rx_page_order,
-		       DMA_FROM_DEVICE);
-
-	rxcb._page = rxb->page;
-	pkt = rxb_addr(&rxcb);
+	dma_unmap_page(trans->dev, rxb->page_dma, max_len, DMA_FROM_DEVICE);
 
-	IWL_DEBUG_RX(trans, "%s, 0x%02x\n",
-		     get_cmd_string(pkt->hdr.cmd), pkt->hdr.cmd);
+	while (offset + sizeof(u32) + sizeof(struct iwl_cmd_header) < max_len) {
+		struct iwl_rx_packet *pkt;
+		struct iwl_device_cmd *cmd;
+		u16 sequence;
+		bool reclaim;
+		int index, cmd_index, err, len;
+		struct iwl_rx_cmd_buffer rxcb = {
+			._offset = offset,
+			._page = rxb->page,
+			._page_stolen = false,
+			.truesize = max_len,
+		};
 
+		pkt = rxb_addr(&rxcb);
 
-	len = le32_to_cpu(pkt->len_n_flags) & FH_RSCSR_FRAME_SIZE_MSK;
-	len += sizeof(u32); /* account for status word */
-	trace_iwlwifi_dev_rx(trans->dev, pkt, len);
-
-	/* Reclaim a command buffer only if this packet is a response
-	 *   to a (driver-originated) command.
-	 * If the packet (e.g. Rx frame) originated from uCode,
-	 *   there is no command buffer to reclaim.
-	 * Ucode should set SEQ_RX_FRAME bit if ucode-originated,
-	 *   but apparently a few don't get set; catch them here. */
-	reclaim = !(pkt->hdr.sequence & SEQ_RX_FRAME);
-	if (reclaim) {
-		int i;
+		if (pkt->len_n_flags == cpu_to_le32(FH_RSCSR_FRAME_INVALID))
+			break;
 
-		for (i = 0; i < trans_pcie->n_no_reclaim_cmds; i++) {
-			if (trans_pcie->no_reclaim_cmds[i] == pkt->hdr.cmd) {
-				reclaim = false;
-				break;
+		IWL_DEBUG_RX(trans, "cmd at offset %d: %s (0x%.2x)\n",
+			rxcb._offset,
+			trans_pcie_get_cmd_string(trans_pcie, pkt->hdr.cmd),
+			pkt->hdr.cmd);
+
+		len = le32_to_cpu(pkt->len_n_flags) & FH_RSCSR_FRAME_SIZE_MSK;
+		len += sizeof(u32); /* account for status word */
+		trace_iwlwifi_dev_rx(trans->dev, pkt, len);
+
+		/* Reclaim a command buffer only if this packet is a response
+		 *   to a (driver-originated) command.
+		 * If the packet (e.g. Rx frame) originated from uCode,
+		 *   there is no command buffer to reclaim.
+		 * Ucode should set SEQ_RX_FRAME bit if ucode-originated,
+		 *   but apparently a few don't get set; catch them here. */
+		reclaim = !(pkt->hdr.sequence & SEQ_RX_FRAME);
+		if (reclaim) {
+			int i;
+
+			for (i = 0; i < trans_pcie->n_no_reclaim_cmds; i++) {
+				if (trans_pcie->no_reclaim_cmds[i] ==
+							pkt->hdr.cmd) {
+					reclaim = false;
+					break;
+				}
 			}
 		}
-	}
 
-	sequence = le16_to_cpu(pkt->hdr.sequence);
-	index = SEQ_TO_INDEX(sequence);
-	cmd_index = get_cmd_index(&txq->q, index);
+		sequence = le16_to_cpu(pkt->hdr.sequence);
+		index = SEQ_TO_INDEX(sequence);
+		cmd_index = get_cmd_index(&txq->q, index);
 
-	if (reclaim)
-		cmd = txq->cmd[cmd_index];
-	else
-		cmd = NULL;
+		if (reclaim)
+			cmd = txq->entries[cmd_index].cmd;
+		else
+			cmd = NULL;
 
-	err = iwl_op_mode_rx(trans->op_mode, &rxcb, cmd);
+		err = iwl_op_mode_rx(trans->op_mode, &rxcb, cmd);
 
-	/*
-	 * XXX: After here, we should always check rxcb._page
-	 * against NULL before touching it or its virtual
-	 * memory (pkt). Because some rx_handler might have
-	 * already taken or freed the pages.
-	 */
+		/*
+		 * After here, we should always check rxcb._page_stolen,
+		 * if it is true then one of the handlers took the page.
+		 */
 
-	if (reclaim) {
-		/* Invoke any callbacks, transfer the buffer to caller,
-		 * and fire off the (possibly) blocking
-		 * iwl_trans_send_cmd()
-		 * as we reclaim the driver command queue */
-		if (rxcb._page)
-			iwl_tx_cmd_complete(trans, &rxcb, err);
-		else
-			IWL_WARN(trans, "Claim null rxb?\n");
+		if (reclaim) {
+			/* Invoke any callbacks, transfer the buffer to caller,
+			 * and fire off the (possibly) blocking
+			 * iwl_trans_send_cmd()
+			 * as we reclaim the driver command queue */
+			if (!rxcb._page_stolen)
+				iwl_tx_cmd_complete(trans, &rxcb, err);
+			else
+				IWL_WARN(trans, "Claim null rxb?\n");
+		}
+
+		page_stolen |= rxcb._page_stolen;
+		offset += ALIGN(len, FH_RSCSR_FRAME_ALIGN);
 	}
 
-	/* page was stolen from us */
-	if (rxcb._page == NULL)
+	/* page was stolen from us -- free our reference */
+	if (page_stolen) {
+		__free_pages(rxb->page, trans_pcie->rx_page_order);
 		rxb->page = NULL;
+	}
 
 	/* Reuse the page if possible. For notification packets and
 	 * SKBs that fail to Rx correctly, add them back into the
@@ -447,7 +465,7 @@ static void iwl_rx_handle_rxbuf(struct iwl_trans *trans,
 	if (rxb->page != NULL) {
 		rxb->page_dma =
 			dma_map_page(trans->dev, rxb->page, 0,
-				PAGE_SIZE << hw_params(trans).rx_page_order,
+				PAGE_SIZE << trans_pcie->rx_page_order,
 				DMA_FROM_DEVICE);
 		list_add_tail(&rxb->list, &rxq->rx_free);
 		rxq->free_count++;
@@ -520,412 +538,32 @@ static void iwl_rx_handle(struct iwl_trans *trans)
 		iwlagn_rx_queue_restock(trans);
 }
 
-static const char * const desc_lookup_text[] = {
-	"OK",
-	"FAIL",
-	"BAD_PARAM",
-	"BAD_CHECKSUM",
-	"NMI_INTERRUPT_WDG",
-	"SYSASSERT",
-	"FATAL_ERROR",
-	"BAD_COMMAND",
-	"HW_ERROR_TUNE_LOCK",
-	"HW_ERROR_TEMPERATURE",
-	"ILLEGAL_CHAN_FREQ",
-	"VCC_NOT_STABLE",
-	"FH_ERROR",
-	"NMI_INTERRUPT_HOST",
-	"NMI_INTERRUPT_ACTION_PT",
-	"NMI_INTERRUPT_UNKNOWN",
-	"UCODE_VERSION_MISMATCH",
-	"HW_ERROR_ABS_LOCK",
-	"HW_ERROR_CAL_LOCK_FAIL",
-	"NMI_INTERRUPT_INST_ACTION_PT",
-	"NMI_INTERRUPT_DATA_ACTION_PT",
-	"NMI_TRM_HW_ER",
-	"NMI_INTERRUPT_TRM",
-	"NMI_INTERRUPT_BREAK_POINT",
-	"DEBUG_0",
-	"DEBUG_1",
-	"DEBUG_2",
-	"DEBUG_3",
-};
-
-static struct { char *name; u8 num; } advanced_lookup[] = {
-	{ "NMI_INTERRUPT_WDG", 0x34 },
-	{ "SYSASSERT", 0x35 },
-	{ "UCODE_VERSION_MISMATCH", 0x37 },
-	{ "BAD_COMMAND", 0x38 },
-	{ "NMI_INTERRUPT_DATA_ACTION_PT", 0x3C },
-	{ "FATAL_ERROR", 0x3D },
-	{ "NMI_TRM_HW_ERR", 0x46 },
-	{ "NMI_INTERRUPT_TRM", 0x4C },
-	{ "NMI_INTERRUPT_BREAK_POINT", 0x54 },
-	{ "NMI_INTERRUPT_WDG_RXF_FULL", 0x5C },
-	{ "NMI_INTERRUPT_WDG_NO_RBD_RXF_FULL", 0x64 },
-	{ "NMI_INTERRUPT_HOST", 0x66 },
-	{ "NMI_INTERRUPT_ACTION_PT", 0x7C },
-	{ "NMI_INTERRUPT_UNKNOWN", 0x84 },
-	{ "NMI_INTERRUPT_INST_ACTION_PT", 0x86 },
-	{ "ADVANCED_SYSASSERT", 0 },
-};
-
-static const char *desc_lookup(u32 num)
-{
-	int i;
-	int max = ARRAY_SIZE(desc_lookup_text);
-
-	if (num < max)
-		return desc_lookup_text[num];
-
-	max = ARRAY_SIZE(advanced_lookup) - 1;
-	for (i = 0; i < max; i++) {
-		if (advanced_lookup[i].num == num)
-			break;
-	}
-	return advanced_lookup[i].name;
-}
-
-#define ERROR_START_OFFSET  (1 * sizeof(u32))
-#define ERROR_ELEM_SIZE     (7 * sizeof(u32))
-
-static void iwl_dump_nic_error_log(struct iwl_trans *trans)
-{
-	u32 base;
-	struct iwl_error_event_table table;
-	struct iwl_trans_pcie *trans_pcie =
-		IWL_TRANS_GET_PCIE_TRANS(trans);
-
-	base = trans->shrd->device_pointers.error_event_table;
-	if (trans->shrd->ucode_type == IWL_UCODE_INIT) {
-		if (!base)
-			base = trans->shrd->fw->init_errlog_ptr;
-	} else {
-		if (!base)
-			base = trans->shrd->fw->inst_errlog_ptr;
-	}
-
-	if (!iwlagn_hw_valid_rtc_data_addr(base)) {
-		IWL_ERR(trans,
-			"Not valid error log pointer 0x%08X for %s uCode\n",
-			base,
-			(trans->shrd->ucode_type == IWL_UCODE_INIT)
-					? "Init" : "RT");
-		return;
-	}
-
-	iwl_read_targ_mem_words(trans, base, &table, sizeof(table));
-
-	if (ERROR_START_OFFSET <= table.valid * ERROR_ELEM_SIZE) {
-		IWL_ERR(trans, "Start IWL Error Log Dump:\n");
-		IWL_ERR(trans, "Status: 0x%08lX, count: %d\n",
-			trans->shrd->status, table.valid);
-	}
-
-	trans_pcie->isr_stats.err_code = table.error_id;
-
-	trace_iwlwifi_dev_ucode_error(trans->dev, table.error_id, table.tsf_low,
-				      table.data1, table.data2, table.line,
-				      table.blink1, table.blink2, table.ilink1,
-				      table.ilink2, table.bcon_time, table.gp1,
-				      table.gp2, table.gp3, table.ucode_ver,
-				      table.hw_ver, table.brd_ver);
-	IWL_ERR(trans, "0x%08X | %-28s\n", table.error_id,
-		desc_lookup(table.error_id));
-	IWL_ERR(trans, "0x%08X | uPc\n", table.pc);
-	IWL_ERR(trans, "0x%08X | branchlink1\n", table.blink1);
-	IWL_ERR(trans, "0x%08X | branchlink2\n", table.blink2);
-	IWL_ERR(trans, "0x%08X | interruptlink1\n", table.ilink1);
-	IWL_ERR(trans, "0x%08X | interruptlink2\n", table.ilink2);
-	IWL_ERR(trans, "0x%08X | data1\n", table.data1);
-	IWL_ERR(trans, "0x%08X | data2\n", table.data2);
-	IWL_ERR(trans, "0x%08X | line\n", table.line);
-	IWL_ERR(trans, "0x%08X | beacon time\n", table.bcon_time);
-	IWL_ERR(trans, "0x%08X | tsf low\n", table.tsf_low);
-	IWL_ERR(trans, "0x%08X | tsf hi\n", table.tsf_hi);
-	IWL_ERR(trans, "0x%08X | time gp1\n", table.gp1);
-	IWL_ERR(trans, "0x%08X | time gp2\n", table.gp2);
-	IWL_ERR(trans, "0x%08X | time gp3\n", table.gp3);
-	IWL_ERR(trans, "0x%08X | uCode version\n", table.ucode_ver);
-	IWL_ERR(trans, "0x%08X | hw version\n", table.hw_ver);
-	IWL_ERR(trans, "0x%08X | board version\n", table.brd_ver);
-	IWL_ERR(trans, "0x%08X | hcmd\n", table.hcmd);
-
-	IWL_ERR(trans, "0x%08X | isr0\n", table.isr0);
-	IWL_ERR(trans, "0x%08X | isr1\n", table.isr1);
-	IWL_ERR(trans, "0x%08X | isr2\n", table.isr2);
-	IWL_ERR(trans, "0x%08X | isr3\n", table.isr3);
-	IWL_ERR(trans, "0x%08X | isr4\n", table.isr4);
-	IWL_ERR(trans, "0x%08X | isr_pref\n", table.isr_pref);
-	IWL_ERR(trans, "0x%08X | wait_event\n", table.wait_event);
-	IWL_ERR(trans, "0x%08X | l2p_control\n", table.l2p_control);
-	IWL_ERR(trans, "0x%08X | l2p_duration\n", table.l2p_duration);
-	IWL_ERR(trans, "0x%08X | l2p_mhvalid\n", table.l2p_mhvalid);
-	IWL_ERR(trans, "0x%08X | l2p_addr_match\n", table.l2p_addr_match);
-	IWL_ERR(trans, "0x%08X | lmpm_pmg_sel\n", table.lmpm_pmg_sel);
-	IWL_ERR(trans, "0x%08X | timestamp\n", table.u_timestamp);
-	IWL_ERR(trans, "0x%08X | flow_handler\n", table.flow_handler);
-}
-
 /**
  * iwl_irq_handle_error - called for HW or SW error interrupt from card
  */
 static void iwl_irq_handle_error(struct iwl_trans *trans)
 {
 	/* W/A for WiFi/WiMAX coex and WiMAX own the RF */
-	if (cfg(trans)->internal_wimax_coex &&
+	if (trans->cfg->internal_wimax_coex &&
 	    (!(iwl_read_prph(trans, APMG_CLK_CTRL_REG) &
 			APMS_CLK_VAL_MRB_FUNC_MODE) ||
 	     (iwl_read_prph(trans, APMG_PS_CTRL_REG) &
 			APMG_PS_CTRL_VAL_RESET_REQ))) {
-		/*
-		 * Keep the restart process from trying to send host
-		 * commands by clearing the ready bit.
-		 */
-		clear_bit(STATUS_READY, &trans->shrd->status);
-		clear_bit(STATUS_HCMD_ACTIVE, &trans->shrd->status);
+		struct iwl_trans_pcie *trans_pcie;
+
+		trans_pcie = IWL_TRANS_GET_PCIE_TRANS(trans);
+		clear_bit(STATUS_HCMD_ACTIVE, &trans_pcie->status);
+		iwl_op_mode_wimax_active(trans->op_mode);
 		wake_up(&trans->wait_command_queue);
-		IWL_ERR(trans, "RF is used by WiMAX\n");
 		return;
 	}
 
-	IWL_ERR(trans, "Loaded firmware version: %s\n",
-		trans->shrd->fw->fw_version);
-
-	iwl_dump_nic_error_log(trans);
 	iwl_dump_csr(trans);
 	iwl_dump_fh(trans, NULL, false);
-	iwl_dump_nic_event_log(trans, false, NULL, false);
 
 	iwl_op_mode_nic_error(trans->op_mode);
 }
 
-#define EVENT_START_OFFSET  (4 * sizeof(u32))
-
-/**
- * iwl_print_event_log - Dump error event log to syslog
- *
- */
-static int iwl_print_event_log(struct iwl_trans *trans, u32 start_idx,
-			       u32 num_events, u32 mode,
-			       int pos, char **buf, size_t bufsz)
-{
-	u32 i;
-	u32 base;       /* SRAM byte address of event log header */
-	u32 event_size; /* 2 u32s, or 3 u32s if timestamp recorded */
-	u32 ptr;        /* SRAM byte address of log data */
-	u32 ev, time, data; /* event log data */
-	unsigned long reg_flags;
-
-	if (num_events == 0)
-		return pos;
-
-	base = trans->shrd->device_pointers.log_event_table;
-	if (trans->shrd->ucode_type == IWL_UCODE_INIT) {
-		if (!base)
-			base = trans->shrd->fw->init_evtlog_ptr;
-	} else {
-		if (!base)
-			base = trans->shrd->fw->inst_evtlog_ptr;
-	}
-
-	if (mode == 0)
-		event_size = 2 * sizeof(u32);
-	else
-		event_size = 3 * sizeof(u32);
-
-	ptr = base + EVENT_START_OFFSET + (start_idx * event_size);
-
-	/* Make sure device is powered up for SRAM reads */
-	spin_lock_irqsave(&trans->reg_lock, reg_flags);
-	if (unlikely(!iwl_grab_nic_access(trans)))
-		goto out_unlock;
-
-	/* Set starting address; reads will auto-increment */
-	iwl_write32(trans, HBUS_TARG_MEM_RADDR, ptr);
-
-	/* "time" is actually "data" for mode 0 (no timestamp).
-	* place event id # at far right for easier visual parsing. */
-	for (i = 0; i < num_events; i++) {
-		ev = iwl_read32(trans, HBUS_TARG_MEM_RDAT);
-		time = iwl_read32(trans, HBUS_TARG_MEM_RDAT);
-		if (mode == 0) {
-			/* data, ev */
-			if (bufsz) {
-				pos += scnprintf(*buf + pos, bufsz - pos,
-						"EVT_LOG:0x%08x:%04u\n",
-						time, ev);
-			} else {
-				trace_iwlwifi_dev_ucode_event(trans->dev, 0,
-					time, ev);
-				IWL_ERR(trans, "EVT_LOG:0x%08x:%04u\n",
-					time, ev);
-			}
-		} else {
-			data = iwl_read32(trans, HBUS_TARG_MEM_RDAT);
-			if (bufsz) {
-				pos += scnprintf(*buf + pos, bufsz - pos,
-						"EVT_LOGT:%010u:0x%08x:%04u\n",
-						 time, data, ev);
-			} else {
-				IWL_ERR(trans, "EVT_LOGT:%010u:0x%08x:%04u\n",
-					time, data, ev);
-				trace_iwlwifi_dev_ucode_event(trans->dev, time,
-					data, ev);
-			}
-		}
-	}
-
-	/* Allow device to power down */
-	iwl_release_nic_access(trans);
-out_unlock:
-	spin_unlock_irqrestore(&trans->reg_lock, reg_flags);
-	return pos;
-}
-
-/**
- * iwl_print_last_event_logs - Dump the newest # of event log to syslog
- */
-static int iwl_print_last_event_logs(struct iwl_trans *trans, u32 capacity,
-				    u32 num_wraps, u32 next_entry,
-				    u32 size, u32 mode,
-				    int pos, char **buf, size_t bufsz)
-{
-	/*
-	 * display the newest DEFAULT_LOG_ENTRIES entries
-	 * i.e the entries just before the next ont that uCode would fill.
-	 */
-	if (num_wraps) {
-		if (next_entry < size) {
-			pos = iwl_print_event_log(trans,
-						capacity - (size - next_entry),
-						size - next_entry, mode,
-						pos, buf, bufsz);
-			pos = iwl_print_event_log(trans, 0,
-						  next_entry, mode,
-						  pos, buf, bufsz);
-		} else
-			pos = iwl_print_event_log(trans, next_entry - size,
-						  size, mode, pos, buf, bufsz);
-	} else {
-		if (next_entry < size) {
-			pos = iwl_print_event_log(trans, 0, next_entry,
-						  mode, pos, buf, bufsz);
-		} else {
-			pos = iwl_print_event_log(trans, next_entry - size,
-						  size, mode, pos, buf, bufsz);
-		}
-	}
-	return pos;
-}
-
-#define DEFAULT_DUMP_EVENT_LOG_ENTRIES (20)
-
-int iwl_dump_nic_event_log(struct iwl_trans *trans, bool full_log,
-			    char **buf, bool display)
-{
-	u32 base;       /* SRAM byte address of event log header */
-	u32 capacity;   /* event log capacity in # entries */
-	u32 mode;       /* 0 - no timestamp, 1 - timestamp recorded */
-	u32 num_wraps;  /* # times uCode wrapped to top of log */
-	u32 next_entry; /* index of next entry to be written by uCode */
-	u32 size;       /* # entries that we'll print */
-	u32 logsize;
-	int pos = 0;
-	size_t bufsz = 0;
-
-	base = trans->shrd->device_pointers.log_event_table;
-	if (trans->shrd->ucode_type == IWL_UCODE_INIT) {
-		logsize = trans->shrd->fw->init_evtlog_size;
-		if (!base)
-			base = trans->shrd->fw->init_evtlog_ptr;
-	} else {
-		logsize = trans->shrd->fw->inst_evtlog_size;
-		if (!base)
-			base = trans->shrd->fw->inst_evtlog_ptr;
-	}
-
-	if (!iwlagn_hw_valid_rtc_data_addr(base)) {
-		IWL_ERR(trans,
-			"Invalid event log pointer 0x%08X for %s uCode\n",
-			base,
-			(trans->shrd->ucode_type == IWL_UCODE_INIT)
-					? "Init" : "RT");
-		return -EINVAL;
-	}
-
-	/* event log header */
-	capacity = iwl_read_targ_mem(trans, base);
-	mode = iwl_read_targ_mem(trans, base + (1 * sizeof(u32)));
-	num_wraps = iwl_read_targ_mem(trans, base + (2 * sizeof(u32)));
-	next_entry = iwl_read_targ_mem(trans, base + (3 * sizeof(u32)));
-
-	if (capacity > logsize) {
-		IWL_ERR(trans, "Log capacity %d is bogus, limit to %d "
-			"entries\n", capacity, logsize);
-		capacity = logsize;
-	}
-
-	if (next_entry > logsize) {
-		IWL_ERR(trans, "Log write index %d is bogus, limit to %d\n",
-			next_entry, logsize);
-		next_entry = logsize;
-	}
-
-	size = num_wraps ? capacity : next_entry;
-
-	/* bail out if nothing in log */
-	if (size == 0) {
-		IWL_ERR(trans, "Start IWL Event Log Dump: nothing in log\n");
-		return pos;
-	}
-
-#ifdef CONFIG_IWLWIFI_DEBUG
-	if (!(iwl_have_debug_level(IWL_DL_FW_ERRORS)) && !full_log)
-		size = (size > DEFAULT_DUMP_EVENT_LOG_ENTRIES)
-			? DEFAULT_DUMP_EVENT_LOG_ENTRIES : size;
-#else
-	size = (size > DEFAULT_DUMP_EVENT_LOG_ENTRIES)
-		? DEFAULT_DUMP_EVENT_LOG_ENTRIES : size;
-#endif
-	IWL_ERR(trans, "Start IWL Event Log Dump: display last %u entries\n",
-		size);
-
-#ifdef CONFIG_IWLWIFI_DEBUG
-	if (display) {
-		if (full_log)
-			bufsz = capacity * 48;
-		else
-			bufsz = size * 48;
-		*buf = kmalloc(bufsz, GFP_KERNEL);
-		if (!*buf)
-			return -ENOMEM;
-	}
-	if (iwl_have_debug_level(IWL_DL_FW_ERRORS) || full_log) {
-		/*
-		 * if uCode has wrapped back to top of log,
-		 * start at the oldest entry,
-		 * i.e the next one that uCode would fill.
-		 */
-		if (num_wraps)
-			pos = iwl_print_event_log(trans, next_entry,
-						capacity - next_entry, mode,
-						pos, buf, bufsz);
-		/* (then/else) start at top of log */
-		pos = iwl_print_event_log(trans, 0,
-					  next_entry, mode, pos, buf, bufsz);
-	} else
-		pos = iwl_print_last_event_logs(trans, capacity, num_wraps,
-						next_entry, size, mode,
-						pos, buf, bufsz);
-#else
-	pos = iwl_print_last_event_logs(trans, capacity, num_wraps,
-					next_entry, size, mode,
-					pos, buf, bufsz);
-#endif
-	return pos;
-}
-
 /* tasklet for iwlagn interrupt */
 void iwl_irq_tasklet(struct iwl_trans *trans)
 {
@@ -963,7 +601,7 @@ void iwl_irq_tasklet(struct iwl_trans *trans)
 	if (iwl_have_debug_level(IWL_DL_ISR)) {
 		/* just for debug */
 		inta_mask = iwl_read32(trans, CSR_INT_MASK);
-		IWL_DEBUG_ISR(trans, "inta 0x%08x, enabled 0x%08x\n ",
+		IWL_DEBUG_ISR(trans, "inta 0x%08x, enabled 0x%08x\n",
 				inta, inta_mask);
 	}
 #endif
@@ -1011,8 +649,7 @@ void iwl_irq_tasklet(struct iwl_trans *trans)
 	if (inta & CSR_INT_BIT_RF_KILL) {
 		bool hw_rfkill;
 
-		hw_rfkill = !(iwl_read32(trans, CSR_GP_CNTRL) &
-				CSR_GP_CNTRL_REG_FLAG_HW_RF_KILL_SW);
+		hw_rfkill = iwl_is_rfkill_set(trans);
 		IWL_WARN(trans, "RF_KILL bit toggled to %s.\n",
 				hw_rfkill ? "disable radio" : "enable radio");
 
@@ -1043,7 +680,7 @@ void iwl_irq_tasklet(struct iwl_trans *trans)
 	if (inta & CSR_INT_BIT_WAKEUP) {
 		IWL_DEBUG_ISR(trans, "Wakeup interrupt\n");
 		iwl_rx_queue_update_write_ptr(trans, &trans_pcie->rxq);
-		for (i = 0; i < cfg(trans)->base_params->num_of_queues; i++)
+		for (i = 0; i < trans->cfg->base_params->num_of_queues; i++)
 			iwl_txq_update_write_ptr(trans,
 						 &trans_pcie->txq[i]);
 
diff --git a/drivers/net/wireless/iwlwifi/iwl-trans-pcie-tx.c b/drivers/net/wireless/iwlwifi/iwl-trans-pcie-tx.c
index e92972fd6ec..21a8a672fbb 100644
--- a/drivers/net/wireless/iwlwifi/iwl-trans-pcie-tx.c
+++ b/drivers/net/wireless/iwlwifi/iwl-trans-pcie-tx.c
@@ -37,47 +37,12 @@
 #include "iwl-agn-hw.h"
 #include "iwl-op-mode.h"
 #include "iwl-trans-pcie-int.h"
+/* FIXME: need to abstract out TX command (once we know what it looks like) */
+#include "iwl-commands.h"
 
 #define IWL_TX_CRC_SIZE 4
 #define IWL_TX_DELIMITER_SIZE 4
 
-/*
- * mac80211 queues, ACs, hardware queues, FIFOs.
- *
- * Cf. http://wireless.kernel.org/en/developers/Documentation/mac80211/queues
- *
- * Mac80211 uses the following numbers, which we get as from it
- * by way of skb_get_queue_mapping(skb):
- *
- *	VO	0
- *	VI	1
- *	BE	2
- *	BK	3
- *
- *
- * Regular (not A-MPDU) frames are put into hardware queues corresponding
- * to the FIFOs, see comments in iwl-prph.h. Aggregated frames get their
- * own queue per aggregation session (RA/TID combination), such queues are
- * set up to map into FIFOs too, for which we need an AC->FIFO mapping. In
- * order to map frames to the right queue, we also need an AC->hw queue
- * mapping. This is implemented here.
- *
- * Due to the way hw queues are set up (by the hw specific code), the AC->hw
- * queue mapping is the identity mapping.
- */
-
-static const u8 tid_to_ac[] = {
-	IEEE80211_AC_BE,
-	IEEE80211_AC_BK,
-	IEEE80211_AC_BK,
-	IEEE80211_AC_BE,
-	IEEE80211_AC_VI,
-	IEEE80211_AC_VI,
-	IEEE80211_AC_VO,
-	IEEE80211_AC_VO
-};
-
-
 /**
  * iwl_trans_txq_update_byte_cnt_tbl - Set up entry in Tx byte-count array
  */
@@ -95,7 +60,7 @@ void iwl_trans_txq_update_byte_cnt_tbl(struct iwl_trans *trans,
 	u16 len = byte_cnt + IWL_TX_CRC_SIZE + IWL_TX_DELIMITER_SIZE;
 	__le16 bc_ent;
 	struct iwl_tx_cmd *tx_cmd =
-		(struct iwl_tx_cmd *) txq->cmd[txq->q.write_ptr]->payload;
+		(void *) txq->entries[txq->q.write_ptr].cmd->payload;
 
 	scd_bc_tbl = trans_pcie->scd_bc_tbls.addr;
 
@@ -136,13 +101,15 @@ void iwl_txq_update_write_ptr(struct iwl_trans *trans, struct iwl_tx_queue *txq)
 	if (txq->need_update == 0)
 		return;
 
-	if (cfg(trans)->base_params->shadow_reg_enable) {
+	if (trans->cfg->base_params->shadow_reg_enable) {
 		/* shadow register enabled */
 		iwl_write32(trans, HBUS_TARG_WRPTR,
 			    txq->q.write_ptr | (txq_id << 8));
 	} else {
+		struct iwl_trans_pcie *trans_pcie =
+			IWL_TRANS_GET_PCIE_TRANS(trans);
 		/* if we're trying to save power */
-		if (test_bit(STATUS_POWER_PMI, &trans->shrd->status)) {
+		if (test_bit(STATUS_TPOWER_PMI, &trans_pcie->status)) {
 			/* wake up nic if it's powered down ...
 			 * uCode will wake up, and interrupt us again, so next
 			 * time we'll skip this part. */
@@ -256,13 +223,14 @@ void iwlagn_txq_free_tfd(struct iwl_trans *trans, struct iwl_tx_queue *txq,
 
 	lockdep_assert_held(&txq->lock);
 
-	iwlagn_unmap_tfd(trans, &txq->meta[index], &tfd_tmp[index], dma_dir);
+	iwlagn_unmap_tfd(trans, &txq->entries[index].meta,
+			 &tfd_tmp[index], dma_dir);
 
 	/* free SKB */
-	if (txq->skbs) {
+	if (txq->entries) {
 		struct sk_buff *skb;
 
-		skb = txq->skbs[index];
+		skb = txq->entries[index].skb;
 
 		/* Can be called from irqs-disabled context
 		 * If skb is not NULL, it means that the whole queue is being
@@ -270,7 +238,7 @@ void iwlagn_txq_free_tfd(struct iwl_trans *trans, struct iwl_tx_queue *txq,
 		 */
 		if (skb) {
 			iwl_op_mode_free_skb(trans->op_mode, skb);
-			txq->skbs[index] = NULL;
+			txq->entries[index].skb = NULL;
 		}
 	}
 }
@@ -393,7 +361,7 @@ static void iwlagn_txq_inval_byte_cnt_tbl(struct iwl_trans *trans,
 	u8 sta_id = 0;
 	__le16 bc_ent;
 	struct iwl_tx_cmd *tx_cmd =
-		(struct iwl_tx_cmd *) txq->cmd[txq->q.read_ptr]->payload;
+		(void *)txq->entries[txq->q.read_ptr].cmd->payload;
 
 	WARN_ON(read_ptr >= TFD_QUEUE_SIZE_MAX);
 
@@ -448,20 +416,17 @@ static void iwlagn_tx_queue_stop_scheduler(struct iwl_trans *trans, u16 txq_id)
 void iwl_trans_set_wr_ptrs(struct iwl_trans *trans,
 				int txq_id, u32 index)
 {
-	IWL_DEBUG_TX_QUEUES(trans, "Q %d  WrPtr: %d", txq_id, index & 0xff);
+	IWL_DEBUG_TX_QUEUES(trans, "Q %d  WrPtr: %d\n", txq_id, index & 0xff);
 	iwl_write_direct32(trans, HBUS_TARG_WRPTR,
 			(index & 0xff) | (txq_id << 8));
 	iwl_write_prph(trans, SCD_QUEUE_RDPTR(txq_id), index);
 }
 
 void iwl_trans_tx_queue_set_status(struct iwl_trans *trans,
-					struct iwl_tx_queue *txq,
-					int tx_fifo_id, int scd_retry)
+				   struct iwl_tx_queue *txq,
+				   int tx_fifo_id, bool active)
 {
-	struct iwl_trans_pcie *trans_pcie = IWL_TRANS_GET_PCIE_TRANS(trans);
 	int txq_id = txq->q.id;
-	int active =
-		test_bit(txq_id, &trans_pcie->txq_ctx_active_msk) ? 1 : 0;
 
 	iwl_write_prph(trans, SCD_QUEUE_STATUS_BITS(txq_id),
 			(active << SCD_QUEUE_STTS_REG_POS_ACTIVE) |
@@ -469,77 +434,22 @@ void iwl_trans_tx_queue_set_status(struct iwl_trans *trans,
 			(1 << SCD_QUEUE_STTS_REG_POS_WSL) |
 			SCD_QUEUE_STTS_REG_MSK);
 
-	txq->sched_retry = scd_retry;
-
 	if (active)
-		IWL_DEBUG_TX_QUEUES(trans, "Activate %s Queue %d on FIFO %d\n",
-			scd_retry ? "BA" : "AC/CMD", txq_id, tx_fifo_id);
+		IWL_DEBUG_TX_QUEUES(trans, "Activate queue %d on FIFO %d\n",
+				    txq_id, tx_fifo_id);
 	else
-		IWL_DEBUG_TX_QUEUES(trans, "Deactivate %s Queue %d\n",
-			scd_retry ? "BA" : "AC/CMD", txq_id);
-}
-
-static inline int get_ac_from_tid(u16 tid)
-{
-	if (likely(tid < ARRAY_SIZE(tid_to_ac)))
-		return tid_to_ac[tid];
-
-	/* no support for TIDs 8-15 yet */
-	return -EINVAL;
-}
-
-static inline int get_fifo_from_tid(struct iwl_trans_pcie *trans_pcie,
-				    u8 ctx, u16 tid)
-{
-	const u8 *ac_to_fifo = trans_pcie->ac_to_fifo[ctx];
-	if (likely(tid < ARRAY_SIZE(tid_to_ac)))
-		return ac_to_fifo[tid_to_ac[tid]];
-
-	/* no support for TIDs 8-15 yet */
-	return -EINVAL;
+		IWL_DEBUG_TX_QUEUES(trans, "Deactivate queue %d\n", txq_id);
 }
 
-static inline bool is_agg_txqid_valid(struct iwl_trans *trans, int txq_id)
+void iwl_trans_pcie_tx_agg_setup(struct iwl_trans *trans, int txq_id, int fifo,
+				 int sta_id, int tid, int frame_limit, u16 ssn)
 {
-	if (txq_id < IWLAGN_FIRST_AMPDU_QUEUE)
-		return false;
-	return txq_id < (IWLAGN_FIRST_AMPDU_QUEUE +
-		hw_params(trans).num_ampdu_queues);
-}
-
-void iwl_trans_pcie_tx_agg_setup(struct iwl_trans *trans,
-				 enum iwl_rxon_context_id ctx, int sta_id,
-				 int tid, int frame_limit, u16 ssn)
-{
-	int tx_fifo, txq_id;
-	u16 ra_tid;
+	struct iwl_trans_pcie *trans_pcie = IWL_TRANS_GET_PCIE_TRANS(trans);
 	unsigned long flags;
+	u16 ra_tid = BUILD_RAxTID(sta_id, tid);
 
-	struct iwl_trans_pcie *trans_pcie =
-		IWL_TRANS_GET_PCIE_TRANS(trans);
-
-	if (WARN_ON(sta_id == IWL_INVALID_STATION))
-		return;
-	if (WARN_ON(tid >= IWL_MAX_TID_COUNT))
-		return;
-
-	tx_fifo = get_fifo_from_tid(trans_pcie, ctx, tid);
-	if (WARN_ON(tx_fifo < 0)) {
-		IWL_ERR(trans, "txq_agg_setup, bad fifo: %d\n", tx_fifo);
-		return;
-	}
-
-	txq_id = trans_pcie->agg_txq[sta_id][tid];
-	if (WARN_ON_ONCE(!is_agg_txqid_valid(trans, txq_id))) {
-		IWL_ERR(trans,
-			"queue number out of range: %d, must be %d to %d\n",
-			txq_id, IWLAGN_FIRST_AMPDU_QUEUE,
-			IWLAGN_FIRST_AMPDU_QUEUE +
-			hw_params(trans).num_ampdu_queues - 1);
-		return;
-	}
-
-	ra_tid = BUILD_RAxTID(sta_id, tid);
+	if (test_and_set_bit(txq_id, trans_pcie->queue_used))
+		WARN_ONCE(1, "queue %d already used - expect issues", txq_id);
 
 	spin_lock_irqsave(&trans_pcie->irq_lock, flags);
 
@@ -550,10 +460,10 @@ void iwl_trans_pcie_tx_agg_setup(struct iwl_trans *trans,
 	iwlagn_tx_queue_set_q2ratid(trans, ra_tid, txq_id);
 
 	/* Set this queue as a chain-building queue */
-	iwl_set_bits_prph(trans, SCD_QUEUECHAIN_SEL, (1<<txq_id));
+	iwl_set_bits_prph(trans, SCD_QUEUECHAIN_SEL, BIT(txq_id));
 
 	/* enable aggregations for the queue */
-	iwl_set_bits_prph(trans, SCD_AGGR_SEL, (1<<txq_id));
+	iwl_set_bits_prph(trans, SCD_AGGR_SEL, BIT(txq_id));
 
 	/* Place first TFD at index corresponding to start sequence number.
 	 * Assumes that ssn_idx is valid (!= 0xFFF) */
@@ -563,92 +473,42 @@ void iwl_trans_pcie_tx_agg_setup(struct iwl_trans *trans,
 
 	/* Set up Tx window size and frame limit for this queue */
 	iwl_write_targ_mem(trans, trans_pcie->scd_base_addr +
-			SCD_CONTEXT_QUEUE_OFFSET(txq_id) +
-			sizeof(u32),
-			((frame_limit <<
-			SCD_QUEUE_CTX_REG2_WIN_SIZE_POS) &
-			SCD_QUEUE_CTX_REG2_WIN_SIZE_MSK) |
-			((frame_limit <<
-			SCD_QUEUE_CTX_REG2_FRAME_LIMIT_POS) &
-			SCD_QUEUE_CTX_REG2_FRAME_LIMIT_MSK));
+			SCD_CONTEXT_QUEUE_OFFSET(txq_id) + sizeof(u32),
+			((frame_limit << SCD_QUEUE_CTX_REG2_WIN_SIZE_POS) &
+				SCD_QUEUE_CTX_REG2_WIN_SIZE_MSK) |
+			((frame_limit << SCD_QUEUE_CTX_REG2_FRAME_LIMIT_POS) &
+				SCD_QUEUE_CTX_REG2_FRAME_LIMIT_MSK));
 
 	iwl_set_bits_prph(trans, SCD_INTERRUPT_MASK, (1 << txq_id));
 
 	/* Set up Status area in SRAM, map to Tx DMA/FIFO, activate the queue */
 	iwl_trans_tx_queue_set_status(trans, &trans_pcie->txq[txq_id],
-					tx_fifo, 1);
-
-	trans_pcie->txq[txq_id].sta_id = sta_id;
-	trans_pcie->txq[txq_id].tid = tid;
+				      fifo, true);
 
 	spin_unlock_irqrestore(&trans_pcie->irq_lock, flags);
 }
 
-/*
- * Find first available (lowest unused) Tx Queue, mark it "active".
- * Called only when finding queue for aggregation.
- * Should never return anything < 7, because they should already
- * be in use as EDCA AC (0-3), Command (4), reserved (5, 6)
- */
-static int iwlagn_txq_ctx_activate_free(struct iwl_trans *trans)
-{
-	struct iwl_trans_pcie *trans_pcie = IWL_TRANS_GET_PCIE_TRANS(trans);
-	int txq_id;
-
-	for (txq_id = 0; txq_id < cfg(trans)->base_params->num_of_queues;
-	     txq_id++)
-		if (!test_and_set_bit(txq_id,
-					&trans_pcie->txq_ctx_active_msk))
-			return txq_id;
-	return -1;
-}
-
-int iwl_trans_pcie_tx_agg_alloc(struct iwl_trans *trans,
-				int sta_id, int tid)
-{
-	struct iwl_trans_pcie *trans_pcie = IWL_TRANS_GET_PCIE_TRANS(trans);
-	int txq_id;
-
-	txq_id = iwlagn_txq_ctx_activate_free(trans);
-	if (txq_id == -1) {
-		IWL_ERR(trans, "No free aggregation queue available\n");
-		return -ENXIO;
-	}
-
-	trans_pcie->agg_txq[sta_id][tid] = txq_id;
-	iwl_set_swq_id(&trans_pcie->txq[txq_id], get_ac_from_tid(tid), txq_id);
-
-	return 0;
-}
-
-int iwl_trans_pcie_tx_agg_disable(struct iwl_trans *trans, int sta_id, int tid)
+void iwl_trans_pcie_tx_agg_disable(struct iwl_trans *trans, int txq_id)
 {
 	struct iwl_trans_pcie *trans_pcie = IWL_TRANS_GET_PCIE_TRANS(trans);
-	u8 txq_id = trans_pcie->agg_txq[sta_id][tid];
 
-	if (WARN_ON_ONCE(!is_agg_txqid_valid(trans, txq_id))) {
-		IWL_ERR(trans,
-			"queue number out of range: %d, must be %d to %d\n",
-			txq_id, IWLAGN_FIRST_AMPDU_QUEUE,
-			IWLAGN_FIRST_AMPDU_QUEUE +
-			hw_params(trans).num_ampdu_queues - 1);
-		return -EINVAL;
+	if (!test_and_clear_bit(txq_id, trans_pcie->queue_used)) {
+		WARN_ONCE(1, "queue %d not used", txq_id);
+		return;
 	}
 
 	iwlagn_tx_queue_stop_scheduler(trans, txq_id);
 
-	iwl_clear_bits_prph(trans, SCD_AGGR_SEL, (1 << txq_id));
+	iwl_clear_bits_prph(trans, SCD_AGGR_SEL, BIT(txq_id));
 
-	trans_pcie->agg_txq[sta_id][tid] = 0;
 	trans_pcie->txq[txq_id].q.read_ptr = 0;
 	trans_pcie->txq[txq_id].q.write_ptr = 0;
-	/* supposes that ssn_idx is valid (!= 0xFFF) */
 	iwl_trans_set_wr_ptrs(trans, txq_id, 0);
 
-	iwl_clear_bits_prph(trans, SCD_INTERRUPT_MASK, (1 << txq_id));
-	iwl_txq_ctx_deactivate(trans_pcie, txq_id);
-	iwl_trans_tx_queue_set_status(trans, &trans_pcie->txq[txq_id], 0, 0);
-	return 0;
+	iwl_clear_bits_prph(trans, SCD_INTERRUPT_MASK, BIT(txq_id));
+
+	iwl_trans_tx_queue_set_status(trans, &trans_pcie->txq[txq_id],
+				      0, false);
 }
 
 /*************** HOST COMMAND QUEUE FUNCTIONS   *****/
@@ -681,11 +541,6 @@ static int iwl_enqueue_hcmd(struct iwl_trans *trans, struct iwl_host_cmd *cmd)
 	int trace_idx;
 #endif
 
-	if (test_bit(STATUS_FW_ERROR, &trans->shrd->status)) {
-		IWL_WARN(trans, "fw recovery, no hcmd send\n");
-		return -EIO;
-	}
-
 	copy_size = sizeof(out_cmd->hdr);
 	cmd_size = sizeof(out_cmd->hdr);
 
@@ -726,8 +581,8 @@ static int iwl_enqueue_hcmd(struct iwl_trans *trans, struct iwl_host_cmd *cmd)
 	}
 
 	idx = get_cmd_index(q, q->write_ptr);
-	out_cmd = txq->cmd[idx];
-	out_meta = &txq->meta[idx];
+	out_cmd = txq->entries[idx].cmd;
+	out_meta = &txq->entries[idx].meta;
 
 	memset(out_meta, 0, sizeof(*out_meta));	/* re-initialize to NULL */
 	if (cmd->flags & CMD_WANT_SKB)
@@ -753,12 +608,11 @@ static int iwl_enqueue_hcmd(struct iwl_trans *trans, struct iwl_host_cmd *cmd)
 		cmd_dest += cmd->len[i];
 	}
 
-	IWL_DEBUG_HC(trans, "Sending command %s (#%x), seq: 0x%04X, "
-			"%d bytes at %d[%d]:%d\n",
-			get_cmd_string(out_cmd->hdr.cmd),
-			out_cmd->hdr.cmd,
-			le16_to_cpu(out_cmd->hdr.sequence), cmd_size,
-			q->write_ptr, idx, trans_pcie->cmd_queue);
+	IWL_DEBUG_HC(trans,
+		"Sending command %s (#%x), seq: 0x%04X, %d bytes at %d[%d]:%d\n",
+		trans_pcie_get_cmd_string(trans_pcie, out_cmd->hdr.cmd),
+		out_cmd->hdr.cmd, le16_to_cpu(out_cmd->hdr.sequence), cmd_size,
+		q->write_ptr, idx, trans_pcie->cmd_queue);
 
 	phys_addr = dma_map_single(trans->dev, &out_cmd->hdr, copy_size,
 				DMA_BIDIRECTIONAL);
@@ -816,6 +670,10 @@ static int iwl_enqueue_hcmd(struct iwl_trans *trans, struct iwl_host_cmd *cmd)
 			       trace_bufs[2], trace_lens[2]);
 #endif
 
+	/* start timer if queue currently empty */
+	if (q->read_ptr == q->write_ptr && trans_pcie->wd_timeout)
+		mod_timer(&txq->stuck_timer, jiffies + trans_pcie->wd_timeout);
+
 	/* Increment and update queue's write index */
 	q->write_ptr = iwl_queue_inc_wrap(q->write_ptr, q->n_bd);
 	iwl_txq_update_write_ptr(trans, txq);
@@ -825,6 +683,22 @@ static int iwl_enqueue_hcmd(struct iwl_trans *trans, struct iwl_host_cmd *cmd)
 	return idx;
 }
 
+static inline void iwl_queue_progress(struct iwl_trans_pcie *trans_pcie,
+				      struct iwl_tx_queue *txq)
+{
+	if (!trans_pcie->wd_timeout)
+		return;
+
+	/*
+	 * if empty delete timer, otherwise move timer forward
+	 * since we're making progress on this queue
+	 */
+	if (txq->q.read_ptr == txq->q.write_ptr)
+		del_timer(&txq->stuck_timer);
+	else
+		mod_timer(&txq->stuck_timer, jiffies + trans_pcie->wd_timeout);
+}
+
 /**
  * iwl_hcmd_queue_reclaim - Reclaim TX command queue entries already Tx'd
  *
@@ -859,6 +733,8 @@ static void iwl_hcmd_queue_reclaim(struct iwl_trans *trans, int txq_id,
 		}
 
 	}
+
+	iwl_queue_progress(trans_pcie, txq);
 }
 
 /**
@@ -899,10 +775,8 @@ void iwl_tx_cmd_complete(struct iwl_trans *trans, struct iwl_rx_cmd_buffer *rxb,
 	spin_lock(&txq->lock);
 
 	cmd_index = get_cmd_index(&txq->q, index);
-	cmd = txq->cmd[cmd_index];
-	meta = &txq->meta[cmd_index];
-
-	txq->time_stamp = jiffies;
+	cmd = txq->entries[cmd_index].cmd;
+	meta = &txq->entries[cmd_index].meta;
 
 	iwlagn_unmap_tfd(trans, meta, &txq->tfds[index],
 			 DMA_BIDIRECTIONAL);
@@ -913,21 +787,23 @@ void iwl_tx_cmd_complete(struct iwl_trans *trans, struct iwl_rx_cmd_buffer *rxb,
 
 		meta->source->resp_pkt = pkt;
 		meta->source->_rx_page_addr = (unsigned long)page_address(p);
-		meta->source->_rx_page_order = hw_params(trans).rx_page_order;
+		meta->source->_rx_page_order = trans_pcie->rx_page_order;
 		meta->source->handler_status = handler_status;
 	}
 
 	iwl_hcmd_queue_reclaim(trans, txq_id, index);
 
 	if (!(meta->flags & CMD_ASYNC)) {
-		if (!test_bit(STATUS_HCMD_ACTIVE, &trans->shrd->status)) {
+		if (!test_bit(STATUS_HCMD_ACTIVE, &trans_pcie->status)) {
 			IWL_WARN(trans,
 				 "HCMD_ACTIVE already clear for command %s\n",
-				 get_cmd_string(cmd->hdr.cmd));
+				 trans_pcie_get_cmd_string(trans_pcie,
+							   cmd->hdr.cmd));
 		}
-		clear_bit(STATUS_HCMD_ACTIVE, &trans->shrd->status);
+		clear_bit(STATUS_HCMD_ACTIVE, &trans_pcie->status);
 		IWL_DEBUG_INFO(trans, "Clearing HCMD_ACTIVE for command %s\n",
-			       get_cmd_string(cmd->hdr.cmd));
+			       trans_pcie_get_cmd_string(trans_pcie,
+							 cmd->hdr.cmd));
 		wake_up(&trans->wait_command_queue);
 	}
 
@@ -940,6 +816,7 @@ void iwl_tx_cmd_complete(struct iwl_trans *trans, struct iwl_rx_cmd_buffer *rxb,
 
 static int iwl_send_cmd_async(struct iwl_trans *trans, struct iwl_host_cmd *cmd)
 {
+	struct iwl_trans_pcie *trans_pcie = IWL_TRANS_GET_PCIE_TRANS(trans);
 	int ret;
 
 	/* An asynchronous command can not expect an SKB to be set. */
@@ -951,7 +828,7 @@ static int iwl_send_cmd_async(struct iwl_trans *trans, struct iwl_host_cmd *cmd)
 	if (ret < 0) {
 		IWL_ERR(trans,
 			"Error sending %s: enqueue_hcmd failed: %d\n",
-			  get_cmd_string(cmd->id), ret);
+			trans_pcie_get_cmd_string(trans_pcie, cmd->id), ret);
 		return ret;
 	}
 	return 0;
@@ -964,55 +841,51 @@ static int iwl_send_cmd_sync(struct iwl_trans *trans, struct iwl_host_cmd *cmd)
 	int ret;
 
 	IWL_DEBUG_INFO(trans, "Attempting to send sync command %s\n",
-			get_cmd_string(cmd->id));
-
-	if (test_bit(STATUS_FW_ERROR, &trans->shrd->status)) {
-		IWL_ERR(trans, "Command %s failed: FW Error\n",
-			       get_cmd_string(cmd->id));
-		return -EIO;
-	}
+		       trans_pcie_get_cmd_string(trans_pcie, cmd->id));
 
 	if (WARN_ON(test_and_set_bit(STATUS_HCMD_ACTIVE,
-				     &trans->shrd->status))) {
+				     &trans_pcie->status))) {
 		IWL_ERR(trans, "Command %s: a command is already active!\n",
-			get_cmd_string(cmd->id));
+			trans_pcie_get_cmd_string(trans_pcie, cmd->id));
 		return -EIO;
 	}
 
 	IWL_DEBUG_INFO(trans, "Setting HCMD_ACTIVE for command %s\n",
-			get_cmd_string(cmd->id));
+		       trans_pcie_get_cmd_string(trans_pcie, cmd->id));
 
 	cmd_idx = iwl_enqueue_hcmd(trans, cmd);
 	if (cmd_idx < 0) {
 		ret = cmd_idx;
-		clear_bit(STATUS_HCMD_ACTIVE, &trans->shrd->status);
+		clear_bit(STATUS_HCMD_ACTIVE, &trans_pcie->status);
 		IWL_ERR(trans,
 			"Error sending %s: enqueue_hcmd failed: %d\n",
-			  get_cmd_string(cmd->id), ret);
+			trans_pcie_get_cmd_string(trans_pcie, cmd->id), ret);
 		return ret;
 	}
 
 	ret = wait_event_timeout(trans->wait_command_queue,
-			!test_bit(STATUS_HCMD_ACTIVE, &trans->shrd->status),
+			!test_bit(STATUS_HCMD_ACTIVE, &trans_pcie->status),
 			HOST_COMPLETE_TIMEOUT);
 	if (!ret) {
-		if (test_bit(STATUS_HCMD_ACTIVE, &trans->shrd->status)) {
+		if (test_bit(STATUS_HCMD_ACTIVE, &trans_pcie->status)) {
 			struct iwl_tx_queue *txq =
 				&trans_pcie->txq[trans_pcie->cmd_queue];
 			struct iwl_queue *q = &txq->q;
 
 			IWL_ERR(trans,
 				"Error sending %s: time out after %dms.\n",
-				get_cmd_string(cmd->id),
+				trans_pcie_get_cmd_string(trans_pcie, cmd->id),
 				jiffies_to_msecs(HOST_COMPLETE_TIMEOUT));
 
 			IWL_ERR(trans,
 				"Current CMD queue read_ptr %d write_ptr %d\n",
 				q->read_ptr, q->write_ptr);
 
-			clear_bit(STATUS_HCMD_ACTIVE, &trans->shrd->status);
-			IWL_DEBUG_INFO(trans, "Clearing HCMD_ACTIVE for command"
-				 "%s\n", get_cmd_string(cmd->id));
+			clear_bit(STATUS_HCMD_ACTIVE, &trans_pcie->status);
+			IWL_DEBUG_INFO(trans,
+				       "Clearing HCMD_ACTIVE for command %s\n",
+				       trans_pcie_get_cmd_string(trans_pcie,
+								 cmd->id));
 			ret = -ETIMEDOUT;
 			goto cancel;
 		}
@@ -1020,7 +893,7 @@ static int iwl_send_cmd_sync(struct iwl_trans *trans, struct iwl_host_cmd *cmd)
 
 	if ((cmd->flags & CMD_WANT_SKB) && !cmd->resp_pkt) {
 		IWL_ERR(trans, "Error: Response NULL in '%s'\n",
-			  get_cmd_string(cmd->id));
+			trans_pcie_get_cmd_string(trans_pcie, cmd->id));
 		ret = -EIO;
 		goto cancel;
 	}
@@ -1035,8 +908,8 @@ cancel:
 		 * in later, it will possibly set an invalid
 		 * address (cmd->meta.source).
 		 */
-		trans_pcie->txq[trans_pcie->cmd_queue].meta[cmd_idx].flags &=
-							~CMD_WANT_SKB;
+		trans_pcie->txq[trans_pcie->cmd_queue].
+			entries[cmd_idx].meta.flags &= ~CMD_WANT_SKB;
 	}
 
 	if (cmd->resp_pkt) {
@@ -1091,17 +964,20 @@ int iwl_tx_queue_reclaim(struct iwl_trans *trans, int txq_id, int index,
 	     q->read_ptr != index;
 	     q->read_ptr = iwl_queue_inc_wrap(q->read_ptr, q->n_bd)) {
 
-		if (WARN_ON_ONCE(txq->skbs[txq->q.read_ptr] == NULL))
+		if (WARN_ON_ONCE(txq->entries[txq->q.read_ptr].skb == NULL))
 			continue;
 
-		__skb_queue_tail(skbs, txq->skbs[txq->q.read_ptr]);
+		__skb_queue_tail(skbs, txq->entries[txq->q.read_ptr].skb);
 
-		txq->skbs[txq->q.read_ptr] = NULL;
+		txq->entries[txq->q.read_ptr].skb = NULL;
 
 		iwlagn_txq_inval_byte_cnt_tbl(trans, txq);
 
 		iwlagn_txq_free_tfd(trans, txq, txq->q.read_ptr, DMA_TO_DEVICE);
 		freed++;
 	}
+
+	iwl_queue_progress(trans_pcie, txq);
+
 	return freed;
 }
diff --git a/drivers/net/wireless/iwlwifi/iwl-trans-pcie.c b/drivers/net/wireless/iwlwifi/iwl-trans-pcie.c
index 4d7b30d3e64..2e57161854b 100644
--- a/drivers/net/wireless/iwlwifi/iwl-trans-pcie.c
+++ b/drivers/net/wireless/iwlwifi/iwl-trans-pcie.c
@@ -68,18 +68,20 @@
 #include <linux/bitops.h>
 #include <linux/gfp.h>
 
+#include "iwl-drv.h"
 #include "iwl-trans.h"
 #include "iwl-trans-pcie-int.h"
 #include "iwl-csr.h"
 #include "iwl-prph.h"
-#include "iwl-shared.h"
 #include "iwl-eeprom.h"
 #include "iwl-agn-hw.h"
+/* FIXME: need to abstract out TX command (once we know what it looks like) */
+#include "iwl-commands.h"
 
 #define IWL_MASK(lo, hi) ((1 << (hi)) | ((1 << (hi)) - (1 << (lo))))
 
 #define SCD_QUEUECHAIN_SEL_ALL(trans, trans_pcie)	\
-	(((1<<cfg(trans)->base_params->num_of_queues) - 1) &\
+	(((1<<trans->cfg->base_params->num_of_queues) - 1) &\
 	(~(1<<(trans_pcie)->cmd_queue)))
 
 static int iwl_trans_rx_alloc(struct iwl_trans *trans)
@@ -132,10 +134,10 @@ static void iwl_trans_rxq_free_rx_bufs(struct iwl_trans *trans)
 		 * to an SKB, so we need to unmap and free potential storage */
 		if (rxq->pool[i].page != NULL) {
 			dma_unmap_page(trans->dev, rxq->pool[i].page_dma,
-				PAGE_SIZE << hw_params(trans).rx_page_order,
+				PAGE_SIZE << trans_pcie->rx_page_order,
 				DMA_FROM_DEVICE);
 			__free_pages(rxq->pool[i].page,
-				     hw_params(trans).rx_page_order);
+				     trans_pcie->rx_page_order);
 			rxq->pool[i].page = NULL;
 		}
 		list_add_tail(&rxq->pool[i].list, &rxq->rx_used);
@@ -145,11 +147,12 @@ static void iwl_trans_rxq_free_rx_bufs(struct iwl_trans *trans)
 static void iwl_trans_rx_hw_init(struct iwl_trans *trans,
 				 struct iwl_rx_queue *rxq)
 {
+	struct iwl_trans_pcie *trans_pcie = IWL_TRANS_GET_PCIE_TRANS(trans);
 	u32 rb_size;
 	const u32 rfdnlog = RX_QUEUE_SIZE_LOG; /* 256 RBDs */
 	u32 rb_timeout = RX_RB_TIMEOUT; /* FIXME: RX_RB_TIMEOUT for all devices? */
 
-	if (iwlagn_mod_params.amsdu_size_8K)
+	if (trans_pcie->rx_buf_size_8k)
 		rb_size = FH_RCSR_RX_CONFIG_REG_VAL_RB_SIZE_8K;
 	else
 		rb_size = FH_RCSR_RX_CONFIG_REG_VAL_RB_SIZE_4K;
@@ -180,7 +183,6 @@ static void iwl_trans_rx_hw_init(struct iwl_trans *trans,
 			   FH_RCSR_RX_CONFIG_CHNL_EN_ENABLE_VAL |
 			   FH_RCSR_CHNL0_RX_IGNORE_RXF_EMPTY |
 			   FH_RCSR_CHNL0_RX_CONFIG_IRQ_DEST_INT_HOST_VAL |
-			   FH_RCSR_CHNL0_RX_CONFIG_SINGLE_FRAME_MSK |
 			   rb_size|
 			   (rb_timeout << FH_RCSR_RX_CONFIG_REG_IRQ_RBTH_POS)|
 			   (rfdnlog << FH_RCSR_RX_CONFIG_RBDCB_SIZE_POS));
@@ -299,6 +301,33 @@ static inline void iwlagn_free_dma_ptr(struct iwl_trans *trans,
 	memset(ptr, 0, sizeof(*ptr));
 }
 
+static void iwl_trans_pcie_queue_stuck_timer(unsigned long data)
+{
+	struct iwl_tx_queue *txq = (void *)data;
+	struct iwl_trans_pcie *trans_pcie = txq->trans_pcie;
+	struct iwl_trans *trans = iwl_trans_pcie_get_trans(trans_pcie);
+
+	spin_lock(&txq->lock);
+	/* check if triggered erroneously */
+	if (txq->q.read_ptr == txq->q.write_ptr) {
+		spin_unlock(&txq->lock);
+		return;
+	}
+	spin_unlock(&txq->lock);
+
+
+	IWL_ERR(trans, "Queue %d stuck for %u ms.\n", txq->q.id,
+		jiffies_to_msecs(trans_pcie->wd_timeout));
+	IWL_ERR(trans, "Current SW read_ptr %d write_ptr %d\n",
+		txq->q.read_ptr, txq->q.write_ptr);
+	IWL_ERR(trans, "Current HW read_ptr %d write_ptr %d\n",
+		iwl_read_prph(trans, SCD_QUEUE_RDPTR(txq->q.id))
+					& (TFD_QUEUE_SIZE_MAX - 1),
+		iwl_read_prph(trans, SCD_QUEUE_WRPTR(txq->q.id)));
+
+	iwl_op_mode_nic_error(trans->op_mode);
+}
+
 static int iwl_trans_txq_alloc(struct iwl_trans *trans,
 				struct iwl_tx_queue *txq, int slots_num,
 				u32 txq_id)
@@ -307,40 +336,31 @@ static int iwl_trans_txq_alloc(struct iwl_trans *trans,
 	int i;
 	struct iwl_trans_pcie *trans_pcie = IWL_TRANS_GET_PCIE_TRANS(trans);
 
-	if (WARN_ON(txq->meta || txq->cmd || txq->skbs || txq->tfds))
+	if (WARN_ON(txq->entries || txq->tfds))
 		return -EINVAL;
 
+	setup_timer(&txq->stuck_timer, iwl_trans_pcie_queue_stuck_timer,
+		    (unsigned long)txq);
+	txq->trans_pcie = trans_pcie;
+
 	txq->q.n_window = slots_num;
 
-	txq->meta = kcalloc(slots_num, sizeof(txq->meta[0]), GFP_KERNEL);
-	txq->cmd = kcalloc(slots_num, sizeof(txq->cmd[0]), GFP_KERNEL);
+	txq->entries = kcalloc(slots_num,
+			       sizeof(struct iwl_pcie_tx_queue_entry),
+			       GFP_KERNEL);
 
-	if (!txq->meta || !txq->cmd)
+	if (!txq->entries)
 		goto error;
 
 	if (txq_id == trans_pcie->cmd_queue)
 		for (i = 0; i < slots_num; i++) {
-			txq->cmd[i] = kmalloc(sizeof(struct iwl_device_cmd),
-						GFP_KERNEL);
-			if (!txq->cmd[i])
+			txq->entries[i].cmd =
+				kmalloc(sizeof(struct iwl_device_cmd),
+					GFP_KERNEL);
+			if (!txq->entries[i].cmd)
 				goto error;
 		}
 
-	/* Alloc driver data array and TFD circular buffer */
-	/* Driver private data, only for Tx (not command) queues,
-	 * not shared with device. */
-	if (txq_id != trans_pcie->cmd_queue) {
-		txq->skbs = kcalloc(TFD_QUEUE_SIZE_MAX, sizeof(txq->skbs[0]),
-				    GFP_KERNEL);
-		if (!txq->skbs) {
-			IWL_ERR(trans, "kmalloc for auxiliary BD "
-				  "structures failed\n");
-			goto error;
-		}
-	} else {
-		txq->skbs = NULL;
-	}
-
 	/* Circular buffer of transmit frame descriptors (TFDs),
 	 * shared with device */
 	txq->tfds = dma_alloc_coherent(trans->dev, tfd_sz,
@@ -353,37 +373,22 @@ static int iwl_trans_txq_alloc(struct iwl_trans *trans,
 
 	return 0;
 error:
-	kfree(txq->skbs);
-	txq->skbs = NULL;
-	/* since txq->cmd has been zeroed,
-	 * all non allocated cmd[i] will be NULL */
-	if (txq->cmd && txq_id == trans_pcie->cmd_queue)
+	if (txq->entries && txq_id == trans_pcie->cmd_queue)
 		for (i = 0; i < slots_num; i++)
-			kfree(txq->cmd[i]);
-	kfree(txq->meta);
-	kfree(txq->cmd);
-	txq->meta = NULL;
-	txq->cmd = NULL;
+			kfree(txq->entries[i].cmd);
+	kfree(txq->entries);
+	txq->entries = NULL;
 
 	return -ENOMEM;
 
 }
 
 static int iwl_trans_txq_init(struct iwl_trans *trans, struct iwl_tx_queue *txq,
-		      int slots_num, u32 txq_id)
+			      int slots_num, u32 txq_id)
 {
 	int ret;
 
 	txq->need_update = 0;
-	memset(txq->meta, 0, sizeof(txq->meta[0]) * slots_num);
-
-	/*
-	 * For the default queues 0-3, set up the swq_id
-	 * already -- all others need to get one later
-	 * (if they need one at all).
-	 */
-	if (txq_id < 4)
-		iwl_set_swq_id(txq, txq_id, txq_id);
 
 	/* TFD_QUEUE_SIZE_MAX must be power-of-two size, otherwise
 	 * iwl_queue_inc_wrap and iwl_queue_dec_wrap are broken. */
@@ -461,7 +466,7 @@ static void iwl_tx_queue_free(struct iwl_trans *trans, int txq_id)
 
 	if (txq_id == trans_pcie->cmd_queue)
 		for (i = 0; i < txq->q.n_window; i++)
-			kfree(txq->cmd[i]);
+			kfree(txq->entries[i].cmd);
 
 	/* De-alloc circular buffer of TFDs */
 	if (txq->q.n_bd) {
@@ -470,15 +475,10 @@ static void iwl_tx_queue_free(struct iwl_trans *trans, int txq_id)
 		memset(&txq->q.dma_addr, 0, sizeof(txq->q.dma_addr));
 	}
 
-	/* De-alloc array of per-TFD driver data */
-	kfree(txq->skbs);
-	txq->skbs = NULL;
+	kfree(txq->entries);
+	txq->entries = NULL;
 
-	/* deallocate arrays */
-	kfree(txq->cmd);
-	kfree(txq->meta);
-	txq->cmd = NULL;
-	txq->meta = NULL;
+	del_timer_sync(&txq->stuck_timer);
 
 	/* 0-fill queue descriptor structure */
 	memset(txq, 0, sizeof(*txq));
@@ -497,7 +497,7 @@ static void iwl_trans_pcie_tx_free(struct iwl_trans *trans)
 	/* Tx queues */
 	if (trans_pcie->txq) {
 		for (txq_id = 0;
-		     txq_id < cfg(trans)->base_params->num_of_queues; txq_id++)
+		     txq_id < trans->cfg->base_params->num_of_queues; txq_id++)
 			iwl_tx_queue_free(trans, txq_id);
 	}
 
@@ -522,7 +522,7 @@ static int iwl_trans_tx_alloc(struct iwl_trans *trans)
 	int txq_id, slots_num;
 	struct iwl_trans_pcie *trans_pcie = IWL_TRANS_GET_PCIE_TRANS(trans);
 
-	u16 scd_bc_tbls_size = cfg(trans)->base_params->num_of_queues *
+	u16 scd_bc_tbls_size = trans->cfg->base_params->num_of_queues *
 			sizeof(struct iwlagn_scd_bc_tbl);
 
 	/*It is not allowed to alloc twice, so warn when this happens.
@@ -546,7 +546,7 @@ static int iwl_trans_tx_alloc(struct iwl_trans *trans)
 		goto error;
 	}
 
-	trans_pcie->txq = kcalloc(cfg(trans)->base_params->num_of_queues,
+	trans_pcie->txq = kcalloc(trans->cfg->base_params->num_of_queues,
 				  sizeof(struct iwl_tx_queue), GFP_KERNEL);
 	if (!trans_pcie->txq) {
 		IWL_ERR(trans, "Not enough memory for txq\n");
@@ -555,7 +555,7 @@ static int iwl_trans_tx_alloc(struct iwl_trans *trans)
 	}
 
 	/* Alloc and init all Tx queues, including the command queue (#4/#9) */
-	for (txq_id = 0; txq_id < cfg(trans)->base_params->num_of_queues;
+	for (txq_id = 0; txq_id < trans->cfg->base_params->num_of_queues;
 	     txq_id++) {
 		slots_num = (txq_id == trans_pcie->cmd_queue) ?
 					TFD_CMD_SLOTS : TFD_TX_CMD_SLOTS;
@@ -601,7 +601,7 @@ static int iwl_tx_init(struct iwl_trans *trans)
 	spin_unlock_irqrestore(&trans_pcie->irq_lock, flags);
 
 	/* Alloc and init all Tx queues, including the command queue (#4/#9) */
-	for (txq_id = 0; txq_id < cfg(trans)->base_params->num_of_queues;
+	for (txq_id = 0; txq_id < trans->cfg->base_params->num_of_queues;
 	     txq_id++) {
 		slots_num = (txq_id == trans_pcie->cmd_queue) ?
 					TFD_CMD_SLOTS : TFD_TX_CMD_SLOTS;
@@ -724,9 +724,9 @@ static int iwl_apm_init(struct iwl_trans *trans)
 	iwl_apm_config(trans);
 
 	/* Configure analog phase-lock-loop before activating to D0A */
-	if (cfg(trans)->base_params->pll_cfg_val)
+	if (trans->cfg->base_params->pll_cfg_val)
 		iwl_set_bit(trans, CSR_ANA_PLL_CFG,
-			    cfg(trans)->base_params->pll_cfg_val);
+			    trans->cfg->base_params->pll_cfg_val);
 
 	/*
 	 * Set "initialization complete" bit to move adapter from
@@ -836,7 +836,7 @@ static int iwl_nic_init(struct iwl_trans *trans)
 	if (iwl_tx_init(trans))
 		return -ENOMEM;
 
-	if (cfg(trans)->base_params->shadow_reg_enable) {
+	if (trans->cfg->base_params->shadow_reg_enable) {
 		/* enable shadow regs in HW */
 		iwl_set_bit(trans, CSR_MAC_SHADOW_REG_CTRL,
 			0x800FFFFF);
@@ -895,59 +895,6 @@ static int iwl_prepare_card_hw(struct iwl_trans *trans)
 	return ret;
 }
 
-#define IWL_AC_UNSET -1
-
-struct queue_to_fifo_ac {
-	s8 fifo, ac;
-};
-
-static const struct queue_to_fifo_ac iwlagn_default_queue_to_tx_fifo[] = {
-	{ IWL_TX_FIFO_VO, IEEE80211_AC_VO, },
-	{ IWL_TX_FIFO_VI, IEEE80211_AC_VI, },
-	{ IWL_TX_FIFO_BE, IEEE80211_AC_BE, },
-	{ IWL_TX_FIFO_BK, IEEE80211_AC_BK, },
-	{ IWLAGN_CMD_FIFO_NUM, IWL_AC_UNSET, },
-	{ IWL_TX_FIFO_UNUSED, IWL_AC_UNSET, },
-	{ IWL_TX_FIFO_UNUSED, IWL_AC_UNSET, },
-	{ IWL_TX_FIFO_UNUSED, IWL_AC_UNSET, },
-	{ IWL_TX_FIFO_UNUSED, IWL_AC_UNSET, },
-	{ IWL_TX_FIFO_UNUSED, IWL_AC_UNSET, },
-	{ IWL_TX_FIFO_UNUSED, IWL_AC_UNSET, },
-};
-
-static const struct queue_to_fifo_ac iwlagn_ipan_queue_to_tx_fifo[] = {
-	{ IWL_TX_FIFO_VO, IEEE80211_AC_VO, },
-	{ IWL_TX_FIFO_VI, IEEE80211_AC_VI, },
-	{ IWL_TX_FIFO_BE, IEEE80211_AC_BE, },
-	{ IWL_TX_FIFO_BK, IEEE80211_AC_BK, },
-	{ IWL_TX_FIFO_BK_IPAN, IEEE80211_AC_BK, },
-	{ IWL_TX_FIFO_BE_IPAN, IEEE80211_AC_BE, },
-	{ IWL_TX_FIFO_VI_IPAN, IEEE80211_AC_VI, },
-	{ IWL_TX_FIFO_VO_IPAN, IEEE80211_AC_VO, },
-	{ IWL_TX_FIFO_BE_IPAN, 2, },
-	{ IWLAGN_CMD_FIFO_NUM, IWL_AC_UNSET, },
-	{ IWL_TX_FIFO_AUX, IWL_AC_UNSET, },
-};
-
-static const u8 iwlagn_bss_ac_to_fifo[] = {
-	IWL_TX_FIFO_VO,
-	IWL_TX_FIFO_VI,
-	IWL_TX_FIFO_BE,
-	IWL_TX_FIFO_BK,
-};
-static const u8 iwlagn_bss_ac_to_queue[] = {
-	0, 1, 2, 3,
-};
-static const u8 iwlagn_pan_ac_to_fifo[] = {
-	IWL_TX_FIFO_VO_IPAN,
-	IWL_TX_FIFO_VI_IPAN,
-	IWL_TX_FIFO_BE_IPAN,
-	IWL_TX_FIFO_BK_IPAN,
-};
-static const u8 iwlagn_pan_ac_to_queue[] = {
-	7, 6, 5, 4,
-};
-
 /*
  * ucode
  */
@@ -1028,34 +975,21 @@ static int iwl_trans_pcie_start_fw(struct iwl_trans *trans,
 				   const struct fw_img *fw)
 {
 	int ret;
-	struct iwl_trans_pcie *trans_pcie =
-		IWL_TRANS_GET_PCIE_TRANS(trans);
 	bool hw_rfkill;
 
-	trans_pcie->ac_to_queue[IWL_RXON_CTX_BSS] = iwlagn_bss_ac_to_queue;
-	trans_pcie->ac_to_queue[IWL_RXON_CTX_PAN] = iwlagn_pan_ac_to_queue;
-
-	trans_pcie->ac_to_fifo[IWL_RXON_CTX_BSS] = iwlagn_bss_ac_to_fifo;
-	trans_pcie->ac_to_fifo[IWL_RXON_CTX_PAN] = iwlagn_pan_ac_to_fifo;
-
-	trans_pcie->mcast_queue[IWL_RXON_CTX_BSS] = 0;
-	trans_pcie->mcast_queue[IWL_RXON_CTX_PAN] = IWL_IPAN_MCAST_QUEUE;
-
 	/* This may fail if AMT took ownership of the device */
 	if (iwl_prepare_card_hw(trans)) {
 		IWL_WARN(trans, "Exit HW not ready\n");
 		return -EIO;
 	}
 
+	iwl_enable_rfkill_int(trans);
+
 	/* If platform's RF_KILL switch is NOT set to KILL */
-	hw_rfkill = !(iwl_read32(trans, CSR_GP_CNTRL) &
-				CSR_GP_CNTRL_REG_FLAG_HW_RF_KILL_SW);
+	hw_rfkill = iwl_is_rfkill_set(trans);
 	iwl_op_mode_hw_rf_kill(trans->op_mode, hw_rfkill);
-
-	if (hw_rfkill) {
-		iwl_enable_rfkill_int(trans);
+	if (hw_rfkill)
 		return -ERFKILL;
-	}
 
 	iwl_write32(trans, CSR_INT, 0xFFFFFFFF);
 
@@ -1098,9 +1032,7 @@ static void iwl_trans_txq_set_sched(struct iwl_trans *trans, u32 mask)
 
 static void iwl_tx_start(struct iwl_trans *trans)
 {
-	const struct queue_to_fifo_ac *queue_to_fifo;
-	struct iwl_trans_pcie *trans_pcie =
-		IWL_TRANS_GET_PCIE_TRANS(trans);
+	struct iwl_trans_pcie *trans_pcie = IWL_TRANS_GET_PCIE_TRANS(trans);
 	u32 a;
 	unsigned long flags;
 	int i, chan;
@@ -1121,7 +1053,7 @@ static void iwl_tx_start(struct iwl_trans *trans)
 		iwl_write_targ_mem(trans, a, 0);
 	for (; a < trans_pcie->scd_base_addr +
 	       SCD_TRANS_TBL_OFFSET_QUEUE(
-				cfg(trans)->base_params->num_of_queues);
+				trans->cfg->base_params->num_of_queues);
 	       a += 4)
 		iwl_write_targ_mem(trans, a, 0);
 
@@ -1144,7 +1076,7 @@ static void iwl_tx_start(struct iwl_trans *trans)
 	iwl_write_prph(trans, SCD_AGGR_SEL, 0);
 
 	/* initiate the queues */
-	for (i = 0; i < cfg(trans)->base_params->num_of_queues; i++) {
+	for (i = 0; i < trans->cfg->base_params->num_of_queues; i++) {
 		iwl_write_prph(trans, SCD_QUEUE_RDPTR(i), 0);
 		iwl_write_direct32(trans, HBUS_TARG_WRPTR, 0 | (i << 8));
 		iwl_write_targ_mem(trans, trans_pcie->scd_base_addr +
@@ -1161,46 +1093,24 @@ static void iwl_tx_start(struct iwl_trans *trans)
 	}
 
 	iwl_write_prph(trans, SCD_INTERRUPT_MASK,
-			IWL_MASK(0, cfg(trans)->base_params->num_of_queues));
+			IWL_MASK(0, trans->cfg->base_params->num_of_queues));
 
 	/* Activate all Tx DMA/FIFO channels */
 	iwl_trans_txq_set_sched(trans, IWL_MASK(0, 7));
 
-	/* map queues to FIFOs */
-	if (trans->shrd->valid_contexts != BIT(IWL_RXON_CTX_BSS))
-		queue_to_fifo = iwlagn_ipan_queue_to_tx_fifo;
-	else
-		queue_to_fifo = iwlagn_default_queue_to_tx_fifo;
-
 	iwl_trans_set_wr_ptrs(trans, trans_pcie->cmd_queue, 0);
 
-	/* make sure all queue are not stopped */
-	memset(&trans_pcie->queue_stopped[0], 0,
-		sizeof(trans_pcie->queue_stopped));
-	for (i = 0; i < 4; i++)
-		atomic_set(&trans_pcie->queue_stop_count[i], 0);
-
-	/* reset to 0 to enable all the queue first */
-	trans_pcie->txq_ctx_active_msk = 0;
+	/* make sure all queue are not stopped/used */
+	memset(trans_pcie->queue_stopped, 0, sizeof(trans_pcie->queue_stopped));
+	memset(trans_pcie->queue_used, 0, sizeof(trans_pcie->queue_used));
 
-	BUILD_BUG_ON(ARRAY_SIZE(iwlagn_default_queue_to_tx_fifo) <
-						IWLAGN_FIRST_AMPDU_QUEUE);
-	BUILD_BUG_ON(ARRAY_SIZE(iwlagn_ipan_queue_to_tx_fifo) <
-						IWLAGN_FIRST_AMPDU_QUEUE);
+	for (i = 0; i < trans_pcie->n_q_to_fifo; i++) {
+		int fifo = trans_pcie->setup_q_to_fifo[i];
 
-	for (i = 0; i < IWLAGN_FIRST_AMPDU_QUEUE; i++) {
-		int fifo = queue_to_fifo[i].fifo;
-		int ac = queue_to_fifo[i].ac;
+		set_bit(i, trans_pcie->queue_used);
 
-		iwl_txq_ctx_activate(trans_pcie, i);
-
-		if (fifo == IWL_TX_FIFO_UNUSED)
-			continue;
-
-		if (ac != IWL_AC_UNSET)
-			iwl_set_swq_id(&trans_pcie->txq[i], ac, i);
 		iwl_trans_tx_queue_set_status(trans, &trans_pcie->txq[i],
-					      fifo, 0);
+					      fifo, true);
 	}
 
 	spin_unlock_irqrestore(&trans_pcie->irq_lock, flags);
@@ -1251,7 +1161,7 @@ static int iwl_trans_tx_stop(struct iwl_trans *trans)
 	}
 
 	/* Unmap DMA from host system and free skb's */
-	for (txq_id = 0; txq_id < cfg(trans)->base_params->num_of_queues;
+	for (txq_id = 0; txq_id < trans->cfg->base_params->num_of_queues;
 	     txq_id++)
 		iwl_tx_queue_unmap(trans, txq_id);
 
@@ -1303,6 +1213,8 @@ static void iwl_trans_pcie_stop_device(struct iwl_trans *trans)
 	iwl_disable_interrupts(trans);
 	spin_unlock_irqrestore(&trans_pcie->irq_lock, flags);
 
+	iwl_enable_rfkill_int(trans);
+
 	/* wait to make sure we flush pending tasklet*/
 	synchronize_irq(trans_pcie->irq);
 	tasklet_kill(&trans_pcie->irq_tasklet);
@@ -1311,6 +1223,12 @@ static void iwl_trans_pcie_stop_device(struct iwl_trans *trans)
 
 	/* stop and reset the on-board processor */
 	iwl_write32(trans, CSR_RESET, CSR_RESET_REG_FLAG_NEVO_RESET);
+
+	/* clear all status bits */
+	clear_bit(STATUS_HCMD_ACTIVE, &trans_pcie->status);
+	clear_bit(STATUS_INT_ENABLED, &trans_pcie->status);
+	clear_bit(STATUS_DEVICE_ENABLED, &trans_pcie->status);
+	clear_bit(STATUS_TPOWER_PMI, &trans_pcie->status);
 }
 
 static void iwl_trans_pcie_wowlan_suspend(struct iwl_trans *trans)
@@ -1325,81 +1243,43 @@ static void iwl_trans_pcie_wowlan_suspend(struct iwl_trans *trans)
 }
 
 static int iwl_trans_pcie_tx(struct iwl_trans *trans, struct sk_buff *skb,
-		struct iwl_device_cmd *dev_cmd, enum iwl_rxon_context_id ctx,
-		u8 sta_id, u8 tid)
+			     struct iwl_device_cmd *dev_cmd, int txq_id)
 {
 	struct iwl_trans_pcie *trans_pcie = IWL_TRANS_GET_PCIE_TRANS(trans);
 	struct ieee80211_hdr *hdr = (struct ieee80211_hdr *)skb->data;
-	struct ieee80211_tx_info *info = IEEE80211_SKB_CB(skb);
 	struct iwl_tx_cmd *tx_cmd = (struct iwl_tx_cmd *) dev_cmd->payload;
 	struct iwl_cmd_meta *out_meta;
 	struct iwl_tx_queue *txq;
 	struct iwl_queue *q;
-
 	dma_addr_t phys_addr = 0;
 	dma_addr_t txcmd_phys;
 	dma_addr_t scratch_phys;
 	u16 len, firstlen, secondlen;
 	u8 wait_write_ptr = 0;
-	u8 txq_id;
-	bool is_agg = false;
 	__le16 fc = hdr->frame_control;
 	u8 hdr_len = ieee80211_hdrlen(fc);
 	u16 __maybe_unused wifi_seq;
 
-	/*
-	 * Send this frame after DTIM -- there's a special queue
-	 * reserved for this for contexts that support AP mode.
-	 */
-	if (info->flags & IEEE80211_TX_CTL_SEND_AFTER_DTIM) {
-		txq_id = trans_pcie->mcast_queue[ctx];
-
-		/*
-		 * The microcode will clear the more data
-		 * bit in the last frame it transmits.
-		 */
-		hdr->frame_control |=
-			cpu_to_le16(IEEE80211_FCTL_MOREDATA);
-	} else if (info->flags & IEEE80211_TX_CTL_TX_OFFCHAN)
-		txq_id = IWL_AUX_QUEUE;
-	else
-		txq_id =
-		    trans_pcie->ac_to_queue[ctx][skb_get_queue_mapping(skb)];
-
-	/* aggregation is on for this <sta,tid> */
-	if (info->flags & IEEE80211_TX_CTL_AMPDU) {
-		WARN_ON(tid >= IWL_MAX_TID_COUNT);
-		txq_id = trans_pcie->agg_txq[sta_id][tid];
-		is_agg = true;
-	}
-
 	txq = &trans_pcie->txq[txq_id];
 	q = &txq->q;
 
-	spin_lock(&txq->lock);
+	if (unlikely(!test_bit(txq_id, trans_pcie->queue_used))) {
+		WARN_ON_ONCE(1);
+		return -EINVAL;
+	}
 
-	/* In AGG mode, the index in the ring must correspond to the WiFi
-	 * sequence number. This is a HW requirements to help the SCD to parse
-	 * the BA.
-	 * 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));
-	WARN_ONCE(is_agg && ((wifi_seq & 0xff) != q->write_ptr),
-		  "Q: %d WiFi Seq %d tfdNum %d",
-		  txq_id, wifi_seq, q->write_ptr);
-#endif
+	spin_lock(&txq->lock);
 
 	/* Set up driver data for this TFD */
-	txq->skbs[q->write_ptr] = skb;
-	txq->cmd[q->write_ptr] = dev_cmd;
+	txq->entries[q->write_ptr].skb = skb;
+	txq->entries[q->write_ptr].cmd = dev_cmd;
 
 	dev_cmd->hdr.cmd = REPLY_TX;
 	dev_cmd->hdr.sequence = cpu_to_le16((u16)(QUEUE_TO_SEQ(txq_id) |
 				INDEX_TO_SEQ(q->write_ptr)));
 
 	/* Set up first empty entry in queue's array of Tx/cmd buffers */
-	out_meta = &txq->meta[q->write_ptr];
+	out_meta = &txq->entries[q->write_ptr].meta;
 
 	/*
 	 * Use the first empty entry in this queue's command buffer array
@@ -1481,6 +1361,10 @@ static int iwl_trans_pcie_tx(struct iwl_trans *trans, struct sk_buff *skb,
 			     &dev_cmd->hdr, firstlen,
 			     skb->data + hdr_len, secondlen);
 
+	/* start timer if queue currently empty */
+	if (q->read_ptr == q->write_ptr && trans_pcie->wd_timeout)
+		mod_timer(&txq->stuck_timer, jiffies + trans_pcie->wd_timeout);
+
 	/* Tell device the write index *just past* this latest filled TFD */
 	q->write_ptr = iwl_queue_inc_wrap(q->write_ptr, q->n_bd);
 	iwl_txq_update_write_ptr(trans, txq);
@@ -1541,8 +1425,10 @@ static int iwl_trans_pcie_start_hw(struct iwl_trans *trans)
 
 	iwl_apm_init(trans);
 
-	hw_rfkill = !(iwl_read32(trans, CSR_GP_CNTRL) &
-				CSR_GP_CNTRL_REG_FLAG_HW_RF_KILL_SW);
+	/* From now on, the op_mode will be kept updated about RF kill state */
+	iwl_enable_rfkill_int(trans);
+
+	hw_rfkill = iwl_is_rfkill_set(trans);
 	iwl_op_mode_hw_rf_kill(trans->op_mode, hw_rfkill);
 
 	return err;
@@ -1555,18 +1441,41 @@ error:
 	return err;
 }
 
-static void iwl_trans_pcie_stop_hw(struct iwl_trans *trans)
+static void iwl_trans_pcie_stop_hw(struct iwl_trans *trans,
+				   bool op_mode_leaving)
 {
+	bool hw_rfkill;
+	unsigned long flags;
+	struct iwl_trans_pcie *trans_pcie = IWL_TRANS_GET_PCIE_TRANS(trans);
+
 	iwl_apm_stop(trans);
 
+	spin_lock_irqsave(&trans_pcie->irq_lock, flags);
+	iwl_disable_interrupts(trans);
+	spin_unlock_irqrestore(&trans_pcie->irq_lock, flags);
+
 	iwl_write32(trans, CSR_INT, 0xFFFFFFFF);
 
-	/* Even if we stop the HW, we still want the RF kill interrupt */
-	iwl_enable_rfkill_int(trans);
+	if (!op_mode_leaving) {
+		/*
+		 * Even if we stop the HW, we still want the RF kill
+		 * interrupt
+		 */
+		iwl_enable_rfkill_int(trans);
+
+		/*
+		 * Check again since the RF kill state may have changed while
+		 * all the interrupts were disabled, in this case we couldn't
+		 * receive the RF kill interrupt and update the state in the
+		 * op_mode.
+		 */
+		hw_rfkill = iwl_is_rfkill_set(trans);
+		iwl_op_mode_hw_rf_kill(trans->op_mode, hw_rfkill);
+	}
 }
 
-static int iwl_trans_pcie_reclaim(struct iwl_trans *trans, int sta_id, int tid,
-		      int txq_id, int ssn, struct sk_buff_head *skbs)
+static void iwl_trans_pcie_reclaim(struct iwl_trans *trans, int txq_id, int ssn,
+				   struct sk_buff_head *skbs)
 {
 	struct iwl_trans_pcie *trans_pcie = IWL_TRANS_GET_PCIE_TRANS(trans);
 	struct iwl_tx_queue *txq = &trans_pcie->txq[txq_id];
@@ -1576,35 +1485,15 @@ static int iwl_trans_pcie_reclaim(struct iwl_trans *trans, int sta_id, int tid,
 
 	spin_lock(&txq->lock);
 
-	txq->time_stamp = jiffies;
-
-	if (unlikely(txq_id >= IWLAGN_FIRST_AMPDU_QUEUE &&
-		     tid != IWL_TID_NON_QOS &&
-		     txq_id != trans_pcie->agg_txq[sta_id][tid])) {
-		/*
-		 * FIXME: this is a uCode bug which need to be addressed,
-		 * log the information and return for now.
-		 * Since it is can possibly happen very often and in order
-		 * not to fill the syslog, don't use IWL_ERR or IWL_WARN
-		 */
-		IWL_DEBUG_TX_QUEUES(trans, "Bad queue mapping txq_id %d, "
-			"agg_txq[sta_id[tid] %d", txq_id,
-			trans_pcie->agg_txq[sta_id][tid]);
-		spin_unlock(&txq->lock);
-		return 1;
-	}
-
 	if (txq->q.read_ptr != tfd_num) {
-		IWL_DEBUG_TX_REPLY(trans, "[Q %d | AC %d] %d -> %d (%d)\n",
-				txq_id, iwl_get_queue_ac(txq), txq->q.read_ptr,
-				tfd_num, ssn);
+		IWL_DEBUG_TX_REPLY(trans, "[Q %d] %d -> %d (%d)\n",
+				   txq_id, txq->q.read_ptr, tfd_num, ssn);
 		freed = iwl_tx_queue_reclaim(trans, txq_id, tfd_num, skbs);
 		if (iwl_queue_space(&txq->q) > txq->q.low_mark)
 			iwl_wake_queue(trans, txq);
 	}
 
 	spin_unlock(&txq->lock);
-	return 0;
 }
 
 static void iwl_trans_pcie_write8(struct iwl_trans *trans, u32 ofs, u8 val)
@@ -1623,7 +1512,7 @@ static u32 iwl_trans_pcie_read32(struct iwl_trans *trans, u32 ofs)
 }
 
 static void iwl_trans_pcie_configure(struct iwl_trans *trans,
-			      const struct iwl_trans_config *trans_cfg)
+				     const struct iwl_trans_config *trans_cfg)
 {
 	struct iwl_trans_pcie *trans_pcie = IWL_TRANS_GET_PCIE_TRANS(trans);
 
@@ -1635,9 +1524,31 @@ static void iwl_trans_pcie_configure(struct iwl_trans *trans,
 	if (trans_pcie->n_no_reclaim_cmds)
 		memcpy(trans_pcie->no_reclaim_cmds, trans_cfg->no_reclaim_cmds,
 		       trans_pcie->n_no_reclaim_cmds * sizeof(u8));
+
+	trans_pcie->n_q_to_fifo = trans_cfg->n_queue_to_fifo;
+
+	if (WARN_ON(trans_pcie->n_q_to_fifo > IWL_MAX_HW_QUEUES))
+		trans_pcie->n_q_to_fifo = IWL_MAX_HW_QUEUES;
+
+	/* at least the command queue must be mapped */
+	WARN_ON(!trans_pcie->n_q_to_fifo);
+
+	memcpy(trans_pcie->setup_q_to_fifo, trans_cfg->queue_to_fifo,
+	       trans_pcie->n_q_to_fifo * sizeof(u8));
+
+	trans_pcie->rx_buf_size_8k = trans_cfg->rx_buf_size_8k;
+	if (trans_pcie->rx_buf_size_8k)
+		trans_pcie->rx_page_order = get_order(8 * 1024);
+	else
+		trans_pcie->rx_page_order = get_order(4 * 1024);
+
+	trans_pcie->wd_timeout =
+		msecs_to_jiffies(trans_cfg->queue_watchdog_timeout);
+
+	trans_pcie->command_names = trans_cfg->command_names;
 }
 
-static void iwl_trans_pcie_free(struct iwl_trans *trans)
+void iwl_trans_pcie_free(struct iwl_trans *trans)
 {
 	struct iwl_trans_pcie *trans_pcie =
 		IWL_TRANS_GET_PCIE_TRANS(trans);
@@ -1656,10 +1567,19 @@ static void iwl_trans_pcie_free(struct iwl_trans *trans)
 	pci_release_regions(trans_pcie->pci_dev);
 	pci_disable_device(trans_pcie->pci_dev);
 
-	trans->shrd->trans = NULL;
 	kfree(trans);
 }
 
+static void iwl_trans_pcie_set_pmi(struct iwl_trans *trans, bool state)
+{
+	struct iwl_trans_pcie *trans_pcie = IWL_TRANS_GET_PCIE_TRANS(trans);
+
+	if (state)
+		set_bit(STATUS_TPOWER_PMI, &trans_pcie->status);
+	else
+		clear_bit(STATUS_TPOWER_PMI, &trans_pcie->status);
+}
+
 #ifdef CONFIG_PM_SLEEP
 static int iwl_trans_pcie_suspend(struct iwl_trans *trans)
 {
@@ -1670,16 +1590,14 @@ static int iwl_trans_pcie_resume(struct iwl_trans *trans)
 {
 	bool hw_rfkill;
 
-	hw_rfkill = !(iwl_read32(trans, CSR_GP_CNTRL) &
-				CSR_GP_CNTRL_REG_FLAG_HW_RF_KILL_SW);
-
-	if (hw_rfkill)
-		iwl_enable_rfkill_int(trans);
-	else
-		iwl_enable_interrupts(trans);
+	iwl_enable_rfkill_int(trans);
 
+	hw_rfkill = iwl_is_rfkill_set(trans);
 	iwl_op_mode_hw_rf_kill(trans->op_mode, hw_rfkill);
 
+	if (!hw_rfkill)
+		iwl_enable_interrupts(trans);
+
 	return 0;
 }
 #endif /* CONFIG_PM_SLEEP */
@@ -1696,7 +1614,7 @@ static int iwl_trans_pcie_wait_tx_queue_empty(struct iwl_trans *trans)
 	int ret = 0;
 
 	/* waiting for all the tx frames complete might take a while */
-	for (cnt = 0; cnt < cfg(trans)->base_params->num_of_queues; cnt++) {
+	for (cnt = 0; cnt < trans->cfg->base_params->num_of_queues; cnt++) {
 		if (cnt == trans_pcie->cmd_queue)
 			continue;
 		txq = &trans_pcie->txq[cnt];
@@ -1714,42 +1632,9 @@ static int iwl_trans_pcie_wait_tx_queue_empty(struct iwl_trans *trans)
 	return ret;
 }
 
-/*
- * On every watchdog tick we check (latest) time stamp. If it does not
- * change during timeout period and queue is not empty we reset firmware.
- */
-static int iwl_trans_pcie_check_stuck_queue(struct iwl_trans *trans, int cnt)
-{
-	struct iwl_trans_pcie *trans_pcie = IWL_TRANS_GET_PCIE_TRANS(trans);
-	struct iwl_tx_queue *txq = &trans_pcie->txq[cnt];
-	struct iwl_queue *q = &txq->q;
-	unsigned long timeout;
-
-	if (q->read_ptr == q->write_ptr) {
-		txq->time_stamp = jiffies;
-		return 0;
-	}
-
-	timeout = txq->time_stamp +
-		  msecs_to_jiffies(hw_params(trans).wd_timeout);
-
-	if (time_after(jiffies, timeout)) {
-		IWL_ERR(trans, "Queue %d stuck for %u ms.\n", q->id,
-			hw_params(trans).wd_timeout);
-		IWL_ERR(trans, "Current SW read_ptr %d write_ptr %d\n",
-			q->read_ptr, q->write_ptr);
-		IWL_ERR(trans, "Current HW read_ptr %d write_ptr %d\n",
-			iwl_read_prph(trans, SCD_QUEUE_RDPTR(cnt))
-				& (TFD_QUEUE_SIZE_MAX - 1),
-			iwl_read_prph(trans, SCD_QUEUE_WRPTR(cnt)));
-		return 1;
-	}
-
-	return 0;
-}
-
 static const char *get_fh_string(int cmd)
 {
+#define IWL_CMD(x) case x: return #x
 	switch (cmd) {
 	IWL_CMD(FH_RSCSR_CHNL0_STTS_WPTR_REG);
 	IWL_CMD(FH_RSCSR_CHNL0_RBDCB_BASE_REG);
@@ -1763,6 +1648,7 @@ static const char *get_fh_string(int cmd)
 	default:
 		return "UNKNOWN";
 	}
+#undef IWL_CMD
 }
 
 int iwl_dump_fh(struct iwl_trans *trans, char **buf, bool display)
@@ -1811,6 +1697,7 @@ int iwl_dump_fh(struct iwl_trans *trans, char **buf, bool display)
 
 static const char *get_csr_string(int cmd)
 {
+#define IWL_CMD(x) case x: return #x
 	switch (cmd) {
 	IWL_CMD(CSR_HW_IF_CONFIG_REG);
 	IWL_CMD(CSR_INT_COALESCING);
@@ -1838,6 +1725,7 @@ static const char *get_csr_string(int cmd)
 	default:
 		return "UNKNOWN";
 	}
+#undef IWL_CMD
 }
 
 void iwl_dump_csr(struct iwl_trans *trans)
@@ -1938,32 +1826,23 @@ static ssize_t iwl_dbgfs_tx_queue_read(struct file *file,
 	int ret;
 	size_t bufsz;
 
-	bufsz = sizeof(char) * 64 * cfg(trans)->base_params->num_of_queues;
+	bufsz = sizeof(char) * 64 * trans->cfg->base_params->num_of_queues;
 
-	if (!trans_pcie->txq) {
-		IWL_ERR(trans, "txq not ready\n");
+	if (!trans_pcie->txq)
 		return -EAGAIN;
-	}
+
 	buf = kzalloc(bufsz, GFP_KERNEL);
 	if (!buf)
 		return -ENOMEM;
 
-	for (cnt = 0; cnt < cfg(trans)->base_params->num_of_queues; cnt++) {
+	for (cnt = 0; cnt < trans->cfg->base_params->num_of_queues; cnt++) {
 		txq = &trans_pcie->txq[cnt];
 		q = &txq->q;
 		pos += scnprintf(buf + pos, bufsz - pos,
-				"hwq %.2d: read=%u write=%u stop=%d"
-				" swq_id=%#.2x (ac %d/hwq %d)\n",
+				"hwq %.2d: read=%u write=%u use=%d stop=%d\n",
 				cnt, q->read_ptr, q->write_ptr,
-				!!test_bit(cnt, trans_pcie->queue_stopped),
-				txq->swq_id, txq->swq_id & 3,
-				(txq->swq_id >> 2) & 0x1f);
-		if (cnt >= 4)
-			continue;
-		/* for the ACs, display the stop count too */
-		pos += scnprintf(buf + pos, bufsz - pos,
-			"        stop-count: %d\n",
-			atomic_read(&trans_pcie->queue_stop_count[cnt]));
+				!!test_bit(cnt, trans_pcie->queue_used),
+				!!test_bit(cnt, trans_pcie->queue_stopped));
 	}
 	ret = simple_read_from_buffer(user_buf, count, ppos, buf, pos);
 	kfree(buf);
@@ -1997,44 +1876,6 @@ static ssize_t iwl_dbgfs_rx_queue_read(struct file *file,
 	return simple_read_from_buffer(user_buf, count, ppos, buf, pos);
 }
 
-static ssize_t iwl_dbgfs_log_event_read(struct file *file,
-					 char __user *user_buf,
-					 size_t count, loff_t *ppos)
-{
-	struct iwl_trans *trans = file->private_data;
-	char *buf;
-	int pos = 0;
-	ssize_t ret = -ENOMEM;
-
-	ret = pos = iwl_dump_nic_event_log(trans, true, &buf, true);
-	if (buf) {
-		ret = simple_read_from_buffer(user_buf, count, ppos, buf, pos);
-		kfree(buf);
-	}
-	return ret;
-}
-
-static ssize_t iwl_dbgfs_log_event_write(struct file *file,
-					const char __user *user_buf,
-					size_t count, loff_t *ppos)
-{
-	struct iwl_trans *trans = file->private_data;
-	u32 event_log_flag;
-	char buf[8];
-	int buf_size;
-
-	memset(buf, 0, sizeof(buf));
-	buf_size = min(count, sizeof(buf) -  1);
-	if (copy_from_user(buf, user_buf, buf_size))
-		return -EFAULT;
-	if (sscanf(buf, "%d", &event_log_flag) != 1)
-		return -EFAULT;
-	if (event_log_flag == 1)
-		iwl_dump_nic_event_log(trans, true, NULL, false);
-
-	return count;
-}
-
 static ssize_t iwl_dbgfs_interrupt_read(struct file *file,
 					char __user *user_buf,
 					size_t count, loff_t *ppos) {
@@ -2050,10 +1891,8 @@ static ssize_t iwl_dbgfs_interrupt_read(struct file *file,
 	ssize_t ret;
 
 	buf = kzalloc(bufsz, GFP_KERNEL);
-	if (!buf) {
-		IWL_ERR(trans, "Can not allocate Buffer\n");
+	if (!buf)
 		return -ENOMEM;
-	}
 
 	pos += scnprintf(buf + pos, bufsz - pos,
 			"Interrupt Statistics Report:\n");
@@ -2161,12 +2000,26 @@ static ssize_t iwl_dbgfs_fh_reg_read(struct file *file,
 	return ret;
 }
 
-DEBUGFS_READ_WRITE_FILE_OPS(log_event);
+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;
+
+	iwl_op_mode_nic_error(trans->op_mode);
+
+	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
@@ -2177,10 +2030,10 @@ static int iwl_trans_pcie_dbgfs_register(struct iwl_trans *trans,
 {
 	DEBUGFS_ADD_FILE(rx_queue, dir, S_IRUSR);
 	DEBUGFS_ADD_FILE(tx_queue, dir, S_IRUSR);
-	DEBUGFS_ADD_FILE(log_event, dir, S_IWUSR | S_IRUSR);
 	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;
 }
 #else
@@ -2190,7 +2043,7 @@ static int iwl_trans_pcie_dbgfs_register(struct iwl_trans *trans,
 
 #endif /*CONFIG_IWLWIFI_DEBUGFS */
 
-const struct iwl_trans_ops trans_ops_pcie = {
+static const struct iwl_trans_ops trans_ops_pcie = {
 	.start_hw = iwl_trans_pcie_start_hw,
 	.stop_hw = iwl_trans_pcie_stop_hw,
 	.fw_alive = iwl_trans_pcie_fw_alive,
@@ -2205,15 +2058,11 @@ const struct iwl_trans_ops trans_ops_pcie = {
 	.reclaim = iwl_trans_pcie_reclaim,
 
 	.tx_agg_disable = iwl_trans_pcie_tx_agg_disable,
-	.tx_agg_alloc = iwl_trans_pcie_tx_agg_alloc,
 	.tx_agg_setup = iwl_trans_pcie_tx_agg_setup,
 
-	.free = iwl_trans_pcie_free,
-
 	.dbgfs_register = iwl_trans_pcie_dbgfs_register,
 
 	.wait_tx_queue_empty = iwl_trans_pcie_wait_tx_queue_empty,
-	.check_stuck_queue = iwl_trans_pcie_check_stuck_queue,
 
 #ifdef CONFIG_PM_SLEEP
 	.suspend = iwl_trans_pcie_suspend,
@@ -2223,11 +2072,12 @@ const struct iwl_trans_ops trans_ops_pcie = {
 	.write32 = iwl_trans_pcie_write32,
 	.read32 = iwl_trans_pcie_read32,
 	.configure = iwl_trans_pcie_configure,
+	.set_pmi = iwl_trans_pcie_set_pmi,
 };
 
-struct iwl_trans *iwl_trans_pcie_alloc(struct iwl_shared *shrd,
-				       struct pci_dev *pdev,
-				       const struct pci_device_id *ent)
+struct iwl_trans *iwl_trans_pcie_alloc(struct pci_dev *pdev,
+				       const struct pci_device_id *ent,
+				       const struct iwl_cfg *cfg)
 {
 	struct iwl_trans_pcie *trans_pcie;
 	struct iwl_trans *trans;
@@ -2243,7 +2093,7 @@ struct iwl_trans *iwl_trans_pcie_alloc(struct iwl_shared *shrd,
 	trans_pcie = IWL_TRANS_GET_PCIE_TRANS(trans);
 
 	trans->ops = &trans_ops_pcie;
-	trans->shrd = shrd;
+	trans->cfg = cfg;
 	trans_pcie->trans = trans;
 	spin_lock_init(&trans_pcie->irq_lock);
 	init_waitqueue_head(&trans_pcie->ucode_write_waitq);
@@ -2325,6 +2175,7 @@ struct iwl_trans *iwl_trans_pcie_alloc(struct iwl_shared *shrd,
 
 	/* Initialize the wait queue for commands */
 	init_waitqueue_head(&trans->wait_command_queue);
+	spin_lock_init(&trans->reg_lock);
 
 	return trans;
 
diff --git a/drivers/net/wireless/iwlwifi/iwl-trans.h b/drivers/net/wireless/iwlwifi/iwl-trans.h
index 0c81cbaa808..79a1e7ae499 100644
--- a/drivers/net/wireless/iwlwifi/iwl-trans.h
+++ b/drivers/net/wireless/iwlwifi/iwl-trans.h
@@ -66,8 +66,9 @@
 #include <linux/ieee80211.h>
 #include <linux/mm.h> /* for page_address */
 
-#include "iwl-shared.h"
 #include "iwl-debug.h"
+#include "iwl-config.h"
+#include "iwl-fw.h"
 
 /**
  * DOC: Transport layer - what is it ?
@@ -104,13 +105,6 @@
  *	6) Eventually, the free function will be called.
  */
 
-struct iwl_priv;
-struct iwl_shared;
-struct iwl_op_mode;
-struct fw_img;
-struct sk_buff;
-struct dentry;
-
 /**
  * DOC: Host command section
  *
@@ -162,6 +156,8 @@ struct iwl_cmd_header {
 
 
 #define FH_RSCSR_FRAME_SIZE_MSK		0x00003FFF	/* bits 0-13 */
+#define FH_RSCSR_FRAME_INVALID		0x55550000
+#define FH_RSCSR_FRAME_ALIGN		0x40
 
 struct iwl_rx_packet {
 	/*
@@ -260,27 +256,43 @@ static inline void iwl_free_resp(struct iwl_host_cmd *cmd)
 
 struct iwl_rx_cmd_buffer {
 	struct page *_page;
+	int _offset;
+	bool _page_stolen;
+	unsigned int truesize;
 };
 
 static inline void *rxb_addr(struct iwl_rx_cmd_buffer *r)
 {
-	return page_address(r->_page);
+	return (void *)((unsigned long)page_address(r->_page) + r->_offset);
+}
+
+static inline int rxb_offset(struct iwl_rx_cmd_buffer *r)
+{
+	return r->_offset;
 }
 
 static inline struct page *rxb_steal_page(struct iwl_rx_cmd_buffer *r)
 {
-	struct page *p = r->_page;
-	r->_page = NULL;
-	return p;
+	r->_page_stolen = true;
+	get_page(r->_page);
+	return r->_page;
 }
 
 #define MAX_NO_RECLAIM_CMDS	6
 
+/*
+ * Maximum number of HW queues the transport layer
+ * currently supports
+ */
+#define IWL_MAX_HW_QUEUES		32
+
 /**
  * struct iwl_trans_config - transport configuration
  *
  * @op_mode: pointer to the upper layer.
- *	Must be set before any other call.
+ * @queue_to_fifo: queue to FIFO mapping to set up by
+ *	default
+ * @n_queue_to_fifo: number of queues to set up
  * @cmd_queue: the index of the command queue.
  *	Must be set before start_fw.
  * @no_reclaim_cmds: Some devices erroneously don't set the
@@ -288,14 +300,29 @@ static inline struct page *rxb_steal_page(struct iwl_rx_cmd_buffer *r)
  *	list of such notifications to filter. Max length is
  *	%MAX_NO_RECLAIM_CMDS.
  * @n_no_reclaim_cmds: # of commands in list
+ * @rx_buf_size_8k: 8 kB RX buffer size needed for A-MSDUs,
+ *	if unset 4k will be the RX buffer size
+ * @queue_watchdog_timeout: time (in ms) after which queues
+ *	are considered stuck and will trigger device restart
+ * @command_names: array of command names, must be 256 entries
+ *	(one for each command); for debugging only
  */
 struct iwl_trans_config {
 	struct iwl_op_mode *op_mode;
+	const u8 *queue_to_fifo;
+	u8 n_queue_to_fifo;
+
 	u8 cmd_queue;
 	const u8 *no_reclaim_cmds;
 	int n_no_reclaim_cmds;
+
+	bool rx_buf_size_8k;
+	unsigned int queue_watchdog_timeout;
+	const char **command_names;
 };
 
+struct iwl_trans;
+
 /**
  * struct iwl_trans_ops - transport specific operations
  *
@@ -304,7 +331,8 @@ struct iwl_trans_config {
  * @start_hw: starts the HW- from that point on, the HW can send interrupts
  *	May sleep
  * @stop_hw: stops the HW- from that point on, the HW will be in low power but
- *	will still issue interrupt if the HW RF kill is triggered.
+ *	will still issue interrupt if the HW RF kill is triggered unless
+ *	op_mode_leaving is true.
  *	May sleep
  * @start_fw: allocates and inits all the resources for the transport
  *	layer. Also kick a fw image.
@@ -322,18 +350,11 @@ struct iwl_trans_config {
  *	Must be atomic
  * @reclaim: free packet until ssn. Returns a list of freed packets.
  *	Must be atomic
- * @tx_agg_alloc: allocate resources for a TX BA session
- *	Must be atomic
  * @tx_agg_setup: setup a tx queue for AMPDU - will be called once the HW is
  *	ready and a successful ADDBA response has been received.
  *	May sleep
  * @tx_agg_disable: de-configure a Tx queue to send AMPDUs
  *	Must be atomic
- * @free: release all the ressource for the transport layer itself such as
- *	irq, tasklet etc... From this point on, the device may not issue
- *	any interrupt (incl. RFKILL).
- *	May sleep
- * @check_stuck_queue: check if a specific queue is stuck
  * @wait_tx_queue_empty: wait until all tx queues are empty
  *	May sleep
  * @dbgfs_register: add the dbgfs files under this directory. Files will be
@@ -346,11 +367,12 @@ struct iwl_trans_config {
  * @configure: configure parameters required by the transport layer from
  *	the op_mode. May be called several times before start_fw, can't be
  *	called after that.
+ * @set_pmi: set the power pmi state
  */
 struct iwl_trans_ops {
 
 	int (*start_hw)(struct iwl_trans *iwl_trans);
-	void (*stop_hw)(struct iwl_trans *iwl_trans);
+	void (*stop_hw)(struct iwl_trans *iwl_trans, bool op_mode_leaving);
 	int (*start_fw)(struct iwl_trans *trans, const struct fw_img *fw);
 	void (*fw_alive)(struct iwl_trans *trans);
 	void (*stop_device)(struct iwl_trans *trans);
@@ -360,23 +382,15 @@ struct iwl_trans_ops {
 	int (*send_cmd)(struct iwl_trans *trans, struct iwl_host_cmd *cmd);
 
 	int (*tx)(struct iwl_trans *trans, struct sk_buff *skb,
-		struct iwl_device_cmd *dev_cmd, enum iwl_rxon_context_id ctx,
-		u8 sta_id, u8 tid);
-	int (*reclaim)(struct iwl_trans *trans, int sta_id, int tid,
-			int txq_id, int ssn, struct sk_buff_head *skbs);
+		  struct iwl_device_cmd *dev_cmd, int queue);
+	void (*reclaim)(struct iwl_trans *trans, int queue, int ssn,
+			struct sk_buff_head *skbs);
 
-	int (*tx_agg_disable)(struct iwl_trans *trans,
-			      int sta_id, int tid);
-	int (*tx_agg_alloc)(struct iwl_trans *trans,
-			    int sta_id, int tid);
-	void (*tx_agg_setup)(struct iwl_trans *trans,
-			     enum iwl_rxon_context_id ctx, int sta_id, int tid,
-			     int frame_limit, u16 ssn);
-
-	void (*free)(struct iwl_trans *trans);
+	void (*tx_agg_setup)(struct iwl_trans *trans, int queue, int fifo,
+			     int sta_id, int tid, int frame_limit, u16 ssn);
+	void (*tx_agg_disable)(struct iwl_trans *trans, int queue);
 
 	int (*dbgfs_register)(struct iwl_trans *trans, struct dentry* dir);
-	int (*check_stuck_queue)(struct iwl_trans *trans, int q);
 	int (*wait_tx_queue_empty)(struct iwl_trans *trans);
 #ifdef CONFIG_PM_SLEEP
 	int (*suspend)(struct iwl_trans *trans);
@@ -387,6 +401,7 @@ struct iwl_trans_ops {
 	u32 (*read32)(struct iwl_trans *trans, u32 ofs);
 	void (*configure)(struct iwl_trans *trans,
 			  const struct iwl_trans_config *trans_cfg);
+	void (*set_pmi)(struct iwl_trans *trans, bool state);
 };
 
 /**
@@ -405,20 +420,19 @@ enum iwl_trans_state {
  *
  * @ops - pointer to iwl_trans_ops
  * @op_mode - pointer to the op_mode
- * @shrd - pointer to iwl_shared which holds shared data from the upper layer
+ * @cfg - pointer to the configuration
  * @reg_lock - protect hw register access
  * @dev - pointer to struct device * that represents the device
  * @hw_id: a u32 with the ID of the device / subdevice.
  *	Set during transport allocation.
  * @hw_id_str: a string with info about HW ID. Set during transport allocation.
- * @nvm_device_type: indicates OTP or eeprom
  * @pm_support: set to true in start_hw if link pm is supported
  * @wait_command_queue: the wait_queue for SYNC host commands
  */
 struct iwl_trans {
 	const struct iwl_trans_ops *ops;
 	struct iwl_op_mode *op_mode;
-	struct iwl_shared *shrd;
+	const struct iwl_cfg *cfg;
 	enum iwl_trans_state state;
 	spinlock_t reg_lock;
 
@@ -427,7 +441,6 @@ struct iwl_trans {
 	u32 hw_id;
 	char hw_id_str[52];
 
-	int    nvm_device_type;
 	bool pm_support;
 
 	wait_queue_head_t wait_command_queue;
@@ -456,11 +469,12 @@ static inline int iwl_trans_start_hw(struct iwl_trans *trans)
 	return trans->ops->start_hw(trans);
 }
 
-static inline void iwl_trans_stop_hw(struct iwl_trans *trans)
+static inline void iwl_trans_stop_hw(struct iwl_trans *trans,
+				     bool op_mode_leaving)
 {
 	might_sleep();
 
-	trans->ops->stop_hw(trans);
+	trans->ops->stop_hw(trans, op_mode_leaving);
 
 	trans->state = IWL_TRANS_NO_FW;
 }
@@ -507,60 +521,42 @@ static inline int iwl_trans_send_cmd(struct iwl_trans *trans,
 }
 
 static inline int iwl_trans_tx(struct iwl_trans *trans, struct sk_buff *skb,
-		struct iwl_device_cmd *dev_cmd, enum iwl_rxon_context_id ctx,
-		u8 sta_id, u8 tid)
-{
-	if (trans->state != IWL_TRANS_FW_ALIVE)
-		IWL_ERR(trans, "%s bad state = %d", __func__, trans->state);
-
-	return trans->ops->tx(trans, skb, dev_cmd, ctx, sta_id, tid);
-}
-
-static inline int iwl_trans_reclaim(struct iwl_trans *trans, int sta_id,
-				 int tid, int txq_id, int ssn,
-				 struct sk_buff_head *skbs)
+			       struct iwl_device_cmd *dev_cmd, int queue)
 {
 	WARN_ONCE(trans->state != IWL_TRANS_FW_ALIVE,
 		  "%s bad state = %d", __func__, trans->state);
 
-	return trans->ops->reclaim(trans, sta_id, tid, txq_id, ssn, skbs);
+	return trans->ops->tx(trans, skb, dev_cmd, queue);
 }
 
-static inline int iwl_trans_tx_agg_disable(struct iwl_trans *trans,
-					    int sta_id, int tid)
+static inline void iwl_trans_reclaim(struct iwl_trans *trans, int queue,
+				     int ssn, struct sk_buff_head *skbs)
 {
 	WARN_ONCE(trans->state != IWL_TRANS_FW_ALIVE,
 		  "%s bad state = %d", __func__, trans->state);
 
-	return trans->ops->tx_agg_disable(trans, sta_id, tid);
+	trans->ops->reclaim(trans, queue, ssn, skbs);
 }
 
-static inline int iwl_trans_tx_agg_alloc(struct iwl_trans *trans,
-					 int sta_id, int tid)
+static inline void iwl_trans_tx_agg_disable(struct iwl_trans *trans, int queue)
 {
 	WARN_ONCE(trans->state != IWL_TRANS_FW_ALIVE,
 		  "%s bad state = %d", __func__, trans->state);
 
-	return trans->ops->tx_agg_alloc(trans, sta_id, tid);
+	trans->ops->tx_agg_disable(trans, queue);
 }
 
-
-static inline void iwl_trans_tx_agg_setup(struct iwl_trans *trans,
-					   enum iwl_rxon_context_id ctx,
-					   int sta_id, int tid,
-					   int frame_limit, u16 ssn)
+static inline void iwl_trans_tx_agg_setup(struct iwl_trans *trans, int queue,
+					  int fifo, int sta_id, int tid,
+					  int frame_limit, u16 ssn)
 {
 	might_sleep();
 
 	WARN_ONCE(trans->state != IWL_TRANS_FW_ALIVE,
 		  "%s bad state = %d", __func__, trans->state);
 
-	trans->ops->tx_agg_setup(trans, ctx, sta_id, tid, frame_limit, ssn);
-}
-
-static inline void iwl_trans_free(struct iwl_trans *trans)
-{
-	trans->ops->free(trans);
+	trans->ops->tx_agg_setup(trans, queue, fifo, sta_id, tid,
+				 frame_limit, ssn);
 }
 
 static inline int iwl_trans_wait_tx_queue_empty(struct iwl_trans *trans)
@@ -571,13 +567,6 @@ static inline int iwl_trans_wait_tx_queue_empty(struct iwl_trans *trans)
 	return trans->ops->wait_tx_queue_empty(trans);
 }
 
-static inline int iwl_trans_check_stuck_queue(struct iwl_trans *trans, int q)
-{
-	WARN_ONCE(trans->state != IWL_TRANS_FW_ALIVE,
-		  "%s bad state = %d", __func__, trans->state);
-
-	return trans->ops->check_stuck_queue(trans, q);
-}
 static inline int iwl_trans_dbgfs_register(struct iwl_trans *trans,
 					    struct dentry *dir)
 {
@@ -611,20 +600,15 @@ static inline u32 iwl_trans_read32(struct iwl_trans *trans, u32 ofs)
 	return trans->ops->read32(trans, ofs);
 }
 
+static inline void iwl_trans_set_pmi(struct iwl_trans *trans, bool state)
+{
+	trans->ops->set_pmi(trans, state);
+}
+
 /*****************************************************
-* Transport layers implementations + their allocation function
+* driver (transport) register/unregister functions
 ******************************************************/
-struct pci_dev;
-struct pci_device_id;
-extern const struct iwl_trans_ops trans_ops_pcie;
-struct iwl_trans *iwl_trans_pcie_alloc(struct iwl_shared *shrd,
-				       struct pci_dev *pdev,
-				       const struct pci_device_id *ent);
 int __must_check iwl_pci_register_driver(void);
 void iwl_pci_unregister_driver(void);
 
-extern const struct iwl_trans_ops trans_ops_idi;
-struct iwl_trans *iwl_trans_idi_alloc(struct iwl_shared *shrd,
-				      void *pdev_void,
-				      const void *ent_void);
 #endif /* __iwl_trans_h__ */
diff --git a/drivers/net/wireless/iwlwifi/iwl-ucode.c b/drivers/net/wireless/iwlwifi/iwl-ucode.c
index 25282872883..bc40dc68b0f 100644
--- a/drivers/net/wireless/iwlwifi/iwl-ucode.c
+++ b/drivers/net/wireless/iwlwifi/iwl-ucode.c
@@ -31,7 +31,6 @@
 #include <linux/init.h>
 
 #include "iwl-dev.h"
-#include "iwl-core.h"
 #include "iwl-io.h"
 #include "iwl-agn-hw.h"
 #include "iwl-agn.h"
@@ -40,37 +39,6 @@
 #include "iwl-fh.h"
 #include "iwl-op-mode.h"
 
-static struct iwl_wimax_coex_event_entry cu_priorities[COEX_NUM_OF_EVENTS] = {
-	{COEX_CU_UNASSOC_IDLE_RP, COEX_CU_UNASSOC_IDLE_WP,
-	 0, COEX_UNASSOC_IDLE_FLAGS},
-	{COEX_CU_UNASSOC_MANUAL_SCAN_RP, COEX_CU_UNASSOC_MANUAL_SCAN_WP,
-	 0, COEX_UNASSOC_MANUAL_SCAN_FLAGS},
-	{COEX_CU_UNASSOC_AUTO_SCAN_RP, COEX_CU_UNASSOC_AUTO_SCAN_WP,
-	 0, COEX_UNASSOC_AUTO_SCAN_FLAGS},
-	{COEX_CU_CALIBRATION_RP, COEX_CU_CALIBRATION_WP,
-	 0, COEX_CALIBRATION_FLAGS},
-	{COEX_CU_PERIODIC_CALIBRATION_RP, COEX_CU_PERIODIC_CALIBRATION_WP,
-	 0, COEX_PERIODIC_CALIBRATION_FLAGS},
-	{COEX_CU_CONNECTION_ESTAB_RP, COEX_CU_CONNECTION_ESTAB_WP,
-	 0, COEX_CONNECTION_ESTAB_FLAGS},
-	{COEX_CU_ASSOCIATED_IDLE_RP, COEX_CU_ASSOCIATED_IDLE_WP,
-	 0, COEX_ASSOCIATED_IDLE_FLAGS},
-	{COEX_CU_ASSOC_MANUAL_SCAN_RP, COEX_CU_ASSOC_MANUAL_SCAN_WP,
-	 0, COEX_ASSOC_MANUAL_SCAN_FLAGS},
-	{COEX_CU_ASSOC_AUTO_SCAN_RP, COEX_CU_ASSOC_AUTO_SCAN_WP,
-	 0, COEX_ASSOC_AUTO_SCAN_FLAGS},
-	{COEX_CU_ASSOC_ACTIVE_LEVEL_RP, COEX_CU_ASSOC_ACTIVE_LEVEL_WP,
-	 0, COEX_ASSOC_ACTIVE_LEVEL_FLAGS},
-	{COEX_CU_RF_ON_RP, COEX_CU_RF_ON_WP, 0, COEX_CU_RF_ON_FLAGS},
-	{COEX_CU_RF_OFF_RP, COEX_CU_RF_OFF_WP, 0, COEX_RF_OFF_FLAGS},
-	{COEX_CU_STAND_ALONE_DEBUG_RP, COEX_CU_STAND_ALONE_DEBUG_WP,
-	 0, COEX_STAND_ALONE_DEBUG_FLAGS},
-	{COEX_CU_IPAN_ASSOC_LEVEL_RP, COEX_CU_IPAN_ASSOC_LEVEL_WP,
-	 0, COEX_IPAN_ASSOC_LEVEL_FLAGS},
-	{COEX_CU_RSRVD1_RP, COEX_CU_RSRVD1_WP, 0, COEX_RSRVD1_FLAGS},
-	{COEX_CU_RSRVD2_RP, COEX_CU_RSRVD2_WP, 0, COEX_RSRVD2_FLAGS}
-};
-
 /******************************************************************************
  *
  * uCode download functions
@@ -93,7 +61,7 @@ static int iwl_set_Xtal_calib(struct iwl_priv *priv)
 {
 	struct iwl_calib_xtal_freq_cmd cmd;
 	__le16 *xtal_calib =
-		(__le16 *)iwl_eeprom_query_addr(priv->shrd, EEPROM_XTAL);
+		(__le16 *)iwl_eeprom_query_addr(priv, EEPROM_XTAL);
 
 	iwl_set_calib_hdr(&cmd.hdr, IWL_PHY_CALIBRATE_CRYSTAL_FRQ_CMD);
 	cmd.cap_pin1 = le16_to_cpu(xtal_calib[0]);
@@ -105,8 +73,7 @@ static int iwl_set_temperature_offset_calib(struct iwl_priv *priv)
 {
 	struct iwl_calib_temperature_offset_cmd cmd;
 	__le16 *offset_calib =
-		(__le16 *)iwl_eeprom_query_addr(priv->shrd,
-						EEPROM_RAW_TEMPERATURE);
+		(__le16 *)iwl_eeprom_query_addr(priv, EEPROM_RAW_TEMPERATURE);
 
 	memset(&cmd, 0, sizeof(cmd));
 	iwl_set_calib_hdr(&cmd.hdr, IWL_PHY_CALIBRATE_TEMP_OFFSET_CMD);
@@ -122,16 +89,15 @@ static int iwl_set_temperature_offset_calib(struct iwl_priv *priv)
 static int iwl_set_temperature_offset_calib_v2(struct iwl_priv *priv)
 {
 	struct iwl_calib_temperature_offset_v2_cmd cmd;
-	__le16 *offset_calib_high = (__le16 *)iwl_eeprom_query_addr(priv->shrd,
+	__le16 *offset_calib_high = (__le16 *)iwl_eeprom_query_addr(priv,
 				     EEPROM_KELVIN_TEMPERATURE);
 	__le16 *offset_calib_low =
-		(__le16 *)iwl_eeprom_query_addr(priv->shrd,
-						EEPROM_RAW_TEMPERATURE);
+		(__le16 *)iwl_eeprom_query_addr(priv, EEPROM_RAW_TEMPERATURE);
 	struct iwl_eeprom_calib_hdr *hdr;
 
 	memset(&cmd, 0, sizeof(cmd));
 	iwl_set_calib_hdr(&cmd.hdr, IWL_PHY_CALIBRATE_TEMP_OFFSET_CMD);
-	hdr = (struct iwl_eeprom_calib_hdr *)iwl_eeprom_query_addr(priv->shrd,
+	hdr = (struct iwl_eeprom_calib_hdr *)iwl_eeprom_query_addr(priv,
 							EEPROM_CALIB_ALL);
 	memcpy(&cmd.radio_sensor_offset_high, offset_calib_high,
 		sizeof(*offset_calib_high));
@@ -174,30 +140,12 @@ static int iwl_send_calib_cfg(struct iwl_priv *priv)
 	return iwl_dvm_send_cmd(priv, &cmd);
 }
 
-int iwlagn_rx_calib_result(struct iwl_priv *priv,
-			    struct iwl_rx_cmd_buffer *rxb,
-			    struct iwl_device_cmd *cmd)
-{
-	struct iwl_rx_packet *pkt = rxb_addr(rxb);
-	struct iwl_calib_hdr *hdr = (struct iwl_calib_hdr *)pkt->data;
-	int len = le32_to_cpu(pkt->len_n_flags) & FH_RSCSR_FRAME_SIZE_MSK;
-
-	/* reduce the size of the length field itself */
-	len -= 4;
-
-	if (iwl_calib_set(priv, hdr, len))
-		IWL_ERR(priv, "Failed to record calibration data %d\n",
-			hdr->op_code);
-
-	return 0;
-}
-
 int iwl_init_alive_start(struct iwl_priv *priv)
 {
 	int ret;
 
-	if (cfg(priv)->bt_params &&
-	    cfg(priv)->bt_params->advanced_bt_coexist) {
+	if (priv->cfg->bt_params &&
+	    priv->cfg->bt_params->advanced_bt_coexist) {
 		/*
 		 * Tell uCode we are ready to perform calibration
 		 * need to perform this before any calibration
@@ -219,8 +167,8 @@ int iwl_init_alive_start(struct iwl_priv *priv)
 	 * temperature offset calibration is only needed for runtime ucode,
 	 * so prepare the value now.
 	 */
-	if (cfg(priv)->need_temp_offset_calib) {
-		if (cfg(priv)->temp_offset_v2)
+	if (priv->cfg->need_temp_offset_calib) {
+		if (priv->cfg->temp_offset_v2)
 			return iwl_set_temperature_offset_calib_v2(priv);
 		else
 			return iwl_set_temperature_offset_calib(priv);
@@ -229,29 +177,13 @@ int iwl_init_alive_start(struct iwl_priv *priv)
 	return 0;
 }
 
-static int iwl_send_wimax_coex(struct iwl_priv *priv)
+int iwl_send_wimax_coex(struct iwl_priv *priv)
 {
 	struct iwl_wimax_coex_cmd coex_cmd;
 
-	if (cfg(priv)->base_params->support_wimax_coexist) {
-		/* UnMask wake up src at associated sleep */
-		coex_cmd.flags = COEX_FLAGS_ASSOC_WA_UNMASK_MSK;
+	/* coexistence is disabled */
+	memset(&coex_cmd, 0, sizeof(coex_cmd));
 
-		/* UnMask wake up src at unassociated sleep */
-		coex_cmd.flags |= COEX_FLAGS_UNASSOC_WA_UNMASK_MSK;
-		memcpy(coex_cmd.sta_prio, cu_priorities,
-			sizeof(struct iwl_wimax_coex_event_entry) *
-			 COEX_NUM_OF_EVENTS);
-
-		/* enabling the coexistence feature */
-		coex_cmd.flags |= COEX_FLAGS_COEX_ENABLE_MSK;
-
-		/* enabling the priorities tables */
-		coex_cmd.flags |= COEX_FLAGS_STA_TABLE_VALID_MSK;
-	} else {
-		/* coexistence is disabled */
-		memset(&coex_cmd, 0, sizeof(coex_cmd));
-	}
 	return iwl_dvm_send_cmd_pdu(priv,
 				COEX_PRIORITY_TABLE_CMD, CMD_SYNC,
 				sizeof(coex_cmd), &coex_cmd);
@@ -311,7 +243,7 @@ static int iwl_alive_notify(struct iwl_priv *priv)
 {
 	int ret;
 
-	iwl_trans_fw_alive(trans(priv));
+	iwl_trans_fw_alive(priv->trans);
 
 	priv->passive_no_rx = false;
 	priv->transport_queue_stop = 0;
@@ -320,7 +252,7 @@ static int iwl_alive_notify(struct iwl_priv *priv)
 	if (ret)
 		return ret;
 
-	if (!cfg(priv)->no_xtal_calib) {
+	if (!priv->cfg->no_xtal_calib) {
 		ret = iwl_set_Xtal_calib(priv);
 		if (ret)
 			return ret;
@@ -349,9 +281,9 @@ static int iwl_verify_sec_sparse(struct iwl_priv *priv,
 		/* read data comes through single port, auto-incr addr */
 		/* NOTE: Use the debugless read so we don't flood kernel log
 		 * if IWL_DL_IO is set */
-		iwl_write_direct32(trans(priv), HBUS_TARG_MEM_RADDR,
+		iwl_write_direct32(priv->trans, HBUS_TARG_MEM_RADDR,
 			i + fw_desc->offset);
-		val = iwl_read32(trans(priv), HBUS_TARG_MEM_RDAT);
+		val = iwl_read32(priv->trans, HBUS_TARG_MEM_RDAT);
 		if (val != le32_to_cpu(*image))
 			return -EIO;
 	}
@@ -370,14 +302,14 @@ static void iwl_print_mismatch_sec(struct iwl_priv *priv,
 
 	IWL_DEBUG_FW(priv, "ucode inst image size is %u\n", len);
 
-	iwl_write_direct32(trans(priv), HBUS_TARG_MEM_RADDR,
+	iwl_write_direct32(priv->trans, HBUS_TARG_MEM_RADDR,
 				fw_desc->offset);
 
 	for (offs = 0;
 	     offs < len && errors < 20;
 	     offs += sizeof(u32), image++) {
 		/* read data comes through single port, auto-incr addr */
-		val = iwl_read32(trans(priv), HBUS_TARG_MEM_RDAT);
+		val = iwl_read32(priv->trans, HBUS_TARG_MEM_RDAT);
 		if (val != le32_to_cpu(*image)) {
 			IWL_ERR(priv, "uCode INST section at "
 				"offset 0x%x, is 0x%x, s/b 0x%x\n",
@@ -417,9 +349,8 @@ struct iwl_alive_data {
 	u8 subtype;
 };
 
-static void iwl_alive_fn(struct iwl_notif_wait_data *notif_wait,
-			    struct iwl_rx_packet *pkt,
-			    void *data)
+static bool iwl_alive_fn(struct iwl_notif_wait_data *notif_wait,
+			 struct iwl_rx_packet *pkt, void *data)
 {
 	struct iwl_priv *priv =
 		container_of(notif_wait, struct iwl_priv, notif_wait);
@@ -433,13 +364,15 @@ static void iwl_alive_fn(struct iwl_notif_wait_data *notif_wait,
 		       palive->is_valid, palive->ver_type,
 		       palive->ver_subtype);
 
-	priv->shrd->device_pointers.error_event_table =
+	priv->device_pointers.error_event_table =
 		le32_to_cpu(palive->error_event_table_ptr);
-	priv->shrd->device_pointers.log_event_table =
+	priv->device_pointers.log_event_table =
 		le32_to_cpu(palive->log_event_table_ptr);
 
 	alive_data->subtype = palive->ver_subtype;
 	alive_data->valid = palive->is_valid == UCODE_VALID_OK;
+
+	return true;
 }
 
 #define UCODE_ALIVE_TIMEOUT	HZ
@@ -453,9 +386,10 @@ int iwl_load_ucode_wait_alive(struct iwl_priv *priv,
 	const struct fw_img *fw;
 	int ret;
 	enum iwl_ucode_type old_type;
+	static const u8 alive_cmd[] = { REPLY_ALIVE };
 
-	old_type = priv->shrd->ucode_type;
-	priv->shrd->ucode_type = ucode_type;
+	old_type = priv->cur_ucode;
+	priv->cur_ucode = ucode_type;
 	fw = iwl_get_ucode_image(priv, ucode_type);
 
 	priv->ucode_loaded = false;
@@ -463,12 +397,13 @@ int iwl_load_ucode_wait_alive(struct iwl_priv *priv,
 	if (!fw)
 		return -EINVAL;
 
-	iwl_init_notification_wait(&priv->notif_wait, &alive_wait, REPLY_ALIVE,
-				      iwl_alive_fn, &alive_data);
+	iwl_init_notification_wait(&priv->notif_wait, &alive_wait,
+				   alive_cmd, ARRAY_SIZE(alive_cmd),
+				   iwl_alive_fn, &alive_data);
 
-	ret = iwl_trans_start_fw(trans(priv), fw);
+	ret = iwl_trans_start_fw(priv->trans, fw);
 	if (ret) {
-		priv->shrd->ucode_type = old_type;
+		priv->cur_ucode = old_type;
 		iwl_remove_notification(&priv->notif_wait, &alive_wait);
 		return ret;
 	}
@@ -480,13 +415,13 @@ int iwl_load_ucode_wait_alive(struct iwl_priv *priv,
 	ret = iwl_wait_notification(&priv->notif_wait, &alive_wait,
 					UCODE_ALIVE_TIMEOUT);
 	if (ret) {
-		priv->shrd->ucode_type = old_type;
+		priv->cur_ucode = old_type;
 		return ret;
 	}
 
 	if (!alive_data.valid) {
 		IWL_ERR(priv, "Loaded ucode is not valid!\n");
-		priv->shrd->ucode_type = old_type;
+		priv->cur_ucode = old_type;
 		return -EIO;
 	}
 
@@ -498,7 +433,7 @@ int iwl_load_ucode_wait_alive(struct iwl_priv *priv,
 	if (ucode_type != IWL_UCODE_WOWLAN) {
 		ret = iwl_verify_ucode(priv, ucode_type);
 		if (ret) {
-			priv->shrd->ucode_type = old_type;
+			priv->cur_ucode = old_type;
 			return ret;
 		}
 
@@ -510,7 +445,7 @@ int iwl_load_ucode_wait_alive(struct iwl_priv *priv,
 	if (ret) {
 		IWL_WARN(priv,
 			"Could not complete ALIVE transition: %d\n", ret);
-		priv->shrd->ucode_type = old_type;
+		priv->cur_ucode = old_type;
 		return ret;
 	}
 
@@ -519,9 +454,38 @@ int iwl_load_ucode_wait_alive(struct iwl_priv *priv,
 	return 0;
 }
 
+static bool iwlagn_wait_calib(struct iwl_notif_wait_data *notif_wait,
+			      struct iwl_rx_packet *pkt, void *data)
+{
+	struct iwl_priv *priv = data;
+	struct iwl_calib_hdr *hdr;
+	int len;
+
+	if (pkt->hdr.cmd != CALIBRATION_RES_NOTIFICATION) {
+		WARN_ON(pkt->hdr.cmd != CALIBRATION_COMPLETE_NOTIFICATION);
+		return true;
+	}
+
+	hdr = (struct iwl_calib_hdr *)pkt->data;
+	len = le32_to_cpu(pkt->len_n_flags) & FH_RSCSR_FRAME_SIZE_MSK;
+
+	/* reduce the size by the length field itself */
+	len -= sizeof(__le32);
+
+	if (iwl_calib_set(priv, hdr, len))
+		IWL_ERR(priv, "Failed to record calibration data %d\n",
+			hdr->op_code);
+
+	return false;
+}
+
 int iwl_run_init_ucode(struct iwl_priv *priv)
 {
 	struct iwl_notification_wait calib_wait;
+	static const u8 calib_complete[] = {
+		CALIBRATION_RES_NOTIFICATION,
+		CALIBRATION_COMPLETE_NOTIFICATION
+	};
 	int ret;
 
 	lockdep_assert_held(&priv->mutex);
@@ -534,8 +498,8 @@ int iwl_run_init_ucode(struct iwl_priv *priv)
 		return 0;
 
 	iwl_init_notification_wait(&priv->notif_wait, &calib_wait,
-				      CALIBRATION_COMPLETE_NOTIFICATION,
-				      NULL, NULL);
+				   calib_complete, ARRAY_SIZE(calib_complete),
+				   iwlagn_wait_calib, priv);
 
 	/* Will also start the device */
 	ret = iwl_load_ucode_wait_alive(priv, IWL_UCODE_INIT);
@@ -561,7 +525,7 @@ int iwl_run_init_ucode(struct iwl_priv *priv)
 	iwl_remove_notification(&priv->notif_wait, &calib_wait);
  out:
 	/* Whatever happened, stop the device */
-	iwl_trans_stop_device(trans(priv));
+	iwl_trans_stop_device(priv->trans);
 	priv->ucode_loaded = false;
 
 	return ret;
diff --git a/drivers/net/wireless/iwmc3200wifi/Kconfig b/drivers/net/wireless/iwmc3200wifi/Kconfig
index 03f998d098c..7107ce53d4d 100644
--- a/drivers/net/wireless/iwmc3200wifi/Kconfig
+++ b/drivers/net/wireless/iwmc3200wifi/Kconfig
@@ -1,5 +1,5 @@
 config IWM
-	tristate "Intel Wireless Multicomm 3200 WiFi driver"
+	tristate "Intel Wireless Multicomm 3200 WiFi driver (EXPERIMENTAL)"
 	depends on MMC && EXPERIMENTAL
 	depends on CFG80211
 	select FW_LOADER
diff --git a/drivers/net/wireless/libertas/Makefile b/drivers/net/wireless/libertas/Makefile
index f7d01bfa2e4..eac72f7bd34 100644
--- a/drivers/net/wireless/libertas/Makefile
+++ b/drivers/net/wireless/libertas/Makefile
@@ -6,6 +6,7 @@ libertas-y += ethtool.o
 libertas-y += main.o
 libertas-y += rx.o
 libertas-y += tx.o
+libertas-y += firmware.o
 libertas-$(CONFIG_LIBERTAS_MESH) += mesh.o
 
 usb8xxx-objs += if_usb.o
diff --git a/drivers/net/wireless/libertas/decl.h b/drivers/net/wireless/libertas/decl.h
index bc951ab4b68..84a3aa7ac57 100644
--- a/drivers/net/wireless/libertas/decl.h
+++ b/drivers/net/wireless/libertas/decl.h
@@ -19,6 +19,10 @@ struct lbs_fw_table {
 };
 
 struct lbs_private;
+typedef void (*lbs_fw_cb)(struct lbs_private *priv, int ret,
+		const struct firmware *helper, const struct firmware *mainfw);
+
+struct lbs_private;
 struct sk_buff;
 struct net_device;
 struct cmd_ds_command;
@@ -66,10 +70,13 @@ int lbs_exit_auto_deep_sleep(struct lbs_private *priv);
 u32 lbs_fw_index_to_data_rate(u8 index);
 u8 lbs_data_rate_to_fw_index(u32 rate);
 
-int lbs_get_firmware(struct device *dev, const char *user_helper,
-			const char *user_mainfw, u32 card_model,
+int lbs_get_firmware(struct device *dev, u32 card_model,
 			const struct lbs_fw_table *fw_table,
 			const struct firmware **helper,
 			const struct firmware **mainfw);
+int lbs_get_firmware_async(struct lbs_private *priv, struct device *device,
+			   u32 card_model, const struct lbs_fw_table *fw_table,
+			   lbs_fw_cb callback);
+void lbs_wait_for_firmware_load(struct lbs_private *priv);
 
 #endif
diff --git a/drivers/net/wireless/libertas/dev.h b/drivers/net/wireless/libertas/dev.h
index f3fd447131c..672005430ac 100644
--- a/drivers/net/wireless/libertas/dev.h
+++ b/drivers/net/wireless/libertas/dev.h
@@ -7,6 +7,7 @@
 #define _LBS_DEV_H_
 
 #include "defs.h"
+#include "decl.h"
 #include "host.h"
 
 #include <linux/kfifo.h>
@@ -180,6 +181,15 @@ struct lbs_private {
 	wait_queue_head_t scan_q;
 	/* Whether the scan was initiated internally and not by cfg80211 */
 	bool internal_scan;
+
+	/* Firmware load */
+	u32 fw_model;
+	wait_queue_head_t fw_waitq;
+	struct device *fw_device;
+	const struct firmware *helper_fw;
+	const struct lbs_fw_table *fw_table;
+	const struct lbs_fw_table *fw_iter;
+	lbs_fw_cb fw_callback;
 };
 
 extern struct cmd_confirm_sleep confirm_sleep;
diff --git a/drivers/net/wireless/libertas/firmware.c b/drivers/net/wireless/libertas/firmware.c
new file mode 100644
index 00000000000..601f2075355
--- /dev/null
+++ b/drivers/net/wireless/libertas/firmware.c
@@ -0,0 +1,224 @@
+/*
+ * Firmware loading and handling functions.
+ */
+
+#include <linux/sched.h>
+#include <linux/firmware.h>
+#include <linux/firmware.h>
+#include <linux/module.h>
+#include <linux/sched.h>
+
+#include "dev.h"
+#include "decl.h"
+
+static void load_next_firmware_from_table(struct lbs_private *private);
+
+static void lbs_fw_loaded(struct lbs_private *priv, int ret,
+	const struct firmware *helper, const struct firmware *mainfw)
+{
+	unsigned long flags;
+
+	lbs_deb_fw("firmware load complete, code %d\n", ret);
+
+	/* User must free helper/mainfw */
+	priv->fw_callback(priv, ret, helper, mainfw);
+
+	spin_lock_irqsave(&priv->driver_lock, flags);
+	priv->fw_callback = NULL;
+	wake_up(&priv->fw_waitq);
+	spin_unlock_irqrestore(&priv->driver_lock, flags);
+}
+
+static void do_load_firmware(struct lbs_private *priv, const char *name,
+	void (*cb)(const struct firmware *fw, void *context))
+{
+	int ret;
+
+	lbs_deb_fw("Requesting %s\n", name);
+	ret = request_firmware_nowait(THIS_MODULE, true, name,
+			priv->fw_device, GFP_KERNEL, priv, cb);
+	if (ret) {
+		lbs_deb_fw("request_firmware_nowait error %d\n", ret);
+		lbs_fw_loaded(priv, ret, NULL, NULL);
+	}
+}
+
+static void main_firmware_cb(const struct firmware *firmware, void *context)
+{
+	struct lbs_private *priv = context;
+
+	if (!firmware) {
+		/* Failed to find firmware: try next table entry */
+		load_next_firmware_from_table(priv);
+		return;
+	}
+
+	/* Firmware found! */
+	lbs_fw_loaded(priv, 0, priv->helper_fw, firmware);
+}
+
+static void helper_firmware_cb(const struct firmware *firmware, void *context)
+{
+	struct lbs_private *priv = context;
+
+	if (!firmware) {
+		/* Failed to find firmware: try next table entry */
+		load_next_firmware_from_table(priv);
+		return;
+	}
+
+	/* Firmware found! */
+	if (priv->fw_iter->fwname) {
+		priv->helper_fw = firmware;
+		do_load_firmware(priv, priv->fw_iter->fwname, main_firmware_cb);
+	} else {
+		/* No main firmware needed for this helper --> success! */
+		lbs_fw_loaded(priv, 0, firmware, NULL);
+	}
+}
+
+static void load_next_firmware_from_table(struct lbs_private *priv)
+{
+	const struct lbs_fw_table *iter;
+
+	if (!priv->fw_iter)
+		iter = priv->fw_table;
+	else
+		iter = ++priv->fw_iter;
+
+	if (priv->helper_fw) {
+		release_firmware(priv->helper_fw);
+		priv->helper_fw = NULL;
+	}
+
+next:
+	if (!iter->helper) {
+		/* End of table hit. */
+		lbs_fw_loaded(priv, -ENOENT, NULL, NULL);
+		return;
+	}
+
+	if (iter->model != priv->fw_model) {
+		iter++;
+		goto next;
+	}
+
+	priv->fw_iter = iter;
+	do_load_firmware(priv, iter->helper, helper_firmware_cb);
+}
+
+void lbs_wait_for_firmware_load(struct lbs_private *priv)
+{
+	wait_event(priv->fw_waitq, priv->fw_callback == NULL);
+}
+
+/**
+ *  lbs_get_firmware_async - Retrieves firmware asynchronously. Can load
+ *  either a helper firmware and a main firmware (2-stage), or just the helper.
+ *
+ *  @priv:      Pointer to lbs_private instance
+ *  @dev:     	A pointer to &device structure
+ *  @card_model: Bus-specific card model ID used to filter firmware table
+ *		elements
+ *  @fw_table:	Table of firmware file names and device model numbers
+ *		terminated by an entry with a NULL helper name
+ *	@callback: User callback to invoke when firmware load succeeds or fails.
+ */
+int lbs_get_firmware_async(struct lbs_private *priv, struct device *device,
+			    u32 card_model, const struct lbs_fw_table *fw_table,
+			    lbs_fw_cb callback)
+{
+	unsigned long flags;
+
+	spin_lock_irqsave(&priv->driver_lock, flags);
+	if (priv->fw_callback) {
+		lbs_deb_fw("firmware load already in progress\n");
+		spin_unlock_irqrestore(&priv->driver_lock, flags);
+		return -EBUSY;
+	}
+
+	priv->fw_device = device;
+	priv->fw_callback = callback;
+	priv->fw_table = fw_table;
+	priv->fw_iter = NULL;
+	priv->fw_model = card_model;
+	spin_unlock_irqrestore(&priv->driver_lock, flags);
+
+	lbs_deb_fw("Starting async firmware load\n");
+	load_next_firmware_from_table(priv);
+	return 0;
+}
+EXPORT_SYMBOL_GPL(lbs_get_firmware_async);
+
+/**
+ *  lbs_get_firmware - Retrieves two-stage firmware
+ *
+ *  @dev:     	A pointer to &device structure
+ *  @card_model: Bus-specific card model ID used to filter firmware table
+ *		elements
+ *  @fw_table:	Table of firmware file names and device model numbers
+ *		terminated by an entry with a NULL helper name
+ *  @helper:	On success, the helper firmware; caller must free
+ *  @mainfw:	On success, the main firmware; caller must free
+ *
+ * Deprecated: use lbs_get_firmware_async() instead.
+ *
+ *  returns:		0 on success, non-zero on failure
+ */
+int lbs_get_firmware(struct device *dev, u32 card_model,
+			const struct lbs_fw_table *fw_table,
+			const struct firmware **helper,
+			const struct firmware **mainfw)
+{
+	const struct lbs_fw_table *iter;
+	int ret;
+
+	BUG_ON(helper == NULL);
+	BUG_ON(mainfw == NULL);
+
+	/* Search for firmware to use from the table. */
+	iter = fw_table;
+	while (iter && iter->helper) {
+		if (iter->model != card_model)
+			goto next;
+
+		if (*helper == NULL) {
+			ret = request_firmware(helper, iter->helper, dev);
+			if (ret)
+				goto next;
+
+			/* If the device has one-stage firmware (ie cf8305) and
+			 * we've got it then we don't need to bother with the
+			 * main firmware.
+			 */
+			if (iter->fwname == NULL)
+				return 0;
+		}
+
+		if (*mainfw == NULL) {
+			ret = request_firmware(mainfw, iter->fwname, dev);
+			if (ret) {
+				/* Clear the helper to ensure we don't have
+				 * mismatched firmware pairs.
+				 */
+				release_firmware(*helper);
+				*helper = NULL;
+			}
+		}
+
+		if (*helper && *mainfw)
+			return 0;
+
+  next:
+		iter++;
+	}
+
+	/* Failed */
+	release_firmware(*helper);
+	*helper = NULL;
+	release_firmware(*mainfw);
+	*mainfw = NULL;
+
+	return -ENOENT;
+}
+EXPORT_SYMBOL_GPL(lbs_get_firmware);
diff --git a/drivers/net/wireless/libertas/if_cs.c b/drivers/net/wireless/libertas/if_cs.c
index 234ee88dec9..16beaf39dc5 100644
--- a/drivers/net/wireless/libertas/if_cs.c
+++ b/drivers/net/wireless/libertas/if_cs.c
@@ -738,6 +738,50 @@ done:
 	return ret;
 }
 
+static void if_cs_prog_firmware(struct lbs_private *priv, int ret,
+				 const struct firmware *helper,
+				 const struct firmware *mainfw)
+{
+	struct if_cs_card *card = priv->card;
+
+	if (ret) {
+		pr_err("failed to find firmware (%d)\n", ret);
+		return;
+	}
+
+	/* Load the firmware */
+	ret = if_cs_prog_helper(card, helper);
+	if (ret == 0 && (card->model != MODEL_8305))
+		ret = if_cs_prog_real(card, mainfw);
+	if (ret)
+		goto out;
+
+	/* Now actually get the IRQ */
+	ret = request_irq(card->p_dev->irq, if_cs_interrupt,
+		IRQF_SHARED, DRV_NAME, card);
+	if (ret) {
+		pr_err("error in request_irq\n");
+		goto out;
+	}
+
+	/*
+	 * Clear any interrupt cause that happened while sending
+	 * firmware/initializing card
+	 */
+	if_cs_write16(card, IF_CS_CARD_INT_CAUSE, IF_CS_BIT_MASK);
+	if_cs_enable_ints(card);
+
+	/* And finally bring the card up */
+	priv->fw_ready = 1;
+	if (lbs_start_card(priv) != 0) {
+		pr_err("could not activate card\n");
+		free_irq(card->p_dev->irq, card);
+	}
+
+out:
+	release_firmware(helper);
+	release_firmware(mainfw);
+}
 
 
 /********************************************************************/
@@ -809,8 +853,6 @@ static int if_cs_probe(struct pcmcia_device *p_dev)
 	unsigned int prod_id;
 	struct lbs_private *priv;
 	struct if_cs_card *card;
-	const struct firmware *helper = NULL;
-	const struct firmware *mainfw = NULL;
 
 	lbs_deb_enter(LBS_DEB_CS);
 
@@ -890,20 +932,6 @@ static int if_cs_probe(struct pcmcia_device *p_dev)
 		goto out2;
 	}
 
-	ret = lbs_get_firmware(&p_dev->dev, NULL, NULL, card->model,
-				&fw_table[0], &helper, &mainfw);
-	if (ret) {
-		pr_err("failed to find firmware (%d)\n", ret);
-		goto out2;
-	}
-
-	/* Load the firmware early, before calling into libertas.ko */
-	ret = if_cs_prog_helper(card, helper);
-	if (ret == 0 && (card->model != MODEL_8305))
-		ret = if_cs_prog_real(card, mainfw);
-	if (ret)
-		goto out2;
-
 	/* Make this card known to the libertas driver */
 	priv = lbs_add_card(card, &p_dev->dev);
 	if (!priv) {
@@ -911,37 +939,22 @@ static int if_cs_probe(struct pcmcia_device *p_dev)
 		goto out2;
 	}
 
-	/* Finish setting up fields in lbs_private */
+	/* Set up fields in lbs_private */
 	card->priv = priv;
 	priv->card = card;
 	priv->hw_host_to_card = if_cs_host_to_card;
 	priv->enter_deep_sleep = NULL;
 	priv->exit_deep_sleep = NULL;
 	priv->reset_deep_sleep_wakeup = NULL;
-	priv->fw_ready = 1;
 
-	/* Now actually get the IRQ */
-	ret = request_irq(p_dev->irq, if_cs_interrupt,
-		IRQF_SHARED, DRV_NAME, card);
+	/* Get firmware */
+	ret = lbs_get_firmware_async(priv, &p_dev->dev, card->model, fw_table,
+				     if_cs_prog_firmware);
 	if (ret) {
-		pr_err("error in request_irq\n");
-		goto out3;
-	}
-
-	/*
-	 * Clear any interrupt cause that happened while sending
-	 * firmware/initializing card
-	 */
-	if_cs_write16(card, IF_CS_CARD_INT_CAUSE, IF_CS_BIT_MASK);
-	if_cs_enable_ints(card);
-
-	/* And finally bring the card up */
-	if (lbs_start_card(priv) != 0) {
-		pr_err("could not activate card\n");
+		pr_err("failed to find firmware (%d)\n", ret);
 		goto out3;
 	}
 
-	ret = 0;
 	goto out;
 
 out3:
@@ -951,11 +964,6 @@ out2:
 out1:
 	pcmcia_disable_device(p_dev);
 out:
-	if (helper)
-		release_firmware(helper);
-	if (mainfw)
-		release_firmware(mainfw);
-
 	lbs_deb_leave_args(LBS_DEB_CS, "ret %d", ret);
 	return ret;
 }
diff --git a/drivers/net/wireless/libertas/if_sdio.c b/drivers/net/wireless/libertas/if_sdio.c
index 9804ebc892d..76caebaa439 100644
--- a/drivers/net/wireless/libertas/if_sdio.c
+++ b/drivers/net/wireless/libertas/if_sdio.c
@@ -65,12 +65,6 @@ static void if_sdio_interrupt(struct sdio_func *func);
  */
 static u8 user_rmmod;
 
-static char *lbs_helper_name = NULL;
-module_param_named(helper_name, lbs_helper_name, charp, 0644);
-
-static char *lbs_fw_name = NULL;
-module_param_named(fw_name, lbs_fw_name, charp, 0644);
-
 static const struct sdio_device_id if_sdio_ids[] = {
 	{ SDIO_DEVICE(SDIO_VENDOR_ID_MARVELL,
 			SDIO_DEVICE_ID_MARVELL_LIBERTAS) },
@@ -123,11 +117,8 @@ struct if_sdio_card {
 	int			model;
 	unsigned long		ioport;
 	unsigned int		scratch_reg;
-
-	const char		*helper;
-	const char		*firmware;
-	bool			helper_allocated;
-	bool			firmware_allocated;
+	bool			started;
+	wait_queue_head_t	pwron_waitq;
 
 	u8			buffer[65536] __attribute__((aligned(4)));
 
@@ -140,6 +131,9 @@ struct if_sdio_card {
 	u8			rx_unit;
 };
 
+static void if_sdio_finish_power_on(struct if_sdio_card *card);
+static int if_sdio_power_off(struct if_sdio_card *card);
+
 /********************************************************************/
 /* I/O                                                              */
 /********************************************************************/
@@ -680,12 +674,39 @@ out:
 	return ret;
 }
 
+static void if_sdio_do_prog_firmware(struct lbs_private *priv, int ret,
+				     const struct firmware *helper,
+				     const struct firmware *mainfw)
+{
+	struct if_sdio_card *card = priv->card;
+
+	if (ret) {
+		pr_err("failed to find firmware (%d)\n", ret);
+		return;
+	}
+
+	ret = if_sdio_prog_helper(card, helper);
+	if (ret)
+		goto out;
+
+	lbs_deb_sdio("Helper firmware loaded\n");
+
+	ret = if_sdio_prog_real(card, mainfw);
+	if (ret)
+		goto out;
+
+	lbs_deb_sdio("Firmware loaded\n");
+	if_sdio_finish_power_on(card);
+
+out:
+	release_firmware(helper);
+	release_firmware(mainfw);
+}
+
 static int if_sdio_prog_firmware(struct if_sdio_card *card)
 {
 	int ret;
 	u16 scratch;
-	const struct firmware *helper = NULL;
-	const struct firmware *mainfw = NULL;
 
 	lbs_deb_enter(LBS_DEB_SDIO);
 
@@ -719,43 +740,18 @@ static int if_sdio_prog_firmware(struct if_sdio_card *card)
 	 */
 	if (scratch == IF_SDIO_FIRMWARE_OK) {
 		lbs_deb_sdio("firmware already loaded\n");
-		goto success;
+		if_sdio_finish_power_on(card);
+		return 0;
 	} else if ((card->model == MODEL_8686) && (scratch & 0x7fff)) {
 		lbs_deb_sdio("firmware may be running\n");
-		goto success;
-	}
-
-	ret = lbs_get_firmware(&card->func->dev, lbs_helper_name, lbs_fw_name,
-				card->model, &fw_table[0], &helper, &mainfw);
-	if (ret) {
-		pr_err("failed to find firmware (%d)\n", ret);
-		goto out;
+		if_sdio_finish_power_on(card);
+		return 0;
 	}
 
-	ret = if_sdio_prog_helper(card, helper);
-	if (ret)
-		goto out;
-
-	lbs_deb_sdio("Helper firmware loaded\n");
-
-	ret = if_sdio_prog_real(card, mainfw);
-	if (ret)
-		goto out;
-
-	lbs_deb_sdio("Firmware loaded\n");
-
-success:
-	sdio_claim_host(card->func);
-	sdio_set_block_size(card->func, IF_SDIO_BLOCK_SIZE);
-	sdio_release_host(card->func);
-	ret = 0;
+	ret = lbs_get_firmware_async(card->priv, &card->func->dev, card->model,
+				     fw_table, if_sdio_do_prog_firmware);
 
 out:
-	if (helper)
-		release_firmware(helper);
-	if (mainfw)
-		release_firmware(mainfw);
-
 	lbs_deb_leave_args(LBS_DEB_SDIO, "ret %d", ret);
 	return ret;
 }
@@ -764,55 +760,15 @@ out:
 /* Power management                                                 */
 /********************************************************************/
 
-static int if_sdio_power_on(struct if_sdio_card *card)
+/* Finish power on sequence (after firmware is loaded) */
+static void if_sdio_finish_power_on(struct if_sdio_card *card)
 {
 	struct sdio_func *func = card->func;
 	struct lbs_private *priv = card->priv;
-	struct mmc_host *host = func->card->host;
 	int ret;
 
 	sdio_claim_host(func);
-
-	ret = sdio_enable_func(func);
-	if (ret)
-		goto release;
-
-	/* For 1-bit transfers to the 8686 model, we need to enable the
-	 * interrupt flag in the CCCR register. Set the MMC_QUIRK_LENIENT_FN0
-	 * bit to allow access to non-vendor registers. */
-	if ((card->model == MODEL_8686) &&
-	    (host->caps & MMC_CAP_SDIO_IRQ) &&
-	    (host->ios.bus_width == MMC_BUS_WIDTH_1)) {
-		u8 reg;
-
-		func->card->quirks |= MMC_QUIRK_LENIENT_FN0;
-		reg = sdio_f0_readb(func, SDIO_CCCR_IF, &ret);
-		if (ret)
-			goto disable;
-
-		reg |= SDIO_BUS_ECSI;
-		sdio_f0_writeb(func, reg, SDIO_CCCR_IF, &ret);
-		if (ret)
-			goto disable;
-	}
-
-	card->ioport = sdio_readb(func, IF_SDIO_IOPORT, &ret);
-	if (ret)
-		goto disable;
-
-	card->ioport |= sdio_readb(func, IF_SDIO_IOPORT + 1, &ret) << 8;
-	if (ret)
-		goto disable;
-
-	card->ioport |= sdio_readb(func, IF_SDIO_IOPORT + 2, &ret) << 16;
-	if (ret)
-		goto disable;
-
-	sdio_release_host(func);
-	ret = if_sdio_prog_firmware(card);
-	sdio_claim_host(func);
-	if (ret)
-		goto disable;
+	sdio_set_block_size(card->func, IF_SDIO_BLOCK_SIZE);
 
 	/*
 	 * Get rx_unit if the chip is SD8688 or newer.
@@ -837,7 +793,7 @@ static int if_sdio_power_on(struct if_sdio_card *card)
 	 */
 	ret = sdio_claim_irq(func, if_sdio_interrupt);
 	if (ret)
-		goto disable;
+		goto release;
 
 	/*
 	 * Enable interrupts now that everything is set up
@@ -863,11 +819,79 @@ static int if_sdio_power_on(struct if_sdio_card *card)
 	}
 
 	priv->fw_ready = 1;
+	wake_up(&card->pwron_waitq);
 
-	return 0;
+	if (!card->started) {
+		ret = lbs_start_card(priv);
+		if_sdio_power_off(card);
+		if (ret == 0) {
+			card->started = true;
+			/* Tell PM core that we don't need the card to be
+			 * powered now */
+			pm_runtime_put_noidle(&func->dev);
+		}
+	}
+
+	return;
 
 release_irq:
 	sdio_release_irq(func);
+release:
+	sdio_release_host(func);
+}
+
+static int if_sdio_power_on(struct if_sdio_card *card)
+{
+	struct sdio_func *func = card->func;
+	struct mmc_host *host = func->card->host;
+	int ret;
+
+	sdio_claim_host(func);
+
+	ret = sdio_enable_func(func);
+	if (ret)
+		goto release;
+
+	/* For 1-bit transfers to the 8686 model, we need to enable the
+	 * interrupt flag in the CCCR register. Set the MMC_QUIRK_LENIENT_FN0
+	 * bit to allow access to non-vendor registers. */
+	if ((card->model == MODEL_8686) &&
+	    (host->caps & MMC_CAP_SDIO_IRQ) &&
+	    (host->ios.bus_width == MMC_BUS_WIDTH_1)) {
+		u8 reg;
+
+		func->card->quirks |= MMC_QUIRK_LENIENT_FN0;
+		reg = sdio_f0_readb(func, SDIO_CCCR_IF, &ret);
+		if (ret)
+			goto disable;
+
+		reg |= SDIO_BUS_ECSI;
+		sdio_f0_writeb(func, reg, SDIO_CCCR_IF, &ret);
+		if (ret)
+			goto disable;
+	}
+
+	card->ioport = sdio_readb(func, IF_SDIO_IOPORT, &ret);
+	if (ret)
+		goto disable;
+
+	card->ioport |= sdio_readb(func, IF_SDIO_IOPORT + 1, &ret) << 8;
+	if (ret)
+		goto disable;
+
+	card->ioport |= sdio_readb(func, IF_SDIO_IOPORT + 2, &ret) << 16;
+	if (ret)
+		goto disable;
+
+	sdio_release_host(func);
+	ret = if_sdio_prog_firmware(card);
+	if (ret) {
+		sdio_disable_func(func);
+		return ret;
+	}
+
+	return 0;
+
 disable:
 	sdio_disable_func(func);
 release:
@@ -1074,11 +1098,17 @@ static int if_sdio_power_save(struct lbs_private *priv)
 static int if_sdio_power_restore(struct lbs_private *priv)
 {
 	struct if_sdio_card *card = priv->card;
+	int r;
 
 	/* Make sure the card will not be powered off by runtime PM */
 	pm_runtime_get_sync(&card->func->dev);
 
-	return if_sdio_power_on(card);
+	r = if_sdio_power_on(card);
+	if (r)
+		return r;
+
+	wait_event(card->pwron_waitq, priv->fw_ready);
+	return 0;
 }
 
 
@@ -1179,6 +1209,7 @@ static int if_sdio_probe(struct sdio_func *func,
 	spin_lock_init(&card->lock);
 	card->workqueue = create_workqueue("libertas_sdio");
 	INIT_WORK(&card->packet_worker, if_sdio_host_to_card_worker);
+	init_waitqueue_head(&card->pwron_waitq);
 
 	/* Check if we support this card */
 	for (i = 0; i < ARRAY_SIZE(fw_table); i++) {
@@ -1220,14 +1251,6 @@ static int if_sdio_probe(struct sdio_func *func,
 	if (ret)
 		goto err_activate_card;
 
-	ret = lbs_start_card(priv);
-	if_sdio_power_off(card);
-	if (ret)
-		goto err_activate_card;
-
-	/* Tell PM core that we don't need the card to be powered now */
-	pm_runtime_put_noidle(&func->dev);
-
 out:
 	lbs_deb_leave_args(LBS_DEB_SDIO, "ret %d", ret);
 
@@ -1244,10 +1267,6 @@ free:
 		kfree(packet);
 	}
 
-	if (card->helper_allocated)
-		kfree(card->helper);
-	if (card->firmware_allocated)
-		kfree(card->firmware);
 	kfree(card);
 
 	goto out;
@@ -1295,12 +1314,6 @@ static void if_sdio_remove(struct sdio_func *func)
 		kfree(packet);
 	}
 
-	if (card->helper_allocated)
-		kfree(card->helper);
-	if (card->firmware_allocated)
-		kfree(card->firmware);
-	kfree(card);
-
 	lbs_deb_leave(LBS_DEB_SDIO);
 }
 
diff --git a/drivers/net/wireless/libertas/if_spi.c b/drivers/net/wireless/libertas/if_spi.c
index 50b1ee7721e..9604a1c4a74 100644
--- a/drivers/net/wireless/libertas/if_spi.c
+++ b/drivers/net/wireless/libertas/if_spi.c
@@ -1064,9 +1064,8 @@ static int if_spi_init_card(struct if_spi_card *card)
 			goto out;
 		}
 
-		err = lbs_get_firmware(&card->spi->dev, NULL, NULL,
-					card->card_id, &fw_table[0], &helper,
-					&mainfw);
+		err = lbs_get_firmware(&card->spi->dev, card->card_id,
+					&fw_table[0], &helper, &mainfw);
 		if (err) {
 			netdev_err(priv->dev, "failed to find firmware (%d)\n",
 				   err);
@@ -1095,10 +1094,8 @@ static int if_spi_init_card(struct if_spi_card *card)
 		goto out;
 
 out:
-	if (helper)
-		release_firmware(helper);
-	if (mainfw)
-		release_firmware(mainfw);
+	release_firmware(helper);
+	release_firmware(mainfw);
 
 	lbs_deb_leave_args(LBS_DEB_SPI, "err %d\n", err);
 
diff --git a/drivers/net/wireless/libertas/if_usb.c b/drivers/net/wireless/libertas/if_usb.c
index 74da5f1ea24..cd3b0d40061 100644
--- a/drivers/net/wireless/libertas/if_usb.c
+++ b/drivers/net/wireless/libertas/if_usb.c
@@ -29,9 +29,6 @@
 
 #define MESSAGE_HEADER_LEN	4
 
-static char *lbs_fw_name = NULL;
-module_param_named(fw_name, lbs_fw_name, charp, 0644);
-
 MODULE_FIRMWARE("libertas/usb8388_v9.bin");
 MODULE_FIRMWARE("libertas/usb8388_v5.bin");
 MODULE_FIRMWARE("libertas/usb8388.bin");
@@ -44,6 +41,16 @@ enum {
 	MODEL_8682 = 0x2
 };
 
+/* table of firmware file names */
+static const struct lbs_fw_table fw_table[] = {
+	{ MODEL_8388, "libertas/usb8388_olpc.bin", NULL },
+	{ MODEL_8388, "libertas/usb8388_v9.bin", NULL },
+	{ MODEL_8388, "libertas/usb8388_v5.bin", NULL },
+	{ MODEL_8388, "libertas/usb8388.bin", NULL },
+	{ MODEL_8388, "usb8388.bin", NULL },
+	{ MODEL_8682, "libertas/usb8682.bin", NULL }
+};
+
 static struct usb_device_id if_usb_table[] = {
 	/* Enter the device signature inside */
 	{ USB_DEVICE(0x1286, 0x2001), .driver_info = MODEL_8388 },
@@ -55,10 +62,9 @@ MODULE_DEVICE_TABLE(usb, if_usb_table);
 
 static void if_usb_receive(struct urb *urb);
 static void if_usb_receive_fwload(struct urb *urb);
-static int __if_usb_prog_firmware(struct if_usb_card *cardp,
-					const char *fwname, int cmd);
-static int if_usb_prog_firmware(struct if_usb_card *cardp,
-					const char *fwname, int cmd);
+static void if_usb_prog_firmware(struct lbs_private *priv, int ret,
+				 const struct firmware *fw,
+				 const struct firmware *unused);
 static int if_usb_host_to_card(struct lbs_private *priv, uint8_t type,
 			       uint8_t *payload, uint16_t nb);
 static int usb_tx_block(struct if_usb_card *cardp, uint8_t *payload,
@@ -67,69 +73,6 @@ static void if_usb_free(struct if_usb_card *cardp);
 static int if_usb_submit_rx_urb(struct if_usb_card *cardp);
 static int if_usb_reset_device(struct if_usb_card *cardp);
 
-/* sysfs hooks */
-
-/*
- *  Set function to write firmware to device's persistent memory
- */
-static ssize_t if_usb_firmware_set(struct device *dev,
-		struct device_attribute *attr, const char *buf, size_t count)
-{
-	struct lbs_private *priv = to_net_dev(dev)->ml_priv;
-	struct if_usb_card *cardp = priv->card;
-	int ret;
-
-	BUG_ON(buf == NULL);
-
-	ret = if_usb_prog_firmware(cardp, buf, BOOT_CMD_UPDATE_FW);
-	if (ret == 0)
-		return count;
-
-	return ret;
-}
-
-/*
- * lbs_flash_fw attribute to be exported per ethX interface through sysfs
- * (/sys/class/net/ethX/lbs_flash_fw).  Use this like so to write firmware to
- * the device's persistent memory:
- * echo usb8388-5.126.0.p5.bin > /sys/class/net/ethX/lbs_flash_fw
- */
-static DEVICE_ATTR(lbs_flash_fw, 0200, NULL, if_usb_firmware_set);
-
-/**
- * if_usb_boot2_set - write firmware to device's persistent memory
- *
- * @dev: target device
- * @attr: device attributes
- * @buf: firmware buffer to write
- * @count: number of bytes to write
- *
- * returns: number of bytes written or negative error code
- */
-static ssize_t if_usb_boot2_set(struct device *dev,
-		struct device_attribute *attr, const char *buf, size_t count)
-{
-	struct lbs_private *priv = to_net_dev(dev)->ml_priv;
-	struct if_usb_card *cardp = priv->card;
-	int ret;
-
-	BUG_ON(buf == NULL);
-
-	ret = if_usb_prog_firmware(cardp, buf, BOOT_CMD_UPDATE_BOOT2);
-	if (ret == 0)
-		return count;
-
-	return ret;
-}
-
-/*
- * lbs_flash_boot2 attribute to be exported per ethX interface through sysfs
- * (/sys/class/net/ethX/lbs_flash_boot2).  Use this like so to write firmware
- * to the device's persistent memory:
- * echo usb8388-5.126.0.p5.bin > /sys/class/net/ethX/lbs_flash_boot2
- */
-static DEVICE_ATTR(lbs_flash_boot2, 0200, NULL, if_usb_boot2_set);
-
 /**
  * if_usb_write_bulk_callback - callback function to handle the status
  * of the URB
@@ -256,6 +199,7 @@ static int if_usb_probe(struct usb_interface *intf,
 	struct usb_endpoint_descriptor *endpoint;
 	struct lbs_private *priv;
 	struct if_usb_card *cardp;
+	int r = -ENOMEM;
 	int i;
 
 	udev = interface_to_usbdev(intf);
@@ -313,20 +257,10 @@ static int if_usb_probe(struct usb_interface *intf,
 		goto dealloc;
 	}
 
-	/* Upload firmware */
-	kparam_block_sysfs_write(fw_name);
-	if (__if_usb_prog_firmware(cardp, lbs_fw_name, BOOT_CMD_FW_BY_USB)) {
-		kparam_unblock_sysfs_write(fw_name);
-		lbs_deb_usbd(&udev->dev, "FW upload failed\n");
-		goto err_prog_firmware;
-	}
-	kparam_unblock_sysfs_write(fw_name);
-
 	if (!(priv = lbs_add_card(cardp, &intf->dev)))
-		goto err_prog_firmware;
+		goto err_add_card;
 
 	cardp->priv = priv;
-	cardp->priv->fw_ready = 1;
 
 	priv->hw_host_to_card = if_usb_host_to_card;
 	priv->enter_deep_sleep = NULL;
@@ -339,42 +273,25 @@ static int if_usb_probe(struct usb_interface *intf,
 
 	cardp->boot2_version = udev->descriptor.bcdDevice;
 
-	if_usb_submit_rx_urb(cardp);
-
-	if (lbs_start_card(priv))
-		goto err_start_card;
-
-	if_usb_setup_firmware(priv);
-
 	usb_get_dev(udev);
 	usb_set_intfdata(intf, cardp);
 
-	if (device_create_file(&priv->dev->dev, &dev_attr_lbs_flash_fw))
-		netdev_err(priv->dev,
-			   "cannot register lbs_flash_fw attribute\n");
-
-	if (device_create_file(&priv->dev->dev, &dev_attr_lbs_flash_boot2))
-		netdev_err(priv->dev,
-			   "cannot register lbs_flash_boot2 attribute\n");
-
-	/*
-	 * EHS_REMOVE_WAKEUP is not supported on all versions of the firmware.
-	 */
-	priv->wol_criteria = EHS_REMOVE_WAKEUP;
-	if (lbs_host_sleep_cfg(priv, priv->wol_criteria, NULL))
-		priv->ehs_remove_supported = false;
+	r = lbs_get_firmware_async(priv, &udev->dev, cardp->model,
+				   fw_table, if_usb_prog_firmware);
+	if (r)
+		goto err_get_fw;
 
 	return 0;
 
-err_start_card:
+err_get_fw:
 	lbs_remove_card(priv);
-err_prog_firmware:
+err_add_card:
 	if_usb_reset_device(cardp);
 dealloc:
 	if_usb_free(cardp);
 
 error:
-	return -ENOMEM;
+	return r;
 }
 
 /**
@@ -389,9 +306,6 @@ static void if_usb_disconnect(struct usb_interface *intf)
 
 	lbs_deb_enter(LBS_DEB_MAIN);
 
-	device_remove_file(&priv->dev->dev, &dev_attr_lbs_flash_boot2);
-	device_remove_file(&priv->dev->dev, &dev_attr_lbs_flash_fw);
-
 	cardp->surprise_removed = 1;
 
 	if (priv) {
@@ -912,121 +826,22 @@ static int check_fwfile_format(const uint8_t *data, uint32_t totlen)
 	return ret;
 }
 
-
-/**
-*  if_usb_prog_firmware - programs the firmware subject to cmd
-*
-*  @cardp:	the if_usb_card descriptor
-*  @fwname:	firmware or boot2 image file name
-*  @cmd:	either BOOT_CMD_FW_BY_USB, BOOT_CMD_UPDATE_FW,
-*		or BOOT_CMD_UPDATE_BOOT2.
-*  returns:	0 or error code
-*/
-static int if_usb_prog_firmware(struct if_usb_card *cardp,
-				const char *fwname, int cmd)
-{
-	struct lbs_private *priv = cardp->priv;
-	unsigned long flags, caps;
-	int ret;
-
-	caps = priv->fwcapinfo;
-	if (((cmd == BOOT_CMD_UPDATE_FW) && !(caps & FW_CAPINFO_FIRMWARE_UPGRADE)) ||
-	    ((cmd == BOOT_CMD_UPDATE_BOOT2) && !(caps & FW_CAPINFO_BOOT2_UPGRADE)))
-		return -EOPNOTSUPP;
-
-	/* Ensure main thread is idle. */
-	spin_lock_irqsave(&priv->driver_lock, flags);
-	while (priv->cur_cmd != NULL || priv->dnld_sent != DNLD_RES_RECEIVED) {
-		spin_unlock_irqrestore(&priv->driver_lock, flags);
-		if (wait_event_interruptible(priv->waitq,
-				(priv->cur_cmd == NULL &&
-				priv->dnld_sent == DNLD_RES_RECEIVED))) {
-			return -ERESTARTSYS;
-		}
-		spin_lock_irqsave(&priv->driver_lock, flags);
-	}
-	priv->dnld_sent = DNLD_BOOTCMD_SENT;
-	spin_unlock_irqrestore(&priv->driver_lock, flags);
-
-	ret = __if_usb_prog_firmware(cardp, fwname, cmd);
-
-	spin_lock_irqsave(&priv->driver_lock, flags);
-	priv->dnld_sent = DNLD_RES_RECEIVED;
-	spin_unlock_irqrestore(&priv->driver_lock, flags);
-
-	wake_up(&priv->waitq);
-
-	return ret;
-}
-
-/* table of firmware file names */
-static const struct {
-	u32 model;
-	const char *fwname;
-} fw_table[] = {
-	{ MODEL_8388, "libertas/usb8388_v9.bin" },
-	{ MODEL_8388, "libertas/usb8388_v5.bin" },
-	{ MODEL_8388, "libertas/usb8388.bin" },
-	{ MODEL_8388, "usb8388.bin" },
-	{ MODEL_8682, "libertas/usb8682.bin" }
-};
-
-#ifdef CONFIG_OLPC
-
-static int try_olpc_fw(struct if_usb_card *cardp)
-{
-	int retval = -ENOENT;
-
-	/* try the OLPC firmware first; fall back to fw_table list */
-	if (machine_is_olpc() && cardp->model == MODEL_8388)
-		retval = request_firmware(&cardp->fw,
-				"libertas/usb8388_olpc.bin", &cardp->udev->dev);
-	return retval;
-}
-
-#else
-static int try_olpc_fw(struct if_usb_card *cardp) { return -ENOENT; }
-#endif /* !CONFIG_OLPC */
-
-static int get_fw(struct if_usb_card *cardp, const char *fwname)
-{
-	int i;
-
-	/* Try user-specified firmware first */
-	if (fwname)
-		return request_firmware(&cardp->fw, fwname, &cardp->udev->dev);
-
-	/* Handle OLPC firmware */
-	if (try_olpc_fw(cardp) == 0)
-		return 0;
-
-	/* Otherwise search for firmware to use */
-	for (i = 0; i < ARRAY_SIZE(fw_table); i++) {
-		if (fw_table[i].model != cardp->model)
-			continue;
-		if (request_firmware(&cardp->fw, fw_table[i].fwname,
-					&cardp->udev->dev) == 0)
-			return 0;
-	}
-
-	return -ENOENT;
-}
-
-static int __if_usb_prog_firmware(struct if_usb_card *cardp,
-					const char *fwname, int cmd)
+static void if_usb_prog_firmware(struct lbs_private *priv, int ret,
+				 const struct firmware *fw,
+				 const struct firmware *unused)
 {
+	struct if_usb_card *cardp = priv->card;
 	int i = 0;
 	static int reset_count = 10;
-	int ret = 0;
 
 	lbs_deb_enter(LBS_DEB_USB);
 
-	ret = get_fw(cardp, fwname);
 	if (ret) {
 		pr_err("failed to find firmware (%d)\n", ret);
 		goto done;
 	}
 
+	cardp->fw = fw;
 	if (check_fwfile_format(cardp->fw->data, cardp->fw->size)) {
 		ret = -EINVAL;
 		goto release_fw;
@@ -1053,7 +868,7 @@ restart:
 	do {
 		int j = 0;
 		i++;
-		if_usb_issue_boot_command(cardp, cmd);
+		if_usb_issue_boot_command(cardp, BOOT_CMD_FW_BY_USB);
 		/* wait for command response */
 		do {
 			j++;
@@ -1109,13 +924,27 @@ restart:
 		goto release_fw;
 	}
 
+	cardp->priv->fw_ready = 1;
+	if_usb_submit_rx_urb(cardp);
+
+	if (lbs_start_card(priv))
+		goto release_fw;
+
+	if_usb_setup_firmware(priv);
+
+	/*
+	 * EHS_REMOVE_WAKEUP is not supported on all versions of the firmware.
+	 */
+	priv->wol_criteria = EHS_REMOVE_WAKEUP;
+	if (lbs_host_sleep_cfg(priv, priv->wol_criteria, NULL))
+		priv->ehs_remove_supported = false;
+
  release_fw:
 	release_firmware(cardp->fw);
 	cardp->fw = NULL;
 
  done:
-	lbs_deb_leave_args(LBS_DEB_USB, "ret %d", ret);
-	return ret;
+	lbs_deb_leave(LBS_DEB_USB);
 }
 
 
@@ -1128,8 +957,10 @@ static int if_usb_suspend(struct usb_interface *intf, pm_message_t message)
 
 	lbs_deb_enter(LBS_DEB_USB);
 
-	if (priv->psstate != PS_STATE_FULL_POWER)
-		return -1;
+	if (priv->psstate != PS_STATE_FULL_POWER) {
+		ret = -1;
+		goto out;
+	}
 
 #ifdef CONFIG_OLPC
 	if (machine_is_olpc()) {
@@ -1180,6 +1011,7 @@ static struct usb_driver if_usb_driver = {
 	.suspend = if_usb_suspend,
 	.resume = if_usb_resume,
 	.reset_resume = if_usb_resume,
+	.disable_hub_initiated_lpm = 1,
 };
 
 module_usb_driver(if_usb_driver);
diff --git a/drivers/net/wireless/libertas/main.c b/drivers/net/wireless/libertas/main.c
index 957681dede1..e96ee0aa843 100644
--- a/drivers/net/wireless/libertas/main.c
+++ b/drivers/net/wireless/libertas/main.c
@@ -878,6 +878,7 @@ static int lbs_init_adapter(struct lbs_private *priv)
 	priv->is_host_sleep_configured = 0;
 	priv->is_host_sleep_activated = 0;
 	init_waitqueue_head(&priv->host_sleep_q);
+	init_waitqueue_head(&priv->fw_waitq);
 	mutex_init(&priv->lock);
 
 	setup_timer(&priv->command_timer, lbs_cmd_timeout_handler,
@@ -1033,7 +1034,11 @@ void lbs_remove_card(struct lbs_private *priv)
 	lbs_deb_enter(LBS_DEB_MAIN);
 
 	lbs_remove_mesh(priv);
-	lbs_scan_deinit(priv);
+
+	if (priv->wiphy_registered)
+		lbs_scan_deinit(priv);
+
+	lbs_wait_for_firmware_load(priv);
 
 	/* worker thread destruction blocks on the in-flight command which
 	 * should have been cleared already in lbs_stop_card().
@@ -1128,6 +1133,11 @@ void lbs_stop_card(struct lbs_private *priv)
 		goto out;
 	dev = priv->dev;
 
+	/* If the netdev isn't registered, it means that lbs_start_card() was
+	 * never called so we have nothing to do here. */
+	if (dev->reg_state != NETREG_REGISTERED)
+		goto out;
+
 	netif_stop_queue(dev);
 	netif_carrier_off(dev);
 
@@ -1177,111 +1187,6 @@ void lbs_notify_command_response(struct lbs_private *priv, u8 resp_idx)
 }
 EXPORT_SYMBOL_GPL(lbs_notify_command_response);
 
-/**
- *  lbs_get_firmware - Retrieves two-stage firmware
- *
- *  @dev:     	A pointer to &device structure
- *  @user_helper: User-defined helper firmware file
- *  @user_mainfw: User-defined main firmware file
- *  @card_model: Bus-specific card model ID used to filter firmware table
- *		elements
- *  @fw_table:	Table of firmware file names and device model numbers
- *		terminated by an entry with a NULL helper name
- *  @helper:	On success, the helper firmware; caller must free
- *  @mainfw:	On success, the main firmware; caller must free
- *
- *  returns:		0 on success, non-zero on failure
- */
-int lbs_get_firmware(struct device *dev, const char *user_helper,
-			const char *user_mainfw, u32 card_model,
-			const struct lbs_fw_table *fw_table,
-			const struct firmware **helper,
-			const struct firmware **mainfw)
-{
-	const struct lbs_fw_table *iter;
-	int ret;
-
-	BUG_ON(helper == NULL);
-	BUG_ON(mainfw == NULL);
-
-	/* Try user-specified firmware first */
-	if (user_helper) {
-		ret = request_firmware(helper, user_helper, dev);
-		if (ret) {
-			dev_err(dev, "couldn't find helper firmware %s\n",
-				user_helper);
-			goto fail;
-		}
-	}
-	if (user_mainfw) {
-		ret = request_firmware(mainfw, user_mainfw, dev);
-		if (ret) {
-			dev_err(dev, "couldn't find main firmware %s\n",
-				user_mainfw);
-			goto fail;
-		}
-	}
-
-	if (*helper && *mainfw)
-		return 0;
-
-	/* Otherwise search for firmware to use.  If neither the helper or
-	 * the main firmware were specified by the user, then we need to
-	 * make sure that found helper & main are from the same entry in
-	 * fw_table.
-	 */
-	iter = fw_table;
-	while (iter && iter->helper) {
-		if (iter->model != card_model)
-			goto next;
-
-		if (*helper == NULL) {
-			ret = request_firmware(helper, iter->helper, dev);
-			if (ret)
-				goto next;
-
-			/* If the device has one-stage firmware (ie cf8305) and
-			 * we've got it then we don't need to bother with the
-			 * main firmware.
-			 */
-			if (iter->fwname == NULL)
-				return 0;
-		}
-
-		if (*mainfw == NULL) {
-			ret = request_firmware(mainfw, iter->fwname, dev);
-			if (ret && !user_helper) {
-				/* Clear the helper if it wasn't user-specified
-				 * and the main firmware load failed, to ensure
-				 * we don't have mismatched firmware pairs.
-				 */
-				release_firmware(*helper);
-				*helper = NULL;
-			}
-		}
-
-		if (*helper && *mainfw)
-			return 0;
-
-  next:
-		iter++;
-	}
-
-  fail:
-	/* Failed */
-	if (*helper) {
-		release_firmware(*helper);
-		*helper = NULL;
-	}
-	if (*mainfw) {
-		release_firmware(*mainfw);
-		*mainfw = NULL;
-	}
-
-	return -ENOENT;
-}
-EXPORT_SYMBOL_GPL(lbs_get_firmware);
-
 static int __init lbs_init_module(void)
 {
 	lbs_deb_enter(LBS_DEB_MAIN);
diff --git a/drivers/net/wireless/libertas_tf/if_usb.c b/drivers/net/wireless/libertas_tf/if_usb.c
index 7ced130f4f9..19a5a92dd77 100644
--- a/drivers/net/wireless/libertas_tf/if_usb.c
+++ b/drivers/net/wireless/libertas_tf/if_usb.c
@@ -920,6 +920,7 @@ static struct usb_driver if_usb_driver = {
 	.id_table = if_usb_table,
 	.suspend = if_usb_suspend,
 	.resume = if_usb_resume,
+	.disable_hub_initiated_lpm = 1,
 };
 
 module_usb_driver(if_usb_driver);
diff --git a/drivers/net/wireless/mac80211_hwsim.c b/drivers/net/wireless/mac80211_hwsim.c
index b7ce6a6e355..fb787df0166 100644
--- a/drivers/net/wireless/mac80211_hwsim.c
+++ b/drivers/net/wireless/mac80211_hwsim.c
@@ -582,11 +582,13 @@ static void mac80211_hwsim_tx_frame_nl(struct ieee80211_hw *hw,
 		goto nla_put_failure;
 	}
 
-	NLA_PUT(skb, HWSIM_ATTR_ADDR_TRANSMITTER,
-		     sizeof(struct mac_address), data->addresses[1].addr);
+	if (nla_put(skb, HWSIM_ATTR_ADDR_TRANSMITTER,
+		    sizeof(struct mac_address), data->addresses[1].addr))
+		goto nla_put_failure;
 
 	/* We get the skb->data */
-	NLA_PUT(skb, HWSIM_ATTR_FRAME, my_skb->len, my_skb->data);
+	if (nla_put(skb, HWSIM_ATTR_FRAME, my_skb->len, my_skb->data))
+		goto nla_put_failure;
 
 	/* We get the flags for this transmission, and we translate them to
 	   wmediumd flags  */
@@ -597,7 +599,8 @@ static void mac80211_hwsim_tx_frame_nl(struct ieee80211_hw *hw,
 	if (info->flags & IEEE80211_TX_CTL_NO_ACK)
 		hwsim_flags |= HWSIM_TX_CTL_NO_ACK;
 
-	NLA_PUT_U32(skb, HWSIM_ATTR_FLAGS, hwsim_flags);
+	if (nla_put_u32(skb, HWSIM_ATTR_FLAGS, hwsim_flags))
+		goto nla_put_failure;
 
 	/* We get the tx control (rate and retries) info*/
 
@@ -606,12 +609,14 @@ static void mac80211_hwsim_tx_frame_nl(struct ieee80211_hw *hw,
 		tx_attempts[i].count = info->status.rates[i].count;
 	}
 
-	NLA_PUT(skb, HWSIM_ATTR_TX_INFO,
-		     sizeof(struct hwsim_tx_rate)*IEEE80211_TX_MAX_RATES,
-		     tx_attempts);
+	if (nla_put(skb, HWSIM_ATTR_TX_INFO,
+		    sizeof(struct hwsim_tx_rate)*IEEE80211_TX_MAX_RATES,
+		    tx_attempts))
+		goto nla_put_failure;
 
 	/* We create a cookie to identify this skb */
-	NLA_PUT_U64(skb, HWSIM_ATTR_COOKIE, (unsigned long) my_skb);
+	if (nla_put_u64(skb, HWSIM_ATTR_COOKIE, (unsigned long) my_skb))
+		goto nla_put_failure;
 
 	genlmsg_end(skb, msg_head);
 	genlmsg_unicast(&init_net, skb, dst_pid);
@@ -632,6 +637,7 @@ static bool mac80211_hwsim_tx_frame_no_nl(struct ieee80211_hw *hw,
 	struct ieee80211_hdr *hdr = (struct ieee80211_hdr *) skb->data;
 	struct ieee80211_tx_info *info = IEEE80211_SKB_CB(skb);
 	struct ieee80211_rx_status rx_status;
+	struct ieee80211_rate *txrate = ieee80211_get_tx_rate(hw, info);
 
 	if (data->idle) {
 		wiphy_debug(hw->wiphy, "Trying to TX when idle - reject\n");
@@ -666,6 +672,7 @@ static bool mac80211_hwsim_tx_frame_no_nl(struct ieee80211_hw *hw,
 	spin_lock(&hwsim_radio_lock);
 	list_for_each_entry(data2, &hwsim_radios, list) {
 		struct sk_buff *nskb;
+		struct ieee80211_mgmt *mgmt;
 
 		if (data == data2)
 			continue;
@@ -683,8 +690,18 @@ static bool mac80211_hwsim_tx_frame_no_nl(struct ieee80211_hw *hw,
 
 		if (mac80211_hwsim_addr_match(data2, hdr->addr1))
 			ack = true;
+
+		/* set bcn timestamp relative to receiver mactime */
 		rx_status.mactime =
-			le64_to_cpu(__mac80211_hwsim_get_tsf(data2));
+				le64_to_cpu(__mac80211_hwsim_get_tsf(data2));
+		mgmt = (struct ieee80211_mgmt *) nskb->data;
+		if (ieee80211_is_beacon(mgmt->frame_control) ||
+		    ieee80211_is_probe_resp(mgmt->frame_control))
+			mgmt->u.beacon.timestamp = cpu_to_le64(
+				rx_status.mactime +
+				(data->tsf_offset - data2->tsf_offset) +
+				24 * 8 * 10 / txrate->bitrate);
+
 		memcpy(IEEE80211_SKB_RXCB(nskb), &rx_status, sizeof(rx_status));
 		ieee80211_rx_irqsafe(data2->hw, nskb);
 	}
@@ -698,12 +715,6 @@ static void mac80211_hwsim_tx(struct ieee80211_hw *hw, struct sk_buff *skb)
 	bool ack;
 	struct ieee80211_tx_info *txi;
 	u32 _pid;
-	struct ieee80211_mgmt *mgmt = (struct ieee80211_mgmt *) skb->data;
-	struct mac80211_hwsim_data *data = hw->priv;
-
-	if (ieee80211_is_beacon(mgmt->frame_control) ||
-	    ieee80211_is_probe_resp(mgmt->frame_control))
-		mgmt->u.beacon.timestamp = __mac80211_hwsim_get_tsf(data);
 
 	mac80211_hwsim_monitor_rx(hw, skb);
 
@@ -735,6 +746,11 @@ static void mac80211_hwsim_tx(struct ieee80211_hw *hw, struct sk_buff *skb)
 		hwsim_check_sta_magic(txi->control.sta);
 
 	ieee80211_tx_info_clear_status(txi);
+
+	/* frame was transmitted at most favorable rate at first attempt */
+	txi->control.rates[0].count = 1;
+	txi->control.rates[1].idx = -1;
+
 	if (!(txi->flags & IEEE80211_TX_CTL_NO_ACK) && ack)
 		txi->flags |= IEEE80211_TX_STAT_ACK;
 	ieee80211_tx_status_irqsafe(hw, skb);
@@ -800,11 +816,9 @@ static void mac80211_hwsim_beacon_tx(void *arg, u8 *mac,
 				     struct ieee80211_vif *vif)
 {
 	struct ieee80211_hw *hw = arg;
-	struct mac80211_hwsim_data *data = hw->priv;
 	struct sk_buff *skb;
 	struct ieee80211_tx_info *info;
 	u32 _pid;
-	struct ieee80211_mgmt *mgmt;
 
 	hwsim_check_magic(vif);
 
@@ -818,9 +832,6 @@ static void mac80211_hwsim_beacon_tx(void *arg, u8 *mac,
 		return;
 	info = IEEE80211_SKB_CB(skb);
 
-	mgmt = (struct ieee80211_mgmt *) skb->data;
-	mgmt->u.beacon.timestamp = __mac80211_hwsim_get_tsf(data);
-
 	mac80211_hwsim_monitor_rx(hw, skb);
 
 	/* wmediumd mode check */
@@ -1108,7 +1119,8 @@ static int mac80211_hwsim_testmode_cmd(struct ieee80211_hw *hw,
 						nla_total_size(sizeof(u32)));
 		if (!skb)
 			return -ENOMEM;
-		NLA_PUT_U32(skb, HWSIM_TM_ATTR_PS, hwsim->ps);
+		if (nla_put_u32(skb, HWSIM_TM_ATTR_PS, hwsim->ps))
+			goto nla_put_failure;
 		return cfg80211_testmode_reply(skb);
 	default:
 		return -EOPNOTSUPP;
@@ -1444,7 +1456,7 @@ DEFINE_SIMPLE_ATTRIBUTE(hwsim_fops_group,
 			hwsim_fops_group_read, hwsim_fops_group_write,
 			"%llx\n");
 
-struct mac80211_hwsim_data *get_hwsim_data_ref_from_addr(
+static struct mac80211_hwsim_data *get_hwsim_data_ref_from_addr(
 			     struct mac_address *addr)
 {
 	struct mac80211_hwsim_data *data;
@@ -1789,9 +1801,11 @@ static int __init init_mac80211_hwsim(void)
 			    IEEE80211_HW_SIGNAL_DBM |
 			    IEEE80211_HW_SUPPORTS_STATIC_SMPS |
 			    IEEE80211_HW_SUPPORTS_DYNAMIC_SMPS |
-			    IEEE80211_HW_AMPDU_AGGREGATION;
+			    IEEE80211_HW_AMPDU_AGGREGATION |
+			    IEEE80211_HW_WANT_MONITOR_VIF;
 
-		hw->wiphy->flags |= WIPHY_FLAG_SUPPORTS_TDLS;
+		hw->wiphy->flags |= WIPHY_FLAG_SUPPORTS_TDLS |
+				    WIPHY_FLAG_HAS_REMAIN_ON_CHANNEL;
 
 		/* ask mac80211 to reserve space for magic */
 		hw->vif_data_size = sizeof(struct hwsim_vif_priv);
diff --git a/drivers/net/wireless/mwifiex/11n.c b/drivers/net/wireless/mwifiex/11n.c
index a5e182b5e94..fe8ebfebcc0 100644
--- a/drivers/net/wireless/mwifiex/11n.c
+++ b/drivers/net/wireless/mwifiex/11n.c
@@ -350,25 +350,26 @@ mwifiex_cmd_append_11n_tlv(struct mwifiex_private *priv,
 		ret_len += sizeof(struct mwifiex_ie_types_htcap);
 	}
 
-	if (bss_desc->bcn_ht_info) {
+	if (bss_desc->bcn_ht_oper) {
 		if (priv->bss_mode == NL80211_IFTYPE_ADHOC) {
 			ht_info = (struct mwifiex_ie_types_htinfo *) *buffer;
 			memset(ht_info, 0,
 			       sizeof(struct mwifiex_ie_types_htinfo));
 			ht_info->header.type =
-					cpu_to_le16(WLAN_EID_HT_INFORMATION);
+					cpu_to_le16(WLAN_EID_HT_OPERATION);
 			ht_info->header.len =
-				cpu_to_le16(sizeof(struct ieee80211_ht_info));
+				cpu_to_le16(
+					sizeof(struct ieee80211_ht_operation));
 
 			memcpy((u8 *) ht_info +
 			       sizeof(struct mwifiex_ie_types_header),
-			       (u8 *) bss_desc->bcn_ht_info +
+			       (u8 *) bss_desc->bcn_ht_oper +
 			       sizeof(struct ieee_types_header),
 			       le16_to_cpu(ht_info->header.len));
 
 			if (!(sband->ht_cap.cap &
 					IEEE80211_HT_CAP_SUP_WIDTH_20_40))
-				ht_info->ht_info.ht_param &=
+				ht_info->ht_oper.ht_param &=
 					~(IEEE80211_HT_PARAM_CHAN_WIDTH_ANY |
 					IEEE80211_HT_PARAM_CHA_SEC_OFFSET);
 
@@ -385,16 +386,16 @@ mwifiex_cmd_append_11n_tlv(struct mwifiex_private *priv,
 			sizeof(struct mwifiex_ie_types_chan_list_param_set) -
 			sizeof(struct mwifiex_ie_types_header));
 		chan_list->chan_scan_param[0].chan_number =
-			bss_desc->bcn_ht_info->control_chan;
+			bss_desc->bcn_ht_oper->primary_chan;
 		chan_list->chan_scan_param[0].radio_type =
 			mwifiex_band_to_radio_type((u8) bss_desc->bss_band);
 
 		if (sband->ht_cap.cap & IEEE80211_HT_CAP_SUP_WIDTH_20_40 &&
-		    bss_desc->bcn_ht_info->ht_param &
+		    bss_desc->bcn_ht_oper->ht_param &
 		    IEEE80211_HT_PARAM_CHAN_WIDTH_ANY)
 			SET_SECONDARYCHAN(chan_list->chan_scan_param[0].
 					  radio_type,
-					  (bss_desc->bcn_ht_info->ht_param &
+					  (bss_desc->bcn_ht_oper->ht_param &
 					  IEEE80211_HT_PARAM_CHA_SEC_OFFSET));
 
 		*buffer += sizeof(struct mwifiex_ie_types_chan_list_param_set);
diff --git a/drivers/net/wireless/mwifiex/11n_aggr.c b/drivers/net/wireless/mwifiex/11n_aggr.c
index 9eefb2a0ce9..ab84eb94374 100644
--- a/drivers/net/wireless/mwifiex/11n_aggr.c
+++ b/drivers/net/wireless/mwifiex/11n_aggr.c
@@ -233,21 +233,27 @@ mwifiex_11n_aggregate_pkt(struct mwifiex_private *priv,
 
 	skb_push(skb_aggr, headroom);
 
-	/*
-	 * Padding per MSDU will affect the length of next
-	 * packet and hence the exact length of next packet
-	 * is uncertain here.
-	 *
-	 * Also, aggregation of transmission buffer, while
-	 * downloading the data to the card, wont gain much
-	 * on the AMSDU packets as the AMSDU packets utilizes
-	 * the transmission buffer space to the maximum
-	 * (adapter->tx_buf_size).
-	 */
-	tx_param.next_pkt_len = 0;
-
-	ret = adapter->if_ops.host_to_card(adapter, MWIFIEX_TYPE_DATA,
-					   skb_aggr, &tx_param);
+	if (adapter->iface_type == MWIFIEX_USB) {
+		adapter->data_sent = true;
+		ret = adapter->if_ops.host_to_card(adapter, MWIFIEX_USB_EP_DATA,
+						   skb_aggr, NULL);
+	} else {
+		/*
+		 * Padding per MSDU will affect the length of next
+		 * packet and hence the exact length of next packet
+		 * is uncertain here.
+		 *
+		 * Also, aggregation of transmission buffer, while
+		 * downloading the data to the card, wont gain much
+		 * on the AMSDU packets as the AMSDU packets utilizes
+		 * the transmission buffer space to the maximum
+		 * (adapter->tx_buf_size).
+		 */
+		tx_param.next_pkt_len = 0;
+
+		ret = adapter->if_ops.host_to_card(adapter, MWIFIEX_TYPE_DATA,
+						   skb_aggr, &tx_param);
+	}
 	switch (ret) {
 	case -EBUSY:
 		spin_lock_irqsave(&priv->wmm.ra_list_spinlock, ra_list_flags);
diff --git a/drivers/net/wireless/mwifiex/Kconfig b/drivers/net/wireless/mwifiex/Kconfig
index 2a078cea830..8e384fae3e6 100644
--- a/drivers/net/wireless/mwifiex/Kconfig
+++ b/drivers/net/wireless/mwifiex/Kconfig
@@ -10,12 +10,12 @@ config MWIFIEX
 	  mwifiex.
 
 config MWIFIEX_SDIO
-	tristate "Marvell WiFi-Ex Driver for SD8787/SD8797"
+	tristate "Marvell WiFi-Ex Driver for SD8786/SD8787/SD8797"
 	depends on MWIFIEX && MMC
 	select FW_LOADER
 	---help---
 	  This adds support for wireless adapters based on Marvell
-	  8787/8797 chipsets with SDIO interface.
+	  8786/8787/8797 chipsets with SDIO interface.
 
 	  If you choose to build it as a module, it will be called
 	  mwifiex_sdio.
@@ -30,3 +30,14 @@ config MWIFIEX_PCIE
 
 	  If you choose to build it as a module, it will be called
 	  mwifiex_pcie.
+
+config MWIFIEX_USB
+	tristate "Marvell WiFi-Ex Driver for USB8797"
+	depends on MWIFIEX && USB
+	select FW_LOADER
+	---help---
+	  This adds support for wireless adapters based on Marvell
+	  Avastar 88W8797 chipset with USB interface.
+
+	  If you choose to build it as a module, it will be called
+	  mwifiex_usb.
diff --git a/drivers/net/wireless/mwifiex/Makefile b/drivers/net/wireless/mwifiex/Makefile
index b0257ad1bbe..3f66ebb0a63 100644
--- a/drivers/net/wireless/mwifiex/Makefile
+++ b/drivers/net/wireless/mwifiex/Makefile
@@ -29,6 +29,8 @@ mwifiex-y += scan.o
 mwifiex-y += join.o
 mwifiex-y += sta_ioctl.o
 mwifiex-y += sta_cmd.o
+mwifiex-y += uap_cmd.o
+mwifiex-y += ie.o
 mwifiex-y += sta_cmdresp.o
 mwifiex-y += sta_event.o
 mwifiex-y += sta_tx.o
@@ -42,3 +44,6 @@ obj-$(CONFIG_MWIFIEX_SDIO) += mwifiex_sdio.o
 
 mwifiex_pcie-y += pcie.o
 obj-$(CONFIG_MWIFIEX_PCIE) += mwifiex_pcie.o
+
+mwifiex_usb-y += usb.o
+obj-$(CONFIG_MWIFIEX_USB) += mwifiex_usb.o
diff --git a/drivers/net/wireless/mwifiex/cfg80211.c b/drivers/net/wireless/mwifiex/cfg80211.c
index 65050384c42..87671446e24 100644
--- a/drivers/net/wireless/mwifiex/cfg80211.c
+++ b/drivers/net/wireless/mwifiex/cfg80211.c
@@ -20,6 +20,23 @@
 #include "cfg80211.h"
 #include "main.h"
 
+static const struct ieee80211_iface_limit mwifiex_ap_sta_limits[] = {
+	{
+		.max = 1, .types = BIT(NL80211_IFTYPE_STATION),
+	},
+	{
+		.max = 1, .types = BIT(NL80211_IFTYPE_AP),
+	},
+};
+
+static const struct ieee80211_iface_combination mwifiex_iface_comb_ap_sta = {
+	.limits = mwifiex_ap_sta_limits,
+	.num_different_channels = 1,
+	.n_limits = ARRAY_SIZE(mwifiex_ap_sta_limits),
+	.max_interfaces = MWIFIEX_MAX_BSS_NUM,
+	.beacon_int_infra_match = true,
+};
+
 /*
  * This function maps the nl802.11 channel type into driver channel type.
  *
@@ -67,7 +84,7 @@ mwifiex_is_alg_wep(u32 cipher)
 /*
  * This function retrieves the private structure from kernel wiphy structure.
  */
-static void *mwifiex_cfg80211_get_priv(struct wiphy *wiphy)
+static void *mwifiex_cfg80211_get_adapter(struct wiphy *wiphy)
 {
 	return (void *) (*(unsigned long *) wiphy_priv(wiphy));
 }
@@ -80,8 +97,10 @@ mwifiex_cfg80211_del_key(struct wiphy *wiphy, struct net_device *netdev,
 			 u8 key_index, bool pairwise, const u8 *mac_addr)
 {
 	struct mwifiex_private *priv = mwifiex_netdev_get_priv(netdev);
+	const u8 bc_mac[] = {0xff, 0xff, 0xff, 0xff, 0xff, 0xff};
+	const u8 *peer_mac = pairwise ? mac_addr : bc_mac;
 
-	if (mwifiex_set_encode(priv, NULL, 0, key_index, 1)) {
+	if (mwifiex_set_encode(priv, NULL, 0, key_index, peer_mac, 1)) {
 		wiphy_err(wiphy, "deleting the crypto keys\n");
 		return -EFAULT;
 	}
@@ -98,7 +117,8 @@ mwifiex_cfg80211_set_tx_power(struct wiphy *wiphy,
 			      enum nl80211_tx_power_setting type,
 			      int mbm)
 {
-	struct mwifiex_private *priv = mwifiex_cfg80211_get_priv(wiphy);
+	struct mwifiex_adapter *adapter = mwifiex_cfg80211_get_adapter(wiphy);
+	struct mwifiex_private *priv;
 	struct mwifiex_power_cfg power_cfg;
 	int dbm = MBM_TO_DBM(mbm);
 
@@ -109,6 +129,8 @@ mwifiex_cfg80211_set_tx_power(struct wiphy *wiphy,
 		power_cfg.is_power_auto = 1;
 	}
 
+	priv = mwifiex_get_priv(adapter, MWIFIEX_BSS_ROLE_ANY);
+
 	return mwifiex_set_tx_power(priv, &power_cfg);
 }
 
@@ -148,7 +170,7 @@ mwifiex_cfg80211_set_default_key(struct wiphy *wiphy, struct net_device *netdev,
 	if (!priv->sec_info.wep_enabled)
 		return 0;
 
-	if (mwifiex_set_encode(priv, NULL, 0, key_index, 0)) {
+	if (mwifiex_set_encode(priv, NULL, 0, key_index, NULL, 0)) {
 		wiphy_err(wiphy, "set default Tx key index\n");
 		return -EFAULT;
 	}
@@ -165,9 +187,11 @@ mwifiex_cfg80211_add_key(struct wiphy *wiphy, struct net_device *netdev,
 			 struct key_params *params)
 {
 	struct mwifiex_private *priv = mwifiex_netdev_get_priv(netdev);
+	const u8 bc_mac[] = {0xff, 0xff, 0xff, 0xff, 0xff, 0xff};
+	const u8 *peer_mac = pairwise ? mac_addr : bc_mac;
 
 	if (mwifiex_set_encode(priv, params->key, params->key_len,
-			       key_index, 0)) {
+			       key_index, peer_mac, 0)) {
 		wiphy_err(wiphy, "crypto keys added\n");
 		return -EFAULT;
 	}
@@ -192,13 +216,13 @@ static int mwifiex_send_domain_info_cmd_fw(struct wiphy *wiphy)
 	enum ieee80211_band band;
 	struct ieee80211_supported_band *sband;
 	struct ieee80211_channel *ch;
-	struct mwifiex_private *priv = mwifiex_cfg80211_get_priv(wiphy);
-	struct mwifiex_adapter *adapter = priv->adapter;
+	struct mwifiex_adapter *adapter = mwifiex_cfg80211_get_adapter(wiphy);
+	struct mwifiex_private *priv;
 	struct mwifiex_802_11d_domain_reg *domain_info = &adapter->domain_reg;
 
 	/* Set country code */
-	domain_info->country_code[0] = priv->country_code[0];
-	domain_info->country_code[1] = priv->country_code[1];
+	domain_info->country_code[0] = adapter->country_code[0];
+	domain_info->country_code[1] = adapter->country_code[1];
 	domain_info->country_code[2] = ' ';
 
 	band = mwifiex_band_to_radio_type(adapter->config_bands);
@@ -250,6 +274,8 @@ static int mwifiex_send_domain_info_cmd_fw(struct wiphy *wiphy)
 
 	domain_info->no_of_triplet = no_of_triplet;
 
+	priv = mwifiex_get_priv(adapter, MWIFIEX_BSS_ROLE_ANY);
+
 	if (mwifiex_send_cmd_async(priv, HostCmd_CMD_802_11D_DOMAIN_INFO,
 				   HostCmd_ACT_GEN_SET, 0, NULL)) {
 		wiphy_err(wiphy, "11D: setting domain info in FW\n");
@@ -272,12 +298,12 @@ static int mwifiex_send_domain_info_cmd_fw(struct wiphy *wiphy)
 static int mwifiex_reg_notifier(struct wiphy *wiphy,
 				struct regulatory_request *request)
 {
-	struct mwifiex_private *priv = mwifiex_cfg80211_get_priv(wiphy);
+	struct mwifiex_adapter *adapter = mwifiex_cfg80211_get_adapter(wiphy);
 
-	wiphy_dbg(wiphy, "info: cfg80211 regulatory domain callback for domain"
-			" %c%c\n", request->alpha2[0], request->alpha2[1]);
+	wiphy_dbg(wiphy, "info: cfg80211 regulatory domain callback for %c%c\n",
+		  request->alpha2[0], request->alpha2[1]);
 
-	memcpy(priv->country_code, request->alpha2, sizeof(request->alpha2));
+	memcpy(adapter->country_code, request->alpha2, sizeof(request->alpha2));
 
 	switch (request->initiator) {
 	case NL80211_REGDOM_SET_BY_DRIVER:
@@ -361,33 +387,10 @@ mwifiex_set_rf_channel(struct mwifiex_private *priv,
 	if (mwifiex_bss_set_channel(priv, &cfp))
 		return -EFAULT;
 
-	return mwifiex_drv_change_adhoc_chan(priv, cfp.channel);
-}
-
-/*
- * CFG802.11 operation handler to set channel.
- *
- * This function can only be used when station is not connected.
- */
-static int
-mwifiex_cfg80211_set_channel(struct wiphy *wiphy, struct net_device *dev,
-			     struct ieee80211_channel *chan,
-			     enum nl80211_channel_type channel_type)
-{
-	struct mwifiex_private *priv;
-
-	if (dev)
-		priv = mwifiex_netdev_get_priv(dev);
+	if (priv->bss_type == MWIFIEX_BSS_TYPE_STA)
+		return mwifiex_drv_change_adhoc_chan(priv, cfp.channel);
 	else
-		priv = mwifiex_cfg80211_get_priv(wiphy);
-
-	if (priv->media_connected) {
-		wiphy_err(wiphy, "This setting is valid only when station "
-				"is not connected\n");
-		return -EINVAL;
-	}
-
-	return mwifiex_set_rf_channel(priv, chan, channel_type);
+		return mwifiex_uap_set_channel(priv, cfp.channel);
 }
 
 /*
@@ -399,18 +402,13 @@ mwifiex_cfg80211_set_channel(struct wiphy *wiphy, struct net_device *dev,
 static int
 mwifiex_set_frag(struct mwifiex_private *priv, u32 frag_thr)
 {
-	int ret;
-
 	if (frag_thr < MWIFIEX_FRAG_MIN_VALUE ||
 	    frag_thr > MWIFIEX_FRAG_MAX_VALUE)
-		return -EINVAL;
-
-	/* Send request to firmware */
-	ret = mwifiex_send_cmd_sync(priv, HostCmd_CMD_802_11_SNMP_MIB,
-				    HostCmd_ACT_GEN_SET, FRAG_THRESH_I,
-				    &frag_thr);
+		frag_thr = MWIFIEX_FRAG_MAX_VALUE;
 
-	return ret;
+	return mwifiex_send_cmd_sync(priv, HostCmd_CMD_802_11_SNMP_MIB,
+				     HostCmd_ACT_GEN_SET, FRAG_THRESH_I,
+				     &frag_thr);
 }
 
 /*
@@ -439,19 +437,85 @@ mwifiex_set_rts(struct mwifiex_private *priv, u32 rts_thr)
 static int
 mwifiex_cfg80211_set_wiphy_params(struct wiphy *wiphy, u32 changed)
 {
-	struct mwifiex_private *priv = mwifiex_cfg80211_get_priv(wiphy);
-	int ret = 0;
+	struct mwifiex_adapter *adapter = mwifiex_cfg80211_get_adapter(wiphy);
+	struct mwifiex_private *priv;
+	struct mwifiex_uap_bss_param *bss_cfg;
+	int ret, bss_started, i;
+
+	for (i = 0; i < adapter->priv_num; i++) {
+		priv = adapter->priv[i];
+
+		switch (priv->bss_role) {
+		case MWIFIEX_BSS_ROLE_UAP:
+			bss_cfg = kzalloc(sizeof(struct mwifiex_uap_bss_param),
+					  GFP_KERNEL);
+			if (!bss_cfg)
+				return -ENOMEM;
+
+			mwifiex_set_sys_config_invalid_data(bss_cfg);
+
+			if (changed & WIPHY_PARAM_RTS_THRESHOLD)
+				bss_cfg->rts_threshold = wiphy->rts_threshold;
+			if (changed & WIPHY_PARAM_FRAG_THRESHOLD)
+				bss_cfg->frag_threshold = wiphy->frag_threshold;
+			if (changed & WIPHY_PARAM_RETRY_LONG)
+				bss_cfg->retry_limit = wiphy->retry_long;
+
+			bss_started = priv->bss_started;
+
+			ret = mwifiex_send_cmd_sync(priv,
+						    HostCmd_CMD_UAP_BSS_STOP,
+						    HostCmd_ACT_GEN_SET, 0,
+						    NULL);
+			if (ret) {
+				wiphy_err(wiphy, "Failed to stop the BSS\n");
+				kfree(bss_cfg);
+				return ret;
+			}
 
-	if (changed & WIPHY_PARAM_RTS_THRESHOLD) {
-		ret = mwifiex_set_rts(priv, wiphy->rts_threshold);
-		if (ret)
-			return ret;
-	}
+			ret = mwifiex_send_cmd_async(priv,
+						     HostCmd_CMD_UAP_SYS_CONFIG,
+						     HostCmd_ACT_GEN_SET,
+						     UAP_BSS_PARAMS_I, bss_cfg);
 
-	if (changed & WIPHY_PARAM_FRAG_THRESHOLD)
-		ret = mwifiex_set_frag(priv, wiphy->frag_threshold);
+			kfree(bss_cfg);
 
-	return ret;
+			if (ret) {
+				wiphy_err(wiphy, "Failed to set bss config\n");
+				return ret;
+			}
+
+			if (!bss_started)
+				break;
+
+			ret = mwifiex_send_cmd_async(priv,
+						     HostCmd_CMD_UAP_BSS_START,
+						     HostCmd_ACT_GEN_SET, 0,
+						     NULL);
+			if (ret) {
+				wiphy_err(wiphy, "Failed to start BSS\n");
+				return ret;
+			}
+
+			break;
+		case MWIFIEX_BSS_ROLE_STA:
+			if (changed & WIPHY_PARAM_RTS_THRESHOLD) {
+				ret = mwifiex_set_rts(priv,
+						      wiphy->rts_threshold);
+				if (ret)
+					return ret;
+			}
+			if (changed & WIPHY_PARAM_FRAG_THRESHOLD) {
+				ret = mwifiex_set_frag(priv,
+						       wiphy->frag_threshold);
+				if (ret)
+					return ret;
+			}
+			break;
+		}
+	}
+
+	return 0;
 }
 
 /*
@@ -466,31 +530,59 @@ mwifiex_cfg80211_change_virtual_intf(struct wiphy *wiphy,
 	int ret;
 	struct mwifiex_private *priv = mwifiex_netdev_get_priv(dev);
 
-	if (priv->bss_mode == type) {
-		wiphy_warn(wiphy, "already set to required type\n");
-		return 0;
-	}
-
-	priv->bss_mode = type;
-
-	switch (type) {
+	switch (dev->ieee80211_ptr->iftype) {
 	case NL80211_IFTYPE_ADHOC:
-		dev->ieee80211_ptr->iftype = NL80211_IFTYPE_ADHOC;
-		wiphy_dbg(wiphy, "info: setting interface type to adhoc\n");
+		switch (type) {
+		case NL80211_IFTYPE_STATION:
+			break;
+		case NL80211_IFTYPE_UNSPECIFIED:
+			wiphy_warn(wiphy, "%s: kept type as IBSS\n", dev->name);
+		case NL80211_IFTYPE_ADHOC:	/* This shouldn't happen */
+			return 0;
+		case NL80211_IFTYPE_AP:
+		default:
+			wiphy_err(wiphy, "%s: changing to %d not supported\n",
+				  dev->name, type);
+			return -EOPNOTSUPP;
+		}
 		break;
 	case NL80211_IFTYPE_STATION:
-		dev->ieee80211_ptr->iftype = NL80211_IFTYPE_STATION;
-		wiphy_dbg(wiphy, "info: setting interface type to managed\n");
+		switch (type) {
+		case NL80211_IFTYPE_ADHOC:
+			break;
+		case NL80211_IFTYPE_UNSPECIFIED:
+			wiphy_warn(wiphy, "%s: kept type as STA\n", dev->name);
+		case NL80211_IFTYPE_STATION:	/* This shouldn't happen */
+			return 0;
+		case NL80211_IFTYPE_AP:
+		default:
+			wiphy_err(wiphy, "%s: changing to %d not supported\n",
+				  dev->name, type);
+			return -EOPNOTSUPP;
+		}
+		break;
+	case NL80211_IFTYPE_AP:
+		switch (type) {
+		case NL80211_IFTYPE_UNSPECIFIED:
+			wiphy_warn(wiphy, "%s: kept type as AP\n", dev->name);
+		case NL80211_IFTYPE_AP:		/* This shouldn't happen */
+			return 0;
+		case NL80211_IFTYPE_ADHOC:
+		case NL80211_IFTYPE_STATION:
+		default:
+			wiphy_err(wiphy, "%s: changing to %d not supported\n",
+				  dev->name, type);
+			return -EOPNOTSUPP;
+		}
 		break;
-	case NL80211_IFTYPE_UNSPECIFIED:
-		dev->ieee80211_ptr->iftype = NL80211_IFTYPE_STATION;
-		wiphy_dbg(wiphy, "info: setting interface type to auto\n");
-		return 0;
 	default:
-		wiphy_err(wiphy, "unknown interface type: %d\n", type);
-		return -EINVAL;
+		wiphy_err(wiphy, "%s: unknown iftype: %d\n",
+			  dev->name, dev->ieee80211_ptr->iftype);
+		return -EOPNOTSUPP;
 	}
 
+	dev->ieee80211_ptr->iftype = type;
+	priv->bss_mode = type;
 	mwifiex_deauthenticate(priv, NULL);
 
 	priv->sec_info.authentication_mode = NL80211_AUTHTYPE_OPEN_SYSTEM;
@@ -516,25 +608,23 @@ static int
 mwifiex_dump_station_info(struct mwifiex_private *priv,
 			  struct station_info *sinfo)
 {
-	struct mwifiex_ds_get_signal signal;
 	struct mwifiex_rate_cfg rate;
-	int ret = 0;
 
 	sinfo->filled = STATION_INFO_RX_BYTES | STATION_INFO_TX_BYTES |
-		STATION_INFO_RX_PACKETS |
-		STATION_INFO_TX_PACKETS
-		| STATION_INFO_SIGNAL | STATION_INFO_TX_BITRATE;
+			STATION_INFO_RX_PACKETS | STATION_INFO_TX_PACKETS |
+			STATION_INFO_TX_BITRATE |
+			STATION_INFO_SIGNAL | STATION_INFO_SIGNAL_AVG;
 
 	/* Get signal information from the firmware */
-	memset(&signal, 0, sizeof(struct mwifiex_ds_get_signal));
-	if (mwifiex_get_signal_info(priv, &signal)) {
-		dev_err(priv->adapter->dev, "getting signal information\n");
-		ret = -EFAULT;
+	if (mwifiex_send_cmd_sync(priv, HostCmd_CMD_RSSI_INFO,
+				  HostCmd_ACT_GEN_GET, 0, NULL)) {
+		dev_err(priv->adapter->dev, "failed to get signal information\n");
+		return -EFAULT;
 	}
 
 	if (mwifiex_drv_get_data_rate(priv, &rate)) {
 		dev_err(priv->adapter->dev, "getting data rate\n");
-		ret = -EFAULT;
+		return -EFAULT;
 	}
 
 	/* Get DTIM period information from firmware */
@@ -557,11 +647,12 @@ mwifiex_dump_station_info(struct mwifiex_private *priv,
 			sinfo->txrate.flags |= RATE_INFO_FLAGS_SHORT_GI;
 	}
 
+	sinfo->signal_avg = priv->bcn_rssi_avg;
 	sinfo->rx_bytes = priv->stats.rx_bytes;
 	sinfo->tx_bytes = priv->stats.tx_bytes;
 	sinfo->rx_packets = priv->stats.rx_packets;
 	sinfo->tx_packets = priv->stats.tx_packets;
-	sinfo->signal = priv->qual_level;
+	sinfo->signal = priv->bcn_rssi_avg;
 	/* bit rate is in 500 kb/s units. Convert it to 100kb/s units */
 	sinfo->txrate.legacy = rate.rate * 5;
 
@@ -581,7 +672,7 @@ mwifiex_dump_station_info(struct mwifiex_private *priv,
 			priv->curr_bss_params.bss_descriptor.beacon_period;
 	}
 
-	return ret;
+	return 0;
 }
 
 /*
@@ -604,6 +695,23 @@ mwifiex_cfg80211_get_station(struct wiphy *wiphy, struct net_device *dev,
 	return mwifiex_dump_station_info(priv, sinfo);
 }
 
+/*
+ * CFG802.11 operation handler to dump station information.
+ */
+static int
+mwifiex_cfg80211_dump_station(struct wiphy *wiphy, struct net_device *dev,
+			      int idx, u8 *mac, struct station_info *sinfo)
+{
+	struct mwifiex_private *priv = mwifiex_netdev_get_priv(dev);
+
+	if (!priv->media_connected || idx)
+		return -ENOENT;
+
+	memcpy(mac, priv->cfg_bssid, ETH_ALEN);
+
+	return mwifiex_dump_station_info(priv, sinfo);
+}
+
 /* Supported rates to be advertised to the cfg80211 */
 
 static struct ieee80211_rate mwifiex_rates[] = {
@@ -750,6 +858,129 @@ static int mwifiex_cfg80211_set_bitrate_mask(struct wiphy *wiphy,
 }
 
 /*
+ * CFG802.11 operation handler for connection quality monitoring.
+ *
+ * This function subscribes/unsubscribes HIGH_RSSI and LOW_RSSI
+ * events to FW.
+ */
+static int mwifiex_cfg80211_set_cqm_rssi_config(struct wiphy *wiphy,
+						struct net_device *dev,
+						s32 rssi_thold, u32 rssi_hyst)
+{
+	struct mwifiex_private *priv = mwifiex_netdev_get_priv(dev);
+	struct mwifiex_ds_misc_subsc_evt subsc_evt;
+
+	priv->cqm_rssi_thold = rssi_thold;
+	priv->cqm_rssi_hyst = rssi_hyst;
+
+	memset(&subsc_evt, 0x00, sizeof(struct mwifiex_ds_misc_subsc_evt));
+	subsc_evt.events = BITMASK_BCN_RSSI_LOW | BITMASK_BCN_RSSI_HIGH;
+
+	/* Subscribe/unsubscribe low and high rssi events */
+	if (rssi_thold && rssi_hyst) {
+		subsc_evt.action = HostCmd_ACT_BITWISE_SET;
+		subsc_evt.bcn_l_rssi_cfg.abs_value = abs(rssi_thold);
+		subsc_evt.bcn_h_rssi_cfg.abs_value = abs(rssi_thold);
+		subsc_evt.bcn_l_rssi_cfg.evt_freq = 1;
+		subsc_evt.bcn_h_rssi_cfg.evt_freq = 1;
+		return mwifiex_send_cmd_sync(priv,
+					     HostCmd_CMD_802_11_SUBSCRIBE_EVENT,
+					     0, 0, &subsc_evt);
+	} else {
+		subsc_evt.action = HostCmd_ACT_BITWISE_CLR;
+		return mwifiex_send_cmd_sync(priv,
+					     HostCmd_CMD_802_11_SUBSCRIBE_EVENT,
+					     0, 0, &subsc_evt);
+	}
+
+	return 0;
+}
+
+/* cfg80211 operation handler for stop ap.
+ * Function stops BSS running at uAP interface.
+ */
+static int mwifiex_cfg80211_stop_ap(struct wiphy *wiphy, struct net_device *dev)
+{
+	struct mwifiex_private *priv = mwifiex_netdev_get_priv(dev);
+
+	if (mwifiex_del_mgmt_ies(priv))
+		wiphy_err(wiphy, "Failed to delete mgmt IEs!\n");
+
+	if (mwifiex_send_cmd_sync(priv, HostCmd_CMD_UAP_BSS_STOP,
+				  HostCmd_ACT_GEN_SET, 0, NULL)) {
+		wiphy_err(wiphy, "Failed to stop the BSS\n");
+		return -1;
+	}
+
+	return 0;
+}
+
+/* cfg80211 operation handler for start_ap.
+ * Function sets beacon period, DTIM period, SSID and security into
+ * AP config structure.
+ * AP is configured with these settings and BSS is started.
+ */
+static int mwifiex_cfg80211_start_ap(struct wiphy *wiphy,
+				     struct net_device *dev,
+				     struct cfg80211_ap_settings *params)
+{
+	struct mwifiex_uap_bss_param *bss_cfg;
+	struct mwifiex_private *priv = mwifiex_netdev_get_priv(dev);
+
+	if (priv->bss_type != MWIFIEX_BSS_TYPE_UAP)
+		return -1;
+	if (mwifiex_set_mgmt_ies(priv, params))
+		return -1;
+
+	bss_cfg = kzalloc(sizeof(struct mwifiex_uap_bss_param), GFP_KERNEL);
+	if (!bss_cfg)
+		return -ENOMEM;
+
+	mwifiex_set_sys_config_invalid_data(bss_cfg);
+
+	if (params->beacon_interval)
+		bss_cfg->beacon_period = params->beacon_interval;
+	if (params->dtim_period)
+		bss_cfg->dtim_period = params->dtim_period;
+
+	if (params->ssid && params->ssid_len) {
+		memcpy(bss_cfg->ssid.ssid, params->ssid, params->ssid_len);
+		bss_cfg->ssid.ssid_len = params->ssid_len;
+	}
+
+	if (mwifiex_set_secure_params(priv, bss_cfg, params)) {
+		kfree(bss_cfg);
+		wiphy_err(wiphy, "Failed to parse secuirty parameters!\n");
+		return -1;
+	}
+
+	if (mwifiex_send_cmd_sync(priv, HostCmd_CMD_UAP_BSS_STOP,
+				  HostCmd_ACT_GEN_SET, 0, NULL)) {
+		wiphy_err(wiphy, "Failed to stop the BSS\n");
+		kfree(bss_cfg);
+		return -1;
+	}
+
+	if (mwifiex_send_cmd_async(priv, HostCmd_CMD_UAP_SYS_CONFIG,
+				   HostCmd_ACT_GEN_SET,
+				   UAP_BSS_PARAMS_I, bss_cfg)) {
+		wiphy_err(wiphy, "Failed to set the SSID\n");
+		kfree(bss_cfg);
+		return -1;
+	}
+
+	kfree(bss_cfg);
+
+	if (mwifiex_send_cmd_async(priv, HostCmd_CMD_UAP_BSS_START,
+				   HostCmd_ACT_GEN_SET, 0, NULL)) {
+		wiphy_err(wiphy, "Failed to start the BSS\n");
+		return -1;
+	}
+
+	return 0;
+}
+
+/*
  * CFG802.11 operation handler for disconnection request.
  *
  * This function does not work when there is already a disconnection
@@ -868,7 +1099,7 @@ mwifiex_cfg80211_assoc(struct mwifiex_private *priv, size_t ssid_len, u8 *ssid,
 	priv->wep_key_curr_index = 0;
 	priv->sec_info.encryption_mode = 0;
 	priv->sec_info.is_authtype_auto = 0;
-	ret = mwifiex_set_encode(priv, NULL, 0, 0, 1);
+	ret = mwifiex_set_encode(priv, NULL, 0, 0, NULL, 1);
 
 	if (mode == NL80211_IFTYPE_ADHOC) {
 		/* "privacy" is set only for ad-hoc mode */
@@ -916,7 +1147,7 @@ mwifiex_cfg80211_assoc(struct mwifiex_private *priv, size_t ssid_len, u8 *ssid,
 				" with key len %d\n", sme->key_len);
 			priv->wep_key_curr_index = sme->key_idx;
 			ret = mwifiex_set_encode(priv, sme->key, sme->key_len,
-							sme->key_idx, 0);
+						 sme->key_idx, NULL, 0);
 		}
 	}
 done:
@@ -995,6 +1226,11 @@ mwifiex_cfg80211_connect(struct wiphy *wiphy, struct net_device *dev,
 		goto done;
 	}
 
+	if (priv->bss_mode == NL80211_IFTYPE_AP) {
+		wiphy_err(wiphy, "skip association request for AP interface\n");
+		goto done;
+	}
+
 	wiphy_dbg(wiphy, "info: Trying to associate to %s and bssid %pM\n",
 		  (char *) sme->ssid, sme->bssid);
 
@@ -1107,6 +1343,17 @@ mwifiex_cfg80211_scan(struct wiphy *wiphy, struct net_device *dev,
 	priv->user_scan_cfg->num_ssids = request->n_ssids;
 	priv->user_scan_cfg->ssid_list = request->ssids;
 
+	if (request->ie && request->ie_len) {
+		for (i = 0; i < MWIFIEX_MAX_VSIE_NUM; i++) {
+			if (priv->vs_ie[i].mask != MWIFIEX_VSIE_MASK_CLEAR)
+				continue;
+			priv->vs_ie[i].mask = MWIFIEX_VSIE_MASK_SCAN;
+			memcpy(&priv->vs_ie[i].ie, request->ie,
+			       request->ie_len);
+			break;
+		}
+	}
+
 	for (i = 0; i < request->n_channels; i++) {
 		chan = request->channels[i];
 		priv->user_scan_cfg->chan_list[i].chan_number = chan->hw_value;
@@ -1124,6 +1371,15 @@ mwifiex_cfg80211_scan(struct wiphy *wiphy, struct net_device *dev,
 	if (mwifiex_set_user_scan_ioctl(priv, priv->user_scan_cfg))
 		return -EFAULT;
 
+	if (request->ie && request->ie_len) {
+		for (i = 0; i < MWIFIEX_MAX_VSIE_NUM; i++) {
+			if (priv->vs_ie[i].mask == MWIFIEX_VSIE_MASK_SCAN) {
+				priv->vs_ie[i].mask = MWIFIEX_VSIE_MASK_CLEAR;
+				memset(&priv->vs_ie[i].ie, 0,
+				       MWIFIEX_MAX_VSIE_LEN);
+			}
+		}
+	}
 	return 0;
 }
 
@@ -1208,15 +1464,12 @@ struct net_device *mwifiex_add_virtual_intf(struct wiphy *wiphy,
 					    u32 *flags,
 					    struct vif_params *params)
 {
-	struct mwifiex_private *priv = mwifiex_cfg80211_get_priv(wiphy);
-	struct mwifiex_adapter *adapter;
+	struct mwifiex_adapter *adapter = mwifiex_cfg80211_get_adapter(wiphy);
+	struct mwifiex_private *priv;
 	struct net_device *dev;
 	void *mdev_priv;
+	struct wireless_dev *wdev;
 
-	if (!priv)
-		return NULL;
-
-	adapter = priv->adapter;
 	if (!adapter)
 		return NULL;
 
@@ -1224,12 +1477,21 @@ struct net_device *mwifiex_add_virtual_intf(struct wiphy *wiphy,
 	case NL80211_IFTYPE_UNSPECIFIED:
 	case NL80211_IFTYPE_STATION:
 	case NL80211_IFTYPE_ADHOC:
+		priv = adapter->priv[MWIFIEX_BSS_TYPE_STA];
 		if (priv->bss_mode) {
-			wiphy_err(wiphy, "cannot create multiple"
-					" station/adhoc interfaces\n");
+			wiphy_err(wiphy,
+				  "cannot create multiple sta/adhoc ifaces\n");
 			return NULL;
 		}
 
+		wdev = kzalloc(sizeof(struct wireless_dev), GFP_KERNEL);
+		if (!wdev)
+			return NULL;
+
+		wdev->wiphy = wiphy;
+		priv->wdev = wdev;
+		wdev->iftype = NL80211_IFTYPE_STATION;
+
 		if (type == NL80211_IFTYPE_UNSPECIFIED)
 			priv->bss_mode = NL80211_IFTYPE_STATION;
 		else
@@ -1237,11 +1499,36 @@ struct net_device *mwifiex_add_virtual_intf(struct wiphy *wiphy,
 
 		priv->bss_type = MWIFIEX_BSS_TYPE_STA;
 		priv->frame_type = MWIFIEX_DATA_FRAME_TYPE_ETH_II;
-		priv->bss_priority = 0;
+		priv->bss_priority = MWIFIEX_BSS_ROLE_STA;
 		priv->bss_role = MWIFIEX_BSS_ROLE_STA;
 		priv->bss_num = 0;
 
 		break;
+	case NL80211_IFTYPE_AP:
+		priv = adapter->priv[MWIFIEX_BSS_TYPE_UAP];
+
+		if (priv->bss_mode) {
+			wiphy_err(wiphy, "Can't create multiple AP interfaces");
+			return NULL;
+		}
+
+		wdev = kzalloc(sizeof(struct wireless_dev), GFP_KERNEL);
+		if (!wdev)
+			return NULL;
+
+		priv->wdev = wdev;
+		wdev->wiphy = wiphy;
+		wdev->iftype = NL80211_IFTYPE_AP;
+
+		priv->bss_type = MWIFIEX_BSS_TYPE_UAP;
+		priv->frame_type = MWIFIEX_DATA_FRAME_TYPE_ETH_II;
+		priv->bss_priority = MWIFIEX_BSS_ROLE_UAP;
+		priv->bss_role = MWIFIEX_BSS_ROLE_UAP;
+		priv->bss_started = 0;
+		priv->bss_num = 0;
+		priv->bss_mode = type;
+
+		break;
 	default:
 		wiphy_err(wiphy, "type not supported\n");
 		return NULL;
@@ -1254,6 +1541,15 @@ struct net_device *mwifiex_add_virtual_intf(struct wiphy *wiphy,
 		goto error;
 	}
 
+	mwifiex_init_priv_params(priv, dev);
+	priv->netdev = dev;
+
+	mwifiex_setup_ht_caps(&wiphy->bands[IEEE80211_BAND_2GHZ]->ht_cap, priv);
+
+	if (adapter->config_bands & BAND_A)
+		mwifiex_setup_ht_caps(
+			&wiphy->bands[IEEE80211_BAND_5GHZ]->ht_cap, priv);
+
 	dev_net_set(dev, wiphy_net(wiphy));
 	dev->ieee80211_ptr = priv->wdev;
 	dev->ieee80211_ptr->iftype = priv->bss_mode;
@@ -1268,9 +1564,6 @@ struct net_device *mwifiex_add_virtual_intf(struct wiphy *wiphy,
 	mdev_priv = netdev_priv(dev);
 	*((unsigned long *) mdev_priv) = (unsigned long) priv;
 
-	priv->netdev = dev;
-	mwifiex_init_priv_params(priv, dev);
-
 	SET_NETDEV_DEV(dev, adapter->dev);
 
 	/* Register network device */
@@ -1340,8 +1633,8 @@ static struct cfg80211_ops mwifiex_cfg80211_ops = {
 	.connect = mwifiex_cfg80211_connect,
 	.disconnect = mwifiex_cfg80211_disconnect,
 	.get_station = mwifiex_cfg80211_get_station,
+	.dump_station = mwifiex_cfg80211_dump_station,
 	.set_wiphy_params = mwifiex_cfg80211_set_wiphy_params,
-	.set_channel = mwifiex_cfg80211_set_channel,
 	.join_ibss = mwifiex_cfg80211_join_ibss,
 	.leave_ibss = mwifiex_cfg80211_leave_ibss,
 	.add_key = mwifiex_cfg80211_add_key,
@@ -1350,6 +1643,9 @@ static struct cfg80211_ops mwifiex_cfg80211_ops = {
 	.set_power_mgmt = mwifiex_cfg80211_set_power_mgmt,
 	.set_tx_power = mwifiex_cfg80211_set_tx_power,
 	.set_bitrate_mask = mwifiex_cfg80211_set_bitrate_mask,
+	.start_ap = mwifiex_cfg80211_start_ap,
+	.stop_ap = mwifiex_cfg80211_stop_ap,
+	.set_cqm_rssi_config = mwifiex_cfg80211_set_cqm_rssi_config,
 };
 
 /*
@@ -1359,75 +1655,67 @@ static struct cfg80211_ops mwifiex_cfg80211_ops = {
  * default parameters and handler function pointers, and finally
  * registers the device.
  */
-int mwifiex_register_cfg80211(struct mwifiex_private *priv)
+
+int mwifiex_register_cfg80211(struct mwifiex_adapter *adapter)
 {
 	int ret;
 	void *wdev_priv;
-	struct wireless_dev *wdev;
-	struct ieee80211_sta_ht_cap *ht_info;
-
-	wdev = kzalloc(sizeof(struct wireless_dev), GFP_KERNEL);
-	if (!wdev) {
-		dev_err(priv->adapter->dev, "%s: allocating wireless device\n",
-			__func__);
-		return -ENOMEM;
-	}
-	wdev->wiphy =
-		wiphy_new(&mwifiex_cfg80211_ops,
-			  sizeof(struct mwifiex_private *));
-	if (!wdev->wiphy) {
-		kfree(wdev);
+	struct wiphy *wiphy;
+	struct mwifiex_private *priv = adapter->priv[MWIFIEX_BSS_TYPE_STA];
+	u8 *country_code;
+
+	/* create a new wiphy for use with cfg80211 */
+	wiphy = wiphy_new(&mwifiex_cfg80211_ops,
+			  sizeof(struct mwifiex_adapter *));
+	if (!wiphy) {
+		dev_err(adapter->dev, "%s: creating new wiphy\n", __func__);
 		return -ENOMEM;
 	}
-	wdev->iftype = NL80211_IFTYPE_STATION;
-	wdev->wiphy->max_scan_ssids = 10;
-	wdev->wiphy->interface_modes = BIT(NL80211_IFTYPE_STATION) |
-				       BIT(NL80211_IFTYPE_ADHOC);
-
-	wdev->wiphy->bands[IEEE80211_BAND_2GHZ] = &mwifiex_band_2ghz;
-	ht_info = &wdev->wiphy->bands[IEEE80211_BAND_2GHZ]->ht_cap;
-	mwifiex_setup_ht_caps(ht_info, priv);
-
-	if (priv->adapter->config_bands & BAND_A) {
-		wdev->wiphy->bands[IEEE80211_BAND_5GHZ] = &mwifiex_band_5ghz;
-		ht_info = &wdev->wiphy->bands[IEEE80211_BAND_5GHZ]->ht_cap;
-		mwifiex_setup_ht_caps(ht_info, priv);
-	} else {
-		wdev->wiphy->bands[IEEE80211_BAND_5GHZ] = NULL;
-	}
+	wiphy->max_scan_ssids = MWIFIEX_MAX_SSID_LIST_LENGTH;
+	wiphy->max_scan_ie_len = MWIFIEX_MAX_VSIE_LEN;
+	wiphy->interface_modes = BIT(NL80211_IFTYPE_STATION) |
+				 BIT(NL80211_IFTYPE_ADHOC) |
+				 BIT(NL80211_IFTYPE_AP);
+
+	wiphy->bands[IEEE80211_BAND_2GHZ] = &mwifiex_band_2ghz;
+	if (adapter->config_bands & BAND_A)
+		wiphy->bands[IEEE80211_BAND_5GHZ] = &mwifiex_band_5ghz;
+	else
+		wiphy->bands[IEEE80211_BAND_5GHZ] = NULL;
 
-	/* Initialize cipher suits */
-	wdev->wiphy->cipher_suites = mwifiex_cipher_suites;
-	wdev->wiphy->n_cipher_suites = ARRAY_SIZE(mwifiex_cipher_suites);
+	wiphy->iface_combinations = &mwifiex_iface_comb_ap_sta;
+	wiphy->n_iface_combinations = 1;
 
-	memcpy(wdev->wiphy->perm_addr, priv->curr_addr, ETH_ALEN);
-	wdev->wiphy->signal_type = CFG80211_SIGNAL_TYPE_MBM;
+	/* Initialize cipher suits */
+	wiphy->cipher_suites = mwifiex_cipher_suites;
+	wiphy->n_cipher_suites = ARRAY_SIZE(mwifiex_cipher_suites);
 
-	/* Reserve space for bss band information */
-	wdev->wiphy->bss_priv_size = sizeof(u8);
+	memcpy(wiphy->perm_addr, priv->curr_addr, ETH_ALEN);
+	wiphy->signal_type = CFG80211_SIGNAL_TYPE_MBM;
+	wiphy->flags |= WIPHY_FLAG_HAVE_AP_SME | WIPHY_FLAG_CUSTOM_REGULATORY;
 
-	wdev->wiphy->reg_notifier = mwifiex_reg_notifier;
+	/* Reserve space for mwifiex specific private data for BSS */
+	wiphy->bss_priv_size = sizeof(struct mwifiex_bss_priv);
 
-	/* Set struct mwifiex_private pointer in wiphy_priv */
-	wdev_priv = wiphy_priv(wdev->wiphy);
+	wiphy->reg_notifier = mwifiex_reg_notifier;
 
-	*(unsigned long *) wdev_priv = (unsigned long) priv;
+	/* Set struct mwifiex_adapter pointer in wiphy_priv */
+	wdev_priv = wiphy_priv(wiphy);
+	*(unsigned long *)wdev_priv = (unsigned long)adapter;
 
-	set_wiphy_dev(wdev->wiphy, (struct device *) priv->adapter->dev);
+	set_wiphy_dev(wiphy, (struct device *)priv->adapter->dev);
 
-	ret = wiphy_register(wdev->wiphy);
+	ret = wiphy_register(wiphy);
 	if (ret < 0) {
-		dev_err(priv->adapter->dev, "%s: registering cfg80211 device\n",
-			__func__);
-		wiphy_free(wdev->wiphy);
-		kfree(wdev);
+		dev_err(adapter->dev,
+			"%s: wiphy_register failed: %d\n", __func__, ret);
+		wiphy_free(wiphy);
 		return ret;
-	} else {
-		dev_dbg(priv->adapter->dev,
-			"info: successfully registered wiphy device\n");
 	}
+	country_code = mwifiex_11d_code_2_region(priv->adapter->region_code);
+	if (country_code && regulatory_hint(wiphy, country_code))
+		dev_err(adapter->dev, "regulatory_hint() failed\n");
 
-	priv->wdev = wdev;
-
+	adapter->wiphy = wiphy;
 	return ret;
 }
diff --git a/drivers/net/wireless/mwifiex/cfg80211.h b/drivers/net/wireless/mwifiex/cfg80211.h
index 76c76c60438..c5848934f11 100644
--- a/drivers/net/wireless/mwifiex/cfg80211.h
+++ b/drivers/net/wireless/mwifiex/cfg80211.h
@@ -24,6 +24,6 @@
 
 #include "main.h"
 
-int mwifiex_register_cfg80211(struct mwifiex_private *);
+int mwifiex_register_cfg80211(struct mwifiex_adapter *);
 
 #endif
diff --git a/drivers/net/wireless/mwifiex/cfp.c b/drivers/net/wireless/mwifiex/cfp.c
index 2fe1c33765b..560871b0e23 100644
--- a/drivers/net/wireless/mwifiex/cfp.c
+++ b/drivers/net/wireless/mwifiex/cfp.c
@@ -71,6 +71,37 @@ u16 region_code_index[MWIFIEX_MAX_REGION_CODE] = { 0x10, 0x20, 0x30,
 
 static u8 supported_rates_n[N_SUPPORTED_RATES] = { 0x02, 0x04, 0 };
 
+struct region_code_mapping {
+	u8 code;
+	u8 region[IEEE80211_COUNTRY_STRING_LEN];
+};
+
+static struct region_code_mapping region_code_mapping_t[] = {
+	{ 0x10, "US " }, /* US FCC */
+	{ 0x20, "CA " }, /* IC Canada */
+	{ 0x30, "EU " }, /* ETSI */
+	{ 0x31, "ES " }, /* Spain */
+	{ 0x32, "FR " }, /* France */
+	{ 0x40, "JP " }, /* Japan */
+	{ 0x41, "JP " }, /* Japan */
+	{ 0x50, "CN " }, /* China */
+};
+
+/* This function converts integer code to region string */
+u8 *mwifiex_11d_code_2_region(u8 code)
+{
+	u8 i;
+	u8 size = sizeof(region_code_mapping_t)/
+				sizeof(struct region_code_mapping);
+
+	/* Look for code in mapping table */
+	for (i = 0; i < size; i++)
+		if (region_code_mapping_t[i].code == code)
+			return region_code_mapping_t[i].region;
+
+	return NULL;
+}
+
 /*
  * This function maps an index in supported rates table into
  * the corresponding data rate.
diff --git a/drivers/net/wireless/mwifiex/cmdevt.c b/drivers/net/wireless/mwifiex/cmdevt.c
index 07f6e009255..51e023ec1de 100644
--- a/drivers/net/wireless/mwifiex/cmdevt.c
+++ b/drivers/net/wireless/mwifiex/cmdevt.c
@@ -139,6 +139,7 @@ static int mwifiex_dnld_cmd_to_fw(struct mwifiex_private *priv,
 	uint16_t cmd_size;
 	struct timeval tstamp;
 	unsigned long flags;
+	__le32 tmp;
 
 	if (!adapter || !cmd_node)
 		return -1;
@@ -178,15 +179,28 @@ static int mwifiex_dnld_cmd_to_fw(struct mwifiex_private *priv,
 		le16_to_cpu(*(__le16 *) ((u8 *) host_cmd + S_DS_GEN)), cmd_size,
 		le16_to_cpu(host_cmd->seq_num));
 
-	skb_push(cmd_node->cmd_skb, INTF_HEADER_LEN);
-
-	ret = adapter->if_ops.host_to_card(adapter, MWIFIEX_TYPE_CMD,
-					   cmd_node->cmd_skb, NULL);
-
-	skb_pull(cmd_node->cmd_skb, INTF_HEADER_LEN);
+	if (adapter->iface_type == MWIFIEX_USB) {
+		tmp = cpu_to_le32(MWIFIEX_USB_TYPE_CMD);
+		skb_push(cmd_node->cmd_skb, MWIFIEX_TYPE_LEN);
+		memcpy(cmd_node->cmd_skb->data, &tmp, MWIFIEX_TYPE_LEN);
+		adapter->cmd_sent = true;
+		ret = adapter->if_ops.host_to_card(adapter,
+						   MWIFIEX_USB_EP_CMD_EVENT,
+						   cmd_node->cmd_skb, NULL);
+		skb_pull(cmd_node->cmd_skb, MWIFIEX_TYPE_LEN);
+		if (ret == -EBUSY)
+			cmd_node->cmd_skb = NULL;
+	} else {
+		skb_push(cmd_node->cmd_skb, INTF_HEADER_LEN);
+		ret = adapter->if_ops.host_to_card(adapter, MWIFIEX_TYPE_CMD,
+						   cmd_node->cmd_skb, NULL);
+		skb_pull(cmd_node->cmd_skb, INTF_HEADER_LEN);
+	}
 
 	if (ret == -1) {
 		dev_err(adapter->dev, "DNLD_CMD: host to card failed\n");
+		if (adapter->iface_type == MWIFIEX_USB)
+			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);
@@ -232,6 +246,9 @@ static int mwifiex_dnld_sleep_confirm_cmd(struct mwifiex_adapter *adapter)
 	struct mwifiex_opt_sleep_confirm *sleep_cfm_buf =
 				(struct mwifiex_opt_sleep_confirm *)
 						adapter->sleep_cfm->data;
+	struct sk_buff *sleep_cfm_tmp;
+	__le32 tmp;
+
 	priv = mwifiex_get_priv(adapter, MWIFIEX_BSS_ROLE_ANY);
 
 	sleep_cfm_buf->seq_num =
@@ -240,10 +257,28 @@ static int mwifiex_dnld_sleep_confirm_cmd(struct mwifiex_adapter *adapter)
 					 priv->bss_type)));
 	adapter->seq_num++;
 
-	skb_push(adapter->sleep_cfm, INTF_HEADER_LEN);
-	ret = adapter->if_ops.host_to_card(adapter, MWIFIEX_TYPE_CMD,
-					   adapter->sleep_cfm, NULL);
-	skb_pull(adapter->sleep_cfm, INTF_HEADER_LEN);
+	if (adapter->iface_type == MWIFIEX_USB) {
+		sleep_cfm_tmp =
+			dev_alloc_skb(sizeof(struct mwifiex_opt_sleep_confirm)
+				      + MWIFIEX_TYPE_LEN);
+		skb_put(sleep_cfm_tmp, sizeof(struct mwifiex_opt_sleep_confirm)
+			+ MWIFIEX_TYPE_LEN);
+		tmp = cpu_to_le32(MWIFIEX_USB_TYPE_CMD);
+		memcpy(sleep_cfm_tmp->data, &tmp, MWIFIEX_TYPE_LEN);
+		memcpy(sleep_cfm_tmp->data + MWIFIEX_TYPE_LEN,
+		       adapter->sleep_cfm->data,
+		       sizeof(struct mwifiex_opt_sleep_confirm));
+		ret = adapter->if_ops.host_to_card(adapter,
+						   MWIFIEX_USB_EP_CMD_EVENT,
+						   sleep_cfm_tmp, NULL);
+		if (ret != -EBUSY)
+			dev_kfree_skb_any(sleep_cfm_tmp);
+	} else {
+		skb_push(adapter->sleep_cfm, INTF_HEADER_LEN);
+		ret = adapter->if_ops.host_to_card(adapter, MWIFIEX_TYPE_CMD,
+						   adapter->sleep_cfm, NULL);
+		skb_pull(adapter->sleep_cfm, INTF_HEADER_LEN);
+	}
 
 	if (ret == -1) {
 		dev_err(adapter->dev, "SLEEP_CFM: failed\n");
@@ -343,7 +378,12 @@ int mwifiex_free_cmd_buffer(struct mwifiex_adapter *adapter)
 		}
 		if (!cmd_array[i].resp_skb)
 			continue;
-		dev_kfree_skb_any(cmd_array[i].resp_skb);
+
+		if (adapter->iface_type == MWIFIEX_USB)
+			adapter->if_ops.cmdrsp_complete(adapter,
+							cmd_array[i].resp_skb);
+		else
+			dev_kfree_skb_any(cmd_array[i].resp_skb);
 	}
 	/* Release struct cmd_ctrl_node */
 	if (adapter->cmd_pool) {
@@ -400,6 +440,11 @@ int mwifiex_process_event(struct mwifiex_adapter *adapter)
 		do_gettimeofday(&tstamp);
 		dev_dbg(adapter->dev, "event: %lu.%lu: cause: %#x\n",
 			tstamp.tv_sec, tstamp.tv_usec, eventcause);
+	} else {
+		/* Handle PS_SLEEP/AWAKE events on STA */
+		priv = mwifiex_get_priv(adapter, MWIFIEX_BSS_ROLE_STA);
+		if (!priv)
+			priv = mwifiex_get_priv(adapter, MWIFIEX_BSS_ROLE_ANY);
 	}
 
 	ret = mwifiex_process_sta_event(priv);
@@ -500,8 +545,20 @@ int mwifiex_send_cmd_async(struct mwifiex_private *priv, uint16_t cmd_no,
 
 	/* Prepare command */
 	if (cmd_no) {
-		ret = mwifiex_sta_prepare_cmd(priv, cmd_no, cmd_action,
-					      cmd_oid, data_buf, cmd_ptr);
+		switch (cmd_no) {
+		case HostCmd_CMD_UAP_SYS_CONFIG:
+		case HostCmd_CMD_UAP_BSS_START:
+		case HostCmd_CMD_UAP_BSS_STOP:
+			ret = mwifiex_uap_prepare_cmd(priv, cmd_no, cmd_action,
+						      cmd_oid, data_buf,
+						      cmd_ptr);
+			break;
+		default:
+			ret = mwifiex_sta_prepare_cmd(priv, cmd_no, cmd_action,
+						      cmd_oid, data_buf,
+						      cmd_ptr);
+			break;
+		}
 	} else {
 		ret = mwifiex_cmd_host_cmd(priv, cmd_ptr, data_buf);
 		cmd_node->cmd_flag |= CMD_F_HOSTCMD;
@@ -1083,6 +1140,7 @@ mwifiex_process_hs_config(struct mwifiex_adapter *adapter)
 						    MWIFIEX_BSS_ROLE_ANY),
 				   false);
 }
+EXPORT_SYMBOL_GPL(mwifiex_process_hs_config);
 
 /*
  * This function handles the command response of a sleep confirm command.
diff --git a/drivers/net/wireless/mwifiex/debugfs.c b/drivers/net/wireless/mwifiex/debugfs.c
index 1a845074c52..a870b5885c0 100644
--- a/drivers/net/wireless/mwifiex/debugfs.c
+++ b/drivers/net/wireless/mwifiex/debugfs.c
@@ -212,7 +212,7 @@ mwifiex_info_read(struct file *file, char __user *ubuf,
 		p += sprintf(p, "essid=\"%s\"\n", info.ssid.ssid);
 		p += sprintf(p, "bssid=\"%pM\"\n", info.bssid);
 		p += sprintf(p, "channel=\"%d\"\n", (int) info.bss_chan);
-		p += sprintf(p, "region_code = \"%02x\"\n", info.region_code);
+		p += sprintf(p, "country_code = \"%s\"\n", info.country_code);
 
 		netdev_for_each_mc_addr(ha, netdev)
 			p += sprintf(p, "multicast_address[%d]=\"%pM\"\n",
diff --git a/drivers/net/wireless/mwifiex/decl.h b/drivers/net/wireless/mwifiex/decl.h
index be5fd1652e5..f918f66e5e2 100644
--- a/drivers/net/wireless/mwifiex/decl.h
+++ b/drivers/net/wireless/mwifiex/decl.h
@@ -28,7 +28,7 @@
 #include <linux/ieee80211.h>
 
 
-#define MWIFIEX_MAX_BSS_NUM         (1)
+#define MWIFIEX_MAX_BSS_NUM         (2)
 
 #define MWIFIEX_MIN_DATA_HEADER_LEN 36	/* sizeof(mwifiex_txpd)
 					 *   + 4 byte alignment
@@ -53,12 +53,19 @@
 #define MWIFIEX_RATE_BITMAP_MCS127 159
 
 #define MWIFIEX_RX_DATA_BUF_SIZE     (4 * 1024)
+#define MWIFIEX_RX_CMD_BUF_SIZE	     (2 * 1024)
+
+#define MAX_BEACON_PERIOD                  (4000)
+#define MIN_BEACON_PERIOD                  (50)
+#define MAX_DTIM_PERIOD                    (100)
+#define MIN_DTIM_PERIOD                    (1)
 
 #define MWIFIEX_RTS_MIN_VALUE              (0)
 #define MWIFIEX_RTS_MAX_VALUE              (2347)
 #define MWIFIEX_FRAG_MIN_VALUE             (256)
 #define MWIFIEX_FRAG_MAX_VALUE             (2346)
 
+#define MWIFIEX_RETRY_LIMIT                14
 #define MWIFIEX_SDIO_BLOCK_SIZE            256
 
 #define MWIFIEX_BUF_FLAG_REQUEUED_PKT      BIT(0)
@@ -91,6 +98,11 @@ struct mwifiex_fw_image {
 	u32 fw_len;
 };
 
+struct mwifiex_802_11_ssid {
+	u32 ssid_len;
+	u8 ssid[IEEE80211_MAX_SSID_LEN];
+};
+
 struct mwifiex_wait_queue {
 	wait_queue_head_t wait;
 	int status;
diff --git a/drivers/net/wireless/mwifiex/fw.h b/drivers/net/wireless/mwifiex/fw.h
index e98fc5af73d..9f674bbebe6 100644
--- a/drivers/net/wireless/mwifiex/fw.h
+++ b/drivers/net/wireless/mwifiex/fw.h
@@ -81,6 +81,11 @@ enum KEY_TYPE_ID {
 #define FIRMWARE_READY_SDIO				0xfedc
 #define FIRMWARE_READY_PCIE				0xfedcba00
 
+enum mwifiex_usb_ep {
+	MWIFIEX_USB_EP_CMD_EVENT = 1,
+	MWIFIEX_USB_EP_DATA = 2,
+};
+
 enum MWIFIEX_802_11_PRIVACY_FILTER {
 	MWIFIEX_802_11_PRIV_FILTER_ACCEPT_ALL,
 	MWIFIEX_802_11_PRIV_FILTER_8021X_WEP
@@ -88,22 +93,51 @@ enum MWIFIEX_802_11_PRIVACY_FILTER {
 
 #define CAL_SNR(RSSI, NF)		((s16)((s16)(RSSI)-(s16)(NF)))
 
+#define UAP_BSS_PARAMS_I			0
+#define UAP_CUSTOM_IE_I				1
+#define MWIFIEX_AUTO_IDX_MASK			0xffff
+#define MWIFIEX_DELETE_MASK			0x0000
+#define MGMT_MASK_ASSOC_REQ			0x01
+#define MGMT_MASK_REASSOC_REQ			0x04
+#define MGMT_MASK_ASSOC_RESP			0x02
+#define MGMT_MASK_REASSOC_RESP			0x08
+#define MGMT_MASK_PROBE_REQ			0x10
+#define MGMT_MASK_PROBE_RESP			0x20
+#define MGMT_MASK_BEACON			0x100
+
+#define TLV_TYPE_UAP_SSID			0x0000
+
 #define PROPRIETARY_TLV_BASE_ID                 0x0100
 #define TLV_TYPE_KEY_MATERIAL       (PROPRIETARY_TLV_BASE_ID + 0)
 #define TLV_TYPE_CHANLIST           (PROPRIETARY_TLV_BASE_ID + 1)
 #define TLV_TYPE_NUMPROBES          (PROPRIETARY_TLV_BASE_ID + 2)
+#define TLV_TYPE_RSSI_LOW           (PROPRIETARY_TLV_BASE_ID + 4)
 #define TLV_TYPE_PASSTHROUGH        (PROPRIETARY_TLV_BASE_ID + 10)
 #define TLV_TYPE_WMMQSTATUS         (PROPRIETARY_TLV_BASE_ID + 16)
 #define TLV_TYPE_WILDCARDSSID       (PROPRIETARY_TLV_BASE_ID + 18)
 #define TLV_TYPE_TSFTIMESTAMP       (PROPRIETARY_TLV_BASE_ID + 19)
+#define TLV_TYPE_RSSI_HIGH          (PROPRIETARY_TLV_BASE_ID + 22)
 #define TLV_TYPE_AUTH_TYPE          (PROPRIETARY_TLV_BASE_ID + 31)
+#define TLV_TYPE_STA_MAC_ADDR       (PROPRIETARY_TLV_BASE_ID + 32)
 #define TLV_TYPE_CHANNELBANDLIST    (PROPRIETARY_TLV_BASE_ID + 42)
+#define TLV_TYPE_UAP_BEACON_PERIOD  (PROPRIETARY_TLV_BASE_ID + 44)
+#define TLV_TYPE_UAP_DTIM_PERIOD    (PROPRIETARY_TLV_BASE_ID + 45)
+#define TLV_TYPE_UAP_RTS_THRESHOLD  (PROPRIETARY_TLV_BASE_ID + 51)
+#define TLV_TYPE_UAP_WPA_PASSPHRASE (PROPRIETARY_TLV_BASE_ID + 60)
+#define TLV_TYPE_UAP_ENCRY_PROTOCOL (PROPRIETARY_TLV_BASE_ID + 64)
+#define TLV_TYPE_UAP_AKMP           (PROPRIETARY_TLV_BASE_ID + 65)
+#define TLV_TYPE_UAP_FRAG_THRESHOLD (PROPRIETARY_TLV_BASE_ID + 70)
 #define TLV_TYPE_RATE_DROP_CONTROL  (PROPRIETARY_TLV_BASE_ID + 82)
 #define TLV_TYPE_RATE_SCOPE         (PROPRIETARY_TLV_BASE_ID + 83)
 #define TLV_TYPE_POWER_GROUP        (PROPRIETARY_TLV_BASE_ID + 84)
+#define TLV_TYPE_UAP_RETRY_LIMIT    (PROPRIETARY_TLV_BASE_ID + 93)
 #define TLV_TYPE_WAPI_IE            (PROPRIETARY_TLV_BASE_ID + 94)
+#define TLV_TYPE_UAP_MGMT_FRAME     (PROPRIETARY_TLV_BASE_ID + 104)
+#define TLV_TYPE_MGMT_IE            (PROPRIETARY_TLV_BASE_ID + 105)
 #define TLV_TYPE_AUTO_DS_PARAM      (PROPRIETARY_TLV_BASE_ID + 113)
 #define TLV_TYPE_PS_PARAM           (PROPRIETARY_TLV_BASE_ID + 114)
+#define TLV_TYPE_PWK_CIPHER         (PROPRIETARY_TLV_BASE_ID + 145)
+#define TLV_TYPE_GWK_CIPHER         (PROPRIETARY_TLV_BASE_ID + 146)
 
 #define MWIFIEX_TX_DATA_BUF_SIZE_2K        2048
 
@@ -194,12 +228,16 @@ enum MWIFIEX_802_11_PRIVACY_FILTER {
 #define HostCmd_CMD_802_11_KEY_MATERIAL               0x005e
 #define HostCmd_CMD_802_11_BG_SCAN_QUERY              0x006c
 #define HostCmd_CMD_WMM_GET_STATUS                    0x0071
+#define HostCmd_CMD_802_11_SUBSCRIBE_EVENT            0x0075
 #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_RSSI_INFO                         0x00a4
 #define HostCmd_CMD_FUNC_INIT                         0x00a9
 #define HostCmd_CMD_FUNC_SHUTDOWN                     0x00aa
+#define HostCmd_CMD_UAP_SYS_CONFIG                    0x00b0
+#define HostCmd_CMD_UAP_BSS_START                     0x00b1
+#define HostCmd_CMD_UAP_BSS_STOP                      0x00b2
 #define HostCmd_CMD_11N_CFG                           0x00cd
 #define HostCmd_CMD_11N_ADDBA_REQ                     0x00ce
 #define HostCmd_CMD_11N_ADDBA_RSP                     0x00cf
@@ -214,6 +252,19 @@ enum MWIFIEX_802_11_PRIVACY_FILTER {
 #define HostCmd_CMD_SET_BSS_MODE                      0x00f7
 #define HostCmd_CMD_PCIE_DESC_DETAILS                 0x00fa
 
+#define PROTOCOL_NO_SECURITY        0x01
+#define PROTOCOL_STATIC_WEP         0x02
+#define PROTOCOL_WPA                0x08
+#define PROTOCOL_WPA2               0x20
+#define PROTOCOL_WPA2_MIXED         0x28
+#define PROTOCOL_EAP                0x40
+#define KEY_MGMT_NONE               0x04
+#define KEY_MGMT_PSK                0x02
+#define KEY_MGMT_EAP                0x01
+#define CIPHER_TKIP                 0x04
+#define CIPHER_AES_CCMP             0x08
+#define VALID_CIPHER_BITMAP         0x0c
+
 enum ENH_PS_MODES {
 	EN_PS = 1,
 	DIS_PS = 2,
@@ -228,6 +279,8 @@ enum ENH_PS_MODES {
 #define HostCmd_RET_BIT                       0x8000
 #define HostCmd_ACT_GEN_GET                   0x0000
 #define HostCmd_ACT_GEN_SET                   0x0001
+#define HostCmd_ACT_BITWISE_SET               0x0002
+#define HostCmd_ACT_BITWISE_CLR               0x0003
 #define HostCmd_RESULT_OK                     0x0000
 
 #define HostCmd_ACT_MAC_RX_ON                 0x0001
@@ -302,15 +355,20 @@ enum ENH_PS_MODES {
 #define EVENT_DATA_SNR_HIGH             0x00000027
 #define EVENT_LINK_QUALITY              0x00000028
 #define EVENT_PORT_RELEASE              0x0000002b
+#define EVENT_UAP_STA_DEAUTH            0x0000002c
+#define EVENT_UAP_STA_ASSOC             0x0000002d
+#define EVENT_UAP_BSS_START             0x0000002e
 #define EVENT_PRE_BEACON_LOST           0x00000031
 #define EVENT_ADDBA                     0x00000033
 #define EVENT_DELBA                     0x00000034
 #define EVENT_BA_STREAM_TIEMOUT         0x00000037
 #define EVENT_AMSDU_AGGR_CTRL           0x00000042
+#define EVENT_UAP_BSS_IDLE              0x00000043
+#define EVENT_UAP_BSS_ACTIVE            0x00000044
 #define EVENT_WEP_ICV_ERR               0x00000046
 #define EVENT_HS_ACT_REQ                0x00000047
 #define EVENT_BW_CHANGE                 0x00000048
-
+#define EVENT_UAP_MIC_COUNTERMEASURES   0x0000004c
 #define EVENT_HOSTWAKE_STAIE		0x0000004d
 
 #define EVENT_ID_MASK                   0xffff
@@ -813,7 +871,7 @@ struct host_cmd_ds_txpwr_cfg {
 struct mwifiex_bcn_param {
 	u8 bssid[ETH_ALEN];
 	u8 rssi;
-	__le32 timestamp[2];
+	__le64 timestamp;
 	__le16 beacon_period;
 	__le16 cap_info_bitmap;
 } __packed;
@@ -982,8 +1040,7 @@ struct mwifiex_ie_types_wmm_queue_status {
 struct ieee_types_vendor_header {
 	u8 element_id;
 	u8 len;
-	u8 oui[3];
-	u8 oui_type;
+	u8 oui[4];	/* 0~2: oui, 3: oui_type */
 	u8 oui_subtype;
 	u8 version;
 } __packed;
@@ -1007,7 +1064,7 @@ struct ieee_types_wmm_parameter {
 	struct ieee_types_vendor_header vend_hdr;
 	u8 qos_info_bitmap;
 	u8 reserved;
-	struct ieee_types_wmm_ac_parameters ac_params[IEEE80211_MAX_QUEUES];
+	struct ieee_types_wmm_ac_parameters ac_params[IEEE80211_NUM_ACS];
 } __packed;
 
 struct ieee_types_wmm_info {
@@ -1028,7 +1085,7 @@ struct ieee_types_wmm_info {
 
 struct host_cmd_ds_wmm_get_status {
 	u8 queue_status_tlv[sizeof(struct mwifiex_ie_types_wmm_queue_status) *
-			      IEEE80211_MAX_QUEUES];
+			      IEEE80211_NUM_ACS];
 	u8 wmm_param_tlv[sizeof(struct ieee_types_wmm_parameter) + 2];
 } __packed;
 
@@ -1045,7 +1102,7 @@ struct mwifiex_ie_types_htcap {
 
 struct mwifiex_ie_types_htinfo {
 	struct mwifiex_ie_types_header header;
-	struct ieee80211_ht_info ht_info;
+	struct ieee80211_ht_operation ht_oper;
 } __packed;
 
 struct mwifiex_ie_types_2040bssco {
@@ -1093,6 +1150,101 @@ struct host_cmd_ds_802_11_eeprom_access {
 	u8 value;
 } __packed;
 
+struct host_cmd_tlv {
+	__le16 type;
+	__le16 len;
+} __packed;
+
+struct mwifiex_assoc_event {
+	u8 sta_addr[ETH_ALEN];
+	__le16 type;
+	__le16 len;
+	__le16 frame_control;
+	__le16 cap_info;
+	__le16 listen_interval;
+	u8 data[0];
+} __packed;
+
+struct host_cmd_ds_sys_config {
+	__le16 action;
+	u8 tlv[0];
+};
+
+struct host_cmd_tlv_akmp {
+	struct host_cmd_tlv tlv;
+	__le16 key_mgmt;
+	__le16 key_mgmt_operation;
+} __packed;
+
+struct host_cmd_tlv_pwk_cipher {
+	struct host_cmd_tlv tlv;
+	__le16 proto;
+	u8 cipher;
+	u8 reserved;
+} __packed;
+
+struct host_cmd_tlv_gwk_cipher {
+	struct host_cmd_tlv tlv;
+	u8 cipher;
+	u8 reserved;
+} __packed;
+
+struct host_cmd_tlv_passphrase {
+	struct host_cmd_tlv tlv;
+	u8 passphrase[0];
+} __packed;
+
+struct host_cmd_tlv_auth_type {
+	struct host_cmd_tlv tlv;
+	u8 auth_type;
+} __packed;
+
+struct host_cmd_tlv_encrypt_protocol {
+	struct host_cmd_tlv tlv;
+	__le16 proto;
+} __packed;
+
+struct host_cmd_tlv_ssid {
+	struct host_cmd_tlv tlv;
+	u8 ssid[0];
+} __packed;
+
+struct host_cmd_tlv_beacon_period {
+	struct host_cmd_tlv tlv;
+	__le16 period;
+} __packed;
+
+struct host_cmd_tlv_dtim_period {
+	struct host_cmd_tlv tlv;
+	u8 period;
+} __packed;
+
+struct host_cmd_tlv_frag_threshold {
+	struct host_cmd_tlv tlv;
+	__le16 frag_thr;
+} __packed;
+
+struct host_cmd_tlv_rts_threshold {
+	struct host_cmd_tlv tlv;
+	__le16 rts_thr;
+} __packed;
+
+struct host_cmd_tlv_retry_limit {
+	struct host_cmd_tlv tlv;
+	u8 limit;
+} __packed;
+
+struct host_cmd_tlv_mac_addr {
+	struct host_cmd_tlv tlv;
+	u8 mac_addr[ETH_ALEN];
+} __packed;
+
+struct host_cmd_tlv_channel_band {
+	struct host_cmd_tlv tlv;
+	u8 band_config;
+	u8 channel;
+} __packed;
+
 struct host_cmd_ds_802_11_rf_channel {
 	__le16 action;
 	__le16 current_channel;
@@ -1146,6 +1298,31 @@ struct host_cmd_ds_pcie_details {
 	u32 sleep_cookie_addr_hi;
 } __packed;
 
+struct mwifiex_ie_types_rssi_threshold {
+	struct mwifiex_ie_types_header header;
+	u8 abs_value;
+	u8 evt_freq;
+} __packed;
+
+struct host_cmd_ds_802_11_subsc_evt {
+	__le16 action;
+	__le16 events;
+} __packed;
+
+struct mwifiex_ie {
+	__le16 ie_index;
+	__le16 mgmt_subtype_mask;
+	__le16 ie_length;
+	u8 ie_buffer[IEEE_MAX_IE_SIZE];
+} __packed;
+
+#define MAX_MGMT_IE_INDEX	16
+struct mwifiex_ie_list {
+	__le16 type;
+	__le16 len;
+	struct mwifiex_ie ie_list[MAX_MGMT_IE_INDEX];
+} __packed;
+
 struct host_cmd_ds_command {
 	__le16 command;
 	__le16 size;
@@ -1195,6 +1372,8 @@ struct host_cmd_ds_command {
 		struct host_cmd_ds_set_bss_mode bss_mode;
 		struct host_cmd_ds_pcie_details pcie_host_spec;
 		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;
 	} params;
 } __packed;
 
diff --git a/drivers/net/wireless/mwifiex/ie.c b/drivers/net/wireless/mwifiex/ie.c
new file mode 100644
index 00000000000..ceb82cd749c
--- /dev/null
+++ b/drivers/net/wireless/mwifiex/ie.c
@@ -0,0 +1,396 @@
+/*
+ * Marvell Wireless LAN device driver: management IE handling- setting and
+ * deleting IE.
+ *
+ * Copyright (C) 2012, 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"
+
+/* This function checks if current IE index is used by any on other interface.
+ * Return: -1: yes, current IE index is used by someone else.
+ *          0: no, current IE index is NOT used by other interface.
+ */
+static int
+mwifiex_ie_index_used_by_other_intf(struct mwifiex_private *priv, u16 idx)
+{
+	int i;
+	struct mwifiex_adapter *adapter = priv->adapter;
+	struct mwifiex_ie *ie;
+
+	for (i = 0; i < adapter->priv_num; i++) {
+		if (adapter->priv[i] != priv) {
+			ie = &adapter->priv[i]->mgmt_ie[idx];
+			if (ie->mgmt_subtype_mask && ie->ie_length)
+				return -1;
+		}
+	}
+
+	return 0;
+}
+
+/* Get unused IE index. This index will be used for setting new IE */
+static int
+mwifiex_ie_get_autoidx(struct mwifiex_private *priv, u16 subtype_mask,
+		       struct mwifiex_ie *ie, u16 *index)
+{
+	u16 mask, len, i;
+
+	for (i = 0; i < priv->adapter->max_mgmt_ie_index; i++) {
+		mask = le16_to_cpu(priv->mgmt_ie[i].mgmt_subtype_mask);
+		len = le16_to_cpu(priv->mgmt_ie[i].ie_length) +
+		      le16_to_cpu(ie->ie_length);
+
+		if (mask == MWIFIEX_AUTO_IDX_MASK)
+			continue;
+
+		if (mask == subtype_mask) {
+			if (len > IEEE_MAX_IE_SIZE)
+				continue;
+
+			*index = i;
+			return 0;
+		}
+
+		if (!priv->mgmt_ie[i].ie_length) {
+			if (mwifiex_ie_index_used_by_other_intf(priv, i))
+				continue;
+
+			*index = i;
+			return 0;
+		}
+	}
+
+	return -1;
+}
+
+/* This function prepares IE data buffer for command to be sent to FW */
+static int
+mwifiex_update_autoindex_ies(struct mwifiex_private *priv,
+			     struct mwifiex_ie_list *ie_list)
+{
+	u16 travel_len, index, mask;
+	s16 input_len;
+	struct mwifiex_ie *ie;
+	u8 *tmp;
+
+	input_len = le16_to_cpu(ie_list->len);
+	travel_len = sizeof(struct host_cmd_tlv);
+
+	ie_list->len = 0;
+
+	while (input_len > 0) {
+		ie = (struct mwifiex_ie *)(((u8 *)ie_list) + travel_len);
+		input_len -= le16_to_cpu(ie->ie_length) + MWIFIEX_IE_HDR_SIZE;
+		travel_len += le16_to_cpu(ie->ie_length) + MWIFIEX_IE_HDR_SIZE;
+
+		index = le16_to_cpu(ie->ie_index);
+		mask = le16_to_cpu(ie->mgmt_subtype_mask);
+
+		if (index == MWIFIEX_AUTO_IDX_MASK) {
+			/* automatic addition */
+			if (mwifiex_ie_get_autoidx(priv, mask, ie, &index))
+				return -1;
+			if (index == MWIFIEX_AUTO_IDX_MASK)
+				return -1;
+
+			tmp = (u8 *)&priv->mgmt_ie[index].ie_buffer;
+			tmp += le16_to_cpu(priv->mgmt_ie[index].ie_length);
+			memcpy(tmp, &ie->ie_buffer, le16_to_cpu(ie->ie_length));
+			le16_add_cpu(&priv->mgmt_ie[index].ie_length,
+				     le16_to_cpu(ie->ie_length));
+			priv->mgmt_ie[index].ie_index = cpu_to_le16(index);
+			priv->mgmt_ie[index].mgmt_subtype_mask =
+							cpu_to_le16(mask);
+
+			ie->ie_index = cpu_to_le16(index);
+			ie->ie_length = priv->mgmt_ie[index].ie_length;
+			memcpy(&ie->ie_buffer, &priv->mgmt_ie[index].ie_buffer,
+			       le16_to_cpu(priv->mgmt_ie[index].ie_length));
+		} else {
+			if (mask != MWIFIEX_DELETE_MASK)
+				return -1;
+			/*
+			 * Check if this index is being used on any
+			 * other interface.
+			 */
+			if (mwifiex_ie_index_used_by_other_intf(priv, index))
+				return -1;
+
+			ie->ie_length = 0;
+			memcpy(&priv->mgmt_ie[index], ie,
+			       sizeof(struct mwifiex_ie));
+		}
+
+		le16_add_cpu(&ie_list->len,
+			     le16_to_cpu(priv->mgmt_ie[index].ie_length) +
+			     MWIFIEX_IE_HDR_SIZE);
+	}
+
+	if (GET_BSS_ROLE(priv) == MWIFIEX_BSS_ROLE_UAP)
+		return mwifiex_send_cmd_async(priv, HostCmd_CMD_UAP_SYS_CONFIG,
+					      HostCmd_ACT_GEN_SET,
+					      UAP_CUSTOM_IE_I, ie_list);
+
+	return 0;
+}
+
+/* Copy individual custom IEs for beacon, probe response and assoc response
+ * and prepare single structure for IE setting.
+ * This function also updates allocated IE indices from driver.
+ */
+static int
+mwifiex_update_uap_custom_ie(struct mwifiex_private *priv,
+			     struct mwifiex_ie *beacon_ie, u16 *beacon_idx,
+			     struct mwifiex_ie *pr_ie, u16 *probe_idx,
+			     struct mwifiex_ie *ar_ie, u16 *assoc_idx)
+{
+	struct mwifiex_ie_list *ap_custom_ie;
+	u8 *pos;
+	u16 len;
+	int ret;
+
+	ap_custom_ie = kzalloc(sizeof(struct mwifiex_ie), GFP_KERNEL);
+	if (!ap_custom_ie)
+		return -ENOMEM;
+
+	ap_custom_ie->type = cpu_to_le16(TLV_TYPE_MGMT_IE);
+	pos = (u8 *)ap_custom_ie->ie_list;
+
+	if (beacon_ie) {
+		len = sizeof(struct mwifiex_ie) - IEEE_MAX_IE_SIZE +
+		      le16_to_cpu(beacon_ie->ie_length);
+		memcpy(pos, beacon_ie, len);
+		pos += len;
+		le16_add_cpu(&ap_custom_ie->len, len);
+	}
+	if (pr_ie) {
+		len = sizeof(struct mwifiex_ie) - IEEE_MAX_IE_SIZE +
+		      le16_to_cpu(pr_ie->ie_length);
+		memcpy(pos, pr_ie, len);
+		pos += len;
+		le16_add_cpu(&ap_custom_ie->len, len);
+	}
+	if (ar_ie) {
+		len = sizeof(struct mwifiex_ie) - IEEE_MAX_IE_SIZE +
+		      le16_to_cpu(ar_ie->ie_length);
+		memcpy(pos, ar_ie, len);
+		pos += len;
+		le16_add_cpu(&ap_custom_ie->len, len);
+	}
+
+	ret = mwifiex_update_autoindex_ies(priv, ap_custom_ie);
+
+	pos = (u8 *)(&ap_custom_ie->ie_list[0].ie_index);
+	if (beacon_ie && *beacon_idx == MWIFIEX_AUTO_IDX_MASK) {
+		/* save beacon ie index after auto-indexing */
+		*beacon_idx = le16_to_cpu(ap_custom_ie->ie_list[0].ie_index);
+		len = sizeof(*beacon_ie) - IEEE_MAX_IE_SIZE +
+		      le16_to_cpu(beacon_ie->ie_length);
+		pos += len;
+	}
+	if (pr_ie && le16_to_cpu(pr_ie->ie_index) == MWIFIEX_AUTO_IDX_MASK) {
+		/* save probe resp ie index after auto-indexing */
+		*probe_idx = *((u16 *)pos);
+		len = sizeof(*pr_ie) - IEEE_MAX_IE_SIZE +
+		      le16_to_cpu(pr_ie->ie_length);
+		pos += len;
+	}
+	if (ar_ie && le16_to_cpu(ar_ie->ie_index) == MWIFIEX_AUTO_IDX_MASK)
+		/* save assoc resp ie index after auto-indexing */
+		*assoc_idx = *((u16 *)pos);
+
+	return ret;
+}
+
+/* This function parses different IEs- Tail IEs, beacon IEs, probe response IEs,
+ * association response IEs from cfg80211_ap_settings function and sets these IE
+ * to FW.
+ */
+int mwifiex_set_mgmt_ies(struct mwifiex_private *priv,
+			 struct cfg80211_ap_settings *params)
+{
+	struct mwifiex_ie *beacon_ie = NULL, *pr_ie = NULL;
+	struct mwifiex_ie *ar_ie = NULL, *rsn_ie = NULL;
+	struct ieee_types_header *ie = NULL;
+	u16 beacon_idx = MWIFIEX_AUTO_IDX_MASK, pr_idx = MWIFIEX_AUTO_IDX_MASK;
+	u16 ar_idx = MWIFIEX_AUTO_IDX_MASK, rsn_idx = MWIFIEX_AUTO_IDX_MASK;
+	u16 mask;
+	int ret = 0;
+
+	if (params->beacon.tail && params->beacon.tail_len) {
+		ie = (void *)cfg80211_find_ie(WLAN_EID_RSN, params->beacon.tail,
+					      params->beacon.tail_len);
+		if (ie) {
+			rsn_ie = kmalloc(sizeof(struct mwifiex_ie), GFP_KERNEL);
+			if (!rsn_ie)
+				return -ENOMEM;
+
+			rsn_ie->ie_index = cpu_to_le16(rsn_idx);
+			mask = MGMT_MASK_BEACON | MGMT_MASK_PROBE_RESP |
+			       MGMT_MASK_ASSOC_RESP;
+			rsn_ie->mgmt_subtype_mask = cpu_to_le16(mask);
+			rsn_ie->ie_length = cpu_to_le16(ie->len + 2);
+			memcpy(rsn_ie->ie_buffer, ie, ie->len + 2);
+
+			if (mwifiex_update_uap_custom_ie(priv, rsn_ie, &rsn_idx,
+							 NULL, NULL,
+							 NULL, NULL)) {
+				ret = -1;
+				goto done;
+			}
+
+			priv->rsn_idx = rsn_idx;
+		}
+	}
+
+	if (params->beacon.beacon_ies && params->beacon.beacon_ies_len) {
+		beacon_ie = kmalloc(sizeof(struct mwifiex_ie), GFP_KERNEL);
+		if (!beacon_ie) {
+			ret = -ENOMEM;
+			goto done;
+		}
+
+		beacon_ie->ie_index = cpu_to_le16(beacon_idx);
+		beacon_ie->mgmt_subtype_mask = cpu_to_le16(MGMT_MASK_BEACON);
+		beacon_ie->ie_length =
+				cpu_to_le16(params->beacon.beacon_ies_len);
+		memcpy(beacon_ie->ie_buffer, params->beacon.beacon_ies,
+		       params->beacon.beacon_ies_len);
+	}
+
+	if (params->beacon.proberesp_ies && params->beacon.proberesp_ies_len) {
+		pr_ie = kmalloc(sizeof(struct mwifiex_ie), GFP_KERNEL);
+		if (!pr_ie) {
+			ret = -ENOMEM;
+			goto done;
+		}
+
+		pr_ie->ie_index = cpu_to_le16(pr_idx);
+		pr_ie->mgmt_subtype_mask = cpu_to_le16(MGMT_MASK_PROBE_RESP);
+		pr_ie->ie_length =
+				cpu_to_le16(params->beacon.proberesp_ies_len);
+		memcpy(pr_ie->ie_buffer, params->beacon.proberesp_ies,
+		       params->beacon.proberesp_ies_len);
+	}
+
+	if (params->beacon.assocresp_ies && params->beacon.assocresp_ies_len) {
+		ar_ie = kmalloc(sizeof(struct mwifiex_ie), GFP_KERNEL);
+		if (!ar_ie) {
+			ret = -ENOMEM;
+			goto done;
+		}
+
+		ar_ie->ie_index = cpu_to_le16(ar_idx);
+		mask = MGMT_MASK_ASSOC_RESP | MGMT_MASK_REASSOC_RESP;
+		ar_ie->mgmt_subtype_mask = cpu_to_le16(mask);
+		ar_ie->ie_length =
+				cpu_to_le16(params->beacon.assocresp_ies_len);
+		memcpy(ar_ie->ie_buffer, params->beacon.assocresp_ies,
+		       params->beacon.assocresp_ies_len);
+	}
+
+	if (beacon_ie || pr_ie || ar_ie) {
+		ret = mwifiex_update_uap_custom_ie(priv, beacon_ie,
+						   &beacon_idx, pr_ie,
+						   &pr_idx, ar_ie, &ar_idx);
+		if (ret)
+			goto done;
+	}
+
+	priv->beacon_idx = beacon_idx;
+	priv->proberesp_idx = pr_idx;
+	priv->assocresp_idx = ar_idx;
+
+done:
+	kfree(beacon_ie);
+	kfree(pr_ie);
+	kfree(ar_ie);
+	kfree(rsn_ie);
+
+	return ret;
+}
+
+/* This function removes management IE set */
+int mwifiex_del_mgmt_ies(struct mwifiex_private *priv)
+{
+	struct mwifiex_ie *beacon_ie = NULL, *pr_ie = NULL;
+	struct mwifiex_ie *ar_ie = NULL, *rsn_ie = NULL;
+	int ret = 0;
+
+	if (priv->rsn_idx != MWIFIEX_AUTO_IDX_MASK) {
+		rsn_ie = kmalloc(sizeof(struct mwifiex_ie), GFP_KERNEL);
+		if (!rsn_ie)
+			return -ENOMEM;
+
+		rsn_ie->ie_index = cpu_to_le16(priv->rsn_idx);
+		rsn_ie->mgmt_subtype_mask = cpu_to_le16(MWIFIEX_DELETE_MASK);
+		rsn_ie->ie_length = 0;
+		if (mwifiex_update_uap_custom_ie(priv, rsn_ie, &priv->rsn_idx,
+						 NULL, &priv->proberesp_idx,
+						 NULL, &priv->assocresp_idx)) {
+			ret = -1;
+			goto done;
+		}
+
+		priv->rsn_idx = MWIFIEX_AUTO_IDX_MASK;
+	}
+
+	if (priv->beacon_idx != MWIFIEX_AUTO_IDX_MASK) {
+		beacon_ie = kmalloc(sizeof(struct mwifiex_ie), GFP_KERNEL);
+		if (!beacon_ie) {
+			ret = -ENOMEM;
+			goto done;
+		}
+		beacon_ie->ie_index = cpu_to_le16(priv->beacon_idx);
+		beacon_ie->mgmt_subtype_mask = cpu_to_le16(MWIFIEX_DELETE_MASK);
+		beacon_ie->ie_length = 0;
+	}
+	if (priv->proberesp_idx != MWIFIEX_AUTO_IDX_MASK) {
+		pr_ie = kmalloc(sizeof(struct mwifiex_ie), GFP_KERNEL);
+		if (!pr_ie) {
+			ret = -ENOMEM;
+			goto done;
+		}
+		pr_ie->ie_index = cpu_to_le16(priv->proberesp_idx);
+		pr_ie->mgmt_subtype_mask = cpu_to_le16(MWIFIEX_DELETE_MASK);
+		pr_ie->ie_length = 0;
+	}
+	if (priv->assocresp_idx != MWIFIEX_AUTO_IDX_MASK) {
+		ar_ie = kmalloc(sizeof(struct mwifiex_ie), GFP_KERNEL);
+		if (!ar_ie) {
+			ret = -ENOMEM;
+			goto done;
+		}
+		ar_ie->ie_index = cpu_to_le16(priv->assocresp_idx);
+		ar_ie->mgmt_subtype_mask = cpu_to_le16(MWIFIEX_DELETE_MASK);
+		ar_ie->ie_length = 0;
+	}
+
+	if (beacon_ie || pr_ie || ar_ie)
+		ret = mwifiex_update_uap_custom_ie(priv,
+						   beacon_ie, &priv->beacon_idx,
+						   pr_ie, &priv->proberesp_idx,
+						   ar_ie, &priv->assocresp_idx);
+
+done:
+	kfree(beacon_ie);
+	kfree(pr_ie);
+	kfree(ar_ie);
+	kfree(rsn_ie);
+
+	return ret;
+}
diff --git a/drivers/net/wireless/mwifiex/init.c b/drivers/net/wireless/mwifiex/init.c
index 54bb4839b57..c1cb004db91 100644
--- a/drivers/net/wireless/mwifiex/init.c
+++ b/drivers/net/wireless/mwifiex/init.c
@@ -131,6 +131,8 @@ static int mwifiex_init_priv(struct mwifiex_private *priv)
 	priv->wmm_qosinfo = 0;
 	priv->curr_bcn_buf = NULL;
 	priv->curr_bcn_size = 0;
+	priv->wps_ie = NULL;
+	priv->wps_ie_len = 0;
 
 	priv->scan_block = false;
 
@@ -186,10 +188,10 @@ static void mwifiex_init_adapter(struct mwifiex_adapter *adapter)
 
 	adapter->cmd_sent = false;
 
-	if (adapter->iface_type == MWIFIEX_PCIE)
-		adapter->data_sent = false;
-	else
+	if (adapter->iface_type == MWIFIEX_SDIO)
 		adapter->data_sent = true;
+	else
+		adapter->data_sent = false;
 
 	adapter->cmd_resp_received = false;
 	adapter->event_received = false;
@@ -277,6 +279,7 @@ static void mwifiex_init_adapter(struct mwifiex_adapter *adapter)
 	memset(&adapter->arp_filter, 0, sizeof(adapter->arp_filter));
 	adapter->arp_filter_size = 0;
 	adapter->channel_type = NL80211_CHAN_HT20;
+	adapter->max_mgmt_ie_index = MAX_MGMT_IE_INDEX;
 }
 
 /*
@@ -377,7 +380,8 @@ mwifiex_free_adapter(struct mwifiex_adapter *adapter)
 
 	dev_dbg(adapter->dev, "info: free scan table\n");
 
-	adapter->if_ops.cleanup_if(adapter);
+	if (adapter->if_ops.cleanup_if)
+		adapter->if_ops.cleanup_if(adapter);
 
 	if (adapter->sleep_cfm)
 		dev_kfree_skb_any(adapter->sleep_cfm);
@@ -417,6 +421,8 @@ int mwifiex_init_lock_list(struct mwifiex_adapter *adapter)
 	spin_lock_init(&adapter->cmd_pending_q_lock);
 	spin_lock_init(&adapter->scan_pending_q_lock);
 
+	skb_queue_head_init(&adapter->usb_rx_data_q);
+
 	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;
@@ -572,6 +578,7 @@ mwifiex_shutdown_drv(struct mwifiex_adapter *adapter)
 	struct mwifiex_private *priv;
 	s32 i;
 	unsigned long flags;
+	struct sk_buff *skb;
 
 	/* mwifiex already shutdown */
 	if (adapter->hw_status == MWIFIEX_HW_STATUS_NOT_READY)
@@ -599,6 +606,18 @@ mwifiex_shutdown_drv(struct mwifiex_adapter *adapter)
 
 	spin_lock_irqsave(&adapter->mwifiex_lock, flags);
 
+	if (adapter->if_ops.data_complete) {
+		while ((skb = skb_dequeue(&adapter->usb_rx_data_q))) {
+			struct mwifiex_rxinfo *rx_info = MWIFIEX_SKB_RXCB(skb);
+
+			priv = adapter->priv[rx_info->bss_num];
+			if (priv)
+				priv->stats.rx_dropped++;
+
+			adapter->if_ops.data_complete(adapter, skb);
+		}
+	}
+
 	/* Free adapter structure */
 	mwifiex_free_adapter(adapter);
 
@@ -628,24 +647,28 @@ int mwifiex_dnld_fw(struct mwifiex_adapter *adapter,
 	int ret;
 	u32 poll_num = 1;
 
-	adapter->winner = 0;
+	if (adapter->if_ops.check_fw_status) {
+		adapter->winner = 0;
 
-	/* Check if firmware is already running */
-	ret = adapter->if_ops.check_fw_status(adapter, poll_num);
-	if (!ret) {
-		dev_notice(adapter->dev,
-			   "WLAN FW already running! Skip FW download\n");
-		goto done;
-	}
-	poll_num = MAX_FIRMWARE_POLL_TRIES;
-
-	/* Check if we are the winner for downloading FW */
-	if (!adapter->winner) {
-		dev_notice(adapter->dev,
-			   "Other intf already running! Skip FW download\n");
-		poll_num = MAX_MULTI_INTERFACE_POLL_TRIES;
-		goto poll_fw;
+		/* check if firmware is already running */
+		ret = adapter->if_ops.check_fw_status(adapter, poll_num);
+		if (!ret) {
+			dev_notice(adapter->dev,
+				   "WLAN FW already running! Skip FW dnld\n");
+			goto done;
+		}
+
+		poll_num = MAX_FIRMWARE_POLL_TRIES;
+
+		/* check if we are the winner for downloading FW */
+		if (!adapter->winner) {
+			dev_notice(adapter->dev,
+				   "FW already running! Skip FW dnld\n");
+			poll_num = MAX_MULTI_INTERFACE_POLL_TRIES;
+			goto poll_fw;
+		}
 	}
+
 	if (pmfw) {
 		/* Download firmware with helper */
 		ret = adapter->if_ops.prog_fw(adapter, pmfw);
@@ -664,6 +687,8 @@ poll_fw:
 	}
 done:
 	/* re-enable host interrupt for mwifiex after fw dnld is successful */
-	adapter->if_ops.enable_int(adapter);
+	if (adapter->if_ops.enable_int)
+		adapter->if_ops.enable_int(adapter);
+
 	return ret;
 }
diff --git a/drivers/net/wireless/mwifiex/ioctl.h b/drivers/net/wireless/mwifiex/ioctl.h
index 7ca4e8234f3..e6be6ee7595 100644
--- a/drivers/net/wireless/mwifiex/ioctl.h
+++ b/drivers/net/wireless/mwifiex/ioctl.h
@@ -62,6 +62,36 @@ enum {
 	BAND_AN = 16,
 };
 
+#define MWIFIEX_WPA_PASSHPHRASE_LEN 64
+struct wpa_param {
+	u8 pairwise_cipher_wpa;
+	u8 pairwise_cipher_wpa2;
+	u8 group_cipher;
+	u32 length;
+	u8 passphrase[MWIFIEX_WPA_PASSHPHRASE_LEN];
+};
+
+#define KEY_MGMT_ON_HOST        0x03
+#define MWIFIEX_AUTH_MODE_AUTO  0xFF
+#define BAND_CONFIG_MANUAL      0x00
+struct mwifiex_uap_bss_param {
+	u8 channel;
+	u8 band_cfg;
+	u16 rts_threshold;
+	u16 frag_threshold;
+	u8 retry_limit;
+	struct mwifiex_802_11_ssid ssid;
+	u8 bcast_ssid_ctl;
+	u8 radio_ctl;
+	u8 dtim_period;
+	u16 beacon_period;
+	u16 auth_mode;
+	u16 protocol;
+	u16 key_mgmt;
+	u16 key_mgmt_operation;
+	struct wpa_param wpa_cfg;
+};
+
 enum {
 	ADHOC_IDLE,
 	ADHOC_STARTED,
@@ -85,34 +115,6 @@ struct mwifiex_ds_get_stats {
 	u32 wep_icv_error[4];
 };
 
-#define BCN_RSSI_AVG_MASK               0x00000002
-#define BCN_NF_AVG_MASK                 0x00000200
-#define ALL_RSSI_INFO_MASK              0x00000fff
-
-struct mwifiex_ds_get_signal {
-	/*
-	 * Bit0:  Last Beacon RSSI,  Bit1:  Average Beacon RSSI,
-	 * Bit2:  Last Data RSSI,    Bit3:  Average Data RSSI,
-	 * Bit4:  Last Beacon SNR,   Bit5:  Average Beacon SNR,
-	 * Bit6:  Last Data SNR,     Bit7:  Average Data SNR,
-	 * Bit8:  Last Beacon NF,    Bit9:  Average Beacon NF,
-	 * Bit10: Last Data NF,      Bit11: Average Data NF
-	 */
-	u16 selector;
-	s16 bcn_rssi_last;
-	s16 bcn_rssi_avg;
-	s16 data_rssi_last;
-	s16 data_rssi_avg;
-	s16 bcn_snr_last;
-	s16 bcn_snr_avg;
-	s16 data_snr_last;
-	s16 data_snr_avg;
-	s16 bcn_nf_last;
-	s16 bcn_nf_avg;
-	s16 data_nf_last;
-	s16 data_nf_avg;
-};
-
 #define MWIFIEX_MAX_VER_STR_LEN    128
 
 struct mwifiex_ver_ext {
@@ -124,7 +126,7 @@ struct mwifiex_bss_info {
 	u32 bss_mode;
 	struct cfg80211_ssid ssid;
 	u32 bss_chan;
-	u32 region_code;
+	u8 country_code[3];
 	u32 media_connected;
 	u32 max_power_level;
 	u32 min_power_level;
@@ -297,6 +299,8 @@ struct mwifiex_ds_read_eeprom {
 
 #define IEEE_MAX_IE_SIZE		256
 
+#define MWIFIEX_IE_HDR_SIZE	(sizeof(struct mwifiex_ie) - IEEE_MAX_IE_SIZE)
+
 struct mwifiex_ds_misc_gen_ie {
 	u32 type;
 	u32 len;
@@ -308,8 +312,30 @@ struct mwifiex_ds_misc_cmd {
 	u8 cmd[MWIFIEX_SIZE_OF_CMD_BUFFER];
 };
 
+#define BITMASK_BCN_RSSI_LOW	BIT(0)
+#define BITMASK_BCN_RSSI_HIGH	BIT(4)
+
+enum subsc_evt_rssi_state {
+	EVENT_HANDLED,
+	RSSI_LOW_RECVD,
+	RSSI_HIGH_RECVD
+};
+
+struct subsc_evt_cfg {
+	u8 abs_value;
+	u8 evt_freq;
+};
+
+struct mwifiex_ds_misc_subsc_evt {
+	u16 action;
+	u16 events;
+	struct subsc_evt_cfg bcn_l_rssi_cfg;
+	struct subsc_evt_cfg bcn_h_rssi_cfg;
+};
+
 #define MWIFIEX_MAX_VSIE_LEN       (256)
 #define MWIFIEX_MAX_VSIE_NUM       (8)
+#define MWIFIEX_VSIE_MASK_CLEAR    0x00
 #define MWIFIEX_VSIE_MASK_SCAN     0x01
 #define MWIFIEX_VSIE_MASK_ASSOC    0x02
 #define MWIFIEX_VSIE_MASK_ADHOC    0x04
diff --git a/drivers/net/wireless/mwifiex/join.c b/drivers/net/wireless/mwifiex/join.c
index 8f9382b9c3c..d6b4fb04011 100644
--- a/drivers/net/wireless/mwifiex/join.c
+++ b/drivers/net/wireless/mwifiex/join.c
@@ -118,15 +118,15 @@ mwifiex_cmd_append_tsf_tlv(struct mwifiex_private *priv, u8 **buffer,
 	*buffer += sizeof(tsf_tlv.header);
 
 	/* TSF at the time when beacon/probe_response was received */
-	tsf_val = cpu_to_le64(bss_desc->network_tsf);
+	tsf_val = cpu_to_le64(bss_desc->fw_tsf);
 	memcpy(*buffer, &tsf_val, sizeof(tsf_val));
 	*buffer += sizeof(tsf_val);
 
-	memcpy(&tsf_val, bss_desc->time_stamp, sizeof(tsf_val));
+	tsf_val = cpu_to_le64(bss_desc->timestamp);
 
 	dev_dbg(priv->adapter->dev,
 		"info: %s: TSF offset calc: %016llx - %016llx\n",
-		__func__, tsf_val, bss_desc->network_tsf);
+		__func__, bss_desc->timestamp, bss_desc->fw_tsf);
 
 	memcpy(*buffer, &tsf_val, sizeof(tsf_val));
 	*buffer += sizeof(tsf_val);
@@ -225,6 +225,48 @@ mwifiex_setup_rates_from_bssdesc(struct mwifiex_private *priv,
 }
 
 /*
+ * This function appends a WPS IE. It is called from the network join command
+ * preparation routine.
+ *
+ * If the IE buffer has been setup by the application, this routine appends
+ * the buffer as a WPS TLV type to the request.
+ */
+static int
+mwifiex_cmd_append_wps_ie(struct mwifiex_private *priv, u8 **buffer)
+{
+	int retLen = 0;
+	struct mwifiex_ie_types_header ie_header;
+
+	if (!buffer || !*buffer)
+		return 0;
+
+	/*
+	 * If there is a wps ie buffer setup, append it to the return
+	 * parameter buffer pointer.
+	 */
+	if (priv->wps_ie_len) {
+		dev_dbg(priv->adapter->dev, "cmd: append wps ie %d to %p\n",
+			priv->wps_ie_len, *buffer);
+
+		/* Wrap the generic IE buffer with a pass through TLV type */
+		ie_header.type = cpu_to_le16(TLV_TYPE_MGMT_IE);
+		ie_header.len = cpu_to_le16(priv->wps_ie_len);
+		memcpy(*buffer, &ie_header, sizeof(ie_header));
+		*buffer += sizeof(ie_header);
+		retLen += sizeof(ie_header);
+
+		memcpy(*buffer, priv->wps_ie, priv->wps_ie_len);
+		*buffer += priv->wps_ie_len;
+		retLen += priv->wps_ie_len;
+
+	}
+
+	kfree(priv->wps_ie);
+	priv->wps_ie_len = 0;
+	return retLen;
+}
+
+/*
  * This function appends a WAPI IE.
  *
  * This function is called from the network join command preparation routine.
@@ -480,6 +522,8 @@ int mwifiex_cmd_802_11_associate(struct mwifiex_private *priv,
 	if (priv->sec_info.wapi_enabled && priv->wapi_ie_len)
 		mwifiex_cmd_append_wapi_ie(priv, &pos);
 
+	if (priv->wps.session_enable && priv->wps_ie_len)
+		mwifiex_cmd_append_wps_ie(priv, &pos);
 
 	mwifiex_cmd_append_generic_ie(priv, &pos);
 
@@ -932,20 +976,20 @@ mwifiex_cmd_802_11_ad_hoc_start(struct mwifiex_private *priv,
 		/* Fill HT INFORMATION */
 		ht_info = (struct mwifiex_ie_types_htinfo *) pos;
 		memset(ht_info, 0, sizeof(struct mwifiex_ie_types_htinfo));
-		ht_info->header.type = cpu_to_le16(WLAN_EID_HT_INFORMATION);
+		ht_info->header.type = cpu_to_le16(WLAN_EID_HT_OPERATION);
 		ht_info->header.len =
-				cpu_to_le16(sizeof(struct ieee80211_ht_info));
+			cpu_to_le16(sizeof(struct ieee80211_ht_operation));
 
-		ht_info->ht_info.control_chan =
+		ht_info->ht_oper.primary_chan =
 			(u8) priv->curr_bss_params.bss_descriptor.channel;
 		if (adapter->sec_chan_offset) {
-			ht_info->ht_info.ht_param = adapter->sec_chan_offset;
-			ht_info->ht_info.ht_param |=
+			ht_info->ht_oper.ht_param = adapter->sec_chan_offset;
+			ht_info->ht_oper.ht_param |=
 					IEEE80211_HT_PARAM_CHAN_WIDTH_ANY;
 		}
-		ht_info->ht_info.operation_mode =
+		ht_info->ht_oper.operation_mode =
 		     cpu_to_le16(IEEE80211_HT_OP_MODE_NON_GF_STA_PRSNT);
-		ht_info->ht_info.basic_set[0] = 0xff;
+		ht_info->ht_oper.basic_set[0] = 0xff;
 		pos += sizeof(struct mwifiex_ie_types_htinfo);
 		cmd_append_size +=
 				sizeof(struct mwifiex_ie_types_htinfo);
@@ -1330,22 +1374,28 @@ static int mwifiex_deauthenticate_infra(struct mwifiex_private *priv, u8 *mac)
  *
  * In case of infra made, it sends deauthentication request, and
  * in case of ad-hoc mode, a stop network request is sent to the firmware.
+ * In AP mode, a command to stop bss is sent to firmware.
  */
 int mwifiex_deauthenticate(struct mwifiex_private *priv, u8 *mac)
 {
-	int ret = 0;
+	if (!priv->media_connected)
+		return 0;
 
-	if (priv->media_connected) {
-		if (priv->bss_mode == NL80211_IFTYPE_STATION) {
-			ret = mwifiex_deauthenticate_infra(priv, mac);
-		} else if (priv->bss_mode == NL80211_IFTYPE_ADHOC) {
-			ret = mwifiex_send_cmd_sync(priv,
-						HostCmd_CMD_802_11_AD_HOC_STOP,
-						HostCmd_ACT_GEN_SET, 0, NULL);
-		}
+	switch (priv->bss_mode) {
+	case NL80211_IFTYPE_STATION:
+		return mwifiex_deauthenticate_infra(priv, mac);
+	case NL80211_IFTYPE_ADHOC:
+		return mwifiex_send_cmd_sync(priv,
+					     HostCmd_CMD_802_11_AD_HOC_STOP,
+					     HostCmd_ACT_GEN_SET, 0, NULL);
+	case NL80211_IFTYPE_AP:
+		return mwifiex_send_cmd_sync(priv, HostCmd_CMD_UAP_BSS_STOP,
+					     HostCmd_ACT_GEN_SET, 0, NULL);
+	default:
+		break;
 	}
 
-	return ret;
+	return 0;
 }
 EXPORT_SYMBOL_GPL(mwifiex_deauthenticate);
 
diff --git a/drivers/net/wireless/mwifiex/main.c b/drivers/net/wireless/mwifiex/main.c
index 9d1b3ca6334..3192855c31c 100644
--- a/drivers/net/wireless/mwifiex/main.c
+++ b/drivers/net/wireless/mwifiex/main.c
@@ -58,22 +58,23 @@ static int mwifiex_register(void *card, struct mwifiex_if_ops *if_ops,
 	memmove(&adapter->if_ops, if_ops, sizeof(struct mwifiex_if_ops));
 
 	/* card specific initialization has been deferred until now .. */
-	if (adapter->if_ops.init_if(adapter))
-		goto error;
+	if (adapter->if_ops.init_if)
+		if (adapter->if_ops.init_if(adapter))
+			goto error;
 
 	adapter->priv_num = 0;
 
-	/* Allocate memory for private structure */
-	adapter->priv[0] = kzalloc(sizeof(struct mwifiex_private), GFP_KERNEL);
-	if (!adapter->priv[0]) {
-		dev_err(adapter->dev,
-			"%s: failed to alloc priv[0]\n", __func__);
-		goto error;
-	}
-
-	adapter->priv_num++;
+	for (i = 0; i < MWIFIEX_MAX_BSS_NUM; i++) {
+		/* Allocate memory for private structure */
+		adapter->priv[i] =
+			kzalloc(sizeof(struct mwifiex_private), GFP_KERNEL);
+		if (!adapter->priv[i])
+			goto error;
 
-	adapter->priv[0]->adapter = adapter;
+		adapter->priv[i]->adapter = adapter;
+		adapter->priv[i]->bss_priority = i;
+		adapter->priv_num++;
+	}
 	mwifiex_init_lock_list(adapter);
 
 	init_timer(&adapter->cmd_timer);
@@ -140,6 +141,7 @@ int mwifiex_main_process(struct mwifiex_adapter *adapter)
 {
 	int ret = 0;
 	unsigned long flags;
+	struct sk_buff *skb;
 
 	spin_lock_irqsave(&adapter->main_proc_lock, flags);
 
@@ -161,7 +163,8 @@ process_start:
 		if (adapter->int_status) {
 			if (adapter->hs_activated)
 				mwifiex_process_hs_config(adapter);
-			adapter->if_ops.process_int_status(adapter);
+			if (adapter->if_ops.process_int_status)
+				adapter->if_ops.process_int_status(adapter);
 		}
 
 		/* Need to wake up the card ? */
@@ -174,6 +177,7 @@ process_start:
 			adapter->if_ops.wakeup(adapter);
 			continue;
 		}
+
 		if (IS_CARD_RX_RCVD(adapter)) {
 			adapter->pm_wakeup_fw_try = false;
 			if (adapter->ps_state == PS_STATE_SLEEP)
@@ -194,6 +198,11 @@ process_start:
 			}
 		}
 
+		/* Check Rx data for USB */
+		if (adapter->iface_type == MWIFIEX_USB)
+			while ((skb = skb_dequeue(&adapter->usb_rx_data_q)))
+				mwifiex_handle_rx_packet(adapter, skb);
+
 		/* Check for Cmd Resp */
 		if (adapter->cmd_resp_received) {
 			adapter->cmd_resp_received = false;
@@ -292,33 +301,35 @@ static void mwifiex_free_adapter(struct mwifiex_adapter *adapter)
 }
 
 /*
- * This function initializes the hardware and firmware.
+ * This function gets firmware and initializes it.
  *
  * The main initialization steps followed are -
  *      - Download the correct firmware to card
- *      - Allocate and initialize the adapter structure
- *      - Initialize the private structures
  *      - Issue the init commands to firmware
  */
-static int mwifiex_init_hw_fw(struct mwifiex_adapter *adapter)
+static void mwifiex_fw_dpc(const struct firmware *firmware, void *context)
 {
-	int ret, err;
+	int ret;
+	char fmt[64];
+	struct mwifiex_private *priv;
+	struct mwifiex_adapter *adapter = context;
 	struct mwifiex_fw_image fw;
 
-	memset(&fw, 0, sizeof(struct mwifiex_fw_image));
-
-	err = request_firmware(&adapter->firmware, adapter->fw_name,
-			       adapter->dev);
-	if (err < 0) {
-		dev_err(adapter->dev, "request_firmware() returned"
-				" error code %#x\n", err);
-		ret = -1;
+	if (!firmware) {
+		dev_err(adapter->dev,
+			"Failed to get firmware %s\n", adapter->fw_name);
 		goto done;
 	}
+
+	memset(&fw, 0, sizeof(struct mwifiex_fw_image));
+	adapter->firmware = firmware;
 	fw.fw_buf = (u8 *) adapter->firmware->data;
 	fw.fw_len = adapter->firmware->size;
 
-	ret = mwifiex_dnld_fw(adapter, &fw);
+	if (adapter->if_ops.dnld_fw)
+		ret = adapter->if_ops.dnld_fw(adapter, &fw);
+	else
+		ret = mwifiex_dnld_fw(adapter, &fw);
 	if (ret == -1)
 		goto done;
 
@@ -335,17 +346,61 @@ static int mwifiex_init_hw_fw(struct mwifiex_adapter *adapter)
 	/* Wait for mwifiex_init to complete */
 	wait_event_interruptible(adapter->init_wait_q,
 				 adapter->init_wait_q_woken);
-	if (adapter->hw_status != MWIFIEX_HW_STATUS_READY) {
-		ret = -1;
+	if (adapter->hw_status != MWIFIEX_HW_STATUS_READY)
 		goto done;
+
+	priv = adapter->priv[MWIFIEX_BSS_ROLE_STA];
+	if (mwifiex_register_cfg80211(adapter)) {
+		dev_err(adapter->dev, "cannot register with cfg80211\n");
+		goto err_init_fw;
 	}
-	ret = 0;
 
+	rtnl_lock();
+	/* Create station interface by default */
+	if (!mwifiex_add_virtual_intf(adapter->wiphy, "mlan%d",
+				      NL80211_IFTYPE_STATION, NULL, NULL)) {
+		dev_err(adapter->dev, "cannot create default STA interface\n");
+		goto err_add_intf;
+	}
+
+	/* Create AP interface by default */
+	if (!mwifiex_add_virtual_intf(adapter->wiphy, "uap%d",
+				      NL80211_IFTYPE_AP, NULL, NULL)) {
+		dev_err(adapter->dev, "cannot create default AP interface\n");
+		goto err_add_intf;
+	}
+	rtnl_unlock();
+
+	mwifiex_drv_get_driver_version(adapter, fmt, sizeof(fmt) - 1);
+	dev_notice(adapter->dev, "driver_version = %s\n", fmt);
+	goto done;
+
+err_add_intf:
+	mwifiex_del_virtual_intf(adapter->wiphy, priv->netdev);
+	rtnl_unlock();
+err_init_fw:
+	pr_debug("info: %s: unregister device\n", __func__);
+	adapter->if_ops.unregister_dev(adapter);
 done:
-	if (adapter->firmware)
-		release_firmware(adapter->firmware);
-	if (ret)
-		ret = -1;
+	release_firmware(adapter->firmware);
+	complete(&adapter->fw_load);
+	return;
+}
+
+/*
+ * This function initializes the hardware and gets firmware.
+ */
+static int mwifiex_init_hw_fw(struct mwifiex_adapter *adapter)
+{
+	int ret;
+
+	init_completion(&adapter->fw_load);
+	ret = request_firmware_nowait(THIS_MODULE, 1, adapter->fw_name,
+				      adapter->dev, GFP_KERNEL, adapter,
+				      mwifiex_fw_dpc);
+	if (ret < 0)
+		dev_err(adapter->dev,
+			"request_firmware_nowait() returned error %d\n", ret);
 	return ret;
 }
 
@@ -585,6 +640,12 @@ void mwifiex_init_priv_params(struct mwifiex_private *priv,
 	priv->current_key_index = 0;
 	priv->media_connected = false;
 	memset(&priv->nick_name, 0, sizeof(priv->nick_name));
+	memset(priv->mgmt_ie, 0,
+	       sizeof(struct mwifiex_ie) * MAX_MGMT_IE_INDEX);
+	priv->beacon_idx = MWIFIEX_AUTO_IDX_MASK;
+	priv->proberesp_idx = MWIFIEX_AUTO_IDX_MASK;
+	priv->assocresp_idx = MWIFIEX_AUTO_IDX_MASK;
+	priv->rsn_idx = MWIFIEX_AUTO_IDX_MASK;
 	priv->num_tx_timeout = 0;
 	memcpy(dev->dev_addr, priv->curr_addr, ETH_ALEN);
 }
@@ -650,8 +711,6 @@ mwifiex_add_card(void *card, struct semaphore *sem,
 		 struct mwifiex_if_ops *if_ops, u8 iface_type)
 {
 	struct mwifiex_adapter *adapter;
-	char fmt[64];
-	struct mwifiex_private *priv;
 
 	if (down_interruptible(sem))
 		goto exit_sem_err;
@@ -692,40 +751,13 @@ mwifiex_add_card(void *card, struct semaphore *sem,
 		goto err_init_fw;
 	}
 
-	priv = adapter->priv[0];
-
-	if (mwifiex_register_cfg80211(priv) != 0) {
-		dev_err(adapter->dev, "cannot register netdevice"
-			       " with cfg80211\n");
-			goto err_init_fw;
-	}
-
-	rtnl_lock();
-	/* Create station interface by default */
-	if (!mwifiex_add_virtual_intf(priv->wdev->wiphy, "mlan%d",
-				      NL80211_IFTYPE_STATION, NULL, NULL)) {
-		rtnl_unlock();
-		dev_err(adapter->dev, "cannot create default station"
-				" interface\n");
-		goto err_add_intf;
-	}
-
-	rtnl_unlock();
-
 	up(sem);
-
-	mwifiex_drv_get_driver_version(adapter, fmt, sizeof(fmt) - 1);
-	dev_notice(adapter->dev, "driver_version = %s\n", fmt);
-
 	return 0;
 
-err_add_intf:
-	rtnl_lock();
-	mwifiex_del_virtual_intf(priv->wdev->wiphy, priv->netdev);
-	rtnl_unlock();
 err_init_fw:
 	pr_debug("info: %s: unregister device\n", __func__);
-	adapter->if_ops.unregister_dev(adapter);
+	if (adapter->if_ops.unregister_dev)
+		adapter->if_ops.unregister_dev(adapter);
 err_registerdev:
 	adapter->surprise_removed = true;
 	mwifiex_terminate_workqueue(adapter);
@@ -811,26 +843,29 @@ int mwifiex_remove_card(struct mwifiex_adapter *adapter, struct semaphore *sem)
 
 		rtnl_lock();
 		if (priv->wdev && priv->netdev)
-			mwifiex_del_virtual_intf(priv->wdev->wiphy,
-						 priv->netdev);
+			mwifiex_del_virtual_intf(adapter->wiphy, priv->netdev);
 		rtnl_unlock();
 	}
 
 	priv = adapter->priv[0];
-	if (!priv)
+	if (!priv || !priv->wdev)
 		goto exit_remove;
 
-	if (priv->wdev) {
-		wiphy_unregister(priv->wdev->wiphy);
-		wiphy_free(priv->wdev->wiphy);
-		kfree(priv->wdev);
+	wiphy_unregister(priv->wdev->wiphy);
+	wiphy_free(priv->wdev->wiphy);
+
+	for (i = 0; i < adapter->priv_num; i++) {
+		priv = adapter->priv[i];
+		if (priv)
+			kfree(priv->wdev);
 	}
 
 	mwifiex_terminate_workqueue(adapter);
 
 	/* Unregister device */
 	dev_dbg(adapter->dev, "info: unregister device\n");
-	adapter->if_ops.unregister_dev(adapter);
+	if (adapter->if_ops.unregister_dev)
+		adapter->if_ops.unregister_dev(adapter);
 	/* Free adapter structure */
 	dev_dbg(adapter->dev, "info: free adapter\n");
 	mwifiex_free_adapter(adapter);
diff --git a/drivers/net/wireless/mwifiex/main.h b/drivers/net/wireless/mwifiex/main.h
index 35225e9b108..bd3b0bf94b9 100644
--- a/drivers/net/wireless/mwifiex/main.h
+++ b/drivers/net/wireless/mwifiex/main.h
@@ -92,9 +92,16 @@ enum {
 #define MWIFIEX_OUI_NOT_PRESENT			0
 #define MWIFIEX_OUI_PRESENT				1
 
+/*
+ * Do not check for data_received for USB, as data_received
+ * is handled in mwifiex_usb_recv for USB
+ */
 #define IS_CARD_RX_RCVD(adapter) (adapter->cmd_resp_received || \
-					adapter->event_received || \
-					adapter->data_received)
+				adapter->event_received || \
+				((adapter->iface_type != MWIFIEX_USB) && \
+				adapter->data_received) || \
+				((adapter->iface_type == MWIFIEX_USB) && \
+				!skb_queue_empty(&adapter->usb_rx_data_q)))
 
 #define MWIFIEX_TYPE_CMD				1
 #define MWIFIEX_TYPE_DATA				0
@@ -109,6 +116,12 @@ enum {
 #define MAX_FREQUENCY_BAND_BG   2484
 
 #define MWIFIEX_EVENT_HEADER_LEN           4
+#define MWIFIEX_UAP_EVENT_EXTRA_HEADER	   2
+
+#define MWIFIEX_TYPE_LEN			4
+#define MWIFIEX_USB_TYPE_CMD			0xF00DFACE
+#define MWIFIEX_USB_TYPE_DATA			0xBEADC0DE
+#define MWIFIEX_USB_TYPE_EVENT			0xBEEFFACE
 
 struct mwifiex_dbg {
 	u32 num_cmd_host_to_card_failure;
@@ -162,6 +175,7 @@ enum MWIFIEX_PS_STATE {
 enum mwifiex_iface_type {
 	MWIFIEX_SDIO,
 	MWIFIEX_PCIE,
+	MWIFIEX_USB
 };
 
 struct mwifiex_add_ba_param {
@@ -201,10 +215,10 @@ struct mwifiex_wmm_desc {
 	u32 packets_out[MAX_NUM_TID];
 	/* spin lock to protect ra_list */
 	spinlock_t ra_list_spinlock;
-	struct mwifiex_wmm_ac_status ac_status[IEEE80211_MAX_QUEUES];
-	enum mwifiex_wmm_ac_e ac_down_graded_vals[IEEE80211_MAX_QUEUES];
+	struct mwifiex_wmm_ac_status ac_status[IEEE80211_NUM_ACS];
+	enum mwifiex_wmm_ac_e ac_down_graded_vals[IEEE80211_NUM_ACS];
 	u32 drv_pkt_delay_max;
-	u8 queue_priority[IEEE80211_MAX_QUEUES];
+	u8 queue_priority[IEEE80211_NUM_ACS];
 	u32 user_pri_pkt_tx_ctrl[WMM_HIGHEST_PRIORITY + 1];	/* UP: 0 to 7 */
 	/* Number of transmit packets queued */
 	atomic_t tx_pkts_queued;
@@ -260,8 +274,8 @@ struct mwifiex_bssdescriptor {
 	 * BAND_A(0X04): 'a' band
 	 */
 	u16 bss_band;
-	u64 network_tsf;
-	u8 time_stamp[8];
+	u64 fw_tsf;
+	u64 timestamp;
 	union ieee_types_phy_param_set phy_param_set;
 	union ieee_types_ss_param_set ss_param_set;
 	u16 cap_info_bitmap;
@@ -269,7 +283,7 @@ struct mwifiex_bssdescriptor {
 	u8  disable_11n;
 	struct ieee80211_ht_cap *bcn_ht_cap;
 	u16 ht_cap_offset;
-	struct ieee80211_ht_info *bcn_ht_info;
+	struct ieee80211_ht_operation *bcn_ht_oper;
 	u16 ht_info_offset;
 	u8 *bcn_bss_co_2040;
 	u16 bss_co_2040_offset;
@@ -357,6 +371,7 @@ struct mwifiex_private {
 	u8 bss_role;
 	u8 bss_priority;
 	u8 bss_num;
+	u8 bss_started;
 	u8 frame_type;
 	u8 curr_addr[ETH_ALEN];
 	u8 media_connected;
@@ -407,6 +422,8 @@ struct mwifiex_private {
 	struct host_cmd_ds_802_11_key_material aes_key;
 	u8 wapi_ie[256];
 	u8 wapi_ie_len;
+	u8 *wps_ie;
+	u8 wps_ie_len;
 	u8 wmm_required;
 	u8 wmm_enabled;
 	u8 wmm_qosinfo;
@@ -448,7 +465,6 @@ struct mwifiex_private {
 	struct dentry *dfs_dev_dir;
 #endif
 	u8 nick_name[16];
-	u8 qual_level, qual_noise;
 	u16 current_key_index;
 	struct semaphore async_sem;
 	u8 scan_pending_on_block;
@@ -456,9 +472,16 @@ struct mwifiex_private {
 	struct cfg80211_scan_request *scan_request;
 	struct mwifiex_user_scan_cfg *user_scan_cfg;
 	u8 cfg_bssid[6];
-	u8 country_code[IEEE80211_COUNTRY_STRING_LEN];
 	struct wps wps;
 	u8 scan_block;
+	s32 cqm_rssi_thold;
+	u32 cqm_rssi_hyst;
+	u8 subsc_evt_rssi_state;
+	struct mwifiex_ie mgmt_ie[MAX_MGMT_IE_INDEX];
+	u16 beacon_idx;
+	u16 proberesp_idx;
+	u16 assocresp_idx;
+	u16 rsn_idx;
 };
 
 enum mwifiex_ba_status {
@@ -518,6 +541,11 @@ struct cmd_ctrl_node {
 	u8 cmd_wait_q_woken;
 };
 
+struct mwifiex_bss_priv {
+	u8 band;
+	u64 fw_tsf;
+};
+
 struct mwifiex_if_ops {
 	int (*init_if) (struct mwifiex_adapter *);
 	void (*cleanup_if) (struct mwifiex_adapter *);
@@ -537,6 +565,8 @@ struct mwifiex_if_ops {
 	void (*cleanup_mpa_buf) (struct mwifiex_adapter *);
 	int (*cmdrsp_complete) (struct mwifiex_adapter *, struct sk_buff *);
 	int (*event_complete) (struct mwifiex_adapter *, struct sk_buff *);
+	int (*data_complete) (struct mwifiex_adapter *, struct sk_buff *);
+	int (*dnld_fw) (struct mwifiex_adapter *, struct mwifiex_fw_image *);
 };
 
 struct mwifiex_adapter {
@@ -547,6 +577,7 @@ struct mwifiex_adapter {
 	char fw_name[32];
 	int winner;
 	struct device *dev;
+	struct wiphy *wiphy;
 	bool surprise_removed;
 	u32 fw_release_number;
 	u16 init_wait_q_woken;
@@ -599,6 +630,7 @@ struct mwifiex_adapter {
 	struct list_head scan_pending_q;
 	/* spin lock for scan_pending_q */
 	spinlock_t scan_pending_q_lock;
+	struct sk_buff_head usb_rx_data_q;
 	u32 scan_processing;
 	u16 region_code;
 	struct mwifiex_802_11d_domain_reg domain_reg;
@@ -651,6 +683,9 @@ struct mwifiex_adapter {
 	u8 scan_wait_q_woken;
 	struct cmd_ctrl_node *cmd_queued;
 	spinlock_t queue_lock;		/* lock for tx queues */
+	struct completion fw_load;
+	u8 country_code[IEEE80211_COUNTRY_STRING_LEN];
+	u16 max_mgmt_ie_index;
 };
 
 int mwifiex_init_lock_list(struct mwifiex_adapter *adapter);
@@ -734,6 +769,9 @@ int mwifiex_process_rx_packet(struct mwifiex_adapter *adapter,
 int mwifiex_sta_prepare_cmd(struct mwifiex_private *, uint16_t cmd_no,
 			    u16 cmd_action, u32 cmd_oid,
 			    void *data_buf, void *cmd_buf);
+int mwifiex_uap_prepare_cmd(struct mwifiex_private *priv, uint16_t cmd_no,
+			    u16 cmd_action, u32 cmd_oid,
+			    void *data_buf, void *cmd_buf);
 int mwifiex_process_sta_cmdresp(struct mwifiex_private *, u16 cmdresp_no,
 				struct host_cmd_ds_command *resp);
 int mwifiex_process_sta_rx_packet(struct mwifiex_adapter *,
@@ -794,6 +832,9 @@ int mwifiex_ret_get_hw_spec(struct mwifiex_private *priv,
 int is_command_pending(struct mwifiex_adapter *adapter);
 void mwifiex_init_priv_params(struct mwifiex_private *priv,
 						struct net_device *dev);
+int mwifiex_set_secure_params(struct mwifiex_private *priv,
+			      struct mwifiex_uap_bss_param *bss_config,
+			      struct cfg80211_ap_settings *params);
 
 /*
  * This function checks if the queuing is RA based or not.
@@ -896,8 +937,6 @@ int mwifiex_bss_start(struct mwifiex_private *priv, struct cfg80211_bss *bss,
 int mwifiex_cancel_hs(struct mwifiex_private *priv, int cmd_type);
 int mwifiex_enable_hs(struct mwifiex_adapter *adapter);
 int mwifiex_disable_auto_ds(struct mwifiex_private *priv);
-int mwifiex_get_signal_info(struct mwifiex_private *priv,
-			    struct mwifiex_ds_get_signal *signal);
 int mwifiex_drv_get_data_rate(struct mwifiex_private *priv,
 			      struct mwifiex_rate_cfg *rate);
 int mwifiex_request_scan(struct mwifiex_private *priv,
@@ -909,7 +948,8 @@ int mwifiex_set_radio(struct mwifiex_private *priv, u8 option);
 int mwifiex_drv_change_adhoc_chan(struct mwifiex_private *priv, u16 channel);
 
 int mwifiex_set_encode(struct mwifiex_private *priv, const u8 *key,
-		       int key_len, u8 key_index, int disable);
+		       int key_len, u8 key_index, const u8 *mac_addr,
+		       int disable);
 
 int mwifiex_set_gen_ie(struct mwifiex_private *priv, u8 *ie, int ie_len);
 
@@ -945,18 +985,16 @@ int mwifiex_set_tx_power(struct mwifiex_private *priv,
 
 int mwifiex_main_process(struct mwifiex_adapter *);
 
+int mwifiex_uap_set_channel(struct mwifiex_private *priv, int channel);
 int mwifiex_bss_set_channel(struct mwifiex_private *,
 			    struct mwifiex_chan_freq_power *cfp);
 int mwifiex_get_bss_info(struct mwifiex_private *,
 			 struct mwifiex_bss_info *);
 int mwifiex_fill_new_bss_desc(struct mwifiex_private *priv,
-			      u8 *bssid, s32 rssi, u8 *ie_buf,
-			      size_t ie_len, u16 beacon_period,
-			      u16 cap_info_bitmap, u8 band,
+			      struct cfg80211_bss *bss,
 			      struct mwifiex_bssdescriptor *bss_desc);
 int mwifiex_update_bss_desc_with_ie(struct mwifiex_adapter *adapter,
-				struct mwifiex_bssdescriptor *bss_entry,
-				u8 *ie_buf, u32 ie_len);
+				    struct mwifiex_bssdescriptor *bss_entry);
 int mwifiex_check_network_compatibility(struct mwifiex_private *priv,
 					struct mwifiex_bssdescriptor *bss_desc);
 
@@ -965,6 +1003,12 @@ struct net_device *mwifiex_add_virtual_intf(struct wiphy *wiphy,
 					u32 *flags, struct vif_params *params);
 int mwifiex_del_virtual_intf(struct wiphy *wiphy, struct net_device *dev);
 
+void mwifiex_set_sys_config_invalid_data(struct mwifiex_uap_bss_param *config);
+
+int mwifiex_set_mgmt_ies(struct mwifiex_private *priv,
+			 struct cfg80211_ap_settings *params);
+int mwifiex_del_mgmt_ies(struct mwifiex_private *priv);
+u8 *mwifiex_11d_code_2_region(u8 code);
 
 #ifdef CONFIG_DEBUG_FS
 void mwifiex_debugfs_init(void);
diff --git a/drivers/net/wireless/mwifiex/pcie.c b/drivers/net/wireless/mwifiex/pcie.c
index 5867facd415..13fbc4eb159 100644
--- a/drivers/net/wireless/mwifiex/pcie.c
+++ b/drivers/net/wireless/mwifiex/pcie.c
@@ -119,6 +119,9 @@ static void mwifiex_pcie_remove(struct pci_dev *pdev)
 	if (!adapter || !adapter->priv_num)
 		return;
 
+	/* In case driver is removed when asynchronous FW load is in progress */
+	wait_for_completion(&adapter->fw_load);
+
 	if (user_rmmod) {
 #ifdef CONFIG_PM
 		if (adapter->is_suspended)
diff --git a/drivers/net/wireless/mwifiex/scan.c b/drivers/net/wireless/mwifiex/scan.c
index aff9cd763f2..74f04571572 100644
--- a/drivers/net/wireless/mwifiex/scan.c
+++ b/drivers/net/wireless/mwifiex/scan.c
@@ -1048,10 +1048,8 @@ mwifiex_ret_802_11_scan_get_tlv_ptrs(struct mwifiex_adapter *adapter,
  * This function parses provided beacon buffer and updates
  * respective fields in bss descriptor structure.
  */
-int
-mwifiex_update_bss_desc_with_ie(struct mwifiex_adapter *adapter,
-				struct mwifiex_bssdescriptor *bss_entry,
-				u8 *ie_buf, u32 ie_len)
+int mwifiex_update_bss_desc_with_ie(struct mwifiex_adapter *adapter,
+				    struct mwifiex_bssdescriptor *bss_entry)
 {
 	int ret = 0;
 	u8 element_id;
@@ -1073,10 +1071,8 @@ mwifiex_update_bss_desc_with_ie(struct mwifiex_adapter *adapter,
 
 	found_data_rate_ie = false;
 	rate_size = 0;
-	current_ptr = ie_buf;
-	bytes_left = ie_len;
-	bss_entry->beacon_buf = ie_buf;
-	bss_entry->beacon_buf_size = ie_len;
+	current_ptr = bss_entry->beacon_buf;
+	bytes_left = bss_entry->beacon_buf_size;
 
 	/* Process variable IE */
 	while (bytes_left >= 2) {
@@ -1221,9 +1217,9 @@ mwifiex_update_bss_desc_with_ie(struct mwifiex_adapter *adapter,
 					sizeof(struct ieee_types_header) -
 					bss_entry->beacon_buf);
 			break;
-		case WLAN_EID_HT_INFORMATION:
-			bss_entry->bcn_ht_info = (struct ieee80211_ht_info *)
-					(current_ptr +
+		case WLAN_EID_HT_OPERATION:
+			bss_entry->bcn_ht_oper =
+				(struct ieee80211_ht_operation *)(current_ptr +
 					sizeof(struct ieee_types_header));
 			bss_entry->ht_info_offset = (u16) (current_ptr +
 					sizeof(struct ieee_types_header) -
@@ -1447,15 +1443,12 @@ int mwifiex_check_network_compatibility(struct mwifiex_private *priv,
 	return ret;
 }
 
-static int
-mwifiex_update_curr_bss_params(struct mwifiex_private *priv, u8 *bssid,
-			       s32 rssi, const u8 *ie_buf, size_t ie_len,
-			       u16 beacon_period, u16 cap_info_bitmap, u8 band)
+static int mwifiex_update_curr_bss_params(struct mwifiex_private *priv,
+					  struct cfg80211_bss *bss)
 {
 	struct mwifiex_bssdescriptor *bss_desc;
 	int ret;
 	unsigned long flags;
-	u8 *beacon_ie;
 
 	/* Allocate and fill new bss descriptor */
 	bss_desc = kzalloc(sizeof(struct mwifiex_bssdescriptor),
@@ -1465,16 +1458,7 @@ mwifiex_update_curr_bss_params(struct mwifiex_private *priv, u8 *bssid,
 		return -ENOMEM;
 	}
 
-	beacon_ie = kmemdup(ie_buf, ie_len, GFP_KERNEL);
-	if (!beacon_ie) {
-		kfree(bss_desc);
-		dev_err(priv->adapter->dev, " failed to alloc beacon_ie\n");
-		return -ENOMEM;
-	}
-
-	ret = mwifiex_fill_new_bss_desc(priv, bssid, rssi, beacon_ie,
-					ie_len, beacon_period,
-					cap_info_bitmap, band, bss_desc);
+	ret = mwifiex_fill_new_bss_desc(priv, bss, bss_desc);
 	if (ret)
 		goto done;
 
@@ -1493,7 +1477,7 @@ mwifiex_update_curr_bss_params(struct mwifiex_private *priv, u8 *bssid,
 	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_info = NULL;
+	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 =
@@ -1514,7 +1498,6 @@ mwifiex_update_curr_bss_params(struct mwifiex_private *priv, u8 *bssid,
 
 done:
 	kfree(bss_desc);
-	kfree(beacon_ie);
 	return 0;
 }
 
@@ -1620,14 +1603,16 @@ int mwifiex_ret_802_11_scan(struct mwifiex_private *priv,
 		const u8 *ie_buf;
 		size_t ie_len;
 		u16 channel = 0;
-		u64 network_tsf = 0;
+		u64 fw_tsf = 0;
 		u16 beacon_size = 0;
 		u32 curr_bcn_bytes;
 		u32 freq;
 		u16 beacon_period;
 		u16 cap_info_bitmap;
 		u8 *current_ptr;
+		u64 timestamp;
 		struct mwifiex_bcn_param *bcn_param;
+		struct mwifiex_bss_priv *bss_priv;
 
 		if (bytes_left >= sizeof(beacon_size)) {
 			/* Extract & convert beacon size from command buffer */
@@ -1667,9 +1652,11 @@ int mwifiex_ret_802_11_scan(struct mwifiex_private *priv,
 
 		memcpy(bssid, bcn_param->bssid, ETH_ALEN);
 
-		rssi = (s32) (bcn_param->rssi);
-		dev_dbg(adapter->dev, "info: InterpretIE: RSSI=%02X\n", rssi);
+		rssi = (s32) bcn_param->rssi;
+		rssi = (-rssi) * 100;		/* Convert dBm to mBm */
+		dev_dbg(adapter->dev, "info: InterpretIE: RSSI=%d\n", rssi);
 
+		timestamp = le64_to_cpu(bcn_param->timestamp);
 		beacon_period = le16_to_cpu(bcn_param->beacon_period);
 
 		cap_info_bitmap = le16_to_cpu(bcn_param->cap_info_bitmap);
@@ -1709,14 +1696,13 @@ int mwifiex_ret_802_11_scan(struct mwifiex_private *priv,
 
 		/*
 		 * If the TSF TLV was appended to the scan results, save this
-		 * entry's TSF value in the networkTSF field.The networkTSF is
-		 * the firmware's TSF value at the time the beacon or probe
-		 * response was received.
+		 * entry's TSF value in the fw_tsf field. It is the firmware's
+		 * TSF value at the time the beacon or probe response was
+		 * received.
 		 */
 		if (tsf_tlv)
-			memcpy(&network_tsf,
-			       &tsf_tlv->tsf_data[idx * TSF_DATA_SIZE],
-			       sizeof(network_tsf));
+			memcpy(&fw_tsf, &tsf_tlv->tsf_data[idx * TSF_DATA_SIZE],
+			       sizeof(fw_tsf));
 
 		if (channel) {
 			struct ieee80211_channel *chan;
@@ -1739,21 +1725,19 @@ int mwifiex_ret_802_11_scan(struct mwifiex_private *priv,
 
 			if (chan && !(chan->flags & IEEE80211_CHAN_DISABLED)) {
 				bss = cfg80211_inform_bss(priv->wdev->wiphy,
-					      chan, bssid, network_tsf,
+					      chan, bssid, timestamp,
 					      cap_info_bitmap, beacon_period,
 					      ie_buf, ie_len, rssi, GFP_KERNEL);
-				*(u8 *)bss->priv = band;
-				cfg80211_put_bss(bss);
-
+				bss_priv = (struct mwifiex_bss_priv *)bss->priv;
+				bss_priv->band = band;
+				bss_priv->fw_tsf = fw_tsf;
 				if (priv->media_connected &&
 				    !memcmp(bssid,
 					    priv->curr_bss_params.bss_descriptor
 					    .mac_address, ETH_ALEN))
-					mwifiex_update_curr_bss_params
-							(priv, bssid, rssi,
-							 ie_buf, ie_len,
-							 beacon_period,
-							 cap_info_bitmap, band);
+					mwifiex_update_curr_bss_params(priv,
+								       bss);
+				cfg80211_put_bss(bss);
 			}
 		} else {
 			dev_dbg(adapter->dev, "missing BSS channel IE\n");
@@ -2019,8 +2003,8 @@ mwifiex_save_curr_bcn(struct mwifiex_private *priv)
 			(curr_bss->beacon_buf +
 			 curr_bss->ht_cap_offset);
 
-	if (curr_bss->bcn_ht_info)
-		curr_bss->bcn_ht_info = (struct ieee80211_ht_info *)
+	if (curr_bss->bcn_ht_oper)
+		curr_bss->bcn_ht_oper = (struct ieee80211_ht_operation *)
 			(curr_bss->beacon_buf +
 			 curr_bss->ht_info_offset);
 
diff --git a/drivers/net/wireless/mwifiex/sdio.c b/drivers/net/wireless/mwifiex/sdio.c
index f8012e2b7f7..e0377473282 100644
--- a/drivers/net/wireless/mwifiex/sdio.c
+++ b/drivers/net/wireless/mwifiex/sdio.c
@@ -123,6 +123,9 @@ mwifiex_sdio_remove(struct sdio_func *func)
 	if (!adapter || !adapter->priv_num)
 		return;
 
+	/* In case driver is removed when asynchronous FW load is in progress */
+	wait_for_completion(&adapter->fw_load);
+
 	if (user_rmmod) {
 		if (adapter->is_suspended)
 			mwifiex_sdio_resume(adapter->dev);
@@ -250,6 +253,8 @@ static int mwifiex_sdio_resume(struct device *dev)
 	return 0;
 }
 
+/* Device ID for SD8786 */
+#define SDIO_DEVICE_ID_MARVELL_8786   (0x9116)
 /* Device ID for SD8787 */
 #define SDIO_DEVICE_ID_MARVELL_8787   (0x9119)
 /* Device ID for SD8797 */
@@ -257,6 +262,7 @@ static int mwifiex_sdio_resume(struct device *dev)
 
 /* WLAN IDs */
 static const struct sdio_device_id mwifiex_ids[] = {
+	{SDIO_DEVICE(SDIO_VENDOR_ID_MARVELL, SDIO_DEVICE_ID_MARVELL_8786)},
 	{SDIO_DEVICE(SDIO_VENDOR_ID_MARVELL, SDIO_DEVICE_ID_MARVELL_8787)},
 	{SDIO_DEVICE(SDIO_VENDOR_ID_MARVELL, SDIO_DEVICE_ID_MARVELL_8797)},
 	{},
@@ -1596,6 +1602,9 @@ static int mwifiex_register_dev(struct mwifiex_adapter *adapter)
 	adapter->dev = &func->dev;
 
 	switch (func->device) {
+	case SDIO_DEVICE_ID_MARVELL_8786:
+		strcpy(adapter->fw_name, SD8786_DEFAULT_FW_NAME);
+		break;
 	case SDIO_DEVICE_ID_MARVELL_8797:
 		strcpy(adapter->fw_name, SD8797_DEFAULT_FW_NAME);
 		break;
@@ -1804,5 +1813,6 @@ MODULE_AUTHOR("Marvell International Ltd.");
 MODULE_DESCRIPTION("Marvell WiFi-Ex SDIO Driver version " SDIO_VERSION);
 MODULE_VERSION(SDIO_VERSION);
 MODULE_LICENSE("GPL v2");
+MODULE_FIRMWARE(SD8786_DEFAULT_FW_NAME);
 MODULE_FIRMWARE(SD8787_DEFAULT_FW_NAME);
 MODULE_FIRMWARE(SD8797_DEFAULT_FW_NAME);
diff --git a/drivers/net/wireless/mwifiex/sdio.h b/drivers/net/wireless/mwifiex/sdio.h
index a3fb322205b..21033738ef0 100644
--- a/drivers/net/wireless/mwifiex/sdio.h
+++ b/drivers/net/wireless/mwifiex/sdio.h
@@ -28,6 +28,7 @@
 
 #include "main.h"
 
+#define SD8786_DEFAULT_FW_NAME "mrvl/sd8786_uapsta.bin"
 #define SD8787_DEFAULT_FW_NAME "mrvl/sd8787_uapsta.bin"
 #define SD8797_DEFAULT_FW_NAME "mrvl/sd8797_uapsta.bin"
 
@@ -193,7 +194,7 @@
 		a->mpa_tx.ports |= (1<<(a->mpa_tx.pkt_cnt+1+(MAX_PORT -	\
 						a->mp_end_port)));	\
 	a->mpa_tx.pkt_cnt++;						\
-} while (0);
+} while (0)
 
 /* SDIO Tx aggregation limit ? */
 #define MP_TX_AGGR_PKT_LIMIT_REACHED(a)					\
@@ -211,7 +212,7 @@
 	a->mpa_tx.buf_len = 0;						\
 	a->mpa_tx.ports = 0;						\
 	a->mpa_tx.start_port = 0;					\
-} while (0);
+} while (0)
 
 /* SDIO Rx aggregation limit ? */
 #define MP_RX_AGGR_PKT_LIMIT_REACHED(a)					\
@@ -242,7 +243,7 @@
 	a->mpa_rx.skb_arr[a->mpa_rx.pkt_cnt] = skb;			\
 	a->mpa_rx.len_arr[a->mpa_rx.pkt_cnt] = skb->len;		\
 	a->mpa_rx.pkt_cnt++;						\
-} while (0);
+} while (0)
 
 /* Reset SDIO Rx aggregation buffer parameters */
 #define MP_RX_AGGR_BUF_RESET(a) do {					\
@@ -250,7 +251,7 @@
 	a->mpa_rx.buf_len = 0;						\
 	a->mpa_rx.ports = 0;						\
 	a->mpa_rx.start_port = 0;					\
-} while (0);
+} while (0)
 
 
 /* data structure for SDIO MPA TX */
diff --git a/drivers/net/wireless/mwifiex/sta_cmd.c b/drivers/net/wireless/mwifiex/sta_cmd.c
index 6c8e4594b48..40e025da6bc 100644
--- a/drivers/net/wireless/mwifiex/sta_cmd.c
+++ b/drivers/net/wireless/mwifiex/sta_cmd.c
@@ -498,7 +498,8 @@ mwifiex_cmd_802_11_key_material(struct mwifiex_private *priv,
 {
 	struct host_cmd_ds_802_11_key_material *key_material =
 		&cmd->params.key_material;
-	u16 key_param_len = 0;
+	struct host_cmd_tlv_mac_addr *tlv_mac;
+	u16 key_param_len = 0, cmd_size;
 	int ret = 0;
 	const u8 bc_mac[] = { 0xff, 0xff, 0xff, 0xff, 0xff, 0xff };
 
@@ -614,11 +615,26 @@ mwifiex_cmd_802_11_key_material(struct mwifiex_private *priv,
 			cpu_to_le16((u16) enc_key->key_len +
 				    KEYPARAMSET_FIXED_LEN);
 
-		key_param_len = (u16) (enc_key->key_len + KEYPARAMSET_FIXED_LEN)
+		key_param_len = (u16)(enc_key->key_len + KEYPARAMSET_FIXED_LEN)
 				+ sizeof(struct mwifiex_ie_types_header);
 
 		cmd->size = cpu_to_le16(sizeof(key_material->action) + S_DS_GEN
 					+ key_param_len);
+
+		if (priv->bss_type == MWIFIEX_BSS_TYPE_UAP) {
+			tlv_mac = (void *)((u8 *)&key_material->key_param_set +
+					   key_param_len);
+			tlv_mac->tlv.type = cpu_to_le16(TLV_TYPE_STA_MAC_ADDR);
+			tlv_mac->tlv.len = cpu_to_le16(ETH_ALEN);
+			memcpy(tlv_mac->mac_addr, enc_key->mac_addr, ETH_ALEN);
+			cmd_size = key_param_len + S_DS_GEN +
+				   sizeof(key_material->action) +
+				   sizeof(struct host_cmd_tlv_mac_addr);
+		} else {
+			cmd_size = key_param_len + S_DS_GEN +
+				   sizeof(key_material->action);
+		}
+		cmd->size = cpu_to_le16(cmd_size);
 	}
 
 	return ret;
@@ -907,6 +923,101 @@ mwifiex_cmd_pcie_host_spec(struct mwifiex_private *priv,
 }
 
 /*
+ * This function prepares command for event subscription, configuration
+ * and query. Events can be subscribed or unsubscribed. Current subscribed
+ * events can be queried. Also, current subscribed events are reported in
+ * every FW response.
+ */
+static int
+mwifiex_cmd_802_11_subsc_evt(struct mwifiex_private *priv,
+			     struct host_cmd_ds_command *cmd,
+			     struct mwifiex_ds_misc_subsc_evt *subsc_evt_cfg)
+{
+	struct host_cmd_ds_802_11_subsc_evt *subsc_evt = &cmd->params.subsc_evt;
+	struct mwifiex_ie_types_rssi_threshold *rssi_tlv;
+	u16 event_bitmap;
+	u8 *pos;
+
+	cmd->command = cpu_to_le16(HostCmd_CMD_802_11_SUBSCRIBE_EVENT);
+	cmd->size = cpu_to_le16(sizeof(struct host_cmd_ds_802_11_subsc_evt) +
+				S_DS_GEN);
+
+	subsc_evt->action = cpu_to_le16(subsc_evt_cfg->action);
+	dev_dbg(priv->adapter->dev, "cmd: action: %d\n", subsc_evt_cfg->action);
+
+	/*For query requests, no configuration TLV structures are to be added.*/
+	if (subsc_evt_cfg->action == HostCmd_ACT_GEN_GET)
+		return 0;
+
+	subsc_evt->events = cpu_to_le16(subsc_evt_cfg->events);
+
+	event_bitmap = subsc_evt_cfg->events;
+	dev_dbg(priv->adapter->dev, "cmd: event bitmap : %16x\n",
+		event_bitmap);
+
+	if (((subsc_evt_cfg->action == HostCmd_ACT_BITWISE_CLR) ||
+	     (subsc_evt_cfg->action == HostCmd_ACT_BITWISE_SET)) &&
+	    (event_bitmap == 0)) {
+		dev_dbg(priv->adapter->dev, "Error: No event specified "
+			"for bitwise action type\n");
+		return -EINVAL;
+	}
+
+	/*
+	 * Append TLV structures for each of the specified events for
+	 * subscribing or re-configuring. This is not required for
+	 * bitwise unsubscribing request.
+	 */
+	if (subsc_evt_cfg->action == HostCmd_ACT_BITWISE_CLR)
+		return 0;
+
+	pos = ((u8 *)subsc_evt) +
+			sizeof(struct host_cmd_ds_802_11_subsc_evt);
+
+	if (event_bitmap & BITMASK_BCN_RSSI_LOW) {
+		rssi_tlv = (struct mwifiex_ie_types_rssi_threshold *) pos;
+
+		rssi_tlv->header.type = cpu_to_le16(TLV_TYPE_RSSI_LOW);
+		rssi_tlv->header.len =
+		    cpu_to_le16(sizeof(struct mwifiex_ie_types_rssi_threshold) -
+				sizeof(struct mwifiex_ie_types_header));
+		rssi_tlv->abs_value = subsc_evt_cfg->bcn_l_rssi_cfg.abs_value;
+		rssi_tlv->evt_freq = subsc_evt_cfg->bcn_l_rssi_cfg.evt_freq;
+
+		dev_dbg(priv->adapter->dev, "Cfg Beacon Low Rssi event, "
+			"RSSI:-%d dBm, Freq:%d\n",
+			subsc_evt_cfg->bcn_l_rssi_cfg.abs_value,
+			subsc_evt_cfg->bcn_l_rssi_cfg.evt_freq);
+
+		pos += sizeof(struct mwifiex_ie_types_rssi_threshold);
+		le16_add_cpu(&cmd->size,
+			     sizeof(struct mwifiex_ie_types_rssi_threshold));
+	}
+
+	if (event_bitmap & BITMASK_BCN_RSSI_HIGH) {
+		rssi_tlv = (struct mwifiex_ie_types_rssi_threshold *) pos;
+
+		rssi_tlv->header.type = cpu_to_le16(TLV_TYPE_RSSI_HIGH);
+		rssi_tlv->header.len =
+		    cpu_to_le16(sizeof(struct mwifiex_ie_types_rssi_threshold) -
+				sizeof(struct mwifiex_ie_types_header));
+		rssi_tlv->abs_value = subsc_evt_cfg->bcn_h_rssi_cfg.abs_value;
+		rssi_tlv->evt_freq = subsc_evt_cfg->bcn_h_rssi_cfg.evt_freq;
+
+		dev_dbg(priv->adapter->dev, "Cfg Beacon High Rssi event, "
+			"RSSI:-%d dBm, Freq:%d\n",
+			subsc_evt_cfg->bcn_h_rssi_cfg.abs_value,
+			subsc_evt_cfg->bcn_h_rssi_cfg.evt_freq);
+
+		pos += sizeof(struct mwifiex_ie_types_rssi_threshold);
+		le16_add_cpu(&cmd->size,
+			     sizeof(struct mwifiex_ie_types_rssi_threshold));
+	}
+
+	return 0;
+}
+
+/*
  * This function prepares the commands before sending them to the firmware.
  *
  * This is a generic function which calls specific command preparation
@@ -1086,6 +1197,9 @@ int mwifiex_sta_prepare_cmd(struct mwifiex_private *priv, uint16_t cmd_no,
 	case HostCmd_CMD_PCIE_DESC_DETAILS:
 		ret = mwifiex_cmd_pcie_host_spec(priv, cmd_ptr, cmd_action);
 		break;
+	case HostCmd_CMD_802_11_SUBSCRIBE_EVENT:
+		ret = mwifiex_cmd_802_11_subsc_evt(priv, cmd_ptr, data_buf);
+		break;
 	default:
 		dev_err(priv->adapter->dev,
 			"PREP_CMD: unknown cmd- %#x\n", cmd_no);
@@ -1150,13 +1264,15 @@ int mwifiex_sta_init_cmd(struct mwifiex_private *priv, u8 first_sta)
 		if (ret)
 			return -1;
 
-		/* Enable IEEE PS by default */
-		priv->adapter->ps_mode = MWIFIEX_802_11_POWER_MODE_PSP;
-		ret = mwifiex_send_cmd_async(priv,
-					     HostCmd_CMD_802_11_PS_MODE_ENH,
-					     EN_AUTO_PS, BITMAP_STA_PS, NULL);
-		if (ret)
-			return -1;
+		if (priv->bss_type != MWIFIEX_BSS_TYPE_UAP) {
+			/* Enable IEEE PS by default */
+			priv->adapter->ps_mode = MWIFIEX_802_11_POWER_MODE_PSP;
+			ret = mwifiex_send_cmd_async(
+					priv, HostCmd_CMD_802_11_PS_MODE_ENH,
+					EN_AUTO_PS, BITMAP_STA_PS, NULL);
+			if (ret)
+				return -1;
+		}
 	}
 
 	/* get tx rate */
@@ -1172,12 +1288,14 @@ int mwifiex_sta_init_cmd(struct mwifiex_private *priv, u8 first_sta)
 	if (ret)
 		return -1;
 
-	/* set ibss coalescing_status */
-	ret = mwifiex_send_cmd_async(priv,
-				     HostCmd_CMD_802_11_IBSS_COALESCING_STATUS,
-				     HostCmd_ACT_GEN_SET, 0, &enable);
-	if (ret)
-		return -1;
+	if (priv->bss_type == MWIFIEX_BSS_TYPE_STA) {
+		/* set ibss coalescing_status */
+		ret = mwifiex_send_cmd_async(
+				priv, HostCmd_CMD_802_11_IBSS_COALESCING_STATUS,
+				HostCmd_ACT_GEN_SET, 0, &enable);
+		if (ret)
+			return -1;
+	}
 
 	memset(&amsdu_aggr_ctrl, 0, sizeof(amsdu_aggr_ctrl));
 	amsdu_aggr_ctrl.enable = true;
@@ -1195,7 +1313,8 @@ int mwifiex_sta_init_cmd(struct mwifiex_private *priv, u8 first_sta)
 	if (ret)
 		return -1;
 
-	if (first_sta) {
+	if (first_sta && priv->adapter->iface_type != MWIFIEX_USB &&
+	    priv->bss_type != MWIFIEX_BSS_TYPE_UAP) {
 		/* Enable auto deep sleep */
 		auto_ds.auto_ds = DEEP_SLEEP_ON;
 		auto_ds.idle_time = DEEP_SLEEP_IDLE_TIME;
@@ -1207,12 +1326,16 @@ int mwifiex_sta_init_cmd(struct mwifiex_private *priv, u8 first_sta)
 			return -1;
 	}
 
-	/* Send cmd to FW to enable/disable 11D function */
-	state_11d = ENABLE_11D;
-	ret = mwifiex_send_cmd_async(priv, HostCmd_CMD_802_11_SNMP_MIB,
-				     HostCmd_ACT_GEN_SET, DOT11D_I, &state_11d);
-	if (ret)
-		dev_err(priv->adapter->dev, "11D: failed to enable 11D\n");
+	if (priv->bss_type != MWIFIEX_BSS_TYPE_UAP) {
+		/* Send cmd to FW to enable/disable 11D function */
+		state_11d = ENABLE_11D;
+		ret = mwifiex_send_cmd_async(priv, HostCmd_CMD_802_11_SNMP_MIB,
+					     HostCmd_ACT_GEN_SET, DOT11D_I,
+					     &state_11d);
+		if (ret)
+			dev_err(priv->adapter->dev,
+				"11D: failed to enable 11D\n");
+	}
 
 	/* Send cmd to FW to configure 11n specific configuration
 	 * (Short GI, Channel BW, Green field support etc.) for transmit
diff --git a/drivers/net/wireless/mwifiex/sta_cmdresp.c b/drivers/net/wireless/mwifiex/sta_cmdresp.c
index 4da19ed0f07..a79ed9bd969 100644
--- a/drivers/net/wireless/mwifiex/sta_cmdresp.c
+++ b/drivers/net/wireless/mwifiex/sta_cmdresp.c
@@ -119,11 +119,11 @@ mwifiex_process_cmdresp_error(struct mwifiex_private *priv,
  * calculated SNR values.
  */
 static int mwifiex_ret_802_11_rssi_info(struct mwifiex_private *priv,
-					struct host_cmd_ds_command *resp,
-					struct mwifiex_ds_get_signal *signal)
+					struct host_cmd_ds_command *resp)
 {
 	struct host_cmd_ds_802_11_rssi_info_rsp *rssi_info_rsp =
 						&resp->params.rssi_info_rsp;
+	struct mwifiex_ds_misc_subsc_evt subsc_evt;
 
 	priv->data_rssi_last = le16_to_cpu(rssi_info_rsp->data_rssi_last);
 	priv->data_nf_last = le16_to_cpu(rssi_info_rsp->data_nf_last);
@@ -137,34 +137,29 @@ static int mwifiex_ret_802_11_rssi_info(struct mwifiex_private *priv,
 	priv->bcn_rssi_avg = le16_to_cpu(rssi_info_rsp->bcn_rssi_avg);
 	priv->bcn_nf_avg = le16_to_cpu(rssi_info_rsp->bcn_nf_avg);
 
-	/* Need to indicate IOCTL complete */
-	if (signal) {
-		memset(signal, 0, sizeof(*signal));
-
-		signal->selector = ALL_RSSI_INFO_MASK;
-
-		/* RSSI */
-		signal->bcn_rssi_last = priv->bcn_rssi_last;
-		signal->bcn_rssi_avg = priv->bcn_rssi_avg;
-		signal->data_rssi_last = priv->data_rssi_last;
-		signal->data_rssi_avg = priv->data_rssi_avg;
-
-		/* SNR */
-		signal->bcn_snr_last =
-			CAL_SNR(priv->bcn_rssi_last, priv->bcn_nf_last);
-		signal->bcn_snr_avg =
-			CAL_SNR(priv->bcn_rssi_avg, priv->bcn_nf_avg);
-		signal->data_snr_last =
-			CAL_SNR(priv->data_rssi_last, priv->data_nf_last);
-		signal->data_snr_avg =
-			CAL_SNR(priv->data_rssi_avg, priv->data_nf_avg);
-
-		/* NF */
-		signal->bcn_nf_last = priv->bcn_nf_last;
-		signal->bcn_nf_avg = priv->bcn_nf_avg;
-		signal->data_nf_last = priv->data_nf_last;
-		signal->data_nf_avg = priv->data_nf_avg;
+	if (priv->subsc_evt_rssi_state == EVENT_HANDLED)
+		return 0;
+
+	/* Resubscribe low and high rssi events with new thresholds */
+	memset(&subsc_evt, 0x00, sizeof(struct mwifiex_ds_misc_subsc_evt));
+	subsc_evt.events = BITMASK_BCN_RSSI_LOW | BITMASK_BCN_RSSI_HIGH;
+	subsc_evt.action = HostCmd_ACT_BITWISE_SET;
+	if (priv->subsc_evt_rssi_state == RSSI_LOW_RECVD) {
+		subsc_evt.bcn_l_rssi_cfg.abs_value = abs(priv->bcn_rssi_avg -
+				priv->cqm_rssi_hyst);
+		subsc_evt.bcn_h_rssi_cfg.abs_value = abs(priv->cqm_rssi_thold);
+	} else if (priv->subsc_evt_rssi_state == RSSI_HIGH_RECVD) {
+		subsc_evt.bcn_l_rssi_cfg.abs_value = abs(priv->cqm_rssi_thold);
+		subsc_evt.bcn_h_rssi_cfg.abs_value = abs(priv->bcn_rssi_avg +
+				priv->cqm_rssi_hyst);
 	}
+	subsc_evt.bcn_l_rssi_cfg.evt_freq = 1;
+	subsc_evt.bcn_h_rssi_cfg.evt_freq = 1;
+
+	priv->subsc_evt_rssi_state = EVENT_HANDLED;
+
+	mwifiex_send_cmd_async(priv, HostCmd_CMD_802_11_SUBSCRIBE_EVENT,
+			       0, 0, &subsc_evt);
 
 	return 0;
 }
@@ -785,6 +780,28 @@ static int mwifiex_ret_ibss_coalescing_status(struct mwifiex_private *priv,
 }
 
 /*
+ * This function handles the command response for subscribe event command.
+ */
+static int mwifiex_ret_subsc_evt(struct mwifiex_private *priv,
+				 struct host_cmd_ds_command *resp,
+				 struct mwifiex_ds_misc_subsc_evt *sub_event)
+{
+	struct host_cmd_ds_802_11_subsc_evt *cmd_sub_event =
+		(struct host_cmd_ds_802_11_subsc_evt *)&resp->params.subsc_evt;
+
+	/* For every subscribe event command (Get/Set/Clear), FW reports the
+	 * current set of subscribed events*/
+	dev_dbg(priv->adapter->dev, "Bitmap of currently subscribed events: %16x\n",
+		le16_to_cpu(cmd_sub_event->events));
+
+	/*Return the subscribed event info for a Get request*/
+	if (sub_event)
+		sub_event->events = le16_to_cpu(cmd_sub_event->events);
+
+	return 0;
+}
+
+/*
  * This function handles the command responses.
  *
  * This is a generic function, which calls command specific
@@ -853,7 +870,7 @@ int mwifiex_process_sta_cmdresp(struct mwifiex_private *priv, u16 cmdresp_no,
 		ret = mwifiex_ret_get_log(priv, resp, data_buf);
 		break;
 	case HostCmd_CMD_RSSI_INFO:
-		ret = mwifiex_ret_802_11_rssi_info(priv, resp, data_buf);
+		ret = mwifiex_ret_802_11_rssi_info(priv, resp);
 		break;
 	case HostCmd_CMD_802_11_SNMP_MIB:
 		ret = mwifiex_ret_802_11_snmp_mib(priv, resp, data_buf);
@@ -924,6 +941,17 @@ int mwifiex_process_sta_cmdresp(struct mwifiex_private *priv, u16 cmdresp_no,
 		break;
 	case HostCmd_CMD_PCIE_DESC_DETAILS:
 		break;
+	case HostCmd_CMD_802_11_SUBSCRIBE_EVENT:
+		ret = mwifiex_ret_subsc_evt(priv, resp, data_buf);
+		break;
+	case HostCmd_CMD_UAP_SYS_CONFIG:
+		break;
+	case HostCmd_CMD_UAP_BSS_START:
+		priv->bss_started = 1;
+		break;
+	case HostCmd_CMD_UAP_BSS_STOP:
+		priv->bss_started = 0;
+		break;
 	default:
 		dev_err(adapter->dev, "CMD_RESP: unknown cmd response %#x\n",
 			resp->command);
diff --git a/drivers/net/wireless/mwifiex/sta_event.c b/drivers/net/wireless/mwifiex/sta_event.c
index cc531b536a5..4ace5a3dcd2 100644
--- a/drivers/net/wireless/mwifiex/sta_event.c
+++ b/drivers/net/wireless/mwifiex/sta_event.c
@@ -128,9 +128,6 @@ mwifiex_reset_connect_state(struct mwifiex_private *priv)
 		mwifiex_stop_net_dev_queue(priv->netdev, adapter);
 	if (netif_carrier_ok(priv->netdev))
 		netif_carrier_off(priv->netdev);
-	/* Reset wireless stats signal info */
-	priv->qual_level = 0;
-	priv->qual_noise = 0;
 }
 
 /*
@@ -187,8 +184,10 @@ mwifiex_reset_connect_state(struct mwifiex_private *priv)
 int mwifiex_process_sta_event(struct mwifiex_private *priv)
 {
 	struct mwifiex_adapter *adapter = priv->adapter;
-	int ret = 0;
+	int len, ret = 0;
 	u32 eventcause = adapter->event_cause;
+	struct station_info sinfo;
+	struct mwifiex_assoc_event *event;
 
 	switch (eventcause) {
 	case EVENT_DUMMY_HOST_WAKEUP_SIGNAL:
@@ -317,6 +316,12 @@ int mwifiex_process_sta_event(struct mwifiex_private *priv)
 		break;
 
 	case EVENT_RSSI_LOW:
+		cfg80211_cqm_rssi_notify(priv->netdev,
+					 NL80211_CQM_RSSI_THRESHOLD_EVENT_LOW,
+					 GFP_KERNEL);
+		mwifiex_send_cmd_async(priv, HostCmd_CMD_RSSI_INFO,
+				       HostCmd_ACT_GEN_GET, 0, NULL);
+		priv->subsc_evt_rssi_state = RSSI_LOW_RECVD;
 		dev_dbg(adapter->dev, "event: Beacon RSSI_LOW\n");
 		break;
 	case EVENT_SNR_LOW:
@@ -326,6 +331,12 @@ int mwifiex_process_sta_event(struct mwifiex_private *priv)
 		dev_dbg(adapter->dev, "event: MAX_FAIL\n");
 		break;
 	case EVENT_RSSI_HIGH:
+		cfg80211_cqm_rssi_notify(priv->netdev,
+					 NL80211_CQM_RSSI_THRESHOLD_EVENT_HIGH,
+					 GFP_KERNEL);
+		mwifiex_send_cmd_async(priv, HostCmd_CMD_RSSI_INFO,
+				       HostCmd_ACT_GEN_GET, 0, NULL);
+		priv->subsc_evt_rssi_state = RSSI_HIGH_RECVD;
 		dev_dbg(adapter->dev, "event: Beacon RSSI_HIGH\n");
 		break;
 	case EVENT_SNR_HIGH:
@@ -393,6 +404,53 @@ int mwifiex_process_sta_event(struct mwifiex_private *priv)
 	case EVENT_HOSTWAKE_STAIE:
 		dev_dbg(adapter->dev, "event: HOSTWAKE_STAIE %d\n", eventcause);
 		break;
+
+	case EVENT_UAP_STA_ASSOC:
+		skb_pull(adapter->event_skb, MWIFIEX_UAP_EVENT_EXTRA_HEADER);
+		memset(&sinfo, 0, sizeof(sinfo));
+		event = (struct mwifiex_assoc_event *)adapter->event_skb->data;
+		if (le16_to_cpu(event->type) == TLV_TYPE_UAP_MGMT_FRAME) {
+			len = -1;
+
+			if (ieee80211_is_assoc_req(event->frame_control))
+				len = 0;
+			else if (ieee80211_is_reassoc_req(event->frame_control))
+				/* There will be ETH_ALEN bytes of
+				 * current_ap_addr before the re-assoc ies.
+				 */
+				len = ETH_ALEN;
+
+			if (len != -1) {
+				sinfo.filled = STATION_INFO_ASSOC_REQ_IES;
+				sinfo.assoc_req_ies = (u8 *)&event->data[len];
+				len = (u8 *)sinfo.assoc_req_ies -
+				      (u8 *)&event->frame_control;
+				sinfo.assoc_req_ies_len =
+					le16_to_cpu(event->len) - (u16)len;
+			}
+		}
+		cfg80211_new_sta(priv->netdev, event->sta_addr, &sinfo,
+				 GFP_KERNEL);
+		break;
+	case EVENT_UAP_STA_DEAUTH:
+		skb_pull(adapter->event_skb, MWIFIEX_UAP_EVENT_EXTRA_HEADER);
+		cfg80211_del_sta(priv->netdev, adapter->event_skb->data,
+				 GFP_KERNEL);
+		break;
+	case EVENT_UAP_BSS_IDLE:
+		priv->media_connected = false;
+		break;
+	case EVENT_UAP_BSS_ACTIVE:
+		priv->media_connected = true;
+		break;
+	case EVENT_UAP_BSS_START:
+		dev_dbg(adapter->dev, "AP EVENT: event id: %#x\n", eventcause);
+		memcpy(priv->netdev->dev_addr, adapter->event_body+2, ETH_ALEN);
+		break;
+	case EVENT_UAP_MIC_COUNTERMEASURES:
+		/* For future development */
+		dev_dbg(adapter->dev, "AP EVENT: event id: %#x\n", eventcause);
+		break;
 	default:
 		dev_dbg(adapter->dev, "event: unknown event id: %#x\n",
 			eventcause);
diff --git a/drivers/net/wireless/mwifiex/sta_ioctl.c b/drivers/net/wireless/mwifiex/sta_ioctl.c
index d7b11defafe..106c449477b 100644
--- a/drivers/net/wireless/mwifiex/sta_ioctl.c
+++ b/drivers/net/wireless/mwifiex/sta_ioctl.c
@@ -155,20 +155,29 @@ int mwifiex_request_set_multicast_list(struct mwifiex_private *priv,
  * information.
  */
 int mwifiex_fill_new_bss_desc(struct mwifiex_private *priv,
-			      u8 *bssid, s32 rssi, u8 *ie_buf,
-			      size_t ie_len, u16 beacon_period,
-			      u16 cap_info_bitmap, u8 band,
+			      struct cfg80211_bss *bss,
 			      struct mwifiex_bssdescriptor *bss_desc)
 {
 	int ret;
+	u8 *beacon_ie;
+	struct mwifiex_bss_priv *bss_priv = (void *)bss->priv;
 
-	memcpy(bss_desc->mac_address, bssid, ETH_ALEN);
-	bss_desc->rssi = rssi;
-	bss_desc->beacon_buf = ie_buf;
-	bss_desc->beacon_buf_size = ie_len;
-	bss_desc->beacon_period = beacon_period;
-	bss_desc->cap_info_bitmap = cap_info_bitmap;
-	bss_desc->bss_band = band;
+	beacon_ie = kmemdup(bss->information_elements, bss->len_beacon_ies,
+			    GFP_KERNEL);
+	if (!beacon_ie) {
+		dev_err(priv->adapter->dev, " failed to alloc beacon_ie\n");
+		return -ENOMEM;
+	}
+
+	memcpy(bss_desc->mac_address, bss->bssid, ETH_ALEN);
+	bss_desc->rssi = bss->signal;
+	bss_desc->beacon_buf = beacon_ie;
+	bss_desc->beacon_buf_size = bss->len_beacon_ies;
+	bss_desc->beacon_period = bss->beacon_interval;
+	bss_desc->cap_info_bitmap = bss->capability;
+	bss_desc->bss_band = bss_priv->band;
+	bss_desc->fw_tsf = bss_priv->fw_tsf;
+	bss_desc->timestamp = bss->tsf;
 	if (bss_desc->cap_info_bitmap & WLAN_CAPABILITY_PRIVACY) {
 		dev_dbg(priv->adapter->dev, "info: InterpretIE: AP WEP enabled\n");
 		bss_desc->privacy = MWIFIEX_802_11_PRIV_FILTER_8021X_WEP;
@@ -180,9 +189,9 @@ 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,
-					      ie_buf, ie_len);
+	ret = mwifiex_update_bss_desc_with_ie(priv->adapter, bss_desc);
 
+	kfree(beacon_ie);
 	return ret;
 }
 
@@ -197,7 +206,6 @@ int mwifiex_bss_start(struct mwifiex_private *priv, struct cfg80211_bss *bss,
 	int ret;
 	struct mwifiex_adapter *adapter = priv->adapter;
 	struct mwifiex_bssdescriptor *bss_desc = NULL;
-	u8 *beacon_ie = NULL;
 
 	priv->scan_block = false;
 
@@ -210,19 +218,7 @@ int mwifiex_bss_start(struct mwifiex_private *priv, struct cfg80211_bss *bss,
 			return -ENOMEM;
 		}
 
-		beacon_ie = kmemdup(bss->information_elements,
-					bss->len_beacon_ies, GFP_KERNEL);
-		if (!beacon_ie) {
-			kfree(bss_desc);
-			dev_err(priv->adapter->dev, " failed to alloc beacon_ie\n");
-			return -ENOMEM;
-		}
-
-		ret = mwifiex_fill_new_bss_desc(priv, bss->bssid, bss->signal,
-						beacon_ie, bss->len_beacon_ies,
-						bss->beacon_interval,
-						bss->capability,
-						*(u8 *)bss->priv, bss_desc);
+		ret = mwifiex_fill_new_bss_desc(priv, bss, bss_desc);
 		if (ret)
 			goto done;
 	}
@@ -269,7 +265,6 @@ int mwifiex_bss_start(struct mwifiex_private *priv, struct cfg80211_bss *bss,
 		    (!mwifiex_ssid_cmp(&priv->curr_bss_params.bss_descriptor.
 				       ssid, &bss_desc->ssid))) {
 			kfree(bss_desc);
-			kfree(beacon_ie);
 			return 0;
 		}
 
@@ -304,7 +299,6 @@ int mwifiex_bss_start(struct mwifiex_private *priv, struct cfg80211_bss *bss,
 
 done:
 	kfree(bss_desc);
-	kfree(beacon_ie);
 	return ret;
 }
 
@@ -468,7 +462,8 @@ int mwifiex_get_bss_info(struct mwifiex_private *priv,
 
 	info->bss_chan = bss_desc->channel;
 
-	info->region_code = adapter->region_code;
+	memcpy(info->country_code, adapter->country_code,
+	       IEEE80211_COUNTRY_STRING_LEN);
 
 	info->media_connected = priv->media_connected;
 
@@ -996,6 +991,39 @@ static int mwifiex_set_wapi_ie(struct mwifiex_private *priv,
 }
 
 /*
+ * IOCTL request handler to set/reset WPS IE.
+ *
+ * The supplied WPS IE is treated as a opaque buffer. Only the first field
+ * is checked to internally enable WPS. If buffer length is zero, the existing
+ * WPS IE is reset.
+ */
+static int mwifiex_set_wps_ie(struct mwifiex_private *priv,
+			       u8 *ie_data_ptr, u16 ie_len)
+{
+	if (ie_len) {
+		priv->wps_ie = kzalloc(MWIFIEX_MAX_VSIE_LEN, GFP_KERNEL);
+		if (!priv->wps_ie)
+			return -ENOMEM;
+		if (ie_len > sizeof(priv->wps_ie)) {
+			dev_dbg(priv->adapter->dev,
+				"info: failed to copy WPS IE, too big\n");
+			kfree(priv->wps_ie);
+			return -1;
+		}
+		memcpy(priv->wps_ie, ie_data_ptr, ie_len);
+		priv->wps_ie_len = ie_len;
+		dev_dbg(priv->adapter->dev, "cmd: Set wps_ie_len=%d IE=%#x\n",
+			priv->wps_ie_len, priv->wps_ie[0]);
+	} else {
+		kfree(priv->wps_ie);
+		priv->wps_ie_len = ie_len;
+		dev_dbg(priv->adapter->dev,
+			"info: Reset wps_ie_len=%d\n", priv->wps_ie_len);
+	}
+	return 0;
+}
+
+/*
  * IOCTL request handler to set WAPI key.
  *
  * This function prepares the correct firmware command and
@@ -1185,46 +1213,14 @@ mwifiex_drv_get_driver_version(struct mwifiex_adapter *adapter, char *version,
 }
 
 /*
- * Sends IOCTL request to get signal information.
- *
- * This function allocates the IOCTL request buffer, fills it
- * with requisite parameters and calls the IOCTL handler.
- */
-int mwifiex_get_signal_info(struct mwifiex_private *priv,
-			    struct mwifiex_ds_get_signal *signal)
-{
-	int status;
-
-	signal->selector = ALL_RSSI_INFO_MASK;
-
-	/* Signal info can be obtained only if connected */
-	if (!priv->media_connected) {
-		dev_dbg(priv->adapter->dev,
-			"info: Can not get signal in disconnected state\n");
-		return -1;
-	}
-
-	status = mwifiex_send_cmd_sync(priv, HostCmd_CMD_RSSI_INFO,
-				       HostCmd_ACT_GEN_GET, 0, signal);
-
-	if (!status) {
-		if (signal->selector & BCN_RSSI_AVG_MASK)
-			priv->qual_level = signal->bcn_rssi_avg;
-		if (signal->selector & BCN_NF_AVG_MASK)
-			priv->qual_noise = signal->bcn_nf_avg;
-	}
-
-	return status;
-}
-
-/*
  * Sends IOCTL request to set encoding parameters.
  *
  * This function allocates the IOCTL request buffer, fills it
  * with requisite parameters and calls the IOCTL handler.
  */
 int mwifiex_set_encode(struct mwifiex_private *priv, const u8 *key,
-			int key_len, u8 key_index, int disable)
+			int key_len, u8 key_index,
+			const u8 *mac_addr, int disable)
 {
 	struct mwifiex_ds_encrypt_key encrypt_key;
 
@@ -1234,8 +1230,12 @@ int mwifiex_set_encode(struct mwifiex_private *priv, const u8 *key,
 		encrypt_key.key_index = key_index;
 		if (key_len)
 			memcpy(encrypt_key.key_material, key, key_len);
+		if (mac_addr)
+			memcpy(encrypt_key.mac_addr, mac_addr, ETH_ALEN);
 	} else {
 		encrypt_key.key_disable = true;
+		if (mac_addr)
+			memcpy(encrypt_key.mac_addr, mac_addr, ETH_ALEN);
 	}
 
 	return mwifiex_sec_ioctl_encrypt_key(priv, &encrypt_key);
@@ -1441,6 +1441,7 @@ mwifiex_set_gen_ie_helper(struct mwifiex_private *priv, u8 *ie_data_ptr,
 			priv->wps.session_enable = true;
 			dev_dbg(priv->adapter->dev,
 				"info: WPS Session Enabled.\n");
+			ret = mwifiex_set_wps_ie(priv, ie_data_ptr, ie_len);
 		}
 
 		/* Append the passed data to the end of the
diff --git a/drivers/net/wireless/mwifiex/sta_rx.c b/drivers/net/wireless/mwifiex/sta_rx.c
index 750b695aca1..02ce3b77d3e 100644
--- a/drivers/net/wireless/mwifiex/sta_rx.c
+++ b/drivers/net/wireless/mwifiex/sta_rx.c
@@ -145,7 +145,12 @@ int mwifiex_process_sta_rx_packet(struct mwifiex_adapter *adapter,
 			" rx_pkt_offset=%d, rx_pkt_length=%d\n", skb->len,
 		       local_rx_pd->rx_pkt_offset, local_rx_pd->rx_pkt_length);
 		priv->stats.rx_dropped++;
-		dev_kfree_skb_any(skb);
+
+		if (adapter->if_ops.data_complete)
+			adapter->if_ops.data_complete(adapter, skb);
+		else
+			dev_kfree_skb_any(skb);
+
 		return ret;
 	}
 
@@ -196,8 +201,12 @@ int mwifiex_process_sta_rx_packet(struct mwifiex_adapter *adapter,
 					     (u8) local_rx_pd->rx_pkt_type,
 					     skb);
 
-	if (ret || (rx_pkt_type == PKT_TYPE_BAR))
-		dev_kfree_skb_any(skb);
+	if (ret || (rx_pkt_type == PKT_TYPE_BAR)) {
+		if (adapter->if_ops.data_complete)
+			adapter->if_ops.data_complete(adapter, skb);
+		else
+			dev_kfree_skb_any(skb);
+	}
 
 	if (ret)
 		priv->stats.rx_dropped++;
diff --git a/drivers/net/wireless/mwifiex/sta_tx.c b/drivers/net/wireless/mwifiex/sta_tx.c
index 7af534feb42..0a046d3a0c1 100644
--- a/drivers/net/wireless/mwifiex/sta_tx.c
+++ b/drivers/net/wireless/mwifiex/sta_tx.c
@@ -149,10 +149,14 @@ int mwifiex_send_null_packet(struct mwifiex_private *priv, u8 flags)
 	local_tx_pd->bss_num = priv->bss_num;
 	local_tx_pd->bss_type = priv->bss_type;
 
-	skb_push(skb, INTF_HEADER_LEN);
-
-	ret = adapter->if_ops.host_to_card(adapter, MWIFIEX_TYPE_DATA,
-					   skb, NULL);
+	if (adapter->iface_type == MWIFIEX_USB) {
+		ret = adapter->if_ops.host_to_card(adapter, MWIFIEX_USB_EP_DATA,
+						   skb, NULL);
+	} else {
+		skb_push(skb, INTF_HEADER_LEN);
+		ret = adapter->if_ops.host_to_card(adapter, MWIFIEX_TYPE_DATA,
+						   skb, NULL);
+	}
 	switch (ret) {
 	case -EBUSY:
 		adapter->data_sent = true;
diff --git a/drivers/net/wireless/mwifiex/txrx.c b/drivers/net/wireless/mwifiex/txrx.c
index d2af8cb9854..e2faec4db10 100644
--- a/drivers/net/wireless/mwifiex/txrx.c
+++ b/drivers/net/wireless/mwifiex/txrx.c
@@ -77,12 +77,23 @@ int mwifiex_process_tx(struct mwifiex_private *priv, struct sk_buff *skb,
 		if (GET_BSS_ROLE(priv) == MWIFIEX_BSS_ROLE_STA)
 			local_tx_pd =
 				(struct txpd *) (head_ptr + INTF_HEADER_LEN);
-
-		ret = adapter->if_ops.host_to_card(adapter, MWIFIEX_TYPE_DATA,
-						   skb, tx_param);
+		if (adapter->iface_type == MWIFIEX_USB) {
+			adapter->data_sent = true;
+			skb_pull(skb, INTF_HEADER_LEN);
+			ret = adapter->if_ops.host_to_card(adapter,
+							   MWIFIEX_USB_EP_DATA,
+							   skb, NULL);
+		} else {
+			ret = adapter->if_ops.host_to_card(adapter,
+							   MWIFIEX_TYPE_DATA,
+							   skb, tx_param);
+		}
 	}
 
 	switch (ret) {
+	case -ENOSR:
+		dev_err(adapter->dev, "data: -ENOSR is returned\n");
+		break;
 	case -EBUSY:
 		if ((GET_BSS_ROLE(priv) == MWIFIEX_BSS_ROLE_STA) &&
 		    (adapter->pps_uapsd_mode) && (adapter->tx_lock_flag)) {
@@ -135,6 +146,9 @@ int mwifiex_write_data_complete(struct mwifiex_adapter *adapter,
 	if (!priv)
 		goto done;
 
+	if (adapter->iface_type == MWIFIEX_USB)
+		adapter->data_sent = false;
+
 	mwifiex_set_trans_start(priv->netdev);
 	if (!status) {
 		priv->stats.tx_packets++;
@@ -162,4 +176,5 @@ done:
 
 	return 0;
 }
+EXPORT_SYMBOL_GPL(mwifiex_write_data_complete);
 
diff --git a/drivers/net/wireless/mwifiex/uap_cmd.c b/drivers/net/wireless/mwifiex/uap_cmd.c
new file mode 100644
index 00000000000..76dfbc42a73
--- /dev/null
+++ b/drivers/net/wireless/mwifiex/uap_cmd.c
@@ -0,0 +1,432 @@
+/*
+ * Marvell Wireless LAN device driver: AP specific command handling
+ *
+ * Copyright (C) 2012, 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"
+
+/* This function parses security related parameters from cfg80211_ap_settings
+ * and sets into FW understandable bss_config structure.
+ */
+int mwifiex_set_secure_params(struct mwifiex_private *priv,
+			      struct mwifiex_uap_bss_param *bss_config,
+			      struct cfg80211_ap_settings *params) {
+	int i;
+
+	switch (params->auth_type) {
+	case NL80211_AUTHTYPE_OPEN_SYSTEM:
+		bss_config->auth_mode = WLAN_AUTH_OPEN;
+		break;
+	case NL80211_AUTHTYPE_SHARED_KEY:
+		bss_config->auth_mode = WLAN_AUTH_SHARED_KEY;
+		break;
+	case NL80211_AUTHTYPE_NETWORK_EAP:
+		bss_config->auth_mode = WLAN_AUTH_LEAP;
+		break;
+	default:
+		bss_config->auth_mode = MWIFIEX_AUTH_MODE_AUTO;
+		break;
+	}
+
+	bss_config->key_mgmt_operation |= KEY_MGMT_ON_HOST;
+
+	for (i = 0; i < params->crypto.n_akm_suites; i++) {
+		switch (params->crypto.akm_suites[i]) {
+		case WLAN_AKM_SUITE_8021X:
+			if (params->crypto.wpa_versions &
+			    NL80211_WPA_VERSION_1) {
+				bss_config->protocol = PROTOCOL_WPA;
+				bss_config->key_mgmt = KEY_MGMT_EAP;
+			}
+			if (params->crypto.wpa_versions &
+			    NL80211_WPA_VERSION_2) {
+				bss_config->protocol = PROTOCOL_WPA2;
+				bss_config->key_mgmt = KEY_MGMT_EAP;
+			}
+			break;
+		case WLAN_AKM_SUITE_PSK:
+			if (params->crypto.wpa_versions &
+			    NL80211_WPA_VERSION_1) {
+				bss_config->protocol = PROTOCOL_WPA;
+				bss_config->key_mgmt = KEY_MGMT_PSK;
+			}
+			if (params->crypto.wpa_versions &
+			    NL80211_WPA_VERSION_2) {
+				bss_config->protocol = PROTOCOL_WPA2;
+				bss_config->key_mgmt = KEY_MGMT_PSK;
+			}
+			break;
+		default:
+			break;
+		}
+	}
+	for (i = 0; i < params->crypto.n_ciphers_pairwise; i++) {
+		switch (params->crypto.ciphers_pairwise[i]) {
+		case WLAN_CIPHER_SUITE_WEP40:
+		case WLAN_CIPHER_SUITE_WEP104:
+			break;
+		case WLAN_CIPHER_SUITE_TKIP:
+			bss_config->wpa_cfg.pairwise_cipher_wpa = CIPHER_TKIP;
+			break;
+		case WLAN_CIPHER_SUITE_CCMP:
+			bss_config->wpa_cfg.pairwise_cipher_wpa2 =
+								CIPHER_AES_CCMP;
+		default:
+			break;
+		}
+	}
+
+	switch (params->crypto.cipher_group) {
+	case WLAN_CIPHER_SUITE_WEP40:
+	case WLAN_CIPHER_SUITE_WEP104:
+		break;
+	case WLAN_CIPHER_SUITE_TKIP:
+		bss_config->wpa_cfg.group_cipher = CIPHER_TKIP;
+		break;
+	case WLAN_CIPHER_SUITE_CCMP:
+		bss_config->wpa_cfg.group_cipher = CIPHER_AES_CCMP;
+		break;
+	default:
+		break;
+	}
+
+	return 0;
+}
+
+/* This function initializes some of mwifiex_uap_bss_param variables.
+ * This helps FW in ignoring invalid values. These values may or may not
+ * be get updated to valid ones at later stage.
+ */
+void mwifiex_set_sys_config_invalid_data(struct mwifiex_uap_bss_param *config)
+{
+	config->bcast_ssid_ctl = 0x7F;
+	config->radio_ctl = 0x7F;
+	config->dtim_period = 0x7F;
+	config->beacon_period = 0x7FFF;
+	config->auth_mode = 0x7F;
+	config->rts_threshold = 0x7FFF;
+	config->frag_threshold = 0x7FFF;
+	config->retry_limit = 0x7F;
+}
+
+/* This function parses BSS related parameters from structure
+ * and prepares TLVs. These TLVs are appended to command buffer.
+*/
+static int
+mwifiex_uap_bss_param_prepare(u8 *tlv, void *cmd_buf, u16 *param_size)
+{
+	struct host_cmd_tlv_dtim_period *dtim_period;
+	struct host_cmd_tlv_beacon_period *beacon_period;
+	struct host_cmd_tlv_ssid *ssid;
+	struct host_cmd_tlv_channel_band *chan_band;
+	struct host_cmd_tlv_frag_threshold *frag_threshold;
+	struct host_cmd_tlv_rts_threshold *rts_threshold;
+	struct host_cmd_tlv_retry_limit *retry_limit;
+	struct host_cmd_tlv_pwk_cipher *pwk_cipher;
+	struct host_cmd_tlv_gwk_cipher *gwk_cipher;
+	struct host_cmd_tlv_encrypt_protocol *encrypt_protocol;
+	struct host_cmd_tlv_auth_type *auth_type;
+	struct host_cmd_tlv_passphrase *passphrase;
+	struct host_cmd_tlv_akmp *tlv_akmp;
+	struct mwifiex_uap_bss_param *bss_cfg = cmd_buf;
+	u16 cmd_size = *param_size;
+
+	if (bss_cfg->ssid.ssid_len) {
+		ssid = (struct host_cmd_tlv_ssid *)tlv;
+		ssid->tlv.type = cpu_to_le16(TLV_TYPE_UAP_SSID);
+		ssid->tlv.len = cpu_to_le16((u16)bss_cfg->ssid.ssid_len);
+		memcpy(ssid->ssid, bss_cfg->ssid.ssid, bss_cfg->ssid.ssid_len);
+		cmd_size += sizeof(struct host_cmd_tlv) +
+			    bss_cfg->ssid.ssid_len;
+		tlv += sizeof(struct host_cmd_tlv) + bss_cfg->ssid.ssid_len;
+	}
+	if (bss_cfg->channel && bss_cfg->channel <= MAX_CHANNEL_BAND_BG) {
+		chan_band = (struct host_cmd_tlv_channel_band *)tlv;
+		chan_band->tlv.type = cpu_to_le16(TLV_TYPE_CHANNELBANDLIST);
+		chan_band->tlv.len =
+			cpu_to_le16(sizeof(struct host_cmd_tlv_channel_band) -
+				    sizeof(struct host_cmd_tlv));
+		chan_band->band_config = bss_cfg->band_cfg;
+		chan_band->channel = bss_cfg->channel;
+		cmd_size += sizeof(struct host_cmd_tlv_channel_band);
+		tlv += sizeof(struct host_cmd_tlv_channel_band);
+	}
+	if (bss_cfg->beacon_period >= MIN_BEACON_PERIOD &&
+	    bss_cfg->beacon_period <= MAX_BEACON_PERIOD) {
+		beacon_period = (struct host_cmd_tlv_beacon_period *)tlv;
+		beacon_period->tlv.type =
+					cpu_to_le16(TLV_TYPE_UAP_BEACON_PERIOD);
+		beacon_period->tlv.len =
+			cpu_to_le16(sizeof(struct host_cmd_tlv_beacon_period) -
+				    sizeof(struct host_cmd_tlv));
+		beacon_period->period = cpu_to_le16(bss_cfg->beacon_period);
+		cmd_size += sizeof(struct host_cmd_tlv_beacon_period);
+		tlv += sizeof(struct host_cmd_tlv_beacon_period);
+	}
+	if (bss_cfg->dtim_period >= MIN_DTIM_PERIOD &&
+	    bss_cfg->dtim_period <= MAX_DTIM_PERIOD) {
+		dtim_period = (struct host_cmd_tlv_dtim_period *)tlv;
+		dtim_period->tlv.type = cpu_to_le16(TLV_TYPE_UAP_DTIM_PERIOD);
+		dtim_period->tlv.len =
+			cpu_to_le16(sizeof(struct host_cmd_tlv_dtim_period) -
+				    sizeof(struct host_cmd_tlv));
+		dtim_period->period = bss_cfg->dtim_period;
+		cmd_size += sizeof(struct host_cmd_tlv_dtim_period);
+		tlv += sizeof(struct host_cmd_tlv_dtim_period);
+	}
+	if (bss_cfg->rts_threshold <= MWIFIEX_RTS_MAX_VALUE) {
+		rts_threshold = (struct host_cmd_tlv_rts_threshold *)tlv;
+		rts_threshold->tlv.type =
+					cpu_to_le16(TLV_TYPE_UAP_RTS_THRESHOLD);
+		rts_threshold->tlv.len =
+			cpu_to_le16(sizeof(struct host_cmd_tlv_rts_threshold) -
+				    sizeof(struct host_cmd_tlv));
+		rts_threshold->rts_thr = cpu_to_le16(bss_cfg->rts_threshold);
+		cmd_size += sizeof(struct host_cmd_tlv_frag_threshold);
+		tlv += sizeof(struct host_cmd_tlv_frag_threshold);
+	}
+	if ((bss_cfg->frag_threshold >= MWIFIEX_FRAG_MIN_VALUE) &&
+	    (bss_cfg->frag_threshold <= MWIFIEX_FRAG_MAX_VALUE)) {
+		frag_threshold = (struct host_cmd_tlv_frag_threshold *)tlv;
+		frag_threshold->tlv.type =
+				cpu_to_le16(TLV_TYPE_UAP_FRAG_THRESHOLD);
+		frag_threshold->tlv.len =
+			cpu_to_le16(sizeof(struct host_cmd_tlv_frag_threshold) -
+				    sizeof(struct host_cmd_tlv));
+		frag_threshold->frag_thr = cpu_to_le16(bss_cfg->frag_threshold);
+		cmd_size += sizeof(struct host_cmd_tlv_frag_threshold);
+		tlv += sizeof(struct host_cmd_tlv_frag_threshold);
+	}
+	if (bss_cfg->retry_limit <= MWIFIEX_RETRY_LIMIT) {
+		retry_limit = (struct host_cmd_tlv_retry_limit *)tlv;
+		retry_limit->tlv.type = cpu_to_le16(TLV_TYPE_UAP_RETRY_LIMIT);
+		retry_limit->tlv.len =
+			cpu_to_le16(sizeof(struct host_cmd_tlv_retry_limit) -
+				    sizeof(struct host_cmd_tlv));
+		retry_limit->limit = (u8)bss_cfg->retry_limit;
+		cmd_size += sizeof(struct host_cmd_tlv_retry_limit);
+		tlv += sizeof(struct host_cmd_tlv_retry_limit);
+	}
+	if ((bss_cfg->protocol & PROTOCOL_WPA) ||
+	    (bss_cfg->protocol & PROTOCOL_WPA2) ||
+	    (bss_cfg->protocol & PROTOCOL_EAP)) {
+		tlv_akmp = (struct host_cmd_tlv_akmp *)tlv;
+		tlv_akmp->tlv.type = cpu_to_le16(TLV_TYPE_UAP_AKMP);
+		tlv_akmp->tlv.len =
+		    cpu_to_le16(sizeof(struct host_cmd_tlv_akmp) -
+				sizeof(struct host_cmd_tlv));
+		tlv_akmp->key_mgmt_operation =
+			cpu_to_le16(bss_cfg->key_mgmt_operation);
+		tlv_akmp->key_mgmt = cpu_to_le16(bss_cfg->key_mgmt);
+		cmd_size += sizeof(struct host_cmd_tlv_akmp);
+		tlv += sizeof(struct host_cmd_tlv_akmp);
+
+		if (bss_cfg->wpa_cfg.pairwise_cipher_wpa &
+				VALID_CIPHER_BITMAP) {
+			pwk_cipher = (struct host_cmd_tlv_pwk_cipher *)tlv;
+			pwk_cipher->tlv.type =
+				cpu_to_le16(TLV_TYPE_PWK_CIPHER);
+			pwk_cipher->tlv.len = cpu_to_le16(
+				sizeof(struct host_cmd_tlv_pwk_cipher) -
+				sizeof(struct host_cmd_tlv));
+			pwk_cipher->proto = cpu_to_le16(PROTOCOL_WPA);
+			pwk_cipher->cipher =
+				bss_cfg->wpa_cfg.pairwise_cipher_wpa;
+			cmd_size += sizeof(struct host_cmd_tlv_pwk_cipher);
+			tlv += sizeof(struct host_cmd_tlv_pwk_cipher);
+		}
+		if (bss_cfg->wpa_cfg.pairwise_cipher_wpa2 &
+				VALID_CIPHER_BITMAP) {
+			pwk_cipher = (struct host_cmd_tlv_pwk_cipher *)tlv;
+			pwk_cipher->tlv.type = cpu_to_le16(TLV_TYPE_PWK_CIPHER);
+			pwk_cipher->tlv.len = cpu_to_le16(
+				sizeof(struct host_cmd_tlv_pwk_cipher) -
+				sizeof(struct host_cmd_tlv));
+			pwk_cipher->proto = cpu_to_le16(PROTOCOL_WPA2);
+			pwk_cipher->cipher =
+				bss_cfg->wpa_cfg.pairwise_cipher_wpa2;
+			cmd_size += sizeof(struct host_cmd_tlv_pwk_cipher);
+			tlv += sizeof(struct host_cmd_tlv_pwk_cipher);
+		}
+		if (bss_cfg->wpa_cfg.group_cipher & VALID_CIPHER_BITMAP) {
+			gwk_cipher = (struct host_cmd_tlv_gwk_cipher *)tlv;
+			gwk_cipher->tlv.type = cpu_to_le16(TLV_TYPE_GWK_CIPHER);
+			gwk_cipher->tlv.len = cpu_to_le16(
+				sizeof(struct host_cmd_tlv_gwk_cipher) -
+				sizeof(struct host_cmd_tlv));
+			gwk_cipher->cipher = bss_cfg->wpa_cfg.group_cipher;
+			cmd_size += sizeof(struct host_cmd_tlv_gwk_cipher);
+			tlv += sizeof(struct host_cmd_tlv_gwk_cipher);
+		}
+		if (bss_cfg->wpa_cfg.length) {
+			passphrase = (struct host_cmd_tlv_passphrase *)tlv;
+			passphrase->tlv.type =
+				cpu_to_le16(TLV_TYPE_UAP_WPA_PASSPHRASE);
+			passphrase->tlv.len =
+				cpu_to_le16(bss_cfg->wpa_cfg.length);
+			memcpy(passphrase->passphrase,
+			       bss_cfg->wpa_cfg.passphrase,
+			       bss_cfg->wpa_cfg.length);
+			cmd_size += sizeof(struct host_cmd_tlv) +
+				    bss_cfg->wpa_cfg.length;
+			tlv += sizeof(struct host_cmd_tlv) +
+			       bss_cfg->wpa_cfg.length;
+		}
+	}
+	if ((bss_cfg->auth_mode <= WLAN_AUTH_SHARED_KEY) ||
+	    (bss_cfg->auth_mode == MWIFIEX_AUTH_MODE_AUTO)) {
+		auth_type = (struct host_cmd_tlv_auth_type *)tlv;
+		auth_type->tlv.type = cpu_to_le16(TLV_TYPE_AUTH_TYPE);
+		auth_type->tlv.len =
+			cpu_to_le16(sizeof(struct host_cmd_tlv_auth_type) -
+			sizeof(struct host_cmd_tlv));
+		auth_type->auth_type = (u8)bss_cfg->auth_mode;
+		cmd_size += sizeof(struct host_cmd_tlv_auth_type);
+		tlv += sizeof(struct host_cmd_tlv_auth_type);
+	}
+	if (bss_cfg->protocol) {
+		encrypt_protocol = (struct host_cmd_tlv_encrypt_protocol *)tlv;
+		encrypt_protocol->tlv.type =
+			cpu_to_le16(TLV_TYPE_UAP_ENCRY_PROTOCOL);
+		encrypt_protocol->tlv.len =
+			cpu_to_le16(sizeof(struct host_cmd_tlv_encrypt_protocol)
+			- sizeof(struct host_cmd_tlv));
+		encrypt_protocol->proto = cpu_to_le16(bss_cfg->protocol);
+		cmd_size += sizeof(struct host_cmd_tlv_encrypt_protocol);
+		tlv += sizeof(struct host_cmd_tlv_encrypt_protocol);
+	}
+
+	*param_size = cmd_size;
+
+	return 0;
+}
+
+/* This function parses custom IEs from IE list and prepares command buffer */
+static int mwifiex_uap_custom_ie_prepare(u8 *tlv, void *cmd_buf, u16 *ie_size)
+{
+	struct mwifiex_ie_list *ap_ie = cmd_buf;
+	struct host_cmd_tlv *tlv_ie = (struct host_cmd_tlv *)tlv;
+
+	if (!ap_ie || !ap_ie->len || !ap_ie->ie_list)
+		return -1;
+
+	*ie_size += le16_to_cpu(ap_ie->len) + sizeof(struct host_cmd_tlv);
+
+	tlv_ie->type = cpu_to_le16(TLV_TYPE_MGMT_IE);
+	tlv_ie->len = ap_ie->len;
+	tlv += sizeof(struct host_cmd_tlv);
+
+	memcpy(tlv, ap_ie->ie_list, le16_to_cpu(ap_ie->len));
+
+	return 0;
+}
+
+/* Parse AP config structure and prepare TLV based command structure
+ * to be sent to FW for uAP configuration
+ */
+static int
+mwifiex_cmd_uap_sys_config(struct host_cmd_ds_command *cmd, u16 cmd_action,
+			   u32 type, void *cmd_buf)
+{
+	u8 *tlv;
+	u16 cmd_size, param_size, ie_size;
+	struct host_cmd_ds_sys_config *sys_cfg;
+
+	cmd->command = cpu_to_le16(HostCmd_CMD_UAP_SYS_CONFIG);
+	cmd_size = (u16)(sizeof(struct host_cmd_ds_sys_config) + S_DS_GEN);
+	sys_cfg = (struct host_cmd_ds_sys_config *)&cmd->params.uap_sys_config;
+	sys_cfg->action = cpu_to_le16(cmd_action);
+	tlv = sys_cfg->tlv;
+
+	switch (type) {
+	case UAP_BSS_PARAMS_I:
+		param_size = cmd_size;
+		if (mwifiex_uap_bss_param_prepare(tlv, cmd_buf, &param_size))
+			return -1;
+		cmd->size = cpu_to_le16(param_size);
+		break;
+	case UAP_CUSTOM_IE_I:
+		ie_size = cmd_size;
+		if (mwifiex_uap_custom_ie_prepare(tlv, cmd_buf, &ie_size))
+			return -1;
+		cmd->size = cpu_to_le16(ie_size);
+		break;
+	default:
+		return -1;
+	}
+
+	return 0;
+}
+
+/* This function prepares the AP specific commands before sending them
+ * to the firmware.
+ * This is a generic function which calls specific command preparation
+ * routines based upon the command number.
+ */
+int mwifiex_uap_prepare_cmd(struct mwifiex_private *priv, u16 cmd_no,
+			    u16 cmd_action, u32 type,
+			    void *data_buf, void *cmd_buf)
+{
+	struct host_cmd_ds_command *cmd = cmd_buf;
+
+	switch (cmd_no) {
+	case HostCmd_CMD_UAP_SYS_CONFIG:
+		if (mwifiex_cmd_uap_sys_config(cmd, cmd_action, type, data_buf))
+			return -1;
+		break;
+	case HostCmd_CMD_UAP_BSS_START:
+	case HostCmd_CMD_UAP_BSS_STOP:
+		cmd->command = cpu_to_le16(cmd_no);
+		cmd->size = cpu_to_le16(S_DS_GEN);
+		break;
+	default:
+		dev_err(priv->adapter->dev,
+			"PREP_CMD: unknown cmd %#x\n", cmd_no);
+		return -1;
+	}
+
+	return 0;
+}
+
+/* This function sets the RF channel for AP.
+ *
+ * This function populates channel information in AP config structure
+ * and sends command to configure channel information in AP.
+ */
+int mwifiex_uap_set_channel(struct mwifiex_private *priv, int channel)
+{
+	struct mwifiex_uap_bss_param *bss_cfg;
+	struct wiphy *wiphy = priv->wdev->wiphy;
+
+	bss_cfg = kzalloc(sizeof(struct mwifiex_uap_bss_param), GFP_KERNEL);
+	if (!bss_cfg)
+		return -ENOMEM;
+
+	bss_cfg->band_cfg = BAND_CONFIG_MANUAL;
+	bss_cfg->channel = channel;
+
+	if (mwifiex_send_cmd_async(priv, HostCmd_CMD_UAP_SYS_CONFIG,
+				   HostCmd_ACT_GEN_SET,
+				   UAP_BSS_PARAMS_I, bss_cfg)) {
+		wiphy_err(wiphy, "Failed to set the uAP channel\n");
+		kfree(bss_cfg);
+		return -1;
+	}
+
+	kfree(bss_cfg);
+	return 0;
+}
diff --git a/drivers/net/wireless/mwifiex/usb.c b/drivers/net/wireless/mwifiex/usb.c
new file mode 100644
index 00000000000..49ebf20c56e
--- /dev/null
+++ b/drivers/net/wireless/mwifiex/usb.c
@@ -0,0 +1,1052 @@
+/*
+ * Marvell Wireless LAN device driver: USB specific handling
+ *
+ * Copyright (C) 2012, 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"
+#include "usb.h"
+
+#define USB_VERSION	"1.0"
+
+static const char usbdriver_name[] = "usb8797";
+
+static u8 user_rmmod;
+static struct mwifiex_if_ops usb_ops;
+static struct semaphore add_remove_card_sem;
+
+static struct usb_device_id mwifiex_usb_table[] = {
+	{USB_DEVICE(USB8797_VID, USB8797_PID_1)},
+	{USB_DEVICE_AND_INTERFACE_INFO(USB8797_VID, USB8797_PID_2,
+				       USB_CLASS_VENDOR_SPEC,
+				       USB_SUBCLASS_VENDOR_SPEC, 0xff)},
+	{ }	/* Terminating entry */
+};
+
+MODULE_DEVICE_TABLE(usb, mwifiex_usb_table);
+
+static int mwifiex_usb_submit_rx_urb(struct urb_context *ctx, int size);
+
+/* This function handles received packet. Necessary action is taken based on
+ * cmd/event/data.
+ */
+static int mwifiex_usb_recv(struct mwifiex_adapter *adapter,
+			    struct sk_buff *skb, u8 ep)
+{
+	struct device *dev = adapter->dev;
+	u32 recv_type;
+	__le32 tmp;
+
+	if (adapter->hs_activated)
+		mwifiex_process_hs_config(adapter);
+
+	if (skb->len < INTF_HEADER_LEN) {
+		dev_err(dev, "%s: invalid skb->len\n", __func__);
+		return -1;
+	}
+
+	switch (ep) {
+	case MWIFIEX_USB_EP_CMD_EVENT:
+		dev_dbg(dev, "%s: EP_CMD_EVENT\n", __func__);
+		skb_copy_from_linear_data(skb, &tmp, INTF_HEADER_LEN);
+		recv_type = le32_to_cpu(tmp);
+		skb_pull(skb, INTF_HEADER_LEN);
+
+		switch (recv_type) {
+		case MWIFIEX_USB_TYPE_CMD:
+			if (skb->len > MWIFIEX_SIZE_OF_CMD_BUFFER) {
+				dev_err(dev, "CMD: skb->len too large\n");
+				return -1;
+			} else if (!adapter->curr_cmd) {
+				dev_dbg(dev, "CMD: no curr_cmd\n");
+				if (adapter->ps_state == PS_STATE_SLEEP_CFM) {
+					mwifiex_process_sleep_confirm_resp(
+							adapter, skb->data,
+							skb->len);
+					return 0;
+				}
+				return -1;
+			}
+
+			adapter->curr_cmd->resp_skb = skb;
+			adapter->cmd_resp_received = true;
+			break;
+		case MWIFIEX_USB_TYPE_EVENT:
+			if (skb->len < sizeof(u32)) {
+				dev_err(dev, "EVENT: skb->len too small\n");
+				return -1;
+			}
+			skb_copy_from_linear_data(skb, &tmp, sizeof(u32));
+			adapter->event_cause = le32_to_cpu(tmp);
+			skb_pull(skb, sizeof(u32));
+			dev_dbg(dev, "event_cause %#x\n", adapter->event_cause);
+
+			if (skb->len > MAX_EVENT_SIZE) {
+				dev_err(dev, "EVENT: event body too large\n");
+				return -1;
+			}
+
+			skb_copy_from_linear_data(skb, adapter->event_body,
+						  skb->len);
+			adapter->event_received = true;
+			adapter->event_skb = skb;
+			break;
+		default:
+			dev_err(dev, "unknown recv_type %#x\n", recv_type);
+			return -1;
+		}
+		break;
+	case MWIFIEX_USB_EP_DATA:
+		dev_dbg(dev, "%s: EP_DATA\n", __func__);
+		if (skb->len > MWIFIEX_RX_DATA_BUF_SIZE) {
+			dev_err(dev, "DATA: skb->len too large\n");
+			return -1;
+		}
+		skb_queue_tail(&adapter->usb_rx_data_q, skb);
+		adapter->data_received = true;
+		break;
+	default:
+		dev_err(dev, "%s: unknown endport %#x\n", __func__, ep);
+		return -1;
+	}
+
+	return -EINPROGRESS;
+}
+
+static void mwifiex_usb_rx_complete(struct urb *urb)
+{
+	struct urb_context *context = (struct urb_context *)urb->context;
+	struct mwifiex_adapter *adapter = context->adapter;
+	struct sk_buff *skb = context->skb;
+	struct usb_card_rec *card;
+	int recv_length = urb->actual_length;
+	int size, status;
+
+	if (!adapter || !adapter->card) {
+		pr_err("mwifiex adapter or card structure is not valid\n");
+		return;
+	}
+
+	card = (struct usb_card_rec *)adapter->card;
+	if (card->rx_cmd_ep == context->ep)
+		atomic_dec(&card->rx_cmd_urb_pending);
+	else
+		atomic_dec(&card->rx_data_urb_pending);
+
+	if (recv_length) {
+		if (urb->status || (adapter->surprise_removed)) {
+			dev_err(adapter->dev,
+				"URB status is failed: %d\n", urb->status);
+			/* Do not free skb in case of command ep */
+			if (card->rx_cmd_ep != context->ep)
+				dev_kfree_skb_any(skb);
+			goto setup_for_next;
+		}
+		if (skb->len > recv_length)
+			skb_trim(skb, recv_length);
+		else
+			skb_put(skb, recv_length - skb->len);
+
+		atomic_inc(&adapter->rx_pending);
+		status = mwifiex_usb_recv(adapter, skb, context->ep);
+
+		dev_dbg(adapter->dev, "info: recv_length=%d, status=%d\n",
+			recv_length, status);
+		if (status == -EINPROGRESS) {
+			queue_work(adapter->workqueue, &adapter->main_work);
+
+			/* urb for data_ep is re-submitted now;
+			 * urb for cmd_ep will be re-submitted in callback
+			 * mwifiex_usb_recv_complete
+			 */
+			if (card->rx_cmd_ep == context->ep)
+				return;
+		} else {
+			atomic_dec(&adapter->rx_pending);
+			if (status == -1)
+				dev_err(adapter->dev,
+					"received data processing failed!\n");
+
+			/* Do not free skb in case of command ep */
+			if (card->rx_cmd_ep != context->ep)
+				dev_kfree_skb_any(skb);
+		}
+	} else if (urb->status) {
+		if (!adapter->is_suspended) {
+			dev_warn(adapter->dev,
+				 "Card is removed: %d\n", urb->status);
+			adapter->surprise_removed = true;
+		}
+		dev_kfree_skb_any(skb);
+		return;
+	} else {
+		/* Do not free skb in case of command ep */
+		if (card->rx_cmd_ep != context->ep)
+			dev_kfree_skb_any(skb);
+
+		/* fall through setup_for_next */
+	}
+
+setup_for_next:
+	if (card->rx_cmd_ep == context->ep)
+		size = MWIFIEX_RX_CMD_BUF_SIZE;
+	else
+		size = MWIFIEX_RX_DATA_BUF_SIZE;
+
+	mwifiex_usb_submit_rx_urb(context, size);
+
+	return;
+}
+
+static void mwifiex_usb_tx_complete(struct urb *urb)
+{
+	struct urb_context *context = (struct urb_context *)(urb->context);
+	struct mwifiex_adapter *adapter = context->adapter;
+	struct usb_card_rec *card = adapter->card;
+
+	dev_dbg(adapter->dev, "%s: status: %d\n", __func__, urb->status);
+
+	if (context->ep == card->tx_cmd_ep) {
+		dev_dbg(adapter->dev, "%s: CMD\n", __func__);
+		atomic_dec(&card->tx_cmd_urb_pending);
+		adapter->cmd_sent = false;
+	} else {
+		dev_dbg(adapter->dev, "%s: DATA\n", __func__);
+		atomic_dec(&card->tx_data_urb_pending);
+		mwifiex_write_data_complete(adapter, context->skb,
+					    urb->status ? -1 : 0);
+	}
+
+	queue_work(adapter->workqueue, &adapter->main_work);
+
+	return;
+}
+
+static int mwifiex_usb_submit_rx_urb(struct urb_context *ctx, int size)
+{
+	struct mwifiex_adapter *adapter = ctx->adapter;
+	struct usb_card_rec *card = (struct usb_card_rec *)adapter->card;
+
+	if (card->rx_cmd_ep != ctx->ep) {
+		ctx->skb = dev_alloc_skb(size);
+		if (!ctx->skb) {
+			dev_err(adapter->dev,
+				"%s: dev_alloc_skb failed\n", __func__);
+			return -ENOMEM;
+		}
+	}
+
+	usb_fill_bulk_urb(ctx->urb, card->udev,
+			  usb_rcvbulkpipe(card->udev, ctx->ep), ctx->skb->data,
+			  size, mwifiex_usb_rx_complete, (void *)ctx);
+
+	if (card->rx_cmd_ep == ctx->ep)
+		atomic_inc(&card->rx_cmd_urb_pending);
+	else
+		atomic_inc(&card->rx_data_urb_pending);
+
+	if (usb_submit_urb(ctx->urb, GFP_ATOMIC)) {
+		dev_err(adapter->dev, "usb_submit_urb failed\n");
+		dev_kfree_skb_any(ctx->skb);
+		ctx->skb = NULL;
+
+		if (card->rx_cmd_ep == ctx->ep)
+			atomic_dec(&card->rx_cmd_urb_pending);
+		else
+			atomic_dec(&card->rx_data_urb_pending);
+
+		return -1;
+	}
+
+	return 0;
+}
+
+static void mwifiex_usb_free(struct usb_card_rec *card)
+{
+	int i;
+
+	if (atomic_read(&card->rx_cmd_urb_pending) && card->rx_cmd.urb)
+		usb_kill_urb(card->rx_cmd.urb);
+
+	usb_free_urb(card->rx_cmd.urb);
+	card->rx_cmd.urb = NULL;
+
+	if (atomic_read(&card->rx_data_urb_pending))
+		for (i = 0; i < MWIFIEX_RX_DATA_URB; i++)
+			if (card->rx_data_list[i].urb)
+				usb_kill_urb(card->rx_data_list[i].urb);
+
+	for (i = 0; i < MWIFIEX_RX_DATA_URB; i++) {
+		usb_free_urb(card->rx_data_list[i].urb);
+		card->rx_data_list[i].urb = NULL;
+	}
+
+	for (i = 0; i < MWIFIEX_TX_DATA_URB; i++) {
+		usb_free_urb(card->tx_data_list[i].urb);
+		card->tx_data_list[i].urb = NULL;
+	}
+
+	usb_free_urb(card->tx_cmd.urb);
+	card->tx_cmd.urb = NULL;
+
+	return;
+}
+
+/* This function probes an mwifiex device and registers it. It allocates
+ * the card structure, initiates the device registration and initialization
+ * procedure by adding a logical interface.
+ */
+static int mwifiex_usb_probe(struct usb_interface *intf,
+			     const struct usb_device_id *id)
+{
+	struct usb_device *udev = interface_to_usbdev(intf);
+	struct usb_host_interface *iface_desc = intf->cur_altsetting;
+	struct usb_endpoint_descriptor *epd;
+	int ret, i;
+	struct usb_card_rec *card;
+	u16 id_vendor, id_product, bcd_device, bcd_usb;
+
+	card = kzalloc(sizeof(struct usb_card_rec), GFP_KERNEL);
+	if (!card)
+		return -ENOMEM;
+
+	id_vendor = le16_to_cpu(udev->descriptor.idVendor);
+	id_product = le16_to_cpu(udev->descriptor.idProduct);
+	bcd_device = le16_to_cpu(udev->descriptor.bcdDevice);
+	bcd_usb = le16_to_cpu(udev->descriptor.bcdUSB);
+	pr_debug("info: VID/PID = %X/%X, Boot2 version = %X\n",
+		 id_vendor, id_product, bcd_device);
+
+	/* PID_1 is used for firmware downloading only */
+	if (id_product == USB8797_PID_1)
+		card->usb_boot_state = USB8797_FW_DNLD;
+	else
+		card->usb_boot_state = USB8797_FW_READY;
+
+	card->udev = udev;
+	card->intf = intf;
+
+	pr_debug("info: bcdUSB=%#x Device Class=%#x SubClass=%#x Protocl=%#x\n",
+		 udev->descriptor.bcdUSB, udev->descriptor.bDeviceClass,
+		 udev->descriptor.bDeviceSubClass,
+		 udev->descriptor.bDeviceProtocol);
+
+	for (i = 0; i < iface_desc->desc.bNumEndpoints; ++i) {
+		epd = &iface_desc->endpoint[i].desc;
+		if (usb_endpoint_dir_in(epd) &&
+		    usb_endpoint_num(epd) == MWIFIEX_USB_EP_CMD_EVENT &&
+		    usb_endpoint_xfer_bulk(epd)) {
+			pr_debug("info: bulk IN: max pkt size: %d, addr: %d\n",
+				 le16_to_cpu(epd->wMaxPacketSize),
+				 epd->bEndpointAddress);
+			card->rx_cmd_ep = usb_endpoint_num(epd);
+			atomic_set(&card->rx_cmd_urb_pending, 0);
+		}
+		if (usb_endpoint_dir_in(epd) &&
+		    usb_endpoint_num(epd) == MWIFIEX_USB_EP_DATA &&
+		    usb_endpoint_xfer_bulk(epd)) {
+			pr_debug("info: bulk IN: max pkt size: %d, addr: %d\n",
+				 le16_to_cpu(epd->wMaxPacketSize),
+				 epd->bEndpointAddress);
+			card->rx_data_ep = usb_endpoint_num(epd);
+			atomic_set(&card->rx_data_urb_pending, 0);
+		}
+		if (usb_endpoint_dir_out(epd) &&
+		    usb_endpoint_num(epd) == MWIFIEX_USB_EP_DATA &&
+		    usb_endpoint_xfer_bulk(epd)) {
+			pr_debug("info: bulk OUT: max pkt size: %d, addr: %d\n",
+				 le16_to_cpu(epd->wMaxPacketSize),
+				 epd->bEndpointAddress);
+			card->tx_data_ep = usb_endpoint_num(epd);
+			atomic_set(&card->tx_data_urb_pending, 0);
+		}
+		if (usb_endpoint_dir_out(epd) &&
+		    usb_endpoint_num(epd) == MWIFIEX_USB_EP_CMD_EVENT &&
+		    usb_endpoint_xfer_bulk(epd)) {
+			pr_debug("info: bulk OUT: max pkt size: %d, addr: %d\n",
+				 le16_to_cpu(epd->wMaxPacketSize),
+				 epd->bEndpointAddress);
+			card->tx_cmd_ep = usb_endpoint_num(epd);
+			atomic_set(&card->tx_cmd_urb_pending, 0);
+			card->bulk_out_maxpktsize =
+					le16_to_cpu(epd->wMaxPacketSize);
+		}
+	}
+
+	usb_set_intfdata(intf, card);
+
+	ret = mwifiex_add_card(card, &add_remove_card_sem, &usb_ops,
+			       MWIFIEX_USB);
+	if (ret) {
+		pr_err("%s: mwifiex_add_card failed: %d\n", __func__, ret);
+		usb_reset_device(udev);
+		kfree(card);
+		return ret;
+	}
+
+	usb_get_dev(udev);
+
+	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 suspended, this function allocates and sends a
+ * 'host sleep activate' request to the firmware and turns off the traffic.
+ */
+static int mwifiex_usb_suspend(struct usb_interface *intf, pm_message_t message)
+{
+	struct usb_card_rec *card = usb_get_intfdata(intf);
+	struct mwifiex_adapter *adapter;
+	int i;
+
+	if (!card || !card->adapter) {
+		pr_err("%s: card or card->adapter is NULL\n", __func__);
+		return 0;
+	}
+	adapter = card->adapter;
+
+	if (unlikely(adapter->is_suspended))
+		dev_warn(adapter->dev, "Device already suspended\n");
+
+	mwifiex_enable_hs(adapter);
+
+	/* 'is_suspended' flag indicates device is suspended.
+	 * It must be set here before the usb_kill_urb() calls. Reason
+	 * is in the complete handlers, urb->status(= -ENOENT) and
+	 * this flag is used in combination to distinguish between a
+	 * 'suspended' state and a 'disconnect' one.
+	 */
+	adapter->is_suspended = true;
+
+	for (i = 0; i < adapter->priv_num; i++)
+		netif_carrier_off(adapter->priv[i]->netdev);
+
+	if (atomic_read(&card->rx_cmd_urb_pending) && card->rx_cmd.urb)
+		usb_kill_urb(card->rx_cmd.urb);
+
+	if (atomic_read(&card->rx_data_urb_pending))
+		for (i = 0; i < MWIFIEX_RX_DATA_URB; i++)
+			if (card->rx_data_list[i].urb)
+				usb_kill_urb(card->rx_data_list[i].urb);
+
+	for (i = 0; i < MWIFIEX_TX_DATA_URB; i++)
+		if (card->tx_data_list[i].urb)
+			usb_kill_urb(card->tx_data_list[i].urb);
+
+	if (card->tx_cmd.urb)
+		usb_kill_urb(card->tx_cmd.urb);
+
+	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_usb_resume(struct usb_interface *intf)
+{
+	struct usb_card_rec *card = usb_get_intfdata(intf);
+	struct mwifiex_adapter *adapter;
+	int i;
+
+	if (!card || !card->adapter) {
+		pr_err("%s: card or card->adapter is NULL\n", __func__);
+		return 0;
+	}
+	adapter = card->adapter;
+
+	if (unlikely(!adapter->is_suspended)) {
+		dev_warn(adapter->dev, "Device already resumed\n");
+		return 0;
+	}
+
+	/* Indicate device resumed. The netdev queue will be resumed only
+	 * after the urbs have been re-submitted
+	 */
+	adapter->is_suspended = false;
+
+	if (!atomic_read(&card->rx_data_urb_pending))
+		for (i = 0; i < MWIFIEX_RX_DATA_URB; i++)
+			mwifiex_usb_submit_rx_urb(&card->rx_data_list[i],
+						  MWIFIEX_RX_DATA_BUF_SIZE);
+
+	if (!atomic_read(&card->rx_cmd_urb_pending)) {
+		card->rx_cmd.skb = dev_alloc_skb(MWIFIEX_RX_CMD_BUF_SIZE);
+		if (card->rx_cmd.skb)
+			mwifiex_usb_submit_rx_urb(&card->rx_cmd,
+						  MWIFIEX_RX_CMD_BUF_SIZE);
+	}
+
+	for (i = 0; i < adapter->priv_num; i++)
+		if (adapter->priv[i]->media_connected)
+			netif_carrier_on(adapter->priv[i]->netdev);
+
+	/* Disable Host Sleep */
+	if (adapter->hs_activated)
+		mwifiex_cancel_hs(mwifiex_get_priv(adapter,
+						   MWIFIEX_BSS_ROLE_ANY),
+				  MWIFIEX_ASYNC_CMD);
+
+#ifdef CONFIG_PM
+	/* Resume handler may be called due to remote wakeup,
+	 * force to exit suspend anyway
+	 */
+	usb_disable_autosuspend(card->udev);
+#endif /* CONFIG_PM */
+
+	return 0;
+}
+
+static void mwifiex_usb_disconnect(struct usb_interface *intf)
+{
+	struct usb_card_rec *card = usb_get_intfdata(intf);
+	struct mwifiex_adapter *adapter;
+	int i;
+
+	if (!card || !card->adapter) {
+		pr_err("%s: card or card->adapter is NULL\n", __func__);
+		return;
+	}
+
+	adapter = card->adapter;
+	if (!adapter->priv_num)
+		return;
+
+	/* In case driver is removed when asynchronous FW downloading is
+	 * in progress
+	 */
+	wait_for_completion(&adapter->fw_load);
+
+	if (user_rmmod) {
+#ifdef CONFIG_PM
+		if (adapter->is_suspended)
+			mwifiex_usb_resume(intf);
+#endif
+		for (i = 0; i < adapter->priv_num; i++)
+			if ((GET_BSS_ROLE(adapter->priv[i]) ==
+			     MWIFIEX_BSS_ROLE_STA) &&
+			    adapter->priv[i]->media_connected)
+				mwifiex_deauthenticate(adapter->priv[i], NULL);
+
+		mwifiex_init_shutdown_fw(mwifiex_get_priv(adapter,
+							  MWIFIEX_BSS_ROLE_ANY),
+					 MWIFIEX_FUNC_SHUTDOWN);
+	}
+
+	mwifiex_usb_free(card);
+
+	dev_dbg(adapter->dev, "%s: removing card\n", __func__);
+	mwifiex_remove_card(adapter, &add_remove_card_sem);
+
+	usb_set_intfdata(intf, NULL);
+	usb_put_dev(interface_to_usbdev(intf));
+	kfree(card);
+
+	return;
+}
+
+static struct usb_driver mwifiex_usb_driver = {
+	.name = usbdriver_name,
+	.probe = mwifiex_usb_probe,
+	.disconnect = mwifiex_usb_disconnect,
+	.id_table = mwifiex_usb_table,
+	.suspend = mwifiex_usb_suspend,
+	.resume = mwifiex_usb_resume,
+	.supports_autosuspend = 1,
+};
+
+static int mwifiex_usb_tx_init(struct mwifiex_adapter *adapter)
+{
+	struct usb_card_rec *card = (struct usb_card_rec *)adapter->card;
+	int i;
+
+	card->tx_cmd.adapter = adapter;
+	card->tx_cmd.ep = card->tx_cmd_ep;
+
+	card->tx_cmd.urb = usb_alloc_urb(0, GFP_KERNEL);
+	if (!card->tx_cmd.urb) {
+		dev_err(adapter->dev, "tx_cmd.urb allocation failed\n");
+		return -ENOMEM;
+	}
+
+	card->tx_data_ix = 0;
+
+	for (i = 0; i < MWIFIEX_TX_DATA_URB; i++) {
+		card->tx_data_list[i].adapter = adapter;
+		card->tx_data_list[i].ep = card->tx_data_ep;
+
+		card->tx_data_list[i].urb = usb_alloc_urb(0, GFP_KERNEL);
+		if (!card->tx_data_list[i].urb) {
+			dev_err(adapter->dev,
+				"tx_data_list[] urb allocation failed\n");
+			return -ENOMEM;
+		}
+	}
+
+	return 0;
+}
+
+static int mwifiex_usb_rx_init(struct mwifiex_adapter *adapter)
+{
+	struct usb_card_rec *card = (struct usb_card_rec *)adapter->card;
+	int i;
+
+	card->rx_cmd.adapter = adapter;
+	card->rx_cmd.ep = card->rx_cmd_ep;
+
+	card->rx_cmd.urb = usb_alloc_urb(0, GFP_KERNEL);
+	if (!card->rx_cmd.urb) {
+		dev_err(adapter->dev, "rx_cmd.urb allocation failed\n");
+		return -ENOMEM;
+	}
+
+	card->rx_cmd.skb = dev_alloc_skb(MWIFIEX_RX_CMD_BUF_SIZE);
+	if (!card->rx_cmd.skb) {
+		dev_err(adapter->dev, "rx_cmd.skb allocation failed\n");
+		return -ENOMEM;
+	}
+
+	if (mwifiex_usb_submit_rx_urb(&card->rx_cmd, MWIFIEX_RX_CMD_BUF_SIZE))
+		return -1;
+
+	for (i = 0; i < MWIFIEX_RX_DATA_URB; i++) {
+		card->rx_data_list[i].adapter = adapter;
+		card->rx_data_list[i].ep = card->rx_data_ep;
+
+		card->rx_data_list[i].urb = usb_alloc_urb(0, GFP_KERNEL);
+		if (!card->rx_data_list[i].urb) {
+			dev_err(adapter->dev,
+				"rx_data_list[] urb allocation failed\n");
+			return -1;
+		}
+		if (mwifiex_usb_submit_rx_urb(&card->rx_data_list[i],
+					      MWIFIEX_RX_DATA_BUF_SIZE))
+			return -1;
+	}
+
+	return 0;
+}
+
+static int mwifiex_write_data_sync(struct mwifiex_adapter *adapter, u8 *pbuf,
+				   u32 *len, u8 ep, u32 timeout)
+{
+	struct usb_card_rec *card = adapter->card;
+	int actual_length, ret;
+
+	if (!(*len % card->bulk_out_maxpktsize))
+		(*len)++;
+
+	/* Send the data block */
+	ret = usb_bulk_msg(card->udev, usb_sndbulkpipe(card->udev, ep), pbuf,
+			   *len, &actual_length, timeout);
+	if (ret) {
+		dev_err(adapter->dev, "usb_bulk_msg for tx failed: %d\n", ret);
+		ret = -1;
+	}
+
+	*len = actual_length;
+
+	return ret;
+}
+
+static int mwifiex_read_data_sync(struct mwifiex_adapter *adapter, u8 *pbuf,
+				  u32 *len, u8 ep, u32 timeout)
+{
+	struct usb_card_rec *card = adapter->card;
+	int actual_length, ret;
+
+	/* Receive the data response */
+	ret = usb_bulk_msg(card->udev, usb_rcvbulkpipe(card->udev, ep), pbuf,
+			   *len, &actual_length, timeout);
+	if (ret) {
+		dev_err(adapter->dev, "usb_bulk_msg for rx failed: %d\n", ret);
+		ret = -1;
+	}
+
+	*len = actual_length;
+
+	return ret;
+}
+
+/* This function write a command/data packet to card. */
+static int mwifiex_usb_host_to_card(struct mwifiex_adapter *adapter, u8 ep,
+				    struct sk_buff *skb,
+				    struct mwifiex_tx_param *tx_param)
+{
+	struct usb_card_rec *card = adapter->card;
+	struct urb_context *context;
+	u8 *data = (u8 *)skb->data;
+	struct urb *tx_urb;
+
+	if (adapter->is_suspended) {
+		dev_err(adapter->dev,
+			"%s: not allowed while suspended\n", __func__);
+		return -1;
+	}
+
+	if (adapter->surprise_removed) {
+		dev_err(adapter->dev, "%s: device removed\n", __func__);
+		return -1;
+	}
+
+	if (ep == card->tx_data_ep &&
+	    atomic_read(&card->tx_data_urb_pending) >= MWIFIEX_TX_DATA_URB) {
+		return -EBUSY;
+	}
+
+	dev_dbg(adapter->dev, "%s: ep=%d\n", __func__, ep);
+
+	if (ep == card->tx_cmd_ep) {
+		context = &card->tx_cmd;
+	} else {
+		if (card->tx_data_ix >= MWIFIEX_TX_DATA_URB)
+			card->tx_data_ix = 0;
+		context = &card->tx_data_list[card->tx_data_ix++];
+	}
+
+	context->adapter = adapter;
+	context->ep = ep;
+	context->skb = skb;
+	tx_urb = context->urb;
+
+	usb_fill_bulk_urb(tx_urb, card->udev, usb_sndbulkpipe(card->udev, ep),
+			  data, skb->len, mwifiex_usb_tx_complete,
+			  (void *)context);
+
+	tx_urb->transfer_flags |= URB_ZERO_PACKET;
+
+	if (ep == card->tx_cmd_ep)
+		atomic_inc(&card->tx_cmd_urb_pending);
+	else
+		atomic_inc(&card->tx_data_urb_pending);
+
+	if (usb_submit_urb(tx_urb, GFP_ATOMIC)) {
+		dev_err(adapter->dev, "%s: usb_submit_urb failed\n", __func__);
+		if (ep == card->tx_cmd_ep) {
+			atomic_dec(&card->tx_cmd_urb_pending);
+		} else {
+			atomic_dec(&card->tx_data_urb_pending);
+			if (card->tx_data_ix)
+				card->tx_data_ix--;
+			else
+				card->tx_data_ix = MWIFIEX_TX_DATA_URB;
+		}
+
+		return -1;
+	} else {
+		if (ep == card->tx_data_ep &&
+		    atomic_read(&card->tx_data_urb_pending) ==
+							MWIFIEX_TX_DATA_URB)
+			return -ENOSR;
+	}
+
+	return -EINPROGRESS;
+}
+
+/* This function register usb device and initialize parameter. */
+static int mwifiex_register_dev(struct mwifiex_adapter *adapter)
+{
+	struct usb_card_rec *card = (struct usb_card_rec *)adapter->card;
+
+	card->adapter = adapter;
+	adapter->dev = &card->udev->dev;
+	strcpy(adapter->fw_name, USB8797_DEFAULT_FW_NAME);
+
+	return 0;
+}
+
+/* This function reads one block of firmware data. */
+static int mwifiex_get_fw_data(struct mwifiex_adapter *adapter,
+			       u32 offset, u32 len, u8 *buf)
+{
+	if (!buf || !len)
+		return -1;
+
+	if (offset + len > adapter->firmware->size)
+		return -1;
+
+	memcpy(buf, adapter->firmware->data + offset, len);
+
+	return 0;
+}
+
+static int mwifiex_prog_fw_w_helper(struct mwifiex_adapter *adapter,
+				    struct mwifiex_fw_image *fw)
+{
+	int ret = 0;
+	u8 *firmware = fw->fw_buf, *recv_buff;
+	u32 retries = USB8797_FW_MAX_RETRY, dlen;
+	u32 fw_seqnum = 0, tlen = 0, dnld_cmd = 0;
+	struct fw_data *fwdata;
+	struct fw_sync_header sync_fw;
+	u8 check_winner = 1;
+
+	if (!firmware) {
+		dev_err(adapter->dev,
+			"No firmware image found! Terminating download\n");
+		ret = -1;
+		goto fw_exit;
+	}
+
+	/* Allocate memory for transmit */
+	fwdata = kzalloc(FW_DNLD_TX_BUF_SIZE, GFP_KERNEL);
+	if (!fwdata)
+		goto fw_exit;
+
+	/* Allocate memory for receive */
+	recv_buff = kzalloc(FW_DNLD_RX_BUF_SIZE, GFP_KERNEL);
+	if (!recv_buff)
+		goto cleanup;
+
+	do {
+		/* Send pseudo data to check winner status first */
+		if (check_winner) {
+			memset(&fwdata->fw_hdr, 0, sizeof(struct fw_header));
+			dlen = 0;
+		} else {
+			/* copy the header of the fw_data to get the length */
+			if (firmware)
+				memcpy(&fwdata->fw_hdr, &firmware[tlen],
+				       sizeof(struct fw_header));
+			else
+				mwifiex_get_fw_data(adapter, tlen,
+						    sizeof(struct fw_header),
+						    (u8 *)&fwdata->fw_hdr);
+
+			dlen = le32_to_cpu(fwdata->fw_hdr.data_len);
+			dnld_cmd = le32_to_cpu(fwdata->fw_hdr.dnld_cmd);
+			tlen += sizeof(struct fw_header);
+
+			if (firmware)
+				memcpy(fwdata->data, &firmware[tlen], dlen);
+			else
+				mwifiex_get_fw_data(adapter, tlen, dlen,
+						    (u8 *)fwdata->data);
+
+			fwdata->seq_num = cpu_to_le32(fw_seqnum);
+			tlen += dlen;
+		}
+
+		/* If the send/receive fails or CRC occurs then retry */
+		while (retries--) {
+			u8 *buf = (u8 *)fwdata;
+			u32 len = FW_DATA_XMIT_SIZE;
+
+			/* send the firmware block */
+			ret = mwifiex_write_data_sync(adapter, buf, &len,
+						MWIFIEX_USB_EP_CMD_EVENT,
+						MWIFIEX_USB_TIMEOUT);
+			if (ret) {
+				dev_err(adapter->dev,
+					"write_data_sync: failed: %d\n", ret);
+				continue;
+			}
+
+			buf = recv_buff;
+			len = FW_DNLD_RX_BUF_SIZE;
+
+			/* Receive the firmware block response */
+			ret = mwifiex_read_data_sync(adapter, buf, &len,
+						MWIFIEX_USB_EP_CMD_EVENT,
+						MWIFIEX_USB_TIMEOUT);
+			if (ret) {
+				dev_err(adapter->dev,
+					"read_data_sync: failed: %d\n", ret);
+				continue;
+			}
+
+			memcpy(&sync_fw, recv_buff,
+			       sizeof(struct fw_sync_header));
+
+			/* check 1st firmware block resp for highest bit set */
+			if (check_winner) {
+				if (le32_to_cpu(sync_fw.cmd) & 0x80000000) {
+					dev_warn(adapter->dev,
+						 "USB is not the winner %#x\n",
+						 sync_fw.cmd);
+
+					/* returning success */
+					ret = 0;
+					goto cleanup;
+				}
+
+				dev_dbg(adapter->dev,
+					"USB is the winner, start to download FW\n");
+
+				check_winner = 0;
+				break;
+			}
+
+			/* check the firmware block response for CRC errors */
+			if (sync_fw.cmd) {
+				dev_err(adapter->dev,
+					"FW received block with CRC %#x\n",
+					sync_fw.cmd);
+				ret = -1;
+				continue;
+			}
+
+			retries = USB8797_FW_MAX_RETRY;
+			break;
+		}
+		fw_seqnum++;
+	} while ((dnld_cmd != FW_HAS_LAST_BLOCK) && retries);
+
+cleanup:
+	dev_dbg(adapter->dev, "%s: %d bytes downloaded\n", __func__, tlen);
+
+	kfree(recv_buff);
+	kfree(fwdata);
+
+	if (retries)
+		ret = 0;
+fw_exit:
+	return ret;
+}
+
+static int mwifiex_usb_dnld_fw(struct mwifiex_adapter *adapter,
+			struct mwifiex_fw_image *fw)
+{
+	int ret;
+	struct usb_card_rec *card = (struct usb_card_rec *)adapter->card;
+
+	if (card->usb_boot_state == USB8797_FW_DNLD) {
+		ret = mwifiex_prog_fw_w_helper(adapter, fw);
+		if (ret)
+			return -1;
+
+		/* Boot state changes after successful firmware download */
+		if (card->usb_boot_state == USB8797_FW_DNLD)
+			return -1;
+	}
+
+	ret = mwifiex_usb_rx_init(adapter);
+	if (!ret)
+		ret = mwifiex_usb_tx_init(adapter);
+
+	return ret;
+}
+
+static void mwifiex_submit_rx_urb(struct mwifiex_adapter *adapter, u8 ep)
+{
+	struct usb_card_rec *card = (struct usb_card_rec *)adapter->card;
+
+	skb_push(card->rx_cmd.skb, INTF_HEADER_LEN);
+	if ((ep == card->rx_cmd_ep) &&
+	    (!atomic_read(&card->rx_cmd_urb_pending)))
+		mwifiex_usb_submit_rx_urb(&card->rx_cmd,
+					  MWIFIEX_RX_CMD_BUF_SIZE);
+
+	return;
+}
+
+static int mwifiex_usb_cmd_event_complete(struct mwifiex_adapter *adapter,
+				       struct sk_buff *skb)
+{
+	atomic_dec(&adapter->rx_pending);
+	mwifiex_submit_rx_urb(adapter, MWIFIEX_USB_EP_CMD_EVENT);
+
+	return 0;
+}
+
+static int mwifiex_usb_data_complete(struct mwifiex_adapter *adapter,
+				     struct sk_buff *skb)
+{
+	atomic_dec(&adapter->rx_pending);
+	dev_kfree_skb_any(skb);
+
+	return 0;
+}
+
+/* This function wakes up the card. */
+static int mwifiex_pm_wakeup_card(struct mwifiex_adapter *adapter)
+{
+	/* Simulation of HS_AWAKE event */
+	adapter->pm_wakeup_fw_try = false;
+	adapter->pm_wakeup_card_req = false;
+	adapter->ps_state = PS_STATE_AWAKE;
+
+	return 0;
+}
+
+static struct mwifiex_if_ops usb_ops = {
+	.register_dev =		mwifiex_register_dev,
+	.wakeup =		mwifiex_pm_wakeup_card,
+	.wakeup_complete =	mwifiex_pm_wakeup_card_complete,
+
+	/* USB specific */
+	.dnld_fw =		mwifiex_usb_dnld_fw,
+	.cmdrsp_complete =	mwifiex_usb_cmd_event_complete,
+	.event_complete =	mwifiex_usb_cmd_event_complete,
+	.data_complete =	mwifiex_usb_data_complete,
+	.host_to_card =		mwifiex_usb_host_to_card,
+};
+
+/* This function initializes the USB driver module.
+ *
+ * This initiates the semaphore and registers the device with
+ * USB bus.
+ */
+static int mwifiex_usb_init_module(void)
+{
+	int ret;
+
+	pr_debug("Marvell USB8797 Driver\n");
+
+	sema_init(&add_remove_card_sem, 1);
+
+	ret = usb_register(&mwifiex_usb_driver);
+	if (ret)
+		pr_err("Driver register failed!\n");
+	else
+		pr_debug("info: Driver registered successfully!\n");
+
+	return ret;
+}
+
+/* This function cleans up the USB driver.
+ *
+ * The following major steps are followed in .disconnect for cleanup:
+ *      - Resume the device if its suspended
+ *      - Disconnect the device if connected
+ *      - Shutdown the firmware
+ *      - Unregister the device from USB bus.
+ */
+static void mwifiex_usb_cleanup_module(void)
+{
+	if (!down_interruptible(&add_remove_card_sem))
+		up(&add_remove_card_sem);
+
+	/* set the flag as user is removing this module */
+	user_rmmod = 1;
+
+	usb_deregister(&mwifiex_usb_driver);
+}
+
+module_init(mwifiex_usb_init_module);
+module_exit(mwifiex_usb_cleanup_module);
+
+MODULE_AUTHOR("Marvell International Ltd.");
+MODULE_DESCRIPTION("Marvell WiFi-Ex USB Driver version" USB_VERSION);
+MODULE_VERSION(USB_VERSION);
+MODULE_LICENSE("GPL v2");
+MODULE_FIRMWARE("mrvl/usb8797_uapsta.bin");
diff --git a/drivers/net/wireless/mwifiex/usb.h b/drivers/net/wireless/mwifiex/usb.h
new file mode 100644
index 00000000000..98c4316cd1a
--- /dev/null
+++ b/drivers/net/wireless/mwifiex/usb.h
@@ -0,0 +1,99 @@
+/*
+ * This file contains definitions for mwifiex USB interface driver.
+ *
+ * Copyright (C) 2012, 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.
+ */
+
+#ifndef _MWIFIEX_USB_H
+#define _MWIFIEX_USB_H
+
+#include <linux/usb.h>
+
+#define USB8797_VID		0x1286
+#define USB8797_PID_1		0x2043
+#define USB8797_PID_2		0x2044
+
+#define USB8797_FW_DNLD		1
+#define USB8797_FW_READY	2
+#define USB8797_FW_MAX_RETRY	3
+
+#define MWIFIEX_TX_DATA_URB	6
+#define MWIFIEX_RX_DATA_URB	6
+#define MWIFIEX_USB_TIMEOUT	100
+
+#define USB8797_DEFAULT_FW_NAME	"mrvl/usb8797_uapsta.bin"
+
+#define FW_DNLD_TX_BUF_SIZE	620
+#define FW_DNLD_RX_BUF_SIZE	2048
+#define FW_HAS_LAST_BLOCK	0x00000004
+
+#define FW_DATA_XMIT_SIZE \
+	(sizeof(struct fw_header) + dlen + sizeof(u32))
+
+struct urb_context {
+	struct mwifiex_adapter *adapter;
+	struct sk_buff *skb;
+	struct urb *urb;
+	u8 ep;
+};
+
+struct usb_card_rec {
+	struct mwifiex_adapter *adapter;
+	struct usb_device *udev;
+	struct usb_interface *intf;
+	u8 rx_cmd_ep;
+	struct urb_context rx_cmd;
+	atomic_t rx_cmd_urb_pending;
+	struct urb_context rx_data_list[MWIFIEX_RX_DATA_URB];
+	u8 usb_boot_state;
+	u8 rx_data_ep;
+	atomic_t rx_data_urb_pending;
+	u8 tx_data_ep;
+	u8 tx_cmd_ep;
+	atomic_t tx_data_urb_pending;
+	atomic_t tx_cmd_urb_pending;
+	int bulk_out_maxpktsize;
+	struct urb_context tx_cmd;
+	int tx_data_ix;
+	struct urb_context tx_data_list[MWIFIEX_TX_DATA_URB];
+};
+
+struct fw_header {
+	__le32 dnld_cmd;
+	__le32 base_addr;
+	__le32 data_len;
+	__le32 crc;
+};
+
+struct fw_sync_header {
+	__le32 cmd;
+	__le32 seq_num;
+};
+
+struct fw_data {
+	struct fw_header fw_hdr;
+	__le32 seq_num;
+	u8 data[1];
+};
+
+/* This function is called after the card has woken up. */
+static inline int
+mwifiex_pm_wakeup_card_complete(struct mwifiex_adapter *adapter)
+{
+	return 0;
+}
+
+#endif /*_MWIFIEX_USB_H */
diff --git a/drivers/net/wireless/mwifiex/util.c b/drivers/net/wireless/mwifiex/util.c
index 6b399976d6c..2864c74bdb6 100644
--- a/drivers/net/wireless/mwifiex/util.c
+++ b/drivers/net/wireless/mwifiex/util.c
@@ -167,6 +167,28 @@ int mwifiex_recv_packet(struct mwifiex_adapter *adapter, struct sk_buff *skb)
 	skb->dev = priv->netdev;
 	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
+	 * 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.
+	 *
+	 * Modifying the truesize here as our allocation for each
+	 * skb is 4K but we only receive 2K packets and this cause
+	 * the kernel to start dropping packets in case where
+	 * application has allocated buffer based on 2K size i.e.
+	 * if there a 64K packet received (in IP fragments and
+	 * application allocates 64K to receive this packet but
+	 * this packet would almost double up because we allocate
+	 * each 1.5K fragment in 4K and pass it up. As soon as the
+	 * 64K limit hits kernel will start to drop rest of the
+	 * fragments. Currently we fail the Filesndl-ht.scr script
+	 * for UDP, hence this fix
+	 */
+	if ((adapter->iface_type == MWIFIEX_USB) &&
+	    (skb->truesize > MWIFIEX_RX_DATA_BUF_SIZE))
+		skb->truesize += (skb->len - MWIFIEX_RX_DATA_BUF_SIZE);
+
 	priv->stats.rx_bytes += skb->len;
 	priv->stats.rx_packets++;
 	if (in_interrupt())
diff --git a/drivers/net/wireless/mwifiex/wmm.c b/drivers/net/wireless/mwifiex/wmm.c
index 5a7316c6f12..f3fc6551585 100644
--- a/drivers/net/wireless/mwifiex/wmm.c
+++ b/drivers/net/wireless/mwifiex/wmm.c
@@ -885,6 +885,10 @@ mwifiex_wmm_get_highest_priolist_ptr(struct mwifiex_adapter *adapter,
 				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 =
@@ -1120,11 +1124,19 @@ mwifiex_send_processed_packet(struct mwifiex_private *priv,
 	tx_info = MWIFIEX_SKB_TXCB(skb);
 
 	spin_unlock_irqrestore(&priv->wmm.ra_list_spinlock, ra_list_flags);
-	tx_param.next_pkt_len =
-		((skb_next) ? skb_next->len +
-		 sizeof(struct txpd) : 0);
-	ret = adapter->if_ops.host_to_card(adapter, MWIFIEX_TYPE_DATA, skb,
-					   &tx_param);
+
+	if (adapter->iface_type == MWIFIEX_USB) {
+		adapter->data_sent = true;
+		ret = adapter->if_ops.host_to_card(adapter, MWIFIEX_USB_EP_DATA,
+						   skb, NULL);
+	} else {
+		tx_param.next_pkt_len =
+			((skb_next) ? skb_next->len +
+			 sizeof(struct txpd) : 0);
+		ret = adapter->if_ops.host_to_card(adapter, MWIFIEX_TYPE_DATA,
+						   skb, &tx_param);
+	}
+
 	switch (ret) {
 	case -EBUSY:
 		dev_dbg(adapter->dev, "data: -EBUSY is returned\n");
diff --git a/drivers/net/wireless/mwl8k.c b/drivers/net/wireless/mwl8k.c
index b48674b577e..cf7bdc66f82 100644
--- a/drivers/net/wireless/mwl8k.c
+++ b/drivers/net/wireless/mwl8k.c
@@ -1235,7 +1235,7 @@ mwl8k_capture_bssid(struct mwl8k_priv *priv, struct ieee80211_hdr *wh)
 {
 	return priv->capture_beacon &&
 		ieee80211_is_beacon(wh->frame_control) &&
-		!compare_ether_addr(wh->addr3, priv->capture_bssid);
+		ether_addr_equal(wh->addr3, priv->capture_bssid);
 }
 
 static inline void mwl8k_save_beacon(struct ieee80211_hw *hw,
@@ -5893,18 +5893,7 @@ static struct pci_driver mwl8k_driver = {
 	.shutdown	= __devexit_p(mwl8k_shutdown),
 };
 
-static int __init mwl8k_init(void)
-{
-	return pci_register_driver(&mwl8k_driver);
-}
-
-static void __exit mwl8k_exit(void)
-{
-	pci_unregister_driver(&mwl8k_driver);
-}
-
-module_init(mwl8k_init);
-module_exit(mwl8k_exit);
+module_pci_driver(mwl8k_driver);
 
 MODULE_DESCRIPTION(MWL8K_DESC);
 MODULE_VERSION(MWL8K_VERSION);
diff --git a/drivers/net/wireless/orinoco/fw.c b/drivers/net/wireless/orinoco/fw.c
index 4df8cf64b56..400a3521764 100644
--- a/drivers/net/wireless/orinoco/fw.c
+++ b/drivers/net/wireless/orinoco/fw.c
@@ -379,11 +379,8 @@ void orinoco_cache_fw(struct orinoco_private *priv, int ap)
 
 void orinoco_uncache_fw(struct orinoco_private *priv)
 {
-	if (priv->cached_pri_fw)
-		release_firmware(priv->cached_pri_fw);
-	if (priv->cached_fw)
-		release_firmware(priv->cached_fw);
-
+	release_firmware(priv->cached_pri_fw);
+	release_firmware(priv->cached_fw);
 	priv->cached_pri_fw = NULL;
 	priv->cached_fw = NULL;
 }
diff --git a/drivers/net/wireless/orinoco/orinoco_usb.c b/drivers/net/wireless/orinoco/orinoco_usb.c
index f634d4582bf..7f53cea2f20 100644
--- a/drivers/net/wireless/orinoco/orinoco_usb.c
+++ b/drivers/net/wireless/orinoco/orinoco_usb.c
@@ -1752,6 +1752,7 @@ static struct usb_driver orinoco_driver = {
 	.probe = ezusb_probe,
 	.disconnect = ezusb_disconnect,
 	.id_table = ezusb_table,
+	.disable_hub_initiated_lpm = 1,
 };
 
 module_usb_driver(orinoco_driver);
diff --git a/drivers/net/wireless/p54/main.c b/drivers/net/wireless/p54/main.c
index ee8af1f047c..7cffea795ad 100644
--- a/drivers/net/wireless/p54/main.c
+++ b/drivers/net/wireless/p54/main.c
@@ -796,11 +796,14 @@ int p54_register_common(struct ieee80211_hw *dev, struct device *pdev)
 		dev_err(pdev, "Cannot register device (%d).\n", err);
 		return err;
 	}
+	priv->registered = true;
 
 #ifdef CONFIG_P54_LEDS
 	err = p54_init_leds(priv);
-	if (err)
+	if (err) {
+		p54_unregister_common(dev);
 		return err;
+	}
 #endif /* CONFIG_P54_LEDS */
 
 	dev_info(pdev, "is registered as '%s'\n", wiphy_name(dev->wiphy));
@@ -840,7 +843,11 @@ void p54_unregister_common(struct ieee80211_hw *dev)
 	p54_unregister_leds(priv);
 #endif /* CONFIG_P54_LEDS */
 
-	ieee80211_unregister_hw(dev);
+	if (priv->registered) {
+		priv->registered = false;
+		ieee80211_unregister_hw(dev);
+	}
+
 	mutex_destroy(&priv->conf_mutex);
 	mutex_destroy(&priv->eeprom_mutex);
 }
diff --git a/drivers/net/wireless/p54/p54.h b/drivers/net/wireless/p54/p54.h
index 452fa3a64aa..40b401ed684 100644
--- a/drivers/net/wireless/p54/p54.h
+++ b/drivers/net/wireless/p54/p54.h
@@ -173,6 +173,7 @@ struct p54_common {
 	struct sk_buff_head tx_pending;
 	struct sk_buff_head tx_queue;
 	struct mutex conf_mutex;
+	bool registered;
 
 	/* memory management (as seen by the firmware) */
 	u32 rx_start;
diff --git a/drivers/net/wireless/p54/p54pci.c b/drivers/net/wireless/p54/p54pci.c
index 45df728183f..89318adc8c7 100644
--- a/drivers/net/wireless/p54/p54pci.c
+++ b/drivers/net/wireless/p54/p54pci.c
@@ -667,15 +667,4 @@ static struct pci_driver p54p_driver = {
 	.driver.pm	= P54P_PM_OPS,
 };
 
-static int __init p54p_init(void)
-{
-	return pci_register_driver(&p54p_driver);
-}
-
-static void __exit p54p_exit(void)
-{
-	pci_unregister_driver(&p54p_driver);
-}
-
-module_init(p54p_init);
-module_exit(p54p_exit);
+module_pci_driver(p54p_driver);
diff --git a/drivers/net/wireless/p54/p54usb.c b/drivers/net/wireless/p54/p54usb.c
index f4d28c39aac..7f207b6e955 100644
--- a/drivers/net/wireless/p54/p54usb.c
+++ b/drivers/net/wireless/p54/p54usb.c
@@ -117,21 +117,18 @@ static const struct {
 	u32 intf;
 	enum p54u_hw_type type;
 	const char *fw;
-	const char *fw_legacy;
 	char hw[20];
 } p54u_fwlist[__NUM_P54U_HWTYPES] = {
 	{
 		.type = P54U_NET2280,
 		.intf = FW_LM86,
 		.fw = "isl3886usb",
-		.fw_legacy = "isl3890usb",
 		.hw = "ISL3886 + net2280",
 	},
 	{
 		.type = P54U_3887,
 		.intf = FW_LM87,
 		.fw = "isl3887usb",
-		.fw_legacy = "isl3887usb_bare",
 		.hw = "ISL3887",
 	},
 };
@@ -208,6 +205,16 @@ static void p54u_free_urbs(struct ieee80211_hw *dev)
 	usb_kill_anchored_urbs(&priv->submitted);
 }
 
+static void p54u_stop(struct ieee80211_hw *dev)
+{
+	/*
+	 * TODO: figure out how to reliably stop the 3887 and net2280 so
+	 * the hardware is still usable next time we want to start it.
+	 * until then, we just stop listening to the hardware..
+	 */
+	p54u_free_urbs(dev);
+}
+
 static int p54u_init_urbs(struct ieee80211_hw *dev)
 {
 	struct p54u_priv *priv = dev->priv;
@@ -257,6 +264,16 @@ static int p54u_init_urbs(struct ieee80211_hw *dev)
 	return ret;
 }
 
+static int p54u_open(struct ieee80211_hw *dev)
+{
+	/*
+	 * TODO: Because we don't know how to reliably stop the 3887 and
+	 * the isl3886+net2280, other than brutally cut off all
+	 * communications. We have to reinitialize the urbs on every start.
+	 */
+	return p54u_init_urbs(dev);
+}
+
 static __le32 p54u_lm87_chksum(const __le32 *data, size_t length)
 {
 	u32 chk = 0;
@@ -836,70 +853,137 @@ fail:
 	return err;
 }
 
-static int p54u_load_firmware(struct ieee80211_hw *dev)
+static int p54_find_type(struct p54u_priv *priv)
 {
-	struct p54u_priv *priv = dev->priv;
-	int err, i;
-
-	BUILD_BUG_ON(ARRAY_SIZE(p54u_fwlist) != __NUM_P54U_HWTYPES);
+	int i;
 
 	for (i = 0; i < __NUM_P54U_HWTYPES; i++)
 		if (p54u_fwlist[i].type == priv->hw_type)
 			break;
-
 	if (i == __NUM_P54U_HWTYPES)
 		return -EOPNOTSUPP;
 
-	err = request_firmware(&priv->fw, p54u_fwlist[i].fw, &priv->udev->dev);
-	if (err) {
-		dev_err(&priv->udev->dev, "(p54usb) cannot load firmware %s "
-					  "(%d)!\n", p54u_fwlist[i].fw, err);
+	return i;
+}
 
-		err = request_firmware(&priv->fw, p54u_fwlist[i].fw_legacy,
-				       &priv->udev->dev);
-		if (err)
-			return err;
-	}
+static int p54u_start_ops(struct p54u_priv *priv)
+{
+	struct ieee80211_hw *dev = priv->common.hw;
+	int ret;
 
-	err = p54_parse_firmware(dev, priv->fw);
-	if (err)
-		goto out;
+	ret = p54_parse_firmware(dev, priv->fw);
+	if (ret)
+		goto err_out;
+
+	ret = p54_find_type(priv);
+	if (ret < 0)
+		goto err_out;
 
-	if (priv->common.fw_interface != p54u_fwlist[i].intf) {
+	if (priv->common.fw_interface != p54u_fwlist[ret].intf) {
 		dev_err(&priv->udev->dev, "wrong firmware, please get "
 			"a firmware for \"%s\" and try again.\n",
-			p54u_fwlist[i].hw);
-		err = -EINVAL;
+			p54u_fwlist[ret].hw);
+		ret = -ENODEV;
+		goto err_out;
 	}
 
-out:
-	if (err)
-		release_firmware(priv->fw);
+	ret = priv->upload_fw(dev);
+	if (ret)
+		goto err_out;
 
-	return err;
+	ret = p54u_open(dev);
+	if (ret)
+		goto err_out;
+
+	ret = p54_read_eeprom(dev);
+	if (ret)
+		goto err_stop;
+
+	p54u_stop(dev);
+
+	ret = p54_register_common(dev, &priv->udev->dev);
+	if (ret)
+		goto err_stop;
+
+	return 0;
+
+err_stop:
+	p54u_stop(dev);
+
+err_out:
+	/*
+	 * p54u_disconnect will do the rest of the
+	 * cleanup
+	 */
+	return ret;
 }
 
-static int p54u_open(struct ieee80211_hw *dev)
+static void p54u_load_firmware_cb(const struct firmware *firmware,
+				  void *context)
 {
-	struct p54u_priv *priv = dev->priv;
+	struct p54u_priv *priv = context;
+	struct usb_device *udev = priv->udev;
 	int err;
 
-	err = p54u_init_urbs(dev);
-	if (err) {
-		return err;
+	complete(&priv->fw_wait_load);
+	if (firmware) {
+		priv->fw = firmware;
+		err = p54u_start_ops(priv);
+	} else {
+		err = -ENOENT;
+		dev_err(&udev->dev, "Firmware not found.\n");
 	}
 
-	priv->common.open = p54u_init_urbs;
+	if (err) {
+		struct device *parent = priv->udev->dev.parent;
 
-	return 0;
+		dev_err(&udev->dev, "failed to initialize device (%d)\n", err);
+
+		if (parent)
+			device_lock(parent);
+
+		device_release_driver(&udev->dev);
+		/*
+		 * At this point p54u_disconnect has already freed
+		 * the "priv" context. Do not use it anymore!
+		 */
+		priv = NULL;
+
+		if (parent)
+			device_unlock(parent);
+	}
+
+	usb_put_dev(udev);
 }
 
-static void p54u_stop(struct ieee80211_hw *dev)
+static int p54u_load_firmware(struct ieee80211_hw *dev,
+			      struct usb_interface *intf)
 {
-	/* TODO: figure out how to reliably stop the 3887 and net2280 so
-	   the hardware is still usable next time we want to start it.
-	   until then, we just stop listening to the hardware.. */
-	p54u_free_urbs(dev);
+	struct usb_device *udev = interface_to_usbdev(intf);
+	struct p54u_priv *priv = dev->priv;
+	struct device *device = &udev->dev;
+	int err, i;
+
+	BUILD_BUG_ON(ARRAY_SIZE(p54u_fwlist) != __NUM_P54U_HWTYPES);
+
+	init_completion(&priv->fw_wait_load);
+	i = p54_find_type(priv);
+	if (i < 0)
+		return i;
+
+	dev_info(&priv->udev->dev, "Loading firmware file %s\n",
+	       p54u_fwlist[i].fw);
+
+	usb_get_dev(udev);
+	err = request_firmware_nowait(THIS_MODULE, 1, p54u_fwlist[i].fw,
+				      device, GFP_KERNEL, priv,
+				      p54u_load_firmware_cb);
+	if (err) {
+		dev_err(&priv->udev->dev, "(p54usb) cannot load firmware %s "
+					  "(%d)!\n", p54u_fwlist[i].fw, err);
+	}
+
+	return err;
 }
 
 static int __devinit p54u_probe(struct usb_interface *intf,
@@ -969,33 +1053,7 @@ static int __devinit p54u_probe(struct usb_interface *intf,
 		priv->common.tx = p54u_tx_net2280;
 		priv->upload_fw = p54u_upload_firmware_net2280;
 	}
-	err = p54u_load_firmware(dev);
-	if (err)
-		goto err_free_dev;
-
-	err = priv->upload_fw(dev);
-	if (err)
-		goto err_free_fw;
-
-	p54u_open(dev);
-	err = p54_read_eeprom(dev);
-	p54u_stop(dev);
-	if (err)
-		goto err_free_fw;
-
-	err = p54_register_common(dev, &udev->dev);
-	if (err)
-		goto err_free_fw;
-
-	return 0;
-
-err_free_fw:
-	release_firmware(priv->fw);
-
-err_free_dev:
-	p54_free_common(dev);
-	usb_set_intfdata(intf, NULL);
-	usb_put_dev(udev);
+	err = p54u_load_firmware(dev, intf);
 	return err;
 }
 
@@ -1007,9 +1065,10 @@ static void __devexit p54u_disconnect(struct usb_interface *intf)
 	if (!dev)
 		return;
 
+	priv = dev->priv;
+	wait_for_completion(&priv->fw_wait_load);
 	p54_unregister_common(dev);
 
-	priv = dev->priv;
 	usb_put_dev(interface_to_usbdev(intf));
 	release_firmware(priv->fw);
 	p54_free_common(dev);
@@ -1072,7 +1131,7 @@ static struct usb_driver p54u_driver = {
 	.name	= "p54usb",
 	.id_table = p54u_table,
 	.probe = p54u_probe,
-	.disconnect = p54u_disconnect,
+	.disconnect = __devexit_p(p54u_disconnect),
 	.pre_reset = p54u_pre_reset,
 	.post_reset = p54u_post_reset,
 #ifdef CONFIG_PM
@@ -1081,6 +1140,7 @@ static struct usb_driver p54u_driver = {
 	.reset_resume = p54u_resume,
 #endif /* CONFIG_PM */
 	.soft_unbind = 1,
+	.disable_hub_initiated_lpm = 1,
 };
 
 module_usb_driver(p54u_driver);
diff --git a/drivers/net/wireless/p54/p54usb.h b/drivers/net/wireless/p54/p54usb.h
index ed4034ade59..d273be7272b 100644
--- a/drivers/net/wireless/p54/p54usb.h
+++ b/drivers/net/wireless/p54/p54usb.h
@@ -143,6 +143,9 @@ struct p54u_priv {
 	struct sk_buff_head rx_queue;
 	struct usb_anchor submitted;
 	const struct firmware *fw;
+
+	/* asynchronous firmware callback */
+	struct completion fw_wait_load;
 };
 
 #endif /* P54USB_H */
diff --git a/drivers/net/wireless/p54/txrx.c b/drivers/net/wireless/p54/txrx.c
index a08a6f0e4dd..82a1cac920b 100644
--- a/drivers/net/wireless/p54/txrx.c
+++ b/drivers/net/wireless/p54/txrx.c
@@ -308,7 +308,7 @@ static void p54_pspoll_workaround(struct p54_common *priv, struct sk_buff *skb)
 		return;
 
 	/* only consider beacons from the associated BSSID */
-	if (compare_ether_addr(hdr->addr3, priv->bssid))
+	if (!ether_addr_equal(hdr->addr3, priv->bssid))
 		return;
 
 	tim = p54_find_ie(skb, WLAN_EID_TIM);
@@ -914,8 +914,7 @@ void p54_tx_80211(struct ieee80211_hw *dev, struct sk_buff *skb)
 	txhdr->hw_queue = queue;
 	txhdr->backlog = priv->tx_stats[queue].len - 1;
 	memset(txhdr->durations, 0, sizeof(txhdr->durations));
-	txhdr->tx_antenna = ((info->antenna_sel_tx == 0) ?
-		2 : info->antenna_sel_tx - 1) & priv->tx_diversity_mask;
+	txhdr->tx_antenna = 2 & priv->tx_diversity_mask;
 	if (priv->rxhw == 5) {
 		txhdr->longbow.cts_rate = cts_rate;
 		txhdr->longbow.output_power = cpu_to_le16(priv->output_power);
diff --git a/drivers/net/wireless/prism54/oid_mgt.c b/drivers/net/wireless/prism54/oid_mgt.c
index 9b796cae4af..a01606b36e0 100644
--- a/drivers/net/wireless/prism54/oid_mgt.c
+++ b/drivers/net/wireless/prism54/oid_mgt.c
@@ -693,8 +693,6 @@ mgt_update_addr(islpci_private *priv)
 	return ret;
 }
 
-#define VEC_SIZE(a) ARRAY_SIZE(a)
-
 int
 mgt_commit(islpci_private *priv)
 {
@@ -704,10 +702,10 @@ mgt_commit(islpci_private *priv)
 	if (islpci_get_state(priv) < PRV_STATE_INIT)
 		return 0;
 
-	rvalue = mgt_commit_list(priv, commit_part1, VEC_SIZE(commit_part1));
+	rvalue = mgt_commit_list(priv, commit_part1, ARRAY_SIZE(commit_part1));
 
 	if (priv->iw_mode != IW_MODE_MONITOR)
-		rvalue |= mgt_commit_list(priv, commit_part2, VEC_SIZE(commit_part2));
+		rvalue |= mgt_commit_list(priv, commit_part2, ARRAY_SIZE(commit_part2));
 
 	u = OID_INL_MODE;
 	rvalue |= mgt_commit_list(priv, &u, 1);
diff --git a/drivers/net/wireless/rndis_wlan.c b/drivers/net/wireless/rndis_wlan.c
index d66e2980bc2..2e9e6af2136 100644
--- a/drivers/net/wireless/rndis_wlan.c
+++ b/drivers/net/wireless/rndis_wlan.c
@@ -88,49 +88,6 @@ module_param_named(workaround_interval, modparam_workaround_interval,
 MODULE_PARM_DESC(workaround_interval,
 	"set stall workaround interval in msecs (0=disabled) (default: 0)");
 
-
-/* various RNDIS OID defs */
-#define OID_GEN_LINK_SPEED			cpu_to_le32(0x00010107)
-#define OID_GEN_RNDIS_CONFIG_PARAMETER		cpu_to_le32(0x0001021b)
-
-#define OID_GEN_XMIT_OK				cpu_to_le32(0x00020101)
-#define OID_GEN_RCV_OK				cpu_to_le32(0x00020102)
-#define OID_GEN_XMIT_ERROR			cpu_to_le32(0x00020103)
-#define OID_GEN_RCV_ERROR			cpu_to_le32(0x00020104)
-#define OID_GEN_RCV_NO_BUFFER			cpu_to_le32(0x00020105)
-
-#define OID_802_3_CURRENT_ADDRESS		cpu_to_le32(0x01010102)
-#define OID_802_3_MULTICAST_LIST		cpu_to_le32(0x01010103)
-#define OID_802_3_MAXIMUM_LIST_SIZE		cpu_to_le32(0x01010104)
-
-#define OID_802_11_BSSID			cpu_to_le32(0x0d010101)
-#define OID_802_11_SSID				cpu_to_le32(0x0d010102)
-#define OID_802_11_INFRASTRUCTURE_MODE		cpu_to_le32(0x0d010108)
-#define OID_802_11_ADD_WEP			cpu_to_le32(0x0d010113)
-#define OID_802_11_REMOVE_WEP			cpu_to_le32(0x0d010114)
-#define OID_802_11_DISASSOCIATE			cpu_to_le32(0x0d010115)
-#define OID_802_11_AUTHENTICATION_MODE		cpu_to_le32(0x0d010118)
-#define OID_802_11_PRIVACY_FILTER		cpu_to_le32(0x0d010119)
-#define OID_802_11_BSSID_LIST_SCAN		cpu_to_le32(0x0d01011a)
-#define OID_802_11_ENCRYPTION_STATUS		cpu_to_le32(0x0d01011b)
-#define OID_802_11_ADD_KEY			cpu_to_le32(0x0d01011d)
-#define OID_802_11_REMOVE_KEY			cpu_to_le32(0x0d01011e)
-#define OID_802_11_ASSOCIATION_INFORMATION	cpu_to_le32(0x0d01011f)
-#define OID_802_11_CAPABILITY			cpu_to_le32(0x0d010122)
-#define OID_802_11_PMKID			cpu_to_le32(0x0d010123)
-#define OID_802_11_NETWORK_TYPES_SUPPORTED	cpu_to_le32(0x0d010203)
-#define OID_802_11_NETWORK_TYPE_IN_USE		cpu_to_le32(0x0d010204)
-#define OID_802_11_TX_POWER_LEVEL		cpu_to_le32(0x0d010205)
-#define OID_802_11_RSSI				cpu_to_le32(0x0d010206)
-#define OID_802_11_RSSI_TRIGGER			cpu_to_le32(0x0d010207)
-#define OID_802_11_FRAGMENTATION_THRESHOLD	cpu_to_le32(0x0d010209)
-#define OID_802_11_RTS_THRESHOLD		cpu_to_le32(0x0d01020a)
-#define OID_802_11_SUPPORTED_RATES		cpu_to_le32(0x0d01020e)
-#define OID_802_11_CONFIGURATION		cpu_to_le32(0x0d010211)
-#define OID_802_11_POWER_MODE			cpu_to_le32(0x0d010216)
-#define OID_802_11_BSSID_LIST			cpu_to_le32(0x0d010217)
-
-
 /* Typical noise/maximum signal level values taken from ndiswrapper iw_ndis.h */
 #define	WL_NOISE	-96	/* typical noise level in dBm */
 #define	WL_SIGMAX	-32	/* typical maximum signal level in dBm */
@@ -149,12 +106,6 @@ MODULE_PARM_DESC(workaround_interval,
 #define BCM4320_DEFAULT_TXPOWER_DBM_50  10
 #define BCM4320_DEFAULT_TXPOWER_DBM_25  7
 
-
-/* codes for "status" field of completion messages */
-#define RNDIS_STATUS_ADAPTER_NOT_READY		cpu_to_le32(0xc0010011)
-#define RNDIS_STATUS_ADAPTER_NOT_OPEN		cpu_to_le32(0xc0010012)
-
-
 /* Known device types */
 #define RNDIS_UNKNOWN	0
 #define RNDIS_BCM4320A	1
@@ -515,7 +466,7 @@ struct rndis_wlan_private {
 	int infra_mode;
 	bool connected;
 	u8 bssid[ETH_ALEN];
-	__le32 current_command_oid;
+	u32 current_command_oid;
 
 	/* encryption stuff */
 	u8 encr_tx_key_index;
@@ -554,9 +505,6 @@ static int rndis_join_ibss(struct wiphy *wiphy, struct net_device *dev,
 
 static int rndis_leave_ibss(struct wiphy *wiphy, struct net_device *dev);
 
-static int rndis_set_channel(struct wiphy *wiphy, struct net_device *dev,
-	struct ieee80211_channel *chan, enum nl80211_channel_type channel_type);
-
 static int rndis_add_key(struct wiphy *wiphy, struct net_device *netdev,
 			 u8 key_index, bool pairwise, const u8 *mac_addr,
 			 struct key_params *params);
@@ -598,7 +546,6 @@ static const struct cfg80211_ops rndis_config_ops = {
 	.disconnect = rndis_disconnect,
 	.join_ibss = rndis_join_ibss,
 	.leave_ibss = rndis_leave_ibss,
-	.set_channel = rndis_set_channel,
 	.add_key = rndis_add_key,
 	.del_key = rndis_del_key,
 	.set_default_key = rndis_set_default_key,
@@ -670,63 +617,63 @@ static int rndis_akm_suite_to_key_mgmt(u32 akm_suite)
 }
 
 #ifdef DEBUG
-static const char *oid_to_string(__le32 oid)
+static const char *oid_to_string(u32 oid)
 {
 	switch (oid) {
 #define OID_STR(oid) case oid: return(#oid)
 		/* from rndis_host.h */
-		OID_STR(OID_802_3_PERMANENT_ADDRESS);
-		OID_STR(OID_GEN_MAXIMUM_FRAME_SIZE);
-		OID_STR(OID_GEN_CURRENT_PACKET_FILTER);
-		OID_STR(OID_GEN_PHYSICAL_MEDIUM);
+		OID_STR(RNDIS_OID_802_3_PERMANENT_ADDRESS);
+		OID_STR(RNDIS_OID_GEN_MAXIMUM_FRAME_SIZE);
+		OID_STR(RNDIS_OID_GEN_CURRENT_PACKET_FILTER);
+		OID_STR(RNDIS_OID_GEN_PHYSICAL_MEDIUM);
 
 		/* from rndis_wlan.c */
-		OID_STR(OID_GEN_LINK_SPEED);
-		OID_STR(OID_GEN_RNDIS_CONFIG_PARAMETER);
-
-		OID_STR(OID_GEN_XMIT_OK);
-		OID_STR(OID_GEN_RCV_OK);
-		OID_STR(OID_GEN_XMIT_ERROR);
-		OID_STR(OID_GEN_RCV_ERROR);
-		OID_STR(OID_GEN_RCV_NO_BUFFER);
-
-		OID_STR(OID_802_3_CURRENT_ADDRESS);
-		OID_STR(OID_802_3_MULTICAST_LIST);
-		OID_STR(OID_802_3_MAXIMUM_LIST_SIZE);
-
-		OID_STR(OID_802_11_BSSID);
-		OID_STR(OID_802_11_SSID);
-		OID_STR(OID_802_11_INFRASTRUCTURE_MODE);
-		OID_STR(OID_802_11_ADD_WEP);
-		OID_STR(OID_802_11_REMOVE_WEP);
-		OID_STR(OID_802_11_DISASSOCIATE);
-		OID_STR(OID_802_11_AUTHENTICATION_MODE);
-		OID_STR(OID_802_11_PRIVACY_FILTER);
-		OID_STR(OID_802_11_BSSID_LIST_SCAN);
-		OID_STR(OID_802_11_ENCRYPTION_STATUS);
-		OID_STR(OID_802_11_ADD_KEY);
-		OID_STR(OID_802_11_REMOVE_KEY);
-		OID_STR(OID_802_11_ASSOCIATION_INFORMATION);
-		OID_STR(OID_802_11_CAPABILITY);
-		OID_STR(OID_802_11_PMKID);
-		OID_STR(OID_802_11_NETWORK_TYPES_SUPPORTED);
-		OID_STR(OID_802_11_NETWORK_TYPE_IN_USE);
-		OID_STR(OID_802_11_TX_POWER_LEVEL);
-		OID_STR(OID_802_11_RSSI);
-		OID_STR(OID_802_11_RSSI_TRIGGER);
-		OID_STR(OID_802_11_FRAGMENTATION_THRESHOLD);
-		OID_STR(OID_802_11_RTS_THRESHOLD);
-		OID_STR(OID_802_11_SUPPORTED_RATES);
-		OID_STR(OID_802_11_CONFIGURATION);
-		OID_STR(OID_802_11_POWER_MODE);
-		OID_STR(OID_802_11_BSSID_LIST);
+		OID_STR(RNDIS_OID_GEN_LINK_SPEED);
+		OID_STR(RNDIS_OID_GEN_RNDIS_CONFIG_PARAMETER);
+
+		OID_STR(RNDIS_OID_GEN_XMIT_OK);
+		OID_STR(RNDIS_OID_GEN_RCV_OK);
+		OID_STR(RNDIS_OID_GEN_XMIT_ERROR);
+		OID_STR(RNDIS_OID_GEN_RCV_ERROR);
+		OID_STR(RNDIS_OID_GEN_RCV_NO_BUFFER);
+
+		OID_STR(RNDIS_OID_802_3_CURRENT_ADDRESS);
+		OID_STR(RNDIS_OID_802_3_MULTICAST_LIST);
+		OID_STR(RNDIS_OID_802_3_MAXIMUM_LIST_SIZE);
+
+		OID_STR(RNDIS_OID_802_11_BSSID);
+		OID_STR(RNDIS_OID_802_11_SSID);
+		OID_STR(RNDIS_OID_802_11_INFRASTRUCTURE_MODE);
+		OID_STR(RNDIS_OID_802_11_ADD_WEP);
+		OID_STR(RNDIS_OID_802_11_REMOVE_WEP);
+		OID_STR(RNDIS_OID_802_11_DISASSOCIATE);
+		OID_STR(RNDIS_OID_802_11_AUTHENTICATION_MODE);
+		OID_STR(RNDIS_OID_802_11_PRIVACY_FILTER);
+		OID_STR(RNDIS_OID_802_11_BSSID_LIST_SCAN);
+		OID_STR(RNDIS_OID_802_11_ENCRYPTION_STATUS);
+		OID_STR(RNDIS_OID_802_11_ADD_KEY);
+		OID_STR(RNDIS_OID_802_11_REMOVE_KEY);
+		OID_STR(RNDIS_OID_802_11_ASSOCIATION_INFORMATION);
+		OID_STR(RNDIS_OID_802_11_CAPABILITY);
+		OID_STR(RNDIS_OID_802_11_PMKID);
+		OID_STR(RNDIS_OID_802_11_NETWORK_TYPES_SUPPORTED);
+		OID_STR(RNDIS_OID_802_11_NETWORK_TYPE_IN_USE);
+		OID_STR(RNDIS_OID_802_11_TX_POWER_LEVEL);
+		OID_STR(RNDIS_OID_802_11_RSSI);
+		OID_STR(RNDIS_OID_802_11_RSSI_TRIGGER);
+		OID_STR(RNDIS_OID_802_11_FRAGMENTATION_THRESHOLD);
+		OID_STR(RNDIS_OID_802_11_RTS_THRESHOLD);
+		OID_STR(RNDIS_OID_802_11_SUPPORTED_RATES);
+		OID_STR(RNDIS_OID_802_11_CONFIGURATION);
+		OID_STR(RNDIS_OID_802_11_POWER_MODE);
+		OID_STR(RNDIS_OID_802_11_BSSID_LIST);
 #undef OID_STR
 	}
 
 	return "?";
 }
 #else
-static const char *oid_to_string(__le32 oid)
+static const char *oid_to_string(u32 oid)
 {
 	return "?";
 }
@@ -736,7 +683,7 @@ static const char *oid_to_string(__le32 oid)
 static int rndis_error_status(__le32 rndis_status)
 {
 	int ret = -EINVAL;
-	switch (rndis_status) {
+	switch (le32_to_cpu(rndis_status)) {
 	case RNDIS_STATUS_SUCCESS:
 		ret = 0;
 		break;
@@ -755,7 +702,7 @@ static int rndis_error_status(__le32 rndis_status)
 	return ret;
 }
 
-static int rndis_query_oid(struct usbnet *dev, __le32 oid, void *data, int *len)
+static int rndis_query_oid(struct usbnet *dev, u32 oid, void *data, int *len)
 {
 	struct rndis_wlan_private *priv = get_rndis_wlan_priv(dev);
 	union {
@@ -782,9 +729,9 @@ static int rndis_query_oid(struct usbnet *dev, __le32 oid, void *data, int *len)
 	mutex_lock(&priv->command_lock);
 
 	memset(u.get, 0, sizeof *u.get);
-	u.get->msg_type = RNDIS_MSG_QUERY;
+	u.get->msg_type = cpu_to_le32(RNDIS_MSG_QUERY);
 	u.get->msg_len = cpu_to_le32(sizeof *u.get);
-	u.get->oid = oid;
+	u.get->oid = cpu_to_le32(oid);
 
 	priv->current_command_oid = oid;
 	ret = rndis_command(dev, u.header, buflen);
@@ -839,7 +786,7 @@ exit_unlock:
 	return ret;
 }
 
-static int rndis_set_oid(struct usbnet *dev, __le32 oid, const void *data,
+static int rndis_set_oid(struct usbnet *dev, u32 oid, const void *data,
 			 int len)
 {
 	struct rndis_wlan_private *priv = get_rndis_wlan_priv(dev);
@@ -866,9 +813,9 @@ static int rndis_set_oid(struct usbnet *dev, __le32 oid, const void *data,
 	mutex_lock(&priv->command_lock);
 
 	memset(u.set, 0, sizeof *u.set);
-	u.set->msg_type = RNDIS_MSG_SET;
+	u.set->msg_type = cpu_to_le32(RNDIS_MSG_SET);
 	u.set->msg_len = cpu_to_le32(sizeof(*u.set) + len);
-	u.set->oid = oid;
+	u.set->oid = cpu_to_le32(oid);
 	u.set->len = cpu_to_le32(len);
 	u.set->offset = cpu_to_le32(sizeof(*u.set) - 8);
 	u.set->handle = cpu_to_le32(0);
@@ -908,7 +855,7 @@ static int rndis_reset(struct usbnet *usbdev)
 
 	reset = (void *)priv->command_buffer;
 	memset(reset, 0, sizeof(*reset));
-	reset->msg_type = RNDIS_MSG_RESET;
+	reset->msg_type = cpu_to_le32(RNDIS_MSG_RESET);
 	reset->msg_len = cpu_to_le32(sizeof(*reset));
 	priv->current_command_oid = 0;
 	ret = rndis_command(usbdev, (void *)reset, CONTROL_BUFFER_SIZE);
@@ -994,7 +941,7 @@ static int rndis_set_config_parameter(struct usbnet *dev, char *param,
 	}
 #endif
 
-	ret = rndis_set_oid(dev, OID_GEN_RNDIS_CONFIG_PARAMETER,
+	ret = rndis_set_oid(dev, RNDIS_OID_GEN_RNDIS_CONFIG_PARAMETER,
 							infobuf, info_len);
 	if (ret != 0)
 		netdev_dbg(dev->net, "setting rndis config parameter failed, %d\n",
@@ -1031,9 +978,9 @@ static int rndis_start_bssid_list_scan(struct usbnet *usbdev)
 {
 	__le32 tmp;
 
-	/* Note: OID_802_11_BSSID_LIST_SCAN clears internal BSS list. */
+	/* Note: RNDIS_OID_802_11_BSSID_LIST_SCAN clears internal BSS list. */
 	tmp = cpu_to_le32(1);
-	return rndis_set_oid(usbdev, OID_802_11_BSSID_LIST_SCAN, &tmp,
+	return rndis_set_oid(usbdev, RNDIS_OID_802_11_BSSID_LIST_SCAN, &tmp,
 							sizeof(tmp));
 }
 
@@ -1042,7 +989,8 @@ static int set_essid(struct usbnet *usbdev, struct ndis_80211_ssid *ssid)
 	struct rndis_wlan_private *priv = get_rndis_wlan_priv(usbdev);
 	int ret;
 
-	ret = rndis_set_oid(usbdev, OID_802_11_SSID, ssid, sizeof(*ssid));
+	ret = rndis_set_oid(usbdev, RNDIS_OID_802_11_SSID,
+			    ssid, sizeof(*ssid));
 	if (ret < 0) {
 		netdev_warn(usbdev->net, "setting SSID failed (%08X)\n", ret);
 		return ret;
@@ -1059,7 +1007,8 @@ static int set_bssid(struct usbnet *usbdev, const u8 *bssid)
 {
 	int ret;
 
-	ret = rndis_set_oid(usbdev, OID_802_11_BSSID, bssid, ETH_ALEN);
+	ret = rndis_set_oid(usbdev, RNDIS_OID_802_11_BSSID,
+			    bssid, ETH_ALEN);
 	if (ret < 0) {
 		netdev_warn(usbdev->net, "setting BSSID[%pM] failed (%08X)\n",
 			    bssid, ret);
@@ -1083,7 +1032,8 @@ static int get_bssid(struct usbnet *usbdev, u8 bssid[ETH_ALEN])
 	int ret, len;
 
 	len = ETH_ALEN;
-	ret = rndis_query_oid(usbdev, OID_802_11_BSSID, bssid, &len);
+	ret = rndis_query_oid(usbdev, RNDIS_OID_802_11_BSSID,
+			      bssid, &len);
 
 	if (ret != 0)
 		memset(bssid, 0, ETH_ALEN);
@@ -1094,8 +1044,9 @@ static int get_bssid(struct usbnet *usbdev, u8 bssid[ETH_ALEN])
 static int get_association_info(struct usbnet *usbdev,
 			struct ndis_80211_assoc_info *info, int len)
 {
-	return rndis_query_oid(usbdev, OID_802_11_ASSOCIATION_INFORMATION,
-				info, &len);
+	return rndis_query_oid(usbdev,
+			RNDIS_OID_802_11_ASSOCIATION_INFORMATION,
+			info, &len);
 }
 
 static bool is_associated(struct usbnet *usbdev)
@@ -1119,7 +1070,9 @@ static int disassociate(struct usbnet *usbdev, bool reset_ssid)
 	int i, ret = 0;
 
 	if (priv->radio_on) {
-		ret = rndis_set_oid(usbdev, OID_802_11_DISASSOCIATE, NULL, 0);
+		ret = rndis_set_oid(usbdev,
+				RNDIS_OID_802_11_DISASSOCIATE,
+				NULL, 0);
 		if (ret == 0) {
 			priv->radio_on = false;
 			netdev_dbg(usbdev->net, "%s(): radio_on = false\n",
@@ -1181,8 +1134,9 @@ static int set_auth_mode(struct usbnet *usbdev, u32 wpa_version,
 		return -ENOTSUPP;
 
 	tmp = cpu_to_le32(auth_mode);
-	ret = rndis_set_oid(usbdev, OID_802_11_AUTHENTICATION_MODE, &tmp,
-								sizeof(tmp));
+	ret = rndis_set_oid(usbdev,
+			    RNDIS_OID_802_11_AUTHENTICATION_MODE,
+			    &tmp, sizeof(tmp));
 	if (ret != 0) {
 		netdev_warn(usbdev->net, "setting auth mode failed (%08X)\n",
 			    ret);
@@ -1208,8 +1162,9 @@ static int set_priv_filter(struct usbnet *usbdev)
 	else
 		tmp = cpu_to_le32(NDIS_80211_PRIV_ACCEPT_ALL);
 
-	return rndis_set_oid(usbdev, OID_802_11_PRIVACY_FILTER, &tmp,
-								sizeof(tmp));
+	return rndis_set_oid(usbdev,
+			     RNDIS_OID_802_11_PRIVACY_FILTER, &tmp,
+			     sizeof(tmp));
 }
 
 static int set_encr_mode(struct usbnet *usbdev, int pairwise, int groupwise)
@@ -1234,8 +1189,9 @@ static int set_encr_mode(struct usbnet *usbdev, int pairwise, int groupwise)
 		encr_mode = NDIS_80211_ENCR_DISABLED;
 
 	tmp = cpu_to_le32(encr_mode);
-	ret = rndis_set_oid(usbdev, OID_802_11_ENCRYPTION_STATUS, &tmp,
-								sizeof(tmp));
+	ret = rndis_set_oid(usbdev,
+			RNDIS_OID_802_11_ENCRYPTION_STATUS, &tmp,
+			sizeof(tmp));
 	if (ret != 0) {
 		netdev_warn(usbdev->net, "setting encr mode failed (%08X)\n",
 			    ret);
@@ -1255,8 +1211,9 @@ static int set_infra_mode(struct usbnet *usbdev, int mode)
 		   __func__, priv->infra_mode);
 
 	tmp = cpu_to_le32(mode);
-	ret = rndis_set_oid(usbdev, OID_802_11_INFRASTRUCTURE_MODE, &tmp,
-								sizeof(tmp));
+	ret = rndis_set_oid(usbdev,
+			    RNDIS_OID_802_11_INFRASTRUCTURE_MODE,
+			    &tmp, sizeof(tmp));
 	if (ret != 0) {
 		netdev_warn(usbdev->net, "setting infra mode failed (%08X)\n",
 			    ret);
@@ -1282,8 +1239,9 @@ static int set_rts_threshold(struct usbnet *usbdev, u32 rts_threshold)
 		rts_threshold = 2347;
 
 	tmp = cpu_to_le32(rts_threshold);
-	return rndis_set_oid(usbdev, OID_802_11_RTS_THRESHOLD, &tmp,
-								sizeof(tmp));
+	return rndis_set_oid(usbdev,
+			     RNDIS_OID_802_11_RTS_THRESHOLD,
+			     &tmp, sizeof(tmp));
 }
 
 static int set_frag_threshold(struct usbnet *usbdev, u32 frag_threshold)
@@ -1296,8 +1254,9 @@ static int set_frag_threshold(struct usbnet *usbdev, u32 frag_threshold)
 		frag_threshold = 2346;
 
 	tmp = cpu_to_le32(frag_threshold);
-	return rndis_set_oid(usbdev, OID_802_11_FRAGMENTATION_THRESHOLD, &tmp,
-								sizeof(tmp));
+	return rndis_set_oid(usbdev,
+			RNDIS_OID_802_11_FRAGMENTATION_THRESHOLD,
+			&tmp, sizeof(tmp));
 }
 
 static void set_default_iw_params(struct usbnet *usbdev)
@@ -1333,7 +1292,9 @@ static int set_channel(struct usbnet *usbdev, int channel)
 	dsconfig = ieee80211_dsss_chan_to_freq(channel) * 1000;
 
 	len = sizeof(config);
-	ret = rndis_query_oid(usbdev, OID_802_11_CONFIGURATION, &config, &len);
+	ret = rndis_query_oid(usbdev,
+			RNDIS_OID_802_11_CONFIGURATION,
+			&config, &len);
 	if (ret < 0) {
 		netdev_dbg(usbdev->net, "%s(): querying configuration failed\n",
 			   __func__);
@@ -1341,8 +1302,9 @@ static int set_channel(struct usbnet *usbdev, int channel)
 	}
 
 	config.ds_config = cpu_to_le32(dsconfig);
-	ret = rndis_set_oid(usbdev, OID_802_11_CONFIGURATION, &config,
-								sizeof(config));
+	ret = rndis_set_oid(usbdev,
+			RNDIS_OID_802_11_CONFIGURATION,
+			&config, sizeof(config));
 
 	netdev_dbg(usbdev->net, "%s(): %d -> %d\n", __func__, channel, ret);
 
@@ -1359,8 +1321,10 @@ static struct ieee80211_channel *get_current_channel(struct usbnet *usbdev,
 
 	/* Get channel and beacon interval */
 	len = sizeof(config);
-	ret = rndis_query_oid(usbdev, OID_802_11_CONFIGURATION, &config, &len);
-	netdev_dbg(usbdev->net, "%s(): OID_802_11_CONFIGURATION -> %d\n",
+	ret = rndis_query_oid(usbdev,
+			RNDIS_OID_802_11_CONFIGURATION,
+			&config, &len);
+	netdev_dbg(usbdev->net, "%s(): RNDIS_OID_802_11_CONFIGURATION -> %d\n",
 				__func__, ret);
 	if (ret < 0)
 		return NULL;
@@ -1413,8 +1377,9 @@ static int add_wep_key(struct usbnet *usbdev, const u8 *key, int key_len,
 				    ret);
 	}
 
-	ret = rndis_set_oid(usbdev, OID_802_11_ADD_WEP, &ndis_key,
-							sizeof(ndis_key));
+	ret = rndis_set_oid(usbdev,
+			RNDIS_OID_802_11_ADD_WEP, &ndis_key,
+			sizeof(ndis_key));
 	if (ret != 0) {
 		netdev_warn(usbdev->net, "adding encryption key %d failed (%08X)\n",
 			    index + 1, ret);
@@ -1504,9 +1469,10 @@ static int add_wpa_key(struct usbnet *usbdev, const u8 *key, int key_len,
 			get_bssid(usbdev, ndis_key.bssid);
 	}
 
-	ret = rndis_set_oid(usbdev, OID_802_11_ADD_KEY, &ndis_key,
-					le32_to_cpu(ndis_key.size));
-	netdev_dbg(usbdev->net, "%s(): OID_802_11_ADD_KEY -> %08X\n",
+	ret = rndis_set_oid(usbdev,
+			RNDIS_OID_802_11_ADD_KEY, &ndis_key,
+			le32_to_cpu(ndis_key.size));
+	netdev_dbg(usbdev->net, "%s(): RNDIS_OID_802_11_ADD_KEY -> %08X\n",
 		   __func__, ret);
 	if (ret != 0)
 		return ret;
@@ -1594,14 +1560,16 @@ static int remove_key(struct usbnet *usbdev, u8 index, const u8 *bssid)
 			memset(remove_key.bssid, 0xff,
 						sizeof(remove_key.bssid));
 
-		ret = rndis_set_oid(usbdev, OID_802_11_REMOVE_KEY, &remove_key,
-							sizeof(remove_key));
+		ret = rndis_set_oid(usbdev,
+				RNDIS_OID_802_11_REMOVE_KEY,
+				&remove_key, sizeof(remove_key));
 		if (ret != 0)
 			return ret;
 	} else {
 		keyindex = cpu_to_le32(index);
-		ret = rndis_set_oid(usbdev, OID_802_11_REMOVE_WEP, &keyindex,
-							sizeof(keyindex));
+		ret = rndis_set_oid(usbdev,
+				RNDIS_OID_802_11_REMOVE_WEP,
+				&keyindex, sizeof(keyindex));
 		if (ret != 0) {
 			netdev_warn(usbdev->net,
 				    "removing encryption key %d failed (%08X)\n",
@@ -1626,14 +1594,14 @@ static void set_multicast_list(struct usbnet *usbdev)
 	char *mc_addrs = NULL;
 	int mc_count;
 
-	basefilter = filter = RNDIS_PACKET_TYPE_DIRECTED |
-			      RNDIS_PACKET_TYPE_BROADCAST;
+	basefilter = filter = cpu_to_le32(RNDIS_PACKET_TYPE_DIRECTED |
+					  RNDIS_PACKET_TYPE_BROADCAST);
 
 	if (usbdev->net->flags & IFF_PROMISC) {
-		filter |= RNDIS_PACKET_TYPE_PROMISCUOUS |
-			RNDIS_PACKET_TYPE_ALL_LOCAL;
+		filter |= cpu_to_le32(RNDIS_PACKET_TYPE_PROMISCUOUS |
+				      RNDIS_PACKET_TYPE_ALL_LOCAL);
 	} else if (usbdev->net->flags & IFF_ALLMULTI) {
-		filter |= RNDIS_PACKET_TYPE_ALL_MULTICAST;
+		filter |= cpu_to_le32(RNDIS_PACKET_TYPE_ALL_MULTICAST);
 	}
 
 	if (filter != basefilter)
@@ -1646,7 +1614,7 @@ static void set_multicast_list(struct usbnet *usbdev)
 	netif_addr_lock_bh(usbdev->net);
 	mc_count = netdev_mc_count(usbdev->net);
 	if (mc_count > priv->multicast_size) {
-		filter |= RNDIS_PACKET_TYPE_ALL_MULTICAST;
+		filter |= cpu_to_le32(RNDIS_PACKET_TYPE_ALL_MULTICAST);
 	} else if (mc_count) {
 		int i = 0;
 
@@ -1669,27 +1637,28 @@ static void set_multicast_list(struct usbnet *usbdev)
 		goto set_filter;
 
 	if (mc_count) {
-		ret = rndis_set_oid(usbdev, OID_802_3_MULTICAST_LIST, mc_addrs,
-				    mc_count * ETH_ALEN);
+		ret = rndis_set_oid(usbdev,
+				RNDIS_OID_802_3_MULTICAST_LIST,
+				mc_addrs, mc_count * ETH_ALEN);
 		kfree(mc_addrs);
 		if (ret == 0)
-			filter |= RNDIS_PACKET_TYPE_MULTICAST;
+			filter |= cpu_to_le32(RNDIS_PACKET_TYPE_MULTICAST);
 		else
-			filter |= RNDIS_PACKET_TYPE_ALL_MULTICAST;
+			filter |= cpu_to_le32(RNDIS_PACKET_TYPE_ALL_MULTICAST);
 
-		netdev_dbg(usbdev->net, "OID_802_3_MULTICAST_LIST(%d, max: %d) -> %d\n",
+		netdev_dbg(usbdev->net, "RNDIS_OID_802_3_MULTICAST_LIST(%d, max: %d) -> %d\n",
 			   mc_count, priv->multicast_size, ret);
 	}
 
 set_filter:
-	ret = rndis_set_oid(usbdev, OID_GEN_CURRENT_PACKET_FILTER, &filter,
+	ret = rndis_set_oid(usbdev, RNDIS_OID_GEN_CURRENT_PACKET_FILTER, &filter,
 							sizeof(filter));
 	if (ret < 0) {
 		netdev_warn(usbdev->net, "couldn't set packet filter: %08x\n",
 			    le32_to_cpu(filter));
 	}
 
-	netdev_dbg(usbdev->net, "OID_GEN_CURRENT_PACKET_FILTER(%08x) -> %d\n",
+	netdev_dbg(usbdev->net, "RNDIS_OID_GEN_CURRENT_PACKET_FILTER(%08x) -> %d\n",
 		   le32_to_cpu(filter), ret);
 }
 
@@ -1748,9 +1717,10 @@ static struct ndis_80211_pmkid *get_device_pmkids(struct usbnet *usbdev)
 	pmkids->length = cpu_to_le32(len);
 	pmkids->bssid_info_count = cpu_to_le32(max_pmkids);
 
-	ret = rndis_query_oid(usbdev, OID_802_11_PMKID, pmkids, &len);
+	ret = rndis_query_oid(usbdev, RNDIS_OID_802_11_PMKID,
+			pmkids, &len);
 	if (ret < 0) {
-		netdev_dbg(usbdev->net, "%s(): OID_802_11_PMKID(%d, %d)"
+		netdev_dbg(usbdev->net, "%s(): RNDIS_OID_802_11_PMKID(%d, %d)"
 				" -> %d\n", __func__, len, max_pmkids, ret);
 
 		kfree(pmkids);
@@ -1776,10 +1746,10 @@ static int set_device_pmkids(struct usbnet *usbdev,
 
 	debug_print_pmkids(usbdev, pmkids, __func__);
 
-	ret = rndis_set_oid(usbdev, OID_802_11_PMKID, pmkids,
-						le32_to_cpu(pmkids->length));
+	ret = rndis_set_oid(usbdev, RNDIS_OID_802_11_PMKID, pmkids,
+			    le32_to_cpu(pmkids->length));
 	if (ret < 0) {
-		netdev_dbg(usbdev->net, "%s(): OID_802_11_PMKID(%d, %d) -> %d"
+		netdev_dbg(usbdev->net, "%s(): RNDIS_OID_802_11_PMKID(%d, %d) -> %d"
 				"\n", __func__, len, num_pmkids, ret);
 	}
 
@@ -1801,8 +1771,8 @@ static struct ndis_80211_pmkid *remove_pmkid(struct usbnet *usbdev,
 		count = max_pmkids;
 
 	for (i = 0; i < count; i++)
-		if (!compare_ether_addr(pmkids->bssid_info[i].bssid,
-							pmksa->bssid))
+		if (ether_addr_equal(pmkids->bssid_info[i].bssid,
+				     pmksa->bssid))
 			break;
 
 	/* pmkid not found */
@@ -1843,8 +1813,8 @@ static struct ndis_80211_pmkid *update_pmkid(struct usbnet *usbdev,
 
 	/* update with new pmkid */
 	for (i = 0; i < count; i++) {
-		if (compare_ether_addr(pmkids->bssid_info[i].bssid,
-							pmksa->bssid))
+		if (!ether_addr_equal(pmkids->bssid_info[i].bssid,
+				      pmksa->bssid))
 			continue;
 
 		memcpy(pmkids->bssid_info[i].pmkid, pmksa->pmkid,
@@ -2113,7 +2083,8 @@ resize_buf:
 	 * resizing until it won't get any bigger.
 	 */
 	new_len = len;
-	ret = rndis_query_oid(usbdev, OID_802_11_BSSID_LIST, buf, &new_len);
+	ret = rndis_query_oid(usbdev, RNDIS_OID_802_11_BSSID_LIST,
+			      buf, &new_len);
 	if (ret != 0 || new_len < sizeof(struct ndis_80211_bssid_list_ex))
 		goto out;
 
@@ -2139,7 +2110,7 @@ resize_buf:
 	while (check_bssid_list_item(bssid, bssid_len, buf, len)) {
 		if (rndis_bss_info_update(usbdev, bssid) && match_bssid &&
 		    matched) {
-			if (compare_ether_addr(bssid->mac, match_bssid))
+			if (!ether_addr_equal(bssid->mac, match_bssid))
 				*matched = true;
 		}
 
@@ -2423,16 +2394,6 @@ static int rndis_leave_ibss(struct wiphy *wiphy, struct net_device *dev)
 	return deauthenticate(usbdev);
 }
 
-static int rndis_set_channel(struct wiphy *wiphy, struct net_device *netdev,
-	struct ieee80211_channel *chan, enum nl80211_channel_type channel_type)
-{
-	struct rndis_wlan_private *priv = wiphy_priv(wiphy);
-	struct usbnet *usbdev = priv->usbdev;
-
-	return set_channel(usbdev,
-			ieee80211_frequency_to_channel(chan->center_freq));
-}
-
 static int rndis_add_key(struct wiphy *wiphy, struct net_device *netdev,
 			 u8 key_index, bool pairwise, const u8 *mac_addr,
 			 struct key_params *params)
@@ -2511,14 +2472,15 @@ static void rndis_fill_station_info(struct usbnet *usbdev,
 	memset(sinfo, 0, sizeof(*sinfo));
 
 	len = sizeof(linkspeed);
-	ret = rndis_query_oid(usbdev, OID_GEN_LINK_SPEED, &linkspeed, &len);
+	ret = rndis_query_oid(usbdev, RNDIS_OID_GEN_LINK_SPEED, &linkspeed, &len);
 	if (ret == 0) {
 		sinfo->txrate.legacy = le32_to_cpu(linkspeed) / 1000;
 		sinfo->filled |= STATION_INFO_TX_BITRATE;
 	}
 
 	len = sizeof(rssi);
-	ret = rndis_query_oid(usbdev, OID_802_11_RSSI, &rssi, &len);
+	ret = rndis_query_oid(usbdev, RNDIS_OID_802_11_RSSI,
+			      &rssi, &len);
 	if (ret == 0) {
 		sinfo->signal = level_to_qual(le32_to_cpu(rssi));
 		sinfo->filled |= STATION_INFO_SIGNAL;
@@ -2531,7 +2493,7 @@ static int rndis_get_station(struct wiphy *wiphy, struct net_device *dev,
 	struct rndis_wlan_private *priv = wiphy_priv(wiphy);
 	struct usbnet *usbdev = priv->usbdev;
 
-	if (compare_ether_addr(priv->bssid, mac))
+	if (!ether_addr_equal(priv->bssid, mac))
 		return -ENOENT;
 
 	rndis_fill_station_info(usbdev, sinfo);
@@ -2624,7 +2586,8 @@ static int rndis_flush_pmksa(struct wiphy *wiphy, struct net_device *netdev)
 	pmkid.length = cpu_to_le32(sizeof(pmkid));
 	pmkid.bssid_info_count = cpu_to_le32(0);
 
-	return rndis_set_oid(usbdev, OID_802_11_PMKID, &pmkid, sizeof(pmkid));
+	return rndis_set_oid(usbdev, RNDIS_OID_802_11_PMKID,
+			     &pmkid, sizeof(pmkid));
 }
 
 static int rndis_set_power_mgmt(struct wiphy *wiphy, struct net_device *dev,
@@ -2654,9 +2617,10 @@ static int rndis_set_power_mgmt(struct wiphy *wiphy, struct net_device *dev,
 	priv->power_mode = power_mode;
 
 	mode = cpu_to_le32(power_mode);
-	ret = rndis_set_oid(usbdev, OID_802_11_POWER_MODE, &mode, sizeof(mode));
+	ret = rndis_set_oid(usbdev, RNDIS_OID_802_11_POWER_MODE,
+			    &mode, sizeof(mode));
 
-	netdev_dbg(usbdev->net, "%s(): OID_802_11_POWER_MODE -> %d\n",
+	netdev_dbg(usbdev->net, "%s(): RNDIS_OID_802_11_POWER_MODE -> %d\n",
 				__func__, ret);
 
 	return ret;
@@ -2693,10 +2657,11 @@ static void rndis_wlan_craft_connected_bss(struct usbnet *usbdev, u8 *bssid,
 	/* Get signal quality, in case of error use rssi=0 and ignore error. */
 	len = sizeof(rssi);
 	rssi = 0;
-	ret = rndis_query_oid(usbdev, OID_802_11_RSSI, &rssi, &len);
+	ret = rndis_query_oid(usbdev, RNDIS_OID_802_11_RSSI,
+			      &rssi, &len);
 	signal = level_to_qual(le32_to_cpu(rssi));
 
-	netdev_dbg(usbdev->net, "%s(): OID_802_11_RSSI -> %d, "
+	netdev_dbg(usbdev->net, "%s(): RNDIS_OID_802_11_RSSI -> %d, "
 		   "rssi:%d, qual: %d\n", __func__, ret, le32_to_cpu(rssi),
 		   level_to_qual(le32_to_cpu(rssi)));
 
@@ -2720,8 +2685,9 @@ static void rndis_wlan_craft_connected_bss(struct usbnet *usbdev, u8 *bssid,
 	/* Get SSID, in case of error, use zero length SSID and ignore error. */
 	len = sizeof(ssid);
 	memset(&ssid, 0, sizeof(ssid));
-	ret = rndis_query_oid(usbdev, OID_802_11_SSID, &ssid, &len);
-	netdev_dbg(usbdev->net, "%s(): OID_802_11_SSID -> %d, len: %d, ssid: "
+	ret = rndis_query_oid(usbdev, RNDIS_OID_802_11_SSID,
+			      &ssid, &len);
+	netdev_dbg(usbdev->net, "%s(): RNDIS_OID_802_11_SSID -> %d, len: %d, ssid: "
 				"'%.32s'\n", __func__, ret,
 				le32_to_cpu(ssid.length), ssid.essid);
 
@@ -2843,7 +2809,7 @@ static void rndis_wlan_do_link_up_work(struct usbnet *usbdev)
 	 * NDIS spec says: "If the device is associated, but the associated
 	 *  BSSID is not in its BSSID scan list, then the driver must add an
 	 *  entry for the BSSID at the end of the data that it returns in
-	 *  response to query of OID_802_11_BSSID_LIST."
+	 *  response to query of RNDIS_OID_802_11_BSSID_LIST."
 	 *
 	 * NOTE: Seems to be true for BCM4320b variant, but not BCM4320a.
 	 */
@@ -3095,15 +3061,15 @@ static void rndis_wlan_indication(struct usbnet *usbdev, void *ind, int buflen)
 	struct rndis_wlan_private *priv = get_rndis_wlan_priv(usbdev);
 	struct rndis_indicate *msg = ind;
 
-	switch (msg->status) {
+	switch (le32_to_cpu(msg->status)) {
 	case RNDIS_STATUS_MEDIA_CONNECT:
-		if (priv->current_command_oid == OID_802_11_ADD_KEY) {
-			/* OID_802_11_ADD_KEY causes sometimes extra
+		if (priv->current_command_oid == RNDIS_OID_802_11_ADD_KEY) {
+			/* RNDIS_OID_802_11_ADD_KEY causes sometimes extra
 			 * "media connect" indications which confuses driver
 			 * and userspace to think that device is
 			 * roaming/reassociating when it isn't.
 			 */
-			netdev_dbg(usbdev->net, "ignored OID_802_11_ADD_KEY triggered 'media connect'\n");
+			netdev_dbg(usbdev->net, "ignored RNDIS_OID_802_11_ADD_KEY triggered 'media connect'\n");
 			return;
 		}
 
@@ -3148,8 +3114,9 @@ static int rndis_wlan_get_caps(struct usbnet *usbdev, struct wiphy *wiphy)
 
 	/* determine supported modes */
 	len = sizeof(networks_supported);
-	retval = rndis_query_oid(usbdev, OID_802_11_NETWORK_TYPES_SUPPORTED,
-						&networks_supported, &len);
+	retval = rndis_query_oid(usbdev,
+				 RNDIS_OID_802_11_NETWORK_TYPES_SUPPORTED,
+				 &networks_supported, &len);
 	if (retval >= 0) {
 		n = le32_to_cpu(networks_supported.num_items);
 		if (n > 8)
@@ -3173,9 +3140,11 @@ static int rndis_wlan_get_caps(struct usbnet *usbdev, struct wiphy *wiphy)
 	/* get device 802.11 capabilities, number of PMKIDs */
 	caps = (struct ndis_80211_capability *)caps_buf;
 	len = sizeof(caps_buf);
-	retval = rndis_query_oid(usbdev, OID_802_11_CAPABILITY, caps, &len);
+	retval = rndis_query_oid(usbdev,
+				 RNDIS_OID_802_11_CAPABILITY,
+				 caps, &len);
 	if (retval >= 0) {
-		netdev_dbg(usbdev->net, "OID_802_11_CAPABILITY -> len %d, "
+		netdev_dbg(usbdev->net, "RNDIS_OID_802_11_CAPABILITY -> len %d, "
 				"ver %d, pmkids %d, auth-encr-pairs %d\n",
 				le32_to_cpu(caps->length),
 				le32_to_cpu(caps->version),
@@ -3247,13 +3216,14 @@ static void rndis_device_poller(struct work_struct *work)
 	}
 
 	len = sizeof(rssi);
-	ret = rndis_query_oid(usbdev, OID_802_11_RSSI, &rssi, &len);
+	ret = rndis_query_oid(usbdev, RNDIS_OID_802_11_RSSI,
+			      &rssi, &len);
 	if (ret == 0) {
 		priv->last_qual = level_to_qual(le32_to_cpu(rssi));
 		rndis_do_cqm(usbdev, le32_to_cpu(rssi));
 	}
 
-	netdev_dbg(usbdev->net, "dev-poller: OID_802_11_RSSI -> %d, rssi:%d, qual: %d\n",
+	netdev_dbg(usbdev->net, "dev-poller: RNDIS_OID_802_11_RSSI -> %d, rssi:%d, qual: %d\n",
 		   ret, le32_to_cpu(rssi), level_to_qual(le32_to_cpu(rssi)));
 
 	/* Workaround transfer stalls on poor quality links.
@@ -3275,15 +3245,18 @@ static void rndis_device_poller(struct work_struct *work)
 		 * working.
 		 */
 		tmp = cpu_to_le32(1);
-		rndis_set_oid(usbdev, OID_802_11_BSSID_LIST_SCAN, &tmp,
-								sizeof(tmp));
+		rndis_set_oid(usbdev,
+			      RNDIS_OID_802_11_BSSID_LIST_SCAN,
+			      &tmp, sizeof(tmp));
 
 		len = CONTROL_BUFFER_SIZE;
 		buf = kmalloc(len, GFP_KERNEL);
 		if (!buf)
 			goto end;
 
-		rndis_query_oid(usbdev, OID_802_11_BSSID_LIST, buf, &len);
+		rndis_query_oid(usbdev,
+				RNDIS_OID_802_11_BSSID_LIST,
+				buf, &len);
 		kfree(buf);
 	}
 
@@ -3465,13 +3438,15 @@ static int rndis_wlan_bind(struct usbnet *usbdev, struct usb_interface *intf)
 	 */
 	usbdev->net->netdev_ops = &rndis_wlan_netdev_ops;
 
-	tmp = RNDIS_PACKET_TYPE_DIRECTED | RNDIS_PACKET_TYPE_BROADCAST;
-	retval = rndis_set_oid(usbdev, OID_GEN_CURRENT_PACKET_FILTER, &tmp,
-								sizeof(tmp));
+	tmp = cpu_to_le32(RNDIS_PACKET_TYPE_DIRECTED | RNDIS_PACKET_TYPE_BROADCAST);
+	retval = rndis_set_oid(usbdev,
+			       RNDIS_OID_GEN_CURRENT_PACKET_FILTER,
+			       &tmp, sizeof(tmp));
 
 	len = sizeof(tmp);
-	retval = rndis_query_oid(usbdev, OID_802_3_MAXIMUM_LIST_SIZE, &tmp,
-								&len);
+	retval = rndis_query_oid(usbdev,
+				 RNDIS_OID_802_3_MAXIMUM_LIST_SIZE,
+				 &tmp, &len);
 	priv->multicast_size = le32_to_cpu(tmp);
 	if (retval < 0 || priv->multicast_size < 0)
 		priv->multicast_size = 0;
@@ -3601,7 +3576,7 @@ static int rndis_wlan_stop(struct usbnet *usbdev)
 	/* Set current packet filter zero to block receiving data packets from
 	   device. */
 	filter = 0;
-	rndis_set_oid(usbdev, OID_GEN_CURRENT_PACKET_FILTER, &filter,
+	rndis_set_oid(usbdev, RNDIS_OID_GEN_CURRENT_PACKET_FILTER, &filter,
 								sizeof(filter));
 
 	return retval;
@@ -3776,6 +3751,7 @@ static struct usb_driver rndis_wlan_driver = {
 	.disconnect =	usbnet_disconnect,
 	.suspend =	usbnet_suspend,
 	.resume =	usbnet_resume,
+	.disable_hub_initiated_lpm = 1,
 };
 
 module_usb_driver(rndis_wlan_driver);
diff --git a/drivers/net/wireless/rt2x00/rt2400pci.c b/drivers/net/wireless/rt2x00/rt2400pci.c
index 3a6b40239bc..5e6b5014316 100644
--- a/drivers/net/wireless/rt2x00/rt2400pci.c
+++ b/drivers/net/wireless/rt2x00/rt2400pci.c
@@ -1828,15 +1828,4 @@ static struct pci_driver rt2400pci_driver = {
 	.resume		= rt2x00pci_resume,
 };
 
-static int __init rt2400pci_init(void)
-{
-	return pci_register_driver(&rt2400pci_driver);
-}
-
-static void __exit rt2400pci_exit(void)
-{
-	pci_unregister_driver(&rt2400pci_driver);
-}
-
-module_init(rt2400pci_init);
-module_exit(rt2400pci_exit);
+module_pci_driver(rt2400pci_driver);
diff --git a/drivers/net/wireless/rt2x00/rt2500pci.c b/drivers/net/wireless/rt2x00/rt2500pci.c
index dcc0e1fcca7..136b849f11b 100644
--- a/drivers/net/wireless/rt2x00/rt2500pci.c
+++ b/drivers/net/wireless/rt2x00/rt2500pci.c
@@ -2119,15 +2119,4 @@ static struct pci_driver rt2500pci_driver = {
 	.resume		= rt2x00pci_resume,
 };
 
-static int __init rt2500pci_init(void)
-{
-	return pci_register_driver(&rt2500pci_driver);
-}
-
-static void __exit rt2500pci_exit(void)
-{
-	pci_unregister_driver(&rt2500pci_driver);
-}
-
-module_init(rt2500pci_init);
-module_exit(rt2500pci_exit);
+module_pci_driver(rt2500pci_driver);
diff --git a/drivers/net/wireless/rt2x00/rt2500usb.c b/drivers/net/wireless/rt2x00/rt2500usb.c
index 1de9c752c88..669aecdb411 100644
--- a/drivers/net/wireless/rt2x00/rt2500usb.c
+++ b/drivers/net/wireless/rt2x00/rt2500usb.c
@@ -1912,7 +1912,7 @@ static struct usb_device_id rt2500usb_device_table[] = {
 	{ USB_DEVICE(0x0b05, 0x1706) },
 	{ USB_DEVICE(0x0b05, 0x1707) },
 	/* Belkin */
-	{ USB_DEVICE(0x050d, 0x7050) },
+	{ USB_DEVICE(0x050d, 0x7050) },	/* FCC ID: K7SF5D7050A ver. 2.x */
 	{ USB_DEVICE(0x050d, 0x7051) },
 	/* Cisco Systems */
 	{ USB_DEVICE(0x13b1, 0x000d) },
@@ -1980,6 +1980,7 @@ static struct usb_driver rt2500usb_driver = {
 	.disconnect	= rt2x00usb_disconnect,
 	.suspend	= rt2x00usb_suspend,
 	.resume		= rt2x00usb_resume,
+	.disable_hub_initiated_lpm = 1,
 };
 
 module_usb_driver(rt2500usb_driver);
diff --git a/drivers/net/wireless/rt2x00/rt2800.h b/drivers/net/wireless/rt2x00/rt2800.h
index 063bfa8b91f..9348521e083 100644
--- a/drivers/net/wireless/rt2x00/rt2800.h
+++ b/drivers/net/wireless/rt2x00/rt2800.h
@@ -83,6 +83,7 @@
 #define REV_RT3090E			0x0211
 #define REV_RT3390E			0x0211
 #define REV_RT5390F			0x0502
+#define REV_RT5390R			0x1502
 
 /*
  * Signal information.
@@ -98,9 +99,11 @@
 #define EEPROM_BASE			0x0000
 #define EEPROM_SIZE			0x0110
 #define BBP_BASE			0x0000
-#define BBP_SIZE			0x0080
+#define BBP_SIZE			0x00ff
 #define RF_BASE				0x0004
 #define RF_SIZE				0x0010
+#define RFCSR_BASE			0x0000
+#define RFCSR_SIZE			0x0040
 
 /*
  * Number of TX queues.
diff --git a/drivers/net/wireless/rt2x00/rt2800lib.c b/drivers/net/wireless/rt2x00/rt2800lib.c
index 6c0a12ea6a1..dfc90d34be6 100644
--- a/drivers/net/wireless/rt2x00/rt2800lib.c
+++ b/drivers/net/wireless/rt2x00/rt2800lib.c
@@ -290,11 +290,25 @@ int rt2800_wait_wpdma_ready(struct rt2x00_dev *rt2x00dev)
 		msleep(10);
 	}
 
-	ERROR(rt2x00dev, "WPDMA TX/RX busy, aborting.\n");
+	ERROR(rt2x00dev, "WPDMA TX/RX busy [0x%08x].\n", reg);
 	return -EACCES;
 }
 EXPORT_SYMBOL_GPL(rt2800_wait_wpdma_ready);
 
+void rt2800_disable_wpdma(struct rt2x00_dev *rt2x00dev)
+{
+	u32 reg;
+
+	rt2800_register_read(rt2x00dev, WPDMA_GLO_CFG, &reg);
+	rt2x00_set_field32(&reg, WPDMA_GLO_CFG_ENABLE_TX_DMA, 0);
+	rt2x00_set_field32(&reg, WPDMA_GLO_CFG_TX_DMA_BUSY, 0);
+	rt2x00_set_field32(&reg, WPDMA_GLO_CFG_ENABLE_RX_DMA, 0);
+	rt2x00_set_field32(&reg, WPDMA_GLO_CFG_RX_DMA_BUSY, 0);
+	rt2x00_set_field32(&reg, WPDMA_GLO_CFG_TX_WRITEBACK_DONE, 1);
+	rt2800_register_write(rt2x00dev, WPDMA_GLO_CFG, reg);
+}
+EXPORT_SYMBOL_GPL(rt2800_disable_wpdma);
+
 static bool rt2800_check_firmware_crc(const u8 *data, const size_t len)
 {
 	u16 fw_crc;
@@ -412,6 +426,8 @@ int rt2800_load_firmware(struct rt2x00_dev *rt2x00dev,
 		rt2800_register_write(rt2x00dev, PWR_PIN_CFG, 0x00000002);
 	}
 
+	rt2800_disable_wpdma(rt2x00dev);
+
 	/*
 	 * Write firmware to the device.
 	 */
@@ -436,10 +452,7 @@ int rt2800_load_firmware(struct rt2x00_dev *rt2x00dev,
 	 * Disable DMA, will be reenabled later when enabling
 	 * the radio.
 	 */
-	rt2800_register_read(rt2x00dev, WPDMA_GLO_CFG, &reg);
-	rt2x00_set_field32(&reg, WPDMA_GLO_CFG_ENABLE_TX_DMA, 0);
-	rt2x00_set_field32(&reg, WPDMA_GLO_CFG_ENABLE_RX_DMA, 0);
-	rt2800_register_write(rt2x00dev, WPDMA_GLO_CFG, reg);
+	rt2800_disable_wpdma(rt2x00dev);
 
 	/*
 	 * Initialize firmware.
@@ -823,6 +836,13 @@ const struct rt2x00debug rt2800_rt2x00debug = {
 		.word_size	= sizeof(u32),
 		.word_count	= RF_SIZE / sizeof(u32),
 	},
+	.rfcsr	= {
+		.read		= rt2800_rfcsr_read,
+		.write		= rt2800_rfcsr_write,
+		.word_base	= RFCSR_BASE,
+		.word_size	= sizeof(u8),
+		.word_count	= RFCSR_SIZE / sizeof(u8),
+	},
 };
 EXPORT_SYMBOL_GPL(rt2800_rt2x00debug);
 #endif /* CONFIG_RT2X00_LIB_DEBUGFS */
@@ -2717,13 +2737,7 @@ static int rt2800_init_registers(struct rt2x00_dev *rt2x00dev)
 	unsigned int i;
 	int ret;
 
-	rt2800_register_read(rt2x00dev, WPDMA_GLO_CFG, &reg);
-	rt2x00_set_field32(&reg, WPDMA_GLO_CFG_ENABLE_TX_DMA, 0);
-	rt2x00_set_field32(&reg, WPDMA_GLO_CFG_TX_DMA_BUSY, 0);
-	rt2x00_set_field32(&reg, WPDMA_GLO_CFG_ENABLE_RX_DMA, 0);
-	rt2x00_set_field32(&reg, WPDMA_GLO_CFG_RX_DMA_BUSY, 0);
-	rt2x00_set_field32(&reg, WPDMA_GLO_CFG_TX_WRITEBACK_DONE, 1);
-	rt2800_register_write(rt2x00dev, WPDMA_GLO_CFG, reg);
+	rt2800_disable_wpdma(rt2x00dev);
 
 	ret = rt2800_drv_init_registers(rt2x00dev);
 	if (ret)
@@ -3349,6 +3363,13 @@ static int rt2800_init_bbp(struct rt2x00_dev *rt2x00dev)
 			rt2800_register_write(rt2x00dev, GPIO_CTRL_CFG, reg);
 		}
 
+		/* This chip has hardware antenna diversity*/
+		if (rt2x00_rt_rev_gte(rt2x00dev, RT5390, REV_RT5390R)) {
+			rt2800_bbp_write(rt2x00dev, 150, 0); /* Disable Antenna Software OFDM */
+			rt2800_bbp_write(rt2x00dev, 151, 0); /* Disable Antenna Software CCK */
+			rt2800_bbp_write(rt2x00dev, 154, 0); /* Clear previously selected antenna */
+		}
+
 		rt2800_bbp_read(rt2x00dev, 152, &value);
 		if (ant == 0)
 			rt2x00_set_field8(&value, BBP152_RX_DEFAULT_ANT, 1);
@@ -3997,10 +4018,7 @@ void rt2800_disable_radio(struct rt2x00_dev *rt2x00dev)
 {
 	u32 reg;
 
-	rt2800_register_read(rt2x00dev, WPDMA_GLO_CFG, &reg);
-	rt2x00_set_field32(&reg, WPDMA_GLO_CFG_ENABLE_TX_DMA, 0);
-	rt2x00_set_field32(&reg, WPDMA_GLO_CFG_ENABLE_RX_DMA, 0);
-	rt2800_register_write(rt2x00dev, WPDMA_GLO_CFG, reg);
+	rt2800_disable_wpdma(rt2x00dev);
 
 	/* Wait for DMA, ignore error */
 	rt2800_wait_wpdma_ready(rt2x00dev);
@@ -4287,6 +4305,11 @@ int rt2800_init_eeprom(struct rt2x00_dev *rt2x00dev)
 		rt2x00dev->default_ant.rx = ANTENNA_A;
 	}
 
+	if (rt2x00_rt_rev_gte(rt2x00dev, RT5390, REV_RT5390R)) {
+		rt2x00dev->default_ant.tx = ANTENNA_HW_DIVERSITY; /* Unused */
+		rt2x00dev->default_ant.rx = ANTENNA_HW_DIVERSITY; /* Unused */
+	}
+
 	/*
 	 * Determine external LNA informations.
 	 */
diff --git a/drivers/net/wireless/rt2x00/rt2800lib.h b/drivers/net/wireless/rt2x00/rt2800lib.h
index 419e36cb06b..18a0b67b4c6 100644
--- a/drivers/net/wireless/rt2x00/rt2800lib.h
+++ b/drivers/net/wireless/rt2x00/rt2800lib.h
@@ -208,5 +208,6 @@ int rt2800_ampdu_action(struct ieee80211_hw *hw, struct ieee80211_vif *vif,
 			u8 buf_size);
 int rt2800_get_survey(struct ieee80211_hw *hw, int idx,
 		      struct survey_info *survey);
+void rt2800_disable_wpdma(struct rt2x00_dev *rt2x00dev);
 
 #endif /* RT2800LIB_H */
diff --git a/drivers/net/wireless/rt2x00/rt2800pci.c b/drivers/net/wireless/rt2x00/rt2800pci.c
index 0397bbf0ce0..cad25bfebd7 100644
--- a/drivers/net/wireless/rt2x00/rt2800pci.c
+++ b/drivers/net/wireless/rt2x00/rt2800pci.c
@@ -361,7 +361,6 @@ 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;
-	u32 reg;
 
 	/*
 	 * Initialize registers.
@@ -394,6 +393,16 @@ static int rt2800pci_init_queues(struct rt2x00_dev *rt2x00dev)
 	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);
+
 	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,
@@ -402,14 +411,7 @@ static int rt2800pci_init_queues(struct rt2x00_dev *rt2x00dev)
 				 rt2x00dev->rx[0].limit - 1);
 	rt2x00pci_register_write(rt2x00dev, RX_DRX_IDX, 0);
 
-	/*
-	 * Enable global DMA configuration
-	 */
-	rt2x00pci_register_read(rt2x00dev, WPDMA_GLO_CFG, &reg);
-	rt2x00_set_field32(&reg, WPDMA_GLO_CFG_ENABLE_TX_DMA, 0);
-	rt2x00_set_field32(&reg, WPDMA_GLO_CFG_ENABLE_RX_DMA, 0);
-	rt2x00_set_field32(&reg, WPDMA_GLO_CFG_TX_WRITEBACK_DONE, 1);
-	rt2x00pci_register_write(rt2x00dev, WPDMA_GLO_CFG, reg);
+	rt2800_disable_wpdma(rt2x00dev);
 
 	rt2x00pci_register_write(rt2x00dev, DELAY_INT_CFG, 0);
 
@@ -504,8 +506,10 @@ static int rt2800pci_enable_radio(struct rt2x00_dev *rt2x00dev)
 {
 	int retval;
 
-	if (unlikely(rt2800_wait_wpdma_ready(rt2x00dev) ||
-		     rt2800pci_init_queues(rt2x00dev)))
+	/* Wait for DMA, ignore error until we initialize queues. */
+	rt2800_wait_wpdma_ready(rt2x00dev);
+
+	if (unlikely(rt2800pci_init_queues(rt2x00dev)))
 		return -EIO;
 
 	retval = rt2800_enable_radio(rt2x00dev);
@@ -1184,8 +1188,11 @@ static DEFINE_PCI_DEVICE_TABLE(rt2800pci_device_table) = {
 	{ PCI_DEVICE(0x1814, 0x3593) },
 #endif
 #ifdef CONFIG_RT2800PCI_RT53XX
+	{ PCI_DEVICE(0x1814, 0x5362) },
 	{ PCI_DEVICE(0x1814, 0x5390) },
+	{ PCI_DEVICE(0x1814, 0x5392) },
 	{ PCI_DEVICE(0x1814, 0x539a) },
+	{ PCI_DEVICE(0x1814, 0x539b) },
 	{ PCI_DEVICE(0x1814, 0x539f) },
 #endif
 	{ 0, }
diff --git a/drivers/net/wireless/rt2x00/rt2800usb.c b/drivers/net/wireless/rt2x00/rt2800usb.c
index 001735f7a66..bf78317a6ad 100644
--- a/drivers/net/wireless/rt2x00/rt2800usb.c
+++ b/drivers/net/wireless/rt2x00/rt2800usb.c
@@ -922,6 +922,7 @@ static struct usb_device_id rt2800usb_device_table[] = {
 	{ USB_DEVICE(0x1482, 0x3c09) },
 	/* AirTies */
 	{ USB_DEVICE(0x1eda, 0x2012) },
+	{ USB_DEVICE(0x1eda, 0x2210) },
 	{ USB_DEVICE(0x1eda, 0x2310) },
 	/* Allwin */
 	{ USB_DEVICE(0x8516, 0x2070) },
@@ -991,6 +992,7 @@ static struct usb_device_id rt2800usb_device_table[] = {
 	/* DVICO */
 	{ USB_DEVICE(0x0fe9, 0xb307) },
 	/* Edimax */
+	{ USB_DEVICE(0x7392, 0x4085) },
 	{ USB_DEVICE(0x7392, 0x7711) },
 	{ USB_DEVICE(0x7392, 0x7717) },
 	{ USB_DEVICE(0x7392, 0x7718) },
@@ -1066,6 +1068,7 @@ static struct usb_device_id rt2800usb_device_table[] = {
 	/* Philips */
 	{ USB_DEVICE(0x0471, 0x200f) },
 	/* Planex */
+	{ USB_DEVICE(0x2019, 0x5201) },
 	{ USB_DEVICE(0x2019, 0xab25) },
 	{ USB_DEVICE(0x2019, 0xed06) },
 	/* Quanta */
@@ -1134,6 +1137,10 @@ static struct usb_device_id rt2800usb_device_table[] = {
 #ifdef CONFIG_RT2800USB_RT33XX
 	/* Belkin */
 	{ USB_DEVICE(0x050d, 0x945b) },
+	/* Panasonic */
+	{ USB_DEVICE(0x083a, 0xb511) },
+	/* Philips */
+	{ USB_DEVICE(0x0471, 0x20dd) },
 	/* Ralink */
 	{ USB_DEVICE(0x148f, 0x3370) },
 	{ USB_DEVICE(0x148f, 0x8070) },
@@ -1145,6 +1152,8 @@ static struct usb_device_id rt2800usb_device_table[] = {
 	{ USB_DEVICE(0x8516, 0x3572) },
 	/* Askey */
 	{ USB_DEVICE(0x1690, 0x0744) },
+	{ USB_DEVICE(0x1690, 0x0761) },
+	{ USB_DEVICE(0x1690, 0x0764) },
 	/* Cisco */
 	{ USB_DEVICE(0x167b, 0x4001) },
 	/* EnGenius */
@@ -1159,20 +1168,25 @@ static struct usb_device_id rt2800usb_device_table[] = {
 	/* Sitecom */
 	{ USB_DEVICE(0x0df6, 0x0041) },
 	{ USB_DEVICE(0x0df6, 0x0062) },
+	{ USB_DEVICE(0x0df6, 0x0065) },
+	{ USB_DEVICE(0x0df6, 0x0066) },
+	{ USB_DEVICE(0x0df6, 0x0068) },
 	/* Toshiba */
 	{ USB_DEVICE(0x0930, 0x0a07) },
 	/* Zinwell */
 	{ USB_DEVICE(0x5a57, 0x0284) },
 #endif
 #ifdef CONFIG_RT2800USB_RT53XX
-	/* Alpha */
-	{ USB_DEVICE(0x2001, 0x3c15) },
-	{ USB_DEVICE(0x2001, 0x3c19) },
 	/* Arcadyan */
 	{ USB_DEVICE(0x043e, 0x7a12) },
 	/* Azurewave */
 	{ USB_DEVICE(0x13d3, 0x3329) },
 	{ USB_DEVICE(0x13d3, 0x3365) },
+	/* D-Link */
+	{ USB_DEVICE(0x2001, 0x3c15) },
+	{ USB_DEVICE(0x2001, 0x3c19) },
+	{ USB_DEVICE(0x2001, 0x3c1c) },
+	{ USB_DEVICE(0x2001, 0x3c1d) },
 	/* LG innotek */
 	{ USB_DEVICE(0x043e, 0x7a22) },
 	/* Panasonic */
@@ -1224,12 +1238,8 @@ static struct usb_device_id rt2800usb_device_table[] = {
 	{ USB_DEVICE(0x07d1, 0x3c0b) },
 	{ USB_DEVICE(0x07d1, 0x3c17) },
 	{ USB_DEVICE(0x2001, 0x3c17) },
-	/* Edimax */
-	{ USB_DEVICE(0x7392, 0x4085) },
 	/* Encore */
 	{ USB_DEVICE(0x203d, 0x14a1) },
-	/* Fujitsu Stylistic 550 */
-	{ USB_DEVICE(0x1690, 0x0761) },
 	/* Gemtek */
 	{ USB_DEVICE(0x15a9, 0x0010) },
 	/* Gigabyte */
@@ -1250,7 +1260,6 @@ static struct usb_device_id rt2800usb_device_table[] = {
 	{ USB_DEVICE(0x05a6, 0x0101) },
 	{ USB_DEVICE(0x1d4d, 0x0010) },
 	/* Planex */
-	{ USB_DEVICE(0x2019, 0x5201) },
 	{ USB_DEVICE(0x2019, 0xab24) },
 	/* Qcom */
 	{ USB_DEVICE(0x18e8, 0x6259) },
@@ -1293,6 +1302,7 @@ static struct usb_driver rt2800usb_driver = {
 	.disconnect	= rt2x00usb_disconnect,
 	.suspend	= rt2x00usb_suspend,
 	.resume		= rt2x00usb_resume,
+	.disable_hub_initiated_lpm = 1,
 };
 
 module_usb_driver(rt2800usb_driver);
diff --git a/drivers/net/wireless/rt2x00/rt2x00.h b/drivers/net/wireless/rt2x00/rt2x00.h
index 471f87cab4a..ca36cccaba3 100644
--- a/drivers/net/wireless/rt2x00/rt2x00.h
+++ b/drivers/net/wireless/rt2x00/rt2x00.h
@@ -692,6 +692,8 @@ enum rt2x00_state_flags {
 	 */
 	CONFIG_CHANNEL_HT40,
 	CONFIG_POWERSAVING,
+	CONFIG_HT_DISABLED,
+	CONFIG_QOS_DISABLED,
 
 	/*
 	 * Mark we currently are sequentially reading TX_STA_FIFO register
@@ -1280,7 +1282,7 @@ void rt2x00lib_dmadone(struct queue_entry *entry);
 void rt2x00lib_txdone(struct queue_entry *entry,
 		      struct txdone_entry_desc *txdesc);
 void rt2x00lib_txdone_noinfo(struct queue_entry *entry, u32 status);
-void rt2x00lib_rxdone(struct queue_entry *entry);
+void rt2x00lib_rxdone(struct queue_entry *entry, gfp_t gfp);
 
 /*
  * mac80211 handlers.
diff --git a/drivers/net/wireless/rt2x00/rt2x00config.c b/drivers/net/wireless/rt2x00/rt2x00config.c
index 293676bfa57..e7361d913e8 100644
--- a/drivers/net/wireless/rt2x00/rt2x00config.c
+++ b/drivers/net/wireless/rt2x00/rt2x00config.c
@@ -217,6 +217,11 @@ void rt2x00lib_config(struct rt2x00_dev *rt2x00dev,
 	libconf.conf = conf;
 
 	if (ieee80211_flags & IEEE80211_CONF_CHANGE_CHANNEL) {
+		if (!conf_is_ht(conf))
+			set_bit(CONFIG_HT_DISABLED, &rt2x00dev->flags);
+		else
+			clear_bit(CONFIG_HT_DISABLED, &rt2x00dev->flags);
+
 		if (conf_is_ht40(conf)) {
 			set_bit(CONFIG_CHANNEL_HT40, &rt2x00dev->flags);
 			hw_value = rt2x00ht_center_channel(rt2x00dev, conf);
diff --git a/drivers/net/wireless/rt2x00/rt2x00debug.c b/drivers/net/wireless/rt2x00/rt2x00debug.c
index 78787fcc919..3bb8cafbac5 100644
--- a/drivers/net/wireless/rt2x00/rt2x00debug.c
+++ b/drivers/net/wireless/rt2x00/rt2x00debug.c
@@ -70,6 +70,7 @@ struct rt2x00debug_intf {
 	 *     - eeprom offset/value files
 	 *     - bbp offset/value files
 	 *     - rf offset/value files
+	 *     - rfcsr offset/value files
 	 *   - queue folder
 	 *     - frame dump file
 	 *     - queue stats file
@@ -89,6 +90,8 @@ struct rt2x00debug_intf {
 	struct dentry *bbp_val_entry;
 	struct dentry *rf_off_entry;
 	struct dentry *rf_val_entry;
+	struct dentry *rfcsr_off_entry;
+	struct dentry *rfcsr_val_entry;
 	struct dentry *queue_folder;
 	struct dentry *queue_frame_dump_entry;
 	struct dentry *queue_stats_entry;
@@ -131,6 +134,7 @@ struct rt2x00debug_intf {
 	unsigned int offset_eeprom;
 	unsigned int offset_bbp;
 	unsigned int offset_rf;
+	unsigned int offset_rfcsr;
 };
 
 void rt2x00debug_update_crypto(struct rt2x00_dev *rt2x00dev,
@@ -525,6 +529,7 @@ RT2X00DEBUGFS_OPS(csr, "0x%.8x\n", u32);
 RT2X00DEBUGFS_OPS(eeprom, "0x%.4x\n", u16);
 RT2X00DEBUGFS_OPS(bbp, "0x%.2x\n", u8);
 RT2X00DEBUGFS_OPS(rf, "0x%.8x\n", u32);
+RT2X00DEBUGFS_OPS(rfcsr, "0x%.2x\n", u8);
 
 static ssize_t rt2x00debug_read_dev_flags(struct file *file,
 					  char __user *buf,
@@ -614,7 +619,7 @@ static struct dentry *rt2x00debug_create_file_chipset(const char *name,
 	const struct rt2x00debug *debug = intf->debug;
 	char *data;
 
-	data = kzalloc(8 * MAX_LINE_LENGTH, GFP_KERNEL);
+	data = kzalloc(9 * MAX_LINE_LENGTH, GFP_KERNEL);
 	if (!data)
 		return NULL;
 
@@ -624,22 +629,22 @@ static struct dentry *rt2x00debug_create_file_chipset(const char *name,
 	data += sprintf(data, "revision:\t%04x\n", intf->rt2x00dev->chip.rev);
 	data += sprintf(data, "\n");
 	data += sprintf(data, "register\tbase\twords\twordsize\n");
-	data += sprintf(data, "csr\t%d\t%d\t%d\n",
-			debug->csr.word_base,
-			debug->csr.word_count,
-			debug->csr.word_size);
-	data += sprintf(data, "eeprom\t%d\t%d\t%d\n",
-			debug->eeprom.word_base,
-			debug->eeprom.word_count,
-			debug->eeprom.word_size);
-	data += sprintf(data, "bbp\t%d\t%d\t%d\n",
-			debug->bbp.word_base,
-			debug->bbp.word_count,
-			debug->bbp.word_size);
-	data += sprintf(data, "rf\t%d\t%d\t%d\n",
-			debug->rf.word_base,
-			debug->rf.word_count,
-			debug->rf.word_size);
+#define RT2X00DEBUGFS_SPRINTF_REGISTER(__name)			\
+{								\
+	if(debug->__name.read)					\
+		data += sprintf(data, __stringify(__name)	\
+				"\t%d\t%d\t%d\n",		\
+				debug->__name.word_base,	\
+				debug->__name.word_count,	\
+				debug->__name.word_size);	\
+}
+	RT2X00DEBUGFS_SPRINTF_REGISTER(csr);
+	RT2X00DEBUGFS_SPRINTF_REGISTER(eeprom);
+	RT2X00DEBUGFS_SPRINTF_REGISTER(bbp);
+	RT2X00DEBUGFS_SPRINTF_REGISTER(rf);
+	RT2X00DEBUGFS_SPRINTF_REGISTER(rfcsr);
+#undef RT2X00DEBUGFS_SPRINTF_REGISTER
+
 	blob->size = strlen(blob->data);
 
 	return debugfs_create_blob(name, S_IRUSR, intf->driver_folder, blob);
@@ -694,31 +699,34 @@ void rt2x00debug_register(struct rt2x00_dev *rt2x00dev)
 	if (IS_ERR(intf->register_folder) || !intf->register_folder)
 		goto exit;
 
-#define RT2X00DEBUGFS_CREATE_REGISTER_ENTRY(__intf, __name)	\
-({								\
-	(__intf)->__name##_off_entry =				\
-	    debugfs_create_u32(__stringify(__name) "_offset",	\
-			       S_IRUSR | S_IWUSR,		\
-			       (__intf)->register_folder,	\
-			       &(__intf)->offset_##__name);	\
-	if (IS_ERR((__intf)->__name##_off_entry)		\
-			|| !(__intf)->__name##_off_entry)	\
-		goto exit;					\
-								\
-	(__intf)->__name##_val_entry =				\
-	    debugfs_create_file(__stringify(__name) "_value",	\
-				S_IRUSR | S_IWUSR,		\
-				(__intf)->register_folder,	\
-				(__intf), &rt2x00debug_fop_##__name);\
-	if (IS_ERR((__intf)->__name##_val_entry)		\
-			|| !(__intf)->__name##_val_entry)	\
-		goto exit;					\
+#define RT2X00DEBUGFS_CREATE_REGISTER_ENTRY(__intf, __name)			\
+({										\
+	if(debug->__name.read) {						\
+		(__intf)->__name##_off_entry =					\
+		debugfs_create_u32(__stringify(__name) "_offset",		\
+				       S_IRUSR | S_IWUSR,			\
+				       (__intf)->register_folder,		\
+				       &(__intf)->offset_##__name);		\
+		if (IS_ERR((__intf)->__name##_off_entry)			\
+				|| !(__intf)->__name##_off_entry)		\
+			goto exit;						\
+										\
+		(__intf)->__name##_val_entry =					\
+		debugfs_create_file(__stringify(__name) "_value",		\
+					S_IRUSR | S_IWUSR,			\
+					(__intf)->register_folder,		\
+					(__intf), &rt2x00debug_fop_##__name);	\
+		if (IS_ERR((__intf)->__name##_val_entry)			\
+				|| !(__intf)->__name##_val_entry)		\
+			goto exit;						\
+	}									\
 })
 
 	RT2X00DEBUGFS_CREATE_REGISTER_ENTRY(intf, csr);
 	RT2X00DEBUGFS_CREATE_REGISTER_ENTRY(intf, eeprom);
 	RT2X00DEBUGFS_CREATE_REGISTER_ENTRY(intf, bbp);
 	RT2X00DEBUGFS_CREATE_REGISTER_ENTRY(intf, rf);
+	RT2X00DEBUGFS_CREATE_REGISTER_ENTRY(intf, rfcsr);
 
 #undef RT2X00DEBUGFS_CREATE_REGISTER_ENTRY
 
@@ -770,6 +778,8 @@ void rt2x00debug_deregister(struct rt2x00_dev *rt2x00dev)
 	debugfs_remove(intf->queue_stats_entry);
 	debugfs_remove(intf->queue_frame_dump_entry);
 	debugfs_remove(intf->queue_folder);
+	debugfs_remove(intf->rfcsr_val_entry);
+	debugfs_remove(intf->rfcsr_off_entry);
 	debugfs_remove(intf->rf_val_entry);
 	debugfs_remove(intf->rf_off_entry);
 	debugfs_remove(intf->bbp_val_entry);
diff --git a/drivers/net/wireless/rt2x00/rt2x00debug.h b/drivers/net/wireless/rt2x00/rt2x00debug.h
index fa11409cb5c..e11d39bdfef 100644
--- a/drivers/net/wireless/rt2x00/rt2x00debug.h
+++ b/drivers/net/wireless/rt2x00/rt2x00debug.h
@@ -65,6 +65,7 @@ struct rt2x00debug {
 	RT2X00DEBUGFS_REGISTER_ENTRY(eeprom, u16);
 	RT2X00DEBUGFS_REGISTER_ENTRY(bbp, u8);
 	RT2X00DEBUGFS_REGISTER_ENTRY(rf, u32);
+	RT2X00DEBUGFS_REGISTER_ENTRY(rfcsr, u8);
 };
 
 #endif /* RT2X00DEBUG_H */
diff --git a/drivers/net/wireless/rt2x00/rt2x00dev.c b/drivers/net/wireless/rt2x00/rt2x00dev.c
index 90cc5e77265..e5404e57625 100644
--- a/drivers/net/wireless/rt2x00/rt2x00dev.c
+++ b/drivers/net/wireless/rt2x00/rt2x00dev.c
@@ -391,9 +391,10 @@ void rt2x00lib_txdone(struct queue_entry *entry,
 		tx_info->flags |= IEEE80211_TX_STAT_AMPDU;
 		tx_info->status.ampdu_len = 1;
 		tx_info->status.ampdu_ack_len = success ? 1 : 0;
-
-		if (!success)
-			tx_info->flags |= IEEE80211_TX_STAT_AMPDU_NO_BACK;
+		/*
+		 * TODO: Need to tear down BA session here
+		 * if not successful.
+		 */
 	}
 
 	if (rate_flags & IEEE80211_TX_RC_USE_RTS_CTS) {
@@ -587,7 +588,7 @@ static int rt2x00lib_rxdone_read_signal(struct rt2x00_dev *rt2x00dev,
 	return 0;
 }
 
-void rt2x00lib_rxdone(struct queue_entry *entry)
+void rt2x00lib_rxdone(struct queue_entry *entry, gfp_t gfp)
 {
 	struct rt2x00_dev *rt2x00dev = entry->queue->rt2x00dev;
 	struct rxdone_entry_desc rxdesc;
@@ -607,7 +608,7 @@ void rt2x00lib_rxdone(struct queue_entry *entry)
 	 * Allocate a new sk_buffer. If no new buffer available, drop the
 	 * received frame and reuse the existing buffer.
 	 */
-	skb = rt2x00queue_alloc_rxskb(entry);
+	skb = rt2x00queue_alloc_rxskb(entry, gfp);
 	if (!skb)
 		goto submit_entry;
 
diff --git a/drivers/net/wireless/rt2x00/rt2x00leds.c b/drivers/net/wireless/rt2x00/rt2x00leds.c
index ca585e34d00..8679d781a26 100644
--- a/drivers/net/wireless/rt2x00/rt2x00leds.c
+++ b/drivers/net/wireless/rt2x00/rt2x00leds.c
@@ -124,17 +124,15 @@ static int rt2x00leds_register_led(struct rt2x00_dev *rt2x00dev,
 
 void rt2x00leds_register(struct rt2x00_dev *rt2x00dev)
 {
-	char dev_name[16];
-	char name[32];
+	char name[36];
 	int retval;
 	unsigned long on_period;
 	unsigned long off_period;
-
-	snprintf(dev_name, sizeof(dev_name), "%s-%s",
-		 rt2x00dev->ops->name, wiphy_name(rt2x00dev->hw->wiphy));
+	const char *phy_name = wiphy_name(rt2x00dev->hw->wiphy);
 
 	if (rt2x00dev->led_radio.flags & LED_INITIALIZED) {
-		snprintf(name, sizeof(name), "%s::radio", dev_name);
+		snprintf(name, sizeof(name), "%s-%s::radio",
+			 rt2x00dev->ops->name, phy_name);
 
 		retval = rt2x00leds_register_led(rt2x00dev,
 						 &rt2x00dev->led_radio,
@@ -144,7 +142,8 @@ void rt2x00leds_register(struct rt2x00_dev *rt2x00dev)
 	}
 
 	if (rt2x00dev->led_assoc.flags & LED_INITIALIZED) {
-		snprintf(name, sizeof(name), "%s::assoc", dev_name);
+		snprintf(name, sizeof(name), "%s-%s::assoc",
+			 rt2x00dev->ops->name, phy_name);
 
 		retval = rt2x00leds_register_led(rt2x00dev,
 						 &rt2x00dev->led_assoc,
@@ -154,7 +153,8 @@ void rt2x00leds_register(struct rt2x00_dev *rt2x00dev)
 	}
 
 	if (rt2x00dev->led_qual.flags & LED_INITIALIZED) {
-		snprintf(name, sizeof(name), "%s::quality", dev_name);
+		snprintf(name, sizeof(name), "%s-%s::quality",
+			 rt2x00dev->ops->name, phy_name);
 
 		retval = rt2x00leds_register_led(rt2x00dev,
 						 &rt2x00dev->led_qual,
diff --git a/drivers/net/wireless/rt2x00/rt2x00lib.h b/drivers/net/wireless/rt2x00/rt2x00lib.h
index 78bd43b8961..a0935987fa3 100644
--- a/drivers/net/wireless/rt2x00/rt2x00lib.h
+++ b/drivers/net/wireless/rt2x00/rt2x00lib.h
@@ -103,7 +103,7 @@ void rt2x00lib_config(struct rt2x00_dev *rt2x00dev,
  * rt2x00queue_alloc_rxskb - allocate a skb for RX purposes.
  * @entry: The entry for which the skb will be applicable.
  */
-struct sk_buff *rt2x00queue_alloc_rxskb(struct queue_entry *entry);
+struct sk_buff *rt2x00queue_alloc_rxskb(struct queue_entry *entry, gfp_t gfp);
 
 /**
  * rt2x00queue_free_skb - free a skb
diff --git a/drivers/net/wireless/rt2x00/rt2x00mac.c b/drivers/net/wireless/rt2x00/rt2x00mac.c
index 2df2eb6d3e0..b49773ef72f 100644
--- a/drivers/net/wireless/rt2x00/rt2x00mac.c
+++ b/drivers/net/wireless/rt2x00/rt2x00mac.c
@@ -709,9 +709,19 @@ void rt2x00mac_bss_info_changed(struct ieee80211_hw *hw,
 			rt2x00dev->intf_associated--;
 
 		rt2x00leds_led_assoc(rt2x00dev, !!rt2x00dev->intf_associated);
+
+		clear_bit(CONFIG_QOS_DISABLED, &rt2x00dev->flags);
 	}
 
 	/*
+	 * Check for access point which do not support 802.11e . We have to
+	 * generate data frames sequence number in S/W for such AP, because
+	 * of H/W bug.
+	 */
+	if (changes & BSS_CHANGED_QOS && !bss_conf->qos)
+		set_bit(CONFIG_QOS_DISABLED, &rt2x00dev->flags);
+
+	/*
 	 * When the erp information has changed, we should perform
 	 * additional configuration steps. For all other changes we are done.
 	 */
diff --git a/drivers/net/wireless/rt2x00/rt2x00pci.c b/drivers/net/wireless/rt2x00/rt2x00pci.c
index 17148bb2442..0a4653a92ca 100644
--- a/drivers/net/wireless/rt2x00/rt2x00pci.c
+++ b/drivers/net/wireless/rt2x00/rt2x00pci.c
@@ -92,7 +92,7 @@ bool rt2x00pci_rxdone(struct rt2x00_dev *rt2x00dev)
 		/*
 		 * Send the frame to rt2x00lib for further processing.
 		 */
-		rt2x00lib_rxdone(entry);
+		rt2x00lib_rxdone(entry, GFP_ATOMIC);
 	}
 
 	return !max_rx;
diff --git a/drivers/net/wireless/rt2x00/rt2x00queue.c b/drivers/net/wireless/rt2x00/rt2x00queue.c
index 9b1b2b7a780..4c662eccf53 100644
--- a/drivers/net/wireless/rt2x00/rt2x00queue.c
+++ b/drivers/net/wireless/rt2x00/rt2x00queue.c
@@ -33,7 +33,7 @@
 #include "rt2x00.h"
 #include "rt2x00lib.h"
 
-struct sk_buff *rt2x00queue_alloc_rxskb(struct queue_entry *entry)
+struct sk_buff *rt2x00queue_alloc_rxskb(struct queue_entry *entry, gfp_t gfp)
 {
 	struct rt2x00_dev *rt2x00dev = entry->queue->rt2x00dev;
 	struct sk_buff *skb;
@@ -68,7 +68,7 @@ struct sk_buff *rt2x00queue_alloc_rxskb(struct queue_entry *entry)
 	/*
 	 * Allocate skbuffer.
 	 */
-	skb = dev_alloc_skb(frame_size + head_size + tail_size);
+	skb = __dev_alloc_skb(frame_size + head_size + tail_size, gfp);
 	if (!skb)
 		return NULL;
 
@@ -213,8 +213,19 @@ static void rt2x00queue_create_tx_descriptor_seq(struct rt2x00_dev *rt2x00dev,
 
 	__set_bit(ENTRY_TXD_GENERATE_SEQ, &txdesc->flags);
 
-	if (!test_bit(REQUIRE_SW_SEQNO, &rt2x00dev->cap_flags))
-		return;
+	if (!test_bit(REQUIRE_SW_SEQNO, &rt2x00dev->cap_flags)) {
+		/*
+		 * rt2800 has a H/W (or F/W) bug, device incorrectly increase
+		 * seqno on retransmited data (non-QOS) frames. To workaround
+		 * the problem let's generate seqno in software if QOS is
+		 * disabled.
+		 */
+		if (test_bit(CONFIG_QOS_DISABLED, &rt2x00dev->flags))
+			__clear_bit(ENTRY_TXD_GENERATE_SEQ, &txdesc->flags);
+		else
+			/* H/W will generate sequence number */
+			return;
+	}
 
 	/*
 	 * The hardware is not able to insert a sequence number. Assign a
@@ -320,14 +331,6 @@ static void rt2x00queue_create_tx_descriptor_ht(struct rt2x00_dev *rt2x00dev,
 		txdesc->u.ht.wcid = sta_priv->wcid;
 	}
 
-	txdesc->u.ht.ba_size = 7;	/* FIXME: What value is needed? */
-
-	/*
-	 * Only one STBC stream is supported for now.
-	 */
-	if (tx_info->flags & IEEE80211_TX_CTL_STBC)
-		txdesc->u.ht.stbc = 1;
-
 	/*
 	 * If IEEE80211_TX_RC_MCS is set txrate->idx just contains the
 	 * mcs rate to be used
@@ -351,6 +354,24 @@ static void rt2x00queue_create_tx_descriptor_ht(struct rt2x00_dev *rt2x00dev,
 			txdesc->u.ht.mcs |= 0x08;
 	}
 
+	if (test_bit(CONFIG_HT_DISABLED, &rt2x00dev->flags)) {
+		if (!(tx_info->flags & IEEE80211_TX_CTL_FIRST_FRAGMENT))
+			txdesc->u.ht.txop = TXOP_SIFS;
+		else
+			txdesc->u.ht.txop = TXOP_BACKOFF;
+
+		/* Left zero on all other settings. */
+		return;
+	}
+
+	txdesc->u.ht.ba_size = 7;	/* FIXME: What value is needed? */
+
+	/*
+	 * Only one STBC stream is supported for now.
+	 */
+	if (tx_info->flags & IEEE80211_TX_CTL_STBC)
+		txdesc->u.ht.stbc = 1;
+
 	/*
 	 * This frame is eligible for an AMPDU, however, don't aggregate
 	 * frames that are intended to probe a specific tx rate.
@@ -1142,7 +1163,7 @@ static int rt2x00queue_alloc_rxskbs(struct data_queue *queue)
 	struct sk_buff *skb;
 
 	for (i = 0; i < queue->limit; i++) {
-		skb = rt2x00queue_alloc_rxskb(&queue->entries[i]);
+		skb = rt2x00queue_alloc_rxskb(&queue->entries[i], GFP_KERNEL);
 		if (!skb)
 			return -ENOMEM;
 		queue->entries[i].skb = skb;
diff --git a/drivers/net/wireless/rt2x00/rt2x00usb.c b/drivers/net/wireless/rt2x00/rt2x00usb.c
index 66094eb21b6..d357d1ed92f 100644
--- a/drivers/net/wireless/rt2x00/rt2x00usb.c
+++ b/drivers/net/wireless/rt2x00/rt2x00usb.c
@@ -358,7 +358,7 @@ static void rt2x00usb_work_rxdone(struct work_struct *work)
 		/*
 		 * Send the frame to rt2x00lib for further processing.
 		 */
-		rt2x00lib_rxdone(entry);
+		rt2x00lib_rxdone(entry, GFP_KERNEL);
 	}
 }
 
diff --git a/drivers/net/wireless/rt2x00/rt61pci.c b/drivers/net/wireless/rt2x00/rt61pci.c
index e0c6d117429..ee22bd74579 100644
--- a/drivers/net/wireless/rt2x00/rt61pci.c
+++ b/drivers/net/wireless/rt2x00/rt61pci.c
@@ -3092,15 +3092,4 @@ static struct pci_driver rt61pci_driver = {
 	.resume		= rt2x00pci_resume,
 };
 
-static int __init rt61pci_init(void)
-{
-	return pci_register_driver(&rt61pci_driver);
-}
-
-static void __exit rt61pci_exit(void)
-{
-	pci_unregister_driver(&rt61pci_driver);
-}
-
-module_init(rt61pci_init);
-module_exit(rt61pci_exit);
+module_pci_driver(rt61pci_driver);
diff --git a/drivers/net/wireless/rt2x00/rt73usb.c b/drivers/net/wireless/rt2x00/rt73usb.c
index e477a964081..77ccbbc7da4 100644
--- a/drivers/net/wireless/rt2x00/rt73usb.c
+++ b/drivers/net/wireless/rt2x00/rt73usb.c
@@ -2412,6 +2412,7 @@ static struct usb_device_id rt73usb_device_table[] = {
 	{ USB_DEVICE(0x0b05, 0x1723) },
 	{ USB_DEVICE(0x0b05, 0x1724) },
 	/* Belkin */
+	{ USB_DEVICE(0x050d, 0x7050) },	/* FCC ID: K7SF5D7050B ver. 3.x */
 	{ USB_DEVICE(0x050d, 0x705a) },
 	{ USB_DEVICE(0x050d, 0x905b) },
 	{ USB_DEVICE(0x050d, 0x905c) },
@@ -2526,6 +2527,7 @@ static struct usb_driver rt73usb_driver = {
 	.disconnect	= rt2x00usb_disconnect,
 	.suspend	= rt2x00usb_suspend,
 	.resume		= rt2x00usb_resume,
+	.disable_hub_initiated_lpm = 1,
 };
 
 module_usb_driver(rt73usb_driver);
diff --git a/drivers/net/wireless/rtl818x/rtl8180/dev.c b/drivers/net/wireless/rtl818x/rtl8180/dev.c
index 2f14a5fb0cb..2bebcb71a1e 100644
--- a/drivers/net/wireless/rtl818x/rtl8180/dev.c
+++ b/drivers/net/wireless/rtl818x/rtl8180/dev.c
@@ -1173,15 +1173,4 @@ static struct pci_driver rtl8180_driver = {
 #endif /* CONFIG_PM */
 };
 
-static int __init rtl8180_init(void)
-{
-	return pci_register_driver(&rtl8180_driver);
-}
-
-static void __exit rtl8180_exit(void)
-{
-	pci_unregister_driver(&rtl8180_driver);
-}
-
-module_init(rtl8180_init);
-module_exit(rtl8180_exit);
+module_pci_driver(rtl8180_driver);
diff --git a/drivers/net/wireless/rtl818x/rtl8187/dev.c b/drivers/net/wireless/rtl818x/rtl8187/dev.c
index cf53ac9d6f2..4fb1ca1b86b 100644
--- a/drivers/net/wireless/rtl818x/rtl8187/dev.c
+++ b/drivers/net/wireless/rtl818x/rtl8187/dev.c
@@ -294,6 +294,7 @@ static void rtl8187_tx(struct ieee80211_hw *dev, struct sk_buff *skb)
 		hdr->retry = cpu_to_le32((info->control.rates[0].count - 1) << 8);
 		hdr->tx_duration =
 			ieee80211_generic_frame_duration(dev, priv->vif,
+							 info->band,
 							 skb->len, txrate);
 		buf = hdr;
 
@@ -1662,6 +1663,7 @@ static struct usb_driver rtl8187_driver = {
 	.id_table	= rtl8187_table,
 	.probe		= rtl8187_probe,
 	.disconnect	= __devexit_p(rtl8187_disconnect),
+	.disable_hub_initiated_lpm = 1,
 };
 
 module_usb_driver(rtl8187_driver);
diff --git a/drivers/net/wireless/rtlwifi/base.c b/drivers/net/wireless/rtlwifi/base.c
index e54488db0e1..f4c852c6749 100644
--- a/drivers/net/wireless/rtlwifi/base.c
+++ b/drivers/net/wireless/rtlwifi/base.c
@@ -1460,7 +1460,7 @@ void rtl_recognize_peer(struct ieee80211_hw *hw, u8 *data, unsigned int len)
 		return;
 
 	/* and only beacons from the associated BSSID, please */
-	if (compare_ether_addr(hdr->addr3, rtlpriv->mac80211.bssid))
+	if (!ether_addr_equal(hdr->addr3, rtlpriv->mac80211.bssid))
 		return;
 
 	if (rtl_find_221_ie(hw, data, len))
diff --git a/drivers/net/wireless/rtlwifi/cam.c b/drivers/net/wireless/rtlwifi/cam.c
index 5c7d57947d2..3d8cc4a0c86 100644
--- a/drivers/net/wireless/rtlwifi/cam.c
+++ b/drivers/net/wireless/rtlwifi/cam.c
@@ -328,10 +328,9 @@ void rtl_cam_del_entry(struct ieee80211_hw *hw, u8 *sta_addr)
 		RT_TRACE(rtlpriv, COMP_SEC, DBG_EMERG, "sta_addr is NULL\n");
 	}
 
-	if ((sta_addr[0]|sta_addr[1]|sta_addr[2]|sta_addr[3]|\
-				sta_addr[4]|sta_addr[5]) == 0) {
+	if (is_zero_ether_addr(sta_addr)) {
 		RT_TRACE(rtlpriv, COMP_SEC, DBG_EMERG,
-			 "sta_addr is 00:00:00:00:00:00\n");
+			 "sta_addr is %pM\n", sta_addr);
 		return;
 	}
 	/* Does STA already exist? */
diff --git a/drivers/net/wireless/rtlwifi/pci.c b/drivers/net/wireless/rtlwifi/pci.c
index cc15fdb3606..2062ea1d7c8 100644
--- a/drivers/net/wireless/rtlwifi/pci.c
+++ b/drivers/net/wireless/rtlwifi/pci.c
@@ -34,6 +34,7 @@
 #include "ps.h"
 #include "efuse.h"
 #include <linux/export.h>
+#include <linux/kmemleak.h>
 
 static const u16 pcibridge_vendors[PCI_BRIDGE_VENDOR_MAX] = {
 	PCI_VENDOR_ID_INTEL,
@@ -1099,6 +1100,7 @@ static int _rtl_pci_init_rx_ring(struct ieee80211_hw *hw)
 			u32 bufferaddress;
 			if (!skb)
 				return 0;
+			kmemleak_not_leak(skb);
 			entry = &rtlpci->rx_ring[rx_queue_idx].desc[i];
 
 			/*skb->dev = dev; */
@@ -1851,14 +1853,6 @@ int __devinit rtl_pci_probe(struct pci_dev *pdev,
 	/*like read eeprom and so on */
 	rtlpriv->cfg->ops->read_eeprom_info(hw);
 
-	if (rtlpriv->cfg->ops->init_sw_vars(hw)) {
-		RT_TRACE(rtlpriv, COMP_ERR, DBG_EMERG, "Can't init_sw_vars\n");
-		err = -ENODEV;
-		goto fail3;
-	}
-
-	rtlpriv->cfg->ops->init_sw_leds(hw);
-
 	/*aspm */
 	rtl_pci_init_aspm(hw);
 
@@ -1877,6 +1871,14 @@ int __devinit rtl_pci_probe(struct pci_dev *pdev,
 		goto fail3;
 	}
 
+	if (rtlpriv->cfg->ops->init_sw_vars(hw)) {
+		RT_TRACE(rtlpriv, COMP_ERR, DBG_EMERG, "Can't init_sw_vars\n");
+		err = -ENODEV;
+		goto fail3;
+	}
+
+	rtlpriv->cfg->ops->init_sw_leds(hw);
+
 	err = sysfs_create_group(&pdev->dev.kobj, &rtl_attribute_group);
 	if (err) {
 		RT_TRACE(rtlpriv, COMP_ERR, DBG_EMERG,
diff --git a/drivers/net/wireless/rtlwifi/ps.c b/drivers/net/wireless/rtlwifi/ps.c
index 5b9c3b5e8c9..5ae26647f34 100644
--- a/drivers/net/wireless/rtlwifi/ps.c
+++ b/drivers/net/wireless/rtlwifi/ps.c
@@ -480,7 +480,7 @@ void rtl_swlps_beacon(struct ieee80211_hw *hw, void *data, unsigned int len)
 		return;
 
 	/* and only beacons from the associated BSSID, please */
-	if (compare_ether_addr(hdr->addr3, rtlpriv->mac80211.bssid))
+	if (!ether_addr_equal(hdr->addr3, rtlpriv->mac80211.bssid))
 		return;
 
 	rtlpriv->psc.last_beacon = jiffies;
diff --git a/drivers/net/wireless/rtlwifi/rc.c b/drivers/net/wireless/rtlwifi/rc.c
index c66f08a0524..d5cbf01da8a 100644
--- a/drivers/net/wireless/rtlwifi/rc.c
+++ b/drivers/net/wireless/rtlwifi/rc.c
@@ -225,8 +225,7 @@ static void rtl_rate_init(void *ppriv,
 static void rtl_rate_update(void *ppriv,
 			    struct ieee80211_supported_band *sband,
 			    struct ieee80211_sta *sta, void *priv_sta,
-			    u32 changed,
-			    enum nl80211_channel_type oper_chan_type)
+			    u32 changed)
 {
 }
 
diff --git a/drivers/net/wireless/rtlwifi/rtl8192c/dm_common.c b/drivers/net/wireless/rtlwifi/rtl8192c/dm_common.c
index 1208b753f62..f7f48c7ac85 100644
--- a/drivers/net/wireless/rtlwifi/rtl8192c/dm_common.c
+++ b/drivers/net/wireless/rtlwifi/rtl8192c/dm_common.c
@@ -33,9 +33,6 @@
 #include "../pci.h"
 #include "../base.h"
 
-struct dig_t dm_digtable;
-static struct ps_t dm_pstable;
-
 #define BT_RSSI_STATE_NORMAL_POWER	BIT_OFFSET_LEN_MASK_32(0, 1)
 #define BT_RSSI_STATE_AMDPU_OFF		BIT_OFFSET_LEN_MASK_32(1, 1)
 #define BT_RSSI_STATE_SPECIAL_LOW	BIT_OFFSET_LEN_MASK_32(2, 1)
@@ -163,33 +160,37 @@ static const u8 cckswing_table_ch14[CCK_TABLE_SIZE][8] = {
 
 static void rtl92c_dm_diginit(struct ieee80211_hw *hw)
 {
-	dm_digtable.dig_enable_flag = true;
-	dm_digtable.dig_ext_port_stage = DIG_EXT_PORT_STAGE_MAX;
-	dm_digtable.cur_igvalue = 0x20;
-	dm_digtable.pre_igvalue = 0x0;
-	dm_digtable.cursta_connectctate = DIG_STA_DISCONNECT;
-	dm_digtable.presta_connectstate = DIG_STA_DISCONNECT;
-	dm_digtable.curmultista_connectstate = DIG_MULTISTA_DISCONNECT;
-	dm_digtable.rssi_lowthresh = DM_DIG_THRESH_LOW;
-	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.backoff_val = DM_DIG_BACKOFF_DEFAULT;
-	dm_digtable.backoff_val_range_max = DM_DIG_BACKOFF_MAX;
-	dm_digtable.backoff_val_range_min = DM_DIG_BACKOFF_MIN;
-	dm_digtable.pre_cck_pd_state = CCK_PD_STAGE_MAX;
-	dm_digtable.cur_cck_pd_state = CCK_PD_STAGE_MAX;
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct dig_t *dm_digtable = &rtlpriv->dm_digtable;
+
+	dm_digtable->dig_enable_flag = true;
+	dm_digtable->dig_ext_port_stage = DIG_EXT_PORT_STAGE_MAX;
+	dm_digtable->cur_igvalue = 0x20;
+	dm_digtable->pre_igvalue = 0x0;
+	dm_digtable->cursta_connectctate = DIG_STA_DISCONNECT;
+	dm_digtable->presta_connectstate = DIG_STA_DISCONNECT;
+	dm_digtable->curmultista_connectstate = DIG_MULTISTA_DISCONNECT;
+	dm_digtable->rssi_lowthresh = DM_DIG_THRESH_LOW;
+	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->backoff_val = DM_DIG_BACKOFF_DEFAULT;
+	dm_digtable->backoff_val_range_max = DM_DIG_BACKOFF_MAX;
+	dm_digtable->backoff_val_range_min = DM_DIG_BACKOFF_MIN;
+	dm_digtable->pre_cck_pd_state = CCK_PD_STAGE_MAX;
+	dm_digtable->cur_cck_pd_state = CCK_PD_STAGE_MAX;
 }
 
 static u8 rtl92c_dm_initial_gain_min_pwdb(struct ieee80211_hw *hw)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct dig_t *dm_digtable = &rtlpriv->dm_digtable;
 	long rssi_val_min = 0;
 
-	if ((dm_digtable.curmultista_connectstate == DIG_MULTISTA_CONNECT) &&
-	    (dm_digtable.cursta_connectctate == DIG_STA_CONNECT)) {
+	if ((dm_digtable->curmultista_connectstate == DIG_MULTISTA_CONNECT) &&
+	    (dm_digtable->cursta_connectctate == DIG_STA_CONNECT)) {
 		if (rtlpriv->dm.entry_min_undecoratedsmoothed_pwdb != 0)
 			rssi_val_min =
 			    (rtlpriv->dm.entry_min_undecoratedsmoothed_pwdb >
@@ -198,10 +199,10 @@ static u8 rtl92c_dm_initial_gain_min_pwdb(struct ieee80211_hw *hw)
 			    rtlpriv->dm.entry_min_undecoratedsmoothed_pwdb;
 		else
 			rssi_val_min = rtlpriv->dm.undecorated_smoothed_pwdb;
-	} else if (dm_digtable.cursta_connectctate == DIG_STA_CONNECT ||
-		   dm_digtable.cursta_connectctate == DIG_STA_BEFORE_CONNECT) {
+	} else if (dm_digtable->cursta_connectctate == DIG_STA_CONNECT ||
+		   dm_digtable->cursta_connectctate == DIG_STA_BEFORE_CONNECT) {
 		rssi_val_min = rtlpriv->dm.undecorated_smoothed_pwdb;
-	} else if (dm_digtable.curmultista_connectstate ==
+	} else if (dm_digtable->curmultista_connectstate ==
 		   DIG_MULTISTA_CONNECT) {
 		rssi_val_min = rtlpriv->dm.entry_min_undecoratedsmoothed_pwdb;
 	}
@@ -260,7 +261,8 @@ static void rtl92c_dm_false_alarm_counter_statistics(struct ieee80211_hw *hw)
 static void rtl92c_dm_ctrl_initgain_by_fa(struct ieee80211_hw *hw)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	u8 value_igi = dm_digtable.cur_igvalue;
+	struct dig_t *dm_digtable = &rtlpriv->dm_digtable;
+	u8 value_igi = dm_digtable->cur_igvalue;
 
 	if (rtlpriv->falsealm_cnt.cnt_all < DM_DIG_FA_TH0)
 		value_igi--;
@@ -277,43 +279,44 @@ static void rtl92c_dm_ctrl_initgain_by_fa(struct ieee80211_hw *hw)
 	if (rtlpriv->falsealm_cnt.cnt_all > 10000)
 		value_igi = 0x32;
 
-	dm_digtable.cur_igvalue = value_igi;
+	dm_digtable->cur_igvalue = value_igi;
 	rtl92c_dm_write_dig(hw);
 }
 
 static void rtl92c_dm_ctrl_initgain_by_rssi(struct ieee80211_hw *hw)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct dig_t *dm_digtable = &rtlpriv->dm_digtable;
 
-	if (rtlpriv->falsealm_cnt.cnt_all > dm_digtable.fa_highthresh) {
-		if ((dm_digtable.backoff_val - 2) <
-		    dm_digtable.backoff_val_range_min)
-			dm_digtable.backoff_val =
-			    dm_digtable.backoff_val_range_min;
+	if (rtlpriv->falsealm_cnt.cnt_all > dm_digtable->fa_highthresh) {
+		if ((dm_digtable->backoff_val - 2) <
+		    dm_digtable->backoff_val_range_min)
+			dm_digtable->backoff_val =
+			    dm_digtable->backoff_val_range_min;
 		else
-			dm_digtable.backoff_val -= 2;
-	} else if (rtlpriv->falsealm_cnt.cnt_all < dm_digtable.fa_lowthresh) {
-		if ((dm_digtable.backoff_val + 2) >
-		    dm_digtable.backoff_val_range_max)
-			dm_digtable.backoff_val =
-			    dm_digtable.backoff_val_range_max;
+			dm_digtable->backoff_val -= 2;
+	} else if (rtlpriv->falsealm_cnt.cnt_all < dm_digtable->fa_lowthresh) {
+		if ((dm_digtable->backoff_val + 2) >
+		    dm_digtable->backoff_val_range_max)
+			dm_digtable->backoff_val =
+			    dm_digtable->backoff_val_range_max;
 		else
-			dm_digtable.backoff_val += 2;
+			dm_digtable->backoff_val += 2;
 	}
 
-	if ((dm_digtable.rssi_val_min + 10 - dm_digtable.backoff_val) >
-	    dm_digtable.rx_gain_range_max)
-		dm_digtable.cur_igvalue = dm_digtable.rx_gain_range_max;
-	else if ((dm_digtable.rssi_val_min + 10 -
-		  dm_digtable.backoff_val) < dm_digtable.rx_gain_range_min)
-		dm_digtable.cur_igvalue = dm_digtable.rx_gain_range_min;
+	if ((dm_digtable->rssi_val_min + 10 - dm_digtable->backoff_val) >
+	    dm_digtable->rx_gain_range_max)
+		dm_digtable->cur_igvalue = dm_digtable->rx_gain_range_max;
+	else if ((dm_digtable->rssi_val_min + 10 -
+		  dm_digtable->backoff_val) < dm_digtable->rx_gain_range_min)
+		dm_digtable->cur_igvalue = dm_digtable->rx_gain_range_min;
 	else
-		dm_digtable.cur_igvalue = dm_digtable.rssi_val_min + 10 -
-		    dm_digtable.backoff_val;
+		dm_digtable->cur_igvalue = dm_digtable->rssi_val_min + 10 -
+		    dm_digtable->backoff_val;
 
 	RT_TRACE(rtlpriv, COMP_DIG, DBG_TRACE,
 		 "rssi_val_min = %x backoff_val %x\n",
-		 dm_digtable.rssi_val_min, dm_digtable.backoff_val);
+		 dm_digtable->rssi_val_min, dm_digtable->backoff_val);
 
 	rtl92c_dm_write_dig(hw);
 }
@@ -322,6 +325,7 @@ static void rtl92c_dm_initial_gain_multi_sta(struct ieee80211_hw *hw)
 {
 	static u8 initialized; /* initialized to false */
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct dig_t *dm_digtable = &rtlpriv->dm_digtable;
 	struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
 	long rssi_strength = rtlpriv->dm.entry_min_undecoratedsmoothed_pwdb;
 	bool multi_sta = false;
@@ -330,68 +334,69 @@ static void rtl92c_dm_initial_gain_multi_sta(struct ieee80211_hw *hw)
 		multi_sta = true;
 
 	if (!multi_sta ||
-	    dm_digtable.cursta_connectctate != DIG_STA_DISCONNECT) {
+	    dm_digtable->cursta_connectctate != DIG_STA_DISCONNECT) {
 		initialized = false;
-		dm_digtable.dig_ext_port_stage = DIG_EXT_PORT_STAGE_MAX;
+		dm_digtable->dig_ext_port_stage = DIG_EXT_PORT_STAGE_MAX;
 		return;
 	} else if (initialized == false) {
 		initialized = true;
-		dm_digtable.dig_ext_port_stage = DIG_EXT_PORT_STAGE_0;
-		dm_digtable.cur_igvalue = 0x20;
+		dm_digtable->dig_ext_port_stage = DIG_EXT_PORT_STAGE_0;
+		dm_digtable->cur_igvalue = 0x20;
 		rtl92c_dm_write_dig(hw);
 	}
 
-	if (dm_digtable.curmultista_connectstate == DIG_MULTISTA_CONNECT) {
-		if ((rssi_strength < dm_digtable.rssi_lowthresh) &&
-		    (dm_digtable.dig_ext_port_stage != DIG_EXT_PORT_STAGE_1)) {
+	if (dm_digtable->curmultista_connectstate == DIG_MULTISTA_CONNECT) {
+		if ((rssi_strength < dm_digtable->rssi_lowthresh) &&
+		    (dm_digtable->dig_ext_port_stage != DIG_EXT_PORT_STAGE_1)) {
 
-			if (dm_digtable.dig_ext_port_stage ==
+			if (dm_digtable->dig_ext_port_stage ==
 			    DIG_EXT_PORT_STAGE_2) {
-				dm_digtable.cur_igvalue = 0x20;
+				dm_digtable->cur_igvalue = 0x20;
 				rtl92c_dm_write_dig(hw);
 			}
 
-			dm_digtable.dig_ext_port_stage = DIG_EXT_PORT_STAGE_1;
-		} else if (rssi_strength > dm_digtable.rssi_highthresh) {
-			dm_digtable.dig_ext_port_stage = DIG_EXT_PORT_STAGE_2;
+			dm_digtable->dig_ext_port_stage = DIG_EXT_PORT_STAGE_1;
+		} else if (rssi_strength > dm_digtable->rssi_highthresh) {
+			dm_digtable->dig_ext_port_stage = DIG_EXT_PORT_STAGE_2;
 			rtl92c_dm_ctrl_initgain_by_fa(hw);
 		}
-	} else if (dm_digtable.dig_ext_port_stage != DIG_EXT_PORT_STAGE_0) {
-		dm_digtable.dig_ext_port_stage = DIG_EXT_PORT_STAGE_0;
-		dm_digtable.cur_igvalue = 0x20;
+	} else if (dm_digtable->dig_ext_port_stage != DIG_EXT_PORT_STAGE_0) {
+		dm_digtable->dig_ext_port_stage = DIG_EXT_PORT_STAGE_0;
+		dm_digtable->cur_igvalue = 0x20;
 		rtl92c_dm_write_dig(hw);
 	}
 
 	RT_TRACE(rtlpriv, COMP_DIG, DBG_TRACE,
 		 "curmultista_connectstate = %x dig_ext_port_stage %x\n",
-		 dm_digtable.curmultista_connectstate,
-		 dm_digtable.dig_ext_port_stage);
+		 dm_digtable->curmultista_connectstate,
+		 dm_digtable->dig_ext_port_stage);
 }
 
 static void rtl92c_dm_initial_gain_sta(struct ieee80211_hw *hw)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct dig_t *dm_digtable = &rtlpriv->dm_digtable;
 
 	RT_TRACE(rtlpriv, COMP_DIG, DBG_TRACE,
 		 "presta_connectstate = %x, cursta_connectctate = %x\n",
-		 dm_digtable.presta_connectstate,
-		 dm_digtable.cursta_connectctate);
+		 dm_digtable->presta_connectstate,
+		 dm_digtable->cursta_connectctate);
 
-	if (dm_digtable.presta_connectstate == dm_digtable.cursta_connectctate
-	    || dm_digtable.cursta_connectctate == DIG_STA_BEFORE_CONNECT
-	    || dm_digtable.cursta_connectctate == DIG_STA_CONNECT) {
+	if (dm_digtable->presta_connectstate == dm_digtable->cursta_connectctate
+	    || dm_digtable->cursta_connectctate == DIG_STA_BEFORE_CONNECT
+	    || dm_digtable->cursta_connectctate == DIG_STA_CONNECT) {
 
-		if (dm_digtable.cursta_connectctate != DIG_STA_DISCONNECT) {
-			dm_digtable.rssi_val_min =
+		if (dm_digtable->cursta_connectctate != DIG_STA_DISCONNECT) {
+			dm_digtable->rssi_val_min =
 			    rtl92c_dm_initial_gain_min_pwdb(hw);
 			rtl92c_dm_ctrl_initgain_by_rssi(hw);
 		}
 	} else {
-		dm_digtable.rssi_val_min = 0;
-		dm_digtable.dig_ext_port_stage = DIG_EXT_PORT_STAGE_MAX;
-		dm_digtable.backoff_val = DM_DIG_BACKOFF_DEFAULT;
-		dm_digtable.cur_igvalue = 0x20;
-		dm_digtable.pre_igvalue = 0;
+		dm_digtable->rssi_val_min = 0;
+		dm_digtable->dig_ext_port_stage = DIG_EXT_PORT_STAGE_MAX;
+		dm_digtable->backoff_val = DM_DIG_BACKOFF_DEFAULT;
+		dm_digtable->cur_igvalue = 0x20;
+		dm_digtable->pre_igvalue = 0;
 		rtl92c_dm_write_dig(hw);
 	}
 }
@@ -400,40 +405,41 @@ static void rtl92c_dm_cck_packet_detection_thresh(struct ieee80211_hw *hw)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
 	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
+	struct dig_t *dm_digtable = &rtlpriv->dm_digtable;
 
-	if (dm_digtable.cursta_connectctate == DIG_STA_CONNECT) {
-		dm_digtable.rssi_val_min = rtl92c_dm_initial_gain_min_pwdb(hw);
+	if (dm_digtable->cursta_connectctate == DIG_STA_CONNECT) {
+		dm_digtable->rssi_val_min = rtl92c_dm_initial_gain_min_pwdb(hw);
 
-		if (dm_digtable.pre_cck_pd_state == CCK_PD_STAGE_LowRssi) {
-			if (dm_digtable.rssi_val_min <= 25)
-				dm_digtable.cur_cck_pd_state =
+		if (dm_digtable->pre_cck_pd_state == CCK_PD_STAGE_LowRssi) {
+			if (dm_digtable->rssi_val_min <= 25)
+				dm_digtable->cur_cck_pd_state =
 				    CCK_PD_STAGE_LowRssi;
 			else
-				dm_digtable.cur_cck_pd_state =
+				dm_digtable->cur_cck_pd_state =
 				    CCK_PD_STAGE_HighRssi;
 		} else {
-			if (dm_digtable.rssi_val_min <= 20)
-				dm_digtable.cur_cck_pd_state =
+			if (dm_digtable->rssi_val_min <= 20)
+				dm_digtable->cur_cck_pd_state =
 				    CCK_PD_STAGE_LowRssi;
 			else
-				dm_digtable.cur_cck_pd_state =
+				dm_digtable->cur_cck_pd_state =
 				    CCK_PD_STAGE_HighRssi;
 		}
 	} else {
-		dm_digtable.cur_cck_pd_state = CCK_PD_STAGE_MAX;
+		dm_digtable->cur_cck_pd_state = CCK_PD_STAGE_MAX;
 	}
 
-	if (dm_digtable.pre_cck_pd_state != dm_digtable.cur_cck_pd_state) {
-		if (dm_digtable.cur_cck_pd_state == CCK_PD_STAGE_LowRssi) {
+	if (dm_digtable->pre_cck_pd_state != dm_digtable->cur_cck_pd_state) {
+		if (dm_digtable->cur_cck_pd_state == CCK_PD_STAGE_LowRssi) {
 			if (rtlpriv->falsealm_cnt.cnt_cck_fail > 800)
-				dm_digtable.cur_cck_fa_state =
+				dm_digtable->cur_cck_fa_state =
 				    CCK_FA_STAGE_High;
 			else
-				dm_digtable.cur_cck_fa_state = CCK_FA_STAGE_Low;
+				dm_digtable->cur_cck_fa_state = CCK_FA_STAGE_Low;
 
-			if (dm_digtable.pre_cck_fa_state !=
-			    dm_digtable.cur_cck_fa_state) {
-				if (dm_digtable.cur_cck_fa_state ==
+			if (dm_digtable->pre_cck_fa_state !=
+			    dm_digtable->cur_cck_fa_state) {
+				if (dm_digtable->cur_cck_fa_state ==
 				    CCK_FA_STAGE_Low)
 					rtl_set_bbreg(hw, RCCK0_CCA, MASKBYTE2,
 						      0x83);
@@ -441,8 +447,8 @@ static void rtl92c_dm_cck_packet_detection_thresh(struct ieee80211_hw *hw)
 					rtl_set_bbreg(hw, RCCK0_CCA, MASKBYTE2,
 						      0xcd);
 
-				dm_digtable.pre_cck_fa_state =
-				    dm_digtable.cur_cck_fa_state;
+				dm_digtable->pre_cck_fa_state =
+				    dm_digtable->cur_cck_fa_state;
 			}
 
 			rtl_set_bbreg(hw, RCCK0_SYSTEM, MASKBYTE1, 0x40);
@@ -458,11 +464,11 @@ static void rtl92c_dm_cck_packet_detection_thresh(struct ieee80211_hw *hw)
 				rtl_set_bbreg(hw, RCCK0_FALSEALARMREPORT,
 					      MASKBYTE2, 0xd3);
 		}
-		dm_digtable.pre_cck_pd_state = dm_digtable.cur_cck_pd_state;
+		dm_digtable->pre_cck_pd_state = dm_digtable->cur_cck_pd_state;
 	}
 
 	RT_TRACE(rtlpriv, COMP_DIG, DBG_TRACE, "CCKPDStage=%x\n",
-		 dm_digtable.cur_cck_pd_state);
+		 dm_digtable->cur_cck_pd_state);
 
 	RT_TRACE(rtlpriv, COMP_DIG, DBG_TRACE, "is92C=%x\n",
 		 IS_92C_SERIAL(rtlhal->version));
@@ -470,31 +476,34 @@ static void rtl92c_dm_cck_packet_detection_thresh(struct ieee80211_hw *hw)
 
 static void rtl92c_dm_ctrl_initgain_by_twoport(struct ieee80211_hw *hw)
 {
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct dig_t *dm_digtable = &rtlpriv->dm_digtable;
 	struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
 
 	if (mac->act_scanning)
 		return;
 
 	if (mac->link_state >= MAC80211_LINKED)
-		dm_digtable.cursta_connectctate = DIG_STA_CONNECT;
+		dm_digtable->cursta_connectctate = DIG_STA_CONNECT;
 	else
-		dm_digtable.cursta_connectctate = DIG_STA_DISCONNECT;
+		dm_digtable->cursta_connectctate = DIG_STA_DISCONNECT;
 
 	rtl92c_dm_initial_gain_sta(hw);
 	rtl92c_dm_initial_gain_multi_sta(hw);
 	rtl92c_dm_cck_packet_detection_thresh(hw);
 
-	dm_digtable.presta_connectstate = dm_digtable.cursta_connectctate;
+	dm_digtable->presta_connectstate = dm_digtable->cursta_connectctate;
 
 }
 
 static void rtl92c_dm_dig(struct ieee80211_hw *hw)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct dig_t *dm_digtable = &rtlpriv->dm_digtable;
 
 	if (rtlpriv->dm.dm_initialgain_enable == false)
 		return;
-	if (dm_digtable.dig_enable_flag == false)
+	if (dm_digtable->dig_enable_flag == false)
 		return;
 
 	rtl92c_dm_ctrl_initgain_by_twoport(hw);
@@ -514,23 +523,24 @@ static void rtl92c_dm_init_dynamic_txpower(struct ieee80211_hw *hw)
 void rtl92c_dm_write_dig(struct ieee80211_hw *hw)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct dig_t *dm_digtable = &rtlpriv->dm_digtable;
 
 	RT_TRACE(rtlpriv, COMP_DIG, DBG_LOUD,
 		 "cur_igvalue = 0x%x, pre_igvalue = 0x%x, backoff_val = %d\n",
-		 dm_digtable.cur_igvalue, dm_digtable.pre_igvalue,
-		 dm_digtable.backoff_val);
+		 dm_digtable->cur_igvalue, dm_digtable->pre_igvalue,
+		 dm_digtable->backoff_val);
 
-	dm_digtable.cur_igvalue += 2;
-	if (dm_digtable.cur_igvalue > 0x3f)
-		dm_digtable.cur_igvalue = 0x3f;
+	dm_digtable->cur_igvalue += 2;
+	if (dm_digtable->cur_igvalue > 0x3f)
+		dm_digtable->cur_igvalue = 0x3f;
 
-	if (dm_digtable.pre_igvalue != dm_digtable.cur_igvalue) {
+	if (dm_digtable->pre_igvalue != dm_digtable->cur_igvalue) {
 		rtl_set_bbreg(hw, ROFDM0_XAAGCCORE1, 0x7f,
-			      dm_digtable.cur_igvalue);
+			      dm_digtable->cur_igvalue);
 		rtl_set_bbreg(hw, ROFDM0_XBAGCCORE1, 0x7f,
-			      dm_digtable.cur_igvalue);
+			      dm_digtable->cur_igvalue);
 
-		dm_digtable.pre_igvalue = dm_digtable.cur_igvalue;
+		dm_digtable->pre_igvalue = dm_digtable->cur_igvalue;
 	}
 }
 EXPORT_SYMBOL(rtl92c_dm_write_dig);
@@ -1223,15 +1233,20 @@ static void rtl92c_dm_refresh_rate_adaptive_mask(struct ieee80211_hw *hw)
 
 static void rtl92c_dm_init_dynamic_bb_powersaving(struct ieee80211_hw *hw)
 {
-	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;
+	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;
 }
 
 void rtl92c_dm_rf_saving(struct ieee80211_hw *hw, u8 bforce_in_normal)
 {
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct ps_t *dm_pstable = &rtlpriv->dm_pstable;
 	static u8 initialize;
 	static u32 reg_874, reg_c70, reg_85c, reg_a74;
 
@@ -1251,27 +1266,27 @@ void rtl92c_dm_rf_saving(struct ieee80211_hw *hw, u8 bforce_in_normal)
 	}
 
 	if (!bforce_in_normal) {
-		if (dm_pstable.rssi_val_min != 0) {
-			if (dm_pstable.pre_rfstate == RF_NORMAL) {
-				if (dm_pstable.rssi_val_min >= 30)
-					dm_pstable.cur_rfstate = RF_SAVE;
+		if (dm_pstable->rssi_val_min != 0) {
+			if (dm_pstable->pre_rfstate == RF_NORMAL) {
+				if (dm_pstable->rssi_val_min >= 30)
+					dm_pstable->cur_rfstate = RF_SAVE;
 				else
-					dm_pstable.cur_rfstate = RF_NORMAL;
+					dm_pstable->cur_rfstate = RF_NORMAL;
 			} else {
-				if (dm_pstable.rssi_val_min <= 25)
-					dm_pstable.cur_rfstate = RF_NORMAL;
+				if (dm_pstable->rssi_val_min <= 25)
+					dm_pstable->cur_rfstate = RF_NORMAL;
 				else
-					dm_pstable.cur_rfstate = RF_SAVE;
+					dm_pstable->cur_rfstate = RF_SAVE;
 			}
 		} else {
-			dm_pstable.cur_rfstate = RF_MAX;
+			dm_pstable->cur_rfstate = RF_MAX;
 		}
 	} else {
-		dm_pstable.cur_rfstate = RF_NORMAL;
+		dm_pstable->cur_rfstate = RF_NORMAL;
 	}
 
-	if (dm_pstable.pre_rfstate != dm_pstable.cur_rfstate) {
-		if (dm_pstable.cur_rfstate == RF_SAVE) {
+	if (dm_pstable->pre_rfstate != dm_pstable->cur_rfstate) {
+		if (dm_pstable->cur_rfstate == RF_SAVE) {
 			rtl_set_bbreg(hw, RFPGA0_XCD_RFINTERFACESW,
 				      0x1C0000, 0x2);
 			rtl_set_bbreg(hw, ROFDM0_AGCPARAMETER1, BIT(3), 0);
@@ -1293,7 +1308,7 @@ void rtl92c_dm_rf_saving(struct ieee80211_hw *hw, u8 bforce_in_normal)
 			rtl_set_bbreg(hw, 0x818, BIT(28), 0x0);
 		}
 
-		dm_pstable.pre_rfstate = dm_pstable.cur_rfstate;
+		dm_pstable->pre_rfstate = dm_pstable->cur_rfstate;
 	}
 }
 EXPORT_SYMBOL(rtl92c_dm_rf_saving);
@@ -1301,36 +1316,37 @@ EXPORT_SYMBOL(rtl92c_dm_rf_saving);
 static void rtl92c_dm_dynamic_bb_powersaving(struct ieee80211_hw *hw)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct ps_t *dm_pstable = &rtlpriv->dm_pstable;
 	struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
 	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
 
 	if (((mac->link_state == MAC80211_NOLINK)) &&
 	    (rtlpriv->dm.entry_min_undecoratedsmoothed_pwdb == 0)) {
-		dm_pstable.rssi_val_min = 0;
+		dm_pstable->rssi_val_min = 0;
 		RT_TRACE(rtlpriv, DBG_LOUD, DBG_LOUD, "Not connected to any\n");
 	}
 
 	if (mac->link_state == MAC80211_LINKED) {
 		if (mac->opmode == NL80211_IFTYPE_ADHOC) {
-			dm_pstable.rssi_val_min =
+			dm_pstable->rssi_val_min =
 			    rtlpriv->dm.entry_min_undecoratedsmoothed_pwdb;
 			RT_TRACE(rtlpriv, DBG_LOUD, DBG_LOUD,
 				 "AP Client PWDB = 0x%lx\n",
-				 dm_pstable.rssi_val_min);
+				 dm_pstable->rssi_val_min);
 		} else {
-			dm_pstable.rssi_val_min =
+			dm_pstable->rssi_val_min =
 			    rtlpriv->dm.undecorated_smoothed_pwdb;
 			RT_TRACE(rtlpriv, DBG_LOUD, DBG_LOUD,
 				 "STA Default Port PWDB = 0x%lx\n",
-				 dm_pstable.rssi_val_min);
+				 dm_pstable->rssi_val_min);
 		}
 	} else {
-		dm_pstable.rssi_val_min =
+		dm_pstable->rssi_val_min =
 		    rtlpriv->dm.entry_min_undecoratedsmoothed_pwdb;
 
 		RT_TRACE(rtlpriv, DBG_LOUD, DBG_LOUD,
 			 "AP Ext Port PWDB = 0x%lx\n",
-			 dm_pstable.rssi_val_min);
+			 dm_pstable->rssi_val_min);
 	}
 
 	if (IS_92C_SERIAL(rtlhal->version))
diff --git a/drivers/net/wireless/rtlwifi/rtl8192c/dm_common.h b/drivers/net/wireless/rtlwifi/rtl8192c/dm_common.h
index 2178e376188..518e208c018 100644
--- a/drivers/net/wireless/rtlwifi/rtl8192c/dm_common.h
+++ b/drivers/net/wireless/rtlwifi/rtl8192c/dm_common.h
@@ -91,40 +91,6 @@
 #define TX_POWER_NEAR_FIELD_THRESH_LVL2		74
 #define TX_POWER_NEAR_FIELD_THRESH_LVL1		67
 
-struct ps_t {
-	u8 pre_ccastate;
-	u8 cur_ccasate;
-	u8 pre_rfstate;
-	u8 cur_rfstate;
-	long rssi_val_min;
-};
-
-struct dig_t {
-	u8 dig_enable_flag;
-	u8 dig_ext_port_stage;
-	u32 rssi_lowthresh;
-	u32 rssi_highthresh;
-	u32 fa_lowthresh;
-	u32 fa_highthresh;
-	u8 cursta_connectctate;
-	u8 presta_connectstate;
-	u8 curmultista_connectstate;
-	u8 pre_igvalue;
-	u8 cur_igvalue;
-	char backoff_val;
-	char backoff_val_range_max;
-	char backoff_val_range_min;
-	u8 rx_gain_range_max;
-	u8 rx_gain_range_min;
-	u8 rssi_val_min;
-	u8 pre_cck_pd_state;
-	u8 cur_cck_pd_state;
-	u8 pre_cck_fa_state;
-	u8 cur_cck_fa_state;
-	u8 pre_ccastate;
-	u8 cur_ccasate;
-};
-
 struct swat_t {
 	u8 failure_cnt;
 	u8 try_flag;
@@ -189,7 +155,6 @@ enum dm_dig_connect_e {
 	DIG_CONNECT_MAX
 };
 
-extern struct dig_t dm_digtable;
 void rtl92c_dm_init(struct ieee80211_hw *hw);
 void rtl92c_dm_watchdog(struct ieee80211_hw *hw);
 void rtl92c_dm_write_dig(struct ieee80211_hw *hw);
diff --git a/drivers/net/wireless/rtlwifi/rtl8192c/fw_common.c b/drivers/net/wireless/rtlwifi/rtl8192c/fw_common.c
index c20b3c30f62..692c8ef5ee8 100644
--- a/drivers/net/wireless/rtlwifi/rtl8192c/fw_common.c
+++ b/drivers/net/wireless/rtlwifi/rtl8192c/fw_common.c
@@ -34,6 +34,7 @@
 #include "../rtl8192ce/def.h"
 #include "fw_common.h"
 #include <linux/export.h>
+#include <linux/kmemleak.h>
 
 static void _rtl92c_enable_fw_download(struct ieee80211_hw *hw, bool enable)
 {
@@ -776,6 +777,8 @@ void rtl92c_set_fw_rsvdpagepkt(struct ieee80211_hw *hw, bool dl_finished)
 	skb = dev_alloc_skb(totalpacketlen);
 	if (!skb)
 		return;
+	kmemleak_not_leak(skb);
+
 	memcpy((u8 *) skb_put(skb, totalpacketlen),
 	       &reserved_page_packet, totalpacketlen);
 
diff --git a/drivers/net/wireless/rtlwifi/rtl8192c/phy_common.c b/drivers/net/wireless/rtlwifi/rtl8192c/phy_common.c
index 4c016241f34..cdcad7d9f15 100644
--- a/drivers/net/wireless/rtlwifi/rtl8192c/phy_common.c
+++ b/drivers/net/wireless/rtlwifi/rtl8192c/phy_common.c
@@ -1881,6 +1881,7 @@ void rtl92c_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",
diff --git a/drivers/net/wireless/rtlwifi/rtl8192ce/dm.h b/drivers/net/wireless/rtlwifi/rtl8192ce/dm.h
index 26747fa8600..d4a3d032c7b 100644
--- a/drivers/net/wireless/rtlwifi/rtl8192ce/dm.h
+++ b/drivers/net/wireless/rtlwifi/rtl8192ce/dm.h
@@ -86,40 +86,6 @@
 #define TX_POWER_NEAR_FIELD_THRESH_LVL2		74
 #define TX_POWER_NEAR_FIELD_THRESH_LVL1		67
 
-struct ps_t {
-	u8 pre_ccastate;
-	u8 cur_ccasate;
-	u8 pre_rfstate;
-	u8 cur_rfstate;
-	long rssi_val_min;
-};
-
-struct dig_t {
-	u8 dig_enable_flag;
-	u8 dig_ext_port_stage;
-	u32 rssi_lowthresh;
-	u32 rssi_highthresh;
-	u32 fa_lowthresh;
-	u32 fa_highthresh;
-	u8 cursta_connectctate;
-	u8 presta_connectstate;
-	u8 curmultista_connectstate;
-	u8 pre_igvalue;
-	u8 cur_igvalue;
-	char backoff_val;
-	char backoff_val_range_max;
-	char backoff_val_range_min;
-	u8 rx_gain_range_max;
-	u8 rx_gain_range_min;
-	u8 rssi_val_min;
-	u8 pre_cck_pd_state;
-	u8 cur_cck_pd_state;
-	u8 pre_cck_fa_state;
-	u8 cur_cck_fa_state;
-	u8 pre_ccastate;
-	u8 cur_ccasate;
-};
-
 struct swat_t {
 	u8 failure_cnt;
 	u8 try_flag;
@@ -184,7 +150,6 @@ enum dm_dig_connect_e {
 	DIG_CONNECT_MAX
 };
 
-extern struct dig_t dm_digtable;
 void rtl92c_dm_init(struct ieee80211_hw *hw);
 void rtl92c_dm_watchdog(struct ieee80211_hw *hw);
 void rtl92c_dm_write_dig(struct ieee80211_hw *hw);
diff --git a/drivers/net/wireless/rtlwifi/rtl8192ce/sw.c b/drivers/net/wireless/rtlwifi/rtl8192ce/sw.c
index 2c3b73366cd..3aa927f8b9b 100644
--- a/drivers/net/wireless/rtlwifi/rtl8192ce/sw.c
+++ b/drivers/net/wireless/rtlwifi/rtl8192ce/sw.c
@@ -389,21 +389,4 @@ static struct pci_driver rtl92ce_driver = {
 	.driver.pm = &rtlwifi_pm_ops,
 };
 
-static int __init rtl92ce_module_init(void)
-{
-	int ret;
-
-	ret = pci_register_driver(&rtl92ce_driver);
-	if (ret)
-		RT_ASSERT(false, "No device found\n");
-
-	return ret;
-}
-
-static void __exit rtl92ce_module_exit(void)
-{
-	pci_unregister_driver(&rtl92ce_driver);
-}
-
-module_init(rtl92ce_module_init);
-module_exit(rtl92ce_module_exit);
+module_pci_driver(rtl92ce_driver);
diff --git a/drivers/net/wireless/rtlwifi/rtl8192ce/trx.c b/drivers/net/wireless/rtlwifi/rtl8192ce/trx.c
index 37b13636a77..3af874e6959 100644
--- a/drivers/net/wireless/rtlwifi/rtl8192ce/trx.c
+++ b/drivers/net/wireless/rtlwifi/rtl8192ce/trx.c
@@ -508,14 +508,14 @@ static void _rtl92ce_translate_rx_signal_stuff(struct ieee80211_hw *hw,
 
 	packet_matchbssid =
 	    ((IEEE80211_FTYPE_CTL != type) &&
-	     (!compare_ether_addr(mac->bssid,
-				  (c_fc & IEEE80211_FCTL_TODS) ?
-				  hdr->addr1 : (c_fc & IEEE80211_FCTL_FROMDS) ?
-				  hdr->addr2 : hdr->addr3)) &&
+	     ether_addr_equal(mac->bssid,
+			      (c_fc & IEEE80211_FCTL_TODS) ? hdr->addr1 :
+			      (c_fc & IEEE80211_FCTL_FROMDS) ? hdr->addr2 :
+			      hdr->addr3) &&
 	     (!pstats->hwerror) && (!pstats->crc) && (!pstats->icv));
 
 	packet_toself = packet_matchbssid &&
-	    (!compare_ether_addr(praddr, rtlefuse->dev_addr));
+	     ether_addr_equal(praddr, rtlefuse->dev_addr);
 
 	if (ieee80211_is_beacon(fc))
 		packet_beacon = true;
diff --git a/drivers/net/wireless/rtlwifi/rtl8192ce/trx.h b/drivers/net/wireless/rtlwifi/rtl8192ce/trx.h
index efb9ab27040..c4adb977736 100644
--- a/drivers/net/wireless/rtlwifi/rtl8192ce/trx.h
+++ b/drivers/net/wireless/rtlwifi/rtl8192ce/trx.h
@@ -530,12 +530,7 @@
 	SET_BITS_OFFSET_LE(__pdesc+28, 0, 32, __val)
 
 #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);
+	memset(__pdesc, 0, min_t(size_t, _size, TX_DESC_NEXT_DESC_OFFSET))
 
 struct rx_fwinfo_92c {
 	u8 gain_trsw[4];
diff --git a/drivers/net/wireless/rtlwifi/rtl8192cu/mac.c b/drivers/net/wireless/rtlwifi/rtl8192cu/mac.c
index 025bdc2eba4..7e91c76582e 100644
--- a/drivers/net/wireless/rtlwifi/rtl8192cu/mac.c
+++ b/drivers/net/wireless/rtlwifi/rtl8192cu/mac.c
@@ -1099,14 +1099,14 @@ void rtl92c_translate_rx_signal_stuff(struct ieee80211_hw *hw,
 	praddr = hdr->addr1;
 	packet_matchbssid =
 	    ((IEEE80211_FTYPE_CTL != type) &&
-	     (!compare_ether_addr(mac->bssid,
-			  (cpu_fc & IEEE80211_FCTL_TODS) ?
-			  hdr->addr1 : (cpu_fc & IEEE80211_FCTL_FROMDS) ?
-			  hdr->addr2 : hdr->addr3)) &&
+	     ether_addr_equal(mac->bssid,
+			      (cpu_fc & IEEE80211_FCTL_TODS) ? hdr->addr1 :
+			      (cpu_fc & IEEE80211_FCTL_FROMDS) ? hdr->addr2 :
+			      hdr->addr3) &&
 	     (!pstats->hwerror) && (!pstats->crc) && (!pstats->icv));
 
 	packet_toself = packet_matchbssid &&
-	    (!compare_ether_addr(praddr, rtlefuse->dev_addr));
+	    ether_addr_equal(praddr, rtlefuse->dev_addr);
 	if (ieee80211_is_beacon(fc))
 		packet_beacon = true;
 	_rtl92c_query_rxphystatus(hw, pstats, pdesc, p_drvinfo,
diff --git a/drivers/net/wireless/rtlwifi/rtl8192cu/sw.c b/drivers/net/wireless/rtlwifi/rtl8192cu/sw.c
index 82c85286ab2..d228358e6a4 100644
--- a/drivers/net/wireless/rtlwifi/rtl8192cu/sw.c
+++ b/drivers/net/wireless/rtlwifi/rtl8192cu/sw.c
@@ -338,6 +338,7 @@ static struct usb_device_id rtl8192c_usb_ids[] = {
 	{RTL_USB_DEVICE(0x2019, 0x1201, rtl92cu_hal_cfg)}, /*Planex-Vencer*/
 
 	/****** 8192CU ********/
+	{RTL_USB_DEVICE(0x050d, 0x1004, rtl92cu_hal_cfg)}, /*Belcom-SurfN300*/
 	{RTL_USB_DEVICE(0x050d, 0x2102, rtl92cu_hal_cfg)}, /*Belcom-Sercomm*/
 	{RTL_USB_DEVICE(0x050d, 0x2103, rtl92cu_hal_cfg)}, /*Belcom-Edimax*/
 	{RTL_USB_DEVICE(0x0586, 0x341f, rtl92cu_hal_cfg)}, /*Zyxel -Abocom*/
@@ -372,6 +373,7 @@ static struct usb_driver rtl8192cu_driver = {
 #ifdef CONFIG_AUTOSUSPEND
 	.supports_autosuspend = 1,
 #endif
+	.disable_hub_initiated_lpm = 1,
 };
 
 module_usb_driver(rtl8192cu_driver);
diff --git a/drivers/net/wireless/rtlwifi/rtl8192de/def.h b/drivers/net/wireless/rtlwifi/rtl8192de/def.h
index eafdf76ed64..939c905f547 100644
--- a/drivers/net/wireless/rtlwifi/rtl8192de/def.h
+++ b/drivers/net/wireless/rtlwifi/rtl8192de/def.h
@@ -151,9 +151,6 @@ enum version_8192d {
 
 /* for 92D */
 #define CHIP_92D_SINGLEPHY		BIT(9)
-#define C_CUT_VERSION			BIT(13)
-#define D_CUT_VERSION			((BIT(12)|BIT(13)))
-#define E_CUT_VERSION			BIT(14)
 
 /* Chip specific */
 #define CHIP_BONDING_IDENTIFIER(_value)	(((_value)>>22)&0x3)
@@ -173,7 +170,10 @@ enum version_8192d {
 #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 CHIP_92D_B_CUT			BIT(12)
+#define CHIP_92D_C_CUT			BIT(13)
+#define CHIP_92D_D_CUT			(BIT(13)|BIT(12))
+#define CHIP_92D_E_CUT			BIT(14)
 
 /* MASK */
 #define IC_TYPE_MASK			(BIT(0)|BIT(1)|BIT(2))
@@ -205,15 +205,13 @@ enum version_8192d {
 					 CHIP_92D) ? true : false)
 #define IS_92D_C_CUT(version)		((IS_92D(version)) ?		\
 				 ((GET_CVID_CUT_VERSION(version) ==	\
-				 0x2000) ? true : false) : false)
+				 CHIP_92D_C_CUT) ? true : false) : false)
 #define IS_92D_D_CUT(version)			((IS_92D(version)) ?	\
 				 ((GET_CVID_CUT_VERSION(version) ==	\
-				 0x3000) ? true : false) : false)
+				 CHIP_92D_D_CUT) ? true : false) : false)
 #define IS_92D_E_CUT(version)		((IS_92D(version)) ?		\
 				 ((GET_CVID_CUT_VERSION(version) ==	\
-				 0x4000) ? true : false) : false)
-#define CHIP_92D_C_CUT			BIT(10)
-#define CHIP_92D_D_CUT			BIT(11)
+				 CHIP_92D_E_CUT) ? true : false) : false)
 
 enum rf_optype {
 	RF_OP_BY_SW_3WIRE = 0,
diff --git a/drivers/net/wireless/rtlwifi/rtl8192de/dm.c b/drivers/net/wireless/rtlwifi/rtl8192de/dm.c
index 4737018c9da..a7d63a84551 100644
--- a/drivers/net/wireless/rtlwifi/rtl8192de/dm.c
+++ b/drivers/net/wireless/rtlwifi/rtl8192de/dm.c
@@ -37,8 +37,6 @@
 
 #define UNDEC_SM_PWDB	entry_min_undecoratedsmoothed_pwdb
 
-struct dig_t de_digtable;
-
 static const u32 ofdmswing_table[OFDM_TABLE_SIZE_92D] = {
 	0x7f8001fe,		/* 0, +6.0dB */
 	0x788001e2,		/* 1, +5.5dB */
@@ -159,27 +157,30 @@ static const u8 cckswing_table_ch14[CCK_TABLE_SIZE][8] = {
 
 static void rtl92d_dm_diginit(struct ieee80211_hw *hw)
 {
-	de_digtable.dig_enable_flag = true;
-	de_digtable.dig_ext_port_stage = DIG_EXT_PORT_STAGE_MAX;
-	de_digtable.cur_igvalue = 0x20;
-	de_digtable.pre_igvalue = 0x0;
-	de_digtable.cursta_connectctate = DIG_STA_DISCONNECT;
-	de_digtable.presta_connectstate = DIG_STA_DISCONNECT;
-	de_digtable.curmultista_connectstate = DIG_MULTISTA_DISCONNECT;
-	de_digtable.rssi_lowthresh = DM_DIG_THRESH_LOW;
-	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.backoff_val = DM_DIG_BACKOFF_DEFAULT;
-	de_digtable.backoff_val_range_max = DM_DIG_BACKOFF_MAX;
-	de_digtable.backoff_val_range_min = DM_DIG_BACKOFF_MIN;
-	de_digtable.pre_cck_pd_state = CCK_PD_STAGE_LOWRSSI;
-	de_digtable.cur_cck_pd_state = CCK_PD_STAGE_MAX;
-	de_digtable.large_fa_hit = 0;
-	de_digtable.recover_cnt = 0;
-	de_digtable.forbidden_igi = DM_DIG_FA_LOWER;
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct dig_t *de_digtable = &rtlpriv->dm_digtable;
+
+	de_digtable->dig_enable_flag = true;
+	de_digtable->dig_ext_port_stage = DIG_EXT_PORT_STAGE_MAX;
+	de_digtable->cur_igvalue = 0x20;
+	de_digtable->pre_igvalue = 0x0;
+	de_digtable->cursta_connectctate = DIG_STA_DISCONNECT;
+	de_digtable->presta_connectstate = DIG_STA_DISCONNECT;
+	de_digtable->curmultista_connectstate = DIG_MULTISTA_DISCONNECT;
+	de_digtable->rssi_lowthresh = DM_DIG_THRESH_LOW;
+	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->backoff_val = DM_DIG_BACKOFF_DEFAULT;
+	de_digtable->backoff_val_range_max = DM_DIG_BACKOFF_MAX;
+	de_digtable->backoff_val_range_min = DM_DIG_BACKOFF_MIN;
+	de_digtable->pre_cck_pd_state = CCK_PD_STAGE_LOWRSSI;
+	de_digtable->cur_cck_pd_state = CCK_PD_STAGE_MAX;
+	de_digtable->large_fa_hit = 0;
+	de_digtable->recover_cnt = 0;
+	de_digtable->forbidden_igi = DM_DIG_FA_LOWER;
 }
 
 static void rtl92d_dm_false_alarm_counter_statistics(struct ieee80211_hw *hw)
@@ -266,68 +267,70 @@ static void rtl92d_dm_false_alarm_counter_statistics(struct ieee80211_hw *hw)
 static void rtl92d_dm_find_minimum_rssi(struct ieee80211_hw *hw)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct dig_t *de_digtable = &rtlpriv->dm_digtable;
 	struct rtl_mac *mac = rtl_mac(rtlpriv);
 
 	/* Determine the minimum RSSI  */
 	if ((mac->link_state < MAC80211_LINKED) &&
 	    (rtlpriv->dm.UNDEC_SM_PWDB == 0)) {
-		de_digtable.min_undecorated_pwdb_for_dm = 0;
+		de_digtable->min_undecorated_pwdb_for_dm = 0;
 		RT_TRACE(rtlpriv, COMP_BB_POWERSAVING, DBG_LOUD,
 			 "Not connected to any\n");
 	}
 	if (mac->link_state >= MAC80211_LINKED) {
 		if (mac->opmode == NL80211_IFTYPE_AP ||
 		    mac->opmode == NL80211_IFTYPE_ADHOC) {
-			de_digtable.min_undecorated_pwdb_for_dm =
+			de_digtable->min_undecorated_pwdb_for_dm =
 			    rtlpriv->dm.UNDEC_SM_PWDB;
 			RT_TRACE(rtlpriv, COMP_BB_POWERSAVING, DBG_LOUD,
 				 "AP Client PWDB = 0x%lx\n",
 				 rtlpriv->dm.UNDEC_SM_PWDB);
 		} else {
-			de_digtable.min_undecorated_pwdb_for_dm =
+			de_digtable->min_undecorated_pwdb_for_dm =
 			    rtlpriv->dm.undecorated_smoothed_pwdb;
 			RT_TRACE(rtlpriv, COMP_BB_POWERSAVING, DBG_LOUD,
 				 "STA Default Port PWDB = 0x%x\n",
-				 de_digtable.min_undecorated_pwdb_for_dm);
+				 de_digtable->min_undecorated_pwdb_for_dm);
 		}
 	} else {
-		de_digtable.min_undecorated_pwdb_for_dm =
+		de_digtable->min_undecorated_pwdb_for_dm =
 		    rtlpriv->dm.UNDEC_SM_PWDB;
 		RT_TRACE(rtlpriv, COMP_BB_POWERSAVING, DBG_LOUD,
 			 "AP Ext Port or disconnect PWDB = 0x%x\n",
-			 de_digtable.min_undecorated_pwdb_for_dm);
+			 de_digtable->min_undecorated_pwdb_for_dm);
 	}
 
 	RT_TRACE(rtlpriv, COMP_DIG, DBG_LOUD, "MinUndecoratedPWDBForDM =%d\n",
-		 de_digtable.min_undecorated_pwdb_for_dm);
+		 de_digtable->min_undecorated_pwdb_for_dm);
 }
 
 static void rtl92d_dm_cck_packet_detection_thresh(struct ieee80211_hw *hw)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct dig_t *de_digtable = &rtlpriv->dm_digtable;
 	unsigned long flag = 0;
 
-	if (de_digtable.cursta_connectctate == DIG_STA_CONNECT) {
-		if (de_digtable.pre_cck_pd_state == CCK_PD_STAGE_LOWRSSI) {
-			if (de_digtable.min_undecorated_pwdb_for_dm <= 25)
-				de_digtable.cur_cck_pd_state =
+	if (de_digtable->cursta_connectctate == DIG_STA_CONNECT) {
+		if (de_digtable->pre_cck_pd_state == CCK_PD_STAGE_LOWRSSI) {
+			if (de_digtable->min_undecorated_pwdb_for_dm <= 25)
+				de_digtable->cur_cck_pd_state =
 							 CCK_PD_STAGE_LOWRSSI;
 			else
-				de_digtable.cur_cck_pd_state =
+				de_digtable->cur_cck_pd_state =
 							 CCK_PD_STAGE_HIGHRSSI;
 		} else {
-			if (de_digtable.min_undecorated_pwdb_for_dm <= 20)
-				de_digtable.cur_cck_pd_state =
+			if (de_digtable->min_undecorated_pwdb_for_dm <= 20)
+				de_digtable->cur_cck_pd_state =
 							 CCK_PD_STAGE_LOWRSSI;
 			else
-				de_digtable.cur_cck_pd_state =
+				de_digtable->cur_cck_pd_state =
 							 CCK_PD_STAGE_HIGHRSSI;
 		}
 	} else {
-		de_digtable.cur_cck_pd_state = CCK_PD_STAGE_LOWRSSI;
+		de_digtable->cur_cck_pd_state = CCK_PD_STAGE_LOWRSSI;
 	}
-	if (de_digtable.pre_cck_pd_state != de_digtable.cur_cck_pd_state) {
-		if (de_digtable.cur_cck_pd_state == CCK_PD_STAGE_LOWRSSI) {
+	if (de_digtable->pre_cck_pd_state != de_digtable->cur_cck_pd_state) {
+		if (de_digtable->cur_cck_pd_state == CCK_PD_STAGE_LOWRSSI) {
 			rtl92d_acquire_cckandrw_pagea_ctl(hw, &flag);
 			rtl_set_bbreg(hw, RCCK0_CCA, BMASKBYTE2, 0x83);
 			rtl92d_release_cckandrw_pagea_ctl(hw, &flag);
@@ -336,13 +339,13 @@ static void rtl92d_dm_cck_packet_detection_thresh(struct ieee80211_hw *hw)
 			rtl_set_bbreg(hw, RCCK0_CCA, BMASKBYTE2, 0xcd);
 			rtl92d_release_cckandrw_pagea_ctl(hw, &flag);
 		}
-		de_digtable.pre_cck_pd_state = de_digtable.cur_cck_pd_state;
+		de_digtable->pre_cck_pd_state = de_digtable->cur_cck_pd_state;
 	}
 	RT_TRACE(rtlpriv, COMP_DIG, DBG_LOUD, "CurSTAConnectState=%s\n",
-		 de_digtable.cursta_connectctate == DIG_STA_CONNECT ?
+		 de_digtable->cursta_connectctate == DIG_STA_CONNECT ?
 		 "DIG_STA_CONNECT " : "DIG_STA_DISCONNECT");
 	RT_TRACE(rtlpriv, COMP_DIG, DBG_LOUD, "CCKPDStage=%s\n",
-		 de_digtable.cur_cck_pd_state == CCK_PD_STAGE_LOWRSSI ?
+		 de_digtable->cur_cck_pd_state == CCK_PD_STAGE_LOWRSSI ?
 		 "Low RSSI " : "High RSSI ");
 	RT_TRACE(rtlpriv, COMP_DIG, DBG_LOUD, "is92d single phy =%x\n",
 		 IS_92D_SINGLEPHY(rtlpriv->rtlhal.version));
@@ -352,37 +355,40 @@ static void rtl92d_dm_cck_packet_detection_thresh(struct ieee80211_hw *hw)
 void rtl92d_dm_write_dig(struct ieee80211_hw *hw)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct dig_t *de_digtable = &rtlpriv->dm_digtable;
 
 	RT_TRACE(rtlpriv, COMP_DIG, DBG_LOUD,
 		 "cur_igvalue = 0x%x, pre_igvalue = 0x%x, backoff_val = %d\n",
-		 de_digtable.cur_igvalue, de_digtable.pre_igvalue,
-		 de_digtable.backoff_val);
-	if (de_digtable.dig_enable_flag == false) {
+		 de_digtable->cur_igvalue, de_digtable->pre_igvalue,
+		 de_digtable->backoff_val);
+	if (de_digtable->dig_enable_flag == false) {
 		RT_TRACE(rtlpriv, COMP_DIG, DBG_LOUD, "DIG is disabled\n");
-		de_digtable.pre_igvalue = 0x17;
+		de_digtable->pre_igvalue = 0x17;
 		return;
 	}
-	if (de_digtable.pre_igvalue != de_digtable.cur_igvalue) {
+	if (de_digtable->pre_igvalue != de_digtable->cur_igvalue) {
 		rtl_set_bbreg(hw, ROFDM0_XAAGCCORE1, 0x7f,
-			      de_digtable.cur_igvalue);
+			      de_digtable->cur_igvalue);
 		rtl_set_bbreg(hw, ROFDM0_XBAGCCORE1, 0x7f,
-			      de_digtable.cur_igvalue);
-		de_digtable.pre_igvalue = de_digtable.cur_igvalue;
+			      de_digtable->cur_igvalue);
+		de_digtable->pre_igvalue = de_digtable->cur_igvalue;
 	}
 }
 
 static void rtl92d_early_mode_enabled(struct rtl_priv *rtlpriv)
 {
+	struct dig_t *de_digtable = &rtlpriv->dm_digtable;
+
 	if ((rtlpriv->mac80211.link_state >= MAC80211_LINKED) &&
 	    (rtlpriv->mac80211.vendor == PEER_CISCO)) {
 		RT_TRACE(rtlpriv, COMP_DIG, DBG_LOUD, "IOT_PEER = CISCO\n");
-		if (de_digtable.last_min_undecorated_pwdb_for_dm >= 50
-		    && de_digtable.min_undecorated_pwdb_for_dm < 50) {
+		if (de_digtable->last_min_undecorated_pwdb_for_dm >= 50
+		    && de_digtable->min_undecorated_pwdb_for_dm < 50) {
 			rtl_write_byte(rtlpriv, REG_EARLY_MODE_CONTROL, 0x00);
 			RT_TRACE(rtlpriv, COMP_DIG, DBG_LOUD,
 				 "Early Mode Off\n");
-		} else if (de_digtable.last_min_undecorated_pwdb_for_dm <= 55 &&
-			   de_digtable.min_undecorated_pwdb_for_dm > 55) {
+		} else if (de_digtable->last_min_undecorated_pwdb_for_dm <= 55 &&
+			   de_digtable->min_undecorated_pwdb_for_dm > 55) {
 			rtl_write_byte(rtlpriv, REG_EARLY_MODE_CONTROL, 0x0f);
 			RT_TRACE(rtlpriv, COMP_DIG, DBG_LOUD,
 				 "Early Mode On\n");
@@ -396,14 +402,15 @@ static void rtl92d_early_mode_enabled(struct rtl_priv *rtlpriv)
 static void rtl92d_dm_dig(struct ieee80211_hw *hw)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	u8 value_igi = de_digtable.cur_igvalue;
+	struct dig_t *de_digtable = &rtlpriv->dm_digtable;
+	u8 value_igi = de_digtable->cur_igvalue;
 	struct false_alarm_statistics *falsealm_cnt = &(rtlpriv->falsealm_cnt);
 
 	RT_TRACE(rtlpriv, COMP_DIG, DBG_LOUD, "==>\n");
 	if (rtlpriv->rtlhal.earlymode_enable) {
 		rtl92d_early_mode_enabled(rtlpriv);
-		de_digtable.last_min_undecorated_pwdb_for_dm =
-				 de_digtable.min_undecorated_pwdb_for_dm;
+		de_digtable->last_min_undecorated_pwdb_for_dm =
+				 de_digtable->min_undecorated_pwdb_for_dm;
 	}
 	if (!rtlpriv->dm.dm_initialgain_enable)
 		return;
@@ -421,9 +428,9 @@ static void rtl92d_dm_dig(struct ieee80211_hw *hw)
 	RT_TRACE(rtlpriv, COMP_DIG, DBG_LOUD, "progress\n");
 	/* Decide the current status and if modify initial gain or not */
 	if (rtlpriv->mac80211.link_state >= MAC80211_LINKED)
-		de_digtable.cursta_connectctate = DIG_STA_CONNECT;
+		de_digtable->cursta_connectctate = DIG_STA_CONNECT;
 	else
-		de_digtable.cursta_connectctate = DIG_STA_DISCONNECT;
+		de_digtable->cursta_connectctate = DIG_STA_DISCONNECT;
 
 	/* adjust initial gain according to false alarm counter */
 	if (falsealm_cnt->cnt_all < DM_DIG_FA_TH0)
@@ -436,64 +443,64 @@ static void rtl92d_dm_dig(struct ieee80211_hw *hw)
 		value_igi += 2;
 	RT_TRACE(rtlpriv, COMP_DIG, DBG_LOUD,
 		 "dm_DIG() Before: large_fa_hit=%d, forbidden_igi=%x\n",
-		 de_digtable.large_fa_hit, de_digtable.forbidden_igi);
+		 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);
+		 de_digtable->recover_cnt, de_digtable->rx_gain_range_min);
 
 	/* deal with abnorally large false alarm */
 	if (falsealm_cnt->cnt_all > 10000) {
 		RT_TRACE(rtlpriv, COMP_DIG, DBG_LOUD,
 			 "dm_DIG(): Abnormally false alarm case\n");
 
-		de_digtable.large_fa_hit++;
-		if (de_digtable.forbidden_igi < de_digtable.cur_igvalue) {
-			de_digtable.forbidden_igi = de_digtable.cur_igvalue;
-			de_digtable.large_fa_hit = 1;
+		de_digtable->large_fa_hit++;
+		if (de_digtable->forbidden_igi < de_digtable->cur_igvalue) {
+			de_digtable->forbidden_igi = de_digtable->cur_igvalue;
+			de_digtable->large_fa_hit = 1;
 		}
-		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;
+		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;
 			else
-				de_digtable.rx_gain_range_min =
-				    (de_digtable.forbidden_igi + 1);
-			de_digtable.recover_cnt = 3600;	/* 3600=2hr */
+				de_digtable->rx_gain_range_min =
+				    (de_digtable->forbidden_igi + 1);
+			de_digtable->recover_cnt = 3600;	/* 3600=2hr */
 		}
 	} else {
 		/* Recovery mechanism for IGI lower bound */
-		if (de_digtable.recover_cnt != 0) {
-			de_digtable.recover_cnt--;
+		if (de_digtable->recover_cnt != 0) {
+			de_digtable->recover_cnt--;
 		} else {
-			if (de_digtable.large_fa_hit == 0) {
-				if ((de_digtable.forbidden_igi - 1) <
+			if (de_digtable->large_fa_hit == 0) {
+				if ((de_digtable->forbidden_igi - 1) <
 				    DM_DIG_FA_LOWER) {
-					de_digtable.forbidden_igi =
+					de_digtable->forbidden_igi =
 							 DM_DIG_FA_LOWER;
-					de_digtable.rx_gain_range_min =
+					de_digtable->rx_gain_range_min =
 							 DM_DIG_FA_LOWER;
 
 				} else {
-					de_digtable.forbidden_igi--;
-					de_digtable.rx_gain_range_min =
-					    (de_digtable.forbidden_igi + 1);
+					de_digtable->forbidden_igi--;
+					de_digtable->rx_gain_range_min =
+					    (de_digtable->forbidden_igi + 1);
 				}
-			} else if (de_digtable.large_fa_hit == 3) {
-				de_digtable.large_fa_hit = 0;
+			} else if (de_digtable->large_fa_hit == 3) {
+				de_digtable->large_fa_hit = 0;
 			}
 		}
 	}
 	RT_TRACE(rtlpriv, COMP_DIG, DBG_LOUD,
 		 "dm_DIG() After: large_fa_hit=%d, forbidden_igi=%x\n",
-		 de_digtable.large_fa_hit, de_digtable.forbidden_igi);
+		 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);
+		 de_digtable->recover_cnt, de_digtable->rx_gain_range_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;
-	de_digtable.cur_igvalue = value_igi;
+	else if (value_igi < de_digtable->rx_gain_range_min)
+		value_igi = de_digtable->rx_gain_range_min;
+	de_digtable->cur_igvalue = value_igi;
 	rtl92d_dm_write_dig(hw);
 	if (rtlpriv->rtlhal.current_bandtype != BAND_ON_5G)
 		rtl92d_dm_cck_packet_detection_thresh(hw);
diff --git a/drivers/net/wireless/rtlwifi/rtl8192de/dm.h b/drivers/net/wireless/rtlwifi/rtl8192de/dm.h
index 91030ec8ac3..3fea0c11c24 100644
--- a/drivers/net/wireless/rtlwifi/rtl8192de/dm.h
+++ b/drivers/net/wireless/rtlwifi/rtl8192de/dm.h
@@ -87,55 +87,6 @@
 #define TX_POWER_NEAR_FIELD_THRESH_LVL1		67
 #define INDEX_MAPPING_NUM			13
 
-struct ps_t {
-	u8 pre_ccastate;
-	u8 cur_ccasate;
-
-	u8 pre_rfstate;
-	u8 cur_rfstate;
-
-	long rssi_val_min;
-};
-
-struct dig_t {
-	u8 dig_enable_flag;
-	u8 dig_ext_port_stage;
-
-	u32 rssi_lowthresh;
-	u32 rssi_highthresh;
-
-	u32 fa_lowthresh;
-	u32 fa_highthresh;
-
-	u8 cursta_connectctate;
-	u8 presta_connectstate;
-	u8 curmultista_connectstate;
-
-	u8 pre_igvalue;
-	u8 cur_igvalue;
-
-	char backoff_val;
-	char backoff_val_range_max;
-	char backoff_val_range_min;
-	u8 rx_gain_range_max;
-	u8 rx_gain_range_min;
-	u8 min_undecorated_pwdb_for_dm;
-	long last_min_undecorated_pwdb_for_dm;
-
-	u8 pre_cck_pd_state;
-	u8 cur_cck_pd_state;
-
-	u8 pre_cck_fa_state;
-	u8 cur_cck_fa_state;
-
-	u8 pre_ccastate;
-	u8 cur_ccasate;
-
-	u8 large_fa_hit;
-	u8 forbidden_igi;
-	u32 recover_cnt;
-};
-
 struct swat {
 	u8 failure_cnt;
 	u8 try_flag;
@@ -200,8 +151,6 @@ enum dm_dig_connect {
 	DIG_CONNECT_MAX
 };
 
-extern struct dig_t de_digtable;
-
 void rtl92d_dm_init(struct ieee80211_hw *hw);
 void rtl92d_dm_watchdog(struct ieee80211_hw *hw);
 void rtl92d_dm_init_edca_turbo(struct ieee80211_hw *hw);
diff --git a/drivers/net/wireless/rtlwifi/rtl8192de/hw.c b/drivers/net/wireless/rtlwifi/rtl8192de/hw.c
index 509f5af38ad..b338d526c42 100644
--- a/drivers/net/wireless/rtlwifi/rtl8192de/hw.c
+++ b/drivers/net/wireless/rtlwifi/rtl8192de/hw.c
@@ -1743,9 +1743,13 @@ static void _rtl92de_efuse_update_chip_version(struct ieee80211_hw *hw)
 		chipver |= CHIP_92D_D_CUT;
 		RT_TRACE(rtlpriv, COMP_INIT, DBG_LOUD, "D-CUT!!!\n");
 		break;
+	case 0xCC33:
+		chipver |= CHIP_92D_E_CUT;
+		RT_TRACE(rtlpriv, COMP_INIT, DBG_LOUD, "E-CUT!!!\n");
+		break;
 	default:
 		chipver |= CHIP_92D_D_CUT;
-		RT_TRACE(rtlpriv, COMP_INIT, DBG_EMERG, "Unkown CUT!\n");
+		RT_TRACE(rtlpriv, COMP_INIT, DBG_EMERG, "Unknown CUT!\n");
 		break;
 	}
 	rtlpriv->rtlhal.version = chipver;
diff --git a/drivers/net/wireless/rtlwifi/rtl8192de/phy.c b/drivers/net/wireless/rtlwifi/rtl8192de/phy.c
index 28fc5fb8057..18380a7829f 100644
--- a/drivers/net/wireless/rtlwifi/rtl8192de/phy.c
+++ b/drivers/net/wireless/rtlwifi/rtl8192de/phy.c
@@ -3064,6 +3064,7 @@ u8 rtl92d_phy_sw_chnl(struct ieee80211_hw *hw)
 static void rtl92d_phy_set_io(struct ieee80211_hw *hw)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct dig_t *de_digtable = &rtlpriv->dm_digtable;
 	struct rtl_phy *rtlphy = &(rtlpriv->phy);
 
 	RT_TRACE(rtlpriv, COMP_CMD, DBG_TRACE,
@@ -3071,13 +3072,13 @@ static void rtl92d_phy_set_io(struct ieee80211_hw *hw)
 		 rtlphy->current_io_type, rtlphy->set_io_inprogress);
 	switch (rtlphy->current_io_type) {
 	case IO_CMD_RESUME_DM_BY_SCAN:
-		de_digtable.cur_igvalue = rtlphy->initgain_backup.xaagccore1;
+		de_digtable->cur_igvalue = rtlphy->initgain_backup.xaagccore1;
 		rtl92d_dm_write_dig(hw);
 		rtl92d_phy_set_txpower_level(hw, rtlphy->current_channel);
 		break;
 	case IO_CMD_PAUSE_DM_BY_SCAN:
-		rtlphy->initgain_backup.xaagccore1 = de_digtable.cur_igvalue;
-		de_digtable.cur_igvalue = 0x37;
+		rtlphy->initgain_backup.xaagccore1 = de_digtable->cur_igvalue;
+		de_digtable->cur_igvalue = 0x37;
 		rtl92d_dm_write_dig(hw);
 		break;
 	default:
diff --git a/drivers/net/wireless/rtlwifi/rtl8192de/trx.c b/drivers/net/wireless/rtlwifi/rtl8192de/trx.c
index a7f6126e2f8..1666ef7fd87 100644
--- a/drivers/net/wireless/rtlwifi/rtl8192de/trx.c
+++ b/drivers/net/wireless/rtlwifi/rtl8192de/trx.c
@@ -466,12 +466,13 @@ static void _rtl92de_translate_rx_signal_stuff(struct ieee80211_hw *hw,
 	type = WLAN_FC_GET_TYPE(fc);
 	praddr = hdr->addr1;
 	packet_matchbssid = ((IEEE80211_FTYPE_CTL != type) &&
-	     (!compare_ether_addr(mac->bssid, (cfc & IEEE80211_FCTL_TODS) ?
-		  hdr->addr1 : (cfc & IEEE80211_FCTL_FROMDS) ?
-		  hdr->addr2 : hdr->addr3)) && (!pstats->hwerror) &&
-		  (!pstats->crc) && (!pstats->icv));
+	     ether_addr_equal(mac->bssid,
+			      (cfc & IEEE80211_FCTL_TODS) ? hdr->addr1 :
+			      (cfc & IEEE80211_FCTL_FROMDS) ? hdr->addr2 :
+			      hdr->addr3) &&
+	     (!pstats->hwerror) && (!pstats->crc) && (!pstats->icv));
 	packet_toself = packet_matchbssid &&
-			(!compare_ether_addr(praddr, rtlefuse->dev_addr));
+			ether_addr_equal(praddr, rtlefuse->dev_addr);
 	if (ieee80211_is_beacon(fc))
 		packet_beacon = true;
 	_rtl92de_query_rxphystatus(hw, pstats, pdesc, p_drvinfo,
diff --git a/drivers/net/wireless/rtlwifi/rtl8192de/trx.h b/drivers/net/wireless/rtlwifi/rtl8192de/trx.h
index 0dc736c2723..057a52431b0 100644
--- a/drivers/net/wireless/rtlwifi/rtl8192de/trx.h
+++ b/drivers/net/wireless/rtlwifi/rtl8192de/trx.h
@@ -530,12 +530,8 @@
 	SET_BITS_OFFSET_LE(__pdesc+28, 0, 32, __val)
 
 #define CLEAR_PCI_TX_DESC_CONTENT(__pdesc, _size)	\
-do {							\
-	if (_size > TX_DESC_NEXT_DESC_OFFSET)		\
-		memset((void *)__pdesc, 0, TX_DESC_NEXT_DESC_OFFSET);	\
-	else						\
-		memset((void *)__pdesc, 0, _size);	\
-} while (0);
+	memset((void *)__pdesc, 0,			\
+	       min_t(size_t, _size, TX_DESC_NEXT_DESC_OFFSET))
 
 /* For 92D early mode */
 #define SET_EARLYMODE_PKTNUM(__paddr, __value)		\
diff --git a/drivers/net/wireless/rtlwifi/rtl8192se/def.h b/drivers/net/wireless/rtlwifi/rtl8192se/def.h
index d1b0a1e1497..20afec62ce0 100644
--- a/drivers/net/wireless/rtlwifi/rtl8192se/def.h
+++ b/drivers/net/wireless/rtlwifi/rtl8192se/def.h
@@ -252,12 +252,7 @@
  * the desc is cleared. */
 #define	TX_DESC_NEXT_DESC_OFFSET			36
 #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);
+	memset(__pdesc, 0, min_t(size_t, _size, TX_DESC_NEXT_DESC_OFFSET))
 
 /* Rx Desc */
 #define RX_STATUS_DESC_SIZE				24
diff --git a/drivers/net/wireless/rtlwifi/rtl8192se/dm.c b/drivers/net/wireless/rtlwifi/rtl8192se/dm.c
index fbabae17259..2e1158026fb 100644
--- a/drivers/net/wireless/rtlwifi/rtl8192se/dm.c
+++ b/drivers/net/wireless/rtlwifi/rtl8192se/dm.c
@@ -35,7 +35,6 @@
 #include "dm.h"
 #include "fw.h"
 
-struct dig_t digtable;
 static const u32 edca_setting_dl[PEER_MAX] = {
 	0xa44f,		/* 0 UNKNOWN */
 	0x5ea44f,	/* 1 REALTEK_90 */
@@ -421,62 +420,64 @@ static void _rtl92s_dm_false_alarm_counter_statistics(struct ieee80211_hw *hw)
 static void rtl92s_backoff_enable_flag(struct ieee80211_hw *hw)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct dig_t *digtable = &rtlpriv->dm_digtable;
 	struct false_alarm_statistics *falsealm_cnt = &(rtlpriv->falsealm_cnt);
 
-	if (falsealm_cnt->cnt_all > digtable.fa_highthresh) {
-		if ((digtable.backoff_val - 6) <
-			digtable.backoffval_range_min)
-			digtable.backoff_val = digtable.backoffval_range_min;
+	if (falsealm_cnt->cnt_all > digtable->fa_highthresh) {
+		if ((digtable->backoff_val - 6) <
+			digtable->backoffval_range_min)
+			digtable->backoff_val = digtable->backoffval_range_min;
 		else
-			digtable.backoff_val -= 6;
-	} else if (falsealm_cnt->cnt_all < digtable.fa_lowthresh) {
-		if ((digtable.backoff_val + 6) >
-			digtable.backoffval_range_max)
-			digtable.backoff_val =
-				 digtable.backoffval_range_max;
+			digtable->backoff_val -= 6;
+	} else if (falsealm_cnt->cnt_all < digtable->fa_lowthresh) {
+		if ((digtable->backoff_val + 6) >
+			digtable->backoffval_range_max)
+			digtable->backoff_val =
+				 digtable->backoffval_range_max;
 		else
-			digtable.backoff_val += 6;
+			digtable->backoff_val += 6;
 	}
 }
 
 static void _rtl92s_dm_initial_gain_sta_beforeconnect(struct ieee80211_hw *hw)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct dig_t *digtable = &rtlpriv->dm_digtable;
 	struct false_alarm_statistics *falsealm_cnt = &(rtlpriv->falsealm_cnt);
 	static u8 initialized, force_write;
 	u8 initial_gain = 0;
 
-	if ((digtable.pre_sta_connectstate == digtable.cur_sta_connectstate) ||
-		(digtable.cur_sta_connectstate == DIG_STA_BEFORE_CONNECT)) {
-		if (digtable.cur_sta_connectstate == DIG_STA_BEFORE_CONNECT) {
+	if ((digtable->pre_sta_connectstate == digtable->cur_sta_connectstate) ||
+		(digtable->cur_sta_connectstate == DIG_STA_BEFORE_CONNECT)) {
+		if (digtable->cur_sta_connectstate == DIG_STA_BEFORE_CONNECT) {
 			if (rtlpriv->psc.rfpwr_state != ERFON)
 				return;
 
-			if (digtable.backoff_enable_flag)
+			if (digtable->backoff_enable_flag)
 				rtl92s_backoff_enable_flag(hw);
 			else
-				digtable.backoff_val = DM_DIG_BACKOFF;
-
-			if ((digtable.rssi_val + 10 - digtable.backoff_val) >
-				digtable.rx_gain_range_max)
-				digtable.cur_igvalue =
-						digtable.rx_gain_range_max;
-			else if ((digtable.rssi_val + 10 - digtable.backoff_val)
-				 < digtable.rx_gain_range_min)
-				digtable.cur_igvalue =
-						digtable.rx_gain_range_min;
+				digtable->backoff_val = DM_DIG_BACKOFF;
+
+			if ((digtable->rssi_val + 10 - digtable->backoff_val) >
+				digtable->rx_gain_range_max)
+				digtable->cur_igvalue =
+						digtable->rx_gain_range_max;
+			else if ((digtable->rssi_val + 10 - digtable->backoff_val)
+				 < digtable->rx_gain_range_min)
+				digtable->cur_igvalue =
+						digtable->rx_gain_range_min;
 			else
-				digtable.cur_igvalue = digtable.rssi_val + 10 -
-						digtable.backoff_val;
+				digtable->cur_igvalue = digtable->rssi_val + 10 -
+						digtable->backoff_val;
 
 			if (falsealm_cnt->cnt_all > 10000)
-				digtable.cur_igvalue =
-					 (digtable.cur_igvalue > 0x33) ?
-					 digtable.cur_igvalue : 0x33;
+				digtable->cur_igvalue =
+					 (digtable->cur_igvalue > 0x33) ?
+					 digtable->cur_igvalue : 0x33;
 
 			if (falsealm_cnt->cnt_all > 16000)
-				digtable.cur_igvalue =
-						 digtable.rx_gain_range_max;
+				digtable->cur_igvalue =
+						 digtable->rx_gain_range_max;
 		/* connected -> connected or disconnected -> disconnected  */
 		} else {
 			/* Firmware control DIG, do nothing in driver dm */
@@ -486,31 +487,31 @@ static void _rtl92s_dm_initial_gain_sta_beforeconnect(struct ieee80211_hw *hw)
 		 * disconnected or beforeconnect->(dis)connected */
 	} else {
 		/* Enable FW DIG */
-		digtable.dig_ext_port_stage = DIG_EXT_PORT_STAGE_MAX;
+		digtable->dig_ext_port_stage = DIG_EXT_PORT_STAGE_MAX;
 		rtl92s_phy_set_fw_cmd(hw, FW_CMD_DIG_ENABLE);
 
-		digtable.backoff_val = DM_DIG_BACKOFF;
-		digtable.cur_igvalue = rtlpriv->phy.default_initialgain[0];
-		digtable.pre_igvalue = 0;
+		digtable->backoff_val = DM_DIG_BACKOFF;
+		digtable->cur_igvalue = rtlpriv->phy.default_initialgain[0];
+		digtable->pre_igvalue = 0;
 		return;
 	}
 
 	/* Forced writing to prevent from fw-dig overwriting. */
-	if (digtable.pre_igvalue != rtl_get_bbreg(hw, ROFDM0_XAAGCCORE1,
+	if (digtable->pre_igvalue != rtl_get_bbreg(hw, ROFDM0_XAAGCCORE1,
 						  MASKBYTE0))
 		force_write = 1;
 
-	if ((digtable.pre_igvalue != digtable.cur_igvalue) ||
+	if ((digtable->pre_igvalue != digtable->cur_igvalue) ||
 	    !initialized || force_write) {
 		/* Disable FW DIG */
 		rtl92s_phy_set_fw_cmd(hw, FW_CMD_DIG_DISABLE);
 
-		initial_gain = (u8)digtable.cur_igvalue;
+		initial_gain = (u8)digtable->cur_igvalue;
 
 		/* Set initial gain. */
 		rtl_set_bbreg(hw, ROFDM0_XAAGCCORE1, MASKBYTE0, initial_gain);
 		rtl_set_bbreg(hw, ROFDM0_XBAGCCORE1, MASKBYTE0, initial_gain);
-		digtable.pre_igvalue = digtable.cur_igvalue;
+		digtable->pre_igvalue = digtable->cur_igvalue;
 		initialized = 1;
 		force_write = 0;
 	}
@@ -519,6 +520,7 @@ static void _rtl92s_dm_initial_gain_sta_beforeconnect(struct ieee80211_hw *hw)
 static void _rtl92s_dm_ctrl_initgain_bytwoport(struct ieee80211_hw *hw)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct dig_t *digtable = &rtlpriv->dm_digtable;
 
 	if (rtlpriv->mac80211.act_scanning)
 		return;
@@ -526,17 +528,17 @@ static void _rtl92s_dm_ctrl_initgain_bytwoport(struct ieee80211_hw *hw)
 	/* Decide the current status and if modify initial gain or not */
 	if (rtlpriv->mac80211.link_state >= MAC80211_LINKED ||
 	    rtlpriv->mac80211.opmode == NL80211_IFTYPE_ADHOC)
-		digtable.cur_sta_connectstate = DIG_STA_CONNECT;
+		digtable->cur_sta_connectstate = DIG_STA_CONNECT;
 	else
-		digtable.cur_sta_connectstate = DIG_STA_DISCONNECT;
+		digtable->cur_sta_connectstate = DIG_STA_DISCONNECT;
 
-	digtable.rssi_val = rtlpriv->dm.undecorated_smoothed_pwdb;
+	digtable->rssi_val = rtlpriv->dm.undecorated_smoothed_pwdb;
 
 	/* Change dig mode to rssi */
-	if (digtable.cur_sta_connectstate != DIG_STA_DISCONNECT) {
-		if (digtable.dig_twoport_algorithm ==
+	if (digtable->cur_sta_connectstate != DIG_STA_DISCONNECT) {
+		if (digtable->dig_twoport_algorithm ==
 		    DIG_TWO_PORT_ALGO_FALSE_ALARM) {
-			digtable.dig_twoport_algorithm = DIG_TWO_PORT_ALGO_RSSI;
+			digtable->dig_twoport_algorithm = DIG_TWO_PORT_ALGO_RSSI;
 			rtl92s_phy_set_fw_cmd(hw, FW_CMD_DIG_MODE_SS);
 		}
 	}
@@ -544,13 +546,14 @@ static void _rtl92s_dm_ctrl_initgain_bytwoport(struct ieee80211_hw *hw)
 	_rtl92s_dm_false_alarm_counter_statistics(hw);
 	_rtl92s_dm_initial_gain_sta_beforeconnect(hw);
 
-	digtable.pre_sta_connectstate = digtable.cur_sta_connectstate;
+	digtable->pre_sta_connectstate = digtable->cur_sta_connectstate;
 }
 
 static void _rtl92s_dm_ctrl_initgain_byrssi(struct ieee80211_hw *hw)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
 	struct rtl_phy *rtlphy = &(rtlpriv->phy);
+	struct dig_t *digtable = &rtlpriv->dm_digtable;
 
 	/* 2T2R TP issue */
 	if (rtlphy->rf_type == RF_2T2R)
@@ -559,7 +562,7 @@ static void _rtl92s_dm_ctrl_initgain_byrssi(struct ieee80211_hw *hw)
 	if (!rtlpriv->dm.dm_initialgain_enable)
 		return;
 
-	if (digtable.dig_enable_flag == false)
+	if (digtable->dig_enable_flag == false)
 		return;
 
 	_rtl92s_dm_ctrl_initgain_bytwoport(hw);
@@ -639,51 +642,52 @@ static void _rtl92s_dm_dynamic_txpower(struct ieee80211_hw *hw)
 static void _rtl92s_dm_init_dig(struct ieee80211_hw *hw)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct dig_t *digtable = &rtlpriv->dm_digtable;
 
 	/* Disable DIG scheme now.*/
-	digtable.dig_enable_flag = true;
-	digtable.backoff_enable_flag = true;
+	digtable->dig_enable_flag = true;
+	digtable->backoff_enable_flag = true;
 
 	if ((rtlpriv->dm.dm_type == DM_TYPE_BYDRIVER) &&
 	    (hal_get_firmwareversion(rtlpriv) >= 0x3c))
-		digtable.dig_algorithm = DIG_ALGO_BY_TOW_PORT;
+		digtable->dig_algorithm = DIG_ALGO_BY_TOW_PORT;
 	else
-		digtable.dig_algorithm =
+		digtable->dig_algorithm =
 			 DIG_ALGO_BEFORE_CONNECT_BY_RSSI_AND_ALARM;
 
-	digtable.dig_twoport_algorithm = DIG_TWO_PORT_ALGO_RSSI;
-	digtable.dig_ext_port_stage = DIG_EXT_PORT_STAGE_MAX;
-	/* off=by real rssi value, on=by digtable.rssi_val for new dig */
-	digtable.dig_dbgmode = DM_DBG_OFF;
-	digtable.dig_slgorithm_switch = 0;
+	digtable->dig_twoport_algorithm = DIG_TWO_PORT_ALGO_RSSI;
+	digtable->dig_ext_port_stage = DIG_EXT_PORT_STAGE_MAX;
+	/* off=by real rssi value, on=by digtable->rssi_val for new dig */
+	digtable->dig_dbgmode = DM_DBG_OFF;
+	digtable->dig_slgorithm_switch = 0;
 
 	/* 2007/10/04 MH Define init gain threshol. */
-	digtable.dig_state = DM_STA_DIG_MAX;
-	digtable.dig_highpwrstate = DM_STA_DIG_MAX;
+	digtable->dig_state = DM_STA_DIG_MAX;
+	digtable->dig_highpwrstate = DM_STA_DIG_MAX;
 
-	digtable.cur_sta_connectstate = DIG_STA_DISCONNECT;
-	digtable.pre_sta_connectstate = DIG_STA_DISCONNECT;
-	digtable.cur_ap_connectstate = DIG_AP_DISCONNECT;
-	digtable.pre_ap_connectstate = DIG_AP_DISCONNECT;
+	digtable->cur_sta_connectstate = DIG_STA_DISCONNECT;
+	digtable->pre_sta_connectstate = DIG_STA_DISCONNECT;
+	digtable->cur_ap_connectstate = DIG_AP_DISCONNECT;
+	digtable->pre_ap_connectstate = DIG_AP_DISCONNECT;
 
-	digtable.rssi_lowthresh = DM_DIG_THRESH_LOW;
-	digtable.rssi_highthresh = DM_DIG_THRESH_HIGH;
+	digtable->rssi_lowthresh = DM_DIG_THRESH_LOW;
+	digtable->rssi_highthresh = DM_DIG_THRESH_HIGH;
 
-	digtable.fa_lowthresh = DM_FALSEALARM_THRESH_LOW;
-	digtable.fa_highthresh = DM_FALSEALARM_THRESH_HIGH;
+	digtable->fa_lowthresh = DM_FALSEALARM_THRESH_LOW;
+	digtable->fa_highthresh = DM_FALSEALARM_THRESH_HIGH;
 
-	digtable.rssi_highpower_lowthresh = DM_DIG_HIGH_PWR_THRESH_LOW;
-	digtable.rssi_highpower_highthresh = DM_DIG_HIGH_PWR_THRESH_HIGH;
+	digtable->rssi_highpower_lowthresh = DM_DIG_HIGH_PWR_THRESH_LOW;
+	digtable->rssi_highpower_highthresh = DM_DIG_HIGH_PWR_THRESH_HIGH;
 
 	/* for dig debug rssi value */
-	digtable.rssi_val = 50;
-	digtable.backoff_val = DM_DIG_BACKOFF;
-	digtable.rx_gain_range_max = DM_DIG_MAX;
+	digtable->rssi_val = 50;
+	digtable->backoff_val = DM_DIG_BACKOFF;
+	digtable->rx_gain_range_max = DM_DIG_MAX;
 
-	digtable.rx_gain_range_min = DM_DIG_MIN;
+	digtable->rx_gain_range_min = DM_DIG_MIN;
 
-	digtable.backoffval_range_max = DM_DIG_BACKOFF_MAX;
-	digtable.backoffval_range_min = DM_DIG_BACKOFF_MIN;
+	digtable->backoffval_range_max = DM_DIG_BACKOFF_MAX;
+	digtable->backoffval_range_min = DM_DIG_BACKOFF_MIN;
 }
 
 static void _rtl92s_dm_init_dynamic_txpower(struct ieee80211_hw *hw)
diff --git a/drivers/net/wireless/rtlwifi/rtl8192se/dm.h b/drivers/net/wireless/rtlwifi/rtl8192se/dm.h
index e1b19a64176..2e9052c8fe4 100644
--- a/drivers/net/wireless/rtlwifi/rtl8192se/dm.h
+++ b/drivers/net/wireless/rtlwifi/rtl8192se/dm.h
@@ -29,48 +29,6 @@
 #ifndef	__RTL_92S_DM_H__
 #define __RTL_92S_DM_H__
 
-struct dig_t {
-	u8 dig_enable_flag;
-	u8 dig_algorithm;
-	u8 dig_twoport_algorithm;
-	u8 dig_ext_port_stage;
-	u8 dig_dbgmode;
-	u8 dig_slgorithm_switch;
-
-	long rssi_lowthresh;
-	long rssi_highthresh;
-
-	u32 fa_lowthresh;
-	u32 fa_highthresh;
-
-	long rssi_highpower_lowthresh;
-	long rssi_highpower_highthresh;
-
-	u8 dig_state;
-	u8 dig_highpwrstate;
-	u8 cur_sta_connectstate;
-	u8 pre_sta_connectstate;
-	u8 cur_ap_connectstate;
-	u8 pre_ap_connectstate;
-
-	u8 cur_pd_thstate;
-	u8 pre_pd_thstate;
-	u8 cur_cs_ratiostate;
-	u8 pre_cs_ratiostate;
-
-	u32 pre_igvalue;
-	u32	cur_igvalue;
-
-	u8 backoff_enable_flag;
-	char backoff_val;
-	char backoffval_range_max;
-	char backoffval_range_min;
-	u8 rx_gain_range_max;
-	u8 rx_gain_range_min;
-
-	long rssi_val;
-};
-
 enum dm_dig_alg {
 	DIG_ALGO_BY_FALSE_ALARM = 0,
 	DIG_ALGO_BY_RSSI	= 1,
@@ -154,8 +112,6 @@ enum dm_ratr_sta {
 #define	DM_DIG_BACKOFF_MAX		12
 #define	DM_DIG_BACKOFF_MIN		-4
 
-extern struct dig_t digtable;
-
 void rtl92s_dm_watchdog(struct ieee80211_hw *hw);
 void rtl92s_dm_init(struct ieee80211_hw *hw);
 void rtl92s_dm_init_edca_turbo(struct ieee80211_hw *hw);
diff --git a/drivers/net/wireless/rtlwifi/rtl8192se/fw.h b/drivers/net/wireless/rtlwifi/rtl8192se/fw.h
index b4afff62643..d53f4332464 100644
--- a/drivers/net/wireless/rtlwifi/rtl8192se/fw.h
+++ b/drivers/net/wireless/rtlwifi/rtl8192se/fw.h
@@ -345,7 +345,7 @@ enum fw_h2c_cmd {
 	do {							\
 		udelay(1000);					\
 		rtlpriv->rtlhal.fwcmd_iomap &= (~_Bit);		\
-	} while (0);
+	} while (0)
 
 #define FW_CMD_IO_UPDATE(rtlpriv, _val)				\
 	rtlpriv->rtlhal.fwcmd_iomap = _val;
@@ -354,13 +354,13 @@ enum fw_h2c_cmd {
 	do {							\
 		rtl_write_word(rtlpriv, LBUS_MON_ADDR, (u16)_val);	\
 		FW_CMD_IO_UPDATE(rtlpriv, _val);		\
-	} while (0);
+	} while (0)
 
 #define FW_CMD_PARA_SET(rtlpriv, _val)				\
 	do {							\
 		rtl_write_dword(rtlpriv, LBUS_ADDR_MASK, _val);	\
 		rtlpriv->rtlhal.fwcmd_ioparam = _val;		\
-	} while (0);
+	} while (0)
 
 #define FW_CMD_IO_QUERY(rtlpriv)				\
 	(u16)(rtlpriv->rtlhal.fwcmd_iomap)
diff --git a/drivers/net/wireless/rtlwifi/rtl8192se/phy.c b/drivers/net/wireless/rtlwifi/rtl8192se/phy.c
index 4a499928e4c..8d7099bc472 100644
--- a/drivers/net/wireless/rtlwifi/rtl8192se/phy.c
+++ b/drivers/net/wireless/rtlwifi/rtl8192se/phy.c
@@ -1450,6 +1450,7 @@ static void _rtl92s_phy_set_fwcmd_io(struct ieee80211_hw *hw)
 bool rtl92s_phy_set_fw_cmd(struct ieee80211_hw *hw, enum fwcmd_iotype fw_cmdio)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct dig_t *digtable = &rtlpriv->dm_digtable;
 	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
 	struct rtl_efuse *rtlefuse = rtl_efuse(rtl_priv(hw));
 	u32	fw_param = FW_CMD_IO_PARA_QUERY(rtlpriv);
@@ -1588,16 +1589,16 @@ bool rtl92s_phy_set_fw_cmd(struct ieee80211_hw *hw, enum fwcmd_iotype fw_cmdio)
 				      FW_SS_CTL);
 
 			if (rtlpriv->dm.dm_flag & HAL_DM_DIG_DISABLE ||
-				!digtable.dig_enable_flag)
+				!digtable->dig_enable_flag)
 				fw_cmdmap &= ~FW_DIG_ENABLE_CTL;
 
 			if ((rtlpriv->dm.dm_flag & HAL_DM_HIPWR_DISABLE) ||
 			    rtlpriv->dm.dynamic_txpower_enable)
 				fw_cmdmap &= ~FW_HIGH_PWR_ENABLE_CTL;
 
-			if ((digtable.dig_ext_port_stage ==
+			if ((digtable->dig_ext_port_stage ==
 			    DIG_EXT_PORT_STAGE_0) ||
-			    (digtable.dig_ext_port_stage ==
+			    (digtable->dig_ext_port_stage ==
 			    DIG_EXT_PORT_STAGE_1))
 				fw_cmdmap &= ~FW_DIG_ENABLE_CTL;
 
diff --git a/drivers/net/wireless/rtlwifi/rtl8192se/sw.c b/drivers/net/wireless/rtlwifi/rtl8192se/sw.c
index f1b36005c6a..730bcc91952 100644
--- a/drivers/net/wireless/rtlwifi/rtl8192se/sw.c
+++ b/drivers/net/wireless/rtlwifi/rtl8192se/sw.c
@@ -450,21 +450,4 @@ static struct pci_driver rtl92se_driver = {
 	.driver.pm = &rtlwifi_pm_ops,
 };
 
-static int __init rtl92se_module_init(void)
-{
-	int ret = 0;
-
-	ret = pci_register_driver(&rtl92se_driver);
-	if (ret)
-		RT_ASSERT(false, "No device found\n");
-
-	return ret;
-}
-
-static void __exit rtl92se_module_exit(void)
-{
-	pci_unregister_driver(&rtl92se_driver);
-}
-
-module_init(rtl92se_module_init);
-module_exit(rtl92se_module_exit);
+module_pci_driver(rtl92se_driver);
diff --git a/drivers/net/wireless/rtlwifi/rtl8192se/trx.c b/drivers/net/wireless/rtlwifi/rtl8192se/trx.c
index 2fd3d13b7ce..812b5858f14 100644
--- a/drivers/net/wireless/rtlwifi/rtl8192se/trx.c
+++ b/drivers/net/wireless/rtlwifi/rtl8192se/trx.c
@@ -492,13 +492,14 @@ static void _rtl92se_translate_rx_signal_stuff(struct ieee80211_hw *hw,
 	praddr = hdr->addr1;
 
 	packet_matchbssid = ((IEEE80211_FTYPE_CTL != type) &&
-	     (!compare_ether_addr(mac->bssid, (cfc & IEEE80211_FCTL_TODS) ?
-			hdr->addr1 : (cfc & IEEE80211_FCTL_FROMDS) ?
-			hdr->addr2 : hdr->addr3)) && (!pstats->hwerror) &&
-			(!pstats->crc) && (!pstats->icv));
+	     ether_addr_equal(mac->bssid,
+			      (cfc & IEEE80211_FCTL_TODS) ? hdr->addr1 :
+			      (cfc & IEEE80211_FCTL_FROMDS) ? hdr->addr2 :
+			      hdr->addr3) &&
+	     (!pstats->hwerror) && (!pstats->crc) && (!pstats->icv));
 
 	packet_toself = packet_matchbssid &&
-	    (!compare_ether_addr(praddr, rtlefuse->dev_addr));
+	    ether_addr_equal(praddr, rtlefuse->dev_addr);
 
 	if (ieee80211_is_beacon(fc))
 		packet_beacon = true;
diff --git a/drivers/net/wireless/rtlwifi/usb.c b/drivers/net/wireless/rtlwifi/usb.c
index d04dbda13f5..a6049d7d51b 100644
--- a/drivers/net/wireless/rtlwifi/usb.c
+++ b/drivers/net/wireless/rtlwifi/usb.c
@@ -971,11 +971,6 @@ int __devinit rtl_usb_probe(struct usb_interface *intf,
 	rtlpriv->cfg->ops->read_chip_version(hw);
 	/*like read eeprom and so on */
 	rtlpriv->cfg->ops->read_eeprom_info(hw);
-	if (rtlpriv->cfg->ops->init_sw_vars(hw)) {
-		RT_TRACE(rtlpriv, COMP_ERR, DBG_EMERG, "Can't init_sw_vars\n");
-		goto error_out;
-	}
-	rtlpriv->cfg->ops->init_sw_leds(hw);
 	err = _rtl_usb_init(hw);
 	if (err)
 		goto error_out;
@@ -987,6 +982,11 @@ int __devinit rtl_usb_probe(struct usb_interface *intf,
 			 "Can't allocate sw for mac80211\n");
 		goto error_out;
 	}
+	if (rtlpriv->cfg->ops->init_sw_vars(hw)) {
+		RT_TRACE(rtlpriv, COMP_ERR, DBG_EMERG, "Can't init_sw_vars\n");
+		goto error_out;
+	}
+	rtlpriv->cfg->ops->init_sw_leds(hw);
 
 	return 0;
 error_out:
diff --git a/drivers/net/wireless/rtlwifi/wifi.h b/drivers/net/wireless/rtlwifi/wifi.h
index 28ebc69218a..bd816aef26d 100644
--- a/drivers/net/wireless/rtlwifi/wifi.h
+++ b/drivers/net/wireless/rtlwifi/wifi.h
@@ -1592,6 +1592,65 @@ struct rtl_debug {
 	char proc_name[20];
 };
 
+struct ps_t {
+	u8 pre_ccastate;
+	u8 cur_ccasate;
+	u8 pre_rfstate;
+	u8 cur_rfstate;
+	long rssi_val_min;
+};
+
+struct dig_t {
+	u32 rssi_lowthresh;
+	u32 rssi_highthresh;
+	u32 fa_lowthresh;
+	u32 fa_highthresh;
+	long last_min_undecorated_pwdb_for_dm;
+	long rssi_highpower_lowthresh;
+	long rssi_highpower_highthresh;
+	u32 recover_cnt;
+	u32 pre_igvalue;
+	u32 cur_igvalue;
+	long rssi_val;
+	u8 dig_enable_flag;
+	u8 dig_ext_port_stage;
+	u8 dig_algorithm;
+	u8 dig_twoport_algorithm;
+	u8 dig_dbgmode;
+	u8 dig_slgorithm_switch;
+	u8 cursta_connectctate;
+	u8 presta_connectstate;
+	u8 curmultista_connectstate;
+	char backoff_val;
+	char backoff_val_range_max;
+	char backoff_val_range_min;
+	u8 rx_gain_range_max;
+	u8 rx_gain_range_min;
+	u8 min_undecorated_pwdb_for_dm;
+	u8 rssi_val_min;
+	u8 pre_cck_pd_state;
+	u8 cur_cck_pd_state;
+	u8 pre_cck_fa_state;
+	u8 cur_cck_fa_state;
+	u8 pre_ccastate;
+	u8 cur_ccasate;
+	u8 large_fa_hit;
+	u8 forbidden_igi;
+	u8 dig_state;
+	u8 dig_highpwrstate;
+	u8 cur_sta_connectstate;
+	u8 pre_sta_connectstate;
+	u8 cur_ap_connectstate;
+	u8 pre_ap_connectstate;
+	u8 cur_pd_thstate;
+	u8 pre_pd_thstate;
+	u8 cur_cs_ratiostate;
+	u8 pre_cs_ratiostate;
+	u8 backoff_enable_flag;
+	char backoffval_range_max;
+	char backoffval_range_min;
+};
+
 struct rtl_priv {
 	struct completion firmware_loading_complete;
 	struct rtl_locks locks;
@@ -1629,6 +1688,10 @@ struct rtl_priv {
 	   interface or hardware */
 	unsigned long status;
 
+	/* tables for dm */
+	struct dig_t dm_digtable;
+	struct ps_t dm_pstable;
+
 	/* data buffer pointer for USB reads */
 	__le32 *usb_data;
 	int usb_data_index;
@@ -1958,37 +2021,35 @@ static inline void rtl_write_dword(struct rtl_priv *rtlpriv,
 static inline u32 rtl_get_bbreg(struct ieee80211_hw *hw,
 				u32 regaddr, u32 bitmask)
 {
-	return ((struct rtl_priv *)(hw)->priv)->cfg->ops->get_bbreg(hw,
-								    regaddr,
-								    bitmask);
+	struct rtl_priv *rtlpriv = hw->priv;
+
+	return rtlpriv->cfg->ops->get_bbreg(hw, regaddr, bitmask);
 }
 
 static inline void rtl_set_bbreg(struct ieee80211_hw *hw, u32 regaddr,
 				 u32 bitmask, u32 data)
 {
-	((struct rtl_priv *)(hw)->priv)->cfg->ops->set_bbreg(hw,
-							     regaddr, bitmask,
-							     data);
+	struct rtl_priv *rtlpriv = hw->priv;
 
+	rtlpriv->cfg->ops->set_bbreg(hw, regaddr, bitmask, data);
 }
 
 static inline u32 rtl_get_rfreg(struct ieee80211_hw *hw,
 				enum radio_path rfpath, u32 regaddr,
 				u32 bitmask)
 {
-	return ((struct rtl_priv *)(hw)->priv)->cfg->ops->get_rfreg(hw,
-								    rfpath,
-								    regaddr,
-								    bitmask);
+	struct rtl_priv *rtlpriv = hw->priv;
+
+	return rtlpriv->cfg->ops->get_rfreg(hw, rfpath, regaddr, bitmask);
 }
 
 static inline void rtl_set_rfreg(struct ieee80211_hw *hw,
 				 enum radio_path rfpath, u32 regaddr,
 				 u32 bitmask, u32 data)
 {
-	((struct rtl_priv *)(hw)->priv)->cfg->ops->set_rfreg(hw,
-							     rfpath, regaddr,
-							     bitmask, data);
+	struct rtl_priv *rtlpriv = hw->priv;
+
+	rtlpriv->cfg->ops->set_rfreg(hw, rfpath, regaddr, bitmask, data);
 }
 
 static inline bool is_hal_stop(struct rtl_hal *rtlhal)
diff --git a/drivers/net/wireless/ti/Kconfig b/drivers/net/wireless/ti/Kconfig
new file mode 100644
index 00000000000..1a72932e221
--- /dev/null
+++ b/drivers/net/wireless/ti/Kconfig
@@ -0,0 +1,14 @@
+menuconfig WL_TI
+	bool "TI Wireless LAN support"
+	---help---
+	  This section contains support for all the wireless drivers
+	  for Texas Instruments WLAN chips, such as wl1251 and the wl12xx
+	  family.
+
+if WL_TI
+source "drivers/net/wireless/ti/wl1251/Kconfig"
+source "drivers/net/wireless/ti/wl12xx/Kconfig"
+
+# keep last for automatic dependencies
+source "drivers/net/wireless/ti/wlcore/Kconfig"
+endif # WL_TI
diff --git a/drivers/net/wireless/ti/Makefile b/drivers/net/wireless/ti/Makefile
new file mode 100644
index 00000000000..0a565622d4a
--- /dev/null
+++ b/drivers/net/wireless/ti/Makefile
@@ -0,0 +1,4 @@
+obj-$(CONFIG_WLCORE)			+= wlcore/
+obj-$(CONFIG_WL12XX)			+= wl12xx/
+obj-$(CONFIG_WL12XX_PLATFORM_DATA)	+= wlcore/
+obj-$(CONFIG_WL1251)			+= wl1251/
diff --git a/drivers/net/wireless/wl1251/Kconfig b/drivers/net/wireless/ti/wl1251/Kconfig
index 1fb65849414..1fb65849414 100644
--- a/drivers/net/wireless/wl1251/Kconfig
+++ b/drivers/net/wireless/ti/wl1251/Kconfig
diff --git a/drivers/net/wireless/wl1251/Makefile b/drivers/net/wireless/ti/wl1251/Makefile
index a5c6328b5f7..a5c6328b5f7 100644
--- a/drivers/net/wireless/wl1251/Makefile
+++ b/drivers/net/wireless/ti/wl1251/Makefile
diff --git a/drivers/net/wireless/wl1251/acx.c b/drivers/net/wireless/ti/wl1251/acx.c
index ad87a1ac646..ad87a1ac646 100644
--- a/drivers/net/wireless/wl1251/acx.c
+++ b/drivers/net/wireless/ti/wl1251/acx.c
diff --git a/drivers/net/wireless/wl1251/acx.h b/drivers/net/wireless/ti/wl1251/acx.h
index c2ba100f9b1..c2ba100f9b1 100644
--- a/drivers/net/wireless/wl1251/acx.h
+++ b/drivers/net/wireless/ti/wl1251/acx.h
diff --git a/drivers/net/wireless/wl1251/boot.c b/drivers/net/wireless/ti/wl1251/boot.c
index a2e5241382d..a2e5241382d 100644
--- a/drivers/net/wireless/wl1251/boot.c
+++ b/drivers/net/wireless/ti/wl1251/boot.c
diff --git a/drivers/net/wireless/wl1251/boot.h b/drivers/net/wireless/ti/wl1251/boot.h
index 7661bc5e466..7661bc5e466 100644
--- a/drivers/net/wireless/wl1251/boot.h
+++ b/drivers/net/wireless/ti/wl1251/boot.h
diff --git a/drivers/net/wireless/wl1251/cmd.c b/drivers/net/wireless/ti/wl1251/cmd.c
index d14d69d733a..d14d69d733a 100644
--- a/drivers/net/wireless/wl1251/cmd.c
+++ b/drivers/net/wireless/ti/wl1251/cmd.c
diff --git a/drivers/net/wireless/wl1251/cmd.h b/drivers/net/wireless/ti/wl1251/cmd.h
index ee4f2b39182..ee4f2b39182 100644
--- a/drivers/net/wireless/wl1251/cmd.h
+++ b/drivers/net/wireless/ti/wl1251/cmd.h
diff --git a/drivers/net/wireless/wl1251/debugfs.c b/drivers/net/wireless/ti/wl1251/debugfs.c
index 448da1f8c22..448da1f8c22 100644
--- a/drivers/net/wireless/wl1251/debugfs.c
+++ b/drivers/net/wireless/ti/wl1251/debugfs.c
diff --git a/drivers/net/wireless/wl1251/debugfs.h b/drivers/net/wireless/ti/wl1251/debugfs.h
index b3417c02a21..b3417c02a21 100644
--- a/drivers/net/wireless/wl1251/debugfs.h
+++ b/drivers/net/wireless/ti/wl1251/debugfs.h
diff --git a/drivers/net/wireless/wl1251/event.c b/drivers/net/wireless/ti/wl1251/event.c
index 9f15ccaf8f0..9f15ccaf8f0 100644
--- a/drivers/net/wireless/wl1251/event.c
+++ b/drivers/net/wireless/ti/wl1251/event.c
diff --git a/drivers/net/wireless/wl1251/event.h b/drivers/net/wireless/ti/wl1251/event.h
index 30eb5d150bf..30eb5d150bf 100644
--- a/drivers/net/wireless/wl1251/event.h
+++ b/drivers/net/wireless/ti/wl1251/event.h
diff --git a/drivers/net/wireless/wl1251/init.c b/drivers/net/wireless/ti/wl1251/init.c
index 89b43d35473..89b43d35473 100644
--- a/drivers/net/wireless/wl1251/init.c
+++ b/drivers/net/wireless/ti/wl1251/init.c
diff --git a/drivers/net/wireless/wl1251/init.h b/drivers/net/wireless/ti/wl1251/init.h
index 543f17582ea..543f17582ea 100644
--- a/drivers/net/wireless/wl1251/init.h
+++ b/drivers/net/wireless/ti/wl1251/init.h
diff --git a/drivers/net/wireless/wl1251/io.c b/drivers/net/wireless/ti/wl1251/io.c
index cdcadbf6ac2..cdcadbf6ac2 100644
--- a/drivers/net/wireless/wl1251/io.c
+++ b/drivers/net/wireless/ti/wl1251/io.c
diff --git a/drivers/net/wireless/wl1251/io.h b/drivers/net/wireless/ti/wl1251/io.h
index d382877c34c..d382877c34c 100644
--- a/drivers/net/wireless/wl1251/io.h
+++ b/drivers/net/wireless/ti/wl1251/io.h
diff --git a/drivers/net/wireless/wl1251/main.c b/drivers/net/wireless/ti/wl1251/main.c
index d1afb8e3b2e..d1afb8e3b2e 100644
--- a/drivers/net/wireless/wl1251/main.c
+++ b/drivers/net/wireless/ti/wl1251/main.c
diff --git a/drivers/net/wireless/wl1251/ps.c b/drivers/net/wireless/ti/wl1251/ps.c
index db719f7d269..db719f7d269 100644
--- a/drivers/net/wireless/wl1251/ps.c
+++ b/drivers/net/wireless/ti/wl1251/ps.c
diff --git a/drivers/net/wireless/wl1251/ps.h b/drivers/net/wireless/ti/wl1251/ps.h
index 75efad246d6..75efad246d6 100644
--- a/drivers/net/wireless/wl1251/ps.h
+++ b/drivers/net/wireless/ti/wl1251/ps.h
diff --git a/drivers/net/wireless/wl1251/reg.h b/drivers/net/wireless/ti/wl1251/reg.h
index a5809019c5c..a5809019c5c 100644
--- a/drivers/net/wireless/wl1251/reg.h
+++ b/drivers/net/wireless/ti/wl1251/reg.h
diff --git a/drivers/net/wireless/wl1251/rx.c b/drivers/net/wireless/ti/wl1251/rx.c
index 6af35265c90..6af35265c90 100644
--- a/drivers/net/wireless/wl1251/rx.c
+++ b/drivers/net/wireless/ti/wl1251/rx.c
diff --git a/drivers/net/wireless/wl1251/rx.h b/drivers/net/wireless/ti/wl1251/rx.h
index 4448f635a4d..4448f635a4d 100644
--- a/drivers/net/wireless/wl1251/rx.h
+++ b/drivers/net/wireless/ti/wl1251/rx.h
diff --git a/drivers/net/wireless/wl1251/sdio.c b/drivers/net/wireless/ti/wl1251/sdio.c
index 1b851f650e0..1b851f650e0 100644
--- a/drivers/net/wireless/wl1251/sdio.c
+++ b/drivers/net/wireless/ti/wl1251/sdio.c
diff --git a/drivers/net/wireless/wl1251/spi.c b/drivers/net/wireless/ti/wl1251/spi.c
index 6248c354fc5..6248c354fc5 100644
--- a/drivers/net/wireless/wl1251/spi.c
+++ b/drivers/net/wireless/ti/wl1251/spi.c
diff --git a/drivers/net/wireless/wl1251/spi.h b/drivers/net/wireless/ti/wl1251/spi.h
index 16d506955cc..16d506955cc 100644
--- a/drivers/net/wireless/wl1251/spi.h
+++ b/drivers/net/wireless/ti/wl1251/spi.h
diff --git a/drivers/net/wireless/wl1251/tx.c b/drivers/net/wireless/ti/wl1251/tx.c
index 28121c590a2..28121c590a2 100644
--- a/drivers/net/wireless/wl1251/tx.c
+++ b/drivers/net/wireless/ti/wl1251/tx.c
diff --git a/drivers/net/wireless/wl1251/tx.h b/drivers/net/wireless/ti/wl1251/tx.h
index 81338d39b43..81338d39b43 100644
--- a/drivers/net/wireless/wl1251/tx.h
+++ b/drivers/net/wireless/ti/wl1251/tx.h
diff --git a/drivers/net/wireless/wl1251/wl1251.h b/drivers/net/wireless/ti/wl1251/wl1251.h
index 9d8f5816c6f..9d8f5816c6f 100644
--- a/drivers/net/wireless/wl1251/wl1251.h
+++ b/drivers/net/wireless/ti/wl1251/wl1251.h
diff --git a/drivers/net/wireless/wl1251/wl12xx_80211.h b/drivers/net/wireless/ti/wl1251/wl12xx_80211.h
index 04ed5149577..04ed5149577 100644
--- a/drivers/net/wireless/wl1251/wl12xx_80211.h
+++ b/drivers/net/wireless/ti/wl1251/wl12xx_80211.h
diff --git a/drivers/net/wireless/ti/wl12xx/Kconfig b/drivers/net/wireless/ti/wl12xx/Kconfig
new file mode 100644
index 00000000000..c2183594655
--- /dev/null
+++ b/drivers/net/wireless/ti/wl12xx/Kconfig
@@ -0,0 +1,9 @@
+config WL12XX
+       tristate "TI wl12xx support"
+	depends on MAC80211
+       select WLCORE
+       ---help---
+	  This module adds support for wireless adapters based on TI wl1271,
+	  wl1273, wl1281 and wl1283 chipsets. This module does *not* include
+	  support for wl1251.  For wl1251 support, use the separate homonymous
+	   driver instead.
diff --git a/drivers/net/wireless/ti/wl12xx/Makefile b/drivers/net/wireless/ti/wl12xx/Makefile
new file mode 100644
index 00000000000..87f64b14db3
--- /dev/null
+++ b/drivers/net/wireless/ti/wl12xx/Makefile
@@ -0,0 +1,3 @@
+wl12xx-objs	= main.o cmd.o acx.o
+
+obj-$(CONFIG_WL12XX)		+= wl12xx.o
diff --git a/drivers/net/wireless/ti/wl12xx/acx.c b/drivers/net/wireless/ti/wl12xx/acx.c
new file mode 100644
index 00000000000..bea06b2d7bf
--- /dev/null
+++ b/drivers/net/wireless/ti/wl12xx/acx.c
@@ -0,0 +1,53 @@
+/*
+ * This file is part of wl12xx
+ *
+ * Copyright (C) 2008-2009 Nokia Corporation
+ * Copyright (C) 2011 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.
+ *
+ * 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 "../wlcore/cmd.h"
+#include "../wlcore/debug.h"
+#include "../wlcore/acx.h"
+
+#include "acx.h"
+
+int wl1271_acx_host_if_cfg_bitmap(struct wl1271 *wl, u32 host_cfg_bitmap)
+{
+	struct wl1271_acx_host_config_bitmap *bitmap_conf;
+	int ret;
+
+	bitmap_conf = kzalloc(sizeof(*bitmap_conf), GFP_KERNEL);
+	if (!bitmap_conf) {
+		ret = -ENOMEM;
+		goto out;
+	}
+
+	bitmap_conf->host_cfg_bitmap = cpu_to_le32(host_cfg_bitmap);
+
+	ret = wl1271_cmd_configure(wl, ACX_HOST_IF_CFG_BITMAP,
+				   bitmap_conf, sizeof(*bitmap_conf));
+	if (ret < 0) {
+		wl1271_warning("wl1271 bitmap config opt failed: %d", ret);
+		goto out;
+	}
+
+out:
+	kfree(bitmap_conf);
+
+	return ret;
+}
diff --git a/drivers/net/wireless/ti/wl12xx/acx.h b/drivers/net/wireless/ti/wl12xx/acx.h
new file mode 100644
index 00000000000..d1f5aba0afc
--- /dev/null
+++ b/drivers/net/wireless/ti/wl12xx/acx.h
@@ -0,0 +1,36 @@
+/*
+ * This file is part of wl12xx
+ *
+ * Copyright (C) 1998-2009, 2011 Texas Instruments. All rights reserved.
+ * Copyright (C) 2008-2010 Nokia 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 published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+ * General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA
+ * 02110-1301 USA
+ *
+ */
+
+#ifndef __WL12XX_ACX_H__
+#define __WL12XX_ACX_H__
+
+#include "../wlcore/wlcore.h"
+
+struct wl1271_acx_host_config_bitmap {
+	struct acx_header header;
+
+	__le32 host_cfg_bitmap;
+} __packed;
+
+int wl1271_acx_host_if_cfg_bitmap(struct wl1271 *wl, u32 host_cfg_bitmap);
+
+#endif /* __WL12XX_ACX_H__ */
diff --git a/drivers/net/wireless/ti/wl12xx/cmd.c b/drivers/net/wireless/ti/wl12xx/cmd.c
new file mode 100644
index 00000000000..8ffaeb5f214
--- /dev/null
+++ b/drivers/net/wireless/ti/wl12xx/cmd.c
@@ -0,0 +1,254 @@
+/*
+ * This file is part of wl12xx
+ *
+ * Copyright (C) 2009-2010 Nokia Corporation
+ * Copyright (C) 2011 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.
+ *
+ * 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 "../wlcore/cmd.h"
+#include "../wlcore/debug.h"
+
+#include "wl12xx.h"
+#include "cmd.h"
+
+int wl1271_cmd_ext_radio_parms(struct wl1271 *wl)
+{
+	struct wl1271_ext_radio_parms_cmd *ext_radio_parms;
+	struct wl12xx_priv *priv = wl->priv;
+	struct wl12xx_conf_rf *rf = &priv->conf.rf;
+	int ret;
+
+	if (!wl->nvs)
+		return -ENODEV;
+
+	ext_radio_parms = kzalloc(sizeof(*ext_radio_parms), GFP_KERNEL);
+	if (!ext_radio_parms)
+		return -ENOMEM;
+
+	ext_radio_parms->test.id = TEST_CMD_INI_FILE_RF_EXTENDED_PARAM;
+
+	memcpy(ext_radio_parms->tx_per_channel_power_compensation_2,
+	       rf->tx_per_channel_power_compensation_2,
+	       CONF_TX_PWR_COMPENSATION_LEN_2);
+	memcpy(ext_radio_parms->tx_per_channel_power_compensation_5,
+	       rf->tx_per_channel_power_compensation_5,
+	       CONF_TX_PWR_COMPENSATION_LEN_5);
+
+	wl1271_dump(DEBUG_CMD, "TEST_CMD_INI_FILE_EXT_RADIO_PARAM: ",
+		    ext_radio_parms, sizeof(*ext_radio_parms));
+
+	ret = wl1271_cmd_test(wl, ext_radio_parms, sizeof(*ext_radio_parms), 0);
+	if (ret < 0)
+		wl1271_warning("TEST_CMD_INI_FILE_RF_EXTENDED_PARAM failed");
+
+	kfree(ext_radio_parms);
+	return ret;
+}
+
+int wl1271_cmd_general_parms(struct wl1271 *wl)
+{
+	struct wl1271_general_parms_cmd *gen_parms;
+	struct wl1271_ini_general_params *gp =
+		&((struct wl1271_nvs_file *)wl->nvs)->general_params;
+	bool answer = false;
+	int ret;
+
+	if (!wl->nvs)
+		return -ENODEV;
+
+	if (gp->tx_bip_fem_manufacturer >= WL1271_INI_FEM_MODULE_COUNT) {
+		wl1271_warning("FEM index from INI out of bounds");
+		return -EINVAL;
+	}
+
+	gen_parms = kzalloc(sizeof(*gen_parms), GFP_KERNEL);
+	if (!gen_parms)
+		return -ENOMEM;
+
+	gen_parms->test.id = TEST_CMD_INI_FILE_GENERAL_PARAM;
+
+	memcpy(&gen_parms->general_params, gp, sizeof(*gp));
+
+	if (gp->tx_bip_fem_auto_detect)
+		answer = true;
+
+	/* Override the REF CLK from the NVS with the one from platform data */
+	gen_parms->general_params.ref_clock = wl->ref_clock;
+
+	ret = wl1271_cmd_test(wl, gen_parms, sizeof(*gen_parms), answer);
+	if (ret < 0) {
+		wl1271_warning("CMD_INI_FILE_GENERAL_PARAM failed");
+		goto out;
+	}
+
+	gp->tx_bip_fem_manufacturer =
+		gen_parms->general_params.tx_bip_fem_manufacturer;
+
+	if (gp->tx_bip_fem_manufacturer >= WL1271_INI_FEM_MODULE_COUNT) {
+		wl1271_warning("FEM index from FW out of bounds");
+		ret = -EINVAL;
+		goto out;
+	}
+
+	wl1271_debug(DEBUG_CMD, "FEM autodetect: %s, manufacturer: %d\n",
+		     answer ? "auto" : "manual", gp->tx_bip_fem_manufacturer);
+
+out:
+	kfree(gen_parms);
+	return ret;
+}
+
+int wl128x_cmd_general_parms(struct wl1271 *wl)
+{
+	struct wl128x_general_parms_cmd *gen_parms;
+	struct wl128x_ini_general_params *gp =
+		&((struct wl128x_nvs_file *)wl->nvs)->general_params;
+	bool answer = false;
+	int ret;
+
+	if (!wl->nvs)
+		return -ENODEV;
+
+	if (gp->tx_bip_fem_manufacturer >= WL1271_INI_FEM_MODULE_COUNT) {
+		wl1271_warning("FEM index from ini out of bounds");
+		return -EINVAL;
+	}
+
+	gen_parms = kzalloc(sizeof(*gen_parms), GFP_KERNEL);
+	if (!gen_parms)
+		return -ENOMEM;
+
+	gen_parms->test.id = TEST_CMD_INI_FILE_GENERAL_PARAM;
+
+	memcpy(&gen_parms->general_params, gp, sizeof(*gp));
+
+	if (gp->tx_bip_fem_auto_detect)
+		answer = true;
+
+	/* Replace REF and TCXO CLKs with the ones from platform data */
+	gen_parms->general_params.ref_clock = wl->ref_clock;
+	gen_parms->general_params.tcxo_ref_clock = wl->tcxo_clock;
+
+	ret = wl1271_cmd_test(wl, gen_parms, sizeof(*gen_parms), answer);
+	if (ret < 0) {
+		wl1271_warning("CMD_INI_FILE_GENERAL_PARAM failed");
+		goto out;
+	}
+
+	gp->tx_bip_fem_manufacturer =
+		gen_parms->general_params.tx_bip_fem_manufacturer;
+
+	if (gp->tx_bip_fem_manufacturer >= WL1271_INI_FEM_MODULE_COUNT) {
+		wl1271_warning("FEM index from FW out of bounds");
+		ret = -EINVAL;
+		goto out;
+	}
+
+	wl1271_debug(DEBUG_CMD, "FEM autodetect: %s, manufacturer: %d\n",
+		     answer ? "auto" : "manual", gp->tx_bip_fem_manufacturer);
+
+out:
+	kfree(gen_parms);
+	return ret;
+}
+
+int wl1271_cmd_radio_parms(struct wl1271 *wl)
+{
+	struct wl1271_nvs_file *nvs = (struct wl1271_nvs_file *)wl->nvs;
+	struct wl1271_radio_parms_cmd *radio_parms;
+	struct wl1271_ini_general_params *gp = &nvs->general_params;
+	int ret;
+
+	if (!wl->nvs)
+		return -ENODEV;
+
+	radio_parms = kzalloc(sizeof(*radio_parms), GFP_KERNEL);
+	if (!radio_parms)
+		return -ENOMEM;
+
+	radio_parms->test.id = TEST_CMD_INI_FILE_RADIO_PARAM;
+
+	/* 2.4GHz parameters */
+	memcpy(&radio_parms->static_params_2, &nvs->stat_radio_params_2,
+	       sizeof(struct wl1271_ini_band_params_2));
+	memcpy(&radio_parms->dyn_params_2,
+	       &nvs->dyn_radio_params_2[gp->tx_bip_fem_manufacturer].params,
+	       sizeof(struct wl1271_ini_fem_params_2));
+
+	/* 5GHz parameters */
+	memcpy(&radio_parms->static_params_5,
+	       &nvs->stat_radio_params_5,
+	       sizeof(struct wl1271_ini_band_params_5));
+	memcpy(&radio_parms->dyn_params_5,
+	       &nvs->dyn_radio_params_5[gp->tx_bip_fem_manufacturer].params,
+	       sizeof(struct wl1271_ini_fem_params_5));
+
+	wl1271_dump(DEBUG_CMD, "TEST_CMD_INI_FILE_RADIO_PARAM: ",
+		    radio_parms, sizeof(*radio_parms));
+
+	ret = wl1271_cmd_test(wl, radio_parms, sizeof(*radio_parms), 0);
+	if (ret < 0)
+		wl1271_warning("CMD_INI_FILE_RADIO_PARAM failed");
+
+	kfree(radio_parms);
+	return ret;
+}
+
+int wl128x_cmd_radio_parms(struct wl1271 *wl)
+{
+	struct wl128x_nvs_file *nvs = (struct wl128x_nvs_file *)wl->nvs;
+	struct wl128x_radio_parms_cmd *radio_parms;
+	struct wl128x_ini_general_params *gp = &nvs->general_params;
+	int ret;
+
+	if (!wl->nvs)
+		return -ENODEV;
+
+	radio_parms = kzalloc(sizeof(*radio_parms), GFP_KERNEL);
+	if (!radio_parms)
+		return -ENOMEM;
+
+	radio_parms->test.id = TEST_CMD_INI_FILE_RADIO_PARAM;
+
+	/* 2.4GHz parameters */
+	memcpy(&radio_parms->static_params_2, &nvs->stat_radio_params_2,
+	       sizeof(struct wl128x_ini_band_params_2));
+	memcpy(&radio_parms->dyn_params_2,
+	       &nvs->dyn_radio_params_2[gp->tx_bip_fem_manufacturer].params,
+	       sizeof(struct wl128x_ini_fem_params_2));
+
+	/* 5GHz parameters */
+	memcpy(&radio_parms->static_params_5,
+	       &nvs->stat_radio_params_5,
+	       sizeof(struct wl128x_ini_band_params_5));
+	memcpy(&radio_parms->dyn_params_5,
+	       &nvs->dyn_radio_params_5[gp->tx_bip_fem_manufacturer].params,
+	       sizeof(struct wl128x_ini_fem_params_5));
+
+	radio_parms->fem_vendor_and_options = nvs->fem_vendor_and_options;
+
+	wl1271_dump(DEBUG_CMD, "TEST_CMD_INI_FILE_RADIO_PARAM: ",
+		    radio_parms, sizeof(*radio_parms));
+
+	ret = wl1271_cmd_test(wl, radio_parms, sizeof(*radio_parms), 0);
+	if (ret < 0)
+		wl1271_warning("CMD_INI_FILE_RADIO_PARAM failed");
+
+	kfree(radio_parms);
+	return ret;
+}
diff --git a/drivers/net/wireless/ti/wl12xx/cmd.h b/drivers/net/wireless/ti/wl12xx/cmd.h
new file mode 100644
index 00000000000..140a0e8829d
--- /dev/null
+++ b/drivers/net/wireless/ti/wl12xx/cmd.h
@@ -0,0 +1,112 @@
+/*
+ * This file is part of wl12xx
+ *
+ * Copyright (C) 1998-2009, 2011 Texas Instruments. All rights reserved.
+ * Copyright (C) 2009 Nokia 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 published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+ * General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA
+ * 02110-1301 USA
+ *
+ */
+
+#ifndef __WL12XX_CMD_H__
+#define __WL12XX_CMD_H__
+
+#include "conf.h"
+
+#define TEST_CMD_INI_FILE_RADIO_PARAM       0x19
+#define TEST_CMD_INI_FILE_GENERAL_PARAM     0x1E
+
+struct wl1271_general_parms_cmd {
+	struct wl1271_cmd_header header;
+
+	struct wl1271_cmd_test_header test;
+
+	struct wl1271_ini_general_params general_params;
+
+	u8 sr_debug_table[WL1271_INI_MAX_SMART_REFLEX_PARAM];
+	u8 sr_sen_n_p;
+	u8 sr_sen_n_p_gain;
+	u8 sr_sen_nrn;
+	u8 sr_sen_prn;
+	u8 padding[3];
+} __packed;
+
+struct wl128x_general_parms_cmd {
+	struct wl1271_cmd_header header;
+
+	struct wl1271_cmd_test_header test;
+
+	struct wl128x_ini_general_params general_params;
+
+	u8 sr_debug_table[WL1271_INI_MAX_SMART_REFLEX_PARAM];
+	u8 sr_sen_n_p;
+	u8 sr_sen_n_p_gain;
+	u8 sr_sen_nrn;
+	u8 sr_sen_prn;
+	u8 padding[3];
+} __packed;
+
+struct wl1271_radio_parms_cmd {
+	struct wl1271_cmd_header header;
+
+	struct wl1271_cmd_test_header test;
+
+	/* Static radio parameters */
+	struct wl1271_ini_band_params_2 static_params_2;
+	struct wl1271_ini_band_params_5 static_params_5;
+
+	/* Dynamic radio parameters */
+	struct wl1271_ini_fem_params_2 dyn_params_2;
+	u8 padding2;
+	struct wl1271_ini_fem_params_5 dyn_params_5;
+	u8 padding3[2];
+} __packed;
+
+struct wl128x_radio_parms_cmd {
+	struct wl1271_cmd_header header;
+
+	struct wl1271_cmd_test_header test;
+
+	/* Static radio parameters */
+	struct wl128x_ini_band_params_2 static_params_2;
+	struct wl128x_ini_band_params_5 static_params_5;
+
+	u8 fem_vendor_and_options;
+
+	/* Dynamic radio parameters */
+	struct wl128x_ini_fem_params_2 dyn_params_2;
+	u8 padding2;
+	struct wl128x_ini_fem_params_5 dyn_params_5;
+} __packed;
+
+#define TEST_CMD_INI_FILE_RF_EXTENDED_PARAM 0x26
+
+struct wl1271_ext_radio_parms_cmd {
+	struct wl1271_cmd_header header;
+
+	struct wl1271_cmd_test_header test;
+
+	u8 tx_per_channel_power_compensation_2[CONF_TX_PWR_COMPENSATION_LEN_2];
+	u8 tx_per_channel_power_compensation_5[CONF_TX_PWR_COMPENSATION_LEN_5];
+	u8 padding[3];
+} __packed;
+
+int wl1271_cmd_general_parms(struct wl1271 *wl);
+int wl128x_cmd_general_parms(struct wl1271 *wl);
+int wl1271_cmd_radio_parms(struct wl1271 *wl);
+int wl128x_cmd_radio_parms(struct wl1271 *wl);
+int wl1271_cmd_ext_radio_parms(struct wl1271 *wl);
+
+#endif /* __WL12XX_CMD_H__ */
diff --git a/drivers/net/wireless/ti/wl12xx/conf.h b/drivers/net/wireless/ti/wl12xx/conf.h
new file mode 100644
index 00000000000..75e29897a0f
--- /dev/null
+++ b/drivers/net/wireless/ti/wl12xx/conf.h
@@ -0,0 +1,50 @@
+/*
+ * This file is part of wl12xx
+ *
+ * Copyright (C) 2011 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.
+ *
+ * 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
+ *
+ */
+
+#ifndef __WL12XX_CONF_H__
+#define __WL12XX_CONF_H__
+
+/* these are number of channels on the band divided by two, rounded up */
+#define CONF_TX_PWR_COMPENSATION_LEN_2 7
+#define CONF_TX_PWR_COMPENSATION_LEN_5 18
+
+struct wl12xx_conf_rf {
+	/*
+	 * Per channel power compensation for 2.4GHz
+	 *
+	 * Range: s8
+	 */
+	u8 tx_per_channel_power_compensation_2[CONF_TX_PWR_COMPENSATION_LEN_2];
+
+	/*
+	 * Per channel power compensation for 5GHz
+	 *
+	 * Range: s8
+	 */
+	u8 tx_per_channel_power_compensation_5[CONF_TX_PWR_COMPENSATION_LEN_5];
+};
+
+struct wl12xx_priv_conf {
+	struct wl12xx_conf_rf rf;
+	struct conf_memory_settings mem_wl127x;
+};
+
+#endif /* __WL12XX_CONF_H__ */
diff --git a/drivers/net/wireless/ti/wl12xx/main.c b/drivers/net/wireless/ti/wl12xx/main.c
new file mode 100644
index 00000000000..d7dd3def07b
--- /dev/null
+++ b/drivers/net/wireless/ti/wl12xx/main.c
@@ -0,0 +1,1388 @@
+/*
+ * This file is part of wl1271
+ *
+ * Copyright (C) 2008-2010 Nokia 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 published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+ * General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA
+ * 02110-1301 USA
+ *
+ */
+
+#include <linux/module.h>
+#include <linux/platform_device.h>
+
+#include <linux/err.h>
+
+#include <linux/wl12xx.h>
+
+#include "../wlcore/wlcore.h"
+#include "../wlcore/debug.h"
+#include "../wlcore/io.h"
+#include "../wlcore/acx.h"
+#include "../wlcore/tx.h"
+#include "../wlcore/rx.h"
+#include "../wlcore/io.h"
+#include "../wlcore/boot.h"
+
+#include "wl12xx.h"
+#include "reg.h"
+#include "cmd.h"
+#include "acx.h"
+
+static struct wlcore_conf wl12xx_conf = {
+	.sg = {
+		.params = {
+			[CONF_SG_ACL_BT_MASTER_MIN_BR] = 10,
+			[CONF_SG_ACL_BT_MASTER_MAX_BR] = 180,
+			[CONF_SG_ACL_BT_SLAVE_MIN_BR] = 10,
+			[CONF_SG_ACL_BT_SLAVE_MAX_BR] = 180,
+			[CONF_SG_ACL_BT_MASTER_MIN_EDR] = 10,
+			[CONF_SG_ACL_BT_MASTER_MAX_EDR] = 80,
+			[CONF_SG_ACL_BT_SLAVE_MIN_EDR] = 10,
+			[CONF_SG_ACL_BT_SLAVE_MAX_EDR] = 80,
+			[CONF_SG_ACL_WLAN_PS_MASTER_BR] = 8,
+			[CONF_SG_ACL_WLAN_PS_SLAVE_BR] = 8,
+			[CONF_SG_ACL_WLAN_PS_MASTER_EDR] = 20,
+			[CONF_SG_ACL_WLAN_PS_SLAVE_EDR] = 20,
+			[CONF_SG_ACL_WLAN_ACTIVE_MASTER_MIN_BR] = 20,
+			[CONF_SG_ACL_WLAN_ACTIVE_MASTER_MAX_BR] = 35,
+			[CONF_SG_ACL_WLAN_ACTIVE_SLAVE_MIN_BR] = 16,
+			[CONF_SG_ACL_WLAN_ACTIVE_SLAVE_MAX_BR] = 35,
+			[CONF_SG_ACL_WLAN_ACTIVE_MASTER_MIN_EDR] = 32,
+			[CONF_SG_ACL_WLAN_ACTIVE_MASTER_MAX_EDR] = 50,
+			[CONF_SG_ACL_WLAN_ACTIVE_SLAVE_MIN_EDR] = 28,
+			[CONF_SG_ACL_WLAN_ACTIVE_SLAVE_MAX_EDR] = 50,
+			[CONF_SG_ACL_ACTIVE_SCAN_WLAN_BR] = 10,
+			[CONF_SG_ACL_ACTIVE_SCAN_WLAN_EDR] = 20,
+			[CONF_SG_ACL_PASSIVE_SCAN_BT_BR] = 75,
+			[CONF_SG_ACL_PASSIVE_SCAN_WLAN_BR] = 15,
+			[CONF_SG_ACL_PASSIVE_SCAN_BT_EDR] = 27,
+			[CONF_SG_ACL_PASSIVE_SCAN_WLAN_EDR] = 17,
+			/* active scan params */
+			[CONF_SG_AUTO_SCAN_PROBE_REQ] = 170,
+			[CONF_SG_ACTIVE_SCAN_DURATION_FACTOR_HV3] = 50,
+			[CONF_SG_ACTIVE_SCAN_DURATION_FACTOR_A2DP] = 100,
+			/* passive scan params */
+			[CONF_SG_PASSIVE_SCAN_DURATION_FACTOR_A2DP_BR] = 800,
+			[CONF_SG_PASSIVE_SCAN_DURATION_FACTOR_A2DP_EDR] = 200,
+			[CONF_SG_PASSIVE_SCAN_DURATION_FACTOR_HV3] = 200,
+			/* passive scan in dual antenna params */
+			[CONF_SG_CONSECUTIVE_HV3_IN_PASSIVE_SCAN] = 0,
+			[CONF_SG_BCN_HV3_COLLISION_THRESH_IN_PASSIVE_SCAN] = 0,
+			[CONF_SG_TX_RX_PROTECTION_BWIDTH_IN_PASSIVE_SCAN] = 0,
+			/* general params */
+			[CONF_SG_STA_FORCE_PS_IN_BT_SCO] = 1,
+			[CONF_SG_ANTENNA_CONFIGURATION] = 0,
+			[CONF_SG_BEACON_MISS_PERCENT] = 60,
+			[CONF_SG_DHCP_TIME] = 5000,
+			[CONF_SG_RXT] = 1200,
+			[CONF_SG_TXT] = 1000,
+			[CONF_SG_ADAPTIVE_RXT_TXT] = 1,
+			[CONF_SG_GENERAL_USAGE_BIT_MAP] = 3,
+			[CONF_SG_HV3_MAX_SERVED] = 6,
+			[CONF_SG_PS_POLL_TIMEOUT] = 10,
+			[CONF_SG_UPSD_TIMEOUT] = 10,
+			[CONF_SG_CONSECUTIVE_CTS_THRESHOLD] = 2,
+			[CONF_SG_STA_RX_WINDOW_AFTER_DTIM] = 5,
+			[CONF_SG_STA_CONNECTION_PROTECTION_TIME] = 30,
+			/* AP params */
+			[CONF_AP_BEACON_MISS_TX] = 3,
+			[CONF_AP_RX_WINDOW_AFTER_BEACON] = 10,
+			[CONF_AP_BEACON_WINDOW_INTERVAL] = 2,
+			[CONF_AP_CONNECTION_PROTECTION_TIME] = 0,
+			[CONF_AP_BT_ACL_VAL_BT_SERVE_TIME] = 25,
+			[CONF_AP_BT_ACL_VAL_WL_SERVE_TIME] = 25,
+			/* CTS Diluting params */
+			[CONF_SG_CTS_DILUTED_BAD_RX_PACKETS_TH] = 0,
+			[CONF_SG_CTS_CHOP_IN_DUAL_ANT_SCO_MASTER] = 0,
+		},
+		.state = CONF_SG_PROTECTIVE,
+	},
+	.rx = {
+		.rx_msdu_life_time           = 512000,
+		.packet_detection_threshold  = 0,
+		.ps_poll_timeout             = 15,
+		.upsd_timeout                = 15,
+		.rts_threshold               = IEEE80211_MAX_RTS_THRESHOLD,
+		.rx_cca_threshold            = 0,
+		.irq_blk_threshold           = 0xFFFF,
+		.irq_pkt_threshold           = 0,
+		.irq_timeout                 = 600,
+		.queue_type                  = CONF_RX_QUEUE_TYPE_LOW_PRIORITY,
+	},
+	.tx = {
+		.tx_energy_detection         = 0,
+		.sta_rc_conf                 = {
+			.enabled_rates       = 0,
+			.short_retry_limit   = 10,
+			.long_retry_limit    = 10,
+			.aflags              = 0,
+		},
+		.ac_conf_count               = 4,
+		.ac_conf                     = {
+			[CONF_TX_AC_BE] = {
+				.ac          = CONF_TX_AC_BE,
+				.cw_min      = 15,
+				.cw_max      = 63,
+				.aifsn       = 3,
+				.tx_op_limit = 0,
+			},
+			[CONF_TX_AC_BK] = {
+				.ac          = CONF_TX_AC_BK,
+				.cw_min      = 15,
+				.cw_max      = 63,
+				.aifsn       = 7,
+				.tx_op_limit = 0,
+			},
+			[CONF_TX_AC_VI] = {
+				.ac          = CONF_TX_AC_VI,
+				.cw_min      = 15,
+				.cw_max      = 63,
+				.aifsn       = CONF_TX_AIFS_PIFS,
+				.tx_op_limit = 3008,
+			},
+			[CONF_TX_AC_VO] = {
+				.ac          = CONF_TX_AC_VO,
+				.cw_min      = 15,
+				.cw_max      = 63,
+				.aifsn       = CONF_TX_AIFS_PIFS,
+				.tx_op_limit = 1504,
+			},
+		},
+		.max_tx_retries = 100,
+		.ap_aging_period = 300,
+		.tid_conf_count = 4,
+		.tid_conf = {
+			[CONF_TX_AC_BE] = {
+				.queue_id    = CONF_TX_AC_BE,
+				.channel_type = CONF_CHANNEL_TYPE_EDCF,
+				.tsid        = CONF_TX_AC_BE,
+				.ps_scheme   = CONF_PS_SCHEME_LEGACY,
+				.ack_policy  = CONF_ACK_POLICY_LEGACY,
+				.apsd_conf   = {0, 0},
+			},
+			[CONF_TX_AC_BK] = {
+				.queue_id    = CONF_TX_AC_BK,
+				.channel_type = CONF_CHANNEL_TYPE_EDCF,
+				.tsid        = CONF_TX_AC_BK,
+				.ps_scheme   = CONF_PS_SCHEME_LEGACY,
+				.ack_policy  = CONF_ACK_POLICY_LEGACY,
+				.apsd_conf   = {0, 0},
+			},
+			[CONF_TX_AC_VI] = {
+				.queue_id    = CONF_TX_AC_VI,
+				.channel_type = CONF_CHANNEL_TYPE_EDCF,
+				.tsid        = CONF_TX_AC_VI,
+				.ps_scheme   = CONF_PS_SCHEME_LEGACY,
+				.ack_policy  = CONF_ACK_POLICY_LEGACY,
+				.apsd_conf   = {0, 0},
+			},
+			[CONF_TX_AC_VO] = {
+				.queue_id    = CONF_TX_AC_VO,
+				.channel_type = CONF_CHANNEL_TYPE_EDCF,
+				.tsid        = CONF_TX_AC_VO,
+				.ps_scheme   = CONF_PS_SCHEME_LEGACY,
+				.ack_policy  = CONF_ACK_POLICY_LEGACY,
+				.apsd_conf   = {0, 0},
+			},
+		},
+		.frag_threshold              = IEEE80211_MAX_FRAG_THRESHOLD,
+		.tx_compl_timeout            = 700,
+		.tx_compl_threshold          = 4,
+		.basic_rate                  = CONF_HW_BIT_RATE_1MBPS,
+		.basic_rate_5                = CONF_HW_BIT_RATE_6MBPS,
+		.tmpl_short_retry_limit      = 10,
+		.tmpl_long_retry_limit       = 10,
+		.tx_watchdog_timeout         = 5000,
+	},
+	.conn = {
+		.wake_up_event               = CONF_WAKE_UP_EVENT_DTIM,
+		.listen_interval             = 1,
+		.suspend_wake_up_event       = CONF_WAKE_UP_EVENT_N_DTIM,
+		.suspend_listen_interval     = 3,
+		.bcn_filt_mode               = CONF_BCN_FILT_MODE_ENABLED,
+		.bcn_filt_ie_count           = 2,
+		.bcn_filt_ie = {
+			[0] = {
+				.ie          = WLAN_EID_CHANNEL_SWITCH,
+				.rule        = CONF_BCN_RULE_PASS_ON_APPEARANCE,
+			},
+			[1] = {
+				.ie          = WLAN_EID_HT_OPERATION,
+				.rule        = CONF_BCN_RULE_PASS_ON_CHANGE,
+			},
+		},
+		.synch_fail_thold            = 10,
+		.bss_lose_timeout            = 100,
+		.beacon_rx_timeout           = 10000,
+		.broadcast_timeout           = 20000,
+		.rx_broadcast_in_ps          = 1,
+		.ps_poll_threshold           = 10,
+		.bet_enable                  = CONF_BET_MODE_ENABLE,
+		.bet_max_consecutive         = 50,
+		.psm_entry_retries           = 8,
+		.psm_exit_retries            = 16,
+		.psm_entry_nullfunc_retries  = 3,
+		.dynamic_ps_timeout          = 40,
+		.forced_ps                   = false,
+		.keep_alive_interval         = 55000,
+		.max_listen_interval         = 20,
+	},
+	.itrim = {
+		.enable = false,
+		.timeout = 50000,
+	},
+	.pm_config = {
+		.host_clk_settling_time = 5000,
+		.host_fast_wakeup_support = false
+	},
+	.roam_trigger = {
+		.trigger_pacing               = 1,
+		.avg_weight_rssi_beacon       = 20,
+		.avg_weight_rssi_data         = 10,
+		.avg_weight_snr_beacon        = 20,
+		.avg_weight_snr_data          = 10,
+	},
+	.scan = {
+		.min_dwell_time_active        = 7500,
+		.max_dwell_time_active        = 30000,
+		.min_dwell_time_passive       = 100000,
+		.max_dwell_time_passive       = 100000,
+		.num_probe_reqs               = 2,
+		.split_scan_timeout           = 50000,
+	},
+	.sched_scan = {
+		/*
+		 * Values are in TU/1000 but since sched scan FW command
+		 * params are in TUs rounding up may occur.
+		 */
+		.base_dwell_time		= 7500,
+		.max_dwell_time_delta		= 22500,
+		/* based on 250bits per probe @1Mbps */
+		.dwell_time_delta_per_probe	= 2000,
+		/* based on 250bits per probe @6Mbps (plus a bit more) */
+		.dwell_time_delta_per_probe_5	= 350,
+		.dwell_time_passive		= 100000,
+		.dwell_time_dfs			= 150000,
+		.num_probe_reqs			= 2,
+		.rssi_threshold			= -90,
+		.snr_threshold			= 0,
+	},
+	.ht = {
+		.rx_ba_win_size = 8,
+		.tx_ba_win_size = 64,
+		.inactivity_timeout = 10000,
+		.tx_ba_tid_bitmap = CONF_TX_BA_ENABLED_TID_BITMAP,
+	},
+	/*
+	 * Memory config for wl127x chips is given in the
+	 * wl12xx_default_priv_conf struct. The below configuration is
+	 * for wl128x chips.
+	 */
+	.mem = {
+		.num_stations                 = 1,
+		.ssid_profiles                = 1,
+		.rx_block_num                 = 40,
+		.tx_min_block_num             = 40,
+		.dynamic_memory               = 1,
+		.min_req_tx_blocks            = 45,
+		.min_req_rx_blocks            = 22,
+		.tx_min                       = 27,
+	},
+	.fm_coex = {
+		.enable                       = true,
+		.swallow_period               = 5,
+		.n_divider_fref_set_1         = 0xff,       /* default */
+		.n_divider_fref_set_2         = 12,
+		.m_divider_fref_set_1         = 148,
+		.m_divider_fref_set_2         = 0xffff,     /* default */
+		.coex_pll_stabilization_time  = 0xffffffff, /* default */
+		.ldo_stabilization_time       = 0xffff,     /* default */
+		.fm_disturbed_band_margin     = 0xff,       /* default */
+		.swallow_clk_diff             = 0xff,       /* default */
+	},
+	.rx_streaming = {
+		.duration                      = 150,
+		.queues                        = 0x1,
+		.interval                      = 20,
+		.always                        = 0,
+	},
+	.fwlog = {
+		.mode                         = WL12XX_FWLOG_ON_DEMAND,
+		.mem_blocks                   = 2,
+		.severity                     = 0,
+		.timestamp                    = WL12XX_FWLOG_TIMESTAMP_DISABLED,
+		.output                       = WL12XX_FWLOG_OUTPUT_HOST,
+		.threshold                    = 0,
+	},
+	.rate = {
+		.rate_retry_score = 32000,
+		.per_add = 8192,
+		.per_th1 = 2048,
+		.per_th2 = 4096,
+		.max_per = 8100,
+		.inverse_curiosity_factor = 5,
+		.tx_fail_low_th = 4,
+		.tx_fail_high_th = 10,
+		.per_alpha_shift = 4,
+		.per_add_shift = 13,
+		.per_beta1_shift = 10,
+		.per_beta2_shift = 8,
+		.rate_check_up = 2,
+		.rate_check_down = 12,
+		.rate_retry_policy = {
+			0x00, 0x00, 0x00, 0x00, 0x00,
+			0x00, 0x00, 0x00, 0x00, 0x00,
+			0x00, 0x00, 0x00,
+		},
+	},
+	.hangover = {
+		.recover_time               = 0,
+		.hangover_period            = 20,
+		.dynamic_mode               = 1,
+		.early_termination_mode     = 1,
+		.max_period                 = 20,
+		.min_period                 = 1,
+		.increase_delta             = 1,
+		.decrease_delta             = 2,
+		.quiet_time                 = 4,
+		.increase_time              = 1,
+		.window_size                = 16,
+	},
+};
+
+static struct wl12xx_priv_conf wl12xx_default_priv_conf = {
+	.rf = {
+		.tx_per_channel_power_compensation_2 = {
+			0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+		},
+		.tx_per_channel_power_compensation_5 = {
+			0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+			0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+			0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+		},
+	},
+	.mem_wl127x = {
+		.num_stations                 = 1,
+		.ssid_profiles                = 1,
+		.rx_block_num                 = 70,
+		.tx_min_block_num             = 40,
+		.dynamic_memory               = 1,
+		.min_req_tx_blocks            = 100,
+		.min_req_rx_blocks            = 22,
+		.tx_min                       = 27,
+	},
+
+};
+
+#define WL12XX_TX_HW_BLOCK_SPARE_DEFAULT        1
+#define WL12XX_TX_HW_BLOCK_GEM_SPARE            2
+#define WL12XX_TX_HW_BLOCK_SIZE                 252
+
+static const u8 wl12xx_rate_to_idx_2ghz[] = {
+	/* MCS rates are used only with 11n */
+	7,                             /* WL12XX_CONF_HW_RXTX_RATE_MCS7_SGI */
+	7,                             /* WL12XX_CONF_HW_RXTX_RATE_MCS7 */
+	6,                             /* WL12XX_CONF_HW_RXTX_RATE_MCS6 */
+	5,                             /* WL12XX_CONF_HW_RXTX_RATE_MCS5 */
+	4,                             /* WL12XX_CONF_HW_RXTX_RATE_MCS4 */
+	3,                             /* WL12XX_CONF_HW_RXTX_RATE_MCS3 */
+	2,                             /* WL12XX_CONF_HW_RXTX_RATE_MCS2 */
+	1,                             /* WL12XX_CONF_HW_RXTX_RATE_MCS1 */
+	0,                             /* WL12XX_CONF_HW_RXTX_RATE_MCS0 */
+
+	11,                            /* WL12XX_CONF_HW_RXTX_RATE_54   */
+	10,                            /* WL12XX_CONF_HW_RXTX_RATE_48   */
+	9,                             /* WL12XX_CONF_HW_RXTX_RATE_36   */
+	8,                             /* WL12XX_CONF_HW_RXTX_RATE_24   */
+
+	/* TI-specific rate */
+	CONF_HW_RXTX_RATE_UNSUPPORTED, /* WL12XX_CONF_HW_RXTX_RATE_22   */
+
+	7,                             /* WL12XX_CONF_HW_RXTX_RATE_18   */
+	6,                             /* WL12XX_CONF_HW_RXTX_RATE_12   */
+	3,                             /* WL12XX_CONF_HW_RXTX_RATE_11   */
+	5,                             /* WL12XX_CONF_HW_RXTX_RATE_9    */
+	4,                             /* WL12XX_CONF_HW_RXTX_RATE_6    */
+	2,                             /* WL12XX_CONF_HW_RXTX_RATE_5_5  */
+	1,                             /* WL12XX_CONF_HW_RXTX_RATE_2    */
+	0                              /* WL12XX_CONF_HW_RXTX_RATE_1    */
+};
+
+static const u8 wl12xx_rate_to_idx_5ghz[] = {
+	/* MCS rates are used only with 11n */
+	7,                             /* WL12XX_CONF_HW_RXTX_RATE_MCS7_SGI */
+	7,                             /* WL12XX_CONF_HW_RXTX_RATE_MCS7 */
+	6,                             /* WL12XX_CONF_HW_RXTX_RATE_MCS6 */
+	5,                             /* WL12XX_CONF_HW_RXTX_RATE_MCS5 */
+	4,                             /* WL12XX_CONF_HW_RXTX_RATE_MCS4 */
+	3,                             /* WL12XX_CONF_HW_RXTX_RATE_MCS3 */
+	2,                             /* WL12XX_CONF_HW_RXTX_RATE_MCS2 */
+	1,                             /* WL12XX_CONF_HW_RXTX_RATE_MCS1 */
+	0,                             /* WL12XX_CONF_HW_RXTX_RATE_MCS0 */
+
+	7,                             /* WL12XX_CONF_HW_RXTX_RATE_54   */
+	6,                             /* WL12XX_CONF_HW_RXTX_RATE_48   */
+	5,                             /* WL12XX_CONF_HW_RXTX_RATE_36   */
+	4,                             /* WL12XX_CONF_HW_RXTX_RATE_24   */
+
+	/* TI-specific rate */
+	CONF_HW_RXTX_RATE_UNSUPPORTED, /* WL12XX_CONF_HW_RXTX_RATE_22   */
+
+	3,                             /* WL12XX_CONF_HW_RXTX_RATE_18   */
+	2,                             /* WL12XX_CONF_HW_RXTX_RATE_12   */
+	CONF_HW_RXTX_RATE_UNSUPPORTED, /* WL12XX_CONF_HW_RXTX_RATE_11   */
+	1,                             /* WL12XX_CONF_HW_RXTX_RATE_9    */
+	0,                             /* WL12XX_CONF_HW_RXTX_RATE_6    */
+	CONF_HW_RXTX_RATE_UNSUPPORTED, /* WL12XX_CONF_HW_RXTX_RATE_5_5  */
+	CONF_HW_RXTX_RATE_UNSUPPORTED, /* WL12XX_CONF_HW_RXTX_RATE_2    */
+	CONF_HW_RXTX_RATE_UNSUPPORTED  /* WL12XX_CONF_HW_RXTX_RATE_1    */
+};
+
+static const u8 *wl12xx_band_rate_to_idx[] = {
+	[IEEE80211_BAND_2GHZ] = wl12xx_rate_to_idx_2ghz,
+	[IEEE80211_BAND_5GHZ] = wl12xx_rate_to_idx_5ghz
+};
+
+enum wl12xx_hw_rates {
+	WL12XX_CONF_HW_RXTX_RATE_MCS7_SGI = 0,
+	WL12XX_CONF_HW_RXTX_RATE_MCS7,
+	WL12XX_CONF_HW_RXTX_RATE_MCS6,
+	WL12XX_CONF_HW_RXTX_RATE_MCS5,
+	WL12XX_CONF_HW_RXTX_RATE_MCS4,
+	WL12XX_CONF_HW_RXTX_RATE_MCS3,
+	WL12XX_CONF_HW_RXTX_RATE_MCS2,
+	WL12XX_CONF_HW_RXTX_RATE_MCS1,
+	WL12XX_CONF_HW_RXTX_RATE_MCS0,
+	WL12XX_CONF_HW_RXTX_RATE_54,
+	WL12XX_CONF_HW_RXTX_RATE_48,
+	WL12XX_CONF_HW_RXTX_RATE_36,
+	WL12XX_CONF_HW_RXTX_RATE_24,
+	WL12XX_CONF_HW_RXTX_RATE_22,
+	WL12XX_CONF_HW_RXTX_RATE_18,
+	WL12XX_CONF_HW_RXTX_RATE_12,
+	WL12XX_CONF_HW_RXTX_RATE_11,
+	WL12XX_CONF_HW_RXTX_RATE_9,
+	WL12XX_CONF_HW_RXTX_RATE_6,
+	WL12XX_CONF_HW_RXTX_RATE_5_5,
+	WL12XX_CONF_HW_RXTX_RATE_2,
+	WL12XX_CONF_HW_RXTX_RATE_1,
+	WL12XX_CONF_HW_RXTX_RATE_MAX,
+};
+
+static struct wlcore_partition_set wl12xx_ptable[PART_TABLE_LEN] = {
+	[PART_DOWN] = {
+		.mem = {
+			.start = 0x00000000,
+			.size  = 0x000177c0
+		},
+		.reg = {
+			.start = REGISTERS_BASE,
+			.size  = 0x00008800
+		},
+		.mem2 = {
+			.start = 0x00000000,
+			.size  = 0x00000000
+		},
+		.mem3 = {
+			.start = 0x00000000,
+			.size  = 0x00000000
+		},
+	},
+
+	[PART_BOOT] = { /* in wl12xx we can use a mix of work and down
+			 * partition here */
+		.mem = {
+			.start = 0x00040000,
+			.size  = 0x00014fc0
+		},
+		.reg = {
+			.start = REGISTERS_BASE,
+			.size  = 0x00008800
+		},
+		.mem2 = {
+			.start = 0x00000000,
+			.size  = 0x00000000
+		},
+		.mem3 = {
+			.start = 0x00000000,
+			.size  = 0x00000000
+		},
+	},
+
+	[PART_WORK] = {
+		.mem = {
+			.start = 0x00040000,
+			.size  = 0x00014fc0
+		},
+		.reg = {
+			.start = REGISTERS_BASE,
+			.size  = 0x0000a000
+		},
+		.mem2 = {
+			.start = 0x003004f8,
+			.size  = 0x00000004
+		},
+		.mem3 = {
+			.start = 0x00040404,
+			.size  = 0x00000000
+		},
+	},
+
+	[PART_DRPW] = {
+		.mem = {
+			.start = 0x00040000,
+			.size  = 0x00014fc0
+		},
+		.reg = {
+			.start = DRPW_BASE,
+			.size  = 0x00006000
+		},
+		.mem2 = {
+			.start = 0x00000000,
+			.size  = 0x00000000
+		},
+		.mem3 = {
+			.start = 0x00000000,
+			.size  = 0x00000000
+		}
+	}
+};
+
+static const int wl12xx_rtable[REG_TABLE_LEN] = {
+	[REG_ECPU_CONTROL]		= WL12XX_REG_ECPU_CONTROL,
+	[REG_INTERRUPT_NO_CLEAR]	= WL12XX_REG_INTERRUPT_NO_CLEAR,
+	[REG_INTERRUPT_ACK]		= WL12XX_REG_INTERRUPT_ACK,
+	[REG_COMMAND_MAILBOX_PTR]	= WL12XX_REG_COMMAND_MAILBOX_PTR,
+	[REG_EVENT_MAILBOX_PTR]		= WL12XX_REG_EVENT_MAILBOX_PTR,
+	[REG_INTERRUPT_TRIG]		= WL12XX_REG_INTERRUPT_TRIG,
+	[REG_INTERRUPT_MASK]		= WL12XX_REG_INTERRUPT_MASK,
+	[REG_PC_ON_RECOVERY]		= WL12XX_SCR_PAD4,
+	[REG_CHIP_ID_B]			= WL12XX_CHIP_ID_B,
+	[REG_CMD_MBOX_ADDRESS]		= WL12XX_CMD_MBOX_ADDRESS,
+
+	/* data access memory addresses, used with partition translation */
+	[REG_SLV_MEM_DATA]		= WL1271_SLV_MEM_DATA,
+	[REG_SLV_REG_DATA]		= WL1271_SLV_REG_DATA,
+
+	/* raw data access memory addresses */
+	[REG_RAW_FW_STATUS_ADDR]	= FW_STATUS_ADDR,
+};
+
+/* TODO: maybe move to a new header file? */
+#define WL127X_FW_NAME_MULTI	"ti-connectivity/wl127x-fw-4-mr.bin"
+#define WL127X_FW_NAME_SINGLE	"ti-connectivity/wl127x-fw-4-sr.bin"
+#define WL127X_PLT_FW_NAME	"ti-connectivity/wl127x-fw-4-plt.bin"
+
+#define WL128X_FW_NAME_MULTI	"ti-connectivity/wl128x-fw-4-mr.bin"
+#define WL128X_FW_NAME_SINGLE	"ti-connectivity/wl128x-fw-4-sr.bin"
+#define WL128X_PLT_FW_NAME	"ti-connectivity/wl128x-fw-4-plt.bin"
+
+static void wl127x_prepare_read(struct wl1271 *wl, u32 rx_desc, u32 len)
+{
+	if (wl->chip.id != CHIP_ID_1283_PG20) {
+		struct wl1271_acx_mem_map *wl_mem_map = wl->target_mem_map;
+		struct wl1271_rx_mem_pool_addr rx_mem_addr;
+
+		/*
+		 * Choose the block we want to read
+		 * For aggregated packets, only the first memory block
+		 * should be retrieved. The FW takes care of the rest.
+		 */
+		u32 mem_block = rx_desc & RX_MEM_BLOCK_MASK;
+
+		rx_mem_addr.addr = (mem_block << 8) +
+			le32_to_cpu(wl_mem_map->packet_memory_pool_start);
+
+		rx_mem_addr.addr_extra = rx_mem_addr.addr + 4;
+
+		wl1271_write(wl, WL1271_SLV_REG_DATA,
+			     &rx_mem_addr, sizeof(rx_mem_addr), false);
+	}
+}
+
+static int wl12xx_identify_chip(struct wl1271 *wl)
+{
+	int ret = 0;
+
+	switch (wl->chip.id) {
+	case CHIP_ID_1271_PG10:
+		wl1271_warning("chip id 0x%x (1271 PG10) support is obsolete",
+			       wl->chip.id);
+
+		/* clear the alignment quirk, since we don't support it */
+		wl->quirks &= ~WLCORE_QUIRK_TX_BLOCKSIZE_ALIGN;
+
+		wl->quirks |= WLCORE_QUIRK_LEGACY_NVS;
+		wl->sr_fw_name = WL127X_FW_NAME_SINGLE;
+		wl->mr_fw_name = WL127X_FW_NAME_MULTI;
+		memcpy(&wl->conf.mem, &wl12xx_default_priv_conf.mem_wl127x,
+		       sizeof(wl->conf.mem));
+
+		/* read data preparation is only needed by wl127x */
+		wl->ops->prepare_read = wl127x_prepare_read;
+
+		break;
+
+	case CHIP_ID_1271_PG20:
+		wl1271_debug(DEBUG_BOOT, "chip id 0x%x (1271 PG20)",
+			     wl->chip.id);
+
+		/* clear the alignment quirk, since we don't support it */
+		wl->quirks &= ~WLCORE_QUIRK_TX_BLOCKSIZE_ALIGN;
+
+		wl->quirks |= WLCORE_QUIRK_LEGACY_NVS;
+		wl->plt_fw_name = WL127X_PLT_FW_NAME;
+		wl->sr_fw_name = WL127X_FW_NAME_SINGLE;
+		wl->mr_fw_name = WL127X_FW_NAME_MULTI;
+		memcpy(&wl->conf.mem, &wl12xx_default_priv_conf.mem_wl127x,
+		       sizeof(wl->conf.mem));
+
+		/* read data preparation is only needed by wl127x */
+		wl->ops->prepare_read = wl127x_prepare_read;
+
+		break;
+
+	case CHIP_ID_1283_PG20:
+		wl1271_debug(DEBUG_BOOT, "chip id 0x%x (1283 PG20)",
+			     wl->chip.id);
+		wl->plt_fw_name = WL128X_PLT_FW_NAME;
+		wl->sr_fw_name = WL128X_FW_NAME_SINGLE;
+		wl->mr_fw_name = WL128X_FW_NAME_MULTI;
+		break;
+	case CHIP_ID_1283_PG10:
+	default:
+		wl1271_warning("unsupported chip id: 0x%x", wl->chip.id);
+		ret = -ENODEV;
+		goto out;
+	}
+
+out:
+	return ret;
+}
+
+static void wl12xx_top_reg_write(struct wl1271 *wl, int addr, u16 val)
+{
+	/* write address >> 1 + 0x30000 to OCP_POR_CTR */
+	addr = (addr >> 1) + 0x30000;
+	wl1271_write32(wl, WL12XX_OCP_POR_CTR, addr);
+
+	/* write value to OCP_POR_WDATA */
+	wl1271_write32(wl, WL12XX_OCP_DATA_WRITE, val);
+
+	/* write 1 to OCP_CMD */
+	wl1271_write32(wl, WL12XX_OCP_CMD, OCP_CMD_WRITE);
+}
+
+static u16 wl12xx_top_reg_read(struct wl1271 *wl, int addr)
+{
+	u32 val;
+	int timeout = OCP_CMD_LOOP;
+
+	/* write address >> 1 + 0x30000 to OCP_POR_CTR */
+	addr = (addr >> 1) + 0x30000;
+	wl1271_write32(wl, WL12XX_OCP_POR_CTR, addr);
+
+	/* write 2 to OCP_CMD */
+	wl1271_write32(wl, WL12XX_OCP_CMD, OCP_CMD_READ);
+
+	/* poll for data ready */
+	do {
+		val = wl1271_read32(wl, WL12XX_OCP_DATA_READ);
+	} while (!(val & OCP_READY_MASK) && --timeout);
+
+	if (!timeout) {
+		wl1271_warning("Top register access timed out.");
+		return 0xffff;
+	}
+
+	/* check data status and return if OK */
+	if ((val & OCP_STATUS_MASK) == OCP_STATUS_OK)
+		return val & 0xffff;
+	else {
+		wl1271_warning("Top register access returned error.");
+		return 0xffff;
+	}
+}
+
+static int wl128x_switch_tcxo_to_fref(struct wl1271 *wl)
+{
+	u16 spare_reg;
+
+	/* Mask bits [2] & [8:4] in the sys_clk_cfg register */
+	spare_reg = wl12xx_top_reg_read(wl, WL_SPARE_REG);
+	if (spare_reg == 0xFFFF)
+		return -EFAULT;
+	spare_reg |= (BIT(3) | BIT(5) | BIT(6));
+	wl12xx_top_reg_write(wl, WL_SPARE_REG, spare_reg);
+
+	/* Enable FREF_CLK_REQ & mux MCS and coex PLLs to FREF */
+	wl12xx_top_reg_write(wl, SYS_CLK_CFG_REG,
+			     WL_CLK_REQ_TYPE_PG2 | MCS_PLL_CLK_SEL_FREF);
+
+	/* Delay execution for 15msec, to let the HW settle */
+	mdelay(15);
+
+	return 0;
+}
+
+static bool wl128x_is_tcxo_valid(struct wl1271 *wl)
+{
+	u16 tcxo_detection;
+
+	tcxo_detection = wl12xx_top_reg_read(wl, TCXO_CLK_DETECT_REG);
+	if (tcxo_detection & TCXO_DET_FAILED)
+		return false;
+
+	return true;
+}
+
+static bool wl128x_is_fref_valid(struct wl1271 *wl)
+{
+	u16 fref_detection;
+
+	fref_detection = wl12xx_top_reg_read(wl, FREF_CLK_DETECT_REG);
+	if (fref_detection & FREF_CLK_DETECT_FAIL)
+		return false;
+
+	return true;
+}
+
+static int wl128x_manually_configure_mcs_pll(struct wl1271 *wl)
+{
+	wl12xx_top_reg_write(wl, MCS_PLL_M_REG, MCS_PLL_M_REG_VAL);
+	wl12xx_top_reg_write(wl, MCS_PLL_N_REG, MCS_PLL_N_REG_VAL);
+	wl12xx_top_reg_write(wl, MCS_PLL_CONFIG_REG, MCS_PLL_CONFIG_REG_VAL);
+
+	return 0;
+}
+
+static int wl128x_configure_mcs_pll(struct wl1271 *wl, int clk)
+{
+	u16 spare_reg;
+	u16 pll_config;
+	u8 input_freq;
+
+	/* Mask bits [3:1] in the sys_clk_cfg register */
+	spare_reg = wl12xx_top_reg_read(wl, WL_SPARE_REG);
+	if (spare_reg == 0xFFFF)
+		return -EFAULT;
+	spare_reg |= BIT(2);
+	wl12xx_top_reg_write(wl, WL_SPARE_REG, spare_reg);
+
+	/* Handle special cases of the TCXO clock */
+	if (wl->tcxo_clock == WL12XX_TCXOCLOCK_16_8 ||
+	    wl->tcxo_clock == WL12XX_TCXOCLOCK_33_6)
+		return wl128x_manually_configure_mcs_pll(wl);
+
+	/* Set the input frequency according to the selected clock source */
+	input_freq = (clk & 1) + 1;
+
+	pll_config = wl12xx_top_reg_read(wl, MCS_PLL_CONFIG_REG);
+	if (pll_config == 0xFFFF)
+		return -EFAULT;
+	pll_config |= (input_freq << MCS_SEL_IN_FREQ_SHIFT);
+	pll_config |= MCS_PLL_ENABLE_HP;
+	wl12xx_top_reg_write(wl, MCS_PLL_CONFIG_REG, pll_config);
+
+	return 0;
+}
+
+/*
+ * WL128x has two clocks input - TCXO and FREF.
+ * TCXO is the main clock of the device, while FREF is used to sync
+ * between the GPS and the cellular modem.
+ * In cases where TCXO is 32.736MHz or 16.368MHz, the FREF will be used
+ * as the WLAN/BT main clock.
+ */
+static int wl128x_boot_clk(struct wl1271 *wl, int *selected_clock)
+{
+	u16 sys_clk_cfg;
+
+	/* For XTAL-only modes, FREF will be used after switching from TCXO */
+	if (wl->ref_clock == WL12XX_REFCLOCK_26_XTAL ||
+	    wl->ref_clock == WL12XX_REFCLOCK_38_XTAL) {
+		if (!wl128x_switch_tcxo_to_fref(wl))
+			return -EINVAL;
+		goto fref_clk;
+	}
+
+	/* Query the HW, to determine which clock source we should use */
+	sys_clk_cfg = wl12xx_top_reg_read(wl, SYS_CLK_CFG_REG);
+	if (sys_clk_cfg == 0xFFFF)
+		return -EINVAL;
+	if (sys_clk_cfg & PRCM_CM_EN_MUX_WLAN_FREF)
+		goto fref_clk;
+
+	/* If TCXO is either 32.736MHz or 16.368MHz, switch to FREF */
+	if (wl->tcxo_clock == WL12XX_TCXOCLOCK_16_368 ||
+	    wl->tcxo_clock == WL12XX_TCXOCLOCK_32_736) {
+		if (!wl128x_switch_tcxo_to_fref(wl))
+			return -EINVAL;
+		goto fref_clk;
+	}
+
+	/* TCXO clock is selected */
+	if (!wl128x_is_tcxo_valid(wl))
+		return -EINVAL;
+	*selected_clock = wl->tcxo_clock;
+	goto config_mcs_pll;
+
+fref_clk:
+	/* FREF clock is selected */
+	if (!wl128x_is_fref_valid(wl))
+		return -EINVAL;
+	*selected_clock = wl->ref_clock;
+
+config_mcs_pll:
+	return wl128x_configure_mcs_pll(wl, *selected_clock);
+}
+
+static int wl127x_boot_clk(struct wl1271 *wl)
+{
+	u32 pause;
+	u32 clk;
+
+	if (WL127X_PG_GET_MAJOR(wl->hw_pg_ver) < 3)
+		wl->quirks |= WLCORE_QUIRK_END_OF_TRANSACTION;
+
+	if (wl->ref_clock == CONF_REF_CLK_19_2_E ||
+	    wl->ref_clock == CONF_REF_CLK_38_4_E ||
+	    wl->ref_clock == CONF_REF_CLK_38_4_M_XTAL)
+		/* ref clk: 19.2/38.4/38.4-XTAL */
+		clk = 0x3;
+	else if (wl->ref_clock == CONF_REF_CLK_26_E ||
+		 wl->ref_clock == CONF_REF_CLK_52_E)
+		/* ref clk: 26/52 */
+		clk = 0x5;
+	else
+		return -EINVAL;
+
+	if (wl->ref_clock != CONF_REF_CLK_19_2_E) {
+		u16 val;
+		/* Set clock type (open drain) */
+		val = wl12xx_top_reg_read(wl, OCP_REG_CLK_TYPE);
+		val &= FREF_CLK_TYPE_BITS;
+		wl12xx_top_reg_write(wl, OCP_REG_CLK_TYPE, val);
+
+		/* Set clock pull mode (no pull) */
+		val = wl12xx_top_reg_read(wl, OCP_REG_CLK_PULL);
+		val |= NO_PULL;
+		wl12xx_top_reg_write(wl, OCP_REG_CLK_PULL, val);
+	} else {
+		u16 val;
+		/* Set clock polarity */
+		val = wl12xx_top_reg_read(wl, OCP_REG_CLK_POLARITY);
+		val &= FREF_CLK_POLARITY_BITS;
+		val |= CLK_REQ_OUTN_SEL;
+		wl12xx_top_reg_write(wl, OCP_REG_CLK_POLARITY, val);
+	}
+
+	wl1271_write32(wl, WL12XX_PLL_PARAMETERS, clk);
+
+	pause = wl1271_read32(wl, WL12XX_PLL_PARAMETERS);
+
+	wl1271_debug(DEBUG_BOOT, "pause1 0x%x", pause);
+
+	pause &= ~(WU_COUNTER_PAUSE_VAL);
+	pause |= WU_COUNTER_PAUSE_VAL;
+	wl1271_write32(wl, WL12XX_WU_COUNTER_PAUSE, pause);
+
+	return 0;
+}
+
+static int wl1271_boot_soft_reset(struct wl1271 *wl)
+{
+	unsigned long timeout;
+	u32 boot_data;
+
+	/* perform soft reset */
+	wl1271_write32(wl, WL12XX_SLV_SOFT_RESET, ACX_SLV_SOFT_RESET_BIT);
+
+	/* SOFT_RESET is self clearing */
+	timeout = jiffies + usecs_to_jiffies(SOFT_RESET_MAX_TIME);
+	while (1) {
+		boot_data = wl1271_read32(wl, WL12XX_SLV_SOFT_RESET);
+		wl1271_debug(DEBUG_BOOT, "soft reset bootdata 0x%x", boot_data);
+		if ((boot_data & ACX_SLV_SOFT_RESET_BIT) == 0)
+			break;
+
+		if (time_after(jiffies, timeout)) {
+			/* 1.2 check pWhalBus->uSelfClearTime if the
+			 * timeout was reached */
+			wl1271_error("soft reset timeout");
+			return -1;
+		}
+
+		udelay(SOFT_RESET_STALL_TIME);
+	}
+
+	/* disable Rx/Tx */
+	wl1271_write32(wl, WL12XX_ENABLE, 0x0);
+
+	/* disable auto calibration on start*/
+	wl1271_write32(wl, WL12XX_SPARE_A2, 0xffff);
+
+	return 0;
+}
+
+static int wl12xx_pre_boot(struct wl1271 *wl)
+{
+	int ret = 0;
+	u32 clk;
+	int selected_clock = -1;
+
+	if (wl->chip.id == CHIP_ID_1283_PG20) {
+		ret = wl128x_boot_clk(wl, &selected_clock);
+		if (ret < 0)
+			goto out;
+	} else {
+		ret = wl127x_boot_clk(wl);
+		if (ret < 0)
+			goto out;
+	}
+
+	/* Continue the ELP wake up sequence */
+	wl1271_write32(wl, WL12XX_WELP_ARM_COMMAND, WELP_ARM_COMMAND_VAL);
+	udelay(500);
+
+	wlcore_set_partition(wl, &wl->ptable[PART_DRPW]);
+
+	/* Read-modify-write DRPW_SCRATCH_START register (see next state)
+	   to be used by DRPw FW. The RTRIM value will be added by the FW
+	   before taking DRPw out of reset */
+
+	clk = wl1271_read32(wl, WL12XX_DRPW_SCRATCH_START);
+
+	wl1271_debug(DEBUG_BOOT, "clk2 0x%x", clk);
+
+	if (wl->chip.id == CHIP_ID_1283_PG20)
+		clk |= ((selected_clock & 0x3) << 1) << 4;
+	else
+		clk |= (wl->ref_clock << 1) << 4;
+
+	wl1271_write32(wl, WL12XX_DRPW_SCRATCH_START, clk);
+
+	wlcore_set_partition(wl, &wl->ptable[PART_WORK]);
+
+	/* Disable interrupts */
+	wlcore_write_reg(wl, REG_INTERRUPT_MASK, WL1271_ACX_INTR_ALL);
+
+	ret = wl1271_boot_soft_reset(wl);
+	if (ret < 0)
+		goto out;
+
+out:
+	return ret;
+}
+
+static void wl12xx_pre_upload(struct wl1271 *wl)
+{
+	u32 tmp;
+
+	/* write firmware's last address (ie. it's length) to
+	 * ACX_EEPROMLESS_IND_REG */
+	wl1271_debug(DEBUG_BOOT, "ACX_EEPROMLESS_IND_REG");
+
+	wl1271_write32(wl, WL12XX_EEPROMLESS_IND, WL12XX_EEPROMLESS_IND);
+
+	tmp = wlcore_read_reg(wl, REG_CHIP_ID_B);
+
+	wl1271_debug(DEBUG_BOOT, "chip id 0x%x", tmp);
+
+	/* 6. read the EEPROM parameters */
+	tmp = wl1271_read32(wl, WL12XX_SCR_PAD2);
+
+	/* WL1271: The reference driver skips steps 7 to 10 (jumps directly
+	 * to upload_fw) */
+
+	if (wl->chip.id == CHIP_ID_1283_PG20)
+		wl12xx_top_reg_write(wl, SDIO_IO_DS, HCI_IO_DS_6MA);
+}
+
+static void wl12xx_enable_interrupts(struct wl1271 *wl)
+{
+	u32 polarity;
+
+	polarity = wl12xx_top_reg_read(wl, OCP_REG_POLARITY);
+
+	/* We use HIGH polarity, so unset the LOW bit */
+	polarity &= ~POLARITY_LOW;
+	wl12xx_top_reg_write(wl, OCP_REG_POLARITY, polarity);
+
+	wlcore_write_reg(wl, REG_INTERRUPT_MASK, WL1271_ACX_ALL_EVENTS_VECTOR);
+
+	wlcore_enable_interrupts(wl);
+	wlcore_write_reg(wl, REG_INTERRUPT_MASK,
+			 WL1271_ACX_INTR_ALL & ~(WL1271_INTR_MASK));
+
+	wl1271_write32(wl, WL12XX_HI_CFG, HI_CFG_DEF_VAL);
+}
+
+static int wl12xx_boot(struct wl1271 *wl)
+{
+	int ret;
+
+	ret = wl12xx_pre_boot(wl);
+	if (ret < 0)
+		goto out;
+
+	ret = wlcore_boot_upload_nvs(wl);
+	if (ret < 0)
+		goto out;
+
+	wl12xx_pre_upload(wl);
+
+	ret = wlcore_boot_upload_firmware(wl);
+	if (ret < 0)
+		goto out;
+
+	ret = wlcore_boot_run_firmware(wl);
+	if (ret < 0)
+		goto out;
+
+	wl12xx_enable_interrupts(wl);
+
+out:
+	return ret;
+}
+
+static void wl12xx_trigger_cmd(struct wl1271 *wl, int cmd_box_addr,
+			       void *buf, size_t len)
+{
+	wl1271_write(wl, cmd_box_addr, buf, len, false);
+	wlcore_write_reg(wl, REG_INTERRUPT_TRIG, WL12XX_INTR_TRIG_CMD);
+}
+
+static void wl12xx_ack_event(struct wl1271 *wl)
+{
+	wlcore_write_reg(wl, REG_INTERRUPT_TRIG, WL12XX_INTR_TRIG_EVENT_ACK);
+}
+
+static u32 wl12xx_calc_tx_blocks(struct wl1271 *wl, u32 len, u32 spare_blks)
+{
+	u32 blk_size = WL12XX_TX_HW_BLOCK_SIZE;
+	u32 align_len = wlcore_calc_packet_alignment(wl, len);
+
+	return (align_len + blk_size - 1) / blk_size + spare_blks;
+}
+
+static void
+wl12xx_set_tx_desc_blocks(struct wl1271 *wl, struct wl1271_tx_hw_descr *desc,
+			  u32 blks, u32 spare_blks)
+{
+	if (wl->chip.id == CHIP_ID_1283_PG20) {
+		desc->wl128x_mem.total_mem_blocks = blks;
+	} else {
+		desc->wl127x_mem.extra_blocks = spare_blks;
+		desc->wl127x_mem.total_mem_blocks = blks;
+	}
+}
+
+static void
+wl12xx_set_tx_desc_data_len(struct wl1271 *wl, struct wl1271_tx_hw_descr *desc,
+			    struct sk_buff *skb)
+{
+	u32 aligned_len = wlcore_calc_packet_alignment(wl, skb->len);
+
+	if (wl->chip.id == CHIP_ID_1283_PG20) {
+		desc->wl128x_mem.extra_bytes = aligned_len - skb->len;
+		desc->length = cpu_to_le16(aligned_len >> 2);
+
+		wl1271_debug(DEBUG_TX,
+			     "tx_fill_hdr: hlid: %d len: %d life: %d mem: %d extra: %d",
+			     desc->hlid,
+			     le16_to_cpu(desc->length),
+			     le16_to_cpu(desc->life_time),
+			     desc->wl128x_mem.total_mem_blocks,
+			     desc->wl128x_mem.extra_bytes);
+	} else {
+		/* calculate number of padding bytes */
+		int pad = aligned_len - skb->len;
+		desc->tx_attr |=
+			cpu_to_le16(pad << TX_HW_ATTR_OFST_LAST_WORD_PAD);
+
+		/* Store the aligned length in terms of words */
+		desc->length = cpu_to_le16(aligned_len >> 2);
+
+		wl1271_debug(DEBUG_TX,
+			     "tx_fill_hdr: pad: %d hlid: %d len: %d life: %d mem: %d",
+			     pad, desc->hlid,
+			     le16_to_cpu(desc->length),
+			     le16_to_cpu(desc->life_time),
+			     desc->wl127x_mem.total_mem_blocks);
+	}
+}
+
+static enum wl_rx_buf_align
+wl12xx_get_rx_buf_align(struct wl1271 *wl, u32 rx_desc)
+{
+	if (rx_desc & RX_BUF_UNALIGNED_PAYLOAD)
+		return WLCORE_RX_BUF_UNALIGNED;
+
+	return WLCORE_RX_BUF_ALIGNED;
+}
+
+static u32 wl12xx_get_rx_packet_len(struct wl1271 *wl, void *rx_data,
+				    u32 data_len)
+{
+	struct wl1271_rx_descriptor *desc = rx_data;
+
+	/* invalid packet */
+	if (data_len < sizeof(*desc) ||
+	    data_len < sizeof(*desc) + desc->pad_len)
+		return 0;
+
+	return data_len - sizeof(*desc) - desc->pad_len;
+}
+
+static void wl12xx_tx_delayed_compl(struct wl1271 *wl)
+{
+	if (wl->fw_status->tx_results_counter == (wl->tx_results_count & 0xff))
+		return;
+
+	wl1271_tx_complete(wl);
+}
+
+static int wl12xx_hw_init(struct wl1271 *wl)
+{
+	int ret;
+
+	if (wl->chip.id == CHIP_ID_1283_PG20) {
+		u32 host_cfg_bitmap = HOST_IF_CFG_RX_FIFO_ENABLE;
+
+		ret = wl128x_cmd_general_parms(wl);
+		if (ret < 0)
+			goto out;
+		ret = wl128x_cmd_radio_parms(wl);
+		if (ret < 0)
+			goto out;
+
+		if (wl->quirks & WLCORE_QUIRK_TX_BLOCKSIZE_ALIGN)
+			/* Enable SDIO padding */
+			host_cfg_bitmap |= HOST_IF_CFG_TX_PAD_TO_SDIO_BLK;
+
+		/* Must be before wl1271_acx_init_mem_config() */
+		ret = wl1271_acx_host_if_cfg_bitmap(wl, host_cfg_bitmap);
+		if (ret < 0)
+			goto out;
+	} else {
+		ret = wl1271_cmd_general_parms(wl);
+		if (ret < 0)
+			goto out;
+		ret = wl1271_cmd_radio_parms(wl);
+		if (ret < 0)
+			goto out;
+		ret = wl1271_cmd_ext_radio_parms(wl);
+		if (ret < 0)
+			goto out;
+	}
+out:
+	return ret;
+}
+
+static u32 wl12xx_sta_get_ap_rate_mask(struct wl1271 *wl,
+				       struct wl12xx_vif *wlvif)
+{
+	return wlvif->rate_set;
+}
+
+static int wl12xx_identify_fw(struct wl1271 *wl)
+{
+	unsigned int *fw_ver = wl->chip.fw_ver;
+
+	/* Only new station firmwares support routing fw logs to the host */
+	if ((fw_ver[FW_VER_IF_TYPE] == FW_VER_IF_TYPE_STA) &&
+	    (fw_ver[FW_VER_MINOR] < FW_VER_MINOR_FWLOG_STA_MIN))
+		wl->quirks |= WLCORE_QUIRK_FWLOG_NOT_IMPLEMENTED;
+
+	/* This feature is not yet supported for AP mode */
+	if (fw_ver[FW_VER_IF_TYPE] == FW_VER_IF_TYPE_AP)
+		wl->quirks |= WLCORE_QUIRK_FWLOG_NOT_IMPLEMENTED;
+
+	return 0;
+}
+
+static void wl12xx_conf_init(struct wl1271 *wl)
+{
+	struct wl12xx_priv *priv = wl->priv;
+
+	/* apply driver default configuration */
+	memcpy(&wl->conf, &wl12xx_conf, sizeof(wl12xx_conf));
+
+	/* apply default private configuration */
+	memcpy(&priv->conf, &wl12xx_default_priv_conf, sizeof(priv->conf));
+}
+
+static bool wl12xx_mac_in_fuse(struct wl1271 *wl)
+{
+	bool supported = false;
+	u8 major, minor;
+
+	if (wl->chip.id == CHIP_ID_1283_PG20) {
+		major = WL128X_PG_GET_MAJOR(wl->hw_pg_ver);
+		minor = WL128X_PG_GET_MINOR(wl->hw_pg_ver);
+
+		/* in wl128x we have the MAC address if the PG is >= (2, 1) */
+		if (major > 2 || (major == 2 && minor >= 1))
+			supported = true;
+	} else {
+		major = WL127X_PG_GET_MAJOR(wl->hw_pg_ver);
+		minor = WL127X_PG_GET_MINOR(wl->hw_pg_ver);
+
+		/* in wl127x we have the MAC address if the PG is >= (3, 1) */
+		if (major == 3 && minor >= 1)
+			supported = true;
+	}
+
+	wl1271_debug(DEBUG_PROBE,
+		     "PG Ver major = %d minor = %d, MAC %s present",
+		     major, minor, supported ? "is" : "is not");
+
+	return supported;
+}
+
+static void wl12xx_get_fuse_mac(struct wl1271 *wl)
+{
+	u32 mac1, mac2;
+
+	wlcore_set_partition(wl, &wl->ptable[PART_DRPW]);
+
+	mac1 = wl1271_read32(wl, WL12XX_REG_FUSE_BD_ADDR_1);
+	mac2 = wl1271_read32(wl, WL12XX_REG_FUSE_BD_ADDR_2);
+
+	/* these are the two parts of the BD_ADDR */
+	wl->fuse_oui_addr = ((mac2 & 0xffff) << 8) +
+		((mac1 & 0xff000000) >> 24);
+	wl->fuse_nic_addr = mac1 & 0xffffff;
+
+	wlcore_set_partition(wl, &wl->ptable[PART_DOWN]);
+}
+
+static s8 wl12xx_get_pg_ver(struct wl1271 *wl)
+{
+	u32 die_info;
+
+	if (wl->chip.id == CHIP_ID_1283_PG20)
+		die_info = wl12xx_top_reg_read(wl, WL128X_REG_FUSE_DATA_2_1);
+	else
+		die_info = wl12xx_top_reg_read(wl, WL127X_REG_FUSE_DATA_2_1);
+
+	return (s8) (die_info & PG_VER_MASK) >> PG_VER_OFFSET;
+}
+
+static void wl12xx_get_mac(struct wl1271 *wl)
+{
+	if (wl12xx_mac_in_fuse(wl))
+		wl12xx_get_fuse_mac(wl);
+}
+
+static struct wlcore_ops wl12xx_ops = {
+	.identify_chip		= wl12xx_identify_chip,
+	.identify_fw		= wl12xx_identify_fw,
+	.boot			= wl12xx_boot,
+	.trigger_cmd		= wl12xx_trigger_cmd,
+	.ack_event		= wl12xx_ack_event,
+	.calc_tx_blocks		= wl12xx_calc_tx_blocks,
+	.set_tx_desc_blocks	= wl12xx_set_tx_desc_blocks,
+	.set_tx_desc_data_len	= wl12xx_set_tx_desc_data_len,
+	.get_rx_buf_align	= wl12xx_get_rx_buf_align,
+	.get_rx_packet_len	= wl12xx_get_rx_packet_len,
+	.tx_immediate_compl	= NULL,
+	.tx_delayed_compl	= wl12xx_tx_delayed_compl,
+	.hw_init		= wl12xx_hw_init,
+	.init_vif		= NULL,
+	.sta_get_ap_rate_mask	= wl12xx_sta_get_ap_rate_mask,
+	.get_pg_ver		= wl12xx_get_pg_ver,
+	.get_mac		= wl12xx_get_mac,
+};
+
+static struct ieee80211_sta_ht_cap wl12xx_ht_cap = {
+	.cap = IEEE80211_HT_CAP_GRN_FLD | IEEE80211_HT_CAP_SGI_20 |
+	       (1 << IEEE80211_HT_CAP_RX_STBC_SHIFT),
+	.ht_supported = true,
+	.ampdu_factor = IEEE80211_HT_MAX_AMPDU_8K,
+	.ampdu_density = IEEE80211_HT_MPDU_DENSITY_8,
+	.mcs = {
+		.rx_mask = { 0xff, 0, 0, 0, 0, 0, 0, 0, 0, 0, },
+		.rx_highest = cpu_to_le16(72),
+		.tx_params = IEEE80211_HT_MCS_TX_DEFINED,
+		},
+};
+
+static int __devinit wl12xx_probe(struct platform_device *pdev)
+{
+	struct wl1271 *wl;
+	struct ieee80211_hw *hw;
+	struct wl12xx_priv *priv;
+
+	hw = wlcore_alloc_hw(sizeof(*priv));
+	if (IS_ERR(hw)) {
+		wl1271_error("can't allocate hw");
+		return PTR_ERR(hw);
+	}
+
+	wl = hw->priv;
+	wl->ops = &wl12xx_ops;
+	wl->ptable = wl12xx_ptable;
+	wl->rtable = wl12xx_rtable;
+	wl->num_tx_desc = 16;
+	wl->normal_tx_spare = WL12XX_TX_HW_BLOCK_SPARE_DEFAULT;
+	wl->gem_tx_spare = WL12XX_TX_HW_BLOCK_GEM_SPARE;
+	wl->band_rate_to_idx = wl12xx_band_rate_to_idx;
+	wl->hw_tx_rate_tbl_size = WL12XX_CONF_HW_RXTX_RATE_MAX;
+	wl->hw_min_ht_rate = WL12XX_CONF_HW_RXTX_RATE_MCS0;
+	wl->fw_status_priv_len = 0;
+	memcpy(&wl->ht_cap, &wl12xx_ht_cap, sizeof(wl12xx_ht_cap));
+	wl12xx_conf_init(wl);
+
+	return wlcore_probe(wl, pdev);
+}
+
+static const struct platform_device_id wl12xx_id_table[] __devinitconst = {
+	{ "wl12xx", 0 },
+	{  } /* Terminating Entry */
+};
+MODULE_DEVICE_TABLE(platform, wl12xx_id_table);
+
+static struct platform_driver wl12xx_driver = {
+	.probe		= wl12xx_probe,
+	.remove		= __devexit_p(wlcore_remove),
+	.id_table	= wl12xx_id_table,
+	.driver = {
+		.name	= "wl12xx_driver",
+		.owner	= THIS_MODULE,
+	}
+};
+
+static int __init wl12xx_init(void)
+{
+	return platform_driver_register(&wl12xx_driver);
+}
+module_init(wl12xx_init);
+
+static void __exit wl12xx_exit(void)
+{
+	platform_driver_unregister(&wl12xx_driver);
+}
+module_exit(wl12xx_exit);
+
+MODULE_LICENSE("GPL v2");
+MODULE_AUTHOR("Luciano Coelho <coelho@ti.com>");
+MODULE_FIRMWARE(WL127X_FW_NAME_SINGLE);
+MODULE_FIRMWARE(WL127X_FW_NAME_MULTI);
+MODULE_FIRMWARE(WL127X_PLT_FW_NAME);
+MODULE_FIRMWARE(WL128X_FW_NAME_SINGLE);
+MODULE_FIRMWARE(WL128X_FW_NAME_MULTI);
+MODULE_FIRMWARE(WL128X_PLT_FW_NAME);
diff --git a/drivers/net/wireless/wl12xx/reg.h b/drivers/net/wireless/ti/wl12xx/reg.h
index 340db324bc2..79ede02e258 100644
--- a/drivers/net/wireless/wl12xx/reg.h
+++ b/drivers/net/wireless/ti/wl12xx/reg.h
@@ -33,16 +33,8 @@
 #define REGISTERS_DOWN_SIZE 0x00008800
 #define REGISTERS_WORK_SIZE 0x0000b000
 
-#define HW_ACCESS_ELP_CTRL_REG_ADDR         0x1FFFC
 #define FW_STATUS_ADDR                      (0x14FC0 + 0xA000)
 
-/* ELP register commands */
-#define ELPCTRL_WAKE_UP             0x1
-#define ELPCTRL_WAKE_UP_WLAN_READY  0x5
-#define ELPCTRL_SLEEP               0x0
-/* ELP WLAN_READY bit */
-#define ELPCTRL_WLAN_READY          0x2
-
 /*===============================================
    Host Software Reset - 32bit RW
  ------------------------------------------
@@ -57,14 +49,14 @@
     (not self-clearing), the Wlan hardware
     exits the software reset state.
 ===============================================*/
-#define ACX_REG_SLV_SOFT_RESET         (REGISTERS_BASE + 0x0000)
+#define WL12XX_SLV_SOFT_RESET		(REGISTERS_BASE + 0x0000)
 
 #define WL1271_SLV_REG_DATA            (REGISTERS_BASE + 0x0008)
 #define WL1271_SLV_REG_ADATA           (REGISTERS_BASE + 0x000c)
 #define WL1271_SLV_MEM_DATA            (REGISTERS_BASE + 0x0018)
 
-#define ACX_REG_INTERRUPT_TRIG         (REGISTERS_BASE + 0x0474)
-#define ACX_REG_INTERRUPT_TRIG_H       (REGISTERS_BASE + 0x0478)
+#define WL12XX_REG_INTERRUPT_TRIG         (REGISTERS_BASE + 0x0474)
+#define WL12XX_REG_INTERRUPT_TRIG_H       (REGISTERS_BASE + 0x0478)
 
 /*=============================================
   Host Interrupt Mask Register - 32bit (RW)
@@ -94,7 +86,7 @@
  21-			-
  Default: 0x0001
 *==============================================*/
-#define ACX_REG_INTERRUPT_MASK         (REGISTERS_BASE + 0x04DC)
+#define WL12XX_REG_INTERRUPT_MASK         (REGISTERS_BASE + 0x04DC)
 
 /*=============================================
   Host Interrupt Mask Set 16bit, (Write only)
@@ -125,7 +117,7 @@
  Reading this register doesn't
  effect its content.
 =============================================*/
-#define ACX_REG_INTERRUPT_NO_CLEAR     (REGISTERS_BASE + 0x04E8)
+#define WL12XX_REG_INTERRUPT_NO_CLEAR     (REGISTERS_BASE + 0x04E8)
 
 /*=============================================
   Host Interrupt Status Clear on Read  Register
@@ -148,9 +140,9 @@
  HINT_STS_ND registers, thus making the
  assotiated interrupt inactive. (0-no effect)
 ==============================================*/
-#define ACX_REG_INTERRUPT_ACK          (REGISTERS_BASE + 0x04F0)
+#define WL12XX_REG_INTERRUPT_ACK          (REGISTERS_BASE + 0x04F0)
 
-#define RX_DRIVER_COUNTER_ADDRESS      (REGISTERS_BASE + 0x0538)
+#define WL12XX_REG_RX_DRIVER_COUNTER	(REGISTERS_BASE + 0x0538)
 
 /* Device Configuration registers*/
 #define SOR_CFG                        (REGISTERS_BASE + 0x0800)
@@ -175,9 +167,9 @@
  1 halt eCPU
  0 enable eCPU
  ===============================================*/
-#define ACX_REG_ECPU_CONTROL           (REGISTERS_BASE + 0x0804)
+#define WL12XX_REG_ECPU_CONTROL           (REGISTERS_BASE + 0x0804)
 
-#define HI_CFG                         (REGISTERS_BASE + 0x0808)
+#define WL12XX_HI_CFG			(REGISTERS_BASE + 0x0808)
 
 /*===============================================
  EEPROM Burst Read Start  - 32bit RW
@@ -196,72 +188,67 @@
 *================================================*/
 #define ACX_REG_EE_START               (REGISTERS_BASE + 0x080C)
 
-#define OCP_POR_CTR                    (REGISTERS_BASE + 0x09B4)
-#define OCP_DATA_WRITE                 (REGISTERS_BASE + 0x09B8)
-#define OCP_DATA_READ                  (REGISTERS_BASE + 0x09BC)
-#define OCP_CMD                        (REGISTERS_BASE + 0x09C0)
-
-#define WL1271_HOST_WR_ACCESS          (REGISTERS_BASE + 0x09F8)
+#define WL12XX_OCP_POR_CTR		(REGISTERS_BASE + 0x09B4)
+#define WL12XX_OCP_DATA_WRITE		(REGISTERS_BASE + 0x09B8)
+#define WL12XX_OCP_DATA_READ		(REGISTERS_BASE + 0x09BC)
+#define WL12XX_OCP_CMD			(REGISTERS_BASE + 0x09C0)
 
-#define CHIP_ID_B                      (REGISTERS_BASE + 0x5674)
+#define WL12XX_HOST_WR_ACCESS		(REGISTERS_BASE + 0x09F8)
 
-#define CHIP_ID_1271_PG10              (0x4030101)
-#define CHIP_ID_1271_PG20              (0x4030111)
-#define CHIP_ID_1283_PG10              (0x05030101)
-#define CHIP_ID_1283_PG20              (0x05030111)
+#define WL12XX_CHIP_ID_B		(REGISTERS_BASE + 0x5674)
 
-#define ENABLE                         (REGISTERS_BASE + 0x5450)
+#define WL12XX_ENABLE			(REGISTERS_BASE + 0x5450)
 
 /* Power Management registers */
-#define ELP_CFG_MODE                   (REGISTERS_BASE + 0x5804)
-#define ELP_CMD                        (REGISTERS_BASE + 0x5808)
-#define PLL_CAL_TIME                   (REGISTERS_BASE + 0x5810)
-#define CLK_REQ_TIME                   (REGISTERS_BASE + 0x5814)
-#define CLK_BUF_TIME                   (REGISTERS_BASE + 0x5818)
+#define WL12XX_ELP_CFG_MODE		(REGISTERS_BASE + 0x5804)
+#define WL12XX_ELP_CMD			(REGISTERS_BASE + 0x5808)
+#define WL12XX_PLL_CAL_TIME		(REGISTERS_BASE + 0x5810)
+#define WL12XX_CLK_REQ_TIME		(REGISTERS_BASE + 0x5814)
+#define WL12XX_CLK_BUF_TIME		(REGISTERS_BASE + 0x5818)
 
-#define CFG_PLL_SYNC_CNT               (REGISTERS_BASE + 0x5820)
+#define WL12XX_CFG_PLL_SYNC_CNT		(REGISTERS_BASE + 0x5820)
 
 /* Scratch Pad registers*/
-#define SCR_PAD0                       (REGISTERS_BASE + 0x5608)
-#define SCR_PAD1                       (REGISTERS_BASE + 0x560C)
-#define SCR_PAD2                       (REGISTERS_BASE + 0x5610)
-#define SCR_PAD3                       (REGISTERS_BASE + 0x5614)
-#define SCR_PAD4                       (REGISTERS_BASE + 0x5618)
-#define SCR_PAD4_SET                   (REGISTERS_BASE + 0x561C)
-#define SCR_PAD4_CLR                   (REGISTERS_BASE + 0x5620)
-#define SCR_PAD5                       (REGISTERS_BASE + 0x5624)
-#define SCR_PAD5_SET                   (REGISTERS_BASE + 0x5628)
-#define SCR_PAD5_CLR                   (REGISTERS_BASE + 0x562C)
-#define SCR_PAD6                       (REGISTERS_BASE + 0x5630)
-#define SCR_PAD7                       (REGISTERS_BASE + 0x5634)
-#define SCR_PAD8                       (REGISTERS_BASE + 0x5638)
-#define SCR_PAD9                       (REGISTERS_BASE + 0x563C)
+#define WL12XX_SCR_PAD0			(REGISTERS_BASE + 0x5608)
+#define WL12XX_SCR_PAD1			(REGISTERS_BASE + 0x560C)
+#define WL12XX_SCR_PAD2			(REGISTERS_BASE + 0x5610)
+#define WL12XX_SCR_PAD3			(REGISTERS_BASE + 0x5614)
+#define WL12XX_SCR_PAD4			(REGISTERS_BASE + 0x5618)
+#define WL12XX_SCR_PAD4_SET		(REGISTERS_BASE + 0x561C)
+#define WL12XX_SCR_PAD4_CLR		(REGISTERS_BASE + 0x5620)
+#define WL12XX_SCR_PAD5			(REGISTERS_BASE + 0x5624)
+#define WL12XX_SCR_PAD5_SET		(REGISTERS_BASE + 0x5628)
+#define WL12XX_SCR_PAD5_CLR		(REGISTERS_BASE + 0x562C)
+#define WL12XX_SCR_PAD6			(REGISTERS_BASE + 0x5630)
+#define WL12XX_SCR_PAD7			(REGISTERS_BASE + 0x5634)
+#define WL12XX_SCR_PAD8			(REGISTERS_BASE + 0x5638)
+#define WL12XX_SCR_PAD9			(REGISTERS_BASE + 0x563C)
 
 /* Spare registers*/
-#define SPARE_A1                       (REGISTERS_BASE + 0x0994)
-#define SPARE_A2                       (REGISTERS_BASE + 0x0998)
-#define SPARE_A3                       (REGISTERS_BASE + 0x099C)
-#define SPARE_A4                       (REGISTERS_BASE + 0x09A0)
-#define SPARE_A5                       (REGISTERS_BASE + 0x09A4)
-#define SPARE_A6                       (REGISTERS_BASE + 0x09A8)
-#define SPARE_A7                       (REGISTERS_BASE + 0x09AC)
-#define SPARE_A8                       (REGISTERS_BASE + 0x09B0)
-#define SPARE_B1                       (REGISTERS_BASE + 0x5420)
-#define SPARE_B2                       (REGISTERS_BASE + 0x5424)
-#define SPARE_B3                       (REGISTERS_BASE + 0x5428)
-#define SPARE_B4                       (REGISTERS_BASE + 0x542C)
-#define SPARE_B5                       (REGISTERS_BASE + 0x5430)
-#define SPARE_B6                       (REGISTERS_BASE + 0x5434)
-#define SPARE_B7                       (REGISTERS_BASE + 0x5438)
-#define SPARE_B8                       (REGISTERS_BASE + 0x543C)
-
-#define PLL_PARAMETERS                 (REGISTERS_BASE + 0x6040)
-#define WU_COUNTER_PAUSE               (REGISTERS_BASE + 0x6008)
-#define WELP_ARM_COMMAND               (REGISTERS_BASE + 0x6100)
-#define DRPW_SCRATCH_START             (DRPW_BASE + 0x002C)
-
-
-#define ACX_SLV_SOFT_RESET_BIT   BIT(1)
+#define WL12XX_SPARE_A1			(REGISTERS_BASE + 0x0994)
+#define WL12XX_SPARE_A2			(REGISTERS_BASE + 0x0998)
+#define WL12XX_SPARE_A3			(REGISTERS_BASE + 0x099C)
+#define WL12XX_SPARE_A4			(REGISTERS_BASE + 0x09A0)
+#define WL12XX_SPARE_A5			(REGISTERS_BASE + 0x09A4)
+#define WL12XX_SPARE_A6			(REGISTERS_BASE + 0x09A8)
+#define WL12XX_SPARE_A7			(REGISTERS_BASE + 0x09AC)
+#define WL12XX_SPARE_A8			(REGISTERS_BASE + 0x09B0)
+#define WL12XX_SPARE_B1			(REGISTERS_BASE + 0x5420)
+#define WL12XX_SPARE_B2			(REGISTERS_BASE + 0x5424)
+#define WL12XX_SPARE_B3			(REGISTERS_BASE + 0x5428)
+#define WL12XX_SPARE_B4			(REGISTERS_BASE + 0x542C)
+#define WL12XX_SPARE_B5			(REGISTERS_BASE + 0x5430)
+#define WL12XX_SPARE_B6			(REGISTERS_BASE + 0x5434)
+#define WL12XX_SPARE_B7			(REGISTERS_BASE + 0x5438)
+#define WL12XX_SPARE_B8			(REGISTERS_BASE + 0x543C)
+
+#define WL12XX_PLL_PARAMETERS		(REGISTERS_BASE + 0x6040)
+#define WL12XX_WU_COUNTER_PAUSE		(REGISTERS_BASE + 0x6008)
+#define WL12XX_WELP_ARM_COMMAND		(REGISTERS_BASE + 0x6100)
+#define WL12XX_DRPW_SCRATCH_START	(DRPW_BASE + 0x002C)
+
+#define WL12XX_CMD_MBOX_ADDRESS		0x407B4
+
 #define ACX_REG_EEPROM_START_BIT BIT(1)
 
 /* Command/Information Mailbox Pointers */
@@ -279,7 +266,7 @@
  the host receives the Init Complete interrupt from
  the Wlan hardware.
  ===============================================*/
-#define REG_COMMAND_MAILBOX_PTR				(SCR_PAD0)
+#define WL12XX_REG_COMMAND_MAILBOX_PTR		(WL12XX_SCR_PAD0)
 
 /*===============================================
   Information Mailbox Pointer - 32bit RW
@@ -294,7 +281,7 @@
  until after the host receives the Init Complete interrupt from
  the Wlan hardware.
  ===============================================*/
-#define REG_EVENT_MAILBOX_PTR				(SCR_PAD1)
+#define WL12XX_REG_EVENT_MAILBOX_PTR		(WL12XX_SCR_PAD1)
 
 /*===============================================
  EEPROM Read/Write Request 32bit RW
@@ -365,26 +352,6 @@
 #define ACX_CONT_WIND_MIN_MASK   0x0000007f
 #define ACX_CONT_WIND_MAX        0x03ff0000
 
-/*===============================================
-  HI_CFG Interface Configuration Register Values
-  ------------------------------------------
-  ===============================================*/
-#define HI_CFG_UART_ENABLE          0x00000004
-#define HI_CFG_RST232_ENABLE        0x00000008
-#define HI_CFG_CLOCK_REQ_SELECT     0x00000010
-#define HI_CFG_HOST_INT_ENABLE      0x00000020
-#define HI_CFG_VLYNQ_OUTPUT_ENABLE  0x00000040
-#define HI_CFG_HOST_INT_ACTIVE_LOW  0x00000080
-#define HI_CFG_UART_TX_OUT_GPIO_15  0x00000100
-#define HI_CFG_UART_TX_OUT_GPIO_14  0x00000200
-#define HI_CFG_UART_TX_OUT_GPIO_7   0x00000400
-
-#define HI_CFG_DEF_VAL              \
-	(HI_CFG_UART_ENABLE |        \
-	HI_CFG_RST232_ENABLE |      \
-	HI_CFG_CLOCK_REQ_SELECT |   \
-	HI_CFG_HOST_INT_ENABLE)
-
 #define REF_FREQ_19_2                       0
 #define REF_FREQ_26_0                       1
 #define REF_FREQ_38_4                       2
@@ -400,38 +367,19 @@
 #define LUT_PARAM_BB_PLL_LOOP_FILTER        5
 #define LUT_PARAM_NUM                       6
 
-#define ACX_EEPROMLESS_IND_REG              (SCR_PAD4)
+#define WL12XX_EEPROMLESS_IND		(WL12XX_SCR_PAD4)
 #define USE_EEPROM                          0
-#define SOFT_RESET_MAX_TIME                 1000000
-#define SOFT_RESET_STALL_TIME               1000
 #define NVS_DATA_BUNDARY_ALIGNMENT          4
 
-
-/* Firmware image load chunk size */
-#define CHUNK_SIZE	16384
-
 /* Firmware image header size */
 #define FW_HDR_SIZE 8
 
-#define ECPU_CONTROL_HALT					0x00000101
-
-
 /******************************************************************************
 
     CHANNELS, BAND & REG DOMAINS definitions
 
 ******************************************************************************/
 
-
-enum {
-	RADIO_BAND_2_4GHZ = 0,  /* 2.4 Ghz band */
-	RADIO_BAND_5GHZ = 1,    /* 5 Ghz band */
-	RADIO_BAND_JAPAN_4_9_GHZ = 2,
-	DEFAULT_BAND = RADIO_BAND_2_4GHZ,
-	INVALID_BAND = 0xFE,
-	MAX_RADIO_BANDS = 0xFF
-};
-
 #define SHORT_PREAMBLE_BIT   BIT(0) /* CCK or Barker depending on the rate */
 #define OFDM_RATE_BIT        BIT(6)
 #define PBCC_RATE_BIT        BIT(7)
@@ -465,14 +413,82 @@ b12-b0 - Supported Rate indicator bits as defined below.
 
 ******************************************************************************/
 
+#define OCP_CMD_LOOP		32
+#define OCP_CMD_WRITE		0x1
+#define OCP_CMD_READ		0x2
+#define OCP_READY_MASK		BIT(18)
+#define OCP_STATUS_MASK		(BIT(16) | BIT(17))
+#define OCP_STATUS_NO_RESP	0x00000
+#define OCP_STATUS_OK		0x10000
+#define OCP_STATUS_REQ_FAILED	0x20000
+#define OCP_STATUS_RESP_ERROR	0x30000
+
+#define OCP_REG_POLARITY     0x0064
+#define OCP_REG_CLK_TYPE     0x0448
+#define OCP_REG_CLK_POLARITY 0x0cb2
+#define OCP_REG_CLK_PULL     0x0cb4
+
+#define POLARITY_LOW         BIT(1)
+#define NO_PULL              (BIT(14) | BIT(15))
+
+#define FREF_CLK_TYPE_BITS     0xfffffe7f
+#define CLK_REQ_PRCM           0x100
+#define FREF_CLK_POLARITY_BITS 0xfffff8ff
+#define CLK_REQ_OUTN_SEL       0x700
+
+#define WU_COUNTER_PAUSE_VAL 0x3FF
+
+/* PLL configuration algorithm for wl128x */
+#define SYS_CLK_CFG_REG              0x2200
+/* Bit[0]   -  0-TCXO,  1-FREF */
+#define MCS_PLL_CLK_SEL_FREF         BIT(0)
+/* Bit[3:2] - 01-TCXO, 10-FREF */
+#define WL_CLK_REQ_TYPE_FREF         BIT(3)
+#define WL_CLK_REQ_TYPE_PG2          (BIT(3) | BIT(2))
+/* Bit[4]   -  0-TCXO,  1-FREF */
+#define PRCM_CM_EN_MUX_WLAN_FREF     BIT(4)
+
+#define TCXO_ILOAD_INT_REG           0x2264
+#define TCXO_CLK_DETECT_REG          0x2266
+
+#define TCXO_DET_FAILED              BIT(4)
+
+#define FREF_ILOAD_INT_REG           0x2084
+#define FREF_CLK_DETECT_REG          0x2086
+#define FREF_CLK_DETECT_FAIL         BIT(4)
+
+/* Use this reg for masking during driver access */
+#define WL_SPARE_REG                 0x2320
+#define WL_SPARE_VAL                 BIT(2)
+/* Bit[6:5:3] -  mask wl write SYS_CLK_CFG[8:5:2:4] */
+#define WL_SPARE_MASK_8526           (BIT(6) | BIT(5) | BIT(3))
+
+#define PLL_LOCK_COUNTERS_REG        0xD8C
+#define PLL_LOCK_COUNTERS_COEX       0x0F
+#define PLL_LOCK_COUNTERS_MCS        0xF0
+#define MCS_PLL_OVERRIDE_REG         0xD90
+#define MCS_PLL_CONFIG_REG           0xD92
+#define MCS_SEL_IN_FREQ_MASK         0x0070
+#define MCS_SEL_IN_FREQ_SHIFT        4
+#define MCS_PLL_CONFIG_REG_VAL       0x73
+#define MCS_PLL_ENABLE_HP            (BIT(0) | BIT(1))
+
+#define MCS_PLL_M_REG                0xD94
+#define MCS_PLL_N_REG                0xD96
+#define MCS_PLL_M_REG_VAL            0xC8
+#define MCS_PLL_N_REG_VAL            0x07
+
+#define SDIO_IO_DS                   0xd14
+
+/* SDIO/wSPI DS configuration values */
+enum {
+	HCI_IO_DS_8MA = 0,
+	HCI_IO_DS_4MA = 1, /* default */
+	HCI_IO_DS_6MA = 2,
+	HCI_IO_DS_2MA = 3,
+};
 
-/*************************************************************************
-
-    Interrupt Trigger Register (Host -> WiLink)
-
-**************************************************************************/
-
-/* Hardware to Embedded CPU Interrupts - first 32-bit register set */
+/* end PLL configuration algorithm for wl128x */
 
 /*
  * Host Command Interrupt. Setting this bit masks
@@ -480,7 +496,7 @@ b12-b0 - Supported Rate indicator bits as defined below.
  * the FW that it has sent a command
  * to the Wlan hardware Command Mailbox.
  */
-#define INTR_TRIG_CMD       BIT(0)
+#define WL12XX_INTR_TRIG_CMD		BIT(0)
 
 /*
  * Host Event Acknowlegde Interrupt. The host
@@ -488,42 +504,27 @@ b12-b0 - Supported Rate indicator bits as defined below.
  * the unsolicited information from the event
  * mailbox.
  */
-#define INTR_TRIG_EVENT_ACK BIT(1)
-
-/*
- * The host sets this bit to inform the Wlan
- * FW that a TX packet is in the XFER
- * Buffer #0.
- */
-#define INTR_TRIG_TX_PROC0 BIT(2)
-
-/*
- * The host sets this bit to inform the FW
- * that it read a packet from RX XFER
- * Buffer #0.
- */
-#define INTR_TRIG_RX_PROC0 BIT(3)
-
-#define INTR_TRIG_DEBUG_ACK BIT(4)
+#define WL12XX_INTR_TRIG_EVENT_ACK	BIT(1)
 
-#define INTR_TRIG_STATE_CHANGED BIT(5)
-
-
-/* Hardware to Embedded CPU Interrupts - second 32-bit register set */
-
-/*
- * The host sets this bit to inform the FW
- * that it read a packet from RX XFER
- * Buffer #1.
- */
-#define INTR_TRIG_RX_PROC1 BIT(17)
+/*===============================================
+  HI_CFG Interface Configuration Register Values
+  ------------------------------------------
+  ===============================================*/
+#define HI_CFG_UART_ENABLE          0x00000004
+#define HI_CFG_RST232_ENABLE        0x00000008
+#define HI_CFG_CLOCK_REQ_SELECT     0x00000010
+#define HI_CFG_HOST_INT_ENABLE      0x00000020
+#define HI_CFG_VLYNQ_OUTPUT_ENABLE  0x00000040
+#define HI_CFG_HOST_INT_ACTIVE_LOW  0x00000080
+#define HI_CFG_UART_TX_OUT_GPIO_15  0x00000100
+#define HI_CFG_UART_TX_OUT_GPIO_14  0x00000200
+#define HI_CFG_UART_TX_OUT_GPIO_7   0x00000400
 
-/*
- * The host sets this bit to inform the Wlan
- * hardware that a TX packet is in the XFER
- * Buffer #1.
- */
-#define INTR_TRIG_TX_PROC1 BIT(18)
+#define HI_CFG_DEF_VAL              \
+	(HI_CFG_UART_ENABLE |        \
+	HI_CFG_RST232_ENABLE |      \
+	HI_CFG_CLOCK_REQ_SELECT |   \
+	HI_CFG_HOST_INT_ENABLE)
 
 #define WL127X_REG_FUSE_DATA_2_1	0x050a
 #define WL128X_REG_FUSE_DATA_2_1	0x2152
diff --git a/drivers/net/wireless/ti/wl12xx/wl12xx.h b/drivers/net/wireless/ti/wl12xx/wl12xx.h
new file mode 100644
index 00000000000..74cd332e23e
--- /dev/null
+++ b/drivers/net/wireless/ti/wl12xx/wl12xx.h
@@ -0,0 +1,31 @@
+/*
+ * This file is part of wl12xx
+ *
+ * Copyright (C) 2011 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.
+ *
+ * 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
+ *
+ */
+
+#ifndef __WL12XX_PRIV_H__
+#define __WL12XX_PRIV_H__
+
+#include "conf.h"
+
+struct wl12xx_priv {
+	struct wl12xx_priv_conf conf;
+};
+
+#endif /* __WL12XX_PRIV_H__ */
diff --git a/drivers/net/wireless/ti/wlcore/Kconfig b/drivers/net/wireless/ti/wlcore/Kconfig
new file mode 100644
index 00000000000..54156b0b5c2
--- /dev/null
+++ b/drivers/net/wireless/ti/wlcore/Kconfig
@@ -0,0 +1,41 @@
+config WLCORE
+	tristate "TI wlcore support"
+	depends on WL_TI && GENERIC_HARDIRQS && MAC80211
+	depends on INET
+	select FW_LOADER
+	---help---
+	  This module contains the main code for TI WLAN chips.  It abstracts
+	  hardware-specific differences among different chipset families.
+	  Each chipset family needs to implement its own lower-level module
+	  that will depend on this module for the common code.
+
+	  If you choose to build a module, it will be called wlcore. Say N if
+	  unsure.
+
+config WLCORE_SPI
+	tristate "TI wlcore SPI support"
+	depends on WLCORE && SPI_MASTER
+	select CRC7
+	---help---
+	  This module adds support for the SPI interface of adapters using
+	  TI WLAN chipsets.  Select this if your platform is using
+	  the SPI bus.
+
+	  If you choose to build a module, it'll be called wlcore_spi.
+	  Say N if unsure.
+
+config WLCORE_SDIO
+	tristate "TI wlcore SDIO support"
+	depends on WLCORE && MMC
+	---help---
+	  This module adds support for the SDIO interface of adapters using
+	  TI WLAN chipsets.  Select this if your platform is using
+	  the SDIO bus.
+
+	  If you choose to build a module, it'll be called wlcore_sdio.
+	  Say N if unsure.
+
+config WL12XX_PLATFORM_DATA
+	bool
+	depends on WLCORE_SDIO != n || WL1251_SDIO != n
+	default y
diff --git a/drivers/net/wireless/ti/wlcore/Makefile b/drivers/net/wireless/ti/wlcore/Makefile
new file mode 100644
index 00000000000..d9fba9e3213
--- /dev/null
+++ b/drivers/net/wireless/ti/wlcore/Makefile
@@ -0,0 +1,15 @@
+wlcore-objs		= main.o cmd.o io.o event.o tx.o rx.o ps.o acx.o \
+			  boot.o init.o debugfs.o scan.o
+
+wlcore_spi-objs 	= spi.o
+wlcore_sdio-objs	= sdio.o
+
+wlcore-$(CONFIG_NL80211_TESTMODE)	+= testmode.o
+obj-$(CONFIG_WLCORE)			+= wlcore.o
+obj-$(CONFIG_WLCORE_SPI)		+= wlcore_spi.o
+obj-$(CONFIG_WLCORE_SDIO)		+= wlcore_sdio.o
+
+# small builtin driver bit
+obj-$(CONFIG_WL12XX_PLATFORM_DATA)	+= wl12xx_platform_data.o
+
+ccflags-y += -D__CHECK_ENDIAN__
diff --git a/drivers/net/wireless/wl12xx/acx.c b/drivers/net/wireless/ti/wlcore/acx.c
index bc96db0683a..509aa881d79 100644
--- a/drivers/net/wireless/wl12xx/acx.c
+++ b/drivers/net/wireless/ti/wlcore/acx.c
@@ -28,11 +28,11 @@
 #include <linux/spi/spi.h>
 #include <linux/slab.h>
 
-#include "wl12xx.h"
+#include "wlcore.h"
 #include "debug.h"
 #include "wl12xx_80211.h"
-#include "reg.h"
 #include "ps.h"
+#include "hw_ops.h"
 
 int wl1271_acx_wake_up_conditions(struct wl1271 *wl, struct wl12xx_vif *wlvif,
 				  u8 wake_up_event, u8 listen_interval)
@@ -757,7 +757,10 @@ int wl1271_acx_sta_rate_policies(struct wl1271 *wl, struct wl12xx_vif *wlvif)
 
 	/* configure one AP supported rate class */
 	acx->rate_policy_idx = cpu_to_le32(wlvif->sta.ap_rate_idx);
-	acx->rate_policy.enabled_rates = cpu_to_le32(wlvif->rate_set);
+
+	/* the AP policy is HW specific */
+	acx->rate_policy.enabled_rates =
+		cpu_to_le32(wlcore_hw_sta_get_ap_rate_mask(wl, wlvif));
 	acx->rate_policy.short_retry_limit = c->short_retry_limit;
 	acx->rate_policy.long_retry_limit = c->long_retry_limit;
 	acx->rate_policy.aflags = c->aflags;
@@ -969,17 +972,14 @@ int wl12xx_acx_mem_cfg(struct wl1271 *wl)
 		goto out;
 	}
 
-	if (wl->chip.id == CHIP_ID_1283_PG20)
-		mem = &wl->conf.mem_wl128x;
-	else
-		mem = &wl->conf.mem_wl127x;
+	mem = &wl->conf.mem;
 
 	/* memory config */
 	mem_conf->num_stations = mem->num_stations;
 	mem_conf->rx_mem_block_num = mem->rx_block_num;
 	mem_conf->tx_min_mem_block_num = mem->tx_min_block_num;
 	mem_conf->num_ssid_profiles = mem->ssid_profiles;
-	mem_conf->total_tx_descriptors = cpu_to_le32(ACX_TX_DESCRIPTORS);
+	mem_conf->total_tx_descriptors = cpu_to_le32(wl->num_tx_desc);
 	mem_conf->dyn_mem_enable = mem->dynamic_memory;
 	mem_conf->tx_free_req = mem->min_req_tx_blocks;
 	mem_conf->rx_free_req = mem->min_req_rx_blocks;
@@ -998,32 +998,6 @@ out:
 	return ret;
 }
 
-int wl1271_acx_host_if_cfg_bitmap(struct wl1271 *wl, u32 host_cfg_bitmap)
-{
-	struct wl1271_acx_host_config_bitmap *bitmap_conf;
-	int ret;
-
-	bitmap_conf = kzalloc(sizeof(*bitmap_conf), GFP_KERNEL);
-	if (!bitmap_conf) {
-		ret = -ENOMEM;
-		goto out;
-	}
-
-	bitmap_conf->host_cfg_bitmap = cpu_to_le32(host_cfg_bitmap);
-
-	ret = wl1271_cmd_configure(wl, ACX_HOST_IF_CFG_BITMAP,
-				   bitmap_conf, sizeof(*bitmap_conf));
-	if (ret < 0) {
-		wl1271_warning("wl1271 bitmap config opt failed: %d", ret);
-		goto out;
-	}
-
-out:
-	kfree(bitmap_conf);
-
-	return ret;
-}
-
 int wl1271_acx_init_mem_config(struct wl1271 *wl)
 {
 	int ret;
@@ -1740,3 +1714,83 @@ out:
 	return ret;
 
 }
+
+/* Set the global behaviour of RX filters - On/Off + default action */
+int wl1271_acx_default_rx_filter_enable(struct wl1271 *wl, bool enable,
+					enum rx_filter_action action)
+{
+	struct acx_default_rx_filter *acx;
+	int ret;
+
+	wl1271_debug(DEBUG_ACX, "acx default rx filter en: %d act: %d",
+		     enable, action);
+
+	acx = kzalloc(sizeof(*acx), GFP_KERNEL);
+	if (!acx)
+		return -ENOMEM;
+
+	acx->enable = enable;
+	acx->default_action = action;
+
+	ret = wl1271_cmd_configure(wl, ACX_ENABLE_RX_DATA_FILTER, acx,
+				   sizeof(*acx));
+	if (ret < 0) {
+		wl1271_warning("acx default rx filter enable failed: %d", ret);
+		goto out;
+	}
+
+out:
+	kfree(acx);
+	return ret;
+}
+
+/* Configure or disable a specific RX filter pattern */
+int wl1271_acx_set_rx_filter(struct wl1271 *wl, u8 index, bool enable,
+			     struct wl12xx_rx_filter *filter)
+{
+	struct acx_rx_filter_cfg *acx;
+	int fields_size = 0;
+	int acx_size;
+	int ret;
+
+	WARN_ON(enable && !filter);
+	WARN_ON(index >= WL1271_MAX_RX_FILTERS);
+
+	wl1271_debug(DEBUG_ACX,
+		     "acx set rx filter idx: %d enable: %d filter: %p",
+		     index, enable, filter);
+
+	if (enable) {
+		fields_size = wl1271_rx_filter_get_fields_size(filter);
+
+		wl1271_debug(DEBUG_ACX, "act: %d num_fields: %d field_size: %d",
+		      filter->action, filter->num_fields, fields_size);
+	}
+
+	acx_size = ALIGN(sizeof(*acx) + fields_size, 4);
+	acx = kzalloc(acx_size, GFP_KERNEL);
+
+	if (!acx)
+		return -ENOMEM;
+
+	acx->enable = enable;
+	acx->index = index;
+
+	if (enable) {
+		acx->num_fields = filter->num_fields;
+		acx->action = filter->action;
+		wl1271_rx_filter_flatten_fields(filter, acx->fields);
+	}
+
+	wl1271_dump(DEBUG_ACX, "RX_FILTER: ", acx, acx_size);
+
+	ret = wl1271_cmd_configure(wl, ACX_SET_RX_DATA_FILTER, acx, acx_size);
+	if (ret < 0) {
+		wl1271_warning("setting rx filter failed: %d", ret);
+		goto out;
+	}
+
+out:
+	kfree(acx);
+	return ret;
+}
diff --git a/drivers/net/wireless/wl12xx/acx.h b/drivers/net/wireless/ti/wlcore/acx.h
index a28fc044034..8106b2ebfe6 100644
--- a/drivers/net/wireless/wl12xx/acx.h
+++ b/drivers/net/wireless/ti/wlcore/acx.h
@@ -25,7 +25,7 @@
 #ifndef __ACX_H__
 #define __ACX_H__
 
-#include "wl12xx.h"
+#include "wlcore.h"
 #include "cmd.h"
 
 /*************************************************************************
@@ -824,16 +824,11 @@ struct wl1271_acx_keep_alive_config {
 	__le32 period;
 } __packed;
 
+/* TODO: maybe this needs to be moved somewhere else? */
 #define HOST_IF_CFG_RX_FIFO_ENABLE     BIT(0)
 #define HOST_IF_CFG_TX_EXTRA_BLKS_SWAP BIT(1)
 #define HOST_IF_CFG_TX_PAD_TO_SDIO_BLK BIT(3)
 
-struct wl1271_acx_host_config_bitmap {
-	struct acx_header header;
-
-	__le32 host_cfg_bitmap;
-} __packed;
-
 enum {
 	WL1271_ACX_TRIG_TYPE_LEVEL = 0,
 	WL1271_ACX_TRIG_TYPE_EDGE,
@@ -1152,6 +1147,32 @@ struct wl12xx_acx_config_hangover {
 	u8 padding[2];
 } __packed;
 
+
+struct acx_default_rx_filter {
+	struct acx_header header;
+	u8 enable;
+
+	/* action of type FILTER_XXX */
+	u8 default_action;
+
+	u8 pad[2];
+} __packed;
+
+
+struct acx_rx_filter_cfg {
+	struct acx_header header;
+
+	u8 enable;
+
+	/* 0 - WL1271_MAX_RX_FILTERS-1 */
+	u8 index;
+
+	u8 action;
+
+	u8 num_fields;
+	u8 fields[0];
+} __packed;
+
 enum {
 	ACX_WAKE_UP_CONDITIONS           = 0x0000,
 	ACX_MEM_CFG                      = 0x0001,
@@ -1274,7 +1295,6 @@ int wl1271_acx_frag_threshold(struct wl1271 *wl, u32 frag_threshold);
 int wl1271_acx_tx_config_options(struct wl1271 *wl);
 int wl12xx_acx_mem_cfg(struct wl1271 *wl);
 int wl1271_acx_init_mem_config(struct wl1271 *wl);
-int wl1271_acx_host_if_cfg_bitmap(struct wl1271 *wl, u32 host_cfg_bitmap);
 int wl1271_acx_init_rx_interrupt(struct wl1271 *wl);
 int wl1271_acx_smart_reflex(struct wl1271 *wl);
 int wl1271_acx_bet_enable(struct wl1271 *wl, struct wl12xx_vif *wlvif,
@@ -1310,5 +1330,9 @@ 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 wl1271_acx_default_rx_filter_enable(struct wl1271 *wl, bool enable,
+					enum rx_filter_action action);
+int wl1271_acx_set_rx_filter(struct wl1271 *wl, u8 index, bool enable,
+			     struct wl12xx_rx_filter *filter);
 
 #endif /* __WL1271_ACX_H__ */
diff --git a/drivers/net/wireless/ti/wlcore/boot.c b/drivers/net/wireless/ti/wlcore/boot.c
new file mode 100644
index 00000000000..9b98230f84c
--- /dev/null
+++ b/drivers/net/wireless/ti/wlcore/boot.c
@@ -0,0 +1,444 @@
+/*
+ * This file is part of wl1271
+ *
+ * Copyright (C) 2008-2010 Nokia Corporation
+ *
+ * Contact: Luciano Coelho <luciano.coelho@nokia.com>
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * version 2 as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+ * General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA
+ * 02110-1301 USA
+ *
+ */
+
+#include <linux/slab.h>
+#include <linux/wl12xx.h>
+#include <linux/export.h>
+
+#include "debug.h"
+#include "acx.h"
+#include "boot.h"
+#include "io.h"
+#include "event.h"
+#include "rx.h"
+#include "hw_ops.h"
+
+static void wl1271_boot_set_ecpu_ctrl(struct wl1271 *wl, u32 flag)
+{
+	u32 cpu_ctrl;
+
+	/* 10.5.0 run the firmware (I) */
+	cpu_ctrl = wlcore_read_reg(wl, REG_ECPU_CONTROL);
+
+	/* 10.5.1 run the firmware (II) */
+	cpu_ctrl |= flag;
+	wlcore_write_reg(wl, REG_ECPU_CONTROL, cpu_ctrl);
+}
+
+static int wlcore_parse_fw_ver(struct wl1271 *wl)
+{
+	int ret;
+
+	ret = sscanf(wl->chip.fw_ver_str + 4, "%u.%u.%u.%u.%u",
+		     &wl->chip.fw_ver[0], &wl->chip.fw_ver[1],
+		     &wl->chip.fw_ver[2], &wl->chip.fw_ver[3],
+		     &wl->chip.fw_ver[4]);
+
+	if (ret != 5) {
+		wl1271_warning("fw version incorrect value");
+		memset(wl->chip.fw_ver, 0, sizeof(wl->chip.fw_ver));
+		return -EINVAL;
+	}
+
+	ret = wlcore_identify_fw(wl);
+	if (ret < 0)
+		return ret;
+
+	return 0;
+}
+
+static int wlcore_boot_fw_version(struct wl1271 *wl)
+{
+	struct wl1271_static_data *static_data;
+	int ret;
+
+	static_data = kmalloc(sizeof(*static_data), GFP_KERNEL | GFP_DMA);
+	if (!static_data) {
+		wl1271_error("Couldn't allocate memory for static data!");
+		return -ENOMEM;
+	}
+
+	wl1271_read(wl, wl->cmd_box_addr, static_data, sizeof(*static_data),
+		    false);
+
+	strncpy(wl->chip.fw_ver_str, static_data->fw_version,
+		sizeof(wl->chip.fw_ver_str));
+
+	kfree(static_data);
+
+	/* make sure the string is NULL-terminated */
+	wl->chip.fw_ver_str[sizeof(wl->chip.fw_ver_str) - 1] = '\0';
+
+	ret = wlcore_parse_fw_ver(wl);
+	if (ret < 0)
+		return ret;
+
+	return 0;
+}
+
+static int wl1271_boot_upload_firmware_chunk(struct wl1271 *wl, void *buf,
+					     size_t fw_data_len, u32 dest)
+{
+	struct wlcore_partition_set partition;
+	int addr, chunk_num, partition_limit;
+	u8 *p, *chunk;
+
+	/* whal_FwCtrl_LoadFwImageSm() */
+
+	wl1271_debug(DEBUG_BOOT, "starting firmware upload");
+
+	wl1271_debug(DEBUG_BOOT, "fw_data_len %zd chunk_size %d",
+		     fw_data_len, CHUNK_SIZE);
+
+	if ((fw_data_len % 4) != 0) {
+		wl1271_error("firmware length not multiple of four");
+		return -EIO;
+	}
+
+	chunk = kmalloc(CHUNK_SIZE, GFP_KERNEL);
+	if (!chunk) {
+		wl1271_error("allocation for firmware upload chunk failed");
+		return -ENOMEM;
+	}
+
+	memcpy(&partition, &wl->ptable[PART_DOWN], sizeof(partition));
+	partition.mem.start = dest;
+	wlcore_set_partition(wl, &partition);
+
+	/* 10.1 set partition limit and chunk num */
+	chunk_num = 0;
+	partition_limit = wl->ptable[PART_DOWN].mem.size;
+
+	while (chunk_num < fw_data_len / CHUNK_SIZE) {
+		/* 10.2 update partition, if needed */
+		addr = dest + (chunk_num + 2) * CHUNK_SIZE;
+		if (addr > partition_limit) {
+			addr = dest + chunk_num * CHUNK_SIZE;
+			partition_limit = chunk_num * CHUNK_SIZE +
+				wl->ptable[PART_DOWN].mem.size;
+			partition.mem.start = addr;
+			wlcore_set_partition(wl, &partition);
+		}
+
+		/* 10.3 upload the chunk */
+		addr = dest + chunk_num * CHUNK_SIZE;
+		p = buf + chunk_num * CHUNK_SIZE;
+		memcpy(chunk, p, CHUNK_SIZE);
+		wl1271_debug(DEBUG_BOOT, "uploading fw chunk 0x%p to 0x%x",
+			     p, addr);
+		wl1271_write(wl, addr, chunk, CHUNK_SIZE, false);
+
+		chunk_num++;
+	}
+
+	/* 10.4 upload the last chunk */
+	addr = dest + chunk_num * CHUNK_SIZE;
+	p = buf + chunk_num * CHUNK_SIZE;
+	memcpy(chunk, p, fw_data_len % CHUNK_SIZE);
+	wl1271_debug(DEBUG_BOOT, "uploading fw last chunk (%zd B) 0x%p to 0x%x",
+		     fw_data_len % CHUNK_SIZE, p, addr);
+	wl1271_write(wl, addr, chunk, fw_data_len % CHUNK_SIZE, false);
+
+	kfree(chunk);
+	return 0;
+}
+
+int wlcore_boot_upload_firmware(struct wl1271 *wl)
+{
+	u32 chunks, addr, len;
+	int ret = 0;
+	u8 *fw;
+
+	fw = wl->fw;
+	chunks = be32_to_cpup((__be32 *) fw);
+	fw += sizeof(u32);
+
+	wl1271_debug(DEBUG_BOOT, "firmware chunks to be uploaded: %u", chunks);
+
+	while (chunks--) {
+		addr = be32_to_cpup((__be32 *) fw);
+		fw += sizeof(u32);
+		len = be32_to_cpup((__be32 *) fw);
+		fw += sizeof(u32);
+
+		if (len > 300000) {
+			wl1271_info("firmware chunk too long: %u", len);
+			return -EINVAL;
+		}
+		wl1271_debug(DEBUG_BOOT, "chunk %d addr 0x%x len %u",
+			     chunks, addr, len);
+		ret = wl1271_boot_upload_firmware_chunk(wl, fw, len, addr);
+		if (ret != 0)
+			break;
+		fw += len;
+	}
+
+	return ret;
+}
+EXPORT_SYMBOL_GPL(wlcore_boot_upload_firmware);
+
+int wlcore_boot_upload_nvs(struct wl1271 *wl)
+{
+	size_t nvs_len, burst_len;
+	int i;
+	u32 dest_addr, val;
+	u8 *nvs_ptr, *nvs_aligned;
+
+	if (wl->nvs == NULL)
+		return -ENODEV;
+
+	if (wl->quirks & WLCORE_QUIRK_LEGACY_NVS) {
+		struct wl1271_nvs_file *nvs =
+			(struct wl1271_nvs_file *)wl->nvs;
+		/*
+		 * FIXME: the LEGACY NVS image support (NVS's missing the 5GHz
+		 * band configurations) can be removed when those NVS files stop
+		 * floating around.
+		 */
+		if (wl->nvs_len == sizeof(struct wl1271_nvs_file) ||
+		    wl->nvs_len == WL1271_INI_LEGACY_NVS_FILE_SIZE) {
+			if (nvs->general_params.dual_mode_select)
+				wl->enable_11a = true;
+		}
+
+		if (wl->nvs_len != sizeof(struct wl1271_nvs_file) &&
+		    (wl->nvs_len != WL1271_INI_LEGACY_NVS_FILE_SIZE ||
+		     wl->enable_11a)) {
+			wl1271_error("nvs size is not as expected: %zu != %zu",
+				wl->nvs_len, sizeof(struct wl1271_nvs_file));
+			kfree(wl->nvs);
+			wl->nvs = NULL;
+			wl->nvs_len = 0;
+			return -EILSEQ;
+		}
+
+		/* only the first part of the NVS needs to be uploaded */
+		nvs_len = sizeof(nvs->nvs);
+		nvs_ptr = (u8 *) nvs->nvs;
+	} else {
+		struct wl128x_nvs_file *nvs = (struct wl128x_nvs_file *)wl->nvs;
+
+		if (wl->nvs_len == sizeof(struct wl128x_nvs_file)) {
+			if (nvs->general_params.dual_mode_select)
+				wl->enable_11a = true;
+		} else {
+			wl1271_error("nvs size is not as expected: %zu != %zu",
+				     wl->nvs_len,
+				     sizeof(struct wl128x_nvs_file));
+			kfree(wl->nvs);
+			wl->nvs = NULL;
+			wl->nvs_len = 0;
+			return -EILSEQ;
+		}
+
+		/* only the first part of the NVS needs to be uploaded */
+		nvs_len = sizeof(nvs->nvs);
+		nvs_ptr = (u8 *)nvs->nvs;
+	}
+
+	/* update current MAC address to NVS */
+	nvs_ptr[11] = wl->addresses[0].addr[0];
+	nvs_ptr[10] = wl->addresses[0].addr[1];
+	nvs_ptr[6] = wl->addresses[0].addr[2];
+	nvs_ptr[5] = wl->addresses[0].addr[3];
+	nvs_ptr[4] = wl->addresses[0].addr[4];
+	nvs_ptr[3] = wl->addresses[0].addr[5];
+
+	/*
+	 * Layout before the actual NVS tables:
+	 * 1 byte : burst length.
+	 * 2 bytes: destination address.
+	 * n bytes: data to burst copy.
+	 *
+	 * This is ended by a 0 length, then the NVS tables.
+	 */
+
+	/* FIXME: Do we need to check here whether the LSB is 1? */
+	while (nvs_ptr[0]) {
+		burst_len = nvs_ptr[0];
+		dest_addr = (nvs_ptr[1] & 0xfe) | ((u32)(nvs_ptr[2] << 8));
+
+		/*
+		 * Due to our new wl1271_translate_reg_addr function,
+		 * we need to add the register partition start address
+		 * to the destination
+		 */
+		dest_addr += wl->curr_part.reg.start;
+
+		/* We move our pointer to the data */
+		nvs_ptr += 3;
+
+		for (i = 0; i < burst_len; i++) {
+			if (nvs_ptr + 3 >= (u8 *) wl->nvs + nvs_len)
+				goto out_badnvs;
+
+			val = (nvs_ptr[0] | (nvs_ptr[1] << 8)
+			       | (nvs_ptr[2] << 16) | (nvs_ptr[3] << 24));
+
+			wl1271_debug(DEBUG_BOOT,
+				     "nvs burst write 0x%x: 0x%x",
+				     dest_addr, val);
+			wl1271_write32(wl, dest_addr, val);
+
+			nvs_ptr += 4;
+			dest_addr += 4;
+		}
+
+		if (nvs_ptr >= (u8 *) wl->nvs + nvs_len)
+			goto out_badnvs;
+	}
+
+	/*
+	 * We've reached the first zero length, the first NVS table
+	 * is located at an aligned offset which is at least 7 bytes further.
+	 * NOTE: The wl->nvs->nvs element must be first, in order to
+	 * simplify the casting, we assume it is at the beginning of
+	 * the wl->nvs structure.
+	 */
+	nvs_ptr = (u8 *)wl->nvs +
+			ALIGN(nvs_ptr - (u8 *)wl->nvs + 7, 4);
+
+	if (nvs_ptr >= (u8 *) wl->nvs + nvs_len)
+		goto out_badnvs;
+
+	nvs_len -= nvs_ptr - (u8 *)wl->nvs;
+
+	/* Now we must set the partition correctly */
+	wlcore_set_partition(wl, &wl->ptable[PART_WORK]);
+
+	/* Copy the NVS tables to a new block to ensure alignment */
+	nvs_aligned = kmemdup(nvs_ptr, nvs_len, GFP_KERNEL);
+	if (!nvs_aligned)
+		return -ENOMEM;
+
+	/* And finally we upload the NVS tables */
+	wlcore_write_data(wl, REG_CMD_MBOX_ADDRESS,
+			  nvs_aligned, nvs_len, false);
+
+	kfree(nvs_aligned);
+	return 0;
+
+out_badnvs:
+	wl1271_error("nvs data is malformed");
+	return -EILSEQ;
+}
+EXPORT_SYMBOL_GPL(wlcore_boot_upload_nvs);
+
+int wlcore_boot_run_firmware(struct wl1271 *wl)
+{
+	int loop, ret;
+	u32 chip_id, intr;
+
+	/* Make sure we have the boot partition */
+	wlcore_set_partition(wl, &wl->ptable[PART_BOOT]);
+
+	wl1271_boot_set_ecpu_ctrl(wl, ECPU_CONTROL_HALT);
+
+	chip_id = wlcore_read_reg(wl, REG_CHIP_ID_B);
+
+	wl1271_debug(DEBUG_BOOT, "chip id after firmware boot: 0x%x", chip_id);
+
+	if (chip_id != wl->chip.id) {
+		wl1271_error("chip id doesn't match after firmware boot");
+		return -EIO;
+	}
+
+	/* wait for init to complete */
+	loop = 0;
+	while (loop++ < INIT_LOOP) {
+		udelay(INIT_LOOP_DELAY);
+		intr = wlcore_read_reg(wl, REG_INTERRUPT_NO_CLEAR);
+
+		if (intr == 0xffffffff) {
+			wl1271_error("error reading hardware complete "
+				     "init indication");
+			return -EIO;
+		}
+		/* check that ACX_INTR_INIT_COMPLETE is enabled */
+		else if (intr & WL1271_ACX_INTR_INIT_COMPLETE) {
+			wlcore_write_reg(wl, REG_INTERRUPT_ACK,
+					 WL1271_ACX_INTR_INIT_COMPLETE);
+			break;
+		}
+	}
+
+	if (loop > INIT_LOOP) {
+		wl1271_error("timeout waiting for the hardware to "
+			     "complete initialization");
+		return -EIO;
+	}
+
+	/* get hardware config command mail box */
+	wl->cmd_box_addr = wlcore_read_reg(wl, REG_COMMAND_MAILBOX_PTR);
+
+	wl1271_debug(DEBUG_MAILBOX, "cmd_box_addr 0x%x", wl->cmd_box_addr);
+
+	/* get hardware config event mail box */
+	wl->mbox_ptr[0] = wlcore_read_reg(wl, REG_EVENT_MAILBOX_PTR);
+	wl->mbox_ptr[1] = wl->mbox_ptr[0] + sizeof(struct event_mailbox);
+
+	wl1271_debug(DEBUG_MAILBOX, "MBOX ptrs: 0x%x 0x%x",
+		     wl->mbox_ptr[0], wl->mbox_ptr[1]);
+
+	ret = wlcore_boot_fw_version(wl);
+	if (ret < 0) {
+		wl1271_error("couldn't boot firmware");
+		return ret;
+	}
+
+	/*
+	 * in case of full asynchronous mode the firmware event must be
+	 * ready to receive event from the command mailbox
+	 */
+
+	/* unmask required mbox events  */
+	wl->event_mask = BSS_LOSE_EVENT_ID |
+		REGAINED_BSS_EVENT_ID |
+		SCAN_COMPLETE_EVENT_ID |
+		ROLE_STOP_COMPLETE_EVENT_ID |
+		RSSI_SNR_TRIGGER_0_EVENT_ID |
+		PSPOLL_DELIVERY_FAILURE_EVENT_ID |
+		SOFT_GEMINI_SENSE_EVENT_ID |
+		PERIODIC_SCAN_REPORT_EVENT_ID |
+		PERIODIC_SCAN_COMPLETE_EVENT_ID |
+		DUMMY_PACKET_EVENT_ID |
+		PEER_REMOVE_COMPLETE_EVENT_ID |
+		BA_SESSION_RX_CONSTRAINT_EVENT_ID |
+		REMAIN_ON_CHANNEL_COMPLETE_EVENT_ID |
+		INACTIVE_STA_EVENT_ID |
+		MAX_TX_RETRY_EVENT_ID |
+		CHANNEL_SWITCH_COMPLETE_EVENT_ID;
+
+	ret = wl1271_event_unmask(wl);
+	if (ret < 0) {
+		wl1271_error("EVENT mask setting failed");
+		return ret;
+	}
+
+	/* set the working partition to its "running" mode offset */
+	wlcore_set_partition(wl, &wl->ptable[PART_WORK]);
+
+	/* firmware startup completed */
+	return 0;
+}
+EXPORT_SYMBOL_GPL(wlcore_boot_run_firmware);
diff --git a/drivers/net/wireless/ti/wlcore/boot.h b/drivers/net/wireless/ti/wlcore/boot.h
new file mode 100644
index 00000000000..094981dd222
--- /dev/null
+++ b/drivers/net/wireless/ti/wlcore/boot.h
@@ -0,0 +1,54 @@
+/*
+ * This file is part of wl1271
+ *
+ * Copyright (C) 2008-2009 Nokia Corporation
+ *
+ * Contact: Luciano Coelho <luciano.coelho@nokia.com>
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * version 2 as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+ * General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA
+ * 02110-1301 USA
+ *
+ */
+
+#ifndef __BOOT_H__
+#define __BOOT_H__
+
+#include "wlcore.h"
+
+int wlcore_boot_upload_firmware(struct wl1271 *wl);
+int wlcore_boot_upload_nvs(struct wl1271 *wl);
+int wlcore_boot_run_firmware(struct wl1271 *wl);
+
+#define WL1271_NO_SUBBANDS 8
+#define WL1271_NO_POWER_LEVELS 4
+#define WL1271_FW_VERSION_MAX_LEN 20
+
+struct wl1271_static_data {
+	u8 mac_address[ETH_ALEN];
+	u8 padding[2];
+	u8 fw_version[WL1271_FW_VERSION_MAX_LEN];
+	u32 hw_version;
+	u8 tx_power_table[WL1271_NO_SUBBANDS][WL1271_NO_POWER_LEVELS];
+};
+
+/* number of times we try to read the INIT interrupt */
+#define INIT_LOOP 20000
+
+/* delay between retries */
+#define INIT_LOOP_DELAY 50
+
+#define WU_COUNTER_PAUSE_VAL 0x3FF
+#define WELP_ARM_COMMAND_VAL 0x4
+
+#endif
diff --git a/drivers/net/wireless/wl12xx/cmd.c b/drivers/net/wireless/ti/wlcore/cmd.c
index 3414fc11e9b..5b128a97144 100644
--- a/drivers/net/wireless/wl12xx/cmd.c
+++ b/drivers/net/wireless/ti/wlcore/cmd.c
@@ -28,9 +28,8 @@
 #include <linux/ieee80211.h>
 #include <linux/slab.h>
 
-#include "wl12xx.h"
+#include "wlcore.h"
 #include "debug.h"
-#include "reg.h"
 #include "io.h"
 #include "acx.h"
 #include "wl12xx_80211.h"
@@ -67,11 +66,15 @@ int wl1271_cmd_send(struct wl1271 *wl, u16 id, void *buf, size_t len,
 
 	wl1271_write(wl, wl->cmd_box_addr, buf, len, false);
 
-	wl1271_write32(wl, ACX_REG_INTERRUPT_TRIG, INTR_TRIG_CMD);
+	/*
+	 * TODO: we just need this because one bit is in a different
+	 * place.  Is there any better way?
+	 */
+	wl->ops->trigger_cmd(wl, wl->cmd_box_addr, buf, len);
 
 	timeout = jiffies + msecs_to_jiffies(WL1271_COMMAND_TIMEOUT);
 
-	intr = wl1271_read32(wl, ACX_REG_INTERRUPT_NO_CLEAR);
+	intr = wlcore_read_reg(wl, REG_INTERRUPT_NO_CLEAR);
 	while (!(intr & WL1271_ACX_INTR_CMD_COMPLETE)) {
 		if (time_after(jiffies, timeout)) {
 			wl1271_error("command complete timeout");
@@ -85,7 +88,7 @@ int wl1271_cmd_send(struct wl1271 *wl, u16 id, void *buf, size_t len,
 		else
 			msleep(1);
 
-		intr = wl1271_read32(wl, ACX_REG_INTERRUPT_NO_CLEAR);
+		intr = wlcore_read_reg(wl, REG_INTERRUPT_NO_CLEAR);
 	}
 
 	/* read back the status code of the command */
@@ -100,8 +103,7 @@ int wl1271_cmd_send(struct wl1271 *wl, u16 id, void *buf, size_t len,
 		goto fail;
 	}
 
-	wl1271_write32(wl, ACX_REG_INTERRUPT_ACK,
-		       WL1271_ACX_INTR_CMD_COMPLETE);
+	wlcore_write_reg(wl, REG_INTERRUPT_ACK, WL1271_ACX_INTR_CMD_COMPLETE);
 	return 0;
 
 fail:
@@ -110,240 +112,20 @@ fail:
 	return ret;
 }
 
-int wl1271_cmd_general_parms(struct wl1271 *wl)
-{
-	struct wl1271_general_parms_cmd *gen_parms;
-	struct wl1271_ini_general_params *gp =
-		&((struct wl1271_nvs_file *)wl->nvs)->general_params;
-	bool answer = false;
-	int ret;
-
-	if (!wl->nvs)
-		return -ENODEV;
-
-	if (gp->tx_bip_fem_manufacturer >= WL1271_INI_FEM_MODULE_COUNT) {
-		wl1271_warning("FEM index from INI out of bounds");
-		return -EINVAL;
-	}
-
-	gen_parms = kzalloc(sizeof(*gen_parms), GFP_KERNEL);
-	if (!gen_parms)
-		return -ENOMEM;
-
-	gen_parms->test.id = TEST_CMD_INI_FILE_GENERAL_PARAM;
-
-	memcpy(&gen_parms->general_params, gp, sizeof(*gp));
-
-	if (gp->tx_bip_fem_auto_detect)
-		answer = true;
-
-	/* Override the REF CLK from the NVS with the one from platform data */
-	gen_parms->general_params.ref_clock = wl->ref_clock;
-
-	ret = wl1271_cmd_test(wl, gen_parms, sizeof(*gen_parms), answer);
-	if (ret < 0) {
-		wl1271_warning("CMD_INI_FILE_GENERAL_PARAM failed");
-		goto out;
-	}
-
-	gp->tx_bip_fem_manufacturer =
-		gen_parms->general_params.tx_bip_fem_manufacturer;
-
-	if (gp->tx_bip_fem_manufacturer >= WL1271_INI_FEM_MODULE_COUNT) {
-		wl1271_warning("FEM index from FW out of bounds");
-		ret = -EINVAL;
-		goto out;
-	}
-
-	wl1271_debug(DEBUG_CMD, "FEM autodetect: %s, manufacturer: %d\n",
-		     answer ? "auto" : "manual", gp->tx_bip_fem_manufacturer);
-
-out:
-	kfree(gen_parms);
-	return ret;
-}
-
-int wl128x_cmd_general_parms(struct wl1271 *wl)
-{
-	struct wl128x_general_parms_cmd *gen_parms;
-	struct wl128x_ini_general_params *gp =
-		&((struct wl128x_nvs_file *)wl->nvs)->general_params;
-	bool answer = false;
-	int ret;
-
-	if (!wl->nvs)
-		return -ENODEV;
-
-	if (gp->tx_bip_fem_manufacturer >= WL1271_INI_FEM_MODULE_COUNT) {
-		wl1271_warning("FEM index from ini out of bounds");
-		return -EINVAL;
-	}
-
-	gen_parms = kzalloc(sizeof(*gen_parms), GFP_KERNEL);
-	if (!gen_parms)
-		return -ENOMEM;
-
-	gen_parms->test.id = TEST_CMD_INI_FILE_GENERAL_PARAM;
-
-	memcpy(&gen_parms->general_params, gp, sizeof(*gp));
-
-	if (gp->tx_bip_fem_auto_detect)
-		answer = true;
-
-	/* Replace REF and TCXO CLKs with the ones from platform data */
-	gen_parms->general_params.ref_clock = wl->ref_clock;
-	gen_parms->general_params.tcxo_ref_clock = wl->tcxo_clock;
-
-	ret = wl1271_cmd_test(wl, gen_parms, sizeof(*gen_parms), answer);
-	if (ret < 0) {
-		wl1271_warning("CMD_INI_FILE_GENERAL_PARAM failed");
-		goto out;
-	}
-
-	gp->tx_bip_fem_manufacturer =
-		gen_parms->general_params.tx_bip_fem_manufacturer;
-
-	if (gp->tx_bip_fem_manufacturer >= WL1271_INI_FEM_MODULE_COUNT) {
-		wl1271_warning("FEM index from FW out of bounds");
-		ret = -EINVAL;
-		goto out;
-	}
-
-	wl1271_debug(DEBUG_CMD, "FEM autodetect: %s, manufacturer: %d\n",
-		     answer ? "auto" : "manual", gp->tx_bip_fem_manufacturer);
-
-out:
-	kfree(gen_parms);
-	return ret;
-}
-
-int wl1271_cmd_radio_parms(struct wl1271 *wl)
-{
-	struct wl1271_nvs_file *nvs = (struct wl1271_nvs_file *)wl->nvs;
-	struct wl1271_radio_parms_cmd *radio_parms;
-	struct wl1271_ini_general_params *gp = &nvs->general_params;
-	int ret;
-
-	if (!wl->nvs)
-		return -ENODEV;
-
-	radio_parms = kzalloc(sizeof(*radio_parms), GFP_KERNEL);
-	if (!radio_parms)
-		return -ENOMEM;
-
-	radio_parms->test.id = TEST_CMD_INI_FILE_RADIO_PARAM;
-
-	/* 2.4GHz parameters */
-	memcpy(&radio_parms->static_params_2, &nvs->stat_radio_params_2,
-	       sizeof(struct wl1271_ini_band_params_2));
-	memcpy(&radio_parms->dyn_params_2,
-	       &nvs->dyn_radio_params_2[gp->tx_bip_fem_manufacturer].params,
-	       sizeof(struct wl1271_ini_fem_params_2));
-
-	/* 5GHz parameters */
-	memcpy(&radio_parms->static_params_5,
-	       &nvs->stat_radio_params_5,
-	       sizeof(struct wl1271_ini_band_params_5));
-	memcpy(&radio_parms->dyn_params_5,
-	       &nvs->dyn_radio_params_5[gp->tx_bip_fem_manufacturer].params,
-	       sizeof(struct wl1271_ini_fem_params_5));
-
-	wl1271_dump(DEBUG_CMD, "TEST_CMD_INI_FILE_RADIO_PARAM: ",
-		    radio_parms, sizeof(*radio_parms));
-
-	ret = wl1271_cmd_test(wl, radio_parms, sizeof(*radio_parms), 0);
-	if (ret < 0)
-		wl1271_warning("CMD_INI_FILE_RADIO_PARAM failed");
-
-	kfree(radio_parms);
-	return ret;
-}
-
-int wl128x_cmd_radio_parms(struct wl1271 *wl)
-{
-	struct wl128x_nvs_file *nvs = (struct wl128x_nvs_file *)wl->nvs;
-	struct wl128x_radio_parms_cmd *radio_parms;
-	struct wl128x_ini_general_params *gp = &nvs->general_params;
-	int ret;
-
-	if (!wl->nvs)
-		return -ENODEV;
-
-	radio_parms = kzalloc(sizeof(*radio_parms), GFP_KERNEL);
-	if (!radio_parms)
-		return -ENOMEM;
-
-	radio_parms->test.id = TEST_CMD_INI_FILE_RADIO_PARAM;
-
-	/* 2.4GHz parameters */
-	memcpy(&radio_parms->static_params_2, &nvs->stat_radio_params_2,
-	       sizeof(struct wl128x_ini_band_params_2));
-	memcpy(&radio_parms->dyn_params_2,
-	       &nvs->dyn_radio_params_2[gp->tx_bip_fem_manufacturer].params,
-	       sizeof(struct wl128x_ini_fem_params_2));
-
-	/* 5GHz parameters */
-	memcpy(&radio_parms->static_params_5,
-	       &nvs->stat_radio_params_5,
-	       sizeof(struct wl128x_ini_band_params_5));
-	memcpy(&radio_parms->dyn_params_5,
-	       &nvs->dyn_radio_params_5[gp->tx_bip_fem_manufacturer].params,
-	       sizeof(struct wl128x_ini_fem_params_5));
-
-	radio_parms->fem_vendor_and_options = nvs->fem_vendor_and_options;
-
-	wl1271_dump(DEBUG_CMD, "TEST_CMD_INI_FILE_RADIO_PARAM: ",
-		    radio_parms, sizeof(*radio_parms));
-
-	ret = wl1271_cmd_test(wl, radio_parms, sizeof(*radio_parms), 0);
-	if (ret < 0)
-		wl1271_warning("CMD_INI_FILE_RADIO_PARAM failed");
-
-	kfree(radio_parms);
-	return ret;
-}
-
-int wl1271_cmd_ext_radio_parms(struct wl1271 *wl)
-{
-	struct wl1271_ext_radio_parms_cmd *ext_radio_parms;
-	struct conf_rf_settings *rf = &wl->conf.rf;
-	int ret;
-
-	if (!wl->nvs)
-		return -ENODEV;
-
-	ext_radio_parms = kzalloc(sizeof(*ext_radio_parms), GFP_KERNEL);
-	if (!ext_radio_parms)
-		return -ENOMEM;
-
-	ext_radio_parms->test.id = TEST_CMD_INI_FILE_RF_EXTENDED_PARAM;
-
-	memcpy(ext_radio_parms->tx_per_channel_power_compensation_2,
-	       rf->tx_per_channel_power_compensation_2,
-	       CONF_TX_PWR_COMPENSATION_LEN_2);
-	memcpy(ext_radio_parms->tx_per_channel_power_compensation_5,
-	       rf->tx_per_channel_power_compensation_5,
-	       CONF_TX_PWR_COMPENSATION_LEN_5);
-
-	wl1271_dump(DEBUG_CMD, "TEST_CMD_INI_FILE_EXT_RADIO_PARAM: ",
-		    ext_radio_parms, sizeof(*ext_radio_parms));
-
-	ret = wl1271_cmd_test(wl, ext_radio_parms, sizeof(*ext_radio_parms), 0);
-	if (ret < 0)
-		wl1271_warning("TEST_CMD_INI_FILE_RF_EXTENDED_PARAM failed");
-
-	kfree(ext_radio_parms);
-	return ret;
-}
-
 /*
  * Poll the mailbox event field until any of the bits in the mask is set or a
  * timeout occurs (WL1271_EVENT_TIMEOUT in msecs)
  */
 static int wl1271_cmd_wait_for_event_or_timeout(struct wl1271 *wl, u32 mask)
 {
-	u32 events_vector, event;
+	u32 *events_vector;
+	u32 event;
 	unsigned long timeout;
+	int ret = 0;
+
+	events_vector = kmalloc(sizeof(*events_vector), GFP_KERNEL | GFP_DMA);
+	if (!events_vector)
+		return -ENOMEM;
 
 	timeout = jiffies + msecs_to_jiffies(WL1271_EVENT_TIMEOUT);
 
@@ -351,21 +133,24 @@ static int wl1271_cmd_wait_for_event_or_timeout(struct wl1271 *wl, u32 mask)
 		if (time_after(jiffies, timeout)) {
 			wl1271_debug(DEBUG_CMD, "timeout waiting for event %d",
 				     (int)mask);
-			return -ETIMEDOUT;
+			ret = -ETIMEDOUT;
+			goto out;
 		}
 
 		msleep(1);
 
 		/* read from both event fields */
-		wl1271_read(wl, wl->mbox_ptr[0], &events_vector,
-			    sizeof(events_vector), false);
-		event = events_vector & mask;
-		wl1271_read(wl, wl->mbox_ptr[1], &events_vector,
-			    sizeof(events_vector), false);
-		event |= events_vector & mask;
+		wl1271_read(wl, wl->mbox_ptr[0], events_vector,
+			    sizeof(*events_vector), false);
+		event = *events_vector & mask;
+		wl1271_read(wl, wl->mbox_ptr[1], events_vector,
+			    sizeof(*events_vector), false);
+		event |= *events_vector & mask;
 	} while (!event);
 
-	return 0;
+out:
+	kfree(events_vector);
+	return ret;
 }
 
 static int wl1271_cmd_wait_for_event(struct wl1271 *wl, u32 mask)
@@ -522,7 +307,7 @@ static int wl12xx_cmd_role_start_dev(struct wl1271 *wl,
 
 	cmd->role_id = wlvif->dev_role_id;
 	if (wlvif->band == IEEE80211_BAND_5GHZ)
-		cmd->band = WL12XX_BAND_5GHZ;
+		cmd->band = WLCORE_BAND_5GHZ;
 	cmd->channel = wlvif->channel;
 
 	if (wlvif->dev_hlid == WL12XX_INVALID_LINK_ID) {
@@ -613,7 +398,7 @@ int wl12xx_cmd_role_start_sta(struct wl1271 *wl, struct wl12xx_vif *wlvif)
 
 	cmd->role_id = wlvif->role_id;
 	if (wlvif->band == IEEE80211_BAND_5GHZ)
-		cmd->band = WL12XX_BAND_5GHZ;
+		cmd->band = WLCORE_BAND_5GHZ;
 	cmd->channel = wlvif->channel;
 	cmd->sta.basic_rate_set = cpu_to_le32(wlvif->basic_rate_set);
 	cmd->sta.beacon_interval = cpu_to_le16(wlvif->beacon_int);
@@ -750,14 +535,14 @@ int wl12xx_cmd_role_start_ap(struct wl1271 *wl, struct wl12xx_vif *wlvif)
 
 	switch (wlvif->band) {
 	case IEEE80211_BAND_2GHZ:
-		cmd->band = RADIO_BAND_2_4GHZ;
+		cmd->band = WLCORE_BAND_2_4GHZ;
 		break;
 	case IEEE80211_BAND_5GHZ:
-		cmd->band = RADIO_BAND_5GHZ;
+		cmd->band = WLCORE_BAND_5GHZ;
 		break;
 	default:
 		wl1271_warning("ap start - unknown band: %d", (int)wlvif->band);
-		cmd->band = RADIO_BAND_2_4GHZ;
+		cmd->band = WLCORE_BAND_2_4GHZ;
 		break;
 	}
 
@@ -830,7 +615,7 @@ int wl12xx_cmd_role_start_ibss(struct wl1271 *wl, struct wl12xx_vif *wlvif)
 
 	cmd->role_id = wlvif->role_id;
 	if (wlvif->band == IEEE80211_BAND_5GHZ)
-		cmd->band = WL12XX_BAND_5GHZ;
+		cmd->band = WLCORE_BAND_5GHZ;
 	cmd->channel = wlvif->channel;
 	cmd->ibss.basic_rate_set = cpu_to_le32(wlvif->basic_rate_set);
 	cmd->ibss.beacon_interval = cpu_to_le16(wlvif->beacon_int);
@@ -904,6 +689,7 @@ int wl1271_cmd_test(struct wl1271 *wl, void *buf, size_t buf_len, u8 answer)
 
 	return ret;
 }
+EXPORT_SYMBOL_GPL(wl1271_cmd_test);
 
 /**
  * read acx from firmware
@@ -960,6 +746,7 @@ int wl1271_cmd_configure(struct wl1271 *wl, u16 id, void *buf, size_t len)
 
 	return 0;
 }
+EXPORT_SYMBOL_GPL(wl1271_cmd_configure);
 
 int wl1271_cmd_data_path(struct wl1271 *wl, bool enable)
 {
@@ -1249,7 +1036,7 @@ int wl1271_cmd_build_arp_rsp(struct wl1271 *wl, struct wl12xx_vif *wlvif)
 	skb_reserve(skb, sizeof(*hdr) + WL1271_EXTRA_SPACE_MAX);
 
 	tmpl = (struct wl12xx_arp_rsp_template *)skb_put(skb, sizeof(*tmpl));
-	memset(tmpl, 0, sizeof(tmpl));
+	memset(tmpl, 0, sizeof(*tmpl));
 
 	/* llc layer */
 	memcpy(tmpl->llc_hdr, rfc1042_header, sizeof(rfc1042_header));
@@ -1298,7 +1085,7 @@ int wl1271_cmd_build_arp_rsp(struct wl1271 *wl, struct wl12xx_vif *wlvif)
 
 	/* mac80211 header */
 	hdr = (struct ieee80211_hdr_3addr *)skb_push(skb, sizeof(*hdr));
-	memset(hdr, 0, sizeof(hdr));
+	memset(hdr, 0, sizeof(*hdr));
 	fc = IEEE80211_FTYPE_DATA | IEEE80211_FCTL_TODS;
 	if (wlvif->sta.qos)
 		fc |= IEEE80211_STYPE_QOS_DATA;
@@ -1730,10 +1517,10 @@ static int wl12xx_cmd_roc(struct wl1271 *wl, struct wl12xx_vif *wlvif,
 	cmd->channel = wlvif->channel;
 	switch (wlvif->band) {
 	case IEEE80211_BAND_2GHZ:
-		cmd->band = RADIO_BAND_2_4GHZ;
+		cmd->band = WLCORE_BAND_2_4GHZ;
 		break;
 	case IEEE80211_BAND_5GHZ:
-		cmd->band = RADIO_BAND_5GHZ;
+		cmd->band = WLCORE_BAND_5GHZ;
 		break;
 	default:
 		wl1271_error("roc - unknown band: %d", (int)wlvif->band);
diff --git a/drivers/net/wireless/wl12xx/cmd.h b/drivers/net/wireless/ti/wlcore/cmd.h
index de217d92516..a46ae07cb77 100644
--- a/drivers/net/wireless/wl12xx/cmd.h
+++ b/drivers/net/wireless/ti/wlcore/cmd.h
@@ -25,17 +25,12 @@
 #ifndef __CMD_H__
 #define __CMD_H__
 
-#include "wl12xx.h"
+#include "wlcore.h"
 
 struct acx_header;
 
 int wl1271_cmd_send(struct wl1271 *wl, u16 id, void *buf, size_t len,
 		    size_t res_len);
-int wl1271_cmd_general_parms(struct wl1271 *wl);
-int wl128x_cmd_general_parms(struct wl1271 *wl);
-int wl1271_cmd_radio_parms(struct wl1271 *wl);
-int wl128x_cmd_radio_parms(struct wl1271 *wl);
-int wl1271_cmd_ext_radio_parms(struct wl1271 *wl);
 int wl12xx_cmd_role_enable(struct wl1271 *wl, u8 *addr, u8 role_type,
 			   u8 *role_id);
 int wl12xx_cmd_role_disable(struct wl1271 *wl, u8 *role_id);
@@ -262,13 +257,13 @@ struct wl12xx_cmd_role_disable {
 	u8 padding[3];
 } __packed;
 
-enum wl12xx_band {
-	WL12XX_BAND_2_4GHZ		= 0,
-	WL12XX_BAND_5GHZ		= 1,
-	WL12XX_BAND_JAPAN_4_9_GHZ	= 2,
-	WL12XX_BAND_DEFAULT		= WL12XX_BAND_2_4GHZ,
-	WL12XX_BAND_INVALID		= 0x7E,
-	WL12XX_BAND_MAX_RADIO		= 0x7F,
+enum wlcore_band {
+	WLCORE_BAND_2_4GHZ		= 0,
+	WLCORE_BAND_5GHZ		= 1,
+	WLCORE_BAND_JAPAN_4_9_GHZ	= 2,
+	WLCORE_BAND_DEFAULT		= WLCORE_BAND_2_4GHZ,
+	WLCORE_BAND_INVALID		= 0x7E,
+	WLCORE_BAND_MAX_RADIO		= 0x7F,
 };
 
 struct wl12xx_cmd_role_start {
@@ -494,83 +489,6 @@ enum wl1271_channel_tune_bands {
 
 #define WL1271_PD_REFERENCE_POINT_BAND_B_G  0
 
-#define TEST_CMD_INI_FILE_RADIO_PARAM       0x19
-#define TEST_CMD_INI_FILE_GENERAL_PARAM     0x1E
-#define TEST_CMD_INI_FILE_RF_EXTENDED_PARAM 0x26
-
-struct wl1271_general_parms_cmd {
-	struct wl1271_cmd_header header;
-
-	struct wl1271_cmd_test_header test;
-
-	struct wl1271_ini_general_params general_params;
-
-	u8 sr_debug_table[WL1271_INI_MAX_SMART_REFLEX_PARAM];
-	u8 sr_sen_n_p;
-	u8 sr_sen_n_p_gain;
-	u8 sr_sen_nrn;
-	u8 sr_sen_prn;
-	u8 padding[3];
-} __packed;
-
-struct wl128x_general_parms_cmd {
-	struct wl1271_cmd_header header;
-
-	struct wl1271_cmd_test_header test;
-
-	struct wl128x_ini_general_params general_params;
-
-	u8 sr_debug_table[WL1271_INI_MAX_SMART_REFLEX_PARAM];
-	u8 sr_sen_n_p;
-	u8 sr_sen_n_p_gain;
-	u8 sr_sen_nrn;
-	u8 sr_sen_prn;
-	u8 padding[3];
-} __packed;
-
-struct wl1271_radio_parms_cmd {
-	struct wl1271_cmd_header header;
-
-	struct wl1271_cmd_test_header test;
-
-	/* Static radio parameters */
-	struct wl1271_ini_band_params_2 static_params_2;
-	struct wl1271_ini_band_params_5 static_params_5;
-
-	/* Dynamic radio parameters */
-	struct wl1271_ini_fem_params_2 dyn_params_2;
-	u8 padding2;
-	struct wl1271_ini_fem_params_5 dyn_params_5;
-	u8 padding3[2];
-} __packed;
-
-struct wl128x_radio_parms_cmd {
-	struct wl1271_cmd_header header;
-
-	struct wl1271_cmd_test_header test;
-
-	/* Static radio parameters */
-	struct wl128x_ini_band_params_2 static_params_2;
-	struct wl128x_ini_band_params_5 static_params_5;
-
-	u8 fem_vendor_and_options;
-
-	/* Dynamic radio parameters */
-	struct wl128x_ini_fem_params_2 dyn_params_2;
-	u8 padding2;
-	struct wl128x_ini_fem_params_5 dyn_params_5;
-} __packed;
-
-struct wl1271_ext_radio_parms_cmd {
-	struct wl1271_cmd_header header;
-
-	struct wl1271_cmd_test_header test;
-
-	u8 tx_per_channel_power_compensation_2[CONF_TX_PWR_COMPENSATION_LEN_2];
-	u8 tx_per_channel_power_compensation_5[CONF_TX_PWR_COMPENSATION_LEN_5];
-	u8 padding[3];
-} __packed;
-
 /*
  * There are three types of disconnections:
  *
diff --git a/drivers/net/wireless/wl12xx/conf.h b/drivers/net/wireless/ti/wlcore/conf.h
index 3e581e19424..fef0db4213b 100644
--- a/drivers/net/wireless/wl12xx/conf.h
+++ b/drivers/net/wireless/ti/wlcore/conf.h
@@ -65,36 +65,7 @@ enum {
 	CONF_HW_RATE_INDEX_MAX     = CONF_HW_RATE_INDEX_54MBPS,
 };
 
-enum {
-	CONF_HW_RXTX_RATE_MCS7_SGI = 0,
-	CONF_HW_RXTX_RATE_MCS7,
-	CONF_HW_RXTX_RATE_MCS6,
-	CONF_HW_RXTX_RATE_MCS5,
-	CONF_HW_RXTX_RATE_MCS4,
-	CONF_HW_RXTX_RATE_MCS3,
-	CONF_HW_RXTX_RATE_MCS2,
-	CONF_HW_RXTX_RATE_MCS1,
-	CONF_HW_RXTX_RATE_MCS0,
-	CONF_HW_RXTX_RATE_54,
-	CONF_HW_RXTX_RATE_48,
-	CONF_HW_RXTX_RATE_36,
-	CONF_HW_RXTX_RATE_24,
-	CONF_HW_RXTX_RATE_22,
-	CONF_HW_RXTX_RATE_18,
-	CONF_HW_RXTX_RATE_12,
-	CONF_HW_RXTX_RATE_11,
-	CONF_HW_RXTX_RATE_9,
-	CONF_HW_RXTX_RATE_6,
-	CONF_HW_RXTX_RATE_5_5,
-	CONF_HW_RXTX_RATE_2,
-	CONF_HW_RXTX_RATE_1,
-	CONF_HW_RXTX_RATE_MAX,
-	CONF_HW_RXTX_RATE_UNSUPPORTED = 0xff
-};
-
-/* Rates between and including these are MCS rates */
-#define CONF_HW_RXTX_RATE_MCS_MIN CONF_HW_RXTX_RATE_MCS7_SGI
-#define CONF_HW_RXTX_RATE_MCS_MAX CONF_HW_RXTX_RATE_MCS0
+#define CONF_HW_RXTX_RATE_UNSUPPORTED 0xff
 
 enum {
 	CONF_SG_DISABLE = 0,
@@ -1096,16 +1067,31 @@ struct conf_scan_settings {
 };
 
 struct conf_sched_scan_settings {
-	/* minimum time to wait on the channel for active scans (in TUs) */
-	u16 min_dwell_time_active;
+	/*
+	 * The base time to wait on the channel for active scans (in TU/1000).
+	 * The minimum dwell time is calculated according to this:
+	 * min_dwell_time = base + num_of_probes_to_be_sent * delta_per_probe
+	 * The maximum dwell time is calculated according to this:
+	 * max_dwell_time = min_dwell_time + max_dwell_time_delta
+	 */
+	u32 base_dwell_time;
 
-	/* maximum time to wait on the channel for active scans (in TUs) */
-	u16 max_dwell_time_active;
+	/* The delta between the min dwell time and max dwell time for
+	 * active scans (in TU/1000s). The max dwell time is used by the FW once
+	 * traffic is detected on the channel.
+	 */
+	u32 max_dwell_time_delta;
+
+	/* Delta added to min dwell time per each probe in 2.4 GHz (TU/1000) */
+	u32 dwell_time_delta_per_probe;
 
-	/* time to wait on the channel for passive scans (in TUs) */
+	/* Delta added to min dwell time per each probe in 5 GHz (TU/1000) */
+	u32 dwell_time_delta_per_probe_5;
+
+	/* time to wait on the channel for passive scans (in TU/1000) */
 	u32 dwell_time_passive;
 
-	/* time to wait on the channel for DFS scans (in TUs) */
+	/* time to wait on the channel for DFS scans (in TU/1000) */
 	u32 dwell_time_dfs;
 
 	/* number of probe requests to send on each channel in active scans */
@@ -1118,26 +1104,6 @@ struct conf_sched_scan_settings {
 	s8 snr_threshold;
 };
 
-/* these are number of channels on the band divided by two, rounded up */
-#define CONF_TX_PWR_COMPENSATION_LEN_2 7
-#define CONF_TX_PWR_COMPENSATION_LEN_5 18
-
-struct conf_rf_settings {
-	/*
-	 * Per channel power compensation for 2.4GHz
-	 *
-	 * Range: s8
-	 */
-	u8 tx_per_channel_power_compensation_2[CONF_TX_PWR_COMPENSATION_LEN_2];
-
-	/*
-	 * Per channel power compensation for 5GHz
-	 *
-	 * Range: s8
-	 */
-	u8 tx_per_channel_power_compensation_5[CONF_TX_PWR_COMPENSATION_LEN_5];
-};
-
 struct conf_ht_setting {
 	u8 rx_ba_win_size;
 	u8 tx_ba_win_size;
@@ -1286,7 +1252,7 @@ struct conf_hangover_settings {
 	u8 window_size;
 };
 
-struct conf_drv_settings {
+struct wlcore_conf {
 	struct conf_sg_settings sg;
 	struct conf_rx_settings rx;
 	struct conf_tx_settings tx;
@@ -1296,16 +1262,13 @@ struct conf_drv_settings {
 	struct conf_roam_trigger_settings roam_trigger;
 	struct conf_scan_settings scan;
 	struct conf_sched_scan_settings sched_scan;
-	struct conf_rf_settings rf;
 	struct conf_ht_setting ht;
-	struct conf_memory_settings mem_wl127x;
-	struct conf_memory_settings mem_wl128x;
+	struct conf_memory_settings mem;
 	struct conf_fm_coex fm_coex;
 	struct conf_rx_streaming_settings rx_streaming;
 	struct conf_fwlog fwlog;
 	struct conf_rate_policy_settings rate;
 	struct conf_hangover_settings hangover;
-	u8 hci_io_ds;
 };
 
 #endif
diff --git a/drivers/net/wireless/wl12xx/debug.h b/drivers/net/wireless/ti/wlcore/debug.h
index ec0fdc25b28..6b800b3cbea 100644
--- a/drivers/net/wireless/wl12xx/debug.h
+++ b/drivers/net/wireless/ti/wlcore/debug.h
@@ -52,6 +52,7 @@ enum {
 	DEBUG_ADHOC     = BIT(16),
 	DEBUG_AP	= BIT(17),
 	DEBUG_PROBE	= BIT(18),
+	DEBUG_IO	= BIT(19),
 	DEBUG_MASTER	= (DEBUG_ADHOC | DEBUG_AP),
 	DEBUG_ALL	= ~0,
 };
diff --git a/drivers/net/wireless/wl12xx/debugfs.c b/drivers/net/wireless/ti/wlcore/debugfs.c
index 564d49575c9..d5aea1ff5ad 100644
--- a/drivers/net/wireless/wl12xx/debugfs.c
+++ b/drivers/net/wireless/ti/wlcore/debugfs.c
@@ -26,7 +26,7 @@
 #include <linux/skbuff.h>
 #include <linux/slab.h>
 
-#include "wl12xx.h"
+#include "wlcore.h"
 #include "debug.h"
 #include "acx.h"
 #include "ps.h"
@@ -647,6 +647,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_INT(is_gem);
 		VIF_STATE_PRINT_LLHEX(tx_security_seq);
 		VIF_STATE_PRINT_INT(tx_security_last_seq_lsb);
 	}
diff --git a/drivers/net/wireless/wl12xx/debugfs.h b/drivers/net/wireless/ti/wlcore/debugfs.h
index 254c5b292cf..a8d3aef011f 100644
--- a/drivers/net/wireless/wl12xx/debugfs.h
+++ b/drivers/net/wireless/ti/wlcore/debugfs.h
@@ -24,7 +24,7 @@
 #ifndef __DEBUGFS_H__
 #define __DEBUGFS_H__
 
-#include "wl12xx.h"
+#include "wlcore.h"
 
 int wl1271_debugfs_init(struct wl1271 *wl);
 void wl1271_debugfs_exit(struct wl1271 *wl);
diff --git a/drivers/net/wireless/wl12xx/event.c b/drivers/net/wireless/ti/wlcore/event.c
index c953717f38e..28e2a633c3b 100644
--- a/drivers/net/wireless/wl12xx/event.c
+++ b/drivers/net/wireless/ti/wlcore/event.c
@@ -21,9 +21,8 @@
  *
  */
 
-#include "wl12xx.h"
+#include "wlcore.h"
 #include "debug.h"
-#include "reg.h"
 #include "io.h"
 #include "event.h"
 #include "ps.h"
@@ -98,12 +97,12 @@ static void wl1271_event_mbox_dump(struct event_mailbox *mbox)
 	wl1271_debug(DEBUG_EVENT, "\tmask: 0x%x", mbox->events_mask);
 }
 
-static int wl1271_event_process(struct wl1271 *wl, struct event_mailbox *mbox)
+static int wl1271_event_process(struct wl1271 *wl)
 {
+	struct event_mailbox *mbox = wl->mbox;
 	struct ieee80211_vif *vif;
 	struct wl12xx_vif *wlvif;
 	u32 vector;
-	bool beacon_loss = false;
 	bool disconnect_sta = false;
 	unsigned long sta_bitmap = 0;
 
@@ -141,20 +140,23 @@ static int wl1271_event_process(struct wl1271 *wl, struct event_mailbox *mbox)
 					       mbox->soft_gemini_sense_info);
 
 	/*
-	 * The BSS_LOSE_EVENT_ID is only needed while psm (and hence beacon
-	 * filtering) is enabled. Without PSM, the stack will receive all
-	 * beacons and can detect beacon loss by itself.
-	 *
-	 * As there's possibility that the driver disables PSM before receiving
-	 * BSS_LOSE_EVENT, beacon loss has to be reported to the stack.
-	 *
+	 * We are HW_MONITOR device. On beacon loss - queue
+	 * connection loss work. Cancel it on REGAINED event.
 	 */
 	if (vector & BSS_LOSE_EVENT_ID) {
 		/* TODO: check for multi-role */
+		int delay = wl->conf.conn.synch_fail_thold *
+					wl->conf.conn.bss_lose_timeout;
 		wl1271_info("Beacon loss detected.");
+		cancel_delayed_work_sync(&wl->connection_loss_work);
+		ieee80211_queue_delayed_work(wl->hw, &wl->connection_loss_work,
+		      msecs_to_jiffies(delay));
+	}
 
-		/* indicate to the stack, that beacons have been lost */
-		beacon_loss = true;
+	if (vector & REGAINED_BSS_EVENT_ID) {
+		/* TODO: check for multi-role */
+		wl1271_info("Beacon regained.");
+		cancel_delayed_work_sync(&wl->connection_loss_work);
 	}
 
 	if (vector & RSSI_SNR_TRIGGER_0_EVENT_ID) {
@@ -196,7 +198,7 @@ static int wl1271_event_process(struct wl1271 *wl, struct event_mailbox *mbox)
 			bool success;
 
 			if (!test_and_clear_bit(WLVIF_FLAG_CS_PROGRESS,
-						&wl->flags))
+						&wlvif->flags))
 				continue;
 
 			success = mbox->channel_switch_status ? false : true;
@@ -257,13 +259,6 @@ static int wl1271_event_process(struct wl1271 *wl, struct event_mailbox *mbox)
 			rcu_read_unlock();
 		}
 	}
-
-	if (beacon_loss)
-		wl12xx_for_each_wlvif_sta(wl, wlvif) {
-			vif = wl12xx_wlvif_to_vif(wlvif);
-			ieee80211_connection_loss(vif);
-		}
-
 	return 0;
 }
 
@@ -278,18 +273,8 @@ int wl1271_event_unmask(struct wl1271 *wl)
 	return 0;
 }
 
-void wl1271_event_mbox_config(struct wl1271 *wl)
-{
-	wl->mbox_ptr[0] = wl1271_read32(wl, REG_EVENT_MAILBOX_PTR);
-	wl->mbox_ptr[1] = wl->mbox_ptr[0] + sizeof(struct event_mailbox);
-
-	wl1271_debug(DEBUG_EVENT, "MBOX ptrs: 0x%x 0x%x",
-		     wl->mbox_ptr[0], wl->mbox_ptr[1]);
-}
-
 int wl1271_event_handle(struct wl1271 *wl, u8 mbox_num)
 {
-	struct event_mailbox mbox;
 	int ret;
 
 	wl1271_debug(DEBUG_EVENT, "EVENT on mbox %d", mbox_num);
@@ -298,16 +283,19 @@ int wl1271_event_handle(struct wl1271 *wl, u8 mbox_num)
 		return -EINVAL;
 
 	/* first we read the mbox descriptor */
-	wl1271_read(wl, wl->mbox_ptr[mbox_num], &mbox,
-		    sizeof(struct event_mailbox), false);
+	wl1271_read(wl, wl->mbox_ptr[mbox_num], wl->mbox,
+		    sizeof(*wl->mbox), false);
 
 	/* process the descriptor */
-	ret = wl1271_event_process(wl, &mbox);
+	ret = wl1271_event_process(wl);
 	if (ret < 0)
 		return ret;
 
-	/* then we let the firmware know it can go on...*/
-	wl1271_write32(wl, ACX_REG_INTERRUPT_TRIG, INTR_TRIG_EVENT_ACK);
+	/*
+	 * TODO: we just need this because one bit is in a different
+	 * place.  Is there any better way?
+	 */
+	wl->ops->ack_event(wl);
 
 	return 0;
 }
diff --git a/drivers/net/wireless/wl12xx/event.h b/drivers/net/wireless/ti/wlcore/event.h
index 057d193d352..8adf18d6c58 100644
--- a/drivers/net/wireless/wl12xx/event.h
+++ b/drivers/net/wireless/ti/wlcore/event.h
@@ -132,8 +132,9 @@ struct event_mailbox {
 	u8 reserved_8[9];
 } __packed;
 
+struct wl1271;
+
 int wl1271_event_unmask(struct wl1271 *wl);
-void wl1271_event_mbox_config(struct wl1271 *wl);
 int wl1271_event_handle(struct wl1271 *wl, u8 mbox);
 
 #endif
diff --git a/drivers/net/wireless/ti/wlcore/hw_ops.h b/drivers/net/wireless/ti/wlcore/hw_ops.h
new file mode 100644
index 00000000000..9384b4d56c2
--- /dev/null
+++ b/drivers/net/wireless/ti/wlcore/hw_ops.h
@@ -0,0 +1,122 @@
+/*
+ * This file is part of wlcore
+ *
+ * Copyright (C) 2011 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.
+ *
+ * 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
+ *
+ */
+
+#ifndef __WLCORE_HW_OPS_H__
+#define __WLCORE_HW_OPS_H__
+
+#include "wlcore.h"
+#include "rx.h"
+
+static inline u32
+wlcore_hw_calc_tx_blocks(struct wl1271 *wl, u32 len, u32 spare_blks)
+{
+	if (!wl->ops->calc_tx_blocks)
+		BUG_ON(1);
+
+	return wl->ops->calc_tx_blocks(wl, len, spare_blks);
+}
+
+static inline void
+wlcore_hw_set_tx_desc_blocks(struct wl1271 *wl, struct wl1271_tx_hw_descr *desc,
+			     u32 blks, u32 spare_blks)
+{
+	if (!wl->ops->set_tx_desc_blocks)
+		BUG_ON(1);
+
+	return wl->ops->set_tx_desc_blocks(wl, desc, blks, spare_blks);
+}
+
+static inline void
+wlcore_hw_set_tx_desc_data_len(struct wl1271 *wl,
+			       struct wl1271_tx_hw_descr *desc,
+			       struct sk_buff *skb)
+{
+	if (!wl->ops->set_tx_desc_data_len)
+		BUG_ON(1);
+
+	wl->ops->set_tx_desc_data_len(wl, desc, skb);
+}
+
+static inline enum wl_rx_buf_align
+wlcore_hw_get_rx_buf_align(struct wl1271 *wl, u32 rx_desc)
+{
+
+	if (!wl->ops->get_rx_buf_align)
+		BUG_ON(1);
+
+	return wl->ops->get_rx_buf_align(wl, rx_desc);
+}
+
+static inline void
+wlcore_hw_prepare_read(struct wl1271 *wl, u32 rx_desc, u32 len)
+{
+	if (wl->ops->prepare_read)
+		wl->ops->prepare_read(wl, rx_desc, len);
+}
+
+static inline u32
+wlcore_hw_get_rx_packet_len(struct wl1271 *wl, void *rx_data, u32 data_len)
+{
+	if (!wl->ops->get_rx_packet_len)
+		BUG_ON(1);
+
+	return wl->ops->get_rx_packet_len(wl, rx_data, data_len);
+}
+
+static inline void wlcore_hw_tx_delayed_compl(struct wl1271 *wl)
+{
+	if (wl->ops->tx_delayed_compl)
+		wl->ops->tx_delayed_compl(wl);
+}
+
+static inline void wlcore_hw_tx_immediate_compl(struct wl1271 *wl)
+{
+	if (wl->ops->tx_immediate_compl)
+		wl->ops->tx_immediate_compl(wl);
+}
+
+static inline int
+wlcore_hw_init_vif(struct wl1271 *wl, struct wl12xx_vif *wlvif)
+{
+	if (wl->ops->init_vif)
+		return wl->ops->init_vif(wl, wlvif);
+
+	return 0;
+}
+
+static inline u32
+wlcore_hw_sta_get_ap_rate_mask(struct wl1271 *wl, struct wl12xx_vif *wlvif)
+{
+	if (!wl->ops->sta_get_ap_rate_mask)
+		BUG_ON(1);
+
+	return wl->ops->sta_get_ap_rate_mask(wl, wlvif);
+}
+
+static inline int wlcore_identify_fw(struct wl1271 *wl)
+{
+	if (wl->ops->identify_fw)
+		return wl->ops->identify_fw(wl);
+
+	return 0;
+}
+
+#endif
diff --git a/drivers/net/wireless/wl12xx/ini.h b/drivers/net/wireless/ti/wlcore/ini.h
index 4cf9ecc5621..4cf9ecc5621 100644
--- a/drivers/net/wireless/wl12xx/ini.h
+++ b/drivers/net/wireless/ti/wlcore/ini.h
diff --git a/drivers/net/wireless/wl12xx/init.c b/drivers/net/wireless/ti/wlcore/init.c
index 203fbebf09e..9f89255eb6e 100644
--- a/drivers/net/wireless/wl12xx/init.c
+++ b/drivers/net/wireless/ti/wlcore/init.c
@@ -30,9 +30,9 @@
 #include "wl12xx_80211.h"
 #include "acx.h"
 #include "cmd.h"
-#include "reg.h"
 #include "tx.h"
 #include "io.h"
+#include "hw_ops.h"
 
 int wl1271_init_templates_config(struct wl1271 *wl)
 {
@@ -319,7 +319,7 @@ static int wl12xx_init_fwlog(struct wl1271 *wl)
 {
 	int ret;
 
-	if (wl->quirks & WL12XX_QUIRK_FWLOG_NOT_IMPLEMENTED)
+	if (wl->quirks & WLCORE_QUIRK_FWLOG_NOT_IMPLEMENTED)
 		return 0;
 
 	ret = wl12xx_cmd_config_fwlog(wl);
@@ -494,26 +494,6 @@ static int wl1271_set_ba_policies(struct wl1271 *wl, struct wl12xx_vif *wlvif)
 	return wl12xx_acx_set_ba_initiator_policy(wl, wlvif);
 }
 
-int wl1271_chip_specific_init(struct wl1271 *wl)
-{
-	int ret = 0;
-
-	if (wl->chip.id == CHIP_ID_1283_PG20) {
-		u32 host_cfg_bitmap = HOST_IF_CFG_RX_FIFO_ENABLE;
-
-		if (!(wl->quirks & WL12XX_QUIRK_NO_BLOCKSIZE_ALIGNMENT))
-			/* Enable SDIO padding */
-			host_cfg_bitmap |= HOST_IF_CFG_TX_PAD_TO_SDIO_BLK;
-
-		/* Must be before wl1271_acx_init_mem_config() */
-		ret = wl1271_acx_host_if_cfg_bitmap(wl, host_cfg_bitmap);
-		if (ret < 0)
-			goto out;
-	}
-out:
-	return ret;
-}
-
 /* vif-specifc initialization */
 static int wl12xx_init_sta_role(struct wl1271 *wl, struct wl12xx_vif *wlvif)
 {
@@ -582,10 +562,17 @@ int wl1271_init_vif_specific(struct wl1271 *wl, struct ieee80211_vif *vif)
 			if (ret < 0)
 				return ret;
 		} else if (!wl->sta_count) {
-			/* Configure for ELP power saving */
-			ret = wl1271_acx_sleep_auth(wl, WL1271_PSM_ELP);
-			if (ret < 0)
-				return ret;
+			if (wl->quirks & WLCORE_QUIRK_NO_ELP) {
+				/* Configure for power always on */
+				ret = wl1271_acx_sleep_auth(wl, WL1271_PSM_CAM);
+				if (ret < 0)
+					return ret;
+			} else {
+				/* Configure for ELP power saving */
+				ret = wl1271_acx_sleep_auth(wl, WL1271_PSM_ELP);
+				if (ret < 0)
+					return ret;
+			}
 		}
 	}
 
@@ -652,6 +639,10 @@ int wl1271_init_vif_specific(struct wl1271 *wl, struct ieee80211_vif *vif)
 	if (ret < 0)
 		return ret;
 
+	ret = wlcore_hw_init_vif(wl, wlvif);
+	if (ret < 0)
+		return ret;
+
 	return 0;
 }
 
@@ -659,27 +650,8 @@ int wl1271_hw_init(struct wl1271 *wl)
 {
 	int ret;
 
-	if (wl->chip.id == CHIP_ID_1283_PG20) {
-		ret = wl128x_cmd_general_parms(wl);
-		if (ret < 0)
-			return ret;
-		ret = wl128x_cmd_radio_parms(wl);
-		if (ret < 0)
-			return ret;
-	} else {
-		ret = wl1271_cmd_general_parms(wl);
-		if (ret < 0)
-			return ret;
-		ret = wl1271_cmd_radio_parms(wl);
-		if (ret < 0)
-			return ret;
-		ret = wl1271_cmd_ext_radio_parms(wl);
-		if (ret < 0)
-			return ret;
-	}
-
-	/* Chip-specific init */
-	ret = wl1271_chip_specific_init(wl);
+	/* Chip-specific hw init */
+	ret = wl->ops->hw_init(wl);
 	if (ret < 0)
 		return ret;
 
diff --git a/drivers/net/wireless/wl12xx/init.h b/drivers/net/wireless/ti/wlcore/init.h
index 2da0f404ef6..a45fbfddec1 100644
--- a/drivers/net/wireless/wl12xx/init.h
+++ b/drivers/net/wireless/ti/wlcore/init.h
@@ -24,7 +24,7 @@
 #ifndef __INIT_H__
 #define __INIT_H__
 
-#include "wl12xx.h"
+#include "wlcore.h"
 
 int wl1271_hw_init_power_auth(struct wl1271 *wl);
 int wl1271_init_templates_config(struct wl1271 *wl);
diff --git a/drivers/net/wireless/wl12xx/io.c b/drivers/net/wireless/ti/wlcore/io.c
index c574a3b31e3..7cd0081aede 100644
--- a/drivers/net/wireless/wl12xx/io.c
+++ b/drivers/net/wireless/ti/wlcore/io.c
@@ -26,84 +26,12 @@
 #include <linux/spi/spi.h>
 #include <linux/interrupt.h>
 
-#include "wl12xx.h"
+#include "wlcore.h"
 #include "debug.h"
 #include "wl12xx_80211.h"
 #include "io.h"
 #include "tx.h"
 
-#define OCP_CMD_LOOP  32
-
-#define OCP_CMD_WRITE 0x1
-#define OCP_CMD_READ  0x2
-
-#define OCP_READY_MASK  BIT(18)
-#define OCP_STATUS_MASK (BIT(16) | BIT(17))
-
-#define OCP_STATUS_NO_RESP    0x00000
-#define OCP_STATUS_OK         0x10000
-#define OCP_STATUS_REQ_FAILED 0x20000
-#define OCP_STATUS_RESP_ERROR 0x30000
-
-struct wl1271_partition_set wl12xx_part_table[PART_TABLE_LEN] = {
-	[PART_DOWN] = {
-		.mem = {
-			.start = 0x00000000,
-			.size  = 0x000177c0
-		},
-		.reg = {
-			.start = REGISTERS_BASE,
-			.size  = 0x00008800
-		},
-		.mem2 = {
-			.start = 0x00000000,
-			.size  = 0x00000000
-		},
-		.mem3 = {
-			.start = 0x00000000,
-			.size  = 0x00000000
-		},
-	},
-
-	[PART_WORK] = {
-		.mem = {
-			.start = 0x00040000,
-			.size  = 0x00014fc0
-		},
-		.reg = {
-			.start = REGISTERS_BASE,
-			.size  = 0x0000a000
-		},
-		.mem2 = {
-			.start = 0x003004f8,
-			.size  = 0x00000004
-		},
-		.mem3 = {
-			.start = 0x00040404,
-			.size  = 0x00000000
-		},
-	},
-
-	[PART_DRPW] = {
-		.mem = {
-			.start = 0x00040000,
-			.size  = 0x00014fc0
-		},
-		.reg = {
-			.start = DRPW_BASE,
-			.size  = 0x00006000
-		},
-		.mem2 = {
-			.start = 0x00000000,
-			.size  = 0x00000000
-		},
-		.mem3 = {
-			.start = 0x00000000,
-			.size  = 0x00000000
-		}
-	}
-};
-
 bool wl1271_set_block_size(struct wl1271 *wl)
 {
 	if (wl->if_ops->set_block_size) {
@@ -114,17 +42,53 @@ bool wl1271_set_block_size(struct wl1271 *wl)
 	return false;
 }
 
-void wl1271_disable_interrupts(struct wl1271 *wl)
+void wlcore_disable_interrupts(struct wl1271 *wl)
 {
 	disable_irq(wl->irq);
 }
+EXPORT_SYMBOL_GPL(wlcore_disable_interrupts);
 
-void wl1271_enable_interrupts(struct wl1271 *wl)
+void wlcore_enable_interrupts(struct wl1271 *wl)
 {
 	enable_irq(wl->irq);
 }
+EXPORT_SYMBOL_GPL(wlcore_enable_interrupts);
 
-/* Set the SPI partitions to access the chip addresses
+int wlcore_translate_addr(struct wl1271 *wl, int addr)
+{
+	struct wlcore_partition_set *part = &wl->curr_part;
+
+	/*
+	 * To translate, first check to which window of addresses the
+	 * particular address belongs. Then subtract the starting address
+	 * of that window from the address. Then, add offset of the
+	 * translated region.
+	 *
+	 * The translated regions occur next to each other in physical device
+	 * memory, so just add the sizes of the preceding address regions to
+	 * get the offset to the new region.
+	 */
+	if ((addr >= part->mem.start) &&
+	    (addr < part->mem.start + part->mem.size))
+		return addr - part->mem.start;
+	else if ((addr >= part->reg.start) &&
+		 (addr < part->reg.start + part->reg.size))
+		return addr - part->reg.start + part->mem.size;
+	else if ((addr >= part->mem2.start) &&
+		 (addr < part->mem2.start + part->mem2.size))
+		return addr - part->mem2.start + part->mem.size +
+			part->reg.size;
+	else if ((addr >= part->mem3.start) &&
+		 (addr < part->mem3.start + part->mem3.size))
+		return addr - part->mem3.start + part->mem.size +
+			part->reg.size + part->mem2.size;
+
+	WARN(1, "HW address 0x%x out of range", addr);
+	return 0;
+}
+EXPORT_SYMBOL_GPL(wlcore_translate_addr);
+
+/* Set the partitions to access the chip addresses
  *
  * To simplify driver code, a fixed (virtual) memory map is defined for
  * register and memory addresses. Because in the chipset, in different stages
@@ -158,33 +122,43 @@ void wl1271_enable_interrupts(struct wl1271 *wl)
  *                                    |    |
  *
  */
-int wl1271_set_partition(struct wl1271 *wl,
-			 struct wl1271_partition_set *p)
+void wlcore_set_partition(struct wl1271 *wl,
+			  const struct wlcore_partition_set *p)
 {
 	/* copy partition info */
-	memcpy(&wl->part, p, sizeof(*p));
+	memcpy(&wl->curr_part, p, sizeof(*p));
 
-	wl1271_debug(DEBUG_SPI, "mem_start %08X mem_size %08X",
+	wl1271_debug(DEBUG_IO, "mem_start %08X mem_size %08X",
 		     p->mem.start, p->mem.size);
-	wl1271_debug(DEBUG_SPI, "reg_start %08X reg_size %08X",
+	wl1271_debug(DEBUG_IO, "reg_start %08X reg_size %08X",
 		     p->reg.start, p->reg.size);
-	wl1271_debug(DEBUG_SPI, "mem2_start %08X mem2_size %08X",
+	wl1271_debug(DEBUG_IO, "mem2_start %08X mem2_size %08X",
 		     p->mem2.start, p->mem2.size);
-	wl1271_debug(DEBUG_SPI, "mem3_start %08X mem3_size %08X",
+	wl1271_debug(DEBUG_IO, "mem3_start %08X mem3_size %08X",
 		     p->mem3.start, p->mem3.size);
 
-	/* write partition info to the chipset */
 	wl1271_raw_write32(wl, HW_PART0_START_ADDR, p->mem.start);
 	wl1271_raw_write32(wl, HW_PART0_SIZE_ADDR, p->mem.size);
 	wl1271_raw_write32(wl, HW_PART1_START_ADDR, p->reg.start);
 	wl1271_raw_write32(wl, HW_PART1_SIZE_ADDR, p->reg.size);
 	wl1271_raw_write32(wl, HW_PART2_START_ADDR, p->mem2.start);
 	wl1271_raw_write32(wl, HW_PART2_SIZE_ADDR, p->mem2.size);
+	/*
+	 * We don't need the size of the last partition, as it is
+	 * automatically calculated based on the total memory size and
+	 * the sizes of the previous partitions.
+	 */
 	wl1271_raw_write32(wl, HW_PART3_START_ADDR, p->mem3.start);
+}
+EXPORT_SYMBOL_GPL(wlcore_set_partition);
 
-	return 0;
+void wlcore_select_partition(struct wl1271 *wl, u8 part)
+{
+	wl1271_debug(DEBUG_IO, "setting partition %d", part);
+
+	wlcore_set_partition(wl, &wl->ptable[part]);
 }
-EXPORT_SYMBOL_GPL(wl1271_set_partition);
+EXPORT_SYMBOL_GPL(wlcore_select_partition);
 
 void wl1271_io_reset(struct wl1271 *wl)
 {
@@ -197,48 +171,3 @@ void wl1271_io_init(struct wl1271 *wl)
 	if (wl->if_ops->init)
 		wl->if_ops->init(wl->dev);
 }
-
-void wl1271_top_reg_write(struct wl1271 *wl, int addr, u16 val)
-{
-	/* write address >> 1 + 0x30000 to OCP_POR_CTR */
-	addr = (addr >> 1) + 0x30000;
-	wl1271_write32(wl, OCP_POR_CTR, addr);
-
-	/* write value to OCP_POR_WDATA */
-	wl1271_write32(wl, OCP_DATA_WRITE, val);
-
-	/* write 1 to OCP_CMD */
-	wl1271_write32(wl, OCP_CMD, OCP_CMD_WRITE);
-}
-
-u16 wl1271_top_reg_read(struct wl1271 *wl, int addr)
-{
-	u32 val;
-	int timeout = OCP_CMD_LOOP;
-
-	/* write address >> 1 + 0x30000 to OCP_POR_CTR */
-	addr = (addr >> 1) + 0x30000;
-	wl1271_write32(wl, OCP_POR_CTR, addr);
-
-	/* write 2 to OCP_CMD */
-	wl1271_write32(wl, OCP_CMD, OCP_CMD_READ);
-
-	/* poll for data ready */
-	do {
-		val = wl1271_read32(wl, OCP_DATA_READ);
-	} while (!(val & OCP_READY_MASK) && --timeout);
-
-	if (!timeout) {
-		wl1271_warning("Top register access timed out.");
-		return 0xffff;
-	}
-
-	/* check data status and return if OK */
-	if ((val & OCP_STATUS_MASK) == OCP_STATUS_OK)
-		return val & 0xffff;
-	else {
-		wl1271_warning("Top register access returned error.");
-		return 0xffff;
-	}
-}
-
diff --git a/drivers/net/wireless/wl12xx/io.h b/drivers/net/wireless/ti/wlcore/io.h
index 4fb3dab8c3b..8942954b56a 100644
--- a/drivers/net/wireless/wl12xx/io.h
+++ b/drivers/net/wireless/ti/wlcore/io.h
@@ -26,7 +26,6 @@
 #define __IO_H__
 
 #include <linux/irqreturn.h>
-#include "reg.h"
 
 #define HW_ACCESS_MEMORY_MAX_RANGE	0x1FFC0
 
@@ -43,15 +42,14 @@
 
 #define HW_ACCESS_PRAM_MAX_RANGE	0x3c000
 
-extern struct wl1271_partition_set wl12xx_part_table[PART_TABLE_LEN];
-
 struct wl1271;
 
-void wl1271_disable_interrupts(struct wl1271 *wl);
-void wl1271_enable_interrupts(struct wl1271 *wl);
+void wlcore_disable_interrupts(struct wl1271 *wl);
+void wlcore_enable_interrupts(struct wl1271 *wl);
 
 void wl1271_io_reset(struct wl1271 *wl);
 void wl1271_io_init(struct wl1271 *wl);
+int wlcore_translate_addr(struct wl1271 *wl, int addr);
 
 /* Raw target IO, address is not translated */
 static inline void wl1271_raw_write(struct wl1271 *wl, int addr, void *buf,
@@ -66,6 +64,18 @@ static inline void wl1271_raw_read(struct wl1271 *wl, int addr, void *buf,
 	wl->if_ops->read(wl->dev, addr, buf, len, fixed);
 }
 
+static inline void wlcore_raw_read_data(struct wl1271 *wl, int reg, void *buf,
+					size_t len, bool fixed)
+{
+	wl1271_raw_read(wl, wl->rtable[reg], buf, len, fixed);
+}
+
+static inline void wlcore_raw_write_data(struct wl1271 *wl, int reg, void *buf,
+					 size_t len, bool fixed)
+{
+	wl1271_raw_write(wl, wl->rtable[reg], buf, len, fixed);
+}
+
 static inline u32 wl1271_raw_read32(struct wl1271 *wl, int addr)
 {
 	wl1271_raw_read(wl, addr, &wl->buffer_32,
@@ -81,36 +91,12 @@ static inline void wl1271_raw_write32(struct wl1271 *wl, int addr, u32 val)
 			     sizeof(wl->buffer_32), false);
 }
 
-/* Translated target IO */
-static inline int wl1271_translate_addr(struct wl1271 *wl, int addr)
-{
-	/*
-	 * To translate, first check to which window of addresses the
-	 * particular address belongs. Then subtract the starting address
-	 * of that window from the address. Then, add offset of the
-	 * translated region.
-	 *
-	 * The translated regions occur next to each other in physical device
-	 * memory, so just add the sizes of the preceding address regions to
-	 * get the offset to the new region.
-	 *
-	 * Currently, only the two first regions are addressed, and the
-	 * assumption is that all addresses will fall into either of those
-	 * two.
-	 */
-	if ((addr >= wl->part.reg.start) &&
-	    (addr < wl->part.reg.start + wl->part.reg.size))
-		return addr - wl->part.reg.start + wl->part.mem.size;
-	else
-		return addr - wl->part.mem.start;
-}
-
 static inline void wl1271_read(struct wl1271 *wl, int addr, void *buf,
 			       size_t len, bool fixed)
 {
 	int physical;
 
-	physical = wl1271_translate_addr(wl, addr);
+	physical = wlcore_translate_addr(wl, addr);
 
 	wl1271_raw_read(wl, physical, buf, len, fixed);
 }
@@ -120,11 +106,23 @@ static inline void wl1271_write(struct wl1271 *wl, int addr, void *buf,
 {
 	int physical;
 
-	physical = wl1271_translate_addr(wl, addr);
+	physical = wlcore_translate_addr(wl, addr);
 
 	wl1271_raw_write(wl, physical, buf, len, fixed);
 }
 
+static inline void wlcore_write_data(struct wl1271 *wl, int reg, void *buf,
+				     size_t len, bool fixed)
+{
+	wl1271_write(wl, wl->rtable[reg], buf, len, fixed);
+}
+
+static inline void wlcore_read_data(struct wl1271 *wl, int reg, void *buf,
+				    size_t len, bool fixed)
+{
+	wl1271_read(wl, wl->rtable[reg], buf, len, fixed);
+}
+
 static inline void wl1271_read_hwaddr(struct wl1271 *wl, int hwaddr,
 				      void *buf, size_t len, bool fixed)
 {
@@ -134,19 +132,30 @@ static inline void wl1271_read_hwaddr(struct wl1271 *wl, int hwaddr,
 	/* Addresses are stored internally as addresses to 32 bytes blocks */
 	addr = hwaddr << 5;
 
-	physical = wl1271_translate_addr(wl, addr);
+	physical = wlcore_translate_addr(wl, addr);
 
 	wl1271_raw_read(wl, physical, buf, len, fixed);
 }
 
 static inline u32 wl1271_read32(struct wl1271 *wl, int addr)
 {
-	return wl1271_raw_read32(wl, wl1271_translate_addr(wl, addr));
+	return wl1271_raw_read32(wl, wlcore_translate_addr(wl, addr));
 }
 
 static inline void wl1271_write32(struct wl1271 *wl, int addr, u32 val)
 {
-	wl1271_raw_write32(wl, wl1271_translate_addr(wl, addr), val);
+	wl1271_raw_write32(wl, wlcore_translate_addr(wl, addr), val);
+}
+
+static inline u32 wlcore_read_reg(struct wl1271 *wl, int reg)
+{
+	return wl1271_raw_read32(wl,
+				 wlcore_translate_addr(wl, wl->rtable[reg]));
+}
+
+static inline void wlcore_write_reg(struct wl1271 *wl, int reg, u32 val)
+{
+	wl1271_raw_write32(wl, wlcore_translate_addr(wl, wl->rtable[reg]), val);
 }
 
 static inline void wl1271_power_off(struct wl1271 *wl)
@@ -164,13 +173,8 @@ static inline int wl1271_power_on(struct wl1271 *wl)
 	return ret;
 }
 
-
-/* Top Register IO */
-void wl1271_top_reg_write(struct wl1271 *wl, int addr, u16 val);
-u16 wl1271_top_reg_read(struct wl1271 *wl, int addr);
-
-int wl1271_set_partition(struct wl1271 *wl,
-			 struct wl1271_partition_set *p);
+void wlcore_set_partition(struct wl1271 *wl,
+			  const struct wlcore_partition_set *p);
 
 bool wl1271_set_block_size(struct wl1271 *wl);
 
@@ -178,4 +182,6 @@ bool wl1271_set_block_size(struct wl1271 *wl);
 
 int wl1271_tx_dummy_packet(struct wl1271 *wl);
 
+void wlcore_select_partition(struct wl1271 *wl, u8 part);
+
 #endif
diff --git a/drivers/net/wireless/wl12xx/main.c b/drivers/net/wireless/ti/wlcore/main.c
index 39002363611..acef93390d3 100644
--- a/drivers/net/wireless/wl12xx/main.c
+++ b/drivers/net/wireless/ti/wlcore/main.c
@@ -35,10 +35,9 @@
 #include <linux/sched.h>
 #include <linux/interrupt.h>
 
-#include "wl12xx.h"
+#include "wlcore.h"
 #include "debug.h"
 #include "wl12xx_80211.h"
-#include "reg.h"
 #include "io.h"
 #include "event.h"
 #include "tx.h"
@@ -50,342 +49,15 @@
 #include "boot.h"
 #include "testmode.h"
 #include "scan.h"
+#include "hw_ops.h"
 
 #define WL1271_BOOT_RETRIES 3
 
-static struct conf_drv_settings default_conf = {
-	.sg = {
-		.params = {
-			[CONF_SG_ACL_BT_MASTER_MIN_BR] = 10,
-			[CONF_SG_ACL_BT_MASTER_MAX_BR] = 180,
-			[CONF_SG_ACL_BT_SLAVE_MIN_BR] = 10,
-			[CONF_SG_ACL_BT_SLAVE_MAX_BR] = 180,
-			[CONF_SG_ACL_BT_MASTER_MIN_EDR] = 10,
-			[CONF_SG_ACL_BT_MASTER_MAX_EDR] = 80,
-			[CONF_SG_ACL_BT_SLAVE_MIN_EDR] = 10,
-			[CONF_SG_ACL_BT_SLAVE_MAX_EDR] = 80,
-			[CONF_SG_ACL_WLAN_PS_MASTER_BR] = 8,
-			[CONF_SG_ACL_WLAN_PS_SLAVE_BR] = 8,
-			[CONF_SG_ACL_WLAN_PS_MASTER_EDR] = 20,
-			[CONF_SG_ACL_WLAN_PS_SLAVE_EDR] = 20,
-			[CONF_SG_ACL_WLAN_ACTIVE_MASTER_MIN_BR] = 20,
-			[CONF_SG_ACL_WLAN_ACTIVE_MASTER_MAX_BR] = 35,
-			[CONF_SG_ACL_WLAN_ACTIVE_SLAVE_MIN_BR] = 16,
-			[CONF_SG_ACL_WLAN_ACTIVE_SLAVE_MAX_BR] = 35,
-			[CONF_SG_ACL_WLAN_ACTIVE_MASTER_MIN_EDR] = 32,
-			[CONF_SG_ACL_WLAN_ACTIVE_MASTER_MAX_EDR] = 50,
-			[CONF_SG_ACL_WLAN_ACTIVE_SLAVE_MIN_EDR] = 28,
-			[CONF_SG_ACL_WLAN_ACTIVE_SLAVE_MAX_EDR] = 50,
-			[CONF_SG_ACL_ACTIVE_SCAN_WLAN_BR] = 10,
-			[CONF_SG_ACL_ACTIVE_SCAN_WLAN_EDR] = 20,
-			[CONF_SG_ACL_PASSIVE_SCAN_BT_BR] = 75,
-			[CONF_SG_ACL_PASSIVE_SCAN_WLAN_BR] = 15,
-			[CONF_SG_ACL_PASSIVE_SCAN_BT_EDR] = 27,
-			[CONF_SG_ACL_PASSIVE_SCAN_WLAN_EDR] = 17,
-			/* active scan params */
-			[CONF_SG_AUTO_SCAN_PROBE_REQ] = 170,
-			[CONF_SG_ACTIVE_SCAN_DURATION_FACTOR_HV3] = 50,
-			[CONF_SG_ACTIVE_SCAN_DURATION_FACTOR_A2DP] = 100,
-			/* passive scan params */
-			[CONF_SG_PASSIVE_SCAN_DURATION_FACTOR_A2DP_BR] = 800,
-			[CONF_SG_PASSIVE_SCAN_DURATION_FACTOR_A2DP_EDR] = 200,
-			[CONF_SG_PASSIVE_SCAN_DURATION_FACTOR_HV3] = 200,
-			/* passive scan in dual antenna params */
-			[CONF_SG_CONSECUTIVE_HV3_IN_PASSIVE_SCAN] = 0,
-			[CONF_SG_BCN_HV3_COLLISION_THRESH_IN_PASSIVE_SCAN] = 0,
-			[CONF_SG_TX_RX_PROTECTION_BWIDTH_IN_PASSIVE_SCAN] = 0,
-			/* general params */
-			[CONF_SG_STA_FORCE_PS_IN_BT_SCO] = 1,
-			[CONF_SG_ANTENNA_CONFIGURATION] = 0,
-			[CONF_SG_BEACON_MISS_PERCENT] = 60,
-			[CONF_SG_DHCP_TIME] = 5000,
-			[CONF_SG_RXT] = 1200,
-			[CONF_SG_TXT] = 1000,
-			[CONF_SG_ADAPTIVE_RXT_TXT] = 1,
-			[CONF_SG_GENERAL_USAGE_BIT_MAP] = 3,
-			[CONF_SG_HV3_MAX_SERVED] = 6,
-			[CONF_SG_PS_POLL_TIMEOUT] = 10,
-			[CONF_SG_UPSD_TIMEOUT] = 10,
-			[CONF_SG_CONSECUTIVE_CTS_THRESHOLD] = 2,
-			[CONF_SG_STA_RX_WINDOW_AFTER_DTIM] = 5,
-			[CONF_SG_STA_CONNECTION_PROTECTION_TIME] = 30,
-			/* AP params */
-			[CONF_AP_BEACON_MISS_TX] = 3,
-			[CONF_AP_RX_WINDOW_AFTER_BEACON] = 10,
-			[CONF_AP_BEACON_WINDOW_INTERVAL] = 2,
-			[CONF_AP_CONNECTION_PROTECTION_TIME] = 0,
-			[CONF_AP_BT_ACL_VAL_BT_SERVE_TIME] = 25,
-			[CONF_AP_BT_ACL_VAL_WL_SERVE_TIME] = 25,
-			/* CTS Diluting params */
-			[CONF_SG_CTS_DILUTED_BAD_RX_PACKETS_TH] = 0,
-			[CONF_SG_CTS_CHOP_IN_DUAL_ANT_SCO_MASTER] = 0,
-		},
-		.state = CONF_SG_PROTECTIVE,
-	},
-	.rx = {
-		.rx_msdu_life_time           = 512000,
-		.packet_detection_threshold  = 0,
-		.ps_poll_timeout             = 15,
-		.upsd_timeout                = 15,
-		.rts_threshold               = IEEE80211_MAX_RTS_THRESHOLD,
-		.rx_cca_threshold            = 0,
-		.irq_blk_threshold           = 0xFFFF,
-		.irq_pkt_threshold           = 0,
-		.irq_timeout                 = 600,
-		.queue_type                  = CONF_RX_QUEUE_TYPE_LOW_PRIORITY,
-	},
-	.tx = {
-		.tx_energy_detection         = 0,
-		.sta_rc_conf                 = {
-			.enabled_rates       = 0,
-			.short_retry_limit   = 10,
-			.long_retry_limit    = 10,
-			.aflags              = 0,
-		},
-		.ac_conf_count               = 4,
-		.ac_conf                     = {
-			[CONF_TX_AC_BE] = {
-				.ac          = CONF_TX_AC_BE,
-				.cw_min      = 15,
-				.cw_max      = 63,
-				.aifsn       = 3,
-				.tx_op_limit = 0,
-			},
-			[CONF_TX_AC_BK] = {
-				.ac          = CONF_TX_AC_BK,
-				.cw_min      = 15,
-				.cw_max      = 63,
-				.aifsn       = 7,
-				.tx_op_limit = 0,
-			},
-			[CONF_TX_AC_VI] = {
-				.ac          = CONF_TX_AC_VI,
-				.cw_min      = 15,
-				.cw_max      = 63,
-				.aifsn       = CONF_TX_AIFS_PIFS,
-				.tx_op_limit = 3008,
-			},
-			[CONF_TX_AC_VO] = {
-				.ac          = CONF_TX_AC_VO,
-				.cw_min      = 15,
-				.cw_max      = 63,
-				.aifsn       = CONF_TX_AIFS_PIFS,
-				.tx_op_limit = 1504,
-			},
-		},
-		.max_tx_retries = 100,
-		.ap_aging_period = 300,
-		.tid_conf_count = 4,
-		.tid_conf = {
-			[CONF_TX_AC_BE] = {
-				.queue_id    = CONF_TX_AC_BE,
-				.channel_type = CONF_CHANNEL_TYPE_EDCF,
-				.tsid        = CONF_TX_AC_BE,
-				.ps_scheme   = CONF_PS_SCHEME_LEGACY,
-				.ack_policy  = CONF_ACK_POLICY_LEGACY,
-				.apsd_conf   = {0, 0},
-			},
-			[CONF_TX_AC_BK] = {
-				.queue_id    = CONF_TX_AC_BK,
-				.channel_type = CONF_CHANNEL_TYPE_EDCF,
-				.tsid        = CONF_TX_AC_BK,
-				.ps_scheme   = CONF_PS_SCHEME_LEGACY,
-				.ack_policy  = CONF_ACK_POLICY_LEGACY,
-				.apsd_conf   = {0, 0},
-			},
-			[CONF_TX_AC_VI] = {
-				.queue_id    = CONF_TX_AC_VI,
-				.channel_type = CONF_CHANNEL_TYPE_EDCF,
-				.tsid        = CONF_TX_AC_VI,
-				.ps_scheme   = CONF_PS_SCHEME_LEGACY,
-				.ack_policy  = CONF_ACK_POLICY_LEGACY,
-				.apsd_conf   = {0, 0},
-			},
-			[CONF_TX_AC_VO] = {
-				.queue_id    = CONF_TX_AC_VO,
-				.channel_type = CONF_CHANNEL_TYPE_EDCF,
-				.tsid        = CONF_TX_AC_VO,
-				.ps_scheme   = CONF_PS_SCHEME_LEGACY,
-				.ack_policy  = CONF_ACK_POLICY_LEGACY,
-				.apsd_conf   = {0, 0},
-			},
-		},
-		.frag_threshold              = IEEE80211_MAX_FRAG_THRESHOLD,
-		.tx_compl_timeout            = 700,
-		.tx_compl_threshold          = 4,
-		.basic_rate                  = CONF_HW_BIT_RATE_1MBPS,
-		.basic_rate_5                = CONF_HW_BIT_RATE_6MBPS,
-		.tmpl_short_retry_limit      = 10,
-		.tmpl_long_retry_limit       = 10,
-		.tx_watchdog_timeout         = 5000,
-	},
-	.conn = {
-		.wake_up_event               = CONF_WAKE_UP_EVENT_DTIM,
-		.listen_interval             = 1,
-		.suspend_wake_up_event       = CONF_WAKE_UP_EVENT_N_DTIM,
-		.suspend_listen_interval     = 3,
-		.bcn_filt_mode               = CONF_BCN_FILT_MODE_ENABLED,
-		.bcn_filt_ie_count           = 2,
-		.bcn_filt_ie = {
-			[0] = {
-				.ie          = WLAN_EID_CHANNEL_SWITCH,
-				.rule        = CONF_BCN_RULE_PASS_ON_APPEARANCE,
-			},
-			[1] = {
-				.ie          = WLAN_EID_HT_INFORMATION,
-				.rule        = CONF_BCN_RULE_PASS_ON_CHANGE,
-			},
-		},
-		.synch_fail_thold            = 10,
-		.bss_lose_timeout            = 100,
-		.beacon_rx_timeout           = 10000,
-		.broadcast_timeout           = 20000,
-		.rx_broadcast_in_ps          = 1,
-		.ps_poll_threshold           = 10,
-		.bet_enable                  = CONF_BET_MODE_ENABLE,
-		.bet_max_consecutive         = 50,
-		.psm_entry_retries           = 8,
-		.psm_exit_retries            = 16,
-		.psm_entry_nullfunc_retries  = 3,
-		.dynamic_ps_timeout          = 200,
-		.forced_ps                   = false,
-		.keep_alive_interval         = 55000,
-		.max_listen_interval         = 20,
-	},
-	.itrim = {
-		.enable = false,
-		.timeout = 50000,
-	},
-	.pm_config = {
-		.host_clk_settling_time = 5000,
-		.host_fast_wakeup_support = false
-	},
-	.roam_trigger = {
-		.trigger_pacing               = 1,
-		.avg_weight_rssi_beacon       = 20,
-		.avg_weight_rssi_data         = 10,
-		.avg_weight_snr_beacon        = 20,
-		.avg_weight_snr_data          = 10,
-	},
-	.scan = {
-		.min_dwell_time_active        = 7500,
-		.max_dwell_time_active        = 30000,
-		.min_dwell_time_passive       = 100000,
-		.max_dwell_time_passive       = 100000,
-		.num_probe_reqs               = 2,
-		.split_scan_timeout           = 50000,
-	},
-	.sched_scan = {
-		/* sched_scan requires dwell times in TU instead of TU/1000 */
-		.min_dwell_time_active = 30,
-		.max_dwell_time_active = 60,
-		.dwell_time_passive    = 100,
-		.dwell_time_dfs        = 150,
-		.num_probe_reqs        = 2,
-		.rssi_threshold        = -90,
-		.snr_threshold         = 0,
-	},
-	.rf = {
-		.tx_per_channel_power_compensation_2 = {
-			0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
-		},
-		.tx_per_channel_power_compensation_5 = {
-			0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
-			0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
-			0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
-		},
-	},
-	.ht = {
-		.rx_ba_win_size = 8,
-		.tx_ba_win_size = 64,
-		.inactivity_timeout = 10000,
-		.tx_ba_tid_bitmap = CONF_TX_BA_ENABLED_TID_BITMAP,
-	},
-	.mem_wl127x = {
-		.num_stations                 = 1,
-		.ssid_profiles                = 1,
-		.rx_block_num                 = 70,
-		.tx_min_block_num             = 40,
-		.dynamic_memory               = 1,
-		.min_req_tx_blocks            = 100,
-		.min_req_rx_blocks            = 22,
-		.tx_min                       = 27,
-	},
-	.mem_wl128x = {
-		.num_stations                 = 1,
-		.ssid_profiles                = 1,
-		.rx_block_num                 = 40,
-		.tx_min_block_num             = 40,
-		.dynamic_memory               = 1,
-		.min_req_tx_blocks            = 45,
-		.min_req_rx_blocks            = 22,
-		.tx_min                       = 27,
-	},
-	.fm_coex = {
-		.enable                       = true,
-		.swallow_period               = 5,
-		.n_divider_fref_set_1         = 0xff,       /* default */
-		.n_divider_fref_set_2         = 12,
-		.m_divider_fref_set_1         = 148,
-		.m_divider_fref_set_2         = 0xffff,     /* default */
-		.coex_pll_stabilization_time  = 0xffffffff, /* default */
-		.ldo_stabilization_time       = 0xffff,     /* default */
-		.fm_disturbed_band_margin     = 0xff,       /* default */
-		.swallow_clk_diff             = 0xff,       /* default */
-	},
-	.rx_streaming = {
-		.duration                      = 150,
-		.queues                        = 0x1,
-		.interval                      = 20,
-		.always                        = 0,
-	},
-	.fwlog = {
-		.mode                         = WL12XX_FWLOG_ON_DEMAND,
-		.mem_blocks                   = 2,
-		.severity                     = 0,
-		.timestamp                    = WL12XX_FWLOG_TIMESTAMP_DISABLED,
-		.output                       = WL12XX_FWLOG_OUTPUT_HOST,
-		.threshold                    = 0,
-	},
-	.hci_io_ds = HCI_IO_DS_6MA,
-	.rate = {
-		.rate_retry_score = 32000,
-		.per_add = 8192,
-		.per_th1 = 2048,
-		.per_th2 = 4096,
-		.max_per = 8100,
-		.inverse_curiosity_factor = 5,
-		.tx_fail_low_th = 4,
-		.tx_fail_high_th = 10,
-		.per_alpha_shift = 4,
-		.per_add_shift = 13,
-		.per_beta1_shift = 10,
-		.per_beta2_shift = 8,
-		.rate_check_up = 2,
-		.rate_check_down = 12,
-		.rate_retry_policy = {
-			0x00, 0x00, 0x00, 0x00, 0x00,
-			0x00, 0x00, 0x00, 0x00, 0x00,
-			0x00, 0x00, 0x00,
-		},
-	},
-	.hangover = {
-		.recover_time               = 0,
-		.hangover_period            = 20,
-		.dynamic_mode               = 1,
-		.early_termination_mode     = 1,
-		.max_period                 = 20,
-		.min_period                 = 1,
-		.increase_delta             = 1,
-		.decrease_delta             = 2,
-		.quiet_time                 = 4,
-		.increase_time              = 1,
-		.window_size                = 16,
-	},
-};
+#define WL1271_BOOT_RETRIES 3
 
 static char *fwlog_param;
 static bool bug_on_recovery;
+static bool no_recovery;
 
 static void __wl1271_op_remove_interface(struct wl1271 *wl,
 					 struct ieee80211_vif *vif,
@@ -628,22 +300,8 @@ out:
 	mutex_unlock(&wl->mutex);
 }
 
-static void wl1271_conf_init(struct wl1271 *wl)
+static void wlcore_adjust_conf(struct wl1271 *wl)
 {
-
-	/*
-	 * This function applies the default configuration to the driver. This
-	 * function is invoked upon driver load (spi probe.)
-	 *
-	 * The configuration is stored in a run-time structure in order to
-	 * facilitate for run-time adjustment of any of the parameters. Making
-	 * changes to the configuration structure will apply the new values on
-	 * the next interface up (wl1271_op_start.)
-	 */
-
-	/* apply driver default configuration */
-	memcpy(&wl->conf, &default_conf, sizeof(default_conf));
-
 	/* Adjust settings according to optional module parameters */
 	if (fwlog_param) {
 		if (!strcmp(fwlog_param, "continuous")) {
@@ -666,28 +324,7 @@ static int wl1271_plt_init(struct wl1271 *wl)
 {
 	int ret;
 
-	if (wl->chip.id == CHIP_ID_1283_PG20)
-		ret = wl128x_cmd_general_parms(wl);
-	else
-		ret = wl1271_cmd_general_parms(wl);
-	if (ret < 0)
-		return ret;
-
-	if (wl->chip.id == CHIP_ID_1283_PG20)
-		ret = wl128x_cmd_radio_parms(wl);
-	else
-		ret = wl1271_cmd_radio_parms(wl);
-	if (ret < 0)
-		return ret;
-
-	if (wl->chip.id != CHIP_ID_1283_PG20) {
-		ret = wl1271_cmd_ext_radio_parms(wl);
-		if (ret < 0)
-			return ret;
-	}
-
-	/* Chip-specific initializations */
-	ret = wl1271_chip_specific_init(wl);
+	ret = wl->ops->hw_init(wl);
 	if (ret < 0)
 		return ret;
 
@@ -750,7 +387,7 @@ static void wl12xx_irq_ps_regulate_link(struct wl1271 *wl,
 
 static void wl12xx_irq_update_links_status(struct wl1271 *wl,
 					   struct wl12xx_vif *wlvif,
-					   struct wl12xx_fw_status *status)
+					   struct wl_fw_status *status)
 {
 	struct wl1271_link *lnk;
 	u32 cur_fw_ps_map;
@@ -770,9 +407,10 @@ static void wl12xx_irq_update_links_status(struct wl1271 *wl,
 
 	for_each_set_bit(hlid, wlvif->ap.sta_hlid_map, WL12XX_MAX_LINKS) {
 		lnk = &wl->links[hlid];
-		cnt = status->tx_lnk_free_pkts[hlid] - lnk->prev_freed_pkts;
+		cnt = status->counters.tx_lnk_free_pkts[hlid] -
+			lnk->prev_freed_pkts;
 
-		lnk->prev_freed_pkts = status->tx_lnk_free_pkts[hlid];
+		lnk->prev_freed_pkts = status->counters.tx_lnk_free_pkts[hlid];
 		lnk->allocated_pkts -= cnt;
 
 		wl12xx_irq_ps_regulate_link(wl, wlvif, hlid,
@@ -781,15 +419,19 @@ static void wl12xx_irq_update_links_status(struct wl1271 *wl,
 }
 
 static void wl12xx_fw_status(struct wl1271 *wl,
-			     struct wl12xx_fw_status *status)
+			     struct wl_fw_status *status)
 {
 	struct wl12xx_vif *wlvif;
 	struct timespec ts;
 	u32 old_tx_blk_count = wl->tx_blocks_available;
 	int avail, freed_blocks;
 	int i;
+	size_t status_len;
+
+	status_len = sizeof(*status) + wl->fw_status_priv_len;
 
-	wl1271_raw_read(wl, FW_STATUS_ADDR, status, sizeof(*status), false);
+	wlcore_raw_read_data(wl, REG_RAW_FW_STATUS_ADDR, status,
+			     status_len, false);
 
 	wl1271_debug(DEBUG_IRQ, "intr: 0x%x (fw_rx_counter = %d, "
 		     "drv_rx_counter = %d, tx_results_counter = %d)",
@@ -801,10 +443,10 @@ static void wl12xx_fw_status(struct wl1271 *wl,
 	for (i = 0; i < NUM_TX_QUEUES; i++) {
 		/* prevent wrap-around in freed-packets counter */
 		wl->tx_allocated_pkts[i] -=
-				(status->tx_released_pkts[i] -
+				(status->counters.tx_released_pkts[i] -
 				wl->tx_pkts_freed[i]) & 0xff;
 
-		wl->tx_pkts_freed[i] = status->tx_released_pkts[i];
+		wl->tx_pkts_freed[i] = status->counters.tx_released_pkts[i];
 	}
 
 	/* prevent wrap-around in total blocks counter */
@@ -927,6 +569,9 @@ static irqreturn_t wl1271_irq(int irq, void *cookie)
 		smp_mb__after_clear_bit();
 
 		wl12xx_fw_status(wl, wl->fw_status);
+
+		wlcore_hw_tx_immediate_compl(wl);
+
 		intr = le32_to_cpu(wl->fw_status->intr);
 		intr &= WL1271_INTR_MASK;
 		if (!intr) {
@@ -963,9 +608,7 @@ static irqreturn_t wl1271_irq(int irq, void *cookie)
 			}
 
 			/* check for tx results */
-			if (wl->fw_status->tx_results_counter !=
-			    (wl->tx_results_count & 0xff))
-				wl1271_tx_complete(wl);
+			wlcore_hw_tx_delayed_compl(wl);
 
 			/* Make sure the deferred queues don't get too long */
 			defer_count = skb_queue_len(&wl->deferred_tx_queue) +
@@ -1046,10 +689,7 @@ static int wl12xx_fetch_firmware(struct wl1271 *wl, bool plt)
 
 	if (plt) {
 		fw_type = WL12XX_FW_TYPE_PLT;
-		if (wl->chip.id == CHIP_ID_1283_PG20)
-			fw_name = WL128X_PLT_FW_NAME;
-		else
-			fw_name	= WL127X_PLT_FW_NAME;
+		fw_name = wl->plt_fw_name;
 	} else {
 		/*
 		 * we can't call wl12xx_get_vif_count() here because
@@ -1057,16 +697,10 @@ static int wl12xx_fetch_firmware(struct wl1271 *wl, bool plt)
 		 */
 		if (wl->last_vif_count > 1) {
 			fw_type = WL12XX_FW_TYPE_MULTI;
-			if (wl->chip.id == CHIP_ID_1283_PG20)
-				fw_name = WL128X_FW_NAME_MULTI;
-			else
-				fw_name = WL127X_FW_NAME_MULTI;
+			fw_name = wl->mr_fw_name;
 		} else {
 			fw_type = WL12XX_FW_TYPE_NORMAL;
-			if (wl->chip.id == CHIP_ID_1283_PG20)
-				fw_name = WL128X_FW_NAME_SINGLE;
-			else
-				fw_name = WL127X_FW_NAME_SINGLE;
+			fw_name = wl->sr_fw_name;
 		}
 	}
 
@@ -1173,7 +807,7 @@ static void wl12xx_read_fwlog_panic(struct wl1271 *wl)
 	u32 first_addr;
 	u8 *block;
 
-	if ((wl->quirks & WL12XX_QUIRK_FWLOG_NOT_IMPLEMENTED) ||
+	if ((wl->quirks & WLCORE_QUIRK_FWLOG_NOT_IMPLEMENTED) ||
 	    (wl->conf.fwlog.mode != WL12XX_FWLOG_ON_DEMAND) ||
 	    (wl->conf.fwlog.mem_blocks == 0))
 		return;
@@ -1239,11 +873,20 @@ static void wl1271_recovery_work(struct work_struct *work)
 	wl12xx_read_fwlog_panic(wl);
 
 	wl1271_info("Hardware recovery in progress. FW ver: %s pc: 0x%x",
-		    wl->chip.fw_ver_str, wl1271_read32(wl, SCR_PAD4));
+		    wl->chip.fw_ver_str,
+		    wlcore_read_reg(wl, REG_PC_ON_RECOVERY));
 
 	BUG_ON(bug_on_recovery &&
 	       !test_bit(WL1271_FLAG_INTENDED_FW_RECOVERY, &wl->flags));
 
+	if (no_recovery) {
+		wl1271_info("No recovery (chosen on module load). Fw will remain stuck.");
+		clear_bit(WL1271_FLAG_RECOVERY_IN_PROGRESS, &wl->flags);
+		goto out_unlock;
+	}
+
+	BUG_ON(bug_on_recovery);
+
 	/*
 	 * Advance security sequence number to overcome potential progress
 	 * in the firmware during recovery. This doens't hurt if the network is
@@ -1290,10 +933,7 @@ out_unlock:
 
 static void wl1271_fw_wakeup(struct wl1271 *wl)
 {
-	u32 elp_reg;
-
-	elp_reg = ELPCTRL_WAKE_UP;
-	wl1271_raw_write32(wl, HW_ACCESS_ELP_CTRL_REG_ADDR, elp_reg);
+	wl1271_raw_write32(wl, HW_ACCESS_ELP_CTRL_REG, ELPCTRL_WAKE_UP);
 }
 
 static int wl1271_setup(struct wl1271 *wl)
@@ -1323,7 +963,7 @@ static int wl12xx_set_power_on(struct wl1271 *wl)
 	wl1271_io_reset(wl);
 	wl1271_io_init(wl);
 
-	wl1271_set_partition(wl, &wl12xx_part_table[PART_DOWN]);
+	wlcore_set_partition(wl, &wl->ptable[PART_BOOT]);
 
 	/* ELP module wake up */
 	wl1271_fw_wakeup(wl);
@@ -1348,44 +988,18 @@ static int wl12xx_chip_wakeup(struct wl1271 *wl, bool plt)
 	 * negligible, we use the same block size for all different
 	 * chip types.
 	 */
-	if (!wl1271_set_block_size(wl))
-		wl->quirks |= WL12XX_QUIRK_NO_BLOCKSIZE_ALIGNMENT;
-
-	switch (wl->chip.id) {
-	case CHIP_ID_1271_PG10:
-		wl1271_warning("chip id 0x%x (1271 PG10) support is obsolete",
-			       wl->chip.id);
-
-		ret = wl1271_setup(wl);
-		if (ret < 0)
-			goto out;
-		wl->quirks |= WL12XX_QUIRK_NO_BLOCKSIZE_ALIGNMENT;
-		break;
-
-	case CHIP_ID_1271_PG20:
-		wl1271_debug(DEBUG_BOOT, "chip id 0x%x (1271 PG20)",
-			     wl->chip.id);
+	if (wl1271_set_block_size(wl))
+		wl->quirks |= WLCORE_QUIRK_TX_BLOCKSIZE_ALIGN;
 
-		ret = wl1271_setup(wl);
-		if (ret < 0)
-			goto out;
-		wl->quirks |= WL12XX_QUIRK_NO_BLOCKSIZE_ALIGNMENT;
-		break;
+	ret = wl->ops->identify_chip(wl);
+	if (ret < 0)
+		goto out;
 
-	case CHIP_ID_1283_PG20:
-		wl1271_debug(DEBUG_BOOT, "chip id 0x%x (1283 PG20)",
-			     wl->chip.id);
+	/* TODO: make sure the lower driver has set things up correctly */
 
-		ret = wl1271_setup(wl);
-		if (ret < 0)
-			goto out;
-		break;
-	case CHIP_ID_1283_PG10:
-	default:
-		wl1271_warning("unsupported chip id: 0x%x", wl->chip.id);
-		ret = -ENODEV;
+	ret = wl1271_setup(wl);
+	if (ret < 0)
 		goto out;
-	}
 
 	ret = wl12xx_fetch_firmware(wl, plt);
 	if (ret < 0)
@@ -1425,7 +1039,7 @@ int wl1271_plt_start(struct wl1271 *wl)
 		if (ret < 0)
 			goto power_off;
 
-		ret = wl1271_boot(wl);
+		ret = wl->ops->boot(wl);
 		if (ret < 0)
 			goto power_off;
 
@@ -1454,7 +1068,7 @@ irq_disable:
 		   work function will not do anything.) Also, any other
 		   possible concurrent operations will fail due to the
 		   current state, hence the wl1271 struct should be safe. */
-		wl1271_disable_interrupts(wl);
+		wlcore_disable_interrupts(wl);
 		wl1271_flush_deferred_work(wl);
 		cancel_work_sync(&wl->netstack_work);
 		mutex_lock(&wl->mutex);
@@ -1481,7 +1095,7 @@ int wl1271_plt_stop(struct wl1271 *wl)
 	 * Otherwise, the interrupt handler might be called and exit without
 	 * reading the interrupt status.
 	 */
-	wl1271_disable_interrupts(wl);
+	wlcore_disable_interrupts(wl);
 	mutex_lock(&wl->mutex);
 	if (!wl->plt) {
 		mutex_unlock(&wl->mutex);
@@ -1491,7 +1105,7 @@ int wl1271_plt_stop(struct wl1271 *wl)
 		 * may have been disabled when op_stop was called. It will,
 		 * however, balance the above call to disable_interrupts().
 		 */
-		wl1271_enable_interrupts(wl);
+		wlcore_enable_interrupts(wl);
 
 		wl1271_error("cannot power down because not in PLT "
 			     "state: %d", wl->state);
@@ -1506,6 +1120,7 @@ int wl1271_plt_stop(struct wl1271 *wl)
 	cancel_work_sync(&wl->recovery_work);
 	cancel_delayed_work_sync(&wl->elp_work);
 	cancel_delayed_work_sync(&wl->tx_watchdog_work);
+	cancel_delayed_work_sync(&wl->connection_loss_work);
 
 	mutex_lock(&wl->mutex);
 	wl1271_power_off(wl);
@@ -1647,20 +1262,281 @@ static struct sk_buff *wl12xx_alloc_dummy_packet(struct wl1271 *wl)
 
 
 #ifdef CONFIG_PM
+static int
+wl1271_validate_wowlan_pattern(struct cfg80211_wowlan_trig_pkt_pattern *p)
+{
+	int num_fields = 0, in_field = 0, fields_size = 0;
+	int i, pattern_len = 0;
+
+	if (!p->mask) {
+		wl1271_warning("No mask in WoWLAN pattern");
+		return -EINVAL;
+	}
+
+	/*
+	 * The pattern is broken up into segments of bytes at different offsets
+	 * that need to be checked by the FW filter. Each segment is called
+	 * a field in the FW API. We verify that the total number of fields
+	 * required for this pattern won't exceed FW limits (8)
+	 * as well as the total fields buffer won't exceed the FW limit.
+	 * Note that if there's a pattern which crosses Ethernet/IP header
+	 * boundary a new field is required.
+	 */
+	for (i = 0; i < p->pattern_len; i++) {
+		if (test_bit(i, (unsigned long *)p->mask)) {
+			if (!in_field) {
+				in_field = 1;
+				pattern_len = 1;
+			} else {
+				if (i == WL1271_RX_FILTER_ETH_HEADER_SIZE) {
+					num_fields++;
+					fields_size += pattern_len +
+						RX_FILTER_FIELD_OVERHEAD;
+					pattern_len = 1;
+				} else
+					pattern_len++;
+			}
+		} else {
+			if (in_field) {
+				in_field = 0;
+				fields_size += pattern_len +
+					RX_FILTER_FIELD_OVERHEAD;
+				num_fields++;
+			}
+		}
+	}
+
+	if (in_field) {
+		fields_size += pattern_len + RX_FILTER_FIELD_OVERHEAD;
+		num_fields++;
+	}
+
+	if (num_fields > WL1271_RX_FILTER_MAX_FIELDS) {
+		wl1271_warning("RX Filter too complex. Too many segments");
+		return -EINVAL;
+	}
+
+	if (fields_size > WL1271_RX_FILTER_MAX_FIELDS_SIZE) {
+		wl1271_warning("RX filter pattern is too big");
+		return -E2BIG;
+	}
+
+	return 0;
+}
+
+struct wl12xx_rx_filter *wl1271_rx_filter_alloc(void)
+{
+	return kzalloc(sizeof(struct wl12xx_rx_filter), GFP_KERNEL);
+}
+
+void wl1271_rx_filter_free(struct wl12xx_rx_filter *filter)
+{
+	int i;
+
+	if (filter == NULL)
+		return;
+
+	for (i = 0; i < filter->num_fields; i++)
+		kfree(filter->fields[i].pattern);
+
+	kfree(filter);
+}
+
+int wl1271_rx_filter_alloc_field(struct wl12xx_rx_filter *filter,
+				 u16 offset, u8 flags,
+				 u8 *pattern, u8 len)
+{
+	struct wl12xx_rx_filter_field *field;
+
+	if (filter->num_fields == WL1271_RX_FILTER_MAX_FIELDS) {
+		wl1271_warning("Max fields per RX filter. can't alloc another");
+		return -EINVAL;
+	}
+
+	field = &filter->fields[filter->num_fields];
+
+	field->pattern = kzalloc(len, GFP_KERNEL);
+	if (!field->pattern) {
+		wl1271_warning("Failed to allocate RX filter pattern");
+		return -ENOMEM;
+	}
+
+	filter->num_fields++;
+
+	field->offset = cpu_to_le16(offset);
+	field->flags = flags;
+	field->len = len;
+	memcpy(field->pattern, pattern, len);
+
+	return 0;
+}
+
+int wl1271_rx_filter_get_fields_size(struct wl12xx_rx_filter *filter)
+{
+	int i, fields_size = 0;
+
+	for (i = 0; i < filter->num_fields; i++)
+		fields_size += filter->fields[i].len +
+			sizeof(struct wl12xx_rx_filter_field) -
+			sizeof(u8 *);
+
+	return fields_size;
+}
+
+void wl1271_rx_filter_flatten_fields(struct wl12xx_rx_filter *filter,
+				    u8 *buf)
+{
+	int i;
+	struct wl12xx_rx_filter_field *field;
+
+	for (i = 0; i < filter->num_fields; i++) {
+		field = (struct wl12xx_rx_filter_field *)buf;
+
+		field->offset = filter->fields[i].offset;
+		field->flags = filter->fields[i].flags;
+		field->len = filter->fields[i].len;
+
+		memcpy(&field->pattern, filter->fields[i].pattern, field->len);
+		buf += sizeof(struct wl12xx_rx_filter_field) -
+			sizeof(u8 *) + field->len;
+	}
+}
+
+/*
+ * Allocates an RX filter returned through f
+ * which needs to be freed using rx_filter_free()
+ */
+static int wl1271_convert_wowlan_pattern_to_rx_filter(
+	struct cfg80211_wowlan_trig_pkt_pattern *p,
+	struct wl12xx_rx_filter **f)
+{
+	int i, j, ret = 0;
+	struct wl12xx_rx_filter *filter;
+	u16 offset;
+	u8 flags, len;
+
+	filter = wl1271_rx_filter_alloc();
+	if (!filter) {
+		wl1271_warning("Failed to alloc rx filter");
+		ret = -ENOMEM;
+		goto err;
+	}
+
+	i = 0;
+	while (i < p->pattern_len) {
+		if (!test_bit(i, (unsigned long *)p->mask)) {
+			i++;
+			continue;
+		}
+
+		for (j = i; j < p->pattern_len; j++) {
+			if (!test_bit(j, (unsigned long *)p->mask))
+				break;
+
+			if (i < WL1271_RX_FILTER_ETH_HEADER_SIZE &&
+			    j >= WL1271_RX_FILTER_ETH_HEADER_SIZE)
+				break;
+		}
+
+		if (i < WL1271_RX_FILTER_ETH_HEADER_SIZE) {
+			offset = i;
+			flags = WL1271_RX_FILTER_FLAG_ETHERNET_HEADER;
+		} else {
+			offset = i - WL1271_RX_FILTER_ETH_HEADER_SIZE;
+			flags = WL1271_RX_FILTER_FLAG_IP_HEADER;
+		}
+
+		len = j - i;
+
+		ret = wl1271_rx_filter_alloc_field(filter,
+						   offset,
+						   flags,
+						   &p->pattern[i], len);
+		if (ret)
+			goto err;
+
+		i = j;
+	}
+
+	filter->action = FILTER_SIGNAL;
+
+	*f = filter;
+	return 0;
+
+err:
+	wl1271_rx_filter_free(filter);
+	*f = NULL;
+
+	return ret;
+}
+
+static int wl1271_configure_wowlan(struct wl1271 *wl,
+				   struct cfg80211_wowlan *wow)
+{
+	int i, ret;
+
+	if (!wow || wow->any || !wow->n_patterns) {
+		wl1271_acx_default_rx_filter_enable(wl, 0, FILTER_SIGNAL);
+		wl1271_rx_filter_clear_all(wl);
+		return 0;
+	}
+
+	if (WARN_ON(wow->n_patterns > WL1271_MAX_RX_FILTERS))
+		return -EINVAL;
+
+	/* Validate all incoming patterns before clearing current FW state */
+	for (i = 0; i < wow->n_patterns; i++) {
+		ret = wl1271_validate_wowlan_pattern(&wow->patterns[i]);
+		if (ret) {
+			wl1271_warning("Bad wowlan pattern %d", i);
+			return ret;
+		}
+	}
+
+	wl1271_acx_default_rx_filter_enable(wl, 0, FILTER_SIGNAL);
+	wl1271_rx_filter_clear_all(wl);
+
+	/* Translate WoWLAN patterns into filters */
+	for (i = 0; i < wow->n_patterns; i++) {
+		struct cfg80211_wowlan_trig_pkt_pattern *p;
+		struct wl12xx_rx_filter *filter = NULL;
+
+		p = &wow->patterns[i];
+
+		ret = wl1271_convert_wowlan_pattern_to_rx_filter(p, &filter);
+		if (ret) {
+			wl1271_warning("Failed to create an RX filter from "
+				       "wowlan pattern %d", i);
+			goto out;
+		}
+
+		ret = wl1271_rx_filter_enable(wl, i, 1, filter);
+
+		wl1271_rx_filter_free(filter);
+		if (ret)
+			goto out;
+	}
+
+	ret = wl1271_acx_default_rx_filter_enable(wl, 1, FILTER_DROP);
+
+out:
+	return ret;
+}
+
 static int wl1271_configure_suspend_sta(struct wl1271 *wl,
-					struct wl12xx_vif *wlvif)
+					struct wl12xx_vif *wlvif,
+					struct cfg80211_wowlan *wow)
 {
 	int ret = 0;
 
-	mutex_lock(&wl->mutex);
-
 	if (!test_bit(WLVIF_FLAG_STA_ASSOCIATED, &wlvif->flags))
-		goto out_unlock;
+		goto out;
 
 	ret = wl1271_ps_elp_wakeup(wl);
 	if (ret < 0)
-		goto out_unlock;
+		goto out;
 
+	wl1271_configure_wowlan(wl, wow);
 	ret = wl1271_acx_wake_up_conditions(wl, wlvif,
 				    wl->conf.conn.suspend_wake_up_event,
 				    wl->conf.conn.suspend_listen_interval);
@@ -1668,11 +1544,9 @@ static int wl1271_configure_suspend_sta(struct wl1271 *wl,
 	if (ret < 0)
 		wl1271_error("suspend: set wake up conditions failed: %d", ret);
 
-
 	wl1271_ps_elp_sleep(wl);
 
-out_unlock:
-	mutex_unlock(&wl->mutex);
+out:
 	return ret;
 
 }
@@ -1682,29 +1556,27 @@ static int wl1271_configure_suspend_ap(struct wl1271 *wl,
 {
 	int ret = 0;
 
-	mutex_lock(&wl->mutex);
-
 	if (!test_bit(WLVIF_FLAG_AP_STARTED, &wlvif->flags))
-		goto out_unlock;
+		goto out;
 
 	ret = wl1271_ps_elp_wakeup(wl);
 	if (ret < 0)
-		goto out_unlock;
+		goto out;
 
 	ret = wl1271_acx_beacon_filter_opt(wl, wlvif, true);
 
 	wl1271_ps_elp_sleep(wl);
-out_unlock:
-	mutex_unlock(&wl->mutex);
+out:
 	return ret;
 
 }
 
 static int wl1271_configure_suspend(struct wl1271 *wl,
-				    struct wl12xx_vif *wlvif)
+				    struct wl12xx_vif *wlvif,
+				    struct cfg80211_wowlan *wow)
 {
 	if (wlvif->bss_type == BSS_TYPE_STA_BSS)
-		return wl1271_configure_suspend_sta(wl, wlvif);
+		return wl1271_configure_suspend_sta(wl, wlvif, wow);
 	if (wlvif->bss_type == BSS_TYPE_AP_BSS)
 		return wl1271_configure_suspend_ap(wl, wlvif);
 	return 0;
@@ -1720,12 +1592,13 @@ static void wl1271_configure_resume(struct wl1271 *wl,
 	if ((!is_ap) && (!is_sta))
 		return;
 
-	mutex_lock(&wl->mutex);
 	ret = wl1271_ps_elp_wakeup(wl);
 	if (ret < 0)
-		goto out;
+		return;
 
 	if (is_sta) {
+		wl1271_configure_wowlan(wl, NULL);
+
 		ret = wl1271_acx_wake_up_conditions(wl, wlvif,
 				    wl->conf.conn.wake_up_event,
 				    wl->conf.conn.listen_interval);
@@ -1739,8 +1612,6 @@ static void wl1271_configure_resume(struct wl1271 *wl,
 	}
 
 	wl1271_ps_elp_sleep(wl);
-out:
-	mutex_unlock(&wl->mutex);
 }
 
 static int wl1271_op_suspend(struct ieee80211_hw *hw,
@@ -1751,18 +1622,21 @@ static int wl1271_op_suspend(struct ieee80211_hw *hw,
 	int ret;
 
 	wl1271_debug(DEBUG_MAC80211, "mac80211 suspend wow=%d", !!wow);
-	WARN_ON(!wow || !wow->any);
+	WARN_ON(!wow);
 
 	wl1271_tx_flush(wl);
 
+	mutex_lock(&wl->mutex);
 	wl->wow_enabled = true;
 	wl12xx_for_each_wlvif(wl, wlvif) {
-		ret = wl1271_configure_suspend(wl, wlvif);
+		ret = wl1271_configure_suspend(wl, wlvif, wow);
 		if (ret < 0) {
+			mutex_unlock(&wl->mutex);
 			wl1271_warning("couldn't prepare device to suspend");
 			return ret;
 		}
 	}
+	mutex_unlock(&wl->mutex);
 	/* flush any remaining work */
 	wl1271_debug(DEBUG_MAC80211, "flushing remaining works");
 
@@ -1770,7 +1644,7 @@ static int wl1271_op_suspend(struct ieee80211_hw *hw,
 	 * disable and re-enable interrupts in order to flush
 	 * the threaded_irq
 	 */
-	wl1271_disable_interrupts(wl);
+	wlcore_disable_interrupts(wl);
 
 	/*
 	 * set suspended flag to avoid triggering a new threaded_irq
@@ -1778,7 +1652,7 @@ static int wl1271_op_suspend(struct ieee80211_hw *hw,
 	 */
 	set_bit(WL1271_FLAG_SUSPENDED, &wl->flags);
 
-	wl1271_enable_interrupts(wl);
+	wlcore_enable_interrupts(wl);
 	flush_work(&wl->tx_work);
 	flush_delayed_work(&wl->elp_work);
 
@@ -1810,12 +1684,15 @@ static int wl1271_op_resume(struct ieee80211_hw *hw)
 		wl1271_debug(DEBUG_MAC80211,
 			     "run postponed irq_work directly");
 		wl1271_irq(0, wl);
-		wl1271_enable_interrupts(wl);
+		wlcore_enable_interrupts(wl);
 	}
+
+	mutex_lock(&wl->mutex);
 	wl12xx_for_each_wlvif(wl, wlvif) {
 		wl1271_configure_resume(wl, wlvif);
 	}
 	wl->wow_enabled = false;
+	mutex_unlock(&wl->mutex);
 
 	return 0;
 }
@@ -1851,7 +1728,7 @@ static void wl1271_op_stop(struct ieee80211_hw *hw)
 	 * Otherwise, the interrupt handler might be called and exit without
 	 * reading the interrupt status.
 	 */
-	wl1271_disable_interrupts(wl);
+	wlcore_disable_interrupts(wl);
 	mutex_lock(&wl->mutex);
 	if (wl->state == WL1271_STATE_OFF) {
 		mutex_unlock(&wl->mutex);
@@ -1861,7 +1738,7 @@ static void wl1271_op_stop(struct ieee80211_hw *hw)
 		 * may have been disabled when op_stop was called. It will,
 		 * however, balance the above call to disable_interrupts().
 		 */
-		wl1271_enable_interrupts(wl);
+		wlcore_enable_interrupts(wl);
 		return;
 	}
 
@@ -1878,6 +1755,7 @@ static void wl1271_op_stop(struct ieee80211_hw *hw)
 	cancel_work_sync(&wl->tx_work);
 	cancel_delayed_work_sync(&wl->elp_work);
 	cancel_delayed_work_sync(&wl->tx_watchdog_work);
+	cancel_delayed_work_sync(&wl->connection_loss_work);
 
 	/* let's notify MAC80211 about the remaining pending TX frames */
 	wl12xx_tx_reset(wl, true);
@@ -1894,7 +1772,6 @@ static void wl1271_op_stop(struct ieee80211_hw *hw)
 	wl->tx_results_count = 0;
 	wl->tx_packets_count = 0;
 	wl->time_offset = 0;
-	wl->tx_spare_blocks = TX_HW_BLOCK_SPARE_DEFAULT;
 	wl->ap_fw_ps_map = 0;
 	wl->ap_ps_map = 0;
 	wl->sched_scanning = false;
@@ -2067,7 +1944,7 @@ static bool wl12xx_init_fw(struct wl1271 *wl)
 		if (ret < 0)
 			goto power_off;
 
-		ret = wl1271_boot(wl);
+		ret = wl->ops->boot(wl);
 		if (ret < 0)
 			goto power_off;
 
@@ -2087,7 +1964,7 @@ irq_disable:
 		   work function will not do anything.) Also, any other
 		   possible concurrent operations will fail due to the
 		   current state, hence the wl1271 struct should be safe. */
-		wl1271_disable_interrupts(wl);
+		wlcore_disable_interrupts(wl);
 		wl1271_flush_deferred_work(wl);
 		cancel_work_sync(&wl->netstack_work);
 		mutex_lock(&wl->mutex);
@@ -2360,10 +2237,12 @@ deinit:
 		for (i = 0; i < CONF_TX_MAX_AC_COUNT; i++)
 			wl12xx_free_rate_policy(wl,
 						&wlvif->ap.ucast_rate_idx[i]);
+		wl1271_free_ap_keys(wl, wlvif);
 	}
 
+	dev_kfree_skb(wlvif->probereq);
+	wlvif->probereq = NULL;
 	wl12xx_tx_reset_wlvif(wl, wlvif);
-	wl1271_free_ap_keys(wl, wlvif);
 	if (wl->last_wlvif == wlvif)
 		wl->last_wlvif = NULL;
 	list_del(&wlvif->list);
@@ -2946,6 +2825,17 @@ static int wl1271_set_key(struct wl1271 *wl, struct wl12xx_vif *wlvif,
 	int ret;
 	bool is_ap = (wlvif->bss_type == BSS_TYPE_AP_BSS);
 
+	/*
+	 * A role set to GEM cipher requires different Tx settings (namely
+	 * spare blocks). Note when we are in this mode so the HW can adjust.
+	 */
+	if (key_type == KEY_GEM) {
+		if (action == KEY_ADD_OR_REPLACE)
+			wlvif->is_gem = true;
+		else if (action == KEY_REMOVE)
+			wlvif->is_gem = false;
+	}
+
 	if (is_ap) {
 		struct wl1271_station *wl_sta;
 		u8 hlid;
@@ -2984,17 +2874,6 @@ static int wl1271_set_key(struct wl1271 *wl, struct wl12xx_vif *wlvif,
 			0xff, 0xff, 0xff, 0xff, 0xff, 0xff
 		};
 
-		/*
-		 * A STA set to GEM cipher requires 2 tx spare blocks.
-		 * Return to default value when GEM cipher key is removed
-		 */
-		if (key_type == KEY_GEM) {
-			if (action == KEY_ADD_OR_REPLACE)
-				wl->tx_spare_blocks = 2;
-			else if (action == KEY_REMOVE)
-				wl->tx_spare_blocks = TX_HW_BLOCK_SPARE_DEFAULT;
-		}
-
 		addr = sta ? sta->addr : bcast_addr;
 
 		if (is_zero_ether_addr(addr)) {
@@ -3791,8 +3670,7 @@ static void wl1271_bss_info_changed_sta(struct wl1271 *wl,
 		wlvif->rssi_thold = bss_conf->cqm_rssi_thold;
 	}
 
-	if (changed & BSS_CHANGED_BSSID &&
-	    (is_ibss || bss_conf->assoc))
+	if (changed & BSS_CHANGED_BSSID)
 		if (!is_zero_ether_addr(bss_conf->bssid)) {
 			ret = wl12xx_cmd_build_null_data(wl, wlvif);
 			if (ret < 0)
@@ -3801,9 +3679,6 @@ static void wl1271_bss_info_changed_sta(struct wl1271 *wl,
 			ret = wl1271_build_qos_null_data(wl, vif);
 			if (ret < 0)
 				goto out;
-
-			/* Need to update the BSSID (for filtering etc) */
-			do_join = true;
 		}
 
 	if (changed & (BSS_CHANGED_ASSOC | BSS_CHANGED_HT)) {
@@ -3830,8 +3705,12 @@ sta_not_found:
 			int ieoffset;
 			wlvif->aid = bss_conf->aid;
 			wlvif->beacon_int = bss_conf->beacon_int;
+			do_join = true;
 			set_assoc = true;
 
+			/* Cancel connection_loss_work */
+			cancel_delayed_work_sync(&wl->connection_loss_work);
+
 			/*
 			 * use basic rates from AP, and determine lowest rate
 			 * to use with control frames.
@@ -4662,60 +4541,12 @@ static struct ieee80211_channel wl1271_channels[] = {
 	{ .hw_value = 14, .center_freq = 2484, .max_power = 25 },
 };
 
-/* mapping to indexes for wl1271_rates */
-static const u8 wl1271_rate_to_idx_2ghz[] = {
-	/* MCS rates are used only with 11n */
-	7,                            /* CONF_HW_RXTX_RATE_MCS7_SGI */
-	7,                            /* CONF_HW_RXTX_RATE_MCS7 */
-	6,                            /* CONF_HW_RXTX_RATE_MCS6 */
-	5,                            /* CONF_HW_RXTX_RATE_MCS5 */
-	4,                            /* CONF_HW_RXTX_RATE_MCS4 */
-	3,                            /* CONF_HW_RXTX_RATE_MCS3 */
-	2,                            /* CONF_HW_RXTX_RATE_MCS2 */
-	1,                            /* CONF_HW_RXTX_RATE_MCS1 */
-	0,                            /* CONF_HW_RXTX_RATE_MCS0 */
-
-	11,                            /* CONF_HW_RXTX_RATE_54   */
-	10,                            /* CONF_HW_RXTX_RATE_48   */
-	9,                             /* CONF_HW_RXTX_RATE_36   */
-	8,                             /* CONF_HW_RXTX_RATE_24   */
-
-	/* TI-specific rate */
-	CONF_HW_RXTX_RATE_UNSUPPORTED, /* CONF_HW_RXTX_RATE_22   */
-
-	7,                             /* CONF_HW_RXTX_RATE_18   */
-	6,                             /* CONF_HW_RXTX_RATE_12   */
-	3,                             /* CONF_HW_RXTX_RATE_11   */
-	5,                             /* CONF_HW_RXTX_RATE_9    */
-	4,                             /* CONF_HW_RXTX_RATE_6    */
-	2,                             /* CONF_HW_RXTX_RATE_5_5  */
-	1,                             /* CONF_HW_RXTX_RATE_2    */
-	0                              /* CONF_HW_RXTX_RATE_1    */
-};
-
-/* 11n STA capabilities */
-#define HW_RX_HIGHEST_RATE	72
-
-#define WL12XX_HT_CAP { \
-	.cap = IEEE80211_HT_CAP_GRN_FLD | IEEE80211_HT_CAP_SGI_20 | \
-	       (1 << IEEE80211_HT_CAP_RX_STBC_SHIFT), \
-	.ht_supported = true, \
-	.ampdu_factor = IEEE80211_HT_MAX_AMPDU_8K, \
-	.ampdu_density = IEEE80211_HT_MPDU_DENSITY_8, \
-	.mcs = { \
-		.rx_mask = { 0xff, 0, 0, 0, 0, 0, 0, 0, 0, 0, }, \
-		.rx_highest = cpu_to_le16(HW_RX_HIGHEST_RATE), \
-		.tx_params = IEEE80211_HT_MCS_TX_DEFINED, \
-		}, \
-}
-
 /* can't be const, mac80211 writes to this */
 static struct ieee80211_supported_band wl1271_band_2ghz = {
 	.channels = wl1271_channels,
 	.n_channels = ARRAY_SIZE(wl1271_channels),
 	.bitrates = wl1271_rates,
 	.n_bitrates = ARRAY_SIZE(wl1271_rates),
-	.ht_cap	= WL12XX_HT_CAP,
 };
 
 /* 5 GHz data rates for WL1273 */
@@ -4784,48 +4615,11 @@ static struct ieee80211_channel wl1271_channels_5ghz[] = {
 	{ .hw_value = 165, .center_freq = 5825, .max_power = 25 },
 };
 
-/* mapping to indexes for wl1271_rates_5ghz */
-static const u8 wl1271_rate_to_idx_5ghz[] = {
-	/* MCS rates are used only with 11n */
-	7,                            /* CONF_HW_RXTX_RATE_MCS7_SGI */
-	7,                            /* CONF_HW_RXTX_RATE_MCS7 */
-	6,                            /* CONF_HW_RXTX_RATE_MCS6 */
-	5,                            /* CONF_HW_RXTX_RATE_MCS5 */
-	4,                            /* CONF_HW_RXTX_RATE_MCS4 */
-	3,                            /* CONF_HW_RXTX_RATE_MCS3 */
-	2,                            /* CONF_HW_RXTX_RATE_MCS2 */
-	1,                            /* CONF_HW_RXTX_RATE_MCS1 */
-	0,                            /* CONF_HW_RXTX_RATE_MCS0 */
-
-	7,                             /* CONF_HW_RXTX_RATE_54   */
-	6,                             /* CONF_HW_RXTX_RATE_48   */
-	5,                             /* CONF_HW_RXTX_RATE_36   */
-	4,                             /* CONF_HW_RXTX_RATE_24   */
-
-	/* TI-specific rate */
-	CONF_HW_RXTX_RATE_UNSUPPORTED, /* CONF_HW_RXTX_RATE_22   */
-
-	3,                             /* CONF_HW_RXTX_RATE_18   */
-	2,                             /* CONF_HW_RXTX_RATE_12   */
-	CONF_HW_RXTX_RATE_UNSUPPORTED, /* CONF_HW_RXTX_RATE_11   */
-	1,                             /* CONF_HW_RXTX_RATE_9    */
-	0,                             /* CONF_HW_RXTX_RATE_6    */
-	CONF_HW_RXTX_RATE_UNSUPPORTED, /* CONF_HW_RXTX_RATE_5_5  */
-	CONF_HW_RXTX_RATE_UNSUPPORTED, /* CONF_HW_RXTX_RATE_2    */
-	CONF_HW_RXTX_RATE_UNSUPPORTED  /* CONF_HW_RXTX_RATE_1    */
-};
-
 static struct ieee80211_supported_band wl1271_band_5ghz = {
 	.channels = wl1271_channels_5ghz,
 	.n_channels = ARRAY_SIZE(wl1271_channels_5ghz),
 	.bitrates = wl1271_rates_5ghz,
 	.n_bitrates = ARRAY_SIZE(wl1271_rates_5ghz),
-	.ht_cap	= WL12XX_HT_CAP,
-};
-
-static const u8 *wl1271_band_rate_to_idx[] = {
-	[IEEE80211_BAND_2GHZ] = wl1271_rate_to_idx_2ghz,
-	[IEEE80211_BAND_5GHZ] = wl1271_rate_to_idx_5ghz
 };
 
 static const struct ieee80211_ops wl1271_ops = {
@@ -4862,18 +4656,18 @@ static const struct ieee80211_ops wl1271_ops = {
 };
 
 
-u8 wl1271_rate_to_idx(int rate, enum ieee80211_band band)
+u8 wlcore_rate_to_idx(struct wl1271 *wl, u8 rate, enum ieee80211_band band)
 {
 	u8 idx;
 
-	BUG_ON(band >= sizeof(wl1271_band_rate_to_idx)/sizeof(u8 *));
+	BUG_ON(band >= 2);
 
-	if (unlikely(rate >= CONF_HW_RXTX_RATE_MAX)) {
+	if (unlikely(rate >= wl->hw_tx_rate_tbl_size)) {
 		wl1271_error("Illegal RX rate from HW: %d", rate);
 		return 0;
 	}
 
-	idx = wl1271_band_rate_to_idx[band][rate];
+	idx = wl->band_rate_to_idx[band][rate];
 	if (unlikely(idx == CONF_HW_RXTX_RATE_UNSUPPORTED)) {
 		wl1271_error("Unsupported RX rate from HW: %d", rate);
 		return 0;
@@ -5027,32 +4821,32 @@ static struct bin_attribute fwlog_attr = {
 	.read = wl1271_sysfs_read_fwlog,
 };
 
-static bool wl12xx_mac_in_fuse(struct wl1271 *wl)
+static void wl1271_connection_loss_work(struct work_struct *work)
 {
-	bool supported = false;
-	u8 major, minor;
+	struct delayed_work *dwork;
+	struct wl1271 *wl;
+	struct ieee80211_vif *vif;
+	struct wl12xx_vif *wlvif;
 
-	if (wl->chip.id == CHIP_ID_1283_PG20) {
-		major = WL128X_PG_GET_MAJOR(wl->hw_pg_ver);
-		minor = WL128X_PG_GET_MINOR(wl->hw_pg_ver);
+	dwork = container_of(work, struct delayed_work, work);
+	wl = container_of(dwork, struct wl1271, connection_loss_work);
 
-		/* in wl128x we have the MAC address if the PG is >= (2, 1) */
-		if (major > 2 || (major == 2 && minor >= 1))
-			supported = true;
-	} else {
-		major = WL127X_PG_GET_MAJOR(wl->hw_pg_ver);
-		minor = WL127X_PG_GET_MINOR(wl->hw_pg_ver);
+	wl1271_info("Connection loss work.");
 
-		/* in wl127x we have the MAC address if the PG is >= (3, 1) */
-		if (major == 3 && minor >= 1)
-			supported = true;
-	}
+	mutex_lock(&wl->mutex);
 
-	wl1271_debug(DEBUG_PROBE,
-		     "PG Ver major = %d minor = %d, MAC %s present",
-		     major, minor, supported ? "is" : "is not");
+	if (unlikely(wl->state == WL1271_STATE_OFF))
+		goto out;
 
-	return supported;
+	/* Call mac80211 connection loss */
+	wl12xx_for_each_wlvif_sta(wl, wlvif) {
+		if (!test_bit(WLVIF_FLAG_STA_ASSOCIATED, &wlvif->flags))
+			goto out;
+		vif = wl12xx_wlvif_to_vif(wlvif);
+		ieee80211_connection_loss(vif);
+	}
+out:
+	mutex_unlock(&wl->mutex);
 }
 
 static void wl12xx_derive_mac_addresses(struct wl1271 *wl,
@@ -5080,47 +4874,23 @@ static void wl12xx_derive_mac_addresses(struct wl1271 *wl,
 	wl->hw->wiphy->addresses = wl->addresses;
 }
 
-static void wl12xx_get_fuse_mac(struct wl1271 *wl)
-{
-	u32 mac1, mac2;
-
-	wl1271_set_partition(wl, &wl12xx_part_table[PART_DRPW]);
-
-	mac1 = wl1271_read32(wl, WL12XX_REG_FUSE_BD_ADDR_1);
-	mac2 = wl1271_read32(wl, WL12XX_REG_FUSE_BD_ADDR_2);
-
-	/* these are the two parts of the BD_ADDR */
-	wl->fuse_oui_addr = ((mac2 & 0xffff) << 8) +
-		((mac1 & 0xff000000) >> 24);
-	wl->fuse_nic_addr = mac1 & 0xffffff;
-
-	wl1271_set_partition(wl, &wl12xx_part_table[PART_DOWN]);
-}
-
 static int wl12xx_get_hw_info(struct wl1271 *wl)
 {
 	int ret;
-	u32 die_info;
 
 	ret = wl12xx_set_power_on(wl);
 	if (ret < 0)
 		goto out;
 
-	wl->chip.id = wl1271_read32(wl, CHIP_ID_B);
+	wl->chip.id = wlcore_read_reg(wl, REG_CHIP_ID_B);
 
-	if (wl->chip.id == CHIP_ID_1283_PG20)
-		die_info = wl1271_top_reg_read(wl, WL128X_REG_FUSE_DATA_2_1);
-	else
-		die_info = wl1271_top_reg_read(wl, WL127X_REG_FUSE_DATA_2_1);
+	wl->fuse_oui_addr = 0;
+	wl->fuse_nic_addr = 0;
 
-	wl->hw_pg_ver = (s8) (die_info & PG_VER_MASK) >> PG_VER_OFFSET;
+	wl->hw_pg_ver = wl->ops->get_pg_ver(wl);
 
-	if (!wl12xx_mac_in_fuse(wl)) {
-		wl->fuse_oui_addr = 0;
-		wl->fuse_nic_addr = 0;
-	} else {
-		wl12xx_get_fuse_mac(wl);
-	}
+	if (wl->ops->get_mac)
+		wl->ops->get_mac(wl);
 
 	wl1271_power_off(wl);
 out:
@@ -5242,7 +5012,8 @@ static int wl1271_init_ieee80211(struct wl1271 *wl)
 	wl->hw->wiphy->max_sched_scan_ie_len = WL1271_CMD_TEMPL_MAX_SIZE -
 		sizeof(struct ieee80211_header);
 
-	wl->hw->wiphy->flags |= WIPHY_FLAG_AP_UAPSD;
+	wl->hw->wiphy->flags |= WIPHY_FLAG_AP_UAPSD |
+				WIPHY_FLAG_HAS_REMAIN_ON_CHANNEL;
 
 	/* make sure all our channels fit in the scanned_ch bitmask */
 	BUILD_BUG_ON(ARRAY_SIZE(wl1271_channels) +
@@ -5254,8 +5025,12 @@ static int wl1271_init_ieee80211(struct wl1271 *wl)
 	 */
 	memcpy(&wl->bands[IEEE80211_BAND_2GHZ], &wl1271_band_2ghz,
 	       sizeof(wl1271_band_2ghz));
+	memcpy(&wl->bands[IEEE80211_BAND_2GHZ].ht_cap, &wl->ht_cap,
+	       sizeof(wl->ht_cap));
 	memcpy(&wl->bands[IEEE80211_BAND_5GHZ], &wl1271_band_5ghz,
 	       sizeof(wl1271_band_5ghz));
+	memcpy(&wl->bands[IEEE80211_BAND_5GHZ].ht_cap, &wl->ht_cap,
+	       sizeof(wl->ht_cap));
 
 	wl->hw->wiphy->bands[IEEE80211_BAND_2GHZ] =
 		&wl->bands[IEEE80211_BAND_2GHZ];
@@ -5279,14 +5054,14 @@ static int wl1271_init_ieee80211(struct wl1271 *wl)
 	wl->hw->sta_data_size = sizeof(struct wl1271_station);
 	wl->hw->vif_data_size = sizeof(struct wl12xx_vif);
 
-	wl->hw->max_rx_aggregation_subframes = 8;
+	wl->hw->max_rx_aggregation_subframes = wl->conf.ht.rx_ba_win_size;
 
 	return 0;
 }
 
 #define WL1271_DEFAULT_CHANNEL 0
 
-static struct ieee80211_hw *wl1271_alloc_hw(void)
+struct ieee80211_hw *wlcore_alloc_hw(size_t priv_size)
 {
 	struct ieee80211_hw *hw;
 	struct wl1271 *wl;
@@ -5305,6 +5080,13 @@ static struct ieee80211_hw *wl1271_alloc_hw(void)
 	wl = hw->priv;
 	memset(wl, 0, sizeof(*wl));
 
+	wl->priv = kzalloc(priv_size, GFP_KERNEL);
+	if (!wl->priv) {
+		wl1271_error("could not alloc wl priv");
+		ret = -ENOMEM;
+		goto err_priv_alloc;
+	}
+
 	INIT_LIST_HEAD(&wl->wlvif_list);
 
 	wl->hw = hw;
@@ -5322,6 +5104,8 @@ static struct ieee80211_hw *wl1271_alloc_hw(void)
 	INIT_WORK(&wl->recovery_work, wl1271_recovery_work);
 	INIT_DELAYED_WORK(&wl->scan_complete_work, wl1271_scan_complete_work);
 	INIT_DELAYED_WORK(&wl->tx_watchdog_work, wl12xx_tx_watchdog_work);
+	INIT_DELAYED_WORK(&wl->connection_loss_work,
+			  wl1271_connection_loss_work);
 
 	wl->freezable_wq = create_freezable_workqueue("wl12xx_wq");
 	if (!wl->freezable_wq) {
@@ -5341,7 +5125,6 @@ static struct ieee80211_hw *wl1271_alloc_hw(void)
 	wl->quirks = 0;
 	wl->platform_quirks = 0;
 	wl->sched_scanning = false;
-	wl->tx_spare_blocks = TX_HW_BLOCK_SPARE_DEFAULT;
 	wl->system_hlid = WL12XX_SYSTEM_HLID;
 	wl->active_sta_count = 0;
 	wl->fwlog_size = 0;
@@ -5351,7 +5134,7 @@ static struct ieee80211_hw *wl1271_alloc_hw(void)
 	__set_bit(WL12XX_SYSTEM_HLID, wl->links_map);
 
 	memset(wl->tx_frames_map, 0, sizeof(wl->tx_frames_map));
-	for (i = 0; i < ACX_TX_DESCRIPTORS; i++)
+	for (i = 0; i < wl->num_tx_desc; i++)
 		wl->tx_frames[i] = NULL;
 
 	spin_lock_init(&wl->wl_lock);
@@ -5360,9 +5143,6 @@ static struct ieee80211_hw *wl1271_alloc_hw(void)
 	wl->fw_type = WL12XX_FW_TYPE_NONE;
 	mutex_init(&wl->mutex);
 
-	/* Apply default driver configuration. */
-	wl1271_conf_init(wl);
-
 	order = get_order(WL1271_AGGR_BUFFER_SIZE);
 	wl->aggr_buf = (u8 *)__get_free_pages(GFP_KERNEL, order);
 	if (!wl->aggr_buf) {
@@ -5383,8 +5163,17 @@ static struct ieee80211_hw *wl1271_alloc_hw(void)
 		goto err_dummy_packet;
 	}
 
+	wl->mbox = kmalloc(sizeof(*wl->mbox), GFP_KERNEL | GFP_DMA);
+	if (!wl->mbox) {
+		ret = -ENOMEM;
+		goto err_fwlog;
+	}
+
 	return hw;
 
+err_fwlog:
+	free_page((unsigned long)wl->fwlog);
+
 err_dummy_packet:
 	dev_kfree_skb(wl->dummy_packet);
 
@@ -5396,14 +5185,18 @@ err_wq:
 
 err_hw:
 	wl1271_debugfs_exit(wl);
+	kfree(wl->priv);
+
+err_priv_alloc:
 	ieee80211_free_hw(hw);
 
 err_hw_alloc:
 
 	return ERR_PTR(ret);
 }
+EXPORT_SYMBOL_GPL(wlcore_alloc_hw);
 
-static int wl1271_free_hw(struct wl1271 *wl)
+int wlcore_free_hw(struct wl1271 *wl)
 {
 	/* Unblock any fwlog readers */
 	mutex_lock(&wl->mutex);
@@ -5433,10 +5226,12 @@ static int wl1271_free_hw(struct wl1271 *wl)
 	kfree(wl->tx_res_if);
 	destroy_workqueue(wl->freezable_wq);
 
+	kfree(wl->priv);
 	ieee80211_free_hw(wl->hw);
 
 	return 0;
 }
+EXPORT_SYMBOL_GPL(wlcore_free_hw);
 
 static irqreturn_t wl12xx_hardirq(int irq, void *cookie)
 {
@@ -5467,22 +5262,22 @@ static irqreturn_t wl12xx_hardirq(int irq, void *cookie)
 	return IRQ_WAKE_THREAD;
 }
 
-static int __devinit wl12xx_probe(struct platform_device *pdev)
+int __devinit wlcore_probe(struct wl1271 *wl, struct platform_device *pdev)
 {
 	struct wl12xx_platform_data *pdata = pdev->dev.platform_data;
-	struct ieee80211_hw *hw;
-	struct wl1271 *wl;
 	unsigned long irqflags;
-	int ret = -ENODEV;
+	int ret;
 
-	hw = wl1271_alloc_hw();
-	if (IS_ERR(hw)) {
-		wl1271_error("can't allocate hw");
-		ret = PTR_ERR(hw);
-		goto out;
+	if (!wl->ops || !wl->ptable) {
+		ret = -EINVAL;
+		goto out_free_hw;
 	}
 
-	wl = hw->priv;
+	BUG_ON(wl->num_tx_desc > WLCORE_MAX_TX_DESCRIPTORS);
+
+	/* adjust some runtime configuration parameters */
+	wlcore_adjust_conf(wl);
+
 	wl->irq = platform_get_irq(pdev, 0);
 	wl->ref_clock = pdata->board_ref_clock;
 	wl->tcxo_clock = pdata->board_tcxo_clock;
@@ -5510,9 +5305,14 @@ static int __devinit wl12xx_probe(struct platform_device *pdev)
 	if (!ret) {
 		wl->irq_wake_enabled = true;
 		device_init_wakeup(wl->dev, 1);
-		if (pdata->pwr_in_suspend)
-			hw->wiphy->wowlan.flags = WIPHY_WOWLAN_ANY;
-
+		if (pdata->pwr_in_suspend) {
+			wl->hw->wiphy->wowlan.flags = WIPHY_WOWLAN_ANY;
+			wl->hw->wiphy->wowlan.n_patterns =
+				WL1271_MAX_RX_FILTERS;
+			wl->hw->wiphy->wowlan.pattern_min_len = 1;
+			wl->hw->wiphy->wowlan.pattern_max_len =
+				WL1271_RX_FILTER_MAX_PATTERN_SIZE;
+		}
 	}
 	disable_irq(wl->irq);
 
@@ -5545,7 +5345,7 @@ static int __devinit wl12xx_probe(struct platform_device *pdev)
 		goto out_hw_pg_ver;
 	}
 
-	return 0;
+	goto out;
 
 out_hw_pg_ver:
 	device_remove_file(wl->dev, &dev_attr_hw_pg_ver);
@@ -5557,13 +5357,14 @@ out_irq:
 	free_irq(wl->irq, wl);
 
 out_free_hw:
-	wl1271_free_hw(wl);
+	wlcore_free_hw(wl);
 
 out:
 	return ret;
 }
+EXPORT_SYMBOL_GPL(wlcore_probe);
 
-static int __devexit wl12xx_remove(struct platform_device *pdev)
+int __devexit wlcore_remove(struct platform_device *pdev)
 {
 	struct wl1271 *wl = platform_get_drvdata(pdev);
 
@@ -5573,38 +5374,11 @@ static int __devexit wl12xx_remove(struct platform_device *pdev)
 	}
 	wl1271_unregister_hw(wl);
 	free_irq(wl->irq, wl);
-	wl1271_free_hw(wl);
+	wlcore_free_hw(wl);
 
 	return 0;
 }
-
-static const struct platform_device_id wl12xx_id_table[] __devinitconst = {
-	{ "wl12xx", 0 },
-	{  } /* Terminating Entry */
-};
-MODULE_DEVICE_TABLE(platform, wl12xx_id_table);
-
-static struct platform_driver wl12xx_driver = {
-	.probe		= wl12xx_probe,
-	.remove		= __devexit_p(wl12xx_remove),
-	.id_table	= wl12xx_id_table,
-	.driver = {
-		.name	= "wl12xx_driver",
-		.owner	= THIS_MODULE,
-	}
-};
-
-static int __init wl12xx_init(void)
-{
-	return platform_driver_register(&wl12xx_driver);
-}
-module_init(wl12xx_init);
-
-static void __exit wl12xx_exit(void)
-{
-	platform_driver_unregister(&wl12xx_driver);
-}
-module_exit(wl12xx_exit);
+EXPORT_SYMBOL_GPL(wlcore_remove);
 
 u32 wl12xx_debug_level = DEBUG_NONE;
 EXPORT_SYMBOL_GPL(wl12xx_debug_level);
@@ -5618,6 +5392,9 @@ MODULE_PARM_DESC(fwlog,
 module_param(bug_on_recovery, bool, S_IRUSR | S_IWUSR);
 MODULE_PARM_DESC(bug_on_recovery, "BUG() on fw recovery");
 
+module_param(no_recovery, bool, S_IRUSR | S_IWUSR);
+MODULE_PARM_DESC(no_recovery, "Prevent HW recovery. FW will remain stuck.");
+
 MODULE_LICENSE("GPL");
 MODULE_AUTHOR("Luciano Coelho <coelho@ti.com>");
 MODULE_AUTHOR("Juuso Oikarinen <juuso.oikarinen@nokia.com>");
diff --git a/drivers/net/wireless/wl12xx/ps.c b/drivers/net/wireless/ti/wlcore/ps.c
index 78f598b4f97..756eee2257b 100644
--- a/drivers/net/wireless/wl12xx/ps.c
+++ b/drivers/net/wireless/ti/wlcore/ps.c
@@ -21,7 +21,6 @@
  *
  */
 
-#include "reg.h"
 #include "ps.h"
 #include "io.h"
 #include "tx.h"
@@ -62,7 +61,7 @@ void wl1271_elp_work(struct work_struct *work)
 	}
 
 	wl1271_debug(DEBUG_PSM, "chip to elp");
-	wl1271_raw_write32(wl, HW_ACCESS_ELP_CTRL_REG_ADDR, ELPCTRL_SLEEP);
+	wl1271_raw_write32(wl, HW_ACCESS_ELP_CTRL_REG, ELPCTRL_SLEEP);
 	set_bit(WL1271_FLAG_IN_ELP, &wl->flags);
 
 out:
@@ -74,6 +73,9 @@ void wl1271_ps_elp_sleep(struct wl1271 *wl)
 {
 	struct wl12xx_vif *wlvif;
 
+	if (wl->quirks & WLCORE_QUIRK_NO_ELP)
+		return;
+
 	/* we shouldn't get consecutive sleep requests */
 	if (WARN_ON(test_and_set_bit(WL1271_FLAG_ELP_REQUESTED, &wl->flags)))
 		return;
@@ -125,7 +127,7 @@ int wl1271_ps_elp_wakeup(struct wl1271 *wl)
 		wl->elp_compl = &compl;
 	spin_unlock_irqrestore(&wl->wl_lock, flags);
 
-	wl1271_raw_write32(wl, HW_ACCESS_ELP_CTRL_REG_ADDR, ELPCTRL_WAKE_UP);
+	wl1271_raw_write32(wl, HW_ACCESS_ELP_CTRL_REG, ELPCTRL_WAKE_UP);
 
 	if (!pending) {
 		ret = wait_for_completion_timeout(
diff --git a/drivers/net/wireless/wl12xx/ps.h b/drivers/net/wireless/ti/wlcore/ps.h
index 5f19d4fbbf2..de4f9da8ed2 100644
--- a/drivers/net/wireless/wl12xx/ps.h
+++ b/drivers/net/wireless/ti/wlcore/ps.h
@@ -24,7 +24,7 @@
 #ifndef __PS_H__
 #define __PS_H__
 
-#include "wl12xx.h"
+#include "wlcore.h"
 #include "acx.h"
 
 int wl1271_ps_set_mode(struct wl1271 *wl, struct wl12xx_vif *wlvif,
diff --git a/drivers/net/wireless/wl12xx/rx.c b/drivers/net/wireless/ti/wlcore/rx.c
index cfa6071704c..1f1d9488dfb 100644
--- a/drivers/net/wireless/wl12xx/rx.c
+++ b/drivers/net/wireless/ti/wlcore/rx.c
@@ -24,34 +24,36 @@
 #include <linux/gfp.h>
 #include <linux/sched.h>
 
-#include "wl12xx.h"
+#include "wlcore.h"
 #include "debug.h"
 #include "acx.h"
-#include "reg.h"
 #include "rx.h"
 #include "tx.h"
 #include "io.h"
+#include "hw_ops.h"
 
-static u8 wl12xx_rx_get_mem_block(struct wl12xx_fw_status *status,
-				  u32 drv_rx_counter)
-{
-	return le32_to_cpu(status->rx_pkt_descs[drv_rx_counter]) &
-		RX_MEM_BLOCK_MASK;
-}
+/*
+ * TODO: this is here just for now, it must be removed when the data
+ * operations are in place.
+ */
+#include "../wl12xx/reg.h"
 
-static u32 wl12xx_rx_get_buf_size(struct wl12xx_fw_status *status,
-				 u32 drv_rx_counter)
+static u32 wlcore_rx_get_buf_size(struct wl1271 *wl,
+				  u32 rx_pkt_desc)
 {
-	return (le32_to_cpu(status->rx_pkt_descs[drv_rx_counter]) &
-		RX_BUF_SIZE_MASK) >> RX_BUF_SIZE_SHIFT_DIV;
+	if (wl->quirks & WLCORE_QUIRK_RX_BLOCKSIZE_ALIGN)
+		return (rx_pkt_desc & ALIGNED_RX_BUF_SIZE_MASK) >>
+		       ALIGNED_RX_BUF_SIZE_SHIFT;
+
+	return (rx_pkt_desc & RX_BUF_SIZE_MASK) >> RX_BUF_SIZE_SHIFT_DIV;
 }
 
-static bool wl12xx_rx_get_unaligned(struct wl12xx_fw_status *status,
-				    u32 drv_rx_counter)
+static u32 wlcore_rx_get_align_buf_size(struct wl1271 *wl, u32 pkt_len)
 {
-	/* Convert the value to bool */
-	return !!(le32_to_cpu(status->rx_pkt_descs[drv_rx_counter]) &
-		RX_BUF_UNALIGNED_PAYLOAD);
+	if (wl->quirks & WLCORE_QUIRK_RX_BLOCKSIZE_ALIGN)
+		return ALIGN(pkt_len, WL12XX_BUS_BLOCK_SIZE);
+
+	return pkt_len;
 }
 
 static void wl1271_rx_status(struct wl1271 *wl,
@@ -66,10 +68,10 @@ static void wl1271_rx_status(struct wl1271 *wl,
 	else
 		status->band = IEEE80211_BAND_5GHZ;
 
-	status->rate_idx = wl1271_rate_to_idx(desc->rate, status->band);
+	status->rate_idx = wlcore_rate_to_idx(wl, desc->rate, status->band);
 
 	/* 11n support */
-	if (desc->rate <= CONF_HW_RXTX_RATE_MCS0)
+	if (desc->rate <= wl->hw_min_ht_rate)
 		status->flag |= RX_FLAG_HT;
 
 	status->signal = desc->rssi;
@@ -98,7 +100,7 @@ static void wl1271_rx_status(struct wl1271 *wl,
 }
 
 static int wl1271_rx_handle_data(struct wl1271 *wl, u8 *data, u32 length,
-				 bool unaligned, u8 *hlid)
+				 enum wl_rx_buf_align rx_align, u8 *hlid)
 {
 	struct wl1271_rx_descriptor *desc;
 	struct sk_buff *skb;
@@ -106,8 +108,9 @@ static int wl1271_rx_handle_data(struct wl1271 *wl, u8 *data, u32 length,
 	u8 *buf;
 	u8 beacon = 0;
 	u8 is_data = 0;
-	u8 reserved = unaligned ? NET_IP_ALIGN : 0;
+	u8 reserved = 0;
 	u16 seq_num;
+	u32 pkt_data_len;
 
 	/*
 	 * In PLT mode we seem to get frames and mac80211 warns about them,
@@ -116,6 +119,16 @@ static int wl1271_rx_handle_data(struct wl1271 *wl, u8 *data, u32 length,
 	if (unlikely(wl->plt))
 		return -EINVAL;
 
+	pkt_data_len = wlcore_hw_get_rx_packet_len(wl, data, length);
+	if (!pkt_data_len) {
+		wl1271_error("Invalid packet arrived from HW. length %d",
+			     length);
+		return -EINVAL;
+	}
+
+	if (rx_align == WLCORE_RX_BUF_UNALIGNED)
+		reserved = NET_IP_ALIGN;
+
 	/* the data read starts with the descriptor */
 	desc = (struct wl1271_rx_descriptor *) data;
 
@@ -142,8 +155,8 @@ static int wl1271_rx_handle_data(struct wl1271 *wl, u8 *data, u32 length,
 		return -EINVAL;
 	}
 
-	/* skb length not included rx descriptor */
-	skb = __dev_alloc_skb(length + reserved - sizeof(*desc), GFP_KERNEL);
+	/* skb length not including rx descriptor */
+	skb = __dev_alloc_skb(pkt_data_len + reserved, GFP_KERNEL);
 	if (!skb) {
 		wl1271_error("Couldn't allocate RX frame");
 		return -ENOMEM;
@@ -152,7 +165,7 @@ static int wl1271_rx_handle_data(struct wl1271 *wl, u8 *data, u32 length,
 	/* reserve the unaligned payload(if any) */
 	skb_reserve(skb, reserved);
 
-	buf = skb_put(skb, length - sizeof(*desc));
+	buf = skb_put(skb, pkt_data_len);
 
 	/*
 	 * Copy packets from aggregation buffer to the skbs without rx
@@ -160,7 +173,10 @@ static int wl1271_rx_handle_data(struct wl1271 *wl, u8 *data, u32 length,
 	 * packets copy the packets in offset of 2 bytes guarantee IP header
 	 * payload aligned to 4 bytes.
 	 */
-	memcpy(buf, data + sizeof(*desc), length - sizeof(*desc));
+	memcpy(buf, data + sizeof(*desc), pkt_data_len);
+	if (rx_align == WLCORE_RX_BUF_PADDED)
+		skb_pull(skb, NET_IP_ALIGN);
+
 	*hlid = desc->hlid;
 
 	hdr = (struct ieee80211_hdr *)skb->data;
@@ -177,36 +193,35 @@ static int wl1271_rx_handle_data(struct wl1271 *wl, u8 *data, u32 length,
 		     beacon ? "beacon" : "",
 		     seq_num, *hlid);
 
-	skb_trim(skb, skb->len - desc->pad_len);
-
 	skb_queue_tail(&wl->deferred_rx_queue, skb);
 	queue_work(wl->freezable_wq, &wl->netstack_work);
 
 	return is_data;
 }
 
-void wl12xx_rx(struct wl1271 *wl, struct wl12xx_fw_status *status)
+void wl12xx_rx(struct wl1271 *wl, struct wl_fw_status *status)
 {
-	struct wl1271_acx_mem_map *wl_mem_map = wl->target_mem_map;
 	unsigned long active_hlids[BITS_TO_LONGS(WL12XX_MAX_LINKS)] = {0};
 	u32 buf_size;
 	u32 fw_rx_counter  = status->fw_rx_counter & NUM_RX_PKT_DESC_MOD_MASK;
 	u32 drv_rx_counter = wl->rx_counter & NUM_RX_PKT_DESC_MOD_MASK;
 	u32 rx_counter;
-	u32 mem_block;
-	u32 pkt_length;
-	u32 pkt_offset;
+	u32 pkt_len, align_pkt_len;
+	u32 pkt_offset, des;
 	u8 hlid;
-	bool unaligned = false;
+	enum wl_rx_buf_align rx_align;
 
 	while (drv_rx_counter != fw_rx_counter) {
 		buf_size = 0;
 		rx_counter = drv_rx_counter;
 		while (rx_counter != fw_rx_counter) {
-			pkt_length = wl12xx_rx_get_buf_size(status, rx_counter);
-			if (buf_size + pkt_length > WL1271_AGGR_BUFFER_SIZE)
+			des = le32_to_cpu(status->rx_pkt_descs[rx_counter]);
+			pkt_len = wlcore_rx_get_buf_size(wl, des);
+			align_pkt_len = wlcore_rx_get_align_buf_size(wl,
+								     pkt_len);
+			if (buf_size + align_pkt_len > WL1271_AGGR_BUFFER_SIZE)
 				break;
-			buf_size += pkt_length;
+			buf_size += align_pkt_len;
 			rx_counter++;
 			rx_counter &= NUM_RX_PKT_DESC_MOD_MASK;
 		}
@@ -216,38 +231,18 @@ void wl12xx_rx(struct wl1271 *wl, struct wl12xx_fw_status *status)
 			break;
 		}
 
-		if (wl->chip.id != CHIP_ID_1283_PG20) {
-			/*
-			 * Choose the block we want to read
-			 * For aggregated packets, only the first memory block
-			 * should be retrieved. The FW takes care of the rest.
-			 */
-			mem_block = wl12xx_rx_get_mem_block(status,
-							    drv_rx_counter);
-
-			wl->rx_mem_pool_addr.addr = (mem_block << 8) +
-			   le32_to_cpu(wl_mem_map->packet_memory_pool_start);
-
-			wl->rx_mem_pool_addr.addr_extra =
-				wl->rx_mem_pool_addr.addr + 4;
-
-			wl1271_write(wl, WL1271_SLV_REG_DATA,
-				     &wl->rx_mem_pool_addr,
-				     sizeof(wl->rx_mem_pool_addr), false);
-		}
-
 		/* Read all available packets at once */
-		wl1271_read(wl, WL1271_SLV_MEM_DATA, wl->aggr_buf,
-				buf_size, true);
+		des = le32_to_cpu(status->rx_pkt_descs[drv_rx_counter]);
+		wlcore_hw_prepare_read(wl, des, buf_size);
+		wlcore_read_data(wl, REG_SLV_MEM_DATA, wl->aggr_buf,
+				 buf_size, true);
 
 		/* Split data into separate packets */
 		pkt_offset = 0;
 		while (pkt_offset < buf_size) {
-			pkt_length = wl12xx_rx_get_buf_size(status,
-					drv_rx_counter);
-
-			unaligned = wl12xx_rx_get_unaligned(status,
-					drv_rx_counter);
+			des = le32_to_cpu(status->rx_pkt_descs[drv_rx_counter]);
+			pkt_len = wlcore_rx_get_buf_size(wl, des);
+			rx_align = wlcore_hw_get_rx_buf_align(wl, des);
 
 			/*
 			 * the handle data call can only fail in memory-outage
@@ -256,7 +251,7 @@ void wl12xx_rx(struct wl1271 *wl, struct wl12xx_fw_status *status)
 			 */
 			if (wl1271_rx_handle_data(wl,
 						  wl->aggr_buf + pkt_offset,
-						  pkt_length, unaligned,
+						  pkt_len, rx_align,
 						  &hlid) == 1) {
 				if (hlid < WL12XX_MAX_LINKS)
 					__set_bit(hlid, active_hlids);
@@ -269,7 +264,7 @@ void wl12xx_rx(struct wl1271 *wl, struct wl12xx_fw_status *status)
 			wl->rx_counter++;
 			drv_rx_counter++;
 			drv_rx_counter &= NUM_RX_PKT_DESC_MOD_MASK;
-			pkt_offset += pkt_length;
+			pkt_offset += wlcore_rx_get_align_buf_size(wl, pkt_len);
 		}
 	}
 
@@ -277,8 +272,45 @@ void wl12xx_rx(struct wl1271 *wl, struct wl12xx_fw_status *status)
 	 * Write the driver's packet counter to the FW. This is only required
 	 * for older hardware revisions
 	 */
-	if (wl->quirks & WL12XX_QUIRK_END_OF_TRANSACTION)
-		wl1271_write32(wl, RX_DRIVER_COUNTER_ADDRESS, wl->rx_counter);
+	if (wl->quirks & WLCORE_QUIRK_END_OF_TRANSACTION)
+		wl1271_write32(wl, WL12XX_REG_RX_DRIVER_COUNTER,
+			       wl->rx_counter);
 
 	wl12xx_rearm_rx_streaming(wl, active_hlids);
 }
+
+int wl1271_rx_filter_enable(struct wl1271 *wl,
+			    int index, bool enable,
+			    struct wl12xx_rx_filter *filter)
+{
+	int ret;
+
+	if (wl->rx_filter_enabled[index] == enable) {
+		wl1271_warning("Request to enable an already "
+			     "enabled rx filter %d", index);
+		return 0;
+	}
+
+	ret = wl1271_acx_set_rx_filter(wl, index, enable, filter);
+
+	if (ret) {
+		wl1271_error("Failed to %s rx data filter %d (err=%d)",
+			     enable ? "enable" : "disable", index, ret);
+		return ret;
+	}
+
+	wl->rx_filter_enabled[index] = enable;
+
+	return 0;
+}
+
+void wl1271_rx_filter_clear_all(struct wl1271 *wl)
+{
+	int i;
+
+	for (i = 0; i < WL1271_MAX_RX_FILTERS; i++) {
+		if (!wl->rx_filter_enabled[i])
+			continue;
+		wl1271_rx_filter_enable(wl, i, 0, NULL);
+	}
+}
diff --git a/drivers/net/wireless/wl12xx/rx.h b/drivers/net/wireless/ti/wlcore/rx.h
index 86ba6b1d0cd..e9a162a864c 100644
--- a/drivers/net/wireless/wl12xx/rx.h
+++ b/drivers/net/wireless/ti/wlcore/rx.h
@@ -96,9 +96,19 @@
 #define RX_MEM_BLOCK_MASK            0xFF
 #define RX_BUF_SIZE_MASK             0xFFF00
 #define RX_BUF_SIZE_SHIFT_DIV        6
+#define ALIGNED_RX_BUF_SIZE_MASK     0xFFFF00
+#define ALIGNED_RX_BUF_SIZE_SHIFT    8
+
 /* If set, the start of IP payload is not 4 bytes aligned */
 #define RX_BUF_UNALIGNED_PAYLOAD     BIT(20)
 
+/* Describes the alignment state of a Rx buffer */
+enum wl_rx_buf_align {
+	WLCORE_RX_BUF_ALIGNED,
+	WLCORE_RX_BUF_UNALIGNED,
+	WLCORE_RX_BUF_PADDED,
+};
+
 enum {
 	WL12XX_RX_CLASS_UNKNOWN,
 	WL12XX_RX_CLASS_MANAGEMENT,
@@ -126,7 +136,11 @@ struct wl1271_rx_descriptor {
 	u8  reserved;
 } __packed;
 
-void wl12xx_rx(struct wl1271 *wl, struct wl12xx_fw_status *status);
+void wl12xx_rx(struct wl1271 *wl, struct wl_fw_status *status);
 u8 wl1271_rate_to_idx(int rate, enum ieee80211_band band);
+int wl1271_rx_filter_enable(struct wl1271 *wl,
+			    int index, bool enable,
+			    struct wl12xx_rx_filter *filter);
+void wl1271_rx_filter_clear_all(struct wl1271 *wl);
 
 #endif
diff --git a/drivers/net/wireless/wl12xx/scan.c b/drivers/net/wireless/ti/wlcore/scan.c
index fcba055ef19..ade21a011c4 100644
--- a/drivers/net/wireless/wl12xx/scan.c
+++ b/drivers/net/wireless/ti/wlcore/scan.c
@@ -23,7 +23,7 @@
 
 #include <linux/ieee80211.h>
 
-#include "wl12xx.h"
+#include "wlcore.h"
 #include "debug.h"
 #include "cmd.h"
 #include "scan.h"
@@ -417,6 +417,23 @@ wl1271_scan_get_sched_scan_channels(struct wl1271 *wl,
 	int i, j;
 	u32 flags;
 	bool force_passive = !req->n_ssids;
+	u32 min_dwell_time_active, max_dwell_time_active, delta_per_probe;
+	u32 dwell_time_passive, dwell_time_dfs;
+
+	if (band == IEEE80211_BAND_5GHZ)
+		delta_per_probe = c->dwell_time_delta_per_probe_5;
+	else
+		delta_per_probe = c->dwell_time_delta_per_probe;
+
+	min_dwell_time_active = c->base_dwell_time +
+		 req->n_ssids * c->num_probe_reqs * delta_per_probe;
+
+	max_dwell_time_active = min_dwell_time_active + c->max_dwell_time_delta;
+
+	min_dwell_time_active = DIV_ROUND_UP(min_dwell_time_active, 1000);
+	max_dwell_time_active = DIV_ROUND_UP(max_dwell_time_active, 1000);
+	dwell_time_passive = DIV_ROUND_UP(c->dwell_time_passive, 1000);
+	dwell_time_dfs = DIV_ROUND_UP(c->dwell_time_dfs, 1000);
 
 	for (i = 0, j = start;
 	     i < req->n_channels && j < max_channels;
@@ -440,21 +457,24 @@ wl1271_scan_get_sched_scan_channels(struct wl1271 *wl,
 				     req->channels[i]->flags);
 			wl1271_debug(DEBUG_SCAN, "max_power %d",
 				     req->channels[i]->max_power);
+			wl1271_debug(DEBUG_SCAN, "min_dwell_time %d max dwell time %d",
+				     min_dwell_time_active,
+				     max_dwell_time_active);
 
 			if (flags & IEEE80211_CHAN_RADAR) {
 				channels[j].flags |= SCAN_CHANNEL_FLAGS_DFS;
 
 				channels[j].passive_duration =
-					cpu_to_le16(c->dwell_time_dfs);
+					cpu_to_le16(dwell_time_dfs);
 			} else {
 				channels[j].passive_duration =
-					cpu_to_le16(c->dwell_time_passive);
+					cpu_to_le16(dwell_time_passive);
 			}
 
 			channels[j].min_duration =
-				cpu_to_le16(c->min_dwell_time_active);
+				cpu_to_le16(min_dwell_time_active);
 			channels[j].max_duration =
-				cpu_to_le16(c->max_dwell_time_active);
+				cpu_to_le16(max_dwell_time_active);
 
 			channels[j].tx_power_att = req->channels[i]->max_power;
 			channels[j].channel = req->channels[i]->hw_value;
diff --git a/drivers/net/wireless/wl12xx/scan.h b/drivers/net/wireless/ti/wlcore/scan.h
index 96ff457a3a0..81ee36ac207 100644
--- a/drivers/net/wireless/wl12xx/scan.h
+++ b/drivers/net/wireless/ti/wlcore/scan.h
@@ -24,7 +24,7 @@
 #ifndef __SCAN_H__
 #define __SCAN_H__
 
-#include "wl12xx.h"
+#include "wlcore.h"
 
 int wl1271_scan(struct wl1271 *wl, struct ieee80211_vif *vif,
 		const u8 *ssid, size_t ssid_len,
@@ -55,7 +55,7 @@ void wl1271_scan_sched_scan_results(struct wl1271 *wl);
 #define WL1271_SCAN_BAND_2_4_GHZ 0
 #define WL1271_SCAN_BAND_5_GHZ 1
 
-#define WL1271_SCAN_TIMEOUT    10000 /* msec */
+#define WL1271_SCAN_TIMEOUT    30000 /* msec */
 
 enum {
 	WL1271_SCAN_STATE_IDLE,
diff --git a/drivers/net/wireless/wl12xx/sdio.c b/drivers/net/wireless/ti/wlcore/sdio.c
index 4b3c32774ba..0a72347cfc4 100644
--- a/drivers/net/wireless/wl12xx/sdio.c
+++ b/drivers/net/wireless/ti/wlcore/sdio.c
@@ -33,7 +33,7 @@
 #include <linux/wl12xx.h>
 #include <linux/pm_runtime.h>
 
-#include "wl12xx.h"
+#include "wlcore.h"
 #include "wl12xx_80211.h"
 #include "io.h"
 
@@ -76,7 +76,7 @@ static void wl12xx_sdio_raw_read(struct device *child, int addr, void *buf,
 
 	sdio_claim_host(func);
 
-	if (unlikely(addr == HW_ACCESS_ELP_CTRL_REG_ADDR)) {
+	if (unlikely(addr == HW_ACCESS_ELP_CTRL_REG)) {
 		((u8 *)buf)[0] = sdio_f0_readb(func, addr, &ret);
 		dev_dbg(child->parent, "sdio read 52 addr 0x%x, byte 0x%02x\n",
 			addr, ((u8 *)buf)[0]);
@@ -105,7 +105,7 @@ static void wl12xx_sdio_raw_write(struct device *child, int addr, void *buf,
 
 	sdio_claim_host(func);
 
-	if (unlikely(addr == HW_ACCESS_ELP_CTRL_REG_ADDR)) {
+	if (unlikely(addr == HW_ACCESS_ELP_CTRL_REG)) {
 		sdio_f0_writeb(func, ((u8 *)buf)[0], addr, &ret);
 		dev_dbg(child->parent, "sdio write 52 addr 0x%x, byte 0x%02x\n",
 			addr, ((u8 *)buf)[0]);
diff --git a/drivers/net/wireless/wl12xx/spi.c b/drivers/net/wireless/ti/wlcore/spi.c
index 2fc18a8dcce..553cd3cbb98 100644
--- a/drivers/net/wireless/wl12xx/spi.c
+++ b/drivers/net/wireless/ti/wlcore/spi.c
@@ -30,12 +30,10 @@
 #include <linux/platform_device.h>
 #include <linux/slab.h>
 
-#include "wl12xx.h"
+#include "wlcore.h"
 #include "wl12xx_80211.h"
 #include "io.h"
 
-#include "reg.h"
-
 #define WSPI_CMD_READ                 0x40000000
 #define WSPI_CMD_WRITE                0x00000000
 #define WSPI_CMD_FIXED                0x20000000
diff --git a/drivers/net/wireless/wl12xx/testmode.c b/drivers/net/wireless/ti/wlcore/testmode.c
index 1e93bb9c024..0e59ea2cdd3 100644
--- a/drivers/net/wireless/wl12xx/testmode.c
+++ b/drivers/net/wireless/ti/wlcore/testmode.c
@@ -25,10 +25,9 @@
 #include <linux/slab.h>
 #include <net/genetlink.h>
 
-#include "wl12xx.h"
+#include "wlcore.h"
 #include "debug.h"
 #include "acx.h"
-#include "reg.h"
 #include "ps.h"
 #include "io.h"
 
@@ -116,7 +115,8 @@ static int wl1271_tm_cmd_test(struct wl1271 *wl, struct nlattr *tb[])
 			goto out_sleep;
 		}
 
-		NLA_PUT(skb, WL1271_TM_ATTR_DATA, buf_len, buf);
+		if (nla_put(skb, WL1271_TM_ATTR_DATA, buf_len, buf))
+			goto nla_put_failure;
 		ret = cfg80211_testmode_reply(skb);
 		if (ret < 0)
 			goto out_sleep;
@@ -178,7 +178,8 @@ static int wl1271_tm_cmd_interrogate(struct wl1271 *wl, struct nlattr *tb[])
 		goto out_free;
 	}
 
-	NLA_PUT(skb, WL1271_TM_ATTR_DATA, sizeof(*cmd), cmd);
+	if (nla_put(skb, WL1271_TM_ATTR_DATA, sizeof(*cmd), cmd))
+		goto nla_put_failure;
 	ret = cfg80211_testmode_reply(skb);
 	if (ret < 0)
 		goto out_free;
@@ -297,7 +298,8 @@ static int wl12xx_tm_cmd_get_mac(struct wl1271 *wl, struct nlattr *tb[])
 		goto out;
 	}
 
-	NLA_PUT(skb, WL1271_TM_ATTR_DATA, ETH_ALEN, mac_addr);
+	if (nla_put(skb, WL1271_TM_ATTR_DATA, ETH_ALEN, mac_addr))
+		goto nla_put_failure;
 	ret = cfg80211_testmode_reply(skb);
 	if (ret < 0)
 		goto out;
diff --git a/drivers/net/wireless/wl12xx/testmode.h b/drivers/net/wireless/ti/wlcore/testmode.h
index 8071654259e..8071654259e 100644
--- a/drivers/net/wireless/wl12xx/testmode.h
+++ b/drivers/net/wireless/ti/wlcore/testmode.h
diff --git a/drivers/net/wireless/wl12xx/tx.c b/drivers/net/wireless/ti/wlcore/tx.c
index 43ae49143d6..6893bc20799 100644
--- a/drivers/net/wireless/wl12xx/tx.c
+++ b/drivers/net/wireless/ti/wlcore/tx.c
@@ -25,13 +25,19 @@
 #include <linux/module.h>
 #include <linux/etherdevice.h>
 
-#include "wl12xx.h"
+#include "wlcore.h"
 #include "debug.h"
 #include "io.h"
-#include "reg.h"
 #include "ps.h"
 #include "tx.h"
 #include "event.h"
+#include "hw_ops.h"
+
+/*
+ * TODO: this is here just for now, it must be removed when the data
+ * operations are in place.
+ */
+#include "../wl12xx/reg.h"
 
 static int wl1271_set_default_wep_key(struct wl1271 *wl,
 				      struct wl12xx_vif *wlvif, u8 id)
@@ -56,8 +62,8 @@ static int wl1271_alloc_tx_id(struct wl1271 *wl, struct sk_buff *skb)
 {
 	int id;
 
-	id = find_first_zero_bit(wl->tx_frames_map, ACX_TX_DESCRIPTORS);
-	if (id >= ACX_TX_DESCRIPTORS)
+	id = find_first_zero_bit(wl->tx_frames_map, wl->num_tx_desc);
+	if (id >= wl->num_tx_desc)
 		return -EBUSY;
 
 	__set_bit(id, wl->tx_frames_map);
@@ -69,7 +75,7 @@ static int wl1271_alloc_tx_id(struct wl1271 *wl, struct sk_buff *skb)
 static void wl1271_free_tx_id(struct wl1271 *wl, int id)
 {
 	if (__test_and_clear_bit(id, wl->tx_frames_map)) {
-		if (unlikely(wl->tx_frames_cnt == ACX_TX_DESCRIPTORS))
+		if (unlikely(wl->tx_frames_cnt == wl->num_tx_desc))
 			clear_bit(WL1271_FLAG_FW_TX_BUSY, &wl->flags);
 
 		wl->tx_frames[id] = NULL;
@@ -167,14 +173,15 @@ u8 wl12xx_tx_get_hlid(struct wl1271 *wl, struct wl12xx_vif *wlvif,
 		return wlvif->dev_hlid;
 }
 
-static unsigned int wl12xx_calc_packet_alignment(struct wl1271 *wl,
-						unsigned int packet_length)
+unsigned int wlcore_calc_packet_alignment(struct wl1271 *wl,
+					  unsigned int packet_length)
 {
-	if (wl->quirks & WL12XX_QUIRK_NO_BLOCKSIZE_ALIGNMENT)
-		return ALIGN(packet_length, WL1271_TX_ALIGN_TO);
-	else
+	if (wl->quirks & WLCORE_QUIRK_TX_BLOCKSIZE_ALIGN)
 		return ALIGN(packet_length, WL12XX_BUS_BLOCK_SIZE);
+	else
+		return ALIGN(packet_length, WL1271_TX_ALIGN_TO);
 }
+EXPORT_SYMBOL(wlcore_calc_packet_alignment);
 
 static int wl1271_tx_allocate(struct wl1271 *wl, struct wl12xx_vif *wlvif,
 			      struct sk_buff *skb, u32 extra, u32 buf_offset,
@@ -182,10 +189,9 @@ static int wl1271_tx_allocate(struct wl1271 *wl, struct wl12xx_vif *wlvif,
 {
 	struct wl1271_tx_hw_descr *desc;
 	u32 total_len = skb->len + sizeof(struct wl1271_tx_hw_descr) + extra;
-	u32 len;
 	u32 total_blocks;
 	int id, ret = -EBUSY, ac;
-	u32 spare_blocks = wl->tx_spare_blocks;
+	u32 spare_blocks = wl->normal_tx_spare;
 	bool is_dummy = false;
 
 	if (buf_offset + total_len > WL1271_AGGR_BUFFER_SIZE)
@@ -196,30 +202,19 @@ static int wl1271_tx_allocate(struct wl1271 *wl, struct wl12xx_vif *wlvif,
 	if (id < 0)
 		return id;
 
-	/* approximate the number of blocks required for this packet
-	   in the firmware */
-	len = wl12xx_calc_packet_alignment(wl, total_len);
-
-	/* in case of a dummy packet, use default amount of spare mem blocks */
-	if (unlikely(wl12xx_is_dummy_packet(wl, skb))) {
+	if (unlikely(wl12xx_is_dummy_packet(wl, skb)))
 		is_dummy = true;
-		spare_blocks = TX_HW_BLOCK_SPARE_DEFAULT;
-	}
+	else if (wlvif->is_gem)
+		spare_blocks = wl->gem_tx_spare;
 
-	total_blocks = (len + TX_HW_BLOCK_SIZE - 1) / TX_HW_BLOCK_SIZE +
-		spare_blocks;
+	total_blocks = wlcore_hw_calc_tx_blocks(wl, total_len, spare_blocks);
 
 	if (total_blocks <= wl->tx_blocks_available) {
 		desc = (struct wl1271_tx_hw_descr *)skb_push(
 			skb, total_len - skb->len);
 
-		/* HW descriptor fields change between wl127x and wl128x */
-		if (wl->chip.id == CHIP_ID_1283_PG20) {
-			desc->wl128x_mem.total_mem_blocks = total_blocks;
-		} else {
-			desc->wl127x_mem.extra_blocks = spare_blocks;
-			desc->wl127x_mem.total_mem_blocks = total_blocks;
-		}
+		wlcore_hw_set_tx_desc_blocks(wl, desc, total_blocks,
+					     spare_blocks);
 
 		desc->id = id;
 
@@ -256,7 +251,7 @@ static void wl1271_tx_fill_hdr(struct wl1271 *wl, struct wl12xx_vif *wlvif,
 {
 	struct timespec ts;
 	struct wl1271_tx_hw_descr *desc;
-	int aligned_len, ac, rate_idx;
+	int ac, rate_idx;
 	s64 hosttime;
 	u16 tx_attr = 0;
 	__le16 frame_control;
@@ -329,44 +324,16 @@ static void wl1271_tx_fill_hdr(struct wl1271 *wl, struct wl12xx_vif *wlvif,
 	}
 
 	tx_attr |= rate_idx << TX_HW_ATTR_OFST_RATE_POLICY;
-	desc->reserved = 0;
-
-	aligned_len = wl12xx_calc_packet_alignment(wl, skb->len);
-
-	if (wl->chip.id == CHIP_ID_1283_PG20) {
-		desc->wl128x_mem.extra_bytes = aligned_len - skb->len;
-		desc->length = cpu_to_le16(aligned_len >> 2);
-
-		wl1271_debug(DEBUG_TX, "tx_fill_hdr: hlid: %d "
-			     "tx_attr: 0x%x len: %d life: %d mem: %d",
-			     desc->hlid, tx_attr,
-			     le16_to_cpu(desc->length),
-			     le16_to_cpu(desc->life_time),
-			     desc->wl128x_mem.total_mem_blocks);
-	} else {
-		int pad;
-
-		/* Store the aligned length in terms of words */
-		desc->length = cpu_to_le16(aligned_len >> 2);
-
-		/* calculate number of padding bytes */
-		pad = aligned_len - skb->len;
-		tx_attr |= pad << TX_HW_ATTR_OFST_LAST_WORD_PAD;
-
-		wl1271_debug(DEBUG_TX, "tx_fill_hdr: pad: %d hlid: %d "
-			     "tx_attr: 0x%x len: %d life: %d mem: %d", pad,
-			     desc->hlid, tx_attr,
-			     le16_to_cpu(desc->length),
-			     le16_to_cpu(desc->life_time),
-			     desc->wl127x_mem.total_mem_blocks);
-	}
 
 	/* for WEP shared auth - no fw encryption is needed */
 	if (ieee80211_is_auth(frame_control) &&
 	    ieee80211_has_protected(frame_control))
 		tx_attr |= TX_HW_ATTR_HOST_ENCRYPT;
 
+	desc->reserved = 0;
 	desc->tx_attr = cpu_to_le16(tx_attr);
+
+	wlcore_hw_set_tx_desc_data_len(wl, desc, skb);
 }
 
 /* caller must hold wl->mutex */
@@ -432,7 +399,7 @@ static int wl1271_prepare_tx_frame(struct wl1271 *wl, struct wl12xx_vif *wlvif,
 	 * In special cases, we want to align to a specific block size
 	 * (eg. for wl128x with SDIO we align to 256).
 	 */
-	total_len = wl12xx_calc_packet_alignment(wl, skb->len);
+	total_len = wlcore_calc_packet_alignment(wl, skb->len);
 
 	memcpy(wl->aggr_buf + buf_offset, skb->data, skb->len);
 	memset(wl->aggr_buf + buf_offset + skb->len, 0, total_len - skb->len);
@@ -718,8 +685,8 @@ void wl1271_tx_work_locked(struct wl1271 *wl)
 			 * Flush buffer and try again.
 			 */
 			wl1271_skb_queue_head(wl, wlvif, skb);
-			wl1271_write(wl, WL1271_SLV_MEM_DATA, wl->aggr_buf,
-				     buf_offset, true);
+			wlcore_write_data(wl, REG_SLV_MEM_DATA, wl->aggr_buf,
+					  buf_offset, true);
 			sent_packets = true;
 			buf_offset = 0;
 			continue;
@@ -753,8 +720,8 @@ void wl1271_tx_work_locked(struct wl1271 *wl)
 
 out_ack:
 	if (buf_offset) {
-		wl1271_write(wl, WL1271_SLV_MEM_DATA, wl->aggr_buf,
-				buf_offset, true);
+		wlcore_write_data(wl, REG_SLV_MEM_DATA, wl->aggr_buf,
+				  buf_offset, true);
 		sent_packets = true;
 	}
 	if (sent_packets) {
@@ -762,8 +729,8 @@ out_ack:
 		 * Interrupt the firmware with the new packets. This is only
 		 * required for older hardware revisions
 		 */
-		if (wl->quirks & WL12XX_QUIRK_END_OF_TRANSACTION)
-			wl1271_write32(wl, WL1271_HOST_WR_ACCESS,
+		if (wl->quirks & WLCORE_QUIRK_END_OF_TRANSACTION)
+			wl1271_write32(wl, WL12XX_HOST_WR_ACCESS,
 				       wl->tx_packets_count);
 
 		wl1271_handle_tx_low_watermark(wl);
@@ -792,11 +759,20 @@ static u8 wl1271_tx_get_rate_flags(u8 rate_class_index)
 {
 	u8 flags = 0;
 
-	if (rate_class_index >= CONF_HW_RXTX_RATE_MCS_MIN &&
-	    rate_class_index <= CONF_HW_RXTX_RATE_MCS_MAX)
+	/*
+	 * TODO: use wl12xx constants when this code is moved to wl12xx, as
+	 * only it uses Tx-completion.
+	 */
+	if (rate_class_index <= 8)
 		flags |= IEEE80211_TX_RC_MCS;
-	if (rate_class_index == CONF_HW_RXTX_RATE_MCS7_SGI)
+
+	/*
+	 * TODO: use wl12xx constants when this code is moved to wl12xx, as
+	 * only it uses Tx-completion.
+	 */
+	if (rate_class_index == 0)
 		flags |= IEEE80211_TX_RC_SHORT_GI;
+
 	return flags;
 }
 
@@ -813,7 +789,7 @@ static void wl1271_tx_complete_packet(struct wl1271 *wl,
 	u8 retries = 0;
 
 	/* check for id legality */
-	if (unlikely(id >= ACX_TX_DESCRIPTORS || wl->tx_frames[id] == NULL)) {
+	if (unlikely(id >= wl->num_tx_desc || wl->tx_frames[id] == NULL)) {
 		wl1271_warning("TX result illegal id: %d", id);
 		return;
 	}
@@ -834,7 +810,7 @@ static void wl1271_tx_complete_packet(struct wl1271 *wl,
 	if (result->status == TX_SUCCESS) {
 		if (!(info->flags & IEEE80211_TX_CTL_NO_ACK))
 			info->flags |= IEEE80211_TX_STAT_ACK;
-		rate = wl1271_rate_to_idx(result->rate_class_index,
+		rate = wlcore_rate_to_idx(wl, result->rate_class_index,
 					  wlvif->band);
 		rate_flags = wl1271_tx_get_rate_flags(result->rate_class_index);
 		retries = result->ack_failures;
@@ -929,6 +905,7 @@ void wl1271_tx_complete(struct wl1271 *wl)
 		wl->tx_results_count++;
 	}
 }
+EXPORT_SYMBOL(wl1271_tx_complete);
 
 void wl1271_tx_reset_link_queues(struct wl1271 *wl, u8 hlid)
 {
@@ -1006,7 +983,7 @@ void wl12xx_tx_reset(struct wl1271 *wl, bool reset_tx_queues)
 	if (reset_tx_queues)
 		wl1271_handle_tx_low_watermark(wl);
 
-	for (i = 0; i < ACX_TX_DESCRIPTORS; i++) {
+	for (i = 0; i < wl->num_tx_desc; i++) {
 		if (wl->tx_frames[i] == NULL)
 			continue;
 
diff --git a/drivers/net/wireless/wl12xx/tx.h b/drivers/net/wireless/ti/wlcore/tx.h
index 5cf8c32d40d..2fd6e5dc6f7 100644
--- a/drivers/net/wireless/wl12xx/tx.h
+++ b/drivers/net/wireless/ti/wlcore/tx.h
@@ -25,9 +25,6 @@
 #ifndef __TX_H__
 #define __TX_H__
 
-#define TX_HW_BLOCK_SPARE_DEFAULT        1
-#define TX_HW_BLOCK_SIZE                 252
-
 #define TX_HW_MGMT_PKT_LIFETIME_TU       2000
 #define TX_HW_AP_MODE_PKT_LIFETIME_TU    8000
 
@@ -212,7 +209,7 @@ void wl1271_tx_complete(struct wl1271 *wl);
 void wl12xx_tx_reset_wlvif(struct wl1271 *wl, struct wl12xx_vif *wlvif);
 void wl12xx_tx_reset(struct wl1271 *wl, bool reset_tx_queues);
 void wl1271_tx_flush(struct wl1271 *wl);
-u8 wl1271_rate_to_idx(int rate, enum ieee80211_band band);
+u8 wlcore_rate_to_idx(struct wl1271 *wl, u8 rate, enum ieee80211_band band);
 u32 wl1271_tx_enabled_rates_get(struct wl1271 *wl, u32 rate_set,
 				enum ieee80211_band rate_band);
 u32 wl1271_tx_min_rate_get(struct wl1271 *wl, u32 rate_set);
@@ -224,6 +221,8 @@ void wl1271_tx_reset_link_queues(struct wl1271 *wl, u8 hlid);
 void wl1271_handle_tx_low_watermark(struct wl1271 *wl);
 bool wl12xx_is_dummy_packet(struct wl1271 *wl, struct sk_buff *skb);
 void wl12xx_rearm_rx_streaming(struct wl1271 *wl, unsigned long *active_hlids);
+unsigned int wlcore_calc_packet_alignment(struct wl1271 *wl,
+					  unsigned int packet_length);
 
 /* from main.c */
 void wl1271_free_sta(struct wl1271 *wl, struct wl12xx_vif *wlvif, u8 hlid);
diff --git a/drivers/net/wireless/wl12xx/wl12xx.h b/drivers/net/wireless/ti/wlcore/wl12xx.h
index 749a15a75d3..f12bdf74518 100644
--- a/drivers/net/wireless/wl12xx/wl12xx.h
+++ b/drivers/net/wireless/ti/wlcore/wl12xx.h
@@ -89,8 +89,6 @@
 #define WL1271_AP_BSS_INDEX        0
 #define WL1271_AP_DEF_BEACON_EXP   20
 
-#define ACX_TX_DESCRIPTORS         16
-
 #define WL1271_AGGR_BUFFER_SIZE (4 * PAGE_SIZE)
 
 enum wl1271_state {
@@ -105,26 +103,6 @@ enum wl12xx_fw_type {
 	WL12XX_FW_TYPE_PLT,
 };
 
-enum wl1271_partition_type {
-	PART_DOWN,
-	PART_WORK,
-	PART_DRPW,
-
-	PART_TABLE_LEN
-};
-
-struct wl1271_partition {
-	u32 size;
-	u32 start;
-};
-
-struct wl1271_partition_set {
-	struct wl1271_partition mem;
-	struct wl1271_partition reg;
-	struct wl1271_partition mem2;
-	struct wl1271_partition mem3;
-};
-
 struct wl1271;
 
 enum {
@@ -167,8 +145,21 @@ struct wl1271_stats {
 
 #define AP_MAX_STATIONS            8
 
+struct wl_fw_packet_counters {
+	/* Cumulative counter of released packets per AC */
+	u8 tx_released_pkts[NUM_TX_QUEUES];
+
+	/* Cumulative counter of freed packets per HLID */
+	u8 tx_lnk_free_pkts[WL12XX_MAX_LINKS];
+
+	/* Cumulative counter of released Voice memory blocks */
+	u8 tx_voice_released_blks;
+
+	u8 padding[3];
+} __packed;
+
 /* FW status registers */
-struct wl12xx_fw_status {
+struct wl_fw_status {
 	__le32 intr;
 	u8  fw_rx_counter;
 	u8  drv_rx_counter;
@@ -195,16 +186,12 @@ struct wl12xx_fw_status {
 	/* Size (in Memory Blocks) of TX pool */
 	__le32 tx_total;
 
-	/* Cumulative counter of released packets per AC */
-	u8 tx_released_pkts[NUM_TX_QUEUES];
-
-	/* Cumulative counter of freed packets per HLID */
-	u8 tx_lnk_free_pkts[WL12XX_MAX_LINKS];
+	struct wl_fw_packet_counters counters;
 
-	/* Cumulative counter of released Voice memory blocks */
-	u8 tx_voice_released_blks;
-	u8 padding_1[3];
 	__le32 log_start_addr;
+
+	/* Private status to be used by the lower drivers */
+	u8 priv[0];
 } __packed;
 
 struct wl1271_rx_mem_pool_addr {
@@ -292,212 +279,37 @@ struct wl1271_link {
 	u8 ba_bitmap;
 };
 
-struct wl1271 {
-	struct ieee80211_hw *hw;
-	bool mac80211_registered;
-
-	struct device *dev;
-
-	void *if_priv;
-
-	struct wl1271_if_operations *if_ops;
-
-	void (*set_power)(bool enable);
-	int irq;
-	int ref_clock;
-
-	spinlock_t wl_lock;
-
-	enum wl1271_state state;
-	enum wl12xx_fw_type fw_type;
-	bool plt;
-	u8 last_vif_count;
-	struct mutex mutex;
-
-	unsigned long flags;
-
-	struct wl1271_partition_set part;
-
-	struct wl1271_chip chip;
-
-	int cmd_box_addr;
-	int event_box_addr;
-
-	u8 *fw;
-	size_t fw_len;
-	void *nvs;
-	size_t nvs_len;
-
-	s8 hw_pg_ver;
-
-	/* address read from the fuse ROM */
-	u32 fuse_oui_addr;
-	u32 fuse_nic_addr;
-
-	/* we have up to 2 MAC addresses */
-	struct mac_address addresses[2];
-	int channel;
-	u8 system_hlid;
-
-	unsigned long links_map[BITS_TO_LONGS(WL12XX_MAX_LINKS)];
-	unsigned long roles_map[BITS_TO_LONGS(WL12XX_MAX_ROLES)];
-	unsigned long roc_map[BITS_TO_LONGS(WL12XX_MAX_ROLES)];
-	unsigned long rate_policies_map[
-			BITS_TO_LONGS(WL12XX_MAX_RATE_POLICIES)];
-
-	struct list_head wlvif_list;
-
-	u8 sta_count;
-	u8 ap_count;
-
-	struct wl1271_acx_mem_map *target_mem_map;
-
-	/* Accounting for allocated / available TX blocks on HW */
-	u32 tx_blocks_freed;
-	u32 tx_blocks_available;
-	u32 tx_allocated_blocks;
-	u32 tx_results_count;
-
-	/* amount of spare TX blocks to use */
-	u32 tx_spare_blocks;
-
-	/* Accounting for allocated / available Tx packets in HW */
-	u32 tx_pkts_freed[NUM_TX_QUEUES];
-	u32 tx_allocated_pkts[NUM_TX_QUEUES];
-
-	/* Transmitted TX packets counter for chipset interface */
-	u32 tx_packets_count;
+#define WL1271_MAX_RX_FILTERS 5
+#define WL1271_RX_FILTER_MAX_FIELDS 8
 
-	/* Time-offset between host and chipset clocks */
-	s64 time_offset;
+#define WL1271_RX_FILTER_ETH_HEADER_SIZE 14
+#define WL1271_RX_FILTER_MAX_FIELDS_SIZE 95
+#define RX_FILTER_FIELD_OVERHEAD				\
+	(sizeof(struct wl12xx_rx_filter_field) - sizeof(u8 *))
+#define WL1271_RX_FILTER_MAX_PATTERN_SIZE			\
+	(WL1271_RX_FILTER_MAX_FIELDS_SIZE - RX_FILTER_FIELD_OVERHEAD)
 
-	/* Frames scheduled for transmission, not handled yet */
-	int tx_queue_count[NUM_TX_QUEUES];
-	long stopped_queues_map;
+#define WL1271_RX_FILTER_FLAG_MASK                BIT(0)
+#define WL1271_RX_FILTER_FLAG_IP_HEADER           0
+#define WL1271_RX_FILTER_FLAG_ETHERNET_HEADER     BIT(1)
 
-	/* Frames received, not handled yet by mac80211 */
-	struct sk_buff_head deferred_rx_queue;
-
-	/* Frames sent, not returned yet to mac80211 */
-	struct sk_buff_head deferred_tx_queue;
-
-	struct work_struct tx_work;
-	struct workqueue_struct *freezable_wq;
-
-	/* Pending TX frames */
-	unsigned long tx_frames_map[BITS_TO_LONGS(ACX_TX_DESCRIPTORS)];
-	struct sk_buff *tx_frames[ACX_TX_DESCRIPTORS];
-	int tx_frames_cnt;
-
-	/* FW Rx counter */
-	u32 rx_counter;
-
-	/* Rx memory pool address */
-	struct wl1271_rx_mem_pool_addr rx_mem_pool_addr;
-
-	/* Intermediate buffer, used for packet aggregation */
-	u8 *aggr_buf;
-
-	/* Reusable dummy packet template */
-	struct sk_buff *dummy_packet;
-
-	/* Network stack work  */
-	struct work_struct netstack_work;
-
-	/* FW log buffer */
-	u8 *fwlog;
-
-	/* Number of valid bytes in the FW log buffer */
-	ssize_t fwlog_size;
-
-	/* Sysfs FW log entry readers wait queue */
-	wait_queue_head_t fwlog_waitq;
-
-	/* Hardware recovery work */
-	struct work_struct recovery_work;
-
-	/* The mbox event mask */
-	u32 event_mask;
-
-	/* Mailbox pointers */
-	u32 mbox_ptr[2];
-
-	/* Are we currently scanning */
-	struct ieee80211_vif *scan_vif;
-	struct wl1271_scan scan;
-	struct delayed_work scan_complete_work;
-
-	bool sched_scanning;
-
-	/* The current band */
-	enum ieee80211_band band;
-
-	struct completion *elp_compl;
-	struct delayed_work elp_work;
-
-	/* in dBm */
-	int power_level;
-
-	struct wl1271_stats stats;
-
-	__le32 buffer_32;
-	u32 buffer_cmd;
-	u32 buffer_busyword[WL1271_BUSY_WORD_CNT];
-
-	struct wl12xx_fw_status *fw_status;
-	struct wl1271_tx_hw_res_if *tx_res_if;
-
-	/* Current chipset configuration */
-	struct conf_drv_settings conf;
-
-	bool sg_enabled;
-
-	bool enable_11a;
-
-	/* Most recently reported noise in dBm */
-	s8 noise;
-
-	/* bands supported by this instance of wl12xx */
-	struct ieee80211_supported_band bands[IEEE80211_NUM_BANDS];
-
-	int tcxo_clock;
-
-	/*
-	 * wowlan trigger was configured during suspend.
-	 * (currently, only "ANY" trigger is supported)
-	 */
-	bool wow_enabled;
-	bool irq_wake_enabled;
-
-	/*
-	 * AP-mode - links indexed by HLID. The global and broadcast links
-	 * are always active.
-	 */
-	struct wl1271_link links[WL12XX_MAX_LINKS];
-
-	/* AP-mode - a bitmap of links currently in PS mode according to FW */
-	u32 ap_fw_ps_map;
-
-	/* AP-mode - a bitmap of links currently in PS mode in mac80211 */
-	unsigned long ap_ps_map;
-
-	/* Quirks of specific hardware revisions */
-	unsigned int quirks;
-
-	/* Platform limitations */
-	unsigned int platform_quirks;
-
-	/* number of currently active RX BA sessions */
-	int ba_rx_session_count;
-
-	/* AP-mode - number of currently connected stations */
-	int active_sta_count;
+enum rx_filter_action {
+	FILTER_DROP = 0,
+	FILTER_SIGNAL = 1,
+	FILTER_FW_HANDLE = 2
+};
 
-	/* last wlvif we transmitted from */
-	struct wl12xx_vif *last_wlvif;
+struct wl12xx_rx_filter_field {
+	__le16 offset;
+	u8 len;
+	u8 flags;
+	u8 *pattern;
+} __packed;
 
-	/* work to fire when Tx is stuck */
-	struct delayed_work tx_watchdog_work;
+struct wl12xx_rx_filter {
+	u8 action;
+	int num_fields;
+	struct wl12xx_rx_filter_field fields[WL1271_RX_FILTER_MAX_FIELDS];
 };
 
 struct wl1271_station {
@@ -605,6 +417,9 @@ struct wl12xx_vif {
 	struct work_struct rx_streaming_disable_work;
 	struct timer_list rx_streaming_timer;
 
+	/* does the current role use GEM for encryption (AP or STA) */
+	bool is_gem;
+
 	/*
 	 * This struct must be last!
 	 * data that has to be saved acrossed reconfigs (e.g. recovery)
@@ -657,6 +472,14 @@ int wl1271_plt_stop(struct wl1271 *wl);
 int wl1271_recalc_rx_streaming(struct wl1271 *wl, struct wl12xx_vif *wlvif);
 void wl12xx_queue_recovery_work(struct wl1271 *wl);
 size_t wl12xx_copy_fwlog(struct wl1271 *wl, u8 *memblock, size_t maxlen);
+int wl1271_rx_filter_alloc_field(struct wl12xx_rx_filter *filter,
+					u16 offset, u8 flags,
+					u8 *pattern, u8 len);
+void wl1271_rx_filter_free(struct wl12xx_rx_filter *filter);
+struct wl12xx_rx_filter *wl1271_rx_filter_alloc(void);
+int wl1271_rx_filter_get_fields_size(struct wl12xx_rx_filter *filter);
+void wl1271_rx_filter_flatten_fields(struct wl12xx_rx_filter *filter,
+				     u8 *buf);
 
 #define JOIN_TIMEOUT 5000 /* 5000 milliseconds to join */
 
@@ -679,17 +502,6 @@ size_t wl12xx_copy_fwlog(struct wl1271 *wl, u8 *memblock, size_t maxlen);
 #define HW_BG_RATES_MASK	0xffff
 #define HW_HT_RATES_OFFSET	16
 
-/* Quirks */
-
-/* Each RX/TX transaction requires an end-of-transaction transfer */
-#define WL12XX_QUIRK_END_OF_TRANSACTION		BIT(0)
-
-/* wl127x and SPI don't support SDIO block size alignment */
-#define WL12XX_QUIRK_NO_BLOCKSIZE_ALIGNMENT	BIT(2)
-
-/* Older firmwares did not implement the FW logger over bus feature */
-#define WL12XX_QUIRK_FWLOG_NOT_IMPLEMENTED	BIT(4)
-
 #define WL12XX_HW_BLOCK_SIZE	256
 
 #endif
diff --git a/drivers/net/wireless/wl12xx/wl12xx_80211.h b/drivers/net/wireless/ti/wlcore/wl12xx_80211.h
index 22b0bc98d7b..22b0bc98d7b 100644
--- a/drivers/net/wireless/wl12xx/wl12xx_80211.h
+++ b/drivers/net/wireless/ti/wlcore/wl12xx_80211.h
diff --git a/drivers/net/wireless/wl12xx/wl12xx_platform_data.c b/drivers/net/wireless/ti/wlcore/wl12xx_platform_data.c
index 998e95895f9..998e95895f9 100644
--- a/drivers/net/wireless/wl12xx/wl12xx_platform_data.c
+++ b/drivers/net/wireless/ti/wlcore/wl12xx_platform_data.c
diff --git a/drivers/net/wireless/ti/wlcore/wlcore.h b/drivers/net/wireless/ti/wlcore/wlcore.h
new file mode 100644
index 00000000000..0b3f0b586f4
--- /dev/null
+++ b/drivers/net/wireless/ti/wlcore/wlcore.h
@@ -0,0 +1,454 @@
+/*
+ * This file is part of wlcore
+ *
+ * Copyright (C) 2011 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.
+ *
+ * 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
+ *
+ */
+
+#ifndef __WLCORE_H__
+#define __WLCORE_H__
+
+#include <linux/platform_device.h>
+
+#include "wl12xx.h"
+#include "event.h"
+
+/* The maximum number of Tx descriptors in all chip families */
+#define WLCORE_MAX_TX_DESCRIPTORS 32
+
+/* forward declaration */
+struct wl1271_tx_hw_descr;
+enum wl_rx_buf_align;
+
+struct wlcore_ops {
+	int (*identify_chip)(struct wl1271 *wl);
+	int (*identify_fw)(struct wl1271 *wl);
+	int (*boot)(struct wl1271 *wl);
+	void (*trigger_cmd)(struct wl1271 *wl, int cmd_box_addr,
+			    void *buf, size_t len);
+	void (*ack_event)(struct wl1271 *wl);
+	u32 (*calc_tx_blocks)(struct wl1271 *wl, u32 len, u32 spare_blks);
+	void (*set_tx_desc_blocks)(struct wl1271 *wl,
+				   struct wl1271_tx_hw_descr *desc,
+				   u32 blks, u32 spare_blks);
+	void (*set_tx_desc_data_len)(struct wl1271 *wl,
+				     struct wl1271_tx_hw_descr *desc,
+				     struct sk_buff *skb);
+	enum wl_rx_buf_align (*get_rx_buf_align)(struct wl1271 *wl,
+						 u32 rx_desc);
+	void (*prepare_read)(struct wl1271 *wl, u32 rx_desc, u32 len);
+	u32 (*get_rx_packet_len)(struct wl1271 *wl, void *rx_data,
+				 u32 data_len);
+	void (*tx_delayed_compl)(struct wl1271 *wl);
+	void (*tx_immediate_compl)(struct wl1271 *wl);
+	int (*hw_init)(struct wl1271 *wl);
+	int (*init_vif)(struct wl1271 *wl, struct wl12xx_vif *wlvif);
+	u32 (*sta_get_ap_rate_mask)(struct wl1271 *wl,
+				    struct wl12xx_vif *wlvif);
+	s8 (*get_pg_ver)(struct wl1271 *wl);
+	void (*get_mac)(struct wl1271 *wl);
+};
+
+enum wlcore_partitions {
+	PART_DOWN,
+	PART_WORK,
+	PART_BOOT,
+	PART_DRPW,
+	PART_TOP_PRCM_ELP_SOC,
+	PART_PHY_INIT,
+
+	PART_TABLE_LEN,
+};
+
+struct wlcore_partition {
+	u32 size;
+	u32 start;
+};
+
+struct wlcore_partition_set {
+	struct wlcore_partition mem;
+	struct wlcore_partition reg;
+	struct wlcore_partition mem2;
+	struct wlcore_partition mem3;
+};
+
+enum wlcore_registers {
+	/* register addresses, used with partition translation */
+	REG_ECPU_CONTROL,
+	REG_INTERRUPT_NO_CLEAR,
+	REG_INTERRUPT_ACK,
+	REG_COMMAND_MAILBOX_PTR,
+	REG_EVENT_MAILBOX_PTR,
+	REG_INTERRUPT_TRIG,
+	REG_INTERRUPT_MASK,
+	REG_PC_ON_RECOVERY,
+	REG_CHIP_ID_B,
+	REG_CMD_MBOX_ADDRESS,
+
+	/* data access memory addresses, used with partition translation */
+	REG_SLV_MEM_DATA,
+	REG_SLV_REG_DATA,
+
+	/* raw data access memory addresses */
+	REG_RAW_FW_STATUS_ADDR,
+
+	REG_TABLE_LEN,
+};
+
+struct wl1271 {
+	struct ieee80211_hw *hw;
+	bool mac80211_registered;
+
+	struct device *dev;
+
+	void *if_priv;
+
+	struct wl1271_if_operations *if_ops;
+
+	void (*set_power)(bool enable);
+	int irq;
+	int ref_clock;
+
+	spinlock_t wl_lock;
+
+	enum wl1271_state state;
+	enum wl12xx_fw_type fw_type;
+	bool plt;
+	u8 last_vif_count;
+	struct mutex mutex;
+
+	unsigned long flags;
+
+	struct wlcore_partition_set curr_part;
+
+	struct wl1271_chip chip;
+
+	int cmd_box_addr;
+
+	u8 *fw;
+	size_t fw_len;
+	void *nvs;
+	size_t nvs_len;
+
+	s8 hw_pg_ver;
+
+	/* address read from the fuse ROM */
+	u32 fuse_oui_addr;
+	u32 fuse_nic_addr;
+
+	/* we have up to 2 MAC addresses */
+	struct mac_address addresses[2];
+	int channel;
+	u8 system_hlid;
+
+	unsigned long links_map[BITS_TO_LONGS(WL12XX_MAX_LINKS)];
+	unsigned long roles_map[BITS_TO_LONGS(WL12XX_MAX_ROLES)];
+	unsigned long roc_map[BITS_TO_LONGS(WL12XX_MAX_ROLES)];
+	unsigned long rate_policies_map[
+			BITS_TO_LONGS(WL12XX_MAX_RATE_POLICIES)];
+
+	struct list_head wlvif_list;
+
+	u8 sta_count;
+	u8 ap_count;
+
+	struct wl1271_acx_mem_map *target_mem_map;
+
+	/* Accounting for allocated / available TX blocks on HW */
+	u32 tx_blocks_freed;
+	u32 tx_blocks_available;
+	u32 tx_allocated_blocks;
+	u32 tx_results_count;
+
+	/* Accounting for allocated / available Tx packets in HW */
+	u32 tx_pkts_freed[NUM_TX_QUEUES];
+	u32 tx_allocated_pkts[NUM_TX_QUEUES];
+
+	/* Transmitted TX packets counter for chipset interface */
+	u32 tx_packets_count;
+
+	/* Time-offset between host and chipset clocks */
+	s64 time_offset;
+
+	/* Frames scheduled for transmission, not handled yet */
+	int tx_queue_count[NUM_TX_QUEUES];
+	long stopped_queues_map;
+
+	/* Frames received, not handled yet by mac80211 */
+	struct sk_buff_head deferred_rx_queue;
+
+	/* Frames sent, not returned yet to mac80211 */
+	struct sk_buff_head deferred_tx_queue;
+
+	struct work_struct tx_work;
+	struct workqueue_struct *freezable_wq;
+
+	/* Pending TX frames */
+	unsigned long tx_frames_map[BITS_TO_LONGS(WLCORE_MAX_TX_DESCRIPTORS)];
+	struct sk_buff *tx_frames[WLCORE_MAX_TX_DESCRIPTORS];
+	int tx_frames_cnt;
+
+	/* FW Rx counter */
+	u32 rx_counter;
+
+	/* Rx memory pool address */
+	struct wl1271_rx_mem_pool_addr rx_mem_pool_addr;
+
+	/* Intermediate buffer, used for packet aggregation */
+	u8 *aggr_buf;
+
+	/* Reusable dummy packet template */
+	struct sk_buff *dummy_packet;
+
+	/* Network stack work  */
+	struct work_struct netstack_work;
+
+	/* FW log buffer */
+	u8 *fwlog;
+
+	/* Number of valid bytes in the FW log buffer */
+	ssize_t fwlog_size;
+
+	/* Sysfs FW log entry readers wait queue */
+	wait_queue_head_t fwlog_waitq;
+
+	/* Hardware recovery work */
+	struct work_struct recovery_work;
+
+	/* Pointer that holds DMA-friendly block for the mailbox */
+	struct event_mailbox *mbox;
+
+	/* The mbox event mask */
+	u32 event_mask;
+
+	/* Mailbox pointers */
+	u32 mbox_ptr[2];
+
+	/* Are we currently scanning */
+	struct ieee80211_vif *scan_vif;
+	struct wl1271_scan scan;
+	struct delayed_work scan_complete_work;
+
+	/* Connection loss work */
+	struct delayed_work connection_loss_work;
+
+	bool sched_scanning;
+
+	/* The current band */
+	enum ieee80211_band band;
+
+	struct completion *elp_compl;
+	struct delayed_work elp_work;
+
+	/* in dBm */
+	int power_level;
+
+	struct wl1271_stats stats;
+
+	__le32 buffer_32;
+	u32 buffer_cmd;
+	u32 buffer_busyword[WL1271_BUSY_WORD_CNT];
+
+	struct wl_fw_status *fw_status;
+	struct wl1271_tx_hw_res_if *tx_res_if;
+
+	/* Current chipset configuration */
+	struct wlcore_conf conf;
+
+	bool sg_enabled;
+
+	bool enable_11a;
+
+	/* Most recently reported noise in dBm */
+	s8 noise;
+
+	/* bands supported by this instance of wl12xx */
+	struct ieee80211_supported_band bands[IEEE80211_NUM_BANDS];
+
+	int tcxo_clock;
+
+	/*
+	 * wowlan trigger was configured during suspend.
+	 * (currently, only "ANY" trigger is supported)
+	 */
+	bool wow_enabled;
+	bool irq_wake_enabled;
+
+	/*
+	 * AP-mode - links indexed by HLID. The global and broadcast links
+	 * are always active.
+	 */
+	struct wl1271_link links[WL12XX_MAX_LINKS];
+
+	/* AP-mode - a bitmap of links currently in PS mode according to FW */
+	u32 ap_fw_ps_map;
+
+	/* AP-mode - a bitmap of links currently in PS mode in mac80211 */
+	unsigned long ap_ps_map;
+
+	/* Quirks of specific hardware revisions */
+	unsigned int quirks;
+
+	/* Platform limitations */
+	unsigned int platform_quirks;
+
+	/* number of currently active RX BA sessions */
+	int ba_rx_session_count;
+
+	/* AP-mode - number of currently connected stations */
+	int active_sta_count;
+
+	/* last wlvif we transmitted from */
+	struct wl12xx_vif *last_wlvif;
+
+	/* work to fire when Tx is stuck */
+	struct delayed_work tx_watchdog_work;
+
+	struct wlcore_ops *ops;
+	/* pointer to the lower driver partition table */
+	const struct wlcore_partition_set *ptable;
+	/* pointer to the lower driver register table */
+	const int *rtable;
+	/* name of the firmwares to load - for PLT, single role, multi-role */
+	const char *plt_fw_name;
+	const char *sr_fw_name;
+	const char *mr_fw_name;
+
+	/* per-chip-family private structure */
+	void *priv;
+
+	/* number of TX descriptors the HW supports. */
+	u32 num_tx_desc;
+
+	/* spare Tx blocks for normal/GEM operating modes */
+	u32 normal_tx_spare;
+	u32 gem_tx_spare;
+
+	/* translate HW Tx rates to standard rate-indices */
+	const u8 **band_rate_to_idx;
+
+	/* size of table for HW rates that can be received from chip */
+	u8 hw_tx_rate_tbl_size;
+
+	/* this HW rate and below are considered HT rates for this chip */
+	u8 hw_min_ht_rate;
+
+	/* HW HT (11n) capabilities */
+	struct ieee80211_sta_ht_cap ht_cap;
+
+	/* size of the private FW status data */
+	size_t fw_status_priv_len;
+
+	/* RX Data filter rule state - enabled/disabled */
+	bool rx_filter_enabled[WL1271_MAX_RX_FILTERS];
+};
+
+int __devinit wlcore_probe(struct wl1271 *wl, struct platform_device *pdev);
+int __devexit wlcore_remove(struct platform_device *pdev);
+struct ieee80211_hw *wlcore_alloc_hw(size_t priv_size);
+int wlcore_free_hw(struct wl1271 *wl);
+
+/* Firmware image load chunk size */
+#define CHUNK_SIZE	16384
+
+/* Quirks */
+
+/* Each RX/TX transaction requires an end-of-transaction transfer */
+#define WLCORE_QUIRK_END_OF_TRANSACTION		BIT(0)
+
+/* wl127x and SPI don't support SDIO block size alignment */
+#define WLCORE_QUIRK_TX_BLOCKSIZE_ALIGN		BIT(2)
+
+/* means aggregated Rx packets are aligned to a SDIO block */
+#define WLCORE_QUIRK_RX_BLOCKSIZE_ALIGN		BIT(3)
+
+/* Older firmwares did not implement the FW logger over bus feature */
+#define WLCORE_QUIRK_FWLOG_NOT_IMPLEMENTED	BIT(4)
+
+/* Older firmwares use an old NVS format */
+#define WLCORE_QUIRK_LEGACY_NVS			BIT(5)
+
+/* Some firmwares may not support ELP */
+#define WLCORE_QUIRK_NO_ELP			BIT(6)
+
+/* TODO: move to the lower drivers when all usages are abstracted */
+#define CHIP_ID_1271_PG10              (0x4030101)
+#define CHIP_ID_1271_PG20              (0x4030111)
+#define CHIP_ID_1283_PG10              (0x05030101)
+#define CHIP_ID_1283_PG20              (0x05030111)
+
+/* TODO: move all these common registers and values elsewhere */
+#define HW_ACCESS_ELP_CTRL_REG		0x1FFFC
+
+/* ELP register commands */
+#define ELPCTRL_WAKE_UP             0x1
+#define ELPCTRL_WAKE_UP_WLAN_READY  0x5
+#define ELPCTRL_SLEEP               0x0
+/* ELP WLAN_READY bit */
+#define ELPCTRL_WLAN_READY          0x2
+
+/*************************************************************************
+
+    Interrupt Trigger Register (Host -> WiLink)
+
+**************************************************************************/
+
+/* Hardware to Embedded CPU Interrupts - first 32-bit register set */
+
+/*
+ * The host sets this bit to inform the Wlan
+ * FW that a TX packet is in the XFER
+ * Buffer #0.
+ */
+#define INTR_TRIG_TX_PROC0 BIT(2)
+
+/*
+ * The host sets this bit to inform the FW
+ * that it read a packet from RX XFER
+ * Buffer #0.
+ */
+#define INTR_TRIG_RX_PROC0 BIT(3)
+
+#define INTR_TRIG_DEBUG_ACK BIT(4)
+
+#define INTR_TRIG_STATE_CHANGED BIT(5)
+
+/* Hardware to Embedded CPU Interrupts - second 32-bit register set */
+
+/*
+ * The host sets this bit to inform the FW
+ * that it read a packet from RX XFER
+ * Buffer #1.
+ */
+#define INTR_TRIG_RX_PROC1 BIT(17)
+
+/*
+ * The host sets this bit to inform the Wlan
+ * hardware that a TX packet is in the XFER
+ * Buffer #1.
+ */
+#define INTR_TRIG_TX_PROC1 BIT(18)
+
+#define ACX_SLV_SOFT_RESET_BIT	BIT(1)
+#define SOFT_RESET_MAX_TIME	1000000
+#define SOFT_RESET_STALL_TIME	1000
+
+#define ECPU_CONTROL_HALT	0x00000101
+
+#define WELP_ARM_COMMAND_VAL	0x4
+
+#endif /* __WLCORE_H__ */
diff --git a/drivers/net/wireless/wl12xx/Kconfig b/drivers/net/wireless/wl12xx/Kconfig
deleted file mode 100644
index af08c8609c6..00000000000
--- a/drivers/net/wireless/wl12xx/Kconfig
+++ /dev/null
@@ -1,48 +0,0 @@
-menuconfig WL12XX_MENU
-	tristate "TI wl12xx driver support"
-	depends on MAC80211 && EXPERIMENTAL
-	---help---
-	  This will enable TI wl12xx driver support for the following chips:
-	  wl1271, wl1273, wl1281 and wl1283.
-	  The drivers make use of the mac80211 stack.
-
-config WL12XX
-	tristate "TI wl12xx support"
-	depends on WL12XX_MENU && GENERIC_HARDIRQS
-	depends on INET
-	select FW_LOADER
-	---help---
-	  This module adds support for wireless adapters based on TI wl1271 and
-	  TI wl1273 chipsets. This module does *not* include support for wl1251.
-	  For wl1251 support, use the separate homonymous driver instead.
-
-	  If you choose to build a module, it will be called wl12xx. Say N if
-	  unsure.
-
-config WL12XX_SPI
-	tristate "TI wl12xx SPI support"
-	depends on WL12XX && SPI_MASTER
-	select CRC7
-	---help---
-	  This module adds support for the SPI interface of adapters using
-	  TI wl12xx chipsets.  Select this if your platform is using
-	  the SPI bus.
-
-	  If you choose to build a module, it'll be called wl12xx_spi.
-	  Say N if unsure.
-
-config WL12XX_SDIO
-	tristate "TI wl12xx SDIO support"
-	depends on WL12XX && MMC
-	---help---
-	  This module adds support for the SDIO interface of adapters using
-	  TI wl12xx chipsets.  Select this if your platform is using
-	  the SDIO bus.
-
-	  If you choose to build a module, it'll be called wl12xx_sdio.
-	  Say N if unsure.
-
-config WL12XX_PLATFORM_DATA
-	bool
-	depends on WL12XX_SDIO != n || WL1251_SDIO != n
-	default y
diff --git a/drivers/net/wireless/wl12xx/Makefile b/drivers/net/wireless/wl12xx/Makefile
deleted file mode 100644
index 98f289c907a..00000000000
--- a/drivers/net/wireless/wl12xx/Makefile
+++ /dev/null
@@ -1,15 +0,0 @@
-wl12xx-objs		= main.o cmd.o io.o event.o tx.o rx.o ps.o acx.o \
-			  boot.o init.o debugfs.o scan.o
-
-wl12xx_spi-objs 	= spi.o
-wl12xx_sdio-objs	= sdio.o
-
-wl12xx-$(CONFIG_NL80211_TESTMODE)	+= testmode.o
-obj-$(CONFIG_WL12XX)			+= wl12xx.o
-obj-$(CONFIG_WL12XX_SPI)		+= wl12xx_spi.o
-obj-$(CONFIG_WL12XX_SDIO)		+= wl12xx_sdio.o
-
-# small builtin driver bit
-obj-$(CONFIG_WL12XX_PLATFORM_DATA)	+= wl12xx_platform_data.o
-
-ccflags-y += -D__CHECK_ENDIAN__
diff --git a/drivers/net/wireless/wl12xx/boot.c b/drivers/net/wireless/wl12xx/boot.c
deleted file mode 100644
index 954101d03f0..00000000000
--- a/drivers/net/wireless/wl12xx/boot.c
+++ /dev/null
@@ -1,786 +0,0 @@
-/*
- * This file is part of wl1271
- *
- * Copyright (C) 2008-2010 Nokia Corporation
- *
- * Contact: Luciano Coelho <luciano.coelho@nokia.com>
- *
- * This program is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * version 2 as published by the Free Software Foundation.
- *
- * This program is distributed in the hope that it will be useful, but
- * WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
- * General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program; if not, write to the Free Software
- * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA
- * 02110-1301 USA
- *
- */
-
-#include <linux/slab.h>
-#include <linux/wl12xx.h>
-#include <linux/export.h>
-
-#include "debug.h"
-#include "acx.h"
-#include "reg.h"
-#include "boot.h"
-#include "io.h"
-#include "event.h"
-#include "rx.h"
-
-static void wl1271_boot_set_ecpu_ctrl(struct wl1271 *wl, u32 flag)
-{
-	u32 cpu_ctrl;
-
-	/* 10.5.0 run the firmware (I) */
-	cpu_ctrl = wl1271_read32(wl, ACX_REG_ECPU_CONTROL);
-
-	/* 10.5.1 run the firmware (II) */
-	cpu_ctrl |= flag;
-	wl1271_write32(wl, ACX_REG_ECPU_CONTROL, cpu_ctrl);
-}
-
-static unsigned int wl12xx_get_fw_ver_quirks(struct wl1271 *wl)
-{
-	unsigned int quirks = 0;
-	unsigned int *fw_ver = wl->chip.fw_ver;
-
-	/* Only new station firmwares support routing fw logs to the host */
-	if ((fw_ver[FW_VER_IF_TYPE] == FW_VER_IF_TYPE_STA) &&
-	    (fw_ver[FW_VER_MINOR] < FW_VER_MINOR_FWLOG_STA_MIN))
-		quirks |= WL12XX_QUIRK_FWLOG_NOT_IMPLEMENTED;
-
-	/* This feature is not yet supported for AP mode */
-	if (fw_ver[FW_VER_IF_TYPE] == FW_VER_IF_TYPE_AP)
-		quirks |= WL12XX_QUIRK_FWLOG_NOT_IMPLEMENTED;
-
-	return quirks;
-}
-
-static void wl1271_parse_fw_ver(struct wl1271 *wl)
-{
-	int ret;
-
-	ret = sscanf(wl->chip.fw_ver_str + 4, "%u.%u.%u.%u.%u",
-		     &wl->chip.fw_ver[0], &wl->chip.fw_ver[1],
-		     &wl->chip.fw_ver[2], &wl->chip.fw_ver[3],
-		     &wl->chip.fw_ver[4]);
-
-	if (ret != 5) {
-		wl1271_warning("fw version incorrect value");
-		memset(wl->chip.fw_ver, 0, sizeof(wl->chip.fw_ver));
-		return;
-	}
-
-	/* Check if any quirks are needed with older fw versions */
-	wl->quirks |= wl12xx_get_fw_ver_quirks(wl);
-}
-
-static void wl1271_boot_fw_version(struct wl1271 *wl)
-{
-	struct wl1271_static_data static_data;
-
-	wl1271_read(wl, wl->cmd_box_addr, &static_data, sizeof(static_data),
-		    false);
-
-	strncpy(wl->chip.fw_ver_str, static_data.fw_version,
-		sizeof(wl->chip.fw_ver_str));
-
-	/* make sure the string is NULL-terminated */
-	wl->chip.fw_ver_str[sizeof(wl->chip.fw_ver_str) - 1] = '\0';
-
-	wl1271_parse_fw_ver(wl);
-}
-
-static int wl1271_boot_upload_firmware_chunk(struct wl1271 *wl, void *buf,
-					     size_t fw_data_len, u32 dest)
-{
-	struct wl1271_partition_set partition;
-	int addr, chunk_num, partition_limit;
-	u8 *p, *chunk;
-
-	/* whal_FwCtrl_LoadFwImageSm() */
-
-	wl1271_debug(DEBUG_BOOT, "starting firmware upload");
-
-	wl1271_debug(DEBUG_BOOT, "fw_data_len %zd chunk_size %d",
-		     fw_data_len, CHUNK_SIZE);
-
-	if ((fw_data_len % 4) != 0) {
-		wl1271_error("firmware length not multiple of four");
-		return -EIO;
-	}
-
-	chunk = kmalloc(CHUNK_SIZE, GFP_KERNEL);
-	if (!chunk) {
-		wl1271_error("allocation for firmware upload chunk failed");
-		return -ENOMEM;
-	}
-
-	memcpy(&partition, &wl12xx_part_table[PART_DOWN], sizeof(partition));
-	partition.mem.start = dest;
-	wl1271_set_partition(wl, &partition);
-
-	/* 10.1 set partition limit and chunk num */
-	chunk_num = 0;
-	partition_limit = wl12xx_part_table[PART_DOWN].mem.size;
-
-	while (chunk_num < fw_data_len / CHUNK_SIZE) {
-		/* 10.2 update partition, if needed */
-		addr = dest + (chunk_num + 2) * CHUNK_SIZE;
-		if (addr > partition_limit) {
-			addr = dest + chunk_num * CHUNK_SIZE;
-			partition_limit = chunk_num * CHUNK_SIZE +
-				wl12xx_part_table[PART_DOWN].mem.size;
-			partition.mem.start = addr;
-			wl1271_set_partition(wl, &partition);
-		}
-
-		/* 10.3 upload the chunk */
-		addr = dest + chunk_num * CHUNK_SIZE;
-		p = buf + chunk_num * CHUNK_SIZE;
-		memcpy(chunk, p, CHUNK_SIZE);
-		wl1271_debug(DEBUG_BOOT, "uploading fw chunk 0x%p to 0x%x",
-			     p, addr);
-		wl1271_write(wl, addr, chunk, CHUNK_SIZE, false);
-
-		chunk_num++;
-	}
-
-	/* 10.4 upload the last chunk */
-	addr = dest + chunk_num * CHUNK_SIZE;
-	p = buf + chunk_num * CHUNK_SIZE;
-	memcpy(chunk, p, fw_data_len % CHUNK_SIZE);
-	wl1271_debug(DEBUG_BOOT, "uploading fw last chunk (%zd B) 0x%p to 0x%x",
-		     fw_data_len % CHUNK_SIZE, p, addr);
-	wl1271_write(wl, addr, chunk, fw_data_len % CHUNK_SIZE, false);
-
-	kfree(chunk);
-	return 0;
-}
-
-static int wl1271_boot_upload_firmware(struct wl1271 *wl)
-{
-	u32 chunks, addr, len;
-	int ret = 0;
-	u8 *fw;
-
-	fw = wl->fw;
-	chunks = be32_to_cpup((__be32 *) fw);
-	fw += sizeof(u32);
-
-	wl1271_debug(DEBUG_BOOT, "firmware chunks to be uploaded: %u", chunks);
-
-	while (chunks--) {
-		addr = be32_to_cpup((__be32 *) fw);
-		fw += sizeof(u32);
-		len = be32_to_cpup((__be32 *) fw);
-		fw += sizeof(u32);
-
-		if (len > 300000) {
-			wl1271_info("firmware chunk too long: %u", len);
-			return -EINVAL;
-		}
-		wl1271_debug(DEBUG_BOOT, "chunk %d addr 0x%x len %u",
-			     chunks, addr, len);
-		ret = wl1271_boot_upload_firmware_chunk(wl, fw, len, addr);
-		if (ret != 0)
-			break;
-		fw += len;
-	}
-
-	return ret;
-}
-
-static int wl1271_boot_upload_nvs(struct wl1271 *wl)
-{
-	size_t nvs_len, burst_len;
-	int i;
-	u32 dest_addr, val;
-	u8 *nvs_ptr, *nvs_aligned;
-
-	if (wl->nvs == NULL)
-		return -ENODEV;
-
-	if (wl->chip.id == CHIP_ID_1283_PG20) {
-		struct wl128x_nvs_file *nvs = (struct wl128x_nvs_file *)wl->nvs;
-
-		if (wl->nvs_len == sizeof(struct wl128x_nvs_file)) {
-			if (nvs->general_params.dual_mode_select)
-				wl->enable_11a = true;
-		} else {
-			wl1271_error("nvs size is not as expected: %zu != %zu",
-				     wl->nvs_len,
-				     sizeof(struct wl128x_nvs_file));
-			kfree(wl->nvs);
-			wl->nvs = NULL;
-			wl->nvs_len = 0;
-			return -EILSEQ;
-		}
-
-		/* only the first part of the NVS needs to be uploaded */
-		nvs_len = sizeof(nvs->nvs);
-		nvs_ptr = (u8 *)nvs->nvs;
-
-	} else {
-		struct wl1271_nvs_file *nvs =
-			(struct wl1271_nvs_file *)wl->nvs;
-		/*
-		 * FIXME: the LEGACY NVS image support (NVS's missing the 5GHz
-		 * band configurations) can be removed when those NVS files stop
-		 * floating around.
-		 */
-		if (wl->nvs_len == sizeof(struct wl1271_nvs_file) ||
-		    wl->nvs_len == WL1271_INI_LEGACY_NVS_FILE_SIZE) {
-			if (nvs->general_params.dual_mode_select)
-				wl->enable_11a = true;
-		}
-
-		if (wl->nvs_len != sizeof(struct wl1271_nvs_file) &&
-		    (wl->nvs_len != WL1271_INI_LEGACY_NVS_FILE_SIZE ||
-		     wl->enable_11a)) {
-			wl1271_error("nvs size is not as expected: %zu != %zu",
-				wl->nvs_len, sizeof(struct wl1271_nvs_file));
-			kfree(wl->nvs);
-			wl->nvs = NULL;
-			wl->nvs_len = 0;
-			return -EILSEQ;
-		}
-
-		/* only the first part of the NVS needs to be uploaded */
-		nvs_len = sizeof(nvs->nvs);
-		nvs_ptr = (u8 *) nvs->nvs;
-	}
-
-	/* update current MAC address to NVS */
-	nvs_ptr[11] = wl->addresses[0].addr[0];
-	nvs_ptr[10] = wl->addresses[0].addr[1];
-	nvs_ptr[6] = wl->addresses[0].addr[2];
-	nvs_ptr[5] = wl->addresses[0].addr[3];
-	nvs_ptr[4] = wl->addresses[0].addr[4];
-	nvs_ptr[3] = wl->addresses[0].addr[5];
-
-	/*
-	 * Layout before the actual NVS tables:
-	 * 1 byte : burst length.
-	 * 2 bytes: destination address.
-	 * n bytes: data to burst copy.
-	 *
-	 * This is ended by a 0 length, then the NVS tables.
-	 */
-
-	/* FIXME: Do we need to check here whether the LSB is 1? */
-	while (nvs_ptr[0]) {
-		burst_len = nvs_ptr[0];
-		dest_addr = (nvs_ptr[1] & 0xfe) | ((u32)(nvs_ptr[2] << 8));
-
-		/*
-		 * Due to our new wl1271_translate_reg_addr function,
-		 * we need to add the REGISTER_BASE to the destination
-		 */
-		dest_addr += REGISTERS_BASE;
-
-		/* We move our pointer to the data */
-		nvs_ptr += 3;
-
-		for (i = 0; i < burst_len; i++) {
-			if (nvs_ptr + 3 >= (u8 *) wl->nvs + nvs_len)
-				goto out_badnvs;
-
-			val = (nvs_ptr[0] | (nvs_ptr[1] << 8)
-			       | (nvs_ptr[2] << 16) | (nvs_ptr[3] << 24));
-
-			wl1271_debug(DEBUG_BOOT,
-				     "nvs burst write 0x%x: 0x%x",
-				     dest_addr, val);
-			wl1271_write32(wl, dest_addr, val);
-
-			nvs_ptr += 4;
-			dest_addr += 4;
-		}
-
-		if (nvs_ptr >= (u8 *) wl->nvs + nvs_len)
-			goto out_badnvs;
-	}
-
-	/*
-	 * We've reached the first zero length, the first NVS table
-	 * is located at an aligned offset which is at least 7 bytes further.
-	 * NOTE: The wl->nvs->nvs element must be first, in order to
-	 * simplify the casting, we assume it is at the beginning of
-	 * the wl->nvs structure.
-	 */
-	nvs_ptr = (u8 *)wl->nvs +
-			ALIGN(nvs_ptr - (u8 *)wl->nvs + 7, 4);
-
-	if (nvs_ptr >= (u8 *) wl->nvs + nvs_len)
-		goto out_badnvs;
-
-	nvs_len -= nvs_ptr - (u8 *)wl->nvs;
-
-	/* Now we must set the partition correctly */
-	wl1271_set_partition(wl, &wl12xx_part_table[PART_WORK]);
-
-	/* Copy the NVS tables to a new block to ensure alignment */
-	nvs_aligned = kmemdup(nvs_ptr, nvs_len, GFP_KERNEL);
-	if (!nvs_aligned)
-		return -ENOMEM;
-
-	/* And finally we upload the NVS tables */
-	wl1271_write(wl, CMD_MBOX_ADDRESS, nvs_aligned, nvs_len, false);
-
-	kfree(nvs_aligned);
-	return 0;
-
-out_badnvs:
-	wl1271_error("nvs data is malformed");
-	return -EILSEQ;
-}
-
-static void wl1271_boot_enable_interrupts(struct wl1271 *wl)
-{
-	wl1271_enable_interrupts(wl);
-	wl1271_write32(wl, ACX_REG_INTERRUPT_MASK,
-		       WL1271_ACX_INTR_ALL & ~(WL1271_INTR_MASK));
-	wl1271_write32(wl, HI_CFG, HI_CFG_DEF_VAL);
-}
-
-static int wl1271_boot_soft_reset(struct wl1271 *wl)
-{
-	unsigned long timeout;
-	u32 boot_data;
-
-	/* perform soft reset */
-	wl1271_write32(wl, ACX_REG_SLV_SOFT_RESET, ACX_SLV_SOFT_RESET_BIT);
-
-	/* SOFT_RESET is self clearing */
-	timeout = jiffies + usecs_to_jiffies(SOFT_RESET_MAX_TIME);
-	while (1) {
-		boot_data = wl1271_read32(wl, ACX_REG_SLV_SOFT_RESET);
-		wl1271_debug(DEBUG_BOOT, "soft reset bootdata 0x%x", boot_data);
-		if ((boot_data & ACX_SLV_SOFT_RESET_BIT) == 0)
-			break;
-
-		if (time_after(jiffies, timeout)) {
-			/* 1.2 check pWhalBus->uSelfClearTime if the
-			 * timeout was reached */
-			wl1271_error("soft reset timeout");
-			return -1;
-		}
-
-		udelay(SOFT_RESET_STALL_TIME);
-	}
-
-	/* disable Rx/Tx */
-	wl1271_write32(wl, ENABLE, 0x0);
-
-	/* disable auto calibration on start*/
-	wl1271_write32(wl, SPARE_A2, 0xffff);
-
-	return 0;
-}
-
-static int wl1271_boot_run_firmware(struct wl1271 *wl)
-{
-	int loop, ret;
-	u32 chip_id, intr;
-
-	wl1271_boot_set_ecpu_ctrl(wl, ECPU_CONTROL_HALT);
-
-	chip_id = wl1271_read32(wl, CHIP_ID_B);
-
-	wl1271_debug(DEBUG_BOOT, "chip id after firmware boot: 0x%x", chip_id);
-
-	if (chip_id != wl->chip.id) {
-		wl1271_error("chip id doesn't match after firmware boot");
-		return -EIO;
-	}
-
-	/* wait for init to complete */
-	loop = 0;
-	while (loop++ < INIT_LOOP) {
-		udelay(INIT_LOOP_DELAY);
-		intr = wl1271_read32(wl, ACX_REG_INTERRUPT_NO_CLEAR);
-
-		if (intr == 0xffffffff) {
-			wl1271_error("error reading hardware complete "
-				     "init indication");
-			return -EIO;
-		}
-		/* check that ACX_INTR_INIT_COMPLETE is enabled */
-		else if (intr & WL1271_ACX_INTR_INIT_COMPLETE) {
-			wl1271_write32(wl, ACX_REG_INTERRUPT_ACK,
-				       WL1271_ACX_INTR_INIT_COMPLETE);
-			break;
-		}
-	}
-
-	if (loop > INIT_LOOP) {
-		wl1271_error("timeout waiting for the hardware to "
-			     "complete initialization");
-		return -EIO;
-	}
-
-	/* get hardware config command mail box */
-	wl->cmd_box_addr = wl1271_read32(wl, REG_COMMAND_MAILBOX_PTR);
-
-	/* get hardware config event mail box */
-	wl->event_box_addr = wl1271_read32(wl, REG_EVENT_MAILBOX_PTR);
-
-	/* set the working partition to its "running" mode offset */
-	wl1271_set_partition(wl, &wl12xx_part_table[PART_WORK]);
-
-	wl1271_debug(DEBUG_MAILBOX, "cmd_box_addr 0x%x event_box_addr 0x%x",
-		     wl->cmd_box_addr, wl->event_box_addr);
-
-	wl1271_boot_fw_version(wl);
-
-	/*
-	 * in case of full asynchronous mode the firmware event must be
-	 * ready to receive event from the command mailbox
-	 */
-
-	/* unmask required mbox events  */
-	wl->event_mask = BSS_LOSE_EVENT_ID |
-		SCAN_COMPLETE_EVENT_ID |
-		ROLE_STOP_COMPLETE_EVENT_ID |
-		RSSI_SNR_TRIGGER_0_EVENT_ID |
-		PSPOLL_DELIVERY_FAILURE_EVENT_ID |
-		SOFT_GEMINI_SENSE_EVENT_ID |
-		PERIODIC_SCAN_REPORT_EVENT_ID |
-		PERIODIC_SCAN_COMPLETE_EVENT_ID |
-		DUMMY_PACKET_EVENT_ID |
-		PEER_REMOVE_COMPLETE_EVENT_ID |
-		BA_SESSION_RX_CONSTRAINT_EVENT_ID |
-		REMAIN_ON_CHANNEL_COMPLETE_EVENT_ID |
-		INACTIVE_STA_EVENT_ID |
-		MAX_TX_RETRY_EVENT_ID |
-		CHANNEL_SWITCH_COMPLETE_EVENT_ID;
-
-	ret = wl1271_event_unmask(wl);
-	if (ret < 0) {
-		wl1271_error("EVENT mask setting failed");
-		return ret;
-	}
-
-	wl1271_event_mbox_config(wl);
-
-	/* firmware startup completed */
-	return 0;
-}
-
-static int wl1271_boot_write_irq_polarity(struct wl1271 *wl)
-{
-	u32 polarity;
-
-	polarity = wl1271_top_reg_read(wl, OCP_REG_POLARITY);
-
-	/* We use HIGH polarity, so unset the LOW bit */
-	polarity &= ~POLARITY_LOW;
-	wl1271_top_reg_write(wl, OCP_REG_POLARITY, polarity);
-
-	return 0;
-}
-
-static int wl128x_switch_tcxo_to_fref(struct wl1271 *wl)
-{
-	u16 spare_reg;
-
-	/* Mask bits [2] & [8:4] in the sys_clk_cfg register */
-	spare_reg = wl1271_top_reg_read(wl, WL_SPARE_REG);
-	if (spare_reg == 0xFFFF)
-		return -EFAULT;
-	spare_reg |= (BIT(3) | BIT(5) | BIT(6));
-	wl1271_top_reg_write(wl, WL_SPARE_REG, spare_reg);
-
-	/* Enable FREF_CLK_REQ & mux MCS and coex PLLs to FREF */
-	wl1271_top_reg_write(wl, SYS_CLK_CFG_REG,
-			     WL_CLK_REQ_TYPE_PG2 | MCS_PLL_CLK_SEL_FREF);
-
-	/* Delay execution for 15msec, to let the HW settle */
-	mdelay(15);
-
-	return 0;
-}
-
-static bool wl128x_is_tcxo_valid(struct wl1271 *wl)
-{
-	u16 tcxo_detection;
-
-	tcxo_detection = wl1271_top_reg_read(wl, TCXO_CLK_DETECT_REG);
-	if (tcxo_detection & TCXO_DET_FAILED)
-		return false;
-
-	return true;
-}
-
-static bool wl128x_is_fref_valid(struct wl1271 *wl)
-{
-	u16 fref_detection;
-
-	fref_detection = wl1271_top_reg_read(wl, FREF_CLK_DETECT_REG);
-	if (fref_detection & FREF_CLK_DETECT_FAIL)
-		return false;
-
-	return true;
-}
-
-static int wl128x_manually_configure_mcs_pll(struct wl1271 *wl)
-{
-	wl1271_top_reg_write(wl, MCS_PLL_M_REG, MCS_PLL_M_REG_VAL);
-	wl1271_top_reg_write(wl, MCS_PLL_N_REG, MCS_PLL_N_REG_VAL);
-	wl1271_top_reg_write(wl, MCS_PLL_CONFIG_REG, MCS_PLL_CONFIG_REG_VAL);
-
-	return 0;
-}
-
-static int wl128x_configure_mcs_pll(struct wl1271 *wl, int clk)
-{
-	u16 spare_reg;
-	u16 pll_config;
-	u8 input_freq;
-
-	/* Mask bits [3:1] in the sys_clk_cfg register */
-	spare_reg = wl1271_top_reg_read(wl, WL_SPARE_REG);
-	if (spare_reg == 0xFFFF)
-		return -EFAULT;
-	spare_reg |= BIT(2);
-	wl1271_top_reg_write(wl, WL_SPARE_REG, spare_reg);
-
-	/* Handle special cases of the TCXO clock */
-	if (wl->tcxo_clock == WL12XX_TCXOCLOCK_16_8 ||
-	    wl->tcxo_clock == WL12XX_TCXOCLOCK_33_6)
-		return wl128x_manually_configure_mcs_pll(wl);
-
-	/* Set the input frequency according to the selected clock source */
-	input_freq = (clk & 1) + 1;
-
-	pll_config = wl1271_top_reg_read(wl, MCS_PLL_CONFIG_REG);
-	if (pll_config == 0xFFFF)
-		return -EFAULT;
-	pll_config |= (input_freq << MCS_SEL_IN_FREQ_SHIFT);
-	pll_config |= MCS_PLL_ENABLE_HP;
-	wl1271_top_reg_write(wl, MCS_PLL_CONFIG_REG, pll_config);
-
-	return 0;
-}
-
-/*
- * WL128x has two clocks input - TCXO and FREF.
- * TCXO is the main clock of the device, while FREF is used to sync
- * between the GPS and the cellular modem.
- * In cases where TCXO is 32.736MHz or 16.368MHz, the FREF will be used
- * as the WLAN/BT main clock.
- */
-static int wl128x_boot_clk(struct wl1271 *wl, int *selected_clock)
-{
-	u16 sys_clk_cfg;
-
-	/* For XTAL-only modes, FREF will be used after switching from TCXO */
-	if (wl->ref_clock == WL12XX_REFCLOCK_26_XTAL ||
-	    wl->ref_clock == WL12XX_REFCLOCK_38_XTAL) {
-		if (!wl128x_switch_tcxo_to_fref(wl))
-			return -EINVAL;
-		goto fref_clk;
-	}
-
-	/* Query the HW, to determine which clock source we should use */
-	sys_clk_cfg = wl1271_top_reg_read(wl, SYS_CLK_CFG_REG);
-	if (sys_clk_cfg == 0xFFFF)
-		return -EINVAL;
-	if (sys_clk_cfg & PRCM_CM_EN_MUX_WLAN_FREF)
-		goto fref_clk;
-
-	/* If TCXO is either 32.736MHz or 16.368MHz, switch to FREF */
-	if (wl->tcxo_clock == WL12XX_TCXOCLOCK_16_368 ||
-	    wl->tcxo_clock == WL12XX_TCXOCLOCK_32_736) {
-		if (!wl128x_switch_tcxo_to_fref(wl))
-			return -EINVAL;
-		goto fref_clk;
-	}
-
-	/* TCXO clock is selected */
-	if (!wl128x_is_tcxo_valid(wl))
-		return -EINVAL;
-	*selected_clock = wl->tcxo_clock;
-	goto config_mcs_pll;
-
-fref_clk:
-	/* FREF clock is selected */
-	if (!wl128x_is_fref_valid(wl))
-		return -EINVAL;
-	*selected_clock = wl->ref_clock;
-
-config_mcs_pll:
-	return wl128x_configure_mcs_pll(wl, *selected_clock);
-}
-
-static int wl127x_boot_clk(struct wl1271 *wl)
-{
-	u32 pause;
-	u32 clk;
-
-	if (WL127X_PG_GET_MAJOR(wl->hw_pg_ver) < 3)
-		wl->quirks |= WL12XX_QUIRK_END_OF_TRANSACTION;
-
-	if (wl->ref_clock == CONF_REF_CLK_19_2_E ||
-	    wl->ref_clock == CONF_REF_CLK_38_4_E ||
-	    wl->ref_clock == CONF_REF_CLK_38_4_M_XTAL)
-		/* ref clk: 19.2/38.4/38.4-XTAL */
-		clk = 0x3;
-	else if (wl->ref_clock == CONF_REF_CLK_26_E ||
-		 wl->ref_clock == CONF_REF_CLK_52_E)
-		/* ref clk: 26/52 */
-		clk = 0x5;
-	else
-		return -EINVAL;
-
-	if (wl->ref_clock != CONF_REF_CLK_19_2_E) {
-		u16 val;
-		/* Set clock type (open drain) */
-		val = wl1271_top_reg_read(wl, OCP_REG_CLK_TYPE);
-		val &= FREF_CLK_TYPE_BITS;
-		wl1271_top_reg_write(wl, OCP_REG_CLK_TYPE, val);
-
-		/* Set clock pull mode (no pull) */
-		val = wl1271_top_reg_read(wl, OCP_REG_CLK_PULL);
-		val |= NO_PULL;
-		wl1271_top_reg_write(wl, OCP_REG_CLK_PULL, val);
-	} else {
-		u16 val;
-		/* Set clock polarity */
-		val = wl1271_top_reg_read(wl, OCP_REG_CLK_POLARITY);
-		val &= FREF_CLK_POLARITY_BITS;
-		val |= CLK_REQ_OUTN_SEL;
-		wl1271_top_reg_write(wl, OCP_REG_CLK_POLARITY, val);
-	}
-
-	wl1271_write32(wl, PLL_PARAMETERS, clk);
-
-	pause = wl1271_read32(wl, PLL_PARAMETERS);
-
-	wl1271_debug(DEBUG_BOOT, "pause1 0x%x", pause);
-
-	pause &= ~(WU_COUNTER_PAUSE_VAL);
-	pause |= WU_COUNTER_PAUSE_VAL;
-	wl1271_write32(wl, WU_COUNTER_PAUSE, pause);
-
-	return 0;
-}
-
-/* uploads NVS and firmware */
-int wl1271_load_firmware(struct wl1271 *wl)
-{
-	int ret = 0;
-	u32 tmp, clk;
-	int selected_clock = -1;
-
-	if (wl->chip.id == CHIP_ID_1283_PG20) {
-		ret = wl128x_boot_clk(wl, &selected_clock);
-		if (ret < 0)
-			goto out;
-	} else {
-		ret = wl127x_boot_clk(wl);
-		if (ret < 0)
-			goto out;
-	}
-
-	/* Continue the ELP wake up sequence */
-	wl1271_write32(wl, WELP_ARM_COMMAND, WELP_ARM_COMMAND_VAL);
-	udelay(500);
-
-	wl1271_set_partition(wl, &wl12xx_part_table[PART_DRPW]);
-
-	/* Read-modify-write DRPW_SCRATCH_START register (see next state)
-	   to be used by DRPw FW. The RTRIM value will be added by the FW
-	   before taking DRPw out of reset */
-
-	wl1271_debug(DEBUG_BOOT, "DRPW_SCRATCH_START %08x", DRPW_SCRATCH_START);
-	clk = wl1271_read32(wl, DRPW_SCRATCH_START);
-
-	wl1271_debug(DEBUG_BOOT, "clk2 0x%x", clk);
-
-	if (wl->chip.id == CHIP_ID_1283_PG20) {
-		clk |= ((selected_clock & 0x3) << 1) << 4;
-	} else {
-		clk |= (wl->ref_clock << 1) << 4;
-	}
-
-	wl1271_write32(wl, DRPW_SCRATCH_START, clk);
-
-	wl1271_set_partition(wl, &wl12xx_part_table[PART_WORK]);
-
-	/* Disable interrupts */
-	wl1271_write32(wl, ACX_REG_INTERRUPT_MASK, WL1271_ACX_INTR_ALL);
-
-	ret = wl1271_boot_soft_reset(wl);
-	if (ret < 0)
-		goto out;
-
-	/* 2. start processing NVS file */
-	ret = wl1271_boot_upload_nvs(wl);
-	if (ret < 0)
-		goto out;
-
-	/* write firmware's last address (ie. it's length) to
-	 * ACX_EEPROMLESS_IND_REG */
-	wl1271_debug(DEBUG_BOOT, "ACX_EEPROMLESS_IND_REG");
-
-	wl1271_write32(wl, ACX_EEPROMLESS_IND_REG, ACX_EEPROMLESS_IND_REG);
-
-	tmp = wl1271_read32(wl, CHIP_ID_B);
-
-	wl1271_debug(DEBUG_BOOT, "chip id 0x%x", tmp);
-
-	/* 6. read the EEPROM parameters */
-	tmp = wl1271_read32(wl, SCR_PAD2);
-
-	/* WL1271: The reference driver skips steps 7 to 10 (jumps directly
-	 * to upload_fw) */
-
-	if (wl->chip.id == CHIP_ID_1283_PG20)
-		wl1271_top_reg_write(wl, SDIO_IO_DS, wl->conf.hci_io_ds);
-
-	ret = wl1271_boot_upload_firmware(wl);
-	if (ret < 0)
-		goto out;
-
-out:
-	return ret;
-}
-EXPORT_SYMBOL_GPL(wl1271_load_firmware);
-
-int wl1271_boot(struct wl1271 *wl)
-{
-	int ret;
-
-	/* upload NVS and firmware */
-	ret = wl1271_load_firmware(wl);
-	if (ret)
-		return ret;
-
-	/* 10.5 start firmware */
-	ret = wl1271_boot_run_firmware(wl);
-	if (ret < 0)
-		goto out;
-
-	ret = wl1271_boot_write_irq_polarity(wl);
-	if (ret < 0)
-		goto out;
-
-	wl1271_write32(wl, ACX_REG_INTERRUPT_MASK,
-		       WL1271_ACX_ALL_EVENTS_VECTOR);
-
-	/* Enable firmware interrupts now */
-	wl1271_boot_enable_interrupts(wl);
-
-	wl1271_event_mbox_config(wl);
-
-out:
-	return ret;
-}
diff --git a/drivers/net/wireless/wl12xx/boot.h b/drivers/net/wireless/wl12xx/boot.h
deleted file mode 100644
index c3adc09f403..00000000000
--- a/drivers/net/wireless/wl12xx/boot.h
+++ /dev/null
@@ -1,120 +0,0 @@
-/*
- * This file is part of wl1271
- *
- * Copyright (C) 2008-2009 Nokia Corporation
- *
- * Contact: Luciano Coelho <luciano.coelho@nokia.com>
- *
- * This program is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * version 2 as published by the Free Software Foundation.
- *
- * This program is distributed in the hope that it will be useful, but
- * WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
- * General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program; if not, write to the Free Software
- * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA
- * 02110-1301 USA
- *
- */
-
-#ifndef __BOOT_H__
-#define __BOOT_H__
-
-#include "wl12xx.h"
-
-int wl1271_boot(struct wl1271 *wl);
-int wl1271_load_firmware(struct wl1271 *wl);
-
-#define WL1271_NO_SUBBANDS 8
-#define WL1271_NO_POWER_LEVELS 4
-#define WL1271_FW_VERSION_MAX_LEN 20
-
-struct wl1271_static_data {
-	u8 mac_address[ETH_ALEN];
-	u8 padding[2];
-	u8 fw_version[WL1271_FW_VERSION_MAX_LEN];
-	u32 hw_version;
-	u8 tx_power_table[WL1271_NO_SUBBANDS][WL1271_NO_POWER_LEVELS];
-};
-
-/* number of times we try to read the INIT interrupt */
-#define INIT_LOOP 20000
-
-/* delay between retries */
-#define INIT_LOOP_DELAY 50
-
-#define WU_COUNTER_PAUSE_VAL 0x3FF
-#define WELP_ARM_COMMAND_VAL 0x4
-
-#define OCP_REG_POLARITY     0x0064
-#define OCP_REG_CLK_TYPE     0x0448
-#define OCP_REG_CLK_POLARITY 0x0cb2
-#define OCP_REG_CLK_PULL     0x0cb4
-
-#define CMD_MBOX_ADDRESS     0x407B4
-
-#define POLARITY_LOW         BIT(1)
-#define NO_PULL              (BIT(14) | BIT(15))
-
-#define FREF_CLK_TYPE_BITS     0xfffffe7f
-#define CLK_REQ_PRCM           0x100
-#define FREF_CLK_POLARITY_BITS 0xfffff8ff
-#define CLK_REQ_OUTN_SEL       0x700
-
-/* PLL configuration algorithm for wl128x */
-#define SYS_CLK_CFG_REG              0x2200
-/* Bit[0]   -  0-TCXO,  1-FREF */
-#define MCS_PLL_CLK_SEL_FREF         BIT(0)
-/* Bit[3:2] - 01-TCXO, 10-FREF */
-#define WL_CLK_REQ_TYPE_FREF         BIT(3)
-#define WL_CLK_REQ_TYPE_PG2          (BIT(3) | BIT(2))
-/* Bit[4]   -  0-TCXO,  1-FREF */
-#define PRCM_CM_EN_MUX_WLAN_FREF     BIT(4)
-
-#define TCXO_ILOAD_INT_REG           0x2264
-#define TCXO_CLK_DETECT_REG          0x2266
-
-#define TCXO_DET_FAILED              BIT(4)
-
-#define FREF_ILOAD_INT_REG           0x2084
-#define FREF_CLK_DETECT_REG          0x2086
-#define FREF_CLK_DETECT_FAIL         BIT(4)
-
-/* Use this reg for masking during driver access */
-#define WL_SPARE_REG                 0x2320
-#define WL_SPARE_VAL                 BIT(2)
-/* Bit[6:5:3] -  mask wl write SYS_CLK_CFG[8:5:2:4] */
-#define WL_SPARE_MASK_8526           (BIT(6) | BIT(5) | BIT(3))
-
-#define PLL_LOCK_COUNTERS_REG        0xD8C
-#define PLL_LOCK_COUNTERS_COEX       0x0F
-#define PLL_LOCK_COUNTERS_MCS        0xF0
-#define MCS_PLL_OVERRIDE_REG         0xD90
-#define MCS_PLL_CONFIG_REG           0xD92
-#define MCS_SEL_IN_FREQ_MASK         0x0070
-#define MCS_SEL_IN_FREQ_SHIFT        4
-#define MCS_PLL_CONFIG_REG_VAL       0x73
-#define MCS_PLL_ENABLE_HP            (BIT(0) | BIT(1))
-
-#define MCS_PLL_M_REG                0xD94
-#define MCS_PLL_N_REG                0xD96
-#define MCS_PLL_M_REG_VAL            0xC8
-#define MCS_PLL_N_REG_VAL            0x07
-
-#define SDIO_IO_DS                   0xd14
-
-/* SDIO/wSPI DS configuration values */
-enum {
-	HCI_IO_DS_8MA = 0,
-	HCI_IO_DS_4MA = 1, /* default */
-	HCI_IO_DS_6MA = 2,
-	HCI_IO_DS_2MA = 3,
-};
-
-/* end PLL configuration algorithm for wl128x */
-
-#endif
diff --git a/drivers/net/wireless/zd1201.c b/drivers/net/wireless/zd1201.c
index a66b93b7ff9..48273dd05b6 100644
--- a/drivers/net/wireless/zd1201.c
+++ b/drivers/net/wireless/zd1201.c
@@ -1905,6 +1905,7 @@ static struct usb_driver zd1201_usb = {
 	.id_table = zd1201_table,
 	.suspend = zd1201_suspend,
 	.resume = zd1201_resume,
+	.disable_hub_initiated_lpm = 1,
 };
 
 module_usb_driver(zd1201_usb);
diff --git a/drivers/net/wireless/zd1211rw/zd_usb.c b/drivers/net/wireless/zd1211rw/zd_usb.c
index f766b3e67c6..af83c43bcdb 100644
--- a/drivers/net/wireless/zd1211rw/zd_usb.c
+++ b/drivers/net/wireless/zd1211rw/zd_usb.c
@@ -1542,6 +1542,7 @@ static struct usb_driver driver = {
 	.disconnect	= disconnect,
 	.pre_reset	= pre_reset,
 	.post_reset	= post_reset,
+	.disable_hub_initiated_lpm = 1,
 };
 
 struct workqueue_struct *zd_workqueue;
diff --git a/drivers/net/xen-netfront.c b/drivers/net/xen-netfront.c
index 0ebbb1906c3..2027afe405f 100644
--- a/drivers/net/xen-netfront.c
+++ b/drivers/net/xen-netfront.c
@@ -1962,9 +1962,6 @@ static int __init netif_init(void)
 	if (!xen_domain())
 		return -ENODEV;
 
-	if (xen_initial_domain())
-		return 0;
-
 	if (xen_hvm_domain() && !xen_platform_pci_unplug)
 		return -ENODEV;
 
@@ -1977,9 +1974,6 @@ module_init(netif_init);
 
 static void __exit netif_exit(void)
 {
-	if (xen_initial_domain())
-		return;
-
 	xenbus_unregister_driver(&netfront_driver);
 }
 module_exit(netif_exit);
diff --git a/drivers/nfc/Kconfig b/drivers/nfc/Kconfig
index 5af959274d4..3b20b73ee64 100644
--- a/drivers/nfc/Kconfig
+++ b/drivers/nfc/Kconfig
@@ -17,6 +17,19 @@ config PN544_NFC
 	  To compile this driver as a module, choose m here. The module will
 	  be called pn544.
 
+config PN544_HCI_NFC
+	tristate "HCI PN544 NFC driver"
+	depends on I2C && NFC_SHDLC
+	select CRC_CCITT
+	default n
+	---help---
+	  NXP PN544 i2c driver.
+	  This is a driver based on the SHDLC and HCI NFC kernel layers and
+	  will thus not work with NXP libnfc library.
+
+	  To compile this driver as a module, choose m here. The module will
+	  be called pn544_hci.
+
 config NFC_PN533
 	tristate "NXP PN533 USB driver"
 	depends on USB
diff --git a/drivers/nfc/Makefile b/drivers/nfc/Makefile
index ab99e8572f0..473e44cef61 100644
--- a/drivers/nfc/Makefile
+++ b/drivers/nfc/Makefile
@@ -3,6 +3,7 @@
 #
 
 obj-$(CONFIG_PN544_NFC)		+= pn544.o
+obj-$(CONFIG_PN544_HCI_NFC)	+= pn544_hci.o
 obj-$(CONFIG_NFC_PN533)		+= pn533.o
 obj-$(CONFIG_NFC_WILINK)	+= nfcwilink.o
 
diff --git a/drivers/nfc/pn533.c b/drivers/nfc/pn533.c
index cb6204f7830..19110f0eb15 100644
--- a/drivers/nfc/pn533.c
+++ b/drivers/nfc/pn533.c
@@ -266,9 +266,13 @@ struct pn533 {
 	int in_maxlen;
 	struct pn533_frame *in_frame;
 
-	struct tasklet_struct tasklet;
-	struct pn533_frame *tklt_in_frame;
-	int tklt_in_error;
+	struct sk_buff_head resp_q;
+
+	struct workqueue_struct	*wq;
+	struct work_struct cmd_work;
+	struct work_struct mi_work;
+	struct pn533_frame *wq_in_frame;
+	int wq_in_error;
 
 	pn533_cmd_complete_t cmd_complete;
 	void *cmd_complete_arg;
@@ -383,15 +387,18 @@ static bool pn533_rx_frame_is_cmd_response(struct pn533_frame *frame, u8 cmd)
 	return (PN533_FRAME_CMD(frame) == PN533_CMD_RESPONSE(cmd));
 }
 
-static void pn533_tasklet_cmd_complete(unsigned long arg)
+
+static void pn533_wq_cmd_complete(struct work_struct *work)
 {
-	struct pn533 *dev = (struct pn533 *) arg;
-	struct pn533_frame *in_frame = dev->tklt_in_frame;
+	struct pn533 *dev = container_of(work, struct pn533, cmd_work);
+	struct pn533_frame *in_frame;
 	int rc;
 
-	if (dev->tklt_in_error)
+	in_frame = dev->wq_in_frame;
+
+	if (dev->wq_in_error)
 		rc = dev->cmd_complete(dev, dev->cmd_complete_arg, NULL,
-							dev->tklt_in_error);
+							dev->wq_in_error);
 	else
 		rc = dev->cmd_complete(dev, dev->cmd_complete_arg,
 					PN533_FRAME_CMD_PARAMS_PTR(in_frame),
@@ -406,7 +413,7 @@ static void pn533_recv_response(struct urb *urb)
 	struct pn533 *dev = urb->context;
 	struct pn533_frame *in_frame;
 
-	dev->tklt_in_frame = NULL;
+	dev->wq_in_frame = NULL;
 
 	switch (urb->status) {
 	case 0:
@@ -417,36 +424,36 @@ static void pn533_recv_response(struct urb *urb)
 	case -ESHUTDOWN:
 		nfc_dev_dbg(&dev->interface->dev, "Urb shutting down with"
 						" status: %d", urb->status);
-		dev->tklt_in_error = urb->status;
-		goto sched_tasklet;
+		dev->wq_in_error = urb->status;
+		goto sched_wq;
 	default:
 		nfc_dev_err(&dev->interface->dev, "Nonzero urb status received:"
 							" %d", urb->status);
-		dev->tklt_in_error = urb->status;
-		goto sched_tasklet;
+		dev->wq_in_error = urb->status;
+		goto sched_wq;
 	}
 
 	in_frame = dev->in_urb->transfer_buffer;
 
 	if (!pn533_rx_frame_is_valid(in_frame)) {
 		nfc_dev_err(&dev->interface->dev, "Received an invalid frame");
-		dev->tklt_in_error = -EIO;
-		goto sched_tasklet;
+		dev->wq_in_error = -EIO;
+		goto sched_wq;
 	}
 
 	if (!pn533_rx_frame_is_cmd_response(in_frame, dev->cmd)) {
 		nfc_dev_err(&dev->interface->dev, "The received frame is not "
 						"response to the last command");
-		dev->tklt_in_error = -EIO;
-		goto sched_tasklet;
+		dev->wq_in_error = -EIO;
+		goto sched_wq;
 	}
 
 	nfc_dev_dbg(&dev->interface->dev, "Received a valid frame");
-	dev->tklt_in_error = 0;
-	dev->tklt_in_frame = in_frame;
+	dev->wq_in_error = 0;
+	dev->wq_in_frame = in_frame;
 
-sched_tasklet:
-	tasklet_schedule(&dev->tasklet);
+sched_wq:
+	queue_work(dev->wq, &dev->cmd_work);
 }
 
 static int pn533_submit_urb_for_response(struct pn533 *dev, gfp_t flags)
@@ -471,21 +478,21 @@ static void pn533_recv_ack(struct urb *urb)
 	case -ESHUTDOWN:
 		nfc_dev_dbg(&dev->interface->dev, "Urb shutting down with"
 						" status: %d", urb->status);
-		dev->tklt_in_error = urb->status;
-		goto sched_tasklet;
+		dev->wq_in_error = urb->status;
+		goto sched_wq;
 	default:
 		nfc_dev_err(&dev->interface->dev, "Nonzero urb status received:"
 							" %d", urb->status);
-		dev->tklt_in_error = urb->status;
-		goto sched_tasklet;
+		dev->wq_in_error = urb->status;
+		goto sched_wq;
 	}
 
 	in_frame = dev->in_urb->transfer_buffer;
 
 	if (!pn533_rx_frame_is_ack(in_frame)) {
 		nfc_dev_err(&dev->interface->dev, "Received an invalid ack");
-		dev->tklt_in_error = -EIO;
-		goto sched_tasklet;
+		dev->wq_in_error = -EIO;
+		goto sched_wq;
 	}
 
 	nfc_dev_dbg(&dev->interface->dev, "Received a valid ack");
@@ -494,15 +501,15 @@ 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->tklt_in_error = rc;
-		goto sched_tasklet;
+		dev->wq_in_error = rc;
+		goto sched_wq;
 	}
 
 	return;
 
-sched_tasklet:
-	dev->tklt_in_frame = NULL;
-	tasklet_schedule(&dev->tasklet);
+sched_wq:
+	dev->wq_in_frame = NULL;
+	queue_work(dev->wq, &dev->cmd_work);
 }
 
 static int pn533_submit_urb_for_ack(struct pn533 *dev, gfp_t flags)
@@ -1184,8 +1191,8 @@ static int pn533_activate_target_nfcdep(struct pn533 *dev)
 	return rc;
 }
 
-static int pn533_activate_target(struct nfc_dev *nfc_dev, u32 target_idx,
-								u32 protocol)
+static int pn533_activate_target(struct nfc_dev *nfc_dev,
+				 struct nfc_target *target, u32 protocol)
 {
 	struct pn533 *dev = nfc_get_drvdata(nfc_dev);
 	int rc;
@@ -1233,7 +1240,8 @@ static int pn533_activate_target(struct nfc_dev *nfc_dev, u32 target_idx,
 	return 0;
 }
 
-static void pn533_deactivate_target(struct nfc_dev *nfc_dev, u32 target_idx)
+static void pn533_deactivate_target(struct nfc_dev *nfc_dev,
+				    struct nfc_target *target)
 {
 	struct pn533 *dev = nfc_get_drvdata(nfc_dev);
 	u8 tg;
@@ -1249,6 +1257,8 @@ static void pn533_deactivate_target(struct nfc_dev *nfc_dev, u32 target_idx)
 
 	dev->tgt_active_prot = 0;
 
+	skb_queue_purge(&dev->resp_q);
+
 	pn533_tx_frame_init(dev->out_frame, PN533_CMD_IN_RELEASE);
 
 	tg = 1;
@@ -1339,7 +1349,7 @@ static int pn533_in_dep_link_up_complete(struct pn533 *dev, void *arg,
 	return 0;
 }
 
-static int pn533_dep_link_up(struct nfc_dev *nfc_dev, int target_idx,
+static int pn533_dep_link_up(struct nfc_dev *nfc_dev, struct nfc_target *target,
 			     u8 comm_mode, u8* gb, size_t gb_len)
 {
 	struct pn533 *dev = nfc_get_drvdata(nfc_dev);
@@ -1447,11 +1457,49 @@ struct pn533_data_exchange_arg {
 	void *cb_context;
 };
 
+static struct sk_buff *pn533_build_response(struct pn533 *dev)
+{
+	struct sk_buff *skb, *tmp, *t;
+	unsigned int skb_len = 0, tmp_len = 0;
+
+	nfc_dev_dbg(&dev->interface->dev, "%s\n", __func__);
+
+	if (skb_queue_empty(&dev->resp_q))
+		return NULL;
+
+	if (skb_queue_len(&dev->resp_q) == 1) {
+		skb = skb_dequeue(&dev->resp_q);
+		goto out;
+	}
+
+	skb_queue_walk_safe(&dev->resp_q, tmp, t)
+		skb_len += tmp->len;
+
+	nfc_dev_dbg(&dev->interface->dev, "%s total length %d\n",
+		    __func__, skb_len);
+
+	skb = alloc_skb(skb_len, GFP_KERNEL);
+	if (skb == NULL)
+		goto out;
+
+	skb_put(skb, skb_len);
+
+	skb_queue_walk_safe(&dev->resp_q, tmp, t) {
+		memcpy(skb->data + tmp_len, tmp->data, tmp->len);
+		tmp_len += tmp->len;
+	}
+
+out:
+	skb_queue_purge(&dev->resp_q);
+
+	return skb;
+}
+
 static int pn533_data_exchange_complete(struct pn533 *dev, void *_arg,
 						u8 *params, int params_len)
 {
 	struct pn533_data_exchange_arg *arg = _arg;
-	struct sk_buff *skb_resp = arg->skb_resp;
+	struct sk_buff *skb = NULL, *skb_resp = arg->skb_resp;
 	struct pn533_frame *in_frame = (struct pn533_frame *) skb_resp->data;
 	int err = 0;
 	u8 status;
@@ -1459,15 +1507,13 @@ static int pn533_data_exchange_complete(struct pn533 *dev, void *_arg,
 
 	nfc_dev_dbg(&dev->interface->dev, "%s", __func__);
 
-	dev_kfree_skb_irq(arg->skb_out);
+	dev_kfree_skb(arg->skb_out);
 
 	if (params_len < 0) { /* error */
 		err = params_len;
 		goto error;
 	}
 
-	skb_put(skb_resp, PN533_FRAME_SIZE(in_frame));
-
 	status = params[0];
 
 	cmd_ret = status & PN533_CMD_RET_MASK;
@@ -1478,34 +1524,35 @@ static int pn533_data_exchange_complete(struct pn533 *dev, void *_arg,
 		goto error;
 	}
 
+	skb_put(skb_resp, PN533_FRAME_SIZE(in_frame));
+	skb_pull(skb_resp, PN533_CMD_DATAEXCH_HEAD_LEN);
+	skb_trim(skb_resp, skb_resp->len - PN533_FRAME_TAIL_SIZE);
+	skb_queue_tail(&dev->resp_q, skb_resp);
+
 	if (status & PN533_CMD_MI_MASK) {
-		/* TODO: Implement support to multi-part data exchange */
-		nfc_dev_err(&dev->interface->dev, "Multi-part message not yet"
-								" supported");
-		/* Prevent the other messages from controller */
-		pn533_send_ack(dev, GFP_ATOMIC);
-		err = -ENOSYS;
-		goto error;
+		queue_work(dev->wq, &dev->mi_work);
+		return -EINPROGRESS;
 	}
 
-	skb_pull(skb_resp, PN533_CMD_DATAEXCH_HEAD_LEN);
-	skb_trim(skb_resp, skb_resp->len - PN533_FRAME_TAIL_SIZE);
+	skb = pn533_build_response(dev);
+	if (skb == NULL)
+		goto error;
 
-	arg->cb(arg->cb_context, skb_resp, 0);
+	arg->cb(arg->cb_context, skb, 0);
 	kfree(arg);
 	return 0;
 
 error:
-	dev_kfree_skb_irq(skb_resp);
+	skb_queue_purge(&dev->resp_q);
+	dev_kfree_skb(skb_resp);
 	arg->cb(arg->cb_context, NULL, err);
 	kfree(arg);
 	return 0;
 }
 
-static int pn533_data_exchange(struct nfc_dev *nfc_dev, u32 target_idx,
-						struct sk_buff *skb,
-						data_exchange_cb_t cb,
-						void *cb_context)
+static int pn533_data_exchange(struct nfc_dev *nfc_dev,
+			       struct nfc_target *target, struct sk_buff *skb,
+			       data_exchange_cb_t cb, void *cb_context)
 {
 	struct pn533 *dev = nfc_get_drvdata(nfc_dev);
 	struct pn533_frame *out_frame, *in_frame;
@@ -1571,6 +1618,68 @@ error:
 	return rc;
 }
 
+static void pn533_wq_mi_recv(struct work_struct *work)
+{
+	struct pn533 *dev = container_of(work, struct pn533, mi_work);
+	struct sk_buff *skb_cmd;
+	struct pn533_data_exchange_arg *arg = dev->cmd_complete_arg;
+	struct pn533_frame *out_frame, *in_frame;
+	struct sk_buff *skb_resp;
+	int skb_resp_len;
+	int rc;
+
+	nfc_dev_dbg(&dev->interface->dev, "%s", __func__);
+
+	/* This is a zero payload size skb */
+	skb_cmd = alloc_skb(PN533_CMD_DATAEXCH_HEAD_LEN + PN533_FRAME_TAIL_SIZE,
+			    GFP_KERNEL);
+	if (skb_cmd == NULL)
+		goto error_cmd;
+
+	skb_reserve(skb_cmd, PN533_CMD_DATAEXCH_HEAD_LEN);
+
+	rc = pn533_data_exchange_tx_frame(dev, skb_cmd);
+	if (rc)
+		goto error_frame;
+
+	skb_resp_len = PN533_CMD_DATAEXCH_HEAD_LEN +
+			PN533_CMD_DATAEXCH_DATA_MAXLEN +
+			PN533_FRAME_TAIL_SIZE;
+	skb_resp = alloc_skb(skb_resp_len, GFP_KERNEL);
+	if (!skb_resp) {
+		rc = -ENOMEM;
+		goto error_frame;
+	}
+
+	in_frame = (struct pn533_frame *) skb_resp->data;
+	out_frame = (struct pn533_frame *) skb_cmd->data;
+
+	arg->skb_resp = skb_resp;
+	arg->skb_out = skb_cmd;
+
+	rc = __pn533_send_cmd_frame_async(dev, out_frame, in_frame,
+					  skb_resp_len,
+					  pn533_data_exchange_complete,
+					  dev->cmd_complete_arg, GFP_KERNEL);
+	if (!rc)
+		return;
+
+	nfc_dev_err(&dev->interface->dev, "Error %d when trying to"
+						" perform data_exchange", rc);
+
+	kfree_skb(skb_resp);
+
+error_frame:
+	kfree_skb(skb_cmd);
+
+error_cmd:
+	pn533_send_ack(dev, GFP_KERNEL);
+
+	kfree(arg);
+
+	up(&dev->cmd_lock);
+}
+
 static int pn533_set_configuration(struct pn533 *dev, u8 cfgitem, u8 *cfgdata,
 								u8 cfgdata_len)
 {
@@ -1668,7 +1777,15 @@ static int pn533_probe(struct usb_interface *interface,
 			NULL, 0,
 			pn533_send_complete, dev);
 
-	tasklet_init(&dev->tasklet, pn533_tasklet_cmd_complete, (ulong)dev);
+	INIT_WORK(&dev->cmd_work, pn533_wq_cmd_complete);
+	INIT_WORK(&dev->mi_work, pn533_wq_mi_recv);
+	dev->wq = alloc_workqueue("pn533",
+				  WQ_NON_REENTRANT | WQ_UNBOUND | WQ_MEM_RECLAIM,
+				  1);
+	if (dev->wq == NULL)
+		goto error;
+
+	skb_queue_head_init(&dev->resp_q);
 
 	usb_set_intfdata(interface, dev);
 
@@ -1678,7 +1795,7 @@ static int pn533_probe(struct usb_interface *interface,
 	rc = pn533_send_cmd_frame_sync(dev, dev->out_frame, dev->in_frame,
 								dev->in_maxlen);
 	if (rc)
-		goto kill_tasklet;
+		goto destroy_wq;
 
 	fw_ver = (struct pn533_fw_version *)
 				PN533_FRAME_CMD_PARAMS_PTR(dev->in_frame);
@@ -1694,7 +1811,7 @@ static int pn533_probe(struct usb_interface *interface,
 					   PN533_CMD_DATAEXCH_HEAD_LEN,
 					   PN533_FRAME_TAIL_SIZE);
 	if (!dev->nfc_dev)
-		goto kill_tasklet;
+		goto destroy_wq;
 
 	nfc_set_parent_dev(dev->nfc_dev, &interface->dev);
 	nfc_set_drvdata(dev->nfc_dev, dev);
@@ -1720,8 +1837,8 @@ static int pn533_probe(struct usb_interface *interface,
 
 free_nfc_dev:
 	nfc_free_device(dev->nfc_dev);
-kill_tasklet:
-	tasklet_kill(&dev->tasklet);
+destroy_wq:
+	destroy_workqueue(dev->wq);
 error:
 	kfree(dev->in_frame);
 	usb_free_urb(dev->in_urb);
@@ -1744,7 +1861,9 @@ static void pn533_disconnect(struct usb_interface *interface)
 	usb_kill_urb(dev->in_urb);
 	usb_kill_urb(dev->out_urb);
 
-	tasklet_kill(&dev->tasklet);
+	destroy_workqueue(dev->wq);
+
+	skb_queue_purge(&dev->resp_q);
 
 	kfree(dev->in_frame);
 	usb_free_urb(dev->in_urb);
diff --git a/drivers/nfc/pn544_hci.c b/drivers/nfc/pn544_hci.c
new file mode 100644
index 00000000000..46f4a9f9f5e
--- /dev/null
+++ b/drivers/nfc/pn544_hci.c
@@ -0,0 +1,947 @@
+/*
+ * HCI based Driver for NXP PN544 NFC Chip
+ *
+ * Copyright (C) 2012  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/crc-ccitt.h>
+#include <linux/module.h>
+#include <linux/delay.h>
+#include <linux/slab.h>
+#include <linux/miscdevice.h>
+#include <linux/interrupt.h>
+#include <linux/gpio.h>
+#include <linux/i2c.h>
+
+#include <linux/nfc.h>
+#include <net/nfc/hci.h>
+#include <net/nfc/shdlc.h>
+
+#include <linux/nfc/pn544.h>
+
+#define DRIVER_DESC "HCI NFC driver for PN544"
+
+#define PN544_HCI_DRIVER_NAME "pn544_hci"
+
+/* Timing restrictions (ms) */
+#define PN544_HCI_RESETVEN_TIME		30
+
+static struct i2c_device_id pn544_hci_id_table[] = {
+	{"pn544", 0},
+	{}
+};
+
+MODULE_DEVICE_TABLE(i2c, pn544_hci_id_table);
+
+#define HCI_MODE 0
+#define FW_MODE 1
+
+/* framing in HCI mode */
+#define PN544_HCI_LLC_LEN		1
+#define PN544_HCI_LLC_CRC		2
+#define PN544_HCI_LLC_LEN_CRC		(PN544_HCI_LLC_LEN + PN544_HCI_LLC_CRC)
+#define PN544_HCI_LLC_MIN_SIZE		(1 + PN544_HCI_LLC_LEN_CRC)
+#define PN544_HCI_LLC_MAX_PAYLOAD	29
+#define PN544_HCI_LLC_MAX_SIZE		(PN544_HCI_LLC_LEN_CRC + 1 + \
+					 PN544_HCI_LLC_MAX_PAYLOAD)
+
+enum pn544_state {
+	PN544_ST_COLD,
+	PN544_ST_FW_READY,
+	PN544_ST_READY,
+};
+
+#define FULL_VERSION_LEN 11
+
+/* Proprietary commands */
+#define PN544_WRITE		0x3f
+
+/* Proprietary gates, events, commands and registers */
+
+/* NFC_HCI_RF_READER_A_GATE additional registers and commands */
+#define PN544_RF_READER_A_AUTO_ACTIVATION			0x10
+#define PN544_RF_READER_A_CMD_CONTINUE_ACTIVATION		0x12
+#define PN544_MIFARE_CMD					0x21
+
+/* Commands that apply to all RF readers */
+#define PN544_RF_READER_CMD_PRESENCE_CHECK	0x30
+#define PN544_RF_READER_CMD_ACTIVATE_NEXT	0x32
+
+/* NFC_HCI_ID_MGMT_GATE additional registers */
+#define PN544_ID_MGMT_FULL_VERSION_SW		0x10
+
+#define PN544_RF_READER_ISO15693_GATE		0x12
+
+#define PN544_RF_READER_F_GATE			0x14
+#define PN544_FELICA_ID				0x04
+#define PN544_FELICA_RAW			0x20
+
+#define PN544_RF_READER_JEWEL_GATE		0x15
+#define PN544_JEWEL_RAW_CMD			0x23
+
+#define PN544_RF_READER_NFCIP1_INITIATOR_GATE	0x30
+#define PN544_RF_READER_NFCIP1_TARGET_GATE	0x31
+
+#define PN544_SYS_MGMT_GATE			0x90
+#define PN544_SYS_MGMT_INFO_NOTIFICATION	0x02
+
+#define PN544_POLLING_LOOP_MGMT_GATE		0x94
+#define PN544_PL_RDPHASES			0x06
+#define PN544_PL_EMULATION			0x07
+#define PN544_PL_NFCT_DEACTIVATED		0x09
+
+#define PN544_SWP_MGMT_GATE			0xA0
+
+#define PN544_NFC_WI_MGMT_GATE			0xA1
+
+static u8 pn544_custom_gates[] = {
+	PN544_SYS_MGMT_GATE,
+	PN544_SWP_MGMT_GATE,
+	PN544_POLLING_LOOP_MGMT_GATE,
+	PN544_NFC_WI_MGMT_GATE,
+	PN544_RF_READER_F_GATE,
+	PN544_RF_READER_JEWEL_GATE,
+	PN544_RF_READER_ISO15693_GATE,
+	PN544_RF_READER_NFCIP1_INITIATOR_GATE,
+	PN544_RF_READER_NFCIP1_TARGET_GATE
+};
+
+/* Largest headroom needed for outgoing custom commands */
+#define PN544_CMDS_HEADROOM	2
+
+struct pn544_hci_info {
+	struct i2c_client *i2c_dev;
+	struct nfc_shdlc *shdlc;
+
+	enum pn544_state state;
+
+	struct mutex info_lock;
+
+	unsigned int gpio_en;
+	unsigned int gpio_irq;
+	unsigned int gpio_fw;
+	unsigned int en_polarity;
+
+	int hard_fault;		/*
+				 * < 0 if hardware error occured (e.g. i2c err)
+				 * and prevents normal operation.
+				 */
+};
+
+static void pn544_hci_platform_init(struct pn544_hci_info *info)
+{
+	int polarity, retry, ret;
+	char rset_cmd[] = { 0x05, 0xF9, 0x04, 0x00, 0xC3, 0xE5 };
+	int count = sizeof(rset_cmd);
+
+	pr_info(DRIVER_DESC ": %s\n", __func__);
+	dev_info(&info->i2c_dev->dev, "Detecting nfc_en polarity\n");
+
+	/* Disable fw download */
+	gpio_set_value(info->gpio_fw, 0);
+
+	for (polarity = 0; polarity < 2; polarity++) {
+		info->en_polarity = polarity;
+		retry = 3;
+		while (retry--) {
+			/* power off */
+			gpio_set_value(info->gpio_en, !info->en_polarity);
+			usleep_range(10000, 15000);
+
+			/* power on */
+			gpio_set_value(info->gpio_en, info->en_polarity);
+			usleep_range(10000, 15000);
+
+			/* send reset */
+			dev_dbg(&info->i2c_dev->dev, "Sending reset cmd\n");
+			ret = i2c_master_send(info->i2c_dev, rset_cmd, count);
+			if (ret == count) {
+				dev_info(&info->i2c_dev->dev,
+					 "nfc_en polarity : active %s\n",
+					 (polarity == 0 ? "low" : "high"));
+				goto out;
+			}
+		}
+	}
+
+	dev_err(&info->i2c_dev->dev,
+		"Could not detect nfc_en polarity, fallback to active high\n");
+
+out:
+	gpio_set_value(info->gpio_en, !info->en_polarity);
+}
+
+static int pn544_hci_enable(struct pn544_hci_info *info, int mode)
+{
+	pr_info(DRIVER_DESC ": %s\n", __func__);
+
+	gpio_set_value(info->gpio_fw, 0);
+	gpio_set_value(info->gpio_en, info->en_polarity);
+	usleep_range(10000, 15000);
+
+	return 0;
+}
+
+static void pn544_hci_disable(struct pn544_hci_info *info)
+{
+	pr_info(DRIVER_DESC ": %s\n", __func__);
+
+	gpio_set_value(info->gpio_fw, 0);
+	gpio_set_value(info->gpio_en, !info->en_polarity);
+	usleep_range(10000, 15000);
+
+	gpio_set_value(info->gpio_en, info->en_polarity);
+	usleep_range(10000, 15000);
+
+	gpio_set_value(info->gpio_en, !info->en_polarity);
+	usleep_range(10000, 15000);
+}
+
+static int pn544_hci_i2c_write(struct i2c_client *client, u8 *buf, int len)
+{
+	int r;
+
+	usleep_range(3000, 6000);
+
+	r = i2c_master_send(client, buf, len);
+
+	if (r == -EREMOTEIO) {	/* Retry, chip was in standby */
+		usleep_range(6000, 10000);
+		r = i2c_master_send(client, buf, len);
+	}
+
+	if (r >= 0 && r != len)
+		r = -EREMOTEIO;
+
+	return r;
+}
+
+static int check_crc(u8 *buf, int buflen)
+{
+	u8 len;
+	u16 crc;
+
+	len = buf[0] + 1;
+	crc = crc_ccitt(0xffff, buf, len - 2);
+	crc = ~crc;
+
+	if (buf[len - 2] != (crc & 0xff) || buf[len - 1] != (crc >> 8)) {
+		pr_err(PN544_HCI_DRIVER_NAME ": CRC error 0x%x != 0x%x 0x%x\n",
+		       crc, buf[len - 1], buf[len - 2]);
+
+		pr_info(DRIVER_DESC ": %s : BAD CRC\n", __func__);
+		print_hex_dump(KERN_DEBUG, "crc: ", DUMP_PREFIX_NONE,
+			       16, 2, buf, buflen, false);
+		return -EPERM;
+	}
+	return 0;
+}
+
+/*
+ * Reads an shdlc frame and returns it in a newly allocated sk_buff. Guarantees
+ * that i2c bus will be flushed and that next read will start on a new frame.
+ * returned skb contains only LLC header and payload.
+ * returns:
+ * -EREMOTEIO : i2c read error (fatal)
+ * -EBADMSG : frame was incorrect and discarded
+ * -ENOMEM : cannot allocate skb, frame dropped
+ */
+static int pn544_hci_i2c_read(struct i2c_client *client, struct sk_buff **skb)
+{
+	int r;
+	u8 len;
+	u8 tmp[PN544_HCI_LLC_MAX_SIZE - 1];
+
+	r = i2c_master_recv(client, &len, 1);
+	if (r != 1) {
+		dev_err(&client->dev, "cannot read len byte\n");
+		return -EREMOTEIO;
+	}
+
+	if ((len < (PN544_HCI_LLC_MIN_SIZE - 1)) ||
+	    (len > (PN544_HCI_LLC_MAX_SIZE - 1))) {
+		dev_err(&client->dev, "invalid len byte\n");
+		r = -EBADMSG;
+		goto flush;
+	}
+
+	*skb = alloc_skb(1 + len, GFP_KERNEL);
+	if (*skb == NULL) {
+		r = -ENOMEM;
+		goto flush;
+	}
+
+	*skb_put(*skb, 1) = len;
+
+	r = i2c_master_recv(client, skb_put(*skb, len), len);
+	if (r != len) {
+		kfree_skb(*skb);
+		return -EREMOTEIO;
+	}
+
+	r = check_crc((*skb)->data, (*skb)->len);
+	if (r != 0) {
+		kfree_skb(*skb);
+		r = -EBADMSG;
+		goto flush;
+	}
+
+	skb_pull(*skb, 1);
+	skb_trim(*skb, (*skb)->len - 2);
+
+	usleep_range(3000, 6000);
+
+	return 0;
+
+flush:
+	if (i2c_master_recv(client, tmp, sizeof(tmp)) < 0)
+		r = -EREMOTEIO;
+
+	usleep_range(3000, 6000);
+
+	return r;
+}
+
+/*
+ * Reads an shdlc frame from the chip. This is not as straightforward as it
+ * seems. There are cases where we could loose the frame start synchronization.
+ * The frame format is len-data-crc, and corruption can occur anywhere while
+ * transiting on i2c bus, such that we could read an invalid len.
+ * In order to recover synchronization with the next frame, we must be sure
+ * to read the real amount of data without using the len byte. We do this by
+ * assuming the following:
+ * - the chip will always present only one single complete frame on the bus
+ *   before triggering the interrupt
+ * - the chip will not present a new frame until we have completely read
+ *   the previous one (or until we have handled the interrupt).
+ * The tricky case is when we read a corrupted len that is less than the real
+ * len. We must detect this here in order to determine that we need to flush
+ * the bus. This is the reason why we check the crc here.
+ */
+static irqreturn_t pn544_hci_irq_thread_fn(int irq, void *dev_id)
+{
+	struct pn544_hci_info *info = dev_id;
+	struct i2c_client *client = info->i2c_dev;
+	struct sk_buff *skb = NULL;
+	int r;
+
+	BUG_ON(!info);
+	BUG_ON(irq != info->i2c_dev->irq);
+
+	dev_dbg(&client->dev, "IRQ\n");
+
+	if (info->hard_fault != 0)
+		return IRQ_HANDLED;
+
+	r = pn544_hci_i2c_read(client, &skb);
+	if (r == -EREMOTEIO) {
+		info->hard_fault = r;
+
+		nfc_shdlc_recv_frame(info->shdlc, NULL);
+
+		return IRQ_HANDLED;
+	} else if ((r == -ENOMEM) || (r == -EBADMSG)) {
+		return IRQ_HANDLED;
+	}
+
+	nfc_shdlc_recv_frame(info->shdlc, skb);
+
+	return IRQ_HANDLED;
+}
+
+static int pn544_hci_open(struct nfc_shdlc *shdlc)
+{
+	struct pn544_hci_info *info = nfc_shdlc_get_clientdata(shdlc);
+	int r = 0;
+
+	mutex_lock(&info->info_lock);
+
+	if (info->state != PN544_ST_COLD) {
+		r = -EBUSY;
+		goto out;
+	}
+
+	r = pn544_hci_enable(info, HCI_MODE);
+
+out:
+	mutex_unlock(&info->info_lock);
+	return r;
+}
+
+static void pn544_hci_close(struct nfc_shdlc *shdlc)
+{
+	struct pn544_hci_info *info = nfc_shdlc_get_clientdata(shdlc);
+
+	mutex_lock(&info->info_lock);
+
+	if (info->state == PN544_ST_COLD)
+		goto out;
+
+	pn544_hci_disable(info);
+
+out:
+	mutex_unlock(&info->info_lock);
+}
+
+static int pn544_hci_ready(struct nfc_shdlc *shdlc)
+{
+	struct nfc_hci_dev *hdev = nfc_shdlc_get_hci_dev(shdlc);
+	struct sk_buff *skb;
+	static struct hw_config {
+		u8 adr[2];
+		u8 value;
+	} hw_config[] = {
+		{{0x9f, 0x9a}, 0x00},
+
+		{{0x98, 0x10}, 0xbc},
+
+		{{0x9e, 0x71}, 0x00},
+
+		{{0x98, 0x09}, 0x00},
+
+		{{0x9e, 0xb4}, 0x00},
+
+		{{0x9e, 0xd9}, 0xff},
+		{{0x9e, 0xda}, 0xff},
+		{{0x9e, 0xdb}, 0x23},
+		{{0x9e, 0xdc}, 0x21},
+		{{0x9e, 0xdd}, 0x22},
+		{{0x9e, 0xde}, 0x24},
+
+		{{0x9c, 0x01}, 0x08},
+
+		{{0x9e, 0xaa}, 0x01},
+
+		{{0x9b, 0xd1}, 0x0d},
+		{{0x9b, 0xd2}, 0x24},
+		{{0x9b, 0xd3}, 0x0a},
+		{{0x9b, 0xd4}, 0x22},
+		{{0x9b, 0xd5}, 0x08},
+		{{0x9b, 0xd6}, 0x1e},
+		{{0x9b, 0xdd}, 0x1c},
+
+		{{0x9b, 0x84}, 0x13},
+		{{0x99, 0x81}, 0x7f},
+		{{0x99, 0x31}, 0x70},
+
+		{{0x98, 0x00}, 0x3f},
+
+		{{0x9f, 0x09}, 0x00},
+
+		{{0x9f, 0x0a}, 0x05},
+
+		{{0x9e, 0xd1}, 0xa1},
+		{{0x99, 0x23}, 0x00},
+
+		{{0x9e, 0x74}, 0x80},
+
+		{{0x9f, 0x28}, 0x10},
+
+		{{0x9f, 0x35}, 0x14},
+
+		{{0x9f, 0x36}, 0x60},
+
+		{{0x9c, 0x31}, 0x00},
+
+		{{0x9c, 0x32}, 0xc8},
+
+		{{0x9c, 0x19}, 0x40},
+
+		{{0x9c, 0x1a}, 0x40},
+
+		{{0x9c, 0x0c}, 0x00},
+
+		{{0x9c, 0x0d}, 0x00},
+
+		{{0x9c, 0x12}, 0x00},
+
+		{{0x9c, 0x13}, 0x00},
+
+		{{0x98, 0xa2}, 0x0e},
+
+		{{0x98, 0x93}, 0x40},
+
+		{{0x98, 0x7d}, 0x02},
+		{{0x98, 0x7e}, 0x00},
+		{{0x9f, 0xc8}, 0x01},
+	};
+	struct hw_config *p = hw_config;
+	int count = ARRAY_SIZE(hw_config);
+	struct sk_buff *res_skb;
+	u8 param[4];
+	int r;
+
+	param[0] = 0;
+	while (count--) {
+		param[1] = p->adr[0];
+		param[2] = p->adr[1];
+		param[3] = p->value;
+
+		r = nfc_hci_send_cmd(hdev, PN544_SYS_MGMT_GATE, PN544_WRITE,
+				     param, 4, &res_skb);
+		if (r < 0)
+			return r;
+
+		if (res_skb->len != 1) {
+			kfree_skb(res_skb);
+			return -EPROTO;
+		}
+
+		if (res_skb->data[0] != p->value) {
+			kfree_skb(res_skb);
+			return -EIO;
+		}
+
+		kfree_skb(res_skb);
+
+		p++;
+	}
+
+	param[0] = NFC_HCI_UICC_HOST_ID;
+	r = nfc_hci_set_param(hdev, NFC_HCI_ADMIN_GATE,
+			      NFC_HCI_ADMIN_WHITELIST, param, 1);
+	if (r < 0)
+		return r;
+
+	param[0] = 0x3d;
+	r = nfc_hci_set_param(hdev, PN544_SYS_MGMT_GATE,
+			      PN544_SYS_MGMT_INFO_NOTIFICATION, param, 1);
+	if (r < 0)
+		return r;
+
+	param[0] = 0x0;
+	r = nfc_hci_set_param(hdev, NFC_HCI_RF_READER_A_GATE,
+			      PN544_RF_READER_A_AUTO_ACTIVATION, param, 1);
+	if (r < 0)
+		return r;
+
+	r = nfc_hci_send_event(hdev, NFC_HCI_RF_READER_A_GATE,
+			       NFC_HCI_EVT_END_OPERATION, NULL, 0);
+	if (r < 0)
+		return r;
+
+	param[0] = 0x1;
+	r = nfc_hci_set_param(hdev, PN544_POLLING_LOOP_MGMT_GATE,
+			      PN544_PL_NFCT_DEACTIVATED, param, 1);
+	if (r < 0)
+		return r;
+
+	param[0] = 0x0;
+	r = nfc_hci_set_param(hdev, PN544_POLLING_LOOP_MGMT_GATE,
+			      PN544_PL_RDPHASES, param, 1);
+	if (r < 0)
+		return r;
+
+	r = nfc_hci_get_param(hdev, NFC_HCI_ID_MGMT_GATE,
+			      PN544_ID_MGMT_FULL_VERSION_SW, &skb);
+	if (r < 0)
+		return r;
+
+	if (skb->len != FULL_VERSION_LEN) {
+		kfree_skb(skb);
+		return -EINVAL;
+	}
+
+	print_hex_dump(KERN_DEBUG, "FULL VERSION SOFTWARE INFO: ",
+		       DUMP_PREFIX_NONE, 16, 1,
+		       skb->data, FULL_VERSION_LEN, false);
+
+	kfree_skb(skb);
+
+	return 0;
+}
+
+static int pn544_hci_xmit(struct nfc_shdlc *shdlc, struct sk_buff *skb)
+{
+	struct pn544_hci_info *info = nfc_shdlc_get_clientdata(shdlc);
+	struct i2c_client *client = info->i2c_dev;
+
+	if (info->hard_fault != 0)
+		return info->hard_fault;
+
+	return pn544_hci_i2c_write(client, skb->data, skb->len);
+}
+
+static int pn544_hci_start_poll(struct nfc_shdlc *shdlc, u32 protocols)
+{
+	struct nfc_hci_dev *hdev = nfc_shdlc_get_hci_dev(shdlc);
+	u8 phases = 0;
+	int r;
+	u8 duration[2];
+	u8 activated;
+
+	pr_info(DRIVER_DESC ": %s protocols = %d\n", __func__, protocols);
+
+	r = nfc_hci_send_event(hdev, NFC_HCI_RF_READER_A_GATE,
+			       NFC_HCI_EVT_END_OPERATION, NULL, 0);
+	if (r < 0)
+		return r;
+
+	duration[0] = 0x18;
+	duration[1] = 0x6a;
+	r = nfc_hci_set_param(hdev, PN544_POLLING_LOOP_MGMT_GATE,
+			      PN544_PL_EMULATION, duration, 2);
+	if (r < 0)
+		return r;
+
+	activated = 0;
+	r = nfc_hci_set_param(hdev, PN544_POLLING_LOOP_MGMT_GATE,
+			      PN544_PL_NFCT_DEACTIVATED, &activated, 1);
+	if (r < 0)
+		return r;
+
+	if (protocols & (NFC_PROTO_ISO14443_MASK | NFC_PROTO_MIFARE_MASK |
+			 NFC_PROTO_JEWEL_MASK))
+		phases |= 1;		/* Type A */
+	if (protocols & NFC_PROTO_FELICA_MASK) {
+		phases |= (1 << 2);	/* Type F 212 */
+		phases |= (1 << 3);	/* Type F 424 */
+	}
+
+	phases |= (1 << 5);		/* NFC active */
+
+	r = nfc_hci_set_param(hdev, PN544_POLLING_LOOP_MGMT_GATE,
+			      PN544_PL_RDPHASES, &phases, 1);
+	if (r < 0)
+		return r;
+
+	r = nfc_hci_send_event(hdev, NFC_HCI_RF_READER_A_GATE,
+			       NFC_HCI_EVT_READER_REQUESTED, NULL, 0);
+	if (r < 0)
+		nfc_hci_send_event(hdev, NFC_HCI_RF_READER_A_GATE,
+				   NFC_HCI_EVT_END_OPERATION, NULL, 0);
+
+	return r;
+}
+
+static int pn544_hci_target_from_gate(struct nfc_shdlc *shdlc, u8 gate,
+				      struct nfc_target *target)
+{
+	switch (gate) {
+	case PN544_RF_READER_F_GATE:
+		target->supported_protocols = NFC_PROTO_FELICA_MASK;
+		break;
+	case PN544_RF_READER_JEWEL_GATE:
+		target->supported_protocols = NFC_PROTO_JEWEL_MASK;
+		target->sens_res = 0x0c00;
+		break;
+	default:
+		return -EPROTO;
+	}
+
+	return 0;
+}
+
+static int pn544_hci_complete_target_discovered(struct nfc_shdlc *shdlc,
+						u8 gate,
+						struct nfc_target *target)
+{
+	struct nfc_hci_dev *hdev = nfc_shdlc_get_hci_dev(shdlc);
+	struct sk_buff *uid_skb;
+	int r = 0;
+
+	if (target->supported_protocols & NFC_PROTO_MIFARE_MASK) {
+		if (target->nfcid1_len != 4 && target->nfcid1_len != 7 &&
+		    target->nfcid1_len != 10)
+			return -EPROTO;
+
+		r = nfc_hci_send_cmd(hdev, NFC_HCI_RF_READER_A_GATE,
+				     PN544_RF_READER_CMD_ACTIVATE_NEXT,
+				     target->nfcid1, target->nfcid1_len, NULL);
+	} else if (target->supported_protocols & NFC_PROTO_FELICA_MASK) {
+		r = nfc_hci_get_param(hdev, PN544_RF_READER_F_GATE,
+				      PN544_FELICA_ID, &uid_skb);
+		if (r < 0)
+			return r;
+
+		if (uid_skb->len != 8) {
+			kfree_skb(uid_skb);
+			return -EPROTO;
+		}
+
+		r = nfc_hci_send_cmd(hdev, PN544_RF_READER_F_GATE,
+				     PN544_RF_READER_CMD_ACTIVATE_NEXT,
+				     uid_skb->data, uid_skb->len, NULL);
+		kfree_skb(uid_skb);
+	} else if (target->supported_protocols & NFC_PROTO_ISO14443_MASK) {
+		/*
+		 * TODO: maybe other ISO 14443 require some kind of continue
+		 * activation, but for now we've seen only this one below.
+		 */
+		if (target->sens_res == 0x4403)	/* Type 4 Mifare DESFire */
+			r = nfc_hci_send_cmd(hdev, NFC_HCI_RF_READER_A_GATE,
+			      PN544_RF_READER_A_CMD_CONTINUE_ACTIVATION,
+			      NULL, 0, NULL);
+	}
+
+	return r;
+}
+
+#define MIFARE_CMD_AUTH_KEY_A	0x60
+#define MIFARE_CMD_AUTH_KEY_B	0x61
+#define MIFARE_CMD_HEADER	2
+#define MIFARE_UID_LEN		4
+#define MIFARE_KEY_LEN		6
+#define MIFARE_CMD_LEN		12
+/*
+ * Returns:
+ * <= 0: driver handled the data exchange
+ *    1: driver doesn't especially handle, please do standard processing
+ */
+static int pn544_hci_data_exchange(struct nfc_shdlc *shdlc,
+				   struct nfc_target *target,
+				   struct sk_buff *skb,
+				   struct sk_buff **res_skb)
+{
+	struct nfc_hci_dev *hdev = nfc_shdlc_get_hci_dev(shdlc);
+	int r;
+
+	pr_info(DRIVER_DESC ": %s for gate=%d\n", __func__,
+		target->hci_reader_gate);
+
+	switch (target->hci_reader_gate) {
+	case NFC_HCI_RF_READER_A_GATE:
+		if (target->supported_protocols & NFC_PROTO_MIFARE_MASK) {
+			/*
+			 * It seems that pn544 is inverting key and UID for
+			 * MIFARE authentication commands.
+			 */
+			if (skb->len == MIFARE_CMD_LEN &&
+			    (skb->data[0] == MIFARE_CMD_AUTH_KEY_A ||
+			     skb->data[0] == MIFARE_CMD_AUTH_KEY_B)) {
+				u8 uid[MIFARE_UID_LEN];
+				u8 *data = skb->data + MIFARE_CMD_HEADER;
+
+				memcpy(uid, data + MIFARE_KEY_LEN,
+				       MIFARE_UID_LEN);
+				memmove(data + MIFARE_UID_LEN, data,
+					MIFARE_KEY_LEN);
+				memcpy(data, uid, MIFARE_UID_LEN);
+			}
+
+			return nfc_hci_send_cmd(hdev, target->hci_reader_gate,
+						PN544_MIFARE_CMD,
+						skb->data, skb->len, res_skb);
+		} else
+			return 1;
+	case PN544_RF_READER_F_GATE:
+		*skb_push(skb, 1) = 0;
+		*skb_push(skb, 1) = 0;
+
+		r = nfc_hci_send_cmd(hdev, target->hci_reader_gate,
+				     PN544_FELICA_RAW,
+				     skb->data, skb->len, res_skb);
+		if (r == 0)
+			skb_pull(*res_skb, 1);
+		return r;
+	case PN544_RF_READER_JEWEL_GATE:
+		return nfc_hci_send_cmd(hdev, target->hci_reader_gate,
+					PN544_JEWEL_RAW_CMD,
+					skb->data, skb->len, res_skb);
+	default:
+		return 1;
+	}
+}
+
+static int pn544_hci_check_presence(struct nfc_shdlc *shdlc,
+				   struct nfc_target *target)
+{
+	struct nfc_hci_dev *hdev = nfc_shdlc_get_hci_dev(shdlc);
+
+	return nfc_hci_send_cmd(hdev, target->hci_reader_gate,
+				PN544_RF_READER_CMD_PRESENCE_CHECK,
+				NULL, 0, NULL);
+}
+
+static struct nfc_shdlc_ops pn544_shdlc_ops = {
+	.open = pn544_hci_open,
+	.close = pn544_hci_close,
+	.hci_ready = pn544_hci_ready,
+	.xmit = pn544_hci_xmit,
+	.start_poll = pn544_hci_start_poll,
+	.target_from_gate = pn544_hci_target_from_gate,
+	.complete_target_discovered = pn544_hci_complete_target_discovered,
+	.data_exchange = pn544_hci_data_exchange,
+	.check_presence = pn544_hci_check_presence,
+};
+
+static int __devinit pn544_hci_probe(struct i2c_client *client,
+				     const struct i2c_device_id *id)
+{
+	struct pn544_hci_info *info;
+	struct pn544_nfc_platform_data *pdata;
+	int r = 0;
+	u32 protocols;
+	struct nfc_hci_init_data init_data;
+
+	dev_dbg(&client->dev, "%s\n", __func__);
+	dev_dbg(&client->dev, "IRQ: %d\n", client->irq);
+
+	if (!i2c_check_functionality(client->adapter, I2C_FUNC_I2C)) {
+		dev_err(&client->dev, "Need I2C_FUNC_I2C\n");
+		return -ENODEV;
+	}
+
+	info = kzalloc(sizeof(struct pn544_hci_info), GFP_KERNEL);
+	if (!info) {
+		dev_err(&client->dev,
+			"Cannot allocate memory for pn544_hci_info.\n");
+		r = -ENOMEM;
+		goto err_info_alloc;
+	}
+
+	info->i2c_dev = client;
+	info->state = PN544_ST_COLD;
+	mutex_init(&info->info_lock);
+	i2c_set_clientdata(client, info);
+
+	pdata = client->dev.platform_data;
+	if (pdata == NULL) {
+		dev_err(&client->dev, "No platform data\n");
+		r = -EINVAL;
+		goto err_pdata;
+	}
+
+	if (pdata->request_resources == NULL) {
+		dev_err(&client->dev, "request_resources() missing\n");
+		r = -EINVAL;
+		goto err_pdata;
+	}
+
+	r = pdata->request_resources(client);
+	if (r) {
+		dev_err(&client->dev, "Cannot get platform resources\n");
+		goto err_pdata;
+	}
+
+	info->gpio_en = pdata->get_gpio(NFC_GPIO_ENABLE);
+	info->gpio_fw = pdata->get_gpio(NFC_GPIO_FW_RESET);
+	info->gpio_irq = pdata->get_gpio(NFC_GPIO_IRQ);
+
+	pn544_hci_platform_init(info);
+
+	r = request_threaded_irq(client->irq, NULL, pn544_hci_irq_thread_fn,
+				 IRQF_TRIGGER_RISING, PN544_HCI_DRIVER_NAME,
+				 info);
+	if (r < 0) {
+		dev_err(&client->dev, "Unable to register IRQ handler\n");
+		goto err_rti;
+	}
+
+	init_data.gate_count = ARRAY_SIZE(pn544_custom_gates);
+
+	memcpy(init_data.gates, pn544_custom_gates,
+	       ARRAY_SIZE(pn544_custom_gates));
+
+	/*
+	 * TODO: Session id must include the driver name + some bus addr
+	 * persistent info to discriminate 2 identical chips
+	 */
+	strcpy(init_data.session_id, "ID544HCI");
+
+	protocols = NFC_PROTO_JEWEL_MASK |
+		    NFC_PROTO_MIFARE_MASK |
+		    NFC_PROTO_FELICA_MASK |
+		    NFC_PROTO_ISO14443_MASK |
+		    NFC_PROTO_NFC_DEP_MASK;
+
+	info->shdlc = nfc_shdlc_allocate(&pn544_shdlc_ops,
+					 &init_data, protocols,
+					 PN544_CMDS_HEADROOM, 0,
+					 PN544_HCI_LLC_MAX_PAYLOAD,
+					 dev_name(&client->dev));
+	if (!info->shdlc) {
+		dev_err(&client->dev, "Cannot allocate nfc shdlc.\n");
+		r = -ENOMEM;
+		goto err_allocshdlc;
+	}
+
+	nfc_shdlc_set_clientdata(info->shdlc, info);
+
+	return 0;
+
+err_allocshdlc:
+	free_irq(client->irq, info);
+
+err_rti:
+	if (pdata->free_resources != NULL)
+		pdata->free_resources();
+
+err_pdata:
+	kfree(info);
+
+err_info_alloc:
+	return r;
+}
+
+static __devexit int pn544_hci_remove(struct i2c_client *client)
+{
+	struct pn544_hci_info *info = i2c_get_clientdata(client);
+	struct pn544_nfc_platform_data *pdata = client->dev.platform_data;
+
+	dev_dbg(&client->dev, "%s\n", __func__);
+
+	nfc_shdlc_free(info->shdlc);
+
+	if (info->state != PN544_ST_COLD) {
+		if (pdata->disable)
+			pdata->disable();
+	}
+
+	free_irq(client->irq, info);
+	if (pdata->free_resources)
+		pdata->free_resources();
+
+	kfree(info);
+
+	return 0;
+}
+
+static struct i2c_driver pn544_hci_driver = {
+	.driver = {
+		   .name = PN544_HCI_DRIVER_NAME,
+		  },
+	.probe = pn544_hci_probe,
+	.id_table = pn544_hci_id_table,
+	.remove = __devexit_p(pn544_hci_remove),
+};
+
+static int __init pn544_hci_init(void)
+{
+	int r;
+
+	pr_debug(DRIVER_DESC ": %s\n", __func__);
+
+	r = i2c_add_driver(&pn544_hci_driver);
+	if (r) {
+		pr_err(PN544_HCI_DRIVER_NAME ": driver registration failed\n");
+		return r;
+	}
+
+	return 0;
+}
+
+static void __exit pn544_hci_exit(void)
+{
+	i2c_del_driver(&pn544_hci_driver);
+}
+
+module_init(pn544_hci_init);
+module_exit(pn544_hci_exit);
+
+MODULE_LICENSE("GPL");
+MODULE_DESCRIPTION(DRIVER_DESC);
diff --git a/drivers/of/Kconfig b/drivers/of/Kconfig
index 8e84ce9765a..dfba3e64d59 100644
--- a/drivers/of/Kconfig
+++ b/drivers/of/Kconfig
@@ -51,12 +51,6 @@ config OF_IRQ
 config OF_DEVICE
 	def_bool y
 
-config OF_GPIO
-	def_bool y
-	depends on GPIOLIB && !SPARC
-	help
-	  OpenFirmware GPIO accessors
-
 config OF_I2C
 	def_tristate I2C
 	depends on I2C && !SPARC
@@ -67,12 +61,6 @@ config OF_NET
 	depends on NETDEVICES
 	def_bool y
 
-config OF_SPI
-	def_tristate SPI
-	depends on SPI && !SPARC
-	help
-	  OpenFirmware SPI accessors
-
 config OF_MDIO
 	def_tristate PHYLIB
 	depends on PHYLIB
diff --git a/drivers/of/Makefile b/drivers/of/Makefile
index aa90e602c8a..e027f444d10 100644
--- a/drivers/of/Makefile
+++ b/drivers/of/Makefile
@@ -4,10 +4,8 @@ obj-$(CONFIG_OF_PROMTREE) += pdt.o
 obj-$(CONFIG_OF_ADDRESS)  += address.o
 obj-$(CONFIG_OF_IRQ)    += irq.o
 obj-$(CONFIG_OF_DEVICE) += device.o platform.o
-obj-$(CONFIG_OF_GPIO)   += gpio.o
 obj-$(CONFIG_OF_I2C)	+= of_i2c.o
 obj-$(CONFIG_OF_NET)	+= of_net.o
-obj-$(CONFIG_OF_SPI)	+= of_spi.o
 obj-$(CONFIG_OF_SELFTEST) += selftest.o
 obj-$(CONFIG_OF_MDIO)	+= of_mdio.o
 obj-$(CONFIG_OF_PCI)	+= of_pci.o
diff --git a/drivers/of/address.c b/drivers/of/address.c
index 66d96f14c27..7e262a6124c 100644
--- a/drivers/of/address.c
+++ b/drivers/of/address.c
@@ -1,4 +1,5 @@
 
+#include <linux/device.h>
 #include <linux/io.h>
 #include <linux/ioport.h>
 #include <linux/module.h>
diff --git a/drivers/of/base.c b/drivers/of/base.c
index 58064498694..d9bfd49b193 100644
--- a/drivers/of/base.c
+++ b/drivers/of/base.c
@@ -1260,3 +1260,44 @@ int of_alias_get_id(struct device_node *np, const char *stem)
 	return id;
 }
 EXPORT_SYMBOL_GPL(of_alias_get_id);
+
+const __be32 *of_prop_next_u32(struct property *prop, const __be32 *cur,
+			       u32 *pu)
+{
+	const void *curv = cur;
+
+	if (!prop)
+		return NULL;
+
+	if (!cur) {
+		curv = prop->value;
+		goto out_val;
+	}
+
+	curv += sizeof(*cur);
+	if (curv >= prop->value + prop->length)
+		return NULL;
+
+out_val:
+	*pu = be32_to_cpup(curv);
+	return curv;
+}
+EXPORT_SYMBOL_GPL(of_prop_next_u32);
+
+const char *of_prop_next_string(struct property *prop, const char *cur)
+{
+	const void *curv = cur;
+
+	if (!prop)
+		return NULL;
+
+	if (!cur)
+		return prop->value;
+
+	curv += strlen(cur) + 1;
+	if (curv >= prop->value + prop->length)
+		return NULL;
+
+	return curv;
+}
+EXPORT_SYMBOL_GPL(of_prop_next_string);
diff --git a/drivers/of/of_i2c.c b/drivers/of/of_i2c.c
index f37fbeb66a4..1e173f35767 100644
--- a/drivers/of/of_i2c.c
+++ b/drivers/of/of_i2c.c
@@ -90,8 +90,22 @@ struct i2c_client *of_find_i2c_device_by_node(struct device_node *node)
 	if (!dev)
 		return NULL;
 
-	return to_i2c_client(dev);
+	return i2c_verify_client(dev);
 }
 EXPORT_SYMBOL(of_find_i2c_device_by_node);
 
+/* must call put_device() when done with returned i2c_adapter device */
+struct i2c_adapter *of_find_i2c_adapter_by_node(struct device_node *node)
+{
+	struct device *dev;
+
+	dev = bus_find_device(&i2c_bus_type, NULL, node,
+					 of_dev_node_match);
+	if (!dev)
+		return NULL;
+
+	return i2c_verify_adapter(dev);
+}
+EXPORT_SYMBOL(of_find_i2c_adapter_by_node);
+
 MODULE_LICENSE("GPL");
diff --git a/drivers/of/of_mdio.c b/drivers/of/of_mdio.c
index 483c0adcad8..2574abde8d9 100644
--- a/drivers/of/of_mdio.c
+++ b/drivers/of/of_mdio.c
@@ -45,6 +45,8 @@ int of_mdiobus_register(struct mii_bus *mdio, struct device_node *np)
 		for (i=0; i<PHY_MAX_ADDR; i++)
 			mdio->irq[i] = PHY_POLL;
 
+	mdio->dev.of_node = np;
+
 	/* Register the MDIO bus */
 	rc = mdiobus_register(mdio);
 	if (rc)
diff --git a/drivers/of/of_pci_irq.c b/drivers/of/of_pci_irq.c
index 93125163dea..67705381321 100644
--- a/drivers/of/of_pci_irq.c
+++ b/drivers/of/of_pci_irq.c
@@ -15,7 +15,7 @@
  * PCI tree until an device-node is found, at which point it will finish
  * resolving using the OF tree walking.
  */
-int of_irq_map_pci(struct pci_dev *pdev, struct of_irq *out_irq)
+int of_irq_map_pci(const struct pci_dev *pdev, struct of_irq *out_irq)
 {
 	struct device_node *dn, *ppnode;
 	struct pci_dev *ppdev;
diff --git a/drivers/of/of_spi.c b/drivers/of/of_spi.c
deleted file mode 100644
index 6dbc074e487..00000000000
--- a/drivers/of/of_spi.c
+++ /dev/null
@@ -1,99 +0,0 @@
-/*
- * SPI OF support routines
- * Copyright (C) 2008 Secret Lab Technologies Ltd.
- *
- * Support routines for deriving SPI device attachments from the device
- * tree.
- */
-
-#include <linux/module.h>
-#include <linux/of.h>
-#include <linux/device.h>
-#include <linux/spi/spi.h>
-#include <linux/of_irq.h>
-#include <linux/of_spi.h>
-
-/**
- * of_register_spi_devices - Register child devices onto the SPI bus
- * @master:	Pointer to spi_master device
- *
- * Registers an spi_device for each child node of master node which has a 'reg'
- * property.
- */
-void of_register_spi_devices(struct spi_master *master)
-{
-	struct spi_device *spi;
-	struct device_node *nc;
-	const __be32 *prop;
-	int rc;
-	int len;
-
-	if (!master->dev.of_node)
-		return;
-
-	for_each_child_of_node(master->dev.of_node, nc) {
-		/* Alloc an spi_device */
-		spi = spi_alloc_device(master);
-		if (!spi) {
-			dev_err(&master->dev, "spi_device alloc error for %s\n",
-				nc->full_name);
-			spi_dev_put(spi);
-			continue;
-		}
-
-		/* Select device driver */
-		if (of_modalias_node(nc, spi->modalias,
-				     sizeof(spi->modalias)) < 0) {
-			dev_err(&master->dev, "cannot find modalias for %s\n",
-				nc->full_name);
-			spi_dev_put(spi);
-			continue;
-		}
-
-		/* Device address */
-		prop = of_get_property(nc, "reg", &len);
-		if (!prop || len < sizeof(*prop)) {
-			dev_err(&master->dev, "%s has no 'reg' property\n",
-				nc->full_name);
-			spi_dev_put(spi);
-			continue;
-		}
-		spi->chip_select = be32_to_cpup(prop);
-
-		/* Mode (clock phase/polarity/etc.) */
-		if (of_find_property(nc, "spi-cpha", NULL))
-			spi->mode |= SPI_CPHA;
-		if (of_find_property(nc, "spi-cpol", NULL))
-			spi->mode |= SPI_CPOL;
-		if (of_find_property(nc, "spi-cs-high", NULL))
-			spi->mode |= SPI_CS_HIGH;
-
-		/* Device speed */
-		prop = of_get_property(nc, "spi-max-frequency", &len);
-		if (!prop || len < sizeof(*prop)) {
-			dev_err(&master->dev, "%s has no 'spi-max-frequency' property\n",
-				nc->full_name);
-			spi_dev_put(spi);
-			continue;
-		}
-		spi->max_speed_hz = be32_to_cpup(prop);
-
-		/* IRQ */
-		spi->irq = irq_of_parse_and_map(nc, 0);
-
-		/* Store a pointer to the node in the device structure */
-		of_node_get(nc);
-		spi->dev.of_node = nc;
-
-		/* Register the new device */
-		request_module(spi->modalias);
-		rc = spi_add_device(spi);
-		if (rc) {
-			dev_err(&master->dev, "spi_device register error %s\n",
-				nc->full_name);
-			spi_dev_put(spi);
-		}
-
-	}
-}
-EXPORT_SYMBOL(of_register_spi_devices);
diff --git a/drivers/parisc/sba_iommu.c b/drivers/parisc/sba_iommu.c
index 8644d5372e7..42cfcd9eb9a 100644
--- a/drivers/parisc/sba_iommu.c
+++ b/drivers/parisc/sba_iommu.c
@@ -44,6 +44,7 @@
 #include <asm/ropes.h>
 #include <asm/mckinley.h>	/* for proc_mckinley_root */
 #include <asm/runway.h>		/* for proc_runway_root */
+#include <asm/page.h>		/* for PAGE0 */
 #include <asm/pdc.h>		/* for PDC_MODEL_* */
 #include <asm/pdcpat.h>		/* for is_pdc_pat() */
 #include <asm/parisc-device.h>
diff --git a/drivers/parisc/superio.c b/drivers/parisc/superio.c
index e3b76d409de..5003458980d 100644
--- a/drivers/parisc/superio.c
+++ b/drivers/parisc/superio.c
@@ -348,7 +348,7 @@ int superio_fixup_irq(struct pci_dev *pcidev)
 		BUG();
 		return -1;
 	}
-	printk("superio_fixup_irq(%s) ven 0x%x dev 0x%x from %p\n",
+	printk("superio_fixup_irq(%s) ven 0x%x dev 0x%x from %pf\n",
 		pci_name(pcidev),
 		pcidev->vendor, pcidev->device,
 		__builtin_return_address(0));
diff --git a/drivers/parport/parport_amiga.c b/drivers/parport/parport_amiga.c
index 8bef6d60f88..ee78e0ee6e0 100644
--- a/drivers/parport/parport_amiga.c
+++ b/drivers/parport/parport_amiga.c
@@ -48,23 +48,6 @@ static unsigned char amiga_read_data(struct parport *p)
 	return ciaa.prb;
 }
 
-#if 0
-static unsigned char control_pc_to_amiga(unsigned char control)
-{
-	unsigned char ret = 0;
-
-	if (control & PARPORT_CONTROL_SELECT) /* XXX: What is SELECP? */
-		;
-	if (control & PARPORT_CONTROL_INIT) /* INITP */
-		/* reset connected to cpu reset pin */;
-	if (control & PARPORT_CONTROL_AUTOFD) /* AUTOLF */
-		/* Not connected */;
-	if (control & PARPORT_CONTROL_STROBE) /* Strobe */
-		/* Handled only directly by hardware */;
-	return ret;
-}
-#endif
-
 static unsigned char control_amiga_to_pc(unsigned char control)
 {
 	return PARPORT_CONTROL_SELECT |
@@ -95,25 +78,6 @@ static unsigned char amiga_frob_control( struct parport *p, unsigned char mask,
 	return old;
 }
 
-#if 0 /* currently unused */
-static unsigned char status_pc_to_amiga(unsigned char status)
-{
-	unsigned char ret = 1;
-
-	if (status & PARPORT_STATUS_BUSY) /* Busy */
-		ret &= ~1;
-	if (status & PARPORT_STATUS_ACK) /* Ack */
-		/* handled in hardware */;
-	if (status & PARPORT_STATUS_PAPEROUT) /* PaperOut */
-		ret |= 2;
-	if (status & PARPORT_STATUS_SELECT) /* select */
-		ret |= 4;
-	if (status & PARPORT_STATUS_ERROR) /* error */
-		/* not connected */;
-	return ret;
-}
-#endif
-
 static unsigned char status_amiga_to_pc(unsigned char status)
 {
 	unsigned char ret = PARPORT_STATUS_BUSY | PARPORT_STATUS_ACK | PARPORT_STATUS_ERROR;
diff --git a/drivers/parport/parport_atari.c b/drivers/parport/parport_atari.c
index 0b28fccec03..7ad59ac68cf 100644
--- a/drivers/parport/parport_atari.c
+++ b/drivers/parport/parport_atari.c
@@ -130,15 +130,6 @@ parport_atari_data_forward(struct parport *p)
 static void
 parport_atari_data_reverse(struct parport *p)
 {
-#if 0 /* too dangerous, can kill sound chip */
-	unsigned long flags;
-
-	local_irq_save(flags);
-	/* Soundchip port B as input. */
-	sound_ym.rd_data_reg_sel = 7;
-	sound_ym.wd_data = sound_ym.rd_data_reg_sel & ~0x40;
-	local_irq_restore(flags);
-#endif
 }
 
 static struct parport_operations parport_atari_ops = {
diff --git a/drivers/parport/parport_mfc3.c b/drivers/parport/parport_mfc3.c
index 1c0c642b3e2..7578d79b368 100644
--- a/drivers/parport/parport_mfc3.c
+++ b/drivers/parport/parport_mfc3.c
@@ -147,25 +147,6 @@ DPRINTK(KERN_DEBUG "frob_control mask %02x, value %02x\n",mask,val);
 	return old;
 }
 
-#if 0 /* currently unused */
-static unsigned char status_pc_to_mfc3(unsigned char status)
-{
-	unsigned char ret = 1;
-
-	if (status & PARPORT_STATUS_BUSY) /* Busy */
-		ret &= ~1;
-	if (status & PARPORT_STATUS_ACK) /* Ack */
-		ret |= 8;
-	if (status & PARPORT_STATUS_PAPEROUT) /* PaperOut */
-		ret |= 2;
-	if (status & PARPORT_STATUS_SELECT) /* select */
-		ret |= 4;
-	if (status & PARPORT_STATUS_ERROR) /* error */
-		ret |= 16;
-	return ret;
-}
-#endif
-
 static unsigned char status_mfc3_to_pc(unsigned char status)
 {
 	unsigned char ret = PARPORT_STATUS_BUSY;
@@ -184,14 +165,6 @@ static unsigned char status_mfc3_to_pc(unsigned char status)
 	return ret;
 }
 
-#if 0 /* currently unused */
-static void mfc3_write_status( struct parport *p, unsigned char status)
-{
-DPRINTK(KERN_DEBUG "write_status %02x\n",status);
-	pia(p)->ppra = (pia(p)->ppra & 0xe0) | status_pc_to_mfc3(status);
-}
-#endif
-
 static unsigned char mfc3_read_status(struct parport *p)
 {
 	unsigned char status;
@@ -201,14 +174,6 @@ DPRINTK(KERN_DEBUG "read_status %02x\n", status);
 	return status;
 }
 
-#if 0 /* currently unused */
-static void mfc3_change_mode( struct parport *p, int m)
-{
-	/* XXX: This port only has one mode, and I am
-	not sure about the corresponding PC-style mode*/
-}
-#endif
-
 static int use_cnt = 0;
 
 static irqreturn_t mfc3_interrupt(int irq, void *dev_id)
diff --git a/drivers/parport/parport_pc.c b/drivers/parport/parport_pc.c
index 0cb64f50cec..5abffe58a9d 100644
--- a/drivers/parport/parport_pc.c
+++ b/drivers/parport/parport_pc.c
@@ -197,54 +197,6 @@ static int change_mode(struct parport *p, int m)
 	ECR_WRITE(p, oecr);
 	return 0;
 }
-
-#ifdef CONFIG_PARPORT_1284
-/* Find FIFO lossage; FIFO is reset */
-#if 0
-static int get_fifo_residue(struct parport *p)
-{
-	int residue;
-	int cnfga;
-	const struct parport_pc_private *priv = p->physport->private_data;
-
-	/* Adjust for the contents of the FIFO. */
-	for (residue = priv->fifo_depth; ; residue--) {
-		if (inb(ECONTROL(p)) & 0x2)
-				/* Full up. */
-			break;
-
-		outb(0, FIFO(p));
-	}
-
-	printk(KERN_DEBUG "%s: %d PWords were left in FIFO\n", p->name,
-		residue);
-
-	/* Reset the FIFO. */
-	frob_set_mode(p, ECR_PS2);
-
-	/* Now change to config mode and clean up. FIXME */
-	frob_set_mode(p, ECR_CNF);
-	cnfga = inb(CONFIGA(p));
-	printk(KERN_DEBUG "%s: cnfgA contains 0x%02x\n", p->name, cnfga);
-
-	if (!(cnfga & (1<<2))) {
-		printk(KERN_DEBUG "%s: Accounting for extra byte\n", p->name);
-		residue++;
-	}
-
-	/* Don't care about partial PWords until support is added for
-	 * PWord != 1 byte. */
-
-	/* Back to PS2 mode. */
-	frob_set_mode(p, ECR_PS2);
-
-	DPRINTK(KERN_DEBUG
-	     "*** get_fifo_residue: done residue collecting (ecr = 0x%2.2x)\n",
-							inb(ECONTROL(p)));
-	return residue;
-}
-#endif  /*  0 */
-#endif /* IEEE 1284 support */
 #endif /* FIFO support */
 
 /*
@@ -940,234 +892,6 @@ static size_t parport_pc_ecp_write_block_pio(struct parport *port,
 
 	return written;
 }
-
-#if 0
-static size_t parport_pc_ecp_read_block_pio(struct parport *port,
-					     void *buf, size_t length,
-					     int flags)
-{
-	size_t left = length;
-	size_t fifofull;
-	int r;
-	const int fifo = FIFO(port);
-	const struct parport_pc_private *priv = port->physport->private_data;
-	const int fifo_depth = priv->fifo_depth;
-	char *bufp = buf;
-
-	port = port->physport;
-	DPRINTK(KERN_DEBUG "parport_pc: parport_pc_ecp_read_block_pio\n");
-	dump_parport_state("enter fcn", port);
-
-	/* Special case: a timeout of zero means we cannot call schedule().
-	 * Also if O_NONBLOCK is set then use the default implementation. */
-	if (port->cad->timeout <= PARPORT_INACTIVITY_O_NONBLOCK)
-		return parport_ieee1284_ecp_read_data(port, buf,
-						       length, flags);
-
-	if (port->ieee1284.mode == IEEE1284_MODE_ECPRLE) {
-		/* If the peripheral is allowed to send RLE compressed
-		 * data, it is possible for a byte to expand to 128
-		 * bytes in the FIFO. */
-		fifofull = 128;
-	} else {
-		fifofull = fifo_depth;
-	}
-
-	/* If the caller wants less than a full FIFO's worth of data,
-	 * go through software emulation.  Otherwise we may have to throw
-	 * away data. */
-	if (length < fifofull)
-		return parport_ieee1284_ecp_read_data(port, buf,
-						       length, flags);
-
-	if (port->ieee1284.phase != IEEE1284_PH_REV_IDLE) {
-		/* change to reverse-idle phase (must be in forward-idle) */
-
-		/* Event 38: Set nAutoFd low (also make sure nStrobe is high) */
-		parport_frob_control(port,
-				      PARPORT_CONTROL_AUTOFD
-				      | PARPORT_CONTROL_STROBE,
-				      PARPORT_CONTROL_AUTOFD);
-		parport_pc_data_reverse(port); /* Must be in PS2 mode */
-		udelay(5);
-		/* Event 39: Set nInit low to initiate bus reversal */
-		parport_frob_control(port,
-				      PARPORT_CONTROL_INIT,
-				      0);
-		/* Event 40: Wait for  nAckReverse (PError) to go low */
-		r = parport_wait_peripheral(port, PARPORT_STATUS_PAPEROUT, 0);
-		if (r) {
-			printk(KERN_DEBUG "%s: PE timeout Event 40 (%d) "
-				"in ecp_read_block_pio\n", port->name, r);
-			return 0;
-		}
-	}
-
-	/* Set up ECP FIFO mode.*/
-/*	parport_pc_frob_control(port,
-				 PARPORT_CONTROL_STROBE |
-				 PARPORT_CONTROL_AUTOFD,
-				 PARPORT_CONTROL_AUTOFD); */
-	r = change_mode(port, ECR_ECP); /* ECP FIFO */
-	if (r)
-		printk(KERN_DEBUG "%s: Warning change_mode ECR_ECP failed\n",
-								port->name);
-
-	port->ieee1284.phase = IEEE1284_PH_REV_DATA;
-
-	/* the first byte must be collected manually */
-	dump_parport_state("pre 43", port);
-	/* Event 43: Wait for nAck to go low */
-	r = parport_wait_peripheral(port, PARPORT_STATUS_ACK, 0);
-	if (r) {
-		/* timed out while reading -- no data */
-		printk(KERN_DEBUG "PIO read timed out (initial byte)\n");
-		goto out_no_data;
-	}
-	/* read byte */
-	*bufp++ = inb(DATA(port));
-	left--;
-	dump_parport_state("43-44", port);
-	/* Event 44: nAutoFd (HostAck) goes high to acknowledge */
-	parport_pc_frob_control(port,
-				 PARPORT_CONTROL_AUTOFD,
-				 0);
-	dump_parport_state("pre 45", port);
-	/* Event 45: Wait for nAck to go high */
-	/* r = parport_wait_peripheral(port, PARPORT_STATUS_ACK,
-						PARPORT_STATUS_ACK); */
-	dump_parport_state("post 45", port);
-	r = 0;
-	if (r) {
-		/* timed out while waiting for peripheral to respond to ack */
-		printk(KERN_DEBUG "ECP PIO read timed out (waiting for nAck)\n");
-
-		/* keep hold of the byte we've got already */
-		goto out_no_data;
-	}
-	/* Event 46: nAutoFd (HostAck) goes low to accept more data */
-	parport_pc_frob_control(port,
-				 PARPORT_CONTROL_AUTOFD,
-				 PARPORT_CONTROL_AUTOFD);
-
-
-	dump_parport_state("rev idle", port);
-	/* Do the transfer. */
-	while (left > fifofull) {
-		int ret;
-		unsigned long expire = jiffies + port->cad->timeout;
-		unsigned char ecrval = inb(ECONTROL(port));
-
-		if (need_resched() && time_before(jiffies, expire))
-			/* Can't yield the port. */
-			schedule();
-
-		/* At this point, the FIFO may already be full. In
-		 * that case ECP is already holding back the
-		 * peripheral (assuming proper design) with a delayed
-		 * handshake.  Work fast to avoid a peripheral
-		 * timeout.  */
-
-		if (ecrval & 0x01) {
-			/* FIFO is empty. Wait for interrupt. */
-			dump_parport_state("FIFO empty", port);
-
-			/* Anyone else waiting for the port? */
-			if (port->waithead) {
-				printk(KERN_DEBUG "Somebody wants the port\n");
-				break;
-			}
-
-			/* Clear serviceIntr */
-			ECR_WRITE(port, ecrval & ~(1<<2));
-false_alarm:
-			dump_parport_state("waiting", port);
-			ret = parport_wait_event(port, HZ);
-			DPRINTK(KERN_DEBUG "parport_wait_event returned %d\n",
-									ret);
-			if (ret < 0)
-				break;
-			ret = 0;
-			if (!time_before(jiffies, expire)) {
-				/* Timed out. */
-				dump_parport_state("timeout", port);
-				printk(KERN_DEBUG "PIO read timed out\n");
-				break;
-			}
-			ecrval = inb(ECONTROL(port));
-			if (!(ecrval & (1<<2))) {
-				if (need_resched() &&
-				    time_before(jiffies, expire)) {
-					schedule();
-				}
-				goto false_alarm;
-			}
-
-			/* Depending on how the FIFO threshold was
-			 * set, how long interrupt service took, and
-			 * how fast the peripheral is, we might be
-			 * lucky and have a just filled FIFO. */
-			continue;
-		}
-
-		if (ecrval & 0x02) {
-			/* FIFO is full. */
-			dump_parport_state("FIFO full", port);
-			insb(fifo, bufp, fifo_depth);
-			bufp += fifo_depth;
-			left -= fifo_depth;
-			continue;
-		}
-
-		DPRINTK(KERN_DEBUG
-		  "*** ecp_read_block_pio: reading one byte from the FIFO\n");
-
-		/* FIFO not filled.  We will cycle this loop for a while
-		 * and either the peripheral will fill it faster,
-		 * tripping a fast empty with insb, or we empty it. */
-		*bufp++ = inb(fifo);
-		left--;
-	}
-
-	/* scoop up anything left in the FIFO */
-	while (left && !(inb(ECONTROL(port) & 0x01))) {
-		*bufp++ = inb(fifo);
-		left--;
-	}
-
-	port->ieee1284.phase = IEEE1284_PH_REV_IDLE;
-	dump_parport_state("rev idle2", port);
-
-out_no_data:
-
-	/* Go to forward idle mode to shut the peripheral up (event 47). */
-	parport_frob_control(port, PARPORT_CONTROL_INIT, PARPORT_CONTROL_INIT);
-
-	/* event 49: PError goes high */
-	r = parport_wait_peripheral(port,
-				     PARPORT_STATUS_PAPEROUT,
-				     PARPORT_STATUS_PAPEROUT);
-	if (r) {
-		printk(KERN_DEBUG
-			"%s: PE timeout FWDIDLE (%d) in ecp_read_block_pio\n",
-			port->name, r);
-	}
-
-	port->ieee1284.phase = IEEE1284_PH_FWD_IDLE;
-
-	/* Finish up. */
-	{
-		int lost = get_fifo_residue(port);
-		if (lost)
-			/* Shouldn't happen with compliant peripherals. */
-			printk(KERN_DEBUG "%s: DATA LOSS (%d bytes)!\n",
-				port->name, lost);
-	}
-
-	dump_parport_state("fwd idle", port);
-	return length - left;
-}
-#endif  /*  0  */
 #endif /* IEEE 1284 support */
 #endif /* Allowed to use FIFO/DMA */
 
@@ -2351,7 +2075,7 @@ struct parport *parport_pc_probe_port(unsigned long int base,
 
 	printk(KERN_INFO "%s: PC-style at 0x%lx", p->name, p->base);
 	if (p->base_hi && priv->ecr)
-		printk(" (0x%lx)", p->base_hi);
+		printk(KERN_CONT " (0x%lx)", p->base_hi);
 	if (p->irq == PARPORT_IRQ_AUTO) {
 		p->irq = PARPORT_IRQ_NONE;
 		parport_irq_probe(p);
@@ -2362,7 +2086,7 @@ struct parport *parport_pc_probe_port(unsigned long int base,
 		p->irq = PARPORT_IRQ_NONE;
 	}
 	if (p->irq != PARPORT_IRQ_NONE) {
-		printk(", irq %d", p->irq);
+		printk(KERN_CONT ", irq %d", p->irq);
 		priv->ctr_writable |= 0x10;
 
 		if (p->dma == PARPORT_DMA_AUTO) {
@@ -2386,21 +2110,21 @@ struct parport *parport_pc_probe_port(unsigned long int base,
 		/* p->ops->ecp_read_data = parport_pc_ecp_read_block_pio; */
 #endif /* IEEE 1284 support */
 		if (p->dma != PARPORT_DMA_NONE) {
-			printk(", dma %d", p->dma);
+			printk(KERN_CONT ", dma %d", p->dma);
 			p->modes |= PARPORT_MODE_DMA;
 		} else
-			printk(", using FIFO");
+			printk(KERN_CONT ", using FIFO");
 	} else
 		/* We can't use the DMA channel after all. */
 		p->dma = PARPORT_DMA_NONE;
 #endif /* Allowed to use FIFO/DMA */
 
-	printk(" [");
+	printk(KERN_CONT " [");
 
 #define printmode(x) \
 	{\
 		if (p->modes & PARPORT_MODE_##x) {\
-			printk("%s%s", f ? "," : "", #x);\
+			printk(KERN_CONT "%s%s", f ? "," : "", #x);\
 			f++;\
 		} \
 	}
@@ -2416,9 +2140,9 @@ struct parport *parport_pc_probe_port(unsigned long int base,
 	}
 #undef printmode
 #ifndef CONFIG_PARPORT_1284
-	printk("(,...)");
+	printk(KERN_CONT "(,...)");
 #endif /* CONFIG_PARPORT_1284 */
-	printk("]\n");
+	printk(KERN_CONT "]\n");
 	if (probedirq != PARPORT_IRQ_NONE)
 		printk(KERN_INFO "%s: irq %d detected\n", p->name, probedirq);
 
diff --git a/drivers/parport/parport_sunbpp.c b/drivers/parport/parport_sunbpp.c
index 9390a534a2b..983a2d2df65 100644
--- a/drivers/parport/parport_sunbpp.c
+++ b/drivers/parport/parport_sunbpp.c
@@ -82,27 +82,6 @@ static unsigned char parport_sunbpp_read_data(struct parport *p)
 	return sbus_readb(&regs->p_dr);
 }
 
-#if 0
-static void control_pc_to_sunbpp(struct parport *p, unsigned char status)
-{
-	struct bpp_regs __iomem *regs = (struct bpp_regs __iomem *)p->base;
-	unsigned char value_tcr = sbus_readb(&regs->p_tcr);
-	unsigned char value_or = sbus_readb(&regs->p_or);
-
-	if (status & PARPORT_CONTROL_STROBE) 
-		value_tcr |= P_TCR_DS;
-	if (status & PARPORT_CONTROL_AUTOFD) 
-		value_or |= P_OR_AFXN;
-	if (status & PARPORT_CONTROL_INIT) 
-		value_or |= P_OR_INIT;
-	if (status & PARPORT_CONTROL_SELECT) 
-		value_or |= P_OR_SLCT_IN;
-
-	sbus_writeb(value_or, &regs->p_or);
-	sbus_writeb(value_tcr, &regs->p_tcr);
-}
-#endif
-
 static unsigned char status_sunbpp_to_pc(struct parport *p)
 {
 	struct bpp_regs __iomem *regs = (struct bpp_regs __iomem *)p->base;
diff --git a/drivers/pci/Makefile b/drivers/pci/Makefile
index 165274c064b..01c001f3b76 100644
--- a/drivers/pci/Makefile
+++ b/drivers/pci/Makefile
@@ -2,7 +2,7 @@
 # Makefile for the PCI bus specific drivers.
 #
 
-obj-y		+= access.o bus.o probe.o remove.o pci.o \
+obj-y		+= access.o bus.o probe.o host-bridge.o remove.o pci.o \
 			pci-driver.o search.o pci-sysfs.o rom.o setup-res.o \
 			irq.o vpd.o
 obj-$(CONFIG_PROC_FS) += proc.o
diff --git a/drivers/pci/host-bridge.c b/drivers/pci/host-bridge.c
new file mode 100644
index 00000000000..a68dc613a5b
--- /dev/null
+++ b/drivers/pci/host-bridge.c
@@ -0,0 +1,96 @@
+/*
+ * host bridge related code
+ */
+
+#include <linux/kernel.h>
+#include <linux/init.h>
+#include <linux/pci.h>
+#include <linux/module.h>
+
+#include "pci.h"
+
+static struct pci_bus *find_pci_root_bus(struct pci_dev *dev)
+{
+	struct pci_bus *bus;
+
+	bus = dev->bus;
+	while (bus->parent)
+		bus = bus->parent;
+
+	return bus;
+}
+
+static struct pci_host_bridge *find_pci_host_bridge(struct pci_dev *dev)
+{
+	struct pci_bus *bus = find_pci_root_bus(dev);
+
+	return to_pci_host_bridge(bus->bridge);
+}
+
+void pci_set_host_bridge_release(struct pci_host_bridge *bridge,
+				 void (*release_fn)(struct pci_host_bridge *),
+				 void *release_data)
+{
+	bridge->release_fn = release_fn;
+	bridge->release_data = release_data;
+}
+
+static bool resource_contains(struct resource *res1, struct resource *res2)
+{
+	return res1->start <= res2->start && res1->end >= res2->end;
+}
+
+void pcibios_resource_to_bus(struct pci_dev *dev, struct pci_bus_region *region,
+			     struct resource *res)
+{
+	struct pci_host_bridge *bridge = find_pci_host_bridge(dev);
+	struct pci_host_bridge_window *window;
+	resource_size_t offset = 0;
+
+	list_for_each_entry(window, &bridge->windows, list) {
+		if (resource_type(res) != resource_type(window->res))
+			continue;
+
+		if (resource_contains(window->res, res)) {
+			offset = window->offset;
+			break;
+		}
+	}
+
+	region->start = res->start - offset;
+	region->end = res->end - offset;
+}
+EXPORT_SYMBOL(pcibios_resource_to_bus);
+
+static bool region_contains(struct pci_bus_region *region1,
+			    struct pci_bus_region *region2)
+{
+	return region1->start <= region2->start && region1->end >= region2->end;
+}
+
+void pcibios_bus_to_resource(struct pci_dev *dev, struct resource *res,
+			     struct pci_bus_region *region)
+{
+	struct pci_host_bridge *bridge = find_pci_host_bridge(dev);
+	struct pci_host_bridge_window *window;
+	resource_size_t offset = 0;
+
+	list_for_each_entry(window, &bridge->windows, list) {
+		struct pci_bus_region bus_region;
+
+		if (resource_type(res) != resource_type(window->res))
+			continue;
+
+		bus_region.start = window->res->start - window->offset;
+		bus_region.end = window->res->end - window->offset;
+
+		if (region_contains(&bus_region, region)) {
+			offset = window->offset;
+			break;
+		}
+	}
+
+	res->start = region->start + offset;
+	res->end = region->end + offset;
+}
+EXPORT_SYMBOL(pcibios_bus_to_resource);
diff --git a/drivers/pci/pci-acpi.c b/drivers/pci/pci-acpi.c
index 1929c0c63b7..61e2fefeeda 100644
--- a/drivers/pci/pci-acpi.c
+++ b/drivers/pci/pci-acpi.c
@@ -223,7 +223,7 @@ static int acpi_pci_set_power_state(struct pci_dev *dev, pci_power_t state)
 		[PCI_D0] = ACPI_STATE_D0,
 		[PCI_D1] = ACPI_STATE_D1,
 		[PCI_D2] = ACPI_STATE_D2,
-		[PCI_D3hot] = ACPI_STATE_D3_HOT,
+		[PCI_D3hot] = ACPI_STATE_D3,
 		[PCI_D3cold] = ACPI_STATE_D3
 	};
 	int error = -EINVAL;
diff --git a/drivers/pci/pci-driver.c b/drivers/pci/pci-driver.c
index 6b54b23b990..bf0cee629b6 100644
--- a/drivers/pci/pci-driver.c
+++ b/drivers/pci/pci-driver.c
@@ -421,6 +421,12 @@ static void pci_device_shutdown(struct device *dev)
 	pci_msix_shutdown(pci_dev);
 
 	/*
+	 * Turn off Bus Master bit on the device to tell it to not
+	 * continue to do DMA
+	 */
+	pci_disable_device(pci_dev);
+
+	/*
 	 * Devices may be enabled to wake up by runtime PM, but they need not
 	 * be supposed to wake up the system from its "power off" state (e.g.
 	 * ACPI S5).  Therefore disable wakeup for all devices that aren't
diff --git a/drivers/pci/pci-sysfs.c b/drivers/pci/pci-sysfs.c
index a55e248618c..86c63fe45d1 100644
--- a/drivers/pci/pci-sysfs.c
+++ b/drivers/pci/pci-sysfs.c
@@ -27,6 +27,7 @@
 #include <linux/security.h>
 #include <linux/pci-aspm.h>
 #include <linux/slab.h>
+#include <linux/vgaarb.h>
 #include "pci.h"
 
 static int sysfs_initialized;	/* = 0 */
@@ -417,6 +418,10 @@ static ssize_t
 boot_vga_show(struct device *dev, struct device_attribute *attr, char *buf)
 {
 	struct pci_dev *pdev = to_pci_dev(dev);
+	struct pci_dev *vga_dev = vga_default_device();
+
+	if (vga_dev)
+		return sprintf(buf, "%u\n", (pdev == vga_dev));
 
 	return sprintf(buf, "%u\n",
 		!!(pdev->resource[PCI_ROM_RESOURCE].flags &
diff --git a/drivers/pci/pci.c b/drivers/pci/pci.c
index 111569ccab4..447e83472c0 100644
--- a/drivers/pci/pci.c
+++ b/drivers/pci/pci.c
@@ -22,6 +22,7 @@
 #include <linux/interrupt.h>
 #include <linux/device.h>
 #include <linux/pm_runtime.h>
+#include <asm-generic/pci-bridge.h>
 #include <asm/setup.h>
 #include "pci.h"
 
@@ -2369,7 +2370,7 @@ void pci_enable_acs(struct pci_dev *dev)
  * number is always 0 (see the Implementation Note in section 2.2.8.1 of
  * the PCI Express Base Specification, Revision 2.1)
  */
-u8 pci_swizzle_interrupt_pin(struct pci_dev *dev, u8 pin)
+u8 pci_swizzle_interrupt_pin(const struct pci_dev *dev, u8 pin)
 {
 	int slot;
 
@@ -3164,18 +3165,12 @@ static int pci_parent_bus_reset(struct pci_dev *dev, int probe)
 	return 0;
 }
 
-static int pci_dev_reset(struct pci_dev *dev, int probe)
+static int __pci_dev_reset(struct pci_dev *dev, int probe)
 {
 	int rc;
 
 	might_sleep();
 
-	if (!probe) {
-		pci_cfg_access_lock(dev);
-		/* block PM suspend, driver probe, etc. */
-		device_lock(&dev->dev);
-	}
-
 	rc = pci_dev_specific_reset(dev, probe);
 	if (rc != -ENOTTY)
 		goto done;
@@ -3194,14 +3189,27 @@ static int pci_dev_reset(struct pci_dev *dev, int probe)
 
 	rc = pci_parent_bus_reset(dev, probe);
 done:
+	return rc;
+}
+
+static int pci_dev_reset(struct pci_dev *dev, int probe)
+{
+	int rc;
+
+	if (!probe) {
+		pci_cfg_access_lock(dev);
+		/* block PM suspend, driver probe, etc. */
+		device_lock(&dev->dev);
+	}
+
+	rc = __pci_dev_reset(dev, probe);
+
 	if (!probe) {
 		device_unlock(&dev->dev);
 		pci_cfg_access_unlock(dev);
 	}
-
 	return rc;
 }
-
 /**
  * __pci_reset_function - reset a PCI device function
  * @dev: PCI device to reset
@@ -3246,7 +3254,7 @@ EXPORT_SYMBOL_GPL(__pci_reset_function);
  */
 int __pci_reset_function_locked(struct pci_dev *dev)
 {
-	return pci_dev_reset(dev, 1);
+	return __pci_dev_reset(dev, 0);
 }
 EXPORT_SYMBOL_GPL(__pci_reset_function_locked);
 
@@ -3893,6 +3901,8 @@ static int __init pci_setup(char *str)
 				pcie_bus_config = PCIE_BUS_PERFORMANCE;
 			} else if (!strncmp(str, "pcie_bus_peer2peer", 18)) {
 				pcie_bus_config = PCIE_BUS_PEER2PEER;
+			} else if (!strncmp(str, "pcie_scan_all", 13)) {
+				pci_add_flags(PCI_SCAN_ALL_PCIE_DEVS);
 			} else {
 				printk(KERN_ERR "PCI: Unknown option `%s'\n",
 						str);
diff --git a/drivers/pci/pcie/portdrv_core.c b/drivers/pci/pcie/portdrv_core.c
index 2f589a54f9b..75915b30ad1 100644
--- a/drivers/pci/pcie/portdrv_core.c
+++ b/drivers/pci/pcie/portdrv_core.c
@@ -249,7 +249,7 @@ static int get_port_device_capability(struct pci_dev *dev)
 	int services = 0, pos;
 	u16 reg16;
 	u32 reg32;
-	int cap_mask;
+	int cap_mask = 0;
 	int err;
 
 	if (pcie_ports_disabled)
diff --git a/drivers/pci/probe.c b/drivers/pci/probe.c
index 5e1ca3c58a7..658ac977cb5 100644
--- a/drivers/pci/probe.c
+++ b/drivers/pci/probe.c
@@ -10,18 +10,16 @@
 #include <linux/module.h>
 #include <linux/cpumask.h>
 #include <linux/pci-aspm.h>
+#include <asm-generic/pci-bridge.h>
 #include "pci.h"
 
 #define CARDBUS_LATENCY_TIMER	176	/* secondary latency timer */
 #define CARDBUS_RESERVE_BUSNR	3
 
-static LIST_HEAD(pci_host_bridges);
-
 /* Ugh.  Need to stop exporting this to modules. */
 LIST_HEAD(pci_root_buses);
 EXPORT_SYMBOL(pci_root_buses);
 
-
 static int find_anything(struct device *dev, void *data)
 {
 	return 1;
@@ -44,82 +42,6 @@ int no_pci_devices(void)
 }
 EXPORT_SYMBOL(no_pci_devices);
 
-static struct pci_host_bridge *pci_host_bridge(struct pci_dev *dev)
-{
-	struct pci_bus *bus;
-	struct pci_host_bridge *bridge;
-
-	bus = dev->bus;
-	while (bus->parent)
-		bus = bus->parent;
-
-	list_for_each_entry(bridge, &pci_host_bridges, list) {
-		if (bridge->bus == bus)
-			return bridge;
-	}
-
-	return NULL;
-}
-
-static bool resource_contains(struct resource *res1, struct resource *res2)
-{
-	return res1->start <= res2->start && res1->end >= res2->end;
-}
-
-void pcibios_resource_to_bus(struct pci_dev *dev, struct pci_bus_region *region,
-			     struct resource *res)
-{
-	struct pci_host_bridge *bridge = pci_host_bridge(dev);
-	struct pci_host_bridge_window *window;
-	resource_size_t offset = 0;
-
-	list_for_each_entry(window, &bridge->windows, list) {
-		if (resource_type(res) != resource_type(window->res))
-			continue;
-
-		if (resource_contains(window->res, res)) {
-			offset = window->offset;
-			break;
-		}
-	}
-
-	region->start = res->start - offset;
-	region->end = res->end - offset;
-}
-EXPORT_SYMBOL(pcibios_resource_to_bus);
-
-static bool region_contains(struct pci_bus_region *region1,
-			    struct pci_bus_region *region2)
-{
-	return region1->start <= region2->start && region1->end >= region2->end;
-}
-
-void pcibios_bus_to_resource(struct pci_dev *dev, struct resource *res,
-			     struct pci_bus_region *region)
-{
-	struct pci_host_bridge *bridge = pci_host_bridge(dev);
-	struct pci_host_bridge_window *window;
-	struct pci_bus_region bus_region;
-	resource_size_t offset = 0;
-
-	list_for_each_entry(window, &bridge->windows, list) {
-		if (resource_type(res) != resource_type(window->res))
-			continue;
-
-		bus_region.start = window->res->start - window->offset;
-		bus_region.end = window->res->end - window->offset;
-
-		if (region_contains(&bus_region, region)) {
-			offset = window->offset;
-			break;
-		}
-	}
-
-	res->start = region->start + offset;
-	res->end = region->end + offset;
-}
-EXPORT_SYMBOL(pcibios_bus_to_resource);
-
 /*
  * PCI Bus Class
  */
@@ -501,6 +423,19 @@ static struct pci_bus * pci_alloc_bus(void)
 	return b;
 }
 
+static struct pci_host_bridge *pci_alloc_host_bridge(struct pci_bus *b)
+{
+	struct pci_host_bridge *bridge;
+
+	bridge = kzalloc(sizeof(*bridge), GFP_KERNEL);
+	if (bridge) {
+		INIT_LIST_HEAD(&bridge->windows);
+		bridge->bus = b;
+	}
+
+	return bridge;
+}
+
 static unsigned char pcix_bus_speed[] = {
 	PCI_SPEED_UNKNOWN,		/* 0 */
 	PCI_SPEED_66MHz_PCIX,		/* 1 */
@@ -1201,7 +1136,14 @@ int pci_cfg_space_size(struct pci_dev *dev)
 
 static void pci_release_bus_bridge_dev(struct device *dev)
 {
-	kfree(dev);
+	struct pci_host_bridge *bridge = to_pci_host_bridge(dev);
+
+	if (bridge->release_fn)
+		bridge->release_fn(bridge);
+
+	pci_free_resource_list(&bridge->windows);
+
+	kfree(bridge);
 }
 
 struct pci_dev *alloc_pci_dev(void)
@@ -1395,10 +1337,13 @@ static unsigned no_next_fn(struct pci_dev *dev, unsigned fn)
 static int only_one_child(struct pci_bus *bus)
 {
 	struct pci_dev *parent = bus->self;
+
 	if (!parent || !pci_is_pcie(parent))
 		return 0;
-	if (parent->pcie_type == PCI_EXP_TYPE_ROOT_PORT ||
-	    parent->pcie_type == PCI_EXP_TYPE_DOWNSTREAM)
+	if (parent->pcie_type == PCI_EXP_TYPE_ROOT_PORT)
+		return 1;
+	if (parent->pcie_type == PCI_EXP_TYPE_DOWNSTREAM &&
+	    !pci_has_flag(PCI_SCAN_ALL_PCIE_DEVS))
 		return 1;
 	return 0;
 }
@@ -1650,28 +1595,19 @@ struct pci_bus *pci_create_root_bus(struct device *parent, int bus,
 	int error;
 	struct pci_host_bridge *bridge;
 	struct pci_bus *b, *b2;
-	struct device *dev;
 	struct pci_host_bridge_window *window, *n;
 	struct resource *res;
 	resource_size_t offset;
 	char bus_addr[64];
 	char *fmt;
 
-	bridge = kzalloc(sizeof(*bridge), GFP_KERNEL);
-	if (!bridge)
-		return NULL;
 
 	b = pci_alloc_bus();
 	if (!b)
-		goto err_bus;
-
-	dev = kzalloc(sizeof(*dev), GFP_KERNEL);
-	if (!dev)
-		goto err_dev;
+		return NULL;
 
 	b->sysdata = sysdata;
 	b->ops = ops;
-
 	b2 = pci_find_bus(pci_domain_nr(b), bus);
 	if (b2) {
 		/* If we already got to this bus through a different bridge, ignore it */
@@ -1679,13 +1615,17 @@ struct pci_bus *pci_create_root_bus(struct device *parent, int bus,
 		goto err_out;
 	}
 
-	dev->parent = parent;
-	dev->release = pci_release_bus_bridge_dev;
-	dev_set_name(dev, "pci%04x:%02x", pci_domain_nr(b), bus);
-	error = device_register(dev);
+	bridge = pci_alloc_host_bridge(b);
+	if (!bridge)
+		goto err_out;
+
+	bridge->dev.parent = parent;
+	bridge->dev.release = pci_release_bus_bridge_dev;
+	dev_set_name(&bridge->dev, "pci%04x:%02x", pci_domain_nr(b), bus);
+	error = device_register(&bridge->dev);
 	if (error)
-		goto dev_reg_err;
-	b->bridge = get_device(dev);
+		goto bridge_dev_reg_err;
+	b->bridge = get_device(&bridge->dev);
 	device_enable_async_suspend(b->bridge);
 	pci_set_bus_of_node(b);
 
@@ -1704,9 +1644,6 @@ struct pci_bus *pci_create_root_bus(struct device *parent, int bus,
 
 	b->number = b->secondary = bus;
 
-	bridge->bus = b;
-	INIT_LIST_HEAD(&bridge->windows);
-
 	if (parent)
 		dev_info(parent, "PCI host bridge to bus %s\n", dev_name(&b->dev));
 	else
@@ -1732,25 +1669,18 @@ struct pci_bus *pci_create_root_bus(struct device *parent, int bus,
 	}
 
 	down_write(&pci_bus_sem);
-	list_add_tail(&bridge->list, &pci_host_bridges);
 	list_add_tail(&b->node, &pci_root_buses);
 	up_write(&pci_bus_sem);
 
 	return b;
 
 class_dev_reg_err:
-	device_unregister(dev);
-dev_reg_err:
-	down_write(&pci_bus_sem);
-	list_del(&bridge->list);
-	list_del(&b->node);
-	up_write(&pci_bus_sem);
+	put_device(&bridge->dev);
+	device_unregister(&bridge->dev);
+bridge_dev_reg_err:
+	kfree(bridge);
 err_out:
-	kfree(dev);
-err_dev:
 	kfree(b);
-err_bus:
-	kfree(bridge);
 	return NULL;
 }
 
diff --git a/drivers/pci/quirks.c b/drivers/pci/quirks.c
index 4bf71028556..2a752167754 100644
--- a/drivers/pci/quirks.c
+++ b/drivers/pci/quirks.c
@@ -2626,6 +2626,18 @@ DECLARE_PCI_FIXUP_FINAL(PCI_VENDOR_ID_ATI, 0x4374,
 DECLARE_PCI_FIXUP_FINAL(PCI_VENDOR_ID_ATI, 0x4375,
 			quirk_msi_intx_disable_bug);
 
+DECLARE_PCI_FIXUP_FINAL(PCI_VENDOR_ID_ATTANSIC, 0x1062,
+			quirk_msi_intx_disable_bug);
+DECLARE_PCI_FIXUP_FINAL(PCI_VENDOR_ID_ATTANSIC, 0x1063,
+			quirk_msi_intx_disable_bug);
+DECLARE_PCI_FIXUP_FINAL(PCI_VENDOR_ID_ATTANSIC, 0x2060,
+			quirk_msi_intx_disable_bug);
+DECLARE_PCI_FIXUP_FINAL(PCI_VENDOR_ID_ATTANSIC, 0x2062,
+			quirk_msi_intx_disable_bug);
+DECLARE_PCI_FIXUP_FINAL(PCI_VENDOR_ID_ATTANSIC, 0x1073,
+			quirk_msi_intx_disable_bug);
+DECLARE_PCI_FIXUP_FINAL(PCI_VENDOR_ID_ATTANSIC, 0x1083,
+			quirk_msi_intx_disable_bug);
 #endif /* CONFIG_PCI_MSI */
 
 /* Allow manual resource allocation for PCI hotplug bridges
@@ -3085,16 +3097,74 @@ static int reset_intel_82599_sfp_virtfn(struct pci_dev *dev, int probe)
 	return 0;
 }
 
+#include "../gpu/drm/i915/i915_reg.h"
+#define MSG_CTL			0x45010
+#define NSDE_PWR_STATE		0xd0100
+#define IGD_OPERATION_TIMEOUT	10000     /* set timeout 10 seconds */
+
+static int reset_ivb_igd(struct pci_dev *dev, int probe)
+{
+	void __iomem *mmio_base;
+	unsigned long timeout;
+	u32 val;
+
+	if (probe)
+		return 0;
+
+	mmio_base = pci_iomap(dev, 0, 0);
+	if (!mmio_base)
+		return -ENOMEM;
+
+	iowrite32(0x00000002, mmio_base + MSG_CTL);
+
+	/*
+	 * Clobbering SOUTH_CHICKEN2 register is fine only if the next
+	 * driver loaded sets the right bits. However, this's a reset and
+	 * the bits have been set by i915 previously, so we clobber
+	 * SOUTH_CHICKEN2 register directly here.
+	 */
+	iowrite32(0x00000005, mmio_base + SOUTH_CHICKEN2);
+
+	val = ioread32(mmio_base + PCH_PP_CONTROL) & 0xfffffffe;
+	iowrite32(val, mmio_base + PCH_PP_CONTROL);
+
+	timeout = jiffies + msecs_to_jiffies(IGD_OPERATION_TIMEOUT);
+	do {
+		val = ioread32(mmio_base + PCH_PP_STATUS);
+		if ((val & 0xb0000000) == 0)
+			goto reset_complete;
+		msleep(10);
+	} while (time_before(jiffies, timeout));
+	dev_warn(&dev->dev, "timeout during reset\n");
+
+reset_complete:
+	iowrite32(0x00000002, mmio_base + NSDE_PWR_STATE);
+
+	pci_iounmap(dev, mmio_base);
+	return 0;
+}
+
 #define PCI_DEVICE_ID_INTEL_82599_SFP_VF   0x10ed
+#define PCI_DEVICE_ID_INTEL_IVB_M_VGA      0x0156
+#define PCI_DEVICE_ID_INTEL_IVB_M2_VGA     0x0166
 
 static const struct pci_dev_reset_methods pci_dev_reset_methods[] = {
 	{ PCI_VENDOR_ID_INTEL, PCI_DEVICE_ID_INTEL_82599_SFP_VF,
 		 reset_intel_82599_sfp_virtfn },
+	{ PCI_VENDOR_ID_INTEL, PCI_DEVICE_ID_INTEL_IVB_M_VGA,
+		reset_ivb_igd },
+	{ PCI_VENDOR_ID_INTEL, PCI_DEVICE_ID_INTEL_IVB_M2_VGA,
+		reset_ivb_igd },
 	{ PCI_VENDOR_ID_INTEL, PCI_ANY_ID,
 		reset_intel_generic_dev },
 	{ 0 }
 };
 
+/*
+ * These device-specific reset methods are here rather than in a driver
+ * because when a host assigns a device to a guest VM, the host may need
+ * to reset the device but probably doesn't have a driver for it.
+ */
 int pci_dev_specific_reset(struct pci_dev *dev, int probe)
 {
 	const struct pci_dev_reset_methods *i;
diff --git a/drivers/pcmcia/Kconfig b/drivers/pcmcia/Kconfig
index bba3ab2066e..8fd255f7ee4 100644
--- a/drivers/pcmcia/Kconfig
+++ b/drivers/pcmcia/Kconfig
@@ -217,7 +217,7 @@ config PCMCIA_PXA2XX
 		    || MACH_ARMCORE || ARCH_PXA_PALM || TRIZEPS_PCMCIA \
 		    || ARCOM_PCMCIA || ARCH_PXA_ESERIES || MACH_STARGATE2 \
 		    || MACH_VPAC270 || MACH_BALLOON3 || MACH_COLIBRI \
-		    || MACH_COLIBRI320)
+		    || MACH_COLIBRI320 || MACH_H4700)
 	select PCMCIA_SA1111 if ARCH_LUBBOCK && SA1111
 	select PCMCIA_SOC_COMMON
 	help
diff --git a/drivers/pcmcia/Makefile b/drivers/pcmcia/Makefile
index 47525de6a63..7745b512a87 100644
--- a/drivers/pcmcia/Makefile
+++ b/drivers/pcmcia/Makefile
@@ -69,6 +69,7 @@ pxa2xx-obj-$(CONFIG_MACH_VPAC270)		+= pxa2xx_vpac270.o
 pxa2xx-obj-$(CONFIG_MACH_BALLOON3)		+= pxa2xx_balloon3.o
 pxa2xx-obj-$(CONFIG_MACH_COLIBRI)		+= pxa2xx_colibri.o
 pxa2xx-obj-$(CONFIG_MACH_COLIBRI320)		+= pxa2xx_colibri.o
+pxa2xx-obj-$(CONFIG_MACH_H4700)			+= pxa2xx_hx4700.o
 
 obj-$(CONFIG_PCMCIA_PXA2XX)			+= pxa2xx_base.o $(pxa2xx-obj-y)
 
diff --git a/drivers/pcmcia/pxa2xx_hx4700.c b/drivers/pcmcia/pxa2xx_hx4700.c
new file mode 100644
index 00000000000..7dfef3ee5b5
--- /dev/null
+++ b/drivers/pcmcia/pxa2xx_hx4700.c
@@ -0,0 +1,121 @@
+/*
+ *  Copyright (C) 2012 Paul Parsons <lost.distance@yahoo.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/platform_device.h>
+#include <linux/err.h>
+#include <linux/gpio.h>
+#include <linux/irq.h>
+
+#include <asm/mach-types.h>
+#include <mach/hx4700.h>
+
+#include "soc_common.h"
+
+static struct gpio gpios[] = {
+	{ GPIO114_HX4700_CF_RESET,    GPIOF_OUT_INIT_LOW,   "CF reset"        },
+	{ EGPIO4_CF_3V3_ON,           GPIOF_OUT_INIT_LOW,   "CF 3.3V enable"  },
+};
+
+static int hx4700_pcmcia_hw_init(struct soc_pcmcia_socket *skt)
+{
+	int ret;
+
+	ret = gpio_request_array(gpios, ARRAY_SIZE(gpios));
+	if (ret)
+		goto out;
+
+	/*
+	 * IRQ type must be set before soc_pcmcia_hw_init() calls request_irq().
+	 * The asic3 default IRQ type is level trigger low level detect, exactly
+	 * the the signal present on GPIOD4_CF_nCD when a CF card is inserted.
+	 * If the IRQ type is not changed, the asic3 interrupt handler will loop
+	 * repeatedly because it is unable to clear the level trigger interrupt.
+	 */
+	irq_set_irq_type(gpio_to_irq(GPIOD4_CF_nCD), IRQ_TYPE_EDGE_BOTH);
+
+	skt->stat[SOC_STAT_CD].gpio = GPIOD4_CF_nCD;
+	skt->stat[SOC_STAT_CD].name = "PCMCIA CD";
+	skt->stat[SOC_STAT_RDY].gpio = GPIO60_HX4700_CF_RNB;
+	skt->stat[SOC_STAT_RDY].name = "PCMCIA Ready";
+
+out:
+	return ret;
+}
+
+static void hx4700_pcmcia_hw_shutdown(struct soc_pcmcia_socket *skt)
+{
+	gpio_free_array(gpios, ARRAY_SIZE(gpios));
+}
+
+static void hx4700_pcmcia_socket_state(struct soc_pcmcia_socket *skt,
+	struct pcmcia_state *state)
+{
+	state->vs_3v = 1;
+	state->vs_Xv = 0;
+}
+
+static int hx4700_pcmcia_configure_socket(struct soc_pcmcia_socket *skt,
+	const socket_state_t *state)
+{
+	switch (state->Vcc) {
+	case 0:
+		gpio_set_value(EGPIO4_CF_3V3_ON, 0);
+		break;
+	case 33:
+		gpio_set_value(EGPIO4_CF_3V3_ON, 1);
+		break;
+	default:
+		printk(KERN_ERR "pcmcia: Unsupported Vcc: %d\n", state->Vcc);
+		return -EINVAL;
+	}
+
+	gpio_set_value(GPIO114_HX4700_CF_RESET, (state->flags & SS_RESET) != 0);
+
+	return 0;
+}
+
+static struct pcmcia_low_level hx4700_pcmcia_ops = {
+	.owner          = THIS_MODULE,
+	.nr             = 1,
+	.hw_init        = hx4700_pcmcia_hw_init,
+	.hw_shutdown    = hx4700_pcmcia_hw_shutdown,
+	.socket_state   = hx4700_pcmcia_socket_state,
+	.configure_socket = hx4700_pcmcia_configure_socket,
+};
+
+static struct platform_device *hx4700_pcmcia_device;
+
+static int __init hx4700_pcmcia_init(void)
+{
+	struct platform_device *pdev;
+
+	if (!machine_is_h4700())
+		return -ENODEV;
+
+	pdev = platform_device_register_data(NULL, "pxa2xx-pcmcia", -1,
+		&hx4700_pcmcia_ops, sizeof(hx4700_pcmcia_ops));
+	if (IS_ERR(pdev))
+		return PTR_ERR(pdev);
+
+	hx4700_pcmcia_device = pdev;
+
+	return 0;
+}
+
+static void __exit hx4700_pcmcia_exit(void)
+{
+	platform_device_unregister(hx4700_pcmcia_device);
+}
+
+module_init(hx4700_pcmcia_init);
+module_exit(hx4700_pcmcia_exit);
+
+MODULE_AUTHOR("Paul Parsons <lost.distance@yahoo.com>");
+MODULE_DESCRIPTION("HP iPAQ hx4700 PCMCIA driver");
+MODULE_LICENSE("GPL");
diff --git a/drivers/pinctrl/Kconfig b/drivers/pinctrl/Kconfig
index abfb9640877..c6e6ae0aa3b 100644
--- a/drivers/pinctrl/Kconfig
+++ b/drivers/pinctrl/Kconfig
@@ -4,7 +4,6 @@
 
 config PINCTRL
 	bool
-	depends on EXPERIMENTAL
 
 if PINCTRL
 
@@ -27,6 +26,35 @@ config DEBUG_PINCTRL
 	help
 	  Say Y here to add some extra checks and diagnostics to PINCTRL calls.
 
+config PINCTRL_IMX
+	bool
+	select PINMUX
+	select PINCONF
+
+config PINCTRL_IMX51
+	bool "IMX51 pinctrl driver"
+	depends on OF
+	depends on SOC_IMX51
+	select PINCTRL_IMX
+	help
+	  Say Y here to enable the imx51 pinctrl driver
+
+config PINCTRL_IMX53
+	bool "IMX53 pinctrl driver"
+	depends on OF
+	depends on SOC_IMX53
+	select PINCTRL_IMX
+	help
+	  Say Y here to enable the imx53 pinctrl driver
+
+config PINCTRL_IMX6Q
+	bool "IMX6Q pinctrl driver"
+	depends on OF
+	depends on SOC_IMX6Q
+	select PINCTRL_IMX
+	help
+	  Say Y here to enable the imx6q pinctrl driver
+
 config PINCTRL_PXA3xx
 	bool
 	select PINMUX
@@ -37,6 +65,31 @@ config PINCTRL_MMP2
 	select PINCTRL_PXA3xx
 	select PINCONF
 
+config PINCTRL_MXS
+	bool
+
+config PINCTRL_IMX23
+	bool
+	select PINMUX
+	select PINCONF
+	select PINCTRL_MXS
+
+config PINCTRL_IMX28
+	bool
+	select PINMUX
+	select PINCONF
+	select PINCTRL_MXS
+
+config PINCTRL_NOMADIK
+	bool "Nomadik pin controller driver"
+	depends on ARCH_U8500 || ARCH_NOMADIK
+	select PINMUX
+	select PINCONF
+
+config PINCTRL_DB8500
+	bool "DB8500 pin controller driver"
+	depends on PINCTRL_NOMADIK && ARCH_U8500
+
 config PINCTRL_PXA168
 	bool "PXA168 pin controller driver"
 	depends on ARCH_MMP
@@ -84,6 +137,8 @@ config PINCTRL_COH901
 	  COH 901 335 and COH 901 571/3. They contain 3, 5 or 7
 	  ports of 8 GPIO pins each.
 
+source "drivers/pinctrl/spear/Kconfig"
+
 endmenu
 
 endif
diff --git a/drivers/pinctrl/Makefile b/drivers/pinctrl/Makefile
index 6d4150b4ece..8c074376cde 100644
--- a/drivers/pinctrl/Makefile
+++ b/drivers/pinctrl/Makefile
@@ -5,9 +5,21 @@ ccflags-$(CONFIG_DEBUG_PINCTRL)	+= -DDEBUG
 obj-$(CONFIG_PINCTRL)		+= core.o
 obj-$(CONFIG_PINMUX)		+= pinmux.o
 obj-$(CONFIG_PINCONF)		+= pinconf.o
+ifeq ($(CONFIG_OF),y)
+obj-$(CONFIG_PINCTRL)		+= devicetree.o
+endif
 obj-$(CONFIG_GENERIC_PINCONF)	+= pinconf-generic.o
+obj-$(CONFIG_PINCTRL_IMX)	+= pinctrl-imx.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_PXA3xx)	+= pinctrl-pxa3xx.o
 obj-$(CONFIG_PINCTRL_MMP2)	+= pinctrl-mmp2.o
+obj-$(CONFIG_PINCTRL_MXS)	+= pinctrl-mxs.o
+obj-$(CONFIG_PINCTRL_IMX23)	+= pinctrl-imx23.o
+obj-$(CONFIG_PINCTRL_IMX28)	+= pinctrl-imx28.o
+obj-$(CONFIG_PINCTRL_NOMADIK)	+= pinctrl-nomadik.o
+obj-$(CONFIG_PINCTRL_DB8500)	+= pinctrl-nomadik-db8500.o
 obj-$(CONFIG_PINCTRL_PXA168)	+= pinctrl-pxa168.o
 obj-$(CONFIG_PINCTRL_PXA910)	+= pinctrl-pxa910.o
 obj-$(CONFIG_PINCTRL_SIRF)	+= pinctrl-sirf.o
@@ -16,3 +28,5 @@ obj-$(CONFIG_PINCTRL_TEGRA20)	+= pinctrl-tegra20.o
 obj-$(CONFIG_PINCTRL_TEGRA30)	+= pinctrl-tegra30.o
 obj-$(CONFIG_PINCTRL_U300)	+= pinctrl-u300.o
 obj-$(CONFIG_PINCTRL_COH901)	+= pinctrl-coh901.o
+
+obj-$(CONFIG_PLAT_SPEAR)	+= spear/
diff --git a/drivers/pinctrl/core.c b/drivers/pinctrl/core.c
index df6296c5f47..c3b331b74fa 100644
--- a/drivers/pinctrl/core.c
+++ b/drivers/pinctrl/core.c
@@ -23,9 +23,11 @@
 #include <linux/sysfs.h>
 #include <linux/debugfs.h>
 #include <linux/seq_file.h>
+#include <linux/pinctrl/consumer.h>
 #include <linux/pinctrl/pinctrl.h>
 #include <linux/pinctrl/machine.h>
 #include "core.h"
+#include "devicetree.h"
 #include "pinmux.h"
 #include "pinconf.h"
 
@@ -41,11 +43,13 @@ struct pinctrl_maps {
 	unsigned num_maps;
 };
 
+static bool pinctrl_dummy_state;
+
 /* Mutex taken by all entry points */
 DEFINE_MUTEX(pinctrl_mutex);
 
 /* Global list of pin control devices (struct pinctrl_dev) */
-static LIST_HEAD(pinctrldev_list);
+LIST_HEAD(pinctrldev_list);
 
 /* List of pin controller handles (struct pinctrl) */
 static LIST_HEAD(pinctrl_list);
@@ -59,6 +63,19 @@ static LIST_HEAD(pinctrl_maps);
 			_i_ < _maps_node_->num_maps; \
 			i++, _map_ = &_maps_node_->maps[_i_])
 
+/**
+ * pinctrl_provide_dummies() - indicate if pinctrl provides dummy state support
+ *
+ * Usually this function is called by platforms without pinctrl driver support
+ * but run with some shared drivers using pinctrl APIs.
+ * After calling this function, the pinctrl core will return successfully
+ * with creating a dummy state for the driver to keep going smoothly.
+ */
+void pinctrl_provide_dummies(void)
+{
+	pinctrl_dummy_state = true;
+}
+
 const char *pinctrl_dev_get_name(struct pinctrl_dev *pctldev)
 {
 	/* We're not allowed to register devices without name */
@@ -124,6 +141,25 @@ int pin_get_from_name(struct pinctrl_dev *pctldev, const char *name)
 }
 
 /**
+ * pin_get_name_from_id() - look up a pin name from a pin id
+ * @pctldev: the pin control device to lookup the pin on
+ * @name: the name of the pin to look up
+ */
+const char *pin_get_name(struct pinctrl_dev *pctldev, const unsigned pin)
+{
+	const struct pin_desc *desc;
+
+	desc = pin_desc_get(pctldev, pin);
+	if (desc == NULL) {
+		dev_err(pctldev->dev, "failed to get pin(%d) name\n",
+			pin);
+		return NULL;
+	}
+
+	return desc->name;
+}
+
+/**
  * pin_is_valid() - check if pin exists on controller
  * @pctldev: the pin control device to check the pin on
  * @pin: pin to check, use the local pin controller index number
@@ -255,7 +291,8 @@ pinctrl_match_gpio_range(struct pinctrl_dev *pctldev, unsigned gpio)
  *
  * Find the pin controller handling a certain GPIO pin from the pinspace of
  * the GPIO subsystem, return the device and the matching GPIO range. Returns
- * negative if the GPIO range could not be found in any device.
+ * -EPROBE_DEFER if the GPIO range could not be found in any device since it
+ * may still have not been registered.
  */
 static int pinctrl_get_device_gpio_range(unsigned gpio,
 					 struct pinctrl_dev **outdev,
@@ -275,7 +312,7 @@ static int pinctrl_get_device_gpio_range(unsigned gpio,
 		}
 	}
 
-	return -EINVAL;
+	return -EPROBE_DEFER;
 }
 
 /**
@@ -318,9 +355,10 @@ int pinctrl_get_group_selector(struct pinctrl_dev *pctldev,
 			       const char *pin_group)
 {
 	const struct pinctrl_ops *pctlops = pctldev->desc->pctlops;
+	unsigned ngroups = pctlops->get_groups_count(pctldev);
 	unsigned group_selector = 0;
 
-	while (pctlops->list_groups(pctldev, group_selector) >= 0) {
+	while (group_selector < ngroups) {
 		const char *gname = pctlops->get_group_name(pctldev,
 							    group_selector);
 		if (!strcmp(gname, pin_group)) {
@@ -360,7 +398,7 @@ int pinctrl_request_gpio(unsigned gpio)
 	ret = pinctrl_get_device_gpio_range(gpio, &pctldev, &range);
 	if (ret) {
 		mutex_unlock(&pinctrl_mutex);
-		return -EINVAL;
+		return ret;
 	}
 
 	/* Convert to the pin controllers number space */
@@ -516,11 +554,14 @@ static int add_setting(struct pinctrl *p, struct pinctrl_map const *map)
 
 	setting->pctldev = get_pinctrl_dev_from_devname(map->ctrl_dev_name);
 	if (setting->pctldev == NULL) {
-		dev_err(p->dev, "unknown pinctrl device %s in map entry",
+		dev_info(p->dev, "unknown pinctrl device %s in map entry, deferring probe",
 			map->ctrl_dev_name);
 		kfree(setting);
-		/* Eventually, this should trigger deferred probe */
-		return -ENODEV;
+		/*
+		 * OK let us guess that the driver is not there yet, and
+		 * let's defer obtaining this pinctrl handle to later...
+		 */
+		return -EPROBE_DEFER;
 	}
 
 	switch (map->type) {
@@ -579,6 +620,13 @@ static struct pinctrl *create_pinctrl(struct device *dev)
 	}
 	p->dev = dev;
 	INIT_LIST_HEAD(&p->states);
+	INIT_LIST_HEAD(&p->dt_maps);
+
+	ret = pinctrl_dt_to_map(p);
+	if (ret < 0) {
+		kfree(p);
+		return ERR_PTR(ret);
+	}
 
 	devname = dev_name(dev);
 
@@ -662,6 +710,8 @@ static void pinctrl_put_locked(struct pinctrl *p, bool inlist)
 		kfree(state);
 	}
 
+	pinctrl_dt_free_maps(p);
+
 	if (inlist)
 		list_del(&p->node);
 	kfree(p);
@@ -685,8 +735,18 @@ static struct pinctrl_state *pinctrl_lookup_state_locked(struct pinctrl *p,
 	struct pinctrl_state *state;
 
 	state = find_state(p, name);
-	if (!state)
-		return ERR_PTR(-ENODEV);
+	if (!state) {
+		if (pinctrl_dummy_state) {
+			/* create dummy state */
+			dev_dbg(p->dev, "using pinctrl dummy state (%s)\n",
+				name);
+			state = create_state(p, name);
+			if (IS_ERR(state))
+				return state;
+		} else {
+			return ERR_PTR(-ENODEV);
+		}
+	}
 
 	return state;
 }
@@ -787,15 +847,63 @@ int pinctrl_select_state(struct pinctrl *p, struct pinctrl_state *state)
 }
 EXPORT_SYMBOL_GPL(pinctrl_select_state);
 
+static void devm_pinctrl_release(struct device *dev, void *res)
+{
+	pinctrl_put(*(struct pinctrl **)res);
+}
+
 /**
- * pinctrl_register_mappings() - register a set of pin controller mappings
- * @maps: the pincontrol mappings table to register. This should probably be
- *	marked with __initdata so it can be discarded after boot. This
- *	function will perform a shallow copy for the mapping entries.
- * @num_maps: the number of maps in the mapping table
+ * struct devm_pinctrl_get() - Resource managed pinctrl_get()
+ * @dev: the device to obtain the handle for
+ *
+ * If there is a need to explicitly destroy the returned struct pinctrl,
+ * devm_pinctrl_put() should be used, rather than plain pinctrl_put().
  */
-int pinctrl_register_mappings(struct pinctrl_map const *maps,
-			      unsigned num_maps)
+struct pinctrl *devm_pinctrl_get(struct device *dev)
+{
+	struct pinctrl **ptr, *p;
+
+	ptr = devres_alloc(devm_pinctrl_release, sizeof(*ptr), GFP_KERNEL);
+	if (!ptr)
+		return ERR_PTR(-ENOMEM);
+
+	p = pinctrl_get(dev);
+	if (!IS_ERR(p)) {
+		*ptr = p;
+		devres_add(dev, ptr);
+	} else {
+		devres_free(ptr);
+	}
+
+	return p;
+}
+EXPORT_SYMBOL_GPL(devm_pinctrl_get);
+
+static int devm_pinctrl_match(struct device *dev, void *res, void *data)
+{
+	struct pinctrl **p = res;
+
+	return *p == data;
+}
+
+/**
+ * devm_pinctrl_put() - Resource managed pinctrl_put()
+ * @p: the pinctrl handle to release
+ *
+ * Deallocate a struct pinctrl obtained via devm_pinctrl_get(). Normally
+ * this function will not need to be called and the resource management
+ * code will ensure that the resource is freed.
+ */
+void devm_pinctrl_put(struct pinctrl *p)
+{
+	WARN_ON(devres_destroy(p->dev, devm_pinctrl_release,
+			       devm_pinctrl_match, p));
+	pinctrl_put(p);
+}
+EXPORT_SYMBOL_GPL(devm_pinctrl_put);
+
+int pinctrl_register_map(struct pinctrl_map const *maps, unsigned num_maps,
+			 bool dup, bool locked)
 {
 	int i, ret;
 	struct pinctrl_maps *maps_node;
@@ -829,13 +937,13 @@ int pinctrl_register_mappings(struct pinctrl_map const *maps,
 		case PIN_MAP_TYPE_MUX_GROUP:
 			ret = pinmux_validate_map(&maps[i], i);
 			if (ret < 0)
-				return 0;
+				return ret;
 			break;
 		case PIN_MAP_TYPE_CONFIGS_PIN:
 		case PIN_MAP_TYPE_CONFIGS_GROUP:
 			ret = pinconf_validate_map(&maps[i], i);
 			if (ret < 0)
-				return 0;
+				return ret;
 			break;
 		default:
 			pr_err("failed to register map %s (%d): invalid type given\n",
@@ -851,20 +959,52 @@ int pinctrl_register_mappings(struct pinctrl_map const *maps,
 	}
 
 	maps_node->num_maps = num_maps;
-	maps_node->maps = kmemdup(maps, sizeof(*maps) * num_maps, GFP_KERNEL);
-	if (!maps_node->maps) {
-		pr_err("failed to duplicate mapping table\n");
-		kfree(maps_node);
-		return -ENOMEM;
+	if (dup) {
+		maps_node->maps = kmemdup(maps, sizeof(*maps) * num_maps,
+					  GFP_KERNEL);
+		if (!maps_node->maps) {
+			pr_err("failed to duplicate mapping table\n");
+			kfree(maps_node);
+			return -ENOMEM;
+		}
+	} else {
+		maps_node->maps = maps;
 	}
 
-	mutex_lock(&pinctrl_mutex);
+	if (!locked)
+		mutex_lock(&pinctrl_mutex);
 	list_add_tail(&maps_node->node, &pinctrl_maps);
-	mutex_unlock(&pinctrl_mutex);
+	if (!locked)
+		mutex_unlock(&pinctrl_mutex);
 
 	return 0;
 }
 
+/**
+ * pinctrl_register_mappings() - register a set of pin controller mappings
+ * @maps: the pincontrol mappings table to register. This should probably be
+ *	marked with __initdata so it can be discarded after boot. This
+ *	function will perform a shallow copy for the mapping entries.
+ * @num_maps: the number of maps in the mapping table
+ */
+int pinctrl_register_mappings(struct pinctrl_map const *maps,
+			      unsigned num_maps)
+{
+	return pinctrl_register_map(maps, num_maps, true, false);
+}
+
+void pinctrl_unregister_map(struct pinctrl_map const *map)
+{
+	struct pinctrl_maps *maps_node;
+
+	list_for_each_entry(maps_node, &pinctrl_maps, node) {
+		if (maps_node->maps == map) {
+			list_del(&maps_node->node);
+			return;
+		}
+	}
+}
+
 #ifdef CONFIG_DEBUG_FS
 
 static int pinctrl_pins_show(struct seq_file *s, void *what)
@@ -906,15 +1046,17 @@ static int pinctrl_groups_show(struct seq_file *s, void *what)
 {
 	struct pinctrl_dev *pctldev = s->private;
 	const struct pinctrl_ops *ops = pctldev->desc->pctlops;
-	unsigned selector = 0;
+	unsigned ngroups, selector = 0;
 
+	ngroups = ops->get_groups_count(pctldev);
 	mutex_lock(&pinctrl_mutex);
 
 	seq_puts(s, "registered pin groups:\n");
-	while (ops->list_groups(pctldev, selector) >= 0) {
+	while (selector < ngroups) {
 		const unsigned *pins;
 		unsigned num_pins;
 		const char *gname = ops->get_group_name(pctldev, selector);
+		const char *pname;
 		int ret;
 		int i;
 
@@ -924,10 +1066,14 @@ static int pinctrl_groups_show(struct seq_file *s, void *what)
 			seq_printf(s, "%s [ERROR GETTING PINS]\n",
 				   gname);
 		else {
-			seq_printf(s, "group: %s, pins = [ ", gname);
-			for (i = 0; i < num_pins; i++)
-				seq_printf(s, "%d ", pins[i]);
-			seq_puts(s, "]\n");
+			seq_printf(s, "group: %s\n", gname);
+			for (i = 0; i < num_pins; i++) {
+				pname = pin_get_name(pctldev, pins[i]);
+				if (WARN_ON(!pname))
+					return -EINVAL;
+				seq_printf(s, "pin %d (%s)\n", pins[i], pname);
+			}
+			seq_puts(s, "\n");
 		}
 		selector++;
 	}
@@ -1226,11 +1372,14 @@ static int pinctrl_check_ops(struct pinctrl_dev *pctldev)
 	const struct pinctrl_ops *ops = pctldev->desc->pctlops;
 
 	if (!ops ||
-	    !ops->list_groups ||
+	    !ops->get_groups_count ||
 	    !ops->get_group_name ||
 	    !ops->get_group_pins)
 		return -EINVAL;
 
+	if (ops->dt_node_to_map && !ops->dt_free_map)
+		return -EINVAL;
+
 	return 0;
 }
 
@@ -1268,37 +1417,29 @@ struct pinctrl_dev *pinctrl_register(struct pinctrl_desc *pctldesc,
 	/* check core ops for sanity */
 	ret = pinctrl_check_ops(pctldev);
 	if (ret) {
-		pr_err("%s pinctrl ops lacks necessary functions\n",
-			pctldesc->name);
+		dev_err(dev, "pinctrl ops lacks necessary functions\n");
 		goto out_err;
 	}
 
 	/* If we're implementing pinmuxing, check the ops for sanity */
 	if (pctldesc->pmxops) {
 		ret = pinmux_check_ops(pctldev);
-		if (ret) {
-			pr_err("%s pinmux ops lacks necessary functions\n",
-			       pctldesc->name);
+		if (ret)
 			goto out_err;
-		}
 	}
 
 	/* If we're implementing pinconfig, check the ops for sanity */
 	if (pctldesc->confops) {
 		ret = pinconf_check_ops(pctldev);
-		if (ret) {
-			pr_err("%s pin config ops lacks necessary functions\n",
-			       pctldesc->name);
+		if (ret)
 			goto out_err;
-		}
 	}
 
 	/* Register all the pins */
-	pr_debug("try to register %d pins on %s...\n",
-		 pctldesc->npins, pctldesc->name);
+	dev_dbg(dev, "try to register %d pins ...\n",  pctldesc->npins);
 	ret = pinctrl_register_pins(pctldev, pctldesc->pins, pctldesc->npins);
 	if (ret) {
-		pr_err("error during pin registration\n");
+		dev_err(dev, "error during pin registration\n");
 		pinctrl_free_pindescs(pctldev, pctldesc->pins,
 				      pctldesc->npins);
 		goto out_err;
@@ -1313,8 +1454,15 @@ struct pinctrl_dev *pinctrl_register(struct pinctrl_desc *pctldesc,
 		struct pinctrl_state *s =
 			pinctrl_lookup_state_locked(pctldev->p,
 						    PINCTRL_STATE_DEFAULT);
-		if (!IS_ERR(s))
-			pinctrl_select_state_locked(pctldev->p, s);
+		if (IS_ERR(s)) {
+			dev_dbg(dev, "failed to lookup the default state\n");
+		} else {
+			ret = pinctrl_select_state_locked(pctldev->p, s);
+			if (ret) {
+				dev_err(dev,
+					"failed to select default state\n");
+			}
+		}
 	}
 
 	mutex_unlock(&pinctrl_mutex);
diff --git a/drivers/pinctrl/core.h b/drivers/pinctrl/core.h
index 17ecf651b12..1f40ff68a8c 100644
--- a/drivers/pinctrl/core.h
+++ b/drivers/pinctrl/core.h
@@ -52,12 +52,15 @@ struct pinctrl_dev {
  * @dev: the device using this pin control handle
  * @states: a list of states for this device
  * @state: the current state
+ * @dt_maps: the mapping table chunks dynamically parsed from device tree for
+ *	this device, if any
  */
 struct pinctrl {
 	struct list_head node;
 	struct device *dev;
 	struct list_head states;
 	struct pinctrl_state *state;
+	struct list_head dt_maps;
 };
 
 /**
@@ -100,7 +103,8 @@ struct pinctrl_setting_configs {
  * struct pinctrl_setting - an individual mux or config setting
  * @node: list node for struct pinctrl_settings's @settings field
  * @type: the type of setting
- * @pctldev: pin control device handling to be programmed
+ * @pctldev: pin control device handling to be programmed. Not used for
+ *   PIN_MAP_TYPE_DUMMY_STATE.
  * @data: Data specific to the setting type
  */
 struct pinctrl_setting {
@@ -144,6 +148,7 @@ struct pin_desc {
 
 struct pinctrl_dev *get_pinctrl_dev_from_devname(const char *dev_name);
 int pin_get_from_name(struct pinctrl_dev *pctldev, const char *name);
+const char *pin_get_name(struct pinctrl_dev *pctldev, const unsigned pin);
 int pinctrl_get_group_selector(struct pinctrl_dev *pctldev,
 			       const char *pin_group);
 
@@ -153,4 +158,9 @@ static inline struct pin_desc *pin_desc_get(struct pinctrl_dev *pctldev,
 	return radix_tree_lookup(&pctldev->pin_desc_tree, pin);
 }
 
+int pinctrl_register_map(struct pinctrl_map const *maps, unsigned num_maps,
+			 bool dup, bool locked);
+void pinctrl_unregister_map(struct pinctrl_map const *map);
+
 extern struct mutex pinctrl_mutex;
+extern struct list_head pinctrldev_list;
diff --git a/drivers/pinctrl/devicetree.c b/drivers/pinctrl/devicetree.c
new file mode 100644
index 00000000000..fcb1de45473
--- /dev/null
+++ b/drivers/pinctrl/devicetree.c
@@ -0,0 +1,249 @@
+/*
+ * Device tree integration for the pin control subsystem
+ *
+ * Copyright (C) 2012 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/device.h>
+#include <linux/of.h>
+#include <linux/pinctrl/pinctrl.h>
+#include <linux/slab.h>
+
+#include "core.h"
+#include "devicetree.h"
+
+/**
+ * struct pinctrl_dt_map - mapping table chunk parsed from device tree
+ * @node: list node for struct pinctrl's @dt_maps field
+ * @pctldev: the pin controller that allocated this struct, and will free it
+ * @maps: the mapping table entries
+ */
+struct pinctrl_dt_map {
+	struct list_head node;
+	struct pinctrl_dev *pctldev;
+	struct pinctrl_map *map;
+	unsigned num_maps;
+};
+
+static void dt_free_map(struct pinctrl_dev *pctldev,
+		     struct pinctrl_map *map, unsigned num_maps)
+{
+	if (pctldev) {
+		struct pinctrl_ops *ops = pctldev->desc->pctlops;
+		ops->dt_free_map(pctldev, map, num_maps);
+	} else {
+		/* There is no pctldev for PIN_MAP_TYPE_DUMMY_STATE */
+		kfree(map);
+	}
+}
+
+void pinctrl_dt_free_maps(struct pinctrl *p)
+{
+	struct pinctrl_dt_map *dt_map, *n1;
+
+	list_for_each_entry_safe(dt_map, n1, &p->dt_maps, node) {
+		pinctrl_unregister_map(dt_map->map);
+		list_del(&dt_map->node);
+		dt_free_map(dt_map->pctldev, dt_map->map,
+			    dt_map->num_maps);
+		kfree(dt_map);
+	}
+
+	of_node_put(p->dev->of_node);
+}
+
+static int dt_remember_or_free_map(struct pinctrl *p, const char *statename,
+				   struct pinctrl_dev *pctldev,
+				   struct pinctrl_map *map, unsigned num_maps)
+{
+	int i;
+	struct pinctrl_dt_map *dt_map;
+
+	/* Initialize common mapping table entry fields */
+	for (i = 0; i < num_maps; i++) {
+		map[i].dev_name = dev_name(p->dev);
+		map[i].name = statename;
+		if (pctldev)
+			map[i].ctrl_dev_name = dev_name(pctldev->dev);
+	}
+
+	/* Remember the converted mapping table entries */
+	dt_map = kzalloc(sizeof(*dt_map), GFP_KERNEL);
+	if (!dt_map) {
+		dev_err(p->dev, "failed to alloc struct pinctrl_dt_map\n");
+		dt_free_map(pctldev, map, num_maps);
+		return -ENOMEM;
+	}
+
+	dt_map->pctldev = pctldev;
+	dt_map->map = map;
+	dt_map->num_maps = num_maps;
+	list_add_tail(&dt_map->node, &p->dt_maps);
+
+	return pinctrl_register_map(map, num_maps, false, true);
+}
+
+static struct pinctrl_dev *find_pinctrl_by_of_node(struct device_node *np)
+{
+	struct pinctrl_dev *pctldev;
+
+	list_for_each_entry(pctldev, &pinctrldev_list, node)
+		if (pctldev->dev->of_node == np)
+			return pctldev;
+
+	return NULL;
+}
+
+static int dt_to_map_one_config(struct pinctrl *p, const char *statename,
+				struct device_node *np_config)
+{
+	struct device_node *np_pctldev;
+	struct pinctrl_dev *pctldev;
+	struct pinctrl_ops *ops;
+	int ret;
+	struct pinctrl_map *map;
+	unsigned num_maps;
+
+	/* Find the pin controller containing np_config */
+	np_pctldev = of_node_get(np_config);
+	for (;;) {
+		np_pctldev = of_get_next_parent(np_pctldev);
+		if (!np_pctldev || of_node_is_root(np_pctldev)) {
+			dev_info(p->dev, "could not find pctldev for node %s, deferring probe\n",
+				np_config->full_name);
+			of_node_put(np_pctldev);
+			/* OK let's just assume this will appear later then */
+			return -EPROBE_DEFER;
+		}
+		pctldev = find_pinctrl_by_of_node(np_pctldev);
+		if (pctldev)
+			break;
+	}
+	of_node_put(np_pctldev);
+
+	/*
+	 * Call pinctrl driver to parse device tree node, and
+	 * generate mapping table entries
+	 */
+	ops = pctldev->desc->pctlops;
+	if (!ops->dt_node_to_map) {
+		dev_err(p->dev, "pctldev %s doesn't support DT\n",
+			dev_name(pctldev->dev));
+		return -ENODEV;
+	}
+	ret = ops->dt_node_to_map(pctldev, np_config, &map, &num_maps);
+	if (ret < 0)
+		return ret;
+
+	/* Stash the mapping table chunk away for later use */
+	return dt_remember_or_free_map(p, statename, pctldev, map, num_maps);
+}
+
+static int dt_remember_dummy_state(struct pinctrl *p, const char *statename)
+{
+	struct pinctrl_map *map;
+
+	map = kzalloc(sizeof(*map), GFP_KERNEL);
+	if (!map) {
+		dev_err(p->dev, "failed to alloc struct pinctrl_map\n");
+		return -ENOMEM;
+	}
+
+	/* There is no pctldev for PIN_MAP_TYPE_DUMMY_STATE */
+	map->type = PIN_MAP_TYPE_DUMMY_STATE;
+
+	return dt_remember_or_free_map(p, statename, NULL, map, 1);
+}
+
+int pinctrl_dt_to_map(struct pinctrl *p)
+{
+	struct device_node *np = p->dev->of_node;
+	int state, ret;
+	char *propname;
+	struct property *prop;
+	const char *statename;
+	const __be32 *list;
+	int size, config;
+	phandle phandle;
+	struct device_node *np_config;
+
+	/* CONFIG_OF enabled, p->dev not instantiated from DT */
+	if (!np) {
+		dev_dbg(p->dev, "no of_node; not parsing pinctrl DT\n");
+		return 0;
+	}
+
+	/* We may store pointers to property names within the node */
+	of_node_get(np);
+
+	/* For each defined state ID */
+	for (state = 0; ; state++) {
+		/* Retrieve the pinctrl-* property */
+		propname = kasprintf(GFP_KERNEL, "pinctrl-%d", state);
+		prop = of_find_property(np, propname, &size);
+		kfree(propname);
+		if (!prop)
+			break;
+		list = prop->value;
+		size /= sizeof(*list);
+
+		/* Determine whether pinctrl-names property names the state */
+		ret = of_property_read_string_index(np, "pinctrl-names",
+						    state, &statename);
+		/*
+		 * If not, statename is just the integer state ID. But rather
+		 * than dynamically allocate it and have to free it later,
+		 * just point part way into the property name for the string.
+		 */
+		if (ret < 0) {
+			/* strlen("pinctrl-") == 8 */
+			statename = prop->name + 8;
+		}
+
+		/* For every referenced pin configuration node in it */
+		for (config = 0; config < size; config++) {
+			phandle = be32_to_cpup(list++);
+
+			/* Look up the pin configuration node */
+			np_config = of_find_node_by_phandle(phandle);
+			if (!np_config) {
+				dev_err(p->dev,
+					"prop %s index %i invalid phandle\n",
+					prop->name, config);
+				ret = -EINVAL;
+				goto err;
+			}
+
+			/* Parse the node */
+			ret = dt_to_map_one_config(p, statename, np_config);
+			of_node_put(np_config);
+			if (ret < 0)
+				goto err;
+		}
+
+		/* No entries in DT? Generate a dummy state table entry */
+		if (!size) {
+			ret = dt_remember_dummy_state(p, statename);
+			if (ret < 0)
+				goto err;
+		}
+	}
+
+	return 0;
+
+err:
+	pinctrl_dt_free_maps(p);
+	return ret;
+}
diff --git a/drivers/pinctrl/devicetree.h b/drivers/pinctrl/devicetree.h
new file mode 100644
index 00000000000..760bc4960f5
--- /dev/null
+++ b/drivers/pinctrl/devicetree.h
@@ -0,0 +1,35 @@
+/*
+ * Internal interface to pinctrl device tree integration
+ *
+ * Copyright (C) 2012 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/>.
+ */
+
+#ifdef CONFIG_OF
+
+void pinctrl_dt_free_maps(struct pinctrl *p);
+int pinctrl_dt_to_map(struct pinctrl *p);
+
+#else
+
+static inline int pinctrl_dt_to_map(struct pinctrl *p)
+{
+	return 0;
+}
+
+static inline void pinctrl_dt_free_maps(struct pinctrl *p)
+{
+}
+
+#endif
diff --git a/drivers/pinctrl/pinconf.c b/drivers/pinctrl/pinconf.c
index 7321e860129..43f474cdc11 100644
--- a/drivers/pinctrl/pinconf.c
+++ b/drivers/pinctrl/pinconf.c
@@ -28,11 +28,17 @@ int pinconf_check_ops(struct pinctrl_dev *pctldev)
 	const struct pinconf_ops *ops = pctldev->desc->confops;
 
 	/* We must be able to read out pin status */
-	if (!ops->pin_config_get && !ops->pin_config_group_get)
+	if (!ops->pin_config_get && !ops->pin_config_group_get) {
+		dev_err(pctldev->dev,
+			"pinconf must be able to read out pin status\n");
 		return -EINVAL;
+	}
 	/* We have to be able to config the pins in SOME way */
-	if (!ops->pin_config_set && !ops->pin_config_group_set)
+	if (!ops->pin_config_set && !ops->pin_config_group_set) {
+		dev_err(pctldev->dev,
+			"pinconf has to be able to set a pins config\n");
 		return -EINVAL;
+	}
 	return 0;
 }
 
@@ -44,9 +50,9 @@ int pinconf_validate_map(struct pinctrl_map const *map, int i)
 		return -EINVAL;
 	}
 
-	if (map->data.configs.num_configs &&
+	if (!map->data.configs.num_configs ||
 			!map->data.configs.configs) {
-		pr_err("failed to register map %s (%d): no configs ptr given\n",
+		pr_err("failed to register map %s (%d): no configs given\n",
 		       map->name, i);
 		return -EINVAL;
 	}
@@ -379,8 +385,16 @@ int pinconf_apply_setting(struct pinctrl_setting const *setting)
 
 void pinconf_show_map(struct seq_file *s, struct pinctrl_map const *map)
 {
+	struct pinctrl_dev *pctldev;
+	const struct pinconf_ops *confops;
 	int i;
 
+	pctldev = get_pinctrl_dev_from_devname(map->ctrl_dev_name);
+	if (pctldev)
+		confops = pctldev->desc->confops;
+	else
+		confops = NULL;
+
 	switch (map->type) {
 	case PIN_MAP_TYPE_CONFIGS_PIN:
 		seq_printf(s, "pin ");
@@ -394,8 +408,15 @@ void pinconf_show_map(struct seq_file *s, struct pinctrl_map const *map)
 
 	seq_printf(s, "%s\n", map->data.configs.group_or_pin);
 
-	for (i = 0; i < map->data.configs.num_configs; i++)
-		seq_printf(s, "config %08lx\n", map->data.configs.configs[i]);
+	for (i = 0; i < map->data.configs.num_configs; i++) {
+		seq_printf(s, "config ");
+		if (confops && confops->pin_config_config_dbg_show)
+			confops->pin_config_config_dbg_show(pctldev, s,
+						map->data.configs.configs[i]);
+		else
+			seq_printf(s, "%08lx", map->data.configs.configs[i]);
+		seq_printf(s, "\n");
+	}
 }
 
 void pinconf_show_setting(struct seq_file *s,
@@ -403,6 +424,7 @@ void pinconf_show_setting(struct seq_file *s,
 {
 	struct pinctrl_dev *pctldev = setting->pctldev;
 	const struct pinctrl_ops *pctlops = pctldev->desc->pctlops;
+	const struct pinconf_ops *confops = pctldev->desc->confops;
 	struct pin_desc *desc;
 	int i;
 
@@ -428,8 +450,15 @@ void pinconf_show_setting(struct seq_file *s,
 	 * FIXME: We should really get the pin controler to dump the config
 	 * values, so they can be decoded to something meaningful.
 	 */
-	for (i = 0; i < setting->data.configs.num_configs; i++)
-		seq_printf(s, " %08lx", setting->data.configs.configs[i]);
+	for (i = 0; i < setting->data.configs.num_configs; i++) {
+		seq_printf(s, " ");
+		if (confops && confops->pin_config_config_dbg_show)
+			confops->pin_config_config_dbg_show(pctldev, s,
+				setting->data.configs.configs[i]);
+		else
+			seq_printf(s, "%08lx",
+				   setting->data.configs.configs[i]);
+	}
 
 	seq_printf(s, "\n");
 }
@@ -448,10 +477,14 @@ static void pinconf_dump_pin(struct pinctrl_dev *pctldev,
 static int pinconf_pins_show(struct seq_file *s, void *what)
 {
 	struct pinctrl_dev *pctldev = s->private;
+	const struct pinconf_ops *ops = pctldev->desc->confops;
 	unsigned i, pin;
 
+	if (!ops || !ops->pin_config_get)
+		return 0;
+
 	seq_puts(s, "Pin config settings per pin\n");
-	seq_puts(s, "Format: pin (name): pinmux setting array\n");
+	seq_puts(s, "Format: pin (name): configs\n");
 
 	mutex_lock(&pinctrl_mutex);
 
@@ -495,17 +528,18 @@ static int pinconf_groups_show(struct seq_file *s, void *what)
 	struct pinctrl_dev *pctldev = s->private;
 	const struct pinctrl_ops *pctlops = pctldev->desc->pctlops;
 	const struct pinconf_ops *ops = pctldev->desc->confops;
+	unsigned ngroups = pctlops->get_groups_count(pctldev);
 	unsigned selector = 0;
 
 	if (!ops || !ops->pin_config_group_get)
 		return 0;
 
 	seq_puts(s, "Pin config settings per pin group\n");
-	seq_puts(s, "Format: group (name): pinmux setting array\n");
+	seq_puts(s, "Format: group (name): configs\n");
 
 	mutex_lock(&pinctrl_mutex);
 
-	while (pctlops->list_groups(pctldev, selector) >= 0) {
+	while (selector < ngroups) {
 		const char *gname = pctlops->get_group_name(pctldev, selector);
 
 		seq_printf(s, "%u (%s):", selector, gname);
diff --git a/drivers/pinctrl/pinconf.h b/drivers/pinctrl/pinconf.h
index 54510de5e8c..e3ed8cb072a 100644
--- a/drivers/pinctrl/pinconf.h
+++ b/drivers/pinctrl/pinconf.h
@@ -19,11 +19,6 @@ int pinconf_map_to_setting(struct pinctrl_map const *map,
 			  struct pinctrl_setting *setting);
 void pinconf_free_setting(struct pinctrl_setting const *setting);
 int pinconf_apply_setting(struct pinctrl_setting const *setting);
-void pinconf_show_map(struct seq_file *s, struct pinctrl_map const *map);
-void pinconf_show_setting(struct seq_file *s,
-			  struct pinctrl_setting const *setting);
-void pinconf_init_device_debugfs(struct dentry *devroot,
-				 struct pinctrl_dev *pctldev);
 
 /*
  * You will only be interested in these if you're using PINCONF
@@ -61,6 +56,18 @@ static inline int pinconf_apply_setting(struct pinctrl_setting const *setting)
 	return 0;
 }
 
+#endif
+
+#if defined(CONFIG_PINCONF) && defined(CONFIG_DEBUG_FS)
+
+void pinconf_show_map(struct seq_file *s, struct pinctrl_map const *map);
+void pinconf_show_setting(struct seq_file *s,
+			  struct pinctrl_setting const *setting);
+void pinconf_init_device_debugfs(struct dentry *devroot,
+				 struct pinctrl_dev *pctldev);
+
+#else
+
 static inline void pinconf_show_map(struct seq_file *s,
 				    struct pinctrl_map const *map)
 {
diff --git a/drivers/pinctrl/pinctrl-coh901.c b/drivers/pinctrl/pinctrl-coh901.c
index 0797eba3e33..55697a5d748 100644
--- a/drivers/pinctrl/pinctrl-coh901.c
+++ b/drivers/pinctrl/pinctrl-coh901.c
@@ -174,7 +174,7 @@ struct u300_gpio_confdata {
 
 
 /* Initial configuration */
-static const struct __initdata u300_gpio_confdata
+static const struct __initconst u300_gpio_confdata
 bs335_gpio_config[BS335_GPIO_NUM_PORTS][U300_GPIO_PINS_PER_PORT] = {
 	/* Port 0, pins 0-7 */
 	{
@@ -255,7 +255,7 @@ bs335_gpio_config[BS335_GPIO_NUM_PORTS][U300_GPIO_PINS_PER_PORT] = {
 	}
 };
 
-static const struct __initdata u300_gpio_confdata
+static const struct __initconst u300_gpio_confdata
 bs365_gpio_config[BS365_GPIO_NUM_PORTS][U300_GPIO_PINS_PER_PORT] = {
 	/* Port 0, pins 0-7 */
 	{
diff --git a/drivers/pinctrl/pinctrl-imx.c b/drivers/pinctrl/pinctrl-imx.c
new file mode 100644
index 00000000000..f6e7c670906
--- /dev/null
+++ b/drivers/pinctrl/pinctrl-imx.c
@@ -0,0 +1,620 @@
+/*
+ * Core driver for the imx pin controller
+ *
+ * Copyright (C) 2012 Freescale Semiconductor, Inc.
+ * Copyright (C) 2012 Linaro Ltd.
+ *
+ * Author: Dong Aisheng <dong.aisheng@linaro.org>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ */
+
+#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/machine.h>
+#include <linux/pinctrl/pinconf.h>
+#include <linux/pinctrl/pinctrl.h>
+#include <linux/pinctrl/pinmux.h>
+#include <linux/slab.h>
+
+#include "core.h"
+#include "pinctrl-imx.h"
+
+#define IMX_PMX_DUMP(info, p, m, c, n)		\
+{						\
+	int i, j;				\
+	printk("Format: Pin Mux Config\n");	\
+	for (i = 0; i < n; i++) {		\
+		j = p[i];			\
+		printk("%s %d 0x%lx\n",		\
+			info->pins[j].name,	\
+			m[i], c[i]);		\
+	}					\
+}
+
+/* The bits in CONFIG cell defined in binding doc*/
+#define IMX_NO_PAD_CTL	0x80000000	/* no pin config need */
+#define IMX_PAD_SION 0x40000000		/* set SION */
+
+/**
+ * @dev: a pointer back to containing device
+ * @base: the offset to the controller in virtual memory
+ */
+struct imx_pinctrl {
+	struct device *dev;
+	struct pinctrl_dev *pctl;
+	void __iomem *base;
+	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 (!pin_reg) {
+		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)
+{
+	const struct imx_pin_group *grp = NULL;
+	int i;
+
+	for (i = 0; i < info->ngroups; i++) {
+		if (!strcmp(info->groups[i].name, name)) {
+			grp = &info->groups[i];
+			break;
+		}
+	}
+
+	return grp;
+}
+
+static int imx_get_groups_count(struct pinctrl_dev *pctldev)
+{
+	struct imx_pinctrl *ipctl = pinctrl_dev_get_drvdata(pctldev);
+	const struct imx_pinctrl_soc_info *info = ipctl->info;
+
+	return info->ngroups;
+}
+
+static const char *imx_get_group_name(struct pinctrl_dev *pctldev,
+				       unsigned selector)
+{
+	struct imx_pinctrl *ipctl = pinctrl_dev_get_drvdata(pctldev);
+	const struct imx_pinctrl_soc_info *info = ipctl->info;
+
+	return info->groups[selector].name;
+}
+
+static int imx_get_group_pins(struct pinctrl_dev *pctldev, unsigned selector,
+			       const unsigned **pins,
+			       unsigned *npins)
+{
+	struct imx_pinctrl *ipctl = pinctrl_dev_get_drvdata(pctldev);
+	const struct imx_pinctrl_soc_info *info = ipctl->info;
+
+	if (selector >= info->ngroups)
+		return -EINVAL;
+
+	*pins = info->groups[selector].pins;
+	*npins = info->groups[selector].npins;
+
+	return 0;
+}
+
+static void imx_pin_dbg_show(struct pinctrl_dev *pctldev, struct seq_file *s,
+		   unsigned offset)
+{
+	seq_printf(s, "%s", dev_name(pctldev->dev));
+}
+
+static int imx_dt_node_to_map(struct pinctrl_dev *pctldev,
+			struct device_node *np,
+			struct pinctrl_map **map, unsigned *num_maps)
+{
+	struct imx_pinctrl *ipctl = pinctrl_dev_get_drvdata(pctldev);
+	const struct imx_pinctrl_soc_info *info = ipctl->info;
+	const struct imx_pin_group *grp;
+	struct pinctrl_map *new_map;
+	struct device_node *parent;
+	int map_num = 1;
+	int i;
+
+	/*
+	 * first find the group of this node and check if we need create
+	 * config maps for pins
+	 */
+	grp = imx_pinctrl_find_group_by_name(info, np->name);
+	if (!grp) {
+		dev_err(info->dev, "unable to find group for node %s\n",
+			np->name);
+		return -EINVAL;
+	}
+
+	for (i = 0; i < grp->npins; i++) {
+		if (!(grp->configs[i] & IMX_NO_PAD_CTL))
+			map_num++;
+	}
+
+	new_map = kmalloc(sizeof(struct pinctrl_map) * map_num, GFP_KERNEL);
+	if (!new_map)
+		return -ENOMEM;
+
+	*map = new_map;
+	*num_maps = map_num;
+
+	/* create mux map */
+	parent = of_get_parent(np);
+	if (!parent)
+		return -EINVAL;
+	new_map[0].type = PIN_MAP_TYPE_MUX_GROUP;
+	new_map[0].data.mux.function = parent->name;
+	new_map[0].data.mux.group = np->name;
+	of_node_put(parent);
+
+	/* create config map */
+	new_map++;
+	for (i = 0; i < grp->npins; i++) {
+		if (!(grp->configs[i] & IMX_NO_PAD_CTL)) {
+			new_map[i].type = PIN_MAP_TYPE_CONFIGS_PIN;
+			new_map[i].data.configs.group_or_pin =
+					pin_get_name(pctldev, grp->pins[i]);
+			new_map[i].data.configs.configs = &grp->configs[i];
+			new_map[i].data.configs.num_configs = 1;
+		}
+	}
+
+	dev_dbg(pctldev->dev, "maps: function %s group %s num %d\n",
+		new_map->data.mux.function, new_map->data.mux.group, map_num);
+
+	return 0;
+}
+
+static void imx_dt_free_map(struct pinctrl_dev *pctldev,
+				struct pinctrl_map *map, unsigned num_maps)
+{
+	int i;
+
+	for (i = 0; i < num_maps; i++)
+		kfree(map);
+}
+
+static struct pinctrl_ops imx_pctrl_ops = {
+	.get_groups_count = imx_get_groups_count,
+	.get_group_name = imx_get_group_name,
+	.get_group_pins = imx_get_group_pins,
+	.pin_dbg_show = imx_pin_dbg_show,
+	.dt_node_to_map = imx_dt_node_to_map,
+	.dt_free_map = imx_dt_free_map,
+
+};
+
+static int imx_pmx_enable(struct pinctrl_dev *pctldev, unsigned selector,
+			   unsigned group)
+{
+	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;
+	unsigned int npins, pin_id;
+	int i;
+
+	/*
+	 * Configure the mux mode for each pin in the group for a specific
+	 * function.
+	 */
+	pins = info->groups[group].pins;
+	npins = info->groups[group].npins;
+	mux = info->groups[group].mux_mode;
+
+	WARN_ON(!pins || !npins || !mux);
+
+	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;
+
+		if (!pin_reg->mux_reg) {
+			dev_err(ipctl->dev, "Pin(%s) does not support mux function\n",
+				info->pins[pin_id].name);
+			return -EINVAL;
+		}
+
+		writel(mux[i], ipctl->base + pin_reg->mux_reg);
+		dev_dbg(ipctl->dev, "write: offset 0x%x val 0x%x\n",
+			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);
+			dev_dbg(ipctl->dev,
+				"==>select_input: offset 0x%x val 0x%x\n",
+				pin_reg->input_reg, pin_reg->input_val);
+		}
+	}
+
+	return 0;
+}
+
+static int imx_pmx_get_funcs_count(struct pinctrl_dev *pctldev)
+{
+	struct imx_pinctrl *ipctl = pinctrl_dev_get_drvdata(pctldev);
+	const struct imx_pinctrl_soc_info *info = ipctl->info;
+
+	return info->nfunctions;
+}
+
+static const char *imx_pmx_get_func_name(struct pinctrl_dev *pctldev,
+					  unsigned selector)
+{
+	struct imx_pinctrl *ipctl = pinctrl_dev_get_drvdata(pctldev);
+	const struct imx_pinctrl_soc_info *info = ipctl->info;
+
+	return info->functions[selector].name;
+}
+
+static int imx_pmx_get_groups(struct pinctrl_dev *pctldev, unsigned selector,
+			       const char * const **groups,
+			       unsigned * const num_groups)
+{
+	struct imx_pinctrl *ipctl = pinctrl_dev_get_drvdata(pctldev);
+	const struct imx_pinctrl_soc_info *info = ipctl->info;
+
+	*groups = info->functions[selector].groups;
+	*num_groups = info->functions[selector].num_groups;
+
+	return 0;
+}
+
+static struct pinmux_ops imx_pmx_ops = {
+	.get_functions_count = imx_pmx_get_funcs_count,
+	.get_function_name = imx_pmx_get_func_name,
+	.get_function_groups = imx_pmx_get_groups,
+	.enable = imx_pmx_enable,
+};
+
+static int imx_pinconf_get(struct pinctrl_dev *pctldev,
+			     unsigned pin_id, unsigned long *config)
+{
+	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;
+
+	if (!pin_reg->conf_reg) {
+		dev_err(info->dev, "Pin(%s) does not support config function\n",
+			info->pins[pin_id].name);
+		return -EINVAL;
+	}
+
+	*config = readl(ipctl->base + pin_reg->conf_reg);
+
+	return 0;
+}
+
+static int imx_pinconf_set(struct pinctrl_dev *pctldev,
+			     unsigned pin_id, unsigned long config)
+{
+	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;
+
+	if (!pin_reg->conf_reg) {
+		dev_err(info->dev, "Pin(%s) does not support config function\n",
+			info->pins[pin_id].name);
+		return -EINVAL;
+	}
+
+	dev_dbg(ipctl->dev, "pinconf set pin %s\n",
+		info->pins[pin_id].name);
+
+	writel(config, ipctl->base + pin_reg->conf_reg);
+	dev_dbg(ipctl->dev, "write: offset 0x%x val 0x%lx\n",
+		pin_reg->conf_reg, config);
+
+	return 0;
+}
+
+static void imx_pinconf_dbg_show(struct pinctrl_dev *pctldev,
+				   struct seq_file *s, unsigned pin_id)
+{
+	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;
+	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;
+	}
+
+	config = readl(ipctl->base + pin_reg->conf_reg);
+	seq_printf(s, "0x%lx", config);
+}
+
+static void imx_pinconf_group_dbg_show(struct pinctrl_dev *pctldev,
+					 struct seq_file *s, unsigned group)
+{
+	struct imx_pinctrl *ipctl = pinctrl_dev_get_drvdata(pctldev);
+	const struct imx_pinctrl_soc_info *info = ipctl->info;
+	struct imx_pin_group *grp;
+	unsigned long config;
+	const char *name;
+	int i, ret;
+
+	if (group > info->ngroups)
+		return;
+
+	seq_printf(s, "\n");
+	grp = &info->groups[group];
+	for (i = 0; i < grp->npins; i++) {
+		name = pin_get_name(pctldev, grp->pins[i]);
+		ret = imx_pinconf_get(pctldev, grp->pins[i], &config);
+		if (ret)
+			return;
+		seq_printf(s, "%s: 0x%lx", name, config);
+	}
+}
+
+struct pinconf_ops imx_pinconf_ops = {
+	.pin_config_get = imx_pinconf_get,
+	.pin_config_set = imx_pinconf_set,
+	.pin_config_dbg_show = imx_pinconf_dbg_show,
+	.pin_config_group_dbg_show = imx_pinconf_group_dbg_show,
+};
+
+static struct pinctrl_desc imx_pinctrl_desc = {
+	.pctlops = &imx_pctrl_ops,
+	.pmxops = &imx_pmx_ops,
+	.confops = &imx_pinconf_ops,
+	.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;
+}
+
+static int __devinit 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;
+	const const __be32 *list;
+	int i, j;
+	u32 config;
+
+	dev_dbg(info->dev, "group(%d): %s\n", index, np->name);
+
+	/* Initialise group */
+	grp->name = np->name;
+
+	/*
+	 * the binding format is fsl,pins = <PIN_FUNC_ID CONFIG ...>,
+	 * do sanity check and calculate pins number
+	 */
+	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");
+		return -EINVAL;
+	}
+
+	grp->npins = size / 2;
+	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->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;
+		}
+		/* 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;
+	}
+
+#ifdef DEBUG
+	IMX_PMX_DUMP(info, grp->pins, grp->mux_mode, grp->configs, grp->npins);
+#endif
+	return 0;
+}
+
+static int __devinit imx_pinctrl_parse_functions(struct device_node *np,
+			struct imx_pinctrl_soc_info *info, u32 index)
+{
+	struct device_node *child;
+	struct imx_pmx_func *func;
+	struct imx_pin_group *grp;
+	int ret;
+	static u32 grp_index;
+	u32 i = 0;
+
+	dev_dbg(info->dev, "parse function(%d): %s\n", index, np->name);
+
+	func = &info->functions[index];
+
+	/* Initialise function */
+	func->name = np->name;
+	func->num_groups = of_get_child_count(np);
+	if (func->num_groups <= 0) {
+		dev_err(info->dev, "no groups defined\n");
+		return -EINVAL;
+	}
+	func->groups = devm_kzalloc(info->dev,
+			func->num_groups * sizeof(char *), GFP_KERNEL);
+
+	for_each_child_of_node(np, child) {
+		func->groups[i] = child->name;
+		grp = &info->groups[grp_index++];
+		ret = imx_pinctrl_parse_groups(child, grp, info, i++);
+		if (ret)
+			return ret;
+	}
+
+	return 0;
+}
+
+static int __devinit imx_pinctrl_probe_dt(struct platform_device *pdev,
+				struct imx_pinctrl_soc_info *info)
+{
+	struct device_node *np = pdev->dev.of_node;
+	struct device_node *child;
+	int ret;
+	u32 nfuncs = 0;
+	u32 i = 0;
+
+	if (!np)
+		return -ENODEV;
+
+	nfuncs = of_get_child_count(np);
+	if (nfuncs <= 0) {
+		dev_err(&pdev->dev, "no functions defined\n");
+		return -EINVAL;
+	}
+
+	info->nfunctions = nfuncs;
+	info->functions = devm_kzalloc(&pdev->dev, nfuncs * sizeof(struct imx_pmx_func),
+					GFP_KERNEL);
+	if (!info->functions)
+		return -ENOMEM;
+
+	info->ngroups = 0;
+	for_each_child_of_node(np, child)
+		info->ngroups += of_get_child_count(child);
+	info->groups = devm_kzalloc(&pdev->dev, info->ngroups * sizeof(struct imx_pin_group),
+					GFP_KERNEL);
+	if (!info->groups)
+		return -ENOMEM;
+
+	for_each_child_of_node(np, child) {
+		ret = imx_pinctrl_parse_functions(child, info, i++);
+		if (ret) {
+			dev_err(&pdev->dev, "failed to parse function\n");
+			return ret;
+		}
+	}
+
+	return 0;
+}
+
+int __devinit imx_pinctrl_probe(struct platform_device *pdev,
+				struct imx_pinctrl_soc_info *info)
+{
+	struct imx_pinctrl *ipctl;
+	struct resource *res;
+	int ret;
+
+	if (!info || !info->pins || !info->npins
+		  || !info->pin_regs || !info->npin_regs) {
+		dev_err(&pdev->dev, "wrong pinctrl info\n");
+		return -EINVAL;
+	}
+	info->dev = &pdev->dev;
+
+	/* Create state holders etc for this driver */
+	ipctl = devm_kzalloc(&pdev->dev, sizeof(*ipctl), GFP_KERNEL);
+	if (!ipctl)
+		return -ENOMEM;
+
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	if (!res)
+		return -ENOENT;
+
+	ipctl->base = devm_request_and_ioremap(&pdev->dev, res);
+	if (!ipctl->base)
+		return -EBUSY;
+
+	imx_pinctrl_desc.name = dev_name(&pdev->dev);
+	imx_pinctrl_desc.pins = info->pins;
+	imx_pinctrl_desc.npins = info->npins;
+
+	ret = imx_pinctrl_probe_dt(pdev, info);
+	if (ret) {
+		dev_err(&pdev->dev, "fail to probe dt properties\n");
+		return ret;
+	}
+
+	ipctl->info = info;
+	ipctl->dev = info->dev;
+	platform_set_drvdata(pdev, ipctl);
+	ipctl->pctl = pinctrl_register(&imx_pinctrl_desc, &pdev->dev, ipctl);
+	if (!ipctl->pctl) {
+		dev_err(&pdev->dev, "could not register IMX pinctrl driver\n");
+		return -EINVAL;
+	}
+
+	dev_info(&pdev->dev, "initialized IMX pinctrl driver\n");
+
+	return 0;
+}
+
+int __devexit imx_pinctrl_remove(struct platform_device *pdev)
+{
+	struct imx_pinctrl *ipctl = platform_get_drvdata(pdev);
+
+	pinctrl_unregister(ipctl->pctl);
+
+	return 0;
+}
diff --git a/drivers/pinctrl/pinctrl-imx.h b/drivers/pinctrl/pinctrl-imx.h
new file mode 100644
index 00000000000..9b65e7828f1
--- /dev/null
+++ b/drivers/pinctrl/pinctrl-imx.h
@@ -0,0 +1,106 @@
+/*
+ * IMX pinmux core definitions
+ *
+ * Copyright (C) 2012 Freescale Semiconductor, Inc.
+ * Copyright (C) 2012 Linaro Ltd.
+ *
+ * Author: Dong Aisheng <dong.aisheng@linaro.org>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ */
+
+#ifndef __DRIVERS_PINCTRL_IMX_H
+#define __DRIVERS_PINCTRL_IMX_H
+
+struct platform_device;
+
+/**
+ * struct imx_pin_group - describes an IMX pin group
+ * @name: the name of this specific pin group
+ * @pins: an array of discrete physical pins used in this group, taken
+ *	from the driver-local pin enumeration space
+ * @npins: the number of pins in this group array, i.e. the number of
+ *	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.
+ * @configs: the config for each pin in this group. The size of this
+ *	array is the same as pins.
+ */
+struct imx_pin_group {
+	const char *name;
+	unsigned int *pins;
+	unsigned npins;
+	unsigned int *mux_mode;
+	unsigned long *configs;
+};
+
+/**
+ * struct imx_pmx_func - describes IMX pinmux functions
+ * @name: the name of this specific function
+ * @groups: corresponding pin groups
+ * @num_groups: the number of groups
+ */
+struct imx_pmx_func {
+	const char *name;
+	const char **groups;
+	unsigned num_groups;
+};
+
+/**
+ * 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_group *groups;
+	unsigned int ngroups;
+	struct imx_pmx_func *functions;
+	unsigned int nfunctions;
+};
+
+#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)
+#define IMX_MUX_MASK	0x7
+#define IOMUXC_CONFIG_SION	(0x1 << 4)
+
+int imx_pinctrl_probe(struct platform_device *pdev,
+			struct imx_pinctrl_soc_info *info);
+int imx_pinctrl_remove(struct platform_device *pdev);
+#endif /* __DRIVERS_PINCTRL_IMX_H */
diff --git a/drivers/pinctrl/pinctrl-imx23.c b/drivers/pinctrl/pinctrl-imx23.c
new file mode 100644
index 00000000000..75d3eff9429
--- /dev/null
+++ b/drivers/pinctrl/pinctrl-imx23.c
@@ -0,0 +1,305 @@
+/*
+ * Copyright 2012 Freescale Semiconductor, Inc.
+ *
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/gpl-license.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+
+#include <linux/init.h>
+#include <linux/module.h>
+#include <linux/of_device.h>
+#include <linux/pinctrl/pinctrl.h>
+#include "pinctrl-mxs.h"
+
+enum imx23_pin_enum {
+	GPMI_D00	= PINID(0, 0),
+	GPMI_D01	= PINID(0, 1),
+	GPMI_D02	= PINID(0, 2),
+	GPMI_D03	= PINID(0, 3),
+	GPMI_D04	= PINID(0, 4),
+	GPMI_D05	= PINID(0, 5),
+	GPMI_D06	= PINID(0, 6),
+	GPMI_D07	= PINID(0, 7),
+	GPMI_D08	= PINID(0, 8),
+	GPMI_D09	= PINID(0, 9),
+	GPMI_D10	= PINID(0, 10),
+	GPMI_D11	= PINID(0, 11),
+	GPMI_D12	= PINID(0, 12),
+	GPMI_D13	= PINID(0, 13),
+	GPMI_D14	= PINID(0, 14),
+	GPMI_D15	= PINID(0, 15),
+	GPMI_CLE	= PINID(0, 16),
+	GPMI_ALE	= PINID(0, 17),
+	GPMI_CE2N	= PINID(0, 18),
+	GPMI_RDY0	= PINID(0, 19),
+	GPMI_RDY1	= PINID(0, 20),
+	GPMI_RDY2	= PINID(0, 21),
+	GPMI_RDY3	= PINID(0, 22),
+	GPMI_WPN	= PINID(0, 23),
+	GPMI_WRN	= PINID(0, 24),
+	GPMI_RDN	= PINID(0, 25),
+	AUART1_CTS	= PINID(0, 26),
+	AUART1_RTS	= PINID(0, 27),
+	AUART1_RX	= PINID(0, 28),
+	AUART1_TX	= PINID(0, 29),
+	I2C_SCL		= PINID(0, 30),
+	I2C_SDA		= PINID(0, 31),
+	LCD_D00		= PINID(1, 0),
+	LCD_D01		= PINID(1, 1),
+	LCD_D02		= PINID(1, 2),
+	LCD_D03		= PINID(1, 3),
+	LCD_D04		= PINID(1, 4),
+	LCD_D05		= PINID(1, 5),
+	LCD_D06		= PINID(1, 6),
+	LCD_D07		= PINID(1, 7),
+	LCD_D08		= PINID(1, 8),
+	LCD_D09		= PINID(1, 9),
+	LCD_D10		= PINID(1, 10),
+	LCD_D11		= PINID(1, 11),
+	LCD_D12		= PINID(1, 12),
+	LCD_D13		= PINID(1, 13),
+	LCD_D14		= PINID(1, 14),
+	LCD_D15		= PINID(1, 15),
+	LCD_D16		= PINID(1, 16),
+	LCD_D17		= PINID(1, 17),
+	LCD_RESET	= PINID(1, 18),
+	LCD_RS		= PINID(1, 19),
+	LCD_WR		= PINID(1, 20),
+	LCD_CS		= PINID(1, 21),
+	LCD_DOTCK	= PINID(1, 22),
+	LCD_ENABLE	= PINID(1, 23),
+	LCD_HSYNC	= PINID(1, 24),
+	LCD_VSYNC	= PINID(1, 25),
+	PWM0		= PINID(1, 26),
+	PWM1		= PINID(1, 27),
+	PWM2		= PINID(1, 28),
+	PWM3		= PINID(1, 29),
+	PWM4		= PINID(1, 30),
+	SSP1_CMD	= PINID(2, 0),
+	SSP1_DETECT	= PINID(2, 1),
+	SSP1_DATA0	= PINID(2, 2),
+	SSP1_DATA1	= PINID(2, 3),
+	SSP1_DATA2	= PINID(2, 4),
+	SSP1_DATA3	= PINID(2, 5),
+	SSP1_SCK	= PINID(2, 6),
+	ROTARYA		= PINID(2, 7),
+	ROTARYB		= PINID(2, 8),
+	EMI_A00		= PINID(2, 9),
+	EMI_A01		= PINID(2, 10),
+	EMI_A02		= PINID(2, 11),
+	EMI_A03		= PINID(2, 12),
+	EMI_A04		= PINID(2, 13),
+	EMI_A05		= PINID(2, 14),
+	EMI_A06		= PINID(2, 15),
+	EMI_A07		= PINID(2, 16),
+	EMI_A08		= PINID(2, 17),
+	EMI_A09		= PINID(2, 18),
+	EMI_A10		= PINID(2, 19),
+	EMI_A11		= PINID(2, 20),
+	EMI_A12		= PINID(2, 21),
+	EMI_BA0		= PINID(2, 22),
+	EMI_BA1		= PINID(2, 23),
+	EMI_CASN	= PINID(2, 24),
+	EMI_CE0N	= PINID(2, 25),
+	EMI_CE1N	= PINID(2, 26),
+	GPMI_CE1N	= PINID(2, 27),
+	GPMI_CE0N	= PINID(2, 28),
+	EMI_CKE		= PINID(2, 29),
+	EMI_RASN	= PINID(2, 30),
+	EMI_WEN		= PINID(2, 31),
+	EMI_D00		= PINID(3, 0),
+	EMI_D01		= PINID(3, 1),
+	EMI_D02		= PINID(3, 2),
+	EMI_D03		= PINID(3, 3),
+	EMI_D04		= PINID(3, 4),
+	EMI_D05		= PINID(3, 5),
+	EMI_D06		= PINID(3, 6),
+	EMI_D07		= PINID(3, 7),
+	EMI_D08		= PINID(3, 8),
+	EMI_D09		= PINID(3, 9),
+	EMI_D10		= PINID(3, 10),
+	EMI_D11		= PINID(3, 11),
+	EMI_D12		= PINID(3, 12),
+	EMI_D13		= PINID(3, 13),
+	EMI_D14		= PINID(3, 14),
+	EMI_D15		= PINID(3, 15),
+	EMI_DQM0	= PINID(3, 16),
+	EMI_DQM1	= PINID(3, 17),
+	EMI_DQS0	= PINID(3, 18),
+	EMI_DQS1	= PINID(3, 19),
+	EMI_CLK		= PINID(3, 20),
+	EMI_CLKN	= PINID(3, 21),
+};
+
+static const struct pinctrl_pin_desc imx23_pins[] = {
+	MXS_PINCTRL_PIN(GPMI_D00),
+	MXS_PINCTRL_PIN(GPMI_D01),
+	MXS_PINCTRL_PIN(GPMI_D02),
+	MXS_PINCTRL_PIN(GPMI_D03),
+	MXS_PINCTRL_PIN(GPMI_D04),
+	MXS_PINCTRL_PIN(GPMI_D05),
+	MXS_PINCTRL_PIN(GPMI_D06),
+	MXS_PINCTRL_PIN(GPMI_D07),
+	MXS_PINCTRL_PIN(GPMI_D08),
+	MXS_PINCTRL_PIN(GPMI_D09),
+	MXS_PINCTRL_PIN(GPMI_D10),
+	MXS_PINCTRL_PIN(GPMI_D11),
+	MXS_PINCTRL_PIN(GPMI_D12),
+	MXS_PINCTRL_PIN(GPMI_D13),
+	MXS_PINCTRL_PIN(GPMI_D14),
+	MXS_PINCTRL_PIN(GPMI_D15),
+	MXS_PINCTRL_PIN(GPMI_CLE),
+	MXS_PINCTRL_PIN(GPMI_ALE),
+	MXS_PINCTRL_PIN(GPMI_CE2N),
+	MXS_PINCTRL_PIN(GPMI_RDY0),
+	MXS_PINCTRL_PIN(GPMI_RDY1),
+	MXS_PINCTRL_PIN(GPMI_RDY2),
+	MXS_PINCTRL_PIN(GPMI_RDY3),
+	MXS_PINCTRL_PIN(GPMI_WPN),
+	MXS_PINCTRL_PIN(GPMI_WRN),
+	MXS_PINCTRL_PIN(GPMI_RDN),
+	MXS_PINCTRL_PIN(AUART1_CTS),
+	MXS_PINCTRL_PIN(AUART1_RTS),
+	MXS_PINCTRL_PIN(AUART1_RX),
+	MXS_PINCTRL_PIN(AUART1_TX),
+	MXS_PINCTRL_PIN(I2C_SCL),
+	MXS_PINCTRL_PIN(I2C_SDA),
+	MXS_PINCTRL_PIN(LCD_D00),
+	MXS_PINCTRL_PIN(LCD_D01),
+	MXS_PINCTRL_PIN(LCD_D02),
+	MXS_PINCTRL_PIN(LCD_D03),
+	MXS_PINCTRL_PIN(LCD_D04),
+	MXS_PINCTRL_PIN(LCD_D05),
+	MXS_PINCTRL_PIN(LCD_D06),
+	MXS_PINCTRL_PIN(LCD_D07),
+	MXS_PINCTRL_PIN(LCD_D08),
+	MXS_PINCTRL_PIN(LCD_D09),
+	MXS_PINCTRL_PIN(LCD_D10),
+	MXS_PINCTRL_PIN(LCD_D11),
+	MXS_PINCTRL_PIN(LCD_D12),
+	MXS_PINCTRL_PIN(LCD_D13),
+	MXS_PINCTRL_PIN(LCD_D14),
+	MXS_PINCTRL_PIN(LCD_D15),
+	MXS_PINCTRL_PIN(LCD_D16),
+	MXS_PINCTRL_PIN(LCD_D17),
+	MXS_PINCTRL_PIN(LCD_RESET),
+	MXS_PINCTRL_PIN(LCD_RS),
+	MXS_PINCTRL_PIN(LCD_WR),
+	MXS_PINCTRL_PIN(LCD_CS),
+	MXS_PINCTRL_PIN(LCD_DOTCK),
+	MXS_PINCTRL_PIN(LCD_ENABLE),
+	MXS_PINCTRL_PIN(LCD_HSYNC),
+	MXS_PINCTRL_PIN(LCD_VSYNC),
+	MXS_PINCTRL_PIN(PWM0),
+	MXS_PINCTRL_PIN(PWM1),
+	MXS_PINCTRL_PIN(PWM2),
+	MXS_PINCTRL_PIN(PWM3),
+	MXS_PINCTRL_PIN(PWM4),
+	MXS_PINCTRL_PIN(SSP1_CMD),
+	MXS_PINCTRL_PIN(SSP1_DETECT),
+	MXS_PINCTRL_PIN(SSP1_DATA0),
+	MXS_PINCTRL_PIN(SSP1_DATA1),
+	MXS_PINCTRL_PIN(SSP1_DATA2),
+	MXS_PINCTRL_PIN(SSP1_DATA3),
+	MXS_PINCTRL_PIN(SSP1_SCK),
+	MXS_PINCTRL_PIN(ROTARYA),
+	MXS_PINCTRL_PIN(ROTARYB),
+	MXS_PINCTRL_PIN(EMI_A00),
+	MXS_PINCTRL_PIN(EMI_A01),
+	MXS_PINCTRL_PIN(EMI_A02),
+	MXS_PINCTRL_PIN(EMI_A03),
+	MXS_PINCTRL_PIN(EMI_A04),
+	MXS_PINCTRL_PIN(EMI_A05),
+	MXS_PINCTRL_PIN(EMI_A06),
+	MXS_PINCTRL_PIN(EMI_A07),
+	MXS_PINCTRL_PIN(EMI_A08),
+	MXS_PINCTRL_PIN(EMI_A09),
+	MXS_PINCTRL_PIN(EMI_A10),
+	MXS_PINCTRL_PIN(EMI_A11),
+	MXS_PINCTRL_PIN(EMI_A12),
+	MXS_PINCTRL_PIN(EMI_BA0),
+	MXS_PINCTRL_PIN(EMI_BA1),
+	MXS_PINCTRL_PIN(EMI_CASN),
+	MXS_PINCTRL_PIN(EMI_CE0N),
+	MXS_PINCTRL_PIN(EMI_CE1N),
+	MXS_PINCTRL_PIN(GPMI_CE1N),
+	MXS_PINCTRL_PIN(GPMI_CE0N),
+	MXS_PINCTRL_PIN(EMI_CKE),
+	MXS_PINCTRL_PIN(EMI_RASN),
+	MXS_PINCTRL_PIN(EMI_WEN),
+	MXS_PINCTRL_PIN(EMI_D00),
+	MXS_PINCTRL_PIN(EMI_D01),
+	MXS_PINCTRL_PIN(EMI_D02),
+	MXS_PINCTRL_PIN(EMI_D03),
+	MXS_PINCTRL_PIN(EMI_D04),
+	MXS_PINCTRL_PIN(EMI_D05),
+	MXS_PINCTRL_PIN(EMI_D06),
+	MXS_PINCTRL_PIN(EMI_D07),
+	MXS_PINCTRL_PIN(EMI_D08),
+	MXS_PINCTRL_PIN(EMI_D09),
+	MXS_PINCTRL_PIN(EMI_D10),
+	MXS_PINCTRL_PIN(EMI_D11),
+	MXS_PINCTRL_PIN(EMI_D12),
+	MXS_PINCTRL_PIN(EMI_D13),
+	MXS_PINCTRL_PIN(EMI_D14),
+	MXS_PINCTRL_PIN(EMI_D15),
+	MXS_PINCTRL_PIN(EMI_DQM0),
+	MXS_PINCTRL_PIN(EMI_DQM1),
+	MXS_PINCTRL_PIN(EMI_DQS0),
+	MXS_PINCTRL_PIN(EMI_DQS1),
+	MXS_PINCTRL_PIN(EMI_CLK),
+	MXS_PINCTRL_PIN(EMI_CLKN),
+};
+
+static struct mxs_regs imx23_regs = {
+	.muxsel = 0x100,
+	.drive = 0x200,
+	.pull = 0x400,
+};
+
+static struct mxs_pinctrl_soc_data imx23_pinctrl_data = {
+	.regs = &imx23_regs,
+	.pins = imx23_pins,
+	.npins = ARRAY_SIZE(imx23_pins),
+};
+
+static int __devinit imx23_pinctrl_probe(struct platform_device *pdev)
+{
+	return mxs_pinctrl_probe(pdev, &imx23_pinctrl_data);
+}
+
+static struct of_device_id imx23_pinctrl_of_match[] __devinitdata = {
+	{ .compatible = "fsl,imx23-pinctrl", },
+	{ /* sentinel */ }
+};
+MODULE_DEVICE_TABLE(of, imx23_pinctrl_of_match);
+
+static struct platform_driver imx23_pinctrl_driver = {
+	.driver = {
+		.name = "imx23-pinctrl",
+		.owner = THIS_MODULE,
+		.of_match_table = imx23_pinctrl_of_match,
+	},
+	.probe = imx23_pinctrl_probe,
+	.remove = __devexit_p(mxs_pinctrl_remove),
+};
+
+static int __init imx23_pinctrl_init(void)
+{
+	return platform_driver_register(&imx23_pinctrl_driver);
+}
+arch_initcall(imx23_pinctrl_init);
+
+static void __exit imx23_pinctrl_exit(void)
+{
+	platform_driver_unregister(&imx23_pinctrl_driver);
+}
+module_exit(imx23_pinctrl_exit);
+
+MODULE_AUTHOR("Shawn Guo <shawn.guo@linaro.org>");
+MODULE_DESCRIPTION("Freescale i.MX23 pinctrl driver");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/pinctrl/pinctrl-imx28.c b/drivers/pinctrl/pinctrl-imx28.c
new file mode 100644
index 00000000000..b973026811a
--- /dev/null
+++ b/drivers/pinctrl/pinctrl-imx28.c
@@ -0,0 +1,421 @@
+/*
+ * Copyright 2012 Freescale Semiconductor, Inc.
+ *
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/gpl-license.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+
+#include <linux/init.h>
+#include <linux/module.h>
+#include <linux/of_device.h>
+#include <linux/pinctrl/pinctrl.h>
+#include "pinctrl-mxs.h"
+
+enum imx28_pin_enum {
+	GPMI_D00	= PINID(0, 0),
+	GPMI_D01	= PINID(0, 1),
+	GPMI_D02	= PINID(0, 2),
+	GPMI_D03	= PINID(0, 3),
+	GPMI_D04	= PINID(0, 4),
+	GPMI_D05	= PINID(0, 5),
+	GPMI_D06	= PINID(0, 6),
+	GPMI_D07	= PINID(0, 7),
+	GPMI_CE0N	= PINID(0, 16),
+	GPMI_CE1N	= PINID(0, 17),
+	GPMI_CE2N	= PINID(0, 18),
+	GPMI_CE3N	= PINID(0, 19),
+	GPMI_RDY0	= PINID(0, 20),
+	GPMI_RDY1	= PINID(0, 21),
+	GPMI_RDY2	= PINID(0, 22),
+	GPMI_RDY3	= PINID(0, 23),
+	GPMI_RDN	= PINID(0, 24),
+	GPMI_WRN	= PINID(0, 25),
+	GPMI_ALE	= PINID(0, 26),
+	GPMI_CLE	= PINID(0, 27),
+	GPMI_RESETN	= PINID(0, 28),
+	LCD_D00		= PINID(1, 0),
+	LCD_D01		= PINID(1, 1),
+	LCD_D02		= PINID(1, 2),
+	LCD_D03		= PINID(1, 3),
+	LCD_D04		= PINID(1, 4),
+	LCD_D05		= PINID(1, 5),
+	LCD_D06		= PINID(1, 6),
+	LCD_D07		= PINID(1, 7),
+	LCD_D08		= PINID(1, 8),
+	LCD_D09		= PINID(1, 9),
+	LCD_D10		= PINID(1, 10),
+	LCD_D11		= PINID(1, 11),
+	LCD_D12		= PINID(1, 12),
+	LCD_D13		= PINID(1, 13),
+	LCD_D14		= PINID(1, 14),
+	LCD_D15		= PINID(1, 15),
+	LCD_D16		= PINID(1, 16),
+	LCD_D17		= PINID(1, 17),
+	LCD_D18		= PINID(1, 18),
+	LCD_D19		= PINID(1, 19),
+	LCD_D20		= PINID(1, 20),
+	LCD_D21		= PINID(1, 21),
+	LCD_D22		= PINID(1, 22),
+	LCD_D23		= PINID(1, 23),
+	LCD_RD_E	= PINID(1, 24),
+	LCD_WR_RWN	= PINID(1, 25),
+	LCD_RS		= PINID(1, 26),
+	LCD_CS		= PINID(1, 27),
+	LCD_VSYNC	= PINID(1, 28),
+	LCD_HSYNC	= PINID(1, 29),
+	LCD_DOTCLK	= PINID(1, 30),
+	LCD_ENABLE	= PINID(1, 31),
+	SSP0_DATA0	= PINID(2, 0),
+	SSP0_DATA1	= PINID(2, 1),
+	SSP0_DATA2	= PINID(2, 2),
+	SSP0_DATA3	= PINID(2, 3),
+	SSP0_DATA4	= PINID(2, 4),
+	SSP0_DATA5	= PINID(2, 5),
+	SSP0_DATA6	= PINID(2, 6),
+	SSP0_DATA7	= PINID(2, 7),
+	SSP0_CMD	= PINID(2, 8),
+	SSP0_DETECT	= PINID(2, 9),
+	SSP0_SCK	= PINID(2, 10),
+	SSP1_SCK	= PINID(2, 12),
+	SSP1_CMD	= PINID(2, 13),
+	SSP1_DATA0	= PINID(2, 14),
+	SSP1_DATA3	= PINID(2, 15),
+	SSP2_SCK	= PINID(2, 16),
+	SSP2_MOSI	= PINID(2, 17),
+	SSP2_MISO	= PINID(2, 18),
+	SSP2_SS0	= PINID(2, 19),
+	SSP2_SS1	= PINID(2, 20),
+	SSP2_SS2	= PINID(2, 21),
+	SSP3_SCK	= PINID(2, 24),
+	SSP3_MOSI	= PINID(2, 25),
+	SSP3_MISO	= PINID(2, 26),
+	SSP3_SS0	= PINID(2, 27),
+	AUART0_RX	= PINID(3, 0),
+	AUART0_TX	= PINID(3, 1),
+	AUART0_CTS	= PINID(3, 2),
+	AUART0_RTS	= PINID(3, 3),
+	AUART1_RX	= PINID(3, 4),
+	AUART1_TX	= PINID(3, 5),
+	AUART1_CTS	= PINID(3, 6),
+	AUART1_RTS	= PINID(3, 7),
+	AUART2_RX	= PINID(3, 8),
+	AUART2_TX	= PINID(3, 9),
+	AUART2_CTS	= PINID(3, 10),
+	AUART2_RTS	= PINID(3, 11),
+	AUART3_RX	= PINID(3, 12),
+	AUART3_TX	= PINID(3, 13),
+	AUART3_CTS	= PINID(3, 14),
+	AUART3_RTS	= PINID(3, 15),
+	PWM0		= PINID(3, 16),
+	PWM1		= PINID(3, 17),
+	PWM2		= PINID(3, 18),
+	SAIF0_MCLK	= PINID(3, 20),
+	SAIF0_LRCLK	= PINID(3, 21),
+	SAIF0_BITCLK	= PINID(3, 22),
+	SAIF0_SDATA0	= PINID(3, 23),
+	I2C0_SCL	= PINID(3, 24),
+	I2C0_SDA	= PINID(3, 25),
+	SAIF1_SDATA0	= PINID(3, 26),
+	SPDIF		= PINID(3, 27),
+	PWM3		= PINID(3, 28),
+	PWM4		= PINID(3, 29),
+	LCD_RESET	= PINID(3, 30),
+	ENET0_MDC	= PINID(4, 0),
+	ENET0_MDIO	= PINID(4, 1),
+	ENET0_RX_EN	= PINID(4, 2),
+	ENET0_RXD0	= PINID(4, 3),
+	ENET0_RXD1	= PINID(4, 4),
+	ENET0_TX_CLK	= PINID(4, 5),
+	ENET0_TX_EN	= PINID(4, 6),
+	ENET0_TXD0	= PINID(4, 7),
+	ENET0_TXD1	= PINID(4, 8),
+	ENET0_RXD2	= PINID(4, 9),
+	ENET0_RXD3	= PINID(4, 10),
+	ENET0_TXD2	= PINID(4, 11),
+	ENET0_TXD3	= PINID(4, 12),
+	ENET0_RX_CLK	= PINID(4, 13),
+	ENET0_COL	= PINID(4, 14),
+	ENET0_CRS	= PINID(4, 15),
+	ENET_CLK	= PINID(4, 16),
+	JTAG_RTCK	= PINID(4, 20),
+	EMI_D00		= PINID(5, 0),
+	EMI_D01		= PINID(5, 1),
+	EMI_D02		= PINID(5, 2),
+	EMI_D03		= PINID(5, 3),
+	EMI_D04		= PINID(5, 4),
+	EMI_D05		= PINID(5, 5),
+	EMI_D06		= PINID(5, 6),
+	EMI_D07		= PINID(5, 7),
+	EMI_D08		= PINID(5, 8),
+	EMI_D09		= PINID(5, 9),
+	EMI_D10		= PINID(5, 10),
+	EMI_D11		= PINID(5, 11),
+	EMI_D12		= PINID(5, 12),
+	EMI_D13		= PINID(5, 13),
+	EMI_D14		= PINID(5, 14),
+	EMI_D15		= PINID(5, 15),
+	EMI_ODT0	= PINID(5, 16),
+	EMI_DQM0	= PINID(5, 17),
+	EMI_ODT1	= PINID(5, 18),
+	EMI_DQM1	= PINID(5, 19),
+	EMI_DDR_OPEN_FB	= PINID(5, 20),
+	EMI_CLK		= PINID(5, 21),
+	EMI_DQS0	= PINID(5, 22),
+	EMI_DQS1	= PINID(5, 23),
+	EMI_DDR_OPEN	= PINID(5, 26),
+	EMI_A00		= PINID(6, 0),
+	EMI_A01		= PINID(6, 1),
+	EMI_A02		= PINID(6, 2),
+	EMI_A03		= PINID(6, 3),
+	EMI_A04		= PINID(6, 4),
+	EMI_A05		= PINID(6, 5),
+	EMI_A06		= PINID(6, 6),
+	EMI_A07		= PINID(6, 7),
+	EMI_A08		= PINID(6, 8),
+	EMI_A09		= PINID(6, 9),
+	EMI_A10		= PINID(6, 10),
+	EMI_A11		= PINID(6, 11),
+	EMI_A12		= PINID(6, 12),
+	EMI_A13		= PINID(6, 13),
+	EMI_A14		= PINID(6, 14),
+	EMI_BA0		= PINID(6, 16),
+	EMI_BA1		= PINID(6, 17),
+	EMI_BA2		= PINID(6, 18),
+	EMI_CASN	= PINID(6, 19),
+	EMI_RASN	= PINID(6, 20),
+	EMI_WEN		= PINID(6, 21),
+	EMI_CE0N	= PINID(6, 22),
+	EMI_CE1N	= PINID(6, 23),
+	EMI_CKE		= PINID(6, 24),
+};
+
+static const struct pinctrl_pin_desc imx28_pins[] = {
+	MXS_PINCTRL_PIN(GPMI_D00),
+	MXS_PINCTRL_PIN(GPMI_D01),
+	MXS_PINCTRL_PIN(GPMI_D02),
+	MXS_PINCTRL_PIN(GPMI_D03),
+	MXS_PINCTRL_PIN(GPMI_D04),
+	MXS_PINCTRL_PIN(GPMI_D05),
+	MXS_PINCTRL_PIN(GPMI_D06),
+	MXS_PINCTRL_PIN(GPMI_D07),
+	MXS_PINCTRL_PIN(GPMI_CE0N),
+	MXS_PINCTRL_PIN(GPMI_CE1N),
+	MXS_PINCTRL_PIN(GPMI_CE2N),
+	MXS_PINCTRL_PIN(GPMI_CE3N),
+	MXS_PINCTRL_PIN(GPMI_RDY0),
+	MXS_PINCTRL_PIN(GPMI_RDY1),
+	MXS_PINCTRL_PIN(GPMI_RDY2),
+	MXS_PINCTRL_PIN(GPMI_RDY3),
+	MXS_PINCTRL_PIN(GPMI_RDN),
+	MXS_PINCTRL_PIN(GPMI_WRN),
+	MXS_PINCTRL_PIN(GPMI_ALE),
+	MXS_PINCTRL_PIN(GPMI_CLE),
+	MXS_PINCTRL_PIN(GPMI_RESETN),
+	MXS_PINCTRL_PIN(LCD_D00),
+	MXS_PINCTRL_PIN(LCD_D01),
+	MXS_PINCTRL_PIN(LCD_D02),
+	MXS_PINCTRL_PIN(LCD_D03),
+	MXS_PINCTRL_PIN(LCD_D04),
+	MXS_PINCTRL_PIN(LCD_D05),
+	MXS_PINCTRL_PIN(LCD_D06),
+	MXS_PINCTRL_PIN(LCD_D07),
+	MXS_PINCTRL_PIN(LCD_D08),
+	MXS_PINCTRL_PIN(LCD_D09),
+	MXS_PINCTRL_PIN(LCD_D10),
+	MXS_PINCTRL_PIN(LCD_D11),
+	MXS_PINCTRL_PIN(LCD_D12),
+	MXS_PINCTRL_PIN(LCD_D13),
+	MXS_PINCTRL_PIN(LCD_D14),
+	MXS_PINCTRL_PIN(LCD_D15),
+	MXS_PINCTRL_PIN(LCD_D16),
+	MXS_PINCTRL_PIN(LCD_D17),
+	MXS_PINCTRL_PIN(LCD_D18),
+	MXS_PINCTRL_PIN(LCD_D19),
+	MXS_PINCTRL_PIN(LCD_D20),
+	MXS_PINCTRL_PIN(LCD_D21),
+	MXS_PINCTRL_PIN(LCD_D22),
+	MXS_PINCTRL_PIN(LCD_D23),
+	MXS_PINCTRL_PIN(LCD_RD_E),
+	MXS_PINCTRL_PIN(LCD_WR_RWN),
+	MXS_PINCTRL_PIN(LCD_RS),
+	MXS_PINCTRL_PIN(LCD_CS),
+	MXS_PINCTRL_PIN(LCD_VSYNC),
+	MXS_PINCTRL_PIN(LCD_HSYNC),
+	MXS_PINCTRL_PIN(LCD_DOTCLK),
+	MXS_PINCTRL_PIN(LCD_ENABLE),
+	MXS_PINCTRL_PIN(SSP0_DATA0),
+	MXS_PINCTRL_PIN(SSP0_DATA1),
+	MXS_PINCTRL_PIN(SSP0_DATA2),
+	MXS_PINCTRL_PIN(SSP0_DATA3),
+	MXS_PINCTRL_PIN(SSP0_DATA4),
+	MXS_PINCTRL_PIN(SSP0_DATA5),
+	MXS_PINCTRL_PIN(SSP0_DATA6),
+	MXS_PINCTRL_PIN(SSP0_DATA7),
+	MXS_PINCTRL_PIN(SSP0_CMD),
+	MXS_PINCTRL_PIN(SSP0_DETECT),
+	MXS_PINCTRL_PIN(SSP0_SCK),
+	MXS_PINCTRL_PIN(SSP1_SCK),
+	MXS_PINCTRL_PIN(SSP1_CMD),
+	MXS_PINCTRL_PIN(SSP1_DATA0),
+	MXS_PINCTRL_PIN(SSP1_DATA3),
+	MXS_PINCTRL_PIN(SSP2_SCK),
+	MXS_PINCTRL_PIN(SSP2_MOSI),
+	MXS_PINCTRL_PIN(SSP2_MISO),
+	MXS_PINCTRL_PIN(SSP2_SS0),
+	MXS_PINCTRL_PIN(SSP2_SS1),
+	MXS_PINCTRL_PIN(SSP2_SS2),
+	MXS_PINCTRL_PIN(SSP3_SCK),
+	MXS_PINCTRL_PIN(SSP3_MOSI),
+	MXS_PINCTRL_PIN(SSP3_MISO),
+	MXS_PINCTRL_PIN(SSP3_SS0),
+	MXS_PINCTRL_PIN(AUART0_RX),
+	MXS_PINCTRL_PIN(AUART0_TX),
+	MXS_PINCTRL_PIN(AUART0_CTS),
+	MXS_PINCTRL_PIN(AUART0_RTS),
+	MXS_PINCTRL_PIN(AUART1_RX),
+	MXS_PINCTRL_PIN(AUART1_TX),
+	MXS_PINCTRL_PIN(AUART1_CTS),
+	MXS_PINCTRL_PIN(AUART1_RTS),
+	MXS_PINCTRL_PIN(AUART2_RX),
+	MXS_PINCTRL_PIN(AUART2_TX),
+	MXS_PINCTRL_PIN(AUART2_CTS),
+	MXS_PINCTRL_PIN(AUART2_RTS),
+	MXS_PINCTRL_PIN(AUART3_RX),
+	MXS_PINCTRL_PIN(AUART3_TX),
+	MXS_PINCTRL_PIN(AUART3_CTS),
+	MXS_PINCTRL_PIN(AUART3_RTS),
+	MXS_PINCTRL_PIN(PWM0),
+	MXS_PINCTRL_PIN(PWM1),
+	MXS_PINCTRL_PIN(PWM2),
+	MXS_PINCTRL_PIN(SAIF0_MCLK),
+	MXS_PINCTRL_PIN(SAIF0_LRCLK),
+	MXS_PINCTRL_PIN(SAIF0_BITCLK),
+	MXS_PINCTRL_PIN(SAIF0_SDATA0),
+	MXS_PINCTRL_PIN(I2C0_SCL),
+	MXS_PINCTRL_PIN(I2C0_SDA),
+	MXS_PINCTRL_PIN(SAIF1_SDATA0),
+	MXS_PINCTRL_PIN(SPDIF),
+	MXS_PINCTRL_PIN(PWM3),
+	MXS_PINCTRL_PIN(PWM4),
+	MXS_PINCTRL_PIN(LCD_RESET),
+	MXS_PINCTRL_PIN(ENET0_MDC),
+	MXS_PINCTRL_PIN(ENET0_MDIO),
+	MXS_PINCTRL_PIN(ENET0_RX_EN),
+	MXS_PINCTRL_PIN(ENET0_RXD0),
+	MXS_PINCTRL_PIN(ENET0_RXD1),
+	MXS_PINCTRL_PIN(ENET0_TX_CLK),
+	MXS_PINCTRL_PIN(ENET0_TX_EN),
+	MXS_PINCTRL_PIN(ENET0_TXD0),
+	MXS_PINCTRL_PIN(ENET0_TXD1),
+	MXS_PINCTRL_PIN(ENET0_RXD2),
+	MXS_PINCTRL_PIN(ENET0_RXD3),
+	MXS_PINCTRL_PIN(ENET0_TXD2),
+	MXS_PINCTRL_PIN(ENET0_TXD3),
+	MXS_PINCTRL_PIN(ENET0_RX_CLK),
+	MXS_PINCTRL_PIN(ENET0_COL),
+	MXS_PINCTRL_PIN(ENET0_CRS),
+	MXS_PINCTRL_PIN(ENET_CLK),
+	MXS_PINCTRL_PIN(JTAG_RTCK),
+	MXS_PINCTRL_PIN(EMI_D00),
+	MXS_PINCTRL_PIN(EMI_D01),
+	MXS_PINCTRL_PIN(EMI_D02),
+	MXS_PINCTRL_PIN(EMI_D03),
+	MXS_PINCTRL_PIN(EMI_D04),
+	MXS_PINCTRL_PIN(EMI_D05),
+	MXS_PINCTRL_PIN(EMI_D06),
+	MXS_PINCTRL_PIN(EMI_D07),
+	MXS_PINCTRL_PIN(EMI_D08),
+	MXS_PINCTRL_PIN(EMI_D09),
+	MXS_PINCTRL_PIN(EMI_D10),
+	MXS_PINCTRL_PIN(EMI_D11),
+	MXS_PINCTRL_PIN(EMI_D12),
+	MXS_PINCTRL_PIN(EMI_D13),
+	MXS_PINCTRL_PIN(EMI_D14),
+	MXS_PINCTRL_PIN(EMI_D15),
+	MXS_PINCTRL_PIN(EMI_ODT0),
+	MXS_PINCTRL_PIN(EMI_DQM0),
+	MXS_PINCTRL_PIN(EMI_ODT1),
+	MXS_PINCTRL_PIN(EMI_DQM1),
+	MXS_PINCTRL_PIN(EMI_DDR_OPEN_FB),
+	MXS_PINCTRL_PIN(EMI_CLK),
+	MXS_PINCTRL_PIN(EMI_DQS0),
+	MXS_PINCTRL_PIN(EMI_DQS1),
+	MXS_PINCTRL_PIN(EMI_DDR_OPEN),
+	MXS_PINCTRL_PIN(EMI_A00),
+	MXS_PINCTRL_PIN(EMI_A01),
+	MXS_PINCTRL_PIN(EMI_A02),
+	MXS_PINCTRL_PIN(EMI_A03),
+	MXS_PINCTRL_PIN(EMI_A04),
+	MXS_PINCTRL_PIN(EMI_A05),
+	MXS_PINCTRL_PIN(EMI_A06),
+	MXS_PINCTRL_PIN(EMI_A07),
+	MXS_PINCTRL_PIN(EMI_A08),
+	MXS_PINCTRL_PIN(EMI_A09),
+	MXS_PINCTRL_PIN(EMI_A10),
+	MXS_PINCTRL_PIN(EMI_A11),
+	MXS_PINCTRL_PIN(EMI_A12),
+	MXS_PINCTRL_PIN(EMI_A13),
+	MXS_PINCTRL_PIN(EMI_A14),
+	MXS_PINCTRL_PIN(EMI_BA0),
+	MXS_PINCTRL_PIN(EMI_BA1),
+	MXS_PINCTRL_PIN(EMI_BA2),
+	MXS_PINCTRL_PIN(EMI_CASN),
+	MXS_PINCTRL_PIN(EMI_RASN),
+	MXS_PINCTRL_PIN(EMI_WEN),
+	MXS_PINCTRL_PIN(EMI_CE0N),
+	MXS_PINCTRL_PIN(EMI_CE1N),
+	MXS_PINCTRL_PIN(EMI_CKE),
+};
+
+static struct mxs_regs imx28_regs = {
+	.muxsel = 0x100,
+	.drive = 0x300,
+	.pull = 0x600,
+};
+
+static struct mxs_pinctrl_soc_data imx28_pinctrl_data = {
+	.regs = &imx28_regs,
+	.pins = imx28_pins,
+	.npins = ARRAY_SIZE(imx28_pins),
+};
+
+static int __devinit imx28_pinctrl_probe(struct platform_device *pdev)
+{
+	return mxs_pinctrl_probe(pdev, &imx28_pinctrl_data);
+}
+
+static struct of_device_id imx28_pinctrl_of_match[] __devinitdata = {
+	{ .compatible = "fsl,imx28-pinctrl", },
+	{ /* sentinel */ }
+};
+MODULE_DEVICE_TABLE(of, imx28_pinctrl_of_match);
+
+static struct platform_driver imx28_pinctrl_driver = {
+	.driver = {
+		.name = "imx28-pinctrl",
+		.owner = THIS_MODULE,
+		.of_match_table = imx28_pinctrl_of_match,
+	},
+	.probe = imx28_pinctrl_probe,
+	.remove = __devexit_p(mxs_pinctrl_remove),
+};
+
+static int __init imx28_pinctrl_init(void)
+{
+	return platform_driver_register(&imx28_pinctrl_driver);
+}
+arch_initcall(imx28_pinctrl_init);
+
+static void __exit imx28_pinctrl_exit(void)
+{
+	platform_driver_unregister(&imx28_pinctrl_driver);
+}
+module_exit(imx28_pinctrl_exit);
+
+MODULE_AUTHOR("Shawn Guo <shawn.guo@linaro.org>");
+MODULE_DESCRIPTION("Freescale i.MX28 pinctrl driver");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/pinctrl/pinctrl-imx51.c b/drivers/pinctrl/pinctrl-imx51.c
new file mode 100644
index 00000000000..689b3c88dd2
--- /dev/null
+++ b/drivers/pinctrl/pinctrl-imx51.c
@@ -0,0 +1,1322 @@
+/*
+ * imx51 pinctrl driver based on imx pinmux core
+ *
+ * Copyright (C) 2012 Freescale Semiconductor, Inc.
+ * Copyright (C) 2012 Linaro, Inc.
+ *
+ * Author: Dong Aisheng <dong.aisheng@linaro.org>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ */
+
+#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 imx51_pads {
+	MX51_PAD_EIM_D16 = 1,
+	MX51_PAD_EIM_D17 = 2,
+	MX51_PAD_EIM_D18 = 3,
+	MX51_PAD_EIM_D19 = 4,
+	MX51_PAD_EIM_D20 = 5,
+	MX51_PAD_EIM_D21 = 6,
+	MX51_PAD_EIM_D22 = 7,
+	MX51_PAD_EIM_D23 = 8,
+	MX51_PAD_EIM_D24 = 9,
+	MX51_PAD_EIM_D25 = 10,
+	MX51_PAD_EIM_D26 = 11,
+	MX51_PAD_EIM_D27 = 12,
+	MX51_PAD_EIM_D28 = 13,
+	MX51_PAD_EIM_D29 = 14,
+	MX51_PAD_EIM_D30 = 15,
+	MX51_PAD_EIM_D31 = 16,
+	MX51_PAD_EIM_A16 = 17,
+	MX51_PAD_EIM_A17 = 18,
+	MX51_PAD_EIM_A18 = 19,
+	MX51_PAD_EIM_A19 = 20,
+	MX51_PAD_EIM_A20 = 21,
+	MX51_PAD_EIM_A21 = 22,
+	MX51_PAD_EIM_A22 = 23,
+	MX51_PAD_EIM_A23 = 24,
+	MX51_PAD_EIM_A24 = 25,
+	MX51_PAD_EIM_A25 = 26,
+	MX51_PAD_EIM_A26 = 27,
+	MX51_PAD_EIM_A27 = 28,
+	MX51_PAD_EIM_EB0 = 29,
+	MX51_PAD_EIM_EB1 = 30,
+	MX51_PAD_EIM_EB2 = 31,
+	MX51_PAD_EIM_EB3 = 32,
+	MX51_PAD_EIM_OE = 33,
+	MX51_PAD_EIM_CS0 = 34,
+	MX51_PAD_EIM_CS1 = 35,
+	MX51_PAD_EIM_CS2 = 36,
+	MX51_PAD_EIM_CS3 = 37,
+	MX51_PAD_EIM_CS4 = 38,
+	MX51_PAD_EIM_CS5 = 39,
+	MX51_PAD_EIM_DTACK = 40,
+	MX51_PAD_EIM_LBA = 41,
+	MX51_PAD_EIM_CRE = 42,
+	MX51_PAD_DRAM_CS1 = 43,
+	MX51_PAD_NANDF_WE_B = 44,
+	MX51_PAD_NANDF_RE_B = 45,
+	MX51_PAD_NANDF_ALE = 46,
+	MX51_PAD_NANDF_CLE = 47,
+	MX51_PAD_NANDF_WP_B = 48,
+	MX51_PAD_NANDF_RB0 = 49,
+	MX51_PAD_NANDF_RB1 = 50,
+	MX51_PAD_NANDF_RB2 = 51,
+	MX51_PAD_NANDF_RB3 = 52,
+	MX51_PAD_GPIO_NAND = 53,
+	MX51_PAD_NANDF_CS0 = 54,
+	MX51_PAD_NANDF_CS1 = 55,
+	MX51_PAD_NANDF_CS2 = 56,
+	MX51_PAD_NANDF_CS3 = 57,
+	MX51_PAD_NANDF_CS4 = 58,
+	MX51_PAD_NANDF_CS5 = 59,
+	MX51_PAD_NANDF_CS6 = 60,
+	MX51_PAD_NANDF_CS7 = 61,
+	MX51_PAD_NANDF_RDY_INT = 62,
+	MX51_PAD_NANDF_D15 = 63,
+	MX51_PAD_NANDF_D14 = 64,
+	MX51_PAD_NANDF_D13 = 65,
+	MX51_PAD_NANDF_D12 = 66,
+	MX51_PAD_NANDF_D11 = 67,
+	MX51_PAD_NANDF_D10 = 68,
+	MX51_PAD_NANDF_D9 = 69,
+	MX51_PAD_NANDF_D8 = 70,
+	MX51_PAD_NANDF_D7 = 71,
+	MX51_PAD_NANDF_D6 = 72,
+	MX51_PAD_NANDF_D5 = 73,
+	MX51_PAD_NANDF_D4 = 74,
+	MX51_PAD_NANDF_D3 = 75,
+	MX51_PAD_NANDF_D2 = 76,
+	MX51_PAD_NANDF_D1 = 77,
+	MX51_PAD_NANDF_D0 = 78,
+	MX51_PAD_CSI1_D8 = 79,
+	MX51_PAD_CSI1_D9 = 80,
+	MX51_PAD_CSI1_D10 = 81,
+	MX51_PAD_CSI1_D11 = 82,
+	MX51_PAD_CSI1_D12 = 83,
+	MX51_PAD_CSI1_D13 = 84,
+	MX51_PAD_CSI1_D14 = 85,
+	MX51_PAD_CSI1_D15 = 86,
+	MX51_PAD_CSI1_D16 = 87,
+	MX51_PAD_CSI1_D17 = 88,
+	MX51_PAD_CSI1_D18 = 89,
+	MX51_PAD_CSI1_D19 = 90,
+	MX51_PAD_CSI1_VSYNC = 91,
+	MX51_PAD_CSI1_HSYNC = 92,
+	MX51_PAD_CSI1_PIXCLK = 93,
+	MX51_PAD_CSI1_MCLK = 94,
+	MX51_PAD_CSI2_D12 = 95,
+	MX51_PAD_CSI2_D13 = 96,
+	MX51_PAD_CSI2_D14 = 97,
+	MX51_PAD_CSI2_D15 = 98,
+	MX51_PAD_CSI2_D16 = 99,
+	MX51_PAD_CSI2_D17 = 100,
+	MX51_PAD_CSI2_D18 = 101,
+	MX51_PAD_CSI2_D19 = 102,
+	MX51_PAD_CSI2_VSYNC = 103,
+	MX51_PAD_CSI2_HSYNC = 104,
+	MX51_PAD_CSI2_PIXCLK = 105,
+	MX51_PAD_I2C1_CLK = 106,
+	MX51_PAD_I2C1_DAT = 107,
+	MX51_PAD_AUD3_BB_TXD = 108,
+	MX51_PAD_AUD3_BB_RXD = 109,
+	MX51_PAD_AUD3_BB_CK = 110,
+	MX51_PAD_AUD3_BB_FS = 111,
+	MX51_PAD_CSPI1_MOSI = 112,
+	MX51_PAD_CSPI1_MISO = 113,
+	MX51_PAD_CSPI1_SS0 = 114,
+	MX51_PAD_CSPI1_SS1 = 115,
+	MX51_PAD_CSPI1_RDY = 116,
+	MX51_PAD_CSPI1_SCLK = 117,
+	MX51_PAD_UART1_RXD = 118,
+	MX51_PAD_UART1_TXD = 119,
+	MX51_PAD_UART1_RTS = 120,
+	MX51_PAD_UART1_CTS = 121,
+	MX51_PAD_UART2_RXD = 122,
+	MX51_PAD_UART2_TXD = 123,
+	MX51_PAD_UART3_RXD = 124,
+	MX51_PAD_UART3_TXD = 125,
+	MX51_PAD_OWIRE_LINE = 126,
+	MX51_PAD_KEY_ROW0 = 127,
+	MX51_PAD_KEY_ROW1 = 128,
+	MX51_PAD_KEY_ROW2 = 129,
+	MX51_PAD_KEY_ROW3 = 130,
+	MX51_PAD_KEY_COL0 = 131,
+	MX51_PAD_KEY_COL1 = 132,
+	MX51_PAD_KEY_COL2 = 133,
+	MX51_PAD_KEY_COL3 = 134,
+	MX51_PAD_KEY_COL4 = 135,
+	MX51_PAD_KEY_COL5 = 136,
+	MX51_PAD_USBH1_CLK = 137,
+	MX51_PAD_USBH1_DIR = 138,
+	MX51_PAD_USBH1_STP = 139,
+	MX51_PAD_USBH1_NXT = 140,
+	MX51_PAD_USBH1_DATA0 = 141,
+	MX51_PAD_USBH1_DATA1 = 142,
+	MX51_PAD_USBH1_DATA2 = 143,
+	MX51_PAD_USBH1_DATA3 = 144,
+	MX51_PAD_USBH1_DATA4 = 145,
+	MX51_PAD_USBH1_DATA5 = 146,
+	MX51_PAD_USBH1_DATA6 = 147,
+	MX51_PAD_USBH1_DATA7 = 148,
+	MX51_PAD_DI1_PIN11 = 149,
+	MX51_PAD_DI1_PIN12 = 150,
+	MX51_PAD_DI1_PIN13 = 151,
+	MX51_PAD_DI1_D0_CS = 152,
+	MX51_PAD_DI1_D1_CS = 153,
+	MX51_PAD_DISPB2_SER_DIN = 154,
+	MX51_PAD_DISPB2_SER_DIO = 155,
+	MX51_PAD_DISPB2_SER_CLK = 156,
+	MX51_PAD_DISPB2_SER_RS = 157,
+	MX51_PAD_DISP1_DAT0 = 158,
+	MX51_PAD_DISP1_DAT1 = 159,
+	MX51_PAD_DISP1_DAT2 = 160,
+	MX51_PAD_DISP1_DAT3 = 161,
+	MX51_PAD_DISP1_DAT4 = 162,
+	MX51_PAD_DISP1_DAT5 = 163,
+	MX51_PAD_DISP1_DAT6 = 164,
+	MX51_PAD_DISP1_DAT7 = 165,
+	MX51_PAD_DISP1_DAT8 = 166,
+	MX51_PAD_DISP1_DAT9 = 167,
+	MX51_PAD_DISP1_DAT10 = 168,
+	MX51_PAD_DISP1_DAT11 = 169,
+	MX51_PAD_DISP1_DAT12 = 170,
+	MX51_PAD_DISP1_DAT13 = 171,
+	MX51_PAD_DISP1_DAT14 = 172,
+	MX51_PAD_DISP1_DAT15 = 173,
+	MX51_PAD_DISP1_DAT16 = 174,
+	MX51_PAD_DISP1_DAT17 = 175,
+	MX51_PAD_DISP1_DAT18 = 176,
+	MX51_PAD_DISP1_DAT19 = 177,
+	MX51_PAD_DISP1_DAT20 = 178,
+	MX51_PAD_DISP1_DAT21 = 179,
+	MX51_PAD_DISP1_DAT22 = 180,
+	MX51_PAD_DISP1_DAT23 = 181,
+	MX51_PAD_DI1_PIN3 = 182,
+	MX51_PAD_DI1_PIN2 = 183,
+	MX51_PAD_DI_GP2 = 184,
+	MX51_PAD_DI_GP3 = 185,
+	MX51_PAD_DI2_PIN4 = 186,
+	MX51_PAD_DI2_PIN2 = 187,
+	MX51_PAD_DI2_PIN3 = 188,
+	MX51_PAD_DI2_DISP_CLK = 189,
+	MX51_PAD_DI_GP4 = 190,
+	MX51_PAD_DISP2_DAT0 = 191,
+	MX51_PAD_DISP2_DAT1 = 192,
+	MX51_PAD_DISP2_DAT2 = 193,
+	MX51_PAD_DISP2_DAT3 = 194,
+	MX51_PAD_DISP2_DAT4 = 195,
+	MX51_PAD_DISP2_DAT5 = 196,
+	MX51_PAD_DISP2_DAT6 = 197,
+	MX51_PAD_DISP2_DAT7 = 198,
+	MX51_PAD_DISP2_DAT8 = 199,
+	MX51_PAD_DISP2_DAT9 = 200,
+	MX51_PAD_DISP2_DAT10 = 201,
+	MX51_PAD_DISP2_DAT11 = 202,
+	MX51_PAD_DISP2_DAT12 = 203,
+	MX51_PAD_DISP2_DAT13 = 204,
+	MX51_PAD_DISP2_DAT14 = 205,
+	MX51_PAD_DISP2_DAT15 = 206,
+	MX51_PAD_SD1_CMD = 207,
+	MX51_PAD_SD1_CLK = 208,
+	MX51_PAD_SD1_DATA0 = 209,
+	MX51_PAD_EIM_DA0 = 210,
+	MX51_PAD_EIM_DA1 = 211,
+	MX51_PAD_EIM_DA2 = 212,
+	MX51_PAD_EIM_DA3 = 213,
+	MX51_PAD_SD1_DATA1 = 214,
+	MX51_PAD_EIM_DA4 = 215,
+	MX51_PAD_EIM_DA5 = 216,
+	MX51_PAD_EIM_DA6 = 217,
+	MX51_PAD_EIM_DA7 = 218,
+	MX51_PAD_SD1_DATA2 = 219,
+	MX51_PAD_EIM_DA10 = 220,
+	MX51_PAD_EIM_DA11 = 221,
+	MX51_PAD_EIM_DA8 = 222,
+	MX51_PAD_EIM_DA9 = 223,
+	MX51_PAD_SD1_DATA3 = 224,
+	MX51_PAD_GPIO1_0 = 225,
+	MX51_PAD_GPIO1_1 = 226,
+	MX51_PAD_EIM_DA12 = 227,
+	MX51_PAD_EIM_DA13 = 228,
+	MX51_PAD_EIM_DA14 = 229,
+	MX51_PAD_EIM_DA15 = 230,
+	MX51_PAD_SD2_CMD = 231,
+	MX51_PAD_SD2_CLK = 232,
+	MX51_PAD_SD2_DATA0 = 233,
+	MX51_PAD_SD2_DATA1 = 234,
+	MX51_PAD_SD2_DATA2 = 235,
+	MX51_PAD_SD2_DATA3 = 236,
+	MX51_PAD_GPIO1_2 = 237,
+	MX51_PAD_GPIO1_3 = 238,
+	MX51_PAD_PMIC_INT_REQ = 239,
+	MX51_PAD_GPIO1_4 = 240,
+	MX51_PAD_GPIO1_5 = 241,
+	MX51_PAD_GPIO1_6 = 242,
+	MX51_PAD_GPIO1_7 = 243,
+	MX51_PAD_GPIO1_8 = 244,
+	MX51_PAD_GPIO1_9 = 245,
+};
+
+/* 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, NO_PAD, 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 */
+};
+
+/* Pad names for the pinmux subsystem */
+static const struct pinctrl_pin_desc imx51_pinctrl_pads[] = {
+	IMX_PINCTRL_PIN(MX51_PAD_EIM_D16),
+	IMX_PINCTRL_PIN(MX51_PAD_EIM_D17),
+	IMX_PINCTRL_PIN(MX51_PAD_EIM_D18),
+	IMX_PINCTRL_PIN(MX51_PAD_EIM_D19),
+	IMX_PINCTRL_PIN(MX51_PAD_EIM_D20),
+	IMX_PINCTRL_PIN(MX51_PAD_EIM_D21),
+	IMX_PINCTRL_PIN(MX51_PAD_EIM_D22),
+	IMX_PINCTRL_PIN(MX51_PAD_EIM_D23),
+	IMX_PINCTRL_PIN(MX51_PAD_EIM_D24),
+	IMX_PINCTRL_PIN(MX51_PAD_EIM_D25),
+	IMX_PINCTRL_PIN(MX51_PAD_EIM_D26),
+	IMX_PINCTRL_PIN(MX51_PAD_EIM_D27),
+	IMX_PINCTRL_PIN(MX51_PAD_EIM_D28),
+	IMX_PINCTRL_PIN(MX51_PAD_EIM_D29),
+	IMX_PINCTRL_PIN(MX51_PAD_EIM_D30),
+	IMX_PINCTRL_PIN(MX51_PAD_EIM_D31),
+	IMX_PINCTRL_PIN(MX51_PAD_EIM_A16),
+	IMX_PINCTRL_PIN(MX51_PAD_EIM_A17),
+	IMX_PINCTRL_PIN(MX51_PAD_EIM_A18),
+	IMX_PINCTRL_PIN(MX51_PAD_EIM_A19),
+	IMX_PINCTRL_PIN(MX51_PAD_EIM_A20),
+	IMX_PINCTRL_PIN(MX51_PAD_EIM_A21),
+	IMX_PINCTRL_PIN(MX51_PAD_EIM_A22),
+	IMX_PINCTRL_PIN(MX51_PAD_EIM_A23),
+	IMX_PINCTRL_PIN(MX51_PAD_EIM_A24),
+	IMX_PINCTRL_PIN(MX51_PAD_EIM_A25),
+	IMX_PINCTRL_PIN(MX51_PAD_EIM_A26),
+	IMX_PINCTRL_PIN(MX51_PAD_EIM_A27),
+	IMX_PINCTRL_PIN(MX51_PAD_EIM_EB0),
+	IMX_PINCTRL_PIN(MX51_PAD_EIM_EB1),
+	IMX_PINCTRL_PIN(MX51_PAD_EIM_EB2),
+	IMX_PINCTRL_PIN(MX51_PAD_EIM_EB3),
+	IMX_PINCTRL_PIN(MX51_PAD_EIM_OE),
+	IMX_PINCTRL_PIN(MX51_PAD_EIM_CS0),
+	IMX_PINCTRL_PIN(MX51_PAD_EIM_CS1),
+	IMX_PINCTRL_PIN(MX51_PAD_EIM_CS2),
+	IMX_PINCTRL_PIN(MX51_PAD_EIM_CS3),
+	IMX_PINCTRL_PIN(MX51_PAD_EIM_CS4),
+	IMX_PINCTRL_PIN(MX51_PAD_EIM_CS5),
+	IMX_PINCTRL_PIN(MX51_PAD_EIM_DTACK),
+	IMX_PINCTRL_PIN(MX51_PAD_EIM_LBA),
+	IMX_PINCTRL_PIN(MX51_PAD_EIM_CRE),
+	IMX_PINCTRL_PIN(MX51_PAD_DRAM_CS1),
+	IMX_PINCTRL_PIN(MX51_PAD_NANDF_WE_B),
+	IMX_PINCTRL_PIN(MX51_PAD_NANDF_RE_B),
+	IMX_PINCTRL_PIN(MX51_PAD_NANDF_ALE),
+	IMX_PINCTRL_PIN(MX51_PAD_NANDF_CLE),
+	IMX_PINCTRL_PIN(MX51_PAD_NANDF_WP_B),
+	IMX_PINCTRL_PIN(MX51_PAD_NANDF_RB0),
+	IMX_PINCTRL_PIN(MX51_PAD_NANDF_RB1),
+	IMX_PINCTRL_PIN(MX51_PAD_NANDF_RB2),
+	IMX_PINCTRL_PIN(MX51_PAD_NANDF_RB3),
+	IMX_PINCTRL_PIN(MX51_PAD_GPIO_NAND),
+	IMX_PINCTRL_PIN(MX51_PAD_NANDF_CS0),
+	IMX_PINCTRL_PIN(MX51_PAD_NANDF_CS1),
+	IMX_PINCTRL_PIN(MX51_PAD_NANDF_CS2),
+	IMX_PINCTRL_PIN(MX51_PAD_NANDF_CS3),
+	IMX_PINCTRL_PIN(MX51_PAD_NANDF_CS4),
+	IMX_PINCTRL_PIN(MX51_PAD_NANDF_CS5),
+	IMX_PINCTRL_PIN(MX51_PAD_NANDF_CS6),
+	IMX_PINCTRL_PIN(MX51_PAD_NANDF_CS7),
+	IMX_PINCTRL_PIN(MX51_PAD_NANDF_RDY_INT),
+	IMX_PINCTRL_PIN(MX51_PAD_NANDF_D15),
+	IMX_PINCTRL_PIN(MX51_PAD_NANDF_D14),
+	IMX_PINCTRL_PIN(MX51_PAD_NANDF_D13),
+	IMX_PINCTRL_PIN(MX51_PAD_NANDF_D12),
+	IMX_PINCTRL_PIN(MX51_PAD_NANDF_D11),
+	IMX_PINCTRL_PIN(MX51_PAD_NANDF_D10),
+	IMX_PINCTRL_PIN(MX51_PAD_NANDF_D9),
+	IMX_PINCTRL_PIN(MX51_PAD_NANDF_D8),
+	IMX_PINCTRL_PIN(MX51_PAD_NANDF_D7),
+	IMX_PINCTRL_PIN(MX51_PAD_NANDF_D6),
+	IMX_PINCTRL_PIN(MX51_PAD_NANDF_D5),
+	IMX_PINCTRL_PIN(MX51_PAD_NANDF_D4),
+	IMX_PINCTRL_PIN(MX51_PAD_NANDF_D3),
+	IMX_PINCTRL_PIN(MX51_PAD_NANDF_D2),
+	IMX_PINCTRL_PIN(MX51_PAD_NANDF_D1),
+	IMX_PINCTRL_PIN(MX51_PAD_NANDF_D0),
+	IMX_PINCTRL_PIN(MX51_PAD_CSI1_D8),
+	IMX_PINCTRL_PIN(MX51_PAD_CSI1_D9),
+	IMX_PINCTRL_PIN(MX51_PAD_CSI1_D10),
+	IMX_PINCTRL_PIN(MX51_PAD_CSI1_D11),
+	IMX_PINCTRL_PIN(MX51_PAD_CSI1_D12),
+	IMX_PINCTRL_PIN(MX51_PAD_CSI1_D13),
+	IMX_PINCTRL_PIN(MX51_PAD_CSI1_D14),
+	IMX_PINCTRL_PIN(MX51_PAD_CSI1_D15),
+	IMX_PINCTRL_PIN(MX51_PAD_CSI1_D16),
+	IMX_PINCTRL_PIN(MX51_PAD_CSI1_D17),
+	IMX_PINCTRL_PIN(MX51_PAD_CSI1_D18),
+	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),
+	IMX_PINCTRL_PIN(MX51_PAD_CSI2_D15),
+	IMX_PINCTRL_PIN(MX51_PAD_CSI2_D16),
+	IMX_PINCTRL_PIN(MX51_PAD_CSI2_D17),
+	IMX_PINCTRL_PIN(MX51_PAD_CSI2_D18),
+	IMX_PINCTRL_PIN(MX51_PAD_CSI2_D19),
+	IMX_PINCTRL_PIN(MX51_PAD_CSI2_VSYNC),
+	IMX_PINCTRL_PIN(MX51_PAD_CSI2_HSYNC),
+	IMX_PINCTRL_PIN(MX51_PAD_CSI2_PIXCLK),
+	IMX_PINCTRL_PIN(MX51_PAD_I2C1_CLK),
+	IMX_PINCTRL_PIN(MX51_PAD_I2C1_DAT),
+	IMX_PINCTRL_PIN(MX51_PAD_AUD3_BB_TXD),
+	IMX_PINCTRL_PIN(MX51_PAD_AUD3_BB_RXD),
+	IMX_PINCTRL_PIN(MX51_PAD_AUD3_BB_CK),
+	IMX_PINCTRL_PIN(MX51_PAD_AUD3_BB_FS),
+	IMX_PINCTRL_PIN(MX51_PAD_CSPI1_MOSI),
+	IMX_PINCTRL_PIN(MX51_PAD_CSPI1_MISO),
+	IMX_PINCTRL_PIN(MX51_PAD_CSPI1_SS0),
+	IMX_PINCTRL_PIN(MX51_PAD_CSPI1_SS1),
+	IMX_PINCTRL_PIN(MX51_PAD_CSPI1_RDY),
+	IMX_PINCTRL_PIN(MX51_PAD_CSPI1_SCLK),
+	IMX_PINCTRL_PIN(MX51_PAD_UART1_RXD),
+	IMX_PINCTRL_PIN(MX51_PAD_UART1_TXD),
+	IMX_PINCTRL_PIN(MX51_PAD_UART1_RTS),
+	IMX_PINCTRL_PIN(MX51_PAD_UART1_CTS),
+	IMX_PINCTRL_PIN(MX51_PAD_UART2_RXD),
+	IMX_PINCTRL_PIN(MX51_PAD_UART2_TXD),
+	IMX_PINCTRL_PIN(MX51_PAD_UART3_RXD),
+	IMX_PINCTRL_PIN(MX51_PAD_UART3_TXD),
+	IMX_PINCTRL_PIN(MX51_PAD_OWIRE_LINE),
+	IMX_PINCTRL_PIN(MX51_PAD_KEY_ROW0),
+	IMX_PINCTRL_PIN(MX51_PAD_KEY_ROW1),
+	IMX_PINCTRL_PIN(MX51_PAD_KEY_ROW2),
+	IMX_PINCTRL_PIN(MX51_PAD_KEY_ROW3),
+	IMX_PINCTRL_PIN(MX51_PAD_KEY_COL0),
+	IMX_PINCTRL_PIN(MX51_PAD_KEY_COL1),
+	IMX_PINCTRL_PIN(MX51_PAD_KEY_COL2),
+	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_USBH1_CLK),
+	IMX_PINCTRL_PIN(MX51_PAD_USBH1_DIR),
+	IMX_PINCTRL_PIN(MX51_PAD_USBH1_STP),
+	IMX_PINCTRL_PIN(MX51_PAD_USBH1_NXT),
+	IMX_PINCTRL_PIN(MX51_PAD_USBH1_DATA0),
+	IMX_PINCTRL_PIN(MX51_PAD_USBH1_DATA1),
+	IMX_PINCTRL_PIN(MX51_PAD_USBH1_DATA2),
+	IMX_PINCTRL_PIN(MX51_PAD_USBH1_DATA3),
+	IMX_PINCTRL_PIN(MX51_PAD_USBH1_DATA4),
+	IMX_PINCTRL_PIN(MX51_PAD_USBH1_DATA5),
+	IMX_PINCTRL_PIN(MX51_PAD_USBH1_DATA6),
+	IMX_PINCTRL_PIN(MX51_PAD_USBH1_DATA7),
+	IMX_PINCTRL_PIN(MX51_PAD_DI1_PIN11),
+	IMX_PINCTRL_PIN(MX51_PAD_DI1_PIN12),
+	IMX_PINCTRL_PIN(MX51_PAD_DI1_PIN13),
+	IMX_PINCTRL_PIN(MX51_PAD_DI1_D0_CS),
+	IMX_PINCTRL_PIN(MX51_PAD_DI1_D1_CS),
+	IMX_PINCTRL_PIN(MX51_PAD_DISPB2_SER_DIN),
+	IMX_PINCTRL_PIN(MX51_PAD_DISPB2_SER_DIO),
+	IMX_PINCTRL_PIN(MX51_PAD_DISPB2_SER_CLK),
+	IMX_PINCTRL_PIN(MX51_PAD_DISPB2_SER_RS),
+	IMX_PINCTRL_PIN(MX51_PAD_DISP1_DAT0),
+	IMX_PINCTRL_PIN(MX51_PAD_DISP1_DAT1),
+	IMX_PINCTRL_PIN(MX51_PAD_DISP1_DAT2),
+	IMX_PINCTRL_PIN(MX51_PAD_DISP1_DAT3),
+	IMX_PINCTRL_PIN(MX51_PAD_DISP1_DAT4),
+	IMX_PINCTRL_PIN(MX51_PAD_DISP1_DAT5),
+	IMX_PINCTRL_PIN(MX51_PAD_DISP1_DAT6),
+	IMX_PINCTRL_PIN(MX51_PAD_DISP1_DAT7),
+	IMX_PINCTRL_PIN(MX51_PAD_DISP1_DAT8),
+	IMX_PINCTRL_PIN(MX51_PAD_DISP1_DAT9),
+	IMX_PINCTRL_PIN(MX51_PAD_DISP1_DAT10),
+	IMX_PINCTRL_PIN(MX51_PAD_DISP1_DAT11),
+	IMX_PINCTRL_PIN(MX51_PAD_DISP1_DAT12),
+	IMX_PINCTRL_PIN(MX51_PAD_DISP1_DAT13),
+	IMX_PINCTRL_PIN(MX51_PAD_DISP1_DAT14),
+	IMX_PINCTRL_PIN(MX51_PAD_DISP1_DAT15),
+	IMX_PINCTRL_PIN(MX51_PAD_DISP1_DAT16),
+	IMX_PINCTRL_PIN(MX51_PAD_DISP1_DAT17),
+	IMX_PINCTRL_PIN(MX51_PAD_DISP1_DAT18),
+	IMX_PINCTRL_PIN(MX51_PAD_DISP1_DAT19),
+	IMX_PINCTRL_PIN(MX51_PAD_DISP1_DAT20),
+	IMX_PINCTRL_PIN(MX51_PAD_DISP1_DAT21),
+	IMX_PINCTRL_PIN(MX51_PAD_DISP1_DAT22),
+	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_DI_GP2),
+	IMX_PINCTRL_PIN(MX51_PAD_DI_GP3),
+	IMX_PINCTRL_PIN(MX51_PAD_DI2_PIN4),
+	IMX_PINCTRL_PIN(MX51_PAD_DI2_PIN2),
+	IMX_PINCTRL_PIN(MX51_PAD_DI2_PIN3),
+	IMX_PINCTRL_PIN(MX51_PAD_DI2_DISP_CLK),
+	IMX_PINCTRL_PIN(MX51_PAD_DI_GP4),
+	IMX_PINCTRL_PIN(MX51_PAD_DISP2_DAT0),
+	IMX_PINCTRL_PIN(MX51_PAD_DISP2_DAT1),
+	IMX_PINCTRL_PIN(MX51_PAD_DISP2_DAT2),
+	IMX_PINCTRL_PIN(MX51_PAD_DISP2_DAT3),
+	IMX_PINCTRL_PIN(MX51_PAD_DISP2_DAT4),
+	IMX_PINCTRL_PIN(MX51_PAD_DISP2_DAT5),
+	IMX_PINCTRL_PIN(MX51_PAD_DISP2_DAT6),
+	IMX_PINCTRL_PIN(MX51_PAD_DISP2_DAT7),
+	IMX_PINCTRL_PIN(MX51_PAD_DISP2_DAT8),
+	IMX_PINCTRL_PIN(MX51_PAD_DISP2_DAT9),
+	IMX_PINCTRL_PIN(MX51_PAD_DISP2_DAT10),
+	IMX_PINCTRL_PIN(MX51_PAD_DISP2_DAT11),
+	IMX_PINCTRL_PIN(MX51_PAD_DISP2_DAT12),
+	IMX_PINCTRL_PIN(MX51_PAD_DISP2_DAT13),
+	IMX_PINCTRL_PIN(MX51_PAD_DISP2_DAT14),
+	IMX_PINCTRL_PIN(MX51_PAD_DISP2_DAT15),
+	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),
+	IMX_PINCTRL_PIN(MX51_PAD_SD2_DATA1),
+	IMX_PINCTRL_PIN(MX51_PAD_SD2_DATA2),
+	IMX_PINCTRL_PIN(MX51_PAD_SD2_DATA3),
+	IMX_PINCTRL_PIN(MX51_PAD_GPIO1_2),
+	IMX_PINCTRL_PIN(MX51_PAD_GPIO1_3),
+	IMX_PINCTRL_PIN(MX51_PAD_PMIC_INT_REQ),
+	IMX_PINCTRL_PIN(MX51_PAD_GPIO1_4),
+	IMX_PINCTRL_PIN(MX51_PAD_GPIO1_5),
+	IMX_PINCTRL_PIN(MX51_PAD_GPIO1_6),
+	IMX_PINCTRL_PIN(MX51_PAD_GPIO1_7),
+	IMX_PINCTRL_PIN(MX51_PAD_GPIO1_8),
+	IMX_PINCTRL_PIN(MX51_PAD_GPIO1_9),
+};
+
+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[] __devinitdata = {
+	{ .compatible = "fsl,imx51-iomuxc", },
+	{ /* sentinel */ }
+};
+
+static int __devinit imx51_pinctrl_probe(struct platform_device *pdev)
+{
+	return imx_pinctrl_probe(pdev, &imx51_pinctrl_info);
+}
+
+static struct platform_driver imx51_pinctrl_driver = {
+	.driver = {
+		.name = "imx51-pinctrl",
+		.owner = THIS_MODULE,
+		.of_match_table = of_match_ptr(imx51_pinctrl_of_match),
+	},
+	.probe = imx51_pinctrl_probe,
+	.remove = __devexit_p(imx_pinctrl_remove),
+};
+
+static int __init imx51_pinctrl_init(void)
+{
+	return platform_driver_register(&imx51_pinctrl_driver);
+}
+arch_initcall(imx51_pinctrl_init);
+
+static void __exit imx51_pinctrl_exit(void)
+{
+	platform_driver_unregister(&imx51_pinctrl_driver);
+}
+module_exit(imx51_pinctrl_exit);
+MODULE_AUTHOR("Dong Aisheng <dong.aisheng@linaro.org>");
+MODULE_DESCRIPTION("Freescale IMX51 pinctrl driver");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/pinctrl/pinctrl-imx53.c b/drivers/pinctrl/pinctrl-imx53.c
new file mode 100644
index 00000000000..1f49e16a9bc
--- /dev/null
+++ b/drivers/pinctrl/pinctrl-imx53.c
@@ -0,0 +1,1649 @@
+/*
+ * imx53 pinctrl driver based on imx pinmux core
+ *
+ * Copyright (C) 2012 Freescale Semiconductor, Inc.
+ * Copyright (C) 2012 Linaro, Inc.
+ *
+ * Author: Dong Aisheng <dong.aisheng@linaro.org>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ */
+
+#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 imx53_pads {
+	MX53_PAD_GPIO_19 = 1,
+	MX53_PAD_KEY_COL0 = 2,
+	MX53_PAD_KEY_ROW0 = 3,
+	MX53_PAD_KEY_COL1 = 4,
+	MX53_PAD_KEY_ROW1 = 5,
+	MX53_PAD_KEY_COL2 = 6,
+	MX53_PAD_KEY_ROW2 = 7,
+	MX53_PAD_KEY_COL3 = 8,
+	MX53_PAD_KEY_ROW3 = 9,
+	MX53_PAD_KEY_COL4 = 10,
+	MX53_PAD_KEY_ROW4 = 11,
+	MX53_PAD_DI0_DISP_CLK = 12,
+	MX53_PAD_DI0_PIN15 = 13,
+	MX53_PAD_DI0_PIN2 = 14,
+	MX53_PAD_DI0_PIN3 = 15,
+	MX53_PAD_DI0_PIN4 = 16,
+	MX53_PAD_DISP0_DAT0 = 17,
+	MX53_PAD_DISP0_DAT1 = 18,
+	MX53_PAD_DISP0_DAT2 = 19,
+	MX53_PAD_DISP0_DAT3 = 20,
+	MX53_PAD_DISP0_DAT4 = 21,
+	MX53_PAD_DISP0_DAT5 = 22,
+	MX53_PAD_DISP0_DAT6 = 23,
+	MX53_PAD_DISP0_DAT7 = 24,
+	MX53_PAD_DISP0_DAT8 = 25,
+	MX53_PAD_DISP0_DAT9 = 26,
+	MX53_PAD_DISP0_DAT10 = 27,
+	MX53_PAD_DISP0_DAT11 = 28,
+	MX53_PAD_DISP0_DAT12 = 29,
+	MX53_PAD_DISP0_DAT13 = 30,
+	MX53_PAD_DISP0_DAT14 = 31,
+	MX53_PAD_DISP0_DAT15 = 32,
+	MX53_PAD_DISP0_DAT16 = 33,
+	MX53_PAD_DISP0_DAT17 = 34,
+	MX53_PAD_DISP0_DAT18 = 35,
+	MX53_PAD_DISP0_DAT19 = 36,
+	MX53_PAD_DISP0_DAT20 = 37,
+	MX53_PAD_DISP0_DAT21 = 38,
+	MX53_PAD_DISP0_DAT22 = 39,
+	MX53_PAD_DISP0_DAT23 = 40,
+	MX53_PAD_CSI0_PIXCLK = 41,
+	MX53_PAD_CSI0_MCLK = 42,
+	MX53_PAD_CSI0_DATA_EN = 43,
+	MX53_PAD_CSI0_VSYNC = 44,
+	MX53_PAD_CSI0_DAT4 = 45,
+	MX53_PAD_CSI0_DAT5 = 46,
+	MX53_PAD_CSI0_DAT6 = 47,
+	MX53_PAD_CSI0_DAT7 = 48,
+	MX53_PAD_CSI0_DAT8 = 49,
+	MX53_PAD_CSI0_DAT9 = 50,
+	MX53_PAD_CSI0_DAT10 = 51,
+	MX53_PAD_CSI0_DAT11 = 52,
+	MX53_PAD_CSI0_DAT12 = 53,
+	MX53_PAD_CSI0_DAT13 = 54,
+	MX53_PAD_CSI0_DAT14 = 55,
+	MX53_PAD_CSI0_DAT15 = 56,
+	MX53_PAD_CSI0_DAT16 = 57,
+	MX53_PAD_CSI0_DAT17 = 58,
+	MX53_PAD_CSI0_DAT18 = 59,
+	MX53_PAD_CSI0_DAT19 = 60,
+	MX53_PAD_EIM_A25 = 61,
+	MX53_PAD_EIM_EB2 = 62,
+	MX53_PAD_EIM_D16 = 63,
+	MX53_PAD_EIM_D17 = 64,
+	MX53_PAD_EIM_D18 = 65,
+	MX53_PAD_EIM_D19 = 66,
+	MX53_PAD_EIM_D20 = 67,
+	MX53_PAD_EIM_D21 = 68,
+	MX53_PAD_EIM_D22 = 69,
+	MX53_PAD_EIM_D23 = 70,
+	MX53_PAD_EIM_EB3 = 71,
+	MX53_PAD_EIM_D24 = 72,
+	MX53_PAD_EIM_D25 = 73,
+	MX53_PAD_EIM_D26 = 74,
+	MX53_PAD_EIM_D27 = 75,
+	MX53_PAD_EIM_D28 = 76,
+	MX53_PAD_EIM_D29 = 77,
+	MX53_PAD_EIM_D30 = 78,
+	MX53_PAD_EIM_D31 = 79,
+	MX53_PAD_EIM_A24 = 80,
+	MX53_PAD_EIM_A23 = 81,
+	MX53_PAD_EIM_A22 = 82,
+	MX53_PAD_EIM_A21 = 83,
+	MX53_PAD_EIM_A20 = 84,
+	MX53_PAD_EIM_A19 = 85,
+	MX53_PAD_EIM_A18 = 86,
+	MX53_PAD_EIM_A17 = 87,
+	MX53_PAD_EIM_A16 = 88,
+	MX53_PAD_EIM_CS0 = 89,
+	MX53_PAD_EIM_CS1 = 90,
+	MX53_PAD_EIM_OE = 91,
+	MX53_PAD_EIM_RW = 92,
+	MX53_PAD_EIM_LBA = 93,
+	MX53_PAD_EIM_EB0 = 94,
+	MX53_PAD_EIM_EB1 = 95,
+	MX53_PAD_EIM_DA0 = 96,
+	MX53_PAD_EIM_DA1 = 97,
+	MX53_PAD_EIM_DA2 = 98,
+	MX53_PAD_EIM_DA3 = 99,
+	MX53_PAD_EIM_DA4 = 100,
+	MX53_PAD_EIM_DA5 = 101,
+	MX53_PAD_EIM_DA6 = 102,
+	MX53_PAD_EIM_DA7 = 103,
+	MX53_PAD_EIM_DA8 = 104,
+	MX53_PAD_EIM_DA9 = 105,
+	MX53_PAD_EIM_DA10 = 106,
+	MX53_PAD_EIM_DA11 = 107,
+	MX53_PAD_EIM_DA12 = 108,
+	MX53_PAD_EIM_DA13 = 109,
+	MX53_PAD_EIM_DA14 = 110,
+	MX53_PAD_EIM_DA15 = 111,
+	MX53_PAD_NANDF_WE_B = 112,
+	MX53_PAD_NANDF_RE_B = 113,
+	MX53_PAD_EIM_WAIT = 114,
+	MX53_PAD_LVDS1_TX3_P = 115,
+	MX53_PAD_LVDS1_TX2_P = 116,
+	MX53_PAD_LVDS1_CLK_P = 117,
+	MX53_PAD_LVDS1_TX1_P = 118,
+	MX53_PAD_LVDS1_TX0_P = 119,
+	MX53_PAD_LVDS0_TX3_P = 120,
+	MX53_PAD_LVDS0_CLK_P = 121,
+	MX53_PAD_LVDS0_TX2_P = 122,
+	MX53_PAD_LVDS0_TX1_P = 123,
+	MX53_PAD_LVDS0_TX0_P = 124,
+	MX53_PAD_GPIO_10 = 125,
+	MX53_PAD_GPIO_11 = 126,
+	MX53_PAD_GPIO_12 = 127,
+	MX53_PAD_GPIO_13 = 128,
+	MX53_PAD_GPIO_14 = 129,
+	MX53_PAD_NANDF_CLE = 130,
+	MX53_PAD_NANDF_ALE = 131,
+	MX53_PAD_NANDF_WP_B = 132,
+	MX53_PAD_NANDF_RB0 = 133,
+	MX53_PAD_NANDF_CS0 = 134,
+	MX53_PAD_NANDF_CS1 = 135,
+	MX53_PAD_NANDF_CS2 = 136,
+	MX53_PAD_NANDF_CS3 = 137,
+	MX53_PAD_FEC_MDIO = 138,
+	MX53_PAD_FEC_REF_CLK = 139,
+	MX53_PAD_FEC_RX_ER = 140,
+	MX53_PAD_FEC_CRS_DV = 141,
+	MX53_PAD_FEC_RXD1 = 142,
+	MX53_PAD_FEC_RXD0 = 143,
+	MX53_PAD_FEC_TX_EN = 144,
+	MX53_PAD_FEC_TXD1 = 145,
+	MX53_PAD_FEC_TXD0 = 146,
+	MX53_PAD_FEC_MDC = 147,
+	MX53_PAD_PATA_DIOW = 148,
+	MX53_PAD_PATA_DMACK = 149,
+	MX53_PAD_PATA_DMARQ = 150,
+	MX53_PAD_PATA_BUFFER_EN = 151,
+	MX53_PAD_PATA_INTRQ = 152,
+	MX53_PAD_PATA_DIOR = 153,
+	MX53_PAD_PATA_RESET_B = 154,
+	MX53_PAD_PATA_IORDY = 155,
+	MX53_PAD_PATA_DA_0 = 156,
+	MX53_PAD_PATA_DA_1 = 157,
+	MX53_PAD_PATA_DA_2 = 158,
+	MX53_PAD_PATA_CS_0 = 159,
+	MX53_PAD_PATA_CS_1 = 160,
+	MX53_PAD_PATA_DATA0 = 161,
+	MX53_PAD_PATA_DATA1 = 162,
+	MX53_PAD_PATA_DATA2 = 163,
+	MX53_PAD_PATA_DATA3 = 164,
+	MX53_PAD_PATA_DATA4 = 165,
+	MX53_PAD_PATA_DATA5 = 166,
+	MX53_PAD_PATA_DATA6 = 167,
+	MX53_PAD_PATA_DATA7 = 168,
+	MX53_PAD_PATA_DATA8 = 169,
+	MX53_PAD_PATA_DATA9 = 170,
+	MX53_PAD_PATA_DATA10 = 171,
+	MX53_PAD_PATA_DATA11 = 172,
+	MX53_PAD_PATA_DATA12 = 173,
+	MX53_PAD_PATA_DATA13 = 174,
+	MX53_PAD_PATA_DATA14 = 175,
+	MX53_PAD_PATA_DATA15 = 176,
+	MX53_PAD_SD1_DATA0 = 177,
+	MX53_PAD_SD1_DATA1 = 178,
+	MX53_PAD_SD1_CMD = 179,
+	MX53_PAD_SD1_DATA2 = 180,
+	MX53_PAD_SD1_CLK = 181,
+	MX53_PAD_SD1_DATA3 = 182,
+	MX53_PAD_SD2_CLK = 183,
+	MX53_PAD_SD2_CMD = 184,
+	MX53_PAD_SD2_DATA3 = 185,
+	MX53_PAD_SD2_DATA2 = 186,
+	MX53_PAD_SD2_DATA1 = 187,
+	MX53_PAD_SD2_DATA0 = 188,
+	MX53_PAD_GPIO_0 = 189,
+	MX53_PAD_GPIO_1 = 190,
+	MX53_PAD_GPIO_9 = 191,
+	MX53_PAD_GPIO_3 = 192,
+	MX53_PAD_GPIO_6 = 193,
+	MX53_PAD_GPIO_2 = 194,
+	MX53_PAD_GPIO_4 = 195,
+	MX53_PAD_GPIO_5 = 196,
+	MX53_PAD_GPIO_7 = 197,
+	MX53_PAD_GPIO_8 = 198,
+	MX53_PAD_GPIO_16 = 199,
+	MX53_PAD_GPIO_17 = 200,
+	MX53_PAD_GPIO_18 = 201,
+};
+
+/* 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, 3), /* 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 */
+};
+
+/* Pad names for the pinmux subsystem */
+static const struct pinctrl_pin_desc imx53_pinctrl_pads[] = {
+	IMX_PINCTRL_PIN(MX53_PAD_GPIO_19),
+	IMX_PINCTRL_PIN(MX53_PAD_KEY_COL0),
+	IMX_PINCTRL_PIN(MX53_PAD_KEY_ROW0),
+	IMX_PINCTRL_PIN(MX53_PAD_KEY_COL1),
+	IMX_PINCTRL_PIN(MX53_PAD_KEY_ROW1),
+	IMX_PINCTRL_PIN(MX53_PAD_KEY_COL2),
+	IMX_PINCTRL_PIN(MX53_PAD_KEY_ROW2),
+	IMX_PINCTRL_PIN(MX53_PAD_KEY_COL3),
+	IMX_PINCTRL_PIN(MX53_PAD_KEY_ROW3),
+	IMX_PINCTRL_PIN(MX53_PAD_KEY_COL4),
+	IMX_PINCTRL_PIN(MX53_PAD_KEY_ROW4),
+	IMX_PINCTRL_PIN(MX53_PAD_DI0_DISP_CLK),
+	IMX_PINCTRL_PIN(MX53_PAD_DI0_PIN15),
+	IMX_PINCTRL_PIN(MX53_PAD_DI0_PIN2),
+	IMX_PINCTRL_PIN(MX53_PAD_DI0_PIN3),
+	IMX_PINCTRL_PIN(MX53_PAD_DI0_PIN4),
+	IMX_PINCTRL_PIN(MX53_PAD_DISP0_DAT0),
+	IMX_PINCTRL_PIN(MX53_PAD_DISP0_DAT1),
+	IMX_PINCTRL_PIN(MX53_PAD_DISP0_DAT2),
+	IMX_PINCTRL_PIN(MX53_PAD_DISP0_DAT3),
+	IMX_PINCTRL_PIN(MX53_PAD_DISP0_DAT4),
+	IMX_PINCTRL_PIN(MX53_PAD_DISP0_DAT5),
+	IMX_PINCTRL_PIN(MX53_PAD_DISP0_DAT6),
+	IMX_PINCTRL_PIN(MX53_PAD_DISP0_DAT7),
+	IMX_PINCTRL_PIN(MX53_PAD_DISP0_DAT8),
+	IMX_PINCTRL_PIN(MX53_PAD_DISP0_DAT9),
+	IMX_PINCTRL_PIN(MX53_PAD_DISP0_DAT10),
+	IMX_PINCTRL_PIN(MX53_PAD_DISP0_DAT11),
+	IMX_PINCTRL_PIN(MX53_PAD_DISP0_DAT12),
+	IMX_PINCTRL_PIN(MX53_PAD_DISP0_DAT13),
+	IMX_PINCTRL_PIN(MX53_PAD_DISP0_DAT14),
+	IMX_PINCTRL_PIN(MX53_PAD_DISP0_DAT15),
+	IMX_PINCTRL_PIN(MX53_PAD_DISP0_DAT16),
+	IMX_PINCTRL_PIN(MX53_PAD_DISP0_DAT17),
+	IMX_PINCTRL_PIN(MX53_PAD_DISP0_DAT18),
+	IMX_PINCTRL_PIN(MX53_PAD_DISP0_DAT19),
+	IMX_PINCTRL_PIN(MX53_PAD_DISP0_DAT20),
+	IMX_PINCTRL_PIN(MX53_PAD_DISP0_DAT21),
+	IMX_PINCTRL_PIN(MX53_PAD_DISP0_DAT22),
+	IMX_PINCTRL_PIN(MX53_PAD_DISP0_DAT23),
+	IMX_PINCTRL_PIN(MX53_PAD_CSI0_PIXCLK),
+	IMX_PINCTRL_PIN(MX53_PAD_CSI0_MCLK),
+	IMX_PINCTRL_PIN(MX53_PAD_CSI0_DATA_EN),
+	IMX_PINCTRL_PIN(MX53_PAD_CSI0_VSYNC),
+	IMX_PINCTRL_PIN(MX53_PAD_CSI0_DAT4),
+	IMX_PINCTRL_PIN(MX53_PAD_CSI0_DAT5),
+	IMX_PINCTRL_PIN(MX53_PAD_CSI0_DAT6),
+	IMX_PINCTRL_PIN(MX53_PAD_CSI0_DAT7),
+	IMX_PINCTRL_PIN(MX53_PAD_CSI0_DAT8),
+	IMX_PINCTRL_PIN(MX53_PAD_CSI0_DAT9),
+	IMX_PINCTRL_PIN(MX53_PAD_CSI0_DAT10),
+	IMX_PINCTRL_PIN(MX53_PAD_CSI0_DAT11),
+	IMX_PINCTRL_PIN(MX53_PAD_CSI0_DAT12),
+	IMX_PINCTRL_PIN(MX53_PAD_CSI0_DAT13),
+	IMX_PINCTRL_PIN(MX53_PAD_CSI0_DAT14),
+	IMX_PINCTRL_PIN(MX53_PAD_CSI0_DAT15),
+	IMX_PINCTRL_PIN(MX53_PAD_CSI0_DAT16),
+	IMX_PINCTRL_PIN(MX53_PAD_CSI0_DAT17),
+	IMX_PINCTRL_PIN(MX53_PAD_CSI0_DAT18),
+	IMX_PINCTRL_PIN(MX53_PAD_CSI0_DAT19),
+	IMX_PINCTRL_PIN(MX53_PAD_EIM_A25),
+	IMX_PINCTRL_PIN(MX53_PAD_EIM_EB2),
+	IMX_PINCTRL_PIN(MX53_PAD_EIM_D16),
+	IMX_PINCTRL_PIN(MX53_PAD_EIM_D17),
+	IMX_PINCTRL_PIN(MX53_PAD_EIM_D18),
+	IMX_PINCTRL_PIN(MX53_PAD_EIM_D19),
+	IMX_PINCTRL_PIN(MX53_PAD_EIM_D20),
+	IMX_PINCTRL_PIN(MX53_PAD_EIM_D21),
+	IMX_PINCTRL_PIN(MX53_PAD_EIM_D22),
+	IMX_PINCTRL_PIN(MX53_PAD_EIM_D23),
+	IMX_PINCTRL_PIN(MX53_PAD_EIM_EB3),
+	IMX_PINCTRL_PIN(MX53_PAD_EIM_D24),
+	IMX_PINCTRL_PIN(MX53_PAD_EIM_D25),
+	IMX_PINCTRL_PIN(MX53_PAD_EIM_D26),
+	IMX_PINCTRL_PIN(MX53_PAD_EIM_D27),
+	IMX_PINCTRL_PIN(MX53_PAD_EIM_D28),
+	IMX_PINCTRL_PIN(MX53_PAD_EIM_D29),
+	IMX_PINCTRL_PIN(MX53_PAD_EIM_D30),
+	IMX_PINCTRL_PIN(MX53_PAD_EIM_D31),
+	IMX_PINCTRL_PIN(MX53_PAD_EIM_A24),
+	IMX_PINCTRL_PIN(MX53_PAD_EIM_A23),
+	IMX_PINCTRL_PIN(MX53_PAD_EIM_A22),
+	IMX_PINCTRL_PIN(MX53_PAD_EIM_A21),
+	IMX_PINCTRL_PIN(MX53_PAD_EIM_A20),
+	IMX_PINCTRL_PIN(MX53_PAD_EIM_A19),
+	IMX_PINCTRL_PIN(MX53_PAD_EIM_A18),
+	IMX_PINCTRL_PIN(MX53_PAD_EIM_A17),
+	IMX_PINCTRL_PIN(MX53_PAD_EIM_A16),
+	IMX_PINCTRL_PIN(MX53_PAD_EIM_CS0),
+	IMX_PINCTRL_PIN(MX53_PAD_EIM_CS1),
+	IMX_PINCTRL_PIN(MX53_PAD_EIM_OE),
+	IMX_PINCTRL_PIN(MX53_PAD_EIM_RW),
+	IMX_PINCTRL_PIN(MX53_PAD_EIM_LBA),
+	IMX_PINCTRL_PIN(MX53_PAD_EIM_EB0),
+	IMX_PINCTRL_PIN(MX53_PAD_EIM_EB1),
+	IMX_PINCTRL_PIN(MX53_PAD_EIM_DA0),
+	IMX_PINCTRL_PIN(MX53_PAD_EIM_DA1),
+	IMX_PINCTRL_PIN(MX53_PAD_EIM_DA2),
+	IMX_PINCTRL_PIN(MX53_PAD_EIM_DA3),
+	IMX_PINCTRL_PIN(MX53_PAD_EIM_DA4),
+	IMX_PINCTRL_PIN(MX53_PAD_EIM_DA5),
+	IMX_PINCTRL_PIN(MX53_PAD_EIM_DA6),
+	IMX_PINCTRL_PIN(MX53_PAD_EIM_DA7),
+	IMX_PINCTRL_PIN(MX53_PAD_EIM_DA8),
+	IMX_PINCTRL_PIN(MX53_PAD_EIM_DA9),
+	IMX_PINCTRL_PIN(MX53_PAD_EIM_DA10),
+	IMX_PINCTRL_PIN(MX53_PAD_EIM_DA11),
+	IMX_PINCTRL_PIN(MX53_PAD_EIM_DA12),
+	IMX_PINCTRL_PIN(MX53_PAD_EIM_DA13),
+	IMX_PINCTRL_PIN(MX53_PAD_EIM_DA14),
+	IMX_PINCTRL_PIN(MX53_PAD_EIM_DA15),
+	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_LVDS1_TX3_P),
+	IMX_PINCTRL_PIN(MX53_PAD_LVDS1_TX2_P),
+	IMX_PINCTRL_PIN(MX53_PAD_LVDS1_CLK_P),
+	IMX_PINCTRL_PIN(MX53_PAD_LVDS1_TX1_P),
+	IMX_PINCTRL_PIN(MX53_PAD_LVDS1_TX0_P),
+	IMX_PINCTRL_PIN(MX53_PAD_LVDS0_TX3_P),
+	IMX_PINCTRL_PIN(MX53_PAD_LVDS0_CLK_P),
+	IMX_PINCTRL_PIN(MX53_PAD_LVDS0_TX2_P),
+	IMX_PINCTRL_PIN(MX53_PAD_LVDS0_TX1_P),
+	IMX_PINCTRL_PIN(MX53_PAD_LVDS0_TX0_P),
+	IMX_PINCTRL_PIN(MX53_PAD_GPIO_10),
+	IMX_PINCTRL_PIN(MX53_PAD_GPIO_11),
+	IMX_PINCTRL_PIN(MX53_PAD_GPIO_12),
+	IMX_PINCTRL_PIN(MX53_PAD_GPIO_13),
+	IMX_PINCTRL_PIN(MX53_PAD_GPIO_14),
+	IMX_PINCTRL_PIN(MX53_PAD_NANDF_CLE),
+	IMX_PINCTRL_PIN(MX53_PAD_NANDF_ALE),
+	IMX_PINCTRL_PIN(MX53_PAD_NANDF_WP_B),
+	IMX_PINCTRL_PIN(MX53_PAD_NANDF_RB0),
+	IMX_PINCTRL_PIN(MX53_PAD_NANDF_CS0),
+	IMX_PINCTRL_PIN(MX53_PAD_NANDF_CS1),
+	IMX_PINCTRL_PIN(MX53_PAD_NANDF_CS2),
+	IMX_PINCTRL_PIN(MX53_PAD_NANDF_CS3),
+	IMX_PINCTRL_PIN(MX53_PAD_FEC_MDIO),
+	IMX_PINCTRL_PIN(MX53_PAD_FEC_REF_CLK),
+	IMX_PINCTRL_PIN(MX53_PAD_FEC_RX_ER),
+	IMX_PINCTRL_PIN(MX53_PAD_FEC_CRS_DV),
+	IMX_PINCTRL_PIN(MX53_PAD_FEC_RXD1),
+	IMX_PINCTRL_PIN(MX53_PAD_FEC_RXD0),
+	IMX_PINCTRL_PIN(MX53_PAD_FEC_TX_EN),
+	IMX_PINCTRL_PIN(MX53_PAD_FEC_TXD1),
+	IMX_PINCTRL_PIN(MX53_PAD_FEC_TXD0),
+	IMX_PINCTRL_PIN(MX53_PAD_FEC_MDC),
+	IMX_PINCTRL_PIN(MX53_PAD_PATA_DIOW),
+	IMX_PINCTRL_PIN(MX53_PAD_PATA_DMACK),
+	IMX_PINCTRL_PIN(MX53_PAD_PATA_DMARQ),
+	IMX_PINCTRL_PIN(MX53_PAD_PATA_BUFFER_EN),
+	IMX_PINCTRL_PIN(MX53_PAD_PATA_INTRQ),
+	IMX_PINCTRL_PIN(MX53_PAD_PATA_DIOR),
+	IMX_PINCTRL_PIN(MX53_PAD_PATA_RESET_B),
+	IMX_PINCTRL_PIN(MX53_PAD_PATA_IORDY),
+	IMX_PINCTRL_PIN(MX53_PAD_PATA_DA_0),
+	IMX_PINCTRL_PIN(MX53_PAD_PATA_DA_1),
+	IMX_PINCTRL_PIN(MX53_PAD_PATA_DA_2),
+	IMX_PINCTRL_PIN(MX53_PAD_PATA_CS_0),
+	IMX_PINCTRL_PIN(MX53_PAD_PATA_CS_1),
+	IMX_PINCTRL_PIN(MX53_PAD_PATA_DATA0),
+	IMX_PINCTRL_PIN(MX53_PAD_PATA_DATA1),
+	IMX_PINCTRL_PIN(MX53_PAD_PATA_DATA2),
+	IMX_PINCTRL_PIN(MX53_PAD_PATA_DATA3),
+	IMX_PINCTRL_PIN(MX53_PAD_PATA_DATA4),
+	IMX_PINCTRL_PIN(MX53_PAD_PATA_DATA5),
+	IMX_PINCTRL_PIN(MX53_PAD_PATA_DATA6),
+	IMX_PINCTRL_PIN(MX53_PAD_PATA_DATA7),
+	IMX_PINCTRL_PIN(MX53_PAD_PATA_DATA8),
+	IMX_PINCTRL_PIN(MX53_PAD_PATA_DATA9),
+	IMX_PINCTRL_PIN(MX53_PAD_PATA_DATA10),
+	IMX_PINCTRL_PIN(MX53_PAD_PATA_DATA11),
+	IMX_PINCTRL_PIN(MX53_PAD_PATA_DATA12),
+	IMX_PINCTRL_PIN(MX53_PAD_PATA_DATA13),
+	IMX_PINCTRL_PIN(MX53_PAD_PATA_DATA14),
+	IMX_PINCTRL_PIN(MX53_PAD_PATA_DATA15),
+	IMX_PINCTRL_PIN(MX53_PAD_SD1_DATA0),
+	IMX_PINCTRL_PIN(MX53_PAD_SD1_DATA1),
+	IMX_PINCTRL_PIN(MX53_PAD_SD1_CMD),
+	IMX_PINCTRL_PIN(MX53_PAD_SD1_DATA2),
+	IMX_PINCTRL_PIN(MX53_PAD_SD1_CLK),
+	IMX_PINCTRL_PIN(MX53_PAD_SD1_DATA3),
+	IMX_PINCTRL_PIN(MX53_PAD_SD2_CLK),
+	IMX_PINCTRL_PIN(MX53_PAD_SD2_CMD),
+	IMX_PINCTRL_PIN(MX53_PAD_SD2_DATA3),
+	IMX_PINCTRL_PIN(MX53_PAD_SD2_DATA2),
+	IMX_PINCTRL_PIN(MX53_PAD_SD2_DATA1),
+	IMX_PINCTRL_PIN(MX53_PAD_SD2_DATA0),
+	IMX_PINCTRL_PIN(MX53_PAD_GPIO_0),
+	IMX_PINCTRL_PIN(MX53_PAD_GPIO_1),
+	IMX_PINCTRL_PIN(MX53_PAD_GPIO_9),
+	IMX_PINCTRL_PIN(MX53_PAD_GPIO_3),
+	IMX_PINCTRL_PIN(MX53_PAD_GPIO_6),
+	IMX_PINCTRL_PIN(MX53_PAD_GPIO_2),
+	IMX_PINCTRL_PIN(MX53_PAD_GPIO_4),
+	IMX_PINCTRL_PIN(MX53_PAD_GPIO_5),
+	IMX_PINCTRL_PIN(MX53_PAD_GPIO_7),
+	IMX_PINCTRL_PIN(MX53_PAD_GPIO_8),
+	IMX_PINCTRL_PIN(MX53_PAD_GPIO_16),
+	IMX_PINCTRL_PIN(MX53_PAD_GPIO_17),
+	IMX_PINCTRL_PIN(MX53_PAD_GPIO_18),
+};
+
+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[] __devinitdata = {
+	{ .compatible = "fsl,imx53-iomuxc", },
+	{ /* sentinel */ }
+};
+
+static int __devinit imx53_pinctrl_probe(struct platform_device *pdev)
+{
+	return imx_pinctrl_probe(pdev, &imx53_pinctrl_info);
+}
+
+static struct platform_driver imx53_pinctrl_driver = {
+	.driver = {
+		.name = "imx53-pinctrl",
+		.owner = THIS_MODULE,
+		.of_match_table = of_match_ptr(imx53_pinctrl_of_match),
+	},
+	.probe = imx53_pinctrl_probe,
+	.remove = __devexit_p(imx_pinctrl_remove),
+};
+
+static int __init imx53_pinctrl_init(void)
+{
+	return platform_driver_register(&imx53_pinctrl_driver);
+}
+arch_initcall(imx53_pinctrl_init);
+
+static void __exit imx53_pinctrl_exit(void)
+{
+	platform_driver_unregister(&imx53_pinctrl_driver);
+}
+module_exit(imx53_pinctrl_exit);
+MODULE_AUTHOR("Dong Aisheng <dong.aisheng@linaro.org>");
+MODULE_DESCRIPTION("Freescale IMX53 pinctrl driver");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/pinctrl/pinctrl-imx6q.c b/drivers/pinctrl/pinctrl-imx6q.c
new file mode 100644
index 00000000000..7737d4d71a3
--- /dev/null
+++ b/drivers/pinctrl/pinctrl-imx6q.c
@@ -0,0 +1,2331 @@
+/*
+ * imx6q pinctrl driver based on imx pinmux core
+ *
+ * Copyright (C) 2012 Freescale Semiconductor, Inc.
+ * Copyright (C) 2012 Linaro, Inc.
+ *
+ * Author: Dong Aisheng <dong.aisheng@linaro.org>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ */
+
+#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 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 */
+};
+
+/* Pad names for the pinmux subsystem */
+static const struct pinctrl_pin_desc imx6q_pinctrl_pads[] = {
+	IMX_PINCTRL_PIN(MX6Q_PAD_SD2_DAT1),
+	IMX_PINCTRL_PIN(MX6Q_PAD_SD2_DAT2),
+	IMX_PINCTRL_PIN(MX6Q_PAD_SD2_DAT0),
+	IMX_PINCTRL_PIN(MX6Q_PAD_RGMII_TXC),
+	IMX_PINCTRL_PIN(MX6Q_PAD_RGMII_TD0),
+	IMX_PINCTRL_PIN(MX6Q_PAD_RGMII_TD1),
+	IMX_PINCTRL_PIN(MX6Q_PAD_RGMII_TD2),
+	IMX_PINCTRL_PIN(MX6Q_PAD_RGMII_TD3),
+	IMX_PINCTRL_PIN(MX6Q_PAD_RGMII_RX_CTL),
+	IMX_PINCTRL_PIN(MX6Q_PAD_RGMII_RD0),
+	IMX_PINCTRL_PIN(MX6Q_PAD_RGMII_TX_CTL),
+	IMX_PINCTRL_PIN(MX6Q_PAD_RGMII_RD1),
+	IMX_PINCTRL_PIN(MX6Q_PAD_RGMII_RD2),
+	IMX_PINCTRL_PIN(MX6Q_PAD_RGMII_RD3),
+	IMX_PINCTRL_PIN(MX6Q_PAD_RGMII_RXC),
+	IMX_PINCTRL_PIN(MX6Q_PAD_EIM_A25),
+	IMX_PINCTRL_PIN(MX6Q_PAD_EIM_EB2),
+	IMX_PINCTRL_PIN(MX6Q_PAD_EIM_D16),
+	IMX_PINCTRL_PIN(MX6Q_PAD_EIM_D17),
+	IMX_PINCTRL_PIN(MX6Q_PAD_EIM_D18),
+	IMX_PINCTRL_PIN(MX6Q_PAD_EIM_D19),
+	IMX_PINCTRL_PIN(MX6Q_PAD_EIM_D20),
+	IMX_PINCTRL_PIN(MX6Q_PAD_EIM_D21),
+	IMX_PINCTRL_PIN(MX6Q_PAD_EIM_D22),
+	IMX_PINCTRL_PIN(MX6Q_PAD_EIM_D23),
+	IMX_PINCTRL_PIN(MX6Q_PAD_EIM_EB3),
+	IMX_PINCTRL_PIN(MX6Q_PAD_EIM_D24),
+	IMX_PINCTRL_PIN(MX6Q_PAD_EIM_D25),
+	IMX_PINCTRL_PIN(MX6Q_PAD_EIM_D26),
+	IMX_PINCTRL_PIN(MX6Q_PAD_EIM_D27),
+	IMX_PINCTRL_PIN(MX6Q_PAD_EIM_D28),
+	IMX_PINCTRL_PIN(MX6Q_PAD_EIM_D29),
+	IMX_PINCTRL_PIN(MX6Q_PAD_EIM_D30),
+	IMX_PINCTRL_PIN(MX6Q_PAD_EIM_D31),
+	IMX_PINCTRL_PIN(MX6Q_PAD_EIM_A24),
+	IMX_PINCTRL_PIN(MX6Q_PAD_EIM_A23),
+	IMX_PINCTRL_PIN(MX6Q_PAD_EIM_A22),
+	IMX_PINCTRL_PIN(MX6Q_PAD_EIM_A21),
+	IMX_PINCTRL_PIN(MX6Q_PAD_EIM_A20),
+	IMX_PINCTRL_PIN(MX6Q_PAD_EIM_A19),
+	IMX_PINCTRL_PIN(MX6Q_PAD_EIM_A18),
+	IMX_PINCTRL_PIN(MX6Q_PAD_EIM_A17),
+	IMX_PINCTRL_PIN(MX6Q_PAD_EIM_A16),
+	IMX_PINCTRL_PIN(MX6Q_PAD_EIM_CS0),
+	IMX_PINCTRL_PIN(MX6Q_PAD_EIM_CS1),
+	IMX_PINCTRL_PIN(MX6Q_PAD_EIM_OE),
+	IMX_PINCTRL_PIN(MX6Q_PAD_EIM_RW),
+	IMX_PINCTRL_PIN(MX6Q_PAD_EIM_LBA),
+	IMX_PINCTRL_PIN(MX6Q_PAD_EIM_EB0),
+	IMX_PINCTRL_PIN(MX6Q_PAD_EIM_EB1),
+	IMX_PINCTRL_PIN(MX6Q_PAD_EIM_DA0),
+	IMX_PINCTRL_PIN(MX6Q_PAD_EIM_DA1),
+	IMX_PINCTRL_PIN(MX6Q_PAD_EIM_DA2),
+	IMX_PINCTRL_PIN(MX6Q_PAD_EIM_DA3),
+	IMX_PINCTRL_PIN(MX6Q_PAD_EIM_DA4),
+	IMX_PINCTRL_PIN(MX6Q_PAD_EIM_DA5),
+	IMX_PINCTRL_PIN(MX6Q_PAD_EIM_DA6),
+	IMX_PINCTRL_PIN(MX6Q_PAD_EIM_DA7),
+	IMX_PINCTRL_PIN(MX6Q_PAD_EIM_DA8),
+	IMX_PINCTRL_PIN(MX6Q_PAD_EIM_DA9),
+	IMX_PINCTRL_PIN(MX6Q_PAD_EIM_DA10),
+	IMX_PINCTRL_PIN(MX6Q_PAD_EIM_DA11),
+	IMX_PINCTRL_PIN(MX6Q_PAD_EIM_DA12),
+	IMX_PINCTRL_PIN(MX6Q_PAD_EIM_DA13),
+	IMX_PINCTRL_PIN(MX6Q_PAD_EIM_DA14),
+	IMX_PINCTRL_PIN(MX6Q_PAD_EIM_DA15),
+	IMX_PINCTRL_PIN(MX6Q_PAD_EIM_WAIT),
+	IMX_PINCTRL_PIN(MX6Q_PAD_EIM_BCLK),
+	IMX_PINCTRL_PIN(MX6Q_PAD_DI0_DISP_CLK),
+	IMX_PINCTRL_PIN(MX6Q_PAD_DI0_PIN15),
+	IMX_PINCTRL_PIN(MX6Q_PAD_DI0_PIN2),
+	IMX_PINCTRL_PIN(MX6Q_PAD_DI0_PIN3),
+	IMX_PINCTRL_PIN(MX6Q_PAD_DI0_PIN4),
+	IMX_PINCTRL_PIN(MX6Q_PAD_DISP0_DAT0),
+	IMX_PINCTRL_PIN(MX6Q_PAD_DISP0_DAT1),
+	IMX_PINCTRL_PIN(MX6Q_PAD_DISP0_DAT2),
+	IMX_PINCTRL_PIN(MX6Q_PAD_DISP0_DAT3),
+	IMX_PINCTRL_PIN(MX6Q_PAD_DISP0_DAT4),
+	IMX_PINCTRL_PIN(MX6Q_PAD_DISP0_DAT5),
+	IMX_PINCTRL_PIN(MX6Q_PAD_DISP0_DAT6),
+	IMX_PINCTRL_PIN(MX6Q_PAD_DISP0_DAT7),
+	IMX_PINCTRL_PIN(MX6Q_PAD_DISP0_DAT8),
+	IMX_PINCTRL_PIN(MX6Q_PAD_DISP0_DAT9),
+	IMX_PINCTRL_PIN(MX6Q_PAD_DISP0_DAT10),
+	IMX_PINCTRL_PIN(MX6Q_PAD_DISP0_DAT11),
+	IMX_PINCTRL_PIN(MX6Q_PAD_DISP0_DAT12),
+	IMX_PINCTRL_PIN(MX6Q_PAD_DISP0_DAT13),
+	IMX_PINCTRL_PIN(MX6Q_PAD_DISP0_DAT14),
+	IMX_PINCTRL_PIN(MX6Q_PAD_DISP0_DAT15),
+	IMX_PINCTRL_PIN(MX6Q_PAD_DISP0_DAT16),
+	IMX_PINCTRL_PIN(MX6Q_PAD_DISP0_DAT17),
+	IMX_PINCTRL_PIN(MX6Q_PAD_DISP0_DAT18),
+	IMX_PINCTRL_PIN(MX6Q_PAD_DISP0_DAT19),
+	IMX_PINCTRL_PIN(MX6Q_PAD_DISP0_DAT20),
+	IMX_PINCTRL_PIN(MX6Q_PAD_DISP0_DAT21),
+	IMX_PINCTRL_PIN(MX6Q_PAD_DISP0_DAT22),
+	IMX_PINCTRL_PIN(MX6Q_PAD_DISP0_DAT23),
+	IMX_PINCTRL_PIN(MX6Q_PAD_ENET_MDIO),
+	IMX_PINCTRL_PIN(MX6Q_PAD_ENET_REF_CLK),
+	IMX_PINCTRL_PIN(MX6Q_PAD_ENET_RX_ER),
+	IMX_PINCTRL_PIN(MX6Q_PAD_ENET_CRS_DV),
+	IMX_PINCTRL_PIN(MX6Q_PAD_ENET_RXD1),
+	IMX_PINCTRL_PIN(MX6Q_PAD_ENET_RXD0),
+	IMX_PINCTRL_PIN(MX6Q_PAD_ENET_TX_EN),
+	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),
+	IMX_PINCTRL_PIN(MX6Q_PAD_KEY_ROW1),
+	IMX_PINCTRL_PIN(MX6Q_PAD_KEY_COL2),
+	IMX_PINCTRL_PIN(MX6Q_PAD_KEY_ROW2),
+	IMX_PINCTRL_PIN(MX6Q_PAD_KEY_COL3),
+	IMX_PINCTRL_PIN(MX6Q_PAD_KEY_ROW3),
+	IMX_PINCTRL_PIN(MX6Q_PAD_KEY_COL4),
+	IMX_PINCTRL_PIN(MX6Q_PAD_KEY_ROW4),
+	IMX_PINCTRL_PIN(MX6Q_PAD_GPIO_0),
+	IMX_PINCTRL_PIN(MX6Q_PAD_GPIO_1),
+	IMX_PINCTRL_PIN(MX6Q_PAD_GPIO_9),
+	IMX_PINCTRL_PIN(MX6Q_PAD_GPIO_3),
+	IMX_PINCTRL_PIN(MX6Q_PAD_GPIO_6),
+	IMX_PINCTRL_PIN(MX6Q_PAD_GPIO_2),
+	IMX_PINCTRL_PIN(MX6Q_PAD_GPIO_4),
+	IMX_PINCTRL_PIN(MX6Q_PAD_GPIO_5),
+	IMX_PINCTRL_PIN(MX6Q_PAD_GPIO_7),
+	IMX_PINCTRL_PIN(MX6Q_PAD_GPIO_8),
+	IMX_PINCTRL_PIN(MX6Q_PAD_GPIO_16),
+	IMX_PINCTRL_PIN(MX6Q_PAD_GPIO_17),
+	IMX_PINCTRL_PIN(MX6Q_PAD_GPIO_18),
+	IMX_PINCTRL_PIN(MX6Q_PAD_GPIO_19),
+	IMX_PINCTRL_PIN(MX6Q_PAD_CSI0_PIXCLK),
+	IMX_PINCTRL_PIN(MX6Q_PAD_CSI0_MCLK),
+	IMX_PINCTRL_PIN(MX6Q_PAD_CSI0_DATA_EN),
+	IMX_PINCTRL_PIN(MX6Q_PAD_CSI0_VSYNC),
+	IMX_PINCTRL_PIN(MX6Q_PAD_CSI0_DAT4),
+	IMX_PINCTRL_PIN(MX6Q_PAD_CSI0_DAT5),
+	IMX_PINCTRL_PIN(MX6Q_PAD_CSI0_DAT6),
+	IMX_PINCTRL_PIN(MX6Q_PAD_CSI0_DAT7),
+	IMX_PINCTRL_PIN(MX6Q_PAD_CSI0_DAT8),
+	IMX_PINCTRL_PIN(MX6Q_PAD_CSI0_DAT9),
+	IMX_PINCTRL_PIN(MX6Q_PAD_CSI0_DAT10),
+	IMX_PINCTRL_PIN(MX6Q_PAD_CSI0_DAT11),
+	IMX_PINCTRL_PIN(MX6Q_PAD_CSI0_DAT12),
+	IMX_PINCTRL_PIN(MX6Q_PAD_CSI0_DAT13),
+	IMX_PINCTRL_PIN(MX6Q_PAD_CSI0_DAT14),
+	IMX_PINCTRL_PIN(MX6Q_PAD_CSI0_DAT15),
+	IMX_PINCTRL_PIN(MX6Q_PAD_CSI0_DAT16),
+	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),
+	IMX_PINCTRL_PIN(MX6Q_PAD_SD3_DAT4),
+	IMX_PINCTRL_PIN(MX6Q_PAD_SD3_CMD),
+	IMX_PINCTRL_PIN(MX6Q_PAD_SD3_CLK),
+	IMX_PINCTRL_PIN(MX6Q_PAD_SD3_DAT0),
+	IMX_PINCTRL_PIN(MX6Q_PAD_SD3_DAT1),
+	IMX_PINCTRL_PIN(MX6Q_PAD_SD3_DAT2),
+	IMX_PINCTRL_PIN(MX6Q_PAD_SD3_DAT3),
+	IMX_PINCTRL_PIN(MX6Q_PAD_SD3_RST),
+	IMX_PINCTRL_PIN(MX6Q_PAD_NANDF_CLE),
+	IMX_PINCTRL_PIN(MX6Q_PAD_NANDF_ALE),
+	IMX_PINCTRL_PIN(MX6Q_PAD_NANDF_WP_B),
+	IMX_PINCTRL_PIN(MX6Q_PAD_NANDF_RB0),
+	IMX_PINCTRL_PIN(MX6Q_PAD_NANDF_CS0),
+	IMX_PINCTRL_PIN(MX6Q_PAD_NANDF_CS1),
+	IMX_PINCTRL_PIN(MX6Q_PAD_NANDF_CS2),
+	IMX_PINCTRL_PIN(MX6Q_PAD_NANDF_CS3),
+	IMX_PINCTRL_PIN(MX6Q_PAD_SD4_CMD),
+	IMX_PINCTRL_PIN(MX6Q_PAD_SD4_CLK),
+	IMX_PINCTRL_PIN(MX6Q_PAD_NANDF_D0),
+	IMX_PINCTRL_PIN(MX6Q_PAD_NANDF_D1),
+	IMX_PINCTRL_PIN(MX6Q_PAD_NANDF_D2),
+	IMX_PINCTRL_PIN(MX6Q_PAD_NANDF_D3),
+	IMX_PINCTRL_PIN(MX6Q_PAD_NANDF_D4),
+	IMX_PINCTRL_PIN(MX6Q_PAD_NANDF_D5),
+	IMX_PINCTRL_PIN(MX6Q_PAD_NANDF_D6),
+	IMX_PINCTRL_PIN(MX6Q_PAD_NANDF_D7),
+	IMX_PINCTRL_PIN(MX6Q_PAD_SD4_DAT0),
+	IMX_PINCTRL_PIN(MX6Q_PAD_SD4_DAT1),
+	IMX_PINCTRL_PIN(MX6Q_PAD_SD4_DAT2),
+	IMX_PINCTRL_PIN(MX6Q_PAD_SD4_DAT3),
+	IMX_PINCTRL_PIN(MX6Q_PAD_SD4_DAT4),
+	IMX_PINCTRL_PIN(MX6Q_PAD_SD4_DAT5),
+	IMX_PINCTRL_PIN(MX6Q_PAD_SD4_DAT6),
+	IMX_PINCTRL_PIN(MX6Q_PAD_SD4_DAT7),
+	IMX_PINCTRL_PIN(MX6Q_PAD_SD1_DAT1),
+	IMX_PINCTRL_PIN(MX6Q_PAD_SD1_DAT0),
+	IMX_PINCTRL_PIN(MX6Q_PAD_SD1_DAT3),
+	IMX_PINCTRL_PIN(MX6Q_PAD_SD1_CMD),
+	IMX_PINCTRL_PIN(MX6Q_PAD_SD1_DAT2),
+	IMX_PINCTRL_PIN(MX6Q_PAD_SD1_CLK),
+	IMX_PINCTRL_PIN(MX6Q_PAD_SD2_CLK),
+	IMX_PINCTRL_PIN(MX6Q_PAD_SD2_CMD),
+	IMX_PINCTRL_PIN(MX6Q_PAD_SD2_DAT3),
+};
+
+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[] __devinitdata = {
+	{ .compatible = "fsl,imx6q-iomuxc", },
+	{ /* sentinel */ }
+};
+
+static int __devinit imx6q_pinctrl_probe(struct platform_device *pdev)
+{
+	return imx_pinctrl_probe(pdev, &imx6q_pinctrl_info);
+}
+
+static struct platform_driver imx6q_pinctrl_driver = {
+	.driver = {
+		.name = "imx6q-pinctrl",
+		.owner = THIS_MODULE,
+		.of_match_table = of_match_ptr(imx6q_pinctrl_of_match),
+	},
+	.probe = imx6q_pinctrl_probe,
+	.remove = __devexit_p(imx_pinctrl_remove),
+};
+
+static int __init imx6q_pinctrl_init(void)
+{
+	return platform_driver_register(&imx6q_pinctrl_driver);
+}
+arch_initcall(imx6q_pinctrl_init);
+
+static void __exit imx6q_pinctrl_exit(void)
+{
+	platform_driver_unregister(&imx6q_pinctrl_driver);
+}
+module_exit(imx6q_pinctrl_exit);
+MODULE_AUTHOR("Dong Aisheng <dong.aisheng@linaro.org>");
+MODULE_DESCRIPTION("Freescale IMX6Q pinctrl driver");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/pinctrl/pinctrl-mxs.c b/drivers/pinctrl/pinctrl-mxs.c
new file mode 100644
index 00000000000..556e45a213e
--- /dev/null
+++ b/drivers/pinctrl/pinctrl-mxs.c
@@ -0,0 +1,528 @@
+/*
+ * Copyright 2012 Freescale Semiconductor, Inc.
+ *
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/gpl-license.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+
+#include <linux/err.h>
+#include <linux/init.h>
+#include <linux/io.h>
+#include <linux/module.h>
+#include <linux/of.h>
+#include <linux/of_address.h>
+#include <linux/pinctrl/machine.h>
+#include <linux/pinctrl/pinconf.h>
+#include <linux/pinctrl/pinctrl.h>
+#include <linux/pinctrl/pinmux.h>
+#include <linux/platform_device.h>
+#include <linux/slab.h>
+#include "core.h"
+#include "pinctrl-mxs.h"
+
+#define SUFFIX_LEN	4
+
+struct mxs_pinctrl_data {
+	struct device *dev;
+	struct pinctrl_dev *pctl;
+	void __iomem *base;
+	struct mxs_pinctrl_soc_data *soc;
+};
+
+static int mxs_get_groups_count(struct pinctrl_dev *pctldev)
+{
+	struct mxs_pinctrl_data *d = pinctrl_dev_get_drvdata(pctldev);
+
+	return d->soc->ngroups;
+}
+
+static const char *mxs_get_group_name(struct pinctrl_dev *pctldev,
+				      unsigned group)
+{
+	struct mxs_pinctrl_data *d = pinctrl_dev_get_drvdata(pctldev);
+
+	return d->soc->groups[group].name;
+}
+
+static int mxs_get_group_pins(struct pinctrl_dev *pctldev, unsigned group,
+			      const unsigned **pins, unsigned *num_pins)
+{
+	struct mxs_pinctrl_data *d = pinctrl_dev_get_drvdata(pctldev);
+
+	*pins = d->soc->groups[group].pins;
+	*num_pins = d->soc->groups[group].npins;
+
+	return 0;
+}
+
+static void mxs_pin_dbg_show(struct pinctrl_dev *pctldev, struct seq_file *s,
+			     unsigned offset)
+{
+	seq_printf(s, " %s", dev_name(pctldev->dev));
+}
+
+static int mxs_dt_node_to_map(struct pinctrl_dev *pctldev,
+			      struct device_node *np,
+			      struct pinctrl_map **map, unsigned *num_maps)
+{
+	struct pinctrl_map *new_map;
+	char *group = NULL;
+	unsigned new_num = 1;
+	unsigned long config = 0;
+	unsigned long *pconfig;
+	int length = strlen(np->name) + SUFFIX_LEN;
+	bool purecfg = false;
+	u32 val, reg;
+	int ret, i = 0;
+
+	/* Check for pin config node which has no 'reg' property */
+	if (of_property_read_u32(np, "reg", &reg))
+		purecfg = true;
+
+	ret = of_property_read_u32(np, "fsl,drive-strength", &val);
+	if (!ret)
+		config = val | MA_PRESENT;
+	ret = of_property_read_u32(np, "fsl,voltage", &val);
+	if (!ret)
+		config |= val << VOL_SHIFT | VOL_PRESENT;
+	ret = of_property_read_u32(np, "fsl,pull-up", &val);
+	if (!ret)
+		config |= val << PULL_SHIFT | PULL_PRESENT;
+
+	/* Check for group node which has both mux and config settings */
+	if (!purecfg && config)
+		new_num = 2;
+
+	new_map = kzalloc(sizeof(*new_map) * new_num, GFP_KERNEL);
+	if (!new_map)
+		return -ENOMEM;
+
+	if (!purecfg) {
+		new_map[i].type = PIN_MAP_TYPE_MUX_GROUP;
+		new_map[i].data.mux.function = np->name;
+
+		/* Compose group name */
+		group = kzalloc(length, GFP_KERNEL);
+		if (!group)
+			return -ENOMEM;
+		snprintf(group, length, "%s.%d", np->name, reg);
+		new_map[i].data.mux.group = group;
+		i++;
+	}
+
+	if (config) {
+		pconfig = kmemdup(&config, sizeof(config), GFP_KERNEL);
+		if (!pconfig) {
+			ret = -ENOMEM;
+			goto free;
+		}
+
+		new_map[i].type = PIN_MAP_TYPE_CONFIGS_GROUP;
+		new_map[i].data.configs.group_or_pin = purecfg ? np->name :
+								 group;
+		new_map[i].data.configs.configs = pconfig;
+		new_map[i].data.configs.num_configs = 1;
+	}
+
+	*map = new_map;
+	*num_maps = new_num;
+
+	return 0;
+
+free:
+	kfree(new_map);
+	return ret;
+}
+
+static void mxs_dt_free_map(struct pinctrl_dev *pctldev,
+			    struct pinctrl_map *map, unsigned num_maps)
+{
+	int i;
+
+	for (i = 0; i < num_maps; i++) {
+		if (map[i].type == PIN_MAP_TYPE_MUX_GROUP)
+			kfree(map[i].data.mux.group);
+		if (map[i].type == PIN_MAP_TYPE_CONFIGS_GROUP)
+			kfree(map[i].data.configs.configs);
+	}
+
+	kfree(map);
+}
+
+static struct pinctrl_ops mxs_pinctrl_ops = {
+	.get_groups_count = mxs_get_groups_count,
+	.get_group_name = mxs_get_group_name,
+	.get_group_pins = mxs_get_group_pins,
+	.pin_dbg_show = mxs_pin_dbg_show,
+	.dt_node_to_map = mxs_dt_node_to_map,
+	.dt_free_map = mxs_dt_free_map,
+};
+
+static int mxs_pinctrl_get_funcs_count(struct pinctrl_dev *pctldev)
+{
+	struct mxs_pinctrl_data *d = pinctrl_dev_get_drvdata(pctldev);
+
+	return d->soc->nfunctions;
+}
+
+static const char *mxs_pinctrl_get_func_name(struct pinctrl_dev *pctldev,
+					     unsigned function)
+{
+	struct mxs_pinctrl_data *d = pinctrl_dev_get_drvdata(pctldev);
+
+	return d->soc->functions[function].name;
+}
+
+static int mxs_pinctrl_get_func_groups(struct pinctrl_dev *pctldev,
+				       unsigned group,
+				       const char * const **groups,
+				       unsigned * const num_groups)
+{
+	struct mxs_pinctrl_data *d = pinctrl_dev_get_drvdata(pctldev);
+
+	*groups = d->soc->functions[group].groups;
+	*num_groups = d->soc->functions[group].ngroups;
+
+	return 0;
+}
+
+static int mxs_pinctrl_enable(struct pinctrl_dev *pctldev, unsigned selector,
+			      unsigned group)
+{
+	struct mxs_pinctrl_data *d = pinctrl_dev_get_drvdata(pctldev);
+	struct mxs_group *g = &d->soc->groups[group];
+	void __iomem *reg;
+	u8 bank, shift;
+	u16 pin;
+	int i;
+
+	for (i = 0; i < g->npins; i++) {
+		bank = PINID_TO_BANK(g->pins[i]);
+		pin = PINID_TO_PIN(g->pins[i]);
+		reg = d->base + d->soc->regs->muxsel;
+		reg += bank * 0x20 + pin / 16 * 0x10;
+		shift = pin % 16 * 2;
+
+		writel(0x3 << shift, reg + CLR);
+		writel(g->muxsel[i] << shift, reg + SET);
+	}
+
+	return 0;
+}
+
+static struct pinmux_ops mxs_pinmux_ops = {
+	.get_functions_count = mxs_pinctrl_get_funcs_count,
+	.get_function_name = mxs_pinctrl_get_func_name,
+	.get_function_groups = mxs_pinctrl_get_func_groups,
+	.enable = mxs_pinctrl_enable,
+};
+
+static int mxs_pinconf_get(struct pinctrl_dev *pctldev,
+			   unsigned pin, unsigned long *config)
+{
+	return -ENOTSUPP;
+}
+
+static int mxs_pinconf_set(struct pinctrl_dev *pctldev,
+			   unsigned pin, unsigned long config)
+{
+	return -ENOTSUPP;
+}
+
+static int mxs_pinconf_group_get(struct pinctrl_dev *pctldev,
+				 unsigned group, unsigned long *config)
+{
+	struct mxs_pinctrl_data *d = pinctrl_dev_get_drvdata(pctldev);
+
+	*config = d->soc->groups[group].config;
+
+	return 0;
+}
+
+static int mxs_pinconf_group_set(struct pinctrl_dev *pctldev,
+				 unsigned group, unsigned long config)
+{
+	struct mxs_pinctrl_data *d = pinctrl_dev_get_drvdata(pctldev);
+	struct mxs_group *g = &d->soc->groups[group];
+	void __iomem *reg;
+	u8 ma, vol, pull, bank, shift;
+	u16 pin;
+	int i;
+
+	ma = CONFIG_TO_MA(config);
+	vol = CONFIG_TO_VOL(config);
+	pull = CONFIG_TO_PULL(config);
+
+	for (i = 0; i < g->npins; i++) {
+		bank = PINID_TO_BANK(g->pins[i]);
+		pin = PINID_TO_PIN(g->pins[i]);
+
+		/* drive */
+		reg = d->base + d->soc->regs->drive;
+		reg += bank * 0x40 + pin / 8 * 0x10;
+
+		/* mA */
+		if (config & MA_PRESENT) {
+			shift = pin % 8 * 4;
+			writel(0x3 << shift, reg + CLR);
+			writel(ma << shift, reg + SET);
+		}
+
+		/* vol */
+		if (config & VOL_PRESENT) {
+			shift = pin % 8 * 4 + 2;
+			if (vol)
+				writel(1 << shift, reg + SET);
+			else
+				writel(1 << shift, reg + CLR);
+		}
+
+		/* pull */
+		if (config & PULL_PRESENT) {
+			reg = d->base + d->soc->regs->pull;
+			reg += bank * 0x10;
+			shift = pin;
+			if (pull)
+				writel(1 << shift, reg + SET);
+			else
+				writel(1 << shift, reg + CLR);
+		}
+	}
+
+	/* cache the config value for mxs_pinconf_group_get() */
+	g->config = config;
+
+	return 0;
+}
+
+static void mxs_pinconf_dbg_show(struct pinctrl_dev *pctldev,
+				 struct seq_file *s, unsigned pin)
+{
+	/* Not support */
+}
+
+static void mxs_pinconf_group_dbg_show(struct pinctrl_dev *pctldev,
+				       struct seq_file *s, unsigned group)
+{
+	unsigned long config;
+
+	if (!mxs_pinconf_group_get(pctldev, group, &config))
+		seq_printf(s, "0x%lx", config);
+}
+
+struct pinconf_ops mxs_pinconf_ops = {
+	.pin_config_get = mxs_pinconf_get,
+	.pin_config_set = mxs_pinconf_set,
+	.pin_config_group_get = mxs_pinconf_group_get,
+	.pin_config_group_set = mxs_pinconf_group_set,
+	.pin_config_dbg_show = mxs_pinconf_dbg_show,
+	.pin_config_group_dbg_show = mxs_pinconf_group_dbg_show,
+};
+
+static struct pinctrl_desc mxs_pinctrl_desc = {
+	.pctlops = &mxs_pinctrl_ops,
+	.pmxops = &mxs_pinmux_ops,
+	.confops = &mxs_pinconf_ops,
+	.owner = THIS_MODULE,
+};
+
+static int __devinit mxs_pinctrl_parse_group(struct platform_device *pdev,
+					     struct device_node *np, int idx,
+					     const char **out_name)
+{
+	struct mxs_pinctrl_data *d = platform_get_drvdata(pdev);
+	struct mxs_group *g = &d->soc->groups[idx];
+	struct property *prop;
+	const char *propname = "fsl,pinmux-ids";
+	char *group;
+	int length = strlen(np->name) + SUFFIX_LEN;
+	int i;
+	u32 val;
+
+	group = devm_kzalloc(&pdev->dev, length, GFP_KERNEL);
+	if (!group)
+		return -ENOMEM;
+	if (of_property_read_u32(np, "reg", &val))
+		snprintf(group, length, "%s", np->name);
+	else
+		snprintf(group, length, "%s.%d", np->name, val);
+	g->name = group;
+
+	prop = of_find_property(np, propname, &length);
+	if (!prop)
+		return -EINVAL;
+	g->npins = length / sizeof(u32);
+
+	g->pins = devm_kzalloc(&pdev->dev, g->npins * sizeof(*g->pins),
+			       GFP_KERNEL);
+	if (!g->pins)
+		return -ENOMEM;
+
+	g->muxsel = devm_kzalloc(&pdev->dev, g->npins * sizeof(*g->muxsel),
+				 GFP_KERNEL);
+	if (!g->muxsel)
+		return -ENOMEM;
+
+	of_property_read_u32_array(np, propname, g->pins, g->npins);
+	for (i = 0; i < g->npins; i++) {
+		g->muxsel[i] = MUXID_TO_MUXSEL(g->pins[i]);
+		g->pins[i] = MUXID_TO_PINID(g->pins[i]);
+	}
+
+	if (out_name)
+		*out_name = g->name;
+
+	return 0;
+}
+
+static int __devinit mxs_pinctrl_probe_dt(struct platform_device *pdev,
+					  struct mxs_pinctrl_data *d)
+{
+	struct mxs_pinctrl_soc_data *soc = d->soc;
+	struct device_node *np = pdev->dev.of_node;
+	struct device_node *child;
+	struct mxs_function *f;
+	const char *gpio_compat = "fsl,mxs-gpio";
+	const char *fn, *fnull = "";
+	int i = 0, idxf = 0, idxg = 0;
+	int ret;
+	u32 val;
+
+	child = of_get_next_child(np, NULL);
+	if (!child) {
+		dev_err(&pdev->dev, "no group is defined\n");
+		return -ENOENT;
+	}
+
+	/* Count total functions and groups */
+	fn = fnull;
+	for_each_child_of_node(np, child) {
+		if (of_device_is_compatible(child, gpio_compat))
+			continue;
+		soc->ngroups++;
+		/* Skip pure pinconf node */
+		if (of_property_read_u32(child, "reg", &val))
+			continue;
+		if (strcmp(fn, child->name)) {
+			fn = child->name;
+			soc->nfunctions++;
+		}
+	}
+
+	soc->functions = devm_kzalloc(&pdev->dev, soc->nfunctions *
+				      sizeof(*soc->functions), GFP_KERNEL);
+	if (!soc->functions)
+		return -ENOMEM;
+
+	soc->groups = devm_kzalloc(&pdev->dev, soc->ngroups *
+				   sizeof(*soc->groups), GFP_KERNEL);
+	if (!soc->groups)
+		return -ENOMEM;
+
+	/* Count groups for each function */
+	fn = fnull;
+	f = &soc->functions[idxf];
+	for_each_child_of_node(np, child) {
+		if (of_device_is_compatible(child, gpio_compat))
+			continue;
+		if (of_property_read_u32(child, "reg", &val))
+			continue;
+		if (strcmp(fn, child->name)) {
+			f = &soc->functions[idxf++];
+			f->name = fn = child->name;
+		}
+		f->ngroups++;
+	};
+
+	/* Get groups for each function */
+	idxf = 0;
+	fn = fnull;
+	for_each_child_of_node(np, child) {
+		if (of_device_is_compatible(child, gpio_compat))
+			continue;
+		if (of_property_read_u32(child, "reg", &val)) {
+			ret = mxs_pinctrl_parse_group(pdev, child,
+						      idxg++, NULL);
+			if (ret)
+				return ret;
+			continue;
+		}
+
+		if (strcmp(fn, child->name)) {
+			f = &soc->functions[idxf++];
+			f->groups = devm_kzalloc(&pdev->dev, f->ngroups *
+						 sizeof(*f->groups),
+						 GFP_KERNEL);
+			if (!f->groups)
+				return -ENOMEM;
+			fn = child->name;
+			i = 0;
+		}
+		ret = mxs_pinctrl_parse_group(pdev, child, idxg++,
+					      &f->groups[i++]);
+		if (ret)
+			return ret;
+	}
+
+	return 0;
+}
+
+int __devinit mxs_pinctrl_probe(struct platform_device *pdev,
+				struct mxs_pinctrl_soc_data *soc)
+{
+	struct device_node *np = pdev->dev.of_node;
+	struct mxs_pinctrl_data *d;
+	int ret;
+
+	d = devm_kzalloc(&pdev->dev, sizeof(*d), GFP_KERNEL);
+	if (!d)
+		return -ENOMEM;
+
+	d->dev = &pdev->dev;
+	d->soc = soc;
+
+	d->base = of_iomap(np, 0);
+	if (!d->base)
+		return -EADDRNOTAVAIL;
+
+	mxs_pinctrl_desc.pins = d->soc->pins;
+	mxs_pinctrl_desc.npins = d->soc->npins;
+	mxs_pinctrl_desc.name = dev_name(&pdev->dev);
+
+	platform_set_drvdata(pdev, d);
+
+	ret = mxs_pinctrl_probe_dt(pdev, d);
+	if (ret) {
+		dev_err(&pdev->dev, "dt probe failed: %d\n", ret);
+		goto err;
+	}
+
+	d->pctl = pinctrl_register(&mxs_pinctrl_desc, &pdev->dev, d);
+	if (!d->pctl) {
+		dev_err(&pdev->dev, "Couldn't register MXS pinctrl driver\n");
+		ret = -EINVAL;
+		goto err;
+	}
+
+	return 0;
+
+err:
+	iounmap(d->base);
+	return ret;
+}
+EXPORT_SYMBOL_GPL(mxs_pinctrl_probe);
+
+int __devexit mxs_pinctrl_remove(struct platform_device *pdev)
+{
+	struct mxs_pinctrl_data *d = platform_get_drvdata(pdev);
+
+	pinctrl_unregister(d->pctl);
+	iounmap(d->base);
+
+	return 0;
+}
+EXPORT_SYMBOL_GPL(mxs_pinctrl_remove);
diff --git a/drivers/pinctrl/pinctrl-mxs.h b/drivers/pinctrl/pinctrl-mxs.h
new file mode 100644
index 00000000000..fdd88d0bae2
--- /dev/null
+++ b/drivers/pinctrl/pinctrl-mxs.h
@@ -0,0 +1,91 @@
+/*
+ * Copyright 2012 Freescale Semiconductor, Inc.
+ *
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/gpl-license.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+
+#ifndef __PINCTRL_MXS_H
+#define __PINCTRL_MXS_H
+
+#include <linux/platform_device.h>
+#include <linux/pinctrl/pinctrl.h>
+
+#define SET	0x4
+#define CLR	0x8
+#define TOG	0xc
+
+#define MXS_PINCTRL_PIN(pin)	PINCTRL_PIN(pin, #pin)
+#define PINID(bank, pin)	((bank) * 32 + (pin))
+
+/*
+ * pinmux-id bit field definitions
+ *
+ * bank:	15..12	(4)
+ * pin:		11..4	(8)
+ * muxsel:	3..0	(4)
+ */
+#define MUXID_TO_PINID(m)	PINID((m) >> 12 & 0xf, (m) >> 4 & 0xff)
+#define MUXID_TO_MUXSEL(m)	((m) & 0xf)
+
+#define PINID_TO_BANK(p)	((p) >> 5)
+#define PINID_TO_PIN(p)		((p) % 32)
+
+/*
+ * pin config bit field definitions
+ *
+ * pull-up:	6..5	(2)
+ * voltage:	4..3	(2)
+ * mA:		2..0	(3)
+ *
+ * MSB of each field is presence bit for the config.
+ */
+#define PULL_PRESENT		(1 << 6)
+#define PULL_SHIFT		5
+#define VOL_PRESENT		(1 << 4)
+#define VOL_SHIFT		3
+#define MA_PRESENT		(1 << 2)
+#define MA_SHIFT		0
+#define CONFIG_TO_PULL(c)	((c) >> PULL_SHIFT & 0x1)
+#define CONFIG_TO_VOL(c)	((c) >> VOL_SHIFT & 0x1)
+#define CONFIG_TO_MA(c)		((c) >> MA_SHIFT & 0x3)
+
+struct mxs_function {
+	const char *name;
+	const char **groups;
+	unsigned ngroups;
+};
+
+struct mxs_group {
+	const char *name;
+	unsigned int *pins;
+	unsigned npins;
+	u8 *muxsel;
+	u8 config;
+};
+
+struct mxs_regs {
+	u16 muxsel;
+	u16 drive;
+	u16 pull;
+};
+
+struct mxs_pinctrl_soc_data {
+	const struct mxs_regs *regs;
+	const struct pinctrl_pin_desc *pins;
+	unsigned npins;
+	struct mxs_function *functions;
+	unsigned nfunctions;
+	struct mxs_group *groups;
+	unsigned ngroups;
+};
+
+int mxs_pinctrl_probe(struct platform_device *pdev,
+		      struct mxs_pinctrl_soc_data *soc);
+int mxs_pinctrl_remove(struct platform_device *pdev);
+
+#endif /* __PINCTRL_MXS_H */
diff --git a/drivers/pinctrl/pinctrl-nomadik-db8500.c b/drivers/pinctrl/pinctrl-nomadik-db8500.c
new file mode 100644
index 00000000000..8b2022276f7
--- /dev/null
+++ b/drivers/pinctrl/pinctrl-nomadik-db8500.c
@@ -0,0 +1,857 @@
+#include <linux/kernel.h>
+#include <linux/pinctrl/pinctrl.h>
+#include "pinctrl-nomadik.h"
+
+/* All the pins that can be used for GPIO and some other functions */
+#define _GPIO(offset)		(offset)
+
+#define DB8500_PIN_AJ5		_GPIO(0)
+#define DB8500_PIN_AJ3		_GPIO(1)
+#define DB8500_PIN_AH4		_GPIO(2)
+#define DB8500_PIN_AH3		_GPIO(3)
+#define DB8500_PIN_AH6		_GPIO(4)
+#define DB8500_PIN_AG6		_GPIO(5)
+#define DB8500_PIN_AF6		_GPIO(6)
+#define DB8500_PIN_AG5		_GPIO(7)
+#define DB8500_PIN_AD5		_GPIO(8)
+#define DB8500_PIN_AE4		_GPIO(9)
+#define DB8500_PIN_AF5		_GPIO(10)
+#define DB8500_PIN_AG4		_GPIO(11)
+#define DB8500_PIN_AC4		_GPIO(12)
+#define DB8500_PIN_AF3		_GPIO(13)
+#define DB8500_PIN_AE3		_GPIO(14)
+#define DB8500_PIN_AC3		_GPIO(15)
+#define DB8500_PIN_AD3		_GPIO(16)
+#define DB8500_PIN_AD4		_GPIO(17)
+#define DB8500_PIN_AC2		_GPIO(18)
+#define DB8500_PIN_AC1		_GPIO(19)
+#define DB8500_PIN_AB4		_GPIO(20)
+#define DB8500_PIN_AB3		_GPIO(21)
+#define DB8500_PIN_AA3		_GPIO(22)
+#define DB8500_PIN_AA4		_GPIO(23)
+#define DB8500_PIN_AB2		_GPIO(24)
+#define DB8500_PIN_Y4		_GPIO(25)
+#define DB8500_PIN_Y2		_GPIO(26)
+#define DB8500_PIN_AA2		_GPIO(27)
+#define DB8500_PIN_AA1		_GPIO(28)
+#define DB8500_PIN_W2		_GPIO(29)
+#define DB8500_PIN_W3		_GPIO(30)
+#define DB8500_PIN_V3		_GPIO(31)
+#define DB8500_PIN_V2		_GPIO(32)
+#define DB8500_PIN_AF2		_GPIO(33)
+#define DB8500_PIN_AE1		_GPIO(34)
+#define DB8500_PIN_AE2		_GPIO(35)
+#define DB8500_PIN_AG2		_GPIO(36)
+/* Hole */
+#define DB8500_PIN_F3		_GPIO(64)
+#define DB8500_PIN_F1		_GPIO(65)
+#define DB8500_PIN_G3		_GPIO(66)
+#define DB8500_PIN_G2		_GPIO(67)
+#define DB8500_PIN_E1		_GPIO(68)
+#define DB8500_PIN_E2		_GPIO(69)
+#define DB8500_PIN_G5		_GPIO(70)
+#define DB8500_PIN_G4		_GPIO(71)
+#define DB8500_PIN_H4		_GPIO(72)
+#define DB8500_PIN_H3		_GPIO(73)
+#define DB8500_PIN_J3		_GPIO(74)
+#define DB8500_PIN_H2		_GPIO(75)
+#define DB8500_PIN_J2		_GPIO(76)
+#define DB8500_PIN_H1		_GPIO(77)
+#define DB8500_PIN_F4		_GPIO(78)
+#define DB8500_PIN_E3		_GPIO(79)
+#define DB8500_PIN_E4		_GPIO(80)
+#define DB8500_PIN_D2		_GPIO(81)
+#define DB8500_PIN_C1		_GPIO(82)
+#define DB8500_PIN_D3		_GPIO(83)
+#define DB8500_PIN_C2		_GPIO(84)
+#define DB8500_PIN_D5		_GPIO(85)
+#define DB8500_PIN_C6		_GPIO(86)
+#define DB8500_PIN_B3		_GPIO(87)
+#define DB8500_PIN_C4		_GPIO(88)
+#define DB8500_PIN_E6		_GPIO(89)
+#define DB8500_PIN_A3		_GPIO(90)
+#define DB8500_PIN_B6		_GPIO(91)
+#define DB8500_PIN_D6		_GPIO(92)
+#define DB8500_PIN_B7		_GPIO(93)
+#define DB8500_PIN_D7		_GPIO(94)
+#define DB8500_PIN_E8		_GPIO(95)
+#define DB8500_PIN_D8		_GPIO(96)
+#define DB8500_PIN_D9		_GPIO(97)
+/* Hole */
+#define DB8500_PIN_A5		_GPIO(128)
+#define DB8500_PIN_B4		_GPIO(129)
+#define DB8500_PIN_C8		_GPIO(130)
+#define DB8500_PIN_A12		_GPIO(131)
+#define DB8500_PIN_C10		_GPIO(132)
+#define DB8500_PIN_B10		_GPIO(133)
+#define DB8500_PIN_B9		_GPIO(134)
+#define DB8500_PIN_A9		_GPIO(135)
+#define DB8500_PIN_C7		_GPIO(136)
+#define DB8500_PIN_A7		_GPIO(137)
+#define DB8500_PIN_C5		_GPIO(138)
+#define DB8500_PIN_C9		_GPIO(139)
+#define DB8500_PIN_B11		_GPIO(140)
+#define DB8500_PIN_C12		_GPIO(141)
+#define DB8500_PIN_C11		_GPIO(142)
+#define DB8500_PIN_D12		_GPIO(143)
+#define DB8500_PIN_B13		_GPIO(144)
+#define DB8500_PIN_C13		_GPIO(145)
+#define DB8500_PIN_D13		_GPIO(146)
+#define DB8500_PIN_C15		_GPIO(147)
+#define DB8500_PIN_B16		_GPIO(148)
+#define DB8500_PIN_B14		_GPIO(149)
+#define DB8500_PIN_C14		_GPIO(150)
+#define DB8500_PIN_D17		_GPIO(151)
+#define DB8500_PIN_D16		_GPIO(152)
+#define DB8500_PIN_B17		_GPIO(153)
+#define DB8500_PIN_C16		_GPIO(154)
+#define DB8500_PIN_C19		_GPIO(155)
+#define DB8500_PIN_C17		_GPIO(156)
+#define DB8500_PIN_A18		_GPIO(157)
+#define DB8500_PIN_C18		_GPIO(158)
+#define DB8500_PIN_B19		_GPIO(159)
+#define DB8500_PIN_B20		_GPIO(160)
+#define DB8500_PIN_D21		_GPIO(161)
+#define DB8500_PIN_D20		_GPIO(162)
+#define DB8500_PIN_C20		_GPIO(163)
+#define DB8500_PIN_B21		_GPIO(164)
+#define DB8500_PIN_C21		_GPIO(165)
+#define DB8500_PIN_A22		_GPIO(166)
+#define DB8500_PIN_B24		_GPIO(167)
+#define DB8500_PIN_C22		_GPIO(168)
+#define DB8500_PIN_D22		_GPIO(169)
+#define DB8500_PIN_C23		_GPIO(170)
+#define DB8500_PIN_D23		_GPIO(171)
+/* Hole */
+#define DB8500_PIN_AJ27		_GPIO(192)
+#define DB8500_PIN_AH27		_GPIO(193)
+#define DB8500_PIN_AF27		_GPIO(194)
+#define DB8500_PIN_AG28		_GPIO(195)
+#define DB8500_PIN_AG26		_GPIO(196)
+#define DB8500_PIN_AH24		_GPIO(197)
+#define DB8500_PIN_AG25		_GPIO(198)
+#define DB8500_PIN_AH23		_GPIO(199)
+#define DB8500_PIN_AH26		_GPIO(200)
+#define DB8500_PIN_AF24		_GPIO(201)
+#define DB8500_PIN_AF25		_GPIO(202)
+#define DB8500_PIN_AE23		_GPIO(203)
+#define DB8500_PIN_AF23		_GPIO(204)
+#define DB8500_PIN_AG23		_GPIO(205)
+#define DB8500_PIN_AG24		_GPIO(206)
+#define DB8500_PIN_AJ23		_GPIO(207)
+#define DB8500_PIN_AH16		_GPIO(208)
+#define DB8500_PIN_AG15		_GPIO(209)
+#define DB8500_PIN_AJ15		_GPIO(210)
+#define DB8500_PIN_AG14		_GPIO(211)
+#define DB8500_PIN_AF13		_GPIO(212)
+#define DB8500_PIN_AG13		_GPIO(213)
+#define DB8500_PIN_AH15		_GPIO(214)
+#define DB8500_PIN_AH13		_GPIO(215)
+#define DB8500_PIN_AG12		_GPIO(216)
+#define DB8500_PIN_AH12		_GPIO(217)
+#define DB8500_PIN_AH11		_GPIO(218)
+#define DB8500_PIN_AG10		_GPIO(219)
+#define DB8500_PIN_AH10		_GPIO(220)
+#define DB8500_PIN_AJ11		_GPIO(221)
+#define DB8500_PIN_AJ9		_GPIO(222)
+#define DB8500_PIN_AH9		_GPIO(223)
+#define DB8500_PIN_AG9		_GPIO(224)
+#define DB8500_PIN_AG8		_GPIO(225)
+#define DB8500_PIN_AF8		_GPIO(226)
+#define DB8500_PIN_AH7		_GPIO(227)
+#define DB8500_PIN_AJ6		_GPIO(228)
+#define DB8500_PIN_AG7		_GPIO(229)
+#define DB8500_PIN_AF7		_GPIO(230)
+/* Hole */
+#define DB8500_PIN_AF28		_GPIO(256)
+#define DB8500_PIN_AE29		_GPIO(257)
+#define DB8500_PIN_AD29		_GPIO(258)
+#define DB8500_PIN_AC29		_GPIO(259)
+#define DB8500_PIN_AD28		_GPIO(260)
+#define DB8500_PIN_AD26		_GPIO(261)
+#define DB8500_PIN_AE26		_GPIO(262)
+#define DB8500_PIN_AG29		_GPIO(263)
+#define DB8500_PIN_AE27		_GPIO(264)
+#define DB8500_PIN_AD27		_GPIO(265)
+#define DB8500_PIN_AC28		_GPIO(266)
+#define DB8500_PIN_AC27		_GPIO(267)
+
+/*
+ * The names of the pins are denoted by GPIO number and ball name, even
+ * though they can be used for other things than GPIO, this is the first
+ * column in the table of the data sheet and often used on schematics and
+ * such.
+ */
+static const struct pinctrl_pin_desc nmk_db8500_pins[] = {
+	PINCTRL_PIN(DB8500_PIN_AJ5, "GPIO0_AJ5"),
+	PINCTRL_PIN(DB8500_PIN_AJ3, "GPIO1_AJ3"),
+	PINCTRL_PIN(DB8500_PIN_AH4, "GPIO2_AH4"),
+	PINCTRL_PIN(DB8500_PIN_AH3, "GPIO3_AH3"),
+	PINCTRL_PIN(DB8500_PIN_AH6, "GPIO4_AH6"),
+	PINCTRL_PIN(DB8500_PIN_AG6, "GPIO5_AG6"),
+	PINCTRL_PIN(DB8500_PIN_AF6, "GPIO6_AF6"),
+	PINCTRL_PIN(DB8500_PIN_AG5, "GPIO7_AG5"),
+	PINCTRL_PIN(DB8500_PIN_AD5, "GPIO8_AD5"),
+	PINCTRL_PIN(DB8500_PIN_AE4, "GPIO9_AE4"),
+	PINCTRL_PIN(DB8500_PIN_AF5, "GPIO10_AF5"),
+	PINCTRL_PIN(DB8500_PIN_AG4, "GPIO11_AG4"),
+	PINCTRL_PIN(DB8500_PIN_AC4, "GPIO12_AC4"),
+	PINCTRL_PIN(DB8500_PIN_AF3, "GPIO13_AF3"),
+	PINCTRL_PIN(DB8500_PIN_AE3, "GPIO14_AE3"),
+	PINCTRL_PIN(DB8500_PIN_AC3, "GPIO15_AC3"),
+	PINCTRL_PIN(DB8500_PIN_AD3, "GPIO16_AD3"),
+	PINCTRL_PIN(DB8500_PIN_AD4, "GPIO17_AD4"),
+	PINCTRL_PIN(DB8500_PIN_AC2, "GPIO18_AC2"),
+	PINCTRL_PIN(DB8500_PIN_AC1, "GPIO19_AC1"),
+	PINCTRL_PIN(DB8500_PIN_AB4, "GPIO20_AB4"),
+	PINCTRL_PIN(DB8500_PIN_AB3, "GPIO21_AB3"),
+	PINCTRL_PIN(DB8500_PIN_AA3, "GPIO22_AA3"),
+	PINCTRL_PIN(DB8500_PIN_AA4, "GPIO23_AA4"),
+	PINCTRL_PIN(DB8500_PIN_AB2, "GPIO24_AB2"),
+	PINCTRL_PIN(DB8500_PIN_Y4, "GPIO25_Y4"),
+	PINCTRL_PIN(DB8500_PIN_Y2, "GPIO26_Y2"),
+	PINCTRL_PIN(DB8500_PIN_AA2, "GPIO27_AA2"),
+	PINCTRL_PIN(DB8500_PIN_AA1, "GPIO28_AA1"),
+	PINCTRL_PIN(DB8500_PIN_W2, "GPIO29_W2"),
+	PINCTRL_PIN(DB8500_PIN_W3, "GPIO30_W3"),
+	PINCTRL_PIN(DB8500_PIN_V3, "GPIO31_V3"),
+	PINCTRL_PIN(DB8500_PIN_V2, "GPIO32_V2"),
+	PINCTRL_PIN(DB8500_PIN_AF2, "GPIO33_AF2"),
+	PINCTRL_PIN(DB8500_PIN_AE1, "GPIO34_AE1"),
+	PINCTRL_PIN(DB8500_PIN_AE2, "GPIO35_AE2"),
+	PINCTRL_PIN(DB8500_PIN_AG2, "GPIO36_AG2"),
+	/* Hole */
+	PINCTRL_PIN(DB8500_PIN_F3, "GPIO64_F3"),
+	PINCTRL_PIN(DB8500_PIN_F1, "GPIO65_F1"),
+	PINCTRL_PIN(DB8500_PIN_G3, "GPIO66_G3"),
+	PINCTRL_PIN(DB8500_PIN_G2, "GPIO67_G2"),
+	PINCTRL_PIN(DB8500_PIN_E1, "GPIO68_E1"),
+	PINCTRL_PIN(DB8500_PIN_E2, "GPIO69_E2"),
+	PINCTRL_PIN(DB8500_PIN_G5, "GPIO70_G5"),
+	PINCTRL_PIN(DB8500_PIN_G4, "GPIO71_G4"),
+	PINCTRL_PIN(DB8500_PIN_H4, "GPIO72_H4"),
+	PINCTRL_PIN(DB8500_PIN_H3, "GPIO73_H3"),
+	PINCTRL_PIN(DB8500_PIN_J3, "GPIO74_J3"),
+	PINCTRL_PIN(DB8500_PIN_H2, "GPIO75_H2"),
+	PINCTRL_PIN(DB8500_PIN_J2, "GPIO76_J2"),
+	PINCTRL_PIN(DB8500_PIN_H1, "GPIO77_H1"),
+	PINCTRL_PIN(DB8500_PIN_F4, "GPIO78_F4"),
+	PINCTRL_PIN(DB8500_PIN_E3, "GPIO79_E3"),
+	PINCTRL_PIN(DB8500_PIN_E4, "GPIO80_E4"),
+	PINCTRL_PIN(DB8500_PIN_D2, "GPIO81_D2"),
+	PINCTRL_PIN(DB8500_PIN_C1, "GPIO82_C1"),
+	PINCTRL_PIN(DB8500_PIN_D3, "GPIO83_D3"),
+	PINCTRL_PIN(DB8500_PIN_C2, "GPIO84_C2"),
+	PINCTRL_PIN(DB8500_PIN_D5, "GPIO85_D5"),
+	PINCTRL_PIN(DB8500_PIN_C6, "GPIO86_C6"),
+	PINCTRL_PIN(DB8500_PIN_B3, "GPIO87_B3"),
+	PINCTRL_PIN(DB8500_PIN_C4, "GPIO88_C4"),
+	PINCTRL_PIN(DB8500_PIN_E6, "GPIO89_E6"),
+	PINCTRL_PIN(DB8500_PIN_A3, "GPIO90_A3"),
+	PINCTRL_PIN(DB8500_PIN_B6, "GPIO91_B6"),
+	PINCTRL_PIN(DB8500_PIN_D6, "GPIO92_D6"),
+	PINCTRL_PIN(DB8500_PIN_B7, "GPIO93_B7"),
+	PINCTRL_PIN(DB8500_PIN_D7, "GPIO94_D7"),
+	PINCTRL_PIN(DB8500_PIN_E8, "GPIO95_E8"),
+	PINCTRL_PIN(DB8500_PIN_D8, "GPIO96_D8"),
+	PINCTRL_PIN(DB8500_PIN_D9, "GPIO97_D9"),
+	/* Hole */
+	PINCTRL_PIN(DB8500_PIN_A5, "GPIO128_A5"),
+	PINCTRL_PIN(DB8500_PIN_B4, "GPIO129_B4"),
+	PINCTRL_PIN(DB8500_PIN_C8, "GPIO130_C8"),
+	PINCTRL_PIN(DB8500_PIN_A12, "GPIO131_A12"),
+	PINCTRL_PIN(DB8500_PIN_C10, "GPIO132_C10"),
+	PINCTRL_PIN(DB8500_PIN_B10, "GPIO133_B10"),
+	PINCTRL_PIN(DB8500_PIN_B9, "GPIO134_B9"),
+	PINCTRL_PIN(DB8500_PIN_A9, "GPIO135_A9"),
+	PINCTRL_PIN(DB8500_PIN_C7, "GPIO136_C7"),
+	PINCTRL_PIN(DB8500_PIN_A7, "GPIO137_A7"),
+	PINCTRL_PIN(DB8500_PIN_C5, "GPIO138_C5"),
+	PINCTRL_PIN(DB8500_PIN_C9, "GPIO139_C9"),
+	PINCTRL_PIN(DB8500_PIN_B11, "GPIO140_B11"),
+	PINCTRL_PIN(DB8500_PIN_C12, "GPIO141_C12"),
+	PINCTRL_PIN(DB8500_PIN_C11, "GPIO142_C11"),
+	PINCTRL_PIN(DB8500_PIN_D12, "GPIO143_D12"),
+	PINCTRL_PIN(DB8500_PIN_B13, "GPIO144_B13"),
+	PINCTRL_PIN(DB8500_PIN_C13, "GPIO145_C13"),
+	PINCTRL_PIN(DB8500_PIN_D13, "GPIO146_D13"),
+	PINCTRL_PIN(DB8500_PIN_C15, "GPIO147_C15"),
+	PINCTRL_PIN(DB8500_PIN_B16, "GPIO148_B16"),
+	PINCTRL_PIN(DB8500_PIN_B14, "GPIO149_B14"),
+	PINCTRL_PIN(DB8500_PIN_C14, "GPIO150_C14"),
+	PINCTRL_PIN(DB8500_PIN_D17, "GPIO151_D17"),
+	PINCTRL_PIN(DB8500_PIN_D16, "GPIO152_D16"),
+	PINCTRL_PIN(DB8500_PIN_B17, "GPIO153_B17"),
+	PINCTRL_PIN(DB8500_PIN_C16, "GPIO154_C16"),
+	PINCTRL_PIN(DB8500_PIN_C19, "GPIO155_C19"),
+	PINCTRL_PIN(DB8500_PIN_C17, "GPIO156_C17"),
+	PINCTRL_PIN(DB8500_PIN_A18, "GPIO157_A18"),
+	PINCTRL_PIN(DB8500_PIN_C18, "GPIO158_C18"),
+	PINCTRL_PIN(DB8500_PIN_B19, "GPIO159_B19"),
+	PINCTRL_PIN(DB8500_PIN_B20, "GPIO160_B20"),
+	PINCTRL_PIN(DB8500_PIN_D21, "GPIO161_D21"),
+	PINCTRL_PIN(DB8500_PIN_D20, "GPIO162_D20"),
+	PINCTRL_PIN(DB8500_PIN_C20, "GPIO163_C20"),
+	PINCTRL_PIN(DB8500_PIN_B21, "GPIO164_B21"),
+	PINCTRL_PIN(DB8500_PIN_C21, "GPIO165_C21"),
+	PINCTRL_PIN(DB8500_PIN_A22, "GPIO166_A22"),
+	PINCTRL_PIN(DB8500_PIN_B24, "GPIO167_B24"),
+	PINCTRL_PIN(DB8500_PIN_C22, "GPIO168_C22"),
+	PINCTRL_PIN(DB8500_PIN_D22, "GPIO169_D22"),
+	PINCTRL_PIN(DB8500_PIN_C23, "GPIO170_C23"),
+	PINCTRL_PIN(DB8500_PIN_D23, "GPIO171_D23"),
+	/* Hole */
+	PINCTRL_PIN(DB8500_PIN_AJ27, "GPIO192_AJ27"),
+	PINCTRL_PIN(DB8500_PIN_AH27, "GPIO193_AH27"),
+	PINCTRL_PIN(DB8500_PIN_AF27, "GPIO194_AF27"),
+	PINCTRL_PIN(DB8500_PIN_AG28, "GPIO195_AG28"),
+	PINCTRL_PIN(DB8500_PIN_AG26, "GPIO196_AG26"),
+	PINCTRL_PIN(DB8500_PIN_AH24, "GPIO197_AH24"),
+	PINCTRL_PIN(DB8500_PIN_AG25, "GPIO198_AG25"),
+	PINCTRL_PIN(DB8500_PIN_AH23, "GPIO199_AH23"),
+	PINCTRL_PIN(DB8500_PIN_AH26, "GPIO200_AH26"),
+	PINCTRL_PIN(DB8500_PIN_AF24, "GPIO201_AF24"),
+	PINCTRL_PIN(DB8500_PIN_AF25, "GPIO202_AF25"),
+	PINCTRL_PIN(DB8500_PIN_AE23, "GPIO203_AE23"),
+	PINCTRL_PIN(DB8500_PIN_AF23, "GPIO204_AF23"),
+	PINCTRL_PIN(DB8500_PIN_AG23, "GPIO205_AG23"),
+	PINCTRL_PIN(DB8500_PIN_AG24, "GPIO206_AG24"),
+	PINCTRL_PIN(DB8500_PIN_AJ23, "GPIO207_AJ23"),
+	PINCTRL_PIN(DB8500_PIN_AH16, "GPIO208_AH16"),
+	PINCTRL_PIN(DB8500_PIN_AG15, "GPIO209_AG15"),
+	PINCTRL_PIN(DB8500_PIN_AJ15, "GPIO210_AJ15"),
+	PINCTRL_PIN(DB8500_PIN_AG14, "GPIO211_AG14"),
+	PINCTRL_PIN(DB8500_PIN_AF13, "GPIO212_AF13"),
+	PINCTRL_PIN(DB8500_PIN_AG13, "GPIO213_AG13"),
+	PINCTRL_PIN(DB8500_PIN_AH15, "GPIO214_AH15"),
+	PINCTRL_PIN(DB8500_PIN_AH13, "GPIO215_AH13"),
+	PINCTRL_PIN(DB8500_PIN_AG12, "GPIO216_AG12"),
+	PINCTRL_PIN(DB8500_PIN_AH12, "GPIO217_AH12"),
+	PINCTRL_PIN(DB8500_PIN_AH11, "GPIO218_AH11"),
+	PINCTRL_PIN(DB8500_PIN_AG10, "GPIO219_AG10"),
+	PINCTRL_PIN(DB8500_PIN_AH10, "GPIO220_AH10"),
+	PINCTRL_PIN(DB8500_PIN_AJ11, "GPIO221_AJ11"),
+	PINCTRL_PIN(DB8500_PIN_AJ9, "GPIO222_AJ9"),
+	PINCTRL_PIN(DB8500_PIN_AH9, "GPIO223_AH9"),
+	PINCTRL_PIN(DB8500_PIN_AG9, "GPIO224_AG9"),
+	PINCTRL_PIN(DB8500_PIN_AG8, "GPIO225_AG8"),
+	PINCTRL_PIN(DB8500_PIN_AF8, "GPIO226_AF8"),
+	PINCTRL_PIN(DB8500_PIN_AH7, "GPIO227_AH7"),
+	PINCTRL_PIN(DB8500_PIN_AJ6, "GPIO228_AJ6"),
+	PINCTRL_PIN(DB8500_PIN_AG7, "GPIO229_AG7"),
+	PINCTRL_PIN(DB8500_PIN_AF7, "GPIO230_AF7"),
+	/* Hole */
+	PINCTRL_PIN(DB8500_PIN_AF28, "GPIO256_AF28"),
+	PINCTRL_PIN(DB8500_PIN_AE29, "GPIO257_AE29"),
+	PINCTRL_PIN(DB8500_PIN_AD29, "GPIO258_AD29"),
+	PINCTRL_PIN(DB8500_PIN_AC29, "GPIO259_AC29"),
+	PINCTRL_PIN(DB8500_PIN_AD28, "GPIO260_AD28"),
+	PINCTRL_PIN(DB8500_PIN_AD26, "GPIO261_AD26"),
+	PINCTRL_PIN(DB8500_PIN_AE26, "GPIO262_AE26"),
+	PINCTRL_PIN(DB8500_PIN_AG29, "GPIO263_AG29"),
+	PINCTRL_PIN(DB8500_PIN_AE27, "GPIO264_AE27"),
+	PINCTRL_PIN(DB8500_PIN_AD27, "GPIO265_AD27"),
+	PINCTRL_PIN(DB8500_PIN_AC28, "GPIO266_AC28"),
+	PINCTRL_PIN(DB8500_PIN_AC27, "GPIO267_AC27"),
+};
+
+#define DB8500_GPIO_RANGE(a, b, c) { .name = "DB8500", .id = a, .base = b, \
+			.pin_base = b, .npins = c }
+
+/*
+ * This matches the 32-pin gpio chips registered by the GPIO portion. This
+ * cannot be const since we assign the struct gpio_chip * pointer at runtime.
+ */
+static struct pinctrl_gpio_range nmk_db8500_ranges[] = {
+	DB8500_GPIO_RANGE(0, 0, 32),
+	DB8500_GPIO_RANGE(1, 32, 5),
+	DB8500_GPIO_RANGE(2, 64, 32),
+	DB8500_GPIO_RANGE(3, 96, 2),
+	DB8500_GPIO_RANGE(4, 128, 32),
+	DB8500_GPIO_RANGE(5, 160, 12),
+	DB8500_GPIO_RANGE(6, 192, 32),
+	DB8500_GPIO_RANGE(7, 224, 7),
+	DB8500_GPIO_RANGE(8, 256, 12),
+};
+
+/*
+ * Read the pin group names like this:
+ * u0_a_1    = first groups of pins for uart0 on alt function a
+ * i2c2_b_2  = second group of pins for i2c2 on alt function b
+ *
+ * The groups are arranged as sets per altfunction column, so we can
+ * mux in one group at a time by selecting the same altfunction for them
+ * all. When functions require pins on different altfunctions, you need
+ * to combine several groups.
+ */
+
+/* Altfunction A column */
+static const unsigned u0_a_1_pins[] = { DB8500_PIN_AJ5, DB8500_PIN_AJ3,
+					DB8500_PIN_AH4, DB8500_PIN_AH3 };
+static const unsigned u1rxtx_a_1_pins[] = { DB8500_PIN_AH6, DB8500_PIN_AG6 };
+static const unsigned u1ctsrts_a_1_pins[] = { DB8500_PIN_AF6, DB8500_PIN_AG5 };
+/* Image processor I2C line, this is driven by image processor firmware */
+static const unsigned ipi2c_a_1_pins[] = { DB8500_PIN_AD5, DB8500_PIN_AE4 };
+static const unsigned ipi2c_a_2_pins[] = { DB8500_PIN_AF5, DB8500_PIN_AG4 };
+/* MSP0 can only be on these pins, but TXD and RXD can be flipped */
+static const unsigned msp0txrx_a_1_pins[] = { DB8500_PIN_AC4, DB8500_PIN_AC3 };
+static const unsigned msp0tfstck_a_1_pins[] = { DB8500_PIN_AF3, DB8500_PIN_AE3 };
+static const unsigned msp0rfsrck_a_1_pins[] = { DB8500_PIN_AD3, DB8500_PIN_AD4 };
+/* Basic pins of the MMC/SD card 0 interface */
+static const unsigned mc0_a_1_pins[] = { DB8500_PIN_AC2, DB8500_PIN_AC1,
+	DB8500_PIN_AB4, DB8500_PIN_AA3, DB8500_PIN_AA4, DB8500_PIN_AB2,
+	DB8500_PIN_Y4, DB8500_PIN_Y2, DB8500_PIN_AA2, DB8500_PIN_AA1 };
+/* Often only 4 bits are used, then these are not needed (only used for MMC) */
+static const unsigned mc0_dat47_a_1_pins[] = { DB8500_PIN_W2, DB8500_PIN_W3,
+	DB8500_PIN_V3, DB8500_PIN_V2};
+static const unsigned mc0dat31dir_a_1_pins[] = { DB8500_PIN_AB3 };
+/* MSP1 can only be on these pins, but TXD and RXD can be flipped */
+static const unsigned msp1txrx_a_1_pins[] = { DB8500_PIN_AF2, DB8500_PIN_AG2 };
+static const unsigned msp1_a_1_pins[] = { DB8500_PIN_AE1, DB8500_PIN_AE2 };
+/* LCD interface */
+static const unsigned lcdb_a_1_pins[] = { DB8500_PIN_F3, DB8500_PIN_F1,
+					  DB8500_PIN_G3, DB8500_PIN_G2 };
+static const unsigned lcdvsi0_a_1_pins[] = { DB8500_PIN_E1 };
+static const unsigned lcdvsi1_a_1_pins[] = { DB8500_PIN_E2 };
+static const unsigned lcd_d0_d7_a_1_pins[] = {
+	DB8500_PIN_G5, DB8500_PIN_G4, DB8500_PIN_H4, DB8500_PIN_H3,
+	DB8500_PIN_J3, DB8500_PIN_H2, DB8500_PIN_J2, DB8500_PIN_H1 };
+/* D8 thru D11 often used as TVOUT lines */
+static const unsigned lcd_d8_d11_a_1_pins[] = { DB8500_PIN_F4,
+	DB8500_PIN_E3, DB8500_PIN_E4, DB8500_PIN_D2 };
+static const unsigned lcd_d12_d23_a_1_pins[] = {
+	DB8500_PIN_C1, DB8500_PIN_D3, DB8500_PIN_C2, DB8500_PIN_D5,
+	DB8500_PIN_C6, DB8500_PIN_B3, DB8500_PIN_C4, DB8500_PIN_E6,
+	DB8500_PIN_A3, DB8500_PIN_B6, DB8500_PIN_D6, DB8500_PIN_B7 };
+static const unsigned kp_a_1_pins[] = { DB8500_PIN_D7, DB8500_PIN_E8,
+	DB8500_PIN_D8, DB8500_PIN_D9 };
+static const unsigned kpskaskb_a_1_pins[] = { DB8500_PIN_D17, DB8500_PIN_D16 };
+static const unsigned kp_a_2_pins[] = {
+	DB8500_PIN_B17, DB8500_PIN_C16, DB8500_PIN_C19, DB8500_PIN_C17,
+	DB8500_PIN_A18, DB8500_PIN_C18, DB8500_PIN_B19, DB8500_PIN_B20,
+	DB8500_PIN_D21, DB8500_PIN_D20, DB8500_PIN_C20, DB8500_PIN_B21,
+	DB8500_PIN_C21, DB8500_PIN_A22, DB8500_PIN_B24, DB8500_PIN_C22 };
+/* MC2 has 8 data lines and no direction control, so only for (e)MMC */
+static const unsigned mc2_a_1_pins[] = { DB8500_PIN_A5, DB8500_PIN_B4,
+	DB8500_PIN_C8, DB8500_PIN_A12, DB8500_PIN_C10, DB8500_PIN_B10,
+	DB8500_PIN_B9, DB8500_PIN_A9, DB8500_PIN_C7, DB8500_PIN_A7,
+	DB8500_PIN_C5 };
+static const unsigned ssp1_a_1_pins[] = { DB8500_PIN_C9, DB8500_PIN_B11,
+					  DB8500_PIN_C12, DB8500_PIN_C11 };
+static const unsigned ssp0_a_1_pins[] = { DB8500_PIN_D12, DB8500_PIN_B13,
+					  DB8500_PIN_C13, DB8500_PIN_D13 };
+static const unsigned i2c0_a_1_pins[] = { DB8500_PIN_C15, DB8500_PIN_B16 };
+/*
+ * Image processor GPIO pins are named "ipgpio" and have their own
+ * numberspace
+ */
+static const unsigned ipgpio0_a_1_pins[] = { DB8500_PIN_B14 };
+static const unsigned ipgpio1_a_1_pins[] = { DB8500_PIN_C14 };
+/* Three modem pins named RF_PURn, MODEM_STATE and MODEM_PWREN */
+static const unsigned modem_a_1_pins[] = { DB8500_PIN_D22, DB8500_PIN_C23,
+					   DB8500_PIN_D23 };
+/*
+ * This MSP cannot switch RX and TX, SCK in a separate group since this
+ * seems to be optional.
+ */
+static const unsigned msp2sck_a_1_pins[] = { DB8500_PIN_AJ27 };
+static const unsigned msp2_a_1_pins[] = { DB8500_PIN_AH27, DB8500_PIN_AF27,
+					  DB8500_PIN_AG28, DB8500_PIN_AG26 };
+static const unsigned mc4_a_1_pins[] = { DB8500_PIN_AH24, DB8500_PIN_AG25,
+	DB8500_PIN_AH23, DB8500_PIN_AH26, DB8500_PIN_AF24, DB8500_PIN_AF25,
+	DB8500_PIN_AE23, DB8500_PIN_AF23, DB8500_PIN_AG23, DB8500_PIN_AG24,
+	DB8500_PIN_AJ23 };
+/* MC1 has only 4 data pins, designed for SD or SDIO exclusively */
+static const unsigned mc1_a_1_pins[] = { DB8500_PIN_AH16, DB8500_PIN_AG15,
+	DB8500_PIN_AJ15, DB8500_PIN_AG14, DB8500_PIN_AF13, DB8500_PIN_AG13,
+	DB8500_PIN_AH15 };
+static const unsigned mc1dir_a_1_pins[] = { DB8500_PIN_AH13, DB8500_PIN_AG12,
+	DB8500_PIN_AH12, DB8500_PIN_AH11 };
+static const unsigned hsir_a_1_pins[] = { DB8500_PIN_AG10, DB8500_PIN_AH10 };
+static const unsigned hsit_a_1_pins[] = { DB8500_PIN_AJ11, DB8500_PIN_AJ9,
+	DB8500_PIN_AH9, DB8500_PIN_AG9, DB8500_PIN_AG8, DB8500_PIN_AF8 };
+static const unsigned clkout_a_1_pins[] = { DB8500_PIN_AH7, DB8500_PIN_AJ6 };
+static const unsigned clkout_a_2_pins[] = { DB8500_PIN_AG7, DB8500_PIN_AF7 };
+static const unsigned usb_a_1_pins[] = { DB8500_PIN_AF28, DB8500_PIN_AE29,
+	DB8500_PIN_AD29, DB8500_PIN_AC29, DB8500_PIN_AD28, DB8500_PIN_AD26,
+	DB8500_PIN_AE26, DB8500_PIN_AG29, DB8500_PIN_AE27, DB8500_PIN_AD27,
+	DB8500_PIN_AC28, DB8500_PIN_AC27 };
+
+/* Altfunction B column */
+static const unsigned trig_b_1_pins[] = { DB8500_PIN_AJ5, DB8500_PIN_AJ3 };
+static const unsigned i2c4_b_1_pins[] = { DB8500_PIN_AH6, DB8500_PIN_AG6 };
+static const unsigned i2c1_b_1_pins[] = { DB8500_PIN_AF6, DB8500_PIN_AG5 };
+static const unsigned i2c2_b_1_pins[] = { DB8500_PIN_AD5, DB8500_PIN_AE4 };
+static const unsigned i2c2_b_2_pins[] = { DB8500_PIN_AF5, DB8500_PIN_AG4 };
+static const unsigned msp0txrx_b_1_pins[] = { DB8500_PIN_AC4, DB8500_PIN_AC3 };
+static const unsigned i2c1_b_2_pins[] = { DB8500_PIN_AD3, DB8500_PIN_AD4 };
+/* Just RX and TX for UART2 */
+static const unsigned u2rxtx_b_1_pins[] = { DB8500_PIN_AC2, DB8500_PIN_AC1 };
+static const unsigned uartmodtx_b_1_pins[] = { DB8500_PIN_AB4 };
+static const unsigned msp0sck_b_1_pins[] = { DB8500_PIN_AB3 };
+static const unsigned uartmodrx_b_1_pins[] = { DB8500_PIN_AA3 };
+static const unsigned stmmod_b_1_pins[] = { DB8500_PIN_AA4, DB8500_PIN_Y4,
+	DB8500_PIN_Y2, DB8500_PIN_AA2, DB8500_PIN_AA1 };
+static const unsigned uartmodrx_b_2_pins[] = { DB8500_PIN_AB2 };
+static const unsigned spi3_b_1_pins[] = { DB8500_PIN_W2, DB8500_PIN_W3,
+					  DB8500_PIN_V3, DB8500_PIN_V2 };
+static const unsigned msp1txrx_b_1_pins[] = { DB8500_PIN_AF2, DB8500_PIN_AG2 };
+static const unsigned kp_b_1_pins[] = { DB8500_PIN_F3, DB8500_PIN_F1,
+	DB8500_PIN_G3, DB8500_PIN_G2, DB8500_PIN_E1, DB8500_PIN_E2,
+	DB8500_PIN_G5, DB8500_PIN_G4, DB8500_PIN_H4, DB8500_PIN_H3,
+	DB8500_PIN_J3, DB8500_PIN_H2, DB8500_PIN_J2, DB8500_PIN_H1,
+	DB8500_PIN_F4, DB8500_PIN_E3, DB8500_PIN_E4, DB8500_PIN_D2,
+	DB8500_PIN_C1, DB8500_PIN_D3, DB8500_PIN_C2, DB8500_PIN_D5 };
+static const unsigned sm_b_1_pins[] = { DB8500_PIN_C6, DB8500_PIN_B3,
+	DB8500_PIN_C4, DB8500_PIN_E6, DB8500_PIN_A3, DB8500_PIN_B6,
+	DB8500_PIN_D6, DB8500_PIN_B7, DB8500_PIN_D7, DB8500_PIN_D8,
+	DB8500_PIN_D9, DB8500_PIN_A5, DB8500_PIN_B4, DB8500_PIN_C8,
+	DB8500_PIN_A12, DB8500_PIN_C10, DB8500_PIN_B10, DB8500_PIN_B9,
+	DB8500_PIN_A9, DB8500_PIN_C7, DB8500_PIN_A7, DB8500_PIN_C5,
+	DB8500_PIN_C9, DB8500_PIN_B14 };
+/* This chip select pin can be "ps0" in alt B so have it separately */
+static const unsigned smcs0_b_1_pins[] = { DB8500_PIN_E8 };
+static const unsigned ipgpio7_b_1_pins[] = { DB8500_PIN_B11 };
+static const unsigned ipgpio2_b_1_pins[] = { DB8500_PIN_C12 };
+static const unsigned ipgpio3_b_1_pins[] = { DB8500_PIN_C11 };
+static const unsigned lcdaclk_b_1_pins[] = { DB8500_PIN_C14 };
+static const unsigned lcda_b_1_pins[] = { DB8500_PIN_D22,
+	DB8500_PIN_C23, DB8500_PIN_D23 };
+static const unsigned lcd_b_1_pins[] = { DB8500_PIN_D17, DB8500_PIN_D16,
+	DB8500_PIN_B17, DB8500_PIN_C16, DB8500_PIN_C19, DB8500_PIN_C17,
+	DB8500_PIN_A18, DB8500_PIN_C18, DB8500_PIN_B19, DB8500_PIN_B20,
+	DB8500_PIN_D21, DB8500_PIN_D20, DB8500_PIN_C20, DB8500_PIN_B21,
+	DB8500_PIN_C21, DB8500_PIN_A22, DB8500_PIN_B24, DB8500_PIN_C22 };
+static const unsigned ddrtrig_b_1_pins[] = { DB8500_PIN_AJ27 };
+static const unsigned pwl_b_1_pins[] = { DB8500_PIN_AF25 };
+static const unsigned spi1_b_1_pins[] = { DB8500_PIN_AG15, DB8500_PIN_AF13,
+					  DB8500_PIN_AG13, DB8500_PIN_AH15 };
+static const unsigned mc3_b_1_pins[] = { DB8500_PIN_AH13, DB8500_PIN_AG12,
+	DB8500_PIN_AH12, DB8500_PIN_AH11, DB8500_PIN_AG10, DB8500_PIN_AH10,
+	DB8500_PIN_AJ11, DB8500_PIN_AJ9, DB8500_PIN_AH9, DB8500_PIN_AG9,
+	DB8500_PIN_AG8 };
+static const unsigned pwl_b_2_pins[] = { DB8500_PIN_AF8 };
+static const unsigned pwl_b_3_pins[] = { DB8500_PIN_AG7 };
+static const unsigned pwl_b_4_pins[] = { DB8500_PIN_AF7 };
+
+/* Altfunction C column */
+static const unsigned ipjtag_c_1_pins[] = { DB8500_PIN_AJ5, DB8500_PIN_AJ3,
+	DB8500_PIN_AH4, DB8500_PIN_AH3, DB8500_PIN_AH6 };
+static const unsigned ipgpio6_c_1_pins[] = { DB8500_PIN_AG6 };
+static const unsigned ipgpio0_c_1_pins[] = { DB8500_PIN_AF6 };
+static const unsigned ipgpio1_c_1_pins[] = { DB8500_PIN_AG5 };
+static const unsigned ipgpio3_c_1_pins[] = { DB8500_PIN_AF5 };
+static const unsigned ipgpio2_c_1_pins[] = { DB8500_PIN_AG4 };
+static const unsigned slim0_c_1_pins[] = { DB8500_PIN_AD3, DB8500_PIN_AD4 };
+/* Optional 4-bit Memory Stick interface */
+static const unsigned ms_c_1_pins[] = { DB8500_PIN_AC2, DB8500_PIN_AC1,
+	DB8500_PIN_AB3, DB8500_PIN_AA3, DB8500_PIN_AA4, DB8500_PIN_AB2,
+	DB8500_PIN_Y4, DB8500_PIN_Y2, DB8500_PIN_AA2, DB8500_PIN_AA1 };
+static const unsigned iptrigout_c_1_pins[] = { DB8500_PIN_AB4 };
+static const unsigned u2rxtx_c_1_pins[] = { DB8500_PIN_W2, DB8500_PIN_W3 };
+static const unsigned u2ctsrts_c_1_pins[] = { DB8500_PIN_V3, DB8500_PIN_V2 };
+static const unsigned u0_c_1_pins[] = { DB8500_PIN_AF2, DB8500_PIN_AE1,
+					DB8500_PIN_AE2, DB8500_PIN_AG2 };
+static const unsigned ipgpio4_c_1_pins[] = { DB8500_PIN_F3 };
+static const unsigned ipgpio5_c_1_pins[] = { DB8500_PIN_F1 };
+static const unsigned ipgpio6_c_2_pins[] = { DB8500_PIN_G3 };
+static const unsigned ipgpio7_c_1_pins[] = { DB8500_PIN_G2 };
+static const unsigned smcleale_c_1_pins[] = { DB8500_PIN_E1, DB8500_PIN_E2 };
+static const unsigned stmape_c_1_pins[] = { DB8500_PIN_G5, DB8500_PIN_G4,
+	DB8500_PIN_H4, DB8500_PIN_H3, DB8500_PIN_J3 };
+static const unsigned u2rxtx_c_2_pins[] = { DB8500_PIN_H2, DB8500_PIN_J2 };
+static const unsigned ipgpio2_c_2_pins[] = { DB8500_PIN_F4 };
+static const unsigned ipgpio3_c_2_pins[] = { DB8500_PIN_E3 };
+static const unsigned ipgpio4_c_2_pins[] = { DB8500_PIN_E4 };
+static const unsigned ipgpio5_c_2_pins[] = { DB8500_PIN_D2 };
+static const unsigned mc5_c_1_pins[] = { DB8500_PIN_C6, DB8500_PIN_B3,
+	DB8500_PIN_C4, DB8500_PIN_E6, DB8500_PIN_A3, DB8500_PIN_B6,
+	DB8500_PIN_D6, DB8500_PIN_B7, DB8500_PIN_D7, DB8500_PIN_D8,
+	DB8500_PIN_D9 };
+static const unsigned mc2rstn_c_1_pins[] = { DB8500_PIN_C8 };
+static const unsigned kp_c_1_pins[] = { DB8500_PIN_C9, DB8500_PIN_B11,
+	DB8500_PIN_C12, DB8500_PIN_C11, DB8500_PIN_D17, DB8500_PIN_D16,
+	DB8500_PIN_C23, DB8500_PIN_D23 };
+static const unsigned smps1_c_1_pins[] = { DB8500_PIN_B14 };
+static const unsigned u2rxtx_c_3_pins[] = { DB8500_PIN_B17, DB8500_PIN_C16 };
+static const unsigned stmape_c_2_pins[] = { DB8500_PIN_C19, DB8500_PIN_C17,
+	DB8500_PIN_A18, DB8500_PIN_C18, DB8500_PIN_B19 };
+static const unsigned uartmodrx_c_1_pins[] = { DB8500_PIN_D21 };
+static const unsigned uartmodtx_c_1_pins[] = { DB8500_PIN_D20 };
+static const unsigned stmmod_c_1_pins[] = { DB8500_PIN_C20, DB8500_PIN_B21,
+	DB8500_PIN_C21, DB8500_PIN_A22, DB8500_PIN_B24 };
+static const unsigned usbsim_c_1_pins[] = { DB8500_PIN_D22 };
+static const unsigned mc4rstn_c_1_pins[] = { DB8500_PIN_AF25 };
+static const unsigned clkout_c_1_pins[] = { DB8500_PIN_AH13, DB8500_PIN_AH12 };
+static const unsigned i2c3_c_1_pins[] = { DB8500_PIN_AG12, DB8500_PIN_AH11 };
+static const unsigned spi0_c_1_pins[] = { DB8500_PIN_AH10, DB8500_PIN_AH9,
+					  DB8500_PIN_AG9, DB8500_PIN_AG8 };
+static const unsigned usbsim_c_2_pins[] = { DB8500_PIN_AF8 };
+static const unsigned i2c3_c_2_pins[] = { DB8500_PIN_AG7, DB8500_PIN_AF7 };
+
+/* Other C1 column */
+static const unsigned kp_oc1_1_pins[] = { DB8500_PIN_C6, DB8500_PIN_B3,
+	DB8500_PIN_C4, DB8500_PIN_E6, DB8500_PIN_A3, DB8500_PIN_B6,
+	DB8500_PIN_D6, DB8500_PIN_B7 };
+static const unsigned spi2_oc1_1_pins[] = { DB8500_PIN_AH13, DB8500_PIN_AG12,
+	DB8500_PIN_AH12, DB8500_PIN_AH11 };
+
+#define DB8500_PIN_GROUP(a,b) { .name = #a, .pins = a##_pins,		\
+			.npins = ARRAY_SIZE(a##_pins), .altsetting = b }
+
+static const struct nmk_pingroup nmk_db8500_groups[] = {
+	/* Altfunction A column */
+	DB8500_PIN_GROUP(u0_a_1, NMK_GPIO_ALT_A),
+	DB8500_PIN_GROUP(u1rxtx_a_1, NMK_GPIO_ALT_A),
+	DB8500_PIN_GROUP(u1ctsrts_a_1, NMK_GPIO_ALT_A),
+	DB8500_PIN_GROUP(ipi2c_a_1, NMK_GPIO_ALT_A),
+	DB8500_PIN_GROUP(ipi2c_a_2, NMK_GPIO_ALT_A),
+	DB8500_PIN_GROUP(msp0txrx_a_1, NMK_GPIO_ALT_A),
+	DB8500_PIN_GROUP(msp0tfstck_a_1, NMK_GPIO_ALT_A),
+	DB8500_PIN_GROUP(msp0rfsrck_a_1, NMK_GPIO_ALT_A),
+	DB8500_PIN_GROUP(mc0_a_1, NMK_GPIO_ALT_A),
+	DB8500_PIN_GROUP(msp1txrx_a_1, NMK_GPIO_ALT_A),
+	DB8500_PIN_GROUP(msp1_a_1, NMK_GPIO_ALT_A),
+	DB8500_PIN_GROUP(lcdb_a_1, NMK_GPIO_ALT_A),
+	DB8500_PIN_GROUP(lcdvsi0_a_1, NMK_GPIO_ALT_A),
+	DB8500_PIN_GROUP(lcdvsi1_a_1, NMK_GPIO_ALT_A),
+	DB8500_PIN_GROUP(lcd_d0_d7_a_1, NMK_GPIO_ALT_A),
+	DB8500_PIN_GROUP(lcd_d8_d11_a_1, NMK_GPIO_ALT_A),
+	DB8500_PIN_GROUP(lcd_d12_d23_a_1, NMK_GPIO_ALT_A),
+	DB8500_PIN_GROUP(kp_a_1, NMK_GPIO_ALT_A),
+	DB8500_PIN_GROUP(mc2_a_1, NMK_GPIO_ALT_A),
+	DB8500_PIN_GROUP(ssp1_a_1, NMK_GPIO_ALT_A),
+	DB8500_PIN_GROUP(ssp0_a_1, NMK_GPIO_ALT_A),
+	DB8500_PIN_GROUP(i2c0_a_1, NMK_GPIO_ALT_A),
+	DB8500_PIN_GROUP(ipgpio0_a_1, NMK_GPIO_ALT_A),
+	DB8500_PIN_GROUP(ipgpio1_a_1, NMK_GPIO_ALT_A),
+	DB8500_PIN_GROUP(msp2sck_a_1, NMK_GPIO_ALT_A),
+	DB8500_PIN_GROUP(msp2_a_1, NMK_GPIO_ALT_A),
+	DB8500_PIN_GROUP(mc4_a_1, NMK_GPIO_ALT_A),
+	DB8500_PIN_GROUP(mc1_a_1, NMK_GPIO_ALT_A),
+	DB8500_PIN_GROUP(hsir_a_1, NMK_GPIO_ALT_A),
+	DB8500_PIN_GROUP(hsit_a_1, NMK_GPIO_ALT_A),
+	DB8500_PIN_GROUP(clkout_a_1, NMK_GPIO_ALT_A),
+	DB8500_PIN_GROUP(clkout_a_2, NMK_GPIO_ALT_A),
+	DB8500_PIN_GROUP(usb_a_1, NMK_GPIO_ALT_A),
+	/* Altfunction B column */
+	DB8500_PIN_GROUP(trig_b_1, NMK_GPIO_ALT_B),
+	DB8500_PIN_GROUP(i2c4_b_1, NMK_GPIO_ALT_B),
+	DB8500_PIN_GROUP(i2c1_b_1, NMK_GPIO_ALT_B),
+	DB8500_PIN_GROUP(i2c2_b_1, NMK_GPIO_ALT_B),
+	DB8500_PIN_GROUP(i2c2_b_2, NMK_GPIO_ALT_B),
+	DB8500_PIN_GROUP(msp0txrx_b_1, NMK_GPIO_ALT_B),
+	DB8500_PIN_GROUP(i2c1_b_2, NMK_GPIO_ALT_B),
+	DB8500_PIN_GROUP(u2rxtx_b_1, NMK_GPIO_ALT_B),
+	DB8500_PIN_GROUP(uartmodtx_b_1, NMK_GPIO_ALT_B),
+	DB8500_PIN_GROUP(msp0sck_b_1, NMK_GPIO_ALT_B),
+	DB8500_PIN_GROUP(uartmodrx_b_1, NMK_GPIO_ALT_B),
+	DB8500_PIN_GROUP(stmmod_b_1, NMK_GPIO_ALT_B),
+	DB8500_PIN_GROUP(uartmodrx_b_2, NMK_GPIO_ALT_B),
+	DB8500_PIN_GROUP(spi3_b_1, NMK_GPIO_ALT_B),
+	DB8500_PIN_GROUP(msp1txrx_b_1, NMK_GPIO_ALT_B),
+	DB8500_PIN_GROUP(kp_b_1, NMK_GPIO_ALT_B),
+	DB8500_PIN_GROUP(sm_b_1, NMK_GPIO_ALT_B),
+	DB8500_PIN_GROUP(smcs0_b_1, NMK_GPIO_ALT_B),
+	DB8500_PIN_GROUP(ipgpio7_b_1, NMK_GPIO_ALT_B),
+	DB8500_PIN_GROUP(ipgpio2_b_1, NMK_GPIO_ALT_B),
+	DB8500_PIN_GROUP(ipgpio3_b_1, NMK_GPIO_ALT_B),
+	DB8500_PIN_GROUP(lcdaclk_b_1, NMK_GPIO_ALT_B),
+	DB8500_PIN_GROUP(lcda_b_1, NMK_GPIO_ALT_B),
+	DB8500_PIN_GROUP(lcd_b_1, NMK_GPIO_ALT_B),
+	DB8500_PIN_GROUP(ddrtrig_b_1, NMK_GPIO_ALT_B),
+	DB8500_PIN_GROUP(pwl_b_1, NMK_GPIO_ALT_B),
+	DB8500_PIN_GROUP(spi1_b_1, NMK_GPIO_ALT_B),
+	DB8500_PIN_GROUP(mc3_b_1, NMK_GPIO_ALT_B),
+	DB8500_PIN_GROUP(pwl_b_2, NMK_GPIO_ALT_B),
+	DB8500_PIN_GROUP(pwl_b_3, NMK_GPIO_ALT_B),
+	DB8500_PIN_GROUP(pwl_b_4, NMK_GPIO_ALT_B),
+	/* Altfunction C column */
+	DB8500_PIN_GROUP(ipjtag_c_1, NMK_GPIO_ALT_C),
+	DB8500_PIN_GROUP(ipgpio6_c_1, NMK_GPIO_ALT_C),
+	DB8500_PIN_GROUP(ipgpio0_c_1, NMK_GPIO_ALT_C),
+	DB8500_PIN_GROUP(ipgpio1_c_1, NMK_GPIO_ALT_C),
+	DB8500_PIN_GROUP(ipgpio3_c_1, NMK_GPIO_ALT_C),
+	DB8500_PIN_GROUP(ipgpio2_c_1, NMK_GPIO_ALT_C),
+	DB8500_PIN_GROUP(slim0_c_1, NMK_GPIO_ALT_C),
+	DB8500_PIN_GROUP(ms_c_1, NMK_GPIO_ALT_C),
+	DB8500_PIN_GROUP(iptrigout_c_1, NMK_GPIO_ALT_C),
+	DB8500_PIN_GROUP(u2rxtx_c_1, NMK_GPIO_ALT_C),
+	DB8500_PIN_GROUP(u2ctsrts_c_1, NMK_GPIO_ALT_C),
+	DB8500_PIN_GROUP(u0_c_1, NMK_GPIO_ALT_C),
+	DB8500_PIN_GROUP(ipgpio4_c_1, NMK_GPIO_ALT_C),
+	DB8500_PIN_GROUP(ipgpio5_c_1, NMK_GPIO_ALT_C),
+	DB8500_PIN_GROUP(ipgpio6_c_1, NMK_GPIO_ALT_C),
+	DB8500_PIN_GROUP(ipgpio7_c_1, NMK_GPIO_ALT_C),
+	DB8500_PIN_GROUP(smcleale_c_1, NMK_GPIO_ALT_C),
+	DB8500_PIN_GROUP(stmape_c_1, NMK_GPIO_ALT_C),
+	DB8500_PIN_GROUP(u2rxtx_c_2, NMK_GPIO_ALT_C),
+	DB8500_PIN_GROUP(ipgpio2_c_2, NMK_GPIO_ALT_C),
+	DB8500_PIN_GROUP(ipgpio3_c_2, NMK_GPIO_ALT_C),
+	DB8500_PIN_GROUP(ipgpio4_c_2, NMK_GPIO_ALT_C),
+	DB8500_PIN_GROUP(ipgpio5_c_2, NMK_GPIO_ALT_C),
+	DB8500_PIN_GROUP(mc5_c_1, NMK_GPIO_ALT_C),
+	DB8500_PIN_GROUP(mc2rstn_c_1, NMK_GPIO_ALT_C),
+	DB8500_PIN_GROUP(kp_c_1, NMK_GPIO_ALT_C),
+	DB8500_PIN_GROUP(smps1_c_1, NMK_GPIO_ALT_C),
+	DB8500_PIN_GROUP(u2rxtx_c_3, NMK_GPIO_ALT_C),
+	DB8500_PIN_GROUP(stmape_c_2, NMK_GPIO_ALT_C),
+	DB8500_PIN_GROUP(uartmodrx_c_1, NMK_GPIO_ALT_C),
+	DB8500_PIN_GROUP(uartmodtx_c_1, NMK_GPIO_ALT_C),
+	DB8500_PIN_GROUP(stmmod_c_1, NMK_GPIO_ALT_C),
+	DB8500_PIN_GROUP(usbsim_c_1, NMK_GPIO_ALT_C),
+	DB8500_PIN_GROUP(mc4rstn_c_1, NMK_GPIO_ALT_C),
+	DB8500_PIN_GROUP(clkout_c_1, NMK_GPIO_ALT_C),
+	DB8500_PIN_GROUP(i2c3_c_1, NMK_GPIO_ALT_C),
+	DB8500_PIN_GROUP(spi0_c_1, NMK_GPIO_ALT_C),
+	DB8500_PIN_GROUP(usbsim_c_2, NMK_GPIO_ALT_C),
+	DB8500_PIN_GROUP(i2c3_c_2, NMK_GPIO_ALT_C),
+	/* Other alt C1 column, these are still configured as alt C */
+	DB8500_PIN_GROUP(kp_oc1_1, NMK_GPIO_ALT_C),
+	DB8500_PIN_GROUP(spi2_oc1_1, NMK_GPIO_ALT_C),
+};
+
+/* We use this macro to define the groups applicable to a function */
+#define DB8500_FUNC_GROUPS(a, b...)	   \
+static const char * const a##_groups[] = { b };
+
+DB8500_FUNC_GROUPS(u0, "u0_a_1", "u0_c_1");
+DB8500_FUNC_GROUPS(u1, "u1rxtx_a_1", "u1ctsrts_a_1");
+/*
+ * UART2 can be muxed out with just RX/TX in four places, CTS+RTS is however
+ * only available on two pins in alternative function C
+ */
+DB8500_FUNC_GROUPS(u2, "u2rxtx_b_1", "u2rxtx_c_1", "u2ctsrts_c_1",
+		   "u2rxtx_c_2", "u2rxtx_c_3");
+DB8500_FUNC_GROUPS(ipi2c, "ipi2c_a_1", "ipi2c_a_2");
+/*
+ * MSP0 can only be on a certain set of pins, but the TX/RX pins can be
+ * switched around by selecting the altfunction A or B. The SCK pin is
+ * only available on the altfunction B.
+ */
+DB8500_FUNC_GROUPS(msp0, "msp0txrx_a_1", "msp0tfstck_a_1", "msp0rfstck_a_1",
+		   "msp0txrx_b_1", "msp0sck_b_1");
+DB8500_FUNC_GROUPS(mc0, "mc0_a_1");
+/* MSP0 can swap RX/TX like MSP0 but has no SCK pin available */
+DB8500_FUNC_GROUPS(msp1, "msp1txrx_a_1", "msp1_a_1", "msp1txrx_b_1");
+DB8500_FUNC_GROUPS(lcdb, "lcdb_a_1");
+DB8500_FUNC_GROUPS(lcd, "lcdvsi0_a_1", "lcdvsi1_a_1", "lcd_d0_d7_a_1",
+	"lcd_d8_d11_a_1", "lcd_d12_d23_a_1", "lcd_b_1");
+DB8500_FUNC_GROUPS(kp, "kp_a_1", "kp_b_1", "kp_c_1", "kp_oc1_1");
+DB8500_FUNC_GROUPS(mc2, "mc2_a_1", "mc2rstn_c_1");
+DB8500_FUNC_GROUPS(ssp1, "ssp1_a_1");
+DB8500_FUNC_GROUPS(ssp0, "ssp0_a_1");
+DB8500_FUNC_GROUPS(i2c0, "i2c0_a_1");
+/* The image processor has 8 GPIO pins that can be muxed out */
+DB8500_FUNC_GROUPS(ipgpio, "ipgpio0_a_1", "ipgpio1_a_1", "ipgpio7_b_1",
+	"ipgpio2_b_1", "ipgpio3_b_1", "ipgpio6_c_1", "ipgpio0_c_1",
+	"ipgpio1_c_1", "ipgpio3_c_1", "ipgpio2_c_1", "ipgpio4_c_1",
+	"ipgpio5_c_1", "ipgpio6_c_2", "ipgpio7_c_1", "ipgpio2_c_2",
+	"ipgpio3_c_2", "ipgpio4_c_2", "ipgpio5_c_2");
+/* MSP2 can not invert the RX/TX pins but has the optional SCK pin */
+DB8500_FUNC_GROUPS(msp2, "msp2sck_a_1", "msp2_a_1");
+DB8500_FUNC_GROUPS(mc4, "mc4_a_1", "mc4rstn_c_1");
+DB8500_FUNC_GROUPS(mc1, "mc1_a_1", "mc1dir_a_1");
+DB8500_FUNC_GROUPS(hsi, "hsir1_a_1", "hsit1_a_1");
+DB8500_FUNC_GROUPS(clkout, "clkout_a_1", "clkout_a_2", "clkout_c_1");
+DB8500_FUNC_GROUPS(usb, "usb_a_1");
+DB8500_FUNC_GROUPS(trig, "trig_b_1");
+DB8500_FUNC_GROUPS(i2c4, "i2c4_b_1");
+DB8500_FUNC_GROUPS(i2c1, "i2c1_b_1", "i2c1_b_2");
+DB8500_FUNC_GROUPS(i2c2, "i2c2_b_1", "i2c2_b_2");
+/*
+ * The modem UART can output its RX and TX pins in some different places,
+ * so select one of each.
+ */
+DB8500_FUNC_GROUPS(uartmod, "uartmodtx_b_1", "uartmodrx_b_1", "uartmodrx_b_2",
+		   "uartmodrx_c_1", "uartmod_tx_c_1");
+DB8500_FUNC_GROUPS(stmmod, "stmmod_b_1", "stmmod_c_1");
+DB8500_FUNC_GROUPS(spi3, "spi3_b_1");
+/* Select between CS0 on alt B or PS1 on alt C */
+DB8500_FUNC_GROUPS(sm, "sm_b_1", "smcs0_b_1", "smcleale_c_1", "smps1_c_1");
+DB8500_FUNC_GROUPS(lcda, "lcdaclk_b_1", "lcda_b_1");
+DB8500_FUNC_GROUPS(ddrtrig, "ddrtrig_b_1");
+DB8500_FUNC_GROUPS(pwl, "pwl_b_1", "pwl_b_2", "pwl_b_3", "pwl_b_4");
+DB8500_FUNC_GROUPS(spi1, "spi1_b_1");
+DB8500_FUNC_GROUPS(mc3, "mc3_b_1");
+DB8500_FUNC_GROUPS(ipjtag, "ipjtag_c_1");
+DB8500_FUNC_GROUPS(slim0, "slim0_c_1");
+DB8500_FUNC_GROUPS(ms, "ms_c_1");
+DB8500_FUNC_GROUPS(iptrigout, "iptrigout_c_1");
+DB8500_FUNC_GROUPS(stmape, "stmape_c_1", "stmape_c_2");
+DB8500_FUNC_GROUPS(mc5, "mc5_c_1");
+DB8500_FUNC_GROUPS(usbsim, "usbsim_c_1", "usbsim_c_2");
+DB8500_FUNC_GROUPS(i2c3, "i2c3_c_1", "i2c3_c_2");
+DB8500_FUNC_GROUPS(spi0, "spi0_c_1");
+DB8500_FUNC_GROUPS(spi2, "spi2_oc1_1");
+
+#define FUNCTION(fname)					\
+	{						\
+		.name = #fname,				\
+		.groups = fname##_groups,		\
+		.ngroups = ARRAY_SIZE(fname##_groups),	\
+	}
+
+static const struct nmk_function nmk_db8500_functions[] = {
+	FUNCTION(u0),
+	FUNCTION(u1),
+	FUNCTION(u2),
+	FUNCTION(ipi2c),
+	FUNCTION(msp0),
+	FUNCTION(mc0),
+	FUNCTION(msp1),
+	FUNCTION(lcdb),
+	FUNCTION(lcd),
+	FUNCTION(kp),
+	FUNCTION(mc2),
+	FUNCTION(ssp1),
+	FUNCTION(ssp0),
+	FUNCTION(i2c0),
+	FUNCTION(ipgpio),
+	FUNCTION(msp2),
+	FUNCTION(mc4),
+	FUNCTION(mc1),
+	FUNCTION(hsi),
+	FUNCTION(clkout),
+	FUNCTION(usb),
+	FUNCTION(trig),
+	FUNCTION(i2c4),
+	FUNCTION(i2c1),
+	FUNCTION(i2c2),
+	FUNCTION(uartmod),
+	FUNCTION(stmmod),
+	FUNCTION(spi3),
+	FUNCTION(sm),
+	FUNCTION(lcda),
+	FUNCTION(ddrtrig),
+	FUNCTION(pwl),
+	FUNCTION(spi1),
+	FUNCTION(mc3),
+	FUNCTION(ipjtag),
+	FUNCTION(slim0),
+	FUNCTION(ms),
+	FUNCTION(iptrigout),
+	FUNCTION(stmape),
+	FUNCTION(mc5),
+	FUNCTION(usbsim),
+	FUNCTION(i2c3),
+	FUNCTION(spi0),
+	FUNCTION(spi2),
+};
+
+static const struct nmk_pinctrl_soc_data nmk_db8500_soc = {
+	.gpio_ranges = nmk_db8500_ranges,
+	.gpio_num_ranges = ARRAY_SIZE(nmk_db8500_ranges),
+	.pins = nmk_db8500_pins,
+	.npins = ARRAY_SIZE(nmk_db8500_pins),
+	.functions = nmk_db8500_functions,
+	.nfunctions = ARRAY_SIZE(nmk_db8500_functions),
+	.groups = nmk_db8500_groups,
+	.ngroups = ARRAY_SIZE(nmk_db8500_groups),
+};
+
+void __devinit
+nmk_pinctrl_db8500_init(const struct nmk_pinctrl_soc_data **soc)
+{
+	*soc = &nmk_db8500_soc;
+}
diff --git a/drivers/gpio/gpio-nomadik.c b/drivers/pinctrl/pinctrl-nomadik.c
index 839624f9fe6..b8e01c3eaa9 100644
--- a/drivers/gpio/gpio-nomadik.c
+++ b/drivers/pinctrl/pinctrl-nomadik.c
@@ -22,14 +22,20 @@
 #include <linux/spinlock.h>
 #include <linux/interrupt.h>
 #include <linux/irq.h>
+#include <linux/irqdomain.h>
 #include <linux/slab.h>
+#include <linux/pinctrl/pinctrl.h>
+#include <linux/pinctrl/pinmux.h>
+#include <linux/pinctrl/pinconf.h>
+/* Since we request GPIOs from ourself */
+#include <linux/pinctrl/consumer.h>
 
 #include <asm/mach/irq.h>
 
 #include <plat/pincfg.h>
 #include <plat/gpio-nomadik.h>
-#include <mach/hardware.h>
-#include <asm/gpio.h>
+
+#include "pinctrl-nomadik.h"
 
 /*
  * The GPIO module in the Nomadik family of Systems-on-Chip is an
@@ -43,6 +49,7 @@
 
 struct nmk_gpio_chip {
 	struct gpio_chip chip;
+	struct irq_domain *domain;
 	void __iomem *addr;
 	struct clk *clk;
 	unsigned int bank;
@@ -58,8 +65,16 @@ struct nmk_gpio_chip {
 	u32 real_wake;
 	u32 rwimsc;
 	u32 fwimsc;
-	u32 slpm;
+	u32 rimsc;
+	u32 fimsc;
 	u32 pull_up;
+	u32 lowemi;
+};
+
+struct nmk_pinctrl {
+	struct device *dev;
+	struct pinctrl_dev *pctl;
+	const struct nmk_pinctrl_soc_data *soc;
 };
 
 static struct nmk_gpio_chip *
@@ -124,6 +139,24 @@ static void __nmk_gpio_set_pull(struct nmk_gpio_chip *nmk_chip,
 	}
 }
 
+static void __nmk_gpio_set_lowemi(struct nmk_gpio_chip *nmk_chip,
+				  unsigned offset, bool lowemi)
+{
+	u32 bit = BIT(offset);
+	bool enabled = nmk_chip->lowemi & bit;
+
+	if (lowemi == enabled)
+		return;
+
+	if (lowemi)
+		nmk_chip->lowemi |= bit;
+	else
+		nmk_chip->lowemi &= ~bit;
+
+	writel_relaxed(nmk_chip->lowemi,
+		       nmk_chip->addr + NMK_GPIO_LOWEMI);
+}
+
 static void __nmk_gpio_make_input(struct nmk_gpio_chip *nmk_chip,
 				  unsigned offset)
 {
@@ -150,8 +183,8 @@ static void __nmk_gpio_set_mode_safe(struct nmk_gpio_chip *nmk_chip,
 				     unsigned offset, int gpio_mode,
 				     bool glitch)
 {
-	u32 rwimsc = readl(nmk_chip->addr + NMK_GPIO_RWIMSC);
-	u32 fwimsc = readl(nmk_chip->addr + NMK_GPIO_FWIMSC);
+	u32 rwimsc = nmk_chip->rwimsc;
+	u32 fwimsc = nmk_chip->fwimsc;
 
 	if (glitch && nmk_chip->set_ioforce) {
 		u32 bit = BIT(offset);
@@ -173,6 +206,36 @@ static void __nmk_gpio_set_mode_safe(struct nmk_gpio_chip *nmk_chip,
 	}
 }
 
+static void
+nmk_gpio_disable_lazy_irq(struct nmk_gpio_chip *nmk_chip, unsigned offset)
+{
+	u32 falling = nmk_chip->fimsc & BIT(offset);
+	u32 rising = nmk_chip->rimsc & BIT(offset);
+	int gpio = nmk_chip->chip.base + offset;
+	int irq = NOMADIK_GPIO_TO_IRQ(gpio);
+	struct irq_data *d = irq_get_irq_data(irq);
+
+	if (!rising && !falling)
+		return;
+
+	if (!d || !irqd_irq_disabled(d))
+		return;
+
+	if (rising) {
+		nmk_chip->rimsc &= ~BIT(offset);
+		writel_relaxed(nmk_chip->rimsc,
+			       nmk_chip->addr + NMK_GPIO_RIMSC);
+	}
+
+	if (falling) {
+		nmk_chip->fimsc &= ~BIT(offset);
+		writel_relaxed(nmk_chip->fimsc,
+			       nmk_chip->addr + NMK_GPIO_FIMSC);
+	}
+
+	dev_dbg(nmk_chip->chip.dev, "%d: clearing interrupt mask\n", gpio);
+}
+
 static void __nmk_config_pin(struct nmk_gpio_chip *nmk_chip, unsigned offset,
 			     pin_cfg_t cfg, bool sleep, unsigned int *slpmregs)
 {
@@ -238,6 +301,17 @@ static void __nmk_config_pin(struct nmk_gpio_chip *nmk_chip, unsigned offset,
 		__nmk_gpio_set_pull(nmk_chip, offset, pull);
 	}
 
+	__nmk_gpio_set_lowemi(nmk_chip, offset, PIN_LOWEMI(cfg));
+
+	/*
+	 * If the pin is switching to altfunc, and there was an interrupt
+	 * installed on it which has been lazy disabled, actually mask the
+	 * interrupt to prevent spurious interrupts that would occur while the
+	 * pin is under control of the peripheral.  Only SKE does this.
+	 */
+	if (af != NMK_GPIO_ALT_GPIO)
+		nmk_gpio_disable_lazy_irq(nmk_chip, offset);
+
 	/*
 	 * If we've backed up the SLPM registers (glitch workaround), modify
 	 * the backups since they will be restored.
@@ -334,7 +408,7 @@ static int __nmk_config_pins(pin_cfg_t *cfgs, int num, bool sleep)
 		struct nmk_gpio_chip *nmk_chip;
 		int pin = PIN_NUM(cfgs[i]);
 
-		nmk_chip = irq_get_chip_data(NOMADIK_GPIO_TO_IRQ(pin));
+		nmk_chip = nmk_gpio_chips[pin / NMK_GPIO_PER_CHIP];
 		if (!nmk_chip) {
 			ret = -EINVAL;
 			break;
@@ -342,7 +416,7 @@ static int __nmk_config_pins(pin_cfg_t *cfgs, int num, bool sleep)
 
 		clk_enable(nmk_chip->clk);
 		spin_lock(&nmk_chip->lock);
-		__nmk_config_pin(nmk_chip, pin - nmk_chip->chip.base,
+		__nmk_config_pin(nmk_chip, pin % NMK_GPIO_PER_CHIP,
 				 cfgs[i], sleep, glitch ? slpm : NULL);
 		spin_unlock(&nmk_chip->lock);
 		clk_disable(nmk_chip->clk);
@@ -426,7 +500,7 @@ int nmk_gpio_set_slpm(int gpio, enum nmk_gpio_slpm mode)
 	struct nmk_gpio_chip *nmk_chip;
 	unsigned long flags;
 
-	nmk_chip = irq_get_chip_data(NOMADIK_GPIO_TO_IRQ(gpio));
+	nmk_chip = nmk_gpio_chips[gpio / NMK_GPIO_PER_CHIP];
 	if (!nmk_chip)
 		return -EINVAL;
 
@@ -434,7 +508,7 @@ int nmk_gpio_set_slpm(int gpio, enum nmk_gpio_slpm mode)
 	spin_lock_irqsave(&nmk_gpio_slpm_lock, flags);
 	spin_lock(&nmk_chip->lock);
 
-	__nmk_gpio_set_slpm(nmk_chip, gpio - nmk_chip->chip.base, mode);
+	__nmk_gpio_set_slpm(nmk_chip, gpio % NMK_GPIO_PER_CHIP, mode);
 
 	spin_unlock(&nmk_chip->lock);
 	spin_unlock_irqrestore(&nmk_gpio_slpm_lock, flags);
@@ -461,13 +535,13 @@ int nmk_gpio_set_pull(int gpio, enum nmk_gpio_pull pull)
 	struct nmk_gpio_chip *nmk_chip;
 	unsigned long flags;
 
-	nmk_chip = irq_get_chip_data(NOMADIK_GPIO_TO_IRQ(gpio));
+	nmk_chip = nmk_gpio_chips[gpio / NMK_GPIO_PER_CHIP];
 	if (!nmk_chip)
 		return -EINVAL;
 
 	clk_enable(nmk_chip->clk);
 	spin_lock_irqsave(&nmk_chip->lock, flags);
-	__nmk_gpio_set_pull(nmk_chip, gpio - nmk_chip->chip.base, pull);
+	__nmk_gpio_set_pull(nmk_chip, gpio % NMK_GPIO_PER_CHIP, pull);
 	spin_unlock_irqrestore(&nmk_chip->lock, flags);
 	clk_disable(nmk_chip->clk);
 
@@ -489,13 +563,13 @@ int nmk_gpio_set_mode(int gpio, int gpio_mode)
 	struct nmk_gpio_chip *nmk_chip;
 	unsigned long flags;
 
-	nmk_chip = irq_get_chip_data(NOMADIK_GPIO_TO_IRQ(gpio));
+	nmk_chip = nmk_gpio_chips[gpio / NMK_GPIO_PER_CHIP];
 	if (!nmk_chip)
 		return -EINVAL;
 
 	clk_enable(nmk_chip->clk);
 	spin_lock_irqsave(&nmk_chip->lock, flags);
-	__nmk_gpio_set_mode(nmk_chip, gpio - nmk_chip->chip.base, gpio_mode);
+	__nmk_gpio_set_mode(nmk_chip, gpio % NMK_GPIO_PER_CHIP, gpio_mode);
 	spin_unlock_irqrestore(&nmk_chip->lock, flags);
 	clk_disable(nmk_chip->clk);
 
@@ -508,11 +582,11 @@ int nmk_gpio_get_mode(int gpio)
 	struct nmk_gpio_chip *nmk_chip;
 	u32 afunc, bfunc, bit;
 
-	nmk_chip = irq_get_chip_data(NOMADIK_GPIO_TO_IRQ(gpio));
+	nmk_chip = nmk_gpio_chips[gpio / NMK_GPIO_PER_CHIP];
 	if (!nmk_chip)
 		return -EINVAL;
 
-	bit = 1 << (gpio - nmk_chip->chip.base);
+	bit = 1 << (gpio % NMK_GPIO_PER_CHIP);
 
 	clk_enable(nmk_chip->clk);
 
@@ -529,21 +603,19 @@ EXPORT_SYMBOL(nmk_gpio_get_mode);
 /* IRQ functions */
 static inline int nmk_gpio_get_bitmask(int gpio)
 {
-	return 1 << (gpio % 32);
+	return 1 << (gpio % NMK_GPIO_PER_CHIP);
 }
 
 static void nmk_gpio_irq_ack(struct irq_data *d)
 {
-	int gpio;
 	struct nmk_gpio_chip *nmk_chip;
 
-	gpio = NOMADIK_IRQ_TO_GPIO(d->irq);
 	nmk_chip = irq_data_get_irq_chip_data(d);
 	if (!nmk_chip)
 		return;
 
 	clk_enable(nmk_chip->clk);
-	writel(nmk_gpio_get_bitmask(gpio), nmk_chip->addr + NMK_GPIO_IC);
+	writel(nmk_gpio_get_bitmask(d->hwirq), nmk_chip->addr + NMK_GPIO_IC);
 	clk_disable(nmk_chip->clk);
 }
 
@@ -556,37 +628,52 @@ static void __nmk_gpio_irq_modify(struct nmk_gpio_chip *nmk_chip,
 				  int gpio, enum nmk_gpio_irq_type which,
 				  bool enable)
 {
-	u32 rimsc = which == WAKE ? NMK_GPIO_RWIMSC : NMK_GPIO_RIMSC;
-	u32 fimsc = which == WAKE ? NMK_GPIO_FWIMSC : NMK_GPIO_FIMSC;
 	u32 bitmask = nmk_gpio_get_bitmask(gpio);
-	u32 reg;
+	u32 *rimscval;
+	u32 *fimscval;
+	u32 rimscreg;
+	u32 fimscreg;
+
+	if (which == NORMAL) {
+		rimscreg = NMK_GPIO_RIMSC;
+		fimscreg = NMK_GPIO_FIMSC;
+		rimscval = &nmk_chip->rimsc;
+		fimscval = &nmk_chip->fimsc;
+	} else  {
+		rimscreg = NMK_GPIO_RWIMSC;
+		fimscreg = NMK_GPIO_FWIMSC;
+		rimscval = &nmk_chip->rwimsc;
+		fimscval = &nmk_chip->fwimsc;
+	}
 
 	/* we must individually set/clear the two edges */
 	if (nmk_chip->edge_rising & bitmask) {
-		reg = readl(nmk_chip->addr + rimsc);
 		if (enable)
-			reg |= bitmask;
+			*rimscval |= bitmask;
 		else
-			reg &= ~bitmask;
-		writel(reg, nmk_chip->addr + rimsc);
+			*rimscval &= ~bitmask;
+		writel(*rimscval, nmk_chip->addr + rimscreg);
 	}
 	if (nmk_chip->edge_falling & bitmask) {
-		reg = readl(nmk_chip->addr + fimsc);
 		if (enable)
-			reg |= bitmask;
+			*fimscval |= bitmask;
 		else
-			reg &= ~bitmask;
-		writel(reg, nmk_chip->addr + fimsc);
+			*fimscval &= ~bitmask;
+		writel(*fimscval, nmk_chip->addr + fimscreg);
 	}
 }
 
 static void __nmk_gpio_set_wake(struct nmk_gpio_chip *nmk_chip,
 				int gpio, bool on)
 {
-	if (nmk_chip->sleepmode) {
-		__nmk_gpio_set_slpm(nmk_chip, gpio - nmk_chip->chip.base,
-				    on ? NMK_GPIO_SLPM_WAKEUP_ENABLE
-				    : NMK_GPIO_SLPM_WAKEUP_DISABLE);
+	/*
+	 * Ensure WAKEUP_ENABLE is on.  No need to disable it if wakeup is
+	 * disabled, since setting SLPM to 1 increases power consumption, and
+	 * wakeup is anyhow controlled by the RIMSC and FIMSC registers.
+	 */
+	if (nmk_chip->sleepmode && on) {
+		__nmk_gpio_set_slpm(nmk_chip, gpio % nmk_chip->chip.base,
+				    NMK_GPIO_SLPM_WAKEUP_ENABLE);
 	}
 
 	__nmk_gpio_irq_modify(nmk_chip, gpio, WAKE, on);
@@ -594,14 +681,12 @@ static void __nmk_gpio_set_wake(struct nmk_gpio_chip *nmk_chip,
 
 static int nmk_gpio_irq_maskunmask(struct irq_data *d, bool enable)
 {
-	int gpio;
 	struct nmk_gpio_chip *nmk_chip;
 	unsigned long flags;
 	u32 bitmask;
 
-	gpio = NOMADIK_IRQ_TO_GPIO(d->irq);
 	nmk_chip = irq_data_get_irq_chip_data(d);
-	bitmask = nmk_gpio_get_bitmask(gpio);
+	bitmask = nmk_gpio_get_bitmask(d->hwirq);
 	if (!nmk_chip)
 		return -EINVAL;
 
@@ -609,10 +694,10 @@ static int nmk_gpio_irq_maskunmask(struct irq_data *d, bool enable)
 	spin_lock_irqsave(&nmk_gpio_slpm_lock, flags);
 	spin_lock(&nmk_chip->lock);
 
-	__nmk_gpio_irq_modify(nmk_chip, gpio, NORMAL, enable);
+	__nmk_gpio_irq_modify(nmk_chip, d->hwirq, NORMAL, enable);
 
 	if (!(nmk_chip->real_wake & bitmask))
-		__nmk_gpio_set_wake(nmk_chip, gpio, enable);
+		__nmk_gpio_set_wake(nmk_chip, d->hwirq, enable);
 
 	spin_unlock(&nmk_chip->lock);
 	spin_unlock_irqrestore(&nmk_gpio_slpm_lock, flags);
@@ -636,20 +721,18 @@ static int nmk_gpio_irq_set_wake(struct irq_data *d, unsigned int on)
 	struct nmk_gpio_chip *nmk_chip;
 	unsigned long flags;
 	u32 bitmask;
-	int gpio;
 
-	gpio = NOMADIK_IRQ_TO_GPIO(d->irq);
 	nmk_chip = irq_data_get_irq_chip_data(d);
 	if (!nmk_chip)
 		return -EINVAL;
-	bitmask = nmk_gpio_get_bitmask(gpio);
+	bitmask = nmk_gpio_get_bitmask(d->hwirq);
 
 	clk_enable(nmk_chip->clk);
 	spin_lock_irqsave(&nmk_gpio_slpm_lock, flags);
 	spin_lock(&nmk_chip->lock);
 
 	if (irqd_irq_disabled(d))
-		__nmk_gpio_set_wake(nmk_chip, gpio, on);
+		__nmk_gpio_set_wake(nmk_chip, d->hwirq, on);
 
 	if (on)
 		nmk_chip->real_wake |= bitmask;
@@ -667,17 +750,14 @@ static int nmk_gpio_irq_set_type(struct irq_data *d, unsigned int type)
 {
 	bool enabled = !irqd_irq_disabled(d);
 	bool wake = irqd_is_wakeup_set(d);
-	int gpio;
 	struct nmk_gpio_chip *nmk_chip;
 	unsigned long flags;
 	u32 bitmask;
 
-	gpio = NOMADIK_IRQ_TO_GPIO(d->irq);
 	nmk_chip = irq_data_get_irq_chip_data(d);
-	bitmask = nmk_gpio_get_bitmask(gpio);
+	bitmask = nmk_gpio_get_bitmask(d->hwirq);
 	if (!nmk_chip)
 		return -EINVAL;
-
 	if (type & IRQ_TYPE_LEVEL_HIGH)
 		return -EINVAL;
 	if (type & IRQ_TYPE_LEVEL_LOW)
@@ -687,10 +767,10 @@ static int nmk_gpio_irq_set_type(struct irq_data *d, unsigned int type)
 	spin_lock_irqsave(&nmk_chip->lock, flags);
 
 	if (enabled)
-		__nmk_gpio_irq_modify(nmk_chip, gpio, NORMAL, false);
+		__nmk_gpio_irq_modify(nmk_chip, d->hwirq, NORMAL, false);
 
 	if (enabled || wake)
-		__nmk_gpio_irq_modify(nmk_chip, gpio, WAKE, false);
+		__nmk_gpio_irq_modify(nmk_chip, d->hwirq, WAKE, false);
 
 	nmk_chip->edge_rising &= ~bitmask;
 	if (type & IRQ_TYPE_EDGE_RISING)
@@ -701,10 +781,10 @@ static int nmk_gpio_irq_set_type(struct irq_data *d, unsigned int type)
 		nmk_chip->edge_falling |= bitmask;
 
 	if (enabled)
-		__nmk_gpio_irq_modify(nmk_chip, gpio, NORMAL, true);
+		__nmk_gpio_irq_modify(nmk_chip, d->hwirq, NORMAL, true);
 
 	if (enabled || wake)
-		__nmk_gpio_irq_modify(nmk_chip, gpio, WAKE, true);
+		__nmk_gpio_irq_modify(nmk_chip, d->hwirq, WAKE, true);
 
 	spin_unlock_irqrestore(&nmk_chip->lock, flags);
 	clk_disable(nmk_chip->clk);
@@ -750,7 +830,7 @@ static void __nmk_gpio_irq_handler(unsigned int irq, struct irq_desc *desc,
 	chained_irq_enter(host_chip, desc);
 
 	nmk_chip = irq_get_handler_data(irq);
-	first_irq = NOMADIK_GPIO_TO_IRQ(nmk_chip->chip.base);
+	first_irq = nmk_chip->domain->revmap_data.legacy.first_irq;
 	while (status) {
 		int bit = __ffs(status);
 
@@ -784,18 +864,6 @@ static void nmk_gpio_secondary_irq_handler(unsigned int irq,
 
 static int nmk_gpio_init_irq(struct nmk_gpio_chip *nmk_chip)
 {
-	unsigned int first_irq;
-	int i;
-
-	first_irq = NOMADIK_GPIO_TO_IRQ(nmk_chip->chip.base);
-	for (i = first_irq; i < first_irq + nmk_chip->chip.ngpio; i++) {
-		irq_set_chip_and_handler(i, &nmk_gpio_irq_chip,
-					 handle_edge_irq);
-		set_irq_flags(i, IRQF_VALID);
-		irq_set_chip_data(i, nmk_chip);
-		irq_set_irq_type(i, IRQ_TYPE_EDGE_FALLING);
-	}
-
 	irq_set_chained_handler(nmk_chip->parent_irq, nmk_gpio_irq_handler);
 	irq_set_handler_data(nmk_chip->parent_irq, nmk_chip);
 
@@ -809,6 +877,25 @@ static int nmk_gpio_init_irq(struct nmk_gpio_chip *nmk_chip)
 }
 
 /* I/O Functions */
+
+static int nmk_gpio_request(struct gpio_chip *chip, unsigned offset)
+{
+	/*
+	 * Map back to global GPIO space and request muxing, the direction
+	 * parameter does not matter for this controller.
+	 */
+	int gpio = chip->base + offset;
+
+	return pinctrl_request_gpio(gpio);
+}
+
+static void nmk_gpio_free(struct gpio_chip *chip, unsigned offset)
+{
+	int gpio = chip->base + offset;
+
+	pinctrl_free_gpio(gpio);
+}
+
 static int nmk_gpio_make_input(struct gpio_chip *chip, unsigned offset)
 {
 	struct nmk_gpio_chip *nmk_chip =
@@ -872,21 +959,23 @@ static int nmk_gpio_to_irq(struct gpio_chip *chip, unsigned offset)
 	struct nmk_gpio_chip *nmk_chip =
 		container_of(chip, struct nmk_gpio_chip, chip);
 
-	return NOMADIK_GPIO_TO_IRQ(nmk_chip->chip.base) + offset;
+	return irq_find_mapping(nmk_chip->domain, offset);
 }
 
 #ifdef CONFIG_DEBUG_FS
 
 #include <linux/seq_file.h>
 
-static void nmk_gpio_dbg_show(struct seq_file *s, struct gpio_chip *chip)
+static void nmk_gpio_dbg_show_one(struct seq_file *s, struct gpio_chip *chip,
+				  unsigned offset, unsigned gpio)
 {
-	int mode;
-	unsigned		i;
-	unsigned		gpio = chip->base;
-	int			is_out;
+	const char *label = gpiochip_is_requested(chip, offset);
 	struct nmk_gpio_chip *nmk_chip =
 		container_of(chip, struct nmk_gpio_chip, chip);
+	int mode;
+	bool is_out;
+	bool pull;
+	u32 bit = 1 << offset;
 	const char *modes[] = {
 		[NMK_GPIO_ALT_GPIO]	= "gpio",
 		[NMK_GPIO_ALT_A]	= "altA",
@@ -895,61 +984,70 @@ static void nmk_gpio_dbg_show(struct seq_file *s, struct gpio_chip *chip)
 	};
 
 	clk_enable(nmk_chip->clk);
-
-	for (i = 0; i < chip->ngpio; i++, gpio++) {
-		const char *label = gpiochip_is_requested(chip, i);
-		bool pull;
-		u32 bit = 1 << i;
-
-		is_out = readl(nmk_chip->addr + NMK_GPIO_DIR) & bit;
-		pull = !(readl(nmk_chip->addr + NMK_GPIO_PDIS) & bit);
-		mode = nmk_gpio_get_mode(gpio);
-		seq_printf(s, " gpio-%-3d (%-20.20s) %s %s %s %s",
-			gpio, label ?: "(none)",
-			is_out ? "out" : "in ",
-			chip->get
-				? (chip->get(chip, i) ? "hi" : "lo")
-				: "?  ",
-			(mode < 0) ? "unknown" : modes[mode],
-			pull ? "pull" : "none");
-
-		if (label && !is_out) {
-			int		irq = gpio_to_irq(gpio);
-			struct irq_desc	*desc = irq_to_desc(irq);
-
-			/* This races with request_irq(), set_irq_type(),
-			 * and set_irq_wake() ... but those are "rare".
-			 */
-			if (irq >= 0 && desc->action) {
-				char *trigger;
-				u32 bitmask = nmk_gpio_get_bitmask(gpio);
-
-				if (nmk_chip->edge_rising & bitmask)
-					trigger = "edge-rising";
-				else if (nmk_chip->edge_falling & bitmask)
-					trigger = "edge-falling";
-				else
-					trigger = "edge-undefined";
-
-				seq_printf(s, " irq-%d %s%s",
-					irq, trigger,
-					irqd_is_wakeup_set(&desc->irq_data)
-						? " wakeup" : "");
-			}
+	is_out = !!(readl(nmk_chip->addr + NMK_GPIO_DIR) & bit);
+	pull = !(readl(nmk_chip->addr + NMK_GPIO_PDIS) & bit);
+	mode = nmk_gpio_get_mode(gpio);
+
+	seq_printf(s, " gpio-%-3d (%-20.20s) %s %s %s %s",
+		   gpio, label ?: "(none)",
+		   is_out ? "out" : "in ",
+		   chip->get
+		   ? (chip->get(chip, offset) ? "hi" : "lo")
+		   : "?  ",
+		   (mode < 0) ? "unknown" : modes[mode],
+		   pull ? "pull" : "none");
+
+	if (label && !is_out) {
+		int		irq = gpio_to_irq(gpio);
+		struct irq_desc	*desc = irq_to_desc(irq);
+
+		/* This races with request_irq(), set_irq_type(),
+		 * and set_irq_wake() ... but those are "rare".
+		 */
+		if (irq >= 0 && desc->action) {
+			char *trigger;
+			u32 bitmask = nmk_gpio_get_bitmask(gpio);
+
+			if (nmk_chip->edge_rising & bitmask)
+				trigger = "edge-rising";
+			else if (nmk_chip->edge_falling & bitmask)
+				trigger = "edge-falling";
+			else
+				trigger = "edge-undefined";
+
+			seq_printf(s, " irq-%d %s%s",
+				   irq, trigger,
+				   irqd_is_wakeup_set(&desc->irq_data)
+				   ? " wakeup" : "");
 		}
+	}
+	clk_disable(nmk_chip->clk);
+}
 
+static void nmk_gpio_dbg_show(struct seq_file *s, struct gpio_chip *chip)
+{
+	unsigned		i;
+	unsigned		gpio = chip->base;
+
+	for (i = 0; i < chip->ngpio; i++, gpio++) {
+		nmk_gpio_dbg_show_one(s, chip, i, gpio);
 		seq_printf(s, "\n");
 	}
-
-	clk_disable(nmk_chip->clk);
 }
 
 #else
+static inline void nmk_gpio_dbg_show_one(struct seq_file *s,
+					 struct gpio_chip *chip,
+					 unsigned offset, unsigned gpio)
+{
+}
 #define nmk_gpio_dbg_show	NULL
 #endif
 
 /* This structure is replicated for each GPIO block allocated at probe time */
 static struct gpio_chip nmk_gpio_template = {
+	.request		= nmk_gpio_request,
+	.free			= nmk_gpio_free,
 	.direction_input	= nmk_gpio_make_input,
 	.get			= nmk_gpio_get_input,
 	.direction_output	= nmk_gpio_make_output,
@@ -1008,21 +1106,11 @@ void nmk_gpio_wakeups_suspend(void)
 
 		clk_enable(chip->clk);
 
-		chip->rwimsc = readl(chip->addr + NMK_GPIO_RWIMSC);
-		chip->fwimsc = readl(chip->addr + NMK_GPIO_FWIMSC);
-
 		writel(chip->rwimsc & chip->real_wake,
 		       chip->addr + NMK_GPIO_RWIMSC);
 		writel(chip->fwimsc & chip->real_wake,
 		       chip->addr + NMK_GPIO_FWIMSC);
 
-		if (chip->sleepmode) {
-			chip->slpm = readl(chip->addr + NMK_GPIO_SLPC);
-
-			/* 0 -> wakeup enable */
-			writel(~chip->real_wake, chip->addr + NMK_GPIO_SLPC);
-		}
-
 		clk_disable(chip->clk);
 	}
 }
@@ -1042,9 +1130,6 @@ void nmk_gpio_wakeups_resume(void)
 		writel(chip->rwimsc, chip->addr + NMK_GPIO_RWIMSC);
 		writel(chip->fwimsc, chip->addr + NMK_GPIO_FWIMSC);
 
-		if (chip->sleepmode)
-			writel(chip->slpm, chip->addr + NMK_GPIO_SLPC);
-
 		clk_disable(chip->clk);
 	}
 }
@@ -1068,19 +1153,62 @@ void nmk_gpio_read_pull(int gpio_bank, u32 *pull_up)
 	}
 }
 
+int nmk_gpio_irq_map(struct irq_domain *d, unsigned int irq,
+			  irq_hw_number_t hwirq)
+{
+	struct nmk_gpio_chip *nmk_chip = d->host_data;
+
+	if (!nmk_chip)
+		return -EINVAL;
+
+	irq_set_chip_and_handler(irq, &nmk_gpio_irq_chip, handle_edge_irq);
+	set_irq_flags(irq, IRQF_VALID);
+	irq_set_chip_data(irq, nmk_chip);
+	irq_set_irq_type(irq, IRQ_TYPE_EDGE_FALLING);
+
+	return 0;
+}
+
+const struct irq_domain_ops nmk_gpio_irq_simple_ops = {
+	.map = nmk_gpio_irq_map,
+	.xlate = irq_domain_xlate_twocell,
+};
+
 static int __devinit nmk_gpio_probe(struct platform_device *dev)
 {
 	struct nmk_gpio_platform_data *pdata = dev->dev.platform_data;
+	struct device_node *np = dev->dev.of_node;
 	struct nmk_gpio_chip *nmk_chip;
 	struct gpio_chip *chip;
 	struct resource *res;
 	struct clk *clk;
 	int secondary_irq;
+	void __iomem *base;
 	int irq;
 	int ret;
 
-	if (!pdata)
+	if (!pdata && !np) {
+		dev_err(&dev->dev, "No platform data or device tree found\n");
 		return -ENODEV;
+	}
+
+	if (np) {
+		pdata = kzalloc(sizeof(*pdata), GFP_KERNEL);
+		if (!pdata)
+			return -ENOMEM;
+
+		if (of_get_property(np, "supports-sleepmode", NULL))
+			pdata->supports_sleepmode = true;
+
+		if (of_property_read_u32(np, "gpio-bank", &dev->id)) {
+			dev_err(&dev->dev, "gpio-bank property not found\n");
+			ret = -EINVAL;
+			goto out;
+		}
+
+		pdata->first_gpio = dev->id * NMK_GPIO_PER_CHIP;
+		pdata->num_gpio   = NMK_GPIO_PER_CHIP;
+	}
 
 	res = platform_get_resource(dev, IORESOURCE_MEM, 0);
 	if (!res) {
@@ -1106,10 +1234,16 @@ static int __devinit nmk_gpio_probe(struct platform_device *dev)
 		goto out;
 	}
 
+	base = ioremap(res->start, resource_size(res));
+	if (!base) {
+		ret = -ENOMEM;
+		goto out_release;
+	}
+
 	clk = clk_get(&dev->dev, NULL);
 	if (IS_ERR(clk)) {
 		ret = PTR_ERR(clk);
-		goto out_release;
+		goto out_unmap;
 	}
 
 	nmk_chip = kzalloc(sizeof(*nmk_chip), GFP_KERNEL);
@@ -1117,13 +1251,14 @@ static int __devinit nmk_gpio_probe(struct platform_device *dev)
 		ret = -ENOMEM;
 		goto out_clk;
 	}
+
 	/*
 	 * The virt address in nmk_chip->addr is in the nomadik register space,
 	 * so we can simply convert the resource address, without remapping
 	 */
 	nmk_chip->bank = dev->id;
 	nmk_chip->clk = clk;
-	nmk_chip->addr = io_p2v(res->start);
+	nmk_chip->addr = base;
 	nmk_chip->chip = nmk_gpio_template;
 	nmk_chip->parent_irq = irq;
 	nmk_chip->secondary_parent_irq = secondary_irq;
@@ -1139,6 +1274,14 @@ static int __devinit nmk_gpio_probe(struct platform_device *dev)
 	chip->dev = &dev->dev;
 	chip->owner = THIS_MODULE;
 
+	clk_enable(nmk_chip->clk);
+	nmk_chip->lowemi = readl_relaxed(nmk_chip->addr + NMK_GPIO_LOWEMI);
+	clk_disable(nmk_chip->clk);
+
+#ifdef CONFIG_OF_GPIO
+	chip->of_node = np;
+#endif
+
 	ret = gpiochip_add(&nmk_chip->chip);
 	if (ret)
 		goto out_free;
@@ -1146,12 +1289,22 @@ static int __devinit nmk_gpio_probe(struct platform_device *dev)
 	BUG_ON(nmk_chip->bank >= ARRAY_SIZE(nmk_gpio_chips));
 
 	nmk_gpio_chips[nmk_chip->bank] = nmk_chip;
+
 	platform_set_drvdata(dev, nmk_chip);
 
+	nmk_chip->domain = irq_domain_add_legacy(np, NMK_GPIO_PER_CHIP,
+						NOMADIK_GPIO_TO_IRQ(pdata->first_gpio),
+						0, &nmk_gpio_irq_simple_ops, nmk_chip);
+	if (!nmk_chip->domain) {
+		pr_err("%s: Failed to create irqdomain\n", np->full_name);
+		ret = -ENOSYS;
+		goto out_free;
+	}
+
 	nmk_gpio_init_irq(nmk_chip);
 
-	dev_info(&dev->dev, "at address %p\n",
-		 nmk_chip->addr);
+	dev_info(&dev->dev, "at address %p\n", nmk_chip->addr);
+
 	return 0;
 
 out_free:
@@ -1159,25 +1312,465 @@ out_free:
 out_clk:
 	clk_disable(clk);
 	clk_put(clk);
+out_unmap:
+	iounmap(base);
 out_release:
 	release_mem_region(res->start, resource_size(res));
 out:
 	dev_err(&dev->dev, "Failure %i for GPIO %i-%i\n", ret,
 		  pdata->first_gpio, pdata->first_gpio+31);
+	if (np)
+		kfree(pdata);
+
 	return ret;
 }
 
+static int nmk_get_groups_cnt(struct pinctrl_dev *pctldev)
+{
+	struct nmk_pinctrl *npct = pinctrl_dev_get_drvdata(pctldev);
+
+	return npct->soc->ngroups;
+}
+
+static const char *nmk_get_group_name(struct pinctrl_dev *pctldev,
+				       unsigned selector)
+{
+	struct nmk_pinctrl *npct = pinctrl_dev_get_drvdata(pctldev);
+
+	return npct->soc->groups[selector].name;
+}
+
+static int nmk_get_group_pins(struct pinctrl_dev *pctldev, unsigned selector,
+			      const unsigned **pins,
+			      unsigned *num_pins)
+{
+	struct nmk_pinctrl *npct = pinctrl_dev_get_drvdata(pctldev);
+
+	*pins = npct->soc->groups[selector].pins;
+	*num_pins = npct->soc->groups[selector].npins;
+	return 0;
+}
+
+static struct pinctrl_gpio_range *
+nmk_match_gpio_range(struct pinctrl_dev *pctldev, unsigned offset)
+{
+	struct nmk_pinctrl *npct = pinctrl_dev_get_drvdata(pctldev);
+	int i;
+
+	for (i = 0; i < npct->soc->gpio_num_ranges; i++) {
+		struct pinctrl_gpio_range *range;
+
+		range = &npct->soc->gpio_ranges[i];
+		if (offset >= range->pin_base &&
+		    offset <= (range->pin_base + range->npins - 1))
+			return range;
+	}
+	return NULL;
+}
+
+static void nmk_pin_dbg_show(struct pinctrl_dev *pctldev, struct seq_file *s,
+		   unsigned offset)
+{
+	struct pinctrl_gpio_range *range;
+	struct gpio_chip *chip;
+
+	range = nmk_match_gpio_range(pctldev, offset);
+	if (!range || !range->gc) {
+		seq_printf(s, "invalid pin offset");
+		return;
+	}
+	chip = range->gc;
+	nmk_gpio_dbg_show_one(s, chip, offset - chip->base, offset);
+}
+
+static struct pinctrl_ops nmk_pinctrl_ops = {
+	.get_groups_count = nmk_get_groups_cnt,
+	.get_group_name = nmk_get_group_name,
+	.get_group_pins = nmk_get_group_pins,
+	.pin_dbg_show = nmk_pin_dbg_show,
+};
+
+static int nmk_pmx_get_funcs_cnt(struct pinctrl_dev *pctldev)
+{
+	struct nmk_pinctrl *npct = pinctrl_dev_get_drvdata(pctldev);
+
+	return npct->soc->nfunctions;
+}
+
+static const char *nmk_pmx_get_func_name(struct pinctrl_dev *pctldev,
+					 unsigned function)
+{
+	struct nmk_pinctrl *npct = pinctrl_dev_get_drvdata(pctldev);
+
+	return npct->soc->functions[function].name;
+}
+
+static int nmk_pmx_get_func_groups(struct pinctrl_dev *pctldev,
+				   unsigned function,
+				   const char * const **groups,
+				   unsigned * const num_groups)
+{
+	struct nmk_pinctrl *npct = pinctrl_dev_get_drvdata(pctldev);
+
+	*groups = npct->soc->functions[function].groups;
+	*num_groups = npct->soc->functions[function].ngroups;
+
+	return 0;
+}
+
+static int nmk_pmx_enable(struct pinctrl_dev *pctldev, unsigned function,
+			  unsigned group)
+{
+	struct nmk_pinctrl *npct = pinctrl_dev_get_drvdata(pctldev);
+	const struct nmk_pingroup *g;
+	static unsigned int slpm[NUM_BANKS];
+	unsigned long flags;
+	bool glitch;
+	int ret = -EINVAL;
+	int i;
+
+	g = &npct->soc->groups[group];
+
+	if (g->altsetting < 0)
+		return -EINVAL;
+
+	dev_dbg(npct->dev, "enable group %s, %u pins\n", g->name, g->npins);
+
+	/* Handle this special glitch on altfunction C */
+	glitch = (g->altsetting == NMK_GPIO_ALT_C);
+
+	if (glitch) {
+		spin_lock_irqsave(&nmk_gpio_slpm_lock, flags);
+
+		/* Initially don't put any pins to sleep when switching */
+		memset(slpm, 0xff, sizeof(slpm));
+
+		/*
+		 * Then mask the pins that need to be sleeping now when we're
+		 * switching to the ALT C function.
+		 */
+		for (i = 0; i < g->npins; i++)
+			slpm[g->pins[i] / NMK_GPIO_PER_CHIP] &= ~BIT(g->pins[i]);
+		nmk_gpio_glitch_slpm_init(slpm);
+	}
+
+	for (i = 0; i < g->npins; i++) {
+		struct pinctrl_gpio_range *range;
+		struct nmk_gpio_chip *nmk_chip;
+		struct gpio_chip *chip;
+		unsigned bit;
+
+		range = nmk_match_gpio_range(pctldev, g->pins[i]);
+		if (!range) {
+			dev_err(npct->dev,
+				"invalid pin offset %d in group %s at index %d\n",
+				g->pins[i], g->name, i);
+			goto out_glitch;
+		}
+		if (!range->gc) {
+			dev_err(npct->dev, "GPIO chip missing in range for pin offset %d in group %s at index %d\n",
+				g->pins[i], g->name, i);
+			goto out_glitch;
+		}
+		chip = range->gc;
+		nmk_chip = container_of(chip, struct nmk_gpio_chip, chip);
+		dev_dbg(npct->dev, "setting pin %d to altsetting %d\n", g->pins[i], g->altsetting);
+
+		clk_enable(nmk_chip->clk);
+		bit = g->pins[i] % NMK_GPIO_PER_CHIP;
+		/*
+		 * If the pin is switching to altfunc, and there was an
+		 * interrupt installed on it which has been lazy disabled,
+		 * actually mask the interrupt to prevent spurious interrupts
+		 * that would occur while the pin is under control of the
+		 * peripheral. Only SKE does this.
+		 */
+		nmk_gpio_disable_lazy_irq(nmk_chip, bit);
+
+		__nmk_gpio_set_mode_safe(nmk_chip, bit, g->altsetting, glitch);
+		clk_disable(nmk_chip->clk);
+	}
+
+	/* When all pins are successfully reconfigured we get here */
+	ret = 0;
+
+out_glitch:
+	if (glitch) {
+		nmk_gpio_glitch_slpm_restore(slpm);
+		spin_unlock_irqrestore(&nmk_gpio_slpm_lock, flags);
+	}
+
+	return ret;
+}
+
+static void nmk_pmx_disable(struct pinctrl_dev *pctldev,
+			    unsigned function, unsigned group)
+{
+	struct nmk_pinctrl *npct = pinctrl_dev_get_drvdata(pctldev);
+	const struct nmk_pingroup *g;
+
+	g = &npct->soc->groups[group];
+
+	if (g->altsetting < 0)
+		return;
+
+	/* Poke out the mux, set the pin to some default state? */
+	dev_dbg(npct->dev, "disable group %s, %u pins\n", g->name, g->npins);
+}
+
+int nmk_gpio_request_enable(struct pinctrl_dev *pctldev,
+			    struct pinctrl_gpio_range *range,
+			    unsigned offset)
+{
+	struct nmk_pinctrl *npct = pinctrl_dev_get_drvdata(pctldev);
+	struct nmk_gpio_chip *nmk_chip;
+	struct gpio_chip *chip;
+	unsigned bit;
+
+	if (!range) {
+		dev_err(npct->dev, "invalid range\n");
+		return -EINVAL;
+	}
+	if (!range->gc) {
+		dev_err(npct->dev, "missing GPIO chip in range\n");
+		return -EINVAL;
+	}
+	chip = range->gc;
+	nmk_chip = container_of(chip, struct nmk_gpio_chip, chip);
+
+	dev_dbg(npct->dev, "enable pin %u as GPIO\n", offset);
+
+	clk_enable(nmk_chip->clk);
+	bit = offset % NMK_GPIO_PER_CHIP;
+	/* There is no glitch when converting any pin to GPIO */
+	__nmk_gpio_set_mode(nmk_chip, bit, NMK_GPIO_ALT_GPIO);
+	clk_disable(nmk_chip->clk);
+
+	return 0;
+}
+
+void nmk_gpio_disable_free(struct pinctrl_dev *pctldev,
+			   struct pinctrl_gpio_range *range,
+			   unsigned offset)
+{
+	struct nmk_pinctrl *npct = pinctrl_dev_get_drvdata(pctldev);
+
+	dev_dbg(npct->dev, "disable pin %u as GPIO\n", offset);
+	/* Set the pin to some default state, GPIO is usually default */
+}
+
+static struct pinmux_ops nmk_pinmux_ops = {
+	.get_functions_count = nmk_pmx_get_funcs_cnt,
+	.get_function_name = nmk_pmx_get_func_name,
+	.get_function_groups = nmk_pmx_get_func_groups,
+	.enable = nmk_pmx_enable,
+	.disable = nmk_pmx_disable,
+	.gpio_request_enable = nmk_gpio_request_enable,
+	.gpio_disable_free = nmk_gpio_disable_free,
+};
+
+int nmk_pin_config_get(struct pinctrl_dev *pctldev,
+		       unsigned pin,
+		       unsigned long *config)
+{
+	/* Not implemented */
+	return -EINVAL;
+}
+
+int nmk_pin_config_set(struct pinctrl_dev *pctldev,
+		       unsigned pin,
+		       unsigned long config)
+{
+	static const char *pullnames[] = {
+		[NMK_GPIO_PULL_NONE]	= "none",
+		[NMK_GPIO_PULL_UP]	= "up",
+		[NMK_GPIO_PULL_DOWN]	= "down",
+		[3] /* illegal */	= "??"
+	};
+	static const char *slpmnames[] = {
+		[NMK_GPIO_SLPM_INPUT]		= "input/wakeup",
+		[NMK_GPIO_SLPM_NOCHANGE]	= "no-change/no-wakeup",
+	};
+	struct nmk_pinctrl *npct = pinctrl_dev_get_drvdata(pctldev);
+	struct nmk_gpio_chip *nmk_chip;
+	struct pinctrl_gpio_range *range;
+	struct gpio_chip *chip;
+	unsigned bit;
+
+	/*
+	 * The pin config contains pin number and altfunction fields, here
+	 * we just ignore that part. It's being handled by the framework and
+	 * pinmux callback respectively.
+	 */
+	pin_cfg_t cfg = (pin_cfg_t) config;
+	int pull = PIN_PULL(cfg);
+	int slpm = PIN_SLPM(cfg);
+	int output = PIN_DIR(cfg);
+	int val = PIN_VAL(cfg);
+	bool lowemi = PIN_LOWEMI(cfg);
+	bool gpiomode = PIN_GPIOMODE(cfg);
+	bool sleep = PIN_SLEEPMODE(cfg);
+
+	range = nmk_match_gpio_range(pctldev, pin);
+	if (!range) {
+		dev_err(npct->dev, "invalid pin offset %d\n", pin);
+		return -EINVAL;
+	}
+	if (!range->gc) {
+		dev_err(npct->dev, "GPIO chip missing in range for pin %d\n",
+			pin);
+		return -EINVAL;
+	}
+	chip = range->gc;
+	nmk_chip = container_of(chip, struct nmk_gpio_chip, chip);
+
+	if (sleep) {
+		int slpm_pull = PIN_SLPM_PULL(cfg);
+		int slpm_output = PIN_SLPM_DIR(cfg);
+		int slpm_val = PIN_SLPM_VAL(cfg);
+
+		/* All pins go into GPIO mode at sleep */
+		gpiomode = true;
+
+		/*
+		 * The SLPM_* values are normal values + 1 to allow zero to
+		 * mean "same as normal".
+		 */
+		if (slpm_pull)
+			pull = slpm_pull - 1;
+		if (slpm_output)
+			output = slpm_output - 1;
+		if (slpm_val)
+			val = slpm_val - 1;
+
+		dev_dbg(nmk_chip->chip.dev, "pin %d: sleep pull %s, dir %s, val %s\n",
+			pin,
+			slpm_pull ? pullnames[pull] : "same",
+			slpm_output ? (output ? "output" : "input") : "same",
+			slpm_val ? (val ? "high" : "low") : "same");
+	}
+
+	dev_dbg(nmk_chip->chip.dev, "pin %d [%#lx]: pull %s, slpm %s (%s%s), lowemi %s\n",
+		pin, cfg, pullnames[pull], slpmnames[slpm],
+		output ? "output " : "input",
+		output ? (val ? "high" : "low") : "",
+		lowemi ? "on" : "off" );
+
+	clk_enable(nmk_chip->clk);
+	bit = pin % NMK_GPIO_PER_CHIP;
+	if (gpiomode)
+		/* No glitch when going to GPIO mode */
+		__nmk_gpio_set_mode(nmk_chip, bit, NMK_GPIO_ALT_GPIO);
+	if (output)
+		__nmk_gpio_make_output(nmk_chip, bit, val);
+	else {
+		__nmk_gpio_make_input(nmk_chip, bit);
+		__nmk_gpio_set_pull(nmk_chip, bit, pull);
+	}
+	/* TODO: isn't this only applicable on output pins? */
+	__nmk_gpio_set_lowemi(nmk_chip, bit, lowemi);
+
+	__nmk_gpio_set_slpm(nmk_chip, bit, slpm);
+	clk_disable(nmk_chip->clk);
+	return 0;
+}
+
+static struct pinconf_ops nmk_pinconf_ops = {
+	.pin_config_get = nmk_pin_config_get,
+	.pin_config_set = nmk_pin_config_set,
+};
+
+static struct pinctrl_desc nmk_pinctrl_desc = {
+	.name = "pinctrl-nomadik",
+	.pctlops = &nmk_pinctrl_ops,
+	.pmxops = &nmk_pinmux_ops,
+	.confops = &nmk_pinconf_ops,
+	.owner = THIS_MODULE,
+};
+
+static int __devinit nmk_pinctrl_probe(struct platform_device *pdev)
+{
+	const struct platform_device_id *platid = platform_get_device_id(pdev);
+	struct nmk_pinctrl *npct;
+	int i;
+
+	npct = devm_kzalloc(&pdev->dev, sizeof(*npct), GFP_KERNEL);
+	if (!npct)
+		return -ENOMEM;
+
+	/* Poke in other ASIC variants here */
+	if (platid->driver_data == PINCTRL_NMK_DB8500)
+		nmk_pinctrl_db8500_init(&npct->soc);
+
+	/*
+	 * We need all the GPIO drivers to probe FIRST, or we will not be able
+	 * to obtain references to the struct gpio_chip * for them, and we
+	 * need this to proceed.
+	 */
+	for (i = 0; i < npct->soc->gpio_num_ranges; i++) {
+		if (!nmk_gpio_chips[i]) {
+			dev_warn(&pdev->dev, "GPIO chip %d not registered yet\n", i);
+			devm_kfree(&pdev->dev, npct);
+			return -EPROBE_DEFER;
+		}
+		npct->soc->gpio_ranges[i].gc = &nmk_gpio_chips[i]->chip;
+	}
+
+	nmk_pinctrl_desc.pins = npct->soc->pins;
+	nmk_pinctrl_desc.npins = npct->soc->npins;
+	npct->dev = &pdev->dev;
+	npct->pctl = pinctrl_register(&nmk_pinctrl_desc, &pdev->dev, npct);
+	if (!npct->pctl) {
+		dev_err(&pdev->dev, "could not register Nomadik pinctrl driver\n");
+		return -EINVAL;
+	}
+
+	/* We will handle a range of GPIO pins */
+	for (i = 0; i < npct->soc->gpio_num_ranges; i++)
+		pinctrl_add_gpio_range(npct->pctl, &npct->soc->gpio_ranges[i]);
+
+	platform_set_drvdata(pdev, npct);
+	dev_info(&pdev->dev, "initialized Nomadik pin control driver\n");
+
+	return 0;
+}
+
+static const struct of_device_id nmk_gpio_match[] = {
+	{ .compatible = "st,nomadik-gpio", },
+	{}
+};
+
 static struct platform_driver nmk_gpio_driver = {
 	.driver = {
 		.owner = THIS_MODULE,
 		.name = "gpio",
+		.of_match_table = nmk_gpio_match,
 	},
 	.probe = nmk_gpio_probe,
 };
 
+static const struct platform_device_id nmk_pinctrl_id[] = {
+	{ "pinctrl-stn8815", PINCTRL_NMK_STN8815 },
+	{ "pinctrl-db8500", PINCTRL_NMK_DB8500 },
+};
+
+static struct platform_driver nmk_pinctrl_driver = {
+	.driver = {
+		.owner = THIS_MODULE,
+		.name = "pinctrl-nomadik",
+	},
+	.probe = nmk_pinctrl_probe,
+	.id_table = nmk_pinctrl_id,
+};
+
 static int __init nmk_gpio_init(void)
 {
-	return platform_driver_register(&nmk_gpio_driver);
+	int ret;
+
+	ret = platform_driver_register(&nmk_gpio_driver);
+	if (ret)
+		return ret;
+	return platform_driver_register(&nmk_pinctrl_driver);
 }
 
 core_initcall(nmk_gpio_init);
diff --git a/drivers/pinctrl/pinctrl-nomadik.h b/drivers/pinctrl/pinctrl-nomadik.h
new file mode 100644
index 00000000000..bc91aed7185
--- /dev/null
+++ b/drivers/pinctrl/pinctrl-nomadik.h
@@ -0,0 +1,77 @@
+#ifndef PINCTRL_PINCTRL_NOMADIK_H
+#define PINCTRL_PINCTRL_NOMADIK_H
+
+#include <plat/gpio-nomadik.h>
+
+/* Package definitions */
+#define PINCTRL_NMK_STN8815	0
+#define PINCTRL_NMK_DB8500	1
+
+/**
+ * struct nmk_function - Nomadik pinctrl mux function
+ * @name: The name of the function, exported to pinctrl core.
+ * @groups: An array of pin groups that may select this function.
+ * @ngroups: The number of entries in @groups.
+ */
+struct nmk_function {
+	const char *name;
+	const char * const *groups;
+	unsigned ngroups;
+};
+
+/**
+ * struct nmk_pingroup - describes a Nomadik pin group
+ * @name: the name of this specific pin group
+ * @pins: an array of discrete physical pins used in this group, taken
+ *	from the driver-local pin enumeration space
+ * @num_pins: the number of pins in this group array, i.e. the number of
+ *	elements in .pins so we can iterate over that array
+ * @altsetting: the altsetting to apply to all pins in this group to
+ *	configure them to be used by a function
+ */
+struct nmk_pingroup {
+	const char *name;
+	const unsigned int *pins;
+	const unsigned npins;
+	int altsetting;
+};
+
+/**
+ * struct nmk_pinctrl_soc_data - Nomadik pin controller per-SoC configuration
+ * @gpio_ranges: An array of GPIO ranges for this SoC
+ * @gpio_num_ranges: The number of GPIO ranges for this SoC
+ * @pins:	An array describing all pins the pin controller affects.
+ *		All pins which are also GPIOs must be listed first within the
+ *		array, and be numbered identically to the GPIO controller's
+ *		numbering.
+ * @npins:	The number of entries in @pins.
+ * @functions:	The functions supported on this SoC.
+ * @nfunction:	The number of entries in @functions.
+ * @groups:	An array describing all pin groups the pin SoC supports.
+ * @ngroups:	The number of entries in @groups.
+ */
+struct nmk_pinctrl_soc_data {
+	struct pinctrl_gpio_range *gpio_ranges;
+	unsigned gpio_num_ranges;
+	const struct pinctrl_pin_desc *pins;
+	unsigned npins;
+	const struct nmk_function *functions;
+	unsigned nfunctions;
+	const struct nmk_pingroup *groups;
+	unsigned ngroups;
+};
+
+#ifdef CONFIG_PINCTRL_DB8500
+
+void nmk_pinctrl_db8500_init(const struct nmk_pinctrl_soc_data **soc);
+
+#else
+
+static inline void
+nmk_pinctrl_db8500_init(const struct nmk_pinctrl_soc_data **soc)
+{
+}
+
+#endif
+
+#endif /* PINCTRL_PINCTRL_NOMADIK_H */
diff --git a/drivers/pinctrl/pinctrl-pxa3xx.c b/drivers/pinctrl/pinctrl-pxa3xx.c
index 079dce0e93e..f14cd6ba4c0 100644
--- a/drivers/pinctrl/pinctrl-pxa3xx.c
+++ b/drivers/pinctrl/pinctrl-pxa3xx.c
@@ -25,20 +25,18 @@ static struct pinctrl_gpio_range pxa3xx_pinctrl_gpio_range = {
 	.pin_base	= 0,
 };
 
-static int pxa3xx_list_groups(struct pinctrl_dev *pctrldev, unsigned selector)
+static int pxa3xx_get_groups_count(struct pinctrl_dev *pctrldev)
 {
 	struct pxa3xx_pinmux_info *info = pinctrl_dev_get_drvdata(pctrldev);
-	if (selector >= info->num_grps)
-		return -EINVAL;
-	return 0;
+
+	return info->num_grps;
 }
 
 static const char *pxa3xx_get_group_name(struct pinctrl_dev *pctrldev,
 					 unsigned selector)
 {
 	struct pxa3xx_pinmux_info *info = pinctrl_dev_get_drvdata(pctrldev);
-	if (selector >= info->num_grps)
-		return NULL;
+
 	return info->grps[selector].name;
 }
 
@@ -48,25 +46,23 @@ static int pxa3xx_get_group_pins(struct pinctrl_dev *pctrldev,
 				 unsigned *num_pins)
 {
 	struct pxa3xx_pinmux_info *info = pinctrl_dev_get_drvdata(pctrldev);
-	if (selector >= info->num_grps)
-		return -EINVAL;
+
 	*pins = info->grps[selector].pins;
 	*num_pins = info->grps[selector].npins;
 	return 0;
 }
 
 static struct pinctrl_ops pxa3xx_pctrl_ops = {
-	.list_groups	= pxa3xx_list_groups,
+	.get_groups_count = pxa3xx_get_groups_count,
 	.get_group_name	= pxa3xx_get_group_name,
 	.get_group_pins	= pxa3xx_get_group_pins,
 };
 
-static int pxa3xx_pmx_list_func(struct pinctrl_dev *pctrldev, unsigned func)
+static int pxa3xx_pmx_get_funcs_count(struct pinctrl_dev *pctrldev)
 {
 	struct pxa3xx_pinmux_info *info = pinctrl_dev_get_drvdata(pctrldev);
-	if (func >= info->num_funcs)
-		return -EINVAL;
-	return 0;
+
+	return info->num_funcs;
 }
 
 static const char *pxa3xx_pmx_get_func_name(struct pinctrl_dev *pctrldev,
@@ -142,11 +138,6 @@ static int pxa3xx_pmx_enable(struct pinctrl_dev *pctrldev, unsigned func,
 	return 0;
 }
 
-static void pxa3xx_pmx_disable(struct pinctrl_dev *pctrldev, unsigned func,
-			       unsigned group)
-{
-}
-
 static int pxa3xx_pmx_request_gpio(struct pinctrl_dev *pctrldev,
 				   struct pinctrl_gpio_range *range,
 				   unsigned pin)
@@ -170,11 +161,10 @@ static int pxa3xx_pmx_request_gpio(struct pinctrl_dev *pctrldev,
 }
 
 static struct pinmux_ops pxa3xx_pmx_ops = {
-	.list_functions		= pxa3xx_pmx_list_func,
+	.get_functions_count	= pxa3xx_pmx_get_funcs_count,
 	.get_function_name	= pxa3xx_pmx_get_func_name,
 	.get_function_groups	= pxa3xx_pmx_get_groups,
 	.enable			= pxa3xx_pmx_enable,
-	.disable		= pxa3xx_pmx_disable,
 	.gpio_request_enable	= pxa3xx_pmx_request_gpio,
 };
 
diff --git a/drivers/pinctrl/pinctrl-sirf.c b/drivers/pinctrl/pinctrl-sirf.c
index 6b3534cc051..ba15b1a29e5 100644
--- a/drivers/pinctrl/pinctrl-sirf.c
+++ b/drivers/pinctrl/pinctrl-sirf.c
@@ -853,18 +853,14 @@ static const struct sirfsoc_pin_group sirfsoc_pin_groups[] = {
 	SIRFSOC_PIN_GROUP("gpsgrp", gps_pins),
 };
 
-static int sirfsoc_list_groups(struct pinctrl_dev *pctldev, unsigned selector)
+static int sirfsoc_get_groups_count(struct pinctrl_dev *pctldev)
 {
-	if (selector >= ARRAY_SIZE(sirfsoc_pin_groups))
-		return -EINVAL;
-	return 0;
+	return ARRAY_SIZE(sirfsoc_pin_groups);
 }
 
 static const char *sirfsoc_get_group_name(struct pinctrl_dev *pctldev,
 				       unsigned selector)
 {
-	if (selector >= ARRAY_SIZE(sirfsoc_pin_groups))
-		return NULL;
 	return sirfsoc_pin_groups[selector].name;
 }
 
@@ -872,8 +868,6 @@ static int sirfsoc_get_group_pins(struct pinctrl_dev *pctldev, unsigned selector
 			       const unsigned **pins,
 			       unsigned *num_pins)
 {
-	if (selector >= ARRAY_SIZE(sirfsoc_pin_groups))
-		return -EINVAL;
 	*pins = sirfsoc_pin_groups[selector].pins;
 	*num_pins = sirfsoc_pin_groups[selector].num_pins;
 	return 0;
@@ -886,7 +880,7 @@ static void sirfsoc_pin_dbg_show(struct pinctrl_dev *pctldev, struct seq_file *s
 }
 
 static struct pinctrl_ops sirfsoc_pctrl_ops = {
-	.list_groups = sirfsoc_list_groups,
+	.get_groups_count = sirfsoc_get_groups_count,
 	.get_group_name = sirfsoc_get_group_name,
 	.get_group_pins = sirfsoc_get_group_pins,
 	.pin_dbg_show = sirfsoc_pin_dbg_show,
@@ -1033,11 +1027,9 @@ static void sirfsoc_pinmux_disable(struct pinctrl_dev *pmxdev, unsigned selector
 	sirfsoc_pinmux_endisable(spmx, selector, false);
 }
 
-static int sirfsoc_pinmux_list_funcs(struct pinctrl_dev *pmxdev, unsigned selector)
+static int sirfsoc_pinmux_get_funcs_count(struct pinctrl_dev *pmxdev)
 {
-	if (selector >= ARRAY_SIZE(sirfsoc_pmx_functions))
-		return -EINVAL;
-	return 0;
+	return ARRAY_SIZE(sirfsoc_pmx_functions);
 }
 
 static const char *sirfsoc_pinmux_get_func_name(struct pinctrl_dev *pctldev,
@@ -1074,9 +1066,9 @@ static int sirfsoc_pinmux_request_gpio(struct pinctrl_dev *pmxdev,
 }
 
 static struct pinmux_ops sirfsoc_pinmux_ops = {
-	.list_functions = sirfsoc_pinmux_list_funcs,
 	.enable = sirfsoc_pinmux_enable,
 	.disable = sirfsoc_pinmux_disable,
+	.get_functions_count = sirfsoc_pinmux_get_funcs_count,
 	.get_function_name = sirfsoc_pinmux_get_func_name,
 	.get_function_groups = sirfsoc_pinmux_get_groups,
 	.gpio_request_enable = sirfsoc_pinmux_request_gpio,
diff --git a/drivers/pinctrl/pinctrl-tegra.c b/drivers/pinctrl/pinctrl-tegra.c
index 9b329688120..b6934867d8d 100644
--- a/drivers/pinctrl/pinctrl-tegra.c
+++ b/drivers/pinctrl/pinctrl-tegra.c
@@ -1,7 +1,7 @@
 /*
  * Driver for the NVIDIA Tegra pinmux
  *
- * Copyright (c) 2011, NVIDIA CORPORATION.  All rights reserved.
+ * Copyright (c) 2011-2012, NVIDIA CORPORATION.  All rights reserved.
  *
  * Derived from code:
  * Copyright (C) 2010 Google, Inc.
@@ -22,17 +22,19 @@
 #include <linux/init.h>
 #include <linux/io.h>
 #include <linux/module.h>
-#include <linux/of_device.h>
+#include <linux/of.h>
+#include <linux/platform_device.h>
+#include <linux/pinctrl/machine.h>
 #include <linux/pinctrl/pinctrl.h>
 #include <linux/pinctrl/pinmux.h>
 #include <linux/pinctrl/pinconf.h>
+#include <linux/slab.h>
 
 #include <mach/pinconf-tegra.h>
 
+#include "core.h"
 #include "pinctrl-tegra.h"
 
-#define DRIVER_NAME "tegra-pinmux-disabled"
-
 struct tegra_pmx {
 	struct device *dev;
 	struct pinctrl_dev *pctl;
@@ -53,15 +55,11 @@ static inline void pmx_writel(struct tegra_pmx *pmx, u32 val, u32 bank, u32 reg)
 	writel(val, pmx->regs[bank] + reg);
 }
 
-static int tegra_pinctrl_list_groups(struct pinctrl_dev *pctldev,
-				     unsigned group)
+static int tegra_pinctrl_get_groups_count(struct pinctrl_dev *pctldev)
 {
 	struct tegra_pmx *pmx = pinctrl_dev_get_drvdata(pctldev);
 
-	if (group >= pmx->soc->ngroups)
-		return -EINVAL;
-
-	return 0;
+	return pmx->soc->ngroups;
 }
 
 static const char *tegra_pinctrl_get_group_name(struct pinctrl_dev *pctldev,
@@ -69,9 +67,6 @@ static const char *tegra_pinctrl_get_group_name(struct pinctrl_dev *pctldev,
 {
 	struct tegra_pmx *pmx = pinctrl_dev_get_drvdata(pctldev);
 
-	if (group >= pmx->soc->ngroups)
-		return NULL;
-
 	return pmx->soc->groups[group].name;
 }
 
@@ -82,38 +77,259 @@ static int tegra_pinctrl_get_group_pins(struct pinctrl_dev *pctldev,
 {
 	struct tegra_pmx *pmx = pinctrl_dev_get_drvdata(pctldev);
 
-	if (group >= pmx->soc->ngroups)
-		return -EINVAL;
-
 	*pins = pmx->soc->groups[group].pins;
 	*num_pins = pmx->soc->groups[group].npins;
 
 	return 0;
 }
 
+#ifdef CONFIG_DEBUG_FS
 static void tegra_pinctrl_pin_dbg_show(struct pinctrl_dev *pctldev,
 				       struct seq_file *s,
 				       unsigned offset)
 {
-	seq_printf(s, " " DRIVER_NAME);
+	seq_printf(s, " %s", dev_name(pctldev->dev));
+}
+#endif
+
+static int reserve_map(struct device *dev, struct pinctrl_map **map,
+		       unsigned *reserved_maps, unsigned *num_maps,
+		       unsigned reserve)
+{
+	unsigned old_num = *reserved_maps;
+	unsigned new_num = *num_maps + reserve;
+	struct pinctrl_map *new_map;
+
+	if (old_num >= new_num)
+		return 0;
+
+	new_map = krealloc(*map, sizeof(*new_map) * new_num, GFP_KERNEL);
+	if (!new_map) {
+		dev_err(dev, "krealloc(map) failed\n");
+		return -ENOMEM;
+	}
+
+	memset(new_map + old_num, 0, (new_num - old_num) * sizeof(*new_map));
+
+	*map = new_map;
+	*reserved_maps = new_num;
+
+	return 0;
+}
+
+static int add_map_mux(struct pinctrl_map **map, unsigned *reserved_maps,
+		       unsigned *num_maps, const char *group,
+		       const char *function)
+{
+	if (WARN_ON(*num_maps == *reserved_maps))
+		return -ENOSPC;
+
+	(*map)[*num_maps].type = PIN_MAP_TYPE_MUX_GROUP;
+	(*map)[*num_maps].data.mux.group = group;
+	(*map)[*num_maps].data.mux.function = function;
+	(*num_maps)++;
+
+	return 0;
+}
+
+static int add_map_configs(struct device *dev, struct pinctrl_map **map,
+			   unsigned *reserved_maps, unsigned *num_maps,
+			   const char *group, unsigned long *configs,
+			   unsigned num_configs)
+{
+	unsigned long *dup_configs;
+
+	if (WARN_ON(*num_maps == *reserved_maps))
+		return -ENOSPC;
+
+	dup_configs = kmemdup(configs, num_configs * sizeof(*dup_configs),
+			      GFP_KERNEL);
+	if (!dup_configs) {
+		dev_err(dev, "kmemdup(configs) failed\n");
+		return -ENOMEM;
+	}
+
+	(*map)[*num_maps].type = PIN_MAP_TYPE_CONFIGS_GROUP;
+	(*map)[*num_maps].data.configs.group_or_pin = group;
+	(*map)[*num_maps].data.configs.configs = dup_configs;
+	(*map)[*num_maps].data.configs.num_configs = num_configs;
+	(*num_maps)++;
+
+	return 0;
+}
+
+static int add_config(struct device *dev, unsigned long **configs,
+		      unsigned *num_configs, unsigned long config)
+{
+	unsigned old_num = *num_configs;
+	unsigned new_num = old_num + 1;
+	unsigned long *new_configs;
+
+	new_configs = krealloc(*configs, sizeof(*new_configs) * new_num,
+			       GFP_KERNEL);
+	if (!new_configs) {
+		dev_err(dev, "krealloc(configs) failed\n");
+		return -ENOMEM;
+	}
+
+	new_configs[old_num] = config;
+
+	*configs = new_configs;
+	*num_configs = new_num;
+
+	return 0;
+}
+
+void tegra_pinctrl_dt_free_map(struct pinctrl_dev *pctldev,
+			       struct pinctrl_map *map, unsigned num_maps)
+{
+	int i;
+
+	for (i = 0; i < num_maps; i++)
+		if (map[i].type == PIN_MAP_TYPE_CONFIGS_GROUP)
+			kfree(map[i].data.configs.configs);
+
+	kfree(map);
+}
+
+static const struct cfg_param {
+	const char *property;
+	enum tegra_pinconf_param param;
+} cfg_params[] = {
+	{"nvidia,pull",			TEGRA_PINCONF_PARAM_PULL},
+	{"nvidia,tristate",		TEGRA_PINCONF_PARAM_TRISTATE},
+	{"nvidia,enable-input",		TEGRA_PINCONF_PARAM_ENABLE_INPUT},
+	{"nvidia,open-drain",		TEGRA_PINCONF_PARAM_OPEN_DRAIN},
+	{"nvidia,lock",			TEGRA_PINCONF_PARAM_LOCK},
+	{"nvidia,io-reset",		TEGRA_PINCONF_PARAM_IORESET},
+	{"nvidia,high-speed-mode",	TEGRA_PINCONF_PARAM_HIGH_SPEED_MODE},
+	{"nvidia,schmitt",		TEGRA_PINCONF_PARAM_SCHMITT},
+	{"nvidia,low-power-mode",	TEGRA_PINCONF_PARAM_LOW_POWER_MODE},
+	{"nvidia,pull-down-strength",	TEGRA_PINCONF_PARAM_DRIVE_DOWN_STRENGTH},
+	{"nvidia,pull-up-strength",	TEGRA_PINCONF_PARAM_DRIVE_UP_STRENGTH},
+	{"nvidia,slew-rate-falling",	TEGRA_PINCONF_PARAM_SLEW_RATE_FALLING},
+	{"nvidia,slew-rate-rising",	TEGRA_PINCONF_PARAM_SLEW_RATE_RISING},
+};
+
+int tegra_pinctrl_dt_subnode_to_map(struct device *dev,
+				    struct device_node *np,
+				    struct pinctrl_map **map,
+				    unsigned *reserved_maps,
+				    unsigned *num_maps)
+{
+	int ret, i;
+	const char *function;
+	u32 val;
+	unsigned long config;
+	unsigned long *configs = NULL;
+	unsigned num_configs = 0;
+	unsigned reserve;
+	struct property *prop;
+	const char *group;
+
+	ret = of_property_read_string(np, "nvidia,function", &function);
+	if (ret < 0) {
+		/* EINVAL=missing, which is fine since it's optional */
+		if (ret != -EINVAL)
+			dev_err(dev,
+				"could not parse property nvidia,function\n");
+		function = NULL;
+	}
+
+	for (i = 0; i < ARRAY_SIZE(cfg_params); i++) {
+		ret = of_property_read_u32(np, cfg_params[i].property, &val);
+		if (!ret) {
+			config = TEGRA_PINCONF_PACK(cfg_params[i].param, val);
+			ret = add_config(dev, &configs, &num_configs, config);
+			if (ret < 0)
+				goto exit;
+		/* EINVAL=missing, which is fine since it's optional */
+		} else if (ret != -EINVAL) {
+			dev_err(dev, "could not parse property %s\n",
+				cfg_params[i].property);
+		}
+	}
+
+	reserve = 0;
+	if (function != NULL)
+		reserve++;
+	if (num_configs)
+		reserve++;
+	ret = of_property_count_strings(np, "nvidia,pins");
+	if (ret < 0) {
+		dev_err(dev, "could not parse property nvidia,pins\n");
+		goto exit;
+	}
+	reserve *= ret;
+
+	ret = reserve_map(dev, map, reserved_maps, num_maps, reserve);
+	if (ret < 0)
+		goto exit;
+
+	of_property_for_each_string(np, "nvidia,pins", prop, group) {
+		if (function) {
+			ret = add_map_mux(map, reserved_maps, num_maps,
+					  group, function);
+			if (ret < 0)
+				goto exit;
+		}
+
+		if (num_configs) {
+			ret = add_map_configs(dev, map, reserved_maps,
+					      num_maps, group, configs,
+					      num_configs);
+			if (ret < 0)
+				goto exit;
+		}
+	}
+
+	ret = 0;
+
+exit:
+	kfree(configs);
+	return ret;
+}
+
+int tegra_pinctrl_dt_node_to_map(struct pinctrl_dev *pctldev,
+				 struct device_node *np_config,
+				 struct pinctrl_map **map, unsigned *num_maps)
+{
+	unsigned reserved_maps;
+	struct device_node *np;
+	int ret;
+
+	reserved_maps = 0;
+	*map = NULL;
+	*num_maps = 0;
+
+	for_each_child_of_node(np_config, np) {
+		ret = tegra_pinctrl_dt_subnode_to_map(pctldev->dev, np, map,
+						      &reserved_maps, num_maps);
+		if (ret < 0) {
+			tegra_pinctrl_dt_free_map(pctldev, *map, *num_maps);
+			return ret;
+		}
+	}
+
+	return 0;
 }
 
 static struct pinctrl_ops tegra_pinctrl_ops = {
-	.list_groups = tegra_pinctrl_list_groups,
+	.get_groups_count = tegra_pinctrl_get_groups_count,
 	.get_group_name = tegra_pinctrl_get_group_name,
 	.get_group_pins = tegra_pinctrl_get_group_pins,
+#ifdef CONFIG_DEBUG_FS
 	.pin_dbg_show = tegra_pinctrl_pin_dbg_show,
+#endif
+	.dt_node_to_map = tegra_pinctrl_dt_node_to_map,
+	.dt_free_map = tegra_pinctrl_dt_free_map,
 };
 
-static int tegra_pinctrl_list_funcs(struct pinctrl_dev *pctldev,
-				    unsigned function)
+static int tegra_pinctrl_get_funcs_count(struct pinctrl_dev *pctldev)
 {
 	struct tegra_pmx *pmx = pinctrl_dev_get_drvdata(pctldev);
 
-	if (function >= pmx->soc->nfunctions)
-		return -EINVAL;
-
-	return 0;
+	return pmx->soc->nfunctions;
 }
 
 static const char *tegra_pinctrl_get_func_name(struct pinctrl_dev *pctldev,
@@ -121,9 +337,6 @@ static const char *tegra_pinctrl_get_func_name(struct pinctrl_dev *pctldev,
 {
 	struct tegra_pmx *pmx = pinctrl_dev_get_drvdata(pctldev);
 
-	if (function >= pmx->soc->nfunctions)
-		return NULL;
-
 	return pmx->soc->functions[function].name;
 }
 
@@ -134,9 +347,6 @@ static int tegra_pinctrl_get_func_groups(struct pinctrl_dev *pctldev,
 {
 	struct tegra_pmx *pmx = pinctrl_dev_get_drvdata(pctldev);
 
-	if (function >= pmx->soc->nfunctions)
-		return -EINVAL;
-
 	*groups = pmx->soc->functions[function].groups;
 	*num_groups = pmx->soc->functions[function].ngroups;
 
@@ -151,18 +361,16 @@ static int tegra_pinctrl_enable(struct pinctrl_dev *pctldev, unsigned function,
 	int i;
 	u32 val;
 
-	if (group >= pmx->soc->ngroups)
-		return -EINVAL;
 	g = &pmx->soc->groups[group];
 
-	if (g->mux_reg < 0)
+	if (WARN_ON(g->mux_reg < 0))
 		return -EINVAL;
 
 	for (i = 0; i < ARRAY_SIZE(g->funcs); i++) {
 		if (g->funcs[i] == function)
 			break;
 	}
-	if (i == ARRAY_SIZE(g->funcs))
+	if (WARN_ON(i == ARRAY_SIZE(g->funcs)))
 		return -EINVAL;
 
 	val = pmx_readl(pmx, g->mux_bank, g->mux_reg);
@@ -180,11 +388,9 @@ static void tegra_pinctrl_disable(struct pinctrl_dev *pctldev,
 	const struct tegra_pingroup *g;
 	u32 val;
 
-	if (group >= pmx->soc->ngroups)
-		return;
 	g = &pmx->soc->groups[group];
 
-	if (g->mux_reg < 0)
+	if (WARN_ON(g->mux_reg < 0))
 		return;
 
 	val = pmx_readl(pmx, g->mux_bank, g->mux_reg);
@@ -194,7 +400,7 @@ static void tegra_pinctrl_disable(struct pinctrl_dev *pctldev,
 }
 
 static struct pinmux_ops tegra_pinmux_ops = {
-	.list_functions = tegra_pinctrl_list_funcs,
+	.get_functions_count = tegra_pinctrl_get_funcs_count,
 	.get_function_name = tegra_pinctrl_get_func_name,
 	.get_function_groups = tegra_pinctrl_get_func_groups,
 	.enable = tegra_pinctrl_enable,
@@ -204,6 +410,7 @@ static struct pinmux_ops tegra_pinmux_ops = {
 static int tegra_pinconf_reg(struct tegra_pmx *pmx,
 			     const struct tegra_pingroup *g,
 			     enum tegra_pinconf_param param,
+			     bool report_err,
 			     s8 *bank, s16 *reg, s8 *bit, s8 *width)
 {
 	switch (param) {
@@ -291,9 +498,10 @@ static int tegra_pinconf_reg(struct tegra_pmx *pmx,
 	}
 
 	if (*reg < 0) {
-		dev_err(pmx->dev,
-			"Config param %04x not supported on group %s\n",
-			param, g->name);
+		if (report_err)
+			dev_err(pmx->dev,
+				"Config param %04x not supported on group %s\n",
+				param, g->name);
 		return -ENOTSUPP;
 	}
 
@@ -303,12 +511,14 @@ static int tegra_pinconf_reg(struct tegra_pmx *pmx,
 static int tegra_pinconf_get(struct pinctrl_dev *pctldev,
 			     unsigned pin, unsigned long *config)
 {
+	dev_err(pctldev->dev, "pin_config_get op not supported\n");
 	return -ENOTSUPP;
 }
 
 static int tegra_pinconf_set(struct pinctrl_dev *pctldev,
 			     unsigned pin, unsigned long config)
 {
+	dev_err(pctldev->dev, "pin_config_set op not supported\n");
 	return -ENOTSUPP;
 }
 
@@ -324,11 +534,10 @@ static int tegra_pinconf_group_get(struct pinctrl_dev *pctldev,
 	s16 reg;
 	u32 val, mask;
 
-	if (group >= pmx->soc->ngroups)
-		return -EINVAL;
 	g = &pmx->soc->groups[group];
 
-	ret = tegra_pinconf_reg(pmx, g, param, &bank, &reg, &bit, &width);
+	ret = tegra_pinconf_reg(pmx, g, param, true, &bank, &reg, &bit,
+				&width);
 	if (ret < 0)
 		return ret;
 
@@ -353,11 +562,10 @@ static int tegra_pinconf_group_set(struct pinctrl_dev *pctldev,
 	s16 reg;
 	u32 val, mask;
 
-	if (group >= pmx->soc->ngroups)
-		return -EINVAL;
 	g = &pmx->soc->groups[group];
 
-	ret = tegra_pinconf_reg(pmx, g, param, &bank, &reg, &bit, &width);
+	ret = tegra_pinconf_reg(pmx, g, param, true, &bank, &reg, &bit,
+				&width);
 	if (ret < 0)
 		return ret;
 
@@ -365,8 +573,10 @@ static int tegra_pinconf_group_set(struct pinctrl_dev *pctldev,
 
 	/* LOCK can't be cleared */
 	if (param == TEGRA_PINCONF_PARAM_LOCK) {
-		if ((val & BIT(bit)) && !arg)
+		if ((val & BIT(bit)) && !arg) {
+			dev_err(pctldev->dev, "LOCK bit cannot be cleared\n");
 			return -EINVAL;
+		}
 	}
 
 	/* Special-case Boolean values; allow any non-zero as true */
@@ -375,8 +585,12 @@ static int tegra_pinconf_group_set(struct pinctrl_dev *pctldev,
 
 	/* Range-check user-supplied value */
 	mask = (1 << width) - 1;
-	if (arg & ~mask)
+	if (arg & ~mask) {
+		dev_err(pctldev->dev,
+			"config %lx: %x too big for %d bit register\n",
+			config, arg, width);
 		return -EINVAL;
+	}
 
 	/* Update register */
 	val &= ~(mask << bit);
@@ -386,23 +600,78 @@ static int tegra_pinconf_group_set(struct pinctrl_dev *pctldev,
 	return 0;
 }
 
+#ifdef CONFIG_DEBUG_FS
 static void tegra_pinconf_dbg_show(struct pinctrl_dev *pctldev,
 				   struct seq_file *s, unsigned offset)
 {
 }
 
+static const char *strip_prefix(const char *s)
+{
+	const char *comma = strchr(s, ',');
+	if (!comma)
+		return s;
+
+	return comma + 1;
+}
+
 static void tegra_pinconf_group_dbg_show(struct pinctrl_dev *pctldev,
-					 struct seq_file *s, unsigned selector)
+					 struct seq_file *s, unsigned group)
 {
+	struct tegra_pmx *pmx = pinctrl_dev_get_drvdata(pctldev);
+	const struct tegra_pingroup *g;
+	int i, ret;
+	s8 bank, bit, width;
+	s16 reg;
+	u32 val;
+
+	g = &pmx->soc->groups[group];
+
+	for (i = 0; i < ARRAY_SIZE(cfg_params); i++) {
+		ret = tegra_pinconf_reg(pmx, g, cfg_params[i].param, false,
+					&bank, &reg, &bit, &width);
+		if (ret < 0)
+			continue;
+
+		val = pmx_readl(pmx, bank, reg);
+		val >>= bit;
+		val &= (1 << width) - 1;
+
+		seq_printf(s, "\n\t%s=%u",
+			   strip_prefix(cfg_params[i].property), val);
+	}
 }
 
+static void tegra_pinconf_config_dbg_show(struct pinctrl_dev *pctldev,
+					  struct seq_file *s,
+					  unsigned long config)
+{
+	enum tegra_pinconf_param param = TEGRA_PINCONF_UNPACK_PARAM(config);
+	u16 arg = TEGRA_PINCONF_UNPACK_ARG(config);
+	const char *pname = "unknown";
+	int i;
+
+	for (i = 0; i < ARRAY_SIZE(cfg_params); i++) {
+		if (cfg_params[i].param == param) {
+			pname = cfg_params[i].property;
+			break;
+		}
+	}
+
+	seq_printf(s, "%s=%d", strip_prefix(pname), arg);
+}
+#endif
+
 struct pinconf_ops tegra_pinconf_ops = {
 	.pin_config_get = tegra_pinconf_get,
 	.pin_config_set = tegra_pinconf_set,
 	.pin_config_group_get = tegra_pinconf_group_get,
 	.pin_config_group_set = tegra_pinconf_group_set,
+#ifdef CONFIG_DEBUG_FS
 	.pin_config_dbg_show = tegra_pinconf_dbg_show,
 	.pin_config_group_dbg_show = tegra_pinconf_group_dbg_show,
+	.pin_config_config_dbg_show = tegra_pinconf_config_dbg_show,
+#endif
 };
 
 static struct pinctrl_gpio_range tegra_pinctrl_gpio_range = {
@@ -412,60 +681,29 @@ static struct pinctrl_gpio_range tegra_pinctrl_gpio_range = {
 };
 
 static struct pinctrl_desc tegra_pinctrl_desc = {
-	.name = DRIVER_NAME,
 	.pctlops = &tegra_pinctrl_ops,
 	.pmxops = &tegra_pinmux_ops,
 	.confops = &tegra_pinconf_ops,
 	.owner = THIS_MODULE,
 };
 
-static struct of_device_id tegra_pinctrl_of_match[] __devinitdata = {
-#ifdef CONFIG_PINCTRL_TEGRA20
-	{
-		.compatible = "nvidia,tegra20-pinmux-disabled",
-		.data = tegra20_pinctrl_init,
-	},
-#endif
-#ifdef CONFIG_PINCTRL_TEGRA30
-	{
-		.compatible = "nvidia,tegra30-pinmux-disabled",
-		.data = tegra30_pinctrl_init,
-	},
-#endif
-	{},
-};
-
-static int __devinit tegra_pinctrl_probe(struct platform_device *pdev)
+int __devinit tegra_pinctrl_probe(struct platform_device *pdev,
+			const struct tegra_pinctrl_soc_data *soc_data)
 {
-	const struct of_device_id *match;
-	tegra_pinctrl_soc_initf initf = NULL;
 	struct tegra_pmx *pmx;
 	struct resource *res;
 	int i;
 
-	match = of_match_device(tegra_pinctrl_of_match, &pdev->dev);
-	if (match)
-		initf = (tegra_pinctrl_soc_initf)match->data;
-#ifdef CONFIG_PINCTRL_TEGRA20
-	if (!initf)
-		initf = tegra20_pinctrl_init;
-#endif
-	if (!initf) {
-		dev_err(&pdev->dev,
-			"Could not determine SoC-specific init func\n");
-		return -EINVAL;
-	}
-
 	pmx = devm_kzalloc(&pdev->dev, sizeof(*pmx), GFP_KERNEL);
 	if (!pmx) {
 		dev_err(&pdev->dev, "Can't alloc tegra_pmx\n");
 		return -ENOMEM;
 	}
 	pmx->dev = &pdev->dev;
-
-	(*initf)(&pmx->soc);
+	pmx->soc = soc_data;
 
 	tegra_pinctrl_gpio_range.npins = pmx->soc->ngpios;
+	tegra_pinctrl_desc.name = dev_name(&pdev->dev);
 	tegra_pinctrl_desc.pins = pmx->soc->pins;
 	tegra_pinctrl_desc.npins = pmx->soc->npins;
 
@@ -520,8 +758,9 @@ static int __devinit tegra_pinctrl_probe(struct platform_device *pdev)
 
 	return 0;
 }
+EXPORT_SYMBOL_GPL(tegra_pinctrl_probe);
 
-static int __devexit tegra_pinctrl_remove(struct platform_device *pdev)
+int __devexit tegra_pinctrl_remove(struct platform_device *pdev)
 {
 	struct tegra_pmx *pmx = platform_get_drvdata(pdev);
 
@@ -530,30 +769,4 @@ static int __devexit tegra_pinctrl_remove(struct platform_device *pdev)
 
 	return 0;
 }
-
-static struct platform_driver tegra_pinctrl_driver = {
-	.driver = {
-		.name = DRIVER_NAME,
-		.owner = THIS_MODULE,
-		.of_match_table = tegra_pinctrl_of_match,
-	},
-	.probe = tegra_pinctrl_probe,
-	.remove = __devexit_p(tegra_pinctrl_remove),
-};
-
-static int __init tegra_pinctrl_init(void)
-{
-	return platform_driver_register(&tegra_pinctrl_driver);
-}
-arch_initcall(tegra_pinctrl_init);
-
-static void __exit tegra_pinctrl_exit(void)
-{
-	platform_driver_unregister(&tegra_pinctrl_driver);
-}
-module_exit(tegra_pinctrl_exit);
-
-MODULE_AUTHOR("Stephen Warren <swarren@nvidia.com>");
-MODULE_DESCRIPTION("NVIDIA Tegra pinctrl driver");
-MODULE_LICENSE("GPL v2");
-MODULE_DEVICE_TABLE(of, tegra_pinctrl_of_match);
+EXPORT_SYMBOL_GPL(tegra_pinctrl_remove);
diff --git a/drivers/pinctrl/pinctrl-tegra.h b/drivers/pinctrl/pinctrl-tegra.h
index 782c795326e..705c007a38c 100644
--- a/drivers/pinctrl/pinctrl-tegra.h
+++ b/drivers/pinctrl/pinctrl-tegra.h
@@ -139,25 +139,8 @@ struct tegra_pinctrl_soc_data {
 	unsigned ngroups;
 };
 
-/**
- * tegra_pinctrl_soc_initf() - Retrieve pin controller details for a SoC.
- * @soc_data:	This pointer must be updated to point at a struct containing
- *		details of the SoC.
- */
-typedef void (*tegra_pinctrl_soc_initf)(
-			const struct tegra_pinctrl_soc_data **soc_data);
-
-/**
- * tegra20_pinctrl_init() - Retrieve pin controller details for Tegra20
- * @soc_data:	This pointer will be updated to point at a struct containing
- *		details of Tegra20's pin controller.
- */
-void tegra20_pinctrl_init(const struct tegra_pinctrl_soc_data **soc_data);
-/**
- * tegra30_pinctrl_init() - Retrieve pin controller details for Tegra20
- * @soc_data:	This pointer will be updated to point at a struct containing
- *		details of Tegra30's pin controller.
- */
-void tegra30_pinctrl_init(const struct tegra_pinctrl_soc_data **soc_data);
+int tegra_pinctrl_probe(struct platform_device *pdev,
+			const struct tegra_pinctrl_soc_data *soc_data);
+int tegra_pinctrl_remove(struct platform_device *pdev);
 
 #endif
diff --git a/drivers/pinctrl/pinctrl-tegra20.c b/drivers/pinctrl/pinctrl-tegra20.c
index f69ff96aa29..a74f9a56853 100644
--- a/drivers/pinctrl/pinctrl-tegra20.c
+++ b/drivers/pinctrl/pinctrl-tegra20.c
@@ -1,7 +1,7 @@
 /*
  * Pinctrl data for the NVIDIA Tegra20 pinmux
  *
- * Copyright (c) 2011, NVIDIA CORPORATION.  All rights reserved.
+ * Copyright (c) 2011-2012, NVIDIA CORPORATION.  All rights reserved.
  *
  * Derived from code:
  * Copyright (C) 2010 Google, Inc.
@@ -17,6 +17,8 @@
  * more details.
  */
 
+#include <linux/module.h>
+#include <linux/of.h>
 #include <linux/platform_device.h>
 #include <linux/pinctrl/pinctrl.h>
 #include <linux/pinctrl/pinmux.h>
@@ -2854,7 +2856,39 @@ static const struct tegra_pinctrl_soc_data tegra20_pinctrl = {
 	.ngroups = ARRAY_SIZE(tegra20_groups),
 };
 
-void __devinit tegra20_pinctrl_init(const struct tegra_pinctrl_soc_data **soc)
+static int __devinit tegra20_pinctrl_probe(struct platform_device *pdev)
 {
-	*soc = &tegra20_pinctrl;
+	return tegra_pinctrl_probe(pdev, &tegra20_pinctrl);
 }
+
+static struct of_device_id tegra20_pinctrl_of_match[] __devinitdata = {
+	{ .compatible = "nvidia,tegra20-pinmux", },
+	{ },
+};
+
+static struct platform_driver tegra20_pinctrl_driver = {
+	.driver = {
+		.name = "tegra20-pinctrl",
+		.owner = THIS_MODULE,
+		.of_match_table = tegra20_pinctrl_of_match,
+	},
+	.probe = tegra20_pinctrl_probe,
+	.remove = __devexit_p(tegra_pinctrl_remove),
+};
+
+static int __init tegra20_pinctrl_init(void)
+{
+	return platform_driver_register(&tegra20_pinctrl_driver);
+}
+arch_initcall(tegra20_pinctrl_init);
+
+static void __exit tegra20_pinctrl_exit(void)
+{
+	platform_driver_unregister(&tegra20_pinctrl_driver);
+}
+module_exit(tegra20_pinctrl_exit);
+
+MODULE_AUTHOR("Stephen Warren <swarren@nvidia.com>");
+MODULE_DESCRIPTION("NVIDIA Tegra20 pinctrl driver");
+MODULE_LICENSE("GPL v2");
+MODULE_DEVICE_TABLE(of, tegra20_pinctrl_of_match);
diff --git a/drivers/pinctrl/pinctrl-tegra30.c b/drivers/pinctrl/pinctrl-tegra30.c
index 4d7571d4a43..0386fdf0da1 100644
--- a/drivers/pinctrl/pinctrl-tegra30.c
+++ b/drivers/pinctrl/pinctrl-tegra30.c
@@ -1,7 +1,7 @@
 /*
  * Pinctrl data for the NVIDIA Tegra30 pinmux
  *
- * Copyright (c) 2011, NVIDIA CORPORATION.  All rights reserved.
+ * Copyright (c) 2011-2012, 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,
@@ -13,6 +13,8 @@
  * more details.
  */
 
+#include <linux/module.h>
+#include <linux/of.h>
 #include <linux/platform_device.h>
 #include <linux/pinctrl/pinctrl.h>
 #include <linux/pinctrl/pinmux.h>
@@ -3720,7 +3722,39 @@ static const struct tegra_pinctrl_soc_data tegra30_pinctrl = {
 	.ngroups = ARRAY_SIZE(tegra30_groups),
 };
 
-void __devinit tegra30_pinctrl_init(const struct tegra_pinctrl_soc_data **soc)
+static int __devinit tegra30_pinctrl_probe(struct platform_device *pdev)
 {
-	*soc = &tegra30_pinctrl;
+	return tegra_pinctrl_probe(pdev, &tegra30_pinctrl);
 }
+
+static struct of_device_id tegra30_pinctrl_of_match[] __devinitdata = {
+	{ .compatible = "nvidia,tegra30-pinmux", },
+	{ },
+};
+
+static struct platform_driver tegra30_pinctrl_driver = {
+	.driver = {
+		.name = "tegra30-pinctrl",
+		.owner = THIS_MODULE,
+		.of_match_table = tegra30_pinctrl_of_match,
+	},
+	.probe = tegra30_pinctrl_probe,
+	.remove = __devexit_p(tegra_pinctrl_remove),
+};
+
+static int __init tegra30_pinctrl_init(void)
+{
+	return platform_driver_register(&tegra30_pinctrl_driver);
+}
+arch_initcall(tegra30_pinctrl_init);
+
+static void __exit tegra30_pinctrl_exit(void)
+{
+	platform_driver_unregister(&tegra30_pinctrl_driver);
+}
+module_exit(tegra30_pinctrl_exit);
+
+MODULE_AUTHOR("Stephen Warren <swarren@nvidia.com>");
+MODULE_DESCRIPTION("NVIDIA Tegra30 pinctrl driver");
+MODULE_LICENSE("GPL v2");
+MODULE_DEVICE_TABLE(of, tegra30_pinctrl_of_match);
diff --git a/drivers/pinctrl/pinctrl-u300.c b/drivers/pinctrl/pinctrl-u300.c
index 26eb8ccd72d..05d029911be 100644
--- a/drivers/pinctrl/pinctrl-u300.c
+++ b/drivers/pinctrl/pinctrl-u300.c
@@ -836,18 +836,14 @@ static const struct u300_pin_group u300_pin_groups[] = {
 	},
 };
 
-static int u300_list_groups(struct pinctrl_dev *pctldev, unsigned selector)
+static int u300_get_groups_count(struct pinctrl_dev *pctldev)
 {
-	if (selector >= ARRAY_SIZE(u300_pin_groups))
-		return -EINVAL;
-	return 0;
+	return ARRAY_SIZE(u300_pin_groups);
 }
 
 static const char *u300_get_group_name(struct pinctrl_dev *pctldev,
 				       unsigned selector)
 {
-	if (selector >= ARRAY_SIZE(u300_pin_groups))
-		return NULL;
 	return u300_pin_groups[selector].name;
 }
 
@@ -855,8 +851,6 @@ static int u300_get_group_pins(struct pinctrl_dev *pctldev, unsigned selector,
 			       const unsigned **pins,
 			       unsigned *num_pins)
 {
-	if (selector >= ARRAY_SIZE(u300_pin_groups))
-		return -EINVAL;
 	*pins = u300_pin_groups[selector].pins;
 	*num_pins = u300_pin_groups[selector].num_pins;
 	return 0;
@@ -869,7 +863,7 @@ static void u300_pin_dbg_show(struct pinctrl_dev *pctldev, struct seq_file *s,
 }
 
 static struct pinctrl_ops u300_pctrl_ops = {
-	.list_groups = u300_list_groups,
+	.get_groups_count = u300_get_groups_count,
 	.get_group_name = u300_get_group_name,
 	.get_group_pins = u300_get_group_pins,
 	.pin_dbg_show = u300_pin_dbg_show,
@@ -991,11 +985,9 @@ static void u300_pmx_disable(struct pinctrl_dev *pctldev, unsigned selector,
 	u300_pmx_endisable(upmx, selector, false);
 }
 
-static int u300_pmx_list_funcs(struct pinctrl_dev *pctldev, unsigned selector)
+static int u300_pmx_get_funcs_count(struct pinctrl_dev *pctldev)
 {
-	if (selector >= ARRAY_SIZE(u300_pmx_functions))
-		return -EINVAL;
-	return 0;
+	return ARRAY_SIZE(u300_pmx_functions);
 }
 
 static const char *u300_pmx_get_func_name(struct pinctrl_dev *pctldev,
@@ -1014,7 +1006,7 @@ static int u300_pmx_get_groups(struct pinctrl_dev *pctldev, unsigned selector,
 }
 
 static struct pinmux_ops u300_pmx_ops = {
-	.list_functions = u300_pmx_list_funcs,
+	.get_functions_count = u300_pmx_get_funcs_count,
 	.get_function_name = u300_pmx_get_func_name,
 	.get_function_groups = u300_pmx_get_groups,
 	.enable = u300_pmx_enable,
diff --git a/drivers/pinctrl/pinmux.c b/drivers/pinctrl/pinmux.c
index 4e62783a573..3d5ac73bd5a 100644
--- a/drivers/pinctrl/pinmux.c
+++ b/drivers/pinctrl/pinmux.c
@@ -33,22 +33,25 @@
 int pinmux_check_ops(struct pinctrl_dev *pctldev)
 {
 	const struct pinmux_ops *ops = pctldev->desc->pmxops;
+	unsigned nfuncs;
 	unsigned selector = 0;
 
 	/* Check that we implement required operations */
-	if (!ops->list_functions ||
+	if (!ops ||
+	    !ops->get_functions_count ||
 	    !ops->get_function_name ||
 	    !ops->get_function_groups ||
-	    !ops->enable ||
-	    !ops->disable)
+	    !ops->enable) {
+		dev_err(pctldev->dev, "pinmux ops lacks necessary functions\n");
 		return -EINVAL;
-
+	}
 	/* Check that all functions registered have names */
-	while (ops->list_functions(pctldev, selector) >= 0) {
+	nfuncs = ops->get_functions_count(pctldev);
+	while (selector < nfuncs) {
 		const char *fname = ops->get_function_name(pctldev,
 							   selector);
 		if (!fname) {
-			pr_err("pinmux ops has no name for function%u\n",
+			dev_err(pctldev->dev, "pinmux ops has no name for function%u\n",
 				selector);
 			return -EINVAL;
 		}
@@ -85,20 +88,23 @@ static int pin_request(struct pinctrl_dev *pctldev,
 	const struct pinmux_ops *ops = pctldev->desc->pmxops;
 	int status = -EINVAL;
 
-	dev_dbg(pctldev->dev, "request pin %d for %s\n", pin, owner);
-
 	desc = pin_desc_get(pctldev, pin);
 	if (desc == NULL) {
 		dev_err(pctldev->dev,
-			"pin is not registered so it cannot be requested\n");
+			"pin %d is not registered so it cannot be requested\n",
+			pin);
 		goto out;
 	}
 
+	dev_dbg(pctldev->dev, "request pin %d (%s) for %s\n",
+		pin, desc->name, owner);
+
 	if (gpio_range) {
 		/* There's no need to support multiple GPIO requests */
 		if (desc->gpio_owner) {
 			dev_err(pctldev->dev,
-				"pin already requested\n");
+				"pin %s already requested by %s; cannot claim for %s\n",
+				desc->name, desc->gpio_owner, owner);
 			goto out;
 		}
 
@@ -106,7 +112,8 @@ static int pin_request(struct pinctrl_dev *pctldev,
 	} else {
 		if (desc->mux_usecount && strcmp(desc->mux_owner, owner)) {
 			dev_err(pctldev->dev,
-				"pin already requested\n");
+				"pin %s already requested by %s; cannot claim for %s\n",
+				desc->name, desc->mux_owner, owner);
 			goto out;
 		}
 
@@ -139,8 +146,7 @@ static int pin_request(struct pinctrl_dev *pctldev,
 		status = 0;
 
 	if (status) {
-		dev_err(pctldev->dev, "->request on device %s failed for pin %d\n",
-		       pctldev->desc->name, pin);
+		dev_err(pctldev->dev, "request() failed for pin %d\n", pin);
 		module_put(pctldev->owner);
 	}
 
@@ -157,7 +163,7 @@ out_free_pin:
 out:
 	if (status)
 		dev_err(pctldev->dev, "pin-%d (%s) status %d\n",
-		       pin, owner, status);
+			pin, owner, status);
 
 	return status;
 }
@@ -287,10 +293,11 @@ static int pinmux_func_name_to_selector(struct pinctrl_dev *pctldev,
 					const char *function)
 {
 	const struct pinmux_ops *ops = pctldev->desc->pmxops;
+	unsigned nfuncs = ops->get_functions_count(pctldev);
 	unsigned selector = 0;
 
 	/* See if this pctldev has this function */
-	while (ops->list_functions(pctldev, selector) >= 0) {
+	while (selector < nfuncs) {
 		const char *fname = ops->get_function_name(pctldev,
 							   selector);
 
@@ -319,18 +326,32 @@ int pinmux_map_to_setting(struct pinctrl_map const *map,
 	const unsigned *pins;
 	unsigned num_pins;
 
-	setting->data.mux.func =
-		pinmux_func_name_to_selector(pctldev, map->data.mux.function);
-	if (setting->data.mux.func < 0)
-		return setting->data.mux.func;
+	if (!pmxops) {
+		dev_err(pctldev->dev, "does not support mux function\n");
+		return -EINVAL;
+	}
+
+	ret = pinmux_func_name_to_selector(pctldev, map->data.mux.function);
+	if (ret < 0) {
+		dev_err(pctldev->dev, "invalid function %s in map table\n",
+			map->data.mux.function);
+		return ret;
+	}
+	setting->data.mux.func = ret;
 
 	ret = pmxops->get_function_groups(pctldev, setting->data.mux.func,
 					  &groups, &num_groups);
-	if (ret < 0)
+	if (ret < 0) {
+		dev_err(pctldev->dev, "can't query groups for function %s\n",
+			map->data.mux.function);
 		return ret;
-	if (!num_groups)
+	}
+	if (!num_groups) {
+		dev_err(pctldev->dev,
+			"function %s can't be selected on any group\n",
+			map->data.mux.function);
 		return -EINVAL;
-
+	}
 	if (map->data.mux.group) {
 		bool found = false;
 		group = map->data.mux.group;
@@ -340,15 +361,23 @@ int pinmux_map_to_setting(struct pinctrl_map const *map,
 				break;
 			}
 		}
-		if (!found)
+		if (!found) {
+			dev_err(pctldev->dev,
+				"invalid group \"%s\" for function \"%s\"\n",
+				group, map->data.mux.function);
 			return -EINVAL;
+		}
 	} else {
 		group = groups[0];
 	}
 
-	setting->data.mux.group = pinctrl_get_group_selector(pctldev, group);
-	if (setting->data.mux.group < 0)
-		return setting->data.mux.group;
+	ret = pinctrl_get_group_selector(pctldev, group);
+	if (ret < 0) {
+		dev_err(pctldev->dev, "invalid group %s in map table\n",
+			map->data.mux.group);
+		return ret;
+	}
+	setting->data.mux.group = ret;
 
 	ret = pctlops->get_group_pins(pctldev, setting->data.mux.group, &pins,
 				      &num_pins);
@@ -364,7 +393,7 @@ int pinmux_map_to_setting(struct pinctrl_map const *map,
 		ret = pin_request(pctldev, pins[i], map->dev_name, NULL);
 		if (ret) {
 			dev_err(pctldev->dev,
-				"could not get request pin %d on device %s\n",
+				"could not request pin %d on device %s\n",
 				pins[i], pinctrl_dev_get_name(pctldev));
 			/* On error release all taken pins */
 			i--; /* this pin just failed */
@@ -467,7 +496,8 @@ void pinmux_disable_setting(struct pinctrl_setting const *setting)
 		desc->mux_setting = NULL;
 	}
 
-	ops->disable(pctldev, setting->data.mux.func, setting->data.mux.group);
+	if (ops->disable)
+		ops->disable(pctldev, setting->data.mux.func, setting->data.mux.group);
 }
 
 #ifdef CONFIG_DEBUG_FS
@@ -477,11 +507,15 @@ static int pinmux_functions_show(struct seq_file *s, void *what)
 {
 	struct pinctrl_dev *pctldev = s->private;
 	const struct pinmux_ops *pmxops = pctldev->desc->pmxops;
+	unsigned nfuncs;
 	unsigned func_selector = 0;
 
-	mutex_lock(&pinctrl_mutex);
+	if (!pmxops)
+		return 0;
 
-	while (pmxops->list_functions(pctldev, func_selector) >= 0) {
+	mutex_lock(&pinctrl_mutex);
+	nfuncs = pmxops->get_functions_count(pctldev);
+	while (func_selector < nfuncs) {
 		const char *func = pmxops->get_function_name(pctldev,
 							  func_selector);
 		const char * const *groups;
@@ -515,6 +549,9 @@ static int pinmux_pins_show(struct seq_file *s, void *what)
 	const struct pinmux_ops *pmxops = pctldev->desc->pmxops;
 	unsigned i, pin;
 
+	if (!pmxops)
+		return 0;
+
 	seq_puts(s, "Pinmux settings per pin\n");
 	seq_puts(s, "Format: pin (name): mux_owner gpio_owner hog?\n");
 
diff --git a/drivers/pinctrl/pinmux.h b/drivers/pinctrl/pinmux.h
index 6fc47003e95..d1a98b1c9fc 100644
--- a/drivers/pinctrl/pinmux.h
+++ b/drivers/pinctrl/pinmux.h
@@ -31,12 +31,6 @@ void pinmux_free_setting(struct pinctrl_setting const *setting);
 int pinmux_enable_setting(struct pinctrl_setting const *setting);
 void pinmux_disable_setting(struct pinctrl_setting const *setting);
 
-void pinmux_show_map(struct seq_file *s, struct pinctrl_map const *map);
-void pinmux_show_setting(struct seq_file *s,
-			 struct pinctrl_setting const *setting);
-void pinmux_init_device_debugfs(struct dentry *devroot,
-				struct pinctrl_dev *pctldev);
-
 #else
 
 static inline int pinmux_check_ops(struct pinctrl_dev *pctldev)
@@ -89,6 +83,18 @@ static inline void pinmux_disable_setting(
 {
 }
 
+#endif
+
+#if defined(CONFIG_PINMUX) && defined(CONFIG_DEBUG_FS)
+
+void pinmux_show_map(struct seq_file *s, struct pinctrl_map const *map);
+void pinmux_show_setting(struct seq_file *s,
+			 struct pinctrl_setting const *setting);
+void pinmux_init_device_debugfs(struct dentry *devroot,
+				struct pinctrl_dev *pctldev);
+
+#else
+
 static inline void pinmux_show_map(struct seq_file *s,
 				   struct pinctrl_map const *map)
 {
diff --git a/drivers/pinctrl/spear/Kconfig b/drivers/pinctrl/spear/Kconfig
new file mode 100644
index 00000000000..91558791e76
--- /dev/null
+++ b/drivers/pinctrl/spear/Kconfig
@@ -0,0 +1,44 @@
+#
+# ST Microelectronics SPEAr PINCTRL drivers
+#
+
+if PLAT_SPEAR
+
+config PINCTRL_SPEAR
+	bool
+	depends on OF
+	select PINMUX
+	help
+	  This enables pin control drivers for SPEAr Platform
+
+config PINCTRL_SPEAR3XX
+	bool
+	depends on ARCH_SPEAR3XX
+	select PINCTRL_SPEAR
+
+config PINCTRL_SPEAR300
+	bool "ST Microelectronics SPEAr300 SoC pin controller driver"
+	depends on MACH_SPEAR300
+	select PINCTRL_SPEAR3XX
+
+config PINCTRL_SPEAR310
+	bool "ST Microelectronics SPEAr310 SoC pin controller driver"
+	depends on MACH_SPEAR310
+	select PINCTRL_SPEAR3XX
+
+config PINCTRL_SPEAR320
+	bool "ST Microelectronics SPEAr320 SoC pin controller driver"
+	depends on MACH_SPEAR320
+	select PINCTRL_SPEAR3XX
+
+config PINCTRL_SPEAR1310
+	bool "ST Microelectronics SPEAr1310 SoC pin controller driver"
+	depends on MACH_SPEAR1310
+	select PINCTRL_SPEAR
+
+config PINCTRL_SPEAR1340
+	bool "ST Microelectronics SPEAr1340 SoC pin controller driver"
+	depends on MACH_SPEAR1340
+	select PINCTRL_SPEAR
+
+endif
diff --git a/drivers/pinctrl/spear/Makefile b/drivers/pinctrl/spear/Makefile
new file mode 100644
index 00000000000..b28a7ba2244
--- /dev/null
+++ b/drivers/pinctrl/spear/Makefile
@@ -0,0 +1,9 @@
+# SPEAr pinmux support
+
+obj-$(CONFIG_PINCTRL_SPEAR)	+= pinctrl-spear.o
+obj-$(CONFIG_PINCTRL_SPEAR3XX)	+= pinctrl-spear3xx.o
+obj-$(CONFIG_PINCTRL_SPEAR300)	+= pinctrl-spear300.o
+obj-$(CONFIG_PINCTRL_SPEAR310)	+= pinctrl-spear310.o
+obj-$(CONFIG_PINCTRL_SPEAR320)	+= pinctrl-spear320.o
+obj-$(CONFIG_PINCTRL_SPEAR1310)	+= pinctrl-spear1310.o
+obj-$(CONFIG_PINCTRL_SPEAR1340)	+= pinctrl-spear1340.o
diff --git a/drivers/pinctrl/spear/pinctrl-spear.c b/drivers/pinctrl/spear/pinctrl-spear.c
new file mode 100644
index 00000000000..5ae50aadf88
--- /dev/null
+++ b/drivers/pinctrl/spear/pinctrl-spear.c
@@ -0,0 +1,354 @@
+/*
+ * Driver for the ST Microelectronics SPEAr pinmux
+ *
+ * Copyright (C) 2012 ST Microelectronics
+ * Viresh Kumar <viresh.kumar@st.com>
+ *
+ * Inspired from:
+ * - U300 Pinctl drivers
+ * - Tegra Pinctl drivers
+ *
+ * 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/err.h>
+#include <linux/io.h>
+#include <linux/module.h>
+#include <linux/of.h>
+#include <linux/of_address.h>
+#include <linux/pinctrl/machine.h>
+#include <linux/pinctrl/pinctrl.h>
+#include <linux/pinctrl/pinmux.h>
+#include <linux/platform_device.h>
+#include <linux/slab.h>
+
+#include "pinctrl-spear.h"
+
+#define DRIVER_NAME "spear-pinmux"
+
+static inline u32 pmx_readl(struct spear_pmx *pmx, u32 reg)
+{
+	return readl_relaxed(pmx->vbase + reg);
+}
+
+static inline void pmx_writel(struct spear_pmx *pmx, u32 val, u32 reg)
+{
+	writel_relaxed(val, pmx->vbase + reg);
+}
+
+static int set_mode(struct spear_pmx *pmx, int mode)
+{
+	struct spear_pmx_mode *pmx_mode = NULL;
+	int i;
+	u32 val;
+
+	if (!pmx->machdata->pmx_modes || !pmx->machdata->npmx_modes)
+		return -EINVAL;
+
+	for (i = 0; i < pmx->machdata->npmx_modes; i++) {
+		if (pmx->machdata->pmx_modes[i]->mode == (1 << mode)) {
+			pmx_mode = pmx->machdata->pmx_modes[i];
+			break;
+		}
+	}
+
+	if (!pmx_mode)
+		return -EINVAL;
+
+	val = pmx_readl(pmx, pmx_mode->reg);
+	val &= ~pmx_mode->mask;
+	val |= pmx_mode->val;
+	pmx_writel(pmx, val, pmx_mode->reg);
+
+	pmx->machdata->mode = pmx_mode->mode;
+	dev_info(pmx->dev, "Configured Mode: %s with id: %x\n\n",
+			pmx_mode->name ? pmx_mode->name : "no_name",
+			pmx_mode->reg);
+
+	return 0;
+}
+
+void __devinit pmx_init_addr(struct spear_pinctrl_machdata *machdata, u16 reg)
+{
+	struct spear_pingroup *pgroup;
+	struct spear_modemux *modemux;
+	int i, j, group;
+
+	for (group = 0; group < machdata->ngroups; group++) {
+		pgroup = machdata->groups[group];
+
+		for (i = 0; i < pgroup->nmodemuxs; i++) {
+			modemux = &pgroup->modemuxs[i];
+
+			for (j = 0; j < modemux->nmuxregs; j++)
+				if (modemux->muxregs[j].reg == 0xFFFF)
+					modemux->muxregs[j].reg = reg;
+		}
+	}
+}
+
+static int spear_pinctrl_get_groups_cnt(struct pinctrl_dev *pctldev)
+{
+	struct spear_pmx *pmx = pinctrl_dev_get_drvdata(pctldev);
+
+	return pmx->machdata->ngroups;
+}
+
+static const char *spear_pinctrl_get_group_name(struct pinctrl_dev *pctldev,
+		unsigned group)
+{
+	struct spear_pmx *pmx = pinctrl_dev_get_drvdata(pctldev);
+
+	return pmx->machdata->groups[group]->name;
+}
+
+static int spear_pinctrl_get_group_pins(struct pinctrl_dev *pctldev,
+		unsigned group, const unsigned **pins, unsigned *num_pins)
+{
+	struct spear_pmx *pmx = pinctrl_dev_get_drvdata(pctldev);
+
+	*pins = pmx->machdata->groups[group]->pins;
+	*num_pins = pmx->machdata->groups[group]->npins;
+
+	return 0;
+}
+
+static void spear_pinctrl_pin_dbg_show(struct pinctrl_dev *pctldev,
+		struct seq_file *s, unsigned offset)
+{
+	seq_printf(s, " " DRIVER_NAME);
+}
+
+int spear_pinctrl_dt_node_to_map(struct pinctrl_dev *pctldev,
+				 struct device_node *np_config,
+				 struct pinctrl_map **map, unsigned *num_maps)
+{
+	struct spear_pmx *pmx = pinctrl_dev_get_drvdata(pctldev);
+	struct device_node *np;
+	struct property *prop;
+	const char *function, *group;
+	int ret, index = 0, count = 0;
+
+	/* calculate number of maps required */
+	for_each_child_of_node(np_config, np) {
+		ret = of_property_read_string(np, "st,function", &function);
+		if (ret < 0)
+			return ret;
+
+		ret = of_property_count_strings(np, "st,pins");
+		if (ret < 0)
+			return ret;
+
+		count += ret;
+	}
+
+	if (!count) {
+		dev_err(pmx->dev, "No child nodes passed via DT\n");
+		return -ENODEV;
+	}
+
+	*map = kzalloc(sizeof(**map) * count, GFP_KERNEL);
+	if (!*map)
+		return -ENOMEM;
+
+	for_each_child_of_node(np_config, np) {
+		of_property_read_string(np, "st,function", &function);
+		of_property_for_each_string(np, "st,pins", prop, group) {
+			(*map)[index].type = PIN_MAP_TYPE_MUX_GROUP;
+			(*map)[index].data.mux.group = group;
+			(*map)[index].data.mux.function = function;
+			index++;
+		}
+	}
+
+	*num_maps = count;
+
+	return 0;
+}
+
+void spear_pinctrl_dt_free_map(struct pinctrl_dev *pctldev,
+		struct pinctrl_map *map, unsigned num_maps)
+{
+	kfree(map);
+}
+
+static struct pinctrl_ops spear_pinctrl_ops = {
+	.get_groups_count = spear_pinctrl_get_groups_cnt,
+	.get_group_name = spear_pinctrl_get_group_name,
+	.get_group_pins = spear_pinctrl_get_group_pins,
+	.pin_dbg_show = spear_pinctrl_pin_dbg_show,
+	.dt_node_to_map = spear_pinctrl_dt_node_to_map,
+	.dt_free_map = spear_pinctrl_dt_free_map,
+};
+
+static int spear_pinctrl_get_funcs_count(struct pinctrl_dev *pctldev)
+{
+	struct spear_pmx *pmx = pinctrl_dev_get_drvdata(pctldev);
+
+	return pmx->machdata->nfunctions;
+}
+
+static const char *spear_pinctrl_get_func_name(struct pinctrl_dev *pctldev,
+		unsigned function)
+{
+	struct spear_pmx *pmx = pinctrl_dev_get_drvdata(pctldev);
+
+	return pmx->machdata->functions[function]->name;
+}
+
+static int spear_pinctrl_get_func_groups(struct pinctrl_dev *pctldev,
+		unsigned function, const char *const **groups,
+		unsigned * const ngroups)
+{
+	struct spear_pmx *pmx = pinctrl_dev_get_drvdata(pctldev);
+
+	*groups = pmx->machdata->functions[function]->groups;
+	*ngroups = pmx->machdata->functions[function]->ngroups;
+
+	return 0;
+}
+
+static int spear_pinctrl_endisable(struct pinctrl_dev *pctldev,
+		unsigned function, unsigned group, bool enable)
+{
+	struct spear_pmx *pmx = pinctrl_dev_get_drvdata(pctldev);
+	const struct spear_pingroup *pgroup;
+	const struct spear_modemux *modemux;
+	struct spear_muxreg *muxreg;
+	u32 val, temp;
+	int i, j;
+	bool found = false;
+
+	pgroup = pmx->machdata->groups[group];
+
+	for (i = 0; i < pgroup->nmodemuxs; i++) {
+		modemux = &pgroup->modemuxs[i];
+
+		/* SoC have any modes */
+		if (pmx->machdata->modes_supported) {
+			if (!(pmx->machdata->mode & modemux->modes))
+				continue;
+		}
+
+		found = true;
+		for (j = 0; j < modemux->nmuxregs; j++) {
+			muxreg = &modemux->muxregs[j];
+
+			val = pmx_readl(pmx, muxreg->reg);
+			val &= ~muxreg->mask;
+
+			if (enable)
+				temp = muxreg->val;
+			else
+				temp = ~muxreg->val;
+
+			val |= temp;
+			pmx_writel(pmx, val, muxreg->reg);
+		}
+	}
+
+	if (!found) {
+		dev_err(pmx->dev, "pinmux group: %s not supported\n",
+				pgroup->name);
+		return -ENODEV;
+	}
+
+	return 0;
+}
+
+static int spear_pinctrl_enable(struct pinctrl_dev *pctldev, unsigned function,
+		unsigned group)
+{
+	return spear_pinctrl_endisable(pctldev, function, group, true);
+}
+
+static void spear_pinctrl_disable(struct pinctrl_dev *pctldev,
+		unsigned function, unsigned group)
+{
+	spear_pinctrl_endisable(pctldev, function, group, false);
+}
+
+static struct pinmux_ops spear_pinmux_ops = {
+	.get_functions_count = spear_pinctrl_get_funcs_count,
+	.get_function_name = spear_pinctrl_get_func_name,
+	.get_function_groups = spear_pinctrl_get_func_groups,
+	.enable = spear_pinctrl_enable,
+	.disable = spear_pinctrl_disable,
+};
+
+static struct pinctrl_desc spear_pinctrl_desc = {
+	.name = DRIVER_NAME,
+	.pctlops = &spear_pinctrl_ops,
+	.pmxops = &spear_pinmux_ops,
+	.owner = THIS_MODULE,
+};
+
+int __devinit spear_pinctrl_probe(struct platform_device *pdev,
+		struct spear_pinctrl_machdata *machdata)
+{
+	struct device_node *np = pdev->dev.of_node;
+	struct resource *res;
+	struct spear_pmx *pmx;
+
+	if (!machdata)
+		return -ENODEV;
+
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	if (!res)
+		return -EINVAL;
+
+	pmx = devm_kzalloc(&pdev->dev, sizeof(*pmx), GFP_KERNEL);
+	if (!pmx) {
+		dev_err(&pdev->dev, "Can't alloc spear_pmx\n");
+		return -ENOMEM;
+	}
+
+	pmx->vbase = devm_ioremap(&pdev->dev, res->start, resource_size(res));
+	if (!pmx->vbase) {
+		dev_err(&pdev->dev, "Couldn't ioremap at index 0\n");
+		return -ENODEV;
+	}
+
+	pmx->dev = &pdev->dev;
+	pmx->machdata = machdata;
+
+	/* configure mode, if supported by SoC */
+	if (machdata->modes_supported) {
+		int mode = 0;
+
+		if (of_property_read_u32(np, "st,pinmux-mode", &mode)) {
+			dev_err(&pdev->dev, "OF: pinmux mode not passed\n");
+			return -EINVAL;
+		}
+
+		if (set_mode(pmx, mode)) {
+			dev_err(&pdev->dev, "OF: Couldn't configure mode: %x\n",
+					mode);
+			return -EINVAL;
+		}
+	}
+
+	platform_set_drvdata(pdev, pmx);
+
+	spear_pinctrl_desc.pins = machdata->pins;
+	spear_pinctrl_desc.npins = machdata->npins;
+
+	pmx->pctl = pinctrl_register(&spear_pinctrl_desc, &pdev->dev, pmx);
+	if (IS_ERR(pmx->pctl)) {
+		dev_err(&pdev->dev, "Couldn't register pinctrl driver\n");
+		return PTR_ERR(pmx->pctl);
+	}
+
+	return 0;
+}
+
+int __devexit spear_pinctrl_remove(struct platform_device *pdev)
+{
+	struct spear_pmx *pmx = platform_get_drvdata(pdev);
+
+	pinctrl_unregister(pmx->pctl);
+
+	return 0;
+}
diff --git a/drivers/pinctrl/spear/pinctrl-spear.h b/drivers/pinctrl/spear/pinctrl-spear.h
new file mode 100644
index 00000000000..9155783bb47
--- /dev/null
+++ b/drivers/pinctrl/spear/pinctrl-spear.h
@@ -0,0 +1,393 @@
+/*
+ * Driver header file for the ST Microelectronics SPEAr pinmux
+ *
+ * Copyright (C) 2012 ST Microelectronics
+ * Viresh Kumar <viresh.kumar@st.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.
+ */
+
+#ifndef __PINMUX_SPEAR_H__
+#define __PINMUX_SPEAR_H__
+
+#include <linux/pinctrl/pinctrl.h>
+#include <linux/types.h>
+
+struct platform_device;
+struct device;
+
+/**
+ * struct spear_pmx_mode - SPEAr pmx mode
+ * @name: name of pmx mode
+ * @mode: mode id
+ * @reg: register for configuring this mode
+ * @mask: mask of this mode in reg
+ * @val: val to be configured at reg after doing (val & mask)
+ */
+struct spear_pmx_mode {
+	const char *const name;
+	u16 mode;
+	u16 reg;
+	u16 mask;
+	u32 val;
+};
+
+/**
+ * struct spear_muxreg - SPEAr mux reg configuration
+ * @reg: register offset
+ * @mask: mask bits
+ * @val: val to be written on mask bits
+ */
+struct spear_muxreg {
+	u16 reg;
+	u32 mask;
+	u32 val;
+};
+
+/**
+ * struct spear_modemux - SPEAr mode mux configuration
+ * @modes: mode ids supported by this group of muxregs
+ * @nmuxregs: number of muxreg configurations to be done for modes
+ * @muxregs: array of muxreg configurations to be done for modes
+ */
+struct spear_modemux {
+	u16 modes;
+	u8 nmuxregs;
+	struct spear_muxreg *muxregs;
+};
+
+/**
+ * struct spear_pingroup - SPEAr pin group configurations
+ * @name: name of pin group
+ * @pins: array containing pin numbers
+ * @npins: size of pins array
+ * @modemuxs: array of modemux configurations for this pin group
+ * @nmodemuxs: size of array modemuxs
+ *
+ * A representation of a group of pins in the SPEAr pin controller. Each group
+ * allows some parameter or parameters to be configured.
+ */
+struct spear_pingroup {
+	const char *name;
+	const unsigned *pins;
+	unsigned npins;
+	struct spear_modemux *modemuxs;
+	unsigned nmodemuxs;
+};
+
+/**
+ * struct spear_function - SPEAr pinctrl mux function
+ * @name: The name of the function, exported to pinctrl core.
+ * @groups: An array of pin groups that may select this function.
+ * @ngroups: The number of entries in @groups.
+ */
+struct spear_function {
+	const char *name;
+	const char *const *groups;
+	unsigned ngroups;
+};
+
+/**
+ * struct spear_pinctrl_machdata - SPEAr pin controller machine driver
+ *	configuration
+ * @pins: An array describing all pins the pin controller affects.
+ *	All pins which are also GPIOs must be listed first within the *array,
+ *	and be numbered identically to the GPIO controller's *numbering.
+ * @npins: The numbmer of entries in @pins.
+ * @functions: An array describing all mux functions the SoC supports.
+ * @nfunctions: The numbmer of entries in @functions.
+ * @groups: An array describing all pin groups the pin SoC supports.
+ * @ngroups: The numbmer of entries in @groups.
+ *
+ * @modes_supported: Does SoC support modes
+ * @mode: mode configured from probe
+ * @pmx_modes: array of modes supported by SoC
+ * @npmx_modes: number of entries in pmx_modes.
+ */
+struct spear_pinctrl_machdata {
+	const struct pinctrl_pin_desc *pins;
+	unsigned npins;
+	struct spear_function **functions;
+	unsigned nfunctions;
+	struct spear_pingroup **groups;
+	unsigned ngroups;
+
+	bool modes_supported;
+	u16 mode;
+	struct spear_pmx_mode **pmx_modes;
+	unsigned npmx_modes;
+};
+
+/**
+ * struct spear_pmx - SPEAr pinctrl mux
+ * @dev: pointer to struct dev of platform_device registered
+ * @pctl: pointer to struct pinctrl_dev
+ * @machdata: pointer to SoC or machine specific structure
+ * @vbase: virtual base address of pinmux controller
+ */
+struct spear_pmx {
+	struct device *dev;
+	struct pinctrl_dev *pctl;
+	struct spear_pinctrl_machdata *machdata;
+	void __iomem *vbase;
+};
+
+/* exported routines */
+void __devinit pmx_init_addr(struct spear_pinctrl_machdata *machdata, u16 reg);
+int __devinit spear_pinctrl_probe(struct platform_device *pdev,
+		struct spear_pinctrl_machdata *machdata);
+int __devexit spear_pinctrl_remove(struct platform_device *pdev);
+
+#define SPEAR_PIN_0_TO_101		\
+	PINCTRL_PIN(0, "PLGPIO0"),	\
+	PINCTRL_PIN(1, "PLGPIO1"),	\
+	PINCTRL_PIN(2, "PLGPIO2"),	\
+	PINCTRL_PIN(3, "PLGPIO3"),	\
+	PINCTRL_PIN(4, "PLGPIO4"),	\
+	PINCTRL_PIN(5, "PLGPIO5"),	\
+	PINCTRL_PIN(6, "PLGPIO6"),	\
+	PINCTRL_PIN(7, "PLGPIO7"),	\
+	PINCTRL_PIN(8, "PLGPIO8"),	\
+	PINCTRL_PIN(9, "PLGPIO9"),	\
+	PINCTRL_PIN(10, "PLGPIO10"),	\
+	PINCTRL_PIN(11, "PLGPIO11"),	\
+	PINCTRL_PIN(12, "PLGPIO12"),	\
+	PINCTRL_PIN(13, "PLGPIO13"),	\
+	PINCTRL_PIN(14, "PLGPIO14"),	\
+	PINCTRL_PIN(15, "PLGPIO15"),	\
+	PINCTRL_PIN(16, "PLGPIO16"),	\
+	PINCTRL_PIN(17, "PLGPIO17"),	\
+	PINCTRL_PIN(18, "PLGPIO18"),	\
+	PINCTRL_PIN(19, "PLGPIO19"),	\
+	PINCTRL_PIN(20, "PLGPIO20"),	\
+	PINCTRL_PIN(21, "PLGPIO21"),	\
+	PINCTRL_PIN(22, "PLGPIO22"),	\
+	PINCTRL_PIN(23, "PLGPIO23"),	\
+	PINCTRL_PIN(24, "PLGPIO24"),	\
+	PINCTRL_PIN(25, "PLGPIO25"),	\
+	PINCTRL_PIN(26, "PLGPIO26"),	\
+	PINCTRL_PIN(27, "PLGPIO27"),	\
+	PINCTRL_PIN(28, "PLGPIO28"),	\
+	PINCTRL_PIN(29, "PLGPIO29"),	\
+	PINCTRL_PIN(30, "PLGPIO30"),	\
+	PINCTRL_PIN(31, "PLGPIO31"),	\
+	PINCTRL_PIN(32, "PLGPIO32"),	\
+	PINCTRL_PIN(33, "PLGPIO33"),	\
+	PINCTRL_PIN(34, "PLGPIO34"),	\
+	PINCTRL_PIN(35, "PLGPIO35"),	\
+	PINCTRL_PIN(36, "PLGPIO36"),	\
+	PINCTRL_PIN(37, "PLGPIO37"),	\
+	PINCTRL_PIN(38, "PLGPIO38"),	\
+	PINCTRL_PIN(39, "PLGPIO39"),	\
+	PINCTRL_PIN(40, "PLGPIO40"),	\
+	PINCTRL_PIN(41, "PLGPIO41"),	\
+	PINCTRL_PIN(42, "PLGPIO42"),	\
+	PINCTRL_PIN(43, "PLGPIO43"),	\
+	PINCTRL_PIN(44, "PLGPIO44"),	\
+	PINCTRL_PIN(45, "PLGPIO45"),	\
+	PINCTRL_PIN(46, "PLGPIO46"),	\
+	PINCTRL_PIN(47, "PLGPIO47"),	\
+	PINCTRL_PIN(48, "PLGPIO48"),	\
+	PINCTRL_PIN(49, "PLGPIO49"),	\
+	PINCTRL_PIN(50, "PLGPIO50"),	\
+	PINCTRL_PIN(51, "PLGPIO51"),	\
+	PINCTRL_PIN(52, "PLGPIO52"),	\
+	PINCTRL_PIN(53, "PLGPIO53"),	\
+	PINCTRL_PIN(54, "PLGPIO54"),	\
+	PINCTRL_PIN(55, "PLGPIO55"),	\
+	PINCTRL_PIN(56, "PLGPIO56"),	\
+	PINCTRL_PIN(57, "PLGPIO57"),	\
+	PINCTRL_PIN(58, "PLGPIO58"),	\
+	PINCTRL_PIN(59, "PLGPIO59"),	\
+	PINCTRL_PIN(60, "PLGPIO60"),	\
+	PINCTRL_PIN(61, "PLGPIO61"),	\
+	PINCTRL_PIN(62, "PLGPIO62"),	\
+	PINCTRL_PIN(63, "PLGPIO63"),	\
+	PINCTRL_PIN(64, "PLGPIO64"),	\
+	PINCTRL_PIN(65, "PLGPIO65"),	\
+	PINCTRL_PIN(66, "PLGPIO66"),	\
+	PINCTRL_PIN(67, "PLGPIO67"),	\
+	PINCTRL_PIN(68, "PLGPIO68"),	\
+	PINCTRL_PIN(69, "PLGPIO69"),	\
+	PINCTRL_PIN(70, "PLGPIO70"),	\
+	PINCTRL_PIN(71, "PLGPIO71"),	\
+	PINCTRL_PIN(72, "PLGPIO72"),	\
+	PINCTRL_PIN(73, "PLGPIO73"),	\
+	PINCTRL_PIN(74, "PLGPIO74"),	\
+	PINCTRL_PIN(75, "PLGPIO75"),	\
+	PINCTRL_PIN(76, "PLGPIO76"),	\
+	PINCTRL_PIN(77, "PLGPIO77"),	\
+	PINCTRL_PIN(78, "PLGPIO78"),	\
+	PINCTRL_PIN(79, "PLGPIO79"),	\
+	PINCTRL_PIN(80, "PLGPIO80"),	\
+	PINCTRL_PIN(81, "PLGPIO81"),	\
+	PINCTRL_PIN(82, "PLGPIO82"),	\
+	PINCTRL_PIN(83, "PLGPIO83"),	\
+	PINCTRL_PIN(84, "PLGPIO84"),	\
+	PINCTRL_PIN(85, "PLGPIO85"),	\
+	PINCTRL_PIN(86, "PLGPIO86"),	\
+	PINCTRL_PIN(87, "PLGPIO87"),	\
+	PINCTRL_PIN(88, "PLGPIO88"),	\
+	PINCTRL_PIN(89, "PLGPIO89"),	\
+	PINCTRL_PIN(90, "PLGPIO90"),	\
+	PINCTRL_PIN(91, "PLGPIO91"),	\
+	PINCTRL_PIN(92, "PLGPIO92"),	\
+	PINCTRL_PIN(93, "PLGPIO93"),	\
+	PINCTRL_PIN(94, "PLGPIO94"),	\
+	PINCTRL_PIN(95, "PLGPIO95"),	\
+	PINCTRL_PIN(96, "PLGPIO96"),	\
+	PINCTRL_PIN(97, "PLGPIO97"),	\
+	PINCTRL_PIN(98, "PLGPIO98"),	\
+	PINCTRL_PIN(99, "PLGPIO99"),	\
+	PINCTRL_PIN(100, "PLGPIO100"),	\
+	PINCTRL_PIN(101, "PLGPIO101")
+
+#define SPEAR_PIN_102_TO_245		\
+	PINCTRL_PIN(102, "PLGPIO102"),	\
+	PINCTRL_PIN(103, "PLGPIO103"),	\
+	PINCTRL_PIN(104, "PLGPIO104"),	\
+	PINCTRL_PIN(105, "PLGPIO105"),	\
+	PINCTRL_PIN(106, "PLGPIO106"),	\
+	PINCTRL_PIN(107, "PLGPIO107"),	\
+	PINCTRL_PIN(108, "PLGPIO108"),	\
+	PINCTRL_PIN(109, "PLGPIO109"),	\
+	PINCTRL_PIN(110, "PLGPIO110"),	\
+	PINCTRL_PIN(111, "PLGPIO111"),	\
+	PINCTRL_PIN(112, "PLGPIO112"),	\
+	PINCTRL_PIN(113, "PLGPIO113"),	\
+	PINCTRL_PIN(114, "PLGPIO114"),	\
+	PINCTRL_PIN(115, "PLGPIO115"),	\
+	PINCTRL_PIN(116, "PLGPIO116"),	\
+	PINCTRL_PIN(117, "PLGPIO117"),	\
+	PINCTRL_PIN(118, "PLGPIO118"),	\
+	PINCTRL_PIN(119, "PLGPIO119"),	\
+	PINCTRL_PIN(120, "PLGPIO120"),	\
+	PINCTRL_PIN(121, "PLGPIO121"),	\
+	PINCTRL_PIN(122, "PLGPIO122"),	\
+	PINCTRL_PIN(123, "PLGPIO123"),	\
+	PINCTRL_PIN(124, "PLGPIO124"),	\
+	PINCTRL_PIN(125, "PLGPIO125"),	\
+	PINCTRL_PIN(126, "PLGPIO126"),	\
+	PINCTRL_PIN(127, "PLGPIO127"),	\
+	PINCTRL_PIN(128, "PLGPIO128"),	\
+	PINCTRL_PIN(129, "PLGPIO129"),	\
+	PINCTRL_PIN(130, "PLGPIO130"),	\
+	PINCTRL_PIN(131, "PLGPIO131"),	\
+	PINCTRL_PIN(132, "PLGPIO132"),	\
+	PINCTRL_PIN(133, "PLGPIO133"),	\
+	PINCTRL_PIN(134, "PLGPIO134"),	\
+	PINCTRL_PIN(135, "PLGPIO135"),	\
+	PINCTRL_PIN(136, "PLGPIO136"),	\
+	PINCTRL_PIN(137, "PLGPIO137"),	\
+	PINCTRL_PIN(138, "PLGPIO138"),	\
+	PINCTRL_PIN(139, "PLGPIO139"),	\
+	PINCTRL_PIN(140, "PLGPIO140"),	\
+	PINCTRL_PIN(141, "PLGPIO141"),	\
+	PINCTRL_PIN(142, "PLGPIO142"),	\
+	PINCTRL_PIN(143, "PLGPIO143"),	\
+	PINCTRL_PIN(144, "PLGPIO144"),	\
+	PINCTRL_PIN(145, "PLGPIO145"),	\
+	PINCTRL_PIN(146, "PLGPIO146"),	\
+	PINCTRL_PIN(147, "PLGPIO147"),	\
+	PINCTRL_PIN(148, "PLGPIO148"),	\
+	PINCTRL_PIN(149, "PLGPIO149"),	\
+	PINCTRL_PIN(150, "PLGPIO150"),	\
+	PINCTRL_PIN(151, "PLGPIO151"),	\
+	PINCTRL_PIN(152, "PLGPIO152"),	\
+	PINCTRL_PIN(153, "PLGPIO153"),	\
+	PINCTRL_PIN(154, "PLGPIO154"),	\
+	PINCTRL_PIN(155, "PLGPIO155"),	\
+	PINCTRL_PIN(156, "PLGPIO156"),	\
+	PINCTRL_PIN(157, "PLGPIO157"),	\
+	PINCTRL_PIN(158, "PLGPIO158"),	\
+	PINCTRL_PIN(159, "PLGPIO159"),	\
+	PINCTRL_PIN(160, "PLGPIO160"),	\
+	PINCTRL_PIN(161, "PLGPIO161"),	\
+	PINCTRL_PIN(162, "PLGPIO162"),	\
+	PINCTRL_PIN(163, "PLGPIO163"),	\
+	PINCTRL_PIN(164, "PLGPIO164"),	\
+	PINCTRL_PIN(165, "PLGPIO165"),	\
+	PINCTRL_PIN(166, "PLGPIO166"),	\
+	PINCTRL_PIN(167, "PLGPIO167"),	\
+	PINCTRL_PIN(168, "PLGPIO168"),	\
+	PINCTRL_PIN(169, "PLGPIO169"),	\
+	PINCTRL_PIN(170, "PLGPIO170"),	\
+	PINCTRL_PIN(171, "PLGPIO171"),	\
+	PINCTRL_PIN(172, "PLGPIO172"),	\
+	PINCTRL_PIN(173, "PLGPIO173"),	\
+	PINCTRL_PIN(174, "PLGPIO174"),	\
+	PINCTRL_PIN(175, "PLGPIO175"),	\
+	PINCTRL_PIN(176, "PLGPIO176"),	\
+	PINCTRL_PIN(177, "PLGPIO177"),	\
+	PINCTRL_PIN(178, "PLGPIO178"),	\
+	PINCTRL_PIN(179, "PLGPIO179"),	\
+	PINCTRL_PIN(180, "PLGPIO180"),	\
+	PINCTRL_PIN(181, "PLGPIO181"),	\
+	PINCTRL_PIN(182, "PLGPIO182"),	\
+	PINCTRL_PIN(183, "PLGPIO183"),	\
+	PINCTRL_PIN(184, "PLGPIO184"),	\
+	PINCTRL_PIN(185, "PLGPIO185"),	\
+	PINCTRL_PIN(186, "PLGPIO186"),	\
+	PINCTRL_PIN(187, "PLGPIO187"),	\
+	PINCTRL_PIN(188, "PLGPIO188"),	\
+	PINCTRL_PIN(189, "PLGPIO189"),	\
+	PINCTRL_PIN(190, "PLGPIO190"),	\
+	PINCTRL_PIN(191, "PLGPIO191"),	\
+	PINCTRL_PIN(192, "PLGPIO192"),	\
+	PINCTRL_PIN(193, "PLGPIO193"),	\
+	PINCTRL_PIN(194, "PLGPIO194"),	\
+	PINCTRL_PIN(195, "PLGPIO195"),	\
+	PINCTRL_PIN(196, "PLGPIO196"),	\
+	PINCTRL_PIN(197, "PLGPIO197"),	\
+	PINCTRL_PIN(198, "PLGPIO198"),	\
+	PINCTRL_PIN(199, "PLGPIO199"),	\
+	PINCTRL_PIN(200, "PLGPIO200"),	\
+	PINCTRL_PIN(201, "PLGPIO201"),	\
+	PINCTRL_PIN(202, "PLGPIO202"),	\
+	PINCTRL_PIN(203, "PLGPIO203"),	\
+	PINCTRL_PIN(204, "PLGPIO204"),	\
+	PINCTRL_PIN(205, "PLGPIO205"),	\
+	PINCTRL_PIN(206, "PLGPIO206"),	\
+	PINCTRL_PIN(207, "PLGPIO207"),	\
+	PINCTRL_PIN(208, "PLGPIO208"),	\
+	PINCTRL_PIN(209, "PLGPIO209"),	\
+	PINCTRL_PIN(210, "PLGPIO210"),	\
+	PINCTRL_PIN(211, "PLGPIO211"),	\
+	PINCTRL_PIN(212, "PLGPIO212"),	\
+	PINCTRL_PIN(213, "PLGPIO213"),	\
+	PINCTRL_PIN(214, "PLGPIO214"),	\
+	PINCTRL_PIN(215, "PLGPIO215"),	\
+	PINCTRL_PIN(216, "PLGPIO216"),	\
+	PINCTRL_PIN(217, "PLGPIO217"),	\
+	PINCTRL_PIN(218, "PLGPIO218"),	\
+	PINCTRL_PIN(219, "PLGPIO219"),	\
+	PINCTRL_PIN(220, "PLGPIO220"),	\
+	PINCTRL_PIN(221, "PLGPIO221"),	\
+	PINCTRL_PIN(222, "PLGPIO222"),	\
+	PINCTRL_PIN(223, "PLGPIO223"),	\
+	PINCTRL_PIN(224, "PLGPIO224"),	\
+	PINCTRL_PIN(225, "PLGPIO225"),	\
+	PINCTRL_PIN(226, "PLGPIO226"),	\
+	PINCTRL_PIN(227, "PLGPIO227"),	\
+	PINCTRL_PIN(228, "PLGPIO228"),	\
+	PINCTRL_PIN(229, "PLGPIO229"),	\
+	PINCTRL_PIN(230, "PLGPIO230"),	\
+	PINCTRL_PIN(231, "PLGPIO231"),	\
+	PINCTRL_PIN(232, "PLGPIO232"),	\
+	PINCTRL_PIN(233, "PLGPIO233"),	\
+	PINCTRL_PIN(234, "PLGPIO234"),	\
+	PINCTRL_PIN(235, "PLGPIO235"),	\
+	PINCTRL_PIN(236, "PLGPIO236"),	\
+	PINCTRL_PIN(237, "PLGPIO237"),	\
+	PINCTRL_PIN(238, "PLGPIO238"),	\
+	PINCTRL_PIN(239, "PLGPIO239"),	\
+	PINCTRL_PIN(240, "PLGPIO240"),	\
+	PINCTRL_PIN(241, "PLGPIO241"),	\
+	PINCTRL_PIN(242, "PLGPIO242"),	\
+	PINCTRL_PIN(243, "PLGPIO243"),	\
+	PINCTRL_PIN(244, "PLGPIO244"),	\
+	PINCTRL_PIN(245, "PLGPIO245")
+
+#endif /* __PINMUX_SPEAR_H__ */
diff --git a/drivers/pinctrl/spear/pinctrl-spear1310.c b/drivers/pinctrl/spear/pinctrl-spear1310.c
new file mode 100644
index 00000000000..fff168be7f0
--- /dev/null
+++ b/drivers/pinctrl/spear/pinctrl-spear1310.c
@@ -0,0 +1,2198 @@
+/*
+ * Driver for the ST Microelectronics SPEAr1310 pinmux
+ *
+ * Copyright (C) 2012 ST Microelectronics
+ * Viresh Kumar <viresh.kumar@st.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/err.h>
+#include <linux/init.h>
+#include <linux/module.h>
+#include <linux/of_device.h>
+#include <linux/platform_device.h>
+#include "pinctrl-spear.h"
+
+#define DRIVER_NAME "spear1310-pinmux"
+
+/* pins */
+static const struct pinctrl_pin_desc spear1310_pins[] = {
+	SPEAR_PIN_0_TO_101,
+	SPEAR_PIN_102_TO_245,
+};
+
+/* registers */
+#define PERIP_CFG					0x32C
+	#define MCIF_SEL_SHIFT				3
+	#define MCIF_SEL_SD				(0x1 << MCIF_SEL_SHIFT)
+	#define MCIF_SEL_CF				(0x2 << MCIF_SEL_SHIFT)
+	#define MCIF_SEL_XD				(0x3 << MCIF_SEL_SHIFT)
+	#define MCIF_SEL_MASK				(0x3 << MCIF_SEL_SHIFT)
+
+#define PCIE_SATA_CFG					0x3A4
+	#define PCIE_SATA2_SEL_PCIE			(0 << 31)
+	#define PCIE_SATA1_SEL_PCIE			(0 << 30)
+	#define PCIE_SATA0_SEL_PCIE			(0 << 29)
+	#define PCIE_SATA2_SEL_SATA			(1 << 31)
+	#define PCIE_SATA1_SEL_SATA			(1 << 30)
+	#define PCIE_SATA0_SEL_SATA			(1 << 29)
+	#define SATA2_CFG_TX_CLK_EN			(1 << 27)
+	#define SATA2_CFG_RX_CLK_EN			(1 << 26)
+	#define SATA2_CFG_POWERUP_RESET			(1 << 25)
+	#define SATA2_CFG_PM_CLK_EN			(1 << 24)
+	#define SATA1_CFG_TX_CLK_EN			(1 << 23)
+	#define SATA1_CFG_RX_CLK_EN			(1 << 22)
+	#define SATA1_CFG_POWERUP_RESET			(1 << 21)
+	#define SATA1_CFG_PM_CLK_EN			(1 << 20)
+	#define SATA0_CFG_TX_CLK_EN			(1 << 19)
+	#define SATA0_CFG_RX_CLK_EN			(1 << 18)
+	#define SATA0_CFG_POWERUP_RESET			(1 << 17)
+	#define SATA0_CFG_PM_CLK_EN			(1 << 16)
+	#define PCIE2_CFG_DEVICE_PRESENT		(1 << 11)
+	#define PCIE2_CFG_POWERUP_RESET			(1 << 10)
+	#define PCIE2_CFG_CORE_CLK_EN			(1 << 9)
+	#define PCIE2_CFG_AUX_CLK_EN			(1 << 8)
+	#define PCIE1_CFG_DEVICE_PRESENT		(1 << 7)
+	#define PCIE1_CFG_POWERUP_RESET			(1 << 6)
+	#define PCIE1_CFG_CORE_CLK_EN			(1 << 5)
+	#define PCIE1_CFG_AUX_CLK_EN			(1 << 4)
+	#define PCIE0_CFG_DEVICE_PRESENT		(1 << 3)
+	#define PCIE0_CFG_POWERUP_RESET			(1 << 2)
+	#define PCIE0_CFG_CORE_CLK_EN			(1 << 1)
+	#define PCIE0_CFG_AUX_CLK_EN			(1 << 0)
+
+#define PAD_FUNCTION_EN_0				0x650
+	#define PMX_UART0_MASK				(1 << 1)
+	#define PMX_I2C0_MASK				(1 << 2)
+	#define PMX_I2S0_MASK				(1 << 3)
+	#define PMX_SSP0_MASK				(1 << 4)
+	#define PMX_CLCD1_MASK				(1 << 5)
+	#define PMX_EGPIO00_MASK			(1 << 6)
+	#define PMX_EGPIO01_MASK			(1 << 7)
+	#define PMX_EGPIO02_MASK			(1 << 8)
+	#define PMX_EGPIO03_MASK			(1 << 9)
+	#define PMX_EGPIO04_MASK			(1 << 10)
+	#define PMX_EGPIO05_MASK			(1 << 11)
+	#define PMX_EGPIO06_MASK			(1 << 12)
+	#define PMX_EGPIO07_MASK			(1 << 13)
+	#define PMX_EGPIO08_MASK			(1 << 14)
+	#define PMX_EGPIO09_MASK			(1 << 15)
+	#define PMX_SMI_MASK				(1 << 16)
+	#define PMX_NAND8_MASK				(1 << 17)
+	#define PMX_GMIICLK_MASK			(1 << 18)
+	#define PMX_GMIICOL_CRS_XFERER_MIITXCLK_MASK	(1 << 19)
+	#define PMX_RXCLK_RDV_TXEN_D03_MASK		(1 << 20)
+	#define PMX_GMIID47_MASK			(1 << 21)
+	#define PMX_MDC_MDIO_MASK			(1 << 22)
+	#define PMX_MCI_DATA8_15_MASK			(1 << 23)
+	#define PMX_NFAD23_MASK				(1 << 24)
+	#define PMX_NFAD24_MASK				(1 << 25)
+	#define PMX_NFAD25_MASK				(1 << 26)
+	#define PMX_NFCE3_MASK				(1 << 27)
+	#define PMX_NFWPRT3_MASK			(1 << 28)
+	#define PMX_NFRSTPWDWN0_MASK			(1 << 29)
+	#define PMX_NFRSTPWDWN1_MASK			(1 << 30)
+	#define PMX_NFRSTPWDWN2_MASK			(1 << 31)
+
+#define PAD_FUNCTION_EN_1				0x654
+	#define PMX_NFRSTPWDWN3_MASK			(1 << 0)
+	#define PMX_SMINCS2_MASK			(1 << 1)
+	#define PMX_SMINCS3_MASK			(1 << 2)
+	#define PMX_CLCD2_MASK				(1 << 3)
+	#define PMX_KBD_ROWCOL68_MASK			(1 << 4)
+	#define PMX_EGPIO10_MASK			(1 << 5)
+	#define PMX_EGPIO11_MASK			(1 << 6)
+	#define PMX_EGPIO12_MASK			(1 << 7)
+	#define PMX_EGPIO13_MASK			(1 << 8)
+	#define PMX_EGPIO14_MASK			(1 << 9)
+	#define PMX_EGPIO15_MASK			(1 << 10)
+	#define PMX_UART0_MODEM_MASK			(1 << 11)
+	#define PMX_GPT0_TMR0_MASK			(1 << 12)
+	#define PMX_GPT0_TMR1_MASK			(1 << 13)
+	#define PMX_GPT1_TMR0_MASK			(1 << 14)
+	#define PMX_GPT1_TMR1_MASK			(1 << 15)
+	#define PMX_I2S1_MASK				(1 << 16)
+	#define PMX_KBD_ROWCOL25_MASK			(1 << 17)
+	#define PMX_NFIO8_15_MASK			(1 << 18)
+	#define PMX_KBD_COL1_MASK			(1 << 19)
+	#define PMX_NFCE1_MASK				(1 << 20)
+	#define PMX_KBD_COL0_MASK			(1 << 21)
+	#define PMX_NFCE2_MASK				(1 << 22)
+	#define PMX_KBD_ROW1_MASK			(1 << 23)
+	#define PMX_NFWPRT1_MASK			(1 << 24)
+	#define PMX_KBD_ROW0_MASK			(1 << 25)
+	#define PMX_NFWPRT2_MASK			(1 << 26)
+	#define PMX_MCIDATA0_MASK			(1 << 27)
+	#define PMX_MCIDATA1_MASK			(1 << 28)
+	#define PMX_MCIDATA2_MASK			(1 << 29)
+	#define PMX_MCIDATA3_MASK			(1 << 30)
+	#define PMX_MCIDATA4_MASK			(1 << 31)
+
+#define PAD_FUNCTION_EN_2				0x658
+	#define PMX_MCIDATA5_MASK			(1 << 0)
+	#define PMX_MCIDATA6_MASK			(1 << 1)
+	#define PMX_MCIDATA7_MASK			(1 << 2)
+	#define PMX_MCIDATA1SD_MASK			(1 << 3)
+	#define PMX_MCIDATA2SD_MASK			(1 << 4)
+	#define PMX_MCIDATA3SD_MASK			(1 << 5)
+	#define PMX_MCIADDR0ALE_MASK			(1 << 6)
+	#define PMX_MCIADDR1CLECLK_MASK			(1 << 7)
+	#define PMX_MCIADDR2_MASK			(1 << 8)
+	#define PMX_MCICECF_MASK			(1 << 9)
+	#define PMX_MCICEXD_MASK			(1 << 10)
+	#define PMX_MCICESDMMC_MASK			(1 << 11)
+	#define PMX_MCICDCF1_MASK			(1 << 12)
+	#define PMX_MCICDCF2_MASK			(1 << 13)
+	#define PMX_MCICDXD_MASK			(1 << 14)
+	#define PMX_MCICDSDMMC_MASK			(1 << 15)
+	#define PMX_MCIDATADIR_MASK			(1 << 16)
+	#define PMX_MCIDMARQWP_MASK			(1 << 17)
+	#define PMX_MCIIORDRE_MASK			(1 << 18)
+	#define PMX_MCIIOWRWE_MASK			(1 << 19)
+	#define PMX_MCIRESETCF_MASK			(1 << 20)
+	#define PMX_MCICS0CE_MASK			(1 << 21)
+	#define PMX_MCICFINTR_MASK			(1 << 22)
+	#define PMX_MCIIORDY_MASK			(1 << 23)
+	#define PMX_MCICS1_MASK				(1 << 24)
+	#define PMX_MCIDMAACK_MASK			(1 << 25)
+	#define PMX_MCISDCMD_MASK			(1 << 26)
+	#define PMX_MCILEDS_MASK			(1 << 27)
+	#define PMX_TOUCH_XY_MASK			(1 << 28)
+	#define PMX_SSP0_CS0_MASK			(1 << 29)
+	#define PMX_SSP0_CS1_2_MASK			(1 << 30)
+
+/* combined macros */
+#define PMX_GMII_MASK		(PMX_GMIICLK_MASK |			\
+				PMX_GMIICOL_CRS_XFERER_MIITXCLK_MASK |	\
+				PMX_RXCLK_RDV_TXEN_D03_MASK |		\
+				PMX_GMIID47_MASK | PMX_MDC_MDIO_MASK)
+
+#define PMX_EGPIO_0_GRP_MASK	(PMX_EGPIO00_MASK | PMX_EGPIO01_MASK |	\
+				PMX_EGPIO02_MASK |			\
+				PMX_EGPIO03_MASK | PMX_EGPIO04_MASK |	\
+				PMX_EGPIO05_MASK | PMX_EGPIO06_MASK |	\
+				PMX_EGPIO07_MASK | PMX_EGPIO08_MASK |	\
+				PMX_EGPIO09_MASK)
+#define PMX_EGPIO_1_GRP_MASK	(PMX_EGPIO10_MASK | PMX_EGPIO11_MASK |	\
+				PMX_EGPIO12_MASK | PMX_EGPIO13_MASK |	\
+				PMX_EGPIO14_MASK | PMX_EGPIO15_MASK)
+
+#define PMX_KEYBOARD_6X6_MASK	(PMX_KBD_ROW0_MASK | PMX_KBD_ROW1_MASK | \
+				PMX_KBD_ROWCOL25_MASK | PMX_KBD_COL0_MASK | \
+				PMX_KBD_COL1_MASK)
+
+#define PMX_NAND8BIT_0_MASK	(PMX_NAND8_MASK | PMX_NFAD23_MASK |	\
+				PMX_NFAD24_MASK | PMX_NFAD25_MASK |	\
+				PMX_NFWPRT3_MASK | PMX_NFRSTPWDWN0_MASK | \
+				PMX_NFRSTPWDWN1_MASK | PMX_NFRSTPWDWN2_MASK | \
+				PMX_NFCE3_MASK)
+#define PMX_NAND8BIT_1_MASK	PMX_NFRSTPWDWN3_MASK
+
+#define PMX_NAND16BIT_1_MASK	(PMX_KBD_ROWCOL25_MASK | PMX_NFIO8_15_MASK)
+#define PMX_NAND_4CHIPS_MASK	(PMX_NFCE1_MASK | PMX_NFCE2_MASK |	\
+				PMX_NFWPRT1_MASK | PMX_NFWPRT2_MASK |	\
+				PMX_KBD_ROW0_MASK | PMX_KBD_ROW1_MASK |	\
+				PMX_KBD_COL0_MASK | PMX_KBD_COL1_MASK)
+
+#define PMX_MCIFALL_1_MASK	0xF8000000
+#define PMX_MCIFALL_2_MASK	0x0FFFFFFF
+
+#define PMX_PCI_REG1_MASK	(PMX_SMINCS2_MASK | PMX_SMINCS3_MASK |	\
+				PMX_CLCD2_MASK | PMX_KBD_ROWCOL68_MASK | \
+				PMX_EGPIO_1_GRP_MASK | PMX_GPT0_TMR0_MASK | \
+				PMX_GPT0_TMR1_MASK | PMX_GPT1_TMR0_MASK | \
+				PMX_GPT1_TMR1_MASK | PMX_I2S1_MASK |	\
+				PMX_NFCE2_MASK)
+#define PMX_PCI_REG2_MASK	(PMX_TOUCH_XY_MASK | PMX_SSP0_CS0_MASK | \
+				PMX_SSP0_CS1_2_MASK)
+
+#define PMX_SMII_0_1_2_MASK	(PMX_CLCD2_MASK | PMX_KBD_ROWCOL68_MASK)
+#define PMX_RGMII_REG0_MASK	(PMX_MCI_DATA8_15_MASK |		\
+				PMX_GMIICOL_CRS_XFERER_MIITXCLK_MASK |	\
+				PMX_GMIID47_MASK)
+#define PMX_RGMII_REG1_MASK	(PMX_KBD_ROWCOL68_MASK | PMX_EGPIO_1_GRP_MASK |\
+				PMX_KBD_ROW1_MASK | PMX_NFWPRT1_MASK |	\
+				PMX_KBD_ROW0_MASK | PMX_NFWPRT2_MASK)
+#define PMX_RGMII_REG2_MASK	(PMX_TOUCH_XY_MASK | PMX_SSP0_CS0_MASK | \
+				PMX_SSP0_CS1_2_MASK)
+
+#define PCIE_CFG_VAL(x)		(PCIE_SATA##x##_SEL_PCIE |	\
+				PCIE##x##_CFG_AUX_CLK_EN |	\
+				PCIE##x##_CFG_CORE_CLK_EN |	\
+				PCIE##x##_CFG_POWERUP_RESET |	\
+				PCIE##x##_CFG_DEVICE_PRESENT)
+#define SATA_CFG_VAL(x)		(PCIE_SATA##x##_SEL_SATA |	\
+				SATA##x##_CFG_PM_CLK_EN |	\
+				SATA##x##_CFG_POWERUP_RESET |	\
+				SATA##x##_CFG_RX_CLK_EN |	\
+				SATA##x##_CFG_TX_CLK_EN)
+
+/* Pad multiplexing for i2c0 device */
+static const unsigned i2c0_pins[] = { 102, 103 };
+static struct spear_muxreg i2c0_muxreg[] = {
+	{
+		.reg = PAD_FUNCTION_EN_0,
+		.mask = PMX_I2C0_MASK,
+		.val = PMX_I2C0_MASK,
+	},
+};
+
+static struct spear_modemux i2c0_modemux[] = {
+	{
+		.muxregs = i2c0_muxreg,
+		.nmuxregs = ARRAY_SIZE(i2c0_muxreg),
+	},
+};
+
+static struct spear_pingroup i2c0_pingroup = {
+	.name = "i2c0_grp",
+	.pins = i2c0_pins,
+	.npins = ARRAY_SIZE(i2c0_pins),
+	.modemuxs = i2c0_modemux,
+	.nmodemuxs = ARRAY_SIZE(i2c0_modemux),
+};
+
+static const char *const i2c0_grps[] = { "i2c0_grp" };
+static struct spear_function i2c0_function = {
+	.name = "i2c0",
+	.groups = i2c0_grps,
+	.ngroups = ARRAY_SIZE(i2c0_grps),
+};
+
+/* Pad multiplexing for ssp0 device */
+static const unsigned ssp0_pins[] = { 109, 110, 111, 112 };
+static struct spear_muxreg ssp0_muxreg[] = {
+	{
+		.reg = PAD_FUNCTION_EN_0,
+		.mask = PMX_SSP0_MASK,
+		.val = PMX_SSP0_MASK,
+	},
+};
+
+static struct spear_modemux ssp0_modemux[] = {
+	{
+		.muxregs = ssp0_muxreg,
+		.nmuxregs = ARRAY_SIZE(ssp0_muxreg),
+	},
+};
+
+static struct spear_pingroup ssp0_pingroup = {
+	.name = "ssp0_grp",
+	.pins = ssp0_pins,
+	.npins = ARRAY_SIZE(ssp0_pins),
+	.modemuxs = ssp0_modemux,
+	.nmodemuxs = ARRAY_SIZE(ssp0_modemux),
+};
+
+/* Pad multiplexing for ssp0_cs0 device */
+static const unsigned ssp0_cs0_pins[] = { 96 };
+static struct spear_muxreg ssp0_cs0_muxreg[] = {
+	{
+		.reg = PAD_FUNCTION_EN_2,
+		.mask = PMX_SSP0_CS0_MASK,
+		.val = PMX_SSP0_CS0_MASK,
+	},
+};
+
+static struct spear_modemux ssp0_cs0_modemux[] = {
+	{
+		.muxregs = ssp0_cs0_muxreg,
+		.nmuxregs = ARRAY_SIZE(ssp0_cs0_muxreg),
+	},
+};
+
+static struct spear_pingroup ssp0_cs0_pingroup = {
+	.name = "ssp0_cs0_grp",
+	.pins = ssp0_cs0_pins,
+	.npins = ARRAY_SIZE(ssp0_cs0_pins),
+	.modemuxs = ssp0_cs0_modemux,
+	.nmodemuxs = ARRAY_SIZE(ssp0_cs0_modemux),
+};
+
+/* ssp0_cs1_2 device */
+static const unsigned ssp0_cs1_2_pins[] = { 94, 95 };
+static struct spear_muxreg ssp0_cs1_2_muxreg[] = {
+	{
+		.reg = PAD_FUNCTION_EN_2,
+		.mask = PMX_SSP0_CS1_2_MASK,
+		.val = PMX_SSP0_CS1_2_MASK,
+	},
+};
+
+static struct spear_modemux ssp0_cs1_2_modemux[] = {
+	{
+		.muxregs = ssp0_cs1_2_muxreg,
+		.nmuxregs = ARRAY_SIZE(ssp0_cs1_2_muxreg),
+	},
+};
+
+static struct spear_pingroup ssp0_cs1_2_pingroup = {
+	.name = "ssp0_cs1_2_grp",
+	.pins = ssp0_cs1_2_pins,
+	.npins = ARRAY_SIZE(ssp0_cs1_2_pins),
+	.modemuxs = ssp0_cs1_2_modemux,
+	.nmodemuxs = ARRAY_SIZE(ssp0_cs1_2_modemux),
+};
+
+static const char *const ssp0_grps[] = { "ssp0_grp", "ssp0_cs0_grp",
+	"ssp0_cs1_2_grp" };
+static struct spear_function ssp0_function = {
+	.name = "ssp0",
+	.groups = ssp0_grps,
+	.ngroups = ARRAY_SIZE(ssp0_grps),
+};
+
+/* Pad multiplexing for i2s0 device */
+static const unsigned i2s0_pins[] = { 104, 105, 106, 107, 108 };
+static struct spear_muxreg i2s0_muxreg[] = {
+	{
+		.reg = PAD_FUNCTION_EN_0,
+		.mask = PMX_I2S0_MASK,
+		.val = PMX_I2S0_MASK,
+	},
+};
+
+static struct spear_modemux i2s0_modemux[] = {
+	{
+		.muxregs = i2s0_muxreg,
+		.nmuxregs = ARRAY_SIZE(i2s0_muxreg),
+	},
+};
+
+static struct spear_pingroup i2s0_pingroup = {
+	.name = "i2s0_grp",
+	.pins = i2s0_pins,
+	.npins = ARRAY_SIZE(i2s0_pins),
+	.modemuxs = i2s0_modemux,
+	.nmodemuxs = ARRAY_SIZE(i2s0_modemux),
+};
+
+static const char *const i2s0_grps[] = { "i2s0_grp" };
+static struct spear_function i2s0_function = {
+	.name = "i2s0",
+	.groups = i2s0_grps,
+	.ngroups = ARRAY_SIZE(i2s0_grps),
+};
+
+/* Pad multiplexing for i2s1 device */
+static const unsigned i2s1_pins[] = { 0, 1, 2, 3 };
+static struct spear_muxreg i2s1_muxreg[] = {
+	{
+		.reg = PAD_FUNCTION_EN_1,
+		.mask = PMX_I2S1_MASK,
+		.val = PMX_I2S1_MASK,
+	},
+};
+
+static struct spear_modemux i2s1_modemux[] = {
+	{
+		.muxregs = i2s1_muxreg,
+		.nmuxregs = ARRAY_SIZE(i2s1_muxreg),
+	},
+};
+
+static struct spear_pingroup i2s1_pingroup = {
+	.name = "i2s1_grp",
+	.pins = i2s1_pins,
+	.npins = ARRAY_SIZE(i2s1_pins),
+	.modemuxs = i2s1_modemux,
+	.nmodemuxs = ARRAY_SIZE(i2s1_modemux),
+};
+
+static const char *const i2s1_grps[] = { "i2s1_grp" };
+static struct spear_function i2s1_function = {
+	.name = "i2s1",
+	.groups = i2s1_grps,
+	.ngroups = ARRAY_SIZE(i2s1_grps),
+};
+
+/* Pad multiplexing for clcd device */
+static const unsigned clcd_pins[] = { 113, 114, 115, 116, 117, 118, 119, 120,
+	121, 122, 123, 124, 125, 126, 127, 128, 129, 130, 131, 132, 133, 134,
+	135, 136, 137, 138, 139, 140, 141, 142 };
+static struct spear_muxreg clcd_muxreg[] = {
+	{
+		.reg = PAD_FUNCTION_EN_0,
+		.mask = PMX_CLCD1_MASK,
+		.val = PMX_CLCD1_MASK,
+	},
+};
+
+static struct spear_modemux clcd_modemux[] = {
+	{
+		.muxregs = clcd_muxreg,
+		.nmuxregs = ARRAY_SIZE(clcd_muxreg),
+	},
+};
+
+static struct spear_pingroup clcd_pingroup = {
+	.name = "clcd_grp",
+	.pins = clcd_pins,
+	.npins = ARRAY_SIZE(clcd_pins),
+	.modemuxs = clcd_modemux,
+	.nmodemuxs = ARRAY_SIZE(clcd_modemux),
+};
+
+static const unsigned clcd_high_res_pins[] = { 30, 31, 32, 33, 34, 35, 36, 37,
+	38, 39, 40, 41, 42, 43, 44, 45, 46, 47, 48, 49, 50, 51, 52, 53 };
+static struct spear_muxreg clcd_high_res_muxreg[] = {
+	{
+		.reg = PAD_FUNCTION_EN_1,
+		.mask = PMX_CLCD2_MASK,
+		.val = PMX_CLCD2_MASK,
+	},
+};
+
+static struct spear_modemux clcd_high_res_modemux[] = {
+	{
+		.muxregs = clcd_high_res_muxreg,
+		.nmuxregs = ARRAY_SIZE(clcd_high_res_muxreg),
+	},
+};
+
+static struct spear_pingroup clcd_high_res_pingroup = {
+	.name = "clcd_high_res_grp",
+	.pins = clcd_high_res_pins,
+	.npins = ARRAY_SIZE(clcd_high_res_pins),
+	.modemuxs = clcd_high_res_modemux,
+	.nmodemuxs = ARRAY_SIZE(clcd_high_res_modemux),
+};
+
+static const char *const clcd_grps[] = { "clcd_grp", "clcd_high_res" };
+static struct spear_function clcd_function = {
+	.name = "clcd",
+	.groups = clcd_grps,
+	.ngroups = ARRAY_SIZE(clcd_grps),
+};
+
+static const unsigned arm_gpio_pins[] = { 18, 19, 20, 21, 22, 23, 143, 144, 145,
+	146, 147, 148, 149, 150, 151, 152 };
+static struct spear_muxreg arm_gpio_muxreg[] = {
+	{
+		.reg = PAD_FUNCTION_EN_0,
+		.mask = PMX_EGPIO_0_GRP_MASK,
+		.val = PMX_EGPIO_0_GRP_MASK,
+	}, {
+		.reg = PAD_FUNCTION_EN_1,
+		.mask = PMX_EGPIO_1_GRP_MASK,
+		.val = PMX_EGPIO_1_GRP_MASK,
+	},
+};
+
+static struct spear_modemux arm_gpio_modemux[] = {
+	{
+		.muxregs = arm_gpio_muxreg,
+		.nmuxregs = ARRAY_SIZE(arm_gpio_muxreg),
+	},
+};
+
+static struct spear_pingroup arm_gpio_pingroup = {
+	.name = "arm_gpio_grp",
+	.pins = arm_gpio_pins,
+	.npins = ARRAY_SIZE(arm_gpio_pins),
+	.modemuxs = arm_gpio_modemux,
+	.nmodemuxs = ARRAY_SIZE(arm_gpio_modemux),
+};
+
+static const char *const arm_gpio_grps[] = { "arm_gpio_grp" };
+static struct spear_function arm_gpio_function = {
+	.name = "arm_gpio",
+	.groups = arm_gpio_grps,
+	.ngroups = ARRAY_SIZE(arm_gpio_grps),
+};
+
+/* Pad multiplexing for smi 2 chips device */
+static const unsigned smi_2_chips_pins[] = { 153, 154, 155, 156, 157 };
+static struct spear_muxreg smi_2_chips_muxreg[] = {
+	{
+		.reg = PAD_FUNCTION_EN_0,
+		.mask = PMX_SMI_MASK,
+		.val = PMX_SMI_MASK,
+	},
+};
+
+static struct spear_modemux smi_2_chips_modemux[] = {
+	{
+		.muxregs = smi_2_chips_muxreg,
+		.nmuxregs = ARRAY_SIZE(smi_2_chips_muxreg),
+	},
+};
+
+static struct spear_pingroup smi_2_chips_pingroup = {
+	.name = "smi_2_chips_grp",
+	.pins = smi_2_chips_pins,
+	.npins = ARRAY_SIZE(smi_2_chips_pins),
+	.modemuxs = smi_2_chips_modemux,
+	.nmodemuxs = ARRAY_SIZE(smi_2_chips_modemux),
+};
+
+static const unsigned smi_4_chips_pins[] = { 54, 55 };
+static struct spear_muxreg smi_4_chips_muxreg[] = {
+	{
+		.reg = PAD_FUNCTION_EN_0,
+		.mask = PMX_SMI_MASK,
+		.val = PMX_SMI_MASK,
+	}, {
+		.reg = PAD_FUNCTION_EN_1,
+		.mask = PMX_SMINCS2_MASK | PMX_SMINCS3_MASK,
+		.val = PMX_SMINCS2_MASK | PMX_SMINCS3_MASK,
+	},
+};
+
+static struct spear_modemux smi_4_chips_modemux[] = {
+	{
+		.muxregs = smi_4_chips_muxreg,
+		.nmuxregs = ARRAY_SIZE(smi_4_chips_muxreg),
+	},
+};
+
+static struct spear_pingroup smi_4_chips_pingroup = {
+	.name = "smi_4_chips_grp",
+	.pins = smi_4_chips_pins,
+	.npins = ARRAY_SIZE(smi_4_chips_pins),
+	.modemuxs = smi_4_chips_modemux,
+	.nmodemuxs = ARRAY_SIZE(smi_4_chips_modemux),
+};
+
+static const char *const smi_grps[] = { "smi_2_chips_grp", "smi_4_chips_grp" };
+static struct spear_function smi_function = {
+	.name = "smi",
+	.groups = smi_grps,
+	.ngroups = ARRAY_SIZE(smi_grps),
+};
+
+/* Pad multiplexing for gmii device */
+static const unsigned gmii_pins[] = { 173, 174, 175, 176, 177, 178, 179, 180,
+	181, 182, 183, 184, 185, 186, 187, 188, 189, 190, 191, 192, 193, 194,
+	195, 196, 197, 198, 199, 200 };
+static struct spear_muxreg gmii_muxreg[] = {
+	{
+		.reg = PAD_FUNCTION_EN_0,
+		.mask = PMX_GMII_MASK,
+		.val = PMX_GMII_MASK,
+	},
+};
+
+static struct spear_modemux gmii_modemux[] = {
+	{
+		.muxregs = gmii_muxreg,
+		.nmuxregs = ARRAY_SIZE(gmii_muxreg),
+	},
+};
+
+static struct spear_pingroup gmii_pingroup = {
+	.name = "gmii_grp",
+	.pins = gmii_pins,
+	.npins = ARRAY_SIZE(gmii_pins),
+	.modemuxs = gmii_modemux,
+	.nmodemuxs = ARRAY_SIZE(gmii_modemux),
+};
+
+static const char *const gmii_grps[] = { "gmii_grp" };
+static struct spear_function gmii_function = {
+	.name = "gmii",
+	.groups = gmii_grps,
+	.ngroups = ARRAY_SIZE(gmii_grps),
+};
+
+/* Pad multiplexing for rgmii device */
+static const unsigned rgmii_pins[] = { 18, 19, 20, 21, 22, 23, 24, 25, 26, 27,
+	28, 29, 86, 87, 88, 89, 90, 91, 92, 93, 94, 95, 96, 97, 98, 99, 175,
+	180, 181, 182, 183, 185, 188, 193, 194, 195, 196, 197, 198, 211, 212 };
+static struct spear_muxreg rgmii_muxreg[] = {
+	{
+		.reg = PAD_FUNCTION_EN_0,
+		.mask = PMX_RGMII_REG0_MASK,
+		.val = 0,
+	}, {
+		.reg = PAD_FUNCTION_EN_1,
+		.mask = PMX_RGMII_REG1_MASK,
+		.val = 0,
+	}, {
+		.reg = PAD_FUNCTION_EN_2,
+		.mask = PMX_RGMII_REG2_MASK,
+		.val = 0,
+	},
+};
+
+static struct spear_modemux rgmii_modemux[] = {
+	{
+		.muxregs = rgmii_muxreg,
+		.nmuxregs = ARRAY_SIZE(rgmii_muxreg),
+	},
+};
+
+static struct spear_pingroup rgmii_pingroup = {
+	.name = "rgmii_grp",
+	.pins = rgmii_pins,
+	.npins = ARRAY_SIZE(rgmii_pins),
+	.modemuxs = rgmii_modemux,
+	.nmodemuxs = ARRAY_SIZE(rgmii_modemux),
+};
+
+static const char *const rgmii_grps[] = { "rgmii_grp" };
+static struct spear_function rgmii_function = {
+	.name = "rgmii",
+	.groups = rgmii_grps,
+	.ngroups = ARRAY_SIZE(rgmii_grps),
+};
+
+/* Pad multiplexing for smii_0_1_2 device */
+static const unsigned smii_0_1_2_pins[] = { 24, 25, 26, 27, 28, 29, 30, 31, 32,
+	33, 34, 35, 36, 37, 38, 39, 40, 41, 42, 43, 44, 45, 46, 47, 48, 49, 50,
+	51, 52, 53, 54, 55 };
+static struct spear_muxreg smii_0_1_2_muxreg[] = {
+	{
+		.reg = PAD_FUNCTION_EN_1,
+		.mask = PMX_SMII_0_1_2_MASK,
+		.val = 0,
+	},
+};
+
+static struct spear_modemux smii_0_1_2_modemux[] = {
+	{
+		.muxregs = smii_0_1_2_muxreg,
+		.nmuxregs = ARRAY_SIZE(smii_0_1_2_muxreg),
+	},
+};
+
+static struct spear_pingroup smii_0_1_2_pingroup = {
+	.name = "smii_0_1_2_grp",
+	.pins = smii_0_1_2_pins,
+	.npins = ARRAY_SIZE(smii_0_1_2_pins),
+	.modemuxs = smii_0_1_2_modemux,
+	.nmodemuxs = ARRAY_SIZE(smii_0_1_2_modemux),
+};
+
+static const char *const smii_0_1_2_grps[] = { "smii_0_1_2_grp" };
+static struct spear_function smii_0_1_2_function = {
+	.name = "smii_0_1_2",
+	.groups = smii_0_1_2_grps,
+	.ngroups = ARRAY_SIZE(smii_0_1_2_grps),
+};
+
+/* Pad multiplexing for ras_mii_txclk device */
+static const unsigned ras_mii_txclk_pins[] = { 98, 99 };
+static struct spear_muxreg ras_mii_txclk_muxreg[] = {
+	{
+		.reg = PAD_FUNCTION_EN_1,
+		.mask = PMX_NFCE2_MASK,
+		.val = 0,
+	},
+};
+
+static struct spear_modemux ras_mii_txclk_modemux[] = {
+	{
+		.muxregs = ras_mii_txclk_muxreg,
+		.nmuxregs = ARRAY_SIZE(ras_mii_txclk_muxreg),
+	},
+};
+
+static struct spear_pingroup ras_mii_txclk_pingroup = {
+	.name = "ras_mii_txclk_grp",
+	.pins = ras_mii_txclk_pins,
+	.npins = ARRAY_SIZE(ras_mii_txclk_pins),
+	.modemuxs = ras_mii_txclk_modemux,
+	.nmodemuxs = ARRAY_SIZE(ras_mii_txclk_modemux),
+};
+
+static const char *const ras_mii_txclk_grps[] = { "ras_mii_txclk_grp" };
+static struct spear_function ras_mii_txclk_function = {
+	.name = "ras_mii_txclk",
+	.groups = ras_mii_txclk_grps,
+	.ngroups = ARRAY_SIZE(ras_mii_txclk_grps),
+};
+
+/* Pad multiplexing for nand 8bit device (cs0 only) */
+static const unsigned nand_8bit_pins[] = { 56, 57, 58, 59, 60, 61, 62, 63, 64,
+	65, 66, 67, 68, 69, 70, 71, 72, 73, 74, 75, 76, 77, 78, 79, 80, 81, 82,
+	83, 84, 85, 158, 159, 160, 161, 162, 163, 164, 165, 166, 167, 168, 169,
+	170, 171, 172, 201, 202, 203, 204, 205, 206, 207, 208, 209, 210, 211,
+	212 };
+static struct spear_muxreg nand_8bit_muxreg[] = {
+	{
+		.reg = PAD_FUNCTION_EN_0,
+		.mask = PMX_NAND8BIT_0_MASK,
+		.val = PMX_NAND8BIT_0_MASK,
+	}, {
+		.reg = PAD_FUNCTION_EN_1,
+		.mask = PMX_NAND8BIT_1_MASK,
+		.val = PMX_NAND8BIT_1_MASK,
+	},
+};
+
+static struct spear_modemux nand_8bit_modemux[] = {
+	{
+		.muxregs = nand_8bit_muxreg,
+		.nmuxregs = ARRAY_SIZE(nand_8bit_muxreg),
+	},
+};
+
+static struct spear_pingroup nand_8bit_pingroup = {
+	.name = "nand_8bit_grp",
+	.pins = nand_8bit_pins,
+	.npins = ARRAY_SIZE(nand_8bit_pins),
+	.modemuxs = nand_8bit_modemux,
+	.nmodemuxs = ARRAY_SIZE(nand_8bit_modemux),
+};
+
+/* Pad multiplexing for nand 16bit device */
+static const unsigned nand_16bit_pins[] = { 201, 202, 203, 204, 207, 208, 209,
+	210 };
+static struct spear_muxreg nand_16bit_muxreg[] = {
+	{
+		.reg = PAD_FUNCTION_EN_1,
+		.mask = PMX_NAND16BIT_1_MASK,
+		.val = PMX_NAND16BIT_1_MASK,
+	},
+};
+
+static struct spear_modemux nand_16bit_modemux[] = {
+	{
+		.muxregs = nand_16bit_muxreg,
+		.nmuxregs = ARRAY_SIZE(nand_16bit_muxreg),
+	},
+};
+
+static struct spear_pingroup nand_16bit_pingroup = {
+	.name = "nand_16bit_grp",
+	.pins = nand_16bit_pins,
+	.npins = ARRAY_SIZE(nand_16bit_pins),
+	.modemuxs = nand_16bit_modemux,
+	.nmodemuxs = ARRAY_SIZE(nand_16bit_modemux),
+};
+
+/* Pad multiplexing for nand 4 chips */
+static const unsigned nand_4_chips_pins[] = { 205, 206, 211, 212 };
+static struct spear_muxreg nand_4_chips_muxreg[] = {
+	{
+		.reg = PAD_FUNCTION_EN_1,
+		.mask = PMX_NAND_4CHIPS_MASK,
+		.val = PMX_NAND_4CHIPS_MASK,
+	},
+};
+
+static struct spear_modemux nand_4_chips_modemux[] = {
+	{
+		.muxregs = nand_4_chips_muxreg,
+		.nmuxregs = ARRAY_SIZE(nand_4_chips_muxreg),
+	},
+};
+
+static struct spear_pingroup nand_4_chips_pingroup = {
+	.name = "nand_4_chips_grp",
+	.pins = nand_4_chips_pins,
+	.npins = ARRAY_SIZE(nand_4_chips_pins),
+	.modemuxs = nand_4_chips_modemux,
+	.nmodemuxs = ARRAY_SIZE(nand_4_chips_modemux),
+};
+
+static const char *const nand_grps[] = { "nand_8bit_grp", "nand_16bit_grp",
+	"nand_4_chips_grp" };
+static struct spear_function nand_function = {
+	.name = "nand",
+	.groups = nand_grps,
+	.ngroups = ARRAY_SIZE(nand_grps),
+};
+
+/* Pad multiplexing for keyboard_6x6 device */
+static const unsigned keyboard_6x6_pins[] = { 201, 202, 203, 204, 205, 206, 207,
+	208, 209, 210, 211, 212 };
+static struct spear_muxreg keyboard_6x6_muxreg[] = {
+	{
+		.reg = PAD_FUNCTION_EN_1,
+		.mask = PMX_KEYBOARD_6X6_MASK | PMX_NFIO8_15_MASK |
+			PMX_NFCE1_MASK | PMX_NFCE2_MASK | PMX_NFWPRT1_MASK |
+			PMX_NFWPRT2_MASK,
+		.val = PMX_KEYBOARD_6X6_MASK,
+	},
+};
+
+static struct spear_modemux keyboard_6x6_modemux[] = {
+	{
+		.muxregs = keyboard_6x6_muxreg,
+		.nmuxregs = ARRAY_SIZE(keyboard_6x6_muxreg),
+	},
+};
+
+static struct spear_pingroup keyboard_6x6_pingroup = {
+	.name = "keyboard_6x6_grp",
+	.pins = keyboard_6x6_pins,
+	.npins = ARRAY_SIZE(keyboard_6x6_pins),
+	.modemuxs = keyboard_6x6_modemux,
+	.nmodemuxs = ARRAY_SIZE(keyboard_6x6_modemux),
+};
+
+/* Pad multiplexing for keyboard_rowcol6_8 device */
+static const unsigned keyboard_rowcol6_8_pins[] = { 24, 25, 26, 27, 28, 29 };
+static struct spear_muxreg keyboard_rowcol6_8_muxreg[] = {
+	{
+		.reg = PAD_FUNCTION_EN_1,
+		.mask = PMX_KBD_ROWCOL68_MASK,
+		.val = PMX_KBD_ROWCOL68_MASK,
+	},
+};
+
+static struct spear_modemux keyboard_rowcol6_8_modemux[] = {
+	{
+		.muxregs = keyboard_rowcol6_8_muxreg,
+		.nmuxregs = ARRAY_SIZE(keyboard_rowcol6_8_muxreg),
+	},
+};
+
+static struct spear_pingroup keyboard_rowcol6_8_pingroup = {
+	.name = "keyboard_rowcol6_8_grp",
+	.pins = keyboard_rowcol6_8_pins,
+	.npins = ARRAY_SIZE(keyboard_rowcol6_8_pins),
+	.modemuxs = keyboard_rowcol6_8_modemux,
+	.nmodemuxs = ARRAY_SIZE(keyboard_rowcol6_8_modemux),
+};
+
+static const char *const keyboard_grps[] = { "keyboard_6x6_grp",
+	"keyboard_rowcol6_8_grp" };
+static struct spear_function keyboard_function = {
+	.name = "keyboard",
+	.groups = keyboard_grps,
+	.ngroups = ARRAY_SIZE(keyboard_grps),
+};
+
+/* Pad multiplexing for uart0 device */
+static const unsigned uart0_pins[] = { 100, 101 };
+static struct spear_muxreg uart0_muxreg[] = {
+	{
+		.reg = PAD_FUNCTION_EN_0,
+		.mask = PMX_UART0_MASK,
+		.val = PMX_UART0_MASK,
+	},
+};
+
+static struct spear_modemux uart0_modemux[] = {
+	{
+		.muxregs = uart0_muxreg,
+		.nmuxregs = ARRAY_SIZE(uart0_muxreg),
+	},
+};
+
+static struct spear_pingroup uart0_pingroup = {
+	.name = "uart0_grp",
+	.pins = uart0_pins,
+	.npins = ARRAY_SIZE(uart0_pins),
+	.modemuxs = uart0_modemux,
+	.nmodemuxs = ARRAY_SIZE(uart0_modemux),
+};
+
+/* Pad multiplexing for uart0_modem device */
+static const unsigned uart0_modem_pins[] = { 12, 13, 14, 15, 16, 17 };
+static struct spear_muxreg uart0_modem_muxreg[] = {
+	{
+		.reg = PAD_FUNCTION_EN_1,
+		.mask = PMX_UART0_MODEM_MASK,
+		.val = PMX_UART0_MODEM_MASK,
+	},
+};
+
+static struct spear_modemux uart0_modem_modemux[] = {
+	{
+		.muxregs = uart0_modem_muxreg,
+		.nmuxregs = ARRAY_SIZE(uart0_modem_muxreg),
+	},
+};
+
+static struct spear_pingroup uart0_modem_pingroup = {
+	.name = "uart0_modem_grp",
+	.pins = uart0_modem_pins,
+	.npins = ARRAY_SIZE(uart0_modem_pins),
+	.modemuxs = uart0_modem_modemux,
+	.nmodemuxs = ARRAY_SIZE(uart0_modem_modemux),
+};
+
+static const char *const uart0_grps[] = { "uart0_grp", "uart0_modem_grp" };
+static struct spear_function uart0_function = {
+	.name = "uart0",
+	.groups = uart0_grps,
+	.ngroups = ARRAY_SIZE(uart0_grps),
+};
+
+/* Pad multiplexing for gpt0_tmr0 device */
+static const unsigned gpt0_tmr0_pins[] = { 10, 11 };
+static struct spear_muxreg gpt0_tmr0_muxreg[] = {
+	{
+		.reg = PAD_FUNCTION_EN_1,
+		.mask = PMX_GPT0_TMR0_MASK,
+		.val = PMX_GPT0_TMR0_MASK,
+	},
+};
+
+static struct spear_modemux gpt0_tmr0_modemux[] = {
+	{
+		.muxregs = gpt0_tmr0_muxreg,
+		.nmuxregs = ARRAY_SIZE(gpt0_tmr0_muxreg),
+	},
+};
+
+static struct spear_pingroup gpt0_tmr0_pingroup = {
+	.name = "gpt0_tmr0_grp",
+	.pins = gpt0_tmr0_pins,
+	.npins = ARRAY_SIZE(gpt0_tmr0_pins),
+	.modemuxs = gpt0_tmr0_modemux,
+	.nmodemuxs = ARRAY_SIZE(gpt0_tmr0_modemux),
+};
+
+/* Pad multiplexing for gpt0_tmr1 device */
+static const unsigned gpt0_tmr1_pins[] = { 8, 9 };
+static struct spear_muxreg gpt0_tmr1_muxreg[] = {
+	{
+		.reg = PAD_FUNCTION_EN_1,
+		.mask = PMX_GPT0_TMR1_MASK,
+		.val = PMX_GPT0_TMR1_MASK,
+	},
+};
+
+static struct spear_modemux gpt0_tmr1_modemux[] = {
+	{
+		.muxregs = gpt0_tmr1_muxreg,
+		.nmuxregs = ARRAY_SIZE(gpt0_tmr1_muxreg),
+	},
+};
+
+static struct spear_pingroup gpt0_tmr1_pingroup = {
+	.name = "gpt0_tmr1_grp",
+	.pins = gpt0_tmr1_pins,
+	.npins = ARRAY_SIZE(gpt0_tmr1_pins),
+	.modemuxs = gpt0_tmr1_modemux,
+	.nmodemuxs = ARRAY_SIZE(gpt0_tmr1_modemux),
+};
+
+static const char *const gpt0_grps[] = { "gpt0_tmr0_grp", "gpt0_tmr1_grp" };
+static struct spear_function gpt0_function = {
+	.name = "gpt0",
+	.groups = gpt0_grps,
+	.ngroups = ARRAY_SIZE(gpt0_grps),
+};
+
+/* Pad multiplexing for gpt1_tmr0 device */
+static const unsigned gpt1_tmr0_pins[] = { 6, 7 };
+static struct spear_muxreg gpt1_tmr0_muxreg[] = {
+	{
+		.reg = PAD_FUNCTION_EN_1,
+		.mask = PMX_GPT1_TMR0_MASK,
+		.val = PMX_GPT1_TMR0_MASK,
+	},
+};
+
+static struct spear_modemux gpt1_tmr0_modemux[] = {
+	{
+		.muxregs = gpt1_tmr0_muxreg,
+		.nmuxregs = ARRAY_SIZE(gpt1_tmr0_muxreg),
+	},
+};
+
+static struct spear_pingroup gpt1_tmr0_pingroup = {
+	.name = "gpt1_tmr0_grp",
+	.pins = gpt1_tmr0_pins,
+	.npins = ARRAY_SIZE(gpt1_tmr0_pins),
+	.modemuxs = gpt1_tmr0_modemux,
+	.nmodemuxs = ARRAY_SIZE(gpt1_tmr0_modemux),
+};
+
+/* Pad multiplexing for gpt1_tmr1 device */
+static const unsigned gpt1_tmr1_pins[] = { 4, 5 };
+static struct spear_muxreg gpt1_tmr1_muxreg[] = {
+	{
+		.reg = PAD_FUNCTION_EN_1,
+		.mask = PMX_GPT1_TMR1_MASK,
+		.val = PMX_GPT1_TMR1_MASK,
+	},
+};
+
+static struct spear_modemux gpt1_tmr1_modemux[] = {
+	{
+		.muxregs = gpt1_tmr1_muxreg,
+		.nmuxregs = ARRAY_SIZE(gpt1_tmr1_muxreg),
+	},
+};
+
+static struct spear_pingroup gpt1_tmr1_pingroup = {
+	.name = "gpt1_tmr1_grp",
+	.pins = gpt1_tmr1_pins,
+	.npins = ARRAY_SIZE(gpt1_tmr1_pins),
+	.modemuxs = gpt1_tmr1_modemux,
+	.nmodemuxs = ARRAY_SIZE(gpt1_tmr1_modemux),
+};
+
+static const char *const gpt1_grps[] = { "gpt1_tmr1_grp", "gpt1_tmr0_grp" };
+static struct spear_function gpt1_function = {
+	.name = "gpt1",
+	.groups = gpt1_grps,
+	.ngroups = ARRAY_SIZE(gpt1_grps),
+};
+
+/* Pad multiplexing for mcif device */
+static const unsigned mcif_pins[] = { 86, 87, 88, 89, 90, 91, 92, 93, 213, 214,
+	215, 216, 217, 218, 219, 220, 221, 222, 223, 224, 225, 226, 227, 228,
+	229, 230, 231, 232, 233, 234, 235, 236, 237, 238, 239, 240, 241, 242,
+	243, 244, 245 };
+#define MCIF_MUXREG						\
+	{							\
+		.reg = PAD_FUNCTION_EN_0,			\
+		.mask = PMX_MCI_DATA8_15_MASK,			\
+		.val = PMX_MCI_DATA8_15_MASK,			\
+	}, {							\
+		.reg = PAD_FUNCTION_EN_1,			\
+		.mask = PMX_MCIFALL_1_MASK | PMX_NFWPRT1_MASK |	\
+			PMX_NFWPRT2_MASK,			\
+		.val = PMX_MCIFALL_1_MASK,			\
+	}, {							\
+		.reg = PAD_FUNCTION_EN_2,			\
+		.mask = PMX_MCIFALL_2_MASK,			\
+		.val = PMX_MCIFALL_2_MASK,			\
+	}
+
+/* sdhci device */
+static struct spear_muxreg sdhci_muxreg[] = {
+	MCIF_MUXREG,
+	{
+		.reg = PERIP_CFG,
+		.mask = MCIF_SEL_MASK,
+		.val = MCIF_SEL_SD,
+	},
+};
+
+static struct spear_modemux sdhci_modemux[] = {
+	{
+		.muxregs = sdhci_muxreg,
+		.nmuxregs = ARRAY_SIZE(sdhci_muxreg),
+	},
+};
+
+static struct spear_pingroup sdhci_pingroup = {
+	.name = "sdhci_grp",
+	.pins = mcif_pins,
+	.npins = ARRAY_SIZE(mcif_pins),
+	.modemuxs = sdhci_modemux,
+	.nmodemuxs = ARRAY_SIZE(sdhci_modemux),
+};
+
+static const char *const sdhci_grps[] = { "sdhci_grp" };
+static struct spear_function sdhci_function = {
+	.name = "sdhci",
+	.groups = sdhci_grps,
+	.ngroups = ARRAY_SIZE(sdhci_grps),
+};
+
+/* cf device */
+static struct spear_muxreg cf_muxreg[] = {
+	MCIF_MUXREG,
+	{
+		.reg = PERIP_CFG,
+		.mask = MCIF_SEL_MASK,
+		.val = MCIF_SEL_CF,
+	},
+};
+
+static struct spear_modemux cf_modemux[] = {
+	{
+		.muxregs = cf_muxreg,
+		.nmuxregs = ARRAY_SIZE(cf_muxreg),
+	},
+};
+
+static struct spear_pingroup cf_pingroup = {
+	.name = "cf_grp",
+	.pins = mcif_pins,
+	.npins = ARRAY_SIZE(mcif_pins),
+	.modemuxs = cf_modemux,
+	.nmodemuxs = ARRAY_SIZE(cf_modemux),
+};
+
+static const char *const cf_grps[] = { "cf_grp" };
+static struct spear_function cf_function = {
+	.name = "cf",
+	.groups = cf_grps,
+	.ngroups = ARRAY_SIZE(cf_grps),
+};
+
+/* xd device */
+static struct spear_muxreg xd_muxreg[] = {
+	MCIF_MUXREG,
+	{
+		.reg = PERIP_CFG,
+		.mask = MCIF_SEL_MASK,
+		.val = MCIF_SEL_XD,
+	},
+};
+
+static struct spear_modemux xd_modemux[] = {
+	{
+		.muxregs = xd_muxreg,
+		.nmuxregs = ARRAY_SIZE(xd_muxreg),
+	},
+};
+
+static struct spear_pingroup xd_pingroup = {
+	.name = "xd_grp",
+	.pins = mcif_pins,
+	.npins = ARRAY_SIZE(mcif_pins),
+	.modemuxs = xd_modemux,
+	.nmodemuxs = ARRAY_SIZE(xd_modemux),
+};
+
+static const char *const xd_grps[] = { "xd_grp" };
+static struct spear_function xd_function = {
+	.name = "xd",
+	.groups = xd_grps,
+	.ngroups = ARRAY_SIZE(xd_grps),
+};
+
+/* Pad multiplexing for touch_xy device */
+static const unsigned touch_xy_pins[] = { 97 };
+static struct spear_muxreg touch_xy_muxreg[] = {
+	{
+		.reg = PAD_FUNCTION_EN_2,
+		.mask = PMX_TOUCH_XY_MASK,
+		.val = PMX_TOUCH_XY_MASK,
+	},
+};
+
+static struct spear_modemux touch_xy_modemux[] = {
+	{
+		.muxregs = touch_xy_muxreg,
+		.nmuxregs = ARRAY_SIZE(touch_xy_muxreg),
+	},
+};
+
+static struct spear_pingroup touch_xy_pingroup = {
+	.name = "touch_xy_grp",
+	.pins = touch_xy_pins,
+	.npins = ARRAY_SIZE(touch_xy_pins),
+	.modemuxs = touch_xy_modemux,
+	.nmodemuxs = ARRAY_SIZE(touch_xy_modemux),
+};
+
+static const char *const touch_xy_grps[] = { "touch_xy_grp" };
+static struct spear_function touch_xy_function = {
+	.name = "touchscreen",
+	.groups = touch_xy_grps,
+	.ngroups = ARRAY_SIZE(touch_xy_grps),
+};
+
+/* Pad multiplexing for uart1 device */
+/* Muxed with I2C */
+static const unsigned uart1_dis_i2c_pins[] = { 102, 103 };
+static struct spear_muxreg uart1_dis_i2c_muxreg[] = {
+	{
+		.reg = PAD_FUNCTION_EN_0,
+		.mask = PMX_I2C0_MASK,
+		.val = 0,
+	},
+};
+
+static struct spear_modemux uart1_dis_i2c_modemux[] = {
+	{
+		.muxregs = uart1_dis_i2c_muxreg,
+		.nmuxregs = ARRAY_SIZE(uart1_dis_i2c_muxreg),
+	},
+};
+
+static struct spear_pingroup uart_1_dis_i2c_pingroup = {
+	.name = "uart1_disable_i2c_grp",
+	.pins = uart1_dis_i2c_pins,
+	.npins = ARRAY_SIZE(uart1_dis_i2c_pins),
+	.modemuxs = uart1_dis_i2c_modemux,
+	.nmodemuxs = ARRAY_SIZE(uart1_dis_i2c_modemux),
+};
+
+/* Muxed with SD/MMC */
+static const unsigned uart1_dis_sd_pins[] = { 214, 215 };
+static struct spear_muxreg uart1_dis_sd_muxreg[] = {
+	{
+		.reg = PAD_FUNCTION_EN_1,
+		.mask = PMX_MCIDATA1_MASK |
+			PMX_MCIDATA2_MASK,
+		.val = 0,
+	},
+};
+
+static struct spear_modemux uart1_dis_sd_modemux[] = {
+	{
+		.muxregs = uart1_dis_sd_muxreg,
+		.nmuxregs = ARRAY_SIZE(uart1_dis_sd_muxreg),
+	},
+};
+
+static struct spear_pingroup uart_1_dis_sd_pingroup = {
+	.name = "uart1_disable_sd_grp",
+	.pins = uart1_dis_sd_pins,
+	.npins = ARRAY_SIZE(uart1_dis_sd_pins),
+	.modemuxs = uart1_dis_sd_modemux,
+	.nmodemuxs = ARRAY_SIZE(uart1_dis_sd_modemux),
+};
+
+static const char *const uart1_grps[] = { "uart1_disable_i2c_grp",
+	"uart1_disable_sd_grp" };
+static struct spear_function uart1_function = {
+	.name = "uart1",
+	.groups = uart1_grps,
+	.ngroups = ARRAY_SIZE(uart1_grps),
+};
+
+/* Pad multiplexing for uart2_3 device */
+static const unsigned uart2_3_pins[] = { 104, 105, 106, 107 };
+static struct spear_muxreg uart2_3_muxreg[] = {
+	{
+		.reg = PAD_FUNCTION_EN_0,
+		.mask = PMX_I2S0_MASK,
+		.val = 0,
+	},
+};
+
+static struct spear_modemux uart2_3_modemux[] = {
+	{
+		.muxregs = uart2_3_muxreg,
+		.nmuxregs = ARRAY_SIZE(uart2_3_muxreg),
+	},
+};
+
+static struct spear_pingroup uart_2_3_pingroup = {
+	.name = "uart2_3_grp",
+	.pins = uart2_3_pins,
+	.npins = ARRAY_SIZE(uart2_3_pins),
+	.modemuxs = uart2_3_modemux,
+	.nmodemuxs = ARRAY_SIZE(uart2_3_modemux),
+};
+
+static const char *const uart2_3_grps[] = { "uart2_3_grp" };
+static struct spear_function uart2_3_function = {
+	.name = "uart2_3",
+	.groups = uart2_3_grps,
+	.ngroups = ARRAY_SIZE(uart2_3_grps),
+};
+
+/* Pad multiplexing for uart4 device */
+static const unsigned uart4_pins[] = { 108, 113 };
+static struct spear_muxreg uart4_muxreg[] = {
+	{
+		.reg = PAD_FUNCTION_EN_0,
+		.mask = PMX_I2S0_MASK | PMX_CLCD1_MASK,
+		.val = 0,
+	},
+};
+
+static struct spear_modemux uart4_modemux[] = {
+	{
+		.muxregs = uart4_muxreg,
+		.nmuxregs = ARRAY_SIZE(uart4_muxreg),
+	},
+};
+
+static struct spear_pingroup uart_4_pingroup = {
+	.name = "uart4_grp",
+	.pins = uart4_pins,
+	.npins = ARRAY_SIZE(uart4_pins),
+	.modemuxs = uart4_modemux,
+	.nmodemuxs = ARRAY_SIZE(uart4_modemux),
+};
+
+static const char *const uart4_grps[] = { "uart4_grp" };
+static struct spear_function uart4_function = {
+	.name = "uart4",
+	.groups = uart4_grps,
+	.ngroups = ARRAY_SIZE(uart4_grps),
+};
+
+/* Pad multiplexing for uart5 device */
+static const unsigned uart5_pins[] = { 114, 115 };
+static struct spear_muxreg uart5_muxreg[] = {
+	{
+		.reg = PAD_FUNCTION_EN_0,
+		.mask = PMX_CLCD1_MASK,
+		.val = 0,
+	},
+};
+
+static struct spear_modemux uart5_modemux[] = {
+	{
+		.muxregs = uart5_muxreg,
+		.nmuxregs = ARRAY_SIZE(uart5_muxreg),
+	},
+};
+
+static struct spear_pingroup uart_5_pingroup = {
+	.name = "uart5_grp",
+	.pins = uart5_pins,
+	.npins = ARRAY_SIZE(uart5_pins),
+	.modemuxs = uart5_modemux,
+	.nmodemuxs = ARRAY_SIZE(uart5_modemux),
+};
+
+static const char *const uart5_grps[] = { "uart5_grp" };
+static struct spear_function uart5_function = {
+	.name = "uart5",
+	.groups = uart5_grps,
+	.ngroups = ARRAY_SIZE(uart5_grps),
+};
+
+/* Pad multiplexing for rs485_0_1_tdm_0_1 device */
+static const unsigned rs485_0_1_tdm_0_1_pins[] = { 116, 117, 118, 119, 120, 121,
+	122, 123, 124, 125, 126, 127, 128, 129, 130, 131, 132, 133, 134, 135,
+	136, 137 };
+static struct spear_muxreg rs485_0_1_tdm_0_1_muxreg[] = {
+	{
+		.reg = PAD_FUNCTION_EN_0,
+		.mask = PMX_CLCD1_MASK,
+		.val = 0,
+	},
+};
+
+static struct spear_modemux rs485_0_1_tdm_0_1_modemux[] = {
+	{
+		.muxregs = rs485_0_1_tdm_0_1_muxreg,
+		.nmuxregs = ARRAY_SIZE(rs485_0_1_tdm_0_1_muxreg),
+	},
+};
+
+static struct spear_pingroup rs485_0_1_tdm_0_1_pingroup = {
+	.name = "rs485_0_1_tdm_0_1_grp",
+	.pins = rs485_0_1_tdm_0_1_pins,
+	.npins = ARRAY_SIZE(rs485_0_1_tdm_0_1_pins),
+	.modemuxs = rs485_0_1_tdm_0_1_modemux,
+	.nmodemuxs = ARRAY_SIZE(rs485_0_1_tdm_0_1_modemux),
+};
+
+static const char *const rs485_0_1_tdm_0_1_grps[] = { "rs485_0_1_tdm_0_1_grp" };
+static struct spear_function rs485_0_1_tdm_0_1_function = {
+	.name = "rs485_0_1_tdm_0_1",
+	.groups = rs485_0_1_tdm_0_1_grps,
+	.ngroups = ARRAY_SIZE(rs485_0_1_tdm_0_1_grps),
+};
+
+/* Pad multiplexing for i2c_1_2 device */
+static const unsigned i2c_1_2_pins[] = { 138, 139, 140, 141 };
+static struct spear_muxreg i2c_1_2_muxreg[] = {
+	{
+		.reg = PAD_FUNCTION_EN_0,
+		.mask = PMX_CLCD1_MASK,
+		.val = 0,
+	},
+};
+
+static struct spear_modemux i2c_1_2_modemux[] = {
+	{
+		.muxregs = i2c_1_2_muxreg,
+		.nmuxregs = ARRAY_SIZE(i2c_1_2_muxreg),
+	},
+};
+
+static struct spear_pingroup i2c_1_2_pingroup = {
+	.name = "i2c_1_2_grp",
+	.pins = i2c_1_2_pins,
+	.npins = ARRAY_SIZE(i2c_1_2_pins),
+	.modemuxs = i2c_1_2_modemux,
+	.nmodemuxs = ARRAY_SIZE(i2c_1_2_modemux),
+};
+
+static const char *const i2c_1_2_grps[] = { "i2c_1_2_grp" };
+static struct spear_function i2c_1_2_function = {
+	.name = "i2c_1_2",
+	.groups = i2c_1_2_grps,
+	.ngroups = ARRAY_SIZE(i2c_1_2_grps),
+};
+
+/* Pad multiplexing for i2c3_dis_smi_clcd device */
+/* Muxed with SMI & CLCD */
+static const unsigned i2c3_dis_smi_clcd_pins[] = { 142, 153 };
+static struct spear_muxreg i2c3_dis_smi_clcd_muxreg[] = {
+	{
+		.reg = PAD_FUNCTION_EN_0,
+		.mask = PMX_CLCD1_MASK | PMX_SMI_MASK,
+		.val = 0,
+	},
+};
+
+static struct spear_modemux i2c3_dis_smi_clcd_modemux[] = {
+	{
+		.muxregs = i2c3_dis_smi_clcd_muxreg,
+		.nmuxregs = ARRAY_SIZE(i2c3_dis_smi_clcd_muxreg),
+	},
+};
+
+static struct spear_pingroup i2c3_dis_smi_clcd_pingroup = {
+	.name = "i2c3_dis_smi_clcd_grp",
+	.pins = i2c3_dis_smi_clcd_pins,
+	.npins = ARRAY_SIZE(i2c3_dis_smi_clcd_pins),
+	.modemuxs = i2c3_dis_smi_clcd_modemux,
+	.nmodemuxs = ARRAY_SIZE(i2c3_dis_smi_clcd_modemux),
+};
+
+/* Pad multiplexing for i2c3_dis_sd_i2s0 device */
+/* Muxed with SD/MMC & I2S1 */
+static const unsigned i2c3_dis_sd_i2s0_pins[] = { 0, 216 };
+static struct spear_muxreg i2c3_dis_sd_i2s0_muxreg[] = {
+	{
+		.reg = PAD_FUNCTION_EN_1,
+		.mask = PMX_I2S1_MASK | PMX_MCIDATA3_MASK,
+		.val = 0,
+	},
+};
+
+static struct spear_modemux i2c3_dis_sd_i2s0_modemux[] = {
+	{
+		.muxregs = i2c3_dis_sd_i2s0_muxreg,
+		.nmuxregs = ARRAY_SIZE(i2c3_dis_sd_i2s0_muxreg),
+	},
+};
+
+static struct spear_pingroup i2c3_dis_sd_i2s0_pingroup = {
+	.name = "i2c3_dis_sd_i2s0_grp",
+	.pins = i2c3_dis_sd_i2s0_pins,
+	.npins = ARRAY_SIZE(i2c3_dis_sd_i2s0_pins),
+	.modemuxs = i2c3_dis_sd_i2s0_modemux,
+	.nmodemuxs = ARRAY_SIZE(i2c3_dis_sd_i2s0_modemux),
+};
+
+static const char *const i2c3_grps[] = { "i2c3_dis_smi_clcd_grp",
+	"i2c3_dis_sd_i2s0_grp" };
+static struct spear_function i2c3_unction = {
+	.name = "i2c3_i2s1",
+	.groups = i2c3_grps,
+	.ngroups = ARRAY_SIZE(i2c3_grps),
+};
+
+/* Pad multiplexing for i2c_4_5_dis_smi device */
+/* Muxed with SMI */
+static const unsigned i2c_4_5_dis_smi_pins[] = { 154, 155, 156, 157 };
+static struct spear_muxreg i2c_4_5_dis_smi_muxreg[] = {
+	{
+		.reg = PAD_FUNCTION_EN_0,
+		.mask = PMX_SMI_MASK,
+		.val = 0,
+	},
+};
+
+static struct spear_modemux i2c_4_5_dis_smi_modemux[] = {
+	{
+		.muxregs = i2c_4_5_dis_smi_muxreg,
+		.nmuxregs = ARRAY_SIZE(i2c_4_5_dis_smi_muxreg),
+	},
+};
+
+static struct spear_pingroup i2c_4_5_dis_smi_pingroup = {
+	.name = "i2c_4_5_dis_smi_grp",
+	.pins = i2c_4_5_dis_smi_pins,
+	.npins = ARRAY_SIZE(i2c_4_5_dis_smi_pins),
+	.modemuxs = i2c_4_5_dis_smi_modemux,
+	.nmodemuxs = ARRAY_SIZE(i2c_4_5_dis_smi_modemux),
+};
+
+/* Pad multiplexing for i2c4_dis_sd device */
+/* Muxed with SD/MMC */
+static const unsigned i2c4_dis_sd_pins[] = { 217, 218 };
+static struct spear_muxreg i2c4_dis_sd_muxreg[] = {
+	{
+		.reg = PAD_FUNCTION_EN_1,
+		.mask = PMX_MCIDATA4_MASK,
+		.val = 0,
+	}, {
+		.reg = PAD_FUNCTION_EN_2,
+		.mask = PMX_MCIDATA5_MASK,
+		.val = 0,
+	},
+};
+
+static struct spear_modemux i2c4_dis_sd_modemux[] = {
+	{
+		.muxregs = i2c4_dis_sd_muxreg,
+		.nmuxregs = ARRAY_SIZE(i2c4_dis_sd_muxreg),
+	},
+};
+
+static struct spear_pingroup i2c4_dis_sd_pingroup = {
+	.name = "i2c4_dis_sd_grp",
+	.pins = i2c4_dis_sd_pins,
+	.npins = ARRAY_SIZE(i2c4_dis_sd_pins),
+	.modemuxs = i2c4_dis_sd_modemux,
+	.nmodemuxs = ARRAY_SIZE(i2c4_dis_sd_modemux),
+};
+
+/* Pad multiplexing for i2c5_dis_sd device */
+/* Muxed with SD/MMC */
+static const unsigned i2c5_dis_sd_pins[] = { 219, 220 };
+static struct spear_muxreg i2c5_dis_sd_muxreg[] = {
+	{
+		.reg = PAD_FUNCTION_EN_2,
+		.mask = PMX_MCIDATA6_MASK |
+			PMX_MCIDATA7_MASK,
+		.val = 0,
+	},
+};
+
+static struct spear_modemux i2c5_dis_sd_modemux[] = {
+	{
+		.muxregs = i2c5_dis_sd_muxreg,
+		.nmuxregs = ARRAY_SIZE(i2c5_dis_sd_muxreg),
+	},
+};
+
+static struct spear_pingroup i2c5_dis_sd_pingroup = {
+	.name = "i2c5_dis_sd_grp",
+	.pins = i2c5_dis_sd_pins,
+	.npins = ARRAY_SIZE(i2c5_dis_sd_pins),
+	.modemuxs = i2c5_dis_sd_modemux,
+	.nmodemuxs = ARRAY_SIZE(i2c5_dis_sd_modemux),
+};
+
+static const char *const i2c_4_5_grps[] = { "i2c5_dis_sd_grp",
+	"i2c4_dis_sd_grp", "i2c_4_5_dis_smi_grp" };
+static struct spear_function i2c_4_5_function = {
+	.name = "i2c_4_5",
+	.groups = i2c_4_5_grps,
+	.ngroups = ARRAY_SIZE(i2c_4_5_grps),
+};
+
+/* Pad multiplexing for i2c_6_7_dis_kbd device */
+/* Muxed with KBD */
+static const unsigned i2c_6_7_dis_kbd_pins[] = { 207, 208, 209, 210 };
+static struct spear_muxreg i2c_6_7_dis_kbd_muxreg[] = {
+	{
+		.reg = PAD_FUNCTION_EN_1,
+		.mask = PMX_KBD_ROWCOL25_MASK,
+		.val = 0,
+	},
+};
+
+static struct spear_modemux i2c_6_7_dis_kbd_modemux[] = {
+	{
+		.muxregs = i2c_6_7_dis_kbd_muxreg,
+		.nmuxregs = ARRAY_SIZE(i2c_6_7_dis_kbd_muxreg),
+	},
+};
+
+static struct spear_pingroup i2c_6_7_dis_kbd_pingroup = {
+	.name = "i2c_6_7_dis_kbd_grp",
+	.pins = i2c_6_7_dis_kbd_pins,
+	.npins = ARRAY_SIZE(i2c_6_7_dis_kbd_pins),
+	.modemuxs = i2c_6_7_dis_kbd_modemux,
+	.nmodemuxs = ARRAY_SIZE(i2c_6_7_dis_kbd_modemux),
+};
+
+/* Pad multiplexing for i2c6_dis_sd device */
+/* Muxed with SD/MMC */
+static const unsigned i2c6_dis_sd_pins[] = { 236, 237 };
+static struct spear_muxreg i2c6_dis_sd_muxreg[] = {
+	{
+		.reg = PAD_FUNCTION_EN_2,
+		.mask = PMX_MCIIORDRE_MASK |
+			PMX_MCIIOWRWE_MASK,
+		.val = 0,
+	},
+};
+
+static struct spear_modemux i2c6_dis_sd_modemux[] = {
+	{
+		.muxregs = i2c6_dis_sd_muxreg,
+		.nmuxregs = ARRAY_SIZE(i2c6_dis_sd_muxreg),
+	},
+};
+
+static struct spear_pingroup i2c6_dis_sd_pingroup = {
+	.name = "i2c6_dis_sd_grp",
+	.pins = i2c6_dis_sd_pins,
+	.npins = ARRAY_SIZE(i2c6_dis_sd_pins),
+	.modemuxs = i2c6_dis_sd_modemux,
+	.nmodemuxs = ARRAY_SIZE(i2c6_dis_sd_modemux),
+};
+
+/* Pad multiplexing for i2c7_dis_sd device */
+static const unsigned i2c7_dis_sd_pins[] = { 238, 239 };
+static struct spear_muxreg i2c7_dis_sd_muxreg[] = {
+	{
+		.reg = PAD_FUNCTION_EN_2,
+		.mask = PMX_MCIRESETCF_MASK |
+			PMX_MCICS0CE_MASK,
+		.val = 0,
+	},
+};
+
+static struct spear_modemux i2c7_dis_sd_modemux[] = {
+	{
+		.muxregs = i2c7_dis_sd_muxreg,
+		.nmuxregs = ARRAY_SIZE(i2c7_dis_sd_muxreg),
+	},
+};
+
+static struct spear_pingroup i2c7_dis_sd_pingroup = {
+	.name = "i2c7_dis_sd_grp",
+	.pins = i2c7_dis_sd_pins,
+	.npins = ARRAY_SIZE(i2c7_dis_sd_pins),
+	.modemuxs = i2c7_dis_sd_modemux,
+	.nmodemuxs = ARRAY_SIZE(i2c7_dis_sd_modemux),
+};
+
+static const char *const i2c_6_7_grps[] = { "i2c6_dis_sd_grp",
+	"i2c7_dis_sd_grp", "i2c_6_7_dis_kbd_grp" };
+static struct spear_function i2c_6_7_function = {
+	.name = "i2c_6_7",
+	.groups = i2c_6_7_grps,
+	.ngroups = ARRAY_SIZE(i2c_6_7_grps),
+};
+
+/* Pad multiplexing for can0_dis_nor device */
+/* Muxed with NOR */
+static const unsigned can0_dis_nor_pins[] = { 56, 57 };
+static struct spear_muxreg can0_dis_nor_muxreg[] = {
+	{
+		.reg = PAD_FUNCTION_EN_0,
+		.mask = PMX_NFRSTPWDWN2_MASK,
+		.val = 0,
+	}, {
+		.reg = PAD_FUNCTION_EN_1,
+		.mask = PMX_NFRSTPWDWN3_MASK,
+		.val = 0,
+	},
+};
+
+static struct spear_modemux can0_dis_nor_modemux[] = {
+	{
+		.muxregs = can0_dis_nor_muxreg,
+		.nmuxregs = ARRAY_SIZE(can0_dis_nor_muxreg),
+	},
+};
+
+static struct spear_pingroup can0_dis_nor_pingroup = {
+	.name = "can0_dis_nor_grp",
+	.pins = can0_dis_nor_pins,
+	.npins = ARRAY_SIZE(can0_dis_nor_pins),
+	.modemuxs = can0_dis_nor_modemux,
+	.nmodemuxs = ARRAY_SIZE(can0_dis_nor_modemux),
+};
+
+/* Pad multiplexing for can0_dis_sd device */
+/* Muxed with SD/MMC */
+static const unsigned can0_dis_sd_pins[] = { 240, 241 };
+static struct spear_muxreg can0_dis_sd_muxreg[] = {
+	{
+		.reg = PAD_FUNCTION_EN_2,
+		.mask = PMX_MCICFINTR_MASK | PMX_MCIIORDY_MASK,
+		.val = 0,
+	},
+};
+
+static struct spear_modemux can0_dis_sd_modemux[] = {
+	{
+		.muxregs = can0_dis_sd_muxreg,
+		.nmuxregs = ARRAY_SIZE(can0_dis_sd_muxreg),
+	},
+};
+
+static struct spear_pingroup can0_dis_sd_pingroup = {
+	.name = "can0_dis_sd_grp",
+	.pins = can0_dis_sd_pins,
+	.npins = ARRAY_SIZE(can0_dis_sd_pins),
+	.modemuxs = can0_dis_sd_modemux,
+	.nmodemuxs = ARRAY_SIZE(can0_dis_sd_modemux),
+};
+
+static const char *const can0_grps[] = { "can0_dis_nor_grp", "can0_dis_sd_grp"
+};
+static struct spear_function can0_function = {
+	.name = "can0",
+	.groups = can0_grps,
+	.ngroups = ARRAY_SIZE(can0_grps),
+};
+
+/* Pad multiplexing for can1_dis_sd device */
+/* Muxed with SD/MMC */
+static const unsigned can1_dis_sd_pins[] = { 242, 243 };
+static struct spear_muxreg can1_dis_sd_muxreg[] = {
+	{
+		.reg = PAD_FUNCTION_EN_2,
+		.mask = PMX_MCICS1_MASK | PMX_MCIDMAACK_MASK,
+		.val = 0,
+	},
+};
+
+static struct spear_modemux can1_dis_sd_modemux[] = {
+	{
+		.muxregs = can1_dis_sd_muxreg,
+		.nmuxregs = ARRAY_SIZE(can1_dis_sd_muxreg),
+	},
+};
+
+static struct spear_pingroup can1_dis_sd_pingroup = {
+	.name = "can1_dis_sd_grp",
+	.pins = can1_dis_sd_pins,
+	.npins = ARRAY_SIZE(can1_dis_sd_pins),
+	.modemuxs = can1_dis_sd_modemux,
+	.nmodemuxs = ARRAY_SIZE(can1_dis_sd_modemux),
+};
+
+/* Pad multiplexing for can1_dis_kbd device */
+/* Muxed with KBD */
+static const unsigned can1_dis_kbd_pins[] = { 201, 202 };
+static struct spear_muxreg can1_dis_kbd_muxreg[] = {
+	{
+		.reg = PAD_FUNCTION_EN_1,
+		.mask = PMX_KBD_ROWCOL25_MASK,
+		.val = 0,
+	},
+};
+
+static struct spear_modemux can1_dis_kbd_modemux[] = {
+	{
+		.muxregs = can1_dis_kbd_muxreg,
+		.nmuxregs = ARRAY_SIZE(can1_dis_kbd_muxreg),
+	},
+};
+
+static struct spear_pingroup can1_dis_kbd_pingroup = {
+	.name = "can1_dis_kbd_grp",
+	.pins = can1_dis_kbd_pins,
+	.npins = ARRAY_SIZE(can1_dis_kbd_pins),
+	.modemuxs = can1_dis_kbd_modemux,
+	.nmodemuxs = ARRAY_SIZE(can1_dis_kbd_modemux),
+};
+
+static const char *const can1_grps[] = { "can1_dis_sd_grp", "can1_dis_kbd_grp"
+};
+static struct spear_function can1_function = {
+	.name = "can1",
+	.groups = can1_grps,
+	.ngroups = ARRAY_SIZE(can1_grps),
+};
+
+/* Pad multiplexing for pci device */
+static const unsigned pci_sata_pins[] = { 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 18,
+	19, 20, 21, 22, 23, 24, 25, 26, 27, 28, 29, 30, 31, 32, 33, 34, 35, 36,
+	37, 38, 39, 40, 41, 42, 43, 44, 45, 46, 47, 48, 49, 50, 51, 52, 53, 54,
+	55, 86, 87, 88, 89, 90, 91, 92, 93, 94, 95, 96, 97, 98, 99 };
+#define PCI_SATA_MUXREG				\
+	{					\
+		.reg = PAD_FUNCTION_EN_0,	\
+		.mask = PMX_MCI_DATA8_15_MASK,	\
+		.val = 0,			\
+	}, {					\
+		.reg = PAD_FUNCTION_EN_1,	\
+		.mask = PMX_PCI_REG1_MASK,	\
+		.val = 0,			\
+	}, {					\
+		.reg = PAD_FUNCTION_EN_2,	\
+		.mask = PMX_PCI_REG2_MASK,	\
+		.val = 0,			\
+	}
+
+/* pad multiplexing for pcie0 device */
+static struct spear_muxreg pcie0_muxreg[] = {
+	PCI_SATA_MUXREG,
+	{
+		.reg = PCIE_SATA_CFG,
+		.mask = PCIE_CFG_VAL(0),
+		.val = PCIE_CFG_VAL(0),
+	},
+};
+
+static struct spear_modemux pcie0_modemux[] = {
+	{
+		.muxregs = pcie0_muxreg,
+		.nmuxregs = ARRAY_SIZE(pcie0_muxreg),
+	},
+};
+
+static struct spear_pingroup pcie0_pingroup = {
+	.name = "pcie0_grp",
+	.pins = pci_sata_pins,
+	.npins = ARRAY_SIZE(pci_sata_pins),
+	.modemuxs = pcie0_modemux,
+	.nmodemuxs = ARRAY_SIZE(pcie0_modemux),
+};
+
+/* pad multiplexing for pcie1 device */
+static struct spear_muxreg pcie1_muxreg[] = {
+	PCI_SATA_MUXREG,
+	{
+		.reg = PCIE_SATA_CFG,
+		.mask = PCIE_CFG_VAL(1),
+		.val = PCIE_CFG_VAL(1),
+	},
+};
+
+static struct spear_modemux pcie1_modemux[] = {
+	{
+		.muxregs = pcie1_muxreg,
+		.nmuxregs = ARRAY_SIZE(pcie1_muxreg),
+	},
+};
+
+static struct spear_pingroup pcie1_pingroup = {
+	.name = "pcie1_grp",
+	.pins = pci_sata_pins,
+	.npins = ARRAY_SIZE(pci_sata_pins),
+	.modemuxs = pcie1_modemux,
+	.nmodemuxs = ARRAY_SIZE(pcie1_modemux),
+};
+
+/* pad multiplexing for pcie2 device */
+static struct spear_muxreg pcie2_muxreg[] = {
+	PCI_SATA_MUXREG,
+	{
+		.reg = PCIE_SATA_CFG,
+		.mask = PCIE_CFG_VAL(2),
+		.val = PCIE_CFG_VAL(2),
+	},
+};
+
+static struct spear_modemux pcie2_modemux[] = {
+	{
+		.muxregs = pcie2_muxreg,
+		.nmuxregs = ARRAY_SIZE(pcie2_muxreg),
+	},
+};
+
+static struct spear_pingroup pcie2_pingroup = {
+	.name = "pcie2_grp",
+	.pins = pci_sata_pins,
+	.npins = ARRAY_SIZE(pci_sata_pins),
+	.modemuxs = pcie2_modemux,
+	.nmodemuxs = ARRAY_SIZE(pcie2_modemux),
+};
+
+static const char *const pci_grps[] = { "pcie0_grp", "pcie1_grp", "pcie2_grp" };
+static struct spear_function pci_function = {
+	.name = "pci",
+	.groups = pci_grps,
+	.ngroups = ARRAY_SIZE(pci_grps),
+};
+
+/* pad multiplexing for sata0 device */
+static struct spear_muxreg sata0_muxreg[] = {
+	PCI_SATA_MUXREG,
+	{
+		.reg = PCIE_SATA_CFG,
+		.mask = SATA_CFG_VAL(0),
+		.val = SATA_CFG_VAL(0),
+	},
+};
+
+static struct spear_modemux sata0_modemux[] = {
+	{
+		.muxregs = sata0_muxreg,
+		.nmuxregs = ARRAY_SIZE(sata0_muxreg),
+	},
+};
+
+static struct spear_pingroup sata0_pingroup = {
+	.name = "sata0_grp",
+	.pins = pci_sata_pins,
+	.npins = ARRAY_SIZE(pci_sata_pins),
+	.modemuxs = sata0_modemux,
+	.nmodemuxs = ARRAY_SIZE(sata0_modemux),
+};
+
+/* pad multiplexing for sata1 device */
+static struct spear_muxreg sata1_muxreg[] = {
+	PCI_SATA_MUXREG,
+	{
+		.reg = PCIE_SATA_CFG,
+		.mask = SATA_CFG_VAL(1),
+		.val = SATA_CFG_VAL(1),
+	},
+};
+
+static struct spear_modemux sata1_modemux[] = {
+	{
+		.muxregs = sata1_muxreg,
+		.nmuxregs = ARRAY_SIZE(sata1_muxreg),
+	},
+};
+
+static struct spear_pingroup sata1_pingroup = {
+	.name = "sata1_grp",
+	.pins = pci_sata_pins,
+	.npins = ARRAY_SIZE(pci_sata_pins),
+	.modemuxs = sata1_modemux,
+	.nmodemuxs = ARRAY_SIZE(sata1_modemux),
+};
+
+/* pad multiplexing for sata2 device */
+static struct spear_muxreg sata2_muxreg[] = {
+	PCI_SATA_MUXREG,
+	{
+		.reg = PCIE_SATA_CFG,
+		.mask = SATA_CFG_VAL(2),
+		.val = SATA_CFG_VAL(2),
+	},
+};
+
+static struct spear_modemux sata2_modemux[] = {
+	{
+		.muxregs = sata2_muxreg,
+		.nmuxregs = ARRAY_SIZE(sata2_muxreg),
+	},
+};
+
+static struct spear_pingroup sata2_pingroup = {
+	.name = "sata2_grp",
+	.pins = pci_sata_pins,
+	.npins = ARRAY_SIZE(pci_sata_pins),
+	.modemuxs = sata2_modemux,
+	.nmodemuxs = ARRAY_SIZE(sata2_modemux),
+};
+
+static const char *const sata_grps[] = { "sata0_grp", "sata1_grp", "sata2_grp"
+};
+static struct spear_function sata_function = {
+	.name = "sata",
+	.groups = sata_grps,
+	.ngroups = ARRAY_SIZE(sata_grps),
+};
+
+/* Pad multiplexing for ssp1_dis_kbd device */
+static const unsigned ssp1_dis_kbd_pins[] = { 203, 204, 205, 206 };
+static struct spear_muxreg ssp1_dis_kbd_muxreg[] = {
+	{
+		.reg = PAD_FUNCTION_EN_1,
+		.mask = PMX_KBD_ROWCOL25_MASK | PMX_KBD_COL1_MASK |
+			PMX_KBD_COL0_MASK | PMX_NFIO8_15_MASK | PMX_NFCE1_MASK |
+			PMX_NFCE2_MASK,
+		.val = 0,
+	},
+};
+
+static struct spear_modemux ssp1_dis_kbd_modemux[] = {
+	{
+		.muxregs = ssp1_dis_kbd_muxreg,
+		.nmuxregs = ARRAY_SIZE(ssp1_dis_kbd_muxreg),
+	},
+};
+
+static struct spear_pingroup ssp1_dis_kbd_pingroup = {
+	.name = "ssp1_dis_kbd_grp",
+	.pins = ssp1_dis_kbd_pins,
+	.npins = ARRAY_SIZE(ssp1_dis_kbd_pins),
+	.modemuxs = ssp1_dis_kbd_modemux,
+	.nmodemuxs = ARRAY_SIZE(ssp1_dis_kbd_modemux),
+};
+
+/* Pad multiplexing for ssp1_dis_sd device */
+static const unsigned ssp1_dis_sd_pins[] = { 224, 226, 227, 228 };
+static struct spear_muxreg ssp1_dis_sd_muxreg[] = {
+	{
+		.reg = PAD_FUNCTION_EN_2,
+		.mask = PMX_MCIADDR0ALE_MASK | PMX_MCIADDR2_MASK |
+			PMX_MCICECF_MASK | PMX_MCICEXD_MASK,
+		.val = 0,
+	},
+};
+
+static struct spear_modemux ssp1_dis_sd_modemux[] = {
+	{
+		.muxregs = ssp1_dis_sd_muxreg,
+		.nmuxregs = ARRAY_SIZE(ssp1_dis_sd_muxreg),
+	},
+};
+
+static struct spear_pingroup ssp1_dis_sd_pingroup = {
+	.name = "ssp1_dis_sd_grp",
+	.pins = ssp1_dis_sd_pins,
+	.npins = ARRAY_SIZE(ssp1_dis_sd_pins),
+	.modemuxs = ssp1_dis_sd_modemux,
+	.nmodemuxs = ARRAY_SIZE(ssp1_dis_sd_modemux),
+};
+
+static const char *const ssp1_grps[] = { "ssp1_dis_kbd_grp",
+	"ssp1_dis_sd_grp" };
+static struct spear_function ssp1_function = {
+	.name = "ssp1",
+	.groups = ssp1_grps,
+	.ngroups = ARRAY_SIZE(ssp1_grps),
+};
+
+/* Pad multiplexing for gpt64 device */
+static const unsigned gpt64_pins[] = { 230, 231, 232, 245 };
+static struct spear_muxreg gpt64_muxreg[] = {
+	{
+		.reg = PAD_FUNCTION_EN_2,
+		.mask = PMX_MCICDCF1_MASK | PMX_MCICDCF2_MASK | PMX_MCICDXD_MASK
+			| PMX_MCILEDS_MASK,
+		.val = 0,
+	},
+};
+
+static struct spear_modemux gpt64_modemux[] = {
+	{
+		.muxregs = gpt64_muxreg,
+		.nmuxregs = ARRAY_SIZE(gpt64_muxreg),
+	},
+};
+
+static struct spear_pingroup gpt64_pingroup = {
+	.name = "gpt64_grp",
+	.pins = gpt64_pins,
+	.npins = ARRAY_SIZE(gpt64_pins),
+	.modemuxs = gpt64_modemux,
+	.nmodemuxs = ARRAY_SIZE(gpt64_modemux),
+};
+
+static const char *const gpt64_grps[] = { "gpt64_grp" };
+static struct spear_function gpt64_function = {
+	.name = "gpt64",
+	.groups = gpt64_grps,
+	.ngroups = ARRAY_SIZE(gpt64_grps),
+};
+
+/* pingroups */
+static struct spear_pingroup *spear1310_pingroups[] = {
+	&i2c0_pingroup,
+	&ssp0_pingroup,
+	&i2s0_pingroup,
+	&i2s1_pingroup,
+	&clcd_pingroup,
+	&clcd_high_res_pingroup,
+	&arm_gpio_pingroup,
+	&smi_2_chips_pingroup,
+	&smi_4_chips_pingroup,
+	&gmii_pingroup,
+	&rgmii_pingroup,
+	&smii_0_1_2_pingroup,
+	&ras_mii_txclk_pingroup,
+	&nand_8bit_pingroup,
+	&nand_16bit_pingroup,
+	&nand_4_chips_pingroup,
+	&keyboard_6x6_pingroup,
+	&keyboard_rowcol6_8_pingroup,
+	&uart0_pingroup,
+	&uart0_modem_pingroup,
+	&gpt0_tmr0_pingroup,
+	&gpt0_tmr1_pingroup,
+	&gpt1_tmr0_pingroup,
+	&gpt1_tmr1_pingroup,
+	&sdhci_pingroup,
+	&cf_pingroup,
+	&xd_pingroup,
+	&touch_xy_pingroup,
+	&ssp0_cs0_pingroup,
+	&ssp0_cs1_2_pingroup,
+	&uart_1_dis_i2c_pingroup,
+	&uart_1_dis_sd_pingroup,
+	&uart_2_3_pingroup,
+	&uart_4_pingroup,
+	&uart_5_pingroup,
+	&rs485_0_1_tdm_0_1_pingroup,
+	&i2c_1_2_pingroup,
+	&i2c3_dis_smi_clcd_pingroup,
+	&i2c3_dis_sd_i2s0_pingroup,
+	&i2c_4_5_dis_smi_pingroup,
+	&i2c4_dis_sd_pingroup,
+	&i2c5_dis_sd_pingroup,
+	&i2c_6_7_dis_kbd_pingroup,
+	&i2c6_dis_sd_pingroup,
+	&i2c7_dis_sd_pingroup,
+	&can0_dis_nor_pingroup,
+	&can0_dis_sd_pingroup,
+	&can1_dis_sd_pingroup,
+	&can1_dis_kbd_pingroup,
+	&pcie0_pingroup,
+	&pcie1_pingroup,
+	&pcie2_pingroup,
+	&sata0_pingroup,
+	&sata1_pingroup,
+	&sata2_pingroup,
+	&ssp1_dis_kbd_pingroup,
+	&ssp1_dis_sd_pingroup,
+	&gpt64_pingroup,
+};
+
+/* functions */
+static struct spear_function *spear1310_functions[] = {
+	&i2c0_function,
+	&ssp0_function,
+	&i2s0_function,
+	&i2s1_function,
+	&clcd_function,
+	&arm_gpio_function,
+	&smi_function,
+	&gmii_function,
+	&rgmii_function,
+	&smii_0_1_2_function,
+	&ras_mii_txclk_function,
+	&nand_function,
+	&keyboard_function,
+	&uart0_function,
+	&gpt0_function,
+	&gpt1_function,
+	&sdhci_function,
+	&cf_function,
+	&xd_function,
+	&touch_xy_function,
+	&uart1_function,
+	&uart2_3_function,
+	&uart4_function,
+	&uart5_function,
+	&rs485_0_1_tdm_0_1_function,
+	&i2c_1_2_function,
+	&i2c3_unction,
+	&i2c_4_5_function,
+	&i2c_6_7_function,
+	&can0_function,
+	&can1_function,
+	&pci_function,
+	&sata_function,
+	&ssp1_function,
+	&gpt64_function,
+};
+
+static struct spear_pinctrl_machdata spear1310_machdata = {
+	.pins = spear1310_pins,
+	.npins = ARRAY_SIZE(spear1310_pins),
+	.groups = spear1310_pingroups,
+	.ngroups = ARRAY_SIZE(spear1310_pingroups),
+	.functions = spear1310_functions,
+	.nfunctions = ARRAY_SIZE(spear1310_functions),
+	.modes_supported = false,
+};
+
+static struct of_device_id spear1310_pinctrl_of_match[] __devinitdata = {
+	{
+		.compatible = "st,spear1310-pinmux",
+	},
+	{},
+};
+
+static int __devinit spear1310_pinctrl_probe(struct platform_device *pdev)
+{
+	return spear_pinctrl_probe(pdev, &spear1310_machdata);
+}
+
+static int __devexit spear1310_pinctrl_remove(struct platform_device *pdev)
+{
+	return spear_pinctrl_remove(pdev);
+}
+
+static struct platform_driver spear1310_pinctrl_driver = {
+	.driver = {
+		.name = DRIVER_NAME,
+		.owner = THIS_MODULE,
+		.of_match_table = spear1310_pinctrl_of_match,
+	},
+	.probe = spear1310_pinctrl_probe,
+	.remove = __devexit_p(spear1310_pinctrl_remove),
+};
+
+static int __init spear1310_pinctrl_init(void)
+{
+	return platform_driver_register(&spear1310_pinctrl_driver);
+}
+arch_initcall(spear1310_pinctrl_init);
+
+static void __exit spear1310_pinctrl_exit(void)
+{
+	platform_driver_unregister(&spear1310_pinctrl_driver);
+}
+module_exit(spear1310_pinctrl_exit);
+
+MODULE_AUTHOR("Viresh Kumar <viresh.kumar@st.com>");
+MODULE_DESCRIPTION("ST Microelectronics SPEAr1310 pinctrl driver");
+MODULE_LICENSE("GPL v2");
+MODULE_DEVICE_TABLE(of, spear1310_pinctrl_of_match);
diff --git a/drivers/pinctrl/spear/pinctrl-spear1340.c b/drivers/pinctrl/spear/pinctrl-spear1340.c
new file mode 100644
index 00000000000..a8ab2a6f51b
--- /dev/null
+++ b/drivers/pinctrl/spear/pinctrl-spear1340.c
@@ -0,0 +1,1989 @@
+/*
+ * Driver for the ST Microelectronics SPEAr1340 pinmux
+ *
+ * Copyright (C) 2012 ST Microelectronics
+ * Viresh Kumar <viresh.kumar@st.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/err.h>
+#include <linux/init.h>
+#include <linux/module.h>
+#include <linux/of_device.h>
+#include <linux/platform_device.h>
+#include "pinctrl-spear.h"
+
+#define DRIVER_NAME "spear1340-pinmux"
+
+/* pins */
+static const struct pinctrl_pin_desc spear1340_pins[] = {
+	SPEAR_PIN_0_TO_101,
+	SPEAR_PIN_102_TO_245,
+	PINCTRL_PIN(246, "PLGPIO246"),
+	PINCTRL_PIN(247, "PLGPIO247"),
+	PINCTRL_PIN(248, "PLGPIO248"),
+	PINCTRL_PIN(249, "PLGPIO249"),
+	PINCTRL_PIN(250, "PLGPIO250"),
+	PINCTRL_PIN(251, "PLGPIO251"),
+};
+
+/* In SPEAr1340 there are two levels of pad muxing */
+/* - pads as gpio OR peripherals */
+#define PAD_FUNCTION_EN_1			0x668
+#define PAD_FUNCTION_EN_2			0x66C
+#define PAD_FUNCTION_EN_3			0x670
+#define PAD_FUNCTION_EN_4			0x674
+#define PAD_FUNCTION_EN_5			0x690
+#define PAD_FUNCTION_EN_6			0x694
+#define PAD_FUNCTION_EN_7			0x698
+#define PAD_FUNCTION_EN_8			0x69C
+
+/* - If peripherals, then primary OR alternate peripheral */
+#define PAD_SHARED_IP_EN_1			0x6A0
+#define PAD_SHARED_IP_EN_2			0x6A4
+
+/*
+ * Macro's for first level of pmx - pads as gpio OR peripherals. There are 8
+ * registers with 32 bits each for handling gpio pads, register 8 has only 26
+ * relevant bits.
+ */
+/* macro's for making pads as gpio's */
+#define PADS_AS_GPIO_REG0_MASK			0xFFFFFFFE
+#define PADS_AS_GPIO_REGS_MASK			0xFFFFFFFF
+#define PADS_AS_GPIO_REG7_MASK			0x07FFFFFF
+
+/* macro's for making pads as peripherals */
+#define FSMC_16_BIT_AND_KBD_ROW_COL_REG0_MASK	0x00000FFE
+#define UART0_ENH_AND_GPT_REG0_MASK		0x0003F000
+#define PWM1_AND_KBD_COL5_REG0_MASK		0x00040000
+#define I2C1_REG0_MASK				0x01080000
+#define SPDIF_IN_REG0_MASK			0x00100000
+#define PWM2_AND_GPT0_TMR0_CPT_REG0_MASK	0x00400000
+#define PWM3_AND_GPT0_TMR1_CLK_REG0_MASK	0x00800000
+#define PWM0_AND_SSP0_CS1_REG0_MASK		0x02000000
+#define VIP_AND_CAM3_REG0_MASK			0xFC200000
+#define VIP_AND_CAM3_REG1_MASK			0x0000000F
+#define VIP_REG1_MASK				0x00001EF0
+#define VIP_AND_CAM2_REG1_MASK			0x007FE100
+#define VIP_AND_CAM1_REG1_MASK			0xFF800000
+#define VIP_AND_CAM1_REG2_MASK			0x00000003
+#define VIP_AND_CAM0_REG2_MASK			0x00001FFC
+#define SMI_REG2_MASK				0x0021E000
+#define SSP0_REG2_MASK				0x001E0000
+#define TS_AND_SSP0_CS2_REG2_MASK		0x00400000
+#define UART0_REG2_MASK				0x01800000
+#define UART1_REG2_MASK				0x06000000
+#define I2S_IN_REG2_MASK			0xF8000000
+#define DEVS_GRP_AND_MIPHY_DBG_REG3_MASK	0x000001FE
+#define I2S_OUT_REG3_MASK			0x000001EF
+#define I2S_IN_REG3_MASK			0x00000010
+#define GMAC_REG3_MASK				0xFFFFFE00
+#define GMAC_REG4_MASK				0x0000001F
+#define DEVS_GRP_AND_MIPHY_DBG_REG4_MASK	0x7FFFFF20
+#define SSP0_CS3_REG4_MASK			0x00000020
+#define I2C0_REG4_MASK				0x000000C0
+#define CEC0_REG4_MASK				0x00000100
+#define CEC1_REG4_MASK				0x00000200
+#define SPDIF_OUT_REG4_MASK			0x00000400
+#define CLCD_REG4_MASK				0x7FFFF800
+#define CLCD_AND_ARM_TRACE_REG4_MASK		0x80000000
+#define CLCD_AND_ARM_TRACE_REG5_MASK		0xFFFFFFFF
+#define CLCD_AND_ARM_TRACE_REG6_MASK		0x00000001
+#define FSMC_PNOR_AND_MCIF_REG6_MASK		0x073FFFFE
+#define MCIF_REG6_MASK				0xF8C00000
+#define MCIF_REG7_MASK				0x000043FF
+#define FSMC_8BIT_REG7_MASK			0x07FFBC00
+
+/* other registers */
+#define PERIP_CFG				0x42C
+	/* PERIP_CFG register masks */
+	#define SSP_CS_CTL_HW			0
+	#define SSP_CS_CTL_SW			1
+	#define SSP_CS_CTL_MASK			1
+	#define SSP_CS_CTL_SHIFT		21
+	#define SSP_CS_VAL_MASK			1
+	#define SSP_CS_VAL_SHIFT		20
+	#define SSP_CS_SEL_CS0			0
+	#define SSP_CS_SEL_CS1			1
+	#define SSP_CS_SEL_CS2			2
+	#define SSP_CS_SEL_MASK			3
+	#define SSP_CS_SEL_SHIFT		18
+
+	#define I2S_CHNL_2_0			(0)
+	#define I2S_CHNL_3_1			(1)
+	#define I2S_CHNL_5_1			(2)
+	#define I2S_CHNL_7_1			(3)
+	#define I2S_CHNL_PLAY_SHIFT		(4)
+	#define I2S_CHNL_PLAY_MASK		(3 << 4)
+	#define I2S_CHNL_REC_SHIFT		(6)
+	#define I2S_CHNL_REC_MASK		(3 << 6)
+
+	#define SPDIF_OUT_ENB_MASK		(1 << 2)
+	#define SPDIF_OUT_ENB_SHIFT		2
+
+	#define MCIF_SEL_SD			1
+	#define MCIF_SEL_CF			2
+	#define MCIF_SEL_XD			3
+	#define MCIF_SEL_MASK			3
+	#define MCIF_SEL_SHIFT			0
+
+#define GMAC_CLK_CFG				0x248
+	#define GMAC_PHY_IF_GMII_VAL		(0 << 3)
+	#define GMAC_PHY_IF_RGMII_VAL		(1 << 3)
+	#define GMAC_PHY_IF_SGMII_VAL		(2 << 3)
+	#define GMAC_PHY_IF_RMII_VAL		(4 << 3)
+	#define GMAC_PHY_IF_SEL_MASK		(7 << 3)
+	#define GMAC_PHY_INPUT_ENB_VAL		0
+	#define GMAC_PHY_SYNT_ENB_VAL		1
+	#define GMAC_PHY_CLK_MASK		1
+	#define GMAC_PHY_CLK_SHIFT		2
+	#define GMAC_PHY_125M_PAD_VAL		0
+	#define GMAC_PHY_PLL2_VAL		1
+	#define GMAC_PHY_OSC3_VAL		2
+	#define GMAC_PHY_INPUT_CLK_MASK		3
+	#define GMAC_PHY_INPUT_CLK_SHIFT	0
+
+#define PCIE_SATA_CFG				0x424
+	/* PCIE CFG MASks */
+	#define PCIE_CFG_DEVICE_PRESENT		(1 << 11)
+	#define PCIE_CFG_POWERUP_RESET		(1 << 10)
+	#define PCIE_CFG_CORE_CLK_EN		(1 << 9)
+	#define PCIE_CFG_AUX_CLK_EN		(1 << 8)
+	#define SATA_CFG_TX_CLK_EN		(1 << 4)
+	#define SATA_CFG_RX_CLK_EN		(1 << 3)
+	#define SATA_CFG_POWERUP_RESET		(1 << 2)
+	#define SATA_CFG_PM_CLK_EN		(1 << 1)
+	#define PCIE_SATA_SEL_PCIE		(0)
+	#define PCIE_SATA_SEL_SATA		(1)
+	#define SATA_PCIE_CFG_MASK		0xF1F
+	#define PCIE_CFG_VAL	(PCIE_SATA_SEL_PCIE | PCIE_CFG_AUX_CLK_EN | \
+				PCIE_CFG_CORE_CLK_EN | PCIE_CFG_POWERUP_RESET |\
+				PCIE_CFG_DEVICE_PRESENT)
+	#define SATA_CFG_VAL	(PCIE_SATA_SEL_SATA | SATA_CFG_PM_CLK_EN | \
+				SATA_CFG_POWERUP_RESET | SATA_CFG_RX_CLK_EN | \
+				SATA_CFG_TX_CLK_EN)
+
+/* Macro's for second level of pmx - pads as primary OR alternate peripheral */
+/* Write 0 to enable FSMC_16_BIT */
+#define KBD_ROW_COL_MASK			(1 << 0)
+
+/* Write 0 to enable UART0_ENH */
+#define GPT_MASK				(1 << 1) /* Only clk & cpt */
+
+/* Write 0 to enable PWM1 */
+#define KBD_COL5_MASK				(1 << 2)
+
+/* Write 0 to enable PWM2 */
+#define GPT0_TMR0_CPT_MASK			(1 << 3) /* Only clk & cpt */
+
+/* Write 0 to enable PWM3 */
+#define GPT0_TMR1_CLK_MASK			(1 << 4) /* Only clk & cpt */
+
+/* Write 0 to enable PWM0 */
+#define SSP0_CS1_MASK				(1 << 5)
+
+/* Write 0 to enable VIP */
+#define CAM3_MASK				(1 << 6)
+
+/* Write 0 to enable VIP */
+#define CAM2_MASK				(1 << 7)
+
+/* Write 0 to enable VIP */
+#define CAM1_MASK				(1 << 8)
+
+/* Write 0 to enable VIP */
+#define CAM0_MASK				(1 << 9)
+
+/* Write 0 to enable TS */
+#define SSP0_CS2_MASK				(1 << 10)
+
+/* Write 0 to enable FSMC PNOR */
+#define MCIF_MASK				(1 << 11)
+
+/* Write 0 to enable CLCD */
+#define ARM_TRACE_MASK				(1 << 12)
+
+/* Write 0 to enable I2S, SSP0_CS2, CEC0, 1, SPDIF out, CLCD */
+#define MIPHY_DBG_MASK				(1 << 13)
+
+/*
+ * Pad multiplexing for making all pads as gpio's. This is done to override the
+ * values passed from bootloader and start from scratch.
+ */
+static const unsigned pads_as_gpio_pins[] = { 251 };
+static struct spear_muxreg pads_as_gpio_muxreg[] = {
+	{
+		.reg = PAD_FUNCTION_EN_1,
+		.mask = PADS_AS_GPIO_REG0_MASK,
+		.val = 0x0,
+	}, {
+		.reg = PAD_FUNCTION_EN_2,
+		.mask = PADS_AS_GPIO_REGS_MASK,
+		.val = 0x0,
+	}, {
+		.reg = PAD_FUNCTION_EN_3,
+		.mask = PADS_AS_GPIO_REGS_MASK,
+		.val = 0x0,
+	}, {
+		.reg = PAD_FUNCTION_EN_4,
+		.mask = PADS_AS_GPIO_REGS_MASK,
+		.val = 0x0,
+	}, {
+		.reg = PAD_FUNCTION_EN_5,
+		.mask = PADS_AS_GPIO_REGS_MASK,
+		.val = 0x0,
+	}, {
+		.reg = PAD_FUNCTION_EN_6,
+		.mask = PADS_AS_GPIO_REGS_MASK,
+		.val = 0x0,
+	}, {
+		.reg = PAD_FUNCTION_EN_7,
+		.mask = PADS_AS_GPIO_REGS_MASK,
+		.val = 0x0,
+	}, {
+		.reg = PAD_FUNCTION_EN_8,
+		.mask = PADS_AS_GPIO_REG7_MASK,
+		.val = 0x0,
+	},
+};
+
+static struct spear_modemux pads_as_gpio_modemux[] = {
+	{
+		.muxregs = pads_as_gpio_muxreg,
+		.nmuxregs = ARRAY_SIZE(pads_as_gpio_muxreg),
+	},
+};
+
+static struct spear_pingroup pads_as_gpio_pingroup = {
+	.name = "pads_as_gpio_grp",
+	.pins = pads_as_gpio_pins,
+	.npins = ARRAY_SIZE(pads_as_gpio_pins),
+	.modemuxs = pads_as_gpio_modemux,
+	.nmodemuxs = ARRAY_SIZE(pads_as_gpio_modemux),
+};
+
+static const char *const pads_as_gpio_grps[] = { "pads_as_gpio_grp" };
+static struct spear_function pads_as_gpio_function = {
+	.name = "pads_as_gpio",
+	.groups = pads_as_gpio_grps,
+	.ngroups = ARRAY_SIZE(pads_as_gpio_grps),
+};
+
+/* Pad multiplexing for fsmc_8bit device */
+static const unsigned fsmc_8bit_pins[] = { 233, 234, 235, 236, 238, 239, 240,
+	241, 242, 243, 244, 245, 246, 247, 248, 249 };
+static struct spear_muxreg fsmc_8bit_muxreg[] = {
+	{
+		.reg = PAD_FUNCTION_EN_8,
+		.mask = FSMC_8BIT_REG7_MASK,
+		.val = FSMC_8BIT_REG7_MASK,
+	}
+};
+
+static struct spear_modemux fsmc_8bit_modemux[] = {
+	{
+		.muxregs = fsmc_8bit_muxreg,
+		.nmuxregs = ARRAY_SIZE(fsmc_8bit_muxreg),
+	},
+};
+
+static struct spear_pingroup fsmc_8bit_pingroup = {
+	.name = "fsmc_8bit_grp",
+	.pins = fsmc_8bit_pins,
+	.npins = ARRAY_SIZE(fsmc_8bit_pins),
+	.modemuxs = fsmc_8bit_modemux,
+	.nmodemuxs = ARRAY_SIZE(fsmc_8bit_modemux),
+};
+
+/* Pad multiplexing for fsmc_16bit device */
+static const unsigned fsmc_16bit_pins[] = { 0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10 };
+static struct spear_muxreg fsmc_16bit_muxreg[] = {
+	{
+		.reg = PAD_SHARED_IP_EN_1,
+		.mask = KBD_ROW_COL_MASK,
+		.val = 0,
+	}, {
+		.reg = PAD_FUNCTION_EN_1,
+		.mask = FSMC_16_BIT_AND_KBD_ROW_COL_REG0_MASK,
+		.val = FSMC_16_BIT_AND_KBD_ROW_COL_REG0_MASK,
+	},
+};
+
+static struct spear_modemux fsmc_16bit_modemux[] = {
+	{
+		.muxregs = fsmc_16bit_muxreg,
+		.nmuxregs = ARRAY_SIZE(fsmc_16bit_muxreg),
+	},
+};
+
+static struct spear_pingroup fsmc_16bit_pingroup = {
+	.name = "fsmc_16bit_grp",
+	.pins = fsmc_16bit_pins,
+	.npins = ARRAY_SIZE(fsmc_16bit_pins),
+	.modemuxs = fsmc_16bit_modemux,
+	.nmodemuxs = ARRAY_SIZE(fsmc_16bit_modemux),
+};
+
+/* pad multiplexing for fsmc_pnor device */
+static const unsigned fsmc_pnor_pins[] = { 192, 193, 194, 195, 196, 197, 198,
+	199, 200, 201, 202, 203, 204, 205, 206, 207, 208, 209, 210, 211, 212,
+	215, 216, 217 };
+static struct spear_muxreg fsmc_pnor_muxreg[] = {
+	{
+		.reg = PAD_SHARED_IP_EN_1,
+		.mask = MCIF_MASK,
+		.val = 0,
+	}, {
+		.reg = PAD_FUNCTION_EN_7,
+		.mask = FSMC_PNOR_AND_MCIF_REG6_MASK,
+		.val = FSMC_PNOR_AND_MCIF_REG6_MASK,
+	},
+};
+
+static struct spear_modemux fsmc_pnor_modemux[] = {
+	{
+		.muxregs = fsmc_pnor_muxreg,
+		.nmuxregs = ARRAY_SIZE(fsmc_pnor_muxreg),
+	},
+};
+
+static struct spear_pingroup fsmc_pnor_pingroup = {
+	.name = "fsmc_pnor_grp",
+	.pins = fsmc_pnor_pins,
+	.npins = ARRAY_SIZE(fsmc_pnor_pins),
+	.modemuxs = fsmc_pnor_modemux,
+	.nmodemuxs = ARRAY_SIZE(fsmc_pnor_modemux),
+};
+
+static const char *const fsmc_grps[] = { "fsmc_8bit_grp", "fsmc_16bit_grp",
+	"fsmc_pnor_grp" };
+static struct spear_function fsmc_function = {
+	.name = "fsmc",
+	.groups = fsmc_grps,
+	.ngroups = ARRAY_SIZE(fsmc_grps),
+};
+
+/* pad multiplexing for keyboard rows-cols device */
+static const unsigned keyboard_row_col_pins[] = { 0, 1, 2, 3, 4, 5, 6, 7, 8, 9,
+	10 };
+static struct spear_muxreg keyboard_row_col_muxreg[] = {
+	{
+		.reg = PAD_SHARED_IP_EN_1,
+		.mask = KBD_ROW_COL_MASK,
+		.val = KBD_ROW_COL_MASK,
+	}, {
+		.reg = PAD_FUNCTION_EN_1,
+		.mask = FSMC_16_BIT_AND_KBD_ROW_COL_REG0_MASK,
+		.val = FSMC_16_BIT_AND_KBD_ROW_COL_REG0_MASK,
+	},
+};
+
+static struct spear_modemux keyboard_row_col_modemux[] = {
+	{
+		.muxregs = keyboard_row_col_muxreg,
+		.nmuxregs = ARRAY_SIZE(keyboard_row_col_muxreg),
+	},
+};
+
+static struct spear_pingroup keyboard_row_col_pingroup = {
+	.name = "keyboard_row_col_grp",
+	.pins = keyboard_row_col_pins,
+	.npins = ARRAY_SIZE(keyboard_row_col_pins),
+	.modemuxs = keyboard_row_col_modemux,
+	.nmodemuxs = ARRAY_SIZE(keyboard_row_col_modemux),
+};
+
+/* pad multiplexing for keyboard col5 device */
+static const unsigned keyboard_col5_pins[] = { 17 };
+static struct spear_muxreg keyboard_col5_muxreg[] = {
+	{
+		.reg = PAD_SHARED_IP_EN_1,
+		.mask = KBD_COL5_MASK,
+		.val = KBD_COL5_MASK,
+	}, {
+		.reg = PAD_FUNCTION_EN_1,
+		.mask = PWM1_AND_KBD_COL5_REG0_MASK,
+		.val = PWM1_AND_KBD_COL5_REG0_MASK,
+	},
+};
+
+static struct spear_modemux keyboard_col5_modemux[] = {
+	{
+		.muxregs = keyboard_col5_muxreg,
+		.nmuxregs = ARRAY_SIZE(keyboard_col5_muxreg),
+	},
+};
+
+static struct spear_pingroup keyboard_col5_pingroup = {
+	.name = "keyboard_col5_grp",
+	.pins = keyboard_col5_pins,
+	.npins = ARRAY_SIZE(keyboard_col5_pins),
+	.modemuxs = keyboard_col5_modemux,
+	.nmodemuxs = ARRAY_SIZE(keyboard_col5_modemux),
+};
+
+static const char *const keyboard_grps[] = { "keyboard_row_col_grp",
+	"keyboard_col5_grp" };
+static struct spear_function keyboard_function = {
+	.name = "keyboard",
+	.groups = keyboard_grps,
+	.ngroups = ARRAY_SIZE(keyboard_grps),
+};
+
+/* pad multiplexing for spdif_in device */
+static const unsigned spdif_in_pins[] = { 19 };
+static struct spear_muxreg spdif_in_muxreg[] = {
+	{
+		.reg = PAD_FUNCTION_EN_1,
+		.mask = SPDIF_IN_REG0_MASK,
+		.val = SPDIF_IN_REG0_MASK,
+	},
+};
+
+static struct spear_modemux spdif_in_modemux[] = {
+	{
+		.muxregs = spdif_in_muxreg,
+		.nmuxregs = ARRAY_SIZE(spdif_in_muxreg),
+	},
+};
+
+static struct spear_pingroup spdif_in_pingroup = {
+	.name = "spdif_in_grp",
+	.pins = spdif_in_pins,
+	.npins = ARRAY_SIZE(spdif_in_pins),
+	.modemuxs = spdif_in_modemux,
+	.nmodemuxs = ARRAY_SIZE(spdif_in_modemux),
+};
+
+static const char *const spdif_in_grps[] = { "spdif_in_grp" };
+static struct spear_function spdif_in_function = {
+	.name = "spdif_in",
+	.groups = spdif_in_grps,
+	.ngroups = ARRAY_SIZE(spdif_in_grps),
+};
+
+/* pad multiplexing for spdif_out device */
+static const unsigned spdif_out_pins[] = { 137 };
+static struct spear_muxreg spdif_out_muxreg[] = {
+	{
+		.reg = PAD_FUNCTION_EN_5,
+		.mask = SPDIF_OUT_REG4_MASK,
+		.val = SPDIF_OUT_REG4_MASK,
+	}, {
+		.reg = PERIP_CFG,
+		.mask = SPDIF_OUT_ENB_MASK,
+		.val = SPDIF_OUT_ENB_MASK,
+	}
+};
+
+static struct spear_modemux spdif_out_modemux[] = {
+	{
+		.muxregs = spdif_out_muxreg,
+		.nmuxregs = ARRAY_SIZE(spdif_out_muxreg),
+	},
+};
+
+static struct spear_pingroup spdif_out_pingroup = {
+	.name = "spdif_out_grp",
+	.pins = spdif_out_pins,
+	.npins = ARRAY_SIZE(spdif_out_pins),
+	.modemuxs = spdif_out_modemux,
+	.nmodemuxs = ARRAY_SIZE(spdif_out_modemux),
+};
+
+static const char *const spdif_out_grps[] = { "spdif_out_grp" };
+static struct spear_function spdif_out_function = {
+	.name = "spdif_out",
+	.groups = spdif_out_grps,
+	.ngroups = ARRAY_SIZE(spdif_out_grps),
+};
+
+/* pad multiplexing for gpt_0_1 device */
+static const unsigned gpt_0_1_pins[] = { 11, 12, 13, 14, 15, 16, 21, 22 };
+static struct spear_muxreg gpt_0_1_muxreg[] = {
+	{
+		.reg = PAD_SHARED_IP_EN_1,
+		.mask = GPT_MASK | GPT0_TMR0_CPT_MASK | GPT0_TMR1_CLK_MASK,
+		.val = GPT_MASK | GPT0_TMR0_CPT_MASK | GPT0_TMR1_CLK_MASK,
+	}, {
+		.reg = PAD_FUNCTION_EN_1,
+		.mask = UART0_ENH_AND_GPT_REG0_MASK |
+			PWM2_AND_GPT0_TMR0_CPT_REG0_MASK |
+			PWM3_AND_GPT0_TMR1_CLK_REG0_MASK,
+		.val = UART0_ENH_AND_GPT_REG0_MASK |
+			PWM2_AND_GPT0_TMR0_CPT_REG0_MASK |
+			PWM3_AND_GPT0_TMR1_CLK_REG0_MASK,
+	},
+};
+
+static struct spear_modemux gpt_0_1_modemux[] = {
+	{
+		.muxregs = gpt_0_1_muxreg,
+		.nmuxregs = ARRAY_SIZE(gpt_0_1_muxreg),
+	},
+};
+
+static struct spear_pingroup gpt_0_1_pingroup = {
+	.name = "gpt_0_1_grp",
+	.pins = gpt_0_1_pins,
+	.npins = ARRAY_SIZE(gpt_0_1_pins),
+	.modemuxs = gpt_0_1_modemux,
+	.nmodemuxs = ARRAY_SIZE(gpt_0_1_modemux),
+};
+
+static const char *const gpt_0_1_grps[] = { "gpt_0_1_grp" };
+static struct spear_function gpt_0_1_function = {
+	.name = "gpt_0_1",
+	.groups = gpt_0_1_grps,
+	.ngroups = ARRAY_SIZE(gpt_0_1_grps),
+};
+
+/* pad multiplexing for pwm0 device */
+static const unsigned pwm0_pins[] = { 24 };
+static struct spear_muxreg pwm0_muxreg[] = {
+	{
+		.reg = PAD_SHARED_IP_EN_1,
+		.mask = SSP0_CS1_MASK,
+		.val = 0,
+	}, {
+		.reg = PAD_FUNCTION_EN_1,
+		.mask = PWM0_AND_SSP0_CS1_REG0_MASK,
+		.val = PWM0_AND_SSP0_CS1_REG0_MASK,
+	},
+};
+
+static struct spear_modemux pwm0_modemux[] = {
+	{
+		.muxregs = pwm0_muxreg,
+		.nmuxregs = ARRAY_SIZE(pwm0_muxreg),
+	},
+};
+
+static struct spear_pingroup pwm0_pingroup = {
+	.name = "pwm0_grp",
+	.pins = pwm0_pins,
+	.npins = ARRAY_SIZE(pwm0_pins),
+	.modemuxs = pwm0_modemux,
+	.nmodemuxs = ARRAY_SIZE(pwm0_modemux),
+};
+
+/* pad multiplexing for pwm1 device */
+static const unsigned pwm1_pins[] = { 17 };
+static struct spear_muxreg pwm1_muxreg[] = {
+	{
+		.reg = PAD_SHARED_IP_EN_1,
+		.mask = KBD_COL5_MASK,
+		.val = 0,
+	}, {
+		.reg = PAD_FUNCTION_EN_1,
+		.mask = PWM1_AND_KBD_COL5_REG0_MASK,
+		.val = PWM1_AND_KBD_COL5_REG0_MASK,
+	},
+};
+
+static struct spear_modemux pwm1_modemux[] = {
+	{
+		.muxregs = pwm1_muxreg,
+		.nmuxregs = ARRAY_SIZE(pwm1_muxreg),
+	},
+};
+
+static struct spear_pingroup pwm1_pingroup = {
+	.name = "pwm1_grp",
+	.pins = pwm1_pins,
+	.npins = ARRAY_SIZE(pwm1_pins),
+	.modemuxs = pwm1_modemux,
+	.nmodemuxs = ARRAY_SIZE(pwm1_modemux),
+};
+
+/* pad multiplexing for pwm2 device */
+static const unsigned pwm2_pins[] = { 21 };
+static struct spear_muxreg pwm2_muxreg[] = {
+	{
+		.reg = PAD_SHARED_IP_EN_1,
+		.mask = GPT0_TMR0_CPT_MASK,
+		.val = 0,
+	}, {
+		.reg = PAD_FUNCTION_EN_1,
+		.mask = PWM2_AND_GPT0_TMR0_CPT_REG0_MASK,
+		.val = PWM2_AND_GPT0_TMR0_CPT_REG0_MASK,
+	},
+};
+
+static struct spear_modemux pwm2_modemux[] = {
+	{
+		.muxregs = pwm2_muxreg,
+		.nmuxregs = ARRAY_SIZE(pwm2_muxreg),
+	},
+};
+
+static struct spear_pingroup pwm2_pingroup = {
+	.name = "pwm2_grp",
+	.pins = pwm2_pins,
+	.npins = ARRAY_SIZE(pwm2_pins),
+	.modemuxs = pwm2_modemux,
+	.nmodemuxs = ARRAY_SIZE(pwm2_modemux),
+};
+
+/* pad multiplexing for pwm3 device */
+static const unsigned pwm3_pins[] = { 22 };
+static struct spear_muxreg pwm3_muxreg[] = {
+	{
+		.reg = PAD_SHARED_IP_EN_1,
+		.mask = GPT0_TMR1_CLK_MASK,
+		.val = 0,
+	}, {
+		.reg = PAD_FUNCTION_EN_1,
+		.mask = PWM3_AND_GPT0_TMR1_CLK_REG0_MASK,
+		.val = PWM3_AND_GPT0_TMR1_CLK_REG0_MASK,
+	},
+};
+
+static struct spear_modemux pwm3_modemux[] = {
+	{
+		.muxregs = pwm3_muxreg,
+		.nmuxregs = ARRAY_SIZE(pwm3_muxreg),
+	},
+};
+
+static struct spear_pingroup pwm3_pingroup = {
+	.name = "pwm3_grp",
+	.pins = pwm3_pins,
+	.npins = ARRAY_SIZE(pwm3_pins),
+	.modemuxs = pwm3_modemux,
+	.nmodemuxs = ARRAY_SIZE(pwm3_modemux),
+};
+
+static const char *const pwm_grps[] = { "pwm0_grp", "pwm1_grp", "pwm2_grp",
+	"pwm3_grp" };
+static struct spear_function pwm_function = {
+	.name = "pwm",
+	.groups = pwm_grps,
+	.ngroups = ARRAY_SIZE(pwm_grps),
+};
+
+/* pad multiplexing for vip_mux device */
+static const unsigned vip_mux_pins[] = { 35, 36, 37, 38, 40, 41, 42, 43 };
+static struct spear_muxreg vip_mux_muxreg[] = {
+	{
+		.reg = PAD_FUNCTION_EN_2,
+		.mask = VIP_REG1_MASK,
+		.val = VIP_REG1_MASK,
+	},
+};
+
+static struct spear_modemux vip_mux_modemux[] = {
+	{
+		.muxregs = vip_mux_muxreg,
+		.nmuxregs = ARRAY_SIZE(vip_mux_muxreg),
+	},
+};
+
+static struct spear_pingroup vip_mux_pingroup = {
+	.name = "vip_mux_grp",
+	.pins = vip_mux_pins,
+	.npins = ARRAY_SIZE(vip_mux_pins),
+	.modemuxs = vip_mux_modemux,
+	.nmodemuxs = ARRAY_SIZE(vip_mux_modemux),
+};
+
+/* pad multiplexing for vip_mux_cam0 (disables cam0) device */
+static const unsigned vip_mux_cam0_pins[] = { 65, 66, 67, 68, 69, 70, 71, 72,
+	73, 74, 75 };
+static struct spear_muxreg vip_mux_cam0_muxreg[] = {
+	{
+		.reg = PAD_SHARED_IP_EN_1,
+		.mask = CAM0_MASK,
+		.val = 0,
+	}, {
+		.reg = PAD_FUNCTION_EN_3,
+		.mask = VIP_AND_CAM0_REG2_MASK,
+		.val = VIP_AND_CAM0_REG2_MASK,
+	},
+};
+
+static struct spear_modemux vip_mux_cam0_modemux[] = {
+	{
+		.muxregs = vip_mux_cam0_muxreg,
+		.nmuxregs = ARRAY_SIZE(vip_mux_cam0_muxreg),
+	},
+};
+
+static struct spear_pingroup vip_mux_cam0_pingroup = {
+	.name = "vip_mux_cam0_grp",
+	.pins = vip_mux_cam0_pins,
+	.npins = ARRAY_SIZE(vip_mux_cam0_pins),
+	.modemuxs = vip_mux_cam0_modemux,
+	.nmodemuxs = ARRAY_SIZE(vip_mux_cam0_modemux),
+};
+
+/* pad multiplexing for vip_mux_cam1 (disables cam1) device */
+static const unsigned vip_mux_cam1_pins[] = { 54, 55, 56, 57, 58, 59, 60, 61,
+	62, 63, 64 };
+static struct spear_muxreg vip_mux_cam1_muxreg[] = {
+	{
+		.reg = PAD_SHARED_IP_EN_1,
+		.mask = CAM1_MASK,
+		.val = 0,
+	}, {
+		.reg = PAD_FUNCTION_EN_2,
+		.mask = VIP_AND_CAM1_REG1_MASK,
+		.val = VIP_AND_CAM1_REG1_MASK,
+	}, {
+		.reg = PAD_FUNCTION_EN_3,
+		.mask = VIP_AND_CAM1_REG2_MASK,
+		.val = VIP_AND_CAM1_REG2_MASK,
+	},
+};
+
+static struct spear_modemux vip_mux_cam1_modemux[] = {
+	{
+		.muxregs = vip_mux_cam1_muxreg,
+		.nmuxregs = ARRAY_SIZE(vip_mux_cam1_muxreg),
+	},
+};
+
+static struct spear_pingroup vip_mux_cam1_pingroup = {
+	.name = "vip_mux_cam1_grp",
+	.pins = vip_mux_cam1_pins,
+	.npins = ARRAY_SIZE(vip_mux_cam1_pins),
+	.modemuxs = vip_mux_cam1_modemux,
+	.nmodemuxs = ARRAY_SIZE(vip_mux_cam1_modemux),
+};
+
+/* pad multiplexing for vip_mux_cam2 (disables cam2) device */
+static const unsigned vip_mux_cam2_pins[] = { 39, 44, 45, 46, 47, 48, 49, 50,
+	51, 52, 53 };
+static struct spear_muxreg vip_mux_cam2_muxreg[] = {
+	{
+		.reg = PAD_SHARED_IP_EN_1,
+		.mask = CAM2_MASK,
+		.val = 0,
+	}, {
+		.reg = PAD_FUNCTION_EN_2,
+		.mask = VIP_AND_CAM2_REG1_MASK,
+		.val = VIP_AND_CAM2_REG1_MASK,
+	},
+};
+
+static struct spear_modemux vip_mux_cam2_modemux[] = {
+	{
+		.muxregs = vip_mux_cam2_muxreg,
+		.nmuxregs = ARRAY_SIZE(vip_mux_cam2_muxreg),
+	},
+};
+
+static struct spear_pingroup vip_mux_cam2_pingroup = {
+	.name = "vip_mux_cam2_grp",
+	.pins = vip_mux_cam2_pins,
+	.npins = ARRAY_SIZE(vip_mux_cam2_pins),
+	.modemuxs = vip_mux_cam2_modemux,
+	.nmodemuxs = ARRAY_SIZE(vip_mux_cam2_modemux),
+};
+
+/* pad multiplexing for vip_mux_cam3 (disables cam3) device */
+static const unsigned vip_mux_cam3_pins[] = { 20, 25, 26, 27, 28, 29, 30, 31,
+	32, 33, 34 };
+static struct spear_muxreg vip_mux_cam3_muxreg[] = {
+	{
+		.reg = PAD_SHARED_IP_EN_1,
+		.mask = CAM3_MASK,
+		.val = 0,
+	}, {
+		.reg = PAD_FUNCTION_EN_1,
+		.mask = VIP_AND_CAM3_REG0_MASK,
+		.val = VIP_AND_CAM3_REG0_MASK,
+	}, {
+		.reg = PAD_FUNCTION_EN_2,
+		.mask = VIP_AND_CAM3_REG1_MASK,
+		.val = VIP_AND_CAM3_REG1_MASK,
+	},
+};
+
+static struct spear_modemux vip_mux_cam3_modemux[] = {
+	{
+		.muxregs = vip_mux_cam3_muxreg,
+		.nmuxregs = ARRAY_SIZE(vip_mux_cam3_muxreg),
+	},
+};
+
+static struct spear_pingroup vip_mux_cam3_pingroup = {
+	.name = "vip_mux_cam3_grp",
+	.pins = vip_mux_cam3_pins,
+	.npins = ARRAY_SIZE(vip_mux_cam3_pins),
+	.modemuxs = vip_mux_cam3_modemux,
+	.nmodemuxs = ARRAY_SIZE(vip_mux_cam3_modemux),
+};
+
+static const char *const vip_grps[] = { "vip_mux_grp", "vip_mux_cam0_grp" ,
+	"vip_mux_cam1_grp" , "vip_mux_cam2_grp", "vip_mux_cam3_grp" };
+static struct spear_function vip_function = {
+	.name = "vip",
+	.groups = vip_grps,
+	.ngroups = ARRAY_SIZE(vip_grps),
+};
+
+/* pad multiplexing for cam0 device */
+static const unsigned cam0_pins[] = { 65, 66, 67, 68, 69, 70, 71, 72, 73, 74, 75
+};
+static struct spear_muxreg cam0_muxreg[] = {
+	{
+		.reg = PAD_SHARED_IP_EN_1,
+		.mask = CAM0_MASK,
+		.val = CAM0_MASK,
+	}, {
+		.reg = PAD_FUNCTION_EN_3,
+		.mask = VIP_AND_CAM0_REG2_MASK,
+		.val = VIP_AND_CAM0_REG2_MASK,
+	},
+};
+
+static struct spear_modemux cam0_modemux[] = {
+	{
+		.muxregs = cam0_muxreg,
+		.nmuxregs = ARRAY_SIZE(cam0_muxreg),
+	},
+};
+
+static struct spear_pingroup cam0_pingroup = {
+	.name = "cam0_grp",
+	.pins = cam0_pins,
+	.npins = ARRAY_SIZE(cam0_pins),
+	.modemuxs = cam0_modemux,
+	.nmodemuxs = ARRAY_SIZE(cam0_modemux),
+};
+
+static const char *const cam0_grps[] = { "cam0_grp" };
+static struct spear_function cam0_function = {
+	.name = "cam0",
+	.groups = cam0_grps,
+	.ngroups = ARRAY_SIZE(cam0_grps),
+};
+
+/* pad multiplexing for cam1 device */
+static const unsigned cam1_pins[] = { 54, 55, 56, 57, 58, 59, 60, 61, 62, 63, 64
+};
+static struct spear_muxreg cam1_muxreg[] = {
+	{
+		.reg = PAD_SHARED_IP_EN_1,
+		.mask = CAM1_MASK,
+		.val = CAM1_MASK,
+	}, {
+		.reg = PAD_FUNCTION_EN_2,
+		.mask = VIP_AND_CAM1_REG1_MASK,
+		.val = VIP_AND_CAM1_REG1_MASK,
+	}, {
+		.reg = PAD_FUNCTION_EN_3,
+		.mask = VIP_AND_CAM1_REG2_MASK,
+		.val = VIP_AND_CAM1_REG2_MASK,
+	},
+};
+
+static struct spear_modemux cam1_modemux[] = {
+	{
+		.muxregs = cam1_muxreg,
+		.nmuxregs = ARRAY_SIZE(cam1_muxreg),
+	},
+};
+
+static struct spear_pingroup cam1_pingroup = {
+	.name = "cam1_grp",
+	.pins = cam1_pins,
+	.npins = ARRAY_SIZE(cam1_pins),
+	.modemuxs = cam1_modemux,
+	.nmodemuxs = ARRAY_SIZE(cam1_modemux),
+};
+
+static const char *const cam1_grps[] = { "cam1_grp" };
+static struct spear_function cam1_function = {
+	.name = "cam1",
+	.groups = cam1_grps,
+	.ngroups = ARRAY_SIZE(cam1_grps),
+};
+
+/* pad multiplexing for cam2 device */
+static const unsigned cam2_pins[] = { 39, 44, 45, 46, 47, 48, 49, 50, 51, 52, 53
+};
+static struct spear_muxreg cam2_muxreg[] = {
+	{
+		.reg = PAD_SHARED_IP_EN_1,
+		.mask = CAM2_MASK,
+		.val = CAM2_MASK,
+	}, {
+		.reg = PAD_FUNCTION_EN_2,
+		.mask = VIP_AND_CAM2_REG1_MASK,
+		.val = VIP_AND_CAM2_REG1_MASK,
+	},
+};
+
+static struct spear_modemux cam2_modemux[] = {
+	{
+		.muxregs = cam2_muxreg,
+		.nmuxregs = ARRAY_SIZE(cam2_muxreg),
+	},
+};
+
+static struct spear_pingroup cam2_pingroup = {
+	.name = "cam2_grp",
+	.pins = cam2_pins,
+	.npins = ARRAY_SIZE(cam2_pins),
+	.modemuxs = cam2_modemux,
+	.nmodemuxs = ARRAY_SIZE(cam2_modemux),
+};
+
+static const char *const cam2_grps[] = { "cam2_grp" };
+static struct spear_function cam2_function = {
+	.name = "cam2",
+	.groups = cam2_grps,
+	.ngroups = ARRAY_SIZE(cam2_grps),
+};
+
+/* pad multiplexing for cam3 device */
+static const unsigned cam3_pins[] = { 20, 25, 26, 27, 28, 29, 30, 31, 32, 33, 34
+};
+static struct spear_muxreg cam3_muxreg[] = {
+	{
+		.reg = PAD_SHARED_IP_EN_1,
+		.mask = CAM3_MASK,
+		.val = CAM3_MASK,
+	}, {
+		.reg = PAD_FUNCTION_EN_1,
+		.mask = VIP_AND_CAM3_REG0_MASK,
+		.val = VIP_AND_CAM3_REG0_MASK,
+	}, {
+		.reg = PAD_FUNCTION_EN_2,
+		.mask = VIP_AND_CAM3_REG1_MASK,
+		.val = VIP_AND_CAM3_REG1_MASK,
+	},
+};
+
+static struct spear_modemux cam3_modemux[] = {
+	{
+		.muxregs = cam3_muxreg,
+		.nmuxregs = ARRAY_SIZE(cam3_muxreg),
+	},
+};
+
+static struct spear_pingroup cam3_pingroup = {
+	.name = "cam3_grp",
+	.pins = cam3_pins,
+	.npins = ARRAY_SIZE(cam3_pins),
+	.modemuxs = cam3_modemux,
+	.nmodemuxs = ARRAY_SIZE(cam3_modemux),
+};
+
+static const char *const cam3_grps[] = { "cam3_grp" };
+static struct spear_function cam3_function = {
+	.name = "cam3",
+	.groups = cam3_grps,
+	.ngroups = ARRAY_SIZE(cam3_grps),
+};
+
+/* pad multiplexing for smi device */
+static const unsigned smi_pins[] = { 76, 77, 78, 79, 84 };
+static struct spear_muxreg smi_muxreg[] = {
+	{
+		.reg = PAD_FUNCTION_EN_3,
+		.mask = SMI_REG2_MASK,
+		.val = SMI_REG2_MASK,
+	},
+};
+
+static struct spear_modemux smi_modemux[] = {
+	{
+		.muxregs = smi_muxreg,
+		.nmuxregs = ARRAY_SIZE(smi_muxreg),
+	},
+};
+
+static struct spear_pingroup smi_pingroup = {
+	.name = "smi_grp",
+	.pins = smi_pins,
+	.npins = ARRAY_SIZE(smi_pins),
+	.modemuxs = smi_modemux,
+	.nmodemuxs = ARRAY_SIZE(smi_modemux),
+};
+
+static const char *const smi_grps[] = { "smi_grp" };
+static struct spear_function smi_function = {
+	.name = "smi",
+	.groups = smi_grps,
+	.ngroups = ARRAY_SIZE(smi_grps),
+};
+
+/* pad multiplexing for ssp0 device */
+static const unsigned ssp0_pins[] = { 80, 81, 82, 83 };
+static struct spear_muxreg ssp0_muxreg[] = {
+	{
+		.reg = PAD_FUNCTION_EN_3,
+		.mask = SSP0_REG2_MASK,
+		.val = SSP0_REG2_MASK,
+	},
+};
+
+static struct spear_modemux ssp0_modemux[] = {
+	{
+		.muxregs = ssp0_muxreg,
+		.nmuxregs = ARRAY_SIZE(ssp0_muxreg),
+	},
+};
+
+static struct spear_pingroup ssp0_pingroup = {
+	.name = "ssp0_grp",
+	.pins = ssp0_pins,
+	.npins = ARRAY_SIZE(ssp0_pins),
+	.modemuxs = ssp0_modemux,
+	.nmodemuxs = ARRAY_SIZE(ssp0_modemux),
+};
+
+/* pad multiplexing for ssp0_cs1 device */
+static const unsigned ssp0_cs1_pins[] = { 24 };
+static struct spear_muxreg ssp0_cs1_muxreg[] = {
+	{
+		.reg = PAD_SHARED_IP_EN_1,
+		.mask = SSP0_CS1_MASK,
+		.val = SSP0_CS1_MASK,
+	}, {
+		.reg = PAD_FUNCTION_EN_1,
+		.mask = PWM0_AND_SSP0_CS1_REG0_MASK,
+		.val = PWM0_AND_SSP0_CS1_REG0_MASK,
+	},
+};
+
+static struct spear_modemux ssp0_cs1_modemux[] = {
+	{
+		.muxregs = ssp0_cs1_muxreg,
+		.nmuxregs = ARRAY_SIZE(ssp0_cs1_muxreg),
+	},
+};
+
+static struct spear_pingroup ssp0_cs1_pingroup = {
+	.name = "ssp0_cs1_grp",
+	.pins = ssp0_cs1_pins,
+	.npins = ARRAY_SIZE(ssp0_cs1_pins),
+	.modemuxs = ssp0_cs1_modemux,
+	.nmodemuxs = ARRAY_SIZE(ssp0_cs1_modemux),
+};
+
+/* pad multiplexing for ssp0_cs2 device */
+static const unsigned ssp0_cs2_pins[] = { 85 };
+static struct spear_muxreg ssp0_cs2_muxreg[] = {
+	{
+		.reg = PAD_SHARED_IP_EN_1,
+		.mask = SSP0_CS2_MASK,
+		.val = SSP0_CS2_MASK,
+	}, {
+		.reg = PAD_FUNCTION_EN_3,
+		.mask = TS_AND_SSP0_CS2_REG2_MASK,
+		.val = TS_AND_SSP0_CS2_REG2_MASK,
+	},
+};
+
+static struct spear_modemux ssp0_cs2_modemux[] = {
+	{
+		.muxregs = ssp0_cs2_muxreg,
+		.nmuxregs = ARRAY_SIZE(ssp0_cs2_muxreg),
+	},
+};
+
+static struct spear_pingroup ssp0_cs2_pingroup = {
+	.name = "ssp0_cs2_grp",
+	.pins = ssp0_cs2_pins,
+	.npins = ARRAY_SIZE(ssp0_cs2_pins),
+	.modemuxs = ssp0_cs2_modemux,
+	.nmodemuxs = ARRAY_SIZE(ssp0_cs2_modemux),
+};
+
+/* pad multiplexing for ssp0_cs3 device */
+static const unsigned ssp0_cs3_pins[] = { 132 };
+static struct spear_muxreg ssp0_cs3_muxreg[] = {
+	{
+		.reg = PAD_FUNCTION_EN_5,
+		.mask = SSP0_CS3_REG4_MASK,
+		.val = SSP0_CS3_REG4_MASK,
+	},
+};
+
+static struct spear_modemux ssp0_cs3_modemux[] = {
+	{
+		.muxregs = ssp0_cs3_muxreg,
+		.nmuxregs = ARRAY_SIZE(ssp0_cs3_muxreg),
+	},
+};
+
+static struct spear_pingroup ssp0_cs3_pingroup = {
+	.name = "ssp0_cs3_grp",
+	.pins = ssp0_cs3_pins,
+	.npins = ARRAY_SIZE(ssp0_cs3_pins),
+	.modemuxs = ssp0_cs3_modemux,
+	.nmodemuxs = ARRAY_SIZE(ssp0_cs3_modemux),
+};
+
+static const char *const ssp0_grps[] = { "ssp0_grp", "ssp0_cs1_grp",
+	"ssp0_cs2_grp", "ssp0_cs3_grp" };
+static struct spear_function ssp0_function = {
+	.name = "ssp0",
+	.groups = ssp0_grps,
+	.ngroups = ARRAY_SIZE(ssp0_grps),
+};
+
+/* pad multiplexing for uart0 device */
+static const unsigned uart0_pins[] = { 86, 87 };
+static struct spear_muxreg uart0_muxreg[] = {
+	{
+		.reg = PAD_FUNCTION_EN_3,
+		.mask = UART0_REG2_MASK,
+		.val = UART0_REG2_MASK,
+	},
+};
+
+static struct spear_modemux uart0_modemux[] = {
+	{
+		.muxregs = uart0_muxreg,
+		.nmuxregs = ARRAY_SIZE(uart0_muxreg),
+	},
+};
+
+static struct spear_pingroup uart0_pingroup = {
+	.name = "uart0_grp",
+	.pins = uart0_pins,
+	.npins = ARRAY_SIZE(uart0_pins),
+	.modemuxs = uart0_modemux,
+	.nmodemuxs = ARRAY_SIZE(uart0_modemux),
+};
+
+/* pad multiplexing for uart0_enh device */
+static const unsigned uart0_enh_pins[] = { 11, 12, 13, 14, 15, 16 };
+static struct spear_muxreg uart0_enh_muxreg[] = {
+	{
+		.reg = PAD_SHARED_IP_EN_1,
+		.mask = GPT_MASK,
+		.val = 0,
+	}, {
+		.reg = PAD_FUNCTION_EN_1,
+		.mask = UART0_ENH_AND_GPT_REG0_MASK,
+		.val = UART0_ENH_AND_GPT_REG0_MASK,
+	},
+};
+
+static struct spear_modemux uart0_enh_modemux[] = {
+	{
+		.muxregs = uart0_enh_muxreg,
+		.nmuxregs = ARRAY_SIZE(uart0_enh_muxreg),
+	},
+};
+
+static struct spear_pingroup uart0_enh_pingroup = {
+	.name = "uart0_enh_grp",
+	.pins = uart0_enh_pins,
+	.npins = ARRAY_SIZE(uart0_enh_pins),
+	.modemuxs = uart0_enh_modemux,
+	.nmodemuxs = ARRAY_SIZE(uart0_enh_modemux),
+};
+
+static const char *const uart0_grps[] = { "uart0_grp", "uart0_enh_grp" };
+static struct spear_function uart0_function = {
+	.name = "uart0",
+	.groups = uart0_grps,
+	.ngroups = ARRAY_SIZE(uart0_grps),
+};
+
+/* pad multiplexing for uart1 device */
+static const unsigned uart1_pins[] = { 88, 89 };
+static struct spear_muxreg uart1_muxreg[] = {
+	{
+		.reg = PAD_FUNCTION_EN_3,
+		.mask = UART1_REG2_MASK,
+		.val = UART1_REG2_MASK,
+	},
+};
+
+static struct spear_modemux uart1_modemux[] = {
+	{
+		.muxregs = uart1_muxreg,
+		.nmuxregs = ARRAY_SIZE(uart1_muxreg),
+	},
+};
+
+static struct spear_pingroup uart1_pingroup = {
+	.name = "uart1_grp",
+	.pins = uart1_pins,
+	.npins = ARRAY_SIZE(uart1_pins),
+	.modemuxs = uart1_modemux,
+	.nmodemuxs = ARRAY_SIZE(uart1_modemux),
+};
+
+static const char *const uart1_grps[] = { "uart1_grp" };
+static struct spear_function uart1_function = {
+	.name = "uart1",
+	.groups = uart1_grps,
+	.ngroups = ARRAY_SIZE(uart1_grps),
+};
+
+/* pad multiplexing for i2s_in device */
+static const unsigned i2s_in_pins[] = { 90, 91, 92, 93, 94, 99 };
+static struct spear_muxreg i2s_in_muxreg[] = {
+	{
+		.reg = PAD_FUNCTION_EN_3,
+		.mask = I2S_IN_REG2_MASK,
+		.val = I2S_IN_REG2_MASK,
+	}, {
+		.reg = PAD_FUNCTION_EN_4,
+		.mask = I2S_IN_REG3_MASK,
+		.val = I2S_IN_REG3_MASK,
+	},
+};
+
+static struct spear_modemux i2s_in_modemux[] = {
+	{
+		.muxregs = i2s_in_muxreg,
+		.nmuxregs = ARRAY_SIZE(i2s_in_muxreg),
+	},
+};
+
+static struct spear_pingroup i2s_in_pingroup = {
+	.name = "i2s_in_grp",
+	.pins = i2s_in_pins,
+	.npins = ARRAY_SIZE(i2s_in_pins),
+	.modemuxs = i2s_in_modemux,
+	.nmodemuxs = ARRAY_SIZE(i2s_in_modemux),
+};
+
+/* pad multiplexing for i2s_out device */
+static const unsigned i2s_out_pins[] = { 95, 96, 97, 98, 100, 101, 102, 103 };
+static struct spear_muxreg i2s_out_muxreg[] = {
+	{
+		.reg = PAD_FUNCTION_EN_4,
+		.mask = I2S_OUT_REG3_MASK,
+		.val = I2S_OUT_REG3_MASK,
+	},
+};
+
+static struct spear_modemux i2s_out_modemux[] = {
+	{
+		.muxregs = i2s_out_muxreg,
+		.nmuxregs = ARRAY_SIZE(i2s_out_muxreg),
+	},
+};
+
+static struct spear_pingroup i2s_out_pingroup = {
+	.name = "i2s_out_grp",
+	.pins = i2s_out_pins,
+	.npins = ARRAY_SIZE(i2s_out_pins),
+	.modemuxs = i2s_out_modemux,
+	.nmodemuxs = ARRAY_SIZE(i2s_out_modemux),
+};
+
+static const char *const i2s_grps[] = { "i2s_in_grp", "i2s_out_grp" };
+static struct spear_function i2s_function = {
+	.name = "i2s",
+	.groups = i2s_grps,
+	.ngroups = ARRAY_SIZE(i2s_grps),
+};
+
+/* pad multiplexing for gmac device */
+static const unsigned gmac_pins[] = { 104, 105, 106, 107, 108, 109, 110, 111,
+	112, 113, 114, 115, 116, 117, 118, 119, 120, 121, 122, 123, 124, 125,
+	126, 127, 128, 129, 130, 131 };
+#define GMAC_MUXREG				\
+	{					\
+		.reg = PAD_FUNCTION_EN_4,	\
+		.mask = GMAC_REG3_MASK,		\
+		.val = GMAC_REG3_MASK,		\
+	}, {					\
+		.reg = PAD_FUNCTION_EN_5,	\
+		.mask = GMAC_REG4_MASK,		\
+		.val = GMAC_REG4_MASK,		\
+	}
+
+/* pad multiplexing for gmii device */
+static struct spear_muxreg gmii_muxreg[] = {
+	GMAC_MUXREG,
+	{
+		.reg = GMAC_CLK_CFG,
+		.mask = GMAC_PHY_IF_SEL_MASK,
+		.val = GMAC_PHY_IF_GMII_VAL,
+	},
+};
+
+static struct spear_modemux gmii_modemux[] = {
+	{
+		.muxregs = gmii_muxreg,
+		.nmuxregs = ARRAY_SIZE(gmii_muxreg),
+	},
+};
+
+static struct spear_pingroup gmii_pingroup = {
+	.name = "gmii_grp",
+	.pins = gmac_pins,
+	.npins = ARRAY_SIZE(gmac_pins),
+	.modemuxs = gmii_modemux,
+	.nmodemuxs = ARRAY_SIZE(gmii_modemux),
+};
+
+/* pad multiplexing for rgmii device */
+static struct spear_muxreg rgmii_muxreg[] = {
+	GMAC_MUXREG,
+	{
+		.reg = GMAC_CLK_CFG,
+		.mask = GMAC_PHY_IF_SEL_MASK,
+		.val = GMAC_PHY_IF_RGMII_VAL,
+	},
+};
+
+static struct spear_modemux rgmii_modemux[] = {
+	{
+		.muxregs = rgmii_muxreg,
+		.nmuxregs = ARRAY_SIZE(rgmii_muxreg),
+	},
+};
+
+static struct spear_pingroup rgmii_pingroup = {
+	.name = "rgmii_grp",
+	.pins = gmac_pins,
+	.npins = ARRAY_SIZE(gmac_pins),
+	.modemuxs = rgmii_modemux,
+	.nmodemuxs = ARRAY_SIZE(rgmii_modemux),
+};
+
+/* pad multiplexing for rmii device */
+static struct spear_muxreg rmii_muxreg[] = {
+	GMAC_MUXREG,
+	{
+		.reg = GMAC_CLK_CFG,
+		.mask = GMAC_PHY_IF_SEL_MASK,
+		.val = GMAC_PHY_IF_RMII_VAL,
+	},
+};
+
+static struct spear_modemux rmii_modemux[] = {
+	{
+		.muxregs = rmii_muxreg,
+		.nmuxregs = ARRAY_SIZE(rmii_muxreg),
+	},
+};
+
+static struct spear_pingroup rmii_pingroup = {
+	.name = "rmii_grp",
+	.pins = gmac_pins,
+	.npins = ARRAY_SIZE(gmac_pins),
+	.modemuxs = rmii_modemux,
+	.nmodemuxs = ARRAY_SIZE(rmii_modemux),
+};
+
+/* pad multiplexing for sgmii device */
+static struct spear_muxreg sgmii_muxreg[] = {
+	GMAC_MUXREG,
+	{
+		.reg = GMAC_CLK_CFG,
+		.mask = GMAC_PHY_IF_SEL_MASK,
+		.val = GMAC_PHY_IF_SGMII_VAL,
+	},
+};
+
+static struct spear_modemux sgmii_modemux[] = {
+	{
+		.muxregs = sgmii_muxreg,
+		.nmuxregs = ARRAY_SIZE(sgmii_muxreg),
+	},
+};
+
+static struct spear_pingroup sgmii_pingroup = {
+	.name = "sgmii_grp",
+	.pins = gmac_pins,
+	.npins = ARRAY_SIZE(gmac_pins),
+	.modemuxs = sgmii_modemux,
+	.nmodemuxs = ARRAY_SIZE(sgmii_modemux),
+};
+
+static const char *const gmac_grps[] = { "gmii_grp", "rgmii_grp", "rmii_grp",
+	"sgmii_grp" };
+static struct spear_function gmac_function = {
+	.name = "gmac",
+	.groups = gmac_grps,
+	.ngroups = ARRAY_SIZE(gmac_grps),
+};
+
+/* pad multiplexing for i2c0 device */
+static const unsigned i2c0_pins[] = { 133, 134 };
+static struct spear_muxreg i2c0_muxreg[] = {
+	{
+		.reg = PAD_FUNCTION_EN_5,
+		.mask = I2C0_REG4_MASK,
+		.val = I2C0_REG4_MASK,
+	},
+};
+
+static struct spear_modemux i2c0_modemux[] = {
+	{
+		.muxregs = i2c0_muxreg,
+		.nmuxregs = ARRAY_SIZE(i2c0_muxreg),
+	},
+};
+
+static struct spear_pingroup i2c0_pingroup = {
+	.name = "i2c0_grp",
+	.pins = i2c0_pins,
+	.npins = ARRAY_SIZE(i2c0_pins),
+	.modemuxs = i2c0_modemux,
+	.nmodemuxs = ARRAY_SIZE(i2c0_modemux),
+};
+
+static const char *const i2c0_grps[] = { "i2c0_grp" };
+static struct spear_function i2c0_function = {
+	.name = "i2c0",
+	.groups = i2c0_grps,
+	.ngroups = ARRAY_SIZE(i2c0_grps),
+};
+
+/* pad multiplexing for i2c1 device */
+static const unsigned i2c1_pins[] = { 18, 23 };
+static struct spear_muxreg i2c1_muxreg[] = {
+	{
+		.reg = PAD_FUNCTION_EN_1,
+		.mask = I2C1_REG0_MASK,
+		.val = I2C1_REG0_MASK,
+	},
+};
+
+static struct spear_modemux i2c1_modemux[] = {
+	{
+		.muxregs = i2c1_muxreg,
+		.nmuxregs = ARRAY_SIZE(i2c1_muxreg),
+	},
+};
+
+static struct spear_pingroup i2c1_pingroup = {
+	.name = "i2c1_grp",
+	.pins = i2c1_pins,
+	.npins = ARRAY_SIZE(i2c1_pins),
+	.modemuxs = i2c1_modemux,
+	.nmodemuxs = ARRAY_SIZE(i2c1_modemux),
+};
+
+static const char *const i2c1_grps[] = { "i2c1_grp" };
+static struct spear_function i2c1_function = {
+	.name = "i2c1",
+	.groups = i2c1_grps,
+	.ngroups = ARRAY_SIZE(i2c1_grps),
+};
+
+/* pad multiplexing for cec0 device */
+static const unsigned cec0_pins[] = { 135 };
+static struct spear_muxreg cec0_muxreg[] = {
+	{
+		.reg = PAD_FUNCTION_EN_5,
+		.mask = CEC0_REG4_MASK,
+		.val = CEC0_REG4_MASK,
+	},
+};
+
+static struct spear_modemux cec0_modemux[] = {
+	{
+		.muxregs = cec0_muxreg,
+		.nmuxregs = ARRAY_SIZE(cec0_muxreg),
+	},
+};
+
+static struct spear_pingroup cec0_pingroup = {
+	.name = "cec0_grp",
+	.pins = cec0_pins,
+	.npins = ARRAY_SIZE(cec0_pins),
+	.modemuxs = cec0_modemux,
+	.nmodemuxs = ARRAY_SIZE(cec0_modemux),
+};
+
+static const char *const cec0_grps[] = { "cec0_grp" };
+static struct spear_function cec0_function = {
+	.name = "cec0",
+	.groups = cec0_grps,
+	.ngroups = ARRAY_SIZE(cec0_grps),
+};
+
+/* pad multiplexing for cec1 device */
+static const unsigned cec1_pins[] = { 136 };
+static struct spear_muxreg cec1_muxreg[] = {
+	{
+		.reg = PAD_FUNCTION_EN_5,
+		.mask = CEC1_REG4_MASK,
+		.val = CEC1_REG4_MASK,
+	},
+};
+
+static struct spear_modemux cec1_modemux[] = {
+	{
+		.muxregs = cec1_muxreg,
+		.nmuxregs = ARRAY_SIZE(cec1_muxreg),
+	},
+};
+
+static struct spear_pingroup cec1_pingroup = {
+	.name = "cec1_grp",
+	.pins = cec1_pins,
+	.npins = ARRAY_SIZE(cec1_pins),
+	.modemuxs = cec1_modemux,
+	.nmodemuxs = ARRAY_SIZE(cec1_modemux),
+};
+
+static const char *const cec1_grps[] = { "cec1_grp" };
+static struct spear_function cec1_function = {
+	.name = "cec1",
+	.groups = cec1_grps,
+	.ngroups = ARRAY_SIZE(cec1_grps),
+};
+
+/* pad multiplexing for mcif devices */
+static const unsigned mcif_pins[] = { 193, 194, 195, 196, 197, 198, 199, 200,
+	201, 202, 203, 204, 205, 206, 207, 208, 209, 210, 211, 212, 213, 214,
+	215, 216, 217, 218, 219, 220, 221, 222, 223, 224, 225, 226, 227, 228,
+	229, 230, 231, 232, 237 };
+#define MCIF_MUXREG							\
+	{								\
+		.reg = PAD_SHARED_IP_EN_1,				\
+		.mask = MCIF_MASK,					\
+		.val = MCIF_MASK,					\
+	}, {								\
+		.reg = PAD_FUNCTION_EN_7,				\
+		.mask = FSMC_PNOR_AND_MCIF_REG6_MASK | MCIF_REG6_MASK,	\
+		.val = FSMC_PNOR_AND_MCIF_REG6_MASK | MCIF_REG6_MASK,	\
+	}, {								\
+		.reg = PAD_FUNCTION_EN_8,				\
+		.mask = MCIF_REG7_MASK,					\
+		.val = MCIF_REG7_MASK,					\
+	}
+
+/* Pad multiplexing for sdhci device */
+static struct spear_muxreg sdhci_muxreg[] = {
+	MCIF_MUXREG,
+	{
+		.reg = PERIP_CFG,
+		.mask = MCIF_SEL_MASK,
+		.val = MCIF_SEL_SD,
+	},
+};
+
+static struct spear_modemux sdhci_modemux[] = {
+	{
+		.muxregs = sdhci_muxreg,
+		.nmuxregs = ARRAY_SIZE(sdhci_muxreg),
+	},
+};
+
+static struct spear_pingroup sdhci_pingroup = {
+	.name = "sdhci_grp",
+	.pins = mcif_pins,
+	.npins = ARRAY_SIZE(mcif_pins),
+	.modemuxs = sdhci_modemux,
+	.nmodemuxs = ARRAY_SIZE(sdhci_modemux),
+};
+
+static const char *const sdhci_grps[] = { "sdhci_grp" };
+static struct spear_function sdhci_function = {
+	.name = "sdhci",
+	.groups = sdhci_grps,
+	.ngroups = ARRAY_SIZE(sdhci_grps),
+};
+
+/* Pad multiplexing for cf device */
+static struct spear_muxreg cf_muxreg[] = {
+	MCIF_MUXREG,
+	{
+		.reg = PERIP_CFG,
+		.mask = MCIF_SEL_MASK,
+		.val = MCIF_SEL_CF,
+	},
+};
+
+static struct spear_modemux cf_modemux[] = {
+	{
+		.muxregs = cf_muxreg,
+		.nmuxregs = ARRAY_SIZE(cf_muxreg),
+	},
+};
+
+static struct spear_pingroup cf_pingroup = {
+	.name = "cf_grp",
+	.pins = mcif_pins,
+	.npins = ARRAY_SIZE(mcif_pins),
+	.modemuxs = cf_modemux,
+	.nmodemuxs = ARRAY_SIZE(cf_modemux),
+};
+
+static const char *const cf_grps[] = { "cf_grp" };
+static struct spear_function cf_function = {
+	.name = "cf",
+	.groups = cf_grps,
+	.ngroups = ARRAY_SIZE(cf_grps),
+};
+
+/* Pad multiplexing for xd device */
+static struct spear_muxreg xd_muxreg[] = {
+	MCIF_MUXREG,
+	{
+		.reg = PERIP_CFG,
+		.mask = MCIF_SEL_MASK,
+		.val = MCIF_SEL_XD,
+	},
+};
+
+static struct spear_modemux xd_modemux[] = {
+	{
+		.muxregs = xd_muxreg,
+		.nmuxregs = ARRAY_SIZE(xd_muxreg),
+	},
+};
+
+static struct spear_pingroup xd_pingroup = {
+	.name = "xd_grp",
+	.pins = mcif_pins,
+	.npins = ARRAY_SIZE(mcif_pins),
+	.modemuxs = xd_modemux,
+	.nmodemuxs = ARRAY_SIZE(xd_modemux),
+};
+
+static const char *const xd_grps[] = { "xd_grp" };
+static struct spear_function xd_function = {
+	.name = "xd",
+	.groups = xd_grps,
+	.ngroups = ARRAY_SIZE(xd_grps),
+};
+
+/* pad multiplexing for clcd device */
+static const unsigned clcd_pins[] = { 138, 139, 140, 141, 142, 143, 144, 145,
+	146, 147, 148, 149, 150, 151, 152, 153, 154, 155, 156, 157, 158, 159,
+	160, 161, 162, 163, 164, 165, 166, 167, 168, 169, 170, 171, 172, 173,
+	174, 175, 176, 177, 178, 179, 180, 181, 182, 183, 184, 185, 186, 187,
+	188, 189, 190, 191 };
+static struct spear_muxreg clcd_muxreg[] = {
+	{
+		.reg = PAD_SHARED_IP_EN_1,
+		.mask = ARM_TRACE_MASK | MIPHY_DBG_MASK,
+		.val = 0,
+	}, {
+		.reg = PAD_FUNCTION_EN_5,
+		.mask = CLCD_REG4_MASK | CLCD_AND_ARM_TRACE_REG4_MASK,
+		.val = CLCD_REG4_MASK | CLCD_AND_ARM_TRACE_REG4_MASK,
+	}, {
+		.reg = PAD_FUNCTION_EN_6,
+		.mask = CLCD_AND_ARM_TRACE_REG5_MASK,
+		.val = CLCD_AND_ARM_TRACE_REG5_MASK,
+	}, {
+		.reg = PAD_FUNCTION_EN_7,
+		.mask = CLCD_AND_ARM_TRACE_REG6_MASK,
+		.val = CLCD_AND_ARM_TRACE_REG6_MASK,
+	},
+};
+
+static struct spear_modemux clcd_modemux[] = {
+	{
+		.muxregs = clcd_muxreg,
+		.nmuxregs = ARRAY_SIZE(clcd_muxreg),
+	},
+};
+
+static struct spear_pingroup clcd_pingroup = {
+	.name = "clcd_grp",
+	.pins = clcd_pins,
+	.npins = ARRAY_SIZE(clcd_pins),
+	.modemuxs = clcd_modemux,
+	.nmodemuxs = ARRAY_SIZE(clcd_modemux),
+};
+
+static const char *const clcd_grps[] = { "clcd_grp" };
+static struct spear_function clcd_function = {
+	.name = "clcd",
+	.groups = clcd_grps,
+	.ngroups = ARRAY_SIZE(clcd_grps),
+};
+
+/* pad multiplexing for arm_trace device */
+static const unsigned arm_trace_pins[] = { 158, 159, 160, 161, 162, 163, 164,
+	165, 166, 167, 168, 169, 170, 171, 172, 173, 174, 175, 176, 177, 178,
+	179, 180, 181, 182, 183, 184, 185, 186, 187, 188, 189, 190, 191, 192,
+	193, 194, 195, 196, 197, 198, 199, 200 };
+static struct spear_muxreg arm_trace_muxreg[] = {
+	{
+		.reg = PAD_SHARED_IP_EN_1,
+		.mask = ARM_TRACE_MASK,
+		.val = ARM_TRACE_MASK,
+	}, {
+		.reg = PAD_FUNCTION_EN_5,
+		.mask = CLCD_AND_ARM_TRACE_REG4_MASK,
+		.val = CLCD_AND_ARM_TRACE_REG4_MASK,
+	}, {
+		.reg = PAD_FUNCTION_EN_6,
+		.mask = CLCD_AND_ARM_TRACE_REG5_MASK,
+		.val = CLCD_AND_ARM_TRACE_REG5_MASK,
+	}, {
+		.reg = PAD_FUNCTION_EN_7,
+		.mask = CLCD_AND_ARM_TRACE_REG6_MASK,
+		.val = CLCD_AND_ARM_TRACE_REG6_MASK,
+	},
+};
+
+static struct spear_modemux arm_trace_modemux[] = {
+	{
+		.muxregs = arm_trace_muxreg,
+		.nmuxregs = ARRAY_SIZE(arm_trace_muxreg),
+	},
+};
+
+static struct spear_pingroup arm_trace_pingroup = {
+	.name = "arm_trace_grp",
+	.pins = arm_trace_pins,
+	.npins = ARRAY_SIZE(arm_trace_pins),
+	.modemuxs = arm_trace_modemux,
+	.nmodemuxs = ARRAY_SIZE(arm_trace_modemux),
+};
+
+static const char *const arm_trace_grps[] = { "arm_trace_grp" };
+static struct spear_function arm_trace_function = {
+	.name = "arm_trace",
+	.groups = arm_trace_grps,
+	.ngroups = ARRAY_SIZE(arm_trace_grps),
+};
+
+/* pad multiplexing for miphy_dbg device */
+static const unsigned miphy_dbg_pins[] = { 96, 97, 98, 99, 100, 101, 102, 103,
+	132, 135, 136, 137, 138, 139, 140, 141, 142, 143, 144, 145, 146, 147,
+	148, 149, 150, 151, 152, 153, 154, 155, 156, 157 };
+static struct spear_muxreg miphy_dbg_muxreg[] = {
+	{
+		.reg = PAD_SHARED_IP_EN_1,
+		.mask = MIPHY_DBG_MASK,
+		.val = MIPHY_DBG_MASK,
+	}, {
+		.reg = PAD_FUNCTION_EN_5,
+		.mask = DEVS_GRP_AND_MIPHY_DBG_REG4_MASK,
+		.val = DEVS_GRP_AND_MIPHY_DBG_REG4_MASK,
+	},
+};
+
+static struct spear_modemux miphy_dbg_modemux[] = {
+	{
+		.muxregs = miphy_dbg_muxreg,
+		.nmuxregs = ARRAY_SIZE(miphy_dbg_muxreg),
+	},
+};
+
+static struct spear_pingroup miphy_dbg_pingroup = {
+	.name = "miphy_dbg_grp",
+	.pins = miphy_dbg_pins,
+	.npins = ARRAY_SIZE(miphy_dbg_pins),
+	.modemuxs = miphy_dbg_modemux,
+	.nmodemuxs = ARRAY_SIZE(miphy_dbg_modemux),
+};
+
+static const char *const miphy_dbg_grps[] = { "miphy_dbg_grp" };
+static struct spear_function miphy_dbg_function = {
+	.name = "miphy_dbg",
+	.groups = miphy_dbg_grps,
+	.ngroups = ARRAY_SIZE(miphy_dbg_grps),
+};
+
+/* pad multiplexing for pcie device */
+static const unsigned pcie_pins[] = { 250 };
+static struct spear_muxreg pcie_muxreg[] = {
+	{
+		.reg = PCIE_SATA_CFG,
+		.mask = SATA_PCIE_CFG_MASK,
+		.val = PCIE_CFG_VAL,
+	},
+};
+
+static struct spear_modemux pcie_modemux[] = {
+	{
+		.muxregs = pcie_muxreg,
+		.nmuxregs = ARRAY_SIZE(pcie_muxreg),
+	},
+};
+
+static struct spear_pingroup pcie_pingroup = {
+	.name = "pcie_grp",
+	.pins = pcie_pins,
+	.npins = ARRAY_SIZE(pcie_pins),
+	.modemuxs = pcie_modemux,
+	.nmodemuxs = ARRAY_SIZE(pcie_modemux),
+};
+
+static const char *const pcie_grps[] = { "pcie_grp" };
+static struct spear_function pcie_function = {
+	.name = "pcie",
+	.groups = pcie_grps,
+	.ngroups = ARRAY_SIZE(pcie_grps),
+};
+
+/* pad multiplexing for sata device */
+static const unsigned sata_pins[] = { 250 };
+static struct spear_muxreg sata_muxreg[] = {
+	{
+		.reg = PCIE_SATA_CFG,
+		.mask = SATA_PCIE_CFG_MASK,
+		.val = SATA_CFG_VAL,
+	},
+};
+
+static struct spear_modemux sata_modemux[] = {
+	{
+		.muxregs = sata_muxreg,
+		.nmuxregs = ARRAY_SIZE(sata_muxreg),
+	},
+};
+
+static struct spear_pingroup sata_pingroup = {
+	.name = "sata_grp",
+	.pins = sata_pins,
+	.npins = ARRAY_SIZE(sata_pins),
+	.modemuxs = sata_modemux,
+	.nmodemuxs = ARRAY_SIZE(sata_modemux),
+};
+
+static const char *const sata_grps[] = { "sata_grp" };
+static struct spear_function sata_function = {
+	.name = "sata",
+	.groups = sata_grps,
+	.ngroups = ARRAY_SIZE(sata_grps),
+};
+
+/* pingroups */
+static struct spear_pingroup *spear1340_pingroups[] = {
+	&pads_as_gpio_pingroup,
+	&fsmc_8bit_pingroup,
+	&fsmc_16bit_pingroup,
+	&fsmc_pnor_pingroup,
+	&keyboard_row_col_pingroup,
+	&keyboard_col5_pingroup,
+	&spdif_in_pingroup,
+	&spdif_out_pingroup,
+	&gpt_0_1_pingroup,
+	&pwm0_pingroup,
+	&pwm1_pingroup,
+	&pwm2_pingroup,
+	&pwm3_pingroup,
+	&vip_mux_pingroup,
+	&vip_mux_cam0_pingroup,
+	&vip_mux_cam1_pingroup,
+	&vip_mux_cam2_pingroup,
+	&vip_mux_cam3_pingroup,
+	&cam0_pingroup,
+	&cam1_pingroup,
+	&cam2_pingroup,
+	&cam3_pingroup,
+	&smi_pingroup,
+	&ssp0_pingroup,
+	&ssp0_cs1_pingroup,
+	&ssp0_cs2_pingroup,
+	&ssp0_cs3_pingroup,
+	&uart0_pingroup,
+	&uart0_enh_pingroup,
+	&uart1_pingroup,
+	&i2s_in_pingroup,
+	&i2s_out_pingroup,
+	&gmii_pingroup,
+	&rgmii_pingroup,
+	&rmii_pingroup,
+	&sgmii_pingroup,
+	&i2c0_pingroup,
+	&i2c1_pingroup,
+	&cec0_pingroup,
+	&cec1_pingroup,
+	&sdhci_pingroup,
+	&cf_pingroup,
+	&xd_pingroup,
+	&clcd_pingroup,
+	&arm_trace_pingroup,
+	&miphy_dbg_pingroup,
+	&pcie_pingroup,
+	&sata_pingroup,
+};
+
+/* functions */
+static struct spear_function *spear1340_functions[] = {
+	&pads_as_gpio_function,
+	&fsmc_function,
+	&keyboard_function,
+	&spdif_in_function,
+	&spdif_out_function,
+	&gpt_0_1_function,
+	&pwm_function,
+	&vip_function,
+	&cam0_function,
+	&cam1_function,
+	&cam2_function,
+	&cam3_function,
+	&smi_function,
+	&ssp0_function,
+	&uart0_function,
+	&uart1_function,
+	&i2s_function,
+	&gmac_function,
+	&i2c0_function,
+	&i2c1_function,
+	&cec0_function,
+	&cec1_function,
+	&sdhci_function,
+	&cf_function,
+	&xd_function,
+	&clcd_function,
+	&arm_trace_function,
+	&miphy_dbg_function,
+	&pcie_function,
+	&sata_function,
+};
+
+static struct spear_pinctrl_machdata spear1340_machdata = {
+	.pins = spear1340_pins,
+	.npins = ARRAY_SIZE(spear1340_pins),
+	.groups = spear1340_pingroups,
+	.ngroups = ARRAY_SIZE(spear1340_pingroups),
+	.functions = spear1340_functions,
+	.nfunctions = ARRAY_SIZE(spear1340_functions),
+	.modes_supported = false,
+};
+
+static struct of_device_id spear1340_pinctrl_of_match[] __devinitdata = {
+	{
+		.compatible = "st,spear1340-pinmux",
+	},
+	{},
+};
+
+static int __devinit spear1340_pinctrl_probe(struct platform_device *pdev)
+{
+	return spear_pinctrl_probe(pdev, &spear1340_machdata);
+}
+
+static int __devexit spear1340_pinctrl_remove(struct platform_device *pdev)
+{
+	return spear_pinctrl_remove(pdev);
+}
+
+static struct platform_driver spear1340_pinctrl_driver = {
+	.driver = {
+		.name = DRIVER_NAME,
+		.owner = THIS_MODULE,
+		.of_match_table = spear1340_pinctrl_of_match,
+	},
+	.probe = spear1340_pinctrl_probe,
+	.remove = __devexit_p(spear1340_pinctrl_remove),
+};
+
+static int __init spear1340_pinctrl_init(void)
+{
+	return platform_driver_register(&spear1340_pinctrl_driver);
+}
+arch_initcall(spear1340_pinctrl_init);
+
+static void __exit spear1340_pinctrl_exit(void)
+{
+	platform_driver_unregister(&spear1340_pinctrl_driver);
+}
+module_exit(spear1340_pinctrl_exit);
+
+MODULE_AUTHOR("Viresh Kumar <viresh.kumar@st.com>");
+MODULE_DESCRIPTION("ST Microelectronics SPEAr1340 pinctrl driver");
+MODULE_LICENSE("GPL v2");
+MODULE_DEVICE_TABLE(of, spear1340_pinctrl_of_match);
diff --git a/drivers/pinctrl/spear/pinctrl-spear300.c b/drivers/pinctrl/spear/pinctrl-spear300.c
new file mode 100644
index 00000000000..9c82a35e4e7
--- /dev/null
+++ b/drivers/pinctrl/spear/pinctrl-spear300.c
@@ -0,0 +1,708 @@
+/*
+ * Driver for the ST Microelectronics SPEAr300 pinmux
+ *
+ * Copyright (C) 2012 ST Microelectronics
+ * Viresh Kumar <viresh.kumar@st.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/err.h>
+#include <linux/init.h>
+#include <linux/module.h>
+#include <linux/of_device.h>
+#include <linux/platform_device.h>
+#include "pinctrl-spear3xx.h"
+
+#define DRIVER_NAME "spear300-pinmux"
+
+/* addresses */
+#define PMX_CONFIG_REG			0x00
+#define MODE_CONFIG_REG			0x04
+
+/* modes */
+#define NAND_MODE			(1 << 0)
+#define NOR_MODE			(1 << 1)
+#define PHOTO_FRAME_MODE		(1 << 2)
+#define LEND_IP_PHONE_MODE		(1 << 3)
+#define HEND_IP_PHONE_MODE		(1 << 4)
+#define LEND_WIFI_PHONE_MODE		(1 << 5)
+#define HEND_WIFI_PHONE_MODE		(1 << 6)
+#define ATA_PABX_WI2S_MODE		(1 << 7)
+#define ATA_PABX_I2S_MODE		(1 << 8)
+#define CAML_LCDW_MODE			(1 << 9)
+#define CAMU_LCD_MODE			(1 << 10)
+#define CAMU_WLCD_MODE			(1 << 11)
+#define CAML_LCD_MODE			(1 << 12)
+
+static struct spear_pmx_mode pmx_mode_nand = {
+	.name = "nand",
+	.mode = NAND_MODE,
+	.reg = MODE_CONFIG_REG,
+	.mask = 0x0000000F,
+	.val = 0x00,
+};
+
+static struct spear_pmx_mode pmx_mode_nor = {
+	.name = "nor",
+	.mode = NOR_MODE,
+	.reg = MODE_CONFIG_REG,
+	.mask = 0x0000000F,
+	.val = 0x01,
+};
+
+static struct spear_pmx_mode pmx_mode_photo_frame = {
+	.name = "photo frame mode",
+	.mode = PHOTO_FRAME_MODE,
+	.reg = MODE_CONFIG_REG,
+	.mask = 0x0000000F,
+	.val = 0x02,
+};
+
+static struct spear_pmx_mode pmx_mode_lend_ip_phone = {
+	.name = "lend ip phone mode",
+	.mode = LEND_IP_PHONE_MODE,
+	.reg = MODE_CONFIG_REG,
+	.mask = 0x0000000F,
+	.val = 0x03,
+};
+
+static struct spear_pmx_mode pmx_mode_hend_ip_phone = {
+	.name = "hend ip phone mode",
+	.mode = HEND_IP_PHONE_MODE,
+	.reg = MODE_CONFIG_REG,
+	.mask = 0x0000000F,
+	.val = 0x04,
+};
+
+static struct spear_pmx_mode pmx_mode_lend_wifi_phone = {
+	.name = "lend wifi phone mode",
+	.mode = LEND_WIFI_PHONE_MODE,
+	.reg = MODE_CONFIG_REG,
+	.mask = 0x0000000F,
+	.val = 0x05,
+};
+
+static struct spear_pmx_mode pmx_mode_hend_wifi_phone = {
+	.name = "hend wifi phone mode",
+	.mode = HEND_WIFI_PHONE_MODE,
+	.reg = MODE_CONFIG_REG,
+	.mask = 0x0000000F,
+	.val = 0x06,
+};
+
+static struct spear_pmx_mode pmx_mode_ata_pabx_wi2s = {
+	.name = "ata pabx wi2s mode",
+	.mode = ATA_PABX_WI2S_MODE,
+	.reg = MODE_CONFIG_REG,
+	.mask = 0x0000000F,
+	.val = 0x07,
+};
+
+static struct spear_pmx_mode pmx_mode_ata_pabx_i2s = {
+	.name = "ata pabx i2s mode",
+	.mode = ATA_PABX_I2S_MODE,
+	.reg = MODE_CONFIG_REG,
+	.mask = 0x0000000F,
+	.val = 0x08,
+};
+
+static struct spear_pmx_mode pmx_mode_caml_lcdw = {
+	.name = "caml lcdw mode",
+	.mode = CAML_LCDW_MODE,
+	.reg = MODE_CONFIG_REG,
+	.mask = 0x0000000F,
+	.val = 0x0C,
+};
+
+static struct spear_pmx_mode pmx_mode_camu_lcd = {
+	.name = "camu lcd mode",
+	.mode = CAMU_LCD_MODE,
+	.reg = MODE_CONFIG_REG,
+	.mask = 0x0000000F,
+	.val = 0x0D,
+};
+
+static struct spear_pmx_mode pmx_mode_camu_wlcd = {
+	.name = "camu wlcd mode",
+	.mode = CAMU_WLCD_MODE,
+	.reg = MODE_CONFIG_REG,
+	.mask = 0x0000000F,
+	.val = 0xE,
+};
+
+static struct spear_pmx_mode pmx_mode_caml_lcd = {
+	.name = "caml lcd mode",
+	.mode = CAML_LCD_MODE,
+	.reg = MODE_CONFIG_REG,
+	.mask = 0x0000000F,
+	.val = 0x0F,
+};
+
+static struct spear_pmx_mode *spear300_pmx_modes[] = {
+	&pmx_mode_nand,
+	&pmx_mode_nor,
+	&pmx_mode_photo_frame,
+	&pmx_mode_lend_ip_phone,
+	&pmx_mode_hend_ip_phone,
+	&pmx_mode_lend_wifi_phone,
+	&pmx_mode_hend_wifi_phone,
+	&pmx_mode_ata_pabx_wi2s,
+	&pmx_mode_ata_pabx_i2s,
+	&pmx_mode_caml_lcdw,
+	&pmx_mode_camu_lcd,
+	&pmx_mode_camu_wlcd,
+	&pmx_mode_caml_lcd,
+};
+
+/* fsmc_2chips_pins */
+static const unsigned fsmc_2chips_pins[] = { 1, 97 };
+static struct spear_muxreg fsmc_2chips_muxreg[] = {
+	{
+		.reg = PMX_CONFIG_REG,
+		.mask = PMX_FIRDA_MASK,
+		.val = 0,
+	},
+};
+
+static struct spear_modemux fsmc_2chips_modemux[] = {
+	{
+		.modes = NAND_MODE | NOR_MODE | PHOTO_FRAME_MODE |
+			ATA_PABX_WI2S_MODE | ATA_PABX_I2S_MODE,
+		.muxregs = fsmc_2chips_muxreg,
+		.nmuxregs = ARRAY_SIZE(fsmc_2chips_muxreg),
+	},
+};
+
+static struct spear_pingroup fsmc_2chips_pingroup = {
+	.name = "fsmc_2chips_grp",
+	.pins = fsmc_2chips_pins,
+	.npins = ARRAY_SIZE(fsmc_2chips_pins),
+	.modemuxs = fsmc_2chips_modemux,
+	.nmodemuxs = ARRAY_SIZE(fsmc_2chips_modemux),
+};
+
+/* fsmc_4chips_pins */
+static const unsigned fsmc_4chips_pins[] = { 1, 2, 3, 97 };
+static struct spear_muxreg fsmc_4chips_muxreg[] = {
+	{
+		.reg = PMX_CONFIG_REG,
+		.mask = PMX_FIRDA_MASK | PMX_UART0_MASK,
+		.val = 0,
+	},
+};
+
+static struct spear_modemux fsmc_4chips_modemux[] = {
+	{
+		.modes = NAND_MODE | NOR_MODE | PHOTO_FRAME_MODE |
+			ATA_PABX_WI2S_MODE | ATA_PABX_I2S_MODE,
+		.muxregs = fsmc_4chips_muxreg,
+		.nmuxregs = ARRAY_SIZE(fsmc_4chips_muxreg),
+	},
+};
+
+static struct spear_pingroup fsmc_4chips_pingroup = {
+	.name = "fsmc_4chips_grp",
+	.pins = fsmc_4chips_pins,
+	.npins = ARRAY_SIZE(fsmc_4chips_pins),
+	.modemuxs = fsmc_4chips_modemux,
+	.nmodemuxs = ARRAY_SIZE(fsmc_4chips_modemux),
+};
+
+static const char *const fsmc_grps[] = { "fsmc_2chips_grp", "fsmc_4chips_grp"
+};
+static struct spear_function fsmc_function = {
+	.name = "fsmc",
+	.groups = fsmc_grps,
+	.ngroups = ARRAY_SIZE(fsmc_grps),
+};
+
+/* clcd_lcdmode_pins */
+static const unsigned clcd_lcdmode_pins[] = { 49, 50 };
+static struct spear_muxreg clcd_lcdmode_muxreg[] = {
+	{
+		.reg = PMX_CONFIG_REG,
+		.mask = PMX_TIMER_0_1_MASK | PMX_TIMER_2_3_MASK,
+		.val = 0,
+	},
+};
+
+static struct spear_modemux clcd_lcdmode_modemux[] = {
+	{
+		.modes = HEND_IP_PHONE_MODE | HEND_WIFI_PHONE_MODE |
+			CAMU_LCD_MODE | CAML_LCD_MODE,
+		.muxregs = clcd_lcdmode_muxreg,
+		.nmuxregs = ARRAY_SIZE(clcd_lcdmode_muxreg),
+	},
+};
+
+static struct spear_pingroup clcd_lcdmode_pingroup = {
+	.name = "clcd_lcdmode_grp",
+	.pins = clcd_lcdmode_pins,
+	.npins = ARRAY_SIZE(clcd_lcdmode_pins),
+	.modemuxs = clcd_lcdmode_modemux,
+	.nmodemuxs = ARRAY_SIZE(clcd_lcdmode_modemux),
+};
+
+/* clcd_pfmode_pins */
+static const unsigned clcd_pfmode_pins[] = { 47, 48, 49, 50 };
+static struct spear_muxreg clcd_pfmode_muxreg[] = {
+	{
+		.reg = PMX_CONFIG_REG,
+		.mask = PMX_TIMER_2_3_MASK,
+		.val = 0,
+	},
+};
+
+static struct spear_modemux clcd_pfmode_modemux[] = {
+	{
+		.modes = PHOTO_FRAME_MODE,
+		.muxregs = clcd_pfmode_muxreg,
+		.nmuxregs = ARRAY_SIZE(clcd_pfmode_muxreg),
+	},
+};
+
+static struct spear_pingroup clcd_pfmode_pingroup = {
+	.name = "clcd_pfmode_grp",
+	.pins = clcd_pfmode_pins,
+	.npins = ARRAY_SIZE(clcd_pfmode_pins),
+	.modemuxs = clcd_pfmode_modemux,
+	.nmodemuxs = ARRAY_SIZE(clcd_pfmode_modemux),
+};
+
+static const char *const clcd_grps[] = { "clcd_lcdmode_grp", "clcd_pfmode_grp"
+};
+static struct spear_function clcd_function = {
+	.name = "clcd",
+	.groups = clcd_grps,
+	.ngroups = ARRAY_SIZE(clcd_grps),
+};
+
+/* tdm_pins */
+static const unsigned tdm_pins[] = { 34, 35, 36, 37, 38 };
+static struct spear_muxreg tdm_muxreg[] = {
+	{
+		.reg = PMX_CONFIG_REG,
+		.mask = PMX_UART0_MODEM_MASK | PMX_SSP_CS_MASK,
+		.val = 0,
+	},
+};
+
+static struct spear_modemux tdm_modemux[] = {
+	{
+		.modes = PHOTO_FRAME_MODE | LEND_IP_PHONE_MODE |
+			HEND_IP_PHONE_MODE | LEND_WIFI_PHONE_MODE
+			| HEND_WIFI_PHONE_MODE | ATA_PABX_WI2S_MODE
+			| ATA_PABX_I2S_MODE | CAML_LCDW_MODE | CAMU_LCD_MODE
+			| CAMU_WLCD_MODE | CAML_LCD_MODE,
+		.muxregs = tdm_muxreg,
+		.nmuxregs = ARRAY_SIZE(tdm_muxreg),
+	},
+};
+
+static struct spear_pingroup tdm_pingroup = {
+	.name = "tdm_grp",
+	.pins = tdm_pins,
+	.npins = ARRAY_SIZE(tdm_pins),
+	.modemuxs = tdm_modemux,
+	.nmodemuxs = ARRAY_SIZE(tdm_modemux),
+};
+
+static const char *const tdm_grps[] = { "tdm_grp" };
+static struct spear_function tdm_function = {
+	.name = "tdm",
+	.groups = tdm_grps,
+	.ngroups = ARRAY_SIZE(tdm_grps),
+};
+
+/* i2c_clk_pins */
+static const unsigned i2c_clk_pins[] = { 45, 46, 47, 48 };
+static struct spear_muxreg i2c_clk_muxreg[] = {
+	{
+		.reg = PMX_CONFIG_REG,
+		.mask = PMX_TIMER_0_1_MASK | PMX_TIMER_2_3_MASK,
+		.val = 0,
+	},
+};
+
+static struct spear_modemux i2c_clk_modemux[] = {
+	{
+		.modes = LEND_IP_PHONE_MODE | HEND_IP_PHONE_MODE |
+			LEND_WIFI_PHONE_MODE | HEND_WIFI_PHONE_MODE |
+			ATA_PABX_WI2S_MODE | ATA_PABX_I2S_MODE | CAML_LCDW_MODE
+			| CAML_LCD_MODE,
+		.muxregs = i2c_clk_muxreg,
+		.nmuxregs = ARRAY_SIZE(i2c_clk_muxreg),
+	},
+};
+
+static struct spear_pingroup i2c_clk_pingroup = {
+	.name = "i2c_clk_grp_grp",
+	.pins = i2c_clk_pins,
+	.npins = ARRAY_SIZE(i2c_clk_pins),
+	.modemuxs = i2c_clk_modemux,
+	.nmodemuxs = ARRAY_SIZE(i2c_clk_modemux),
+};
+
+static const char *const i2c_grps[] = { "i2c_clk_grp" };
+static struct spear_function i2c_function = {
+	.name = "i2c1",
+	.groups = i2c_grps,
+	.ngroups = ARRAY_SIZE(i2c_grps),
+};
+
+/* caml_pins */
+static const unsigned caml_pins[] = { 12, 13, 14, 15, 16, 17, 18, 19, 20, 21 };
+static struct spear_muxreg caml_muxreg[] = {
+	{
+		.reg = PMX_CONFIG_REG,
+		.mask = PMX_MII_MASK,
+		.val = 0,
+	},
+};
+
+static struct spear_modemux caml_modemux[] = {
+	{
+		.modes = CAML_LCDW_MODE | CAML_LCD_MODE,
+		.muxregs = caml_muxreg,
+		.nmuxregs = ARRAY_SIZE(caml_muxreg),
+	},
+};
+
+static struct spear_pingroup caml_pingroup = {
+	.name = "caml_grp",
+	.pins = caml_pins,
+	.npins = ARRAY_SIZE(caml_pins),
+	.modemuxs = caml_modemux,
+	.nmodemuxs = ARRAY_SIZE(caml_modemux),
+};
+
+/* camu_pins */
+static const unsigned camu_pins[] = { 16, 17, 18, 19, 20, 21, 45, 46, 47, 48 };
+static struct spear_muxreg camu_muxreg[] = {
+	{
+		.reg = PMX_CONFIG_REG,
+		.mask = PMX_TIMER_0_1_MASK | PMX_TIMER_2_3_MASK | PMX_MII_MASK,
+		.val = 0,
+	},
+};
+
+static struct spear_modemux camu_modemux[] = {
+	{
+		.modes = CAMU_LCD_MODE | CAMU_WLCD_MODE,
+		.muxregs = camu_muxreg,
+		.nmuxregs = ARRAY_SIZE(camu_muxreg),
+	},
+};
+
+static struct spear_pingroup camu_pingroup = {
+	.name = "camu_grp",
+	.pins = camu_pins,
+	.npins = ARRAY_SIZE(camu_pins),
+	.modemuxs = camu_modemux,
+	.nmodemuxs = ARRAY_SIZE(camu_modemux),
+};
+
+static const char *const cam_grps[] = { "caml_grp", "camu_grp" };
+static struct spear_function cam_function = {
+	.name = "cam",
+	.groups = cam_grps,
+	.ngroups = ARRAY_SIZE(cam_grps),
+};
+
+/* dac_pins */
+static const unsigned dac_pins[] = { 43, 44 };
+static struct spear_muxreg dac_muxreg[] = {
+	{
+		.reg = PMX_CONFIG_REG,
+		.mask = PMX_TIMER_0_1_MASK,
+		.val = 0,
+	},
+};
+
+static struct spear_modemux dac_modemux[] = {
+	{
+		.modes = ATA_PABX_I2S_MODE | CAML_LCDW_MODE | CAMU_LCD_MODE
+			| CAMU_WLCD_MODE | CAML_LCD_MODE,
+		.muxregs = dac_muxreg,
+		.nmuxregs = ARRAY_SIZE(dac_muxreg),
+	},
+};
+
+static struct spear_pingroup dac_pingroup = {
+	.name = "dac_grp",
+	.pins = dac_pins,
+	.npins = ARRAY_SIZE(dac_pins),
+	.modemuxs = dac_modemux,
+	.nmodemuxs = ARRAY_SIZE(dac_modemux),
+};
+
+static const char *const dac_grps[] = { "dac_grp" };
+static struct spear_function dac_function = {
+	.name = "dac",
+	.groups = dac_grps,
+	.ngroups = ARRAY_SIZE(dac_grps),
+};
+
+/* i2s_pins */
+static const unsigned i2s_pins[] = { 39, 40, 41, 42 };
+static struct spear_muxreg i2s_muxreg[] = {
+	{
+		.reg = PMX_CONFIG_REG,
+		.mask = PMX_UART0_MODEM_MASK,
+		.val = 0,
+	},
+};
+
+static struct spear_modemux i2s_modemux[] = {
+	{
+		.modes = LEND_IP_PHONE_MODE | HEND_IP_PHONE_MODE
+			| LEND_WIFI_PHONE_MODE | HEND_WIFI_PHONE_MODE |
+			ATA_PABX_I2S_MODE | CAML_LCDW_MODE | CAMU_LCD_MODE
+			| CAMU_WLCD_MODE | CAML_LCD_MODE,
+		.muxregs = i2s_muxreg,
+		.nmuxregs = ARRAY_SIZE(i2s_muxreg),
+	},
+};
+
+static struct spear_pingroup i2s_pingroup = {
+	.name = "i2s_grp",
+	.pins = i2s_pins,
+	.npins = ARRAY_SIZE(i2s_pins),
+	.modemuxs = i2s_modemux,
+	.nmodemuxs = ARRAY_SIZE(i2s_modemux),
+};
+
+static const char *const i2s_grps[] = { "i2s_grp" };
+static struct spear_function i2s_function = {
+	.name = "i2s",
+	.groups = i2s_grps,
+	.ngroups = ARRAY_SIZE(i2s_grps),
+};
+
+/* sdhci_4bit_pins */
+static const unsigned sdhci_4bit_pins[] = { 28, 29, 30, 31, 32, 33 };
+static struct spear_muxreg sdhci_4bit_muxreg[] = {
+	{
+		.reg = PMX_CONFIG_REG,
+		.mask = PMX_GPIO_PIN0_MASK | PMX_GPIO_PIN1_MASK |
+			PMX_GPIO_PIN2_MASK | PMX_GPIO_PIN3_MASK |
+			PMX_GPIO_PIN4_MASK | PMX_GPIO_PIN5_MASK,
+		.val = 0,
+	},
+};
+
+static struct spear_modemux sdhci_4bit_modemux[] = {
+	{
+		.modes = PHOTO_FRAME_MODE | LEND_IP_PHONE_MODE |
+			HEND_IP_PHONE_MODE | LEND_WIFI_PHONE_MODE |
+			HEND_WIFI_PHONE_MODE | CAML_LCDW_MODE | CAMU_LCD_MODE |
+			CAMU_WLCD_MODE | CAML_LCD_MODE | ATA_PABX_WI2S_MODE,
+		.muxregs = sdhci_4bit_muxreg,
+		.nmuxregs = ARRAY_SIZE(sdhci_4bit_muxreg),
+	},
+};
+
+static struct spear_pingroup sdhci_4bit_pingroup = {
+	.name = "sdhci_4bit_grp",
+	.pins = sdhci_4bit_pins,
+	.npins = ARRAY_SIZE(sdhci_4bit_pins),
+	.modemuxs = sdhci_4bit_modemux,
+	.nmodemuxs = ARRAY_SIZE(sdhci_4bit_modemux),
+};
+
+/* sdhci_8bit_pins */
+static const unsigned sdhci_8bit_pins[] = { 24, 25, 26, 27, 28, 29, 30, 31, 32,
+	33 };
+static struct spear_muxreg sdhci_8bit_muxreg[] = {
+	{
+		.reg = PMX_CONFIG_REG,
+		.mask = PMX_GPIO_PIN0_MASK | PMX_GPIO_PIN1_MASK |
+			PMX_GPIO_PIN2_MASK | PMX_GPIO_PIN3_MASK |
+			PMX_GPIO_PIN4_MASK | PMX_GPIO_PIN5_MASK | PMX_MII_MASK,
+		.val = 0,
+	},
+};
+
+static struct spear_modemux sdhci_8bit_modemux[] = {
+	{
+		.modes = PHOTO_FRAME_MODE | LEND_IP_PHONE_MODE |
+			HEND_IP_PHONE_MODE | LEND_WIFI_PHONE_MODE |
+			HEND_WIFI_PHONE_MODE | CAML_LCDW_MODE | CAMU_LCD_MODE |
+			CAMU_WLCD_MODE | CAML_LCD_MODE,
+		.muxregs = sdhci_8bit_muxreg,
+		.nmuxregs = ARRAY_SIZE(sdhci_8bit_muxreg),
+	},
+};
+
+static struct spear_pingroup sdhci_8bit_pingroup = {
+	.name = "sdhci_8bit_grp",
+	.pins = sdhci_8bit_pins,
+	.npins = ARRAY_SIZE(sdhci_8bit_pins),
+	.modemuxs = sdhci_8bit_modemux,
+	.nmodemuxs = ARRAY_SIZE(sdhci_8bit_modemux),
+};
+
+static const char *const sdhci_grps[] = { "sdhci_4bit_grp", "sdhci_8bit_grp" };
+static struct spear_function sdhci_function = {
+	.name = "sdhci",
+	.groups = sdhci_grps,
+	.ngroups = ARRAY_SIZE(sdhci_grps),
+};
+
+/* gpio1_0_to_3_pins */
+static const unsigned gpio1_0_to_3_pins[] = { 39, 40, 41, 42 };
+static struct spear_muxreg gpio1_0_to_3_muxreg[] = {
+	{
+		.reg = PMX_CONFIG_REG,
+		.mask = PMX_UART0_MODEM_MASK,
+		.val = 0,
+	},
+};
+
+static struct spear_modemux gpio1_0_to_3_modemux[] = {
+	{
+		.modes = PHOTO_FRAME_MODE,
+		.muxregs = gpio1_0_to_3_muxreg,
+		.nmuxregs = ARRAY_SIZE(gpio1_0_to_3_muxreg),
+	},
+};
+
+static struct spear_pingroup gpio1_0_to_3_pingroup = {
+	.name = "gpio1_0_to_3_grp",
+	.pins = gpio1_0_to_3_pins,
+	.npins = ARRAY_SIZE(gpio1_0_to_3_pins),
+	.modemuxs = gpio1_0_to_3_modemux,
+	.nmodemuxs = ARRAY_SIZE(gpio1_0_to_3_modemux),
+};
+
+/* gpio1_4_to_7_pins */
+static const unsigned gpio1_4_to_7_pins[] = { 43, 44, 45, 46 };
+
+static struct spear_muxreg gpio1_4_to_7_muxreg[] = {
+	{
+		.reg = PMX_CONFIG_REG,
+		.mask = PMX_TIMER_0_1_MASK | PMX_TIMER_2_3_MASK,
+		.val = 0,
+	},
+};
+
+static struct spear_modemux gpio1_4_to_7_modemux[] = {
+	{
+		.modes = PHOTO_FRAME_MODE,
+		.muxregs = gpio1_4_to_7_muxreg,
+		.nmuxregs = ARRAY_SIZE(gpio1_4_to_7_muxreg),
+	},
+};
+
+static struct spear_pingroup gpio1_4_to_7_pingroup = {
+	.name = "gpio1_4_to_7_grp",
+	.pins = gpio1_4_to_7_pins,
+	.npins = ARRAY_SIZE(gpio1_4_to_7_pins),
+	.modemuxs = gpio1_4_to_7_modemux,
+	.nmodemuxs = ARRAY_SIZE(gpio1_4_to_7_modemux),
+};
+
+static const char *const gpio1_grps[] = { "gpio1_0_to_3_grp", "gpio1_4_to_7_grp"
+};
+static struct spear_function gpio1_function = {
+	.name = "gpio1",
+	.groups = gpio1_grps,
+	.ngroups = ARRAY_SIZE(gpio1_grps),
+};
+
+/* pingroups */
+static struct spear_pingroup *spear300_pingroups[] = {
+	SPEAR3XX_COMMON_PINGROUPS,
+	&fsmc_2chips_pingroup,
+	&fsmc_4chips_pingroup,
+	&clcd_lcdmode_pingroup,
+	&clcd_pfmode_pingroup,
+	&tdm_pingroup,
+	&i2c_clk_pingroup,
+	&caml_pingroup,
+	&camu_pingroup,
+	&dac_pingroup,
+	&i2s_pingroup,
+	&sdhci_4bit_pingroup,
+	&sdhci_8bit_pingroup,
+	&gpio1_0_to_3_pingroup,
+	&gpio1_4_to_7_pingroup,
+};
+
+/* functions */
+static struct spear_function *spear300_functions[] = {
+	SPEAR3XX_COMMON_FUNCTIONS,
+	&fsmc_function,
+	&clcd_function,
+	&tdm_function,
+	&i2c_function,
+	&cam_function,
+	&dac_function,
+	&i2s_function,
+	&sdhci_function,
+	&gpio1_function,
+};
+
+static struct of_device_id spear300_pinctrl_of_match[] __devinitdata = {
+	{
+		.compatible = "st,spear300-pinmux",
+	},
+	{},
+};
+
+static int __devinit spear300_pinctrl_probe(struct platform_device *pdev)
+{
+	int ret;
+
+	spear3xx_machdata.groups = spear300_pingroups;
+	spear3xx_machdata.ngroups = ARRAY_SIZE(spear300_pingroups);
+	spear3xx_machdata.functions = spear300_functions;
+	spear3xx_machdata.nfunctions = ARRAY_SIZE(spear300_functions);
+
+	spear3xx_machdata.modes_supported = true;
+	spear3xx_machdata.pmx_modes = spear300_pmx_modes;
+	spear3xx_machdata.npmx_modes = ARRAY_SIZE(spear300_pmx_modes);
+
+	pmx_init_addr(&spear3xx_machdata, PMX_CONFIG_REG);
+
+	ret = spear_pinctrl_probe(pdev, &spear3xx_machdata);
+	if (ret)
+		return ret;
+
+	return 0;
+}
+
+static int __devexit spear300_pinctrl_remove(struct platform_device *pdev)
+{
+	return spear_pinctrl_remove(pdev);
+}
+
+static struct platform_driver spear300_pinctrl_driver = {
+	.driver = {
+		.name = DRIVER_NAME,
+		.owner = THIS_MODULE,
+		.of_match_table = spear300_pinctrl_of_match,
+	},
+	.probe = spear300_pinctrl_probe,
+	.remove = __devexit_p(spear300_pinctrl_remove),
+};
+
+static int __init spear300_pinctrl_init(void)
+{
+	return platform_driver_register(&spear300_pinctrl_driver);
+}
+arch_initcall(spear300_pinctrl_init);
+
+static void __exit spear300_pinctrl_exit(void)
+{
+	platform_driver_unregister(&spear300_pinctrl_driver);
+}
+module_exit(spear300_pinctrl_exit);
+
+MODULE_AUTHOR("Viresh Kumar <viresh.kumar@st.com>");
+MODULE_DESCRIPTION("ST Microelectronics SPEAr300 pinctrl driver");
+MODULE_LICENSE("GPL v2");
+MODULE_DEVICE_TABLE(of, spear300_pinctrl_of_match);
diff --git a/drivers/pinctrl/spear/pinctrl-spear310.c b/drivers/pinctrl/spear/pinctrl-spear310.c
new file mode 100644
index 00000000000..1a970760512
--- /dev/null
+++ b/drivers/pinctrl/spear/pinctrl-spear310.c
@@ -0,0 +1,431 @@
+/*
+ * Driver for the ST Microelectronics SPEAr310 pinmux
+ *
+ * Copyright (C) 2012 ST Microelectronics
+ * Viresh Kumar <viresh.kumar@st.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/err.h>
+#include <linux/init.h>
+#include <linux/module.h>
+#include <linux/of_device.h>
+#include <linux/platform_device.h>
+#include "pinctrl-spear3xx.h"
+
+#define DRIVER_NAME "spear310-pinmux"
+
+/* addresses */
+#define PMX_CONFIG_REG			0x08
+
+/* emi_cs_0_to_5_pins */
+static const unsigned emi_cs_0_to_5_pins[] = { 45, 46, 47, 48, 49, 50 };
+static struct spear_muxreg emi_cs_0_to_5_muxreg[] = {
+	{
+		.reg = PMX_CONFIG_REG,
+		.mask = PMX_TIMER_0_1_MASK | PMX_TIMER_2_3_MASK,
+		.val = 0,
+	},
+};
+
+static struct spear_modemux emi_cs_0_to_5_modemux[] = {
+	{
+		.muxregs = emi_cs_0_to_5_muxreg,
+		.nmuxregs = ARRAY_SIZE(emi_cs_0_to_5_muxreg),
+	},
+};
+
+static struct spear_pingroup emi_cs_0_to_5_pingroup = {
+	.name = "emi_cs_0_to_5_grp",
+	.pins = emi_cs_0_to_5_pins,
+	.npins = ARRAY_SIZE(emi_cs_0_to_5_pins),
+	.modemuxs = emi_cs_0_to_5_modemux,
+	.nmodemuxs = ARRAY_SIZE(emi_cs_0_to_5_modemux),
+};
+
+static const char *const emi_cs_0_to_5_grps[] = { "emi_cs_0_to_5_grp" };
+static struct spear_function emi_cs_0_to_5_function = {
+	.name = "emi",
+	.groups = emi_cs_0_to_5_grps,
+	.ngroups = ARRAY_SIZE(emi_cs_0_to_5_grps),
+};
+
+/* uart1_pins */
+static const unsigned uart1_pins[] = { 0, 1 };
+static struct spear_muxreg uart1_muxreg[] = {
+	{
+		.reg = PMX_CONFIG_REG,
+		.mask = PMX_FIRDA_MASK,
+		.val = 0,
+	},
+};
+
+static struct spear_modemux uart1_modemux[] = {
+	{
+		.muxregs = uart1_muxreg,
+		.nmuxregs = ARRAY_SIZE(uart1_muxreg),
+	},
+};
+
+static struct spear_pingroup uart1_pingroup = {
+	.name = "uart1_grp",
+	.pins = uart1_pins,
+	.npins = ARRAY_SIZE(uart1_pins),
+	.modemuxs = uart1_modemux,
+	.nmodemuxs = ARRAY_SIZE(uart1_modemux),
+};
+
+static const char *const uart1_grps[] = { "uart1_grp" };
+static struct spear_function uart1_function = {
+	.name = "uart1",
+	.groups = uart1_grps,
+	.ngroups = ARRAY_SIZE(uart1_grps),
+};
+
+/* uart2_pins */
+static const unsigned uart2_pins[] = { 43, 44 };
+static struct spear_muxreg uart2_muxreg[] = {
+	{
+		.reg = PMX_CONFIG_REG,
+		.mask = PMX_TIMER_0_1_MASK,
+		.val = 0,
+	},
+};
+
+static struct spear_modemux uart2_modemux[] = {
+	{
+		.muxregs = uart2_muxreg,
+		.nmuxregs = ARRAY_SIZE(uart2_muxreg),
+	},
+};
+
+static struct spear_pingroup uart2_pingroup = {
+	.name = "uart2_grp",
+	.pins = uart2_pins,
+	.npins = ARRAY_SIZE(uart2_pins),
+	.modemuxs = uart2_modemux,
+	.nmodemuxs = ARRAY_SIZE(uart2_modemux),
+};
+
+static const char *const uart2_grps[] = { "uart2_grp" };
+static struct spear_function uart2_function = {
+	.name = "uart2",
+	.groups = uart2_grps,
+	.ngroups = ARRAY_SIZE(uart2_grps),
+};
+
+/* uart3_pins */
+static const unsigned uart3_pins[] = { 37, 38 };
+static struct spear_muxreg uart3_muxreg[] = {
+	{
+		.reg = PMX_CONFIG_REG,
+		.mask = PMX_UART0_MODEM_MASK,
+		.val = 0,
+	},
+};
+
+static struct spear_modemux uart3_modemux[] = {
+	{
+		.muxregs = uart3_muxreg,
+		.nmuxregs = ARRAY_SIZE(uart3_muxreg),
+	},
+};
+
+static struct spear_pingroup uart3_pingroup = {
+	.name = "uart3_grp",
+	.pins = uart3_pins,
+	.npins = ARRAY_SIZE(uart3_pins),
+	.modemuxs = uart3_modemux,
+	.nmodemuxs = ARRAY_SIZE(uart3_modemux),
+};
+
+static const char *const uart3_grps[] = { "uart3_grp" };
+static struct spear_function uart3_function = {
+	.name = "uart3",
+	.groups = uart3_grps,
+	.ngroups = ARRAY_SIZE(uart3_grps),
+};
+
+/* uart4_pins */
+static const unsigned uart4_pins[] = { 39, 40 };
+static struct spear_muxreg uart4_muxreg[] = {
+	{
+		.reg = PMX_CONFIG_REG,
+		.mask = PMX_UART0_MODEM_MASK,
+		.val = 0,
+	},
+};
+
+static struct spear_modemux uart4_modemux[] = {
+	{
+		.muxregs = uart4_muxreg,
+		.nmuxregs = ARRAY_SIZE(uart4_muxreg),
+	},
+};
+
+static struct spear_pingroup uart4_pingroup = {
+	.name = "uart4_grp",
+	.pins = uart4_pins,
+	.npins = ARRAY_SIZE(uart4_pins),
+	.modemuxs = uart4_modemux,
+	.nmodemuxs = ARRAY_SIZE(uart4_modemux),
+};
+
+static const char *const uart4_grps[] = { "uart4_grp" };
+static struct spear_function uart4_function = {
+	.name = "uart4",
+	.groups = uart4_grps,
+	.ngroups = ARRAY_SIZE(uart4_grps),
+};
+
+/* uart5_pins */
+static const unsigned uart5_pins[] = { 41, 42 };
+static struct spear_muxreg uart5_muxreg[] = {
+	{
+		.reg = PMX_CONFIG_REG,
+		.mask = PMX_UART0_MODEM_MASK,
+		.val = 0,
+	},
+};
+
+static struct spear_modemux uart5_modemux[] = {
+	{
+		.muxregs = uart5_muxreg,
+		.nmuxregs = ARRAY_SIZE(uart5_muxreg),
+	},
+};
+
+static struct spear_pingroup uart5_pingroup = {
+	.name = "uart5_grp",
+	.pins = uart5_pins,
+	.npins = ARRAY_SIZE(uart5_pins),
+	.modemuxs = uart5_modemux,
+	.nmodemuxs = ARRAY_SIZE(uart5_modemux),
+};
+
+static const char *const uart5_grps[] = { "uart5_grp" };
+static struct spear_function uart5_function = {
+	.name = "uart5",
+	.groups = uart5_grps,
+	.ngroups = ARRAY_SIZE(uart5_grps),
+};
+
+/* fsmc_pins */
+static const unsigned fsmc_pins[] = { 34, 35, 36 };
+static struct spear_muxreg fsmc_muxreg[] = {
+	{
+		.reg = PMX_CONFIG_REG,
+		.mask = PMX_SSP_CS_MASK,
+		.val = 0,
+	},
+};
+
+static struct spear_modemux fsmc_modemux[] = {
+	{
+		.muxregs = fsmc_muxreg,
+		.nmuxregs = ARRAY_SIZE(fsmc_muxreg),
+	},
+};
+
+static struct spear_pingroup fsmc_pingroup = {
+	.name = "fsmc_grp",
+	.pins = fsmc_pins,
+	.npins = ARRAY_SIZE(fsmc_pins),
+	.modemuxs = fsmc_modemux,
+	.nmodemuxs = ARRAY_SIZE(fsmc_modemux),
+};
+
+static const char *const fsmc_grps[] = { "fsmc_grp" };
+static struct spear_function fsmc_function = {
+	.name = "fsmc",
+	.groups = fsmc_grps,
+	.ngroups = ARRAY_SIZE(fsmc_grps),
+};
+
+/* rs485_0_pins */
+static const unsigned rs485_0_pins[] = { 19, 20, 21, 22, 23 };
+static struct spear_muxreg rs485_0_muxreg[] = {
+	{
+		.reg = PMX_CONFIG_REG,
+		.mask = PMX_MII_MASK,
+		.val = 0,
+	},
+};
+
+static struct spear_modemux rs485_0_modemux[] = {
+	{
+		.muxregs = rs485_0_muxreg,
+		.nmuxregs = ARRAY_SIZE(rs485_0_muxreg),
+	},
+};
+
+static struct spear_pingroup rs485_0_pingroup = {
+	.name = "rs485_0_grp",
+	.pins = rs485_0_pins,
+	.npins = ARRAY_SIZE(rs485_0_pins),
+	.modemuxs = rs485_0_modemux,
+	.nmodemuxs = ARRAY_SIZE(rs485_0_modemux),
+};
+
+static const char *const rs485_0_grps[] = { "rs485_0" };
+static struct spear_function rs485_0_function = {
+	.name = "rs485_0",
+	.groups = rs485_0_grps,
+	.ngroups = ARRAY_SIZE(rs485_0_grps),
+};
+
+/* rs485_1_pins */
+static const unsigned rs485_1_pins[] = { 14, 15, 16, 17, 18 };
+static struct spear_muxreg rs485_1_muxreg[] = {
+	{
+		.reg = PMX_CONFIG_REG,
+		.mask = PMX_MII_MASK,
+		.val = 0,
+	},
+};
+
+static struct spear_modemux rs485_1_modemux[] = {
+	{
+		.muxregs = rs485_1_muxreg,
+		.nmuxregs = ARRAY_SIZE(rs485_1_muxreg),
+	},
+};
+
+static struct spear_pingroup rs485_1_pingroup = {
+	.name = "rs485_1_grp",
+	.pins = rs485_1_pins,
+	.npins = ARRAY_SIZE(rs485_1_pins),
+	.modemuxs = rs485_1_modemux,
+	.nmodemuxs = ARRAY_SIZE(rs485_1_modemux),
+};
+
+static const char *const rs485_1_grps[] = { "rs485_1" };
+static struct spear_function rs485_1_function = {
+	.name = "rs485_1",
+	.groups = rs485_1_grps,
+	.ngroups = ARRAY_SIZE(rs485_1_grps),
+};
+
+/* tdm_pins */
+static const unsigned tdm_pins[] = { 10, 11, 12, 13 };
+static struct spear_muxreg tdm_muxreg[] = {
+	{
+		.reg = PMX_CONFIG_REG,
+		.mask = PMX_MII_MASK,
+		.val = 0,
+	},
+};
+
+static struct spear_modemux tdm_modemux[] = {
+	{
+		.muxregs = tdm_muxreg,
+		.nmuxregs = ARRAY_SIZE(tdm_muxreg),
+	},
+};
+
+static struct spear_pingroup tdm_pingroup = {
+	.name = "tdm_grp",
+	.pins = tdm_pins,
+	.npins = ARRAY_SIZE(tdm_pins),
+	.modemuxs = tdm_modemux,
+	.nmodemuxs = ARRAY_SIZE(tdm_modemux),
+};
+
+static const char *const tdm_grps[] = { "tdm_grp" };
+static struct spear_function tdm_function = {
+	.name = "tdm",
+	.groups = tdm_grps,
+	.ngroups = ARRAY_SIZE(tdm_grps),
+};
+
+/* pingroups */
+static struct spear_pingroup *spear310_pingroups[] = {
+	SPEAR3XX_COMMON_PINGROUPS,
+	&emi_cs_0_to_5_pingroup,
+	&uart1_pingroup,
+	&uart2_pingroup,
+	&uart3_pingroup,
+	&uart4_pingroup,
+	&uart5_pingroup,
+	&fsmc_pingroup,
+	&rs485_0_pingroup,
+	&rs485_1_pingroup,
+	&tdm_pingroup,
+};
+
+/* functions */
+static struct spear_function *spear310_functions[] = {
+	SPEAR3XX_COMMON_FUNCTIONS,
+	&emi_cs_0_to_5_function,
+	&uart1_function,
+	&uart2_function,
+	&uart3_function,
+	&uart4_function,
+	&uart5_function,
+	&fsmc_function,
+	&rs485_0_function,
+	&rs485_1_function,
+	&tdm_function,
+};
+
+static struct of_device_id spear310_pinctrl_of_match[] __devinitdata = {
+	{
+		.compatible = "st,spear310-pinmux",
+	},
+	{},
+};
+
+static int __devinit spear310_pinctrl_probe(struct platform_device *pdev)
+{
+	int ret;
+
+	spear3xx_machdata.groups = spear310_pingroups;
+	spear3xx_machdata.ngroups = ARRAY_SIZE(spear310_pingroups);
+	spear3xx_machdata.functions = spear310_functions;
+	spear3xx_machdata.nfunctions = ARRAY_SIZE(spear310_functions);
+
+	pmx_init_addr(&spear3xx_machdata, PMX_CONFIG_REG);
+
+	spear3xx_machdata.modes_supported = false;
+
+	ret = spear_pinctrl_probe(pdev, &spear3xx_machdata);
+	if (ret)
+		return ret;
+
+	return 0;
+}
+
+static int __devexit spear310_pinctrl_remove(struct platform_device *pdev)
+{
+	return spear_pinctrl_remove(pdev);
+}
+
+static struct platform_driver spear310_pinctrl_driver = {
+	.driver = {
+		.name = DRIVER_NAME,
+		.owner = THIS_MODULE,
+		.of_match_table = spear310_pinctrl_of_match,
+	},
+	.probe = spear310_pinctrl_probe,
+	.remove = __devexit_p(spear310_pinctrl_remove),
+};
+
+static int __init spear310_pinctrl_init(void)
+{
+	return platform_driver_register(&spear310_pinctrl_driver);
+}
+arch_initcall(spear310_pinctrl_init);
+
+static void __exit spear310_pinctrl_exit(void)
+{
+	platform_driver_unregister(&spear310_pinctrl_driver);
+}
+module_exit(spear310_pinctrl_exit);
+
+MODULE_AUTHOR("Viresh Kumar <viresh.kumar@st.com>");
+MODULE_DESCRIPTION("ST Microelectronics SPEAr310 pinctrl driver");
+MODULE_LICENSE("GPL v2");
+MODULE_DEVICE_TABLE(of, SPEAr310_pinctrl_of_match);
diff --git a/drivers/pinctrl/spear/pinctrl-spear320.c b/drivers/pinctrl/spear/pinctrl-spear320.c
new file mode 100644
index 00000000000..de726e6c283
--- /dev/null
+++ b/drivers/pinctrl/spear/pinctrl-spear320.c
@@ -0,0 +1,3468 @@
+/*
+ * Driver for the ST Microelectronics SPEAr320 pinmux
+ *
+ * Copyright (C) 2012 ST Microelectronics
+ * Viresh Kumar <viresh.kumar@st.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/err.h>
+#include <linux/init.h>
+#include <linux/module.h>
+#include <linux/of_device.h>
+#include <linux/platform_device.h>
+#include "pinctrl-spear3xx.h"
+
+#define DRIVER_NAME "spear320-pinmux"
+
+/* addresses */
+#define PMX_CONFIG_REG			0x0C
+#define MODE_CONFIG_REG			0x10
+#define MODE_EXT_CONFIG_REG		0x18
+
+/* modes */
+#define AUTO_NET_SMII_MODE	(1 << 0)
+#define AUTO_NET_MII_MODE	(1 << 1)
+#define AUTO_EXP_MODE		(1 << 2)
+#define SMALL_PRINTERS_MODE	(1 << 3)
+#define EXTENDED_MODE		(1 << 4)
+
+static struct spear_pmx_mode pmx_mode_auto_net_smii = {
+	.name = "Automation Networking SMII mode",
+	.mode = AUTO_NET_SMII_MODE,
+	.reg = MODE_CONFIG_REG,
+	.mask = 0x00000007,
+	.val = 0x0,
+};
+
+static struct spear_pmx_mode pmx_mode_auto_net_mii = {
+	.name = "Automation Networking MII mode",
+	.mode = AUTO_NET_MII_MODE,
+	.reg = MODE_CONFIG_REG,
+	.mask = 0x00000007,
+	.val = 0x1,
+};
+
+static struct spear_pmx_mode pmx_mode_auto_exp = {
+	.name = "Automation Expanded mode",
+	.mode = AUTO_EXP_MODE,
+	.reg = MODE_CONFIG_REG,
+	.mask = 0x00000007,
+	.val = 0x2,
+};
+
+static struct spear_pmx_mode pmx_mode_small_printers = {
+	.name = "Small Printers mode",
+	.mode = SMALL_PRINTERS_MODE,
+	.reg = MODE_CONFIG_REG,
+	.mask = 0x00000007,
+	.val = 0x3,
+};
+
+static struct spear_pmx_mode pmx_mode_extended = {
+	.name = "extended mode",
+	.mode = EXTENDED_MODE,
+	.reg = MODE_EXT_CONFIG_REG,
+	.mask = 0x00000001,
+	.val = 0x1,
+};
+
+static struct spear_pmx_mode *spear320_pmx_modes[] = {
+	&pmx_mode_auto_net_smii,
+	&pmx_mode_auto_net_mii,
+	&pmx_mode_auto_exp,
+	&pmx_mode_small_printers,
+	&pmx_mode_extended,
+};
+
+/* Extended mode registers and their offsets */
+#define EXT_CTRL_REG				0x0018
+	#define MII_MDIO_MASK			(1 << 4)
+	#define MII_MDIO_10_11_VAL		0
+	#define MII_MDIO_81_VAL			(1 << 4)
+	#define EMI_FSMC_DYNAMIC_MUX_MASK	(1 << 5)
+	#define MAC_MODE_MII			0
+	#define MAC_MODE_RMII			1
+	#define MAC_MODE_SMII			2
+	#define MAC_MODE_SS_SMII		3
+	#define MAC_MODE_MASK			0x3
+	#define MAC1_MODE_SHIFT			16
+	#define MAC2_MODE_SHIFT			18
+
+#define IP_SEL_PAD_0_9_REG			0x00A4
+	#define PMX_PL_0_1_MASK			(0x3F << 0)
+	#define PMX_UART2_PL_0_1_VAL		0x0
+	#define PMX_I2C2_PL_0_1_VAL		(0x4 | (0x4 << 3))
+
+	#define PMX_PL_2_3_MASK			(0x3F << 6)
+	#define PMX_I2C2_PL_2_3_VAL		0x0
+	#define PMX_UART6_PL_2_3_VAL		((0x1 << 6) | (0x1 << 9))
+	#define PMX_UART1_ENH_PL_2_3_VAL	((0x4 << 6) | (0x4 << 9))
+
+	#define PMX_PL_4_5_MASK			(0x3F << 12)
+	#define PMX_UART5_PL_4_5_VAL		((0x1 << 12) | (0x1 << 15))
+	#define PMX_UART1_ENH_PL_4_5_VAL	((0x4 << 12) | (0x4 << 15))
+	#define PMX_PL_5_MASK			(0x7 << 15)
+	#define PMX_TOUCH_Y_PL_5_VAL		0x0
+
+	#define PMX_PL_6_7_MASK			(0x3F << 18)
+	#define PMX_PL_6_MASK			(0x7 << 18)
+	#define PMX_PL_7_MASK			(0x7 << 21)
+	#define PMX_UART4_PL_6_7_VAL		((0x1 << 18) | (0x1 << 21))
+	#define PMX_PWM_3_PL_6_VAL		(0x2 << 18)
+	#define PMX_PWM_2_PL_7_VAL		(0x2 << 21)
+	#define PMX_UART1_ENH_PL_6_7_VAL	((0x4 << 18) | (0x4 << 21))
+
+	#define PMX_PL_8_9_MASK			(0x3F << 24)
+	#define PMX_UART3_PL_8_9_VAL		((0x1 << 24) | (0x1 << 27))
+	#define PMX_PWM_0_1_PL_8_9_VAL		((0x2 << 24) | (0x2 << 27))
+	#define PMX_I2C1_PL_8_9_VAL		((0x4 << 24) | (0x4 << 27))
+
+#define IP_SEL_PAD_10_19_REG			0x00A8
+	#define PMX_PL_10_11_MASK		(0x3F << 0)
+	#define PMX_SMII_PL_10_11_VAL		0
+	#define PMX_RMII_PL_10_11_VAL		((0x4 << 0) | (0x4 << 3))
+
+	#define PMX_PL_12_MASK			(0x7 << 6)
+	#define PMX_PWM3_PL_12_VAL		0
+	#define PMX_SDHCI_CD_PL_12_VAL		(0x4 << 6)
+
+	#define PMX_PL_13_14_MASK		(0x3F << 9)
+	#define PMX_PL_13_MASK			(0x7 << 9)
+	#define PMX_PL_14_MASK			(0x7 << 12)
+	#define PMX_SSP2_PL_13_14_15_16_VAL	0
+	#define PMX_UART4_PL_13_14_VAL		((0x1 << 9) | (0x1 << 12))
+	#define PMX_RMII_PL_13_14_VAL		((0x4 << 9) | (0x4 << 12))
+	#define PMX_PWM2_PL_13_VAL		(0x2 << 9)
+	#define PMX_PWM1_PL_14_VAL		(0x2 << 12)
+
+	#define PMX_PL_15_MASK			(0x7 << 15)
+	#define PMX_PWM0_PL_15_VAL		(0x2 << 15)
+	#define PMX_PL_15_16_MASK		(0x3F << 15)
+	#define PMX_UART3_PL_15_16_VAL		((0x1 << 15) | (0x1 << 18))
+	#define PMX_RMII_PL_15_16_VAL		((0x4 << 15) | (0x4 << 18))
+
+	#define PMX_PL_17_18_MASK		(0x3F << 21)
+	#define PMX_SSP1_PL_17_18_19_20_VAL	0
+	#define PMX_RMII_PL_17_18_VAL		((0x4 << 21) | (0x4 << 24))
+
+	#define PMX_PL_19_MASK			(0x7 << 27)
+	#define PMX_I2C2_PL_19_VAL		(0x1 << 27)
+	#define PMX_RMII_PL_19_VAL		(0x4 << 27)
+
+#define IP_SEL_PAD_20_29_REG			0x00AC
+	#define PMX_PL_20_MASK			(0x7 << 0)
+	#define PMX_I2C2_PL_20_VAL		(0x1 << 0)
+	#define PMX_RMII_PL_20_VAL		(0x4 << 0)
+
+	#define PMX_PL_21_TO_27_MASK		(0x1FFFFF << 3)
+	#define PMX_SMII_PL_21_TO_27_VAL	0
+	#define PMX_RMII_PL_21_TO_27_VAL	((0x4 << 3) | (0x4 << 6) | (0x4 << 9) | (0x4 << 12) | (0x4 << 15) | (0x4 << 18) | (0x4 << 21))
+
+	#define PMX_PL_28_29_MASK		(0x3F << 24)
+	#define PMX_PL_28_MASK			(0x7 << 24)
+	#define PMX_PL_29_MASK			(0x7 << 27)
+	#define PMX_UART1_PL_28_29_VAL		0
+	#define PMX_PWM_3_PL_28_VAL		(0x4 << 24)
+	#define PMX_PWM_2_PL_29_VAL		(0x4 << 27)
+
+#define IP_SEL_PAD_30_39_REG			0x00B0
+	#define PMX_PL_30_31_MASK		(0x3F << 0)
+	#define PMX_CAN1_PL_30_31_VAL		(0)
+	#define PMX_PL_30_MASK			(0x7 << 0)
+	#define PMX_PL_31_MASK			(0x7 << 3)
+	#define PMX_PWM1_EXT_PL_30_VAL		(0x4 << 0)
+	#define PMX_PWM0_EXT_PL_31_VAL		(0x4 << 3)
+	#define PMX_UART1_ENH_PL_31_VAL		(0x3 << 3)
+
+	#define PMX_PL_32_33_MASK		(0x3F << 6)
+	#define PMX_CAN0_PL_32_33_VAL		0
+	#define PMX_UART1_ENH_PL_32_33_VAL	((0x3 << 6) | (0x3 << 9))
+	#define PMX_SSP2_PL_32_33_VAL		((0x4 << 6) | (0x4 << 9))
+
+	#define PMX_PL_34_MASK			(0x7 << 12)
+	#define PMX_PWM2_PL_34_VAL		0
+	#define PMX_UART1_ENH_PL_34_VAL		(0x2 << 12)
+	#define PMX_SSP2_PL_34_VAL		(0x4 << 12)
+
+	#define PMX_PL_35_MASK			(0x7 << 15)
+	#define PMX_I2S_REF_CLK_PL_35_VAL	0
+	#define PMX_UART1_ENH_PL_35_VAL		(0x2 << 15)
+	#define PMX_SSP2_PL_35_VAL		(0x4 << 15)
+
+	#define PMX_PL_36_MASK			(0x7 << 18)
+	#define PMX_TOUCH_X_PL_36_VAL		0
+	#define PMX_UART1_ENH_PL_36_VAL		(0x2 << 18)
+	#define PMX_SSP1_PL_36_VAL		(0x4 << 18)
+
+	#define PMX_PL_37_38_MASK		(0x3F << 21)
+	#define PMX_PWM0_1_PL_37_38_VAL		0
+	#define PMX_UART5_PL_37_38_VAL		((0x2 << 21) | (0x2 << 24))
+	#define PMX_SSP1_PL_37_38_VAL		((0x4 << 21) | (0x4 << 24))
+
+	#define PMX_PL_39_MASK			(0x7 << 27)
+	#define PMX_I2S_PL_39_VAL		0
+	#define PMX_UART4_PL_39_VAL		(0x2 << 27)
+	#define PMX_SSP1_PL_39_VAL		(0x4 << 27)
+
+#define IP_SEL_PAD_40_49_REG			0x00B4
+	#define PMX_PL_40_MASK			(0x7 << 0)
+	#define PMX_I2S_PL_40_VAL		0
+	#define PMX_UART4_PL_40_VAL		(0x2 << 0)
+	#define PMX_PWM3_PL_40_VAL		(0x4 << 0)
+
+	#define PMX_PL_41_42_MASK		(0x3F << 3)
+	#define PMX_PL_41_MASK			(0x7 << 3)
+	#define PMX_PL_42_MASK			(0x7 << 6)
+	#define PMX_I2S_PL_41_42_VAL		0
+	#define PMX_UART3_PL_41_42_VAL		((0x2 << 3) | (0x2 << 6))
+	#define PMX_PWM2_PL_41_VAL		(0x4 << 3)
+	#define PMX_PWM1_PL_42_VAL		(0x4 << 6)
+
+	#define PMX_PL_43_MASK			(0x7 << 9)
+	#define PMX_SDHCI_PL_43_VAL		0
+	#define PMX_UART1_ENH_PL_43_VAL		(0x2 << 9)
+	#define PMX_PWM0_PL_43_VAL		(0x4 << 9)
+
+	#define PMX_PL_44_45_MASK		(0x3F << 12)
+	#define PMX_SDHCI_PL_44_45_VAL	0
+	#define PMX_UART1_ENH_PL_44_45_VAL	((0x2 << 12) | (0x2 << 15))
+	#define PMX_SSP2_PL_44_45_VAL		((0x4 << 12) | (0x4 << 15))
+
+	#define PMX_PL_46_47_MASK		(0x3F << 18)
+	#define PMX_SDHCI_PL_46_47_VAL	0
+	#define PMX_FSMC_EMI_PL_46_47_VAL	((0x2 << 18) | (0x2 << 21))
+	#define PMX_SSP2_PL_46_47_VAL		((0x4 << 18) | (0x4 << 21))
+
+	#define PMX_PL_48_49_MASK		(0x3F << 24)
+	#define PMX_SDHCI_PL_48_49_VAL	0
+	#define PMX_FSMC_EMI_PL_48_49_VAL	((0x2 << 24) | (0x2 << 27))
+	#define PMX_SSP1_PL_48_49_VAL		((0x4 << 24) | (0x4 << 27))
+
+#define IP_SEL_PAD_50_59_REG			0x00B8
+	#define PMX_PL_50_51_MASK		(0x3F << 0)
+	#define PMX_EMI_PL_50_51_VAL		((0x2 << 0) | (0x2 << 3))
+	#define PMX_SSP1_PL_50_51_VAL		((0x4 << 0) | (0x4 << 3))
+	#define PMX_PL_50_MASK			(0x7 << 0)
+	#define PMX_PL_51_MASK			(0x7 << 3)
+	#define PMX_SDHCI_PL_50_VAL		0
+	#define PMX_SDHCI_CD_PL_51_VAL		0
+
+	#define PMX_PL_52_53_MASK		(0x3F << 6)
+	#define PMX_FSMC_PL_52_53_VAL		0
+	#define PMX_EMI_PL_52_53_VAL		((0x2 << 6) | (0x2 << 9))
+	#define PMX_UART3_PL_52_53_VAL		((0x4 << 6) | (0x4 << 9))
+
+	#define PMX_PL_54_55_56_MASK		(0x1FF << 12)
+	#define PMX_FSMC_EMI_PL_54_55_56_VAL	((0x2 << 12) | (0x2 << 15) | (0x2 << 18))
+
+	#define PMX_PL_57_MASK			(0x7 << 21)
+	#define PMX_FSMC_PL_57_VAL		0
+	#define PMX_PWM3_PL_57_VAL		(0x4 << 21)
+
+	#define PMX_PL_58_59_MASK		(0x3F << 24)
+	#define PMX_PL_58_MASK			(0x7 << 24)
+	#define PMX_PL_59_MASK			(0x7 << 27)
+	#define PMX_FSMC_EMI_PL_58_59_VAL	((0x2 << 24) | (0x2 << 27))
+	#define PMX_PWM2_PL_58_VAL		(0x4 << 24)
+	#define PMX_PWM1_PL_59_VAL		(0x4 << 27)
+
+#define IP_SEL_PAD_60_69_REG			0x00BC
+	#define PMX_PL_60_MASK			(0x7 << 0)
+	#define PMX_FSMC_PL_60_VAL		0
+	#define PMX_PWM0_PL_60_VAL		(0x4 << 0)
+
+	#define PMX_PL_61_TO_64_MASK		(0xFFF << 3)
+	#define PMX_FSMC_PL_61_TO_64_VAL	((0x2 << 3) | (0x2 << 6) | (0x2 << 9) | (0x2 << 12))
+	#define PMX_SSP2_PL_61_TO_64_VAL	((0x4 << 3) | (0x4 << 6) | (0x4 << 9) | (0x4 << 12))
+
+	#define PMX_PL_65_TO_68_MASK		(0xFFF << 15)
+	#define PMX_FSMC_PL_65_TO_68_VAL	((0x2 << 15) | (0x2 << 18) | (0x2 << 21) | (0x2 << 24))
+	#define PMX_SSP1_PL_65_TO_68_VAL	((0x4 << 15) | (0x4 << 18) | (0x4 << 21) | (0x4 << 24))
+
+	#define PMX_PL_69_MASK			(0x7 << 27)
+	#define PMX_CLCD_PL_69_VAL		(0)
+	#define PMX_EMI_PL_69_VAL		(0x2 << 27)
+	#define PMX_SPP_PL_69_VAL		(0x3 << 27)
+	#define PMX_UART5_PL_69_VAL		(0x4 << 27)
+
+#define IP_SEL_PAD_70_79_REG			0x00C0
+	#define PMX_PL_70_MASK			(0x7 << 0)
+	#define PMX_CLCD_PL_70_VAL		(0)
+	#define PMX_FSMC_EMI_PL_70_VAL		(0x2 << 0)
+	#define PMX_SPP_PL_70_VAL		(0x3 << 0)
+	#define PMX_UART5_PL_70_VAL		(0x4 << 0)
+
+	#define PMX_PL_71_72_MASK		(0x3F << 3)
+	#define PMX_CLCD_PL_71_72_VAL		(0)
+	#define PMX_FSMC_EMI_PL_71_72_VAL	((0x2 << 3) | (0x2 << 6))
+	#define PMX_SPP_PL_71_72_VAL		((0x3 << 3) | (0x3 << 6))
+	#define PMX_UART4_PL_71_72_VAL		((0x4 << 3) | (0x4 << 6))
+
+	#define PMX_PL_73_MASK			(0x7 << 9)
+	#define PMX_CLCD_PL_73_VAL		(0)
+	#define PMX_FSMC_EMI_PL_73_VAL		(0x2 << 9)
+	#define PMX_SPP_PL_73_VAL		(0x3 << 9)
+	#define PMX_UART3_PL_73_VAL		(0x4 << 9)
+
+	#define PMX_PL_74_MASK			(0x7 << 12)
+	#define PMX_CLCD_PL_74_VAL		(0)
+	#define PMX_EMI_PL_74_VAL		(0x2 << 12)
+	#define PMX_SPP_PL_74_VAL		(0x3 << 12)
+	#define PMX_UART3_PL_74_VAL		(0x4 << 12)
+
+	#define PMX_PL_75_76_MASK		(0x3F << 15)
+	#define PMX_CLCD_PL_75_76_VAL		(0)
+	#define PMX_EMI_PL_75_76_VAL		((0x2 << 15) | (0x2 << 18))
+	#define PMX_SPP_PL_75_76_VAL		((0x3 << 15) | (0x3 << 18))
+	#define PMX_I2C2_PL_75_76_VAL		((0x4 << 15) | (0x4 << 18))
+
+	#define PMX_PL_77_78_79_MASK		(0x1FF << 21)
+	#define PMX_CLCD_PL_77_78_79_VAL	(0)
+	#define PMX_EMI_PL_77_78_79_VAL		((0x2 << 21) | (0x2 << 24) | (0x2 << 27))
+	#define PMX_SPP_PL_77_78_79_VAL		((0x3 << 21) | (0x3 << 24) | (0x3 << 27))
+	#define PMX_RS485_PL_77_78_79_VAL	((0x4 << 21) | (0x4 << 24) | (0x4 << 27))
+
+#define IP_SEL_PAD_80_89_REG			0x00C4
+	#define PMX_PL_80_TO_85_MASK		(0x3FFFF << 0)
+	#define PMX_CLCD_PL_80_TO_85_VAL	0
+	#define PMX_MII2_PL_80_TO_85_VAL	((0x1 << 0) | (0x1 << 3) | (0x1 << 6) | (0x1 << 9) | (0x1 << 12) | (0x1 << 15))
+	#define PMX_EMI_PL_80_TO_85_VAL		((0x2 << 0) | (0x2 << 3) | (0x2 << 6) | (0x2 << 9) | (0x2 << 12) | (0x2 << 15))
+	#define PMX_SPP_PL_80_TO_85_VAL		((0x3 << 0) | (0x3 << 3) | (0x3 << 6) | (0x3 << 9) | (0x3 << 12) | (0x3 << 15))
+	#define PMX_UART1_ENH_PL_80_TO_85_VAL	((0x4 << 0) | (0x4 << 3) | (0x4 << 6) | (0x4 << 9) | (0x4 << 12) | (0x4 << 15))
+
+	#define PMX_PL_86_87_MASK		(0x3F << 18)
+	#define PMX_PL_86_MASK			(0x7 << 18)
+	#define PMX_PL_87_MASK			(0x7 << 21)
+	#define PMX_CLCD_PL_86_87_VAL		0
+	#define PMX_MII2_PL_86_87_VAL		((0x1 << 18) | (0x1 << 21))
+	#define PMX_EMI_PL_86_87_VAL		((0x2 << 18) | (0x2 << 21))
+	#define PMX_PWM3_PL_86_VAL		(0x4 << 18)
+	#define PMX_PWM2_PL_87_VAL		(0x4 << 21)
+
+	#define PMX_PL_88_89_MASK		(0x3F << 24)
+	#define PMX_CLCD_PL_88_89_VAL		0
+	#define PMX_MII2_PL_88_89_VAL		((0x1 << 24) | (0x1 << 27))
+	#define PMX_EMI_PL_88_89_VAL		((0x2 << 24) | (0x2 << 27))
+	#define PMX_UART6_PL_88_89_VAL		((0x3 << 24) | (0x3 << 27))
+	#define PMX_PWM0_1_PL_88_89_VAL		((0x4 << 24) | (0x4 << 27))
+
+#define IP_SEL_PAD_90_99_REG			0x00C8
+	#define PMX_PL_90_91_MASK		(0x3F << 0)
+	#define PMX_CLCD_PL_90_91_VAL		0
+	#define PMX_MII2_PL_90_91_VAL		((0x1 << 0) | (0x1 << 3))
+	#define PMX_EMI1_PL_90_91_VAL		((0x2 << 0) | (0x2 << 3))
+	#define PMX_UART5_PL_90_91_VAL		((0x3 << 0) | (0x3 << 3))
+	#define PMX_SSP2_PL_90_91_VAL		((0x4 << 0) | (0x4 << 3))
+
+	#define PMX_PL_92_93_MASK		(0x3F << 6)
+	#define PMX_CLCD_PL_92_93_VAL		0
+	#define PMX_MII2_PL_92_93_VAL		((0x1 << 6) | (0x1 << 9))
+	#define PMX_EMI1_PL_92_93_VAL		((0x2 << 6) | (0x2 << 9))
+	#define PMX_UART4_PL_92_93_VAL		((0x3 << 6) | (0x3 << 9))
+	#define PMX_SSP2_PL_92_93_VAL		((0x4 << 6) | (0x4 << 9))
+
+	#define PMX_PL_94_95_MASK		(0x3F << 12)
+	#define PMX_CLCD_PL_94_95_VAL		0
+	#define PMX_MII2_PL_94_95_VAL		((0x1 << 12) | (0x1 << 15))
+	#define PMX_EMI1_PL_94_95_VAL		((0x2 << 12) | (0x2 << 15))
+	#define PMX_UART3_PL_94_95_VAL		((0x3 << 12) | (0x3 << 15))
+	#define PMX_SSP1_PL_94_95_VAL		((0x4 << 12) | (0x4 << 15))
+
+	#define PMX_PL_96_97_MASK		(0x3F << 18)
+	#define PMX_CLCD_PL_96_97_VAL		0
+	#define PMX_MII2_PL_96_97_VAL		((0x1 << 18) | (0x1 << 21))
+	#define PMX_EMI1_PL_96_97_VAL		((0x2 << 18) | (0x2 << 21))
+	#define PMX_I2C2_PL_96_97_VAL		((0x3 << 18) | (0x3 << 21))
+	#define PMX_SSP1_PL_96_97_VAL		((0x4 << 18) | (0x4 << 21))
+
+	#define PMX_PL_98_MASK			(0x7 << 24)
+	#define PMX_CLCD_PL_98_VAL		0
+	#define PMX_I2C1_PL_98_VAL		(0x2 << 24)
+	#define PMX_UART3_PL_98_VAL		(0x4 << 24)
+
+	#define PMX_PL_99_MASK			(0x7 << 27)
+	#define PMX_SDHCI_PL_99_VAL		0
+	#define PMX_I2C1_PL_99_VAL		(0x2 << 27)
+	#define PMX_UART3_PL_99_VAL		(0x4 << 27)
+
+#define IP_SEL_MIX_PAD_REG			0x00CC
+	#define PMX_PL_100_101_MASK		(0x3F << 0)
+	#define PMX_SDHCI_PL_100_101_VAL	0
+	#define PMX_UART4_PL_100_101_VAL	((0x4 << 0) | (0x4 << 3))
+
+	#define PMX_SSP1_PORT_SEL_MASK		(0x7 << 8)
+	#define PMX_SSP1_PORT_94_TO_97_VAL	0
+	#define PMX_SSP1_PORT_65_TO_68_VAL	(0x1 << 8)
+	#define PMX_SSP1_PORT_48_TO_51_VAL	(0x2 << 8)
+	#define PMX_SSP1_PORT_36_TO_39_VAL	(0x3 << 8)
+	#define PMX_SSP1_PORT_17_TO_20_VAL	(0x4 << 8)
+
+	#define PMX_SSP2_PORT_SEL_MASK		(0x7 << 11)
+	#define PMX_SSP2_PORT_90_TO_93_VAL	0
+	#define PMX_SSP2_PORT_61_TO_64_VAL	(0x1 << 11)
+	#define PMX_SSP2_PORT_44_TO_47_VAL	(0x2 << 11)
+	#define PMX_SSP2_PORT_32_TO_35_VAL	(0x3 << 11)
+	#define PMX_SSP2_PORT_13_TO_16_VAL	(0x4 << 11)
+
+	#define PMX_UART1_ENH_PORT_SEL_MASK		(0x3 << 14)
+	#define PMX_UART1_ENH_PORT_81_TO_85_VAL		0
+	#define PMX_UART1_ENH_PORT_44_45_34_36_VAL	(0x1 << 14)
+	#define PMX_UART1_ENH_PORT_32_TO_34_36_VAL	(0x2 << 14)
+	#define PMX_UART1_ENH_PORT_3_TO_5_7_VAL		(0x3 << 14)
+
+	#define PMX_UART3_PORT_SEL_MASK		(0x7 << 16)
+	#define PMX_UART3_PORT_94_VAL		0
+	#define PMX_UART3_PORT_73_VAL		(0x1 << 16)
+	#define PMX_UART3_PORT_52_VAL		(0x2 << 16)
+	#define PMX_UART3_PORT_41_VAL		(0x3 << 16)
+	#define PMX_UART3_PORT_15_VAL		(0x4 << 16)
+	#define PMX_UART3_PORT_8_VAL		(0x5 << 16)
+	#define PMX_UART3_PORT_99_VAL		(0x6 << 16)
+
+	#define PMX_UART4_PORT_SEL_MASK		(0x7 << 19)
+	#define PMX_UART4_PORT_92_VAL		0
+	#define PMX_UART4_PORT_71_VAL		(0x1 << 19)
+	#define PMX_UART4_PORT_39_VAL		(0x2 << 19)
+	#define PMX_UART4_PORT_13_VAL		(0x3 << 19)
+	#define PMX_UART4_PORT_6_VAL		(0x4 << 19)
+	#define PMX_UART4_PORT_101_VAL		(0x5 << 19)
+
+	#define PMX_UART5_PORT_SEL_MASK		(0x3 << 22)
+	#define PMX_UART5_PORT_90_VAL		0
+	#define PMX_UART5_PORT_69_VAL		(0x1 << 22)
+	#define PMX_UART5_PORT_37_VAL		(0x2 << 22)
+	#define PMX_UART5_PORT_4_VAL		(0x3 << 22)
+
+	#define PMX_UART6_PORT_SEL_MASK		(0x1 << 24)
+	#define PMX_UART6_PORT_88_VAL		0
+	#define PMX_UART6_PORT_2_VAL		(0x1 << 24)
+
+	#define PMX_I2C1_PORT_SEL_MASK		(0x1 << 25)
+	#define PMX_I2C1_PORT_8_9_VAL		0
+	#define PMX_I2C1_PORT_98_99_VAL		(0x1 << 25)
+
+	#define PMX_I2C2_PORT_SEL_MASK		(0x3 << 26)
+	#define PMX_I2C2_PORT_96_97_VAL		0
+	#define PMX_I2C2_PORT_75_76_VAL		(0x1 << 26)
+	#define PMX_I2C2_PORT_19_20_VAL		(0x2 << 26)
+	#define PMX_I2C2_PORT_2_3_VAL		(0x3 << 26)
+	#define PMX_I2C2_PORT_0_1_VAL		(0x4 << 26)
+
+	#define PMX_SDHCI_CD_PORT_SEL_MASK	(0x1 << 29)
+	#define PMX_SDHCI_CD_PORT_12_VAL	0
+	#define PMX_SDHCI_CD_PORT_51_VAL	(0x1 << 29)
+
+/* Pad multiplexing for CLCD device */
+static const unsigned clcd_pins[] = { 69, 70, 71, 72, 73, 74, 75, 76, 77, 78,
+	79, 80, 81, 82, 83, 84, 85, 86, 87, 88, 89, 90, 91, 92, 93, 94, 95, 96,
+	97 };
+static struct spear_muxreg clcd_muxreg[] = {
+	{
+		.reg = IP_SEL_PAD_60_69_REG,
+		.mask = PMX_PL_69_MASK,
+		.val = PMX_CLCD_PL_69_VAL,
+	}, {
+		.reg = IP_SEL_PAD_70_79_REG,
+		.mask = PMX_PL_70_MASK | PMX_PL_71_72_MASK | PMX_PL_73_MASK |
+			PMX_PL_74_MASK | PMX_PL_75_76_MASK |
+			PMX_PL_77_78_79_MASK,
+		.val = PMX_CLCD_PL_70_VAL | PMX_CLCD_PL_71_72_VAL |
+			PMX_CLCD_PL_73_VAL | PMX_CLCD_PL_74_VAL |
+			PMX_CLCD_PL_75_76_VAL | PMX_CLCD_PL_77_78_79_VAL,
+	}, {
+		.reg = IP_SEL_PAD_80_89_REG,
+		.mask = PMX_PL_80_TO_85_MASK | PMX_PL_86_87_MASK |
+			PMX_PL_88_89_MASK,
+		.val = PMX_CLCD_PL_80_TO_85_VAL | PMX_CLCD_PL_86_87_VAL |
+			PMX_CLCD_PL_88_89_VAL,
+	}, {
+		.reg = IP_SEL_PAD_90_99_REG,
+		.mask = PMX_PL_90_91_MASK | PMX_PL_92_93_MASK |
+			PMX_PL_94_95_MASK | PMX_PL_96_97_MASK | PMX_PL_98_MASK,
+		.val = PMX_CLCD_PL_90_91_VAL | PMX_CLCD_PL_92_93_VAL |
+			PMX_CLCD_PL_94_95_VAL | PMX_CLCD_PL_96_97_VAL |
+			PMX_CLCD_PL_98_VAL,
+	},
+};
+
+static struct spear_modemux clcd_modemux[] = {
+	{
+		.modes = EXTENDED_MODE,
+		.muxregs = clcd_muxreg,
+		.nmuxregs = ARRAY_SIZE(clcd_muxreg),
+	},
+};
+
+static struct spear_pingroup clcd_pingroup = {
+	.name = "clcd_grp",
+	.pins = clcd_pins,
+	.npins = ARRAY_SIZE(clcd_pins),
+	.modemuxs = clcd_modemux,
+	.nmodemuxs = ARRAY_SIZE(clcd_modemux),
+};
+
+static const char *const clcd_grps[] = { "clcd_grp" };
+static struct spear_function clcd_function = {
+	.name = "clcd",
+	.groups = clcd_grps,
+	.ngroups = ARRAY_SIZE(clcd_grps),
+};
+
+/* Pad multiplexing for EMI (Parallel NOR flash) device */
+static const unsigned emi_pins[] = { 46, 47, 48, 49, 50, 51, 52, 53, 54, 55, 56,
+	57, 58, 59, 60, 61, 62, 63, 64, 65, 66, 67, 68, 69, 70, 71, 72, 73, 74,
+	75, 76, 77, 78, 79, 80, 81, 82, 83, 84, 85, 86, 87, 88, 89, 90, 91, 92,
+	93, 94, 95, 96, 97 };
+static struct spear_muxreg emi_muxreg[] = {
+	{
+		.reg = PMX_CONFIG_REG,
+		.mask = PMX_TIMER_0_1_MASK | PMX_TIMER_2_3_MASK,
+		.val = 0,
+	},
+};
+
+static struct spear_muxreg emi_ext_muxreg[] = {
+	{
+		.reg = IP_SEL_PAD_40_49_REG,
+		.mask = PMX_PL_46_47_MASK | PMX_PL_48_49_MASK,
+		.val = PMX_FSMC_EMI_PL_46_47_VAL | PMX_FSMC_EMI_PL_48_49_VAL,
+	}, {
+		.reg = IP_SEL_PAD_50_59_REG,
+		.mask = PMX_PL_50_51_MASK | PMX_PL_52_53_MASK |
+			PMX_PL_54_55_56_MASK | PMX_PL_58_59_MASK,
+		.val = PMX_EMI_PL_50_51_VAL | PMX_EMI_PL_52_53_VAL |
+			PMX_FSMC_EMI_PL_54_55_56_VAL |
+			PMX_FSMC_EMI_PL_58_59_VAL,
+	}, {
+		.reg = IP_SEL_PAD_60_69_REG,
+		.mask = PMX_PL_69_MASK,
+		.val = PMX_EMI_PL_69_VAL,
+	}, {
+		.reg = IP_SEL_PAD_70_79_REG,
+		.mask = PMX_PL_70_MASK | PMX_PL_71_72_MASK | PMX_PL_73_MASK |
+			PMX_PL_74_MASK | PMX_PL_75_76_MASK |
+			PMX_PL_77_78_79_MASK,
+		.val = PMX_FSMC_EMI_PL_70_VAL | PMX_FSMC_EMI_PL_71_72_VAL |
+			PMX_FSMC_EMI_PL_73_VAL | PMX_EMI_PL_74_VAL |
+			PMX_EMI_PL_75_76_VAL | PMX_EMI_PL_77_78_79_VAL,
+	}, {
+		.reg = IP_SEL_PAD_80_89_REG,
+		.mask = PMX_PL_80_TO_85_MASK | PMX_PL_86_87_MASK |
+			PMX_PL_88_89_MASK,
+		.val = PMX_EMI_PL_80_TO_85_VAL | PMX_EMI_PL_86_87_VAL |
+			PMX_EMI_PL_88_89_VAL,
+	}, {
+		.reg = IP_SEL_PAD_90_99_REG,
+		.mask = PMX_PL_90_91_MASK | PMX_PL_92_93_MASK |
+			PMX_PL_94_95_MASK | PMX_PL_96_97_MASK,
+		.val = PMX_EMI1_PL_90_91_VAL | PMX_EMI1_PL_92_93_VAL |
+			PMX_EMI1_PL_94_95_VAL | PMX_EMI1_PL_96_97_VAL,
+	}, {
+		.reg = EXT_CTRL_REG,
+		.mask = EMI_FSMC_DYNAMIC_MUX_MASK,
+		.val = EMI_FSMC_DYNAMIC_MUX_MASK,
+	},
+};
+
+static struct spear_modemux emi_modemux[] = {
+	{
+		.modes = AUTO_EXP_MODE | EXTENDED_MODE,
+		.muxregs = emi_muxreg,
+		.nmuxregs = ARRAY_SIZE(emi_muxreg),
+	}, {
+		.modes = EXTENDED_MODE,
+		.muxregs = emi_ext_muxreg,
+		.nmuxregs = ARRAY_SIZE(emi_ext_muxreg),
+	},
+};
+
+static struct spear_pingroup emi_pingroup = {
+	.name = "emi_grp",
+	.pins = emi_pins,
+	.npins = ARRAY_SIZE(emi_pins),
+	.modemuxs = emi_modemux,
+	.nmodemuxs = ARRAY_SIZE(emi_modemux),
+};
+
+static const char *const emi_grps[] = { "emi_grp" };
+static struct spear_function emi_function = {
+	.name = "emi",
+	.groups = emi_grps,
+	.ngroups = ARRAY_SIZE(emi_grps),
+};
+
+/* Pad multiplexing for FSMC (NAND flash) device */
+static const unsigned fsmc_8bit_pins[] = { 52, 53, 54, 55, 56, 57, 58, 59, 60,
+	61, 62, 63, 64, 65, 66, 67, 68 };
+static struct spear_muxreg fsmc_8bit_muxreg[] = {
+	{
+		.reg = IP_SEL_PAD_50_59_REG,
+		.mask = PMX_PL_52_53_MASK | PMX_PL_54_55_56_MASK |
+			PMX_PL_57_MASK | PMX_PL_58_59_MASK,
+		.val = PMX_FSMC_PL_52_53_VAL | PMX_FSMC_EMI_PL_54_55_56_VAL |
+			PMX_FSMC_PL_57_VAL | PMX_FSMC_EMI_PL_58_59_VAL,
+	}, {
+		.reg = IP_SEL_PAD_60_69_REG,
+		.mask = PMX_PL_60_MASK | PMX_PL_61_TO_64_MASK |
+			PMX_PL_65_TO_68_MASK,
+		.val = PMX_FSMC_PL_60_VAL | PMX_FSMC_PL_61_TO_64_VAL |
+			PMX_FSMC_PL_65_TO_68_VAL,
+	}, {
+		.reg = EXT_CTRL_REG,
+		.mask = EMI_FSMC_DYNAMIC_MUX_MASK,
+		.val = EMI_FSMC_DYNAMIC_MUX_MASK,
+	},
+};
+
+static struct spear_modemux fsmc_8bit_modemux[] = {
+	{
+		.modes = EXTENDED_MODE,
+		.muxregs = fsmc_8bit_muxreg,
+		.nmuxregs = ARRAY_SIZE(fsmc_8bit_muxreg),
+	},
+};
+
+static struct spear_pingroup fsmc_8bit_pingroup = {
+	.name = "fsmc_8bit_grp",
+	.pins = fsmc_8bit_pins,
+	.npins = ARRAY_SIZE(fsmc_8bit_pins),
+	.modemuxs = fsmc_8bit_modemux,
+	.nmodemuxs = ARRAY_SIZE(fsmc_8bit_modemux),
+};
+
+static const unsigned fsmc_16bit_pins[] = { 46, 47, 48, 49, 52, 53, 54, 55, 56,
+	57, 58, 59, 60, 61, 62, 63, 64, 65, 66, 67, 68, 70, 71, 72, 73 };
+static struct spear_muxreg fsmc_16bit_autoexp_muxreg[] = {
+	{
+		.reg = PMX_CONFIG_REG,
+		.mask = PMX_TIMER_0_1_MASK | PMX_TIMER_2_3_MASK,
+		.val = 0,
+	},
+};
+
+static struct spear_muxreg fsmc_16bit_muxreg[] = {
+	{
+		.reg = IP_SEL_PAD_40_49_REG,
+		.mask = PMX_PL_46_47_MASK | PMX_PL_48_49_MASK,
+		.val = PMX_FSMC_EMI_PL_46_47_VAL | PMX_FSMC_EMI_PL_48_49_VAL,
+	}, {
+		.reg = IP_SEL_PAD_70_79_REG,
+		.mask = PMX_PL_70_MASK | PMX_PL_71_72_MASK | PMX_PL_73_MASK,
+		.val = PMX_FSMC_EMI_PL_70_VAL | PMX_FSMC_EMI_PL_71_72_VAL |
+			PMX_FSMC_EMI_PL_73_VAL,
+	}
+};
+
+static struct spear_modemux fsmc_16bit_modemux[] = {
+	{
+		.modes = EXTENDED_MODE,
+		.muxregs = fsmc_8bit_muxreg,
+		.nmuxregs = ARRAY_SIZE(fsmc_8bit_muxreg),
+	}, {
+		.modes = AUTO_EXP_MODE | EXTENDED_MODE,
+		.muxregs = fsmc_16bit_autoexp_muxreg,
+		.nmuxregs = ARRAY_SIZE(fsmc_16bit_autoexp_muxreg),
+	}, {
+		.modes = EXTENDED_MODE,
+		.muxregs = fsmc_16bit_muxreg,
+		.nmuxregs = ARRAY_SIZE(fsmc_16bit_muxreg),
+	},
+};
+
+static struct spear_pingroup fsmc_16bit_pingroup = {
+	.name = "fsmc_16bit_grp",
+	.pins = fsmc_16bit_pins,
+	.npins = ARRAY_SIZE(fsmc_16bit_pins),
+	.modemuxs = fsmc_16bit_modemux,
+	.nmodemuxs = ARRAY_SIZE(fsmc_16bit_modemux),
+};
+
+static const char *const fsmc_grps[] = { "fsmc_8bit_grp", "fsmc_16bit_grp" };
+static struct spear_function fsmc_function = {
+	.name = "fsmc",
+	.groups = fsmc_grps,
+	.ngroups = ARRAY_SIZE(fsmc_grps),
+};
+
+/* Pad multiplexing for SPP device */
+static const unsigned spp_pins[] = { 69, 70, 71, 72, 73, 74, 75, 76, 77, 78, 79,
+	80, 81, 82, 83, 84, 85 };
+static struct spear_muxreg spp_muxreg[] = {
+	{
+		.reg = IP_SEL_PAD_60_69_REG,
+		.mask = PMX_PL_69_MASK,
+		.val = PMX_SPP_PL_69_VAL,
+	}, {
+		.reg = IP_SEL_PAD_70_79_REG,
+		.mask = PMX_PL_70_MASK | PMX_PL_71_72_MASK | PMX_PL_73_MASK |
+			PMX_PL_74_MASK | PMX_PL_75_76_MASK |
+			PMX_PL_77_78_79_MASK,
+		.val = PMX_SPP_PL_70_VAL | PMX_SPP_PL_71_72_VAL |
+			PMX_SPP_PL_73_VAL | PMX_SPP_PL_74_VAL |
+			PMX_SPP_PL_75_76_VAL | PMX_SPP_PL_77_78_79_VAL,
+	}, {
+		.reg = IP_SEL_PAD_80_89_REG,
+		.mask = PMX_PL_80_TO_85_MASK,
+		.val = PMX_SPP_PL_80_TO_85_VAL,
+	},
+};
+
+static struct spear_modemux spp_modemux[] = {
+	{
+		.modes = EXTENDED_MODE,
+		.muxregs = spp_muxreg,
+		.nmuxregs = ARRAY_SIZE(spp_muxreg),
+	},
+};
+
+static struct spear_pingroup spp_pingroup = {
+	.name = "spp_grp",
+	.pins = spp_pins,
+	.npins = ARRAY_SIZE(spp_pins),
+	.modemuxs = spp_modemux,
+	.nmodemuxs = ARRAY_SIZE(spp_modemux),
+};
+
+static const char *const spp_grps[] = { "spp_grp" };
+static struct spear_function spp_function = {
+	.name = "spp",
+	.groups = spp_grps,
+	.ngroups = ARRAY_SIZE(spp_grps),
+};
+
+/* Pad multiplexing for SDHCI device */
+static const unsigned sdhci_led_pins[] = { 34 };
+static struct spear_muxreg sdhci_led_muxreg[] = {
+	{
+		.reg = PMX_CONFIG_REG,
+		.mask = PMX_SSP_CS_MASK,
+		.val = 0,
+	},
+};
+
+static struct spear_muxreg sdhci_led_ext_muxreg[] = {
+	{
+		.reg = IP_SEL_PAD_30_39_REG,
+		.mask = PMX_PL_34_MASK,
+		.val = PMX_PWM2_PL_34_VAL,
+	},
+};
+
+static struct spear_modemux sdhci_led_modemux[] = {
+	{
+		.modes = AUTO_NET_SMII_MODE | AUTO_NET_MII_MODE | EXTENDED_MODE,
+		.muxregs = sdhci_led_muxreg,
+		.nmuxregs = ARRAY_SIZE(sdhci_led_muxreg),
+	}, {
+		.modes = EXTENDED_MODE,
+		.muxregs = sdhci_led_ext_muxreg,
+		.nmuxregs = ARRAY_SIZE(sdhci_led_ext_muxreg),
+	},
+};
+
+static struct spear_pingroup sdhci_led_pingroup = {
+	.name = "sdhci_led_grp",
+	.pins = sdhci_led_pins,
+	.npins = ARRAY_SIZE(sdhci_led_pins),
+	.modemuxs = sdhci_led_modemux,
+	.nmodemuxs = ARRAY_SIZE(sdhci_led_modemux),
+};
+
+static const unsigned sdhci_cd_12_pins[] = { 12, 43, 44, 45, 46, 47, 48, 49,
+	50};
+static const unsigned sdhci_cd_51_pins[] = { 43, 44, 45, 46, 47, 48, 49, 50, 51
+};
+static struct spear_muxreg sdhci_muxreg[] = {
+	{
+		.reg = PMX_CONFIG_REG,
+		.mask = PMX_TIMER_0_1_MASK | PMX_TIMER_2_3_MASK,
+		.val = 0,
+	},
+};
+
+static struct spear_muxreg sdhci_ext_muxreg[] = {
+	{
+		.reg = IP_SEL_PAD_40_49_REG,
+		.mask = PMX_PL_43_MASK | PMX_PL_44_45_MASK | PMX_PL_46_47_MASK |
+			PMX_PL_48_49_MASK,
+		.val = PMX_SDHCI_PL_43_VAL | PMX_SDHCI_PL_44_45_VAL |
+			PMX_SDHCI_PL_46_47_VAL | PMX_SDHCI_PL_48_49_VAL,
+	}, {
+		.reg = IP_SEL_PAD_50_59_REG,
+		.mask = PMX_PL_50_MASK,
+		.val = PMX_SDHCI_PL_50_VAL,
+	}, {
+		.reg = IP_SEL_PAD_90_99_REG,
+		.mask = PMX_PL_99_MASK,
+		.val = PMX_SDHCI_PL_99_VAL,
+	}, {
+		.reg = IP_SEL_MIX_PAD_REG,
+		.mask = PMX_PL_100_101_MASK,
+		.val = PMX_SDHCI_PL_100_101_VAL,
+	},
+};
+
+static struct spear_muxreg sdhci_cd_12_muxreg[] = {
+	{
+		.reg = PMX_CONFIG_REG,
+		.mask = PMX_MII_MASK,
+		.val = 0,
+	}, {
+		.reg = IP_SEL_PAD_10_19_REG,
+		.mask = PMX_PL_12_MASK,
+		.val = PMX_SDHCI_CD_PL_12_VAL,
+	}, {
+		.reg = IP_SEL_MIX_PAD_REG,
+		.mask = PMX_SDHCI_CD_PORT_SEL_MASK,
+		.val = PMX_SDHCI_CD_PORT_12_VAL,
+	},
+};
+
+static struct spear_muxreg sdhci_cd_51_muxreg[] = {
+	{
+		.reg = IP_SEL_PAD_50_59_REG,
+		.mask = PMX_PL_51_MASK,
+		.val = PMX_SDHCI_CD_PL_51_VAL,
+	}, {
+		.reg = IP_SEL_MIX_PAD_REG,
+		.mask = PMX_SDHCI_CD_PORT_SEL_MASK,
+		.val = PMX_SDHCI_CD_PORT_51_VAL,
+	},
+};
+
+#define pmx_sdhci_common_modemux					\
+	{								\
+		.modes = AUTO_NET_SMII_MODE | AUTO_NET_MII_MODE |	\
+			SMALL_PRINTERS_MODE | EXTENDED_MODE,		\
+		.muxregs = sdhci_muxreg,				\
+		.nmuxregs = ARRAY_SIZE(sdhci_muxreg),			\
+	}, {								\
+		.modes = EXTENDED_MODE,					\
+		.muxregs = sdhci_ext_muxreg,				\
+		.nmuxregs = ARRAY_SIZE(sdhci_ext_muxreg),		\
+	}
+
+static struct spear_modemux sdhci_modemux[][3] = {
+	{
+		/* select pin 12 for cd */
+		pmx_sdhci_common_modemux,
+		{
+			.modes = EXTENDED_MODE,
+			.muxregs = sdhci_cd_12_muxreg,
+			.nmuxregs = ARRAY_SIZE(sdhci_cd_12_muxreg),
+		},
+	}, {
+		/* select pin 51 for cd */
+		pmx_sdhci_common_modemux,
+		{
+			.modes = EXTENDED_MODE,
+			.muxregs = sdhci_cd_51_muxreg,
+			.nmuxregs = ARRAY_SIZE(sdhci_cd_51_muxreg),
+		},
+	}
+};
+
+static struct spear_pingroup sdhci_pingroup[] = {
+	{
+		.name = "sdhci_cd_12_grp",
+		.pins = sdhci_cd_12_pins,
+		.npins = ARRAY_SIZE(sdhci_cd_12_pins),
+		.modemuxs = sdhci_modemux[0],
+		.nmodemuxs = ARRAY_SIZE(sdhci_modemux[0]),
+	}, {
+		.name = "sdhci_cd_51_grp",
+		.pins = sdhci_cd_51_pins,
+		.npins = ARRAY_SIZE(sdhci_cd_51_pins),
+		.modemuxs = sdhci_modemux[1],
+		.nmodemuxs = ARRAY_SIZE(sdhci_modemux[1]),
+	},
+};
+
+static const char *const sdhci_grps[] = { "sdhci_cd_12_grp", "sdhci_cd_51_grp",
+	"sdhci_led_grp" };
+
+static struct spear_function sdhci_function = {
+	.name = "sdhci",
+	.groups = sdhci_grps,
+	.ngroups = ARRAY_SIZE(sdhci_grps),
+};
+
+/* Pad multiplexing for I2S device */
+static const unsigned i2s_pins[] = { 35, 39, 40, 41, 42 };
+static struct spear_muxreg i2s_muxreg[] = {
+	{
+		.reg = PMX_CONFIG_REG,
+		.mask = PMX_SSP_CS_MASK,
+		.val = 0,
+	}, {
+		.reg = PMX_CONFIG_REG,
+		.mask = PMX_UART0_MODEM_MASK,
+		.val = 0,
+	},
+};
+
+static struct spear_muxreg i2s_ext_muxreg[] = {
+	{
+		.reg = IP_SEL_PAD_30_39_REG,
+		.mask = PMX_PL_35_MASK | PMX_PL_39_MASK,
+		.val = PMX_I2S_REF_CLK_PL_35_VAL | PMX_I2S_PL_39_VAL,
+	}, {
+		.reg = IP_SEL_PAD_40_49_REG,
+		.mask = PMX_PL_40_MASK | PMX_PL_41_42_MASK,
+		.val = PMX_I2S_PL_40_VAL | PMX_I2S_PL_41_42_VAL,
+	},
+};
+
+static struct spear_modemux i2s_modemux[] = {
+	{
+		.modes = AUTO_NET_SMII_MODE | AUTO_NET_MII_MODE | EXTENDED_MODE,
+		.muxregs = i2s_muxreg,
+		.nmuxregs = ARRAY_SIZE(i2s_muxreg),
+	}, {
+		.modes = EXTENDED_MODE,
+		.muxregs = i2s_ext_muxreg,
+		.nmuxregs = ARRAY_SIZE(i2s_ext_muxreg),
+	},
+};
+
+static struct spear_pingroup i2s_pingroup = {
+	.name = "i2s_grp",
+	.pins = i2s_pins,
+	.npins = ARRAY_SIZE(i2s_pins),
+	.modemuxs = i2s_modemux,
+	.nmodemuxs = ARRAY_SIZE(i2s_modemux),
+};
+
+static const char *const i2s_grps[] = { "i2s_grp" };
+static struct spear_function i2s_function = {
+	.name = "i2s",
+	.groups = i2s_grps,
+	.ngroups = ARRAY_SIZE(i2s_grps),
+};
+
+/* Pad multiplexing for UART1 device */
+static const unsigned uart1_pins[] = { 28, 29 };
+static struct spear_muxreg uart1_muxreg[] = {
+	{
+		.reg = PMX_CONFIG_REG,
+		.mask = PMX_GPIO_PIN0_MASK | PMX_GPIO_PIN1_MASK,
+		.val = 0,
+	},
+};
+
+static struct spear_muxreg uart1_ext_muxreg[] = {
+	{
+		.reg = IP_SEL_PAD_20_29_REG,
+		.mask = PMX_PL_28_29_MASK,
+		.val = PMX_UART1_PL_28_29_VAL,
+	},
+};
+
+static struct spear_modemux uart1_modemux[] = {
+	{
+		.modes = AUTO_NET_SMII_MODE | AUTO_NET_MII_MODE | AUTO_EXP_MODE
+			| SMALL_PRINTERS_MODE | EXTENDED_MODE,
+		.muxregs = uart1_muxreg,
+		.nmuxregs = ARRAY_SIZE(uart1_muxreg),
+	}, {
+		.modes = EXTENDED_MODE,
+		.muxregs = uart1_ext_muxreg,
+		.nmuxregs = ARRAY_SIZE(uart1_ext_muxreg),
+	},
+};
+
+static struct spear_pingroup uart1_pingroup = {
+	.name = "uart1_grp",
+	.pins = uart1_pins,
+	.npins = ARRAY_SIZE(uart1_pins),
+	.modemuxs = uart1_modemux,
+	.nmodemuxs = ARRAY_SIZE(uart1_modemux),
+};
+
+static const char *const uart1_grps[] = { "uart1_grp" };
+static struct spear_function uart1_function = {
+	.name = "uart1",
+	.groups = uart1_grps,
+	.ngroups = ARRAY_SIZE(uart1_grps),
+};
+
+/* Pad multiplexing for UART1 Modem device */
+static const unsigned uart1_modem_2_to_7_pins[] = { 2, 3, 4, 5, 6, 7 };
+static const unsigned uart1_modem_31_to_36_pins[] = { 31, 32, 33, 34, 35, 36 };
+static const unsigned uart1_modem_34_to_45_pins[] = { 34, 35, 36, 43, 44, 45 };
+static const unsigned uart1_modem_80_to_85_pins[] = { 80, 81, 82, 83, 84, 85 };
+
+static struct spear_muxreg uart1_modem_ext_2_to_7_muxreg[] = {
+	{
+		.reg = PMX_CONFIG_REG,
+		.mask = PMX_UART0_MASK | PMX_I2C_MASK | PMX_SSP_MASK,
+		.val = 0,
+	}, {
+		.reg = IP_SEL_PAD_0_9_REG,
+		.mask = PMX_PL_2_3_MASK | PMX_PL_6_7_MASK,
+		.val = PMX_UART1_ENH_PL_2_3_VAL | PMX_UART1_ENH_PL_4_5_VAL |
+			PMX_UART1_ENH_PL_6_7_VAL,
+	}, {
+		.reg = IP_SEL_MIX_PAD_REG,
+		.mask = PMX_UART1_ENH_PORT_SEL_MASK,
+		.val = PMX_UART1_ENH_PORT_3_TO_5_7_VAL,
+	},
+};
+
+static struct spear_muxreg uart1_modem_31_to_36_muxreg[] = {
+	{
+		.reg = PMX_CONFIG_REG,
+		.mask = PMX_GPIO_PIN3_MASK | PMX_GPIO_PIN4_MASK |
+			PMX_GPIO_PIN5_MASK | PMX_SSP_CS_MASK,
+		.val = 0,
+	},
+};
+
+static struct spear_muxreg uart1_modem_ext_31_to_36_muxreg[] = {
+	{
+		.reg = IP_SEL_PAD_30_39_REG,
+		.mask = PMX_PL_31_MASK | PMX_PL_32_33_MASK | PMX_PL_34_MASK |
+			PMX_PL_35_MASK | PMX_PL_36_MASK,
+		.val = PMX_UART1_ENH_PL_31_VAL | PMX_UART1_ENH_PL_32_33_VAL |
+			PMX_UART1_ENH_PL_34_VAL | PMX_UART1_ENH_PL_35_VAL |
+			PMX_UART1_ENH_PL_36_VAL,
+	}, {
+		.reg = IP_SEL_MIX_PAD_REG,
+		.mask = PMX_UART1_ENH_PORT_SEL_MASK,
+		.val = PMX_UART1_ENH_PORT_32_TO_34_36_VAL,
+	},
+};
+
+static struct spear_muxreg uart1_modem_34_to_45_muxreg[] = {
+	{
+		.reg = PMX_CONFIG_REG,
+		.mask = PMX_TIMER_0_1_MASK | PMX_TIMER_2_3_MASK |
+			PMX_SSP_CS_MASK,
+		.val = 0,
+	},
+};
+
+static struct spear_muxreg uart1_modem_ext_34_to_45_muxreg[] = {
+	{
+		.reg = IP_SEL_PAD_30_39_REG,
+		.mask = PMX_PL_34_MASK | PMX_PL_35_MASK | PMX_PL_36_MASK,
+		.val = PMX_UART1_ENH_PL_34_VAL | PMX_UART1_ENH_PL_35_VAL |
+			PMX_UART1_ENH_PL_36_VAL,
+	}, {
+		.reg = IP_SEL_PAD_40_49_REG,
+		.mask = PMX_PL_43_MASK | PMX_PL_44_45_MASK,
+		.val = PMX_UART1_ENH_PL_43_VAL | PMX_UART1_ENH_PL_44_45_VAL,
+	}, {
+		.reg = IP_SEL_MIX_PAD_REG,
+		.mask = PMX_UART1_ENH_PORT_SEL_MASK,
+		.val = PMX_UART1_ENH_PORT_44_45_34_36_VAL,
+	},
+};
+
+static struct spear_muxreg uart1_modem_ext_80_to_85_muxreg[] = {
+	{
+		.reg = IP_SEL_PAD_80_89_REG,
+		.mask = PMX_PL_80_TO_85_MASK,
+		.val = PMX_UART1_ENH_PL_80_TO_85_VAL,
+	}, {
+		.reg = IP_SEL_PAD_40_49_REG,
+		.mask = PMX_PL_43_MASK | PMX_PL_44_45_MASK,
+		.val = PMX_UART1_ENH_PL_43_VAL | PMX_UART1_ENH_PL_44_45_VAL,
+	}, {
+		.reg = IP_SEL_MIX_PAD_REG,
+		.mask = PMX_UART1_ENH_PORT_SEL_MASK,
+		.val = PMX_UART1_ENH_PORT_81_TO_85_VAL,
+	},
+};
+
+static struct spear_modemux uart1_modem_2_to_7_modemux[] = {
+	{
+		.modes = EXTENDED_MODE,
+		.muxregs = uart1_modem_ext_2_to_7_muxreg,
+		.nmuxregs = ARRAY_SIZE(uart1_modem_ext_2_to_7_muxreg),
+	},
+};
+
+static struct spear_modemux uart1_modem_31_to_36_modemux[] = {
+	{
+		.modes = SMALL_PRINTERS_MODE | EXTENDED_MODE,
+		.muxregs = uart1_modem_31_to_36_muxreg,
+		.nmuxregs = ARRAY_SIZE(uart1_modem_31_to_36_muxreg),
+	}, {
+		.modes = EXTENDED_MODE,
+		.muxregs = uart1_modem_ext_31_to_36_muxreg,
+		.nmuxregs = ARRAY_SIZE(uart1_modem_ext_31_to_36_muxreg),
+	},
+};
+
+static struct spear_modemux uart1_modem_34_to_45_modemux[] = {
+	{
+		.modes = AUTO_EXP_MODE | EXTENDED_MODE,
+		.muxregs = uart1_modem_34_to_45_muxreg,
+		.nmuxregs = ARRAY_SIZE(uart1_modem_34_to_45_muxreg),
+	}, {
+		.modes = EXTENDED_MODE,
+		.muxregs = uart1_modem_ext_34_to_45_muxreg,
+		.nmuxregs = ARRAY_SIZE(uart1_modem_ext_34_to_45_muxreg),
+	},
+};
+
+static struct spear_modemux uart1_modem_80_to_85_modemux[] = {
+	{
+		.modes = EXTENDED_MODE,
+		.muxregs = uart1_modem_ext_80_to_85_muxreg,
+		.nmuxregs = ARRAY_SIZE(uart1_modem_ext_80_to_85_muxreg),
+	},
+};
+
+static struct spear_pingroup uart1_modem_pingroup[] = {
+	{
+		.name = "uart1_modem_2_to_7_grp",
+		.pins = uart1_modem_2_to_7_pins,
+		.npins = ARRAY_SIZE(uart1_modem_2_to_7_pins),
+		.modemuxs = uart1_modem_2_to_7_modemux,
+		.nmodemuxs = ARRAY_SIZE(uart1_modem_2_to_7_modemux),
+	}, {
+		.name = "uart1_modem_31_to_36_grp",
+		.pins = uart1_modem_31_to_36_pins,
+		.npins = ARRAY_SIZE(uart1_modem_31_to_36_pins),
+		.modemuxs = uart1_modem_31_to_36_modemux,
+		.nmodemuxs = ARRAY_SIZE(uart1_modem_31_to_36_modemux),
+	}, {
+		.name = "uart1_modem_34_to_45_grp",
+		.pins = uart1_modem_34_to_45_pins,
+		.npins = ARRAY_SIZE(uart1_modem_34_to_45_pins),
+		.modemuxs = uart1_modem_34_to_45_modemux,
+		.nmodemuxs = ARRAY_SIZE(uart1_modem_34_to_45_modemux),
+	}, {
+		.name = "uart1_modem_80_to_85_grp",
+		.pins = uart1_modem_80_to_85_pins,
+		.npins = ARRAY_SIZE(uart1_modem_80_to_85_pins),
+		.modemuxs = uart1_modem_80_to_85_modemux,
+		.nmodemuxs = ARRAY_SIZE(uart1_modem_80_to_85_modemux),
+	},
+};
+
+static const char *const uart1_modem_grps[] = { "uart1_modem_2_to_7_grp",
+	"uart1_modem_31_to_36_grp", "uart1_modem_34_to_45_grp",
+	"uart1_modem_80_to_85_grp" };
+static struct spear_function uart1_modem_function = {
+	.name = "uart1_modem",
+	.groups = uart1_modem_grps,
+	.ngroups = ARRAY_SIZE(uart1_modem_grps),
+};
+
+/* Pad multiplexing for UART2 device */
+static const unsigned uart2_pins[] = { 0, 1 };
+static struct spear_muxreg uart2_muxreg[] = {
+	{
+		.reg = PMX_CONFIG_REG,
+		.mask = PMX_FIRDA_MASK,
+		.val = 0,
+	},
+};
+
+static struct spear_muxreg uart2_ext_muxreg[] = {
+	{
+		.reg = IP_SEL_PAD_0_9_REG,
+		.mask = PMX_PL_0_1_MASK,
+		.val = PMX_UART2_PL_0_1_VAL,
+	},
+};
+
+static struct spear_modemux uart2_modemux[] = {
+	{
+		.modes = AUTO_NET_SMII_MODE | AUTO_NET_MII_MODE | AUTO_EXP_MODE
+			| SMALL_PRINTERS_MODE | EXTENDED_MODE,
+		.muxregs = uart2_muxreg,
+		.nmuxregs = ARRAY_SIZE(uart2_muxreg),
+	}, {
+		.modes = EXTENDED_MODE,
+		.muxregs = uart2_ext_muxreg,
+		.nmuxregs = ARRAY_SIZE(uart2_ext_muxreg),
+	},
+};
+
+static struct spear_pingroup uart2_pingroup = {
+	.name = "uart2_grp",
+	.pins = uart2_pins,
+	.npins = ARRAY_SIZE(uart2_pins),
+	.modemuxs = uart2_modemux,
+	.nmodemuxs = ARRAY_SIZE(uart2_modemux),
+};
+
+static const char *const uart2_grps[] = { "uart2_grp" };
+static struct spear_function uart2_function = {
+	.name = "uart2",
+	.groups = uart2_grps,
+	.ngroups = ARRAY_SIZE(uart2_grps),
+};
+
+/* Pad multiplexing for uart3 device */
+static const unsigned uart3_pins[][2] = { { 8, 9 }, { 15, 16 }, { 41, 42 },
+	{ 52, 53 }, { 73, 74 }, { 94, 95 }, { 98, 99 } };
+
+static struct spear_muxreg uart3_ext_8_9_muxreg[] = {
+	{
+		.reg = PMX_CONFIG_REG,
+		.mask = PMX_SSP_MASK,
+		.val = 0,
+	}, {
+		.reg = IP_SEL_PAD_0_9_REG,
+		.mask = PMX_PL_8_9_MASK,
+		.val = PMX_UART3_PL_8_9_VAL,
+	}, {
+		.reg = IP_SEL_MIX_PAD_REG,
+		.mask = PMX_UART3_PORT_SEL_MASK,
+		.val = PMX_UART3_PORT_8_VAL,
+	},
+};
+
+static struct spear_muxreg uart3_ext_15_16_muxreg[] = {
+	{
+		.reg = PMX_CONFIG_REG,
+		.mask = PMX_MII_MASK,
+		.val = 0,
+	}, {
+		.reg = IP_SEL_PAD_10_19_REG,
+		.mask = PMX_PL_15_16_MASK,
+		.val = PMX_UART3_PL_15_16_VAL,
+	}, {
+		.reg = IP_SEL_MIX_PAD_REG,
+		.mask = PMX_UART3_PORT_SEL_MASK,
+		.val = PMX_UART3_PORT_15_VAL,
+	},
+};
+
+static struct spear_muxreg uart3_ext_41_42_muxreg[] = {
+	{
+		.reg = PMX_CONFIG_REG,
+		.mask = PMX_UART0_MODEM_MASK,
+		.val = 0,
+	}, {
+		.reg = IP_SEL_PAD_40_49_REG,
+		.mask = PMX_PL_41_42_MASK,
+		.val = PMX_UART3_PL_41_42_VAL,
+	}, {
+		.reg = IP_SEL_MIX_PAD_REG,
+		.mask = PMX_UART3_PORT_SEL_MASK,
+		.val = PMX_UART3_PORT_41_VAL,
+	},
+};
+
+static struct spear_muxreg uart3_ext_52_53_muxreg[] = {
+	{
+		.reg = IP_SEL_PAD_50_59_REG,
+		.mask = PMX_PL_52_53_MASK,
+		.val = PMX_UART3_PL_52_53_VAL,
+	}, {
+		.reg = IP_SEL_MIX_PAD_REG,
+		.mask = PMX_UART3_PORT_SEL_MASK,
+		.val = PMX_UART3_PORT_52_VAL,
+	},
+};
+
+static struct spear_muxreg uart3_ext_73_74_muxreg[] = {
+	{
+		.reg = IP_SEL_PAD_70_79_REG,
+		.mask = PMX_PL_73_MASK | PMX_PL_74_MASK,
+		.val = PMX_UART3_PL_73_VAL | PMX_UART3_PL_74_VAL,
+	}, {
+		.reg = IP_SEL_MIX_PAD_REG,
+		.mask = PMX_UART3_PORT_SEL_MASK,
+		.val = PMX_UART3_PORT_73_VAL,
+	},
+};
+
+static struct spear_muxreg uart3_ext_94_95_muxreg[] = {
+	{
+		.reg = IP_SEL_PAD_90_99_REG,
+		.mask = PMX_PL_94_95_MASK,
+		.val = PMX_UART3_PL_94_95_VAL,
+	}, {
+		.reg = IP_SEL_MIX_PAD_REG,
+		.mask = PMX_UART3_PORT_SEL_MASK,
+		.val = PMX_UART3_PORT_94_VAL,
+	},
+};
+
+static struct spear_muxreg uart3_ext_98_99_muxreg[] = {
+	{
+		.reg = IP_SEL_PAD_90_99_REG,
+		.mask = PMX_PL_98_MASK | PMX_PL_99_MASK,
+		.val = PMX_UART3_PL_98_VAL | PMX_UART3_PL_99_VAL,
+	}, {
+		.reg = IP_SEL_MIX_PAD_REG,
+		.mask = PMX_UART3_PORT_SEL_MASK,
+		.val = PMX_UART3_PORT_99_VAL,
+	},
+};
+
+static struct spear_modemux uart3_modemux[][1] = {
+	{
+		/* Select signals on pins 8_9 */
+		{
+			.modes = EXTENDED_MODE,
+			.muxregs = uart3_ext_8_9_muxreg,
+			.nmuxregs = ARRAY_SIZE(uart3_ext_8_9_muxreg),
+		},
+	}, {
+		/* Select signals on pins 15_16 */
+		{
+			.modes = EXTENDED_MODE,
+			.muxregs = uart3_ext_15_16_muxreg,
+			.nmuxregs = ARRAY_SIZE(uart3_ext_15_16_muxreg),
+		},
+	}, {
+		/* Select signals on pins 41_42 */
+		{
+			.modes = EXTENDED_MODE,
+			.muxregs = uart3_ext_41_42_muxreg,
+			.nmuxregs = ARRAY_SIZE(uart3_ext_41_42_muxreg),
+		},
+	}, {
+		/* Select signals on pins 52_53 */
+		{
+			.modes = EXTENDED_MODE,
+			.muxregs = uart3_ext_52_53_muxreg,
+			.nmuxregs = ARRAY_SIZE(uart3_ext_52_53_muxreg),
+		},
+	}, {
+		/* Select signals on pins 73_74 */
+		{
+			.modes = EXTENDED_MODE,
+			.muxregs = uart3_ext_73_74_muxreg,
+			.nmuxregs = ARRAY_SIZE(uart3_ext_73_74_muxreg),
+		},
+	}, {
+		/* Select signals on pins 94_95 */
+		{
+			.modes = EXTENDED_MODE,
+			.muxregs = uart3_ext_94_95_muxreg,
+			.nmuxregs = ARRAY_SIZE(uart3_ext_94_95_muxreg),
+		},
+	}, {
+		/* Select signals on pins 98_99 */
+		{
+			.modes = EXTENDED_MODE,
+			.muxregs = uart3_ext_98_99_muxreg,
+			.nmuxregs = ARRAY_SIZE(uart3_ext_98_99_muxreg),
+		},
+	},
+};
+
+static struct spear_pingroup uart3_pingroup[] = {
+	{
+		.name = "uart3_8_9_grp",
+		.pins = uart3_pins[0],
+		.npins = ARRAY_SIZE(uart3_pins[0]),
+		.modemuxs = uart3_modemux[0],
+		.nmodemuxs = ARRAY_SIZE(uart3_modemux[0]),
+	}, {
+		.name = "uart3_15_16_grp",
+		.pins = uart3_pins[1],
+		.npins = ARRAY_SIZE(uart3_pins[1]),
+		.modemuxs = uart3_modemux[1],
+		.nmodemuxs = ARRAY_SIZE(uart3_modemux[1]),
+	}, {
+		.name = "uart3_41_42_grp",
+		.pins = uart3_pins[2],
+		.npins = ARRAY_SIZE(uart3_pins[2]),
+		.modemuxs = uart3_modemux[2],
+		.nmodemuxs = ARRAY_SIZE(uart3_modemux[2]),
+	}, {
+		.name = "uart3_52_53_grp",
+		.pins = uart3_pins[3],
+		.npins = ARRAY_SIZE(uart3_pins[3]),
+		.modemuxs = uart3_modemux[3],
+		.nmodemuxs = ARRAY_SIZE(uart3_modemux[3]),
+	}, {
+		.name = "uart3_73_74_grp",
+		.pins = uart3_pins[4],
+		.npins = ARRAY_SIZE(uart3_pins[4]),
+		.modemuxs = uart3_modemux[4],
+		.nmodemuxs = ARRAY_SIZE(uart3_modemux[4]),
+	}, {
+		.name = "uart3_94_95_grp",
+		.pins = uart3_pins[5],
+		.npins = ARRAY_SIZE(uart3_pins[5]),
+		.modemuxs = uart3_modemux[5],
+		.nmodemuxs = ARRAY_SIZE(uart3_modemux[5]),
+	}, {
+		.name = "uart3_98_99_grp",
+		.pins = uart3_pins[6],
+		.npins = ARRAY_SIZE(uart3_pins[6]),
+		.modemuxs = uart3_modemux[6],
+		.nmodemuxs = ARRAY_SIZE(uart3_modemux[6]),
+	},
+};
+
+static const char *const uart3_grps[] = { "uart3_8_9_grp", "uart3_15_16_grp",
+	"uart3_41_42_grp", "uart3_52_53_grp", "uart3_73_74_grp",
+	"uart3_94_95_grp", "uart3_98_99_grp" };
+
+static struct spear_function uart3_function = {
+	.name = "uart3",
+	.groups = uart3_grps,
+	.ngroups = ARRAY_SIZE(uart3_grps),
+};
+
+/* Pad multiplexing for uart4 device */
+static const unsigned uart4_pins[][2] = { { 6, 7 }, { 13, 14 }, { 39, 40 },
+	{ 71, 72 }, { 92, 93 }, { 100, 101 } };
+
+static struct spear_muxreg uart4_ext_6_7_muxreg[] = {
+	{
+		.reg = PMX_CONFIG_REG,
+		.mask = PMX_SSP_MASK,
+		.val = 0,
+	}, {
+		.reg = IP_SEL_PAD_0_9_REG,
+		.mask = PMX_PL_6_7_MASK,
+		.val = PMX_UART4_PL_6_7_VAL,
+	}, {
+		.reg = IP_SEL_MIX_PAD_REG,
+		.mask = PMX_UART4_PORT_SEL_MASK,
+		.val = PMX_UART4_PORT_6_VAL,
+	},
+};
+
+static struct spear_muxreg uart4_ext_13_14_muxreg[] = {
+	{
+		.reg = PMX_CONFIG_REG,
+		.mask = PMX_MII_MASK,
+		.val = 0,
+	}, {
+		.reg = IP_SEL_PAD_10_19_REG,
+		.mask = PMX_PL_13_14_MASK,
+		.val = PMX_UART4_PL_13_14_VAL,
+	}, {
+		.reg = IP_SEL_MIX_PAD_REG,
+		.mask = PMX_UART4_PORT_SEL_MASK,
+		.val = PMX_UART4_PORT_13_VAL,
+	},
+};
+
+static struct spear_muxreg uart4_ext_39_40_muxreg[] = {
+	{
+		.reg = PMX_CONFIG_REG,
+		.mask = PMX_UART0_MODEM_MASK,
+		.val = 0,
+	}, {
+		.reg = IP_SEL_PAD_30_39_REG,
+		.mask = PMX_PL_39_MASK,
+		.val = PMX_UART4_PL_39_VAL,
+	}, {
+		.reg = IP_SEL_PAD_40_49_REG,
+		.mask = PMX_PL_40_MASK,
+		.val = PMX_UART4_PL_40_VAL,
+	}, {
+		.reg = IP_SEL_MIX_PAD_REG,
+		.mask = PMX_UART4_PORT_SEL_MASK,
+		.val = PMX_UART4_PORT_39_VAL,
+	},
+};
+
+static struct spear_muxreg uart4_ext_71_72_muxreg[] = {
+	{
+		.reg = IP_SEL_PAD_70_79_REG,
+		.mask = PMX_PL_71_72_MASK,
+		.val = PMX_UART4_PL_71_72_VAL,
+	}, {
+		.reg = IP_SEL_MIX_PAD_REG,
+		.mask = PMX_UART4_PORT_SEL_MASK,
+		.val = PMX_UART4_PORT_71_VAL,
+	},
+};
+
+static struct spear_muxreg uart4_ext_92_93_muxreg[] = {
+	{
+		.reg = IP_SEL_PAD_90_99_REG,
+		.mask = PMX_PL_92_93_MASK,
+		.val = PMX_UART4_PL_92_93_VAL,
+	}, {
+		.reg = IP_SEL_MIX_PAD_REG,
+		.mask = PMX_UART4_PORT_SEL_MASK,
+		.val = PMX_UART4_PORT_92_VAL,
+	},
+};
+
+static struct spear_muxreg uart4_ext_100_101_muxreg[] = {
+	{
+		.reg = IP_SEL_MIX_PAD_REG,
+		.mask = PMX_PL_100_101_MASK |
+			PMX_UART4_PORT_SEL_MASK,
+		.val = PMX_UART4_PL_100_101_VAL |
+			PMX_UART4_PORT_101_VAL,
+	},
+};
+
+static struct spear_modemux uart4_modemux[][1] = {
+	{
+		/* Select signals on pins 6_7 */
+		{
+			.modes = EXTENDED_MODE,
+			.muxregs = uart4_ext_6_7_muxreg,
+			.nmuxregs = ARRAY_SIZE(uart4_ext_6_7_muxreg),
+		},
+	}, {
+		/* Select signals on pins 13_14 */
+		{
+			.modes = EXTENDED_MODE,
+			.muxregs = uart4_ext_13_14_muxreg,
+			.nmuxregs = ARRAY_SIZE(uart4_ext_13_14_muxreg),
+		},
+	}, {
+		/* Select signals on pins 39_40 */
+		{
+			.modes = EXTENDED_MODE,
+			.muxregs = uart4_ext_39_40_muxreg,
+			.nmuxregs = ARRAY_SIZE(uart4_ext_39_40_muxreg),
+		},
+	}, {
+		/* Select signals on pins 71_72 */
+		{
+			.modes = EXTENDED_MODE,
+			.muxregs = uart4_ext_71_72_muxreg,
+			.nmuxregs = ARRAY_SIZE(uart4_ext_71_72_muxreg),
+		},
+	}, {
+		/* Select signals on pins 92_93 */
+		{
+			.modes = EXTENDED_MODE,
+			.muxregs = uart4_ext_92_93_muxreg,
+			.nmuxregs = ARRAY_SIZE(uart4_ext_92_93_muxreg),
+		},
+	}, {
+		/* Select signals on pins 100_101_ */
+		{
+			.modes = EXTENDED_MODE,
+			.muxregs = uart4_ext_100_101_muxreg,
+			.nmuxregs = ARRAY_SIZE(uart4_ext_100_101_muxreg),
+		},
+	},
+};
+
+static struct spear_pingroup uart4_pingroup[] = {
+	{
+		.name = "uart4_6_7_grp",
+		.pins = uart4_pins[0],
+		.npins = ARRAY_SIZE(uart4_pins[0]),
+		.modemuxs = uart4_modemux[0],
+		.nmodemuxs = ARRAY_SIZE(uart4_modemux[0]),
+	}, {
+		.name = "uart4_13_14_grp",
+		.pins = uart4_pins[1],
+		.npins = ARRAY_SIZE(uart4_pins[1]),
+		.modemuxs = uart4_modemux[1],
+		.nmodemuxs = ARRAY_SIZE(uart4_modemux[1]),
+	}, {
+		.name = "uart4_39_40_grp",
+		.pins = uart4_pins[2],
+		.npins = ARRAY_SIZE(uart4_pins[2]),
+		.modemuxs = uart4_modemux[2],
+		.nmodemuxs = ARRAY_SIZE(uart4_modemux[2]),
+	}, {
+		.name = "uart4_71_72_grp",
+		.pins = uart4_pins[3],
+		.npins = ARRAY_SIZE(uart4_pins[3]),
+		.modemuxs = uart4_modemux[3],
+		.nmodemuxs = ARRAY_SIZE(uart4_modemux[3]),
+	}, {
+		.name = "uart4_92_93_grp",
+		.pins = uart4_pins[4],
+		.npins = ARRAY_SIZE(uart4_pins[4]),
+		.modemuxs = uart4_modemux[4],
+		.nmodemuxs = ARRAY_SIZE(uart4_modemux[4]),
+	}, {
+		.name = "uart4_100_101_grp",
+		.pins = uart4_pins[5],
+		.npins = ARRAY_SIZE(uart4_pins[5]),
+		.modemuxs = uart4_modemux[5],
+		.nmodemuxs = ARRAY_SIZE(uart4_modemux[5]),
+	},
+};
+
+static const char *const uart4_grps[] = { "uart4_6_7_grp", "uart4_13_14_grp",
+	"uart4_39_40_grp", "uart4_71_72_grp", "uart4_92_93_grp",
+	"uart4_100_101_grp" };
+
+static struct spear_function uart4_function = {
+	.name = "uart4",
+	.groups = uart4_grps,
+	.ngroups = ARRAY_SIZE(uart4_grps),
+};
+
+/* Pad multiplexing for uart5 device */
+static const unsigned uart5_pins[][2] = { { 4, 5 }, { 37, 38 }, { 69, 70 },
+	{ 90, 91 } };
+
+static struct spear_muxreg uart5_ext_4_5_muxreg[] = {
+	{
+		.reg = PMX_CONFIG_REG,
+		.mask = PMX_I2C_MASK,
+		.val = 0,
+	}, {
+		.reg = IP_SEL_PAD_0_9_REG,
+		.mask = PMX_PL_4_5_MASK,
+		.val = PMX_UART5_PL_4_5_VAL,
+	}, {
+		.reg = IP_SEL_MIX_PAD_REG,
+		.mask = PMX_UART5_PORT_SEL_MASK,
+		.val = PMX_UART5_PORT_4_VAL,
+	},
+};
+
+static struct spear_muxreg uart5_ext_37_38_muxreg[] = {
+	{
+		.reg = PMX_CONFIG_REG,
+		.mask = PMX_UART0_MODEM_MASK,
+		.val = 0,
+	}, {
+		.reg = IP_SEL_PAD_30_39_REG,
+		.mask = PMX_PL_37_38_MASK,
+		.val = PMX_UART5_PL_37_38_VAL,
+	}, {
+		.reg = IP_SEL_MIX_PAD_REG,
+		.mask = PMX_UART5_PORT_SEL_MASK,
+		.val = PMX_UART5_PORT_37_VAL,
+	},
+};
+
+static struct spear_muxreg uart5_ext_69_70_muxreg[] = {
+	{
+		.reg = IP_SEL_PAD_60_69_REG,
+		.mask = PMX_PL_69_MASK,
+		.val = PMX_UART5_PL_69_VAL,
+	}, {
+		.reg = IP_SEL_PAD_70_79_REG,
+		.mask = PMX_PL_70_MASK,
+		.val = PMX_UART5_PL_70_VAL,
+	}, {
+		.reg = IP_SEL_MIX_PAD_REG,
+		.mask = PMX_UART5_PORT_SEL_MASK,
+		.val = PMX_UART5_PORT_69_VAL,
+	},
+};
+
+static struct spear_muxreg uart5_ext_90_91_muxreg[] = {
+	{
+		.reg = IP_SEL_PAD_90_99_REG,
+		.mask = PMX_PL_90_91_MASK,
+		.val = PMX_UART5_PL_90_91_VAL,
+	}, {
+		.reg = IP_SEL_MIX_PAD_REG,
+		.mask = PMX_UART5_PORT_SEL_MASK,
+		.val = PMX_UART5_PORT_90_VAL,
+	},
+};
+
+static struct spear_modemux uart5_modemux[][1] = {
+	{
+		/* Select signals on pins 4_5 */
+		{
+			.modes = EXTENDED_MODE,
+			.muxregs = uart5_ext_4_5_muxreg,
+			.nmuxregs = ARRAY_SIZE(uart5_ext_4_5_muxreg),
+		},
+	}, {
+		/* Select signals on pins 37_38 */
+		{
+			.modes = EXTENDED_MODE,
+			.muxregs = uart5_ext_37_38_muxreg,
+			.nmuxregs = ARRAY_SIZE(uart5_ext_37_38_muxreg),
+		},
+	}, {
+		/* Select signals on pins 69_70 */
+		{
+			.modes = EXTENDED_MODE,
+			.muxregs = uart5_ext_69_70_muxreg,
+			.nmuxregs = ARRAY_SIZE(uart5_ext_69_70_muxreg),
+		},
+	}, {
+		/* Select signals on pins 90_91 */
+		{
+			.modes = EXTENDED_MODE,
+			.muxregs = uart5_ext_90_91_muxreg,
+			.nmuxregs = ARRAY_SIZE(uart5_ext_90_91_muxreg),
+		},
+	},
+};
+
+static struct spear_pingroup uart5_pingroup[] = {
+	{
+		.name = "uart5_4_5_grp",
+		.pins = uart5_pins[0],
+		.npins = ARRAY_SIZE(uart5_pins[0]),
+		.modemuxs = uart5_modemux[0],
+		.nmodemuxs = ARRAY_SIZE(uart5_modemux[0]),
+	}, {
+		.name = "uart5_37_38_grp",
+		.pins = uart5_pins[1],
+		.npins = ARRAY_SIZE(uart5_pins[1]),
+		.modemuxs = uart5_modemux[1],
+		.nmodemuxs = ARRAY_SIZE(uart5_modemux[1]),
+	}, {
+		.name = "uart5_69_70_grp",
+		.pins = uart5_pins[2],
+		.npins = ARRAY_SIZE(uart5_pins[2]),
+		.modemuxs = uart5_modemux[2],
+		.nmodemuxs = ARRAY_SIZE(uart5_modemux[2]),
+	}, {
+		.name = "uart5_90_91_grp",
+		.pins = uart5_pins[3],
+		.npins = ARRAY_SIZE(uart5_pins[3]),
+		.modemuxs = uart5_modemux[3],
+		.nmodemuxs = ARRAY_SIZE(uart5_modemux[3]),
+	},
+};
+
+static const char *const uart5_grps[] = { "uart5_4_5_grp", "uart5_37_38_grp",
+	"uart5_69_70_grp", "uart5_90_91_grp" };
+static struct spear_function uart5_function = {
+	.name = "uart5",
+	.groups = uart5_grps,
+	.ngroups = ARRAY_SIZE(uart5_grps),
+};
+
+/* Pad multiplexing for uart6 device */
+static const unsigned uart6_pins[][2] = { { 2, 3 }, { 88, 89 } };
+static struct spear_muxreg uart6_ext_2_3_muxreg[] = {
+	{
+		.reg = PMX_CONFIG_REG,
+		.mask = PMX_UART0_MASK,
+		.val = 0,
+	}, {
+		.reg = IP_SEL_PAD_0_9_REG,
+		.mask = PMX_PL_2_3_MASK,
+		.val = PMX_UART6_PL_2_3_VAL,
+	}, {
+		.reg = IP_SEL_MIX_PAD_REG,
+		.mask = PMX_UART6_PORT_SEL_MASK,
+		.val = PMX_UART6_PORT_2_VAL,
+	},
+};
+
+static struct spear_muxreg uart6_ext_88_89_muxreg[] = {
+	{
+		.reg = IP_SEL_PAD_80_89_REG,
+		.mask = PMX_PL_88_89_MASK,
+		.val = PMX_UART6_PL_88_89_VAL,
+	}, {
+		.reg = IP_SEL_MIX_PAD_REG,
+		.mask = PMX_UART6_PORT_SEL_MASK,
+		.val = PMX_UART6_PORT_88_VAL,
+	},
+};
+
+static struct spear_modemux uart6_modemux[][1] = {
+	{
+		/* Select signals on pins 2_3 */
+		{
+			.modes = EXTENDED_MODE,
+			.muxregs = uart6_ext_2_3_muxreg,
+			.nmuxregs = ARRAY_SIZE(uart6_ext_2_3_muxreg),
+		},
+	}, {
+		/* Select signals on pins 88_89 */
+		{
+			.modes = EXTENDED_MODE,
+			.muxregs = uart6_ext_88_89_muxreg,
+			.nmuxregs = ARRAY_SIZE(uart6_ext_88_89_muxreg),
+		},
+	},
+};
+
+static struct spear_pingroup uart6_pingroup[] = {
+	{
+		.name = "uart6_2_3_grp",
+		.pins = uart6_pins[0],
+		.npins = ARRAY_SIZE(uart6_pins[0]),
+		.modemuxs = uart6_modemux[0],
+		.nmodemuxs = ARRAY_SIZE(uart6_modemux[0]),
+	}, {
+		.name = "uart6_88_89_grp",
+		.pins = uart6_pins[1],
+		.npins = ARRAY_SIZE(uart6_pins[1]),
+		.modemuxs = uart6_modemux[1],
+		.nmodemuxs = ARRAY_SIZE(uart6_modemux[1]),
+	},
+};
+
+static const char *const uart6_grps[] = { "uart6_2_3_grp", "uart6_88_89_grp" };
+static struct spear_function uart6_function = {
+	.name = "uart6",
+	.groups = uart6_grps,
+	.ngroups = ARRAY_SIZE(uart6_grps),
+};
+
+/* UART - RS485 pmx */
+static const unsigned rs485_pins[] = { 77, 78, 79 };
+static struct spear_muxreg rs485_muxreg[] = {
+	{
+		.reg = IP_SEL_PAD_70_79_REG,
+		.mask = PMX_PL_77_78_79_MASK,
+		.val = PMX_RS485_PL_77_78_79_VAL,
+	},
+};
+
+static struct spear_modemux rs485_modemux[] = {
+	{
+		.modes = EXTENDED_MODE,
+		.muxregs = rs485_muxreg,
+		.nmuxregs = ARRAY_SIZE(rs485_muxreg),
+	},
+};
+
+static struct spear_pingroup rs485_pingroup = {
+	.name = "rs485_grp",
+	.pins = rs485_pins,
+	.npins = ARRAY_SIZE(rs485_pins),
+	.modemuxs = rs485_modemux,
+	.nmodemuxs = ARRAY_SIZE(rs485_modemux),
+};
+
+static const char *const rs485_grps[] = { "rs485_grp" };
+static struct spear_function rs485_function = {
+	.name = "rs485",
+	.groups = rs485_grps,
+	.ngroups = ARRAY_SIZE(rs485_grps),
+};
+
+/* Pad multiplexing for Touchscreen device */
+static const unsigned touchscreen_pins[] = { 5, 36 };
+static struct spear_muxreg touchscreen_muxreg[] = {
+	{
+		.reg = PMX_CONFIG_REG,
+		.mask = PMX_I2C_MASK | PMX_SSP_CS_MASK,
+		.val = 0,
+	},
+};
+
+static struct spear_muxreg touchscreen_ext_muxreg[] = {
+	{
+		.reg = IP_SEL_PAD_0_9_REG,
+		.mask = PMX_PL_5_MASK,
+		.val = PMX_TOUCH_Y_PL_5_VAL,
+	}, {
+		.reg = IP_SEL_PAD_30_39_REG,
+		.mask = PMX_PL_36_MASK,
+		.val = PMX_TOUCH_X_PL_36_VAL,
+	},
+};
+
+static struct spear_modemux touchscreen_modemux[] = {
+	{
+		.modes = AUTO_NET_SMII_MODE | EXTENDED_MODE,
+		.muxregs = touchscreen_muxreg,
+		.nmuxregs = ARRAY_SIZE(touchscreen_muxreg),
+	}, {
+		.modes = EXTENDED_MODE,
+		.muxregs = touchscreen_ext_muxreg,
+		.nmuxregs = ARRAY_SIZE(touchscreen_ext_muxreg),
+	},
+};
+
+static struct spear_pingroup touchscreen_pingroup = {
+	.name = "touchscreen_grp",
+	.pins = touchscreen_pins,
+	.npins = ARRAY_SIZE(touchscreen_pins),
+	.modemuxs = touchscreen_modemux,
+	.nmodemuxs = ARRAY_SIZE(touchscreen_modemux),
+};
+
+static const char *const touchscreen_grps[] = { "touchscreen_grp" };
+static struct spear_function touchscreen_function = {
+	.name = "touchscreen",
+	.groups = touchscreen_grps,
+	.ngroups = ARRAY_SIZE(touchscreen_grps),
+};
+
+/* Pad multiplexing for CAN device */
+static const unsigned can0_pins[] = { 32, 33 };
+static struct spear_muxreg can0_muxreg[] = {
+	{
+		.reg = PMX_CONFIG_REG,
+		.mask = PMX_GPIO_PIN4_MASK | PMX_GPIO_PIN5_MASK,
+		.val = 0,
+	},
+};
+
+static struct spear_muxreg can0_ext_muxreg[] = {
+	{
+		.reg = IP_SEL_PAD_30_39_REG,
+		.mask = PMX_PL_32_33_MASK,
+		.val = PMX_CAN0_PL_32_33_VAL,
+	},
+};
+
+static struct spear_modemux can0_modemux[] = {
+	{
+		.modes = AUTO_NET_SMII_MODE | AUTO_NET_MII_MODE | AUTO_EXP_MODE
+			| EXTENDED_MODE,
+		.muxregs = can0_muxreg,
+		.nmuxregs = ARRAY_SIZE(can0_muxreg),
+	}, {
+		.modes = EXTENDED_MODE,
+		.muxregs = can0_ext_muxreg,
+		.nmuxregs = ARRAY_SIZE(can0_ext_muxreg),
+	},
+};
+
+static struct spear_pingroup can0_pingroup = {
+	.name = "can0_grp",
+	.pins = can0_pins,
+	.npins = ARRAY_SIZE(can0_pins),
+	.modemuxs = can0_modemux,
+	.nmodemuxs = ARRAY_SIZE(can0_modemux),
+};
+
+static const char *const can0_grps[] = { "can0_grp" };
+static struct spear_function can0_function = {
+	.name = "can0",
+	.groups = can0_grps,
+	.ngroups = ARRAY_SIZE(can0_grps),
+};
+
+static const unsigned can1_pins[] = { 30, 31 };
+static struct spear_muxreg can1_muxreg[] = {
+	{
+		.reg = PMX_CONFIG_REG,
+		.mask = PMX_GPIO_PIN2_MASK | PMX_GPIO_PIN3_MASK,
+		.val = 0,
+	},
+};
+
+static struct spear_muxreg can1_ext_muxreg[] = {
+	{
+		.reg = IP_SEL_PAD_30_39_REG,
+		.mask = PMX_PL_30_31_MASK,
+		.val = PMX_CAN1_PL_30_31_VAL,
+	},
+};
+
+static struct spear_modemux can1_modemux[] = {
+	{
+		.modes = AUTO_NET_SMII_MODE | AUTO_NET_MII_MODE | AUTO_EXP_MODE
+			| EXTENDED_MODE,
+		.muxregs = can1_muxreg,
+		.nmuxregs = ARRAY_SIZE(can1_muxreg),
+	}, {
+		.modes = EXTENDED_MODE,
+		.muxregs = can1_ext_muxreg,
+		.nmuxregs = ARRAY_SIZE(can1_ext_muxreg),
+	},
+};
+
+static struct spear_pingroup can1_pingroup = {
+	.name = "can1_grp",
+	.pins = can1_pins,
+	.npins = ARRAY_SIZE(can1_pins),
+	.modemuxs = can1_modemux,
+	.nmodemuxs = ARRAY_SIZE(can1_modemux),
+};
+
+static const char *const can1_grps[] = { "can1_grp" };
+static struct spear_function can1_function = {
+	.name = "can1",
+	.groups = can1_grps,
+	.ngroups = ARRAY_SIZE(can1_grps),
+};
+
+/* Pad multiplexing for PWM0_1 device */
+static const unsigned pwm0_1_pins[][2] = { { 37, 38 }, { 14, 15 }, { 8, 9 },
+	{ 30, 31 }, { 42, 43 }, { 59, 60 }, { 88, 89 } };
+
+static struct spear_muxreg pwm0_1_pin_8_9_muxreg[] = {
+	{
+		.reg = PMX_CONFIG_REG,
+		.mask = PMX_SSP_MASK,
+		.val = 0,
+	}, {
+		.reg = IP_SEL_PAD_0_9_REG,
+		.mask = PMX_PL_8_9_MASK,
+		.val = PMX_PWM_0_1_PL_8_9_VAL,
+	},
+};
+
+static struct spear_muxreg pwm0_1_autoexpsmallpri_muxreg[] = {
+	{
+		.reg = PMX_CONFIG_REG,
+		.mask = PMX_MII_MASK,
+		.val = 0,
+	},
+};
+
+static struct spear_muxreg pwm0_1_pin_14_15_muxreg[] = {
+	{
+		.reg = IP_SEL_PAD_10_19_REG,
+		.mask = PMX_PL_14_MASK | PMX_PL_15_MASK,
+		.val = PMX_PWM1_PL_14_VAL | PMX_PWM0_PL_15_VAL,
+	},
+};
+
+static struct spear_muxreg pwm0_1_pin_30_31_muxreg[] = {
+	{
+		.reg = PMX_CONFIG_REG,
+		.mask = PMX_GPIO_PIN2_MASK | PMX_GPIO_PIN3_MASK,
+		.val = 0,
+	}, {
+		.reg = IP_SEL_PAD_30_39_REG,
+		.mask = PMX_PL_30_MASK | PMX_PL_31_MASK,
+		.val = PMX_PWM1_EXT_PL_30_VAL | PMX_PWM0_EXT_PL_31_VAL,
+	},
+};
+
+static struct spear_muxreg pwm0_1_net_muxreg[] = {
+	{
+		.reg = PMX_CONFIG_REG,
+		.mask = PMX_UART0_MODEM_MASK,
+		.val = 0,
+	},
+};
+
+static struct spear_muxreg pwm0_1_pin_37_38_muxreg[] = {
+	{
+		.reg = IP_SEL_PAD_30_39_REG,
+		.mask = PMX_PL_37_38_MASK,
+		.val = PMX_PWM0_1_PL_37_38_VAL,
+	},
+};
+
+static struct spear_muxreg pwm0_1_pin_42_43_muxreg[] = {
+	{
+		.reg = PMX_CONFIG_REG,
+		.mask = PMX_UART0_MODEM_MASK | PMX_TIMER_0_1_MASK ,
+		.val = 0,
+	}, {
+		.reg = IP_SEL_PAD_40_49_REG,
+		.mask = PMX_PL_42_MASK | PMX_PL_43_MASK,
+		.val = PMX_PWM1_PL_42_VAL |
+			PMX_PWM0_PL_43_VAL,
+	},
+};
+
+static struct spear_muxreg pwm0_1_pin_59_60_muxreg[] = {
+	{
+		.reg = IP_SEL_PAD_50_59_REG,
+		.mask = PMX_PL_59_MASK,
+		.val = PMX_PWM1_PL_59_VAL,
+	}, {
+		.reg = IP_SEL_PAD_60_69_REG,
+		.mask = PMX_PL_60_MASK,
+		.val = PMX_PWM0_PL_60_VAL,
+	},
+};
+
+static struct spear_muxreg pwm0_1_pin_88_89_muxreg[] = {
+	{
+		.reg = IP_SEL_PAD_80_89_REG,
+		.mask = PMX_PL_88_89_MASK,
+		.val = PMX_PWM0_1_PL_88_89_VAL,
+	},
+};
+
+static struct spear_modemux pwm0_1_pin_8_9_modemux[] = {
+	{
+		.modes = EXTENDED_MODE,
+		.muxregs = pwm0_1_pin_8_9_muxreg,
+		.nmuxregs = ARRAY_SIZE(pwm0_1_pin_8_9_muxreg),
+	},
+};
+
+static struct spear_modemux pwm0_1_pin_14_15_modemux[] = {
+	{
+		.modes = AUTO_EXP_MODE | SMALL_PRINTERS_MODE | EXTENDED_MODE,
+		.muxregs = pwm0_1_autoexpsmallpri_muxreg,
+		.nmuxregs = ARRAY_SIZE(pwm0_1_autoexpsmallpri_muxreg),
+	}, {
+		.modes = EXTENDED_MODE,
+		.muxregs = pwm0_1_pin_14_15_muxreg,
+		.nmuxregs = ARRAY_SIZE(pwm0_1_pin_14_15_muxreg),
+	},
+};
+
+static struct spear_modemux pwm0_1_pin_30_31_modemux[] = {
+	{
+		.modes = EXTENDED_MODE,
+		.muxregs = pwm0_1_pin_30_31_muxreg,
+		.nmuxregs = ARRAY_SIZE(pwm0_1_pin_30_31_muxreg),
+	},
+};
+
+static struct spear_modemux pwm0_1_pin_37_38_modemux[] = {
+	{
+		.modes = AUTO_NET_SMII_MODE | AUTO_NET_MII_MODE | EXTENDED_MODE,
+		.muxregs = pwm0_1_net_muxreg,
+		.nmuxregs = ARRAY_SIZE(pwm0_1_net_muxreg),
+	}, {
+		.modes = EXTENDED_MODE,
+		.muxregs = pwm0_1_pin_37_38_muxreg,
+		.nmuxregs = ARRAY_SIZE(pwm0_1_pin_37_38_muxreg),
+	},
+};
+
+static struct spear_modemux pwm0_1_pin_42_43_modemux[] = {
+	{
+		.modes = EXTENDED_MODE,
+		.muxregs = pwm0_1_pin_42_43_muxreg,
+		.nmuxregs = ARRAY_SIZE(pwm0_1_pin_42_43_muxreg),
+	},
+};
+
+static struct spear_modemux pwm0_1_pin_59_60_modemux[] = {
+	{
+		.modes = EXTENDED_MODE,
+		.muxregs = pwm0_1_pin_59_60_muxreg,
+		.nmuxregs = ARRAY_SIZE(pwm0_1_pin_59_60_muxreg),
+	},
+};
+
+static struct spear_modemux pwm0_1_pin_88_89_modemux[] = {
+	{
+		.modes = EXTENDED_MODE,
+		.muxregs = pwm0_1_pin_88_89_muxreg,
+		.nmuxregs = ARRAY_SIZE(pwm0_1_pin_88_89_muxreg),
+	},
+};
+
+static struct spear_pingroup pwm0_1_pingroup[] = {
+	{
+		.name = "pwm0_1_pin_8_9_grp",
+		.pins = pwm0_1_pins[0],
+		.npins = ARRAY_SIZE(pwm0_1_pins[0]),
+		.modemuxs = pwm0_1_pin_8_9_modemux,
+		.nmodemuxs = ARRAY_SIZE(pwm0_1_pin_8_9_modemux),
+	}, {
+		.name = "pwm0_1_pin_14_15_grp",
+		.pins = pwm0_1_pins[1],
+		.npins = ARRAY_SIZE(pwm0_1_pins[1]),
+		.modemuxs = pwm0_1_pin_14_15_modemux,
+		.nmodemuxs = ARRAY_SIZE(pwm0_1_pin_14_15_modemux),
+	}, {
+		.name = "pwm0_1_pin_30_31_grp",
+		.pins = pwm0_1_pins[2],
+		.npins = ARRAY_SIZE(pwm0_1_pins[2]),
+		.modemuxs = pwm0_1_pin_30_31_modemux,
+		.nmodemuxs = ARRAY_SIZE(pwm0_1_pin_30_31_modemux),
+	}, {
+		.name = "pwm0_1_pin_37_38_grp",
+		.pins = pwm0_1_pins[3],
+		.npins = ARRAY_SIZE(pwm0_1_pins[3]),
+		.modemuxs = pwm0_1_pin_37_38_modemux,
+		.nmodemuxs = ARRAY_SIZE(pwm0_1_pin_37_38_modemux),
+	}, {
+		.name = "pwm0_1_pin_42_43_grp",
+		.pins = pwm0_1_pins[4],
+		.npins = ARRAY_SIZE(pwm0_1_pins[4]),
+		.modemuxs = pwm0_1_pin_42_43_modemux,
+		.nmodemuxs = ARRAY_SIZE(pwm0_1_pin_42_43_modemux),
+	}, {
+		.name = "pwm0_1_pin_59_60_grp",
+		.pins = pwm0_1_pins[5],
+		.npins = ARRAY_SIZE(pwm0_1_pins[5]),
+		.modemuxs = pwm0_1_pin_59_60_modemux,
+		.nmodemuxs = ARRAY_SIZE(pwm0_1_pin_59_60_modemux),
+	}, {
+		.name = "pwm0_1_pin_88_89_grp",
+		.pins = pwm0_1_pins[6],
+		.npins = ARRAY_SIZE(pwm0_1_pins[6]),
+		.modemuxs = pwm0_1_pin_88_89_modemux,
+		.nmodemuxs = ARRAY_SIZE(pwm0_1_pin_88_89_modemux),
+	},
+};
+
+static const char *const pwm0_1_grps[] = { "pwm0_1_pin_8_9_grp",
+	"pwm0_1_pin_14_15_grp", "pwm0_1_pin_30_31_grp", "pwm0_1_pin_37_38_grp",
+	"pwm0_1_pin_42_43_grp", "pwm0_1_pin_59_60_grp", "pwm0_1_pin_88_89_grp"
+};
+
+static struct spear_function pwm0_1_function = {
+	.name = "pwm0_1",
+	.groups = pwm0_1_grps,
+	.ngroups = ARRAY_SIZE(pwm0_1_grps),
+};
+
+/* Pad multiplexing for PWM2 device */
+static const unsigned pwm2_pins[][1] = { { 7 }, { 13 }, { 29 }, { 34 }, { 41 },
+	{ 58 }, { 87 } };
+static struct spear_muxreg pwm2_net_muxreg[] = {
+	{
+		.reg = PMX_CONFIG_REG,
+		.mask = PMX_SSP_CS_MASK,
+		.val = 0,
+	},
+};
+
+static struct spear_muxreg pwm2_pin_7_muxreg[] = {
+	{
+		.reg = IP_SEL_PAD_0_9_REG,
+		.mask = PMX_PL_7_MASK,
+		.val = PMX_PWM_2_PL_7_VAL,
+	},
+};
+
+static struct spear_muxreg pwm2_autoexpsmallpri_muxreg[] = {
+	{
+		.reg = PMX_CONFIG_REG,
+		.mask = PMX_MII_MASK,
+		.val = 0,
+	},
+};
+
+static struct spear_muxreg pwm2_pin_13_muxreg[] = {
+	{
+		.reg = IP_SEL_PAD_10_19_REG,
+		.mask = PMX_PL_13_MASK,
+		.val = PMX_PWM2_PL_13_VAL,
+	},
+};
+
+static struct spear_muxreg pwm2_pin_29_muxreg[] = {
+	{
+		.reg = PMX_CONFIG_REG,
+		.mask = PMX_GPIO_PIN1_MASK,
+		.val = 0,
+	}, {
+		.reg = IP_SEL_PAD_20_29_REG,
+		.mask = PMX_PL_29_MASK,
+		.val = PMX_PWM_2_PL_29_VAL,
+	},
+};
+
+static struct spear_muxreg pwm2_pin_34_muxreg[] = {
+	{
+		.reg = PMX_CONFIG_REG,
+		.mask = PMX_SSP_CS_MASK,
+		.val = 0,
+	}, {
+		.reg = IP_SEL_PAD_30_39_REG,
+		.mask = PMX_PL_34_MASK,
+		.val = PMX_PWM2_PL_34_VAL,
+	},
+};
+
+static struct spear_muxreg pwm2_pin_41_muxreg[] = {
+	{
+		.reg = PMX_CONFIG_REG,
+		.mask = PMX_UART0_MODEM_MASK,
+		.val = 0,
+	}, {
+		.reg = IP_SEL_PAD_40_49_REG,
+		.mask = PMX_PL_41_MASK,
+		.val = PMX_PWM2_PL_41_VAL,
+	},
+};
+
+static struct spear_muxreg pwm2_pin_58_muxreg[] = {
+	{
+		.reg = IP_SEL_PAD_50_59_REG,
+		.mask = PMX_PL_58_MASK,
+		.val = PMX_PWM2_PL_58_VAL,
+	},
+};
+
+static struct spear_muxreg pwm2_pin_87_muxreg[] = {
+	{
+		.reg = IP_SEL_PAD_80_89_REG,
+		.mask = PMX_PL_87_MASK,
+		.val = PMX_PWM2_PL_87_VAL,
+	},
+};
+
+static struct spear_modemux pwm2_pin_7_modemux[] = {
+	{
+		.modes = AUTO_NET_SMII_MODE | AUTO_NET_MII_MODE | EXTENDED_MODE,
+		.muxregs = pwm2_net_muxreg,
+		.nmuxregs = ARRAY_SIZE(pwm2_net_muxreg),
+	}, {
+		.modes = EXTENDED_MODE,
+		.muxregs = pwm2_pin_7_muxreg,
+		.nmuxregs = ARRAY_SIZE(pwm2_pin_7_muxreg),
+	},
+};
+static struct spear_modemux pwm2_pin_13_modemux[] = {
+	{
+		.modes = AUTO_EXP_MODE | SMALL_PRINTERS_MODE | EXTENDED_MODE,
+		.muxregs = pwm2_autoexpsmallpri_muxreg,
+		.nmuxregs = ARRAY_SIZE(pwm2_autoexpsmallpri_muxreg),
+	}, {
+		.modes = EXTENDED_MODE,
+		.muxregs = pwm2_pin_13_muxreg,
+		.nmuxregs = ARRAY_SIZE(pwm2_pin_13_muxreg),
+	},
+};
+static struct spear_modemux pwm2_pin_29_modemux[] = {
+	{
+		.modes = EXTENDED_MODE,
+		.muxregs = pwm2_pin_29_muxreg,
+		.nmuxregs = ARRAY_SIZE(pwm2_pin_29_muxreg),
+	},
+};
+static struct spear_modemux pwm2_pin_34_modemux[] = {
+	{
+		.modes = EXTENDED_MODE,
+		.muxregs = pwm2_pin_34_muxreg,
+		.nmuxregs = ARRAY_SIZE(pwm2_pin_34_muxreg),
+	},
+};
+
+static struct spear_modemux pwm2_pin_41_modemux[] = {
+	{
+		.modes = EXTENDED_MODE,
+		.muxregs = pwm2_pin_41_muxreg,
+		.nmuxregs = ARRAY_SIZE(pwm2_pin_41_muxreg),
+	},
+};
+
+static struct spear_modemux pwm2_pin_58_modemux[] = {
+	{
+		.modes = EXTENDED_MODE,
+		.muxregs = pwm2_pin_58_muxreg,
+		.nmuxregs = ARRAY_SIZE(pwm2_pin_58_muxreg),
+	},
+};
+
+static struct spear_modemux pwm2_pin_87_modemux[] = {
+	{
+		.modes = EXTENDED_MODE,
+		.muxregs = pwm2_pin_87_muxreg,
+		.nmuxregs = ARRAY_SIZE(pwm2_pin_87_muxreg),
+	},
+};
+
+static struct spear_pingroup pwm2_pingroup[] = {
+	{
+		.name = "pwm2_pin_7_grp",
+		.pins = pwm2_pins[0],
+		.npins = ARRAY_SIZE(pwm2_pins[0]),
+		.modemuxs = pwm2_pin_7_modemux,
+		.nmodemuxs = ARRAY_SIZE(pwm2_pin_7_modemux),
+	}, {
+		.name = "pwm2_pin_13_grp",
+		.pins = pwm2_pins[1],
+		.npins = ARRAY_SIZE(pwm2_pins[1]),
+		.modemuxs = pwm2_pin_13_modemux,
+		.nmodemuxs = ARRAY_SIZE(pwm2_pin_13_modemux),
+	}, {
+		.name = "pwm2_pin_29_grp",
+		.pins = pwm2_pins[2],
+		.npins = ARRAY_SIZE(pwm2_pins[2]),
+		.modemuxs = pwm2_pin_29_modemux,
+		.nmodemuxs = ARRAY_SIZE(pwm2_pin_29_modemux),
+	}, {
+		.name = "pwm2_pin_34_grp",
+		.pins = pwm2_pins[3],
+		.npins = ARRAY_SIZE(pwm2_pins[3]),
+		.modemuxs = pwm2_pin_34_modemux,
+		.nmodemuxs = ARRAY_SIZE(pwm2_pin_34_modemux),
+	}, {
+		.name = "pwm2_pin_41_grp",
+		.pins = pwm2_pins[4],
+		.npins = ARRAY_SIZE(pwm2_pins[4]),
+		.modemuxs = pwm2_pin_41_modemux,
+		.nmodemuxs = ARRAY_SIZE(pwm2_pin_41_modemux),
+	}, {
+		.name = "pwm2_pin_58_grp",
+		.pins = pwm2_pins[5],
+		.npins = ARRAY_SIZE(pwm2_pins[5]),
+		.modemuxs = pwm2_pin_58_modemux,
+		.nmodemuxs = ARRAY_SIZE(pwm2_pin_58_modemux),
+	}, {
+		.name = "pwm2_pin_87_grp",
+		.pins = pwm2_pins[6],
+		.npins = ARRAY_SIZE(pwm2_pins[6]),
+		.modemuxs = pwm2_pin_87_modemux,
+		.nmodemuxs = ARRAY_SIZE(pwm2_pin_87_modemux),
+	},
+};
+
+static const char *const pwm2_grps[] = { "pwm2_pin_7_grp", "pwm2_pin_13_grp",
+	"pwm2_pin_29_grp", "pwm2_pin_34_grp", "pwm2_pin_41_grp",
+	"pwm2_pin_58_grp", "pwm2_pin_87_grp" };
+static struct spear_function pwm2_function = {
+	.name = "pwm2",
+	.groups = pwm2_grps,
+	.ngroups = ARRAY_SIZE(pwm2_grps),
+};
+
+/* Pad multiplexing for PWM3 device */
+static const unsigned pwm3_pins[][1] = { { 6 }, { 12 }, { 28 }, { 40 }, { 57 },
+	{ 86 } };
+static struct spear_muxreg pwm3_pin_6_muxreg[] = {
+	{
+		.reg = PMX_CONFIG_REG,
+		.mask = PMX_SSP_MASK,
+		.val = 0,
+	}, {
+		.reg = IP_SEL_PAD_0_9_REG,
+		.mask = PMX_PL_6_MASK,
+		.val = PMX_PWM_3_PL_6_VAL,
+	},
+};
+
+static struct spear_muxreg pwm3_muxreg[] = {
+	{
+		.reg = PMX_CONFIG_REG,
+		.mask = PMX_MII_MASK,
+		.val = 0,
+	},
+};
+
+static struct spear_muxreg pwm3_pin_12_muxreg[] = {
+	{
+		.reg = IP_SEL_PAD_10_19_REG,
+		.mask = PMX_PL_12_MASK,
+		.val = PMX_PWM3_PL_12_VAL,
+	},
+};
+
+static struct spear_muxreg pwm3_pin_28_muxreg[] = {
+	{
+		.reg = PMX_CONFIG_REG,
+		.mask = PMX_GPIO_PIN0_MASK,
+		.val = 0,
+	}, {
+		.reg = IP_SEL_PAD_20_29_REG,
+		.mask = PMX_PL_28_MASK,
+		.val = PMX_PWM_3_PL_28_VAL,
+	},
+};
+
+static struct spear_muxreg pwm3_pin_40_muxreg[] = {
+	{
+		.reg = PMX_CONFIG_REG,
+		.mask = PMX_UART0_MODEM_MASK,
+		.val = 0,
+	}, {
+		.reg = IP_SEL_PAD_40_49_REG,
+		.mask = PMX_PL_40_MASK,
+		.val = PMX_PWM3_PL_40_VAL,
+	},
+};
+
+static struct spear_muxreg pwm3_pin_57_muxreg[] = {
+	{
+		.reg = IP_SEL_PAD_50_59_REG,
+		.mask = PMX_PL_57_MASK,
+		.val = PMX_PWM3_PL_57_VAL,
+	},
+};
+
+static struct spear_muxreg pwm3_pin_86_muxreg[] = {
+	{
+		.reg = IP_SEL_PAD_80_89_REG,
+		.mask = PMX_PL_86_MASK,
+		.val = PMX_PWM3_PL_86_VAL,
+	},
+};
+
+static struct spear_modemux pwm3_pin_6_modemux[] = {
+	{
+		.modes = EXTENDED_MODE,
+		.muxregs = pwm3_pin_6_muxreg,
+		.nmuxregs = ARRAY_SIZE(pwm3_pin_6_muxreg),
+	},
+};
+
+static struct spear_modemux pwm3_pin_12_modemux[] = {
+	{
+		.modes = AUTO_EXP_MODE | SMALL_PRINTERS_MODE |
+			AUTO_NET_SMII_MODE | EXTENDED_MODE,
+		.muxregs = pwm3_muxreg,
+		.nmuxregs = ARRAY_SIZE(pwm3_muxreg),
+	}, {
+		.modes = EXTENDED_MODE,
+		.muxregs = pwm3_pin_12_muxreg,
+		.nmuxregs = ARRAY_SIZE(pwm3_pin_12_muxreg),
+	},
+};
+
+static struct spear_modemux pwm3_pin_28_modemux[] = {
+	{
+		.modes = EXTENDED_MODE,
+		.muxregs = pwm3_pin_28_muxreg,
+		.nmuxregs = ARRAY_SIZE(pwm3_pin_28_muxreg),
+	},
+};
+
+static struct spear_modemux pwm3_pin_40_modemux[] = {
+	{
+		.modes = EXTENDED_MODE,
+		.muxregs = pwm3_pin_40_muxreg,
+		.nmuxregs = ARRAY_SIZE(pwm3_pin_40_muxreg),
+	},
+};
+
+static struct spear_modemux pwm3_pin_57_modemux[] = {
+	{
+		.modes = EXTENDED_MODE,
+		.muxregs = pwm3_pin_57_muxreg,
+		.nmuxregs = ARRAY_SIZE(pwm3_pin_57_muxreg),
+	},
+};
+
+static struct spear_modemux pwm3_pin_86_modemux[] = {
+	{
+		.modes = EXTENDED_MODE,
+		.muxregs = pwm3_pin_86_muxreg,
+		.nmuxregs = ARRAY_SIZE(pwm3_pin_86_muxreg),
+	},
+};
+
+static struct spear_pingroup pwm3_pingroup[] = {
+	{
+		.name = "pwm3_pin_6_grp",
+		.pins = pwm3_pins[0],
+		.npins = ARRAY_SIZE(pwm3_pins[0]),
+		.modemuxs = pwm3_pin_6_modemux,
+		.nmodemuxs = ARRAY_SIZE(pwm3_pin_6_modemux),
+	}, {
+		.name = "pwm3_pin_12_grp",
+		.pins = pwm3_pins[1],
+		.npins = ARRAY_SIZE(pwm3_pins[1]),
+		.modemuxs = pwm3_pin_12_modemux,
+		.nmodemuxs = ARRAY_SIZE(pwm3_pin_12_modemux),
+	}, {
+		.name = "pwm3_pin_28_grp",
+		.pins = pwm3_pins[2],
+		.npins = ARRAY_SIZE(pwm3_pins[2]),
+		.modemuxs = pwm3_pin_28_modemux,
+		.nmodemuxs = ARRAY_SIZE(pwm3_pin_28_modemux),
+	}, {
+		.name = "pwm3_pin_40_grp",
+		.pins = pwm3_pins[3],
+		.npins = ARRAY_SIZE(pwm3_pins[3]),
+		.modemuxs = pwm3_pin_40_modemux,
+		.nmodemuxs = ARRAY_SIZE(pwm3_pin_40_modemux),
+	}, {
+		.name = "pwm3_pin_57_grp",
+		.pins = pwm3_pins[4],
+		.npins = ARRAY_SIZE(pwm3_pins[4]),
+		.modemuxs = pwm3_pin_57_modemux,
+		.nmodemuxs = ARRAY_SIZE(pwm3_pin_57_modemux),
+	}, {
+		.name = "pwm3_pin_86_grp",
+		.pins = pwm3_pins[5],
+		.npins = ARRAY_SIZE(pwm3_pins[5]),
+		.modemuxs = pwm3_pin_86_modemux,
+		.nmodemuxs = ARRAY_SIZE(pwm3_pin_86_modemux),
+	},
+};
+
+static const char *const pwm3_grps[] = { "pwm3_pin_6_grp", "pwm3_pin_12_grp",
+	"pwm3_pin_28_grp", "pwm3_pin_40_grp", "pwm3_pin_57_grp",
+	"pwm3_pin_86_grp" };
+static struct spear_function pwm3_function = {
+	.name = "pwm3",
+	.groups = pwm3_grps,
+	.ngroups = ARRAY_SIZE(pwm3_grps),
+};
+
+/* Pad multiplexing for SSP1 device */
+static const unsigned ssp1_pins[][2] = { { 17, 20 }, { 36, 39 }, { 48, 51 },
+	{ 65, 68 }, { 94, 97 } };
+static struct spear_muxreg ssp1_muxreg[] = {
+	{
+		.reg = PMX_CONFIG_REG,
+		.mask = PMX_MII_MASK,
+		.val = 0,
+	},
+};
+
+static struct spear_muxreg ssp1_ext_17_20_muxreg[] = {
+	{
+		.reg = IP_SEL_PAD_10_19_REG,
+		.mask = PMX_PL_17_18_MASK | PMX_PL_19_MASK,
+		.val = PMX_SSP1_PL_17_18_19_20_VAL,
+	}, {
+		.reg = IP_SEL_PAD_20_29_REG,
+		.mask = PMX_PL_20_MASK,
+		.val = PMX_SSP1_PL_17_18_19_20_VAL,
+	}, {
+		.reg = IP_SEL_MIX_PAD_REG,
+		.mask = PMX_SSP1_PORT_SEL_MASK,
+		.val = PMX_SSP1_PORT_17_TO_20_VAL,
+	},
+};
+
+static struct spear_muxreg ssp1_ext_36_39_muxreg[] = {
+	{
+		.reg = PMX_CONFIG_REG,
+		.mask = PMX_UART0_MODEM_MASK | PMX_SSP_CS_MASK,
+		.val = 0,
+	}, {
+		.reg = IP_SEL_PAD_30_39_REG,
+		.mask = PMX_PL_36_MASK | PMX_PL_37_38_MASK | PMX_PL_39_MASK,
+		.val = PMX_SSP1_PL_36_VAL | PMX_SSP1_PL_37_38_VAL |
+			PMX_SSP1_PL_39_VAL,
+	}, {
+		.reg = IP_SEL_MIX_PAD_REG,
+		.mask = PMX_SSP1_PORT_SEL_MASK,
+		.val = PMX_SSP1_PORT_36_TO_39_VAL,
+	},
+};
+
+static struct spear_muxreg ssp1_ext_48_51_muxreg[] = {
+	{
+		.reg = PMX_CONFIG_REG,
+		.mask = PMX_TIMER_0_1_MASK | PMX_TIMER_2_3_MASK,
+		.val = 0,
+	}, {
+		.reg = IP_SEL_PAD_40_49_REG,
+		.mask = PMX_PL_48_49_MASK,
+		.val = PMX_SSP1_PL_48_49_VAL,
+	}, {
+		.reg = IP_SEL_PAD_50_59_REG,
+		.mask = PMX_PL_50_51_MASK,
+		.val = PMX_SSP1_PL_50_51_VAL,
+	}, {
+		.reg = IP_SEL_MIX_PAD_REG,
+		.mask = PMX_SSP1_PORT_SEL_MASK,
+		.val = PMX_SSP1_PORT_48_TO_51_VAL,
+	},
+};
+
+static struct spear_muxreg ssp1_ext_65_68_muxreg[] = {
+	{
+		.reg = IP_SEL_PAD_60_69_REG,
+		.mask = PMX_PL_65_TO_68_MASK,
+		.val = PMX_SSP1_PL_65_TO_68_VAL,
+	}, {
+		.reg = IP_SEL_MIX_PAD_REG,
+		.mask = PMX_SSP1_PORT_SEL_MASK,
+		.val = PMX_SSP1_PORT_65_TO_68_VAL,
+	},
+};
+
+static struct spear_muxreg ssp1_ext_94_97_muxreg[] = {
+	{
+		.reg = IP_SEL_PAD_90_99_REG,
+		.mask = PMX_PL_94_95_MASK | PMX_PL_96_97_MASK,
+		.val = PMX_SSP1_PL_94_95_VAL | PMX_SSP1_PL_96_97_VAL,
+	}, {
+		.reg = IP_SEL_MIX_PAD_REG,
+		.mask = PMX_SSP1_PORT_SEL_MASK,
+		.val = PMX_SSP1_PORT_94_TO_97_VAL,
+	},
+};
+
+static struct spear_modemux ssp1_17_20_modemux[] = {
+	{
+		.modes = SMALL_PRINTERS_MODE | AUTO_NET_SMII_MODE |
+			EXTENDED_MODE,
+		.muxregs = ssp1_muxreg,
+		.nmuxregs = ARRAY_SIZE(ssp1_muxreg),
+	}, {
+		.modes = EXTENDED_MODE,
+		.muxregs = ssp1_ext_17_20_muxreg,
+		.nmuxregs = ARRAY_SIZE(ssp1_ext_17_20_muxreg),
+	},
+};
+
+static struct spear_modemux ssp1_36_39_modemux[] = {
+	{
+		.modes = EXTENDED_MODE,
+		.muxregs = ssp1_ext_36_39_muxreg,
+		.nmuxregs = ARRAY_SIZE(ssp1_ext_36_39_muxreg),
+	},
+};
+
+static struct spear_modemux ssp1_48_51_modemux[] = {
+	{
+		.modes = EXTENDED_MODE,
+		.muxregs = ssp1_ext_48_51_muxreg,
+		.nmuxregs = ARRAY_SIZE(ssp1_ext_48_51_muxreg),
+	},
+};
+static struct spear_modemux ssp1_65_68_modemux[] = {
+	{
+		.modes = EXTENDED_MODE,
+		.muxregs = ssp1_ext_65_68_muxreg,
+		.nmuxregs = ARRAY_SIZE(ssp1_ext_65_68_muxreg),
+	},
+};
+
+static struct spear_modemux ssp1_94_97_modemux[] = {
+	{
+		.modes = EXTENDED_MODE,
+		.muxregs = ssp1_ext_94_97_muxreg,
+		.nmuxregs = ARRAY_SIZE(ssp1_ext_94_97_muxreg),
+	},
+};
+
+static struct spear_pingroup ssp1_pingroup[] = {
+	{
+		.name = "ssp1_17_20_grp",
+		.pins = ssp1_pins[0],
+		.npins = ARRAY_SIZE(ssp1_pins[0]),
+		.modemuxs = ssp1_17_20_modemux,
+		.nmodemuxs = ARRAY_SIZE(ssp1_17_20_modemux),
+	}, {
+		.name = "ssp1_36_39_grp",
+		.pins = ssp1_pins[1],
+		.npins = ARRAY_SIZE(ssp1_pins[1]),
+		.modemuxs = ssp1_36_39_modemux,
+		.nmodemuxs = ARRAY_SIZE(ssp1_36_39_modemux),
+	}, {
+		.name = "ssp1_48_51_grp",
+		.pins = ssp1_pins[2],
+		.npins = ARRAY_SIZE(ssp1_pins[2]),
+		.modemuxs = ssp1_48_51_modemux,
+		.nmodemuxs = ARRAY_SIZE(ssp1_48_51_modemux),
+	}, {
+		.name = "ssp1_65_68_grp",
+		.pins = ssp1_pins[3],
+		.npins = ARRAY_SIZE(ssp1_pins[3]),
+		.modemuxs = ssp1_65_68_modemux,
+		.nmodemuxs = ARRAY_SIZE(ssp1_65_68_modemux),
+	}, {
+		.name = "ssp1_94_97_grp",
+		.pins = ssp1_pins[4],
+		.npins = ARRAY_SIZE(ssp1_pins[4]),
+		.modemuxs = ssp1_94_97_modemux,
+		.nmodemuxs = ARRAY_SIZE(ssp1_94_97_modemux),
+	},
+};
+
+static const char *const ssp1_grps[] = { "ssp1_17_20_grp", "ssp1_36_39_grp",
+	"ssp1_48_51_grp", "ssp1_65_68_grp", "ssp1_94_97_grp"
+};
+static struct spear_function ssp1_function = {
+	.name = "ssp1",
+	.groups = ssp1_grps,
+	.ngroups = ARRAY_SIZE(ssp1_grps),
+};
+
+/* Pad multiplexing for SSP2 device */
+static const unsigned ssp2_pins[][2] = { { 13, 16 }, { 32, 35 }, { 44, 47 },
+	{ 61, 64 }, { 90, 93 } };
+static struct spear_muxreg ssp2_muxreg[] = {
+	{
+		.reg = PMX_CONFIG_REG,
+		.mask = PMX_MII_MASK,
+		.val = 0,
+	},
+};
+
+static struct spear_muxreg ssp2_ext_13_16_muxreg[] = {
+	{
+		.reg = IP_SEL_PAD_10_19_REG,
+		.mask = PMX_PL_13_14_MASK | PMX_PL_15_16_MASK,
+		.val = PMX_SSP2_PL_13_14_15_16_VAL,
+	}, {
+		.reg = IP_SEL_MIX_PAD_REG,
+		.mask = PMX_SSP2_PORT_SEL_MASK,
+		.val = PMX_SSP2_PORT_13_TO_16_VAL,
+	},
+};
+
+static struct spear_muxreg ssp2_ext_32_35_muxreg[] = {
+	{
+		.reg = PMX_CONFIG_REG,
+		.mask = PMX_SSP_CS_MASK | PMX_GPIO_PIN4_MASK |
+			PMX_GPIO_PIN5_MASK,
+		.val = 0,
+	}, {
+		.reg = IP_SEL_PAD_30_39_REG,
+		.mask = PMX_PL_32_33_MASK | PMX_PL_34_MASK | PMX_PL_35_MASK,
+		.val = PMX_SSP2_PL_32_33_VAL | PMX_SSP2_PL_34_VAL |
+			PMX_SSP2_PL_35_VAL,
+	}, {
+		.reg = IP_SEL_MIX_PAD_REG,
+		.mask = PMX_SSP2_PORT_SEL_MASK,
+		.val = PMX_SSP2_PORT_32_TO_35_VAL,
+	},
+};
+
+static struct spear_muxreg ssp2_ext_44_47_muxreg[] = {
+	{
+		.reg = PMX_CONFIG_REG,
+		.mask = PMX_TIMER_0_1_MASK | PMX_TIMER_2_3_MASK,
+		.val = 0,
+	}, {
+		.reg = IP_SEL_PAD_40_49_REG,
+		.mask = PMX_PL_44_45_MASK | PMX_PL_46_47_MASK,
+		.val = PMX_SSP2_PL_44_45_VAL | PMX_SSP2_PL_46_47_VAL,
+	}, {
+		.reg = IP_SEL_MIX_PAD_REG,
+		.mask = PMX_SSP2_PORT_SEL_MASK,
+		.val = PMX_SSP2_PORT_44_TO_47_VAL,
+	},
+};
+
+static struct spear_muxreg ssp2_ext_61_64_muxreg[] = {
+	{
+		.reg = IP_SEL_PAD_60_69_REG,
+		.mask = PMX_PL_61_TO_64_MASK,
+		.val = PMX_SSP2_PL_61_TO_64_VAL,
+	}, {
+		.reg = IP_SEL_MIX_PAD_REG,
+		.mask = PMX_SSP2_PORT_SEL_MASK,
+		.val = PMX_SSP2_PORT_61_TO_64_VAL,
+	},
+};
+
+static struct spear_muxreg ssp2_ext_90_93_muxreg[] = {
+	{
+		.reg = IP_SEL_PAD_90_99_REG,
+		.mask = PMX_PL_90_91_MASK | PMX_PL_92_93_MASK,
+		.val = PMX_SSP2_PL_90_91_VAL | PMX_SSP2_PL_92_93_VAL,
+	}, {
+		.reg = IP_SEL_MIX_PAD_REG,
+		.mask = PMX_SSP2_PORT_SEL_MASK,
+		.val = PMX_SSP2_PORT_90_TO_93_VAL,
+	},
+};
+
+static struct spear_modemux ssp2_13_16_modemux[] = {
+	{
+		.modes = AUTO_NET_SMII_MODE | EXTENDED_MODE,
+		.muxregs = ssp2_muxreg,
+		.nmuxregs = ARRAY_SIZE(ssp2_muxreg),
+	}, {
+		.modes = EXTENDED_MODE,
+		.muxregs = ssp2_ext_13_16_muxreg,
+		.nmuxregs = ARRAY_SIZE(ssp2_ext_13_16_muxreg),
+	},
+};
+
+static struct spear_modemux ssp2_32_35_modemux[] = {
+	{
+		.modes = EXTENDED_MODE,
+		.muxregs = ssp2_ext_32_35_muxreg,
+		.nmuxregs = ARRAY_SIZE(ssp2_ext_32_35_muxreg),
+	},
+};
+
+static struct spear_modemux ssp2_44_47_modemux[] = {
+	{
+		.modes = EXTENDED_MODE,
+		.muxregs = ssp2_ext_44_47_muxreg,
+		.nmuxregs = ARRAY_SIZE(ssp2_ext_44_47_muxreg),
+	},
+};
+
+static struct spear_modemux ssp2_61_64_modemux[] = {
+	{
+		.modes = EXTENDED_MODE,
+		.muxregs = ssp2_ext_61_64_muxreg,
+		.nmuxregs = ARRAY_SIZE(ssp2_ext_61_64_muxreg),
+	},
+};
+
+static struct spear_modemux ssp2_90_93_modemux[] = {
+	{
+		.modes = EXTENDED_MODE,
+		.muxregs = ssp2_ext_90_93_muxreg,
+		.nmuxregs = ARRAY_SIZE(ssp2_ext_90_93_muxreg),
+	},
+};
+
+static struct spear_pingroup ssp2_pingroup[] = {
+	{
+		.name = "ssp2_13_16_grp",
+		.pins = ssp2_pins[0],
+		.npins = ARRAY_SIZE(ssp2_pins[0]),
+		.modemuxs = ssp2_13_16_modemux,
+		.nmodemuxs = ARRAY_SIZE(ssp2_13_16_modemux),
+	}, {
+		.name = "ssp2_32_35_grp",
+		.pins = ssp2_pins[1],
+		.npins = ARRAY_SIZE(ssp2_pins[1]),
+		.modemuxs = ssp2_32_35_modemux,
+		.nmodemuxs = ARRAY_SIZE(ssp2_32_35_modemux),
+	}, {
+		.name = "ssp2_44_47_grp",
+		.pins = ssp2_pins[2],
+		.npins = ARRAY_SIZE(ssp2_pins[2]),
+		.modemuxs = ssp2_44_47_modemux,
+		.nmodemuxs = ARRAY_SIZE(ssp2_44_47_modemux),
+	}, {
+		.name = "ssp2_61_64_grp",
+		.pins = ssp2_pins[3],
+		.npins = ARRAY_SIZE(ssp2_pins[3]),
+		.modemuxs = ssp2_61_64_modemux,
+		.nmodemuxs = ARRAY_SIZE(ssp2_61_64_modemux),
+	}, {
+		.name = "ssp2_90_93_grp",
+		.pins = ssp2_pins[4],
+		.npins = ARRAY_SIZE(ssp2_pins[4]),
+		.modemuxs = ssp2_90_93_modemux,
+		.nmodemuxs = ARRAY_SIZE(ssp2_90_93_modemux),
+	},
+};
+
+static const char *const ssp2_grps[] = { "ssp2_13_16_grp", "ssp2_32_35_grp",
+	"ssp2_44_47_grp", "ssp2_61_64_grp", "ssp2_90_93_grp" };
+static struct spear_function ssp2_function = {
+	.name = "ssp2",
+	.groups = ssp2_grps,
+	.ngroups = ARRAY_SIZE(ssp2_grps),
+};
+
+/* Pad multiplexing for cadence mii2 as mii device */
+static const unsigned mii2_pins[] = { 80, 81, 82, 83, 84, 85, 86, 87, 88, 89,
+	90, 91, 92, 93, 94, 95, 96, 97 };
+static struct spear_muxreg mii2_muxreg[] = {
+	{
+		.reg = IP_SEL_PAD_80_89_REG,
+		.mask = PMX_PL_80_TO_85_MASK | PMX_PL_86_87_MASK |
+			PMX_PL_88_89_MASK,
+		.val = PMX_MII2_PL_80_TO_85_VAL | PMX_MII2_PL_86_87_VAL |
+			PMX_MII2_PL_88_89_VAL,
+	}, {
+		.reg = IP_SEL_PAD_90_99_REG,
+		.mask = PMX_PL_90_91_MASK | PMX_PL_92_93_MASK |
+			PMX_PL_94_95_MASK | PMX_PL_96_97_MASK,
+		.val = PMX_MII2_PL_90_91_VAL | PMX_MII2_PL_92_93_VAL |
+			PMX_MII2_PL_94_95_VAL | PMX_MII2_PL_96_97_VAL,
+	}, {
+		.reg = EXT_CTRL_REG,
+		.mask = (MAC_MODE_MASK << MAC2_MODE_SHIFT) |
+			(MAC_MODE_MASK << MAC1_MODE_SHIFT) |
+			MII_MDIO_MASK,
+		.val = (MAC_MODE_MII << MAC2_MODE_SHIFT) |
+			(MAC_MODE_MII << MAC1_MODE_SHIFT) |
+			MII_MDIO_81_VAL,
+	},
+};
+
+static struct spear_modemux mii2_modemux[] = {
+	{
+		.modes = EXTENDED_MODE,
+		.muxregs = mii2_muxreg,
+		.nmuxregs = ARRAY_SIZE(mii2_muxreg),
+	},
+};
+
+static struct spear_pingroup mii2_pingroup = {
+	.name = "mii2_grp",
+	.pins = mii2_pins,
+	.npins = ARRAY_SIZE(mii2_pins),
+	.modemuxs = mii2_modemux,
+	.nmodemuxs = ARRAY_SIZE(mii2_modemux),
+};
+
+static const char *const mii2_grps[] = { "mii2_grp" };
+static struct spear_function mii2_function = {
+	.name = "mii2",
+	.groups = mii2_grps,
+	.ngroups = ARRAY_SIZE(mii2_grps),
+};
+
+/* Pad multiplexing for cadence mii 1_2 as smii or rmii device */
+static const unsigned smii0_1_pins[] = { 10, 11, 13, 14, 15, 16, 17, 18, 19, 20,
+	21, 22, 23, 24, 25, 26, 27 };
+static const unsigned rmii0_1_pins[] = { 10, 11, 21, 22, 23, 24, 25, 26, 27 };
+static struct spear_muxreg mii0_1_muxreg[] = {
+	{
+		.reg = PMX_CONFIG_REG,
+		.mask = PMX_MII_MASK,
+		.val = 0,
+	},
+};
+
+static struct spear_muxreg smii0_1_ext_muxreg[] = {
+	{
+		.reg = IP_SEL_PAD_10_19_REG,
+		.mask = PMX_PL_10_11_MASK,
+		.val = PMX_SMII_PL_10_11_VAL,
+	}, {
+		.reg = IP_SEL_PAD_20_29_REG,
+		.mask = PMX_PL_21_TO_27_MASK,
+		.val = PMX_SMII_PL_21_TO_27_VAL,
+	}, {
+		.reg = EXT_CTRL_REG,
+		.mask = (MAC_MODE_MASK << MAC2_MODE_SHIFT) |
+			(MAC_MODE_MASK << MAC1_MODE_SHIFT) |
+			MII_MDIO_MASK,
+		.val = (MAC_MODE_SMII << MAC2_MODE_SHIFT)
+			| (MAC_MODE_SMII << MAC1_MODE_SHIFT)
+			| MII_MDIO_10_11_VAL,
+	},
+};
+
+static struct spear_muxreg rmii0_1_ext_muxreg[] = {
+	{
+		.reg = IP_SEL_PAD_10_19_REG,
+		.mask = PMX_PL_10_11_MASK | PMX_PL_13_14_MASK |
+			PMX_PL_15_16_MASK | PMX_PL_17_18_MASK | PMX_PL_19_MASK,
+		.val = PMX_RMII_PL_10_11_VAL | PMX_RMII_PL_13_14_VAL |
+			PMX_RMII_PL_15_16_VAL | PMX_RMII_PL_17_18_VAL |
+			PMX_RMII_PL_19_VAL,
+	}, {
+		.reg = IP_SEL_PAD_20_29_REG,
+		.mask = PMX_PL_20_MASK | PMX_PL_21_TO_27_MASK,
+		.val = PMX_RMII_PL_20_VAL | PMX_RMII_PL_21_TO_27_VAL,
+	}, {
+		.reg = EXT_CTRL_REG,
+		.mask = (MAC_MODE_MASK << MAC2_MODE_SHIFT) |
+			(MAC_MODE_MASK << MAC1_MODE_SHIFT) |
+			MII_MDIO_MASK,
+		.val = (MAC_MODE_RMII << MAC2_MODE_SHIFT)
+			| (MAC_MODE_RMII << MAC1_MODE_SHIFT)
+			| MII_MDIO_10_11_VAL,
+	},
+};
+
+static struct spear_modemux mii0_1_modemux[][2] = {
+	{
+		/* configure as smii */
+		{
+			.modes = AUTO_NET_SMII_MODE | AUTO_EXP_MODE |
+				SMALL_PRINTERS_MODE | EXTENDED_MODE,
+			.muxregs = mii0_1_muxreg,
+			.nmuxregs = ARRAY_SIZE(mii0_1_muxreg),
+		}, {
+			.modes = EXTENDED_MODE,
+			.muxregs = smii0_1_ext_muxreg,
+			.nmuxregs = ARRAY_SIZE(smii0_1_ext_muxreg),
+		},
+	}, {
+		/* configure as rmii */
+		{
+			.modes = AUTO_NET_SMII_MODE | AUTO_EXP_MODE |
+				SMALL_PRINTERS_MODE | EXTENDED_MODE,
+			.muxregs = mii0_1_muxreg,
+			.nmuxregs = ARRAY_SIZE(mii0_1_muxreg),
+		}, {
+			.modes = EXTENDED_MODE,
+			.muxregs = rmii0_1_ext_muxreg,
+			.nmuxregs = ARRAY_SIZE(rmii0_1_ext_muxreg),
+		},
+	},
+};
+
+static struct spear_pingroup mii0_1_pingroup[] = {
+	{
+		.name = "smii0_1_grp",
+		.pins = smii0_1_pins,
+		.npins = ARRAY_SIZE(smii0_1_pins),
+		.modemuxs = mii0_1_modemux[0],
+		.nmodemuxs = ARRAY_SIZE(mii0_1_modemux[0]),
+	}, {
+		.name = "rmii0_1_grp",
+		.pins = rmii0_1_pins,
+		.npins = ARRAY_SIZE(rmii0_1_pins),
+		.modemuxs = mii0_1_modemux[1],
+		.nmodemuxs = ARRAY_SIZE(mii0_1_modemux[1]),
+	},
+};
+
+static const char *const mii0_1_grps[] = { "smii0_1_grp", "rmii0_1_grp" };
+static struct spear_function mii0_1_function = {
+	.name = "mii0_1",
+	.groups = mii0_1_grps,
+	.ngroups = ARRAY_SIZE(mii0_1_grps),
+};
+
+/* Pad multiplexing for i2c1 device */
+static const unsigned i2c1_pins[][2] = { { 8, 9 }, { 98, 99 } };
+static struct spear_muxreg i2c1_ext_8_9_muxreg[] = {
+	{
+		.reg = PMX_CONFIG_REG,
+		.mask = PMX_SSP_CS_MASK,
+		.val = 0,
+	}, {
+		.reg = IP_SEL_PAD_0_9_REG,
+		.mask = PMX_PL_8_9_MASK,
+		.val = PMX_I2C1_PL_8_9_VAL,
+	}, {
+		.reg = IP_SEL_MIX_PAD_REG,
+		.mask = PMX_I2C1_PORT_SEL_MASK,
+		.val = PMX_I2C1_PORT_8_9_VAL,
+	},
+};
+
+static struct spear_muxreg i2c1_ext_98_99_muxreg[] = {
+	{
+		.reg = IP_SEL_PAD_90_99_REG,
+		.mask = PMX_PL_98_MASK | PMX_PL_99_MASK,
+		.val = PMX_I2C1_PL_98_VAL | PMX_I2C1_PL_99_VAL,
+	}, {
+		.reg = IP_SEL_MIX_PAD_REG,
+		.mask = PMX_I2C1_PORT_SEL_MASK,
+		.val = PMX_I2C1_PORT_98_99_VAL,
+	},
+};
+
+static struct spear_modemux i2c1_modemux[][1] = {
+	{
+		/* Select signals on pins 8-9 */
+		{
+			.modes = EXTENDED_MODE,
+			.muxregs = i2c1_ext_8_9_muxreg,
+			.nmuxregs = ARRAY_SIZE(i2c1_ext_8_9_muxreg),
+		},
+	}, {
+		/* Select signals on pins 98-99 */
+		{
+			.modes = EXTENDED_MODE,
+			.muxregs = i2c1_ext_98_99_muxreg,
+			.nmuxregs = ARRAY_SIZE(i2c1_ext_98_99_muxreg),
+		},
+	},
+};
+
+static struct spear_pingroup i2c1_pingroup[] = {
+	{
+		.name = "i2c1_8_9_grp",
+		.pins = i2c1_pins[0],
+		.npins = ARRAY_SIZE(i2c1_pins[0]),
+		.modemuxs = i2c1_modemux[0],
+		.nmodemuxs = ARRAY_SIZE(i2c1_modemux[0]),
+	}, {
+		.name = "i2c1_98_99_grp",
+		.pins = i2c1_pins[1],
+		.npins = ARRAY_SIZE(i2c1_pins[1]),
+		.modemuxs = i2c1_modemux[1],
+		.nmodemuxs = ARRAY_SIZE(i2c1_modemux[1]),
+	},
+};
+
+static const char *const i2c1_grps[] = { "i2c1_8_9_grp", "i2c1_98_99_grp" };
+static struct spear_function i2c1_function = {
+	.name = "i2c1",
+	.groups = i2c1_grps,
+	.ngroups = ARRAY_SIZE(i2c1_grps),
+};
+
+/* Pad multiplexing for i2c2 device */
+static const unsigned i2c2_pins[][2] = { { 0, 1 }, { 2, 3 }, { 19, 20 },
+	{ 75, 76 }, { 96, 97 } };
+static struct spear_muxreg i2c2_ext_0_1_muxreg[] = {
+	{
+		.reg = PMX_CONFIG_REG,
+		.mask = PMX_FIRDA_MASK,
+		.val = 0,
+	}, {
+		.reg = IP_SEL_PAD_0_9_REG,
+		.mask = PMX_PL_0_1_MASK,
+		.val = PMX_I2C2_PL_0_1_VAL,
+	}, {
+		.reg = IP_SEL_MIX_PAD_REG,
+		.mask = PMX_I2C2_PORT_SEL_MASK,
+		.val = PMX_I2C2_PORT_0_1_VAL,
+	},
+};
+
+static struct spear_muxreg i2c2_ext_2_3_muxreg[] = {
+	{
+		.reg = PMX_CONFIG_REG,
+		.mask = PMX_UART0_MASK,
+		.val = 0,
+	}, {
+		.reg = IP_SEL_PAD_0_9_REG,
+		.mask = PMX_PL_2_3_MASK,
+		.val = PMX_I2C2_PL_2_3_VAL,
+	}, {
+		.reg = IP_SEL_MIX_PAD_REG,
+		.mask = PMX_I2C2_PORT_SEL_MASK,
+		.val = PMX_I2C2_PORT_2_3_VAL,
+	},
+};
+
+static struct spear_muxreg i2c2_ext_19_20_muxreg[] = {
+	{
+		.reg = PMX_CONFIG_REG,
+		.mask = PMX_MII_MASK,
+		.val = 0,
+	}, {
+		.reg = IP_SEL_PAD_10_19_REG,
+		.mask = PMX_PL_19_MASK,
+		.val = PMX_I2C2_PL_19_VAL,
+	}, {
+		.reg = IP_SEL_PAD_20_29_REG,
+		.mask = PMX_PL_20_MASK,
+		.val = PMX_I2C2_PL_20_VAL,
+	}, {
+		.reg = IP_SEL_MIX_PAD_REG,
+		.mask = PMX_I2C2_PORT_SEL_MASK,
+		.val = PMX_I2C2_PORT_19_20_VAL,
+	},
+};
+
+static struct spear_muxreg i2c2_ext_75_76_muxreg[] = {
+	{
+		.reg = IP_SEL_PAD_70_79_REG,
+		.mask = PMX_PL_75_76_MASK,
+		.val = PMX_I2C2_PL_75_76_VAL,
+	}, {
+		.reg = IP_SEL_MIX_PAD_REG,
+		.mask = PMX_I2C2_PORT_SEL_MASK,
+		.val = PMX_I2C2_PORT_75_76_VAL,
+	},
+};
+
+static struct spear_muxreg i2c2_ext_96_97_muxreg[] = {
+	{
+		.reg = IP_SEL_PAD_90_99_REG,
+		.mask = PMX_PL_96_97_MASK,
+		.val = PMX_I2C2_PL_96_97_VAL,
+	}, {
+		.reg = IP_SEL_MIX_PAD_REG,
+		.mask = PMX_I2C2_PORT_SEL_MASK,
+		.val = PMX_I2C2_PORT_96_97_VAL,
+	},
+};
+
+static struct spear_modemux i2c2_modemux[][1] = {
+	{
+		/* Select signals on pins 0_1 */
+		{
+			.modes = EXTENDED_MODE,
+			.muxregs = i2c2_ext_0_1_muxreg,
+			.nmuxregs = ARRAY_SIZE(i2c2_ext_0_1_muxreg),
+		},
+	}, {
+		/* Select signals on pins 2_3 */
+		{
+			.modes = EXTENDED_MODE,
+			.muxregs = i2c2_ext_2_3_muxreg,
+			.nmuxregs = ARRAY_SIZE(i2c2_ext_2_3_muxreg),
+		},
+	}, {
+		/* Select signals on pins 19_20 */
+		{
+			.modes = EXTENDED_MODE,
+			.muxregs = i2c2_ext_19_20_muxreg,
+			.nmuxregs = ARRAY_SIZE(i2c2_ext_19_20_muxreg),
+		},
+	}, {
+		/* Select signals on pins 75_76 */
+		{
+			.modes = EXTENDED_MODE,
+			.muxregs = i2c2_ext_75_76_muxreg,
+			.nmuxregs = ARRAY_SIZE(i2c2_ext_75_76_muxreg),
+		},
+	}, {
+		/* Select signals on pins 96_97 */
+		{
+			.modes = EXTENDED_MODE,
+			.muxregs = i2c2_ext_96_97_muxreg,
+			.nmuxregs = ARRAY_SIZE(i2c2_ext_96_97_muxreg),
+		},
+	},
+};
+
+static struct spear_pingroup i2c2_pingroup[] = {
+	{
+		.name = "i2c2_0_1_grp",
+		.pins = i2c2_pins[0],
+		.npins = ARRAY_SIZE(i2c2_pins[0]),
+		.modemuxs = i2c2_modemux[0],
+		.nmodemuxs = ARRAY_SIZE(i2c2_modemux[0]),
+	}, {
+		.name = "i2c2_2_3_grp",
+		.pins = i2c2_pins[1],
+		.npins = ARRAY_SIZE(i2c2_pins[1]),
+		.modemuxs = i2c2_modemux[1],
+		.nmodemuxs = ARRAY_SIZE(i2c2_modemux[1]),
+	}, {
+		.name = "i2c2_19_20_grp",
+		.pins = i2c2_pins[2],
+		.npins = ARRAY_SIZE(i2c2_pins[2]),
+		.modemuxs = i2c2_modemux[2],
+		.nmodemuxs = ARRAY_SIZE(i2c2_modemux[2]),
+	}, {
+		.name = "i2c2_75_76_grp",
+		.pins = i2c2_pins[3],
+		.npins = ARRAY_SIZE(i2c2_pins[3]),
+		.modemuxs = i2c2_modemux[3],
+		.nmodemuxs = ARRAY_SIZE(i2c2_modemux[3]),
+	}, {
+		.name = "i2c2_96_97_grp",
+		.pins = i2c2_pins[4],
+		.npins = ARRAY_SIZE(i2c2_pins[4]),
+		.modemuxs = i2c2_modemux[4],
+		.nmodemuxs = ARRAY_SIZE(i2c2_modemux[4]),
+	},
+};
+
+static const char *const i2c2_grps[] = { "i2c2_0_1_grp", "i2c2_2_3_grp",
+	"i2c2_19_20_grp", "i2c2_75_76_grp", "i2c2_96_97_grp" };
+static struct spear_function i2c2_function = {
+	.name = "i2c2",
+	.groups = i2c2_grps,
+	.ngroups = ARRAY_SIZE(i2c2_grps),
+};
+
+/* pingroups */
+static struct spear_pingroup *spear320_pingroups[] = {
+	SPEAR3XX_COMMON_PINGROUPS,
+	&clcd_pingroup,
+	&emi_pingroup,
+	&fsmc_8bit_pingroup,
+	&fsmc_16bit_pingroup,
+	&spp_pingroup,
+	&sdhci_led_pingroup,
+	&sdhci_pingroup[0],
+	&sdhci_pingroup[1],
+	&i2s_pingroup,
+	&uart1_pingroup,
+	&uart1_modem_pingroup[0],
+	&uart1_modem_pingroup[1],
+	&uart1_modem_pingroup[2],
+	&uart1_modem_pingroup[3],
+	&uart2_pingroup,
+	&uart3_pingroup[0],
+	&uart3_pingroup[1],
+	&uart3_pingroup[2],
+	&uart3_pingroup[3],
+	&uart3_pingroup[4],
+	&uart3_pingroup[5],
+	&uart3_pingroup[6],
+	&uart4_pingroup[0],
+	&uart4_pingroup[1],
+	&uart4_pingroup[2],
+	&uart4_pingroup[3],
+	&uart4_pingroup[4],
+	&uart4_pingroup[5],
+	&uart5_pingroup[0],
+	&uart5_pingroup[1],
+	&uart5_pingroup[2],
+	&uart5_pingroup[3],
+	&uart6_pingroup[0],
+	&uart6_pingroup[1],
+	&rs485_pingroup,
+	&touchscreen_pingroup,
+	&can0_pingroup,
+	&can1_pingroup,
+	&pwm0_1_pingroup[0],
+	&pwm0_1_pingroup[1],
+	&pwm0_1_pingroup[2],
+	&pwm0_1_pingroup[3],
+	&pwm0_1_pingroup[4],
+	&pwm0_1_pingroup[5],
+	&pwm0_1_pingroup[6],
+	&pwm2_pingroup[0],
+	&pwm2_pingroup[1],
+	&pwm2_pingroup[2],
+	&pwm2_pingroup[3],
+	&pwm2_pingroup[4],
+	&pwm2_pingroup[5],
+	&pwm2_pingroup[6],
+	&pwm3_pingroup[0],
+	&pwm3_pingroup[1],
+	&pwm3_pingroup[2],
+	&pwm3_pingroup[3],
+	&pwm3_pingroup[4],
+	&pwm3_pingroup[5],
+	&ssp1_pingroup[0],
+	&ssp1_pingroup[1],
+	&ssp1_pingroup[2],
+	&ssp1_pingroup[3],
+	&ssp1_pingroup[4],
+	&ssp2_pingroup[0],
+	&ssp2_pingroup[1],
+	&ssp2_pingroup[2],
+	&ssp2_pingroup[3],
+	&ssp2_pingroup[4],
+	&mii2_pingroup,
+	&mii0_1_pingroup[0],
+	&mii0_1_pingroup[1],
+	&i2c1_pingroup[0],
+	&i2c1_pingroup[1],
+	&i2c2_pingroup[0],
+	&i2c2_pingroup[1],
+	&i2c2_pingroup[2],
+	&i2c2_pingroup[3],
+	&i2c2_pingroup[4],
+};
+
+/* functions */
+static struct spear_function *spear320_functions[] = {
+	SPEAR3XX_COMMON_FUNCTIONS,
+	&clcd_function,
+	&emi_function,
+	&fsmc_function,
+	&spp_function,
+	&sdhci_function,
+	&i2s_function,
+	&uart1_function,
+	&uart1_modem_function,
+	&uart2_function,
+	&uart3_function,
+	&uart4_function,
+	&uart5_function,
+	&uart6_function,
+	&rs485_function,
+	&touchscreen_function,
+	&can0_function,
+	&can1_function,
+	&pwm0_1_function,
+	&pwm2_function,
+	&pwm3_function,
+	&ssp1_function,
+	&ssp2_function,
+	&mii2_function,
+	&mii0_1_function,
+	&i2c1_function,
+	&i2c2_function,
+};
+
+static struct of_device_id spear320_pinctrl_of_match[] __devinitdata = {
+	{
+		.compatible = "st,spear320-pinmux",
+	},
+	{},
+};
+
+static int __devinit spear320_pinctrl_probe(struct platform_device *pdev)
+{
+	int ret;
+
+	spear3xx_machdata.groups = spear320_pingroups;
+	spear3xx_machdata.ngroups = ARRAY_SIZE(spear320_pingroups);
+	spear3xx_machdata.functions = spear320_functions;
+	spear3xx_machdata.nfunctions = ARRAY_SIZE(spear320_functions);
+
+	spear3xx_machdata.modes_supported = true;
+	spear3xx_machdata.pmx_modes = spear320_pmx_modes;
+	spear3xx_machdata.npmx_modes = ARRAY_SIZE(spear320_pmx_modes);
+
+	pmx_init_addr(&spear3xx_machdata, PMX_CONFIG_REG);
+
+	ret = spear_pinctrl_probe(pdev, &spear3xx_machdata);
+	if (ret)
+		return ret;
+
+	return 0;
+}
+
+static int __devexit spear320_pinctrl_remove(struct platform_device *pdev)
+{
+	return spear_pinctrl_remove(pdev);
+}
+
+static struct platform_driver spear320_pinctrl_driver = {
+	.driver = {
+		.name = DRIVER_NAME,
+		.owner = THIS_MODULE,
+		.of_match_table = spear320_pinctrl_of_match,
+	},
+	.probe = spear320_pinctrl_probe,
+	.remove = __devexit_p(spear320_pinctrl_remove),
+};
+
+static int __init spear320_pinctrl_init(void)
+{
+	return platform_driver_register(&spear320_pinctrl_driver);
+}
+arch_initcall(spear320_pinctrl_init);
+
+static void __exit spear320_pinctrl_exit(void)
+{
+	platform_driver_unregister(&spear320_pinctrl_driver);
+}
+module_exit(spear320_pinctrl_exit);
+
+MODULE_AUTHOR("Viresh Kumar <viresh.kumar@st.com>");
+MODULE_DESCRIPTION("ST Microelectronics SPEAr320 pinctrl driver");
+MODULE_LICENSE("GPL v2");
+MODULE_DEVICE_TABLE(of, spear320_pinctrl_of_match);
diff --git a/drivers/pinctrl/spear/pinctrl-spear3xx.c b/drivers/pinctrl/spear/pinctrl-spear3xx.c
new file mode 100644
index 00000000000..91c883bc46a
--- /dev/null
+++ b/drivers/pinctrl/spear/pinctrl-spear3xx.c
@@ -0,0 +1,487 @@
+/*
+ * Driver for the ST Microelectronics SPEAr3xx pinmux
+ *
+ * Copyright (C) 2012 ST Microelectronics
+ * Viresh Kumar <viresh.kumar@st.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/pinctrl/pinctrl.h>
+
+#include "pinctrl-spear3xx.h"
+
+/* pins */
+static const struct pinctrl_pin_desc spear3xx_pins[] = {
+	SPEAR_PIN_0_TO_101,
+};
+
+/* firda_pins */
+static const unsigned firda_pins[] = { 0, 1 };
+static struct spear_muxreg firda_muxreg[] = {
+	{
+		.reg = -1,
+		.mask = PMX_FIRDA_MASK,
+		.val = PMX_FIRDA_MASK,
+	},
+};
+
+static struct spear_modemux firda_modemux[] = {
+	{
+		.modes = ~0,
+		.muxregs = firda_muxreg,
+		.nmuxregs = ARRAY_SIZE(firda_muxreg),
+	},
+};
+
+struct spear_pingroup spear3xx_firda_pingroup = {
+	.name = "firda_grp",
+	.pins = firda_pins,
+	.npins = ARRAY_SIZE(firda_pins),
+	.modemuxs = firda_modemux,
+	.nmodemuxs = ARRAY_SIZE(firda_modemux),
+};
+
+static const char *const firda_grps[] = { "firda_grp" };
+struct spear_function spear3xx_firda_function = {
+	.name = "firda",
+	.groups = firda_grps,
+	.ngroups = ARRAY_SIZE(firda_grps),
+};
+
+/* i2c_pins */
+static const unsigned i2c_pins[] = { 4, 5 };
+static struct spear_muxreg i2c_muxreg[] = {
+	{
+		.reg = -1,
+		.mask = PMX_I2C_MASK,
+		.val = PMX_I2C_MASK,
+	},
+};
+
+static struct spear_modemux i2c_modemux[] = {
+	{
+		.modes = ~0,
+		.muxregs = i2c_muxreg,
+		.nmuxregs = ARRAY_SIZE(i2c_muxreg),
+	},
+};
+
+struct spear_pingroup spear3xx_i2c_pingroup = {
+	.name = "i2c0_grp",
+	.pins = i2c_pins,
+	.npins = ARRAY_SIZE(i2c_pins),
+	.modemuxs = i2c_modemux,
+	.nmodemuxs = ARRAY_SIZE(i2c_modemux),
+};
+
+static const char *const i2c_grps[] = { "i2c0_grp" };
+struct spear_function spear3xx_i2c_function = {
+	.name = "i2c0",
+	.groups = i2c_grps,
+	.ngroups = ARRAY_SIZE(i2c_grps),
+};
+
+/* ssp_cs_pins */
+static const unsigned ssp_cs_pins[] = { 34, 35, 36 };
+static struct spear_muxreg ssp_cs_muxreg[] = {
+	{
+		.reg = -1,
+		.mask = PMX_SSP_CS_MASK,
+		.val = PMX_SSP_CS_MASK,
+	},
+};
+
+static struct spear_modemux ssp_cs_modemux[] = {
+	{
+		.modes = ~0,
+		.muxregs = ssp_cs_muxreg,
+		.nmuxregs = ARRAY_SIZE(ssp_cs_muxreg),
+	},
+};
+
+struct spear_pingroup spear3xx_ssp_cs_pingroup = {
+	.name = "ssp_cs_grp",
+	.pins = ssp_cs_pins,
+	.npins = ARRAY_SIZE(ssp_cs_pins),
+	.modemuxs = ssp_cs_modemux,
+	.nmodemuxs = ARRAY_SIZE(ssp_cs_modemux),
+};
+
+static const char *const ssp_cs_grps[] = { "ssp_cs_grp" };
+struct spear_function spear3xx_ssp_cs_function = {
+	.name = "ssp_cs",
+	.groups = ssp_cs_grps,
+	.ngroups = ARRAY_SIZE(ssp_cs_grps),
+};
+
+/* ssp_pins */
+static const unsigned ssp_pins[] = { 6, 7, 8, 9 };
+static struct spear_muxreg ssp_muxreg[] = {
+	{
+		.reg = -1,
+		.mask = PMX_SSP_MASK,
+		.val = PMX_SSP_MASK,
+	},
+};
+
+static struct spear_modemux ssp_modemux[] = {
+	{
+		.modes = ~0,
+		.muxregs = ssp_muxreg,
+		.nmuxregs = ARRAY_SIZE(ssp_muxreg),
+	},
+};
+
+struct spear_pingroup spear3xx_ssp_pingroup = {
+	.name = "ssp0_grp",
+	.pins = ssp_pins,
+	.npins = ARRAY_SIZE(ssp_pins),
+	.modemuxs = ssp_modemux,
+	.nmodemuxs = ARRAY_SIZE(ssp_modemux),
+};
+
+static const char *const ssp_grps[] = { "ssp0_grp" };
+struct spear_function spear3xx_ssp_function = {
+	.name = "ssp0",
+	.groups = ssp_grps,
+	.ngroups = ARRAY_SIZE(ssp_grps),
+};
+
+/* mii_pins */
+static const unsigned mii_pins[] = { 10, 11, 12, 13, 14, 15, 16, 17, 18, 19, 20,
+	21, 22, 23, 24, 25, 26, 27 };
+static struct spear_muxreg mii_muxreg[] = {
+	{
+		.reg = -1,
+		.mask = PMX_MII_MASK,
+		.val = PMX_MII_MASK,
+	},
+};
+
+static struct spear_modemux mii_modemux[] = {
+	{
+		.modes = ~0,
+		.muxregs = mii_muxreg,
+		.nmuxregs = ARRAY_SIZE(mii_muxreg),
+	},
+};
+
+struct spear_pingroup spear3xx_mii_pingroup = {
+	.name = "mii0_grp",
+	.pins = mii_pins,
+	.npins = ARRAY_SIZE(mii_pins),
+	.modemuxs = mii_modemux,
+	.nmodemuxs = ARRAY_SIZE(mii_modemux),
+};
+
+static const char *const mii_grps[] = { "mii0_grp" };
+struct spear_function spear3xx_mii_function = {
+	.name = "mii0",
+	.groups = mii_grps,
+	.ngroups = ARRAY_SIZE(mii_grps),
+};
+
+/* gpio0_pin0_pins */
+static const unsigned gpio0_pin0_pins[] = { 28 };
+static struct spear_muxreg gpio0_pin0_muxreg[] = {
+	{
+		.reg = -1,
+		.mask = PMX_GPIO_PIN0_MASK,
+		.val = PMX_GPIO_PIN0_MASK,
+	},
+};
+
+static struct spear_modemux gpio0_pin0_modemux[] = {
+	{
+		.modes = ~0,
+		.muxregs = gpio0_pin0_muxreg,
+		.nmuxregs = ARRAY_SIZE(gpio0_pin0_muxreg),
+	},
+};
+
+struct spear_pingroup spear3xx_gpio0_pin0_pingroup = {
+	.name = "gpio0_pin0_grp",
+	.pins = gpio0_pin0_pins,
+	.npins = ARRAY_SIZE(gpio0_pin0_pins),
+	.modemuxs = gpio0_pin0_modemux,
+	.nmodemuxs = ARRAY_SIZE(gpio0_pin0_modemux),
+};
+
+/* gpio0_pin1_pins */
+static const unsigned gpio0_pin1_pins[] = { 29 };
+static struct spear_muxreg gpio0_pin1_muxreg[] = {
+	{
+		.reg = -1,
+		.mask = PMX_GPIO_PIN1_MASK,
+		.val = PMX_GPIO_PIN1_MASK,
+	},
+};
+
+static struct spear_modemux gpio0_pin1_modemux[] = {
+	{
+		.modes = ~0,
+		.muxregs = gpio0_pin1_muxreg,
+		.nmuxregs = ARRAY_SIZE(gpio0_pin1_muxreg),
+	},
+};
+
+struct spear_pingroup spear3xx_gpio0_pin1_pingroup = {
+	.name = "gpio0_pin1_grp",
+	.pins = gpio0_pin1_pins,
+	.npins = ARRAY_SIZE(gpio0_pin1_pins),
+	.modemuxs = gpio0_pin1_modemux,
+	.nmodemuxs = ARRAY_SIZE(gpio0_pin1_modemux),
+};
+
+/* gpio0_pin2_pins */
+static const unsigned gpio0_pin2_pins[] = { 30 };
+static struct spear_muxreg gpio0_pin2_muxreg[] = {
+	{
+		.reg = -1,
+		.mask = PMX_GPIO_PIN2_MASK,
+		.val = PMX_GPIO_PIN2_MASK,
+	},
+};
+
+static struct spear_modemux gpio0_pin2_modemux[] = {
+	{
+		.modes = ~0,
+		.muxregs = gpio0_pin2_muxreg,
+		.nmuxregs = ARRAY_SIZE(gpio0_pin2_muxreg),
+	},
+};
+
+struct spear_pingroup spear3xx_gpio0_pin2_pingroup = {
+	.name = "gpio0_pin2_grp",
+	.pins = gpio0_pin2_pins,
+	.npins = ARRAY_SIZE(gpio0_pin2_pins),
+	.modemuxs = gpio0_pin2_modemux,
+	.nmodemuxs = ARRAY_SIZE(gpio0_pin2_modemux),
+};
+
+/* gpio0_pin3_pins */
+static const unsigned gpio0_pin3_pins[] = { 31 };
+static struct spear_muxreg gpio0_pin3_muxreg[] = {
+	{
+		.reg = -1,
+		.mask = PMX_GPIO_PIN3_MASK,
+		.val = PMX_GPIO_PIN3_MASK,
+	},
+};
+
+static struct spear_modemux gpio0_pin3_modemux[] = {
+	{
+		.modes = ~0,
+		.muxregs = gpio0_pin3_muxreg,
+		.nmuxregs = ARRAY_SIZE(gpio0_pin3_muxreg),
+	},
+};
+
+struct spear_pingroup spear3xx_gpio0_pin3_pingroup = {
+	.name = "gpio0_pin3_grp",
+	.pins = gpio0_pin3_pins,
+	.npins = ARRAY_SIZE(gpio0_pin3_pins),
+	.modemuxs = gpio0_pin3_modemux,
+	.nmodemuxs = ARRAY_SIZE(gpio0_pin3_modemux),
+};
+
+/* gpio0_pin4_pins */
+static const unsigned gpio0_pin4_pins[] = { 32 };
+static struct spear_muxreg gpio0_pin4_muxreg[] = {
+	{
+		.reg = -1,
+		.mask = PMX_GPIO_PIN4_MASK,
+		.val = PMX_GPIO_PIN4_MASK,
+	},
+};
+
+static struct spear_modemux gpio0_pin4_modemux[] = {
+	{
+		.modes = ~0,
+		.muxregs = gpio0_pin4_muxreg,
+		.nmuxregs = ARRAY_SIZE(gpio0_pin4_muxreg),
+	},
+};
+
+struct spear_pingroup spear3xx_gpio0_pin4_pingroup = {
+	.name = "gpio0_pin4_grp",
+	.pins = gpio0_pin4_pins,
+	.npins = ARRAY_SIZE(gpio0_pin4_pins),
+	.modemuxs = gpio0_pin4_modemux,
+	.nmodemuxs = ARRAY_SIZE(gpio0_pin4_modemux),
+};
+
+/* gpio0_pin5_pins */
+static const unsigned gpio0_pin5_pins[] = { 33 };
+static struct spear_muxreg gpio0_pin5_muxreg[] = {
+	{
+		.reg = -1,
+		.mask = PMX_GPIO_PIN5_MASK,
+		.val = PMX_GPIO_PIN5_MASK,
+	},
+};
+
+static struct spear_modemux gpio0_pin5_modemux[] = {
+	{
+		.modes = ~0,
+		.muxregs = gpio0_pin5_muxreg,
+		.nmuxregs = ARRAY_SIZE(gpio0_pin5_muxreg),
+	},
+};
+
+struct spear_pingroup spear3xx_gpio0_pin5_pingroup = {
+	.name = "gpio0_pin5_grp",
+	.pins = gpio0_pin5_pins,
+	.npins = ARRAY_SIZE(gpio0_pin5_pins),
+	.modemuxs = gpio0_pin5_modemux,
+	.nmodemuxs = ARRAY_SIZE(gpio0_pin5_modemux),
+};
+
+static const char *const gpio0_grps[] = { "gpio0_pin0_grp", "gpio0_pin1_grp",
+	"gpio0_pin2_grp", "gpio0_pin3_grp", "gpio0_pin4_grp", "gpio0_pin5_grp",
+};
+struct spear_function spear3xx_gpio0_function = {
+	.name = "gpio0",
+	.groups = gpio0_grps,
+	.ngroups = ARRAY_SIZE(gpio0_grps),
+};
+
+/* uart0_ext_pins */
+static const unsigned uart0_ext_pins[] = { 37, 38, 39, 40, 41, 42 };
+static struct spear_muxreg uart0_ext_muxreg[] = {
+	{
+		.reg = -1,
+		.mask = PMX_UART0_MODEM_MASK,
+		.val = PMX_UART0_MODEM_MASK,
+	},
+};
+
+static struct spear_modemux uart0_ext_modemux[] = {
+	{
+		.modes = ~0,
+		.muxregs = uart0_ext_muxreg,
+		.nmuxregs = ARRAY_SIZE(uart0_ext_muxreg),
+	},
+};
+
+struct spear_pingroup spear3xx_uart0_ext_pingroup = {
+	.name = "uart0_ext_grp",
+	.pins = uart0_ext_pins,
+	.npins = ARRAY_SIZE(uart0_ext_pins),
+	.modemuxs = uart0_ext_modemux,
+	.nmodemuxs = ARRAY_SIZE(uart0_ext_modemux),
+};
+
+static const char *const uart0_ext_grps[] = { "uart0_ext_grp" };
+struct spear_function spear3xx_uart0_ext_function = {
+	.name = "uart0_ext",
+	.groups = uart0_ext_grps,
+	.ngroups = ARRAY_SIZE(uart0_ext_grps),
+};
+
+/* uart0_pins */
+static const unsigned uart0_pins[] = { 2, 3 };
+static struct spear_muxreg uart0_muxreg[] = {
+	{
+		.reg = -1,
+		.mask = PMX_UART0_MASK,
+		.val = PMX_UART0_MASK,
+	},
+};
+
+static struct spear_modemux uart0_modemux[] = {
+	{
+		.modes = ~0,
+		.muxregs = uart0_muxreg,
+		.nmuxregs = ARRAY_SIZE(uart0_muxreg),
+	},
+};
+
+struct spear_pingroup spear3xx_uart0_pingroup = {
+	.name = "uart0_grp",
+	.pins = uart0_pins,
+	.npins = ARRAY_SIZE(uart0_pins),
+	.modemuxs = uart0_modemux,
+	.nmodemuxs = ARRAY_SIZE(uart0_modemux),
+};
+
+static const char *const uart0_grps[] = { "uart0_grp" };
+struct spear_function spear3xx_uart0_function = {
+	.name = "uart0",
+	.groups = uart0_grps,
+	.ngroups = ARRAY_SIZE(uart0_grps),
+};
+
+/* timer_0_1_pins */
+static const unsigned timer_0_1_pins[] = { 43, 44, 47, 48 };
+static struct spear_muxreg timer_0_1_muxreg[] = {
+	{
+		.reg = -1,
+		.mask = PMX_TIMER_0_1_MASK,
+		.val = PMX_TIMER_0_1_MASK,
+	},
+};
+
+static struct spear_modemux timer_0_1_modemux[] = {
+	{
+		.modes = ~0,
+		.muxregs = timer_0_1_muxreg,
+		.nmuxregs = ARRAY_SIZE(timer_0_1_muxreg),
+	},
+};
+
+struct spear_pingroup spear3xx_timer_0_1_pingroup = {
+	.name = "timer_0_1_grp",
+	.pins = timer_0_1_pins,
+	.npins = ARRAY_SIZE(timer_0_1_pins),
+	.modemuxs = timer_0_1_modemux,
+	.nmodemuxs = ARRAY_SIZE(timer_0_1_modemux),
+};
+
+static const char *const timer_0_1_grps[] = { "timer_0_1_grp" };
+struct spear_function spear3xx_timer_0_1_function = {
+	.name = "timer_0_1",
+	.groups = timer_0_1_grps,
+	.ngroups = ARRAY_SIZE(timer_0_1_grps),
+};
+
+/* timer_2_3_pins */
+static const unsigned timer_2_3_pins[] = { 45, 46, 49, 50 };
+static struct spear_muxreg timer_2_3_muxreg[] = {
+	{
+		.reg = -1,
+		.mask = PMX_TIMER_2_3_MASK,
+		.val = PMX_TIMER_2_3_MASK,
+	},
+};
+
+static struct spear_modemux timer_2_3_modemux[] = {
+	{
+		.modes = ~0,
+		.muxregs = timer_2_3_muxreg,
+		.nmuxregs = ARRAY_SIZE(timer_2_3_muxreg),
+	},
+};
+
+struct spear_pingroup spear3xx_timer_2_3_pingroup = {
+	.name = "timer_2_3_grp",
+	.pins = timer_2_3_pins,
+	.npins = ARRAY_SIZE(timer_2_3_pins),
+	.modemuxs = timer_2_3_modemux,
+	.nmodemuxs = ARRAY_SIZE(timer_2_3_modemux),
+};
+
+static const char *const timer_2_3_grps[] = { "timer_2_3_grp" };
+struct spear_function spear3xx_timer_2_3_function = {
+	.name = "timer_2_3",
+	.groups = timer_2_3_grps,
+	.ngroups = ARRAY_SIZE(timer_2_3_grps),
+};
+
+struct spear_pinctrl_machdata spear3xx_machdata = {
+	.pins = spear3xx_pins,
+	.npins = ARRAY_SIZE(spear3xx_pins),
+};
diff --git a/drivers/pinctrl/spear/pinctrl-spear3xx.h b/drivers/pinctrl/spear/pinctrl-spear3xx.h
new file mode 100644
index 00000000000..5d5fdd8df7b
--- /dev/null
+++ b/drivers/pinctrl/spear/pinctrl-spear3xx.h
@@ -0,0 +1,92 @@
+/*
+ * Header file for the ST Microelectronics SPEAr3xx pinmux
+ *
+ * Copyright (C) 2012 ST Microelectronics
+ * Viresh Kumar <viresh.kumar@st.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.
+ */
+
+#ifndef __PINMUX_SPEAR3XX_H__
+#define __PINMUX_SPEAR3XX_H__
+
+#include "pinctrl-spear.h"
+
+/* pad mux declarations */
+#define PMX_FIRDA_MASK		(1 << 14)
+#define PMX_I2C_MASK		(1 << 13)
+#define PMX_SSP_CS_MASK		(1 << 12)
+#define PMX_SSP_MASK		(1 << 11)
+#define PMX_MII_MASK		(1 << 10)
+#define PMX_GPIO_PIN0_MASK	(1 << 9)
+#define PMX_GPIO_PIN1_MASK	(1 << 8)
+#define PMX_GPIO_PIN2_MASK	(1 << 7)
+#define PMX_GPIO_PIN3_MASK	(1 << 6)
+#define PMX_GPIO_PIN4_MASK	(1 << 5)
+#define PMX_GPIO_PIN5_MASK	(1 << 4)
+#define PMX_UART0_MODEM_MASK	(1 << 3)
+#define PMX_UART0_MASK		(1 << 2)
+#define PMX_TIMER_2_3_MASK	(1 << 1)
+#define PMX_TIMER_0_1_MASK	(1 << 0)
+
+extern struct spear_pingroup spear3xx_firda_pingroup;
+extern struct spear_pingroup spear3xx_gpio0_pin0_pingroup;
+extern struct spear_pingroup spear3xx_gpio0_pin1_pingroup;
+extern struct spear_pingroup spear3xx_gpio0_pin2_pingroup;
+extern struct spear_pingroup spear3xx_gpio0_pin3_pingroup;
+extern struct spear_pingroup spear3xx_gpio0_pin4_pingroup;
+extern struct spear_pingroup spear3xx_gpio0_pin5_pingroup;
+extern struct spear_pingroup spear3xx_i2c_pingroup;
+extern struct spear_pingroup spear3xx_mii_pingroup;
+extern struct spear_pingroup spear3xx_ssp_cs_pingroup;
+extern struct spear_pingroup spear3xx_ssp_pingroup;
+extern struct spear_pingroup spear3xx_timer_0_1_pingroup;
+extern struct spear_pingroup spear3xx_timer_2_3_pingroup;
+extern struct spear_pingroup spear3xx_uart0_ext_pingroup;
+extern struct spear_pingroup spear3xx_uart0_pingroup;
+
+#define SPEAR3XX_COMMON_PINGROUPS		\
+	&spear3xx_firda_pingroup,		\
+	&spear3xx_gpio0_pin0_pingroup,		\
+	&spear3xx_gpio0_pin1_pingroup,		\
+	&spear3xx_gpio0_pin2_pingroup,		\
+	&spear3xx_gpio0_pin3_pingroup,		\
+	&spear3xx_gpio0_pin4_pingroup,		\
+	&spear3xx_gpio0_pin5_pingroup,		\
+	&spear3xx_i2c_pingroup,			\
+	&spear3xx_mii_pingroup,			\
+	&spear3xx_ssp_cs_pingroup,		\
+	&spear3xx_ssp_pingroup,			\
+	&spear3xx_timer_0_1_pingroup,		\
+	&spear3xx_timer_2_3_pingroup,		\
+	&spear3xx_uart0_ext_pingroup,		\
+	&spear3xx_uart0_pingroup
+
+extern struct spear_function spear3xx_firda_function;
+extern struct spear_function spear3xx_gpio0_function;
+extern struct spear_function spear3xx_i2c_function;
+extern struct spear_function spear3xx_mii_function;
+extern struct spear_function spear3xx_ssp_cs_function;
+extern struct spear_function spear3xx_ssp_function;
+extern struct spear_function spear3xx_timer_0_1_function;
+extern struct spear_function spear3xx_timer_2_3_function;
+extern struct spear_function spear3xx_uart0_ext_function;
+extern struct spear_function spear3xx_uart0_function;
+
+#define SPEAR3XX_COMMON_FUNCTIONS		\
+	&spear3xx_firda_function,		\
+	&spear3xx_gpio0_function,		\
+	&spear3xx_i2c_function,			\
+	&spear3xx_mii_function,			\
+	&spear3xx_ssp_cs_function,		\
+	&spear3xx_ssp_function,			\
+	&spear3xx_timer_0_1_function,		\
+	&spear3xx_timer_2_3_function,		\
+	&spear3xx_uart0_ext_function,		\
+	&spear3xx_uart0_function
+
+extern struct spear_pinctrl_machdata spear3xx_machdata;
+
+#endif /* __PINMUX_SPEAR3XX_H__ */
diff --git a/drivers/platform/x86/fujitsu-laptop.c b/drivers/platform/x86/fujitsu-laptop.c
index 6b26666b37f..c4c1a5444b3 100644
--- a/drivers/platform/x86/fujitsu-laptop.c
+++ b/drivers/platform/x86/fujitsu-laptop.c
@@ -1,7 +1,7 @@
 /*-*-linux-c-*-*/
 
 /*
-  Copyright (C) 2007,2008 Jonathan Woithe <jwoithe@physics.adelaide.edu.au>
+  Copyright (C) 2007,2008 Jonathan Woithe <jwoithe@just42.net>
   Copyright (C) 2008 Peter Gruber <nokos@gmx.net>
   Copyright (C) 2008 Tony Vroon <tony@linx.net>
   Based on earlier work:
diff --git a/drivers/platform/x86/toshiba_acpi.c b/drivers/platform/x86/toshiba_acpi.c
index ee79ce64d9d..57787d87d9a 100644
--- a/drivers/platform/x86/toshiba_acpi.c
+++ b/drivers/platform/x86/toshiba_acpi.c
@@ -1104,6 +1104,7 @@ static int __devinit toshiba_acpi_add(struct acpi_device *acpi_dev)
 
 	mutex_init(&dev->mutex);
 
+	memset(&props, 0, sizeof(props));
 	props.type = BACKLIGHT_PLATFORM;
 	props.max_brightness = HCI_LCD_BRIGHTNESS_LEVELS - 1;
 	dev->backlight_dev = backlight_device_register("toshiba",
diff --git a/drivers/power/bq27x00_battery.c b/drivers/power/bq27x00_battery.c
index 222ccd872ac..f5d6d379f2f 100644
--- a/drivers/power/bq27x00_battery.c
+++ b/drivers/power/bq27x00_battery.c
@@ -451,7 +451,7 @@ static int bq27x00_battery_capacity_level(struct bq27x00_device_info *di,
 }
 
 /*
- * Return the battery Voltage in milivolts
+ * Return the battery Voltage in millivolts
  * Or < 0 if something fails.
  */
 static int bq27x00_battery_voltage(struct bq27x00_device_info *di,
diff --git a/drivers/power/wm831x_power.c b/drivers/power/wm831x_power.c
index 987332b71d8..fc1ad955118 100644
--- a/drivers/power/wm831x_power.c
+++ b/drivers/power/wm831x_power.c
@@ -565,7 +565,7 @@ static __devinit int wm831x_power_probe(struct platform_device *pdev)
 			    goto err_usb;
 	}
 
-	irq = platform_get_irq_byname(pdev, "SYSLO");
+	irq = wm831x_irq(wm831x, platform_get_irq_byname(pdev, "SYSLO"));
 	ret = request_threaded_irq(irq, NULL, wm831x_syslo_irq,
 				   IRQF_TRIGGER_RISING, "System power low",
 				   power);
@@ -575,7 +575,7 @@ static __devinit int wm831x_power_probe(struct platform_device *pdev)
 		goto err_battery;
 	}
 
-	irq = platform_get_irq_byname(pdev, "PWR SRC");
+	irq = wm831x_irq(wm831x, platform_get_irq_byname(pdev, "PWR SRC"));
 	ret = request_threaded_irq(irq, NULL, wm831x_pwr_src_irq,
 				   IRQF_TRIGGER_RISING, "Power source",
 				   power);
@@ -586,7 +586,9 @@ static __devinit int wm831x_power_probe(struct platform_device *pdev)
 	}
 
 	for (i = 0; i < ARRAY_SIZE(wm831x_bat_irqs); i++) {
-		irq = platform_get_irq_byname(pdev, wm831x_bat_irqs[i]);
+		irq = wm831x_irq(wm831x,
+				 platform_get_irq_byname(pdev,
+							 wm831x_bat_irqs[i]));
 		ret = request_threaded_irq(irq, NULL, wm831x_bat_irq,
 					   IRQF_TRIGGER_RISING,
 					   wm831x_bat_irqs[i],
@@ -606,10 +608,10 @@ err_bat_irq:
 		irq = platform_get_irq_byname(pdev, wm831x_bat_irqs[i]);
 		free_irq(irq, power);
 	}
-	irq = platform_get_irq_byname(pdev, "PWR SRC");
+	irq = wm831x_irq(wm831x, platform_get_irq_byname(pdev, "PWR SRC"));
 	free_irq(irq, power);
 err_syslo:
-	irq = platform_get_irq_byname(pdev, "SYSLO");
+	irq = wm831x_irq(wm831x, platform_get_irq_byname(pdev, "SYSLO"));
 	free_irq(irq, power);
 err_battery:
 	if (power->have_battery)
@@ -626,17 +628,20 @@ err_kmalloc:
 static __devexit int wm831x_power_remove(struct platform_device *pdev)
 {
 	struct wm831x_power *wm831x_power = platform_get_drvdata(pdev);
+	struct wm831x *wm831x = wm831x_power->wm831x;
 	int irq, i;
 
 	for (i = 0; i < ARRAY_SIZE(wm831x_bat_irqs); i++) {
-		irq = platform_get_irq_byname(pdev, wm831x_bat_irqs[i]);
+		irq = wm831x_irq(wm831x, 
+				 platform_get_irq_byname(pdev,
+							 wm831x_bat_irqs[i]));
 		free_irq(irq, wm831x_power);
 	}
 
-	irq = platform_get_irq_byname(pdev, "PWR SRC");
+	irq = wm831x_irq(wm831x, platform_get_irq_byname(pdev, "PWR SRC"));
 	free_irq(irq, wm831x_power);
 
-	irq = platform_get_irq_byname(pdev, "SYSLO");
+	irq = wm831x_irq(wm831x, platform_get_irq_byname(pdev, "SYSLO"));
 	free_irq(irq, wm831x_power);
 
 	if (wm831x_power->have_battery)
diff --git a/drivers/ps3/ps3av.c b/drivers/ps3/ps3av.c
index a409fa050a1..93d0a8b7718 100644
--- a/drivers/ps3/ps3av.c
+++ b/drivers/ps3/ps3av.c
@@ -338,7 +338,7 @@ int ps3av_do_pkt(u32 cid, u16 send_len, size_t usr_buf_size,
 	mutex_unlock(&ps3av->mutex);
 	return 0;
 
-      err:
+err:
 	mutex_unlock(&ps3av->mutex);
 	printk(KERN_ERR "%s: failed cid:%x res:%d\n", __func__, cid, res);
 	return res;
@@ -477,7 +477,6 @@ int ps3av_set_audio_mode(u32 ch, u32 fs, u32 word_bits, u32 format, u32 source)
 
 	return 0;
 }
-
 EXPORT_SYMBOL_GPL(ps3av_set_audio_mode);
 
 static int ps3av_set_videomode(void)
@@ -501,7 +500,7 @@ static void ps3av_set_videomode_packet(u32 id)
 
 	video_mode = &video_mode_table[id & PS3AV_MODE_MASK];
 
-	avb_param.num_of_video_pkt = PS3AV_AVB_NUM_VIDEO;	/* num of head */
+	avb_param.num_of_video_pkt = PS3AV_AVB_NUM_VIDEO; /* num of head */
 	avb_param.num_of_audio_pkt = 0;
 	avb_param.num_of_av_video_pkt = ps3av->av_hw_conf.num_of_hdmi +
 					ps3av->av_hw_conf.num_of_avmulti;
@@ -521,7 +520,7 @@ static void ps3av_set_videomode_packet(u32 id)
 #ifndef PS3AV_HDMI_YUV
 		if (ps3av->av_port[i] == PS3AV_CMD_AVPORT_HDMI_0 ||
 		    ps3av->av_port[i] == PS3AV_CMD_AVPORT_HDMI_1)
-			av_video_cs = RGB8;	/* use RGB for HDMI */
+			av_video_cs = RGB8; /* use RGB for HDMI */
 #endif
 		len += ps3av_cmd_set_av_video_cs(&avb_param.buf[len],
 						 ps3av->av_port[i],
@@ -590,8 +589,8 @@ static void ps3avd(struct work_struct *work)
 #define SHIFT_VESA	8
 
 static const struct {
-	unsigned mask : 19;
-	unsigned id :  4;
+	unsigned mask:19;
+	unsigned id:4;
 } ps3av_preferred_modes[] = {
 	{ PS3AV_RESBIT_WUXGA      << SHIFT_VESA, PS3AV_MODE_WUXGA   },
 	{ PS3AV_RESBIT_1920x1080P << SHIFT_60,   PS3AV_MODE_1080P60 },
@@ -667,7 +666,8 @@ static enum ps3av_mode_num ps3av_hdmi_get_id(struct ps3av_info_monitor *info)
 	return id;
 }
 
-static void ps3av_monitor_info_dump(const struct ps3av_pkt_av_get_monitor_info *monitor_info)
+static void ps3av_monitor_info_dump(
+	const struct ps3av_pkt_av_get_monitor_info *monitor_info)
 {
 	const struct ps3av_info_monitor *info = &monitor_info->info;
 	const struct ps3av_info_audio *audio = info->audio;
@@ -717,8 +717,8 @@ static void ps3av_monitor_info_dump(const struct ps3av_pkt_av_get_monitor_info *
 
 	/* audio block */
 	for (i = 0; i < info->num_of_audio_block; i++) {
-		pr_debug("audio[%d] type: %02x max_ch: %02x fs: %02x sbit: "
-			 "%02x\n",
+		pr_debug(
+			"audio[%d] type: %02x max_ch: %02x fs: %02x sbit: %02x\n",
 			 i, audio->type, audio->max_num_of_ch, audio->fs,
 			 audio->sbit);
 		audio++;
@@ -870,21 +870,18 @@ int ps3av_set_video_mode(int id)
 
 	return 0;
 }
-
 EXPORT_SYMBOL_GPL(ps3av_set_video_mode);
 
 int ps3av_get_auto_mode(void)
 {
 	return ps3av_auto_videomode(&ps3av->av_hw_conf);
 }
-
 EXPORT_SYMBOL_GPL(ps3av_get_auto_mode);
 
 int ps3av_get_mode(void)
 {
 	return ps3av ? ps3av->ps3av_mode : 0;
 }
-
 EXPORT_SYMBOL_GPL(ps3av_get_mode);
 
 /* get resolution by video_mode */
@@ -902,7 +899,6 @@ int ps3av_video_mode2res(u32 id, u32 *xres, u32 *yres)
 	*yres = video_mode_table[id].y;
 	return 0;
 }
-
 EXPORT_SYMBOL_GPL(ps3av_video_mode2res);
 
 /* mute */
@@ -911,7 +907,6 @@ int ps3av_video_mute(int mute)
 	return ps3av_set_av_video_mute(mute ? PS3AV_CMD_MUTE_ON
 					    : PS3AV_CMD_MUTE_OFF);
 }
-
 EXPORT_SYMBOL_GPL(ps3av_video_mute);
 
 /* mute analog output only */
@@ -935,7 +930,6 @@ int ps3av_audio_mute(int mute)
 	return ps3av_set_audio_mute(mute ? PS3AV_CMD_MUTE_ON
 					 : PS3AV_CMD_MUTE_OFF);
 }
-
 EXPORT_SYMBOL_GPL(ps3av_audio_mute);
 
 static int __devinit ps3av_probe(struct ps3_system_bus_device *dev)
diff --git a/drivers/ptp/Kconfig b/drivers/ptp/Kconfig
index cd9bc3b129b..ffdf712f9a6 100644
--- a/drivers/ptp/Kconfig
+++ b/drivers/ptp/Kconfig
@@ -70,7 +70,7 @@ config DP83640_PHY
 	  using the SO_TIMESTAMPING API.
 
 	  In order for this to work, your MAC driver must also
-	  implement the skb_tx_timetamp() function.
+	  implement the skb_tx_timestamp() function.
 
 config PTP_1588_CLOCK_PCH
 	tristate "Intel PCH EG20T as PTP clock"
@@ -78,9 +78,13 @@ config PTP_1588_CLOCK_PCH
 	depends on PCH_GBE
 	help
 	  This driver adds support for using the PCH EG20T as a PTP
-	  clock. This clock is only useful if your PTP programs are
-	  getting hardware time stamps on the PTP Ethernet packets
-	  using the SO_TIMESTAMPING API.
+	  clock. The hardware supports time stamping of PTP packets
+	  when using the end-to-end delay (E2E) mechansim. The peer
+	  delay mechansim (P2P) is not supported.
+
+	  This clock is only useful if your PTP programs are getting
+	  hardware time stamps on the PTP Ethernet packets using the
+	  SO_TIMESTAMPING API.
 
 	  To compile this driver as a module, choose M here: the module
 	  will be called ptp_pch.
diff --git a/drivers/ptp/ptp_clock.c b/drivers/ptp/ptp_clock.c
index f519a131238..1e528b539a0 100644
--- a/drivers/ptp/ptp_clock.c
+++ b/drivers/ptp/ptp_clock.c
@@ -304,6 +304,12 @@ void ptp_clock_event(struct ptp_clock *ptp, struct ptp_clock_event *event)
 }
 EXPORT_SYMBOL(ptp_clock_event);
 
+int ptp_clock_index(struct ptp_clock *ptp)
+{
+	return ptp->index;
+}
+EXPORT_SYMBOL(ptp_clock_index);
+
 /* module operations */
 
 static void __exit ptp_exit(void)
diff --git a/drivers/ptp/ptp_ixp46x.c b/drivers/ptp/ptp_ixp46x.c
index 6f2782bb5f4..e03c40692b0 100644
--- a/drivers/ptp/ptp_ixp46x.c
+++ b/drivers/ptp/ptp_ixp46x.c
@@ -284,6 +284,7 @@ static void __exit ptp_ixp_exit(void)
 {
 	free_irq(MASTER_IRQ, &ixp_clock);
 	free_irq(SLAVE_IRQ, &ixp_clock);
+	ixp46x_phc_index = -1;
 	ptp_clock_unregister(ixp_clock.ptp_clock);
 }
 
@@ -302,6 +303,8 @@ static int __init ptp_ixp_init(void)
 	if (IS_ERR(ixp_clock.ptp_clock))
 		return PTR_ERR(ixp_clock.ptp_clock);
 
+	ixp46x_phc_index = ptp_clock_index(ixp_clock.ptp_clock);
+
 	__raw_writel(DEFAULT_ADDEND, &ixp_clock.regs->addend);
 	__raw_writel(1, &ixp_clock.regs->trgt_lo);
 	__raw_writel(0, &ixp_clock.regs->trgt_hi);
diff --git a/drivers/ptp/ptp_pch.c b/drivers/ptp/ptp_pch.c
index 375eb04c16e..3a9c17eced1 100644
--- a/drivers/ptp/ptp_pch.c
+++ b/drivers/ptp/ptp_pch.c
@@ -30,6 +30,7 @@
 #include <linux/module.h>
 #include <linux/pci.h>
 #include <linux/ptp_clock_kernel.h>
+#include <linux/slab.h>
 
 #define STATION_ADDR_LEN	20
 #define PCI_DEVICE_ID_PCH_1588	0x8819
@@ -261,6 +262,7 @@ u64 pch_rx_snap_read(struct pci_dev *pdev)
 
 	ns = ((u64) hi) << 32;
 	ns |= lo;
+	ns <<= TICKS_NS_SHIFT;
 
 	return ns;
 }
@@ -277,6 +279,7 @@ u64 pch_tx_snap_read(struct pci_dev *pdev)
 
 	ns = ((u64) hi) << 32;
 	ns |= lo;
+	ns <<= TICKS_NS_SHIFT;
 
 	return ns;
 }
@@ -306,7 +309,7 @@ static void pch_reset(struct pch_dev *chip)
  *				    traffic on the  ethernet interface
  * @addr:	dress which contain the column separated address to be used.
  */
-static int pch_set_station_address(u8 *addr, struct pci_dev *pdev)
+int pch_set_station_address(u8 *addr, struct pci_dev *pdev)
 {
 	s32 i;
 	struct pch_dev *chip = pci_get_drvdata(pdev);
@@ -350,6 +353,7 @@ static int pch_set_station_address(u8 *addr, struct pci_dev *pdev)
 	}
 	return 0;
 }
+EXPORT_SYMBOL(pch_set_station_address);
 
 /*
  * Interrupt service routine
@@ -649,8 +653,6 @@ pch_probe(struct pci_dev *pdev, const struct pci_device_id *id)
 	iowrite32(1, &chip->regs->trgt_lo);
 	iowrite32(0, &chip->regs->trgt_hi);
 	iowrite32(PCH_TSE_TTIPEND, &chip->regs->event);
-	/* Version: IEEE1588 v1 and IEEE1588-2008,  Mode: All Evwnt, Locked  */
-	iowrite32(0x80020000, &chip->regs->ch_control);
 
 	pch_eth_enable_set(chip);
 
diff --git a/drivers/regulator/88pm8607.c b/drivers/regulator/88pm8607.c
index 28b81ae4cf7..c3482b954cb 100644
--- a/drivers/regulator/88pm8607.c
+++ b/drivers/regulator/88pm8607.c
@@ -27,13 +27,8 @@ struct pm8607_regulator_info {
 	unsigned int	*vol_table;
 	unsigned int	*vol_suspend;
 
-	int	vol_reg;
-	int	vol_shift;
-	int	vol_nbits;
 	int	update_reg;
 	int	update_bit;
-	int	enable_reg;
-	int	enable_bit;
 	int	slope_double;
 };
 
@@ -216,7 +211,7 @@ static int pm8607_list_voltage(struct regulator_dev *rdev, unsigned index)
 	struct pm8607_regulator_info *info = rdev_get_drvdata(rdev);
 	int ret = -EINVAL;
 
-	if (info->vol_table && (index < (1 << info->vol_nbits))) {
+	if (info->vol_table && (index < rdev->desc->n_voltages)) {
 		ret = info->vol_table[index];
 		if (info->slope_double)
 			ret <<= 1;
@@ -224,51 +219,16 @@ static int pm8607_list_voltage(struct regulator_dev *rdev, unsigned index)
 	return ret;
 }
 
-static int choose_voltage(struct regulator_dev *rdev, int min_uV, int max_uV)
+static int pm8607_set_voltage_sel(struct regulator_dev *rdev, unsigned selector)
 {
 	struct pm8607_regulator_info *info = rdev_get_drvdata(rdev);
-	int i, ret = -ENOENT;
-
-	if (info->slope_double) {
-		min_uV = min_uV >> 1;
-		max_uV = max_uV >> 1;
-	}
-	if (info->vol_table) {
-		for (i = 0; i < (1 << info->vol_nbits); i++) {
-			if (!info->vol_table[i])
-				break;
-			if ((min_uV <= info->vol_table[i])
-				&& (max_uV >= info->vol_table[i])) {
-				ret = i;
-				break;
-			}
-		}
-	}
-	if (ret < 0)
-		pr_err("invalid voltage range (%d %d) uV\n", min_uV, max_uV);
-	return ret;
-}
-
-static int pm8607_set_voltage(struct regulator_dev *rdev,
-			      int min_uV, int max_uV, unsigned *selector)
-{
-	struct pm8607_regulator_info *info = rdev_get_drvdata(rdev);
-	uint8_t val, mask;
+	uint8_t val;
 	int ret;
 
-	if (min_uV > max_uV) {
-		pr_err("invalid voltage range (%d, %d) uV\n", min_uV, max_uV);
-		return -EINVAL;
-	}
+	val = (uint8_t)(selector << (ffs(rdev->desc->vsel_mask) - 1));
 
-	ret = choose_voltage(rdev, min_uV, max_uV);
-	if (ret < 0)
-		return -EINVAL;
-	*selector = ret;
-	val = (uint8_t)(ret << info->vol_shift);
-	mask = ((1 << info->vol_nbits) - 1)  << info->vol_shift;
-
-	ret = pm860x_set_bits(info->i2c, info->vol_reg, mask, val);
+	ret = pm860x_set_bits(info->i2c, rdev->desc->vsel_reg,
+			      rdev->desc->vsel_mask, val);
 	if (ret)
 		return ret;
 	switch (info->desc.id) {
@@ -282,60 +242,16 @@ static int pm8607_set_voltage(struct regulator_dev *rdev,
 	return ret;
 }
 
-static int pm8607_get_voltage(struct regulator_dev *rdev)
-{
-	struct pm8607_regulator_info *info = rdev_get_drvdata(rdev);
-	uint8_t val, mask;
-	int ret;
-
-	ret = pm860x_reg_read(info->i2c, info->vol_reg);
-	if (ret < 0)
-		return ret;
-
-	mask = ((1 << info->vol_nbits) - 1)  << info->vol_shift;
-	val = ((unsigned char)ret & mask) >> info->vol_shift;
-
-	return pm8607_list_voltage(rdev, val);
-}
-
-static int pm8607_enable(struct regulator_dev *rdev)
-{
-	struct pm8607_regulator_info *info = rdev_get_drvdata(rdev);
-
-	return pm860x_set_bits(info->i2c, info->enable_reg,
-			       1 << info->enable_bit,
-			       1 << info->enable_bit);
-}
-
-static int pm8607_disable(struct regulator_dev *rdev)
-{
-	struct pm8607_regulator_info *info = rdev_get_drvdata(rdev);
-
-	return pm860x_set_bits(info->i2c, info->enable_reg,
-			       1 << info->enable_bit, 0);
-}
-
-static int pm8607_is_enabled(struct regulator_dev *rdev)
-{
-	struct pm8607_regulator_info *info = rdev_get_drvdata(rdev);
-	int ret;
-
-	ret = pm860x_reg_read(info->i2c, info->enable_reg);
-	if (ret < 0)
-		return ret;
-
-	return !!((unsigned char)ret & (1 << info->enable_bit));
-}
-
 static struct regulator_ops pm8607_regulator_ops = {
-	.set_voltage	= pm8607_set_voltage,
-	.get_voltage	= pm8607_get_voltage,
-	.enable		= pm8607_enable,
-	.disable	= pm8607_disable,
-	.is_enabled	= pm8607_is_enabled,
+	.list_voltage	= pm8607_list_voltage,
+	.set_voltage_sel = pm8607_set_voltage_sel,
+	.get_voltage_sel = regulator_get_voltage_sel_regmap,
+	.enable = regulator_enable_regmap,
+	.disable = regulator_disable_regmap,
+	.is_enabled = regulator_is_enabled_regmap,
 };
 
-#define PM8607_DVC(vreg, nbits, ureg, ubit, ereg, ebit)			\
+#define PM8607_DVC(vreg, ureg, ubit, ereg, ebit)			\
 {									\
 	.desc	= {							\
 		.name	= #vreg,					\
@@ -343,20 +259,20 @@ static struct regulator_ops pm8607_regulator_ops = {
 		.type	= REGULATOR_VOLTAGE,				\
 		.id	= PM8607_ID_##vreg,				\
 		.owner	= THIS_MODULE,					\
+		.n_voltages = ARRAY_SIZE(vreg##_table),			\
+		.vsel_reg = PM8607_##vreg,				\
+		.vsel_mask = ARRAY_SIZE(vreg##_table) - 1,		\
+		.enable_reg = PM8607_##ereg,				\
+		.enable_mask = 1 << (ebit),				\
 	},								\
-	.vol_reg	= PM8607_##vreg,				\
-	.vol_shift	= (0),						\
-	.vol_nbits	= (nbits),					\
 	.update_reg	= PM8607_##ureg,				\
 	.update_bit	= (ubit),					\
-	.enable_reg	= PM8607_##ereg,				\
-	.enable_bit	= (ebit),					\
 	.slope_double	= (0),						\
 	.vol_table	= (unsigned int *)&vreg##_table,		\
 	.vol_suspend	= (unsigned int *)&vreg##_suspend_table,	\
 }
 
-#define PM8607_LDO(_id, vreg, shift, nbits, ereg, ebit)			\
+#define PM8607_LDO(_id, vreg, shift, ereg, ebit)			\
 {									\
 	.desc	= {							\
 		.name	= "LDO" #_id,					\
@@ -364,35 +280,35 @@ static struct regulator_ops pm8607_regulator_ops = {
 		.type	= REGULATOR_VOLTAGE,				\
 		.id	= PM8607_ID_LDO##_id,				\
 		.owner	= THIS_MODULE,					\
+		.n_voltages = ARRAY_SIZE(LDO##_id##_table),		\
+		.vsel_reg = PM8607_##vreg,				\
+		.vsel_mask = (ARRAY_SIZE(LDO##_id##_table) - 1) << (shift), \
+		.enable_reg = PM8607_##ereg,				\
+		.enable_mask = 1 << (ebit),				\
 	},								\
-	.vol_reg	= PM8607_##vreg,				\
-	.vol_shift	= (shift),					\
-	.vol_nbits	= (nbits),					\
-	.enable_reg	= PM8607_##ereg,				\
-	.enable_bit	= (ebit),					\
 	.slope_double	= (0),						\
 	.vol_table	= (unsigned int *)&LDO##_id##_table,		\
 	.vol_suspend	= (unsigned int *)&LDO##_id##_suspend_table,	\
 }
 
 static struct pm8607_regulator_info pm8607_regulator_info[] = {
-	PM8607_DVC(BUCK1, 6, GO, 0, SUPPLIES_EN11, 0),
-	PM8607_DVC(BUCK2, 6, GO, 1, SUPPLIES_EN11, 1),
-	PM8607_DVC(BUCK3, 6, GO, 2, SUPPLIES_EN11, 2),
-
-	PM8607_LDO( 1,         LDO1, 0, 2, SUPPLIES_EN11, 3),
-	PM8607_LDO( 2,         LDO2, 0, 3, SUPPLIES_EN11, 4),
-	PM8607_LDO( 3,         LDO3, 0, 3, SUPPLIES_EN11, 5),
-	PM8607_LDO( 4,         LDO4, 0, 3, SUPPLIES_EN11, 6),
-	PM8607_LDO( 5,         LDO5, 0, 2, SUPPLIES_EN11, 7),
-	PM8607_LDO( 6,         LDO6, 0, 3, SUPPLIES_EN12, 0),
-	PM8607_LDO( 7,         LDO7, 0, 3, SUPPLIES_EN12, 1),
-	PM8607_LDO( 8,         LDO8, 0, 3, SUPPLIES_EN12, 2),
-	PM8607_LDO( 9,         LDO9, 0, 3, SUPPLIES_EN12, 3),
-	PM8607_LDO(10,        LDO10, 0, 4, SUPPLIES_EN12, 4),
-	PM8607_LDO(12,        LDO12, 0, 4, SUPPLIES_EN12, 5),
-	PM8607_LDO(13, VIBRATOR_SET, 1, 3,  VIBRATOR_SET, 0),
-	PM8607_LDO(14,        LDO14, 0, 3, SUPPLIES_EN12, 6),
+	PM8607_DVC(BUCK1, GO, 0, SUPPLIES_EN11, 0),
+	PM8607_DVC(BUCK2, GO, 1, SUPPLIES_EN11, 1),
+	PM8607_DVC(BUCK3, GO, 2, SUPPLIES_EN11, 2),
+
+	PM8607_LDO(1,         LDO1, 0, SUPPLIES_EN11, 3),
+	PM8607_LDO(2,         LDO2, 0, SUPPLIES_EN11, 4),
+	PM8607_LDO(3,         LDO3, 0, SUPPLIES_EN11, 5),
+	PM8607_LDO(4,         LDO4, 0, SUPPLIES_EN11, 6),
+	PM8607_LDO(5,         LDO5, 0, SUPPLIES_EN11, 7),
+	PM8607_LDO(6,         LDO6, 0, SUPPLIES_EN12, 0),
+	PM8607_LDO(7,         LDO7, 0, SUPPLIES_EN12, 1),
+	PM8607_LDO(8,         LDO8, 0, SUPPLIES_EN12, 2),
+	PM8607_LDO(9,         LDO9, 0, SUPPLIES_EN12, 3),
+	PM8607_LDO(10,        LDO10, 0, SUPPLIES_EN12, 4),
+	PM8607_LDO(12,        LDO12, 0, SUPPLIES_EN12, 5),
+	PM8607_LDO(13, VIBRATOR_SET, 1, VIBRATOR_SET, 0),
+	PM8607_LDO(14,        LDO14, 0, SUPPLIES_EN12, 6),
 };
 
 static int __devinit pm8607_regulator_probe(struct platform_device *pdev)
@@ -400,6 +316,7 @@ static int __devinit pm8607_regulator_probe(struct platform_device *pdev)
 	struct pm860x_chip *chip = dev_get_drvdata(pdev->dev.parent);
 	struct pm8607_regulator_info *info = NULL;
 	struct regulator_init_data *pdata = pdev->dev.platform_data;
+	struct regulator_config config = { };
 	struct resource *res;
 	int i;
 
@@ -425,9 +342,17 @@ static int __devinit pm8607_regulator_probe(struct platform_device *pdev)
 	if ((i == PM8607_ID_BUCK3) && info->chip->buck3_double)
 		info->slope_double = 1;
 
+	config.dev = &pdev->dev;
+	config.init_data = pdata;
+	config.driver_data = info;
+
+	if (chip->id == CHIP_PM8607)
+		config.regmap = chip->regmap;
+	else
+		config.regmap = chip->regmap_companion;
+
 	/* replace driver_data with info */
-	info->regulator = regulator_register(&info->desc, &pdev->dev,
-					     pdata, info, NULL);
+	info->regulator = regulator_register(&info->desc, &config);
 	if (IS_ERR(info->regulator)) {
 		dev_err(&pdev->dev, "failed to register regulator %s\n",
 			info->desc.name);
diff --git a/drivers/regulator/Kconfig b/drivers/regulator/Kconfig
index 36db5a441eb..c86b8864e41 100644
--- a/drivers/regulator/Kconfig
+++ b/drivers/regulator/Kconfig
@@ -223,6 +223,16 @@ config REGULATOR_PCF50633
 	 Say Y here to support the voltage regulators and convertors
 	 on PCF50633
 
+config REGULATOR_RC5T583
+	tristate "RICOH RC5T583 Power regulators"
+	depends on MFD_RC5T583
+	help
+	  Select this option to enable the power regulator of RICOH
+	  PMIC RC5T583.
+	  This driver supports the control of different power rails of device
+	  through regulator interface. The device supports multiple DCDC/LDO
+	  outputs which can be controlled by i2c communication.
+
 config REGULATOR_S5M8767
 	tristate "Samsung S5M8767A voltage regulator"
 	depends on MFD_S5M_CORE
@@ -258,6 +268,18 @@ config REGULATOR_DB8500_PRCMU
 	  This driver supports the voltage domain regulators controlled by the
 	  DB8500 PRCMU
 
+config REGULATOR_PALMAS
+	tristate "TI Palmas PMIC Regulators"
+	depends on MFD_PALMAS
+	help
+	  If you wish to control the regulators on the Palmas series of
+	  chips say Y here. This will enable support for all the software
+	  controllable SMPS/LDO regulators.
+
+	  The regulators available on Palmas series chips vary depending
+	  on the muxing. This is handled automatically in the driver by
+	  reading the mux info from OTP.
+
 config REGULATOR_TPS6105X
 	tristate "TI TPS6105X Power regulators"
 	depends on TPS6105X
@@ -268,11 +290,11 @@ config REGULATOR_TPS6105X
 	  audio amplifiers.
 
 config REGULATOR_TPS62360
-	tristate "TI TPS62360 Power Regulator"
+	tristate "TI TPS6236x Power Regulator"
 	depends on I2C
 	select REGMAP_I2C
 	help
-	  This driver supports TPS62360 voltage regulator chip. This
+	  This driver supports TPS6236x voltage regulator chip. This
 	  regulator is meant for processor core supply. This chip is
 	  high-frequency synchronous step down dc-dc converter optimized
 	  for battery-powered portable applications.
@@ -294,6 +316,13 @@ config REGULATOR_TPS6507X
 	  three step-down converters and two general-purpose LDO voltage regulators.
 	  It supports TI's software based Class-2 SmartReflex implementation.
 
+config REGULATOR_TPS65090
+	tristate "TI TPS65090 Power regulator"
+	depends on MFD_TPS65090
+	help
+	  This driver provides support for the voltage regulators on the
+	  TI TPS65090 PMIC.
+
 config REGULATOR_TPS65217
 	tristate "TI TPS65217 Power regulators"
 	depends on MFD_TPS65217
diff --git a/drivers/regulator/Makefile b/drivers/regulator/Makefile
index 94b52745e95..977fd46909a 100644
--- a/drivers/regulator/Makefile
+++ b/drivers/regulator/Makefile
@@ -9,7 +9,6 @@ obj-$(CONFIG_REGULATOR_FIXED_VOLTAGE) += fixed.o
 obj-$(CONFIG_REGULATOR_VIRTUAL_CONSUMER) += virtual.o
 obj-$(CONFIG_REGULATOR_USERSPACE_CONSUMER) += userspace-consumer.o
 
-obj-$(CONFIG_REGULATOR_GPIO) += gpio-regulator.o
 obj-$(CONFIG_REGULATOR_88PM8607) += 88pm8607.o
 obj-$(CONFIG_REGULATOR_AAT2870) += aat2870-regulator.o
 obj-$(CONFIG_REGULATOR_AB3100) += ab3100.o
@@ -20,6 +19,7 @@ obj-$(CONFIG_REGULATOR_DA903X)	+= da903x.o
 obj-$(CONFIG_REGULATOR_DA9052)	+= da9052-regulator.o
 obj-$(CONFIG_REGULATOR_DBX500_PRCMU) += dbx500-prcmu.o
 obj-$(CONFIG_REGULATOR_DB8500_PRCMU) += db8500-prcmu.o
+obj-$(CONFIG_REGULATOR_GPIO) += gpio-regulator.o
 obj-$(CONFIG_REGULATOR_ISL6271A) += isl6271a-regulator.o
 obj-$(CONFIG_REGULATOR_LP3971) += lp3971.o
 obj-$(CONFIG_REGULATOR_LP3972) += lp3972.o
@@ -33,13 +33,16 @@ obj-$(CONFIG_REGULATOR_MAX8998) += max8998.o
 obj-$(CONFIG_REGULATOR_MC13783) += mc13783-regulator.o
 obj-$(CONFIG_REGULATOR_MC13892) += mc13892-regulator.o
 obj-$(CONFIG_REGULATOR_MC13XXX_CORE) +=  mc13xxx-regulator-core.o
+obj-$(CONFIG_REGULATOR_PALMAS) += palmas-regulator.o
 obj-$(CONFIG_REGULATOR_PCAP) += pcap-regulator.o
 obj-$(CONFIG_REGULATOR_PCF50633) += pcf50633-regulator.o
+obj-$(CONFIG_REGULATOR_RC5T583)  += rc5t583-regulator.o
 obj-$(CONFIG_REGULATOR_S5M8767) += s5m8767.o
 obj-$(CONFIG_REGULATOR_TPS6105X) += tps6105x-regulator.o
 obj-$(CONFIG_REGULATOR_TPS62360) += tps62360-regulator.o
 obj-$(CONFIG_REGULATOR_TPS65023) += tps65023-regulator.o
 obj-$(CONFIG_REGULATOR_TPS6507X) += tps6507x-regulator.o
+obj-$(CONFIG_REGULATOR_TPS65090) += tps65090-regulator.o
 obj-$(CONFIG_REGULATOR_TPS65217) += tps65217-regulator.o
 obj-$(CONFIG_REGULATOR_TPS6524X) += tps6524x-regulator.o
 obj-$(CONFIG_REGULATOR_TPS6586X) += tps6586x-regulator.o
diff --git a/drivers/regulator/aat2870-regulator.c b/drivers/regulator/aat2870-regulator.c
index 9ed5c5d84e1..06776ca945f 100644
--- a/drivers/regulator/aat2870-regulator.c
+++ b/drivers/regulator/aat2870-regulator.c
@@ -24,7 +24,6 @@
 #include <linux/err.h>
 #include <linux/module.h>
 #include <linux/slab.h>
-#include <linux/delay.h>
 #include <linux/platform_device.h>
 #include <linux/regulator/driver.h>
 #include <linux/regulator/machine.h>
@@ -178,6 +177,7 @@ static struct aat2870_regulator *aat2870_get_regulator(int id)
 static int aat2870_regulator_probe(struct platform_device *pdev)
 {
 	struct aat2870_regulator *ri;
+	struct regulator_config config = { 0 };
 	struct regulator_dev *rdev;
 
 	ri = aat2870_get_regulator(pdev->id);
@@ -187,8 +187,11 @@ static int aat2870_regulator_probe(struct platform_device *pdev)
 	}
 	ri->aat2870 = dev_get_drvdata(pdev->dev.parent);
 
-	rdev = regulator_register(&ri->desc, &pdev->dev,
-				  pdev->dev.platform_data, ri, NULL);
+	config.dev = &pdev->dev;
+	config.driver_data = ri;
+	config.init_data = pdev->dev.platform_data;
+
+	rdev = regulator_register(&ri->desc, &config);
 	if (IS_ERR(rdev)) {
 		dev_err(&pdev->dev, "Failed to register regulator %s\n",
 			ri->desc.name);
@@ -231,3 +234,4 @@ module_exit(aat2870_regulator_exit);
 MODULE_DESCRIPTION("AnalogicTech AAT2870 Regulator");
 MODULE_LICENSE("GPL");
 MODULE_AUTHOR("Jin Park <jinyoungp@nvidia.com>");
+MODULE_ALIAS("platform:aat2870-regulator");
diff --git a/drivers/regulator/ab3100.c b/drivers/regulator/ab3100.c
index 042271aace6..03f4d9c604e 100644
--- a/drivers/regulator/ab3100.c
+++ b/drivers/regulator/ab3100.c
@@ -13,7 +13,6 @@
 #include <linux/kernel.h>
 #include <linux/init.h>
 #include <linux/err.h>
-#include <linux/delay.h>
 #include <linux/platform_device.h>
 #include <linux/regulator/driver.h>
 #include <linux/mfd/abx500.h>
@@ -305,53 +304,12 @@ static int ab3100_get_voltage_regulator(struct regulator_dev *reg)
 	return abreg->typ_voltages[regval];
 }
 
-static int ab3100_get_best_voltage_index(struct regulator_dev *reg,
-				   int min_uV, int max_uV)
-{
-	struct ab3100_regulator *abreg = reg->reg_data;
-	int i;
-	int bestmatch;
-	int bestindex;
-
-	/*
-	 * Locate the minimum voltage fitting the criteria on
-	 * this regulator. The switchable voltages are not
-	 * in strict falling order so we need to check them
-	 * all for the best match.
-	 */
-	bestmatch = INT_MAX;
-	bestindex = -1;
-	for (i = 0; i < abreg->voltages_len; i++) {
-		if (abreg->typ_voltages[i] <= max_uV &&
-		    abreg->typ_voltages[i] >= min_uV &&
-		    abreg->typ_voltages[i] < bestmatch) {
-			bestmatch = abreg->typ_voltages[i];
-			bestindex = i;
-		}
-	}
-
-	if (bestindex < 0) {
-		dev_warn(&reg->dev, "requested %d<=x<=%d uV, out of range!\n",
-			 min_uV, max_uV);
-		return -EINVAL;
-	}
-	return bestindex;
-}
-
-static int ab3100_set_voltage_regulator(struct regulator_dev *reg,
-					int min_uV, int max_uV,
-					unsigned *selector)
+static int ab3100_set_voltage_regulator_sel(struct regulator_dev *reg,
+					    unsigned selector)
 {
 	struct ab3100_regulator *abreg = reg->reg_data;
 	u8 regval;
 	int err;
-	int bestindex;
-
-	bestindex = ab3100_get_best_voltage_index(reg, min_uV, max_uV);
-	if (bestindex < 0)
-		return bestindex;
-
-	*selector = bestindex;
 
 	err = abx500_get_register_interruptible(abreg->dev, 0,
 						abreg->regreg, &regval);
@@ -364,7 +322,7 @@ static int ab3100_set_voltage_regulator(struct regulator_dev *reg,
 
 	/* The highest three bits control the variable regulators */
 	regval &= ~0xE0;
-	regval |= (bestindex << 5);
+	regval |= (selector << 5);
 
 	err = abx500_set_register_interruptible(abreg->dev, 0,
 						abreg->regreg, regval);
@@ -392,7 +350,7 @@ static int ab3100_set_suspend_voltage_regulator(struct regulator_dev *reg,
 		return -EINVAL;
 
 	/* LDO E and BUCK have special suspend voltages you can set */
-	bestindex = ab3100_get_best_voltage_index(reg, uV, uV);
+	bestindex = regulator_map_voltage_iterate(reg, uV, uV);
 
 	err = abx500_get_register_interruptible(abreg->dev, 0,
 						targetreg, &regval);
@@ -464,7 +422,7 @@ static struct regulator_ops regulator_ops_variable = {
 	.disable     = ab3100_disable_regulator,
 	.is_enabled  = ab3100_is_enabled_regulator,
 	.get_voltage = ab3100_get_voltage_regulator,
-	.set_voltage = ab3100_set_voltage_regulator,
+	.set_voltage_sel = ab3100_set_voltage_regulator_sel,
 	.list_voltage = ab3100_list_voltage_regulator,
 	.enable_time = ab3100_enable_time_regulator,
 };
@@ -474,7 +432,7 @@ static struct regulator_ops regulator_ops_variable_sleepable = {
 	.disable     = ab3100_disable_regulator,
 	.is_enabled  = ab3100_is_enabled_regulator,
 	.get_voltage = ab3100_get_voltage_regulator,
-	.set_voltage = ab3100_set_voltage_regulator,
+	.set_voltage_sel = ab3100_set_voltage_regulator_sel,
 	.set_suspend_voltage = ab3100_set_suspend_voltage_regulator,
 	.list_voltage = ab3100_list_voltage_regulator,
 	.enable_time = ab3100_enable_time_regulator,
@@ -582,6 +540,7 @@ ab3100_regulator_desc[AB3100_NUM_REGULATORS] = {
 static int __devinit ab3100_regulators_probe(struct platform_device *pdev)
 {
 	struct ab3100_platform_data *plfdata = pdev->dev.platform_data;
+	struct regulator_config config = { };
 	int err = 0;
 	u8 data;
 	int i;
@@ -627,15 +586,15 @@ static int __devinit ab3100_regulators_probe(struct platform_device *pdev)
 		reg->dev = &pdev->dev;
 		reg->plfdata = plfdata;
 
+		config.dev = &pdev->dev;
+		config.driver_data = reg;
+		config.init_data = &plfdata->reg_constraints[i];
+
 		/*
 		 * Register the regulator, pass around
 		 * the ab3100_regulator struct
 		 */
-		rdev = regulator_register(&ab3100_regulator_desc[i],
-					  &pdev->dev,
-					  &plfdata->reg_constraints[i],
-					  reg, NULL);
-
+		rdev = regulator_register(&ab3100_regulator_desc[i], &config);
 		if (IS_ERR(rdev)) {
 			err = PTR_ERR(rdev);
 			dev_err(&pdev->dev,
diff --git a/drivers/regulator/ab8500.c b/drivers/regulator/ab8500.c
index c7ee4c15d6f..e1b8c54ace5 100644
--- a/drivers/regulator/ab8500.c
+++ b/drivers/regulator/ab8500.c
@@ -18,9 +18,12 @@
 #include <linux/platform_device.h>
 #include <linux/mfd/abx500.h>
 #include <linux/mfd/abx500/ab8500.h>
+#include <linux/of.h>
+#include <linux/regulator/of_regulator.h>
 #include <linux/regulator/driver.h>
 #include <linux/regulator/machine.h>
 #include <linux/regulator/ab8500.h>
+#include <linux/slab.h>
 
 /**
  * struct ab8500_regulator_info - ab8500 regulator information
@@ -234,25 +237,8 @@ static int ab8500_regulator_get_voltage_sel(struct regulator_dev *rdev)
 		return val;
 }
 
-static int ab8500_get_best_voltage_index(struct regulator_dev *rdev,
-		int min_uV, int max_uV)
-{
-	struct ab8500_regulator_info *info = rdev_get_drvdata(rdev);
-	int i;
-
-	/* check the supported voltage */
-	for (i = 0; i < info->voltages_len; i++) {
-		if ((info->voltages[i] >= min_uV) &&
-		    (info->voltages[i] <= max_uV))
-			return i;
-	}
-
-	return -EINVAL;
-}
-
-static int ab8500_regulator_set_voltage(struct regulator_dev *rdev,
-					int min_uV, int max_uV,
-					unsigned *selector)
+static int ab8500_regulator_set_voltage_sel(struct regulator_dev *rdev,
+					    unsigned selector)
 {
 	int ret;
 	struct ab8500_regulator_info *info = rdev_get_drvdata(rdev);
@@ -263,18 +249,8 @@ static int ab8500_regulator_set_voltage(struct regulator_dev *rdev,
 		return -EINVAL;
 	}
 
-	/* get the appropriate voltages within the range */
-	ret = ab8500_get_best_voltage_index(rdev, min_uV, max_uV);
-	if (ret < 0) {
-		dev_err(rdev_get_dev(rdev),
-				"couldn't get best voltage for regulator\n");
-		return ret;
-	}
-
-	*selector = ret;
-
 	/* set the registers for the request */
-	regval = (u8)ret;
+	regval = (u8)selector;
 	ret = abx500_mask_and_set_register_interruptible(info->dev,
 			info->voltage_bank, info->voltage_reg,
 			info->voltage_mask, regval);
@@ -319,7 +295,7 @@ static struct regulator_ops ab8500_regulator_ops = {
 	.disable	= ab8500_regulator_disable,
 	.is_enabled	= ab8500_regulator_is_enabled,
 	.get_voltage_sel = ab8500_regulator_get_voltage_sel,
-	.set_voltage	= ab8500_regulator_set_voltage,
+	.set_voltage_sel = ab8500_regulator_set_voltage_sel,
 	.list_voltage	= ab8500_list_voltage,
 	.enable_time	= ab8500_regulator_enable_time,
 	.set_voltage_time_sel = ab8500_regulator_set_voltage_time_sel,
@@ -735,12 +711,139 @@ static struct ab8500_reg_init ab8500_reg_init[] = {
 	REG_INIT(AB8500_REGUCTRLDISCH2,		0x04, 0x44, 0x16),
 };
 
+static __devinit int
+ab8500_regulator_init_registers(struct platform_device *pdev, int id, int value)
+{
+	int err;
+
+	if (value & ~ab8500_reg_init[id].mask) {
+		dev_err(&pdev->dev,
+			"Configuration error: value outside mask.\n");
+		return -EINVAL;
+	}
+
+	err = abx500_mask_and_set_register_interruptible(
+		&pdev->dev,
+		ab8500_reg_init[id].bank,
+		ab8500_reg_init[id].addr,
+		ab8500_reg_init[id].mask,
+		value);
+	if (err < 0) {
+		dev_err(&pdev->dev,
+			"Failed to initialize 0x%02x, 0x%02x.\n",
+			ab8500_reg_init[id].bank,
+			ab8500_reg_init[id].addr);
+		return err;
+	}
+
+	dev_vdbg(&pdev->dev,
+		"init: 0x%02x, 0x%02x, 0x%02x, 0x%02x\n",
+		ab8500_reg_init[id].bank,
+		ab8500_reg_init[id].addr,
+		ab8500_reg_init[id].mask,
+		value);
+
+	return 0;
+}
+
+static __devinit int ab8500_regulator_register(struct platform_device *pdev,
+					struct regulator_init_data *init_data,
+					int id,
+					struct device_node *np)
+{
+	struct ab8500_regulator_info *info = NULL;
+	struct regulator_config config = { };
+	int err;
+
+	/* assign per-regulator data */
+	info = &ab8500_regulator_info[id];
+	info->dev = &pdev->dev;
+
+	config.dev = &pdev->dev;
+	config.init_data = init_data;
+	config.driver_data = info;
+	config.of_node = np;
+
+	/* fix for hardware before ab8500v2.0 */
+	if (abx500_get_chip_id(info->dev) < 0x20) {
+		if (info->desc.id == AB8500_LDO_AUX3) {
+			info->desc.n_voltages =
+				ARRAY_SIZE(ldo_vauxn_voltages);
+			info->voltages = ldo_vauxn_voltages;
+			info->voltages_len =
+				ARRAY_SIZE(ldo_vauxn_voltages);
+			info->voltage_mask = 0xf;
+		}
+	}
+
+	/* register regulator with framework */
+	info->regulator = regulator_register(&info->desc, &config);
+	if (IS_ERR(info->regulator)) {
+		err = PTR_ERR(info->regulator);
+		dev_err(&pdev->dev, "failed to register regulator %s\n",
+			info->desc.name);
+		/* when we fail, un-register all earlier regulators */
+		while (--id >= 0) {
+			info = &ab8500_regulator_info[id];
+			regulator_unregister(info->regulator);
+		}
+		return err;
+	}
+
+	return 0;
+}
+
+static struct of_regulator_match ab8500_regulator_matches[] = {
+	{ .name	= "LDO-AUX1",    .driver_data = (void *) AB8500_LDO_AUX1, },
+	{ .name	= "LDO-AUX2",    .driver_data = (void *) AB8500_LDO_AUX2, },
+	{ .name	= "LDO-AUX3",    .driver_data = (void *) AB8500_LDO_AUX3, },
+	{ .name	= "LDO-INTCORE", .driver_data = (void *) AB8500_LDO_INTCORE, },
+	{ .name	= "LDO-TVOUT",   .driver_data = (void *) AB8500_LDO_TVOUT, },
+	{ .name = "LDO-USB",     .driver_data = (void *) AB8500_LDO_USB, },
+	{ .name = "LDO-AUDIO",   .driver_data = (void *) AB8500_LDO_AUDIO, },
+	{ .name	= "LDO-ANAMIC1", .driver_data = (void *) AB8500_LDO_ANAMIC1, },
+	{ .name	= "LDO-ANAMIC2", .driver_data = (void *) AB8500_LDO_ANAMIC2, },
+	{ .name	= "LDO-DMIC",    .driver_data = (void *) AB8500_LDO_DMIC, },
+	{ .name	= "LDO-ANA",     .driver_data = (void *) AB8500_LDO_ANA, },
+};
+
+static __devinit int
+ab8500_regulator_of_probe(struct platform_device *pdev, struct device_node *np)
+{
+	int err, i;
+
+	for (i = 0; i < ARRAY_SIZE(ab8500_regulator_info); i++) {
+		err = ab8500_regulator_register(
+			pdev, ab8500_regulator_matches[i].init_data,
+			i, ab8500_regulator_matches[i].of_node);
+		if (err)
+			return err;
+	}
+
+	return 0;
+}
+
 static __devinit int ab8500_regulator_probe(struct platform_device *pdev)
 {
 	struct ab8500 *ab8500 = dev_get_drvdata(pdev->dev.parent);
 	struct ab8500_platform_data *pdata;
+	struct device_node *np = pdev->dev.of_node;
 	int i, err;
 
+	if (np) {
+		err = of_regulator_match(&pdev->dev, np,
+					ab8500_regulator_matches,
+					ARRAY_SIZE(ab8500_regulator_matches));
+		if (err < 0) {
+			dev_err(&pdev->dev,
+				"Error parsing regulator init data: %d\n", err);
+			return err;
+		}
+
+		err = ab8500_regulator_of_probe(pdev, np);
+		return err;
+	}
+
 	if (!ab8500) {
 		dev_err(&pdev->dev, "null mfd parent\n");
 		return -EINVAL;
@@ -759,8 +862,7 @@ static __devinit int ab8500_regulator_probe(struct platform_device *pdev)
 
 	/* initialize registers */
 	for (i = 0; i < pdata->num_regulator_reg_init; i++) {
-		int id;
-		u8 value;
+		int id, value;
 
 		id = pdata->regulator_reg_init[i].id;
 		value = pdata->regulator_reg_init[i].value;
@@ -771,70 +873,17 @@ static __devinit int ab8500_regulator_probe(struct platform_device *pdev)
 				"Configuration error: id outside range.\n");
 			return -EINVAL;
 		}
-		if (value & ~ab8500_reg_init[id].mask) {
-			dev_err(&pdev->dev,
-				"Configuration error: value outside mask.\n");
-			return -EINVAL;
-		}
 
-		/* initialize register */
-		err = abx500_mask_and_set_register_interruptible(&pdev->dev,
-			ab8500_reg_init[id].bank,
-			ab8500_reg_init[id].addr,
-			ab8500_reg_init[id].mask,
-			value);
-		if (err < 0) {
-			dev_err(&pdev->dev,
-				"Failed to initialize 0x%02x, 0x%02x.\n",
-				ab8500_reg_init[id].bank,
-				ab8500_reg_init[id].addr);
+		err = ab8500_regulator_init_registers(pdev, id, value);
+		if (err < 0)
 			return err;
-		}
-		dev_vdbg(&pdev->dev,
-			"  init: 0x%02x, 0x%02x, 0x%02x, 0x%02x\n",
-			ab8500_reg_init[id].bank,
-			ab8500_reg_init[id].addr,
-			ab8500_reg_init[id].mask,
-			value);
 	}
 
 	/* register all regulators */
 	for (i = 0; i < ARRAY_SIZE(ab8500_regulator_info); i++) {
-		struct ab8500_regulator_info *info = NULL;
-
-		/* assign per-regulator data */
-		info = &ab8500_regulator_info[i];
-		info->dev = &pdev->dev;
-
-		/* fix for hardware before ab8500v2.0 */
-		if (abx500_get_chip_id(info->dev) < 0x20) {
-			if (info->desc.id == AB8500_LDO_AUX3) {
-				info->desc.n_voltages =
-					ARRAY_SIZE(ldo_vauxn_voltages);
-				info->voltages = ldo_vauxn_voltages;
-				info->voltages_len =
-					ARRAY_SIZE(ldo_vauxn_voltages);
-				info->voltage_mask = 0xf;
-			}
-		}
-
-		/* register regulator with framework */
-		info->regulator = regulator_register(&info->desc, &pdev->dev,
-				&pdata->regulator[i], info, NULL);
-		if (IS_ERR(info->regulator)) {
-			err = PTR_ERR(info->regulator);
-			dev_err(&pdev->dev, "failed to register regulator %s\n",
-					info->desc.name);
-			/* when we fail, un-register all earlier regulators */
-			while (--i >= 0) {
-				info = &ab8500_regulator_info[i];
-				regulator_unregister(info->regulator);
-			}
+		err = ab8500_regulator_register(pdev, &pdata->regulator[i], i, NULL);
+		if (err < 0)
 			return err;
-		}
-
-		dev_vdbg(rdev_get_dev(info->regulator),
-			"%s-probed\n", info->desc.name);
 	}
 
 	return 0;
@@ -857,12 +906,18 @@ static __devexit int ab8500_regulator_remove(struct platform_device *pdev)
 	return 0;
 }
 
+static const struct of_device_id ab8500_regulator_match[] = {
+        { .compatible = "stericsson,ab8500-regulator", },
+        {}
+};
+
 static struct platform_driver ab8500_regulator_driver = {
 	.probe = ab8500_regulator_probe,
 	.remove = __devexit_p(ab8500_regulator_remove),
 	.driver         = {
 		.name   = "ab8500-regulator",
 		.owner  = THIS_MODULE,
+		.of_match_table = ab8500_regulator_match,
 	},
 };
 
diff --git a/drivers/regulator/ad5398.c b/drivers/regulator/ad5398.c
index 26d23adfc66..46d05f38baf 100644
--- a/drivers/regulator/ad5398.c
+++ b/drivers/regulator/ad5398.c
@@ -99,8 +99,8 @@ static int ad5398_set_current_limit(struct regulator_dev *rdev, int min_uA, int
 	if (ad5398_calc_current(chip, selector) > max_uA)
 		return -EINVAL;
 
-	dev_dbg(&client->dev, "changing current %dmA\n",
-		ad5398_calc_current(chip, selector) / 1000);
+	dev_dbg(&client->dev, "changing current %duA\n",
+		ad5398_calc_current(chip, selector));
 
 	/* read chip enable bit */
 	ret = ad5398_read_reg(client, &data);
@@ -184,7 +184,7 @@ static struct regulator_ops ad5398_ops = {
 	.is_enabled = ad5398_is_enabled,
 };
 
-static struct regulator_desc ad5398_reg = {
+static const struct regulator_desc ad5398_reg = {
 	.name = "isink",
 	.id = 0,
 	.ops = &ad5398_ops,
@@ -212,6 +212,7 @@ static int __devinit ad5398_probe(struct i2c_client *client,
 				const struct i2c_device_id *id)
 {
 	struct regulator_init_data *init_data = client->dev.platform_data;
+	struct regulator_config config = { };
 	struct ad5398_chip_info *chip;
 	const struct ad5398_current_data_format *df =
 			(struct ad5398_current_data_format *)id->driver_data;
@@ -220,10 +221,14 @@ static int __devinit ad5398_probe(struct i2c_client *client,
 	if (!init_data)
 		return -EINVAL;
 
-	chip = kzalloc(sizeof(*chip), GFP_KERNEL);
+	chip = devm_kzalloc(&client->dev, sizeof(*chip), GFP_KERNEL);
 	if (!chip)
 		return -ENOMEM;
 
+	config.dev = &client->dev;
+	config.init_data = init_data;
+	config.driver_data = chip;
+
 	chip->client = client;
 
 	chip->min_uA = df->min_uA;
@@ -232,8 +237,7 @@ static int __devinit ad5398_probe(struct i2c_client *client,
 	chip->current_offset = df->current_offset;
 	chip->current_mask = (chip->current_level - 1) << chip->current_offset;
 
-	chip->rdev = regulator_register(&ad5398_reg, &client->dev,
-					init_data, chip, NULL);
+	chip->rdev = regulator_register(&ad5398_reg, &config);
 	if (IS_ERR(chip->rdev)) {
 		ret = PTR_ERR(chip->rdev);
 		dev_err(&client->dev, "failed to register %s %s\n",
@@ -246,7 +250,6 @@ static int __devinit ad5398_probe(struct i2c_client *client,
 	return 0;
 
 err:
-	kfree(chip);
 	return ret;
 }
 
@@ -255,8 +258,6 @@ static int __devexit ad5398_remove(struct i2c_client *client)
 	struct ad5398_chip_info *chip = i2c_get_clientdata(client);
 
 	regulator_unregister(chip->rdev);
-	kfree(chip);
-
 	return 0;
 }
 
diff --git a/drivers/regulator/anatop-regulator.c b/drivers/regulator/anatop-regulator.c
index 81fd606e47b..3660bace123 100644
--- a/drivers/regulator/anatop-regulator.c
+++ b/drivers/regulator/anatop-regulator.c
@@ -47,7 +47,7 @@ static int anatop_set_voltage(struct regulator_dev *reg, int min_uV,
 				  int max_uV, unsigned *selector)
 {
 	struct anatop_regulator *anatop_reg = rdev_get_drvdata(reg);
-	u32 val, sel;
+	u32 val, sel, mask;
 	int uv;
 
 	uv = min_uV;
@@ -71,11 +71,10 @@ static int anatop_set_voltage(struct regulator_dev *reg, int min_uV,
 	val = anatop_reg->min_bit_val + sel;
 	*selector = sel;
 	dev_dbg(&reg->dev, "%s: calculated val %d\n", __func__, val);
-	anatop_set_bits(anatop_reg->mfd,
-			anatop_reg->control_reg,
-			anatop_reg->vol_bit_shift,
-			anatop_reg->vol_bit_width,
-			val);
+	mask = ((1 << anatop_reg->vol_bit_width) - 1) <<
+		anatop_reg->vol_bit_shift;
+	val <<= anatop_reg->vol_bit_shift;
+	anatop_write_reg(anatop_reg->mfd, anatop_reg->control_reg, val, mask);
 
 	return 0;
 }
@@ -88,10 +87,9 @@ static int anatop_get_voltage_sel(struct regulator_dev *reg)
 	if (!anatop_reg->control_reg)
 		return -ENOTSUPP;
 
-	val = anatop_get_bits(anatop_reg->mfd,
-			      anatop_reg->control_reg,
-			      anatop_reg->vol_bit_shift,
-			      anatop_reg->vol_bit_width);
+	val = anatop_read_reg(anatop_reg->mfd, anatop_reg->control_reg);
+	val = (val & ((1 << anatop_reg->vol_bit_width) - 1)) >>
+		anatop_reg->vol_bit_shift;
 
 	return val - anatop_reg->min_bit_val;
 }
@@ -122,6 +120,7 @@ static int __devinit anatop_regulator_probe(struct platform_device *pdev)
 	struct anatop_regulator *sreg;
 	struct regulator_init_data *initdata;
 	struct anatop *anatopmfd = dev_get_drvdata(pdev->dev.parent);
+	struct regulator_config config = { };
 	int ret = 0;
 
 	initdata = of_get_regulator_init_data(dev, np);
@@ -178,9 +177,13 @@ static int __devinit anatop_regulator_probe(struct platform_device *pdev)
 	rdesc->n_voltages = (sreg->max_voltage - sreg->min_voltage)
 		/ 25000 + 1;
 
+	config.dev = &pdev->dev;
+	config.init_data = initdata;
+	config.driver_data = sreg;
+	config.of_node = pdev->dev.of_node;
+
 	/* register regulator */
-	rdev = regulator_register(rdesc, dev,
-				  initdata, sreg, pdev->dev.of_node);
+	rdev = regulator_register(rdesc, &config);
 	if (IS_ERR(rdev)) {
 		dev_err(dev, "failed to register %s\n",
 			rdesc->name);
diff --git a/drivers/regulator/core.c b/drivers/regulator/core.c
index e70dd382a00..7584a74eec8 100644
--- a/drivers/regulator/core.c
+++ b/drivers/regulator/core.c
@@ -24,6 +24,7 @@
 #include <linux/suspend.h>
 #include <linux/delay.h>
 #include <linux/of.h>
+#include <linux/regmap.h>
 #include <linux/regulator/of_regulator.h>
 #include <linux/regulator/consumer.h>
 #include <linux/regulator/driver.h>
@@ -74,6 +75,7 @@ struct regulator_map {
 struct regulator {
 	struct device *dev;
 	struct list_head list;
+	unsigned int always_on:1;
 	int uA_load;
 	int min_uV;
 	int max_uV;
@@ -155,6 +157,17 @@ static struct device_node *of_get_regulator(struct device *dev, const char *supp
 	return regnode;
 }
 
+static int _regulator_can_change_status(struct regulator_dev *rdev)
+{
+	if (!rdev->constraints)
+		return 0;
+
+	if (rdev->constraints->valid_ops_mask & REGULATOR_CHANGE_STATUS)
+		return 1;
+	else
+		return 0;
+}
+
 /* Platform voltage constraint check */
 static int regulator_check_voltage(struct regulator_dev *rdev,
 				   int *min_uV, int *max_uV)
@@ -649,7 +662,7 @@ static void drms_uA_update(struct regulator_dev *rdev)
 	/* get input voltage */
 	input_uV = 0;
 	if (rdev->supply)
-		input_uV = _regulator_get_voltage(rdev);
+		input_uV = regulator_get_voltage(rdev->supply);
 	if (input_uV <= 0)
 		input_uV = rdev->constraints->input_uV;
 	if (input_uV <= 0)
@@ -673,17 +686,14 @@ static int suspend_set_state(struct regulator_dev *rdev,
 	struct regulator_state *rstate)
 {
 	int ret = 0;
-	bool can_set_state;
-
-	can_set_state = rdev->desc->ops->set_suspend_enable &&
-		rdev->desc->ops->set_suspend_disable;
 
 	/* If we have no suspend mode configration don't set anything;
-	 * only warn if the driver actually makes the suspend mode
-	 * configurable.
+	 * only warn if the driver implements set_suspend_voltage or
+	 * set_suspend_mode callback.
 	 */
 	if (!rstate->enabled && !rstate->disabled) {
-		if (can_set_state)
+		if (rdev->desc->ops->set_suspend_voltage ||
+		    rdev->desc->ops->set_suspend_mode)
 			rdev_warn(rdev, "No configuration\n");
 		return 0;
 	}
@@ -693,15 +703,13 @@ static int suspend_set_state(struct regulator_dev *rdev,
 		return -EINVAL;
 	}
 
-	if (!can_set_state) {
-		rdev_err(rdev, "no way to set suspend state\n");
-		return -EINVAL;
-	}
-
-	if (rstate->enabled)
+	if (rstate->enabled && rdev->desc->ops->set_suspend_enable)
 		ret = rdev->desc->ops->set_suspend_enable(rdev);
-	else
+	else if (rstate->disabled && rdev->desc->ops->set_suspend_disable)
 		ret = rdev->desc->ops->set_suspend_disable(rdev);
+	else /* OK if set_suspend_enable or set_suspend_disable is NULL */
+		ret = 0;
+
 	if (ret < 0) {
 		rdev_err(rdev, "failed to enabled/disable\n");
 		return ret;
@@ -1146,6 +1154,15 @@ static struct regulator *create_regulator(struct regulator_dev *rdev,
 				   &regulator->max_uV);
 	}
 
+	/*
+	 * Check now if the regulator is an always on regulator - if
+	 * it is then we don't need to do nearly so much work for
+	 * enable/disable calls.
+	 */
+	if (!_regulator_can_change_status(rdev) &&
+	    _regulator_is_enabled(rdev))
+		regulator->always_on = true;
+
 	mutex_unlock(&rdev->mutex);
 	return regulator;
 link_name_err:
@@ -1169,26 +1186,52 @@ static int _regulator_get_enable_time(struct regulator_dev *rdev)
 }
 
 static struct regulator_dev *regulator_dev_lookup(struct device *dev,
-							 const char *supply)
+						  const char *supply,
+						  int *ret)
 {
 	struct regulator_dev *r;
 	struct device_node *node;
+	struct regulator_map *map;
+	const char *devname = NULL;
 
 	/* first do a dt based lookup */
 	if (dev && dev->of_node) {
 		node = of_get_regulator(dev, supply);
-		if (node)
+		if (node) {
 			list_for_each_entry(r, &regulator_list, list)
 				if (r->dev.parent &&
 					node == r->dev.of_node)
 					return r;
+		} else {
+			/*
+			 * If we couldn't even get the node then it's
+			 * not just that the device didn't register
+			 * yet, there's no node and we'll never
+			 * succeed.
+			 */
+			*ret = -ENODEV;
+		}
 	}
 
 	/* if not found, try doing it non-dt way */
+	if (dev)
+		devname = dev_name(dev);
+
 	list_for_each_entry(r, &regulator_list, list)
 		if (strcmp(rdev_get_name(r), supply) == 0)
 			return r;
 
+	list_for_each_entry(map, &regulator_map_list, list) {
+		/* If the mapping has a device set up it must match */
+		if (map->dev_name &&
+		    (!devname || strcmp(map->dev_name, devname)))
+			continue;
+
+		if (strcmp(map->supply, supply) == 0)
+			return map->regulator;
+	}
+
+
 	return NULL;
 }
 
@@ -1197,7 +1240,6 @@ static struct regulator *_regulator_get(struct device *dev, const char *id,
 					int exclusive)
 {
 	struct regulator_dev *rdev;
-	struct regulator_map *map;
 	struct regulator *regulator = ERR_PTR(-EPROBE_DEFER);
 	const char *devname = NULL;
 	int ret;
@@ -1212,22 +1254,10 @@ static struct regulator *_regulator_get(struct device *dev, const char *id,
 
 	mutex_lock(&regulator_list_mutex);
 
-	rdev = regulator_dev_lookup(dev, id);
+	rdev = regulator_dev_lookup(dev, id, &ret);
 	if (rdev)
 		goto found;
 
-	list_for_each_entry(map, &regulator_map_list, list) {
-		/* If the mapping has a device set up it must match */
-		if (map->dev_name &&
-		    (!devname || strcmp(map->dev_name, devname)))
-			continue;
-
-		if (strcmp(map->supply, id) == 0) {
-			rdev = map->regulator;
-			goto found;
-		}
-	}
-
 	if (board_wants_dummy_regulator) {
 		rdev = dummy_regulator_rdev;
 		goto found;
@@ -1431,20 +1461,12 @@ void devm_regulator_put(struct regulator *regulator)
 
 	rc = devres_destroy(regulator->dev, devm_regulator_release,
 			    devm_regulator_match, regulator);
-	WARN_ON(rc);
-}
-EXPORT_SYMBOL_GPL(devm_regulator_put);
-
-static int _regulator_can_change_status(struct regulator_dev *rdev)
-{
-	if (!rdev->constraints)
-		return 0;
-
-	if (rdev->constraints->valid_ops_mask & REGULATOR_CHANGE_STATUS)
-		return 1;
+	if (rc == 0)
+		regulator_put(regulator);
 	else
-		return 0;
+		WARN_ON(rc);
 }
+EXPORT_SYMBOL_GPL(devm_regulator_put);
 
 /* locks held by regulator_enable() */
 static int _regulator_enable(struct regulator_dev *rdev)
@@ -1525,6 +1547,9 @@ int regulator_enable(struct regulator *regulator)
 	struct regulator_dev *rdev = regulator->rdev;
 	int ret = 0;
 
+	if (regulator->always_on)
+		return 0;
+
 	if (rdev->supply) {
 		ret = regulator_enable(rdev->supply);
 		if (ret != 0)
@@ -1603,6 +1628,9 @@ int regulator_disable(struct regulator *regulator)
 	struct regulator_dev *rdev = regulator->rdev;
 	int ret = 0;
 
+	if (regulator->always_on)
+		return 0;
+
 	mutex_lock(&rdev->mutex);
 	ret = _regulator_disable(rdev);
 	mutex_unlock(&rdev->mutex);
@@ -1711,6 +1739,9 @@ int regulator_disable_deferred(struct regulator *regulator, int ms)
 	struct regulator_dev *rdev = regulator->rdev;
 	int ret;
 
+	if (regulator->always_on)
+		return 0;
+
 	mutex_lock(&rdev->mutex);
 	rdev->deferred_disables++;
 	mutex_unlock(&rdev->mutex);
@@ -1724,6 +1755,61 @@ int regulator_disable_deferred(struct regulator *regulator, int ms)
 }
 EXPORT_SYMBOL_GPL(regulator_disable_deferred);
 
+/**
+ * regulator_is_enabled_regmap - standard is_enabled() for regmap users
+ *
+ * @rdev: regulator to operate on
+ *
+ * Regulators that use regmap for their register I/O can set the
+ * enable_reg and enable_mask fields in their descriptor and then use
+ * this as their is_enabled operation, saving some code.
+ */
+int regulator_is_enabled_regmap(struct regulator_dev *rdev)
+{
+	unsigned int val;
+	int ret;
+
+	ret = regmap_read(rdev->regmap, rdev->desc->enable_reg, &val);
+	if (ret != 0)
+		return ret;
+
+	return (val & rdev->desc->enable_mask) != 0;
+}
+EXPORT_SYMBOL_GPL(regulator_is_enabled_regmap);
+
+/**
+ * regulator_enable_regmap - standard enable() for regmap users
+ *
+ * @rdev: regulator to operate on
+ *
+ * Regulators that use regmap for their register I/O can set the
+ * enable_reg and enable_mask fields in their descriptor and then use
+ * this as their enable() operation, saving some code.
+ */
+int regulator_enable_regmap(struct regulator_dev *rdev)
+{
+	return regmap_update_bits(rdev->regmap, rdev->desc->enable_reg,
+				  rdev->desc->enable_mask,
+				  rdev->desc->enable_mask);
+}
+EXPORT_SYMBOL_GPL(regulator_enable_regmap);
+
+/**
+ * regulator_disable_regmap - standard disable() for regmap users
+ *
+ * @rdev: regulator to operate on
+ *
+ * Regulators that use regmap for their register I/O can set the
+ * enable_reg and enable_mask fields in their descriptor and then use
+ * this as their disable() operation, saving some code.
+ */
+int regulator_disable_regmap(struct regulator_dev *rdev)
+{
+	return regmap_update_bits(rdev->regmap, rdev->desc->enable_reg,
+				  rdev->desc->enable_mask, 0);
+}
+EXPORT_SYMBOL_GPL(regulator_disable_regmap);
+
 static int _regulator_is_enabled(struct regulator_dev *rdev)
 {
 	/* If we don't know then assume that the regulator is always on */
@@ -1749,6 +1835,9 @@ int regulator_is_enabled(struct regulator *regulator)
 {
 	int ret;
 
+	if (regulator->always_on)
+		return 1;
+
 	mutex_lock(&regulator->rdev->mutex);
 	ret = _regulator_is_enabled(regulator->rdev);
 	mutex_unlock(&regulator->rdev->mutex);
@@ -1774,6 +1863,26 @@ int regulator_count_voltages(struct regulator *regulator)
 EXPORT_SYMBOL_GPL(regulator_count_voltages);
 
 /**
+ * regulator_list_voltage_linear - List voltages with simple calculation
+ *
+ * @rdev: Regulator device
+ * @selector: Selector to convert into a voltage
+ *
+ * Regulators with a simple linear mapping between voltages and
+ * selectors can set min_uV and uV_step in the regulator descriptor
+ * and then use this function as their list_voltage() operation,
+ */
+int regulator_list_voltage_linear(struct regulator_dev *rdev,
+				  unsigned int selector)
+{
+	if (selector >= rdev->desc->n_voltages)
+		return -EINVAL;
+
+	return rdev->desc->min_uV + (rdev->desc->uV_step * selector);
+}
+EXPORT_SYMBOL_GPL(regulator_list_voltage_linear);
+
+/**
  * regulator_list_voltage - enumerate supported voltages
  * @regulator: regulator source
  * @selector: identify voltage to list
@@ -1837,75 +1946,183 @@ int regulator_is_supported_voltage(struct regulator *regulator,
 }
 EXPORT_SYMBOL_GPL(regulator_is_supported_voltage);
 
+/**
+ * regulator_get_voltage_sel_regmap - standard get_voltage_sel for regmap users
+ *
+ * @rdev: regulator to operate on
+ *
+ * Regulators that use regmap for their register I/O can set the
+ * vsel_reg and vsel_mask fields in their descriptor and then use this
+ * as their get_voltage_vsel operation, saving some code.
+ */
+int regulator_get_voltage_sel_regmap(struct regulator_dev *rdev)
+{
+	unsigned int val;
+	int ret;
+
+	ret = regmap_read(rdev->regmap, rdev->desc->vsel_reg, &val);
+	if (ret != 0)
+		return ret;
+
+	val &= rdev->desc->vsel_mask;
+	val >>= ffs(rdev->desc->vsel_mask) - 1;
+
+	return val;
+}
+EXPORT_SYMBOL_GPL(regulator_get_voltage_sel_regmap);
+
+/**
+ * regulator_set_voltage_sel_regmap - standard set_voltage_sel for regmap users
+ *
+ * @rdev: regulator to operate on
+ * @sel: Selector to set
+ *
+ * Regulators that use regmap for their register I/O can set the
+ * vsel_reg and vsel_mask fields in their descriptor and then use this
+ * as their set_voltage_vsel operation, saving some code.
+ */
+int regulator_set_voltage_sel_regmap(struct regulator_dev *rdev, unsigned sel)
+{
+	sel <<= ffs(rdev->desc->vsel_mask) - 1;
+
+	return regmap_update_bits(rdev->regmap, rdev->desc->vsel_reg,
+				  rdev->desc->vsel_mask, sel);
+}
+EXPORT_SYMBOL_GPL(regulator_set_voltage_sel_regmap);
+
+/**
+ * regulator_map_voltage_iterate - map_voltage() based on list_voltage()
+ *
+ * @rdev: Regulator to operate on
+ * @min_uV: Lower bound for voltage
+ * @max_uV: Upper bound for voltage
+ *
+ * Drivers implementing set_voltage_sel() and list_voltage() can use
+ * this as their map_voltage() operation.  It will find a suitable
+ * voltage by calling list_voltage() until it gets something in bounds
+ * for the requested voltages.
+ */
+int regulator_map_voltage_iterate(struct regulator_dev *rdev,
+				  int min_uV, int max_uV)
+{
+	int best_val = INT_MAX;
+	int selector = 0;
+	int i, ret;
+
+	/* Find the smallest voltage that falls within the specified
+	 * range.
+	 */
+	for (i = 0; i < rdev->desc->n_voltages; i++) {
+		ret = rdev->desc->ops->list_voltage(rdev, i);
+		if (ret < 0)
+			continue;
+
+		if (ret < best_val && ret >= min_uV && ret <= max_uV) {
+			best_val = ret;
+			selector = i;
+		}
+	}
+
+	if (best_val != INT_MAX)
+		return selector;
+	else
+		return -EINVAL;
+}
+EXPORT_SYMBOL_GPL(regulator_map_voltage_iterate);
+
+/**
+ * regulator_map_voltage_linear - map_voltage() for simple linear mappings
+ *
+ * @rdev: Regulator to operate on
+ * @min_uV: Lower bound for voltage
+ * @max_uV: Upper bound for voltage
+ *
+ * Drivers providing min_uV and uV_step in their regulator_desc can
+ * use this as their map_voltage() operation.
+ */
+int regulator_map_voltage_linear(struct regulator_dev *rdev,
+				 int min_uV, int max_uV)
+{
+	int ret, voltage;
+
+	if (!rdev->desc->uV_step) {
+		BUG_ON(!rdev->desc->uV_step);
+		return -EINVAL;
+	}
+
+	ret = DIV_ROUND_UP(min_uV - rdev->desc->min_uV, rdev->desc->uV_step);
+	if (ret < 0)
+		return ret;
+
+	/* Map back into a voltage to verify we're still in bounds */
+	voltage = rdev->desc->ops->list_voltage(rdev, ret);
+	if (voltage < min_uV || voltage > max_uV)
+		return -EINVAL;
+
+	return ret;
+}
+EXPORT_SYMBOL_GPL(regulator_map_voltage_linear);
+
 static int _regulator_do_set_voltage(struct regulator_dev *rdev,
 				     int min_uV, int max_uV)
 {
 	int ret;
 	int delay = 0;
+	int best_val;
 	unsigned int selector;
+	int old_selector = -1;
 
 	trace_regulator_set_voltage(rdev_get_name(rdev), min_uV, max_uV);
 
 	min_uV += rdev->constraints->uV_offset;
 	max_uV += rdev->constraints->uV_offset;
 
+	/*
+	 * If we can't obtain the old selector there is not enough
+	 * info to call set_voltage_time_sel().
+	 */
+	if (rdev->desc->ops->set_voltage_time_sel &&
+	    rdev->desc->ops->get_voltage_sel) {
+		old_selector = rdev->desc->ops->get_voltage_sel(rdev);
+		if (old_selector < 0)
+			return old_selector;
+	}
+
 	if (rdev->desc->ops->set_voltage) {
 		ret = rdev->desc->ops->set_voltage(rdev, min_uV, max_uV,
 						   &selector);
-
-		if (rdev->desc->ops->list_voltage)
-			selector = rdev->desc->ops->list_voltage(rdev,
-								 selector);
-		else
-			selector = -1;
 	} else if (rdev->desc->ops->set_voltage_sel) {
-		int best_val = INT_MAX;
-		int i;
-
-		selector = 0;
-
-		/* Find the smallest voltage that falls within the specified
-		 * range.
-		 */
-		for (i = 0; i < rdev->desc->n_voltages; i++) {
-			ret = rdev->desc->ops->list_voltage(rdev, i);
-			if (ret < 0)
-				continue;
+		if (rdev->desc->ops->map_voltage)
+			ret = rdev->desc->ops->map_voltage(rdev, min_uV,
+							   max_uV);
+		else
+			ret = regulator_map_voltage_iterate(rdev, min_uV,
+							    max_uV);
 
-			if (ret < best_val && ret >= min_uV && ret <= max_uV) {
-				best_val = ret;
-				selector = i;
-			}
+		if (ret >= 0) {
+			selector = ret;
+			ret = rdev->desc->ops->set_voltage_sel(rdev, ret);
 		}
+	} else {
+		ret = -EINVAL;
+	}
 
-		/*
-		 * If we can't obtain the old selector there is not enough
-		 * info to call set_voltage_time_sel().
-		 */
-		if (rdev->desc->ops->set_voltage_time_sel &&
-		    rdev->desc->ops->get_voltage_sel) {
-			unsigned int old_selector = 0;
+	if (rdev->desc->ops->list_voltage)
+		best_val = rdev->desc->ops->list_voltage(rdev, selector);
+	else
+		best_val = -1;
 
-			ret = rdev->desc->ops->get_voltage_sel(rdev);
-			if (ret < 0)
-				return ret;
-			old_selector = ret;
-			ret = rdev->desc->ops->set_voltage_time_sel(rdev,
-						old_selector, selector);
-			if (ret < 0)
-				rdev_warn(rdev, "set_voltage_time_sel() failed: %d\n", ret);
-			else
-				delay = ret;
-		}
+	/* Call set_voltage_time_sel if successfully obtained old_selector */
+	if (ret == 0 && old_selector >= 0 &&
+	    rdev->desc->ops->set_voltage_time_sel) {
 
-		if (best_val != INT_MAX) {
-			ret = rdev->desc->ops->set_voltage_sel(rdev, selector);
-			selector = best_val;
-		} else {
-			ret = -EINVAL;
+		delay = rdev->desc->ops->set_voltage_time_sel(rdev,
+						old_selector, selector);
+		if (delay < 0) {
+			rdev_warn(rdev, "set_voltage_time_sel() failed: %d\n",
+				  delay);
+			delay = 0;
 		}
-	} else {
-		ret = -EINVAL;
 	}
 
 	/* Insert any necessary delays */
@@ -1920,7 +2137,7 @@ static int _regulator_do_set_voltage(struct regulator_dev *rdev,
 		_notifier_call_chain(rdev, REGULATOR_EVENT_VOLTAGE_CHANGE,
 				     NULL);
 
-	trace_regulator_set_voltage_complete(rdev_get_name(rdev), selector);
+	trace_regulator_set_voltage_complete(rdev_get_name(rdev), best_val);
 
 	return ret;
 }
@@ -2324,6 +2541,9 @@ int regulator_set_optimum_mode(struct regulator *regulator, int uA_load)
 	 */
 	ret = -EINVAL;
 
+	if (!rdev->desc->ops->set_mode)
+		goto out;
+
 	/* get output voltage */
 	output_uV = _regulator_get_voltage(rdev);
 	if (output_uV <= 0) {
@@ -2525,9 +2745,13 @@ int regulator_bulk_enable(int num_consumers,
 	int i;
 	int ret = 0;
 
-	for (i = 0; i < num_consumers; i++)
-		async_schedule_domain(regulator_bulk_enable_async,
-				      &consumers[i], &async_domain);
+	for (i = 0; i < num_consumers; i++) {
+		if (consumers[i].consumer->always_on)
+			consumers[i].ret = 0;
+		else
+			async_schedule_domain(regulator_bulk_enable_async,
+					      &consumers[i], &async_domain);
+	}
 
 	async_synchronize_full_domain(&async_domain);
 
@@ -2566,7 +2790,7 @@ int regulator_bulk_disable(int num_consumers,
 			   struct regulator_bulk_data *consumers)
 {
 	int i;
-	int ret;
+	int ret, r;
 
 	for (i = num_consumers - 1; i >= 0; --i) {
 		ret = regulator_disable(consumers[i].consumer);
@@ -2578,8 +2802,12 @@ int regulator_bulk_disable(int num_consumers,
 
 err:
 	pr_err("Failed to disable %s: %d\n", consumers[i].supply, ret);
-	for (++i; i < num_consumers; ++i)
-		regulator_enable(consumers[i].consumer);
+	for (++i; i < num_consumers; ++i) {
+		r = regulator_enable(consumers[i].consumer);
+		if (r != 0)
+			pr_err("Failed to reename %s: %d\n",
+			       consumers[i].supply, r);
+	}
 
 	return ret;
 }
@@ -2756,10 +2984,6 @@ static int add_regulator_attributes(struct regulator_dev *rdev)
 			return status;
 	}
 
-	/* suspend mode constraints need multiple supporting methods */
-	if (!(ops->set_suspend_enable && ops->set_suspend_disable))
-		return status;
-
 	status = device_create_file(dev, &dev_attr_suspend_standby_state);
 	if (status < 0)
 		return status;
@@ -2820,28 +3044,29 @@ static void rdev_init_debugfs(struct regulator_dev *rdev)
 /**
  * regulator_register - register regulator
  * @regulator_desc: regulator to register
- * @dev: struct device for the regulator
- * @init_data: platform provided init data, passed through by driver
- * @driver_data: private regulator data
- * @of_node: OpenFirmware node to parse for device tree bindings (may be
- *           NULL).
+ * @config: runtime configuration for regulator
  *
  * Called by regulator drivers to register a regulator.
  * Returns 0 on success.
  */
-struct regulator_dev *regulator_register(struct regulator_desc *regulator_desc,
-	struct device *dev, const struct regulator_init_data *init_data,
-	void *driver_data, struct device_node *of_node)
+struct regulator_dev *
+regulator_register(const struct regulator_desc *regulator_desc,
+		   const struct regulator_config *config)
 {
 	const struct regulation_constraints *constraints = NULL;
+	const struct regulator_init_data *init_data;
 	static atomic_t regulator_no = ATOMIC_INIT(0);
 	struct regulator_dev *rdev;
+	struct device *dev;
 	int ret, i;
 	const char *supply = NULL;
 
-	if (regulator_desc == NULL)
+	if (regulator_desc == NULL || config == NULL)
 		return ERR_PTR(-EINVAL);
 
+	dev = config->dev;
+	WARN_ON(!dev);
+
 	if (regulator_desc->name == NULL || regulator_desc->ops == NULL)
 		return ERR_PTR(-EINVAL);
 
@@ -2865,6 +3090,8 @@ struct regulator_dev *regulator_register(struct regulator_desc *regulator_desc,
 		return ERR_PTR(-EINVAL);
 	}
 
+	init_data = config->init_data;
+
 	rdev = kzalloc(sizeof(struct regulator_dev), GFP_KERNEL);
 	if (rdev == NULL)
 		return ERR_PTR(-ENOMEM);
@@ -2872,9 +3099,10 @@ struct regulator_dev *regulator_register(struct regulator_desc *regulator_desc,
 	mutex_lock(&regulator_list_mutex);
 
 	mutex_init(&rdev->mutex);
-	rdev->reg_data = driver_data;
+	rdev->reg_data = config->driver_data;
 	rdev->owner = regulator_desc->owner;
 	rdev->desc = regulator_desc;
+	rdev->regmap = config->regmap;
 	INIT_LIST_HEAD(&rdev->consumer_list);
 	INIT_LIST_HEAD(&rdev->list);
 	BLOCKING_INIT_NOTIFIER_HEAD(&rdev->notifier);
@@ -2889,7 +3117,7 @@ struct regulator_dev *regulator_register(struct regulator_desc *regulator_desc,
 
 	/* register with sysfs */
 	rdev->dev.class = &regulator_class;
-	rdev->dev.of_node = of_node;
+	rdev->dev.of_node = config->of_node;
 	rdev->dev.parent = dev;
 	dev_set_name(&rdev->dev, "regulator.%d",
 		     atomic_inc_return(&regulator_no) - 1);
@@ -2922,7 +3150,7 @@ struct regulator_dev *regulator_register(struct regulator_desc *regulator_desc,
 	if (supply) {
 		struct regulator_dev *r;
 
-		r = regulator_dev_lookup(dev, supply);
+		r = regulator_dev_lookup(dev, supply, &ret);
 
 		if (!r) {
 			dev_err(dev, "Failed to find supply %s\n", supply);
@@ -2935,8 +3163,7 @@ struct regulator_dev *regulator_register(struct regulator_desc *regulator_desc,
 			goto scrub;
 
 		/* Enable supply if rail is enabled */
-		if (rdev->desc->ops->is_enabled &&
-				rdev->desc->ops->is_enabled(rdev)) {
+		if (_regulator_is_enabled(rdev)) {
 			ret = regulator_enable(rdev->supply);
 			if (ret < 0)
 				goto scrub;
@@ -2968,6 +3195,8 @@ unset_supplies:
 	unset_regulator_supplies(rdev);
 
 scrub:
+	if (rdev->supply)
+		regulator_put(rdev->supply);
 	kfree(rdev->constraints);
 	device_unregister(&rdev->dev);
 	/* device core frees rdev */
@@ -3066,7 +3295,7 @@ int regulator_suspend_finish(void)
 				goto unlock;
 			if (!ops->disable)
 				goto unlock;
-			if (ops->is_enabled && !ops->is_enabled(rdev))
+			if (!_regulator_is_enabled(rdev))
 				goto unlock;
 
 			error = ops->disable(rdev);
diff --git a/drivers/regulator/da903x.c b/drivers/regulator/da903x.c
index 1851f0929ef..1005f5f7e60 100644
--- a/drivers/regulator/da903x.c
+++ b/drivers/regulator/da903x.c
@@ -76,9 +76,7 @@
 struct da903x_regulator_info {
 	struct regulator_desc desc;
 
-	int	min_uV;
 	int	max_uV;
-	int	step_uV;
 	int	vol_reg;
 	int	vol_shift;
 	int	vol_nbits;
@@ -88,10 +86,6 @@ struct da903x_regulator_info {
 	int	enable_bit;
 };
 
-static int da9034_ldo12_data[] = { 1700, 1750, 1800, 1850, 1900, 1950,
-				   2000, 2050, 2700, 2750, 2800, 2850,
-				   2900, 2950, 3000, 3050 };
-
 static inline struct device *to_da903x_dev(struct regulator_dev *rdev)
 {
 	return rdev_get_dev(rdev)->parent->parent;
@@ -100,34 +94,26 @@ static inline struct device *to_da903x_dev(struct regulator_dev *rdev)
 static inline int check_range(struct da903x_regulator_info *info,
 				int min_uV, int max_uV)
 {
-	if (min_uV < info->min_uV || min_uV > info->max_uV)
+	if (min_uV < info->desc.min_uV || min_uV > info->max_uV)
 		return -EINVAL;
 
 	return 0;
 }
 
 /* DA9030/DA9034 common operations */
-static int da903x_set_ldo_voltage(struct regulator_dev *rdev,
-				  int min_uV, int max_uV, unsigned *selector)
+static int da903x_set_voltage_sel(struct regulator_dev *rdev, unsigned selector)
 {
 	struct da903x_regulator_info *info = rdev_get_drvdata(rdev);
 	struct device *da9034_dev = to_da903x_dev(rdev);
 	uint8_t val, mask;
 
-	if (check_range(info, min_uV, max_uV)) {
-		pr_err("invalid voltage range (%d, %d) uV\n", min_uV, max_uV);
-		return -EINVAL;
-	}
-
-	val = DIV_ROUND_UP(min_uV - info->min_uV, info->step_uV);
-	*selector = val;
-	val <<= info->vol_shift;
+	val = selector << info->vol_shift;
 	mask = ((1 << info->vol_nbits) - 1)  << info->vol_shift;
 
 	return da903x_update(da9034_dev, info->vol_reg, val, mask);
 }
 
-static int da903x_get_voltage(struct regulator_dev *rdev)
+static int da903x_get_voltage_sel(struct regulator_dev *rdev)
 {
 	struct da903x_regulator_info *info = rdev_get_drvdata(rdev);
 	struct device *da9034_dev = to_da903x_dev(rdev);
@@ -141,7 +127,7 @@ static int da903x_get_voltage(struct regulator_dev *rdev)
 	mask = ((1 << info->vol_nbits) - 1)  << info->vol_shift;
 	val = (val & mask) >> info->vol_shift;
 
-	return info->min_uV + info->step_uV * val;
+	return val;
 }
 
 static int da903x_enable(struct regulator_dev *rdev)
@@ -176,35 +162,16 @@ static int da903x_is_enabled(struct regulator_dev *rdev)
 	return !!(reg_val & (1 << info->enable_bit));
 }
 
-static int da903x_list_voltage(struct regulator_dev *rdev, unsigned selector)
-{
-	struct da903x_regulator_info *info = rdev_get_drvdata(rdev);
-	int ret;
-
-	ret = info->min_uV + info->step_uV * selector;
-	if (ret > info->max_uV)
-		return -EINVAL;
-	return ret;
-}
-
 /* DA9030 specific operations */
-static int da9030_set_ldo1_15_voltage(struct regulator_dev *rdev,
-				      int min_uV, int max_uV,
-				      unsigned *selector)
+static int da9030_set_ldo1_15_voltage_sel(struct regulator_dev *rdev,
+					  unsigned selector)
 {
 	struct da903x_regulator_info *info = rdev_get_drvdata(rdev);
 	struct device *da903x_dev = to_da903x_dev(rdev);
 	uint8_t val, mask;
 	int ret;
 
-	if (check_range(info, min_uV, max_uV)) {
-		pr_err("invalid voltage range (%d, %d) uV\n", min_uV, max_uV);
-		return -EINVAL;
-	}
-
-	val = DIV_ROUND_UP(min_uV - info->min_uV, info->step_uV);
-	*selector = val;
-	val <<= info->vol_shift;
+	val = selector << info->vol_shift;
 	mask = ((1 << info->vol_nbits) - 1)  << info->vol_shift;
 	val |= DA9030_LDO_UNLOCK; /* have to set UNLOCK bits */
 	mask |= DA9030_LDO_UNLOCK_MASK;
@@ -217,73 +184,57 @@ static int da9030_set_ldo1_15_voltage(struct regulator_dev *rdev,
 	return da903x_update(da903x_dev, info->vol_reg, val, mask);
 }
 
-static int da9030_set_ldo14_voltage(struct regulator_dev *rdev,
-				    int min_uV, int max_uV,
-				    unsigned *selector)
+static int da9030_map_ldo14_voltage(struct regulator_dev *rdev,
+				    int min_uV, int max_uV)
 {
 	struct da903x_regulator_info *info = rdev_get_drvdata(rdev);
-	struct device *da903x_dev = to_da903x_dev(rdev);
-	uint8_t val, mask;
-	int thresh;
+	int thresh, sel;
 
 	if (check_range(info, min_uV, max_uV)) {
 		pr_err("invalid voltage range (%d, %d) uV\n", min_uV, max_uV);
 		return -EINVAL;
 	}
 
-	thresh = (info->max_uV + info->min_uV) / 2;
+	thresh = (info->max_uV + info->desc.min_uV) / 2;
 	if (min_uV < thresh) {
-		val = DIV_ROUND_UP(thresh - min_uV, info->step_uV);
-		val |= 0x4;
+		sel = DIV_ROUND_UP(thresh - min_uV, info->desc.uV_step);
+		sel |= 0x4;
 	} else {
-		val = DIV_ROUND_UP(min_uV - thresh, info->step_uV);
+		sel = DIV_ROUND_UP(min_uV - thresh, info->desc.uV_step);
 	}
 
-	*selector = val;
-	val <<= info->vol_shift;
-	mask = ((1 << info->vol_nbits) - 1)  << info->vol_shift;
-
-	return da903x_update(da903x_dev, info->vol_reg, val, mask);
+	return sel;
 }
 
-static int da9030_get_ldo14_voltage(struct regulator_dev *rdev)
+static int da9030_list_ldo14_voltage(struct regulator_dev *rdev,
+				     unsigned selector)
 {
 	struct da903x_regulator_info *info = rdev_get_drvdata(rdev);
-	struct device *da903x_dev = to_da903x_dev(rdev);
-	uint8_t val, mask;
-	int ret;
+	int volt;
 
-	ret = da903x_read(da903x_dev, info->vol_reg, &val);
-	if (ret)
-		return ret;
+	if (selector & 0x4)
+		volt = rdev->desc->min_uV +
+		       rdev->desc->uV_step * (3 - (selector & ~0x4));
+	else
+		volt = (info->max_uV + rdev->desc->min_uV) / 2 +
+		       rdev->desc->uV_step * (selector & ~0x4);
 
-	mask = ((1 << info->vol_nbits) - 1)  << info->vol_shift;
-	val = (val & mask) >> info->vol_shift;
+	if (volt > info->max_uV)
+		return -EINVAL;
 
-	if (val & 0x4)
-		return info->min_uV + info->step_uV * (3 - (val & ~0x4));
-	else
-		return (info->max_uV + info->min_uV) / 2 +
-			info->step_uV * (val & ~0x4);
+	return volt;
 }
 
 /* DA9034 specific operations */
-static int da9034_set_dvc_voltage(struct regulator_dev *rdev,
-				  int min_uV, int max_uV, unsigned *selector)
+static int da9034_set_dvc_voltage_sel(struct regulator_dev *rdev,
+				      unsigned selector)
 {
 	struct da903x_regulator_info *info = rdev_get_drvdata(rdev);
 	struct device *da9034_dev = to_da903x_dev(rdev);
 	uint8_t val, mask;
 	int ret;
 
-	if (check_range(info, min_uV, max_uV)) {
-		pr_err("invalid voltage range (%d, %d) uV\n", min_uV, max_uV);
-		return -EINVAL;
-	}
-
-	val = DIV_ROUND_UP(min_uV - info->min_uV, info->step_uV);
-	*selector = val;
-	val <<= info->vol_shift;
+	val = selector << info->vol_shift;
 	mask = ((1 << info->vol_nbits) - 1)  << info->vol_shift;
 
 	ret = da903x_update(da9034_dev, info->vol_reg, val, mask);
@@ -295,59 +246,45 @@ static int da9034_set_dvc_voltage(struct regulator_dev *rdev,
 	return ret;
 }
 
-static int da9034_set_ldo12_voltage(struct regulator_dev *rdev,
-				    int min_uV, int max_uV, unsigned *selector)
+static int da9034_map_ldo12_voltage(struct regulator_dev *rdev,
+				    int min_uV, int max_uV)
 {
 	struct da903x_regulator_info *info = rdev_get_drvdata(rdev);
-	struct device *da9034_dev = to_da903x_dev(rdev);
-	uint8_t val, mask;
+	int sel;
 
 	if (check_range(info, min_uV, max_uV)) {
 		pr_err("invalid voltage range (%d, %d) uV\n", min_uV, max_uV);
 		return -EINVAL;
 	}
 
-	val = DIV_ROUND_UP(min_uV - info->min_uV, info->step_uV);
-	val = (val >= 20) ? val - 12 : ((val > 7) ? 8 : val);
-	*selector = val;
-	val <<= info->vol_shift;
-	mask = ((1 << info->vol_nbits) - 1)  << info->vol_shift;
+	sel = DIV_ROUND_UP(min_uV - info->desc.min_uV, info->desc.uV_step);
+	sel = (sel >= 20) ? sel - 12 : ((sel > 7) ? 8 : sel);
 
-	return da903x_update(da9034_dev, info->vol_reg, val, mask);
+	return sel;
 }
 
-static int da9034_get_ldo12_voltage(struct regulator_dev *rdev)
+static int da9034_list_ldo12_voltage(struct regulator_dev *rdev,
+				     unsigned selector)
 {
 	struct da903x_regulator_info *info = rdev_get_drvdata(rdev);
-	struct device *da9034_dev = to_da903x_dev(rdev);
-	uint8_t val, mask;
-	int ret;
-
-	ret = da903x_read(da9034_dev, info->vol_reg, &val);
-	if (ret)
-		return ret;
-
-	mask = ((1 << info->vol_nbits) - 1)  << info->vol_shift;
-	val = (val & mask) >> info->vol_shift;
+	int volt;
 
-	if (val >= 8)
-		return 2700000 + info->step_uV * (val - 8);
-
-	return info->min_uV + info->step_uV * val;
-}
+	if (selector >= 8)
+		volt = 2700000 + rdev->desc->uV_step * (selector - 8);
+	else
+		volt = rdev->desc->min_uV + rdev->desc->uV_step * selector;
 
-static int da9034_list_ldo12_voltage(struct regulator_dev *rdev,
-				     unsigned selector)
-{
-	if (selector >= ARRAY_SIZE(da9034_ldo12_data))
+	if (volt > info->max_uV)
 		return -EINVAL;
-	return da9034_ldo12_data[selector] * 1000;
+
+	return volt;
 }
 
 static struct regulator_ops da903x_regulator_ldo_ops = {
-	.set_voltage	= da903x_set_ldo_voltage,
-	.get_voltage	= da903x_get_voltage,
-	.list_voltage	= da903x_list_voltage,
+	.set_voltage_sel = da903x_set_voltage_sel,
+	.get_voltage_sel = da903x_get_voltage_sel,
+	.list_voltage	= regulator_list_voltage_linear,
+	.map_voltage	= regulator_map_voltage_linear,
 	.enable		= da903x_enable,
 	.disable	= da903x_disable,
 	.is_enabled	= da903x_is_enabled,
@@ -355,9 +292,10 @@ static struct regulator_ops da903x_regulator_ldo_ops = {
 
 /* NOTE: this is dedicated for the insane DA9030 LDO14 */
 static struct regulator_ops da9030_regulator_ldo14_ops = {
-	.set_voltage	= da9030_set_ldo14_voltage,
-	.get_voltage	= da9030_get_ldo14_voltage,
-	.list_voltage	= da903x_list_voltage,
+	.set_voltage_sel = da903x_set_voltage_sel,
+	.get_voltage_sel = da903x_get_voltage_sel,
+	.list_voltage	= da9030_list_ldo14_voltage,
+	.map_voltage	= da9030_map_ldo14_voltage,
 	.enable		= da903x_enable,
 	.disable	= da903x_disable,
 	.is_enabled	= da903x_is_enabled,
@@ -365,18 +303,20 @@ static struct regulator_ops da9030_regulator_ldo14_ops = {
 
 /* NOTE: this is dedicated for the DA9030 LDO1 and LDO15 that have locks  */
 static struct regulator_ops da9030_regulator_ldo1_15_ops = {
-	.set_voltage	= da9030_set_ldo1_15_voltage,
-	.get_voltage	= da903x_get_voltage,
-	.list_voltage	= da903x_list_voltage,
+	.set_voltage_sel = da9030_set_ldo1_15_voltage_sel,
+	.get_voltage_sel = da903x_get_voltage_sel,
+	.list_voltage	= regulator_list_voltage_linear,
+	.map_voltage	= regulator_map_voltage_linear,
 	.enable		= da903x_enable,
 	.disable	= da903x_disable,
 	.is_enabled	= da903x_is_enabled,
 };
 
 static struct regulator_ops da9034_regulator_dvc_ops = {
-	.set_voltage	= da9034_set_dvc_voltage,
-	.get_voltage	= da903x_get_voltage,
-	.list_voltage	= da903x_list_voltage,
+	.set_voltage_sel = da9034_set_dvc_voltage_sel,
+	.get_voltage_sel = da903x_get_voltage_sel,
+	.list_voltage	= regulator_list_voltage_linear,
+	.map_voltage	= regulator_map_voltage_linear,
 	.enable		= da903x_enable,
 	.disable	= da903x_disable,
 	.is_enabled	= da903x_is_enabled,
@@ -384,9 +324,10 @@ static struct regulator_ops da9034_regulator_dvc_ops = {
 
 /* NOTE: this is dedicated for the insane LDO12 */
 static struct regulator_ops da9034_regulator_ldo12_ops = {
-	.set_voltage	= da9034_set_ldo12_voltage,
-	.get_voltage	= da9034_get_ldo12_voltage,
+	.set_voltage_sel = da903x_set_voltage_sel,
+	.get_voltage_sel = da903x_get_voltage_sel,
 	.list_voltage	= da9034_list_ldo12_voltage,
+	.map_voltage	= da9034_map_ldo12_voltage,
 	.enable		= da903x_enable,
 	.disable	= da903x_disable,
 	.is_enabled	= da903x_is_enabled,
@@ -401,10 +342,10 @@ static struct regulator_ops da9034_regulator_ldo12_ops = {
 		.id	= _pmic##_ID_LDO##_id,				\
 		.n_voltages = (step) ? ((max - min) / step + 1) : 1,	\
 		.owner	= THIS_MODULE,					\
+		.min_uV	 = (min) * 1000,				\
+		.uV_step = (step) * 1000,				\
 	},								\
-	.min_uV		= (min) * 1000,					\
 	.max_uV		= (max) * 1000,					\
-	.step_uV	= (step) * 1000,				\
 	.vol_reg	= _pmic##_##vreg,				\
 	.vol_shift	= (shift),					\
 	.vol_nbits	= (nbits),					\
@@ -421,10 +362,10 @@ static struct regulator_ops da9034_regulator_ldo12_ops = {
 		.id	= _pmic##_ID_##_id,				\
 		.n_voltages = (step) ? ((max - min) / step + 1) : 1,	\
 		.owner	= THIS_MODULE,					\
+		.min_uV = (min) * 1000,					\
+		.uV_step = (step) * 1000,				\
 	},								\
-	.min_uV		= (min) * 1000,					\
 	.max_uV		= (max) * 1000,					\
-	.step_uV	= (step) * 1000,				\
 	.vol_reg	= _pmic##_##vreg,				\
 	.vol_shift	= (0),						\
 	.vol_nbits	= (nbits),					\
@@ -517,6 +458,7 @@ static int __devinit da903x_regulator_probe(struct platform_device *pdev)
 {
 	struct da903x_regulator_info *ri = NULL;
 	struct regulator_dev *rdev;
+	struct regulator_config config = { };
 
 	ri = find_regulator_info(pdev->id);
 	if (ri == NULL) {
@@ -527,7 +469,7 @@ static int __devinit da903x_regulator_probe(struct platform_device *pdev)
 	/* Workaround for the weird LDO12 voltage setting */
 	if (ri->desc.id == DA9034_ID_LDO12) {
 		ri->desc.ops = &da9034_regulator_ldo12_ops;
-		ri->desc.n_voltages = ARRAY_SIZE(da9034_ldo12_data);
+		ri->desc.n_voltages = 16;
 	}
 
 	if (ri->desc.id == DA9030_ID_LDO14)
@@ -536,8 +478,11 @@ static int __devinit da903x_regulator_probe(struct platform_device *pdev)
 	if (ri->desc.id == DA9030_ID_LDO1 || ri->desc.id == DA9030_ID_LDO15)
 		ri->desc.ops = &da9030_regulator_ldo1_15_ops;
 
-	rdev = regulator_register(&ri->desc, &pdev->dev,
-				  pdev->dev.platform_data, ri, NULL);
+	config.dev = &pdev->dev;
+	config.init_data = pdev->dev.platform_data;
+	config.driver_data = ri;
+
+	rdev = regulator_register(&ri->desc, &config);
 	if (IS_ERR(rdev)) {
 		dev_err(&pdev->dev, "failed to register regulator %s\n",
 				ri->desc.name);
diff --git a/drivers/regulator/da9052-regulator.c b/drivers/regulator/da9052-regulator.c
index 09915e89705..88976d8d44e 100644
--- a/drivers/regulator/da9052-regulator.c
+++ b/drivers/regulator/da9052-regulator.c
@@ -19,6 +19,10 @@
 #include <linux/platform_device.h>
 #include <linux/regulator/driver.h>
 #include <linux/regulator/machine.h>
+#ifdef CONFIG_OF
+#include <linux/of.h>
+#include <linux/regulator/of_regulator.h>
+#endif
 
 #include <linux/mfd/da9052/da9052.h>
 #include <linux/mfd/da9052/reg.h>
@@ -37,6 +41,22 @@
 #define DA9052_BUCK_ILIM_MASK_EVEN	0x0c
 #define DA9052_BUCK_ILIM_MASK_ODD	0xc0
 
+/* DA9052 REGULATOR IDs */
+#define DA9052_ID_BUCK1		0
+#define DA9052_ID_BUCK2		1
+#define DA9052_ID_BUCK3		2
+#define DA9052_ID_BUCK4		3
+#define DA9052_ID_LDO1		4
+#define DA9052_ID_LDO2		5
+#define DA9052_ID_LDO3		6
+#define DA9052_ID_LDO4		7
+#define DA9052_ID_LDO5		8
+#define DA9052_ID_LDO6		9
+#define DA9052_ID_LDO7		10
+#define DA9052_ID_LDO8		11
+#define DA9052_ID_LDO9		12
+#define DA9052_ID_LDO10		13
+
 static const u32 da9052_current_limits[3][4] = {
 	{700000, 800000, 1000000, 1200000},	/* DA9052-BC BUCKs */
 	{1600000, 2000000, 2400000, 3000000},	/* DA9053-AA/Bx BUCK-CORE */
@@ -50,8 +70,6 @@ struct da9052_regulator_info {
 	int step_uV;
 	int min_uV;
 	int max_uV;
-	unsigned char volt_shift;
-	unsigned char en_bit;
 	unsigned char activate_bit;
 };
 
@@ -70,42 +88,6 @@ static int verify_range(struct da9052_regulator_info *info,
 	return 0;
 }
 
-static int da9052_regulator_enable(struct regulator_dev *rdev)
-{
-	struct da9052_regulator *regulator = rdev_get_drvdata(rdev);
-	struct da9052_regulator_info *info = regulator->info;
-	int offset = rdev_get_id(rdev);
-
-	return da9052_reg_update(regulator->da9052,
-				 DA9052_BUCKCORE_REG + offset,
-				 1 << info->en_bit, 1 << info->en_bit);
-}
-
-static int da9052_regulator_disable(struct regulator_dev *rdev)
-{
-	struct da9052_regulator *regulator = rdev_get_drvdata(rdev);
-	struct da9052_regulator_info *info = regulator->info;
-	int offset = rdev_get_id(rdev);
-
-	return da9052_reg_update(regulator->da9052,
-				 DA9052_BUCKCORE_REG + offset,
-				 1 << info->en_bit, 0);
-}
-
-static int da9052_regulator_is_enabled(struct regulator_dev *rdev)
-{
-	struct da9052_regulator *regulator = rdev_get_drvdata(rdev);
-	struct da9052_regulator_info *info = regulator->info;
-	int offset = rdev_get_id(rdev);
-	int ret;
-
-	ret = da9052_reg_read(regulator->da9052, DA9052_BUCKCORE_REG + offset);
-	if (ret < 0)
-		return ret;
-
-	return ret & (1 << info->en_bit);
-}
-
 static int da9052_dcdc_get_current_limit(struct regulator_dev *rdev)
 {
 	struct da9052_regulator *regulator = rdev_get_drvdata(rdev);
@@ -173,36 +155,23 @@ static int da9052_dcdc_set_current_limit(struct regulator_dev *rdev, int min_uA,
 					 reg_val << 6);
 }
 
-static int da9052_list_buckperi_voltage(struct regulator_dev *rdev,
-					 unsigned int selector)
-{
-	struct da9052_regulator *regulator = rdev_get_drvdata(rdev);
-	struct da9052_regulator_info *info = regulator->info;
-	int volt_uV;
-
-	if ((regulator->da9052->chip_id == DA9052) &&
-	    (selector >= DA9052_BUCK_PERI_REG_MAP_UPTO_3uV)) {
-		volt_uV = ((DA9052_BUCK_PERI_REG_MAP_UPTO_3uV * info->step_uV)
-			    + info->min_uV);
-		volt_uV += (selector - DA9052_BUCK_PERI_REG_MAP_UPTO_3uV)
-			    * (DA9052_BUCK_PERI_3uV_STEP);
-	} else
-			volt_uV = (selector * info->step_uV) + info->min_uV;
-
-	if (volt_uV > info->max_uV)
-		return -EINVAL;
-
-	return volt_uV;
-}
-
 static int da9052_list_voltage(struct regulator_dev *rdev,
 				unsigned int selector)
 {
 	struct da9052_regulator *regulator = rdev_get_drvdata(rdev);
 	struct da9052_regulator_info *info = regulator->info;
+	int id = rdev_get_id(rdev);
 	int volt_uV;
 
-	volt_uV = info->min_uV + info->step_uV * selector;
+	if ((id == DA9052_ID_BUCK4) && (regulator->da9052->chip_id == DA9052)
+		&& (selector >= DA9052_BUCK_PERI_REG_MAP_UPTO_3uV)) {
+		volt_uV = ((DA9052_BUCK_PERI_REG_MAP_UPTO_3uV * info->step_uV)
+			  + info->min_uV);
+		volt_uV += (selector - DA9052_BUCK_PERI_REG_MAP_UPTO_3uV)
+				    * (DA9052_BUCK_PERI_3uV_STEP);
+	} else {
+		volt_uV = (selector * info->step_uV) + info->min_uV;
+	}
 
 	if (volt_uV > info->max_uV)
 		return -EINVAL;
@@ -210,103 +179,13 @@ static int da9052_list_voltage(struct regulator_dev *rdev,
 	return volt_uV;
 }
 
-static int da9052_regulator_set_voltage_int(struct regulator_dev *rdev,
-					     int min_uV, int max_uV,
-					     unsigned int *selector)
-{
-	struct da9052_regulator *regulator = rdev_get_drvdata(rdev);
-	struct da9052_regulator_info *info = regulator->info;
-	int offset = rdev_get_id(rdev);
-	int ret;
-
-	ret = verify_range(info, min_uV, max_uV);
-	if (ret < 0)
-		return ret;
-
-	if (min_uV < info->min_uV)
-		min_uV = info->min_uV;
-
-	*selector = DIV_ROUND_UP(min_uV - info->min_uV, info->step_uV);
-
-	ret = da9052_list_voltage(rdev, *selector);
-	if (ret < 0)
-		return ret;
-
-	return da9052_reg_update(regulator->da9052,
-				 DA9052_BUCKCORE_REG + offset,
-				 (1 << info->volt_shift) - 1, *selector);
-}
-
-static int da9052_set_ldo_voltage(struct regulator_dev *rdev,
-				   int min_uV, int max_uV,
-				   unsigned int *selector)
-{
-	return da9052_regulator_set_voltage_int(rdev, min_uV, max_uV, selector);
-}
-
-static int da9052_set_ldo5_6_voltage(struct regulator_dev *rdev,
-				      int min_uV, int max_uV,
-				      unsigned int *selector)
-{
-	struct da9052_regulator *regulator = rdev_get_drvdata(rdev);
-	struct da9052_regulator_info *info = regulator->info;
-	int ret;
-
-	ret = da9052_regulator_set_voltage_int(rdev, min_uV, max_uV, selector);
-	if (ret < 0)
-		return ret;
-
-	/* Some LDOs are DVC controlled which requires enabling of
-	 * the LDO activate bit to implment the changes on the
-	 * LDO output.
-	*/
-	return da9052_reg_update(regulator->da9052, DA9052_SUPPLY_REG,
-				 info->activate_bit, info->activate_bit);
-}
-
-static int da9052_set_dcdc_voltage(struct regulator_dev *rdev,
-				    int min_uV, int max_uV,
-				    unsigned int *selector)
-{
-	struct da9052_regulator *regulator = rdev_get_drvdata(rdev);
-	struct da9052_regulator_info *info = regulator->info;
-	int ret;
-
-	ret = da9052_regulator_set_voltage_int(rdev, min_uV, max_uV, selector);
-	if (ret < 0)
-		return ret;
-
-	/* Some DCDCs are DVC controlled which requires enabling of
-	 * the DCDC activate bit to implment the changes on the
-	 * DCDC output.
-	*/
-	return da9052_reg_update(regulator->da9052, DA9052_SUPPLY_REG,
-				 info->activate_bit, info->activate_bit);
-}
-
-static int da9052_get_regulator_voltage_sel(struct regulator_dev *rdev)
-{
-	struct da9052_regulator *regulator = rdev_get_drvdata(rdev);
-	struct da9052_regulator_info *info = regulator->info;
-	int offset = rdev_get_id(rdev);
-	int ret;
-
-	ret = da9052_reg_read(regulator->da9052, DA9052_BUCKCORE_REG + offset);
-	if (ret < 0)
-		return ret;
-
-	ret &= ((1 << info->volt_shift) - 1);
-
-	return ret;
-}
-
-static int da9052_set_buckperi_voltage(struct regulator_dev *rdev, int min_uV,
-					int max_uV, unsigned int *selector)
+static int da9052_map_voltage(struct regulator_dev *rdev,
+			      int min_uV, int max_uV)
 {
 	struct da9052_regulator *regulator = rdev_get_drvdata(rdev);
 	struct da9052_regulator_info *info = regulator->info;
-	int offset = rdev_get_id(rdev);
-	int ret;
+	int id = rdev_get_id(rdev);
+	int ret, sel;
 
 	ret = verify_range(info, min_uV, max_uV);
 	if (ret < 0)
@@ -315,192 +194,147 @@ static int da9052_set_buckperi_voltage(struct regulator_dev *rdev, int min_uV,
 	if (min_uV < info->min_uV)
 		min_uV = info->min_uV;
 
-	if ((regulator->da9052->chip_id == DA9052) &&
-	    (min_uV >= DA9052_CONST_3uV))
-		*selector = DA9052_BUCK_PERI_REG_MAP_UPTO_3uV +
-			    DIV_ROUND_UP(min_uV - DA9052_CONST_3uV,
-					 DA9052_BUCK_PERI_3uV_STEP);
-	else
-		*selector = DIV_ROUND_UP(min_uV - info->min_uV, info->step_uV);
+	if ((id == DA9052_ID_BUCK4) && (regulator->da9052->chip_id == DA9052)
+		&& (min_uV >= DA9052_CONST_3uV)) {
+			sel = DA9052_BUCK_PERI_REG_MAP_UPTO_3uV +
+			      DIV_ROUND_UP(min_uV - DA9052_CONST_3uV,
+					   DA9052_BUCK_PERI_3uV_STEP);
+	} else {
+		sel = DIV_ROUND_UP(min_uV - info->min_uV, info->step_uV);
+	}
 
-	ret = da9052_list_buckperi_voltage(rdev, *selector);
+	ret = da9052_list_voltage(rdev, sel);
 	if (ret < 0)
 		return ret;
 
-	return da9052_reg_update(regulator->da9052,
-				 DA9052_BUCKCORE_REG + offset,
-				 (1 << info->volt_shift) - 1, *selector);
+	return sel;
 }
 
-static int da9052_get_buckperi_voltage_sel(struct regulator_dev *rdev)
+static int da9052_regulator_set_voltage_sel(struct regulator_dev *rdev,
+					    unsigned int selector)
 {
 	struct da9052_regulator *regulator = rdev_get_drvdata(rdev);
 	struct da9052_regulator_info *info = regulator->info;
-	int offset = rdev_get_id(rdev);
+	int id = rdev_get_id(rdev);
 	int ret;
 
-	ret = da9052_reg_read(regulator->da9052, DA9052_BUCKCORE_REG + offset);
+	ret = da9052_reg_update(regulator->da9052, rdev->desc->vsel_reg,
+				rdev->desc->vsel_mask, selector);
 	if (ret < 0)
 		return ret;
 
-	ret &= ((1 << info->volt_shift) - 1);
+	/* Some LDOs and DCDCs are DVC controlled which requires enabling of
+	 * the activate bit to implment the changes on the output.
+	 */
+	switch (id) {
+	case DA9052_ID_BUCK1:
+	case DA9052_ID_BUCK2:
+	case DA9052_ID_BUCK3:
+	case DA9052_ID_LDO2:
+	case DA9052_ID_LDO3:
+		ret = da9052_reg_update(regulator->da9052, DA9052_SUPPLY_REG,
+					info->activate_bit, info->activate_bit);
+		break;
+	}
 
 	return ret;
 }
 
-static struct regulator_ops da9052_buckperi_ops = {
-	.list_voltage = da9052_list_buckperi_voltage,
-	.get_voltage_sel = da9052_get_buckperi_voltage_sel,
-	.set_voltage = da9052_set_buckperi_voltage,
-
-	.get_current_limit = da9052_dcdc_get_current_limit,
-	.set_current_limit = da9052_dcdc_set_current_limit,
-
-	.is_enabled = da9052_regulator_is_enabled,
-	.enable = da9052_regulator_enable,
-	.disable = da9052_regulator_disable,
-};
-
 static struct regulator_ops da9052_dcdc_ops = {
-	.set_voltage = da9052_set_dcdc_voltage,
 	.get_current_limit = da9052_dcdc_get_current_limit,
 	.set_current_limit = da9052_dcdc_set_current_limit,
 
 	.list_voltage = da9052_list_voltage,
-	.get_voltage_sel = da9052_get_regulator_voltage_sel,
-	.is_enabled = da9052_regulator_is_enabled,
-	.enable = da9052_regulator_enable,
-	.disable = da9052_regulator_disable,
-};
-
-static struct regulator_ops da9052_ldo5_6_ops = {
-	.set_voltage = da9052_set_ldo5_6_voltage,
-
-	.list_voltage = da9052_list_voltage,
-	.get_voltage_sel = da9052_get_regulator_voltage_sel,
-	.is_enabled = da9052_regulator_is_enabled,
-	.enable = da9052_regulator_enable,
-	.disable = da9052_regulator_disable,
+	.map_voltage = da9052_map_voltage,
+	.get_voltage_sel = regulator_get_voltage_sel_regmap,
+	.set_voltage_sel = da9052_regulator_set_voltage_sel,
+	.is_enabled = regulator_is_enabled_regmap,
+	.enable = regulator_enable_regmap,
+	.disable = regulator_disable_regmap,
 };
 
 static struct regulator_ops da9052_ldo_ops = {
-	.set_voltage = da9052_set_ldo_voltage,
-
 	.list_voltage = da9052_list_voltage,
-	.get_voltage_sel = da9052_get_regulator_voltage_sel,
-	.is_enabled = da9052_regulator_is_enabled,
-	.enable = da9052_regulator_enable,
-	.disable = da9052_regulator_disable,
+	.map_voltage = da9052_map_voltage,
+	.get_voltage_sel = regulator_get_voltage_sel_regmap,
+	.set_voltage_sel = da9052_regulator_set_voltage_sel,
+	.is_enabled = regulator_is_enabled_regmap,
+	.enable = regulator_enable_regmap,
+	.disable = regulator_disable_regmap,
 };
 
-#define DA9052_LDO5_6(_id, step, min, max, sbits, ebits, abits) \
-{\
-	.reg_desc = {\
-		.name = "LDO" #_id,\
-		.ops = &da9052_ldo5_6_ops,\
-		.type = REGULATOR_VOLTAGE,\
-		.id = _id,\
-		.n_voltages = (max - min) / step + 1, \
-		.owner = THIS_MODULE,\
-	},\
-	.min_uV = (min) * 1000,\
-	.max_uV = (max) * 1000,\
-	.step_uV = (step) * 1000,\
-	.volt_shift = (sbits),\
-	.en_bit = (ebits),\
-	.activate_bit = (abits),\
-}
-
 #define DA9052_LDO(_id, step, min, max, sbits, ebits, abits) \
 {\
 	.reg_desc = {\
-		.name = "LDO" #_id,\
+		.name = #_id,\
 		.ops = &da9052_ldo_ops,\
 		.type = REGULATOR_VOLTAGE,\
-		.id = _id,\
+		.id = DA9052_ID_##_id,\
 		.n_voltages = (max - min) / step + 1, \
 		.owner = THIS_MODULE,\
+		.vsel_reg = DA9052_BUCKCORE_REG + DA9052_ID_##_id, \
+		.vsel_mask = (1 << (sbits)) - 1,\
+		.enable_reg = DA9052_BUCKCORE_REG + DA9052_ID_##_id, \
+		.enable_mask = 1 << (ebits),\
 	},\
 	.min_uV = (min) * 1000,\
 	.max_uV = (max) * 1000,\
 	.step_uV = (step) * 1000,\
-	.volt_shift = (sbits),\
-	.en_bit = (ebits),\
 	.activate_bit = (abits),\
 }
 
 #define DA9052_DCDC(_id, step, min, max, sbits, ebits, abits) \
 {\
 	.reg_desc = {\
-		.name = "BUCK" #_id,\
+		.name = #_id,\
 		.ops = &da9052_dcdc_ops,\
 		.type = REGULATOR_VOLTAGE,\
-		.id = _id,\
+		.id = DA9052_ID_##_id,\
 		.n_voltages = (max - min) / step + 1, \
 		.owner = THIS_MODULE,\
+		.vsel_reg = DA9052_BUCKCORE_REG + DA9052_ID_##_id, \
+		.vsel_mask = (1 << (sbits)) - 1,\
+		.enable_reg = DA9052_BUCKCORE_REG + DA9052_ID_##_id, \
+		.enable_mask = 1 << (ebits),\
 	},\
 	.min_uV = (min) * 1000,\
 	.max_uV = (max) * 1000,\
 	.step_uV = (step) * 1000,\
-	.volt_shift = (sbits),\
-	.en_bit = (ebits),\
-	.activate_bit = (abits),\
-}
-
-#define DA9052_BUCKPERI(_id, step, min, max, sbits, ebits, abits) \
-{\
-	.reg_desc = {\
-		.name = "BUCK" #_id,\
-		.ops = &da9052_buckperi_ops,\
-		.type = REGULATOR_VOLTAGE,\
-		.id = _id,\
-		.n_voltages = (max - min) / step + 1, \
-		.owner = THIS_MODULE,\
-	},\
-	.min_uV = (min) * 1000,\
-	.max_uV = (max) * 1000,\
-	.step_uV = (step) * 1000,\
-	.volt_shift = (sbits),\
-	.en_bit = (ebits),\
 	.activate_bit = (abits),\
 }
 
 static struct da9052_regulator_info da9052_regulator_info[] = {
-	/* Buck1 - 4 */
-	DA9052_DCDC(0, 25, 500, 2075, 6, 6, DA9052_SUPPLY_VBCOREGO),
-	DA9052_DCDC(1, 25, 500, 2075, 6, 6, DA9052_SUPPLY_VBPROGO),
-	DA9052_DCDC(2, 25, 925, 2500, 6, 6, DA9052_SUPPLY_VBMEMGO),
-	DA9052_BUCKPERI(3, 50, 1800, 3600, 5, 6, 0),
-	/* LD01 - LDO10 */
-	DA9052_LDO(4, 50, 600, 1800, 5, 6, 0),
-	DA9052_LDO5_6(5, 25, 600, 1800, 6, 6, DA9052_SUPPLY_VLDO2GO),
-	DA9052_LDO5_6(6, 25, 1725, 3300, 6, 6, DA9052_SUPPLY_VLDO3GO),
-	DA9052_LDO(7, 25, 1725, 3300, 6, 6, 0),
-	DA9052_LDO(8, 50, 1200, 3600, 6, 6, 0),
-	DA9052_LDO(9, 50, 1200, 3600, 6, 6, 0),
-	DA9052_LDO(10, 50, 1200, 3600, 6, 6, 0),
-	DA9052_LDO(11, 50, 1200, 3600, 6, 6, 0),
-	DA9052_LDO(12, 50, 1250, 3650, 6, 6, 0),
-	DA9052_LDO(13, 50, 1200, 3600, 6, 6, 0),
+	DA9052_DCDC(BUCK1, 25, 500, 2075, 6, 6, DA9052_SUPPLY_VBCOREGO),
+	DA9052_DCDC(BUCK2, 25, 500, 2075, 6, 6, DA9052_SUPPLY_VBPROGO),
+	DA9052_DCDC(BUCK3, 25, 925, 2500, 6, 6, DA9052_SUPPLY_VBMEMGO),
+	DA9052_DCDC(BUCK4, 50, 1800, 3600, 5, 6, 0),
+	DA9052_LDO(LDO1, 50, 600, 1800, 5, 6, 0),
+	DA9052_LDO(LDO2, 25, 600, 1800, 6, 6, DA9052_SUPPLY_VLDO2GO),
+	DA9052_LDO(LDO3, 25, 1725, 3300, 6, 6, DA9052_SUPPLY_VLDO3GO),
+	DA9052_LDO(LDO4, 25, 1725, 3300, 6, 6, 0),
+	DA9052_LDO(LDO5, 50, 1200, 3600, 6, 6, 0),
+	DA9052_LDO(LDO6, 50, 1200, 3600, 6, 6, 0),
+	DA9052_LDO(LDO7, 50, 1200, 3600, 6, 6, 0),
+	DA9052_LDO(LDO8, 50, 1200, 3600, 6, 6, 0),
+	DA9052_LDO(LDO9, 50, 1250, 3650, 6, 6, 0),
+	DA9052_LDO(LDO10, 50, 1200, 3600, 6, 6, 0),
 };
 
 static struct da9052_regulator_info da9053_regulator_info[] = {
-	/* Buck1 - 4 */
-	DA9052_DCDC(0, 25, 500, 2075, 6, 6, DA9052_SUPPLY_VBCOREGO),
-	DA9052_DCDC(1, 25, 500, 2075, 6, 6, DA9052_SUPPLY_VBPROGO),
-	DA9052_DCDC(2, 25, 925, 2500, 6, 6, DA9052_SUPPLY_VBMEMGO),
-	DA9052_BUCKPERI(3, 25, 925, 2500, 6, 6, 0),
-	/* LD01 - LDO10 */
-	DA9052_LDO(4, 50, 600, 1800, 5, 6, 0),
-	DA9052_LDO5_6(5, 25, 600, 1800, 6, 6, DA9052_SUPPLY_VLDO2GO),
-	DA9052_LDO5_6(6, 25, 1725, 3300, 6, 6, DA9052_SUPPLY_VLDO3GO),
-	DA9052_LDO(7, 25, 1725, 3300, 6, 6, 0),
-	DA9052_LDO(8, 50, 1200, 3600, 6, 6, 0),
-	DA9052_LDO(9, 50, 1200, 3600, 6, 6, 0),
-	DA9052_LDO(10, 50, 1200, 3600, 6, 6, 0),
-	DA9052_LDO(11, 50, 1200, 3600, 6, 6, 0),
-	DA9052_LDO(12, 50, 1250, 3650, 6, 6, 0),
-	DA9052_LDO(13, 50, 1200, 3600, 6, 6, 0),
+	DA9052_DCDC(BUCK1, 25, 500, 2075, 6, 6, DA9052_SUPPLY_VBCOREGO),
+	DA9052_DCDC(BUCK2, 25, 500, 2075, 6, 6, DA9052_SUPPLY_VBPROGO),
+	DA9052_DCDC(BUCK3, 25, 925, 2500, 6, 6, DA9052_SUPPLY_VBMEMGO),
+	DA9052_DCDC(BUCK4, 25, 925, 2500, 6, 6, 0),
+	DA9052_LDO(LDO1, 50, 600, 1800, 5, 6, 0),
+	DA9052_LDO(LDO2, 25, 600, 1800, 6, 6, DA9052_SUPPLY_VLDO2GO),
+	DA9052_LDO(LDO3, 25, 1725, 3300, 6, 6, DA9052_SUPPLY_VLDO3GO),
+	DA9052_LDO(LDO4, 25, 1725, 3300, 6, 6, 0),
+	DA9052_LDO(LDO5, 50, 1200, 3600, 6, 6, 0),
+	DA9052_LDO(LDO6, 50, 1200, 3600, 6, 6, 0),
+	DA9052_LDO(LDO7, 50, 1200, 3600, 6, 6, 0),
+	DA9052_LDO(LDO8, 50, 1200, 3600, 6, 6, 0),
+	DA9052_LDO(LDO9, 50, 1250, 3650, 6, 6, 0),
+	DA9052_LDO(LDO10, 50, 1200, 3600, 6, 6, 0),
 };
 
 static inline struct da9052_regulator_info *find_regulator_info(u8 chip_id,
@@ -533,10 +367,10 @@ static inline struct da9052_regulator_info *find_regulator_info(u8 chip_id,
 
 static int __devinit da9052_regulator_probe(struct platform_device *pdev)
 {
+	struct regulator_config config = { };
 	struct da9052_regulator *regulator;
 	struct da9052 *da9052;
 	struct da9052_pdata *pdata;
-	int ret;
 
 	regulator = devm_kzalloc(&pdev->dev, sizeof(struct da9052_regulator),
 				 GFP_KERNEL);
@@ -551,26 +385,49 @@ static int __devinit da9052_regulator_probe(struct platform_device *pdev)
 					      pdev->id);
 	if (regulator->info == NULL) {
 		dev_err(&pdev->dev, "invalid regulator ID specified\n");
-		ret = -EINVAL;
-		goto err;
+		return -EINVAL;
 	}
+
+	config.dev = &pdev->dev;
+	config.driver_data = regulator;
+	config.regmap = da9052->regmap;
+	if (pdata && pdata->regulators) {
+		config.init_data = pdata->regulators[pdev->id];
+	} else {
+#ifdef CONFIG_OF
+		struct device_node *nproot = da9052->dev->of_node;
+		struct device_node *np;
+
+		if (!nproot)
+			return -ENODEV;
+
+		nproot = of_find_node_by_name(nproot, "regulators");
+		if (!nproot)
+			return -ENODEV;
+
+		for (np = of_get_next_child(nproot, NULL); np;
+		     np = of_get_next_child(nproot, np)) {
+			if (!of_node_cmp(np->name,
+					 regulator->info->reg_desc.name)) {
+				config.init_data = of_get_regulator_init_data(
+					&pdev->dev, np);
+				break;
+			}
+		}
+#endif
+	}
+
 	regulator->rdev = regulator_register(&regulator->info->reg_desc,
-					     &pdev->dev,
-					     pdata->regulators[pdev->id],
-					     regulator, NULL);
+					     &config);
 	if (IS_ERR(regulator->rdev)) {
 		dev_err(&pdev->dev, "failed to register regulator %s\n",
 			regulator->info->reg_desc.name);
-		ret = PTR_ERR(regulator->rdev);
-		goto err;
+		return PTR_ERR(regulator->rdev);
 	}
 
 	platform_set_drvdata(pdev, regulator);
 
 	return 0;
-err:
-	devm_kfree(&pdev->dev, regulator);
-	return ret;
 }
 
 static int __devexit da9052_regulator_remove(struct platform_device *pdev)
@@ -578,8 +435,6 @@ static int __devexit da9052_regulator_remove(struct platform_device *pdev)
 	struct da9052_regulator *regulator = platform_get_drvdata(pdev);
 
 	regulator_unregister(regulator->rdev);
-	devm_kfree(&pdev->dev, regulator);
-
 	return 0;
 }
 
diff --git a/drivers/regulator/db8500-prcmu.c b/drivers/regulator/db8500-prcmu.c
index 4bd25e75efa..968f97f3cb3 100644
--- a/drivers/regulator/db8500-prcmu.c
+++ b/drivers/regulator/db8500-prcmu.c
@@ -17,6 +17,8 @@
 #include <linux/regulator/driver.h>
 #include <linux/regulator/machine.h>
 #include <linux/regulator/db8500-prcmu.h>
+#include <linux/regulator/of_regulator.h>
+#include <linux/of.h>
 #include <linux/module.h>
 #include "dbx500-prcmu.h"
 
@@ -410,45 +412,120 @@ dbx500_regulator_info[DB8500_NUM_REGULATORS] = {
 	},
 };
 
+static __devinit int db8500_regulator_register(struct platform_device *pdev,
+					struct regulator_init_data *init_data,
+					int id,
+					struct device_node *np)
+{
+	struct dbx500_regulator_info *info;
+	struct regulator_config config = { };
+	int err;
+
+	/* assign per-regulator data */
+	info = &dbx500_regulator_info[id];
+	info->dev = &pdev->dev;
+
+	config.dev = &pdev->dev;
+	config.init_data = init_data;
+	config.driver_data = info;
+	config.of_node = np;
+
+	/* register with the regulator framework */
+	info->rdev = regulator_register(&info->desc, &config);
+	if (IS_ERR(info->rdev)) {
+		err = PTR_ERR(info->rdev);
+		dev_err(&pdev->dev, "failed to register %s: err %i\n",
+			info->desc.name, err);
+
+		/* if failing, unregister all earlier regulators */
+		while (--id >= 0) {
+			info = &dbx500_regulator_info[id];
+			regulator_unregister(info->rdev);
+		}
+		return err;
+	}
+
+	dev_dbg(rdev_get_dev(info->rdev),
+		"regulator-%s-probed\n", info->desc.name);
+
+	return 0;
+}
+
+static struct of_regulator_match db8500_regulator_matches[] = {
+	{ .name	= "db8500-vape",          .driver_data = (void *) DB8500_REGULATOR_VAPE, },
+	{ .name	= "db8500-varm",          .driver_data = (void *) DB8500_REGULATOR_VARM, },
+	{ .name	= "db8500-vmodem",        .driver_data = (void *) DB8500_REGULATOR_VMODEM, },
+	{ .name	= "db8500-vpll",          .driver_data = (void *) DB8500_REGULATOR_VPLL, },
+	{ .name	= "db8500-vsmps1",        .driver_data = (void *) DB8500_REGULATOR_VSMPS1, },
+	{ .name	= "db8500-vsmps2",        .driver_data = (void *) DB8500_REGULATOR_VSMPS2, },
+	{ .name	= "db8500-vsmps3",        .driver_data = (void *) DB8500_REGULATOR_VSMPS3, },
+	{ .name	= "db8500-vrf1",          .driver_data = (void *) DB8500_REGULATOR_VRF1, },
+	{ .name	= "db8500-sva-mmdsp",     .driver_data = (void *) DB8500_REGULATOR_SWITCH_SVAMMDSP, },
+	{ .name	= "db8500-sva-mmdsp-ret", .driver_data = (void *) DB8500_REGULATOR_SWITCH_SVAMMDSPRET, },
+	{ .name	= "db8500-sva-pipe",      .driver_data = (void *) DB8500_REGULATOR_SWITCH_SVAPIPE, },
+	{ .name	= "db8500-sia-mmdsp",     .driver_data = (void *) DB8500_REGULATOR_SWITCH_SIAMMDSP, },
+	{ .name	= "db8500-sia-mmdsp-ret", .driver_data = (void *) DB8500_REGULATOR_SWITCH_SIAMMDSPRET, },
+	{ .name	= "db8500-sia-pipe",      .driver_data = (void *) DB8500_REGULATOR_SWITCH_SIAPIPE, },
+	{ .name	= "db8500-sga",           .driver_data = (void *) DB8500_REGULATOR_SWITCH_SGA, },
+	{ .name	= "db8500-b2r2-mcde",     .driver_data = (void *) DB8500_REGULATOR_SWITCH_B2R2_MCDE, },
+	{ .name	= "db8500-esram12",       .driver_data = (void *) DB8500_REGULATOR_SWITCH_ESRAM12, },
+	{ .name	= "db8500-esram12-ret",   .driver_data = (void *) DB8500_REGULATOR_SWITCH_ESRAM12RET, },
+	{ .name	= "db8500-esram34",       .driver_data = (void *) DB8500_REGULATOR_SWITCH_ESRAM34, },
+	{ .name	= "db8500-esram34-ret",   .driver_data = (void *) DB8500_REGULATOR_SWITCH_ESRAM34RET, },
+};
+
+static __devinit int
+db8500_regulator_of_probe(struct platform_device *pdev,
+			struct device_node *np)
+{
+	int i, err;
+
+	for (i = 0; i < ARRAY_SIZE(dbx500_regulator_info); i++) {
+		err = db8500_regulator_register(
+			pdev, db8500_regulator_matches[i].init_data,
+			i, db8500_regulator_matches[i].of_node);
+		if (err)
+			return err;
+	}
+
+	return 0;
+}
+
 static int __devinit db8500_regulator_probe(struct platform_device *pdev)
 {
 	struct regulator_init_data *db8500_init_data =
 					dev_get_platdata(&pdev->dev);
+	struct device_node *np = pdev->dev.of_node;
 	int i, err;
 
 	/* register all regulators */
-	for (i = 0; i < ARRAY_SIZE(dbx500_regulator_info); i++) {
-		struct dbx500_regulator_info *info;
-		struct regulator_init_data *init_data = &db8500_init_data[i];
-
-		/* assign per-regulator data */
-		info = &dbx500_regulator_info[i];
-		info->dev = &pdev->dev;
-
-		/* register with the regulator framework */
-		info->rdev = regulator_register(&info->desc, &pdev->dev,
-				init_data, info, NULL);
-		if (IS_ERR(info->rdev)) {
-			err = PTR_ERR(info->rdev);
-			dev_err(&pdev->dev, "failed to register %s: err %i\n",
-				info->desc.name, err);
-
-			/* if failing, unregister all earlier regulators */
-			while (--i >= 0) {
-				info = &dbx500_regulator_info[i];
-				regulator_unregister(info->rdev);
-			}
+	if (np) {
+		err = of_regulator_match(&pdev->dev, np,
+					db8500_regulator_matches,
+					ARRAY_SIZE(db8500_regulator_matches));
+		if (err < 0) {
+			dev_err(&pdev->dev,
+				"Error parsing regulator init data: %d\n", err);
 			return err;
 		}
 
-		dev_dbg(rdev_get_dev(info->rdev),
-			"regulator-%s-probed\n", info->desc.name);
+		err = db8500_regulator_of_probe(pdev, np);
+		if (err)
+			return err;
+	} else {
+		for (i = 0; i < ARRAY_SIZE(dbx500_regulator_info); i++) {
+			err = db8500_regulator_register(pdev,
+							&db8500_init_data[i],
+							i, NULL);
+			if (err)
+				return err;
+		}
 	}
+
 	err = ux500_regulator_debug_init(pdev,
 					 dbx500_regulator_info,
 					 ARRAY_SIZE(dbx500_regulator_info));
-
-	return err;
+	return 0;
 }
 
 static int __exit db8500_regulator_remove(struct platform_device *pdev)
@@ -470,10 +547,16 @@ static int __exit db8500_regulator_remove(struct platform_device *pdev)
 	return 0;
 }
 
+static const struct of_device_id db8500_prcmu_regulator_match[] = {
+        { .compatible = "stericsson,db8500-prcmu-regulator", },
+        {}
+};
+
 static struct platform_driver db8500_regulator_driver = {
 	.driver = {
 		.name = "db8500-prcmu-regulators",
 		.owner = THIS_MODULE,
+		.of_match_table = db8500_prcmu_regulator_match,
 	},
 	.probe = db8500_regulator_probe,
 	.remove = __exit_p(db8500_regulator_remove),
diff --git a/drivers/regulator/dummy.c b/drivers/regulator/dummy.c
index 0ee00de4be7..86f655c7f7a 100644
--- a/drivers/regulator/dummy.c
+++ b/drivers/regulator/dummy.c
@@ -39,10 +39,13 @@ static struct regulator_desc dummy_desc = {
 
 static int __devinit dummy_regulator_probe(struct platform_device *pdev)
 {
+	struct regulator_config config = { };
 	int ret;
 
-	dummy_regulator_rdev = regulator_register(&dummy_desc, NULL,
-						  &dummy_initdata, NULL, NULL);
+	config.dev = &pdev->dev;
+	config.init_data = &dummy_initdata;
+
+	dummy_regulator_rdev = regulator_register(&dummy_desc, &config);
 	if (IS_ERR(dummy_regulator_rdev)) {
 		ret = PTR_ERR(dummy_regulator_rdev);
 		pr_err("Failed to register regulator: %d\n", ret);
diff --git a/drivers/regulator/fixed.c b/drivers/regulator/fixed.c
index 40f38030b39..f09fe7b20e8 100644
--- a/drivers/regulator/fixed.c
+++ b/drivers/regulator/fixed.c
@@ -25,7 +25,6 @@
 #include <linux/regulator/driver.h>
 #include <linux/regulator/fixed.h>
 #include <linux/gpio.h>
-#include <linux/delay.h>
 #include <linux/slab.h>
 #include <linux/of.h>
 #include <linux/of_gpio.h>
@@ -91,6 +90,9 @@ of_get_fixed_voltage_config(struct device *dev)
 	if (of_find_property(np, "enable-active-high", NULL))
 		config->enable_high = true;
 
+	if (of_find_property(np, "gpio-open-drain", NULL))
+		config->gpio_is_open_drain = true;
+
 	return config;
 }
 
@@ -105,10 +107,8 @@ static int fixed_voltage_enable(struct regulator_dev *dev)
 {
 	struct fixed_voltage_data *data = rdev_get_drvdata(dev);
 
-	if (gpio_is_valid(data->gpio)) {
-		gpio_set_value_cansleep(data->gpio, data->enable_high);
-		data->is_enabled = true;
-	}
+	gpio_set_value_cansleep(data->gpio, data->enable_high);
+	data->is_enabled = true;
 
 	return 0;
 }
@@ -117,10 +117,8 @@ static int fixed_voltage_disable(struct regulator_dev *dev)
 {
 	struct fixed_voltage_data *data = rdev_get_drvdata(dev);
 
-	if (gpio_is_valid(data->gpio)) {
-		gpio_set_value_cansleep(data->gpio, !data->enable_high);
-		data->is_enabled = false;
-	}
+	gpio_set_value_cansleep(data->gpio, !data->enable_high);
+	data->is_enabled = false;
 
 	return 0;
 }
@@ -153,7 +151,7 @@ static int fixed_voltage_list_voltage(struct regulator_dev *dev,
 	return data->microvolts;
 }
 
-static struct regulator_ops fixed_voltage_ops = {
+static struct regulator_ops fixed_voltage_gpio_ops = {
 	.is_enabled = fixed_voltage_is_enabled,
 	.enable = fixed_voltage_enable,
 	.disable = fixed_voltage_disable,
@@ -162,10 +160,16 @@ static struct regulator_ops fixed_voltage_ops = {
 	.list_voltage = fixed_voltage_list_voltage,
 };
 
+static struct regulator_ops fixed_voltage_ops = {
+	.get_voltage = fixed_voltage_get_voltage,
+	.list_voltage = fixed_voltage_list_voltage,
+};
+
 static int __devinit reg_fixed_voltage_probe(struct platform_device *pdev)
 {
 	struct fixed_voltage_config *config;
 	struct fixed_voltage_data *drvdata;
+	struct regulator_config cfg = { };
 	int ret;
 
 	if (pdev->dev.of_node)
@@ -176,7 +180,8 @@ static int __devinit reg_fixed_voltage_probe(struct platform_device *pdev)
 	if (!config)
 		return -ENOMEM;
 
-	drvdata = kzalloc(sizeof(struct fixed_voltage_data), GFP_KERNEL);
+	drvdata = devm_kzalloc(&pdev->dev, sizeof(struct fixed_voltage_data),
+			       GFP_KERNEL);
 	if (drvdata == NULL) {
 		dev_err(&pdev->dev, "Failed to allocate device data\n");
 		ret = -ENOMEM;
@@ -191,7 +196,6 @@ static int __devinit reg_fixed_voltage_probe(struct platform_device *pdev)
 	}
 	drvdata->desc.type = REGULATOR_VOLTAGE;
 	drvdata->desc.owner = THIS_MODULE;
-	drvdata->desc.ops = &fixed_voltage_ops;
 
 	if (config->microvolts)
 		drvdata->desc.n_voltages = 1;
@@ -201,6 +205,7 @@ static int __devinit reg_fixed_voltage_probe(struct platform_device *pdev)
 	drvdata->startup_delay = config->startup_delay;
 
 	if (gpio_is_valid(config->gpio)) {
+		int gpio_flag;
 		drvdata->enable_high = config->enable_high;
 
 		/* FIXME: Remove below print warning
@@ -218,39 +223,39 @@ static int __devinit reg_fixed_voltage_probe(struct platform_device *pdev)
 			dev_warn(&pdev->dev,
 				"using GPIO 0 for regulator enable control\n");
 
-		ret = gpio_request(config->gpio, config->supply_name);
-		if (ret) {
-			dev_err(&pdev->dev,
-			   "Could not obtain regulator enable GPIO %d: %d\n",
-							config->gpio, ret);
-			goto err_name;
-		}
-
-		/* set output direction without changing state
+		/*
+		 * set output direction without changing state
 		 * to prevent glitch
 		 */
 		drvdata->is_enabled = config->enabled_at_boot;
 		ret = drvdata->is_enabled ?
 				config->enable_high : !config->enable_high;
+		gpio_flag = ret ? GPIOF_OUT_INIT_HIGH : GPIOF_OUT_INIT_LOW;
+
+		if (config->gpio_is_open_drain)
+			gpio_flag |= GPIOF_OPEN_DRAIN;
 
-		ret = gpio_direction_output(config->gpio, ret);
+		ret = gpio_request_one(config->gpio, gpio_flag,
+						config->supply_name);
 		if (ret) {
 			dev_err(&pdev->dev,
-			   "Could not configure regulator enable GPIO %d direction: %d\n",
+			   "Could not obtain regulator enable GPIO %d: %d\n",
 							config->gpio, ret);
-			goto err_gpio;
+			goto err_name;
 		}
 
+		drvdata->desc.ops = &fixed_voltage_gpio_ops;
+
 	} else {
-		/* Regulator without GPIO control is considered
-		 * always enabled
-		 */
-		drvdata->is_enabled = true;
+		drvdata->desc.ops = &fixed_voltage_ops;
 	}
 
-	drvdata->dev = regulator_register(&drvdata->desc, &pdev->dev,
-					  config->init_data, drvdata,
-					  pdev->dev.of_node);
+	cfg.dev = &pdev->dev;
+	cfg.init_data = config->init_data;
+	cfg.driver_data = drvdata;
+	cfg.of_node = pdev->dev.of_node;
+
+	drvdata->dev = regulator_register(&drvdata->desc, &cfg);
 	if (IS_ERR(drvdata->dev)) {
 		ret = PTR_ERR(drvdata->dev);
 		dev_err(&pdev->dev, "Failed to register regulator: %d\n", ret);
@@ -270,7 +275,6 @@ err_gpio:
 err_name:
 	kfree(drvdata->desc.name);
 err:
-	kfree(drvdata);
 	return ret;
 }
 
@@ -282,7 +286,6 @@ static int __devexit reg_fixed_voltage_remove(struct platform_device *pdev)
 	if (gpio_is_valid(drvdata->gpio))
 		gpio_free(drvdata->gpio);
 	kfree(drvdata->desc.name);
-	kfree(drvdata);
 
 	return 0;
 }
diff --git a/drivers/regulator/gpio-regulator.c b/drivers/regulator/gpio-regulator.c
index 42e1cb1835e..9997d7aaca8 100644
--- a/drivers/regulator/gpio-regulator.c
+++ b/drivers/regulator/gpio-regulator.c
@@ -30,7 +30,6 @@
 #include <linux/regulator/machine.h>
 #include <linux/regulator/gpio-regulator.h>
 #include <linux/gpio.h>
-#include <linux/delay.h>
 #include <linux/slab.h>
 
 struct gpio_regulator_data {
@@ -105,15 +104,15 @@ static int gpio_regulator_set_value(struct regulator_dev *dev,
 					int min, int max)
 {
 	struct gpio_regulator_data *data = rdev_get_drvdata(dev);
-	int ptr, target, state;
+	int ptr, target, state, best_val = INT_MAX;
 
-	target = -1;
 	for (ptr = 0; ptr < data->nr_states; ptr++)
-		if (data->states[ptr].value >= min &&
+		if (data->states[ptr].value < best_val &&
+		    data->states[ptr].value >= min &&
 		    data->states[ptr].value <= max)
 			target = data->states[ptr].gpios;
 
-	if (target < 0)
+	if (best_val == INT_MAX)
 		return -EINVAL;
 
 	for (ptr = 0; ptr < data->nr_gpios; ptr++) {
@@ -172,9 +171,11 @@ static int __devinit gpio_regulator_probe(struct platform_device *pdev)
 {
 	struct gpio_regulator_config *config = pdev->dev.platform_data;
 	struct gpio_regulator_data *drvdata;
+	struct regulator_config cfg = { };
 	int ptr, ret, state;
 
-	drvdata = kzalloc(sizeof(struct gpio_regulator_data), GFP_KERNEL);
+	drvdata = devm_kzalloc(&pdev->dev, sizeof(struct gpio_regulator_data),
+			       GFP_KERNEL);
 	if (drvdata == NULL) {
 		dev_err(&pdev->dev, "Failed to allocate device data\n");
 		return -ENOMEM;
@@ -283,8 +284,11 @@ static int __devinit gpio_regulator_probe(struct platform_device *pdev)
 	}
 	drvdata->state = state;
 
-	drvdata->dev = regulator_register(&drvdata->desc, &pdev->dev,
-					  config->init_data, drvdata, NULL);
+	cfg.dev = &pdev->dev;
+	cfg.init_data = config->init_data;
+	cfg.driver_data = &drvdata;
+
+	drvdata->dev = regulator_register(&drvdata->desc, &cfg);
 	if (IS_ERR(drvdata->dev)) {
 		ret = PTR_ERR(drvdata->dev);
 		dev_err(&pdev->dev, "Failed to register regulator: %d\n", ret);
@@ -307,7 +311,6 @@ err_memgpio:
 err_name:
 	kfree(drvdata->desc.name);
 err:
-	kfree(drvdata);
 	return ret;
 }
 
@@ -326,7 +329,6 @@ static int __devexit gpio_regulator_remove(struct platform_device *pdev)
 		gpio_free(drvdata->enable_gpio);
 
 	kfree(drvdata->desc.name);
-	kfree(drvdata);
 
 	return 0;
 }
diff --git a/drivers/regulator/isl6271a-regulator.c b/drivers/regulator/isl6271a-regulator.c
index 775f5fd208c..56d273f2560 100644
--- a/drivers/regulator/isl6271a-regulator.c
+++ b/drivers/regulator/isl6271a-regulator.c
@@ -22,7 +22,6 @@
 #include <linux/platform_device.h>
 #include <linux/regulator/driver.h>
 #include <linux/i2c.h>
-#include <linux/delay.h>
 #include <linux/slab.h>
 
 #define	ISL6271A_VOLTAGE_MIN	850000
@@ -36,47 +35,30 @@ struct isl_pmic {
 	struct mutex		mtx;
 };
 
-static int isl6271a_get_voltage(struct regulator_dev *dev)
+static int isl6271a_get_voltage_sel(struct regulator_dev *dev)
 {
 	struct isl_pmic *pmic = rdev_get_drvdata(dev);
-	int idx, data;
+	int idx;
 
 	mutex_lock(&pmic->mtx);
 
 	idx = i2c_smbus_read_byte(pmic->client);
-	if (idx < 0) {
+	if (idx < 0)
 		dev_err(&pmic->client->dev, "Error getting voltage\n");
-		data = idx;
-		goto out;
-	}
-
-	/* Convert the data from chip to microvolts */
-	data = ISL6271A_VOLTAGE_MIN + (ISL6271A_VOLTAGE_STEP * (idx & 0xf));
 
-out:
 	mutex_unlock(&pmic->mtx);
-	return data;
+	return idx;
 }
 
-static int isl6271a_set_voltage(struct regulator_dev *dev,
-				int minuV, int maxuV,
-				unsigned *selector)
+static int isl6271a_set_voltage_sel(struct regulator_dev *dev,
+				    unsigned selector)
 {
 	struct isl_pmic *pmic = rdev_get_drvdata(dev);
-	int err, data;
-
-	if (minuV < ISL6271A_VOLTAGE_MIN || minuV > ISL6271A_VOLTAGE_MAX)
-		return -EINVAL;
-	if (maxuV < ISL6271A_VOLTAGE_MIN || maxuV > ISL6271A_VOLTAGE_MAX)
-		return -EINVAL;
-
-	data = DIV_ROUND_UP(minuV - ISL6271A_VOLTAGE_MIN,
-			    ISL6271A_VOLTAGE_STEP);
-	*selector = data;
+	int err;
 
 	mutex_lock(&pmic->mtx);
 
-	err = i2c_smbus_write_byte(pmic->client, data);
+	err = i2c_smbus_write_byte(pmic->client, selector);
 	if (err < 0)
 		dev_err(&pmic->client->dev, "Error setting voltage\n");
 
@@ -84,15 +66,11 @@ static int isl6271a_set_voltage(struct regulator_dev *dev,
 	return err;
 }
 
-static int isl6271a_list_voltage(struct regulator_dev *dev, unsigned selector)
-{
-	return ISL6271A_VOLTAGE_MIN + (ISL6271A_VOLTAGE_STEP * selector);
-}
-
 static struct regulator_ops isl_core_ops = {
-	.get_voltage	= isl6271a_get_voltage,
-	.set_voltage	= isl6271a_set_voltage,
-	.list_voltage	= isl6271a_list_voltage,
+	.get_voltage_sel = isl6271a_get_voltage_sel,
+	.set_voltage_sel = isl6271a_set_voltage_sel,
+	.list_voltage	= regulator_list_voltage_linear,
+	.map_voltage	= regulator_map_voltage_linear,
 };
 
 static int isl6271a_get_fixed_voltage(struct regulator_dev *dev)
@@ -112,7 +90,7 @@ static struct regulator_ops isl_fixed_ops = {
 	.list_voltage	= isl6271a_list_fixed_voltage,
 };
 
-static struct regulator_desc isl_rd[] = {
+static const struct regulator_desc isl_rd[] = {
 	{
 		.name		= "Core Buck",
 		.id		= 0,
@@ -120,6 +98,8 @@ static struct regulator_desc isl_rd[] = {
 		.ops		= &isl_core_ops,
 		.type		= REGULATOR_VOLTAGE,
 		.owner		= THIS_MODULE,
+		.min_uV		= ISL6271A_VOLTAGE_MIN,
+		.uV_step	= ISL6271A_VOLTAGE_STEP,
 	}, {
 		.name		= "LDO1",
 		.id		= 1,
@@ -140,6 +120,7 @@ static struct regulator_desc isl_rd[] = {
 static int __devinit isl6271a_probe(struct i2c_client *i2c,
 				     const struct i2c_device_id *id)
 {
+	struct regulator_config config = { };
 	struct regulator_init_data *init_data	= i2c->dev.platform_data;
 	struct isl_pmic *pmic;
 	int err, i;
@@ -147,12 +128,7 @@ static int __devinit isl6271a_probe(struct i2c_client *i2c,
 	if (!i2c_check_functionality(i2c->adapter, I2C_FUNC_SMBUS_BYTE_DATA))
 		return -EIO;
 
-	if (!init_data) {
-		dev_err(&i2c->dev, "no platform data supplied\n");
-		return -EIO;
-	}
-
-	pmic = kzalloc(sizeof(struct isl_pmic), GFP_KERNEL);
+	pmic = devm_kzalloc(&i2c->dev, sizeof(struct isl_pmic), GFP_KERNEL);
 	if (!pmic)
 		return -ENOMEM;
 
@@ -161,8 +137,14 @@ static int __devinit isl6271a_probe(struct i2c_client *i2c,
 	mutex_init(&pmic->mtx);
 
 	for (i = 0; i < 3; i++) {
-		pmic->rdev[i] = regulator_register(&isl_rd[i], &i2c->dev,
-						init_data, pmic, NULL);
+		config.dev = &i2c->dev;
+		if (i == 0)
+			config.init_data = init_data;
+		else
+			config.init_data = 0;
+		config.driver_data = pmic;
+
+		pmic->rdev[i] = regulator_register(&isl_rd[i], &config);
 		if (IS_ERR(pmic->rdev[i])) {
 			dev_err(&i2c->dev, "failed to register %s\n", id->name);
 			err = PTR_ERR(pmic->rdev[i]);
@@ -177,8 +159,6 @@ static int __devinit isl6271a_probe(struct i2c_client *i2c,
 error:
 	while (--i >= 0)
 		regulator_unregister(pmic->rdev[i]);
-
-	kfree(pmic);
 	return err;
 }
 
@@ -189,9 +169,6 @@ static int __devexit isl6271a_remove(struct i2c_client *i2c)
 
 	for (i = 0; i < 3; i++)
 		regulator_unregister(pmic->rdev[i]);
-
-	kfree(pmic);
-
 	return 0;
 }
 
diff --git a/drivers/regulator/lp3971.c b/drivers/regulator/lp3971.c
index 0cfabd318a5..981bea9cb9d 100644
--- a/drivers/regulator/lp3971.c
+++ b/drivers/regulator/lp3971.c
@@ -124,6 +124,10 @@ static const int *ldo_voltage_map[] = {
 static int lp3971_ldo_list_voltage(struct regulator_dev *dev, unsigned index)
 {
 	int ldo = rdev_get_id(dev) - LP3971_LDO1;
+
+	if (index > LDO_VOL_MAX_IDX)
+		return -EINVAL;
+
 	return 1000 * LDO_VOL_VALUE_MAP(ldo)[index];
 }
 
@@ -168,32 +172,15 @@ static int lp3971_ldo_get_voltage(struct regulator_dev *dev)
 	return 1000 * LDO_VOL_VALUE_MAP(ldo)[val];
 }
 
-static int lp3971_ldo_set_voltage(struct regulator_dev *dev,
-				  int min_uV, int max_uV,
-				  unsigned int *selector)
+static int lp3971_ldo_set_voltage_sel(struct regulator_dev *dev,
+				      unsigned int selector)
 {
 	struct lp3971 *lp3971 = rdev_get_drvdata(dev);
 	int ldo = rdev_get_id(dev) - LP3971_LDO1;
-	int min_vol = min_uV / 1000, max_vol = max_uV / 1000;
-	const int *vol_map = LDO_VOL_VALUE_MAP(ldo);
-	u16 val;
-
-	if (min_vol < vol_map[LDO_VOL_MIN_IDX] ||
-	    min_vol > vol_map[LDO_VOL_MAX_IDX])
-		return -EINVAL;
-
-	for (val = LDO_VOL_MIN_IDX; val <= LDO_VOL_MAX_IDX; val++)
-		if (vol_map[val] >= min_vol)
-			break;
-
-	if (val > LDO_VOL_MAX_IDX || vol_map[val] > max_vol)
-		return -EINVAL;
-
-	*selector = val;
 
 	return lp3971_set_bits(lp3971, LP3971_LDO_VOL_CONTR_REG(ldo),
 			LDO_VOL_CONTR_MASK << LDO_VOL_CONTR_SHIFT(ldo),
-			val << LDO_VOL_CONTR_SHIFT(ldo));
+			selector << LDO_VOL_CONTR_SHIFT(ldo));
 }
 
 static struct regulator_ops lp3971_ldo_ops = {
@@ -202,11 +189,14 @@ static struct regulator_ops lp3971_ldo_ops = {
 	.enable = lp3971_ldo_enable,
 	.disable = lp3971_ldo_disable,
 	.get_voltage = lp3971_ldo_get_voltage,
-	.set_voltage = lp3971_ldo_set_voltage,
+	.set_voltage_sel = lp3971_ldo_set_voltage_sel,
 };
 
 static int lp3971_dcdc_list_voltage(struct regulator_dev *dev, unsigned index)
 {
+	if (index < BUCK_TARGET_VOL_MIN_IDX || index > BUCK_TARGET_VOL_MAX_IDX)
+		return -EINVAL;
+
 	return 1000 * buck_voltage_map[index];
 }
 
@@ -259,33 +249,15 @@ static int lp3971_dcdc_get_voltage(struct regulator_dev *dev)
 	return val;
 }
 
-static int lp3971_dcdc_set_voltage(struct regulator_dev *dev,
-				   int min_uV, int max_uV,
-				   unsigned int *selector)
+static int lp3971_dcdc_set_voltage_sel(struct regulator_dev *dev,
+				       unsigned int selector)
 {
 	struct lp3971 *lp3971 = rdev_get_drvdata(dev);
 	int buck = rdev_get_id(dev) - LP3971_DCDC1;
-	int min_vol = min_uV / 1000, max_vol = max_uV / 1000;
-	const int *vol_map = buck_voltage_map;
-	u16 val;
 	int ret;
 
-	if (min_vol < vol_map[BUCK_TARGET_VOL_MIN_IDX] ||
-	    min_vol > vol_map[BUCK_TARGET_VOL_MAX_IDX])
-		return -EINVAL;
-
-	for (val = BUCK_TARGET_VOL_MIN_IDX; val <= BUCK_TARGET_VOL_MAX_IDX;
-	     val++)
-		if (vol_map[val] >= min_vol)
-			break;
-
-	if (val > BUCK_TARGET_VOL_MAX_IDX || vol_map[val] > max_vol)
-		return -EINVAL;
-
-	*selector = val;
-
 	ret = lp3971_set_bits(lp3971, LP3971_BUCK_TARGET_VOL1_REG(buck),
-	       BUCK_TARGET_VOL_MASK, val);
+	       BUCK_TARGET_VOL_MASK, selector);
 	if (ret)
 		return ret;
 
@@ -306,10 +278,10 @@ static struct regulator_ops lp3971_dcdc_ops = {
 	.enable = lp3971_dcdc_enable,
 	.disable = lp3971_dcdc_disable,
 	.get_voltage = lp3971_dcdc_get_voltage,
-	.set_voltage = lp3971_dcdc_set_voltage,
+	.set_voltage_sel = lp3971_dcdc_set_voltage_sel,
 };
 
-static struct regulator_desc regulators[] = {
+static const struct regulator_desc regulators[] = {
 	{
 		.name = "LDO1",
 		.id = LP3971_LDO1,
@@ -449,10 +421,15 @@ static int __devinit setup_regulators(struct lp3971 *lp3971,
 
 	/* Instantiate the regulators */
 	for (i = 0; i < pdata->num_regulators; i++) {
+		struct regulator_config config = { };
 		struct lp3971_regulator_subdev *reg = &pdata->regulators[i];
-		lp3971->rdev[i] = regulator_register(&regulators[reg->id],
-				lp3971->dev, reg->initdata, lp3971, NULL);
 
+		config.dev = lp3971->dev;
+		config.init_data = reg->initdata;
+		config.driver_data = lp3971;
+
+		lp3971->rdev[i] = regulator_register(&regulators[reg->id],
+						     &config);
 		if (IS_ERR(lp3971->rdev[i])) {
 			err = PTR_ERR(lp3971->rdev[i]);
 			dev_err(lp3971->dev, "regulator init failed: %d\n",
@@ -545,23 +522,7 @@ static struct i2c_driver lp3971_i2c_driver = {
 	.id_table = lp3971_i2c_id,
 };
 
-static int __init lp3971_module_init(void)
-{
-	int ret;
-
-	ret = i2c_add_driver(&lp3971_i2c_driver);
-	if (ret != 0)
-		pr_err("Failed to register I2C driver: %d\n", ret);
-
-	return ret;
-}
-module_init(lp3971_module_init);
-
-static void __exit lp3971_module_exit(void)
-{
-	i2c_del_driver(&lp3971_i2c_driver);
-}
-module_exit(lp3971_module_exit);
+module_i2c_driver(lp3971_i2c_driver);
 
 MODULE_LICENSE("GPL");
 MODULE_AUTHOR("Marek Szyprowski <m.szyprowski@samsung.com>");
diff --git a/drivers/regulator/lp3972.c b/drivers/regulator/lp3972.c
index 49a15eefe5f..de073df7d34 100644
--- a/drivers/regulator/lp3972.c
+++ b/drivers/regulator/lp3972.c
@@ -245,6 +245,11 @@ static int lp3972_set_bits(struct lp3972 *lp3972, u8 reg, u16 mask, u16 val)
 static int lp3972_ldo_list_voltage(struct regulator_dev *dev, unsigned index)
 {
 	int ldo = rdev_get_id(dev) - LP3972_LDO1;
+
+	if (index < LP3972_LDO_VOL_MIN_IDX(ldo) ||
+	    index > LP3972_LDO_VOL_MAX_IDX(ldo))
+		return -EINVAL;
+
 	return 1000 * LP3972_LDO_VOL_VALUE_MAP(ldo)[index];
 }
 
@@ -292,34 +297,16 @@ static int lp3972_ldo_get_voltage(struct regulator_dev *dev)
 	return 1000 * LP3972_LDO_VOL_VALUE_MAP(ldo)[val];
 }
 
-static int lp3972_ldo_set_voltage(struct regulator_dev *dev,
-				  int min_uV, int max_uV,
-				  unsigned int *selector)
+static int lp3972_ldo_set_voltage_sel(struct regulator_dev *dev,
+				      unsigned int selector)
 {
 	struct lp3972 *lp3972 = rdev_get_drvdata(dev);
 	int ldo = rdev_get_id(dev) - LP3972_LDO1;
-	int min_vol = min_uV / 1000, max_vol = max_uV / 1000;
-	const int *vol_map = LP3972_LDO_VOL_VALUE_MAP(ldo);
-	u16 val;
 	int shift, ret;
 
-	if (min_vol < vol_map[LP3972_LDO_VOL_MIN_IDX(ldo)] ||
-	    min_vol > vol_map[LP3972_LDO_VOL_MAX_IDX(ldo)])
-		return -EINVAL;
-
-	for (val = LP3972_LDO_VOL_MIN_IDX(ldo);
-		val <= LP3972_LDO_VOL_MAX_IDX(ldo); val++)
-		if (vol_map[val] >= min_vol)
-			break;
-
-	if (val > LP3972_LDO_VOL_MAX_IDX(ldo) || vol_map[val] > max_vol)
-		return -EINVAL;
-
-	*selector = val;
-
 	shift = LP3972_LDO_VOL_CONTR_SHIFT(ldo);
 	ret = lp3972_set_bits(lp3972, LP3972_LDO_VOL_CONTR_REG(ldo),
-		LP3972_LDO_VOL_MASK(ldo) << shift, val << shift);
+		LP3972_LDO_VOL_MASK(ldo) << shift, selector << shift);
 
 	if (ret)
 		return ret;
@@ -355,12 +342,17 @@ static struct regulator_ops lp3972_ldo_ops = {
 	.enable = lp3972_ldo_enable,
 	.disable = lp3972_ldo_disable,
 	.get_voltage = lp3972_ldo_get_voltage,
-	.set_voltage = lp3972_ldo_set_voltage,
+	.set_voltage_sel = lp3972_ldo_set_voltage_sel,
 };
 
 static int lp3972_dcdc_list_voltage(struct regulator_dev *dev, unsigned index)
 {
 	int buck = rdev_get_id(dev) - LP3972_DCDC1;
+
+	if (index < LP3972_BUCK_VOL_MIN_IDX(buck) ||
+	    index > LP3972_BUCK_VOL_MAX_IDX(buck))
+		return -EINVAL;
+
 	return 1000 * buck_voltage_map[buck][index];
 }
 
@@ -419,34 +411,15 @@ static int lp3972_dcdc_get_voltage(struct regulator_dev *dev)
 	return val;
 }
 
-static int lp3972_dcdc_set_voltage(struct regulator_dev *dev,
-				   int min_uV, int max_uV,
-				   unsigned int *selector)
+static int lp3972_dcdc_set_voltage_sel(struct regulator_dev *dev,
+				       unsigned int selector)
 {
 	struct lp3972 *lp3972 = rdev_get_drvdata(dev);
 	int buck = rdev_get_id(dev) - LP3972_DCDC1;
-	int min_vol = min_uV / 1000, max_vol = max_uV / 1000;
-	const int *vol_map = buck_voltage_map[buck];
-	u16 val;
 	int ret;
 
-	if (min_vol < vol_map[LP3972_BUCK_VOL_MIN_IDX(buck)] ||
-	    min_vol > vol_map[LP3972_BUCK_VOL_MAX_IDX(buck)])
-		return -EINVAL;
-
-	for (val = LP3972_BUCK_VOL_MIN_IDX(buck);
-		val <= LP3972_BUCK_VOL_MAX_IDX(buck); val++)
-		if (vol_map[val] >= min_vol)
-			break;
-
-	if (val > LP3972_BUCK_VOL_MAX_IDX(buck) ||
-	    vol_map[val] > max_vol)
-		return -EINVAL;
-
-	*selector = val;
-
 	ret = lp3972_set_bits(lp3972, LP3972_BUCK_VOL1_REG(buck),
-				LP3972_BUCK_VOL_MASK, val);
+				LP3972_BUCK_VOL_MASK, selector);
 	if (ret)
 		return ret;
 
@@ -468,10 +441,10 @@ static struct regulator_ops lp3972_dcdc_ops = {
 	.enable = lp3972_dcdc_enable,
 	.disable = lp3972_dcdc_disable,
 	.get_voltage = lp3972_dcdc_get_voltage,
-	.set_voltage = lp3972_dcdc_set_voltage,
+	.set_voltage_sel = lp3972_dcdc_set_voltage_sel,
 };
 
-static struct regulator_desc regulators[] = {
+static const struct regulator_desc regulators[] = {
 	{
 		.name = "LDO1",
 		.id = LP3972_LDO1,
@@ -554,9 +527,14 @@ static int __devinit setup_regulators(struct lp3972 *lp3972,
 	/* Instantiate the regulators */
 	for (i = 0; i < pdata->num_regulators; i++) {
 		struct lp3972_regulator_subdev *reg = &pdata->regulators[i];
-		lp3972->rdev[i] = regulator_register(&regulators[reg->id],
-				lp3972->dev, reg->initdata, lp3972, NULL);
+		struct regulator_config config = { };
 
+		config.dev = lp3972->dev;
+		config.init_data = reg->initdata;
+		config.driver_data = lp3972;
+
+		lp3972->rdev[i] = regulator_register(&regulators[reg->id],
+						     &config);
 		if (IS_ERR(lp3972->rdev[i])) {
 			err = PTR_ERR(lp3972->rdev[i]);
 			dev_err(lp3972->dev, "regulator init failed: %d\n",
diff --git a/drivers/regulator/max1586.c b/drivers/regulator/max1586.c
index 282d2ee0604..b9444ee08da 100644
--- a/drivers/regulator/max1586.c
+++ b/drivers/regulator/max1586.c
@@ -161,7 +161,7 @@ static struct regulator_ops max1586_v6_ops = {
 	.list_voltage = max1586_v6_list,
 };
 
-static struct regulator_desc max1586_reg[] = {
+static const struct regulator_desc max1586_reg[] = {
 	{
 		.name = "Output_V3",
 		.id = MAX1586_V3,
@@ -185,21 +185,21 @@ static int __devinit max1586_pmic_probe(struct i2c_client *client,
 {
 	struct regulator_dev **rdev;
 	struct max1586_platform_data *pdata = client->dev.platform_data;
+	struct regulator_config config = { };
 	struct max1586_data *max1586;
 	int i, id, ret = -ENOMEM;
 
-	max1586 = kzalloc(sizeof(struct max1586_data) +
+	max1586 = devm_kzalloc(&client->dev, sizeof(struct max1586_data) +
 			sizeof(struct regulator_dev *) * (MAX1586_V6 + 1),
 			GFP_KERNEL);
 	if (!max1586)
-		goto out;
+		return -ENOMEM;
 
 	max1586->client = client;
 
-	if (!pdata->v3_gain) {
-		ret = -EINVAL;
-		goto out_unmap;
-	}
+	if (!pdata->v3_gain)
+		return -EINVAL;
+
 	max1586->min_uV = MAX1586_V3_MIN_UV / 1000 * pdata->v3_gain / 1000;
 	max1586->max_uV = MAX1586_V3_MAX_UV / 1000 * pdata->v3_gain / 1000;
 
@@ -212,9 +212,12 @@ static int __devinit max1586_pmic_probe(struct i2c_client *client,
 			dev_err(&client->dev, "invalid regulator id %d\n", id);
 			goto err;
 		}
-		rdev[i] = regulator_register(&max1586_reg[id], &client->dev,
-					     pdata->subdevs[i].platform_data,
-					     max1586, NULL);
+
+		config.dev = &client->dev;
+		config.init_data = pdata->subdevs[i].platform_data;
+		config.driver_data = max1586;
+
+		rdev[i] = regulator_register(&max1586_reg[id], &config);
 		if (IS_ERR(rdev[i])) {
 			ret = PTR_ERR(rdev[i]);
 			dev_err(&client->dev, "failed to register %s\n",
@@ -230,9 +233,6 @@ static int __devinit max1586_pmic_probe(struct i2c_client *client,
 err:
 	while (--i >= 0)
 		regulator_unregister(rdev[i]);
-out_unmap:
-	kfree(max1586);
-out:
 	return ret;
 }
 
@@ -244,8 +244,6 @@ static int __devexit max1586_pmic_remove(struct i2c_client *client)
 	for (i = 0; i <= MAX1586_V6; i++)
 		if (max1586->rdev[i])
 			regulator_unregister(max1586->rdev[i]);
-	kfree(max1586);
-
 	return 0;
 }
 
diff --git a/drivers/regulator/max8649.c b/drivers/regulator/max8649.c
index 824c650436e..1f4bb80457b 100644
--- a/drivers/regulator/max8649.c
+++ b/drivers/regulator/max8649.c
@@ -53,7 +53,6 @@ struct max8649_regulator_info {
 	struct device		*dev;
 	struct regmap		*regmap;
 
-	int		vol_reg;
 	unsigned	mode:2;	/* bit[1:0] = VID1, VID0 */
 	unsigned	extclk_freq:2;
 	unsigned	extclk:1;
@@ -61,53 +60,6 @@ struct max8649_regulator_info {
 	unsigned	ramp_down:1;
 };
 
-/* I2C operations */
-
-static inline int check_range(int min_uV, int max_uV)
-{
-	if ((min_uV < MAX8649_DCDC_VMIN) || (max_uV > MAX8649_DCDC_VMAX)
-		|| (min_uV > max_uV))
-		return -EINVAL;
-	return 0;
-}
-
-static int max8649_list_voltage(struct regulator_dev *rdev, unsigned index)
-{
-	return (MAX8649_DCDC_VMIN + index * MAX8649_DCDC_STEP);
-}
-
-static int max8649_get_voltage(struct regulator_dev *rdev)
-{
-	struct max8649_regulator_info *info = rdev_get_drvdata(rdev);
-	unsigned int val;
-	unsigned char data;
-	int ret;
-
-	ret = regmap_read(info->regmap, info->vol_reg, &val);
-	if (ret != 0)
-		return ret;
-	data = (unsigned char)val & MAX8649_VOL_MASK;
-	return max8649_list_voltage(rdev, data);
-}
-
-static int max8649_set_voltage(struct regulator_dev *rdev,
-			       int min_uV, int max_uV, unsigned *selector)
-{
-	struct max8649_regulator_info *info = rdev_get_drvdata(rdev);
-	unsigned char data, mask;
-
-	if (check_range(min_uV, max_uV)) {
-		dev_err(info->dev, "invalid voltage range (%d, %d) uV\n",
-			min_uV, max_uV);
-		return -EINVAL;
-	}
-	data = DIV_ROUND_UP(min_uV - MAX8649_DCDC_VMIN, MAX8649_DCDC_STEP);
-	mask = MAX8649_VOL_MASK;
-	*selector = data & mask;
-
-	return regmap_update_bits(info->regmap, info->vol_reg, mask, data);
-}
-
 /* EN_PD means pulldown on EN input */
 static int max8649_enable(struct regulator_dev *rdev)
 {
@@ -145,11 +97,11 @@ static int max8649_enable_time(struct regulator_dev *rdev)
 	unsigned int val;
 
 	/* get voltage */
-	ret = regmap_read(info->regmap, info->vol_reg, &val);
+	ret = regmap_read(info->regmap, rdev->desc->vsel_reg, &val);
 	if (ret != 0)
 		return ret;
 	val &= MAX8649_VOL_MASK;
-	voltage = max8649_list_voltage(rdev, (unsigned char)val); /* uV */
+	voltage = regulator_list_voltage_linear(rdev, (unsigned char)val);
 
 	/* get rate */
 	ret = regmap_read(info->regmap, MAX8649_RAMP, &val);
@@ -167,11 +119,11 @@ static int max8649_set_mode(struct regulator_dev *rdev, unsigned int mode)
 
 	switch (mode) {
 	case REGULATOR_MODE_FAST:
-		regmap_update_bits(info->regmap, info->vol_reg, MAX8649_FORCE_PWM,
-				   MAX8649_FORCE_PWM);
+		regmap_update_bits(info->regmap, rdev->desc->vsel_reg,
+				   MAX8649_FORCE_PWM, MAX8649_FORCE_PWM);
 		break;
 	case REGULATOR_MODE_NORMAL:
-		regmap_update_bits(info->regmap, info->vol_reg,
+		regmap_update_bits(info->regmap, rdev->desc->vsel_reg,
 				   MAX8649_FORCE_PWM, 0);
 		break;
 	default:
@@ -186,7 +138,7 @@ static unsigned int max8649_get_mode(struct regulator_dev *rdev)
 	unsigned int val;
 	int ret;
 
-	ret = regmap_read(info->regmap, info->vol_reg, &val);
+	ret = regmap_read(info->regmap, rdev->desc->vsel_reg, &val);
 	if (ret != 0)
 		return ret;
 	if (val & MAX8649_FORCE_PWM)
@@ -195,9 +147,10 @@ static unsigned int max8649_get_mode(struct regulator_dev *rdev)
 }
 
 static struct regulator_ops max8649_dcdc_ops = {
-	.set_voltage	= max8649_set_voltage,
-	.get_voltage	= max8649_get_voltage,
-	.list_voltage	= max8649_list_voltage,
+	.set_voltage_sel = regulator_set_voltage_sel_regmap,
+	.get_voltage_sel = regulator_get_voltage_sel_regmap,
+	.list_voltage	= regulator_list_voltage_linear,
+	.map_voltage	= regulator_map_voltage_linear,
 	.enable		= max8649_enable,
 	.disable	= max8649_disable,
 	.is_enabled	= max8649_is_enabled,
@@ -213,6 +166,9 @@ static struct regulator_desc dcdc_desc = {
 	.type		= REGULATOR_VOLTAGE,
 	.n_voltages	= 1 << 6,
 	.owner		= THIS_MODULE,
+	.vsel_mask	= MAX8649_VOL_MASK,
+	.min_uV		= MAX8649_DCDC_VMIN,
+	.uV_step	= MAX8649_DCDC_STEP,
 };
 
 static struct regmap_config max8649_regmap_config = {
@@ -225,21 +181,23 @@ static int __devinit max8649_regulator_probe(struct i2c_client *client,
 {
 	struct max8649_platform_data *pdata = client->dev.platform_data;
 	struct max8649_regulator_info *info = NULL;
+	struct regulator_config config = { };
 	unsigned int val;
 	unsigned char data;
 	int ret;
 
-	info = kzalloc(sizeof(struct max8649_regulator_info), GFP_KERNEL);
+	info = devm_kzalloc(&client->dev, sizeof(struct max8649_regulator_info),
+			    GFP_KERNEL);
 	if (!info) {
 		dev_err(&client->dev, "No enough memory\n");
 		return -ENOMEM;
 	}
 
-	info->regmap = regmap_init_i2c(client, &max8649_regmap_config);
+	info->regmap = devm_regmap_init_i2c(client, &max8649_regmap_config);
 	if (IS_ERR(info->regmap)) {
 		ret = PTR_ERR(info->regmap);
 		dev_err(&client->dev, "Failed to allocate register map: %d\n", ret);
-		goto fail;
+		return ret;
 	}
 
 	info->dev = &client->dev;
@@ -248,16 +206,16 @@ static int __devinit max8649_regulator_probe(struct i2c_client *client,
 	info->mode = pdata->mode;
 	switch (info->mode) {
 	case 0:
-		info->vol_reg = MAX8649_MODE0;
+		dcdc_desc.vsel_reg = MAX8649_MODE0;
 		break;
 	case 1:
-		info->vol_reg = MAX8649_MODE1;
+		dcdc_desc.vsel_reg = MAX8649_MODE1;
 		break;
 	case 2:
-		info->vol_reg = MAX8649_MODE2;
+		dcdc_desc.vsel_reg = MAX8649_MODE2;
 		break;
 	case 3:
-		info->vol_reg = MAX8649_MODE3;
+		dcdc_desc.vsel_reg = MAX8649_MODE3;
 		break;
 	default:
 		break;
@@ -267,7 +225,7 @@ static int __devinit max8649_regulator_probe(struct i2c_client *client,
 	if (ret != 0) {
 		dev_err(info->dev, "Failed to detect ID of MAX8649:%d\n",
 			ret);
-		goto out;
+		return ret;
 	}
 	dev_info(info->dev, "Detected MAX8649 (ID:%x)\n", val);
 
@@ -277,7 +235,8 @@ static int __devinit max8649_regulator_probe(struct i2c_client *client,
 	/* enable/disable external clock synchronization */
 	info->extclk = pdata->extclk;
 	data = (info->extclk) ? MAX8649_SYNC_EXTCLK : 0;
-	regmap_update_bits(info->regmap, info->vol_reg, MAX8649_SYNC_EXTCLK, data);
+	regmap_update_bits(info->regmap, dcdc_desc.vsel_reg,
+			   MAX8649_SYNC_EXTCLK, data);
 	if (info->extclk) {
 		/* set external clock frequency */
 		info->extclk_freq = pdata->extclk_freq;
@@ -297,22 +256,18 @@ static int __devinit max8649_regulator_probe(struct i2c_client *client,
 				   MAX8649_RAMP_DOWN);
 	}
 
-	info->regulator = regulator_register(&dcdc_desc, &client->dev,
-					     pdata->regulator, info, NULL);
+	config.dev = &client->dev;
+	config.init_data = pdata->regulator;
+	config.driver_data = info;
+
+	info->regulator = regulator_register(&dcdc_desc, &config);
 	if (IS_ERR(info->regulator)) {
 		dev_err(info->dev, "failed to register regulator %s\n",
 			dcdc_desc.name);
-		ret = PTR_ERR(info->regulator);
-		goto out;
+		return PTR_ERR(info->regulator);
 	}
 
-	dev_info(info->dev, "Max8649 regulator device is detected.\n");
 	return 0;
-out:
-	regmap_exit(info->regmap);
-fail:
-	kfree(info);
-	return ret;
 }
 
 static int __devexit max8649_regulator_remove(struct i2c_client *client)
@@ -322,8 +277,6 @@ static int __devexit max8649_regulator_remove(struct i2c_client *client)
 	if (info) {
 		if (info->regulator)
 			regulator_unregister(info->regulator);
-		regmap_exit(info->regmap);
-		kfree(info);
 	}
 
 	return 0;
@@ -360,4 +313,3 @@ module_exit(max8649_exit);
 MODULE_DESCRIPTION("MAXIM 8649 voltage regulator driver");
 MODULE_AUTHOR("Haojian Zhuang <haojian.zhuang@marvell.com>");
 MODULE_LICENSE("GPL");
-
diff --git a/drivers/regulator/max8660.c b/drivers/regulator/max8660.c
index 4c5b05311f4..8d531742f59 100644
--- a/drivers/regulator/max8660.c
+++ b/drivers/regulator/max8660.c
@@ -126,42 +126,22 @@ static int max8660_dcdc_disable(struct regulator_dev *rdev)
 	return max8660_write(max8660, MAX8660_OVER1, mask, 0);
 }
 
-static int max8660_dcdc_list(struct regulator_dev *rdev, unsigned selector)
-{
-	if (selector > MAX8660_DCDC_MAX_SEL)
-		return -EINVAL;
-	return MAX8660_DCDC_MIN_UV + selector * MAX8660_DCDC_STEP;
-}
-
-static int max8660_dcdc_get(struct regulator_dev *rdev)
+static int max8660_dcdc_get_voltage_sel(struct regulator_dev *rdev)
 {
 	struct max8660 *max8660 = rdev_get_drvdata(rdev);
+
 	u8 reg = (rdev_get_id(rdev) == MAX8660_V3) ? MAX8660_ADTV2 : MAX8660_SDTV2;
 	u8 selector = max8660->shadow_regs[reg];
-	return MAX8660_DCDC_MIN_UV + selector * MAX8660_DCDC_STEP;
+	return selector;
 }
 
-static int max8660_dcdc_set(struct regulator_dev *rdev, int min_uV, int max_uV,
-			    unsigned int *s)
+static int max8660_dcdc_set_voltage_sel(struct regulator_dev *rdev,
+					unsigned int selector)
 {
 	struct max8660 *max8660 = rdev_get_drvdata(rdev);
-	u8 reg, selector, bits;
+	u8 reg, bits;
 	int ret;
 
-	if (min_uV < MAX8660_DCDC_MIN_UV || min_uV > MAX8660_DCDC_MAX_UV)
-		return -EINVAL;
-	if (max_uV < MAX8660_DCDC_MIN_UV || max_uV > MAX8660_DCDC_MAX_UV)
-		return -EINVAL;
-
-	selector = DIV_ROUND_UP(min_uV - MAX8660_DCDC_MIN_UV,
-				MAX8660_DCDC_STEP);
-
-	ret = max8660_dcdc_list(rdev, selector);
-	if (ret < 0 || ret > max_uV)
-		return -EINVAL;
-
-	*s = selector;
-
 	reg = (rdev_get_id(rdev) == MAX8660_V3) ? MAX8660_ADTV2 : MAX8660_SDTV2;
 	ret = max8660_write(max8660, reg, 0, selector);
 	if (ret)
@@ -174,9 +154,10 @@ static int max8660_dcdc_set(struct regulator_dev *rdev, int min_uV, int max_uV,
 
 static struct regulator_ops max8660_dcdc_ops = {
 	.is_enabled = max8660_dcdc_is_enabled,
-	.list_voltage = max8660_dcdc_list,
-	.set_voltage = max8660_dcdc_set,
-	.get_voltage = max8660_dcdc_get,
+	.list_voltage = regulator_list_voltage_linear,
+	.map_voltage = regulator_map_voltage_linear,
+	.set_voltage_sel = max8660_dcdc_set_voltage_sel,
+	.get_voltage_sel = max8660_dcdc_get_voltage_sel,
 };
 
 
@@ -184,42 +165,20 @@ static struct regulator_ops max8660_dcdc_ops = {
  * LDO5 functions
  */
 
-static int max8660_ldo5_list(struct regulator_dev *rdev, unsigned selector)
-{
-	if (selector > MAX8660_LDO5_MAX_SEL)
-		return -EINVAL;
-	return MAX8660_LDO5_MIN_UV + selector * MAX8660_LDO5_STEP;
-}
-
-static int max8660_ldo5_get(struct regulator_dev *rdev)
+static int max8660_ldo5_get_voltage_sel(struct regulator_dev *rdev)
 {
 	struct max8660 *max8660 = rdev_get_drvdata(rdev);
-	u8 selector = max8660->shadow_regs[MAX8660_MDTV2];
 
-	return MAX8660_LDO5_MIN_UV + selector * MAX8660_LDO5_STEP;
+	u8 selector = max8660->shadow_regs[MAX8660_MDTV2];
+	return selector;
 }
 
-static int max8660_ldo5_set(struct regulator_dev *rdev, int min_uV, int max_uV,
-			    unsigned int *s)
+static int max8660_ldo5_set_voltage_sel(struct regulator_dev *rdev,
+					unsigned int selector)
 {
 	struct max8660 *max8660 = rdev_get_drvdata(rdev);
-	u8 selector;
 	int ret;
 
-	if (min_uV < MAX8660_LDO5_MIN_UV || min_uV > MAX8660_LDO5_MAX_UV)
-		return -EINVAL;
-	if (max_uV < MAX8660_LDO5_MIN_UV || max_uV > MAX8660_LDO5_MAX_UV)
-		return -EINVAL;
-
-	selector = DIV_ROUND_UP(min_uV - MAX8660_LDO5_MIN_UV,
-				MAX8660_LDO5_STEP);
-
-	ret = max8660_ldo5_list(rdev, selector);
-	if (ret < 0 || ret > max_uV)
-		return -EINVAL;
-
-	*s = selector;
-
 	ret = max8660_write(max8660, MAX8660_MDTV2, 0, selector);
 	if (ret)
 		return ret;
@@ -229,9 +188,10 @@ static int max8660_ldo5_set(struct regulator_dev *rdev, int min_uV, int max_uV,
 }
 
 static struct regulator_ops max8660_ldo5_ops = {
-	.list_voltage = max8660_ldo5_list,
-	.set_voltage = max8660_ldo5_set,
-	.get_voltage = max8660_ldo5_get,
+	.list_voltage = regulator_list_voltage_linear,
+	.map_voltage = regulator_map_voltage_linear,
+	.set_voltage_sel = max8660_ldo5_set_voltage_sel,
+	.get_voltage_sel = max8660_ldo5_get_voltage_sel,
 };
 
 
@@ -261,59 +221,38 @@ static int max8660_ldo67_disable(struct regulator_dev *rdev)
 	return max8660_write(max8660, MAX8660_OVER2, mask, 0);
 }
 
-static int max8660_ldo67_list(struct regulator_dev *rdev, unsigned selector)
-{
-	if (selector > MAX8660_LDO67_MAX_SEL)
-		return -EINVAL;
-	return MAX8660_LDO67_MIN_UV + selector * MAX8660_LDO67_STEP;
-}
-
-static int max8660_ldo67_get(struct regulator_dev *rdev)
+static int max8660_ldo67_get_voltage_sel(struct regulator_dev *rdev)
 {
 	struct max8660 *max8660 = rdev_get_drvdata(rdev);
+
 	u8 shift = (rdev_get_id(rdev) == MAX8660_V6) ? 0 : 4;
 	u8 selector = (max8660->shadow_regs[MAX8660_L12VCR] >> shift) & 0xf;
-
-	return MAX8660_LDO67_MIN_UV + selector * MAX8660_LDO67_STEP;
+	return selector;
 }
 
-static int max8660_ldo67_set(struct regulator_dev *rdev, int min_uV,
-			     int max_uV, unsigned int *s)
+static int max8660_ldo67_set_voltage_sel(struct regulator_dev *rdev,
+					 unsigned int selector)
 {
 	struct max8660 *max8660 = rdev_get_drvdata(rdev);
-	u8 selector;
-	int ret;
-
-	if (min_uV < MAX8660_LDO67_MIN_UV || min_uV > MAX8660_LDO67_MAX_UV)
-		return -EINVAL;
-	if (max_uV < MAX8660_LDO67_MIN_UV || max_uV > MAX8660_LDO67_MAX_UV)
-		return -EINVAL;
-
-	selector = DIV_ROUND_UP(min_uV - MAX8660_LDO67_MIN_UV,
-				MAX8660_LDO67_STEP);
-
-	ret = max8660_ldo67_list(rdev, selector);
-	if (ret < 0 || ret > max_uV)
-		return -EINVAL;
-
-	*s = selector;
 
 	if (rdev_get_id(rdev) == MAX8660_V6)
 		return max8660_write(max8660, MAX8660_L12VCR, 0xf0, selector);
 	else
-		return max8660_write(max8660, MAX8660_L12VCR, 0x0f, selector << 4);
+		return max8660_write(max8660, MAX8660_L12VCR, 0x0f,
+				     selector << 4);
 }
 
 static struct regulator_ops max8660_ldo67_ops = {
 	.is_enabled = max8660_ldo67_is_enabled,
 	.enable = max8660_ldo67_enable,
 	.disable = max8660_ldo67_disable,
-	.list_voltage = max8660_ldo67_list,
-	.get_voltage = max8660_ldo67_get,
-	.set_voltage = max8660_ldo67_set,
+	.list_voltage = regulator_list_voltage_linear,
+	.map_voltage = regulator_map_voltage_linear,
+	.get_voltage_sel = max8660_ldo67_get_voltage_sel,
+	.set_voltage_sel = max8660_ldo67_set_voltage_sel,
 };
 
-static struct regulator_desc max8660_reg[] = {
+static const struct regulator_desc max8660_reg[] = {
 	{
 		.name = "V3(DCDC)",
 		.id = MAX8660_V3,
@@ -321,6 +260,8 @@ static struct regulator_desc max8660_reg[] = {
 		.type = REGULATOR_VOLTAGE,
 		.n_voltages = MAX8660_DCDC_MAX_SEL + 1,
 		.owner = THIS_MODULE,
+		.min_uV = MAX8660_DCDC_MIN_UV,
+		.uV_step = MAX8660_DCDC_STEP,
 	},
 	{
 		.name = "V4(DCDC)",
@@ -329,6 +270,8 @@ static struct regulator_desc max8660_reg[] = {
 		.type = REGULATOR_VOLTAGE,
 		.n_voltages = MAX8660_DCDC_MAX_SEL + 1,
 		.owner = THIS_MODULE,
+		.min_uV = MAX8660_DCDC_MIN_UV,
+		.uV_step = MAX8660_DCDC_STEP,
 	},
 	{
 		.name = "V5(LDO)",
@@ -337,6 +280,8 @@ static struct regulator_desc max8660_reg[] = {
 		.type = REGULATOR_VOLTAGE,
 		.n_voltages = MAX8660_LDO5_MAX_SEL + 1,
 		.owner = THIS_MODULE,
+		.min_uV = MAX8660_LDO5_MIN_UV,
+		.uV_step = MAX8660_LDO5_STEP,
 	},
 	{
 		.name = "V6(LDO)",
@@ -345,6 +290,8 @@ static struct regulator_desc max8660_reg[] = {
 		.type = REGULATOR_VOLTAGE,
 		.n_voltages = MAX8660_LDO67_MAX_SEL + 1,
 		.owner = THIS_MODULE,
+		.min_uV = MAX8660_LDO67_MIN_UV,
+		.uV_step = MAX8660_LDO67_STEP,
 	},
 	{
 		.name = "V7(LDO)",
@@ -353,6 +300,8 @@ static struct regulator_desc max8660_reg[] = {
 		.type = REGULATOR_VOLTAGE,
 		.n_voltages = MAX8660_LDO67_MAX_SEL + 1,
 		.owner = THIS_MODULE,
+		.min_uV = MAX8660_LDO67_MIN_UV,
+		.uV_step = MAX8660_LDO67_STEP,
 	},
 };
 
@@ -361,21 +310,20 @@ static int __devinit max8660_probe(struct i2c_client *client,
 {
 	struct regulator_dev **rdev;
 	struct max8660_platform_data *pdata = client->dev.platform_data;
+	struct regulator_config config = { };
 	struct max8660 *max8660;
 	int boot_on, i, id, ret = -EINVAL;
 
 	if (pdata->num_subdevs > MAX8660_V_END) {
 		dev_err(&client->dev, "Too many regulators found!\n");
-		goto out;
+		return -EINVAL;
 	}
 
-	max8660 = kzalloc(sizeof(struct max8660) +
+	max8660 = devm_kzalloc(&client->dev, sizeof(struct max8660) +
 			sizeof(struct regulator_dev *) * MAX8660_V_END,
 			GFP_KERNEL);
-	if (!max8660) {
-		ret = -ENOMEM;
-		goto out;
-	}
+	if (!max8660)
+		return -ENOMEM;
 
 	max8660->client = client;
 	rdev = max8660->rdev;
@@ -404,7 +352,7 @@ static int __devinit max8660_probe(struct i2c_client *client,
 	for (i = 0; i < pdata->num_subdevs; i++) {
 
 		if (!pdata->subdevs[i].platform_data)
-			goto err_free;
+			goto err_out;
 
 		boot_on = pdata->subdevs[i].platform_data->constraints.boot_on;
 
@@ -430,7 +378,7 @@ static int __devinit max8660_probe(struct i2c_client *client,
 		case MAX8660_V7:
 			if (!strcmp(i2c_id->name, "max8661")) {
 				dev_err(&client->dev, "Regulator not on this chip!\n");
-				goto err_free;
+				goto err_out;
 			}
 
 			if (boot_on)
@@ -440,7 +388,7 @@ static int __devinit max8660_probe(struct i2c_client *client,
 		default:
 			dev_err(&client->dev, "invalid regulator %s\n",
 				 pdata->subdevs[i].name);
-			goto err_free;
+			goto err_out;
 		}
 	}
 
@@ -449,9 +397,11 @@ static int __devinit max8660_probe(struct i2c_client *client,
 
 		id = pdata->subdevs[i].id;
 
-		rdev[i] = regulator_register(&max8660_reg[id], &client->dev,
-					     pdata->subdevs[i].platform_data,
-					     max8660, NULL);
+		config.dev = &client->dev;
+		config.init_data = pdata->subdevs[i].platform_data;
+		config.driver_data = max8660;
+
+		rdev[i] = regulator_register(&max8660_reg[id], &config);
 		if (IS_ERR(rdev[i])) {
 			ret = PTR_ERR(rdev[i]);
 			dev_err(&client->dev, "failed to register %s\n",
@@ -461,15 +411,12 @@ static int __devinit max8660_probe(struct i2c_client *client,
 	}
 
 	i2c_set_clientdata(client, max8660);
-	dev_info(&client->dev, "Maxim 8660/8661 regulator driver loaded\n");
 	return 0;
 
 err_unregister:
 	while (--i >= 0)
 		regulator_unregister(rdev[i]);
-err_free:
-	kfree(max8660);
-out:
+err_out:
 	return ret;
 }
 
@@ -481,8 +428,6 @@ static int __devexit max8660_remove(struct i2c_client *client)
 	for (i = 0; i < MAX8660_V_END; i++)
 		if (max8660->rdev[i])
 			regulator_unregister(max8660->rdev[i]);
-	kfree(max8660);
-
 	return 0;
 }
 
diff --git a/drivers/regulator/max8925-regulator.c b/drivers/regulator/max8925-regulator.c
index 2f242f43096..43dc97ec393 100644
--- a/drivers/regulator/max8925-regulator.c
+++ b/drivers/regulator/max8925-regulator.c
@@ -38,50 +38,20 @@ struct max8925_regulator_info {
 	struct i2c_client	*i2c;
 	struct max8925_chip	*chip;
 
-	int	min_uV;
-	int	max_uV;
-	int	step_uV;
 	int	vol_reg;
-	int	vol_shift;
-	int	vol_nbits;
 	int	enable_reg;
 };
 
-static inline int check_range(struct max8925_regulator_info *info,
-			      int min_uV, int max_uV)
-{
-	if (min_uV < info->min_uV || min_uV > info->max_uV)
-		return -EINVAL;
-
-	return 0;
-}
-
-static int max8925_list_voltage(struct regulator_dev *rdev, unsigned index)
-{
-	struct max8925_regulator_info *info = rdev_get_drvdata(rdev);
-	return info->min_uV + index * info->step_uV;
-}
-
-static int max8925_set_voltage(struct regulator_dev *rdev,
-			       int min_uV, int max_uV, unsigned int *selector)
+static int max8925_set_voltage_sel(struct regulator_dev *rdev,
+				   unsigned int selector)
 {
 	struct max8925_regulator_info *info = rdev_get_drvdata(rdev);
-	unsigned char data, mask;
-
-	if (check_range(info, min_uV, max_uV)) {
-		dev_err(info->chip->dev, "invalid voltage range (%d, %d) uV\n",
-			min_uV, max_uV);
-		return -EINVAL;
-	}
-	data = DIV_ROUND_UP(min_uV - info->min_uV, info->step_uV);
-	*selector = data;
-	data <<= info->vol_shift;
-	mask = ((1 << info->vol_nbits) - 1) << info->vol_shift;
+	unsigned char mask = rdev->desc->n_voltages - 1;
 
-	return max8925_set_bits(info->i2c, info->vol_reg, mask, data);
+	return max8925_set_bits(info->i2c, info->vol_reg, mask, selector);
 }
 
-static int max8925_get_voltage(struct regulator_dev *rdev)
+static int max8925_get_voltage_sel(struct regulator_dev *rdev)
 {
 	struct max8925_regulator_info *info = rdev_get_drvdata(rdev);
 	unsigned char data, mask;
@@ -90,10 +60,10 @@ static int max8925_get_voltage(struct regulator_dev *rdev)
 	ret = max8925_reg_read(info->i2c, info->vol_reg);
 	if (ret < 0)
 		return ret;
-	mask = ((1 << info->vol_nbits) - 1) << info->vol_shift;
-	data = (ret & mask) >> info->vol_shift;
+	mask = rdev->desc->n_voltages - 1;
+	data = ret & mask;
 
-	return max8925_list_voltage(rdev, data);
+	return data;
 }
 
 static int max8925_enable(struct regulator_dev *rdev)
@@ -163,8 +133,10 @@ static int max8925_set_dvm_disable(struct regulator_dev *rdev)
 }
 
 static struct regulator_ops max8925_regulator_sdv_ops = {
-	.set_voltage		= max8925_set_voltage,
-	.get_voltage		= max8925_get_voltage,
+	.map_voltage		= regulator_map_voltage_linear,
+	.list_voltage		= regulator_list_voltage_linear,
+	.set_voltage_sel	= max8925_set_voltage_sel,
+	.get_voltage_sel	= max8925_get_voltage_sel,
 	.enable			= max8925_enable,
 	.disable		= max8925_disable,
 	.is_enabled		= max8925_is_enabled,
@@ -174,8 +146,10 @@ static struct regulator_ops max8925_regulator_sdv_ops = {
 };
 
 static struct regulator_ops max8925_regulator_ldo_ops = {
-	.set_voltage		= max8925_set_voltage,
-	.get_voltage		= max8925_get_voltage,
+	.map_voltage		= regulator_map_voltage_linear,
+	.list_voltage		= regulator_list_voltage_linear,
+	.set_voltage_sel	= max8925_set_voltage_sel,
+	.get_voltage_sel	= max8925_get_voltage_sel,
 	.enable			= max8925_enable,
 	.disable		= max8925_disable,
 	.is_enabled		= max8925_is_enabled,
@@ -189,13 +163,11 @@ static struct regulator_ops max8925_regulator_ldo_ops = {
 		.type	= REGULATOR_VOLTAGE,			\
 		.id	= MAX8925_ID_SD##_id,			\
 		.owner	= THIS_MODULE,				\
+		.n_voltages = 64,				\
+		.min_uV = min * 1000,				\
+		.uV_step = step * 1000,				\
 	},							\
-	.min_uV		= min * 1000,				\
-	.max_uV		= max * 1000,				\
-	.step_uV	= step * 1000,				\
 	.vol_reg	= MAX8925_SDV##_id,			\
-	.vol_shift	= 0,					\
-	.vol_nbits	= 6,					\
 	.enable_reg	= MAX8925_SDCTL##_id,			\
 }
 
@@ -207,13 +179,11 @@ static struct regulator_ops max8925_regulator_ldo_ops = {
 		.type	= REGULATOR_VOLTAGE,			\
 		.id	= MAX8925_ID_LDO##_id,			\
 		.owner	= THIS_MODULE,				\
+		.n_voltages = 64,				\
+		.min_uV = min * 1000,				\
+		.uV_step = step * 1000,				\
 	},							\
-	.min_uV		= min * 1000,				\
-	.max_uV		= max * 1000,				\
-	.step_uV	= step * 1000,				\
 	.vol_reg	= MAX8925_LDOVOUT##_id,			\
-	.vol_shift	= 0,					\
-	.vol_nbits	= 6,					\
 	.enable_reg	= MAX8925_LDOCTL##_id,			\
 }
 
@@ -261,6 +231,7 @@ static int __devinit max8925_regulator_probe(struct platform_device *pdev)
 {
 	struct max8925_chip *chip = dev_get_drvdata(pdev->dev.parent);
 	struct max8925_platform_data *pdata = chip->dev->platform_data;
+	struct regulator_config config = { };
 	struct max8925_regulator_info *ri;
 	struct regulator_dev *rdev;
 
@@ -272,8 +243,11 @@ static int __devinit max8925_regulator_probe(struct platform_device *pdev)
 	ri->i2c = chip->i2c;
 	ri->chip = chip;
 
-	rdev = regulator_register(&ri->desc, &pdev->dev,
-				  pdata->regulator[pdev->id], ri, NULL);
+	config.dev = &pdev->dev;
+	config.init_data = pdata->regulator[pdev->id];
+	config.driver_data = ri;
+
+	rdev = regulator_register(&ri->desc, &config);
 	if (IS_ERR(rdev)) {
 		dev_err(&pdev->dev, "failed to register regulator %s\n",
 				ri->desc.name);
@@ -319,4 +293,3 @@ MODULE_LICENSE("GPL");
 MODULE_AUTHOR("Haojian Zhuang <haojian.zhuang@marvell.com>");
 MODULE_DESCRIPTION("Regulator Driver for Maxim 8925 PMIC");
 MODULE_ALIAS("platform:max8925-regulator");
-
diff --git a/drivers/regulator/max8952.c b/drivers/regulator/max8952.c
index 75d89400c12..910c9b26d49 100644
--- a/drivers/regulator/max8952.c
+++ b/drivers/regulator/max8952.c
@@ -69,11 +69,6 @@ static int max8952_write_reg(struct max8952_data *max8952,
 	return i2c_smbus_write_byte_data(max8952->client, reg, value);
 }
 
-static int max8952_voltage(struct max8952_data *max8952, u8 mode)
-{
-	return (max8952->pdata->dvs_mode[mode] * 10 + 770) * 1000;
-}
-
 static int max8952_list_voltage(struct regulator_dev *rdev,
 		unsigned int selector)
 {
@@ -82,7 +77,7 @@ static int max8952_list_voltage(struct regulator_dev *rdev,
 	if (rdev_get_id(rdev) != 0)
 		return -EINVAL;
 
-	return max8952_voltage(max8952, selector);
+	return (max8952->pdata->dvs_mode[selector] * 10 + 770) * 1000;
 }
 
 static int max8952_is_enabled(struct regulator_dev *rdev)
@@ -117,7 +112,7 @@ static int max8952_disable(struct regulator_dev *rdev)
 	return 0;
 }
 
-static int max8952_get_voltage(struct regulator_dev *rdev)
+static int max8952_get_voltage_sel(struct regulator_dev *rdev)
 {
 	struct max8952_data *max8952 = rdev_get_drvdata(rdev);
 	u8 vid = 0;
@@ -127,14 +122,13 @@ static int max8952_get_voltage(struct regulator_dev *rdev)
 	if (max8952->vid1)
 		vid += 2;
 
-	return max8952_voltage(max8952, vid);
+	return vid;
 }
 
-static int max8952_set_voltage(struct regulator_dev *rdev,
-			       int min_uV, int max_uV, unsigned *selector)
+static int max8952_set_voltage_sel(struct regulator_dev *rdev,
+				   unsigned selector)
 {
 	struct max8952_data *max8952 = rdev_get_drvdata(rdev);
-	s8 vid = -1, i;
 
 	if (!gpio_is_valid(max8952->pdata->gpio_vid0) ||
 			!gpio_is_valid(max8952->pdata->gpio_vid1)) {
@@ -142,23 +136,10 @@ static int max8952_set_voltage(struct regulator_dev *rdev,
 		return -EPERM;
 	}
 
-	for (i = 0; i < MAX8952_NUM_DVS_MODE; i++) {
-		int volt = max8952_voltage(max8952, i);
-
-		/* Set the voltage as low as possible within the range */
-		if (volt <= max_uV && volt >= min_uV)
-			if (vid == -1 || max8952_voltage(max8952, vid) > volt)
-				vid = i;
-	}
-
-	if (vid >= 0 && vid < MAX8952_NUM_DVS_MODE) {
-		max8952->vid0 = (vid % 2 == 1);
-		max8952->vid1 = (((vid >> 1) % 2) == 1);
-		*selector = vid;
-		gpio_set_value(max8952->pdata->gpio_vid0, max8952->vid0);
-		gpio_set_value(max8952->pdata->gpio_vid1, max8952->vid1);
-	} else
-		return -EINVAL;
+	max8952->vid0 = selector & 0x1;
+	max8952->vid1 = (selector >> 1) & 0x1;
+	gpio_set_value(max8952->pdata->gpio_vid0, max8952->vid0);
+	gpio_set_value(max8952->pdata->gpio_vid1, max8952->vid1);
 
 	return 0;
 }
@@ -168,12 +149,12 @@ static struct regulator_ops max8952_ops = {
 	.is_enabled		= max8952_is_enabled,
 	.enable			= max8952_enable,
 	.disable		= max8952_disable,
-	.get_voltage		= max8952_get_voltage,
-	.set_voltage		= max8952_set_voltage,
+	.get_voltage_sel	= max8952_get_voltage_sel,
+	.set_voltage_sel	= max8952_set_voltage_sel,
 	.set_suspend_disable	= max8952_disable,
 };
 
-static struct regulator_desc regulator = {
+static const struct regulator_desc regulator = {
 	.name		= "MAX8952_VOUT",
 	.id		= 0,
 	.n_voltages	= MAX8952_NUM_DVS_MODE,
@@ -187,6 +168,7 @@ static int __devinit max8952_pmic_probe(struct i2c_client *client,
 {
 	struct i2c_adapter *adapter = to_i2c_adapter(client->dev.parent);
 	struct max8952_platform_data *pdata = client->dev.platform_data;
+	struct regulator_config config = { };
 	struct max8952_data *max8952;
 
 	int ret = 0, err = 0;
@@ -199,7 +181,8 @@ static int __devinit max8952_pmic_probe(struct i2c_client *client,
 	if (!i2c_check_functionality(adapter, I2C_FUNC_SMBUS_BYTE))
 		return -EIO;
 
-	max8952 = kzalloc(sizeof(struct max8952_data), GFP_KERNEL);
+	max8952 = devm_kzalloc(&client->dev, sizeof(struct max8952_data),
+			       GFP_KERNEL);
 	if (!max8952)
 		return -ENOMEM;
 
@@ -207,18 +190,21 @@ static int __devinit max8952_pmic_probe(struct i2c_client *client,
 	max8952->dev = &client->dev;
 	max8952->pdata = pdata;
 
-	max8952->rdev = regulator_register(&regulator, max8952->dev,
-			&pdata->reg_data, max8952, NULL);
+	config.dev = max8952->dev;
+	config.init_data = &pdata->reg_data;
+	config.driver_data = max8952;
+
+	max8952->rdev = regulator_register(&regulator, &config);
 
 	if (IS_ERR(max8952->rdev)) {
 		ret = PTR_ERR(max8952->rdev);
 		dev_err(max8952->dev, "regulator init failed (%d)\n", ret);
-		goto err_reg;
+		return ret;
 	}
 
 	max8952->en = !!(pdata->reg_data.constraints.boot_on);
-	max8952->vid0 = (pdata->default_mode % 2) == 1;
-	max8952->vid1 = ((pdata->default_mode >> 1) % 2) == 1;
+	max8952->vid0 = pdata->default_mode & 0x1;
+	max8952->vid1 = (pdata->default_mode >> 1) & 0x1;
 
 	if (gpio_is_valid(pdata->gpio_en)) {
 		if (!gpio_request(pdata->gpio_en, "MAX8952 EN"))
@@ -241,13 +227,13 @@ static int __devinit max8952_pmic_probe(struct i2c_client *client,
 			gpio_is_valid(pdata->gpio_vid1)) {
 		if (!gpio_request(pdata->gpio_vid0, "MAX8952 VID0"))
 			gpio_direction_output(pdata->gpio_vid0,
-					(pdata->default_mode) % 2);
+					(pdata->default_mode) & 0x1);
 		else
 			err = 1;
 
 		if (!gpio_request(pdata->gpio_vid1, "MAX8952 VID1"))
 			gpio_direction_output(pdata->gpio_vid1,
-				(pdata->default_mode >> 1) % 2);
+				(pdata->default_mode >> 1) & 0x1);
 		else {
 			if (!err)
 				gpio_free(pdata->gpio_vid0);
@@ -310,10 +296,6 @@ static int __devinit max8952_pmic_probe(struct i2c_client *client,
 	i2c_set_clientdata(client, max8952);
 
 	return 0;
-
-err_reg:
-	kfree(max8952);
-	return ret;
 }
 
 static int __devexit max8952_pmic_remove(struct i2c_client *client)
@@ -327,8 +309,6 @@ static int __devexit max8952_pmic_remove(struct i2c_client *client)
 	gpio_free(pdata->gpio_vid0);
 	gpio_free(pdata->gpio_vid1);
 	gpio_free(pdata->gpio_en);
-
-	kfree(max8952);
 	return 0;
 }
 
diff --git a/drivers/regulator/max8997.c b/drivers/regulator/max8997.c
index 96579296f04..704cd49ef37 100644
--- a/drivers/regulator/max8997.c
+++ b/drivers/regulator/max8997.c
@@ -22,7 +22,6 @@
  */
 
 #include <linux/bug.h>
-#include <linux/delay.h>
 #include <linux/err.h>
 #include <linux/gpio.h>
 #include <linux/slab.h>
@@ -68,29 +67,28 @@ struct voltage_map_desc {
 	int min;
 	int max;
 	int step;
-	unsigned int n_bits;
 };
 
 /* Voltage maps in mV */
 static const struct voltage_map_desc ldo_voltage_map_desc = {
-	.min = 800,	.max = 3950,	.step = 50,	.n_bits = 6,
+	.min = 800,	.max = 3950,	.step = 50,
 }; /* LDO1 ~ 18, 21 all */
 
 static const struct voltage_map_desc buck1245_voltage_map_desc = {
-	.min = 650,	.max = 2225,	.step = 25,	.n_bits = 6,
+	.min = 650,	.max = 2225,	.step = 25,
 }; /* Buck1, 2, 4, 5 */
 
 static const struct voltage_map_desc buck37_voltage_map_desc = {
-	.min = 750,	.max = 3900,	.step = 50,	.n_bits = 6,
+	.min = 750,	.max = 3900,	.step = 50,
 }; /* Buck3, 7 */
 
 /* current map in mA */
 static const struct voltage_map_desc charger_current_map_desc = {
-	.min = 200,	.max = 950,	.step = 50,	.n_bits = 4,
+	.min = 200,	.max = 950,	.step = 50,
 };
 
 static const struct voltage_map_desc topoff_current_map_desc = {
-	.min = 50,	.max = 200,	.step = 10,	.n_bits = 4,
+	.min = 50,	.max = 200,	.step = 10,
 };
 
 static const struct voltage_map_desc *reg_voltage_map[] = {
@@ -279,9 +277,7 @@ static int max8997_reg_is_enabled(struct regulator_dev *rdev)
 	u8 val;
 
 	ret = max8997_get_enable_register(rdev, &reg, &mask, &pattern);
-	if (ret == -EINVAL)
-		return 1; /* "not controllable" */
-	else if (ret)
+	if (ret)
 		return ret;
 
 	ret = max8997_read_reg(i2c, reg, &val);
@@ -320,6 +316,7 @@ static int max8997_reg_disable(struct regulator_dev *rdev)
 static int max8997_get_voltage_register(struct regulator_dev *rdev,
 		int *_reg, int *_shift, int *_mask)
 {
+	struct max8997_data *max8997 = rdev_get_drvdata(rdev);
 	int rid = rdev_get_id(rdev);
 	int reg, shift = 0, mask = 0x3f;
 
@@ -329,9 +326,13 @@ static int max8997_get_voltage_register(struct regulator_dev *rdev,
 		break;
 	case MAX8997_BUCK1:
 		reg = MAX8997_REG_BUCK1DVS1;
+		if (max8997->buck1_gpiodvs)
+			reg += max8997->buck125_gpioindex;
 		break;
 	case MAX8997_BUCK2:
 		reg = MAX8997_REG_BUCK2DVS1;
+		if (max8997->buck2_gpiodvs)
+			reg += max8997->buck125_gpioindex;
 		break;
 	case MAX8997_BUCK3:
 		reg = MAX8997_REG_BUCK3DVS;
@@ -341,6 +342,8 @@ static int max8997_get_voltage_register(struct regulator_dev *rdev,
 		break;
 	case MAX8997_BUCK5:
 		reg = MAX8997_REG_BUCK5DVS1;
+		if (max8997->buck5_gpiodvs)
+			reg += max8997->buck125_gpioindex;
 		break;
 	case MAX8997_BUCK7:
 		reg = MAX8997_REG_BUCK7DVS;
@@ -376,23 +379,17 @@ static int max8997_get_voltage_register(struct regulator_dev *rdev,
 	return 0;
 }
 
-static int max8997_get_voltage(struct regulator_dev *rdev)
+static int max8997_get_voltage_sel(struct regulator_dev *rdev)
 {
 	struct max8997_data *max8997 = rdev_get_drvdata(rdev);
 	struct i2c_client *i2c = max8997->iodev->i2c;
 	int reg, shift, mask, ret;
-	int rid = rdev_get_id(rdev);
 	u8 val;
 
 	ret = max8997_get_voltage_register(rdev, &reg, &shift, &mask);
 	if (ret)
 		return ret;
 
-	if ((rid == MAX8997_BUCK1 && max8997->buck1_gpiodvs) ||
-			(rid == MAX8997_BUCK2 && max8997->buck2_gpiodvs) ||
-			(rid == MAX8997_BUCK5 && max8997->buck5_gpiodvs))
-		reg += max8997->buck125_gpioindex;
-
 	ret = max8997_read_reg(i2c, reg, &val);
 	if (ret)
 		return ret;
@@ -400,22 +397,14 @@ static int max8997_get_voltage(struct regulator_dev *rdev)
 	val >>= shift;
 	val &= mask;
 
-	if (rdev->desc && rdev->desc->ops && rdev->desc->ops->list_voltage)
-		return rdev->desc->ops->list_voltage(rdev, val);
-
-	/*
-	 * max8997_list_voltage returns value for any rdev with voltage_map,
-	 * which works for "CHARGER" and "CHARGER TOPOFF" that do not have
-	 * list_voltage ops (they are current regulators).
-	 */
-	return max8997_list_voltage(rdev, val);
+	return val;
 }
 
 static inline int max8997_get_voltage_proper_val(
 		const struct voltage_map_desc *desc,
 		int min_vol, int max_vol)
 {
-	int i = 0;
+	int i;
 
 	if (desc == NULL)
 		return -EINVAL;
@@ -423,14 +412,12 @@ static inline int max8997_get_voltage_proper_val(
 	if (max_vol < desc->min || min_vol > desc->max)
 		return -EINVAL;
 
-	while (desc->min + desc->step * i < min_vol &&
-			desc->min + desc->step * i < desc->max)
-		i++;
+	if (min_vol < desc->min)
+		min_vol = desc->min;
 
-	if (desc->min + desc->step * i > max_vol)
-		return -EINVAL;
+	i = DIV_ROUND_UP(min_vol - desc->min, desc->step);
 
-	if (i >= (1 << desc->n_bits))
+	if (desc->min + desc->step * i > max_vol)
 		return -EINVAL;
 
 	return i;
@@ -499,9 +486,7 @@ static int max8997_set_voltage_ldobuck(struct regulator_dev *rdev,
 	int min_vol = min_uV / 1000, max_vol = max_uV / 1000;
 	const struct voltage_map_desc *desc;
 	int rid = rdev_get_id(rdev);
-	int reg, shift = 0, mask, ret;
-	int i;
-	u8 org;
+	int i, reg, shift, mask, ret;
 
 	switch (rid) {
 	case MAX8997_LDO1 ... MAX8997_LDO21:
@@ -530,21 +515,50 @@ static int max8997_set_voltage_ldobuck(struct regulator_dev *rdev,
 	if (ret)
 		return ret;
 
-	max8997_read_reg(i2c, reg, &org);
-	org = (org & mask) >> shift;
-
 	ret = max8997_update_reg(i2c, reg, i << shift, mask << shift);
 	*selector = i;
 
-	if (rid == MAX8997_BUCK1 || rid == MAX8997_BUCK2 ||
-			rid == MAX8997_BUCK4 || rid == MAX8997_BUCK5) {
-		/* If the voltage is increasing */
-		if (org < i)
-			udelay(DIV_ROUND_UP(desc->step * (i - org),
-						max8997->ramp_delay));
+	return ret;
+}
+
+static int max8997_set_voltage_ldobuck_time_sel(struct regulator_dev *rdev,
+						unsigned int old_selector,
+						unsigned int new_selector)
+{
+	struct max8997_data *max8997 = rdev_get_drvdata(rdev);
+	int rid = rdev_get_id(rdev);
+	const struct voltage_map_desc *desc = reg_voltage_map[rid];
+
+	/* Delay is required only if the voltage is increasing */
+	if (old_selector >= new_selector)
+		return 0;
+
+	/* No need to delay if gpio_dvs_mode */
+	switch (rid) {
+	case MAX8997_BUCK1:
+		if (max8997->buck1_gpiodvs)
+			return 0;
+		break;
+	case MAX8997_BUCK2:
+		if (max8997->buck2_gpiodvs)
+			return 0;
+		break;
+	case MAX8997_BUCK5:
+		if (max8997->buck5_gpiodvs)
+			return 0;
+		break;
 	}
 
-	return ret;
+	switch (rid) {
+	case MAX8997_BUCK1:
+	case MAX8997_BUCK2:
+	case MAX8997_BUCK4:
+	case MAX8997_BUCK5:
+		return DIV_ROUND_UP(desc->step * (new_selector - old_selector),
+				    max8997->ramp_delay);
+	}
+
+	return 0;
 }
 
 /*
@@ -684,7 +698,7 @@ static int max8997_set_voltage_buck(struct regulator_dev *rdev,
 		}
 
 		new_val++;
-	} while (desc->min + desc->step + new_val <= desc->max);
+	} while (desc->min + desc->step * new_val <= desc->max);
 
 	new_idx = tmp_idx;
 	new_val = tmp_val;
@@ -751,11 +765,6 @@ static int max8997_set_voltage_safeout(struct regulator_dev *rdev,
 	return ret;
 }
 
-static int max8997_reg_enable_suspend(struct regulator_dev *rdev)
-{
-	return 0;
-}
-
 static int max8997_reg_disable_suspend(struct regulator_dev *rdev)
 {
 	struct max8997_data *max8997 = rdev_get_drvdata(rdev);
@@ -788,9 +797,9 @@ static struct regulator_ops max8997_ldo_ops = {
 	.is_enabled		= max8997_reg_is_enabled,
 	.enable			= max8997_reg_enable,
 	.disable		= max8997_reg_disable,
-	.get_voltage		= max8997_get_voltage,
+	.get_voltage_sel	= max8997_get_voltage_sel,
 	.set_voltage		= max8997_set_voltage_ldobuck,
-	.set_suspend_enable	= max8997_reg_enable_suspend,
+	.set_voltage_time_sel	= max8997_set_voltage_ldobuck_time_sel,
 	.set_suspend_disable	= max8997_reg_disable_suspend,
 };
 
@@ -799,9 +808,9 @@ static struct regulator_ops max8997_buck_ops = {
 	.is_enabled		= max8997_reg_is_enabled,
 	.enable			= max8997_reg_enable,
 	.disable		= max8997_reg_disable,
-	.get_voltage		= max8997_get_voltage,
+	.get_voltage_sel	= max8997_get_voltage_sel,
 	.set_voltage		= max8997_set_voltage_buck,
-	.set_suspend_enable	= max8997_reg_enable_suspend,
+	.set_voltage_time_sel	= max8997_set_voltage_ldobuck_time_sel,
 	.set_suspend_disable	= max8997_reg_disable_suspend,
 };
 
@@ -810,7 +819,6 @@ static struct regulator_ops max8997_fixedvolt_ops = {
 	.is_enabled		= max8997_reg_is_enabled,
 	.enable			= max8997_reg_enable,
 	.disable		= max8997_reg_disable,
-	.set_suspend_enable	= max8997_reg_enable_suspend,
 	.set_suspend_disable	= max8997_reg_disable_suspend,
 };
 
@@ -819,144 +827,117 @@ static struct regulator_ops max8997_safeout_ops = {
 	.is_enabled		= max8997_reg_is_enabled,
 	.enable			= max8997_reg_enable,
 	.disable		= max8997_reg_disable,
-	.get_voltage		= max8997_get_voltage,
+	.get_voltage_sel	= max8997_get_voltage_sel,
 	.set_voltage		= max8997_set_voltage_safeout,
-	.set_suspend_enable	= max8997_reg_enable_suspend,
 	.set_suspend_disable	= max8997_reg_disable_suspend,
 };
 
 static struct regulator_ops max8997_fixedstate_ops = {
 	.list_voltage		= max8997_list_voltage_charger_cv,
-	.get_voltage		= max8997_get_voltage,
+	.get_voltage_sel	= max8997_get_voltage_sel,
 	.set_voltage		= max8997_set_voltage_charger_cv,
 };
 
-static int max8997_set_voltage_ldobuck_wrap(struct regulator_dev *rdev,
-		int min_uV, int max_uV)
+static int max8997_set_current_limit(struct regulator_dev *rdev,
+				     int min_uA, int max_uA)
 {
 	unsigned dummy;
+	int rid = rdev_get_id(rdev);
+
+	if (rid != MAX8997_CHARGER && rid != MAX8997_CHARGER_TOPOFF)
+		return -EINVAL;
 
-	return max8997_set_voltage_ldobuck(rdev, min_uV, max_uV, &dummy);
+	/* Reuse max8997_set_voltage_ldobuck to set current_limit. */
+	return max8997_set_voltage_ldobuck(rdev, min_uA, max_uA, &dummy);
 }
 
+static int max8997_get_current_limit(struct regulator_dev *rdev)
+{
+	int sel, rid = rdev_get_id(rdev);
+
+	if (rid != MAX8997_CHARGER && rid != MAX8997_CHARGER_TOPOFF)
+		return -EINVAL;
+
+	sel = max8997_get_voltage_sel(rdev);
+	if (sel < 0)
+		return sel;
+
+	/* Reuse max8997_list_voltage to get current_limit. */
+	return max8997_list_voltage(rdev, sel);
+}
 
 static struct regulator_ops max8997_charger_ops = {
 	.is_enabled		= max8997_reg_is_enabled,
 	.enable			= max8997_reg_enable,
 	.disable		= max8997_reg_disable,
-	.get_current_limit	= max8997_get_voltage,
-	.set_current_limit	= max8997_set_voltage_ldobuck_wrap,
+	.get_current_limit	= max8997_get_current_limit,
+	.set_current_limit	= max8997_set_current_limit,
 };
 
 static struct regulator_ops max8997_charger_fixedstate_ops = {
-	.is_enabled		= max8997_reg_is_enabled,
-	.get_current_limit	= max8997_get_voltage,
-	.set_current_limit	= max8997_set_voltage_ldobuck_wrap,
+	.get_current_limit	= max8997_get_current_limit,
+	.set_current_limit	= max8997_set_current_limit,
 };
 
-#define regulator_desc_ldo(num)		{	\
-	.name		= "LDO"#num,		\
-	.id		= MAX8997_LDO##num,	\
-	.ops		= &max8997_ldo_ops,	\
+#define MAX8997_VOLTAGE_REGULATOR(_name, _ops) {\
+	.name		= #_name,		\
+	.id		= MAX8997_##_name,	\
+	.ops		= &_ops,		\
 	.type		= REGULATOR_VOLTAGE,	\
 	.owner		= THIS_MODULE,		\
 }
-#define regulator_desc_buck(num)		{	\
-	.name		= "BUCK"#num,		\
-	.id		= MAX8997_BUCK##num,	\
-	.ops		= &max8997_buck_ops,	\
-	.type		= REGULATOR_VOLTAGE,	\
+
+#define MAX8997_CURRENT_REGULATOR(_name, _ops) {\
+	.name		= #_name,		\
+	.id		= MAX8997_##_name,	\
+	.ops		= &_ops,		\
+	.type		= REGULATOR_CURRENT,	\
 	.owner		= THIS_MODULE,		\
 }
 
 static struct regulator_desc regulators[] = {
-	regulator_desc_ldo(1),
-	regulator_desc_ldo(2),
-	regulator_desc_ldo(3),
-	regulator_desc_ldo(4),
-	regulator_desc_ldo(5),
-	regulator_desc_ldo(6),
-	regulator_desc_ldo(7),
-	regulator_desc_ldo(8),
-	regulator_desc_ldo(9),
-	regulator_desc_ldo(10),
-	regulator_desc_ldo(11),
-	regulator_desc_ldo(12),
-	regulator_desc_ldo(13),
-	regulator_desc_ldo(14),
-	regulator_desc_ldo(15),
-	regulator_desc_ldo(16),
-	regulator_desc_ldo(17),
-	regulator_desc_ldo(18),
-	regulator_desc_ldo(21),
-	regulator_desc_buck(1),
-	regulator_desc_buck(2),
-	regulator_desc_buck(3),
-	regulator_desc_buck(4),
-	regulator_desc_buck(5),
-	{
-		.name	= "BUCK6",
-		.id	= MAX8997_BUCK6,
-		.ops	= &max8997_fixedvolt_ops,
-		.type	= REGULATOR_VOLTAGE,
-		.owner	= THIS_MODULE,
-	},
-	regulator_desc_buck(7),
-	{
-		.name	= "EN32KHz_AP",
-		.id	= MAX8997_EN32KHZ_AP,
-		.ops	= &max8997_fixedvolt_ops,
-		.type	= REGULATOR_VOLTAGE,
-		.owner	= THIS_MODULE,
-	}, {
-		.name	= "EN32KHz_CP",
-		.id	= MAX8997_EN32KHZ_CP,
-		.ops	= &max8997_fixedvolt_ops,
-		.type	= REGULATOR_VOLTAGE,
-		.owner	= THIS_MODULE,
-	}, {
-		.name	= "ENVICHG",
-		.id	= MAX8997_ENVICHG,
-		.ops	= &max8997_fixedvolt_ops,
-		.type	= REGULATOR_VOLTAGE,
-		.owner	= THIS_MODULE,
-	}, {
-		.name	= "ESAFEOUT1",
-		.id	= MAX8997_ESAFEOUT1,
-		.ops	= &max8997_safeout_ops,
-		.type	= REGULATOR_VOLTAGE,
-		.owner	 = THIS_MODULE,
-	}, {
-		.name	= "ESAFEOUT2",
-		.id	= MAX8997_ESAFEOUT2,
-		.ops	= &max8997_safeout_ops,
-		.type	= REGULATOR_VOLTAGE,
-		.owner	 = THIS_MODULE,
-	}, {
-		.name	= "CHARGER_CV",
-		.id	= MAX8997_CHARGER_CV,
-		.ops	= &max8997_fixedstate_ops,
-		.type	= REGULATOR_VOLTAGE,
-		.owner	 = THIS_MODULE,
-	}, {
-		.name	= "CHARGER",
-		.id	= MAX8997_CHARGER,
-		.ops	= &max8997_charger_ops,
-		.type	= REGULATOR_CURRENT,
-		.owner	 = THIS_MODULE,
-	}, {
-		.name	= "CHARGER_TOPOFF",
-		.id	= MAX8997_CHARGER_TOPOFF,
-		.ops	= &max8997_charger_fixedstate_ops,
-		.type	= REGULATOR_CURRENT,
-		.owner	 = THIS_MODULE,
-	},
+	MAX8997_VOLTAGE_REGULATOR(LDO1, max8997_ldo_ops),
+	MAX8997_VOLTAGE_REGULATOR(LDO2, max8997_ldo_ops),
+	MAX8997_VOLTAGE_REGULATOR(LDO3, max8997_ldo_ops),
+	MAX8997_VOLTAGE_REGULATOR(LDO4, max8997_ldo_ops),
+	MAX8997_VOLTAGE_REGULATOR(LDO5, max8997_ldo_ops),
+	MAX8997_VOLTAGE_REGULATOR(LDO6, max8997_ldo_ops),
+	MAX8997_VOLTAGE_REGULATOR(LDO7, max8997_ldo_ops),
+	MAX8997_VOLTAGE_REGULATOR(LDO8, max8997_ldo_ops),
+	MAX8997_VOLTAGE_REGULATOR(LDO9, max8997_ldo_ops),
+	MAX8997_VOLTAGE_REGULATOR(LDO10, max8997_ldo_ops),
+	MAX8997_VOLTAGE_REGULATOR(LDO11, max8997_ldo_ops),
+	MAX8997_VOLTAGE_REGULATOR(LDO12, max8997_ldo_ops),
+	MAX8997_VOLTAGE_REGULATOR(LDO13, max8997_ldo_ops),
+	MAX8997_VOLTAGE_REGULATOR(LDO14, max8997_ldo_ops),
+	MAX8997_VOLTAGE_REGULATOR(LDO15, max8997_ldo_ops),
+	MAX8997_VOLTAGE_REGULATOR(LDO16, max8997_ldo_ops),
+	MAX8997_VOLTAGE_REGULATOR(LDO17, max8997_ldo_ops),
+	MAX8997_VOLTAGE_REGULATOR(LDO18, max8997_ldo_ops),
+	MAX8997_VOLTAGE_REGULATOR(LDO21, max8997_ldo_ops),
+	MAX8997_VOLTAGE_REGULATOR(BUCK1, max8997_buck_ops),
+	MAX8997_VOLTAGE_REGULATOR(BUCK2, max8997_buck_ops),
+	MAX8997_VOLTAGE_REGULATOR(BUCK3, max8997_buck_ops),
+	MAX8997_VOLTAGE_REGULATOR(BUCK4, max8997_buck_ops),
+	MAX8997_VOLTAGE_REGULATOR(BUCK5, max8997_buck_ops),
+	MAX8997_VOLTAGE_REGULATOR(BUCK6, max8997_fixedvolt_ops),
+	MAX8997_VOLTAGE_REGULATOR(BUCK7, max8997_buck_ops),
+	MAX8997_VOLTAGE_REGULATOR(EN32KHZ_AP, max8997_fixedvolt_ops),
+	MAX8997_VOLTAGE_REGULATOR(EN32KHZ_CP, max8997_fixedvolt_ops),
+	MAX8997_VOLTAGE_REGULATOR(ENVICHG, max8997_fixedvolt_ops),
+	MAX8997_VOLTAGE_REGULATOR(ESAFEOUT1, max8997_safeout_ops),
+	MAX8997_VOLTAGE_REGULATOR(ESAFEOUT2, max8997_safeout_ops),
+	MAX8997_VOLTAGE_REGULATOR(CHARGER_CV, max8997_fixedstate_ops),
+	MAX8997_CURRENT_REGULATOR(CHARGER, max8997_charger_ops),
+	MAX8997_CURRENT_REGULATOR(CHARGER_TOPOFF,
+				  max8997_charger_fixedstate_ops),
 };
 
 static __devinit int max8997_pmic_probe(struct platform_device *pdev)
 {
 	struct max8997_dev *iodev = dev_get_drvdata(pdev->dev.parent);
 	struct max8997_platform_data *pdata = dev_get_platdata(iodev->dev);
+	struct regulator_config config = { };
 	struct regulator_dev **rdev;
 	struct max8997_data *max8997;
 	struct i2c_client *i2c;
@@ -968,16 +949,15 @@ static __devinit int max8997_pmic_probe(struct platform_device *pdev)
 		return -ENODEV;
 	}
 
-	max8997 = kzalloc(sizeof(struct max8997_data), GFP_KERNEL);
+	max8997 = devm_kzalloc(&pdev->dev, sizeof(struct max8997_data),
+			       GFP_KERNEL);
 	if (!max8997)
 		return -ENOMEM;
 
 	size = sizeof(struct regulator_dev *) * pdata->num_regulators;
-	max8997->rdev = kzalloc(size, GFP_KERNEL);
-	if (!max8997->rdev) {
-		kfree(max8997);
+	max8997->rdev = devm_kzalloc(&pdev->dev, size, GFP_KERNEL);
+	if (!max8997->rdev)
 		return -ENOMEM;
-	}
 
 	rdev = max8997->rdev;
 	max8997->dev = &pdev->dev;
@@ -1001,7 +981,7 @@ static __devinit int max8997_pmic_probe(struct platform_device *pdev)
 					pdata->buck1_voltage[i] / 1000 +
 					buck1245_voltage_map_desc.step);
 		if (ret < 0)
-			goto err_alloc;
+			goto err_out;
 
 		max8997->buck2_vol[i] = ret =
 			max8997_get_voltage_proper_val(
@@ -1010,7 +990,7 @@ static __devinit int max8997_pmic_probe(struct platform_device *pdev)
 					pdata->buck2_voltage[i] / 1000 +
 					buck1245_voltage_map_desc.step);
 		if (ret < 0)
-			goto err_alloc;
+			goto err_out;
 
 		max8997->buck5_vol[i] = ret =
 			max8997_get_voltage_proper_val(
@@ -1019,7 +999,7 @@ static __devinit int max8997_pmic_probe(struct platform_device *pdev)
 					pdata->buck5_voltage[i] / 1000 +
 					buck1245_voltage_map_desc.step);
 		if (ret < 0)
-			goto err_alloc;
+			goto err_out;
 
 		if (max_buck1 < max8997->buck1_vol[i])
 			max_buck1 = max8997->buck1_vol[i];
@@ -1052,7 +1032,7 @@ static __devinit int max8997_pmic_probe(struct platform_device *pdev)
 				!gpio_is_valid(pdata->buck125_gpios[2])) {
 			dev_err(&pdev->dev, "GPIO NOT VALID\n");
 			ret = -EINVAL;
-			goto err_alloc;
+			goto err_out;
 		}
 
 		ret = gpio_request(pdata->buck125_gpios[0],
@@ -1061,7 +1041,7 @@ static __devinit int max8997_pmic_probe(struct platform_device *pdev)
 			dev_warn(&pdev->dev, "Duplicated gpio request"
 					" on SET1\n");
 		else if (ret)
-			goto err_alloc;
+			goto err_out;
 		else
 			gpio1set = true;
 
@@ -1073,7 +1053,7 @@ static __devinit int max8997_pmic_probe(struct platform_device *pdev)
 		else if (ret) {
 			if (gpio1set)
 				gpio_free(pdata->buck125_gpios[0]);
-			goto err_alloc;
+			goto err_out;
 		} else
 			gpio2set = true;
 
@@ -1087,7 +1067,7 @@ static __devinit int max8997_pmic_probe(struct platform_device *pdev)
 				gpio_free(pdata->buck125_gpios[0]);
 			if (gpio2set)
 				gpio_free(pdata->buck125_gpios[1]);
-			goto err_alloc;
+			goto err_out;
 		}
 
 		gpio_direction_output(pdata->buck125_gpios[0],
@@ -1140,8 +1120,11 @@ static __devinit int max8997_pmic_probe(struct platform_device *pdev)
 		else if (id == MAX8997_CHARGER_CV)
 			regulators[id].n_voltages = 16;
 
-		rdev[i] = regulator_register(&regulators[id], max8997->dev,
-				pdata->regulators[i].initdata, max8997, NULL);
+		config.dev = max8997->dev;
+		config.init_data = pdata->regulators[i].initdata;
+		config.driver_data = max8997;
+
+		rdev[i] = regulator_register(&regulators[id], &config);
 		if (IS_ERR(rdev[i])) {
 			ret = PTR_ERR(rdev[i]);
 			dev_err(max8997->dev, "regulator init failed for %d\n",
@@ -1153,13 +1136,9 @@ static __devinit int max8997_pmic_probe(struct platform_device *pdev)
 
 	return 0;
 err:
-	for (i = 0; i < max8997->num_regulators; i++)
-		if (rdev[i])
-			regulator_unregister(rdev[i]);
-err_alloc:
-	kfree(max8997->rdev);
-	kfree(max8997);
-
+	while (--i >= 0)
+		regulator_unregister(rdev[i]);
+err_out:
 	return ret;
 }
 
@@ -1170,12 +1149,7 @@ static int __devexit max8997_pmic_remove(struct platform_device *pdev)
 	int i;
 
 	for (i = 0; i < max8997->num_regulators; i++)
-		if (rdev[i])
-			regulator_unregister(rdev[i]);
-
-	kfree(max8997->rdev);
-	kfree(max8997);
-
+		regulator_unregister(rdev[i]);
 	return 0;
 }
 
diff --git a/drivers/regulator/max8998.c b/drivers/regulator/max8998.c
index 5890265eeac..18bb58b9b96 100644
--- a/drivers/regulator/max8998.c
+++ b/drivers/regulator/max8998.c
@@ -28,7 +28,6 @@
 #include <linux/slab.h>
 #include <linux/interrupt.h>
 #include <linux/mutex.h>
-#include <linux/delay.h>
 #include <linux/platform_device.h>
 #include <linux/regulator/driver.h>
 #include <linux/mfd/max8998.h>
@@ -277,7 +276,7 @@ static int max8998_get_voltage_register(struct regulator_dev *rdev,
 	return 0;
 }
 
-static int max8998_get_voltage(struct regulator_dev *rdev)
+static int max8998_get_voltage_sel(struct regulator_dev *rdev)
 {
 	struct max8998_data *max8998 = rdev_get_drvdata(rdev);
 	struct i2c_client *i2c = max8998->iodev->i2c;
@@ -295,7 +294,7 @@ static int max8998_get_voltage(struct regulator_dev *rdev)
 	val >>= shift;
 	val &= mask;
 
-	return max8998_list_voltage(rdev, val);
+	return val;
 }
 
 static int max8998_set_voltage_ldo(struct regulator_dev *rdev,
@@ -306,8 +305,7 @@ static int max8998_set_voltage_ldo(struct regulator_dev *rdev,
 	int min_vol = min_uV / 1000, max_vol = max_uV / 1000;
 	const struct voltage_map_desc *desc;
 	int ldo = rdev_get_id(rdev);
-	int reg, shift = 0, mask, ret;
-	int i = 0;
+	int reg, shift = 0, mask, ret, i;
 
 	if (ldo >= ARRAY_SIZE(ldo_voltage_map))
 		return -EINVAL;
@@ -319,9 +317,10 @@ static int max8998_set_voltage_ldo(struct regulator_dev *rdev,
 	if (max_vol < desc->min || min_vol > desc->max)
 		return -EINVAL;
 
-	while (desc->min + desc->step*i < min_vol &&
-	       desc->min + desc->step*i < desc->max)
-		i++;
+	if (min_vol < desc->min)
+		min_vol = desc->min;
+
+	i = DIV_ROUND_UP(min_vol - desc->min, desc->step);
 
 	if (desc->min + desc->step*i > max_vol)
 		return -EINVAL;
@@ -359,8 +358,7 @@ static int max8998_set_voltage_buck(struct regulator_dev *rdev,
 	const struct voltage_map_desc *desc;
 	int buck = rdev_get_id(rdev);
 	int reg, shift = 0, mask, ret;
-	int difference = 0, i = 0, j = 0, previous_vol = 0;
-	u8 val = 0;
+	int i, j, previous_sel;
 	static u8 buck1_last_val;
 
 	if (buck >= ARRAY_SIZE(ldo_voltage_map))
@@ -374,9 +372,10 @@ static int max8998_set_voltage_buck(struct regulator_dev *rdev,
 	if (max_vol < desc->min || min_vol > desc->max)
 		return -EINVAL;
 
-	while (desc->min + desc->step*i < min_vol &&
-	       desc->min + desc->step*i < desc->max)
-		i++;
+	if (min_vol < desc->min)
+		min_vol = desc->min;
+
+	i = DIV_ROUND_UP(min_vol - desc->min, desc->step);
 
 	if (desc->min + desc->step*i > max_vol)
 		return -EINVAL;
@@ -387,13 +386,14 @@ static int max8998_set_voltage_buck(struct regulator_dev *rdev,
 	if (ret)
 		return ret;
 
-	previous_vol = max8998_get_voltage(rdev);
+	previous_sel = max8998_get_voltage_sel(rdev);
 
 	/* Check if voltage needs to be changed */
 	/* if previous_voltage equal new voltage, return */
-	if (previous_vol == max8998_list_voltage(rdev, i)) {
+	if (previous_sel == i) {
 		dev_dbg(max8998->dev, "No voltage change, old:%d, new:%d\n",
-			previous_vol, max8998_list_voltage(rdev, i));
+			max8998_list_voltage(rdev, previous_sel),
+			max8998_list_voltage(rdev, i));
 		return ret;
 	}
 
@@ -482,19 +482,40 @@ buck2_exit:
 		break;
 	}
 
+	return ret;
+}
+
+static int max8998_set_voltage_buck_time_sel(struct regulator_dev *rdev,
+					     unsigned int old_selector,
+					     unsigned int new_selector)
+{
+	struct max8998_data *max8998 = rdev_get_drvdata(rdev);
+	struct i2c_client *i2c = max8998->iodev->i2c;
+	const struct voltage_map_desc *desc;
+	int buck = rdev_get_id(rdev);
+	u8 val = 0;
+	int difference, ret;
+
+	if (buck < MAX8998_BUCK1 || buck > MAX8998_BUCK4)
+		return -EINVAL;
+
+	desc = ldo_voltage_map[buck];
+
 	/* Voltage stabilization */
-	max8998_read_reg(i2c, MAX8998_REG_ONOFF4, &val);
+	ret = max8998_read_reg(i2c, MAX8998_REG_ONOFF4, &val);
+	if (ret)
+		return ret;
 
 	/* lp3974 hasn't got ENRAMP bit - ramp is assumed as true */
 	/* MAX8998 has ENRAMP bit implemented, so test it*/
 	if (max8998->iodev->type == TYPE_MAX8998 && !(val & MAX8998_ENRAMP))
-		return ret;
+		return 0;
 
-	difference = desc->min + desc->step*i - previous_vol/1000;
+	difference = (new_selector - old_selector) * desc->step;
 	if (difference > 0)
-		udelay(difference / ((val & 0x0f) + 1));
+		return difference / ((val & 0x0f) + 1);
 
-	return ret;
+	return 0;
 }
 
 static struct regulator_ops max8998_ldo_ops = {
@@ -502,7 +523,7 @@ static struct regulator_ops max8998_ldo_ops = {
 	.is_enabled		= max8998_ldo_is_enabled,
 	.enable			= max8998_ldo_enable,
 	.disable		= max8998_ldo_disable,
-	.get_voltage		= max8998_get_voltage,
+	.get_voltage_sel	= max8998_get_voltage_sel,
 	.set_voltage		= max8998_set_voltage_ldo,
 	.set_suspend_enable	= max8998_ldo_enable,
 	.set_suspend_disable	= max8998_ldo_disable,
@@ -513,8 +534,9 @@ static struct regulator_ops max8998_buck_ops = {
 	.is_enabled		= max8998_ldo_is_enabled,
 	.enable			= max8998_ldo_enable,
 	.disable		= max8998_ldo_disable,
-	.get_voltage		= max8998_get_voltage,
+	.get_voltage_sel	= max8998_get_voltage_sel,
 	.set_voltage		= max8998_set_voltage_buck,
+	.set_voltage_time_sel	= max8998_set_voltage_buck_time_sel,
 	.set_suspend_enable	= max8998_ldo_enable,
 	.set_suspend_disable	= max8998_ldo_disable,
 };
@@ -685,6 +707,7 @@ static __devinit int max8998_pmic_probe(struct platform_device *pdev)
 {
 	struct max8998_dev *iodev = dev_get_drvdata(pdev->dev.parent);
 	struct max8998_platform_data *pdata = dev_get_platdata(iodev->dev);
+	struct regulator_config config = { };
 	struct regulator_dev **rdev;
 	struct max8998_data *max8998;
 	struct i2c_client *i2c;
@@ -695,16 +718,15 @@ static __devinit int max8998_pmic_probe(struct platform_device *pdev)
 		return -ENODEV;
 	}
 
-	max8998 = kzalloc(sizeof(struct max8998_data), GFP_KERNEL);
+	max8998 = devm_kzalloc(&pdev->dev, sizeof(struct max8998_data),
+			       GFP_KERNEL);
 	if (!max8998)
 		return -ENOMEM;
 
 	size = sizeof(struct regulator_dev *) * pdata->num_regulators;
-	max8998->rdev = kzalloc(size, GFP_KERNEL);
-	if (!max8998->rdev) {
-		kfree(max8998);
+	max8998->rdev = devm_kzalloc(&pdev->dev, size, GFP_KERNEL);
+	if (!max8998->rdev)
 		return -ENOMEM;
-	}
 
 	rdev = max8998->rdev;
 	max8998->dev = &pdev->dev;
@@ -728,14 +750,14 @@ static __devinit int max8998_pmic_probe(struct platform_device *pdev)
 			printk(KERN_ERR "MAX8998 SET1 GPIO defined as 0 !\n");
 			WARN_ON(!pdata->buck1_set1);
 			ret = -EIO;
-			goto err_free_mem;
+			goto err_out;
 		}
 		/* Check if SET2 is not equal to 0 */
 		if (!pdata->buck1_set2) {
 			printk(KERN_ERR "MAX8998 SET2 GPIO defined as 0 !\n");
 			WARN_ON(!pdata->buck1_set2);
 			ret = -EIO;
-			goto err_free_mem;
+			goto err_out;
 		}
 
 		gpio_request(pdata->buck1_set1, "MAX8998 BUCK1_SET1");
@@ -755,7 +777,7 @@ static __devinit int max8998_pmic_probe(struct platform_device *pdev)
 		max8998->buck1_vol[0] = i;
 		ret = max8998_write_reg(i2c, MAX8998_REG_BUCK1_VOLTAGE1, i);
 		if (ret)
-			goto err_free_mem;
+			goto err_out;
 
 		/* Set predefined value for BUCK1 register 2 */
 		i = 0;
@@ -767,7 +789,7 @@ static __devinit int max8998_pmic_probe(struct platform_device *pdev)
 		max8998->buck1_vol[1] = i;
 		ret = max8998_write_reg(i2c, MAX8998_REG_BUCK1_VOLTAGE2, i);
 		if (ret)
-			goto err_free_mem;
+			goto err_out;
 
 		/* Set predefined value for BUCK1 register 3 */
 		i = 0;
@@ -779,7 +801,7 @@ static __devinit int max8998_pmic_probe(struct platform_device *pdev)
 		max8998->buck1_vol[2] = i;
 		ret = max8998_write_reg(i2c, MAX8998_REG_BUCK1_VOLTAGE3, i);
 		if (ret)
-			goto err_free_mem;
+			goto err_out;
 
 		/* Set predefined value for BUCK1 register 4 */
 		i = 0;
@@ -791,7 +813,7 @@ static __devinit int max8998_pmic_probe(struct platform_device *pdev)
 		max8998->buck1_vol[3] = i;
 		ret = max8998_write_reg(i2c, MAX8998_REG_BUCK1_VOLTAGE4, i);
 		if (ret)
-			goto err_free_mem;
+			goto err_out;
 
 	}
 
@@ -801,7 +823,7 @@ static __devinit int max8998_pmic_probe(struct platform_device *pdev)
 			printk(KERN_ERR "MAX8998 SET3 GPIO defined as 0 !\n");
 			WARN_ON(!pdata->buck2_set3);
 			ret = -EIO;
-			goto err_free_mem;
+			goto err_out;
 		}
 		gpio_request(pdata->buck2_set3, "MAX8998 BUCK2_SET3");
 		gpio_direction_output(pdata->buck2_set3,
@@ -816,7 +838,7 @@ static __devinit int max8998_pmic_probe(struct platform_device *pdev)
 		max8998->buck2_vol[0] = i;
 		ret = max8998_write_reg(i2c, MAX8998_REG_BUCK2_VOLTAGE1, i);
 		if (ret)
-			goto err_free_mem;
+			goto err_out;
 
 		/* BUCK2 register 2 */
 		i = 0;
@@ -827,7 +849,7 @@ static __devinit int max8998_pmic_probe(struct platform_device *pdev)
 		max8998->buck2_vol[1] = i;
 		ret = max8998_write_reg(i2c, MAX8998_REG_BUCK2_VOLTAGE2, i);
 		if (ret)
-			goto err_free_mem;
+			goto err_out;
 	}
 
 	for (i = 0; i < pdata->num_regulators; i++) {
@@ -840,8 +862,12 @@ static __devinit int max8998_pmic_probe(struct platform_device *pdev)
 			int count = (desc->max - desc->min) / desc->step + 1;
 			regulators[index].n_voltages = count;
 		}
-		rdev[i] = regulator_register(&regulators[index], max8998->dev,
-				pdata->regulators[i].initdata, max8998, NULL);
+
+		config.dev = max8998->dev;
+		config.init_data = pdata->regulators[i].initdata;
+		config.driver_data = max8998;
+
+		rdev[i] = regulator_register(&regulators[index], &config);
 		if (IS_ERR(rdev[i])) {
 			ret = PTR_ERR(rdev[i]);
 			dev_err(max8998->dev, "regulator init failed\n");
@@ -853,14 +879,9 @@ static __devinit int max8998_pmic_probe(struct platform_device *pdev)
 
 	return 0;
 err:
-	for (i = 0; i < max8998->num_regulators; i++)
-		if (rdev[i])
-			regulator_unregister(rdev[i]);
-
-err_free_mem:
-	kfree(max8998->rdev);
-	kfree(max8998);
-
+	while (--i >= 0)
+		regulator_unregister(rdev[i]);
+err_out:
 	return ret;
 }
 
@@ -871,12 +892,7 @@ static int __devexit max8998_pmic_remove(struct platform_device *pdev)
 	int i;
 
 	for (i = 0; i < max8998->num_regulators; i++)
-		if (rdev[i])
-			regulator_unregister(rdev[i]);
-
-	kfree(max8998->rdev);
-	kfree(max8998);
-
+		regulator_unregister(rdev[i]);
 	return 0;
 }
 
diff --git a/drivers/regulator/mc13783-regulator.c b/drivers/regulator/mc13783-regulator.c
index 6c0face87ff..7dcdfa283e9 100644
--- a/drivers/regulator/mc13783-regulator.c
+++ b/drivers/regulator/mc13783-regulator.c
@@ -340,6 +340,7 @@ static int __devinit mc13783_regulator_probe(struct platform_device *pdev)
 	struct mc13xxx_regulator_platform_data *pdata =
 		dev_get_platdata(&pdev->dev);
 	struct mc13xxx_regulator_init_data *init_data;
+	struct regulator_config config = { };
 	int i, ret;
 
 	dev_dbg(&pdev->dev, "%s id %d\n", __func__, pdev->id);
@@ -357,11 +358,16 @@ static int __devinit mc13783_regulator_probe(struct platform_device *pdev)
 	priv->mc13xxx = mc13783;
 
 	for (i = 0; i < pdata->num_regulators; i++) {
+		struct regulator_desc *desc;
+
 		init_data = &pdata->regulators[i];
-		priv->regulators[i] = regulator_register(
-				&mc13783_regulators[init_data->id].desc,
-				&pdev->dev, init_data->init_data, priv, NULL);
+		desc = &mc13783_regulators[init_data->id].desc;
+
+		config.dev = &pdev->dev;
+		config.init_data = init_data->init_data;
+		config.driver_data = priv;
 
+		priv->regulators[i] = regulator_register(desc, &config);
 		if (IS_ERR(priv->regulators[i])) {
 			dev_err(&pdev->dev, "failed to register regulator %s\n",
 				mc13783_regulators[i].desc.name);
diff --git a/drivers/regulator/mc13892-regulator.c b/drivers/regulator/mc13892-regulator.c
index 845aa2263b8..970a233dbe4 100644
--- a/drivers/regulator/mc13892-regulator.c
+++ b/drivers/regulator/mc13892-regulator.c
@@ -428,24 +428,15 @@ static int mc13892_sw_regulator_get_voltage(struct regulator_dev *rdev)
 	return val;
 }
 
-static int mc13892_sw_regulator_set_voltage(struct regulator_dev *rdev,
-		int min_uV, int max_uV, unsigned *selector)
+static int mc13892_sw_regulator_set_voltage_sel(struct regulator_dev *rdev,
+						unsigned selector)
 {
 	struct mc13xxx_regulator_priv *priv = rdev_get_drvdata(rdev);
 	int hi, value, mask, id = rdev_get_id(rdev);
 	u32 valread;
 	int ret;
 
-	dev_dbg(rdev_get_dev(rdev), "%s id: %d min_uV: %d max_uV: %d\n",
-		__func__, id, min_uV, max_uV);
-
-	/* Find the best index */
-	value = mc13xxx_get_best_voltage_index(rdev, min_uV, max_uV);
-	dev_dbg(rdev_get_dev(rdev), "%s best value: %d\n", __func__, value);
-	if (value < 0)
-		return value;
-
-	value = mc13892_regulators[id].voltages[value];
+	value = mc13892_regulators[id].voltages[selector];
 
 	mc13xxx_lock(priv->mc13xxx);
 	ret = mc13xxx_reg_read(priv->mc13xxx,
@@ -480,7 +471,7 @@ err:
 static struct regulator_ops mc13892_sw_regulator_ops = {
 	.is_enabled = mc13xxx_sw_regulator_is_enabled,
 	.list_voltage = mc13xxx_regulator_list_voltage,
-	.set_voltage = mc13892_sw_regulator_set_voltage,
+	.set_voltage_sel = mc13892_sw_regulator_set_voltage_sel,
 	.get_voltage = mc13892_sw_regulator_get_voltage,
 };
 
@@ -528,6 +519,7 @@ static int __devinit mc13892_regulator_probe(struct platform_device *pdev)
 	struct mc13xxx_regulator_platform_data *pdata =
 		dev_get_platdata(&pdev->dev);
 	struct mc13xxx_regulator_init_data *mc13xxx_data;
+	struct regulator_config config = { };
 	int i, ret;
 	int num_regulators = 0;
 	u32 val;
@@ -597,9 +589,12 @@ static int __devinit mc13892_regulator_probe(struct platform_device *pdev)
 		}
 		desc = &mc13892_regulators[id].desc;
 
-		priv->regulators[i] = regulator_register(
-			desc, &pdev->dev, init_data, priv, node);
+		config.dev = &pdev->dev;
+		config.init_data = init_data;
+		config.driver_data = priv;
+		config.of_node = node;
 
+		priv->regulators[i] = regulator_register(desc, &config);
 		if (IS_ERR(priv->regulators[i])) {
 			dev_err(&pdev->dev, "failed to register regulator %s\n",
 				mc13892_regulators[i].desc.name);
diff --git a/drivers/regulator/mc13xxx-regulator-core.c b/drivers/regulator/mc13xxx-regulator-core.c
index 62dcd0a432b..4fa9704739b 100644
--- a/drivers/regulator/mc13xxx-regulator-core.c
+++ b/drivers/regulator/mc13xxx-regulator-core.c
@@ -94,62 +94,18 @@ int mc13xxx_regulator_list_voltage(struct regulator_dev *rdev,
 }
 EXPORT_SYMBOL_GPL(mc13xxx_regulator_list_voltage);
 
-int mc13xxx_get_best_voltage_index(struct regulator_dev *rdev,
-						int min_uV, int max_uV)
+static int mc13xxx_regulator_set_voltage_sel(struct regulator_dev *rdev,
+					     unsigned selector)
 {
 	struct mc13xxx_regulator_priv *priv = rdev_get_drvdata(rdev);
 	struct mc13xxx_regulator *mc13xxx_regulators = priv->mc13xxx_regulators;
-	int reg_id = rdev_get_id(rdev);
-	int i;
-	int bestmatch;
-	int bestindex;
-
-	/*
-	 * Locate the minimum voltage fitting the criteria on
-	 * this regulator. The switchable voltages are not
-	 * in strict falling order so we need to check them
-	 * all for the best match.
-	 */
-	bestmatch = INT_MAX;
-	bestindex = -1;
-	for (i = 0; i < mc13xxx_regulators[reg_id].desc.n_voltages; i++) {
-		if (mc13xxx_regulators[reg_id].voltages[i] >= min_uV &&
-		    mc13xxx_regulators[reg_id].voltages[i] < bestmatch) {
-			bestmatch = mc13xxx_regulators[reg_id].voltages[i];
-			bestindex = i;
-		}
-	}
-
-	if (bestindex < 0 || bestmatch > max_uV) {
-		dev_warn(&rdev->dev, "no possible value for %d<=x<=%d uV\n",
-				min_uV, max_uV);
-		return -EINVAL;
-	}
-	return bestindex;
-}
-EXPORT_SYMBOL_GPL(mc13xxx_get_best_voltage_index);
-
-static int mc13xxx_regulator_set_voltage(struct regulator_dev *rdev, int min_uV,
-		int max_uV, unsigned *selector)
-{
-	struct mc13xxx_regulator_priv *priv = rdev_get_drvdata(rdev);
-	struct mc13xxx_regulator *mc13xxx_regulators = priv->mc13xxx_regulators;
-	int value, id = rdev_get_id(rdev);
+	int id = rdev_get_id(rdev);
 	int ret;
 
-	dev_dbg(rdev_get_dev(rdev), "%s id: %d min_uV: %d max_uV: %d\n",
-		__func__, id, min_uV, max_uV);
-
-	/* Find the best index */
-	value = mc13xxx_get_best_voltage_index(rdev, min_uV, max_uV);
-	dev_dbg(rdev_get_dev(rdev), "%s best value: %d\n", __func__, value);
-	if (value < 0)
-		return value;
-
 	mc13xxx_lock(priv->mc13xxx);
 	ret = mc13xxx_reg_rmw(priv->mc13xxx, mc13xxx_regulators[id].vsel_reg,
 			mc13xxx_regulators[id].vsel_mask,
-			value << mc13xxx_regulators[id].vsel_shift);
+			selector << mc13xxx_regulators[id].vsel_shift);
 	mc13xxx_unlock(priv->mc13xxx);
 
 	return ret;
@@ -187,7 +143,7 @@ struct regulator_ops mc13xxx_regulator_ops = {
 	.disable = mc13xxx_regulator_disable,
 	.is_enabled = mc13xxx_regulator_is_enabled,
 	.list_voltage = mc13xxx_regulator_list_voltage,
-	.set_voltage = mc13xxx_regulator_set_voltage,
+	.set_voltage_sel = mc13xxx_regulator_set_voltage_sel,
 	.get_voltage = mc13xxx_regulator_get_voltage,
 };
 EXPORT_SYMBOL_GPL(mc13xxx_regulator_ops);
diff --git a/drivers/regulator/mc13xxx.h b/drivers/regulator/mc13xxx.h
index b3961c658b0..044aba4d28e 100644
--- a/drivers/regulator/mc13xxx.h
+++ b/drivers/regulator/mc13xxx.h
@@ -35,8 +35,6 @@ struct mc13xxx_regulator_priv {
 
 extern int mc13xxx_sw_regulator(struct regulator_dev *rdev);
 extern int mc13xxx_sw_regulator_is_enabled(struct regulator_dev *rdev);
-extern int mc13xxx_get_best_voltage_index(struct regulator_dev *rdev,
-						int min_uV, int max_uV);
 extern int mc13xxx_regulator_list_voltage(struct regulator_dev *rdev,
 						unsigned selector);
 extern int mc13xxx_fixed_regulator_set_voltage(struct regulator_dev *rdev,
diff --git a/drivers/regulator/of_regulator.c b/drivers/regulator/of_regulator.c
index 679734d26a1..56593b75168 100644
--- a/drivers/regulator/of_regulator.c
+++ b/drivers/regulator/of_regulator.c
@@ -14,6 +14,7 @@
 #include <linux/slab.h>
 #include <linux/of.h>
 #include <linux/regulator/machine.h>
+#include <linux/regulator/of_regulator.h>
 
 static void of_get_regulation_constraints(struct device_node *np,
 					struct regulator_init_data **init_data)
@@ -85,3 +86,49 @@ struct regulator_init_data *of_get_regulator_init_data(struct device *dev,
 	return init_data;
 }
 EXPORT_SYMBOL_GPL(of_get_regulator_init_data);
+
+/**
+ * of_regulator_match - extract regulator init data
+ * @dev: device requesting the data
+ * @node: parent device node of the regulators
+ * @matches: match table for the regulators
+ * @num_matches: number of entries in match table
+ *
+ * This function uses a match table specified by the regulator driver and
+ * looks up the corresponding init data in the device tree. Note that the
+ * match table is modified in place.
+ *
+ * Returns the number of matches found or a negative error code on failure.
+ */
+int of_regulator_match(struct device *dev, struct device_node *node,
+		       struct of_regulator_match *matches,
+		       unsigned int num_matches)
+{
+	unsigned int count = 0;
+	unsigned int i;
+
+	if (!dev || !node)
+		return -EINVAL;
+
+	for (i = 0; i < num_matches; i++) {
+		struct of_regulator_match *match = &matches[i];
+		struct device_node *child;
+
+		child = of_find_node_by_name(node, match->name);
+		if (!child)
+			continue;
+
+		match->init_data = of_get_regulator_init_data(dev, child);
+		if (!match->init_data) {
+			dev_err(dev, "failed to parse DT for regulator %s\n",
+				child->name);
+			return -EINVAL;
+		}
+
+		match->of_node = child;
+		count++;
+	}
+
+	return count;
+}
+EXPORT_SYMBOL_GPL(of_regulator_match);
diff --git a/drivers/regulator/palmas-regulator.c b/drivers/regulator/palmas-regulator.c
new file mode 100644
index 00000000000..c4435f608df
--- /dev/null
+++ b/drivers/regulator/palmas-regulator.c
@@ -0,0 +1,822 @@
+/*
+ * Driver for Regulator part of Palmas PMIC Chips
+ *
+ * Copyright 2011-2012 Texas Instruments Inc.
+ *
+ * Author: Graeme Gregory <gg@slimlogic.co.uk>
+ *
+ *  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/init.h>
+#include <linux/err.h>
+#include <linux/platform_device.h>
+#include <linux/regulator/driver.h>
+#include <linux/regulator/machine.h>
+#include <linux/slab.h>
+#include <linux/regmap.h>
+#include <linux/mfd/palmas.h>
+
+struct regs_info {
+	char	*name;
+	u8	vsel_addr;
+	u8	ctrl_addr;
+	u8	tstep_addr;
+};
+
+static const struct regs_info palmas_regs_info[] = {
+	{
+		.name		= "SMPS12",
+		.vsel_addr	= PALMAS_SMPS12_VOLTAGE,
+		.ctrl_addr	= PALMAS_SMPS12_CTRL,
+		.tstep_addr	= PALMAS_SMPS12_TSTEP,
+	},
+	{
+		.name		= "SMPS123",
+		.vsel_addr	= PALMAS_SMPS12_VOLTAGE,
+		.ctrl_addr	= PALMAS_SMPS12_CTRL,
+		.tstep_addr	= PALMAS_SMPS12_TSTEP,
+	},
+	{
+		.name		= "SMPS3",
+		.vsel_addr	= PALMAS_SMPS3_VOLTAGE,
+		.ctrl_addr	= PALMAS_SMPS3_CTRL,
+	},
+	{
+		.name		= "SMPS45",
+		.vsel_addr	= PALMAS_SMPS45_VOLTAGE,
+		.ctrl_addr	= PALMAS_SMPS45_CTRL,
+		.tstep_addr	= PALMAS_SMPS45_TSTEP,
+	},
+	{
+		.name		= "SMPS457",
+		.vsel_addr	= PALMAS_SMPS45_VOLTAGE,
+		.ctrl_addr	= PALMAS_SMPS45_CTRL,
+		.tstep_addr	= PALMAS_SMPS45_TSTEP,
+	},
+	{
+		.name		= "SMPS6",
+		.vsel_addr	= PALMAS_SMPS6_VOLTAGE,
+		.ctrl_addr	= PALMAS_SMPS6_CTRL,
+		.tstep_addr	= PALMAS_SMPS6_TSTEP,
+	},
+	{
+		.name		= "SMPS7",
+		.vsel_addr	= PALMAS_SMPS7_VOLTAGE,
+		.ctrl_addr	= PALMAS_SMPS7_CTRL,
+	},
+	{
+		.name		= "SMPS8",
+		.vsel_addr	= PALMAS_SMPS8_VOLTAGE,
+		.ctrl_addr	= PALMAS_SMPS8_CTRL,
+		.tstep_addr	= PALMAS_SMPS8_TSTEP,
+	},
+	{
+		.name		= "SMPS9",
+		.vsel_addr	= PALMAS_SMPS9_VOLTAGE,
+		.ctrl_addr	= PALMAS_SMPS9_CTRL,
+	},
+	{
+		.name		= "SMPS10",
+	},
+	{
+		.name		= "LDO1",
+		.vsel_addr	= PALMAS_LDO1_VOLTAGE,
+		.ctrl_addr	= PALMAS_LDO1_CTRL,
+	},
+	{
+		.name		= "LDO2",
+		.vsel_addr	= PALMAS_LDO2_VOLTAGE,
+		.ctrl_addr	= PALMAS_LDO2_CTRL,
+	},
+	{
+		.name		= "LDO3",
+		.vsel_addr	= PALMAS_LDO3_VOLTAGE,
+		.ctrl_addr	= PALMAS_LDO3_CTRL,
+	},
+	{
+		.name		= "LDO4",
+		.vsel_addr	= PALMAS_LDO4_VOLTAGE,
+		.ctrl_addr	= PALMAS_LDO4_CTRL,
+	},
+	{
+		.name		= "LDO5",
+		.vsel_addr	= PALMAS_LDO5_VOLTAGE,
+		.ctrl_addr	= PALMAS_LDO5_CTRL,
+	},
+	{
+		.name		= "LDO6",
+		.vsel_addr	= PALMAS_LDO6_VOLTAGE,
+		.ctrl_addr	= PALMAS_LDO6_CTRL,
+	},
+	{
+		.name		= "LDO7",
+		.vsel_addr	= PALMAS_LDO7_VOLTAGE,
+		.ctrl_addr	= PALMAS_LDO7_CTRL,
+	},
+	{
+		.name		= "LDO8",
+		.vsel_addr	= PALMAS_LDO8_VOLTAGE,
+		.ctrl_addr	= PALMAS_LDO8_CTRL,
+	},
+	{
+		.name		= "LDO9",
+		.vsel_addr	= PALMAS_LDO9_VOLTAGE,
+		.ctrl_addr	= PALMAS_LDO9_CTRL,
+	},
+	{
+		.name		= "LDOLN",
+		.vsel_addr	= PALMAS_LDOLN_VOLTAGE,
+		.ctrl_addr	= PALMAS_LDOLN_CTRL,
+	},
+	{
+		.name		= "LDOUSB",
+		.vsel_addr	= PALMAS_LDOUSB_VOLTAGE,
+		.ctrl_addr	= PALMAS_LDOUSB_CTRL,
+	},
+};
+
+#define SMPS_CTRL_MODE_OFF		0x00
+#define SMPS_CTRL_MODE_ON		0x01
+#define SMPS_CTRL_MODE_ECO		0x02
+#define SMPS_CTRL_MODE_PWM		0x03
+
+/* These values are derived from the data sheet. And are the number of steps
+ * where there is a voltage change, the ranges at beginning and end of register
+ * max/min values where there are no change are ommitted.
+ *
+ * So they are basically (maxV-minV)/stepV
+ */
+#define PALMAS_SMPS_NUM_VOLTAGES	116
+#define PALMAS_SMPS10_NUM_VOLTAGES	2
+#define PALMAS_LDO_NUM_VOLTAGES		50
+
+#define SMPS10_VSEL			(1<<3)
+#define SMPS10_BOOST_EN			(1<<2)
+#define SMPS10_BYPASS_EN		(1<<1)
+#define SMPS10_SWITCH_EN		(1<<0)
+
+#define REGULATOR_SLAVE			0
+
+static int palmas_smps_read(struct palmas *palmas, unsigned int reg,
+		unsigned int *dest)
+{
+	unsigned int addr;
+
+	addr = PALMAS_BASE_TO_REG(PALMAS_SMPS_BASE, reg);
+
+	return regmap_read(palmas->regmap[REGULATOR_SLAVE], addr, dest);
+}
+
+static int palmas_smps_write(struct palmas *palmas, unsigned int reg,
+		unsigned int value)
+{
+	unsigned int addr;
+
+	addr = PALMAS_BASE_TO_REG(PALMAS_SMPS_BASE, reg);
+
+	return regmap_write(palmas->regmap[REGULATOR_SLAVE], addr, value);
+}
+
+static int palmas_ldo_read(struct palmas *palmas, unsigned int reg,
+		unsigned int *dest)
+{
+	unsigned int addr;
+
+	addr = PALMAS_BASE_TO_REG(PALMAS_LDO_BASE, reg);
+
+	return regmap_read(palmas->regmap[REGULATOR_SLAVE], addr, dest);
+}
+
+static int palmas_ldo_write(struct palmas *palmas, unsigned int reg,
+		unsigned int value)
+{
+	unsigned int addr;
+
+	addr = PALMAS_BASE_TO_REG(PALMAS_LDO_BASE, reg);
+
+	return regmap_write(palmas->regmap[REGULATOR_SLAVE], addr, value);
+}
+
+static int palmas_is_enabled_smps(struct regulator_dev *dev)
+{
+	struct palmas_pmic *pmic = rdev_get_drvdata(dev);
+	int id = rdev_get_id(dev);
+	unsigned int reg;
+
+	palmas_smps_read(pmic->palmas, palmas_regs_info[id].ctrl_addr, &reg);
+
+	reg &= PALMAS_SMPS12_CTRL_STATUS_MASK;
+	reg >>= PALMAS_SMPS12_CTRL_STATUS_SHIFT;
+
+	return !!(reg);
+}
+
+static int palmas_enable_smps(struct regulator_dev *dev)
+{
+	struct palmas_pmic *pmic = rdev_get_drvdata(dev);
+	int id = rdev_get_id(dev);
+	unsigned int reg;
+
+	palmas_smps_read(pmic->palmas, palmas_regs_info[id].ctrl_addr, &reg);
+
+	reg &= ~PALMAS_SMPS12_CTRL_MODE_ACTIVE_MASK;
+	reg |= SMPS_CTRL_MODE_ON;
+
+	palmas_smps_write(pmic->palmas, palmas_regs_info[id].ctrl_addr, reg);
+
+	return 0;
+}
+
+static int palmas_disable_smps(struct regulator_dev *dev)
+{
+	struct palmas_pmic *pmic = rdev_get_drvdata(dev);
+	int id = rdev_get_id(dev);
+	unsigned int reg;
+
+	palmas_smps_read(pmic->palmas, palmas_regs_info[id].ctrl_addr, &reg);
+
+	reg &= ~PALMAS_SMPS12_CTRL_MODE_ACTIVE_MASK;
+
+	palmas_smps_write(pmic->palmas, palmas_regs_info[id].ctrl_addr, reg);
+
+	return 0;
+}
+
+
+static int palmas_set_mode_smps(struct regulator_dev *dev, unsigned int mode)
+{
+	struct palmas_pmic *pmic = rdev_get_drvdata(dev);
+	int id = rdev_get_id(dev);
+	unsigned int reg;
+
+	palmas_smps_read(pmic->palmas, palmas_regs_info[id].ctrl_addr, &reg);
+	reg &= ~PALMAS_SMPS12_CTRL_STATUS_MASK;
+	reg >>= PALMAS_SMPS12_CTRL_STATUS_SHIFT;
+
+	switch (mode) {
+	case REGULATOR_MODE_NORMAL:
+		reg |= SMPS_CTRL_MODE_ON;
+		break;
+	case REGULATOR_MODE_IDLE:
+		reg |= SMPS_CTRL_MODE_ECO;
+		break;
+	case REGULATOR_MODE_FAST:
+		reg |= SMPS_CTRL_MODE_PWM;
+		break;
+	default:
+		return -EINVAL;
+	}
+	palmas_smps_write(pmic->palmas, palmas_regs_info[id].ctrl_addr, reg);
+
+	return 0;
+}
+
+static unsigned int palmas_get_mode_smps(struct regulator_dev *dev)
+{
+	struct palmas_pmic *pmic = rdev_get_drvdata(dev);
+	int id = rdev_get_id(dev);
+	unsigned int reg;
+
+	palmas_smps_read(pmic->palmas, palmas_regs_info[id].ctrl_addr, &reg);
+	reg &= PALMAS_SMPS12_CTRL_STATUS_MASK;
+	reg >>= PALMAS_SMPS12_CTRL_STATUS_SHIFT;
+
+	switch (reg) {
+	case SMPS_CTRL_MODE_ON:
+		return REGULATOR_MODE_NORMAL;
+	case SMPS_CTRL_MODE_ECO:
+		return REGULATOR_MODE_IDLE;
+	case SMPS_CTRL_MODE_PWM:
+		return REGULATOR_MODE_FAST;
+	}
+
+	return 0;
+}
+
+static int palmas_list_voltage_smps(struct regulator_dev *dev,
+					unsigned selector)
+{
+	struct palmas_pmic *pmic = rdev_get_drvdata(dev);
+	int id = rdev_get_id(dev);
+	int mult = 1;
+
+	if (!selector)
+		return 0;
+
+	/* Read the multiplier set in VSEL register to return
+	 * the correct voltage.
+	 */
+	if (pmic->range[id])
+		mult = 2;
+
+	/* Voltage is (0.49V + (selector * 0.01V)) * RANGE
+	 * as defined in data sheet. RANGE is either x1 or x2
+	 */
+	return  (490000 + (selector * 10000)) * mult;
+}
+
+static int palmas_get_voltage_smps_sel(struct regulator_dev *dev)
+{
+	struct palmas_pmic *pmic = rdev_get_drvdata(dev);
+	int id = rdev_get_id(dev);
+	int selector;
+	unsigned int reg;
+	unsigned int addr;
+
+	addr = palmas_regs_info[id].vsel_addr;
+
+	palmas_smps_read(pmic->palmas, addr, &reg);
+
+	selector = reg & PALMAS_SMPS12_VOLTAGE_VSEL_MASK;
+
+	/* Adjust selector to match list_voltage ranges */
+	if ((selector > 0) && (selector < 6))
+		selector = 6;
+	if (!selector)
+		selector = 5;
+	if (selector > 121)
+		selector = 121;
+	selector -= 5;
+
+	return selector;
+}
+
+static int palmas_set_voltage_smps_sel(struct regulator_dev *dev,
+		unsigned selector)
+{
+	struct palmas_pmic *pmic = rdev_get_drvdata(dev);
+	int id = rdev_get_id(dev);
+	unsigned int reg = 0;
+	unsigned int addr;
+
+	addr = palmas_regs_info[id].vsel_addr;
+
+	/* Make sure we don't change the value of RANGE */
+	if (pmic->range[id])
+		reg |= PALMAS_SMPS12_VOLTAGE_RANGE;
+
+	/* Adjust the linux selector into range used in VSEL register */
+	if (selector)
+		reg |= selector + 5;
+
+	palmas_smps_write(pmic->palmas, addr, reg);
+
+	return 0;
+}
+
+static int palmas_map_voltage_smps(struct regulator_dev *rdev,
+		int min_uV, int max_uV)
+{
+	int ret, voltage;
+
+	ret = ((min_uV - 500000) / 10000) + 1;
+	if (ret < 0)
+		return ret;
+
+	/* Map back into a voltage to verify we're still in bounds */
+	voltage = palmas_list_voltage_smps(rdev, ret);
+	if (voltage < min_uV || voltage > max_uV)
+		return -EINVAL;
+
+	return ret;
+}
+
+static struct regulator_ops palmas_ops_smps = {
+	.is_enabled		= palmas_is_enabled_smps,
+	.enable			= palmas_enable_smps,
+	.disable		= palmas_disable_smps,
+	.set_mode		= palmas_set_mode_smps,
+	.get_mode		= palmas_get_mode_smps,
+	.get_voltage_sel	= palmas_get_voltage_smps_sel,
+	.set_voltage_sel	= palmas_set_voltage_smps_sel,
+	.list_voltage		= palmas_list_voltage_smps,
+	.map_voltage		= palmas_map_voltage_smps,
+};
+
+static int palmas_list_voltage_smps10(struct regulator_dev *dev,
+					unsigned selector)
+{
+	return 3750000 + (selector * 1250000);
+}
+
+static struct regulator_ops palmas_ops_smps10 = {
+	.is_enabled		= regulator_is_enabled_regmap,
+	.enable			= regulator_enable_regmap,
+	.disable		= regulator_disable_regmap,
+	.get_voltage_sel	= regulator_get_voltage_sel_regmap,
+	.set_voltage_sel	= regulator_set_voltage_sel_regmap,
+	.list_voltage		= palmas_list_voltage_smps10,
+};
+
+static int palmas_is_enabled_ldo(struct regulator_dev *dev)
+{
+	struct palmas_pmic *pmic = rdev_get_drvdata(dev);
+	int id = rdev_get_id(dev);
+	unsigned int reg;
+
+	palmas_ldo_read(pmic->palmas, palmas_regs_info[id].ctrl_addr, &reg);
+
+	reg &= PALMAS_LDO1_CTRL_STATUS;
+
+	return !!(reg);
+}
+
+static int palmas_list_voltage_ldo(struct regulator_dev *dev,
+					unsigned selector)
+{
+	if (!selector)
+		return 0;
+
+	/* voltage is 0.85V + (selector * 0.05v) */
+	return  850000 + (selector * 50000);
+}
+
+static int palmas_get_voltage_ldo_sel(struct regulator_dev *dev)
+{
+	struct palmas_pmic *pmic = rdev_get_drvdata(dev);
+	int id = rdev_get_id(dev);
+	int selector;
+	unsigned int reg;
+	unsigned int addr;
+
+	addr = palmas_regs_info[id].vsel_addr;
+
+	palmas_ldo_read(pmic->palmas, addr, &reg);
+
+	selector = reg & PALMAS_LDO1_VOLTAGE_VSEL_MASK;
+
+	/* Adjust selector to match list_voltage ranges */
+	if (selector > 49)
+		selector = 49;
+
+	return selector;
+}
+
+static int palmas_set_voltage_ldo_sel(struct regulator_dev *dev,
+		unsigned selector)
+{
+	struct palmas_pmic *pmic = rdev_get_drvdata(dev);
+	int id = rdev_get_id(dev);
+	unsigned int reg = 0;
+	unsigned int addr;
+
+	addr = palmas_regs_info[id].vsel_addr;
+
+	reg = selector;
+
+	palmas_ldo_write(pmic->palmas, addr, reg);
+
+	return 0;
+}
+
+static int palmas_map_voltage_ldo(struct regulator_dev *rdev,
+		int min_uV, int max_uV)
+{
+	int ret, voltage;
+
+	ret = ((min_uV - 900000) / 50000) + 1;
+	if (ret < 0)
+		return ret;
+
+	/* Map back into a voltage to verify we're still in bounds */
+	voltage = palmas_list_voltage_ldo(rdev, ret);
+	if (voltage < min_uV || voltage > max_uV)
+		return -EINVAL;
+
+	return ret;
+}
+
+static struct regulator_ops palmas_ops_ldo = {
+	.is_enabled		= palmas_is_enabled_ldo,
+	.enable			= regulator_enable_regmap,
+	.disable		= regulator_disable_regmap,
+	.get_voltage_sel	= palmas_get_voltage_ldo_sel,
+	.set_voltage_sel	= palmas_set_voltage_ldo_sel,
+	.list_voltage		= palmas_list_voltage_ldo,
+	.map_voltage		= palmas_map_voltage_ldo,
+};
+
+/*
+ * setup the hardware based sleep configuration of the SMPS/LDO regulators
+ * from the platform data. This is different to the software based control
+ * supported by the regulator framework as it is controlled by toggling
+ * pins on the PMIC such as PREQ, SYSEN, ...
+ */
+static int palmas_smps_init(struct palmas *palmas, int id,
+		struct palmas_reg_init *reg_init)
+{
+	unsigned int reg;
+	unsigned int addr;
+	int ret;
+
+	addr = palmas_regs_info[id].ctrl_addr;
+
+	ret = palmas_smps_read(palmas, addr, &reg);
+	if (ret)
+		return ret;
+
+	if (id != PALMAS_REG_SMPS10) {
+		if (reg_init->warm_reset)
+			reg |= PALMAS_SMPS12_CTRL_WR_S;
+
+		if (reg_init->roof_floor)
+			reg |= PALMAS_SMPS12_CTRL_ROOF_FLOOR_EN;
+
+		if (reg_init->mode_sleep) {
+			reg &= ~PALMAS_SMPS12_CTRL_MODE_SLEEP_MASK;
+			reg |= reg_init->mode_sleep <<
+					PALMAS_SMPS12_CTRL_MODE_SLEEP_SHIFT;
+		}
+	} else {
+		if (reg_init->mode_sleep) {
+			reg &= ~PALMAS_SMPS10_CTRL_MODE_SLEEP_MASK;
+			reg |= reg_init->mode_sleep <<
+					PALMAS_SMPS10_CTRL_MODE_SLEEP_SHIFT;
+		}
+
+	}
+	ret = palmas_smps_write(palmas, addr, reg);
+	if (ret)
+		return ret;
+
+	if (palmas_regs_info[id].tstep_addr && reg_init->tstep) {
+		addr = palmas_regs_info[id].tstep_addr;
+
+		reg = reg_init->tstep & PALMAS_SMPS12_TSTEP_TSTEP_MASK;
+
+		ret = palmas_smps_write(palmas, addr, reg);
+		if (ret)
+			return ret;
+	}
+
+	if (palmas_regs_info[id].vsel_addr && reg_init->vsel) {
+		addr = palmas_regs_info[id].vsel_addr;
+
+		reg = reg_init->vsel;
+
+		ret = palmas_smps_write(palmas, addr, reg);
+		if (ret)
+			return ret;
+	}
+
+
+	return 0;
+}
+
+static int palmas_ldo_init(struct palmas *palmas, int id,
+		struct palmas_reg_init *reg_init)
+{
+	unsigned int reg;
+	unsigned int addr;
+	int ret;
+
+	addr = palmas_regs_info[id].ctrl_addr;
+
+	ret = palmas_smps_read(palmas, addr, &reg);
+	if (ret)
+		return ret;
+
+	if (reg_init->warm_reset)
+		reg |= PALMAS_LDO1_CTRL_WR_S;
+
+	if (reg_init->mode_sleep)
+		reg |= PALMAS_LDO1_CTRL_MODE_SLEEP;
+
+	ret = palmas_smps_write(palmas, addr, reg);
+	if (ret)
+		return ret;
+
+	return 0;
+}
+
+static __devinit int palmas_probe(struct platform_device *pdev)
+{
+	struct palmas *palmas = dev_get_drvdata(pdev->dev.parent);
+	struct palmas_pmic_platform_data *pdata = pdev->dev.platform_data;
+	struct regulator_dev *rdev;
+	struct regulator_config config = { };
+	struct palmas_pmic *pmic;
+	struct palmas_reg_init *reg_init;
+	int id = 0, ret;
+	unsigned int addr, reg;
+
+	if (!pdata)
+		return -EINVAL;
+	if (!pdata->reg_data)
+		return -EINVAL;
+
+	pmic = devm_kzalloc(&pdev->dev, sizeof(*pmic), GFP_KERNEL);
+	if (!pmic)
+		return -ENOMEM;
+
+	pmic->dev = &pdev->dev;
+	pmic->palmas = palmas;
+	palmas->pmic = pmic;
+	platform_set_drvdata(pdev, pmic);
+
+	ret = palmas_smps_read(palmas, PALMAS_SMPS_CTRL, &reg);
+	if (ret)
+		goto err_unregister_regulator;
+
+	if (reg & PALMAS_SMPS_CTRL_SMPS12_SMPS123_EN)
+		pmic->smps123 = 1;
+
+	if (reg & PALMAS_SMPS_CTRL_SMPS45_SMPS457_EN)
+		pmic->smps457 = 1;
+
+	config.regmap = palmas->regmap[REGULATOR_SLAVE];
+	config.dev = &pdev->dev;
+	config.driver_data = pmic;
+
+	for (id = 0; id < PALMAS_REG_LDO1; id++) {
+
+		/*
+		 * Miss out regulators which are not available due
+		 * to slaving configurations.
+		 */
+		switch (id) {
+		case PALMAS_REG_SMPS12:
+		case PALMAS_REG_SMPS3:
+			if (pmic->smps123)
+				continue;
+			break;
+		case PALMAS_REG_SMPS123:
+			if (!pmic->smps123)
+				continue;
+			break;
+		case PALMAS_REG_SMPS45:
+		case PALMAS_REG_SMPS7:
+			if (pmic->smps457)
+				continue;
+			break;
+		case PALMAS_REG_SMPS457:
+			if (!pmic->smps457)
+				continue;
+		}
+
+		/* Register the regulators */
+		pmic->desc[id].name = palmas_regs_info[id].name;
+		pmic->desc[id].id = id;
+
+		if (id != PALMAS_REG_SMPS10) {
+			pmic->desc[id].ops = &palmas_ops_smps;
+			pmic->desc[id].n_voltages = PALMAS_SMPS_NUM_VOLTAGES;
+		} else {
+			pmic->desc[id].n_voltages = PALMAS_SMPS10_NUM_VOLTAGES;
+			pmic->desc[id].ops = &palmas_ops_smps10;
+			pmic->desc[id].vsel_reg = PALMAS_SMPS10_CTRL;
+			pmic->desc[id].vsel_mask = SMPS10_VSEL;
+			pmic->desc[id].enable_reg = PALMAS_SMPS10_STATUS;
+			pmic->desc[id].enable_mask = SMPS10_BOOST_EN;
+		}
+
+		pmic->desc[id].type = REGULATOR_VOLTAGE;
+		pmic->desc[id].owner = THIS_MODULE;
+
+		/* Initialise sleep/init values from platform data */
+		if (pdata && pdata->reg_init) {
+			reg_init = pdata->reg_init[id];
+			if (reg_init) {
+				ret = palmas_smps_init(palmas, id, reg_init);
+				if (ret)
+					goto err_unregister_regulator;
+			}
+		}
+
+		/*
+		 * read and store the RANGE bit for later use
+		 * This must be done before regulator is probed otherwise
+		 * we error in probe with unsuportable ranges.
+		 */
+		if (id != PALMAS_REG_SMPS10) {
+			addr = palmas_regs_info[id].vsel_addr;
+
+			ret = palmas_smps_read(pmic->palmas, addr, &reg);
+			if (ret)
+				goto err_unregister_regulator;
+			if (reg & PALMAS_SMPS12_VOLTAGE_RANGE)
+				pmic->range[id] = 1;
+		}
+
+		if (pdata && pdata->reg_data)
+			config.init_data = pdata->reg_data[id];
+		else
+			config.init_data = NULL;
+
+		rdev = regulator_register(&pmic->desc[id], &config);
+		if (IS_ERR(rdev)) {
+			dev_err(&pdev->dev,
+				"failed to register %s regulator\n",
+				pdev->name);
+			ret = PTR_ERR(rdev);
+			goto err_unregister_regulator;
+		}
+
+		/* Save regulator for cleanup */
+		pmic->rdev[id] = rdev;
+	}
+
+	/* Start this loop from the id left from previous loop */
+	for (; id < PALMAS_NUM_REGS; id++) {
+
+		/* Miss out regulators which are not available due
+		 * to alternate functions.
+		 */
+
+		/* Register the regulators */
+		pmic->desc[id].name = palmas_regs_info[id].name;
+		pmic->desc[id].id = id;
+		pmic->desc[id].n_voltages = PALMAS_LDO_NUM_VOLTAGES;
+
+		pmic->desc[id].ops = &palmas_ops_ldo;
+
+		pmic->desc[id].type = REGULATOR_VOLTAGE;
+		pmic->desc[id].owner = THIS_MODULE;
+		pmic->desc[id].enable_reg = palmas_regs_info[id].ctrl_addr;
+		pmic->desc[id].enable_mask = PALMAS_LDO1_CTRL_MODE_ACTIVE;
+
+		if (pdata && pdata->reg_data)
+			config.init_data = pdata->reg_data[id];
+		else
+			config.init_data = NULL;
+
+		rdev = regulator_register(&pmic->desc[id], &config);
+		if (IS_ERR(rdev)) {
+			dev_err(&pdev->dev,
+				"failed to register %s regulator\n",
+				pdev->name);
+			ret = PTR_ERR(rdev);
+			goto err_unregister_regulator;
+		}
+
+		/* Save regulator for cleanup */
+		pmic->rdev[id] = rdev;
+
+		/* Initialise sleep/init values from platform data */
+		if (pdata->reg_init) {
+			reg_init = pdata->reg_init[id];
+			if (reg_init) {
+				ret = palmas_ldo_init(palmas, id, reg_init);
+				if (ret)
+					goto err_unregister_regulator;
+			}
+		}
+	}
+
+	return 0;
+
+err_unregister_regulator:
+	while (--id >= 0)
+		regulator_unregister(pmic->rdev[id]);
+	kfree(pmic->rdev);
+	kfree(pmic->desc);
+	kfree(pmic);
+	return ret;
+}
+
+static int __devexit palmas_remove(struct platform_device *pdev)
+{
+	struct palmas_pmic *pmic = platform_get_drvdata(pdev);
+	int id;
+
+	for (id = 0; id < PALMAS_NUM_REGS; id++)
+		regulator_unregister(pmic->rdev[id]);
+
+	kfree(pmic->rdev);
+	kfree(pmic->desc);
+	kfree(pmic);
+	return 0;
+}
+
+static struct platform_driver palmas_driver = {
+	.driver = {
+		.name = "palmas-pmic",
+		.owner = THIS_MODULE,
+	},
+	.probe = palmas_probe,
+	.remove = __devexit_p(palmas_remove),
+};
+
+static int __init palmas_init(void)
+{
+	return platform_driver_register(&palmas_driver);
+}
+subsys_initcall(palmas_init);
+
+static void __exit palmas_exit(void)
+{
+	platform_driver_unregister(&palmas_driver);
+}
+module_exit(palmas_exit);
+
+MODULE_AUTHOR("Graeme Gregory <gg@slimlogic.co.uk>");
+MODULE_DESCRIPTION("Palmas voltage regulator driver");
+MODULE_LICENSE("GPL");
+MODULE_ALIAS("platform:palmas-pmic");
diff --git a/drivers/regulator/pcap-regulator.c b/drivers/regulator/pcap-regulator.c
index a5aab1b08bc..8211101121f 100644
--- a/drivers/regulator/pcap-regulator.c
+++ b/drivers/regulator/pcap-regulator.c
@@ -150,57 +150,33 @@ static struct pcap_regulator vreg_table[] = {
 	VREG_INFO(SW2S,  PCAP_REG_LOWPWR,  NA, 20, NA, NA), */
 };
 
-static int pcap_regulator_set_voltage(struct regulator_dev *rdev,
-				      int min_uV, int max_uV,
-				      unsigned *selector)
+static int pcap_regulator_set_voltage_sel(struct regulator_dev *rdev,
+					  unsigned selector)
 {
 	struct pcap_regulator *vreg = &vreg_table[rdev_get_id(rdev)];
 	void *pcap = rdev_get_drvdata(rdev);
-	int uV;
-	u8 i;
 
 	/* the regulator doesn't support voltage switching */
 	if (vreg->n_voltages == 1)
 		return -EINVAL;
 
-	for (i = 0; i < vreg->n_voltages; i++) {
-		/* For V1 the first is not the best match */
-		if (i == 0 && rdev_get_id(rdev) == V1)
-			i = 1;
-		else if (i + 1 == vreg->n_voltages && rdev_get_id(rdev) == V1)
-			i = 0;
-
-		uV = vreg->voltage_table[i] * 1000;
-		if (min_uV <= uV && uV <= max_uV) {
-			*selector = i;
-			return ezx_pcap_set_bits(pcap, vreg->reg,
-					(vreg->n_voltages - 1) << vreg->index,
-					i << vreg->index);
-		}
-
-		if (i == 0 && rdev_get_id(rdev) == V1)
-			i = vreg->n_voltages - 1;
-	}
-
-	/* the requested voltage range is not supported by this regulator */
-	return -EINVAL;
+	return ezx_pcap_set_bits(pcap, vreg->reg,
+				 (vreg->n_voltages - 1) << vreg->index,
+				 selector << vreg->index);
 }
 
-static int pcap_regulator_get_voltage(struct regulator_dev *rdev)
+static int pcap_regulator_get_voltage_sel(struct regulator_dev *rdev)
 {
 	struct pcap_regulator *vreg = &vreg_table[rdev_get_id(rdev)];
 	void *pcap = rdev_get_drvdata(rdev);
 	u32 tmp;
-	int mV;
 
 	if (vreg->n_voltages == 1)
-		return vreg->voltage_table[0] * 1000;
+		return 0;
 
 	ezx_pcap_read(pcap, vreg->reg, &tmp);
 	tmp = ((tmp >> vreg->index) & (vreg->n_voltages - 1));
-	mV = vreg->voltage_table[tmp];
-
-	return mV * 1000;
+	return tmp;
 }
 
 static int pcap_regulator_enable(struct regulator_dev *rdev)
@@ -248,8 +224,8 @@ static int pcap_regulator_list_voltage(struct regulator_dev *rdev,
 
 static struct regulator_ops pcap_regulator_ops = {
 	.list_voltage	= pcap_regulator_list_voltage,
-	.set_voltage	= pcap_regulator_set_voltage,
-	.get_voltage	= pcap_regulator_get_voltage,
+	.set_voltage_sel = pcap_regulator_set_voltage_sel,
+	.get_voltage_sel = pcap_regulator_get_voltage_sel,
 	.enable		= pcap_regulator_enable,
 	.disable	= pcap_regulator_disable,
 	.is_enabled	= pcap_regulator_is_enabled,
@@ -265,7 +241,7 @@ static struct regulator_ops pcap_regulator_ops = {
 		.owner		= THIS_MODULE,			\
 	}
 
-static struct regulator_desc pcap_regulators[] = {
+static const struct regulator_desc pcap_regulators[] = {
 	VREG(V1), VREG(V2), VREG(V3), VREG(V4), VREG(V5), VREG(V6), VREG(V7),
 	VREG(V8), VREG(V9), VREG(V10), VREG(VAUX1), VREG(VAUX2), VREG(VAUX3),
 	VREG(VAUX4), VREG(VSIM), VREG(VSIM2), VREG(VVIB), VREG(SW1), VREG(SW2),
@@ -275,9 +251,13 @@ static int __devinit pcap_regulator_probe(struct platform_device *pdev)
 {
 	struct regulator_dev *rdev;
 	void *pcap = dev_get_drvdata(pdev->dev.parent);
+	struct regulator_config config = { };
+
+	config.dev = &pdev->dev;
+	config.init_data = pdev->dev.platform_data;
+	config.driver_data = pcap;
 
-	rdev = regulator_register(&pcap_regulators[pdev->id], &pdev->dev,
-				pdev->dev.platform_data, pcap, NULL);
+	rdev = regulator_register(&pcap_regulators[pdev->id], &config);
 	if (IS_ERR(rdev))
 		return PTR_ERR(rdev);
 
diff --git a/drivers/regulator/pcf50633-regulator.c b/drivers/regulator/pcf50633-regulator.c
index 6db46c632f1..3c9d14c0017 100644
--- a/drivers/regulator/pcf50633-regulator.c
+++ b/drivers/regulator/pcf50633-regulator.c
@@ -24,35 +24,25 @@
 #include <linux/mfd/pcf50633/core.h>
 #include <linux/mfd/pcf50633/pmic.h>
 
-#define PCF50633_REGULATOR(_name, _id, _n) 		\
-	{					\
-		.name = _name, 			\
-		.id = _id,			\
-		.ops = &pcf50633_regulator_ops,	\
-		.n_voltages = _n, \
-		.type = REGULATOR_VOLTAGE, 	\
-		.owner = THIS_MODULE, 		\
+#define PCF50633_REGULATOR(_name, _id, _n)			\
+	{							\
+		.name = _name,					\
+		.id = PCF50633_REGULATOR_##_id,			\
+		.ops = &pcf50633_regulator_ops,			\
+		.n_voltages = _n,				\
+		.type = REGULATOR_VOLTAGE,			\
+		.owner = THIS_MODULE,				\
+		.vsel_reg = PCF50633_REG_##_id##OUT,		\
+		.vsel_mask = 0xff,				\
+		.enable_reg = PCF50633_REG_##_id##OUT + 1,	\
+		.enable_mask = PCF50633_REGULATOR_ON,		\
 	}
 
-static const u8 pcf50633_regulator_registers[PCF50633_NUM_REGULATORS] = {
-	[PCF50633_REGULATOR_AUTO]	= PCF50633_REG_AUTOOUT,
-	[PCF50633_REGULATOR_DOWN1]	= PCF50633_REG_DOWN1OUT,
-	[PCF50633_REGULATOR_DOWN2]	= PCF50633_REG_DOWN2OUT,
-	[PCF50633_REGULATOR_MEMLDO]	= PCF50633_REG_MEMLDOOUT,
-	[PCF50633_REGULATOR_LDO1]	= PCF50633_REG_LDO1OUT,
-	[PCF50633_REGULATOR_LDO2]	= PCF50633_REG_LDO2OUT,
-	[PCF50633_REGULATOR_LDO3]	= PCF50633_REG_LDO3OUT,
-	[PCF50633_REGULATOR_LDO4]	= PCF50633_REG_LDO4OUT,
-	[PCF50633_REGULATOR_LDO5]	= PCF50633_REG_LDO5OUT,
-	[PCF50633_REGULATOR_LDO6]	= PCF50633_REG_LDO6OUT,
-	[PCF50633_REGULATOR_HCLDO]	= PCF50633_REG_HCLDOOUT,
-};
-
 /* Bits from voltage value */
 static u8 auto_voltage_bits(unsigned int millivolts)
 {
 	if (millivolts < 1800)
-		return 0;
+		return 0x2f;
 	if (millivolts > 3800)
 		return 0xff;
 
@@ -87,6 +77,9 @@ static u8 ldo_voltage_bits(unsigned int millivolts)
 /* Obtain voltage value from bits */
 static unsigned int auto_voltage_value(u8 bits)
 {
+	/* AUTOOUT: 00000000 to 00101110 are reserved.
+	 * Return 0 for bits in reserved range, which means this selector code
+	 * can't be used on this system */
 	if (bits < 0x2f)
 		return 0;
 
@@ -123,7 +116,7 @@ static int pcf50633_regulator_set_voltage(struct regulator_dev *rdev,
 
 	millivolts = min_uV / 1000;
 
-	regnr = pcf50633_regulator_registers[regulator_id];
+	regnr = rdev->desc->vsel_reg;
 
 	switch (regulator_id) {
 	case PCF50633_REGULATOR_AUTO:
@@ -154,20 +147,22 @@ static int pcf50633_regulator_set_voltage(struct regulator_dev *rdev,
 	return pcf50633_reg_write(pcf, regnr, volt_bits);
 }
 
-static int pcf50633_regulator_voltage_value(enum pcf50633_regulator_id id,
-						u8 bits)
+static int pcf50633_regulator_list_voltage(struct regulator_dev *rdev,
+						unsigned int index)
 {
+	int regulator_id = rdev_get_id(rdev);
+
 	int millivolts;
 
-	switch (id) {
+	switch (regulator_id) {
 	case PCF50633_REGULATOR_AUTO:
-		millivolts = auto_voltage_value(bits);
+		millivolts = auto_voltage_value(index);
 		break;
 	case PCF50633_REGULATOR_DOWN1:
-		millivolts = down_voltage_value(bits);
+		millivolts = down_voltage_value(index);
 		break;
 	case PCF50633_REGULATOR_DOWN2:
-		millivolts = down_voltage_value(bits);
+		millivolts = down_voltage_value(index);
 		break;
 	case PCF50633_REGULATOR_LDO1:
 	case PCF50633_REGULATOR_LDO2:
@@ -177,7 +172,7 @@ static int pcf50633_regulator_voltage_value(enum pcf50633_regulator_id id,
 	case PCF50633_REGULATOR_LDO6:
 	case PCF50633_REGULATOR_HCLDO:
 	case PCF50633_REGULATOR_MEMLDO:
-		millivolts = ldo_voltage_value(bits);
+		millivolts = ldo_voltage_value(index);
 		break;
 	default:
 		return -EINVAL;
@@ -186,140 +181,44 @@ static int pcf50633_regulator_voltage_value(enum pcf50633_regulator_id id,
 	return millivolts * 1000;
 }
 
-static int pcf50633_regulator_get_voltage(struct regulator_dev *rdev)
-{
-	struct pcf50633 *pcf;
-	int regulator_id;
-	u8 volt_bits, regnr;
-
-	pcf = rdev_get_drvdata(rdev);
-
-	regulator_id = rdev_get_id(rdev);
-	if (regulator_id >= PCF50633_NUM_REGULATORS)
-		return -EINVAL;
-
-	regnr = pcf50633_regulator_registers[regulator_id];
-
-	volt_bits = pcf50633_reg_read(pcf, regnr);
-
-	return pcf50633_regulator_voltage_value(regulator_id, volt_bits);
-}
-
-static int pcf50633_regulator_list_voltage(struct regulator_dev *rdev,
-						unsigned int index)
-{
-	struct pcf50633 *pcf;
-	int regulator_id;
-
-	pcf = rdev_get_drvdata(rdev);
-
-	regulator_id = rdev_get_id(rdev);
-
-	switch (regulator_id) {
-	case PCF50633_REGULATOR_AUTO:
-		index += 0x2f;
-		break;
-	default:
-		break;
-	}
-
-	return pcf50633_regulator_voltage_value(regulator_id, index);
-}
-
-static int pcf50633_regulator_enable(struct regulator_dev *rdev)
-{
-	struct pcf50633 *pcf = rdev_get_drvdata(rdev);
-	int regulator_id;
-	u8 regnr;
-
-	regulator_id = rdev_get_id(rdev);
-	if (regulator_id >= PCF50633_NUM_REGULATORS)
-		return -EINVAL;
-
-	/* The *ENA register is always one after the *OUT register */
-	regnr = pcf50633_regulator_registers[regulator_id] + 1;
-
-	return pcf50633_reg_set_bit_mask(pcf, regnr, PCF50633_REGULATOR_ON,
-						       PCF50633_REGULATOR_ON);
-}
-
-static int pcf50633_regulator_disable(struct regulator_dev *rdev)
-{
-	struct pcf50633 *pcf = rdev_get_drvdata(rdev);
-	int regulator_id;
-	u8 regnr;
-
-	regulator_id = rdev_get_id(rdev);
-	if (regulator_id >= PCF50633_NUM_REGULATORS)
-		return -EINVAL;
-
-	/* the *ENA register is always one after the *OUT register */
-	regnr = pcf50633_regulator_registers[regulator_id] + 1;
-
-	return pcf50633_reg_set_bit_mask(pcf, regnr,
-					PCF50633_REGULATOR_ON, 0);
-}
-
-static int pcf50633_regulator_is_enabled(struct regulator_dev *rdev)
-{
-	struct pcf50633 *pcf = rdev_get_drvdata(rdev);
-	int regulator_id = rdev_get_id(rdev);
-	u8 regnr;
-
-	regulator_id = rdev_get_id(rdev);
-	if (regulator_id >= PCF50633_NUM_REGULATORS)
-		return -EINVAL;
-
-	/* the *ENA register is always one after the *OUT register */
-	regnr = pcf50633_regulator_registers[regulator_id] + 1;
-
-	return pcf50633_reg_read(pcf, regnr) & PCF50633_REGULATOR_ON;
-}
-
 static struct regulator_ops pcf50633_regulator_ops = {
 	.set_voltage = pcf50633_regulator_set_voltage,
-	.get_voltage = pcf50633_regulator_get_voltage,
+	.get_voltage_sel = regulator_get_voltage_sel_regmap,
 	.list_voltage = pcf50633_regulator_list_voltage,
-	.enable = pcf50633_regulator_enable,
-	.disable = pcf50633_regulator_disable,
-	.is_enabled = pcf50633_regulator_is_enabled,
+	.enable = regulator_enable_regmap,
+	.disable = regulator_disable_regmap,
+	.is_enabled = regulator_is_enabled_regmap,
 };
 
-static struct regulator_desc regulators[] = {
-	[PCF50633_REGULATOR_AUTO] =
-		PCF50633_REGULATOR("auto", PCF50633_REGULATOR_AUTO, 81),
-	[PCF50633_REGULATOR_DOWN1] =
-		PCF50633_REGULATOR("down1", PCF50633_REGULATOR_DOWN1, 96),
-	[PCF50633_REGULATOR_DOWN2] =
-		PCF50633_REGULATOR("down2", PCF50633_REGULATOR_DOWN2, 96),
-	[PCF50633_REGULATOR_LDO1] =
-		PCF50633_REGULATOR("ldo1", PCF50633_REGULATOR_LDO1, 28),
-	[PCF50633_REGULATOR_LDO2] =
-		PCF50633_REGULATOR("ldo2", PCF50633_REGULATOR_LDO2, 28),
-	[PCF50633_REGULATOR_LDO3] =
-		PCF50633_REGULATOR("ldo3", PCF50633_REGULATOR_LDO3, 28),
-	[PCF50633_REGULATOR_LDO4] =
-		PCF50633_REGULATOR("ldo4", PCF50633_REGULATOR_LDO4, 28),
-	[PCF50633_REGULATOR_LDO5] =
-		PCF50633_REGULATOR("ldo5", PCF50633_REGULATOR_LDO5, 28),
-	[PCF50633_REGULATOR_LDO6] =
-		PCF50633_REGULATOR("ldo6", PCF50633_REGULATOR_LDO6, 28),
-	[PCF50633_REGULATOR_HCLDO] =
-		PCF50633_REGULATOR("hcldo", PCF50633_REGULATOR_HCLDO, 28),
-	[PCF50633_REGULATOR_MEMLDO] =
-		PCF50633_REGULATOR("memldo", PCF50633_REGULATOR_MEMLDO, 28),
+static const struct regulator_desc regulators[] = {
+	[PCF50633_REGULATOR_AUTO] = PCF50633_REGULATOR("auto", AUTO, 128),
+	[PCF50633_REGULATOR_DOWN1] = PCF50633_REGULATOR("down1", DOWN1, 96),
+	[PCF50633_REGULATOR_DOWN2] = PCF50633_REGULATOR("down2", DOWN2, 96),
+	[PCF50633_REGULATOR_LDO1] = PCF50633_REGULATOR("ldo1", LDO1, 28),
+	[PCF50633_REGULATOR_LDO2] = PCF50633_REGULATOR("ldo2", LDO2, 28),
+	[PCF50633_REGULATOR_LDO3] = PCF50633_REGULATOR("ldo3", LDO3, 28),
+	[PCF50633_REGULATOR_LDO4] = PCF50633_REGULATOR("ldo4", LDO4, 28),
+	[PCF50633_REGULATOR_LDO5] = PCF50633_REGULATOR("ldo5", LDO5, 28),
+	[PCF50633_REGULATOR_LDO6] = PCF50633_REGULATOR("ldo6", LDO6, 28),
+	[PCF50633_REGULATOR_HCLDO] = PCF50633_REGULATOR("hcldo", HCLDO, 28),
+	[PCF50633_REGULATOR_MEMLDO] = PCF50633_REGULATOR("memldo", MEMLDO, 28),
 };
 
 static int __devinit pcf50633_regulator_probe(struct platform_device *pdev)
 {
 	struct regulator_dev *rdev;
 	struct pcf50633 *pcf;
+	struct regulator_config config = { };
 
 	/* Already set by core driver */
 	pcf = dev_to_pcf50633(pdev->dev.parent);
 
-	rdev = regulator_register(&regulators[pdev->id], &pdev->dev,
-				  pdev->dev.platform_data, pcf, NULL);
+	config.dev = &pdev->dev;
+	config.init_data = pdev->dev.platform_data;
+	config.driver_data = pcf;
+	config.regmap = pcf->regmap;
+
+	rdev = regulator_register(&regulators[pdev->id], &config);
 	if (IS_ERR(rdev))
 		return PTR_ERR(rdev);
 
diff --git a/drivers/regulator/rc5t583-regulator.c b/drivers/regulator/rc5t583-regulator.c
new file mode 100644
index 00000000000..1d34e64a130
--- /dev/null
+++ b/drivers/regulator/rc5t583-regulator.c
@@ -0,0 +1,255 @@
+/*
+ * Regulator driver for RICOH RC5T583 power management chip.
+ *
+ * Copyright (c) 2011-2012, NVIDIA CORPORATION.  All rights reserved.
+ * Author: Laxman dewangan <ldewangan@nvidia.com>
+ *
+ * based on code
+ *      Copyright (C) 2011 RICOH COMPANY,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, see <http://www.gnu.org/licenses/>.
+ *
+ */
+
+#include <linux/module.h>
+#include <linux/init.h>
+#include <linux/slab.h>
+#include <linux/err.h>
+#include <linux/platform_device.h>
+#include <linux/regulator/driver.h>
+#include <linux/regulator/machine.h>
+#include <linux/gpio.h>
+#include <linux/mfd/rc5t583.h>
+
+struct rc5t583_regulator_info {
+	int			deepsleep_id;
+
+	/* Regulator register address.*/
+	uint8_t			reg_disc_reg;
+	uint8_t			disc_bit;
+	uint8_t			deepsleep_reg;
+
+	/* Regulator specific turn-on delay  and voltage settling time*/
+	int			enable_uv_per_us;
+	int			change_uv_per_us;
+
+	/* Used by regulator core */
+	struct regulator_desc	desc;
+};
+
+struct rc5t583_regulator {
+	struct rc5t583_regulator_info *reg_info;
+
+	/* Devices */
+	struct device		*dev;
+	struct rc5t583		*mfd;
+	struct regulator_dev	*rdev;
+};
+
+static int rc5t583_regulator_enable_time(struct regulator_dev *rdev)
+{
+	struct rc5t583_regulator *reg = rdev_get_drvdata(rdev);
+	int vsel = regulator_get_voltage_sel_regmap(rdev);
+	int curr_uV = regulator_list_voltage_linear(rdev, vsel);
+
+	return DIV_ROUND_UP(curr_uV, reg->reg_info->enable_uv_per_us);
+}
+
+static int rc5t583_set_voltage_time_sel(struct regulator_dev *rdev,
+		unsigned int old_selector, unsigned int new_selector)
+{
+	struct rc5t583_regulator *reg = rdev_get_drvdata(rdev);
+	int old_uV, new_uV;
+	old_uV = regulator_list_voltage_linear(rdev, old_selector);
+
+	if (old_uV < 0)
+		return old_uV;
+
+	new_uV = regulator_list_voltage_linear(rdev, new_selector);
+	if (new_uV < 0)
+		return new_uV;
+
+	return DIV_ROUND_UP(abs(old_uV - new_uV),
+				reg->reg_info->change_uv_per_us);
+}
+
+
+static struct regulator_ops rc5t583_ops = {
+	.is_enabled		= regulator_is_enabled_regmap,
+	.enable			= regulator_enable_regmap,
+	.disable		= regulator_disable_regmap,
+	.enable_time		= rc5t583_regulator_enable_time,
+	.get_voltage_sel	= regulator_get_voltage_sel_regmap,
+	.set_voltage_sel	= regulator_set_voltage_sel_regmap,
+	.list_voltage		= regulator_list_voltage_linear,
+	.map_voltage		= regulator_map_voltage_linear,
+	.set_voltage_time_sel	= rc5t583_set_voltage_time_sel,
+};
+
+#define RC5T583_REG(_id, _en_reg, _en_bit, _disc_reg, _disc_bit, \
+		_vout_mask, _min_mv, _max_mv, _step_uV, _enable_mv) \
+{								\
+	.reg_disc_reg	= RC5T583_REG_##_disc_reg,		\
+	.disc_bit	= _disc_bit,				\
+	.deepsleep_reg	= RC5T583_REG_##_id##DAC_DS,		\
+	.enable_uv_per_us = _enable_mv * 1000,			\
+	.change_uv_per_us = 40 * 1000,				\
+	.deepsleep_id	= RC5T583_DS_##_id,			\
+	.desc = {						\
+		.name = "rc5t583-regulator-"#_id,		\
+		.id = RC5T583_REGULATOR_##_id,			\
+		.n_voltages = (_max_mv - _min_mv) * 1000 / _step_uV + 1, \
+		.ops = &rc5t583_ops,				\
+		.type = REGULATOR_VOLTAGE,			\
+		.owner = THIS_MODULE,				\
+		.vsel_reg = RC5T583_REG_##_id##DAC,		\
+		.vsel_mask = _vout_mask,			\
+		.enable_reg = RC5T583_REG_##_en_reg,		\
+		.enable_mask = BIT(_en_bit),			\
+		.min_uV	= _min_mv * 1000,			\
+		.uV_step = _step_uV,				\
+	},							\
+}
+
+static struct rc5t583_regulator_info rc5t583_reg_info[RC5T583_REGULATOR_MAX] = {
+	RC5T583_REG(DC0, DC0CTL, 0, DC0CTL, 1, 0x7F, 700, 1500, 12500, 4),
+	RC5T583_REG(DC1, DC1CTL, 0, DC1CTL, 1, 0x7F, 700, 1500, 12500, 14),
+	RC5T583_REG(DC2, DC2CTL, 0, DC2CTL, 1, 0x7F, 900, 2400, 12500, 14),
+	RC5T583_REG(DC3, DC3CTL, 0, DC3CTL, 1, 0x7F, 900, 2400, 12500, 14),
+	RC5T583_REG(LDO0, LDOEN2, 0, LDODIS2, 0, 0x7F, 900, 3400, 25000, 160),
+	RC5T583_REG(LDO1, LDOEN2, 1, LDODIS2, 1, 0x7F, 900, 3400, 25000, 160),
+	RC5T583_REG(LDO2, LDOEN2, 2, LDODIS2, 2, 0x7F, 900, 3400, 25000, 160),
+	RC5T583_REG(LDO3, LDOEN2, 3, LDODIS2, 3, 0x7F, 900, 3400, 25000, 160),
+	RC5T583_REG(LDO4, LDOEN2, 4, LDODIS2, 4, 0x3F, 750, 1500, 12500, 133),
+	RC5T583_REG(LDO5, LDOEN2, 5, LDODIS2, 5, 0x7F, 900, 3400, 25000, 267),
+	RC5T583_REG(LDO6, LDOEN2, 6, LDODIS2, 6, 0x7F, 900, 3400, 25000, 133),
+	RC5T583_REG(LDO7, LDOEN2, 7, LDODIS2, 7, 0x7F, 900, 3400, 25000, 233),
+	RC5T583_REG(LDO8, LDOEN1, 0, LDODIS1, 0, 0x7F, 900, 3400, 25000, 233),
+	RC5T583_REG(LDO9, LDOEN1, 1, LDODIS1, 1, 0x7F, 900, 3400, 25000, 133),
+};
+
+static int __devinit rc5t583_regulator_probe(struct platform_device *pdev)
+{
+	struct rc5t583 *rc5t583 = dev_get_drvdata(pdev->dev.parent);
+	struct rc5t583_platform_data *pdata = dev_get_platdata(rc5t583->dev);
+	struct regulator_init_data *reg_data;
+	struct regulator_config config = { };
+	struct rc5t583_regulator *reg = NULL;
+	struct rc5t583_regulator *regs;
+	struct regulator_dev *rdev;
+	struct rc5t583_regulator_info *ri;
+	int ret;
+	int id;
+
+	if (!pdata) {
+		dev_err(&pdev->dev, "No platform data, exiting...\n");
+		return -ENODEV;
+	}
+
+	regs = devm_kzalloc(&pdev->dev, RC5T583_REGULATOR_MAX *
+			sizeof(struct rc5t583_regulator), GFP_KERNEL);
+	if (!regs) {
+		dev_err(&pdev->dev, "Memory allocation failed exiting..\n");
+		return -ENOMEM;
+	}
+
+
+	for (id = 0; id < RC5T583_REGULATOR_MAX; ++id) {
+		reg_data = pdata->reg_init_data[id];
+
+		/* No need to register if there is no regulator data */
+		if (!reg_data)
+			continue;
+
+		reg = &regs[id];
+		ri = &rc5t583_reg_info[id];
+		reg->reg_info = ri;
+		reg->mfd = rc5t583;
+		reg->dev = &pdev->dev;
+
+		if (ri->deepsleep_id == RC5T583_DS_NONE)
+			goto skip_ext_pwr_config;
+
+		ret = rc5t583_ext_power_req_config(rc5t583->dev,
+				ri->deepsleep_id,
+				pdata->regulator_ext_pwr_control[id],
+				pdata->regulator_deepsleep_slot[id]);
+		/*
+		 * Configuring external control is not a major issue,
+		 * just give warning.
+		 */
+		if (ret < 0)
+			dev_warn(&pdev->dev,
+				"Failed to configure ext control %d\n", id);
+
+skip_ext_pwr_config:
+		config.dev = &pdev->dev;
+		config.init_data = reg_data;
+		config.driver_data = reg;
+		config.regmap = rc5t583->regmap;
+
+		rdev = regulator_register(&ri->desc, &config);
+		if (IS_ERR(rdev)) {
+			dev_err(&pdev->dev, "Failed to register regulator %s\n",
+						ri->desc.name);
+			ret = PTR_ERR(rdev);
+			goto clean_exit;
+		}
+		reg->rdev = rdev;
+	}
+	platform_set_drvdata(pdev, regs);
+	return 0;
+
+clean_exit:
+	while (--id >= 0)
+		regulator_unregister(regs[id].rdev);
+
+	return ret;
+}
+
+static int __devexit rc5t583_regulator_remove(struct platform_device *pdev)
+{
+	struct rc5t583_regulator *regs = platform_get_drvdata(pdev);
+	int id;
+
+	for (id = 0; id < RC5T583_REGULATOR_MAX; ++id)
+		regulator_unregister(regs[id].rdev);
+	return 0;
+}
+
+static struct platform_driver rc5t583_regulator_driver = {
+	.driver	= {
+		.name	= "rc5t583-regulator",
+		.owner	= THIS_MODULE,
+	},
+	.probe		= rc5t583_regulator_probe,
+	.remove		= __devexit_p(rc5t583_regulator_remove),
+};
+
+static int __init rc5t583_regulator_init(void)
+{
+	return platform_driver_register(&rc5t583_regulator_driver);
+}
+subsys_initcall(rc5t583_regulator_init);
+
+static void __exit rc5t583_regulator_exit(void)
+{
+	platform_driver_unregister(&rc5t583_regulator_driver);
+}
+module_exit(rc5t583_regulator_exit);
+
+MODULE_AUTHOR("Laxman Dewangan <ldewangan@nvidia.com>");
+MODULE_DESCRIPTION("RC5T583 regulator driver");
+MODULE_ALIAS("platform:rc5t583-regulator");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/regulator/s5m8767.c b/drivers/regulator/s5m8767.c
index 4ca2db05900..290d6fc0102 100644
--- a/drivers/regulator/s5m8767.c
+++ b/drivers/regulator/s5m8767.c
@@ -12,7 +12,6 @@
  */
 
 #include <linux/bug.h>
-#include <linux/delay.h>
 #include <linux/err.h>
 #include <linux/gpio.h>
 #include <linux/slab.h>
@@ -28,6 +27,7 @@ struct s5m8767_info {
 	struct s5m87xx_dev *iodev;
 	int num_regulators;
 	struct regulator_dev **rdev;
+	struct s5m_opmode_data *opmode;
 
 	int ramp_delay;
 	bool buck2_ramp;
@@ -141,9 +141,56 @@ static int s5m8767_list_voltage(struct regulator_dev *rdev,
 	return val;
 }
 
-static int s5m8767_get_register(struct regulator_dev *rdev, int *reg)
+static unsigned int s5m8767_opmode_reg[][4] = {
+	/* {OFF, ON, LOWPOWER, SUSPEND} */
+	/* LDO1 ... LDO28 */
+	{0x0, 0x3, 0x2, 0x1}, /* LDO1 */
+	{0x0, 0x3, 0x2, 0x1},
+	{0x0, 0x3, 0x2, 0x1},
+	{0x0, 0x0, 0x0, 0x0},
+	{0x0, 0x3, 0x2, 0x1}, /* LDO5 */
+	{0x0, 0x3, 0x2, 0x1},
+	{0x0, 0x3, 0x2, 0x1},
+	{0x0, 0x3, 0x2, 0x1},
+	{0x0, 0x3, 0x2, 0x1},
+	{0x0, 0x3, 0x2, 0x1}, /* LDO10 */
+	{0x0, 0x3, 0x2, 0x1},
+	{0x0, 0x3, 0x2, 0x1},
+	{0x0, 0x3, 0x2, 0x1},
+	{0x0, 0x3, 0x2, 0x1},
+	{0x0, 0x3, 0x2, 0x1}, /* LDO15 */
+	{0x0, 0x3, 0x2, 0x1},
+	{0x0, 0x3, 0x2, 0x1},
+	{0x0, 0x0, 0x0, 0x0},
+	{0x0, 0x3, 0x2, 0x1},
+	{0x0, 0x3, 0x2, 0x1}, /* LDO20 */
+	{0x0, 0x3, 0x2, 0x1},
+	{0x0, 0x3, 0x2, 0x1},
+	{0x0, 0x0, 0x0, 0x0},
+	{0x0, 0x3, 0x2, 0x1},
+	{0x0, 0x3, 0x2, 0x1}, /* LDO25 */
+	{0x0, 0x3, 0x2, 0x1},
+	{0x0, 0x3, 0x2, 0x1},
+	{0x0, 0x3, 0x2, 0x1}, /* LDO28 */
+
+	/* BUCK1 ... BUCK9 */
+	{0x0, 0x3, 0x1, 0x1}, /* BUCK1 */
+	{0x0, 0x3, 0x1, 0x1},
+	{0x0, 0x3, 0x1, 0x1},
+	{0x0, 0x3, 0x1, 0x1},
+	{0x0, 0x3, 0x2, 0x1}, /* BUCK5 */
+	{0x0, 0x3, 0x1, 0x1},
+	{0x0, 0x3, 0x1, 0x1},
+	{0x0, 0x3, 0x1, 0x1},
+	{0x0, 0x3, 0x1, 0x1}, /* BUCK9 */
+};
+
+static int s5m8767_get_register(struct regulator_dev *rdev, int *reg,
+				int *enable_ctrl)
 {
 	int reg_id = rdev_get_id(rdev);
+	unsigned int mode;
+	struct s5m8767_info *s5m8767 = rdev_get_drvdata(rdev);
 
 	switch (reg_id) {
 	case S5M8767_LDO1 ... S5M8767_LDO2:
@@ -168,6 +215,8 @@ static int s5m8767_get_register(struct regulator_dev *rdev, int *reg)
 		return -EINVAL;
 	}
 
+	mode = s5m8767->opmode[reg_id].mode;
+	*enable_ctrl = s5m8767_opmode_reg[reg_id][mode] << S5M8767_ENCTRL_SHIFT;
 	return 0;
 }
 
@@ -175,10 +224,10 @@ static int s5m8767_reg_is_enabled(struct regulator_dev *rdev)
 {
 	struct s5m8767_info *s5m8767 = rdev_get_drvdata(rdev);
 	int ret, reg;
-	int mask = 0xc0, pattern = 0xc0;
+	int mask = 0xc0, enable_ctrl;
 	u8 val;
 
-	ret = s5m8767_get_register(rdev, &reg);
+	ret = s5m8767_get_register(rdev, &reg, &enable_ctrl);
 	if (ret == -EINVAL)
 		return 1;
 	else if (ret)
@@ -188,33 +237,33 @@ static int s5m8767_reg_is_enabled(struct regulator_dev *rdev)
 	if (ret)
 		return ret;
 
-	return (val & mask) == pattern;
+	return (val & mask) == enable_ctrl;
 }
 
 static int s5m8767_reg_enable(struct regulator_dev *rdev)
 {
 	struct s5m8767_info *s5m8767 = rdev_get_drvdata(rdev);
 	int ret, reg;
-	int mask = 0xc0, pattern = 0xc0;
+	int mask = 0xc0, enable_ctrl;
 
-	ret = s5m8767_get_register(rdev, &reg);
+	ret = s5m8767_get_register(rdev, &reg, &enable_ctrl);
 	if (ret)
 		return ret;
 
-	return s5m_reg_update(s5m8767->iodev, reg, pattern, mask);
+	return s5m_reg_update(s5m8767->iodev, reg, enable_ctrl, mask);
 }
 
 static int s5m8767_reg_disable(struct regulator_dev *rdev)
 {
 	struct s5m8767_info *s5m8767 = rdev_get_drvdata(rdev);
 	int ret, reg;
-	int  mask = 0xc0, pattern = 0xc0;
+	int  mask = 0xc0, enable_ctrl;
 
-	ret = s5m8767_get_register(rdev, &reg);
+	ret = s5m8767_get_register(rdev, &reg, &enable_ctrl);
 	if (ret)
 		return ret;
 
-	return s5m_reg_update(s5m8767->iodev, reg, ~pattern, mask);
+	return s5m_reg_update(s5m8767->iodev, reg, ~mask, mask);
 }
 
 static int s5m8767_get_voltage_register(struct regulator_dev *rdev, int *_reg)
@@ -297,7 +346,10 @@ static int s5m8767_convert_voltage_to_sel(
 	if (max_vol < desc->min || min_vol > desc->max)
 		return -EINVAL;
 
-	selector = (min_vol - desc->min) / desc->step;
+	if (min_vol < desc->min)
+		min_vol = desc->min;
+
+	selector = DIV_ROUND_UP(min_vol - desc->min, desc->step);
 
 	if (desc->min + desc->step * selector > max_vol)
 		return -EINVAL;
@@ -305,14 +357,33 @@ static int s5m8767_convert_voltage_to_sel(
 	return selector;
 }
 
+static inline void s5m8767_set_high(struct s5m8767_info *s5m8767)
+{
+	int temp_index = s5m8767->buck_gpioindex;
+
+	gpio_set_value(s5m8767->buck_gpios[0], (temp_index >> 2) & 0x1);
+	gpio_set_value(s5m8767->buck_gpios[1], (temp_index >> 1) & 0x1);
+	gpio_set_value(s5m8767->buck_gpios[2], temp_index & 0x1);
+}
+
+static inline void s5m8767_set_low(struct s5m8767_info *s5m8767)
+{
+	int temp_index = s5m8767->buck_gpioindex;
+
+	gpio_set_value(s5m8767->buck_gpios[2], temp_index & 0x1);
+	gpio_set_value(s5m8767->buck_gpios[1], (temp_index >> 1) & 0x1);
+	gpio_set_value(s5m8767->buck_gpios[0], (temp_index >> 2) & 0x1);
+}
+
 static int s5m8767_set_voltage(struct regulator_dev *rdev,
 				int min_uV, int max_uV, unsigned *selector)
 {
 	struct s5m8767_info *s5m8767 = rdev_get_drvdata(rdev);
 	const struct s5m_voltage_desc *desc;
 	int reg_id = rdev_get_id(rdev);
-	int sel, reg, mask, ret;
+	int sel, reg, mask, ret = 0, old_index, index = 0;
 	u8 val;
+	u8 *buck234_vol = NULL;
 
 	switch (reg_id) {
 	case S5M8767_LDO1 ... S5M8767_LDO28:
@@ -320,6 +391,12 @@ static int s5m8767_set_voltage(struct regulator_dev *rdev,
 		break;
 	case S5M8767_BUCK1 ... S5M8767_BUCK6:
 		mask = 0xff;
+		if (reg_id == S5M8767_BUCK2 && s5m8767->buck2_gpiodvs)
+			buck234_vol = &s5m8767->buck2_vol[0];
+		else if (reg_id == S5M8767_BUCK3 && s5m8767->buck3_gpiodvs)
+			buck234_vol = &s5m8767->buck3_vol[0];
+		else if (reg_id == S5M8767_BUCK4 && s5m8767->buck4_gpiodvs)
+			buck234_vol = &s5m8767->buck4_vol[0];
 		break;
 	case S5M8767_BUCK7 ... S5M8767_BUCK8:
 		return -EINVAL;
@@ -336,102 +413,32 @@ static int s5m8767_set_voltage(struct regulator_dev *rdev,
 	if (sel < 0)
 		return sel;
 
-	ret = s5m8767_get_voltage_register(rdev, &reg);
-	if (ret)
-		return ret;
-
-	s5m_reg_read(s5m8767->iodev, reg, &val);
-	val &= ~mask;
-	val |= sel;
-
-	ret = s5m_reg_write(s5m8767->iodev, reg, val);
-	*selector = sel;
-
-	return ret;
-}
-
-static inline void s5m8767_set_high(struct s5m8767_info *s5m8767)
-{
-	int temp_index = s5m8767->buck_gpioindex;
-
-	gpio_set_value(s5m8767->buck_gpios[0], (temp_index >> 2) & 0x1);
-	gpio_set_value(s5m8767->buck_gpios[1], (temp_index >> 1) & 0x1);
-	gpio_set_value(s5m8767->buck_gpios[2], temp_index & 0x1);
-}
-
-static inline void s5m8767_set_low(struct s5m8767_info *s5m8767)
-{
-	int temp_index = s5m8767->buck_gpioindex;
-
-	gpio_set_value(s5m8767->buck_gpios[2], temp_index & 0x1);
-	gpio_set_value(s5m8767->buck_gpios[1], (temp_index >> 1) & 0x1);
-	gpio_set_value(s5m8767->buck_gpios[0], (temp_index >> 2) & 0x1);
-}
-
-static int s5m8767_set_voltage_buck(struct regulator_dev *rdev,
-				    int min_uV, int max_uV, unsigned *selector)
-{
-	struct s5m8767_info *s5m8767 = rdev_get_drvdata(rdev);
-	int reg_id = rdev_get_id(rdev);
-	const struct s5m_voltage_desc *desc;
-	int new_val, old_val, i = 0;
-
-	if (reg_id < S5M8767_BUCK1 || reg_id > S5M8767_BUCK6)
-		return -EINVAL;
-
-	switch (reg_id) {
-	case S5M8767_BUCK1:
-		return s5m8767_set_voltage(rdev, min_uV, max_uV, selector);
-	case S5M8767_BUCK2 ... S5M8767_BUCK4:
-		break;
-	case S5M8767_BUCK5 ... S5M8767_BUCK6:
-		return s5m8767_set_voltage(rdev, min_uV, max_uV, selector);
-	case S5M8767_BUCK9:
-		return s5m8767_set_voltage(rdev, min_uV, max_uV, selector);
-	}
+	/* buck234_vol != NULL means to control buck234 voltage via DVS GPIO */
+	if (buck234_vol) {
+		while (*buck234_vol != sel) {
+			buck234_vol++;
+			index++;
+		}
+		old_index = s5m8767->buck_gpioindex;
+		s5m8767->buck_gpioindex = index;
+
+		if (index > old_index)
+			s5m8767_set_high(s5m8767);
+		else
+			s5m8767_set_low(s5m8767);
+	} else {
+		ret = s5m8767_get_voltage_register(rdev, &reg);
+		if (ret)
+			return ret;
 
-	desc = reg_voltage_map[reg_id];
-	new_val = s5m8767_convert_voltage_to_sel(desc, min_uV, max_uV);
-	if (new_val < 0)
-		return new_val;
+		s5m_reg_read(s5m8767->iodev, reg, &val);
+		val = (val & ~mask) | sel;
 
-	switch (reg_id) {
-	case S5M8767_BUCK2:
-		if (s5m8767->buck2_gpiodvs) {
-			while (s5m8767->buck2_vol[i] != new_val)
-				i++;
-		} else
-			return s5m8767_set_voltage(rdev, min_uV,
-						   max_uV, selector);
-		break;
-	case S5M8767_BUCK3:
-		if (s5m8767->buck3_gpiodvs) {
-			while (s5m8767->buck3_vol[i] != new_val)
-				i++;
-		} else
-			return s5m8767_set_voltage(rdev, min_uV,
-						   max_uV, selector);
-		break;
-	case S5M8767_BUCK4:
-		if (s5m8767->buck3_gpiodvs) {
-			while (s5m8767->buck4_vol[i] != new_val)
-				i++;
-		} else
-			return s5m8767_set_voltage(rdev, min_uV,
-						   max_uV, selector);
-		break;
+		ret = s5m_reg_write(s5m8767->iodev, reg, val);
 	}
 
-	old_val = s5m8767->buck_gpioindex;
-	s5m8767->buck_gpioindex = i;
-
-	if (i > old_val)
-		s5m8767_set_high(s5m8767);
-	else
-		s5m8767_set_low(s5m8767);
-
-	*selector = new_val;
-	return 0;
+	*selector = sel;
+	return ret;
 }
 
 static int s5m8767_set_voltage_time_sel(struct regulator_dev *rdev,
@@ -450,7 +457,7 @@ static int s5m8767_set_voltage_time_sel(struct regulator_dev *rdev,
 	return 0;
 }
 
-static struct regulator_ops s5m8767_ldo_ops = {
+static struct regulator_ops s5m8767_ops = {
 	.list_voltage		= s5m8767_list_voltage,
 	.is_enabled		= s5m8767_reg_is_enabled,
 	.enable			= s5m8767_reg_enable,
@@ -460,75 +467,59 @@ static struct regulator_ops s5m8767_ldo_ops = {
 	.set_voltage_time_sel	= s5m8767_set_voltage_time_sel,
 };
 
-static struct regulator_ops s5m8767_buck_ops = {
-	.list_voltage		= s5m8767_list_voltage,
-	.is_enabled		= s5m8767_reg_is_enabled,
-	.enable			= s5m8767_reg_enable,
-	.disable		= s5m8767_reg_disable,
-	.get_voltage_sel	= s5m8767_get_voltage_sel,
-	.set_voltage		= s5m8767_set_voltage_buck,
-	.set_voltage_time_sel	= s5m8767_set_voltage_time_sel,
-};
-
-#define regulator_desc_ldo(num)		{	\
-	.name		= "LDO"#num,		\
-	.id		= S5M8767_LDO##num,	\
-	.ops		= &s5m8767_ldo_ops,	\
-	.type		= REGULATOR_VOLTAGE,	\
-	.owner		= THIS_MODULE,		\
-}
-#define regulator_desc_buck(num)	{	\
-	.name		= "BUCK"#num,		\
-	.id		= S5M8767_BUCK##num,	\
-	.ops		= &s5m8767_buck_ops,	\
+#define s5m8767_regulator_desc(_name) {		\
+	.name		= #_name,		\
+	.id		= S5M8767_##_name,	\
+	.ops		= &s5m8767_ops,		\
 	.type		= REGULATOR_VOLTAGE,	\
 	.owner		= THIS_MODULE,		\
 }
 
 static struct regulator_desc regulators[] = {
-	regulator_desc_ldo(1),
-	regulator_desc_ldo(2),
-	regulator_desc_ldo(3),
-	regulator_desc_ldo(4),
-	regulator_desc_ldo(5),
-	regulator_desc_ldo(6),
-	regulator_desc_ldo(7),
-	regulator_desc_ldo(8),
-	regulator_desc_ldo(9),
-	regulator_desc_ldo(10),
-	regulator_desc_ldo(11),
-	regulator_desc_ldo(12),
-	regulator_desc_ldo(13),
-	regulator_desc_ldo(14),
-	regulator_desc_ldo(15),
-	regulator_desc_ldo(16),
-	regulator_desc_ldo(17),
-	regulator_desc_ldo(18),
-	regulator_desc_ldo(19),
-	regulator_desc_ldo(20),
-	regulator_desc_ldo(21),
-	regulator_desc_ldo(22),
-	regulator_desc_ldo(23),
-	regulator_desc_ldo(24),
-	regulator_desc_ldo(25),
-	regulator_desc_ldo(26),
-	regulator_desc_ldo(27),
-	regulator_desc_ldo(28),
-	regulator_desc_buck(1),
-	regulator_desc_buck(2),
-	regulator_desc_buck(3),
-	regulator_desc_buck(4),
-	regulator_desc_buck(5),
-	regulator_desc_buck(6),
-	regulator_desc_buck(7),
-	regulator_desc_buck(8),
-	regulator_desc_buck(9),
+	s5m8767_regulator_desc(LDO1),
+	s5m8767_regulator_desc(LDO2),
+	s5m8767_regulator_desc(LDO3),
+	s5m8767_regulator_desc(LDO4),
+	s5m8767_regulator_desc(LDO5),
+	s5m8767_regulator_desc(LDO6),
+	s5m8767_regulator_desc(LDO7),
+	s5m8767_regulator_desc(LDO8),
+	s5m8767_regulator_desc(LDO9),
+	s5m8767_regulator_desc(LDO10),
+	s5m8767_regulator_desc(LDO11),
+	s5m8767_regulator_desc(LDO12),
+	s5m8767_regulator_desc(LDO13),
+	s5m8767_regulator_desc(LDO14),
+	s5m8767_regulator_desc(LDO15),
+	s5m8767_regulator_desc(LDO16),
+	s5m8767_regulator_desc(LDO17),
+	s5m8767_regulator_desc(LDO18),
+	s5m8767_regulator_desc(LDO19),
+	s5m8767_regulator_desc(LDO20),
+	s5m8767_regulator_desc(LDO21),
+	s5m8767_regulator_desc(LDO22),
+	s5m8767_regulator_desc(LDO23),
+	s5m8767_regulator_desc(LDO24),
+	s5m8767_regulator_desc(LDO25),
+	s5m8767_regulator_desc(LDO26),
+	s5m8767_regulator_desc(LDO27),
+	s5m8767_regulator_desc(LDO28),
+	s5m8767_regulator_desc(BUCK1),
+	s5m8767_regulator_desc(BUCK2),
+	s5m8767_regulator_desc(BUCK3),
+	s5m8767_regulator_desc(BUCK4),
+	s5m8767_regulator_desc(BUCK5),
+	s5m8767_regulator_desc(BUCK6),
+	s5m8767_regulator_desc(BUCK7),
+	s5m8767_regulator_desc(BUCK8),
+	s5m8767_regulator_desc(BUCK9),
 };
 
 static __devinit int s5m8767_pmic_probe(struct platform_device *pdev)
 {
 	struct s5m87xx_dev *iodev = dev_get_drvdata(pdev->dev.parent);
 	struct s5m_platform_data *pdata = dev_get_platdata(iodev->dev);
+	struct regulator_config config = { };
 	struct regulator_dev **rdev;
 	struct s5m8767_info *s5m8767;
 	int i, ret, size;
@@ -586,6 +577,7 @@ static __devinit int s5m8767_pmic_probe(struct platform_device *pdev)
 	s5m8767->buck2_ramp = pdata->buck2_ramp_enable;
 	s5m8767->buck3_ramp = pdata->buck3_ramp_enable;
 	s5m8767->buck4_ramp = pdata->buck4_ramp_enable;
+	s5m8767->opmode = pdata->opmode;
 
 	for (i = 0; i < 8; i++) {
 		if (s5m8767->buck2_gpiodvs) {
@@ -723,8 +715,11 @@ static __devinit int s5m8767_pmic_probe(struct platform_device *pdev)
 			regulators[id].n_voltages =
 				(desc->max - desc->min) / desc->step + 1;
 
-		rdev[i] = regulator_register(&regulators[id], s5m8767->dev,
-				pdata->regulators[i].initdata, s5m8767, NULL);
+		config.dev = s5m8767->dev;
+		config.init_data = pdata->regulators[i].initdata;
+		config.driver_data = s5m8767;
+
+		rdev[i] = regulator_register(&regulators[id], &config);
 		if (IS_ERR(rdev[i])) {
 			ret = PTR_ERR(rdev[i]);
 			dev_err(s5m8767->dev, "regulator init failed for %d\n",
diff --git a/drivers/regulator/tps6105x-regulator.c b/drivers/regulator/tps6105x-regulator.c
index d9278da18a9..d840d8440a9 100644
--- a/drivers/regulator/tps6105x-regulator.c
+++ b/drivers/regulator/tps6105x-regulator.c
@@ -123,7 +123,7 @@ static struct regulator_ops tps6105x_regulator_ops = {
 	.list_voltage	= tps6105x_regulator_list_voltage,
 };
 
-static struct regulator_desc tps6105x_regulator_desc = {
+static const struct regulator_desc tps6105x_regulator_desc = {
 	.name		= "tps6105x-boost",
 	.ops		= &tps6105x_regulator_ops,
 	.type		= REGULATOR_VOLTAGE,
@@ -139,6 +139,7 @@ static int __devinit tps6105x_regulator_probe(struct platform_device *pdev)
 {
 	struct tps6105x *tps6105x = dev_get_platdata(&pdev->dev);
 	struct tps6105x_platform_data *pdata = tps6105x->pdata;
+	struct regulator_config config = { };
 	int ret;
 
 	/* This instance is not set for regulator mode so bail out */
@@ -148,11 +149,13 @@ static int __devinit tps6105x_regulator_probe(struct platform_device *pdev)
 		return 0;
 	}
 
+	config.dev = &tps6105x->client->dev;
+	config.init_data = pdata->regulator_data;
+	config.driver_data = tps6105x;
+
 	/* Register regulator with framework */
 	tps6105x->regulator = regulator_register(&tps6105x_regulator_desc,
-					     &tps6105x->client->dev,
-					     pdata->regulator_data, tps6105x,
-					     NULL);
+						 &config);
 	if (IS_ERR(tps6105x->regulator)) {
 		ret = PTR_ERR(tps6105x->regulator);
 		dev_err(&tps6105x->client->dev,
diff --git a/drivers/regulator/tps62360-regulator.c b/drivers/regulator/tps62360-regulator.c
index e2ec73068ee..e534269ed44 100644
--- a/drivers/regulator/tps62360-regulator.c
+++ b/drivers/regulator/tps62360-regulator.c
@@ -1,7 +1,7 @@
 /*
  * tps62360.c -- TI tps62360
  *
- * Driver for processor core supply tps62360 and tps62361B
+ * Driver for processor core supply tps62360, tps62361B, tps62362 and tps62363.
  *
  * Copyright (c) 2012, NVIDIA Corporation.
  *
@@ -26,13 +26,16 @@
 #include <linux/module.h>
 #include <linux/init.h>
 #include <linux/err.h>
+#include <linux/of.h>
+#include <linux/of_device.h>
+#include <linux/of_gpio.h>
+#include <linux/regulator/of_regulator.h>
 #include <linux/platform_device.h>
 #include <linux/regulator/driver.h>
 #include <linux/regulator/machine.h>
 #include <linux/regulator/tps62360.h>
 #include <linux/gpio.h>
 #include <linux/i2c.h>
-#include <linux/delay.h>
 #include <linux/slab.h>
 #include <linux/regmap.h>
 
@@ -46,20 +49,20 @@
 #define REG_RAMPCTRL		6
 #define REG_CHIPID		8
 
-enum chips {TPS62360, TPS62361};
+#define FORCE_PWM_ENABLE	BIT(7)
 
-#define TPS62360_BASE_VOLTAGE	770
+enum chips {TPS62360, TPS62361, TPS62362, TPS62363};
+
+#define TPS62360_BASE_VOLTAGE	770000
 #define TPS62360_N_VOLTAGES	64
 
-#define TPS62361_BASE_VOLTAGE	500
+#define TPS62361_BASE_VOLTAGE	500000
 #define TPS62361_N_VOLTAGES	128
 
 /* tps 62360 chip information */
 struct tps62360_chip {
-	const char *name;
 	struct device *dev;
 	struct regulator_desc desc;
-	struct i2c_client *client;
 	struct regulator_dev *rdev;
 	struct regmap *regmap;
 	int chip_id;
@@ -68,12 +71,12 @@ struct tps62360_chip {
 	int voltage_base;
 	u8 voltage_reg_mask;
 	bool en_internal_pulldn;
-	bool en_force_pwm;
 	bool en_discharge;
 	bool valid_gpios;
 	int lru_index[4];
 	int curr_vset_vsel[4];
 	int curr_vset_id;
+	int change_uv_per_us;
 };
 
 /*
@@ -99,6 +102,7 @@ static bool find_voltage_set_register(struct tps62360_chip *tps,
 	bool found = false;
 	int new_vset_reg = tps->lru_index[3];
 	int found_index = 3;
+
 	for (i = 0; i < 4; ++i) {
 		if (tps->curr_vset_vsel[tps->lru_index[i]] == req_vsel) {
 			new_vset_reg = tps->lru_index[i];
@@ -117,7 +121,7 @@ update_lru_index:
 	return found;
 }
 
-static int tps62360_dcdc_get_voltage(struct regulator_dev *dev)
+static int tps62360_dcdc_get_voltage_sel(struct regulator_dev *dev)
 {
 	struct tps62360_chip *tps = rdev_get_drvdata(dev);
 	int vsel;
@@ -126,196 +130,312 @@ static int tps62360_dcdc_get_voltage(struct regulator_dev *dev)
 
 	ret = regmap_read(tps->regmap, REG_VSET0 + tps->curr_vset_id, &data);
 	if (ret < 0) {
-		dev_err(tps->dev, "%s: Error in reading register %d\n",
-			__func__, REG_VSET0 + tps->curr_vset_id);
+		dev_err(tps->dev, "%s(): register %d read failed with err %d\n",
+			__func__, REG_VSET0 + tps->curr_vset_id, ret);
 		return ret;
 	}
 	vsel = (int)data & tps->voltage_reg_mask;
-	return (tps->voltage_base + vsel * 10) * 1000;
+	return vsel;
 }
 
-static int tps62360_dcdc_set_voltage(struct regulator_dev *dev,
-	     int min_uV, int max_uV, unsigned *selector)
+static int tps62360_dcdc_set_voltage_sel(struct regulator_dev *dev,
+					 unsigned selector)
 {
 	struct tps62360_chip *tps = rdev_get_drvdata(dev);
-	int vsel;
 	int ret;
 	bool found = false;
 	int new_vset_id = tps->curr_vset_id;
 
-	if (max_uV < min_uV)
-		return -EINVAL;
-
-	if (min_uV >
-		((tps->voltage_base + (tps->desc.n_voltages - 1) * 10) * 1000))
-		return -EINVAL;
-
-	if (max_uV < tps->voltage_base * 1000)
-		return -EINVAL;
-
-	vsel = DIV_ROUND_UP(min_uV - (tps->voltage_base * 1000), 10000);
-	if (selector)
-		*selector = (vsel & tps->voltage_reg_mask);
-
 	/*
 	 * If gpios are available to select the VSET register then least
 	 * recently used register for new configuration.
 	 */
 	if (tps->valid_gpios)
-		found = find_voltage_set_register(tps, vsel, &new_vset_id);
+		found = find_voltage_set_register(tps, selector, &new_vset_id);
 
 	if (!found) {
 		ret = regmap_update_bits(tps->regmap, REG_VSET0 + new_vset_id,
-				tps->voltage_reg_mask, vsel);
+				tps->voltage_reg_mask, selector);
 		if (ret < 0) {
-			dev_err(tps->dev, "%s: Error in updating register %d\n",
-				 __func__, REG_VSET0 + new_vset_id);
+			dev_err(tps->dev,
+				"%s(): register %d update failed with err %d\n",
+				 __func__, REG_VSET0 + new_vset_id, ret);
 			return ret;
 		}
 		tps->curr_vset_id = new_vset_id;
-		tps->curr_vset_vsel[new_vset_id] = vsel;
+		tps->curr_vset_vsel[new_vset_id] = selector;
 	}
 
 	/* Select proper VSET register vio gpios */
 	if (tps->valid_gpios) {
-		gpio_set_value_cansleep(tps->vsel0_gpio,
-					new_vset_id & 0x1);
+		gpio_set_value_cansleep(tps->vsel0_gpio, new_vset_id & 0x1);
 		gpio_set_value_cansleep(tps->vsel1_gpio,
 					(new_vset_id >> 1) & 0x1);
 	}
 	return 0;
 }
 
-static int tps62360_dcdc_list_voltage(struct regulator_dev *dev,
-					unsigned selector)
+static int tps62360_set_voltage_time_sel(struct regulator_dev *rdev,
+		unsigned int old_selector, unsigned int new_selector)
 {
-	struct tps62360_chip *tps = rdev_get_drvdata(dev);
+	struct tps62360_chip *tps = rdev_get_drvdata(rdev);
+	int old_uV, new_uV;
 
-	if (selector >= tps->desc.n_voltages)
-		return -EINVAL;
-	return (tps->voltage_base + selector * 10) * 1000;
+	old_uV = regulator_list_voltage_linear(rdev, old_selector);
+	if (old_uV < 0)
+		return old_uV;
+
+	new_uV = regulator_list_voltage_linear(rdev, new_selector);
+	if (new_uV < 0)
+		return new_uV;
+
+	return DIV_ROUND_UP(abs(old_uV - new_uV), tps->change_uv_per_us);
 }
 
-static struct regulator_ops tps62360_dcdc_ops = {
-	.get_voltage = tps62360_dcdc_get_voltage,
-	.set_voltage = tps62360_dcdc_set_voltage,
-	.list_voltage = tps62360_dcdc_list_voltage,
-};
+static int tps62360_set_mode(struct regulator_dev *rdev, unsigned int mode)
+{
+	struct tps62360_chip *tps = rdev_get_drvdata(rdev);
+	int i;
+	int val;
+	int ret;
+
+	/* Enable force PWM mode in FAST mode only. */
+	switch (mode) {
+	case REGULATOR_MODE_FAST:
+		val = FORCE_PWM_ENABLE;
+		break;
+
+	case REGULATOR_MODE_NORMAL:
+		val = 0;
+		break;
 
-static int tps62360_init_force_pwm(struct tps62360_chip *tps,
-	struct tps62360_regulator_platform_data *pdata,
-	int vset_id)
+	default:
+		return -EINVAL;
+	}
+
+	if (!tps->valid_gpios) {
+		ret = regmap_update_bits(tps->regmap,
+			REG_VSET0 + tps->curr_vset_id, FORCE_PWM_ENABLE, val);
+		if (ret < 0)
+			dev_err(tps->dev,
+				"%s(): register %d update failed with err %d\n",
+				__func__, REG_VSET0 + tps->curr_vset_id, ret);
+		return ret;
+	}
+
+	/* If gpios are valid then all register set need to be control */
+	for (i = 0; i < 4; ++i) {
+		ret = regmap_update_bits(tps->regmap,
+					REG_VSET0 + i, FORCE_PWM_ENABLE, val);
+		if (ret < 0) {
+			dev_err(tps->dev,
+				"%s(): register %d update failed with err %d\n",
+				__func__, REG_VSET0 + i, ret);
+			return ret;
+		}
+	}
+	return ret;
+}
+
+static unsigned int tps62360_get_mode(struct regulator_dev *rdev)
 {
+	struct tps62360_chip *tps = rdev_get_drvdata(rdev);
 	unsigned int data;
 	int ret;
-	ret = regmap_read(tps->regmap, REG_VSET0 + vset_id, &data);
+
+	ret = regmap_read(tps->regmap, REG_VSET0 + tps->curr_vset_id, &data);
 	if (ret < 0) {
-		dev_err(tps->dev, "%s() fails in writing reg %d\n",
-			__func__, REG_VSET0 + vset_id);
+		dev_err(tps->dev, "%s(): register %d read failed with err %d\n",
+			__func__, REG_VSET0 + tps->curr_vset_id, ret);
 		return ret;
 	}
-	tps->curr_vset_vsel[vset_id] = data & tps->voltage_reg_mask;
-	if (pdata->en_force_pwm)
-		data |= BIT(7);
-	else
-		data &= ~BIT(7);
-	ret = regmap_write(tps->regmap, REG_VSET0 + vset_id, data);
-	if (ret < 0)
-		dev_err(tps->dev, "%s() fails in writing reg %d\n",
-				__func__, REG_VSET0 + vset_id);
-	return ret;
+	return (data & FORCE_PWM_ENABLE) ?
+				REGULATOR_MODE_FAST : REGULATOR_MODE_NORMAL;
 }
 
-static int tps62360_init_dcdc(struct tps62360_chip *tps,
+static struct regulator_ops tps62360_dcdc_ops = {
+	.get_voltage_sel	= tps62360_dcdc_get_voltage_sel,
+	.set_voltage_sel	= tps62360_dcdc_set_voltage_sel,
+	.list_voltage		= regulator_list_voltage_linear,
+	.map_voltage		= regulator_map_voltage_linear,
+	.set_voltage_time_sel	= tps62360_set_voltage_time_sel,
+	.set_mode		= tps62360_set_mode,
+	.get_mode		= tps62360_get_mode,
+};
+
+static int __devinit tps62360_init_dcdc(struct tps62360_chip *tps,
 		struct tps62360_regulator_platform_data *pdata)
 {
 	int ret;
-	int i;
+	unsigned int ramp_ctrl;
 
-	/* Initailize internal pull up/down control */
+	/* Initialize internal pull up/down control */
 	if (tps->en_internal_pulldn)
 		ret = regmap_write(tps->regmap, REG_CONTROL, 0xE0);
 	else
 		ret = regmap_write(tps->regmap, REG_CONTROL, 0x0);
 	if (ret < 0) {
-		dev_err(tps->dev, "%s() fails in writing reg %d\n",
-			__func__, REG_CONTROL);
+		dev_err(tps->dev,
+			"%s(): register %d write failed with err %d\n",
+			__func__, REG_CONTROL, ret);
 		return ret;
 	}
 
-	/* Initailize force PWM mode */
-	if (tps->valid_gpios) {
-		for (i = 0; i < 4; ++i) {
-			ret = tps62360_init_force_pwm(tps, pdata, i);
-			if (ret < 0)
-				return ret;
-		}
-	} else {
-		ret = tps62360_init_force_pwm(tps, pdata, tps->curr_vset_id);
-		if (ret < 0)
-			return ret;
-	}
-
 	/* Reset output discharge path to reduce power consumption */
 	ret = regmap_update_bits(tps->regmap, REG_RAMPCTRL, BIT(2), 0);
-	if (ret < 0)
-		dev_err(tps->dev, "%s() fails in updating reg %d\n",
-			__func__, REG_RAMPCTRL);
+	if (ret < 0) {
+		dev_err(tps->dev,
+			"%s(): register %d update failed with err %d\n",
+			__func__, REG_RAMPCTRL, ret);
+		return ret;
+	}
+
+	/* Get ramp value from ramp control register */
+	ret = regmap_read(tps->regmap, REG_RAMPCTRL, &ramp_ctrl);
+	if (ret < 0) {
+		dev_err(tps->dev,
+			"%s(): register %d read failed with err %d\n",
+			__func__, REG_RAMPCTRL, ret);
+		return ret;
+	}
+	ramp_ctrl = (ramp_ctrl >> 4) & 0x7;
+
+	/* ramp mV/us = 32/(2^ramp_ctrl) */
+	tps->change_uv_per_us = DIV_ROUND_UP(32000, BIT(ramp_ctrl));
 	return ret;
 }
 
 static const struct regmap_config tps62360_regmap_config = {
-	.reg_bits = 8,
-	.val_bits = 8,
+	.reg_bits		= 8,
+	.val_bits		= 8,
+	.max_register		= REG_CHIPID,
+	.cache_type		= REGCACHE_RBTREE,
 };
 
+static struct tps62360_regulator_platform_data *
+	of_get_tps62360_platform_data(struct device *dev)
+{
+	struct tps62360_regulator_platform_data *pdata;
+	struct device_node *np = dev->of_node;
+
+	pdata = devm_kzalloc(dev, sizeof(*pdata), GFP_KERNEL);
+	if (!pdata) {
+		dev_err(dev, "Memory alloc failed for platform data\n");
+		return NULL;
+	}
+
+	pdata->reg_init_data = of_get_regulator_init_data(dev, dev->of_node);
+	if (!pdata->reg_init_data) {
+		dev_err(dev, "Not able to get OF regulator init data\n");
+		return NULL;
+	}
+
+	pdata->vsel0_gpio = of_get_named_gpio(np, "vsel0-gpio", 0);
+	pdata->vsel1_gpio = of_get_named_gpio(np, "vsel1-gpio", 0);
+
+	if (of_find_property(np, "ti,vsel0-state-high", NULL))
+		pdata->vsel0_def_state = 1;
+
+	if (of_find_property(np, "ti,vsel1-state-high", NULL))
+		pdata->vsel1_def_state = 1;
+
+	if (of_find_property(np, "ti,enable-pull-down", NULL))
+		pdata->en_internal_pulldn = true;
+
+	if (of_find_property(np, "ti,enable-vout-discharge", NULL))
+		pdata->en_discharge = true;
+
+	return pdata;
+}
+
+#if defined(CONFIG_OF)
+static const struct of_device_id tps62360_of_match[] = {
+	 { .compatible = "ti,tps62360", .data = (void *)TPS62360},
+	 { .compatible = "ti,tps62361", .data = (void *)TPS62361},
+	 { .compatible = "ti,tps62362", .data = (void *)TPS62362},
+	 { .compatible = "ti,tps62363", .data = (void *)TPS62363},
+	{},
+};
+MODULE_DEVICE_TABLE(of, tps62360_of_match);
+#endif
+
 static int __devinit tps62360_probe(struct i2c_client *client,
 				     const struct i2c_device_id *id)
 {
+	struct regulator_config config = { };
 	struct tps62360_regulator_platform_data *pdata;
 	struct regulator_dev *rdev;
 	struct tps62360_chip *tps;
 	int ret;
 	int i;
+	int chip_id;
 
 	pdata = client->dev.platform_data;
+	chip_id = id->driver_data;
+
+	if (client->dev.of_node) {
+		const struct of_device_id *match;
+		match = of_match_device(of_match_ptr(tps62360_of_match),
+				&client->dev);
+		if (!match) {
+			dev_err(&client->dev, "Error: No device match found\n");
+			return -ENODEV;
+		}
+		chip_id = (int)match->data;
+		if (!pdata)
+			pdata = of_get_tps62360_platform_data(&client->dev);
+	}
+
 	if (!pdata) {
-		dev_err(&client->dev, "%s() Err: Platform data not found\n",
+		dev_err(&client->dev, "%s(): Platform data not found\n",
 						__func__);
 		return -EIO;
 	}
 
 	tps = devm_kzalloc(&client->dev, sizeof(*tps), GFP_KERNEL);
 	if (!tps) {
-		dev_err(&client->dev, "%s() Err: Memory allocation fails\n",
+		dev_err(&client->dev, "%s(): Memory allocation failed\n",
 						__func__);
 		return -ENOMEM;
 	}
 
-	tps->en_force_pwm = pdata->en_force_pwm;
 	tps->en_discharge = pdata->en_discharge;
 	tps->en_internal_pulldn = pdata->en_internal_pulldn;
 	tps->vsel0_gpio = pdata->vsel0_gpio;
 	tps->vsel1_gpio = pdata->vsel1_gpio;
-	tps->client = client;
 	tps->dev = &client->dev;
-	tps->name = id->name;
-	tps->voltage_base = (id->driver_data == TPS62360) ?
-				TPS62360_BASE_VOLTAGE : TPS62361_BASE_VOLTAGE;
-	tps->voltage_reg_mask = (id->driver_data == TPS62360) ? 0x3F : 0x7F;
+
+	switch (chip_id) {
+	case TPS62360:
+	case TPS62362:
+		tps->voltage_base = TPS62360_BASE_VOLTAGE;
+		tps->voltage_reg_mask = 0x3F;
+		tps->desc.n_voltages = TPS62360_N_VOLTAGES;
+		break;
+	case TPS62361:
+	case TPS62363:
+		tps->voltage_base = TPS62361_BASE_VOLTAGE;
+		tps->voltage_reg_mask = 0x7F;
+		tps->desc.n_voltages = TPS62361_N_VOLTAGES;
+		break;
+	default:
+		return -ENODEV;
+	}
 
 	tps->desc.name = id->name;
 	tps->desc.id = 0;
-	tps->desc.n_voltages = (id->driver_data == TPS62360) ?
-				TPS62360_N_VOLTAGES : TPS62361_N_VOLTAGES;
 	tps->desc.ops = &tps62360_dcdc_ops;
 	tps->desc.type = REGULATOR_VOLTAGE;
 	tps->desc.owner = THIS_MODULE;
-	tps->regmap = regmap_init_i2c(client, &tps62360_regmap_config);
+	tps->desc.min_uV = tps->voltage_base;
+	tps->desc.uV_step = 10000;
+
+	tps->regmap = devm_regmap_init_i2c(client, &tps62360_regmap_config);
 	if (IS_ERR(tps->regmap)) {
 		ret = PTR_ERR(tps->regmap);
-		dev_err(&client->dev, "%s() Err: Failed to allocate register"
-			"map: %d\n", __func__, ret);
+		dev_err(&client->dev,
+			"%s(): regmap allocation failed with err %d\n",
+			__func__, ret);
 		return ret;
 	}
 	i2c_set_clientdata(client, tps);
@@ -326,35 +446,26 @@ static int __devinit tps62360_probe(struct i2c_client *client,
 	tps->valid_gpios = false;
 
 	if (gpio_is_valid(tps->vsel0_gpio) && gpio_is_valid(tps->vsel1_gpio)) {
-		ret = gpio_request(tps->vsel0_gpio, "tps62360-vsel0");
+		int gpio_flags;
+		gpio_flags = (pdata->vsel0_def_state) ?
+				GPIOF_OUT_INIT_HIGH : GPIOF_OUT_INIT_LOW;
+		ret = gpio_request_one(tps->vsel0_gpio,
+				gpio_flags, "tps62360-vsel0");
 		if (ret) {
 			dev_err(&client->dev,
-				"Err: Could not obtain vsel0 GPIO %d: %d\n",
-						tps->vsel0_gpio, ret);
-			goto err_gpio0;
-		}
-		ret = gpio_direction_output(tps->vsel0_gpio,
-					pdata->vsel0_def_state);
-		if (ret) {
-			dev_err(&client->dev, "Err: Could not set direction of"
-				"vsel0 GPIO %d: %d\n", tps->vsel0_gpio, ret);
-			gpio_free(tps->vsel0_gpio);
+				"%s(): Could not obtain vsel0 GPIO %d: %d\n",
+				__func__, tps->vsel0_gpio, ret);
 			goto err_gpio0;
 		}
 
-		ret = gpio_request(tps->vsel1_gpio, "tps62360-vsel1");
+		gpio_flags = (pdata->vsel1_def_state) ?
+				GPIOF_OUT_INIT_HIGH : GPIOF_OUT_INIT_LOW;
+		ret = gpio_request_one(tps->vsel1_gpio,
+				gpio_flags, "tps62360-vsel1");
 		if (ret) {
 			dev_err(&client->dev,
-				"Err: Could not obtain vsel1 GPIO %d: %d\n",
-						tps->vsel1_gpio, ret);
-			goto err_gpio1;
-		}
-		ret = gpio_direction_output(tps->vsel1_gpio,
-					pdata->vsel1_def_state);
-		if (ret) {
-			dev_err(&client->dev, "Err: Could not set direction of"
-				"vsel1 GPIO %d: %d\n", tps->vsel1_gpio, ret);
-			gpio_free(tps->vsel1_gpio);
+				"%s(): Could not obtain vsel1 GPIO %d: %d\n",
+				__func__, tps->vsel1_gpio, ret);
 			goto err_gpio1;
 		}
 		tps->valid_gpios = true;
@@ -371,17 +482,22 @@ static int __devinit tps62360_probe(struct i2c_client *client,
 
 	ret = tps62360_init_dcdc(tps, pdata);
 	if (ret < 0) {
-		dev_err(tps->dev, "%s() Err: Init fails with = %d\n",
+		dev_err(tps->dev, "%s(): Init failed with err = %d\n",
 				__func__, ret);
 		goto err_init;
 	}
 
+	config.dev = &client->dev;
+	config.init_data = pdata->reg_init_data;
+	config.driver_data = tps;
+	config.of_node = client->dev.of_node;
+
 	/* Register the regulators */
-	rdev = regulator_register(&tps->desc, &client->dev,
-				&pdata->reg_init_data, tps, NULL);
+	rdev = regulator_register(&tps->desc, &config);
 	if (IS_ERR(rdev)) {
-		dev_err(tps->dev, "%s() Err: Failed to register %s\n",
-				__func__, id->name);
+		dev_err(tps->dev,
+			"%s(): regulator register failed with err %s\n",
+			__func__, id->name);
 		ret = PTR_ERR(rdev);
 		goto err_init;
 	}
@@ -396,7 +512,6 @@ err_gpio1:
 	if (gpio_is_valid(tps->vsel0_gpio))
 		gpio_free(tps->vsel0_gpio);
 err_gpio0:
-	regmap_exit(tps->regmap);
 	return ret;
 }
 
@@ -417,7 +532,6 @@ static int __devexit tps62360_remove(struct i2c_client *client)
 		gpio_free(tps->vsel0_gpio);
 
 	regulator_unregister(tps->rdev);
-	regmap_exit(tps->regmap);
 	return 0;
 }
 
@@ -432,13 +546,16 @@ static void tps62360_shutdown(struct i2c_client *client)
 	/* Configure the output discharge path */
 	st = regmap_update_bits(tps->regmap, REG_RAMPCTRL, BIT(2), BIT(2));
 	if (st < 0)
-		dev_err(tps->dev, "%s() fails in updating reg %d\n",
-			__func__, REG_RAMPCTRL);
+		dev_err(tps->dev,
+			"%s(): register %d update failed with err %d\n",
+			__func__, REG_RAMPCTRL, st);
 }
 
 static const struct i2c_device_id tps62360_id[] = {
 	{.name = "tps62360", .driver_data = TPS62360},
 	{.name = "tps62361", .driver_data = TPS62361},
+	{.name = "tps62362", .driver_data = TPS62362},
+	{.name = "tps62363", .driver_data = TPS62363},
 	{},
 };
 
@@ -448,6 +565,7 @@ static struct i2c_driver tps62360_i2c_driver = {
 	.driver = {
 		.name = "tps62360",
 		.owner = THIS_MODULE,
+		.of_match_table = of_match_ptr(tps62360_of_match),
 	},
 	.probe = tps62360_probe,
 	.remove = __devexit_p(tps62360_remove),
@@ -468,5 +586,5 @@ static void __exit tps62360_cleanup(void)
 module_exit(tps62360_cleanup);
 
 MODULE_AUTHOR("Laxman Dewangan <ldewangan@nvidia.com>");
-MODULE_DESCRIPTION("TPS62360 voltage regulator driver");
+MODULE_DESCRIPTION("TPS6236x voltage regulator driver");
 MODULE_LICENSE("GPL v2");
diff --git a/drivers/regulator/tps65023-regulator.c b/drivers/regulator/tps65023-regulator.c
index 43e4902d7af..f841bd0db6a 100644
--- a/drivers/regulator/tps65023-regulator.c
+++ b/drivers/regulator/tps65023-regulator.c
@@ -23,7 +23,6 @@
 #include <linux/regulator/driver.h>
 #include <linux/regulator/machine.h>
 #include <linux/i2c.h>
-#include <linux/delay.h>
 #include <linux/slab.h>
 #include <linux/regmap.h>
 
@@ -72,7 +71,7 @@
 
 /* LDO_CTRL bitfields */
 #define TPS65023_LDO_CTRL_LDOx_SHIFT(ldo_id)	((ldo_id)*4)
-#define TPS65023_LDO_CTRL_LDOx_MASK(ldo_id)	(0xF0 >> ((ldo_id)*4))
+#define TPS65023_LDO_CTRL_LDOx_MASK(ldo_id)	(0x0F << ((ldo_id)*4))
 
 /* Number of step-down converters available */
 #define TPS65023_NUM_DCDC		3
@@ -139,7 +138,6 @@ struct tps_info {
 /* PMIC details */
 struct tps_pmic {
 	struct regulator_desc desc[TPS65023_NUM_REGULATOR];
-	struct i2c_client *client;
 	struct regulator_dev *rdev[TPS65023_NUM_REGULATOR];
 	const struct tps_info *info[TPS65023_NUM_REGULATOR];
 	struct regmap *regmap;
@@ -152,96 +150,6 @@ struct tps_driver_data {
 	u8 core_regulator;
 };
 
-static int tps65023_dcdc_is_enabled(struct regulator_dev *dev)
-{
-	struct tps_pmic *tps = rdev_get_drvdata(dev);
-	int data, dcdc = rdev_get_id(dev);
-	int ret;
-	u8 shift;
-
-	if (dcdc < TPS65023_DCDC_1 || dcdc > TPS65023_DCDC_3)
-		return -EINVAL;
-
-	shift = TPS65023_NUM_REGULATOR - dcdc;
-	ret = regmap_read(tps->regmap, TPS65023_REG_REG_CTRL, &data);
-
-	if (ret != 0)
-		return ret;
-	else
-		return (data & 1<<shift) ? 1 : 0;
-}
-
-static int tps65023_ldo_is_enabled(struct regulator_dev *dev)
-{
-	struct tps_pmic *tps = rdev_get_drvdata(dev);
-	int data, ldo = rdev_get_id(dev);
-	int ret;
-	u8 shift;
-
-	if (ldo < TPS65023_LDO_1 || ldo > TPS65023_LDO_2)
-		return -EINVAL;
-
-	shift = (ldo == TPS65023_LDO_1 ? 1 : 2);
-	ret = regmap_read(tps->regmap, TPS65023_REG_REG_CTRL, &data);
-
-	if (ret != 0)
-		return ret;
-	else
-		return (data & 1<<shift) ? 1 : 0;
-}
-
-static int tps65023_dcdc_enable(struct regulator_dev *dev)
-{
-	struct tps_pmic *tps = rdev_get_drvdata(dev);
-	int dcdc = rdev_get_id(dev);
-	u8 shift;
-
-	if (dcdc < TPS65023_DCDC_1 || dcdc > TPS65023_DCDC_3)
-		return -EINVAL;
-
-	shift = TPS65023_NUM_REGULATOR - dcdc;
-	return regmap_update_bits(tps->regmap, TPS65023_REG_REG_CTRL, 1 << shift, 1 << shift);
-}
-
-static int tps65023_dcdc_disable(struct regulator_dev *dev)
-{
-	struct tps_pmic *tps = rdev_get_drvdata(dev);
-	int dcdc = rdev_get_id(dev);
-	u8 shift;
-
-	if (dcdc < TPS65023_DCDC_1 || dcdc > TPS65023_DCDC_3)
-		return -EINVAL;
-
-	shift = TPS65023_NUM_REGULATOR - dcdc;
-	return regmap_update_bits(tps->regmap, TPS65023_REG_REG_CTRL, 1 << shift, 0);
-}
-
-static int tps65023_ldo_enable(struct regulator_dev *dev)
-{
-	struct tps_pmic *tps = rdev_get_drvdata(dev);
-	int ldo = rdev_get_id(dev);
-	u8 shift;
-
-	if (ldo < TPS65023_LDO_1 || ldo > TPS65023_LDO_2)
-		return -EINVAL;
-
-	shift = (ldo == TPS65023_LDO_1 ? 1 : 2);
-	return regmap_update_bits(tps->regmap, TPS65023_REG_REG_CTRL, 1 << shift, 1 << shift);
-}
-
-static int tps65023_ldo_disable(struct regulator_dev *dev)
-{
-	struct tps_pmic *tps = rdev_get_drvdata(dev);
-	int ldo = rdev_get_id(dev);
-	u8 shift;
-
-	if (ldo < TPS65023_LDO_1 || ldo > TPS65023_LDO_2)
-		return -EINVAL;
-
-	shift = (ldo == TPS65023_LDO_1 ? 1 : 2);
-	return regmap_update_bits(tps->regmap, TPS65023_REG_REG_CTRL, 1 << shift, 0);
-}
-
 static int tps65023_dcdc_get_voltage(struct regulator_dev *dev)
 {
 	struct tps_pmic *tps = rdev_get_drvdata(dev);
@@ -261,50 +169,28 @@ static int tps65023_dcdc_get_voltage(struct regulator_dev *dev)
 		return tps->info[dcdc]->min_uV;
 }
 
-static int tps65023_dcdc_set_voltage(struct regulator_dev *dev,
-				     int min_uV, int max_uV,
-				     unsigned *selector)
+static int tps65023_dcdc_set_voltage_sel(struct regulator_dev *dev,
+					 unsigned selector)
 {
 	struct tps_pmic *tps = rdev_get_drvdata(dev);
 	int dcdc = rdev_get_id(dev);
-	int vsel;
 	int ret;
 
 	if (dcdc != tps->core_regulator)
 		return -EINVAL;
-	if (min_uV < tps->info[dcdc]->min_uV
-			|| min_uV > tps->info[dcdc]->max_uV)
-		return -EINVAL;
-	if (max_uV < tps->info[dcdc]->min_uV
-			|| max_uV > tps->info[dcdc]->max_uV)
-		return -EINVAL;
-
-	for (vsel = 0; vsel < tps->info[dcdc]->table_len; vsel++) {
-		int mV = tps->info[dcdc]->table[vsel];
-		int uV = mV * 1000;
-
-		/* Break at the first in-range value */
-		if (min_uV <= uV && uV <= max_uV)
-			break;
-	}
 
-	*selector = vsel;
-
-	if (vsel == tps->info[dcdc]->table_len)
-		goto failed;
-
-	ret = regmap_write(tps->regmap, TPS65023_REG_DEF_CORE, vsel);
+	ret = regmap_write(tps->regmap, TPS65023_REG_DEF_CORE, selector);
+	if (ret)
+		goto out;
 
 	/* Tell the chip that we have changed the value in DEFCORE
 	 * and its time to update the core voltage
 	 */
-	regmap_update_bits(tps->regmap, TPS65023_REG_CON_CTRL2,
-			TPS65023_REG_CTRL2_GO, TPS65023_REG_CTRL2_GO);
+	ret = regmap_update_bits(tps->regmap, TPS65023_REG_CON_CTRL2,
+				 TPS65023_REG_CTRL2_GO, TPS65023_REG_CTRL2_GO);
 
+out:
 	return ret;
-
-failed:
-	return -EINVAL;
 }
 
 static int tps65023_ldo_get_voltage(struct regulator_dev *dev)
@@ -325,42 +211,15 @@ static int tps65023_ldo_get_voltage(struct regulator_dev *dev)
 	return tps->info[ldo]->table[data] * 1000;
 }
 
-static int tps65023_ldo_set_voltage(struct regulator_dev *dev,
-				    int min_uV, int max_uV, unsigned *selector)
+static int tps65023_ldo_set_voltage_sel(struct regulator_dev *dev,
+					unsigned selector)
 {
 	struct tps_pmic *tps = rdev_get_drvdata(dev);
-	int data, vsel, ldo = rdev_get_id(dev);
-	int ret;
-
-	if (ldo < TPS65023_LDO_1 || ldo > TPS65023_LDO_2)
-		return -EINVAL;
+	int ldo_index = rdev_get_id(dev) - TPS65023_LDO_1;
 
-	if (min_uV < tps->info[ldo]->min_uV || min_uV > tps->info[ldo]->max_uV)
-		return -EINVAL;
-	if (max_uV < tps->info[ldo]->min_uV || max_uV > tps->info[ldo]->max_uV)
-		return -EINVAL;
-
-	for (vsel = 0; vsel < tps->info[ldo]->table_len; vsel++) {
-		int mV = tps->info[ldo]->table[vsel];
-		int uV = mV * 1000;
-
-		/* Break at the first in-range value */
-		if (min_uV <= uV && uV <= max_uV)
-			break;
-	}
-
-	if (vsel == tps->info[ldo]->table_len)
-		return -EINVAL;
-
-	*selector = vsel;
-
-	ret = regmap_read(tps->regmap, TPS65023_REG_LDO_CTRL, &data);
-	if (ret != 0)
-		return ret;
-
-	data &= TPS65023_LDO_CTRL_LDOx_MASK(ldo - TPS65023_LDO_1);
-	data |= (vsel << (TPS65023_LDO_CTRL_LDOx_SHIFT(ldo - TPS65023_LDO_1)));
-	return regmap_write(tps->regmap, TPS65023_REG_LDO_CTRL, data);
+	return regmap_update_bits(tps->regmap, TPS65023_REG_LDO_CTRL,
+			TPS65023_LDO_CTRL_LDOx_MASK(ldo_index),
+			selector << TPS65023_LDO_CTRL_LDOx_SHIFT(ldo_index));
 }
 
 static int tps65023_dcdc_list_voltage(struct regulator_dev *dev,
@@ -398,21 +257,21 @@ static int tps65023_ldo_list_voltage(struct regulator_dev *dev,
 
 /* Operations permitted on VDCDCx */
 static struct regulator_ops tps65023_dcdc_ops = {
-	.is_enabled = tps65023_dcdc_is_enabled,
-	.enable = tps65023_dcdc_enable,
-	.disable = tps65023_dcdc_disable,
+	.is_enabled = regulator_is_enabled_regmap,
+	.enable = regulator_enable_regmap,
+	.disable = regulator_disable_regmap,
 	.get_voltage = tps65023_dcdc_get_voltage,
-	.set_voltage = tps65023_dcdc_set_voltage,
+	.set_voltage_sel = tps65023_dcdc_set_voltage_sel,
 	.list_voltage = tps65023_dcdc_list_voltage,
 };
 
 /* Operations permitted on LDOx */
 static struct regulator_ops tps65023_ldo_ops = {
-	.is_enabled = tps65023_ldo_is_enabled,
-	.enable = tps65023_ldo_enable,
-	.disable = tps65023_ldo_disable,
+	.is_enabled = regulator_is_enabled_regmap,
+	.enable = regulator_enable_regmap,
+	.disable = regulator_disable_regmap,
 	.get_voltage = tps65023_ldo_get_voltage,
-	.set_voltage = tps65023_ldo_set_voltage,
+	.set_voltage_sel = tps65023_ldo_set_voltage_sel,
 	.list_voltage = tps65023_ldo_list_voltage,
 };
 
@@ -426,6 +285,7 @@ static int __devinit tps_65023_probe(struct i2c_client *client,
 {
 	const struct tps_driver_data *drv_data = (void *)id->driver_data;
 	const struct tps_info *info = drv_data->info;
+	struct regulator_config config = { };
 	struct regulator_init_data *init_data;
 	struct regulator_dev *rdev;
 	struct tps_pmic *tps;
@@ -443,20 +303,19 @@ static int __devinit tps_65023_probe(struct i2c_client *client,
 	if (!init_data)
 		return -EIO;
 
-	tps = kzalloc(sizeof(*tps), GFP_KERNEL);
+	tps = devm_kzalloc(&client->dev, sizeof(*tps), GFP_KERNEL);
 	if (!tps)
 		return -ENOMEM;
 
-	tps->regmap = regmap_init_i2c(client, &tps65023_regmap_config);
+	tps->regmap = devm_regmap_init_i2c(client, &tps65023_regmap_config);
 	if (IS_ERR(tps->regmap)) {
 		error = PTR_ERR(tps->regmap);
 		dev_err(&client->dev, "Failed to allocate register map: %d\n",
 			error);
-		goto fail_alloc;
+		return error;
 	}
 
 	/* common for all regulators */
-	tps->client = client;
 	tps->core_regulator = drv_data->core_regulator;
 
 	for (i = 0; i < TPS65023_NUM_REGULATOR; i++, info++, init_data++) {
@@ -471,9 +330,22 @@ static int __devinit tps_65023_probe(struct i2c_client *client,
 		tps->desc[i].type = REGULATOR_VOLTAGE;
 		tps->desc[i].owner = THIS_MODULE;
 
+		tps->desc[i].enable_reg = TPS65023_REG_REG_CTRL;
+		if (i == TPS65023_LDO_1)
+			tps->desc[i].enable_mask = 1 << 1;
+		else if (i == TPS65023_LDO_2)
+			tps->desc[i].enable_mask = 1 << 2;
+		else /* DCDCx */
+			tps->desc[i].enable_mask =
+					1 << (TPS65023_NUM_REGULATOR - i);
+
+		config.dev = &client->dev;
+		config.init_data = init_data;
+		config.driver_data = tps;
+		config.regmap = tps->regmap;
+
 		/* Register the regulators */
-		rdev = regulator_register(&tps->desc[i], &client->dev,
-					  init_data, tps, NULL);
+		rdev = regulator_register(&tps->desc[i], &config);
 		if (IS_ERR(rdev)) {
 			dev_err(&client->dev, "failed to register %s\n",
 				id->name);
@@ -496,19 +368,9 @@ static int __devinit tps_65023_probe(struct i2c_client *client,
  fail:
 	while (--i >= 0)
 		regulator_unregister(tps->rdev[i]);
-
-	regmap_exit(tps->regmap);
- fail_alloc:
-	kfree(tps);
 	return error;
 }
 
-/**
- * tps_65023_remove - TPS65023 driver i2c remove handler
- * @client: i2c driver client device structure
- *
- * Unregister TPS driver as an i2c client device driver
- */
 static int __devexit tps_65023_remove(struct i2c_client *client)
 {
 	struct tps_pmic *tps = i2c_get_clientdata(client);
@@ -516,10 +378,6 @@ static int __devexit tps_65023_remove(struct i2c_client *client)
 
 	for (i = 0; i < TPS65023_NUM_REGULATOR; i++)
 		regulator_unregister(tps->rdev[i]);
-
-	regmap_exit(tps->regmap);
-	kfree(tps);
-
 	return 0;
 }
 
@@ -638,13 +496,13 @@ static struct tps_driver_data tps65020_drv_data = {
 };
 
 static struct tps_driver_data tps65021_drv_data = {
-		.info = tps65021_regs,
-		.core_regulator = TPS65023_DCDC_3,
+	.info = tps65021_regs,
+	.core_regulator = TPS65023_DCDC_3,
 };
 
 static struct tps_driver_data tps65023_drv_data = {
-		.info = tps65023_regs,
-		.core_regulator = TPS65023_DCDC_1,
+	.info = tps65023_regs,
+	.core_regulator = TPS65023_DCDC_1,
 };
 
 static const struct i2c_device_id tps_65023_id[] = {
@@ -669,22 +527,12 @@ static struct i2c_driver tps_65023_i2c_driver = {
 	.id_table = tps_65023_id,
 };
 
-/**
- * tps_65023_init
- *
- * Module init function
- */
 static int __init tps_65023_init(void)
 {
 	return i2c_add_driver(&tps_65023_i2c_driver);
 }
 subsys_initcall(tps_65023_init);
 
-/**
- * tps_65023_cleanup
- *
- * Module exit function
- */
 static void __exit tps_65023_cleanup(void)
 {
 	i2c_del_driver(&tps_65023_i2c_driver);
diff --git a/drivers/regulator/tps6507x-regulator.c b/drivers/regulator/tps6507x-regulator.c
index 832833fe8aa..da38be1016a 100644
--- a/drivers/regulator/tps6507x-regulator.c
+++ b/drivers/regulator/tps6507x-regulator.c
@@ -23,7 +23,6 @@
 #include <linux/regulator/driver.h>
 #include <linux/regulator/machine.h>
 #include <linux/regulator/tps6507x.h>
-#include <linux/delay.h>
 #include <linux/slab.h>
 #include <linux/mfd/tps6507x.h>
 
@@ -283,7 +282,7 @@ static int tps6507x_pmic_disable(struct regulator_dev *dev)
 					1 << shift);
 }
 
-static int tps6507x_pmic_get_voltage(struct regulator_dev *dev)
+static int tps6507x_pmic_get_voltage_sel(struct regulator_dev *dev)
 {
 	struct tps6507x_pmic *tps = rdev_get_drvdata(dev);
 	int data, rid = rdev_get_id(dev);
@@ -325,7 +324,7 @@ static int tps6507x_pmic_get_voltage(struct regulator_dev *dev)
 		return data;
 
 	data &= mask;
-	return tps->info[rid]->table[data] * 1000;
+	return data;
 }
 
 static int tps6507x_pmic_set_voltage_sel(struct regulator_dev *dev,
@@ -395,7 +394,7 @@ static struct regulator_ops tps6507x_pmic_ops = {
 	.is_enabled = tps6507x_pmic_is_enabled,
 	.enable = tps6507x_pmic_enable,
 	.disable = tps6507x_pmic_disable,
-	.get_voltage = tps6507x_pmic_get_voltage,
+	.get_voltage_sel = tps6507x_pmic_get_voltage_sel,
 	.set_voltage_sel = tps6507x_pmic_set_voltage_sel,
 	.list_voltage = tps6507x_pmic_list_voltage,
 };
@@ -404,6 +403,7 @@ static __devinit int tps6507x_pmic_probe(struct platform_device *pdev)
 {
 	struct tps6507x_dev *tps6507x_dev = dev_get_drvdata(pdev->dev.parent);
 	struct tps_info *info = &tps6507x_pmic_regs[0];
+	struct regulator_config config = { };
 	struct regulator_init_data *init_data;
 	struct regulator_dev *rdev;
 	struct tps6507x_pmic *tps;
@@ -428,7 +428,7 @@ static __devinit int tps6507x_pmic_probe(struct platform_device *pdev)
 	if (!init_data)
 		return -EINVAL;
 
-	tps = kzalloc(sizeof(*tps), GFP_KERNEL);
+	tps = devm_kzalloc(&pdev->dev, sizeof(*tps), GFP_KERNEL);
 	if (!tps)
 		return -ENOMEM;
 
@@ -453,8 +453,11 @@ static __devinit int tps6507x_pmic_probe(struct platform_device *pdev)
 		tps->desc[i].type = REGULATOR_VOLTAGE;
 		tps->desc[i].owner = THIS_MODULE;
 
-		rdev = regulator_register(&tps->desc[i],
-					tps6507x_dev->dev, init_data, tps, NULL);
+		config.dev = tps6507x_dev->dev;
+		config.init_data = init_data;
+		config.driver_data = tps;
+
+		rdev = regulator_register(&tps->desc[i], &config);
 		if (IS_ERR(rdev)) {
 			dev_err(tps6507x_dev->dev,
 				"failed to register %s regulator\n",
@@ -475,8 +478,6 @@ static __devinit int tps6507x_pmic_probe(struct platform_device *pdev)
 fail:
 	while (--i >= 0)
 		regulator_unregister(tps->rdev[i]);
-
-	kfree(tps);
 	return error;
 }
 
@@ -488,9 +489,6 @@ static int __devexit tps6507x_pmic_remove(struct platform_device *pdev)
 
 	for (i = 0; i < TPS6507X_NUM_REGULATOR; i++)
 		regulator_unregister(tps->rdev[i]);
-
-	kfree(tps);
-
 	return 0;
 }
 
diff --git a/drivers/regulator/tps65090-regulator.c b/drivers/regulator/tps65090-regulator.c
new file mode 100644
index 00000000000..001ad554ac6
--- /dev/null
+++ b/drivers/regulator/tps65090-regulator.c
@@ -0,0 +1,150 @@
+/*
+ * Regulator driver for tps65090 power management chip.
+ *
+ * Copyright (c) 2012, 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/module.h>
+#include <linux/init.h>
+#include <linux/slab.h>
+#include <linux/err.h>
+#include <linux/platform_device.h>
+#include <linux/regulator/driver.h>
+#include <linux/regulator/machine.h>
+#include <linux/mfd/tps65090.h>
+#include <linux/regulator/tps65090-regulator.h>
+
+struct tps65090_regulator {
+	int		id;
+	/* used by regulator core */
+	struct regulator_desc	desc;
+
+	/* Device */
+	struct device		*dev;
+};
+
+static struct regulator_ops tps65090_ops = {
+	.enable = regulator_enable_regmap,
+	.disable = regulator_disable_regmap,
+	.is_enabled = regulator_is_enabled_regmap,
+};
+
+#define tps65090_REG(_id)				\
+{							\
+	.id		= TPS65090_ID_##_id,		\
+	.desc = {					\
+		.name = tps65090_rails(_id),		\
+		.id = TPS65090_ID_##_id,		\
+		.ops = &tps65090_ops,			\
+		.type = REGULATOR_VOLTAGE,		\
+		.owner = THIS_MODULE,			\
+		.enable_reg = (TPS65090_ID_##_id) + 12,	\
+		.enable_mask = BIT(0),			\
+	},						\
+}
+
+static struct tps65090_regulator TPS65090_regulator[] = {
+	tps65090_REG(DCDC1),
+	tps65090_REG(DCDC2),
+	tps65090_REG(DCDC3),
+	tps65090_REG(FET1),
+	tps65090_REG(FET2),
+	tps65090_REG(FET3),
+	tps65090_REG(FET4),
+	tps65090_REG(FET5),
+	tps65090_REG(FET6),
+	tps65090_REG(FET7),
+};
+
+static inline struct tps65090_regulator *find_regulator_info(int id)
+{
+	struct tps65090_regulator *ri;
+	int i;
+
+	for (i = 0; i < ARRAY_SIZE(TPS65090_regulator); i++) {
+		ri = &TPS65090_regulator[i];
+		if (ri->desc.id == id)
+			return ri;
+	}
+	return NULL;
+}
+
+static int __devinit tps65090_regulator_probe(struct platform_device *pdev)
+{
+	struct tps65090 *tps65090_mfd = dev_get_drvdata(pdev->dev.parent);
+	struct tps65090_regulator *ri = NULL;
+	struct regulator_config config = { };
+	struct regulator_dev *rdev;
+	struct tps65090_regulator_platform_data *tps_pdata;
+	int id = pdev->id;
+
+	dev_dbg(&pdev->dev, "Probing regulator %d\n", id);
+
+	ri = find_regulator_info(id);
+	if (ri == NULL) {
+		dev_err(&pdev->dev, "invalid regulator ID specified\n");
+		return -EINVAL;
+	}
+	tps_pdata = pdev->dev.platform_data;
+	ri->dev = &pdev->dev;
+
+	config.dev = &pdev->dev;
+	config.init_data = &tps_pdata->regulator;
+	config.driver_data = ri;
+	config.regmap = tps65090_mfd->rmap;
+
+	rdev = regulator_register(&ri->desc, &config);
+	if (IS_ERR(rdev)) {
+		dev_err(&pdev->dev, "failed to register regulator %s\n",
+				ri->desc.name);
+		return PTR_ERR(rdev);
+	}
+
+	platform_set_drvdata(pdev, rdev);
+	return 0;
+}
+
+static int __devexit tps65090_regulator_remove(struct platform_device *pdev)
+{
+	struct regulator_dev *rdev = platform_get_drvdata(pdev);
+
+	regulator_unregister(rdev);
+	return 0;
+}
+
+static struct platform_driver tps65090_regulator_driver = {
+	.driver	= {
+		.name	= "tps65090-regulator",
+		.owner	= THIS_MODULE,
+	},
+	.probe		= tps65090_regulator_probe,
+	.remove		= __devexit_p(tps65090_regulator_remove),
+};
+
+static int __init tps65090_regulator_init(void)
+{
+	return platform_driver_register(&tps65090_regulator_driver);
+}
+subsys_initcall(tps65090_regulator_init);
+
+static void __exit tps65090_regulator_exit(void)
+{
+	platform_driver_unregister(&tps65090_regulator_driver);
+}
+module_exit(tps65090_regulator_exit);
+
+MODULE_DESCRIPTION("tps65090 regulator driver");
+MODULE_AUTHOR("Venu Byravarasu <vbyravarasu@nvidia.com>");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/regulator/tps65217-regulator.c b/drivers/regulator/tps65217-regulator.c
index e39521b4277..9d371d2cbca 100644
--- a/drivers/regulator/tps65217-regulator.c
+++ b/drivers/regulator/tps65217-regulator.c
@@ -213,65 +213,56 @@ static int tps65217_pmic_get_voltage_sel(struct regulator_dev *dev)
 	return selector;
 }
 
-static int tps65217_pmic_ldo1_set_voltage_sel(struct regulator_dev *dev,
-						unsigned selector)
+static int tps65217_pmic_set_voltage_sel(struct regulator_dev *dev,
+					 unsigned selector)
 {
+	int ret;
 	struct tps65217 *tps = rdev_get_drvdata(dev);
-	int ldo = rdev_get_id(dev);
+	unsigned int rid = rdev_get_id(dev);
 
-	if (ldo != TPS65217_LDO_1)
-		return -EINVAL;
+	/* Set the voltage based on vsel value and write protect level is 2 */
+	ret = tps65217_set_bits(tps, tps->info[rid]->set_vout_reg,
+				tps->info[rid]->set_vout_mask,
+				selector, TPS65217_PROTECT_L2);
 
-	if (selector >= tps->info[ldo]->table_len)
-		return -EINVAL;
+	/* Set GO bit for DCDCx to initiate voltage transistion */
+	switch (rid) {
+	case TPS65217_DCDC_1 ... TPS65217_DCDC_3:
+		ret = tps65217_set_bits(tps, TPS65217_REG_DEFSLEW,
+				       TPS65217_DEFSLEW_GO, TPS65217_DEFSLEW_GO,
+				       TPS65217_PROTECT_L2);
+		break;
+	}
 
-	/* Set the voltage based on vsel value and write protect level is 2 */
-	return tps65217_set_bits(tps, tps->info[ldo]->set_vout_reg,
-					tps->info[ldo]->set_vout_mask,
-					selector, TPS65217_PROTECT_L2);
+	return ret;
 }
 
-static int tps65217_pmic_set_voltage(struct regulator_dev *dev,
-				  int min_uV, int max_uV, unsigned *selector)
+static int tps65217_pmic_map_voltage(struct regulator_dev *dev,
+				     int min_uV, int max_uV)
 {
-	int ret;
+
 	struct tps65217 *tps = rdev_get_drvdata(dev);
-	unsigned int rid = rdev_get_id(dev);
+	unsigned int sel, rid = rdev_get_id(dev);
+	int ret;
 
-	/* LDO1 implements set_voltage_sel callback */
+	/* LDO1 uses regulator_map_voltage_iterate() */
 	if (rid == TPS65217_LDO_1)
 		return -EINVAL;
 
 	if (rid < TPS65217_DCDC_1 || rid > TPS65217_LDO_4)
 		return -EINVAL;
 
-	if (min_uV < tps->info[rid]->min_uV
-		|| min_uV > tps->info[rid]->max_uV)
+	if (min_uV < tps->info[rid]->min_uV || min_uV > tps->info[rid]->max_uV)
 		return -EINVAL;
 
-	if (max_uV < tps->info[rid]->min_uV
-		|| max_uV > tps->info[rid]->max_uV)
+	if (max_uV < tps->info[rid]->min_uV || max_uV > tps->info[rid]->max_uV)
 		return -EINVAL;
 
-	ret = tps->info[rid]->uv_to_vsel(min_uV, selector);
+	ret = tps->info[rid]->uv_to_vsel(min_uV, &sel);
 	if (ret)
 		return ret;
 
-	/* Set the voltage based on vsel value and write protect level is 2 */
-	ret = tps65217_set_bits(tps, tps->info[rid]->set_vout_reg,
-				tps->info[rid]->set_vout_mask,
-				*selector, TPS65217_PROTECT_L2);
-
-	/* Set GO bit for DCDCx to initiate voltage transistion */
-	switch (rid) {
-	case TPS65217_DCDC_1 ... TPS65217_DCDC_3:
-		ret = tps65217_set_bits(tps, TPS65217_REG_DEFSLEW,
-				       TPS65217_DEFSLEW_GO, TPS65217_DEFSLEW_GO,
-				       TPS65217_PROTECT_L2);
-		break;
-	}
-
-	return ret;
+	return sel;
 }
 
 static int tps65217_pmic_list_voltage(struct regulator_dev *dev,
@@ -298,8 +289,9 @@ static struct regulator_ops tps65217_pmic_ops = {
 	.enable			= tps65217_pmic_enable,
 	.disable		= tps65217_pmic_disable,
 	.get_voltage_sel	= tps65217_pmic_get_voltage_sel,
-	.set_voltage		= tps65217_pmic_set_voltage,
+	.set_voltage_sel	= tps65217_pmic_set_voltage_sel,
 	.list_voltage		= tps65217_pmic_list_voltage,
+	.map_voltage		= tps65217_pmic_map_voltage,
 };
 
 /* Operations permitted on LDO1 */
@@ -308,11 +300,11 @@ static struct regulator_ops tps65217_pmic_ldo1_ops = {
 	.enable			= tps65217_pmic_enable,
 	.disable		= tps65217_pmic_disable,
 	.get_voltage_sel	= tps65217_pmic_get_voltage_sel,
-	.set_voltage_sel	= tps65217_pmic_ldo1_set_voltage_sel,
+	.set_voltage_sel	= tps65217_pmic_set_voltage_sel,
 	.list_voltage		= tps65217_pmic_list_voltage,
 };
 
-static struct regulator_desc regulators[] = {
+static const struct regulator_desc regulators[] = {
 	TPS65217_REGULATOR("DCDC1", TPS65217_DCDC_1, tps65217_pmic_ops, 64),
 	TPS65217_REGULATOR("DCDC2", TPS65217_DCDC_2, tps65217_pmic_ops, 64),
 	TPS65217_REGULATOR("DCDC3", TPS65217_DCDC_3, tps65217_pmic_ops, 64),
@@ -327,13 +319,17 @@ static int __devinit tps65217_regulator_probe(struct platform_device *pdev)
 	struct regulator_dev *rdev;
 	struct tps65217 *tps;
 	struct tps_info *info = &tps65217_pmic_regs[pdev->id];
+	struct regulator_config config = { };
 
 	/* Already set by core driver */
 	tps = dev_to_tps65217(pdev->dev.parent);
 	tps->info[pdev->id] = info;
 
-	rdev = regulator_register(&regulators[pdev->id], &pdev->dev,
-				  pdev->dev.platform_data, tps, NULL);
+	config.dev = &pdev->dev;
+	config.init_data = pdev->dev.platform_data;
+	config.driver_data = tps;
+
+	rdev = regulator_register(&regulators[pdev->id], &config);
 	if (IS_ERR(rdev))
 		return PTR_ERR(rdev);
 
diff --git a/drivers/regulator/tps6524x-regulator.c b/drivers/regulator/tps6524x-regulator.c
index 4a421be6d4f..b88b3df8238 100644
--- a/drivers/regulator/tps6524x-regulator.c
+++ b/drivers/regulator/tps6524x-regulator.c
@@ -458,12 +458,10 @@ static int list_voltage(struct regulator_dev *rdev, unsigned selector)
 		info->voltages[selector] : -EINVAL);
 }
 
-static int set_voltage(struct regulator_dev *rdev, int min_uV, int max_uV,
-		       unsigned *selector)
+static int set_voltage_sel(struct regulator_dev *rdev, unsigned selector)
 {
 	const struct supply_info *info;
 	struct tps6524x *hw;
-	unsigned i;
 
 	hw	= rdev_get_drvdata(rdev);
 	info	= &supply_info[rdev_get_id(rdev)];
@@ -471,20 +469,10 @@ static int set_voltage(struct regulator_dev *rdev, int min_uV, int max_uV,
 	if (info->flags & FIXED_VOLTAGE)
 		return -EINVAL;
 
-	for (i = 0; i < info->n_voltages; i++)
-		if (min_uV <= info->voltages[i] &&
-		    max_uV >= info->voltages[i])
-			break;
-
-	if (i >= info->n_voltages)
-		i = info->n_voltages - 1;
-
-	*selector = i;
-
-	return write_field(hw, &info->voltage, i);
+	return write_field(hw, &info->voltage, selector);
 }
 
-static int get_voltage(struct regulator_dev *rdev)
+static int get_voltage_sel(struct regulator_dev *rdev)
 {
 	const struct supply_info *info;
 	struct tps6524x *hw;
@@ -502,7 +490,7 @@ static int get_voltage(struct regulator_dev *rdev)
 	if (WARN_ON(ret >= info->n_voltages))
 		return -EIO;
 
-	return info->voltages[ret];
+	return ret;
 }
 
 static int set_current_limit(struct regulator_dev *rdev, int min_uA,
@@ -587,8 +575,8 @@ static struct regulator_ops regulator_ops = {
 	.is_enabled		= is_supply_enabled,
 	.enable			= enable_supply,
 	.disable		= disable_supply,
-	.get_voltage		= get_voltage,
-	.set_voltage		= set_voltage,
+	.get_voltage_sel	= get_voltage_sel,
+	.set_voltage_sel	= set_voltage_sel,
 	.list_voltage		= list_voltage,
 	.set_current_limit	= set_current_limit,
 	.get_current_limit	= get_current_limit,
@@ -607,7 +595,6 @@ static int pmic_remove(struct spi_device *spi)
 		hw->rdev[i] = NULL;
 	}
 	spi_set_drvdata(spi, NULL);
-	kfree(hw);
 	return 0;
 }
 
@@ -617,6 +604,7 @@ static int __devinit pmic_probe(struct spi_device *spi)
 	struct device *dev = &spi->dev;
 	const struct supply_info *info = supply_info;
 	struct regulator_init_data *init_data;
+	struct regulator_config config = { };
 	int ret = 0, i;
 
 	init_data = dev->platform_data;
@@ -625,7 +613,7 @@ static int __devinit pmic_probe(struct spi_device *spi)
 		return -EINVAL;
 	}
 
-	hw = kzalloc(sizeof(struct tps6524x), GFP_KERNEL);
+	hw = devm_kzalloc(&spi->dev, sizeof(struct tps6524x), GFP_KERNEL);
 	if (!hw) {
 		dev_err(dev, "cannot allocate regulator private data\n");
 		return -ENOMEM;
@@ -648,8 +636,11 @@ static int __devinit pmic_probe(struct spi_device *spi)
 		if (info->flags & FIXED_VOLTAGE)
 			hw->desc[i].n_voltages = 1;
 
-		hw->rdev[i] = regulator_register(&hw->desc[i], dev,
-						 init_data, hw, NULL);
+		config.dev = dev;
+		config.init_data = init_data;
+		config.driver_data = hw;
+
+		hw->rdev[i] = regulator_register(&hw->desc[i], &config);
 		if (IS_ERR(hw->rdev[i])) {
 			ret = PTR_ERR(hw->rdev[i]);
 			hw->rdev[i] = NULL;
@@ -673,17 +664,7 @@ static struct spi_driver pmic_driver = {
 	},
 };
 
-static int __init pmic_driver_init(void)
-{
-	return spi_register_driver(&pmic_driver);
-}
-module_init(pmic_driver_init);
-
-static void __exit pmic_driver_exit(void)
-{
-	spi_unregister_driver(&pmic_driver);
-}
-module_exit(pmic_driver_exit);
+module_spi_driver(pmic_driver);
 
 MODULE_DESCRIPTION("TPS6524X PMIC Driver");
 MODULE_AUTHOR("Cyril Chemparathy");
diff --git a/drivers/regulator/tps6586x-regulator.c b/drivers/regulator/tps6586x-regulator.c
index cfc1f16f777..c0a21457538 100644
--- a/drivers/regulator/tps6586x-regulator.c
+++ b/drivers/regulator/tps6586x-regulator.c
@@ -75,8 +75,7 @@ static inline struct device *to_tps6586x_dev(struct regulator_dev *rdev)
 	return rdev_get_dev(rdev)->parent->parent;
 }
 
-static int tps6586x_ldo_list_voltage(struct regulator_dev *rdev,
-				     unsigned selector)
+static int tps6586x_list_voltage(struct regulator_dev *rdev, unsigned selector)
 {
 	struct tps6586x_regulator *info = rdev_get_drvdata(rdev);
 	int rid = rdev_get_id(rdev);
@@ -89,47 +88,34 @@ static int tps6586x_ldo_list_voltage(struct regulator_dev *rdev,
 }
 
 
-static int __tps6586x_ldo_set_voltage(struct device *parent,
-				      struct tps6586x_regulator *ri,
-				      int min_uV, int max_uV,
-				      unsigned *selector)
+static int tps6586x_set_voltage_sel(struct regulator_dev *rdev,
+				    unsigned selector)
 {
-	int val, uV;
+	struct tps6586x_regulator *ri = rdev_get_drvdata(rdev);
+	struct device *parent = to_tps6586x_dev(rdev);
+	int ret, val, rid = rdev_get_id(rdev);
 	uint8_t mask;
 
-	for (val = 0; val < ri->desc.n_voltages; val++) {
-		uV = ri->voltages[val] * 1000;
-
-		/* LDO0 has minimal voltage 1.2 rather than 1.25 */
-		if (ri->desc.id == TPS6586X_ID_LDO_0 && val == 0)
-			uV -= 50 * 1000;
-
-		/* use the first in-range value */
-		if (min_uV <= uV && uV <= max_uV) {
-
-			*selector = val;
+	val = selector << ri->volt_shift;
+	mask = ((1 << ri->volt_nbits) - 1) << ri->volt_shift;
 
-			val <<= ri->volt_shift;
-			mask = ((1 << ri->volt_nbits) - 1) << ri->volt_shift;
+	ret = tps6586x_update(parent, ri->volt_reg, val, mask);
+	if (ret)
+		return ret;
 
-			return tps6586x_update(parent, ri->volt_reg, val, mask);
-		}
+	/* Update go bit for DVM regulators */
+	switch (rid) {
+	case TPS6586X_ID_LDO_2:
+	case TPS6586X_ID_LDO_4:
+	case TPS6586X_ID_SM_0:
+	case TPS6586X_ID_SM_1:
+		ret = tps6586x_set_bits(parent, ri->go_reg, 1 << ri->go_bit);
+		break;
 	}
-
-	return -EINVAL;
-}
-
-static int tps6586x_ldo_set_voltage(struct regulator_dev *rdev,
-				    int min_uV, int max_uV, unsigned *selector)
-{
-	struct tps6586x_regulator *ri = rdev_get_drvdata(rdev);
-	struct device *parent = to_tps6586x_dev(rdev);
-
-	return __tps6586x_ldo_set_voltage(parent, ri, min_uV, max_uV,
-					  selector);
+	return ret;
 }
 
-static int tps6586x_ldo_get_voltage(struct regulator_dev *rdev)
+static int tps6586x_get_voltage_sel(struct regulator_dev *rdev)
 {
 	struct tps6586x_regulator *ri = rdev_get_drvdata(rdev);
 	struct device *parent = to_tps6586x_dev(rdev);
@@ -146,22 +132,7 @@ static int tps6586x_ldo_get_voltage(struct regulator_dev *rdev)
 	if (val >= ri->desc.n_voltages)
 		BUG();
 
-	return ri->voltages[val] * 1000;
-}
-
-static int tps6586x_dvm_set_voltage(struct regulator_dev *rdev,
-				    int min_uV, int max_uV, unsigned *selector)
-{
-	struct tps6586x_regulator *ri = rdev_get_drvdata(rdev);
-	struct device *parent = to_tps6586x_dev(rdev);
-	int ret;
-
-	ret = __tps6586x_ldo_set_voltage(parent, ri, min_uV, max_uV,
-					 selector);
-	if (ret)
-		return ret;
-
-	return tps6586x_set_bits(parent, ri->go_reg, 1 << ri->go_bit);
+	return val;
 }
 
 static int tps6586x_regulator_enable(struct regulator_dev *rdev)
@@ -196,20 +167,10 @@ static int tps6586x_regulator_is_enabled(struct regulator_dev *rdev)
 	return !!(reg_val & (1 << ri->enable_bit[0]));
 }
 
-static struct regulator_ops tps6586x_regulator_ldo_ops = {
-	.list_voltage = tps6586x_ldo_list_voltage,
-	.get_voltage = tps6586x_ldo_get_voltage,
-	.set_voltage = tps6586x_ldo_set_voltage,
-
-	.is_enabled = tps6586x_regulator_is_enabled,
-	.enable = tps6586x_regulator_enable,
-	.disable = tps6586x_regulator_disable,
-};
-
-static struct regulator_ops tps6586x_regulator_dvm_ops = {
-	.list_voltage = tps6586x_ldo_list_voltage,
-	.get_voltage = tps6586x_ldo_get_voltage,
-	.set_voltage = tps6586x_dvm_set_voltage,
+static struct regulator_ops tps6586x_regulator_ops = {
+	.list_voltage = tps6586x_list_voltage,
+	.get_voltage_sel = tps6586x_get_voltage_sel,
+	.set_voltage_sel = tps6586x_set_voltage_sel,
 
 	.is_enabled = tps6586x_regulator_is_enabled,
 	.enable = tps6586x_regulator_enable,
@@ -241,11 +202,11 @@ static int tps6586x_dvm_voltages[] = {
 	1325, 1350, 1375, 1400, 1425, 1450, 1475, 1500,
 };
 
-#define TPS6586X_REGULATOR(_id, vdata, _ops, vreg, shift, nbits,	\
+#define TPS6586X_REGULATOR(_id, vdata, vreg, shift, nbits,		\
 			   ereg0, ebit0, ereg1, ebit1)			\
 	.desc	= {							\
 		.name	= "REG-" #_id,					\
-		.ops	= &tps6586x_regulator_##_ops,			\
+		.ops	= &tps6586x_regulator_ops,			\
 		.type	= REGULATOR_VOLTAGE,				\
 		.id	= TPS6586X_ID_##_id,				\
 		.n_voltages = ARRAY_SIZE(tps6586x_##vdata##_voltages),	\
@@ -267,14 +228,14 @@ static int tps6586x_dvm_voltages[] = {
 #define TPS6586X_LDO(_id, vdata, vreg, shift, nbits,			\
 		     ereg0, ebit0, ereg1, ebit1)			\
 {									\
-	TPS6586X_REGULATOR(_id, vdata, ldo_ops, vreg, shift, nbits,	\
+	TPS6586X_REGULATOR(_id, vdata, vreg, shift, nbits,		\
 			   ereg0, ebit0, ereg1, ebit1)			\
 }
 
 #define TPS6586X_DVM(_id, vdata, vreg, shift, nbits,			\
 		     ereg0, ebit0, ereg1, ebit1, goreg, gobit)		\
 {									\
-	TPS6586X_REGULATOR(_id, vdata, dvm_ops, vreg, shift, nbits,	\
+	TPS6586X_REGULATOR(_id, vdata, vreg, shift, nbits,		\
 			   ereg0, ebit0, ereg1, ebit1)			\
 	TPS6586X_REGULATOR_DVM_GOREG(goreg, gobit)			\
 }
@@ -384,6 +345,7 @@ static inline struct tps6586x_regulator *find_regulator_info(int id)
 static int __devinit tps6586x_regulator_probe(struct platform_device *pdev)
 {
 	struct tps6586x_regulator *ri = NULL;
+	struct regulator_config config = { };
 	struct regulator_dev *rdev;
 	int id = pdev->id;
 	int err;
@@ -400,8 +362,12 @@ static int __devinit tps6586x_regulator_probe(struct platform_device *pdev)
 	if (err)
 		return err;
 
-	rdev = regulator_register(&ri->desc, &pdev->dev,
-				  pdev->dev.platform_data, ri, NULL);
+	config.dev = &pdev->dev;
+	config.of_node = pdev->dev.of_node;
+	config.init_data = pdev->dev.platform_data;
+	config.driver_data = ri;
+
+	rdev = regulator_register(&ri->desc, &config);
 	if (IS_ERR(rdev)) {
 		dev_err(&pdev->dev, "failed to register regulator %s\n",
 				ri->desc.name);
diff --git a/drivers/regulator/tps65910-regulator.c b/drivers/regulator/tps65910-regulator.c
index 4a37c2b6367..6bf864b4bdf 100644
--- a/drivers/regulator/tps65910-regulator.c
+++ b/drivers/regulator/tps65910-regulator.c
@@ -20,10 +20,10 @@
 #include <linux/platform_device.h>
 #include <linux/regulator/driver.h>
 #include <linux/regulator/machine.h>
-#include <linux/delay.h>
 #include <linux/slab.h>
 #include <linux/gpio.h>
 #include <linux/mfd/tps65910.h>
+#include <linux/regulator/of_regulator.h>
 
 #define TPS65910_SUPPLY_STATE_ENABLED	0x1
 #define EXT_SLEEP_CONTROL (TPS65910_SLEEP_CONTROL_EXT_INPUT_EN1 |	\
@@ -31,54 +31,54 @@
 			TPS65910_SLEEP_CONTROL_EXT_INPUT_EN3 |		\
 			TPS65911_SLEEP_CONTROL_EXT_INPUT_SLEEP)
 
-/* supported VIO voltages in milivolts */
+/* supported VIO voltages in millivolts */
 static const u16 VIO_VSEL_table[] = {
 	1500, 1800, 2500, 3300,
 };
 
 /* VSEL tables for TPS65910 specific LDOs and dcdc's */
 
-/* supported VDD3 voltages in milivolts */
+/* supported VDD3 voltages in millivolts */
 static const u16 VDD3_VSEL_table[] = {
 	5000,
 };
 
-/* supported VDIG1 voltages in milivolts */
+/* supported VDIG1 voltages in millivolts */
 static const u16 VDIG1_VSEL_table[] = {
 	1200, 1500, 1800, 2700,
 };
 
-/* supported VDIG2 voltages in milivolts */
+/* supported VDIG2 voltages in millivolts */
 static const u16 VDIG2_VSEL_table[] = {
 	1000, 1100, 1200, 1800,
 };
 
-/* supported VPLL voltages in milivolts */
+/* supported VPLL voltages in millivolts */
 static const u16 VPLL_VSEL_table[] = {
 	1000, 1100, 1800, 2500,
 };
 
-/* supported VDAC voltages in milivolts */
+/* supported VDAC voltages in millivolts */
 static const u16 VDAC_VSEL_table[] = {
 	1800, 2600, 2800, 2850,
 };
 
-/* supported VAUX1 voltages in milivolts */
+/* supported VAUX1 voltages in millivolts */
 static const u16 VAUX1_VSEL_table[] = {
 	1800, 2500, 2800, 2850,
 };
 
-/* supported VAUX2 voltages in milivolts */
+/* supported VAUX2 voltages in millivolts */
 static const u16 VAUX2_VSEL_table[] = {
 	1800, 2800, 2900, 3300,
 };
 
-/* supported VAUX33 voltages in milivolts */
+/* supported VAUX33 voltages in millivolts */
 static const u16 VAUX33_VSEL_table[] = {
 	1800, 2000, 2800, 3300,
 };
 
-/* supported VMMC voltages in milivolts */
+/* supported VMMC voltages in millivolts */
 static const u16 VMMC_VSEL_table[] = {
 	1800, 2800, 3000, 3300,
 };
@@ -94,11 +94,11 @@ struct tps_info {
 
 static struct tps_info tps65910_regs[] = {
 	{
-		.name = "VRTC",
+		.name = "vrtc",
 		.enable_time_us = 2200,
 	},
 	{
-		.name = "VIO",
+		.name = "vio",
 		.min_uV = 1500000,
 		.max_uV = 3300000,
 		.n_voltages = ARRAY_SIZE(VIO_VSEL_table),
@@ -106,19 +106,19 @@ static struct tps_info tps65910_regs[] = {
 		.enable_time_us = 350,
 	},
 	{
-		.name = "VDD1",
+		.name = "vdd1",
 		.min_uV = 600000,
 		.max_uV = 4500000,
 		.enable_time_us = 350,
 	},
 	{
-		.name = "VDD2",
+		.name = "vdd2",
 		.min_uV = 600000,
 		.max_uV = 4500000,
 		.enable_time_us = 350,
 	},
 	{
-		.name = "VDD3",
+		.name = "vdd3",
 		.min_uV = 5000000,
 		.max_uV = 5000000,
 		.n_voltages = ARRAY_SIZE(VDD3_VSEL_table),
@@ -126,7 +126,7 @@ static struct tps_info tps65910_regs[] = {
 		.enable_time_us = 200,
 	},
 	{
-		.name = "VDIG1",
+		.name = "vdig1",
 		.min_uV = 1200000,
 		.max_uV = 2700000,
 		.n_voltages = ARRAY_SIZE(VDIG1_VSEL_table),
@@ -134,7 +134,7 @@ static struct tps_info tps65910_regs[] = {
 		.enable_time_us = 100,
 	},
 	{
-		.name = "VDIG2",
+		.name = "vdig2",
 		.min_uV = 1000000,
 		.max_uV = 1800000,
 		.n_voltages = ARRAY_SIZE(VDIG2_VSEL_table),
@@ -142,7 +142,7 @@ static struct tps_info tps65910_regs[] = {
 		.enable_time_us = 100,
 	},
 	{
-		.name = "VPLL",
+		.name = "vpll",
 		.min_uV = 1000000,
 		.max_uV = 2500000,
 		.n_voltages = ARRAY_SIZE(VPLL_VSEL_table),
@@ -150,7 +150,7 @@ static struct tps_info tps65910_regs[] = {
 		.enable_time_us = 100,
 	},
 	{
-		.name = "VDAC",
+		.name = "vdac",
 		.min_uV = 1800000,
 		.max_uV = 2850000,
 		.n_voltages = ARRAY_SIZE(VDAC_VSEL_table),
@@ -158,7 +158,7 @@ static struct tps_info tps65910_regs[] = {
 		.enable_time_us = 100,
 	},
 	{
-		.name = "VAUX1",
+		.name = "vaux1",
 		.min_uV = 1800000,
 		.max_uV = 2850000,
 		.n_voltages = ARRAY_SIZE(VAUX1_VSEL_table),
@@ -166,7 +166,7 @@ static struct tps_info tps65910_regs[] = {
 		.enable_time_us = 100,
 	},
 	{
-		.name = "VAUX2",
+		.name = "vaux2",
 		.min_uV = 1800000,
 		.max_uV = 3300000,
 		.n_voltages = ARRAY_SIZE(VAUX2_VSEL_table),
@@ -174,7 +174,7 @@ static struct tps_info tps65910_regs[] = {
 		.enable_time_us = 100,
 	},
 	{
-		.name = "VAUX33",
+		.name = "vaux33",
 		.min_uV = 1800000,
 		.max_uV = 3300000,
 		.n_voltages = ARRAY_SIZE(VAUX33_VSEL_table),
@@ -182,7 +182,7 @@ static struct tps_info tps65910_regs[] = {
 		.enable_time_us = 100,
 	},
 	{
-		.name = "VMMC",
+		.name = "vmmc",
 		.min_uV = 1800000,
 		.max_uV = 3300000,
 		.n_voltages = ARRAY_SIZE(VMMC_VSEL_table),
@@ -193,11 +193,11 @@ static struct tps_info tps65910_regs[] = {
 
 static struct tps_info tps65911_regs[] = {
 	{
-		.name = "VRTC",
+		.name = "vrtc",
 		.enable_time_us = 2200,
 	},
 	{
-		.name = "VIO",
+		.name = "vio",
 		.min_uV = 1500000,
 		.max_uV = 3300000,
 		.n_voltages = ARRAY_SIZE(VIO_VSEL_table),
@@ -205,77 +205,77 @@ static struct tps_info tps65911_regs[] = {
 		.enable_time_us = 350,
 	},
 	{
-		.name = "VDD1",
+		.name = "vdd1",
 		.min_uV = 600000,
 		.max_uV = 4500000,
 		.n_voltages = 73,
 		.enable_time_us = 350,
 	},
 	{
-		.name = "VDD2",
+		.name = "vdd2",
 		.min_uV = 600000,
 		.max_uV = 4500000,
 		.n_voltages = 73,
 		.enable_time_us = 350,
 	},
 	{
-		.name = "VDDCTRL",
+		.name = "vddctrl",
 		.min_uV = 600000,
 		.max_uV = 1400000,
 		.n_voltages = 65,
 		.enable_time_us = 900,
 	},
 	{
-		.name = "LDO1",
+		.name = "ldo1",
 		.min_uV = 1000000,
 		.max_uV = 3300000,
 		.n_voltages = 47,
 		.enable_time_us = 420,
 	},
 	{
-		.name = "LDO2",
+		.name = "ldo2",
 		.min_uV = 1000000,
 		.max_uV = 3300000,
 		.n_voltages = 47,
 		.enable_time_us = 420,
 	},
 	{
-		.name = "LDO3",
+		.name = "ldo3",
 		.min_uV = 1000000,
 		.max_uV = 3300000,
 		.n_voltages = 24,
 		.enable_time_us = 230,
 	},
 	{
-		.name = "LDO4",
+		.name = "ldo4",
 		.min_uV = 1000000,
 		.max_uV = 3300000,
 		.n_voltages = 47,
 		.enable_time_us = 230,
 	},
 	{
-		.name = "LDO5",
+		.name = "ldo5",
 		.min_uV = 1000000,
 		.max_uV = 3300000,
 		.n_voltages = 24,
 		.enable_time_us = 230,
 	},
 	{
-		.name = "LDO6",
+		.name = "ldo6",
 		.min_uV = 1000000,
 		.max_uV = 3300000,
 		.n_voltages = 24,
 		.enable_time_us = 230,
 	},
 	{
-		.name = "LDO7",
+		.name = "ldo7",
 		.min_uV = 1000000,
 		.max_uV = 3300000,
 		.n_voltages = 24,
 		.enable_time_us = 230,
 	},
 	{
-		.name = "LDO8",
+		.name = "ldo8",
 		.min_uV = 1000000,
 		.max_uV = 3300000,
 		.n_voltages = 24,
@@ -331,21 +331,16 @@ struct tps65910_reg {
 
 static inline int tps65910_read(struct tps65910_reg *pmic, u8 reg)
 {
-	u8 val;
+	unsigned int val;
 	int err;
 
-	err = pmic->mfd->read(pmic->mfd, reg, 1, &val);
+	err = tps65910_reg_read(pmic->mfd, reg, &val);
 	if (err)
 		return err;
 
 	return val;
 }
 
-static inline int tps65910_write(struct tps65910_reg *pmic, u8 reg, u8 val)
-{
-	return pmic->mfd->write(pmic->mfd, reg, 1, &val);
-}
-
 static int tps65910_modify_bits(struct tps65910_reg *pmic, u8 reg,
 					u8 set_mask, u8 clear_mask)
 {
@@ -362,7 +357,7 @@ static int tps65910_modify_bits(struct tps65910_reg *pmic, u8 reg,
 
 	data &= ~clear_mask;
 	data |= set_mask;
-	err = tps65910_write(pmic, reg, data);
+	err = tps65910_reg_write(pmic->mfd, reg, data);
 	if (err)
 		dev_err(pmic->mfd->dev, "Write for reg 0x%x failed\n", reg);
 
@@ -371,7 +366,7 @@ out:
 	return err;
 }
 
-static int tps65910_reg_read(struct tps65910_reg *pmic, u8 reg)
+static int tps65910_reg_read_locked(struct tps65910_reg *pmic, u8 reg)
 {
 	int data;
 
@@ -385,13 +380,13 @@ static int tps65910_reg_read(struct tps65910_reg *pmic, u8 reg)
 	return data;
 }
 
-static int tps65910_reg_write(struct tps65910_reg *pmic, u8 reg, u8 val)
+static int tps65910_reg_write_locked(struct tps65910_reg *pmic, u8 reg, u8 val)
 {
 	int err;
 
 	mutex_lock(&pmic->mutex);
 
-	err = tps65910_write(pmic, reg, val);
+	err = tps65910_reg_write(pmic->mfd, reg, val);
 	if (err < 0)
 		dev_err(pmic->mfd->dev, "Write for reg 0x%x failed\n", reg);
 
@@ -467,48 +462,6 @@ static int tps65911_get_ctrl_register(int id)
 	}
 }
 
-static int tps65910_is_enabled(struct regulator_dev *dev)
-{
-	struct tps65910_reg *pmic = rdev_get_drvdata(dev);
-	int reg, value, id = rdev_get_id(dev);
-
-	reg = pmic->get_ctrl_reg(id);
-	if (reg < 0)
-		return reg;
-
-	value = tps65910_reg_read(pmic, reg);
-	if (value < 0)
-		return value;
-
-	return value & TPS65910_SUPPLY_STATE_ENABLED;
-}
-
-static int tps65910_enable(struct regulator_dev *dev)
-{
-	struct tps65910_reg *pmic = rdev_get_drvdata(dev);
-	struct tps65910 *mfd = pmic->mfd;
-	int reg, id = rdev_get_id(dev);
-
-	reg = pmic->get_ctrl_reg(id);
-	if (reg < 0)
-		return reg;
-
-	return tps65910_set_bits(mfd, reg, TPS65910_SUPPLY_STATE_ENABLED);
-}
-
-static int tps65910_disable(struct regulator_dev *dev)
-{
-	struct tps65910_reg *pmic = rdev_get_drvdata(dev);
-	struct tps65910 *mfd = pmic->mfd;
-	int reg, id = rdev_get_id(dev);
-
-	reg = pmic->get_ctrl_reg(id);
-	if (reg < 0)
-		return reg;
-
-	return tps65910_clear_bits(mfd, reg, TPS65910_SUPPLY_STATE_ENABLED);
-}
-
 static int tps65910_enable_time(struct regulator_dev *dev)
 {
 	struct tps65910_reg *pmic = rdev_get_drvdata(dev);
@@ -532,9 +485,9 @@ static int tps65910_set_mode(struct regulator_dev *dev, unsigned int mode)
 							LDO_ST_MODE_BIT);
 	case REGULATOR_MODE_IDLE:
 		value = LDO_ST_ON_BIT | LDO_ST_MODE_BIT;
-		return tps65910_set_bits(mfd, reg, value);
+		return tps65910_reg_set_bits(mfd, reg, value);
 	case REGULATOR_MODE_STANDBY:
-		return tps65910_clear_bits(mfd, reg, LDO_ST_ON_BIT);
+		return tps65910_reg_clear_bits(mfd, reg, LDO_ST_ON_BIT);
 	}
 
 	return -EINVAL;
@@ -549,7 +502,7 @@ static unsigned int tps65910_get_mode(struct regulator_dev *dev)
 	if (reg < 0)
 		return reg;
 
-	value = tps65910_reg_read(pmic, reg);
+	value = tps65910_reg_read_locked(pmic, reg);
 	if (value < 0)
 		return value;
 
@@ -569,28 +522,28 @@ static int tps65910_get_voltage_dcdc_sel(struct regulator_dev *dev)
 
 	switch (id) {
 	case TPS65910_REG_VDD1:
-		opvsel = tps65910_reg_read(pmic, TPS65910_VDD1_OP);
-		mult = tps65910_reg_read(pmic, TPS65910_VDD1);
+		opvsel = tps65910_reg_read_locked(pmic, TPS65910_VDD1_OP);
+		mult = tps65910_reg_read_locked(pmic, TPS65910_VDD1);
 		mult = (mult & VDD1_VGAIN_SEL_MASK) >> VDD1_VGAIN_SEL_SHIFT;
-		srvsel = tps65910_reg_read(pmic, TPS65910_VDD1_SR);
+		srvsel = tps65910_reg_read_locked(pmic, TPS65910_VDD1_SR);
 		sr = opvsel & VDD1_OP_CMD_MASK;
 		opvsel &= VDD1_OP_SEL_MASK;
 		srvsel &= VDD1_SR_SEL_MASK;
 		vselmax = 75;
 		break;
 	case TPS65910_REG_VDD2:
-		opvsel = tps65910_reg_read(pmic, TPS65910_VDD2_OP);
-		mult = tps65910_reg_read(pmic, TPS65910_VDD2);
+		opvsel = tps65910_reg_read_locked(pmic, TPS65910_VDD2_OP);
+		mult = tps65910_reg_read_locked(pmic, TPS65910_VDD2);
 		mult = (mult & VDD2_VGAIN_SEL_MASK) >> VDD2_VGAIN_SEL_SHIFT;
-		srvsel = tps65910_reg_read(pmic, TPS65910_VDD2_SR);
+		srvsel = tps65910_reg_read_locked(pmic, TPS65910_VDD2_SR);
 		sr = opvsel & VDD2_OP_CMD_MASK;
 		opvsel &= VDD2_OP_SEL_MASK;
 		srvsel &= VDD2_SR_SEL_MASK;
 		vselmax = 75;
 		break;
 	case TPS65911_REG_VDDCTRL:
-		opvsel = tps65910_reg_read(pmic, TPS65911_VDDCTRL_OP);
-		srvsel = tps65910_reg_read(pmic, TPS65911_VDDCTRL_SR);
+		opvsel = tps65910_reg_read_locked(pmic, TPS65911_VDDCTRL_OP);
+		srvsel = tps65910_reg_read_locked(pmic, TPS65911_VDDCTRL_SR);
 		sr = opvsel & VDDCTRL_OP_CMD_MASK;
 		opvsel &= VDDCTRL_OP_SEL_MASK;
 		srvsel &= VDDCTRL_SR_SEL_MASK;
@@ -621,16 +574,16 @@ static int tps65910_get_voltage_dcdc_sel(struct regulator_dev *dev)
 	return -EINVAL;
 }
 
-static int tps65910_get_voltage(struct regulator_dev *dev)
+static int tps65910_get_voltage_sel(struct regulator_dev *dev)
 {
 	struct tps65910_reg *pmic = rdev_get_drvdata(dev);
-	int reg, value, id = rdev_get_id(dev), voltage = 0;
+	int reg, value, id = rdev_get_id(dev);
 
 	reg = pmic->get_ctrl_reg(id);
 	if (reg < 0)
 		return reg;
 
-	value = tps65910_reg_read(pmic, reg);
+	value = tps65910_reg_read_locked(pmic, reg);
 	if (value < 0)
 		return value;
 
@@ -651,9 +604,7 @@ static int tps65910_get_voltage(struct regulator_dev *dev)
 		return -EINVAL;
 	}
 
-	voltage = pmic->info[id]->voltage_table[value] * 1000;
-
-	return voltage;
+	return value;
 }
 
 static int tps65910_get_voltage_vdd3(struct regulator_dev *dev)
@@ -661,15 +612,15 @@ static int tps65910_get_voltage_vdd3(struct regulator_dev *dev)
 	return 5 * 1000 * 1000;
 }
 
-static int tps65911_get_voltage(struct regulator_dev *dev)
+static int tps65911_get_voltage_sel(struct regulator_dev *dev)
 {
 	struct tps65910_reg *pmic = rdev_get_drvdata(dev);
-	int step_mv, id = rdev_get_id(dev);
+	int id = rdev_get_id(dev);
 	u8 value, reg;
 
 	reg = pmic->get_ctrl_reg(id);
 
-	value = tps65910_reg_read(pmic, reg);
+	value = tps65910_reg_read_locked(pmic, reg);
 
 	switch (id) {
 	case TPS65911_REG_LDO1:
@@ -677,13 +628,6 @@ static int tps65911_get_voltage(struct regulator_dev *dev)
 	case TPS65911_REG_LDO4:
 		value &= LDO1_SEL_MASK;
 		value >>= LDO_SEL_SHIFT;
-		/* The first 5 values of the selector correspond to 1V */
-		if (value < 5)
-			value = 0;
-		else
-			value -= 4;
-
-		step_mv = 50;
 		break;
 	case TPS65911_REG_LDO3:
 	case TPS65911_REG_LDO5:
@@ -692,23 +636,16 @@ static int tps65911_get_voltage(struct regulator_dev *dev)
 	case TPS65911_REG_LDO8:
 		value &= LDO3_SEL_MASK;
 		value >>= LDO_SEL_SHIFT;
-		/* The first 3 values of the selector correspond to 1V */
-		if (value < 3)
-			value = 0;
-		else
-			value -= 2;
-
-		step_mv = 100;
 		break;
 	case TPS65910_REG_VIO:
 		value &= LDO_SEL_MASK;
 		value >>= LDO_SEL_SHIFT;
-		return pmic->info[id]->voltage_table[value] * 1000;
+		break;
 	default:
 		return -EINVAL;
 	}
 
-	return (LDO_MIN_VOLT + value * step_mv) * 1000;
+	return value;
 }
 
 static int tps65910_set_voltage_dcdc_sel(struct regulator_dev *dev,
@@ -728,7 +665,7 @@ static int tps65910_set_voltage_dcdc_sel(struct regulator_dev *dev,
 		tps65910_modify_bits(pmic, TPS65910_VDD1,
 				(dcdc_mult << VDD1_VGAIN_SEL_SHIFT),
 						VDD1_VGAIN_SEL_MASK);
-		tps65910_reg_write(pmic, TPS65910_VDD1_OP, vsel);
+		tps65910_reg_write_locked(pmic, TPS65910_VDD1_OP, vsel);
 		break;
 	case TPS65910_REG_VDD2:
 		dcdc_mult = (selector / VDD1_2_NUM_VOLT_FINE) + 1;
@@ -739,11 +676,11 @@ static int tps65910_set_voltage_dcdc_sel(struct regulator_dev *dev,
 		tps65910_modify_bits(pmic, TPS65910_VDD2,
 				(dcdc_mult << VDD2_VGAIN_SEL_SHIFT),
 						VDD1_VGAIN_SEL_MASK);
-		tps65910_reg_write(pmic, TPS65910_VDD2_OP, vsel);
+		tps65910_reg_write_locked(pmic, TPS65910_VDD2_OP, vsel);
 		break;
 	case TPS65911_REG_VDDCTRL:
 		vsel = selector + 3;
-		tps65910_reg_write(pmic, TPS65911_VDDCTRL_OP, vsel);
+		tps65910_reg_write_locked(pmic, TPS65911_VDDCTRL_OP, vsel);
 	}
 
 	return 0;
@@ -914,9 +851,9 @@ static int tps65910_set_voltage_dcdc_time_sel(struct regulator_dev *dev,
 
 /* Regulator ops (except VRTC) */
 static struct regulator_ops tps65910_ops_dcdc = {
-	.is_enabled		= tps65910_is_enabled,
-	.enable			= tps65910_enable,
-	.disable		= tps65910_disable,
+	.is_enabled		= regulator_is_enabled_regmap,
+	.enable			= regulator_enable_regmap,
+	.disable		= regulator_disable_regmap,
 	.enable_time		= tps65910_enable_time,
 	.set_mode		= tps65910_set_mode,
 	.get_mode		= tps65910_get_mode,
@@ -927,9 +864,9 @@ static struct regulator_ops tps65910_ops_dcdc = {
 };
 
 static struct regulator_ops tps65910_ops_vdd3 = {
-	.is_enabled		= tps65910_is_enabled,
-	.enable			= tps65910_enable,
-	.disable		= tps65910_disable,
+	.is_enabled		= regulator_is_enabled_regmap,
+	.enable			= regulator_enable_regmap,
+	.disable		= regulator_disable_regmap,
 	.enable_time		= tps65910_enable_time,
 	.set_mode		= tps65910_set_mode,
 	.get_mode		= tps65910_get_mode,
@@ -938,25 +875,25 @@ static struct regulator_ops tps65910_ops_vdd3 = {
 };
 
 static struct regulator_ops tps65910_ops = {
-	.is_enabled		= tps65910_is_enabled,
-	.enable			= tps65910_enable,
-	.disable		= tps65910_disable,
+	.is_enabled		= regulator_is_enabled_regmap,
+	.enable			= regulator_enable_regmap,
+	.disable		= regulator_disable_regmap,
 	.enable_time		= tps65910_enable_time,
 	.set_mode		= tps65910_set_mode,
 	.get_mode		= tps65910_get_mode,
-	.get_voltage		= tps65910_get_voltage,
+	.get_voltage_sel	= tps65910_get_voltage_sel,
 	.set_voltage_sel	= tps65910_set_voltage_sel,
 	.list_voltage		= tps65910_list_voltage,
 };
 
 static struct regulator_ops tps65911_ops = {
-	.is_enabled		= tps65910_is_enabled,
-	.enable			= tps65910_enable,
-	.disable		= tps65910_disable,
+	.is_enabled		= regulator_is_enabled_regmap,
+	.enable			= regulator_enable_regmap,
+	.disable		= regulator_disable_regmap,
 	.enable_time		= tps65910_enable_time,
 	.set_mode		= tps65910_set_mode,
 	.get_mode		= tps65910_get_mode,
-	.get_voltage		= tps65911_get_voltage,
+	.get_voltage_sel	= tps65911_get_voltage_sel,
 	.set_voltage_sel	= tps65911_set_voltage_sel,
 	.list_voltage		= tps65911_list_voltage,
 };
@@ -994,10 +931,10 @@ static int tps65910_set_ext_sleep_config(struct tps65910_reg *pmic,
 
 	/* External EN1 control */
 	if (ext_sleep_config & TPS65910_SLEEP_CONTROL_EXT_INPUT_EN1)
-		ret = tps65910_set_bits(mfd,
+		ret = tps65910_reg_set_bits(mfd,
 				TPS65910_EN1_LDO_ASS + regoffs, bit_pos);
 	else
-		ret = tps65910_clear_bits(mfd,
+		ret = tps65910_reg_clear_bits(mfd,
 				TPS65910_EN1_LDO_ASS + regoffs, bit_pos);
 	if (ret < 0) {
 		dev_err(mfd->dev,
@@ -1007,10 +944,10 @@ static int tps65910_set_ext_sleep_config(struct tps65910_reg *pmic,
 
 	/* External EN2 control */
 	if (ext_sleep_config & TPS65910_SLEEP_CONTROL_EXT_INPUT_EN2)
-		ret = tps65910_set_bits(mfd,
+		ret = tps65910_reg_set_bits(mfd,
 				TPS65910_EN2_LDO_ASS + regoffs, bit_pos);
 	else
-		ret = tps65910_clear_bits(mfd,
+		ret = tps65910_reg_clear_bits(mfd,
 				TPS65910_EN2_LDO_ASS + regoffs, bit_pos);
 	if (ret < 0) {
 		dev_err(mfd->dev,
@@ -1022,10 +959,10 @@ static int tps65910_set_ext_sleep_config(struct tps65910_reg *pmic,
 	if ((tps65910_chip_id(mfd) == TPS65910) &&
 			(id >= TPS65910_REG_VDIG1)) {
 		if (ext_sleep_config & TPS65910_SLEEP_CONTROL_EXT_INPUT_EN3)
-			ret = tps65910_set_bits(mfd,
+			ret = tps65910_reg_set_bits(mfd,
 				TPS65910_EN3_LDO_ASS + regoffs, bit_pos);
 		else
-			ret = tps65910_clear_bits(mfd,
+			ret = tps65910_reg_clear_bits(mfd,
 				TPS65910_EN3_LDO_ASS + regoffs, bit_pos);
 		if (ret < 0) {
 			dev_err(mfd->dev,
@@ -1037,10 +974,10 @@ static int tps65910_set_ext_sleep_config(struct tps65910_reg *pmic,
 	/* Return if no external control is selected */
 	if (!(ext_sleep_config & EXT_SLEEP_CONTROL)) {
 		/* Clear all sleep controls */
-		ret = tps65910_clear_bits(mfd,
+		ret = tps65910_reg_clear_bits(mfd,
 			TPS65910_SLEEP_KEEP_LDO_ON + regoffs, bit_pos);
 		if (!ret)
-			ret = tps65910_clear_bits(mfd,
+			ret = tps65910_reg_clear_bits(mfd,
 				TPS65910_SLEEP_SET_LDO_OFF + regoffs, bit_pos);
 		if (ret < 0)
 			dev_err(mfd->dev,
@@ -1059,32 +996,33 @@ static int tps65910_set_ext_sleep_config(struct tps65910_reg *pmic,
 				(tps65910_chip_id(mfd) == TPS65911))) {
 		int op_reg_add = pmic->get_ctrl_reg(id) + 1;
 		int sr_reg_add = pmic->get_ctrl_reg(id) + 2;
-		int opvsel = tps65910_reg_read(pmic, op_reg_add);
-		int srvsel = tps65910_reg_read(pmic, sr_reg_add);
+		int opvsel = tps65910_reg_read_locked(pmic, op_reg_add);
+		int srvsel = tps65910_reg_read_locked(pmic, sr_reg_add);
 		if (opvsel & VDD1_OP_CMD_MASK) {
 			u8 reg_val = srvsel & VDD1_OP_SEL_MASK;
-			ret = tps65910_reg_write(pmic, op_reg_add, reg_val);
+			ret = tps65910_reg_write_locked(pmic, op_reg_add,
+							reg_val);
 			if (ret < 0) {
 				dev_err(mfd->dev,
 					"Error in configuring op register\n");
 				return ret;
 			}
 		}
-		ret = tps65910_reg_write(pmic, sr_reg_add, 0);
+		ret = tps65910_reg_write_locked(pmic, sr_reg_add, 0);
 		if (ret < 0) {
 			dev_err(mfd->dev, "Error in settting sr register\n");
 			return ret;
 		}
 	}
 
-	ret = tps65910_clear_bits(mfd,
+	ret = tps65910_reg_clear_bits(mfd,
 			TPS65910_SLEEP_KEEP_LDO_ON + regoffs, bit_pos);
 	if (!ret) {
 		if (ext_sleep_config & TPS65911_SLEEP_CONTROL_EXT_INPUT_SLEEP)
-			ret = tps65910_set_bits(mfd,
+			ret = tps65910_reg_set_bits(mfd,
 				TPS65910_SLEEP_SET_LDO_OFF + regoffs, bit_pos);
 		else
-			ret = tps65910_clear_bits(mfd,
+			ret = tps65910_reg_clear_bits(mfd,
 				TPS65910_SLEEP_SET_LDO_OFF + regoffs, bit_pos);
 	}
 	if (ret < 0)
@@ -1094,30 +1032,148 @@ static int tps65910_set_ext_sleep_config(struct tps65910_reg *pmic,
 	return ret;
 }
 
+#ifdef CONFIG_OF
+
+static struct of_regulator_match tps65910_matches[] = {
+	{ .name = "vrtc",	.driver_data = (void *) &tps65910_regs[0] },
+	{ .name = "vio",	.driver_data = (void *) &tps65910_regs[1] },
+	{ .name = "vdd1",	.driver_data = (void *) &tps65910_regs[2] },
+	{ .name = "vdd2",	.driver_data = (void *) &tps65910_regs[3] },
+	{ .name = "vdd3",	.driver_data = (void *) &tps65910_regs[4] },
+	{ .name = "vdig1",	.driver_data = (void *) &tps65910_regs[5] },
+	{ .name = "vdig2",	.driver_data = (void *) &tps65910_regs[6] },
+	{ .name = "vpll",	.driver_data = (void *) &tps65910_regs[7] },
+	{ .name = "vdac",	.driver_data = (void *) &tps65910_regs[8] },
+	{ .name = "vaux1",	.driver_data = (void *) &tps65910_regs[9] },
+	{ .name = "vaux2",	.driver_data = (void *) &tps65910_regs[10] },
+	{ .name = "vaux33",	.driver_data = (void *) &tps65910_regs[11] },
+	{ .name = "vmmc",	.driver_data = (void *) &tps65910_regs[12] },
+};
+
+static struct of_regulator_match tps65911_matches[] = {
+	{ .name = "vrtc",	.driver_data = (void *) &tps65911_regs[0] },
+	{ .name = "vio",	.driver_data = (void *) &tps65911_regs[1] },
+	{ .name = "vdd1",	.driver_data = (void *) &tps65911_regs[2] },
+	{ .name = "vdd2",	.driver_data = (void *) &tps65911_regs[3] },
+	{ .name = "vddctrl",	.driver_data = (void *) &tps65911_regs[4] },
+	{ .name = "ldo1",	.driver_data = (void *) &tps65911_regs[5] },
+	{ .name = "ldo2",	.driver_data = (void *) &tps65911_regs[6] },
+	{ .name = "ldo3",	.driver_data = (void *) &tps65911_regs[7] },
+	{ .name = "ldo4",	.driver_data = (void *) &tps65911_regs[8] },
+	{ .name = "ldo5",	.driver_data = (void *) &tps65911_regs[9] },
+	{ .name = "ldo6",	.driver_data = (void *) &tps65911_regs[10] },
+	{ .name = "ldo7",	.driver_data = (void *) &tps65911_regs[11] },
+	{ .name = "ldo8",	.driver_data = (void *) &tps65911_regs[12] },
+};
+
+static struct tps65910_board *tps65910_parse_dt_reg_data(
+		struct platform_device *pdev,
+		struct of_regulator_match **tps65910_reg_matches)
+{
+	struct tps65910_board *pmic_plat_data;
+	struct tps65910 *tps65910 = dev_get_drvdata(pdev->dev.parent);
+	struct device_node *np = pdev->dev.parent->of_node;
+	struct device_node *regulators;
+	struct of_regulator_match *matches;
+	unsigned int prop;
+	int idx = 0, ret, count;
+
+	pmic_plat_data = devm_kzalloc(&pdev->dev, sizeof(*pmic_plat_data),
+					GFP_KERNEL);
+
+	if (!pmic_plat_data) {
+		dev_err(&pdev->dev, "Failure to alloc pdata for regulators.\n");
+		return NULL;
+	}
+
+	regulators = of_find_node_by_name(np, "regulators");
+	if (!regulators) {
+		dev_err(&pdev->dev, "regulator node not found\n");
+		return NULL;
+	}
+
+	switch (tps65910_chip_id(tps65910)) {
+	case TPS65910:
+		count = ARRAY_SIZE(tps65910_matches);
+		matches = tps65910_matches;
+		break;
+	case TPS65911:
+		count = ARRAY_SIZE(tps65911_matches);
+		matches = tps65911_matches;
+		break;
+	default:
+		dev_err(&pdev->dev, "Invalid tps chip version\n");
+		return NULL;
+	}
+
+	ret = of_regulator_match(pdev->dev.parent, regulators, matches, count);
+	if (ret < 0) {
+		dev_err(&pdev->dev, "Error parsing regulator init data: %d\n",
+			ret);
+		return NULL;
+	}
+
+	*tps65910_reg_matches = matches;
+
+	for (idx = 0; idx < count; idx++) {
+		if (!matches[idx].init_data || !matches[idx].of_node)
+			continue;
+
+		pmic_plat_data->tps65910_pmic_init_data[idx] =
+							matches[idx].init_data;
+
+		ret = of_property_read_u32(matches[idx].of_node,
+				"ti,regulator-ext-sleep-control", &prop);
+		if (!ret)
+			pmic_plat_data->regulator_ext_sleep_control[idx] = prop;
+	}
+
+	return pmic_plat_data;
+}
+#else
+static inline struct tps65910_board *tps65910_parse_dt_reg_data(
+			struct platform_device *pdev,
+			struct of_regulator_match **tps65910_reg_matches)
+{
+	*tps65910_reg_matches = NULL;
+	return 0;
+}
+#endif
+
 static __devinit int tps65910_probe(struct platform_device *pdev)
 {
 	struct tps65910 *tps65910 = dev_get_drvdata(pdev->dev.parent);
+	struct regulator_config config = { };
 	struct tps_info *info;
 	struct regulator_init_data *reg_data;
 	struct regulator_dev *rdev;
 	struct tps65910_reg *pmic;
 	struct tps65910_board *pmic_plat_data;
+	struct of_regulator_match *tps65910_reg_matches = NULL;
 	int i, err;
 
 	pmic_plat_data = dev_get_platdata(tps65910->dev);
-	if (!pmic_plat_data)
+	if (!pmic_plat_data && tps65910->dev->of_node)
+		pmic_plat_data = tps65910_parse_dt_reg_data(pdev,
+						&tps65910_reg_matches);
+
+	if (!pmic_plat_data) {
+		dev_err(&pdev->dev, "Platform data not found\n");
 		return -EINVAL;
+	}
 
-	pmic = kzalloc(sizeof(*pmic), GFP_KERNEL);
-	if (!pmic)
+	pmic = devm_kzalloc(&pdev->dev, sizeof(*pmic), GFP_KERNEL);
+	if (!pmic) {
+		dev_err(&pdev->dev, "Memory allocation failed for pmic\n");
 		return -ENOMEM;
+	}
 
 	mutex_init(&pmic->mutex);
 	pmic->mfd = tps65910;
 	platform_set_drvdata(pdev, pmic);
 
 	/* Give control of all register to control port */
-	tps65910_set_bits(pmic->mfd, TPS65910_DEVCTRL,
+	tps65910_reg_set_bits(pmic->mfd, TPS65910_DEVCTRL,
 				DEVCTRL_SR_CTL_I2C_SEL_MASK);
 
 	switch(tps65910_chip_id(tps65910)) {
@@ -1134,30 +1190,29 @@ static __devinit int tps65910_probe(struct platform_device *pdev)
 		info = tps65911_regs;
 		break;
 	default:
-		pr_err("Invalid tps chip version\n");
-		kfree(pmic);
+		dev_err(&pdev->dev, "Invalid tps chip version\n");
 		return -ENODEV;
 	}
 
-	pmic->desc = kcalloc(pmic->num_regulators,
+	pmic->desc = devm_kzalloc(&pdev->dev, pmic->num_regulators *
 			sizeof(struct regulator_desc), GFP_KERNEL);
 	if (!pmic->desc) {
-		err = -ENOMEM;
-		goto err_free_pmic;
+		dev_err(&pdev->dev, "Memory alloc fails for desc\n");
+		return -ENOMEM;
 	}
 
-	pmic->info = kcalloc(pmic->num_regulators,
+	pmic->info = devm_kzalloc(&pdev->dev, pmic->num_regulators *
 			sizeof(struct tps_info *), GFP_KERNEL);
 	if (!pmic->info) {
-		err = -ENOMEM;
-		goto err_free_desc;
+		dev_err(&pdev->dev, "Memory alloc fails for info\n");
+		return -ENOMEM;
 	}
 
-	pmic->rdev = kcalloc(pmic->num_regulators,
+	pmic->rdev = devm_kzalloc(&pdev->dev, pmic->num_regulators *
 			sizeof(struct regulator_dev *), GFP_KERNEL);
 	if (!pmic->rdev) {
-		err = -ENOMEM;
-		goto err_free_info;
+		dev_err(&pdev->dev, "Memory alloc fails for rdev\n");
+		return -ENOMEM;
 	}
 
 	for (i = 0; i < pmic->num_regulators && i < TPS65910_NUM_REGS;
@@ -1205,9 +1260,18 @@ static __devinit int tps65910_probe(struct platform_device *pdev)
 
 		pmic->desc[i].type = REGULATOR_VOLTAGE;
 		pmic->desc[i].owner = THIS_MODULE;
+		pmic->desc[i].enable_reg = pmic->get_ctrl_reg(i);
+		pmic->desc[i].enable_mask = TPS65910_SUPPLY_STATE_ENABLED;
+
+		config.dev = tps65910->dev;
+		config.init_data = reg_data;
+		config.driver_data = pmic;
+		config.regmap = tps65910->regmap;
+
+		if (tps65910_reg_matches)
+			config.of_node = tps65910_reg_matches[i].of_node;
 
-		rdev = regulator_register(&pmic->desc[i],
-				tps65910->dev, reg_data, pmic, NULL);
+		rdev = regulator_register(&pmic->desc[i], &config);
 		if (IS_ERR(rdev)) {
 			dev_err(tps65910->dev,
 				"failed to register %s regulator\n",
@@ -1224,13 +1288,6 @@ static __devinit int tps65910_probe(struct platform_device *pdev)
 err_unregister_regulator:
 	while (--i >= 0)
 		regulator_unregister(pmic->rdev[i]);
-	kfree(pmic->rdev);
-err_free_info:
-	kfree(pmic->info);
-err_free_desc:
-	kfree(pmic->desc);
-err_free_pmic:
-	kfree(pmic);
 	return err;
 }
 
@@ -1242,10 +1299,6 @@ static int __devexit tps65910_remove(struct platform_device *pdev)
 	for (i = 0; i < pmic->num_regulators; i++)
 		regulator_unregister(pmic->rdev[i]);
 
-	kfree(pmic->rdev);
-	kfree(pmic->info);
-	kfree(pmic->desc);
-	kfree(pmic);
 	return 0;
 }
 
diff --git a/drivers/regulator/tps65912-regulator.c b/drivers/regulator/tps65912-regulator.c
index b36799b1f53..18b2a1dcb4b 100644
--- a/drivers/regulator/tps65912-regulator.c
+++ b/drivers/regulator/tps65912-regulator.c
@@ -20,7 +20,6 @@
 #include <linux/platform_device.h>
 #include <linux/regulator/driver.h>
 #include <linux/regulator/machine.h>
-#include <linux/delay.h>
 #include <linux/slab.h>
 #include <linux/gpio.h>
 #include <linux/mfd/tps65912.h>
@@ -372,12 +371,14 @@ static unsigned int tps65912_get_mode(struct regulator_dev *dev)
 	return mode;
 }
 
-static int tps65912_list_voltage_dcdc(struct regulator_dev *dev,
-					unsigned selector)
+static int tps65912_list_voltage(struct regulator_dev *dev, unsigned selector)
 {
 	struct tps65912_reg *pmic = rdev_get_drvdata(dev);
 	int range, voltage = 0, id = rdev_get_id(dev);
 
+	if (id >= TPS65912_REG_LDO1 && id <= TPS65912_REG_LDO10)
+		return tps65912_vsel_to_uv_ldo(selector);
+
 	if (id > TPS65912_REG_DCDC4)
 		return -EINVAL;
 
@@ -404,7 +405,7 @@ static int tps65912_list_voltage_dcdc(struct regulator_dev *dev,
 	return voltage;
 }
 
-static int tps65912_get_voltage_dcdc(struct regulator_dev *dev)
+static int tps65912_get_voltage_sel(struct regulator_dev *dev)
 {
 	struct tps65912_reg *pmic = rdev_get_drvdata(dev);
 	struct tps65912 *mfd = pmic->mfd;
@@ -418,7 +419,7 @@ static int tps65912_get_voltage_dcdc(struct regulator_dev *dev)
 	vsel = tps65912_reg_read(mfd, reg);
 	vsel &= 0x3F;
 
-	return tps65912_list_voltage_dcdc(dev, vsel);
+	return vsel;
 }
 
 static int tps65912_set_voltage_sel(struct regulator_dev *dev,
@@ -436,32 +437,6 @@ static int tps65912_set_voltage_sel(struct regulator_dev *dev,
 	return tps65912_reg_write(mfd, reg, selector | value);
 }
 
-static int tps65912_get_voltage_ldo(struct regulator_dev *dev)
-{
-	struct tps65912_reg *pmic = rdev_get_drvdata(dev);
-	struct tps65912 *mfd = pmic->mfd;
-	int id = rdev_get_id(dev);
-	int vsel = 0;
-	u8 reg;
-
-	reg = tps65912_get_sel_register(pmic, id);
-	vsel = tps65912_reg_read(mfd, reg);
-	vsel &= 0x3F;
-
-	return tps65912_vsel_to_uv_ldo(vsel);
-}
-
-static int tps65912_list_voltage_ldo(struct regulator_dev *dev,
-					unsigned selector)
-{
-	int ldo = rdev_get_id(dev);
-
-	if (ldo < TPS65912_REG_LDO1 || ldo > TPS65912_REG_LDO10)
-		return -EINVAL;
-
-	return tps65912_vsel_to_uv_ldo(selector);
-}
-
 /* Operations permitted on DCDCx */
 static struct regulator_ops tps65912_ops_dcdc = {
 	.is_enabled = tps65912_reg_is_enabled,
@@ -469,9 +444,9 @@ static struct regulator_ops tps65912_ops_dcdc = {
 	.disable = tps65912_reg_disable,
 	.set_mode = tps65912_set_mode,
 	.get_mode = tps65912_get_mode,
-	.get_voltage = tps65912_get_voltage_dcdc,
+	.get_voltage_sel = tps65912_get_voltage_sel,
 	.set_voltage_sel = tps65912_set_voltage_sel,
-	.list_voltage = tps65912_list_voltage_dcdc,
+	.list_voltage = tps65912_list_voltage,
 };
 
 /* Operations permitted on LDOx */
@@ -479,14 +454,15 @@ static struct regulator_ops tps65912_ops_ldo = {
 	.is_enabled = tps65912_reg_is_enabled,
 	.enable = tps65912_reg_enable,
 	.disable = tps65912_reg_disable,
-	.get_voltage = tps65912_get_voltage_ldo,
+	.get_voltage_sel = tps65912_get_voltage_sel,
 	.set_voltage_sel = tps65912_set_voltage_sel,
-	.list_voltage = tps65912_list_voltage_ldo,
+	.list_voltage = tps65912_list_voltage,
 };
 
 static __devinit int tps65912_probe(struct platform_device *pdev)
 {
 	struct tps65912 *tps65912 = dev_get_drvdata(pdev->dev.parent);
+	struct regulator_config config = { };
 	struct tps_info *info;
 	struct regulator_init_data *reg_data;
 	struct regulator_dev *rdev;
@@ -500,7 +476,7 @@ static __devinit int tps65912_probe(struct platform_device *pdev)
 
 	reg_data = pmic_plat_data->tps65912_pmic_init_data;
 
-	pmic = kzalloc(sizeof(*pmic), GFP_KERNEL);
+	pmic = devm_kzalloc(&pdev->dev, sizeof(*pmic), GFP_KERNEL);
 	if (!pmic)
 		return -ENOMEM;
 
@@ -524,8 +500,12 @@ static __devinit int tps65912_probe(struct platform_device *pdev)
 		pmic->desc[i].type = REGULATOR_VOLTAGE;
 		pmic->desc[i].owner = THIS_MODULE;
 		range = tps65912_get_range(pmic, i);
-		rdev = regulator_register(&pmic->desc[i],
-					tps65912->dev, reg_data, pmic, NULL);
+
+		config.dev = tps65912->dev;
+		config.init_data = reg_data;
+		config.driver_data = pmic;
+
+		rdev = regulator_register(&pmic->desc[i], &config);
 		if (IS_ERR(rdev)) {
 			dev_err(tps65912->dev,
 				"failed to register %s regulator\n",
@@ -542,8 +522,6 @@ static __devinit int tps65912_probe(struct platform_device *pdev)
 err:
 	while (--i >= 0)
 		regulator_unregister(pmic->rdev[i]);
-
-	kfree(pmic);
 	return err;
 }
 
@@ -554,8 +532,6 @@ static int __devexit tps65912_remove(struct platform_device *pdev)
 
 	for (i = 0; i < TPS65912_NUM_REGULATOR; i++)
 		regulator_unregister(tps65912_reg->rdev[i]);
-
-	kfree(tps65912_reg);
 	return 0;
 }
 
diff --git a/drivers/regulator/twl-regulator.c b/drivers/regulator/twl-regulator.c
index 9cdfc389ca2..c7390711d95 100644
--- a/drivers/regulator/twl-regulator.c
+++ b/drivers/regulator/twl-regulator.c
@@ -12,7 +12,6 @@
 #include <linux/module.h>
 #include <linux/init.h>
 #include <linux/err.h>
-#include <linux/delay.h>
 #include <linux/platform_device.h>
 #include <linux/of.h>
 #include <linux/of_device.h>
@@ -175,15 +174,14 @@ static int twl6030reg_is_enabled(struct regulator_dev *rdev)
 	struct twlreg_info	*info = rdev_get_drvdata(rdev);
 	int			grp = 0, val;
 
-	if (!(twl_class_is_6030() && (info->features & TWL6025_SUBCLASS)))
-		grp = twlreg_read(info, TWL_MODULE_PM_RECEIVER, VREG_GRP);
-	if (grp < 0)
-		return grp;
-
-	if (!(twl_class_is_6030() && (info->features & TWL6025_SUBCLASS)))
+	if (!(twl_class_is_6030() && (info->features & TWL6025_SUBCLASS))) {
+		grp = twlreg_grp(rdev);
+		if (grp < 0)
+			return grp;
 		grp &= P1_GRP_6030;
-	else
+	} else {
 		grp = 1;
+	}
 
 	val = twlreg_read(info, TWL_MODULE_PM_RECEIVER, VREG_STATE);
 	val = TWL6030_CFG_STATE_APP(val);
@@ -197,7 +195,7 @@ static int twl4030reg_enable(struct regulator_dev *rdev)
 	int			grp;
 	int			ret;
 
-	grp = twlreg_read(info, TWL_MODULE_PM_RECEIVER, VREG_GRP);
+	grp = twlreg_grp(rdev);
 	if (grp < 0)
 		return grp;
 
@@ -205,8 +203,6 @@ static int twl4030reg_enable(struct regulator_dev *rdev)
 
 	ret = twlreg_write(info, TWL_MODULE_PM_RECEIVER, VREG_GRP, grp);
 
-	udelay(info->delay);
-
 	return ret;
 }
 
@@ -217,17 +213,28 @@ static int twl6030reg_enable(struct regulator_dev *rdev)
 	int			ret;
 
 	if (!(twl_class_is_6030() && (info->features & TWL6025_SUBCLASS)))
-		grp = twlreg_read(info, TWL_MODULE_PM_RECEIVER, VREG_GRP);
+		grp = twlreg_grp(rdev);
 	if (grp < 0)
 		return grp;
 
 	ret = twlreg_write(info, TWL_MODULE_PM_RECEIVER, VREG_STATE,
 			grp << TWL6030_CFG_STATE_GRP_SHIFT |
 			TWL6030_CFG_STATE_ON);
+	return ret;
+}
 
-	udelay(info->delay);
+static int twl4030reg_enable_time(struct regulator_dev *rdev)
+{
+	struct twlreg_info	*info = rdev_get_drvdata(rdev);
 
-	return ret;
+	return info->delay;
+}
+
+static int twl6030reg_enable_time(struct regulator_dev *rdev)
+{
+	struct twlreg_info	*info = rdev_get_drvdata(rdev);
+
+	return info->delay;
 }
 
 static int twl4030reg_disable(struct regulator_dev *rdev)
@@ -236,7 +243,7 @@ static int twl4030reg_disable(struct regulator_dev *rdev)
 	int			grp;
 	int			ret;
 
-	grp = twlreg_read(info, TWL_MODULE_PM_RECEIVER, VREG_GRP);
+	grp = twlreg_grp(rdev);
 	if (grp < 0)
 		return grp;
 
@@ -348,7 +355,7 @@ static int twl6030reg_set_mode(struct regulator_dev *rdev, unsigned mode)
 	int val;
 
 	if (!(twl_class_is_6030() && (info->features & TWL6025_SUBCLASS)))
-		grp = twlreg_read(info, TWL_MODULE_PM_RECEIVER, VREG_GRP);
+		grp = twlreg_grp(rdev);
 
 	if (grp < 0)
 		return grp;
@@ -388,14 +395,12 @@ static int twl6030reg_set_mode(struct regulator_dev *rdev, unsigned mode)
  * VAUX3 at 3V is incorrectly listed in some TI manuals as unsupported.
  * TI are revising the twl5030/tps659x0 specs to support that 3.0V setting.
  */
-#ifdef CONFIG_TWL4030_ALLOW_UNSUPPORTED
-#define UNSUP_MASK	0x0000
-#else
 #define UNSUP_MASK	0x8000
-#endif
 
 #define UNSUP(x)	(UNSUP_MASK | (x))
-#define IS_UNSUP(x)	(UNSUP_MASK & (x))
+#define IS_UNSUP(info, x)			\
+	((UNSUP_MASK & (x)) &&			\
+	 !((info)->features & TWL4030_ALLOW_UNSUPPORTED))
 #define LDO_MV(x)	(~UNSUP_MASK & (x))
 
 
@@ -469,35 +474,16 @@ static int twl4030ldo_list_voltage(struct regulator_dev *rdev, unsigned index)
 	struct twlreg_info	*info = rdev_get_drvdata(rdev);
 	int			mV = info->table[index];
 
-	return IS_UNSUP(mV) ? 0 : (LDO_MV(mV) * 1000);
+	return IS_UNSUP(info, mV) ? 0 : (LDO_MV(mV) * 1000);
 }
 
 static int
-twl4030ldo_set_voltage(struct regulator_dev *rdev, int min_uV, int max_uV,
-		       unsigned *selector)
+twl4030ldo_set_voltage_sel(struct regulator_dev *rdev, unsigned selector)
 {
 	struct twlreg_info	*info = rdev_get_drvdata(rdev);
-	int			vsel;
 
-	for (vsel = 0; vsel < info->table_len; vsel++) {
-		int mV = info->table[vsel];
-		int uV;
-
-		if (IS_UNSUP(mV))
-			continue;
-		uV = LDO_MV(mV) * 1000;
-
-		/* REVISIT for VAUX2, first match may not be best/lowest */
-
-		/* use the first in-range value */
-		if (min_uV <= uV && uV <= max_uV) {
-			*selector = vsel;
-			return twlreg_write(info, TWL_MODULE_PM_RECEIVER,
-							VREG_VOLTAGE, vsel);
-		}
-	}
-
-	return -EDOM;
+	return twlreg_write(info, TWL_MODULE_PM_RECEIVER, VREG_VOLTAGE,
+			    selector);
 }
 
 static int twl4030ldo_get_voltage(struct regulator_dev *rdev)
@@ -516,12 +502,13 @@ static int twl4030ldo_get_voltage(struct regulator_dev *rdev)
 static struct regulator_ops twl4030ldo_ops = {
 	.list_voltage	= twl4030ldo_list_voltage,
 
-	.set_voltage	= twl4030ldo_set_voltage,
+	.set_voltage_sel = twl4030ldo_set_voltage_sel,
 	.get_voltage	= twl4030ldo_get_voltage,
 
 	.enable		= twl4030reg_enable,
 	.disable	= twl4030reg_disable,
 	.is_enabled	= twl4030reg_is_enabled,
+	.enable_time	= twl4030reg_enable_time,
 
 	.set_mode	= twl4030reg_set_mode,
 
@@ -642,6 +629,7 @@ static struct regulator_ops twl6030ldo_ops = {
 	.enable		= twl6030reg_enable,
 	.disable	= twl6030reg_disable,
 	.is_enabled	= twl6030reg_is_enabled,
+	.enable_time	= twl6030reg_enable_time,
 
 	.set_mode	= twl6030reg_set_mode,
 
@@ -675,6 +663,7 @@ static struct regulator_ops twl4030fixed_ops = {
 	.enable		= twl4030reg_enable,
 	.disable	= twl4030reg_disable,
 	.is_enabled	= twl4030reg_is_enabled,
+	.enable_time	= twl4030reg_enable_time,
 
 	.set_mode	= twl4030reg_set_mode,
 
@@ -689,6 +678,7 @@ static struct regulator_ops twl6030fixed_ops = {
 	.enable		= twl6030reg_enable,
 	.disable	= twl6030reg_disable,
 	.is_enabled	= twl6030reg_is_enabled,
+	.enable_time	= twl6030reg_enable_time,
 
 	.set_mode	= twl6030reg_set_mode,
 
@@ -699,6 +689,7 @@ static struct regulator_ops twl6030_fixed_resource = {
 	.enable		= twl6030reg_enable,
 	.disable	= twl6030reg_disable,
 	.is_enabled	= twl6030reg_is_enabled,
+	.enable_time	= twl6030reg_enable_time,
 	.get_status	= twl6030reg_get_status,
 };
 
@@ -806,10 +797,7 @@ twl6030smps_set_voltage(struct regulator_dev *rdev, int min_uV, int max_uV,
 			vsel = 0;
 		else if ((min_uV >= 600000) && (min_uV <= 1300000)) {
 			int calc_uV;
-			vsel = (min_uV - 600000) / 125;
-			if (vsel % 100)
-				vsel += 100;
-			vsel /= 100;
+			vsel = DIV_ROUND_UP(min_uV - 600000, 12500);
 			vsel++;
 			calc_uV = twl6030smps_list_voltage(rdev, vsel);
 			if (calc_uV > max_uV)
@@ -836,10 +824,7 @@ twl6030smps_set_voltage(struct regulator_dev *rdev, int min_uV, int max_uV,
 			vsel = 0;
 		else if ((min_uV >= 700000) && (min_uV <= 1420000)) {
 			int calc_uV;
-			vsel = (min_uV - 700000) / 125;
-			if (vsel % 100)
-				vsel += 100;
-			vsel /= 100;
+			vsel = DIV_ROUND_UP(min_uV - 700000, 12500);
 			vsel++;
 			calc_uV = twl6030smps_list_voltage(rdev, vsel);
 			if (calc_uV > max_uV)
@@ -862,24 +847,18 @@ twl6030smps_set_voltage(struct regulator_dev *rdev, int min_uV, int max_uV,
 			return -EINVAL;
 		break;
 	case SMPS_EXTENDED_EN:
-		if (min_uV == 0)
+		if (min_uV == 0) {
 			vsel = 0;
-		else if ((min_uV >= 1852000) && (max_uV <= 4013600)) {
-			vsel = (min_uV - 1852000) / 386;
-			if (vsel % 100)
-				vsel += 100;
-			vsel /= 100;
+		} else if ((min_uV >= 1852000) && (max_uV <= 4013600)) {
+			vsel = DIV_ROUND_UP(min_uV - 1852000, 38600);
 			vsel++;
 		}
 		break;
 	case SMPS_OFFSET_EN|SMPS_EXTENDED_EN:
-		if (min_uV == 0)
+		if (min_uV == 0) {
 			vsel = 0;
-		else if ((min_uV >= 2161000) && (max_uV <= 4321000)) {
-			vsel = (min_uV - 2161000) / 386;
-			if (vsel % 100)
-				vsel += 100;
-			vsel /= 100;
+		} else if ((min_uV >= 2161000) && (max_uV <= 4321000)) {
+			vsel = DIV_ROUND_UP(min_uV - 2161000, 38600);
 			vsel++;
 		}
 		break;
@@ -907,6 +886,7 @@ static struct regulator_ops twlsmps_ops = {
 	.enable			= twl6030reg_enable,
 	.disable		= twl6030reg_disable,
 	.is_enabled		= twl6030reg_is_enabled,
+	.enable_time		= twl6030reg_enable_time,
 
 	.set_mode		= twl6030reg_set_mode,
 
@@ -1194,6 +1174,7 @@ static int __devinit twlreg_probe(struct platform_device *pdev)
 	struct regulator_dev		*rdev;
 	struct twl_regulator_driver_data	*drvdata;
 	const struct of_device_id	*match;
+	struct regulator_config		config = { };
 
 	match = of_match_device(twl_of_match, &pdev->dev);
 	if (match) {
@@ -1207,10 +1188,12 @@ static int __devinit twlreg_probe(struct platform_device *pdev)
 		initdata = pdev->dev.platform_data;
 		for (i = 0, info = NULL; i < ARRAY_SIZE(twl_of_match); i++) {
 			info = twl_of_match[i].data;
-			if (!info || info->desc.id != id)
-				continue;
-			break;
+			if (info && info->desc.id == id)
+				break;
 		}
+		if (i == ARRAY_SIZE(twl_of_match))
+			return -ENODEV;
+
 		drvdata = initdata->driver_data;
 		if (!drvdata)
 			return -EINVAL;
@@ -1273,8 +1256,12 @@ static int __devinit twlreg_probe(struct platform_device *pdev)
 		break;
 	}
 
-	rdev = regulator_register(&info->desc, &pdev->dev, initdata, info,
-							pdev->dev.of_node);
+	config.dev = &pdev->dev;
+	config.init_data = initdata;
+	config.driver_data = info;
+	config.of_node = pdev->dev.of_node;
+
+	rdev = regulator_register(&info->desc, &config);
 	if (IS_ERR(rdev)) {
 		dev_err(&pdev->dev, "can't register %s, %ld\n",
 				info->desc.name, PTR_ERR(rdev));
diff --git a/drivers/regulator/userspace-consumer.c b/drivers/regulator/userspace-consumer.c
index 518667ef9a0..a7c8deb5f28 100644
--- a/drivers/regulator/userspace-consumer.c
+++ b/drivers/regulator/userspace-consumer.c
@@ -115,7 +115,9 @@ static int regulator_userspace_consumer_probe(struct platform_device *pdev)
 	if (!pdata)
 		return -EINVAL;
 
-	drvdata = kzalloc(sizeof(struct userspace_consumer_data), GFP_KERNEL);
+	drvdata = devm_kzalloc(&pdev->dev,
+			       sizeof(struct userspace_consumer_data),
+			       GFP_KERNEL);
 	if (drvdata == NULL)
 		return -ENOMEM;
 
@@ -125,16 +127,16 @@ static int regulator_userspace_consumer_probe(struct platform_device *pdev)
 
 	mutex_init(&drvdata->lock);
 
-	ret = regulator_bulk_get(&pdev->dev, drvdata->num_supplies,
-				 drvdata->supplies);
+	ret = devm_regulator_bulk_get(&pdev->dev, drvdata->num_supplies,
+				      drvdata->supplies);
 	if (ret) {
 		dev_err(&pdev->dev, "Failed to get supplies: %d\n", ret);
-		goto err_alloc_supplies;
+		return ret;
 	}
 
 	ret = sysfs_create_group(&pdev->dev.kobj, &attr_group);
 	if (ret != 0)
-		goto err_create_attrs;
+		return ret;
 
 	if (pdata->init_on) {
 		ret = regulator_bulk_enable(drvdata->num_supplies,
@@ -154,11 +156,6 @@ static int regulator_userspace_consumer_probe(struct platform_device *pdev)
 err_enable:
 	sysfs_remove_group(&pdev->dev.kobj, &attr_group);
 
-err_create_attrs:
-	regulator_bulk_free(drvdata->num_supplies, drvdata->supplies);
-
-err_alloc_supplies:
-	kfree(drvdata);
 	return ret;
 }
 
@@ -171,9 +168,6 @@ static int regulator_userspace_consumer_remove(struct platform_device *pdev)
 	if (data->enabled)
 		regulator_bulk_disable(data->num_supplies, data->supplies);
 
-	regulator_bulk_free(data->num_supplies, data->supplies);
-	kfree(data);
-
 	return 0;
 }
 
diff --git a/drivers/regulator/virtual.c b/drivers/regulator/virtual.c
index ee0b161c998..c038e742253 100644
--- a/drivers/regulator/virtual.c
+++ b/drivers/regulator/virtual.c
@@ -121,7 +121,7 @@ static ssize_t set_min_uV(struct device *dev, struct device_attribute *attr,
 	struct virtual_consumer_data *data = dev_get_drvdata(dev);
 	long val;
 
-	if (strict_strtol(buf, 10, &val) != 0)
+	if (kstrtol(buf, 10, &val) != 0)
 		return count;
 
 	mutex_lock(&data->lock);
@@ -147,7 +147,7 @@ static ssize_t set_max_uV(struct device *dev, struct device_attribute *attr,
 	struct virtual_consumer_data *data = dev_get_drvdata(dev);
 	long val;
 
-	if (strict_strtol(buf, 10, &val) != 0)
+	if (kstrtol(buf, 10, &val) != 0)
 		return count;
 
 	mutex_lock(&data->lock);
@@ -173,7 +173,7 @@ static ssize_t set_min_uA(struct device *dev, struct device_attribute *attr,
 	struct virtual_consumer_data *data = dev_get_drvdata(dev);
 	long val;
 
-	if (strict_strtol(buf, 10, &val) != 0)
+	if (kstrtol(buf, 10, &val) != 0)
 		return count;
 
 	mutex_lock(&data->lock);
@@ -199,7 +199,7 @@ static ssize_t set_max_uA(struct device *dev, struct device_attribute *attr,
 	struct virtual_consumer_data *data = dev_get_drvdata(dev);
 	long val;
 
-	if (strict_strtol(buf, 10, &val) != 0)
+	if (kstrtol(buf, 10, &val) != 0)
 		return count;
 
 	mutex_lock(&data->lock);
@@ -291,18 +291,19 @@ static int __devinit regulator_virtual_probe(struct platform_device *pdev)
 	struct virtual_consumer_data *drvdata;
 	int ret;
 
-	drvdata = kzalloc(sizeof(struct virtual_consumer_data), GFP_KERNEL);
+	drvdata = devm_kzalloc(&pdev->dev, sizeof(struct virtual_consumer_data),
+			       GFP_KERNEL);
 	if (drvdata == NULL)
 		return -ENOMEM;
 
 	mutex_init(&drvdata->lock);
 
-	drvdata->regulator = regulator_get(&pdev->dev, reg_id);
+	drvdata->regulator = devm_regulator_get(&pdev->dev, reg_id);
 	if (IS_ERR(drvdata->regulator)) {
 		ret = PTR_ERR(drvdata->regulator);
 		dev_err(&pdev->dev, "Failed to obtain supply '%s': %d\n",
 			reg_id, ret);
-		goto err;
+		return ret;
 	}
 
 	ret = sysfs_create_group(&pdev->dev.kobj,
@@ -310,7 +311,7 @@ static int __devinit regulator_virtual_probe(struct platform_device *pdev)
 	if (ret != 0) {
 		dev_err(&pdev->dev,
 			"Failed to create attribute group: %d\n", ret);
-		goto err_regulator;
+		return ret;
 	}
 
 	drvdata->mode = regulator_get_mode(drvdata->regulator);
@@ -318,12 +319,6 @@ static int __devinit regulator_virtual_probe(struct platform_device *pdev)
 	platform_set_drvdata(pdev, drvdata);
 
 	return 0;
-
-err_regulator:
-	regulator_put(drvdata->regulator);
-err:
-	kfree(drvdata);
-	return ret;
 }
 
 static int __devexit regulator_virtual_remove(struct platform_device *pdev)
@@ -334,9 +329,6 @@ static int __devexit regulator_virtual_remove(struct platform_device *pdev)
 
 	if (drvdata->enabled)
 		regulator_disable(drvdata->regulator);
-	regulator_put(drvdata->regulator);
-
-	kfree(drvdata);
 
 	platform_set_drvdata(pdev, NULL);
 
diff --git a/drivers/regulator/wm831x-dcdc.c b/drivers/regulator/wm831x-dcdc.c
index ff810e787ea..099da11e989 100644
--- a/drivers/regulator/wm831x-dcdc.c
+++ b/drivers/regulator/wm831x-dcdc.c
@@ -35,7 +35,7 @@
 #define WM831X_DCDC_MODE_IDLE    2
 #define WM831X_DCDC_MODE_STANDBY 3
 
-#define WM831X_DCDC_MAX_NAME 6
+#define WM831X_DCDC_MAX_NAME 9
 
 /* Register offsets in control block */
 #define WM831X_DCDC_CONTROL_1     0
@@ -50,6 +50,7 @@
 
 struct wm831x_dcdc {
 	char name[WM831X_DCDC_MAX_NAME];
+	char supply_name[WM831X_DCDC_MAX_NAME];
 	struct regulator_desc desc;
 	int base;
 	struct wm831x *wm831x;
@@ -60,41 +61,6 @@ struct wm831x_dcdc {
 	int dvs_vsel;
 };
 
-static int wm831x_dcdc_is_enabled(struct regulator_dev *rdev)
-{
-	struct wm831x_dcdc *dcdc = rdev_get_drvdata(rdev);
-	struct wm831x *wm831x = dcdc->wm831x;
-	int mask = 1 << rdev_get_id(rdev);
-	int reg;
-
-	reg = wm831x_reg_read(wm831x, WM831X_DCDC_ENABLE);
-	if (reg < 0)
-		return reg;
-
-	if (reg & mask)
-		return 1;
-	else
-		return 0;
-}
-
-static int wm831x_dcdc_enable(struct regulator_dev *rdev)
-{
-	struct wm831x_dcdc *dcdc = rdev_get_drvdata(rdev);
-	struct wm831x *wm831x = dcdc->wm831x;
-	int mask = 1 << rdev_get_id(rdev);
-
-	return wm831x_set_bits(wm831x, WM831X_DCDC_ENABLE, mask, mask);
-}
-
-static int wm831x_dcdc_disable(struct regulator_dev *rdev)
-{
-	struct wm831x_dcdc *dcdc = rdev_get_drvdata(rdev);
-	struct wm831x *wm831x = dcdc->wm831x;
-	int mask = 1 << rdev_get_id(rdev);
-
-	return wm831x_set_bits(wm831x, WM831X_DCDC_ENABLE, mask, 0);
-}
-
 static unsigned int wm831x_dcdc_get_mode(struct regulator_dev *rdev)
 
 {
@@ -414,9 +380,9 @@ static struct regulator_ops wm831x_buckv_ops = {
 	.set_current_limit = wm831x_buckv_set_current_limit,
 	.get_current_limit = wm831x_buckv_get_current_limit,
 
-	.is_enabled = wm831x_dcdc_is_enabled,
-	.enable = wm831x_dcdc_enable,
-	.disable = wm831x_dcdc_disable,
+	.is_enabled = regulator_is_enabled_regmap,
+	.enable = regulator_enable_regmap,
+	.disable = regulator_disable_regmap,
 	.get_status = wm831x_dcdc_get_status,
 	.get_mode = wm831x_dcdc_get_mode,
 	.set_mode = wm831x_dcdc_set_mode,
@@ -437,23 +403,17 @@ static __devinit void wm831x_buckv_dvs_init(struct wm831x_dcdc *dcdc,
 	if (!pdata || !pdata->dvs_gpio)
 		return;
 
-	ret = gpio_request(pdata->dvs_gpio, "DCDC DVS");
-	if (ret < 0) {
-		dev_err(wm831x->dev, "Failed to get %s DVS GPIO: %d\n",
-			dcdc->name, ret);
-		return;
-	}
-
 	/* gpiolib won't let us read the GPIO status so pick the higher
 	 * of the two existing voltages so we take it as platform data.
 	 */
 	dcdc->dvs_gpio_state = pdata->dvs_init_state;
 
-	ret = gpio_direction_output(pdata->dvs_gpio, dcdc->dvs_gpio_state);
+	ret = gpio_request_one(pdata->dvs_gpio,
+			       dcdc->dvs_gpio_state ? GPIOF_INIT_HIGH : 0,
+			       "DCDC DVS");
 	if (ret < 0) {
-		dev_err(wm831x->dev, "Failed to enable %s DVS GPIO: %d\n",
+		dev_err(wm831x->dev, "Failed to get %s DVS GPIO: %d\n",
 			dcdc->name, ret);
-		gpio_free(pdata->dvs_gpio);
 		return;
 	}
 
@@ -498,6 +458,7 @@ static __devinit int wm831x_buckv_probe(struct platform_device *pdev)
 {
 	struct wm831x *wm831x = dev_get_drvdata(pdev->dev.parent);
 	struct wm831x_pdata *pdata = wm831x->dev->platform_data;
+	struct regulator_config config = { };
 	int id;
 	struct wm831x_dcdc *dcdc;
 	struct resource *res;
@@ -511,9 +472,6 @@ static __devinit int wm831x_buckv_probe(struct platform_device *pdev)
 
 	dev_dbg(&pdev->dev, "Probing DCDC%d\n", id + 1);
 
-	if (pdata == NULL || pdata->dcdc[id] == NULL)
-		return -ENODEV;
-
 	dcdc = devm_kzalloc(&pdev->dev,  sizeof(struct wm831x_dcdc),
 			    GFP_KERNEL);
 	if (dcdc == NULL) {
@@ -533,11 +491,18 @@ static __devinit int wm831x_buckv_probe(struct platform_device *pdev)
 
 	snprintf(dcdc->name, sizeof(dcdc->name), "DCDC%d", id + 1);
 	dcdc->desc.name = dcdc->name;
+
+	snprintf(dcdc->supply_name, sizeof(dcdc->supply_name),
+		 "DC%dVDD", id + 1);
+	dcdc->desc.supply_name = dcdc->supply_name;
+
 	dcdc->desc.id = id;
 	dcdc->desc.type = REGULATOR_VOLTAGE;
 	dcdc->desc.n_voltages = WM831X_BUCKV_MAX_SELECTOR + 1;
 	dcdc->desc.ops = &wm831x_buckv_ops;
 	dcdc->desc.owner = THIS_MODULE;
+	dcdc->desc.enable_reg = WM831X_DCDC_ENABLE;
+	dcdc->desc.enable_mask = 1 << id;
 
 	ret = wm831x_reg_read(wm831x, dcdc->base + WM831X_DCDC_ON_CONFIG);
 	if (ret < 0) {
@@ -553,11 +518,16 @@ static __devinit int wm831x_buckv_probe(struct platform_device *pdev)
 	}
 	dcdc->dvs_vsel = ret & WM831X_DC1_DVS_VSEL_MASK;
 
-	if (pdata->dcdc[id])
+	if (pdata && pdata->dcdc[id])
 		wm831x_buckv_dvs_init(dcdc, pdata->dcdc[id]->driver_data);
 
-	dcdc->regulator = regulator_register(&dcdc->desc, &pdev->dev,
-					     pdata->dcdc[id], dcdc, NULL);
+	config.dev = pdev->dev.parent;
+	if (pdata)
+		config.init_data = pdata->dcdc[id];
+	config.driver_data = dcdc;
+	config.regmap = wm831x->regmap;
+
+	dcdc->regulator = regulator_register(&dcdc->desc, &config);
 	if (IS_ERR(dcdc->regulator)) {
 		ret = PTR_ERR(dcdc->regulator);
 		dev_err(wm831x->dev, "Failed to register DCDC%d: %d\n",
@@ -565,7 +535,7 @@ static __devinit int wm831x_buckv_probe(struct platform_device *pdev)
 		goto err;
 	}
 
-	irq = platform_get_irq_byname(pdev, "UV");
+	irq = wm831x_irq(wm831x, platform_get_irq_byname(pdev, "UV"));
 	ret = request_threaded_irq(irq, NULL, wm831x_dcdc_uv_irq,
 				   IRQF_TRIGGER_RISING, dcdc->name, dcdc);
 	if (ret != 0) {
@@ -574,7 +544,7 @@ static __devinit int wm831x_buckv_probe(struct platform_device *pdev)
 		goto err_regulator;
 	}
 
-	irq = platform_get_irq_byname(pdev, "HC");
+	irq = wm831x_irq(wm831x, platform_get_irq_byname(pdev, "HC"));
 	ret = request_threaded_irq(irq, NULL, wm831x_dcdc_oc_irq,
 				   IRQF_TRIGGER_RISING, dcdc->name, dcdc);
 	if (ret != 0) {
@@ -588,7 +558,8 @@ static __devinit int wm831x_buckv_probe(struct platform_device *pdev)
 	return 0;
 
 err_uv:
-	free_irq(platform_get_irq_byname(pdev, "UV"), dcdc);
+	free_irq(wm831x_irq(wm831x, platform_get_irq_byname(pdev, "UV")),
+		 dcdc);
 err_regulator:
 	regulator_unregister(dcdc->regulator);
 err:
@@ -600,11 +571,14 @@ err:
 static __devexit int wm831x_buckv_remove(struct platform_device *pdev)
 {
 	struct wm831x_dcdc *dcdc = platform_get_drvdata(pdev);
+	struct wm831x *wm831x = dcdc->wm831x;
 
 	platform_set_drvdata(pdev, NULL);
 
-	free_irq(platform_get_irq_byname(pdev, "HC"), dcdc);
-	free_irq(platform_get_irq_byname(pdev, "UV"), dcdc);
+	free_irq(wm831x_irq(wm831x, platform_get_irq_byname(pdev, "HC")),
+			    dcdc);
+	free_irq(wm831x_irq(wm831x, platform_get_irq_byname(pdev, "UV")),
+			    dcdc);
 	regulator_unregister(dcdc->regulator);
 	if (dcdc->dvs_gpio)
 		gpio_free(dcdc->dvs_gpio);
@@ -675,29 +649,15 @@ static int wm831x_buckp_set_suspend_voltage(struct regulator_dev *rdev,
 	return wm831x_buckp_set_voltage_int(rdev, reg, uV, uV, &selector);
 }
 
-static int wm831x_buckp_get_voltage_sel(struct regulator_dev *rdev)
-{
-	struct wm831x_dcdc *dcdc = rdev_get_drvdata(rdev);
-	struct wm831x *wm831x = dcdc->wm831x;
-	u16 reg = dcdc->base + WM831X_DCDC_ON_CONFIG;
-	int val;
-
-	val = wm831x_reg_read(wm831x, reg);
-	if (val < 0)
-		return val;
-
-	return val & WM831X_DC3_ON_VSEL_MASK;
-}
-
 static struct regulator_ops wm831x_buckp_ops = {
 	.set_voltage = wm831x_buckp_set_voltage,
-	.get_voltage_sel = wm831x_buckp_get_voltage_sel,
+	.get_voltage_sel = regulator_get_voltage_sel_regmap,
 	.list_voltage = wm831x_buckp_list_voltage,
 	.set_suspend_voltage = wm831x_buckp_set_suspend_voltage,
 
-	.is_enabled = wm831x_dcdc_is_enabled,
-	.enable = wm831x_dcdc_enable,
-	.disable = wm831x_dcdc_disable,
+	.is_enabled = regulator_is_enabled_regmap,
+	.enable = regulator_enable_regmap,
+	.disable = regulator_disable_regmap,
 	.get_status = wm831x_dcdc_get_status,
 	.get_mode = wm831x_dcdc_get_mode,
 	.set_mode = wm831x_dcdc_set_mode,
@@ -708,6 +668,7 @@ static __devinit int wm831x_buckp_probe(struct platform_device *pdev)
 {
 	struct wm831x *wm831x = dev_get_drvdata(pdev->dev.parent);
 	struct wm831x_pdata *pdata = wm831x->dev->platform_data;
+	struct regulator_config config = { };
 	int id;
 	struct wm831x_dcdc *dcdc;
 	struct resource *res;
@@ -721,9 +682,6 @@ static __devinit int wm831x_buckp_probe(struct platform_device *pdev)
 
 	dev_dbg(&pdev->dev, "Probing DCDC%d\n", id + 1);
 
-	if (pdata == NULL || pdata->dcdc[id] == NULL)
-		return -ENODEV;
-
 	dcdc = devm_kzalloc(&pdev->dev, sizeof(struct wm831x_dcdc),
 			    GFP_KERNEL);
 	if (dcdc == NULL) {
@@ -743,14 +701,28 @@ static __devinit int wm831x_buckp_probe(struct platform_device *pdev)
 
 	snprintf(dcdc->name, sizeof(dcdc->name), "DCDC%d", id + 1);
 	dcdc->desc.name = dcdc->name;
+
+	snprintf(dcdc->supply_name, sizeof(dcdc->supply_name),
+		 "DC%dVDD", id + 1);
+	dcdc->desc.supply_name = dcdc->supply_name;
+
 	dcdc->desc.id = id;
 	dcdc->desc.type = REGULATOR_VOLTAGE;
 	dcdc->desc.n_voltages = WM831X_BUCKP_MAX_SELECTOR + 1;
 	dcdc->desc.ops = &wm831x_buckp_ops;
 	dcdc->desc.owner = THIS_MODULE;
-
-	dcdc->regulator = regulator_register(&dcdc->desc, &pdev->dev,
-					     pdata->dcdc[id], dcdc, NULL);
+	dcdc->desc.vsel_reg = dcdc->base + WM831X_DCDC_ON_CONFIG;
+	dcdc->desc.vsel_mask = WM831X_DC3_ON_VSEL_MASK;
+	dcdc->desc.enable_reg = WM831X_DCDC_ENABLE;
+	dcdc->desc.enable_mask = 1 << id;
+
+	config.dev = pdev->dev.parent;
+	if (pdata)
+		config.init_data = pdata->dcdc[id];
+	config.driver_data = dcdc;
+	config.regmap = wm831x->regmap;
+
+	dcdc->regulator = regulator_register(&dcdc->desc, &config);
 	if (IS_ERR(dcdc->regulator)) {
 		ret = PTR_ERR(dcdc->regulator);
 		dev_err(wm831x->dev, "Failed to register DCDC%d: %d\n",
@@ -758,7 +730,7 @@ static __devinit int wm831x_buckp_probe(struct platform_device *pdev)
 		goto err;
 	}
 
-	irq = platform_get_irq_byname(pdev, "UV");
+	irq = wm831x_irq(wm831x, platform_get_irq_byname(pdev, "UV"));
 	ret = request_threaded_irq(irq, NULL, wm831x_dcdc_uv_irq,
 				   IRQF_TRIGGER_RISING,	dcdc->name, dcdc);
 	if (ret != 0) {
@@ -783,7 +755,8 @@ static __devexit int wm831x_buckp_remove(struct platform_device *pdev)
 
 	platform_set_drvdata(pdev, NULL);
 
-	free_irq(platform_get_irq_byname(pdev, "UV"), dcdc);
+	free_irq(wm831x_irq(dcdc->wm831x, platform_get_irq_byname(pdev, "UV")),
+			    dcdc);
 	regulator_unregister(dcdc->regulator);
 
 	return 0;
@@ -832,15 +805,16 @@ static int wm831x_boostp_get_status(struct regulator_dev *rdev)
 static struct regulator_ops wm831x_boostp_ops = {
 	.get_status = wm831x_boostp_get_status,
 
-	.is_enabled = wm831x_dcdc_is_enabled,
-	.enable = wm831x_dcdc_enable,
-	.disable = wm831x_dcdc_disable,
+	.is_enabled = regulator_is_enabled_regmap,
+	.enable = regulator_enable_regmap,
+	.disable = regulator_disable_regmap,
 };
 
 static __devinit int wm831x_boostp_probe(struct platform_device *pdev)
 {
 	struct wm831x *wm831x = dev_get_drvdata(pdev->dev.parent);
 	struct wm831x_pdata *pdata = wm831x->dev->platform_data;
+	struct regulator_config config = { };
 	int id = pdev->id % ARRAY_SIZE(pdata->dcdc);
 	struct wm831x_dcdc *dcdc;
 	struct resource *res;
@@ -851,7 +825,7 @@ static __devinit int wm831x_boostp_probe(struct platform_device *pdev)
 	if (pdata == NULL || pdata->dcdc[id] == NULL)
 		return -ENODEV;
 
-	dcdc = kzalloc(sizeof(struct wm831x_dcdc), GFP_KERNEL);
+	dcdc = devm_kzalloc(&pdev->dev, sizeof(struct wm831x_dcdc), GFP_KERNEL);
 	if (dcdc == NULL) {
 		dev_err(&pdev->dev, "Unable to allocate private data\n");
 		return -ENOMEM;
@@ -873,9 +847,16 @@ static __devinit int wm831x_boostp_probe(struct platform_device *pdev)
 	dcdc->desc.type = REGULATOR_VOLTAGE;
 	dcdc->desc.ops = &wm831x_boostp_ops;
 	dcdc->desc.owner = THIS_MODULE;
+	dcdc->desc.enable_reg = WM831X_DCDC_ENABLE;
+	dcdc->desc.enable_mask = 1 << id;
 
-	dcdc->regulator = regulator_register(&dcdc->desc, &pdev->dev,
-					     pdata->dcdc[id], dcdc, NULL);
+	config.dev = pdev->dev.parent;
+	if (pdata)
+		config.init_data = pdata->dcdc[id];
+	config.driver_data = dcdc;
+	config.regmap = wm831x->regmap;
+
+	dcdc->regulator = regulator_register(&dcdc->desc, &config);
 	if (IS_ERR(dcdc->regulator)) {
 		ret = PTR_ERR(dcdc->regulator);
 		dev_err(wm831x->dev, "Failed to register DCDC%d: %d\n",
@@ -883,7 +864,7 @@ static __devinit int wm831x_boostp_probe(struct platform_device *pdev)
 		goto err;
 	}
 
-	irq = platform_get_irq_byname(pdev, "UV");
+	irq = wm831x_irq(wm831x, platform_get_irq_byname(pdev, "UV"));
 	ret = request_threaded_irq(irq, NULL, wm831x_dcdc_uv_irq,
 				   IRQF_TRIGGER_RISING, dcdc->name,
 				   dcdc);
@@ -900,7 +881,6 @@ static __devinit int wm831x_boostp_probe(struct platform_device *pdev)
 err_regulator:
 	regulator_unregister(dcdc->regulator);
 err:
-	kfree(dcdc);
 	return ret;
 }
 
@@ -910,9 +890,9 @@ static __devexit int wm831x_boostp_remove(struct platform_device *pdev)
 
 	platform_set_drvdata(pdev, NULL);
 
-	free_irq(platform_get_irq_byname(pdev, "UV"), dcdc);
+	free_irq(wm831x_irq(dcdc->wm831x, platform_get_irq_byname(pdev, "UV")),
+		 dcdc);
 	regulator_unregister(dcdc->regulator);
-	kfree(dcdc);
 
 	return 0;
 }
@@ -936,9 +916,9 @@ static struct platform_driver wm831x_boostp_driver = {
 #define WM831X_EPE_BASE 6
 
 static struct regulator_ops wm831x_epe_ops = {
-	.is_enabled = wm831x_dcdc_is_enabled,
-	.enable = wm831x_dcdc_enable,
-	.disable = wm831x_dcdc_disable,
+	.is_enabled = regulator_is_enabled_regmap,
+	.enable = regulator_enable_regmap,
+	.disable = regulator_disable_regmap,
 	.get_status = wm831x_dcdc_get_status,
 };
 
@@ -946,16 +926,14 @@ static __devinit int wm831x_epe_probe(struct platform_device *pdev)
 {
 	struct wm831x *wm831x = dev_get_drvdata(pdev->dev.parent);
 	struct wm831x_pdata *pdata = wm831x->dev->platform_data;
+	struct regulator_config config = { };
 	int id = pdev->id % ARRAY_SIZE(pdata->epe);
 	struct wm831x_dcdc *dcdc;
 	int ret;
 
 	dev_dbg(&pdev->dev, "Probing EPE%d\n", id + 1);
 
-	if (pdata == NULL || pdata->epe[id] == NULL)
-		return -ENODEV;
-
-	dcdc = kzalloc(sizeof(struct wm831x_dcdc), GFP_KERNEL);
+	dcdc = devm_kzalloc(&pdev->dev, sizeof(struct wm831x_dcdc), GFP_KERNEL);
 	if (dcdc == NULL) {
 		dev_err(&pdev->dev, "Unable to allocate private data\n");
 		return -ENOMEM;
@@ -972,9 +950,16 @@ static __devinit int wm831x_epe_probe(struct platform_device *pdev)
 	dcdc->desc.ops = &wm831x_epe_ops;
 	dcdc->desc.type = REGULATOR_VOLTAGE;
 	dcdc->desc.owner = THIS_MODULE;
+	dcdc->desc.enable_reg = WM831X_DCDC_ENABLE;
+	dcdc->desc.enable_mask = 1 << dcdc->desc.id;
+
+	config.dev = pdev->dev.parent;
+	if (pdata)
+		config.init_data = pdata->epe[id];
+	config.driver_data = dcdc;
+	config.regmap = wm831x->regmap;
 
-	dcdc->regulator = regulator_register(&dcdc->desc, &pdev->dev,
-					     pdata->epe[id], dcdc, NULL);
+	dcdc->regulator = regulator_register(&dcdc->desc, &config);
 	if (IS_ERR(dcdc->regulator)) {
 		ret = PTR_ERR(dcdc->regulator);
 		dev_err(wm831x->dev, "Failed to register EPE%d: %d\n",
@@ -987,7 +972,6 @@ static __devinit int wm831x_epe_probe(struct platform_device *pdev)
 	return 0;
 
 err:
-	kfree(dcdc);
 	return ret;
 }
 
@@ -996,9 +980,7 @@ static __devexit int wm831x_epe_remove(struct platform_device *pdev)
 	struct wm831x_dcdc *dcdc = platform_get_drvdata(pdev);
 
 	platform_set_drvdata(pdev, NULL);
-
 	regulator_unregister(dcdc->regulator);
-	kfree(dcdc);
 
 	return 0;
 }
diff --git a/drivers/regulator/wm831x-isink.c b/drivers/regulator/wm831x-isink.c
index b414e09c562..0d207c29771 100644
--- a/drivers/regulator/wm831x-isink.c
+++ b/drivers/regulator/wm831x-isink.c
@@ -154,6 +154,7 @@ static __devinit int wm831x_isink_probe(struct platform_device *pdev)
 	struct wm831x_pdata *pdata = wm831x->dev->platform_data;
 	struct wm831x_isink *isink;
 	int id = pdev->id % ARRAY_SIZE(pdata->isink);
+	struct regulator_config config = { };
 	struct resource *res;
 	int ret, irq;
 
@@ -189,8 +190,11 @@ static __devinit int wm831x_isink_probe(struct platform_device *pdev)
 	isink->desc.type = REGULATOR_CURRENT;
 	isink->desc.owner = THIS_MODULE;
 
-	isink->regulator = regulator_register(&isink->desc, &pdev->dev,
-					     pdata->isink[id], isink, NULL);
+	config.dev = pdev->dev.parent;
+	config.init_data = pdata->isink[id];
+	config.driver_data = isink;
+
+	isink->regulator = regulator_register(&isink->desc, &config);
 	if (IS_ERR(isink->regulator)) {
 		ret = PTR_ERR(isink->regulator);
 		dev_err(wm831x->dev, "Failed to register ISINK%d: %d\n",
@@ -198,7 +202,7 @@ static __devinit int wm831x_isink_probe(struct platform_device *pdev)
 		goto err;
 	}
 
-	irq = platform_get_irq(pdev, 0);
+	irq = wm831x_irq(wm831x, platform_get_irq(pdev, 0));
 	ret = request_threaded_irq(irq, NULL, wm831x_isink_irq,
 				   IRQF_TRIGGER_RISING, isink->name, isink);
 	if (ret != 0) {
@@ -223,7 +227,7 @@ static __devexit int wm831x_isink_remove(struct platform_device *pdev)
 
 	platform_set_drvdata(pdev, NULL);
 
-	free_irq(platform_get_irq(pdev, 0), isink);
+	free_irq(wm831x_irq(isink->wm831x, platform_get_irq(pdev, 0)), isink);
 
 	regulator_unregister(isink->regulator);
 
diff --git a/drivers/regulator/wm831x-ldo.c b/drivers/regulator/wm831x-ldo.c
index 641e9f6499d..a9a28d8ac18 100644
--- a/drivers/regulator/wm831x-ldo.c
+++ b/drivers/regulator/wm831x-ldo.c
@@ -25,7 +25,7 @@
 #include <linux/mfd/wm831x/regulator.h>
 #include <linux/mfd/wm831x/pdata.h>
 
-#define WM831X_LDO_MAX_NAME 6
+#define WM831X_LDO_MAX_NAME 9
 
 #define WM831X_LDO_CONTROL       0
 #define WM831X_LDO_ON_CONTROL    1
@@ -36,6 +36,7 @@
 
 struct wm831x_ldo {
 	char name[WM831X_LDO_MAX_NAME];
+	char supply_name[WM831X_LDO_MAX_NAME];
 	struct regulator_desc desc;
 	int base;
 	struct wm831x *wm831x;
@@ -46,41 +47,6 @@ struct wm831x_ldo {
  * Shared
  */
 
-static int wm831x_ldo_is_enabled(struct regulator_dev *rdev)
-{
-	struct wm831x_ldo *ldo = rdev_get_drvdata(rdev);
-	struct wm831x *wm831x = ldo->wm831x;
-	int mask = 1 << rdev_get_id(rdev);
-	int reg;
-
-	reg = wm831x_reg_read(wm831x, WM831X_LDO_ENABLE);
-	if (reg < 0)
-		return reg;
-
-	if (reg & mask)
-		return 1;
-	else
-		return 0;
-}
-
-static int wm831x_ldo_enable(struct regulator_dev *rdev)
-{
-	struct wm831x_ldo *ldo = rdev_get_drvdata(rdev);
-	struct wm831x *wm831x = ldo->wm831x;
-	int mask = 1 << rdev_get_id(rdev);
-
-	return wm831x_set_bits(wm831x, WM831X_LDO_ENABLE, mask, mask);
-}
-
-static int wm831x_ldo_disable(struct regulator_dev *rdev)
-{
-	struct wm831x_ldo *ldo = rdev_get_drvdata(rdev);
-	struct wm831x *wm831x = ldo->wm831x;
-	int mask = 1 << rdev_get_id(rdev);
-
-	return wm831x_set_bits(wm831x, WM831X_LDO_ENABLE, mask, 0);
-}
-
 static irqreturn_t wm831x_ldo_uv_irq(int irq, void *data)
 {
 	struct wm831x_ldo *ldo = data;
@@ -105,7 +71,7 @@ static int wm831x_gp_ldo_list_voltage(struct regulator_dev *rdev,
 	/* 0.9-1.6V in 50mV steps */
 	if (selector <= WM831X_GP_LDO_SELECTOR_LOW)
 		return 900000 + (selector * 50000);
-	/* 1.7-3.3V in 50mV steps */
+	/* 1.7-3.3V in 100mV steps */
 	if (selector <= WM831X_GP_LDO_MAX_SELECTOR)
 		return 1600000 + ((selector - WM831X_GP_LDO_SELECTOR_LOW)
 				  * 100000);
@@ -160,22 +126,6 @@ static int wm831x_gp_ldo_set_suspend_voltage(struct regulator_dev *rdev,
 	return wm831x_gp_ldo_set_voltage_int(rdev, reg, uV, uV, &selector);
 }
 
-static int wm831x_gp_ldo_get_voltage_sel(struct regulator_dev *rdev)
-{
-	struct wm831x_ldo *ldo = rdev_get_drvdata(rdev);
-	struct wm831x *wm831x = ldo->wm831x;
-	int reg = ldo->base + WM831X_LDO_ON_CONTROL;
-	int ret;
-
-	ret = wm831x_reg_read(wm831x, reg);
-	if (ret < 0)
-		return ret;
-
-	ret &= WM831X_LDO1_ON_VSEL_MASK;
-
-	return ret;
-}
-
 static unsigned int wm831x_gp_ldo_get_mode(struct regulator_dev *rdev)
 {
 	struct wm831x_ldo *ldo = rdev_get_drvdata(rdev);
@@ -293,7 +243,7 @@ static unsigned int wm831x_gp_ldo_get_optimum_mode(struct regulator_dev *rdev,
 
 static struct regulator_ops wm831x_gp_ldo_ops = {
 	.list_voltage = wm831x_gp_ldo_list_voltage,
-	.get_voltage_sel = wm831x_gp_ldo_get_voltage_sel,
+	.get_voltage_sel = regulator_get_voltage_sel_regmap,
 	.set_voltage = wm831x_gp_ldo_set_voltage,
 	.set_suspend_voltage = wm831x_gp_ldo_set_suspend_voltage,
 	.get_mode = wm831x_gp_ldo_get_mode,
@@ -301,15 +251,16 @@ static struct regulator_ops wm831x_gp_ldo_ops = {
 	.get_status = wm831x_gp_ldo_get_status,
 	.get_optimum_mode = wm831x_gp_ldo_get_optimum_mode,
 
-	.is_enabled = wm831x_ldo_is_enabled,
-	.enable = wm831x_ldo_enable,
-	.disable = wm831x_ldo_disable,
+	.is_enabled = regulator_is_enabled_regmap,
+	.enable = regulator_enable_regmap,
+	.disable = regulator_disable_regmap,
 };
 
 static __devinit int wm831x_gp_ldo_probe(struct platform_device *pdev)
 {
 	struct wm831x *wm831x = dev_get_drvdata(pdev->dev.parent);
 	struct wm831x_pdata *pdata = wm831x->dev->platform_data;
+	struct regulator_config config = { };
 	int id;
 	struct wm831x_ldo *ldo;
 	struct resource *res;
@@ -323,9 +274,6 @@ static __devinit int wm831x_gp_ldo_probe(struct platform_device *pdev)
 
 	dev_dbg(&pdev->dev, "Probing LDO%d\n", id + 1);
 
-	if (pdata == NULL || pdata->ldo[id] == NULL)
-		return -ENODEV;
-
 	ldo = devm_kzalloc(&pdev->dev, sizeof(struct wm831x_ldo), GFP_KERNEL);
 	if (ldo == NULL) {
 		dev_err(&pdev->dev, "Unable to allocate private data\n");
@@ -344,14 +292,28 @@ static __devinit int wm831x_gp_ldo_probe(struct platform_device *pdev)
 
 	snprintf(ldo->name, sizeof(ldo->name), "LDO%d", id + 1);
 	ldo->desc.name = ldo->name;
+
+	snprintf(ldo->supply_name, sizeof(ldo->supply_name),
+		 "LDO%dVDD", id + 1);
+	ldo->desc.supply_name = ldo->supply_name;
+
 	ldo->desc.id = id;
 	ldo->desc.type = REGULATOR_VOLTAGE;
 	ldo->desc.n_voltages = WM831X_GP_LDO_MAX_SELECTOR + 1;
 	ldo->desc.ops = &wm831x_gp_ldo_ops;
 	ldo->desc.owner = THIS_MODULE;
-
-	ldo->regulator = regulator_register(&ldo->desc, &pdev->dev,
-					     pdata->ldo[id], ldo, NULL);
+	ldo->desc.vsel_reg = ldo->base + WM831X_LDO_ON_CONTROL;
+	ldo->desc.vsel_mask = WM831X_LDO1_ON_VSEL_MASK;
+	ldo->desc.enable_reg = WM831X_LDO_ENABLE;
+	ldo->desc.enable_mask = 1 << id;
+
+	config.dev = pdev->dev.parent;
+	if (pdata)
+		config.init_data = pdata->ldo[id];
+	config.driver_data = ldo;
+	config.regmap = wm831x->regmap;
+
+	ldo->regulator = regulator_register(&ldo->desc, &config);
 	if (IS_ERR(ldo->regulator)) {
 		ret = PTR_ERR(ldo->regulator);
 		dev_err(wm831x->dev, "Failed to register LDO%d: %d\n",
@@ -359,7 +321,7 @@ static __devinit int wm831x_gp_ldo_probe(struct platform_device *pdev)
 		goto err;
 	}
 
-	irq = platform_get_irq_byname(pdev, "UV");
+	irq = wm831x_irq(wm831x, platform_get_irq_byname(pdev, "UV"));
 	ret = request_threaded_irq(irq, NULL, wm831x_ldo_uv_irq,
 				   IRQF_TRIGGER_RISING, ldo->name,
 				   ldo);
@@ -385,7 +347,8 @@ static __devexit int wm831x_gp_ldo_remove(struct platform_device *pdev)
 
 	platform_set_drvdata(pdev, NULL);
 
-	free_irq(platform_get_irq_byname(pdev, "UV"), ldo);
+	free_irq(wm831x_irq(ldo->wm831x,
+			    platform_get_irq_byname(pdev, "UV")), ldo);
 	regulator_unregister(ldo->regulator);
 
 	return 0;
@@ -414,7 +377,7 @@ static int wm831x_aldo_list_voltage(struct regulator_dev *rdev,
 	/* 1-1.6V in 50mV steps */
 	if (selector <= WM831X_ALDO_SELECTOR_LOW)
 		return 1000000 + (selector * 50000);
-	/* 1.7-3.5V in 50mV steps */
+	/* 1.7-3.5V in 100mV steps */
 	if (selector <= WM831X_ALDO_MAX_SELECTOR)
 		return 1600000 + ((selector - WM831X_ALDO_SELECTOR_LOW)
 				  * 100000);
@@ -468,22 +431,6 @@ static int wm831x_aldo_set_suspend_voltage(struct regulator_dev *rdev,
 	return wm831x_aldo_set_voltage_int(rdev, reg, uV, uV, &selector);
 }
 
-static int wm831x_aldo_get_voltage_sel(struct regulator_dev *rdev)
-{
-	struct wm831x_ldo *ldo = rdev_get_drvdata(rdev);
-	struct wm831x *wm831x = ldo->wm831x;
-	int reg = ldo->base + WM831X_LDO_ON_CONTROL;
-	int ret;
-
-	ret = wm831x_reg_read(wm831x, reg);
-	if (ret < 0)
-		return ret;
-
-	ret &= WM831X_LDO7_ON_VSEL_MASK;
-
-	return ret;
-}
-
 static unsigned int wm831x_aldo_get_mode(struct regulator_dev *rdev)
 {
 	struct wm831x_ldo *ldo = rdev_get_drvdata(rdev);
@@ -559,22 +506,23 @@ static int wm831x_aldo_get_status(struct regulator_dev *rdev)
 
 static struct regulator_ops wm831x_aldo_ops = {
 	.list_voltage = wm831x_aldo_list_voltage,
-	.get_voltage_sel = wm831x_aldo_get_voltage_sel,
+	.get_voltage_sel = regulator_get_voltage_sel_regmap,
 	.set_voltage = wm831x_aldo_set_voltage,
 	.set_suspend_voltage = wm831x_aldo_set_suspend_voltage,
 	.get_mode = wm831x_aldo_get_mode,
 	.set_mode = wm831x_aldo_set_mode,
 	.get_status = wm831x_aldo_get_status,
 
-	.is_enabled = wm831x_ldo_is_enabled,
-	.enable = wm831x_ldo_enable,
-	.disable = wm831x_ldo_disable,
+	.is_enabled = regulator_is_enabled_regmap,
+	.enable = regulator_enable_regmap,
+	.disable = regulator_disable_regmap,
 };
 
 static __devinit int wm831x_aldo_probe(struct platform_device *pdev)
 {
 	struct wm831x *wm831x = dev_get_drvdata(pdev->dev.parent);
 	struct wm831x_pdata *pdata = wm831x->dev->platform_data;
+	struct regulator_config config = { };
 	int id;
 	struct wm831x_ldo *ldo;
 	struct resource *res;
@@ -588,9 +536,6 @@ static __devinit int wm831x_aldo_probe(struct platform_device *pdev)
 
 	dev_dbg(&pdev->dev, "Probing LDO%d\n", id + 1);
 
-	if (pdata == NULL || pdata->ldo[id] == NULL)
-		return -ENODEV;
-
 	ldo = devm_kzalloc(&pdev->dev, sizeof(struct wm831x_ldo), GFP_KERNEL);
 	if (ldo == NULL) {
 		dev_err(&pdev->dev, "Unable to allocate private data\n");
@@ -609,14 +554,28 @@ static __devinit int wm831x_aldo_probe(struct platform_device *pdev)
 
 	snprintf(ldo->name, sizeof(ldo->name), "LDO%d", id + 1);
 	ldo->desc.name = ldo->name;
+
+	snprintf(ldo->supply_name, sizeof(ldo->supply_name),
+		 "LDO%dVDD", id + 1);
+	ldo->desc.supply_name = ldo->supply_name;
+
 	ldo->desc.id = id;
 	ldo->desc.type = REGULATOR_VOLTAGE;
 	ldo->desc.n_voltages = WM831X_ALDO_MAX_SELECTOR + 1;
 	ldo->desc.ops = &wm831x_aldo_ops;
 	ldo->desc.owner = THIS_MODULE;
-
-	ldo->regulator = regulator_register(&ldo->desc, &pdev->dev,
-					     pdata->ldo[id], ldo, NULL);
+	ldo->desc.vsel_reg = ldo->base + WM831X_LDO_ON_CONTROL;
+	ldo->desc.vsel_mask = WM831X_LDO7_ON_VSEL_MASK;
+	ldo->desc.enable_reg = WM831X_LDO_ENABLE;
+	ldo->desc.enable_mask = 1 << id;
+
+	config.dev = pdev->dev.parent;
+	if (pdata)
+		config.init_data = pdata->ldo[id];
+	config.driver_data = ldo;
+	config.regmap = wm831x->regmap;
+
+	ldo->regulator = regulator_register(&ldo->desc, &config);
 	if (IS_ERR(ldo->regulator)) {
 		ret = PTR_ERR(ldo->regulator);
 		dev_err(wm831x->dev, "Failed to register LDO%d: %d\n",
@@ -624,7 +583,7 @@ static __devinit int wm831x_aldo_probe(struct platform_device *pdev)
 		goto err;
 	}
 
-	irq = platform_get_irq_byname(pdev, "UV");
+	irq = wm831x_irq(wm831x, platform_get_irq_byname(pdev, "UV"));
 	ret = request_threaded_irq(irq, NULL, wm831x_ldo_uv_irq,
 				   IRQF_TRIGGER_RISING, ldo->name, ldo);
 	if (ret != 0) {
@@ -647,7 +606,8 @@ static __devexit int wm831x_aldo_remove(struct platform_device *pdev)
 {
 	struct wm831x_ldo *ldo = platform_get_drvdata(pdev);
 
-	free_irq(platform_get_irq_byname(pdev, "UV"), ldo);
+	free_irq(wm831x_irq(ldo->wm831x, platform_get_irq_byname(pdev, "UV")),
+		 ldo);
 	regulator_unregister(ldo->regulator);
 
 	return 0;
@@ -668,15 +628,6 @@ static struct platform_driver wm831x_aldo_driver = {
 
 #define WM831X_ALIVE_LDO_MAX_SELECTOR 0xf
 
-static int wm831x_alive_ldo_list_voltage(struct regulator_dev *rdev,
-				      unsigned int selector)
-{
-	/* 0.8-1.55V in 50mV steps */
-	if (selector <= WM831X_ALIVE_LDO_MAX_SELECTOR)
-		return 800000 + (selector * 50000);
-	return -EINVAL;
-}
-
 static int wm831x_alive_ldo_set_voltage_int(struct regulator_dev *rdev,
 					    int reg,
 					    int min_uV, int max_uV,
@@ -688,7 +639,7 @@ static int wm831x_alive_ldo_set_voltage_int(struct regulator_dev *rdev,
 
 	vsel = (min_uV - 800000) / 50000;
 
-	ret = wm831x_alive_ldo_list_voltage(rdev, vsel);
+	ret = regulator_list_voltage_linear(rdev, vsel);
 	if (ret < 0)
 		return ret;
 	if (ret < min_uV || ret > max_uV)
@@ -720,22 +671,6 @@ static int wm831x_alive_ldo_set_suspend_voltage(struct regulator_dev *rdev,
 	return wm831x_alive_ldo_set_voltage_int(rdev, reg, uV, uV, &selector);
 }
 
-static int wm831x_alive_ldo_get_voltage_sel(struct regulator_dev *rdev)
-{
-	struct wm831x_ldo *ldo = rdev_get_drvdata(rdev);
-	struct wm831x *wm831x = ldo->wm831x;
-	int reg = ldo->base + WM831X_ALIVE_LDO_ON_CONTROL;
-	int ret;
-
-	ret = wm831x_reg_read(wm831x, reg);
-	if (ret < 0)
-		return ret;
-
-	ret &= WM831X_LDO11_ON_VSEL_MASK;
-
-	return ret;
-}
-
 static int wm831x_alive_ldo_get_status(struct regulator_dev *rdev)
 {
 	struct wm831x_ldo *ldo = rdev_get_drvdata(rdev);
@@ -754,21 +689,22 @@ static int wm831x_alive_ldo_get_status(struct regulator_dev *rdev)
 }
 
 static struct regulator_ops wm831x_alive_ldo_ops = {
-	.list_voltage = wm831x_alive_ldo_list_voltage,
-	.get_voltage_sel = wm831x_alive_ldo_get_voltage_sel,
+	.list_voltage = regulator_list_voltage_linear,
+	.get_voltage_sel = regulator_get_voltage_sel_regmap,
 	.set_voltage = wm831x_alive_ldo_set_voltage,
 	.set_suspend_voltage = wm831x_alive_ldo_set_suspend_voltage,
 	.get_status = wm831x_alive_ldo_get_status,
 
-	.is_enabled = wm831x_ldo_is_enabled,
-	.enable = wm831x_ldo_enable,
-	.disable = wm831x_ldo_disable,
+	.is_enabled = regulator_is_enabled_regmap,
+	.enable = regulator_enable_regmap,
+	.disable = regulator_disable_regmap,
 };
 
 static __devinit int wm831x_alive_ldo_probe(struct platform_device *pdev)
 {
 	struct wm831x *wm831x = dev_get_drvdata(pdev->dev.parent);
 	struct wm831x_pdata *pdata = wm831x->dev->platform_data;
+	struct regulator_config config = { };
 	int id;
 	struct wm831x_ldo *ldo;
 	struct resource *res;
@@ -783,9 +719,6 @@ static __devinit int wm831x_alive_ldo_probe(struct platform_device *pdev)
 
 	dev_dbg(&pdev->dev, "Probing LDO%d\n", id + 1);
 
-	if (pdata == NULL || pdata->ldo[id] == NULL)
-		return -ENODEV;
-
 	ldo = devm_kzalloc(&pdev->dev, sizeof(struct wm831x_ldo), GFP_KERNEL);
 	if (ldo == NULL) {
 		dev_err(&pdev->dev, "Unable to allocate private data\n");
@@ -804,14 +737,30 @@ static __devinit int wm831x_alive_ldo_probe(struct platform_device *pdev)
 
 	snprintf(ldo->name, sizeof(ldo->name), "LDO%d", id + 1);
 	ldo->desc.name = ldo->name;
+
+	snprintf(ldo->supply_name, sizeof(ldo->supply_name),
+		 "LDO%dVDD", id + 1);
+	ldo->desc.supply_name = ldo->supply_name;
+
 	ldo->desc.id = id;
 	ldo->desc.type = REGULATOR_VOLTAGE;
 	ldo->desc.n_voltages = WM831X_ALIVE_LDO_MAX_SELECTOR + 1;
 	ldo->desc.ops = &wm831x_alive_ldo_ops;
 	ldo->desc.owner = THIS_MODULE;
-
-	ldo->regulator = regulator_register(&ldo->desc, &pdev->dev,
-					     pdata->ldo[id], ldo, NULL);
+	ldo->desc.vsel_reg = ldo->base + WM831X_ALIVE_LDO_ON_CONTROL;
+	ldo->desc.vsel_mask = WM831X_LDO11_ON_VSEL_MASK;
+	ldo->desc.enable_reg = WM831X_LDO_ENABLE;
+	ldo->desc.enable_mask = 1 << id;
+	ldo->desc.min_uV = 800000;
+	ldo->desc.uV_step = 50000;
+
+	config.dev = pdev->dev.parent;
+	if (pdata)
+		config.init_data = pdata->ldo[id];
+	config.driver_data = ldo;
+	config.regmap = wm831x->regmap;
+
+	ldo->regulator = regulator_register(&ldo->desc, &config);
 	if (IS_ERR(ldo->regulator)) {
 		ret = PTR_ERR(ldo->regulator);
 		dev_err(wm831x->dev, "Failed to register LDO%d: %d\n",
diff --git a/drivers/regulator/wm8350-regulator.c b/drivers/regulator/wm8350-regulator.c
index 05ecfb87231..94e550dc70b 100644
--- a/drivers/regulator/wm8350-regulator.c
+++ b/drivers/regulator/wm8350-regulator.c
@@ -1269,7 +1269,7 @@ static struct regulator_ops wm8350_isink_ops = {
 	.enable_time = wm8350_isink_enable_time,
 };
 
-static struct regulator_desc wm8350_reg[NUM_WM8350_REGULATORS] = {
+static const struct regulator_desc wm8350_reg[NUM_WM8350_REGULATORS] = {
 	{
 		.name = "DCDC1",
 		.id = WM8350_DCDC_1,
@@ -1398,6 +1398,7 @@ static irqreturn_t pmic_uv_handler(int irq, void *data)
 static int wm8350_regulator_probe(struct platform_device *pdev)
 {
 	struct wm8350 *wm8350 = dev_get_drvdata(&pdev->dev);
+	struct regulator_config config = { };
 	struct regulator_dev *rdev;
 	int ret;
 	u16 val;
@@ -1425,10 +1426,12 @@ static int wm8350_regulator_probe(struct platform_device *pdev)
 		break;
 	}
 
+	config.dev = &pdev->dev;
+	config.init_data = pdev->dev.platform_data;
+	config.driver_data = dev_get_drvdata(&pdev->dev);
+
 	/* register regulator */
-	rdev = regulator_register(&wm8350_reg[pdev->id], &pdev->dev,
-				  pdev->dev.platform_data,
-				  dev_get_drvdata(&pdev->dev), NULL);
+	rdev = regulator_register(&wm8350_reg[pdev->id], &config);
 	if (IS_ERR(rdev)) {
 		dev_err(&pdev->dev, "failed to register %s\n",
 			wm8350_reg[pdev->id].name);
diff --git a/drivers/regulator/wm8400-regulator.c b/drivers/regulator/wm8400-regulator.c
index 8477153780b..69a2b7ce5e4 100644
--- a/drivers/regulator/wm8400-regulator.c
+++ b/drivers/regulator/wm8400-regulator.c
@@ -19,31 +19,6 @@
 #include <linux/regulator/driver.h>
 #include <linux/mfd/wm8400-private.h>
 
-static int wm8400_ldo_is_enabled(struct regulator_dev *dev)
-{
-	struct wm8400 *wm8400 = rdev_get_drvdata(dev);
-	u16 val;
-
-	val = wm8400_reg_read(wm8400, WM8400_LDO1_CONTROL + rdev_get_id(dev));
-	return (val & WM8400_LDO1_ENA) != 0;
-}
-
-static int wm8400_ldo_enable(struct regulator_dev *dev)
-{
-	struct wm8400 *wm8400 = rdev_get_drvdata(dev);
-
-	return wm8400_set_bits(wm8400, WM8400_LDO1_CONTROL + rdev_get_id(dev),
-			       WM8400_LDO1_ENA, WM8400_LDO1_ENA);
-}
-
-static int wm8400_ldo_disable(struct regulator_dev *dev)
-{
-	struct wm8400 *wm8400 = rdev_get_drvdata(dev);
-
-	return wm8400_set_bits(wm8400, WM8400_LDO1_CONTROL + rdev_get_id(dev),
-			       WM8400_LDO1_ENA, 0);
-}
-
 static int wm8400_ldo_list_voltage(struct regulator_dev *dev,
 				   unsigned selector)
 {
@@ -56,21 +31,9 @@ static int wm8400_ldo_list_voltage(struct regulator_dev *dev,
 		return 1600000 + ((selector - 14) * 100000);
 }
 
-static int wm8400_ldo_get_voltage_sel(struct regulator_dev *dev)
+static int wm8400_ldo_map_voltage(struct regulator_dev *dev,
+				  int min_uV, int max_uV)
 {
-	struct wm8400 *wm8400 = rdev_get_drvdata(dev);
-	u16 val;
-
-	val = wm8400_reg_read(wm8400, WM8400_LDO1_CONTROL + rdev_get_id(dev));
-	val &= WM8400_LDO1_VSEL_MASK;
-
-	return val;
-}
-
-static int wm8400_ldo_set_voltage(struct regulator_dev *dev,
-				  int min_uV, int max_uV, unsigned *selector)
-{
-	struct wm8400 *wm8400 = rdev_get_drvdata(dev);
 	u16 val;
 
 	if (min_uV < 900000 || min_uV > 3300000)
@@ -94,92 +57,19 @@ static int wm8400_ldo_set_voltage(struct regulator_dev *dev,
 		val += 0xf;
 	}
 
-	*selector = val;
-
-	return wm8400_set_bits(wm8400, WM8400_LDO1_CONTROL + rdev_get_id(dev),
-			       WM8400_LDO1_VSEL_MASK, val);
+	return val;
 }
 
 static struct regulator_ops wm8400_ldo_ops = {
-	.is_enabled = wm8400_ldo_is_enabled,
-	.enable = wm8400_ldo_enable,
-	.disable = wm8400_ldo_disable,
+	.is_enabled = regulator_is_enabled_regmap,
+	.enable = regulator_enable_regmap,
+	.disable = regulator_disable_regmap,
 	.list_voltage = wm8400_ldo_list_voltage,
-	.get_voltage_sel = wm8400_ldo_get_voltage_sel,
-	.set_voltage = wm8400_ldo_set_voltage,
+	.get_voltage_sel = regulator_get_voltage_sel_regmap,
+	.set_voltage_sel = regulator_set_voltage_sel_regmap,
+	.map_voltage = wm8400_ldo_map_voltage,
 };
 
-static int wm8400_dcdc_is_enabled(struct regulator_dev *dev)
-{
-	struct wm8400 *wm8400 = rdev_get_drvdata(dev);
-	int offset = (rdev_get_id(dev) - WM8400_DCDC1) * 2;
-	u16 val;
-
-	val = wm8400_reg_read(wm8400, WM8400_DCDC1_CONTROL_1 + offset);
-	return (val & WM8400_DC1_ENA) != 0;
-}
-
-static int wm8400_dcdc_enable(struct regulator_dev *dev)
-{
-	struct wm8400 *wm8400 = rdev_get_drvdata(dev);
-	int offset = (rdev_get_id(dev) - WM8400_DCDC1) * 2;
-
-	return wm8400_set_bits(wm8400, WM8400_DCDC1_CONTROL_1 + offset,
-			       WM8400_DC1_ENA, WM8400_DC1_ENA);
-}
-
-static int wm8400_dcdc_disable(struct regulator_dev *dev)
-{
-	struct wm8400 *wm8400 = rdev_get_drvdata(dev);
-	int offset = (rdev_get_id(dev) - WM8400_DCDC1) * 2;
-
-	return wm8400_set_bits(wm8400, WM8400_DCDC1_CONTROL_1 + offset,
-			       WM8400_DC1_ENA, 0);
-}
-
-static int wm8400_dcdc_list_voltage(struct regulator_dev *dev,
-				    unsigned selector)
-{
-	if (selector > WM8400_DC1_VSEL_MASK)
-		return -EINVAL;
-
-	return 850000 + (selector * 25000);
-}
-
-static int wm8400_dcdc_get_voltage_sel(struct regulator_dev *dev)
-{
-	struct wm8400 *wm8400 = rdev_get_drvdata(dev);
-	u16 val;
-	int offset = (rdev_get_id(dev) - WM8400_DCDC1) * 2;
-
-	val = wm8400_reg_read(wm8400, WM8400_DCDC1_CONTROL_1 + offset);
-	val &= WM8400_DC1_VSEL_MASK;
-
-	return val;
-}
-
-static int wm8400_dcdc_set_voltage(struct regulator_dev *dev,
-				   int min_uV, int max_uV, unsigned *selector)
-{
-	struct wm8400 *wm8400 = rdev_get_drvdata(dev);
-	u16 val;
-	int offset = (rdev_get_id(dev) - WM8400_DCDC1) * 2;
-
-	if (min_uV < 850000)
-		return -EINVAL;
-
-	val = DIV_ROUND_UP(min_uV - 850000, 25000);
-
-	if (850000 + (25000 * val) > max_uV)
-		return -EINVAL;
-	BUG_ON(850000 + (25000 * val) < min_uV);
-
-	*selector = val;
-
-	return wm8400_set_bits(wm8400, WM8400_DCDC1_CONTROL_1 + offset,
-			       WM8400_DC1_VSEL_MASK, val);
-}
-
 static unsigned int wm8400_dcdc_get_mode(struct regulator_dev *dev)
 {
 	struct wm8400 *wm8400 = rdev_get_drvdata(dev);
@@ -258,12 +148,12 @@ static unsigned int wm8400_dcdc_get_optimum_mode(struct regulator_dev *dev,
 }
 
 static struct regulator_ops wm8400_dcdc_ops = {
-	.is_enabled = wm8400_dcdc_is_enabled,
-	.enable = wm8400_dcdc_enable,
-	.disable = wm8400_dcdc_disable,
-	.list_voltage = wm8400_dcdc_list_voltage,
-	.get_voltage_sel = wm8400_dcdc_get_voltage_sel,
-	.set_voltage = wm8400_dcdc_set_voltage,
+	.is_enabled = regulator_is_enabled_regmap,
+	.enable = regulator_enable_regmap,
+	.disable = regulator_disable_regmap,
+	.list_voltage = regulator_list_voltage_linear,
+	.get_voltage_sel = regulator_get_voltage_sel_regmap,
+	.set_voltage_sel = regulator_set_voltage_sel_regmap,
 	.get_mode = wm8400_dcdc_get_mode,
 	.set_mode = wm8400_dcdc_set_mode,
 	.get_optimum_mode = wm8400_dcdc_get_optimum_mode,
@@ -274,7 +164,11 @@ static struct regulator_desc regulators[] = {
 		.name = "LDO1",
 		.id = WM8400_LDO1,
 		.ops = &wm8400_ldo_ops,
+		.enable_reg = WM8400_LDO1_CONTROL,
+		.enable_mask = WM8400_LDO1_ENA,
 		.n_voltages = WM8400_LDO1_VSEL_MASK + 1,
+		.vsel_reg = WM8400_LDO1_CONTROL,
+		.vsel_mask = WM8400_LDO1_VSEL_MASK,
 		.type = REGULATOR_VOLTAGE,
 		.owner = THIS_MODULE,
 	},
@@ -282,15 +176,23 @@ static struct regulator_desc regulators[] = {
 		.name = "LDO2",
 		.id = WM8400_LDO2,
 		.ops = &wm8400_ldo_ops,
+		.enable_reg = WM8400_LDO2_CONTROL,
+		.enable_mask = WM8400_LDO2_ENA,
 		.n_voltages = WM8400_LDO2_VSEL_MASK + 1,
 		.type = REGULATOR_VOLTAGE,
+		.vsel_reg = WM8400_LDO2_CONTROL,
+		.vsel_mask = WM8400_LDO2_VSEL_MASK,
 		.owner = THIS_MODULE,
 	},
 	{
 		.name = "LDO3",
 		.id = WM8400_LDO3,
 		.ops = &wm8400_ldo_ops,
+		.enable_reg = WM8400_LDO3_CONTROL,
+		.enable_mask = WM8400_LDO3_ENA,
 		.n_voltages = WM8400_LDO3_VSEL_MASK + 1,
+		.vsel_reg = WM8400_LDO3_CONTROL,
+		.vsel_mask = WM8400_LDO3_VSEL_MASK,
 		.type = REGULATOR_VOLTAGE,
 		.owner = THIS_MODULE,
 	},
@@ -298,7 +200,11 @@ static struct regulator_desc regulators[] = {
 		.name = "LDO4",
 		.id = WM8400_LDO4,
 		.ops = &wm8400_ldo_ops,
+		.enable_reg = WM8400_LDO4_CONTROL,
+		.enable_mask = WM8400_LDO4_ENA,
 		.n_voltages = WM8400_LDO4_VSEL_MASK + 1,
+		.vsel_reg = WM8400_LDO4_CONTROL,
+		.vsel_mask = WM8400_LDO4_VSEL_MASK,
 		.type = REGULATOR_VOLTAGE,
 		.owner = THIS_MODULE,
 	},
@@ -306,7 +212,13 @@ static struct regulator_desc regulators[] = {
 		.name = "DCDC1",
 		.id = WM8400_DCDC1,
 		.ops = &wm8400_dcdc_ops,
+		.enable_reg = WM8400_DCDC1_CONTROL_1,
+		.enable_mask = WM8400_DC1_ENA_MASK,
 		.n_voltages = WM8400_DC1_VSEL_MASK + 1,
+		.vsel_reg = WM8400_DCDC1_CONTROL_1,
+		.vsel_mask = WM8400_DC1_VSEL_MASK,
+		.min_uV = 850000,
+		.uV_step = 25000,
 		.type = REGULATOR_VOLTAGE,
 		.owner = THIS_MODULE,
 	},
@@ -314,7 +226,13 @@ static struct regulator_desc regulators[] = {
 		.name = "DCDC2",
 		.id = WM8400_DCDC2,
 		.ops = &wm8400_dcdc_ops,
+		.enable_reg = WM8400_DCDC2_CONTROL_1,
+		.enable_mask = WM8400_DC1_ENA_MASK,
 		.n_voltages = WM8400_DC2_VSEL_MASK + 1,
+		.vsel_reg = WM8400_DCDC2_CONTROL_1,
+		.vsel_mask = WM8400_DC2_VSEL_MASK,
+		.min_uV = 850000,
+		.uV_step = 25000,
 		.type = REGULATOR_VOLTAGE,
 		.owner = THIS_MODULE,
 	},
@@ -323,11 +241,15 @@ static struct regulator_desc regulators[] = {
 static int __devinit wm8400_regulator_probe(struct platform_device *pdev)
 {
 	struct wm8400 *wm8400 = container_of(pdev, struct wm8400, regulators[pdev->id]);
+	struct regulator_config config = { };
 	struct regulator_dev *rdev;
 
-	rdev = regulator_register(&regulators[pdev->id], &pdev->dev,
-				  pdev->dev.platform_data, wm8400, NULL);
+	config.dev = &pdev->dev;
+	config.init_data = pdev->dev.platform_data;
+	config.driver_data = wm8400;
+	config.regmap = wm8400->regmap;
 
+	rdev = regulator_register(&regulators[pdev->id], &config);
 	if (IS_ERR(rdev))
 		return PTR_ERR(rdev);
 
diff --git a/drivers/regulator/wm8994-regulator.c b/drivers/regulator/wm8994-regulator.c
index 75ed402d9f4..9a994316e63 100644
--- a/drivers/regulator/wm8994-regulator.c
+++ b/drivers/regulator/wm8994-regulator.c
@@ -86,36 +86,6 @@ static int wm8994_ldo1_list_voltage(struct regulator_dev *rdev,
 	return (selector * 100000) + 2400000;
 }
 
-static int wm8994_ldo1_get_voltage_sel(struct regulator_dev *rdev)
-{
-	struct wm8994_ldo *ldo = rdev_get_drvdata(rdev);
-	int val;
-
-	val = wm8994_reg_read(ldo->wm8994, WM8994_LDO_1);
-	if (val < 0)
-		return val;
-
-	return (val & WM8994_LDO1_VSEL_MASK) >> WM8994_LDO1_VSEL_SHIFT;
-}
-
-static int wm8994_ldo1_set_voltage(struct regulator_dev *rdev,
-				   int min_uV, int max_uV, unsigned *s)
-{
-	struct wm8994_ldo *ldo = rdev_get_drvdata(rdev);
-	int selector, v;
-
-	selector = (min_uV - 2400000) / 100000;
-	v = wm8994_ldo1_list_voltage(rdev, selector);
-	if (v < 0 || v > max_uV)
-		return -EINVAL;
-
-	*s = selector;
-	selector <<= WM8994_LDO1_VSEL_SHIFT;
-
-	return wm8994_set_bits(ldo->wm8994, WM8994_LDO_1,
-			       WM8994_LDO1_VSEL_MASK, selector);
-}
-
 static struct regulator_ops wm8994_ldo1_ops = {
 	.enable = wm8994_ldo_enable,
 	.disable = wm8994_ldo_disable,
@@ -123,8 +93,8 @@ static struct regulator_ops wm8994_ldo1_ops = {
 	.enable_time = wm8994_ldo_enable_time,
 
 	.list_voltage = wm8994_ldo1_list_voltage,
-	.get_voltage_sel = wm8994_ldo1_get_voltage_sel,
-	.set_voltage = wm8994_ldo1_set_voltage,
+	.get_voltage_sel = regulator_get_voltage_sel_regmap,
+	.set_voltage_sel = regulator_set_voltage_sel_regmap,
 };
 
 static int wm8994_ldo2_list_voltage(struct regulator_dev *rdev,
@@ -153,51 +123,6 @@ static int wm8994_ldo2_list_voltage(struct regulator_dev *rdev,
 	}
 }
 
-static int wm8994_ldo2_get_voltage_sel(struct regulator_dev *rdev)
-{
-	struct wm8994_ldo *ldo = rdev_get_drvdata(rdev);
-	int val;
-
-	val = wm8994_reg_read(ldo->wm8994, WM8994_LDO_2);
-	if (val < 0)
-		return val;
-
-	return (val & WM8994_LDO2_VSEL_MASK) >> WM8994_LDO2_VSEL_SHIFT;
-}
-
-static int wm8994_ldo2_set_voltage(struct regulator_dev *rdev,
-				   int min_uV, int max_uV, unsigned *s)
-{
-	struct wm8994_ldo *ldo = rdev_get_drvdata(rdev);
-	int selector, v;
-
-	switch (ldo->wm8994->type) {
-	case WM8994:
-		selector = (min_uV - 900000) / 100000;
-		break;
-	case WM8958:
-		selector = (min_uV - 1000000) / 100000;
-		break;
-	case WM1811:
-		selector = (min_uV - 950000) / 100000;
-		if (selector == 0)
-			selector = 1;
-		break;
-	default:
-		return -EINVAL;
-	}
-
-	v = wm8994_ldo2_list_voltage(rdev, selector);
-	if (v < 0 || v > max_uV)
-		return -EINVAL;
-
-	*s = selector;
-	selector <<= WM8994_LDO2_VSEL_SHIFT;
-
-	return wm8994_set_bits(ldo->wm8994, WM8994_LDO_2,
-			       WM8994_LDO2_VSEL_MASK, selector);
-}
-
 static struct regulator_ops wm8994_ldo2_ops = {
 	.enable = wm8994_ldo_enable,
 	.disable = wm8994_ldo_disable,
@@ -205,16 +130,18 @@ static struct regulator_ops wm8994_ldo2_ops = {
 	.enable_time = wm8994_ldo_enable_time,
 
 	.list_voltage = wm8994_ldo2_list_voltage,
-	.get_voltage_sel = wm8994_ldo2_get_voltage_sel,
-	.set_voltage = wm8994_ldo2_set_voltage,
+	.get_voltage_sel = regulator_get_voltage_sel_regmap,
+	.set_voltage_sel = regulator_set_voltage_sel_regmap,
 };
 
-static struct regulator_desc wm8994_ldo_desc[] = {
+static const struct regulator_desc wm8994_ldo_desc[] = {
 	{
 		.name = "LDO1",
 		.id = 1,
 		.type = REGULATOR_VOLTAGE,
 		.n_voltages = WM8994_LDO1_MAX_SELECTOR + 1,
+		.vsel_reg = WM8994_LDO_1,
+		.vsel_mask = WM8994_LDO1_VSEL_MASK,
 		.ops = &wm8994_ldo1_ops,
 		.owner = THIS_MODULE,
 	},
@@ -223,6 +150,8 @@ static struct regulator_desc wm8994_ldo_desc[] = {
 		.id = 2,
 		.type = REGULATOR_VOLTAGE,
 		.n_voltages = WM8994_LDO2_MAX_SELECTOR + 1,
+		.vsel_reg = WM8994_LDO_2,
+		.vsel_mask = WM8994_LDO2_VSEL_MASK,
 		.ops = &wm8994_ldo2_ops,
 		.owner = THIS_MODULE,
 	},
@@ -233,14 +162,12 @@ static __devinit int wm8994_ldo_probe(struct platform_device *pdev)
 	struct wm8994 *wm8994 = dev_get_drvdata(pdev->dev.parent);
 	struct wm8994_pdata *pdata = wm8994->dev->platform_data;
 	int id = pdev->id % ARRAY_SIZE(pdata->ldo);
+	struct regulator_config config = { };
 	struct wm8994_ldo *ldo;
 	int ret;
 
 	dev_dbg(&pdev->dev, "Probing LDO%d\n", id + 1);
 
-	if (!pdata)
-		return -ENODEV;
-
 	ldo = devm_kzalloc(&pdev->dev, sizeof(struct wm8994_ldo), GFP_KERNEL);
 	if (ldo == NULL) {
 		dev_err(&pdev->dev, "Unable to allocate private data\n");
@@ -252,24 +179,22 @@ static __devinit int wm8994_ldo_probe(struct platform_device *pdev)
 	if (pdata->ldo[id].enable && gpio_is_valid(pdata->ldo[id].enable)) {
 		ldo->enable = pdata->ldo[id].enable;
 
-		ret = gpio_request(ldo->enable, "WM8994 LDO enable");
+		ret = gpio_request_one(ldo->enable, 0, "WM8994 LDO enable");
 		if (ret < 0) {
 			dev_err(&pdev->dev, "Failed to get enable GPIO: %d\n",
 				ret);
 			goto err;
 		}
-
-		ret = gpio_direction_output(ldo->enable, ldo->is_enabled);
-		if (ret < 0) {
-			dev_err(&pdev->dev, "Failed to set GPIO up: %d\n",
-				ret);
-			goto err_gpio;
-		}
 	} else
 		ldo->is_enabled = true;
 
-	ldo->regulator = regulator_register(&wm8994_ldo_desc[id], &pdev->dev,
-					     pdata->ldo[id].init_data, ldo, NULL);
+	config.dev = wm8994->dev;
+	config.driver_data = ldo;
+	config.regmap = wm8994->regmap;
+	if (pdata)
+		config.init_data = pdata->ldo[id].init_data;
+
+	ldo->regulator = regulator_register(&wm8994_ldo_desc[id], &config);
 	if (IS_ERR(ldo->regulator)) {
 		ret = PTR_ERR(ldo->regulator);
 		dev_err(wm8994->dev, "Failed to register LDO%d: %d\n",
diff --git a/drivers/remoteproc/remoteproc_core.c b/drivers/remoteproc/remoteproc_core.c
index ee15c68fb51..d6f8adaa26e 100644
--- a/drivers/remoteproc/remoteproc_core.c
+++ b/drivers/remoteproc/remoteproc_core.c
@@ -354,7 +354,7 @@ static void __rproc_free_vrings(struct rproc_vdev *rvdev, int i)
 {
 	struct rproc *rproc = rvdev->rproc;
 
-	for (i--; i > 0; i--) {
+	for (i--; i >= 0; i--) {
 		struct rproc_vring *rvring = &rvdev->vring[i];
 		int size = PAGE_ALIGN(vring_size(rvring->len, rvring->align));
 
@@ -1105,8 +1105,7 @@ static void rproc_fw_config_virtio(const struct firmware *fw, void *context)
 		goto out;
 
 out:
-	if (fw)
-		release_firmware(fw);
+	release_firmware(fw);
 	/* allow rproc_unregister() contexts, if any, to proceed */
 	complete_all(&rproc->firmware_loading_complete);
 }
diff --git a/drivers/rtc/Kconfig b/drivers/rtc/Kconfig
index 8c8377d50c4..08cbdb900a1 100644
--- a/drivers/rtc/Kconfig
+++ b/drivers/rtc/Kconfig
@@ -620,27 +620,6 @@ config RTC_DRV_MSM6242
 	  This driver can also be built as a module. If so, the module
 	  will be called rtc-msm6242.
 
-config RTC_DRV_IMXDI
-	tristate "Freescale IMX DryIce Real Time Clock"
-	depends on ARCH_MX25
-	depends on RTC_CLASS
-	help
-	   Support for Freescale IMX DryIce RTC
-
-	   This driver can also be built as a module, if so, the module
-	   will be called "rtc-imxdi".
-
-config RTC_MXC
-	tristate "Freescale MXC Real Time Clock"
-	depends on ARCH_MXC
-	depends on RTC_CLASS
-	help
-	   If you say yes here you get support for the Freescale MXC
-	   RTC module.
-
-	   This driver can also be built as a module, if so, the module
-	   will be called "rtc-mxc".
-
 config RTC_DRV_BQ4802
 	tristate "TI BQ4802"
 	help
@@ -738,6 +717,16 @@ config RTC_DRV_DAVINCI
 	  This driver can also be built as a module. If so, the module
 	  will be called rtc-davinci.
 
+config RTC_DRV_IMXDI
+	tristate "Freescale IMX DryIce Real Time Clock"
+	depends on SOC_IMX25
+	depends on RTC_CLASS
+	help
+	   Support for Freescale IMX DryIce RTC
+
+	   This driver can also be built as a module, if so, the module
+	   will be called "rtc-imxdi".
+
 config RTC_DRV_OMAP
 	tristate "TI OMAP1"
 	depends on ARCH_OMAP15XX || ARCH_OMAP16XX || ARCH_OMAP730 || ARCH_DAVINCI_DA8XX
@@ -838,7 +827,7 @@ config RTC_DRV_AT32AP700X
 
 config RTC_DRV_AT91RM9200
 	tristate "AT91RM9200 or some AT91SAM9 RTC"
-	depends on ARCH_AT91RM9200 || ARCH_AT91SAM9RL || ARCH_AT91SAM9G45
+	depends on ARCH_AT91
 	help
 	  Driver for the internal RTC (Realtime Clock) module found on
 	  Atmel AT91RM9200's and some  AT91SAM9 chips. On AT91SAM9 chips
@@ -1087,4 +1076,15 @@ config RTC_DRV_LOONGSON1
 	  This driver can also be built as a module. If so, the module
 	  will be called rtc-ls1x.
 
+config RTC_DRV_MXC
+	tristate "Freescale MXC Real Time Clock"
+	depends on ARCH_MXC
+	depends on RTC_CLASS
+	help
+	   If you say yes here you get support for the Freescale MXC
+	   RTC module.
+
+	   This driver can also be built as a module, if so, the module
+	   will be called "rtc-mxc".
+
 endif # RTC_CLASS
diff --git a/drivers/rtc/Makefile b/drivers/rtc/Makefile
index 727ae7786e6..2973921c30d 100644
--- a/drivers/rtc/Makefile
+++ b/drivers/rtc/Makefile
@@ -61,7 +61,7 @@ obj-$(CONFIG_RTC_DRV_M41T94)	+= rtc-m41t94.o
 obj-$(CONFIG_RTC_DRV_M48T35)	+= rtc-m48t35.o
 obj-$(CONFIG_RTC_DRV_M48T59)	+= rtc-m48t59.o
 obj-$(CONFIG_RTC_DRV_M48T86)	+= rtc-m48t86.o
-obj-$(CONFIG_RTC_MXC)		+= rtc-mxc.o
+obj-$(CONFIG_RTC_DRV_MXC)	+= rtc-mxc.o
 obj-$(CONFIG_RTC_DRV_MAX6900)	+= rtc-max6900.o
 obj-$(CONFIG_RTC_DRV_MAX8925)	+= rtc-max8925.o
 obj-$(CONFIG_RTC_DRV_MAX8998)	+= rtc-max8998.o
diff --git a/drivers/rtc/rtc-ds1307.c b/drivers/rtc/rtc-ds1307.c
index c293d0cdb10..836710ce750 100644
--- a/drivers/rtc/rtc-ds1307.c
+++ b/drivers/rtc/rtc-ds1307.c
@@ -17,8 +17,7 @@
 #include <linux/string.h>
 #include <linux/rtc.h>
 #include <linux/bcd.h>
-
-
+#include <linux/rtc/ds1307.h>
 
 /*
  * We can't determine type by probing, but if we expect pre-Linux code
@@ -92,7 +91,8 @@ enum ds_type {
 #	define DS1337_BIT_A2I		0x02
 #	define DS1337_BIT_A1I		0x01
 #define DS1339_REG_ALARM1_SECS	0x07
-#define DS1339_REG_TRICKLE	0x10
+
+#define DS13XX_TRICKLE_CHARGER_MAGIC	0xa0
 
 #define RX8025_REG_CTRL1	0x0e
 #	define RX8025_BIT_2412		0x20
@@ -124,6 +124,7 @@ struct chip_desc {
 	unsigned		alarm:1;
 	u16			nvram_offset;
 	u16			nvram_size;
+	u16			trickle_charger_reg;
 };
 
 static const struct chip_desc chips[last_ds_type] = {
@@ -140,6 +141,13 @@ static const struct chip_desc chips[last_ds_type] = {
 	},
 	[ds_1339] = {
 		.alarm		= 1,
+		.trickle_charger_reg = 0x10,
+	},
+	[ds_1340] = {
+		.trickle_charger_reg = 0x08,
+	},
+	[ds_1388] = {
+		.trickle_charger_reg = 0x0a,
 	},
 	[ds_3231] = {
 		.alarm		= 1,
@@ -619,6 +627,7 @@ static int __devinit ds1307_probe(struct i2c_client *client,
 	struct i2c_adapter	*adapter = to_i2c_adapter(client->dev.parent);
 	int			want_irq = false;
 	unsigned char		*buf;
+	struct ds1307_platform_data *pdata = client->dev.platform_data;
 	static const int	bbsqi_bitpos[] = {
 		[ds_1337] = 0,
 		[ds_1339] = DS1339_BIT_BBSQI,
@@ -637,7 +646,10 @@ static int __devinit ds1307_probe(struct i2c_client *client,
 
 	ds1307->client	= client;
 	ds1307->type	= id->driver_data;
-	ds1307->offset	= 0;
+
+	if (pdata && pdata->trickle_charger_setup && chip->trickle_charger_reg)
+		i2c_smbus_write_byte_data(client, chip->trickle_charger_reg,
+			DS13XX_TRICKLE_CHARGER_MAGIC | pdata->trickle_charger_setup);
 
 	buf = ds1307->regs;
 	if (i2c_check_functionality(adapter, I2C_FUNC_SMBUS_I2C_BLOCK)) {
diff --git a/drivers/rtc/rtc-ep93xx.c b/drivers/rtc/rtc-ep93xx.c
index 14a42a1edc6..9602278ff98 100644
--- a/drivers/rtc/rtc-ep93xx.c
+++ b/drivers/rtc/rtc-ep93xx.c
@@ -127,7 +127,7 @@ static const struct attribute_group ep93xx_rtc_sysfs_files = {
 	.attrs	= ep93xx_rtc_attrs,
 };
 
-static int __init ep93xx_rtc_probe(struct platform_device *pdev)
+static int __devinit ep93xx_rtc_probe(struct platform_device *pdev)
 {
 	struct ep93xx_rtc *ep93xx_rtc;
 	struct resource *res;
@@ -174,7 +174,7 @@ exit:
 	return err;
 }
 
-static int __exit ep93xx_rtc_remove(struct platform_device *pdev)
+static int __devexit ep93xx_rtc_remove(struct platform_device *pdev)
 {
 	struct ep93xx_rtc *ep93xx_rtc = platform_get_drvdata(pdev);
 
@@ -186,31 +186,19 @@ static int __exit ep93xx_rtc_remove(struct platform_device *pdev)
 	return 0;
 }
 
-/* work with hotplug and coldplug */
-MODULE_ALIAS("platform:ep93xx-rtc");
-
 static struct platform_driver ep93xx_rtc_driver = {
 	.driver		= {
 		.name	= "ep93xx-rtc",
 		.owner	= THIS_MODULE,
 	},
-	.remove		= __exit_p(ep93xx_rtc_remove),
+	.probe		= ep93xx_rtc_probe,
+	.remove		= __devexit_p(ep93xx_rtc_remove),
 };
 
-static int __init ep93xx_rtc_init(void)
-{
-        return platform_driver_probe(&ep93xx_rtc_driver, ep93xx_rtc_probe);
-}
-
-static void __exit ep93xx_rtc_exit(void)
-{
-	platform_driver_unregister(&ep93xx_rtc_driver);
-}
+module_platform_driver(ep93xx_rtc_driver);
 
 MODULE_AUTHOR("Alessandro Zummo <a.zummo@towertech.it>");
 MODULE_DESCRIPTION("EP93XX RTC driver");
 MODULE_LICENSE("GPL");
 MODULE_VERSION(DRV_VERSION);
-
-module_init(ep93xx_rtc_init);
-module_exit(ep93xx_rtc_exit);
+MODULE_ALIAS("platform:ep93xx-rtc");
diff --git a/drivers/rtc/rtc-imxdi.c b/drivers/rtc/rtc-imxdi.c
index d93a9608b1f..891cd6c61d0 100644
--- a/drivers/rtc/rtc-imxdi.c
+++ b/drivers/rtc/rtc-imxdi.c
@@ -405,7 +405,7 @@ static int dryice_rtc_probe(struct platform_device *pdev)
 	imxdi->clk = clk_get(&pdev->dev, NULL);
 	if (IS_ERR(imxdi->clk))
 		return PTR_ERR(imxdi->clk);
-	clk_enable(imxdi->clk);
+	clk_prepare_enable(imxdi->clk);
 
 	/*
 	 * Initialize dryice hardware
@@ -470,7 +470,7 @@ static int dryice_rtc_probe(struct platform_device *pdev)
 	return 0;
 
 err:
-	clk_disable(imxdi->clk);
+	clk_disable_unprepare(imxdi->clk);
 	clk_put(imxdi->clk);
 
 	return rc;
@@ -487,7 +487,7 @@ static int __devexit dryice_rtc_remove(struct platform_device *pdev)
 
 	rtc_device_unregister(imxdi->rtc);
 
-	clk_disable(imxdi->clk);
+	clk_disable_unprepare(imxdi->clk);
 	clk_put(imxdi->clk);
 
 	return 0;
diff --git a/drivers/rtc/rtc-lpc32xx.c b/drivers/rtc/rtc-lpc32xx.c
index 63c72189c64..d5218553741 100644
--- a/drivers/rtc/rtc-lpc32xx.c
+++ b/drivers/rtc/rtc-lpc32xx.c
@@ -19,6 +19,7 @@
 #include <linux/rtc.h>
 #include <linux/slab.h>
 #include <linux/io.h>
+#include <linux/of.h>
 
 /*
  * Clock and Power control register offsets
@@ -386,13 +387,22 @@ static const struct dev_pm_ops lpc32xx_rtc_pm_ops = {
 #define LPC32XX_RTC_PM_OPS NULL
 #endif
 
+#ifdef CONFIG_OF
+static const struct of_device_id lpc32xx_rtc_match[] = {
+	{ .compatible = "nxp,lpc3220-rtc" },
+	{ }
+};
+MODULE_DEVICE_TABLE(of, lpc32xx_rtc_match);
+#endif
+
 static struct platform_driver lpc32xx_rtc_driver = {
 	.probe		= lpc32xx_rtc_probe,
 	.remove		= __devexit_p(lpc32xx_rtc_remove),
 	.driver = {
 		.name	= RTC_NAME,
 		.owner	= THIS_MODULE,
-		.pm	= LPC32XX_RTC_PM_OPS
+		.pm	= LPC32XX_RTC_PM_OPS,
+		.of_match_table = of_match_ptr(lpc32xx_rtc_match),
 	},
 };
 
diff --git a/drivers/rtc/rtc-m41t93.c b/drivers/rtc/rtc-m41t93.c
index 10f1c29436e..efab3d48cb1 100644
--- a/drivers/rtc/rtc-m41t93.c
+++ b/drivers/rtc/rtc-m41t93.c
@@ -48,6 +48,7 @@ static inline int m41t93_set_reg(struct spi_device *spi, u8 addr, u8 data)
 static int m41t93_set_time(struct device *dev, struct rtc_time *tm)
 {
 	struct spi_device *spi = to_spi_device(dev);
+	int tmp;
 	u8 buf[9] = {0x80};        /* write cmd + 8 data bytes */
 	u8 * const data = &buf[1]; /* ptr to first data byte */
 
@@ -62,6 +63,30 @@ static int m41t93_set_time(struct device *dev, struct rtc_time *tm)
 		return -EINVAL;
 	}
 
+	tmp = spi_w8r8(spi, M41T93_REG_FLAGS);
+	if (tmp < 0)
+		return tmp;
+
+	if (tmp & M41T93_FLAG_OF) {
+		dev_warn(&spi->dev, "OF bit is set, resetting.\n");
+		m41t93_set_reg(spi, M41T93_REG_FLAGS, tmp & ~M41T93_FLAG_OF);
+
+		tmp = spi_w8r8(spi, M41T93_REG_FLAGS);
+		if (tmp < 0) {
+			return tmp;
+		} else if (tmp & M41T93_FLAG_OF) {
+			/* OF cannot be immediately reset: oscillator has to be
+			 * restarted. */
+			u8 reset_osc = buf[M41T93_REG_ST_SEC] | M41T93_FLAG_ST;
+
+			dev_warn(&spi->dev,
+				 "OF bit is still set, kickstarting clock.\n");
+			m41t93_set_reg(spi, M41T93_REG_ST_SEC, reset_osc);
+			reset_osc &= ~M41T93_FLAG_ST;
+			m41t93_set_reg(spi, M41T93_REG_ST_SEC, reset_osc);
+		}
+	}
+
 	data[M41T93_REG_SSEC]		= 0;
 	data[M41T93_REG_ST_SEC]		= bin2bcd(tm->tm_sec);
 	data[M41T93_REG_MIN]		= bin2bcd(tm->tm_min);
@@ -89,10 +114,7 @@ static int m41t93_get_time(struct device *dev, struct rtc_time *tm)
 	   1. halt bit (HT) is set: the clock is running but update of readout
 	      registers has been disabled due to power failure. This is normal
 	      case after poweron. Time is valid after resetting HT bit.
-	   2. oscillator fail bit (OF) is set. Oscillator has be stopped and
-	      time is invalid:
-	      a) OF can be immeditely reset.
-	      b) OF cannot be immediately reset: oscillator has to be restarted.
+	   2. oscillator fail bit (OF) is set: time is invalid.
 	*/
 	tmp = spi_w8r8(spi, M41T93_REG_ALM_HOUR_HT);
 	if (tmp < 0)
@@ -110,21 +132,7 @@ static int m41t93_get_time(struct device *dev, struct rtc_time *tm)
 
 	if (tmp & M41T93_FLAG_OF) {
 		ret = -EINVAL;
-		dev_warn(&spi->dev, "OF bit is set, resetting.\n");
-		m41t93_set_reg(spi, M41T93_REG_FLAGS, tmp & ~M41T93_FLAG_OF);
-
-		tmp = spi_w8r8(spi, M41T93_REG_FLAGS);
-		if (tmp < 0)
-			return tmp;
-		else if (tmp & M41T93_FLAG_OF) {
-			u8 reset_osc = buf[M41T93_REG_ST_SEC] | M41T93_FLAG_ST;
-
-			dev_warn(&spi->dev,
-				 "OF bit is still set, kickstarting clock.\n");
-			m41t93_set_reg(spi, M41T93_REG_ST_SEC, reset_osc);
-			reset_osc &= ~M41T93_FLAG_ST;
-			m41t93_set_reg(spi, M41T93_REG_ST_SEC, reset_osc);
-		}
+		dev_warn(&spi->dev, "OF bit is set, write time to restart.\n");
 	}
 
 	if (tmp & M41T93_FLAG_BL)
diff --git a/drivers/rtc/rtc-pcf8563.c b/drivers/rtc/rtc-pcf8563.c
index bc0677de199..97a3284bb7c 100644
--- a/drivers/rtc/rtc-pcf8563.c
+++ b/drivers/rtc/rtc-pcf8563.c
@@ -64,6 +64,7 @@ struct pcf8563 {
 	 * 1970...2069.
 	 */
 	int c_polarity;	/* 0: MO_C=1 means 19xx, otherwise MO_C=1 means 20xx */
+	int voltage_low; /* incicates if a low_voltage was detected */
 };
 
 /*
@@ -86,9 +87,11 @@ static int pcf8563_get_datetime(struct i2c_client *client, struct rtc_time *tm)
 		return -EIO;
 	}
 
-	if (buf[PCF8563_REG_SC] & PCF8563_SC_LV)
+	if (buf[PCF8563_REG_SC] & PCF8563_SC_LV) {
+		pcf8563->voltage_low = 1;
 		dev_info(&client->dev,
 			"low voltage detected, date/time is not reliable.\n");
+	}
 
 	dev_dbg(&client->dev,
 		"%s: raw data is st1=%02x, st2=%02x, sec=%02x, min=%02x, hr=%02x, "
@@ -173,6 +176,44 @@ static int pcf8563_set_datetime(struct i2c_client *client, struct rtc_time *tm)
 	return 0;
 }
 
+#ifdef CONFIG_RTC_INTF_DEV
+static int pcf8563_rtc_ioctl(struct device *dev, unsigned int cmd, unsigned long arg)
+{
+	struct pcf8563 *pcf8563 = i2c_get_clientdata(to_i2c_client(dev));
+	struct rtc_time tm;
+
+	switch (cmd) {
+	case RTC_VL_READ:
+		if (pcf8563->voltage_low)
+			dev_info(dev, "low voltage detected, date/time is not reliable.\n");
+
+		if (copy_to_user((void __user *)arg, &pcf8563->voltage_low,
+					sizeof(int)))
+			return -EFAULT;
+		return 0;
+	case RTC_VL_CLR:
+		/*
+		 * Clear the VL bit in the seconds register in case
+		 * the time has not been set already (which would
+		 * have cleared it). This does not really matter
+		 * because of the cached voltage_low value but do it
+		 * anyway for consistency.
+		 */
+		if (pcf8563_get_datetime(to_i2c_client(dev), &tm))
+			pcf8563_set_datetime(to_i2c_client(dev), &tm);
+
+		/* Clear the cached value. */
+		pcf8563->voltage_low = 0;
+
+		return 0;
+	default:
+		return -ENOIOCTLCMD;
+	}
+}
+#else
+#define pcf8563_rtc_ioctl NULL
+#endif
+
 static int pcf8563_rtc_read_time(struct device *dev, struct rtc_time *tm)
 {
 	return pcf8563_get_datetime(to_i2c_client(dev), tm);
@@ -184,6 +225,7 @@ static int pcf8563_rtc_set_time(struct device *dev, struct rtc_time *tm)
 }
 
 static const struct rtc_class_ops pcf8563_rtc_ops = {
+	.ioctl		= pcf8563_rtc_ioctl,
 	.read_time	= pcf8563_rtc_read_time,
 	.set_time	= pcf8563_rtc_set_time,
 };
diff --git a/drivers/rtc/rtc-pl031.c b/drivers/rtc/rtc-pl031.c
index 684ef4bbfce..cc0533994f6 100644
--- a/drivers/rtc/rtc-pl031.c
+++ b/drivers/rtc/rtc-pl031.c
@@ -220,17 +220,9 @@ static irqreturn_t pl031_interrupt(int irq, void *dev_id)
 	unsigned long events = 0;
 
 	rtcmis = readl(ldata->base + RTC_MIS);
-	if (rtcmis) {
-		writel(rtcmis, ldata->base + RTC_ICR);
-
-		if (rtcmis & RTC_BIT_AI)
-			events |= (RTC_AF | RTC_IRQF);
-
-		/* Timer interrupt is only available in ST variants */
-		if ((rtcmis & RTC_BIT_PI) &&
-			(ldata->hw_designer == AMBA_VENDOR_ST))
-			events |= (RTC_PF | RTC_IRQF);
-
+	if (rtcmis & RTC_BIT_AI) {
+		writel(RTC_BIT_AI, ldata->base + RTC_ICR);
+		events |= (RTC_AF | RTC_IRQF);
 		rtc_update_irq(ldata->rtc, 1, events);
 
 		return IRQ_HANDLED;
@@ -312,6 +304,7 @@ static int pl031_probe(struct amba_device *adev, const struct amba_id *id)
 	int ret;
 	struct pl031_local *ldata;
 	struct rtc_class_ops *ops = id->data;
+	unsigned long time;
 
 	ret = amba_request_regions(adev, NULL);
 	if (ret)
@@ -343,6 +336,23 @@ static int pl031_probe(struct amba_device *adev, const struct amba_id *id)
 		writel(readl(ldata->base + RTC_CR) | RTC_CR_CWEN,
 		       ldata->base + RTC_CR);
 
+	/*
+	 * On ST PL031 variants, the RTC reset value does not provide correct
+	 * weekday for 2000-01-01. Correct the erroneous sunday to saturday.
+	 */
+	if (ldata->hw_designer == AMBA_VENDOR_ST) {
+		if (readl(ldata->base + RTC_YDR) == 0x2000) {
+			time = readl(ldata->base + RTC_DR);
+			if ((time &
+			     (RTC_MON_MASK | RTC_MDAY_MASK | RTC_WDAY_MASK))
+			    == 0x02120000) {
+				time = time | (0x7 << RTC_WDAY_SHIFT);
+				writel(0x2000, ldata->base + RTC_YLR);
+				writel(time, ldata->base + RTC_LR);
+			}
+		}
+	}
+
 	ldata->rtc = rtc_device_register("pl031", &adev->dev, ops,
 					THIS_MODULE);
 	if (IS_ERR(ldata->rtc)) {
diff --git a/drivers/rtc/rtc-s3c.c b/drivers/rtc/rtc-s3c.c
index 3f3a2975236..7e6af0b22f1 100644
--- a/drivers/rtc/rtc-s3c.c
+++ b/drivers/rtc/rtc-s3c.c
@@ -670,6 +670,7 @@ static int s3c_rtc_resume(struct platform_device *pdev)
 #define s3c_rtc_resume  NULL
 #endif
 
+#ifdef CONFIG_OF
 static struct s3c_rtc_drv_data s3c_rtc_drv_data_array[] = {
 	[TYPE_S3C2410] = { TYPE_S3C2410 },
 	[TYPE_S3C2416] = { TYPE_S3C2416 },
@@ -677,7 +678,6 @@ static struct s3c_rtc_drv_data s3c_rtc_drv_data_array[] = {
 	[TYPE_S3C64XX] = { TYPE_S3C64XX },
 };
 
-#ifdef CONFIG_OF
 static const struct of_device_id s3c_rtc_dt_match[] = {
 	{
 		.compatible = "samsung,s3c2410-rtc",
diff --git a/drivers/rtc/rtc-spear.c b/drivers/rtc/rtc-spear.c
index e38da0dc418..1f76320e545 100644
--- a/drivers/rtc/rtc-spear.c
+++ b/drivers/rtc/rtc-spear.c
@@ -16,6 +16,7 @@
 #include <linux/io.h>
 #include <linux/irq.h>
 #include <linux/module.h>
+#include <linux/of.h>
 #include <linux/platform_device.h>
 #include <linux/rtc.h>
 #include <linux/slab.h>
@@ -519,6 +520,14 @@ static void spear_rtc_shutdown(struct platform_device *pdev)
 	clk_disable(config->clk);
 }
 
+#ifdef CONFIG_OF
+static const struct of_device_id spear_rtc_id_table[] = {
+	{ .compatible = "st,spear600-rtc" },
+	{}
+};
+MODULE_DEVICE_TABLE(of, spear_rtc_id_table);
+#endif
+
 static struct platform_driver spear_rtc_driver = {
 	.probe = spear_rtc_probe,
 	.remove = __devexit_p(spear_rtc_remove),
@@ -527,6 +536,7 @@ static struct platform_driver spear_rtc_driver = {
 	.shutdown = spear_rtc_shutdown,
 	.driver = {
 		.name = "rtc-spear",
+		.of_match_table = of_match_ptr(spear_rtc_id_table),
 	},
 };
 
diff --git a/drivers/rtc/rtc-tegra.c b/drivers/rtc/rtc-tegra.c
index 75259fe3860..c006025cecc 100644
--- a/drivers/rtc/rtc-tegra.c
+++ b/drivers/rtc/rtc-tegra.c
@@ -309,7 +309,8 @@ static int __devinit tegra_rtc_probe(struct platform_device *pdev)
 	struct resource *res;
 	int ret;
 
-	info = kzalloc(sizeof(struct tegra_rtc_info), GFP_KERNEL);
+	info = devm_kzalloc(&pdev->dev, sizeof(struct tegra_rtc_info),
+		GFP_KERNEL);
 	if (!info)
 		return -ENOMEM;
 
@@ -317,29 +318,18 @@ static int __devinit tegra_rtc_probe(struct platform_device *pdev)
 	if (!res) {
 		dev_err(&pdev->dev,
 			"Unable to allocate resources for device.\n");
-		ret = -EBUSY;
-		goto err_free_info;
+		return -EBUSY;
 	}
 
-	if (!request_mem_region(res->start, resource_size(res), pdev->name)) {
-		dev_err(&pdev->dev,
-			"Unable to request mem region for device.\n");
-		ret = -EBUSY;
-		goto err_free_info;
+	info->rtc_base = devm_request_and_ioremap(&pdev->dev, res);
+	if (!info->rtc_base) {
+		dev_err(&pdev->dev, "Unable to request mem region and grab IOs for device.\n");
+		return -EBUSY;
 	}
 
 	info->tegra_rtc_irq = platform_get_irq(pdev, 0);
-	if (info->tegra_rtc_irq <= 0) {
-		ret = -EBUSY;
-		goto err_release_mem_region;
-	}
-
-	info->rtc_base = ioremap_nocache(res->start, resource_size(res));
-	if (!info->rtc_base) {
-		dev_err(&pdev->dev, "Unable to grab IOs for device.\n");
-		ret = -EBUSY;
-		goto err_release_mem_region;
-	}
+	if (info->tegra_rtc_irq <= 0)
+		return -EBUSY;
 
 	/* set context info. */
 	info->pdev = pdev;
@@ -362,11 +352,12 @@ static int __devinit tegra_rtc_probe(struct platform_device *pdev)
 		dev_err(&pdev->dev,
 			"Unable to register device (err=%d).\n",
 			ret);
-		goto err_iounmap;
+		return ret;
 	}
 
-	ret = request_irq(info->tegra_rtc_irq, tegra_rtc_irq_handler,
-		IRQF_TRIGGER_HIGH, "rtc alarm", &pdev->dev);
+	ret = devm_request_irq(&pdev->dev, info->tegra_rtc_irq,
+			tegra_rtc_irq_handler, IRQF_TRIGGER_HIGH,
+			"rtc alarm", &pdev->dev);
 	if (ret) {
 		dev_err(&pdev->dev,
 			"Unable to request interrupt for device (err=%d).\n",
@@ -380,12 +371,6 @@ static int __devinit tegra_rtc_probe(struct platform_device *pdev)
 
 err_dev_unreg:
 	rtc_device_unregister(info->rtc_dev);
-err_iounmap:
-	iounmap(info->rtc_base);
-err_release_mem_region:
-	release_mem_region(res->start, resource_size(res));
-err_free_info:
-	kfree(info);
 
 	return ret;
 }
@@ -393,17 +378,8 @@ err_free_info:
 static int __devexit tegra_rtc_remove(struct platform_device *pdev)
 {
 	struct tegra_rtc_info *info = platform_get_drvdata(pdev);
-	struct resource *res;
-
-	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
-	if (!res)
-		return -EBUSY;
 
-	free_irq(info->tegra_rtc_irq, &pdev->dev);
 	rtc_device_unregister(info->rtc_dev);
-	iounmap(info->rtc_base);
-	release_mem_region(res->start, resource_size(res));
-	kfree(info);
 
 	platform_set_drvdata(pdev, NULL);
 
diff --git a/drivers/rtc/rtc-wm831x.c b/drivers/rtc/rtc-wm831x.c
index 3b6e6a67e76..59c6245e042 100644
--- a/drivers/rtc/rtc-wm831x.c
+++ b/drivers/rtc/rtc-wm831x.c
@@ -396,7 +396,7 @@ static int wm831x_rtc_probe(struct platform_device *pdev)
 {
 	struct wm831x *wm831x = dev_get_drvdata(pdev->dev.parent);
 	struct wm831x_rtc *wm831x_rtc;
-	int alm_irq = platform_get_irq_byname(pdev, "ALM");
+	int alm_irq = wm831x_irq(wm831x, platform_get_irq_byname(pdev, "ALM"));
 	int ret = 0;
 
 	wm831x_rtc = devm_kzalloc(&pdev->dev, sizeof(*wm831x_rtc), GFP_KERNEL);
diff --git a/drivers/s390/char/con3215.c b/drivers/s390/char/con3215.c
index 4f9f1dcc155..6c0116d48c7 100644
--- a/drivers/s390/char/con3215.c
+++ b/drivers/s390/char/con3215.c
@@ -20,6 +20,7 @@
 #include <linux/interrupt.h>
 #include <linux/err.h>
 #include <linux/reboot.h>
+#include <linux/serial.h> /* ASYNC_* flags */
 #include <linux/slab.h>
 #include <asm/ccwdev.h>
 #include <asm/cio.h>
@@ -44,14 +45,11 @@
 #define RAW3215_TIMEOUT	    HZ/10     /* time for delayed output */
 
 #define RAW3215_FIXED	    1	      /* 3215 console device is not be freed */
-#define RAW3215_ACTIVE	    2	      /* set if the device is in use */
 #define RAW3215_WORKING	    4	      /* set if a request is being worked on */
 #define RAW3215_THROTTLED   8	      /* set if reading is disabled */
 #define RAW3215_STOPPED	    16	      /* set if writing is disabled */
-#define RAW3215_CLOSING	    32	      /* set while in close process */
 #define RAW3215_TIMER_RUNS  64	      /* set if the output delay timer is on */
 #define RAW3215_FLUSHING    128	      /* set to flush buffer (no delay) */
-#define RAW3215_FROZEN	    256	      /* set if 3215 is frozen for suspend */
 
 #define TAB_STOP_SIZE	    8	      /* tab stop size */
 
@@ -76,6 +74,7 @@ struct raw3215_req {
 } __attribute__ ((aligned(8)));
 
 struct raw3215_info {
+	struct tty_port port;
 	struct ccw_device *cdev;      /* device for tty driver */
 	spinlock_t *lock;	      /* pointer to irq lock */
 	int flags;		      /* state flags */
@@ -84,7 +83,6 @@ struct raw3215_info {
 	int head;		      /* first free byte in output buffer */
 	int count;		      /* number of bytes in output buffer */
 	int written;		      /* number of bytes in write requests */
-	struct tty_struct *tty;	      /* pointer to tty structure if present */
 	struct raw3215_req *queued_read; /* pointer to queued read requests */
 	struct raw3215_req *queued_write;/* pointer to queued write requests */
 	struct tasklet_struct tlet;   /* tasklet to invoke tty_wakeup */
@@ -293,7 +291,7 @@ static void raw3215_timeout(unsigned long __data)
 	if (raw->flags & RAW3215_TIMER_RUNS) {
 		del_timer(&raw->timer);
 		raw->flags &= ~RAW3215_TIMER_RUNS;
-		if (!(raw->flags & RAW3215_FROZEN)) {
+		if (!(raw->port.flags & ASYNC_SUSPENDED)) {
 			raw3215_mk_write_req(raw);
 			raw3215_start_io(raw);
 		}
@@ -309,7 +307,8 @@ static void raw3215_timeout(unsigned long __data)
  */
 static inline void raw3215_try_io(struct raw3215_info *raw)
 {
-	if (!(raw->flags & RAW3215_ACTIVE) || (raw->flags & RAW3215_FROZEN))
+	if (!(raw->port.flags & ASYNC_INITIALIZED) ||
+			(raw->port.flags & ASYNC_SUSPENDED))
 		return;
 	if (raw->queued_read != NULL)
 		raw3215_start_io(raw);
@@ -324,10 +323,7 @@ static inline void raw3215_try_io(struct raw3215_info *raw)
 			}
 		} else if (!(raw->flags & RAW3215_TIMER_RUNS)) {
 			/* delay small writes */
-			init_timer(&raw->timer);
 			raw->timer.expires = RAW3215_TIMEOUT + jiffies;
-			raw->timer.data = (unsigned long) raw;
-			raw->timer.function = raw3215_timeout;
 			add_timer(&raw->timer);
 			raw->flags |= RAW3215_TIMER_RUNS;
 		}
@@ -340,17 +336,21 @@ static inline void raw3215_try_io(struct raw3215_info *raw)
 static void raw3215_wakeup(unsigned long data)
 {
 	struct raw3215_info *raw = (struct raw3215_info *) data;
-	tty_wakeup(raw->tty);
+	struct tty_struct *tty;
+
+	tty = tty_port_tty_get(&raw->port);
+	tty_wakeup(tty);
+	tty_kref_put(tty);
 }
 
 /*
  * Try to start the next IO and wake up processes waiting on the tty.
  */
-static void raw3215_next_io(struct raw3215_info *raw)
+static void raw3215_next_io(struct raw3215_info *raw, struct tty_struct *tty)
 {
 	raw3215_mk_write_req(raw);
 	raw3215_try_io(raw);
-	if (raw->tty && RAW3215_BUFFER_SIZE - raw->count >= RAW3215_MIN_SPACE)
+	if (tty && RAW3215_BUFFER_SIZE - raw->count >= RAW3215_MIN_SPACE)
 		tasklet_schedule(&raw->tlet);
 }
 
@@ -368,10 +368,11 @@ static void raw3215_irq(struct ccw_device *cdev, unsigned long intparm,
 
 	raw = dev_get_drvdata(&cdev->dev);
 	req = (struct raw3215_req *) intparm;
+	tty = tty_port_tty_get(&raw->port);
 	cstat = irb->scsw.cmd.cstat;
 	dstat = irb->scsw.cmd.dstat;
 	if (cstat != 0)
-		raw3215_next_io(raw);
+		raw3215_next_io(raw, tty);
 	if (dstat & 0x01) { /* we got a unit exception */
 		dstat &= ~0x01;	 /* we can ignore it */
 	}
@@ -381,13 +382,13 @@ static void raw3215_irq(struct ccw_device *cdev, unsigned long intparm,
 			break;
 		/* Attention interrupt, someone hit the enter key */
 		raw3215_mk_read_req(raw);
-		raw3215_next_io(raw);
+		raw3215_next_io(raw, tty);
 		break;
 	case 0x08:
 	case 0x0C:
 		/* Channel end interrupt. */
 		if ((raw = req->info) == NULL)
-			return;		     /* That shouldn't happen ... */
+			goto put_tty;	     /* That shouldn't happen ... */
 		if (req->type == RAW3215_READ) {
 			/* store residual count, then wait for device end */
 			req->residual = irb->scsw.cmd.count;
@@ -397,11 +398,10 @@ static void raw3215_irq(struct ccw_device *cdev, unsigned long intparm,
 	case 0x04:
 		/* Device end interrupt. */
 		if ((raw = req->info) == NULL)
-			return;		     /* That shouldn't happen ... */
-		if (req->type == RAW3215_READ && raw->tty != NULL) {
+			goto put_tty;	     /* That shouldn't happen ... */
+		if (req->type == RAW3215_READ && tty != NULL) {
 			unsigned int cchar;
 
-			tty = raw->tty;
 			count = 160 - req->residual;
 			EBCASC(raw->inbuf, count);
 			cchar = ctrlchar_handle(raw->inbuf, count, tty);
@@ -411,7 +411,7 @@ static void raw3215_irq(struct ccw_device *cdev, unsigned long intparm,
 
 			case CTRLCHAR_CTRL:
 				tty_insert_flip_char(tty, cchar, TTY_NORMAL);
-				tty_flip_buffer_push(raw->tty);
+				tty_flip_buffer_push(tty);
 				break;
 
 			case CTRLCHAR_NONE:
@@ -424,7 +424,7 @@ static void raw3215_irq(struct ccw_device *cdev, unsigned long intparm,
 				} else
 					count -= 2;
 				tty_insert_flip_string(tty, raw->inbuf, count);
-				tty_flip_buffer_push(raw->tty);
+				tty_flip_buffer_push(tty);
 				break;
 			}
 		} else if (req->type == RAW3215_WRITE) {
@@ -439,7 +439,7 @@ static void raw3215_irq(struct ccw_device *cdev, unsigned long intparm,
 		    raw->queued_read == NULL) {
 			wake_up_interruptible(&raw->empty_wait);
 		}
-		raw3215_next_io(raw);
+		raw3215_next_io(raw, tty);
 		break;
 	default:
 		/* Strange interrupt, I'll do my best to clean up */
@@ -451,9 +451,10 @@ static void raw3215_irq(struct ccw_device *cdev, unsigned long intparm,
 			raw->flags &= ~RAW3215_WORKING;
 			raw3215_free_req(req);
 		}
-		raw3215_next_io(raw);
+		raw3215_next_io(raw, tty);
 	}
-	return;
+put_tty:
+	tty_kref_put(tty);
 }
 
 /*
@@ -487,7 +488,7 @@ static void raw3215_make_room(struct raw3215_info *raw, unsigned int length)
 		/* While console is frozen for suspend we have no other
 		 * choice but to drop message from the buffer to make
 		 * room for even more messages. */
-		if (raw->flags & RAW3215_FROZEN) {
+		if (raw->port.flags & ASYNC_SUSPENDED) {
 			raw3215_drop_line(raw);
 			continue;
 		}
@@ -609,10 +610,10 @@ static int raw3215_startup(struct raw3215_info *raw)
 {
 	unsigned long flags;
 
-	if (raw->flags & RAW3215_ACTIVE)
+	if (raw->port.flags & ASYNC_INITIALIZED)
 		return 0;
 	raw->line_pos = 0;
-	raw->flags |= RAW3215_ACTIVE;
+	raw->port.flags |= ASYNC_INITIALIZED;
 	spin_lock_irqsave(get_ccwdev_lock(raw->cdev), flags);
 	raw3215_try_io(raw);
 	spin_unlock_irqrestore(get_ccwdev_lock(raw->cdev), flags);
@@ -628,14 +629,15 @@ static void raw3215_shutdown(struct raw3215_info *raw)
 	DECLARE_WAITQUEUE(wait, current);
 	unsigned long flags;
 
-	if (!(raw->flags & RAW3215_ACTIVE) || (raw->flags & RAW3215_FIXED))
+	if (!(raw->port.flags & ASYNC_INITIALIZED) ||
+			(raw->flags & RAW3215_FIXED))
 		return;
 	/* Wait for outstanding requests, then free irq */
 	spin_lock_irqsave(get_ccwdev_lock(raw->cdev), flags);
 	if ((raw->flags & RAW3215_WORKING) ||
 	    raw->queued_write != NULL ||
 	    raw->queued_read != NULL) {
-		raw->flags |= RAW3215_CLOSING;
+		raw->port.flags |= ASYNC_CLOSING;
 		add_wait_queue(&raw->empty_wait, &wait);
 		set_current_state(TASK_INTERRUPTIBLE);
 		spin_unlock_irqrestore(get_ccwdev_lock(raw->cdev), flags);
@@ -643,11 +645,41 @@ static void raw3215_shutdown(struct raw3215_info *raw)
 		spin_lock_irqsave(get_ccwdev_lock(raw->cdev), flags);
 		remove_wait_queue(&raw->empty_wait, &wait);
 		set_current_state(TASK_RUNNING);
-		raw->flags &= ~(RAW3215_ACTIVE | RAW3215_CLOSING);
+		raw->port.flags &= ~(ASYNC_INITIALIZED | ASYNC_CLOSING);
 	}
 	spin_unlock_irqrestore(get_ccwdev_lock(raw->cdev), flags);
 }
 
+static struct raw3215_info *raw3215_alloc_info(void)
+{
+	struct raw3215_info *info;
+
+	info = kzalloc(sizeof(struct raw3215_info), GFP_KERNEL | GFP_DMA);
+	if (!info)
+		return NULL;
+
+	info->buffer = kzalloc(RAW3215_BUFFER_SIZE, GFP_KERNEL | GFP_DMA);
+	info->inbuf = kzalloc(RAW3215_INBUF_SIZE, GFP_KERNEL | GFP_DMA);
+	if (!info->buffer || !info->inbuf) {
+		kfree(info);
+		return NULL;
+	}
+
+	setup_timer(&info->timer, raw3215_timeout, (unsigned long)info);
+	init_waitqueue_head(&info->empty_wait);
+	tasklet_init(&info->tlet, raw3215_wakeup, (unsigned long)info);
+	tty_port_init(&info->port);
+
+	return info;
+}
+
+static void raw3215_free_info(struct raw3215_info *raw)
+{
+	kfree(raw->inbuf);
+	kfree(raw->buffer);
+	kfree(raw);
+}
+
 static int raw3215_probe (struct ccw_device *cdev)
 {
 	struct raw3215_info *raw;
@@ -656,11 +688,15 @@ static int raw3215_probe (struct ccw_device *cdev)
 	/* Console is special. */
 	if (raw3215[0] && (raw3215[0] == dev_get_drvdata(&cdev->dev)))
 		return 0;
-	raw = kmalloc(sizeof(struct raw3215_info) +
-		      RAW3215_INBUF_SIZE, GFP_KERNEL|GFP_DMA);
+
+	raw = raw3215_alloc_info();
 	if (raw == NULL)
 		return -ENOMEM;
 
+	raw->cdev = cdev;
+	dev_set_drvdata(&cdev->dev, raw);
+	cdev->handler = raw3215_irq;
+
 	spin_lock(&raw3215_device_lock);
 	for (line = 0; line < NR_3215; line++) {
 		if (!raw3215[line]) {
@@ -670,28 +706,10 @@ static int raw3215_probe (struct ccw_device *cdev)
 	}
 	spin_unlock(&raw3215_device_lock);
 	if (line == NR_3215) {
-		kfree(raw);
+		raw3215_free_info(raw);
 		return -ENODEV;
 	}
 
-	raw->cdev = cdev;
-	raw->inbuf = (char *) raw + sizeof(struct raw3215_info);
-	memset(raw, 0, sizeof(struct raw3215_info));
-	raw->buffer = kmalloc(RAW3215_BUFFER_SIZE,
-				       GFP_KERNEL|GFP_DMA);
-	if (raw->buffer == NULL) {
-		spin_lock(&raw3215_device_lock);
-		raw3215[line] = NULL;
-		spin_unlock(&raw3215_device_lock);
-		kfree(raw);
-		return -ENOMEM;
-	}
-	init_waitqueue_head(&raw->empty_wait);
-	tasklet_init(&raw->tlet, raw3215_wakeup, (unsigned long) raw);
-
-	dev_set_drvdata(&cdev->dev, raw);
-	cdev->handler = raw3215_irq;
-
 	return 0;
 }
 
@@ -703,8 +721,7 @@ static void raw3215_remove (struct ccw_device *cdev)
 	raw = dev_get_drvdata(&cdev->dev);
 	if (raw) {
 		dev_set_drvdata(&cdev->dev, NULL);
-		kfree(raw->buffer);
-		kfree(raw);
+		raw3215_free_info(raw);
 	}
 }
 
@@ -741,7 +758,7 @@ static int raw3215_pm_stop(struct ccw_device *cdev)
 	raw = dev_get_drvdata(&cdev->dev);
 	spin_lock_irqsave(get_ccwdev_lock(raw->cdev), flags);
 	raw3215_make_room(raw, RAW3215_BUFFER_SIZE);
-	raw->flags |= RAW3215_FROZEN;
+	raw->port.flags |= ASYNC_SUSPENDED;
 	spin_unlock_irqrestore(get_ccwdev_lock(raw->cdev), flags);
 	return 0;
 }
@@ -754,7 +771,7 @@ static int raw3215_pm_start(struct ccw_device *cdev)
 	/* Allow I/O again and flush output buffer. */
 	raw = dev_get_drvdata(&cdev->dev);
 	spin_lock_irqsave(get_ccwdev_lock(raw->cdev), flags);
-	raw->flags &= ~RAW3215_FROZEN;
+	raw->port.flags &= ~ASYNC_SUSPENDED;
 	raw->flags |= RAW3215_FLUSHING;
 	raw3215_try_io(raw);
 	raw->flags &= ~RAW3215_FLUSHING;
@@ -827,7 +844,7 @@ static void con3215_flush(void)
 	unsigned long flags;
 
 	raw = raw3215[0];  /* console 3215 is the first one */
-	if (raw->flags & RAW3215_FROZEN)
+	if (raw->port.flags & ASYNC_SUSPENDED)
 		/* The console is still frozen for suspend. */
 		if (ccw_device_force_console())
 			/* Forcing didn't work, no panic message .. */
@@ -897,23 +914,16 @@ static int __init con3215_init(void)
 	if (IS_ERR(cdev))
 		return -ENODEV;
 
-	raw3215[0] = raw = (struct raw3215_info *)
-		kzalloc(sizeof(struct raw3215_info), GFP_KERNEL | GFP_DMA);
-	raw->buffer = kzalloc(RAW3215_BUFFER_SIZE, GFP_KERNEL | GFP_DMA);
-	raw->inbuf = kzalloc(RAW3215_INBUF_SIZE, GFP_KERNEL | GFP_DMA);
+	raw3215[0] = raw = raw3215_alloc_info();
 	raw->cdev = cdev;
 	dev_set_drvdata(&cdev->dev, raw);
 	cdev->handler = raw3215_irq;
 
 	raw->flags |= RAW3215_FIXED;
-	init_waitqueue_head(&raw->empty_wait);
-	tasklet_init(&raw->tlet, raw3215_wakeup, (unsigned long) raw);
 
 	/* Request the console irq */
 	if (raw3215_startup(raw) != 0) {
-		kfree(raw->inbuf);
-		kfree(raw->buffer);
-		kfree(raw);
+		raw3215_free_info(raw);
 		raw3215[0] = NULL;
 		return -ENODEV;
 	}
@@ -940,7 +950,7 @@ static int tty3215_open(struct tty_struct *tty, struct file * filp)
 		return -ENODEV;
 
 	tty->driver_data = raw;
-	raw->tty = tty;
+	tty_port_tty_set(&raw->port, tty);
 
 	tty->low_latency = 0;  /* don't use bottom half for pushing chars */
 	/*
@@ -971,7 +981,7 @@ static void tty3215_close(struct tty_struct *tty, struct file * filp)
 	raw3215_shutdown(raw);
 	tasklet_kill(&raw->tlet);
 	tty->closing = 0;
-	raw->tty = NULL;
+	tty_port_tty_set(&raw->port, NULL);
 }
 
 /*
diff --git a/drivers/s390/char/keyboard.c b/drivers/s390/char/keyboard.c
index 80658819248..7ef9cfdc17d 100644
--- a/drivers/s390/char/keyboard.c
+++ b/drivers/s390/char/keyboard.c
@@ -199,7 +199,7 @@ handle_diacr(struct kbd_data *kbd, unsigned int ch)
 	if (ch == ' ' || ch == d)
 		return d;
 
-	kbd_put_queue(kbd->tty, d);
+	kbd_put_queue(kbd->port, d);
 	return ch;
 }
 
@@ -221,7 +221,7 @@ k_self(struct kbd_data *kbd, unsigned char value)
 {
 	if (kbd->diacr)
 		value = handle_diacr(kbd, value);
-	kbd_put_queue(kbd->tty, value);
+	kbd_put_queue(kbd->port, value);
 }
 
 /*
@@ -239,7 +239,7 @@ static void
 k_fn(struct kbd_data *kbd, unsigned char value)
 {
 	if (kbd->func_table[value])
-		kbd_puts_queue(kbd->tty, kbd->func_table[value]);
+		kbd_puts_queue(kbd->port, kbd->func_table[value]);
 }
 
 static void
@@ -257,20 +257,20 @@ k_spec(struct kbd_data *kbd, unsigned char value)
  * but we need only 16 bits here
  */
 static void
-to_utf8(struct tty_struct *tty, ushort c) 
+to_utf8(struct tty_port *port, ushort c)
 {
 	if (c < 0x80)
 		/*  0******* */
-		kbd_put_queue(tty, c);
+		kbd_put_queue(port, c);
 	else if (c < 0x800) {
 		/* 110***** 10****** */
-		kbd_put_queue(tty, 0xc0 | (c >> 6));
-		kbd_put_queue(tty, 0x80 | (c & 0x3f));
+		kbd_put_queue(port, 0xc0 | (c >> 6));
+		kbd_put_queue(port, 0x80 | (c & 0x3f));
 	} else {
 		/* 1110**** 10****** 10****** */
-		kbd_put_queue(tty, 0xe0 | (c >> 12));
-		kbd_put_queue(tty, 0x80 | ((c >> 6) & 0x3f));
-		kbd_put_queue(tty, 0x80 | (c & 0x3f));
+		kbd_put_queue(port, 0xe0 | (c >> 12));
+		kbd_put_queue(port, 0x80 | ((c >> 6) & 0x3f));
+		kbd_put_queue(port, 0x80 | (c & 0x3f));
 	}
 }
 
@@ -283,7 +283,7 @@ kbd_keycode(struct kbd_data *kbd, unsigned int keycode)
 	unsigned short keysym;
 	unsigned char type, value;
 
-	if (!kbd || !kbd->tty)
+	if (!kbd)
 		return;
 
 	if (keycode >= 384)
@@ -323,7 +323,7 @@ kbd_keycode(struct kbd_data *kbd, unsigned int keycode)
 #endif
 		(*k_handler[type])(kbd, value);
 	} else
-		to_utf8(kbd->tty, keysym);
+		to_utf8(kbd->port, keysym);
 }
 
 /*
@@ -457,6 +457,7 @@ do_kdgkb_ioctl(struct kbd_data *kbd, struct kbsentry __user *u_kbs,
 
 int kbd_ioctl(struct kbd_data *kbd, unsigned int cmd, unsigned long arg)
 {
+	struct tty_struct *tty;
 	void __user *argp;
 	unsigned int ct;
 	int perm;
@@ -467,7 +468,10 @@ int kbd_ioctl(struct kbd_data *kbd, unsigned int cmd, unsigned long arg)
 	 * To have permissions to do most of the vt ioctls, we either have
 	 * to be the owner of the tty, or have CAP_SYS_TTY_CONFIG.
 	 */
-	perm = current->signal->tty == kbd->tty || capable(CAP_SYS_TTY_CONFIG);
+	tty = tty_port_tty_get(kbd->port);
+	/* FIXME this test is pretty racy */
+	perm = current->signal->tty == tty || capable(CAP_SYS_TTY_CONFIG);
+	tty_kref_put(tty);
 	switch (cmd) {
 	case KDGKBTYPE:
 		return put_user(KB_101, (char __user *)argp);
diff --git a/drivers/s390/char/keyboard.h b/drivers/s390/char/keyboard.h
index 7e736aaeae6..f682f4e4968 100644
--- a/drivers/s390/char/keyboard.h
+++ b/drivers/s390/char/keyboard.h
@@ -21,7 +21,7 @@ typedef void (fn_handler_fn)(struct kbd_data *);
  */
 
 struct kbd_data {
-	struct tty_struct *tty;
+	struct tty_port *port;
 	unsigned short **key_maps;
 	char **func_table;
 	fn_handler_fn **fn_handler;
@@ -42,16 +42,24 @@ int kbd_ioctl(struct kbd_data *, unsigned int, unsigned long);
  * Helper Functions.
  */
 static inline void
-kbd_put_queue(struct tty_struct *tty, int ch)
+kbd_put_queue(struct tty_port *port, int ch)
 {
+	struct tty_struct *tty = tty_port_tty_get(port);
+	if (!tty)
+		return;
 	tty_insert_flip_char(tty, ch, 0);
 	tty_schedule_flip(tty);
+	tty_kref_put(tty);
 }
 
 static inline void
-kbd_puts_queue(struct tty_struct *tty, char *cp)
+kbd_puts_queue(struct tty_port *port, char *cp)
 {
+	struct tty_struct *tty = tty_port_tty_get(port);
+	if (!tty)
+		return;
 	while (*cp)
 		tty_insert_flip_char(tty, *cp++, 0);
 	tty_schedule_flip(tty);
+	tty_kref_put(tty);
 }
diff --git a/drivers/s390/char/sclp_cmd.c b/drivers/s390/char/sclp_cmd.c
index 231a1d85127..766cb7b19b4 100644
--- a/drivers/s390/char/sclp_cmd.c
+++ b/drivers/s390/char/sclp_cmd.c
@@ -17,6 +17,7 @@
 #include <linux/mm.h>
 #include <linux/mmzone.h>
 #include <linux/memory.h>
+#include <linux/module.h>
 #include <linux/platform_device.h>
 #include <asm/chpid.h>
 #include <asm/sclp.h>
@@ -38,7 +39,8 @@ struct read_info_sccb {
 	u64	facilities;		/* 48-55 */
 	u8	_reserved2[84 - 56];	/* 56-83 */
 	u8	fac84;			/* 84 */
-	u8	_reserved3[91 - 85];	/* 85-90 */
+	u8	fac85;			/* 85 */
+	u8	_reserved3[91 - 86];	/* 86-90 */
 	u8	flags;			/* 91 */
 	u8	_reserved4[100 - 92];	/* 92-99 */
 	u32	rnsize2;		/* 100-103 */
@@ -51,6 +53,7 @@ static int __initdata early_read_info_sccb_valid;
 
 u64 sclp_facilities;
 static u8 sclp_fac84;
+static u8 sclp_fac85;
 static unsigned long long rzm;
 static unsigned long long rnmax;
 
@@ -112,6 +115,7 @@ void __init sclp_facilities_detect(void)
 	sccb = &early_read_info_sccb;
 	sclp_facilities = sccb->facilities;
 	sclp_fac84 = sccb->fac84;
+	sclp_fac85 = sccb->fac85;
 	rnmax = sccb->rnmax ? sccb->rnmax : sccb->rnmax2;
 	rzm = sccb->rnsize ? sccb->rnsize : sccb->rnsize2;
 	rzm <<= 20;
@@ -127,6 +131,12 @@ unsigned long long sclp_get_rzm(void)
 	return rzm;
 }
 
+u8 sclp_get_fac85(void)
+{
+	return sclp_fac85;
+}
+EXPORT_SYMBOL_GPL(sclp_get_fac85);
+
 /*
  * This function will be called after sclp_facilities_detect(), which gets
  * called from early.c code. Therefore the sccb should have valid contents.
@@ -352,7 +362,17 @@ out:
 
 static int sclp_assign_storage(u16 rn)
 {
-	return do_assign_storage(0x000d0001, rn);
+	unsigned long long start, address;
+	int rc;
+
+	rc = do_assign_storage(0x000d0001, rn);
+	if (rc)
+		goto out;
+	start = address = rn2addr(rn);
+	for (; address < start + rzm; address += PAGE_SIZE)
+		page_set_storage_key(address, PAGE_DEFAULT_KEY, 0);
+out:
+	return rc;
 }
 
 static int sclp_unassign_storage(u16 rn)
diff --git a/drivers/s390/char/sclp_tty.c b/drivers/s390/char/sclp_tty.c
index 40a9d69c898..e66a75b3822 100644
--- a/drivers/s390/char/sclp_tty.c
+++ b/drivers/s390/char/sclp_tty.c
@@ -48,7 +48,7 @@ static struct sclp_buffer *sclp_ttybuf;
 /* Timer for delayed output of console messages. */
 static struct timer_list sclp_tty_timer;
 
-static struct tty_struct *sclp_tty;
+static struct tty_port sclp_port;
 static unsigned char sclp_tty_chars[SCLP_TTY_BUF_SIZE];
 static unsigned short int sclp_tty_chars_count;
 
@@ -64,7 +64,7 @@ static int sclp_tty_columns = 80;
 static int
 sclp_tty_open(struct tty_struct *tty, struct file *filp)
 {
-	sclp_tty = tty;
+	tty_port_tty_set(&sclp_port, tty);
 	tty->driver_data = NULL;
 	tty->low_latency = 0;
 	return 0;
@@ -76,7 +76,7 @@ sclp_tty_close(struct tty_struct *tty, struct file *filp)
 {
 	if (tty->count > 1)
 		return;
-	sclp_tty = NULL;
+	tty_port_tty_set(&sclp_port, NULL);
 }
 
 /*
@@ -108,6 +108,7 @@ sclp_tty_write_room (struct tty_struct *tty)
 static void
 sclp_ttybuf_callback(struct sclp_buffer *buffer, int rc)
 {
+	struct tty_struct *tty;
 	unsigned long flags;
 	void *page;
 
@@ -126,8 +127,10 @@ sclp_ttybuf_callback(struct sclp_buffer *buffer, int rc)
 		spin_unlock_irqrestore(&sclp_tty_lock, flags);
 	} while (buffer && sclp_emit_buffer(buffer, sclp_ttybuf_callback));
 	/* check if the tty needs a wake up call */
-	if (sclp_tty != NULL) {
-		tty_wakeup(sclp_tty);
+	tty = tty_port_tty_get(&sclp_port);
+	if (tty != NULL) {
+		tty_wakeup(tty);
+		tty_kref_put(tty);
 	}
 }
 
@@ -326,21 +329,22 @@ sclp_tty_flush_buffer(struct tty_struct *tty)
 static void
 sclp_tty_input(unsigned char* buf, unsigned int count)
 {
+	struct tty_struct *tty = tty_port_tty_get(&sclp_port);
 	unsigned int cchar;
 
 	/*
 	 * If this tty driver is currently closed
 	 * then throw the received input away.
 	 */
-	if (sclp_tty == NULL)
+	if (tty == NULL)
 		return;
-	cchar = ctrlchar_handle(buf, count, sclp_tty);
+	cchar = ctrlchar_handle(buf, count, tty);
 	switch (cchar & CTRLCHAR_MASK) {
 	case CTRLCHAR_SYSRQ:
 		break;
 	case CTRLCHAR_CTRL:
-		tty_insert_flip_char(sclp_tty, cchar, TTY_NORMAL);
-		tty_flip_buffer_push(sclp_tty);
+		tty_insert_flip_char(tty, cchar, TTY_NORMAL);
+		tty_flip_buffer_push(tty);
 		break;
 	case CTRLCHAR_NONE:
 		/* send (normal) input to line discipline */
@@ -348,13 +352,14 @@ sclp_tty_input(unsigned char* buf, unsigned int count)
 		    (strncmp((const char *) buf + count - 2, "^n", 2) &&
 		     strncmp((const char *) buf + count - 2, "\252n", 2))) {
 			/* add the auto \n */
-			tty_insert_flip_string(sclp_tty, buf, count);
-			tty_insert_flip_char(sclp_tty, '\n', TTY_NORMAL);
+			tty_insert_flip_string(tty, buf, count);
+			tty_insert_flip_char(tty, '\n', TTY_NORMAL);
 		} else
-			tty_insert_flip_string(sclp_tty, buf, count - 2);
-		tty_flip_buffer_push(sclp_tty);
+			tty_insert_flip_string(tty, buf, count - 2);
+		tty_flip_buffer_push(tty);
 		break;
 	}
+	tty_kref_put(tty);
 }
 
 /*
@@ -543,7 +548,7 @@ sclp_tty_init(void)
 		sclp_tty_tolower = 1;
 	}
 	sclp_tty_chars_count = 0;
-	sclp_tty = NULL;
+	tty_port_init(&sclp_port);
 
 	rc = sclp_register(&sclp_input_event);
 	if (rc) {
diff --git a/drivers/s390/char/sclp_vt220.c b/drivers/s390/char/sclp_vt220.c
index b635472ae66..edfc0fd73dc 100644
--- a/drivers/s390/char/sclp_vt220.c
+++ b/drivers/s390/char/sclp_vt220.c
@@ -34,7 +34,6 @@
 #define SCLP_VT220_DEVICE_NAME		"ttysclp"
 #define SCLP_VT220_CONSOLE_NAME		"ttyS"
 #define SCLP_VT220_CONSOLE_INDEX	1	/* console=ttyS1 */
-#define SCLP_VT220_BUF_SIZE		80
 
 /* Representation of a single write request */
 struct sclp_vt220_request {
@@ -56,8 +55,7 @@ struct sclp_vt220_sccb {
 /* Structures and data needed to register tty driver */
 static struct tty_driver *sclp_vt220_driver;
 
-/* The tty_struct that the kernel associated with us */
-static struct tty_struct *sclp_vt220_tty;
+static struct tty_port sclp_vt220_port;
 
 /* Lock to protect internal data from concurrent access */
 static spinlock_t sclp_vt220_lock;
@@ -116,6 +114,7 @@ static struct sclp_register sclp_vt220_register = {
 static void
 sclp_vt220_process_queue(struct sclp_vt220_request *request)
 {
+	struct tty_struct *tty;
 	unsigned long flags;
 	void *page;
 
@@ -141,8 +140,10 @@ sclp_vt220_process_queue(struct sclp_vt220_request *request)
 	if (request == NULL && sclp_vt220_flush_later)
 		sclp_vt220_emit_current();
 	/* Check if the tty needs a wake up call */
-	if (sclp_vt220_tty != NULL) {
-		tty_wakeup(sclp_vt220_tty);
+	tty = tty_port_tty_get(&sclp_vt220_port);
+	if (tty) {
+		tty_wakeup(tty);
+		tty_kref_put(tty);
 	}
 }
 
@@ -460,11 +461,12 @@ sclp_vt220_write(struct tty_struct *tty, const unsigned char *buf, int count)
 static void
 sclp_vt220_receiver_fn(struct evbuf_header *evbuf)
 {
+	struct tty_struct *tty = tty_port_tty_get(&sclp_vt220_port);
 	char *buffer;
 	unsigned int count;
 
 	/* Ignore input if device is not open */
-	if (sclp_vt220_tty == NULL)
+	if (tty == NULL)
 		return;
 
 	buffer = (char *) ((addr_t) evbuf + sizeof(struct evbuf_header));
@@ -478,10 +480,11 @@ sclp_vt220_receiver_fn(struct evbuf_header *evbuf)
 		/* Send input to line discipline */
 		buffer++;
 		count--;
-		tty_insert_flip_string(sclp_vt220_tty, buffer, count);
-		tty_flip_buffer_push(sclp_vt220_tty);
+		tty_insert_flip_string(tty, buffer, count);
+		tty_flip_buffer_push(tty);
 		break;
 	}
+	tty_kref_put(tty);
 }
 
 /*
@@ -491,10 +494,7 @@ static int
 sclp_vt220_open(struct tty_struct *tty, struct file *filp)
 {
 	if (tty->count == 1) {
-		sclp_vt220_tty = tty;
-		tty->driver_data = kmalloc(SCLP_VT220_BUF_SIZE, GFP_KERNEL);
-		if (tty->driver_data == NULL)
-			return -ENOMEM;
+		tty_port_tty_set(&sclp_vt220_port, tty);
 		tty->low_latency = 0;
 		if (!tty->winsize.ws_row && !tty->winsize.ws_col) {
 			tty->winsize.ws_row = 24;
@@ -510,11 +510,8 @@ sclp_vt220_open(struct tty_struct *tty, struct file *filp)
 static void
 sclp_vt220_close(struct tty_struct *tty, struct file *filp)
 {
-	if (tty->count == 1) {
-		sclp_vt220_tty = NULL;
-		kfree(tty->driver_data);
-		tty->driver_data = NULL;
-	}
+	if (tty->count == 1)
+		tty_port_tty_set(&sclp_vt220_port, NULL);
 }
 
 /*
@@ -635,9 +632,9 @@ static int __init __sclp_vt220_init(int num_pages)
 	INIT_LIST_HEAD(&sclp_vt220_empty);
 	INIT_LIST_HEAD(&sclp_vt220_outqueue);
 	init_timer(&sclp_vt220_timer);
+	tty_port_init(&sclp_vt220_port);
 	sclp_vt220_current_request = NULL;
 	sclp_vt220_buffered_chars = 0;
-	sclp_vt220_tty = NULL;
 	sclp_vt220_flush_later = 0;
 
 	/* Allocate pages for output buffering */
diff --git a/drivers/s390/char/tape.h b/drivers/s390/char/tape.h
index 267b54e8ff5..bc6c7cfd36b 100644
--- a/drivers/s390/char/tape.h
+++ b/drivers/s390/char/tape.h
@@ -154,12 +154,6 @@ struct tape_discipline {
 	struct tape_request *(*read_block)(struct tape_device *, size_t);
 	struct tape_request *(*write_block)(struct tape_device *, size_t);
 	void (*process_eov)(struct tape_device*);
-#ifdef CONFIG_S390_TAPE_BLOCK
-	/* Block device stuff. */
-	struct tape_request *(*bread)(struct tape_device *, struct request *);
-	void (*check_locate)(struct tape_device *, struct tape_request *);
-	void (*free_bread)(struct tape_request *);
-#endif
 	/* ioctl function for additional ioctls. */
 	int (*ioctl_fn)(struct tape_device *, unsigned int, unsigned long);
 	/* Array of tape commands with TAPE_NR_MTOPS entries */
@@ -182,26 +176,6 @@ struct tape_char_data {
 	int block_size;			/*   of size block_size. */
 };
 
-#ifdef CONFIG_S390_TAPE_BLOCK
-/* Block Frontend Data */
-struct tape_blk_data
-{
-	struct tape_device *	device;
-	/* Block device request queue. */
-	struct request_queue *	request_queue;
-	spinlock_t		request_queue_lock;
-
-	/* Task to move entries from block request to CCS request queue. */
-	struct work_struct	requeue_task;
-	atomic_t		requeue_scheduled;
-
-	/* Current position on the tape. */
-	long			block_position;
-	int			medium_changed;
-	struct gendisk *	disk;
-};
-#endif
-
 /* Tape Info */
 struct tape_device {
 	/* entry in tape_device_list */
@@ -248,10 +222,6 @@ struct tape_device {
 
 	/* Character device frontend data */
 	struct tape_char_data		char_data;
-#ifdef CONFIG_S390_TAPE_BLOCK
-	/* Block dev frontend data */
-	struct tape_blk_data		blk_data;
-#endif
 
 	/* Function to start or stop the next request later. */
 	struct delayed_work		tape_dnr;
@@ -313,19 +283,6 @@ extern void tapechar_exit(void);
 extern int  tapechar_setup_device(struct tape_device *);
 extern void tapechar_cleanup_device(struct tape_device *);
 
-/* Externals from tape_block.c */
-#ifdef CONFIG_S390_TAPE_BLOCK
-extern int tapeblock_init (void);
-extern void tapeblock_exit(void);
-extern int tapeblock_setup_device(struct tape_device *);
-extern void tapeblock_cleanup_device(struct tape_device *);
-#else
-static inline int tapeblock_init (void) {return 0;}
-static inline void tapeblock_exit (void) {;}
-static inline int tapeblock_setup_device(struct tape_device *t) {return 0;}
-static inline void tapeblock_cleanup_device (struct tape_device *t) {;}
-#endif
-
 /* tape initialisation functions */
 #ifdef CONFIG_PROC_FS
 extern void tape_proc_init (void);
diff --git a/drivers/s390/char/tape_34xx.c b/drivers/s390/char/tape_34xx.c
index 934ef33eb9a..b28de80b7ca 100644
--- a/drivers/s390/char/tape_34xx.c
+++ b/drivers/s390/char/tape_34xx.c
@@ -323,20 +323,6 @@ tape_34xx_unit_check(struct tape_device *device, struct tape_request *request,
 	inhibit_cu_recovery = (*device->modeset_byte & 0x80) ? 1 : 0;
 	sense = irb->ecw;
 
-#ifdef CONFIG_S390_TAPE_BLOCK
-	if (request->op == TO_BLOCK) {
-		/*
-		 * Recovery for block device requests. Set the block_position
-		 * to something invalid and retry.
-		 */
-		device->blk_data.block_position = -1;
-		if (request->retries-- <= 0)
-			return tape_34xx_erp_failed(request, -EIO);
-		else
-			return tape_34xx_erp_retry(request);
-	}
-#endif
-
 	if (
 		sense[0] & SENSE_COMMAND_REJECT &&
 		sense[1] & SENSE_WRITE_PROTECT
@@ -1129,123 +1115,6 @@ tape_34xx_mtseek(struct tape_device *device, int mt_count)
 	return tape_do_io_free(device, request);
 }
 
-#ifdef CONFIG_S390_TAPE_BLOCK
-/*
- * Tape block read for 34xx.
- */
-static struct tape_request *
-tape_34xx_bread(struct tape_device *device, struct request *req)
-{
-	struct tape_request *request;
-	struct ccw1 *ccw;
-	int count = 0;
-	unsigned off;
-	char *dst;
-	struct bio_vec *bv;
-	struct req_iterator iter;
-	struct tape_34xx_block_id *	start_block;
-
-	DBF_EVENT(6, "xBREDid:");
-
-	/* Count the number of blocks for the request. */
-	rq_for_each_segment(bv, req, iter)
-		count += bv->bv_len >> (TAPEBLOCK_HSEC_S2B + 9);
-
-	/* Allocate the ccw request. */
-	request = tape_alloc_request(3+count+1, 8);
-	if (IS_ERR(request))
-		return request;
-
-	/* Setup ccws. */
-	request->op = TO_BLOCK;
-	start_block = (struct tape_34xx_block_id *) request->cpdata;
-	start_block->block = blk_rq_pos(req) >> TAPEBLOCK_HSEC_S2B;
-	DBF_EVENT(6, "start_block = %i\n", start_block->block);
-
-	ccw = request->cpaddr;
-	ccw = tape_ccw_cc(ccw, MODE_SET_DB, 1, device->modeset_byte);
-
-	/*
-	 * We always setup a nop after the mode set ccw. This slot is
-	 * used in tape_std_check_locate to insert a locate ccw if the
-	 * current tape position doesn't match the start block to be read.
-	 * The second nop will be filled with a read block id which is in
-	 * turn used by tape_34xx_free_bread to populate the segment bid
-	 * table.
-	 */
-	ccw = tape_ccw_cc(ccw, NOP, 0, NULL);
-	ccw = tape_ccw_cc(ccw, NOP, 0, NULL);
-
-	rq_for_each_segment(bv, req, iter) {
-		dst = kmap(bv->bv_page) + bv->bv_offset;
-		for (off = 0; off < bv->bv_len; off += TAPEBLOCK_HSEC_SIZE) {
-			ccw->flags = CCW_FLAG_CC;
-			ccw->cmd_code = READ_FORWARD;
-			ccw->count = TAPEBLOCK_HSEC_SIZE;
-			set_normalized_cda(ccw, (void*) __pa(dst));
-			ccw++;
-			dst += TAPEBLOCK_HSEC_SIZE;
-		}
-	}
-
-	ccw = tape_ccw_end(ccw, NOP, 0, NULL);
-	DBF_EVENT(6, "xBREDccwg\n");
-	return request;
-}
-
-static void
-tape_34xx_free_bread (struct tape_request *request)
-{
-	struct ccw1* ccw;
-
-	ccw = request->cpaddr;
-	if ((ccw + 2)->cmd_code == READ_BLOCK_ID) {
-		struct {
-			struct tape_34xx_block_id	cbid;
-			struct tape_34xx_block_id	dbid;
-		} __attribute__ ((packed)) *rbi_data;
-
-		rbi_data = request->cpdata;
-
-		if (request->device)
-			tape_34xx_add_sbid(request->device, rbi_data->cbid);
-	}
-
-	/* Last ccw is a nop and doesn't need clear_normalized_cda */
-	for (; ccw->flags & CCW_FLAG_CC; ccw++)
-		if (ccw->cmd_code == READ_FORWARD)
-			clear_normalized_cda(ccw);
-	tape_free_request(request);
-}
-
-/*
- * check_locate is called just before the tape request is passed to
- * the common io layer for execution. It has to check the current
- * tape position and insert a locate ccw if it doesn't match the
- * start block for the request.
- */
-static void
-tape_34xx_check_locate(struct tape_device *device, struct tape_request *request)
-{
-	struct tape_34xx_block_id *	start_block;
-
-	start_block = (struct tape_34xx_block_id *) request->cpdata;
-	if (start_block->block == device->blk_data.block_position)
-		return;
-
-	DBF_LH(4, "Block seek(%06d+%06d)\n", start_block->block, device->bof);
-	start_block->wrap    = 0;
-	start_block->segment = 1;
-	start_block->format  = (*device->modeset_byte & 0x08) ?
-				TAPE34XX_FMT_3480_XF :
-				TAPE34XX_FMT_3480;
-	start_block->block   = start_block->block + device->bof;
-	tape_34xx_merge_sbid(device, start_block);
-	tape_ccw_cc(request->cpaddr + 1, LOCATE, 4, request->cpdata);
-	tape_ccw_cc(request->cpaddr + 2, READ_BLOCK_ID, 8, request->cpdata);
-}
-#endif
-
 /*
  * List of 3480/3490 magnetic tape commands.
  */
@@ -1295,11 +1164,6 @@ static struct tape_discipline tape_discipline_34xx = {
 	.irq = tape_34xx_irq,
 	.read_block = tape_std_read_block,
 	.write_block = tape_std_write_block,
-#ifdef CONFIG_S390_TAPE_BLOCK
-	.bread = tape_34xx_bread,
-	.free_bread = tape_34xx_free_bread,
-	.check_locate = tape_34xx_check_locate,
-#endif
 	.ioctl_fn = tape_34xx_ioctl,
 	.mtop_array = tape_34xx_mtop
 };
diff --git a/drivers/s390/char/tape_3590.c b/drivers/s390/char/tape_3590.c
index 49c6aab7ad7..a5c6614b0db 100644
--- a/drivers/s390/char/tape_3590.c
+++ b/drivers/s390/char/tape_3590.c
@@ -670,92 +670,6 @@ tape_3590_schedule_work(struct tape_device *device, enum tape_op op)
 	return 0;
 }
 
-#ifdef CONFIG_S390_TAPE_BLOCK
-/*
- * Tape Block READ
- */
-static struct tape_request *
-tape_3590_bread(struct tape_device *device, struct request *req)
-{
-	struct tape_request *request;
-	struct ccw1 *ccw;
-	int count = 0, start_block;
-	unsigned off;
-	char *dst;
-	struct bio_vec *bv;
-	struct req_iterator iter;
-
-	DBF_EVENT(6, "xBREDid:");
-	start_block = blk_rq_pos(req) >> TAPEBLOCK_HSEC_S2B;
-	DBF_EVENT(6, "start_block = %i\n", start_block);
-
-	rq_for_each_segment(bv, req, iter)
-		count += bv->bv_len >> (TAPEBLOCK_HSEC_S2B + 9);
-
-	request = tape_alloc_request(2 + count + 1, 4);
-	if (IS_ERR(request))
-		return request;
-	request->op = TO_BLOCK;
-	*(__u32 *) request->cpdata = start_block;
-	ccw = request->cpaddr;
-	ccw = tape_ccw_cc(ccw, MODE_SET_DB, 1, device->modeset_byte);
-
-	/*
-	 * We always setup a nop after the mode set ccw. This slot is
-	 * used in tape_std_check_locate to insert a locate ccw if the
-	 * current tape position doesn't match the start block to be read.
-	 */
-	ccw = tape_ccw_cc(ccw, NOP, 0, NULL);
-
-	rq_for_each_segment(bv, req, iter) {
-		dst = page_address(bv->bv_page) + bv->bv_offset;
-		for (off = 0; off < bv->bv_len; off += TAPEBLOCK_HSEC_SIZE) {
-			ccw->flags = CCW_FLAG_CC;
-			ccw->cmd_code = READ_FORWARD;
-			ccw->count = TAPEBLOCK_HSEC_SIZE;
-			set_normalized_cda(ccw, (void *) __pa(dst));
-			ccw++;
-			dst += TAPEBLOCK_HSEC_SIZE;
-		}
-		BUG_ON(off > bv->bv_len);
-	}
-	ccw = tape_ccw_end(ccw, NOP, 0, NULL);
-	DBF_EVENT(6, "xBREDccwg\n");
-	return request;
-}
-
-static void
-tape_3590_free_bread(struct tape_request *request)
-{
-	struct ccw1 *ccw;
-
-	/* Last ccw is a nop and doesn't need clear_normalized_cda */
-	for (ccw = request->cpaddr; ccw->flags & CCW_FLAG_CC; ccw++)
-		if (ccw->cmd_code == READ_FORWARD)
-			clear_normalized_cda(ccw);
-	tape_free_request(request);
-}
-
-/*
- * check_locate is called just before the tape request is passed to
- * the common io layer for execution. It has to check the current
- * tape position and insert a locate ccw if it doesn't match the
- * start block for the request.
- */
-static void
-tape_3590_check_locate(struct tape_device *device, struct tape_request *request)
-{
-	__u32 *start_block;
-
-	start_block = (__u32 *) request->cpdata;
-	if (*start_block != device->blk_data.block_position) {
-		/* Add the start offset of the file to get the real block. */
-		*start_block += device->bof;
-		tape_ccw_cc(request->cpaddr + 1, LOCATE, 4, request->cpdata);
-	}
-}
-#endif
-
 static void tape_3590_med_state_set(struct tape_device *device,
 				    struct tape_3590_med_sense *sense)
 {
@@ -1423,20 +1337,6 @@ tape_3590_unit_check(struct tape_device *device, struct tape_request *request,
 {
 	struct tape_3590_sense *sense;
 
-#ifdef CONFIG_S390_TAPE_BLOCK
-	if (request->op == TO_BLOCK) {
-		/*
-		 * Recovery for block device requests. Set the block_position
-		 * to something invalid and retry.
-		 */
-		device->blk_data.block_position = -1;
-		if (request->retries-- <= 0)
-			return tape_3590_erp_failed(device, request, irb, -EIO);
-		else
-			return tape_3590_erp_retry(device, request, irb);
-	}
-#endif
-
 	sense = (struct tape_3590_sense *) irb->ecw;
 
 	DBF_EVENT(6, "Unit Check: RQC = %x\n", sense->rc_rqc);
@@ -1729,11 +1629,6 @@ static struct tape_discipline tape_discipline_3590 = {
 	.irq = tape_3590_irq,
 	.read_block = tape_std_read_block,
 	.write_block = tape_std_write_block,
-#ifdef CONFIG_S390_TAPE_BLOCK
-	.bread = tape_3590_bread,
-	.free_bread = tape_3590_free_bread,
-	.check_locate = tape_3590_check_locate,
-#endif
 	.ioctl_fn = tape_3590_ioctl,
 	.mtop_array = tape_3590_mtop
 };
diff --git a/drivers/s390/char/tape_char.c b/drivers/s390/char/tape_char.c
index 87cd0ab242d..46886a7578c 100644
--- a/drivers/s390/char/tape_char.c
+++ b/drivers/s390/char/tape_char.c
@@ -161,11 +161,6 @@ tapechar_read(struct file *filp, char __user *data, size_t count, loff_t *ppos)
 	if (rc)
 		return rc;
 
-#ifdef CONFIG_S390_TAPE_BLOCK
-	/* Changes position. */
-	device->blk_data.medium_changed = 1;
-#endif
-
 	DBF_EVENT(6, "TCHAR:nbytes: %lx\n", block_size);
 	/* Let the discipline build the ccw chain. */
 	request = device->discipline->read_block(device, block_size);
@@ -218,11 +213,6 @@ tapechar_write(struct file *filp, const char __user *data, size_t count, loff_t
 	if (rc)
 		return rc;
 
-#ifdef CONFIG_S390_TAPE_BLOCK
-	/* Changes position. */
-	device->blk_data.medium_changed = 1;
-#endif
-
 	DBF_EVENT(6,"TCHAR:nbytes: %lx\n", block_size);
 	DBF_EVENT(6, "TCHAR:nblocks: %x\n", nblocks);
 	/* Let the discipline build the ccw chain. */
@@ -379,9 +369,6 @@ __tapechar_ioctl(struct tape_device *device,
 			case MTBSFM:
 			case MTFSFM:
 			case MTSEEK:
-#ifdef CONFIG_S390_TAPE_BLOCK
-				device->blk_data.medium_changed = 1;
-#endif
 				if (device->required_tapemarks)
 					tape_std_terminate_write(device);
 			default:
diff --git a/drivers/s390/char/tape_core.c b/drivers/s390/char/tape_core.c
index b3a3e8e8656..585618663ba 100644
--- a/drivers/s390/char/tape_core.c
+++ b/drivers/s390/char/tape_core.c
@@ -401,9 +401,6 @@ tape_generic_online(struct tape_device *device,
 	rc = tapechar_setup_device(device);
 	if (rc)
 		goto out_minor;
-	rc = tapeblock_setup_device(device);
-	if (rc)
-		goto out_char;
 
 	tape_state_set(device, TS_UNUSED);
 
@@ -411,8 +408,6 @@ tape_generic_online(struct tape_device *device,
 
 	return 0;
 
-out_char:
-	tapechar_cleanup_device(device);
 out_minor:
 	tape_remove_minor(device);
 out_discipline:
@@ -426,7 +421,6 @@ out:
 static void
 tape_cleanup_device(struct tape_device *device)
 {
-	tapeblock_cleanup_device(device);
 	tapechar_cleanup_device(device);
 	device->discipline->cleanup_device(device);
 	module_put(device->discipline->owner);
@@ -785,10 +779,6 @@ __tape_start_io(struct tape_device *device, struct tape_request *request)
 {
 	int rc;
 
-#ifdef CONFIG_S390_TAPE_BLOCK
-	if (request->op == TO_BLOCK)
-		device->discipline->check_locate(device, request);
-#endif
 	rc = ccw_device_start(
 		device->cdev,
 		request->cpaddr,
@@ -1253,7 +1243,7 @@ __tape_do_irq (struct ccw_device *cdev, unsigned long intparm, struct irb *irb)
 }
 
 /*
- * Tape device open function used by tape_char & tape_block frontends.
+ * Tape device open function used by tape_char frontend.
  */
 int
 tape_open(struct tape_device *device)
@@ -1283,7 +1273,7 @@ tape_open(struct tape_device *device)
 }
 
 /*
- * Tape device release function used by tape_char & tape_block frontends.
+ * Tape device release function used by tape_char frontend.
  */
 int
 tape_release(struct tape_device *device)
@@ -1344,7 +1334,6 @@ tape_init (void)
 	DBF_EVENT(3, "tape init\n");
 	tape_proc_init();
 	tapechar_init ();
-	tapeblock_init ();
 	return 0;
 }
 
@@ -1358,7 +1347,6 @@ tape_exit(void)
 
 	/* Get rid of the frontends */
 	tapechar_exit();
-	tapeblock_exit();
 	tape_proc_cleanup();
 	debug_unregister (TAPE_DBF_AREA);
 }
diff --git a/drivers/s390/char/tty3270.c b/drivers/s390/char/tty3270.c
index b43445a55cb..10ec690197c 100644
--- a/drivers/s390/char/tty3270.c
+++ b/drivers/s390/char/tty3270.c
@@ -61,7 +61,7 @@ struct tty3270_line {
  */
 struct tty3270 {
 	struct raw3270_view view;
-	struct tty_struct *tty;		/* Pointer to tty structure */
+	struct tty_port port;
 	void **freemem_pages;		/* Array of pages used for freemem. */
 	struct list_head freemem;	/* List of free memory for strings. */
 
@@ -324,9 +324,8 @@ tty3270_blank_line(struct tty3270 *tp)
 static void
 tty3270_write_callback(struct raw3270_request *rq, void *data)
 {
-	struct tty3270 *tp;
+	struct tty3270 *tp = container_of(rq->view, struct tty3270, view);
 
-	tp = (struct tty3270 *) rq->view;
 	if (rq->rc != 0) {
 		/* Write wasn't successful. Refresh all. */
 		tp->update_flags = TTY_UPDATE_ALL;
@@ -450,10 +449,9 @@ tty3270_rcl_add(struct tty3270 *tp, char *input, int len)
 static void
 tty3270_rcl_backward(struct kbd_data *kbd)
 {
-	struct tty3270 *tp;
+	struct tty3270 *tp = container_of(kbd->port, struct tty3270, port);
 	struct string *s;
 
-	tp = kbd->tty->driver_data;
 	spin_lock_bh(&tp->view.lock);
 	if (tp->inattr == TF_INPUT) {
 		if (tp->rcl_walk && tp->rcl_walk->prev != &tp->rcl_lines)
@@ -478,9 +476,8 @@ tty3270_rcl_backward(struct kbd_data *kbd)
 static void
 tty3270_exit_tty(struct kbd_data *kbd)
 {
-	struct tty3270 *tp;
+	struct tty3270 *tp = container_of(kbd->port, struct tty3270, port);
 
-	tp = kbd->tty->driver_data;
 	raw3270_deactivate_view(&tp->view);
 }
 
@@ -490,10 +487,9 @@ tty3270_exit_tty(struct kbd_data *kbd)
 static void
 tty3270_scroll_forward(struct kbd_data *kbd)
 {
-	struct tty3270 *tp;
+	struct tty3270 *tp = container_of(kbd->port, struct tty3270, port);
 	int nr_up;
 
-	tp = kbd->tty->driver_data;
 	spin_lock_bh(&tp->view.lock);
 	nr_up = tp->nr_up - tp->view.rows + 2;
 	if (nr_up < 0)
@@ -513,10 +509,9 @@ tty3270_scroll_forward(struct kbd_data *kbd)
 static void
 tty3270_scroll_backward(struct kbd_data *kbd)
 {
-	struct tty3270 *tp;
+	struct tty3270 *tp = container_of(kbd->port, struct tty3270, port);
 	int nr_up;
 
-	tp = kbd->tty->driver_data;
 	spin_lock_bh(&tp->view.lock);
 	nr_up = tp->nr_up + tp->view.rows - 2;
 	if (nr_up + tp->view.rows - 2 > tp->nr_lines)
@@ -537,11 +532,10 @@ static void
 tty3270_read_tasklet(struct raw3270_request *rrq)
 {
 	static char kreset_data = TW_KR;
-	struct tty3270 *tp;
+	struct tty3270 *tp = container_of(rrq->view, struct tty3270, view);
 	char *input;
 	int len;
 
-	tp = (struct tty3270 *) rrq->view;
 	spin_lock_bh(&tp->view.lock);
 	/*
 	 * Two AID keys are special: For 0x7d (enter) the input line
@@ -577,13 +571,10 @@ tty3270_read_tasklet(struct raw3270_request *rrq)
 	raw3270_request_add_data(tp->kreset, &kreset_data, 1);
 	raw3270_start(&tp->view, tp->kreset);
 
-	/* Emit input string. */
-	if (tp->tty) {
-		while (len-- > 0)
-			kbd_keycode(tp->kbd, *input++);
-		/* Emit keycode for AID byte. */
-		kbd_keycode(tp->kbd, 256 + tp->input->string[0]);
-	}
+	while (len-- > 0)
+		kbd_keycode(tp->kbd, *input++);
+	/* Emit keycode for AID byte. */
+	kbd_keycode(tp->kbd, 256 + tp->input->string[0]);
 
 	raw3270_request_reset(rrq);
 	xchg(&tp->read, rrq);
@@ -596,9 +587,10 @@ tty3270_read_tasklet(struct raw3270_request *rrq)
 static void
 tty3270_read_callback(struct raw3270_request *rq, void *data)
 {
+	struct tty3270 *tp = container_of(rq->view, struct tty3270, view);
 	raw3270_get_view(rq->view);
 	/* Schedule tasklet to pass input to tty. */
-	tasklet_schedule(&((struct tty3270 *) rq->view)->readlet);
+	tasklet_schedule(&tp->readlet);
 }
 
 /*
@@ -635,9 +627,8 @@ tty3270_issue_read(struct tty3270 *tp, int lock)
 static int
 tty3270_activate(struct raw3270_view *view)
 {
-	struct tty3270 *tp;
+	struct tty3270 *tp = container_of(view, struct tty3270, view);
 
-	tp = (struct tty3270 *) view;
 	tp->update_flags = TTY_UPDATE_ALL;
 	tty3270_set_timer(tp, 1);
 	return 0;
@@ -646,9 +637,8 @@ tty3270_activate(struct raw3270_view *view)
 static void
 tty3270_deactivate(struct raw3270_view *view)
 {
-	struct tty3270 *tp;
+	struct tty3270 *tp = container_of(view, struct tty3270, view);
 
-	tp = (struct tty3270 *) view;
 	del_timer(&tp->timer);
 }
 
@@ -690,6 +680,17 @@ tty3270_alloc_view(void)
 	if (!tp->freemem_pages)
 		goto out_tp;
 	INIT_LIST_HEAD(&tp->freemem);
+	INIT_LIST_HEAD(&tp->lines);
+	INIT_LIST_HEAD(&tp->update);
+	INIT_LIST_HEAD(&tp->rcl_lines);
+	tp->rcl_max = 20;
+	tty_port_init(&tp->port);
+	setup_timer(&tp->timer, (void (*)(unsigned long)) tty3270_update,
+		    (unsigned long) tp);
+	tasklet_init(&tp->readlet,
+		     (void (*)(unsigned long)) tty3270_read_tasklet,
+		     (unsigned long) tp->read);
+
 	for (pages = 0; pages < TTY3270_STRING_PAGES; pages++) {
 		tp->freemem_pages[pages] = (void *)
 			__get_free_pages(GFP_KERNEL|GFP_DMA, 0);
@@ -794,16 +795,15 @@ tty3270_free_screen(struct tty3270 *tp)
 static void
 tty3270_release(struct raw3270_view *view)
 {
-	struct tty3270 *tp;
-	struct tty_struct *tty;
+	struct tty3270 *tp = container_of(view, struct tty3270, view);
+	struct tty_struct *tty = tty_port_tty_get(&tp->port);
 
-	tp = (struct tty3270 *) view;
-	tty = tp->tty;
 	if (tty) {
 		tty->driver_data = NULL;
-		tp->tty = tp->kbd->tty = NULL;
+		tty_port_tty_set(&tp->port, NULL);
 		tty_hangup(tty);
 		raw3270_put_view(&tp->view);
+		tty_kref_put(tty);
 	}
 }
 
@@ -813,8 +813,9 @@ tty3270_release(struct raw3270_view *view)
 static void
 tty3270_free(struct raw3270_view *view)
 {
-	tty3270_free_screen((struct tty3270 *) view);
-	tty3270_free_view((struct tty3270 *) view);
+	struct tty3270 *tp = container_of(view, struct tty3270, view);
+	tty3270_free_screen(tp);
+	tty3270_free_view(tp);
 }
 
 /*
@@ -823,14 +824,13 @@ tty3270_free(struct raw3270_view *view)
 static void
 tty3270_del_views(void)
 {
-	struct tty3270 *tp;
 	int i;
 
 	for (i = 0; i < tty3270_max_index; i++) {
-		tp = (struct tty3270 *)
+		struct raw3270_view *view =
 			raw3270_find_view(&tty3270_fn, i + RAW3270_FIRSTMINOR);
-		if (!IS_ERR(tp))
-			raw3270_del_view(&tp->view);
+		if (!IS_ERR(view))
+			raw3270_del_view(view);
 	}
 }
 
@@ -848,22 +848,23 @@ static struct raw3270_fn tty3270_fn = {
 static int
 tty3270_open(struct tty_struct *tty, struct file * filp)
 {
+	struct raw3270_view *view;
 	struct tty3270 *tp;
 	int i, rc;
 
 	if (tty->count > 1)
 		return 0;
 	/* Check if the tty3270 is already there. */
-	tp = (struct tty3270 *)
-		raw3270_find_view(&tty3270_fn,
+	view = raw3270_find_view(&tty3270_fn,
 				  tty->index + RAW3270_FIRSTMINOR);
-	if (!IS_ERR(tp)) {
+	if (!IS_ERR(view)) {
+		tp = container_of(view, struct tty3270, view);
 		tty->driver_data = tp;
 		tty->winsize.ws_row = tp->view.rows - 2;
 		tty->winsize.ws_col = tp->view.cols;
 		tty->low_latency = 0;
-		tp->tty = tty;
-		tp->kbd->tty = tty;
+		/* why to reassign? */
+		tty_port_tty_set(&tp->port, tty);
 		tp->inattr = TF_INPUT;
 		return 0;
 	}
@@ -871,7 +872,7 @@ tty3270_open(struct tty_struct *tty, struct file * filp)
 		tty3270_max_index = tty->index + 1;
 
 	/* Quick exit if there is no device for tty->index. */
-	if (PTR_ERR(tp) == -ENODEV)
+	if (PTR_ERR(view) == -ENODEV)
 		return -ENODEV;
 
 	/* Allocate tty3270 structure on first open. */
@@ -879,16 +880,6 @@ tty3270_open(struct tty_struct *tty, struct file * filp)
 	if (IS_ERR(tp))
 		return PTR_ERR(tp);
 
-	INIT_LIST_HEAD(&tp->lines);
-	INIT_LIST_HEAD(&tp->update);
-	INIT_LIST_HEAD(&tp->rcl_lines);
-	tp->rcl_max = 20;
-	setup_timer(&tp->timer, (void (*)(unsigned long)) tty3270_update,
-		    (unsigned long) tp);
-	tasklet_init(&tp->readlet, 
-		     (void (*)(unsigned long)) tty3270_read_tasklet,
-		     (unsigned long) tp->read);
-
 	rc = raw3270_add_view(&tp->view, &tty3270_fn,
 			      tty->index + RAW3270_FIRSTMINOR);
 	if (rc) {
@@ -903,7 +894,7 @@ tty3270_open(struct tty_struct *tty, struct file * filp)
 		return rc;
 	}
 
-	tp->tty = tty;
+	tty_port_tty_set(&tp->port, tty);
 	tty->low_latency = 0;
 	tty->driver_data = tp;
 	tty->winsize.ws_row = tp->view.rows - 2;
@@ -917,7 +908,7 @@ tty3270_open(struct tty_struct *tty, struct file * filp)
 	for (i = 0; i < tp->view.rows - 2; i++)
 		tty3270_blank_line(tp);
 
-	tp->kbd->tty = tty;
+	tp->kbd->port = &tp->port;
 	tp->kbd->fn_handler[KVAL(K_INCRCONSOLE)] = tty3270_exit_tty;
 	tp->kbd->fn_handler[KVAL(K_SCROLLBACK)] = tty3270_scroll_backward;
 	tp->kbd->fn_handler[KVAL(K_SCROLLFORW)] = tty3270_scroll_forward;
@@ -935,14 +926,13 @@ tty3270_open(struct tty_struct *tty, struct file * filp)
 static void
 tty3270_close(struct tty_struct *tty, struct file * filp)
 {
-	struct tty3270 *tp;
+	struct tty3270 *tp = tty->driver_data;
 
 	if (tty->count > 1)
 		return;
-	tp = (struct tty3270 *) tty->driver_data;
 	if (tp) {
 		tty->driver_data = NULL;
-		tp->tty = tp->kbd->tty = NULL;
+		tty_port_tty_set(&tp->port, NULL);
 		raw3270_put_view(&tp->view);
 	}
 }
@@ -1391,7 +1381,7 @@ tty3270_escape_sequence(struct tty3270 *tp, char ch)
 			tty3270_lf(tp);
 			break;
 		case 'Z':		/* Respond ID. */
-			kbd_puts_queue(tp->tty, "\033[?6c");
+			kbd_puts_queue(&tp->port, "\033[?6c");
 			break;
 		case '7':		/* Save cursor position. */
 			tp->saved_cx = tp->cx;
@@ -1437,11 +1427,11 @@ tty3270_escape_sequence(struct tty3270 *tp, char ch)
 	tp->esc_state = ESnormal;
 	if (ch == 'n' && !tp->esc_ques) {
 		if (tp->esc_par[0] == 5)		/* Status report. */
-			kbd_puts_queue(tp->tty, "\033[0n");
+			kbd_puts_queue(&tp->port, "\033[0n");
 		else if (tp->esc_par[0] == 6) {	/* Cursor report. */
 			char buf[40];
 			sprintf(buf, "\033[%d;%dR", tp->cy + 1, tp->cx + 1);
-			kbd_puts_queue(tp->tty, buf);
+			kbd_puts_queue(&tp->port, buf);
 		}
 		return;
 	}
@@ -1513,12 +1503,13 @@ tty3270_escape_sequence(struct tty3270 *tp, char ch)
  * String write routine for 3270 ttys
  */
 static void
-tty3270_do_write(struct tty3270 *tp, const unsigned char *buf, int count)
+tty3270_do_write(struct tty3270 *tp, struct tty_struct *tty,
+		const unsigned char *buf, int count)
 {
 	int i_msg, i;
 
 	spin_lock_bh(&tp->view.lock);
-	for (i_msg = 0; !tp->tty->stopped && i_msg < count; i_msg++) {
+	for (i_msg = 0; !tty->stopped && i_msg < count; i_msg++) {
 		if (tp->esc_state != 0) {
 			/* Continue escape sequence. */
 			tty3270_escape_sequence(tp, buf[i_msg]);
@@ -1595,10 +1586,10 @@ tty3270_write(struct tty_struct * tty,
 	if (!tp)
 		return 0;
 	if (tp->char_count > 0) {
-		tty3270_do_write(tp, tp->char_buf, tp->char_count);
+		tty3270_do_write(tp, tty, tp->char_buf, tp->char_count);
 		tp->char_count = 0;
 	}
-	tty3270_do_write(tp, buf, count);
+	tty3270_do_write(tp, tty, buf, count);
 	return count;
 }
 
@@ -1629,7 +1620,7 @@ tty3270_flush_chars(struct tty_struct *tty)
 	if (!tp)
 		return;
 	if (tp->char_count > 0) {
-		tty3270_do_write(tp, tp->char_buf, tp->char_count);
+		tty3270_do_write(tp, tty, tp->char_buf, tp->char_count);
 		tp->char_count = 0;
 	}
 }
diff --git a/drivers/s390/cio/ccwgroup.c b/drivers/s390/cio/ccwgroup.c
index 5f1dc6fb570..731470e6849 100644
--- a/drivers/s390/cio/ccwgroup.c
+++ b/drivers/s390/cio/ccwgroup.c
@@ -1,7 +1,7 @@
 /*
  *  bus driver for ccwgroup
  *
- *  Copyright IBM Corp. 2002, 2009
+ *  Copyright IBM Corp. 2002, 2012
  *
  *  Author(s): Arnd Bergmann (arndb@de.ibm.com)
  *	       Cornelia Huck (cornelia.huck@de.ibm.com)
@@ -15,10 +15,13 @@
 #include <linux/ctype.h>
 #include <linux/dcache.h>
 
+#include <asm/cio.h>
 #include <asm/ccwdev.h>
 #include <asm/ccwgroup.h>
 
-#define CCW_BUS_ID_SIZE		20
+#include "device.h"
+
+#define CCW_BUS_ID_SIZE		10
 
 /* In Linux 2.4, we had a channel device layer called "chandev"
  * that did all sorts of obscure stuff for networking devices.
@@ -27,19 +30,6 @@
  * to devices that use multiple subchannels.
  */
 
-/* a device matches a driver if all its slave devices match the same
- * entry of the driver */
-static int ccwgroup_bus_match(struct device *dev, struct device_driver * drv)
-{
-	struct ccwgroup_device *gdev = to_ccwgroupdev(dev);
-	struct ccwgroup_driver *gdrv = to_ccwgroupdrv(drv);
-
-	if (gdev->creator_id == gdrv->driver_id)
-		return 1;
-
-	return 0;
-}
-
 static struct bus_type ccwgroup_bus_type;
 
 static void __ccwgroup_remove_symlinks(struct ccwgroup_device *gdev)
@@ -254,9 +244,10 @@ static int __ccwgroup_create_symlinks(struct ccwgroup_device *gdev)
 	return 0;
 }
 
-static int __get_next_bus_id(const char **buf, char *bus_id)
+static int __get_next_id(const char **buf, struct ccw_dev_id *id)
 {
-	int rc, len;
+	unsigned int cssid, ssid, devno;
+	int ret = 0, len;
 	char *start, *end;
 
 	start = (char *)*buf;
@@ -271,49 +262,40 @@ static int __get_next_bus_id(const char **buf, char *bus_id)
 		len = end - start + 1;
 		end++;
 	}
-	if (len < CCW_BUS_ID_SIZE) {
-		strlcpy(bus_id, start, len);
-		rc = 0;
+	if (len <= CCW_BUS_ID_SIZE) {
+		if (sscanf(start, "%2x.%1x.%04x", &cssid, &ssid, &devno) != 3)
+			ret = -EINVAL;
 	} else
-		rc = -EINVAL;
-	*buf = end;
-	return rc;
-}
-
-static int __is_valid_bus_id(char bus_id[CCW_BUS_ID_SIZE])
-{
-	int cssid, ssid, devno;
+		ret = -EINVAL;
 
-	/* Must be of form %x.%x.%04x */
-	if (sscanf(bus_id, "%x.%1x.%04x", &cssid, &ssid, &devno) != 3)
-		return 0;
-	return 1;
+	if (!ret) {
+		id->ssid = ssid;
+		id->devno = devno;
+	}
+	*buf = end;
+	return ret;
 }
 
 /**
- * ccwgroup_create_from_string() - create and register a ccw group device
- * @root: parent device for the new device
- * @creator_id: identifier of creating driver
- * @cdrv: ccw driver of slave devices
+ * ccwgroup_create_dev() - create and register a ccw group device
+ * @parent: parent device for the new device
+ * @gdrv: driver for the new group device
  * @num_devices: number of slave devices
  * @buf: buffer containing comma separated bus ids of slave devices
  *
- * Create and register a new ccw group device as a child of @root. Slave
- * devices are obtained from the list of bus ids given in @buf and must all
- * belong to @cdrv.
+ * Create and register a new ccw group device as a child of @parent. Slave
+ * devices are obtained from the list of bus ids given in @buf.
  * Returns:
  *  %0 on success and an error code on failure.
  * Context:
  *  non-atomic
  */
-int ccwgroup_create_from_string(struct device *root, unsigned int creator_id,
-				struct ccw_driver *cdrv, int num_devices,
-				const char *buf)
+int ccwgroup_create_dev(struct device *parent, struct ccwgroup_driver *gdrv,
+			int num_devices, const char *buf)
 {
 	struct ccwgroup_device *gdev;
+	struct ccw_dev_id dev_id;
 	int rc, i;
-	char tmp_bus_id[CCW_BUS_ID_SIZE];
-	const char *curr_buf;
 
 	gdev = kzalloc(sizeof(*gdev) + num_devices * sizeof(gdev->cdev[0]),
 		       GFP_KERNEL);
@@ -323,29 +305,24 @@ int ccwgroup_create_from_string(struct device *root, unsigned int creator_id,
 	atomic_set(&gdev->onoff, 0);
 	mutex_init(&gdev->reg_mutex);
 	mutex_lock(&gdev->reg_mutex);
-	gdev->creator_id = creator_id;
 	gdev->count = num_devices;
 	gdev->dev.bus = &ccwgroup_bus_type;
-	gdev->dev.parent = root;
+	gdev->dev.parent = parent;
 	gdev->dev.release = ccwgroup_release;
 	device_initialize(&gdev->dev);
 
-	curr_buf = buf;
-	for (i = 0; i < num_devices && curr_buf; i++) {
-		rc = __get_next_bus_id(&curr_buf, tmp_bus_id);
+	for (i = 0; i < num_devices && buf; i++) {
+		rc = __get_next_id(&buf, &dev_id);
 		if (rc != 0)
 			goto error;
-		if (!__is_valid_bus_id(tmp_bus_id)) {
-			rc = -EINVAL;
-			goto error;
-		}
-		gdev->cdev[i] = get_ccwdev_by_busid(cdrv, tmp_bus_id);
+		gdev->cdev[i] = get_ccwdev_by_dev_id(&dev_id);
 		/*
 		 * All devices have to be of the same type in
 		 * order to be grouped.
 		 */
-		if (!gdev->cdev[i]
-		    || gdev->cdev[i]->id.driver_info !=
+		if (!gdev->cdev[i] || !gdev->cdev[i]->drv ||
+		    gdev->cdev[i]->drv != gdev->cdev[0]->drv ||
+		    gdev->cdev[i]->id.driver_info !=
 		    gdev->cdev[0]->id.driver_info) {
 			rc = -EINVAL;
 			goto error;
@@ -361,18 +338,25 @@ int ccwgroup_create_from_string(struct device *root, unsigned int creator_id,
 		spin_unlock_irq(gdev->cdev[i]->ccwlock);
 	}
 	/* Check for sufficient number of bus ids. */
-	if (i < num_devices && !curr_buf) {
+	if (i < num_devices) {
 		rc = -EINVAL;
 		goto error;
 	}
 	/* Check for trailing stuff. */
-	if (i == num_devices && strlen(curr_buf) > 0) {
+	if (i == num_devices && strlen(buf) > 0) {
 		rc = -EINVAL;
 		goto error;
 	}
 
 	dev_set_name(&gdev->dev, "%s", dev_name(&gdev->cdev[0]->dev));
 	gdev->dev.groups = ccwgroup_attr_groups;
+
+	if (gdrv) {
+		gdev->dev.driver = &gdrv->driver;
+		rc = gdrv->setup ? gdrv->setup(gdev) : 0;
+		if (rc)
+			goto error;
+	}
 	rc = device_add(&gdev->dev);
 	if (rc)
 		goto error;
@@ -397,7 +381,7 @@ error:
 	put_device(&gdev->dev);
 	return rc;
 }
-EXPORT_SYMBOL(ccwgroup_create_from_string);
+EXPORT_SYMBOL(ccwgroup_create_dev);
 
 static int ccwgroup_notifier(struct notifier_block *nb, unsigned long action,
 			     void *data)
@@ -440,14 +424,6 @@ module_exit(cleanup_ccwgroup);
 
 /************************** driver stuff ******************************/
 
-static int ccwgroup_probe(struct device *dev)
-{
-	struct ccwgroup_device *gdev = to_ccwgroupdev(dev);
-	struct ccwgroup_driver *gdrv = to_ccwgroupdrv(dev->driver);
-
-	return gdrv->probe ? gdrv->probe(gdev) : -ENODEV;
-}
-
 static int ccwgroup_remove(struct device *dev)
 {
 	struct ccwgroup_device *gdev = to_ccwgroupdev(dev);
@@ -542,8 +518,6 @@ static const struct dev_pm_ops ccwgroup_pm_ops = {
 
 static struct bus_type ccwgroup_bus_type = {
 	.name   = "ccwgroup",
-	.match  = ccwgroup_bus_match,
-	.probe  = ccwgroup_probe,
 	.remove = ccwgroup_remove,
 	.shutdown = ccwgroup_shutdown,
 	.pm = &ccwgroup_pm_ops,
diff --git a/drivers/s390/cio/cio.c b/drivers/s390/cio/cio.c
index a49c46c9198..a6ddaed8793 100644
--- a/drivers/s390/cio/cio.c
+++ b/drivers/s390/cio/cio.c
@@ -656,51 +656,34 @@ static struct io_subchannel_private console_priv;
 static int console_subchannel_in_use;
 
 /*
- * Use cio_tpi to get a pending interrupt and call the interrupt handler.
- * Return non-zero if an interrupt was processed, zero otherwise.
+ * Use cio_tsch to update the subchannel status and call the interrupt handler
+ * if status had been pending. Called with the console_subchannel lock.
  */
-static int cio_tpi(void)
+static void cio_tsch(struct subchannel *sch)
 {
-	struct tpi_info *tpi_info;
-	struct subchannel *sch;
 	struct irb *irb;
 	int irq_context;
 
-	tpi_info = (struct tpi_info *)&S390_lowcore.subchannel_id;
-	if (tpi(NULL) != 1)
-		return 0;
-	kstat_cpu(smp_processor_id()).irqs[IO_INTERRUPT]++;
-	if (tpi_info->adapter_IO) {
-		do_adapter_IO(tpi_info->isc);
-		return 1;
-	}
 	irb = (struct irb *)&S390_lowcore.irb;
 	/* Store interrupt response block to lowcore. */
-	if (tsch(tpi_info->schid, irb) != 0) {
+	if (tsch(sch->schid, irb) != 0)
 		/* Not status pending or not operational. */
-		kstat_cpu(smp_processor_id()).irqs[IOINT_CIO]++;
-		return 1;
-	}
-	sch = (struct subchannel *)(unsigned long)tpi_info->intparm;
-	if (!sch) {
-		kstat_cpu(smp_processor_id()).irqs[IOINT_CIO]++;
-		return 1;
-	}
+		return;
+	memcpy(&sch->schib.scsw, &irb->scsw, sizeof(union scsw));
+	/* Call interrupt handler with updated status. */
 	irq_context = in_interrupt();
-	if (!irq_context)
+	if (!irq_context) {
 		local_bh_disable();
-	irq_enter();
-	spin_lock(sch->lock);
-	memcpy(&sch->schib.scsw, &irb->scsw, sizeof(union scsw));
+		irq_enter();
+	}
 	if (sch->driver && sch->driver->irq)
 		sch->driver->irq(sch);
 	else
 		kstat_cpu(smp_processor_id()).irqs[IOINT_CIO]++;
-	spin_unlock(sch->lock);
-	irq_exit();
-	if (!irq_context)
+	if (!irq_context) {
+		irq_exit();
 		_local_bh_enable();
-	return 1;
+	}
 }
 
 void *cio_get_console_priv(void)
@@ -712,34 +695,16 @@ void *cio_get_console_priv(void)
  * busy wait for the next interrupt on the console
  */
 void wait_cons_dev(void)
-	__releases(console_subchannel.lock)
-	__acquires(console_subchannel.lock)
 {
-	unsigned long cr6      __attribute__ ((aligned (8)));
-	unsigned long save_cr6 __attribute__ ((aligned (8)));
-
-	/* 
-	 * before entering the spinlock we may already have
-	 * processed the interrupt on a different CPU...
-	 */
 	if (!console_subchannel_in_use)
 		return;
 
-	/* disable all but the console isc */
-	__ctl_store (save_cr6, 6, 6);
-	cr6 = 1UL << (31 - CONSOLE_ISC);
-	__ctl_load (cr6, 6, 6);
-
-	do {
-		spin_unlock(console_subchannel.lock);
-		if (!cio_tpi())
-			cpu_relax();
-		spin_lock(console_subchannel.lock);
-	} while (console_subchannel.schib.scsw.cmd.actl != 0);
-	/*
-	 * restore previous isc value
-	 */
-	__ctl_load (save_cr6, 6, 6);
+	while (1) {
+		cio_tsch(&console_subchannel);
+		if (console_subchannel.schib.scsw.cmd.actl == 0)
+			break;
+		udelay_simple(100);
+	}
 }
 
 static int
diff --git a/drivers/s390/cio/device.c b/drivers/s390/cio/device.c
index 02d01525946..f8f952d5204 100644
--- a/drivers/s390/cio/device.c
+++ b/drivers/s390/cio/device.c
@@ -695,7 +695,17 @@ static int match_dev_id(struct device *dev, void *data)
 	return ccw_dev_id_is_equal(&cdev->private->dev_id, dev_id);
 }
 
-static struct ccw_device *get_ccwdev_by_dev_id(struct ccw_dev_id *dev_id)
+/**
+ * get_ccwdev_by_dev_id() - obtain device from a ccw device id
+ * @dev_id: id of the device to be searched
+ *
+ * This function searches all devices attached to the ccw bus for a device
+ * matching @dev_id.
+ * Returns:
+ *  If a device is found its reference count is increased and returned;
+ *  else %NULL is returned.
+ */
+struct ccw_device *get_ccwdev_by_dev_id(struct ccw_dev_id *dev_id)
 {
 	struct device *dev;
 
@@ -703,6 +713,7 @@ static struct ccw_device *get_ccwdev_by_dev_id(struct ccw_dev_id *dev_id)
 
 	return dev ? to_ccwdev(dev) : NULL;
 }
+EXPORT_SYMBOL_GPL(get_ccwdev_by_dev_id);
 
 static void ccw_device_do_unbind_bind(struct ccw_device *cdev)
 {
diff --git a/drivers/s390/cio/device.h b/drivers/s390/cio/device.h
index 179824b3082..6bace694239 100644
--- a/drivers/s390/cio/device.h
+++ b/drivers/s390/cio/device.h
@@ -101,6 +101,7 @@ int ccw_device_test_sense_data(struct ccw_device *);
 void ccw_device_schedule_sch_unregister(struct ccw_device *);
 int ccw_purge_blacklisted(void);
 void ccw_device_sched_todo(struct ccw_device *cdev, enum cdev_todo todo);
+struct ccw_device *get_ccwdev_by_dev_id(struct ccw_dev_id *dev_id);
 
 /* Function prototypes for device status and basic sense stuff. */
 void ccw_device_accumulate_irb(struct ccw_device *, struct irb *);
diff --git a/drivers/s390/cio/qdio_main.c b/drivers/s390/cio/qdio_main.c
index 35c685c374e..7493efafa0d 100644
--- a/drivers/s390/cio/qdio_main.c
+++ b/drivers/s390/cio/qdio_main.c
@@ -63,7 +63,7 @@ static inline int do_siga_input(unsigned long schid, unsigned int mask,
 		"	ipm	%0\n"
 		"	srl	%0,28\n"
 		: "=d" (cc)
-		: "d" (__fc), "d" (__schid), "d" (__mask) : "cc", "memory");
+		: "d" (__fc), "d" (__schid), "d" (__mask) : "cc");
 	return cc;
 }
 
@@ -74,7 +74,7 @@ static inline int do_siga_input(unsigned long schid, unsigned int mask,
  * @bb: busy bit indicator, set only if SIGA-w/wt could not access a buffer
  * @fc: function code to perform
  *
- * Returns cc or QDIO_ERROR_SIGA_ACCESS_EXCEPTION.
+ * Returns condition code.
  * Note: For IQDC unicast queues only the highest priority queue is processed.
  */
 static inline int do_siga_output(unsigned long schid, unsigned long mask,
@@ -85,18 +85,16 @@ static inline int do_siga_output(unsigned long schid, unsigned long mask,
 	register unsigned long __schid asm("1") = schid;
 	register unsigned long __mask asm("2") = mask;
 	register unsigned long __aob asm("3") = aob;
-	int cc = QDIO_ERROR_SIGA_ACCESS_EXCEPTION;
+	int cc;
 
 	asm volatile(
 		"	siga	0\n"
-		"0:	ipm	%0\n"
+		"	ipm	%0\n"
 		"	srl	%0,28\n"
-		"1:\n"
-		EX_TABLE(0b, 1b)
-		: "+d" (cc), "+d" (__fc), "+d" (__schid), "+d" (__mask),
-		  "+d" (__aob)
-		: : "cc", "memory");
-	*bb = ((unsigned int) __fc) >> 31;
+		: "=d" (cc), "+d" (__fc), "+d" (__aob)
+		: "d" (__schid), "d" (__mask)
+		: "cc");
+	*bb = __fc >> 31;
 	return cc;
 }
 
@@ -167,7 +165,7 @@ again:
 
 	DBF_ERROR("%4x EQBS ERROR", SCH_NO(q));
 	DBF_ERROR("%3d%3d%2d", count, tmp_count, nr);
-	q->handler(q->irq_ptr->cdev, QDIO_ERROR_ACTIVATE_CHECK_CONDITION,
+	q->handler(q->irq_ptr->cdev, QDIO_ERROR_GET_BUF_STATE,
 		   q->nr, q->first_to_kick, count, q->irq_ptr->int_parm);
 	return 0;
 }
@@ -215,7 +213,7 @@ again:
 
 	DBF_ERROR("%4x SQBS ERROR", SCH_NO(q));
 	DBF_ERROR("%3d%3d%2d", count, tmp_count, nr);
-	q->handler(q->irq_ptr->cdev, QDIO_ERROR_ACTIVATE_CHECK_CONDITION,
+	q->handler(q->irq_ptr->cdev, QDIO_ERROR_SET_BUF_STATE,
 		   q->nr, q->first_to_kick, count, q->irq_ptr->int_parm);
 	return 0;
 }
@@ -313,7 +311,7 @@ static inline int qdio_siga_sync(struct qdio_q *q, unsigned int output,
 	cc = do_siga_sync(schid, output, input, fc);
 	if (unlikely(cc))
 		DBF_ERROR("%4x SIGA-S:%2d", SCH_NO(q), cc);
-	return cc;
+	return (cc) ? -EIO : 0;
 }
 
 static inline int qdio_siga_sync_q(struct qdio_q *q)
@@ -384,7 +382,7 @@ static inline int qdio_siga_input(struct qdio_q *q)
 	cc = do_siga_input(schid, q->mask, fc);
 	if (unlikely(cc))
 		DBF_ERROR("%4x SIGA-R:%2d", SCH_NO(q), cc);
-	return cc;
+	return (cc) ? -EIO : 0;
 }
 
 #define qdio_siga_sync_out(q) qdio_siga_sync(q, ~0U, 0)
@@ -443,7 +441,7 @@ static void process_buffer_error(struct qdio_q *q, int count)
 	unsigned char state = (q->is_input_q) ? SLSB_P_INPUT_NOT_INIT :
 					SLSB_P_OUTPUT_NOT_INIT;
 
-	q->qdio_error |= QDIO_ERROR_SLSB_STATE;
+	q->qdio_error = QDIO_ERROR_SLSB_STATE;
 
 	/* special handling for no target buffer empty */
 	if ((!q->is_input_q &&
@@ -519,7 +517,7 @@ static int get_inbound_buffer_frontier(struct qdio_q *q)
 	int count, stop;
 	unsigned char state = 0;
 
-	q->timestamp = get_clock_fast();
+	q->timestamp = get_clock();
 
 	/*
 	 * Don't check 128 buffers, as otherwise qdio_inbound_q_moved
@@ -575,7 +573,7 @@ static int qdio_inbound_q_moved(struct qdio_q *q)
 
 	bufnr = get_inbound_buffer_frontier(q);
 
-	if ((bufnr != q->last_move) || q->qdio_error) {
+	if (bufnr != q->last_move) {
 		q->last_move = bufnr;
 		if (!is_thinint_irq(q->irq_ptr) && MACHINE_IS_LPAR)
 			q->u.in.timestamp = get_clock();
@@ -790,7 +788,7 @@ static int get_outbound_buffer_frontier(struct qdio_q *q)
 	int count, stop;
 	unsigned char state = 0;
 
-	q->timestamp = get_clock_fast();
+	q->timestamp = get_clock();
 
 	if (need_siga_sync(q))
 		if (((queue_type(q) != QDIO_IQDIO_QFMT) &&
@@ -863,7 +861,7 @@ static inline int qdio_outbound_q_moved(struct qdio_q *q)
 
 	bufnr = get_outbound_buffer_frontier(q);
 
-	if ((bufnr != q->last_move) || q->qdio_error) {
+	if (bufnr != q->last_move) {
 		q->last_move = bufnr;
 		DBF_DEV_EVENT(DBF_INFO, q->irq_ptr, "out moved:%1d", q->nr);
 		return 1;
@@ -894,13 +892,16 @@ retry:
 				goto retry;
 			}
 			DBF_ERROR("%4x cc2 BBC:%1d", SCH_NO(q), q->nr);
-			cc |= QDIO_ERROR_SIGA_BUSY;
-		} else
+			cc = -EBUSY;
+		} else {
 			DBF_DEV_EVENT(DBF_INFO, q->irq_ptr, "siga-w cc2:%1d", q->nr);
+			cc = -ENOBUFS;
+		}
 		break;
 	case 1:
 	case 3:
 		DBF_ERROR("%4x SIGA-W:%1d", SCH_NO(q), cc);
+		cc = -EIO;
 		break;
 	}
 	if (retries) {
@@ -1090,7 +1091,7 @@ static void qdio_handle_activate_check(struct ccw_device *cdev,
 	}
 
 	count = sub_buf(q->first_to_check, q->first_to_kick);
-	q->handler(q->irq_ptr->cdev, QDIO_ERROR_ACTIVATE_CHECK_CONDITION,
+	q->handler(q->irq_ptr->cdev, QDIO_ERROR_ACTIVATE,
 		   q->nr, q->first_to_kick, count, irq_ptr->int_parm);
 no_handler:
 	qdio_set_state(irq_ptr, QDIO_IRQ_STATE_STOPPED);
@@ -1691,7 +1692,7 @@ int do_QDIO(struct ccw_device *cdev, unsigned int callflags,
 		      "do%02x b:%02x c:%02x", callflags, bufnr, count);
 
 	if (irq_ptr->state != QDIO_IRQ_STATE_ACTIVE)
-		return -EBUSY;
+		return -EIO;
 	if (!count)
 		return 0;
 	if (callflags & QDIO_FLAG_SYNC_INPUT)
diff --git a/drivers/s390/crypto/ap_bus.c b/drivers/s390/crypto/ap_bus.c
index 7e9a72eb2fe..b987d461958 100644
--- a/drivers/s390/crypto/ap_bus.c
+++ b/drivers/s390/crypto/ap_bus.c
@@ -215,7 +215,7 @@ ap_queue_interruption_control(ap_qid_t qid, void *ind)
 	register struct ap_queue_status reg1_out asm ("1");
 	register void *reg2 asm ("2") = ind;
 	asm volatile(
-		".long 0xb2af0000"		/* PQAP(RAPQ) */
+		".long 0xb2af0000"		/* PQAP(AQIC) */
 		: "+d" (reg0), "+d" (reg1_in), "=d" (reg1_out), "+d" (reg2)
 		:
 		: "cc" );
@@ -232,7 +232,7 @@ __ap_query_functions(ap_qid_t qid, unsigned int *functions)
 	register unsigned long reg2 asm ("2");
 
 	asm volatile(
-		".long 0xb2af0000\n"
+		".long 0xb2af0000\n"		/* PQAP(TAPQ) */
 		"0:\n"
 		EX_TABLE(0b, 0b)
 		: "+d" (reg0), "+d" (reg1), "=d" (reg2)
@@ -391,7 +391,7 @@ __ap_send(ap_qid_t qid, unsigned long long psmid, void *msg, size_t length,
 		reg0 |= 0x400000UL;
 
 	asm volatile (
-		"0: .long 0xb2ad0042\n"		/* DQAP */
+		"0: .long 0xb2ad0042\n"		/* NQAP */
 		"   brc   2,0b"
 		: "+d" (reg0), "=d" (reg1), "+d" (reg2), "+d" (reg3)
 		: "d" (reg4), "d" (reg5), "m" (*(msgblock *) msg)
@@ -450,7 +450,7 @@ __ap_recv(ap_qid_t qid, unsigned long long *psmid, void *msg, size_t length)
 
 
 	asm volatile(
-		"0: .long 0xb2ae0064\n"
+		"0: .long 0xb2ae0064\n"		/* DQAP */
 		"   brc   6,0b\n"
 		: "+d" (reg0), "=d" (reg1), "+d" (reg2),
 		"+d" (reg4), "+d" (reg5), "+d" (reg6), "+d" (reg7),
@@ -836,12 +836,12 @@ static void __ap_flush_queue(struct ap_device *ap_dev)
 	list_for_each_entry_safe(ap_msg, next, &ap_dev->pendingq, list) {
 		list_del_init(&ap_msg->list);
 		ap_dev->pendingq_count--;
-		ap_dev->drv->receive(ap_dev, ap_msg, ERR_PTR(-ENODEV));
+		ap_msg->receive(ap_dev, ap_msg, ERR_PTR(-ENODEV));
 	}
 	list_for_each_entry_safe(ap_msg, next, &ap_dev->requestq, list) {
 		list_del_init(&ap_msg->list);
 		ap_dev->requestq_count--;
-		ap_dev->drv->receive(ap_dev, ap_msg, ERR_PTR(-ENODEV));
+		ap_msg->receive(ap_dev, ap_msg, ERR_PTR(-ENODEV));
 	}
 }
 
@@ -1329,7 +1329,7 @@ static int ap_poll_read(struct ap_device *ap_dev, unsigned long *flags)
 				continue;
 			list_del_init(&ap_msg->list);
 			ap_dev->pendingq_count--;
-			ap_dev->drv->receive(ap_dev, ap_msg, ap_dev->reply);
+			ap_msg->receive(ap_dev, ap_msg, ap_dev->reply);
 			break;
 		}
 		if (ap_dev->queue_count > 0)
@@ -1450,10 +1450,10 @@ static int __ap_queue_message(struct ap_device *ap_dev, struct ap_message *ap_ms
 			return -EBUSY;
 		case AP_RESPONSE_REQ_FAC_NOT_INST:
 		case AP_RESPONSE_MESSAGE_TOO_BIG:
-			ap_dev->drv->receive(ap_dev, ap_msg, ERR_PTR(-EINVAL));
+			ap_msg->receive(ap_dev, ap_msg, ERR_PTR(-EINVAL));
 			return -EINVAL;
 		default:	/* Device is gone. */
-			ap_dev->drv->receive(ap_dev, ap_msg, ERR_PTR(-ENODEV));
+			ap_msg->receive(ap_dev, ap_msg, ERR_PTR(-ENODEV));
 			return -ENODEV;
 		}
 	} else {
@@ -1471,6 +1471,10 @@ void ap_queue_message(struct ap_device *ap_dev, struct ap_message *ap_msg)
 	unsigned long flags;
 	int rc;
 
+	/* For asynchronous message handling a valid receive-callback
+	 * is required. */
+	BUG_ON(!ap_msg->receive);
+
 	spin_lock_bh(&ap_dev->lock);
 	if (!ap_dev->unregistered) {
 		/* Make room on the queue by polling for finished requests. */
@@ -1482,7 +1486,7 @@ void ap_queue_message(struct ap_device *ap_dev, struct ap_message *ap_msg)
 		if (rc == -ENODEV)
 			ap_dev->unregistered = 1;
 	} else {
-		ap_dev->drv->receive(ap_dev, ap_msg, ERR_PTR(-ENODEV));
+		ap_msg->receive(ap_dev, ap_msg, ERR_PTR(-ENODEV));
 		rc = -ENODEV;
 	}
 	spin_unlock_bh(&ap_dev->lock);
diff --git a/drivers/s390/crypto/ap_bus.h b/drivers/s390/crypto/ap_bus.h
index d960a6309ee..726fc65809d 100644
--- a/drivers/s390/crypto/ap_bus.h
+++ b/drivers/s390/crypto/ap_bus.h
@@ -136,9 +136,6 @@ struct ap_driver {
 
 	int (*probe)(struct ap_device *);
 	void (*remove)(struct ap_device *);
-	/* receive is called from tasklet context */
-	void (*receive)(struct ap_device *, struct ap_message *,
-			struct ap_message *);
 	int request_timeout;		/* request timeout in jiffies */
 };
 
@@ -183,6 +180,9 @@ struct ap_message {
 
 	void *private;			/* ap driver private pointer. */
 	unsigned int special:1;		/* Used for special commands. */
+	/* receive is called from tasklet context */
+	void (*receive)(struct ap_device *, struct ap_message *,
+			struct ap_message *);
 };
 
 #define AP_DEVICE(dt)					\
@@ -199,6 +199,7 @@ static inline void ap_init_message(struct ap_message *ap_msg)
 	ap_msg->psmid = 0;
 	ap_msg->length = 0;
 	ap_msg->special = 0;
+	ap_msg->receive = NULL;
 }
 
 /*
diff --git a/drivers/s390/crypto/zcrypt_cex2a.c b/drivers/s390/crypto/zcrypt_cex2a.c
index 08428672816..46812440425 100644
--- a/drivers/s390/crypto/zcrypt_cex2a.c
+++ b/drivers/s390/crypto/zcrypt_cex2a.c
@@ -77,7 +77,6 @@ static void zcrypt_cex2a_receive(struct ap_device *, struct ap_message *,
 static struct ap_driver zcrypt_cex2a_driver = {
 	.probe = zcrypt_cex2a_probe,
 	.remove = zcrypt_cex2a_remove,
-	.receive = zcrypt_cex2a_receive,
 	.ids = zcrypt_cex2a_ids,
 	.request_timeout = CEX2A_CLEANUP_TIME,
 };
@@ -349,6 +348,7 @@ static long zcrypt_cex2a_modexpo(struct zcrypt_device *zdev,
 		ap_msg.message = kmalloc(CEX3A_MAX_MESSAGE_SIZE, GFP_KERNEL);
 	if (!ap_msg.message)
 		return -ENOMEM;
+	ap_msg.receive = zcrypt_cex2a_receive;
 	ap_msg.psmid = (((unsigned long long) current->pid) << 32) +
 				atomic_inc_return(&zcrypt_step);
 	ap_msg.private = &work;
@@ -390,6 +390,7 @@ static long zcrypt_cex2a_modexpo_crt(struct zcrypt_device *zdev,
 		ap_msg.message = kmalloc(CEX3A_MAX_MESSAGE_SIZE, GFP_KERNEL);
 	if (!ap_msg.message)
 		return -ENOMEM;
+	ap_msg.receive = zcrypt_cex2a_receive;
 	ap_msg.psmid = (((unsigned long long) current->pid) << 32) +
 				atomic_inc_return(&zcrypt_step);
 	ap_msg.private = &work;
diff --git a/drivers/s390/crypto/zcrypt_pcica.c b/drivers/s390/crypto/zcrypt_pcica.c
index 0effca92545..ad7951c21b7 100644
--- a/drivers/s390/crypto/zcrypt_pcica.c
+++ b/drivers/s390/crypto/zcrypt_pcica.c
@@ -67,7 +67,6 @@ static void zcrypt_pcica_receive(struct ap_device *, struct ap_message *,
 static struct ap_driver zcrypt_pcica_driver = {
 	.probe = zcrypt_pcica_probe,
 	.remove = zcrypt_pcica_remove,
-	.receive = zcrypt_pcica_receive,
 	.ids = zcrypt_pcica_ids,
 	.request_timeout = PCICA_CLEANUP_TIME,
 };
@@ -284,6 +283,7 @@ static long zcrypt_pcica_modexpo(struct zcrypt_device *zdev,
 	ap_msg.message = kmalloc(PCICA_MAX_MESSAGE_SIZE, GFP_KERNEL);
 	if (!ap_msg.message)
 		return -ENOMEM;
+	ap_msg.receive = zcrypt_pcica_receive;
 	ap_msg.psmid = (((unsigned long long) current->pid) << 32) +
 				atomic_inc_return(&zcrypt_step);
 	ap_msg.private = &work;
@@ -322,6 +322,7 @@ static long zcrypt_pcica_modexpo_crt(struct zcrypt_device *zdev,
 	ap_msg.message = kmalloc(PCICA_MAX_MESSAGE_SIZE, GFP_KERNEL);
 	if (!ap_msg.message)
 		return -ENOMEM;
+	ap_msg.receive = zcrypt_pcica_receive;
 	ap_msg.psmid = (((unsigned long long) current->pid) << 32) +
 				atomic_inc_return(&zcrypt_step);
 	ap_msg.private = &work;
diff --git a/drivers/s390/crypto/zcrypt_pcicc.c b/drivers/s390/crypto/zcrypt_pcicc.c
index f9523c0cc8d..e5dd335fda5 100644
--- a/drivers/s390/crypto/zcrypt_pcicc.c
+++ b/drivers/s390/crypto/zcrypt_pcicc.c
@@ -79,7 +79,6 @@ static void zcrypt_pcicc_receive(struct ap_device *, struct ap_message *,
 static struct ap_driver zcrypt_pcicc_driver = {
 	.probe = zcrypt_pcicc_probe,
 	.remove = zcrypt_pcicc_remove,
-	.receive = zcrypt_pcicc_receive,
 	.ids = zcrypt_pcicc_ids,
 	.request_timeout = PCICC_CLEANUP_TIME,
 };
@@ -488,6 +487,7 @@ static long zcrypt_pcicc_modexpo(struct zcrypt_device *zdev,
 	ap_msg.message = (void *) get_zeroed_page(GFP_KERNEL);
 	if (!ap_msg.message)
 		return -ENOMEM;
+	ap_msg.receive = zcrypt_pcicc_receive;
 	ap_msg.length = PAGE_SIZE;
 	ap_msg.psmid = (((unsigned long long) current->pid) << 32) +
 				atomic_inc_return(&zcrypt_step);
@@ -527,6 +527,7 @@ static long zcrypt_pcicc_modexpo_crt(struct zcrypt_device *zdev,
 	ap_msg.message = (void *) get_zeroed_page(GFP_KERNEL);
 	if (!ap_msg.message)
 		return -ENOMEM;
+	ap_msg.receive = zcrypt_pcicc_receive;
 	ap_msg.length = PAGE_SIZE;
 	ap_msg.psmid = (((unsigned long long) current->pid) << 32) +
 				atomic_inc_return(&zcrypt_step);
diff --git a/drivers/s390/crypto/zcrypt_pcixcc.c b/drivers/s390/crypto/zcrypt_pcixcc.c
index cf1cbd4747f..f7cc4340181 100644
--- a/drivers/s390/crypto/zcrypt_pcixcc.c
+++ b/drivers/s390/crypto/zcrypt_pcixcc.c
@@ -89,7 +89,6 @@ static void zcrypt_pcixcc_receive(struct ap_device *, struct ap_message *,
 static struct ap_driver zcrypt_pcixcc_driver = {
 	.probe = zcrypt_pcixcc_probe,
 	.remove = zcrypt_pcixcc_remove,
-	.receive = zcrypt_pcixcc_receive,
 	.ids = zcrypt_pcixcc_ids,
 	.request_timeout = PCIXCC_CLEANUP_TIME,
 };
@@ -698,6 +697,7 @@ static long zcrypt_pcixcc_modexpo(struct zcrypt_device *zdev,
 	ap_msg.message = (void *) get_zeroed_page(GFP_KERNEL);
 	if (!ap_msg.message)
 		return -ENOMEM;
+	ap_msg.receive = zcrypt_pcixcc_receive;
 	ap_msg.psmid = (((unsigned long long) current->pid) << 32) +
 				atomic_inc_return(&zcrypt_step);
 	ap_msg.private = &resp_type;
@@ -738,6 +738,7 @@ static long zcrypt_pcixcc_modexpo_crt(struct zcrypt_device *zdev,
 	ap_msg.message = (void *) get_zeroed_page(GFP_KERNEL);
 	if (!ap_msg.message)
 		return -ENOMEM;
+	ap_msg.receive = zcrypt_pcixcc_receive;
 	ap_msg.psmid = (((unsigned long long) current->pid) << 32) +
 				atomic_inc_return(&zcrypt_step);
 	ap_msg.private = &resp_type;
@@ -778,6 +779,7 @@ static long zcrypt_pcixcc_send_cprb(struct zcrypt_device *zdev,
 	ap_msg.message = kmalloc(PCIXCC_MAX_XCRB_MESSAGE_SIZE, GFP_KERNEL);
 	if (!ap_msg.message)
 		return -ENOMEM;
+	ap_msg.receive = zcrypt_pcixcc_receive;
 	ap_msg.psmid = (((unsigned long long) current->pid) << 32) +
 				atomic_inc_return(&zcrypt_step);
 	ap_msg.private = &resp_type;
@@ -818,6 +820,7 @@ static long zcrypt_pcixcc_rng(struct zcrypt_device *zdev,
 	ap_msg.message = kmalloc(PCIXCC_MAX_XCRB_MESSAGE_SIZE, GFP_KERNEL);
 	if (!ap_msg.message)
 		return -ENOMEM;
+	ap_msg.receive = zcrypt_pcixcc_receive;
 	ap_msg.psmid = (((unsigned long long) current->pid) << 32) +
 				atomic_inc_return(&zcrypt_step);
 	ap_msg.private = &resp_type;
diff --git a/drivers/s390/net/Kconfig b/drivers/s390/net/Kconfig
index 9b66d2d1809..dfda748c400 100644
--- a/drivers/s390/net/Kconfig
+++ b/drivers/s390/net/Kconfig
@@ -4,11 +4,10 @@ menu "S/390 network device drivers"
 config LCS
 	def_tristate m
 	prompt "Lan Channel Station Interface"
-	depends on CCW && NETDEVICES && (ETHERNET || TR || FDDI)
+	depends on CCW && NETDEVICES && (ETHERNET || FDDI)
 	help
 	   Select this option if you want to use LCS networking on IBM System z.
-	   This device driver supports Token Ring (IEEE 802.5),
-	   FDDI (IEEE 802.7) and Ethernet.
+	   This device driver supports FDDI (IEEE 802.7) and Ethernet.
 	   To compile as a module, choose M. The module name is lcs.
 	   If you do not know what it is, it's safe to choose Y.
 
diff --git a/drivers/s390/net/claw.c b/drivers/s390/net/claw.c
index b41fae37d3a..6b1ff90d2f0 100644
--- a/drivers/s390/net/claw.c
+++ b/drivers/s390/net/claw.c
@@ -136,7 +136,6 @@ static inline void
 claw_set_busy(struct net_device *dev)
 {
  ((struct claw_privbk *)dev->ml_priv)->tbusy = 1;
- eieio();
 }
 
 static inline void
@@ -144,13 +143,11 @@ claw_clear_busy(struct net_device *dev)
 {
 	clear_bit(0, &(((struct claw_privbk *) dev->ml_priv)->tbusy));
 	netif_wake_queue(dev);
-	eieio();
 }
 
 static inline int
 claw_check_busy(struct net_device *dev)
 {
-	eieio();
 	return ((struct claw_privbk *) dev->ml_priv)->tbusy;
 }
 
@@ -233,8 +230,6 @@ static ssize_t claw_rbuff_show(struct device *dev,
 static ssize_t claw_rbuff_write(struct device *dev,
 	struct device_attribute *attr,
 	const char *buf, size_t count);
-static int claw_add_files(struct device *dev);
-static void claw_remove_files(struct device *dev);
 
 /*   Functions for System Validate  */
 static int claw_process_control( struct net_device *dev, struct ccwbk * p_ccw);
@@ -267,12 +262,10 @@ static struct ccwgroup_driver claw_group_driver = {
 		.owner	= THIS_MODULE,
 		.name	= "claw",
 	},
-        .max_slaves  = 2,
-        .driver_id   = 0xC3D3C1E6,
-        .probe       = claw_probe,
-        .remove      = claw_remove_device,
-        .set_online  = claw_new_device,
-        .set_offline = claw_shutdown_device,
+	.setup	     = claw_probe,
+	.remove      = claw_remove_device,
+	.set_online  = claw_new_device,
+	.set_offline = claw_shutdown_device,
 	.prepare     = claw_pm_prepare,
 };
 
@@ -293,30 +286,24 @@ static struct ccw_driver claw_ccw_driver = {
 	.int_class = IOINT_CLW,
 };
 
-static ssize_t
-claw_driver_group_store(struct device_driver *ddrv, const char *buf,
-			size_t count)
+static ssize_t claw_driver_group_store(struct device_driver *ddrv,
+				       const char *buf,	size_t count)
 {
 	int err;
-	err = ccwgroup_create_from_string(claw_root_dev,
-					  claw_group_driver.driver_id,
-					  &claw_ccw_driver, 2, buf);
+	err = ccwgroup_create_dev(claw_root_dev, &claw_group_driver, 2, buf);
 	return err ? err : count;
 }
-
 static DRIVER_ATTR(group, 0200, NULL, claw_driver_group_store);
 
-static struct attribute *claw_group_attrs[] = {
+static struct attribute *claw_drv_attrs[] = {
 	&driver_attr_group.attr,
 	NULL,
 };
-
-static struct attribute_group claw_group_attr_group = {
-	.attrs = claw_group_attrs,
+static struct attribute_group claw_drv_attr_group = {
+	.attrs = claw_drv_attrs,
 };
-
-static const struct attribute_group *claw_group_attr_groups[] = {
-	&claw_group_attr_group,
+static const struct attribute_group *claw_drv_attr_groups[] = {
+	&claw_drv_attr_group,
 	NULL,
 };
 
@@ -324,60 +311,6 @@ static const struct attribute_group *claw_group_attr_groups[] = {
 *       Key functions
 */
 
-/*----------------------------------------------------------------*
- *   claw_probe                                                   *
- *      this function is called for each CLAW device.             *
- *----------------------------------------------------------------*/
-static int
-claw_probe(struct ccwgroup_device *cgdev)
-{
-	int  		rc;
-	struct claw_privbk *privptr=NULL;
-
-	CLAW_DBF_TEXT(2, setup, "probe");
-	if (!get_device(&cgdev->dev))
-		return -ENODEV;
-	privptr = kzalloc(sizeof(struct claw_privbk), GFP_KERNEL);
-	dev_set_drvdata(&cgdev->dev, privptr);
-	if (privptr == NULL) {
-		probe_error(cgdev);
-		put_device(&cgdev->dev);
-		CLAW_DBF_TEXT_(2, setup, "probex%d", -ENOMEM);
-		return -ENOMEM;
-	}
-	privptr->p_mtc_envelope= kzalloc( MAX_ENVELOPE_SIZE, GFP_KERNEL);
-	privptr->p_env = kzalloc(sizeof(struct claw_env), GFP_KERNEL);
-        if ((privptr->p_mtc_envelope==NULL) || (privptr->p_env==NULL)) {
-                probe_error(cgdev);
-		put_device(&cgdev->dev);
-		CLAW_DBF_TEXT_(2, setup, "probex%d", -ENOMEM);
-                return -ENOMEM;
-        }
-	memcpy(privptr->p_env->adapter_name,WS_NAME_NOT_DEF,8);
-	memcpy(privptr->p_env->host_name,WS_NAME_NOT_DEF,8);
-	memcpy(privptr->p_env->api_type,WS_NAME_NOT_DEF,8);
-	privptr->p_env->packing = 0;
-	privptr->p_env->write_buffers = 5;
-	privptr->p_env->read_buffers = 5;
-	privptr->p_env->read_size = CLAW_FRAME_SIZE;
-	privptr->p_env->write_size = CLAW_FRAME_SIZE;
-	rc = claw_add_files(&cgdev->dev);
-	if (rc) {
-		probe_error(cgdev);
-		put_device(&cgdev->dev);
-		dev_err(&cgdev->dev, "Creating the /proc files for a new"
-		" CLAW device failed\n");
-		CLAW_DBF_TEXT_(2, setup, "probex%d", rc);
-		return rc;
-	}
-	privptr->p_env->p_priv = privptr;
-        cgdev->cdev[0]->handler = claw_irq_handler;
-	cgdev->cdev[1]->handler = claw_irq_handler;
-	CLAW_DBF_TEXT(2, setup, "prbext 0");
-
-        return 0;
-}  /*  end of claw_probe       */
-
 /*-------------------------------------------------------------------*
  *   claw_tx                                                         *
  *-------------------------------------------------------------------*/
@@ -3093,7 +3026,6 @@ claw_remove_device(struct ccwgroup_device *cgdev)
 	dev_info(&cgdev->dev, " will be removed.\n");
 	if (cgdev->state == CCWGROUP_ONLINE)
 		claw_shutdown_device(cgdev);
-	claw_remove_files(&cgdev->dev);
 	kfree(priv->p_mtc_envelope);
 	priv->p_mtc_envelope=NULL;
 	kfree(priv->p_env);
@@ -3321,7 +3253,6 @@ claw_rbuff_write(struct device *dev, struct device_attribute *attr,
 	CLAW_DBF_TEXT_(2, setup, "RB=%d", p_env->read_buffers);
 	return count;
 }
-
 static DEVICE_ATTR(read_buffer, 0644, claw_rbuff_show, claw_rbuff_write);
 
 static struct attribute *claw_attr[] = {
@@ -3332,40 +3263,73 @@ static struct attribute *claw_attr[] = {
 	&dev_attr_host_name.attr,
 	NULL,
 };
-
 static struct attribute_group claw_attr_group = {
 	.attrs = claw_attr,
 };
+static const struct attribute_group *claw_attr_groups[] = {
+	&claw_attr_group,
+	NULL,
+};
+static const struct device_type claw_devtype = {
+	.name = "claw",
+	.groups = claw_attr_groups,
+};
 
-static int
-claw_add_files(struct device *dev)
+/*----------------------------------------------------------------*
+ *   claw_probe 						  *
+ *	this function is called for each CLAW device.		  *
+ *----------------------------------------------------------------*/
+static int claw_probe(struct ccwgroup_device *cgdev)
 {
-	CLAW_DBF_TEXT(2, setup, "add_file");
-	return sysfs_create_group(&dev->kobj, &claw_attr_group);
-}
+	struct claw_privbk *privptr = NULL;
 
-static void
-claw_remove_files(struct device *dev)
-{
-	CLAW_DBF_TEXT(2, setup, "rem_file");
-	sysfs_remove_group(&dev->kobj, &claw_attr_group);
-}
+	CLAW_DBF_TEXT(2, setup, "probe");
+	if (!get_device(&cgdev->dev))
+		return -ENODEV;
+	privptr = kzalloc(sizeof(struct claw_privbk), GFP_KERNEL);
+	dev_set_drvdata(&cgdev->dev, privptr);
+	if (privptr == NULL) {
+		probe_error(cgdev);
+		put_device(&cgdev->dev);
+		CLAW_DBF_TEXT_(2, setup, "probex%d", -ENOMEM);
+		return -ENOMEM;
+	}
+	privptr->p_mtc_envelope = kzalloc(MAX_ENVELOPE_SIZE, GFP_KERNEL);
+	privptr->p_env = kzalloc(sizeof(struct claw_env), GFP_KERNEL);
+	if ((privptr->p_mtc_envelope == NULL) || (privptr->p_env == NULL)) {
+		probe_error(cgdev);
+		put_device(&cgdev->dev);
+		CLAW_DBF_TEXT_(2, setup, "probex%d", -ENOMEM);
+		return -ENOMEM;
+	}
+	memcpy(privptr->p_env->adapter_name, WS_NAME_NOT_DEF, 8);
+	memcpy(privptr->p_env->host_name, WS_NAME_NOT_DEF, 8);
+	memcpy(privptr->p_env->api_type, WS_NAME_NOT_DEF, 8);
+	privptr->p_env->packing = 0;
+	privptr->p_env->write_buffers = 5;
+	privptr->p_env->read_buffers = 5;
+	privptr->p_env->read_size = CLAW_FRAME_SIZE;
+	privptr->p_env->write_size = CLAW_FRAME_SIZE;
+	privptr->p_env->p_priv = privptr;
+	cgdev->cdev[0]->handler = claw_irq_handler;
+	cgdev->cdev[1]->handler = claw_irq_handler;
+	cgdev->dev.type = &claw_devtype;
+	CLAW_DBF_TEXT(2, setup, "prbext 0");
+
+	return 0;
+}  /*  end of claw_probe       */
 
 /*--------------------------------------------------------------------*
 *    claw_init  and cleanup                                           *
 *---------------------------------------------------------------------*/
 
-static void __exit
-claw_cleanup(void)
+static void __exit claw_cleanup(void)
 {
-	driver_remove_file(&claw_group_driver.driver,
-			   &driver_attr_group);
 	ccwgroup_driver_unregister(&claw_group_driver);
 	ccw_driver_unregister(&claw_ccw_driver);
 	root_device_unregister(claw_root_dev);
 	claw_unregister_debug_facility();
 	pr_info("Driver unloaded\n");
-
 }
 
 /**
@@ -3374,8 +3338,7 @@ claw_cleanup(void)
  *
  * @return 0 on success, !0 on error.
  */
-static int __init
-claw_init(void)
+static int __init claw_init(void)
 {
 	int ret = 0;
 
@@ -3394,7 +3357,7 @@ claw_init(void)
 	ret = ccw_driver_register(&claw_ccw_driver);
 	if (ret)
 		goto ccw_err;
-	claw_group_driver.driver.groups = claw_group_attr_groups;
+	claw_group_driver.driver.groups = claw_drv_attr_groups;
 	ret = ccwgroup_driver_register(&claw_group_driver);
 	if (ret)
 		goto ccwgroup_err;
diff --git a/drivers/s390/net/ctcm_main.c b/drivers/s390/net/ctcm_main.c
index 11f3b071f30..3cd25544a27 100644
--- a/drivers/s390/net/ctcm_main.c
+++ b/drivers/s390/net/ctcm_main.c
@@ -1296,6 +1296,11 @@ static void ctcm_irq_handler(struct ccw_device *cdev,
 
 }
 
+static const struct device_type ctcm_devtype = {
+	.name = "ctcm",
+	.groups = ctcm_attr_groups,
+};
+
 /**
  * Add ctcm specific attributes.
  * Add ctcm private data.
@@ -1307,7 +1312,6 @@ static void ctcm_irq_handler(struct ccw_device *cdev,
 static int ctcm_probe_device(struct ccwgroup_device *cgdev)
 {
 	struct ctcm_priv *priv;
-	int rc;
 
 	CTCM_DBF_TEXT_(SETUP, CTC_DBF_INFO,
 			"%s %p",
@@ -1324,17 +1328,11 @@ static int ctcm_probe_device(struct ccwgroup_device *cgdev)
 		put_device(&cgdev->dev);
 		return -ENOMEM;
 	}
-
-	rc = ctcm_add_files(&cgdev->dev);
-	if (rc) {
-		kfree(priv);
-		put_device(&cgdev->dev);
-		return rc;
-	}
 	priv->buffer_size = CTCM_BUFSIZE_DEFAULT;
 	cgdev->cdev[0]->handler = ctcm_irq_handler;
 	cgdev->cdev[1]->handler = ctcm_irq_handler;
 	dev_set_drvdata(&cgdev->dev, priv);
+	cgdev->dev.type = &ctcm_devtype;
 
 	return 0;
 }
@@ -1611,11 +1609,6 @@ static int ctcm_new_device(struct ccwgroup_device *cgdev)
 		goto out_dev;
 	}
 
-	if (ctcm_add_attributes(&cgdev->dev)) {
-		result = -ENODEV;
-		goto out_unregister;
-	}
-
 	strlcpy(priv->fsm->name, dev->name, sizeof(priv->fsm->name));
 
 	dev_info(&dev->dev,
@@ -1629,8 +1622,6 @@ static int ctcm_new_device(struct ccwgroup_device *cgdev)
 			priv->channel[CTCM_WRITE]->id, priv->protocol);
 
 	return 0;
-out_unregister:
-	unregister_netdev(dev);
 out_dev:
 	ctcm_free_netdevice(dev);
 out_ccw2:
@@ -1669,7 +1660,6 @@ static int ctcm_shutdown_device(struct ccwgroup_device *cgdev)
 		/* Close the device */
 		ctcm_close(dev);
 		dev->flags &= ~IFF_RUNNING;
-		ctcm_remove_attributes(&cgdev->dev);
 		channel_free(priv->channel[CTCM_READ]);
 	} else
 		dev = NULL;
@@ -1711,7 +1701,6 @@ static void ctcm_remove_device(struct ccwgroup_device *cgdev)
 
 	if (cgdev->state == CCWGROUP_ONLINE)
 		ctcm_shutdown_device(cgdev);
-	ctcm_remove_files(&cgdev->dev);
 	dev_set_drvdata(&cgdev->dev, NULL);
 	kfree(priv);
 	put_device(&cgdev->dev);
@@ -1778,9 +1767,7 @@ static struct ccwgroup_driver ctcm_group_driver = {
 		.owner	= THIS_MODULE,
 		.name	= CTC_DRIVER_NAME,
 	},
-	.max_slaves  = 2,
-	.driver_id   = 0xC3E3C3D4,	/* CTCM */
-	.probe       = ctcm_probe_device,
+	.setup	     = ctcm_probe_device,
 	.remove      = ctcm_remove_device,
 	.set_online  = ctcm_new_device,
 	.set_offline = ctcm_shutdown_device,
@@ -1789,31 +1776,25 @@ static struct ccwgroup_driver ctcm_group_driver = {
 	.restore     = ctcm_pm_resume,
 };
 
-static ssize_t
-ctcm_driver_group_store(struct device_driver *ddrv, const char *buf,
-			size_t count)
+static ssize_t ctcm_driver_group_store(struct device_driver *ddrv,
+				       const char *buf,	size_t count)
 {
 	int err;
 
-	err = ccwgroup_create_from_string(ctcm_root_dev,
-					  ctcm_group_driver.driver_id,
-					  &ctcm_ccw_driver, 2, buf);
+	err = ccwgroup_create_dev(ctcm_root_dev, &ctcm_group_driver, 2, buf);
 	return err ? err : count;
 }
-
 static DRIVER_ATTR(group, 0200, NULL, ctcm_driver_group_store);
 
-static struct attribute *ctcm_group_attrs[] = {
+static struct attribute *ctcm_drv_attrs[] = {
 	&driver_attr_group.attr,
 	NULL,
 };
-
-static struct attribute_group ctcm_group_attr_group = {
-	.attrs = ctcm_group_attrs,
+static struct attribute_group ctcm_drv_attr_group = {
+	.attrs = ctcm_drv_attrs,
 };
-
-static const struct attribute_group *ctcm_group_attr_groups[] = {
-	&ctcm_group_attr_group,
+static const struct attribute_group *ctcm_drv_attr_groups[] = {
+	&ctcm_drv_attr_group,
 	NULL,
 };
 
@@ -1829,7 +1810,6 @@ static const struct attribute_group *ctcm_group_attr_groups[] = {
  */
 static void __exit ctcm_exit(void)
 {
-	driver_remove_file(&ctcm_group_driver.driver, &driver_attr_group);
 	ccwgroup_driver_unregister(&ctcm_group_driver);
 	ccw_driver_unregister(&ctcm_ccw_driver);
 	root_device_unregister(ctcm_root_dev);
@@ -1867,7 +1847,7 @@ static int __init ctcm_init(void)
 	ret = ccw_driver_register(&ctcm_ccw_driver);
 	if (ret)
 		goto ccw_err;
-	ctcm_group_driver.driver.groups = ctcm_group_attr_groups;
+	ctcm_group_driver.driver.groups = ctcm_drv_attr_groups;
 	ret = ccwgroup_driver_register(&ctcm_group_driver);
 	if (ret)
 		goto ccwgroup_err;
diff --git a/drivers/s390/net/ctcm_main.h b/drivers/s390/net/ctcm_main.h
index 24d5215eb0c..b9056a55d99 100644
--- a/drivers/s390/net/ctcm_main.h
+++ b/drivers/s390/net/ctcm_main.h
@@ -225,13 +225,7 @@ struct ctcm_priv {
 int ctcm_open(struct net_device *dev);
 int ctcm_close(struct net_device *dev);
 
-/*
- * prototypes for non-static sysfs functions
- */
-int ctcm_add_attributes(struct device *dev);
-void ctcm_remove_attributes(struct device *dev);
-int ctcm_add_files(struct device *dev);
-void ctcm_remove_files(struct device *dev);
+extern const struct attribute_group *ctcm_attr_groups[];
 
 /*
  * Compatibility macros for busy handling
diff --git a/drivers/s390/net/ctcm_sysfs.c b/drivers/s390/net/ctcm_sysfs.c
index 650aec1839e..0c27ae72647 100644
--- a/drivers/s390/net/ctcm_sysfs.c
+++ b/drivers/s390/net/ctcm_sysfs.c
@@ -13,6 +13,7 @@
 #define KMSG_COMPONENT "ctcm"
 #define pr_fmt(fmt) KMSG_COMPONENT ": " fmt
 
+#include <linux/device.h>
 #include <linux/sysfs.h>
 #include <linux/slab.h>
 #include "ctcm_main.h"
@@ -108,10 +109,12 @@ static void ctcm_print_statistics(struct ctcm_priv *priv)
 }
 
 static ssize_t stats_show(struct device *dev,
-				struct device_attribute *attr, char *buf)
+			  struct device_attribute *attr, char *buf)
 {
+	struct ccwgroup_device *gdev = to_ccwgroupdev(dev);
 	struct ctcm_priv *priv = dev_get_drvdata(dev);
-	if (!priv)
+
+	if (!priv || gdev->state != CCWGROUP_ONLINE)
 		return -ENODEV;
 	ctcm_print_statistics(priv);
 	return sprintf(buf, "0\n");
@@ -190,34 +193,14 @@ static struct attribute *ctcm_attr[] = {
 	&dev_attr_protocol.attr,
 	&dev_attr_type.attr,
 	&dev_attr_buffer.attr,
+	&dev_attr_stats.attr,
 	NULL,
 };
 
 static struct attribute_group ctcm_attr_group = {
 	.attrs = ctcm_attr,
 };
-
-int ctcm_add_attributes(struct device *dev)
-{
-	int rc;
-
-	rc = device_create_file(dev, &dev_attr_stats);
-
-	return rc;
-}
-
-void ctcm_remove_attributes(struct device *dev)
-{
-	device_remove_file(dev, &dev_attr_stats);
-}
-
-int ctcm_add_files(struct device *dev)
-{
-	return sysfs_create_group(&dev->kobj, &ctcm_attr_group);
-}
-
-void ctcm_remove_files(struct device *dev)
-{
-	sysfs_remove_group(&dev->kobj, &ctcm_attr_group);
-}
-
+const struct attribute_group *ctcm_attr_groups[] = {
+	&ctcm_attr_group,
+	NULL,
+};
diff --git a/drivers/s390/net/lcs.c b/drivers/s390/net/lcs.c
index 687efe4d589..a3adf4b1c60 100644
--- a/drivers/s390/net/lcs.c
+++ b/drivers/s390/net/lcs.c
@@ -30,7 +30,6 @@
 #include <linux/if.h>
 #include <linux/netdevice.h>
 #include <linux/etherdevice.h>
-#include <linux/trdevice.h>
 #include <linux/fddidevice.h>
 #include <linux/inetdevice.h>
 #include <linux/in.h>
@@ -50,8 +49,7 @@
 #include "lcs.h"
 
 
-#if !defined(CONFIG_ETHERNET) && \
-    !defined(CONFIG_TR) && !defined(CONFIG_FDDI)
+#if !defined(CONFIG_ETHERNET) && !defined(CONFIG_FDDI)
 #error Cannot compile lcs.c without some net devices switched on.
 #endif
 
@@ -1166,10 +1164,7 @@ static void
 lcs_get_mac_for_ipm(__be32 ipm, char *mac, struct net_device *dev)
 {
 	LCS_DBF_TEXT(4,trace, "getmac");
-	if (dev->type == ARPHRD_IEEE802_TR)
-		ip_tr_mc_map(ipm, mac);
-	else
-		ip_eth_mc_map(ipm, mac);
+	ip_eth_mc_map(ipm, mac);
 }
 
 /**
@@ -1641,12 +1636,6 @@ lcs_startlan_auto(struct lcs_card *card)
 		return 0;
 
 #endif
-#ifdef CONFIG_TR
-	card->lan_type = LCS_FRAME_TYPE_TR;
-	rc = lcs_send_startlan(card, LCS_INITIATOR_TCPIP);
-	if (rc == 0)
-		return 0;
-#endif
 #ifdef CONFIG_FDDI
 	card->lan_type = LCS_FRAME_TYPE_FDDI;
 	rc = lcs_send_startlan(card, LCS_INITIATOR_TCPIP);
@@ -2051,10 +2040,17 @@ static struct attribute * lcs_attrs[] = {
 	&dev_attr_recover.attr,
 	NULL,
 };
-
 static struct attribute_group lcs_attr_group = {
 	.attrs = lcs_attrs,
 };
+static const struct attribute_group *lcs_attr_groups[] = {
+	&lcs_attr_group,
+	NULL,
+};
+static const struct device_type lcs_devtype = {
+	.name = "lcs",
+	.groups = lcs_attr_groups,
+};
 
 /**
  * lcs_probe_device is called on establishing a new ccwgroup_device.
@@ -2063,7 +2059,6 @@ static int
 lcs_probe_device(struct ccwgroup_device *ccwgdev)
 {
 	struct lcs_card *card;
-	int ret;
 
 	if (!get_device(&ccwgdev->dev))
 		return -ENODEV;
@@ -2075,12 +2070,6 @@ lcs_probe_device(struct ccwgroup_device *ccwgdev)
 		put_device(&ccwgdev->dev);
                 return -ENOMEM;
         }
-	ret = sysfs_create_group(&ccwgdev->dev.kobj, &lcs_attr_group);
-	if (ret) {
-		lcs_free_card(card);
-		put_device(&ccwgdev->dev);
-		return ret;
-        }
 	dev_set_drvdata(&ccwgdev->dev, card);
 	ccwgdev->cdev[0]->handler = lcs_irq;
 	ccwgdev->cdev[1]->handler = lcs_irq;
@@ -2089,7 +2078,9 @@ lcs_probe_device(struct ccwgroup_device *ccwgdev)
 	card->thread_start_mask = 0;
 	card->thread_allowed_mask = 0;
 	card->thread_running_mask = 0;
-        return 0;
+	ccwgdev->dev.type = &lcs_devtype;
+
+	return 0;
 }
 
 static int
@@ -2172,12 +2163,6 @@ lcs_new_device(struct ccwgroup_device *ccwgdev)
 		dev = alloc_etherdev(0);
 		break;
 #endif
-#ifdef CONFIG_TR
-	case LCS_FRAME_TYPE_TR:
-		card->lan_type_trans = tr_type_trans;
-		dev = alloc_trdev(0);
-		break;
-#endif
 #ifdef CONFIG_FDDI
 	case LCS_FRAME_TYPE_FDDI:
 		card->lan_type_trans = fddi_type_trans;
@@ -2323,9 +2308,9 @@ lcs_remove_device(struct ccwgroup_device *ccwgdev)
 	}
 	if (card->dev)
 		unregister_netdev(card->dev);
-	sysfs_remove_group(&ccwgdev->dev.kobj, &lcs_attr_group);
 	lcs_cleanup_card(card);
 	lcs_free_card(card);
+	dev_set_drvdata(&ccwgdev->dev, NULL);
 	put_device(&ccwgdev->dev);
 }
 
@@ -2410,9 +2395,7 @@ static struct ccwgroup_driver lcs_group_driver = {
 		.owner	= THIS_MODULE,
 		.name	= "lcs",
 	},
-	.max_slaves  = 2,
-	.driver_id   = 0xD3C3E2,
-	.probe       = lcs_probe_device,
+	.setup	     = lcs_probe_device,
 	.remove      = lcs_remove_device,
 	.set_online  = lcs_new_device,
 	.set_offline = lcs_shutdown_device,
@@ -2423,30 +2406,24 @@ static struct ccwgroup_driver lcs_group_driver = {
 	.restore     = lcs_restore,
 };
 
-static ssize_t
-lcs_driver_group_store(struct device_driver *ddrv, const char *buf,
-		       size_t count)
+static ssize_t lcs_driver_group_store(struct device_driver *ddrv,
+				      const char *buf, size_t count)
 {
 	int err;
-	err = ccwgroup_create_from_string(lcs_root_dev,
-					  lcs_group_driver.driver_id,
-					  &lcs_ccw_driver, 2, buf);
+	err = ccwgroup_create_dev(lcs_root_dev, &lcs_group_driver, 2, buf);
 	return err ? err : count;
 }
-
 static DRIVER_ATTR(group, 0200, NULL, lcs_driver_group_store);
 
-static struct attribute *lcs_group_attrs[] = {
+static struct attribute *lcs_drv_attrs[] = {
 	&driver_attr_group.attr,
 	NULL,
 };
-
-static struct attribute_group lcs_group_attr_group = {
-	.attrs = lcs_group_attrs,
+static struct attribute_group lcs_drv_attr_group = {
+	.attrs = lcs_drv_attrs,
 };
-
-static const struct attribute_group *lcs_group_attr_groups[] = {
-	&lcs_group_attr_group,
+static const struct attribute_group *lcs_drv_attr_groups[] = {
+	&lcs_drv_attr_group,
 	NULL,
 };
 
@@ -2470,7 +2447,7 @@ __init lcs_init_module(void)
 	rc = ccw_driver_register(&lcs_ccw_driver);
 	if (rc)
 		goto ccw_err;
-	lcs_group_driver.driver.groups = lcs_group_attr_groups;
+	lcs_group_driver.driver.groups = lcs_drv_attr_groups;
 	rc = ccwgroup_driver_register(&lcs_group_driver);
 	if (rc)
 		goto ccwgroup_err;
@@ -2496,8 +2473,6 @@ __exit lcs_cleanup_module(void)
 {
 	pr_info("Terminating lcs module.\n");
 	LCS_DBF_TEXT(0, trace, "cleanup");
-	driver_remove_file(&lcs_group_driver.driver,
-			   &driver_attr_group);
 	ccwgroup_driver_unregister(&lcs_group_driver);
 	ccw_driver_unregister(&lcs_ccw_driver);
 	root_device_unregister(lcs_root_dev);
diff --git a/drivers/s390/net/qeth_core.h b/drivers/s390/net/qeth_core.h
index ec7921b5138..06e8f31ff3d 100644
--- a/drivers/s390/net/qeth_core.h
+++ b/drivers/s390/net/qeth_core.h
@@ -13,8 +13,6 @@
 
 #include <linux/if.h>
 #include <linux/if_arp.h>
-#include <linux/if_tr.h>
-#include <linux/trdevice.h>
 #include <linux/etherdevice.h>
 #include <linux/if_vlan.h>
 #include <linux/ctype.h>
@@ -676,8 +674,6 @@ struct qeth_card_options {
 	struct qeth_ipa_info adp; /*Adapter parameters*/
 	struct qeth_routing_info route6;
 	struct qeth_ipa_info ipa6;
-	int broadcast_mode;
-	int macaddr_mode;
 	int fake_broadcast;
 	int add_hhlen;
 	int layer2;
@@ -711,7 +707,16 @@ struct qeth_discipline {
 	qdio_handler_t *input_handler;
 	qdio_handler_t *output_handler;
 	int (*recover)(void *ptr);
-	struct ccwgroup_driver *ccwgdriver;
+	int (*setup) (struct ccwgroup_device *);
+	void (*remove) (struct ccwgroup_device *);
+	int (*set_online) (struct ccwgroup_device *);
+	int (*set_offline) (struct ccwgroup_device *);
+	void (*shutdown)(struct ccwgroup_device *);
+	int (*prepare) (struct ccwgroup_device *);
+	void (*complete) (struct ccwgroup_device *);
+	int (*freeze)(struct ccwgroup_device *);
+	int (*thaw) (struct ccwgroup_device *);
+	int (*restore)(struct ccwgroup_device *);
 };
 
 struct qeth_vlan_vid {
@@ -775,7 +780,7 @@ struct qeth_card {
 	struct qeth_perf_stats perf_stats;
 	int read_or_write_problem;
 	struct qeth_osn_info osn_info;
-	struct qeth_discipline discipline;
+	struct qeth_discipline *discipline;
 	atomic_t force_alloc_skb;
 	struct service_level qeth_service_level;
 	struct qdio_ssqd_desc ssqd;
@@ -841,16 +846,15 @@ static inline int qeth_is_diagass_supported(struct qeth_card *card,
 	return card->info.diagass_support & (__u32)cmd;
 }
 
-extern struct ccwgroup_driver qeth_l2_ccwgroup_driver;
-extern struct ccwgroup_driver qeth_l3_ccwgroup_driver;
+extern struct qeth_discipline qeth_l2_discipline;
+extern struct qeth_discipline qeth_l3_discipline;
+extern const struct attribute_group *qeth_generic_attr_groups[];
+extern const struct attribute_group *qeth_osn_attr_groups[];
+
 const char *qeth_get_cardname_short(struct qeth_card *);
 int qeth_realloc_buffer_pool(struct qeth_card *, int);
 int qeth_core_load_discipline(struct qeth_card *, enum qeth_discipline_id);
 void qeth_core_free_discipline(struct qeth_card *);
-int qeth_core_create_device_attributes(struct device *);
-void qeth_core_remove_device_attributes(struct device *);
-int qeth_core_create_osn_attributes(struct device *);
-void qeth_core_remove_osn_attributes(struct device *);
 void qeth_buffer_reclaim_work(struct work_struct *);
 
 /* exports for qeth discipline device drivers */
diff --git a/drivers/s390/net/qeth_core_main.c b/drivers/s390/net/qeth_core_main.c
index 8334dadc681..e118e1e1e1c 100644
--- a/drivers/s390/net/qeth_core_main.c
+++ b/drivers/s390/net/qeth_core_main.c
@@ -1329,8 +1329,6 @@ static void qeth_set_intial_options(struct qeth_card *card)
 {
 	card->options.route4.type = NO_ROUTER;
 	card->options.route6.type = NO_ROUTER;
-	card->options.broadcast_mode = QETH_TR_BROADCAST_ALLRINGS;
-	card->options.macaddr_mode = QETH_TR_MACADDR_NONCANONICAL;
 	card->options.fake_broadcast = 0;
 	card->options.add_hhlen = DEFAULT_ADD_HHLEN;
 	card->options.performance_stats = 0;
@@ -1365,7 +1363,7 @@ static void qeth_start_kernel_thread(struct work_struct *work)
 	    card->write.state != CH_STATE_UP)
 		return;
 	if (qeth_do_start_thread(card, QETH_RECOVER_THREAD)) {
-		ts = kthread_run(card->discipline.recover, (void *)card,
+		ts = kthread_run(card->discipline->recover, (void *)card,
 				"qeth_recover");
 		if (IS_ERR(ts)) {
 			qeth_clear_thread_start_bit(card, QETH_RECOVER_THREAD);
@@ -3339,7 +3337,7 @@ static void qeth_flush_buffers(struct qeth_qdio_out_q *queue, int index,
 	if (rc) {
 		queue->card->stats.tx_errors += count;
 		/* ignore temporary SIGA errors without busy condition */
-		if (rc == QDIO_ERROR_SIGA_TARGET)
+		if (rc == -ENOBUFS)
 			return;
 		QETH_CARD_TEXT(queue->card, 2, "flushbuf");
 		QETH_CARD_TEXT_(queue->card, 2, " q%d", queue->queue_no);
@@ -3533,7 +3531,7 @@ void qeth_qdio_output_handler(struct ccw_device *ccwdev,
 	int i;
 
 	QETH_CARD_TEXT(card, 6, "qdouhdl");
-	if (qdio_error & QDIO_ERROR_ACTIVATE_CHECK_CONDITION) {
+	if (qdio_error & QDIO_ERROR_FATAL) {
 		QETH_CARD_TEXT(card, 2, "achkcond");
 		netif_stop_queue(card->dev);
 		qeth_schedule_recovery(card);
@@ -4629,7 +4627,7 @@ static int qeth_qdio_establish(struct qeth_card *card)
 		goto out_free_in_sbals;
 	}
 	for (i = 0; i < card->qdio.no_in_queues; ++i)
-		queue_start_poll[i] = card->discipline.start_poll;
+		queue_start_poll[i] = card->discipline->start_poll;
 
 	qeth_qdio_establish_cq(card, in_sbal_ptrs, queue_start_poll);
 
@@ -4653,8 +4651,8 @@ static int qeth_qdio_establish(struct qeth_card *card)
 	init_data.qib_param_field        = qib_param_field;
 	init_data.no_input_qs            = card->qdio.no_in_queues;
 	init_data.no_output_qs           = card->qdio.no_out_queues;
-	init_data.input_handler          = card->discipline.input_handler;
-	init_data.output_handler         = card->discipline.output_handler;
+	init_data.input_handler 	 = card->discipline->input_handler;
+	init_data.output_handler	 = card->discipline->output_handler;
 	init_data.queue_start_poll_array = queue_start_poll;
 	init_data.int_parm               = (unsigned long) card;
 	init_data.input_sbal_addr_array  = (void **) in_sbal_ptrs;
@@ -4739,13 +4737,6 @@ static struct ccw_driver qeth_ccw_driver = {
 	.remove = ccwgroup_remove_ccwdev,
 };
 
-static int qeth_core_driver_group(const char *buf, struct device *root_dev,
-				unsigned long driver_id)
-{
-	return ccwgroup_create_from_string(root_dev, driver_id,
-					   &qeth_ccw_driver, 3, buf);
-}
-
 int qeth_core_hardsetup_card(struct qeth_card *card)
 {
 	int retries = 0;
@@ -4911,11 +4902,7 @@ struct sk_buff *qeth_core_get_next_skb(struct qeth_card *card,
 		break;
 	case QETH_HEADER_TYPE_LAYER3:
 		skb_len = (*hdr)->hdr.l3.length;
-		if ((card->info.link_type == QETH_LINK_TYPE_LANE_TR) ||
-		    (card->info.link_type == QETH_LINK_TYPE_HSTR))
-			headroom = TR_HLEN;
-		else
-			headroom = ETH_HLEN;
+		headroom = ETH_HLEN;
 		break;
 	case QETH_HEADER_TYPE_OSN:
 		skb_len = (*hdr)->hdr.osn.pdu_length;
@@ -5046,17 +5033,15 @@ int qeth_core_load_discipline(struct qeth_card *card,
 	mutex_lock(&qeth_mod_mutex);
 	switch (discipline) {
 	case QETH_DISCIPLINE_LAYER3:
-		card->discipline.ccwgdriver = try_then_request_module(
-			symbol_get(qeth_l3_ccwgroup_driver),
-			"qeth_l3");
+		card->discipline = try_then_request_module(
+			symbol_get(qeth_l3_discipline), "qeth_l3");
 		break;
 	case QETH_DISCIPLINE_LAYER2:
-		card->discipline.ccwgdriver = try_then_request_module(
-			symbol_get(qeth_l2_ccwgroup_driver),
-			"qeth_l2");
+		card->discipline = try_then_request_module(
+			symbol_get(qeth_l2_discipline), "qeth_l2");
 		break;
 	}
-	if (!card->discipline.ccwgdriver) {
+	if (!card->discipline) {
 		dev_err(&card->gdev->dev, "There is no kernel module to "
 			"support discipline %d\n", discipline);
 		rc = -EINVAL;
@@ -5068,12 +5053,21 @@ int qeth_core_load_discipline(struct qeth_card *card,
 void qeth_core_free_discipline(struct qeth_card *card)
 {
 	if (card->options.layer2)
-		symbol_put(qeth_l2_ccwgroup_driver);
+		symbol_put(qeth_l2_discipline);
 	else
-		symbol_put(qeth_l3_ccwgroup_driver);
-	card->discipline.ccwgdriver = NULL;
+		symbol_put(qeth_l3_discipline);
+	card->discipline = NULL;
 }
 
+static const struct device_type qeth_generic_devtype = {
+	.name = "qeth_generic",
+	.groups = qeth_generic_attr_groups,
+};
+static const struct device_type qeth_osn_devtype = {
+	.name = "qeth_osn",
+	.groups = qeth_osn_attr_groups,
+};
+
 static int qeth_core_probe_device(struct ccwgroup_device *gdev)
 {
 	struct qeth_card *card;
@@ -5128,18 +5122,17 @@ static int qeth_core_probe_device(struct ccwgroup_device *gdev)
 	}
 
 	if (card->info.type == QETH_CARD_TYPE_OSN)
-		rc = qeth_core_create_osn_attributes(dev);
+		gdev->dev.type = &qeth_osn_devtype;
 	else
-		rc = qeth_core_create_device_attributes(dev);
-	if (rc)
-		goto err_dbf;
+		gdev->dev.type = &qeth_generic_devtype;
+
 	switch (card->info.type) {
 	case QETH_CARD_TYPE_OSN:
 	case QETH_CARD_TYPE_OSM:
 		rc = qeth_core_load_discipline(card, QETH_DISCIPLINE_LAYER2);
 		if (rc)
-			goto err_attr;
-		rc = card->discipline.ccwgdriver->probe(card->gdev);
+			goto err_dbf;
+		rc = card->discipline->setup(card->gdev);
 		if (rc)
 			goto err_disc;
 	case QETH_CARD_TYPE_OSD:
@@ -5157,11 +5150,6 @@ static int qeth_core_probe_device(struct ccwgroup_device *gdev)
 
 err_disc:
 	qeth_core_free_discipline(card);
-err_attr:
-	if (card->info.type == QETH_CARD_TYPE_OSN)
-		qeth_core_remove_osn_attributes(dev);
-	else
-		qeth_core_remove_device_attributes(dev);
 err_dbf:
 	debug_unregister(card->debug);
 err_card:
@@ -5178,14 +5166,8 @@ static void qeth_core_remove_device(struct ccwgroup_device *gdev)
 
 	QETH_DBF_TEXT(SETUP, 2, "removedv");
 
-	if (card->info.type == QETH_CARD_TYPE_OSN) {
-		qeth_core_remove_osn_attributes(&gdev->dev);
-	} else {
-		qeth_core_remove_device_attributes(&gdev->dev);
-	}
-
-	if (card->discipline.ccwgdriver) {
-		card->discipline.ccwgdriver->remove(gdev);
+	if (card->discipline) {
+		card->discipline->remove(gdev);
 		qeth_core_free_discipline(card);
 	}
 
@@ -5205,7 +5187,7 @@ static int qeth_core_set_online(struct ccwgroup_device *gdev)
 	int rc = 0;
 	int def_discipline;
 
-	if (!card->discipline.ccwgdriver) {
+	if (!card->discipline) {
 		if (card->info.type == QETH_CARD_TYPE_IQD)
 			def_discipline = QETH_DISCIPLINE_LAYER3;
 		else
@@ -5213,11 +5195,11 @@ static int qeth_core_set_online(struct ccwgroup_device *gdev)
 		rc = qeth_core_load_discipline(card, def_discipline);
 		if (rc)
 			goto err;
-		rc = card->discipline.ccwgdriver->probe(card->gdev);
+		rc = card->discipline->setup(card->gdev);
 		if (rc)
 			goto err;
 	}
-	rc = card->discipline.ccwgdriver->set_online(gdev);
+	rc = card->discipline->set_online(gdev);
 err:
 	return rc;
 }
@@ -5225,58 +5207,52 @@ err:
 static int qeth_core_set_offline(struct ccwgroup_device *gdev)
 {
 	struct qeth_card *card = dev_get_drvdata(&gdev->dev);
-	return card->discipline.ccwgdriver->set_offline(gdev);
+	return card->discipline->set_offline(gdev);
 }
 
 static void qeth_core_shutdown(struct ccwgroup_device *gdev)
 {
 	struct qeth_card *card = dev_get_drvdata(&gdev->dev);
-	if (card->discipline.ccwgdriver &&
-	    card->discipline.ccwgdriver->shutdown)
-		card->discipline.ccwgdriver->shutdown(gdev);
+	if (card->discipline && card->discipline->shutdown)
+		card->discipline->shutdown(gdev);
 }
 
 static int qeth_core_prepare(struct ccwgroup_device *gdev)
 {
 	struct qeth_card *card = dev_get_drvdata(&gdev->dev);
-	if (card->discipline.ccwgdriver &&
-	    card->discipline.ccwgdriver->prepare)
-		return card->discipline.ccwgdriver->prepare(gdev);
+	if (card->discipline && card->discipline->prepare)
+		return card->discipline->prepare(gdev);
 	return 0;
 }
 
 static void qeth_core_complete(struct ccwgroup_device *gdev)
 {
 	struct qeth_card *card = dev_get_drvdata(&gdev->dev);
-	if (card->discipline.ccwgdriver &&
-	    card->discipline.ccwgdriver->complete)
-		card->discipline.ccwgdriver->complete(gdev);
+	if (card->discipline && card->discipline->complete)
+		card->discipline->complete(gdev);
 }
 
 static int qeth_core_freeze(struct ccwgroup_device *gdev)
 {
 	struct qeth_card *card = dev_get_drvdata(&gdev->dev);
-	if (card->discipline.ccwgdriver &&
-	    card->discipline.ccwgdriver->freeze)
-		return card->discipline.ccwgdriver->freeze(gdev);
+	if (card->discipline && card->discipline->freeze)
+		return card->discipline->freeze(gdev);
 	return 0;
 }
 
 static int qeth_core_thaw(struct ccwgroup_device *gdev)
 {
 	struct qeth_card *card = dev_get_drvdata(&gdev->dev);
-	if (card->discipline.ccwgdriver &&
-	    card->discipline.ccwgdriver->thaw)
-		return card->discipline.ccwgdriver->thaw(gdev);
+	if (card->discipline && card->discipline->thaw)
+		return card->discipline->thaw(gdev);
 	return 0;
 }
 
 static int qeth_core_restore(struct ccwgroup_device *gdev)
 {
 	struct qeth_card *card = dev_get_drvdata(&gdev->dev);
-	if (card->discipline.ccwgdriver &&
-	    card->discipline.ccwgdriver->restore)
-		return card->discipline.ccwgdriver->restore(gdev);
+	if (card->discipline && card->discipline->restore)
+		return card->discipline->restore(gdev);
 	return 0;
 }
 
@@ -5285,8 +5261,7 @@ static struct ccwgroup_driver qeth_core_ccwgroup_driver = {
 		.owner = THIS_MODULE,
 		.name = "qeth",
 	},
-	.driver_id = 0xD8C5E3C8,
-	.probe = qeth_core_probe_device,
+	.setup = qeth_core_probe_device,
 	.remove = qeth_core_remove_device,
 	.set_online = qeth_core_set_online,
 	.set_offline = qeth_core_set_offline,
@@ -5298,21 +5273,30 @@ static struct ccwgroup_driver qeth_core_ccwgroup_driver = {
 	.restore = qeth_core_restore,
 };
 
-static ssize_t
-qeth_core_driver_group_store(struct device_driver *ddrv, const char *buf,
-			   size_t count)
+static ssize_t qeth_core_driver_group_store(struct device_driver *ddrv,
+					    const char *buf, size_t count)
 {
 	int err;
-	err = qeth_core_driver_group(buf, qeth_core_root_dev,
-					qeth_core_ccwgroup_driver.driver_id);
-	if (err)
-		return err;
-	else
-		return count;
-}
 
+	err = ccwgroup_create_dev(qeth_core_root_dev,
+				  &qeth_core_ccwgroup_driver, 3, buf);
+
+	return err ? err : count;
+}
 static DRIVER_ATTR(group, 0200, NULL, qeth_core_driver_group_store);
 
+static struct attribute *qeth_drv_attrs[] = {
+	&driver_attr_group.attr,
+	NULL,
+};
+static struct attribute_group qeth_drv_attr_group = {
+	.attrs = qeth_drv_attrs,
+};
+static const struct attribute_group *qeth_drv_attr_groups[] = {
+	&qeth_drv_attr_group,
+	NULL,
+};
+
 static struct {
 	const char str[ETH_GSTRING_LEN];
 } qeth_ethtool_stats_keys[] = {
@@ -5550,49 +5534,41 @@ static int __init qeth_core_init(void)
 	rc = qeth_register_dbf_views();
 	if (rc)
 		goto out_err;
-	rc = ccw_driver_register(&qeth_ccw_driver);
-	if (rc)
-		goto ccw_err;
-	rc = ccwgroup_driver_register(&qeth_core_ccwgroup_driver);
-	if (rc)
-		goto ccwgroup_err;
-	rc = driver_create_file(&qeth_core_ccwgroup_driver.driver,
-				&driver_attr_group);
-	if (rc)
-		goto driver_err;
 	qeth_core_root_dev = root_device_register("qeth");
 	rc = IS_ERR(qeth_core_root_dev) ? PTR_ERR(qeth_core_root_dev) : 0;
 	if (rc)
 		goto register_err;
-
 	qeth_core_header_cache = kmem_cache_create("qeth_hdr",
 			sizeof(struct qeth_hdr) + ETH_HLEN, 64, 0, NULL);
 	if (!qeth_core_header_cache) {
 		rc = -ENOMEM;
 		goto slab_err;
 	}
-
 	qeth_qdio_outbuf_cache = kmem_cache_create("qeth_buf",
 			sizeof(struct qeth_qdio_out_buffer), 0, 0, NULL);
 	if (!qeth_qdio_outbuf_cache) {
 		rc = -ENOMEM;
 		goto cqslab_err;
 	}
+	rc = ccw_driver_register(&qeth_ccw_driver);
+	if (rc)
+		goto ccw_err;
+	qeth_core_ccwgroup_driver.driver.groups = qeth_drv_attr_groups;
+	rc = ccwgroup_driver_register(&qeth_core_ccwgroup_driver);
+	if (rc)
+		goto ccwgroup_err;
 
 	return 0;
+
+ccwgroup_err:
+	ccw_driver_unregister(&qeth_ccw_driver);
+ccw_err:
+	kmem_cache_destroy(qeth_qdio_outbuf_cache);
 cqslab_err:
 	kmem_cache_destroy(qeth_core_header_cache);
 slab_err:
 	root_device_unregister(qeth_core_root_dev);
 register_err:
-	driver_remove_file(&qeth_core_ccwgroup_driver.driver,
-			   &driver_attr_group);
-driver_err:
-	ccwgroup_driver_unregister(&qeth_core_ccwgroup_driver);
-ccwgroup_err:
-	ccw_driver_unregister(&qeth_ccw_driver);
-ccw_err:
-	QETH_DBF_MESSAGE(2, "Initialization failed with code %d\n", rc);
 	qeth_unregister_dbf_views();
 out_err:
 	pr_err("Initializing the qeth device driver failed\n");
@@ -5601,13 +5577,11 @@ out_err:
 
 static void __exit qeth_core_exit(void)
 {
-	root_device_unregister(qeth_core_root_dev);
-	driver_remove_file(&qeth_core_ccwgroup_driver.driver,
-			   &driver_attr_group);
 	ccwgroup_driver_unregister(&qeth_core_ccwgroup_driver);
 	ccw_driver_unregister(&qeth_ccw_driver);
 	kmem_cache_destroy(qeth_qdio_outbuf_cache);
 	kmem_cache_destroy(qeth_core_header_cache);
+	root_device_unregister(qeth_core_root_dev);
 	qeth_unregister_dbf_views();
 	pr_info("core functions removed\n");
 }
diff --git a/drivers/s390/net/qeth_core_mpc.h b/drivers/s390/net/qeth_core_mpc.h
index ff41e42004a..a11b30c3842 100644
--- a/drivers/s390/net/qeth_core_mpc.h
+++ b/drivers/s390/net/qeth_core_mpc.h
@@ -70,16 +70,6 @@ enum qeth_link_types {
 	QETH_LINK_TYPE_ATM_NATIVE   = 0x90,
 };
 
-enum qeth_tr_macaddr_modes {
-	QETH_TR_MACADDR_NONCANONICAL = 0,
-	QETH_TR_MACADDR_CANONICAL    = 1,
-};
-
-enum qeth_tr_broadcast_modes {
-	QETH_TR_BROADCAST_ALLRINGS = 0,
-	QETH_TR_BROADCAST_LOCAL    = 1,
-};
-
 /*
  * Routing stuff
  */
diff --git a/drivers/s390/net/qeth_core_sys.c b/drivers/s390/net/qeth_core_sys.c
index 0a8e86c1b0e..f163af575c4 100644
--- a/drivers/s390/net/qeth_core_sys.c
+++ b/drivers/s390/net/qeth_core_sys.c
@@ -434,8 +434,8 @@ static ssize_t qeth_dev_layer2_store(struct device *dev,
 		goto out;
 	else {
 		card->info.mac_bits  = 0;
-		if (card->discipline.ccwgdriver) {
-			card->discipline.ccwgdriver->remove(card->gdev);
+		if (card->discipline) {
+			card->discipline->remove(card->gdev);
 			qeth_core_free_discipline(card);
 		}
 	}
@@ -444,7 +444,7 @@ static ssize_t qeth_dev_layer2_store(struct device *dev,
 	if (rc)
 		goto out;
 
-	rc = card->discipline.ccwgdriver->probe(card->gdev);
+	rc = card->discipline->setup(card->gdev);
 out:
 	mutex_unlock(&card->discipline_mutex);
 	return rc ? rc : count;
@@ -693,7 +693,6 @@ static struct attribute *qeth_blkt_device_attrs[] = {
 	&dev_attr_inter_jumbo.attr,
 	NULL,
 };
-
 static struct attribute_group qeth_device_blkt_group = {
 	.name = "blkt",
 	.attrs = qeth_blkt_device_attrs,
@@ -716,11 +715,16 @@ static struct attribute *qeth_device_attrs[] = {
 	&dev_attr_hw_trap.attr,
 	NULL,
 };
-
 static struct attribute_group qeth_device_attr_group = {
 	.attrs = qeth_device_attrs,
 };
 
+const struct attribute_group *qeth_generic_attr_groups[] = {
+	&qeth_device_attr_group,
+	&qeth_device_blkt_group,
+	NULL,
+};
+
 static struct attribute *qeth_osn_device_attrs[] = {
 	&dev_attr_state.attr,
 	&dev_attr_chpid.attr,
@@ -730,37 +734,10 @@ static struct attribute *qeth_osn_device_attrs[] = {
 	&dev_attr_recover.attr,
 	NULL,
 };
-
 static struct attribute_group qeth_osn_device_attr_group = {
 	.attrs = qeth_osn_device_attrs,
 };
-
-int qeth_core_create_device_attributes(struct device *dev)
-{
-	int ret;
-	ret = sysfs_create_group(&dev->kobj, &qeth_device_attr_group);
-	if (ret)
-		return ret;
-	ret = sysfs_create_group(&dev->kobj, &qeth_device_blkt_group);
-	if (ret)
-		sysfs_remove_group(&dev->kobj, &qeth_device_attr_group);
-
-	return 0;
-}
-
-void qeth_core_remove_device_attributes(struct device *dev)
-{
-	sysfs_remove_group(&dev->kobj, &qeth_device_attr_group);
-	sysfs_remove_group(&dev->kobj, &qeth_device_blkt_group);
-}
-
-int qeth_core_create_osn_attributes(struct device *dev)
-{
-	return sysfs_create_group(&dev->kobj, &qeth_osn_device_attr_group);
-}
-
-void qeth_core_remove_osn_attributes(struct device *dev)
-{
-	sysfs_remove_group(&dev->kobj, &qeth_osn_device_attr_group);
-	return;
-}
+const struct attribute_group *qeth_osn_attr_groups[] = {
+	&qeth_osn_device_attr_group,
+	NULL,
+};
diff --git a/drivers/s390/net/qeth_l2_main.c b/drivers/s390/net/qeth_l2_main.c
index 0e7c29d1d7e..426986518e9 100644
--- a/drivers/s390/net/qeth_l2_main.c
+++ b/drivers/s390/net/qeth_l2_main.c
@@ -882,12 +882,6 @@ static int qeth_l2_probe_device(struct ccwgroup_device *gdev)
 	INIT_LIST_HEAD(&card->mc_list);
 	card->options.layer2 = 1;
 	card->info.hwtrap = 0;
-	card->discipline.start_poll = qeth_qdio_start_poll;
-	card->discipline.input_handler = (qdio_handler_t *)
-		qeth_qdio_input_handler;
-	card->discipline.output_handler = (qdio_handler_t *)
-		qeth_qdio_output_handler;
-	card->discipline.recover = qeth_l2_recover;
 	return 0;
 }
 
@@ -1227,8 +1221,12 @@ out:
 	return rc;
 }
 
-struct ccwgroup_driver qeth_l2_ccwgroup_driver = {
-	.probe = qeth_l2_probe_device,
+struct qeth_discipline qeth_l2_discipline = {
+	.start_poll = qeth_qdio_start_poll,
+	.input_handler = (qdio_handler_t *) qeth_qdio_input_handler,
+	.output_handler = (qdio_handler_t *) qeth_qdio_output_handler,
+	.recover = qeth_l2_recover,
+	.setup = qeth_l2_probe_device,
 	.remove = qeth_l2_remove_device,
 	.set_online = qeth_l2_set_online,
 	.set_offline = qeth_l2_set_offline,
@@ -1237,7 +1235,7 @@ struct ccwgroup_driver qeth_l2_ccwgroup_driver = {
 	.thaw = qeth_l2_pm_resume,
 	.restore = qeth_l2_pm_resume,
 };
-EXPORT_SYMBOL_GPL(qeth_l2_ccwgroup_driver);
+EXPORT_SYMBOL_GPL(qeth_l2_discipline);
 
 static int qeth_osn_send_control_data(struct qeth_card *card, int len,
 			   struct qeth_cmd_buffer *iob)
diff --git a/drivers/s390/net/qeth_l3_main.c b/drivers/s390/net/qeth_l3_main.c
index f8592160768..7be5e977569 100644
--- a/drivers/s390/net/qeth_l3_main.c
+++ b/drivers/s390/net/qeth_l3_main.c
@@ -976,57 +976,6 @@ static inline u8 qeth_l3_get_qeth_hdr_flags6(int cast_type)
 	return ct | QETH_CAST_UNICAST;
 }
 
-static int qeth_l3_send_setadp_mode(struct qeth_card *card, __u32 command,
-					__u32 mode)
-{
-	int rc;
-	struct qeth_cmd_buffer *iob;
-	struct qeth_ipa_cmd *cmd;
-
-	QETH_CARD_TEXT(card, 4, "adpmode");
-
-	iob = qeth_get_adapter_cmd(card, command,
-				   sizeof(struct qeth_ipacmd_setadpparms));
-	cmd = (struct qeth_ipa_cmd *)(iob->data+IPA_PDU_HEADER_SIZE);
-	cmd->data.setadapterparms.data.mode = mode;
-	rc = qeth_send_ipa_cmd(card, iob, qeth_default_setadapterparms_cb,
-			       NULL);
-	return rc;
-}
-
-static int qeth_l3_setadapter_hstr(struct qeth_card *card)
-{
-	int rc;
-
-	QETH_CARD_TEXT(card, 4, "adphstr");
-
-	if (qeth_adp_supported(card, IPA_SETADP_SET_BROADCAST_MODE)) {
-		rc = qeth_l3_send_setadp_mode(card,
-					IPA_SETADP_SET_BROADCAST_MODE,
-					card->options.broadcast_mode);
-		if (rc)
-			QETH_DBF_MESSAGE(2, "couldn't set broadcast mode on "
-				   "device %s: x%x\n",
-				   CARD_BUS_ID(card), rc);
-		rc = qeth_l3_send_setadp_mode(card,
-					IPA_SETADP_ALTER_MAC_ADDRESS,
-					card->options.macaddr_mode);
-		if (rc)
-			QETH_DBF_MESSAGE(2, "couldn't set macaddr mode on "
-				   "device %s: x%x\n", CARD_BUS_ID(card), rc);
-		return rc;
-	}
-	if (card->options.broadcast_mode == QETH_TR_BROADCAST_LOCAL)
-		QETH_DBF_MESSAGE(2, "set adapter parameters not available "
-			   "to set broadcast mode, using ALLRINGS "
-			   "on device %s:\n", CARD_BUS_ID(card));
-	if (card->options.macaddr_mode == QETH_TR_MACADDR_CANONICAL)
-		QETH_DBF_MESSAGE(2, "set adapter parameters not available "
-			   "to set macaddr mode, using NONCANONICAL "
-			   "on device %s:\n", CARD_BUS_ID(card));
-	return 0;
-}
-
 static int qeth_l3_setadapter_parms(struct qeth_card *card)
 {
 	int rc;
@@ -1052,10 +1001,6 @@ static int qeth_l3_setadapter_parms(struct qeth_card *card)
 				" address failed\n");
 	}
 
-	if ((card->info.link_type == QETH_LINK_TYPE_HSTR) ||
-	    (card->info.link_type == QETH_LINK_TYPE_LANE_TR))
-		rc = qeth_l3_setadapter_hstr(card);
-
 	return rc;
 }
 
@@ -1671,10 +1616,7 @@ qeth_diags_trace(struct qeth_card *card, enum qeth_diags_trace_cmds diags_cmd)
 static void qeth_l3_get_mac_for_ipm(__u32 ipm, char *mac,
 				struct net_device *dev)
 {
-	if (dev->type == ARPHRD_IEEE802_TR)
-		ip_tr_mc_map(ipm, mac);
-	else
-		ip_eth_mc_map(ipm, mac);
+	ip_eth_mc_map(ipm, mac);
 }
 
 static void qeth_l3_add_mc(struct qeth_card *card, struct in_device *in4_dev)
@@ -1922,8 +1864,6 @@ static inline int qeth_l3_rebuild_skb(struct qeth_card *card,
 #endif
 			case __constant_htons(ETH_P_IP):
 				ip_hdr = (struct iphdr *)skb->data;
-				(card->dev->type == ARPHRD_IEEE802_TR) ?
-				ip_tr_mc_map(ip_hdr->daddr, tg_addr):
 				ip_eth_mc_map(ip_hdr->daddr, tg_addr);
 				break;
 			default:
@@ -1959,12 +1899,7 @@ static inline int qeth_l3_rebuild_skb(struct qeth_card *card,
 				tg_addr, "FAKELL", card->dev->addr_len);
 	}
 
-#ifdef CONFIG_TR
-	if (card->dev->type == ARPHRD_IEEE802_TR)
-		skb->protocol = tr_type_trans(skb, card->dev);
-	else
-#endif
-		skb->protocol = eth_type_trans(skb, card->dev);
+	skb->protocol = eth_type_trans(skb, card->dev);
 
 	if (hdr->hdr.l3.ext_flags &
 	    (QETH_HDR_EXT_VLAN_FRAME | QETH_HDR_EXT_INCLUDE_VLAN_TAG)) {
@@ -2138,7 +2073,7 @@ static int qeth_l3_verify_vlan_dev(struct net_device *dev,
 		struct net_device *netdev;
 
 		rcu_read_lock();
-		netdev = __vlan_find_dev_deep(dev, vid);
+		netdev = __vlan_find_dev_deep(card->dev, vid);
 		rcu_read_unlock();
 		if (netdev == dev) {
 			rc = QETH_VLAN_CARD;
@@ -2883,13 +2818,7 @@ static void qeth_l3_fill_header(struct qeth_card *card, struct qeth_hdr *hdr,
 			hdr->hdr.l3.flags &= ~QETH_HDR_PASSTHRU;
 		memcpy(hdr->hdr.l3.dest_addr, pkey, 16);
 	} else {
-		/* passthrough */
-		if ((skb->dev->type == ARPHRD_IEEE802_TR) &&
-			!memcmp(skb->data + sizeof(struct qeth_hdr) +
-			sizeof(__u16), skb->dev->broadcast, 6)) {
-			hdr->hdr.l3.flags = QETH_CAST_BROADCAST |
-						QETH_HDR_PASSTHRU;
-		} else if (!memcmp(skb->data + sizeof(struct qeth_hdr),
+		if (!memcmp(skb->data + sizeof(struct qeth_hdr),
 			    skb->dev->broadcast, 6)) {
 			/* broadcast? */
 			hdr->hdr.l3.flags = QETH_CAST_BROADCAST |
@@ -3031,10 +2960,7 @@ static int qeth_l3_hard_start_xmit(struct sk_buff *skb, struct net_device *dev)
 			skb_pull(new_skb, ETH_HLEN);
 	} else {
 		if (ipv == 4) {
-			if (card->dev->type == ARPHRD_IEEE802_TR)
-				skb_pull(new_skb, TR_HLEN);
-			else
-				skb_pull(new_skb, ETH_HLEN);
+			skb_pull(new_skb, ETH_HLEN);
 		}
 
 		if (ipv != 4 && vlan_tx_tag_present(new_skb)) {
@@ -3318,12 +3244,8 @@ static int qeth_l3_setup_netdev(struct qeth_card *card)
 	    card->info.type == QETH_CARD_TYPE_OSX) {
 		if ((card->info.link_type == QETH_LINK_TYPE_LANE_TR) ||
 		    (card->info.link_type == QETH_LINK_TYPE_HSTR)) {
-#ifdef CONFIG_TR
-			card->dev = alloc_trdev(0);
-#endif
-			if (!card->dev)
-				return -ENODEV;
-			card->dev->netdev_ops = &qeth_l3_netdev_ops;
+			pr_info("qeth_l3: ignoring TR device\n");
+			return -ENODEV;
 		} else {
 			card->dev = alloc_etherdev(0);
 			if (!card->dev)
@@ -3376,12 +3298,6 @@ static int qeth_l3_probe_device(struct ccwgroup_device *gdev)
 	qeth_l3_create_device_attributes(&gdev->dev);
 	card->options.layer2 = 0;
 	card->info.hwtrap = 0;
-	card->discipline.start_poll = qeth_qdio_start_poll;
-	card->discipline.input_handler = (qdio_handler_t *)
-		qeth_qdio_input_handler;
-	card->discipline.output_handler = (qdio_handler_t *)
-		qeth_qdio_output_handler;
-	card->discipline.recover = qeth_l3_recover;
 	return 0;
 }
 
@@ -3656,8 +3572,12 @@ out:
 	return rc;
 }
 
-struct ccwgroup_driver qeth_l3_ccwgroup_driver = {
-	.probe = qeth_l3_probe_device,
+struct qeth_discipline qeth_l3_discipline = {
+	.start_poll = qeth_qdio_start_poll,
+	.input_handler = (qdio_handler_t *) qeth_qdio_input_handler,
+	.output_handler = (qdio_handler_t *) qeth_qdio_output_handler,
+	.recover = qeth_l3_recover,
+	.setup = qeth_l3_probe_device,
 	.remove = qeth_l3_remove_device,
 	.set_online = qeth_l3_set_online,
 	.set_offline = qeth_l3_set_offline,
@@ -3666,7 +3586,7 @@ struct ccwgroup_driver qeth_l3_ccwgroup_driver = {
 	.thaw = qeth_l3_pm_resume,
 	.restore = qeth_l3_pm_resume,
 };
-EXPORT_SYMBOL_GPL(qeth_l3_ccwgroup_driver);
+EXPORT_SYMBOL_GPL(qeth_l3_discipline);
 
 static int qeth_l3_ip_event(struct notifier_block *this,
 			    unsigned long event, void *ptr)
@@ -3680,9 +3600,9 @@ static int qeth_l3_ip_event(struct notifier_block *this,
 		return NOTIFY_DONE;
 
 	card = qeth_l3_get_card_from_dev(dev);
-	QETH_CARD_TEXT(card, 3, "ipevent");
 	if (!card)
 		return NOTIFY_DONE;
+	QETH_CARD_TEXT(card, 3, "ipevent");
 
 	addr = qeth_l3_get_addr_buffer(QETH_PROT_IPV4);
 	if (addr != NULL) {
diff --git a/drivers/s390/net/qeth_l3_sys.c b/drivers/s390/net/qeth_l3_sys.c
index d979bb26522..4cafedf950a 100644
--- a/drivers/s390/net/qeth_l3_sys.c
+++ b/drivers/s390/net/qeth_l3_sys.c
@@ -175,116 +175,6 @@ out:
 static DEVICE_ATTR(fake_broadcast, 0644, qeth_l3_dev_fake_broadcast_show,
 		   qeth_l3_dev_fake_broadcast_store);
 
-static ssize_t qeth_l3_dev_broadcast_mode_show(struct device *dev,
-				struct device_attribute *attr, char *buf)
-{
-	struct qeth_card *card = dev_get_drvdata(dev);
-
-	if (!card)
-		return -EINVAL;
-
-	if (!((card->info.link_type == QETH_LINK_TYPE_HSTR) ||
-	      (card->info.link_type == QETH_LINK_TYPE_LANE_TR)))
-		return sprintf(buf, "n/a\n");
-
-	return sprintf(buf, "%s\n", (card->options.broadcast_mode ==
-				     QETH_TR_BROADCAST_ALLRINGS)?
-		       "all rings":"local");
-}
-
-static ssize_t qeth_l3_dev_broadcast_mode_store(struct device *dev,
-		struct device_attribute *attr, const char *buf, size_t count)
-{
-	struct qeth_card *card = dev_get_drvdata(dev);
-	char *tmp;
-	int rc = 0;
-
-	if (!card)
-		return -EINVAL;
-
-	mutex_lock(&card->conf_mutex);
-	if ((card->state != CARD_STATE_DOWN) &&
-	    (card->state != CARD_STATE_RECOVER)) {
-		rc = -EPERM;
-		goto out;
-	}
-
-	if (!((card->info.link_type == QETH_LINK_TYPE_HSTR) ||
-	      (card->info.link_type == QETH_LINK_TYPE_LANE_TR))) {
-		rc = -EINVAL;
-		goto out;
-	}
-
-	tmp = strsep((char **) &buf, "\n");
-
-	if (!strcmp(tmp, "local"))
-		card->options.broadcast_mode = QETH_TR_BROADCAST_LOCAL;
-	else if (!strcmp(tmp, "all_rings"))
-		card->options.broadcast_mode = QETH_TR_BROADCAST_ALLRINGS;
-	else
-		rc = -EINVAL;
-out:
-	mutex_unlock(&card->conf_mutex);
-	return rc ? rc : count;
-}
-
-static DEVICE_ATTR(broadcast_mode, 0644, qeth_l3_dev_broadcast_mode_show,
-		   qeth_l3_dev_broadcast_mode_store);
-
-static ssize_t qeth_l3_dev_canonical_macaddr_show(struct device *dev,
-				struct device_attribute *attr, char *buf)
-{
-	struct qeth_card *card = dev_get_drvdata(dev);
-
-	if (!card)
-		return -EINVAL;
-
-	if (!((card->info.link_type == QETH_LINK_TYPE_HSTR) ||
-	      (card->info.link_type == QETH_LINK_TYPE_LANE_TR)))
-		return sprintf(buf, "n/a\n");
-
-	return sprintf(buf, "%i\n", (card->options.macaddr_mode ==
-				     QETH_TR_MACADDR_CANONICAL)? 1:0);
-}
-
-static ssize_t qeth_l3_dev_canonical_macaddr_store(struct device *dev,
-		struct device_attribute *attr, const char *buf, size_t count)
-{
-	struct qeth_card *card = dev_get_drvdata(dev);
-	char *tmp;
-	int i, rc = 0;
-
-	if (!card)
-		return -EINVAL;
-
-	mutex_lock(&card->conf_mutex);
-	if ((card->state != CARD_STATE_DOWN) &&
-	    (card->state != CARD_STATE_RECOVER)) {
-		rc = -EPERM;
-		goto out;
-	}
-
-	if (!((card->info.link_type == QETH_LINK_TYPE_HSTR) ||
-	      (card->info.link_type == QETH_LINK_TYPE_LANE_TR))) {
-		rc = -EINVAL;
-		goto out;
-	}
-
-	i = simple_strtoul(buf, &tmp, 16);
-	if ((i == 0) || (i == 1))
-		card->options.macaddr_mode = i?
-			QETH_TR_MACADDR_CANONICAL :
-			QETH_TR_MACADDR_NONCANONICAL;
-	else
-		rc = -EINVAL;
-out:
-	mutex_unlock(&card->conf_mutex);
-	return rc ? rc : count;
-}
-
-static DEVICE_ATTR(canonical_macaddr, 0644, qeth_l3_dev_canonical_macaddr_show,
-		   qeth_l3_dev_canonical_macaddr_store);
-
 static ssize_t qeth_l3_dev_sniffer_show(struct device *dev,
 		struct device_attribute *attr, char *buf)
 {
@@ -458,8 +348,6 @@ static struct attribute *qeth_l3_device_attrs[] = {
 	&dev_attr_route4.attr,
 	&dev_attr_route6.attr,
 	&dev_attr_fake_broadcast.attr,
-	&dev_attr_broadcast_mode.attr,
-	&dev_attr_canonical_macaddr.attr,
 	&dev_attr_sniffer.attr,
 	&dev_attr_hsuid.attr,
 	NULL,
diff --git a/drivers/scsi/Kconfig b/drivers/scsi/Kconfig
index 29684c8142b..e9559782d3e 100644
--- a/drivers/scsi/Kconfig
+++ b/drivers/scsi/Kconfig
@@ -408,6 +408,7 @@ config BLK_DEV_3W_XXXX_RAID
 config SCSI_HPSA
 	tristate "HP Smart Array SCSI driver"
 	depends on PCI && SCSI
+	select CHECK_SIGNATURE
 	help
 	  This driver supports HP Smart Array Controllers (circa 2009).
 	  It is a SCSI alternative to the cciss driver, which is a block
@@ -807,19 +808,6 @@ config SCSI_FUTURE_DOMAIN
 	  To compile this driver as a module, choose M here: the
 	  module will be called fdomain.
 
-config SCSI_FD_MCS
-	tristate "Future Domain MCS-600/700 SCSI support"
-	depends on MCA_LEGACY && SCSI
-	---help---
-	  This is support for Future Domain MCS 600/700 MCA SCSI adapters.
-	  Some PS/2 computers are equipped with IBM Fast SCSI Adapter/A which
-	  is identical to the MCS 700 and hence also supported by this driver.
-	  This driver also supports the Reply SB16/SCSI card (the SCSI part).
-	  It supports multiple adapters in the same system.
-
-	  To compile this driver as a module, choose M here: the
-	  module will be called fd_mcs.
-
 config SCSI_GDTH
 	tristate "Intel/ICP (former GDT SCSI Disk Array) RAID Controller support"
 	depends on (ISA || EISA || PCI) && SCSI && ISA_DMA_API
@@ -889,76 +877,6 @@ config SCSI_GENERIC_NCR53C400
 	  not detect your card.  See the file
 	  <file:Documentation/scsi/g_NCR5380.txt> for details.
 
-config SCSI_IBMMCA
-	tristate "IBMMCA SCSI support"
-	depends on MCA && SCSI
-	---help---
-	  This is support for the IBM SCSI adapter found in many of the PS/2
-	  series computers.  These machines have an MCA bus, so you need to
-	  answer Y to "MCA support" as well and read
-	  <file:Documentation/mca.txt>.
-
-	  If the adapter isn't found during boot (a common problem for models
-	  56, 57, 76, and 77) you'll need to use the 'ibmmcascsi=<pun>' kernel
-	  option, where <pun> is the id of the SCSI subsystem (usually 7, but
-	  if that doesn't work check your reference diskette).  Owners of
-	  model 95 with a LED-matrix-display can in addition activate some
-	  activity info like under OS/2, but more informative, by setting
-	  'ibmmcascsi=display' as an additional kernel parameter.  Try "man
-	  bootparam" or see the documentation of your boot loader about how to
-	  pass options to the kernel.
-
-	  To compile this driver as a module, choose M here: the
-	  module will be called ibmmca.
-
-config IBMMCA_SCSI_ORDER_STANDARD
-	bool "Standard SCSI-order"
-	depends on SCSI_IBMMCA
-	---help---
-	  In the PC-world and in most modern SCSI-BIOS-setups, SCSI-hard disks
-	  are assigned to the drive letters, starting with the lowest SCSI-id
-	  (physical number -- pun) to be drive C:, as seen from DOS and
-	  similar operating systems. When looking into papers describing the
-	  ANSI-SCSI-standard, this assignment of drives appears to be wrong.
-	  The SCSI-standard follows a hardware-hierarchy which says that id 7
-	  has the highest priority and id 0 the lowest. Therefore, the host
-	  adapters are still today everywhere placed as SCSI-id 7 by default.
-	  In the SCSI-standard, the drive letters express the priority of the
-	  disk. C: should be the hard disk, or a partition on it, with the
-	  highest priority. This must therefore be the disk with the highest
-	  SCSI-id (e.g. 6) and not the one with the lowest! IBM-BIOS kept the
-	  original definition of the SCSI-standard as also industrial- and
-	  process-control-machines, like VME-CPUs running under realtime-OSes
-	  (e.g. LynxOS, OS9) do.
-
-	  If you like to run Linux on your MCA-machine with the same
-	  assignment of hard disks as seen from e.g. DOS or OS/2 on your
-	  machine, which is in addition conformant to the SCSI-standard, you
-	  must say Y here. This is also necessary for MCA-Linux users who want
-	  to keep downward compatibility to older releases of the
-	  IBM-MCA-SCSI-driver (older than driver-release 2.00 and older than
-	  June 1997).
-
-	  If you like to have the lowest SCSI-id assigned as drive C:, as
-	  modern SCSI-BIOSes do, which does not conform to the standard, but
-	  is widespread and common in the PC-world of today, you must say N
-	  here. If unsure, say Y.
-
-config IBMMCA_SCSI_DEV_RESET
-	bool "Reset SCSI-devices at boottime"
-	depends on SCSI_IBMMCA
-	---help---
-	  By default, SCSI-devices are reset when the machine is powered on.
-	  However, some devices exist, like special-control-devices,
-	  SCSI-CNC-machines, SCSI-printer or scanners of older type, that do
-	  not reset when switched on. If you say Y here, each device connected
-	  to your SCSI-bus will be issued a reset-command after it has been
-	  probed, while the kernel is booting. This may cause problems with
-	  more modern devices, like hard disks, which do not appreciate these
-	  reset commands, and can cause your system to hang. So say Y only if
-	  you know that one of your older devices needs it; N is the safe
-	  answer.
-
 config SCSI_IPS
 	tristate "IBM ServeRAID support"
 	depends on PCI && SCSI
diff --git a/drivers/scsi/Makefile b/drivers/scsi/Makefile
index 8deedeaf560..1a3368b0861 100644
--- a/drivers/scsi/Makefile
+++ b/drivers/scsi/Makefile
@@ -75,7 +75,6 @@ obj-$(CONFIG_SCSI_AIC94XX)	+= aic94xx/
 obj-$(CONFIG_SCSI_PM8001)	+= pm8001/
 obj-$(CONFIG_SCSI_ISCI)		+= isci/
 obj-$(CONFIG_SCSI_IPS)		+= ips.o
-obj-$(CONFIG_SCSI_FD_MCS)	+= fd_mcs.o
 obj-$(CONFIG_SCSI_FUTURE_DOMAIN)+= fdomain.o
 obj-$(CONFIG_SCSI_IN2000)	+= in2000.o
 obj-$(CONFIG_SCSI_GENERIC_NCR5380) += g_NCR5380.o
@@ -100,7 +99,6 @@ obj-$(CONFIG_SCSI_SYM53C8XX_2)	+= sym53c8xx_2/
 obj-$(CONFIG_SCSI_ZALON)	+= zalon7xx.o
 obj-$(CONFIG_SCSI_EATA_PIO)	+= eata_pio.o
 obj-$(CONFIG_SCSI_7000FASST)	+= wd7000.o
-obj-$(CONFIG_SCSI_IBMMCA)	+= ibmmca.o
 obj-$(CONFIG_SCSI_EATA)		+= eata.o
 obj-$(CONFIG_SCSI_DC395x)	+= dc395x.o
 obj-$(CONFIG_SCSI_DC390T)	+= tmscsim.o
diff --git a/drivers/scsi/aacraid/src.c b/drivers/scsi/aacraid/src.c
index 2bee51506a9..76282063630 100644
--- a/drivers/scsi/aacraid/src.c
+++ b/drivers/scsi/aacraid/src.c
@@ -424,6 +424,8 @@ static int aac_src_deliver_message(struct fib *fib)
 static int aac_src_ioremap(struct aac_dev *dev, u32 size)
 {
 	if (!size) {
+		iounmap(dev->regs.src.bar1);
+		dev->regs.src.bar1 = NULL;
 		iounmap(dev->regs.src.bar0);
 		dev->base = dev->regs.src.bar0 = NULL;
 		return 0;
diff --git a/drivers/scsi/aha1542.c b/drivers/scsi/aha1542.c
index ede91f37800..f79c8f9e33a 100644
--- a/drivers/scsi/aha1542.c
+++ b/drivers/scsi/aha1542.c
@@ -22,7 +22,7 @@
  *        Added module command-line options
  *        19-Jul-99
  *  Modified by Adam Fritzler
- *        Added proper detection of the AHA-1640 (MCA version of AHA-1540)
+ *        Added proper detection of the AHA-1640 (MCA, now deleted)
  */
 
 #include <linux/module.h>
@@ -37,8 +37,6 @@
 #include <linux/spinlock.h>
 #include <linux/isapnp.h>
 #include <linux/blkdev.h>
-#include <linux/mca.h>
-#include <linux/mca-legacy.h>
 #include <linux/slab.h>
 
 #include <asm/dma.h>
@@ -71,7 +69,7 @@
 #define MAXBOARDS 4		/* Increase this and the sizes of the
 				   arrays below, if you need more.. */
 
-/* Boards 3,4 slots are reserved for ISAPnP/MCA scans */
+/* Boards 3,4 slots are reserved for ISAPnP scans */
 
 static unsigned int bases[MAXBOARDS] __initdata = {0x330, 0x334, 0, 0};
 
@@ -1009,66 +1007,6 @@ static int __init aha1542_detect(struct scsi_host_template * tpnt)
 #endif
 
 	/*
-	 *	Find MicroChannel cards (AHA1640)
-	 */
-#ifdef CONFIG_MCA_LEGACY
-	if(MCA_bus) {
-		int slot = 0;
-		int pos = 0;
-
-		for (indx = 0; (slot != MCA_NOTFOUND) && (indx < ARRAY_SIZE(bases)); indx++) {
-
-			if (bases[indx])
-				continue;
-
-			/* Detect only AHA-1640 cards -- MCA ID 0F1F */
-			slot = mca_find_unused_adapter(0x0f1f, slot);
-			if (slot == MCA_NOTFOUND)
-				break;
-
-			/* Found one */
-			pos = mca_read_stored_pos(slot, 3);
-
-			/* Decode address */
-			if (pos & 0x80) {
-				if (pos & 0x02) {
-					if (pos & 0x01)
-						bases[indx] = 0x334;
-					else
-						bases[indx] = 0x234;
-				} else {
-					if (pos & 0x01)
-						bases[indx] = 0x134;
-				}
-			} else {
-				if (pos & 0x02) {
-					if (pos & 0x01)
-						bases[indx] = 0x330;
-					else
-						bases[indx] = 0x230;
-				} else {
-					if (pos & 0x01)
-						bases[indx] = 0x130;
-				}
-			}
-
-			/* No need to decode IRQ and Arb level -- those are
-			 * read off the card later.
-			 */
-			printk(KERN_INFO "Found an AHA-1640 in MCA slot %d, I/O 0x%04x\n", slot, bases[indx]);
-
-			mca_set_adapter_name(slot, "Adapter AHA-1640");
-			mca_set_adapter_procfn(slot, NULL, NULL);
-			mca_mark_as_used(slot);
-
-			/* Go on */
-			slot++;
-		}
-
-	}
-#endif
-
-	/*
 	 *	Hunt for ISA Plug'n'Pray Adaptecs (AHA1535)
 	 */
 
diff --git a/drivers/scsi/aic94xx/aic94xx_seq.c b/drivers/scsi/aic94xx/aic94xx_seq.c
index 390168f62a1..5fdca93892a 100644
--- a/drivers/scsi/aic94xx/aic94xx_seq.c
+++ b/drivers/scsi/aic94xx/aic94xx_seq.c
@@ -1228,8 +1228,7 @@ static int asd_seq_start_lseq(struct asd_ha_struct *asd_ha, int lseq)
 
 int asd_release_firmware(void)
 {
-	if (sequencer_fw)
-		release_firmware(sequencer_fw);
+	release_firmware(sequencer_fw);
 	return 0;
 }
 
diff --git a/drivers/scsi/atari_scsi.c b/drivers/scsi/atari_scsi.c
index 04a154f87e3..df740cbbaef 100644
--- a/drivers/scsi/atari_scsi.c
+++ b/drivers/scsi/atari_scsi.c
@@ -572,7 +572,7 @@ static void falcon_get_lock(void)
 }
 
 
-int __init atari_scsi_detect(struct scsi_host_template *host)
+static int __init atari_scsi_detect(struct scsi_host_template *host)
 {
 	static int called = 0;
 	struct Scsi_Host *instance;
@@ -724,7 +724,7 @@ int __init atari_scsi_detect(struct scsi_host_template *host)
 	return 1;
 }
 
-int atari_scsi_release(struct Scsi_Host *sh)
+static int atari_scsi_release(struct Scsi_Host *sh)
 {
 	if (IS_A_TT())
 		free_irq(IRQ_TT_MFP_SCSI, sh);
@@ -734,17 +734,21 @@ int atari_scsi_release(struct Scsi_Host *sh)
 	return 1;
 }
 
-void __init atari_scsi_setup(char *str, int *ints)
+#ifndef MODULE
+static int __init atari_scsi_setup(char *str)
 {
 	/* Format of atascsi parameter is:
 	 *   atascsi=<can_queue>,<cmd_per_lun>,<sg_tablesize>,<hostid>,<use_tags>
 	 * Defaults depend on TT or Falcon, hostid determined at run time.
 	 * Negative values mean don't change.
 	 */
+	int ints[6];
+
+	get_options(str, ARRAY_SIZE(ints), ints);
 
 	if (ints[0] < 1) {
 		printk("atari_scsi_setup: no arguments!\n");
-		return;
+		return 0;
 	}
 
 	if (ints[0] >= 1) {
@@ -777,9 +781,14 @@ void __init atari_scsi_setup(char *str, int *ints)
 			setup_use_tagged_queuing = !!ints[5];
 	}
 #endif
+
+	return 1;
 }
 
-int atari_scsi_bus_reset(Scsi_Cmnd *cmd)
+__setup("atascsi=", atari_scsi_setup);
+#endif /* !MODULE */
+
+static int atari_scsi_bus_reset(Scsi_Cmnd *cmd)
 {
 	int rv;
 	struct NCR5380_hostdata *hostdata =
@@ -852,7 +861,7 @@ static void __init atari_scsi_reset_boot(void)
 #endif
 
 
-const char *atari_scsi_info(struct Scsi_Host *host)
+static const char *atari_scsi_info(struct Scsi_Host *host)
 {
 	/* atari_scsi_detect() is verbose enough... */
 	static const char string[] = "Atari native SCSI";
@@ -862,8 +871,9 @@ const char *atari_scsi_info(struct Scsi_Host *host)
 
 #if defined(REAL_DMA)
 
-unsigned long atari_scsi_dma_setup(struct Scsi_Host *instance, void *data,
-				   unsigned long count, int dir)
+static unsigned long atari_scsi_dma_setup(struct Scsi_Host *instance,
+					  void *data, unsigned long count,
+					  int dir)
 {
 	unsigned long addr = virt_to_phys(data);
 
diff --git a/drivers/scsi/atari_scsi.h b/drivers/scsi/atari_scsi.h
index efadb8d567c..bd52df78b20 100644
--- a/drivers/scsi/atari_scsi.h
+++ b/drivers/scsi/atari_scsi.h
@@ -18,11 +18,6 @@
 /* (I_HAVE_OVERRUNS stuff removed) */
 
 #ifndef ASM
-int atari_scsi_detect (struct scsi_host_template *);
-const char *atari_scsi_info (struct Scsi_Host *);
-int atari_scsi_reset (Scsi_Cmnd *, unsigned int);
-int atari_scsi_release (struct Scsi_Host *);
-
 /* The values for CMD_PER_LUN and CAN_QUEUE are somehow arbitrary. Higher
  * values should work, too; try it! (but cmd_per_lun costs memory!) */
 
diff --git a/drivers/scsi/be2iscsi/be.h b/drivers/scsi/be2iscsi/be.h
index 1d7b976c850..a50b6a9030e 100644
--- a/drivers/scsi/be2iscsi/be.h
+++ b/drivers/scsi/be2iscsi/be.h
@@ -132,10 +132,6 @@ struct be_ctrl_info {
 		((u32)((((size_t)(_address) & (PAGE_SIZE_4K - 1)) +	\
 			(size) + (PAGE_SIZE_4K - 1)) >> PAGE_SHIFT_4K))
 
-/* Byte offset into the page corresponding to given address */
-#define OFFSET_IN_PAGE(addr)						\
-		((size_t)(addr) & (PAGE_SIZE_4K-1))
-
 /* Returns bit offset within a DWORD of a bitfield */
 #define AMAP_BIT_OFFSET(_struct, field)					\
 		(((size_t)&(((_struct *)0)->field))%32)
diff --git a/drivers/scsi/be2iscsi/be_cmds.c b/drivers/scsi/be2iscsi/be_cmds.c
index cdb15364bc6..d2e9e933f7a 100644
--- a/drivers/scsi/be2iscsi/be_cmds.c
+++ b/drivers/scsi/be2iscsi/be_cmds.c
@@ -15,6 +15,8 @@
  * Costa Mesa, CA 92626
  */
 
+#include <scsi/iscsi_proto.h>
+
 #include "be.h"
 #include "be_mgmt.h"
 #include "be_main.h"
diff --git a/drivers/scsi/be2iscsi/be_cmds.h b/drivers/scsi/be2iscsi/be_cmds.h
index 8b40a5b4366..b0b36c6a145 100644
--- a/drivers/scsi/be2iscsi/be_cmds.h
+++ b/drivers/scsi/be2iscsi/be_cmds.h
@@ -23,7 +23,7 @@
  * firmware in the BE. These requests are communicated to the processor
  * using Work Request Blocks (WRBs) submitted to the MCC-WRB ring or via one
  * WRB inside a MAILBOX.
- * The commands are serviced by the ARM processor in the BladeEngine's MPU.
+ * The commands are serviced by the ARM processor in the OneConnect's MPU.
  */
 struct be_sge {
 	u32 pa_lo;
@@ -163,7 +163,8 @@ struct be_mcc_mailbox {
 #define OPCODE_COMMON_ISCSI_CFG_REMOVE_SGL_PAGES        3
 #define OPCODE_COMMON_ISCSI_NTWK_GET_NIC_CONFIG		7
 #define OPCODE_COMMON_ISCSI_NTWK_SET_VLAN		14
-#define OPCODE_COMMON_ISCSI_NTWK_CONFIGURE_STATELESS_IP_ADDR	17
+#define OPCODE_COMMON_ISCSI_NTWK_CONFIG_STATELESS_IP_ADDR	17
+#define OPCODE_COMMON_ISCSI_NTWK_REL_STATELESS_IP_ADDR	18
 #define OPCODE_COMMON_ISCSI_NTWK_MODIFY_IP_ADDR		21
 #define OPCODE_COMMON_ISCSI_NTWK_GET_DEFAULT_GATEWAY	22
 #define OPCODE_COMMON_ISCSI_NTWK_MODIFY_DEFAULT_GATEWAY 23
@@ -274,15 +275,15 @@ struct mgmt_conn_login_options {
 	struct	mgmt_auth_method_format auth_data;
 } __packed;
 
-struct ip_address_format {
+struct ip_addr_format {
 	u16 size_of_structure;
 	u8 reserved;
 	u8 ip_type;
-	u8 ip_address[16];
+	u8 addr[16];
 	u32 rsvd0;
 } __packed;
 
-struct	mgmt_conn_info {
+struct mgmt_conn_info {
 	u32	connection_handle;
 	u32	connection_status;
 	u16	src_port;
@@ -290,9 +291,9 @@ struct	mgmt_conn_info {
 	u16	dest_port_redirected;
 	u16	cid;
 	u32	estimated_throughput;
-	struct	ip_address_format	src_ipaddr;
-	struct	ip_address_format	dest_ipaddr;
-	struct	ip_address_format	dest_ipaddr_redirected;
+	struct	ip_addr_format	src_ipaddr;
+	struct	ip_addr_format	dest_ipaddr;
+	struct	ip_addr_format	dest_ipaddr_redirected;
 	struct	mgmt_conn_login_options	negotiated_login_options;
 } __packed;
 
@@ -322,43 +323,115 @@ struct mgmt_session_info {
 	struct	mgmt_conn_info	conn_list[1];
 } __packed;
 
-struct  be_cmd_req_get_session {
+struct be_cmd_get_session_req {
 	struct be_cmd_req_hdr hdr;
 	u32 session_handle;
 } __packed;
 
-struct  be_cmd_resp_get_session {
+struct be_cmd_get_session_resp {
 	struct be_cmd_resp_hdr hdr;
 	struct mgmt_session_info session_info;
 } __packed;
 
 struct mac_addr {
-	u16 size_of_struct;
+	u16 size_of_structure;
 	u8 addr[ETH_ALEN];
 } __packed;
 
-struct be_cmd_req_get_boot_target {
+struct be_cmd_get_boot_target_req {
 	struct be_cmd_req_hdr hdr;
 } __packed;
 
-struct be_cmd_resp_get_boot_target {
+struct be_cmd_get_boot_target_resp {
 	struct be_cmd_resp_hdr hdr;
 	u32  boot_session_count;
 	int  boot_session_handle;
 };
 
-struct be_cmd_req_mac_query {
+struct be_cmd_mac_query_req {
 	struct be_cmd_req_hdr hdr;
 	u8 type;
 	u8 permanent;
 	u16 if_id;
 } __packed;
 
-struct be_cmd_resp_mac_query {
+struct be_cmd_get_mac_resp {
 	struct be_cmd_resp_hdr hdr;
 	struct mac_addr mac;
 };
 
+struct be_ip_addr_subnet_format {
+	u16 size_of_structure;
+	u8 ip_type;
+	u8 ipv6_prefix_length;
+	u8 addr[16];
+	u8 subnet_mask[16];
+	u32 rsvd0;
+} __packed;
+
+struct be_cmd_get_if_info_req {
+	struct be_cmd_req_hdr hdr;
+	u32 interface_hndl;
+	u32 ip_type;
+} __packed;
+
+struct be_cmd_get_if_info_resp {
+	struct be_cmd_req_hdr hdr;
+	u32 interface_hndl;
+	u32 vlan_priority;
+	u32 ip_addr_count;
+	u32 dhcp_state;
+	struct be_ip_addr_subnet_format ip_addr;
+} __packed;
+
+struct be_ip_addr_record {
+	u32 action;
+	u32 interface_hndl;
+	struct be_ip_addr_subnet_format ip_addr;
+	u32 status;
+} __packed;
+
+struct be_ip_addr_record_params {
+	u32 record_entry_count;
+	struct be_ip_addr_record ip_record;
+} __packed;
+
+struct be_cmd_set_ip_addr_req {
+	struct be_cmd_req_hdr hdr;
+	struct be_ip_addr_record_params ip_params;
+} __packed;
+
+
+struct be_cmd_set_dhcp_req {
+	struct be_cmd_req_hdr hdr;
+	u32 interface_hndl;
+	u32 ip_type;
+	u32 flags;
+	u32 retry_count;
+} __packed;
+
+struct be_cmd_rel_dhcp_req {
+	struct be_cmd_req_hdr hdr;
+	u32 interface_hndl;
+	u32 ip_type;
+} __packed;
+
+struct be_cmd_set_def_gateway_req {
+	struct be_cmd_req_hdr hdr;
+	u32 action;
+	struct ip_addr_format ip_addr;
+} __packed;
+
+struct be_cmd_get_def_gateway_req {
+	struct be_cmd_req_hdr hdr;
+	u32 ip_type;
+} __packed;
+
+struct be_cmd_get_def_gateway_resp {
+	struct be_cmd_req_hdr hdr;
+	struct ip_addr_format ip_addr;
+} __packed;
+
 /******************** Create CQ ***************************/
 /**
  * Pseudo amap definition in which each bit of the actual structure is defined
@@ -489,7 +562,7 @@ struct be_cmd_req_modify_eq_delay {
 
 #define ETH_ALEN	6
 
-struct be_cmd_req_get_mac_addr {
+struct be_cmd_get_nic_conf_req {
 	struct be_cmd_req_hdr hdr;
 	u32 nic_port_count;
 	u32 speed;
@@ -501,7 +574,7 @@ struct be_cmd_req_get_mac_addr {
 	u32 rsvd[23];
 };
 
-struct be_cmd_resp_get_mac_addr {
+struct be_cmd_get_nic_conf_resp {
 	struct be_cmd_resp_hdr hdr;
 	u32 nic_port_count;
 	u32 speed;
@@ -513,6 +586,39 @@ struct be_cmd_resp_get_mac_addr {
 	u32 rsvd[23];
 };
 
+#define BEISCSI_ALIAS_LEN 32
+
+struct be_cmd_hba_name {
+	struct be_cmd_req_hdr hdr;
+	u16 flags;
+	u16 rsvd0;
+	u8 initiator_name[ISCSI_NAME_LEN];
+	u8 initiator_alias[BEISCSI_ALIAS_LEN];
+} __packed;
+
+struct be_cmd_ntwk_link_status_req {
+	struct be_cmd_req_hdr hdr;
+	u32 rsvd0;
+} __packed;
+
+/*** Port Speed Values ***/
+#define BE2ISCSI_LINK_SPEED_ZERO	0x00
+#define BE2ISCSI_LINK_SPEED_10MBPS	0x01
+#define BE2ISCSI_LINK_SPEED_100MBPS	0x02
+#define BE2ISCSI_LINK_SPEED_1GBPS	0x03
+#define BE2ISCSI_LINK_SPEED_10GBPS	0x04
+struct be_cmd_ntwk_link_status_resp {
+	struct be_cmd_resp_hdr hdr;
+	u8 phys_port;
+	u8 mac_duplex;
+	u8 mac_speed;
+	u8 mac_fault;
+	u8 mgmt_mac_duplex;
+	u8 mgmt_mac_speed;
+	u16 qos_link_speed;
+	u32 logical_link_speed;
+} __packed;
+
 int beiscsi_cmd_eq_create(struct be_ctrl_info *ctrl,
 			  struct be_queue_info *eq, int eq_delay);
 
@@ -530,11 +636,8 @@ int beiscsi_cmd_mccq_create(struct beiscsi_hba *phba,
 int be_poll_mcc(struct be_ctrl_info *ctrl);
 int mgmt_check_supported_fw(struct be_ctrl_info *ctrl,
 				      struct beiscsi_hba *phba);
-unsigned int be_cmd_get_mac_addr(struct beiscsi_hba *phba);
-unsigned int beiscsi_get_boot_target(struct beiscsi_hba *phba);
-unsigned int beiscsi_get_session_info(struct beiscsi_hba *phba,
-				  u32 boot_session_handle,
-				  struct be_dma_mem *nonemb_cmd);
+unsigned int be_cmd_get_initname(struct beiscsi_hba *phba);
+unsigned int be_cmd_get_port_speed(struct beiscsi_hba *phba);
 
 void free_mcc_tag(struct be_ctrl_info *ctrl, unsigned int tag);
 /*ISCSI Functuions */
@@ -715,7 +818,7 @@ struct be_eq_delay_params_in {
 
 struct tcp_connect_and_offload_in {
 	struct be_cmd_req_hdr hdr;
-	struct ip_address_format ip_address;
+	struct ip_addr_format ip_address;
 	u16 tcp_port;
 	u16 cid;
 	u16 cq_id;
@@ -792,13 +895,14 @@ struct be_fw_cfg {
 	u32 function_caps;
 } __packed;
 
-struct be_all_if_id {
+struct be_cmd_get_all_if_id_req {
 	struct be_cmd_req_hdr hdr;
 	u32 if_count;
 	u32 if_hndl_list[1];
 } __packed;
 
 #define ISCSI_OPCODE_SCSI_DATA_OUT		5
+#define OPCODE_COMMON_NTWK_LINK_STATUS_QUERY 5
 #define OPCODE_COMMON_MODIFY_EQ_DELAY		41
 #define OPCODE_COMMON_ISCSI_CLEANUP		59
 #define	OPCODE_COMMON_TCP_UPLOAD		56
@@ -810,6 +914,8 @@ struct be_all_if_id {
 #define OPCODE_ISCSI_INI_DRIVER_OFFLOAD_SESSION 41
 #define OPCODE_ISCSI_INI_DRIVER_INVALIDATE_CONNECTION 42
 #define OPCODE_ISCSI_INI_BOOT_GET_BOOT_TARGET	52
+#define OPCODE_COMMON_WRITE_FLASH		96
+#define OPCODE_COMMON_READ_FLASH		97
 
 /* --- CMD_ISCSI_INVALIDATE_CONNECTION_TYPE --- */
 #define CMD_ISCSI_COMMAND_INVALIDATE		1
diff --git a/drivers/scsi/be2iscsi/be_iscsi.c b/drivers/scsi/be2iscsi/be_iscsi.c
index 33c8f09c7ac..43f35034585 100644
--- a/drivers/scsi/be2iscsi/be_iscsi.c
+++ b/drivers/scsi/be2iscsi/be_iscsi.c
@@ -23,6 +23,8 @@
 #include <scsi/scsi_cmnd.h>
 #include <scsi/scsi_device.h>
 #include <scsi/scsi_host.h>
+#include <scsi/scsi_netlink.h>
+#include <net/netlink.h>
 #include <scsi/scsi.h>
 
 #include "be_iscsi.h"
@@ -207,6 +209,301 @@ int beiscsi_conn_bind(struct iscsi_cls_session *cls_session,
 	return beiscsi_bindconn_cid(phba, beiscsi_conn, beiscsi_ep->ep_cid);
 }
 
+static int beiscsi_create_ipv4_iface(struct beiscsi_hba *phba)
+{
+	if (phba->ipv4_iface)
+		return 0;
+
+	phba->ipv4_iface = iscsi_create_iface(phba->shost,
+					      &beiscsi_iscsi_transport,
+					      ISCSI_IFACE_TYPE_IPV4,
+					      0, 0);
+	if (!phba->ipv4_iface) {
+		shost_printk(KERN_ERR, phba->shost, "Could not "
+			     "create default IPv4 address.\n");
+		return -ENODEV;
+	}
+
+	return 0;
+}
+
+static int beiscsi_create_ipv6_iface(struct beiscsi_hba *phba)
+{
+	if (phba->ipv6_iface)
+		return 0;
+
+	phba->ipv6_iface = iscsi_create_iface(phba->shost,
+					      &beiscsi_iscsi_transport,
+					      ISCSI_IFACE_TYPE_IPV6,
+					      0, 0);
+	if (!phba->ipv6_iface) {
+		shost_printk(KERN_ERR, phba->shost, "Could not "
+			     "create default IPv6 address.\n");
+		return -ENODEV;
+	}
+
+	return 0;
+}
+
+void beiscsi_create_def_ifaces(struct beiscsi_hba *phba)
+{
+	struct be_cmd_get_if_info_resp if_info;
+
+	if (!mgmt_get_if_info(phba, BE2_IPV4, &if_info))
+		beiscsi_create_ipv4_iface(phba);
+
+	if (!mgmt_get_if_info(phba, BE2_IPV6, &if_info))
+		beiscsi_create_ipv6_iface(phba);
+}
+
+void beiscsi_destroy_def_ifaces(struct beiscsi_hba *phba)
+{
+	if (phba->ipv6_iface)
+		iscsi_destroy_iface(phba->ipv6_iface);
+	if (phba->ipv4_iface)
+		iscsi_destroy_iface(phba->ipv4_iface);
+}
+
+static int
+beiscsi_set_static_ip(struct Scsi_Host *shost,
+		struct iscsi_iface_param_info *iface_param,
+		void *data, uint32_t dt_len)
+{
+	struct beiscsi_hba *phba = iscsi_host_priv(shost);
+	struct iscsi_iface_param_info *iface_ip = NULL;
+	struct iscsi_iface_param_info *iface_subnet = NULL;
+	struct nlattr *nla;
+	int ret;
+
+
+	switch (iface_param->param) {
+	case ISCSI_NET_PARAM_IPV4_BOOTPROTO:
+		nla = nla_find(data, dt_len, ISCSI_NET_PARAM_IPV4_ADDR);
+		if (nla)
+			iface_ip = nla_data(nla);
+
+		nla = nla_find(data, dt_len, ISCSI_NET_PARAM_IPV4_SUBNET);
+		if (nla)
+			iface_subnet = nla_data(nla);
+		break;
+	case ISCSI_NET_PARAM_IPV4_ADDR:
+		iface_ip = iface_param;
+		nla = nla_find(data, dt_len, ISCSI_NET_PARAM_IPV4_SUBNET);
+		if (nla)
+			iface_subnet = nla_data(nla);
+		break;
+	case ISCSI_NET_PARAM_IPV4_SUBNET:
+		iface_subnet = iface_param;
+		nla = nla_find(data, dt_len, ISCSI_NET_PARAM_IPV4_ADDR);
+		if (nla)
+			iface_ip = nla_data(nla);
+		break;
+	default:
+		shost_printk(KERN_ERR, shost, "Unsupported param %d\n",
+			     iface_param->param);
+	}
+
+	if (!iface_ip || !iface_subnet) {
+		shost_printk(KERN_ERR, shost, "IP and Subnet Mask required\n");
+		return -EINVAL;
+	}
+
+	ret = mgmt_set_ip(phba, iface_ip, iface_subnet,
+			ISCSI_BOOTPROTO_STATIC);
+
+	return ret;
+}
+
+static int
+beiscsi_set_ipv4(struct Scsi_Host *shost,
+		struct iscsi_iface_param_info *iface_param,
+		void *data, uint32_t dt_len)
+{
+	struct beiscsi_hba *phba = iscsi_host_priv(shost);
+	int ret = 0;
+
+	/* Check the param */
+	switch (iface_param->param) {
+	case ISCSI_NET_PARAM_IPV4_GW:
+		ret = mgmt_set_gateway(phba, iface_param);
+		break;
+	case ISCSI_NET_PARAM_IPV4_BOOTPROTO:
+		if (iface_param->value[0] == ISCSI_BOOTPROTO_DHCP)
+			ret = mgmt_set_ip(phba, iface_param,
+					NULL, ISCSI_BOOTPROTO_DHCP);
+		else if (iface_param->value[0] == ISCSI_BOOTPROTO_STATIC)
+			ret = beiscsi_set_static_ip(shost, iface_param,
+						    data, dt_len);
+		else
+			shost_printk(KERN_ERR, shost, "Invalid BOOTPROTO: %d\n",
+					iface_param->value[0]);
+		break;
+	case ISCSI_NET_PARAM_IFACE_ENABLE:
+		if (iface_param->value[0] == ISCSI_IFACE_ENABLE)
+			ret = beiscsi_create_ipv4_iface(phba);
+		else
+			iscsi_destroy_iface(phba->ipv4_iface);
+		break;
+	case ISCSI_NET_PARAM_IPV4_SUBNET:
+	case ISCSI_NET_PARAM_IPV4_ADDR:
+		ret = beiscsi_set_static_ip(shost, iface_param,
+					    data, dt_len);
+		break;
+	default:
+		shost_printk(KERN_ERR, shost, "Param %d not supported\n",
+			     iface_param->param);
+	}
+
+	return ret;
+}
+
+static int
+beiscsi_set_ipv6(struct Scsi_Host *shost,
+		struct iscsi_iface_param_info *iface_param,
+		void *data, uint32_t dt_len)
+{
+	struct beiscsi_hba *phba = iscsi_host_priv(shost);
+	int ret = 0;
+
+	switch (iface_param->param) {
+	case ISCSI_NET_PARAM_IFACE_ENABLE:
+		if (iface_param->value[0] == ISCSI_IFACE_ENABLE)
+			ret = beiscsi_create_ipv6_iface(phba);
+		else {
+			iscsi_destroy_iface(phba->ipv6_iface);
+			ret = 0;
+		}
+		break;
+	case ISCSI_NET_PARAM_IPV6_ADDR:
+		ret = mgmt_set_ip(phba, iface_param, NULL,
+				  ISCSI_BOOTPROTO_STATIC);
+		break;
+	default:
+		shost_printk(KERN_ERR, shost, "Param %d not supported\n",
+			     iface_param->param);
+	}
+
+	return ret;
+}
+
+int be2iscsi_iface_set_param(struct Scsi_Host *shost,
+		void *data, uint32_t dt_len)
+{
+	struct iscsi_iface_param_info *iface_param = NULL;
+	struct nlattr *attrib;
+	uint32_t rm_len = dt_len;
+	int ret = 0 ;
+
+	nla_for_each_attr(attrib, data, dt_len, rm_len) {
+		iface_param = nla_data(attrib);
+
+		if (iface_param->param_type != ISCSI_NET_PARAM)
+			continue;
+
+		/*
+		 * BE2ISCSI only supports 1 interface
+		 */
+		if (iface_param->iface_num) {
+			shost_printk(KERN_ERR, shost, "Invalid iface_num %d."
+				     "Only iface_num 0 is supported.\n",
+				     iface_param->iface_num);
+			return -EINVAL;
+		}
+
+		switch (iface_param->iface_type) {
+		case ISCSI_IFACE_TYPE_IPV4:
+			ret = beiscsi_set_ipv4(shost, iface_param,
+					       data, dt_len);
+			break;
+		case ISCSI_IFACE_TYPE_IPV6:
+			ret = beiscsi_set_ipv6(shost, iface_param,
+					       data, dt_len);
+			break;
+		default:
+			shost_printk(KERN_ERR, shost,
+				     "Invalid iface type :%d passed\n",
+				     iface_param->iface_type);
+			break;
+		}
+
+		if (ret)
+			return ret;
+	}
+
+	return ret;
+}
+
+static int be2iscsi_get_if_param(struct beiscsi_hba *phba,
+		struct iscsi_iface *iface, int param,
+		char *buf)
+{
+	struct be_cmd_get_if_info_resp if_info;
+	int len, ip_type = BE2_IPV4;
+
+	memset(&if_info, 0, sizeof(if_info));
+
+	if (iface->iface_type == ISCSI_IFACE_TYPE_IPV6)
+		ip_type = BE2_IPV6;
+
+	len = mgmt_get_if_info(phba, ip_type, &if_info);
+	if (len)
+		return len;
+
+	switch (param) {
+	case ISCSI_NET_PARAM_IPV4_ADDR:
+		len = sprintf(buf, "%pI4\n", &if_info.ip_addr.addr);
+		break;
+	case ISCSI_NET_PARAM_IPV6_ADDR:
+		len = sprintf(buf, "%pI6\n", &if_info.ip_addr.addr);
+		break;
+	case ISCSI_NET_PARAM_IPV4_BOOTPROTO:
+		if (!if_info.dhcp_state)
+			len = sprintf(buf, "static");
+		else
+			len = sprintf(buf, "dhcp");
+		break;
+	case ISCSI_NET_PARAM_IPV4_SUBNET:
+		len = sprintf(buf, "%pI4\n", &if_info.ip_addr.subnet_mask);
+		break;
+	default:
+		WARN_ON(1);
+	}
+
+	return len;
+}
+
+int be2iscsi_iface_get_param(struct iscsi_iface *iface,
+		enum iscsi_param_type param_type,
+		int param, char *buf)
+{
+	struct Scsi_Host *shost = iscsi_iface_to_shost(iface);
+	struct beiscsi_hba *phba = iscsi_host_priv(shost);
+	struct be_cmd_get_def_gateway_resp gateway;
+	int len = -ENOSYS;
+
+	switch (param) {
+	case ISCSI_NET_PARAM_IPV4_ADDR:
+	case ISCSI_NET_PARAM_IPV4_SUBNET:
+	case ISCSI_NET_PARAM_IPV4_BOOTPROTO:
+	case ISCSI_NET_PARAM_IPV6_ADDR:
+		len = be2iscsi_get_if_param(phba, iface, param, buf);
+		break;
+	case ISCSI_NET_PARAM_IFACE_ENABLE:
+		len = sprintf(buf, "enabled");
+		break;
+	case ISCSI_NET_PARAM_IPV4_GW:
+		memset(&gateway, 0, sizeof(gateway));
+		len = mgmt_get_gateway(phba, BE2_IPV4, &gateway);
+		if (!len)
+			len = sprintf(buf, "%pI4\n", &gateway.ip_addr.addr);
+		break;
+	default:
+		len = -ENOSYS;
+	}
+
+	return len;
+}
+
 /**
  * beiscsi_ep_get_param - get the iscsi parameter
  * @ep: pointer to iscsi ep
@@ -221,7 +518,7 @@ int beiscsi_ep_get_param(struct iscsi_endpoint *ep,
 	struct beiscsi_endpoint *beiscsi_ep = ep->dd_data;
 	int len = 0;
 
-	SE_DEBUG(DBG_LVL_8, "In beiscsi_conn_get_param, param= %d\n", param);
+	SE_DEBUG(DBG_LVL_8, "In beiscsi_ep_get_param, param= %d\n", param);
 
 	switch (param) {
 	case ISCSI_PARAM_CONN_PORT:
@@ -279,6 +576,121 @@ int beiscsi_set_param(struct iscsi_cls_conn *cls_conn,
 }
 
 /**
+ * beiscsi_get_initname - Read Initiator Name from flash
+ * @buf: buffer bointer
+ * @phba: The device priv structure instance
+ *
+ * returns number of bytes
+ */
+static int beiscsi_get_initname(char *buf, struct beiscsi_hba *phba)
+{
+	int rc;
+	unsigned int tag, wrb_num;
+	unsigned short status, extd_status;
+	struct be_mcc_wrb *wrb;
+	struct be_cmd_hba_name *resp;
+	struct be_queue_info *mccq = &phba->ctrl.mcc_obj.q;
+
+	tag = be_cmd_get_initname(phba);
+	if (!tag) {
+		SE_DEBUG(DBG_LVL_1, "Getting Initiator Name Failed\n");
+		return -EBUSY;
+	} else
+		wait_event_interruptible(phba->ctrl.mcc_wait[tag],
+				phba->ctrl.mcc_numtag[tag]);
+
+	wrb_num = (phba->ctrl.mcc_numtag[tag] & 0x00FF0000) >> 16;
+	extd_status = (phba->ctrl.mcc_numtag[tag] & 0x0000FF00) >> 8;
+	status = phba->ctrl.mcc_numtag[tag] & 0x000000FF;
+
+	if (status || extd_status) {
+		SE_DEBUG(DBG_LVL_1, "MailBox Command Failed with "
+				"status = %d extd_status = %d\n",
+				status, extd_status);
+		free_mcc_tag(&phba->ctrl, tag);
+		return -EAGAIN;
+	}
+	wrb = queue_get_wrb(mccq, wrb_num);
+	free_mcc_tag(&phba->ctrl, tag);
+	resp = embedded_payload(wrb);
+	rc = sprintf(buf, "%s\n", resp->initiator_name);
+	return rc;
+}
+
+/**
+ * beiscsi_get_port_state - Get the Port State
+ * @shost : pointer to scsi_host structure
+ *
+ * returns number of bytes
+ */
+static void beiscsi_get_port_state(struct Scsi_Host *shost)
+{
+	struct beiscsi_hba *phba = iscsi_host_priv(shost);
+	struct iscsi_cls_host *ihost = shost->shost_data;
+
+	ihost->port_state = (phba->state == BE_ADAPTER_UP) ?
+		ISCSI_PORT_STATE_UP : ISCSI_PORT_STATE_DOWN;
+}
+
+/**
+ * beiscsi_get_port_speed  - Get the Port Speed from Adapter
+ * @shost : pointer to scsi_host structure
+ *
+ * returns Success/Failure
+ */
+static int beiscsi_get_port_speed(struct Scsi_Host *shost)
+{
+	unsigned int tag, wrb_num;
+	unsigned short status, extd_status;
+	struct be_mcc_wrb *wrb;
+	struct be_cmd_ntwk_link_status_resp *resp;
+	struct beiscsi_hba *phba = iscsi_host_priv(shost);
+	struct iscsi_cls_host *ihost = shost->shost_data;
+	struct be_queue_info *mccq = &phba->ctrl.mcc_obj.q;
+
+	tag = be_cmd_get_port_speed(phba);
+	if (!tag) {
+		SE_DEBUG(DBG_LVL_1, "Getting Port Speed Failed\n");
+		 return -EBUSY;
+	 } else
+		wait_event_interruptible(phba->ctrl.mcc_wait[tag],
+				phba->ctrl.mcc_numtag[tag]);
+
+	wrb_num = (phba->ctrl.mcc_numtag[tag] & 0x00FF0000) >> 16;
+	extd_status = (phba->ctrl.mcc_numtag[tag] & 0x0000FF00) >> 8;
+	status = phba->ctrl.mcc_numtag[tag] & 0x000000FF;
+
+	if (status || extd_status) {
+		SE_DEBUG(DBG_LVL_1, "MailBox Command Failed with "
+				"status = %d extd_status = %d\n",
+				status, extd_status);
+		free_mcc_tag(&phba->ctrl, tag);
+		return -EAGAIN;
+	}
+	wrb = queue_get_wrb(mccq, wrb_num);
+	free_mcc_tag(&phba->ctrl, tag);
+	resp = embedded_payload(wrb);
+
+	switch (resp->mac_speed) {
+	case BE2ISCSI_LINK_SPEED_10MBPS:
+		ihost->port_speed = ISCSI_PORT_SPEED_10MBPS;
+		break;
+	case BE2ISCSI_LINK_SPEED_100MBPS:
+		ihost->port_speed = BE2ISCSI_LINK_SPEED_100MBPS;
+		break;
+	case BE2ISCSI_LINK_SPEED_1GBPS:
+		ihost->port_speed = ISCSI_PORT_SPEED_1GBPS;
+		break;
+	case BE2ISCSI_LINK_SPEED_10GBPS:
+		ihost->port_speed = ISCSI_PORT_SPEED_10GBPS;
+		break;
+	default:
+		ihost->port_speed = ISCSI_PORT_SPEED_UNKNOWN;
+	}
+	return 0;
+}
+
+/**
  * beiscsi_get_host_param - get the iscsi parameter
  * @shost: pointer to scsi_host structure
  * @param: parameter type identifier
@@ -301,6 +713,27 @@ int beiscsi_get_host_param(struct Scsi_Host *shost,
 			return status;
 		}
 		break;
+	case ISCSI_HOST_PARAM_INITIATOR_NAME:
+		status = beiscsi_get_initname(buf, phba);
+		if (status < 0) {
+			SE_DEBUG(DBG_LVL_1,
+					"Retreiving Initiator Name Failed\n");
+			return status;
+		}
+		break;
+	case ISCSI_HOST_PARAM_PORT_STATE:
+		beiscsi_get_port_state(shost);
+		status = sprintf(buf, "%s\n", iscsi_get_port_state_name(shost));
+		break;
+	case ISCSI_HOST_PARAM_PORT_SPEED:
+		status = beiscsi_get_port_speed(shost);
+		if (status) {
+			SE_DEBUG(DBG_LVL_1,
+					"Retreiving Port Speed Failed\n");
+			return status;
+		}
+		status = sprintf(buf, "%s\n", iscsi_get_port_speed_name(shost));
+		break;
 	default:
 		return iscsi_host_get_param(shost, param, buf);
 	}
@@ -309,46 +742,21 @@ int beiscsi_get_host_param(struct Scsi_Host *shost,
 
 int beiscsi_get_macaddr(char *buf, struct beiscsi_hba *phba)
 {
-	struct be_cmd_resp_get_mac_addr *resp;
-	struct be_mcc_wrb *wrb;
-	unsigned int tag, wrb_num;
-	unsigned short status, extd_status;
-	struct be_queue_info *mccq = &phba->ctrl.mcc_obj.q;
+	struct be_cmd_get_nic_conf_resp resp;
 	int rc;
 
-	if (phba->read_mac_address)
-		return sysfs_format_mac(buf, phba->mac_address,
-					ETH_ALEN);
+	if (strlen(phba->mac_address))
+		return strlcpy(buf, phba->mac_address, PAGE_SIZE);
 
-	tag = be_cmd_get_mac_addr(phba);
-	if (!tag) {
-		SE_DEBUG(DBG_LVL_1, "be_cmd_get_mac_addr Failed\n");
-		return -EBUSY;
-	} else
-		wait_event_interruptible(phba->ctrl.mcc_wait[tag],
-					 phba->ctrl.mcc_numtag[tag]);
+	memset(&resp, 0, sizeof(resp));
+	rc = mgmt_get_nic_conf(phba, &resp);
+	if (rc)
+		return rc;
 
-	wrb_num = (phba->ctrl.mcc_numtag[tag] & 0x00FF0000) >> 16;
-	extd_status = (phba->ctrl.mcc_numtag[tag] & 0x0000FF00) >> 8;
-	status = phba->ctrl.mcc_numtag[tag] & 0x000000FF;
-	if (status || extd_status) {
-		SE_DEBUG(DBG_LVL_1, "Failed to get be_cmd_get_mac_addr"
-				    " status = %d extd_status = %d\n",
-				    status, extd_status);
-		free_mcc_tag(&phba->ctrl, tag);
-		return -EAGAIN;
-	}
-	wrb = queue_get_wrb(mccq, wrb_num);
-	free_mcc_tag(&phba->ctrl, tag);
-	resp = embedded_payload(wrb);
-	memcpy(phba->mac_address, resp->mac_address, ETH_ALEN);
-	rc = sysfs_format_mac(buf, phba->mac_address,
-			       ETH_ALEN);
-	phba->read_mac_address = 1;
-	return rc;
+	memcpy(phba->mac_address, resp.mac_address, ETH_ALEN);
+	return sysfs_format_mac(buf, phba->mac_address, ETH_ALEN);
 }
 
-
 /**
  * beiscsi_conn_get_stats - get the iscsi stats
  * @cls_conn: pointer to iscsi cls conn
@@ -736,11 +1144,24 @@ void beiscsi_ep_disconnect(struct iscsi_endpoint *ep)
 umode_t be2iscsi_attr_is_visible(int param_type, int param)
 {
 	switch (param_type) {
+	case ISCSI_NET_PARAM:
+		switch (param) {
+		case ISCSI_NET_PARAM_IFACE_ENABLE:
+		case ISCSI_NET_PARAM_IPV4_ADDR:
+		case ISCSI_NET_PARAM_IPV4_SUBNET:
+		case ISCSI_NET_PARAM_IPV4_BOOTPROTO:
+		case ISCSI_NET_PARAM_IPV4_GW:
+		case ISCSI_NET_PARAM_IPV6_ADDR:
+			return S_IRUGO;
+		default:
+			return 0;
+		}
 	case ISCSI_HOST_PARAM:
 		switch (param) {
 		case ISCSI_HOST_PARAM_HWADDRESS:
-		case ISCSI_HOST_PARAM_IPADDRESS:
 		case ISCSI_HOST_PARAM_INITIATOR_NAME:
+		case ISCSI_HOST_PARAM_PORT_STATE:
+		case ISCSI_HOST_PARAM_PORT_SPEED:
 			return S_IRUGO;
 		default:
 			return 0;
diff --git a/drivers/scsi/be2iscsi/be_iscsi.h b/drivers/scsi/be2iscsi/be_iscsi.h
index 5c45be13450..8b826fc06bc 100644
--- a/drivers/scsi/be2iscsi/be_iscsi.h
+++ b/drivers/scsi/be2iscsi/be_iscsi.h
@@ -25,6 +25,21 @@
 
 #define BE2_IPV4  0x1
 #define BE2_IPV6  0x10
+#define BE2_DHCP_V4 0x05
+
+#define NON_BLOCKING 0x0
+#define BLOCKING 0x1
+
+void beiscsi_create_def_ifaces(struct beiscsi_hba *phba);
+
+void beiscsi_destroy_def_ifaces(struct beiscsi_hba *phba);
+
+int be2iscsi_iface_get_param(struct iscsi_iface *iface,
+			     enum iscsi_param_type param_type,
+			     int param, char *buf);
+
+int be2iscsi_iface_set_param(struct Scsi_Host *shost,
+			     void *data, uint32_t count);
 
 umode_t be2iscsi_attr_is_visible(int param_type, int param);
 
diff --git a/drivers/scsi/be2iscsi/be_main.c b/drivers/scsi/be2iscsi/be_main.c
index 375756fa95c..0b1d99c99fd 100644
--- a/drivers/scsi/be2iscsi/be_main.c
+++ b/drivers/scsi/be2iscsi/be_main.c
@@ -28,8 +28,11 @@
 #include <linux/semaphore.h>
 #include <linux/iscsi_boot_sysfs.h>
 #include <linux/module.h>
+#include <linux/bsg-lib.h>
 
 #include <scsi/libiscsi.h>
+#include <scsi/scsi_bsg_iscsi.h>
+#include <scsi/scsi_netlink.h>
 #include <scsi/scsi_transport_iscsi.h>
 #include <scsi/scsi_transport.h>
 #include <scsi/scsi_cmnd.h>
@@ -48,7 +51,8 @@ static unsigned int num_hba = 0;
 
 MODULE_DEVICE_TABLE(pci, beiscsi_pci_id_table);
 MODULE_DESCRIPTION(DRV_DESC " " BUILD_STR);
-MODULE_AUTHOR("ServerEngines Corporation");
+MODULE_VERSION(BUILD_STR);
+MODULE_AUTHOR("Emulex Corporation");
 MODULE_LICENSE("GPL");
 module_param(be_iopoll_budget, int, 0);
 module_param(enable_msix, int, 0);
@@ -147,15 +151,15 @@ static int beiscsi_eh_device_reset(struct scsi_cmnd *sc)
 	struct invalidate_command_table *inv_tbl;
 	struct be_dma_mem nonemb_cmd;
 	unsigned int cid, tag, i, num_invalidate;
-	int rc = FAILED;
 
 	/* invalidate iocbs */
 	cls_session = starget_to_session(scsi_target(sc->device));
 	session = cls_session->dd_data;
 	spin_lock_bh(&session->lock);
-	if (!session->leadconn || session->state != ISCSI_STATE_LOGGED_IN)
-		goto unlock;
-
+	if (!session->leadconn || session->state != ISCSI_STATE_LOGGED_IN) {
+		spin_unlock_bh(&session->lock);
+		return FAILED;
+	}
 	conn = session->leadconn;
 	beiscsi_conn = conn->dd_data;
 	phba = beiscsi_conn->phba;
@@ -208,9 +212,6 @@ static int beiscsi_eh_device_reset(struct scsi_cmnd *sc)
 	pci_free_consistent(phba->ctrl.pdev, nonemb_cmd.size,
 			    nonemb_cmd.va, nonemb_cmd.dma);
 	return iscsi_eh_device_reset(sc);
-unlock:
-	spin_unlock_bh(&session->lock);
-	return rc;
 }
 
 static ssize_t beiscsi_show_boot_tgt_info(void *data, int type, char *buf)
@@ -230,10 +231,10 @@ static ssize_t beiscsi_show_boot_tgt_info(void *data, int type, char *buf)
 	case ISCSI_BOOT_TGT_IP_ADDR:
 		if (boot_conn->dest_ipaddr.ip_type == 0x1)
 			rc = sprintf(buf, "%pI4\n",
-				(char *)&boot_conn->dest_ipaddr.ip_address);
+				(char *)&boot_conn->dest_ipaddr.addr);
 		else
 			rc = sprintf(str, "%pI6\n",
-				(char *)&boot_conn->dest_ipaddr.ip_address);
+				(char *)&boot_conn->dest_ipaddr.addr);
 		break;
 	case ISCSI_BOOT_TGT_PORT:
 		rc = sprintf(str, "%d\n", boot_conn->dest_port);
@@ -311,12 +312,8 @@ static ssize_t beiscsi_show_boot_eth_info(void *data, int type, char *buf)
 		rc = sprintf(str, "0\n");
 		break;
 	case ISCSI_BOOT_ETH_MAC:
-		rc  = beiscsi_get_macaddr(buf, phba);
-		if (rc < 0) {
-			SE_DEBUG(DBG_LVL_1, "beiscsi_get_macaddr Failed\n");
-			return rc;
-		}
-	break;
+		rc  = beiscsi_get_macaddr(str, phba);
+		break;
 	default:
 		rc = -ENOSYS;
 		break;
@@ -394,7 +391,7 @@ MODULE_DEVICE_TABLE(pci, beiscsi_pci_id_table);
 
 static struct scsi_host_template beiscsi_sht = {
 	.module = THIS_MODULE,
-	.name = "ServerEngines 10Gbe open-iscsi Initiator Driver",
+	.name = "Emulex 10Gbe open-iscsi Initiator Driver",
 	.proc_name = DRV_NAME,
 	.queuecommand = iscsi_queuecommand,
 	.change_queue_depth = iscsi_change_queue_depth,
@@ -409,6 +406,8 @@ static struct scsi_host_template beiscsi_sht = {
 	.max_sectors = BEISCSI_MAX_SECTORS,
 	.cmd_per_lun = BEISCSI_CMD_PER_LUN,
 	.use_clustering = ENABLE_CLUSTERING,
+	.vendor_id = SCSI_NL_VID_TYPE_PCI | BE_VENDOR_ID,
+
 };
 
 static struct scsi_transport_template *beiscsi_scsi_transport;
@@ -435,6 +434,7 @@ static struct beiscsi_hba *beiscsi_hba_alloc(struct pci_dev *pcidev)
 	phba->shost = shost;
 	phba->pcidev = pci_dev_get(pcidev);
 	pci_set_drvdata(pcidev, phba);
+	phba->interface_handle = 0xFFFFFFFF;
 
 	if (iscsi_host_add(shost, &phba->pcidev->dev))
 		goto free_devices;
@@ -544,8 +544,7 @@ static int be_ctrl_init(struct beiscsi_hba *phba, struct pci_dev *pdev)
 						  &mbox_mem_alloc->dma);
 	if (!mbox_mem_alloc->va) {
 		beiscsi_unmap_pci_function(phba);
-		status = -ENOMEM;
-		return status;
+		return -ENOMEM;
 	}
 
 	mbox_mem_align->size = sizeof(struct be_mcc_mailbox);
@@ -1252,9 +1251,9 @@ hwi_complete_drvr_msgs(struct beiscsi_conn *beiscsi_conn,
 	task = pwrb_handle->pio_handle;
 
 	io_task = task->dd_data;
-	spin_lock(&phba->mgmt_sgl_lock);
+	spin_lock_bh(&phba->mgmt_sgl_lock);
 	free_mgmt_sgl_handle(phba, io_task->psgl_handle);
-	spin_unlock(&phba->mgmt_sgl_lock);
+	spin_unlock_bh(&phba->mgmt_sgl_lock);
 	spin_lock_bh(&session->lock);
 	free_wrb_handle(phba, pwrb_context, pwrb_handle);
 	spin_unlock_bh(&session->lock);
@@ -1370,8 +1369,6 @@ hwi_get_async_handle(struct beiscsi_hba *phba,
 	struct be_bus_address phys_addr;
 	struct list_head *pbusy_list;
 	struct async_pdu_handle *pasync_handle = NULL;
-	int buffer_len = 0;
-	unsigned char buffer_index = -1;
 	unsigned char is_header = 0;
 
 	phys_addr.u.a32.address_lo =
@@ -1392,22 +1389,11 @@ hwi_get_async_handle(struct beiscsi_hba *phba,
 		pbusy_list = hwi_get_async_busy_list(pasync_ctx, 1,
 			(pdpdu_cqe->dw[offsetof(struct amap_i_t_dpdu_cqe,
 			index) / 32] & PDUCQE_INDEX_MASK));
-
-		buffer_len = (unsigned int)(phys_addr.u.a64.address -
-				pasync_ctx->async_header.pa_base.u.a64.address);
-
-		buffer_index = buffer_len /
-				pasync_ctx->async_header.buffer_size;
-
 		break;
 	case UNSOL_DATA_NOTIFY:
 		pbusy_list = hwi_get_async_busy_list(pasync_ctx, 0, (pdpdu_cqe->
 					dw[offsetof(struct amap_i_t_dpdu_cqe,
 					index) / 32] & PDUCQE_INDEX_MASK));
-		buffer_len = (unsigned long)(phys_addr.u.a64.address -
-					pasync_ctx->async_data.pa_base.u.
-					a64.address);
-		buffer_index = buffer_len / pasync_ctx->async_data.buffer_size;
 		break;
 	default:
 		pbusy_list = NULL;
@@ -1418,11 +1404,9 @@ hwi_get_async_handle(struct beiscsi_hba *phba,
 		return NULL;
 	}
 
-	WARN_ON(!(buffer_index <= pasync_ctx->async_data.num_entries));
 	WARN_ON(list_empty(pbusy_list));
 	list_for_each_entry(pasync_handle, pbusy_list, link) {
-		WARN_ON(pasync_handle->consumed);
-		if (pasync_handle->index == buffer_index)
+		if (pasync_handle->pa.u.a64.address == phys_addr.u.a64.address)
 			break;
 	}
 
@@ -1449,15 +1433,13 @@ hwi_update_async_writables(struct hwi_async_pdu_context *pasync_ctx,
 	unsigned int num_entries, writables = 0;
 	unsigned int *pep_read_ptr, *pwritables;
 
-
+	num_entries = pasync_ctx->num_entries;
 	if (is_header) {
 		pep_read_ptr = &pasync_ctx->async_header.ep_read_ptr;
 		pwritables = &pasync_ctx->async_header.writables;
-		num_entries = pasync_ctx->async_header.num_entries;
 	} else {
 		pep_read_ptr = &pasync_ctx->async_data.ep_read_ptr;
 		pwritables = &pasync_ctx->async_data.writables;
-		num_entries = pasync_ctx->async_data.num_entries;
 	}
 
 	while ((*pep_read_ptr) != cq_index) {
@@ -1491,14 +1473,13 @@ hwi_update_async_writables(struct hwi_async_pdu_context *pasync_ctx,
 	return 0;
 }
 
-static unsigned int hwi_free_async_msg(struct beiscsi_hba *phba,
+static void hwi_free_async_msg(struct beiscsi_hba *phba,
 				       unsigned int cri)
 {
 	struct hwi_controller *phwi_ctrlr;
 	struct hwi_async_pdu_context *pasync_ctx;
 	struct async_pdu_handle *pasync_handle, *tmp_handle;
 	struct list_head *plist;
-	unsigned int i = 0;
 
 	phwi_ctrlr = phba->phwi_ctrlr;
 	pasync_ctx = HWI_GET_ASYNC_PDU_CTX(phwi_ctrlr);
@@ -1508,23 +1489,20 @@ static unsigned int hwi_free_async_msg(struct beiscsi_hba *phba,
 	list_for_each_entry_safe(pasync_handle, tmp_handle, plist, link) {
 		list_del(&pasync_handle->link);
 
-		if (i == 0) {
+		if (pasync_handle->is_header) {
 			list_add_tail(&pasync_handle->link,
 				      &pasync_ctx->async_header.free_list);
 			pasync_ctx->async_header.free_entries++;
-			i++;
 		} else {
 			list_add_tail(&pasync_handle->link,
 				      &pasync_ctx->async_data.free_list);
 			pasync_ctx->async_data.free_entries++;
-			i++;
 		}
 	}
 
 	INIT_LIST_HEAD(&pasync_ctx->async_entry[cri].wait_queue.list);
 	pasync_ctx->async_entry[cri].wait_queue.hdr_received = 0;
 	pasync_ctx->async_entry[cri].wait_queue.bytes_received = 0;
-	return 0;
 }
 
 static struct phys_addr *
@@ -1557,16 +1535,15 @@ static void hwi_post_async_buffers(struct beiscsi_hba *phba,
 
 	phwi_ctrlr = phba->phwi_ctrlr;
 	pasync_ctx = HWI_GET_ASYNC_PDU_CTX(phwi_ctrlr);
+	num_entries = pasync_ctx->num_entries;
 
 	if (is_header) {
-		num_entries = pasync_ctx->async_header.num_entries;
 		writables = min(pasync_ctx->async_header.writables,
 				pasync_ctx->async_header.free_entries);
 		pfree_link = pasync_ctx->async_header.free_list.next;
 		host_write_num = pasync_ctx->async_header.host_write_ptr;
 		ring_id = phwi_ctrlr->default_pdu_hdr.id;
 	} else {
-		num_entries = pasync_ctx->async_data.num_entries;
 		writables = min(pasync_ctx->async_data.writables,
 				pasync_ctx->async_data.free_entries);
 		pfree_link = pasync_ctx->async_data.free_list.next;
@@ -1673,7 +1650,7 @@ hwi_fwd_async_msg(struct beiscsi_conn *beiscsi_conn,
 			}
 			memcpy(pfirst_buffer + offset,
 			       pasync_handle->pbuffer, buf_len);
-			offset = buf_len;
+			offset += buf_len;
 		}
 		index++;
 	}
@@ -1682,10 +1659,9 @@ hwi_fwd_async_msg(struct beiscsi_conn *beiscsi_conn,
 					   (beiscsi_conn->beiscsi_conn_cid -
 					    phba->fw_config.iscsi_cid_start),
 					    phdr, hdr_len, pfirst_buffer,
-					    buf_len);
+					    offset);
 
-	if (status == 0)
-		hwi_free_async_msg(phba, cri);
+	hwi_free_async_msg(phba, cri);
 	return 0;
 }
 
@@ -2229,7 +2205,7 @@ static int beiscsi_alloc_mem(struct beiscsi_hba *phba)
 	struct mem_array *mem_arr, *mem_arr_orig;
 	unsigned int i, j, alloc_size, curr_alloc_size;
 
-	phba->phwi_ctrlr = kmalloc(phba->params.hwi_ws_sz, GFP_KERNEL);
+	phba->phwi_ctrlr = kzalloc(phba->params.hwi_ws_sz, GFP_KERNEL);
 	if (!phba->phwi_ctrlr)
 		return -ENOMEM;
 
@@ -2349,27 +2325,21 @@ static void iscsi_init_global_templates(struct beiscsi_hba *phba)
 	AMAP_SET_BITS(struct amap_pdu_nop_out, i_bit, pnop_out, 0);
 }
 
-static void beiscsi_init_wrb_handle(struct beiscsi_hba *phba)
+static int beiscsi_init_wrb_handle(struct beiscsi_hba *phba)
 {
 	struct be_mem_descriptor *mem_descr_wrbh, *mem_descr_wrb;
-	struct wrb_handle *pwrb_handle;
+	struct wrb_handle *pwrb_handle = NULL;
 	struct hwi_controller *phwi_ctrlr;
 	struct hwi_wrb_context *pwrb_context;
-	struct iscsi_wrb *pwrb;
-	unsigned int num_cxn_wrbh;
-	unsigned int num_cxn_wrb, j, idx, index;
+	struct iscsi_wrb *pwrb = NULL;
+	unsigned int num_cxn_wrbh = 0;
+	unsigned int num_cxn_wrb = 0, j, idx = 0, index;
 
 	mem_descr_wrbh = phba->init_mem;
 	mem_descr_wrbh += HWI_MEM_WRBH;
 
 	mem_descr_wrb = phba->init_mem;
 	mem_descr_wrb += HWI_MEM_WRB;
-
-	idx = 0;
-	pwrb_handle = mem_descr_wrbh->mem_array[idx].virtual_address;
-	num_cxn_wrbh = ((mem_descr_wrbh->mem_array[idx].size) /
-			((sizeof(struct wrb_handle)) *
-			 phba->params.wrbs_per_cxn));
 	phwi_ctrlr = phba->phwi_ctrlr;
 
 	for (index = 0; index < phba->params.cxns_per_ctrl * 2; index += 2) {
@@ -2377,12 +2347,32 @@ static void beiscsi_init_wrb_handle(struct beiscsi_hba *phba)
 		pwrb_context->pwrb_handle_base =
 				kzalloc(sizeof(struct wrb_handle *) *
 					phba->params.wrbs_per_cxn, GFP_KERNEL);
+		if (!pwrb_context->pwrb_handle_base) {
+			shost_printk(KERN_ERR, phba->shost,
+					"Mem Alloc Failed. Failing to load\n");
+			goto init_wrb_hndl_failed;
+		}
 		pwrb_context->pwrb_handle_basestd =
 				kzalloc(sizeof(struct wrb_handle *) *
 					phba->params.wrbs_per_cxn, GFP_KERNEL);
+		if (!pwrb_context->pwrb_handle_basestd) {
+			shost_printk(KERN_ERR, phba->shost,
+					"Mem Alloc Failed. Failing to load\n");
+			goto init_wrb_hndl_failed;
+		}
+		if (!num_cxn_wrbh) {
+			pwrb_handle =
+				mem_descr_wrbh->mem_array[idx].virtual_address;
+			num_cxn_wrbh = ((mem_descr_wrbh->mem_array[idx].size) /
+					((sizeof(struct wrb_handle)) *
+					 phba->params.wrbs_per_cxn));
+			idx++;
+		}
+		pwrb_context->alloc_index = 0;
+		pwrb_context->wrb_handles_available = 0;
+		pwrb_context->free_index = 0;
+
 		if (num_cxn_wrbh) {
-			pwrb_context->alloc_index = 0;
-			pwrb_context->wrb_handles_available = 0;
 			for (j = 0; j < phba->params.wrbs_per_cxn; j++) {
 				pwrb_context->pwrb_handle_base[j] = pwrb_handle;
 				pwrb_context->pwrb_handle_basestd[j] =
@@ -2391,49 +2381,21 @@ static void beiscsi_init_wrb_handle(struct beiscsi_hba *phba)
 				pwrb_handle->wrb_index = j;
 				pwrb_handle++;
 			}
-			pwrb_context->free_index = 0;
-			num_cxn_wrbh--;
-		} else {
-			idx++;
-			pwrb_handle =
-			    mem_descr_wrbh->mem_array[idx].virtual_address;
-			num_cxn_wrbh =
-			    ((mem_descr_wrbh->mem_array[idx].size) /
-			     ((sizeof(struct wrb_handle)) *
-			      phba->params.wrbs_per_cxn));
-			pwrb_context->alloc_index = 0;
-			for (j = 0; j < phba->params.wrbs_per_cxn; j++) {
-				pwrb_context->pwrb_handle_base[j] = pwrb_handle;
-				pwrb_context->pwrb_handle_basestd[j] =
-				    pwrb_handle;
-				pwrb_context->wrb_handles_available++;
-				pwrb_handle->wrb_index = j;
-				pwrb_handle++;
-			}
-			pwrb_context->free_index = 0;
 			num_cxn_wrbh--;
 		}
 	}
 	idx = 0;
-	pwrb = mem_descr_wrb->mem_array[idx].virtual_address;
-	num_cxn_wrb = (mem_descr_wrb->mem_array[idx].size) /
-		      ((sizeof(struct iscsi_wrb) *
-			phba->params.wrbs_per_cxn));
 	for (index = 0; index < phba->params.cxns_per_ctrl * 2; index += 2) {
 		pwrb_context = &phwi_ctrlr->wrb_context[index];
-		if (num_cxn_wrb) {
-			for (j = 0; j < phba->params.wrbs_per_cxn; j++) {
-				pwrb_handle = pwrb_context->pwrb_handle_base[j];
-				pwrb_handle->pwrb = pwrb;
-				pwrb++;
-			}
-			num_cxn_wrb--;
-		} else {
-			idx++;
+		if (!num_cxn_wrb) {
 			pwrb = mem_descr_wrb->mem_array[idx].virtual_address;
 			num_cxn_wrb = (mem_descr_wrb->mem_array[idx].size) /
-				      ((sizeof(struct iscsi_wrb) *
-					phba->params.wrbs_per_cxn));
+				((sizeof(struct iscsi_wrb) *
+				  phba->params.wrbs_per_cxn));
+			idx++;
+		}
+
+		if (num_cxn_wrb) {
 			for (j = 0; j < phba->params.wrbs_per_cxn; j++) {
 				pwrb_handle = pwrb_context->pwrb_handle_base[j];
 				pwrb_handle->pwrb = pwrb;
@@ -2442,6 +2404,14 @@ static void beiscsi_init_wrb_handle(struct beiscsi_hba *phba)
 			num_cxn_wrb--;
 		}
 	}
+	return 0;
+init_wrb_hndl_failed:
+	for (j = index; j > 0; j--) {
+		pwrb_context = &phwi_ctrlr->wrb_context[j];
+		kfree(pwrb_context->pwrb_handle_base);
+		kfree(pwrb_context->pwrb_handle_basestd);
+	}
+	return -ENOMEM;
 }
 
 static void hwi_init_async_pdu_ctx(struct beiscsi_hba *phba)
@@ -2450,7 +2420,7 @@ static void hwi_init_async_pdu_ctx(struct beiscsi_hba *phba)
 	struct hba_parameters *p = &phba->params;
 	struct hwi_async_pdu_context *pasync_ctx;
 	struct async_pdu_handle *pasync_header_h, *pasync_data_h;
-	unsigned int index;
+	unsigned int index, idx, num_per_mem, num_async_data;
 	struct be_mem_descriptor *mem_descr;
 
 	mem_descr = (struct be_mem_descriptor *)phba->init_mem;
@@ -2462,10 +2432,8 @@ static void hwi_init_async_pdu_ctx(struct beiscsi_hba *phba)
 	pasync_ctx = phwi_ctrlr->phwi_ctxt->pasync_ctx;
 	memset(pasync_ctx, 0, sizeof(*pasync_ctx));
 
-	pasync_ctx->async_header.num_entries = p->asyncpdus_per_ctrl;
-	pasync_ctx->async_header.buffer_size = p->defpdu_hdr_sz;
-	pasync_ctx->async_data.buffer_size = p->defpdu_data_sz;
-	pasync_ctx->async_data.num_entries = p->asyncpdus_per_ctrl;
+	pasync_ctx->num_entries = p->asyncpdus_per_ctrl;
+	pasync_ctx->buffer_size = p->defpdu_hdr_sz;
 
 	mem_descr = (struct be_mem_descriptor *)phba->init_mem;
 	mem_descr += HWI_MEM_ASYNC_HEADER_BUF;
@@ -2510,19 +2478,6 @@ static void hwi_init_async_pdu_ctx(struct beiscsi_hba *phba)
 	pasync_ctx->async_header.writables = 0;
 	INIT_LIST_HEAD(&pasync_ctx->async_header.free_list);
 
-	mem_descr = (struct be_mem_descriptor *)phba->init_mem;
-	mem_descr += HWI_MEM_ASYNC_DATA_BUF;
-	if (mem_descr->mem_array[0].virtual_address) {
-		SE_DEBUG(DBG_LVL_8,
-			 "hwi_init_async_pdu_ctx HWI_MEM_ASYNC_DATA_BUF"
-			 "va=%p\n", mem_descr->mem_array[0].virtual_address);
-	} else
-		shost_printk(KERN_WARNING, phba->shost,
-			    "No Virtual address\n");
-	pasync_ctx->async_data.va_base =
-			mem_descr->mem_array[0].virtual_address;
-	pasync_ctx->async_data.pa_base.u.a64.address =
-			mem_descr->mem_array[0].bus_address.u.a64.address;
 
 	mem_descr = (struct be_mem_descriptor *)phba->init_mem;
 	mem_descr += HWI_MEM_ASYNC_DATA_RING;
@@ -2553,6 +2508,25 @@ static void hwi_init_async_pdu_ctx(struct beiscsi_hba *phba)
 	pasync_data_h =
 		(struct async_pdu_handle *)pasync_ctx->async_data.handle_base;
 
+	mem_descr = (struct be_mem_descriptor *)phba->init_mem;
+	mem_descr += HWI_MEM_ASYNC_DATA_BUF;
+	if (mem_descr->mem_array[0].virtual_address) {
+		SE_DEBUG(DBG_LVL_8,
+			 "hwi_init_async_pdu_ctx HWI_MEM_ASYNC_DATA_BUF"
+			 "va=%p\n", mem_descr->mem_array[0].virtual_address);
+	} else
+		shost_printk(KERN_WARNING, phba->shost,
+			    "No Virtual address\n");
+	idx = 0;
+	pasync_ctx->async_data.va_base =
+			mem_descr->mem_array[idx].virtual_address;
+	pasync_ctx->async_data.pa_base.u.a64.address =
+			mem_descr->mem_array[idx].bus_address.u.a64.address;
+
+	num_async_data = ((mem_descr->mem_array[idx].size) /
+				phba->params.defpdu_data_sz);
+	num_per_mem = 0;
+
 	for (index = 0; index < p->asyncpdus_per_ctrl; index++) {
 		pasync_header_h->cri = -1;
 		pasync_header_h->index = (char)index;
@@ -2578,14 +2552,29 @@ static void hwi_init_async_pdu_ctx(struct beiscsi_hba *phba)
 		pasync_data_h->cri = -1;
 		pasync_data_h->index = (char)index;
 		INIT_LIST_HEAD(&pasync_data_h->link);
+
+		if (!num_async_data) {
+			num_per_mem = 0;
+			idx++;
+			pasync_ctx->async_data.va_base =
+				mem_descr->mem_array[idx].virtual_address;
+			pasync_ctx->async_data.pa_base.u.a64.address =
+				mem_descr->mem_array[idx].
+				bus_address.u.a64.address;
+
+			num_async_data = ((mem_descr->mem_array[idx].size) /
+					phba->params.defpdu_data_sz);
+		}
 		pasync_data_h->pbuffer =
 			(void *)((unsigned long)
 			(pasync_ctx->async_data.va_base) +
-			(p->defpdu_data_sz * index));
+			(p->defpdu_data_sz * num_per_mem));
 
 		pasync_data_h->pa.u.a64.address =
 		    pasync_ctx->async_data.pa_base.u.a64.address +
-		    (p->defpdu_data_sz * index);
+		    (p->defpdu_data_sz * num_per_mem);
+		num_per_mem++;
+		num_async_data--;
 
 		list_add_tail(&pasync_data_h->link,
 			      &pasync_ctx->async_data.free_list);
@@ -2913,9 +2902,11 @@ beiscsi_post_pages(struct beiscsi_hba *phba)
 static void be_queue_free(struct beiscsi_hba *phba, struct be_queue_info *q)
 {
 	struct be_dma_mem *mem = &q->dma_mem;
-	if (mem->va)
+	if (mem->va) {
 		pci_free_consistent(phba->pcidev, mem->size,
 			mem->va, mem->dma);
+		mem->va = NULL;
+	}
 }
 
 static int be_queue_alloc(struct beiscsi_hba *phba, struct be_queue_info *q,
@@ -3215,7 +3206,7 @@ static int hwi_init_port(struct beiscsi_hba *phba)
 error:
 	shost_printk(KERN_ERR, phba->shost, "hwi_init_port failed");
 	hwi_cleanup(phba);
-	return -ENOMEM;
+	return status;
 }
 
 static int hwi_init_controller(struct beiscsi_hba *phba)
@@ -3236,7 +3227,9 @@ static int hwi_init_controller(struct beiscsi_hba *phba)
 	}
 
 	iscsi_init_global_templates(phba);
-	beiscsi_init_wrb_handle(phba);
+	if (beiscsi_init_wrb_handle(phba))
+		return -ENOMEM;
+
 	hwi_init_async_pdu_ctx(phba);
 	if (hwi_init_port(phba) != 0) {
 		shost_printk(KERN_ERR, phba->shost,
@@ -3288,7 +3281,7 @@ static int beiscsi_init_controller(struct beiscsi_hba *phba)
 
 free_init:
 	beiscsi_free_mem(phba);
-	return -ENOMEM;
+	return ret;
 }
 
 static int beiscsi_init_sgl_handle(struct beiscsi_hba *phba)
@@ -3475,8 +3468,8 @@ static void hwi_disable_intr(struct beiscsi_hba *phba)
 
 static int beiscsi_get_boot_info(struct beiscsi_hba *phba)
 {
-	struct be_cmd_resp_get_boot_target *boot_resp;
-	struct be_cmd_resp_get_session *session_resp;
+	struct be_cmd_get_boot_target_resp *boot_resp;
+	struct be_cmd_get_session_resp *session_resp;
 	struct be_mcc_wrb *wrb;
 	struct be_dma_mem nonemb_cmd;
 	unsigned int tag, wrb_num;
@@ -3484,9 +3477,9 @@ static int beiscsi_get_boot_info(struct beiscsi_hba *phba)
 	struct be_queue_info *mccq = &phba->ctrl.mcc_obj.q;
 	int ret = -ENOMEM;
 
-	tag = beiscsi_get_boot_target(phba);
+	tag = mgmt_get_boot_target(phba);
 	if (!tag) {
-		SE_DEBUG(DBG_LVL_1, "be_cmd_get_mac_addr Failed\n");
+		SE_DEBUG(DBG_LVL_1, "beiscsi_get_boot_info Failed\n");
 		return -EAGAIN;
 	} else
 		wait_event_interruptible(phba->ctrl.mcc_wait[tag],
@@ -3496,7 +3489,7 @@ static int beiscsi_get_boot_info(struct beiscsi_hba *phba)
 	extd_status = (phba->ctrl.mcc_numtag[tag] & 0x0000FF00) >> 8;
 	status = phba->ctrl.mcc_numtag[tag] & 0x000000FF;
 	if (status || extd_status) {
-		SE_DEBUG(DBG_LVL_1, "be_cmd_get_mac_addr Failed"
+		SE_DEBUG(DBG_LVL_1, "beiscsi_get_boot_info Failed"
 				    " status = %d extd_status = %d\n",
 				    status, extd_status);
 		free_mcc_tag(&phba->ctrl, tag);
@@ -3522,8 +3515,8 @@ static int beiscsi_get_boot_info(struct beiscsi_hba *phba)
 	}
 
 	memset(nonemb_cmd.va, 0, sizeof(*session_resp));
-	tag = beiscsi_get_session_info(phba,
-		boot_resp->boot_session_handle, &nonemb_cmd);
+	tag = mgmt_get_session_info(phba, boot_resp->boot_session_handle,
+				    &nonemb_cmd);
 	if (!tag) {
 		SE_DEBUG(DBG_LVL_1, "beiscsi_get_session_info"
 			" Failed\n");
@@ -3696,6 +3689,57 @@ static void beiscsi_clean_port(struct beiscsi_hba *phba)
 	kfree(phba->ep_array);
 }
 
+static void beiscsi_cleanup_task(struct iscsi_task *task)
+{
+	struct beiscsi_io_task *io_task = task->dd_data;
+	struct iscsi_conn *conn = task->conn;
+	struct beiscsi_conn *beiscsi_conn = conn->dd_data;
+	struct beiscsi_hba *phba = beiscsi_conn->phba;
+	struct beiscsi_session *beiscsi_sess = beiscsi_conn->beiscsi_sess;
+	struct hwi_wrb_context *pwrb_context;
+	struct hwi_controller *phwi_ctrlr;
+
+	phwi_ctrlr = phba->phwi_ctrlr;
+	pwrb_context = &phwi_ctrlr->wrb_context[beiscsi_conn->beiscsi_conn_cid
+			- phba->fw_config.iscsi_cid_start];
+
+	if (io_task->cmd_bhs) {
+		pci_pool_free(beiscsi_sess->bhs_pool, io_task->cmd_bhs,
+			      io_task->bhs_pa.u.a64.address);
+		io_task->cmd_bhs = NULL;
+	}
+
+	if (task->sc) {
+		if (io_task->pwrb_handle) {
+			free_wrb_handle(phba, pwrb_context,
+					io_task->pwrb_handle);
+			io_task->pwrb_handle = NULL;
+		}
+
+		if (io_task->psgl_handle) {
+			spin_lock(&phba->io_sgl_lock);
+			free_io_sgl_handle(phba, io_task->psgl_handle);
+			spin_unlock(&phba->io_sgl_lock);
+			io_task->psgl_handle = NULL;
+		}
+	} else {
+		if (!beiscsi_conn->login_in_progress) {
+			if (io_task->pwrb_handle) {
+				free_wrb_handle(phba, pwrb_context,
+						io_task->pwrb_handle);
+				io_task->pwrb_handle = NULL;
+			}
+			if (io_task->psgl_handle) {
+				spin_lock(&phba->mgmt_sgl_lock);
+				free_mgmt_sgl_handle(phba,
+						     io_task->psgl_handle);
+				spin_unlock(&phba->mgmt_sgl_lock);
+				io_task->psgl_handle = NULL;
+			}
+		}
+	}
+}
+
 void
 beiscsi_offload_connection(struct beiscsi_conn *beiscsi_conn,
 			   struct beiscsi_offload_params *params)
@@ -3704,12 +3748,19 @@ beiscsi_offload_connection(struct beiscsi_conn *beiscsi_conn,
 	struct iscsi_target_context_update_wrb *pwrb = NULL;
 	struct be_mem_descriptor *mem_descr;
 	struct beiscsi_hba *phba = beiscsi_conn->phba;
+	struct iscsi_task *task = beiscsi_conn->task;
+	struct iscsi_session *session = task->conn->session;
 	u32 doorbell = 0;
 
 	/*
 	 * We can always use 0 here because it is reserved by libiscsi for
 	 * login/startup related tasks.
 	 */
+	beiscsi_conn->login_in_progress = 0;
+	spin_lock_bh(&session->lock);
+	beiscsi_cleanup_task(task);
+	spin_unlock_bh(&session->lock);
+
 	pwrb_handle = alloc_wrb_handle(phba, (beiscsi_conn->beiscsi_conn_cid -
 				       phba->fw_config.iscsi_cid_start));
 	pwrb = (struct iscsi_target_context_update_wrb *)pwrb_handle->pwrb;
@@ -3823,7 +3874,7 @@ static int beiscsi_alloc_pdu(struct iscsi_task *task, uint8_t opcode)
 	task->hdr = (struct iscsi_hdr *)&io_task->cmd_bhs->iscsi_hdr;
 	task->hdr_max = sizeof(struct be_cmd_bhs);
 	io_task->psgl_handle = NULL;
-	io_task->psgl_handle = NULL;
+	io_task->pwrb_handle = NULL;
 
 	if (task->sc) {
 		spin_lock(&phba->io_sgl_lock);
@@ -3865,6 +3916,7 @@ static int beiscsi_alloc_pdu(struct iscsi_task *task, uint8_t opcode)
 				io_task->pwrb_handle =
 						beiscsi_conn->plogin_wrb_handle;
 			}
+			beiscsi_conn->task = task;
 		} else {
 			spin_lock(&phba->mgmt_sgl_lock);
 			io_task->psgl_handle = alloc_mgmt_sgl_handle(phba);
@@ -3907,53 +3959,11 @@ free_hndls:
 	io_task->pwrb_handle = NULL;
 	pci_pool_free(beiscsi_sess->bhs_pool, io_task->cmd_bhs,
 		      io_task->bhs_pa.u.a64.address);
+	io_task->cmd_bhs = NULL;
 	SE_DEBUG(DBG_LVL_1, "Alloc of SGL_ICD Failed\n");
 	return -ENOMEM;
 }
 
-static void beiscsi_cleanup_task(struct iscsi_task *task)
-{
-	struct beiscsi_io_task *io_task = task->dd_data;
-	struct iscsi_conn *conn = task->conn;
-	struct beiscsi_conn *beiscsi_conn = conn->dd_data;
-	struct beiscsi_hba *phba = beiscsi_conn->phba;
-	struct beiscsi_session *beiscsi_sess = beiscsi_conn->beiscsi_sess;
-	struct hwi_wrb_context *pwrb_context;
-	struct hwi_controller *phwi_ctrlr;
-
-	phwi_ctrlr = phba->phwi_ctrlr;
-	pwrb_context = &phwi_ctrlr->wrb_context[beiscsi_conn->beiscsi_conn_cid
-			- phba->fw_config.iscsi_cid_start];
-	if (io_task->pwrb_handle) {
-		free_wrb_handle(phba, pwrb_context, io_task->pwrb_handle);
-		io_task->pwrb_handle = NULL;
-	}
-
-	if (io_task->cmd_bhs) {
-		pci_pool_free(beiscsi_sess->bhs_pool, io_task->cmd_bhs,
-			      io_task->bhs_pa.u.a64.address);
-	}
-
-	if (task->sc) {
-		if (io_task->psgl_handle) {
-			spin_lock(&phba->io_sgl_lock);
-			free_io_sgl_handle(phba, io_task->psgl_handle);
-			spin_unlock(&phba->io_sgl_lock);
-			io_task->psgl_handle = NULL;
-		}
-	} else {
-		if (task->hdr &&
-		   ((task->hdr->opcode & ISCSI_OPCODE_MASK) == ISCSI_OP_LOGIN))
-			return;
-		if (io_task->psgl_handle) {
-			spin_lock(&phba->mgmt_sgl_lock);
-			free_mgmt_sgl_handle(phba, io_task->psgl_handle);
-			spin_unlock(&phba->mgmt_sgl_lock);
-			io_task->psgl_handle = NULL;
-		}
-	}
-}
-
 static int beiscsi_iotask(struct iscsi_task *task, struct scatterlist *sg,
 			  unsigned int num_sg, unsigned int xferlen,
 			  unsigned int writedir)
@@ -3993,7 +4003,8 @@ static int beiscsi_iotask(struct iscsi_task *task, struct scatterlist *sg,
 	       &io_task->cmd_bhs->iscsi_hdr.lun, sizeof(struct scsi_lun));
 
 	AMAP_SET_BITS(struct amap_iscsi_wrb, lun, pwrb,
-		      cpu_to_be16(*(unsigned short *)&io_task->cmd_bhs->iscsi_hdr.lun));
+		      cpu_to_be16(*(unsigned short *)
+				  &io_task->cmd_bhs->iscsi_hdr.lun));
 	AMAP_SET_BITS(struct amap_iscsi_wrb, r2t_exp_dtl, pwrb, xferlen);
 	AMAP_SET_BITS(struct amap_iscsi_wrb, wrb_idx, pwrb,
 		      io_task->pwrb_handle->wrb_index);
@@ -4126,6 +4137,76 @@ static int beiscsi_task_xmit(struct iscsi_task *task)
 	return beiscsi_iotask(task, sg, num_sg, xferlen, writedir);
 }
 
+/**
+ * beiscsi_bsg_request - handle bsg request from ISCSI transport
+ * @job: job to handle
+ */
+static int beiscsi_bsg_request(struct bsg_job *job)
+{
+	struct Scsi_Host *shost;
+	struct beiscsi_hba *phba;
+	struct iscsi_bsg_request *bsg_req = job->request;
+	int rc = -EINVAL;
+	unsigned int tag;
+	struct be_dma_mem nonemb_cmd;
+	struct be_cmd_resp_hdr *resp;
+	struct iscsi_bsg_reply *bsg_reply = job->reply;
+	unsigned short status, extd_status;
+
+	shost = iscsi_job_to_shost(job);
+	phba = iscsi_host_priv(shost);
+
+	switch (bsg_req->msgcode) {
+	case ISCSI_BSG_HST_VENDOR:
+		nonemb_cmd.va = pci_alloc_consistent(phba->ctrl.pdev,
+					job->request_payload.payload_len,
+					&nonemb_cmd.dma);
+		if (nonemb_cmd.va == NULL) {
+			SE_DEBUG(DBG_LVL_1, "Failed to allocate memory for "
+				 "beiscsi_bsg_request\n");
+			return -EIO;
+		}
+		tag = mgmt_vendor_specific_fw_cmd(&phba->ctrl, phba, job,
+						  &nonemb_cmd);
+		if (!tag) {
+			SE_DEBUG(DBG_LVL_1, "be_cmd_get_mac_addr Failed\n");
+			pci_free_consistent(phba->ctrl.pdev, nonemb_cmd.size,
+					    nonemb_cmd.va, nonemb_cmd.dma);
+			return -EAGAIN;
+		} else
+			wait_event_interruptible(phba->ctrl.mcc_wait[tag],
+						 phba->ctrl.mcc_numtag[tag]);
+		extd_status = (phba->ctrl.mcc_numtag[tag] & 0x0000FF00) >> 8;
+		status = phba->ctrl.mcc_numtag[tag] & 0x000000FF;
+		free_mcc_tag(&phba->ctrl, tag);
+		resp = (struct be_cmd_resp_hdr *)nonemb_cmd.va;
+		sg_copy_from_buffer(job->reply_payload.sg_list,
+				    job->reply_payload.sg_cnt,
+				    nonemb_cmd.va, (resp->response_length
+				    + sizeof(*resp)));
+		bsg_reply->reply_payload_rcv_len = resp->response_length;
+		bsg_reply->result = status;
+		bsg_job_done(job, bsg_reply->result,
+			     bsg_reply->reply_payload_rcv_len);
+		pci_free_consistent(phba->ctrl.pdev, nonemb_cmd.size,
+				    nonemb_cmd.va, nonemb_cmd.dma);
+		if (status || extd_status) {
+			SE_DEBUG(DBG_LVL_1, "be_cmd_get_mac_addr Failed"
+				 " status = %d extd_status = %d\n",
+				 status, extd_status);
+			return -EIO;
+		}
+		break;
+
+	default:
+		SE_DEBUG(DBG_LVL_1, "Unsupported bsg command: 0x%x\n",
+			 bsg_req->msgcode);
+		break;
+	}
+
+	return rc;
+}
+
 static void beiscsi_quiesce(struct beiscsi_hba *phba)
 {
 	struct hwi_controller *phwi_ctrlr;
@@ -4183,6 +4264,7 @@ static void beiscsi_remove(struct pci_dev *pcidev)
 		return;
 	}
 
+	beiscsi_destroy_def_ifaces(phba);
 	beiscsi_quiesce(phba);
 	iscsi_boot_destroy_kset(phba->boot_kset);
 	iscsi_host_remove(phba->shost);
@@ -4267,8 +4349,11 @@ static int __devinit beiscsi_dev_probe(struct pci_dev *pcidev,
 	phba->num_cpus = num_cpus;
 	SE_DEBUG(DBG_LVL_8, "num_cpus = %d\n", phba->num_cpus);
 
-	if (enable_msix)
+	if (enable_msix) {
 		beiscsi_msix_enable(phba);
+		if (!phba->msix_enabled)
+			phba->num_cpus = 1;
+	}
 	ret = be_ctrl_init(phba, pcidev);
 	if (ret) {
 		shost_printk(KERN_ERR, phba->shost, "beiscsi_dev_probe-"
@@ -4366,8 +4451,9 @@ static int __devinit beiscsi_dev_probe(struct pci_dev *pcidev,
 		 * iscsi boot.
 		 */
 		shost_printk(KERN_ERR, phba->shost, "Could not set up "
-			     "iSCSI boot info.");
+			     "iSCSI boot info.\n");
 
+	beiscsi_create_def_ifaces(phba);
 	SE_DEBUG(DBG_LVL_8, "\n\n\n SUCCESS - DRIVER LOADED\n\n\n");
 	return 0;
 
@@ -4418,6 +4504,8 @@ struct iscsi_transport beiscsi_iscsi_transport = {
 	.bind_conn = beiscsi_conn_bind,
 	.destroy_conn = iscsi_conn_teardown,
 	.attr_is_visible = be2iscsi_attr_is_visible,
+	.set_iface_param = be2iscsi_iface_set_param,
+	.get_iface_param = be2iscsi_iface_get_param,
 	.set_param = beiscsi_set_param,
 	.get_conn_param = iscsi_conn_get_param,
 	.get_session_param = iscsi_session_get_param,
@@ -4435,6 +4523,7 @@ struct iscsi_transport beiscsi_iscsi_transport = {
 	.ep_poll = beiscsi_ep_poll,
 	.ep_disconnect = beiscsi_ep_disconnect,
 	.session_recovery_timedout = iscsi_session_recovery_timedout,
+	.bsg_request = beiscsi_bsg_request,
 };
 
 static struct pci_driver beiscsi_pci_driver = {
diff --git a/drivers/scsi/be2iscsi/be_main.h b/drivers/scsi/be2iscsi/be_main.h
index b4a06d5e5f9..40fea6ec879 100644
--- a/drivers/scsi/be2iscsi/be_main.h
+++ b/drivers/scsi/be2iscsi/be_main.h
@@ -34,9 +34,9 @@
 
 #include "be.h"
 #define DRV_NAME		"be2iscsi"
-#define BUILD_STR		"4.1.239.0"
-#define BE_NAME			"ServerEngines BladeEngine2" \
-				"Linux iSCSI Driver version" BUILD_STR
+#define BUILD_STR		"4.2.162.0"
+#define BE_NAME			"Emulex OneConnect" \
+				"Open-iSCSI Driver version" BUILD_STR
 #define DRV_DESC		BE_NAME " " "Driver"
 
 #define BE_VENDOR_ID		0x19A2
@@ -316,6 +316,8 @@ struct beiscsi_hba {
 	struct iscsi_endpoint **ep_array;
 	struct iscsi_boot_kset *boot_kset;
 	struct Scsi_Host *shost;
+	struct iscsi_iface *ipv4_iface;
+	struct iscsi_iface *ipv6_iface;
 	struct {
 		/**
 		 * group together since they are used most frequently
@@ -345,7 +347,7 @@ struct beiscsi_hba {
 	struct work_struct work_cqs;	/* The work being queued */
 	struct be_ctrl_info ctrl;
 	unsigned int generation;
-	unsigned int read_mac_address;
+	unsigned int interface_handle;
 	struct mgmt_session_info boot_sess;
 	struct invalidate_command_table inv_tbl[128];
 
@@ -525,8 +527,6 @@ struct hwi_async_pdu_context {
 
 		unsigned int free_entries;
 		unsigned int busy_entries;
-		unsigned int buffer_size;
-		unsigned int num_entries;
 
 		struct list_head free_list;
 	} async_header;
@@ -543,11 +543,12 @@ struct hwi_async_pdu_context {
 
 		unsigned int free_entries;
 		unsigned int busy_entries;
-		unsigned int buffer_size;
 		struct list_head free_list;
-		unsigned int num_entries;
 	} async_data;
 
+	unsigned int buffer_size;
+	unsigned int num_entries;
+
 	/**
 	 * This is a varying size list! Do not add anything
 	 * after this entry!!
diff --git a/drivers/scsi/be2iscsi/be_mgmt.c b/drivers/scsi/be2iscsi/be_mgmt.c
index 44762cfa3e1..01bb04cd9e7 100644
--- a/drivers/scsi/be2iscsi/be_mgmt.c
+++ b/drivers/scsi/be2iscsi/be_mgmt.c
@@ -17,15 +17,17 @@
  * Costa Mesa, CA 92626
  */
 
+#include <linux/bsg-lib.h>
+#include <scsi/scsi_transport_iscsi.h>
+#include <scsi/scsi_bsg_iscsi.h>
 #include "be_mgmt.h"
 #include "be_iscsi.h"
-#include <scsi/scsi_transport_iscsi.h>
 
-unsigned int beiscsi_get_boot_target(struct beiscsi_hba *phba)
+unsigned int mgmt_get_boot_target(struct beiscsi_hba *phba)
 {
 	struct be_ctrl_info *ctrl = &phba->ctrl;
 	struct be_mcc_wrb *wrb;
-	struct be_cmd_req_get_mac_addr *req;
+	struct be_cmd_get_boot_target_req *req;
 	unsigned int tag = 0;
 
 	SE_DEBUG(DBG_LVL_8, "In bescsi_get_boot_target\n");
@@ -42,22 +44,22 @@ unsigned int beiscsi_get_boot_target(struct beiscsi_hba *phba)
 	be_wrb_hdr_prepare(wrb, sizeof(*req), true, 0);
 	be_cmd_hdr_prepare(&req->hdr, CMD_SUBSYSTEM_ISCSI_INI,
 			   OPCODE_ISCSI_INI_BOOT_GET_BOOT_TARGET,
-			   sizeof(*req));
+			   sizeof(struct be_cmd_get_boot_target_resp));
 
 	be_mcc_notify(phba);
 	spin_unlock(&ctrl->mbox_lock);
 	return tag;
 }
 
-unsigned int beiscsi_get_session_info(struct beiscsi_hba *phba,
-				  u32 boot_session_handle,
-				  struct be_dma_mem *nonemb_cmd)
+unsigned int mgmt_get_session_info(struct beiscsi_hba *phba,
+				   u32 boot_session_handle,
+				   struct be_dma_mem *nonemb_cmd)
 {
 	struct be_ctrl_info *ctrl = &phba->ctrl;
 	struct be_mcc_wrb *wrb;
 	unsigned int tag = 0;
-	struct  be_cmd_req_get_session *req;
-	struct be_cmd_resp_get_session *resp;
+	struct  be_cmd_get_session_req *req;
+	struct be_cmd_get_session_resp *resp;
 	struct be_sge *sge;
 
 	SE_DEBUG(DBG_LVL_8, "In beiscsi_get_session_info\n");
@@ -187,6 +189,72 @@ int mgmt_check_supported_fw(struct be_ctrl_info *ctrl,
 	return status;
 }
 
+unsigned int mgmt_vendor_specific_fw_cmd(struct be_ctrl_info *ctrl,
+					 struct beiscsi_hba *phba,
+					 struct bsg_job *job,
+					 struct be_dma_mem *nonemb_cmd)
+{
+	struct be_cmd_resp_hdr *resp;
+	struct be_mcc_wrb *wrb = wrb_from_mccq(phba);
+	struct be_sge *mcc_sge = nonembedded_sgl(wrb);
+	unsigned int tag = 0;
+	struct iscsi_bsg_request *bsg_req = job->request;
+	struct be_bsg_vendor_cmd *req = nonemb_cmd->va;
+	unsigned short region, sector_size, sector, offset;
+
+	nonemb_cmd->size = job->request_payload.payload_len;
+	memset(nonemb_cmd->va, 0, nonemb_cmd->size);
+	resp = nonemb_cmd->va;
+	region =  bsg_req->rqst_data.h_vendor.vendor_cmd[1];
+	sector_size =  bsg_req->rqst_data.h_vendor.vendor_cmd[2];
+	sector =  bsg_req->rqst_data.h_vendor.vendor_cmd[3];
+	offset =  bsg_req->rqst_data.h_vendor.vendor_cmd[4];
+	req->region = region;
+	req->sector = sector;
+	req->offset = offset;
+	spin_lock(&ctrl->mbox_lock);
+	memset(wrb, 0, sizeof(*wrb));
+
+	switch (bsg_req->rqst_data.h_vendor.vendor_cmd[0]) {
+	case BEISCSI_WRITE_FLASH:
+		offset = sector * sector_size + offset;
+		be_cmd_hdr_prepare(&req->hdr, CMD_SUBSYSTEM_ISCSI,
+				   OPCODE_COMMON_WRITE_FLASH, sizeof(*req));
+		sg_copy_to_buffer(job->request_payload.sg_list,
+				  job->request_payload.sg_cnt,
+				  nonemb_cmd->va + offset, job->request_len);
+		break;
+	case BEISCSI_READ_FLASH:
+		be_cmd_hdr_prepare(&req->hdr, CMD_SUBSYSTEM_ISCSI,
+			   OPCODE_COMMON_READ_FLASH, sizeof(*req));
+		break;
+	default:
+		shost_printk(KERN_WARNING, phba->shost,
+			     "Unsupported cmd = 0x%x\n\n", bsg_req->rqst_data.
+			     h_vendor.vendor_cmd[0]);
+		spin_unlock(&ctrl->mbox_lock);
+		return -ENOSYS;
+	}
+
+	tag = alloc_mcc_tag(phba);
+	if (!tag) {
+		spin_unlock(&ctrl->mbox_lock);
+		return tag;
+	}
+
+	be_wrb_hdr_prepare(wrb, nonemb_cmd->size, false,
+			   job->request_payload.sg_cnt);
+	mcc_sge->pa_hi = cpu_to_le32(upper_32_bits(nonemb_cmd->dma));
+	mcc_sge->pa_lo = cpu_to_le32(nonemb_cmd->dma & 0xFFFFFFFF);
+	mcc_sge->len = cpu_to_le32(nonemb_cmd->size);
+	wrb->tag0 |= tag;
+
+	be_mcc_notify(phba);
+
+	spin_unlock(&ctrl->mbox_lock);
+	return tag;
+}
+
 int mgmt_epfw_cleanup(struct beiscsi_hba *phba, unsigned short chute)
 {
 	struct be_ctrl_info *ctrl = &phba->ctrl;
@@ -328,7 +396,6 @@ int mgmt_open_connection(struct beiscsi_hba *phba,
 			 struct sockaddr *dst_addr,
 			 struct beiscsi_endpoint *beiscsi_ep,
 			 struct be_dma_mem *nonemb_cmd)
-
 {
 	struct hwi_controller *phwi_ctrlr;
 	struct hwi_context_memory *phwi_context;
@@ -374,17 +441,17 @@ int mgmt_open_connection(struct beiscsi_hba *phba,
 	if (dst_addr->sa_family == PF_INET) {
 		__be32 s_addr = daddr_in->sin_addr.s_addr;
 		req->ip_address.ip_type = BE2_IPV4;
-		req->ip_address.ip_address[0] = s_addr & 0x000000ff;
-		req->ip_address.ip_address[1] = (s_addr & 0x0000ff00) >> 8;
-		req->ip_address.ip_address[2] = (s_addr & 0x00ff0000) >> 16;
-		req->ip_address.ip_address[3] = (s_addr & 0xff000000) >> 24;
+		req->ip_address.addr[0] = s_addr & 0x000000ff;
+		req->ip_address.addr[1] = (s_addr & 0x0000ff00) >> 8;
+		req->ip_address.addr[2] = (s_addr & 0x00ff0000) >> 16;
+		req->ip_address.addr[3] = (s_addr & 0xff000000) >> 24;
 		req->tcp_port = ntohs(daddr_in->sin_port);
 		beiscsi_ep->dst_addr = daddr_in->sin_addr.s_addr;
 		beiscsi_ep->dst_tcpport = ntohs(daddr_in->sin_port);
 		beiscsi_ep->ip_type = BE2_IPV4;
 	} else if (dst_addr->sa_family == PF_INET6) {
 		req->ip_address.ip_type = BE2_IPV6;
-		memcpy(&req->ip_address.ip_address,
+		memcpy(&req->ip_address.addr,
 		       &daddr_in6->sin6_addr.in6_u.u6_addr8, 16);
 		req->tcp_port = ntohs(daddr_in6->sin6_port);
 		beiscsi_ep->dst_tcpport = ntohs(daddr_in6->sin6_port);
@@ -419,14 +486,399 @@ int mgmt_open_connection(struct beiscsi_hba *phba,
 	return tag;
 }
 
-unsigned int be_cmd_get_mac_addr(struct beiscsi_hba *phba)
+unsigned int mgmt_get_all_if_id(struct beiscsi_hba *phba)
 {
 	struct be_ctrl_info *ctrl = &phba->ctrl;
-	struct be_mcc_wrb *wrb;
-	struct be_cmd_req_get_mac_addr *req;
+	struct be_mcc_wrb *wrb = wrb_from_mbox(&ctrl->mbox_mem);
+	struct be_cmd_get_all_if_id_req *req = embedded_payload(wrb);
+	struct be_cmd_get_all_if_id_req *pbe_allid = req;
+	int status = 0;
+
+	memset(wrb, 0, sizeof(*wrb));
+
+	spin_lock(&ctrl->mbox_lock);
+
+	be_wrb_hdr_prepare(wrb, sizeof(*req), true, 0);
+	be_cmd_hdr_prepare(&req->hdr, CMD_SUBSYSTEM_ISCSI,
+			   OPCODE_COMMON_ISCSI_NTWK_GET_ALL_IF_ID,
+			   sizeof(*req));
+	status = be_mbox_notify(ctrl);
+	if (!status)
+		phba->interface_handle = pbe_allid->if_hndl_list[0];
+	else {
+		shost_printk(KERN_WARNING, phba->shost,
+			     "Failed in mgmt_get_all_if_id\n");
+	}
+	spin_unlock(&ctrl->mbox_lock);
+
+	return status;
+}
+
+static int mgmt_exec_nonemb_cmd(struct beiscsi_hba *phba,
+				struct be_dma_mem *nonemb_cmd, void *resp_buf,
+				int resp_buf_len)
+{
+	struct be_ctrl_info *ctrl = &phba->ctrl;
+	struct be_mcc_wrb *wrb = wrb_from_mccq(phba);
+	unsigned short status, extd_status;
+	struct be_sge *sge;
+	unsigned int tag;
+	int rc = 0;
+
+	spin_lock(&ctrl->mbox_lock);
+	tag = alloc_mcc_tag(phba);
+	if (!tag) {
+		spin_unlock(&ctrl->mbox_lock);
+		rc = -ENOMEM;
+		goto free_cmd;
+	}
+	memset(wrb, 0, sizeof(*wrb));
+	wrb->tag0 |= tag;
+	sge = nonembedded_sgl(wrb);
+
+	be_wrb_hdr_prepare(wrb, nonemb_cmd->size, false, 1);
+	sge->pa_hi = cpu_to_le32(upper_32_bits(nonemb_cmd->dma));
+	sge->pa_lo = cpu_to_le32(nonemb_cmd->dma & 0xFFFFFFFF);
+	sge->len = cpu_to_le32(nonemb_cmd->size);
+
+	be_mcc_notify(phba);
+	spin_unlock(&ctrl->mbox_lock);
+
+	wait_event_interruptible(phba->ctrl.mcc_wait[tag],
+				 phba->ctrl.mcc_numtag[tag]);
+
+	extd_status = (phba->ctrl.mcc_numtag[tag] & 0x0000FF00) >> 8;
+	status = phba->ctrl.mcc_numtag[tag] & 0x000000FF;
+	if (status || extd_status) {
+		SE_DEBUG(DBG_LVL_1,
+			 "mgmt_exec_nonemb_cmd Failed status = %d"
+			 "extd_status = %d\n", status, extd_status);
+		rc = -EIO;
+		goto free_tag;
+	}
+
+	if (resp_buf)
+		memcpy(resp_buf, nonemb_cmd->va, resp_buf_len);
+
+free_tag:
+	free_mcc_tag(&phba->ctrl, tag);
+free_cmd:
+	pci_free_consistent(ctrl->pdev, nonemb_cmd->size,
+			    nonemb_cmd->va, nonemb_cmd->dma);
+	return rc;
+}
+
+static int mgmt_alloc_cmd_data(struct beiscsi_hba *phba, struct be_dma_mem *cmd,
+			       int iscsi_cmd, int size)
+{
+	cmd->va = pci_alloc_consistent(phba->ctrl.pdev, sizeof(size),
+				       &cmd->dma);
+	if (!cmd->va) {
+		SE_DEBUG(DBG_LVL_1, "Failed to allocate memory for if info\n");
+		return -ENOMEM;
+	}
+	memset(cmd->va, 0, sizeof(size));
+	cmd->size = size;
+	be_cmd_hdr_prepare(cmd->va, CMD_SUBSYSTEM_ISCSI, iscsi_cmd, size);
+	return 0;
+}
+
+static int
+mgmt_static_ip_modify(struct beiscsi_hba *phba,
+		      struct be_cmd_get_if_info_resp *if_info,
+		      struct iscsi_iface_param_info *ip_param,
+		      struct iscsi_iface_param_info *subnet_param,
+		      uint32_t ip_action)
+{
+	struct be_cmd_set_ip_addr_req *req;
+	struct be_dma_mem nonemb_cmd;
+	uint32_t ip_type;
+	int rc;
+
+	rc = mgmt_alloc_cmd_data(phba, &nonemb_cmd,
+				 OPCODE_COMMON_ISCSI_NTWK_MODIFY_IP_ADDR,
+				 sizeof(*req));
+	if (rc)
+		return rc;
+
+	ip_type = (ip_param->param == ISCSI_NET_PARAM_IPV6_ADDR) ?
+		BE2_IPV6 : BE2_IPV4 ;
+
+	req = nonemb_cmd.va;
+	req->ip_params.record_entry_count = 1;
+	req->ip_params.ip_record.action = ip_action;
+	req->ip_params.ip_record.interface_hndl =
+		phba->interface_handle;
+	req->ip_params.ip_record.ip_addr.size_of_structure =
+		sizeof(struct be_ip_addr_subnet_format);
+	req->ip_params.ip_record.ip_addr.ip_type = ip_type;
+
+	if (ip_action == IP_ACTION_ADD) {
+		memcpy(req->ip_params.ip_record.ip_addr.addr, ip_param->value,
+		       ip_param->len);
+
+		if (subnet_param)
+			memcpy(req->ip_params.ip_record.ip_addr.subnet_mask,
+			       subnet_param->value, subnet_param->len);
+	} else {
+		memcpy(req->ip_params.ip_record.ip_addr.addr,
+		       if_info->ip_addr.addr, ip_param->len);
+
+		memcpy(req->ip_params.ip_record.ip_addr.subnet_mask,
+		       if_info->ip_addr.subnet_mask, ip_param->len);
+	}
+
+	rc = mgmt_exec_nonemb_cmd(phba, &nonemb_cmd, NULL, 0);
+	if (rc < 0)
+		shost_printk(KERN_WARNING, phba->shost,
+			     "Failed to Modify existing IP Address\n");
+	return rc;
+}
+
+static int mgmt_modify_gateway(struct beiscsi_hba *phba, uint8_t *gt_addr,
+			       uint32_t gtway_action, uint32_t param_len)
+{
+	struct be_cmd_set_def_gateway_req *req;
+	struct be_dma_mem nonemb_cmd;
+	int rt_val;
+
+
+	rt_val = mgmt_alloc_cmd_data(phba, &nonemb_cmd,
+				OPCODE_COMMON_ISCSI_NTWK_MODIFY_DEFAULT_GATEWAY,
+				sizeof(*req));
+	if (rt_val)
+		return rt_val;
+
+	req = nonemb_cmd.va;
+	req->action = gtway_action;
+	req->ip_addr.ip_type = BE2_IPV4;
+
+	memcpy(req->ip_addr.addr, gt_addr, param_len);
+
+	return mgmt_exec_nonemb_cmd(phba, &nonemb_cmd, NULL, 0);
+}
+
+int mgmt_set_ip(struct beiscsi_hba *phba,
+		struct iscsi_iface_param_info *ip_param,
+		struct iscsi_iface_param_info *subnet_param,
+		uint32_t boot_proto)
+{
+	struct be_cmd_get_def_gateway_resp gtway_addr_set;
+	struct be_cmd_get_if_info_resp if_info;
+	struct be_cmd_set_dhcp_req *dhcpreq;
+	struct be_cmd_rel_dhcp_req *reldhcp;
+	struct be_dma_mem nonemb_cmd;
+	uint8_t *gtway_addr;
+	uint32_t ip_type;
+	int rc;
+
+	if (mgmt_get_all_if_id(phba))
+		return -EIO;
+
+	memset(&if_info, 0, sizeof(if_info));
+	ip_type = (ip_param->param == ISCSI_NET_PARAM_IPV6_ADDR) ?
+		BE2_IPV6 : BE2_IPV4 ;
+
+	rc = mgmt_get_if_info(phba, ip_type, &if_info);
+	if (rc)
+		return rc;
+
+	if (boot_proto == ISCSI_BOOTPROTO_DHCP) {
+		if (if_info.dhcp_state) {
+			shost_printk(KERN_WARNING, phba->shost,
+				     "DHCP Already Enabled\n");
+			return 0;
+		}
+		/* The ip_param->len is 1 in DHCP case. Setting
+		   proper IP len as this it is used while
+		   freeing the Static IP.
+		 */
+		ip_param->len = (ip_param->param == ISCSI_NET_PARAM_IPV6_ADDR) ?
+				IP_V6_LEN : IP_V4_LEN;
+
+	} else {
+		if (if_info.dhcp_state) {
+
+			memset(&if_info, 0, sizeof(if_info));
+			rc = mgmt_alloc_cmd_data(phba, &nonemb_cmd,
+				OPCODE_COMMON_ISCSI_NTWK_REL_STATELESS_IP_ADDR,
+				sizeof(*reldhcp));
+
+			if (rc)
+				return rc;
+
+			reldhcp = nonemb_cmd.va;
+			reldhcp->interface_hndl = phba->interface_handle;
+			reldhcp->ip_type = ip_type;
+
+			rc = mgmt_exec_nonemb_cmd(phba, &nonemb_cmd, NULL, 0);
+			if (rc < 0) {
+				shost_printk(KERN_WARNING, phba->shost,
+					     "Failed to Delete existing dhcp\n");
+				return rc;
+			}
+		}
+	}
+
+	/* Delete the Static IP Set */
+	if (if_info.ip_addr.addr[0]) {
+		rc = mgmt_static_ip_modify(phba, &if_info, ip_param, NULL,
+					   IP_ACTION_DEL);
+		if (rc)
+			return rc;
+	}
+
+	/* Delete the Gateway settings if mode change is to DHCP */
+	if (boot_proto == ISCSI_BOOTPROTO_DHCP) {
+		memset(&gtway_addr_set, 0, sizeof(gtway_addr_set));
+		rc = mgmt_get_gateway(phba, BE2_IPV4, &gtway_addr_set);
+		if (rc) {
+			shost_printk(KERN_WARNING, phba->shost,
+				     "Failed to Get Gateway Addr\n");
+			return rc;
+		}
+
+		if (gtway_addr_set.ip_addr.addr[0]) {
+			gtway_addr = (uint8_t *)&gtway_addr_set.ip_addr.addr;
+			rc = mgmt_modify_gateway(phba, gtway_addr,
+						 IP_ACTION_DEL, IP_V4_LEN);
+
+			if (rc) {
+				shost_printk(KERN_WARNING, phba->shost,
+					     "Failed to clear Gateway Addr Set\n");
+				return rc;
+			}
+		}
+	}
+
+	/* Set Adapter to DHCP/Static Mode */
+	if (boot_proto == ISCSI_BOOTPROTO_DHCP) {
+		rc = mgmt_alloc_cmd_data(phba, &nonemb_cmd,
+			OPCODE_COMMON_ISCSI_NTWK_CONFIG_STATELESS_IP_ADDR,
+			sizeof(*dhcpreq));
+		if (rc)
+			return rc;
+
+		dhcpreq = nonemb_cmd.va;
+		dhcpreq->flags = BLOCKING;
+		dhcpreq->retry_count = 1;
+		dhcpreq->interface_hndl = phba->interface_handle;
+		dhcpreq->ip_type = BE2_DHCP_V4;
+
+		return mgmt_exec_nonemb_cmd(phba, &nonemb_cmd, NULL, 0);
+	} else {
+		return mgmt_static_ip_modify(phba, &if_info, ip_param,
+					     subnet_param, IP_ACTION_ADD);
+	}
+
+	return rc;
+}
+
+int mgmt_set_gateway(struct beiscsi_hba *phba,
+		     struct iscsi_iface_param_info *gateway_param)
+{
+	struct be_cmd_get_def_gateway_resp gtway_addr_set;
+	uint8_t *gtway_addr;
+	int rt_val;
+
+	memset(&gtway_addr_set, 0, sizeof(gtway_addr_set));
+	rt_val = mgmt_get_gateway(phba, BE2_IPV4, &gtway_addr_set);
+	if (rt_val) {
+		shost_printk(KERN_WARNING, phba->shost,
+			     "Failed to Get Gateway Addr\n");
+		return rt_val;
+	}
+
+	if (gtway_addr_set.ip_addr.addr[0]) {
+		gtway_addr = (uint8_t *)&gtway_addr_set.ip_addr.addr;
+		rt_val = mgmt_modify_gateway(phba, gtway_addr, IP_ACTION_DEL,
+					     gateway_param->len);
+		if (rt_val) {
+			shost_printk(KERN_WARNING, phba->shost,
+				     "Failed to clear Gateway Addr Set\n");
+			return rt_val;
+		}
+	}
+
+	gtway_addr = (uint8_t *)&gateway_param->value;
+	rt_val = mgmt_modify_gateway(phba, gtway_addr, IP_ACTION_ADD,
+				     gateway_param->len);
+
+	if (rt_val)
+		shost_printk(KERN_WARNING, phba->shost,
+			     "Failed to Set Gateway Addr\n");
+
+	return rt_val;
+}
+
+int mgmt_get_gateway(struct beiscsi_hba *phba, int ip_type,
+		     struct be_cmd_get_def_gateway_resp *gateway)
+{
+	struct be_cmd_get_def_gateway_req *req;
+	struct be_dma_mem nonemb_cmd;
+	int rc;
+
+	rc = mgmt_alloc_cmd_data(phba, &nonemb_cmd,
+				 OPCODE_COMMON_ISCSI_NTWK_GET_DEFAULT_GATEWAY,
+				 sizeof(*gateway));
+	if (rc)
+		return rc;
+
+	req = nonemb_cmd.va;
+	req->ip_type = ip_type;
+
+	return mgmt_exec_nonemb_cmd(phba, &nonemb_cmd, gateway,
+				    sizeof(*gateway));
+}
+
+int mgmt_get_if_info(struct beiscsi_hba *phba, int ip_type,
+		     struct be_cmd_get_if_info_resp *if_info)
+{
+	struct be_cmd_get_if_info_req *req;
+	struct be_dma_mem nonemb_cmd;
+	int rc;
+
+	if (mgmt_get_all_if_id(phba))
+		return -EIO;
+
+	rc = mgmt_alloc_cmd_data(phba, &nonemb_cmd,
+				 OPCODE_COMMON_ISCSI_NTWK_GET_IF_INFO,
+				 sizeof(*if_info));
+	if (rc)
+		return rc;
+
+	req = nonemb_cmd.va;
+	req->interface_hndl = phba->interface_handle;
+	req->ip_type = ip_type;
+
+	return mgmt_exec_nonemb_cmd(phba, &nonemb_cmd, if_info,
+				    sizeof(*if_info));
+}
+
+int mgmt_get_nic_conf(struct beiscsi_hba *phba,
+		      struct be_cmd_get_nic_conf_resp *nic)
+{
+	struct be_dma_mem nonemb_cmd;
+	int rc;
+
+	rc = mgmt_alloc_cmd_data(phba, &nonemb_cmd,
+				 OPCODE_COMMON_ISCSI_NTWK_GET_NIC_CONFIG,
+				 sizeof(*nic));
+	if (rc)
+		return rc;
+
+	return mgmt_exec_nonemb_cmd(phba, &nonemb_cmd, nic, sizeof(*nic));
+}
+
+
+
+unsigned int be_cmd_get_initname(struct beiscsi_hba *phba)
+{
 	unsigned int tag = 0;
+	struct be_mcc_wrb *wrb;
+	struct be_cmd_hba_name *req;
+	struct be_ctrl_info *ctrl = &phba->ctrl;
 
-	SE_DEBUG(DBG_LVL_8, "In be_cmd_get_mac_addr\n");
 	spin_lock(&ctrl->mbox_lock);
 	tag = alloc_mcc_tag(phba);
 	if (!tag) {
@@ -438,12 +890,38 @@ unsigned int be_cmd_get_mac_addr(struct beiscsi_hba *phba)
 	req = embedded_payload(wrb);
 	wrb->tag0 |= tag;
 	be_wrb_hdr_prepare(wrb, sizeof(*req), true, 0);
-	be_cmd_hdr_prepare(&req->hdr, CMD_SUBSYSTEM_ISCSI,
-			   OPCODE_COMMON_ISCSI_NTWK_GET_NIC_CONFIG,
-			   sizeof(*req));
+	be_cmd_hdr_prepare(&req->hdr, CMD_SUBSYSTEM_ISCSI_INI,
+			OPCODE_ISCSI_INI_CFG_GET_HBA_NAME,
+			sizeof(*req));
 
 	be_mcc_notify(phba);
 	spin_unlock(&ctrl->mbox_lock);
 	return tag;
 }
 
+unsigned int be_cmd_get_port_speed(struct beiscsi_hba *phba)
+{
+	unsigned int tag = 0;
+	struct be_mcc_wrb *wrb;
+	struct be_cmd_ntwk_link_status_req *req;
+	struct be_ctrl_info *ctrl = &phba->ctrl;
+
+	spin_lock(&ctrl->mbox_lock);
+	tag = alloc_mcc_tag(phba);
+	if (!tag) {
+		spin_unlock(&ctrl->mbox_lock);
+		return tag;
+	}
+
+	wrb = wrb_from_mccq(phba);
+	req = embedded_payload(wrb);
+	wrb->tag0 |= tag;
+	be_wrb_hdr_prepare(wrb, sizeof(*req), true, 0);
+	be_cmd_hdr_prepare(&req->hdr, CMD_SUBSYSTEM_COMMON,
+			OPCODE_COMMON_NTWK_LINK_STATUS_QUERY,
+			sizeof(*req));
+
+	be_mcc_notify(phba);
+	spin_unlock(&ctrl->mbox_lock);
+	return tag;
+}
diff --git a/drivers/scsi/be2iscsi/be_mgmt.h b/drivers/scsi/be2iscsi/be_mgmt.h
index 08428824ace..5c2e37693ca 100644
--- a/drivers/scsi/be2iscsi/be_mgmt.h
+++ b/drivers/scsi/be2iscsi/be_mgmt.h
@@ -20,11 +20,16 @@
 #ifndef _BEISCSI_MGMT_
 #define _BEISCSI_MGMT_
 
-#include <linux/types.h>
-#include <linux/list.h>
+#include <scsi/scsi_bsg_iscsi.h>
 #include "be_iscsi.h"
 #include "be_main.h"
 
+#define IP_ACTION_ADD	0x01
+#define IP_ACTION_DEL	0x02
+
+#define IP_V6_LEN	16
+#define IP_V4_LEN	4
+
 /**
  * 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
@@ -98,6 +103,10 @@ unsigned int mgmt_invalidate_icds(struct beiscsi_hba *phba,
 				struct invalidate_command_table *inv_tbl,
 				unsigned int num_invalidate, unsigned int cid,
 				struct be_dma_mem *nonemb_cmd);
+unsigned int mgmt_vendor_specific_fw_cmd(struct be_ctrl_info *ctrl,
+					 struct beiscsi_hba *phba,
+					 struct bsg_job *job,
+					 struct be_dma_mem *nonemb_cmd);
 
 struct iscsi_invalidate_connection_params_in {
 	struct be_cmd_req_hdr hdr;
@@ -204,6 +213,13 @@ struct be_mgmt_controller_attributes_resp {
 	struct mgmt_controller_attributes params;
 } __packed;
 
+struct be_bsg_vendor_cmd {
+	struct be_cmd_req_hdr hdr;
+	unsigned short region;
+	unsigned short offset;
+	unsigned short sector;
+} __packed;
+
 /* configuration management */
 
 #define GET_MGMT_CONTROLLER_WS(phba)    (phba->pmgmt_ws)
@@ -219,12 +235,15 @@ struct be_mgmt_controller_attributes_resp {
 				/* the CMD_RESPONSE_HEADER  */
 
 #define ISCSI_GET_PDU_TEMPLATE_ADDRESS(pc, pa) {\
-    pa->lo = phba->init_mem[ISCSI_MEM_GLOBAL_HEADER].mem_array[0].\
+	pa->lo = phba->init_mem[ISCSI_MEM_GLOBAL_HEADER].mem_array[0].\
 					bus_address.u.a32.address_lo;  \
-    pa->hi = phba->init_mem[ISCSI_MEM_GLOBAL_HEADER].mem_array[0].\
+	pa->hi = phba->init_mem[ISCSI_MEM_GLOBAL_HEADER].mem_array[0].\
 					bus_address.u.a32.address_hi;  \
 }
 
+#define BEISCSI_WRITE_FLASH 0
+#define BEISCSI_READ_FLASH 1
+
 struct beiscsi_endpoint {
 	struct beiscsi_hba *phba;
 	struct beiscsi_sess *sess;
@@ -248,4 +267,27 @@ unsigned int mgmt_invalidate_connection(struct beiscsi_hba *phba,
 					 unsigned short issue_reset,
 					 unsigned short savecfg_flag);
 
+int mgmt_set_ip(struct beiscsi_hba *phba,
+		struct iscsi_iface_param_info *ip_param,
+		struct iscsi_iface_param_info *subnet_param,
+		uint32_t boot_proto);
+
+unsigned int mgmt_get_boot_target(struct beiscsi_hba *phba);
+
+unsigned int mgmt_get_session_info(struct beiscsi_hba *phba,
+				   u32 boot_session_handle,
+				   struct be_dma_mem *nonemb_cmd);
+
+int mgmt_get_nic_conf(struct beiscsi_hba *phba,
+		      struct be_cmd_get_nic_conf_resp *mac);
+
+int mgmt_get_if_info(struct beiscsi_hba *phba, int ip_type,
+		     struct be_cmd_get_if_info_resp *if_info);
+
+int mgmt_get_gateway(struct beiscsi_hba *phba, int ip_type,
+		     struct be_cmd_get_def_gateway_resp *gateway);
+
+int mgmt_set_gateway(struct beiscsi_hba *phba,
+		     struct iscsi_iface_param_info *gateway_param);
+
 #endif
diff --git a/drivers/scsi/bfa/bfa_fcs.h b/drivers/scsi/bfa/bfa_fcs.h
index e75e07d2591..51c9e134571 100644
--- a/drivers/scsi/bfa/bfa_fcs.h
+++ b/drivers/scsi/bfa/bfa_fcs.h
@@ -799,9 +799,6 @@ struct bfad_port_s *bfa_fcb_lport_new(struct bfad_s *bfad,
 				      enum bfa_lport_role roles,
 				      struct bfad_vf_s *vf_drv,
 				      struct bfad_vport_s *vp_drv);
-void bfa_fcb_lport_delete(struct bfad_s *bfad, enum bfa_lport_role roles,
-			  struct bfad_vf_s *vf_drv,
-			  struct bfad_vport_s *vp_drv);
 
 /*
  * vport callbacks
diff --git a/drivers/scsi/bfa/bfa_fcs_lport.c b/drivers/scsi/bfa/bfa_fcs_lport.c
index 5d2a1307e5c..937000db62a 100644
--- a/drivers/scsi/bfa/bfa_fcs_lport.c
+++ b/drivers/scsi/bfa/bfa_fcs_lport.c
@@ -616,7 +616,7 @@ bfa_fcs_lport_online_actions(struct bfa_fcs_lport_s *port)
 	__port_action[port->fabric->fab_type].online(port);
 
 	wwn2str(lpwwn_buf, bfa_fcs_lport_get_pwwn(port));
-	BFA_LOG(KERN_INFO, bfad, bfa_log_level,
+	BFA_LOG(KERN_WARNING, bfad, bfa_log_level,
 		"Logical port online: WWN = %s Role = %s\n",
 		lpwwn_buf, "Initiator");
 	bfa_fcs_lport_aen_post(port, BFA_LPORT_AEN_ONLINE);
@@ -639,12 +639,12 @@ bfa_fcs_lport_offline_actions(struct bfa_fcs_lport_s *port)
 	wwn2str(lpwwn_buf, bfa_fcs_lport_get_pwwn(port));
 	if (bfa_sm_cmp_state(port->fabric,
 			bfa_fcs_fabric_sm_online) == BFA_TRUE) {
-		BFA_LOG(KERN_ERR, bfad, bfa_log_level,
+		BFA_LOG(KERN_WARNING, bfad, bfa_log_level,
 		"Logical port lost fabric connectivity: WWN = %s Role = %s\n",
 		lpwwn_buf, "Initiator");
 		bfa_fcs_lport_aen_post(port, BFA_LPORT_AEN_DISCONNECT);
 	} else {
-		BFA_LOG(KERN_INFO, bfad, bfa_log_level,
+		BFA_LOG(KERN_WARNING, bfad, bfa_log_level,
 		"Logical port taken offline: WWN = %s Role = %s\n",
 		lpwwn_buf, "Initiator");
 		bfa_fcs_lport_aen_post(port, BFA_LPORT_AEN_OFFLINE);
@@ -709,14 +709,10 @@ bfa_fcs_lport_deleted(struct bfa_fcs_lport_s *port)
 	bfa_fcs_lport_aen_post(port, BFA_LPORT_AEN_DELETE);
 
 	/* Base port will be deleted by the OS driver */
-	if (port->vport) {
-		bfa_fcb_lport_delete(port->fcs->bfad, port->port_cfg.roles,
-				port->fabric->vf_drv,
-				port->vport ? port->vport->vport_drv : NULL);
+	if (port->vport)
 		bfa_fcs_vport_delete_comp(port->vport);
-	} else {
+	else
 		bfa_wc_down(&port->fabric->wc);
-	}
 }
 
 
@@ -5714,17 +5710,23 @@ bfa_fcs_vport_free(struct bfa_fcs_vport_s *vport)
 			(struct bfad_vport_s *)vport->vport_drv;
 
 	bfa_fcs_fabric_delvport(__vport_fabric(vport), vport);
+	bfa_lps_delete(vport->lps);
 
-	if (vport_drv->comp_del)
+	if (vport_drv->comp_del) {
 		complete(vport_drv->comp_del);
-	else
-		kfree(vport_drv);
+		return;
+	}
 
-	bfa_lps_delete(vport->lps);
+	/*
+	 * We queue the vport delete work to the IM work_q from here.
+	 * The memory for the bfad_vport_s is freed from the FC function
+	 * template vport_delete entry point.
+	 */
+	if (vport_drv)
+		bfad_im_port_delete(vport_drv->drv_port.bfad,
+				&vport_drv->drv_port);
 }
 
-
-
 /*
  *  fcs_vport_public FCS virtual port public interfaces
  */
diff --git a/drivers/scsi/bfa/bfad.c b/drivers/scsi/bfa/bfad.c
index 404fd10ddb2..2e4b0be14a2 100644
--- a/drivers/scsi/bfa/bfad.c
+++ b/drivers/scsi/bfa/bfad.c
@@ -456,23 +456,6 @@ bfa_fcb_lport_new(struct bfad_s *bfad, struct bfa_fcs_lport_s *port,
 	return port_drv;
 }
 
-void
-bfa_fcb_lport_delete(struct bfad_s *bfad, enum bfa_lport_role roles,
-		    struct bfad_vf_s *vf_drv, struct bfad_vport_s *vp_drv)
-{
-	struct bfad_port_s    *port_drv;
-
-	/* this will be only called from rmmod context */
-	if (vp_drv && !vp_drv->comp_del) {
-		port_drv = (vp_drv) ? (&(vp_drv)->drv_port) :
-				((vf_drv) ? (&(vf_drv)->base_port) :
-				(&(bfad)->pport));
-		bfa_trc(bfad, roles);
-		if (roles & BFA_LPORT_ROLE_FCP_IM)
-			bfad_im_port_delete(bfad, port_drv);
-	}
-}
-
 /*
  * FCS RPORT alloc callback, after successful PLOGI by FCS
  */
diff --git a/drivers/scsi/bfa/bfad_attr.c b/drivers/scsi/bfa/bfad_attr.c
index 7b1ecd2b3ff..8b6c6bf7837 100644
--- a/drivers/scsi/bfa/bfad_attr.c
+++ b/drivers/scsi/bfa/bfad_attr.c
@@ -497,6 +497,7 @@ bfad_im_vport_delete(struct fc_vport *fc_vport)
 	if (im_port->flags & BFAD_PORT_DELETE) {
 		bfad_scsi_host_free(bfad, im_port);
 		list_del(&vport->list_entry);
+		kfree(vport);
 		return 0;
 	}
 
@@ -758,25 +759,10 @@ bfad_im_model_desc_show(struct device *dev, struct device_attribute *attr,
 	else if (!strcmp(model, "Brocade-804"))
 		snprintf(model_descr, BFA_ADAPTER_MODEL_DESCR_LEN,
 			"Brocade 8Gbps FC HBA for HP Bladesystem C-class");
-	else if (!strcmp(model, "Brocade-902") ||
-		 !strcmp(model, "Brocade-1741"))
+	else if (!strcmp(model, "Brocade-1741"))
 		snprintf(model_descr, BFA_ADAPTER_MODEL_DESCR_LEN,
 			"Brocade 10Gbps CNA for Dell M-Series Blade Servers");
-	else if (strstr(model, "Brocade-1560")) {
-		if (nports == 1)
-			snprintf(model_descr, BFA_ADAPTER_MODEL_DESCR_LEN,
-				"Brocade 16Gbps PCIe single port FC HBA");
-		else
-			snprintf(model_descr, BFA_ADAPTER_MODEL_DESCR_LEN,
-				"Brocade 16Gbps PCIe dual port FC HBA");
-	} else if (strstr(model, "Brocade-1710")) {
-		if (nports == 1)
-			snprintf(model_descr, BFA_ADAPTER_MODEL_DESCR_LEN,
-				"Brocade 10Gbps single port CNA");
-		else
-			snprintf(model_descr, BFA_ADAPTER_MODEL_DESCR_LEN,
-				"Brocade 10Gbps dual port CNA");
-	} else if (strstr(model, "Brocade-1860")) {
+	else if (strstr(model, "Brocade-1860")) {
 		if (nports == 1 && bfa_ioc_is_cna(&bfad->bfa.ioc))
 			snprintf(model_descr, BFA_ADAPTER_MODEL_DESCR_LEN,
 				"Brocade 10Gbps single port CNA");
diff --git a/drivers/scsi/bnx2i/57xx_iscsi_constants.h b/drivers/scsi/bnx2i/57xx_iscsi_constants.h
index 495a841645f..25093a04123 100644
--- a/drivers/scsi/bnx2i/57xx_iscsi_constants.h
+++ b/drivers/scsi/bnx2i/57xx_iscsi_constants.h
@@ -1,6 +1,6 @@
 /* 57xx_iscsi_constants.h: Broadcom NetXtreme II iSCSI HSI
  *
- * Copyright (c) 2006 - 2011 Broadcom Corporation
+ * Copyright (c) 2006 - 2012 Broadcom Corporation
  *
  * This program is free software; you can redistribute it and/or modify
  * it under the terms of the GNU General Public License as published by
diff --git a/drivers/scsi/bnx2i/57xx_iscsi_hsi.h b/drivers/scsi/bnx2i/57xx_iscsi_hsi.h
index 72118db89a2..dc0a08e69c8 100644
--- a/drivers/scsi/bnx2i/57xx_iscsi_hsi.h
+++ b/drivers/scsi/bnx2i/57xx_iscsi_hsi.h
@@ -1,6 +1,6 @@
 /* 57xx_iscsi_hsi.h: Broadcom NetXtreme II iSCSI HSI.
  *
- * Copyright (c) 2006 - 2011 Broadcom Corporation
+ * Copyright (c) 2006 - 2012 Broadcom Corporation
  *
  * This program is free software; you can redistribute it and/or modify
  * it under the terms of the GNU General Public License as published by
diff --git a/drivers/scsi/bnx2i/bnx2i.h b/drivers/scsi/bnx2i/bnx2i.h
index 0bd70e80efe..0c53c28dc3d 100644
--- a/drivers/scsi/bnx2i/bnx2i.h
+++ b/drivers/scsi/bnx2i/bnx2i.h
@@ -1,6 +1,6 @@
 /* bnx2i.h: Broadcom NetXtreme II iSCSI driver.
  *
- * Copyright (c) 2006 - 2011 Broadcom Corporation
+ * Copyright (c) 2006 - 2012 Broadcom Corporation
  * Copyright (c) 2007, 2008 Red Hat, Inc.  All rights reserved.
  * Copyright (c) 2007, 2008 Mike Christie
  *
diff --git a/drivers/scsi/bnx2i/bnx2i_hwi.c b/drivers/scsi/bnx2i/bnx2i_hwi.c
index f9d6f412909..ece47e50228 100644
--- a/drivers/scsi/bnx2i/bnx2i_hwi.c
+++ b/drivers/scsi/bnx2i/bnx2i_hwi.c
@@ -1,6 +1,6 @@
 /* bnx2i_hwi.c: Broadcom NetXtreme II iSCSI driver.
  *
- * Copyright (c) 2006 - 2011 Broadcom Corporation
+ * Copyright (c) 2006 - 2012 Broadcom Corporation
  * Copyright (c) 2007, 2008 Red Hat, Inc.  All rights reserved.
  * Copyright (c) 2007, 2008 Mike Christie
  *
diff --git a/drivers/scsi/bnx2i/bnx2i_init.c b/drivers/scsi/bnx2i/bnx2i_init.c
index 4927cca733d..8b6816706ee 100644
--- a/drivers/scsi/bnx2i/bnx2i_init.c
+++ b/drivers/scsi/bnx2i/bnx2i_init.c
@@ -1,6 +1,6 @@
 /* bnx2i.c: Broadcom NetXtreme II iSCSI driver.
  *
- * Copyright (c) 2006 - 2011 Broadcom Corporation
+ * Copyright (c) 2006 - 2012 Broadcom Corporation
  * Copyright (c) 2007, 2008 Red Hat, Inc.  All rights reserved.
  * Copyright (c) 2007, 2008 Mike Christie
  *
@@ -18,8 +18,8 @@ static struct list_head adapter_list = LIST_HEAD_INIT(adapter_list);
 static u32 adapter_count;
 
 #define DRV_MODULE_NAME		"bnx2i"
-#define DRV_MODULE_VERSION	"2.7.0.3"
-#define DRV_MODULE_RELDATE	"Jun 15, 2011"
+#define DRV_MODULE_VERSION	"2.7.2.2"
+#define DRV_MODULE_RELDATE	"Apr 25, 2012"
 
 static char version[] __devinitdata =
 		"Broadcom NetXtreme II iSCSI Driver " DRV_MODULE_NAME \
diff --git a/drivers/scsi/bnx2i/bnx2i_iscsi.c b/drivers/scsi/bnx2i/bnx2i_iscsi.c
index 1a44b45e7be..f8d516b5316 100644
--- a/drivers/scsi/bnx2i/bnx2i_iscsi.c
+++ b/drivers/scsi/bnx2i/bnx2i_iscsi.c
@@ -1,7 +1,7 @@
 /*
  * bnx2i_iscsi.c: Broadcom NetXtreme II iSCSI driver.
  *
- * Copyright (c) 2006 - 2011 Broadcom Corporation
+ * Copyright (c) 2006 - 2012 Broadcom Corporation
  * Copyright (c) 2007, 2008 Red Hat, Inc.  All rights reserved.
  * Copyright (c) 2007, 2008 Mike Christie
  *
@@ -2244,6 +2244,7 @@ static struct scsi_host_template bnx2i_host_template = {
 	.eh_device_reset_handler = iscsi_eh_device_reset,
 	.eh_target_reset_handler = iscsi_eh_recover_target,
 	.change_queue_depth	= iscsi_change_queue_depth,
+	.target_alloc		= iscsi_target_alloc,
 	.can_queue		= 2048,
 	.max_sectors		= 127,
 	.cmd_per_lun		= 128,
diff --git a/drivers/scsi/bnx2i/bnx2i_sysfs.c b/drivers/scsi/bnx2i/bnx2i_sysfs.c
index 83a77f7244d..c61cf7a4365 100644
--- a/drivers/scsi/bnx2i/bnx2i_sysfs.c
+++ b/drivers/scsi/bnx2i/bnx2i_sysfs.c
@@ -1,6 +1,6 @@
 /* bnx2i_sysfs.c: Broadcom NetXtreme II iSCSI driver.
  *
- * Copyright (c) 2004 - 2011 Broadcom Corporation
+ * Copyright (c) 2004 - 2012 Broadcom Corporation
  *
  * This program is free software; you can redistribute it and/or modify
  * it under the terms of the GNU General Public License as published by
diff --git a/drivers/scsi/device_handler/scsi_dh_alua.c b/drivers/scsi/device_handler/scsi_dh_alua.c
index 04c5cea47a2..fda9cdea0e6 100644
--- a/drivers/scsi/device_handler/scsi_dh_alua.c
+++ b/drivers/scsi/device_handler/scsi_dh_alua.c
@@ -55,11 +55,16 @@
 #define ALUA_FAILOVER_TIMEOUT		(60 * HZ)
 #define ALUA_FAILOVER_RETRIES		5
 
+/* flags passed from user level */
+#define ALUA_OPTIMIZE_STPG		1
+
 struct alua_dh_data {
 	int			group_id;
 	int			rel_port;
 	int			tpgs;
 	int			state;
+	int			pref;
+	unsigned		flags; /* used for optimizing STPG */
 	unsigned char		inq[ALUA_INQUIRY_SIZE];
 	unsigned char		*buff;
 	int			bufflen;
@@ -554,14 +559,16 @@ static int alua_rtpg(struct scsi_device *sdev, struct alua_dh_data *h)
 	for (k = 4, ucp = h->buff + 4; k < len; k += off, ucp += off) {
 		if (h->group_id == (ucp[2] << 8) + ucp[3]) {
 			h->state = ucp[0] & 0x0f;
+			h->pref = ucp[0] >> 7;
 			valid_states = ucp[1];
 		}
 		off = 8 + (ucp[7] * 4);
 	}
 
 	sdev_printk(KERN_INFO, sdev,
-		    "%s: port group %02x state %c supports %c%c%c%c%c%c%c\n",
+		    "%s: port group %02x state %c %s supports %c%c%c%c%c%c%c\n",
 		    ALUA_DH_NAME, h->group_id, print_alua_state(h->state),
+		    h->pref ? "preferred" : "non-preferred",
 		    valid_states&TPGS_SUPPORT_TRANSITION?'T':'t',
 		    valid_states&TPGS_SUPPORT_OFFLINE?'O':'o',
 		    valid_states&TPGS_SUPPORT_LBA_DEPENDENT?'L':'l',
@@ -621,6 +628,37 @@ static int alua_initialize(struct scsi_device *sdev, struct alua_dh_data *h)
 out:
 	return err;
 }
+/*
+ * alua_set_params - set/unset the optimize flag
+ * @sdev: device on the path to be activated
+ * params - parameters in the following format
+ *      "no_of_params\0param1\0param2\0param3\0...\0"
+ * For example, to set the flag pass the following parameters
+ * from multipath.conf
+ *     hardware_handler        "2 alua 1"
+ */
+static int alua_set_params(struct scsi_device *sdev, const char *params)
+{
+	struct alua_dh_data *h = get_alua_data(sdev);
+	unsigned int optimize = 0, argc;
+	const char *p = params;
+	int result = SCSI_DH_OK;
+
+	if ((sscanf(params, "%u", &argc) != 1) || (argc != 1))
+		return -EINVAL;
+
+	while (*p++)
+		;
+	if ((sscanf(p, "%u", &optimize) != 1) || (optimize > 1))
+		return -EINVAL;
+
+	if (optimize)
+		h->flags |= ALUA_OPTIMIZE_STPG;
+	else
+		h->flags &= ~ALUA_OPTIMIZE_STPG;
+
+	return result;
+}
 
 /*
  * alua_activate - activate a path
@@ -637,14 +675,37 @@ static int alua_activate(struct scsi_device *sdev,
 {
 	struct alua_dh_data *h = get_alua_data(sdev);
 	int err = SCSI_DH_OK;
+	int stpg = 0;
 
 	err = alua_rtpg(sdev, h);
 	if (err != SCSI_DH_OK)
 		goto out;
 
-	if (h->tpgs & TPGS_MODE_EXPLICIT &&
-	    h->state != TPGS_STATE_OPTIMIZED &&
-	    h->state != TPGS_STATE_LBA_DEPENDENT) {
+	if (h->tpgs & TPGS_MODE_EXPLICIT) {
+		switch (h->state) {
+		case TPGS_STATE_NONOPTIMIZED:
+			stpg = 1;
+			if ((h->flags & ALUA_OPTIMIZE_STPG) &&
+			    (!h->pref) &&
+			    (h->tpgs & TPGS_MODE_IMPLICIT))
+				stpg = 0;
+			break;
+		case TPGS_STATE_STANDBY:
+			stpg = 1;
+			break;
+		case TPGS_STATE_UNAVAILABLE:
+		case TPGS_STATE_OFFLINE:
+			err = SCSI_DH_IO;
+			break;
+		case TPGS_STATE_TRANSITIONING:
+			err = SCSI_DH_RETRY;
+			break;
+		default:
+			break;
+		}
+	}
+
+	if (stpg) {
 		h->callback_fn = fn;
 		h->callback_data = data;
 		err = submit_stpg(h);
@@ -698,6 +759,7 @@ static struct scsi_device_handler alua_dh = {
 	.prep_fn = alua_prep_fn,
 	.check_sense = alua_check_sense,
 	.activate = alua_activate,
+	.set_params = alua_set_params,
 	.match = alua_match,
 };
 
diff --git a/drivers/scsi/esp_scsi.c b/drivers/scsi/esp_scsi.c
index 394ed9e79fd..34552bf1c02 100644
--- a/drivers/scsi/esp_scsi.c
+++ b/drivers/scsi/esp_scsi.c
@@ -1000,7 +1000,7 @@ static int esp_check_spur_intr(struct esp *esp)
 
 static void esp_schedule_reset(struct esp *esp)
 {
-	esp_log_reset("ESP: esp_schedule_reset() from %p\n",
+	esp_log_reset("ESP: esp_schedule_reset() from %pf\n",
 		      __builtin_return_address(0));
 	esp->flags |= ESP_FLAG_RESETTING;
 	esp_event(esp, ESP_EVENT_RESET);
diff --git a/drivers/scsi/fcoe/fcoe.c b/drivers/scsi/fcoe/fcoe.c
index 335e8519280..76e3d0b5bfa 100644
--- a/drivers/scsi/fcoe/fcoe.c
+++ b/drivers/scsi/fcoe/fcoe.c
@@ -411,20 +411,18 @@ out:
 }
 
 /**
- * fcoe_interface_cleanup() - Clean up a FCoE interface
+ * fcoe_interface_remove() - remove FCoE interface from netdev
  * @fcoe: The FCoE interface to be cleaned up
  *
  * Caller must be holding the RTNL mutex
  */
-static void fcoe_interface_cleanup(struct fcoe_interface *fcoe)
+static void fcoe_interface_remove(struct fcoe_interface *fcoe)
 {
 	struct net_device *netdev = fcoe->netdev;
 	struct fcoe_ctlr *fip = &fcoe->ctlr;
 	u8 flogi_maddr[ETH_ALEN];
 	const struct net_device_ops *ops;
 
-	rtnl_lock();
-
 	/*
 	 * Don't listen for Ethernet packets anymore.
 	 * synchronize_net() ensures that the packet handlers are not running
@@ -453,12 +451,28 @@ static void fcoe_interface_cleanup(struct fcoe_interface *fcoe)
 			FCOE_NETDEV_DBG(netdev, "Failed to disable FCoE"
 					" specific feature for LLD.\n");
 	}
+	fcoe->removed = 1;
+}
+
+
+/**
+ * fcoe_interface_cleanup() - Clean up a FCoE interface
+ * @fcoe: The FCoE interface to be cleaned up
+ */
+static void fcoe_interface_cleanup(struct fcoe_interface *fcoe)
+{
+	struct net_device *netdev = fcoe->netdev;
+	struct fcoe_ctlr *fip = &fcoe->ctlr;
 
+	rtnl_lock();
+	if (!fcoe->removed)
+		fcoe_interface_remove(fcoe);
 	rtnl_unlock();
 
 	/* Release the self-reference taken during fcoe_interface_create() */
 	/* tear-down the FCoE controller */
 	fcoe_ctlr_destroy(fip);
+	scsi_host_put(fcoe->ctlr.lp->host);
 	kfree(fcoe);
 	dev_put(netdev);
 	module_put(THIS_MODULE);
@@ -522,13 +536,11 @@ static void fcoe_update_src_mac(struct fc_lport *lport, u8 *addr)
 	struct fcoe_port *port = lport_priv(lport);
 	struct fcoe_interface *fcoe = port->priv;
 
-	rtnl_lock();
 	if (!is_zero_ether_addr(port->data_src_addr))
 		dev_uc_del(fcoe->netdev, port->data_src_addr);
 	if (!is_zero_ether_addr(addr))
 		dev_uc_add(fcoe->netdev, addr);
 	memcpy(port->data_src_addr, addr, ETH_ALEN);
-	rtnl_unlock();
 }
 
 /**
@@ -941,6 +953,10 @@ static void fcoe_if_destroy(struct fc_lport *lport)
 	rtnl_lock();
 	if (!is_zero_ether_addr(port->data_src_addr))
 		dev_uc_del(netdev, port->data_src_addr);
+	if (lport->vport)
+		synchronize_net();
+	else
+		fcoe_interface_remove(fcoe);
 	rtnl_unlock();
 
 	/* Free queued packets for the per-CPU receive threads */
@@ -959,8 +975,12 @@ static void fcoe_if_destroy(struct fc_lport *lport)
 	/* Free memory used by statistical counters */
 	fc_lport_free_stats(lport);
 
-	/* Release the Scsi_Host */
-	scsi_host_put(lport->host);
+	/*
+	 * Release the Scsi_Host for vport but hold on to
+	 * master lport until it fcoe interface fully cleaned-up.
+	 */
+	if (lport->vport)
+		scsi_host_put(lport->host);
 }
 
 /**
@@ -2274,10 +2294,9 @@ static void fcoe_percpu_clean(struct fc_lport *lport)
 			continue;
 
 		skb = dev_alloc_skb(0);
-		if (!skb) {
-			spin_unlock_bh(&pp->fcoe_rx_list.lock);
+		if (!skb)
 			continue;
-		}
+
 		skb->destructor = fcoe_percpu_flush_done;
 
 		spin_lock_bh(&pp->fcoe_rx_list.lock);
diff --git a/drivers/scsi/fcoe/fcoe.h b/drivers/scsi/fcoe/fcoe.h
index 3c2733a12aa..96ac938d39c 100644
--- a/drivers/scsi/fcoe/fcoe.h
+++ b/drivers/scsi/fcoe/fcoe.h
@@ -71,7 +71,8 @@ do {                                                            	\
  * @ctlr:	      The FCoE controller (for FIP)
  * @oem:	      The offload exchange manager for all local port
  *		      instances associated with this port
- * This structure is 1:1 with a net devive.
+ * @removed:	      Indicates fcoe interface removed from net device
+ * This structure is 1:1 with a net device.
  */
 struct fcoe_interface {
 	struct list_head   list;
@@ -81,6 +82,7 @@ struct fcoe_interface {
 	struct packet_type fip_packet_type;
 	struct fcoe_ctlr   ctlr;
 	struct fc_exch_mgr *oem;
+	u8	removed;
 };
 
 #define fcoe_from_ctlr(fip) container_of(fip, struct fcoe_interface, ctlr)
diff --git a/drivers/scsi/fcoe/fcoe_ctlr.c b/drivers/scsi/fcoe/fcoe_ctlr.c
index 249a106888d..5a4c7250aa7 100644
--- a/drivers/scsi/fcoe/fcoe_ctlr.c
+++ b/drivers/scsi/fcoe/fcoe_ctlr.c
@@ -1883,7 +1883,13 @@ static void fcoe_ctlr_vn_send(struct fcoe_ctlr *fip,
 	frame = (struct fip_frame *)skb->data;
 	memset(frame, 0, len);
 	memcpy(frame->eth.h_dest, dest, ETH_ALEN);
-	memcpy(frame->eth.h_source, fip->ctl_src_addr, ETH_ALEN);
+
+	if (sub == FIP_SC_VN_BEACON) {
+		hton24(frame->eth.h_source, FIP_VN_FC_MAP);
+		hton24(frame->eth.h_source + 3, fip->port_id);
+	} else {
+		memcpy(frame->eth.h_source, fip->ctl_src_addr, ETH_ALEN);
+	}
 	frame->eth.h_proto = htons(ETH_P_FIP);
 
 	frame->fip.fip_ver = FIP_VER_ENCAPS(FIP_VER);
diff --git a/drivers/scsi/fd_mcs.c b/drivers/scsi/fd_mcs.c
deleted file mode 100644
index 53bfcaa86f0..00000000000
--- a/drivers/scsi/fd_mcs.c
+++ /dev/null
@@ -1,1354 +0,0 @@
-/* fd_mcs.c -- Future Domain MCS 600/700 (or IBM OEM) driver
- *
- * FutureDomain MCS-600/700 v0.2 03/11/1998 by ZP Gu (zpg@castle.net)
- *
- * This driver is cloned from fdomain.* to specifically support
- * the Future Domain MCS 600/700 MCA SCSI adapters. Some PS/2s
- * also equipped with IBM Fast SCSI Adapter/A which is an OEM
- * of MCS 700.
- *
- * This driver also supports Reply SB16/SCSI card (the SCSI part).
- *
- * What makes this driver different is that this driver is MCA only
- * and it supports multiple adapters in the same system, IRQ 
- * sharing, some driver statistics, and maps highest SCSI id to sda.
- * All cards are auto-detected.
- *
- * Assumptions: TMC-1800/18C50/18C30, BIOS >= 3.4
- *
- * LILO command-line options:
- *   fd_mcs=<FIFO_COUNT>[,<FIFO_SIZE>]
- *
- * ********************************************************
- * Please see Copyrights/Comments in fdomain.* for credits.
- * Following is from fdomain.c for acknowledgement:
- *
- * Created: Sun May  3 18:53:19 1992 by faith@cs.unc.edu
- * Revised: Wed Oct  2 11:10:55 1996 by r.faith@ieee.org
- * Author: Rickard E. Faith, faith@cs.unc.edu
- * Copyright 1992, 1993, 1994, 1995, 1996 Rickard E. Faith
- *
- * $Id: fdomain.c,v 5.45 1996/10/02 15:13:06 root Exp $
-
- * 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.
-
- **************************************************************************
-
- NOTES ON USER DEFINABLE OPTIONS:
-
- DEBUG: This turns on the printing of various debug information.
-
- ENABLE_PARITY: This turns on SCSI parity checking.  With the current
- driver, all attached devices must support SCSI parity.  If none of your
- devices support parity, then you can probably get the driver to work by
- turning this option off.  I have no way of testing this, however, and it
- would appear that no one ever uses this option.
-
- FIFO_COUNT: The host adapter has an 8K cache (host adapters based on the
- 18C30 chip have a 2k cache).  When this many 512 byte blocks are filled by
- the SCSI device, an interrupt will be raised.  Therefore, this could be as
- low as 0, or as high as 16.  Note, however, that values which are too high
- or too low seem to prevent any interrupts from occurring, and thereby lock
- up the machine.  I have found that 2 is a good number, but throughput may
- be increased by changing this value to values which are close to 2.
- Please let me know if you try any different values.
- [*****Now a runtime option*****]
-
- RESELECTION: This is no longer an option, since I gave up trying to
- implement it in version 4.x of this driver.  It did not improve
- performance at all and made the driver unstable (because I never found one
- of the two race conditions which were introduced by the multiple
- outstanding command code).  The instability seems a very high price to pay
- just so that you don't have to wait for the tape to rewind.  If you want
- this feature implemented, send me patches.  I'll be happy to send a copy
- of my (broken) driver to anyone who would like to see a copy.
-
- **************************************************************************/
-
-#include <linux/module.h>
-#include <linux/init.h>
-#include <linux/interrupt.h>
-#include <linux/blkdev.h>
-#include <linux/errno.h>
-#include <linux/string.h>
-#include <linux/ioport.h>
-#include <linux/proc_fs.h>
-#include <linux/delay.h>
-#include <linux/mca.h>
-#include <linux/spinlock.h>
-#include <linux/slab.h>
-#include <scsi/scsicam.h>
-#include <linux/mca-legacy.h>
-
-#include <asm/io.h>
-
-#include "scsi.h"
-#include <scsi/scsi_host.h>
-
-#define DRIVER_VERSION "v0.2 by ZP Gu<zpg@castle.net>"
-
-/* START OF USER DEFINABLE OPTIONS */
-
-#define DEBUG            0	/* Enable debugging output */
-#define ENABLE_PARITY    1	/* Enable SCSI Parity */
-
-/* END OF USER DEFINABLE OPTIONS */
-
-#if DEBUG
-#define EVERY_ACCESS     0	/* Write a line on every scsi access */
-#define ERRORS_ONLY      1	/* Only write a line if there is an error */
-#define DEBUG_MESSAGES   1	/* Debug MESSAGE IN phase */
-#define DEBUG_ABORT      1	/* Debug abort() routine */
-#define DEBUG_RESET      1	/* Debug reset() routine */
-#define DEBUG_RACE       1	/* Debug interrupt-driven race condition */
-#else
-#define EVERY_ACCESS     0	/* LEAVE THESE ALONE--CHANGE THE ONES ABOVE */
-#define ERRORS_ONLY      0
-#define DEBUG_MESSAGES   0
-#define DEBUG_ABORT      0
-#define DEBUG_RESET      0
-#define DEBUG_RACE       0
-#endif
-
-/* Errors are reported on the line, so we don't need to report them again */
-#if EVERY_ACCESS
-#undef ERRORS_ONLY
-#define ERRORS_ONLY      0
-#endif
-
-#if ENABLE_PARITY
-#define PARITY_MASK      0x08
-#else
-#define PARITY_MASK      0x00
-#endif
-
-enum chip_type {
-	unknown = 0x00,
-	tmc1800 = 0x01,
-	tmc18c50 = 0x02,
-	tmc18c30 = 0x03,
-};
-
-enum {
-	in_arbitration = 0x02,
-	in_selection = 0x04,
-	in_other = 0x08,
-	disconnect = 0x10,
-	aborted = 0x20,
-	sent_ident = 0x40,
-};
-
-enum in_port_type {
-	Read_SCSI_Data = 0,
-	SCSI_Status = 1,
-	TMC_Status = 2,
-	FIFO_Status = 3,	/* tmc18c50/tmc18c30 only */
-	Interrupt_Cond = 4,	/* tmc18c50/tmc18c30 only */
-	LSB_ID_Code = 5,
-	MSB_ID_Code = 6,
-	Read_Loopback = 7,
-	SCSI_Data_NoACK = 8,
-	Interrupt_Status = 9,
-	Configuration1 = 10,
-	Configuration2 = 11,	/* tmc18c50/tmc18c30 only */
-	Read_FIFO = 12,
-	FIFO_Data_Count = 14
-};
-
-enum out_port_type {
-	Write_SCSI_Data = 0,
-	SCSI_Cntl = 1,
-	Interrupt_Cntl = 2,
-	SCSI_Mode_Cntl = 3,
-	TMC_Cntl = 4,
-	Memory_Cntl = 5,	/* tmc18c50/tmc18c30 only */
-	Write_Loopback = 7,
-	IO_Control = 11,	/* tmc18c30 only */
-	Write_FIFO = 12
-};
-
-struct fd_hostdata {
-	unsigned long _bios_base;
-	int _bios_major;
-	int _bios_minor;
-	volatile int _in_command;
-	Scsi_Cmnd *_current_SC;
-	enum chip_type _chip;
-	int _adapter_mask;
-	int _fifo_count;	/* Number of 512 byte blocks before INTR */
-
-	char _adapter_name[64];
-#if DEBUG_RACE
-	volatile int _in_interrupt_flag;
-#endif
-
-	int _SCSI_Mode_Cntl_port;
-	int _FIFO_Data_Count_port;
-	int _Interrupt_Cntl_port;
-	int _Interrupt_Status_port;
-	int _Interrupt_Cond_port;
-	int _Read_FIFO_port;
-	int _Read_SCSI_Data_port;
-	int _SCSI_Cntl_port;
-	int _SCSI_Data_NoACK_port;
-	int _SCSI_Status_port;
-	int _TMC_Cntl_port;
-	int _TMC_Status_port;
-	int _Write_FIFO_port;
-	int _Write_SCSI_Data_port;
-
-	int _FIFO_Size;		/* = 0x2000;  8k FIFO for
-				   pre-tmc18c30 chips */
-	/* simple stats */
-	int _Bytes_Read;
-	int _Bytes_Written;
-	int _INTR_Processed;
-};
-
-#define FD_MAX_HOSTS 3		/* enough? */
-
-#define HOSTDATA(shpnt) ((struct fd_hostdata *) shpnt->hostdata)
-#define bios_base             (HOSTDATA(shpnt)->_bios_base)
-#define bios_major            (HOSTDATA(shpnt)->_bios_major)
-#define bios_minor            (HOSTDATA(shpnt)->_bios_minor)
-#define in_command            (HOSTDATA(shpnt)->_in_command)
-#define current_SC            (HOSTDATA(shpnt)->_current_SC)
-#define chip                  (HOSTDATA(shpnt)->_chip)
-#define adapter_mask          (HOSTDATA(shpnt)->_adapter_mask)
-#define FIFO_COUNT            (HOSTDATA(shpnt)->_fifo_count)
-#define adapter_name          (HOSTDATA(shpnt)->_adapter_name)
-#if DEBUG_RACE
-#define in_interrupt_flag     (HOSTDATA(shpnt)->_in_interrupt_flag)
-#endif
-#define SCSI_Mode_Cntl_port   (HOSTDATA(shpnt)->_SCSI_Mode_Cntl_port)
-#define FIFO_Data_Count_port  (HOSTDATA(shpnt)->_FIFO_Data_Count_port)
-#define Interrupt_Cntl_port   (HOSTDATA(shpnt)->_Interrupt_Cntl_port)
-#define Interrupt_Status_port (HOSTDATA(shpnt)->_Interrupt_Status_port)
-#define Interrupt_Cond_port   (HOSTDATA(shpnt)->_Interrupt_Cond_port)
-#define Read_FIFO_port        (HOSTDATA(shpnt)->_Read_FIFO_port)
-#define Read_SCSI_Data_port   (HOSTDATA(shpnt)->_Read_SCSI_Data_port)
-#define SCSI_Cntl_port        (HOSTDATA(shpnt)->_SCSI_Cntl_port)
-#define SCSI_Data_NoACK_port  (HOSTDATA(shpnt)->_SCSI_Data_NoACK_port)
-#define SCSI_Status_port      (HOSTDATA(shpnt)->_SCSI_Status_port)
-#define TMC_Cntl_port         (HOSTDATA(shpnt)->_TMC_Cntl_port)
-#define TMC_Status_port       (HOSTDATA(shpnt)->_TMC_Status_port)
-#define Write_FIFO_port       (HOSTDATA(shpnt)->_Write_FIFO_port)
-#define Write_SCSI_Data_port  (HOSTDATA(shpnt)->_Write_SCSI_Data_port)
-#define FIFO_Size             (HOSTDATA(shpnt)->_FIFO_Size)
-#define Bytes_Read            (HOSTDATA(shpnt)->_Bytes_Read)
-#define Bytes_Written         (HOSTDATA(shpnt)->_Bytes_Written)
-#define INTR_Processed        (HOSTDATA(shpnt)->_INTR_Processed)
-
-struct fd_mcs_adapters_struct {
-	char *name;
-	int id;
-	enum chip_type fd_chip;
-	int fifo_size;
-	int fifo_count;
-};
-
-#define REPLY_ID 0x5137
-
-static struct fd_mcs_adapters_struct fd_mcs_adapters[] = {
-	{"Future Domain SCSI Adapter MCS-700(18C50)",
-	 0x60e9,
-	 tmc18c50,
-	 0x2000,
-	 4},
-	{"Future Domain SCSI Adapter MCS-600/700(TMC-1800)",
-	 0x6127,
-	 tmc1800,
-	 0x2000,
-	 4},
-	{"Reply Sound Blaster/SCSI Adapter",
-	 REPLY_ID,
-	 tmc18c30,
-	 0x800,
-	 2},
-};
-
-#define FD_BRDS ARRAY_SIZE(fd_mcs_adapters)
-
-static irqreturn_t fd_mcs_intr(int irq, void *dev_id);
-
-static unsigned long addresses[] = { 0xc8000, 0xca000, 0xce000, 0xde000 };
-static unsigned short ports[] = { 0x140, 0x150, 0x160, 0x170 };
-static unsigned short interrupts[] = { 3, 5, 10, 11, 12, 14, 15, 0 };
-
-/* host information */
-static int found = 0;
-static struct Scsi_Host *hosts[FD_MAX_HOSTS + 1] = { NULL };
-
-static int user_fifo_count = 0;
-static int user_fifo_size = 0;
-
-#ifndef MODULE
-static int __init fd_mcs_setup(char *str)
-{
-	static int done_setup = 0;
-	int ints[3];
-
-	get_options(str, 3, ints);
-	if (done_setup++ || ints[0] < 1 || ints[0] > 2 || ints[1] < 1 || ints[1] > 16) {
-		printk("fd_mcs: usage: fd_mcs=FIFO_COUNT, FIFO_SIZE\n");
-		return 0;
-	}
-
-	user_fifo_count = ints[0] >= 1 ? ints[1] : 0;
-	user_fifo_size = ints[0] >= 2 ? ints[2] : 0;
-	return 1;
-}
-
-__setup("fd_mcs=", fd_mcs_setup);
-#endif /* !MODULE */
-
-static void print_banner(struct Scsi_Host *shpnt)
-{
-	printk("scsi%d <fd_mcs>: ", shpnt->host_no);
-
-	if (bios_base) {
-		printk("BIOS at 0x%lX", bios_base);
-	} else {
-		printk("No BIOS");
-	}
-
-	printk(", HostID %d, %s Chip, IRQ %d, IO 0x%lX\n", shpnt->this_id, chip == tmc18c50 ? "TMC-18C50" : (chip == tmc18c30 ? "TMC-18C30" : (chip == tmc1800 ? "TMC-1800" : "Unknown")), shpnt->irq, shpnt->io_port);
-}
-
-
-static void do_pause(unsigned amount)
-{				/* Pause for amount*10 milliseconds */
-	do {
-		mdelay(10);
-	} while (--amount);
-}
-
-static void fd_mcs_make_bus_idle(struct Scsi_Host *shpnt)
-{
-	outb(0, SCSI_Cntl_port);
-	outb(0, SCSI_Mode_Cntl_port);
-	if (chip == tmc18c50 || chip == tmc18c30)
-		outb(0x21 | PARITY_MASK, TMC_Cntl_port);	/* Clear forced intr. */
-	else
-		outb(0x01 | PARITY_MASK, TMC_Cntl_port);
-}
-
-static int fd_mcs_detect(struct scsi_host_template * tpnt)
-{
-	int loop;
-	struct Scsi_Host *shpnt;
-
-	/* get id, port, bios, irq */
-	int slot;
-	u_char pos2, pos3, pos4;
-	int id, port, irq;
-	unsigned long bios;
-
-	/* if not MCA machine, return */
-	if (!MCA_bus)
-		return 0;
-
-	/* changeable? */
-	id = 7;
-
-	for (loop = 0; loop < FD_BRDS; loop++) {
-		slot = 0;
-		while (MCA_NOTFOUND != (slot = mca_find_adapter(fd_mcs_adapters[loop].id, slot))) {
-
-			/* if we get this far, an adapter has been detected and is
-			   enabled */
-
-			printk(KERN_INFO "scsi  <fd_mcs>: %s at slot %d\n", fd_mcs_adapters[loop].name, slot + 1);
-
-			pos2 = mca_read_stored_pos(slot, 2);
-			pos3 = mca_read_stored_pos(slot, 3);
-			pos4 = mca_read_stored_pos(slot, 4);
-
-			/* ready for next probe */
-			slot++;
-
-			if (fd_mcs_adapters[loop].id == REPLY_ID) {	/* reply card */
-				static int reply_irq[] = { 10, 11, 14, 15 };
-
-				bios = 0;	/* no bios */
-
-				if (pos2 & 0x2)
-					port = ports[pos4 & 0x3];
-				else
-					continue;
-
-				/* can't really disable it, same as irq=10 */
-				irq = reply_irq[((pos4 >> 2) & 0x1) + 2 * ((pos4 >> 4) & 0x1)];
-			} else {
-				bios = addresses[pos2 >> 6];
-				port = ports[(pos2 >> 4) & 0x03];
-				irq = interrupts[(pos2 >> 1) & 0x07];
-			}
-
-			if (irq) {
-				/* claim the slot */
-				mca_set_adapter_name(slot - 1, fd_mcs_adapters[loop].name);
-
-				/* check irq/region */
-				if (request_irq(irq, fd_mcs_intr, IRQF_SHARED, "fd_mcs", hosts)) {
-					printk(KERN_ERR "fd_mcs: interrupt is not available, skipping...\n");
-					continue;
-				}
-
-				/* request I/O region */
-				if (request_region(port, 0x10, "fd_mcs")) {
-					printk(KERN_ERR "fd_mcs: I/O region is already in use, skipping...\n");
-					continue;
-				}
-				/* register */
-				if (!(shpnt = scsi_register(tpnt, sizeof(struct fd_hostdata)))) {
-					printk(KERN_ERR "fd_mcs: scsi_register() failed\n");
-					release_region(port, 0x10);
-					free_irq(irq, hosts);
-					continue;
-				}
-
-
-				/* save name */
-				strcpy(adapter_name, fd_mcs_adapters[loop].name);
-
-				/* chip/fifo */
-				chip = fd_mcs_adapters[loop].fd_chip;
-				/* use boot time value if available */
-				FIFO_COUNT = user_fifo_count ? user_fifo_count : fd_mcs_adapters[loop].fifo_count;
-				FIFO_Size = user_fifo_size ? user_fifo_size : fd_mcs_adapters[loop].fifo_size;
-
-/* FIXME: Do we need to keep this bit of code inside NOT_USED around at all? */
-#ifdef NOT_USED
-				/* *************************************************** */
-				/* Try to toggle 32-bit mode.  This only
-				   works on an 18c30 chip.  (User reports
-				   say this works, so we should switch to
-				   it in the near future.) */
-				outb(0x80, port + IO_Control);
-				if ((inb(port + Configuration2) & 0x80) == 0x80) {
-					outb(0x00, port + IO_Control);
-					if ((inb(port + Configuration2) & 0x80) == 0x00) {
-						chip = tmc18c30;
-						FIFO_Size = 0x800;	/* 2k FIFO */
-
-						printk("FIRST: chip=%s, fifo_size=0x%x\n", (chip == tmc18c30) ? "tmc18c30" : "tmc18c50", FIFO_Size);
-					}
-				}
-
-				/* That should have worked, but appears to
-				   have problems.  Let's assume it is an
-				   18c30 if the RAM is disabled. */
-
-				if (inb(port + Configuration2) & 0x02) {
-					chip = tmc18c30;
-					FIFO_Size = 0x800;	/* 2k FIFO */
-
-					printk("SECOND: chip=%s, fifo_size=0x%x\n", (chip == tmc18c30) ? "tmc18c30" : "tmc18c50", FIFO_Size);
-				}
-				/* *************************************************** */
-#endif
-
-				/* IBM/ANSI scsi scan ordering */
-				/* Stick this back in when the scsi.c changes are there */
-				shpnt->reverse_ordering = 1;
-
-
-				/* saving info */
-				hosts[found++] = shpnt;
-
-				shpnt->this_id = id;
-				shpnt->irq = irq;
-				shpnt->io_port = port;
-				shpnt->n_io_port = 0x10;
-
-				/* save */
-				bios_base = bios;
-				adapter_mask = (1 << id);
-
-				/* save more */
-				SCSI_Mode_Cntl_port = port + SCSI_Mode_Cntl;
-				FIFO_Data_Count_port = port + FIFO_Data_Count;
-				Interrupt_Cntl_port = port + Interrupt_Cntl;
-				Interrupt_Status_port = port + Interrupt_Status;
-				Interrupt_Cond_port = port + Interrupt_Cond;
-				Read_FIFO_port = port + Read_FIFO;
-				Read_SCSI_Data_port = port + Read_SCSI_Data;
-				SCSI_Cntl_port = port + SCSI_Cntl;
-				SCSI_Data_NoACK_port = port + SCSI_Data_NoACK;
-				SCSI_Status_port = port + SCSI_Status;
-				TMC_Cntl_port = port + TMC_Cntl;
-				TMC_Status_port = port + TMC_Status;
-				Write_FIFO_port = port + Write_FIFO;
-				Write_SCSI_Data_port = port + Write_SCSI_Data;
-
-				Bytes_Read = 0;
-				Bytes_Written = 0;
-				INTR_Processed = 0;
-
-				/* say something */
-				print_banner(shpnt);
-
-				/* reset */
-				outb(1, SCSI_Cntl_port);
-				do_pause(2);
-				outb(0, SCSI_Cntl_port);
-				do_pause(115);
-				outb(0, SCSI_Mode_Cntl_port);
-				outb(PARITY_MASK, TMC_Cntl_port);
-				/* done reset */
-			}
-		}
-
-		if (found == FD_MAX_HOSTS) {
-			printk("fd_mcs: detecting reached max=%d host adapters.\n", FD_MAX_HOSTS);
-			break;
-		}
-	}
-
-	return found;
-}
-
-static const char *fd_mcs_info(struct Scsi_Host *shpnt)
-{
-	return adapter_name;
-}
-
-static int TOTAL_INTR = 0;
-
-/*
- * 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 fd_mcs_proc_info(struct Scsi_Host *shpnt, char *buffer, char **start, off_t offset, int length, int inout)
-{
-	int len = 0;
-
-	if (inout)
-		return (-ENOSYS);
-
-	*start = buffer + offset;
-
-	len += sprintf(buffer + len, "Future Domain MCS-600/700 Driver %s\n", DRIVER_VERSION);
-	len += sprintf(buffer + len, "HOST #%d: %s\n", shpnt->host_no, adapter_name);
-	len += sprintf(buffer + len, "FIFO Size=0x%x, FIFO Count=%d\n", FIFO_Size, FIFO_COUNT);
-	len += sprintf(buffer + len, "DriverCalls=%d, Interrupts=%d, BytesRead=%d, BytesWrite=%d\n\n", TOTAL_INTR, INTR_Processed, Bytes_Read, Bytes_Written);
-
-	if ((len -= offset) <= 0)
-		return 0;
-	if (len > length)
-		len = length;
-	return len;
-}
-
-static int fd_mcs_select(struct Scsi_Host *shpnt, int target)
-{
-	int status;
-	unsigned long timeout;
-
-	outb(0x82, SCSI_Cntl_port);	/* Bus Enable + Select */
-	outb(adapter_mask | (1 << target), SCSI_Data_NoACK_port);
-
-	/* Stop arbitration and enable parity */
-	outb(PARITY_MASK, TMC_Cntl_port);
-
-	timeout = 350;		/* 350mS -- because of timeouts
-				   (was 250mS) */
-
-	do {
-		status = inb(SCSI_Status_port);	/* Read adapter status */
-		if (status & 1) {	/* Busy asserted */
-			/* Enable SCSI Bus (on error, should make bus idle with 0) */
-			outb(0x80, SCSI_Cntl_port);
-			return 0;
-		}
-		udelay(1000);	/* wait one msec */
-	} while (--timeout);
-
-	/* Make bus idle */
-	fd_mcs_make_bus_idle(shpnt);
-#if EVERY_ACCESS
-	if (!target)
-		printk("Selection failed\n");
-#endif
-#if ERRORS_ONLY
-	if (!target) {
-		static int flag = 0;
-
-		if (!flag)	/* Skip first failure for all chips. */
-			++flag;
-		else
-			printk("fd_mcs: Selection failed\n");
-	}
-#endif
-	return 1;
-}
-
-static void my_done(struct Scsi_Host *shpnt, int error)
-{
-	if (in_command) {
-		in_command = 0;
-		outb(0x00, Interrupt_Cntl_port);
-		fd_mcs_make_bus_idle(shpnt);
-		current_SC->result = error;
-		current_SC->scsi_done(current_SC);
-	} else {
-		panic("fd_mcs: my_done() called outside of command\n");
-	}
-#if DEBUG_RACE
-	in_interrupt_flag = 0;
-#endif
-}
-
-/* only my_done needs to be protected  */
-static irqreturn_t fd_mcs_intr(int irq, void *dev_id)
-{
-	unsigned long flags;
-	int status;
-	int done = 0;
-	unsigned data_count, tmp_count;
-
-	int i = 0;
-	struct Scsi_Host *shpnt;
-
-	TOTAL_INTR++;
-
-	/* search for one adapter-response on shared interrupt */
-	while ((shpnt = hosts[i++])) {
-		if ((inb(TMC_Status_port)) & 1)
-			break;
-	}
-
-	/* return if some other device on this IRQ caused the interrupt */
-	if (!shpnt) {
-		return IRQ_NONE;
-	}
-
-	INTR_Processed++;
-
-	outb(0x00, Interrupt_Cntl_port);
-
-	/* Abort calls my_done, so we do nothing here. */
-	if (current_SC->SCp.phase & aborted) {
-#if DEBUG_ABORT
-		printk("Interrupt after abort, ignoring\n");
-#endif
-		/* return IRQ_HANDLED; */
-	}
-#if DEBUG_RACE
-	++in_interrupt_flag;
-#endif
-
-	if (current_SC->SCp.phase & in_arbitration) {
-		status = inb(TMC_Status_port);	/* Read adapter status */
-		if (!(status & 0x02)) {
-#if EVERY_ACCESS
-			printk(" AFAIL ");
-#endif
-			spin_lock_irqsave(shpnt->host_lock, flags);
-			my_done(shpnt, DID_BUS_BUSY << 16);
-			spin_unlock_irqrestore(shpnt->host_lock, flags);
-			return IRQ_HANDLED;
-		}
-		current_SC->SCp.phase = in_selection;
-
-		outb(0x40 | FIFO_COUNT, Interrupt_Cntl_port);
-
-		outb(0x82, SCSI_Cntl_port);	/* Bus Enable + Select */
-		outb(adapter_mask | (1 << scmd_id(current_SC)), SCSI_Data_NoACK_port);
-
-		/* Stop arbitration and enable parity */
-		outb(0x10 | PARITY_MASK, TMC_Cntl_port);
-#if DEBUG_RACE
-		in_interrupt_flag = 0;
-#endif
-		return IRQ_HANDLED;
-	} else if (current_SC->SCp.phase & in_selection) {
-		status = inb(SCSI_Status_port);
-		if (!(status & 0x01)) {
-			/* Try again, for slow devices */
-			if (fd_mcs_select(shpnt, scmd_id(current_SC))) {
-#if EVERY_ACCESS
-				printk(" SFAIL ");
-#endif
-				spin_lock_irqsave(shpnt->host_lock, flags);
-				my_done(shpnt, DID_NO_CONNECT << 16);
-				spin_unlock_irqrestore(shpnt->host_lock, flags);
-				return IRQ_HANDLED;
-			} else {
-#if EVERY_ACCESS
-				printk(" AltSel ");
-#endif
-				/* Stop arbitration and enable parity */
-				outb(0x10 | PARITY_MASK, TMC_Cntl_port);
-			}
-		}
-		current_SC->SCp.phase = in_other;
-		outb(0x90 | FIFO_COUNT, Interrupt_Cntl_port);
-		outb(0x80, SCSI_Cntl_port);
-#if DEBUG_RACE
-		in_interrupt_flag = 0;
-#endif
-		return IRQ_HANDLED;
-	}
-
-	/* current_SC->SCp.phase == in_other: this is the body of the routine */
-
-	status = inb(SCSI_Status_port);
-
-	if (status & 0x10) {	/* REQ */
-
-		switch (status & 0x0e) {
-
-		case 0x08:	/* COMMAND OUT */
-			outb(current_SC->cmnd[current_SC->SCp.sent_command++], Write_SCSI_Data_port);
-#if EVERY_ACCESS
-			printk("CMD = %x,", current_SC->cmnd[current_SC->SCp.sent_command - 1]);
-#endif
-			break;
-		case 0x00:	/* DATA OUT -- tmc18c50/tmc18c30 only */
-			if (chip != tmc1800 && !current_SC->SCp.have_data_in) {
-				current_SC->SCp.have_data_in = -1;
-				outb(0xd0 | PARITY_MASK, TMC_Cntl_port);
-			}
-			break;
-		case 0x04:	/* DATA IN -- tmc18c50/tmc18c30 only */
-			if (chip != tmc1800 && !current_SC->SCp.have_data_in) {
-				current_SC->SCp.have_data_in = 1;
-				outb(0x90 | PARITY_MASK, TMC_Cntl_port);
-			}
-			break;
-		case 0x0c:	/* STATUS IN */
-			current_SC->SCp.Status = inb(Read_SCSI_Data_port);
-#if EVERY_ACCESS
-			printk("Status = %x, ", current_SC->SCp.Status);
-#endif
-#if ERRORS_ONLY
-			if (current_SC->SCp.Status && current_SC->SCp.Status != 2 && current_SC->SCp.Status != 8) {
-				printk("ERROR fd_mcs: target = %d, command = %x, status = %x\n", current_SC->device->id, current_SC->cmnd[0], current_SC->SCp.Status);
-			}
-#endif
-			break;
-		case 0x0a:	/* MESSAGE OUT */
-			outb(MESSAGE_REJECT, Write_SCSI_Data_port);	/* Reject */
-			break;
-		case 0x0e:	/* MESSAGE IN */
-			current_SC->SCp.Message = inb(Read_SCSI_Data_port);
-#if EVERY_ACCESS
-			printk("Message = %x, ", current_SC->SCp.Message);
-#endif
-			if (!current_SC->SCp.Message)
-				++done;
-#if DEBUG_MESSAGES || EVERY_ACCESS
-			if (current_SC->SCp.Message) {
-				printk("fd_mcs: message = %x\n", current_SC->SCp.Message);
-			}
-#endif
-			break;
-		}
-	}
-
-	if (chip == tmc1800 && !current_SC->SCp.have_data_in && (current_SC->SCp.sent_command >= current_SC->cmd_len)) {
-		/* We have to get the FIFO direction
-		   correct, so I've made a table based
-		   on the SCSI Standard of which commands
-		   appear to require a DATA OUT phase.
-		 */
-		/*
-		   p. 94: Command for all device types
-		   CHANGE DEFINITION            40 DATA OUT
-		   COMPARE                      39 DATA OUT
-		   COPY                         18 DATA OUT
-		   COPY AND VERIFY              3a DATA OUT
-		   INQUIRY                      12 
-		   LOG SELECT                   4c DATA OUT
-		   LOG SENSE                    4d
-		   MODE SELECT (6)              15 DATA OUT
-		   MODE SELECT (10)             55 DATA OUT
-		   MODE SENSE (6)               1a
-		   MODE SENSE (10)              5a
-		   READ BUFFER                  3c
-		   RECEIVE DIAGNOSTIC RESULTS   1c
-		   REQUEST SENSE                03
-		   SEND DIAGNOSTIC              1d DATA OUT
-		   TEST UNIT READY              00
-		   WRITE BUFFER                 3b DATA OUT
-
-		   p.178: Commands for direct-access devices (not listed on p. 94)
-		   FORMAT UNIT                  04 DATA OUT
-		   LOCK-UNLOCK CACHE            36
-		   PRE-FETCH                    34
-		   PREVENT-ALLOW MEDIUM REMOVAL 1e
-		   READ (6)/RECEIVE             08
-		   READ (10)                    3c
-		   READ CAPACITY                25
-		   READ DEFECT DATA (10)        37
-		   READ LONG                    3e
-		   REASSIGN BLOCKS              07 DATA OUT
-		   RELEASE                      17
-		   RESERVE                      16 DATA OUT
-		   REZERO UNIT/REWIND           01
-		   SEARCH DATA EQUAL (10)       31 DATA OUT
-		   SEARCH DATA HIGH (10)        30 DATA OUT
-		   SEARCH DATA LOW (10)         32 DATA OUT
-		   SEEK (6)                     0b
-		   SEEK (10)                    2b
-		   SET LIMITS (10)              33
-		   START STOP UNIT              1b
-		   SYNCHRONIZE CACHE            35
-		   VERIFY (10)                  2f
-		   WRITE (6)/PRINT/SEND         0a DATA OUT
-		   WRITE (10)/SEND              2a DATA OUT
-		   WRITE AND VERIFY (10)        2e DATA OUT
-		   WRITE LONG                   3f DATA OUT
-		   WRITE SAME                   41 DATA OUT ?
-
-		   p. 261: Commands for sequential-access devices (not previously listed)
-		   ERASE                        19
-		   LOAD UNLOAD                  1b
-		   LOCATE                       2b
-		   READ BLOCK LIMITS            05
-		   READ POSITION                34
-		   READ REVERSE                 0f
-		   RECOVER BUFFERED DATA        14
-		   SPACE                        11
-		   WRITE FILEMARKS              10 ?
-
-		   p. 298: Commands for printer devices (not previously listed)
-		   ****** NOT SUPPORTED BY THIS DRIVER, since 0b is SEEK (6) *****
-		   SLEW AND PRINT               0b DATA OUT  -- same as seek
-		   STOP PRINT                   1b
-		   SYNCHRONIZE BUFFER           10
-
-		   p. 315: Commands for processor devices (not previously listed)
-
-		   p. 321: Commands for write-once devices (not previously listed)
-		   MEDIUM SCAN                  38
-		   READ (12)                    a8
-		   SEARCH DATA EQUAL (12)       b1 DATA OUT
-		   SEARCH DATA HIGH (12)        b0 DATA OUT
-		   SEARCH DATA LOW (12)         b2 DATA OUT
-		   SET LIMITS (12)              b3
-		   VERIFY (12)                  af
-		   WRITE (12)                   aa DATA OUT
-		   WRITE AND VERIFY (12)        ae DATA OUT
-
-		   p. 332: Commands for CD-ROM devices (not previously listed)
-		   PAUSE/RESUME                 4b
-		   PLAY AUDIO (10)              45
-		   PLAY AUDIO (12)              a5
-		   PLAY AUDIO MSF               47
-		   PLAY TRACK RELATIVE (10)     49
-		   PLAY TRACK RELATIVE (12)     a9
-		   READ HEADER                  44
-		   READ SUB-CHANNEL             42
-		   READ TOC                     43
-
-		   p. 370: Commands for scanner devices (not previously listed)
-		   GET DATA BUFFER STATUS       34
-		   GET WINDOW                   25
-		   OBJECT POSITION              31
-		   SCAN                         1b
-		   SET WINDOW                   24 DATA OUT
-
-		   p. 391: Commands for optical memory devices (not listed)
-		   ERASE (10)                   2c
-		   ERASE (12)                   ac
-		   MEDIUM SCAN                  38 DATA OUT
-		   READ DEFECT DATA (12)        b7
-		   READ GENERATION              29
-		   READ UPDATED BLOCK           2d
-		   UPDATE BLOCK                 3d DATA OUT
-
-		   p. 419: Commands for medium changer devices (not listed)
-		   EXCHANGE MEDIUM              46
-		   INITIALIZE ELEMENT STATUS    07
-		   MOVE MEDIUM                  a5
-		   POSITION TO ELEMENT          2b
-		   READ ELEMENT STATUS          b8
-		   REQUEST VOL. ELEMENT ADDRESS b5
-		   SEND VOLUME TAG              b6 DATA OUT
-
-		   p. 454: Commands for communications devices (not listed previously)
-		   GET MESSAGE (6)              08
-		   GET MESSAGE (10)             28
-		   GET MESSAGE (12)             a8
-		 */
-
-		switch (current_SC->cmnd[0]) {
-		case CHANGE_DEFINITION:
-		case COMPARE:
-		case COPY:
-		case COPY_VERIFY:
-		case LOG_SELECT:
-		case MODE_SELECT:
-		case MODE_SELECT_10:
-		case SEND_DIAGNOSTIC:
-		case WRITE_BUFFER:
-
-		case FORMAT_UNIT:
-		case REASSIGN_BLOCKS:
-		case RESERVE:
-		case SEARCH_EQUAL:
-		case SEARCH_HIGH:
-		case SEARCH_LOW:
-		case WRITE_6:
-		case WRITE_10:
-		case WRITE_VERIFY:
-		case 0x3f:
-		case 0x41:
-
-		case 0xb1:
-		case 0xb0:
-		case 0xb2:
-		case 0xaa:
-		case 0xae:
-
-		case 0x24:
-
-		case 0x38:
-		case 0x3d:
-
-		case 0xb6:
-
-		case 0xea:	/* alternate number for WRITE LONG */
-
-			current_SC->SCp.have_data_in = -1;
-			outb(0xd0 | PARITY_MASK, TMC_Cntl_port);
-			break;
-
-		case 0x00:
-		default:
-
-			current_SC->SCp.have_data_in = 1;
-			outb(0x90 | PARITY_MASK, TMC_Cntl_port);
-			break;
-		}
-	}
-
-	if (current_SC->SCp.have_data_in == -1) {	/* DATA OUT */
-		while ((data_count = FIFO_Size - inw(FIFO_Data_Count_port)) > 512) {
-#if EVERY_ACCESS
-			printk("DC=%d, ", data_count);
-#endif
-			if (data_count > current_SC->SCp.this_residual)
-				data_count = current_SC->SCp.this_residual;
-			if (data_count > 0) {
-#if EVERY_ACCESS
-				printk("%d OUT, ", data_count);
-#endif
-				if (data_count == 1) {
-					Bytes_Written++;
-
-					outb(*current_SC->SCp.ptr++, Write_FIFO_port);
-					--current_SC->SCp.this_residual;
-				} else {
-					data_count >>= 1;
-					tmp_count = data_count << 1;
-					outsw(Write_FIFO_port, current_SC->SCp.ptr, data_count);
-					current_SC->SCp.ptr += tmp_count;
-					Bytes_Written += tmp_count;
-					current_SC->SCp.this_residual -= tmp_count;
-				}
-			}
-			if (!current_SC->SCp.this_residual) {
-				if (current_SC->SCp.buffers_residual) {
-					--current_SC->SCp.buffers_residual;
-					++current_SC->SCp.buffer;
-					current_SC->SCp.ptr = sg_virt(current_SC->SCp.buffer);
-					current_SC->SCp.this_residual = current_SC->SCp.buffer->length;
-				} else
-					break;
-			}
-		}
-	} else if (current_SC->SCp.have_data_in == 1) {	/* DATA IN */
-		while ((data_count = inw(FIFO_Data_Count_port)) > 0) {
-#if EVERY_ACCESS
-			printk("DC=%d, ", data_count);
-#endif
-			if (data_count > current_SC->SCp.this_residual)
-				data_count = current_SC->SCp.this_residual;
-			if (data_count) {
-#if EVERY_ACCESS
-				printk("%d IN, ", data_count);
-#endif
-				if (data_count == 1) {
-					Bytes_Read++;
-					*current_SC->SCp.ptr++ = inb(Read_FIFO_port);
-					--current_SC->SCp.this_residual;
-				} else {
-					data_count >>= 1;	/* Number of words */
-					tmp_count = data_count << 1;
-					insw(Read_FIFO_port, current_SC->SCp.ptr, data_count);
-					current_SC->SCp.ptr += tmp_count;
-					Bytes_Read += tmp_count;
-					current_SC->SCp.this_residual -= tmp_count;
-				}
-			}
-			if (!current_SC->SCp.this_residual && current_SC->SCp.buffers_residual) {
-				--current_SC->SCp.buffers_residual;
-				++current_SC->SCp.buffer;
-				current_SC->SCp.ptr = sg_virt(current_SC->SCp.buffer);
-				current_SC->SCp.this_residual = current_SC->SCp.buffer->length;
-			}
-		}
-	}
-
-	if (done) {
-#if EVERY_ACCESS
-		printk(" ** IN DONE %d ** ", current_SC->SCp.have_data_in);
-#endif
-
-#if EVERY_ACCESS
-		printk("BEFORE MY_DONE. . .");
-#endif
-		spin_lock_irqsave(shpnt->host_lock, flags);
-		my_done(shpnt, (current_SC->SCp.Status & 0xff)
-			| ((current_SC->SCp.Message & 0xff) << 8) | (DID_OK << 16));
-		spin_unlock_irqrestore(shpnt->host_lock, flags);
-#if EVERY_ACCESS
-		printk("RETURNING.\n");
-#endif
-
-	} else {
-		if (current_SC->SCp.phase & disconnect) {
-			outb(0xd0 | FIFO_COUNT, Interrupt_Cntl_port);
-			outb(0x00, SCSI_Cntl_port);
-		} else {
-			outb(0x90 | FIFO_COUNT, Interrupt_Cntl_port);
-		}
-	}
-#if DEBUG_RACE
-	in_interrupt_flag = 0;
-#endif
-	return IRQ_HANDLED;
-}
-
-static int fd_mcs_release(struct Scsi_Host *shpnt)
-{
-	int i, this_host, irq_usage;
-
-	release_region(shpnt->io_port, shpnt->n_io_port);
-
-	this_host = -1;
-	irq_usage = 0;
-	for (i = 0; i < found; i++) {
-		if (shpnt == hosts[i])
-			this_host = i;
-		if (shpnt->irq == hosts[i]->irq)
-			irq_usage++;
-	}
-
-	/* only for the last one */
-	if (1 == irq_usage)
-		free_irq(shpnt->irq, hosts);
-
-	found--;
-
-	for (i = this_host; i < found; i++)
-		hosts[i] = hosts[i + 1];
-
-	hosts[found] = NULL;
-
-	return 0;
-}
-
-static int fd_mcs_queue_lck(Scsi_Cmnd * SCpnt, void (*done) (Scsi_Cmnd *))
-{
-	struct Scsi_Host *shpnt = SCpnt->device->host;
-
-	if (in_command) {
-		panic("fd_mcs: fd_mcs_queue() NOT REENTRANT!\n");
-	}
-#if EVERY_ACCESS
-	printk("queue: target = %d cmnd = 0x%02x pieces = %d size = %u\n",
-		SCpnt->target, *(unsigned char *) SCpnt->cmnd,
-		scsi_sg_count(SCpnt), scsi_bufflen(SCpnt));
-#endif
-
-	fd_mcs_make_bus_idle(shpnt);
-
-	SCpnt->scsi_done = done;	/* Save this for the done function */
-	current_SC = SCpnt;
-
-	/* Initialize static data */
-
-	if (scsi_bufflen(current_SC)) {
-		current_SC->SCp.buffer = scsi_sglist(current_SC);
-		current_SC->SCp.ptr = sg_virt(current_SC->SCp.buffer);
-		current_SC->SCp.this_residual = current_SC->SCp.buffer->length;
-		current_SC->SCp.buffers_residual = scsi_sg_count(current_SC) - 1;
-	} else {
-		current_SC->SCp.ptr = NULL;
-		current_SC->SCp.this_residual = 0;
-		current_SC->SCp.buffer = NULL;
-		current_SC->SCp.buffers_residual = 0;
-	}
-
-
-	current_SC->SCp.Status = 0;
-	current_SC->SCp.Message = 0;
-	current_SC->SCp.have_data_in = 0;
-	current_SC->SCp.sent_command = 0;
-	current_SC->SCp.phase = in_arbitration;
-
-	/* Start arbitration */
-	outb(0x00, Interrupt_Cntl_port);
-	outb(0x00, SCSI_Cntl_port);	/* Disable data drivers */
-	outb(adapter_mask, SCSI_Data_NoACK_port);	/* Set our id bit */
-	in_command = 1;
-	outb(0x20, Interrupt_Cntl_port);
-	outb(0x14 | PARITY_MASK, TMC_Cntl_port);	/* Start arbitration */
-
-	return 0;
-}
-
-static DEF_SCSI_QCMD(fd_mcs_queue)
-
-#if DEBUG_ABORT || DEBUG_RESET
-static void fd_mcs_print_info(Scsi_Cmnd * SCpnt)
-{
-	unsigned int imr;
-	unsigned int irr;
-	unsigned int isr;
-	struct Scsi_Host *shpnt = SCpnt->host;
-
-	if (!SCpnt || !SCpnt->host) {
-		printk("fd_mcs: cannot provide detailed information\n");
-	}
-
-	printk("%s\n", fd_mcs_info(SCpnt->host));
-	print_banner(SCpnt->host);
-	switch (SCpnt->SCp.phase) {
-	case in_arbitration:
-		printk("arbitration ");
-		break;
-	case in_selection:
-		printk("selection ");
-		break;
-	case in_other:
-		printk("other ");
-		break;
-	default:
-		printk("unknown ");
-		break;
-	}
-
-	printk("(%d), target = %d cmnd = 0x%02x pieces = %d size = %u\n",
-		SCpnt->SCp.phase, SCpnt->device->id, *(unsigned char *) SCpnt->cmnd,
-		scsi_sg_count(SCpnt), scsi_bufflen(SCpnt));
-	printk("sent_command = %d, have_data_in = %d, timeout = %d\n", SCpnt->SCp.sent_command, SCpnt->SCp.have_data_in, SCpnt->timeout);
-#if DEBUG_RACE
-	printk("in_interrupt_flag = %d\n", in_interrupt_flag);
-#endif
-
-	imr = (inb(0x0a1) << 8) + inb(0x21);
-	outb(0x0a, 0xa0);
-	irr = inb(0xa0) << 8;
-	outb(0x0a, 0x20);
-	irr += inb(0x20);
-	outb(0x0b, 0xa0);
-	isr = inb(0xa0) << 8;
-	outb(0x0b, 0x20);
-	isr += inb(0x20);
-
-	/* Print out interesting information */
-	printk("IMR = 0x%04x", imr);
-	if (imr & (1 << shpnt->irq))
-		printk(" (masked)");
-	printk(", IRR = 0x%04x, ISR = 0x%04x\n", irr, isr);
-
-	printk("SCSI Status      = 0x%02x\n", inb(SCSI_Status_port));
-	printk("TMC Status       = 0x%02x", inb(TMC_Status_port));
-	if (inb(TMC_Status_port) & 1)
-		printk(" (interrupt)");
-	printk("\n");
-	printk("Interrupt Status = 0x%02x", inb(Interrupt_Status_port));
-	if (inb(Interrupt_Status_port) & 0x08)
-		printk(" (enabled)");
-	printk("\n");
-	if (chip == tmc18c50 || chip == tmc18c30) {
-		printk("FIFO Status      = 0x%02x\n", inb(shpnt->io_port + FIFO_Status));
-		printk("Int. Condition   = 0x%02x\n", inb(shpnt->io_port + Interrupt_Cond));
-	}
-	printk("Configuration 1  = 0x%02x\n", inb(shpnt->io_port + Configuration1));
-	if (chip == tmc18c50 || chip == tmc18c30)
-		printk("Configuration 2  = 0x%02x\n", inb(shpnt->io_port + Configuration2));
-}
-#endif
-
-static int fd_mcs_abort(Scsi_Cmnd * SCpnt)
-{
-	struct Scsi_Host *shpnt = SCpnt->device->host;
-
-	unsigned long flags;
-#if EVERY_ACCESS || ERRORS_ONLY || DEBUG_ABORT
-	printk("fd_mcs: abort ");
-#endif
-
-	spin_lock_irqsave(shpnt->host_lock, flags);
-	if (!in_command) {
-#if EVERY_ACCESS || ERRORS_ONLY
-		printk(" (not in command)\n");
-#endif
-		spin_unlock_irqrestore(shpnt->host_lock, flags);
-		return FAILED;
-	} else
-		printk("\n");
-
-#if DEBUG_ABORT
-	fd_mcs_print_info(SCpnt);
-#endif
-
-	fd_mcs_make_bus_idle(shpnt);
-
-	current_SC->SCp.phase |= aborted;
-
-	current_SC->result = DID_ABORT << 16;
-
-	/* Aborts are not done well. . . */
-	my_done(shpnt, DID_ABORT << 16);
-
-	spin_unlock_irqrestore(shpnt->host_lock, flags);
-	return SUCCESS;
-}
-
-static int fd_mcs_bus_reset(Scsi_Cmnd * SCpnt) {
-	struct Scsi_Host *shpnt = SCpnt->device->host;
-	unsigned long flags;
-
-#if DEBUG_RESET
-	static int called_once = 0;
-#endif
-
-#if ERRORS_ONLY
-	if (SCpnt)
-		printk("fd_mcs: SCSI Bus Reset\n");
-#endif
-
-#if DEBUG_RESET
-	if (called_once)
-		fd_mcs_print_info(current_SC);
-	called_once = 1;
-#endif
-
-	spin_lock_irqsave(shpnt->host_lock, flags);
-
-	outb(1, SCSI_Cntl_port);
-	do_pause(2);
-	outb(0, SCSI_Cntl_port);
-	do_pause(115);
-	outb(0, SCSI_Mode_Cntl_port);
-	outb(PARITY_MASK, TMC_Cntl_port);
-
-	spin_unlock_irqrestore(shpnt->host_lock, flags);
-
-	/* Unless this is the very first call (i.e., SCPnt == NULL), everything
-	   is probably hosed at this point.  We will, however, try to keep
-	   things going by informing the high-level code that we need help. */
-		return SUCCESS;
-}
-
-#include <scsi/scsi_ioctl.h>
-
-static int fd_mcs_biosparam(struct scsi_device * disk, struct block_device *bdev,
-			    sector_t capacity, int *info_array) 
-{
-	unsigned char *p = scsi_bios_ptable(bdev);
-	int size = capacity;
-
-	/* BIOS >= 3.4 for MCA cards */
-	/* This algorithm was provided by Future Domain (much thanks!). */
-
-	if (p && p[65] == 0xaa && p[64] == 0x55	/* Partition table valid */
-	    && p[4]) {	/* Partition type */
-		/* The partition table layout is as follows:
-
-		   Start: 0x1b3h
-		   Offset: 0 = partition status
-		   1 = starting head
-		   2 = starting sector and cylinder (word, encoded)
-		   4 = partition type
-		   5 = ending head
-		   6 = ending sector and cylinder (word, encoded)
-		   8 = starting absolute sector (double word)
-		   c = number of sectors (double word)
-		   Signature: 0x1fe = 0x55aa
-
-		   So, this algorithm assumes:
-		   1) the first partition table is in use,
-		   2) the data in the first entry is correct, and
-		   3) partitions never divide cylinders
-
-		   Note that (1) may be FALSE for NetBSD (and other BSD flavors),
-		   as well as for Linux.  Note also, that Linux doesn't pay any
-		   attention to the fields that are used by this algorithm -- it
-		   only uses the absolute sector data.  Recent versions of Linux's
-		   fdisk(1) will fill this data in correctly, and forthcoming
-		   versions will check for consistency.
-
-		   Checking for a non-zero partition type is not part of the
-		   Future Domain algorithm, but it seemed to be a reasonable thing
-		   to do, especially in the Linux and BSD worlds. */
-
-		info_array[0] = p[5] + 1;	/* heads */
-		info_array[1] = p[6] & 0x3f;	/* sectors */
-	} else {
-		/* Note that this new method guarantees that there will always be
-		   less than 1024 cylinders on a platter.  This is good for drives
-		   up to approximately 7.85GB (where 1GB = 1024 * 1024 kB). */
-		if ((unsigned int) size >= 0x7e0000U) 
-		{
-			info_array[0] = 0xff;	/* heads   = 255 */
-			info_array[1] = 0x3f;	/* sectors =  63 */
-		} else if ((unsigned int) size >= 0x200000U) {
-			info_array[0] = 0x80;	/* heads   = 128 */
-			info_array[1] = 0x3f;	/* sectors =  63 */
-		} else {
-			info_array[0] = 0x40;	/* heads   =  64 */
-			info_array[1] = 0x20;	/* sectors =  32 */
-		}
-	}
-	/* For both methods, compute the cylinders */
-	info_array[2] = (unsigned int) size / (info_array[0] * info_array[1]);
-	kfree(p);
-	return 0;
-}
-
-static struct scsi_host_template driver_template = {
-	.proc_name			= "fd_mcs",
-	.proc_info			= fd_mcs_proc_info,
-	.detect				= fd_mcs_detect,
-	.release			= fd_mcs_release,
-	.info				= fd_mcs_info,
-	.queuecommand   		= fd_mcs_queue, 
-	.eh_abort_handler		= fd_mcs_abort,
-	.eh_bus_reset_handler		= fd_mcs_bus_reset,
-	.bios_param     		= fd_mcs_biosparam,
-	.can_queue      		= 1,
-	.this_id        		= 7,
-	.sg_tablesize   		= 64,
-	.cmd_per_lun    		= 1,
-	.use_clustering 		= DISABLE_CLUSTERING,
-};
-#include "scsi_module.c"
-
-MODULE_LICENSE("GPL");
diff --git a/drivers/scsi/hosts.c b/drivers/scsi/hosts.c
index 351dc0b86fa..a3a056a9db6 100644
--- a/drivers/scsi/hosts.c
+++ b/drivers/scsi/hosts.c
@@ -218,6 +218,9 @@ int scsi_add_host_with_dma(struct Scsi_Host *shost, struct device *dev,
 
 	if (!shost->shost_gendev.parent)
 		shost->shost_gendev.parent = dev ? dev : &platform_bus;
+	if (!dma_dev)
+		dma_dev = shost->shost_gendev.parent;
+
 	shost->dma_dev = dma_dev;
 
 	error = device_add(&shost->shost_gendev);
diff --git a/drivers/scsi/hpsa.c b/drivers/scsi/hpsa.c
index 500e20dd56e..796482badf1 100644
--- a/drivers/scsi/hpsa.c
+++ b/drivers/scsi/hpsa.c
@@ -159,6 +159,7 @@ static int hpsa_change_queue_depth(struct scsi_device *sdev,
 	int qdepth, int reason);
 
 static int hpsa_eh_device_reset_handler(struct scsi_cmnd *scsicmd);
+static int hpsa_eh_abort_handler(struct scsi_cmnd *scsicmd);
 static int hpsa_slave_alloc(struct scsi_device *sdev);
 static void hpsa_slave_destroy(struct scsi_device *sdev);
 
@@ -171,7 +172,7 @@ static void check_ioctl_unit_attention(struct ctlr_info *h,
 static void calc_bucket_map(int *bucket, int num_buckets,
 	int nsgs, int *bucket_map);
 static __devinit void hpsa_put_ctlr_into_performant_mode(struct ctlr_info *h);
-static inline u32 next_command(struct ctlr_info *h);
+static inline u32 next_command(struct ctlr_info *h, u8 q);
 static int __devinit hpsa_find_cfg_addrs(struct pci_dev *pdev,
 	void __iomem *vaddr, u32 *cfg_base_addr, u64 *cfg_base_addr_index,
 	u64 *cfg_offset);
@@ -180,6 +181,7 @@ static int __devinit hpsa_pci_find_memory_BAR(struct pci_dev *pdev,
 static int __devinit hpsa_lookup_board_id(struct pci_dev *pdev, u32 *board_id);
 static int __devinit hpsa_wait_for_board_state(struct pci_dev *pdev,
 	void __iomem *vaddr, int wait_for_ready);
+static inline void finish_cmd(struct CommandList *c);
 #define BOARD_NOT_READY 0
 #define BOARD_READY 1
 
@@ -234,6 +236,16 @@ static int check_for_unit_attention(struct ctlr_info *h,
 	return 1;
 }
 
+static int check_for_busy(struct ctlr_info *h, struct CommandList *c)
+{
+	if (c->err_info->CommandStatus != CMD_TARGET_STATUS ||
+		(c->err_info->ScsiStatus != SAM_STAT_BUSY &&
+		 c->err_info->ScsiStatus != SAM_STAT_TASK_SET_FULL))
+		return 0;
+	dev_warn(&h->pdev->dev, HPSA "device busy");
+	return 1;
+}
+
 static ssize_t host_store_rescan(struct device *dev,
 				 struct device_attribute *attr,
 				 const char *buf, size_t count)
@@ -368,7 +380,7 @@ static inline int is_logical_dev_addr_mode(unsigned char scsi3addr[])
 }
 
 static const char *raid_label[] = { "0", "4", "1(1+0)", "5", "5+1", "ADG",
-	"UNKNOWN"
+	"1(ADM)", "UNKNOWN"
 };
 #define RAID_UNKNOWN (ARRAY_SIZE(raid_label) - 1)
 
@@ -497,6 +509,7 @@ static struct scsi_host_template hpsa_driver_template = {
 	.change_queue_depth	= hpsa_change_queue_depth,
 	.this_id		= -1,
 	.use_clustering		= ENABLE_CLUSTERING,
+	.eh_abort_handler	= hpsa_eh_abort_handler,
 	.eh_device_reset_handler = hpsa_eh_device_reset_handler,
 	.ioctl			= hpsa_ioctl,
 	.slave_alloc		= hpsa_slave_alloc,
@@ -516,24 +529,28 @@ static inline void addQ(struct list_head *list, struct CommandList *c)
 	list_add_tail(&c->list, list);
 }
 
-static inline u32 next_command(struct ctlr_info *h)
+static inline u32 next_command(struct ctlr_info *h, u8 q)
 {
 	u32 a;
+	struct reply_pool *rq = &h->reply_queue[q];
+	unsigned long flags;
 
 	if (unlikely(!(h->transMethod & CFGTBL_Trans_Performant)))
-		return h->access.command_completed(h);
+		return h->access.command_completed(h, q);
 
-	if ((*(h->reply_pool_head) & 1) == (h->reply_pool_wraparound)) {
-		a = *(h->reply_pool_head); /* Next cmd in ring buffer */
-		(h->reply_pool_head)++;
+	if ((rq->head[rq->current_entry] & 1) == rq->wraparound) {
+		a = rq->head[rq->current_entry];
+		rq->current_entry++;
+		spin_lock_irqsave(&h->lock, flags);
 		h->commands_outstanding--;
+		spin_unlock_irqrestore(&h->lock, flags);
 	} else {
 		a = FIFO_EMPTY;
 	}
 	/* Check for wraparound */
-	if (h->reply_pool_head == (h->reply_pool + h->max_commands)) {
-		h->reply_pool_head = h->reply_pool;
-		h->reply_pool_wraparound ^= 1;
+	if (rq->current_entry == h->max_commands) {
+		rq->current_entry = 0;
+		rq->wraparound ^= 1;
 	}
 	return a;
 }
@@ -544,8 +561,41 @@ static inline u32 next_command(struct ctlr_info *h)
  */
 static void set_performant_mode(struct ctlr_info *h, struct CommandList *c)
 {
-	if (likely(h->transMethod & CFGTBL_Trans_Performant))
+	if (likely(h->transMethod & CFGTBL_Trans_Performant)) {
 		c->busaddr |= 1 | (h->blockFetchTable[c->Header.SGList] << 1);
+		if (likely(h->msix_vector))
+			c->Header.ReplyQueue =
+				smp_processor_id() % h->nreply_queues;
+	}
+}
+
+static int is_firmware_flash_cmd(u8 *cdb)
+{
+	return cdb[0] == BMIC_WRITE && cdb[6] == BMIC_FLASH_FIRMWARE;
+}
+
+/*
+ * During firmware flash, the heartbeat register may not update as frequently
+ * as it should.  So we dial down lockup detection during firmware flash. and
+ * dial it back up when firmware flash completes.
+ */
+#define HEARTBEAT_SAMPLE_INTERVAL_DURING_FLASH (240 * HZ)
+#define HEARTBEAT_SAMPLE_INTERVAL (30 * HZ)
+static void dial_down_lockup_detection_during_fw_flash(struct ctlr_info *h,
+		struct CommandList *c)
+{
+	if (!is_firmware_flash_cmd(c->Request.CDB))
+		return;
+	atomic_inc(&h->firmware_flash_in_progress);
+	h->heartbeat_sample_interval = HEARTBEAT_SAMPLE_INTERVAL_DURING_FLASH;
+}
+
+static void dial_up_lockup_detection_on_fw_flash_complete(struct ctlr_info *h,
+		struct CommandList *c)
+{
+	if (is_firmware_flash_cmd(c->Request.CDB) &&
+		atomic_dec_and_test(&h->firmware_flash_in_progress))
+		h->heartbeat_sample_interval = HEARTBEAT_SAMPLE_INTERVAL;
 }
 
 static void enqueue_cmd_and_start_io(struct ctlr_info *h,
@@ -554,11 +604,12 @@ static void enqueue_cmd_and_start_io(struct ctlr_info *h,
 	unsigned long flags;
 
 	set_performant_mode(h, c);
+	dial_down_lockup_detection_during_fw_flash(h, c);
 	spin_lock_irqsave(&h->lock, flags);
 	addQ(&h->reqQ, c);
 	h->Qdepth++;
-	start_io(h);
 	spin_unlock_irqrestore(&h->lock, flags);
+	start_io(h);
 }
 
 static inline void removeQ(struct CommandList *c)
@@ -1193,7 +1244,7 @@ static void complete_scsi_command(struct CommandList *cp)
 				break;
 			}
 			/* Must be some other type of check condition */
-			dev_warn(&h->pdev->dev, "cp %p has check condition: "
+			dev_dbg(&h->pdev->dev, "cp %p has check condition: "
 					"unknown type: "
 					"Sense: 0x%x, ASC: 0x%x, ASCQ: 0x%x, "
 					"Returning result: 0x%x, "
@@ -1370,16 +1421,24 @@ static void hpsa_scsi_do_simple_cmd_core_if_no_lockup(struct ctlr_info *h,
 	}
 }
 
+#define MAX_DRIVER_CMD_RETRIES 25
 static void hpsa_scsi_do_simple_cmd_with_retry(struct ctlr_info *h,
 	struct CommandList *c, int data_direction)
 {
-	int retry_count = 0;
+	int backoff_time = 10, retry_count = 0;
 
 	do {
 		memset(c->err_info, 0, sizeof(*c->err_info));
 		hpsa_scsi_do_simple_cmd_core(h, c);
 		retry_count++;
-	} while (check_for_unit_attention(h, c) && retry_count <= 3);
+		if (retry_count > 3) {
+			msleep(backoff_time);
+			if (backoff_time < 1000)
+				backoff_time *= 2;
+		}
+	} while ((check_for_unit_attention(h, c) ||
+			check_for_busy(h, c)) &&
+			retry_count <= MAX_DRIVER_CMD_RETRIES);
 	hpsa_pci_unmap(h->pdev, c, 1, data_direction);
 }
 
@@ -2065,9 +2124,8 @@ static int hpsa_scsi_queue_command_lck(struct scsi_cmnd *cmd,
 		done(cmd);
 		return 0;
 	}
-	/* Need a lock as this is being allocated from the pool */
-	c = cmd_alloc(h);
 	spin_unlock_irqrestore(&h->lock, flags);
+	c = cmd_alloc(h);
 	if (c == NULL) {			/* trouble... */
 		dev_err(&h->pdev->dev, "cmd_alloc returned NULL!\n");
 		return SCSI_MLQUEUE_HOST_BUSY;
@@ -2334,6 +2392,261 @@ static int hpsa_eh_device_reset_handler(struct scsi_cmnd *scsicmd)
 	return FAILED;
 }
 
+static void swizzle_abort_tag(u8 *tag)
+{
+	u8 original_tag[8];
+
+	memcpy(original_tag, tag, 8);
+	tag[0] = original_tag[3];
+	tag[1] = original_tag[2];
+	tag[2] = original_tag[1];
+	tag[3] = original_tag[0];
+	tag[4] = original_tag[7];
+	tag[5] = original_tag[6];
+	tag[6] = original_tag[5];
+	tag[7] = original_tag[4];
+}
+
+static int hpsa_send_abort(struct ctlr_info *h, unsigned char *scsi3addr,
+	struct CommandList *abort, int swizzle)
+{
+	int rc = IO_OK;
+	struct CommandList *c;
+	struct ErrorInfo *ei;
+
+	c = cmd_special_alloc(h);
+	if (c == NULL) {	/* trouble... */
+		dev_warn(&h->pdev->dev, "cmd_special_alloc returned NULL!\n");
+		return -ENOMEM;
+	}
+
+	fill_cmd(c, HPSA_ABORT_MSG, h, abort, 0, 0, scsi3addr, TYPE_MSG);
+	if (swizzle)
+		swizzle_abort_tag(&c->Request.CDB[4]);
+	hpsa_scsi_do_simple_cmd_core(h, c);
+	dev_dbg(&h->pdev->dev, "%s: Tag:0x%08x:%08x: do_simple_cmd_core completed.\n",
+		__func__, abort->Header.Tag.upper, abort->Header.Tag.lower);
+	/* no unmap needed here because no data xfer. */
+
+	ei = c->err_info;
+	switch (ei->CommandStatus) {
+	case CMD_SUCCESS:
+		break;
+	case CMD_UNABORTABLE: /* Very common, don't make noise. */
+		rc = -1;
+		break;
+	default:
+		dev_dbg(&h->pdev->dev, "%s: Tag:0x%08x:%08x: interpreting error.\n",
+			__func__, abort->Header.Tag.upper,
+			abort->Header.Tag.lower);
+		hpsa_scsi_interpret_error(c);
+		rc = -1;
+		break;
+	}
+	cmd_special_free(h, c);
+	dev_dbg(&h->pdev->dev, "%s: Tag:0x%08x:%08x: Finished.\n", __func__,
+		abort->Header.Tag.upper, abort->Header.Tag.lower);
+	return rc;
+}
+
+/*
+ * hpsa_find_cmd_in_queue
+ *
+ * Used to determine whether a command (find) is still present
+ * in queue_head.   Optionally excludes the last element of queue_head.
+ *
+ * This is used to avoid unnecessary aborts.  Commands in h->reqQ have
+ * not yet been submitted, and so can be aborted by the driver without
+ * sending an abort to the hardware.
+ *
+ * Returns pointer to command if found in queue, NULL otherwise.
+ */
+static struct CommandList *hpsa_find_cmd_in_queue(struct ctlr_info *h,
+			struct scsi_cmnd *find, struct list_head *queue_head)
+{
+	unsigned long flags;
+	struct CommandList *c = NULL;	/* ptr into cmpQ */
+
+	if (!find)
+		return 0;
+	spin_lock_irqsave(&h->lock, flags);
+	list_for_each_entry(c, queue_head, list) {
+		if (c->scsi_cmd == NULL) /* e.g.: passthru ioctl */
+			continue;
+		if (c->scsi_cmd == find) {
+			spin_unlock_irqrestore(&h->lock, flags);
+			return c;
+		}
+	}
+	spin_unlock_irqrestore(&h->lock, flags);
+	return NULL;
+}
+
+static struct CommandList *hpsa_find_cmd_in_queue_by_tag(struct ctlr_info *h,
+					u8 *tag, struct list_head *queue_head)
+{
+	unsigned long flags;
+	struct CommandList *c;
+
+	spin_lock_irqsave(&h->lock, flags);
+	list_for_each_entry(c, queue_head, list) {
+		if (memcmp(&c->Header.Tag, tag, 8) != 0)
+			continue;
+		spin_unlock_irqrestore(&h->lock, flags);
+		return c;
+	}
+	spin_unlock_irqrestore(&h->lock, flags);
+	return NULL;
+}
+
+/* Some Smart Arrays need the abort tag swizzled, and some don't.  It's hard to
+ * tell which kind we're dealing with, so we send the abort both ways.  There
+ * shouldn't be any collisions between swizzled and unswizzled tags due to the
+ * way we construct our tags but we check anyway in case the assumptions which
+ * make this true someday become false.
+ */
+static int hpsa_send_abort_both_ways(struct ctlr_info *h,
+	unsigned char *scsi3addr, struct CommandList *abort)
+{
+	u8 swizzled_tag[8];
+	struct CommandList *c;
+	int rc = 0, rc2 = 0;
+
+	/* we do not expect to find the swizzled tag in our queue, but
+	 * check anyway just to be sure the assumptions which make this
+	 * the case haven't become wrong.
+	 */
+	memcpy(swizzled_tag, &abort->Request.CDB[4], 8);
+	swizzle_abort_tag(swizzled_tag);
+	c = hpsa_find_cmd_in_queue_by_tag(h, swizzled_tag, &h->cmpQ);
+	if (c != NULL) {
+		dev_warn(&h->pdev->dev, "Unexpectedly found byte-swapped tag in completion queue.\n");
+		return hpsa_send_abort(h, scsi3addr, abort, 0);
+	}
+	rc = hpsa_send_abort(h, scsi3addr, abort, 0);
+
+	/* if the command is still in our queue, we can't conclude that it was
+	 * aborted (it might have just completed normally) but in any case
+	 * we don't need to try to abort it another way.
+	 */
+	c = hpsa_find_cmd_in_queue(h, abort->scsi_cmd, &h->cmpQ);
+	if (c)
+		rc2 = hpsa_send_abort(h, scsi3addr, abort, 1);
+	return rc && rc2;
+}
+
+/* Send an abort for the specified command.
+ *	If the device and controller support it,
+ *		send a task abort request.
+ */
+static int hpsa_eh_abort_handler(struct scsi_cmnd *sc)
+{
+
+	int i, rc;
+	struct ctlr_info *h;
+	struct hpsa_scsi_dev_t *dev;
+	struct CommandList *abort; /* pointer to command to be aborted */
+	struct CommandList *found;
+	struct scsi_cmnd *as;	/* ptr to scsi cmd inside aborted command. */
+	char msg[256];		/* For debug messaging. */
+	int ml = 0;
+
+	/* Find the controller of the command to be aborted */
+	h = sdev_to_hba(sc->device);
+	if (WARN(h == NULL,
+			"ABORT REQUEST FAILED, Controller lookup failed.\n"))
+		return FAILED;
+
+	/* Check that controller supports some kind of task abort */
+	if (!(HPSATMF_PHYS_TASK_ABORT & h->TMFSupportFlags) &&
+		!(HPSATMF_LOG_TASK_ABORT & h->TMFSupportFlags))
+		return FAILED;
+
+	memset(msg, 0, sizeof(msg));
+	ml += sprintf(msg+ml, "ABORT REQUEST on C%d:B%d:T%d:L%d ",
+		h->scsi_host->host_no, sc->device->channel,
+		sc->device->id, sc->device->lun);
+
+	/* Find the device of the command to be aborted */
+	dev = sc->device->hostdata;
+	if (!dev) {
+		dev_err(&h->pdev->dev, "%s FAILED, Device lookup failed.\n",
+				msg);
+		return FAILED;
+	}
+
+	/* Get SCSI command to be aborted */
+	abort = (struct CommandList *) sc->host_scribble;
+	if (abort == NULL) {
+		dev_err(&h->pdev->dev, "%s FAILED, Command to abort is NULL.\n",
+				msg);
+		return FAILED;
+	}
+
+	ml += sprintf(msg+ml, "Tag:0x%08x:%08x ",
+		abort->Header.Tag.upper, abort->Header.Tag.lower);
+	as  = (struct scsi_cmnd *) abort->scsi_cmd;
+	if (as != NULL)
+		ml += sprintf(msg+ml, "Command:0x%x SN:0x%lx ",
+			as->cmnd[0], as->serial_number);
+	dev_dbg(&h->pdev->dev, "%s\n", msg);
+	dev_warn(&h->pdev->dev, "Abort request on C%d:B%d:T%d:L%d\n",
+		h->scsi_host->host_no, dev->bus, dev->target, dev->lun);
+
+	/* Search reqQ to See if command is queued but not submitted,
+	 * if so, complete the command with aborted status and remove
+	 * it from the reqQ.
+	 */
+	found = hpsa_find_cmd_in_queue(h, sc, &h->reqQ);
+	if (found) {
+		found->err_info->CommandStatus = CMD_ABORTED;
+		finish_cmd(found);
+		dev_info(&h->pdev->dev, "%s Request SUCCEEDED (driver queue).\n",
+				msg);
+		return SUCCESS;
+	}
+
+	/* not in reqQ, if also not in cmpQ, must have already completed */
+	found = hpsa_find_cmd_in_queue(h, sc, &h->cmpQ);
+	if (!found)  {
+		dev_dbg(&h->pdev->dev, "%s Request FAILED (not known to driver).\n",
+				msg);
+		return SUCCESS;
+	}
+
+	/*
+	 * Command is in flight, or possibly already completed
+	 * by the firmware (but not to the scsi mid layer) but we can't
+	 * distinguish which.  Send the abort down.
+	 */
+	rc = hpsa_send_abort_both_ways(h, dev->scsi3addr, abort);
+	if (rc != 0) {
+		dev_dbg(&h->pdev->dev, "%s Request FAILED.\n", msg);
+		dev_warn(&h->pdev->dev, "FAILED abort on device C%d:B%d:T%d:L%d\n",
+			h->scsi_host->host_no,
+			dev->bus, dev->target, dev->lun);
+		return FAILED;
+	}
+	dev_info(&h->pdev->dev, "%s REQUEST SUCCEEDED.\n", msg);
+
+	/* If the abort(s) above completed and actually aborted the
+	 * command, then the command to be aborted should already be
+	 * completed.  If not, wait around a bit more to see if they
+	 * manage to complete normally.
+	 */
+#define ABORT_COMPLETE_WAIT_SECS 30
+	for (i = 0; i < ABORT_COMPLETE_WAIT_SECS * 10; i++) {
+		found = hpsa_find_cmd_in_queue(h, sc, &h->cmpQ);
+		if (!found)
+			return SUCCESS;
+		msleep(100);
+	}
+	dev_warn(&h->pdev->dev, "%s FAILED. Aborted command has not completed after %d seconds.\n",
+		msg, ABORT_COMPLETE_WAIT_SECS);
+	return FAILED;
+}
+
+
 /*
  * For operations that cannot sleep, a command block is allocated at init,
  * and managed by cmd_alloc() and cmd_free() using a simple bitmap to track
@@ -2346,14 +2659,21 @@ static struct CommandList *cmd_alloc(struct ctlr_info *h)
 	int i;
 	union u64bit temp64;
 	dma_addr_t cmd_dma_handle, err_dma_handle;
+	unsigned long flags;
 
+	spin_lock_irqsave(&h->lock, flags);
 	do {
 		i = find_first_zero_bit(h->cmd_pool_bits, h->nr_cmds);
-		if (i == h->nr_cmds)
+		if (i == h->nr_cmds) {
+			spin_unlock_irqrestore(&h->lock, flags);
 			return NULL;
+		}
 	} while (test_and_set_bit
 		 (i & (BITS_PER_LONG - 1),
 		  h->cmd_pool_bits + (i / BITS_PER_LONG)) != 0);
+	h->nr_allocs++;
+	spin_unlock_irqrestore(&h->lock, flags);
+
 	c = h->cmd_pool + i;
 	memset(c, 0, sizeof(*c));
 	cmd_dma_handle = h->cmd_pool_dhandle
@@ -2362,7 +2682,6 @@ static struct CommandList *cmd_alloc(struct ctlr_info *h)
 	memset(c->err_info, 0, sizeof(*c->err_info));
 	err_dma_handle = h->errinfo_pool_dhandle
 	    + i * sizeof(*c->err_info);
-	h->nr_allocs++;
 
 	c->cmdindex = i;
 
@@ -2418,11 +2737,14 @@ static struct CommandList *cmd_special_alloc(struct ctlr_info *h)
 static void cmd_free(struct ctlr_info *h, struct CommandList *c)
 {
 	int i;
+	unsigned long flags;
 
 	i = c - h->cmd_pool;
+	spin_lock_irqsave(&h->lock, flags);
 	clear_bit(i & (BITS_PER_LONG - 1),
 		  h->cmd_pool_bits + (i / BITS_PER_LONG));
 	h->nr_frees++;
+	spin_unlock_irqrestore(&h->lock, flags);
 }
 
 static void cmd_special_free(struct ctlr_info *h, struct CommandList *c)
@@ -2866,6 +3188,7 @@ static void fill_cmd(struct CommandList *c, u8 cmd, struct ctlr_info *h,
 	int cmd_type)
 {
 	int pci_dir = XFER_NONE;
+	struct CommandList *a; /* for commands to be aborted */
 
 	c->cmd_type = CMD_IOCTL_PEND;
 	c->Header.ReplyQueue = 0;
@@ -2949,8 +3272,35 @@ static void fill_cmd(struct CommandList *c, u8 cmd, struct ctlr_info *h,
 			c->Request.CDB[5] = 0x00;
 			c->Request.CDB[6] = 0x00;
 			c->Request.CDB[7] = 0x00;
+			break;
+		case  HPSA_ABORT_MSG:
+			a = buff;       /* point to command to be aborted */
+			dev_dbg(&h->pdev->dev, "Abort Tag:0x%08x:%08x using request Tag:0x%08x:%08x\n",
+				a->Header.Tag.upper, a->Header.Tag.lower,
+				c->Header.Tag.upper, c->Header.Tag.lower);
+			c->Request.CDBLen = 16;
+			c->Request.Type.Type = TYPE_MSG;
+			c->Request.Type.Attribute = ATTR_SIMPLE;
+			c->Request.Type.Direction = XFER_WRITE;
+			c->Request.Timeout = 0; /* Don't time out */
+			c->Request.CDB[0] = HPSA_TASK_MANAGEMENT;
+			c->Request.CDB[1] = HPSA_TMF_ABORT_TASK;
+			c->Request.CDB[2] = 0x00; /* reserved */
+			c->Request.CDB[3] = 0x00; /* reserved */
+			/* Tag to abort goes in CDB[4]-CDB[11] */
+			c->Request.CDB[4] = a->Header.Tag.lower & 0xFF;
+			c->Request.CDB[5] = (a->Header.Tag.lower >> 8) & 0xFF;
+			c->Request.CDB[6] = (a->Header.Tag.lower >> 16) & 0xFF;
+			c->Request.CDB[7] = (a->Header.Tag.lower >> 24) & 0xFF;
+			c->Request.CDB[8] = a->Header.Tag.upper & 0xFF;
+			c->Request.CDB[9] = (a->Header.Tag.upper >> 8) & 0xFF;
+			c->Request.CDB[10] = (a->Header.Tag.upper >> 16) & 0xFF;
+			c->Request.CDB[11] = (a->Header.Tag.upper >> 24) & 0xFF;
+			c->Request.CDB[12] = 0x00; /* reserved */
+			c->Request.CDB[13] = 0x00; /* reserved */
+			c->Request.CDB[14] = 0x00; /* reserved */
+			c->Request.CDB[15] = 0x00; /* reserved */
 		break;
-
 		default:
 			dev_warn(&h->pdev->dev, "unknown message type %d\n",
 				cmd);
@@ -2998,7 +3348,9 @@ static void __iomem *remap_pci_mem(ulong base, ulong size)
 static void start_io(struct ctlr_info *h)
 {
 	struct CommandList *c;
+	unsigned long flags;
 
+	spin_lock_irqsave(&h->lock, flags);
 	while (!list_empty(&h->reqQ)) {
 		c = list_entry(h->reqQ.next, struct CommandList, list);
 		/* can't do anything if fifo is full */
@@ -3011,17 +3363,28 @@ static void start_io(struct ctlr_info *h)
 		removeQ(c);
 		h->Qdepth--;
 
-		/* Tell the controller execute command */
-		h->access.submit_command(h, c);
-
 		/* Put job onto the completed Q */
 		addQ(&h->cmpQ, c);
+
+		/* Must increment commands_outstanding before unlocking
+		 * and submitting to avoid race checking for fifo full
+		 * condition.
+		 */
+		h->commands_outstanding++;
+		if (h->commands_outstanding > h->max_outstanding)
+			h->max_outstanding = h->commands_outstanding;
+
+		/* Tell the controller execute command */
+		spin_unlock_irqrestore(&h->lock, flags);
+		h->access.submit_command(h, c);
+		spin_lock_irqsave(&h->lock, flags);
 	}
+	spin_unlock_irqrestore(&h->lock, flags);
 }
 
-static inline unsigned long get_next_completion(struct ctlr_info *h)
+static inline unsigned long get_next_completion(struct ctlr_info *h, u8 q)
 {
-	return h->access.command_completed(h);
+	return h->access.command_completed(h, q);
 }
 
 static inline bool interrupt_pending(struct ctlr_info *h)
@@ -3045,9 +3408,14 @@ static inline int bad_tag(struct ctlr_info *h, u32 tag_index,
 	return 0;
 }
 
-static inline void finish_cmd(struct CommandList *c, u32 raw_tag)
+static inline void finish_cmd(struct CommandList *c)
 {
+	unsigned long flags;
+
+	spin_lock_irqsave(&c->h->lock, flags);
 	removeQ(c);
+	spin_unlock_irqrestore(&c->h->lock, flags);
+	dial_up_lockup_detection_on_fw_flash_complete(c->h, c);
 	if (likely(c->cmd_type == CMD_SCSI))
 		complete_scsi_command(c);
 	else if (c->cmd_type == CMD_IOCTL_PEND)
@@ -3075,36 +3443,38 @@ static inline u32 hpsa_tag_discard_error_bits(struct ctlr_info *h, u32 tag)
 }
 
 /* process completion of an indexed ("direct lookup") command */
-static inline u32 process_indexed_cmd(struct ctlr_info *h,
+static inline void process_indexed_cmd(struct ctlr_info *h,
 	u32 raw_tag)
 {
 	u32 tag_index;
 	struct CommandList *c;
 
 	tag_index = hpsa_tag_to_index(raw_tag);
-	if (bad_tag(h, tag_index, raw_tag))
-		return next_command(h);
-	c = h->cmd_pool + tag_index;
-	finish_cmd(c, raw_tag);
-	return next_command(h);
+	if (!bad_tag(h, tag_index, raw_tag)) {
+		c = h->cmd_pool + tag_index;
+		finish_cmd(c);
+	}
 }
 
 /* process completion of a non-indexed command */
-static inline u32 process_nonindexed_cmd(struct ctlr_info *h,
+static inline void process_nonindexed_cmd(struct ctlr_info *h,
 	u32 raw_tag)
 {
 	u32 tag;
 	struct CommandList *c = NULL;
+	unsigned long flags;
 
 	tag = hpsa_tag_discard_error_bits(h, raw_tag);
+	spin_lock_irqsave(&h->lock, flags);
 	list_for_each_entry(c, &h->cmpQ, list) {
 		if ((c->busaddr & 0xFFFFFFE0) == (tag & 0xFFFFFFE0)) {
-			finish_cmd(c, raw_tag);
-			return next_command(h);
+			spin_unlock_irqrestore(&h->lock, flags);
+			finish_cmd(c);
+			return;
 		}
 	}
+	spin_unlock_irqrestore(&h->lock, flags);
 	bad_tag(h, h->nr_cmds + 1, raw_tag);
-	return next_command(h);
 }
 
 /* Some controllers, like p400, will give us one interrupt
@@ -3126,10 +3496,20 @@ static int ignore_bogus_interrupt(struct ctlr_info *h)
 	return 1;
 }
 
-static irqreturn_t hpsa_intx_discard_completions(int irq, void *dev_id)
+/*
+ * Convert &h->q[x] (passed to interrupt handlers) back to h.
+ * Relies on (h-q[x] == x) being true for x such that
+ * 0 <= x < MAX_REPLY_QUEUES.
+ */
+static struct ctlr_info *queue_to_hba(u8 *queue)
 {
-	struct ctlr_info *h = dev_id;
-	unsigned long flags;
+	return container_of((queue - *queue), struct ctlr_info, q[0]);
+}
+
+static irqreturn_t hpsa_intx_discard_completions(int irq, void *queue)
+{
+	struct ctlr_info *h = queue_to_hba(queue);
+	u8 q = *(u8 *) queue;
 	u32 raw_tag;
 
 	if (ignore_bogus_interrupt(h))
@@ -3137,74 +3517,68 @@ static irqreturn_t hpsa_intx_discard_completions(int irq, void *dev_id)
 
 	if (interrupt_not_for_us(h))
 		return IRQ_NONE;
-	spin_lock_irqsave(&h->lock, flags);
 	h->last_intr_timestamp = get_jiffies_64();
 	while (interrupt_pending(h)) {
-		raw_tag = get_next_completion(h);
+		raw_tag = get_next_completion(h, q);
 		while (raw_tag != FIFO_EMPTY)
-			raw_tag = next_command(h);
+			raw_tag = next_command(h, q);
 	}
-	spin_unlock_irqrestore(&h->lock, flags);
 	return IRQ_HANDLED;
 }
 
-static irqreturn_t hpsa_msix_discard_completions(int irq, void *dev_id)
+static irqreturn_t hpsa_msix_discard_completions(int irq, void *queue)
 {
-	struct ctlr_info *h = dev_id;
-	unsigned long flags;
+	struct ctlr_info *h = queue_to_hba(queue);
 	u32 raw_tag;
+	u8 q = *(u8 *) queue;
 
 	if (ignore_bogus_interrupt(h))
 		return IRQ_NONE;
 
-	spin_lock_irqsave(&h->lock, flags);
 	h->last_intr_timestamp = get_jiffies_64();
-	raw_tag = get_next_completion(h);
+	raw_tag = get_next_completion(h, q);
 	while (raw_tag != FIFO_EMPTY)
-		raw_tag = next_command(h);
-	spin_unlock_irqrestore(&h->lock, flags);
+		raw_tag = next_command(h, q);
 	return IRQ_HANDLED;
 }
 
-static irqreturn_t do_hpsa_intr_intx(int irq, void *dev_id)
+static irqreturn_t do_hpsa_intr_intx(int irq, void *queue)
 {
-	struct ctlr_info *h = dev_id;
-	unsigned long flags;
+	struct ctlr_info *h = queue_to_hba((u8 *) queue);
 	u32 raw_tag;
+	u8 q = *(u8 *) queue;
 
 	if (interrupt_not_for_us(h))
 		return IRQ_NONE;
-	spin_lock_irqsave(&h->lock, flags);
 	h->last_intr_timestamp = get_jiffies_64();
 	while (interrupt_pending(h)) {
-		raw_tag = get_next_completion(h);
+		raw_tag = get_next_completion(h, q);
 		while (raw_tag != FIFO_EMPTY) {
-			if (hpsa_tag_contains_index(raw_tag))
-				raw_tag = process_indexed_cmd(h, raw_tag);
+			if (likely(hpsa_tag_contains_index(raw_tag)))
+				process_indexed_cmd(h, raw_tag);
 			else
-				raw_tag = process_nonindexed_cmd(h, raw_tag);
+				process_nonindexed_cmd(h, raw_tag);
+			raw_tag = next_command(h, q);
 		}
 	}
-	spin_unlock_irqrestore(&h->lock, flags);
 	return IRQ_HANDLED;
 }
 
-static irqreturn_t do_hpsa_intr_msi(int irq, void *dev_id)
+static irqreturn_t do_hpsa_intr_msi(int irq, void *queue)
 {
-	struct ctlr_info *h = dev_id;
-	unsigned long flags;
+	struct ctlr_info *h = queue_to_hba(queue);
 	u32 raw_tag;
+	u8 q = *(u8 *) queue;
 
-	spin_lock_irqsave(&h->lock, flags);
 	h->last_intr_timestamp = get_jiffies_64();
-	raw_tag = get_next_completion(h);
+	raw_tag = get_next_completion(h, q);
 	while (raw_tag != FIFO_EMPTY) {
-		if (hpsa_tag_contains_index(raw_tag))
-			raw_tag = process_indexed_cmd(h, raw_tag);
+		if (likely(hpsa_tag_contains_index(raw_tag)))
+			process_indexed_cmd(h, raw_tag);
 		else
-			raw_tag = process_nonindexed_cmd(h, raw_tag);
+			process_nonindexed_cmd(h, raw_tag);
+		raw_tag = next_command(h, q);
 	}
-	spin_unlock_irqrestore(&h->lock, flags);
 	return IRQ_HANDLED;
 }
 
@@ -3638,10 +4012,13 @@ static int find_PCI_BAR_index(struct pci_dev *pdev, unsigned long pci_bar_addr)
 static void __devinit hpsa_interrupt_mode(struct ctlr_info *h)
 {
 #ifdef CONFIG_PCI_MSI
-	int err;
-	struct msix_entry hpsa_msix_entries[4] = { {0, 0}, {0, 1},
-	{0, 2}, {0, 3}
-	};
+	int err, i;
+	struct msix_entry hpsa_msix_entries[MAX_REPLY_QUEUES];
+
+	for (i = 0; i < MAX_REPLY_QUEUES; i++) {
+		hpsa_msix_entries[i].vector = 0;
+		hpsa_msix_entries[i].entry = i;
+	}
 
 	/* Some boards advertise MSI but don't really support it */
 	if ((h->board_id == 0x40700E11) || (h->board_id == 0x40800E11) ||
@@ -3649,12 +4026,11 @@ static void __devinit hpsa_interrupt_mode(struct ctlr_info *h)
 		goto default_int_mode;
 	if (pci_find_capability(h->pdev, PCI_CAP_ID_MSIX)) {
 		dev_info(&h->pdev->dev, "MSIX\n");
-		err = pci_enable_msix(h->pdev, hpsa_msix_entries, 4);
+		err = pci_enable_msix(h->pdev, hpsa_msix_entries,
+						MAX_REPLY_QUEUES);
 		if (!err) {
-			h->intr[0] = hpsa_msix_entries[0].vector;
-			h->intr[1] = hpsa_msix_entries[1].vector;
-			h->intr[2] = hpsa_msix_entries[2].vector;
-			h->intr[3] = hpsa_msix_entries[3].vector;
+			for (i = 0; i < MAX_REPLY_QUEUES; i++)
+				h->intr[i] = hpsa_msix_entries[i].vector;
 			h->msix_vector = 1;
 			return;
 		}
@@ -3705,14 +4081,6 @@ static int __devinit hpsa_lookup_board_id(struct pci_dev *pdev, u32 *board_id)
 	return ARRAY_SIZE(products) - 1; /* generic unknown smart array */
 }
 
-static inline bool hpsa_board_disabled(struct pci_dev *pdev)
-{
-	u16 command;
-
-	(void) pci_read_config_word(pdev, PCI_COMMAND, &command);
-	return ((command & PCI_COMMAND_MEMORY) == 0);
-}
-
 static int __devinit hpsa_pci_find_memory_BAR(struct pci_dev *pdev,
 	unsigned long *memory_bar)
 {
@@ -3838,14 +4206,14 @@ static void __devinit hpsa_find_board_params(struct ctlr_info *h)
 		h->maxsgentries = 31; /* default to traditional values */
 		h->chainsize = 0;
 	}
+
+	/* Find out what task management functions are supported and cache */
+	h->TMFSupportFlags = readl(&(h->cfgtable->TMFSupportFlags));
 }
 
 static inline bool hpsa_CISS_signature_present(struct ctlr_info *h)
 {
-	if ((readb(&h->cfgtable->Signature[0]) != 'C') ||
-	    (readb(&h->cfgtable->Signature[1]) != 'I') ||
-	    (readb(&h->cfgtable->Signature[2]) != 'S') ||
-	    (readb(&h->cfgtable->Signature[3]) != 'S')) {
+	if (!check_signature(h->cfgtable->Signature, "CISS", 4)) {
 		dev_warn(&h->pdev->dev, "not a valid CISS config table\n");
 		return false;
 	}
@@ -3932,11 +4300,6 @@ static int __devinit hpsa_pci_init(struct ctlr_info *h)
 	h->product_name = products[prod_index].product_name;
 	h->access = *(products[prod_index].access);
 
-	if (hpsa_board_disabled(h->pdev)) {
-		dev_warn(&h->pdev->dev, "controller appears to be disabled\n");
-		return -ENODEV;
-	}
-
 	pci_disable_link_state(h->pdev, PCIE_LINK_STATE_L0S |
 			       PCIE_LINK_STATE_L1 | PCIE_LINK_STATE_CLKPM);
 
@@ -3946,6 +4309,9 @@ static int __devinit hpsa_pci_init(struct ctlr_info *h)
 		return err;
 	}
 
+	/* Enable bus mastering (pci_disable_device may disable this) */
+	pci_set_master(h->pdev);
+
 	err = pci_request_regions(h->pdev, HPSA);
 	if (err) {
 		dev_err(&h->pdev->dev,
@@ -3987,10 +4353,7 @@ err_out_free_res:
 		iounmap(h->cfgtable);
 	if (h->vaddr)
 		iounmap(h->vaddr);
-	/*
-	 * Deliberately omit pci_disable_device(): it does something nasty to
-	 * Smart Array controllers that pci_enable_device does not undo
-	 */
+	pci_disable_device(h->pdev);
 	pci_release_regions(h->pdev);
 	return err;
 }
@@ -4081,14 +4444,33 @@ static int hpsa_request_irq(struct ctlr_info *h,
 	irqreturn_t (*msixhandler)(int, void *),
 	irqreturn_t (*intxhandler)(int, void *))
 {
-	int rc;
+	int rc, i;
 
-	if (h->msix_vector || h->msi_vector)
-		rc = request_irq(h->intr[h->intr_mode], msixhandler,
-				0, h->devname, h);
-	else
-		rc = request_irq(h->intr[h->intr_mode], intxhandler,
-				IRQF_SHARED, h->devname, h);
+	/*
+	 * initialize h->q[x] = x so that interrupt handlers know which
+	 * queue to process.
+	 */
+	for (i = 0; i < MAX_REPLY_QUEUES; i++)
+		h->q[i] = (u8) i;
+
+	if (h->intr_mode == PERF_MODE_INT && h->msix_vector) {
+		/* If performant mode and MSI-X, use multiple reply queues */
+		for (i = 0; i < MAX_REPLY_QUEUES; i++)
+			rc = request_irq(h->intr[i], msixhandler,
+					0, h->devname,
+					&h->q[i]);
+	} else {
+		/* Use single reply pool */
+		if (h->msix_vector || h->msi_vector) {
+			rc = request_irq(h->intr[h->intr_mode],
+				msixhandler, 0, h->devname,
+				&h->q[h->intr_mode]);
+		} else {
+			rc = request_irq(h->intr[h->intr_mode],
+				intxhandler, IRQF_SHARED, h->devname,
+				&h->q[h->intr_mode]);
+		}
+	}
 	if (rc) {
 		dev_err(&h->pdev->dev, "unable to get irq %d for %s\n",
 		       h->intr[h->intr_mode], h->devname);
@@ -4121,15 +4503,38 @@ static int __devinit hpsa_kdump_soft_reset(struct ctlr_info *h)
 	return 0;
 }
 
-static void hpsa_undo_allocations_after_kdump_soft_reset(struct ctlr_info *h)
+static void free_irqs(struct ctlr_info *h)
 {
-	free_irq(h->intr[h->intr_mode], h);
+	int i;
+
+	if (!h->msix_vector || h->intr_mode != PERF_MODE_INT) {
+		/* Single reply queue, only one irq to free */
+		i = h->intr_mode;
+		free_irq(h->intr[i], &h->q[i]);
+		return;
+	}
+
+	for (i = 0; i < MAX_REPLY_QUEUES; i++)
+		free_irq(h->intr[i], &h->q[i]);
+}
+
+static void hpsa_free_irqs_and_disable_msix(struct ctlr_info *h)
+{
+	free_irqs(h);
 #ifdef CONFIG_PCI_MSI
-	if (h->msix_vector)
-		pci_disable_msix(h->pdev);
-	else if (h->msi_vector)
-		pci_disable_msi(h->pdev);
+	if (h->msix_vector) {
+		if (h->pdev->msix_enabled)
+			pci_disable_msix(h->pdev);
+	} else if (h->msi_vector) {
+		if (h->pdev->msi_enabled)
+			pci_disable_msi(h->pdev);
+	}
 #endif /* CONFIG_PCI_MSI */
+}
+
+static void hpsa_undo_allocations_after_kdump_soft_reset(struct ctlr_info *h)
+{
+	hpsa_free_irqs_and_disable_msix(h);
 	hpsa_free_sg_chain_blocks(h);
 	hpsa_free_cmd_pool(h);
 	kfree(h->blockFetchTable);
@@ -4165,7 +4570,7 @@ static void fail_all_cmds_on_list(struct ctlr_info *h, struct list_head *list)
 	while (!list_empty(list)) {
 		c = list_entry(list->next, struct CommandList, list);
 		c->err_info->CommandStatus = CMD_HARDWARE_ERR;
-		finish_cmd(c, c->Header.Tag.lower);
+		finish_cmd(c);
 	}
 }
 
@@ -4188,9 +4593,6 @@ static void controller_lockup_detected(struct ctlr_info *h)
 	spin_unlock_irqrestore(&h->lock, flags);
 }
 
-#define HEARTBEAT_SAMPLE_INTERVAL (10 * HZ)
-#define HEARTBEAT_CHECK_MINIMUM_INTERVAL (HEARTBEAT_SAMPLE_INTERVAL / 2)
-
 static void detect_controller_lockup(struct ctlr_info *h)
 {
 	u64 now;
@@ -4201,7 +4603,7 @@ static void detect_controller_lockup(struct ctlr_info *h)
 	now = get_jiffies_64();
 	/* If we've received an interrupt recently, we're ok. */
 	if (time_after64(h->last_intr_timestamp +
-				(HEARTBEAT_CHECK_MINIMUM_INTERVAL), now))
+				(h->heartbeat_sample_interval), now))
 		return;
 
 	/*
@@ -4210,7 +4612,7 @@ static void detect_controller_lockup(struct ctlr_info *h)
 	 * otherwise don't care about signals in this thread.
 	 */
 	if (time_after64(h->last_heartbeat_timestamp +
-				(HEARTBEAT_CHECK_MINIMUM_INTERVAL), now))
+				(h->heartbeat_sample_interval), now))
 		return;
 
 	/* If heartbeat has not changed since we last looked, we're not ok. */
@@ -4252,6 +4654,7 @@ static void add_ctlr_to_lockup_detector_list(struct ctlr_info *h)
 {
 	unsigned long flags;
 
+	h->heartbeat_sample_interval = HEARTBEAT_SAMPLE_INTERVAL;
 	spin_lock_irqsave(&lockup_detector_lock, flags);
 	list_add_tail(&h->lockup_list, &hpsa_ctlr_list);
 	spin_unlock_irqrestore(&lockup_detector_lock, flags);
@@ -4391,7 +4794,7 @@ reinit_after_soft_reset:
 		spin_lock_irqsave(&h->lock, flags);
 		h->access.set_intr_mask(h, HPSA_INTR_OFF);
 		spin_unlock_irqrestore(&h->lock, flags);
-		free_irq(h->intr[h->intr_mode], h);
+		free_irqs(h);
 		rc = hpsa_request_irq(h, hpsa_msix_discard_completions,
 					hpsa_intx_discard_completions);
 		if (rc) {
@@ -4441,7 +4844,7 @@ reinit_after_soft_reset:
 clean4:
 	hpsa_free_sg_chain_blocks(h);
 	hpsa_free_cmd_pool(h);
-	free_irq(h->intr[h->intr_mode], h);
+	free_irqs(h);
 clean2:
 clean1:
 	kfree(h);
@@ -4484,13 +4887,7 @@ static void hpsa_shutdown(struct pci_dev *pdev)
 	 */
 	hpsa_flush_cache(h);
 	h->access.set_intr_mask(h, HPSA_INTR_OFF);
-	free_irq(h->intr[h->intr_mode], h);
-#ifdef CONFIG_PCI_MSI
-	if (h->msix_vector)
-		pci_disable_msix(h->pdev);
-	else if (h->msi_vector)
-		pci_disable_msi(h->pdev);
-#endif				/* CONFIG_PCI_MSI */
+	hpsa_free_irqs_and_disable_msix(h);
 }
 
 static void __devexit hpsa_free_device_info(struct ctlr_info *h)
@@ -4529,10 +4926,7 @@ static void __devexit hpsa_remove_one(struct pci_dev *pdev)
 	kfree(h->cmd_pool_bits);
 	kfree(h->blockFetchTable);
 	kfree(h->hba_inquiry_data);
-	/*
-	 * Deliberately omit pci_disable_device(): it does something nasty to
-	 * Smart Array controllers that pci_enable_device does not undo
-	 */
+	pci_disable_device(pdev);
 	pci_release_regions(pdev);
 	pci_set_drvdata(pdev, NULL);
 	kfree(h);
@@ -4627,11 +5021,8 @@ static __devinit void hpsa_enter_performant_mode(struct ctlr_info *h,
 	 * 10 = 6 s/g entry or 24k
 	 */
 
-	h->reply_pool_wraparound = 1; /* spec: init to 1 */
-
 	/* Controller spec: zero out this buffer. */
 	memset(h->reply_pool, 0, h->reply_pool_size);
-	h->reply_pool_head = h->reply_pool;
 
 	bft[7] = SG_ENTRIES_IN_CMD + 4;
 	calc_bucket_map(bft, ARRAY_SIZE(bft),
@@ -4641,12 +5032,19 @@ static __devinit void hpsa_enter_performant_mode(struct ctlr_info *h,
 
 	/* size of controller ring buffer */
 	writel(h->max_commands, &h->transtable->RepQSize);
-	writel(1, &h->transtable->RepQCount);
+	writel(h->nreply_queues, &h->transtable->RepQCount);
 	writel(0, &h->transtable->RepQCtrAddrLow32);
 	writel(0, &h->transtable->RepQCtrAddrHigh32);
-	writel(h->reply_pool_dhandle, &h->transtable->RepQAddr0Low32);
-	writel(0, &h->transtable->RepQAddr0High32);
-	writel(CFGTBL_Trans_Performant | use_short_tags,
+
+	for (i = 0; i < h->nreply_queues; i++) {
+		writel(0, &h->transtable->RepQAddr[i].upper);
+		writel(h->reply_pool_dhandle +
+			(h->max_commands * sizeof(u64) * i),
+			&h->transtable->RepQAddr[i].lower);
+	}
+
+	writel(CFGTBL_Trans_Performant | use_short_tags |
+		CFGTBL_Trans_enable_directed_msix,
 		&(h->cfgtable->HostWrite.TransportRequest));
 	writel(CFGTBL_ChangeReq, h->vaddr + SA5_DOORBELL);
 	hpsa_wait_for_mode_change_ack(h);
@@ -4664,6 +5062,7 @@ static __devinit void hpsa_enter_performant_mode(struct ctlr_info *h,
 static __devinit void hpsa_put_ctlr_into_performant_mode(struct ctlr_info *h)
 {
 	u32 trans_support;
+	int i;
 
 	if (hpsa_simple_mode)
 		return;
@@ -4672,12 +5071,20 @@ static __devinit void hpsa_put_ctlr_into_performant_mode(struct ctlr_info *h)
 	if (!(trans_support & PERFORMANT_MODE))
 		return;
 
+	h->nreply_queues = h->msix_vector ? MAX_REPLY_QUEUES : 1;
 	hpsa_get_max_perf_mode_cmds(h);
 	/* Performant mode ring buffer and supporting data structures */
-	h->reply_pool_size = h->max_commands * sizeof(u64);
+	h->reply_pool_size = h->max_commands * sizeof(u64) * h->nreply_queues;
 	h->reply_pool = pci_alloc_consistent(h->pdev, h->reply_pool_size,
 				&(h->reply_pool_dhandle));
 
+	for (i = 0; i < h->nreply_queues; i++) {
+		h->reply_queue[i].head = &h->reply_pool[h->max_commands * i];
+		h->reply_queue[i].size = h->max_commands;
+		h->reply_queue[i].wraparound = 1;  /* spec: init to 1 */
+		h->reply_queue[i].current_entry = 0;
+	}
+
 	/* Need a block fetch table for performant mode */
 	h->blockFetchTable = kmalloc(((SG_ENTRIES_IN_CMD + 1) *
 				sizeof(u32)), GFP_KERNEL);
diff --git a/drivers/scsi/hpsa.h b/drivers/scsi/hpsa.h
index 7b28d54fa87..981647989bf 100644
--- a/drivers/scsi/hpsa.h
+++ b/drivers/scsi/hpsa.h
@@ -34,7 +34,7 @@ struct access_method {
 	void (*set_intr_mask)(struct ctlr_info *h, unsigned long val);
 	unsigned long (*fifo_full)(struct ctlr_info *h);
 	bool (*intr_pending)(struct ctlr_info *h);
-	unsigned long (*command_completed)(struct ctlr_info *h);
+	unsigned long (*command_completed)(struct ctlr_info *h, u8 q);
 };
 
 struct hpsa_scsi_dev_t {
@@ -48,6 +48,13 @@ struct hpsa_scsi_dev_t {
 	unsigned char raid_level;	/* from inquiry page 0xC1 */
 };
 
+struct reply_pool {
+	u64 *head;
+	size_t size;
+	u8 wraparound;
+	u32 current_entry;
+};
+
 struct ctlr_info {
 	int	ctlr;
 	char	devname[8];
@@ -68,7 +75,7 @@ struct ctlr_info {
 #	define DOORBELL_INT	1
 #	define SIMPLE_MODE_INT	2
 #	define MEMQ_MODE_INT	3
-	unsigned int intr[4];
+	unsigned int intr[MAX_REPLY_QUEUES];
 	unsigned int msix_vector;
 	unsigned int msi_vector;
 	int intr_mode; /* either PERF_MODE_INT or SIMPLE_MODE_INT */
@@ -78,7 +85,6 @@ struct ctlr_info {
 	struct list_head reqQ;
 	struct list_head cmpQ;
 	unsigned int Qdepth;
-	unsigned int maxQsinceinit;
 	unsigned int maxSG;
 	spinlock_t lock;
 	int maxsgentries;
@@ -111,20 +117,45 @@ struct ctlr_info {
 	unsigned long transMethod;
 
 	/*
-	 * Performant mode completion buffer
+	 * Performant mode completion buffers
 	 */
 	u64 *reply_pool;
-	dma_addr_t reply_pool_dhandle;
-	u64 *reply_pool_head;
 	size_t reply_pool_size;
-	unsigned char reply_pool_wraparound;
+	struct reply_pool reply_queue[MAX_REPLY_QUEUES];
+	u8 nreply_queues;
+	dma_addr_t reply_pool_dhandle;
 	u32 *blockFetchTable;
 	unsigned char *hba_inquiry_data;
 	u64 last_intr_timestamp;
 	u32 last_heartbeat;
 	u64 last_heartbeat_timestamp;
+	u32 heartbeat_sample_interval;
+	atomic_t firmware_flash_in_progress;
 	u32 lockup_detected;
 	struct list_head lockup_list;
+	/* Address of h->q[x] is passed to intr handler to know which queue */
+	u8 q[MAX_REPLY_QUEUES];
+	u32 TMFSupportFlags; /* cache what task mgmt funcs are supported. */
+#define HPSATMF_BITS_SUPPORTED  (1 << 0)
+#define HPSATMF_PHYS_LUN_RESET  (1 << 1)
+#define HPSATMF_PHYS_NEX_RESET  (1 << 2)
+#define HPSATMF_PHYS_TASK_ABORT (1 << 3)
+#define HPSATMF_PHYS_TSET_ABORT (1 << 4)
+#define HPSATMF_PHYS_CLEAR_ACA  (1 << 5)
+#define HPSATMF_PHYS_CLEAR_TSET (1 << 6)
+#define HPSATMF_PHYS_QRY_TASK   (1 << 7)
+#define HPSATMF_PHYS_QRY_TSET   (1 << 8)
+#define HPSATMF_PHYS_QRY_ASYNC  (1 << 9)
+#define HPSATMF_MASK_SUPPORTED  (1 << 16)
+#define HPSATMF_LOG_LUN_RESET   (1 << 17)
+#define HPSATMF_LOG_NEX_RESET   (1 << 18)
+#define HPSATMF_LOG_TASK_ABORT  (1 << 19)
+#define HPSATMF_LOG_TSET_ABORT  (1 << 20)
+#define HPSATMF_LOG_CLEAR_ACA   (1 << 21)
+#define HPSATMF_LOG_CLEAR_TSET  (1 << 22)
+#define HPSATMF_LOG_QRY_TASK    (1 << 23)
+#define HPSATMF_LOG_QRY_TSET    (1 << 24)
+#define HPSATMF_LOG_QRY_ASYNC   (1 << 25)
 };
 #define HPSA_ABORT_MSG 0
 #define HPSA_DEVICE_RESET_MSG 1
@@ -216,9 +247,6 @@ static void SA5_submit_command(struct ctlr_info *h,
 		c->Header.Tag.lower);
 	writel(c->busaddr, h->vaddr + SA5_REQUEST_PORT_OFFSET);
 	(void) readl(h->vaddr + SA5_SCRATCHPAD_OFFSET);
-	h->commands_outstanding++;
-	if (h->commands_outstanding > h->max_outstanding)
-		h->max_outstanding = h->commands_outstanding;
 }
 
 /*
@@ -254,16 +282,17 @@ static void SA5_performant_intr_mask(struct ctlr_info *h, unsigned long val)
 	}
 }
 
-static unsigned long SA5_performant_completed(struct ctlr_info *h)
+static unsigned long SA5_performant_completed(struct ctlr_info *h, u8 q)
 {
-	unsigned long register_value = FIFO_EMPTY;
+	struct reply_pool *rq = &h->reply_queue[q];
+	unsigned long flags, register_value = FIFO_EMPTY;
 
-	/* flush the controller write of the reply queue by reading
-	 * outbound doorbell status register.
-	 */
-	register_value = readl(h->vaddr + SA5_OUTDB_STATUS);
 	/* msi auto clears the interrupt pending bit. */
 	if (!(h->msi_vector || h->msix_vector)) {
+		/* flush the controller write of the reply queue by reading
+		 * outbound doorbell status register.
+		 */
+		register_value = readl(h->vaddr + SA5_OUTDB_STATUS);
 		writel(SA5_OUTDB_CLEAR_PERF_BIT, h->vaddr + SA5_OUTDB_CLEAR);
 		/* Do a read in order to flush the write to the controller
 		 * (as per spec.)
@@ -271,19 +300,20 @@ static unsigned long SA5_performant_completed(struct ctlr_info *h)
 		register_value = readl(h->vaddr + SA5_OUTDB_STATUS);
 	}
 
-	if ((*(h->reply_pool_head) & 1) == (h->reply_pool_wraparound)) {
-		register_value = *(h->reply_pool_head);
-		(h->reply_pool_head)++;
+	if ((rq->head[rq->current_entry] & 1) == rq->wraparound) {
+		register_value = rq->head[rq->current_entry];
+		rq->current_entry++;
+		spin_lock_irqsave(&h->lock, flags);
 		h->commands_outstanding--;
+		spin_unlock_irqrestore(&h->lock, flags);
 	} else {
 		register_value = FIFO_EMPTY;
 	}
 	/* Check for wraparound */
-	if (h->reply_pool_head == (h->reply_pool + h->max_commands)) {
-		h->reply_pool_head = h->reply_pool;
-		h->reply_pool_wraparound ^= 1;
+	if (rq->current_entry == h->max_commands) {
+		rq->current_entry = 0;
+		rq->wraparound ^= 1;
 	}
-
 	return register_value;
 }
 
@@ -303,13 +333,18 @@ static unsigned long SA5_fifo_full(struct ctlr_info *h)
  *   returns value read from hardware.
  *     returns FIFO_EMPTY if there is nothing to read
  */
-static unsigned long SA5_completed(struct ctlr_info *h)
+static unsigned long SA5_completed(struct ctlr_info *h,
+	__attribute__((unused)) u8 q)
 {
 	unsigned long register_value
 		= readl(h->vaddr + SA5_REPLY_PORT_OFFSET);
+	unsigned long flags;
 
-	if (register_value != FIFO_EMPTY)
+	if (register_value != FIFO_EMPTY) {
+		spin_lock_irqsave(&h->lock, flags);
 		h->commands_outstanding--;
+		spin_unlock_irqrestore(&h->lock, flags);
+	}
 
 #ifdef HPSA_DEBUG
 	if (register_value != FIFO_EMPTY)
diff --git a/drivers/scsi/hpsa_cmd.h b/drivers/scsi/hpsa_cmd.h
index 8049815d8c1..a894f2eca7a 100644
--- a/drivers/scsi/hpsa_cmd.h
+++ b/drivers/scsi/hpsa_cmd.h
@@ -82,6 +82,29 @@
 #define TYPE_CMD				0x00
 #define TYPE_MSG				0x01
 
+/* Message Types  */
+#define HPSA_TASK_MANAGEMENT    0x00
+#define HPSA_RESET              0x01
+#define HPSA_SCAN               0x02
+#define HPSA_NOOP               0x03
+
+#define HPSA_CTLR_RESET_TYPE    0x00
+#define HPSA_BUS_RESET_TYPE     0x01
+#define HPSA_TARGET_RESET_TYPE  0x03
+#define HPSA_LUN_RESET_TYPE     0x04
+#define HPSA_NEXUS_RESET_TYPE   0x05
+
+/* Task Management Functions */
+#define HPSA_TMF_ABORT_TASK     0x00
+#define HPSA_TMF_ABORT_TASK_SET 0x01
+#define HPSA_TMF_CLEAR_ACA      0x02
+#define HPSA_TMF_CLEAR_TASK_SET 0x03
+#define HPSA_TMF_QUERY_TASK     0x04
+#define HPSA_TMF_QUERY_TASK_SET 0x05
+#define HPSA_TMF_QUERY_ASYNCEVENT 0x06
+
+
+
 /* config space register offsets */
 #define CFG_VENDORID            0x00
 #define CFG_DEVICEID            0x02
@@ -106,6 +129,7 @@
 #define CFGTBL_Trans_Simple     0x00000002l
 #define CFGTBL_Trans_Performant 0x00000004l
 #define CFGTBL_Trans_use_short_tags 0x20000000l
+#define CFGTBL_Trans_enable_directed_msix (1 << 30)
 
 #define CFGTBL_BusType_Ultra2   0x00000001l
 #define CFGTBL_BusType_Ultra3   0x00000002l
@@ -162,6 +186,7 @@ struct SenseSubsystem_info {
 #define BMIC_WRITE 0x27
 #define BMIC_CACHE_FLUSH 0xc2
 #define HPSA_CACHE_FLUSH 0x01	/* C2 was already being used by HPSA */
+#define BMIC_FLASH_FIRMWARE 0xF7
 
 /* Command List Structure */
 union SCSI3Addr {
@@ -337,11 +362,17 @@ struct CfgTable {
 	u32		MaxPhysicalDevices;
 	u32		MaxPhysicalDrivesPerLogicalUnit;
 	u32		MaxPerformantModeCommands;
-	u8		reserved[0x78 - 0x58];
+	u32		MaxBlockFetch;
+	u32		PowerConservationSupport;
+	u32		PowerConservationEnable;
+	u32		TMFSupportFlags;
+	u8		TMFTagMask[8];
+	u8		reserved[0x78 - 0x70];
 	u32		misc_fw_support; /* offset 0x78 */
 #define			MISC_FW_DOORBELL_RESET (0x02)
 #define			MISC_FW_DOORBELL_RESET2 (0x010)
 	u8		driver_version[32];
+
 };
 
 #define NUM_BLOCKFETCH_ENTRIES 8
@@ -351,8 +382,8 @@ struct TransTable_struct {
 	u32            RepQCount;
 	u32            RepQCtrAddrLow32;
 	u32            RepQCtrAddrHigh32;
-	u32            RepQAddr0Low32;
-	u32            RepQAddr0High32;
+#define MAX_REPLY_QUEUES 8
+	struct vals32  RepQAddr[MAX_REPLY_QUEUES];
 };
 
 struct hpsa_pci_info {
diff --git a/drivers/scsi/ibmmca.c b/drivers/scsi/ibmmca.c
deleted file mode 100644
index cd09132d5d7..00000000000
--- a/drivers/scsi/ibmmca.c
+++ /dev/null
@@ -1,2379 +0,0 @@
-/*
- Low Level Linux Driver for the IBM Microchannel SCSI Subsystem for
- Linux Kernel >= 2.4.0.
- Copyright (c) 1995 Strom Systems, Inc. under the terms of the GNU
- General Public License. Written by Martin Kolinek, December 1995.
- Further development by: Chris Beauregard, Klaus Kudielka, Michael Lang
- See the file Documentation/scsi/ibmmca.txt for a detailed description
- of this driver, the commandline arguments and the history of its
- development.
- See the WWW-page: http://www.uni-mainz.de/~langm000/linux.html for latest
- updates, info and ADF-files for adapters supported by this driver.
-
- Alan Cox <alan@lxorguk.ukuu.org.uk>
- Updated for Linux 2.5.45 to use the new error handler, cleaned up the
- lock macros and did a few unavoidable locking tweaks, plus one locking
- fix in the irq and completion path.
- 
- */
-
-#include <linux/module.h>
-#include <linux/kernel.h>
-#include <linux/types.h>
-#include <linux/ctype.h>
-#include <linux/string.h>
-#include <linux/interrupt.h>
-#include <linux/ioport.h>
-#include <linux/delay.h>
-#include <linux/blkdev.h>
-#include <linux/proc_fs.h>
-#include <linux/stat.h>
-#include <linux/mca.h>
-#include <linux/spinlock.h>
-#include <linux/init.h>
-
-#include <asm/io.h>
-
-#include "scsi.h"
-#include <scsi/scsi_host.h>
-
-/* Common forward declarations for all Linux-versions: */
-static int ibmmca_queuecommand (struct Scsi_Host *, struct scsi_cmnd *);
-static int ibmmca_abort (Scsi_Cmnd *);
-static int ibmmca_host_reset (Scsi_Cmnd *);
-static int ibmmca_biosparam (struct scsi_device *, struct block_device *, sector_t, int *);
-static int ibmmca_proc_info(struct Scsi_Host *shpnt, char *buffer, char **start, off_t offset, int length, int inout);
-
-
-
-/* current version of this driver-source: */
-#define IBMMCA_SCSI_DRIVER_VERSION "4.0b-ac"
-
-/* driver configuration */
-#define IM_MAX_HOSTS     8	/* maximum number of host adapters */
-#define IM_RESET_DELAY	60	/* seconds allowed for a reset */
-
-/* driver debugging - #undef all for normal operation */
-/* if defined: count interrupts and ignore this special one: */
-#undef	IM_DEBUG_TIMEOUT	//50
-#define TIMEOUT_PUN	0
-#define TIMEOUT_LUN	0
-/* verbose interrupt: */
-#undef IM_DEBUG_INT
-/* verbose queuecommand: */
-#undef IM_DEBUG_CMD
-/* verbose queucommand for specific SCSI-device type: */
-#undef IM_DEBUG_CMD_SPEC_DEV
-/* verbose device probing */
-#undef IM_DEBUG_PROBE
-
-/* device type that shall be displayed on syslog (only during debugging): */
-#define IM_DEBUG_CMD_DEVICE	TYPE_TAPE
-
-/* relative addresses of hardware registers on a subsystem */
-#define IM_CMD_REG(h)	((h)->io_port)	/*Command Interface, (4 bytes long) */
-#define IM_ATTN_REG(h)	((h)->io_port+4)	/*Attention (1 byte) */
-#define IM_CTR_REG(h)	((h)->io_port+5)	/*Basic Control (1 byte) */
-#define IM_INTR_REG(h)	((h)->io_port+6)	/*Interrupt Status (1 byte, r/o) */
-#define IM_STAT_REG(h)	((h)->io_port+7)	/*Basic Status (1 byte, read only) */
-
-/* basic I/O-port of first adapter */
-#define IM_IO_PORT	0x3540
-/* maximum number of hosts that can be found */
-#define IM_N_IO_PORT	8
-
-/*requests going into the upper nibble of the Attention register */
-/*note: the lower nibble specifies the device(0-14), or subsystem(15) */
-#define IM_IMM_CMD	0x10	/*immediate command */
-#define IM_SCB		0x30	/*Subsystem Control Block command */
-#define IM_LONG_SCB	0x40	/*long Subsystem Control Block command */
-#define IM_EOI		0xe0	/*end-of-interrupt request */
-
-/*values for bits 7,1,0 of Basic Control reg. (bits 6-2 reserved) */
-#define IM_HW_RESET	0x80	/*hardware reset */
-#define IM_ENABLE_DMA	0x02	/*enable subsystem's busmaster DMA */
-#define IM_ENABLE_INTR	0x01	/*enable interrupts to the system */
-
-/*to interpret the upper nibble of Interrupt Status register */
-/*note: the lower nibble specifies the device(0-14), or subsystem(15) */
-#define IM_SCB_CMD_COMPLETED			0x10
-#define IM_SCB_CMD_COMPLETED_WITH_RETRIES	0x50
-#define IM_LOOP_SCATTER_BUFFER_FULL		0x60
-#define IM_ADAPTER_HW_FAILURE			0x70
-#define IM_IMMEDIATE_CMD_COMPLETED		0xa0
-#define IM_CMD_COMPLETED_WITH_FAILURE		0xc0
-#define IM_CMD_ERROR				0xe0
-#define IM_SOFTWARE_SEQUENCING_ERROR		0xf0
-
-/*to interpret bits 3-0 of Basic Status register (bits 7-4 reserved) */
-#define IM_CMD_REG_FULL		0x08
-#define IM_CMD_REG_EMPTY	0x04
-#define IM_INTR_REQUEST		0x02
-#define IM_BUSY			0x01
-
-/*immediate commands (word written into low 2 bytes of command reg) */
-#define IM_RESET_IMM_CMD	0x0400
-#define IM_FEATURE_CTR_IMM_CMD	0x040c
-#define IM_DMA_PACING_IMM_CMD	0x040d
-#define IM_ASSIGN_IMM_CMD	0x040e
-#define IM_ABORT_IMM_CMD	0x040f
-#define IM_FORMAT_PREP_IMM_CMD	0x0417
-
-/*SCB (Subsystem Control Block) structure */
-struct im_scb {
-	unsigned short command;	/*command word (read, etc.) */
-	unsigned short enable;	/*enable word, modifies cmd */
-	union {
-		unsigned long log_blk_adr;	/*block address on SCSI device */
-		unsigned char scsi_cmd_length;	/*6,10,12, for other scsi cmd */
-	} u1;
-	unsigned long sys_buf_adr;	/*physical system memory adr */
-	unsigned long sys_buf_length;	/*size of sys mem buffer */
-	unsigned long tsb_adr;	/*Termination Status Block adr */
-	unsigned long scb_chain_adr;	/*optional SCB chain address */
-	union {
-		struct {
-			unsigned short count;	/*block count, on SCSI device */
-			unsigned short length;	/*block length, on SCSI device */
-		} blk;
-		unsigned char scsi_command[12];	/*other scsi command */
-	} u2;
-};
-
-/*structure scatter-gather element (for list of system memory areas) */
-struct im_sge {
-	void *address;
-	unsigned long byte_length;
-};
-
-/*structure returned by a get_pos_info command: */
-struct im_pos_info {
-	unsigned short pos_id;	/* adapter id */
-	unsigned char pos_3a;	/* pos 3 (if pos 6 = 0) */
-	unsigned char pos_2;	/* pos 2 */
-	unsigned char int_level;	/* interrupt level IRQ 11 or 14 */
-	unsigned char pos_4a;	/* pos 4 (if pos 6 = 0) */
-	unsigned short connector_size;	/* MCA connector size: 16 or 32 Bit */
-	unsigned char num_luns;	/* number of supported luns per device */
-	unsigned char num_puns;	/* number of supported puns */
-	unsigned char pacing_factor;	/* pacing factor */
-	unsigned char num_ldns;	/* number of ldns available */
-	unsigned char eoi_off;	/* time EOI and interrupt inactive */
-	unsigned char max_busy;	/* time between reset and busy on */
-	unsigned short cache_stat;	/* ldn cachestat. Bit=1 = not cached */
-	unsigned short retry_stat;	/* retry status of ldns. Bit=1=disabled */
-	unsigned char pos_4b;	/* pos 4 (if pos 6 = 1) */
-	unsigned char pos_3b;	/* pos 3 (if pos 6 = 1) */
-	unsigned char pos_6;	/* pos 6 */
-	unsigned char pos_5;	/* pos 5 */
-	unsigned short max_overlap;	/* maximum overlapping requests */
-	unsigned short num_bus;	/* number of SCSI-busses */
-};
-
-/*values for SCB command word */
-#define IM_NO_SYNCHRONOUS      0x0040	/*flag for any command */
-#define IM_NO_DISCONNECT       0x0080	/*flag for any command */
-#define IM_READ_DATA_CMD       0x1c01
-#define IM_WRITE_DATA_CMD      0x1c02
-#define IM_READ_VERIFY_CMD     0x1c03
-#define IM_WRITE_VERIFY_CMD    0x1c04
-#define IM_REQUEST_SENSE_CMD   0x1c08
-#define IM_READ_CAPACITY_CMD   0x1c09
-#define IM_DEVICE_INQUIRY_CMD  0x1c0b
-#define IM_READ_LOGICAL_CMD    0x1c2a
-#define IM_OTHER_SCSI_CMD_CMD  0x241f
-
-/* unused, but supported, SCB commands */
-#define IM_GET_COMMAND_COMPLETE_STATUS_CMD   0x1c07	/* command status */
-#define IM_GET_POS_INFO_CMD                  0x1c0a	/* returns neat stuff */
-#define IM_READ_PREFETCH_CMD                 0x1c31	/* caching controller only */
-#define IM_FOMAT_UNIT_CMD                    0x1c16	/* format unit */
-#define IM_REASSIGN_BLOCK_CMD                0x1c18	/* in case of error */
-
-/*values to set bits in the enable word of SCB */
-#define IM_READ_CONTROL              0x8000
-#define IM_REPORT_TSB_ONLY_ON_ERROR  0x4000
-#define IM_RETRY_ENABLE              0x2000
-#define IM_POINTER_TO_LIST           0x1000
-#define IM_SUPRESS_EXCEPTION_SHORT   0x0400
-#define IM_BYPASS_BUFFER             0x0200
-#define IM_CHAIN_ON_NO_ERROR         0x0001
-
-/*TSB (Termination Status Block) structure */
-struct im_tsb {
-	unsigned short end_status;
-	unsigned short reserved1;
-	unsigned long residual_byte_count;
-	unsigned long sg_list_element_adr;
-	unsigned short status_length;
-	unsigned char dev_status;
-	unsigned char cmd_status;
-	unsigned char dev_error;
-	unsigned char cmd_error;
-	unsigned short reserved2;
-	unsigned short reserved3;
-	unsigned short low_of_last_scb_adr;
-	unsigned short high_of_last_scb_adr;
-};
-
-/*subsystem uses interrupt request level 14 */
-#define IM_IRQ     14
-/*SCSI-2 F/W may evade to interrupt 11 */
-#define IM_IRQ_FW  11
-
-/* Model 95 has an additional alphanumeric display, which can be used
-   to display SCSI-activities. 8595 models do not have any disk led, which
-   makes this feature quite useful.
-   The regular PS/2 disk led is turned on/off by bits 6,7 of system
-   control port. */
-
-/* LED display-port (actually, last LED on display) */
-#define MOD95_LED_PORT	   0x108
-/* system-control-register of PS/2s with diskindicator */
-#define PS2_SYS_CTR        0x92
-/* activity displaying methods */
-#define LED_DISP           1
-#define LED_ADISP          2
-#define LED_ACTIVITY       4
-/* failed intr */
-#define CMD_FAIL           255
-
-/* The SCSI-ID(!) of the accessed SCSI-device is shown on PS/2-95 machines' LED
-   displays. ldn is no longer displayed here, because the ldn mapping is now 
-   done dynamically and the ldn <-> pun,lun maps can be looked-up at boottime 
-   or during uptime in /proc/scsi/ibmmca/<host_no> in case of trouble, 
-   interest, debugging or just for having fun. The left number gives the
-   host-adapter number and the right shows the accessed SCSI-ID. */
-
-/* display_mode is set by the ibmmcascsi= command line arg */
-static int display_mode = 0;
-/* set default adapter timeout */
-static unsigned int adapter_timeout = 45;
-/* for probing on feature-command: */
-static unsigned int global_command_error_excuse = 0;
-/* global setting by command line for adapter_speed */
-static int global_adapter_speed = 0;	/* full speed by default */
-
-/* Panel / LED on, do it right for F/W addressin, too. adisplay will
- * just ignore ids>7, as the panel has only 7 digits available */
-#define PS2_DISK_LED_ON(ad,id) { if (display_mode & LED_DISP) { if (id>9) \
-    outw((ad+48)|((id+55)<<8), MOD95_LED_PORT ); else \
-    outw((ad+48)|((id+48)<<8), MOD95_LED_PORT ); } else \
-    if (display_mode & LED_ADISP) { if (id<7) outb((char)(id+48),MOD95_LED_PORT+1+id); \
-    outb((char)(ad+48), MOD95_LED_PORT); } \
-    if ((display_mode & LED_ACTIVITY)||(!display_mode)) \
-    outb(inb(PS2_SYS_CTR) | 0xc0, PS2_SYS_CTR); }
-
-/* Panel / LED off */
-/* bug fixed, Dec 15, 1997, where | was replaced by & here */
-#define PS2_DISK_LED_OFF() { if (display_mode & LED_DISP) \
-    outw(0x2020, MOD95_LED_PORT ); else if (display_mode & LED_ADISP) { \
-    outl(0x20202020,MOD95_LED_PORT); outl(0x20202020,MOD95_LED_PORT+4); } \
-    if ((display_mode & LED_ACTIVITY)||(!display_mode)) \
-    outb(inb(PS2_SYS_CTR) & 0x3f, PS2_SYS_CTR); }
-
-/* types of different supported hardware that goes to hostdata special */
-#define IBM_SCSI2_FW     0
-#define IBM_7568_WCACHE  1
-#define IBM_EXP_UNIT     2
-#define IBM_SCSI_WCACHE  3
-#define IBM_SCSI         4
-#define IBM_INTEGSCSI	 5
-
-/* other special flags for hostdata structure */
-#define FORCED_DETECTION         100
-#define INTEGRATED_SCSI          101
-
-/* List of possible IBM-SCSI-adapters */
-static short ibmmca_id_table[] = {
-	0x8efc,
-	0x8efd,
-	0x8ef8,
-	0x8eff,
-	0x8efe,
-	/* No entry for integrated SCSI, that's part of the register */
-	0
-};
-
-static const char *ibmmca_description[] = {
-	"IBM SCSI-2 F/W Adapter",	/* special = 0 */
-	"IBM 7568 Industrial Computer SCSI Adapter w/Cache",	/* special = 1 */
-	"IBM Expansion Unit SCSI Controller",	/* special = 2 */
-	"IBM SCSI Adapter w/Cache",	/* special = 3 */
-	"IBM SCSI Adapter",	/* special = 4 */
-	"IBM Integrated SCSI Controller", /* special = 5 */
-};
-
-/* Max number of logical devices (can be up from 0 to 14).  15 is the address
-of the adapter itself. */
-#define MAX_LOG_DEV  15
-
-/*local data for a logical device */
-struct logical_device {
-	struct im_scb scb;	/* SCSI-subsystem-control-block structure */
-	struct im_tsb tsb;	/* SCSI command complete status block structure */
-	struct im_sge sge[16];	/* scatter gather list structure */
-	unsigned char buf[256];	/* SCSI command return data buffer */
-	Scsi_Cmnd *cmd;		/* SCSI-command that is currently in progress */
-	int device_type;	/* type of the SCSI-device. See include/scsi/scsi.h
-				   for interpretation of the possible values */
-	int block_length;	/* blocksize of a particular logical SCSI-device */
-	int cache_flag;		/* 1 if this is uncached, 0 if cache is present for ldn */
-	int retry_flag;		/* 1 if adapter retry is disabled, 0 if enabled */
-};
-
-/* statistics of the driver during operations (for proc_info) */
-struct Driver_Statistics {
-	/* SCSI statistics on the adapter */
-	int ldn_access[MAX_LOG_DEV + 1];	/* total accesses on a ldn */
-	int ldn_read_access[MAX_LOG_DEV + 1];	/* total read-access on a ldn */
-	int ldn_write_access[MAX_LOG_DEV + 1];	/* total write-access on a ldn */
-	int ldn_inquiry_access[MAX_LOG_DEV + 1];	/* total inquiries on a ldn */
-	int ldn_modeselect_access[MAX_LOG_DEV + 1];	/* total mode selects on ldn */
-	int scbs;		/* short SCBs queued */
-	int long_scbs;		/* long SCBs queued */
-	int total_accesses;	/* total accesses on all ldns */
-	int total_interrupts;	/* total interrupts (should be
-				   same as total_accesses) */
-	int total_errors;	/* command completed with error */
-	/* dynamical assignment statistics */
-	int total_scsi_devices;	/* number of physical pun,lun */
-	int dyn_flag;		/* flag showing dynamical mode */
-	int dynamical_assignments;	/* number of remappings of ldns */
-	int ldn_assignments[MAX_LOG_DEV + 1];	/* number of remappings of each
-						   ldn */
-};
-
-/* data structure for each host adapter */
-struct ibmmca_hostdata {
-	/* array of logical devices: */
-	struct logical_device _ld[MAX_LOG_DEV + 1];
-	/* array to convert (pun, lun) into logical device number: */
-	unsigned char _get_ldn[16][8];
-	/*array that contains the information about the physical SCSI-devices
-	   attached to this host adapter: */
-	unsigned char _get_scsi[16][8];
-	/* used only when checking logical devices: */
-	int _local_checking_phase_flag;
-	/* report received interrupt: */
-	int _got_interrupt;
-	/* report termination-status of SCSI-command: */
-	int _stat_result;
-	/* reset status (used only when doing reset): */
-	int _reset_status;
-	/* code of the last SCSI command (needed for panic info): */
-	int _last_scsi_command[MAX_LOG_DEV + 1];
-	/* identifier of the last SCSI-command type */
-	int _last_scsi_type[MAX_LOG_DEV + 1];
-	/* last blockcount */
-	int _last_scsi_blockcount[MAX_LOG_DEV + 1];
-	/* last locgical block address */
-	unsigned long _last_scsi_logical_block[MAX_LOG_DEV + 1];
-	/* Counter that points on the next reassignable ldn for dynamical
-	   remapping. The default value is 7, that is the first reassignable
-	   number in the list at boottime: */
-	int _next_ldn;
-	/* Statistics-structure for this IBM-SCSI-host: */
-	struct Driver_Statistics _IBM_DS;
-	/* This hostadapters pos-registers pos2 until pos6 */
-	unsigned int _pos[8];
-	/* assign a special variable, that contains dedicated info about the
-	   adaptertype */
-	int _special;
-	/* connector size on the MCA bus */
-	int _connector_size;
-	/* synchronous SCSI transfer rate bitpattern */
-	int _adapter_speed;
-};
-
-/* macros to access host data structure */
-#define subsystem_pun(h) ((h)->this_id)
-#define subsystem_maxid(h) ((h)->max_id)
-#define ld(h) (((struct ibmmca_hostdata *) (h)->hostdata)->_ld)
-#define get_ldn(h) (((struct ibmmca_hostdata *) (h)->hostdata)->_get_ldn)
-#define get_scsi(h) (((struct ibmmca_hostdata *) (h)->hostdata)->_get_scsi)
-#define local_checking_phase_flag(h) (((struct ibmmca_hostdata *) (h)->hostdata)->_local_checking_phase_flag)
-#define got_interrupt(h) (((struct ibmmca_hostdata *) (h)->hostdata)->_got_interrupt)
-#define stat_result(h) (((struct ibmmca_hostdata *) (h)->hostdata)->_stat_result)
-#define reset_status(h) (((struct ibmmca_hostdata *) (h)->hostdata)->_reset_status)
-#define last_scsi_command(h) (((struct ibmmca_hostdata *) (h)->hostdata)->_last_scsi_command)
-#define last_scsi_type(h) (((struct ibmmca_hostdata *) (h)->hostdata)->_last_scsi_type)
-#define last_scsi_blockcount(h) (((struct ibmmca_hostdata *) (h)->hostdata)->_last_scsi_blockcount)
-#define last_scsi_logical_block(h) (((struct ibmmca_hostdata *) (h)->hostdata)->_last_scsi_logical_block)
-#define last_scsi_type(h) (((struct ibmmca_hostdata *) (h)->hostdata)->_last_scsi_type)
-#define next_ldn(h) (((struct ibmmca_hostdata *) (h)->hostdata)->_next_ldn)
-#define IBM_DS(h) (((struct ibmmca_hostdata *) (h)->hostdata)->_IBM_DS)
-#define special(h) (((struct ibmmca_hostdata *) (h)->hostdata)->_special)
-#define subsystem_connector_size(h) (((struct ibmmca_hostdata *) (h)->hostdata)->_connector_size)
-#define adapter_speed(h) (((struct ibmmca_hostdata *) (h)->hostdata)->_adapter_speed)
-#define pos2(h) (((struct ibmmca_hostdata *) (h)->hostdata)->_pos[2])
-#define pos3(h) (((struct ibmmca_hostdata *) (h)->hostdata)->_pos[3])
-#define pos4(h) (((struct ibmmca_hostdata *) (h)->hostdata)->_pos[4])
-#define pos5(h) (((struct ibmmca_hostdata *) (h)->hostdata)->_pos[5])
-#define pos6(h) (((struct ibmmca_hostdata *) (h)->hostdata)->_pos[6])
-
-/* Define a arbitrary number as subsystem-marker-type. This number is, as
-   described in the ANSI-SCSI-standard, not occupied by other device-types. */
-#define TYPE_IBM_SCSI_ADAPTER   0x2F
-
-/* Define 0xFF for no device type, because this type is not defined within
-   the ANSI-SCSI-standard, therefore, it can be used and should not cause any
-   harm. */
-#define TYPE_NO_DEVICE          0xFF
-
-/* define medium-changer. If this is not defined previously, e.g. Linux
-   2.0.x, define this type here. */
-#ifndef TYPE_MEDIUM_CHANGER
-#define TYPE_MEDIUM_CHANGER     0x08
-#endif
-
-/* define possible operations for the immediate_assign command */
-#define SET_LDN        0
-#define REMOVE_LDN     1
-
-/* ldn which is used to probe the SCSI devices */
-#define PROBE_LDN      0
-
-/* reset status flag contents */
-#define IM_RESET_NOT_IN_PROGRESS         0
-#define IM_RESET_IN_PROGRESS             1
-#define IM_RESET_FINISHED_OK             2
-#define IM_RESET_FINISHED_FAIL           3
-#define IM_RESET_NOT_IN_PROGRESS_NO_INT  4
-#define IM_RESET_FINISHED_OK_NO_INT      5
-
-/* define undefined SCSI-command */
-#define NO_SCSI                  0xffff
-
-/*-----------------------------------------------------------------------*/
-
-/* if this is nonzero, ibmmcascsi option has been passed to the kernel */
-static int io_port[IM_MAX_HOSTS] = { 0, 0, 0, 0, 0, 0, 0, 0 };
-static int scsi_id[IM_MAX_HOSTS] = { 7, 7, 7, 7, 7, 7, 7, 7 };
-
-/* fill module-parameters only, when this define is present.
-   (that is kernel version 2.1.x) */
-#if defined(MODULE)
-static char *boot_options = NULL;
-module_param(boot_options, charp, 0);
-module_param_array(io_port, int, NULL, 0);
-module_param_array(scsi_id, int, NULL, 0);
-
-MODULE_LICENSE("GPL");
-#endif
-/*counter of concurrent disk read/writes, to turn on/off disk led */
-static int disk_rw_in_progress = 0;
-
-static unsigned int pos[8];	/* whole pos register-line for diagnosis */
-/* Taking into account the additions, made by ZP Gu.
- * This selects now the preset value from the configfile and
- * offers the 'normal' commandline option to be accepted */
-#ifdef CONFIG_IBMMCA_SCSI_ORDER_STANDARD
-static char ibm_ansi_order = 1;
-#else
-static char ibm_ansi_order = 0;
-#endif
-
-static void issue_cmd(struct Scsi_Host *, unsigned long, unsigned char);
-static void internal_done(Scsi_Cmnd * cmd);
-static void check_devices(struct Scsi_Host *, int);
-static int immediate_assign(struct Scsi_Host *, unsigned int, unsigned int, unsigned int, unsigned int);
-static int immediate_feature(struct Scsi_Host *, unsigned int, unsigned int);
-#ifdef CONFIG_IBMMCA_SCSI_DEV_RESET
-static int immediate_reset(struct Scsi_Host *, unsigned int);
-#endif
-static int device_inquiry(struct Scsi_Host *, int);
-static int read_capacity(struct Scsi_Host *, int);
-static int get_pos_info(struct Scsi_Host *);
-static char *ti_p(int);
-static char *ti_l(int);
-static char *ibmrate(unsigned int, int);
-static int probe_display(int);
-static int probe_bus_mode(struct Scsi_Host *);
-static int device_exists(struct Scsi_Host *, int, int *, int *);
-static int option_setup(char *);
-/* local functions needed for proc_info */
-static int ldn_access_load(struct Scsi_Host *, int);
-static int ldn_access_total_read_write(struct Scsi_Host *);
-
-static irqreturn_t interrupt_handler(int irq, void *dev_id)
-{
-	unsigned int intr_reg;
-	unsigned int cmd_result;
-	unsigned int ldn;
-	unsigned long flags;
-	Scsi_Cmnd *cmd;
-	int lastSCSI;
-	struct device *dev = dev_id;
-	struct Scsi_Host *shpnt = dev_get_drvdata(dev);
-
-	spin_lock_irqsave(shpnt->host_lock, flags);
-
-	if(!(inb(IM_STAT_REG(shpnt)) & IM_INTR_REQUEST)) {
-		spin_unlock_irqrestore(shpnt->host_lock, flags);
-		return IRQ_NONE;
-	}
-
-	/* the reset-function already did all the job, even ints got
-	   renabled on the subsystem, so just return */
-	if ((reset_status(shpnt) == IM_RESET_NOT_IN_PROGRESS_NO_INT) || (reset_status(shpnt) == IM_RESET_FINISHED_OK_NO_INT)) {
-		reset_status(shpnt) = IM_RESET_NOT_IN_PROGRESS;
-		spin_unlock_irqrestore(shpnt->host_lock, flags);
-		return IRQ_HANDLED;
-	}
-
-	/*must wait for attention reg not busy, then send EOI to subsystem */
-	while (1) {
-		if (!(inb(IM_STAT_REG(shpnt)) & IM_BUSY))
-			break;
-		cpu_relax();
-	}
-
-	/*get command result and logical device */
-	intr_reg = (unsigned char) (inb(IM_INTR_REG(shpnt)));
-	cmd_result = intr_reg & 0xf0;
-	ldn = intr_reg & 0x0f;
-	/* get the last_scsi_command here */
-	lastSCSI = last_scsi_command(shpnt)[ldn];
-	outb(IM_EOI | ldn, IM_ATTN_REG(shpnt));
-	
-	/*these should never happen (hw fails, or a local programming bug) */
-	if (!global_command_error_excuse) {
-		switch (cmd_result) {
-			/* Prevent from Ooopsing on error to show the real reason */
-		case IM_ADAPTER_HW_FAILURE:
-		case IM_SOFTWARE_SEQUENCING_ERROR:
-		case IM_CMD_ERROR:
-			printk(KERN_ERR "IBM MCA SCSI: Fatal Subsystem ERROR!\n");
-			printk(KERN_ERR "              Last cmd=0x%x, ena=%x, len=", lastSCSI, ld(shpnt)[ldn].scb.enable);
-			if (ld(shpnt)[ldn].cmd)
-				printk("%ld/%ld,", (long) (scsi_bufflen(ld(shpnt)[ldn].cmd)), (long) (ld(shpnt)[ldn].scb.sys_buf_length));
-			else
-				printk("none,");
-			if (ld(shpnt)[ldn].cmd)
-				printk("Blocksize=%d", ld(shpnt)[ldn].scb.u2.blk.length);
-			else
-				printk("Blocksize=none");
-			printk(", host=%p, ldn=0x%x\n", shpnt, ldn);
-			if (ld(shpnt)[ldn].cmd) {
-				printk(KERN_ERR "Blockcount=%d/%d\n", last_scsi_blockcount(shpnt)[ldn], ld(shpnt)[ldn].scb.u2.blk.count);
-				printk(KERN_ERR "Logical block=%lx/%lx\n", last_scsi_logical_block(shpnt)[ldn], ld(shpnt)[ldn].scb.u1.log_blk_adr);
-			}
-			printk(KERN_ERR "Reason given: %s\n", (cmd_result == IM_ADAPTER_HW_FAILURE) ? "HARDWARE FAILURE" : (cmd_result == IM_SOFTWARE_SEQUENCING_ERROR) ? "SOFTWARE SEQUENCING ERROR" : (cmd_result == IM_CMD_ERROR) ? "COMMAND ERROR" : "UNKNOWN");
-			/* if errors appear, enter this section to give detailed info */
-			printk(KERN_ERR "IBM MCA SCSI: Subsystem Error-Status follows:\n");
-			printk(KERN_ERR "              Command Type................: %x\n", last_scsi_type(shpnt)[ldn]);
-			printk(KERN_ERR "              Attention Register..........: %x\n", inb(IM_ATTN_REG(shpnt)));
-			printk(KERN_ERR "              Basic Control Register......: %x\n", inb(IM_CTR_REG(shpnt)));
-			printk(KERN_ERR "              Interrupt Status Register...: %x\n", intr_reg);
-			printk(KERN_ERR "              Basic Status Register.......: %x\n", inb(IM_STAT_REG(shpnt)));
-			if ((last_scsi_type(shpnt)[ldn] == IM_SCB) || (last_scsi_type(shpnt)[ldn] == IM_LONG_SCB)) {
-				printk(KERN_ERR "              SCB-Command.................: %x\n", ld(shpnt)[ldn].scb.command);
-				printk(KERN_ERR "              SCB-Enable..................: %x\n", ld(shpnt)[ldn].scb.enable);
-				printk(KERN_ERR "              SCB-logical block address...: %lx\n", ld(shpnt)[ldn].scb.u1.log_blk_adr);
-				printk(KERN_ERR "              SCB-system buffer address...: %lx\n", ld(shpnt)[ldn].scb.sys_buf_adr);
-				printk(KERN_ERR "              SCB-system buffer length....: %lx\n", ld(shpnt)[ldn].scb.sys_buf_length);
-				printk(KERN_ERR "              SCB-tsb address.............: %lx\n", ld(shpnt)[ldn].scb.tsb_adr);
-				printk(KERN_ERR "              SCB-Chain address...........: %lx\n", ld(shpnt)[ldn].scb.scb_chain_adr);
-				printk(KERN_ERR "              SCB-block count.............: %x\n", ld(shpnt)[ldn].scb.u2.blk.count);
-				printk(KERN_ERR "              SCB-block length............: %x\n", ld(shpnt)[ldn].scb.u2.blk.length);
-			}
-			printk(KERN_ERR "              Send this report to the maintainer.\n");
-			panic("IBM MCA SCSI: Fatal error message from the subsystem (0x%X,0x%X)!\n", lastSCSI, cmd_result);
-			break;
-		}
-	} else {
-		/* The command error handling is made silent, but we tell the
-		 * calling function, that there is a reported error from the
-		 * adapter. */
-		switch (cmd_result) {
-		case IM_ADAPTER_HW_FAILURE:
-		case IM_SOFTWARE_SEQUENCING_ERROR:
-		case IM_CMD_ERROR:
-			global_command_error_excuse = CMD_FAIL;
-			break;
-		default:
-			global_command_error_excuse = 0;
-			break;
-		}
-	}
-	/* if no panic appeared, increase the interrupt-counter */
-	IBM_DS(shpnt).total_interrupts++;
-	/*only for local checking phase */
-	if (local_checking_phase_flag(shpnt)) {
-		stat_result(shpnt) = cmd_result;
-		got_interrupt(shpnt) = 1;
-		reset_status(shpnt) = IM_RESET_FINISHED_OK;
-		last_scsi_command(shpnt)[ldn] = NO_SCSI;
-		spin_unlock_irqrestore(shpnt->host_lock, flags);
-		return IRQ_HANDLED;
-	}
-	/* handling of commands coming from upper level of scsi driver */
-	if (last_scsi_type(shpnt)[ldn] == IM_IMM_CMD) {
-		/* verify ldn, and may handle rare reset immediate command */
-		if ((reset_status(shpnt) == IM_RESET_IN_PROGRESS) && (last_scsi_command(shpnt)[ldn] == IM_RESET_IMM_CMD)) {
-			if (cmd_result == IM_CMD_COMPLETED_WITH_FAILURE) {
-				disk_rw_in_progress = 0;
-				PS2_DISK_LED_OFF();
-				reset_status(shpnt) = IM_RESET_FINISHED_FAIL;
-			} else {
-				/*reset disk led counter, turn off disk led */
-				disk_rw_in_progress = 0;
-				PS2_DISK_LED_OFF();
-				reset_status(shpnt) = IM_RESET_FINISHED_OK;
-			}
-			stat_result(shpnt) = cmd_result;
-			last_scsi_command(shpnt)[ldn] = NO_SCSI;
-			last_scsi_type(shpnt)[ldn] = 0;
-			spin_unlock_irqrestore(shpnt->host_lock, flags);
-			return IRQ_HANDLED;
-		} else if (last_scsi_command(shpnt)[ldn] == IM_ABORT_IMM_CMD) {
-			/* react on SCSI abort command */
-#ifdef IM_DEBUG_PROBE
-			printk("IBM MCA SCSI: Interrupt from SCSI-abort.\n");
-#endif
-			disk_rw_in_progress = 0;
-			PS2_DISK_LED_OFF();
-			cmd = ld(shpnt)[ldn].cmd;
-			ld(shpnt)[ldn].cmd = NULL;
-			if (cmd_result == IM_CMD_COMPLETED_WITH_FAILURE)
-				cmd->result = DID_NO_CONNECT << 16;
-			else
-				cmd->result = DID_ABORT << 16;
-			stat_result(shpnt) = cmd_result;
-			last_scsi_command(shpnt)[ldn] = NO_SCSI;
-			last_scsi_type(shpnt)[ldn] = 0;
-			if (cmd->scsi_done)
-				(cmd->scsi_done) (cmd);	/* should be the internal_done */
-			spin_unlock_irqrestore(shpnt->host_lock, flags);
-			return IRQ_HANDLED;
-		} else {
-			disk_rw_in_progress = 0;
-			PS2_DISK_LED_OFF();
-			reset_status(shpnt) = IM_RESET_FINISHED_OK;
-			stat_result(shpnt) = cmd_result;
-			last_scsi_command(shpnt)[ldn] = NO_SCSI;
-			spin_unlock_irqrestore(shpnt->host_lock, flags);
-			return IRQ_HANDLED;
-		}
-	}
-	last_scsi_command(shpnt)[ldn] = NO_SCSI;
-	last_scsi_type(shpnt)[ldn] = 0;
-	cmd = ld(shpnt)[ldn].cmd;
-	ld(shpnt)[ldn].cmd = NULL;
-#ifdef IM_DEBUG_TIMEOUT
-	if (cmd) {
-		if ((cmd->target == TIMEOUT_PUN) && (cmd->device->lun == TIMEOUT_LUN)) {
-			spin_unlock_irqsave(shpnt->host_lock, flags);
-			printk("IBM MCA SCSI: Ignoring interrupt from pun=%x, lun=%x.\n", cmd->target, cmd->device->lun);
-			return IRQ_HANDLED;
-		}
-	}
-#endif
-	/*if no command structure, just return, else clear cmd */
-	if (!cmd)
-	{
-		spin_unlock_irqrestore(shpnt->host_lock, flags);
-		return IRQ_HANDLED;
-	}
-
-#ifdef IM_DEBUG_INT
-	printk("cmd=%02x ireg=%02x ds=%02x cs=%02x de=%02x ce=%02x\n", cmd->cmnd[0], intr_reg, ld(shpnt)[ldn].tsb.dev_status, ld(shpnt)[ldn].tsb.cmd_status, ld(shpnt)[ldn].tsb.dev_error, ld(shpnt)[ldn].tsb.cmd_error);
-#endif
-	/*if this is end of media read/write, may turn off PS/2 disk led */
-	if ((ld(shpnt)[ldn].device_type != TYPE_NO_LUN) && (ld(shpnt)[ldn].device_type != TYPE_NO_DEVICE)) {
-		/* only access this, if there was a valid device addressed */
-		if (--disk_rw_in_progress == 0)
-			PS2_DISK_LED_OFF();
-	}
-
-	/* IBM describes the status-mask to be 0x1e, but this is not conform
-	 * with SCSI-definition, I suppose, the reason for it is that IBM
-	 * adapters do not support CMD_TERMINATED, TASK_SET_FULL and
-	 * ACA_ACTIVE as returning statusbyte information. (ML) */
-	if (cmd_result == IM_CMD_COMPLETED_WITH_FAILURE) {
-		cmd->result = (unsigned char) (ld(shpnt)[ldn].tsb.dev_status & 0x1e);
-		IBM_DS(shpnt).total_errors++;
-	} else
-		cmd->result = 0;
-	/* write device status into cmd->result, and call done function */
-	if (lastSCSI == NO_SCSI) {	/* unexpected interrupt :-( */
-		cmd->result |= DID_BAD_INTR << 16;
-		printk("IBM MCA SCSI: WARNING - Interrupt from non-pending SCSI-command!\n");
-	} else			/* things went right :-) */
-		cmd->result |= DID_OK << 16;
-	if (cmd->scsi_done)
-		(cmd->scsi_done) (cmd);
-	spin_unlock_irqrestore(shpnt->host_lock, flags);
-	return IRQ_HANDLED;
-}
-
-static void issue_cmd(struct Scsi_Host *shpnt, unsigned long cmd_reg,
-		      unsigned char attn_reg)
-{
-	unsigned long flags;
-	/* must wait for attention reg not busy */
-	while (1) {
-		spin_lock_irqsave(shpnt->host_lock, flags);
-		if (!(inb(IM_STAT_REG(shpnt)) & IM_BUSY))
-			break;
-		spin_unlock_irqrestore(shpnt->host_lock, flags);
-	}
-	/* write registers and enable system interrupts */
-	outl(cmd_reg, IM_CMD_REG(shpnt));
-	outb(attn_reg, IM_ATTN_REG(shpnt));
-	spin_unlock_irqrestore(shpnt->host_lock, flags);
-}
-
-static void internal_done(Scsi_Cmnd * cmd)
-{
-	cmd->SCp.Status++;
-	return;
-}
-
-/* SCSI-SCB-command for device_inquiry */
-static int device_inquiry(struct Scsi_Host *shpnt, int ldn)
-{
-	int retr;
-	struct im_scb *scb;
-	struct im_tsb *tsb;
-	unsigned char *buf;
-
-	scb = &(ld(shpnt)[ldn].scb);
-	tsb = &(ld(shpnt)[ldn].tsb);
-	buf = (unsigned char *) (&(ld(shpnt)[ldn].buf));
-	ld(shpnt)[ldn].tsb.dev_status = 0;	/* prepare statusblock */
-	for (retr = 0; retr < 3; retr++) {
-		/* fill scb with inquiry command */
-		scb->command = IM_DEVICE_INQUIRY_CMD | IM_NO_DISCONNECT;
-		scb->enable = IM_REPORT_TSB_ONLY_ON_ERROR | IM_READ_CONTROL | IM_SUPRESS_EXCEPTION_SHORT | IM_RETRY_ENABLE | IM_BYPASS_BUFFER;
-		last_scsi_command(shpnt)[ldn] = IM_DEVICE_INQUIRY_CMD;
-		last_scsi_type(shpnt)[ldn] = IM_SCB;
-		scb->sys_buf_adr = isa_virt_to_bus(buf);
-		scb->sys_buf_length = 255;	/* maximum bufferlength gives max info */
-		scb->tsb_adr = isa_virt_to_bus(tsb);
-		/* issue scb to passed ldn, and busy wait for interrupt */
-		got_interrupt(shpnt) = 0;
-		issue_cmd(shpnt, isa_virt_to_bus(scb), IM_SCB | ldn);
-		while (!got_interrupt(shpnt))
-			barrier();
-
-		/*if command successful, break */
-		if ((stat_result(shpnt) == IM_SCB_CMD_COMPLETED) || (stat_result(shpnt) == IM_SCB_CMD_COMPLETED_WITH_RETRIES))
-			return 1;
-	}
-	/*if all three retries failed, return "no device at this ldn" */
-	if (retr >= 3)
-		return 0;
-	else
-		return 1;
-}
-
-static int read_capacity(struct Scsi_Host *shpnt, int ldn)
-{
-	int retr;
-	struct im_scb *scb;
-	struct im_tsb *tsb;
-	unsigned char *buf;
-
-	scb = &(ld(shpnt)[ldn].scb);
-	tsb = &(ld(shpnt)[ldn].tsb);
-	buf = (unsigned char *) (&(ld(shpnt)[ldn].buf));
-	ld(shpnt)[ldn].tsb.dev_status = 0;
-	for (retr = 0; retr < 3; retr++) {
-		/*fill scb with read capacity command */
-		scb->command = IM_READ_CAPACITY_CMD;
-		scb->enable = IM_REPORT_TSB_ONLY_ON_ERROR | IM_READ_CONTROL | IM_RETRY_ENABLE | IM_BYPASS_BUFFER;
-		last_scsi_command(shpnt)[ldn] = IM_READ_CAPACITY_CMD;
-		last_scsi_type(shpnt)[ldn] = IM_SCB;
-		scb->sys_buf_adr = isa_virt_to_bus(buf);
-		scb->sys_buf_length = 8;
-		scb->tsb_adr = isa_virt_to_bus(tsb);
-		/*issue scb to passed ldn, and busy wait for interrupt */
-		got_interrupt(shpnt) = 0;
-		issue_cmd(shpnt, isa_virt_to_bus(scb), IM_SCB | ldn);
-		while (!got_interrupt(shpnt))
-			barrier();
-
-		/*if got capacity, get block length and return one device found */
-		if ((stat_result(shpnt) == IM_SCB_CMD_COMPLETED) || (stat_result(shpnt) == IM_SCB_CMD_COMPLETED_WITH_RETRIES))
-			return 1;
-	}
-	/*if all three retries failed, return "no device at this ldn" */
-	if (retr >= 3)
-		return 0;
-	else
-		return 1;
-}
-
-static int get_pos_info(struct Scsi_Host *shpnt)
-{
-	int retr;
-	struct im_scb *scb;
-	struct im_tsb *tsb;
-	unsigned char *buf;
-
-	scb = &(ld(shpnt)[MAX_LOG_DEV].scb);
-	tsb = &(ld(shpnt)[MAX_LOG_DEV].tsb);
-	buf = (unsigned char *) (&(ld(shpnt)[MAX_LOG_DEV].buf));
-	ld(shpnt)[MAX_LOG_DEV].tsb.dev_status = 0;
-	for (retr = 0; retr < 3; retr++) {
-		/*fill scb with get_pos_info command */
-		scb->command = IM_GET_POS_INFO_CMD;
-		scb->enable = IM_READ_CONTROL | IM_REPORT_TSB_ONLY_ON_ERROR | IM_RETRY_ENABLE | IM_BYPASS_BUFFER;
-		last_scsi_command(shpnt)[MAX_LOG_DEV] = IM_GET_POS_INFO_CMD;
-		last_scsi_type(shpnt)[MAX_LOG_DEV] = IM_SCB;
-		scb->sys_buf_adr = isa_virt_to_bus(buf);
-		if (special(shpnt) == IBM_SCSI2_FW)
-			scb->sys_buf_length = 256;	/* get all info from F/W adapter */
-		else
-			scb->sys_buf_length = 18;	/* get exactly 18 bytes for other SCSI */
-		scb->tsb_adr = isa_virt_to_bus(tsb);
-		/*issue scb to ldn=15, and busy wait for interrupt */
-		got_interrupt(shpnt) = 0;
-		issue_cmd(shpnt, isa_virt_to_bus(scb), IM_SCB | MAX_LOG_DEV);
-		
-		/* FIXME: timeout */
-		while (!got_interrupt(shpnt))
-			barrier();
-
-		/*if got POS-stuff, get block length and return one device found */
-		if ((stat_result(shpnt) == IM_SCB_CMD_COMPLETED) || (stat_result(shpnt) == IM_SCB_CMD_COMPLETED_WITH_RETRIES))
-			return 1;
-	}
-	/* if all three retries failed, return "no device at this ldn" */
-	if (retr >= 3)
-		return 0;
-	else
-		return 1;
-}
-
-/* SCSI-immediate-command for assign. This functions maps/unmaps specific
- ldn-numbers on SCSI (PUN,LUN). It is needed for presetting of the
- subsystem and for dynamical remapping od ldns. */
-static int immediate_assign(struct Scsi_Host *shpnt, unsigned int pun,
-			    unsigned int lun, unsigned int ldn,
-			    unsigned int operation)
-{
-	int retr;
-	unsigned long imm_cmd;
-
-	for (retr = 0; retr < 3; retr++) {
-		/* select mutation level of the SCSI-adapter */
-		switch (special(shpnt)) {
-		case IBM_SCSI2_FW:
-			imm_cmd = (unsigned long) (IM_ASSIGN_IMM_CMD);
-			imm_cmd |= (unsigned long) ((lun & 7) << 24);
-			imm_cmd |= (unsigned long) ((operation & 1) << 23);
-			imm_cmd |= (unsigned long) ((pun & 7) << 20) | ((pun & 8) << 24);
-			imm_cmd |= (unsigned long) ((ldn & 15) << 16);
-			break;
-		default:
-			imm_cmd = inl(IM_CMD_REG(shpnt));
-			imm_cmd &= (unsigned long) (0xF8000000);	/* keep reserved bits */
-			imm_cmd |= (unsigned long) (IM_ASSIGN_IMM_CMD);
-			imm_cmd |= (unsigned long) ((lun & 7) << 24);
-			imm_cmd |= (unsigned long) ((operation & 1) << 23);
-			imm_cmd |= (unsigned long) ((pun & 7) << 20);
-			imm_cmd |= (unsigned long) ((ldn & 15) << 16);
-			break;
-		}
-		last_scsi_command(shpnt)[MAX_LOG_DEV] = IM_ASSIGN_IMM_CMD;
-		last_scsi_type(shpnt)[MAX_LOG_DEV] = IM_IMM_CMD;
-		got_interrupt(shpnt) = 0;
-		issue_cmd(shpnt, (unsigned long) (imm_cmd), IM_IMM_CMD | MAX_LOG_DEV);
-		while (!got_interrupt(shpnt))
-			barrier();
-
-		/*if command successful, break */
-		if (stat_result(shpnt) == IM_IMMEDIATE_CMD_COMPLETED)
-			return 1;
-	}
-	if (retr >= 3)
-		return 0;
-	else
-		return 1;
-}
-
-static int immediate_feature(struct Scsi_Host *shpnt, unsigned int speed, unsigned int timeout)
-{
-	int retr;
-	unsigned long imm_cmd;
-
-	for (retr = 0; retr < 3; retr++) {
-		/* select mutation level of the SCSI-adapter */
-		imm_cmd = IM_FEATURE_CTR_IMM_CMD;
-		imm_cmd |= (unsigned long) ((speed & 0x7) << 29);
-		imm_cmd |= (unsigned long) ((timeout & 0x1fff) << 16);
-		last_scsi_command(shpnt)[MAX_LOG_DEV] = IM_FEATURE_CTR_IMM_CMD;
-		last_scsi_type(shpnt)[MAX_LOG_DEV] = IM_IMM_CMD;
-		got_interrupt(shpnt) = 0;
-		/* we need to run into command errors in order to probe for the
-		 * right speed! */
-		global_command_error_excuse = 1;
-		issue_cmd(shpnt, (unsigned long) (imm_cmd), IM_IMM_CMD | MAX_LOG_DEV);
-		
-		/* FIXME: timeout */
-		while (!got_interrupt(shpnt))
-			barrier();
-		if (global_command_error_excuse == CMD_FAIL) {
-			global_command_error_excuse = 0;
-			return 2;
-		} else
-			global_command_error_excuse = 0;
-		/*if command successful, break */
-		if (stat_result(shpnt) == IM_IMMEDIATE_CMD_COMPLETED)
-			return 1;
-	}
-	if (retr >= 3)
-		return 0;
-	else
-		return 1;
-}
-
-#ifdef CONFIG_IBMMCA_SCSI_DEV_RESET
-static int immediate_reset(struct Scsi_Host *shpnt, unsigned int ldn)
-{
-	int retries;
-	int ticks;
-	unsigned long imm_command;
-
-	for (retries = 0; retries < 3; retries++) {
-		imm_command = inl(IM_CMD_REG(shpnt));
-		imm_command &= (unsigned long) (0xFFFF0000);	/* keep reserved bits */
-		imm_command |= (unsigned long) (IM_RESET_IMM_CMD);
-		last_scsi_command(shpnt)[ldn] = IM_RESET_IMM_CMD;
-		last_scsi_type(shpnt)[ldn] = IM_IMM_CMD;
-		got_interrupt(shpnt) = 0;
-		reset_status(shpnt) = IM_RESET_IN_PROGRESS;
-		issue_cmd(shpnt, (unsigned long) (imm_command), IM_IMM_CMD | ldn);
-		ticks = IM_RESET_DELAY * HZ;
-		while (reset_status(shpnt) == IM_RESET_IN_PROGRESS && --ticks) {
-			udelay((1 + 999 / HZ) * 1000);
-			barrier();
-		}
-		/* if reset did not complete, just complain */
-		if (!ticks) {
-			printk(KERN_ERR "IBM MCA SCSI: reset did not complete within %d seconds.\n", IM_RESET_DELAY);
-			reset_status(shpnt) = IM_RESET_FINISHED_OK;
-			/* did not work, finish */
-			return 1;
-		}
-		/*if command successful, break */
-		if (stat_result(shpnt) == IM_IMMEDIATE_CMD_COMPLETED)
-			return 1;
-	}
-	if (retries >= 3)
-		return 0;
-	else
-		return 1;
-}
-#endif
-
-/* type-interpreter for physical device numbers */
-static char *ti_p(int dev)
-{
-	switch (dev) {
-	case TYPE_IBM_SCSI_ADAPTER:
-		return ("A");
-	case TYPE_DISK:
-		return ("D");
-	case TYPE_TAPE:
-		return ("T");
-	case TYPE_PROCESSOR:
-		return ("P");
-	case TYPE_WORM:
-		return ("W");
-	case TYPE_ROM:
-		return ("R");
-	case TYPE_SCANNER:
-		return ("S");
-	case TYPE_MOD:
-		return ("M");
-	case TYPE_MEDIUM_CHANGER:
-		return ("C");
-	case TYPE_NO_LUN:
-		return ("+");	/* show NO_LUN */
-	}
-	return ("-");		/* TYPE_NO_DEVICE and others */
-}
-
-/* interpreter for logical device numbers (ldn) */
-static char *ti_l(int val)
-{
-	const char hex[16] = "0123456789abcdef";
-	static char answer[2];
-
-	answer[1] = (char) (0x0);
-	if (val <= MAX_LOG_DEV)
-		answer[0] = hex[val];
-	else
-		answer[0] = '-';
-	return (char *) &answer;
-}
-
-/* transfers bitpattern of the feature command to values in MHz */
-static char *ibmrate(unsigned int speed, int i)
-{
-	switch (speed) {
-	case 0:
-		return i ? "5.00" : "10.00";
-	case 1:
-		return i ? "4.00" : "8.00";
-	case 2:
-		return i ? "3.33" : "6.66";
-	case 3:
-		return i ? "2.86" : "5.00";
-	case 4:
-		return i ? "2.50" : "4.00";
-	case 5:
-		return i ? "2.22" : "3.10";
-	case 6:
-		return i ? "2.00" : "2.50";
-	case 7:
-		return i ? "1.82" : "2.00";
-	}
-	return "---";
-}
-
-static int probe_display(int what)
-{
-	static int rotator = 0;
-	const char rotor[] = "|/-\\";
-
-	if (!(display_mode & LED_DISP))
-		return 0;
-	if (!what) {
-		outl(0x20202020, MOD95_LED_PORT);
-		outl(0x20202020, MOD95_LED_PORT + 4);
-	} else {
-		outb('S', MOD95_LED_PORT + 7);
-		outb('C', MOD95_LED_PORT + 6);
-		outb('S', MOD95_LED_PORT + 5);
-		outb('I', MOD95_LED_PORT + 4);
-		outb('i', MOD95_LED_PORT + 3);
-		outb('n', MOD95_LED_PORT + 2);
-		outb('i', MOD95_LED_PORT + 1);
-		outb((char) (rotor[rotator]), MOD95_LED_PORT);
-		rotator++;
-		if (rotator > 3)
-			rotator = 0;
-	}
-	return 0;
-}
-
-static int probe_bus_mode(struct Scsi_Host *shpnt)
-{
-	struct im_pos_info *info;
-	int num_bus = 0;
-	int ldn;
-
-	info = (struct im_pos_info *) (&(ld(shpnt)[MAX_LOG_DEV].buf));
-	if (get_pos_info(shpnt)) {
-		if (info->connector_size & 0xf000)
-			subsystem_connector_size(shpnt) = 16;
-		else
-			subsystem_connector_size(shpnt) = 32;
-		num_bus |= (info->pos_4b & 8) >> 3;
-		for (ldn = 0; ldn <= MAX_LOG_DEV; ldn++) {
-			if ((special(shpnt) == IBM_SCSI_WCACHE) || (special(shpnt) == IBM_7568_WCACHE)) {
-				if (!((info->cache_stat >> ldn) & 1))
-					ld(shpnt)[ldn].cache_flag = 0;
-			}
-			if (!((info->retry_stat >> ldn) & 1))
-				ld(shpnt)[ldn].retry_flag = 0;
-		}
-#ifdef IM_DEBUG_PROBE
-		printk("IBM MCA SCSI: SCSI-Cache bits: ");
-		for (ldn = 0; ldn <= MAX_LOG_DEV; ldn++) {
-			printk("%d", ld(shpnt)[ldn].cache_flag);
-		}
-		printk("\nIBM MCA SCSI: SCSI-Retry bits: ");
-		for (ldn = 0; ldn <= MAX_LOG_DEV; ldn++) {
-			printk("%d", ld(shpnt)[ldn].retry_flag);
-		}
-		printk("\n");
-#endif
-	}
-	return num_bus;
-}
-
-/* probing scsi devices */
-static void check_devices(struct Scsi_Host *shpnt, int adaptertype)
-{
-	int id, lun, ldn, ticks;
-	int count_devices;	/* local counter for connected device */
-	int max_pun;
-	int num_bus;
-	int speedrun;		/* local adapter_speed check variable */
-
-	/* assign default values to certain variables */
-	ticks = 0;
-	count_devices = 0;
-	IBM_DS(shpnt).dyn_flag = 0;	/* normally no need for dynamical ldn management */
-	IBM_DS(shpnt).total_errors = 0;	/* set errorcounter to 0 */
-	next_ldn(shpnt) = 7;	/* next ldn to be assigned is 7, because 0-6 is 'hardwired' */
-
-	/* initialize the very important driver-informational arrays/structs */
-	memset(ld(shpnt), 0, sizeof(ld(shpnt)));
-	for (ldn = 0; ldn <= MAX_LOG_DEV; ldn++) {
-		last_scsi_command(shpnt)[ldn] = NO_SCSI;	/* emptify last SCSI-command storage */
-		last_scsi_type(shpnt)[ldn] = 0;
-		ld(shpnt)[ldn].cache_flag = 1;
-		ld(shpnt)[ldn].retry_flag = 1;
-	}
-	memset(get_ldn(shpnt), TYPE_NO_DEVICE, sizeof(get_ldn(shpnt)));	/* this is essential ! */
-	memset(get_scsi(shpnt), TYPE_NO_DEVICE, sizeof(get_scsi(shpnt)));	/* this is essential ! */
-	for (lun = 0; lun < 8; lun++) {
-		/* mark the adapter at its pun on all luns */
-		get_scsi(shpnt)[subsystem_pun(shpnt)][lun] = TYPE_IBM_SCSI_ADAPTER;
-		get_ldn(shpnt)[subsystem_pun(shpnt)][lun] = MAX_LOG_DEV;	/* make sure, the subsystem
-											   ldn is active for all
-											   luns. */
-	}
-	probe_display(0);	/* Supercool display usage during SCSI-probing. */
-	/* This makes sense, when booting without any */
-	/* monitor connected on model XX95. */
-
-	/* STEP 1: */
-	adapter_speed(shpnt) = global_adapter_speed;
-	speedrun = adapter_speed(shpnt);
-	while (immediate_feature(shpnt, speedrun, adapter_timeout) == 2) {
-		probe_display(1);
-		if (speedrun == 7)
-			panic("IBM MCA SCSI: Cannot set Synchronous-Transfer-Rate!\n");
-		speedrun++;
-		if (speedrun > 7)
-			speedrun = 7;
-	}
-	adapter_speed(shpnt) = speedrun;
-	/* Get detailed information about the current adapter, necessary for
-	 * device operations: */
-	num_bus = probe_bus_mode(shpnt);
-
-	/* num_bus contains only valid data for the F/W adapter! */
-	if (adaptertype == IBM_SCSI2_FW) {	/* F/W SCSI adapter: */
-		/* F/W adapter PUN-space extension evaluation: */
-		if (num_bus) {
-			printk(KERN_INFO "IBM MCA SCSI: Separate bus mode (wide-addressing enabled)\n");
-			subsystem_maxid(shpnt) = 16;
-		} else {
-			printk(KERN_INFO "IBM MCA SCSI: Combined bus mode (wide-addressing disabled)\n");
-			subsystem_maxid(shpnt) = 8;
-		}
-		printk(KERN_INFO "IBM MCA SCSI: Sync.-Rate (F/W: 20, Int.: 10, Ext.: %s) MBytes/s\n", ibmrate(speedrun, adaptertype));
-	} else			/* all other IBM SCSI adapters: */
-		printk(KERN_INFO "IBM MCA SCSI: Synchronous-SCSI-Transfer-Rate: %s MBytes/s\n", ibmrate(speedrun, adaptertype));
-
-	/* assign correct PUN device space */
-	max_pun = subsystem_maxid(shpnt);
-
-#ifdef IM_DEBUG_PROBE
-	printk("IBM MCA SCSI: Current SCSI-host index: %d\n", shpnt);
-	printk("IBM MCA SCSI: Removing default logical SCSI-device mapping.");
-#else
-	printk(KERN_INFO "IBM MCA SCSI: Dev. Order: %s, Mapping (takes <2min): ", (ibm_ansi_order) ? "ANSI" : "New");
-#endif
-	for (ldn = 0; ldn < MAX_LOG_DEV; ldn++) {
-		probe_display(1);
-#ifdef IM_DEBUG_PROBE
-		printk(".");
-#endif
-		immediate_assign(shpnt, 0, 0, ldn, REMOVE_LDN);	/* remove ldn (wherever) */
-	}
-	lun = 0;		/* default lun is 0 */
-#ifndef IM_DEBUG_PROBE
-	printk("cleared,");
-#endif
-	/* STEP 2: */
-#ifdef IM_DEBUG_PROBE
-	printk("\nIBM MCA SCSI: Scanning SCSI-devices.");
-#endif
-	for (id = 0; id < max_pun; id++)
-#ifdef CONFIG_SCSI_MULTI_LUN
-		for (lun = 0; lun < 8; lun++)
-#endif
-		{
-			probe_display(1);
-#ifdef IM_DEBUG_PROBE
-			printk(".");
-#endif
-			if (id != subsystem_pun(shpnt)) {
-				/* if pun is not the adapter: */
-				/* set ldn=0 to pun,lun */
-				immediate_assign(shpnt, id, lun, PROBE_LDN, SET_LDN);
-				if (device_inquiry(shpnt, PROBE_LDN)) {	/* probe device */
-					get_scsi(shpnt)[id][lun] = (unsigned char) (ld(shpnt)[PROBE_LDN].buf[0]);
-					/* entry, even for NO_LUN */
-					if (ld(shpnt)[PROBE_LDN].buf[0] != TYPE_NO_LUN)
-						count_devices++;	/* a existing device is found */
-				}
-				/* remove ldn */
-				immediate_assign(shpnt, id, lun, PROBE_LDN, REMOVE_LDN);
-			}
-		}
-#ifndef IM_DEBUG_PROBE
-	printk("scanned,");
-#endif
-	/* STEP 3: */
-#ifdef IM_DEBUG_PROBE
-	printk("\nIBM MCA SCSI: Mapping SCSI-devices.");
-#endif
-	ldn = 0;
-	lun = 0;
-#ifdef CONFIG_SCSI_MULTI_LUN
-	for (lun = 0; lun < 8 && ldn < MAX_LOG_DEV; lun++)
-#endif
-		for (id = 0; id < max_pun && ldn < MAX_LOG_DEV; id++) {
-			probe_display(1);
-#ifdef IM_DEBUG_PROBE
-			printk(".");
-#endif
-			if (id != subsystem_pun(shpnt)) {
-				if (get_scsi(shpnt)[id][lun] != TYPE_NO_LUN && get_scsi(shpnt)[id][lun] != TYPE_NO_DEVICE) {
-					/* Only map if accepted type. Always enter for
-					   lun == 0 to get no gaps into ldn-mapping for ldn<7. */
-					immediate_assign(shpnt, id, lun, ldn, SET_LDN);
-					get_ldn(shpnt)[id][lun] = ldn;	/* map ldn */
-					if (device_exists(shpnt, ldn, &ld(shpnt)[ldn].block_length, &ld(shpnt)[ldn].device_type)) {
-#ifdef CONFIG_IBMMCA_SCSI_DEV_RESET
-						printk("resetting device at ldn=%x ... ", ldn);
-						immediate_reset(shpnt, ldn);
-#endif
-						ldn++;
-					} else {
-						/* device vanished, probably because we don't know how to
-						 * handle it or because it has problems */
-						if (lun > 0) {
-							/* remove mapping */
-							get_ldn(shpnt)[id][lun] = TYPE_NO_DEVICE;
-							immediate_assign(shpnt, 0, 0, ldn, REMOVE_LDN);
-						} else
-							ldn++;
-					}
-				} else if (lun == 0) {
-					/* map lun == 0, even if no device exists */
-					immediate_assign(shpnt, id, lun, ldn, SET_LDN);
-					get_ldn(shpnt)[id][lun] = ldn;	/* map ldn */
-					ldn++;
-				}
-			}
-		}
-	/* STEP 4: */
-
-	/* map remaining ldns to non-existing devices */
-	for (lun = 1; lun < 8 && ldn < MAX_LOG_DEV; lun++)
-		for (id = 0; id < max_pun && ldn < MAX_LOG_DEV; id++) {
-			if (get_scsi(shpnt)[id][lun] == TYPE_NO_LUN || get_scsi(shpnt)[id][lun] == TYPE_NO_DEVICE) {
-				probe_display(1);
-				/* Map remaining ldns only to NON-existing pun,lun
-				   combinations to make sure an inquiry will fail.
-				   For MULTI_LUN, it is needed to avoid adapter autonome
-				   SCSI-remapping. */
-				immediate_assign(shpnt, id, lun, ldn, SET_LDN);
-				get_ldn(shpnt)[id][lun] = ldn;
-				ldn++;
-			}
-		}
-#ifndef IM_DEBUG_PROBE
-	printk("mapped.");
-#endif
-	printk("\n");
-#ifdef IM_DEBUG_PROBE
-	if (ibm_ansi_order)
-		printk("IBM MCA SCSI: Device order: IBM/ANSI (pun=7 is first).\n");
-	else
-		printk("IBM MCA SCSI: Device order: New Industry Standard (pun=0 is first).\n");
-#endif
-
-#ifdef IM_DEBUG_PROBE
-	/* Show the physical and logical mapping during boot. */
-	printk("IBM MCA SCSI: Determined SCSI-device-mapping:\n");
-	printk("    Physical SCSI-Device Map               Logical SCSI-Device Map\n");
-	printk("ID\\LUN  0  1  2  3  4  5  6  7       ID\\LUN  0  1  2  3  4  5  6  7\n");
-	for (id = 0; id < max_pun; id++) {
-		printk("%2d     ", id);
-		for (lun = 0; lun < 8; lun++)
-			printk("%2s ", ti_p(get_scsi(shpnt)[id][lun]));
-		printk("      %2d     ", id);
-		for (lun = 0; lun < 8; lun++)
-			printk("%2s ", ti_l(get_ldn(shpnt)[id][lun]));
-		printk("\n");
-	}
-#endif
-
-	/* assign total number of found SCSI-devices to the statistics struct */
-	IBM_DS(shpnt).total_scsi_devices = count_devices;
-
-	/* decide for output in /proc-filesystem, if the configuration of
-	   SCSI-devices makes dynamical reassignment of devices necessary */
-	if (count_devices >= MAX_LOG_DEV)
-		IBM_DS(shpnt).dyn_flag = 1;	/* dynamical assignment is necessary */
-	else
-		IBM_DS(shpnt).dyn_flag = 0;	/* dynamical assignment is not necessary */
-
-	/* If no SCSI-devices are assigned, return 1 in order to cause message. */
-	if (ldn == 0)
-		printk("IBM MCA SCSI: Warning: No SCSI-devices found/assigned!\n");
-
-	/* reset the counters for statistics on the current adapter */
-	IBM_DS(shpnt).scbs = 0;
-	IBM_DS(shpnt).long_scbs = 0;
-	IBM_DS(shpnt).total_accesses = 0;
-	IBM_DS(shpnt).total_interrupts = 0;
-	IBM_DS(shpnt).dynamical_assignments = 0;
-	memset(IBM_DS(shpnt).ldn_access, 0x0, sizeof(IBM_DS(shpnt).ldn_access));
-	memset(IBM_DS(shpnt).ldn_read_access, 0x0, sizeof(IBM_DS(shpnt).ldn_read_access));
-	memset(IBM_DS(shpnt).ldn_write_access, 0x0, sizeof(IBM_DS(shpnt).ldn_write_access));
-	memset(IBM_DS(shpnt).ldn_inquiry_access, 0x0, sizeof(IBM_DS(shpnt).ldn_inquiry_access));
-	memset(IBM_DS(shpnt).ldn_modeselect_access, 0x0, sizeof(IBM_DS(shpnt).ldn_modeselect_access));
-	memset(IBM_DS(shpnt).ldn_assignments, 0x0, sizeof(IBM_DS(shpnt).ldn_assignments));
-	probe_display(0);
-	return;
-}
-
-static int device_exists(struct Scsi_Host *shpnt, int ldn, int *block_length, int *device_type)
-{
-	unsigned char *buf;
-	/* if no valid device found, return immediately with 0 */
-	if (!(device_inquiry(shpnt, ldn)))
-		return 0;
-	buf = (unsigned char *) (&(ld(shpnt)[ldn].buf));
-	if (*buf == TYPE_ROM) {
-		*device_type = TYPE_ROM;
-		*block_length = 2048;	/* (standard blocksize for yellow-/red-book) */
-		return 1;
-	}
-	if (*buf == TYPE_WORM) {
-		*device_type = TYPE_WORM;
-		*block_length = 2048;
-		return 1;
-	}
-	if (*buf == TYPE_DISK) {
-		*device_type = TYPE_DISK;
-		if (read_capacity(shpnt, ldn)) {
-			*block_length = *(buf + 7) + (*(buf + 6) << 8) + (*(buf + 5) << 16) + (*(buf + 4) << 24);
-			return 1;
-		} else
-			return 0;
-	}
-	if (*buf == TYPE_MOD) {
-		*device_type = TYPE_MOD;
-		if (read_capacity(shpnt, ldn)) {
-			*block_length = *(buf + 7) + (*(buf + 6) << 8) + (*(buf + 5) << 16) + (*(buf + 4) << 24);
-			return 1;
-		} else
-			return 0;
-	}
-	if (*buf == TYPE_TAPE) {
-		*device_type = TYPE_TAPE;
-		*block_length = 0;	/* not in use (setting by mt and mtst in op.) */
-		return 1;
-	}
-	if (*buf == TYPE_PROCESSOR) {
-		*device_type = TYPE_PROCESSOR;
-		*block_length = 0;	/* they set their stuff on drivers */
-		return 1;
-	}
-	if (*buf == TYPE_SCANNER) {
-		*device_type = TYPE_SCANNER;
-		*block_length = 0;	/* they set their stuff on drivers */
-		return 1;
-	}
-	if (*buf == TYPE_MEDIUM_CHANGER) {
-		*device_type = TYPE_MEDIUM_CHANGER;
-		*block_length = 0;	/* One never knows, what to expect on a medium
-					   changer device. */
-		return 1;
-	}
-	return 0;
-}
-
-static void internal_ibmmca_scsi_setup(char *str, int *ints)
-{
-	int i, j, io_base, id_base;
-	char *token;
-
-	io_base = 0;
-	id_base = 0;
-	if (str) {
-		j = 0;
-		while ((token = strsep(&str, ",")) != NULL) {
-			if (!strcmp(token, "activity"))
-				display_mode |= LED_ACTIVITY;
-			if (!strcmp(token, "display"))
-				display_mode |= LED_DISP;
-			if (!strcmp(token, "adisplay"))
-				display_mode |= LED_ADISP;
-			if (!strcmp(token, "normal"))
-				ibm_ansi_order = 0;
-			if (!strcmp(token, "ansi"))
-				ibm_ansi_order = 1;
-			if (!strcmp(token, "fast"))
-				global_adapter_speed = 0;
-			if (!strcmp(token, "medium"))
-				global_adapter_speed = 4;
-			if (!strcmp(token, "slow"))
-				global_adapter_speed = 7;
-			if ((*token == '-') || (isdigit(*token))) {
-				if (!(j % 2) && (io_base < IM_MAX_HOSTS))
-					io_port[io_base++] = simple_strtoul(token, NULL, 0);
-				if ((j % 2) && (id_base < IM_MAX_HOSTS))
-					scsi_id[id_base++] = simple_strtoul(token, NULL, 0);
-				j++;
-			}
-		}
-	} else if (ints) {
-		for (i = 0; i < IM_MAX_HOSTS && 2 * i + 2 < ints[0]; i++) {
-			io_port[i] = ints[2 * i + 2];
-			scsi_id[i] = ints[2 * i + 2];
-		}
-	}
-	return;
-}
-
-#if 0
- FIXME NEED TO MOVE TO SYSFS
-
-static int ibmmca_getinfo(char *buf, int slot, void *dev_id)
-{
-	struct Scsi_Host *shpnt;
-	int len, speciale, connectore, k;
-	unsigned int pos[8];
-	unsigned long flags;
-	struct Scsi_Host *dev = dev_id;
-
-	spin_lock_irqsave(dev->host_lock, flags);
-
-	shpnt = dev;		/* assign host-structure to local pointer */
-	len = 0;		/* set filled text-buffer index to 0 */
-	/* get the _special contents of the hostdata structure */
-	speciale = ((struct ibmmca_hostdata *) shpnt->hostdata)->_special;
-	connectore = ((struct ibmmca_hostdata *) shpnt->hostdata)->_connector_size;
-	for (k = 2; k < 4; k++)
-		pos[k] = ((struct ibmmca_hostdata *) shpnt->hostdata)->_pos[k];
-	if (speciale == FORCED_DETECTION) {	/* forced detection */
-		len += sprintf(buf + len,
-			       "Adapter category: forced detected\n" "***************************************\n" "***  Forced detected SCSI Adapter   ***\n" "***  No chip-information available  ***\n" "***************************************\n");
-	} else if (speciale == INTEGRATED_SCSI) {
-		/* if the integrated subsystem has been found automatically: */
-		len += sprintf(buf + len,
-			       "Adapter category: integrated\n" "Chip revision level: %d\n" "Chip status: %s\n" "8 kByte NVRAM status: %s\n", ((pos[2] & 0xf0) >> 4), (pos[2] & 1) ? "enabled" : "disabled", (pos[2] & 2) ? "locked" : "accessible");
-	} else if ((speciale >= 0) && (speciale < ARRAY_SIZE(subsys_list))) {
-		/* if the subsystem is a slot adapter */
-		len += sprintf(buf + len, "Adapter category: slot-card\n" "ROM Segment Address: ");
-		if ((pos[2] & 0xf0) == 0xf0)
-			len += sprintf(buf + len, "off\n");
-		else
-			len += sprintf(buf + len, "0x%x\n", ((pos[2] & 0xf0) << 13) + 0xc0000);
-		len += sprintf(buf + len, "Chip status: %s\n", (pos[2] & 1) ? "enabled" : "disabled");
-		len += sprintf(buf + len, "Adapter I/O Offset: 0x%x\n", ((pos[2] & 0x0e) << 2));
-	} else {
-		len += sprintf(buf + len, "Adapter category: unknown\n");
-	}
-	/* common subsystem information to write to the slotn file */
-	len += sprintf(buf + len, "Subsystem PUN: %d\n", shpnt->this_id);
-	len += sprintf(buf + len, "I/O base address range: 0x%x-0x%x\n", (unsigned int) (shpnt->io_port), (unsigned int) (shpnt->io_port + 7));
-	len += sprintf(buf + len, "MCA-slot size: %d bits", connectore);
-	/* Now make sure, the bufferlength is devidable by 4 to avoid
-	 * paging problems of the buffer. */
-	while (len % sizeof(int) != (sizeof(int) - 1))
-		len += sprintf(buf + len, " ");
-	len += sprintf(buf + len, "\n");
-
-	spin_unlock_irqrestore(shpnt->host_lock, flags);
-
-	return len;
-}
-#endif
-
-static struct scsi_host_template ibmmca_driver_template = {
-          .proc_name      = "ibmmca",
-	  .proc_info	  = ibmmca_proc_info,
-          .name           = "IBM SCSI-Subsystem",
-          .queuecommand   = ibmmca_queuecommand,
-	  .eh_abort_handler = ibmmca_abort,
-	  .eh_host_reset_handler = ibmmca_host_reset,
-          .bios_param     = ibmmca_biosparam,
-          .can_queue      = 16,
-          .this_id        = 7,
-          .sg_tablesize   = 16,
-          .cmd_per_lun    = 1,
-          .use_clustering = ENABLE_CLUSTERING,
-};
-
-static int ibmmca_probe(struct device *dev)
-{
-	struct Scsi_Host *shpnt;
-	int port, id, i, j, k, irq, enabled, ret = -EINVAL;
-	struct mca_device *mca_dev = to_mca_device(dev);
-	const char *description = ibmmca_description[mca_dev->index];
-
-	/* First of all, print the version number of the driver. This is
-	 * important to allow better user bugreports in case of already
-	 * having problems with the MCA_bus probing. */
-	printk(KERN_INFO "IBM MCA SCSI: Version %s\n", IBMMCA_SCSI_DRIVER_VERSION);
-	/* The POS2-register of all PS/2 model SCSI-subsystems has the following
-	 * interpretation of bits:
-	 *                             Bit 7 - 4 : Chip Revision ID (Release)
-	 *                             Bit 3 - 2 : Reserved
-	 *                             Bit 1     : 8k NVRAM Disabled
-	 *                             Bit 0     : Chip Enable (EN-Signal)
-	 * The POS3-register is interpreted as follows:
-	 *                             Bit 7 - 5 : SCSI ID
-	 *                             Bit 4     : Reserved = 0
-	 *                             Bit 3 - 0 : Reserved = 0
-	 * (taken from "IBM, PS/2 Hardware Interface Technical Reference, Common
-	 * Interfaces (1991)").
-	 * In short words, this means, that IBM PS/2 machines only support
-	 * 1 single subsystem by default. The slot-adapters must have another
-	 * configuration on pos2. Here, one has to assume the following
-	 * things for POS2-register:
-	 *                             Bit 7 - 4 : Chip Revision ID (Release)
-	 *                             Bit 3 - 1 : port offset factor
-	 *                             Bit 0     : Chip Enable (EN-Signal)
-	 * As I found a patch here, setting the IO-registers to 0x3540 forced,
-	 * as there was a 0x05 in POS2 on a model 56, I assume, that the
-	 * port 0x3540 must be fix for integrated SCSI-controllers.
-	 * Ok, this discovery leads to the following implementation: (M.Lang) */
-
-	/* first look for the IBM SCSI integrated subsystem on the motherboard */
-	for (j = 0; j < 8; j++)	/* read the pos-information */
-		pos[j] = mca_device_read_pos(mca_dev, j);
-	id = (pos[3] & 0xe0) >> 5; /* this is correct and represents the PUN */
-	enabled = (pos[2] &0x01);
-	if (!enabled) {
-		printk(KERN_WARNING "IBM MCA SCSI: WARNING - Your SCSI-subsystem is disabled!\n");
-		printk(KERN_WARNING "              SCSI-operations may not work.\n");
-	}
-
-	/* pos2 = pos3 = 0xff if there is no integrated SCSI-subsystem present, but
-	 * if we ignore the settings of all surrounding pos registers, it is not
-	 * completely sufficient to only check pos2 and pos3. */
-	/* Therefore, now the following if statement is used to
-	 * make sure, we see a real integrated onboard SCSI-interface and no
-	 * internal system information, which gets mapped to some pos registers
-	 * on models 95xx. */
-	if (mca_dev->slot == MCA_INTEGSCSI &&
-	    ((!pos[0] && !pos[1] && pos[2] > 0 &&
-	      pos[3] > 0 && !pos[4] && !pos[5] &&
-	      !pos[6] && !pos[7]) ||
-	     (pos[0] == 0xff && pos[1] == 0xff &&
-	      pos[2] < 0xff && pos[3] < 0xff &&
-	      pos[4] == 0xff && pos[5] == 0xff &&
-	      pos[6] == 0xff && pos[7] == 0xff))) {
-		irq = IM_IRQ;
-		port = IM_IO_PORT;
-	} else {
-		irq = IM_IRQ;
-		port = IM_IO_PORT + ((pos[2] &0x0e) << 2);
-		if ((mca_dev->index == IBM_SCSI2_FW) && (pos[6] != 0)) {
-			printk(KERN_ERR "IBM MCA SCSI: ERROR - Wrong POS(6)-register setting!\n");
-			printk(KERN_ERR "              Impossible to determine adapter PUN!\n");
-			printk(KERN_ERR "              Guessing adapter PUN = 7.\n");
-			id = 7;
-		} else {
-			id = (pos[3] & 0xe0) >> 5;	/* get subsystem PUN */
-			if (mca_dev->index == IBM_SCSI2_FW) {
-				id |= (pos[3] & 0x10) >> 1;	/* get subsystem PUN high-bit
-								 * for F/W adapters */
-			}
-		}
-		if ((mca_dev->index == IBM_SCSI2_FW) &&
-		    (pos[4] & 0x01) && (pos[6] == 0)) {
-			/* IRQ11 is used by SCSI-2 F/W Adapter/A */
-			printk(KERN_DEBUG "IBM MCA SCSI: SCSI-2 F/W adapter needs IRQ 11.\n");
-			irq = IM_IRQ_FW;
-		}
-	}
-
-
-
-	/* give detailed information on the subsystem. This helps me
-	 * additionally during debugging and analyzing bug-reports. */
-	printk(KERN_INFO "IBM MCA SCSI: %s found, io=0x%x, scsi id=%d,\n",
-	       description, port, id);
-	if (mca_dev->slot == MCA_INTEGSCSI)
-		printk(KERN_INFO "              chip rev.=%d, 8K NVRAM=%s, subsystem=%s\n", ((pos[2] & 0xf0) >> 4), (pos[2] & 2) ? "locked" : "accessible", (pos[2] & 1) ? "enabled." : "disabled.");
-	else {
-		if ((pos[2] & 0xf0) == 0xf0)
-			printk(KERN_DEBUG "              ROM Addr.=off,");
-		else
-			printk(KERN_DEBUG "              ROM Addr.=0x%x,", ((pos[2] & 0xf0) << 13) + 0xc0000);
-
-		printk(KERN_DEBUG " port-offset=0x%x, subsystem=%s\n", ((pos[2] & 0x0e) << 2), (pos[2] & 1) ? "enabled." : "disabled.");
-	}
-
-	/* check I/O region */
-	if (!request_region(port, IM_N_IO_PORT, description)) {
-		printk(KERN_ERR "IBM MCA SCSI: Unable to get I/O region 0x%x-0x%x (%d ports).\n", port, port + IM_N_IO_PORT - 1, IM_N_IO_PORT);
-		goto out_fail;
-	}
-
-	/* register host */
-	shpnt = scsi_host_alloc(&ibmmca_driver_template,
-				sizeof(struct ibmmca_hostdata));
-	if (!shpnt) {
-		printk(KERN_ERR "IBM MCA SCSI: Unable to register host.\n");
-		goto out_release;
-	}
-
-	dev_set_drvdata(dev, shpnt);
-	if(request_irq(irq, interrupt_handler, IRQF_SHARED, description, dev)) {
-		printk(KERN_ERR "IBM MCA SCSI: failed to request interrupt %d\n", irq);
-		goto out_free_host;
-	}
-
-	/* request I/O region */
-	special(shpnt) = mca_dev->index;	/* important assignment or else crash! */
-	subsystem_connector_size(shpnt) = 0;	/* preset slot-size */
-	shpnt->irq = irq;	/* assign necessary stuff for the adapter */
-	shpnt->io_port = port;
-	shpnt->n_io_port = IM_N_IO_PORT;
-	shpnt->this_id = id;
-	shpnt->max_id = 8;	/* 8 PUNs are default */
-	/* now, the SCSI-subsystem is connected to Linux */
-
-#ifdef IM_DEBUG_PROBE
-	ctrl = (unsigned int) (inb(IM_CTR_REG(found)));	/* get control-register status */
-	printk("IBM MCA SCSI: Control Register contents: %x, status: %x\n", ctrl, inb(IM_STAT_REG(found)));
-	printk("IBM MCA SCSI: This adapters' POS-registers: ");
-	for (i = 0; i < 8; i++)
-		printk("%x ", pos[i]);
-	printk("\n");
-#endif
-	reset_status(shpnt) = IM_RESET_NOT_IN_PROGRESS;
-
-	for (i = 0; i < 16; i++)	/* reset the tables */
-		for (j = 0; j < 8; j++)
-			get_ldn(shpnt)[i][j] = MAX_LOG_DEV;
-
-	/* check which logical devices exist */
-	/* after this line, local interrupting is possible: */
-	local_checking_phase_flag(shpnt) = 1;
-	check_devices(shpnt, mca_dev->index);	/* call by value, using the global variable hosts */
-	local_checking_phase_flag(shpnt) = 0;
-
-	/* an ibm mca subsystem has been detected */
-
-	for (k = 2; k < 7; k++)
-		((struct ibmmca_hostdata *) shpnt->hostdata)->_pos[k] = pos[k];
-	((struct ibmmca_hostdata *) shpnt->hostdata)->_special = INTEGRATED_SCSI;
-	mca_device_set_name(mca_dev, description);
-	/* FIXME: NEED TO REPLUMB TO SYSFS
-	   mca_set_adapter_procfn(MCA_INTEGSCSI, (MCA_ProcFn) ibmmca_getinfo, shpnt);
-	*/
-	mca_device_set_claim(mca_dev, 1);
-	if (scsi_add_host(shpnt, dev)) {
-		dev_printk(KERN_ERR, dev, "IBM MCA SCSI: scsi_add_host failed\n");
-		goto out_free_host;
-	}
-	scsi_scan_host(shpnt);
-
-	return 0;
- out_free_host:
-	scsi_host_put(shpnt);
- out_release:
-	release_region(port, IM_N_IO_PORT);
- out_fail:
-	return ret;
-}
-
-static int __devexit ibmmca_remove(struct device *dev)
-{
-	struct Scsi_Host *shpnt = dev_get_drvdata(dev);
-	scsi_remove_host(shpnt);
-	release_region(shpnt->io_port, shpnt->n_io_port);
-	free_irq(shpnt->irq, dev);
-	scsi_host_put(shpnt);
-	return 0;
-}
-
-/* The following routine is the SCSI command queue for the midlevel driver */
-static int ibmmca_queuecommand_lck(Scsi_Cmnd * cmd, void (*done) (Scsi_Cmnd *))
-{
-	unsigned int ldn;
-	unsigned int scsi_cmd;
-	struct im_scb *scb;
-	struct Scsi_Host *shpnt;
-	int current_ldn;
-	int id, lun;
-	int target;
-	int max_pun;
-	int i;
-	struct scatterlist *sg;
-
-	shpnt = cmd->device->host;
-
-	max_pun = subsystem_maxid(shpnt);
-	if (ibm_ansi_order) {
-		target = max_pun - 1 - cmd->device->id;
-		if ((target <= subsystem_pun(shpnt)) && (cmd->device->id <= subsystem_pun(shpnt)))
-			target--;
-		else if ((target >= subsystem_pun(shpnt)) && (cmd->device->id >= subsystem_pun(shpnt)))
-			target++;
-	} else
-		target = cmd->device->id;
-
-	/* if (target,lun) is NO LUN or not existing at all, return error */
-	if ((get_scsi(shpnt)[target][cmd->device->lun] == TYPE_NO_LUN) || (get_scsi(shpnt)[target][cmd->device->lun] == TYPE_NO_DEVICE)) {
-		cmd->result = DID_NO_CONNECT << 16;
-		if (done)
-			done(cmd);
-		return 0;
-	}
-
-	/*if (target,lun) unassigned, do further checks... */
-	ldn = get_ldn(shpnt)[target][cmd->device->lun];
-	if (ldn >= MAX_LOG_DEV) {	/* on invalid ldn do special stuff */
-		if (ldn > MAX_LOG_DEV) {	/* dynamical remapping if ldn unassigned */
-			current_ldn = next_ldn(shpnt);	/* stop-value for one circle */
-			while (ld(shpnt)[next_ldn(shpnt)].cmd) {	/* search for a occupied, but not in */
-				/* command-processing ldn. */
-				next_ldn(shpnt)++;
-				if (next_ldn(shpnt) >= MAX_LOG_DEV)
-					next_ldn(shpnt) = 7;
-				if (current_ldn == next_ldn(shpnt)) {	/* One circle done ? */
-					/* no non-processing ldn found */
-					scmd_printk(KERN_WARNING, cmd,
-	"IBM MCA SCSI: Cannot assign SCSI-device dynamically!\n"
-	"              On ldn 7-14 SCSI-commands everywhere in progress.\n"
-	"              Reporting DID_NO_CONNECT for device.\n");
-					cmd->result = DID_NO_CONNECT << 16;	/* return no connect */
-					if (done)
-						done(cmd);
-					return 0;
-				}
-			}
-
-			/* unmap non-processing ldn */
-			for (id = 0; id < max_pun; id++)
-				for (lun = 0; lun < 8; lun++) {
-					if (get_ldn(shpnt)[id][lun] == next_ldn(shpnt)) {
-						get_ldn(shpnt)[id][lun] = TYPE_NO_DEVICE;
-						get_scsi(shpnt)[id][lun] = TYPE_NO_DEVICE;
-						/* unmap entry */
-					}
-				}
-			/* set reduced interrupt_handler-mode for checking */
-			local_checking_phase_flag(shpnt) = 1;
-			/* map found ldn to pun,lun */
-			get_ldn(shpnt)[target][cmd->device->lun] = next_ldn(shpnt);
-			/* change ldn to the right value, that is now next_ldn */
-			ldn = next_ldn(shpnt);
-			/* unassign all ldns (pun,lun,ldn does not matter for remove) */
-			immediate_assign(shpnt, 0, 0, 0, REMOVE_LDN);
-			/* set only LDN for remapped device */
-			immediate_assign(shpnt, target, cmd->device->lun, ldn, SET_LDN);
-			/* get device information for ld[ldn] */
-			if (device_exists(shpnt, ldn, &ld(shpnt)[ldn].block_length, &ld(shpnt)[ldn].device_type)) {
-				ld(shpnt)[ldn].cmd = NULL;	/* To prevent panic set 0, because
-								   devices that were not assigned,
-								   should have nothing in progress. */
-				get_scsi(shpnt)[target][cmd->device->lun] = ld(shpnt)[ldn].device_type;
-				/* increase assignment counters for statistics in /proc */
-				IBM_DS(shpnt).dynamical_assignments++;
-				IBM_DS(shpnt).ldn_assignments[ldn]++;
-			} else
-				/* panic here, because a device, found at boottime has
-				   vanished */
-				panic("IBM MCA SCSI: ldn=0x%x, SCSI-device on (%d,%d) vanished!\n", ldn, target, cmd->device->lun);
-			/* unassign again all ldns (pun,lun,ldn does not matter for remove) */
-			immediate_assign(shpnt, 0, 0, 0, REMOVE_LDN);
-			/* remap all ldns, as written in the pun/lun table */
-			lun = 0;
-#ifdef CONFIG_SCSI_MULTI_LUN
-			for (lun = 0; lun < 8; lun++)
-#endif
-				for (id = 0; id < max_pun; id++) {
-					if (get_ldn(shpnt)[id][lun] <= MAX_LOG_DEV)
-						immediate_assign(shpnt, id, lun, get_ldn(shpnt)[id][lun], SET_LDN);
-				}
-			/* set back to normal interrupt_handling */
-			local_checking_phase_flag(shpnt) = 0;
-#ifdef IM_DEBUG_PROBE
-			/* Information on syslog terminal */
-			printk("IBM MCA SCSI: ldn=0x%x dynamically reassigned to (%d,%d).\n", ldn, target, cmd->device->lun);
-#endif
-			/* increase next_ldn for next dynamical assignment */
-			next_ldn(shpnt)++;
-			if (next_ldn(shpnt) >= MAX_LOG_DEV)
-				next_ldn(shpnt) = 7;
-		} else {	/* wall against Linux accesses to the subsystem adapter */
-			cmd->result = DID_BAD_TARGET << 16;
-			if (done)
-				done(cmd);
-			return 0;
-		}
-	}
-
-	/*verify there is no command already in progress for this log dev */
-	if (ld(shpnt)[ldn].cmd)
-		panic("IBM MCA SCSI: cmd already in progress for this ldn.\n");
-
-	/*save done in cmd, and save cmd for the interrupt handler */
-	cmd->scsi_done = done;
-	ld(shpnt)[ldn].cmd = cmd;
-
-	/*fill scb information independent of the scsi command */
-	scb = &(ld(shpnt)[ldn].scb);
-	ld(shpnt)[ldn].tsb.dev_status = 0;
-	scb->enable = IM_REPORT_TSB_ONLY_ON_ERROR | IM_RETRY_ENABLE;
-	scb->tsb_adr = isa_virt_to_bus(&(ld(shpnt)[ldn].tsb));
-	scsi_cmd = cmd->cmnd[0];
-
-	if (scsi_sg_count(cmd)) {
-		BUG_ON(scsi_sg_count(cmd) > 16);
-
-		scsi_for_each_sg(cmd, sg, scsi_sg_count(cmd), i) {
-			ld(shpnt)[ldn].sge[i].address = (void *) (isa_page_to_bus(sg_page(sg)) + sg->offset);
-			ld(shpnt)[ldn].sge[i].byte_length = sg->length;
-		}
-		scb->enable |= IM_POINTER_TO_LIST;
-		scb->sys_buf_adr = isa_virt_to_bus(&(ld(shpnt)[ldn].sge[0]));
-		scb->sys_buf_length = scsi_sg_count(cmd) * sizeof(struct im_sge);
-	} else {
- 		scb->sys_buf_adr = isa_virt_to_bus(scsi_sglist(cmd));
-		/* recent Linux midlevel SCSI places 1024 byte for inquiry
-		 * command. Far too much for old PS/2 hardware. */
-		switch (scsi_cmd) {
-			/* avoid command errors by setting bufferlengths to
-			 * ANSI-standard. Beware of forcing it to 255,
-			 * this could SEGV the kernel!!! */
-		case INQUIRY:
-		case REQUEST_SENSE:
-		case MODE_SENSE:
-		case MODE_SELECT:
-			if (scsi_bufflen(cmd) > 255)
-				scb->sys_buf_length = 255;
-			else
-				scb->sys_buf_length = scsi_bufflen(cmd);
-			break;
-		case TEST_UNIT_READY:
-			scb->sys_buf_length = 0;
-			break;
-		default:
-			scb->sys_buf_length = scsi_bufflen(cmd);
-			break;
-		}
-	}
-	/*fill scb information dependent on scsi command */
-
-#ifdef IM_DEBUG_CMD
-	printk("issue scsi cmd=%02x to ldn=%d\n", scsi_cmd, ldn);
-#endif
-
-	/* for specific device-type debugging: */
-#ifdef IM_DEBUG_CMD_SPEC_DEV
-	if (ld(shpnt)[ldn].device_type == IM_DEBUG_CMD_DEVICE)
-		printk("(SCSI-device-type=0x%x) issue scsi cmd=%02x to ldn=%d\n", ld(shpnt)[ldn].device_type, scsi_cmd, ldn);
-#endif
-
-	/* for possible panics store current command */
-	last_scsi_command(shpnt)[ldn] = scsi_cmd;
-	last_scsi_type(shpnt)[ldn] = IM_SCB;
-	/* update statistical info */
-	IBM_DS(shpnt).total_accesses++;
-	IBM_DS(shpnt).ldn_access[ldn]++;
-
-	switch (scsi_cmd) {
-	case READ_6:
-	case WRITE_6:
-	case READ_10:
-	case WRITE_10:
-	case READ_12:
-	case WRITE_12:
-		/* Distinguish between disk and other devices. Only disks (that are the
-		   most frequently accessed devices) should be supported by the
-		   IBM-SCSI-Subsystem commands. */
-		switch (ld(shpnt)[ldn].device_type) {
-		case TYPE_DISK:	/* for harddisks enter here ... */
-		case TYPE_MOD:	/* ... try it also for MO-drives (send flames as */
-			/*     you like, if this won't work.) */
-			if (scsi_cmd == READ_6 || scsi_cmd == READ_10 || scsi_cmd == READ_12) {
-				/* read command preparations */
-				scb->enable |= IM_READ_CONTROL;
-				IBM_DS(shpnt).ldn_read_access[ldn]++;	/* increase READ-access on ldn stat. */
-				scb->command = IM_READ_DATA_CMD | IM_NO_DISCONNECT;
-			} else {	/* write command preparations */
-				IBM_DS(shpnt).ldn_write_access[ldn]++;	/* increase write-count on ldn stat. */
-				scb->command = IM_WRITE_DATA_CMD | IM_NO_DISCONNECT;
-			}
-			if (scsi_cmd == READ_6 || scsi_cmd == WRITE_6) {
-				scb->u1.log_blk_adr = (((unsigned) cmd->cmnd[3]) << 0) | (((unsigned) cmd->cmnd[2]) << 8) | ((((unsigned) cmd->cmnd[1]) & 0x1f) << 16);
-				scb->u2.blk.count = (unsigned) cmd->cmnd[4];
-			} else {
-				scb->u1.log_blk_adr = (((unsigned) cmd->cmnd[5]) << 0) | (((unsigned) cmd->cmnd[4]) << 8) | (((unsigned) cmd->cmnd[3]) << 16) | (((unsigned) cmd->cmnd[2]) << 24);
-				scb->u2.blk.count = (((unsigned) cmd->cmnd[8]) << 0) | (((unsigned) cmd->cmnd[7]) << 8);
-			}
-			last_scsi_logical_block(shpnt)[ldn] = scb->u1.log_blk_adr;
-			last_scsi_blockcount(shpnt)[ldn] = scb->u2.blk.count;
-			scb->u2.blk.length = ld(shpnt)[ldn].block_length;
-			break;
-			/* for other devices, enter here. Other types are not known by
-			   Linux! TYPE_NO_LUN is forbidden as valid device. */
-		case TYPE_ROM:
-		case TYPE_TAPE:
-		case TYPE_PROCESSOR:
-		case TYPE_WORM:
-		case TYPE_SCANNER:
-		case TYPE_MEDIUM_CHANGER:
-			/* If there is a sequential-device, IBM recommends to use
-			   IM_OTHER_SCSI_CMD_CMD instead of subsystem READ/WRITE.
-			   This includes CD-ROM devices, too, due to the partial sequential
-			   read capabilities. */
-			scb->command = IM_OTHER_SCSI_CMD_CMD;
-			if (scsi_cmd == READ_6 || scsi_cmd == READ_10 || scsi_cmd == READ_12)
-				/* enable READ */
-				scb->enable |= IM_READ_CONTROL;
-			scb->enable |= IM_BYPASS_BUFFER;
-			scb->u1.scsi_cmd_length = cmd->cmd_len;
-			memcpy(scb->u2.scsi_command, cmd->cmnd, cmd->cmd_len);
-			last_scsi_type(shpnt)[ldn] = IM_LONG_SCB;
-			/* Read/write on this non-disk devices is also displayworthy,
-			   so flash-up the LED/display. */
-			break;
-		}
-		break;
-	case INQUIRY:
-		IBM_DS(shpnt).ldn_inquiry_access[ldn]++;
-		scb->command = IM_DEVICE_INQUIRY_CMD;
-		scb->enable |= IM_READ_CONTROL | IM_SUPRESS_EXCEPTION_SHORT | IM_BYPASS_BUFFER;
-		scb->u1.log_blk_adr = 0;
-		break;
-	case TEST_UNIT_READY:
-		scb->command = IM_OTHER_SCSI_CMD_CMD;
-		scb->enable |= IM_READ_CONTROL | IM_SUPRESS_EXCEPTION_SHORT | IM_BYPASS_BUFFER;
-		scb->u1.log_blk_adr = 0;
-		scb->u1.scsi_cmd_length = 6;
-		memcpy(scb->u2.scsi_command, cmd->cmnd, 6);
-		last_scsi_type(shpnt)[ldn] = IM_LONG_SCB;
-		break;
-	case READ_CAPACITY:
-		/* the length of system memory buffer must be exactly 8 bytes */
-		scb->command = IM_READ_CAPACITY_CMD;
-		scb->enable |= IM_READ_CONTROL | IM_BYPASS_BUFFER;
-		if (scb->sys_buf_length > 8)
-			scb->sys_buf_length = 8;
-		break;
-		/* Commands that need read-only-mode (system <- device): */
-	case REQUEST_SENSE:
-		scb->command = IM_REQUEST_SENSE_CMD;
-		scb->enable |= IM_READ_CONTROL | IM_SUPRESS_EXCEPTION_SHORT | IM_BYPASS_BUFFER;
-		break;
-		/* Commands that need write-only-mode (system -> device): */
-	case MODE_SELECT:
-	case MODE_SELECT_10:
-		IBM_DS(shpnt).ldn_modeselect_access[ldn]++;
-		scb->command = IM_OTHER_SCSI_CMD_CMD;
-		scb->enable |= IM_SUPRESS_EXCEPTION_SHORT | IM_BYPASS_BUFFER;	/*Select needs WRITE-enabled */
-		scb->u1.scsi_cmd_length = cmd->cmd_len;
-		memcpy(scb->u2.scsi_command, cmd->cmnd, cmd->cmd_len);
-		last_scsi_type(shpnt)[ldn] = IM_LONG_SCB;
-		break;
-		/* For other commands, read-only is useful. Most other commands are
-		   running without an input-data-block. */
-	default:
-		scb->command = IM_OTHER_SCSI_CMD_CMD;
-		scb->enable |= IM_READ_CONTROL | IM_SUPRESS_EXCEPTION_SHORT | IM_BYPASS_BUFFER;
-		scb->u1.scsi_cmd_length = cmd->cmd_len;
-		memcpy(scb->u2.scsi_command, cmd->cmnd, cmd->cmd_len);
-		last_scsi_type(shpnt)[ldn] = IM_LONG_SCB;
-		break;
-	}
-	/*issue scb command, and return */
-	if (++disk_rw_in_progress == 1)
-		PS2_DISK_LED_ON(shpnt->host_no, target);
-
-	if (last_scsi_type(shpnt)[ldn] == IM_LONG_SCB) {
-		issue_cmd(shpnt, isa_virt_to_bus(scb), IM_LONG_SCB | ldn);
-		IBM_DS(shpnt).long_scbs++;
-	} else {
-		issue_cmd(shpnt, isa_virt_to_bus(scb), IM_SCB | ldn);
-		IBM_DS(shpnt).scbs++;
-	}
-	return 0;
-}
-
-static DEF_SCSI_QCMD(ibmmca_queuecommand)
-
-static int __ibmmca_abort(Scsi_Cmnd * cmd)
-{
-	/* Abort does not work, as the adapter never generates an interrupt on
-	 * whatever situation is simulated, even when really pending commands
-	 * are running on the adapters' hardware ! */
-
-	struct Scsi_Host *shpnt;
-	unsigned int ldn;
-	void (*saved_done) (Scsi_Cmnd *);
-	int target;
-	int max_pun;
-	unsigned long imm_command;
-
-#ifdef IM_DEBUG_PROBE
-	printk("IBM MCA SCSI: Abort subroutine called...\n");
-#endif
-
-	shpnt = cmd->device->host;
-
-	max_pun = subsystem_maxid(shpnt);
-	if (ibm_ansi_order) {
-		target = max_pun - 1 - cmd->device->id;
-		if ((target <= subsystem_pun(shpnt)) && (cmd->device->id <= subsystem_pun(shpnt)))
-			target--;
-		else if ((target >= subsystem_pun(shpnt)) && (cmd->device->id >= subsystem_pun(shpnt)))
-			target++;
-	} else
-		target = cmd->device->id;
-
-	/* get logical device number, and disable system interrupts */
-	printk(KERN_WARNING "IBM MCA SCSI: Sending abort to device pun=%d, lun=%d.\n", target, cmd->device->lun);
-	ldn = get_ldn(shpnt)[target][cmd->device->lun];
-
-	/*if cmd for this ldn has already finished, no need to abort */
-	if (!ld(shpnt)[ldn].cmd) {
-		    return SUCCESS;
-	}
-
-	/* Clear ld.cmd, save done function, install internal done,
-	 * send abort immediate command (this enables sys. interrupts),
-	 * and wait until the interrupt arrives.
-	 */
-	saved_done = cmd->scsi_done;
-	cmd->scsi_done = internal_done;
-	cmd->SCp.Status = 0;
-	last_scsi_command(shpnt)[ldn] = IM_ABORT_IMM_CMD;
-	last_scsi_type(shpnt)[ldn] = IM_IMM_CMD;
-	imm_command = inl(IM_CMD_REG(shpnt));
-	imm_command &= (unsigned long) (0xffff0000);	/* mask reserved stuff */
-	imm_command |= (unsigned long) (IM_ABORT_IMM_CMD);
-	/* must wait for attention reg not busy */
-	/* FIXME - timeout, politeness */
-	while (1) {
-		if (!(inb(IM_STAT_REG(shpnt)) & IM_BUSY))
-			break;
-	}
-	/* write registers and enable system interrupts */
-	outl(imm_command, IM_CMD_REG(shpnt));
-	outb(IM_IMM_CMD | ldn, IM_ATTN_REG(shpnt));
-#ifdef IM_DEBUG_PROBE
-	printk("IBM MCA SCSI: Abort queued to adapter...\n");
-#endif
-	spin_unlock_irq(shpnt->host_lock);
-	while (!cmd->SCp.Status)
-		yield();
-	spin_lock_irq(shpnt->host_lock);
-	cmd->scsi_done = saved_done;
-#ifdef IM_DEBUG_PROBE
-	printk("IBM MCA SCSI: Abort returned with adapter response...\n");
-#endif
-
-	/*if abort went well, call saved done, then return success or error */
-	if (cmd->result == (DID_ABORT << 16)) 
-	{
-		cmd->result |= DID_ABORT << 16;
-		if (cmd->scsi_done)
-			(cmd->scsi_done) (cmd);
-		ld(shpnt)[ldn].cmd = NULL;
-#ifdef IM_DEBUG_PROBE
-		printk("IBM MCA SCSI: Abort finished with success.\n");
-#endif
-		return SUCCESS;
-	} else {
-		cmd->result |= DID_NO_CONNECT << 16;
-		if (cmd->scsi_done)
-			(cmd->scsi_done) (cmd);
-		ld(shpnt)[ldn].cmd = NULL;
-#ifdef IM_DEBUG_PROBE
-		printk("IBM MCA SCSI: Abort failed.\n");
-#endif
-		return FAILED;
-	}
-}
-
-static int ibmmca_abort(Scsi_Cmnd * cmd)
-{
-	struct Scsi_Host *shpnt = cmd->device->host;
-	int rc;
-
-	spin_lock_irq(shpnt->host_lock);
-	rc = __ibmmca_abort(cmd);
-	spin_unlock_irq(shpnt->host_lock);
-
-	return rc;
-}
-
-static int __ibmmca_host_reset(Scsi_Cmnd * cmd)
-{
-	struct Scsi_Host *shpnt;
-	Scsi_Cmnd *cmd_aid;
-	int ticks, i;
-	unsigned long imm_command;
-
-	BUG_ON(cmd == NULL);
-
-	ticks = IM_RESET_DELAY * HZ;
-	shpnt = cmd->device->host;
-
-	if (local_checking_phase_flag(shpnt)) {
-		printk(KERN_WARNING "IBM MCA SCSI: unable to reset while checking devices.\n");
-		return FAILED;
-	}
-
-	/* issue reset immediate command to subsystem, and wait for interrupt */
-	printk("IBM MCA SCSI: resetting all devices.\n");
-	reset_status(shpnt) = IM_RESET_IN_PROGRESS;
-	last_scsi_command(shpnt)[0xf] = IM_RESET_IMM_CMD;
-	last_scsi_type(shpnt)[0xf] = IM_IMM_CMD;
-	imm_command = inl(IM_CMD_REG(shpnt));
-	imm_command &= (unsigned long) (0xffff0000);	/* mask reserved stuff */
-	imm_command |= (unsigned long) (IM_RESET_IMM_CMD);
-	/* must wait for attention reg not busy */
-	while (1) {
-		if (!(inb(IM_STAT_REG(shpnt)) & IM_BUSY))
-			break;
-		spin_unlock_irq(shpnt->host_lock);
-		yield();
-		spin_lock_irq(shpnt->host_lock);
-	}
-	/*write registers and enable system interrupts */
-	outl(imm_command, IM_CMD_REG(shpnt));
-	outb(IM_IMM_CMD | 0xf, IM_ATTN_REG(shpnt));
-	/* wait for interrupt finished or intr_stat register to be set, as the
-	 * interrupt will not be executed, while we are in here! */
-	 
-	/* FIXME: This is really really icky we so want a sleeping version of this ! */
-	while (reset_status(shpnt) == IM_RESET_IN_PROGRESS && --ticks && ((inb(IM_INTR_REG(shpnt)) & 0x8f) != 0x8f)) {
-		udelay((1 + 999 / HZ) * 1000);
-		barrier();
-	}
-	/* if reset did not complete, just return an error */
-	if (!ticks) {
-		printk(KERN_ERR "IBM MCA SCSI: reset did not complete within %d seconds.\n", IM_RESET_DELAY);
-		reset_status(shpnt) = IM_RESET_FINISHED_FAIL;
-		return FAILED;
-	}
-
-	if ((inb(IM_INTR_REG(shpnt)) & 0x8f) == 0x8f) {
-		/* analysis done by this routine and not by the intr-routine */
-		if (inb(IM_INTR_REG(shpnt)) == 0xaf)
-			reset_status(shpnt) = IM_RESET_FINISHED_OK_NO_INT;
-		else if (inb(IM_INTR_REG(shpnt)) == 0xcf)
-			reset_status(shpnt) = IM_RESET_FINISHED_FAIL;
-		else		/* failed, 4get it */
-			reset_status(shpnt) = IM_RESET_NOT_IN_PROGRESS_NO_INT;
-		outb(IM_EOI | 0xf, IM_ATTN_REG(shpnt));
-	}
-
-	/* if reset failed, just return an error */
-	if (reset_status(shpnt) == IM_RESET_FINISHED_FAIL) {
-		printk(KERN_ERR "IBM MCA SCSI: reset failed.\n");
-		return FAILED;
-	}
-
-	/* so reset finished ok - call outstanding done's, and return success */
-	printk(KERN_INFO "IBM MCA SCSI: Reset successfully completed.\n");
-	for (i = 0; i < MAX_LOG_DEV; i++) {
-		cmd_aid = ld(shpnt)[i].cmd;
-		if (cmd_aid && cmd_aid->scsi_done) {
-			ld(shpnt)[i].cmd = NULL;
-			cmd_aid->result = DID_RESET << 16;
-		}
-	}
-	return SUCCESS;
-}
-
-static int ibmmca_host_reset(Scsi_Cmnd * cmd)
-{
-	struct Scsi_Host *shpnt = cmd->device->host;
-	int rc;
-
-	spin_lock_irq(shpnt->host_lock);
-	rc = __ibmmca_host_reset(cmd);
-	spin_unlock_irq(shpnt->host_lock);
-
-	return rc;
-}
-
-static int ibmmca_biosparam(struct scsi_device *sdev, struct block_device *bdev, sector_t capacity, int *info)
-{
-	int size = capacity;
-	info[0] = 64;
-	info[1] = 32;
-	info[2] = size / (info[0] * info[1]);
-	if (info[2] >= 1024) {
-		info[0] = 128;
-		info[1] = 63;
-		info[2] = size / (info[0] * info[1]);
-		if (info[2] >= 1024) {
-			info[0] = 255;
-			info[1] = 63;
-			info[2] = size / (info[0] * info[1]);
-			if (info[2] >= 1024)
-				info[2] = 1023;
-		}
-	}
-	return 0;
-}
-
-/* calculate percentage of total accesses on a ldn */
-static int ldn_access_load(struct Scsi_Host *shpnt, int ldn)
-{
-	if (IBM_DS(shpnt).total_accesses == 0)
-		return (0);
-	if (IBM_DS(shpnt).ldn_access[ldn] == 0)
-		return (0);
-	return (IBM_DS(shpnt).ldn_access[ldn] * 100) / IBM_DS(shpnt).total_accesses;
-}
-
-/* calculate total amount of r/w-accesses */
-static int ldn_access_total_read_write(struct Scsi_Host *shpnt)
-{
-	int a;
-	int i;
-
-	a = 0;
-	for (i = 0; i <= MAX_LOG_DEV; i++)
-		a += IBM_DS(shpnt).ldn_read_access[i] + IBM_DS(shpnt).ldn_write_access[i];
-	return (a);
-}
-
-static int ldn_access_total_inquiry(struct Scsi_Host *shpnt)
-{
-	int a;
-	int i;
-
-	a = 0;
-	for (i = 0; i <= MAX_LOG_DEV; i++)
-		a += IBM_DS(shpnt).ldn_inquiry_access[i];
-	return (a);
-}
-
-static int ldn_access_total_modeselect(struct Scsi_Host *shpnt)
-{
-	int a;
-	int i;
-
-	a = 0;
-	for (i = 0; i <= MAX_LOG_DEV; i++)
-		a += IBM_DS(shpnt).ldn_modeselect_access[i];
-	return (a);
-}
-
-/* routine to display info in the proc-fs-structure (a deluxe feature) */
-static int ibmmca_proc_info(struct Scsi_Host *shpnt, char *buffer, char **start, off_t offset, int length, int inout)
-{
-	int len = 0;
-	int i, id, lun;
-	unsigned long flags;
-	int max_pun;
-
-	
-	spin_lock_irqsave(shpnt->host_lock, flags);	/* Check it */
-
-	max_pun = subsystem_maxid(shpnt);
-
-	len += sprintf(buffer + len, "\n             IBM-SCSI-Subsystem-Linux-Driver, Version %s\n\n\n", IBMMCA_SCSI_DRIVER_VERSION);
-	len += sprintf(buffer + len, " SCSI Access-Statistics:\n");
-	len += sprintf(buffer + len, "               Device Scanning Order....: %s\n", (ibm_ansi_order) ? "IBM/ANSI" : "New Industry Standard");
-#ifdef CONFIG_SCSI_MULTI_LUN
-	len += sprintf(buffer + len, "               Multiple LUN probing.....: Yes\n");
-#else
-	len += sprintf(buffer + len, "               Multiple LUN probing.....: No\n");
-#endif
-	len += sprintf(buffer + len, "               This Hostnumber..........: %d\n", shpnt->host_no);
-	len += sprintf(buffer + len, "               Base I/O-Port............: 0x%x\n", (unsigned int) (IM_CMD_REG(shpnt)));
-	len += sprintf(buffer + len, "               (Shared) IRQ.............: %d\n", IM_IRQ);
-	len += sprintf(buffer + len, "               Total Interrupts.........: %d\n", IBM_DS(shpnt).total_interrupts);
-	len += sprintf(buffer + len, "               Total SCSI Accesses......: %d\n", IBM_DS(shpnt).total_accesses);
-	len += sprintf(buffer + len, "               Total short SCBs.........: %d\n", IBM_DS(shpnt).scbs);
-	len += sprintf(buffer + len, "               Total long SCBs..........: %d\n", IBM_DS(shpnt).long_scbs);
-	len += sprintf(buffer + len, "                 Total SCSI READ/WRITE..: %d\n", ldn_access_total_read_write(shpnt));
-	len += sprintf(buffer + len, "                 Total SCSI Inquiries...: %d\n", ldn_access_total_inquiry(shpnt));
-	len += sprintf(buffer + len, "                 Total SCSI Modeselects.: %d\n", ldn_access_total_modeselect(shpnt));
-	len += sprintf(buffer + len, "                 Total SCSI other cmds..: %d\n", IBM_DS(shpnt).total_accesses - ldn_access_total_read_write(shpnt)
-		       - ldn_access_total_modeselect(shpnt)
-		       - ldn_access_total_inquiry(shpnt));
-	len += sprintf(buffer + len, "               Total SCSI command fails.: %d\n\n", IBM_DS(shpnt).total_errors);
-	len += sprintf(buffer + len, " Logical-Device-Number (LDN) Access-Statistics:\n");
-	len += sprintf(buffer + len, "         LDN | Accesses [%%] |   READ    |   WRITE   | ASSIGNMENTS\n");
-	len += sprintf(buffer + len, "        -----|--------------|-----------|-----------|--------------\n");
-	for (i = 0; i <= MAX_LOG_DEV; i++)
-		len += sprintf(buffer + len, "         %2X  |    %3d       |  %8d |  %8d | %8d\n", i, ldn_access_load(shpnt, i), IBM_DS(shpnt).ldn_read_access[i], IBM_DS(shpnt).ldn_write_access[i], IBM_DS(shpnt).ldn_assignments[i]);
-	len += sprintf(buffer + len, "        -----------------------------------------------------------\n\n");
-	len += sprintf(buffer + len, " Dynamical-LDN-Assignment-Statistics:\n");
-	len += sprintf(buffer + len, "               Number of physical SCSI-devices..: %d (+ Adapter)\n", IBM_DS(shpnt).total_scsi_devices);
-	len += sprintf(buffer + len, "               Dynamical Assignment necessary...: %s\n", IBM_DS(shpnt).dyn_flag ? "Yes" : "No ");
-	len += sprintf(buffer + len, "               Next LDN to be assigned..........: 0x%x\n", next_ldn(shpnt));
-	len += sprintf(buffer + len, "               Dynamical assignments done yet...: %d\n", IBM_DS(shpnt).dynamical_assignments);
-	len += sprintf(buffer + len, "\n Current SCSI-Device-Mapping:\n");
-	len += sprintf(buffer + len, "        Physical SCSI-Device Map               Logical SCSI-Device Map\n");
-	len += sprintf(buffer + len, "    ID\\LUN  0  1  2  3  4  5  6  7       ID\\LUN  0  1  2  3  4  5  6  7\n");
-	for (id = 0; id < max_pun; id++) {
-		len += sprintf(buffer + len, "    %2d     ", id);
-		for (lun = 0; lun < 8; lun++)
-			len += sprintf(buffer + len, "%2s ", ti_p(get_scsi(shpnt)[id][lun]));
-		len += sprintf(buffer + len, "      %2d     ", id);
-		for (lun = 0; lun < 8; lun++)
-			len += sprintf(buffer + len, "%2s ", ti_l(get_ldn(shpnt)[id][lun]));
-		len += sprintf(buffer + len, "\n");
-	}
-
-	len += sprintf(buffer + len, "(A = IBM-Subsystem, D = Harddisk, T = Tapedrive, P = Processor, W = WORM,\n");
-	len += sprintf(buffer + len, " R = CD-ROM, S = Scanner, M = MO-Drive, C = Medium-Changer, + = unprovided LUN,\n");
-	len += sprintf(buffer + len, " - = nothing found, nothing assigned or unprobed LUN)\n\n");
-
-	*start = buffer + offset;
-	len -= offset;
-	if (len > length)
-		len = length;
-	spin_unlock_irqrestore(shpnt->host_lock, flags);
-	return len;
-}
-
-static int option_setup(char *str)
-{
-	int ints[IM_MAX_HOSTS];
-	char *cur = str;
-	int i = 1;
-
-	while (cur && isdigit(*cur) && i < IM_MAX_HOSTS) {
-		ints[i++] = simple_strtoul(cur, NULL, 0);
-		if ((cur = strchr(cur, ',')) != NULL)
-			cur++;
-	}
-	ints[0] = i - 1;
-	internal_ibmmca_scsi_setup(cur, ints);
-	return 1;
-}
-
-__setup("ibmmcascsi=", option_setup);
-
-static struct mca_driver ibmmca_driver = {
-	.id_table = ibmmca_id_table,
-	.driver = {
-		.name	= "ibmmca",
-		.bus	= &mca_bus_type,
-		.probe	= ibmmca_probe,
-		.remove	= __devexit_p(ibmmca_remove),
-	},
-};
-
-static int __init ibmmca_init(void)
-{
-#ifdef MODULE
-	/* If the driver is run as module, read from conf.modules or cmd-line */
-	if (boot_options)
-		option_setup(boot_options);
-#endif
-
-	return mca_register_driver_integrated(&ibmmca_driver, MCA_INTEGSCSI);
-}
-
-static void __exit ibmmca_exit(void)
-{
-	mca_unregister_driver(&ibmmca_driver);
-}
-
-module_init(ibmmca_init);
-module_exit(ibmmca_exit);
diff --git a/drivers/scsi/isci/host.c b/drivers/scsi/isci/host.c
index d4bf9c12ecd..45385f53164 100644
--- a/drivers/scsi/isci/host.c
+++ b/drivers/scsi/isci/host.c
@@ -192,22 +192,27 @@ static bool sci_controller_completion_queue_has_entries(struct isci_host *ihost)
 
 static bool sci_controller_isr(struct isci_host *ihost)
 {
-	if (sci_controller_completion_queue_has_entries(ihost)) {
+	if (sci_controller_completion_queue_has_entries(ihost))
 		return true;
-	} else {
-		/*
-		 * we have a spurious interrupt it could be that we have already
-		 * emptied the completion queue from a previous interrupt */
-		writel(SMU_ISR_COMPLETION, &ihost->smu_registers->interrupt_status);
 
-		/*
-		 * There is a race in the hardware that could cause us not to be notified
-		 * of an interrupt completion if we do not take this step.  We will mask
-		 * then unmask the interrupts so if there is another interrupt pending
-		 * the clearing of the interrupt source we get the next interrupt message. */
+	/* we have a spurious interrupt it could be that we have already
+	 * emptied the completion queue from a previous interrupt
+	 * FIXME: really!?
+	 */
+	writel(SMU_ISR_COMPLETION, &ihost->smu_registers->interrupt_status);
+
+	/* There is a race in the hardware that could cause us not to be
+	 * notified of an interrupt completion if we do not take this
+	 * step.  We will mask then unmask the interrupts so if there is
+	 * another interrupt pending the clearing of the interrupt
+	 * source we get the next interrupt message.
+	 */
+	spin_lock(&ihost->scic_lock);
+	if (test_bit(IHOST_IRQ_ENABLED, &ihost->flags)) {
 		writel(0xFF000000, &ihost->smu_registers->interrupt_mask);
 		writel(0, &ihost->smu_registers->interrupt_mask);
 	}
+	spin_unlock(&ihost->scic_lock);
 
 	return false;
 }
@@ -642,7 +647,6 @@ static void isci_host_start_complete(struct isci_host *ihost, enum sci_status co
 	if (completion_status != SCI_SUCCESS)
 		dev_info(&ihost->pdev->dev,
 			"controller start timed out, continuing...\n");
-	isci_host_change_state(ihost, isci_ready);
 	clear_bit(IHOST_START_PENDING, &ihost->flags);
 	wake_up(&ihost->eventq);
 }
@@ -657,12 +661,7 @@ int isci_host_scan_finished(struct Scsi_Host *shost, unsigned long time)
 
 	sas_drain_work(ha);
 
-	dev_dbg(&ihost->pdev->dev,
-		"%s: ihost->status = %d, time = %ld\n",
-		 __func__, isci_host_get_state(ihost), time);
-
 	return 1;
-
 }
 
 /**
@@ -704,14 +703,15 @@ static u32 sci_controller_get_suggested_start_timeout(struct isci_host *ihost)
 
 static void sci_controller_enable_interrupts(struct isci_host *ihost)
 {
-	BUG_ON(ihost->smu_registers == NULL);
+	set_bit(IHOST_IRQ_ENABLED, &ihost->flags);
 	writel(0, &ihost->smu_registers->interrupt_mask);
 }
 
 void sci_controller_disable_interrupts(struct isci_host *ihost)
 {
-	BUG_ON(ihost->smu_registers == NULL);
+	clear_bit(IHOST_IRQ_ENABLED, &ihost->flags);
 	writel(0xffffffff, &ihost->smu_registers->interrupt_mask);
+	readl(&ihost->smu_registers->interrupt_mask); /* flush */
 }
 
 static void sci_controller_enable_port_task_scheduler(struct isci_host *ihost)
@@ -822,7 +822,7 @@ static void sci_controller_initialize_unsolicited_frame_queue(struct isci_host *
 	       &ihost->scu_registers->sdma.unsolicited_frame_put_pointer);
 }
 
-static void sci_controller_transition_to_ready(struct isci_host *ihost, enum sci_status status)
+void sci_controller_transition_to_ready(struct isci_host *ihost, enum sci_status status)
 {
 	if (ihost->sm.current_state_id == SCIC_STARTING) {
 		/*
@@ -849,6 +849,7 @@ static bool is_phy_starting(struct isci_phy *iphy)
 	case SCI_PHY_SUB_AWAIT_SATA_POWER:
 	case SCI_PHY_SUB_AWAIT_SATA_PHY_EN:
 	case SCI_PHY_SUB_AWAIT_SATA_SPEED_EN:
+	case SCI_PHY_SUB_AWAIT_OSSP_EN:
 	case SCI_PHY_SUB_AWAIT_SIG_FIS_UF:
 	case SCI_PHY_SUB_FINAL:
 		return true;
@@ -857,6 +858,39 @@ static bool is_phy_starting(struct isci_phy *iphy)
 	}
 }
 
+bool is_controller_start_complete(struct isci_host *ihost)
+{
+	int i;
+
+	for (i = 0; i < SCI_MAX_PHYS; i++) {
+		struct isci_phy *iphy = &ihost->phys[i];
+		u32 state = iphy->sm.current_state_id;
+
+		/* in apc mode we need to check every phy, in
+		 * mpc mode we only need to check phys that have
+		 * been configured into a port
+		 */
+		if (is_port_config_apc(ihost))
+			/* pass */;
+		else if (!phy_get_non_dummy_port(iphy))
+			continue;
+
+		/* The controller start operation is complete iff:
+		 * - all links have been given an opportunity to start
+		 * - have no indication of a connected device
+		 * - have an indication of a connected device and it has
+		 *   finished the link training process.
+		 */
+		if ((iphy->is_in_link_training == false && state == SCI_PHY_INITIAL) ||
+		    (iphy->is_in_link_training == false && state == SCI_PHY_STOPPED) ||
+		    (iphy->is_in_link_training == true && is_phy_starting(iphy)) ||
+		    (ihost->port_agent.phy_ready_mask != ihost->port_agent.phy_configured_mask))
+			return false;
+	}
+
+	return true;
+}
+
 /**
  * sci_controller_start_next_phy - start phy
  * @scic: controller
@@ -877,36 +911,7 @@ static enum sci_status sci_controller_start_next_phy(struct isci_host *ihost)
 		return status;
 
 	if (ihost->next_phy_to_start >= SCI_MAX_PHYS) {
-		bool is_controller_start_complete = true;
-		u32 state;
-		u8 index;
-
-		for (index = 0; index < SCI_MAX_PHYS; index++) {
-			iphy = &ihost->phys[index];
-			state = iphy->sm.current_state_id;
-
-			if (!phy_get_non_dummy_port(iphy))
-				continue;
-
-			/* The controller start operation is complete iff:
-			 * - all links have been given an opportunity to start
-			 * - have no indication of a connected device
-			 * - have an indication of a connected device and it has
-			 *   finished the link training process.
-			 */
-			if ((iphy->is_in_link_training == false && state == SCI_PHY_INITIAL) ||
-			    (iphy->is_in_link_training == false && state == SCI_PHY_STOPPED) ||
-			    (iphy->is_in_link_training == true && is_phy_starting(iphy)) ||
-			    (ihost->port_agent.phy_ready_mask != ihost->port_agent.phy_configured_mask)) {
-				is_controller_start_complete = false;
-				break;
-			}
-		}
-
-		/*
-		 * The controller has successfully finished the start process.
-		 * Inform the SCI Core user and transition to the READY state. */
-		if (is_controller_start_complete == true) {
+		if (is_controller_start_complete(ihost)) {
 			sci_controller_transition_to_ready(ihost, SCI_SUCCESS);
 			sci_del_timer(&ihost->phy_timer);
 			ihost->phy_startup_timer_pending = false;
@@ -987,9 +992,8 @@ static enum sci_status sci_controller_start(struct isci_host *ihost,
 	u16 index;
 
 	if (ihost->sm.current_state_id != SCIC_INITIALIZED) {
-		dev_warn(&ihost->pdev->dev,
-			 "SCIC Controller start operation requested in "
-			 "invalid state\n");
+		dev_warn(&ihost->pdev->dev, "%s invalid state: %d\n",
+			 __func__, ihost->sm.current_state_id);
 		return SCI_FAILURE_INVALID_STATE;
 	}
 
@@ -1053,9 +1057,8 @@ void isci_host_scan_start(struct Scsi_Host *shost)
 	spin_unlock_irq(&ihost->scic_lock);
 }
 
-static void isci_host_stop_complete(struct isci_host *ihost, enum sci_status completion_status)
+static void isci_host_stop_complete(struct isci_host *ihost)
 {
-	isci_host_change_state(ihost, isci_stopped);
 	sci_controller_disable_interrupts(ihost);
 	clear_bit(IHOST_STOP_PENDING, &ihost->flags);
 	wake_up(&ihost->eventq);
@@ -1074,6 +1077,32 @@ static void sci_controller_completion_handler(struct isci_host *ihost)
 	writel(0, &ihost->smu_registers->interrupt_mask);
 }
 
+void ireq_done(struct isci_host *ihost, struct isci_request *ireq, struct sas_task *task)
+{
+	task->lldd_task = NULL;
+	if (!test_bit(IREQ_ABORT_PATH_ACTIVE, &ireq->flags) &&
+	    !(task->task_state_flags & SAS_TASK_STATE_ABORTED)) {
+		if (test_bit(IREQ_COMPLETE_IN_TARGET, &ireq->flags)) {
+			/* Normal notification (task_done) */
+			dev_dbg(&ihost->pdev->dev,
+				"%s: Normal - ireq/task = %p/%p\n",
+				__func__, ireq, task);
+
+			task->task_done(task);
+		} else {
+			dev_dbg(&ihost->pdev->dev,
+				"%s: Error - ireq/task = %p/%p\n",
+				__func__, ireq, task);
+
+			sas_task_abort(task);
+		}
+	}
+	if (test_and_clear_bit(IREQ_ABORT_PATH_ACTIVE, &ireq->flags))
+		wake_up_all(&ihost->eventq);
+
+	if (!test_bit(IREQ_NO_AUTO_FREE_TAG, &ireq->flags))
+		isci_free_tag(ihost, ireq->io_tag);
+}
 /**
  * isci_host_completion_routine() - This function is the delayed service
  *    routine that calls the sci core library's completion handler. It's
@@ -1082,107 +1111,15 @@ static void sci_controller_completion_handler(struct isci_host *ihost)
  * @data: This parameter specifies the ISCI host object
  *
  */
-static void isci_host_completion_routine(unsigned long data)
+void isci_host_completion_routine(unsigned long data)
 {
 	struct isci_host *ihost = (struct isci_host *)data;
-	struct list_head    completed_request_list;
-	struct list_head    errored_request_list;
-	struct list_head    *current_position;
-	struct list_head    *next_position;
-	struct isci_request *request;
-	struct isci_request *next_request;
-	struct sas_task     *task;
 	u16 active;
 
-	INIT_LIST_HEAD(&completed_request_list);
-	INIT_LIST_HEAD(&errored_request_list);
-
 	spin_lock_irq(&ihost->scic_lock);
-
 	sci_controller_completion_handler(ihost);
-
-	/* Take the lists of completed I/Os from the host. */
-
-	list_splice_init(&ihost->requests_to_complete,
-			 &completed_request_list);
-
-	/* Take the list of errored I/Os from the host. */
-	list_splice_init(&ihost->requests_to_errorback,
-			 &errored_request_list);
-
 	spin_unlock_irq(&ihost->scic_lock);
 
-	/* Process any completions in the lists. */
-	list_for_each_safe(current_position, next_position,
-			   &completed_request_list) {
-
-		request = list_entry(current_position, struct isci_request,
-				     completed_node);
-		task = isci_request_access_task(request);
-
-		/* Normal notification (task_done) */
-		dev_dbg(&ihost->pdev->dev,
-			"%s: Normal - request/task = %p/%p\n",
-			__func__,
-			request,
-			task);
-
-		/* Return the task to libsas */
-		if (task != NULL) {
-
-			task->lldd_task = NULL;
-			if (!(task->task_state_flags & SAS_TASK_STATE_ABORTED)) {
-
-				/* If the task is already in the abort path,
-				* the task_done callback cannot be called.
-				*/
-				task->task_done(task);
-			}
-		}
-
-		spin_lock_irq(&ihost->scic_lock);
-		isci_free_tag(ihost, request->io_tag);
-		spin_unlock_irq(&ihost->scic_lock);
-	}
-	list_for_each_entry_safe(request, next_request, &errored_request_list,
-				 completed_node) {
-
-		task = isci_request_access_task(request);
-
-		/* Use sas_task_abort */
-		dev_warn(&ihost->pdev->dev,
-			 "%s: Error - request/task = %p/%p\n",
-			 __func__,
-			 request,
-			 task);
-
-		if (task != NULL) {
-
-			/* Put the task into the abort path if it's not there
-			 * already.
-			 */
-			if (!(task->task_state_flags & SAS_TASK_STATE_ABORTED))
-				sas_task_abort(task);
-
-		} else {
-			/* This is a case where the request has completed with a
-			 * status such that it needed further target servicing,
-			 * but the sas_task reference has already been removed
-			 * from the request.  Since it was errored, it was not
-			 * being aborted, so there is nothing to do except free
-			 * it.
-			 */
-
-			spin_lock_irq(&ihost->scic_lock);
-			/* Remove the request from the remote device's list
-			* of pending requests.
-			*/
-			list_del_init(&request->dev_node);
-			isci_free_tag(ihost, request->io_tag);
-			spin_unlock_irq(&ihost->scic_lock);
-		}
-	}
-
 	/* the coalesence timeout doubles at each encoding step, so
 	 * update it based on the ilog2 value of the outstanding requests
 	 */
@@ -1213,9 +1150,8 @@ static void isci_host_completion_routine(unsigned long data)
 static enum sci_status sci_controller_stop(struct isci_host *ihost, u32 timeout)
 {
 	if (ihost->sm.current_state_id != SCIC_READY) {
-		dev_warn(&ihost->pdev->dev,
-			 "SCIC Controller stop operation requested in "
-			 "invalid state\n");
+		dev_warn(&ihost->pdev->dev, "%s invalid state: %d\n",
+			 __func__, ihost->sm.current_state_id);
 		return SCI_FAILURE_INVALID_STATE;
 	}
 
@@ -1241,7 +1177,7 @@ static enum sci_status sci_controller_reset(struct isci_host *ihost)
 	switch (ihost->sm.current_state_id) {
 	case SCIC_RESET:
 	case SCIC_READY:
-	case SCIC_STOPPED:
+	case SCIC_STOPPING:
 	case SCIC_FAILED:
 		/*
 		 * The reset operation is not a graceful cleanup, just
@@ -1250,13 +1186,50 @@ static enum sci_status sci_controller_reset(struct isci_host *ihost)
 		sci_change_state(&ihost->sm, SCIC_RESETTING);
 		return SCI_SUCCESS;
 	default:
-		dev_warn(&ihost->pdev->dev,
-			 "SCIC Controller reset operation requested in "
-			 "invalid state\n");
+		dev_warn(&ihost->pdev->dev, "%s invalid state: %d\n",
+			 __func__, ihost->sm.current_state_id);
 		return SCI_FAILURE_INVALID_STATE;
 	}
 }
 
+static enum sci_status sci_controller_stop_phys(struct isci_host *ihost)
+{
+	u32 index;
+	enum sci_status status;
+	enum sci_status phy_status;
+
+	status = SCI_SUCCESS;
+
+	for (index = 0; index < SCI_MAX_PHYS; index++) {
+		phy_status = sci_phy_stop(&ihost->phys[index]);
+
+		if (phy_status != SCI_SUCCESS &&
+		    phy_status != SCI_FAILURE_INVALID_STATE) {
+			status = SCI_FAILURE;
+
+			dev_warn(&ihost->pdev->dev,
+				 "%s: Controller stop operation failed to stop "
+				 "phy %d because of status %d.\n",
+				 __func__,
+				 ihost->phys[index].phy_index, phy_status);
+		}
+	}
+
+	return status;
+}
+
+
+/**
+ * isci_host_deinit - shutdown frame reception and dma
+ * @ihost: host to take down
+ *
+ * This is called in either the driver shutdown or the suspend path.  In
+ * the shutdown case libsas went through port teardown and normal device
+ * removal (i.e. physical links stayed up to service scsi_device removal
+ * commands).  In the suspend case we disable the hardware without
+ * notifying libsas of the link down events since we want libsas to
+ * remember the domain across the suspend/resume cycle
+ */
 void isci_host_deinit(struct isci_host *ihost)
 {
 	int i;
@@ -1265,17 +1238,6 @@ void isci_host_deinit(struct isci_host *ihost)
 	for (i = 0; i < isci_gpio_count(ihost); i++)
 		writel(SGPIO_HW_CONTROL, &ihost->scu_registers->peg0.sgpio.output_data_select[i]);
 
-	isci_host_change_state(ihost, isci_stopping);
-	for (i = 0; i < SCI_MAX_PORTS; i++) {
-		struct isci_port *iport = &ihost->ports[i];
-		struct isci_remote_device *idev, *d;
-
-		list_for_each_entry_safe(idev, d, &iport->remote_dev_list, node) {
-			if (test_bit(IDEV_ALLOCATED, &idev->flags))
-				isci_remote_device_stop(ihost, idev);
-		}
-	}
-
 	set_bit(IHOST_STOP_PENDING, &ihost->flags);
 
 	spin_lock_irq(&ihost->scic_lock);
@@ -1284,12 +1246,21 @@ void isci_host_deinit(struct isci_host *ihost)
 
 	wait_for_stop(ihost);
 
+	/* phy stop is after controller stop to allow port and device to
+	 * go idle before shutting down the phys, but the expectation is
+	 * that i/o has been shut off well before we reach this
+	 * function.
+	 */
+	sci_controller_stop_phys(ihost);
+
 	/* disable sgpio: where the above wait should give time for the
 	 * enclosure to sample the gpios going inactive
 	 */
 	writel(0, &ihost->scu_registers->peg0.sgpio.interface_control);
 
+	spin_lock_irq(&ihost->scic_lock);
 	sci_controller_reset(ihost);
+	spin_unlock_irq(&ihost->scic_lock);
 
 	/* Cancel any/all outstanding port timers */
 	for (i = 0; i < ihost->logical_port_entries; i++) {
@@ -1328,29 +1299,6 @@ static void __iomem *smu_base(struct isci_host *isci_host)
 	return pcim_iomap_table(pdev)[SCI_SMU_BAR * 2] + SCI_SMU_BAR_SIZE * id;
 }
 
-static void isci_user_parameters_get(struct sci_user_parameters *u)
-{
-	int i;
-
-	for (i = 0; i < SCI_MAX_PHYS; i++) {
-		struct sci_phy_user_params *u_phy = &u->phys[i];
-
-		u_phy->max_speed_generation = phy_gen;
-
-		/* we are not exporting these for now */
-		u_phy->align_insertion_frequency = 0x7f;
-		u_phy->in_connection_align_insertion_frequency = 0xff;
-		u_phy->notify_enable_spin_up_insertion_frequency = 0x33;
-	}
-
-	u->stp_inactivity_timeout = stp_inactive_to;
-	u->ssp_inactivity_timeout = ssp_inactive_to;
-	u->stp_max_occupancy_timeout = stp_max_occ_to;
-	u->ssp_max_occupancy_timeout = ssp_max_occ_to;
-	u->no_outbound_task_timeout = no_outbound_task_to;
-	u->max_concurr_spinup = max_concurr_spinup;
-}
-
 static void sci_controller_initial_state_enter(struct sci_base_state_machine *sm)
 {
 	struct isci_host *ihost = container_of(sm, typeof(*ihost), sm);
@@ -1510,32 +1458,6 @@ static void sci_controller_ready_state_exit(struct sci_base_state_machine *sm)
 	sci_controller_set_interrupt_coalescence(ihost, 0, 0);
 }
 
-static enum sci_status sci_controller_stop_phys(struct isci_host *ihost)
-{
-	u32 index;
-	enum sci_status status;
-	enum sci_status phy_status;
-
-	status = SCI_SUCCESS;
-
-	for (index = 0; index < SCI_MAX_PHYS; index++) {
-		phy_status = sci_phy_stop(&ihost->phys[index]);
-
-		if (phy_status != SCI_SUCCESS &&
-		    phy_status != SCI_FAILURE_INVALID_STATE) {
-			status = SCI_FAILURE;
-
-			dev_warn(&ihost->pdev->dev,
-				 "%s: Controller stop operation failed to stop "
-				 "phy %d because of status %d.\n",
-				 __func__,
-				 ihost->phys[index].phy_index, phy_status);
-		}
-	}
-
-	return status;
-}
-
 static enum sci_status sci_controller_stop_ports(struct isci_host *ihost)
 {
 	u32 index;
@@ -1595,10 +1517,11 @@ static void sci_controller_stopping_state_enter(struct sci_base_state_machine *s
 {
 	struct isci_host *ihost = container_of(sm, typeof(*ihost), sm);
 
-	/* Stop all of the components for this controller */
-	sci_controller_stop_phys(ihost);
-	sci_controller_stop_ports(ihost);
 	sci_controller_stop_devices(ihost);
+	sci_controller_stop_ports(ihost);
+
+	if (!sci_controller_has_remote_devices_stopping(ihost))
+		isci_host_stop_complete(ihost);
 }
 
 static void sci_controller_stopping_state_exit(struct sci_base_state_machine *sm)
@@ -1624,6 +1547,9 @@ static void sci_controller_reset_hardware(struct isci_host *ihost)
 
 	/* The write to the UFQGP clears the UFQPR */
 	writel(0, &ihost->scu_registers->sdma.unsolicited_frame_get_pointer);
+
+	/* clear all interrupts */
+	writel(~SMU_INTERRUPT_STATUS_RESERVED_MASK, &ihost->smu_registers->interrupt_status);
 }
 
 static void sci_controller_resetting_state_enter(struct sci_base_state_machine *sm)
@@ -1655,59 +1581,9 @@ static const struct sci_base_state sci_controller_state_table[] = {
 		.enter_state = sci_controller_stopping_state_enter,
 		.exit_state = sci_controller_stopping_state_exit,
 	},
-	[SCIC_STOPPED] = {},
 	[SCIC_FAILED] = {}
 };
 
-static void sci_controller_set_default_config_parameters(struct isci_host *ihost)
-{
-	/* these defaults are overridden by the platform / firmware */
-	u16 index;
-
-	/* Default to APC mode. */
-	ihost->oem_parameters.controller.mode_type = SCIC_PORT_AUTOMATIC_CONFIGURATION_MODE;
-
-	/* Default to APC mode. */
-	ihost->oem_parameters.controller.max_concurr_spin_up = 1;
-
-	/* Default to no SSC operation. */
-	ihost->oem_parameters.controller.do_enable_ssc = false;
-
-	/* Default to short cables on all phys. */
-	ihost->oem_parameters.controller.cable_selection_mask = 0;
-
-	/* Initialize all of the port parameter information to narrow ports. */
-	for (index = 0; index < SCI_MAX_PORTS; index++) {
-		ihost->oem_parameters.ports[index].phy_mask = 0;
-	}
-
-	/* Initialize all of the phy parameter information. */
-	for (index = 0; index < SCI_MAX_PHYS; index++) {
-		/* Default to 3G (i.e. Gen 2). */
-		ihost->user_parameters.phys[index].max_speed_generation =
-			SCIC_SDS_PARM_GEN2_SPEED;
-
-		/* the frequencies cannot be 0 */
-		ihost->user_parameters.phys[index].align_insertion_frequency = 0x7f;
-		ihost->user_parameters.phys[index].in_connection_align_insertion_frequency = 0xff;
-		ihost->user_parameters.phys[index].notify_enable_spin_up_insertion_frequency = 0x33;
-
-		/*
-		 * Previous Vitesse based expanders had a arbitration issue that
-		 * is worked around by having the upper 32-bits of SAS address
-		 * with a value greater then the Vitesse company identifier.
-		 * Hence, usage of 0x5FCFFFFF. */
-		ihost->oem_parameters.phys[index].sas_address.low = 0x1 + ihost->id;
-		ihost->oem_parameters.phys[index].sas_address.high = 0x5FCFFFFF;
-	}
-
-	ihost->user_parameters.stp_inactivity_timeout = 5;
-	ihost->user_parameters.ssp_inactivity_timeout = 5;
-	ihost->user_parameters.stp_max_occupancy_timeout = 5;
-	ihost->user_parameters.ssp_max_occupancy_timeout = 20;
-	ihost->user_parameters.no_outbound_task_timeout = 2;
-}
-
 static void controller_timeout(unsigned long data)
 {
 	struct sci_timer *tmr = (struct sci_timer *)data;
@@ -1724,7 +1600,7 @@ static void controller_timeout(unsigned long data)
 		sci_controller_transition_to_ready(ihost, SCI_FAILURE_TIMEOUT);
 	else if (sm->current_state_id == SCIC_STOPPING) {
 		sci_change_state(sm, SCIC_FAILED);
-		isci_host_stop_complete(ihost, SCI_FAILURE_TIMEOUT);
+		isci_host_stop_complete(ihost);
 	} else	/* / @todo Now what do we want to do in this case? */
 		dev_err(&ihost->pdev->dev,
 			"%s: Controller timer fired when controller was not "
@@ -1764,9 +1640,6 @@ static enum sci_status sci_controller_construct(struct isci_host *ihost,
 
 	sci_init_timer(&ihost->timer, controller_timeout);
 
-	/* Initialize the User and OEM parameters to default values. */
-	sci_controller_set_default_config_parameters(ihost);
-
 	return sci_controller_reset(ihost);
 }
 
@@ -1846,27 +1719,6 @@ int sci_oem_parameters_validate(struct sci_oem_params *oem, u8 version)
 	return 0;
 }
 
-static enum sci_status sci_oem_parameters_set(struct isci_host *ihost)
-{
-	u32 state = ihost->sm.current_state_id;
-	struct isci_pci_info *pci_info = to_pci_info(ihost->pdev);
-
-	if (state == SCIC_RESET ||
-	    state == SCIC_INITIALIZING ||
-	    state == SCIC_INITIALIZED) {
-		u8 oem_version = pci_info->orom ? pci_info->orom->hdr.version :
-			ISCI_ROM_VER_1_0;
-
-		if (sci_oem_parameters_validate(&ihost->oem_parameters,
-						oem_version))
-			return SCI_FAILURE_INVALID_PARAMETER_VALUE;
-
-		return SCI_SUCCESS;
-	}
-
-	return SCI_FAILURE_INVALID_STATE;
-}
-
 static u8 max_spin_up(struct isci_host *ihost)
 {
 	if (ihost->user_parameters.max_concurr_spinup)
@@ -1914,7 +1766,7 @@ static void power_control_timeout(unsigned long data)
 		ihost->power_control.phys_granted_power++;
 		sci_phy_consume_power_handler(iphy);
 
-		if (iphy->protocol == SCIC_SDS_PHY_PROTOCOL_SAS) {
+		if (iphy->protocol == SAS_PROTOCOL_SSP) {
 			u8 j;
 
 			for (j = 0; j < SCI_MAX_PHYS; j++) {
@@ -1988,7 +1840,7 @@ void sci_controller_power_control_queue_insert(struct isci_host *ihost,
 				       sizeof(current_phy->frame_rcvd.iaf.sas_addr));
 
 			if (current_phy->sm.current_state_id == SCI_PHY_READY &&
-			    current_phy->protocol == SCIC_SDS_PHY_PROTOCOL_SAS &&
+			    current_phy->protocol == SAS_PROTOCOL_SSP &&
 			    other == 0) {
 				sci_phy_consume_power_handler(iphy);
 				break;
@@ -2279,9 +2131,8 @@ static enum sci_status sci_controller_initialize(struct isci_host *ihost)
 	unsigned long i, state, val;
 
 	if (ihost->sm.current_state_id != SCIC_RESET) {
-		dev_warn(&ihost->pdev->dev,
-			 "SCIC Controller initialize operation requested "
-			 "in invalid state\n");
+		dev_warn(&ihost->pdev->dev, "%s invalid state: %d\n",
+			 __func__, ihost->sm.current_state_id);
 		return SCI_FAILURE_INVALID_STATE;
 	}
 
@@ -2384,96 +2235,76 @@ static enum sci_status sci_controller_initialize(struct isci_host *ihost)
 	return result;
 }
 
-static enum sci_status sci_user_parameters_set(struct isci_host *ihost,
-					       struct sci_user_parameters *sci_parms)
-{
-	u32 state = ihost->sm.current_state_id;
-
-	if (state == SCIC_RESET ||
-	    state == SCIC_INITIALIZING ||
-	    state == SCIC_INITIALIZED) {
-		u16 index;
-
-		/*
-		 * Validate the user parameters.  If they are not legal, then
-		 * return a failure.
-		 */
-		for (index = 0; index < SCI_MAX_PHYS; index++) {
-			struct sci_phy_user_params *user_phy;
-
-			user_phy = &sci_parms->phys[index];
-
-			if (!((user_phy->max_speed_generation <=
-						SCIC_SDS_PARM_MAX_SPEED) &&
-			      (user_phy->max_speed_generation >
-						SCIC_SDS_PARM_NO_SPEED)))
-				return SCI_FAILURE_INVALID_PARAMETER_VALUE;
-
-			if (user_phy->in_connection_align_insertion_frequency <
-					3)
-				return SCI_FAILURE_INVALID_PARAMETER_VALUE;
-
-			if ((user_phy->in_connection_align_insertion_frequency <
-						3) ||
-			    (user_phy->align_insertion_frequency == 0) ||
-			    (user_phy->
-				notify_enable_spin_up_insertion_frequency ==
-						0))
-				return SCI_FAILURE_INVALID_PARAMETER_VALUE;
-		}
-
-		if ((sci_parms->stp_inactivity_timeout == 0) ||
-		    (sci_parms->ssp_inactivity_timeout == 0) ||
-		    (sci_parms->stp_max_occupancy_timeout == 0) ||
-		    (sci_parms->ssp_max_occupancy_timeout == 0) ||
-		    (sci_parms->no_outbound_task_timeout == 0))
-			return SCI_FAILURE_INVALID_PARAMETER_VALUE;
-
-		memcpy(&ihost->user_parameters, sci_parms, sizeof(*sci_parms));
-
-		return SCI_SUCCESS;
-	}
-
-	return SCI_FAILURE_INVALID_STATE;
-}
-
-static int sci_controller_mem_init(struct isci_host *ihost)
+static int sci_controller_dma_alloc(struct isci_host *ihost)
 {
 	struct device *dev = &ihost->pdev->dev;
-	dma_addr_t dma;
 	size_t size;
-	int err;
+	int i;
+
+	/* detect re-initialization */
+	if (ihost->completion_queue)
+		return 0;
 
 	size = SCU_MAX_COMPLETION_QUEUE_ENTRIES * sizeof(u32);
-	ihost->completion_queue = dmam_alloc_coherent(dev, size, &dma, GFP_KERNEL);
+	ihost->completion_queue = dmam_alloc_coherent(dev, size, &ihost->cq_dma,
+						      GFP_KERNEL);
 	if (!ihost->completion_queue)
 		return -ENOMEM;
 
-	writel(lower_32_bits(dma), &ihost->smu_registers->completion_queue_lower);
-	writel(upper_32_bits(dma), &ihost->smu_registers->completion_queue_upper);
-
 	size = ihost->remote_node_entries * sizeof(union scu_remote_node_context);
-	ihost->remote_node_context_table = dmam_alloc_coherent(dev, size, &dma,
+	ihost->remote_node_context_table = dmam_alloc_coherent(dev, size, &ihost->rnc_dma,
 							       GFP_KERNEL);
+
 	if (!ihost->remote_node_context_table)
 		return -ENOMEM;
 
-	writel(lower_32_bits(dma), &ihost->smu_registers->remote_node_context_lower);
-	writel(upper_32_bits(dma), &ihost->smu_registers->remote_node_context_upper);
-
 	size = ihost->task_context_entries * sizeof(struct scu_task_context),
-	ihost->task_context_table = dmam_alloc_coherent(dev, size, &dma, GFP_KERNEL);
+	ihost->task_context_table = dmam_alloc_coherent(dev, size, &ihost->tc_dma,
+							GFP_KERNEL);
 	if (!ihost->task_context_table)
 		return -ENOMEM;
 
-	ihost->task_context_dma = dma;
-	writel(lower_32_bits(dma), &ihost->smu_registers->host_task_table_lower);
-	writel(upper_32_bits(dma), &ihost->smu_registers->host_task_table_upper);
+	size = SCI_UFI_TOTAL_SIZE;
+	ihost->ufi_buf = dmam_alloc_coherent(dev, size, &ihost->ufi_dma, GFP_KERNEL);
+	if (!ihost->ufi_buf)
+		return -ENOMEM;
+
+	for (i = 0; i < SCI_MAX_IO_REQUESTS; i++) {
+		struct isci_request *ireq;
+		dma_addr_t dma;
+
+		ireq = dmam_alloc_coherent(dev, sizeof(*ireq), &dma, GFP_KERNEL);
+		if (!ireq)
+			return -ENOMEM;
+
+		ireq->tc = &ihost->task_context_table[i];
+		ireq->owning_controller = ihost;
+		ireq->request_daddr = dma;
+		ireq->isci_host = ihost;
+		ihost->reqs[i] = ireq;
+	}
+
+	return 0;
+}
+
+static int sci_controller_mem_init(struct isci_host *ihost)
+{
+	int err = sci_controller_dma_alloc(ihost);
 
-	err = sci_unsolicited_frame_control_construct(ihost);
 	if (err)
 		return err;
 
+	writel(lower_32_bits(ihost->cq_dma), &ihost->smu_registers->completion_queue_lower);
+	writel(upper_32_bits(ihost->cq_dma), &ihost->smu_registers->completion_queue_upper);
+
+	writel(lower_32_bits(ihost->rnc_dma), &ihost->smu_registers->remote_node_context_lower);
+	writel(upper_32_bits(ihost->rnc_dma), &ihost->smu_registers->remote_node_context_upper);
+
+	writel(lower_32_bits(ihost->tc_dma), &ihost->smu_registers->host_task_table_lower);
+	writel(upper_32_bits(ihost->tc_dma), &ihost->smu_registers->host_task_table_upper);
+
+	sci_unsolicited_frame_control_construct(ihost);
+
 	/*
 	 * Inform the silicon as to the location of the UF headers and
 	 * address table.
@@ -2491,22 +2322,22 @@ static int sci_controller_mem_init(struct isci_host *ihost)
 	return 0;
 }
 
+/**
+ * isci_host_init - (re-)initialize hardware and internal (private) state
+ * @ihost: host to init
+ *
+ * Any public facing objects (like asd_sas_port, and asd_sas_phys), or
+ * one-time initialization objects like locks and waitqueues, are
+ * not touched (they are initialized in isci_host_alloc)
+ */
 int isci_host_init(struct isci_host *ihost)
 {
-	int err = 0, i;
+	int i, err;
 	enum sci_status status;
-	struct sci_user_parameters sci_user_params;
-	struct isci_pci_info *pci_info = to_pci_info(ihost->pdev);
-
-	spin_lock_init(&ihost->state_lock);
-	spin_lock_init(&ihost->scic_lock);
-	init_waitqueue_head(&ihost->eventq);
-
-	isci_host_change_state(ihost, isci_starting);
-
-	status = sci_controller_construct(ihost, scu_base(ihost),
-					  smu_base(ihost));
 
+	spin_lock_irq(&ihost->scic_lock);
+	status = sci_controller_construct(ihost, scu_base(ihost), smu_base(ihost));
+	spin_unlock_irq(&ihost->scic_lock);
 	if (status != SCI_SUCCESS) {
 		dev_err(&ihost->pdev->dev,
 			"%s: sci_controller_construct failed - status = %x\n",
@@ -2515,48 +2346,6 @@ int isci_host_init(struct isci_host *ihost)
 		return -ENODEV;
 	}
 
-	ihost->sas_ha.dev = &ihost->pdev->dev;
-	ihost->sas_ha.lldd_ha = ihost;
-
-	/*
-	 * grab initial values stored in the controller object for OEM and USER
-	 * parameters
-	 */
-	isci_user_parameters_get(&sci_user_params);
-	status = sci_user_parameters_set(ihost, &sci_user_params);
-	if (status != SCI_SUCCESS) {
-		dev_warn(&ihost->pdev->dev,
-			 "%s: sci_user_parameters_set failed\n",
-			 __func__);
-		return -ENODEV;
-	}
-
-	/* grab any OEM parameters specified in orom */
-	if (pci_info->orom) {
-		status = isci_parse_oem_parameters(&ihost->oem_parameters,
-						   pci_info->orom,
-						   ihost->id);
-		if (status != SCI_SUCCESS) {
-			dev_warn(&ihost->pdev->dev,
-				 "parsing firmware oem parameters failed\n");
-			return -EINVAL;
-		}
-	}
-
-	status = sci_oem_parameters_set(ihost);
-	if (status != SCI_SUCCESS) {
-		dev_warn(&ihost->pdev->dev,
-				"%s: sci_oem_parameters_set failed\n",
-				__func__);
-		return -ENODEV;
-	}
-
-	tasklet_init(&ihost->completion_tasklet,
-		     isci_host_completion_routine, (unsigned long)ihost);
-
-	INIT_LIST_HEAD(&ihost->requests_to_complete);
-	INIT_LIST_HEAD(&ihost->requests_to_errorback);
-
 	spin_lock_irq(&ihost->scic_lock);
 	status = sci_controller_initialize(ihost);
 	spin_unlock_irq(&ihost->scic_lock);
@@ -2572,43 +2361,12 @@ int isci_host_init(struct isci_host *ihost)
 	if (err)
 		return err;
 
-	for (i = 0; i < SCI_MAX_PORTS; i++)
-		isci_port_init(&ihost->ports[i], ihost, i);
-
-	for (i = 0; i < SCI_MAX_PHYS; i++)
-		isci_phy_init(&ihost->phys[i], ihost, i);
-
 	/* enable sgpio */
 	writel(1, &ihost->scu_registers->peg0.sgpio.interface_control);
 	for (i = 0; i < isci_gpio_count(ihost); i++)
 		writel(SGPIO_HW_CONTROL, &ihost->scu_registers->peg0.sgpio.output_data_select[i]);
 	writel(0, &ihost->scu_registers->peg0.sgpio.vendor_specific_code);
 
-	for (i = 0; i < SCI_MAX_REMOTE_DEVICES; i++) {
-		struct isci_remote_device *idev = &ihost->devices[i];
-
-		INIT_LIST_HEAD(&idev->reqs_in_process);
-		INIT_LIST_HEAD(&idev->node);
-	}
-
-	for (i = 0; i < SCI_MAX_IO_REQUESTS; i++) {
-		struct isci_request *ireq;
-		dma_addr_t dma;
-
-		ireq = dmam_alloc_coherent(&ihost->pdev->dev,
-					   sizeof(struct isci_request), &dma,
-					   GFP_KERNEL);
-		if (!ireq)
-			return -ENOMEM;
-
-		ireq->tc = &ihost->task_context_table[i];
-		ireq->owning_controller = ihost;
-		spin_lock_init(&ireq->state_lock);
-		ireq->request_daddr = dma;
-		ireq->isci_host = ihost;
-		ihost->reqs[i] = ireq;
-	}
-
 	return 0;
 }
 
@@ -2654,7 +2412,7 @@ void sci_controller_link_down(struct isci_host *ihost, struct isci_port *iport,
 	}
 }
 
-static bool sci_controller_has_remote_devices_stopping(struct isci_host *ihost)
+bool sci_controller_has_remote_devices_stopping(struct isci_host *ihost)
 {
 	u32 index;
 
@@ -2680,7 +2438,7 @@ void sci_controller_remote_device_stopped(struct isci_host *ihost,
 	}
 
 	if (!sci_controller_has_remote_devices_stopping(ihost))
-		sci_change_state(&ihost->sm, SCIC_STOPPED);
+		isci_host_stop_complete(ihost);
 }
 
 void sci_controller_post_request(struct isci_host *ihost, u32 request)
@@ -2842,7 +2600,8 @@ enum sci_status sci_controller_start_io(struct isci_host *ihost,
 	enum sci_status status;
 
 	if (ihost->sm.current_state_id != SCIC_READY) {
-		dev_warn(&ihost->pdev->dev, "invalid state to start I/O");
+		dev_warn(&ihost->pdev->dev, "%s invalid state: %d\n",
+			 __func__, ihost->sm.current_state_id);
 		return SCI_FAILURE_INVALID_STATE;
 	}
 
@@ -2866,22 +2625,26 @@ enum sci_status sci_controller_terminate_request(struct isci_host *ihost,
 	enum sci_status status;
 
 	if (ihost->sm.current_state_id != SCIC_READY) {
-		dev_warn(&ihost->pdev->dev,
-			 "invalid state to terminate request\n");
+		dev_warn(&ihost->pdev->dev, "%s invalid state: %d\n",
+			 __func__, ihost->sm.current_state_id);
 		return SCI_FAILURE_INVALID_STATE;
 	}
-
 	status = sci_io_request_terminate(ireq);
-	if (status != SCI_SUCCESS)
-		return status;
 
-	/*
-	 * Utilize the original post context command and or in the POST_TC_ABORT
-	 * request sub-type.
-	 */
-	sci_controller_post_request(ihost,
-				    ireq->post_context | SCU_CONTEXT_COMMAND_REQUEST_POST_TC_ABORT);
-	return SCI_SUCCESS;
+	dev_dbg(&ihost->pdev->dev, "%s: status=%d; ireq=%p; flags=%lx\n",
+		__func__, status, ireq, ireq->flags);
+
+	if ((status == SCI_SUCCESS) &&
+	    !test_bit(IREQ_PENDING_ABORT, &ireq->flags) &&
+	    !test_and_set_bit(IREQ_TC_ABORT_POSTED, &ireq->flags)) {
+		/* Utilize the original post context command and or in the
+		 * POST_TC_ABORT request sub-type.
+		 */
+		sci_controller_post_request(
+			ihost, ireq->post_context |
+				SCU_CONTEXT_COMMAND_REQUEST_POST_TC_ABORT);
+	}
+	return status;
 }
 
 /**
@@ -2915,7 +2678,8 @@ enum sci_status sci_controller_complete_io(struct isci_host *ihost,
 		clear_bit(IREQ_ACTIVE, &ireq->flags);
 		return SCI_SUCCESS;
 	default:
-		dev_warn(&ihost->pdev->dev, "invalid state to complete I/O");
+		dev_warn(&ihost->pdev->dev, "%s invalid state: %d\n",
+			 __func__, ihost->sm.current_state_id);
 		return SCI_FAILURE_INVALID_STATE;
 	}
 
@@ -2926,7 +2690,8 @@ enum sci_status sci_controller_continue_io(struct isci_request *ireq)
 	struct isci_host *ihost = ireq->owning_controller;
 
 	if (ihost->sm.current_state_id != SCIC_READY) {
-		dev_warn(&ihost->pdev->dev, "invalid state to continue I/O");
+		dev_warn(&ihost->pdev->dev, "%s invalid state: %d\n",
+			 __func__, ihost->sm.current_state_id);
 		return SCI_FAILURE_INVALID_STATE;
 	}
 
diff --git a/drivers/scsi/isci/host.h b/drivers/scsi/isci/host.h
index adbad69d106..9ab58e0540e 100644
--- a/drivers/scsi/isci/host.h
+++ b/drivers/scsi/isci/host.h
@@ -55,6 +55,7 @@
 #ifndef _SCI_HOST_H_
 #define _SCI_HOST_H_
 
+#include <scsi/sas_ata.h>
 #include "remote_device.h"
 #include "phy.h"
 #include "isci.h"
@@ -108,6 +109,8 @@ struct sci_port_configuration_agent;
 typedef void (*port_config_fn)(struct isci_host *,
 			       struct sci_port_configuration_agent *,
 			       struct isci_port *, struct isci_phy *);
+bool is_port_config_apc(struct isci_host *ihost);
+bool is_controller_start_complete(struct isci_host *ihost);
 
 struct sci_port_configuration_agent {
 	u16 phy_configured_mask;
@@ -157,13 +160,17 @@ struct isci_host {
 	struct sci_power_control power_control;
 	u8 io_request_sequence[SCI_MAX_IO_REQUESTS];
 	struct scu_task_context *task_context_table;
-	dma_addr_t task_context_dma;
+	dma_addr_t tc_dma;
 	union scu_remote_node_context *remote_node_context_table;
+	dma_addr_t rnc_dma;
 	u32 *completion_queue;
+	dma_addr_t cq_dma;
 	u32 completion_queue_get;
 	u32 logical_port_entries;
 	u32 remote_node_entries;
 	u32 task_context_entries;
+	void *ufi_buf;
+	dma_addr_t ufi_dma;
 	struct sci_unsolicited_frame_control uf_control;
 
 	/* phy startup */
@@ -190,17 +197,13 @@ struct isci_host {
 	struct asd_sas_port sas_ports[SCI_MAX_PORTS];
 	struct sas_ha_struct sas_ha;
 
-	spinlock_t state_lock;
 	struct pci_dev *pdev;
-	enum isci_status status;
 	#define IHOST_START_PENDING 0
 	#define IHOST_STOP_PENDING 1
+	#define IHOST_IRQ_ENABLED 2
 	unsigned long flags;
 	wait_queue_head_t eventq;
-	struct Scsi_Host *shost;
 	struct tasklet_struct completion_tasklet;
-	struct list_head requests_to_complete;
-	struct list_head requests_to_errorback;
 	spinlock_t scic_lock;
 	struct isci_request *reqs[SCI_MAX_IO_REQUESTS];
 	struct isci_remote_device devices[SCI_MAX_REMOTE_DEVICES];
@@ -274,13 +277,6 @@ enum sci_controller_states {
 	SCIC_STOPPING,
 
 	/**
-	 * This state indicates that the controller has successfully been stopped.
-	 * In this state no new IO operations are permitted.
-	 * This state is entered from the STOPPING state.
-	 */
-	SCIC_STOPPED,
-
-	/**
 	 * This state indicates that the controller could not successfully be
 	 * initialized.  In this state no new IO operations are permitted.
 	 * This state is entered from the INITIALIZING state.
@@ -309,32 +305,16 @@ static inline struct isci_pci_info *to_pci_info(struct pci_dev *pdev)
 	return pci_get_drvdata(pdev);
 }
 
+static inline struct Scsi_Host *to_shost(struct isci_host *ihost)
+{
+	return ihost->sas_ha.core.shost;
+}
+
 #define for_each_isci_host(id, ihost, pdev) \
 	for (id = 0, ihost = to_pci_info(pdev)->hosts[id]; \
 	     id < ARRAY_SIZE(to_pci_info(pdev)->hosts) && ihost; \
 	     ihost = to_pci_info(pdev)->hosts[++id])
 
-static inline enum isci_status isci_host_get_state(struct isci_host *isci_host)
-{
-	return isci_host->status;
-}
-
-static inline void isci_host_change_state(struct isci_host *isci_host,
-					  enum isci_status status)
-{
-	unsigned long flags;
-
-	dev_dbg(&isci_host->pdev->dev,
-		"%s: isci_host = %p, state = 0x%x",
-		__func__,
-		isci_host,
-		status);
-	spin_lock_irqsave(&isci_host->state_lock, flags);
-	isci_host->status = status;
-	spin_unlock_irqrestore(&isci_host->state_lock, flags);
-
-}
-
 static inline void wait_for_start(struct isci_host *ihost)
 {
 	wait_event(ihost->eventq, !test_bit(IHOST_START_PENDING, &ihost->flags));
@@ -360,6 +340,11 @@ static inline struct isci_host *dev_to_ihost(struct domain_device *dev)
 	return dev->port->ha->lldd_ha;
 }
 
+static inline struct isci_host *idev_to_ihost(struct isci_remote_device *idev)
+{
+	return dev_to_ihost(idev->domain_dev);
+}
+
 /* we always use protocol engine group zero */
 #define ISCI_PEG 0
 
@@ -378,8 +363,7 @@ static inline int sci_remote_device_node_count(struct isci_remote_device *idev)
 {
 	struct domain_device *dev = idev->domain_dev;
 
-	if ((dev->dev_type == SATA_DEV || (dev->tproto & SAS_PROTOCOL_STP)) &&
-	    !idev->is_direct_attached)
+	if (dev_is_sata(dev) && dev->parent)
 		return SCU_STP_REMOTE_NODE_COUNT;
 	return SCU_SSP_REMOTE_NODE_COUNT;
 }
@@ -475,36 +459,17 @@ void sci_controller_free_remote_node_context(
 	struct isci_remote_device *idev,
 	u16 node_id);
 
-struct isci_request *sci_request_by_tag(struct isci_host *ihost,
-					     u16 io_tag);
-
-void sci_controller_power_control_queue_insert(
-	struct isci_host *ihost,
-	struct isci_phy *iphy);
-
-void sci_controller_power_control_queue_remove(
-	struct isci_host *ihost,
-	struct isci_phy *iphy);
-
-void sci_controller_link_up(
-	struct isci_host *ihost,
-	struct isci_port *iport,
-	struct isci_phy *iphy);
-
-void sci_controller_link_down(
-	struct isci_host *ihost,
-	struct isci_port *iport,
-	struct isci_phy *iphy);
-
-void sci_controller_remote_device_stopped(
-	struct isci_host *ihost,
-	struct isci_remote_device *idev);
-
-void sci_controller_copy_task_context(
-	struct isci_host *ihost,
-	struct isci_request *ireq);
-
-void sci_controller_register_setup(struct isci_host *ihost);
+struct isci_request *sci_request_by_tag(struct isci_host *ihost, u16 io_tag);
+void sci_controller_power_control_queue_insert(struct isci_host *ihost,
+					       struct isci_phy *iphy);
+void sci_controller_power_control_queue_remove(struct isci_host *ihost,
+					       struct isci_phy *iphy);
+void sci_controller_link_up(struct isci_host *ihost, struct isci_port *iport,
+			    struct isci_phy *iphy);
+void sci_controller_link_down(struct isci_host *ihost, struct isci_port *iport,
+			      struct isci_phy *iphy);
+void sci_controller_remote_device_stopped(struct isci_host *ihost,
+					  struct isci_remote_device *idev);
 
 enum sci_status sci_controller_continue_io(struct isci_request *ireq);
 int isci_host_scan_finished(struct Scsi_Host *, unsigned long);
@@ -512,29 +477,14 @@ void isci_host_scan_start(struct Scsi_Host *);
 u16 isci_alloc_tag(struct isci_host *ihost);
 enum sci_status isci_free_tag(struct isci_host *ihost, u16 io_tag);
 void isci_tci_free(struct isci_host *ihost, u16 tci);
+void ireq_done(struct isci_host *ihost, struct isci_request *ireq, struct sas_task *task);
 
 int isci_host_init(struct isci_host *);
-
-void isci_host_init_controller_names(
-	struct isci_host *isci_host,
-	unsigned int controller_idx);
-
-void isci_host_deinit(
-	struct isci_host *);
-
-void isci_host_port_link_up(
-	struct isci_host *,
-	struct isci_port *,
-	struct isci_phy *);
-int isci_host_dev_found(struct domain_device *);
-
-void isci_host_remote_device_start_complete(
-	struct isci_host *,
-	struct isci_remote_device *,
-	enum sci_status);
-
-void sci_controller_disable_interrupts(
-	struct isci_host *ihost);
+void isci_host_completion_routine(unsigned long data);
+void isci_host_deinit(struct isci_host *);
+void sci_controller_disable_interrupts(struct isci_host *ihost);
+bool sci_controller_has_remote_devices_stopping(struct isci_host *ihost);
+void sci_controller_transition_to_ready(struct isci_host *ihost, enum sci_status status);
 
 enum sci_status sci_controller_start_io(
 	struct isci_host *ihost,
diff --git a/drivers/scsi/isci/init.c b/drivers/scsi/isci/init.c
index 5137db5a5d8..47e28b55502 100644
--- a/drivers/scsi/isci/init.c
+++ b/drivers/scsi/isci/init.c
@@ -271,13 +271,12 @@ static void isci_unregister(struct isci_host *isci_host)
 	if (!isci_host)
 		return;
 
-	shost = isci_host->shost;
-
 	sas_unregister_ha(&isci_host->sas_ha);
 
-	sas_remove_host(isci_host->shost);
-	scsi_remove_host(isci_host->shost);
-	scsi_host_put(isci_host->shost);
+	shost = to_shost(isci_host);
+	sas_remove_host(shost);
+	scsi_remove_host(shost);
+	scsi_host_put(shost);
 }
 
 static int __devinit isci_pci_init(struct pci_dev *pdev)
@@ -397,38 +396,199 @@ static int isci_setup_interrupts(struct pci_dev *pdev)
 	return err;
 }
 
+static void isci_user_parameters_get(struct sci_user_parameters *u)
+{
+	int i;
+
+	for (i = 0; i < SCI_MAX_PHYS; i++) {
+		struct sci_phy_user_params *u_phy = &u->phys[i];
+
+		u_phy->max_speed_generation = phy_gen;
+
+		/* we are not exporting these for now */
+		u_phy->align_insertion_frequency = 0x7f;
+		u_phy->in_connection_align_insertion_frequency = 0xff;
+		u_phy->notify_enable_spin_up_insertion_frequency = 0x33;
+	}
+
+	u->stp_inactivity_timeout = stp_inactive_to;
+	u->ssp_inactivity_timeout = ssp_inactive_to;
+	u->stp_max_occupancy_timeout = stp_max_occ_to;
+	u->ssp_max_occupancy_timeout = ssp_max_occ_to;
+	u->no_outbound_task_timeout = no_outbound_task_to;
+	u->max_concurr_spinup = max_concurr_spinup;
+}
+
+static enum sci_status sci_user_parameters_set(struct isci_host *ihost,
+					       struct sci_user_parameters *sci_parms)
+{
+	u16 index;
+
+	/*
+	 * Validate the user parameters.  If they are not legal, then
+	 * return a failure.
+	 */
+	for (index = 0; index < SCI_MAX_PHYS; index++) {
+		struct sci_phy_user_params *u;
+
+		u = &sci_parms->phys[index];
+
+		if (!((u->max_speed_generation <= SCIC_SDS_PARM_MAX_SPEED) &&
+		      (u->max_speed_generation > SCIC_SDS_PARM_NO_SPEED)))
+			return SCI_FAILURE_INVALID_PARAMETER_VALUE;
+
+		if (u->in_connection_align_insertion_frequency < 3)
+			return SCI_FAILURE_INVALID_PARAMETER_VALUE;
+
+		if ((u->in_connection_align_insertion_frequency < 3) ||
+		    (u->align_insertion_frequency == 0) ||
+		    (u->notify_enable_spin_up_insertion_frequency == 0))
+			return SCI_FAILURE_INVALID_PARAMETER_VALUE;
+	}
+
+	if ((sci_parms->stp_inactivity_timeout == 0) ||
+	    (sci_parms->ssp_inactivity_timeout == 0) ||
+	    (sci_parms->stp_max_occupancy_timeout == 0) ||
+	    (sci_parms->ssp_max_occupancy_timeout == 0) ||
+	    (sci_parms->no_outbound_task_timeout == 0))
+		return SCI_FAILURE_INVALID_PARAMETER_VALUE;
+
+	memcpy(&ihost->user_parameters, sci_parms, sizeof(*sci_parms));
+
+	return SCI_SUCCESS;
+}
+
+static void sci_oem_defaults(struct isci_host *ihost)
+{
+	/* these defaults are overridden by the platform / firmware */
+	struct sci_user_parameters *user = &ihost->user_parameters;
+	struct sci_oem_params *oem = &ihost->oem_parameters;
+	int i;
+
+	/* Default to APC mode. */
+	oem->controller.mode_type = SCIC_PORT_AUTOMATIC_CONFIGURATION_MODE;
+
+	/* Default to APC mode. */
+	oem->controller.max_concurr_spin_up = 1;
+
+	/* Default to no SSC operation. */
+	oem->controller.do_enable_ssc = false;
+
+	/* Default to short cables on all phys. */
+	oem->controller.cable_selection_mask = 0;
+
+	/* Initialize all of the port parameter information to narrow ports. */
+	for (i = 0; i < SCI_MAX_PORTS; i++)
+		oem->ports[i].phy_mask = 0;
+
+	/* Initialize all of the phy parameter information. */
+	for (i = 0; i < SCI_MAX_PHYS; i++) {
+		/* Default to 3G (i.e. Gen 2). */
+		user->phys[i].max_speed_generation = SCIC_SDS_PARM_GEN2_SPEED;
+
+		/* the frequencies cannot be 0 */
+		user->phys[i].align_insertion_frequency = 0x7f;
+		user->phys[i].in_connection_align_insertion_frequency = 0xff;
+		user->phys[i].notify_enable_spin_up_insertion_frequency = 0x33;
+
+		/* Previous Vitesse based expanders had a arbitration issue that
+		 * is worked around by having the upper 32-bits of SAS address
+		 * with a value greater then the Vitesse company identifier.
+		 * Hence, usage of 0x5FCFFFFF.
+		 */
+		oem->phys[i].sas_address.low = 0x1 + ihost->id;
+		oem->phys[i].sas_address.high = 0x5FCFFFFF;
+	}
+
+	user->stp_inactivity_timeout = 5;
+	user->ssp_inactivity_timeout = 5;
+	user->stp_max_occupancy_timeout = 5;
+	user->ssp_max_occupancy_timeout = 20;
+	user->no_outbound_task_timeout = 2;
+}
+
 static struct isci_host *isci_host_alloc(struct pci_dev *pdev, int id)
 {
-	struct isci_host *isci_host;
+	struct isci_orom *orom = to_pci_info(pdev)->orom;
+	struct sci_user_parameters sci_user_params;
+	u8 oem_version = ISCI_ROM_VER_1_0;
+	struct isci_host *ihost;
 	struct Scsi_Host *shost;
-	int err;
+	int err, i;
 
-	isci_host = devm_kzalloc(&pdev->dev, sizeof(*isci_host), GFP_KERNEL);
-	if (!isci_host)
+	ihost = devm_kzalloc(&pdev->dev, sizeof(*ihost), GFP_KERNEL);
+	if (!ihost)
 		return NULL;
 
-	isci_host->pdev = pdev;
-	isci_host->id = id;
+	ihost->pdev = pdev;
+	ihost->id = id;
+	spin_lock_init(&ihost->scic_lock);
+	init_waitqueue_head(&ihost->eventq);
+	ihost->sas_ha.dev = &ihost->pdev->dev;
+	ihost->sas_ha.lldd_ha = ihost;
+	tasklet_init(&ihost->completion_tasklet,
+		     isci_host_completion_routine, (unsigned long)ihost);
+
+	/* validate module parameters */
+	/* TODO: kill struct sci_user_parameters and reference directly */
+	sci_oem_defaults(ihost);
+	isci_user_parameters_get(&sci_user_params);
+	if (sci_user_parameters_set(ihost, &sci_user_params)) {
+		dev_warn(&pdev->dev,
+			 "%s: sci_user_parameters_set failed\n", __func__);
+		return NULL;
+	}
+
+	/* sanity check platform (or 'firmware') oem parameters */
+	if (orom) {
+		if (id < 0 || id >= SCI_MAX_CONTROLLERS || id > orom->hdr.num_elements) {
+			dev_warn(&pdev->dev, "parsing firmware oem parameters failed\n");
+			return NULL;
+		}
+		ihost->oem_parameters = orom->ctrl[id];
+		oem_version = orom->hdr.version;
+	}
+
+	/* validate oem parameters (platform, firmware, or built-in defaults) */
+	if (sci_oem_parameters_validate(&ihost->oem_parameters, oem_version)) {
+		dev_warn(&pdev->dev, "oem parameter validation failed\n");
+		return NULL;
+	}
+
+	for (i = 0; i < SCI_MAX_PORTS; i++) {
+		struct isci_port *iport = &ihost->ports[i];
+
+		INIT_LIST_HEAD(&iport->remote_dev_list);
+		iport->isci_host = ihost;
+	}
+
+	for (i = 0; i < SCI_MAX_PHYS; i++)
+		isci_phy_init(&ihost->phys[i], ihost, i);
+
+	for (i = 0; i < SCI_MAX_REMOTE_DEVICES; i++) {
+		struct isci_remote_device *idev = &ihost->devices[i];
+
+		INIT_LIST_HEAD(&idev->node);
+	}
 
 	shost = scsi_host_alloc(&isci_sht, sizeof(void *));
 	if (!shost)
 		return NULL;
-	isci_host->shost = shost;
 
 	dev_info(&pdev->dev, "%sSCU controller %d: phy 3-0 cables: "
 		 "{%s, %s, %s, %s}\n",
-		 (is_cable_select_overridden() ? "* " : ""), isci_host->id,
-		 lookup_cable_names(decode_cable_selection(isci_host, 3)),
-		 lookup_cable_names(decode_cable_selection(isci_host, 2)),
-		 lookup_cable_names(decode_cable_selection(isci_host, 1)),
-		 lookup_cable_names(decode_cable_selection(isci_host, 0)));
+		 (is_cable_select_overridden() ? "* " : ""), ihost->id,
+		 lookup_cable_names(decode_cable_selection(ihost, 3)),
+		 lookup_cable_names(decode_cable_selection(ihost, 2)),
+		 lookup_cable_names(decode_cable_selection(ihost, 1)),
+		 lookup_cable_names(decode_cable_selection(ihost, 0)));
 
-	err = isci_host_init(isci_host);
+	err = isci_host_init(ihost);
 	if (err)
 		goto err_shost;
 
-	SHOST_TO_SAS_HA(shost) = &isci_host->sas_ha;
-	isci_host->sas_ha.core.shost = shost;
+	SHOST_TO_SAS_HA(shost) = &ihost->sas_ha;
+	ihost->sas_ha.core.shost = shost;
 	shost->transportt = isci_transport_template;
 
 	shost->max_id = ~0;
@@ -439,11 +599,11 @@ static struct isci_host *isci_host_alloc(struct pci_dev *pdev, int id)
 	if (err)
 		goto err_shost;
 
-	err = isci_register_sas_ha(isci_host);
+	err = isci_register_sas_ha(ihost);
 	if (err)
 		goto err_shost_remove;
 
-	return isci_host;
+	return ihost;
 
  err_shost_remove:
 	scsi_remove_host(shost);
@@ -476,7 +636,7 @@ static int __devinit isci_pci_probe(struct pci_dev *pdev, const struct pci_devic
 	if (!orom)
 		orom = isci_request_oprom(pdev);
 
-	for (i = 0; orom && i < ARRAY_SIZE(orom->ctrl); i++) {
+	for (i = 0; orom && i < num_controllers(pdev); i++) {
 		if (sci_oem_parameters_validate(&orom->ctrl[i],
 						orom->hdr.version)) {
 			dev_warn(&pdev->dev,
@@ -525,11 +685,11 @@ static int __devinit isci_pci_probe(struct pci_dev *pdev, const struct pci_devic
 		pci_info->hosts[i] = h;
 
 		/* turn on DIF support */
-		scsi_host_set_prot(h->shost,
+		scsi_host_set_prot(to_shost(h),
 				   SHOST_DIF_TYPE1_PROTECTION |
 				   SHOST_DIF_TYPE2_PROTECTION |
 				   SHOST_DIF_TYPE3_PROTECTION);
-		scsi_host_set_guard(h->shost, SHOST_DIX_GUARD_CRC);
+		scsi_host_set_guard(to_shost(h), SHOST_DIX_GUARD_CRC);
 	}
 
 	err = isci_setup_interrupts(pdev);
@@ -537,7 +697,7 @@ static int __devinit isci_pci_probe(struct pci_dev *pdev, const struct pci_devic
 		goto err_host_alloc;
 
 	for_each_isci_host(i, isci_host, pdev)
-		scsi_scan_host(isci_host->shost);
+		scsi_scan_host(to_shost(isci_host));
 
 	return 0;
 
diff --git a/drivers/scsi/isci/phy.c b/drivers/scsi/isci/phy.c
index fab3586840b..18f43d4c30b 100644
--- a/drivers/scsi/isci/phy.c
+++ b/drivers/scsi/isci/phy.c
@@ -580,7 +580,7 @@ static void sci_phy_start_sas_link_training(struct isci_phy *iphy)
 
 	sci_change_state(&iphy->sm, SCI_PHY_SUB_AWAIT_SAS_SPEED_EN);
 
-	iphy->protocol = SCIC_SDS_PHY_PROTOCOL_SAS;
+	iphy->protocol = SAS_PROTOCOL_SSP;
 }
 
 static void sci_phy_start_sata_link_training(struct isci_phy *iphy)
@@ -591,7 +591,7 @@ static void sci_phy_start_sata_link_training(struct isci_phy *iphy)
 	 */
 	sci_change_state(&iphy->sm, SCI_PHY_SUB_AWAIT_SATA_POWER);
 
-	iphy->protocol = SCIC_SDS_PHY_PROTOCOL_SATA;
+	iphy->protocol = SAS_PROTOCOL_SATA;
 }
 
 /**
@@ -668,6 +668,19 @@ static const char *phy_event_name(u32 event_code)
 		phy_to_host(iphy)->id, iphy->phy_index, \
 		phy_state_name(state), phy_event_name(code), code)
 
+
+void scu_link_layer_set_txcomsas_timeout(struct isci_phy *iphy, u32 timeout)
+{
+	u32 val;
+
+	/* Extend timeout */
+	val = readl(&iphy->link_layer_registers->transmit_comsas_signal);
+	val &= ~SCU_SAS_LLTXCOMSAS_GEN_VAL(NEGTIME, SCU_SAS_LINK_LAYER_TXCOMSAS_NEGTIME_MASK);
+	val |= SCU_SAS_LLTXCOMSAS_GEN_VAL(NEGTIME, timeout);
+
+	writel(val, &iphy->link_layer_registers->transmit_comsas_signal);
+}
+
 enum sci_status sci_phy_event_handler(struct isci_phy *iphy, u32 event_code)
 {
 	enum sci_phy_states state = iphy->sm.current_state_id;
@@ -683,6 +696,13 @@ enum sci_status sci_phy_event_handler(struct isci_phy *iphy, u32 event_code)
 			sci_phy_start_sata_link_training(iphy);
 			iphy->is_in_link_training = true;
 			break;
+		case SCU_EVENT_RECEIVED_IDENTIFY_TIMEOUT:
+		       /* Extend timeout value */
+		       scu_link_layer_set_txcomsas_timeout(iphy, SCU_SAS_LINK_LAYER_TXCOMSAS_NEGTIME_EXTENDED);
+
+		       /* Start the oob/sn state machine over again */
+		       sci_change_state(&iphy->sm, SCI_PHY_STARTING);
+		       break;
 		default:
 			phy_event_dbg(iphy, state, event_code);
 			return SCI_FAILURE;
@@ -717,9 +737,19 @@ enum sci_status sci_phy_event_handler(struct isci_phy *iphy, u32 event_code)
 			sci_phy_start_sata_link_training(iphy);
 			break;
 		case SCU_EVENT_LINK_FAILURE:
+			/* Change the timeout value to default */
+			scu_link_layer_set_txcomsas_timeout(iphy, SCU_SAS_LINK_LAYER_TXCOMSAS_NEGTIME_DEFAULT);
+
 			/* Link failure change state back to the starting state */
 			sci_change_state(&iphy->sm, SCI_PHY_STARTING);
 			break;
+		case SCU_EVENT_RECEIVED_IDENTIFY_TIMEOUT:
+		       /* Extend the timeout value */
+		       scu_link_layer_set_txcomsas_timeout(iphy, SCU_SAS_LINK_LAYER_TXCOMSAS_NEGTIME_EXTENDED);
+
+		       /* Start the oob/sn state machine over again */
+		       sci_change_state(&iphy->sm, SCI_PHY_STARTING);
+		       break;
 		default:
 			phy_event_warn(iphy, state, event_code);
 			return SCI_FAILURE;
@@ -740,7 +770,14 @@ enum sci_status sci_phy_event_handler(struct isci_phy *iphy, u32 event_code)
 			sci_phy_start_sata_link_training(iphy);
 			break;
 		case SCU_EVENT_RECEIVED_IDENTIFY_TIMEOUT:
+			/* Extend the timeout value */
+			scu_link_layer_set_txcomsas_timeout(iphy, SCU_SAS_LINK_LAYER_TXCOMSAS_NEGTIME_EXTENDED);
+
+			/* Start the oob/sn state machine over again */
+			sci_change_state(&iphy->sm, SCI_PHY_STARTING);
+			break;
 		case SCU_EVENT_LINK_FAILURE:
+			scu_link_layer_set_txcomsas_timeout(iphy, SCU_SAS_LINK_LAYER_TXCOMSAS_NEGTIME_DEFAULT);
 		case SCU_EVENT_HARD_RESET_RECEIVED:
 			/* Start the oob/sn state machine over again */
 			sci_change_state(&iphy->sm, SCI_PHY_STARTING);
@@ -753,6 +790,9 @@ enum sci_status sci_phy_event_handler(struct isci_phy *iphy, u32 event_code)
 	case SCI_PHY_SUB_AWAIT_SAS_POWER:
 		switch (scu_get_event_code(event_code)) {
 		case SCU_EVENT_LINK_FAILURE:
+			/* Change the timeout value to default */
+			scu_link_layer_set_txcomsas_timeout(iphy, SCU_SAS_LINK_LAYER_TXCOMSAS_NEGTIME_DEFAULT);
+
 			/* Link failure change state back to the starting state */
 			sci_change_state(&iphy->sm, SCI_PHY_STARTING);
 			break;
@@ -764,6 +804,9 @@ enum sci_status sci_phy_event_handler(struct isci_phy *iphy, u32 event_code)
 	case SCI_PHY_SUB_AWAIT_SATA_POWER:
 		switch (scu_get_event_code(event_code)) {
 		case SCU_EVENT_LINK_FAILURE:
+			/* Change the timeout value to default */
+			scu_link_layer_set_txcomsas_timeout(iphy, SCU_SAS_LINK_LAYER_TXCOMSAS_NEGTIME_DEFAULT);
+
 			/* Link failure change state back to the starting state */
 			sci_change_state(&iphy->sm, SCI_PHY_STARTING);
 			break;
@@ -788,6 +831,9 @@ enum sci_status sci_phy_event_handler(struct isci_phy *iphy, u32 event_code)
 	case SCI_PHY_SUB_AWAIT_SATA_PHY_EN:
 		switch (scu_get_event_code(event_code)) {
 		case SCU_EVENT_LINK_FAILURE:
+			/* Change the timeout value to default */
+			scu_link_layer_set_txcomsas_timeout(iphy, SCU_SAS_LINK_LAYER_TXCOMSAS_NEGTIME_DEFAULT);
+
 			/* Link failure change state back to the starting state */
 			sci_change_state(&iphy->sm, SCI_PHY_STARTING);
 			break;
@@ -797,7 +843,7 @@ enum sci_status sci_phy_event_handler(struct isci_phy *iphy, u32 event_code)
 			 */
 			break;
 		case SCU_EVENT_SATA_PHY_DETECTED:
-			iphy->protocol = SCIC_SDS_PHY_PROTOCOL_SATA;
+			iphy->protocol = SAS_PROTOCOL_SATA;
 
 			/* We have received the SATA PHY notification change state */
 			sci_change_state(&iphy->sm, SCI_PHY_SUB_AWAIT_SATA_SPEED_EN);
@@ -836,6 +882,9 @@ enum sci_status sci_phy_event_handler(struct isci_phy *iphy, u32 event_code)
 						       SCI_PHY_SUB_AWAIT_SIG_FIS_UF);
 			break;
 		case SCU_EVENT_LINK_FAILURE:
+			/* Change the timeout value to default */
+			scu_link_layer_set_txcomsas_timeout(iphy, SCU_SAS_LINK_LAYER_TXCOMSAS_NEGTIME_DEFAULT);
+
 			/* Link failure change state back to the starting state */
 			sci_change_state(&iphy->sm, SCI_PHY_STARTING);
 			break;
@@ -859,6 +908,9 @@ enum sci_status sci_phy_event_handler(struct isci_phy *iphy, u32 event_code)
 			break;
 
 		case SCU_EVENT_LINK_FAILURE:
+			/* Change the timeout value to default */
+			scu_link_layer_set_txcomsas_timeout(iphy, SCU_SAS_LINK_LAYER_TXCOMSAS_NEGTIME_DEFAULT);
+
 			/* Link failure change state back to the starting state */
 			sci_change_state(&iphy->sm, SCI_PHY_STARTING);
 			break;
@@ -871,16 +923,26 @@ enum sci_status sci_phy_event_handler(struct isci_phy *iphy, u32 event_code)
 	case SCI_PHY_READY:
 		switch (scu_get_event_code(event_code)) {
 		case SCU_EVENT_LINK_FAILURE:
+			/* Set default timeout */
+			scu_link_layer_set_txcomsas_timeout(iphy, SCU_SAS_LINK_LAYER_TXCOMSAS_NEGTIME_DEFAULT);
+
 			/* Link failure change state back to the starting state */
 			sci_change_state(&iphy->sm, SCI_PHY_STARTING);
 			break;
 		case SCU_EVENT_BROADCAST_CHANGE:
+		case SCU_EVENT_BROADCAST_SES:
+		case SCU_EVENT_BROADCAST_RESERVED0:
+		case SCU_EVENT_BROADCAST_RESERVED1:
+		case SCU_EVENT_BROADCAST_EXPANDER:
+		case SCU_EVENT_BROADCAST_AEN:
 			/* Broadcast change received. Notify the port. */
 			if (phy_get_non_dummy_port(iphy) != NULL)
 				sci_port_broadcast_change_received(iphy->owning_port, iphy);
 			else
 				iphy->bcn_received_while_port_unassigned = true;
 			break;
+		case SCU_EVENT_BROADCAST_RESERVED3:
+		case SCU_EVENT_BROADCAST_RESERVED4:
 		default:
 			phy_event_warn(iphy, state, event_code);
 			return SCI_FAILURE_INVALID_STATE;
@@ -1215,7 +1277,7 @@ static void sci_phy_starting_state_enter(struct sci_base_state_machine *sm)
 	scu_link_layer_start_oob(iphy);
 
 	/* We don't know what kind of phy we are going to be just yet */
-	iphy->protocol = SCIC_SDS_PHY_PROTOCOL_UNKNOWN;
+	iphy->protocol = SAS_PROTOCOL_NONE;
 	iphy->bcn_received_while_port_unassigned = false;
 
 	if (iphy->sm.previous_state_id == SCI_PHY_READY)
@@ -1250,7 +1312,7 @@ static void sci_phy_resetting_state_enter(struct sci_base_state_machine *sm)
 	 */
 	sci_port_deactivate_phy(iphy->owning_port, iphy, false);
 
-	if (iphy->protocol == SCIC_SDS_PHY_PROTOCOL_SAS) {
+	if (iphy->protocol == SAS_PROTOCOL_SSP) {
 		scu_link_layer_tx_hard_reset(iphy);
 	} else {
 		/* The SCU does not need to have a discrete reset state so
@@ -1316,7 +1378,7 @@ void sci_phy_construct(struct isci_phy *iphy,
 	iphy->owning_port = iport;
 	iphy->phy_index = phy_index;
 	iphy->bcn_received_while_port_unassigned = false;
-	iphy->protocol = SCIC_SDS_PHY_PROTOCOL_UNKNOWN;
+	iphy->protocol = SAS_PROTOCOL_NONE;
 	iphy->link_layer_registers = NULL;
 	iphy->max_negotiated_speed = SAS_LINK_RATE_UNKNOWN;
 
@@ -1380,12 +1442,14 @@ int isci_phy_control(struct asd_sas_phy *sas_phy,
 	switch (func) {
 	case PHY_FUNC_DISABLE:
 		spin_lock_irqsave(&ihost->scic_lock, flags);
+		scu_link_layer_start_oob(iphy);
 		sci_phy_stop(iphy);
 		spin_unlock_irqrestore(&ihost->scic_lock, flags);
 		break;
 
 	case PHY_FUNC_LINK_RESET:
 		spin_lock_irqsave(&ihost->scic_lock, flags);
+		scu_link_layer_start_oob(iphy);
 		sci_phy_stop(iphy);
 		sci_phy_start(iphy);
 		spin_unlock_irqrestore(&ihost->scic_lock, flags);
diff --git a/drivers/scsi/isci/phy.h b/drivers/scsi/isci/phy.h
index 0e45833ba06..45fecfa36a9 100644
--- a/drivers/scsi/isci/phy.h
+++ b/drivers/scsi/isci/phy.h
@@ -76,13 +76,6 @@
  */
 #define SCIC_SDS_SATA_LINK_TRAINING_TIMEOUT  250
 
-enum sci_phy_protocol {
-	SCIC_SDS_PHY_PROTOCOL_UNKNOWN,
-	SCIC_SDS_PHY_PROTOCOL_SAS,
-	SCIC_SDS_PHY_PROTOCOL_SATA,
-	SCIC_SDS_MAX_PHY_PROTOCOLS
-};
-
 /**
  * isci_phy - hba local phy infrastructure
  * @sm:
@@ -95,7 +88,7 @@ struct isci_phy {
 	struct sci_base_state_machine sm;
 	struct isci_port *owning_port;
 	enum sas_linkrate max_negotiated_speed;
-	enum sci_phy_protocol protocol;
+	enum sas_protocol protocol;
 	u8 phy_index;
 	bool bcn_received_while_port_unassigned;
 	bool is_in_link_training;
diff --git a/drivers/scsi/isci/port.c b/drivers/scsi/isci/port.c
index 5fada73b71f..2fb85bf7544 100644
--- a/drivers/scsi/isci/port.c
+++ b/drivers/scsi/isci/port.c
@@ -184,7 +184,7 @@ static void isci_port_link_up(struct isci_host *isci_host,
 
 	sci_port_get_properties(iport, &properties);
 
-	if (iphy->protocol == SCIC_SDS_PHY_PROTOCOL_SATA) {
+	if (iphy->protocol == SAS_PROTOCOL_SATA) {
 		u64 attached_sas_address;
 
 		iphy->sas_phy.oob_mode = SATA_OOB_MODE;
@@ -204,7 +204,7 @@ static void isci_port_link_up(struct isci_host *isci_host,
 
 		memcpy(&iphy->sas_phy.attached_sas_addr,
 		       &attached_sas_address, sizeof(attached_sas_address));
-	} else if (iphy->protocol == SCIC_SDS_PHY_PROTOCOL_SAS) {
+	} else if (iphy->protocol == SAS_PROTOCOL_SSP) {
 		iphy->sas_phy.oob_mode = SAS_OOB_MODE;
 		iphy->sas_phy.frame_rcvd_size = sizeof(struct sas_identify_frame);
 
@@ -251,10 +251,10 @@ static void isci_port_link_down(struct isci_host *isci_host,
 		if (isci_phy->sas_phy.port &&
 		    isci_phy->sas_phy.port->num_phys == 1) {
 			/* change the state for all devices on this port.  The
-			 * next task sent to this device will be returned as
-			 * SAS_TASK_UNDELIVERED, and the scsi mid layer will
-			 * remove the target
-			 */
+			* next task sent to this device will be returned as
+			* SAS_TASK_UNDELIVERED, and the scsi mid layer will
+			* remove the target
+			*/
 			list_for_each_entry(isci_device,
 					    &isci_port->remote_dev_list,
 					    node) {
@@ -517,7 +517,7 @@ void sci_port_get_attached_sas_address(struct isci_port *iport, struct sci_sas_a
 	 */
 	iphy = sci_port_get_a_connected_phy(iport);
 	if (iphy) {
-		if (iphy->protocol != SCIC_SDS_PHY_PROTOCOL_SATA) {
+		if (iphy->protocol != SAS_PROTOCOL_SATA) {
 			sci_phy_get_attached_sas_address(iphy, sas);
 		} else {
 			sci_phy_get_sas_address(iphy, sas);
@@ -624,7 +624,7 @@ static void sci_port_activate_phy(struct isci_port *iport,
 {
 	struct isci_host *ihost = iport->owning_controller;
 
-	if (iphy->protocol != SCIC_SDS_PHY_PROTOCOL_SATA && (flags & PF_RESUME))
+	if (iphy->protocol != SAS_PROTOCOL_SATA && (flags & PF_RESUME))
 		sci_phy_resume(iphy);
 
 	iport->active_phy_mask |= 1 << iphy->phy_index;
@@ -751,12 +751,10 @@ static bool sci_port_is_wide(struct isci_port *iport)
  * wide ports and direct attached phys.  Since there are no wide ported SATA
  * devices this could become an invalid port configuration.
  */
-bool sci_port_link_detected(
-	struct isci_port *iport,
-	struct isci_phy *iphy)
+bool sci_port_link_detected(struct isci_port *iport, struct isci_phy *iphy)
 {
 	if ((iport->logical_port_index != SCIC_SDS_DUMMY_PORT) &&
-	    (iphy->protocol == SCIC_SDS_PHY_PROTOCOL_SATA)) {
+	    (iphy->protocol == SAS_PROTOCOL_SATA)) {
 		if (sci_port_is_wide(iport)) {
 			sci_port_invalid_link_up(iport, iphy);
 			return false;
@@ -1201,6 +1199,8 @@ enum sci_status sci_port_add_phy(struct isci_port *iport,
 	enum sci_status status;
 	enum sci_port_states state;
 
+	sci_port_bcn_enable(iport);
+
 	state = iport->sm.current_state_id;
 	switch (state) {
 	case SCI_PORT_STOPPED: {
@@ -1548,6 +1548,29 @@ static void sci_port_failed_state_enter(struct sci_base_state_machine *sm)
 	isci_port_hard_reset_complete(iport, SCI_FAILURE_TIMEOUT);
 }
 
+void sci_port_set_hang_detection_timeout(struct isci_port *iport, u32 timeout)
+{
+	int phy_index;
+	u32 phy_mask = iport->active_phy_mask;
+
+	if (timeout)
+		++iport->hang_detect_users;
+	else if (iport->hang_detect_users > 1)
+		--iport->hang_detect_users;
+	else
+		iport->hang_detect_users = 0;
+
+	if (timeout || (iport->hang_detect_users == 0)) {
+		for (phy_index = 0; phy_index < SCI_MAX_PHYS; phy_index++) {
+			if ((phy_mask >> phy_index) & 1) {
+				writel(timeout,
+				       &iport->phy_table[phy_index]
+					  ->link_layer_registers
+					  ->link_layer_hang_detection_timeout);
+			}
+		}
+	}
+}
 /* --------------------------------------------------------------------------- */
 
 static const struct sci_base_state sci_port_state_table[] = {
@@ -1596,6 +1619,7 @@ void sci_port_construct(struct isci_port *iport, u8 index,
 
 	iport->started_request_count = 0;
 	iport->assigned_device_count = 0;
+	iport->hang_detect_users = 0;
 
 	iport->reserved_rni = SCU_DUMMY_INDEX;
 	iport->reserved_tag = SCI_CONTROLLER_INVALID_IO_TAG;
@@ -1608,13 +1632,6 @@ void sci_port_construct(struct isci_port *iport, u8 index,
 		iport->phy_table[index] = NULL;
 }
 
-void isci_port_init(struct isci_port *iport, struct isci_host *ihost, int index)
-{
-	INIT_LIST_HEAD(&iport->remote_dev_list);
-	INIT_LIST_HEAD(&iport->domain_dev_list);
-	iport->isci_host = ihost;
-}
-
 void sci_port_broadcast_change_received(struct isci_port *iport, struct isci_phy *iphy)
 {
 	struct isci_host *ihost = iport->owning_controller;
@@ -1671,17 +1688,6 @@ int isci_port_perform_hard_reset(struct isci_host *ihost, struct isci_port *ipor
 			__func__, iport, status);
 
 	}
-
-	/* If the hard reset for the port has failed, consider this
-	 * the same as link failures on all phys in the port.
-	 */
-	if (ret != TMF_RESP_FUNC_COMPLETE) {
-
-		dev_err(&ihost->pdev->dev,
-			"%s: iport = %p; hard reset failed "
-			"(0x%x) - driving explicit link fail for all phys\n",
-			__func__, iport, iport->hard_reset_status);
-	}
 	return ret;
 }
 
@@ -1740,7 +1746,7 @@ void isci_port_formed(struct asd_sas_phy *phy)
 	struct isci_host *ihost = phy->ha->lldd_ha;
 	struct isci_phy *iphy = to_iphy(phy);
 	struct asd_sas_port *port = phy->port;
-	struct isci_port *iport;
+	struct isci_port *iport = NULL;
 	unsigned long flags;
 	int i;
 
diff --git a/drivers/scsi/isci/port.h b/drivers/scsi/isci/port.h
index 6b56240c205..861e8f72811 100644
--- a/drivers/scsi/isci/port.h
+++ b/drivers/scsi/isci/port.h
@@ -97,7 +97,6 @@ enum isci_status {
 struct isci_port {
 	struct isci_host *isci_host;
 	struct list_head remote_dev_list;
-	struct list_head domain_dev_list;
 	#define IPORT_RESET_PENDING 0
 	unsigned long state;
 	enum sci_status hard_reset_status;
@@ -112,6 +111,7 @@ struct isci_port {
 	u16 reserved_tag;
 	u32 started_request_count;
 	u32 assigned_device_count;
+	u32 hang_detect_users;
 	u32 not_ready_reason;
 	struct isci_phy *phy_table[SCI_MAX_PHYS];
 	struct isci_host *owning_controller;
@@ -270,14 +270,13 @@ void sci_port_get_attached_sas_address(
 	struct isci_port *iport,
 	struct sci_sas_address *sas_address);
 
+void sci_port_set_hang_detection_timeout(
+	struct isci_port *isci_port,
+	u32 timeout);
+
 void isci_port_formed(struct asd_sas_phy *);
 void isci_port_deformed(struct asd_sas_phy *);
 
-void isci_port_init(
-	struct isci_port *port,
-	struct isci_host *host,
-	int index);
-
 int isci_port_perform_hard_reset(struct isci_host *ihost, struct isci_port *iport,
 				 struct isci_phy *iphy);
 int isci_ata_check_ready(struct domain_device *dev);
diff --git a/drivers/scsi/isci/port_config.c b/drivers/scsi/isci/port_config.c
index 6d1e9544cbe..cd962da4a57 100644
--- a/drivers/scsi/isci/port_config.c
+++ b/drivers/scsi/isci/port_config.c
@@ -57,7 +57,7 @@
 
 #define SCIC_SDS_MPC_RECONFIGURATION_TIMEOUT    (10)
 #define SCIC_SDS_APC_RECONFIGURATION_TIMEOUT    (10)
-#define SCIC_SDS_APC_WAIT_LINK_UP_NOTIFICATION  (250)
+#define SCIC_SDS_APC_WAIT_LINK_UP_NOTIFICATION  (1000)
 
 enum SCIC_SDS_APC_ACTIVITY {
 	SCIC_SDS_APC_SKIP_PHY,
@@ -472,13 +472,9 @@ sci_apc_agent_validate_phy_configuration(struct isci_host *ihost,
  * down event or a link up event where we can not yet tell to which a phy
  * belongs.
  */
-static void sci_apc_agent_start_timer(
-	struct sci_port_configuration_agent *port_agent,
-	u32 timeout)
+static void sci_apc_agent_start_timer(struct sci_port_configuration_agent *port_agent,
+				      u32 timeout)
 {
-	if (port_agent->timer_pending)
-		sci_del_timer(&port_agent->timer);
-
 	port_agent->timer_pending = true;
 	sci_mod_timer(&port_agent->timer, timeout);
 }
@@ -697,6 +693,9 @@ static void apc_agent_timeout(unsigned long data)
 						   &ihost->phys[index], false);
 	}
 
+	if (is_controller_start_complete(ihost))
+		sci_controller_transition_to_ready(ihost, SCI_SUCCESS);
+
 done:
 	spin_unlock_irqrestore(&ihost->scic_lock, flags);
 }
@@ -732,6 +731,11 @@ void sci_port_configuration_agent_construct(
 	}
 }
 
+bool is_port_config_apc(struct isci_host *ihost)
+{
+	return ihost->port_agent.link_up_handler == sci_apc_agent_link_up;
+}
+
 enum sci_status sci_port_configuration_agent_initialize(
 	struct isci_host *ihost,
 	struct sci_port_configuration_agent *port_agent)
diff --git a/drivers/scsi/isci/probe_roms.c b/drivers/scsi/isci/probe_roms.c
index 9b8117b9d75..4d95654c3fd 100644
--- a/drivers/scsi/isci/probe_roms.c
+++ b/drivers/scsi/isci/probe_roms.c
@@ -112,18 +112,6 @@ struct isci_orom *isci_request_oprom(struct pci_dev *pdev)
 	return rom;
 }
 
-enum sci_status isci_parse_oem_parameters(struct sci_oem_params *oem,
-					  struct isci_orom *orom, int scu_index)
-{
-	/* check for valid inputs */
-	if (scu_index < 0 || scu_index >= SCI_MAX_CONTROLLERS ||
-	    scu_index > orom->hdr.num_elements || !oem)
-		return -EINVAL;
-
-	*oem = orom->ctrl[scu_index];
-	return 0;
-}
-
 struct isci_orom *isci_request_firmware(struct pci_dev *pdev, const struct firmware *fw)
 {
 	struct isci_orom *orom = NULL, *data;
diff --git a/drivers/scsi/isci/probe_roms.h b/drivers/scsi/isci/probe_roms.h
index bb0e9d4d97c..e08b578241f 100644
--- a/drivers/scsi/isci/probe_roms.h
+++ b/drivers/scsi/isci/probe_roms.h
@@ -156,8 +156,6 @@ int sci_oem_parameters_validate(struct sci_oem_params *oem, u8 version);
 
 struct isci_orom;
 struct isci_orom *isci_request_oprom(struct pci_dev *pdev);
-enum sci_status isci_parse_oem_parameters(struct sci_oem_params *oem,
-					  struct isci_orom *orom, int scu_index);
 struct isci_orom *isci_request_firmware(struct pci_dev *pdev, const struct firmware *fw);
 struct isci_orom *isci_get_efi_var(struct pci_dev *pdev);
 
diff --git a/drivers/scsi/isci/registers.h b/drivers/scsi/isci/registers.h
index 7eb0ccd45fe..97f3ceb8d72 100644
--- a/drivers/scsi/isci/registers.h
+++ b/drivers/scsi/isci/registers.h
@@ -1239,6 +1239,14 @@ struct scu_transport_layer_registers {
 #define SCU_SAS_LLCTL_GEN_BIT(name) \
 	SCU_GEN_BIT(SCU_SAS_LINK_LAYER_CONTROL_ ## name)
 
+#define SCU_SAS_LINK_LAYER_TXCOMSAS_NEGTIME_DEFAULT                     (0xF0)
+#define SCU_SAS_LINK_LAYER_TXCOMSAS_NEGTIME_EXTENDED                    (0x1FF)
+#define SCU_SAS_LINK_LAYER_TXCOMSAS_NEGTIME_SHIFT                       (0)
+#define SCU_SAS_LINK_LAYER_TXCOMSAS_NEGTIME_MASK                        (0x3FF)
+
+#define SCU_SAS_LLTXCOMSAS_GEN_VAL(name, value) \
+	SCU_GEN_VALUE(SCU_SAS_LINK_LAYER_TXCOMSAS_ ## name, value)
+
 
 /* #define SCU_FRXHECR_DCNT_OFFSET      0x00B0 */
 #define SCU_PSZGCR_OFFSET           0x00E4
diff --git a/drivers/scsi/isci/remote_device.c b/drivers/scsi/isci/remote_device.c
index 8f501b0a81d..c3aa6c5457b 100644
--- a/drivers/scsi/isci/remote_device.c
+++ b/drivers/scsi/isci/remote_device.c
@@ -72,46 +72,11 @@ const char *dev_state_name(enum sci_remote_device_states state)
 }
 #undef C
 
-/**
- * isci_remote_device_not_ready() - This function is called by the ihost when
- *    the remote device is not ready. We mark the isci device as ready (not
- *    "ready_for_io") and signal the waiting proccess.
- * @isci_host: This parameter specifies the isci host object.
- * @isci_device: This parameter specifies the remote device
- *
- * sci_lock is held on entrance to this function.
- */
-static void isci_remote_device_not_ready(struct isci_host *ihost,
-				  struct isci_remote_device *idev, u32 reason)
+enum sci_status sci_remote_device_suspend(struct isci_remote_device *idev,
+					  enum sci_remote_node_suspension_reasons reason)
 {
-	struct isci_request *ireq;
-
-	dev_dbg(&ihost->pdev->dev,
-		"%s: isci_device = %p\n", __func__, idev);
-
-	switch (reason) {
-	case SCIC_REMOTE_DEVICE_NOT_READY_STOP_REQUESTED:
-		set_bit(IDEV_GONE, &idev->flags);
-		break;
-	case SCIC_REMOTE_DEVICE_NOT_READY_SATA_SDB_ERROR_FIS_RECEIVED:
-		set_bit(IDEV_IO_NCQERROR, &idev->flags);
-
-		/* Kill all outstanding requests for the device. */
-		list_for_each_entry(ireq, &idev->reqs_in_process, dev_node) {
-
-			dev_dbg(&ihost->pdev->dev,
-				"%s: isci_device = %p request = %p\n",
-				__func__, idev, ireq);
-
-			sci_controller_terminate_request(ihost,
-							  idev,
-							  ireq);
-		}
-		/* Fall through into the default case... */
-	default:
-		clear_bit(IDEV_IO_READY, &idev->flags);
-		break;
-	}
+	return sci_remote_node_context_suspend(&idev->rnc, reason,
+					       SCI_SOFTWARE_SUSPEND_EXPECTED_EVENT);
 }
 
 /**
@@ -133,18 +98,29 @@ static void isci_remote_device_ready(struct isci_host *ihost, struct isci_remote
 		wake_up(&ihost->eventq);
 }
 
-/* called once the remote node context is ready to be freed.
- * The remote device can now report that its stop operation is complete. none
- */
-static void rnc_destruct_done(void *_dev)
+static enum sci_status sci_remote_device_terminate_req(
+	struct isci_host *ihost,
+	struct isci_remote_device *idev,
+	int check_abort,
+	struct isci_request *ireq)
 {
-	struct isci_remote_device *idev = _dev;
+	if (!test_bit(IREQ_ACTIVE, &ireq->flags) ||
+	    (ireq->target_device != idev) ||
+	    (check_abort && !test_bit(IREQ_PENDING_ABORT, &ireq->flags)))
+		return SCI_SUCCESS;
 
-	BUG_ON(idev->started_request_count != 0);
-	sci_change_state(&idev->sm, SCI_DEV_STOPPED);
+	dev_dbg(&ihost->pdev->dev,
+		"%s: idev=%p; flags=%lx; req=%p; req target=%p\n",
+		__func__, idev, idev->flags, ireq, ireq->target_device);
+
+	set_bit(IREQ_ABORT_PATH_ACTIVE, &ireq->flags);
+
+	return sci_controller_terminate_request(ihost, idev, ireq);
 }
 
-static enum sci_status sci_remote_device_terminate_requests(struct isci_remote_device *idev)
+static enum sci_status sci_remote_device_terminate_reqs_checkabort(
+	struct isci_remote_device *idev,
+	int chk)
 {
 	struct isci_host *ihost = idev->owning_port->owning_controller;
 	enum sci_status status  = SCI_SUCCESS;
@@ -154,18 +130,210 @@ static enum sci_status sci_remote_device_terminate_requests(struct isci_remote_d
 		struct isci_request *ireq = ihost->reqs[i];
 		enum sci_status s;
 
-		if (!test_bit(IREQ_ACTIVE, &ireq->flags) ||
-		    ireq->target_device != idev)
-			continue;
-
-		s = sci_controller_terminate_request(ihost, idev, ireq);
+		s = sci_remote_device_terminate_req(ihost, idev, chk, ireq);
 		if (s != SCI_SUCCESS)
 			status = s;
 	}
+	return status;
+}
+
+static bool isci_compare_suspendcount(
+	struct isci_remote_device *idev,
+	u32 localcount)
+{
+	smp_rmb();
+
+	/* Check for a change in the suspend count, or the RNC
+	 * being destroyed.
+	 */
+	return (localcount != idev->rnc.suspend_count)
+	    || sci_remote_node_context_is_being_destroyed(&idev->rnc);
+}
+
+static bool isci_check_reqterm(
+	struct isci_host *ihost,
+	struct isci_remote_device *idev,
+	struct isci_request *ireq,
+	u32 localcount)
+{
+	unsigned long flags;
+	bool res;
 
+	spin_lock_irqsave(&ihost->scic_lock, flags);
+	res = isci_compare_suspendcount(idev, localcount)
+		&& !test_bit(IREQ_ABORT_PATH_ACTIVE, &ireq->flags);
+	spin_unlock_irqrestore(&ihost->scic_lock, flags);
+
+	return res;
+}
+
+static bool isci_check_devempty(
+	struct isci_host *ihost,
+	struct isci_remote_device *idev,
+	u32 localcount)
+{
+	unsigned long flags;
+	bool res;
+
+	spin_lock_irqsave(&ihost->scic_lock, flags);
+	res = isci_compare_suspendcount(idev, localcount)
+		&& idev->started_request_count == 0;
+	spin_unlock_irqrestore(&ihost->scic_lock, flags);
+
+	return res;
+}
+
+enum sci_status isci_remote_device_terminate_requests(
+	struct isci_host *ihost,
+	struct isci_remote_device *idev,
+	struct isci_request *ireq)
+{
+	enum sci_status status = SCI_SUCCESS;
+	unsigned long flags;
+	u32 rnc_suspend_count;
+
+	spin_lock_irqsave(&ihost->scic_lock, flags);
+
+	if (isci_get_device(idev) == NULL) {
+		dev_dbg(&ihost->pdev->dev, "%s: failed isci_get_device(idev=%p)\n",
+			__func__, idev);
+		spin_unlock_irqrestore(&ihost->scic_lock, flags);
+		status = SCI_FAILURE;
+	} else {
+		/* If already suspended, don't wait for another suspension. */
+		smp_rmb();
+		rnc_suspend_count
+			= sci_remote_node_context_is_suspended(&idev->rnc)
+				? 0 : idev->rnc.suspend_count;
+
+		dev_dbg(&ihost->pdev->dev,
+			"%s: idev=%p, ireq=%p; started_request_count=%d, "
+				"rnc_suspend_count=%d, rnc.suspend_count=%d"
+				"about to wait\n",
+			__func__, idev, ireq, idev->started_request_count,
+			rnc_suspend_count, idev->rnc.suspend_count);
+
+		#define MAX_SUSPEND_MSECS 10000
+		if (ireq) {
+			/* Terminate a specific TC. */
+			set_bit(IREQ_NO_AUTO_FREE_TAG, &ireq->flags);
+			sci_remote_device_terminate_req(ihost, idev, 0, ireq);
+			spin_unlock_irqrestore(&ihost->scic_lock, flags);
+			if (!wait_event_timeout(ihost->eventq,
+						isci_check_reqterm(ihost, idev, ireq,
+								   rnc_suspend_count),
+						msecs_to_jiffies(MAX_SUSPEND_MSECS))) {
+
+				dev_warn(&ihost->pdev->dev, "%s host%d timeout single\n",
+					 __func__, ihost->id);
+				dev_dbg(&ihost->pdev->dev,
+					 "%s: ******* Timeout waiting for "
+					 "suspend; idev=%p, current state %s; "
+					 "started_request_count=%d, flags=%lx\n\t"
+					 "rnc_suspend_count=%d, rnc.suspend_count=%d "
+					 "RNC: current state %s, current "
+					 "suspend_type %x dest state %d;\n"
+					 "ireq=%p, ireq->flags = %lx\n",
+					 __func__, idev,
+					 dev_state_name(idev->sm.current_state_id),
+					 idev->started_request_count, idev->flags,
+					 rnc_suspend_count, idev->rnc.suspend_count,
+					 rnc_state_name(idev->rnc.sm.current_state_id),
+					 idev->rnc.suspend_type,
+					 idev->rnc.destination_state,
+					 ireq, ireq->flags);
+			}
+			spin_lock_irqsave(&ihost->scic_lock, flags);
+			clear_bit(IREQ_NO_AUTO_FREE_TAG, &ireq->flags);
+			if (!test_bit(IREQ_ABORT_PATH_ACTIVE, &ireq->flags))
+				isci_free_tag(ihost, ireq->io_tag);
+			spin_unlock_irqrestore(&ihost->scic_lock, flags);
+		} else {
+			/* Terminate all TCs. */
+			sci_remote_device_terminate_requests(idev);
+			spin_unlock_irqrestore(&ihost->scic_lock, flags);
+			if (!wait_event_timeout(ihost->eventq,
+						isci_check_devempty(ihost, idev,
+								    rnc_suspend_count),
+						msecs_to_jiffies(MAX_SUSPEND_MSECS))) {
+
+				dev_warn(&ihost->pdev->dev, "%s host%d timeout all\n",
+					 __func__, ihost->id);
+				dev_dbg(&ihost->pdev->dev,
+					"%s: ******* Timeout waiting for "
+					"suspend; idev=%p, current state %s; "
+					"started_request_count=%d, flags=%lx\n\t"
+					"rnc_suspend_count=%d, "
+					"RNC: current state %s, "
+					"rnc.suspend_count=%d, current "
+					"suspend_type %x dest state %d\n",
+					__func__, idev,
+					dev_state_name(idev->sm.current_state_id),
+					idev->started_request_count, idev->flags,
+					rnc_suspend_count,
+					rnc_state_name(idev->rnc.sm.current_state_id),
+					idev->rnc.suspend_count,
+					idev->rnc.suspend_type,
+					idev->rnc.destination_state);
+			}
+		}
+		dev_dbg(&ihost->pdev->dev, "%s: idev=%p, wait done\n",
+			__func__, idev);
+		isci_put_device(idev);
+	}
 	return status;
 }
 
+/**
+* isci_remote_device_not_ready() - This function is called by the ihost when
+*    the remote device is not ready. We mark the isci device as ready (not
+*    "ready_for_io") and signal the waiting proccess.
+* @isci_host: This parameter specifies the isci host object.
+* @isci_device: This parameter specifies the remote device
+*
+* sci_lock is held on entrance to this function.
+*/
+static void isci_remote_device_not_ready(struct isci_host *ihost,
+					 struct isci_remote_device *idev,
+					 u32 reason)
+{
+	dev_dbg(&ihost->pdev->dev,
+		"%s: isci_device = %p; reason = %d\n", __func__, idev, reason);
+
+	switch (reason) {
+	case SCIC_REMOTE_DEVICE_NOT_READY_SATA_SDB_ERROR_FIS_RECEIVED:
+		set_bit(IDEV_IO_NCQERROR, &idev->flags);
+
+		/* Suspend the remote device so the I/O can be terminated. */
+		sci_remote_device_suspend(idev, SCI_SW_SUSPEND_NORMAL);
+
+		/* Kill all outstanding requests for the device. */
+		sci_remote_device_terminate_requests(idev);
+
+		/* Fall through into the default case... */
+	default:
+		clear_bit(IDEV_IO_READY, &idev->flags);
+		break;
+	}
+}
+
+/* called once the remote node context is ready to be freed.
+ * The remote device can now report that its stop operation is complete. none
+ */
+static void rnc_destruct_done(void *_dev)
+{
+	struct isci_remote_device *idev = _dev;
+
+	BUG_ON(idev->started_request_count != 0);
+	sci_change_state(&idev->sm, SCI_DEV_STOPPED);
+}
+
+enum sci_status sci_remote_device_terminate_requests(
+	struct isci_remote_device *idev)
+{
+	return sci_remote_device_terminate_reqs_checkabort(idev, 0);
+}
+
 enum sci_status sci_remote_device_stop(struct isci_remote_device *idev,
 					u32 timeout)
 {
@@ -201,13 +369,16 @@ enum sci_status sci_remote_device_stop(struct isci_remote_device *idev,
 	case SCI_SMP_DEV_IDLE:
 	case SCI_SMP_DEV_CMD:
 		sci_change_state(sm, SCI_DEV_STOPPING);
-		if (idev->started_request_count == 0) {
+		if (idev->started_request_count == 0)
 			sci_remote_node_context_destruct(&idev->rnc,
-							      rnc_destruct_done, idev);
-			return SCI_SUCCESS;
-		} else
-			return sci_remote_device_terminate_requests(idev);
-		break;
+							 rnc_destruct_done,
+							 idev);
+		else {
+			sci_remote_device_suspend(
+				idev, SCI_SW_SUSPEND_LINKHANG_DETECT);
+			sci_remote_device_terminate_requests(idev);
+		}
+		return SCI_SUCCESS;
 	case SCI_DEV_STOPPING:
 		/* All requests should have been terminated, but if there is an
 		 * attempt to stop a device already in the stopping state, then
@@ -265,22 +436,6 @@ enum sci_status sci_remote_device_reset_complete(struct isci_remote_device *idev
 	return SCI_SUCCESS;
 }
 
-enum sci_status sci_remote_device_suspend(struct isci_remote_device *idev,
-					       u32 suspend_type)
-{
-	struct sci_base_state_machine *sm = &idev->sm;
-	enum sci_remote_device_states state = sm->current_state_id;
-
-	if (state != SCI_STP_DEV_CMD) {
-		dev_warn(scirdev_to_dev(idev), "%s: in wrong state: %s\n",
-			 __func__, dev_state_name(state));
-		return SCI_FAILURE_INVALID_STATE;
-	}
-
-	return sci_remote_node_context_suspend(&idev->rnc,
-						    suspend_type, NULL, NULL);
-}
-
 enum sci_status sci_remote_device_frame_handler(struct isci_remote_device *idev,
 						     u32 frame_index)
 {
@@ -412,9 +567,9 @@ static void atapi_remote_device_resume_done(void *_dev)
 enum sci_status sci_remote_device_event_handler(struct isci_remote_device *idev,
 						     u32 event_code)
 {
+	enum sci_status status;
 	struct sci_base_state_machine *sm = &idev->sm;
 	enum sci_remote_device_states state = sm->current_state_id;
-	enum sci_status status;
 
 	switch (scu_get_event_type(event_code)) {
 	case SCU_EVENT_TYPE_RNC_OPS_MISC:
@@ -427,9 +582,7 @@ enum sci_status sci_remote_device_event_handler(struct isci_remote_device *idev,
 			status = SCI_SUCCESS;
 
 			/* Suspend the associated RNC */
-			sci_remote_node_context_suspend(&idev->rnc,
-							      SCI_SOFTWARE_SUSPENSION,
-							      NULL, NULL);
+			sci_remote_device_suspend(idev, SCI_SW_SUSPEND_NORMAL);
 
 			dev_dbg(scirdev_to_dev(idev),
 				"%s: device: %p event code: %x: %s\n",
@@ -455,6 +608,10 @@ enum sci_status sci_remote_device_event_handler(struct isci_remote_device *idev,
 	if (status != SCI_SUCCESS)
 		return status;
 
+	/* Decode device-specific states that may require an RNC resume during
+	 * normal operation.  When the abort path is active, these resumes are
+	 * managed when the abort path exits.
+	 */
 	if (state == SCI_STP_DEV_ATAPI_ERROR) {
 		/* For ATAPI error state resume the RNC right away. */
 		if (scu_get_event_type(event_code) == SCU_EVENT_TYPE_RNC_SUSPEND_TX ||
@@ -743,10 +900,6 @@ enum sci_status sci_remote_device_start_task(struct isci_host *ihost,
 		if (status != SCI_SUCCESS)
 			return status;
 
-		status = sci_remote_node_context_start_task(&idev->rnc, ireq);
-		if (status != SCI_SUCCESS)
-			goto out;
-
 		status = sci_request_start(ireq);
 		if (status != SCI_SUCCESS)
 			goto out;
@@ -765,11 +918,11 @@ enum sci_status sci_remote_device_start_task(struct isci_host *ihost,
 		 * the correct action when the remote node context is suspended
 		 * and later resumed.
 		 */
-		sci_remote_node_context_suspend(&idev->rnc,
-				SCI_SOFTWARE_SUSPENSION, NULL, NULL);
-		sci_remote_node_context_resume(&idev->rnc,
-				sci_remote_device_continue_request,
-						    idev);
+		sci_remote_device_suspend(idev,
+					  SCI_SW_SUSPEND_LINKHANG_DETECT);
+
+		status = sci_remote_node_context_start_task(&idev->rnc, ireq,
+				sci_remote_device_continue_request, idev);
 
 	out:
 		sci_remote_device_start_request(idev, ireq, status);
@@ -783,7 +936,9 @@ enum sci_status sci_remote_device_start_task(struct isci_host *ihost,
 		if (status != SCI_SUCCESS)
 			return status;
 
-		status = sci_remote_node_context_start_task(&idev->rnc, ireq);
+		/* Resume the RNC as needed: */
+		status = sci_remote_node_context_start_task(&idev->rnc, ireq,
+							    NULL, NULL);
 		if (status != SCI_SUCCESS)
 			break;
 
@@ -892,7 +1047,7 @@ static void isci_remote_device_deconstruct(struct isci_host *ihost, struct isci_
 	 * here should go through isci_remote_device_nuke_requests.
 	 * If we hit this condition, we will need a way to complete
 	 * io requests in process */
-	BUG_ON(!list_empty(&idev->reqs_in_process));
+	BUG_ON(idev->started_request_count > 0);
 
 	sci_remote_device_destruct(idev);
 	list_del_init(&idev->node);
@@ -954,14 +1109,21 @@ static void sci_remote_device_ready_state_exit(struct sci_base_state_machine *sm
 static void sci_remote_device_resetting_state_enter(struct sci_base_state_machine *sm)
 {
 	struct isci_remote_device *idev = container_of(sm, typeof(*idev), sm);
+	struct isci_host *ihost = idev->owning_port->owning_controller;
 
-	sci_remote_node_context_suspend(
-		&idev->rnc, SCI_SOFTWARE_SUSPENSION, NULL, NULL);
+	dev_dbg(&ihost->pdev->dev,
+		"%s: isci_device = %p\n", __func__, idev);
+
+	sci_remote_device_suspend(idev, SCI_SW_SUSPEND_LINKHANG_DETECT);
 }
 
 static void sci_remote_device_resetting_state_exit(struct sci_base_state_machine *sm)
 {
 	struct isci_remote_device *idev = container_of(sm, typeof(*idev), sm);
+	struct isci_host *ihost = idev->owning_port->owning_controller;
+
+	dev_dbg(&ihost->pdev->dev,
+		"%s: isci_device = %p\n", __func__, idev);
 
 	sci_remote_node_context_resume(&idev->rnc, NULL, NULL);
 }
@@ -1113,33 +1275,20 @@ static enum sci_status sci_remote_device_da_construct(struct isci_port *iport,
 {
 	enum sci_status status;
 	struct sci_port_properties properties;
-	struct domain_device *dev = idev->domain_dev;
 
 	sci_remote_device_construct(iport, idev);
 
-	/*
-	 * This information is request to determine how many remote node context
-	 * entries will be needed to store the remote node.
-	 */
-	idev->is_direct_attached = true;
-
 	sci_port_get_properties(iport, &properties);
 	/* Get accurate port width from port's phy mask for a DA device. */
 	idev->device_port_width = hweight32(properties.phy_mask);
 
 	status = sci_controller_allocate_remote_node_context(iport->owning_controller,
-								  idev,
-								  &idev->rnc.remote_node_index);
+							     idev,
+							     &idev->rnc.remote_node_index);
 
 	if (status != SCI_SUCCESS)
 		return status;
 
-	if (dev->dev_type == SAS_END_DEV || dev->dev_type == SATA_DEV ||
-	    (dev->tproto & SAS_PROTOCOL_STP) || dev_is_expander(dev))
-		/* pass */;
-	else
-		return SCI_FAILURE_UNSUPPORTED_PROTOCOL;
-
 	idev->connection_rate = sci_port_get_max_allowed_speed(iport);
 
 	return SCI_SUCCESS;
@@ -1171,19 +1320,13 @@ static enum sci_status sci_remote_device_ea_construct(struct isci_port *iport,
 	if (status != SCI_SUCCESS)
 		return status;
 
-	if (dev->dev_type == SAS_END_DEV || dev->dev_type == SATA_DEV ||
-	    (dev->tproto & SAS_PROTOCOL_STP) || dev_is_expander(dev))
-		/* pass */;
-	else
-		return SCI_FAILURE_UNSUPPORTED_PROTOCOL;
-
-	/*
-	 * For SAS-2 the physical link rate is actually a logical link
+	/* For SAS-2 the physical link rate is actually a logical link
 	 * rate that incorporates multiplexing.  The SCU doesn't
 	 * incorporate multiplexing and for the purposes of the
 	 * connection the logical link rate is that same as the
 	 * physical.  Furthermore, the SAS-2 and SAS-1.1 fields overlay
-	 * one another, so this code works for both situations. */
+	 * one another, so this code works for both situations.
+	 */
 	idev->connection_rate = min_t(u16, sci_port_get_max_allowed_speed(iport),
 					 dev->linkrate);
 
@@ -1193,6 +1336,105 @@ static enum sci_status sci_remote_device_ea_construct(struct isci_port *iport,
 	return SCI_SUCCESS;
 }
 
+enum sci_status sci_remote_device_resume(
+	struct isci_remote_device *idev,
+	scics_sds_remote_node_context_callback cb_fn,
+	void *cb_p)
+{
+	enum sci_status status;
+
+	status = sci_remote_node_context_resume(&idev->rnc, cb_fn, cb_p);
+	if (status != SCI_SUCCESS)
+		dev_dbg(scirdev_to_dev(idev), "%s: failed to resume: %d\n",
+			__func__, status);
+	return status;
+}
+
+static void isci_remote_device_resume_from_abort_complete(void *cbparam)
+{
+	struct isci_remote_device *idev = cbparam;
+	struct isci_host *ihost = idev->owning_port->owning_controller;
+	scics_sds_remote_node_context_callback abort_resume_cb =
+		idev->abort_resume_cb;
+
+	dev_dbg(scirdev_to_dev(idev), "%s: passing-along resume: %p\n",
+		__func__, abort_resume_cb);
+
+	if (abort_resume_cb != NULL) {
+		idev->abort_resume_cb = NULL;
+		abort_resume_cb(idev->abort_resume_cbparam);
+	}
+	clear_bit(IDEV_ABORT_PATH_RESUME_PENDING, &idev->flags);
+	wake_up(&ihost->eventq);
+}
+
+static bool isci_remote_device_test_resume_done(
+	struct isci_host *ihost,
+	struct isci_remote_device *idev)
+{
+	unsigned long flags;
+	bool done;
+
+	spin_lock_irqsave(&ihost->scic_lock, flags);
+	done = !test_bit(IDEV_ABORT_PATH_RESUME_PENDING, &idev->flags)
+		|| test_bit(IDEV_STOP_PENDING, &idev->flags)
+		|| sci_remote_node_context_is_being_destroyed(&idev->rnc);
+	spin_unlock_irqrestore(&ihost->scic_lock, flags);
+
+	return done;
+}
+
+void isci_remote_device_wait_for_resume_from_abort(
+	struct isci_host *ihost,
+	struct isci_remote_device *idev)
+{
+	dev_dbg(&ihost->pdev->dev, "%s: starting resume wait: %p\n",
+		 __func__, idev);
+
+	#define MAX_RESUME_MSECS 10000
+	if (!wait_event_timeout(ihost->eventq,
+				isci_remote_device_test_resume_done(ihost, idev),
+				msecs_to_jiffies(MAX_RESUME_MSECS))) {
+
+		dev_warn(&ihost->pdev->dev, "%s: #### Timeout waiting for "
+			 "resume: %p\n", __func__, idev);
+	}
+	clear_bit(IDEV_ABORT_PATH_RESUME_PENDING, &idev->flags);
+
+	dev_dbg(&ihost->pdev->dev, "%s: resume wait done: %p\n",
+		 __func__, idev);
+}
+
+enum sci_status isci_remote_device_resume_from_abort(
+	struct isci_host *ihost,
+	struct isci_remote_device *idev)
+{
+	unsigned long flags;
+	enum sci_status status = SCI_SUCCESS;
+	int destroyed;
+
+	spin_lock_irqsave(&ihost->scic_lock, flags);
+	/* Preserve any current resume callbacks, for instance from other
+	 * resumptions.
+	 */
+	idev->abort_resume_cb = idev->rnc.user_callback;
+	idev->abort_resume_cbparam = idev->rnc.user_cookie;
+	set_bit(IDEV_ABORT_PATH_RESUME_PENDING, &idev->flags);
+	clear_bit(IDEV_ABORT_PATH_ACTIVE, &idev->flags);
+	destroyed = sci_remote_node_context_is_being_destroyed(&idev->rnc);
+	if (!destroyed)
+		status = sci_remote_device_resume(
+			idev, isci_remote_device_resume_from_abort_complete,
+			idev);
+	spin_unlock_irqrestore(&ihost->scic_lock, flags);
+	if (!destroyed && (status == SCI_SUCCESS))
+		isci_remote_device_wait_for_resume_from_abort(ihost, idev);
+	else
+		clear_bit(IDEV_ABORT_PATH_RESUME_PENDING, &idev->flags);
+
+	return status;
+}
+
 /**
  * sci_remote_device_start() - This method will start the supplied remote
  *    device.  This method enables normal IO requests to flow through to the
@@ -1207,7 +1449,7 @@ static enum sci_status sci_remote_device_ea_construct(struct isci_port *iport,
  * the device when there have been no phys added to it.
  */
 static enum sci_status sci_remote_device_start(struct isci_remote_device *idev,
-						u32 timeout)
+					       u32 timeout)
 {
 	struct sci_base_state_machine *sm = &idev->sm;
 	enum sci_remote_device_states state = sm->current_state_id;
@@ -1219,9 +1461,8 @@ static enum sci_status sci_remote_device_start(struct isci_remote_device *idev,
 		return SCI_FAILURE_INVALID_STATE;
 	}
 
-	status = sci_remote_node_context_resume(&idev->rnc,
-						     remote_device_resume_done,
-						     idev);
+	status = sci_remote_device_resume(idev, remote_device_resume_done,
+					  idev);
 	if (status != SCI_SUCCESS)
 		return status;
 
@@ -1259,20 +1500,6 @@ static enum sci_status isci_remote_device_construct(struct isci_port *iport,
 	return status;
 }
 
-void isci_remote_device_nuke_requests(struct isci_host *ihost, struct isci_remote_device *idev)
-{
-	DECLARE_COMPLETION_ONSTACK(aborted_task_completion);
-
-	dev_dbg(&ihost->pdev->dev,
-		"%s: idev = %p\n", __func__, idev);
-
-	/* Cleanup all requests pending for this device. */
-	isci_terminate_pending_requests(ihost, idev);
-
-	dev_dbg(&ihost->pdev->dev,
-		"%s: idev = %p, done\n", __func__, idev);
-}
-
 /**
  * This function builds the isci_remote_device when a libsas dev_found message
  *    is received.
@@ -1297,10 +1524,6 @@ isci_remote_device_alloc(struct isci_host *ihost, struct isci_port *iport)
 		dev_warn(&ihost->pdev->dev, "%s: failed\n", __func__);
 		return NULL;
 	}
-
-	if (WARN_ONCE(!list_empty(&idev->reqs_in_process), "found requests in process\n"))
-		return NULL;
-
 	if (WARN_ONCE(!list_empty(&idev->node), "found non-idle remote device\n"))
 		return NULL;
 
@@ -1342,14 +1565,8 @@ enum sci_status isci_remote_device_stop(struct isci_host *ihost, struct isci_rem
 	spin_lock_irqsave(&ihost->scic_lock, flags);
 	idev->domain_dev->lldd_dev = NULL; /* disable new lookups */
 	set_bit(IDEV_GONE, &idev->flags);
-	spin_unlock_irqrestore(&ihost->scic_lock, flags);
-
-	/* Kill all outstanding requests. */
-	isci_remote_device_nuke_requests(ihost, idev);
 
 	set_bit(IDEV_STOP_PENDING, &idev->flags);
-
-	spin_lock_irqsave(&ihost->scic_lock, flags);
 	status = sci_remote_device_stop(idev, 50);
 	spin_unlock_irqrestore(&ihost->scic_lock, flags);
 
@@ -1359,6 +1576,9 @@ enum sci_status isci_remote_device_stop(struct isci_host *ihost, struct isci_rem
 	else
 		wait_for_device_stop(ihost, idev);
 
+	dev_dbg(&ihost->pdev->dev,
+		"%s: isci_device = %p, waiting done.\n", __func__, idev);
+
 	return status;
 }
 
@@ -1434,3 +1654,73 @@ int isci_remote_device_found(struct domain_device *dev)
 
 	return status == SCI_SUCCESS ? 0 : -ENODEV;
 }
+
+enum sci_status isci_remote_device_suspend_terminate(
+	struct isci_host *ihost,
+	struct isci_remote_device *idev,
+	struct isci_request *ireq)
+{
+	unsigned long flags;
+	enum sci_status status;
+
+	/* Put the device into suspension. */
+	spin_lock_irqsave(&ihost->scic_lock, flags);
+	set_bit(IDEV_ABORT_PATH_ACTIVE, &idev->flags);
+	sci_remote_device_suspend(idev, SCI_SW_SUSPEND_LINKHANG_DETECT);
+	spin_unlock_irqrestore(&ihost->scic_lock, flags);
+
+	/* Terminate and wait for the completions. */
+	status = isci_remote_device_terminate_requests(ihost, idev, ireq);
+	if (status != SCI_SUCCESS)
+		dev_dbg(&ihost->pdev->dev,
+			"%s: isci_remote_device_terminate_requests(%p) "
+				"returned %d!\n",
+			__func__, idev, status);
+
+	/* NOTE: RNC resumption is left to the caller! */
+	return status;
+}
+
+int isci_remote_device_is_safe_to_abort(
+	struct isci_remote_device *idev)
+{
+	return sci_remote_node_context_is_safe_to_abort(&idev->rnc);
+}
+
+enum sci_status sci_remote_device_abort_requests_pending_abort(
+	struct isci_remote_device *idev)
+{
+	return sci_remote_device_terminate_reqs_checkabort(idev, 1);
+}
+
+enum sci_status isci_remote_device_reset_complete(
+	struct isci_host *ihost,
+	struct isci_remote_device *idev)
+{
+	unsigned long flags;
+	enum sci_status status;
+
+	spin_lock_irqsave(&ihost->scic_lock, flags);
+	status = sci_remote_device_reset_complete(idev);
+	spin_unlock_irqrestore(&ihost->scic_lock, flags);
+
+	return status;
+}
+
+void isci_dev_set_hang_detection_timeout(
+	struct isci_remote_device *idev,
+	u32 timeout)
+{
+	if (dev_is_sata(idev->domain_dev)) {
+		if (timeout) {
+			if (test_and_set_bit(IDEV_RNC_LLHANG_ENABLED,
+					     &idev->flags))
+				return;  /* Already enabled. */
+		} else if (!test_and_clear_bit(IDEV_RNC_LLHANG_ENABLED,
+					       &idev->flags))
+			return;  /* Not enabled. */
+
+		sci_port_set_hang_detection_timeout(idev->owning_port,
+						    timeout);
+	}
+}
diff --git a/drivers/scsi/isci/remote_device.h b/drivers/scsi/isci/remote_device.h
index 58637ee08f5..7674caae1d8 100644
--- a/drivers/scsi/isci/remote_device.h
+++ b/drivers/scsi/isci/remote_device.h
@@ -85,27 +85,38 @@ struct isci_remote_device {
 	#define IDEV_GONE 3
 	#define IDEV_IO_READY 4
 	#define IDEV_IO_NCQERROR 5
+	#define IDEV_RNC_LLHANG_ENABLED 6
+	#define IDEV_ABORT_PATH_ACTIVE 7
+	#define IDEV_ABORT_PATH_RESUME_PENDING 8
 	unsigned long flags;
 	struct kref kref;
 	struct isci_port *isci_port;
 	struct domain_device *domain_dev;
 	struct list_head node;
-	struct list_head reqs_in_process;
 	struct sci_base_state_machine sm;
 	u32 device_port_width;
 	enum sas_linkrate connection_rate;
-	bool is_direct_attached;
 	struct isci_port *owning_port;
 	struct sci_remote_node_context rnc;
 	/* XXX unify with device reference counting and delete */
 	u32 started_request_count;
 	struct isci_request *working_request;
 	u32 not_ready_reason;
+	scics_sds_remote_node_context_callback abort_resume_cb;
+	void *abort_resume_cbparam;
 };
 
 #define ISCI_REMOTE_DEVICE_START_TIMEOUT 5000
 
 /* device reference routines must be called under sci_lock */
+static inline struct isci_remote_device *isci_get_device(
+	struct isci_remote_device *idev)
+{
+	if (idev)
+		kref_get(&idev->kref);
+	return idev;
+}
+
 static inline struct isci_remote_device *isci_lookup_device(struct domain_device *dev)
 {
 	struct isci_remote_device *idev = dev->lldd_dev;
@@ -302,6 +313,8 @@ static inline void sci_remote_device_decrement_request_count(struct isci_remote_
 		idev->started_request_count--;
 }
 
+void isci_dev_set_hang_detection_timeout(struct isci_remote_device *idev, u32 timeout);
+
 enum sci_status sci_remote_device_frame_handler(
 	struct isci_remote_device *idev,
 	u32 frame_index);
@@ -325,12 +338,50 @@ enum sci_status sci_remote_device_complete_io(
 	struct isci_remote_device *idev,
 	struct isci_request *ireq);
 
-enum sci_status sci_remote_device_suspend(
-	struct isci_remote_device *idev,
-	u32 suspend_type);
-
 void sci_remote_device_post_request(
 	struct isci_remote_device *idev,
 	u32 request);
 
+enum sci_status sci_remote_device_terminate_requests(
+	struct isci_remote_device *idev);
+
+int isci_remote_device_is_safe_to_abort(
+	struct isci_remote_device *idev);
+
+enum sci_status
+sci_remote_device_abort_requests_pending_abort(
+	struct isci_remote_device *idev);
+
+enum sci_status isci_remote_device_suspend(
+	struct isci_host *ihost,
+	struct isci_remote_device *idev);
+
+enum sci_status sci_remote_device_resume(
+	struct isci_remote_device *idev,
+	scics_sds_remote_node_context_callback cb_fn,
+	void *cb_p);
+
+enum sci_status isci_remote_device_resume_from_abort(
+	struct isci_host *ihost,
+	struct isci_remote_device *idev);
+
+enum sci_status isci_remote_device_reset(
+	struct isci_host *ihost,
+	struct isci_remote_device *idev);
+
+enum sci_status isci_remote_device_reset_complete(
+	struct isci_host *ihost,
+	struct isci_remote_device *idev);
+
+enum sci_status isci_remote_device_suspend_terminate(
+	struct isci_host *ihost,
+	struct isci_remote_device *idev,
+	struct isci_request *ireq);
+
+enum sci_status isci_remote_device_terminate_requests(
+	struct isci_host *ihost,
+	struct isci_remote_device *idev,
+	struct isci_request *ireq);
+enum sci_status sci_remote_device_suspend(struct isci_remote_device *idev,
+					  enum sci_remote_node_suspension_reasons reason);
 #endif /* !defined(_ISCI_REMOTE_DEVICE_H_) */
diff --git a/drivers/scsi/isci/remote_node_context.c b/drivers/scsi/isci/remote_node_context.c
index 3a9463481f3..1910100638a 100644
--- a/drivers/scsi/isci/remote_node_context.c
+++ b/drivers/scsi/isci/remote_node_context.c
@@ -52,7 +52,7 @@
  * (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 <scsi/sas_ata.h>
 #include "host.h"
 #include "isci.h"
 #include "remote_device.h"
@@ -90,6 +90,15 @@ bool sci_remote_node_context_is_ready(
 	return false;
 }
 
+bool sci_remote_node_context_is_suspended(struct sci_remote_node_context *sci_rnc)
+{
+	u32 current_state = sci_rnc->sm.current_state_id;
+
+	if (current_state == SCI_RNC_TX_RX_SUSPENDED)
+		return true;
+	return false;
+}
+
 static union scu_remote_node_context *sci_rnc_by_id(struct isci_host *ihost, u16 id)
 {
 	if (id < ihost->remote_node_entries &&
@@ -131,7 +140,7 @@ static void sci_remote_node_context_construct_buffer(struct sci_remote_node_cont
 
 	rnc->ssp.arbitration_wait_time = 0;
 
-	if (dev->dev_type == SATA_DEV || (dev->tproto & SAS_PROTOCOL_STP)) {
+	if (dev_is_sata(dev)) {
 		rnc->ssp.connection_occupancy_timeout =
 			ihost->user_parameters.stp_max_occupancy_timeout;
 		rnc->ssp.connection_inactivity_timeout =
@@ -151,7 +160,6 @@ static void sci_remote_node_context_construct_buffer(struct sci_remote_node_cont
 	rnc->ssp.oaf_source_zone_group = 0;
 	rnc->ssp.oaf_more_compatibility_features = 0;
 }
-
 /**
  *
  * @sci_rnc:
@@ -165,23 +173,30 @@ static void sci_remote_node_context_construct_buffer(struct sci_remote_node_cont
 static void sci_remote_node_context_setup_to_resume(
 	struct sci_remote_node_context *sci_rnc,
 	scics_sds_remote_node_context_callback callback,
-	void *callback_parameter)
+	void *callback_parameter,
+	enum sci_remote_node_context_destination_state dest_param)
 {
-	if (sci_rnc->destination_state != SCIC_SDS_REMOTE_NODE_DESTINATION_STATE_FINAL) {
-		sci_rnc->destination_state = SCIC_SDS_REMOTE_NODE_DESTINATION_STATE_READY;
-		sci_rnc->user_callback     = callback;
-		sci_rnc->user_cookie       = callback_parameter;
+	if (sci_rnc->destination_state != RNC_DEST_FINAL) {
+		sci_rnc->destination_state = dest_param;
+		if (callback != NULL) {
+			sci_rnc->user_callback = callback;
+			sci_rnc->user_cookie   = callback_parameter;
+		}
 	}
 }
 
-static void sci_remote_node_context_setup_to_destory(
+static void sci_remote_node_context_setup_to_destroy(
 	struct sci_remote_node_context *sci_rnc,
 	scics_sds_remote_node_context_callback callback,
 	void *callback_parameter)
 {
-	sci_rnc->destination_state = SCIC_SDS_REMOTE_NODE_DESTINATION_STATE_FINAL;
+	struct isci_host *ihost = idev_to_ihost(rnc_to_dev(sci_rnc));
+
+	sci_rnc->destination_state = RNC_DEST_FINAL;
 	sci_rnc->user_callback     = callback;
 	sci_rnc->user_cookie       = callback_parameter;
+
+	wake_up(&ihost->eventq);
 }
 
 /**
@@ -203,9 +218,19 @@ static void sci_remote_node_context_notify_user(
 
 static void sci_remote_node_context_continue_state_transitions(struct sci_remote_node_context *rnc)
 {
-	if (rnc->destination_state == SCIC_SDS_REMOTE_NODE_DESTINATION_STATE_READY)
+	switch (rnc->destination_state) {
+	case RNC_DEST_READY:
+	case RNC_DEST_SUSPENDED_RESUME:
+		rnc->destination_state = RNC_DEST_READY;
+		/* Fall through... */
+	case RNC_DEST_FINAL:
 		sci_remote_node_context_resume(rnc, rnc->user_callback,
-						    rnc->user_cookie);
+					       rnc->user_cookie);
+		break;
+	default:
+		rnc->destination_state = RNC_DEST_UNSPECIFIED;
+		break;
+	}
 }
 
 static void sci_remote_node_context_validate_context_buffer(struct sci_remote_node_context *sci_rnc)
@@ -219,13 +244,12 @@ static void sci_remote_node_context_validate_context_buffer(struct sci_remote_no
 
 	rnc_buffer->ssp.is_valid = true;
 
-	if (!idev->is_direct_attached &&
-	    (dev->dev_type == SATA_DEV || (dev->tproto & SAS_PROTOCOL_STP))) {
+	if (dev_is_sata(dev) && dev->parent) {
 		sci_remote_device_post_request(idev, SCU_CONTEXT_COMMAND_POST_RNC_96);
 	} else {
 		sci_remote_device_post_request(idev, SCU_CONTEXT_COMMAND_POST_RNC_32);
 
-		if (idev->is_direct_attached)
+		if (!dev->parent)
 			sci_port_setup_transports(idev->owning_port,
 						  sci_rnc->remote_node_index);
 	}
@@ -248,13 +272,18 @@ static void sci_remote_node_context_invalidate_context_buffer(struct sci_remote_
 static void sci_remote_node_context_initial_state_enter(struct sci_base_state_machine *sm)
 {
 	struct sci_remote_node_context *rnc = container_of(sm, typeof(*rnc), sm);
+	struct isci_remote_device *idev = rnc_to_dev(rnc);
+	struct isci_host *ihost = idev->owning_port->owning_controller;
 
 	/* Check to see if we have gotten back to the initial state because
 	 * someone requested to destroy the remote node context object.
 	 */
 	if (sm->previous_state_id == SCI_RNC_INVALIDATING) {
-		rnc->destination_state = SCIC_SDS_REMOTE_NODE_DESTINATION_STATE_UNSPECIFIED;
+		rnc->destination_state = RNC_DEST_UNSPECIFIED;
 		sci_remote_node_context_notify_user(rnc);
+
+		smp_wmb();
+		wake_up(&ihost->eventq);
 	}
 }
 
@@ -269,6 +298,8 @@ static void sci_remote_node_context_invalidating_state_enter(struct sci_base_sta
 {
 	struct sci_remote_node_context *rnc = container_of(sm, typeof(*rnc), sm);
 
+	/* Terminate all outstanding requests. */
+	sci_remote_device_terminate_requests(rnc_to_dev(rnc));
 	sci_remote_node_context_invalidate_context_buffer(rnc);
 }
 
@@ -287,10 +318,8 @@ static void sci_remote_node_context_resuming_state_enter(struct sci_base_state_m
 	 * resume because of a target reset we also need to update
 	 * the STPTLDARNI register with the RNi of the device
 	 */
-	if ((dev->dev_type == SATA_DEV || (dev->tproto & SAS_PROTOCOL_STP)) &&
-	    idev->is_direct_attached)
-		sci_port_setup_transports(idev->owning_port,
-					       rnc->remote_node_index);
+	if (dev_is_sata(dev) && !dev->parent)
+		sci_port_setup_transports(idev->owning_port, rnc->remote_node_index);
 
 	sci_remote_device_post_request(idev, SCU_CONTEXT_COMMAND_POST_RNC_RESUME);
 }
@@ -298,10 +327,22 @@ static void sci_remote_node_context_resuming_state_enter(struct sci_base_state_m
 static void sci_remote_node_context_ready_state_enter(struct sci_base_state_machine *sm)
 {
 	struct sci_remote_node_context *rnc = container_of(sm, typeof(*rnc), sm);
+	enum sci_remote_node_context_destination_state dest_select;
+	int tell_user = 1;
+
+	dest_select = rnc->destination_state;
+	rnc->destination_state = RNC_DEST_UNSPECIFIED;
 
-	rnc->destination_state = SCIC_SDS_REMOTE_NODE_DESTINATION_STATE_UNSPECIFIED;
+	if ((dest_select == RNC_DEST_SUSPENDED) ||
+	    (dest_select == RNC_DEST_SUSPENDED_RESUME)) {
+		sci_remote_node_context_suspend(
+			rnc, rnc->suspend_reason,
+			SCI_SOFTWARE_SUSPEND_EXPECTED_EVENT);
 
-	if (rnc->user_callback)
+		if (dest_select == RNC_DEST_SUSPENDED_RESUME)
+			tell_user = 0;  /* Wait until ready again. */
+	}
+	if (tell_user)
 		sci_remote_node_context_notify_user(rnc);
 }
 
@@ -315,10 +356,34 @@ static void sci_remote_node_context_tx_suspended_state_enter(struct sci_base_sta
 static void sci_remote_node_context_tx_rx_suspended_state_enter(struct sci_base_state_machine *sm)
 {
 	struct sci_remote_node_context *rnc = container_of(sm, typeof(*rnc), sm);
+	struct isci_remote_device *idev = rnc_to_dev(rnc);
+	struct isci_host *ihost = idev->owning_port->owning_controller;
+	u32 new_count = rnc->suspend_count + 1;
+
+	if (new_count == 0)
+		rnc->suspend_count = 1;
+	else
+		rnc->suspend_count = new_count;
+	smp_wmb();
 
+	/* Terminate outstanding requests pending abort. */
+	sci_remote_device_abort_requests_pending_abort(idev);
+
+	wake_up(&ihost->eventq);
 	sci_remote_node_context_continue_state_transitions(rnc);
 }
 
+static void sci_remote_node_context_await_suspend_state_exit(
+	struct sci_base_state_machine *sm)
+{
+	struct sci_remote_node_context *rnc
+		= container_of(sm, typeof(*rnc), sm);
+	struct isci_remote_device *idev = rnc_to_dev(rnc);
+
+	if (dev_is_sata(idev->domain_dev))
+		isci_dev_set_hang_detection_timeout(idev, 0);
+}
+
 static const struct sci_base_state sci_remote_node_context_state_table[] = {
 	[SCI_RNC_INITIAL] = {
 		.enter_state = sci_remote_node_context_initial_state_enter,
@@ -341,7 +406,9 @@ static const struct sci_base_state sci_remote_node_context_state_table[] = {
 	[SCI_RNC_TX_RX_SUSPENDED] = {
 		.enter_state = sci_remote_node_context_tx_rx_suspended_state_enter,
 	},
-	[SCI_RNC_AWAIT_SUSPENSION] = { },
+	[SCI_RNC_AWAIT_SUSPENSION] = {
+		.exit_state = sci_remote_node_context_await_suspend_state_exit,
+	},
 };
 
 void sci_remote_node_context_construct(struct sci_remote_node_context *rnc,
@@ -350,7 +417,7 @@ void sci_remote_node_context_construct(struct sci_remote_node_context *rnc,
 	memset(rnc, 0, sizeof(struct sci_remote_node_context));
 
 	rnc->remote_node_index = remote_node_index;
-	rnc->destination_state = SCIC_SDS_REMOTE_NODE_DESTINATION_STATE_UNSPECIFIED;
+	rnc->destination_state = RNC_DEST_UNSPECIFIED;
 
 	sci_init_sm(&rnc->sm, sci_remote_node_context_state_table, SCI_RNC_INITIAL);
 }
@@ -359,6 +426,7 @@ enum sci_status sci_remote_node_context_event_handler(struct sci_remote_node_con
 							   u32 event_code)
 {
 	enum scis_sds_remote_node_context_states state;
+	u32 next_state;
 
 	state = sci_rnc->sm.current_state_id;
 	switch (state) {
@@ -373,18 +441,18 @@ enum sci_status sci_remote_node_context_event_handler(struct sci_remote_node_con
 		break;
 	case SCI_RNC_INVALIDATING:
 		if (scu_get_event_code(event_code) == SCU_EVENT_POST_RNC_INVALIDATE_COMPLETE) {
-			if (sci_rnc->destination_state == SCIC_SDS_REMOTE_NODE_DESTINATION_STATE_FINAL)
-				state = SCI_RNC_INITIAL;
+			if (sci_rnc->destination_state == RNC_DEST_FINAL)
+				next_state = SCI_RNC_INITIAL;
 			else
-				state = SCI_RNC_POSTING;
-			sci_change_state(&sci_rnc->sm, state);
+				next_state = SCI_RNC_POSTING;
+			sci_change_state(&sci_rnc->sm, next_state);
 		} else {
 			switch (scu_get_event_type(event_code)) {
 			case SCU_EVENT_TYPE_RNC_SUSPEND_TX:
 			case SCU_EVENT_TYPE_RNC_SUSPEND_TX_RX:
 				/* We really dont care if the hardware is going to suspend
 				 * the device since it's being invalidated anyway */
-				dev_dbg(scirdev_to_dev(rnc_to_dev(sci_rnc)),
+				dev_warn(scirdev_to_dev(rnc_to_dev(sci_rnc)),
 					"%s: SCIC Remote Node Context 0x%p was "
 					"suspeneded by hardware while being "
 					"invalidated.\n", __func__, sci_rnc);
@@ -403,7 +471,7 @@ enum sci_status sci_remote_node_context_event_handler(struct sci_remote_node_con
 			case SCU_EVENT_TYPE_RNC_SUSPEND_TX_RX:
 				/* We really dont care if the hardware is going to suspend
 				 * the device since it's being resumed anyway */
-				dev_dbg(scirdev_to_dev(rnc_to_dev(sci_rnc)),
+				dev_warn(scirdev_to_dev(rnc_to_dev(sci_rnc)),
 					"%s: SCIC Remote Node Context 0x%p was "
 					"suspeneded by hardware while being resumed.\n",
 					__func__, sci_rnc);
@@ -417,11 +485,11 @@ enum sci_status sci_remote_node_context_event_handler(struct sci_remote_node_con
 		switch (scu_get_event_type(event_code)) {
 		case SCU_EVENT_TL_RNC_SUSPEND_TX:
 			sci_change_state(&sci_rnc->sm, SCI_RNC_TX_SUSPENDED);
-			sci_rnc->suspension_code = scu_get_event_specifier(event_code);
+			sci_rnc->suspend_type = scu_get_event_type(event_code);
 			break;
 		case SCU_EVENT_TL_RNC_SUSPEND_TX_RX:
 			sci_change_state(&sci_rnc->sm, SCI_RNC_TX_RX_SUSPENDED);
-			sci_rnc->suspension_code = scu_get_event_specifier(event_code);
+			sci_rnc->suspend_type = scu_get_event_type(event_code);
 			break;
 		default:
 			goto out;
@@ -430,27 +498,29 @@ enum sci_status sci_remote_node_context_event_handler(struct sci_remote_node_con
 	case SCI_RNC_AWAIT_SUSPENSION:
 		switch (scu_get_event_type(event_code)) {
 		case SCU_EVENT_TL_RNC_SUSPEND_TX:
-			sci_change_state(&sci_rnc->sm, SCI_RNC_TX_SUSPENDED);
-			sci_rnc->suspension_code = scu_get_event_specifier(event_code);
+			next_state = SCI_RNC_TX_SUSPENDED;
 			break;
 		case SCU_EVENT_TL_RNC_SUSPEND_TX_RX:
-			sci_change_state(&sci_rnc->sm, SCI_RNC_TX_RX_SUSPENDED);
-			sci_rnc->suspension_code = scu_get_event_specifier(event_code);
+			next_state = SCI_RNC_TX_RX_SUSPENDED;
 			break;
 		default:
 			goto out;
 		}
+		if (sci_rnc->suspend_type == scu_get_event_type(event_code))
+			sci_change_state(&sci_rnc->sm, next_state);
 		break;
 	default:
 		dev_warn(scirdev_to_dev(rnc_to_dev(sci_rnc)),
-			 "%s: invalid state %d\n", __func__, state);
+			 "%s: invalid state: %s\n", __func__,
+			 rnc_state_name(state));
 		return SCI_FAILURE_INVALID_STATE;
 	}
 	return SCI_SUCCESS;
 
  out:
 	dev_warn(scirdev_to_dev(rnc_to_dev(sci_rnc)),
-		 "%s: code: %#x state: %d\n", __func__, event_code, state);
+		 "%s: code: %#x state: %s\n", __func__, event_code,
+		 rnc_state_name(state));
 	return SCI_FAILURE;
 
 }
@@ -464,20 +534,23 @@ enum sci_status sci_remote_node_context_destruct(struct sci_remote_node_context
 	state = sci_rnc->sm.current_state_id;
 	switch (state) {
 	case SCI_RNC_INVALIDATING:
-		sci_remote_node_context_setup_to_destory(sci_rnc, cb_fn, cb_p);
+		sci_remote_node_context_setup_to_destroy(sci_rnc, cb_fn, cb_p);
 		return SCI_SUCCESS;
 	case SCI_RNC_POSTING:
 	case SCI_RNC_RESUMING:
 	case SCI_RNC_READY:
 	case SCI_RNC_TX_SUSPENDED:
 	case SCI_RNC_TX_RX_SUSPENDED:
-	case SCI_RNC_AWAIT_SUSPENSION:
-		sci_remote_node_context_setup_to_destory(sci_rnc, cb_fn, cb_p);
+		sci_remote_node_context_setup_to_destroy(sci_rnc, cb_fn, cb_p);
 		sci_change_state(&sci_rnc->sm, SCI_RNC_INVALIDATING);
 		return SCI_SUCCESS;
+	case SCI_RNC_AWAIT_SUSPENSION:
+		sci_remote_node_context_setup_to_destroy(sci_rnc, cb_fn, cb_p);
+		return SCI_SUCCESS;
 	case SCI_RNC_INITIAL:
 		dev_warn(scirdev_to_dev(rnc_to_dev(sci_rnc)),
-			 "%s: invalid state %d\n", __func__, state);
+			 "%s: invalid state: %s\n", __func__,
+			 rnc_state_name(state));
 		/* We have decided that the destruct request on the remote node context
 		 * can not fail since it is either in the initial/destroyed state or is
 		 * can be destroyed.
@@ -485,35 +558,101 @@ enum sci_status sci_remote_node_context_destruct(struct sci_remote_node_context
 		return SCI_SUCCESS;
 	default:
 		dev_warn(scirdev_to_dev(rnc_to_dev(sci_rnc)),
-			 "%s: invalid state %d\n", __func__, state);
+			 "%s: invalid state %s\n", __func__,
+			 rnc_state_name(state));
 		return SCI_FAILURE_INVALID_STATE;
 	}
 }
 
-enum sci_status sci_remote_node_context_suspend(struct sci_remote_node_context *sci_rnc,
-						     u32 suspend_type,
-						     scics_sds_remote_node_context_callback cb_fn,
-						     void *cb_p)
+enum sci_status sci_remote_node_context_suspend(
+			struct sci_remote_node_context *sci_rnc,
+			enum sci_remote_node_suspension_reasons suspend_reason,
+			u32 suspend_type)
 {
-	enum scis_sds_remote_node_context_states state;
+	enum scis_sds_remote_node_context_states state
+		= sci_rnc->sm.current_state_id;
+	struct isci_remote_device *idev = rnc_to_dev(sci_rnc);
+	enum sci_status status = SCI_FAILURE_INVALID_STATE;
+	enum sci_remote_node_context_destination_state dest_param =
+		RNC_DEST_UNSPECIFIED;
+
+	dev_dbg(scirdev_to_dev(idev),
+		"%s: current state %s, current suspend_type %x dest state %d,"
+			" arg suspend_reason %d, arg suspend_type %x",
+		__func__, rnc_state_name(state), sci_rnc->suspend_type,
+		sci_rnc->destination_state, suspend_reason,
+		suspend_type);
+
+	/* Disable automatic state continuations if explicitly suspending. */
+	if ((suspend_reason == SCI_HW_SUSPEND) ||
+	    (sci_rnc->destination_state == RNC_DEST_FINAL))
+		dest_param = sci_rnc->destination_state;
 
-	state = sci_rnc->sm.current_state_id;
-	if (state != SCI_RNC_READY) {
+	switch (state) {
+	case SCI_RNC_READY:
+		break;
+	case SCI_RNC_INVALIDATING:
+		if (sci_rnc->destination_state == RNC_DEST_FINAL) {
+			dev_warn(scirdev_to_dev(idev),
+				 "%s: already destroying %p\n",
+				 __func__, sci_rnc);
+			return SCI_FAILURE_INVALID_STATE;
+		}
+		/* Fall through and handle like SCI_RNC_POSTING */
+	case SCI_RNC_RESUMING:
+		/* Fall through and handle like SCI_RNC_POSTING */
+	case SCI_RNC_POSTING:
+		/* Set the destination state to AWAIT - this signals the
+		 * entry into the SCI_RNC_READY state that a suspension
+		 * needs to be done immediately.
+		 */
+		if (sci_rnc->destination_state != RNC_DEST_FINAL)
+			sci_rnc->destination_state = RNC_DEST_SUSPENDED;
+		sci_rnc->suspend_type = suspend_type;
+		sci_rnc->suspend_reason = suspend_reason;
+		return SCI_SUCCESS;
+
+	case SCI_RNC_TX_SUSPENDED:
+		if (suspend_type == SCU_EVENT_TL_RNC_SUSPEND_TX)
+			status = SCI_SUCCESS;
+		break;
+	case SCI_RNC_TX_RX_SUSPENDED:
+		if (suspend_type == SCU_EVENT_TL_RNC_SUSPEND_TX_RX)
+			status = SCI_SUCCESS;
+		break;
+	case SCI_RNC_AWAIT_SUSPENSION:
+		if ((sci_rnc->suspend_type == SCU_EVENT_TL_RNC_SUSPEND_TX_RX)
+		    || (suspend_type == sci_rnc->suspend_type))
+			return SCI_SUCCESS;
+		break;
+	default:
 		dev_warn(scirdev_to_dev(rnc_to_dev(sci_rnc)),
-			 "%s: invalid state %d\n", __func__, state);
+			 "%s: invalid state %s\n", __func__,
+			 rnc_state_name(state));
 		return SCI_FAILURE_INVALID_STATE;
 	}
+	sci_rnc->destination_state = dest_param;
+	sci_rnc->suspend_type = suspend_type;
+	sci_rnc->suspend_reason = suspend_reason;
+
+	if (status == SCI_SUCCESS) { /* Already in the destination state? */
+		struct isci_host *ihost = idev->owning_port->owning_controller;
+
+		wake_up_all(&ihost->eventq); /* Let observers look. */
+		return SCI_SUCCESS;
+	}
+	if ((suspend_reason == SCI_SW_SUSPEND_NORMAL) ||
+	    (suspend_reason == SCI_SW_SUSPEND_LINKHANG_DETECT)) {
 
-	sci_rnc->user_callback   = cb_fn;
-	sci_rnc->user_cookie     = cb_p;
-	sci_rnc->suspension_code = suspend_type;
+		if (suspend_reason == SCI_SW_SUSPEND_LINKHANG_DETECT)
+			isci_dev_set_hang_detection_timeout(idev, 0x00000001);
 
-	if (suspend_type == SCI_SOFTWARE_SUSPENSION) {
-		sci_remote_device_post_request(rnc_to_dev(sci_rnc),
-						    SCU_CONTEXT_COMMAND_POST_RNC_SUSPEND_TX);
+		sci_remote_device_post_request(
+			idev, SCI_SOFTWARE_SUSPEND_CMD);
 	}
+	if (state != SCI_RNC_AWAIT_SUSPENSION)
+		sci_change_state(&sci_rnc->sm, SCI_RNC_AWAIT_SUSPENSION);
 
-	sci_change_state(&sci_rnc->sm, SCI_RNC_AWAIT_SUSPENSION);
 	return SCI_SUCCESS;
 }
 
@@ -522,56 +661,86 @@ enum sci_status sci_remote_node_context_resume(struct sci_remote_node_context *s
 						    void *cb_p)
 {
 	enum scis_sds_remote_node_context_states state;
+	struct isci_remote_device *idev = rnc_to_dev(sci_rnc);
 
 	state = sci_rnc->sm.current_state_id;
+	dev_dbg(scirdev_to_dev(idev),
+		"%s: state %s, cb_fn = %p, cb_p = %p; dest_state = %d; "
+			"dev resume path %s\n",
+		__func__, rnc_state_name(state), cb_fn, cb_p,
+		sci_rnc->destination_state,
+		test_bit(IDEV_ABORT_PATH_ACTIVE, &idev->flags)
+			? "<abort active>" : "<normal>");
+
 	switch (state) {
 	case SCI_RNC_INITIAL:
 		if (sci_rnc->remote_node_index == SCIC_SDS_REMOTE_NODE_CONTEXT_INVALID_INDEX)
 			return SCI_FAILURE_INVALID_STATE;
 
-		sci_remote_node_context_setup_to_resume(sci_rnc, cb_fn, cb_p);
-		sci_remote_node_context_construct_buffer(sci_rnc);
-		sci_change_state(&sci_rnc->sm, SCI_RNC_POSTING);
+		sci_remote_node_context_setup_to_resume(sci_rnc, cb_fn,	cb_p,
+							RNC_DEST_READY);
+		if (!test_bit(IDEV_ABORT_PATH_ACTIVE, &idev->flags)) {
+			sci_remote_node_context_construct_buffer(sci_rnc);
+			sci_change_state(&sci_rnc->sm, SCI_RNC_POSTING);
+		}
 		return SCI_SUCCESS;
+
 	case SCI_RNC_POSTING:
 	case SCI_RNC_INVALIDATING:
 	case SCI_RNC_RESUMING:
-		if (sci_rnc->destination_state != SCIC_SDS_REMOTE_NODE_DESTINATION_STATE_READY)
-			return SCI_FAILURE_INVALID_STATE;
-
-		sci_rnc->user_callback = cb_fn;
-		sci_rnc->user_cookie   = cb_p;
+		/* We are still waiting to post when a resume was
+		 * requested.
+		 */
+		switch (sci_rnc->destination_state) {
+		case RNC_DEST_SUSPENDED:
+		case RNC_DEST_SUSPENDED_RESUME:
+			/* Previously waiting to suspend after posting.
+			 * Now continue onto resumption.
+			 */
+			sci_remote_node_context_setup_to_resume(
+				sci_rnc, cb_fn, cb_p,
+				RNC_DEST_SUSPENDED_RESUME);
+			break;
+		default:
+			sci_remote_node_context_setup_to_resume(
+				sci_rnc, cb_fn, cb_p,
+				RNC_DEST_READY);
+			break;
+		}
 		return SCI_SUCCESS;
-	case SCI_RNC_TX_SUSPENDED: {
-		struct isci_remote_device *idev = rnc_to_dev(sci_rnc);
-		struct domain_device *dev = idev->domain_dev;
-
-		sci_remote_node_context_setup_to_resume(sci_rnc, cb_fn, cb_p);
-
-		/* TODO: consider adding a resume action of NONE, INVALIDATE, WRITE_TLCR */
-		if (dev->dev_type == SAS_END_DEV || dev_is_expander(dev))
-			sci_change_state(&sci_rnc->sm, SCI_RNC_RESUMING);
-		else if (dev->dev_type == SATA_DEV || (dev->tproto & SAS_PROTOCOL_STP)) {
-			if (idev->is_direct_attached) {
-				/* @todo Fix this since I am being silly in writing to the STPTLDARNI register. */
-				sci_change_state(&sci_rnc->sm, SCI_RNC_RESUMING);
-			} else {
-				sci_change_state(&sci_rnc->sm, SCI_RNC_INVALIDATING);
+
+	case SCI_RNC_TX_SUSPENDED:
+	case SCI_RNC_TX_RX_SUSPENDED:
+		{
+			struct domain_device *dev = idev->domain_dev;
+			/* If this is an expander attached SATA device we must
+			 * invalidate and repost the RNC since this is the only
+			 * way to clear the TCi to NCQ tag mapping table for
+			 * the RNi. All other device types we can just resume.
+			 */
+			sci_remote_node_context_setup_to_resume(
+				sci_rnc, cb_fn, cb_p, RNC_DEST_READY);
+
+			if (!test_bit(IDEV_ABORT_PATH_ACTIVE, &idev->flags)) {
+				if ((dev_is_sata(dev) && dev->parent) ||
+				    (sci_rnc->destination_state == RNC_DEST_FINAL))
+					sci_change_state(&sci_rnc->sm,
+							 SCI_RNC_INVALIDATING);
+				else
+					sci_change_state(&sci_rnc->sm,
+							 SCI_RNC_RESUMING);
 			}
-		} else
-			return SCI_FAILURE;
+		}
 		return SCI_SUCCESS;
-	}
-	case SCI_RNC_TX_RX_SUSPENDED:
-		sci_remote_node_context_setup_to_resume(sci_rnc, cb_fn, cb_p);
-		sci_change_state(&sci_rnc->sm, SCI_RNC_RESUMING);
-		return SCI_FAILURE_INVALID_STATE;
+
 	case SCI_RNC_AWAIT_SUSPENSION:
-		sci_remote_node_context_setup_to_resume(sci_rnc, cb_fn, cb_p);
+		sci_remote_node_context_setup_to_resume(
+			sci_rnc, cb_fn, cb_p, RNC_DEST_SUSPENDED_RESUME);
 		return SCI_SUCCESS;
 	default:
 		dev_warn(scirdev_to_dev(rnc_to_dev(sci_rnc)),
-			 "%s: invalid state %d\n", __func__, state);
+			 "%s: invalid state %s\n", __func__,
+			 rnc_state_name(state));
 		return SCI_FAILURE_INVALID_STATE;
 	}
 }
@@ -590,35 +759,51 @@ enum sci_status sci_remote_node_context_start_io(struct sci_remote_node_context
 	case SCI_RNC_TX_RX_SUSPENDED:
 	case SCI_RNC_AWAIT_SUSPENSION:
 		dev_warn(scirdev_to_dev(rnc_to_dev(sci_rnc)),
-			 "%s: invalid state %d\n", __func__, state);
+			 "%s: invalid state %s\n", __func__,
+			 rnc_state_name(state));
 		return SCI_FAILURE_REMOTE_DEVICE_RESET_REQUIRED;
 	default:
-		break;
+		dev_dbg(scirdev_to_dev(rnc_to_dev(sci_rnc)),
+			"%s: invalid state %s\n", __func__,
+			rnc_state_name(state));
+		return SCI_FAILURE_INVALID_STATE;
 	}
-	dev_dbg(scirdev_to_dev(rnc_to_dev(sci_rnc)),
-		"%s: requested to start IO while still resuming, %d\n",
-		__func__, state);
-	return SCI_FAILURE_INVALID_STATE;
 }
 
-enum sci_status sci_remote_node_context_start_task(struct sci_remote_node_context *sci_rnc,
-							struct isci_request *ireq)
+enum sci_status sci_remote_node_context_start_task(
+	struct sci_remote_node_context *sci_rnc,
+	struct isci_request *ireq,
+	scics_sds_remote_node_context_callback cb_fn,
+	void *cb_p)
+{
+	enum sci_status status = sci_remote_node_context_resume(sci_rnc,
+								cb_fn, cb_p);
+	if (status != SCI_SUCCESS)
+		dev_warn(scirdev_to_dev(rnc_to_dev(sci_rnc)),
+			"%s: resume failed: %d\n", __func__, status);
+	return status;
+}
+
+int sci_remote_node_context_is_safe_to_abort(
+	struct sci_remote_node_context *sci_rnc)
 {
 	enum scis_sds_remote_node_context_states state;
 
 	state = sci_rnc->sm.current_state_id;
 	switch (state) {
+	case SCI_RNC_INVALIDATING:
+	case SCI_RNC_TX_RX_SUSPENDED:
+		return 1;
+	case SCI_RNC_POSTING:
 	case SCI_RNC_RESUMING:
 	case SCI_RNC_READY:
-	case SCI_RNC_AWAIT_SUSPENSION:
-		return SCI_SUCCESS;
 	case SCI_RNC_TX_SUSPENDED:
-	case SCI_RNC_TX_RX_SUSPENDED:
-		sci_remote_node_context_resume(sci_rnc, NULL, NULL);
-		return SCI_SUCCESS;
+	case SCI_RNC_AWAIT_SUSPENSION:
+	case SCI_RNC_INITIAL:
+		return 0;
 	default:
 		dev_warn(scirdev_to_dev(rnc_to_dev(sci_rnc)),
 			 "%s: invalid state %d\n", __func__, state);
-		return SCI_FAILURE_INVALID_STATE;
+		return 0;
 	}
 }
diff --git a/drivers/scsi/isci/remote_node_context.h b/drivers/scsi/isci/remote_node_context.h
index a241e0f4c86..a703b9ce0c2 100644
--- a/drivers/scsi/isci/remote_node_context.h
+++ b/drivers/scsi/isci/remote_node_context.h
@@ -75,8 +75,13 @@
  */
 #define SCIC_SDS_REMOTE_NODE_CONTEXT_INVALID_INDEX    0x0FFF
 
-#define SCU_HARDWARE_SUSPENSION  (0)
-#define SCI_SOFTWARE_SUSPENSION  (1)
+enum sci_remote_node_suspension_reasons {
+	SCI_HW_SUSPEND,
+	SCI_SW_SUSPEND_NORMAL,
+	SCI_SW_SUSPEND_LINKHANG_DETECT
+};
+#define SCI_SOFTWARE_SUSPEND_CMD SCU_CONTEXT_COMMAND_POST_RNC_SUSPEND_TX_RX
+#define SCI_SOFTWARE_SUSPEND_EXPECTED_EVENT SCU_EVENT_TL_RNC_SUSPEND_TX_RX
 
 struct isci_request;
 struct isci_remote_device;
@@ -137,9 +142,13 @@ const char *rnc_state_name(enum scis_sds_remote_node_context_states state);
  * node context.
  */
 enum sci_remote_node_context_destination_state {
-	SCIC_SDS_REMOTE_NODE_DESTINATION_STATE_UNSPECIFIED,
-	SCIC_SDS_REMOTE_NODE_DESTINATION_STATE_READY,
-	SCIC_SDS_REMOTE_NODE_DESTINATION_STATE_FINAL
+	RNC_DEST_UNSPECIFIED,
+	RNC_DEST_READY,
+	RNC_DEST_FINAL,
+	RNC_DEST_SUSPENDED,       /* Set when suspend during post/invalidate */
+	RNC_DEST_SUSPENDED_RESUME /* Set when a resume was done during posting
+				   * or invalidating and already suspending.
+				   */
 };
 
 /**
@@ -156,10 +165,12 @@ struct sci_remote_node_context {
 	u16 remote_node_index;
 
 	/**
-	 * This field is the recored suspension code or the reason for the remote node
+	 * This field is the recored suspension type of the remote node
 	 * context suspension.
 	 */
-	u32 suspension_code;
+	u32 suspend_type;
+	enum sci_remote_node_suspension_reasons suspend_reason;
+	u32 suspend_count;
 
 	/**
 	 * This field is true if the remote node context is resuming from its current
@@ -193,6 +204,8 @@ void sci_remote_node_context_construct(struct sci_remote_node_context *rnc,
 bool sci_remote_node_context_is_ready(
 	struct sci_remote_node_context *sci_rnc);
 
+bool sci_remote_node_context_is_suspended(struct sci_remote_node_context *sci_rnc);
+
 enum sci_status sci_remote_node_context_event_handler(struct sci_remote_node_context *sci_rnc,
 							   u32 event_code);
 enum sci_status sci_remote_node_context_destruct(struct sci_remote_node_context *sci_rnc,
@@ -200,14 +213,24 @@ enum sci_status sci_remote_node_context_destruct(struct sci_remote_node_context
 						      void *callback_parameter);
 enum sci_status sci_remote_node_context_suspend(struct sci_remote_node_context *sci_rnc,
 						     u32 suspend_type,
-						     scics_sds_remote_node_context_callback cb_fn,
-						     void *cb_p);
+						     u32 suspension_code);
 enum sci_status sci_remote_node_context_resume(struct sci_remote_node_context *sci_rnc,
 						    scics_sds_remote_node_context_callback cb_fn,
 						    void *cb_p);
 enum sci_status sci_remote_node_context_start_task(struct sci_remote_node_context *sci_rnc,
-							struct isci_request *ireq);
+						   struct isci_request *ireq,
+						   scics_sds_remote_node_context_callback cb_fn,
+						   void *cb_p);
 enum sci_status sci_remote_node_context_start_io(struct sci_remote_node_context *sci_rnc,
 						      struct isci_request *ireq);
+int sci_remote_node_context_is_safe_to_abort(
+	struct sci_remote_node_context *sci_rnc);
 
+static inline bool sci_remote_node_context_is_being_destroyed(
+	struct sci_remote_node_context *sci_rnc)
+{
+	return (sci_rnc->destination_state == RNC_DEST_FINAL)
+		|| ((sci_rnc->sm.current_state_id == SCI_RNC_INITIAL)
+		    && (sci_rnc->destination_state == RNC_DEST_UNSPECIFIED));
+}
 #endif  /* _SCIC_SDS_REMOTE_NODE_CONTEXT_H_ */
diff --git a/drivers/scsi/isci/request.c b/drivers/scsi/isci/request.c
index 2def1e3960f..7a0431c7349 100644
--- a/drivers/scsi/isci/request.c
+++ b/drivers/scsi/isci/request.c
@@ -92,11 +92,11 @@ static dma_addr_t to_sgl_element_pair_dma(struct isci_host *ihost,
 	if (idx == 0) {
 		offset = (void *) &ireq->tc->sgl_pair_ab -
 			 (void *) &ihost->task_context_table[0];
-		return ihost->task_context_dma + offset;
+		return ihost->tc_dma + offset;
 	} else if (idx == 1) {
 		offset = (void *) &ireq->tc->sgl_pair_cd -
 			 (void *) &ihost->task_context_table[0];
-		return ihost->task_context_dma + offset;
+		return ihost->tc_dma + offset;
 	}
 
 	return sci_io_request_get_dma_addr(ireq, &ireq->sg_table[idx - 2]);
@@ -730,7 +730,7 @@ static enum sci_status sci_io_request_construct_basic_ssp(struct isci_request *i
 {
 	struct sas_task *task = isci_request_access_task(ireq);
 
-	ireq->protocol = SCIC_SSP_PROTOCOL;
+	ireq->protocol = SAS_PROTOCOL_SSP;
 
 	scu_ssp_io_request_construct_task_context(ireq,
 						  task->data_dir,
@@ -763,7 +763,7 @@ static enum sci_status sci_io_request_construct_basic_sata(struct isci_request *
 	bool copy = false;
 	struct sas_task *task = isci_request_access_task(ireq);
 
-	ireq->protocol = SCIC_STP_PROTOCOL;
+	ireq->protocol = SAS_PROTOCOL_STP;
 
 	copy = (task->data_dir == DMA_NONE) ? false : true;
 
@@ -863,6 +863,8 @@ sci_io_request_terminate(struct isci_request *ireq)
 
 	switch (state) {
 	case SCI_REQ_CONSTRUCTED:
+		/* Set to make sure no HW terminate posting is done: */
+		set_bit(IREQ_TC_ABORT_POSTED, &ireq->flags);
 		ireq->scu_status = SCU_TASK_DONE_TASK_ABORT;
 		ireq->sci_status = SCI_FAILURE_IO_TERMINATED;
 		sci_change_state(&ireq->sm, SCI_REQ_COMPLETED);
@@ -883,8 +885,7 @@ sci_io_request_terminate(struct isci_request *ireq)
 	case SCI_REQ_ATAPI_WAIT_PIO_SETUP:
 	case SCI_REQ_ATAPI_WAIT_D2H:
 	case SCI_REQ_ATAPI_WAIT_TC_COMP:
-		sci_change_state(&ireq->sm, SCI_REQ_ABORTING);
-		return SCI_SUCCESS;
+		/* Fall through and change state to ABORTING... */
 	case SCI_REQ_TASK_WAIT_TC_RESP:
 		/* The task frame was already confirmed to have been
 		 * sent by the SCU HW.  Since the state machine is
@@ -893,20 +894,21 @@ sci_io_request_terminate(struct isci_request *ireq)
 		 * and don't wait for the task response.
 		 */
 		sci_change_state(&ireq->sm, SCI_REQ_ABORTING);
-		sci_change_state(&ireq->sm, SCI_REQ_COMPLETED);
-		return SCI_SUCCESS;
+		/* Fall through and handle like ABORTING... */
 	case SCI_REQ_ABORTING:
-		/* If a request has a termination requested twice, return
-		 * a failure indication, since HW confirmation of the first
-		 * abort is still outstanding.
+		if (!isci_remote_device_is_safe_to_abort(ireq->target_device))
+			set_bit(IREQ_PENDING_ABORT, &ireq->flags);
+		else
+			clear_bit(IREQ_PENDING_ABORT, &ireq->flags);
+		/* If the request is only waiting on the remote device
+		 * suspension, return SUCCESS so the caller will wait too.
 		 */
+		return SCI_SUCCESS;
 	case SCI_REQ_COMPLETED:
 	default:
 		dev_warn(&ireq->owning_controller->pdev->dev,
 			 "%s: SCIC IO Request requested to abort while in wrong "
-			 "state %d\n",
-			 __func__,
-			 ireq->sm.current_state_id);
+			 "state %d\n", __func__, ireq->sm.current_state_id);
 		break;
 	}
 
@@ -1070,7 +1072,7 @@ request_started_state_tc_event(struct isci_request *ireq,
 	case SCU_MAKE_COMPLETION_STATUS(SCU_TASK_DONE_UNEXP_SDBFIS):
 	case SCU_MAKE_COMPLETION_STATUS(SCU_TASK_DONE_REG_ERR):
 	case SCU_MAKE_COMPLETION_STATUS(SCU_TASK_DONE_SDB_ERR):
-		if (ireq->protocol == SCIC_STP_PROTOCOL) {
+		if (ireq->protocol == SAS_PROTOCOL_STP) {
 			ireq->scu_status = SCU_GET_COMPLETION_TL_STATUS(completion_code) >>
 					   SCU_COMPLETION_TL_STATUS_SHIFT;
 			ireq->sci_status = SCI_FAILURE_REMOTE_DEVICE_RESET_REQUIRED;
@@ -2117,7 +2119,7 @@ static enum sci_status stp_request_udma_await_tc_event(struct isci_request *ireq
 		 */
 		if (ireq->stp.rsp.fis_type == FIS_REGD2H) {
 			sci_remote_device_suspend(ireq->target_device,
-				SCU_EVENT_SPECIFIC(SCU_NORMALIZE_COMPLETION_STATUS(completion_code)));
+						  SCI_SW_SUSPEND_NORMAL);
 
 			ireq->scu_status = SCU_TASK_DONE_CHECK_RESPONSE;
 			ireq->sci_status = SCI_FAILURE_IO_RESPONSE_VALID;
@@ -2138,13 +2140,6 @@ static enum sci_status stp_request_udma_await_tc_event(struct isci_request *ireq
 	/* TODO We can retry the command for SCU_TASK_DONE_CMD_LL_R_ERR
 	 * - this comes only for B0
 	 */
-	case SCU_MAKE_COMPLETION_STATUS(SCU_TASK_DONE_INV_FIS_LEN):
-	case SCU_MAKE_COMPLETION_STATUS(SCU_TASK_DONE_MAX_PLD_ERR):
-	case SCU_MAKE_COMPLETION_STATUS(SCU_TASK_DONE_LL_R_ERR):
-	case SCU_MAKE_COMPLETION_STATUS(SCU_TASK_DONE_CMD_LL_R_ERR):
-		sci_remote_device_suspend(ireq->target_device,
-			SCU_EVENT_SPECIFIC(SCU_NORMALIZE_COMPLETION_STATUS(completion_code)));
-		/* Fall through to the default case */
 	default:
 		/* All other completion status cause the IO to be complete. */
 		ireq->scu_status = SCU_NORMALIZE_COMPLETION_STATUS(completion_code);
@@ -2262,15 +2257,151 @@ static enum sci_status atapi_data_tc_completion_handler(struct isci_request *ire
 	return status;
 }
 
+static int sci_request_smp_completion_status_is_tx_suspend(
+	unsigned int completion_status)
+{
+	switch (completion_status) {
+	case SCU_TASK_OPEN_REJECT_WRONG_DESTINATION:
+	case SCU_TASK_OPEN_REJECT_RESERVED_ABANDON_1:
+	case SCU_TASK_OPEN_REJECT_RESERVED_ABANDON_2:
+	case SCU_TASK_OPEN_REJECT_RESERVED_ABANDON_3:
+	case SCU_TASK_OPEN_REJECT_BAD_DESTINATION:
+	case SCU_TASK_OPEN_REJECT_ZONE_VIOLATION:
+		return 1;
+	}
+	return 0;
+}
+
+static int sci_request_smp_completion_status_is_tx_rx_suspend(
+	unsigned int completion_status)
+{
+	return 0; /* There are no Tx/Rx SMP suspend conditions. */
+}
+
+static int sci_request_ssp_completion_status_is_tx_suspend(
+	unsigned int completion_status)
+{
+	switch (completion_status) {
+	case SCU_TASK_DONE_TX_RAW_CMD_ERR:
+	case SCU_TASK_DONE_LF_ERR:
+	case SCU_TASK_OPEN_REJECT_WRONG_DESTINATION:
+	case SCU_TASK_OPEN_REJECT_RESERVED_ABANDON_1:
+	case SCU_TASK_OPEN_REJECT_RESERVED_ABANDON_2:
+	case SCU_TASK_OPEN_REJECT_RESERVED_ABANDON_3:
+	case SCU_TASK_OPEN_REJECT_BAD_DESTINATION:
+	case SCU_TASK_OPEN_REJECT_ZONE_VIOLATION:
+	case SCU_TASK_OPEN_REJECT_STP_RESOURCES_BUSY:
+	case SCU_TASK_OPEN_REJECT_PROTOCOL_NOT_SUPPORTED:
+	case SCU_TASK_OPEN_REJECT_CONNECTION_RATE_NOT_SUPPORTED:
+		return 1;
+	}
+	return 0;
+}
+
+static int sci_request_ssp_completion_status_is_tx_rx_suspend(
+	unsigned int completion_status)
+{
+	return 0; /* There are no Tx/Rx SSP suspend conditions. */
+}
+
+static int sci_request_stpsata_completion_status_is_tx_suspend(
+	unsigned int completion_status)
+{
+	switch (completion_status) {
+	case SCU_TASK_DONE_TX_RAW_CMD_ERR:
+	case SCU_TASK_DONE_LL_R_ERR:
+	case SCU_TASK_DONE_LL_PERR:
+	case SCU_TASK_DONE_REG_ERR:
+	case SCU_TASK_DONE_SDB_ERR:
+	case SCU_TASK_OPEN_REJECT_WRONG_DESTINATION:
+	case SCU_TASK_OPEN_REJECT_RESERVED_ABANDON_1:
+	case SCU_TASK_OPEN_REJECT_RESERVED_ABANDON_2:
+	case SCU_TASK_OPEN_REJECT_RESERVED_ABANDON_3:
+	case SCU_TASK_OPEN_REJECT_BAD_DESTINATION:
+	case SCU_TASK_OPEN_REJECT_ZONE_VIOLATION:
+	case SCU_TASK_OPEN_REJECT_STP_RESOURCES_BUSY:
+	case SCU_TASK_OPEN_REJECT_PROTOCOL_NOT_SUPPORTED:
+	case SCU_TASK_OPEN_REJECT_CONNECTION_RATE_NOT_SUPPORTED:
+		return 1;
+	}
+	return 0;
+}
+
+
+static int sci_request_stpsata_completion_status_is_tx_rx_suspend(
+	unsigned int completion_status)
+{
+	switch (completion_status) {
+	case SCU_TASK_DONE_LF_ERR:
+	case SCU_TASK_DONE_LL_SY_TERM:
+	case SCU_TASK_DONE_LL_LF_TERM:
+	case SCU_TASK_DONE_BREAK_RCVD:
+	case SCU_TASK_DONE_INV_FIS_LEN:
+	case SCU_TASK_DONE_UNEXP_FIS:
+	case SCU_TASK_DONE_UNEXP_SDBFIS:
+	case SCU_TASK_DONE_MAX_PLD_ERR:
+		return 1;
+	}
+	return 0;
+}
+
+static void sci_request_handle_suspending_completions(
+	struct isci_request *ireq,
+	u32 completion_code)
+{
+	int is_tx = 0;
+	int is_tx_rx = 0;
+
+	switch (ireq->protocol) {
+	case SAS_PROTOCOL_SMP:
+		is_tx = sci_request_smp_completion_status_is_tx_suspend(
+			completion_code);
+		is_tx_rx = sci_request_smp_completion_status_is_tx_rx_suspend(
+			completion_code);
+		break;
+	case SAS_PROTOCOL_SSP:
+		is_tx = sci_request_ssp_completion_status_is_tx_suspend(
+			completion_code);
+		is_tx_rx = sci_request_ssp_completion_status_is_tx_rx_suspend(
+			completion_code);
+		break;
+	case SAS_PROTOCOL_STP:
+		is_tx = sci_request_stpsata_completion_status_is_tx_suspend(
+			completion_code);
+		is_tx_rx =
+			sci_request_stpsata_completion_status_is_tx_rx_suspend(
+				completion_code);
+		break;
+	default:
+		dev_warn(&ireq->isci_host->pdev->dev,
+			 "%s: request %p has no valid protocol\n",
+			 __func__, ireq);
+		break;
+	}
+	if (is_tx || is_tx_rx) {
+		BUG_ON(is_tx && is_tx_rx);
+
+		sci_remote_node_context_suspend(
+			&ireq->target_device->rnc,
+			SCI_HW_SUSPEND,
+			(is_tx_rx) ? SCU_EVENT_TL_RNC_SUSPEND_TX_RX
+				   : SCU_EVENT_TL_RNC_SUSPEND_TX);
+	}
+}
+
 enum sci_status
 sci_io_request_tc_completion(struct isci_request *ireq,
-				  u32 completion_code)
+			     u32 completion_code)
 {
 	enum sci_base_request_states state;
 	struct isci_host *ihost = ireq->owning_controller;
 
 	state = ireq->sm.current_state_id;
 
+	/* Decode those completions that signal upcoming suspension events. */
+	sci_request_handle_suspending_completions(
+		ireq, SCU_GET_COMPLETION_TL_STATUS(completion_code));
+
 	switch (state) {
 	case SCI_REQ_STARTED:
 		return request_started_state_tc_event(ireq, completion_code);
@@ -2362,9 +2493,6 @@ static void isci_request_process_response_iu(
  * @request: This parameter is the completed isci_request object.
  * @response_ptr: This parameter specifies the service response for the I/O.
  * @status_ptr: This parameter specifies the exec status for the I/O.
- * @complete_to_host_ptr: This parameter specifies the action to be taken by
- *    the LLDD with respect to completing this request or forcing an abort
- *    condition on the I/O.
  * @open_rej_reason: This parameter specifies the encoded reason for the
  *    abandon-class reject.
  *
@@ -2375,14 +2503,12 @@ static void isci_request_set_open_reject_status(
 	struct sas_task *task,
 	enum service_response *response_ptr,
 	enum exec_status *status_ptr,
-	enum isci_completion_selection *complete_to_host_ptr,
 	enum sas_open_rej_reason open_rej_reason)
 {
 	/* Task in the target is done. */
 	set_bit(IREQ_COMPLETE_IN_TARGET, &request->flags);
 	*response_ptr                     = SAS_TASK_UNDELIVERED;
 	*status_ptr                       = SAS_OPEN_REJECT;
-	*complete_to_host_ptr             = isci_perform_normal_io_completion;
 	task->task_status.open_rej_reason = open_rej_reason;
 }
 
@@ -2392,9 +2518,6 @@ static void isci_request_set_open_reject_status(
  * @request: This parameter is the completed isci_request object.
  * @response_ptr: This parameter specifies the service response for the I/O.
  * @status_ptr: This parameter specifies the exec status for the I/O.
- * @complete_to_host_ptr: This parameter specifies the action to be taken by
- *    the LLDD with respect to completing this request or forcing an abort
- *    condition on the I/O.
  *
  * none.
  */
@@ -2403,8 +2526,7 @@ static void isci_request_handle_controller_specific_errors(
 	struct isci_request *request,
 	struct sas_task *task,
 	enum service_response *response_ptr,
-	enum exec_status *status_ptr,
-	enum isci_completion_selection *complete_to_host_ptr)
+	enum exec_status *status_ptr)
 {
 	unsigned int cstatus;
 
@@ -2445,9 +2567,6 @@ static void isci_request_handle_controller_specific_errors(
 				*status_ptr = SAS_ABORTED_TASK;
 
 			set_bit(IREQ_COMPLETE_IN_TARGET, &request->flags);
-
-			*complete_to_host_ptr =
-				isci_perform_normal_io_completion;
 		} else {
 			/* Task in the target is not done. */
 			*response_ptr = SAS_TASK_UNDELIVERED;
@@ -2458,9 +2577,6 @@ static void isci_request_handle_controller_specific_errors(
 				*status_ptr = SAM_STAT_TASK_ABORTED;
 
 			clear_bit(IREQ_COMPLETE_IN_TARGET, &request->flags);
-
-			*complete_to_host_ptr =
-				isci_perform_error_io_completion;
 		}
 
 		break;
@@ -2489,8 +2605,6 @@ static void isci_request_handle_controller_specific_errors(
 			*status_ptr = SAS_ABORTED_TASK;
 
 		set_bit(IREQ_COMPLETE_IN_TARGET, &request->flags);
-
-		*complete_to_host_ptr = isci_perform_normal_io_completion;
 		break;
 
 
@@ -2501,7 +2615,7 @@ static void isci_request_handle_controller_specific_errors(
 
 		isci_request_set_open_reject_status(
 			request, task, response_ptr, status_ptr,
-			complete_to_host_ptr, SAS_OREJ_WRONG_DEST);
+			SAS_OREJ_WRONG_DEST);
 		break;
 
 	case SCU_TASK_OPEN_REJECT_ZONE_VIOLATION:
@@ -2511,56 +2625,56 @@ static void isci_request_handle_controller_specific_errors(
 		 */
 		isci_request_set_open_reject_status(
 			request, task, response_ptr, status_ptr,
-			complete_to_host_ptr, SAS_OREJ_RESV_AB0);
+			SAS_OREJ_RESV_AB0);
 		break;
 
 	case SCU_TASK_OPEN_REJECT_RESERVED_ABANDON_1:
 
 		isci_request_set_open_reject_status(
 			request, task, response_ptr, status_ptr,
-			complete_to_host_ptr, SAS_OREJ_RESV_AB1);
+			SAS_OREJ_RESV_AB1);
 		break;
 
 	case SCU_TASK_OPEN_REJECT_RESERVED_ABANDON_2:
 
 		isci_request_set_open_reject_status(
 			request, task, response_ptr, status_ptr,
-			complete_to_host_ptr, SAS_OREJ_RESV_AB2);
+			SAS_OREJ_RESV_AB2);
 		break;
 
 	case SCU_TASK_OPEN_REJECT_RESERVED_ABANDON_3:
 
 		isci_request_set_open_reject_status(
 			request, task, response_ptr, status_ptr,
-			complete_to_host_ptr, SAS_OREJ_RESV_AB3);
+			SAS_OREJ_RESV_AB3);
 		break;
 
 	case SCU_TASK_OPEN_REJECT_BAD_DESTINATION:
 
 		isci_request_set_open_reject_status(
 			request, task, response_ptr, status_ptr,
-			complete_to_host_ptr, SAS_OREJ_BAD_DEST);
+			SAS_OREJ_BAD_DEST);
 		break;
 
 	case SCU_TASK_OPEN_REJECT_STP_RESOURCES_BUSY:
 
 		isci_request_set_open_reject_status(
 			request, task, response_ptr, status_ptr,
-			complete_to_host_ptr, SAS_OREJ_STP_NORES);
+			SAS_OREJ_STP_NORES);
 		break;
 
 	case SCU_TASK_OPEN_REJECT_PROTOCOL_NOT_SUPPORTED:
 
 		isci_request_set_open_reject_status(
 			request, task, response_ptr, status_ptr,
-			complete_to_host_ptr, SAS_OREJ_EPROTO);
+			SAS_OREJ_EPROTO);
 		break;
 
 	case SCU_TASK_OPEN_REJECT_CONNECTION_RATE_NOT_SUPPORTED:
 
 		isci_request_set_open_reject_status(
 			request, task, response_ptr, status_ptr,
-			complete_to_host_ptr, SAS_OREJ_CONN_RATE);
+			SAS_OREJ_CONN_RATE);
 		break;
 
 	case SCU_TASK_DONE_LL_R_ERR:
@@ -2592,95 +2706,12 @@ static void isci_request_handle_controller_specific_errors(
 		*response_ptr = SAS_TASK_UNDELIVERED;
 		*status_ptr = SAM_STAT_TASK_ABORTED;
 
-		if (task->task_proto == SAS_PROTOCOL_SMP) {
+		if (task->task_proto == SAS_PROTOCOL_SMP)
 			set_bit(IREQ_COMPLETE_IN_TARGET, &request->flags);
-
-			*complete_to_host_ptr = isci_perform_normal_io_completion;
-		} else {
+		else
 			clear_bit(IREQ_COMPLETE_IN_TARGET, &request->flags);
-
-			*complete_to_host_ptr = isci_perform_error_io_completion;
-		}
-		break;
-	}
-}
-
-/**
- * isci_task_save_for_upper_layer_completion() - This function saves the
- *    request for later completion to the upper layer driver.
- * @host: This parameter is a pointer to the host on which the the request
- *    should be queued (either as an error or success).
- * @request: This parameter is the completed request.
- * @response: This parameter is the response code for the completed task.
- * @status: This parameter is the status code for the completed task.
- *
- * none.
- */
-static void isci_task_save_for_upper_layer_completion(
-	struct isci_host *host,
-	struct isci_request *request,
-	enum service_response response,
-	enum exec_status status,
-	enum isci_completion_selection task_notification_selection)
-{
-	struct sas_task *task = isci_request_access_task(request);
-
-	task_notification_selection
-		= isci_task_set_completion_status(task, response, status,
-						  task_notification_selection);
-
-	/* Tasks aborted specifically by a call to the lldd_abort_task
-	 * function should not be completed to the host in the regular path.
-	 */
-	switch (task_notification_selection) {
-
-	case isci_perform_normal_io_completion:
-		/* Normal notification (task_done) */
-
-		/* Add to the completed list. */
-		list_add(&request->completed_node,
-			 &host->requests_to_complete);
-
-		/* Take the request off the device's pending request list. */
-		list_del_init(&request->dev_node);
-		break;
-
-	case isci_perform_aborted_io_completion:
-		/* No notification to libsas because this request is
-		 * already in the abort path.
-		 */
-		/* Wake up whatever process was waiting for this
-		 * request to complete.
-		 */
-		WARN_ON(request->io_request_completion == NULL);
-
-		if (request->io_request_completion != NULL) {
-
-			/* Signal whoever is waiting that this
-			* request is complete.
-			*/
-			complete(request->io_request_completion);
-		}
-		break;
-
-	case isci_perform_error_io_completion:
-		/* Use sas_task_abort */
-		/* Add to the aborted list. */
-		list_add(&request->completed_node,
-			 &host->requests_to_errorback);
-		break;
-
-	default:
-		/* Add to the error to libsas list. */
-		list_add(&request->completed_node,
-			 &host->requests_to_errorback);
 		break;
 	}
-	dev_dbg(&host->pdev->dev,
-		"%s: %d - task = %p, response=%d (%d), status=%d (%d)\n",
-		__func__, task_notification_selection, task,
-		(task) ? task->task_status.resp : 0, response,
-		(task) ? task->task_status.stat : 0, status);
 }
 
 static void isci_process_stp_response(struct sas_task *task, struct dev_to_host_fis *fis)
@@ -2715,295 +2746,164 @@ static void isci_request_io_request_complete(struct isci_host *ihost,
 	struct isci_remote_device *idev = request->target_device;
 	enum service_response response = SAS_TASK_UNDELIVERED;
 	enum exec_status status = SAS_ABORTED_TASK;
-	enum isci_request_status request_status;
-	enum isci_completion_selection complete_to_host
-		= isci_perform_normal_io_completion;
 
 	dev_dbg(&ihost->pdev->dev,
-		"%s: request = %p, task = %p,\n"
+		"%s: request = %p, task = %p, "
 		"task->data_dir = %d completion_status = 0x%x\n",
-		__func__,
-		request,
-		task,
-		task->data_dir,
-		completion_status);
+		__func__, request, task, task->data_dir, completion_status);
 
-	spin_lock(&request->state_lock);
-	request_status = request->status;
+	/* The request is done from an SCU HW perspective. */
 
-	/* Decode the request status.  Note that if the request has been
-	 * aborted by a task management function, we don't care
-	 * what the status is.
-	 */
-	switch (request_status) {
-
-	case aborted:
-		/* "aborted" indicates that the request was aborted by a task
-		 * management function, since once a task management request is
-		 * perfomed by the device, the request only completes because
-		 * of the subsequent driver terminate.
-		 *
-		 * Aborted also means an external thread is explicitly managing
-		 * this request, so that we do not complete it up the stack.
-		 *
-		 * The target is still there (since the TMF was successful).
-		 */
-		set_bit(IREQ_COMPLETE_IN_TARGET, &request->flags);
-		response = SAS_TASK_COMPLETE;
+	/* This is an active request being completed from the core. */
+	switch (completion_status) {
 
-		/* See if the device has been/is being stopped. Note
-		 * that we ignore the quiesce state, since we are
-		 * concerned about the actual device state.
-		 */
-		if (!idev)
-			status = SAS_DEVICE_UNKNOWN;
-		else
-			status = SAS_ABORTED_TASK;
+	case SCI_IO_FAILURE_RESPONSE_VALID:
+		dev_dbg(&ihost->pdev->dev,
+			"%s: SCI_IO_FAILURE_RESPONSE_VALID (%p/%p)\n",
+			__func__, request, task);
+
+		if (sas_protocol_ata(task->task_proto)) {
+			isci_process_stp_response(task, &request->stp.rsp);
+		} else if (SAS_PROTOCOL_SSP == task->task_proto) {
+
+			/* crack the iu response buffer. */
+			resp_iu = &request->ssp.rsp;
+			isci_request_process_response_iu(task, resp_iu,
+							 &ihost->pdev->dev);
+
+		} else if (SAS_PROTOCOL_SMP == task->task_proto) {
+
+			dev_err(&ihost->pdev->dev,
+				"%s: SCI_IO_FAILURE_RESPONSE_VALID: "
+					"SAS_PROTOCOL_SMP protocol\n",
+				__func__);
 
-		complete_to_host = isci_perform_aborted_io_completion;
-		/* This was an aborted request. */
+		} else
+			dev_err(&ihost->pdev->dev,
+				"%s: unknown protocol\n", __func__);
 
-		spin_unlock(&request->state_lock);
+		/* use the task status set in the task struct by the
+		* isci_request_process_response_iu call.
+		*/
+		set_bit(IREQ_COMPLETE_IN_TARGET, &request->flags);
+		response = task->task_status.resp;
+		status = task->task_status.stat;
 		break;
 
-	case aborting:
-		/* aborting means that the task management function tried and
-		 * failed to abort the request. We need to note the request
-		 * as SAS_TASK_UNDELIVERED, so that the scsi mid layer marks the
-		 * target as down.
-		 *
-		 * Aborting also means an external thread is explicitly managing
-		 * this request, so that we do not complete it up the stack.
-		 */
+	case SCI_IO_SUCCESS:
+	case SCI_IO_SUCCESS_IO_DONE_EARLY:
+
+		response = SAS_TASK_COMPLETE;
+		status   = SAM_STAT_GOOD;
 		set_bit(IREQ_COMPLETE_IN_TARGET, &request->flags);
-		response = SAS_TASK_UNDELIVERED;
 
-		if (!idev)
-			/* The device has been /is being stopped. Note that
-			 * we ignore the quiesce state, since we are
-			 * concerned about the actual device state.
-			 */
-			status = SAS_DEVICE_UNKNOWN;
-		else
-			status = SAS_PHY_DOWN;
+		if (completion_status == SCI_IO_SUCCESS_IO_DONE_EARLY) {
 
-		complete_to_host = isci_perform_aborted_io_completion;
+			/* This was an SSP / STP / SATA transfer.
+			* There is a possibility that less data than
+			* the maximum was transferred.
+			*/
+			u32 transferred_length = sci_req_tx_bytes(request);
 
-		/* This was an aborted request. */
+			task->task_status.residual
+				= task->total_xfer_len - transferred_length;
+
+			/* If there were residual bytes, call this an
+			* underrun.
+			*/
+			if (task->task_status.residual != 0)
+				status = SAS_DATA_UNDERRUN;
 
-		spin_unlock(&request->state_lock);
+			dev_dbg(&ihost->pdev->dev,
+				"%s: SCI_IO_SUCCESS_IO_DONE_EARLY %d\n",
+				__func__, status);
+
+		} else
+			dev_dbg(&ihost->pdev->dev, "%s: SCI_IO_SUCCESS\n",
+				__func__);
 		break;
 
-	case terminating:
+	case SCI_IO_FAILURE_TERMINATED:
 
-		/* This was an terminated request.  This happens when
-		 * the I/O is being terminated because of an action on
-		 * the device (reset, tear down, etc.), and the I/O needs
-		 * to be completed up the stack.
-		 */
+		dev_dbg(&ihost->pdev->dev,
+			"%s: SCI_IO_FAILURE_TERMINATED (%p/%p)\n",
+			__func__, request, task);
+
+		/* The request was terminated explicitly. */
 		set_bit(IREQ_COMPLETE_IN_TARGET, &request->flags);
 		response = SAS_TASK_UNDELIVERED;
 
 		/* See if the device has been/is being stopped. Note
-		 * that we ignore the quiesce state, since we are
-		 * concerned about the actual device state.
-		 */
+		* that we ignore the quiesce state, since we are
+		* concerned about the actual device state.
+		*/
 		if (!idev)
 			status = SAS_DEVICE_UNKNOWN;
 		else
 			status = SAS_ABORTED_TASK;
-
-		complete_to_host = isci_perform_aborted_io_completion;
-
-		/* This was a terminated request. */
-
-		spin_unlock(&request->state_lock);
 		break;
 
-	case dead:
-		/* This was a terminated request that timed-out during the
-		 * termination process.  There is no task to complete to
-		 * libsas.
-		 */
-		complete_to_host = isci_perform_normal_io_completion;
-		spin_unlock(&request->state_lock);
-		break;
+	case SCI_FAILURE_CONTROLLER_SPECIFIC_IO_ERR:
 
-	default:
-
-		/* The request is done from an SCU HW perspective. */
-		request->status = completed;
-
-		spin_unlock(&request->state_lock);
-
-		/* This is an active request being completed from the core. */
-		switch (completion_status) {
-
-		case SCI_IO_FAILURE_RESPONSE_VALID:
-			dev_dbg(&ihost->pdev->dev,
-				"%s: SCI_IO_FAILURE_RESPONSE_VALID (%p/%p)\n",
-				__func__,
-				request,
-				task);
-
-			if (sas_protocol_ata(task->task_proto)) {
-				isci_process_stp_response(task, &request->stp.rsp);
-			} else if (SAS_PROTOCOL_SSP == task->task_proto) {
-
-				/* crack the iu response buffer. */
-				resp_iu = &request->ssp.rsp;
-				isci_request_process_response_iu(task, resp_iu,
-								 &ihost->pdev->dev);
-
-			} else if (SAS_PROTOCOL_SMP == task->task_proto) {
-
-				dev_err(&ihost->pdev->dev,
-					"%s: SCI_IO_FAILURE_RESPONSE_VALID: "
-					"SAS_PROTOCOL_SMP protocol\n",
-					__func__);
-
-			} else
-				dev_err(&ihost->pdev->dev,
-					"%s: unknown protocol\n", __func__);
-
-			/* use the task status set in the task struct by the
-			 * isci_request_process_response_iu call.
-			 */
-			set_bit(IREQ_COMPLETE_IN_TARGET, &request->flags);
-			response = task->task_status.resp;
-			status = task->task_status.stat;
-			break;
+		isci_request_handle_controller_specific_errors(idev, request,
+							       task, &response,
+							       &status);
+		break;
 
-		case SCI_IO_SUCCESS:
-		case SCI_IO_SUCCESS_IO_DONE_EARLY:
+	case SCI_IO_FAILURE_REMOTE_DEVICE_RESET_REQUIRED:
+		/* This is a special case, in that the I/O completion
+		* is telling us that the device needs a reset.
+		* In order for the device reset condition to be
+		* noticed, the I/O has to be handled in the error
+		* handler.  Set the reset flag and cause the
+		* SCSI error thread to be scheduled.
+		*/
+		spin_lock_irqsave(&task->task_state_lock, task_flags);
+		task->task_state_flags |= SAS_TASK_NEED_DEV_RESET;
+		spin_unlock_irqrestore(&task->task_state_lock, task_flags);
 
-			response = SAS_TASK_COMPLETE;
-			status   = SAM_STAT_GOOD;
-			set_bit(IREQ_COMPLETE_IN_TARGET, &request->flags);
+		/* Fail the I/O. */
+		response = SAS_TASK_UNDELIVERED;
+		status = SAM_STAT_TASK_ABORTED;
 
-			if (completion_status == SCI_IO_SUCCESS_IO_DONE_EARLY) {
+		clear_bit(IREQ_COMPLETE_IN_TARGET, &request->flags);
+		break;
 
-				/* This was an SSP / STP / SATA transfer.
-				 * There is a possibility that less data than
-				 * the maximum was transferred.
-				 */
-				u32 transferred_length = sci_req_tx_bytes(request);
+	case SCI_FAILURE_RETRY_REQUIRED:
 
-				task->task_status.residual
-					= task->total_xfer_len - transferred_length;
+		/* Fail the I/O so it can be retried. */
+		response = SAS_TASK_UNDELIVERED;
+		if (!idev)
+			status = SAS_DEVICE_UNKNOWN;
+		else
+			status = SAS_ABORTED_TASK;
 
-				/* If there were residual bytes, call this an
-				 * underrun.
-				 */
-				if (task->task_status.residual != 0)
-					status = SAS_DATA_UNDERRUN;
+		set_bit(IREQ_COMPLETE_IN_TARGET, &request->flags);
+		break;
 
-				dev_dbg(&ihost->pdev->dev,
-					"%s: SCI_IO_SUCCESS_IO_DONE_EARLY %d\n",
-					__func__,
-					status);
 
-			} else
-				dev_dbg(&ihost->pdev->dev,
-					"%s: SCI_IO_SUCCESS\n",
-					__func__);
+	default:
+		/* Catch any otherwise unhandled error codes here. */
+		dev_dbg(&ihost->pdev->dev,
+			"%s: invalid completion code: 0x%x - "
+				"isci_request = %p\n",
+			__func__, completion_status, request);
 
-			break;
+		response = SAS_TASK_UNDELIVERED;
 
-		case SCI_IO_FAILURE_TERMINATED:
-			dev_dbg(&ihost->pdev->dev,
-				"%s: SCI_IO_FAILURE_TERMINATED (%p/%p)\n",
-				__func__,
-				request,
-				task);
+		/* See if the device has been/is being stopped. Note
+		* that we ignore the quiesce state, since we are
+		* concerned about the actual device state.
+		*/
+		if (!idev)
+			status = SAS_DEVICE_UNKNOWN;
+		else
+			status = SAS_ABORTED_TASK;
 
-			/* The request was terminated explicitly.  No handling
-			 * is needed in the SCSI error handler path.
-			 */
+		if (SAS_PROTOCOL_SMP == task->task_proto)
 			set_bit(IREQ_COMPLETE_IN_TARGET, &request->flags);
-			response = SAS_TASK_UNDELIVERED;
-
-			/* See if the device has been/is being stopped. Note
-			 * that we ignore the quiesce state, since we are
-			 * concerned about the actual device state.
-			 */
-			if (!idev)
-				status = SAS_DEVICE_UNKNOWN;
-			else
-				status = SAS_ABORTED_TASK;
-
-			complete_to_host = isci_perform_normal_io_completion;
-			break;
-
-		case SCI_FAILURE_CONTROLLER_SPECIFIC_IO_ERR:
-
-			isci_request_handle_controller_specific_errors(
-				idev, request, task, &response, &status,
-				&complete_to_host);
-
-			break;
-
-		case SCI_IO_FAILURE_REMOTE_DEVICE_RESET_REQUIRED:
-			/* This is a special case, in that the I/O completion
-			 * is telling us that the device needs a reset.
-			 * In order for the device reset condition to be
-			 * noticed, the I/O has to be handled in the error
-			 * handler.  Set the reset flag and cause the
-			 * SCSI error thread to be scheduled.
-			 */
-			spin_lock_irqsave(&task->task_state_lock, task_flags);
-			task->task_state_flags |= SAS_TASK_NEED_DEV_RESET;
-			spin_unlock_irqrestore(&task->task_state_lock, task_flags);
-
-			/* Fail the I/O. */
-			response = SAS_TASK_UNDELIVERED;
-			status = SAM_STAT_TASK_ABORTED;
-
-			complete_to_host = isci_perform_error_io_completion;
+		else
 			clear_bit(IREQ_COMPLETE_IN_TARGET, &request->flags);
-			break;
-
-		case SCI_FAILURE_RETRY_REQUIRED:
-
-			/* Fail the I/O so it can be retried. */
-			response = SAS_TASK_UNDELIVERED;
-			if (!idev)
-				status = SAS_DEVICE_UNKNOWN;
-			else
-				status = SAS_ABORTED_TASK;
-
-			complete_to_host = isci_perform_normal_io_completion;
-			set_bit(IREQ_COMPLETE_IN_TARGET, &request->flags);
-			break;
-
-
-		default:
-			/* Catch any otherwise unhandled error codes here. */
-			dev_dbg(&ihost->pdev->dev,
-				 "%s: invalid completion code: 0x%x - "
-				 "isci_request = %p\n",
-				 __func__, completion_status, request);
-
-			response = SAS_TASK_UNDELIVERED;
-
-			/* See if the device has been/is being stopped. Note
-			 * that we ignore the quiesce state, since we are
-			 * concerned about the actual device state.
-			 */
-			if (!idev)
-				status = SAS_DEVICE_UNKNOWN;
-			else
-				status = SAS_ABORTED_TASK;
-
-			if (SAS_PROTOCOL_SMP == task->task_proto) {
-				set_bit(IREQ_COMPLETE_IN_TARGET, &request->flags);
-				complete_to_host = isci_perform_normal_io_completion;
-			} else {
-				clear_bit(IREQ_COMPLETE_IN_TARGET, &request->flags);
-				complete_to_host = isci_perform_error_io_completion;
-			}
-			break;
-		}
 		break;
 	}
 
@@ -3038,10 +2938,18 @@ static void isci_request_io_request_complete(struct isci_host *ihost,
 		break;
 	}
 
-	/* Put the completed request on the correct list */
-	isci_task_save_for_upper_layer_completion(ihost, request, response,
-						  status, complete_to_host
-						  );
+	spin_lock_irqsave(&task->task_state_lock, task_flags);
+
+	task->task_status.resp = response;
+	task->task_status.stat = status;
+
+	if (test_bit(IREQ_COMPLETE_IN_TARGET, &request->flags)) {
+		/* Normal notification (task_done) */
+		task->task_state_flags |= SAS_TASK_STATE_DONE;
+		task->task_state_flags &= ~(SAS_TASK_AT_INITIATOR |
+					    SAS_TASK_STATE_PENDING);
+	}
+	spin_unlock_irqrestore(&task->task_state_lock, task_flags);
 
 	/* complete the io request to the core. */
 	sci_controller_complete_io(ihost, request->target_device, request);
@@ -3051,6 +2959,8 @@ static void isci_request_io_request_complete(struct isci_host *ihost,
 	 * task to recognize the already completed case.
 	 */
 	set_bit(IREQ_TERMINATED, &request->flags);
+
+	ireq_done(ihost, request, task);
 }
 
 static void sci_request_started_state_enter(struct sci_base_state_machine *sm)
@@ -3169,7 +3079,7 @@ sci_general_request_construct(struct isci_host *ihost,
 	sci_init_sm(&ireq->sm, sci_request_state_table, SCI_REQ_INIT);
 
 	ireq->target_device = idev;
-	ireq->protocol = SCIC_NO_PROTOCOL;
+	ireq->protocol = SAS_PROTOCOL_NONE;
 	ireq->saved_rx_frame_index = SCU_INVALID_FRAME_INDEX;
 
 	ireq->sci_status   = SCI_SUCCESS;
@@ -3193,7 +3103,7 @@ sci_io_request_construct(struct isci_host *ihost,
 
 	if (dev->dev_type == SAS_END_DEV)
 		/* pass */;
-	else if (dev->dev_type == SATA_DEV || (dev->tproto & SAS_PROTOCOL_STP))
+	else if (dev_is_sata(dev))
 		memset(&ireq->stp.cmd, 0, sizeof(ireq->stp.cmd));
 	else if (dev_is_expander(dev))
 		/* pass */;
@@ -3215,10 +3125,15 @@ enum sci_status sci_task_request_construct(struct isci_host *ihost,
 	/* Build the common part of the request */
 	sci_general_request_construct(ihost, idev, ireq);
 
-	if (dev->dev_type == SAS_END_DEV ||
-	    dev->dev_type == SATA_DEV || (dev->tproto & SAS_PROTOCOL_STP)) {
+	if (dev->dev_type == SAS_END_DEV || dev_is_sata(dev)) {
 		set_bit(IREQ_TMF, &ireq->flags);
 		memset(ireq->tc, 0, sizeof(struct scu_task_context));
+
+		/* Set the protocol indicator. */
+		if (dev_is_sata(dev))
+			ireq->protocol = SAS_PROTOCOL_STP;
+		else
+			ireq->protocol = SAS_PROTOCOL_SSP;
 	} else
 		status = SCI_FAILURE_UNSUPPORTED_PROTOCOL;
 
@@ -3311,7 +3226,7 @@ sci_io_request_construct_smp(struct device *dev,
 	if (!dma_map_sg(dev, sg, 1, DMA_TO_DEVICE))
 		return SCI_FAILURE;
 
-	ireq->protocol = SCIC_SMP_PROTOCOL;
+	ireq->protocol = SAS_PROTOCOL_SMP;
 
 	/* byte swap the smp request. */
 
@@ -3496,9 +3411,6 @@ static struct isci_request *isci_request_from_tag(struct isci_host *ihost, u16 t
 	ireq->io_request_completion = NULL;
 	ireq->flags = 0;
 	ireq->num_sg_entries = 0;
-	INIT_LIST_HEAD(&ireq->completed_node);
-	INIT_LIST_HEAD(&ireq->dev_node);
-	isci_request_change_state(ireq, allocated);
 
 	return ireq;
 }
@@ -3582,26 +3494,15 @@ int isci_request_execute(struct isci_host *ihost, struct isci_remote_device *ide
 		spin_unlock_irqrestore(&ihost->scic_lock, flags);
 		return status;
 	}
-
 	/* Either I/O started OK, or the core has signaled that
 	 * the device needs a target reset.
-	 *
-	 * In either case, hold onto the I/O for later.
-	 *
-	 * Update it's status and add it to the list in the
-	 * remote device object.
 	 */
-	list_add(&ireq->dev_node, &idev->reqs_in_process);
-
-	if (status == SCI_SUCCESS) {
-		isci_request_change_state(ireq, started);
-	} else {
+	if (status != SCI_SUCCESS) {
 		/* The request did not really start in the
 		 * hardware, so clear the request handle
 		 * here so no terminations will be done.
 		 */
 		set_bit(IREQ_TERMINATED, &ireq->flags);
-		isci_request_change_state(ireq, completed);
 	}
 	spin_unlock_irqrestore(&ihost->scic_lock, flags);
 
diff --git a/drivers/scsi/isci/request.h b/drivers/scsi/isci/request.h
index 057f2378452..aff95317fcf 100644
--- a/drivers/scsi/isci/request.h
+++ b/drivers/scsi/isci/request.h
@@ -61,30 +61,6 @@
 #include "scu_task_context.h"
 
 /**
- * struct isci_request_status - This enum defines the possible states of an I/O
- *    request.
- *
- *
- */
-enum isci_request_status {
-	unallocated = 0x00,
-	allocated   = 0x01,
-	started     = 0x02,
-	completed   = 0x03,
-	aborting    = 0x04,
-	aborted     = 0x05,
-	terminating = 0x06,
-	dead        = 0x07
-};
-
-enum sci_request_protocol {
-	SCIC_NO_PROTOCOL,
-	SCIC_SMP_PROTOCOL,
-	SCIC_SSP_PROTOCOL,
-	SCIC_STP_PROTOCOL
-}; /* XXX remove me, use sas_task.{dev|task_proto} instead */;
-
-/**
  * isci_stp_request - extra request infrastructure to handle pio/atapi protocol
  * @pio_len - number of bytes requested at PIO setup
  * @status - pio setup ending status value to tell us if we need
@@ -104,11 +80,14 @@ struct isci_stp_request {
 };
 
 struct isci_request {
-	enum isci_request_status status;
 	#define IREQ_COMPLETE_IN_TARGET 0
 	#define IREQ_TERMINATED 1
 	#define IREQ_TMF 2
 	#define IREQ_ACTIVE 3
+	#define IREQ_PENDING_ABORT 4 /* Set == device was not suspended yet */
+	#define IREQ_TC_ABORT_POSTED 5
+	#define IREQ_ABORT_PATH_ACTIVE 6
+	#define IREQ_NO_AUTO_FREE_TAG 7 /* Set when being explicitly managed */
 	unsigned long flags;
 	/* XXX kill ttype and ttype_ptr, allocate full sas_task */
 	union ttype_ptr_union {
@@ -116,11 +95,6 @@ struct isci_request {
 		struct isci_tmf *tmf_task_ptr;  /* When ttype==tmf_task */
 	} ttype_ptr;
 	struct isci_host *isci_host;
-	/* For use in the requests_to_{complete|abort} lists: */
-	struct list_head completed_node;
-	/* For use in the reqs_in_process list: */
-	struct list_head dev_node;
-	spinlock_t state_lock;
 	dma_addr_t request_daddr;
 	dma_addr_t zero_scatter_daddr;
 	unsigned int num_sg_entries;
@@ -140,7 +114,7 @@ struct isci_request {
 	struct isci_host *owning_controller;
 	struct isci_remote_device *target_device;
 	u16 io_tag;
-	enum sci_request_protocol protocol;
+	enum sas_protocol protocol;
 	u32 scu_status; /* hardware result */
 	u32 sci_status; /* upper layer disposition */
 	u32 post_context;
@@ -309,92 +283,6 @@ sci_io_request_get_dma_addr(struct isci_request *ireq, void *virt_addr)
 	return ireq->request_daddr + (requested_addr - base_addr);
 }
 
-/**
- * isci_request_change_state() - This function sets the status of the request
- *    object.
- * @request: This parameter points to the isci_request object
- * @status: This Parameter is the new status of the object
- *
- */
-static inline enum isci_request_status
-isci_request_change_state(struct isci_request *isci_request,
-			  enum isci_request_status status)
-{
-	enum isci_request_status old_state;
-	unsigned long flags;
-
-	dev_dbg(&isci_request->isci_host->pdev->dev,
-		"%s: isci_request = %p, state = 0x%x\n",
-		__func__,
-		isci_request,
-		status);
-
-	BUG_ON(isci_request == NULL);
-
-	spin_lock_irqsave(&isci_request->state_lock, flags);
-	old_state = isci_request->status;
-	isci_request->status = status;
-	spin_unlock_irqrestore(&isci_request->state_lock, flags);
-
-	return old_state;
-}
-
-/**
- * isci_request_change_started_to_newstate() - This function sets the status of
- *    the request object.
- * @request: This parameter points to the isci_request object
- * @status: This Parameter is the new status of the object
- *
- * state previous to any change.
- */
-static inline enum isci_request_status
-isci_request_change_started_to_newstate(struct isci_request *isci_request,
-					struct completion *completion_ptr,
-					enum isci_request_status newstate)
-{
-	enum isci_request_status old_state;
-	unsigned long flags;
-
-	spin_lock_irqsave(&isci_request->state_lock, flags);
-
-	old_state = isci_request->status;
-
-	if (old_state == started || old_state == aborting) {
-		BUG_ON(isci_request->io_request_completion != NULL);
-
-		isci_request->io_request_completion = completion_ptr;
-		isci_request->status = newstate;
-	}
-
-	spin_unlock_irqrestore(&isci_request->state_lock, flags);
-
-	dev_dbg(&isci_request->isci_host->pdev->dev,
-		"%s: isci_request = %p, old_state = 0x%x\n",
-		__func__,
-		isci_request,
-		old_state);
-
-	return old_state;
-}
-
-/**
- * isci_request_change_started_to_aborted() - This function sets the status of
- *    the request object.
- * @request: This parameter points to the isci_request object
- * @completion_ptr: This parameter is saved as the kernel completion structure
- *    signalled when the old request completes.
- *
- * state previous to any change.
- */
-static inline enum isci_request_status
-isci_request_change_started_to_aborted(struct isci_request *isci_request,
-				       struct completion *completion_ptr)
-{
-	return isci_request_change_started_to_newstate(isci_request,
-						       completion_ptr,
-						       aborted);
-}
-
 #define isci_request_access_task(req) ((req)->ttype_ptr.io_task_ptr)
 
 #define isci_request_access_tmf(req) ((req)->ttype_ptr.tmf_task_ptr)
@@ -404,8 +292,6 @@ struct isci_request *isci_tmf_request_from_tag(struct isci_host *ihost,
 					       u16 tag);
 int isci_request_execute(struct isci_host *ihost, struct isci_remote_device *idev,
 			 struct sas_task *task, u16 tag);
-void isci_terminate_pending_requests(struct isci_host *ihost,
-				     struct isci_remote_device *idev);
 enum sci_status
 sci_task_request_construct(struct isci_host *ihost,
 			    struct isci_remote_device *idev,
@@ -421,5 +307,4 @@ static inline int isci_task_is_ncq_recovery(struct sas_task *task)
 		task->ata_task.fis.lbal == ATA_LOG_SATA_NCQ);
 
 }
-
 #endif /* !defined(_ISCI_REQUEST_H_) */
diff --git a/drivers/scsi/isci/scu_completion_codes.h b/drivers/scsi/isci/scu_completion_codes.h
index c8b329c695f..071cb74a211 100644
--- a/drivers/scsi/isci/scu_completion_codes.h
+++ b/drivers/scsi/isci/scu_completion_codes.h
@@ -224,6 +224,7 @@
  * 32-bit value like we want, each immediate value must be cast to a u32.
  */
 #define SCU_TASK_DONE_GOOD                                  ((u32)0x00)
+#define SCU_TASK_DONE_TX_RAW_CMD_ERR                        ((u32)0x08)
 #define SCU_TASK_DONE_CRC_ERR                               ((u32)0x14)
 #define SCU_TASK_DONE_CHECK_RESPONSE                        ((u32)0x14)
 #define SCU_TASK_DONE_GEN_RESPONSE                          ((u32)0x15)
@@ -237,6 +238,7 @@
 #define SCU_TASK_DONE_LL_LF_TERM                            ((u32)0x1A)
 #define SCU_TASK_DONE_DATA_LEN_ERR                          ((u32)0x1A)
 #define SCU_TASK_DONE_LL_CL_TERM                            ((u32)0x1B)
+#define SCU_TASK_DONE_BREAK_RCVD                            ((u32)0x1B)
 #define SCU_TASK_DONE_LL_ABORT_ERR                          ((u32)0x1B)
 #define SCU_TASK_DONE_SEQ_INV_TYPE                          ((u32)0x1C)
 #define SCU_TASK_DONE_UNEXP_XR                              ((u32)0x1C)
diff --git a/drivers/scsi/isci/task.c b/drivers/scsi/isci/task.c
index 374254ede9d..6bc74eb012c 100644
--- a/drivers/scsi/isci/task.c
+++ b/drivers/scsi/isci/task.c
@@ -78,54 +78,25 @@ static void isci_task_refuse(struct isci_host *ihost, struct sas_task *task,
 			     enum exec_status status)
 
 {
-	enum isci_completion_selection disposition;
+	unsigned long flags;
 
-	disposition = isci_perform_normal_io_completion;
-	disposition = isci_task_set_completion_status(task, response, status,
-						      disposition);
+	/* Normal notification (task_done) */
+	dev_dbg(&ihost->pdev->dev, "%s: task = %p, response=%d, status=%d\n",
+		__func__, task, response, status);
 
-	/* Tasks aborted specifically by a call to the lldd_abort_task
-	 * function should not be completed to the host in the regular path.
-	 */
-	switch (disposition) {
-	case isci_perform_normal_io_completion:
-		/* Normal notification (task_done) */
-		dev_dbg(&ihost->pdev->dev,
-			"%s: Normal - task = %p, response=%d, "
-			"status=%d\n",
-			__func__, task, response, status);
-
-		task->lldd_task = NULL;
-		task->task_done(task);
-		break;
-
-	case isci_perform_aborted_io_completion:
-		/*
-		 * No notification because this request is already in the
-		 * abort path.
-		 */
-		dev_dbg(&ihost->pdev->dev,
-			"%s: Aborted - task = %p, response=%d, "
-			"status=%d\n",
-			__func__, task, response, status);
-		break;
+	spin_lock_irqsave(&task->task_state_lock, flags);
 
-	case isci_perform_error_io_completion:
-		/* Use sas_task_abort */
-		dev_dbg(&ihost->pdev->dev,
-			"%s: Error - task = %p, response=%d, "
-			"status=%d\n",
-			__func__, task, response, status);
-		sas_task_abort(task);
-		break;
+	task->task_status.resp = response;
+	task->task_status.stat = status;
 
-	default:
-		dev_dbg(&ihost->pdev->dev,
-			"%s: isci task notification default case!",
-			__func__);
-		sas_task_abort(task);
-		break;
-	}
+	/* Normal notification (task_done) */
+	task->task_state_flags |= SAS_TASK_STATE_DONE;
+	task->task_state_flags &= ~(SAS_TASK_AT_INITIATOR |
+				    SAS_TASK_STATE_PENDING);
+	task->lldd_task = NULL;
+	spin_unlock_irqrestore(&task->task_state_lock, flags);
+
+	task->task_done(task);
 }
 
 #define for_each_sas_task(num, task) \
@@ -289,60 +260,6 @@ static struct isci_request *isci_task_request_build(struct isci_host *ihost,
 	return ireq;
 }
 
-/**
-* isci_request_mark_zombie() - This function must be called with scic_lock held.
-*/
-static void isci_request_mark_zombie(struct isci_host *ihost, struct isci_request *ireq)
-{
-	struct completion *tmf_completion = NULL;
-	struct completion *req_completion;
-
-	/* Set the request state to "dead". */
-	ireq->status = dead;
-
-	req_completion = ireq->io_request_completion;
-	ireq->io_request_completion = NULL;
-
-	if (test_bit(IREQ_TMF, &ireq->flags)) {
-		/* Break links with the TMF request. */
-		struct isci_tmf *tmf = isci_request_access_tmf(ireq);
-
-		/* In the case where a task request is dying,
-		 * the thread waiting on the complete will sit and
-		 * timeout unless we wake it now.  Since the TMF
-		 * has a default error status, complete it here
-		 * to wake the waiting thread.
-		 */
-		if (tmf) {
-			tmf_completion = tmf->complete;
-			tmf->complete = NULL;
-		}
-		ireq->ttype_ptr.tmf_task_ptr = NULL;
-		dev_dbg(&ihost->pdev->dev, "%s: tmf_code %d, managed tag %#x\n",
-			__func__, tmf->tmf_code, tmf->io_tag);
-	} else {
-		/* Break links with the sas_task - the callback is done
-		 * elsewhere.
-		 */
-		struct sas_task *task = isci_request_access_task(ireq);
-
-		if (task)
-			task->lldd_task = NULL;
-
-		ireq->ttype_ptr.io_task_ptr = NULL;
-	}
-
-	dev_warn(&ihost->pdev->dev, "task context unrecoverable (tag: %#x)\n",
-		 ireq->io_tag);
-
-	/* Don't force waiting threads to timeout. */
-	if (req_completion)
-		complete(req_completion);
-
-	if (tmf_completion != NULL)
-		complete(tmf_completion);
-}
-
 static int isci_task_execute_tmf(struct isci_host *ihost,
 				 struct isci_remote_device *idev,
 				 struct isci_tmf *tmf, unsigned long timeout_ms)
@@ -400,17 +317,11 @@ static int isci_task_execute_tmf(struct isci_host *ihost,
 		spin_unlock_irqrestore(&ihost->scic_lock, flags);
 		goto err_tci;
 	}
-
-	if (tmf->cb_state_func != NULL)
-		tmf->cb_state_func(isci_tmf_started, tmf, tmf->cb_data);
-
-	isci_request_change_state(ireq, started);
-
-	/* add the request to the remote device request list. */
-	list_add(&ireq->dev_node, &idev->reqs_in_process);
-
 	spin_unlock_irqrestore(&ihost->scic_lock, flags);
 
+	/* The RNC must be unsuspended before the TMF can get a response. */
+	isci_remote_device_resume_from_abort(ihost, idev);
+
 	/* Wait for the TMF to complete, or a timeout. */
 	timeleft = wait_for_completion_timeout(&completion,
 					       msecs_to_jiffies(timeout_ms));
@@ -419,32 +330,7 @@ static int isci_task_execute_tmf(struct isci_host *ihost,
 		/* The TMF did not complete - this could be because
 		 * of an unplug.  Terminate the TMF request now.
 		 */
-		spin_lock_irqsave(&ihost->scic_lock, flags);
-
-		if (tmf->cb_state_func != NULL)
-			tmf->cb_state_func(isci_tmf_timed_out, tmf,
-					   tmf->cb_data);
-
-		sci_controller_terminate_request(ihost, idev, ireq);
-
-		spin_unlock_irqrestore(&ihost->scic_lock, flags);
-
-		timeleft = wait_for_completion_timeout(
-			&completion,
-			msecs_to_jiffies(ISCI_TERMINATION_TIMEOUT_MSEC));
-
-		if (!timeleft) {
-			/* Strange condition - the termination of the TMF
-			 * request timed-out.
-			 */
-			spin_lock_irqsave(&ihost->scic_lock, flags);
-
-			/* If the TMF status has not changed, kill it. */
-			if (tmf->status == SCI_FAILURE_TIMEOUT)
-				isci_request_mark_zombie(ihost, ireq);
-
-			spin_unlock_irqrestore(&ihost->scic_lock, flags);
-		}
+		isci_remote_device_suspend_terminate(ihost, idev, ireq);
 	}
 
 	isci_print_tmf(ihost, tmf);
@@ -476,315 +362,21 @@ static int isci_task_execute_tmf(struct isci_host *ihost,
 }
 
 static void isci_task_build_tmf(struct isci_tmf *tmf,
-				enum isci_tmf_function_codes code,
-				void (*tmf_sent_cb)(enum isci_tmf_cb_state,
-						    struct isci_tmf *,
-						    void *),
-				void *cb_data)
+				enum isci_tmf_function_codes code)
 {
 	memset(tmf, 0, sizeof(*tmf));
-
-	tmf->tmf_code      = code;
-	tmf->cb_state_func = tmf_sent_cb;
-	tmf->cb_data       = cb_data;
+	tmf->tmf_code = code;
 }
 
 static void isci_task_build_abort_task_tmf(struct isci_tmf *tmf,
 					   enum isci_tmf_function_codes code,
-					   void (*tmf_sent_cb)(enum isci_tmf_cb_state,
-							       struct isci_tmf *,
-							       void *),
 					   struct isci_request *old_request)
 {
-	isci_task_build_tmf(tmf, code, tmf_sent_cb, old_request);
+	isci_task_build_tmf(tmf, code);
 	tmf->io_tag = old_request->io_tag;
 }
 
 /**
- * isci_task_validate_request_to_abort() - This function checks the given I/O
- *    against the "started" state.  If the request is still "started", it's
- *    state is changed to aborted. NOTE: isci_host->scic_lock MUST BE HELD
- *    BEFORE CALLING THIS FUNCTION.
- * @isci_request: This parameter specifies the request object to control.
- * @isci_host: This parameter specifies the ISCI host object
- * @isci_device: This is the device to which the request is pending.
- * @aborted_io_completion: This is a completion structure that will be added to
- *    the request in case it is changed to aborting; this completion is
- *    triggered when the request is fully completed.
- *
- * Either "started" on successful change of the task status to "aborted", or
- * "unallocated" if the task cannot be controlled.
- */
-static enum isci_request_status isci_task_validate_request_to_abort(
-	struct isci_request *isci_request,
-	struct isci_host *isci_host,
-	struct isci_remote_device *isci_device,
-	struct completion *aborted_io_completion)
-{
-	enum isci_request_status old_state = unallocated;
-
-	/* Only abort the task if it's in the
-	 *  device's request_in_process list
-	 */
-	if (isci_request && !list_empty(&isci_request->dev_node)) {
-		old_state = isci_request_change_started_to_aborted(
-			isci_request, aborted_io_completion);
-
-	}
-
-	return old_state;
-}
-
-static int isci_request_is_dealloc_managed(enum isci_request_status stat)
-{
-	switch (stat) {
-	case aborted:
-	case aborting:
-	case terminating:
-	case completed:
-	case dead:
-		return true;
-	default:
-		return false;
-	}
-}
-
-/**
- * isci_terminate_request_core() - This function will terminate the given
- *    request, and wait for it to complete.  This function must only be called
- *    from a thread that can wait.  Note that the request is terminated and
- *    completed (back to the host, if started there).
- * @ihost: This SCU.
- * @idev: The target.
- * @isci_request: The I/O request to be terminated.
- *
- */
-static void isci_terminate_request_core(struct isci_host *ihost,
-					struct isci_remote_device *idev,
-					struct isci_request *isci_request)
-{
-	enum sci_status status      = SCI_SUCCESS;
-	bool was_terminated         = false;
-	bool needs_cleanup_handling = false;
-	unsigned long     flags;
-	unsigned long     termination_completed = 1;
-	struct completion *io_request_completion;
-
-	dev_dbg(&ihost->pdev->dev,
-		"%s: device = %p; request = %p\n",
-		__func__, idev, isci_request);
-
-	spin_lock_irqsave(&ihost->scic_lock, flags);
-
-	io_request_completion = isci_request->io_request_completion;
-
-	/* Note that we are not going to control
-	 * the target to abort the request.
-	 */
-	set_bit(IREQ_COMPLETE_IN_TARGET, &isci_request->flags);
-
-	/* Make sure the request wasn't just sitting around signalling
-	 * device condition (if the request handle is NULL, then the
-	 * request completed but needed additional handling here).
-	 */
-	if (!test_bit(IREQ_TERMINATED, &isci_request->flags)) {
-		was_terminated = true;
-		needs_cleanup_handling = true;
-		status = sci_controller_terminate_request(ihost,
-							   idev,
-							   isci_request);
-	}
-	spin_unlock_irqrestore(&ihost->scic_lock, flags);
-
-	/*
-	 * The only time the request to terminate will
-	 * fail is when the io request is completed and
-	 * being aborted.
-	 */
-	if (status != SCI_SUCCESS) {
-		dev_dbg(&ihost->pdev->dev,
-			"%s: sci_controller_terminate_request"
-			" returned = 0x%x\n",
-			__func__, status);
-
-		isci_request->io_request_completion = NULL;
-
-	} else {
-		if (was_terminated) {
-			dev_dbg(&ihost->pdev->dev,
-				"%s: before completion wait (%p/%p)\n",
-				__func__, isci_request, io_request_completion);
-
-			/* Wait here for the request to complete. */
-			termination_completed
-				= wait_for_completion_timeout(
-				   io_request_completion,
-				   msecs_to_jiffies(ISCI_TERMINATION_TIMEOUT_MSEC));
-
-			if (!termination_completed) {
-
-				/* The request to terminate has timed out.  */
-				spin_lock_irqsave(&ihost->scic_lock, flags);
-
-				/* Check for state changes. */
-				if (!test_bit(IREQ_TERMINATED,
-					      &isci_request->flags)) {
-
-					/* The best we can do is to have the
-					 * request die a silent death if it
-					 * ever really completes.
-					 */
-					isci_request_mark_zombie(ihost,
-								 isci_request);
-					needs_cleanup_handling = true;
-				} else
-					termination_completed = 1;
-
-				spin_unlock_irqrestore(&ihost->scic_lock,
-						       flags);
-
-				if (!termination_completed) {
-
-					dev_dbg(&ihost->pdev->dev,
-						"%s: *** Timeout waiting for "
-						"termination(%p/%p)\n",
-						__func__, io_request_completion,
-						isci_request);
-
-					/* The request can no longer be referenced
-					 * safely since it may go away if the
-					 * termination every really does complete.
-					 */
-					isci_request = NULL;
-				}
-			}
-			if (termination_completed)
-				dev_dbg(&ihost->pdev->dev,
-					"%s: after completion wait (%p/%p)\n",
-					__func__, isci_request, io_request_completion);
-		}
-
-		if (termination_completed) {
-
-			isci_request->io_request_completion = NULL;
-
-			/* Peek at the status of the request.  This will tell
-			 * us if there was special handling on the request such that it
-			 * needs to be detached and freed here.
-			 */
-			spin_lock_irqsave(&isci_request->state_lock, flags);
-
-			needs_cleanup_handling
-				= isci_request_is_dealloc_managed(
-					isci_request->status);
-
-			spin_unlock_irqrestore(&isci_request->state_lock, flags);
-
-		}
-		if (needs_cleanup_handling) {
-
-			dev_dbg(&ihost->pdev->dev,
-				"%s: cleanup isci_device=%p, request=%p\n",
-				__func__, idev, isci_request);
-
-			if (isci_request != NULL) {
-				spin_lock_irqsave(&ihost->scic_lock, flags);
-				isci_free_tag(ihost, isci_request->io_tag);
-				isci_request_change_state(isci_request, unallocated);
-				list_del_init(&isci_request->dev_node);
-				spin_unlock_irqrestore(&ihost->scic_lock, flags);
-			}
-		}
-	}
-}
-
-/**
- * isci_terminate_pending_requests() - This function will change the all of the
- *    requests on the given device's state to "aborting", will terminate the
- *    requests, and wait for them to complete.  This function must only be
- *    called from a thread that can wait.  Note that the requests are all
- *    terminated and completed (back to the host, if started there).
- * @isci_host: This parameter specifies SCU.
- * @idev: This parameter specifies the target.
- *
- */
-void isci_terminate_pending_requests(struct isci_host *ihost,
-				     struct isci_remote_device *idev)
-{
-	struct completion request_completion;
-	enum isci_request_status old_state;
-	unsigned long flags;
-	LIST_HEAD(list);
-
-	spin_lock_irqsave(&ihost->scic_lock, flags);
-	list_splice_init(&idev->reqs_in_process, &list);
-
-	/* assumes that isci_terminate_request_core deletes from the list */
-	while (!list_empty(&list)) {
-		struct isci_request *ireq = list_entry(list.next, typeof(*ireq), dev_node);
-
-		/* Change state to "terminating" if it is currently
-		 * "started".
-		 */
-		old_state = isci_request_change_started_to_newstate(ireq,
-								    &request_completion,
-								    terminating);
-		switch (old_state) {
-		case started:
-		case completed:
-		case aborting:
-			break;
-		default:
-			/* termination in progress, or otherwise dispositioned.
-			 * We know the request was on 'list' so should be safe
-			 * to move it back to reqs_in_process
-			 */
-			list_move(&ireq->dev_node, &idev->reqs_in_process);
-			ireq = NULL;
-			break;
-		}
-
-		if (!ireq)
-			continue;
-		spin_unlock_irqrestore(&ihost->scic_lock, flags);
-
-		init_completion(&request_completion);
-
-		dev_dbg(&ihost->pdev->dev,
-			 "%s: idev=%p request=%p; task=%p old_state=%d\n",
-			 __func__, idev, ireq,
-			(!test_bit(IREQ_TMF, &ireq->flags)
-				? isci_request_access_task(ireq)
-				: NULL),
-			old_state);
-
-		/* If the old_state is started:
-		 * This request was not already being aborted. If it had been,
-		 * then the aborting I/O (ie. the TMF request) would not be in
-		 * the aborting state, and thus would be terminated here.  Note
-		 * that since the TMF completion's call to the kernel function
-		 * "complete()" does not happen until the pending I/O request
-		 * terminate fully completes, we do not have to implement a
-		 * special wait here for already aborting requests - the
-		 * termination of the TMF request will force the request
-		 * to finish it's already started terminate.
-		 *
-		 * If old_state == completed:
-		 * This request completed from the SCU hardware perspective
-		 * and now just needs cleaning up in terms of freeing the
-		 * request and potentially calling up to libsas.
-		 *
-		 * If old_state == aborting:
-		 * This request has already gone through a TMF timeout, but may
-		 * not have been terminated; needs cleaning up at least.
-		 */
-		isci_terminate_request_core(ihost, idev, ireq);
-		spin_lock_irqsave(&ihost->scic_lock, flags);
-	}
-	spin_unlock_irqrestore(&ihost->scic_lock, flags);
-}
-
-/**
  * isci_task_send_lu_reset_sas() - This function is called by of the SAS Domain
  *    Template functions.
  * @lun: This parameter specifies the lun to be reset.
@@ -807,7 +399,7 @@ static int isci_task_send_lu_reset_sas(
 	 * value is "TMF_RESP_FUNC_COMPLETE", or the request timed-out (or
 	 * was otherwise unable to be executed ("TMF_RESP_FUNC_FAILED").
 	 */
-	isci_task_build_tmf(&tmf, isci_tmf_ssp_lun_reset, NULL, NULL);
+	isci_task_build_tmf(&tmf, isci_tmf_ssp_lun_reset);
 
 	#define ISCI_LU_RESET_TIMEOUT_MS 2000 /* 2 second timeout. */
 	ret = isci_task_execute_tmf(isci_host, isci_device, &tmf, ISCI_LU_RESET_TIMEOUT_MS);
@@ -826,42 +418,44 @@ static int isci_task_send_lu_reset_sas(
 
 int isci_task_lu_reset(struct domain_device *dev, u8 *lun)
 {
-	struct isci_host *isci_host = dev_to_ihost(dev);
-	struct isci_remote_device *isci_device;
+	struct isci_host *ihost = dev_to_ihost(dev);
+	struct isci_remote_device *idev;
 	unsigned long flags;
-	int ret;
+	int ret = TMF_RESP_FUNC_COMPLETE;
 
-	spin_lock_irqsave(&isci_host->scic_lock, flags);
-	isci_device = isci_lookup_device(dev);
-	spin_unlock_irqrestore(&isci_host->scic_lock, flags);
+	spin_lock_irqsave(&ihost->scic_lock, flags);
+	idev = isci_get_device(dev->lldd_dev);
+	spin_unlock_irqrestore(&ihost->scic_lock, flags);
 
-	dev_dbg(&isci_host->pdev->dev,
+	dev_dbg(&ihost->pdev->dev,
 		"%s: domain_device=%p, isci_host=%p; isci_device=%p\n",
-		 __func__, dev, isci_host, isci_device);
+		__func__, dev, ihost, idev);
 
-	if (!isci_device) {
-		/* If the device is gone, stop the escalations. */
-		dev_dbg(&isci_host->pdev->dev, "%s: No dev\n", __func__);
+	if (!idev) {
+		/* If the device is gone, escalate to I_T_Nexus_Reset. */
+		dev_dbg(&ihost->pdev->dev, "%s: No dev\n", __func__);
 
-		ret = TMF_RESP_FUNC_COMPLETE;
+		ret = TMF_RESP_FUNC_FAILED;
 		goto out;
 	}
 
-	/* Send the task management part of the reset. */
-	if (dev_is_sata(dev)) {
-		sas_ata_schedule_reset(dev);
-		ret = TMF_RESP_FUNC_COMPLETE;
-	} else
-		ret = isci_task_send_lu_reset_sas(isci_host, isci_device, lun);
-
-	/* If the LUN reset worked, all the I/O can now be terminated. */
-	if (ret == TMF_RESP_FUNC_COMPLETE)
-		/* Terminate all I/O now. */
-		isci_terminate_pending_requests(isci_host,
-						isci_device);
-
+	/* Suspend the RNC, kill all TCs */
+	if (isci_remote_device_suspend_terminate(ihost, idev, NULL)
+	    != SCI_SUCCESS) {
+		/* The suspend/terminate only fails if isci_get_device fails */
+		ret = TMF_RESP_FUNC_FAILED;
+		goto out;
+	}
+	/* All pending I/Os have been terminated and cleaned up. */
+	if (!test_bit(IDEV_GONE, &idev->flags)) {
+		if (dev_is_sata(dev))
+			sas_ata_schedule_reset(dev);
+		else
+			/* Send the task management part of the reset. */
+			ret = isci_task_send_lu_reset_sas(ihost, idev, lun);
+	}
  out:
-	isci_put_device(isci_device);
+	isci_put_device(idev);
 	return ret;
 }
 
@@ -882,63 +476,6 @@ int isci_task_clear_nexus_ha(struct sas_ha_struct *ha)
 /* Task Management Functions. Must be called from process context.	 */
 
 /**
- * isci_abort_task_process_cb() - This is a helper function for the abort task
- *    TMF command.  It manages the request state with respect to the successful
- *    transmission / completion of the abort task request.
- * @cb_state: This parameter specifies when this function was called - after
- *    the TMF request has been started and after it has timed-out.
- * @tmf: This parameter specifies the TMF in progress.
- *
- *
- */
-static void isci_abort_task_process_cb(
-	enum isci_tmf_cb_state cb_state,
-	struct isci_tmf *tmf,
-	void *cb_data)
-{
-	struct isci_request *old_request;
-
-	old_request = (struct isci_request *)cb_data;
-
-	dev_dbg(&old_request->isci_host->pdev->dev,
-		"%s: tmf=%p, old_request=%p\n",
-		__func__, tmf, old_request);
-
-	switch (cb_state) {
-
-	case isci_tmf_started:
-		/* The TMF has been started.  Nothing to do here, since the
-		 * request state was already set to "aborted" by the abort
-		 * task function.
-		 */
-		if ((old_request->status != aborted)
-			&& (old_request->status != completed))
-			dev_dbg(&old_request->isci_host->pdev->dev,
-				"%s: Bad request status (%d): tmf=%p, old_request=%p\n",
-				__func__, old_request->status, tmf, old_request);
-		break;
-
-	case isci_tmf_timed_out:
-
-		/* Set the task's state to "aborting", since the abort task
-		 * function thread set it to "aborted" (above) in anticipation
-		 * of the task management request working correctly.  Since the
-		 * timeout has now fired, the TMF request failed.  We set the
-		 * state such that the request completion will indicate the
-		 * device is no longer present.
-		 */
-		isci_request_change_state(old_request, aborting);
-		break;
-
-	default:
-		dev_dbg(&old_request->isci_host->pdev->dev,
-			"%s: Bad cb_state (%d): tmf=%p, old_request=%p\n",
-			__func__, cb_state, tmf, old_request);
-		break;
-	}
-}
-
-/**
  * isci_task_abort_task() - This function is one of the SAS Domain Template
  *    functions. This function is called by libsas to abort a specified task.
  * @task: This parameter specifies the SAS task to abort.
@@ -947,22 +484,20 @@ static void isci_abort_task_process_cb(
  */
 int isci_task_abort_task(struct sas_task *task)
 {
-	struct isci_host *isci_host = dev_to_ihost(task->dev);
+	struct isci_host *ihost = dev_to_ihost(task->dev);
 	DECLARE_COMPLETION_ONSTACK(aborted_io_completion);
 	struct isci_request       *old_request = NULL;
-	enum isci_request_status  old_state;
-	struct isci_remote_device *isci_device = NULL;
+	struct isci_remote_device *idev = NULL;
 	struct isci_tmf           tmf;
 	int                       ret = TMF_RESP_FUNC_FAILED;
 	unsigned long             flags;
-	int                       perform_termination = 0;
 
 	/* Get the isci_request reference from the task.  Note that
 	 * this check does not depend on the pending request list
 	 * in the device, because tasks driving resets may land here
 	 * after completion in the core.
 	 */
-	spin_lock_irqsave(&isci_host->scic_lock, flags);
+	spin_lock_irqsave(&ihost->scic_lock, flags);
 	spin_lock(&task->task_state_lock);
 
 	old_request = task->lldd_task;
@@ -971,20 +506,29 @@ int isci_task_abort_task(struct sas_task *task)
 	if (!(task->task_state_flags & SAS_TASK_STATE_DONE) &&
 	    (task->task_state_flags & SAS_TASK_AT_INITIATOR) &&
 	    old_request)
-		isci_device = isci_lookup_device(task->dev);
+		idev = isci_get_device(task->dev->lldd_dev);
 
 	spin_unlock(&task->task_state_lock);
-	spin_unlock_irqrestore(&isci_host->scic_lock, flags);
+	spin_unlock_irqrestore(&ihost->scic_lock, flags);
 
-	dev_dbg(&isci_host->pdev->dev,
-		"%s: dev = %p, task = %p, old_request == %p\n",
-		__func__, isci_device, task, old_request);
+	dev_warn(&ihost->pdev->dev,
+		 "%s: dev = %p (%s%s), task = %p, old_request == %p\n",
+		 __func__, idev,
+		 (dev_is_sata(task->dev) ? "STP/SATA"
+					 : ((dev_is_expander(task->dev))
+						? "SMP"
+						: "SSP")),
+		 ((idev) ? ((test_bit(IDEV_GONE, &idev->flags))
+			   ? " IDEV_GONE"
+			   : "")
+			 : " <NULL>"),
+		 task, old_request);
 
 	/* Device reset conditions signalled in task_state_flags are the
 	 * responsbility of libsas to observe at the start of the error
 	 * handler thread.
 	 */
-	if (!isci_device || !old_request) {
+	if (!idev || !old_request) {
 		/* The request has already completed and there
 		* is nothing to do here other than to set the task
 		* done bit, and indicate that the task abort function
@@ -998,108 +542,72 @@ int isci_task_abort_task(struct sas_task *task)
 
 		ret = TMF_RESP_FUNC_COMPLETE;
 
-		dev_dbg(&isci_host->pdev->dev,
-			"%s: abort task not needed for %p\n",
-			__func__, task);
+		dev_warn(&ihost->pdev->dev,
+			 "%s: abort task not needed for %p\n",
+			 __func__, task);
 		goto out;
 	}
-
-	spin_lock_irqsave(&isci_host->scic_lock, flags);
-
-	/* Check the request status and change to "aborted" if currently
-	 * "starting"; if true then set the I/O kernel completion
-	 * struct that will be triggered when the request completes.
-	 */
-	old_state = isci_task_validate_request_to_abort(
-				old_request, isci_host, isci_device,
-				&aborted_io_completion);
-	if ((old_state != started) &&
-	    (old_state != completed) &&
-	    (old_state != aborting)) {
-
-		spin_unlock_irqrestore(&isci_host->scic_lock, flags);
-
-		/* The request was already being handled by someone else (because
-		* they got to set the state away from started).
-		*/
-		dev_dbg(&isci_host->pdev->dev,
-			"%s:  device = %p; old_request %p already being aborted\n",
-			__func__,
-			isci_device, old_request);
-		ret = TMF_RESP_FUNC_COMPLETE;
+	/* Suspend the RNC, kill the TC */
+	if (isci_remote_device_suspend_terminate(ihost, idev, old_request)
+	    != SCI_SUCCESS) {
+		dev_warn(&ihost->pdev->dev,
+			 "%s: isci_remote_device_reset_terminate(dev=%p, "
+				 "req=%p, task=%p) failed\n",
+			 __func__, idev, old_request, task);
+		ret = TMF_RESP_FUNC_FAILED;
 		goto out;
 	}
+	spin_lock_irqsave(&ihost->scic_lock, flags);
+
 	if (task->task_proto == SAS_PROTOCOL_SMP ||
 	    sas_protocol_ata(task->task_proto) ||
-	    test_bit(IREQ_COMPLETE_IN_TARGET, &old_request->flags)) {
+	    test_bit(IREQ_COMPLETE_IN_TARGET, &old_request->flags) ||
+	    test_bit(IDEV_GONE, &idev->flags)) {
 
-		spin_unlock_irqrestore(&isci_host->scic_lock, flags);
+		spin_unlock_irqrestore(&ihost->scic_lock, flags);
 
-		dev_dbg(&isci_host->pdev->dev,
-			"%s: %s request"
-			" or complete_in_target (%d), thus no TMF\n",
-			__func__,
-			((task->task_proto == SAS_PROTOCOL_SMP)
-				? "SMP"
-				: (sas_protocol_ata(task->task_proto)
-					? "SATA/STP"
-					: "<other>")
-			 ),
-			test_bit(IREQ_COMPLETE_IN_TARGET, &old_request->flags));
-
-		if (test_bit(IREQ_COMPLETE_IN_TARGET, &old_request->flags)) {
-			spin_lock_irqsave(&task->task_state_lock, flags);
-			task->task_state_flags |= SAS_TASK_STATE_DONE;
-			task->task_state_flags &= ~(SAS_TASK_AT_INITIATOR |
-						    SAS_TASK_STATE_PENDING);
-			spin_unlock_irqrestore(&task->task_state_lock, flags);
-			ret = TMF_RESP_FUNC_COMPLETE;
-		} else {
-			spin_lock_irqsave(&task->task_state_lock, flags);
-			task->task_state_flags &= ~(SAS_TASK_AT_INITIATOR |
-						    SAS_TASK_STATE_PENDING);
-			spin_unlock_irqrestore(&task->task_state_lock, flags);
-		}
+		/* No task to send, so explicitly resume the device here */
+		isci_remote_device_resume_from_abort(ihost, idev);
 
-		/* STP and SMP devices are not sent a TMF, but the
-		 * outstanding I/O request is terminated below.  This is
-		 * because SATA/STP and SMP discovery path timeouts directly
-		 * call the abort task interface for cleanup.
-		 */
-		perform_termination = 1;
+		dev_warn(&ihost->pdev->dev,
+			 "%s: %s request"
+				 " or complete_in_target (%d), "
+				 "or IDEV_GONE (%d), thus no TMF\n",
+			 __func__,
+			 ((task->task_proto == SAS_PROTOCOL_SMP)
+			  ? "SMP"
+			  : (sas_protocol_ata(task->task_proto)
+				? "SATA/STP"
+				: "<other>")
+			  ),
+			 test_bit(IREQ_COMPLETE_IN_TARGET,
+				  &old_request->flags),
+			 test_bit(IDEV_GONE, &idev->flags));
+
+		spin_lock_irqsave(&task->task_state_lock, flags);
+		task->task_state_flags &= ~(SAS_TASK_AT_INITIATOR |
+					    SAS_TASK_STATE_PENDING);
+		task->task_state_flags |= SAS_TASK_STATE_DONE;
+		spin_unlock_irqrestore(&task->task_state_lock, flags);
 
+		ret = TMF_RESP_FUNC_COMPLETE;
 	} else {
 		/* Fill in the tmf stucture */
 		isci_task_build_abort_task_tmf(&tmf, isci_tmf_ssp_task_abort,
-					       isci_abort_task_process_cb,
 					       old_request);
 
-		spin_unlock_irqrestore(&isci_host->scic_lock, flags);
+		spin_unlock_irqrestore(&ihost->scic_lock, flags);
 
+		/* Send the task management request. */
 		#define ISCI_ABORT_TASK_TIMEOUT_MS 500 /* 1/2 second timeout */
-		ret = isci_task_execute_tmf(isci_host, isci_device, &tmf,
+		ret = isci_task_execute_tmf(ihost, idev, &tmf,
 					    ISCI_ABORT_TASK_TIMEOUT_MS);
-
-		if (ret == TMF_RESP_FUNC_COMPLETE)
-			perform_termination = 1;
-		else
-			dev_dbg(&isci_host->pdev->dev,
-				"%s: isci_task_send_tmf failed\n", __func__);
 	}
-	if (perform_termination) {
-		set_bit(IREQ_COMPLETE_IN_TARGET, &old_request->flags);
-
-		/* Clean up the request on our side, and wait for the aborted
-		 * I/O to complete.
-		 */
-		isci_terminate_request_core(isci_host, isci_device,
-					    old_request);
-	}
-
-	/* Make sure we do not leave a reference to aborted_io_completion */
-	old_request->io_request_completion = NULL;
- out:
-	isci_put_device(isci_device);
+out:
+	dev_warn(&ihost->pdev->dev,
+		 "%s: Done; dev = %p, task = %p , old_request == %p\n",
+		 __func__, idev, task, old_request);
+	isci_put_device(idev);
 	return ret;
 }
 
@@ -1195,14 +703,11 @@ isci_task_request_complete(struct isci_host *ihost,
 {
 	struct isci_tmf *tmf = isci_request_access_tmf(ireq);
 	struct completion *tmf_complete = NULL;
-	struct completion *request_complete = ireq->io_request_completion;
 
 	dev_dbg(&ihost->pdev->dev,
 		"%s: request = %p, status=%d\n",
 		__func__, ireq, completion_status);
 
-	isci_request_change_state(ireq, completed);
-
 	set_bit(IREQ_COMPLETE_IN_TARGET, &ireq->flags);
 
 	if (tmf) {
@@ -1226,20 +731,11 @@ isci_task_request_complete(struct isci_host *ihost,
 	 */
 	set_bit(IREQ_TERMINATED, &ireq->flags);
 
-	/* As soon as something is in the terminate path, deallocation is
-	 * managed there.  Note that the final non-managed state of a task
-	 * request is "completed".
-	 */
-	if ((ireq->status == completed) ||
-	    !isci_request_is_dealloc_managed(ireq->status)) {
-		isci_request_change_state(ireq, unallocated);
-		isci_free_tag(ihost, ireq->io_tag);
-		list_del_init(&ireq->dev_node);
-	}
+	if (test_and_clear_bit(IREQ_ABORT_PATH_ACTIVE, &ireq->flags))
+		wake_up_all(&ihost->eventq);
 
-	/* "request_complete" is set if the task was being terminated. */
-	if (request_complete)
-		complete(request_complete);
+	if (!test_bit(IREQ_NO_AUTO_FREE_TAG, &ireq->flags))
+		isci_free_tag(ihost, ireq->io_tag);
 
 	/* The task management part completes last. */
 	if (tmf_complete)
@@ -1250,48 +746,38 @@ static int isci_reset_device(struct isci_host *ihost,
 			     struct domain_device *dev,
 			     struct isci_remote_device *idev)
 {
-	int rc;
-	unsigned long flags;
-	enum sci_status status;
+	int rc = TMF_RESP_FUNC_COMPLETE, reset_stat = -1;
 	struct sas_phy *phy = sas_get_local_phy(dev);
 	struct isci_port *iport = dev->port->lldd_port;
 
 	dev_dbg(&ihost->pdev->dev, "%s: idev %p\n", __func__, idev);
 
-	spin_lock_irqsave(&ihost->scic_lock, flags);
-	status = sci_remote_device_reset(idev);
-	spin_unlock_irqrestore(&ihost->scic_lock, flags);
-
-	if (status != SCI_SUCCESS) {
-		dev_dbg(&ihost->pdev->dev,
-			 "%s: sci_remote_device_reset(%p) returned %d!\n",
-			 __func__, idev, status);
+	/* Suspend the RNC, terminate all outstanding TCs. */
+	if (isci_remote_device_suspend_terminate(ihost, idev, NULL)
+	    != SCI_SUCCESS) {
 		rc = TMF_RESP_FUNC_FAILED;
 		goto out;
 	}
-
-	if (scsi_is_sas_phy_local(phy)) {
-		struct isci_phy *iphy = &ihost->phys[phy->number];
-
-		rc = isci_port_perform_hard_reset(ihost, iport, iphy);
-	} else
-		rc = sas_phy_reset(phy, !dev_is_sata(dev));
-
-	/* Terminate in-progress I/O now. */
-	isci_remote_device_nuke_requests(ihost, idev);
-
-	/* Since all pending TCs have been cleaned, resume the RNC. */
-	spin_lock_irqsave(&ihost->scic_lock, flags);
-	status = sci_remote_device_reset_complete(idev);
-	spin_unlock_irqrestore(&ihost->scic_lock, flags);
-
-	if (status != SCI_SUCCESS) {
-		dev_dbg(&ihost->pdev->dev,
-			 "%s: sci_remote_device_reset_complete(%p) "
-			 "returned %d!\n", __func__, idev, status);
+	/* Note that since the termination for outstanding requests succeeded,
+	 * this function will return success.  This is because the resets will
+	 * only fail if the device has been removed (ie. hotplug), and the
+	 * primary duty of this function is to cleanup tasks, so that is the
+	 * relevant status.
+	 */
+	if (!test_bit(IDEV_GONE, &idev->flags)) {
+		if (scsi_is_sas_phy_local(phy)) {
+			struct isci_phy *iphy = &ihost->phys[phy->number];
+
+			reset_stat = isci_port_perform_hard_reset(ihost, iport,
+								  iphy);
+		} else
+			reset_stat = sas_phy_reset(phy, !dev_is_sata(dev));
 	}
+	/* Explicitly resume the RNC here, since there was no task sent. */
+	isci_remote_device_resume_from_abort(ihost, idev);
 
-	dev_dbg(&ihost->pdev->dev, "%s: idev %p complete.\n", __func__, idev);
+	dev_dbg(&ihost->pdev->dev, "%s: idev %p complete, reset_stat=%d.\n",
+		__func__, idev, reset_stat);
  out:
 	sas_put_local_phy(phy);
 	return rc;
@@ -1305,7 +791,7 @@ int isci_task_I_T_nexus_reset(struct domain_device *dev)
 	int ret;
 
 	spin_lock_irqsave(&ihost->scic_lock, flags);
-	idev = isci_lookup_device(dev);
+	idev = isci_get_device(dev->lldd_dev);
 	spin_unlock_irqrestore(&ihost->scic_lock, flags);
 
 	if (!idev) {
diff --git a/drivers/scsi/isci/task.h b/drivers/scsi/isci/task.h
index 7b6d0e32fd9..9c06cbad1d2 100644
--- a/drivers/scsi/isci/task.h
+++ b/drivers/scsi/isci/task.h
@@ -63,19 +63,6 @@
 struct isci_request;
 
 /**
- * enum isci_tmf_cb_state - This enum defines the possible states in which the
- *    TMF callback function is invoked during the TMF execution process.
- *
- *
- */
-enum isci_tmf_cb_state {
-
-	isci_tmf_init_state = 0,
-	isci_tmf_started,
-	isci_tmf_timed_out
-};
-
-/**
  * enum isci_tmf_function_codes - This enum defines the possible preparations
  *    of task management requests.
  *
@@ -87,6 +74,7 @@ enum isci_tmf_function_codes {
 	isci_tmf_ssp_task_abort = TMF_ABORT_TASK,
 	isci_tmf_ssp_lun_reset  = TMF_LU_RESET,
 };
+
 /**
  * struct isci_tmf - This class represents the task management object which
  *    acts as an interface to libsas for processing task management requests
@@ -106,15 +94,6 @@ struct isci_tmf {
 	u16 io_tag;
 	enum isci_tmf_function_codes tmf_code;
 	int status;
-
-	/* The optional callback function allows the user process to
-	 * track the TMF transmit / timeout conditions.
-	 */
-	void (*cb_state_func)(
-		enum isci_tmf_cb_state,
-		struct isci_tmf *, void *);
-	void *cb_data;
-
 };
 
 static inline void isci_print_tmf(struct isci_host *ihost, struct isci_tmf *tmf)
@@ -208,113 +187,4 @@ int isci_queuecommand(
 	struct scsi_cmnd *scsi_cmd,
 	void (*donefunc)(struct scsi_cmnd *));
 
-/**
- * enum isci_completion_selection - This enum defines the possible actions to
- *    take with respect to a given request's notification back to libsas.
- *
- *
- */
-enum isci_completion_selection {
-
-	isci_perform_normal_io_completion,      /* Normal notify (task_done) */
-	isci_perform_aborted_io_completion,     /* No notification.   */
-	isci_perform_error_io_completion        /* Use sas_task_abort */
-};
-
-/**
- * isci_task_set_completion_status() - This function sets the completion status
- *    for the request.
- * @task: This parameter is the completed request.
- * @response: This parameter is the response code for the completed task.
- * @status: This parameter is the status code for the completed task.
- *
-* @return The new notification mode for the request.
-*/
-static inline enum isci_completion_selection
-isci_task_set_completion_status(
-	struct sas_task *task,
-	enum service_response response,
-	enum exec_status status,
-	enum isci_completion_selection task_notification_selection)
-{
-	unsigned long flags;
-
-	spin_lock_irqsave(&task->task_state_lock, flags);
-
-	/* If a device reset is being indicated, make sure the I/O
-	* is in the error path.
-	*/
-	if (task->task_state_flags & SAS_TASK_NEED_DEV_RESET) {
-		/* Fail the I/O to make sure it goes into the error path. */
-		response = SAS_TASK_UNDELIVERED;
-		status = SAM_STAT_TASK_ABORTED;
-
-		task_notification_selection = isci_perform_error_io_completion;
-	}
-	task->task_status.resp = response;
-	task->task_status.stat = status;
-
-	switch (task->task_proto) {
-
-	case SAS_PROTOCOL_SATA:
-	case SAS_PROTOCOL_STP:
-	case SAS_PROTOCOL_SATA | SAS_PROTOCOL_STP:
-
-		if (task_notification_selection
-		    == isci_perform_error_io_completion) {
-			/* SATA/STP I/O has it's own means of scheduling device
-			* error handling on the normal path.
-			*/
-			task_notification_selection
-				= isci_perform_normal_io_completion;
-		}
-		break;
-	default:
-		break;
-	}
-
-	switch (task_notification_selection) {
-
-	case isci_perform_error_io_completion:
-
-		if (task->task_proto == SAS_PROTOCOL_SMP) {
-			/* There is no error escalation in the SMP case.
-			 * Convert to a normal completion to avoid the
-			 * timeout in the discovery path and to let the
-			 * next action take place quickly.
-			 */
-			task_notification_selection
-				= isci_perform_normal_io_completion;
-
-			/* Fall through to the normal case... */
-		} else {
-			/* Use sas_task_abort */
-			/* Leave SAS_TASK_STATE_DONE clear
-			 * Leave SAS_TASK_AT_INITIATOR set.
-			 */
-			break;
-		}
-
-	case isci_perform_aborted_io_completion:
-		/* This path can occur with task-managed requests as well as
-		 * requests terminated because of LUN or device resets.
-		 */
-		/* Fall through to the normal case... */
-	case isci_perform_normal_io_completion:
-		/* Normal notification (task_done) */
-		task->task_state_flags |= SAS_TASK_STATE_DONE;
-		task->task_state_flags &= ~(SAS_TASK_AT_INITIATOR |
-					    SAS_TASK_STATE_PENDING);
-		break;
-	default:
-		WARN_ONCE(1, "unknown task_notification_selection: %d\n",
-			 task_notification_selection);
-		break;
-	}
-
-	spin_unlock_irqrestore(&task->task_state_lock, flags);
-
-	return task_notification_selection;
-
-}
 #endif /* !defined(_SCI_TASK_H_) */
diff --git a/drivers/scsi/isci/unsolicited_frame_control.c b/drivers/scsi/isci/unsolicited_frame_control.c
index 16f88ab939c..04a6d0d59a2 100644
--- a/drivers/scsi/isci/unsolicited_frame_control.c
+++ b/drivers/scsi/isci/unsolicited_frame_control.c
@@ -57,31 +57,19 @@
 #include "unsolicited_frame_control.h"
 #include "registers.h"
 
-int sci_unsolicited_frame_control_construct(struct isci_host *ihost)
+void sci_unsolicited_frame_control_construct(struct isci_host *ihost)
 {
 	struct sci_unsolicited_frame_control *uf_control = &ihost->uf_control;
 	struct sci_unsolicited_frame *uf;
-	u32 buf_len, header_len, i;
-	dma_addr_t dma;
-	size_t size;
-	void *virt;
-
-	/*
-	 * Prepare all of the memory sizes for the UF headers, UF address
-	 * table, and UF buffers themselves.
-	 */
-	buf_len = SCU_MAX_UNSOLICITED_FRAMES * SCU_UNSOLICITED_FRAME_BUFFER_SIZE;
-	header_len = SCU_MAX_UNSOLICITED_FRAMES * sizeof(struct scu_unsolicited_frame_header);
-	size = buf_len + header_len + SCU_MAX_UNSOLICITED_FRAMES * sizeof(uf_control->address_table.array[0]);
+	dma_addr_t dma = ihost->ufi_dma;
+	void *virt = ihost->ufi_buf;
+	int i;
 
 	/*
 	 * The Unsolicited Frame buffers are set at the start of the UF
 	 * memory descriptor entry. The headers and address table will be
 	 * placed after the buffers.
 	 */
-	virt = dmam_alloc_coherent(&ihost->pdev->dev, size, &dma, GFP_KERNEL);
-	if (!virt)
-		return -ENOMEM;
 
 	/*
 	 * Program the location of the UF header table into the SCU.
@@ -93,8 +81,8 @@ int sci_unsolicited_frame_control_construct(struct isci_host *ihost)
 	 *   headers, since we program the UF address table pointers to
 	 *   NULL.
 	 */
-	uf_control->headers.physical_address = dma + buf_len;
-	uf_control->headers.array = virt + buf_len;
+	uf_control->headers.physical_address = dma + SCI_UFI_BUF_SIZE;
+	uf_control->headers.array = virt + SCI_UFI_BUF_SIZE;
 
 	/*
 	 * Program the location of the UF address table into the SCU.
@@ -103,8 +91,8 @@ int sci_unsolicited_frame_control_construct(struct isci_host *ihost)
 	 *   byte boundary already due to above programming headers being on a
 	 *   64-bit boundary and headers are on a 64-bytes in size.
 	 */
-	uf_control->address_table.physical_address = dma + buf_len + header_len;
-	uf_control->address_table.array = virt + buf_len + header_len;
+	uf_control->address_table.physical_address = dma + SCI_UFI_BUF_SIZE + SCI_UFI_HDR_SIZE;
+	uf_control->address_table.array = virt + SCI_UFI_BUF_SIZE + SCI_UFI_HDR_SIZE;
 	uf_control->get = 0;
 
 	/*
@@ -135,8 +123,6 @@ int sci_unsolicited_frame_control_construct(struct isci_host *ihost)
 		virt += SCU_UNSOLICITED_FRAME_BUFFER_SIZE;
 		dma += SCU_UNSOLICITED_FRAME_BUFFER_SIZE;
 	}
-
-	return 0;
 }
 
 enum sci_status sci_unsolicited_frame_control_get_header(struct sci_unsolicited_frame_control *uf_control,
diff --git a/drivers/scsi/isci/unsolicited_frame_control.h b/drivers/scsi/isci/unsolicited_frame_control.h
index 75d896686f5..1bc551ec611 100644
--- a/drivers/scsi/isci/unsolicited_frame_control.h
+++ b/drivers/scsi/isci/unsolicited_frame_control.h
@@ -257,9 +257,13 @@ struct sci_unsolicited_frame_control {
 
 };
 
+#define SCI_UFI_BUF_SIZE (SCU_MAX_UNSOLICITED_FRAMES * SCU_UNSOLICITED_FRAME_BUFFER_SIZE)
+#define SCI_UFI_HDR_SIZE (SCU_MAX_UNSOLICITED_FRAMES * sizeof(struct scu_unsolicited_frame_header))
+#define SCI_UFI_TOTAL_SIZE (SCI_UFI_BUF_SIZE + SCI_UFI_HDR_SIZE + SCU_MAX_UNSOLICITED_FRAMES * sizeof(u64))
+
 struct isci_host;
 
-int sci_unsolicited_frame_control_construct(struct isci_host *ihost);
+void sci_unsolicited_frame_control_construct(struct isci_host *ihost);
 
 enum sci_status sci_unsolicited_frame_control_get_header(
 	struct sci_unsolicited_frame_control *uf_control,
diff --git a/drivers/scsi/iscsi_tcp.c b/drivers/scsi/iscsi_tcp.c
index 453a740fa68..922086105b4 100644
--- a/drivers/scsi/iscsi_tcp.c
+++ b/drivers/scsi/iscsi_tcp.c
@@ -662,7 +662,7 @@ iscsi_sw_tcp_conn_bind(struct iscsi_cls_session *cls_session,
 
 	/* setup Socket parameters */
 	sk = sock->sk;
-	sk->sk_reuse = 1;
+	sk->sk_reuse = SK_CAN_REUSE;
 	sk->sk_sndtimeo = 15 * HZ; /* FIXME: make it configurable */
 	sk->sk_allocation = GFP_ATOMIC;
 
diff --git a/drivers/scsi/libfc/fc_lport.c b/drivers/scsi/libfc/fc_lport.c
index cc83b66d45b..c1402fb499a 100644
--- a/drivers/scsi/libfc/fc_lport.c
+++ b/drivers/scsi/libfc/fc_lport.c
@@ -648,6 +648,7 @@ int fc_lport_destroy(struct fc_lport *lport)
 	lport->tt.fcp_abort_io(lport);
 	lport->tt.disc_stop_final(lport);
 	lport->tt.exch_mgr_reset(lport, 0, 0);
+	cancel_delayed_work_sync(&lport->retry_work);
 	fc_fc4_del_lport(lport);
 	return 0;
 }
@@ -1564,7 +1565,6 @@ static void fc_lport_timeout(struct work_struct *work)
 
 	switch (lport->state) {
 	case LPORT_ST_DISABLED:
-		WARN_ON(1);
 		break;
 	case LPORT_ST_READY:
 		break;
diff --git a/drivers/scsi/lpfc/lpfc.h b/drivers/scsi/lpfc/lpfc.h
index 3a1ffdd6d83..e5da6da20f8 100644
--- a/drivers/scsi/lpfc/lpfc.h
+++ b/drivers/scsi/lpfc/lpfc.h
@@ -93,6 +93,9 @@ struct lpfc_sli2_slim;
 /* lpfc wait event data ready flag */
 #define LPFC_DATA_READY		(1<<0)
 
+/* queue dump line buffer size */
+#define LPFC_LBUF_SZ		128
+
 enum lpfc_polling_flags {
 	ENABLE_FCP_RING_POLLING = 0x1,
 	DISABLE_FCP_RING_INT    = 0x2
@@ -620,6 +623,7 @@ struct lpfc_hba {
 #define HBA_AER_ENABLED		0x1000 /* AER enabled with HBA */
 #define HBA_DEVLOSS_TMO         0x2000 /* HBA in devloss timeout */
 #define HBA_RRQ_ACTIVE		0x4000 /* process the rrq active list */
+#define HBA_FCP_IOQ_FLUSH	0x8000 /* FCP I/O queues being flushed */
 	uint32_t fcp_ring_in_use; /* When polling test if intr-hndlr active*/
 	struct lpfc_dmabuf slim2p;
 
diff --git a/drivers/scsi/lpfc/lpfc_bsg.c b/drivers/scsi/lpfc/lpfc_bsg.c
index 141e4b40bb5..253d9a85734 100644
--- a/drivers/scsi/lpfc/lpfc_bsg.c
+++ b/drivers/scsi/lpfc/lpfc_bsg.c
@@ -1,7 +1,7 @@
 /*******************************************************************
  * This file is part of the Emulex Linux Device Driver for         *
  * Fibre Channel Host Bus Adapters.                                *
- * Copyright (C) 2009-2011 Emulex.  All rights reserved.           *
+ * Copyright (C) 2009-2012 Emulex.  All rights reserved.           *
  * EMULEX and SLI are trademarks of Emulex.                        *
  * www.emulex.com                                                  *
  *                                                                 *
@@ -599,6 +599,7 @@ lpfc_bsg_rport_els(struct fc_bsg_job *job)
 
 	cmdiocbq->iocb_cmpl = lpfc_bsg_rport_els_cmp;
 	cmdiocbq->context1 = dd_data;
+	cmdiocbq->context_un.ndlp = ndlp;
 	cmdiocbq->context2 = rspiocbq;
 	dd_data->type = TYPE_IOCB;
 	dd_data->context_un.iocb.cmdiocbq = cmdiocbq;
@@ -3978,6 +3979,7 @@ lpfc_bsg_handle_sli_cfg_mbox(struct lpfc_hba *phba, struct fc_bsg_job *job,
 		} else if (subsys == SLI_CONFIG_SUBSYS_COMN) {
 			switch (opcode) {
 			case COMN_OPCODE_GET_CNTL_ADDL_ATTRIBUTES:
+			case COMN_OPCODE_GET_CNTL_ATTRIBUTES:
 				lpfc_printf_log(phba, KERN_INFO, LOG_LIBDFC,
 						"3106 Handled SLI_CONFIG "
 						"subsys_comn, opcode:x%x\n",
diff --git a/drivers/scsi/lpfc/lpfc_bsg.h b/drivers/scsi/lpfc/lpfc_bsg.h
index edfe61fc52b..67f7d0a160d 100644
--- a/drivers/scsi/lpfc/lpfc_bsg.h
+++ b/drivers/scsi/lpfc/lpfc_bsg.h
@@ -1,7 +1,7 @@
 /*******************************************************************
  * This file is part of the Emulex Linux Device Driver for         *
  * Fibre Channel Host Bus Adapters.                                *
- * Copyright (C) 2010 Emulex.  All rights reserved.                *
+ * Copyright (C) 2010-2012 Emulex.  All rights reserved.                *
  * EMULEX and SLI are trademarks of Emulex.                        *
  * www.emulex.com                                                  *
  *                                                                 *
@@ -249,6 +249,7 @@ struct lpfc_sli_config_emb1_subsys {
 #define COMN_OPCODE_READ_OBJECT_LIST	0xAD
 #define COMN_OPCODE_DELETE_OBJECT	0xAE
 #define COMN_OPCODE_GET_CNTL_ADDL_ATTRIBUTES	0x79
+#define COMN_OPCODE_GET_CNTL_ATTRIBUTES	0x20
 	uint32_t timeout;
 	uint32_t request_length;
 	uint32_t word9;
diff --git a/drivers/scsi/lpfc/lpfc_crtn.h b/drivers/scsi/lpfc/lpfc_crtn.h
index 330dd7192a7..9b2a16f3bc7 100644
--- a/drivers/scsi/lpfc/lpfc_crtn.h
+++ b/drivers/scsi/lpfc/lpfc_crtn.h
@@ -254,6 +254,7 @@ int
 lpfc_sli_handle_fast_ring_event(struct lpfc_hba *,
 			struct lpfc_sli_ring *, uint32_t);
 
+struct lpfc_iocbq *__lpfc_sli_get_iocbq(struct lpfc_hba *);
 struct lpfc_iocbq * lpfc_sli_get_iocbq(struct lpfc_hba *);
 void lpfc_sli_release_iocbq(struct lpfc_hba *, struct lpfc_iocbq *);
 uint16_t lpfc_sli_next_iotag(struct lpfc_hba *, struct lpfc_iocbq *);
@@ -460,6 +461,7 @@ int lpfc_hba_init_link_fc_topology(struct lpfc_hba *, uint32_t, uint32_t);
 int lpfc_issue_reg_vfi(struct lpfc_vport *);
 int lpfc_issue_unreg_vfi(struct lpfc_vport *);
 int lpfc_selective_reset(struct lpfc_hba *);
-int lpfc_sli4_read_config(struct lpfc_hba *phba);
-int lpfc_scsi_buf_update(struct lpfc_hba *phba);
-void lpfc_sli4_node_prep(struct lpfc_hba *phba);
+int lpfc_sli4_read_config(struct lpfc_hba *);
+void lpfc_sli4_node_prep(struct lpfc_hba *);
+int lpfc_sli4_xri_sgl_update(struct lpfc_hba *);
+void lpfc_free_sgl_list(struct lpfc_hba *, struct list_head *);
diff --git a/drivers/scsi/lpfc/lpfc_debugfs.c b/drivers/scsi/lpfc/lpfc_debugfs.c
index af04b0d6688..3217d63ed28 100644
--- a/drivers/scsi/lpfc/lpfc_debugfs.c
+++ b/drivers/scsi/lpfc/lpfc_debugfs.c
@@ -4466,3 +4466,49 @@ lpfc_debugfs_terminate(struct lpfc_vport *vport)
 #endif
 	return;
 }
+
+/*
+ * Driver debug utility routines outside of debugfs. The debug utility
+ * routines implemented here is intended to be used in the instrumented
+ * debug driver for debugging host or port issues.
+ */
+
+/**
+ * lpfc_debug_dump_all_queues - dump all the queues with a hba
+ * @phba: Pointer to HBA context object.
+ *
+ * This function dumps entries of all the queues asociated with the @phba.
+ **/
+void
+lpfc_debug_dump_all_queues(struct lpfc_hba *phba)
+{
+	int fcp_wqidx;
+
+	/*
+	 * Dump Work Queues (WQs)
+	 */
+	lpfc_debug_dump_mbx_wq(phba);
+	lpfc_debug_dump_els_wq(phba);
+
+	for (fcp_wqidx = 0; fcp_wqidx < phba->cfg_fcp_wq_count; fcp_wqidx++)
+		lpfc_debug_dump_fcp_wq(phba, fcp_wqidx);
+
+	lpfc_debug_dump_hdr_rq(phba);
+	lpfc_debug_dump_dat_rq(phba);
+	/*
+	 * Dump Complete Queues (CQs)
+	 */
+	lpfc_debug_dump_mbx_cq(phba);
+	lpfc_debug_dump_els_cq(phba);
+
+	for (fcp_wqidx = 0; fcp_wqidx < phba->cfg_fcp_wq_count; fcp_wqidx++)
+		lpfc_debug_dump_fcp_cq(phba, fcp_wqidx);
+
+	/*
+	 * Dump Event Queues (EQs)
+	 */
+	lpfc_debug_dump_sp_eq(phba);
+
+	for (fcp_wqidx = 0; fcp_wqidx < phba->cfg_fcp_wq_count; fcp_wqidx++)
+		lpfc_debug_dump_fcp_eq(phba, fcp_wqidx);
+}
diff --git a/drivers/scsi/lpfc/lpfc_debugfs.h b/drivers/scsi/lpfc/lpfc_debugfs.h
index f83bd944edd..616c400dae1 100644
--- a/drivers/scsi/lpfc/lpfc_debugfs.h
+++ b/drivers/scsi/lpfc/lpfc_debugfs.h
@@ -267,3 +267,421 @@ struct lpfc_idiag {
 #define LPFC_DISC_TRC_DISCOVERY		0xef    /* common mask for general
 						 * discovery */
 #endif /* H_LPFC_DEBUG_FS */
+
+
+/*
+ * Driver debug utility routines outside of debugfs. The debug utility
+ * routines implemented here is intended to be used in the instrumented
+ * debug driver for debugging host or port issues.
+ */
+
+/**
+ * lpfc_debug_dump_qe - dump an specific entry from a queue
+ * @q: Pointer to the queue descriptor.
+ * @idx: Index to the entry on the queue.
+ *
+ * This function dumps an entry indexed by @idx from a queue specified by the
+ * queue descriptor @q.
+ **/
+static inline void
+lpfc_debug_dump_qe(struct lpfc_queue *q, uint32_t idx)
+{
+	char line_buf[LPFC_LBUF_SZ];
+	int i, esize, qe_word_cnt, len;
+	uint32_t *pword;
+
+	/* sanity checks */
+	if (!q)
+		return;
+	if (idx >= q->entry_count)
+		return;
+
+	esize = q->entry_size;
+	qe_word_cnt = esize / sizeof(uint32_t);
+	pword = q->qe[idx].address;
+
+	len = 0;
+	len += snprintf(line_buf+len, LPFC_LBUF_SZ-len, "QE[%04d]: ", idx);
+	if (qe_word_cnt > 8)
+		printk(KERN_ERR "%s\n", line_buf);
+
+	for (i = 0; i < qe_word_cnt; i++) {
+		if (!(i % 8)) {
+			if (i != 0)
+				printk(KERN_ERR "%s\n", line_buf);
+			if (qe_word_cnt > 8) {
+				len = 0;
+				memset(line_buf, 0, LPFC_LBUF_SZ);
+				len += snprintf(line_buf+len, LPFC_LBUF_SZ-len,
+						"%03d: ", i);
+			}
+		}
+		len += snprintf(line_buf+len, LPFC_LBUF_SZ-len, "%08x ",
+				((uint32_t)*pword) & 0xffffffff);
+		pword++;
+	}
+	if (qe_word_cnt <= 8 || (i - 1) % 8)
+		printk(KERN_ERR "%s\n", line_buf);
+}
+
+/**
+ * lpfc_debug_dump_q - dump all entries from an specific queue
+ * @q: Pointer to the queue descriptor.
+ *
+ * This function dumps all entries from a queue specified by the queue
+ * descriptor @q.
+ **/
+static inline void
+lpfc_debug_dump_q(struct lpfc_queue *q)
+{
+	int idx, entry_count;
+
+	/* sanity check */
+	if (!q)
+		return;
+
+	dev_printk(KERN_ERR, &(((q->phba))->pcidev)->dev,
+		"%d: [qid:%d, type:%d, subtype:%d, "
+		"qe_size:%d, qe_count:%d, "
+		"host_index:%d, port_index:%d]\n",
+		(q->phba)->brd_no,
+		q->queue_id, q->type, q->subtype,
+		q->entry_size, q->entry_count,
+		q->host_index, q->hba_index);
+	entry_count = q->entry_count;
+	for (idx = 0; idx < entry_count; idx++)
+		lpfc_debug_dump_qe(q, idx);
+	printk(KERN_ERR "\n");
+}
+
+/**
+ * lpfc_debug_dump_fcp_wq - dump all entries from a fcp work queue
+ * @phba: Pointer to HBA context object.
+ * @fcp_wqidx: Index to a FCP work queue.
+ *
+ * This function dumps all entries from a FCP work queue specified by the
+ * @fcp_wqidx.
+ **/
+static inline void
+lpfc_debug_dump_fcp_wq(struct lpfc_hba *phba, int fcp_wqidx)
+{
+	/* sanity check */
+	if (fcp_wqidx >= phba->cfg_fcp_wq_count)
+		return;
+
+	printk(KERN_ERR "FCP WQ: WQ[Idx:%d|Qid:%d]\n",
+		fcp_wqidx, phba->sli4_hba.fcp_wq[fcp_wqidx]->queue_id);
+	lpfc_debug_dump_q(phba->sli4_hba.fcp_wq[fcp_wqidx]);
+}
+
+/**
+ * lpfc_debug_dump_fcp_cq - dump all entries from a fcp work queue's cmpl queue
+ * @phba: Pointer to HBA context object.
+ * @fcp_wqidx: Index to a FCP work queue.
+ *
+ * This function dumps all entries from a FCP complete queue which is
+ * associated to the FCP work queue specified by the @fcp_wqidx.
+ **/
+static inline void
+lpfc_debug_dump_fcp_cq(struct lpfc_hba *phba, int fcp_wqidx)
+{
+	int fcp_cqidx, fcp_cqid;
+
+	/* sanity check */
+	if (fcp_wqidx >= phba->cfg_fcp_wq_count)
+		return;
+
+	fcp_cqid = phba->sli4_hba.fcp_wq[fcp_wqidx]->assoc_qid;
+	for (fcp_cqidx = 0; fcp_cqidx < phba->cfg_fcp_eq_count; fcp_cqidx++)
+		if (phba->sli4_hba.fcp_cq[fcp_cqidx]->queue_id == fcp_cqid)
+			break;
+	if (fcp_cqidx >= phba->cfg_fcp_eq_count)
+		return;
+
+	printk(KERN_ERR "FCP CQ: WQ[Idx:%d|Qid%d]->CQ[Idx%d|Qid%d]:\n",
+		fcp_wqidx, phba->sli4_hba.fcp_wq[fcp_wqidx]->queue_id,
+		fcp_cqidx, fcp_cqid);
+	lpfc_debug_dump_q(phba->sli4_hba.fcp_cq[fcp_cqidx]);
+}
+
+/**
+ * lpfc_debug_dump_fcp_eq - dump all entries from a fcp work queue's evt queue
+ * @phba: Pointer to HBA context object.
+ * @fcp_wqidx: Index to a FCP work queue.
+ *
+ * This function dumps all entries from a FCP event queue which is
+ * associated to the FCP work queue specified by the @fcp_wqidx.
+ **/
+static inline void
+lpfc_debug_dump_fcp_eq(struct lpfc_hba *phba, int fcp_wqidx)
+{
+	struct lpfc_queue *qdesc;
+	int fcp_eqidx, fcp_eqid;
+	int fcp_cqidx, fcp_cqid;
+
+	/* sanity check */
+	if (fcp_wqidx >= phba->cfg_fcp_wq_count)
+		return;
+	fcp_cqid = phba->sli4_hba.fcp_wq[fcp_wqidx]->assoc_qid;
+	for (fcp_cqidx = 0; fcp_cqidx < phba->cfg_fcp_eq_count; fcp_cqidx++)
+		if (phba->sli4_hba.fcp_cq[fcp_cqidx]->queue_id == fcp_cqid)
+			break;
+	if (fcp_cqidx >= phba->cfg_fcp_eq_count)
+		return;
+
+	if (phba->cfg_fcp_eq_count == 0) {
+		fcp_eqidx = -1;
+		fcp_eqid = phba->sli4_hba.sp_eq->queue_id;
+		qdesc = phba->sli4_hba.sp_eq;
+	} else {
+		fcp_eqidx = fcp_cqidx;
+		fcp_eqid = phba->sli4_hba.fp_eq[fcp_eqidx]->queue_id;
+		qdesc = phba->sli4_hba.fp_eq[fcp_eqidx];
+	}
+
+	printk(KERN_ERR "FCP EQ: WQ[Idx:%d|Qid:%d]->CQ[Idx:%d|Qid:%d]->"
+		"EQ[Idx:%d|Qid:%d]\n",
+		fcp_wqidx, phba->sli4_hba.fcp_wq[fcp_wqidx]->queue_id,
+		fcp_cqidx, fcp_cqid, fcp_eqidx, fcp_eqid);
+	lpfc_debug_dump_q(qdesc);
+}
+
+/**
+ * lpfc_debug_dump_els_wq - dump all entries from the els work queue
+ * @phba: Pointer to HBA context object.
+ *
+ * This function dumps all entries from the ELS work queue.
+ **/
+static inline void
+lpfc_debug_dump_els_wq(struct lpfc_hba *phba)
+{
+	printk(KERN_ERR "ELS WQ: WQ[Qid:%d]:\n",
+		phba->sli4_hba.els_wq->queue_id);
+	lpfc_debug_dump_q(phba->sli4_hba.els_wq);
+}
+
+/**
+ * lpfc_debug_dump_mbx_wq - dump all entries from the mbox work queue
+ * @phba: Pointer to HBA context object.
+ *
+ * This function dumps all entries from the MBOX work queue.
+ **/
+static inline void
+lpfc_debug_dump_mbx_wq(struct lpfc_hba *phba)
+{
+	printk(KERN_ERR "MBX WQ: WQ[Qid:%d]\n",
+		phba->sli4_hba.mbx_wq->queue_id);
+	lpfc_debug_dump_q(phba->sli4_hba.mbx_wq);
+}
+
+/**
+ * lpfc_debug_dump_dat_rq - dump all entries from the receive data queue
+ * @phba: Pointer to HBA context object.
+ *
+ * This function dumps all entries from the receive data queue.
+ **/
+static inline void
+lpfc_debug_dump_dat_rq(struct lpfc_hba *phba)
+{
+	printk(KERN_ERR "DAT RQ: RQ[Qid:%d]\n",
+		phba->sli4_hba.dat_rq->queue_id);
+	lpfc_debug_dump_q(phba->sli4_hba.dat_rq);
+}
+
+/**
+ * lpfc_debug_dump_hdr_rq - dump all entries from the receive header queue
+ * @phba: Pointer to HBA context object.
+ *
+ * This function dumps all entries from the receive header queue.
+ **/
+static inline void
+lpfc_debug_dump_hdr_rq(struct lpfc_hba *phba)
+{
+	printk(KERN_ERR "HDR RQ: RQ[Qid:%d]\n",
+		phba->sli4_hba.hdr_rq->queue_id);
+	lpfc_debug_dump_q(phba->sli4_hba.hdr_rq);
+}
+
+/**
+ * lpfc_debug_dump_els_cq - dump all entries from the els complete queue
+ * @phba: Pointer to HBA context object.
+ *
+ * This function dumps all entries from the els complete queue.
+ **/
+static inline void
+lpfc_debug_dump_els_cq(struct lpfc_hba *phba)
+{
+	printk(KERN_ERR "ELS CQ: WQ[Qid:%d]->CQ[Qid:%d]\n",
+		phba->sli4_hba.els_wq->queue_id,
+		phba->sli4_hba.els_cq->queue_id);
+	lpfc_debug_dump_q(phba->sli4_hba.els_cq);
+}
+
+/**
+ * lpfc_debug_dump_mbx_cq - dump all entries from the mbox complete queue
+ * @phba: Pointer to HBA context object.
+ *
+ * This function dumps all entries from the mbox complete queue.
+ **/
+static inline void
+lpfc_debug_dump_mbx_cq(struct lpfc_hba *phba)
+{
+	printk(KERN_ERR "MBX CQ: WQ[Qid:%d]->CQ[Qid:%d]\n",
+		phba->sli4_hba.mbx_wq->queue_id,
+		phba->sli4_hba.mbx_cq->queue_id);
+	lpfc_debug_dump_q(phba->sli4_hba.mbx_cq);
+}
+
+/**
+ * lpfc_debug_dump_sp_eq - dump all entries from slow-path event queue
+ * @phba: Pointer to HBA context object.
+ *
+ * This function dumps all entries from the slow-path event queue.
+ **/
+static inline void
+lpfc_debug_dump_sp_eq(struct lpfc_hba *phba)
+{
+	printk(KERN_ERR "SP EQ: WQ[Qid:%d/Qid:%d]->CQ[Qid:%d/Qid:%d]->"
+		"EQ[Qid:%d]:\n",
+		phba->sli4_hba.mbx_wq->queue_id,
+		phba->sli4_hba.els_wq->queue_id,
+		phba->sli4_hba.mbx_cq->queue_id,
+		phba->sli4_hba.els_cq->queue_id,
+		phba->sli4_hba.sp_eq->queue_id);
+	lpfc_debug_dump_q(phba->sli4_hba.sp_eq);
+}
+
+/**
+ * lpfc_debug_dump_wq_by_id - dump all entries from a work queue by queue id
+ * @phba: Pointer to HBA context object.
+ * @qid: Work queue identifier.
+ *
+ * This function dumps all entries from a work queue identified by the queue
+ * identifier.
+ **/
+static inline void
+lpfc_debug_dump_wq_by_id(struct lpfc_hba *phba, int qid)
+{
+	int wq_idx;
+
+	for (wq_idx = 0; wq_idx < phba->cfg_fcp_wq_count; wq_idx++)
+		if (phba->sli4_hba.fcp_wq[wq_idx]->queue_id == qid)
+			break;
+	if (wq_idx < phba->cfg_fcp_wq_count) {
+		printk(KERN_ERR "FCP WQ[Idx:%d|Qid:%d]\n", wq_idx, qid);
+		lpfc_debug_dump_q(phba->sli4_hba.fcp_wq[wq_idx]);
+		return;
+	}
+
+	if (phba->sli4_hba.els_wq->queue_id == qid) {
+		printk(KERN_ERR "ELS WQ[Qid:%d]\n", qid);
+		lpfc_debug_dump_q(phba->sli4_hba.els_wq);
+	}
+}
+
+/**
+ * lpfc_debug_dump_mq_by_id - dump all entries from a mbox queue by queue id
+ * @phba: Pointer to HBA context object.
+ * @qid: Mbox work queue identifier.
+ *
+ * This function dumps all entries from a mbox work queue identified by the
+ * queue identifier.
+ **/
+static inline void
+lpfc_debug_dump_mq_by_id(struct lpfc_hba *phba, int qid)
+{
+	if (phba->sli4_hba.mbx_wq->queue_id == qid) {
+		printk(KERN_ERR "MBX WQ[Qid:%d]\n", qid);
+		lpfc_debug_dump_q(phba->sli4_hba.mbx_wq);
+	}
+}
+
+/**
+ * lpfc_debug_dump_rq_by_id - dump all entries from a receive queue by queue id
+ * @phba: Pointer to HBA context object.
+ * @qid: Receive queue identifier.
+ *
+ * This function dumps all entries from a receive queue identified by the
+ * queue identifier.
+ **/
+static inline void
+lpfc_debug_dump_rq_by_id(struct lpfc_hba *phba, int qid)
+{
+	if (phba->sli4_hba.hdr_rq->queue_id == qid) {
+		printk(KERN_ERR "HDR RQ[Qid:%d]\n", qid);
+		lpfc_debug_dump_q(phba->sli4_hba.hdr_rq);
+		return;
+	}
+	if (phba->sli4_hba.dat_rq->queue_id == qid) {
+		printk(KERN_ERR "DAT RQ[Qid:%d]\n", qid);
+		lpfc_debug_dump_q(phba->sli4_hba.dat_rq);
+	}
+}
+
+/**
+ * lpfc_debug_dump_cq_by_id - dump all entries from a cmpl queue by queue id
+ * @phba: Pointer to HBA context object.
+ * @qid: Complete queue identifier.
+ *
+ * This function dumps all entries from a complete queue identified by the
+ * queue identifier.
+ **/
+static inline void
+lpfc_debug_dump_cq_by_id(struct lpfc_hba *phba, int qid)
+{
+	int cq_idx = 0;
+
+	do {
+		if (phba->sli4_hba.fcp_cq[cq_idx]->queue_id == qid)
+			break;
+	} while (++cq_idx < phba->cfg_fcp_eq_count);
+
+	if (cq_idx < phba->cfg_fcp_eq_count) {
+		printk(KERN_ERR "FCP CQ[Idx:%d|Qid:%d]\n", cq_idx, qid);
+		lpfc_debug_dump_q(phba->sli4_hba.fcp_cq[cq_idx]);
+		return;
+	}
+
+	if (phba->sli4_hba.els_cq->queue_id == qid) {
+		printk(KERN_ERR "ELS CQ[Qid:%d]\n", qid);
+		lpfc_debug_dump_q(phba->sli4_hba.els_cq);
+		return;
+	}
+
+	if (phba->sli4_hba.mbx_cq->queue_id == qid) {
+		printk(KERN_ERR "MBX CQ[Qid:%d]\n", qid);
+		lpfc_debug_dump_q(phba->sli4_hba.mbx_cq);
+	}
+}
+
+/**
+ * lpfc_debug_dump_eq_by_id - dump all entries from an event queue by queue id
+ * @phba: Pointer to HBA context object.
+ * @qid: Complete queue identifier.
+ *
+ * This function dumps all entries from an event queue identified by the
+ * queue identifier.
+ **/
+static inline void
+lpfc_debug_dump_eq_by_id(struct lpfc_hba *phba, int qid)
+{
+	int eq_idx;
+
+	for (eq_idx = 0; eq_idx < phba->cfg_fcp_eq_count; eq_idx++) {
+		if (phba->sli4_hba.fp_eq[eq_idx]->queue_id == qid)
+			break;
+	}
+
+	if (eq_idx < phba->cfg_fcp_eq_count) {
+		printk(KERN_ERR "FCP EQ[Idx:%d|Qid:%d]\n", eq_idx, qid);
+		lpfc_debug_dump_q(phba->sli4_hba.fp_eq[eq_idx]);
+		return;
+	}
+
+	if (phba->sli4_hba.sp_eq->queue_id == qid) {
+		printk(KERN_ERR "SP EQ[|Qid:%d]\n", qid);
+		lpfc_debug_dump_q(phba->sli4_hba.sp_eq);
+	}
+}
+
+void lpfc_debug_dump_all_queues(struct lpfc_hba *);
diff --git a/drivers/scsi/lpfc/lpfc_els.c b/drivers/scsi/lpfc/lpfc_els.c
index 3407b39e0a3..d54ae199979 100644
--- a/drivers/scsi/lpfc/lpfc_els.c
+++ b/drivers/scsi/lpfc/lpfc_els.c
@@ -230,27 +230,43 @@ lpfc_prep_els_iocb(struct lpfc_vport *vport, uint8_t expectRsp,
 
 	INIT_LIST_HEAD(&pbuflist->list);
 
-	icmd->un.elsreq64.bdl.addrHigh = putPaddrHigh(pbuflist->phys);
-	icmd->un.elsreq64.bdl.addrLow = putPaddrLow(pbuflist->phys);
-	icmd->un.elsreq64.bdl.bdeFlags = BUFF_TYPE_BLP_64;
-	icmd->un.elsreq64.remoteID = did;	/* DID */
 	if (expectRsp) {
+		icmd->un.elsreq64.bdl.addrHigh = putPaddrHigh(pbuflist->phys);
+		icmd->un.elsreq64.bdl.addrLow = putPaddrLow(pbuflist->phys);
+		icmd->un.elsreq64.bdl.bdeFlags = BUFF_TYPE_BLP_64;
 		icmd->un.elsreq64.bdl.bdeSize = (2 * sizeof(struct ulp_bde64));
+
+		icmd->un.elsreq64.remoteID = did;		/* DID */
 		icmd->ulpCommand = CMD_ELS_REQUEST64_CR;
 		icmd->ulpTimeout = phba->fc_ratov * 2;
 	} else {
-		icmd->un.elsreq64.bdl.bdeSize = sizeof(struct ulp_bde64);
+		icmd->un.xseq64.bdl.addrHigh = putPaddrHigh(pbuflist->phys);
+		icmd->un.xseq64.bdl.addrLow = putPaddrLow(pbuflist->phys);
+		icmd->un.xseq64.bdl.bdeFlags = BUFF_TYPE_BLP_64;
+		icmd->un.xseq64.bdl.bdeSize = sizeof(struct ulp_bde64);
+		icmd->un.xseq64.xmit_els_remoteID = did;	/* DID */
 		icmd->ulpCommand = CMD_XMIT_ELS_RSP64_CX;
 	}
 	icmd->ulpBdeCount = 1;
 	icmd->ulpLe = 1;
 	icmd->ulpClass = CLASS3;
 
-	if (phba->sli3_options & LPFC_SLI3_NPIV_ENABLED) {
-		icmd->un.elsreq64.myID = vport->fc_myDID;
+	/*
+	 * If we have NPIV enabled, we want to send ELS traffic by VPI.
+	 * For SLI4, since the driver controls VPIs we also want to include
+	 * all ELS pt2pt protocol traffic as well.
+	 */
+	if ((phba->sli3_options & LPFC_SLI3_NPIV_ENABLED) ||
+		((phba->sli_rev == LPFC_SLI_REV4) &&
+		    (vport->fc_flag & FC_PT2PT))) {
+
+		if (expectRsp) {
+			icmd->un.elsreq64.myID = vport->fc_myDID;
+
+			/* For ELS_REQUEST64_CR, use the VPI by default */
+			icmd->ulpContext = phba->vpi_ids[vport->vpi];
+		}
 
-		/* For ELS_REQUEST64_CR, use the VPI by default */
-		icmd->ulpContext = phba->vpi_ids[vport->vpi];
 		icmd->ulpCt_h = 0;
 		/* The CT field must be 0=INVALID_RPI for the ECHO cmd */
 		if (elscmd == ELS_CMD_ECHO)
@@ -438,9 +454,10 @@ lpfc_issue_reg_vfi(struct lpfc_vport *vport)
 	int rc = 0;
 
 	sp = &phba->fc_fabparam;
-	/* move forward in case of SLI4 FC port loopback test */
+	/* move forward in case of SLI4 FC port loopback test and pt2pt mode */
 	if ((phba->sli_rev == LPFC_SLI_REV4) &&
-	    !(phba->link_flag & LS_LOOPBACK_MODE)) {
+	    !(phba->link_flag & LS_LOOPBACK_MODE) &&
+	    !(vport->fc_flag & FC_PT2PT)) {
 		ndlp = lpfc_findnode_did(vport, Fabric_DID);
 		if (!ndlp || !NLP_CHK_NODE_ACT(ndlp)) {
 			rc = -ENODEV;
@@ -707,14 +724,17 @@ lpfc_cmpl_els_flogi_fabric(struct lpfc_vport *vport, struct lpfc_nodelist *ndlp,
 			lpfc_sli4_unreg_all_rpis(vport);
 			lpfc_mbx_unreg_vpi(vport);
 			spin_lock_irq(shost->host_lock);
-			vport->fc_flag |= FC_VPORT_NEEDS_REG_VPI;
-			/*
-			* If VPI is unreged, driver need to do INIT_VPI
-			* before re-registering
-			*/
 			vport->fc_flag |= FC_VPORT_NEEDS_INIT_VPI;
 			spin_unlock_irq(shost->host_lock);
 		}
+
+		/*
+		 * For SLI3 and SLI4, the VPI needs to be reregistered in
+		 * response to this fabric parameter change event.
+		 */
+		spin_lock_irq(shost->host_lock);
+		vport->fc_flag |= FC_VPORT_NEEDS_REG_VPI;
+		spin_unlock_irq(shost->host_lock);
 	} else if ((phba->sli_rev == LPFC_SLI_REV4) &&
 		!(vport->fc_flag & FC_VPORT_NEEDS_REG_VPI)) {
 			/*
@@ -817,6 +837,17 @@ lpfc_cmpl_els_flogi_nport(struct lpfc_vport *vport, struct lpfc_nodelist *ndlp,
 			mempool_free(mbox, phba->mbox_mem_pool);
 			goto fail;
 		}
+
+		/*
+		 * For SLI4, the VFI/VPI are registered AFTER the
+		 * Nport with the higher WWPN sends the PLOGI with
+		 * an assigned NPortId.
+		 */
+
+		/* not equal */
+		if ((phba->sli_rev == LPFC_SLI_REV4) && rc)
+			lpfc_issue_reg_vfi(vport);
+
 		/* Decrement ndlp reference count indicating that ndlp can be
 		 * safely released when other references to it are done.
 		 */
@@ -2972,7 +3003,7 @@ lpfc_els_retry(struct lpfc_hba *phba, struct lpfc_iocbq *cmdiocb,
 			 * ABTS we cannot generate and RRQ.
 			 */
 			lpfc_set_rrq_active(phba, ndlp,
-					 cmdiocb->sli4_xritag, 0, 0);
+					 cmdiocb->sli4_lxritag, 0, 0);
 		}
 		break;
 	case IOSTAT_LOCAL_REJECT:
@@ -3803,10 +3834,11 @@ lpfc_els_rsp_acc(struct lpfc_vport *vport, uint32_t flag,
 	/* Xmit ELS ACC response tag <ulpIoTag> */
 	lpfc_printf_vlog(vport, KERN_INFO, LOG_ELS,
 			 "0128 Xmit ELS ACC response tag x%x, XRI: x%x, "
-			 "DID: x%x, nlp_flag: x%x nlp_state: x%x RPI: x%x\n",
+			 "DID: x%x, nlp_flag: x%x nlp_state: x%x RPI: x%x "
+			 "fc_flag x%x\n",
 			 elsiocb->iotag, elsiocb->iocb.ulpContext,
 			 ndlp->nlp_DID, ndlp->nlp_flag, ndlp->nlp_state,
-			 ndlp->nlp_rpi);
+			 ndlp->nlp_rpi, vport->fc_flag);
 	if (ndlp->nlp_flag & NLP_LOGO_ACC) {
 		spin_lock_irq(shost->host_lock);
 		ndlp->nlp_flag &= ~NLP_LOGO_ACC;
@@ -4936,8 +4968,6 @@ lpfc_els_rcv_flogi(struct lpfc_vport *vport, struct lpfc_iocbq *cmdiocb,
 		return 1;
 	}
 
-	did = Fabric_DID;
-
 	if ((lpfc_check_sparm(vport, ndlp, sp, CLASS3, 1))) {
 		/* For a FLOGI we accept, then if our portname is greater
 		 * then the remote portname we initiate Nport login.
@@ -4976,26 +5006,82 @@ lpfc_els_rcv_flogi(struct lpfc_vport *vport, struct lpfc_iocbq *cmdiocb,
 			spin_lock_irq(shost->host_lock);
 			vport->fc_flag |= FC_PT2PT_PLOGI;
 			spin_unlock_irq(shost->host_lock);
+
+			/* If we have the high WWPN we can assign our own
+			 * myDID; otherwise, we have to WAIT for a PLOGI
+			 * from the remote NPort to find out what it
+			 * will be.
+			 */
+			vport->fc_myDID = PT2PT_LocalID;
 		}
+
+		/*
+		 * The vport state should go to LPFC_FLOGI only
+		 * AFTER we issue a FLOGI, not receive one.
+		 */
 		spin_lock_irq(shost->host_lock);
 		vport->fc_flag |= FC_PT2PT;
 		vport->fc_flag &= ~(FC_FABRIC | FC_PUBLIC_LOOP);
 		spin_unlock_irq(shost->host_lock);
+
+		/*
+		 * We temporarily set fc_myDID to make it look like we are
+		 * a Fabric. This is done just so we end up with the right
+		 * did / sid on the FLOGI ACC rsp.
+		 */
+		did = vport->fc_myDID;
+		vport->fc_myDID = Fabric_DID;
+
 	} else {
 		/* Reject this request because invalid parameters */
 		stat.un.b.lsRjtRsvd0 = 0;
 		stat.un.b.lsRjtRsnCode = LSRJT_UNABLE_TPC;
 		stat.un.b.lsRjtRsnCodeExp = LSEXP_SPARM_OPTIONS;
 		stat.un.b.vendorUnique = 0;
+
+		/*
+		 * We temporarily set fc_myDID to make it look like we are
+		 * a Fabric. This is done just so we end up with the right
+		 * did / sid on the FLOGI LS_RJT rsp.
+		 */
+		did = vport->fc_myDID;
+		vport->fc_myDID = Fabric_DID;
+
 		lpfc_els_rsp_reject(vport, stat.un.lsRjtError, cmdiocb, ndlp,
 			NULL);
+
+		/* Now lets put fc_myDID back to what its supposed to be */
+		vport->fc_myDID = did;
+
 		return 1;
 	}
 
 	/* Send back ACC */
 	lpfc_els_rsp_acc(vport, ELS_CMD_PLOGI, cmdiocb, ndlp, NULL);
 
+	/* Now lets put fc_myDID back to what its supposed to be */
+	vport->fc_myDID = did;
+
+	if (!(vport->fc_flag & FC_PT2PT_PLOGI)) {
+
+		mbox = mempool_alloc(phba->mbox_mem_pool, GFP_KERNEL);
+		if (!mbox)
+			goto fail;
+
+		lpfc_config_link(phba, mbox);
+
+		mbox->mbox_cmpl = lpfc_sli_def_mbox_cmpl;
+		mbox->vport = vport;
+		rc = lpfc_sli_issue_mbox(phba, mbox, MBX_NOWAIT);
+		if (rc == MBX_NOT_FINISHED) {
+			mempool_free(mbox, phba->mbox_mem_pool);
+			goto fail;
+		}
+	}
+
 	return 0;
+fail:
+	return 1;
 }
 
 /**
@@ -5176,7 +5262,6 @@ lpfc_els_rsp_rls_acc(struct lpfc_hba *phba, LPFC_MBOXQ_t *pmb)
 	}
 
 	cmdsize = sizeof(struct RLS_RSP) + sizeof(uint32_t);
-	mempool_free(pmb, phba->mbox_mem_pool);
 	elsiocb = lpfc_prep_els_iocb(phba->pport, 0, cmdsize,
 				     lpfc_max_els_tries, ndlp,
 				     ndlp->nlp_DID, ELS_CMD_ACC);
@@ -5184,8 +5269,10 @@ lpfc_els_rsp_rls_acc(struct lpfc_hba *phba, LPFC_MBOXQ_t *pmb)
 	/* Decrement the ndlp reference count from previous mbox command */
 	lpfc_nlp_put(ndlp);
 
-	if (!elsiocb)
+	if (!elsiocb) {
+		mempool_free(pmb, phba->mbox_mem_pool);
 		return;
+	}
 
 	icmd = &elsiocb->iocb;
 	icmd->ulpContext = rxid;
@@ -5202,7 +5289,7 @@ lpfc_els_rsp_rls_acc(struct lpfc_hba *phba, LPFC_MBOXQ_t *pmb)
 	rls_rsp->primSeqErrCnt = cpu_to_be32(mb->un.varRdLnk.primSeqErrCnt);
 	rls_rsp->invalidXmitWord = cpu_to_be32(mb->un.varRdLnk.invalidXmitWord);
 	rls_rsp->crcCnt = cpu_to_be32(mb->un.varRdLnk.crcCnt);
-
+	mempool_free(pmb, phba->mbox_mem_pool);
 	/* Xmit ELS RLS ACC response tag <ulpIoTag> */
 	lpfc_printf_vlog(ndlp->vport, KERN_INFO, LOG_ELS,
 			 "2874 Xmit ELS RLS ACC response tag x%x xri x%x, "
@@ -5586,7 +5673,7 @@ lpfc_issue_els_rrq(struct lpfc_vport *vport, struct lpfc_nodelist *ndlp,
 	pcmd += sizeof(uint32_t);
 	els_rrq = (struct RRQ *) pcmd;
 
-	bf_set(rrq_oxid, els_rrq, rrq->xritag);
+	bf_set(rrq_oxid, els_rrq, phba->sli4_hba.xri_ids[rrq->xritag]);
 	bf_set(rrq_rxid, els_rrq, rrq->rxid);
 	bf_set(rrq_did, els_rrq, vport->fc_myDID);
 	els_rrq->rrq = cpu_to_be32(els_rrq->rrq);
@@ -7873,7 +7960,9 @@ lpfc_sli4_els_xri_aborted(struct lpfc_hba *phba,
 			sglq_entry->state = SGL_FREED;
 			spin_unlock(&phba->sli4_hba.abts_sgl_list_lock);
 			spin_unlock_irqrestore(&phba->hbalock, iflag);
-			lpfc_set_rrq_active(phba, ndlp, xri, rxid, 1);
+			lpfc_set_rrq_active(phba, ndlp,
+				sglq_entry->sli4_lxritag,
+				rxid, 1);
 
 			/* Check if TXQ queue needs to be serviced */
 			if (pring->txq_cnt)
diff --git a/drivers/scsi/lpfc/lpfc_hbadisc.c b/drivers/scsi/lpfc/lpfc_hbadisc.c
index b507536dc5b..5bb269e224f 100644
--- a/drivers/scsi/lpfc/lpfc_hbadisc.c
+++ b/drivers/scsi/lpfc/lpfc_hbadisc.c
@@ -713,6 +713,7 @@ lpfc_do_work(void *p)
 	int rc;
 
 	set_user_nice(current, -20);
+	current->flags |= PF_NOFREEZE;
 	phba->data_flags = 0;
 
 	while (!kthread_should_stop()) {
@@ -1094,7 +1095,7 @@ lpfc_mbx_cmpl_local_config_link(struct lpfc_hba *phba, LPFC_MBOXQ_t *pmb)
 	/* Start discovery by sending a FLOGI. port_state is identically
 	 * LPFC_FLOGI while waiting for FLOGI cmpl
 	 */
-	if (vport->port_state != LPFC_FLOGI)
+	if (vport->port_state != LPFC_FLOGI || vport->fc_flag & FC_PT2PT_PLOGI)
 		lpfc_initial_flogi(vport);
 	return;
 
@@ -2881,9 +2882,14 @@ lpfc_mbx_cmpl_reg_vfi(struct lpfc_hba *phba, LPFC_MBOXQ_t *mboxq)
 	}
 
 	if (vport->port_state == LPFC_FABRIC_CFG_LINK) {
-		/* For private loop just start discovery and we are done. */
-		if ((phba->fc_topology == LPFC_TOPOLOGY_LOOP) &&
-		    !(vport->fc_flag & FC_PUBLIC_LOOP)) {
+		/*
+		 * For private loop or for NPort pt2pt,
+		 * just start discovery and we are done.
+		 */
+		if ((vport->fc_flag & FC_PT2PT) ||
+		    ((phba->fc_topology == LPFC_TOPOLOGY_LOOP) &&
+		    !(vport->fc_flag & FC_PUBLIC_LOOP))) {
+
 			/* Use loop map to make discovery list */
 			lpfc_disc_list_loopmap(vport);
 			/* Start discovery */
@@ -5490,9 +5496,9 @@ lpfc_nlp_release(struct kref *kref)
 		ndlp->nlp_DID, ndlp->nlp_flag, ndlp->nlp_type);
 
 	lpfc_printf_vlog(ndlp->vport, KERN_INFO, LOG_NODE,
-			"0279 lpfc_nlp_release: ndlp:x%p "
+			"0279 lpfc_nlp_release: ndlp:x%p did %x "
 			"usgmap:x%x refcnt:%d\n",
-			(void *)ndlp, ndlp->nlp_usg_map,
+			(void *)ndlp, ndlp->nlp_DID, ndlp->nlp_usg_map,
 			atomic_read(&ndlp->kref.refcount));
 
 	/* remove ndlp from action. */
diff --git a/drivers/scsi/lpfc/lpfc_hw.h b/drivers/scsi/lpfc/lpfc_hw.h
index 5f280b5ae3d..41bb1d2fb62 100644
--- a/drivers/scsi/lpfc/lpfc_hw.h
+++ b/drivers/scsi/lpfc/lpfc_hw.h
@@ -3374,6 +3374,9 @@ typedef struct {
 	WORD5 w5;		/* Header control/status word */
 } XMT_SEQ_FIELDS64;
 
+/* This word is remote ports D_ID for XMIT_ELS_RSP64 */
+#define xmit_els_remoteID xrsqRo
+
 /* IOCB Command template for 64 bit RCV_SEQUENCE64 */
 typedef struct {
 	struct ulp_bde64 rcvBde;
diff --git a/drivers/scsi/lpfc/lpfc_hw4.h b/drivers/scsi/lpfc/lpfc_hw4.h
index 91f09761bd3..f1946dfda5b 100644
--- a/drivers/scsi/lpfc/lpfc_hw4.h
+++ b/drivers/scsi/lpfc/lpfc_hw4.h
@@ -228,19 +228,15 @@ struct lpfc_sli4_flags {
 #define lpfc_idx_rsrc_rdy_MASK		0x00000001
 #define lpfc_idx_rsrc_rdy_WORD		word0
 #define LPFC_IDX_RSRC_RDY		1
-#define lpfc_xri_rsrc_rdy_SHIFT		1
-#define lpfc_xri_rsrc_rdy_MASK		0x00000001
-#define lpfc_xri_rsrc_rdy_WORD		word0
-#define LPFC_XRI_RSRC_RDY		1
-#define lpfc_rpi_rsrc_rdy_SHIFT		2
+#define lpfc_rpi_rsrc_rdy_SHIFT		1
 #define lpfc_rpi_rsrc_rdy_MASK		0x00000001
 #define lpfc_rpi_rsrc_rdy_WORD		word0
 #define LPFC_RPI_RSRC_RDY		1
-#define lpfc_vpi_rsrc_rdy_SHIFT		3
+#define lpfc_vpi_rsrc_rdy_SHIFT		2
 #define lpfc_vpi_rsrc_rdy_MASK		0x00000001
 #define lpfc_vpi_rsrc_rdy_WORD		word0
 #define LPFC_VPI_RSRC_RDY		1
-#define lpfc_vfi_rsrc_rdy_SHIFT		4
+#define lpfc_vfi_rsrc_rdy_SHIFT		3
 #define lpfc_vfi_rsrc_rdy_MASK		0x00000001
 #define lpfc_vfi_rsrc_rdy_WORD		word0
 #define LPFC_VFI_RSRC_RDY		1
@@ -3299,7 +3295,13 @@ struct els_request64_wqe {
 struct xmit_els_rsp64_wqe {
 	struct ulp_bde64 bde;
 	uint32_t response_payload_len;
-	uint32_t rsvd4;
+	uint32_t word4;
+#define els_rsp64_sid_SHIFT         0
+#define els_rsp64_sid_MASK          0x00FFFFFF
+#define els_rsp64_sid_WORD          word4
+#define els_rsp64_sp_SHIFT          24
+#define els_rsp64_sp_MASK           0x00000001
+#define els_rsp64_sp_WORD           word4
 	struct wqe_did wqe_dest;
 	struct wqe_common wqe_com; /* words 6-11 */
 	uint32_t word12;
diff --git a/drivers/scsi/lpfc/lpfc_init.c b/drivers/scsi/lpfc/lpfc_init.c
index 9598fdcb08a..411ed48d79d 100644
--- a/drivers/scsi/lpfc/lpfc_init.c
+++ b/drivers/scsi/lpfc/lpfc_init.c
@@ -64,8 +64,8 @@ static int lpfc_sli4_queue_verify(struct lpfc_hba *);
 static int lpfc_create_bootstrap_mbox(struct lpfc_hba *);
 static int lpfc_setup_endian_order(struct lpfc_hba *);
 static void lpfc_destroy_bootstrap_mbox(struct lpfc_hba *);
-static void lpfc_free_sgl_list(struct lpfc_hba *);
-static int lpfc_init_sgl_list(struct lpfc_hba *);
+static void lpfc_free_els_sgl_list(struct lpfc_hba *);
+static void lpfc_init_sgl_list(struct lpfc_hba *);
 static int lpfc_init_active_sgl_array(struct lpfc_hba *);
 static void lpfc_free_active_sgl(struct lpfc_hba *);
 static int lpfc_hba_down_post_s3(struct lpfc_hba *phba);
@@ -2767,47 +2767,14 @@ lpfc_offline(struct lpfc_hba *phba)
 }
 
 /**
- * lpfc_scsi_buf_update - Update the scsi_buffers that are already allocated.
- * @phba: pointer to lpfc hba data structure.
- *
- * This routine goes through all the scsi buffers in the system and updates the
- * Physical XRIs assigned to the SCSI buffer because these may change after any
- * firmware reset
- *
- * Return codes
- *   0 - successful (for now, it always returns 0)
- **/
-int
-lpfc_scsi_buf_update(struct lpfc_hba *phba)
-{
-	struct lpfc_scsi_buf *sb, *sb_next;
-
-	spin_lock_irq(&phba->hbalock);
-	spin_lock(&phba->scsi_buf_list_lock);
-	list_for_each_entry_safe(sb, sb_next, &phba->lpfc_scsi_buf_list, list) {
-		sb->cur_iocbq.sli4_xritag =
-			phba->sli4_hba.xri_ids[sb->cur_iocbq.sli4_lxritag];
-		set_bit(sb->cur_iocbq.sli4_lxritag, phba->sli4_hba.xri_bmask);
-		phba->sli4_hba.max_cfg_param.xri_used++;
-		phba->sli4_hba.xri_count++;
-	}
-	spin_unlock(&phba->scsi_buf_list_lock);
-	spin_unlock_irq(&phba->hbalock);
-	return 0;
-}
-
-/**
  * lpfc_scsi_free - Free all the SCSI buffers and IOCBs from driver lists
  * @phba: pointer to lpfc hba data structure.
  *
  * This routine is to free all the SCSI buffers and IOCBs from the driver
  * list back to kernel. It is called from lpfc_pci_remove_one to free
  * the internal resources before the device is removed from the system.
- *
- * Return codes
- *   0 - successful (for now, it always returns 0)
  **/
-static int
+static void
 lpfc_scsi_free(struct lpfc_hba *phba)
 {
 	struct lpfc_scsi_buf *sb, *sb_next;
@@ -2833,7 +2800,178 @@ lpfc_scsi_free(struct lpfc_hba *phba)
 	}
 
 	spin_unlock_irq(&phba->hbalock);
+}
+
+/**
+ * lpfc_sli4_xri_sgl_update - update xri-sgl sizing and mapping
+ * @phba: pointer to lpfc hba data structure.
+ *
+ * This routine first calculates the sizes of the current els and allocated
+ * scsi sgl lists, and then goes through all sgls to updates the physical
+ * XRIs assigned due to port function reset. During port initialization, the
+ * current els and allocated scsi sgl lists are 0s.
+ *
+ * Return codes
+ *   0 - successful (for now, it always returns 0)
+ **/
+int
+lpfc_sli4_xri_sgl_update(struct lpfc_hba *phba)
+{
+	struct lpfc_sglq *sglq_entry = NULL, *sglq_entry_next = NULL;
+	struct lpfc_scsi_buf *psb = NULL, *psb_next = NULL;
+	uint16_t i, lxri, xri_cnt, els_xri_cnt, scsi_xri_cnt;
+	LIST_HEAD(els_sgl_list);
+	LIST_HEAD(scsi_sgl_list);
+	int rc;
+
+	/*
+	 * update on pci function's els xri-sgl list
+	 */
+	els_xri_cnt = lpfc_sli4_get_els_iocb_cnt(phba);
+	if (els_xri_cnt > phba->sli4_hba.els_xri_cnt) {
+		/* els xri-sgl expanded */
+		xri_cnt = els_xri_cnt - phba->sli4_hba.els_xri_cnt;
+		lpfc_printf_log(phba, KERN_INFO, LOG_SLI,
+				"3157 ELS xri-sgl count increased from "
+				"%d to %d\n", phba->sli4_hba.els_xri_cnt,
+				els_xri_cnt);
+		/* allocate the additional els sgls */
+		for (i = 0; i < xri_cnt; i++) {
+			sglq_entry = kzalloc(sizeof(struct lpfc_sglq),
+					     GFP_KERNEL);
+			if (sglq_entry == NULL) {
+				lpfc_printf_log(phba, KERN_ERR, LOG_SLI,
+						"2562 Failure to allocate an "
+						"ELS sgl entry:%d\n", i);
+				rc = -ENOMEM;
+				goto out_free_mem;
+			}
+			sglq_entry->buff_type = GEN_BUFF_TYPE;
+			sglq_entry->virt = lpfc_mbuf_alloc(phba, 0,
+							   &sglq_entry->phys);
+			if (sglq_entry->virt == NULL) {
+				kfree(sglq_entry);
+				lpfc_printf_log(phba, KERN_ERR, LOG_SLI,
+						"2563 Failure to allocate an "
+						"ELS mbuf:%d\n", i);
+				rc = -ENOMEM;
+				goto out_free_mem;
+			}
+			sglq_entry->sgl = sglq_entry->virt;
+			memset(sglq_entry->sgl, 0, LPFC_BPL_SIZE);
+			sglq_entry->state = SGL_FREED;
+			list_add_tail(&sglq_entry->list, &els_sgl_list);
+		}
+		spin_lock(&phba->hbalock);
+		list_splice_init(&els_sgl_list, &phba->sli4_hba.lpfc_sgl_list);
+		spin_unlock(&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;
+		lpfc_printf_log(phba, KERN_INFO, LOG_SLI,
+				"3158 ELS xri-sgl count decreased from "
+				"%d to %d\n", phba->sli4_hba.els_xri_cnt,
+				els_xri_cnt);
+		spin_lock_irq(&phba->hbalock);
+		list_splice_init(&phba->sli4_hba.lpfc_sgl_list, &els_sgl_list);
+		spin_unlock_irq(&phba->hbalock);
+		/* release extra els sgls from list */
+		for (i = 0; i < xri_cnt; i++) {
+			list_remove_head(&els_sgl_list,
+					 sglq_entry, struct lpfc_sglq, list);
+			if (sglq_entry) {
+				lpfc_mbuf_free(phba, sglq_entry->virt,
+					       sglq_entry->phys);
+				kfree(sglq_entry);
+			}
+		}
+		spin_lock_irq(&phba->hbalock);
+		list_splice_init(&els_sgl_list, &phba->sli4_hba.lpfc_sgl_list);
+		spin_unlock_irq(&phba->hbalock);
+	} else
+		lpfc_printf_log(phba, KERN_INFO, LOG_SLI,
+				"3163 ELS xri-sgl count unchanged: %d\n",
+				els_xri_cnt);
+	phba->sli4_hba.els_xri_cnt = els_xri_cnt;
+
+	/* update xris to els sgls on the list */
+	sglq_entry = NULL;
+	sglq_entry_next = NULL;
+	list_for_each_entry_safe(sglq_entry, sglq_entry_next,
+				 &phba->sli4_hba.lpfc_sgl_list, list) {
+		lxri = lpfc_sli4_next_xritag(phba);
+		if (lxri == NO_XRI) {
+			lpfc_printf_log(phba, KERN_ERR, LOG_SLI,
+					"2400 Failed to allocate xri for "
+					"ELS sgl\n");
+			rc = -ENOMEM;
+			goto out_free_mem;
+		}
+		sglq_entry->sli4_lxritag = lxri;
+		sglq_entry->sli4_xritag = phba->sli4_hba.xri_ids[lxri];
+	}
+
+	/*
+	 * update on pci function's allocated scsi xri-sgl list
+	 */
+	phba->total_scsi_bufs = 0;
+
+	/* maximum number of xris available for scsi buffers */
+	phba->sli4_hba.scsi_xri_max = phba->sli4_hba.max_cfg_param.max_xri -
+				      els_xri_cnt;
+
+	lpfc_printf_log(phba, KERN_INFO, LOG_SLI,
+			"2401 Current allocated SCSI xri-sgl count:%d, "
+			"maximum  SCSI xri count:%d\n",
+			phba->sli4_hba.scsi_xri_cnt,
+			phba->sli4_hba.scsi_xri_max);
+
+	spin_lock_irq(&phba->scsi_buf_list_lock);
+	list_splice_init(&phba->lpfc_scsi_buf_list, &scsi_sgl_list);
+	spin_unlock_irq(&phba->scsi_buf_list_lock);
+
+	if (phba->sli4_hba.scsi_xri_cnt > phba->sli4_hba.scsi_xri_max) {
+		/* max scsi xri shrinked below the allocated scsi buffers */
+		scsi_xri_cnt = phba->sli4_hba.scsi_xri_cnt -
+					phba->sli4_hba.scsi_xri_max;
+		/* release the extra allocated scsi buffers */
+		for (i = 0; i < scsi_xri_cnt; i++) {
+			list_remove_head(&scsi_sgl_list, psb,
+					 struct lpfc_scsi_buf, list);
+			pci_pool_free(phba->lpfc_scsi_dma_buf_pool, psb->data,
+				      psb->dma_handle);
+			kfree(psb);
+		}
+		spin_lock_irq(&phba->scsi_buf_list_lock);
+		phba->sli4_hba.scsi_xri_cnt -= scsi_xri_cnt;
+		spin_unlock_irq(&phba->scsi_buf_list_lock);
+	}
+
+	/* update xris associated to remaining allocated scsi buffers */
+	psb = NULL;
+	psb_next = NULL;
+	list_for_each_entry_safe(psb, psb_next, &scsi_sgl_list, list) {
+		lxri = lpfc_sli4_next_xritag(phba);
+		if (lxri == NO_XRI) {
+			lpfc_printf_log(phba, KERN_ERR, LOG_SLI,
+					"2560 Failed to allocate xri for "
+					"scsi buffer\n");
+			rc = -ENOMEM;
+			goto out_free_mem;
+		}
+		psb->cur_iocbq.sli4_lxritag = lxri;
+		psb->cur_iocbq.sli4_xritag = phba->sli4_hba.xri_ids[lxri];
+	}
+	spin_lock(&phba->scsi_buf_list_lock);
+	list_splice_init(&scsi_sgl_list, &phba->lpfc_scsi_buf_list);
+	spin_unlock(&phba->scsi_buf_list_lock);
+
 	return 0;
+
+out_free_mem:
+	lpfc_free_els_sgl_list(phba);
+	lpfc_scsi_free(phba);
+	return rc;
 }
 
 /**
@@ -4636,18 +4774,15 @@ lpfc_sli4_driver_resource_setup(struct lpfc_hba *phba)
 	if (rc)
 		goto out_free_bsmbx;
 
-	/* Initialize and populate the iocb list per host */
-	rc = lpfc_init_sgl_list(phba);
-	if (rc) {
-		lpfc_printf_log(phba, KERN_ERR, LOG_INIT,
-				"1400 Failed to initialize sgl list.\n");
-		goto out_destroy_cq_event_pool;
-	}
+	/* Initialize sgl lists per host */
+	lpfc_init_sgl_list(phba);
+
+	/* Allocate and initialize active sgl array */
 	rc = lpfc_init_active_sgl_array(phba);
 	if (rc) {
 		lpfc_printf_log(phba, KERN_ERR, LOG_INIT,
 				"1430 Failed to initialize sgl list.\n");
-		goto out_free_sgl_list;
+		goto out_destroy_cq_event_pool;
 	}
 	rc = lpfc_sli4_init_rpi_hdrs(phba);
 	if (rc) {
@@ -4722,8 +4857,6 @@ out_remove_rpi_hdrs:
 	lpfc_sli4_remove_rpi_hdrs(phba);
 out_free_active_sgl:
 	lpfc_free_active_sgl(phba);
-out_free_sgl_list:
-	lpfc_free_sgl_list(phba);
 out_destroy_cq_event_pool:
 	lpfc_sli4_cq_event_pool_destroy(phba);
 out_free_bsmbx:
@@ -4760,10 +4893,7 @@ lpfc_sli4_driver_resource_unset(struct lpfc_hba *phba)
 
 	/* Free the ELS sgl list */
 	lpfc_free_active_sgl(phba);
-	lpfc_free_sgl_list(phba);
-
-	/* Free the SCSI sgl management array */
-	kfree(phba->sli4_hba.lpfc_scsi_psb_array);
+	lpfc_free_els_sgl_list(phba);
 
 	/* Free the completion queue EQ event pool */
 	lpfc_sli4_cq_event_release_all(phba);
@@ -4990,29 +5120,42 @@ out_free_iocbq:
 }
 
 /**
- * lpfc_free_sgl_list - Free sgl list.
+ * lpfc_free_sgl_list - Free a given sgl list.
  * @phba: pointer to lpfc hba data structure.
+ * @sglq_list: pointer to the head of sgl list.
  *
- * This routine is invoked to free the driver's sgl list and memory.
+ * This routine is invoked to free a give sgl list and memory.
  **/
-static void
-lpfc_free_sgl_list(struct lpfc_hba *phba)
+void
+lpfc_free_sgl_list(struct lpfc_hba *phba, struct list_head *sglq_list)
 {
 	struct lpfc_sglq *sglq_entry = NULL, *sglq_next = NULL;
+
+	list_for_each_entry_safe(sglq_entry, sglq_next, sglq_list, list) {
+		list_del(&sglq_entry->list);
+		lpfc_mbuf_free(phba, sglq_entry->virt, sglq_entry->phys);
+		kfree(sglq_entry);
+	}
+}
+
+/**
+ * lpfc_free_els_sgl_list - Free els sgl list.
+ * @phba: pointer to lpfc hba data structure.
+ *
+ * This routine is invoked to free the driver's els sgl list and memory.
+ **/
+static void
+lpfc_free_els_sgl_list(struct lpfc_hba *phba)
+{
 	LIST_HEAD(sglq_list);
 
+	/* Retrieve all els sgls from driver list */
 	spin_lock_irq(&phba->hbalock);
 	list_splice_init(&phba->sli4_hba.lpfc_sgl_list, &sglq_list);
 	spin_unlock_irq(&phba->hbalock);
 
-	list_for_each_entry_safe(sglq_entry, sglq_next,
-				 &sglq_list, list) {
-		list_del(&sglq_entry->list);
-		lpfc_mbuf_free(phba, sglq_entry->virt, sglq_entry->phys);
-		kfree(sglq_entry);
-		phba->sli4_hba.total_sglq_bufs--;
-	}
-	kfree(phba->sli4_hba.lpfc_els_sgl_array);
+	/* Now free the sgl list */
+	lpfc_free_sgl_list(phba, &sglq_list);
 }
 
 /**
@@ -5057,99 +5200,19 @@ lpfc_free_active_sgl(struct lpfc_hba *phba)
  * This routine is invoked to allocate and initizlize the driver's sgl
  * list and set up the sgl xritag tag array accordingly.
  *
- * Return codes
- *	0 - successful
- *	other values - error
  **/
-static int
+static void
 lpfc_init_sgl_list(struct lpfc_hba *phba)
 {
-	struct lpfc_sglq *sglq_entry = NULL;
-	int i;
-	int els_xri_cnt;
-
-	els_xri_cnt = lpfc_sli4_get_els_iocb_cnt(phba);
-	lpfc_printf_log(phba, KERN_INFO, LOG_SLI,
-				"2400 ELS XRI count %d.\n",
-				els_xri_cnt);
 	/* Initialize and populate the sglq list per host/VF. */
 	INIT_LIST_HEAD(&phba->sli4_hba.lpfc_sgl_list);
 	INIT_LIST_HEAD(&phba->sli4_hba.lpfc_abts_els_sgl_list);
 
-	/* Sanity check on XRI management */
-	if (phba->sli4_hba.max_cfg_param.max_xri <= els_xri_cnt) {
-		lpfc_printf_log(phba, KERN_ERR, LOG_SLI,
-				"2562 No room left for SCSI XRI allocation: "
-				"max_xri=%d, els_xri=%d\n",
-				phba->sli4_hba.max_cfg_param.max_xri,
-				els_xri_cnt);
-		return -ENOMEM;
-	}
-
-	/* Allocate memory for the ELS XRI management array */
-	phba->sli4_hba.lpfc_els_sgl_array =
-			kzalloc((sizeof(struct lpfc_sglq *) * els_xri_cnt),
-			GFP_KERNEL);
-
-	if (!phba->sli4_hba.lpfc_els_sgl_array) {
-		lpfc_printf_log(phba, KERN_ERR, LOG_SLI,
-				"2401 Failed to allocate memory for ELS "
-				"XRI management array of size %d.\n",
-				els_xri_cnt);
-		return -ENOMEM;
-	}
+	/* els xri-sgl book keeping */
+	phba->sli4_hba.els_xri_cnt = 0;
 
-	/* Keep the SCSI XRI into the XRI management array */
-	phba->sli4_hba.scsi_xri_max =
-			phba->sli4_hba.max_cfg_param.max_xri - els_xri_cnt;
+	/* scsi xri-buffer book keeping */
 	phba->sli4_hba.scsi_xri_cnt = 0;
-	phba->sli4_hba.lpfc_scsi_psb_array =
-			kzalloc((sizeof(struct lpfc_scsi_buf *) *
-			phba->sli4_hba.scsi_xri_max), GFP_KERNEL);
-
-	if (!phba->sli4_hba.lpfc_scsi_psb_array) {
-		lpfc_printf_log(phba, KERN_ERR, LOG_SLI,
-				"2563 Failed to allocate memory for SCSI "
-				"XRI management array of size %d.\n",
-				phba->sli4_hba.scsi_xri_max);
-		kfree(phba->sli4_hba.lpfc_els_sgl_array);
-		return -ENOMEM;
-	}
-
-	for (i = 0; i < els_xri_cnt; i++) {
-		sglq_entry = kzalloc(sizeof(struct lpfc_sglq), GFP_KERNEL);
-		if (sglq_entry == NULL) {
-			printk(KERN_ERR "%s: only allocated %d sgls of "
-				"expected %d count. Unloading driver.\n",
-				__func__, i, els_xri_cnt);
-			goto out_free_mem;
-		}
-
-		sglq_entry->buff_type = GEN_BUFF_TYPE;
-		sglq_entry->virt = lpfc_mbuf_alloc(phba, 0, &sglq_entry->phys);
-		if (sglq_entry->virt == NULL) {
-			kfree(sglq_entry);
-			printk(KERN_ERR "%s: failed to allocate mbuf.\n"
-				"Unloading driver.\n", __func__);
-			goto out_free_mem;
-		}
-		sglq_entry->sgl = sglq_entry->virt;
-		memset(sglq_entry->sgl, 0, LPFC_BPL_SIZE);
-
-		/* The list order is used by later block SGL registraton */
-		spin_lock_irq(&phba->hbalock);
-		sglq_entry->state = SGL_FREED;
-		list_add_tail(&sglq_entry->list, &phba->sli4_hba.lpfc_sgl_list);
-		phba->sli4_hba.lpfc_els_sgl_array[i] = sglq_entry;
-		phba->sli4_hba.total_sglq_bufs++;
-		spin_unlock_irq(&phba->hbalock);
-	}
-	return 0;
-
-out_free_mem:
-	kfree(phba->sli4_hba.lpfc_scsi_psb_array);
-	lpfc_free_sgl_list(phba);
-	return -ENOMEM;
 }
 
 /**
@@ -7320,9 +7383,11 @@ lpfc_pci_function_reset(struct lpfc_hba *phba)
 					phba->sli4_hba.u.if_type2.ERR2regaddr);
 				lpfc_printf_log(phba, KERN_ERR, LOG_INIT,
 					"2890 Port error detected during port "
-					"reset(%d): port status reg 0x%x, "
+					"reset(%d): wait_tmo:%d ms, "
+					"port status reg 0x%x, "
 					"error 1=0x%x, error 2=0x%x\n",
-					num_resets, reg_data.word0,
+					num_resets, rdy_chk*10,
+					reg_data.word0,
 					phba->work_status[0],
 					phba->work_status[1]);
 				rc = -ENODEV;
@@ -8694,8 +8759,11 @@ lpfc_pci_remove_one_s3(struct pci_dev *pdev)
 	/* Release all the vports against this physical port */
 	vports = lpfc_create_vport_work_array(phba);
 	if (vports != NULL)
-		for (i = 1; i <= phba->max_vports && vports[i] != NULL; i++)
+		for (i = 0; i <= phba->max_vports && vports[i] != NULL; i++) {
+			if (vports[i]->port_type == LPFC_PHYSICAL_PORT)
+				continue;
 			fc_vport_terminate(vports[i]->fc_vport);
+		}
 	lpfc_destroy_vport_work_array(phba, vports);
 
 	/* Remove FC host and then SCSI host with the physical port */
@@ -9115,8 +9183,12 @@ lpfc_sli4_get_els_iocb_cnt(struct lpfc_hba *phba)
 			return 50;
 		else if (max_xri <= 1024)
 			return 100;
-		else
+		else if (max_xri <= 1536)
 			return 150;
+		else if (max_xri <= 2048)
+			return 200;
+		else
+			return 250;
 	} else
 		return 0;
 }
@@ -9455,8 +9527,11 @@ lpfc_pci_remove_one_s4(struct pci_dev *pdev)
 	/* Release all the vports against this physical port */
 	vports = lpfc_create_vport_work_array(phba);
 	if (vports != NULL)
-		for (i = 1; i <= phba->max_vports && vports[i] != NULL; i++)
+		for (i = 0; i <= phba->max_vports && vports[i] != NULL; i++) {
+			if (vports[i]->port_type == LPFC_PHYSICAL_PORT)
+				continue;
 			fc_vport_terminate(vports[i]->fc_vport);
+		}
 	lpfc_destroy_vport_work_array(phba, vports);
 
 	/* Remove FC host and then SCSI host with the physical port */
diff --git a/drivers/scsi/lpfc/lpfc_nportdisc.c b/drivers/scsi/lpfc/lpfc_nportdisc.c
index 15ca2a9a0cd..9133a97f045 100644
--- a/drivers/scsi/lpfc/lpfc_nportdisc.c
+++ b/drivers/scsi/lpfc/lpfc_nportdisc.c
@@ -367,8 +367,10 @@ lpfc_rcv_plogi(struct lpfc_vport *vport, struct lpfc_nodelist *ndlp,
 		return 1;
 	}
 
+	/* Check for Nport to NPort pt2pt protocol */
 	if ((vport->fc_flag & FC_PT2PT) &&
 	    !(vport->fc_flag & FC_PT2PT_PLOGI)) {
+
 		/* rcv'ed PLOGI decides what our NPortId will be */
 		vport->fc_myDID = icmd->un.rcvels.parmRo;
 		mbox = mempool_alloc(phba->mbox_mem_pool, GFP_KERNEL);
@@ -382,6 +384,13 @@ lpfc_rcv_plogi(struct lpfc_vport *vport, struct lpfc_nodelist *ndlp,
 			mempool_free(mbox, phba->mbox_mem_pool);
 			goto out;
 		}
+		/*
+		 * For SLI4, the VFI/VPI are registered AFTER the
+		 * Nport with the higher WWPN sends us a PLOGI with
+		 * our assigned NPortId.
+		 */
+		if (phba->sli_rev == LPFC_SLI_REV4)
+			lpfc_issue_reg_vfi(vport);
 
 		lpfc_can_disctmo(vport);
 	}
diff --git a/drivers/scsi/lpfc/lpfc_scsi.c b/drivers/scsi/lpfc/lpfc_scsi.c
index 88f3a83dbd2..66e09069f28 100644
--- a/drivers/scsi/lpfc/lpfc_scsi.c
+++ b/drivers/scsi/lpfc/lpfc_scsi.c
@@ -399,6 +399,14 @@ lpfc_ramp_down_queue_handler(struct lpfc_hba *phba)
 	num_rsrc_err = atomic_read(&phba->num_rsrc_err);
 	num_cmd_success = atomic_read(&phba->num_cmd_success);
 
+	/*
+	 * The error and success command counters are global per
+	 * driver instance.  If another handler has already
+	 * operated on this error event, just exit.
+	 */
+	if (num_rsrc_err == 0)
+		return;
+
 	vports = lpfc_create_vport_work_array(phba);
 	if (vports != NULL)
 		for (i = 0; i <= phba->max_vports && vports[i] != NULL; i++) {
@@ -688,7 +696,8 @@ lpfc_sli4_fcp_xri_aborted(struct lpfc_hba *phba,
 			rrq_empty = list_empty(&phba->active_rrq_list);
 			spin_unlock_irqrestore(&phba->hbalock, iflag);
 			if (ndlp) {
-				lpfc_set_rrq_active(phba, ndlp, xri, rxid, 1);
+				lpfc_set_rrq_active(phba, ndlp,
+					psb->cur_iocbq.sli4_lxritag, rxid, 1);
 				lpfc_sli4_abts_err_handler(phba, ndlp, axri);
 			}
 			lpfc_release_scsi_buf_s4(phba, psb);
@@ -718,72 +727,162 @@ lpfc_sli4_fcp_xri_aborted(struct lpfc_hba *phba,
 }
 
 /**
- * lpfc_sli4_repost_scsi_sgl_list - Repsot the Scsi buffers sgl pages as block
+ * lpfc_sli4_post_scsi_sgl_list - Psot blocks of scsi buffer sgls from a list
  * @phba: pointer to lpfc hba data structure.
+ * @post_sblist: pointer to the scsi buffer list.
  *
- * This routine walks the list of scsi buffers that have been allocated and
- * repost them to the HBA by using SGL block post. This is needed after a
- * pci_function_reset/warm_start or start. The lpfc_hba_down_post_s4 routine
- * is responsible for moving all scsi buffers on the lpfc_abts_scsi_sgl_list
- * to the lpfc_scsi_buf_list. If the repost fails, reject all scsi buffers.
+ * This routine walks a list of scsi buffers that was passed in. It attempts
+ * to construct blocks of scsi buffer sgls which contains contiguous xris and
+ * uses the non-embedded SGL block post mailbox commands to post to the port.
+ * For single SCSI buffer sgl with non-contiguous xri, if any, it shall use
+ * embedded SGL post mailbox command for posting. The @post_sblist passed in
+ * must be local list, thus no lock is needed when manipulate the list.
  *
- * Returns: 0 = success, non-zero failure.
+ * Returns: 0 = failure, non-zero number of successfully posted buffers.
  **/
 int
-lpfc_sli4_repost_scsi_sgl_list(struct lpfc_hba *phba)
+lpfc_sli4_post_scsi_sgl_list(struct lpfc_hba *phba,
+			     struct list_head *post_sblist, int sb_count)
 {
-	struct lpfc_scsi_buf *psb;
-	int index, status, bcnt = 0, rcnt = 0, rc = 0;
-	LIST_HEAD(sblist);
-
-	for (index = 0; index < phba->sli4_hba.scsi_xri_cnt; index++) {
-		psb = phba->sli4_hba.lpfc_scsi_psb_array[index];
-		if (psb) {
-			/* Remove from SCSI buffer list */
-			list_del(&psb->list);
-			/* Add it to a local SCSI buffer list */
-			list_add_tail(&psb->list, &sblist);
-			if (++rcnt == LPFC_NEMBED_MBOX_SGL_CNT) {
-				bcnt = rcnt;
-				rcnt = 0;
+	struct lpfc_scsi_buf *psb, *psb_next;
+	int status;
+	int post_cnt = 0, block_cnt = 0, num_posting = 0, num_posted = 0;
+	dma_addr_t pdma_phys_bpl1;
+	int last_xritag = NO_XRI;
+	LIST_HEAD(prep_sblist);
+	LIST_HEAD(blck_sblist);
+	LIST_HEAD(scsi_sblist);
+
+	/* sanity check */
+	if (sb_count <= 0)
+		return -EINVAL;
+
+	list_for_each_entry_safe(psb, psb_next, post_sblist, list) {
+		list_del_init(&psb->list);
+		block_cnt++;
+		if ((last_xritag != NO_XRI) &&
+		    (psb->cur_iocbq.sli4_xritag != last_xritag + 1)) {
+			/* a hole in xri block, form a sgl posting block */
+			list_splice_init(&prep_sblist, &blck_sblist);
+			post_cnt = block_cnt - 1;
+			/* prepare list for next posting block */
+			list_add_tail(&psb->list, &prep_sblist);
+			block_cnt = 1;
+		} else {
+			/* prepare list for next posting block */
+			list_add_tail(&psb->list, &prep_sblist);
+			/* enough sgls for non-embed sgl mbox command */
+			if (block_cnt == LPFC_NEMBED_MBOX_SGL_CNT) {
+				list_splice_init(&prep_sblist, &blck_sblist);
+				post_cnt = block_cnt;
+				block_cnt = 0;
 			}
-		} else
-			/* A hole present in the XRI array, need to skip */
-			bcnt = rcnt;
+		}
+		num_posting++;
+		last_xritag = psb->cur_iocbq.sli4_xritag;
 
-		if (index == phba->sli4_hba.scsi_xri_cnt - 1)
-			/* End of XRI array for SCSI buffer, complete */
-			bcnt = rcnt;
+		/* end of repost sgl list condition for SCSI buffers */
+		if (num_posting == sb_count) {
+			if (post_cnt == 0) {
+				/* last sgl posting block */
+				list_splice_init(&prep_sblist, &blck_sblist);
+				post_cnt = block_cnt;
+			} else if (block_cnt == 1) {
+				/* last single sgl with non-contiguous xri */
+				if (phba->cfg_sg_dma_buf_size > SGL_PAGE_SIZE)
+					pdma_phys_bpl1 = psb->dma_phys_bpl +
+								SGL_PAGE_SIZE;
+				else
+					pdma_phys_bpl1 = 0;
+				status = lpfc_sli4_post_sgl(phba,
+						psb->dma_phys_bpl,
+						pdma_phys_bpl1,
+						psb->cur_iocbq.sli4_xritag);
+				if (status) {
+					/* failure, put on abort scsi list */
+					psb->exch_busy = 1;
+				} else {
+					/* success, put on SCSI buffer list */
+					psb->exch_busy = 0;
+					psb->status = IOSTAT_SUCCESS;
+					num_posted++;
+				}
+				/* success, put on SCSI buffer sgl list */
+				list_add_tail(&psb->list, &scsi_sblist);
+			}
+		}
 
-		/* Continue until collect up to a nembed page worth of sgls */
-		if (bcnt == 0)
+		/* continue until a nembed page worth of sgls */
+		if (post_cnt == 0)
 			continue;
-		/* Now, post the SCSI buffer list sgls as a block */
-		if (!phba->sli4_hba.extents_in_use)
-			status = lpfc_sli4_post_scsi_sgl_block(phba,
-							&sblist,
-							bcnt);
-		else
-			status = lpfc_sli4_post_scsi_sgl_blk_ext(phba,
-							&sblist,
-							bcnt);
-		/* Reset SCSI buffer count for next round of posting */
-		bcnt = 0;
-		while (!list_empty(&sblist)) {
-			list_remove_head(&sblist, psb, struct lpfc_scsi_buf,
-					 list);
+
+		/* post block of SCSI buffer list sgls */
+		status = lpfc_sli4_post_scsi_sgl_block(phba, &blck_sblist,
+						       post_cnt);
+
+		/* don't reset xirtag due to hole in xri block */
+		if (block_cnt == 0)
+			last_xritag = NO_XRI;
+
+		/* reset SCSI buffer post count for next round of posting */
+		post_cnt = 0;
+
+		/* put posted SCSI buffer-sgl posted on SCSI buffer sgl list */
+		while (!list_empty(&blck_sblist)) {
+			list_remove_head(&blck_sblist, psb,
+					 struct lpfc_scsi_buf, list);
 			if (status) {
-				/* Put this back on the abort scsi list */
+				/* failure, put on abort scsi list */
 				psb->exch_busy = 1;
-				rc++;
 			} else {
+				/* success, put on SCSI buffer list */
 				psb->exch_busy = 0;
 				psb->status = IOSTAT_SUCCESS;
+				num_posted++;
 			}
-			/* Put it back into the SCSI buffer list */
-			lpfc_release_scsi_buf_s4(phba, psb);
+			list_add_tail(&psb->list, &scsi_sblist);
 		}
 	}
+	/* Push SCSI buffers with sgl posted to the availble list */
+	while (!list_empty(&scsi_sblist)) {
+		list_remove_head(&scsi_sblist, psb,
+				 struct lpfc_scsi_buf, list);
+		lpfc_release_scsi_buf_s4(phba, psb);
+	}
+	return num_posted;
+}
+
+/**
+ * lpfc_sli4_repost_scsi_sgl_list - Repsot all the allocated scsi buffer sgls
+ * @phba: pointer to lpfc hba data structure.
+ *
+ * This routine walks the list of scsi buffers that have been allocated and
+ * repost them to the port by using SGL block post. This is needed after a
+ * pci_function_reset/warm_start or start. The lpfc_hba_down_post_s4 routine
+ * is responsible for moving all scsi buffers on the lpfc_abts_scsi_sgl_list
+ * to the lpfc_scsi_buf_list. If the repost fails, reject all scsi buffers.
+ *
+ * Returns: 0 = success, non-zero failure.
+ **/
+int
+lpfc_sli4_repost_scsi_sgl_list(struct lpfc_hba *phba)
+{
+	LIST_HEAD(post_sblist);
+	int num_posted, rc = 0;
+
+	/* get all SCSI buffers need to repost to a local list */
+	spin_lock(&phba->scsi_buf_list_lock);
+	list_splice_init(&phba->lpfc_scsi_buf_list, &post_sblist);
+	spin_unlock(&phba->scsi_buf_list_lock);
+
+	/* post the list of scsi buffer sgls to port if available */
+	if (!list_empty(&post_sblist)) {
+		num_posted = lpfc_sli4_post_scsi_sgl_list(phba, &post_sblist,
+						phba->sli4_hba.scsi_xri_cnt);
+		/* failed to post any scsi buffer, return error */
+		if (num_posted == 0)
+			rc = -EIO;
+	}
 	return rc;
 }
 
@@ -792,12 +891,13 @@ lpfc_sli4_repost_scsi_sgl_list(struct lpfc_hba *phba)
  * @vport: The virtual port for which this call being executed.
  * @num_to_allocate: The requested number of buffers to allocate.
  *
- * This routine allocates a scsi buffer for device with SLI-4 interface spec,
+ * This routine allocates scsi buffers for device with SLI-4 interface spec,
  * the scsi buffer contains all the necessary information needed to initiate
- * a SCSI I/O.
+ * a SCSI I/O. After allocating up to @num_to_allocate SCSI buffers and put
+ * them on a list, it post them to the port by using SGL block post.
  *
  * Return codes:
- *   int - number of scsi buffers that were allocated.
+ *   int - number of scsi buffers that were allocated and posted.
  *   0 = failure, less than num_to_alloc is a partial failure.
  **/
 static int
@@ -810,22 +910,21 @@ lpfc_new_scsi_buf_s4(struct lpfc_vport *vport, int num_to_alloc)
 	dma_addr_t pdma_phys_fcp_cmd;
 	dma_addr_t pdma_phys_fcp_rsp;
 	dma_addr_t pdma_phys_bpl, pdma_phys_bpl1;
-	uint16_t iotag, last_xritag = NO_XRI, lxri = 0;
-	int status = 0, index;
-	int bcnt;
-	int non_sequential_xri = 0;
-	LIST_HEAD(sblist);
+	uint16_t iotag, lxri = 0;
+	int bcnt, num_posted;
+	LIST_HEAD(prep_sblist);
+	LIST_HEAD(post_sblist);
+	LIST_HEAD(scsi_sblist);
 
 	for (bcnt = 0; bcnt < num_to_alloc; bcnt++) {
 		psb = kzalloc(sizeof(struct lpfc_scsi_buf), GFP_KERNEL);
 		if (!psb)
 			break;
-
 		/*
-		 * Get memory from the pci pool to map the virt space to pci bus
-		 * space for an I/O.  The DMA buffer includes space for the
-		 * struct fcp_cmnd, struct fcp_rsp and the number of bde's
-		 * necessary to support the sg_tablesize.
+		 * Get memory from the pci pool to map the virt space to
+		 * pci bus space for an I/O. The DMA buffer includes space
+		 * for the struct fcp_cmnd, struct fcp_rsp and the number
+		 * of bde's necessary to support the sg_tablesize.
 		 */
 		psb->data = pci_pool_alloc(phba->lpfc_scsi_dma_buf_pool,
 						GFP_KERNEL, &psb->dma_handle);
@@ -833,8 +932,6 @@ lpfc_new_scsi_buf_s4(struct lpfc_vport *vport, int num_to_alloc)
 			kfree(psb);
 			break;
 		}
-
-		/* Initialize virtual ptrs to dma_buf region. */
 		memset(psb->data, 0, phba->cfg_sg_dma_buf_size);
 
 		/* Allocate iotag for psb->cur_iocbq. */
@@ -855,16 +952,7 @@ lpfc_new_scsi_buf_s4(struct lpfc_vport *vport, int num_to_alloc)
 		}
 		psb->cur_iocbq.sli4_lxritag = lxri;
 		psb->cur_iocbq.sli4_xritag = phba->sli4_hba.xri_ids[lxri];
-		if (last_xritag != NO_XRI
-			&& psb->cur_iocbq.sli4_xritag != (last_xritag+1)) {
-			non_sequential_xri = 1;
-		} else
-			list_add_tail(&psb->list, &sblist);
-		last_xritag = psb->cur_iocbq.sli4_xritag;
-
-		index = phba->sli4_hba.scsi_xri_cnt++;
 		psb->cur_iocbq.iocb_flag |= LPFC_IO_FCP;
-
 		psb->fcp_bpl = psb->data;
 		psb->fcp_cmnd = (psb->data + phba->cfg_sg_dma_buf_size)
 			- (sizeof(struct fcp_cmnd) + sizeof(struct fcp_rsp));
@@ -880,9 +968,9 @@ lpfc_new_scsi_buf_s4(struct lpfc_vport *vport, int num_to_alloc)
 		pdma_phys_fcp_rsp = pdma_phys_fcp_cmd + sizeof(struct fcp_cmnd);
 
 		/*
-		 * The first two bdes are the FCP_CMD and FCP_RSP.  The balance
-		 * are sg list bdes.  Initialize the first two and leave the
-		 * rest for queuecommand.
+		 * The first two bdes are the FCP_CMD and FCP_RSP.
+		 * The balance are sg list bdes. Initialize the
+		 * first two and leave the rest for queuecommand.
 		 */
 		sgl->addr_hi = cpu_to_le32(putPaddrHigh(pdma_phys_fcp_cmd));
 		sgl->addr_lo = cpu_to_le32(putPaddrLow(pdma_phys_fcp_cmd));
@@ -917,62 +1005,31 @@ lpfc_new_scsi_buf_s4(struct lpfc_vport *vport, int num_to_alloc)
 		iocb->ulpBdeCount = 1;
 		iocb->ulpLe = 1;
 		iocb->ulpClass = CLASS3;
-		psb->cur_iocbq.context1  = psb;
+		psb->cur_iocbq.context1 = psb;
 		if (phba->cfg_sg_dma_buf_size > SGL_PAGE_SIZE)
 			pdma_phys_bpl1 = pdma_phys_bpl + SGL_PAGE_SIZE;
 		else
 			pdma_phys_bpl1 = 0;
 		psb->dma_phys_bpl = pdma_phys_bpl;
-		phba->sli4_hba.lpfc_scsi_psb_array[index] = psb;
-		if (non_sequential_xri) {
-			status = lpfc_sli4_post_sgl(phba, pdma_phys_bpl,
-						pdma_phys_bpl1,
-						psb->cur_iocbq.sli4_xritag);
-			if (status) {
-				/* Put this back on the abort scsi list */
-				psb->exch_busy = 1;
-			} else {
-				psb->exch_busy = 0;
-				psb->status = IOSTAT_SUCCESS;
-			}
-			/* Put it back into the SCSI buffer list */
-			lpfc_release_scsi_buf_s4(phba, psb);
-			break;
-		}
-	}
-	if (bcnt) {
-		if (!phba->sli4_hba.extents_in_use)
-			status = lpfc_sli4_post_scsi_sgl_block(phba,
-								&sblist,
-								bcnt);
-		else
-			status = lpfc_sli4_post_scsi_sgl_blk_ext(phba,
-								&sblist,
-								bcnt);
-
-		if (status) {
-			lpfc_printf_log(phba, KERN_ERR, LOG_MBOX | LOG_SLI,
-					"3021 SCSI SGL post error %d\n",
-					status);
-			bcnt = 0;
-		}
-		/* Reset SCSI buffer count for next round of posting */
-		while (!list_empty(&sblist)) {
-			list_remove_head(&sblist, psb, struct lpfc_scsi_buf,
-				 list);
-			if (status) {
-				/* Put this back on the abort scsi list */
-				psb->exch_busy = 1;
-			} else {
-				psb->exch_busy = 0;
-				psb->status = IOSTAT_SUCCESS;
-			}
-			/* Put it back into the SCSI buffer list */
-			lpfc_release_scsi_buf_s4(phba, psb);
-		}
+
+		/* add the scsi buffer to a post list */
+		list_add_tail(&psb->list, &post_sblist);
+		spin_lock_irq(&phba->scsi_buf_list_lock);
+		phba->sli4_hba.scsi_xri_cnt++;
+		spin_unlock_irq(&phba->scsi_buf_list_lock);
 	}
+	lpfc_printf_log(phba, KERN_INFO, LOG_BG,
+			"3021 Allocate %d out of %d requested new SCSI "
+			"buffers\n", bcnt, num_to_alloc);
+
+	/* post the list of scsi buffer sgls to port if available */
+	if (!list_empty(&post_sblist))
+		num_posted = lpfc_sli4_post_scsi_sgl_list(phba,
+							  &post_sblist, bcnt);
+	else
+		num_posted = 0;
 
-	return bcnt + non_sequential_xri;
+	return num_posted;
 }
 
 /**
@@ -1043,7 +1100,7 @@ lpfc_get_scsi_buf_s4(struct lpfc_hba *phba, struct lpfc_nodelist *ndlp)
 	list_for_each_entry(lpfc_cmd, &phba->lpfc_scsi_buf_list,
 							list) {
 		if (lpfc_test_rrq_active(phba, ndlp,
-					 lpfc_cmd->cur_iocbq.sli4_xritag))
+					 lpfc_cmd->cur_iocbq.sli4_lxritag))
 			continue;
 		list_del(&lpfc_cmd->list);
 		found = 1;
@@ -1897,7 +1954,9 @@ lpfc_bg_setup_bpl(struct lpfc_hba *phba, struct scsi_cmnd *sc,
 	dma_addr_t physaddr;
 	int i = 0, num_bde = 0, status;
 	int datadir = sc->sc_data_direction;
+#ifdef CONFIG_SCSI_LPFC_DEBUG_FS
 	uint32_t rc;
+#endif
 	uint32_t checking = 1;
 	uint32_t reftag;
 	unsigned blksize;
@@ -2034,7 +2093,9 @@ lpfc_bg_setup_bpl_prot(struct lpfc_hba *phba, struct scsi_cmnd *sc,
 	int datadir = sc->sc_data_direction;
 	unsigned char pgdone = 0, alldone = 0;
 	unsigned blksize;
+#ifdef CONFIG_SCSI_LPFC_DEBUG_FS
 	uint32_t rc;
+#endif
 	uint32_t checking = 1;
 	uint32_t reftag;
 	uint8_t txop, rxop;
@@ -2253,7 +2314,9 @@ lpfc_bg_setup_sgl(struct lpfc_hba *phba, struct scsi_cmnd *sc,
 	uint32_t reftag;
 	unsigned blksize;
 	uint8_t txop, rxop;
+#ifdef CONFIG_SCSI_LPFC_DEBUG_FS
 	uint32_t rc;
+#endif
 	uint32_t checking = 1;
 	uint32_t dma_len;
 	uint32_t dma_offset = 0;
@@ -2383,7 +2446,9 @@ lpfc_bg_setup_sgl_prot(struct lpfc_hba *phba, struct scsi_cmnd *sc,
 	uint32_t reftag;
 	uint8_t txop, rxop;
 	uint32_t dma_len;
+#ifdef CONFIG_SCSI_LPFC_DEBUG_FS
 	uint32_t rc;
+#endif
 	uint32_t checking = 1;
 	uint32_t dma_offset = 0;
 	int num_sge = 0;
@@ -3604,11 +3669,16 @@ lpfc_scsi_cmd_iocb_cmpl(struct lpfc_hba *phba, struct lpfc_iocbq *pIocbIn,
 			logit = LOG_FCP | LOG_FCP_UNDER;
 		lpfc_printf_vlog(vport, KERN_WARNING, logit,
 			 "9030 FCP cmd x%x failed <%d/%d> "
-			 "status: x%x result: x%x Data: x%x x%x\n",
+			 "status: x%x result: x%x "
+			 "sid: x%x did: x%x oxid: x%x "
+			 "Data: x%x x%x\n",
 			 cmd->cmnd[0],
 			 cmd->device ? cmd->device->id : 0xffff,
 			 cmd->device ? cmd->device->lun : 0xffff,
 			 lpfc_cmd->status, lpfc_cmd->result,
+			 vport->fc_myDID, pnode->nlp_DID,
+			 phba->sli_rev == LPFC_SLI_REV4 ?
+			     lpfc_cmd->cur_iocbq.sli4_xritag : 0xffff,
 			 pIocbOut->iocb.ulpContext,
 			 lpfc_cmd->cur_iocbq.iocb.ulpIoTag);
 
@@ -3689,8 +3759,8 @@ lpfc_scsi_cmd_iocb_cmpl(struct lpfc_hba *phba, struct lpfc_iocbq *pIocbIn,
 				 * ABTS we cannot generate and RRQ.
 				 */
 				lpfc_set_rrq_active(phba, pnode,
-						lpfc_cmd->cur_iocbq.sli4_xritag,
-						0, 0);
+					lpfc_cmd->cur_iocbq.sli4_lxritag,
+					0, 0);
 			}
 		/* else: fall through */
 		default:
@@ -4348,8 +4418,20 @@ lpfc_abort_handler(struct scsi_cmnd *cmnd)
 	ret = fc_block_scsi_eh(cmnd);
 	if (ret)
 		return ret;
+
+	spin_lock_irq(&phba->hbalock);
+	/* driver queued commands are in process of being flushed */
+	if (phba->hba_flag & HBA_FCP_IOQ_FLUSH) {
+		spin_unlock_irq(&phba->hbalock);
+		lpfc_printf_vlog(vport, KERN_WARNING, LOG_FCP,
+			"3168 SCSI Layer abort requested I/O has been "
+			"flushed by LLD.\n");
+		return FAILED;
+	}
+
 	lpfc_cmd = (struct lpfc_scsi_buf *)cmnd->host_scribble;
 	if (!lpfc_cmd) {
+		spin_unlock_irq(&phba->hbalock);
 		lpfc_printf_vlog(vport, KERN_WARNING, LOG_FCP,
 			 "2873 SCSI Layer I/O Abort Request IO CMPL Status "
 			 "x%x ID %d LUN %d\n",
@@ -4357,23 +4439,34 @@ lpfc_abort_handler(struct scsi_cmnd *cmnd)
 		return SUCCESS;
 	}
 
+	iocb = &lpfc_cmd->cur_iocbq;
+	/* the command is in process of being cancelled */
+	if (!(iocb->iocb_flag & LPFC_IO_ON_TXCMPLQ)) {
+		spin_unlock_irq(&phba->hbalock);
+		lpfc_printf_vlog(vport, KERN_WARNING, LOG_FCP,
+			"3169 SCSI Layer abort requested I/O has been "
+			"cancelled by LLD.\n");
+		return FAILED;
+	}
 	/*
 	 * If pCmd field of the corresponding lpfc_scsi_buf structure
 	 * points to a different SCSI command, then the driver has
 	 * already completed this command, but the midlayer did not
-	 * see the completion before the eh fired.  Just return
-	 * SUCCESS.
+	 * see the completion before the eh fired. Just return SUCCESS.
 	 */
-	iocb = &lpfc_cmd->cur_iocbq;
-	if (lpfc_cmd->pCmd != cmnd)
-		goto out;
+	if (lpfc_cmd->pCmd != cmnd) {
+		lpfc_printf_vlog(vport, KERN_WARNING, LOG_FCP,
+			"3170 SCSI Layer abort requested I/O has been "
+			"completed by LLD.\n");
+		goto out_unlock;
+	}
 
 	BUG_ON(iocb->context1 != lpfc_cmd);
 
-	abtsiocb = lpfc_sli_get_iocbq(phba);
+	abtsiocb = __lpfc_sli_get_iocbq(phba);
 	if (abtsiocb == NULL) {
 		ret = FAILED;
-		goto out;
+		goto out_unlock;
 	}
 
 	/*
@@ -4405,6 +4498,9 @@ lpfc_abort_handler(struct scsi_cmnd *cmnd)
 
 	abtsiocb->iocb_cmpl = lpfc_sli_abort_fcp_cmpl;
 	abtsiocb->vport = vport;
+	/* no longer need the lock after this point */
+	spin_unlock_irq(&phba->hbalock);
+
 	if (lpfc_sli_issue_iocb(phba, LPFC_FCP_RING, abtsiocb, 0) ==
 	    IOCB_ERROR) {
 		lpfc_sli_release_iocbq(phba, abtsiocb);
@@ -4421,10 +4517,7 @@ lpfc_abort_handler(struct scsi_cmnd *cmnd)
 	wait_event_timeout(waitq,
 			  (lpfc_cmd->pCmd != cmnd),
 			   (2*vport->cfg_devloss_tmo*HZ));
-
-	spin_lock_irq(shost->host_lock);
 	lpfc_cmd->waitq = NULL;
-	spin_unlock_irq(shost->host_lock);
 
 	if (lpfc_cmd->pCmd == cmnd) {
 		ret = FAILED;
@@ -4434,8 +4527,11 @@ lpfc_abort_handler(struct scsi_cmnd *cmnd)
 				 "LUN %d\n",
 				 ret, cmnd->device->id, cmnd->device->lun);
 	}
+	goto out;
 
- out:
+out_unlock:
+	spin_unlock_irq(&phba->hbalock);
+out:
 	lpfc_printf_vlog(vport, KERN_WARNING, LOG_FCP,
 			 "0749 SCSI Layer I/O Abort Request Status x%x ID %d "
 			 "LUN %d\n", ret, cmnd->device->id,
@@ -4863,6 +4959,43 @@ lpfc_bus_reset_handler(struct scsi_cmnd *cmnd)
 }
 
 /**
+ * lpfc_host_reset_handler - scsi_host_template eh_host_reset_handler entry pt
+ * @cmnd: Pointer to scsi_cmnd data structure.
+ *
+ * This routine does host reset to the adaptor port. It brings the HBA
+ * offline, performs a board restart, and then brings the board back online.
+ * The lpfc_offline calls lpfc_sli_hba_down which will abort and local
+ * reject all outstanding SCSI commands to the host and error returned
+ * back to SCSI mid-level. As this will be SCSI mid-level's last resort
+ * of error handling, it will only return error if resetting of the adapter
+ * is not successful; in all other cases, will return success.
+ *
+ * Return code :
+ *  0x2003 - Error
+ *  0x2002 - Success
+ **/
+static int
+lpfc_host_reset_handler(struct scsi_cmnd *cmnd)
+{
+	struct Scsi_Host *shost = cmnd->device->host;
+	struct lpfc_vport *vport = (struct lpfc_vport *) shost->hostdata;
+	struct lpfc_hba *phba = vport->phba;
+	int rc, ret = SUCCESS;
+
+	lpfc_offline_prep(phba);
+	lpfc_offline(phba);
+	rc = lpfc_sli_brdrestart(phba);
+	if (rc)
+		ret = FAILED;
+	lpfc_online(phba);
+	lpfc_unblock_mgmt_io(phba);
+
+	lpfc_printf_log(phba, KERN_ERR, LOG_FCP,
+			"3172 SCSI layer issued Host Reset Data: x%x\n", ret);
+	return ret;
+}
+
+/**
  * lpfc_slave_alloc - scsi_host_template slave_alloc entry point
  * @sdev: Pointer to scsi_device.
  *
@@ -4994,6 +5127,7 @@ struct scsi_host_template lpfc_template = {
 	.eh_device_reset_handler = lpfc_device_reset_handler,
 	.eh_target_reset_handler = lpfc_target_reset_handler,
 	.eh_bus_reset_handler	= lpfc_bus_reset_handler,
+	.eh_host_reset_handler  = lpfc_host_reset_handler,
 	.slave_alloc		= lpfc_slave_alloc,
 	.slave_configure	= lpfc_slave_configure,
 	.slave_destroy		= lpfc_slave_destroy,
diff --git a/drivers/scsi/lpfc/lpfc_sli.c b/drivers/scsi/lpfc/lpfc_sli.c
index dbaf5b963bf..b4720a10981 100644
--- a/drivers/scsi/lpfc/lpfc_sli.c
+++ b/drivers/scsi/lpfc/lpfc_sli.c
@@ -67,6 +67,8 @@ static void lpfc_sli4_send_seq_to_ulp(struct lpfc_vport *,
 				      struct hbq_dmabuf *);
 static int lpfc_sli4_fp_handle_wcqe(struct lpfc_hba *, struct lpfc_queue *,
 				    struct lpfc_cqe *);
+static int lpfc_sli4_post_els_sgl_list(struct lpfc_hba *, struct list_head *,
+				       int);
 
 static IOCB_t *
 lpfc_get_iocb_from_iocbq(struct lpfc_iocbq *iocbq)
@@ -500,7 +502,7 @@ lpfc_resp_iocb(struct lpfc_hba *phba, struct lpfc_sli_ring *pring)
  * allocation is successful, it returns pointer to the newly
  * allocated iocb object else it returns NULL.
  **/
-static struct lpfc_iocbq *
+struct lpfc_iocbq *
 __lpfc_sli_get_iocbq(struct lpfc_hba *phba)
 {
 	struct list_head *lpfc_iocb_list = &phba->lpfc_iocb_list;
@@ -875,6 +877,9 @@ __lpfc_sli_get_sglq(struct lpfc_hba *phba, struct lpfc_iocbq *piocbq)
 	} else  if ((piocbq->iocb.ulpCommand == CMD_GEN_REQUEST64_CR) &&
 			!(piocbq->iocb_flag & LPFC_IO_LIBDFC))
 		ndlp = piocbq->context_un.ndlp;
+	else  if ((piocbq->iocb.ulpCommand == CMD_ELS_REQUEST64_CR) &&
+			(piocbq->iocb_flag & LPFC_IO_LIBDFC))
+		ndlp = piocbq->context_un.ndlp;
 	else
 		ndlp = piocbq->context1;
 
@@ -883,7 +888,7 @@ __lpfc_sli_get_sglq(struct lpfc_hba *phba, struct lpfc_iocbq *piocbq)
 	while (!found) {
 		if (!sglq)
 			return NULL;
-		if (lpfc_test_rrq_active(phba, ndlp, sglq->sli4_xritag)) {
+		if (lpfc_test_rrq_active(phba, ndlp, sglq->sli4_lxritag)) {
 			/* This xri has an rrq outstanding for this DID.
 			 * put it back in the list and get another xri.
 			 */
@@ -1257,7 +1262,7 @@ lpfc_sli_ringtxcmpl_put(struct lpfc_hba *phba, struct lpfc_sli_ring *pring,
 			struct lpfc_iocbq *piocb)
 {
 	list_add_tail(&piocb->list, &pring->txcmplq);
-	piocb->iocb_flag |= LPFC_IO_ON_Q;
+	piocb->iocb_flag |= LPFC_IO_ON_TXCMPLQ;
 	pring->txcmplq_cnt++;
 	if (pring->txcmplq_cnt > pring->txcmplq_max)
 		pring->txcmplq_max = pring->txcmplq_cnt;
@@ -2556,9 +2561,9 @@ lpfc_sli_iocbq_lookup(struct lpfc_hba *phba,
 	if (iotag != 0 && iotag <= phba->sli.last_iotag) {
 		cmd_iocb = phba->sli.iocbq_lookup[iotag];
 		list_del_init(&cmd_iocb->list);
-		if (cmd_iocb->iocb_flag & LPFC_IO_ON_Q) {
+		if (cmd_iocb->iocb_flag & LPFC_IO_ON_TXCMPLQ) {
 			pring->txcmplq_cnt--;
-			cmd_iocb->iocb_flag &= ~LPFC_IO_ON_Q;
+			cmd_iocb->iocb_flag &= ~LPFC_IO_ON_TXCMPLQ;
 		}
 		return cmd_iocb;
 	}
@@ -2591,14 +2596,14 @@ lpfc_sli_iocbq_lookup_by_tag(struct lpfc_hba *phba,
 
 	if (iotag != 0 && iotag <= phba->sli.last_iotag) {
 		cmd_iocb = phba->sli.iocbq_lookup[iotag];
-		list_del_init(&cmd_iocb->list);
-		if (cmd_iocb->iocb_flag & LPFC_IO_ON_Q) {
-			cmd_iocb->iocb_flag &= ~LPFC_IO_ON_Q;
+		if (cmd_iocb->iocb_flag & LPFC_IO_ON_TXCMPLQ) {
+			/* 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;
 		}
-		return cmd_iocb;
 	}
-
 	lpfc_printf_log(phba, KERN_ERR, LOG_SLI,
 			"0372 iotag x%x is out off range: max iotag (x%x)\n",
 			iotag, phba->sli.last_iotag);
@@ -3466,6 +3471,9 @@ lpfc_sli_flush_fcp_rings(struct lpfc_hba *phba)
 	/* 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;
 	spin_unlock_irq(&phba->hbalock);
 
 	/* Flush the txq */
@@ -3877,6 +3885,7 @@ lpfc_sli4_brdreset(struct lpfc_hba *phba)
 {
 	struct lpfc_sli *psli = &phba->sli;
 	uint16_t cfg_value;
+	int rc;
 
 	/* Reset HBA */
 	lpfc_printf_log(phba, KERN_INFO, LOG_SLI,
@@ -3905,12 +3914,12 @@ lpfc_sli4_brdreset(struct lpfc_hba *phba)
 
 	/* Perform FCoE PCI function reset */
 	lpfc_sli4_queue_destroy(phba);
-	lpfc_pci_function_reset(phba);
+	rc = lpfc_pci_function_reset(phba);
 
 	/* Restore PCI cmd register */
 	pci_write_config_word(phba->pcidev, PCI_COMMAND, cfg_value);
 
-	return 0;
+	return rc;
 }
 
 /**
@@ -4002,6 +4011,7 @@ lpfc_sli_brdrestart_s4(struct lpfc_hba *phba)
 {
 	struct lpfc_sli *psli = &phba->sli;
 	uint32_t hba_aer_enabled;
+	int rc;
 
 	/* Restart HBA */
 	lpfc_printf_log(phba, KERN_INFO, LOG_SLI,
@@ -4011,7 +4021,7 @@ lpfc_sli_brdrestart_s4(struct lpfc_hba *phba)
 	/* Take PCIe device Advanced Error Reporting (AER) state */
 	hba_aer_enabled = phba->hba_flag & HBA_AER_ENABLED;
 
-	lpfc_sli4_brdreset(phba);
+	rc = lpfc_sli4_brdreset(phba);
 
 	spin_lock_irq(&phba->hbalock);
 	phba->pport->stopped = 0;
@@ -4028,7 +4038,7 @@ lpfc_sli_brdrestart_s4(struct lpfc_hba *phba)
 
 	lpfc_hba_down_post(phba);
 
-	return 0;
+	return rc;
 }
 
 /**
@@ -4967,7 +4977,12 @@ lpfc_sli4_get_avail_extnt_rsrc(struct lpfc_hba *phba, uint16_t type,
 			      &rsrc_info->u.rsp);
 	*extnt_size = bf_get(lpfc_mbx_get_rsrc_extent_info_size,
 			     &rsrc_info->u.rsp);
- err_exit:
+
+	lpfc_printf_log(phba, KERN_INFO, LOG_SLI,
+			"3162 Retrieved extents type-%d from port: count:%d, "
+			"size:%d\n", type, *extnt_count, *extnt_size);
+
+err_exit:
 	mempool_free(mbox, phba->mbox_mem_pool);
 	return rc;
 }
@@ -5051,7 +5066,7 @@ lpfc_sli4_chk_avail_extnt_rsrc(struct lpfc_hba *phba, uint16_t type)
  *   0: if successful
  **/
 static int
-lpfc_sli4_cfg_post_extnts(struct lpfc_hba *phba, uint16_t *extnt_cnt,
+lpfc_sli4_cfg_post_extnts(struct lpfc_hba *phba, uint16_t extnt_cnt,
 			  uint16_t type, bool *emb, LPFC_MBOXQ_t *mbox)
 {
 	int rc = 0;
@@ -5060,7 +5075,7 @@ lpfc_sli4_cfg_post_extnts(struct lpfc_hba *phba, uint16_t *extnt_cnt,
 	uint32_t alloc_len, mbox_tmo;
 
 	/* Calculate the total requested length of the dma memory */
-	req_len = *extnt_cnt * sizeof(uint16_t);
+	req_len = extnt_cnt * sizeof(uint16_t);
 
 	/*
 	 * Calculate the size of an embedded mailbox.  The uint32_t
@@ -5075,7 +5090,7 @@ lpfc_sli4_cfg_post_extnts(struct lpfc_hba *phba, uint16_t *extnt_cnt,
 	 */
 	*emb = LPFC_SLI4_MBX_EMBED;
 	if (req_len > emb_len) {
-		req_len = *extnt_cnt * sizeof(uint16_t) +
+		req_len = extnt_cnt * sizeof(uint16_t) +
 			sizeof(union lpfc_sli4_cfg_shdr) +
 			sizeof(uint32_t);
 		*emb = LPFC_SLI4_MBX_NEMBED;
@@ -5091,7 +5106,7 @@ lpfc_sli4_cfg_post_extnts(struct lpfc_hba *phba, uint16_t *extnt_cnt,
 			"size (x%x)\n", alloc_len, req_len);
 		return -ENOMEM;
 	}
-	rc = lpfc_sli4_mbox_rsrc_extent(phba, mbox, *extnt_cnt, type, *emb);
+	rc = lpfc_sli4_mbox_rsrc_extent(phba, mbox, extnt_cnt, type, *emb);
 	if (unlikely(rc))
 		return -EIO;
 
@@ -5149,17 +5164,15 @@ lpfc_sli4_alloc_extent(struct lpfc_hba *phba, uint16_t type)
 		return -ENOMEM;
 	}
 
-	lpfc_printf_log(phba, KERN_INFO, LOG_MBOX | LOG_INIT,
-			"2903 Available Resource Extents "
-			"for resource type 0x%x: Count: 0x%x, "
-			"Size 0x%x\n", type, rsrc_cnt,
-			rsrc_size);
+	lpfc_printf_log(phba, KERN_INFO, LOG_MBOX | LOG_INIT | LOG_SLI,
+			"2903 Post resource extents type-0x%x: "
+			"count:%d, size %d\n", type, rsrc_cnt, rsrc_size);
 
 	mbox = (LPFC_MBOXQ_t *) mempool_alloc(phba->mbox_mem_pool, GFP_KERNEL);
 	if (!mbox)
 		return -ENOMEM;
 
-	rc = lpfc_sli4_cfg_post_extnts(phba, &rsrc_cnt, type, &emb, mbox);
+	rc = lpfc_sli4_cfg_post_extnts(phba, rsrc_cnt, type, &emb, mbox);
 	if (unlikely(rc)) {
 		rc = -EIO;
 		goto err_exit;
@@ -5250,6 +5263,7 @@ lpfc_sli4_alloc_extent(struct lpfc_hba *phba, uint16_t type)
 			rc = -ENOMEM;
 			goto err_exit;
 		}
+		phba->sli4_hba.max_cfg_param.xri_used = 0;
 		phba->sli4_hba.xri_ids = kzalloc(rsrc_id_cnt *
 						 sizeof(uint16_t),
 						 GFP_KERNEL);
@@ -5420,7 +5434,6 @@ lpfc_sli4_dealloc_extent(struct lpfc_hba *phba, uint16_t type)
 	case LPFC_RSC_TYPE_FCOE_XRI:
 		kfree(phba->sli4_hba.xri_bmask);
 		kfree(phba->sli4_hba.xri_ids);
-		bf_set(lpfc_xri_rsrc_rdy, &phba->sli4_hba.sli4_flags, 0);
 		list_for_each_entry_safe(rsrc_blk, rsrc_blk_next,
 				    &phba->sli4_hba.lpfc_xri_blk_list, list) {
 			list_del_init(&rsrc_blk->list);
@@ -5612,7 +5625,6 @@ lpfc_sli4_alloc_resource_identifiers(struct lpfc_hba *phba)
 			goto free_vpi_ids;
 		}
 		phba->sli4_hba.max_cfg_param.xri_used = 0;
-		phba->sli4_hba.xri_count = 0;
 		phba->sli4_hba.xri_ids = kzalloc(count *
 						 sizeof(uint16_t),
 						 GFP_KERNEL);
@@ -5694,7 +5706,6 @@ lpfc_sli4_dealloc_resource_identifiers(struct lpfc_hba *phba)
 		bf_set(lpfc_vpi_rsrc_rdy, &phba->sli4_hba.sli4_flags, 0);
 		kfree(phba->sli4_hba.xri_bmask);
 		kfree(phba->sli4_hba.xri_ids);
-		bf_set(lpfc_xri_rsrc_rdy, &phba->sli4_hba.sli4_flags, 0);
 		kfree(phba->sli4_hba.vfi_bmask);
 		kfree(phba->sli4_hba.vfi_ids);
 		bf_set(lpfc_vfi_rsrc_rdy, &phba->sli4_hba.sli4_flags, 0);
@@ -5853,6 +5864,149 @@ lpfc_sli4_get_allocated_extnts(struct lpfc_hba *phba, uint16_t type,
 }
 
 /**
+ * lpfc_sli4_repost_els_sgl_list - Repsot the els buffers sgl pages as block
+ * @phba: pointer to lpfc hba data structure.
+ *
+ * This routine walks the list of els buffers that have been allocated and
+ * repost them to the port by using SGL block post. This is needed after a
+ * pci_function_reset/warm_start or start. It attempts to construct blocks
+ * of els buffer sgls which contains contiguous xris and uses the non-embedded
+ * SGL block post mailbox commands to post them to the port. For single els
+ * buffer sgl with non-contiguous xri, if any, it shall use embedded SGL post
+ * mailbox command for posting.
+ *
+ * Returns: 0 = success, non-zero failure.
+ **/
+static int
+lpfc_sli4_repost_els_sgl_list(struct lpfc_hba *phba)
+{
+	struct lpfc_sglq *sglq_entry = NULL;
+	struct lpfc_sglq *sglq_entry_next = NULL;
+	struct lpfc_sglq *sglq_entry_first = NULL;
+	int status, post_cnt = 0, num_posted = 0, block_cnt = 0;
+	int last_xritag = NO_XRI;
+	LIST_HEAD(prep_sgl_list);
+	LIST_HEAD(blck_sgl_list);
+	LIST_HEAD(allc_sgl_list);
+	LIST_HEAD(post_sgl_list);
+	LIST_HEAD(free_sgl_list);
+
+	spin_lock(&phba->hbalock);
+	list_splice_init(&phba->sli4_hba.lpfc_sgl_list, &allc_sgl_list);
+	spin_unlock(&phba->hbalock);
+
+	list_for_each_entry_safe(sglq_entry, sglq_entry_next,
+				 &allc_sgl_list, list) {
+		list_del_init(&sglq_entry->list);
+		block_cnt++;
+		if ((last_xritag != NO_XRI) &&
+		    (sglq_entry->sli4_xritag != last_xritag + 1)) {
+			/* a hole in xri block, form a sgl posting block */
+			list_splice_init(&prep_sgl_list, &blck_sgl_list);
+			post_cnt = block_cnt - 1;
+			/* prepare list for next posting block */
+			list_add_tail(&sglq_entry->list, &prep_sgl_list);
+			block_cnt = 1;
+		} else {
+			/* prepare list for next posting block */
+			list_add_tail(&sglq_entry->list, &prep_sgl_list);
+			/* enough sgls for non-embed sgl mbox command */
+			if (block_cnt == LPFC_NEMBED_MBOX_SGL_CNT) {
+				list_splice_init(&prep_sgl_list,
+						 &blck_sgl_list);
+				post_cnt = block_cnt;
+				block_cnt = 0;
+			}
+		}
+		num_posted++;
+
+		/* keep track of last sgl's xritag */
+		last_xritag = sglq_entry->sli4_xritag;
+
+		/* end of repost sgl list condition for els buffers */
+		if (num_posted == phba->sli4_hba.els_xri_cnt) {
+			if (post_cnt == 0) {
+				list_splice_init(&prep_sgl_list,
+						 &blck_sgl_list);
+				post_cnt = block_cnt;
+			} else if (block_cnt == 1) {
+				status = lpfc_sli4_post_sgl(phba,
+						sglq_entry->phys, 0,
+						sglq_entry->sli4_xritag);
+				if (!status) {
+					/* successful, put sgl to posted list */
+					list_add_tail(&sglq_entry->list,
+						      &post_sgl_list);
+				} else {
+					/* Failure, put sgl to free list */
+					lpfc_printf_log(phba, KERN_WARNING,
+						LOG_SLI,
+						"3159 Failed to post els "
+						"sgl, xritag:x%x\n",
+						sglq_entry->sli4_xritag);
+					list_add_tail(&sglq_entry->list,
+						      &free_sgl_list);
+					spin_lock_irq(&phba->hbalock);
+					phba->sli4_hba.els_xri_cnt--;
+					spin_unlock_irq(&phba->hbalock);
+				}
+			}
+		}
+
+		/* continue until a nembed page worth of sgls */
+		if (post_cnt == 0)
+			continue;
+
+		/* post the els buffer list sgls as a block */
+		status = lpfc_sli4_post_els_sgl_list(phba, &blck_sgl_list,
+						     post_cnt);
+
+		if (!status) {
+			/* success, put sgl list to posted sgl list */
+			list_splice_init(&blck_sgl_list, &post_sgl_list);
+		} else {
+			/* Failure, put sgl list to free sgl list */
+			sglq_entry_first = list_first_entry(&blck_sgl_list,
+							    struct lpfc_sglq,
+							    list);
+			lpfc_printf_log(phba, KERN_WARNING, LOG_SLI,
+					"3160 Failed to post els sgl-list, "
+					"xritag:x%x-x%x\n",
+					sglq_entry_first->sli4_xritag,
+					(sglq_entry_first->sli4_xritag +
+					 post_cnt - 1));
+			list_splice_init(&blck_sgl_list, &free_sgl_list);
+			spin_lock_irq(&phba->hbalock);
+			phba->sli4_hba.els_xri_cnt -= post_cnt;
+			spin_unlock_irq(&phba->hbalock);
+		}
+
+		/* don't reset xirtag due to hole in xri block */
+		if (block_cnt == 0)
+			last_xritag = NO_XRI;
+
+		/* reset els sgl post count for next round of posting */
+		post_cnt = 0;
+	}
+
+	/* free the els sgls failed to post */
+	lpfc_free_sgl_list(phba, &free_sgl_list);
+
+	/* push els sgls posted to the availble list */
+	if (!list_empty(&post_sgl_list)) {
+		spin_lock(&phba->hbalock);
+		list_splice_init(&post_sgl_list,
+				 &phba->sli4_hba.lpfc_sgl_list);
+		spin_unlock(&phba->hbalock);
+	} else {
+		lpfc_printf_log(phba, KERN_ERR, LOG_SLI,
+				"3161 Failure to post els sgl to port.\n");
+		return -EIO;
+	}
+	return 0;
+}
+
+/**
  * lpfc_sli4_hba_setup - SLI4 device intialization PCI function
  * @phba: Pointer to HBA context object.
  *
@@ -5923,6 +6077,8 @@ lpfc_sli4_hba_setup(struct lpfc_hba *phba)
 	else
 		phba->hba_flag &= ~HBA_FIP_SUPPORT;
 
+	phba->hba_flag &= ~HBA_FCP_IOQ_FLUSH;
+
 	if (phba->sli_rev != LPFC_SLI_REV4) {
 		lpfc_printf_log(phba, KERN_ERR, LOG_MBOX | LOG_SLI,
 			"0376 READ_REV Error. SLI Level %d "
@@ -6063,8 +6219,6 @@ lpfc_sli4_hba_setup(struct lpfc_hba *phba)
 				"rc = x%x\n", rc);
 		goto out_free_mbox;
 	}
-	/* update physical xri mappings in the scsi buffers */
-	lpfc_scsi_buf_update(phba);
 
 	/* Read the port's service parameters. */
 	rc = lpfc_read_sparam(phba, mboxq, vport->vpi);
@@ -6105,28 +6259,26 @@ lpfc_sli4_hba_setup(struct lpfc_hba *phba)
 	fc_host_node_name(shost) = wwn_to_u64(vport->fc_nodename.u.wwn);
 	fc_host_port_name(shost) = wwn_to_u64(vport->fc_portname.u.wwn);
 
-	/* Register SGL pool to the device using non-embedded mailbox command */
-	if (!phba->sli4_hba.extents_in_use) {
-		rc = lpfc_sli4_post_els_sgl_list(phba);
-		if (unlikely(rc)) {
-			lpfc_printf_log(phba, KERN_ERR, LOG_MBOX | LOG_SLI,
-					"0582 Error %d during els sgl post "
-					"operation\n", rc);
-			rc = -ENODEV;
-			goto out_free_mbox;
-		}
-	} else {
-		rc = lpfc_sli4_post_els_sgl_list_ext(phba);
-		if (unlikely(rc)) {
-			lpfc_printf_log(phba, KERN_ERR, LOG_MBOX | LOG_SLI,
-					"2560 Error %d during els sgl post "
-					"operation\n", rc);
-			rc = -ENODEV;
-			goto out_free_mbox;
-		}
+	/* update host els and scsi xri-sgl sizes and mappings */
+	rc = lpfc_sli4_xri_sgl_update(phba);
+	if (unlikely(rc)) {
+		lpfc_printf_log(phba, KERN_ERR, LOG_MBOX | LOG_SLI,
+				"1400 Failed to update xri-sgl size and "
+				"mapping: %d\n", rc);
+		goto out_free_mbox;
 	}
 
-	/* Register SCSI SGL pool to the device */
+	/* register the els sgl pool to the port */
+	rc = lpfc_sli4_repost_els_sgl_list(phba);
+	if (unlikely(rc)) {
+		lpfc_printf_log(phba, KERN_ERR, LOG_MBOX | LOG_SLI,
+				"0582 Error %d during els sgl post "
+				"operation\n", rc);
+		rc = -ENODEV;
+		goto out_free_mbox;
+	}
+
+	/* register the allocated scsi sgl pool to the port */
 	rc = lpfc_sli4_repost_scsi_sgl_list(phba);
 	if (unlikely(rc)) {
 		lpfc_printf_log(phba, KERN_ERR, LOG_MBOX | LOG_SLI,
@@ -7060,14 +7212,19 @@ lpfc_sli_issue_mbox_s4(struct lpfc_hba *phba, LPFC_MBOXQ_t *mboxq,
 		if (rc != MBX_SUCCESS)
 			lpfc_printf_log(phba, KERN_WARNING, LOG_MBOX | LOG_SLI,
 					"(%d):2541 Mailbox command x%x "
-					"(x%x/x%x) cannot issue Data: "
-					"x%x x%x\n",
+					"(x%x/x%x) failure: "
+					"mqe_sta: x%x mcqe_sta: x%x/x%x "
+					"Data: x%x x%x\n,",
 					mboxq->vport ? mboxq->vport->vpi : 0,
 					mboxq->u.mb.mbxCommand,
 					lpfc_sli_config_mbox_subsys_get(phba,
 									mboxq),
 					lpfc_sli_config_mbox_opcode_get(phba,
 									mboxq),
+					bf_get(lpfc_mqe_status, &mboxq->u.mqe),
+					bf_get(lpfc_mcqe_status, &mboxq->mcqe),
+					bf_get(lpfc_mcqe_ext_status,
+					       &mboxq->mcqe),
 					psli->sli_flag, flag);
 		return rc;
 	} else if (flag == MBX_POLL) {
@@ -7086,18 +7243,22 @@ lpfc_sli_issue_mbox_s4(struct lpfc_hba *phba, LPFC_MBOXQ_t *mboxq,
 			/* Successfully blocked, now issue sync mbox cmd */
 			rc = lpfc_sli4_post_sync_mbox(phba, mboxq);
 			if (rc != MBX_SUCCESS)
-				lpfc_printf_log(phba, KERN_ERR,
+				lpfc_printf_log(phba, KERN_WARNING,
 					LOG_MBOX | LOG_SLI,
-					"(%d):2597 Mailbox command "
-					"x%x (x%x/x%x) cannot issue "
-					"Data: x%x x%x\n",
-					mboxq->vport ?
-					mboxq->vport->vpi : 0,
+					"(%d):2597 Sync Mailbox command "
+					"x%x (x%x/x%x) failure: "
+					"mqe_sta: x%x mcqe_sta: x%x/x%x "
+					"Data: x%x x%x\n,",
+					mboxq->vport ? mboxq->vport->vpi : 0,
 					mboxq->u.mb.mbxCommand,
 					lpfc_sli_config_mbox_subsys_get(phba,
 									mboxq),
 					lpfc_sli_config_mbox_opcode_get(phba,
 									mboxq),
+					bf_get(lpfc_mqe_status, &mboxq->u.mqe),
+					bf_get(lpfc_mcqe_status, &mboxq->mcqe),
+					bf_get(lpfc_mcqe_ext_status,
+					       &mboxq->mcqe),
 					psli->sli_flag, flag);
 			/* Unblock the async mailbox posting afterward */
 			lpfc_sli4_async_mbox_unblock(phba);
@@ -7712,7 +7873,10 @@ lpfc_sli4_iocb2wqe(struct lpfc_hba *phba, struct lpfc_iocbq *iocbq,
 
 	switch (iocbq->iocb.ulpCommand) {
 	case CMD_ELS_REQUEST64_CR:
-		ndlp = (struct lpfc_nodelist *)iocbq->context1;
+		if (iocbq->iocb_flag & LPFC_IO_LIBDFC)
+			ndlp = iocbq->context_un.ndlp;
+		else
+			ndlp = (struct lpfc_nodelist *)iocbq->context1;
 		if (!iocbq->iocb.ulpLe) {
 			lpfc_printf_log(phba, KERN_ERR, LOG_SLI,
 				"2007 Only Limited Edition cmd Format"
@@ -7751,9 +7915,13 @@ lpfc_sli4_iocb2wqe(struct lpfc_hba *phba, struct lpfc_iocbq *iocbq,
 				bf_set(els_req64_sp, &wqe->els_req, 1);
 				bf_set(els_req64_sid, &wqe->els_req,
 					iocbq->vport->fc_myDID);
+				if ((*pcmd == ELS_CMD_FLOGI) &&
+					!(phba->fc_topology ==
+						LPFC_TOPOLOGY_LOOP))
+					bf_set(els_req64_sid, &wqe->els_req, 0);
 				bf_set(wqe_ct, &wqe->els_req.wqe_com, 1);
 				bf_set(wqe_ctxt_tag, &wqe->els_req.wqe_com,
-					phba->vpi_ids[phba->pport->vpi]);
+					phba->vpi_ids[iocbq->vport->vpi]);
 			} else if (pcmd && iocbq->context1) {
 				bf_set(wqe_ct, &wqe->els_req.wqe_com, 0);
 				bf_set(wqe_ctxt_tag, &wqe->els_req.wqe_com,
@@ -7908,11 +8076,25 @@ lpfc_sli4_iocb2wqe(struct lpfc_hba *phba, struct lpfc_iocbq *iocbq,
 		/* words0-2 BDE memcpy */
 		/* word3 iocb=iotag32 wqe=response_payload_len */
 		wqe->xmit_els_rsp.response_payload_len = xmit_len;
-		/* word4 iocb=did wge=rsvd. */
-		wqe->xmit_els_rsp.rsvd4 = 0;
+		/* word4 */
+		wqe->xmit_els_rsp.word4 = 0;
 		/* word5 iocb=rsvd wge=did */
 		bf_set(wqe_els_did, &wqe->xmit_els_rsp.wqe_dest,
-			 iocbq->iocb.un.elsreq64.remoteID);
+			 iocbq->iocb.un.xseq64.xmit_els_remoteID);
+
+		if_type = bf_get(lpfc_sli_intf_if_type,
+					&phba->sli4_hba.sli_intf);
+		if (if_type == LPFC_SLI_INTF_IF_TYPE_2) {
+			if (iocbq->vport->fc_flag & FC_PT2PT) {
+				bf_set(els_rsp64_sp, &wqe->xmit_els_rsp, 1);
+				bf_set(els_rsp64_sid, &wqe->xmit_els_rsp,
+					iocbq->vport->fc_myDID);
+				if (iocbq->vport->fc_myDID == Fabric_DID) {
+					bf_set(wqe_els_did,
+						&wqe->xmit_els_rsp.wqe_dest, 0);
+				}
+			}
+		}
 		bf_set(wqe_ct, &wqe->xmit_els_rsp.wqe_com,
 		       ((iocbq->iocb.ulpCt_h << 1) | iocbq->iocb.ulpCt_l));
 		bf_set(wqe_pu, &wqe->xmit_els_rsp.wqe_com, iocbq->iocb.ulpPU);
@@ -7932,11 +8114,11 @@ lpfc_sli4_iocb2wqe(struct lpfc_hba *phba, struct lpfc_iocbq *iocbq,
 		pcmd = (uint32_t *) (((struct lpfc_dmabuf *)
 					iocbq->context2)->virt);
 		if (phba->fc_topology == LPFC_TOPOLOGY_LOOP) {
-				bf_set(els_req64_sp, &wqe->els_req, 1);
-				bf_set(els_req64_sid, &wqe->els_req,
+				bf_set(els_rsp64_sp, &wqe->xmit_els_rsp, 1);
+				bf_set(els_rsp64_sid, &wqe->xmit_els_rsp,
 					iocbq->vport->fc_myDID);
-				bf_set(wqe_ct, &wqe->els_req.wqe_com, 1);
-				bf_set(wqe_ctxt_tag, &wqe->els_req.wqe_com,
+				bf_set(wqe_ct, &wqe->xmit_els_rsp.wqe_com, 1);
+				bf_set(wqe_ctxt_tag, &wqe->xmit_els_rsp.wqe_com,
 					phba->vpi_ids[phba->pport->vpi]);
 		}
 		command_type = OTHER_COMMAND;
@@ -13080,9 +13262,7 @@ lpfc_sli4_alloc_xri(struct lpfc_hba *phba)
 	} else {
 		set_bit(xri, phba->sli4_hba.xri_bmask);
 		phba->sli4_hba.max_cfg_param.xri_used++;
-		phba->sli4_hba.xri_count++;
 	}
-
 	spin_unlock_irq(&phba->hbalock);
 	return xri;
 }
@@ -13098,7 +13278,6 @@ void
 __lpfc_sli4_free_xri(struct lpfc_hba *phba, int xri)
 {
 	if (test_and_clear_bit(xri, phba->sli4_hba.xri_bmask)) {
-		phba->sli4_hba.xri_count--;
 		phba->sli4_hba.max_cfg_param.xri_used--;
 	}
 }
@@ -13134,46 +13313,45 @@ lpfc_sli4_next_xritag(struct lpfc_hba *phba)
 	uint16_t xri_index;
 
 	xri_index = lpfc_sli4_alloc_xri(phba);
-	if (xri_index != NO_XRI)
-		return xri_index;
-
-	lpfc_printf_log(phba, KERN_ERR, LOG_SLI,
-			"2004 Failed to allocate XRI.last XRITAG is %d"
-			" Max XRI is %d, Used XRI is %d\n",
-			xri_index,
-			phba->sli4_hba.max_cfg_param.max_xri,
-			phba->sli4_hba.max_cfg_param.xri_used);
-	return NO_XRI;
+	if (xri_index == NO_XRI)
+		lpfc_printf_log(phba, KERN_WARNING, LOG_SLI,
+				"2004 Failed to allocate XRI.last XRITAG is %d"
+				" Max XRI is %d, Used XRI is %d\n",
+				xri_index,
+				phba->sli4_hba.max_cfg_param.max_xri,
+				phba->sli4_hba.max_cfg_param.xri_used);
+	return xri_index;
 }
 
 /**
  * lpfc_sli4_post_els_sgl_list - post a block of ELS sgls to the port.
  * @phba: pointer to lpfc hba data structure.
+ * @post_sgl_list: pointer to els sgl entry list.
+ * @count: number of els sgl entries on the list.
  *
  * This routine is invoked to post a block of driver's sgl pages to the
  * HBA using non-embedded mailbox command. No Lock is held. This routine
  * is only called when the driver is loading and after all IO has been
  * stopped.
  **/
-int
-lpfc_sli4_post_els_sgl_list(struct lpfc_hba *phba)
+static int
+lpfc_sli4_post_els_sgl_list(struct lpfc_hba *phba,
+			    struct list_head *post_sgl_list,
+			    int post_cnt)
 {
-	struct lpfc_sglq *sglq_entry;
+	struct lpfc_sglq *sglq_entry = NULL, *sglq_next = NULL;
 	struct lpfc_mbx_post_uembed_sgl_page1 *sgl;
 	struct sgl_page_pairs *sgl_pg_pairs;
 	void *viraddr;
 	LPFC_MBOXQ_t *mbox;
 	uint32_t reqlen, alloclen, pg_pairs;
 	uint32_t mbox_tmo;
-	uint16_t xritag_start = 0, lxri = 0;
-	int els_xri_cnt, rc = 0;
+	uint16_t xritag_start = 0;
+	int rc = 0;
 	uint32_t shdr_status, shdr_add_status;
 	union lpfc_sli4_cfg_shdr *shdr;
 
-	/* The number of sgls to be posted */
-	els_xri_cnt = lpfc_sli4_get_els_iocb_cnt(phba);
-
-	reqlen = els_xri_cnt * sizeof(struct sgl_page_pairs) +
+	reqlen = phba->sli4_hba.els_xri_cnt * sizeof(struct sgl_page_pairs) +
 		 sizeof(union lpfc_sli4_cfg_shdr) + sizeof(uint32_t);
 	if (reqlen > SLI4_PAGE_SIZE) {
 		lpfc_printf_log(phba, KERN_WARNING, LOG_INIT,
@@ -13203,25 +13381,8 @@ lpfc_sli4_post_els_sgl_list(struct lpfc_hba *phba)
 	sgl = (struct lpfc_mbx_post_uembed_sgl_page1 *)viraddr;
 	sgl_pg_pairs = &sgl->sgl_pg_pairs;
 
-	for (pg_pairs = 0; pg_pairs < els_xri_cnt; pg_pairs++) {
-		sglq_entry = phba->sli4_hba.lpfc_els_sgl_array[pg_pairs];
-
-		/*
-		 * Assign the sglq a physical xri only if the driver has not
-		 * initialized those resources.  A port reset only needs
-		 * the sglq's posted.
-		 */
-		if (bf_get(lpfc_xri_rsrc_rdy, &phba->sli4_hba.sli4_flags) !=
-		    LPFC_XRI_RSRC_RDY) {
-			lxri = lpfc_sli4_next_xritag(phba);
-			if (lxri == NO_XRI) {
-				lpfc_sli4_mbox_cmd_free(phba, mbox);
-				return -ENOMEM;
-			}
-			sglq_entry->sli4_lxritag = lxri;
-			sglq_entry->sli4_xritag = phba->sli4_hba.xri_ids[lxri];
-		}
-
+	pg_pairs = 0;
+	list_for_each_entry_safe(sglq_entry, sglq_next, post_sgl_list, list) {
 		/* Set up the sge entry */
 		sgl_pg_pairs->sgl_pg0_addr_lo =
 				cpu_to_le32(putPaddrLow(sglq_entry->phys));
@@ -13236,11 +13397,12 @@ lpfc_sli4_post_els_sgl_list(struct lpfc_hba *phba)
 		if (pg_pairs == 0)
 			xritag_start = sglq_entry->sli4_xritag;
 		sgl_pg_pairs++;
+		pg_pairs++;
 	}
 
 	/* Complete initialization and perform endian conversion. */
 	bf_set(lpfc_post_sgl_pages_xri, sgl, xritag_start);
-	bf_set(lpfc_post_sgl_pages_xricnt, sgl, els_xri_cnt);
+	bf_set(lpfc_post_sgl_pages_xricnt, sgl, phba->sli4_hba.els_xri_cnt);
 	sgl->word0 = cpu_to_le32(sgl->word0);
 	if (!phba->sli4_hba.intr_enable)
 		rc = lpfc_sli_issue_mbox(phba, mbox, MBX_POLL);
@@ -13260,183 +13422,6 @@ lpfc_sli4_post_els_sgl_list(struct lpfc_hba *phba)
 				shdr_status, shdr_add_status, rc);
 		rc = -ENXIO;
 	}
-
-	if (rc == 0)
-		bf_set(lpfc_xri_rsrc_rdy, &phba->sli4_hba.sli4_flags,
-		       LPFC_XRI_RSRC_RDY);
-	return rc;
-}
-
-/**
- * lpfc_sli4_post_els_sgl_list_ext - post a block of ELS sgls to the port.
- * @phba: pointer to lpfc hba data structure.
- *
- * This routine is invoked to post a block of driver's sgl pages to the
- * HBA using non-embedded mailbox command. No Lock is held. This routine
- * is only called when the driver is loading and after all IO has been
- * stopped.
- **/
-int
-lpfc_sli4_post_els_sgl_list_ext(struct lpfc_hba *phba)
-{
-	struct lpfc_sglq *sglq_entry;
-	struct lpfc_mbx_post_uembed_sgl_page1 *sgl;
-	struct sgl_page_pairs *sgl_pg_pairs;
-	void *viraddr;
-	LPFC_MBOXQ_t *mbox;
-	uint32_t reqlen, alloclen, index;
-	uint32_t mbox_tmo;
-	uint16_t rsrc_start, rsrc_size, els_xri_cnt, post_els_xri_cnt;
-	uint16_t xritag_start = 0, lxri = 0;
-	struct lpfc_rsrc_blks *rsrc_blk;
-	int cnt, ttl_cnt, rc = 0;
-	int loop_cnt;
-	uint32_t shdr_status, shdr_add_status;
-	union lpfc_sli4_cfg_shdr *shdr;
-
-	/* The number of sgls to be posted */
-	els_xri_cnt = lpfc_sli4_get_els_iocb_cnt(phba);
-
-	reqlen = els_xri_cnt * sizeof(struct sgl_page_pairs) +
-		 sizeof(union lpfc_sli4_cfg_shdr) + sizeof(uint32_t);
-	if (reqlen > SLI4_PAGE_SIZE) {
-		lpfc_printf_log(phba, KERN_WARNING, LOG_INIT,
-				"2989 Block sgl registration required DMA "
-				"size (%d) great than a page\n", reqlen);
-		return -ENOMEM;
-	}
-
-	cnt = 0;
-	ttl_cnt = 0;
-	post_els_xri_cnt = els_xri_cnt;
-	list_for_each_entry(rsrc_blk, &phba->sli4_hba.lpfc_xri_blk_list,
-			    list) {
-		rsrc_start = rsrc_blk->rsrc_start;
-		rsrc_size = rsrc_blk->rsrc_size;
-
-		lpfc_printf_log(phba, KERN_INFO, LOG_INIT,
-				"3014 Working ELS Extent start %d, cnt %d\n",
-				rsrc_start, rsrc_size);
-
-		loop_cnt = min(post_els_xri_cnt, rsrc_size);
-		if (loop_cnt < post_els_xri_cnt) {
-			post_els_xri_cnt -= loop_cnt;
-			ttl_cnt += loop_cnt;
-		} else
-			ttl_cnt += post_els_xri_cnt;
-
-		mbox = mempool_alloc(phba->mbox_mem_pool, GFP_KERNEL);
-		if (!mbox)
-			return -ENOMEM;
-		/*
-		 * Allocate DMA memory and set up the non-embedded mailbox
-		 * command.
-		 */
-		alloclen = lpfc_sli4_config(phba, mbox,
-					LPFC_MBOX_SUBSYSTEM_FCOE,
-					LPFC_MBOX_OPCODE_FCOE_POST_SGL_PAGES,
-					reqlen, LPFC_SLI4_MBX_NEMBED);
-		if (alloclen < reqlen) {
-			lpfc_printf_log(phba, KERN_ERR, LOG_INIT,
-					"2987 Allocated DMA memory size (%d) "
-					"is less than the requested DMA memory "
-					"size (%d)\n", alloclen, reqlen);
-			lpfc_sli4_mbox_cmd_free(phba, mbox);
-			return -ENOMEM;
-		}
-
-		/* Set up the SGL pages in the non-embedded DMA pages */
-		viraddr = mbox->sge_array->addr[0];
-		sgl = (struct lpfc_mbx_post_uembed_sgl_page1 *)viraddr;
-		sgl_pg_pairs = &sgl->sgl_pg_pairs;
-
-		/*
-		 * The starting resource may not begin at zero. Control
-		 * the loop variants via the block resource parameters,
-		 * but handle the sge pointers with a zero-based index
-		 * that doesn't get reset per loop pass.
-		 */
-		for (index = rsrc_start;
-		     index < rsrc_start + loop_cnt;
-		     index++) {
-			sglq_entry = phba->sli4_hba.lpfc_els_sgl_array[cnt];
-
-			/*
-			 * Assign the sglq a physical xri only if the driver
-			 * has not initialized those resources.  A port reset
-			 * only needs the sglq's posted.
-			 */
-			if (bf_get(lpfc_xri_rsrc_rdy,
-				   &phba->sli4_hba.sli4_flags) !=
-				   LPFC_XRI_RSRC_RDY) {
-				lxri = lpfc_sli4_next_xritag(phba);
-				if (lxri == NO_XRI) {
-					lpfc_sli4_mbox_cmd_free(phba, mbox);
-					rc = -ENOMEM;
-					goto err_exit;
-				}
-				sglq_entry->sli4_lxritag = lxri;
-				sglq_entry->sli4_xritag =
-						phba->sli4_hba.xri_ids[lxri];
-			}
-
-			/* Set up the sge entry */
-			sgl_pg_pairs->sgl_pg0_addr_lo =
-				cpu_to_le32(putPaddrLow(sglq_entry->phys));
-			sgl_pg_pairs->sgl_pg0_addr_hi =
-				cpu_to_le32(putPaddrHigh(sglq_entry->phys));
-			sgl_pg_pairs->sgl_pg1_addr_lo =
-				cpu_to_le32(putPaddrLow(0));
-			sgl_pg_pairs->sgl_pg1_addr_hi =
-				cpu_to_le32(putPaddrHigh(0));
-
-			/* Track the starting physical XRI for the mailbox. */
-			if (index == rsrc_start)
-				xritag_start = sglq_entry->sli4_xritag;
-			sgl_pg_pairs++;
-			cnt++;
-		}
-
-		/* Complete initialization and perform endian conversion. */
-		rsrc_blk->rsrc_used += loop_cnt;
-		bf_set(lpfc_post_sgl_pages_xri, sgl, xritag_start);
-		bf_set(lpfc_post_sgl_pages_xricnt, sgl, loop_cnt);
-		sgl->word0 = cpu_to_le32(sgl->word0);
-
-		lpfc_printf_log(phba, KERN_INFO, LOG_INIT,
-				"3015 Post ELS Extent SGL, start %d, "
-				"cnt %d, used %d\n",
-				xritag_start, loop_cnt, rsrc_blk->rsrc_used);
-		if (!phba->sli4_hba.intr_enable)
-			rc = lpfc_sli_issue_mbox(phba, mbox, MBX_POLL);
-		else {
-			mbox_tmo = lpfc_mbox_tmo_val(phba, mbox);
-			rc = lpfc_sli_issue_mbox_wait(phba, mbox, mbox_tmo);
-		}
-		shdr = (union lpfc_sli4_cfg_shdr *) &sgl->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 (rc != MBX_TIMEOUT)
-			lpfc_sli4_mbox_cmd_free(phba, mbox);
-		if (shdr_status || shdr_add_status || rc) {
-			lpfc_printf_log(phba, KERN_ERR, LOG_SLI,
-					"2988 POST_SGL_BLOCK mailbox "
-					"command failed status x%x "
-					"add_status x%x mbx status x%x\n",
-					shdr_status, shdr_add_status, rc);
-			rc = -ENXIO;
-			goto err_exit;
-		}
-		if (ttl_cnt >= els_xri_cnt)
-			break;
-	}
-
- err_exit:
-	if (rc == 0)
-		bf_set(lpfc_xri_rsrc_rdy, &phba->sli4_hba.sli4_flags,
-		       LPFC_XRI_RSRC_RDY);
 	return rc;
 }
 
@@ -13452,8 +13437,9 @@ lpfc_sli4_post_els_sgl_list_ext(struct lpfc_hba *phba)
  *
  **/
 int
-lpfc_sli4_post_scsi_sgl_block(struct lpfc_hba *phba, struct list_head *sblist,
-			      int cnt)
+lpfc_sli4_post_scsi_sgl_block(struct lpfc_hba *phba,
+			      struct list_head *sblist,
+			      int count)
 {
 	struct lpfc_scsi_buf *psb;
 	struct lpfc_mbx_post_uembed_sgl_page1 *sgl;
@@ -13469,7 +13455,7 @@ lpfc_sli4_post_scsi_sgl_block(struct lpfc_hba *phba, struct list_head *sblist,
 	union lpfc_sli4_cfg_shdr *shdr;
 
 	/* Calculate the requested length of the dma memory */
-	reqlen = cnt * sizeof(struct sgl_page_pairs) +
+	reqlen = count * sizeof(struct sgl_page_pairs) +
 		 sizeof(union lpfc_sli4_cfg_shdr) + sizeof(uint32_t);
 	if (reqlen > SLI4_PAGE_SIZE) {
 		lpfc_printf_log(phba, KERN_WARNING, LOG_INIT,
@@ -13553,169 +13539,6 @@ lpfc_sli4_post_scsi_sgl_block(struct lpfc_hba *phba, struct list_head *sblist,
 }
 
 /**
- * lpfc_sli4_post_scsi_sgl_blk_ext - post a block of scsi sgls to the port.
- * @phba: pointer to lpfc hba data structure.
- * @sblist: pointer to scsi buffer list.
- * @count: number of scsi buffers on the list.
- *
- * This routine is invoked to post a block of @count scsi sgl pages from a
- * SCSI buffer list @sblist to the HBA using non-embedded mailbox command.
- * No Lock is held.
- *
- **/
-int
-lpfc_sli4_post_scsi_sgl_blk_ext(struct lpfc_hba *phba, struct list_head *sblist,
-				int cnt)
-{
-	struct lpfc_scsi_buf *psb = NULL;
-	struct lpfc_mbx_post_uembed_sgl_page1 *sgl;
-	struct sgl_page_pairs *sgl_pg_pairs;
-	void *viraddr;
-	LPFC_MBOXQ_t *mbox;
-	uint32_t reqlen, alloclen, pg_pairs;
-	uint32_t mbox_tmo;
-	uint16_t xri_start = 0, scsi_xri_start;
-	uint16_t rsrc_range;
-	int rc = 0, avail_cnt;
-	uint32_t shdr_status, shdr_add_status;
-	dma_addr_t pdma_phys_bpl1;
-	union lpfc_sli4_cfg_shdr *shdr;
-	struct lpfc_rsrc_blks *rsrc_blk;
-	uint32_t xri_cnt = 0;
-
-	/* Calculate the total requested length of the dma memory */
-	reqlen = cnt * sizeof(struct sgl_page_pairs) +
-		 sizeof(union lpfc_sli4_cfg_shdr) + sizeof(uint32_t);
-	if (reqlen > SLI4_PAGE_SIZE) {
-		lpfc_printf_log(phba, KERN_WARNING, LOG_INIT,
-				"2932 Block sgl registration required DMA "
-				"size (%d) great than a page\n", reqlen);
-		return -ENOMEM;
-	}
-
-	/*
-	 * The use of extents requires the driver to post the sgl headers
-	 * in multiple postings to meet the contiguous resource assignment.
-	 */
-	psb = list_prepare_entry(psb, sblist, list);
-	scsi_xri_start = phba->sli4_hba.scsi_xri_start;
-	list_for_each_entry(rsrc_blk, &phba->sli4_hba.lpfc_xri_blk_list,
-			    list) {
-		rsrc_range = rsrc_blk->rsrc_start + rsrc_blk->rsrc_size;
-		if (rsrc_range < scsi_xri_start)
-			continue;
-		else if (rsrc_blk->rsrc_used >= rsrc_blk->rsrc_size)
-			continue;
-		else
-			avail_cnt = rsrc_blk->rsrc_size - rsrc_blk->rsrc_used;
-
-		reqlen = (avail_cnt * sizeof(struct sgl_page_pairs)) +
-			sizeof(union lpfc_sli4_cfg_shdr) + sizeof(uint32_t);
-		/*
-		 * Allocate DMA memory and set up the non-embedded mailbox
-		 * command. The mbox is used to post an SGL page per loop
-		 * but the DMA memory has a use-once semantic so the mailbox
-		 * is used and freed per loop pass.
-		 */
-		mbox = mempool_alloc(phba->mbox_mem_pool, GFP_KERNEL);
-		if (!mbox) {
-			lpfc_printf_log(phba, KERN_ERR, LOG_INIT,
-					"2933 Failed to allocate mbox cmd "
-					"memory\n");
-			return -ENOMEM;
-		}
-		alloclen = lpfc_sli4_config(phba, mbox,
-					LPFC_MBOX_SUBSYSTEM_FCOE,
-					LPFC_MBOX_OPCODE_FCOE_POST_SGL_PAGES,
-					reqlen,
-					LPFC_SLI4_MBX_NEMBED);
-		if (alloclen < reqlen) {
-			lpfc_printf_log(phba, KERN_ERR, LOG_INIT,
-					"2934 Allocated DMA memory size (%d) "
-					"is less than the requested DMA memory "
-					"size (%d)\n", alloclen, reqlen);
-			lpfc_sli4_mbox_cmd_free(phba, mbox);
-			return -ENOMEM;
-		}
-
-		/* Get the first SGE entry from the non-embedded DMA memory */
-		viraddr = mbox->sge_array->addr[0];
-
-		/* Set up the SGL pages in the non-embedded DMA pages */
-		sgl = (struct lpfc_mbx_post_uembed_sgl_page1 *)viraddr;
-		sgl_pg_pairs = &sgl->sgl_pg_pairs;
-
-		/* pg_pairs tracks posted SGEs per loop iteration. */
-		pg_pairs = 0;
-		list_for_each_entry_continue(psb, sblist, list) {
-			/* Set up the sge entry */
-			sgl_pg_pairs->sgl_pg0_addr_lo =
-				cpu_to_le32(putPaddrLow(psb->dma_phys_bpl));
-			sgl_pg_pairs->sgl_pg0_addr_hi =
-				cpu_to_le32(putPaddrHigh(psb->dma_phys_bpl));
-			if (phba->cfg_sg_dma_buf_size > SGL_PAGE_SIZE)
-				pdma_phys_bpl1 = psb->dma_phys_bpl +
-					SGL_PAGE_SIZE;
-			else
-				pdma_phys_bpl1 = 0;
-			sgl_pg_pairs->sgl_pg1_addr_lo =
-				cpu_to_le32(putPaddrLow(pdma_phys_bpl1));
-			sgl_pg_pairs->sgl_pg1_addr_hi =
-				cpu_to_le32(putPaddrHigh(pdma_phys_bpl1));
-			/* Keep the first xri for this extent. */
-			if (pg_pairs == 0)
-				xri_start = psb->cur_iocbq.sli4_xritag;
-			sgl_pg_pairs++;
-			pg_pairs++;
-			xri_cnt++;
-
-			/*
-			 * Track two exit conditions - the loop has constructed
-			 * all of the caller's SGE pairs or all available
-			 * resource IDs in this extent are consumed.
-			 */
-			if ((xri_cnt == cnt) || (pg_pairs >= avail_cnt))
-				break;
-		}
-		rsrc_blk->rsrc_used += pg_pairs;
-		bf_set(lpfc_post_sgl_pages_xri, sgl, xri_start);
-		bf_set(lpfc_post_sgl_pages_xricnt, sgl, pg_pairs);
-
-		lpfc_printf_log(phba, KERN_INFO, LOG_INIT,
-				"3016 Post SCSI Extent SGL, start %d, cnt %d "
-				"blk use %d\n",
-				xri_start, pg_pairs, rsrc_blk->rsrc_used);
-		/* Perform endian conversion if necessary */
-		sgl->word0 = cpu_to_le32(sgl->word0);
-		if (!phba->sli4_hba.intr_enable)
-			rc = lpfc_sli_issue_mbox(phba, mbox, MBX_POLL);
-		else {
-			mbox_tmo = lpfc_mbox_tmo_val(phba, mbox);
-			rc = lpfc_sli_issue_mbox_wait(phba, mbox, mbox_tmo);
-		}
-		shdr = (union lpfc_sli4_cfg_shdr *) &sgl->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 (rc != MBX_TIMEOUT)
-			lpfc_sli4_mbox_cmd_free(phba, mbox);
-		if (shdr_status || shdr_add_status || rc) {
-			lpfc_printf_log(phba, KERN_ERR, LOG_SLI,
-					"2935 POST_SGL_BLOCK mailbox command "
-					"failed status x%x add_status x%x "
-					"mbx status x%x\n",
-					shdr_status, shdr_add_status, rc);
-			return -ENXIO;
-		}
-
-		/* Post only what is requested. */
-		if (xri_cnt >= cnt)
-			break;
-	}
-	return rc;
-}
-
-/**
  * lpfc_fc_frame_check - Check that this frame is a valid frame to handle
  * @phba: pointer to lpfc_hba struct that the frame was received on
  * @fc_hdr: A pointer to the FC Header data (In Big Endian Format)
@@ -13839,8 +13662,13 @@ lpfc_fc_frame_to_vport(struct lpfc_hba *phba, struct fc_frame_header *fc_hdr,
 	uint32_t did = (fc_hdr->fh_d_id[0] << 16 |
 			fc_hdr->fh_d_id[1] << 8 |
 			fc_hdr->fh_d_id[2]);
+
 	if (did == Fabric_DID)
 		return phba->pport;
+	if ((phba->pport->fc_flag & FC_PT2PT) &&
+		!(phba->link_state == LPFC_HBA_READY))
+		return phba->pport;
+
 	vports = lpfc_create_vport_work_array(phba);
 	if (vports != NULL)
 		for (i = 0; i <= phba->max_vpi && vports[i] != NULL; i++) {
@@ -14133,7 +13961,6 @@ lpfc_sli4_xri_inrange(struct lpfc_hba *phba,
 	return NO_XRI;
 }
 
-
 /**
  * lpfc_sli4_seq_abort_rsp - bls rsp to sequence abort
  * @phba: Pointer to HBA context object.
@@ -14148,7 +13975,7 @@ lpfc_sli4_seq_abort_rsp(struct lpfc_hba *phba,
 {
 	struct lpfc_iocbq *ctiocb = NULL;
 	struct lpfc_nodelist *ndlp;
-	uint16_t oxid, rxid;
+	uint16_t oxid, rxid, xri, lxri;
 	uint32_t sid, fctl;
 	IOCB_t *icmd;
 	int rc;
@@ -14167,8 +13994,6 @@ lpfc_sli4_seq_abort_rsp(struct lpfc_hba *phba,
 				"SID:x%x\n", oxid, sid);
 		return;
 	}
-	if (lpfc_sli4_xri_inrange(phba, rxid))
-		lpfc_set_rrq_active(phba, ndlp, rxid, oxid, 0);
 
 	/* Allocate buffer for rsp iocb */
 	ctiocb = lpfc_sli_get_iocbq(phba);
@@ -14199,13 +14024,24 @@ lpfc_sli4_seq_abort_rsp(struct lpfc_hba *phba,
 	ctiocb->sli4_lxritag = NO_XRI;
 	ctiocb->sli4_xritag = NO_XRI;
 
+	if (fctl & FC_FC_EX_CTX)
+		/* Exchange responder sent the abort so we
+		 * own the oxid.
+		 */
+		xri = oxid;
+	else
+		xri = rxid;
+	lxri = lpfc_sli4_xri_inrange(phba, xri);
+	if (lxri != NO_XRI)
+		lpfc_set_rrq_active(phba, ndlp, lxri,
+			(xri == oxid) ? rxid : oxid, 0);
 	/* If the oxid maps to the FCP XRI range or if it is out of range,
 	 * send a BLS_RJT.  The driver no longer has that exchange.
 	 * Override the IOCB for a BA_RJT.
 	 */
-	if (oxid > (phba->sli4_hba.max_cfg_param.max_xri +
+	if (xri > (phba->sli4_hba.max_cfg_param.max_xri +
 		    phba->sli4_hba.max_cfg_param.xri_base) ||
-	    oxid > (lpfc_sli4_get_els_iocb_cnt(phba) +
+	    xri > (lpfc_sli4_get_els_iocb_cnt(phba) +
 		    phba->sli4_hba.max_cfg_param.xri_base)) {
 		icmd->un.xseq64.w5.hcsw.Rctl = FC_RCTL_BA_RJT;
 		bf_set(lpfc_vndr_code, &icmd->un.bls_rsp, 0);
@@ -14377,7 +14213,15 @@ lpfc_prep_seq(struct lpfc_vport *vport, struct hbq_dmabuf *seq_dmabuf)
 		/* Initialize the first IOCB. */
 		first_iocbq->iocb.unsli3.rcvsli3.acc_len = 0;
 		first_iocbq->iocb.ulpStatus = IOSTAT_SUCCESS;
-		first_iocbq->iocb.ulpCommand = CMD_IOCB_RCV_SEQ64_CX;
+
+		/* Check FC Header to see what TYPE of frame we are rcv'ing */
+		if (sli4_type_from_fc_hdr(fc_hdr) == FC_TYPE_ELS) {
+			first_iocbq->iocb.ulpCommand = CMD_IOCB_RCV_ELS64_CX;
+			first_iocbq->iocb.un.rcvels.parmRo =
+				sli4_did_from_fc_hdr(fc_hdr);
+			first_iocbq->iocb.ulpPU = PARM_NPIV_DID;
+		} else
+			first_iocbq->iocb.ulpCommand = CMD_IOCB_RCV_SEQ64_CX;
 		first_iocbq->iocb.ulpContext = NO_XRI;
 		first_iocbq->iocb.unsli3.rcvsli3.ox_id =
 			be16_to_cpu(fc_hdr->fh_ox_id);
@@ -14507,6 +14351,7 @@ lpfc_sli4_handle_received_buffer(struct lpfc_hba *phba,
 	struct fc_frame_header *fc_hdr;
 	struct lpfc_vport *vport;
 	uint32_t fcfi;
+	uint32_t did;
 
 	/* Process each received buffer */
 	fc_hdr = (struct fc_frame_header *)dmabuf->hbuf.virt;
@@ -14522,12 +14367,32 @@ lpfc_sli4_handle_received_buffer(struct lpfc_hba *phba,
 	else
 		fcfi = bf_get(lpfc_rcqe_fcf_id,
 			      &dmabuf->cq_event.cqe.rcqe_cmpl);
+
 	vport = lpfc_fc_frame_to_vport(phba, fc_hdr, fcfi);
-	if (!vport || !(vport->vpi_state & LPFC_VPI_REGISTERED)) {
+	if (!vport) {
 		/* throw out the frame */
 		lpfc_in_buf_free(phba, &dmabuf->dbuf);
 		return;
 	}
+
+	/* d_id this frame is directed to */
+	did = sli4_did_from_fc_hdr(fc_hdr);
+
+	/* vport is registered unless we rcv a FLOGI directed to Fabric_DID */
+	if (!(vport->vpi_state & LPFC_VPI_REGISTERED) &&
+		(did != Fabric_DID)) {
+		/*
+		 * Throw out the frame if we are not pt2pt.
+		 * The pt2pt protocol allows for discovery frames
+		 * to be received without a registered VPI.
+		 */
+		if (!(vport->fc_flag & FC_PT2PT) ||
+			(phba->link_state == LPFC_HBA_READY)) {
+			lpfc_in_buf_free(phba, &dmabuf->dbuf);
+			return;
+		}
+	}
+
 	/* Handle the basic abort sequence (BA_ABTS) event */
 	if (fc_hdr->fh_r_ctl == FC_RCTL_BA_ABTS) {
 		lpfc_sli4_handle_unsol_abort(vport, dmabuf);
diff --git a/drivers/scsi/lpfc/lpfc_sli.h b/drivers/scsi/lpfc/lpfc_sli.h
index 3290b8e7ab6..2626f58c074 100644
--- a/drivers/scsi/lpfc/lpfc_sli.h
+++ b/drivers/scsi/lpfc/lpfc_sli.h
@@ -68,7 +68,7 @@ struct lpfc_iocbq {
 #define LPFC_EXCHANGE_BUSY	0x40    /* SLI4 hba reported XB in response */
 #define LPFC_USE_FCPWQIDX	0x80    /* Submit to specified FCPWQ index */
 #define DSS_SECURITY_OP		0x100	/* security IO */
-#define LPFC_IO_ON_Q		0x200	/* The IO is still on the TXCMPLQ */
+#define LPFC_IO_ON_TXCMPLQ	0x200	/* The IO is still on the TXCMPLQ */
 #define LPFC_IO_DIF		0x400	/* T10 DIF IO */
 
 #define LPFC_FIP_ELS_ID_MASK	0xc000	/* ELS_ID range 0-3, non-shifted mask */
diff --git a/drivers/scsi/lpfc/lpfc_sli4.h b/drivers/scsi/lpfc/lpfc_sli4.h
index c19d139618b..a4a77080091 100644
--- a/drivers/scsi/lpfc/lpfc_sli4.h
+++ b/drivers/scsi/lpfc/lpfc_sli4.h
@@ -75,11 +75,19 @@
 	 (fc_hdr)->fh_s_id[1] <<  8 | \
 	 (fc_hdr)->fh_s_id[2])
 
+#define sli4_did_from_fc_hdr(fc_hdr)  \
+	((fc_hdr)->fh_d_id[0] << 16 | \
+	 (fc_hdr)->fh_d_id[1] <<  8 | \
+	 (fc_hdr)->fh_d_id[2])
+
 #define sli4_fctl_from_fc_hdr(fc_hdr)  \
 	((fc_hdr)->fh_f_ctl[0] << 16 | \
 	 (fc_hdr)->fh_f_ctl[1] <<  8 | \
 	 (fc_hdr)->fh_f_ctl[2])
 
+#define sli4_type_from_fc_hdr(fc_hdr)  \
+	((fc_hdr)->fh_type)
+
 #define LPFC_FW_RESET_MAXIMUM_WAIT_10MS_CNT 12000
 
 enum lpfc_sli4_queue_type {
@@ -493,14 +501,12 @@ struct lpfc_sli4_hba {
 	uint16_t next_rpi;
 	uint16_t scsi_xri_max;
 	uint16_t scsi_xri_cnt;
+	uint16_t els_xri_cnt;
 	uint16_t scsi_xri_start;
 	struct list_head lpfc_free_sgl_list;
 	struct list_head lpfc_sgl_list;
-	struct lpfc_sglq **lpfc_els_sgl_array;
 	struct list_head lpfc_abts_els_sgl_list;
-	struct lpfc_scsi_buf **lpfc_scsi_psb_array;
 	struct list_head lpfc_abts_scsi_buf_list;
-	uint32_t total_sglq_bufs;
 	struct lpfc_sglq **lpfc_sglq_active_list;
 	struct list_head lpfc_rpi_hdr_list;
 	unsigned long *rpi_bmask;
@@ -509,7 +515,6 @@ struct lpfc_sli4_hba {
 	struct list_head lpfc_rpi_blk_list;
 	unsigned long *xri_bmask;
 	uint16_t *xri_ids;
-	uint16_t xri_count;
 	struct list_head lpfc_xri_blk_list;
 	unsigned long *vfi_bmask;
 	uint16_t *vfi_ids;
@@ -614,11 +619,7 @@ int lpfc_sli4_post_sgl(struct lpfc_hba *, dma_addr_t, dma_addr_t, uint16_t);
 int lpfc_sli4_repost_scsi_sgl_list(struct lpfc_hba *);
 uint16_t lpfc_sli4_next_xritag(struct lpfc_hba *);
 int lpfc_sli4_post_async_mbox(struct lpfc_hba *);
-int lpfc_sli4_post_els_sgl_list(struct lpfc_hba *phba);
-int lpfc_sli4_post_els_sgl_list_ext(struct lpfc_hba *phba);
 int lpfc_sli4_post_scsi_sgl_block(struct lpfc_hba *, struct list_head *, int);
-int lpfc_sli4_post_scsi_sgl_blk_ext(struct lpfc_hba *, struct list_head *,
-				    int);
 struct lpfc_cq_event *__lpfc_sli4_cq_event_alloc(struct lpfc_hba *);
 struct lpfc_cq_event *lpfc_sli4_cq_event_alloc(struct lpfc_hba *);
 void __lpfc_sli4_cq_event_release(struct lpfc_hba *, struct lpfc_cq_event *);
diff --git a/drivers/scsi/lpfc/lpfc_version.h b/drivers/scsi/lpfc/lpfc_version.h
index 25cefc254b7..59c57a40998 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.30"
+#define LPFC_DRIVER_VERSION "8.3.31"
 #define LPFC_DRIVER_NAME		"lpfc"
 #define LPFC_SP_DRIVER_HANDLER_NAME	"lpfc:sp"
 #define LPFC_FP_DRIVER_HANDLER_NAME	"lpfc:fp"
diff --git a/drivers/scsi/megaraid/megaraid_sas.h b/drivers/scsi/megaraid/megaraid_sas.h
index e5f416f8042..e8f89264768 100644
--- a/drivers/scsi/megaraid/megaraid_sas.h
+++ b/drivers/scsi/megaraid/megaraid_sas.h
@@ -33,9 +33,9 @@
 /*
  * MegaRAID SAS Driver meta data
  */
-#define MEGASAS_VERSION				"00.00.06.14-rc1"
-#define MEGASAS_RELDATE				"Jan. 6, 2012"
-#define MEGASAS_EXT_VERSION			"Fri. Jan. 6 17:00:00 PDT 2012"
+#define MEGASAS_VERSION				"00.00.06.15-rc1"
+#define MEGASAS_RELDATE				"Mar. 19, 2012"
+#define MEGASAS_EXT_VERSION			"Mon. Mar. 19 17:00:00 PDT 2012"
 
 /*
  * Device IDs
diff --git a/drivers/scsi/megaraid/megaraid_sas_base.c b/drivers/scsi/megaraid/megaraid_sas_base.c
index 8b300be4428..dc27598785e 100644
--- a/drivers/scsi/megaraid/megaraid_sas_base.c
+++ b/drivers/scsi/megaraid/megaraid_sas_base.c
@@ -18,7 +18,7 @@
  *  Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
  *
  *  FILE: megaraid_sas_base.c
- *  Version : v00.00.06.14-rc1
+ *  Version : v00.00.06.15-rc1
  *
  *  Authors: LSI Corporation
  *           Sreenivas Bagalkote
diff --git a/drivers/scsi/megaraid/megaraid_sas_fp.c b/drivers/scsi/megaraid/megaraid_sas_fp.c
index 294abb0defa..e3d251a2e26 100644
--- a/drivers/scsi/megaraid/megaraid_sas_fp.c
+++ b/drivers/scsi/megaraid/megaraid_sas_fp.c
@@ -362,15 +362,20 @@ MR_BuildRaidContext(struct megasas_instance *instance,
 	/* assume this IO needs the full row - we'll adjust if not true */
 	regSize             = stripSize;
 
-	/* If IO spans more than 1 strip, fp is not possible
-	   FP is not possible for writes on non-0 raid levels
-	   FP is not possible if LD is not capable */
-	if (num_strips > 1 || (!isRead && raid->level != 0) ||
-	    !raid->capability.fpCapable) {
+	/* Check if we can send this I/O via FastPath */
+	if (raid->capability.fpCapable) {
+		if (isRead)
+			io_info->fpOkForIo = (raid->capability.fpReadCapable &&
+					      ((num_strips == 1) ||
+					       raid->capability.
+					       fpReadAcrossStripe));
+		else
+			io_info->fpOkForIo = (raid->capability.fpWriteCapable &&
+					      ((num_strips == 1) ||
+					       raid->capability.
+					       fpWriteAcrossStripe));
+	} else
 		io_info->fpOkForIo = FALSE;
-	} else {
-		io_info->fpOkForIo = TRUE;
-	}
 
 	if (numRows == 1) {
 		/* single-strip IOs can always lock only the data needed */
diff --git a/drivers/scsi/megaraid/megaraid_sas_fusion.c b/drivers/scsi/megaraid/megaraid_sas_fusion.c
index bfd87fab39a..a610cf1d484 100644
--- a/drivers/scsi/megaraid/megaraid_sas_fusion.c
+++ b/drivers/scsi/megaraid/megaraid_sas_fusion.c
@@ -634,9 +634,7 @@ megasas_ioc_init_fusion(struct megasas_instance *instance)
 		fusion->reply_frames_desc_phys;
 	IOCInitMessage->SystemRequestFrameBaseAddress =
 		fusion->io_request_frames_phys;
-	/* Set to 0 for none or 1 MSI-X vectors */
-	IOCInitMessage->HostMSIxVectors = (instance->msix_vectors > 0 ?
-					   instance->msix_vectors : 0);
+	IOCInitMessage->HostMSIxVectors = instance->msix_vectors;
 	init_frame = (struct megasas_init_frame *)cmd->frame;
 	memset(init_frame, 0, MEGAMFI_FRAME_SIZE);
 
diff --git a/drivers/scsi/mpt2sas/mpi/mpi2.h b/drivers/scsi/mpt2sas/mpi/mpi2.h
index a01f0aa66f2..a80f3220c64 100644
--- a/drivers/scsi/mpt2sas/mpi/mpi2.h
+++ b/drivers/scsi/mpt2sas/mpi/mpi2.h
@@ -8,7 +8,7 @@
  *                  scatter/gather formats.
  *  Creation Date:  June 21, 2006
  *
- *  mpi2.h Version:  02.00.22
+ *  mpi2.h Version:  02.00.23
  *
  *  Version History
  *  ---------------
@@ -71,6 +71,7 @@
  *  03-09-11  02.00.20  Bumped MPI2_HEADER_VERSION_UNIT.
  *  05-25-11  02.00.21  Bumped MPI2_HEADER_VERSION_UNIT.
  *  08-24-11  02.00.22  Bumped MPI2_HEADER_VERSION_UNIT.
+ *  11-18-11  02.00.23  Bumped MPI2_HEADER_VERSION_UNIT.
  *  --------------------------------------------------------------------------
  */
 
@@ -96,7 +97,7 @@
 #define MPI2_VERSION_02_00                  (0x0200)
 
 /* versioning for this MPI header set */
-#define MPI2_HEADER_VERSION_UNIT            (0x16)
+#define MPI2_HEADER_VERSION_UNIT            (0x17)
 #define MPI2_HEADER_VERSION_DEV             (0x00)
 #define MPI2_HEADER_VERSION_UNIT_MASK       (0xFF00)
 #define MPI2_HEADER_VERSION_UNIT_SHIFT      (8)
@@ -480,7 +481,7 @@ typedef union _MPI2_REPLY_DESCRIPTORS_UNION
     MPI2_RAID_ACCELERATOR_SUCCESS_REPLY_DESCRIPTOR  RAIDAcceleratorSuccess;
     U64                                             Words;
 } MPI2_REPLY_DESCRIPTORS_UNION, MPI2_POINTER PTR_MPI2_REPLY_DESCRIPTORS_UNION,
-  Mpi2ReplyDescriptorsUnion_t, MPI2_POINTER pMpi2ReplyDescriptorsUnion_t;
+Mpi2ReplyDescriptorsUnion_t, MPI2_POINTER pMpi2ReplyDescriptorsUnion_t;
 
 
 
diff --git a/drivers/scsi/mpt2sas/mpi/mpi2_cnfg.h b/drivers/scsi/mpt2sas/mpi/mpi2_cnfg.h
index 3a023dad77a..737fa8cfb54 100644
--- a/drivers/scsi/mpt2sas/mpi/mpi2_cnfg.h
+++ b/drivers/scsi/mpt2sas/mpi/mpi2_cnfg.h
@@ -6,7 +6,7 @@
  *          Title:  MPI Configuration messages and pages
  *  Creation Date:  November 10, 2006
  *
- *    mpi2_cnfg.h Version:  02.00.21
+ *    mpi2_cnfg.h Version:  02.00.22
  *
  *  Version History
  *  ---------------
@@ -146,7 +146,9 @@
  *                      Added SpinupFlags field containing a Disable Spin-up
  *                      bit to the MPI2_SAS_IOUNIT4_SPINUP_GROUP fields of
  *                      SAS IO Unit Page 4.
-
+ *  11-18-11  02.00.22  Added define MPI2_IOCPAGE6_CAP_FLAGS_4K_SECTORS_SUPPORT.
+ *                      Added UEFIVersion field to BIOS Page 1 and defined new
+ *                      BiosOptions bits.
  *  --------------------------------------------------------------------------
  */
 
@@ -1131,9 +1133,10 @@ typedef struct _MPI2_CONFIG_PAGE_IOC_6
 } MPI2_CONFIG_PAGE_IOC_6, MPI2_POINTER PTR_MPI2_CONFIG_PAGE_IOC_6,
   Mpi2IOCPage6_t, MPI2_POINTER pMpi2IOCPage6_t;
 
-#define MPI2_IOCPAGE6_PAGEVERSION                       (0x04)
+#define MPI2_IOCPAGE6_PAGEVERSION                       (0x05)
 
 /* defines for IOC Page 6 CapabilitiesFlags */
+#define MPI2_IOCPAGE6_CAP_FLAGS_4K_SECTORS_SUPPORT      (0x00000020)
 #define MPI2_IOCPAGE6_CAP_FLAGS_RAID10_SUPPORT          (0x00000010)
 #define MPI2_IOCPAGE6_CAP_FLAGS_RAID1_SUPPORT           (0x00000008)
 #define MPI2_IOCPAGE6_CAP_FLAGS_RAID1E_SUPPORT          (0x00000004)
@@ -1204,24 +1207,29 @@ typedef struct _MPI2_CONFIG_PAGE_IOC_8
 
 typedef struct _MPI2_CONFIG_PAGE_BIOS_1
 {
-    MPI2_CONFIG_PAGE_HEADER Header;                     /* 0x00 */
-    U32                     BiosOptions;                /* 0x04 */
-    U32                     IOCSettings;                /* 0x08 */
-    U32                     Reserved1;                  /* 0x0C */
-    U32                     DeviceSettings;             /* 0x10 */
-    U16                     NumberOfDevices;            /* 0x14 */
-    U16                     Reserved2;                  /* 0x16 */
-    U16                     IOTimeoutBlockDevicesNonRM; /* 0x18 */
-    U16                     IOTimeoutSequential;        /* 0x1A */
-    U16                     IOTimeoutOther;             /* 0x1C */
-    U16                     IOTimeoutBlockDevicesRM;    /* 0x1E */
+	MPI2_CONFIG_PAGE_HEADER Header;                     /* 0x00 */
+	U32                     BiosOptions;                /* 0x04 */
+	U32                     IOCSettings;                /* 0x08 */
+	U32                     Reserved1;                  /* 0x0C */
+	U32                     DeviceSettings;             /* 0x10 */
+	U16                     NumberOfDevices;            /* 0x14 */
+	U16                     UEFIVersion;                /* 0x16 */
+	U16                     IOTimeoutBlockDevicesNonRM; /* 0x18 */
+	U16                     IOTimeoutSequential;        /* 0x1A */
+	U16                     IOTimeoutOther;             /* 0x1C */
+	U16                     IOTimeoutBlockDevicesRM;    /* 0x1E */
 } MPI2_CONFIG_PAGE_BIOS_1, MPI2_POINTER PTR_MPI2_CONFIG_PAGE_BIOS_1,
   Mpi2BiosPage1_t, MPI2_POINTER pMpi2BiosPage1_t;
 
-#define MPI2_BIOSPAGE1_PAGEVERSION                      (0x04)
+#define MPI2_BIOSPAGE1_PAGEVERSION                      (0x05)
 
 /* values for BIOS Page 1 BiosOptions field */
-#define MPI2_BIOSPAGE1_OPTIONS_DISABLE_BIOS             (0x00000001)
+#define MPI2_BIOSPAGE1_OPTIONS_MASK_UEFI_HII_REGISTRATION   (0x00000006)
+#define MPI2_BIOSPAGE1_OPTIONS_ENABLE_UEFI_HII              (0x00000000)
+#define MPI2_BIOSPAGE1_OPTIONS_DISABLE_UEFI_HII             (0x00000002)
+#define MPI2_BIOSPAGE1_OPTIONS_VERSION_CHECK_UEFI_HII       (0x00000004)
+
+#define MPI2_BIOSPAGE1_OPTIONS_DISABLE_BIOS                 (0x00000001)
 
 /* values for BIOS Page 1 IOCSettings field */
 #define MPI2_BIOSPAGE1_IOCSET_MASK_BOOT_PREFERENCE      (0x00030000)
@@ -1248,6 +1256,13 @@ typedef struct _MPI2_CONFIG_PAGE_BIOS_1
 #define MPI2_BIOSPAGE1_DEVSET_DISABLE_NON_RM_LUN        (0x00000002)
 #define MPI2_BIOSPAGE1_DEVSET_DISABLE_OTHER_LUN         (0x00000001)
 
+/* defines for BIOS Page 1 UEFIVersion field */
+#define MPI2_BIOSPAGE1_UEFI_VER_MAJOR_MASK              (0xFF00)
+#define MPI2_BIOSPAGE1_UEFI_VER_MAJOR_SHIFT             (8)
+#define MPI2_BIOSPAGE1_UEFI_VER_MINOR_MASK              (0x00FF)
+#define MPI2_BIOSPAGE1_UEFI_VER_MINOR_SHIFT             (0)
+
+
 
 /* BIOS Page 2 */
 
@@ -2216,6 +2231,27 @@ typedef struct _MPI2_CONFIG_PAGE_SASIOUNIT_8 {
 
 
 
+/* SAS IO Unit Page 16 */
+
+typedef struct _MPI2_CONFIG_PAGE_SASIOUNIT16 {
+	MPI2_CONFIG_EXTENDED_PAGE_HEADER  Header;                  /* 0x00 */
+	U64                         TimeStamp;                     /* 0x08 */
+	U32                         Reserved1;                     /* 0x10 */
+	U32                         Reserved2;                     /* 0x14 */
+	U32                         FastPathPendedRequests;        /* 0x18 */
+	U32                         FastPathUnPendedRequests;      /* 0x1C */
+	U32                         FastPathHostRequestStarts;     /* 0x20 */
+	U32                         FastPathFirmwareRequestStarts; /* 0x24 */
+	U32                         FastPathHostCompletions;       /* 0x28 */
+	U32                         FastPathFirmwareCompletions;   /* 0x2C */
+	U32                         NonFastPathRequestStarts;      /* 0x30 */
+	U32			    NonFastPathHostCompletions;    /* 0x30 */
+} MPI2_CONFIG_PAGE_SASIOUNIT16,
+MPI2_POINTER PTR_MPI2_CONFIG_PAGE_SASIOUNIT16,
+Mpi2SasIOUnitPage16_t, MPI2_POINTER pMpi2SasIOUnitPage16_t;
+
+#define MPI2_SASIOUNITPAGE16_PAGEVERSION    (0x00)
+
 
 /****************************************************************************
 *   SAS Expander Config Pages
diff --git a/drivers/scsi/mpt2sas/mpt2sas_base.c b/drivers/scsi/mpt2sas/mpt2sas_base.c
index 8a59a772fdf..6102ef2cb2d 100644
--- a/drivers/scsi/mpt2sas/mpt2sas_base.c
+++ b/drivers/scsi/mpt2sas/mpt2sas_base.c
@@ -699,6 +699,11 @@ _base_display_reply_info(struct MPT2SAS_ADAPTER *ioc, u16 smid, u8 msix_index,
 	u16 ioc_status;
 
 	mpi_reply = mpt2sas_base_get_reply_virt_addr(ioc, reply);
+	if (unlikely(!mpi_reply)) {
+		printk(MPT2SAS_ERR_FMT "mpi_reply not valid at %s:%d/%s()!\n",
+			ioc->name, __FILE__, __LINE__, __func__);
+		return;
+	}
 	ioc_status = le16_to_cpu(mpi_reply->IOCStatus);
 #ifdef CONFIG_SCSI_MPT2SAS_LOGGING
 	if ((ioc_status & MPI2_IOCSTATUS_MASK) &&
@@ -930,16 +935,18 @@ _base_interrupt(int irq, void *bus_id)
 		else if (request_desript_type ==
 		    MPI2_RPY_DESCRIPT_FLAGS_TARGETASSIST_SUCCESS)
 			goto next;
-		if (smid)
+		if (smid) {
 			cb_idx = _base_get_cb_idx(ioc, smid);
-		if (smid && cb_idx != 0xFF) {
-			rc = mpt_callbacks[cb_idx](ioc, smid, msix_index,
-			    reply);
+		if ((likely(cb_idx < MPT_MAX_CALLBACKS))
+			    && (likely(mpt_callbacks[cb_idx] != NULL))) {
+				rc = mpt_callbacks[cb_idx](ioc, smid,
+				    msix_index, reply);
 			if (reply)
-				_base_display_reply_info(ioc, smid, msix_index,
-				    reply);
+				_base_display_reply_info(ioc, smid,
+				    msix_index, reply);
 			if (rc)
 				mpt2sas_base_free_smid(ioc, smid);
+			}
 		}
 		if (!smid)
 			_base_async_event(ioc, msix_index, reply);
@@ -3343,7 +3350,7 @@ _base_get_port_facts(struct MPT2SAS_ADAPTER *ioc, int port, int sleep_flag)
 	}
 
 	pfacts = &ioc->pfacts[port];
-	memset(pfacts, 0, sizeof(Mpi2PortFactsReply_t));
+	memset(pfacts, 0, sizeof(struct mpt2sas_port_facts));
 	pfacts->PortNumber = mpi_reply.PortNumber;
 	pfacts->VP_ID = mpi_reply.VP_ID;
 	pfacts->VF_ID = mpi_reply.VF_ID;
@@ -3385,7 +3392,7 @@ _base_get_ioc_facts(struct MPT2SAS_ADAPTER *ioc, int sleep_flag)
 	}
 
 	facts = &ioc->facts;
-	memset(facts, 0, sizeof(Mpi2IOCFactsReply_t));
+	memset(facts, 0, sizeof(struct mpt2sas_facts));
 	facts->MsgVersion = le16_to_cpu(mpi_reply.MsgVersion);
 	facts->HeaderVersion = le16_to_cpu(mpi_reply.HeaderVersion);
 	facts->VP_ID = mpi_reply.VP_ID;
@@ -4153,7 +4160,8 @@ _base_make_ioc_operational(struct MPT2SAS_ADAPTER *ioc, int sleep_flag)
 	if (ioc->is_driver_loading) {
 		if (ioc->is_warpdrive && ioc->manu_pg10.OEMIdentifier
 		    == 0x80) {
-			hide_flag = (u8) (ioc->manu_pg10.OEMSpecificFlags0 &
+			hide_flag = (u8) (
+			    le32_to_cpu(ioc->manu_pg10.OEMSpecificFlags0) &
 			    MFG_PAGE10_HIDE_SSDS_MASK);
 			if (hide_flag != MFG_PAGE10_HIDE_SSDS_MASK)
 				ioc->mfg_pg10_hide_flag = hide_flag;
@@ -4262,7 +4270,7 @@ mpt2sas_base_attach(struct MPT2SAS_ADAPTER *ioc)
 		goto out_free_resources;
 
 	ioc->pfacts = kcalloc(ioc->facts.NumberOfPorts,
-	    sizeof(Mpi2PortFactsReply_t), GFP_KERNEL);
+	    sizeof(struct mpt2sas_port_facts), GFP_KERNEL);
 	if (!ioc->pfacts) {
 		r = -ENOMEM;
 		goto out_free_resources;
@@ -4279,7 +4287,6 @@ mpt2sas_base_attach(struct MPT2SAS_ADAPTER *ioc)
 		goto out_free_resources;
 
 	init_waitqueue_head(&ioc->reset_wq);
-
 	/* allocate memory pd handle bitmask list */
 	ioc->pd_handles_sz = (ioc->facts.MaxDevHandle / 8);
 	if (ioc->facts.MaxDevHandle % 8)
@@ -4290,7 +4297,12 @@ mpt2sas_base_attach(struct MPT2SAS_ADAPTER *ioc)
 		r = -ENOMEM;
 		goto out_free_resources;
 	}
-
+	ioc->blocking_handles = kzalloc(ioc->pd_handles_sz,
+	    GFP_KERNEL);
+	if (!ioc->blocking_handles) {
+		r = -ENOMEM;
+		goto out_free_resources;
+	}
 	ioc->fwfault_debug = mpt2sas_fwfault_debug;
 
 	/* base internal command bits */
@@ -4377,6 +4389,7 @@ mpt2sas_base_attach(struct MPT2SAS_ADAPTER *ioc)
 	if (ioc->is_warpdrive)
 		kfree(ioc->reply_post_host_index);
 	kfree(ioc->pd_handles);
+	kfree(ioc->blocking_handles);
 	kfree(ioc->tm_cmds.reply);
 	kfree(ioc->transport_cmds.reply);
 	kfree(ioc->scsih_cmds.reply);
@@ -4418,6 +4431,7 @@ mpt2sas_base_detach(struct MPT2SAS_ADAPTER *ioc)
 	if (ioc->is_warpdrive)
 		kfree(ioc->reply_post_host_index);
 	kfree(ioc->pd_handles);
+	kfree(ioc->blocking_handles);
 	kfree(ioc->pfacts);
 	kfree(ioc->ctl_cmds.reply);
 	kfree(ioc->ctl_cmds.sense);
diff --git a/drivers/scsi/mpt2sas/mpt2sas_base.h b/drivers/scsi/mpt2sas/mpt2sas_base.h
index c7459fdc06c..b6dd3a5de7f 100644
--- a/drivers/scsi/mpt2sas/mpt2sas_base.h
+++ b/drivers/scsi/mpt2sas/mpt2sas_base.h
@@ -69,8 +69,8 @@
 #define MPT2SAS_DRIVER_NAME		"mpt2sas"
 #define MPT2SAS_AUTHOR	"LSI Corporation <DL-MPTFusionLinux@lsi.com>"
 #define MPT2SAS_DESCRIPTION	"LSI MPT Fusion SAS 2.0 Device Driver"
-#define MPT2SAS_DRIVER_VERSION		"12.100.00.00"
-#define MPT2SAS_MAJOR_VERSION		12
+#define MPT2SAS_DRIVER_VERSION		"13.100.00.00"
+#define MPT2SAS_MAJOR_VERSION		13
 #define MPT2SAS_MINOR_VERSION		100
 #define MPT2SAS_BUILD_VERSION		00
 #define MPT2SAS_RELEASE_VERSION		00
@@ -720,6 +720,7 @@ typedef void (*MPT2SAS_FLUSH_RUNNING_CMDS)(struct MPT2SAS_ADAPTER *ioc);
  * @io_missing_delay: time for IO completed by fw when PDR enabled
  * @device_missing_delay: time for device missing by fw when PDR enabled
  * @sas_id : used for setting volume target IDs
+ * @blocking_handles: bitmask used to identify which devices need blocking
  * @pd_handles : bitmask for PD handles
  * @pd_handles_sz : size of pd_handle bitmask
  * @config_page_sz: config page size
@@ -889,7 +890,7 @@ struct MPT2SAS_ADAPTER {
 	u8		io_missing_delay;
 	u16		device_missing_delay;
 	int		sas_id;
-
+	void		*blocking_handles;
 	void		*pd_handles;
 	u16		pd_handles_sz;
 
@@ -1058,7 +1059,8 @@ int mpt2sas_scsih_issue_tm(struct MPT2SAS_ADAPTER *ioc, u16 handle,
 void mpt2sas_scsih_set_tm_flag(struct MPT2SAS_ADAPTER *ioc, u16 handle);
 void mpt2sas_scsih_clear_tm_flag(struct MPT2SAS_ADAPTER *ioc, u16 handle);
 void mpt2sas_expander_remove(struct MPT2SAS_ADAPTER *ioc, u64 sas_address);
-void mpt2sas_device_remove(struct MPT2SAS_ADAPTER *ioc, u64 sas_address);
+void mpt2sas_device_remove_by_sas_address(struct MPT2SAS_ADAPTER *ioc,
+		u64 sas_address);
 struct _sas_node *mpt2sas_scsih_expander_find_by_handle(struct MPT2SAS_ADAPTER *ioc,
     u16 handle);
 struct _sas_node *mpt2sas_scsih_expander_find_by_sas_address(struct MPT2SAS_ADAPTER
diff --git a/drivers/scsi/mpt2sas/mpt2sas_ctl.c b/drivers/scsi/mpt2sas/mpt2sas_ctl.c
index 3b9a28efea8..49bdd2dc845 100644
--- a/drivers/scsi/mpt2sas/mpt2sas_ctl.c
+++ b/drivers/scsi/mpt2sas/mpt2sas_ctl.c
@@ -620,11 +620,10 @@ _ctl_set_task_mid(struct MPT2SAS_ADAPTER *ioc, struct mpt2_ioctl_command *karg,
  * @ioc: per adapter object
  * @karg - (struct mpt2_ioctl_command)
  * @mf - pointer to mf in user space
- * @state - NON_BLOCKING or BLOCKING
  */
 static long
-_ctl_do_mpt_command(struct MPT2SAS_ADAPTER *ioc,
-    struct mpt2_ioctl_command karg, void __user *mf, enum block_state state)
+_ctl_do_mpt_command(struct MPT2SAS_ADAPTER *ioc, struct mpt2_ioctl_command karg,
+	void __user *mf)
 {
 	MPI2RequestHeader_t *mpi_request = NULL, *request;
 	MPI2DefaultReply_t *mpi_reply;
@@ -647,11 +646,6 @@ _ctl_do_mpt_command(struct MPT2SAS_ADAPTER *ioc,
 
 	issue_reset = 0;
 
-	if (state == NON_BLOCKING && !mutex_trylock(&ioc->ctl_cmds.mutex))
-		return -EAGAIN;
-	else if (mutex_lock_interruptible(&ioc->ctl_cmds.mutex))
-		return -ERESTARTSYS;
-
 	if (ioc->ctl_cmds.status != MPT2_CMD_NOT_USED) {
 		printk(MPT2SAS_ERR_FMT "%s: ctl_cmd in use\n",
 		    ioc->name, __func__);
@@ -871,8 +865,16 @@ _ctl_do_mpt_command(struct MPT2SAS_ADAPTER *ioc,
 		if (smp_request->PassthroughFlags &
 		    MPI2_SMP_PT_REQ_PT_FLAGS_IMMEDIATE)
 			data = (u8 *)&smp_request->SGL;
-		else
+		else {
+			if (unlikely(data_out == NULL)) {
+				printk(KERN_ERR "failure at %s:%d/%s()!\n",
+				    __FILE__, __LINE__, __func__);
+				mpt2sas_base_free_smid(ioc, smid);
+				ret = -EINVAL;
+				goto out;
+			}
 			data = data_out;
+		}
 
 		if (data[1] == 0x91 && (data[10] == 1 || data[10] == 2)) {
 			ioc->ioc_link_reset_in_progress = 1;
@@ -985,7 +987,8 @@ _ctl_do_mpt_command(struct MPT2SAS_ADAPTER *ioc,
 		ret = -ENODATA;
 		if ((mpi_request->Function == MPI2_FUNCTION_SCSI_IO_REQUEST ||
 		    mpi_request->Function ==
-		    MPI2_FUNCTION_RAID_SCSI_IO_PASSTHROUGH)) {
+		    MPI2_FUNCTION_RAID_SCSI_IO_PASSTHROUGH ||
+		    mpi_request->Function == MPI2_FUNCTION_SATA_PASSTHROUGH)) {
 			printk(MPT2SAS_INFO_FMT "issue target reset: handle "
 			    "= (0x%04x)\n", ioc->name,
 			    le16_to_cpu(mpi_request->FunctionDependent1));
@@ -1013,27 +1016,24 @@ _ctl_do_mpt_command(struct MPT2SAS_ADAPTER *ioc,
 
 	kfree(mpi_request);
 	ioc->ctl_cmds.status = MPT2_CMD_NOT_USED;
-	mutex_unlock(&ioc->ctl_cmds.mutex);
 	return ret;
 }
 
 /**
  * _ctl_getiocinfo - main handler for MPT2IOCINFO opcode
+ * @ioc: per adapter object
  * @arg - user space buffer containing ioctl content
  */
 static long
-_ctl_getiocinfo(void __user *arg)
+_ctl_getiocinfo(struct MPT2SAS_ADAPTER *ioc, void __user *arg)
 {
 	struct mpt2_ioctl_iocinfo karg;
-	struct MPT2SAS_ADAPTER *ioc;
 
 	if (copy_from_user(&karg, arg, sizeof(karg))) {
 		printk(KERN_ERR "failure at %s:%d/%s()!\n",
 		    __FILE__, __LINE__, __func__);
 		return -EFAULT;
 	}
-	if (_ctl_verify_adapter(karg.hdr.ioc_number, &ioc) == -1 || !ioc)
-		return -ENODEV;
 
 	dctlprintk(ioc, printk(MPT2SAS_INFO_FMT "%s: enter\n", ioc->name,
 	    __func__));
@@ -1069,21 +1069,19 @@ _ctl_getiocinfo(void __user *arg)
 
 /**
  * _ctl_eventquery - main handler for MPT2EVENTQUERY opcode
+ * @ioc: per adapter object
  * @arg - user space buffer containing ioctl content
  */
 static long
-_ctl_eventquery(void __user *arg)
+_ctl_eventquery(struct MPT2SAS_ADAPTER *ioc, void __user *arg)
 {
 	struct mpt2_ioctl_eventquery karg;
-	struct MPT2SAS_ADAPTER *ioc;
 
 	if (copy_from_user(&karg, arg, sizeof(karg))) {
 		printk(KERN_ERR "failure at %s:%d/%s()!\n",
 		    __FILE__, __LINE__, __func__);
 		return -EFAULT;
 	}
-	if (_ctl_verify_adapter(karg.hdr.ioc_number, &ioc) == -1 || !ioc)
-		return -ENODEV;
 
 	dctlprintk(ioc, printk(MPT2SAS_INFO_FMT "%s: enter\n", ioc->name,
 	    __func__));
@@ -1102,21 +1100,19 @@ _ctl_eventquery(void __user *arg)
 
 /**
  * _ctl_eventenable - main handler for MPT2EVENTENABLE opcode
+ * @ioc: per adapter object
  * @arg - user space buffer containing ioctl content
  */
 static long
-_ctl_eventenable(void __user *arg)
+_ctl_eventenable(struct MPT2SAS_ADAPTER *ioc, void __user *arg)
 {
 	struct mpt2_ioctl_eventenable karg;
-	struct MPT2SAS_ADAPTER *ioc;
 
 	if (copy_from_user(&karg, arg, sizeof(karg))) {
 		printk(KERN_ERR "failure at %s:%d/%s()!\n",
 		    __FILE__, __LINE__, __func__);
 		return -EFAULT;
 	}
-	if (_ctl_verify_adapter(karg.hdr.ioc_number, &ioc) == -1 || !ioc)
-		return -ENODEV;
 
 	dctlprintk(ioc, printk(MPT2SAS_INFO_FMT "%s: enter\n", ioc->name,
 	    __func__));
@@ -1142,13 +1138,13 @@ _ctl_eventenable(void __user *arg)
 
 /**
  * _ctl_eventreport - main handler for MPT2EVENTREPORT opcode
+ * @ioc: per adapter object
  * @arg - user space buffer containing ioctl content
  */
 static long
-_ctl_eventreport(void __user *arg)
+_ctl_eventreport(struct MPT2SAS_ADAPTER *ioc, void __user *arg)
 {
 	struct mpt2_ioctl_eventreport karg;
-	struct MPT2SAS_ADAPTER *ioc;
 	u32 number_bytes, max_events, max;
 	struct mpt2_ioctl_eventreport __user *uarg = arg;
 
@@ -1157,8 +1153,6 @@ _ctl_eventreport(void __user *arg)
 		    __FILE__, __LINE__, __func__);
 		return -EFAULT;
 	}
-	if (_ctl_verify_adapter(karg.hdr.ioc_number, &ioc) == -1 || !ioc)
-		return -ENODEV;
 
 	dctlprintk(ioc, printk(MPT2SAS_INFO_FMT "%s: enter\n", ioc->name,
 	    __func__));
@@ -1188,13 +1182,13 @@ _ctl_eventreport(void __user *arg)
 
 /**
  * _ctl_do_reset - main handler for MPT2HARDRESET opcode
+ * @ioc: per adapter object
  * @arg - user space buffer containing ioctl content
  */
 static long
-_ctl_do_reset(void __user *arg)
+_ctl_do_reset(struct MPT2SAS_ADAPTER *ioc, void __user *arg)
 {
 	struct mpt2_ioctl_diag_reset karg;
-	struct MPT2SAS_ADAPTER *ioc;
 	int retval;
 
 	if (copy_from_user(&karg, arg, sizeof(karg))) {
@@ -1202,8 +1196,6 @@ _ctl_do_reset(void __user *arg)
 		    __FILE__, __LINE__, __func__);
 		return -EFAULT;
 	}
-	if (_ctl_verify_adapter(karg.hdr.ioc_number, &ioc) == -1 || !ioc)
-		return -ENODEV;
 
 	if (ioc->shost_recovery || ioc->pci_error_recovery ||
 		ioc->is_driver_loading)
@@ -1292,13 +1284,13 @@ _ctl_btdh_search_raid_device(struct MPT2SAS_ADAPTER *ioc,
 
 /**
  * _ctl_btdh_mapping - main handler for MPT2BTDHMAPPING opcode
+ * @ioc: per adapter object
  * @arg - user space buffer containing ioctl content
  */
 static long
-_ctl_btdh_mapping(void __user *arg)
+_ctl_btdh_mapping(struct MPT2SAS_ADAPTER *ioc, void __user *arg)
 {
 	struct mpt2_ioctl_btdh_mapping karg;
-	struct MPT2SAS_ADAPTER *ioc;
 	int rc;
 
 	if (copy_from_user(&karg, arg, sizeof(karg))) {
@@ -1306,8 +1298,6 @@ _ctl_btdh_mapping(void __user *arg)
 		    __FILE__, __LINE__, __func__);
 		return -EFAULT;
 	}
-	if (_ctl_verify_adapter(karg.hdr.ioc_number, &ioc) == -1 || !ioc)
-		return -ENODEV;
 
 	dctlprintk(ioc, printk(MPT2SAS_INFO_FMT "%s\n", ioc->name,
 	    __func__));
@@ -1576,17 +1566,16 @@ mpt2sas_enable_diag_buffer(struct MPT2SAS_ADAPTER *ioc, u8 bits_to_register)
 
 /**
  * _ctl_diag_register - application register with driver
+ * @ioc: per adapter object
  * @arg - user space buffer containing ioctl content
- * @state - NON_BLOCKING or BLOCKING
  *
  * This will allow the driver to setup any required buffers that will be
  * needed by firmware to communicate with the driver.
  */
 static long
-_ctl_diag_register(void __user *arg, enum block_state state)
+_ctl_diag_register(struct MPT2SAS_ADAPTER *ioc, void __user *arg)
 {
 	struct mpt2_diag_register karg;
-	struct MPT2SAS_ADAPTER *ioc;
 	long rc;
 
 	if (copy_from_user(&karg, arg, sizeof(karg))) {
@@ -1594,30 +1583,23 @@ _ctl_diag_register(void __user *arg, enum block_state state)
 		    __FILE__, __LINE__, __func__);
 		return -EFAULT;
 	}
-	if (_ctl_verify_adapter(karg.hdr.ioc_number, &ioc) == -1 || !ioc)
-		return -ENODEV;
 
-	if (state == NON_BLOCKING && !mutex_trylock(&ioc->ctl_cmds.mutex))
-		return -EAGAIN;
-	else if (mutex_lock_interruptible(&ioc->ctl_cmds.mutex))
-		return -ERESTARTSYS;
 	rc = _ctl_diag_register_2(ioc, &karg);
-	mutex_unlock(&ioc->ctl_cmds.mutex);
 	return rc;
 }
 
 /**
  * _ctl_diag_unregister - application unregister with driver
+ * @ioc: per adapter object
  * @arg - user space buffer containing ioctl content
  *
  * This will allow the driver to cleanup any memory allocated for diag
  * messages and to free up any resources.
  */
 static long
-_ctl_diag_unregister(void __user *arg)
+_ctl_diag_unregister(struct MPT2SAS_ADAPTER *ioc, void __user *arg)
 {
 	struct mpt2_diag_unregister karg;
-	struct MPT2SAS_ADAPTER *ioc;
 	void *request_data;
 	dma_addr_t request_data_dma;
 	u32 request_data_sz;
@@ -1628,8 +1610,6 @@ _ctl_diag_unregister(void __user *arg)
 		    __FILE__, __LINE__, __func__);
 		return -EFAULT;
 	}
-	if (_ctl_verify_adapter(karg.hdr.ioc_number, &ioc) == -1 || !ioc)
-		return -ENODEV;
 
 	dctlprintk(ioc, printk(MPT2SAS_INFO_FMT "%s\n", ioc->name,
 	    __func__));
@@ -1678,6 +1658,7 @@ _ctl_diag_unregister(void __user *arg)
 
 /**
  * _ctl_diag_query - query relevant info associated with diag buffers
+ * @ioc: per adapter object
  * @arg - user space buffer containing ioctl content
  *
  * The application will send only buffer_type and unique_id.  Driver will
@@ -1685,10 +1666,9 @@ _ctl_diag_unregister(void __user *arg)
  * 0x00, the driver will return info specified by Buffer Type.
  */
 static long
-_ctl_diag_query(void __user *arg)
+_ctl_diag_query(struct MPT2SAS_ADAPTER *ioc, void __user *arg)
 {
 	struct mpt2_diag_query karg;
-	struct MPT2SAS_ADAPTER *ioc;
 	void *request_data;
 	int i;
 	u8 buffer_type;
@@ -1698,8 +1678,6 @@ _ctl_diag_query(void __user *arg)
 		    __FILE__, __LINE__, __func__);
 		return -EFAULT;
 	}
-	if (_ctl_verify_adapter(karg.hdr.ioc_number, &ioc) == -1 || !ioc)
-		return -ENODEV;
 
 	dctlprintk(ioc, printk(MPT2SAS_INFO_FMT "%s\n", ioc->name,
 	    __func__));
@@ -1866,17 +1844,15 @@ _ctl_send_release(struct MPT2SAS_ADAPTER *ioc, u8 buffer_type, u8 *issue_reset)
 /**
  * _ctl_diag_release - request to send Diag Release Message to firmware
  * @arg - user space buffer containing ioctl content
- * @state - NON_BLOCKING or BLOCKING
  *
  * This allows ownership of the specified buffer to returned to the driver,
  * allowing an application to read the buffer without fear that firmware is
  * overwritting information in the buffer.
  */
 static long
-_ctl_diag_release(void __user *arg, enum block_state state)
+_ctl_diag_release(struct MPT2SAS_ADAPTER *ioc, void __user *arg)
 {
 	struct mpt2_diag_release karg;
-	struct MPT2SAS_ADAPTER *ioc;
 	void *request_data;
 	int rc;
 	u8 buffer_type;
@@ -1887,8 +1863,6 @@ _ctl_diag_release(void __user *arg, enum block_state state)
 		    __FILE__, __LINE__, __func__);
 		return -EFAULT;
 	}
-	if (_ctl_verify_adapter(karg.hdr.ioc_number, &ioc) == -1 || !ioc)
-		return -ENODEV;
 
 	dctlprintk(ioc, printk(MPT2SAS_INFO_FMT "%s\n", ioc->name,
 	    __func__));
@@ -1942,32 +1916,25 @@ _ctl_diag_release(void __user *arg, enum block_state state)
 		return 0;
 	}
 
-	if (state == NON_BLOCKING && !mutex_trylock(&ioc->ctl_cmds.mutex))
-		return -EAGAIN;
-	else if (mutex_lock_interruptible(&ioc->ctl_cmds.mutex))
-		return -ERESTARTSYS;
-
 	rc = _ctl_send_release(ioc, buffer_type, &issue_reset);
 
 	if (issue_reset)
 		mpt2sas_base_hard_reset_handler(ioc, CAN_SLEEP,
 		    FORCE_BIG_HAMMER);
 
-	mutex_unlock(&ioc->ctl_cmds.mutex);
 	return rc;
 }
 
 /**
  * _ctl_diag_read_buffer - request for copy of the diag buffer
+ * @ioc: per adapter object
  * @arg - user space buffer containing ioctl content
- * @state - NON_BLOCKING or BLOCKING
  */
 static long
-_ctl_diag_read_buffer(void __user *arg, enum block_state state)
+_ctl_diag_read_buffer(struct MPT2SAS_ADAPTER *ioc, void __user *arg)
 {
 	struct mpt2_diag_read_buffer karg;
 	struct mpt2_diag_read_buffer __user *uarg = arg;
-	struct MPT2SAS_ADAPTER *ioc;
 	void *request_data, *diag_data;
 	Mpi2DiagBufferPostRequest_t *mpi_request;
 	Mpi2DiagBufferPostReply_t *mpi_reply;
@@ -1983,8 +1950,6 @@ _ctl_diag_read_buffer(void __user *arg, enum block_state state)
 		    __FILE__, __LINE__, __func__);
 		return -EFAULT;
 	}
-	if (_ctl_verify_adapter(karg.hdr.ioc_number, &ioc) == -1 || !ioc)
-		return -ENODEV;
 
 	dctlprintk(ioc, printk(MPT2SAS_INFO_FMT "%s\n", ioc->name,
 	    __func__));
@@ -2055,10 +2020,6 @@ _ctl_diag_read_buffer(void __user *arg, enum block_state state)
 	}
 	/* Get a free request frame and save the message context.
 	*/
-	if (state == NON_BLOCKING && !mutex_trylock(&ioc->ctl_cmds.mutex))
-		return -EAGAIN;
-	else if (mutex_lock_interruptible(&ioc->ctl_cmds.mutex))
-		return -ERESTARTSYS;
 
 	if (ioc->ctl_cmds.status != MPT2_CMD_NOT_USED) {
 		printk(MPT2SAS_ERR_FMT "%s: ctl_cmd in use\n",
@@ -2139,115 +2100,170 @@ _ctl_diag_read_buffer(void __user *arg, enum block_state state)
  out:
 
 	ioc->ctl_cmds.status = MPT2_CMD_NOT_USED;
-	mutex_unlock(&ioc->ctl_cmds.mutex);
 	return rc;
 }
 
+
+#ifdef CONFIG_COMPAT
+/**
+ * _ctl_compat_mpt_command - convert 32bit pointers to 64bit.
+ * @ioc: per adapter object
+ * @cmd - ioctl opcode
+ * @arg - (struct mpt2_ioctl_command32)
+ *
+ * MPT2COMMAND32 - Handle 32bit applications running on 64bit os.
+ */
+static long
+_ctl_compat_mpt_command(struct MPT2SAS_ADAPTER *ioc, unsigned cmd,
+	void __user *arg)
+{
+	struct mpt2_ioctl_command32 karg32;
+	struct mpt2_ioctl_command32 __user *uarg;
+	struct mpt2_ioctl_command karg;
+
+	if (_IOC_SIZE(cmd) != sizeof(struct mpt2_ioctl_command32))
+		return -EINVAL;
+
+	uarg = (struct mpt2_ioctl_command32 __user *) arg;
+
+	if (copy_from_user(&karg32, (char __user *)arg, sizeof(karg32))) {
+		printk(KERN_ERR "failure at %s:%d/%s()!\n",
+		    __FILE__, __LINE__, __func__);
+		return -EFAULT;
+	}
+
+	memset(&karg, 0, sizeof(struct mpt2_ioctl_command));
+	karg.hdr.ioc_number = karg32.hdr.ioc_number;
+	karg.hdr.port_number = karg32.hdr.port_number;
+	karg.hdr.max_data_size = karg32.hdr.max_data_size;
+	karg.timeout = karg32.timeout;
+	karg.max_reply_bytes = karg32.max_reply_bytes;
+	karg.data_in_size = karg32.data_in_size;
+	karg.data_out_size = karg32.data_out_size;
+	karg.max_sense_bytes = karg32.max_sense_bytes;
+	karg.data_sge_offset = karg32.data_sge_offset;
+	karg.reply_frame_buf_ptr = compat_ptr(karg32.reply_frame_buf_ptr);
+	karg.data_in_buf_ptr = compat_ptr(karg32.data_in_buf_ptr);
+	karg.data_out_buf_ptr = compat_ptr(karg32.data_out_buf_ptr);
+	karg.sense_data_ptr = compat_ptr(karg32.sense_data_ptr);
+	return _ctl_do_mpt_command(ioc, karg, &uarg->mf);
+}
+#endif
+
 /**
  * _ctl_ioctl_main - main ioctl entry point
  * @file - (struct file)
  * @cmd - ioctl opcode
  * @arg -
+ * compat - handles 32 bit applications in 64bit os
  */
 static long
-_ctl_ioctl_main(struct file *file, unsigned int cmd, void __user *arg)
+_ctl_ioctl_main(struct file *file, unsigned int cmd, void __user *arg,
+	u8 compat)
 {
+	struct MPT2SAS_ADAPTER *ioc;
+	struct mpt2_ioctl_header ioctl_header;
 	enum block_state state;
 	long ret = -EINVAL;
 
-	state = (file->f_flags & O_NONBLOCK) ? NON_BLOCKING :
-	    BLOCKING;
+	/* get IOCTL header */
+	if (copy_from_user(&ioctl_header, (char __user *)arg,
+	    sizeof(struct mpt2_ioctl_header))) {
+		printk(KERN_ERR "failure at %s:%d/%s()!\n",
+		    __FILE__, __LINE__, __func__);
+		return -EFAULT;
+	}
+
+	if (_ctl_verify_adapter(ioctl_header.ioc_number, &ioc) == -1 || !ioc)
+		return -ENODEV;
+	if (ioc->shost_recovery || ioc->pci_error_recovery ||
+	    ioc->is_driver_loading)
+		return -EAGAIN;
+
+	state = (file->f_flags & O_NONBLOCK) ? NON_BLOCKING : BLOCKING;
+	if (state == NON_BLOCKING && !mutex_trylock(&ioc->ctl_cmds.mutex))
+		return -EAGAIN;
+	else if (mutex_lock_interruptible(&ioc->ctl_cmds.mutex))
+		return -ERESTARTSYS;
 
 	switch (cmd) {
 	case MPT2IOCINFO:
 		if (_IOC_SIZE(cmd) == sizeof(struct mpt2_ioctl_iocinfo))
-			ret = _ctl_getiocinfo(arg);
+			ret = _ctl_getiocinfo(ioc, arg);
 		break;
+#ifdef CONFIG_COMPAT
+	case MPT2COMMAND32:
+#endif
 	case MPT2COMMAND:
 	{
-		struct mpt2_ioctl_command karg;
 		struct mpt2_ioctl_command __user *uarg;
-		struct MPT2SAS_ADAPTER *ioc;
-
+		struct mpt2_ioctl_command karg;
+#ifdef CONFIG_COMPAT
+		if (compat) {
+			ret = _ctl_compat_mpt_command(ioc, cmd, arg);
+			break;
+		}
+#endif
 		if (copy_from_user(&karg, arg, sizeof(karg))) {
 			printk(KERN_ERR "failure at %s:%d/%s()!\n",
 			    __FILE__, __LINE__, __func__);
-			return -EFAULT;
+			ret = -EFAULT;
+			break;
 		}
 
-		if (_ctl_verify_adapter(karg.hdr.ioc_number, &ioc) == -1 ||
-		    !ioc)
-			return -ENODEV;
-
-		if (ioc->shost_recovery || ioc->pci_error_recovery ||
-				ioc->is_driver_loading)
-			return -EAGAIN;
-
 		if (_IOC_SIZE(cmd) == sizeof(struct mpt2_ioctl_command)) {
 			uarg = arg;
-			ret = _ctl_do_mpt_command(ioc, karg, &uarg->mf, state);
+			ret = _ctl_do_mpt_command(ioc, karg, &uarg->mf);
 		}
 		break;
 	}
 	case MPT2EVENTQUERY:
 		if (_IOC_SIZE(cmd) == sizeof(struct mpt2_ioctl_eventquery))
-			ret = _ctl_eventquery(arg);
+			ret = _ctl_eventquery(ioc, arg);
 		break;
 	case MPT2EVENTENABLE:
 		if (_IOC_SIZE(cmd) == sizeof(struct mpt2_ioctl_eventenable))
-			ret = _ctl_eventenable(arg);
+			ret = _ctl_eventenable(ioc, arg);
 		break;
 	case MPT2EVENTREPORT:
-		ret = _ctl_eventreport(arg);
+		ret = _ctl_eventreport(ioc, arg);
 		break;
 	case MPT2HARDRESET:
 		if (_IOC_SIZE(cmd) == sizeof(struct mpt2_ioctl_diag_reset))
-			ret = _ctl_do_reset(arg);
+			ret = _ctl_do_reset(ioc, arg);
 		break;
 	case MPT2BTDHMAPPING:
 		if (_IOC_SIZE(cmd) == sizeof(struct mpt2_ioctl_btdh_mapping))
-			ret = _ctl_btdh_mapping(arg);
+			ret = _ctl_btdh_mapping(ioc, arg);
 		break;
 	case MPT2DIAGREGISTER:
 		if (_IOC_SIZE(cmd) == sizeof(struct mpt2_diag_register))
-			ret = _ctl_diag_register(arg, state);
+			ret = _ctl_diag_register(ioc, arg);
 		break;
 	case MPT2DIAGUNREGISTER:
 		if (_IOC_SIZE(cmd) == sizeof(struct mpt2_diag_unregister))
-			ret = _ctl_diag_unregister(arg);
+			ret = _ctl_diag_unregister(ioc, arg);
 		break;
 	case MPT2DIAGQUERY:
 		if (_IOC_SIZE(cmd) == sizeof(struct mpt2_diag_query))
-			ret = _ctl_diag_query(arg);
+			ret = _ctl_diag_query(ioc, arg);
 		break;
 	case MPT2DIAGRELEASE:
 		if (_IOC_SIZE(cmd) == sizeof(struct mpt2_diag_release))
-			ret = _ctl_diag_release(arg, state);
+			ret = _ctl_diag_release(ioc, arg);
 		break;
 	case MPT2DIAGREADBUFFER:
 		if (_IOC_SIZE(cmd) == sizeof(struct mpt2_diag_read_buffer))
-			ret = _ctl_diag_read_buffer(arg, state);
+			ret = _ctl_diag_read_buffer(ioc, arg);
 		break;
 	default:
-	{
-		struct mpt2_ioctl_command karg;
-		struct MPT2SAS_ADAPTER *ioc;
-
-		if (copy_from_user(&karg, arg, sizeof(karg))) {
-			printk(KERN_ERR "failure at %s:%d/%s()!\n",
-			    __FILE__, __LINE__, __func__);
-			return -EFAULT;
-		}
-
-		if (_ctl_verify_adapter(karg.hdr.ioc_number, &ioc) == -1 ||
-		    !ioc)
-			return -ENODEV;
 
 		dctlprintk(ioc, printk(MPT2SAS_INFO_FMT
 		    "unsupported ioctl opcode(0x%08x)\n", ioc->name, cmd));
 		break;
 	}
-	}
+
+	mutex_unlock(&ioc->ctl_cmds.mutex);
 	return ret;
 }
 
@@ -2262,66 +2278,11 @@ _ctl_ioctl(struct file *file, unsigned int cmd, unsigned long arg)
 {
 	long ret;
 
-	mutex_lock(&_ctl_mutex);
-	ret = _ctl_ioctl_main(file, cmd, (void __user *)arg);
-	mutex_unlock(&_ctl_mutex);
+	ret = _ctl_ioctl_main(file, cmd, (void __user *)arg, 0);
 	return ret;
 }
-
 #ifdef CONFIG_COMPAT
 /**
- * _ctl_compat_mpt_command - convert 32bit pointers to 64bit.
- * @file - (struct file)
- * @cmd - ioctl opcode
- * @arg - (struct mpt2_ioctl_command32)
- *
- * MPT2COMMAND32 - Handle 32bit applications running on 64bit os.
- */
-static long
-_ctl_compat_mpt_command(struct file *file, unsigned cmd, unsigned long arg)
-{
-	struct mpt2_ioctl_command32 karg32;
-	struct mpt2_ioctl_command32 __user *uarg;
-	struct mpt2_ioctl_command karg;
-	struct MPT2SAS_ADAPTER *ioc;
-	enum block_state state;
-
-	if (_IOC_SIZE(cmd) != sizeof(struct mpt2_ioctl_command32))
-		return -EINVAL;
-
-	uarg = (struct mpt2_ioctl_command32 __user *) arg;
-
-	if (copy_from_user(&karg32, (char __user *)arg, sizeof(karg32))) {
-		printk(KERN_ERR "failure at %s:%d/%s()!\n",
-		    __FILE__, __LINE__, __func__);
-		return -EFAULT;
-	}
-	if (_ctl_verify_adapter(karg32.hdr.ioc_number, &ioc) == -1 || !ioc)
-		return -ENODEV;
-
-	if (ioc->shost_recovery || ioc->pci_error_recovery ||
-			ioc->is_driver_loading)
-		return -EAGAIN;
-
-	memset(&karg, 0, sizeof(struct mpt2_ioctl_command));
-	karg.hdr.ioc_number = karg32.hdr.ioc_number;
-	karg.hdr.port_number = karg32.hdr.port_number;
-	karg.hdr.max_data_size = karg32.hdr.max_data_size;
-	karg.timeout = karg32.timeout;
-	karg.max_reply_bytes = karg32.max_reply_bytes;
-	karg.data_in_size = karg32.data_in_size;
-	karg.data_out_size = karg32.data_out_size;
-	karg.max_sense_bytes = karg32.max_sense_bytes;
-	karg.data_sge_offset = karg32.data_sge_offset;
-	karg.reply_frame_buf_ptr = compat_ptr(karg32.reply_frame_buf_ptr);
-	karg.data_in_buf_ptr = compat_ptr(karg32.data_in_buf_ptr);
-	karg.data_out_buf_ptr = compat_ptr(karg32.data_out_buf_ptr);
-	karg.sense_data_ptr = compat_ptr(karg32.sense_data_ptr);
-	state = (file->f_flags & O_NONBLOCK) ? NON_BLOCKING : BLOCKING;
-	return _ctl_do_mpt_command(ioc, karg, &uarg->mf, state);
-}
-
-/**
  * _ctl_ioctl_compat - main ioctl entry point (compat)
  * @file -
  * @cmd -
@@ -2334,12 +2295,7 @@ _ctl_ioctl_compat(struct file *file, unsigned cmd, unsigned long arg)
 {
 	long ret;
 
-	mutex_lock(&_ctl_mutex);
-	if (cmd == MPT2COMMAND32)
-		ret = _ctl_compat_mpt_command(file, cmd, arg);
-	else
-		ret = _ctl_ioctl_main(file, cmd, (void __user *)arg);
-	mutex_unlock(&_ctl_mutex);
+	ret = _ctl_ioctl_main(file, cmd, (void __user *)arg, 1);
 	return ret;
 }
 #endif
@@ -2884,7 +2840,7 @@ _ctl_host_trace_buffer_enable_store(struct device *cdev,
 	struct mpt2_diag_register diag_register;
 	u8 issue_reset = 0;
 
-	if (sscanf(buf, "%s", str) != 1)
+	if (sscanf(buf, "%9s", str) != 1)
 		return -EINVAL;
 
 	if (!strcmp(str, "post")) {
diff --git a/drivers/scsi/mpt2sas/mpt2sas_scsih.c b/drivers/scsi/mpt2sas/mpt2sas_scsih.c
index d953a57e779..76973e8ca4b 100644
--- a/drivers/scsi/mpt2sas/mpt2sas_scsih.c
+++ b/drivers/scsi/mpt2sas/mpt2sas_scsih.c
@@ -579,14 +579,12 @@ _scsih_sas_device_remove(struct MPT2SAS_ADAPTER *ioc,
 		return;
 
 	spin_lock_irqsave(&ioc->sas_device_lock, flags);
-	if (mpt2sas_scsih_sas_device_find_by_sas_address(ioc,
-	    sas_device->sas_address)) {
-		list_del(&sas_device->list);
-		kfree(sas_device);
-	}
+	list_del(&sas_device->list);
+	kfree(sas_device);
 	spin_unlock_irqrestore(&ioc->sas_device_lock, flags);
 }
 
+
 /**
  * _scsih_sas_device_add - insert sas_device to the list.
  * @ioc: per adapter object
@@ -645,8 +643,8 @@ _scsih_sas_device_init_add(struct MPT2SAS_ADAPTER *ioc,
 
 	spin_lock_irqsave(&ioc->sas_device_lock, flags);
 	list_add_tail(&sas_device->list, &ioc->sas_device_init_list);
-	spin_unlock_irqrestore(&ioc->sas_device_lock, flags);
 	_scsih_determine_boot_device(ioc, sas_device, 0);
+	spin_unlock_irqrestore(&ioc->sas_device_lock, flags);
 }
 
 /**
@@ -755,7 +753,6 @@ _scsih_raid_device_add(struct MPT2SAS_ADAPTER *ioc,
  * @ioc: per adapter object
  * @raid_device: raid_device object
  *
- * This is removed from the raid_device_list link list.
  */
 static void
 _scsih_raid_device_remove(struct MPT2SAS_ADAPTER *ioc,
@@ -765,7 +762,6 @@ _scsih_raid_device_remove(struct MPT2SAS_ADAPTER *ioc,
 
 	spin_lock_irqsave(&ioc->raid_device_lock, flags);
 	list_del(&raid_device->list);
-	memset(raid_device, 0, sizeof(struct _raid_device));
 	kfree(raid_device);
 	spin_unlock_irqrestore(&ioc->raid_device_lock, flags);
 }
@@ -1199,10 +1195,10 @@ _scsih_adjust_queue_depth(struct scsi_device *sdev, int qdepth)
 	spin_lock_irqsave(&ioc->sas_device_lock, flags);
 	sas_device = mpt2sas_scsih_sas_device_find_by_sas_address(ioc,
 	   sas_device_priv_data->sas_target->sas_address);
-	spin_unlock_irqrestore(&ioc->sas_device_lock, flags);
 	if (sas_device && sas_device->device_info &
 	    MPI2_SAS_DEVICE_INFO_SATA_DEVICE)
 		max_depth = MPT2SAS_SATA_QUEUE_DEPTH;
+	spin_unlock_irqrestore(&ioc->sas_device_lock, flags);
 
  not_sata:
 
@@ -1299,7 +1295,8 @@ _scsih_target_alloc(struct scsi_target *starget)
 			sas_target_priv_data->handle = raid_device->handle;
 			sas_target_priv_data->sas_address = raid_device->wwid;
 			sas_target_priv_data->flags |= MPT_TARGET_FLAGS_VOLUME;
-			sas_target_priv_data->raid_device = raid_device;
+			if (ioc->is_warpdrive)
+				sas_target_priv_data->raid_device = raid_device;
 			raid_device->starget = starget;
 		}
 		spin_unlock_irqrestore(&ioc->raid_device_lock, flags);
@@ -1465,12 +1462,12 @@ _scsih_slave_destroy(struct scsi_device *sdev)
 /**
  * _scsih_display_sata_capabilities - sata capabilities
  * @ioc: per adapter object
- * @sas_device: the sas_device object
+ * @handle: device handle
  * @sdev: scsi device struct
  */
 static void
 _scsih_display_sata_capabilities(struct MPT2SAS_ADAPTER *ioc,
-    struct _sas_device *sas_device, struct scsi_device *sdev)
+	u16 handle, struct scsi_device *sdev)
 {
 	Mpi2ConfigReply_t mpi_reply;
 	Mpi2SasDevicePage0_t sas_device_pg0;
@@ -1479,7 +1476,7 @@ _scsih_display_sata_capabilities(struct MPT2SAS_ADAPTER *ioc,
 	u32 device_info;
 
 	if ((mpt2sas_config_get_sas_device_pg0(ioc, &mpi_reply, &sas_device_pg0,
-	    MPI2_SAS_DEVICE_PGAD_FORM_HANDLE, sas_device->handle))) {
+	    MPI2_SAS_DEVICE_PGAD_FORM_HANDLE, handle))) {
 		printk(MPT2SAS_ERR_FMT "failure at %s:%d/%s()!\n",
 		    ioc->name, __FILE__, __LINE__, __func__);
 		return;
@@ -1537,27 +1534,40 @@ _scsih_get_resync(struct device *dev)
 	Mpi2RaidVolPage0_t vol_pg0;
 	Mpi2ConfigReply_t mpi_reply;
 	u32 volume_status_flags;
-	u8 percent_complete = 0;
+	u8 percent_complete;
+	u16 handle;
+
+	percent_complete = 0;
+	handle = 0;
+	if (ioc->is_warpdrive)
+		goto out;
 
 	spin_lock_irqsave(&ioc->raid_device_lock, flags);
 	raid_device = _scsih_raid_device_find_by_id(ioc, sdev->id,
 	    sdev->channel);
+	if (raid_device) {
+		handle = raid_device->handle;
+		percent_complete = raid_device->percent_complete;
+	}
 	spin_unlock_irqrestore(&ioc->raid_device_lock, flags);
 
-	if (!raid_device || ioc->is_warpdrive)
+	if (!handle)
 		goto out;
 
 	if (mpt2sas_config_get_raid_volume_pg0(ioc, &mpi_reply, &vol_pg0,
-	     MPI2_RAID_VOLUME_PGAD_FORM_HANDLE, raid_device->handle,
+	     MPI2_RAID_VOLUME_PGAD_FORM_HANDLE, handle,
 	     sizeof(Mpi2RaidVolPage0_t))) {
 		printk(MPT2SAS_ERR_FMT "failure at %s:%d/%s()!\n",
 		    ioc->name, __FILE__, __LINE__, __func__);
+		percent_complete = 0;
 		goto out;
 	}
 
 	volume_status_flags = le32_to_cpu(vol_pg0.VolumeStatusFlags);
-	if (volume_status_flags & MPI2_RAIDVOL0_STATUS_FLAG_RESYNC_IN_PROGRESS)
-		percent_complete = raid_device->percent_complete;
+	if (!(volume_status_flags &
+	    MPI2_RAIDVOL0_STATUS_FLAG_RESYNC_IN_PROGRESS))
+		percent_complete = 0;
+
  out:
 	raid_set_resync(mpt2sas_raid_template, dev, percent_complete);
 }
@@ -1577,17 +1587,20 @@ _scsih_get_state(struct device *dev)
 	Mpi2ConfigReply_t mpi_reply;
 	u32 volstate;
 	enum raid_state state = RAID_STATE_UNKNOWN;
+	u16 handle = 0;
 
 	spin_lock_irqsave(&ioc->raid_device_lock, flags);
 	raid_device = _scsih_raid_device_find_by_id(ioc, sdev->id,
 	    sdev->channel);
+	if (raid_device)
+		handle = raid_device->handle;
 	spin_unlock_irqrestore(&ioc->raid_device_lock, flags);
 
 	if (!raid_device)
 		goto out;
 
 	if (mpt2sas_config_get_raid_volume_pg0(ioc, &mpi_reply, &vol_pg0,
-	     MPI2_RAID_VOLUME_PGAD_FORM_HANDLE, raid_device->handle,
+	     MPI2_RAID_VOLUME_PGAD_FORM_HANDLE, handle,
 	     sizeof(Mpi2RaidVolPage0_t))) {
 		printk(MPT2SAS_ERR_FMT "failure at %s:%d/%s()!\n",
 		    ioc->name, __FILE__, __LINE__, __func__);
@@ -1620,14 +1633,14 @@ _scsih_get_state(struct device *dev)
 /**
  * _scsih_set_level - set raid level
  * @sdev: scsi device struct
- * @raid_device: raid_device object
+ * @volume_type: volume type
  */
 static void
-_scsih_set_level(struct scsi_device *sdev, struct _raid_device *raid_device)
+_scsih_set_level(struct scsi_device *sdev, u8 volume_type)
 {
 	enum raid_level level = RAID_LEVEL_UNKNOWN;
 
-	switch (raid_device->volume_type) {
+	switch (volume_type) {
 	case MPI2_RAID_VOL_TYPE_RAID0:
 		level = RAID_LEVEL_0;
 		break;
@@ -1722,6 +1735,7 @@ _scsih_disable_ddio(struct MPT2SAS_ADAPTER *ioc)
 	struct _raid_device *raid_device;
 	u16 handle;
 	u16 ioc_status;
+	unsigned long flags;
 
 	handle = 0xFFFF;
 	while (!(mpt2sas_config_get_raid_volume_pg1(ioc, &mpi_reply,
@@ -1731,9 +1745,11 @@ _scsih_disable_ddio(struct MPT2SAS_ADAPTER *ioc)
 		if (ioc_status == MPI2_IOCSTATUS_CONFIG_INVALID_PAGE)
 			break;
 		handle = le16_to_cpu(vol_pg1.DevHandle);
+		spin_lock_irqsave(&ioc->raid_device_lock, flags);
 		raid_device = _scsih_raid_device_find_by_handle(ioc, handle);
 		if (raid_device)
 			raid_device->direct_io_enabled = 0;
+		spin_unlock_irqrestore(&ioc->raid_device_lock, flags);
 	}
 	return;
 }
@@ -1838,7 +1854,8 @@ _scsih_init_warpdrive_properties(struct MPT2SAS_ADAPTER *ioc,
 		if (mpt2sas_config_get_phys_disk_pg0(ioc, &mpi_reply,
 		    &pd_pg0, MPI2_PHYSDISK_PGAD_FORM_PHYSDISKNUM,
 		    vol_pg0->PhysDisk[count].PhysDiskNum) ||
-		    pd_pg0.DevHandle == MPT2SAS_INVALID_DEVICE_HANDLE) {
+		     le16_to_cpu(pd_pg0.DevHandle) ==
+		    MPT2SAS_INVALID_DEVICE_HANDLE) {
 			printk(MPT2SAS_INFO_FMT "WarpDrive : Direct IO is "
 			    "disabled for the drive with handle(0x%04x) member"
 			    "handle retrieval failed for member number=%d\n",
@@ -1968,19 +1985,21 @@ _scsih_slave_configure(struct scsi_device *sdev)
 	u8 ssp_target = 0;
 	char *ds = "";
 	char *r_level = "";
+	u16 handle, volume_handle = 0;
+	u64 volume_wwid = 0;
 
 	qdepth = 1;
 	sas_device_priv_data = sdev->hostdata;
 	sas_device_priv_data->configured_lun = 1;
 	sas_device_priv_data->flags &= ~MPT_DEVICE_FLAGS_INIT;
 	sas_target_priv_data = sas_device_priv_data->sas_target;
+	handle = sas_target_priv_data->handle;
 
 	/* raid volume handling */
 	if (sas_target_priv_data->flags & MPT_TARGET_FLAGS_VOLUME) {
 
 		spin_lock_irqsave(&ioc->raid_device_lock, flags);
-		raid_device = _scsih_raid_device_find_by_handle(ioc,
-		     sas_target_priv_data->handle);
+		raid_device = _scsih_raid_device_find_by_handle(ioc, handle);
 		spin_unlock_irqrestore(&ioc->raid_device_lock, flags);
 		if (!raid_device) {
 			dfailprintk(ioc, printk(MPT2SAS_WARN_FMT
@@ -1989,8 +2008,6 @@ _scsih_slave_configure(struct scsi_device *sdev)
 			return 1;
 		}
 
-		_scsih_get_volume_capabilities(ioc, raid_device);
-
 		if (_scsih_get_volume_capabilities(ioc, raid_device)) {
 			dfailprintk(ioc, printk(MPT2SAS_WARN_FMT
 			    "failure at %s:%d/%s()!\n", ioc->name, __FILE__,
@@ -2058,68 +2075,67 @@ _scsih_slave_configure(struct scsi_device *sdev)
 		_scsih_change_queue_depth(sdev, qdepth, SCSI_QDEPTH_DEFAULT);
 		/* raid transport support */
 		if (!ioc->is_warpdrive)
-			_scsih_set_level(sdev, raid_device);
+			_scsih_set_level(sdev, raid_device->volume_type);
 		return 0;
 	}
 
 	/* non-raid handling */
-	spin_lock_irqsave(&ioc->sas_device_lock, flags);
-	sas_device = mpt2sas_scsih_sas_device_find_by_sas_address(ioc,
-	   sas_device_priv_data->sas_target->sas_address);
-	spin_unlock_irqrestore(&ioc->sas_device_lock, flags);
-	if (sas_device) {
-		if (sas_target_priv_data->flags &
-		    MPT_TARGET_FLAGS_RAID_COMPONENT) {
-			if (mpt2sas_config_get_volume_handle(ioc,
-			    sas_device->handle, &sas_device->volume_handle)) {
-				dfailprintk(ioc, printk(MPT2SAS_WARN_FMT
-				    "failure at %s:%d/%s()!\n", ioc->name,
-				    __FILE__, __LINE__, __func__));
-				return 1;
-			}
-			if (sas_device->volume_handle &&
-			    mpt2sas_config_get_volume_wwid(ioc,
-			    sas_device->volume_handle,
-			    &sas_device->volume_wwid)) {
-				dfailprintk(ioc, printk(MPT2SAS_WARN_FMT
-				    "failure at %s:%d/%s()!\n", ioc->name,
-				    __FILE__, __LINE__, __func__));
-				return 1;
-			}
+	if (sas_target_priv_data->flags & MPT_TARGET_FLAGS_RAID_COMPONENT) {
+		if (mpt2sas_config_get_volume_handle(ioc, handle,
+		    &volume_handle)) {
+			dfailprintk(ioc, printk(MPT2SAS_WARN_FMT
+			    "failure at %s:%d/%s()!\n", ioc->name,
+			    __FILE__, __LINE__, __func__));
+			return 1;
 		}
-		if (sas_device->device_info & MPI2_SAS_DEVICE_INFO_SSP_TARGET) {
-			qdepth = MPT2SAS_SAS_QUEUE_DEPTH;
-			ssp_target = 1;
-			ds = "SSP";
-		} else {
-			qdepth = MPT2SAS_SATA_QUEUE_DEPTH;
-			if (sas_device->device_info &
-			    MPI2_SAS_DEVICE_INFO_STP_TARGET)
-				ds = "STP";
-			else if (sas_device->device_info &
-			    MPI2_SAS_DEVICE_INFO_SATA_DEVICE)
-				ds = "SATA";
+		if (volume_handle && mpt2sas_config_get_volume_wwid(ioc,
+		    volume_handle, &volume_wwid)) {
+			dfailprintk(ioc, printk(MPT2SAS_WARN_FMT
+			    "failure at %s:%d/%s()!\n", ioc->name,
+			    __FILE__, __LINE__, __func__));
+			return 1;
 		}
+	}
 
-		sdev_printk(KERN_INFO, sdev, "%s: handle(0x%04x), "
-		    "sas_addr(0x%016llx), phy(%d), device_name(0x%016llx)\n",
-		    ds, sas_device->handle,
-		    (unsigned long long)sas_device->sas_address,
-		    sas_device->phy,
-		    (unsigned long long)sas_device->device_name);
-		sdev_printk(KERN_INFO, sdev, "%s: "
-		    "enclosure_logical_id(0x%016llx), slot(%d)\n", ds,
-		    (unsigned long long) sas_device->enclosure_logical_id,
-		    sas_device->slot);
-
-		if (!ssp_target)
-			_scsih_display_sata_capabilities(ioc, sas_device, sdev);
-	} else {
+	spin_lock_irqsave(&ioc->sas_device_lock, flags);
+	sas_device = mpt2sas_scsih_sas_device_find_by_sas_address(ioc,
+	   sas_device_priv_data->sas_target->sas_address);
+	if (!sas_device) {
+		spin_unlock_irqrestore(&ioc->sas_device_lock, flags);
 		dfailprintk(ioc, printk(MPT2SAS_WARN_FMT
-		    "failure at %s:%d/%s()!\n", ioc->name, __FILE__, __LINE__,
-		    __func__));
+			"failure at %s:%d/%s()!\n", ioc->name, __FILE__,
+			__LINE__, __func__));
 		return 1;
 	}
+	sas_device->volume_handle = volume_handle;
+	sas_device->volume_wwid = volume_wwid;
+	if (sas_device->device_info & MPI2_SAS_DEVICE_INFO_SSP_TARGET) {
+		qdepth = MPT2SAS_SAS_QUEUE_DEPTH;
+		ssp_target = 1;
+		ds = "SSP";
+	} else {
+		qdepth = MPT2SAS_SATA_QUEUE_DEPTH;
+		if (sas_device->device_info & MPI2_SAS_DEVICE_INFO_STP_TARGET)
+			ds = "STP";
+		else if (sas_device->device_info &
+		    MPI2_SAS_DEVICE_INFO_SATA_DEVICE)
+			ds = "SATA";
+	}
+	sdev_printk(KERN_INFO, sdev, "%s: handle(0x%04x), "
+	    "sas_addr(0x%016llx), phy(%d), device_name(0x%016llx)\n",
+	    ds, sas_device->handle,
+	    (unsigned long long)sas_device->sas_address,
+	    sas_device->phy,
+	    (unsigned long long)sas_device->device_name);
+	sdev_printk(KERN_INFO, sdev, "%s: "
+	    "enclosure_logical_id(0x%016llx), slot(%d)\n", ds,
+	    (unsigned long long) sas_device->enclosure_logical_id,
+	    sas_device->slot);
+
+	spin_unlock_irqrestore(&ioc->sas_device_lock, flags);
+	if (!ssp_target)
+		_scsih_display_sata_capabilities(ioc, handle, sdev);
+
 
 	_scsih_change_queue_depth(sdev, qdepth, SCSI_QDEPTH_DEFAULT);
 
@@ -2899,7 +2915,7 @@ _scsih_ublock_io_all_device(struct MPT2SAS_ADAPTER *ioc)
  * During device pull we need to appropiately set the sdev state.
  */
 static void
-_scsih_ublock_io_device(struct MPT2SAS_ADAPTER *ioc, u16 handle)
+_scsih_ublock_io_device(struct MPT2SAS_ADAPTER *ioc, u64 sas_address)
 {
 	struct MPT2SAS_DEVICE *sas_device_priv_data;
 	struct scsi_device *sdev;
@@ -2910,10 +2926,12 @@ _scsih_ublock_io_device(struct MPT2SAS_ADAPTER *ioc, u16 handle)
 			continue;
 		if (!sas_device_priv_data->block)
 			continue;
-		if (sas_device_priv_data->sas_target->handle == handle) {
+		if (sas_device_priv_data->sas_target->sas_address ==
+								sas_address) {
 			dewtprintk(ioc, sdev_printk(KERN_INFO, sdev,
 			    MPT2SAS_INFO_FMT "SDEV_RUNNING: "
-			    "handle(0x%04x)\n", ioc->name, handle));
+			    "sas address(0x%016llx)\n", ioc->name,
+				(unsigned long long)sas_address));
 			sas_device_priv_data->block = 0;
 			scsi_internal_device_unblock(sdev);
 		}
@@ -3006,10 +3024,10 @@ _scsih_block_io_to_children_attached_to_ex(struct MPT2SAS_ADAPTER *ioc,
 			sas_device =
 			    mpt2sas_scsih_sas_device_find_by_sas_address(ioc,
 			   mpt2sas_port->remote_identify.sas_address);
+			if (sas_device)
+				set_bit(sas_device->handle,
+				    ioc->blocking_handles);
 			spin_unlock_irqrestore(&ioc->sas_device_lock, flags);
-			if (!sas_device)
-				continue;
-			_scsih_block_io_device(ioc, sas_device->handle);
 		}
 	}
 
@@ -3020,12 +3038,9 @@ _scsih_block_io_to_children_attached_to_ex(struct MPT2SAS_ADAPTER *ioc,
 		    SAS_EDGE_EXPANDER_DEVICE ||
 		    mpt2sas_port->remote_identify.device_type ==
 		    SAS_FANOUT_EXPANDER_DEVICE) {
-
-			spin_lock_irqsave(&ioc->sas_node_lock, flags);
 			expander_sibling =
 			    mpt2sas_scsih_expander_find_by_sas_address(
 			    ioc, mpt2sas_port->remote_identify.sas_address);
-			spin_unlock_irqrestore(&ioc->sas_node_lock, flags);
 			_scsih_block_io_to_children_attached_to_ex(ioc,
 			    expander_sibling);
 		}
@@ -3124,7 +3139,7 @@ _scsih_tm_tr_send(struct MPT2SAS_ADAPTER *ioc, u16 handle)
 		dewtprintk(ioc, printk(MPT2SAS_INFO_FMT "setting delete flag: "
 		"handle(0x%04x), sas_addr(0x%016llx)\n", ioc->name, handle,
 			(unsigned long long)sas_address));
-		_scsih_ublock_io_device(ioc, handle);
+		_scsih_ublock_io_device(ioc, sas_address);
 		sas_target_priv_data->handle = MPT2SAS_INVALID_DEVICE_HANDLE;
 	}
 
@@ -3174,16 +3189,19 @@ static u8
 _scsih_sas_control_complete(struct MPT2SAS_ADAPTER *ioc, u16 smid,
     u8 msix_index, u32 reply)
 {
-#ifdef CONFIG_SCSI_MPT2SAS_LOGGING
 	Mpi2SasIoUnitControlReply_t *mpi_reply =
 	    mpt2sas_base_get_reply_virt_addr(ioc, reply);
-#endif
-	dewtprintk(ioc, printk(MPT2SAS_INFO_FMT
-	    "sc_complete:handle(0x%04x), (open) "
-	    "smid(%d), ioc_status(0x%04x), loginfo(0x%08x)\n",
-	    ioc->name, le16_to_cpu(mpi_reply->DevHandle), smid,
-	    le16_to_cpu(mpi_reply->IOCStatus),
-	    le32_to_cpu(mpi_reply->IOCLogInfo)));
+	if (likely(mpi_reply)) {
+		dewtprintk(ioc, printk(MPT2SAS_INFO_FMT
+		"sc_complete:handle(0x%04x), (open) "
+		"smid(%d), ioc_status(0x%04x), loginfo(0x%08x)\n",
+		ioc->name, le16_to_cpu(mpi_reply->DevHandle), smid,
+		le16_to_cpu(mpi_reply->IOCStatus),
+		le32_to_cpu(mpi_reply->IOCLogInfo)));
+	} else {
+		printk(MPT2SAS_ERR_FMT "mpi_reply not valid at %s:%d/%s()!\n",
+		    ioc->name, __FILE__, __LINE__, __func__);
+	}
 	return 1;
 }
 
@@ -3262,7 +3280,11 @@ _scsih_tm_volume_tr_complete(struct MPT2SAS_ADAPTER *ioc, u16 smid,
 		   "progress!\n", __func__, ioc->name));
 		return 1;
 	}
-
+	if (unlikely(!mpi_reply)) {
+		printk(MPT2SAS_ERR_FMT "mpi_reply not valid at %s:%d/%s()!\n",
+		    ioc->name, __FILE__, __LINE__, __func__);
+		return 1;
+	}
 	mpi_request_tm = mpt2sas_base_get_msg_frame(ioc, smid);
 	handle = le16_to_cpu(mpi_request_tm->DevHandle);
 	if (handle != le16_to_cpu(mpi_reply->DevHandle)) {
@@ -3325,7 +3347,11 @@ _scsih_tm_tr_complete(struct MPT2SAS_ADAPTER *ioc, u16 smid, u8 msix_index,
 		    "operational\n", __func__, ioc->name));
 		return 1;
 	}
-
+	if (unlikely(!mpi_reply)) {
+		printk(MPT2SAS_ERR_FMT "mpi_reply not valid at %s:%d/%s()!\n",
+		    ioc->name, __FILE__, __LINE__, __func__);
+		return 1;
+	}
 	mpi_request_tm = mpt2sas_base_get_msg_frame(ioc, smid);
 	handle = le16_to_cpu(mpi_request_tm->DevHandle);
 	if (handle != le16_to_cpu(mpi_reply->DevHandle)) {
@@ -3441,14 +3467,20 @@ _scsih_check_topo_delete_events(struct MPT2SAS_ADAPTER *ioc,
 		_scsih_block_io_to_children_attached_directly(ioc, event_data);
 		return;
 	}
-
-	if (event_data->ExpStatus == MPI2_EVENT_SAS_TOPO_ES_DELAY_NOT_RESPONDING
-	 || event_data->ExpStatus == MPI2_EVENT_SAS_TOPO_ES_NOT_RESPONDING) {
+	if (event_data->ExpStatus ==
+	    MPI2_EVENT_SAS_TOPO_ES_DELAY_NOT_RESPONDING) {
+		/* put expander attached devices into blocking state */
 		spin_lock_irqsave(&ioc->sas_node_lock, flags);
 		sas_expander = mpt2sas_scsih_expander_find_by_handle(ioc,
 		    expander_handle);
-		spin_unlock_irqrestore(&ioc->sas_node_lock, flags);
 		_scsih_block_io_to_children_attached_to_ex(ioc, sas_expander);
+		spin_unlock_irqrestore(&ioc->sas_node_lock, flags);
+		do {
+			handle = find_first_bit(ioc->blocking_handles,
+			    ioc->facts.MaxDevHandle);
+			if (handle < ioc->facts.MaxDevHandle)
+				_scsih_block_io_device(ioc, handle);
+		} while (test_and_clear_bit(handle, ioc->blocking_handles));
 	} else if (event_data->ExpStatus == MPI2_EVENT_SAS_TOPO_ES_RESPONDING)
 		_scsih_block_io_to_children_attached_directly(ioc, event_data);
 
@@ -4446,8 +4478,8 @@ _scsih_io_done(struct MPT2SAS_ADAPTER *ioc, u16 smid, u8 msix_index, u32 reply)
 	    != MPI2_IOCSTATUS_SCSI_TASK_TERMINATED)) {
 		spin_lock_irqsave(&ioc->scsi_lookup_lock, flags);
 		ioc->scsi_lookup[smid - 1].scmd = scmd;
-		spin_unlock_irqrestore(&ioc->scsi_lookup_lock, flags);
 		_scsih_scsi_direct_io_set(ioc, smid, 0);
+		spin_unlock_irqrestore(&ioc->scsi_lookup_lock, flags);
 		memcpy(mpi_request->CDB.CDB32, scmd->cmnd, scmd->cmd_len);
 		mpi_request->DevHandle =
 		    cpu_to_le16(sas_device_priv_data->sas_target->handle);
@@ -5020,13 +5052,11 @@ mpt2sas_expander_remove(struct MPT2SAS_ADAPTER *ioc, u64 sas_address)
 	spin_lock_irqsave(&ioc->sas_node_lock, flags);
 	sas_expander = mpt2sas_scsih_expander_find_by_sas_address(ioc,
 	    sas_address);
-	if (!sas_expander) {
-		spin_unlock_irqrestore(&ioc->sas_node_lock, flags);
-		return;
-	}
-	list_del(&sas_expander->list);
+	if (sas_expander)
+		list_del(&sas_expander->list);
 	spin_unlock_irqrestore(&ioc->sas_node_lock, flags);
-	_scsih_expander_node_remove(ioc, sas_expander);
+	if (sas_expander)
+		_scsih_expander_node_remove(ioc, sas_expander);
 }
 
 /**
@@ -5106,6 +5136,7 @@ _scsih_check_device(struct MPT2SAS_ADAPTER *ioc, u16 handle)
 	struct MPT2SAS_TARGET *sas_target_priv_data;
 	u32 device_info;
 
+
 	if ((mpt2sas_config_get_sas_device_pg0(ioc, &mpi_reply, &sas_device_pg0,
 	    MPI2_SAS_DEVICE_PGAD_FORM_HANDLE, handle)))
 		return;
@@ -5139,21 +5170,24 @@ _scsih_check_device(struct MPT2SAS_ADAPTER *ioc, u16 handle)
 		sas_target_priv_data->handle = handle;
 		sas_device->handle = handle;
 	}
-	spin_unlock_irqrestore(&ioc->sas_device_lock, flags);
 
 	/* check if device is present */
 	if (!(le16_to_cpu(sas_device_pg0.Flags) &
 	    MPI2_SAS_DEVICE0_FLAGS_DEVICE_PRESENT)) {
 		printk(MPT2SAS_ERR_FMT "device is not present "
 		    "handle(0x%04x), flags!!!\n", ioc->name, handle);
+		spin_unlock_irqrestore(&ioc->sas_device_lock, flags);
 		return;
 	}
 
 	/* check if there were any issues with discovery */
 	if (_scsih_check_access_status(ioc, sas_address, handle,
-	    sas_device_pg0.AccessStatus))
+	    sas_device_pg0.AccessStatus)) {
+		spin_unlock_irqrestore(&ioc->sas_device_lock, flags);
 		return;
-	_scsih_ublock_io_device(ioc, handle);
+	}
+	spin_unlock_irqrestore(&ioc->sas_device_lock, flags);
+	_scsih_ublock_io_device(ioc, sas_address);
 
 }
 
@@ -5280,54 +5314,71 @@ static void
 _scsih_remove_device(struct MPT2SAS_ADAPTER *ioc,
     struct _sas_device *sas_device)
 {
-	struct _sas_device sas_device_backup;
 	struct MPT2SAS_TARGET *sas_target_priv_data;
 
-	if (!sas_device)
-		return;
-
-	memcpy(&sas_device_backup, sas_device, sizeof(struct _sas_device));
-	_scsih_sas_device_remove(ioc, sas_device);
-
 	dewtprintk(ioc, printk(MPT2SAS_INFO_FMT "%s: enter: "
 	    "handle(0x%04x), sas_addr(0x%016llx)\n", ioc->name, __func__,
-	    sas_device_backup.handle, (unsigned long long)
-	    sas_device_backup.sas_address));
+		sas_device->handle, (unsigned long long)
+	    sas_device->sas_address));
 
-	if (sas_device_backup.starget && sas_device_backup.starget->hostdata) {
-		sas_target_priv_data = sas_device_backup.starget->hostdata;
+	if (sas_device->starget && sas_device->starget->hostdata) {
+		sas_target_priv_data = sas_device->starget->hostdata;
 		sas_target_priv_data->deleted = 1;
-		_scsih_ublock_io_device(ioc, sas_device_backup.handle);
+		_scsih_ublock_io_device(ioc, sas_device->sas_address);
 		sas_target_priv_data->handle =
 		     MPT2SAS_INVALID_DEVICE_HANDLE;
 	}
 
-	_scsih_ublock_io_device(ioc, sas_device_backup.handle);
-
 	if (!ioc->hide_drives)
 		mpt2sas_transport_port_remove(ioc,
-		    sas_device_backup.sas_address,
-		    sas_device_backup.sas_address_parent);
+		    sas_device->sas_address,
+		    sas_device->sas_address_parent);
 
 	printk(MPT2SAS_INFO_FMT "removing handle(0x%04x), sas_addr"
-	    "(0x%016llx)\n", ioc->name, sas_device_backup.handle,
-	    (unsigned long long) sas_device_backup.sas_address);
+	    "(0x%016llx)\n", ioc->name, sas_device->handle,
+	    (unsigned long long) sas_device->sas_address);
 
 	dewtprintk(ioc, printk(MPT2SAS_INFO_FMT "%s: exit: "
 	    "handle(0x%04x), sas_addr(0x%016llx)\n", ioc->name, __func__,
-	    sas_device_backup.handle, (unsigned long long)
-	    sas_device_backup.sas_address));
+	    sas_device->handle, (unsigned long long)
+	    sas_device->sas_address));
+	kfree(sas_device);
+}
+/**
+ * _scsih_device_remove_by_handle - removing device object by handle
+ * @ioc: per adapter object
+ * @handle: device handle
+ *
+ * Return nothing.
+ */
+static void
+_scsih_device_remove_by_handle(struct MPT2SAS_ADAPTER *ioc, u16 handle)
+{
+	struct _sas_device *sas_device;
+	unsigned long flags;
+
+	if (ioc->shost_recovery)
+		return;
+
+	spin_lock_irqsave(&ioc->sas_device_lock, flags);
+	sas_device = _scsih_sas_device_find_by_handle(ioc, handle);
+	if (sas_device)
+		list_del(&sas_device->list);
+	spin_unlock_irqrestore(&ioc->sas_device_lock, flags);
+	if (sas_device)
+		_scsih_remove_device(ioc, sas_device);
 }
 
 /**
- * mpt2sas_device_remove - removing device object
+ * mpt2sas_device_remove_by_sas_address - removing device object by sas address
  * @ioc: per adapter object
- * @sas_address: expander sas_address
+ * @sas_address: device sas_address
  *
  * Return nothing.
  */
 void
-mpt2sas_device_remove(struct MPT2SAS_ADAPTER *ioc, u64 sas_address)
+mpt2sas_device_remove_by_sas_address(struct MPT2SAS_ADAPTER *ioc,
+	u64 sas_address)
 {
 	struct _sas_device *sas_device;
 	unsigned long flags;
@@ -5338,14 +5389,12 @@ mpt2sas_device_remove(struct MPT2SAS_ADAPTER *ioc, u64 sas_address)
 	spin_lock_irqsave(&ioc->sas_device_lock, flags);
 	sas_device = mpt2sas_scsih_sas_device_find_by_sas_address(ioc,
 	    sas_address);
-	if (!sas_device) {
-		spin_unlock_irqrestore(&ioc->sas_device_lock, flags);
-		return;
-	}
+	if (sas_device)
+		list_del(&sas_device->list);
 	spin_unlock_irqrestore(&ioc->sas_device_lock, flags);
-	_scsih_remove_device(ioc, sas_device);
+	if (sas_device)
+		_scsih_remove_device(ioc, sas_device);
 }
-
 #ifdef CONFIG_SCSI_MPT2SAS_LOGGING
 /**
  * _scsih_sas_topology_change_event_debug - debug for topology event
@@ -5442,7 +5491,6 @@ _scsih_sas_topology_change_event(struct MPT2SAS_ADAPTER *ioc,
 	u16 reason_code;
 	u8 phy_number, max_phys;
 	struct _sas_node *sas_expander;
-	struct _sas_device *sas_device;
 	u64 sas_address;
 	unsigned long flags;
 	u8 link_rate, prev_link_rate;
@@ -5477,15 +5525,17 @@ _scsih_sas_topology_change_event(struct MPT2SAS_ADAPTER *ioc,
 	spin_lock_irqsave(&ioc->sas_node_lock, flags);
 	sas_expander = mpt2sas_scsih_expander_find_by_handle(ioc,
 	    parent_handle);
-	spin_unlock_irqrestore(&ioc->sas_node_lock, flags);
 	if (sas_expander) {
 		sas_address = sas_expander->sas_address;
 		max_phys = sas_expander->num_phys;
 	} else if (parent_handle < ioc->sas_hba.num_phys) {
 		sas_address = ioc->sas_hba.sas_address;
 		max_phys = ioc->sas_hba.num_phys;
-	} else
+	} else {
+	spin_unlock_irqrestore(&ioc->sas_node_lock, flags);
 		return;
+	}
+	spin_unlock_irqrestore(&ioc->sas_node_lock, flags);
 
 	/* handle siblings events */
 	for (i = 0; i < event_data->NumEntries; i++) {
@@ -5540,16 +5590,7 @@ _scsih_sas_topology_change_event(struct MPT2SAS_ADAPTER *ioc,
 			break;
 		case MPI2_EVENT_SAS_TOPO_RC_TARG_NOT_RESPONDING:
 
-			spin_lock_irqsave(&ioc->sas_device_lock, flags);
-			sas_device = _scsih_sas_device_find_by_handle(ioc,
-			    handle);
-			if (!sas_device) {
-				spin_unlock_irqrestore(&ioc->sas_device_lock,
-				    flags);
-				break;
-			}
-			spin_unlock_irqrestore(&ioc->sas_device_lock, flags);
-			_scsih_remove_device(ioc, sas_device);
+			_scsih_device_remove_by_handle(ioc, handle);
 			break;
 		}
 	}
@@ -5672,20 +5713,24 @@ _scsih_sas_device_status_change_event(struct MPT2SAS_ADAPTER *ioc,
 	sas_address = le64_to_cpu(event_data->SASAddress);
 	sas_device = mpt2sas_scsih_sas_device_find_by_sas_address(ioc,
 	    sas_address);
-	spin_unlock_irqrestore(&ioc->sas_device_lock, flags);
 
-	if (!sas_device || !sas_device->starget)
+	if (!sas_device || !sas_device->starget) {
+		spin_unlock_irqrestore(&ioc->sas_device_lock, flags);
 		return;
+	}
 
 	target_priv_data = sas_device->starget->hostdata;
-	if (!target_priv_data)
+	if (!target_priv_data) {
+		spin_unlock_irqrestore(&ioc->sas_device_lock, flags);
 		return;
+	}
 
 	if (event_data->ReasonCode ==
 	    MPI2_EVENT_SAS_DEV_STAT_RC_INTERNAL_DEVICE_RESET)
 		target_priv_data->tm_busy = 1;
 	else
 		target_priv_data->tm_busy = 0;
+	spin_unlock_irqrestore(&ioc->sas_device_lock, flags);
 }
 
 #ifdef CONFIG_SCSI_MPT2SAS_LOGGING
@@ -5950,30 +5995,6 @@ _scsih_reprobe_lun(struct scsi_device *sdev, void *no_uld_attach)
 }
 
 /**
- * _scsih_reprobe_target - reprobing target
- * @starget: scsi target struct
- * @no_uld_attach: sdev->no_uld_attach flag setting
- *
- * Note: no_uld_attach flag determines whether the disk device is attached
- * to block layer. A value of `1` means to not attach.
- **/
-static void
-_scsih_reprobe_target(struct scsi_target *starget, int no_uld_attach)
-{
-	struct MPT2SAS_TARGET *sas_target_priv_data;
-
-	if (starget == NULL)
-		return;
-	sas_target_priv_data = starget->hostdata;
-	if (no_uld_attach)
-		sas_target_priv_data->flags |= MPT_TARGET_FLAGS_RAID_COMPONENT;
-	else
-		sas_target_priv_data->flags &= ~MPT_TARGET_FLAGS_RAID_COMPONENT;
-
-	starget_for_each_device(starget, no_uld_attach ? (void *)1 : NULL,
-	    _scsih_reprobe_lun);
-}
-/**
  * _scsih_sas_volume_add - add new volume
  * @ioc: per adapter object
  * @element: IR config element data
@@ -6024,8 +6045,11 @@ _scsih_sas_volume_add(struct MPT2SAS_ADAPTER *ioc,
 		    raid_device->id, 0);
 		if (rc)
 			_scsih_raid_device_remove(ioc, raid_device);
-	} else
+	} else {
+		spin_lock_irqsave(&ioc->raid_device_lock, flags);
 		_scsih_determine_boot_device(ioc, raid_device, 1);
+		spin_unlock_irqrestore(&ioc->raid_device_lock, flags);
+	}
 }
 
 /**
@@ -6042,21 +6066,25 @@ _scsih_sas_volume_delete(struct MPT2SAS_ADAPTER *ioc, u16 handle)
 	struct _raid_device *raid_device;
 	unsigned long flags;
 	struct MPT2SAS_TARGET *sas_target_priv_data;
+	struct scsi_target *starget = NULL;
 
 	spin_lock_irqsave(&ioc->raid_device_lock, flags);
 	raid_device = _scsih_raid_device_find_by_handle(ioc, handle);
-	spin_unlock_irqrestore(&ioc->raid_device_lock, flags);
-	if (!raid_device)
-		return;
-	if (raid_device->starget) {
-		sas_target_priv_data = raid_device->starget->hostdata;
-		sas_target_priv_data->deleted = 1;
-		scsi_remove_target(&raid_device->starget->dev);
+	if (raid_device) {
+		if (raid_device->starget) {
+			starget = raid_device->starget;
+			sas_target_priv_data = starget->hostdata;
+			sas_target_priv_data->deleted = 1;
+		}
+		printk(MPT2SAS_INFO_FMT "removing handle(0x%04x), wwid"
+		    "(0x%016llx)\n", ioc->name,  raid_device->handle,
+		    (unsigned long long) raid_device->wwid);
+		list_del(&raid_device->list);
+		kfree(raid_device);
 	}
-	printk(MPT2SAS_INFO_FMT "removing handle(0x%04x), wwid"
-	    "(0x%016llx)\n", ioc->name,  raid_device->handle,
-	    (unsigned long long) raid_device->wwid);
-	_scsih_raid_device_remove(ioc, raid_device);
+	spin_unlock_irqrestore(&ioc->raid_device_lock, flags);
+	if (starget)
+		scsi_remove_target(&starget->dev);
 }
 
 /**
@@ -6072,20 +6100,31 @@ _scsih_sas_pd_expose(struct MPT2SAS_ADAPTER *ioc,
     Mpi2EventIrConfigElement_t *element)
 {
 	struct _sas_device *sas_device;
+	struct scsi_target *starget = NULL;
+	struct MPT2SAS_TARGET *sas_target_priv_data;
 	unsigned long flags;
 	u16 handle = le16_to_cpu(element->PhysDiskDevHandle);
 
 	spin_lock_irqsave(&ioc->sas_device_lock, flags);
 	sas_device = _scsih_sas_device_find_by_handle(ioc, handle);
+	if (sas_device) {
+		sas_device->volume_handle = 0;
+		sas_device->volume_wwid = 0;
+		clear_bit(handle, ioc->pd_handles);
+		if (sas_device->starget && sas_device->starget->hostdata) {
+			starget = sas_device->starget;
+			sas_target_priv_data = starget->hostdata;
+			sas_target_priv_data->flags &=
+			    ~MPT_TARGET_FLAGS_RAID_COMPONENT;
+		}
+	}
 	spin_unlock_irqrestore(&ioc->sas_device_lock, flags);
 	if (!sas_device)
 		return;
 
 	/* exposing raid component */
-	sas_device->volume_handle = 0;
-	sas_device->volume_wwid = 0;
-	clear_bit(handle, ioc->pd_handles);
-	_scsih_reprobe_target(sas_device->starget, 0);
+	if (starget)
+		starget_for_each_device(starget, NULL, _scsih_reprobe_lun);
 }
 
 /**
@@ -6101,23 +6140,38 @@ _scsih_sas_pd_hide(struct MPT2SAS_ADAPTER *ioc,
     Mpi2EventIrConfigElement_t *element)
 {
 	struct _sas_device *sas_device;
+	struct scsi_target *starget = NULL;
+	struct MPT2SAS_TARGET *sas_target_priv_data;
 	unsigned long flags;
 	u16 handle = le16_to_cpu(element->PhysDiskDevHandle);
+	u16 volume_handle = 0;
+	u64 volume_wwid = 0;
+
+	mpt2sas_config_get_volume_handle(ioc, handle, &volume_handle);
+	if (volume_handle)
+		mpt2sas_config_get_volume_wwid(ioc, volume_handle,
+		    &volume_wwid);
 
 	spin_lock_irqsave(&ioc->sas_device_lock, flags);
 	sas_device = _scsih_sas_device_find_by_handle(ioc, handle);
+	if (sas_device) {
+		set_bit(handle, ioc->pd_handles);
+		if (sas_device->starget && sas_device->starget->hostdata) {
+			starget = sas_device->starget;
+			sas_target_priv_data = starget->hostdata;
+			sas_target_priv_data->flags |=
+			    MPT_TARGET_FLAGS_RAID_COMPONENT;
+			sas_device->volume_handle = volume_handle;
+			sas_device->volume_wwid = volume_wwid;
+		}
+	}
 	spin_unlock_irqrestore(&ioc->sas_device_lock, flags);
 	if (!sas_device)
 		return;
 
 	/* hiding raid component */
-	mpt2sas_config_get_volume_handle(ioc, handle,
-	    &sas_device->volume_handle);
-	mpt2sas_config_get_volume_wwid(ioc, sas_device->volume_handle,
-	    &sas_device->volume_wwid);
-	set_bit(handle, ioc->pd_handles);
-	_scsih_reprobe_target(sas_device->starget, 1);
-
+	if (starget)
+		starget_for_each_device(starget, (void *)1, _scsih_reprobe_lun);
 }
 
 /**
@@ -6132,16 +6186,9 @@ static void
 _scsih_sas_pd_delete(struct MPT2SAS_ADAPTER *ioc,
     Mpi2EventIrConfigElement_t *element)
 {
-	struct _sas_device *sas_device;
-	unsigned long flags;
 	u16 handle = le16_to_cpu(element->PhysDiskDevHandle);
 
-	spin_lock_irqsave(&ioc->sas_device_lock, flags);
-	sas_device = _scsih_sas_device_find_by_handle(ioc, handle);
-	spin_unlock_irqrestore(&ioc->sas_device_lock, flags);
-	if (!sas_device)
-		return;
-	_scsih_remove_device(ioc, sas_device);
+	_scsih_device_remove_by_handle(ioc, handle);
 }
 
 /**
@@ -6583,18 +6630,13 @@ _scsih_sas_ir_operation_status_event(struct MPT2SAS_ADAPTER *ioc,
 	/* code added for raid transport support */
 	if (event_data->RAIDOperation == MPI2_EVENT_IR_RAIDOP_RESYNC) {
 
-		handle = le16_to_cpu(event_data->VolDevHandle);
-
 		spin_lock_irqsave(&ioc->raid_device_lock, flags);
+		handle = le16_to_cpu(event_data->VolDevHandle);
 		raid_device = _scsih_raid_device_find_by_handle(ioc, handle);
-		spin_unlock_irqrestore(&ioc->raid_device_lock, flags);
-
-		if (!raid_device)
-			return;
-
-		if (event_data->RAIDOperation == MPI2_EVENT_IR_RAIDOP_RESYNC)
+		if (raid_device)
 			raid_device->percent_complete =
 			    event_data->PercentComplete;
+		spin_unlock_irqrestore(&ioc->raid_device_lock, flags);
 	}
 }
 
@@ -6761,13 +6803,18 @@ _scsih_mark_responding_raid_device(struct MPT2SAS_ADAPTER *ioc, u64 wwid,
 			 * required data for Direct IO
 			 */
 			_scsih_init_warpdrive_properties(ioc, raid_device);
-			if (raid_device->handle == handle)
+			spin_lock_irqsave(&ioc->raid_device_lock, flags);
+			if (raid_device->handle == handle) {
+				spin_unlock_irqrestore(&ioc->raid_device_lock,
+				    flags);
 				return;
+			}
 			printk(KERN_INFO "\thandle changed from(0x%04x)!!!\n",
 			    raid_device->handle);
 			raid_device->handle = handle;
 			if (sas_target_priv_data)
 				sas_target_priv_data->handle = handle;
+			spin_unlock_irqrestore(&ioc->raid_device_lock, flags);
 			return;
 		}
 	}
@@ -6939,58 +6986,56 @@ static void
 _scsih_remove_unresponding_sas_devices(struct MPT2SAS_ADAPTER *ioc)
 {
 	struct _sas_device *sas_device, *sas_device_next;
-	struct _sas_node *sas_expander;
+	struct _sas_node *sas_expander, *sas_expander_next;
 	struct _raid_device *raid_device, *raid_device_next;
+	struct list_head tmp_list;
+	unsigned long flags;
 
 	printk(MPT2SAS_INFO_FMT "removing unresponding devices: start\n",
 	    ioc->name);
 
+	/* removing unresponding end devices */
+	printk(MPT2SAS_INFO_FMT "removing unresponding devices: end-devices\n",
+	    ioc->name);
 	list_for_each_entry_safe(sas_device, sas_device_next,
 	    &ioc->sas_device_list, list) {
-		if (sas_device->responding) {
+		if (!sas_device->responding)
+			mpt2sas_device_remove_by_sas_address(ioc,
+				sas_device->sas_address);
+		else
 			sas_device->responding = 0;
-			continue;
-		}
-		if (sas_device->starget)
-			starget_printk(KERN_INFO, sas_device->starget,
-			    "removing: handle(0x%04x), sas_addr(0x%016llx), "
-			    "enclosure logical id(0x%016llx), slot(%d)\n",
-			    sas_device->handle,
-			    (unsigned long long)sas_device->sas_address,
-			    (unsigned long long)
-			    sas_device->enclosure_logical_id,
-			    sas_device->slot);
-		_scsih_remove_device(ioc, sas_device);
 	}
 
-	if (!ioc->ir_firmware)
-		goto retry_expander_search;
-
-	list_for_each_entry_safe(raid_device, raid_device_next,
-	    &ioc->raid_device_list, list) {
-		if (raid_device->responding) {
-			raid_device->responding = 0;
-			continue;
-		}
-		if (raid_device->starget) {
-			starget_printk(KERN_INFO, raid_device->starget,
-			    "removing: handle(0x%04x), wwid(0x%016llx)\n",
-			      raid_device->handle,
-			    (unsigned long long)raid_device->wwid);
-			scsi_remove_target(&raid_device->starget->dev);
+	/* removing unresponding volumes */
+	if (ioc->ir_firmware) {
+		printk(MPT2SAS_INFO_FMT "removing unresponding devices: "
+		    "volumes\n", ioc->name);
+		list_for_each_entry_safe(raid_device, raid_device_next,
+		    &ioc->raid_device_list, list) {
+			if (!raid_device->responding)
+				_scsih_sas_volume_delete(ioc,
+				    raid_device->handle);
+			else
+				raid_device->responding = 0;
 		}
-		_scsih_raid_device_remove(ioc, raid_device);
 	}
-
- retry_expander_search:
-	sas_expander = NULL;
-	list_for_each_entry(sas_expander, &ioc->sas_expander_list, list) {
-		if (sas_expander->responding) {
+	/* removing unresponding expanders */
+	printk(MPT2SAS_INFO_FMT "removing unresponding devices: expanders\n",
+	    ioc->name);
+	spin_lock_irqsave(&ioc->sas_node_lock, flags);
+	INIT_LIST_HEAD(&tmp_list);
+	list_for_each_entry_safe(sas_expander, sas_expander_next,
+	    &ioc->sas_expander_list, list) {
+		if (!sas_expander->responding)
+			list_move_tail(&sas_expander->list, &tmp_list);
+		else
 			sas_expander->responding = 0;
-			continue;
-		}
-		mpt2sas_expander_remove(ioc, sas_expander->sas_address);
-		goto retry_expander_search;
+	}
+	spin_unlock_irqrestore(&ioc->sas_node_lock, flags);
+	list_for_each_entry_safe(sas_expander, sas_expander_next, &tmp_list,
+	    list) {
+		list_del(&sas_expander->list);
+		_scsih_expander_node_remove(ioc, sas_expander);
 	}
 	printk(MPT2SAS_INFO_FMT "removing unresponding devices: complete\n",
 	    ioc->name);
@@ -7043,6 +7088,7 @@ _scsih_scan_for_devices_after_reset(struct MPT2SAS_ADAPTER *ioc)
 	struct _sas_device *sas_device;
 	struct _sas_node *expander_device;
 	static struct _raid_device *raid_device;
+	unsigned long flags;
 
 	printk(MPT2SAS_INFO_FMT "scan devices: start\n", ioc->name);
 
@@ -7057,8 +7103,10 @@ _scsih_scan_for_devices_after_reset(struct MPT2SAS_ADAPTER *ioc)
 		if (ioc_status == MPI2_IOCSTATUS_CONFIG_INVALID_PAGE)
 			break;
 		handle = le16_to_cpu(expander_pg0.DevHandle);
+		spin_lock_irqsave(&ioc->sas_node_lock, flags);
 		expander_device = mpt2sas_scsih_expander_find_by_sas_address(
 		    ioc, le64_to_cpu(expander_pg0.SASAddress));
+		spin_unlock_irqrestore(&ioc->sas_node_lock, flags);
 		if (expander_device)
 			_scsih_refresh_expander_links(ioc, expander_device,
 			    handle);
@@ -7080,7 +7128,9 @@ _scsih_scan_for_devices_after_reset(struct MPT2SAS_ADAPTER *ioc)
 			break;
 		phys_disk_num = pd_pg0.PhysDiskNum;
 		handle = le16_to_cpu(pd_pg0.DevHandle);
+		spin_lock_irqsave(&ioc->sas_device_lock, flags);
 		sas_device = _scsih_sas_device_find_by_handle(ioc, handle);
+		spin_unlock_irqrestore(&ioc->sas_device_lock, flags);
 		if (sas_device)
 			continue;
 		if (mpt2sas_config_get_sas_device_pg0(ioc, &mpi_reply,
@@ -7107,8 +7157,10 @@ _scsih_scan_for_devices_after_reset(struct MPT2SAS_ADAPTER *ioc)
 		if (ioc_status == MPI2_IOCSTATUS_CONFIG_INVALID_PAGE)
 			break;
 		handle = le16_to_cpu(volume_pg1.DevHandle);
+		spin_lock_irqsave(&ioc->raid_device_lock, flags);
 		raid_device = _scsih_raid_device_find_by_wwid(ioc,
 		    le64_to_cpu(volume_pg1.WWID));
+		spin_unlock_irqrestore(&ioc->raid_device_lock, flags);
 		if (raid_device)
 			continue;
 		if (mpt2sas_config_get_raid_volume_pg0(ioc, &mpi_reply,
@@ -7140,8 +7192,10 @@ _scsih_scan_for_devices_after_reset(struct MPT2SAS_ADAPTER *ioc)
 		if (!(_scsih_is_end_device(
 		    le32_to_cpu(sas_device_pg0.DeviceInfo))))
 			continue;
+		spin_lock_irqsave(&ioc->sas_device_lock, flags);
 		sas_device = mpt2sas_scsih_sas_device_find_by_sas_address(ioc,
 		    le64_to_cpu(sas_device_pg0.SASAddress));
+		spin_unlock_irqrestore(&ioc->sas_device_lock, flags);
 		if (sas_device)
 			continue;
 		parent_handle = le16_to_cpu(sas_device_pg0.ParentDevHandle);
@@ -7235,7 +7289,7 @@ _firmware_event_work(struct work_struct *work)
 
 	switch (fw_event->event) {
 	case MPT2SAS_REMOVE_UNRESPONDING_DEVICES:
-		while (scsi_host_in_recovery(ioc->shost))
+		while (scsi_host_in_recovery(ioc->shost) || ioc->shost_recovery)
 			ssleep(1);
 		_scsih_remove_unresponding_sas_devices(ioc);
 		_scsih_scan_for_devices_after_reset(ioc);
@@ -7313,6 +7367,13 @@ mpt2sas_scsih_event_callback(struct MPT2SAS_ADAPTER *ioc, u8 msix_index,
 		return 1;
 
 	mpi_reply = mpt2sas_base_get_reply_virt_addr(ioc, reply);
+
+	if (unlikely(!mpi_reply)) {
+		printk(MPT2SAS_ERR_FMT "mpi_reply not valid at %s:%d/%s()!\n",
+		    ioc->name, __FILE__, __LINE__, __func__);
+		return 1;
+	}
+
 	event = le16_to_cpu(mpi_reply->Event);
 
 	switch (event) {
@@ -7353,14 +7414,14 @@ mpt2sas_scsih_event_callback(struct MPT2SAS_ADAPTER *ioc, u8 msix_index,
 	case MPI2_EVENT_LOG_ENTRY_ADDED:
 	{
 		Mpi2EventDataLogEntryAdded_t *log_entry;
-		u32 *log_code;
+		__le32 *log_code;
 
 		if (!ioc->is_warpdrive)
 			break;
 
 		log_entry = (Mpi2EventDataLogEntryAdded_t *)
 		    mpi_reply->EventData;
-		log_code = (u32 *)log_entry->LogData;
+		log_code = (__le32 *)log_entry->LogData;
 
 		if (le16_to_cpu(log_entry->LogEntryQualifier)
 		    != MPT2_WARPDRIVE_LOGENTRY)
@@ -7487,7 +7548,7 @@ _scsih_expander_node_remove(struct MPT2SAS_ADAPTER *ioc,
 			return;
 		if (mpt2sas_port->remote_identify.device_type ==
 		    SAS_END_DEVICE)
-			mpt2sas_device_remove(ioc,
+			mpt2sas_device_remove_by_sas_address(ioc,
 			    mpt2sas_port->remote_identify.sas_address);
 		else if (mpt2sas_port->remote_identify.device_type ==
 		    SAS_EDGE_EXPANDER_DEVICE ||
@@ -7661,7 +7722,7 @@ _scsih_remove(struct pci_dev *pdev)
 	   &ioc->sas_hba.sas_port_list, port_list) {
 		if (mpt2sas_port->remote_identify.device_type ==
 		    SAS_END_DEVICE)
-			mpt2sas_device_remove(ioc,
+			mpt2sas_device_remove_by_sas_address(ioc,
 			    mpt2sas_port->remote_identify.sas_address);
 		else if (mpt2sas_port->remote_identify.device_type ==
 		    SAS_EDGE_EXPANDER_DEVICE ||
@@ -7733,11 +7794,11 @@ _scsih_probe_boot_devices(struct MPT2SAS_ADAPTER *ioc)
 		if (rc)
 			_scsih_raid_device_remove(ioc, raid_device);
 	} else {
+		spin_lock_irqsave(&ioc->sas_device_lock, flags);
 		sas_device = device;
 		handle = sas_device->handle;
 		sas_address_parent = sas_device->sas_address_parent;
 		sas_address = sas_device->sas_address;
-		spin_lock_irqsave(&ioc->sas_device_lock, flags);
 		list_move_tail(&sas_device->list, &ioc->sas_device_list);
 		spin_unlock_irqrestore(&ioc->sas_device_lock, flags);
 
@@ -8061,8 +8122,8 @@ _scsih_probe(struct pci_dev *pdev, const struct pci_device_id *id)
  out_thread_fail:
 	list_del(&ioc->list);
 	scsi_remove_host(shost);
-	scsi_host_put(shost);
  out_add_shost_fail:
+	scsi_host_put(shost);
 	return -ENODEV;
 }
 
diff --git a/drivers/scsi/mpt2sas/mpt2sas_transport.c b/drivers/scsi/mpt2sas/mpt2sas_transport.c
index 831047466a5..c6cf20f6072 100644
--- a/drivers/scsi/mpt2sas/mpt2sas_transport.c
+++ b/drivers/scsi/mpt2sas/mpt2sas_transport.c
@@ -163,12 +163,15 @@ _transport_set_identify(struct MPT2SAS_ADAPTER *ioc, u16 handle,
 		return -EIO;
 	}
 
-	memset(identify, 0, sizeof(*identify));
+	memset(identify, 0, sizeof(struct sas_identify));
 	device_info = le32_to_cpu(sas_device_pg0.DeviceInfo);
 
 	/* sas_address */
 	identify->sas_address = le64_to_cpu(sas_device_pg0.SASAddress);
 
+	/* phy number of the parent device this device is linked to */
+	identify->phy_identifier = sas_device_pg0.PhyNum;
+
 	/* device_type */
 	switch (device_info & MPI2_SAS_DEVICE_INFO_MASK_DEVICE_TYPE) {
 	case MPI2_SAS_DEVICE_INFO_NO_DEVICE:
@@ -484,7 +487,7 @@ _transport_delete_port(struct MPT2SAS_ADAPTER *ioc,
 
 	ioc->logging_level |= MPT_DEBUG_TRANSPORT;
 	if (device_type == SAS_END_DEVICE)
-		mpt2sas_device_remove(ioc, sas_address);
+		mpt2sas_device_remove_by_sas_address(ioc, sas_address);
 	else if (device_type == SAS_EDGE_EXPANDER_DEVICE ||
 	    device_type == SAS_FANOUT_EXPANDER_DEVICE)
 		mpt2sas_expander_remove(ioc, sas_address);
@@ -792,9 +795,10 @@ mpt2sas_transport_port_remove(struct MPT2SAS_ADAPTER *ioc, u64 sas_address,
 	spin_lock_irqsave(&ioc->sas_node_lock, flags);
 	sas_node = _transport_sas_node_find_by_sas_address(ioc,
 	    sas_address_parent);
-	spin_unlock_irqrestore(&ioc->sas_node_lock, flags);
-	if (!sas_node)
+	if (!sas_node) {
+		spin_unlock_irqrestore(&ioc->sas_node_lock, flags);
 		return;
+	}
 	list_for_each_entry_safe(mpt2sas_port, next, &sas_node->sas_port_list,
 	    port_list) {
 		if (mpt2sas_port->remote_identify.sas_address != sas_address)
@@ -804,8 +808,10 @@ mpt2sas_transport_port_remove(struct MPT2SAS_ADAPTER *ioc, u64 sas_address,
 		goto out;
 	}
  out:
-	if (!found)
+	if (!found) {
+		spin_unlock_irqrestore(&ioc->sas_node_lock, flags);
 		return;
+	}
 
 	for (i = 0; i < sas_node->num_phys; i++) {
 		if (sas_node->phy[i].remote_identify.sas_address == sas_address)
@@ -813,6 +819,7 @@ mpt2sas_transport_port_remove(struct MPT2SAS_ADAPTER *ioc, u64 sas_address,
 			    sizeof(struct sas_identify));
 	}
 
+	spin_unlock_irqrestore(&ioc->sas_node_lock, flags);
 	list_for_each_entry_safe(mpt2sas_phy, next_phy,
 	    &mpt2sas_port->phy_list, port_siblings) {
 		if ((ioc->logging_level & MPT_DEBUG_TRANSPORT))
@@ -986,12 +993,14 @@ mpt2sas_transport_update_links(struct MPT2SAS_ADAPTER *ioc,
 
 	spin_lock_irqsave(&ioc->sas_node_lock, flags);
 	sas_node = _transport_sas_node_find_by_sas_address(ioc, sas_address);
-	spin_unlock_irqrestore(&ioc->sas_node_lock, flags);
-	if (!sas_node)
+	if (!sas_node) {
+		spin_unlock_irqrestore(&ioc->sas_node_lock, flags);
 		return;
+	}
 
 	mpt2sas_phy = &sas_node->phy[phy_number];
 	mpt2sas_phy->attached_handle = handle;
+	spin_unlock_irqrestore(&ioc->sas_node_lock, flags);
 	if (handle && (link_rate >= MPI2_SAS_NEG_LINK_RATE_1_5)) {
 		_transport_set_identify(ioc, handle,
 		    &mpt2sas_phy->remote_identify);
@@ -1310,17 +1319,20 @@ _transport_get_enclosure_identifier(struct sas_rphy *rphy, u64 *identifier)
 	struct MPT2SAS_ADAPTER *ioc = rphy_to_ioc(rphy);
 	struct _sas_device *sas_device;
 	unsigned long flags;
+	int rc;
 
 	spin_lock_irqsave(&ioc->sas_device_lock, flags);
 	sas_device = mpt2sas_scsih_sas_device_find_by_sas_address(ioc,
 	    rphy->identify.sas_address);
+	if (sas_device) {
+		*identifier = sas_device->enclosure_logical_id;
+		rc = 0;
+	} else {
+		*identifier = 0;
+		rc = -ENXIO;
+	}
 	spin_unlock_irqrestore(&ioc->sas_device_lock, flags);
-
-	if (!sas_device)
-		return -ENXIO;
-
-	*identifier = sas_device->enclosure_logical_id;
-	return 0;
+	return rc;
 }
 
 /**
@@ -1335,16 +1347,17 @@ _transport_get_bay_identifier(struct sas_rphy *rphy)
 	struct MPT2SAS_ADAPTER *ioc = rphy_to_ioc(rphy);
 	struct _sas_device *sas_device;
 	unsigned long flags;
+	int rc;
 
 	spin_lock_irqsave(&ioc->sas_device_lock, flags);
 	sas_device = mpt2sas_scsih_sas_device_find_by_sas_address(ioc,
 	    rphy->identify.sas_address);
+	if (sas_device)
+		rc = sas_device->slot;
+	else
+		rc = -ENXIO;
 	spin_unlock_irqrestore(&ioc->sas_device_lock, flags);
-
-	if (!sas_device)
-		return -ENXIO;
-
-	return sas_device->slot;
+	return rc;
 }
 
 /* phy control request structure */
@@ -1629,11 +1642,13 @@ _transport_phy_enable(struct sas_phy *phy, int enable)
 {
 	struct MPT2SAS_ADAPTER *ioc = phy_to_ioc(phy);
 	Mpi2SasIOUnitPage1_t *sas_iounit_pg1 = NULL;
+	Mpi2SasIOUnitPage0_t *sas_iounit_pg0 = NULL;
 	Mpi2ConfigReply_t mpi_reply;
 	u16 ioc_status;
 	u16 sz;
 	int rc = 0;
 	unsigned long flags;
+	int i, discovery_active;
 
 	spin_lock_irqsave(&ioc->sas_node_lock, flags);
 	if (_transport_sas_node_find_by_sas_address(ioc,
@@ -1651,7 +1666,50 @@ _transport_phy_enable(struct sas_phy *phy, int enable)
 
 	/* handle hba phys */
 
-	/* sas_iounit page 1 */
+	/* read sas_iounit page 0 */
+	sz = offsetof(Mpi2SasIOUnitPage0_t, PhyData) + (ioc->sas_hba.num_phys *
+	    sizeof(Mpi2SasIOUnit0PhyData_t));
+	sas_iounit_pg0 = kzalloc(sz, GFP_KERNEL);
+	if (!sas_iounit_pg0) {
+		printk(MPT2SAS_ERR_FMT "failure at %s:%d/%s()!\n",
+		    ioc->name, __FILE__, __LINE__, __func__);
+		rc = -ENOMEM;
+		goto out;
+	}
+	if ((mpt2sas_config_get_sas_iounit_pg0(ioc, &mpi_reply,
+	    sas_iounit_pg0, sz))) {
+		printk(MPT2SAS_ERR_FMT "failure at %s:%d/%s()!\n",
+		    ioc->name, __FILE__, __LINE__, __func__);
+		rc = -ENXIO;
+		goto out;
+	}
+	ioc_status = le16_to_cpu(mpi_reply.IOCStatus) &
+	    MPI2_IOCSTATUS_MASK;
+	if (ioc_status != MPI2_IOCSTATUS_SUCCESS) {
+		printk(MPT2SAS_ERR_FMT "failure at %s:%d/%s()!\n",
+		    ioc->name, __FILE__, __LINE__, __func__);
+		rc = -EIO;
+		goto out;
+	}
+
+	/* unable to enable/disable phys when when discovery is active */
+	for (i = 0, discovery_active = 0; i < ioc->sas_hba.num_phys ; i++) {
+		if (sas_iounit_pg0->PhyData[i].PortFlags &
+		    MPI2_SASIOUNIT0_PORTFLAGS_DISCOVERY_IN_PROGRESS) {
+			printk(MPT2SAS_ERR_FMT "discovery is active on "
+			    "port = %d, phy = %d: unable to enable/disable "
+			    "phys, try again later!\n", ioc->name,
+			    sas_iounit_pg0->PhyData[i].Port, i);
+			discovery_active = 1;
+		}
+	}
+
+	if (discovery_active) {
+		rc = -EAGAIN;
+		goto out;
+	}
+
+	/* read sas_iounit page 1 */
 	sz = offsetof(Mpi2SasIOUnitPage1_t, PhyData) + (ioc->sas_hba.num_phys *
 	    sizeof(Mpi2SasIOUnit1PhyData_t));
 	sas_iounit_pg1 = kzalloc(sz, GFP_KERNEL);
@@ -1676,7 +1734,18 @@ _transport_phy_enable(struct sas_phy *phy, int enable)
 		rc = -EIO;
 		goto out;
 	}
-
+	/* copy Port/PortFlags/PhyFlags from page 0 */
+	for (i = 0; i < ioc->sas_hba.num_phys ; i++) {
+		sas_iounit_pg1->PhyData[i].Port =
+		    sas_iounit_pg0->PhyData[i].Port;
+		sas_iounit_pg1->PhyData[i].PortFlags =
+		    (sas_iounit_pg0->PhyData[i].PortFlags &
+		    MPI2_SASIOUNIT0_PORTFLAGS_AUTO_PORT_CONFIG);
+		sas_iounit_pg1->PhyData[i].PhyFlags =
+		    (sas_iounit_pg0->PhyData[i].PhyFlags &
+		    (MPI2_SASIOUNIT0_PHYFLAGS_ZONING_ENABLED +
+		    MPI2_SASIOUNIT0_PHYFLAGS_PHY_DISABLED));
+	}
 	if (enable)
 		sas_iounit_pg1->PhyData[phy->number].PhyFlags
 		    &= ~MPI2_SASIOUNIT1_PHYFLAGS_PHY_DISABLE;
@@ -1692,6 +1761,7 @@ _transport_phy_enable(struct sas_phy *phy, int enable)
 
  out:
 	kfree(sas_iounit_pg1);
+	kfree(sas_iounit_pg0);
 	return rc;
 }
 
@@ -1828,7 +1898,7 @@ _transport_smp_handler(struct Scsi_Host *shost, struct sas_rphy *rphy,
 	struct MPT2SAS_ADAPTER *ioc = shost_priv(shost);
 	Mpi2SmpPassthroughRequest_t *mpi_request;
 	Mpi2SmpPassthroughReply_t *mpi_reply;
-	int rc;
+	int rc, i;
 	u16 smid;
 	u32 ioc_state;
 	unsigned long timeleft;
@@ -1837,24 +1907,20 @@ _transport_smp_handler(struct Scsi_Host *shost, struct sas_rphy *rphy,
 	u8 issue_reset = 0;
 	dma_addr_t dma_addr_in = 0;
 	dma_addr_t dma_addr_out = 0;
+	dma_addr_t pci_dma_in = 0;
+	dma_addr_t pci_dma_out = 0;
+	void *pci_addr_in = NULL;
+	void *pci_addr_out = NULL;
 	u16 wait_state_count;
 	struct request *rsp = req->next_rq;
+	struct bio_vec *bvec = NULL;
 
 	if (!rsp) {
 		printk(MPT2SAS_ERR_FMT "%s: the smp response space is "
 		    "missing\n", ioc->name, __func__);
 		return -EINVAL;
 	}
-
-	/* do we need to support multiple segments? */
-	if (req->bio->bi_vcnt > 1 || rsp->bio->bi_vcnt > 1) {
-		printk(MPT2SAS_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));
-		return -EINVAL;
-	}
-
-	if (ioc->shost_recovery) {
+	if (ioc->shost_recovery || ioc->pci_error_recovery) {
 		printk(MPT2SAS_INFO_FMT "%s: host reset in progress!\n",
 		    __func__, ioc->name);
 		return -EFAULT;
@@ -1872,6 +1938,59 @@ _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) {
+		u32 offset = 0;
+
+		/* Allocate memory and copy the request */
+		pci_addr_out = pci_alloc_consistent(ioc->pdev,
+		    blk_rq_bytes(req), &pci_dma_out);
+		if (!pci_addr_out) {
+			printk(MPT2SAS_INFO_FMT "%s(): PCI Addr out = NULL\n",
+			    ioc->name, __func__);
+			rc = -ENOMEM;
+			goto out;
+		}
+
+		bio_for_each_segment(bvec, req->bio, i) {
+			memcpy(pci_addr_out + offset,
+			    page_address(bvec->bv_page) + bvec->bv_offset,
+			    bvec->bv_len);
+			offset += bvec->bv_len;
+		}
+	} else {
+		dma_addr_out = pci_map_single(ioc->pdev, bio_data(req->bio),
+		    blk_rq_bytes(req), PCI_DMA_BIDIRECTIONAL);
+		if (!dma_addr_out) {
+			printk(MPT2SAS_INFO_FMT "%s(): DMA Addr out = NULL\n",
+			    ioc->name, __func__);
+			rc = -ENOMEM;
+			goto free_pci;
+		}
+	}
+
+	/* Check if the response needs to be populated across
+	 * multiple segments */
+	if (rsp->bio->bi_vcnt > 1) {
+		pci_addr_in = pci_alloc_consistent(ioc->pdev, blk_rq_bytes(rsp),
+		    &pci_dma_in);
+		if (!pci_addr_in) {
+			printk(MPT2SAS_INFO_FMT "%s(): PCI Addr in = NULL\n",
+			    ioc->name, __func__);
+			rc = -ENOMEM;
+			goto unmap;
+		}
+	} else {
+		dma_addr_in =  pci_map_single(ioc->pdev, bio_data(rsp->bio),
+		    blk_rq_bytes(rsp), PCI_DMA_BIDIRECTIONAL);
+		if (!dma_addr_in) {
+			printk(MPT2SAS_INFO_FMT "%s(): DMA Addr in = NULL\n",
+			    ioc->name, __func__);
+			rc = -ENOMEM;
+			goto unmap;
+		}
+	}
+
 	wait_state_count = 0;
 	ioc_state = mpt2sas_base_get_iocstate(ioc, 1);
 	while (ioc_state != MPI2_IOC_STATE_OPERATIONAL) {
@@ -1880,7 +1999,7 @@ _transport_smp_handler(struct Scsi_Host *shost, struct sas_rphy *rphy,
 			    "%s: failed due to ioc not operational\n",
 			    ioc->name, __func__);
 			rc = -EFAULT;
-			goto out;
+			goto unmap;
 		}
 		ssleep(1);
 		ioc_state = mpt2sas_base_get_iocstate(ioc, 1);
@@ -1897,7 +2016,7 @@ _transport_smp_handler(struct Scsi_Host *shost, struct sas_rphy *rphy,
 		printk(MPT2SAS_ERR_FMT "%s: failed obtaining a smid\n",
 		    ioc->name, __func__);
 		rc = -EAGAIN;
-		goto out;
+		goto unmap;
 	}
 
 	rc = 0;
@@ -1919,16 +2038,14 @@ _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;
-	dma_addr_out = pci_map_single(ioc->pdev, bio_data(req->bio),
-		blk_rq_bytes(req), PCI_DMA_BIDIRECTIONAL);
-	if (!dma_addr_out) {
-		mpt2sas_base_free_smid(ioc, smid);
-		goto unmap;
+	if (req->bio->bi_vcnt > 1) {
+		ioc->base_add_sg_single(psge, sgl_flags |
+		    (blk_rq_bytes(req) - 4), pci_dma_out);
+	} else {
+		ioc->base_add_sg_single(psge, sgl_flags |
+		    (blk_rq_bytes(req) - 4), dma_addr_out);
 	}
 
-	ioc->base_add_sg_single(psge, sgl_flags | (blk_rq_bytes(req) - 4),
-	    dma_addr_out);
-
 	/* incr sgel */
 	psge += ioc->sge_size;
 
@@ -1937,16 +2054,14 @@ _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;
-	dma_addr_in = pci_map_single(ioc->pdev, bio_data(rsp->bio),
-				     blk_rq_bytes(rsp), PCI_DMA_BIDIRECTIONAL);
-	if (!dma_addr_in) {
-		mpt2sas_base_free_smid(ioc, smid);
-		goto unmap;
+	if (rsp->bio->bi_vcnt > 1) {
+		ioc->base_add_sg_single(psge, sgl_flags |
+		    (blk_rq_bytes(rsp) + 4), pci_dma_in);
+	} else {
+		ioc->base_add_sg_single(psge, sgl_flags |
+		    (blk_rq_bytes(rsp) + 4), dma_addr_in);
 	}
 
-	ioc->base_add_sg_single(psge, sgl_flags | (blk_rq_bytes(rsp) + 4),
-	    dma_addr_in);
-
 	dtransportprintk(ioc, printk(MPT2SAS_INFO_FMT "%s - "
 	    "sending smp request\n", ioc->name, __func__));
 
@@ -1982,6 +2097,27 @@ _transport_smp_handler(struct Scsi_Host *shost, struct sas_rphy *rphy,
 		req->resid_len = 0;
 		rsp->resid_len -=
 		    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) {
+			u32 offset = 0;
+			u32 bytes_to_copy =
+			    le16_to_cpu(mpi_reply->ResponseDataLength);
+			bio_for_each_segment(bvec, rsp->bio, i) {
+				if (bytes_to_copy <= bvec->bv_len) {
+					memcpy(page_address(bvec->bv_page) +
+					    bvec->bv_offset, pci_addr_in +
+					    offset, bytes_to_copy);
+					break;
+				} else {
+					memcpy(page_address(bvec->bv_page) +
+					    bvec->bv_offset, pci_addr_in +
+					    offset, bvec->bv_len);
+					bytes_to_copy -= bvec->bv_len;
+				}
+				offset += bvec->bv_len;
+			}
+		}
 	} else {
 		dtransportprintk(ioc, printk(MPT2SAS_INFO_FMT
 		    "%s - no reply\n", ioc->name, __func__));
@@ -2003,6 +2139,15 @@ _transport_smp_handler(struct Scsi_Host *shost, struct sas_rphy *rphy,
 		pci_unmap_single(ioc->pdev, dma_addr_in, blk_rq_bytes(rsp),
 		    PCI_DMA_BIDIRECTIONAL);
 
+ free_pci:
+	if (pci_addr_out)
+		pci_free_consistent(ioc->pdev, blk_rq_bytes(req), pci_addr_out,
+		    pci_dma_out);
+
+	if (pci_addr_in)
+		pci_free_consistent(ioc->pdev, blk_rq_bytes(rsp), pci_addr_in,
+		    pci_dma_in);
+
  out:
 	ioc->transport_cmds.status = MPT2_CMD_NOT_USED;
 	mutex_unlock(&ioc->transport_cmds.mutex);
diff --git a/drivers/scsi/pm8001/pm8001_defs.h b/drivers/scsi/pm8001/pm8001_defs.h
index 944afada61e..c3d20c8d4ab 100644
--- a/drivers/scsi/pm8001/pm8001_defs.h
+++ b/drivers/scsi/pm8001/pm8001_defs.h
@@ -66,9 +66,10 @@ enum port_type {
 
 /* driver compile-time configuration */
 #define	PM8001_MAX_CCB		 512	/* max ccbs supported */
+#define PM8001_MPI_QUEUE         1024   /* maximum mpi queue entries */
 #define	PM8001_MAX_INB_NUM	 1
 #define	PM8001_MAX_OUTB_NUM	 1
-#define	PM8001_CAN_QUEUE	 128	/* SCSI Queue depth */
+#define	PM8001_CAN_QUEUE	 508	/* SCSI Queue depth */
 
 /* unchangeable hardware details */
 #define	PM8001_MAX_PHYS		 8	/* max. possible phys */
diff --git a/drivers/scsi/pm8001/pm8001_hwi.c b/drivers/scsi/pm8001/pm8001_hwi.c
index 9d82ee5c10d..bf54aafc2d7 100644
--- a/drivers/scsi/pm8001/pm8001_hwi.c
+++ b/drivers/scsi/pm8001/pm8001_hwi.c
@@ -192,7 +192,7 @@ init_default_table_values(struct pm8001_hba_info *pm8001_ha)
 	pm8001_ha->main_cfg_tbl.fatal_err_interrupt		= 0x01;
 	for (i = 0; i < qn; i++) {
 		pm8001_ha->inbnd_q_tbl[i].element_pri_size_cnt	=
-			0x00000100 | (0x00000040 << 16) | (0x00<<30);
+			PM8001_MPI_QUEUE | (64 << 16) | (0x00<<30);
 		pm8001_ha->inbnd_q_tbl[i].upper_base_addr	=
 			pm8001_ha->memoryMap.region[IB].phys_addr_hi;
 		pm8001_ha->inbnd_q_tbl[i].lower_base_addr	=
@@ -218,7 +218,7 @@ init_default_table_values(struct pm8001_hba_info *pm8001_ha)
 	}
 	for (i = 0; i < qn; i++) {
 		pm8001_ha->outbnd_q_tbl[i].element_size_cnt	=
-			256 | (64 << 16) | (1<<30);
+			PM8001_MPI_QUEUE | (64 << 16) | (0x01<<30);
 		pm8001_ha->outbnd_q_tbl[i].upper_base_addr	=
 			pm8001_ha->memoryMap.region[OB].phys_addr_hi;
 		pm8001_ha->outbnd_q_tbl[i].lower_base_addr	=
@@ -1245,7 +1245,7 @@ static int mpi_msg_free_get(struct inbound_queue_table *circularQ,
 	/* Stores the new consumer index */
 	consumer_index = pm8001_read_32(circularQ->ci_virt);
 	circularQ->consumer_index = cpu_to_le32(consumer_index);
-	if (((circularQ->producer_idx + bcCount) % 256) ==
+	if (((circularQ->producer_idx + bcCount) % PM8001_MPI_QUEUE) ==
 		le32_to_cpu(circularQ->consumer_index)) {
 		*messagePtr = NULL;
 		return -1;
@@ -1253,7 +1253,8 @@ static int mpi_msg_free_get(struct inbound_queue_table *circularQ,
 	/* get memory IOMB buffer address */
 	offset = circularQ->producer_idx * 64;
 	/* increment to next bcCount element */
-	circularQ->producer_idx = (circularQ->producer_idx + bcCount) % 256;
+	circularQ->producer_idx = (circularQ->producer_idx + bcCount)
+				% PM8001_MPI_QUEUE;
 	/* Adds that distance to the base of the region virtual address plus
 	the message header size*/
 	msgHeader = (struct mpi_msg_hdr *)(circularQ->base_virt	+ offset);
@@ -1326,7 +1327,8 @@ static u32 mpi_msg_free_set(struct pm8001_hba_info *pm8001_ha, void *pMsg,
 		return 0;
 	}
 	/* free the circular queue buffer elements associated with the message*/
-	circularQ->consumer_idx = (circularQ->consumer_idx + bc) % 256;
+	circularQ->consumer_idx = (circularQ->consumer_idx + bc)
+				% PM8001_MPI_QUEUE;
 	/* update the CI of outbound queue */
 	pm8001_cw32(pm8001_ha, circularQ->ci_pci_bar, circularQ->ci_offset,
 		circularQ->consumer_idx);
@@ -1383,7 +1385,8 @@ static u32 mpi_msg_consume(struct pm8001_hba_info *pm8001_ha,
 					circularQ->consumer_idx =
 						(circularQ->consumer_idx +
 						((le32_to_cpu(msgHeader_tmp)
-						>> 24) & 0x1f)) % 256;
+						 >> 24) & 0x1f))
+							% PM8001_MPI_QUEUE;
 					msgHeader_tmp = 0;
 					pm8001_write_32(msgHeader, 0, 0);
 					/* update the CI of outbound queue */
@@ -1396,7 +1399,7 @@ static u32 mpi_msg_consume(struct pm8001_hba_info *pm8001_ha,
 				circularQ->consumer_idx =
 					(circularQ->consumer_idx +
 					((le32_to_cpu(msgHeader_tmp) >> 24) &
-					0x1f)) % 256;
+					0x1f)) % PM8001_MPI_QUEUE;
 				msgHeader_tmp = 0;
 				pm8001_write_32(msgHeader, 0, 0);
 				/* update the CI of outbound queue */
@@ -3357,7 +3360,7 @@ mpi_fw_flash_update_resp(struct pm8001_hba_info *pm8001_ha, void *piomb)
 	struct fw_control_ex	fw_control_context;
 	struct fw_flash_Update_resp *ppayload =
 		(struct fw_flash_Update_resp *)(piomb + 4);
-	u32 tag = ppayload->tag;
+	u32 tag = le32_to_cpu(ppayload->tag);
 	struct pm8001_ccb_info *ccb = &pm8001_ha->ccb_info[tag];
 	status = le32_to_cpu(ppayload->status);
 	memcpy(&fw_control_context,
@@ -3703,8 +3706,8 @@ static int mpi_hw_event(struct pm8001_hba_info *pm8001_ha, void* piomb)
  */
 static void process_one_iomb(struct pm8001_hba_info *pm8001_ha, void *piomb)
 {
-	u32 pHeader = (u32)*(u32 *)piomb;
-	u8 opc = (u8)(pHeader & 0xFFF);
+	__le32 pHeader = *(__le32 *)piomb;
+	u8 opc = (u8)((le32_to_cpu(pHeader)) & 0xFFF);
 
 	PM8001_MSG_DBG(pm8001_ha, pm8001_printk("process_one_iomb:"));
 
diff --git a/drivers/scsi/pm8001/pm8001_hwi.h b/drivers/scsi/pm8001/pm8001_hwi.h
index 1a4611eb032..d437309cb1e 100644
--- a/drivers/scsi/pm8001/pm8001_hwi.h
+++ b/drivers/scsi/pm8001/pm8001_hwi.h
@@ -599,7 +599,7 @@ struct fw_flash_Update_req {
  *
  */
 struct fw_flash_Update_resp {
-	dma_addr_t	tag;
+	__le32	tag;
 	__le32	status;
 	u32	reserved[13];
 } __attribute__((packed, aligned(4)));
diff --git a/drivers/scsi/pm8001/pm8001_init.c b/drivers/scsi/pm8001/pm8001_init.c
index 36efaa7c3a5..0267c22f874 100644
--- a/drivers/scsi/pm8001/pm8001_init.c
+++ b/drivers/scsi/pm8001/pm8001_init.c
@@ -235,15 +235,15 @@ static int __devinit pm8001_alloc(struct pm8001_hba_info *pm8001_ha)
 	pm8001_ha->memoryMap.region[PI].alignment = 4;
 
 	/* MPI Memory region 5 inbound queues */
-	pm8001_ha->memoryMap.region[IB].num_elements = 256;
+	pm8001_ha->memoryMap.region[IB].num_elements = PM8001_MPI_QUEUE;
 	pm8001_ha->memoryMap.region[IB].element_size = 64;
-	pm8001_ha->memoryMap.region[IB].total_len = 256 * 64;
+	pm8001_ha->memoryMap.region[IB].total_len = PM8001_MPI_QUEUE * 64;
 	pm8001_ha->memoryMap.region[IB].alignment = 64;
 
-	/* MPI Memory region 6 inbound queues */
-	pm8001_ha->memoryMap.region[OB].num_elements = 256;
+	/* MPI Memory region 6 outbound queues */
+	pm8001_ha->memoryMap.region[OB].num_elements = PM8001_MPI_QUEUE;
 	pm8001_ha->memoryMap.region[OB].element_size = 64;
-	pm8001_ha->memoryMap.region[OB].total_len = 256 * 64;
+	pm8001_ha->memoryMap.region[OB].total_len = PM8001_MPI_QUEUE * 64;
 	pm8001_ha->memoryMap.region[OB].alignment = 64;
 
 	/* Memory region write DMA*/
diff --git a/drivers/scsi/qla1280.c b/drivers/scsi/qla1280.c
index 6c6486f626e..538230be5cc 100644
--- a/drivers/scsi/qla1280.c
+++ b/drivers/scsi/qla1280.c
@@ -4473,17 +4473,14 @@ qla1280_exit(void)
 	pci_unregister_driver(&qla1280_pci_driver);
 	/* release any allocated firmware images */
 	for (i = 0; i < QL_NUM_FW_IMAGES; i++) {
-		if (qla1280_fw_tbl[i].fw) {
-			release_firmware(qla1280_fw_tbl[i].fw);
-			qla1280_fw_tbl[i].fw = NULL;
-		}
+		release_firmware(qla1280_fw_tbl[i].fw);
+		qla1280_fw_tbl[i].fw = NULL;
 	}
 }
 
 module_init(qla1280_init);
 module_exit(qla1280_exit);
 
-
 MODULE_AUTHOR("Qlogic & Jes Sorensen");
 MODULE_DESCRIPTION("Qlogic ISP SCSI (qla1x80/qla1x160) driver");
 MODULE_LICENSE("GPL");
diff --git a/drivers/scsi/qla2xxx/qla_bsg.c b/drivers/scsi/qla2xxx/qla_bsg.c
index f74cc0602f3..bc3cc6d9111 100644
--- a/drivers/scsi/qla2xxx/qla_bsg.c
+++ b/drivers/scsi/qla2xxx/qla_bsg.c
@@ -1367,6 +1367,9 @@ qla2x00_read_optrom(struct fc_bsg_job *bsg_job)
 	struct qla_hw_data *ha = vha->hw;
 	int rval = 0;
 
+	if (ha->flags.isp82xx_reset_hdlr_active)
+		return -EBUSY;
+
 	rval = qla2x00_optrom_setup(bsg_job, vha, 0);
 	if (rval)
 		return rval;
diff --git a/drivers/scsi/qla2xxx/qla_dbg.c b/drivers/scsi/qla2xxx/qla_dbg.c
index 897731b93df..62324a1d557 100644
--- a/drivers/scsi/qla2xxx/qla_dbg.c
+++ b/drivers/scsi/qla2xxx/qla_dbg.c
@@ -15,7 +15,7 @@
  * | Mailbox commands             |       0x113e       | 0x112c-0x112e  |
  * |                              |                    | 0x113a         |
  * | Device Discovery             |       0x2086       | 0x2020-0x2022  |
- * | Queue Command and IO tracing |       0x302f       | 0x3006,0x3008  |
+ * | Queue Command and IO tracing |       0x3030       | 0x3006,0x3008  |
  * |                              |                    | 0x302d-0x302e  |
  * | DPC Thread                   |       0x401c       |		|
  * | Async Events                 |       0x505d       | 0x502b-0x502f  |
diff --git a/drivers/scsi/qla2xxx/qla_isr.c b/drivers/scsi/qla2xxx/qla_isr.c
index f79844ce712..ce42288049b 100644
--- a/drivers/scsi/qla2xxx/qla_isr.c
+++ b/drivers/scsi/qla2xxx/qla_isr.c
@@ -1715,13 +1715,24 @@ qla2x00_status_entry(scsi_qla_host_t *vha, struct rsp_que *rsp, void *pkt)
 				res = DID_ERROR << 16;
 				break;
 			}
-		} else {
+		} 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, 0x301f,
 			    "Dropped frame(s) detected (0x%x "
-			    "of 0x%x bytes).\n", resid, scsi_bufflen(cp));
+			    "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, 0x3030,
+			    "scsi_status: 0x%x, lscsi_status: 0x%x\n",
+			    scsi_status, lscsi_status);
 		}
 
 		res = DID_OK << 16 | lscsi_status;
diff --git a/drivers/scsi/qla2xxx/qla_nx.c b/drivers/scsi/qla2xxx/qla_nx.c
index f0528539bbb..de722a93343 100644
--- a/drivers/scsi/qla2xxx/qla_nx.c
+++ b/drivers/scsi/qla2xxx/qla_nx.c
@@ -3125,6 +3125,7 @@ qla82xx_need_reset_handler(scsi_qla_host_t *vha)
 		ql_log(ql_log_info, vha, 0x00b7,
 		    "HW State: COLD/RE-INIT.\n");
 		qla82xx_wr_32(ha, QLA82XX_CRB_DEV_STATE, QLA82XX_DEV_COLD);
+		qla82xx_set_rst_ready(ha);
 		if (ql2xmdenable) {
 			if (qla82xx_md_collect(vha))
 				ql_log(ql_log_warn, vha, 0xb02c,
diff --git a/drivers/scsi/qla2xxx/qla_os.c b/drivers/scsi/qla2xxx/qla_os.c
index a2f999273a5..c9c56a8427f 100644
--- a/drivers/scsi/qla2xxx/qla_os.c
+++ b/drivers/scsi/qla2xxx/qla_os.c
@@ -3577,9 +3577,25 @@ void qla2x00_relogin(struct scsi_qla_host *vha)
 						continue;
 					/* Attempt a retry. */
 					status = 1;
-				} else
+				} else {
 					status = qla2x00_fabric_login(vha,
 					    fcport, &next_loopid);
+					if (status ==  QLA_SUCCESS) {
+						int status2;
+						uint8_t opts;
+
+						opts = 0;
+						if (fcport->flags &
+						    FCF_FCP2_DEVICE)
+							opts |= BIT_1;
+							status2 =
+							    qla2x00_get_port_database(
+								vha, fcport,
+								opts);
+						if (status2 != QLA_SUCCESS)
+							status = 1;
+					}
+				}
 			} else
 				status = qla2x00_local_device_login(vha,
 								fcport);
@@ -4106,8 +4122,7 @@ qla2x00_release_firmware(void)
 
 	mutex_lock(&qla_fw_lock);
 	for (idx = 0; idx < FW_BLOBS; idx++)
-		if (qla_fw_blobs[idx].fw)
-			release_firmware(qla_fw_blobs[idx].fw);
+		release_firmware(qla_fw_blobs[idx].fw);
 	mutex_unlock(&qla_fw_lock);
 }
 
diff --git a/drivers/scsi/qla2xxx/qla_sup.c b/drivers/scsi/qla2xxx/qla_sup.c
index 3c13c0a6be6..a683e766d1a 100644
--- a/drivers/scsi/qla2xxx/qla_sup.c
+++ b/drivers/scsi/qla2xxx/qla_sup.c
@@ -1017,6 +1017,9 @@ qla2xxx_flash_npiv_conf(scsi_qla_host_t *vha)
 	    !IS_CNA_CAPABLE(ha) && !IS_QLA2031(ha))
 		return;
 
+	if (ha->flags.isp82xx_reset_hdlr_active)
+		return;
+
 	ha->isp_ops->read_optrom(vha, (uint8_t *)&hdr,
 	    ha->flt_region_npiv_conf << 2, sizeof(struct qla_npiv_header));
 	if (hdr.version == __constant_cpu_to_le16(0xffff))
diff --git a/drivers/scsi/qla2xxx/qla_version.h b/drivers/scsi/qla2xxx/qla_version.h
index 29d780c3804..f5fdb16bec9 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.03.07.13-k"
+#define QLA2XXX_VERSION      "8.04.00.03-k"
 
 #define QLA_DRIVER_MAJOR_VER	8
-#define QLA_DRIVER_MINOR_VER	3
-#define QLA_DRIVER_PATCH_VER	7
+#define QLA_DRIVER_MINOR_VER	4
+#define QLA_DRIVER_PATCH_VER	0
 #define QLA_DRIVER_BETA_VER	3
diff --git a/drivers/scsi/scsi.c b/drivers/scsi/scsi.c
index 07322ecff90..61c82a345f8 100644
--- a/drivers/scsi/scsi.c
+++ b/drivers/scsi/scsi.c
@@ -90,6 +90,12 @@ unsigned int scsi_logging_level;
 EXPORT_SYMBOL(scsi_logging_level);
 #endif
 
+#if IS_ENABLED(CONFIG_PM) || IS_ENABLED(CONFIG_BLK_DEV_SD)
+/* sd and scsi_pm need to coordinate flushing async actions */
+LIST_HEAD(scsi_sd_probe_domain);
+EXPORT_SYMBOL(scsi_sd_probe_domain);
+#endif
+
 /* NB: These are exposed through /proc/scsi/scsi and form part of the ABI.
  * You may not alter any existing entry (although adding new ones is
  * encouraged once assigned by ANSI/INCITS T10
diff --git a/drivers/scsi/scsi_error.c b/drivers/scsi/scsi_error.c
index 386f0c53bea..d0f71e5d065 100644
--- a/drivers/scsi/scsi_error.c
+++ b/drivers/scsi/scsi_error.c
@@ -664,7 +664,7 @@ static void scsi_abort_eh_cmnd(struct scsi_cmnd *scmd)
 }
 
 /**
- * scsi_eh_prep_cmnd  - Save a scsi command info as part of error recory
+ * scsi_eh_prep_cmnd  - Save a scsi command info as part of error recovery
  * @scmd:       SCSI command structure to hijack
  * @ses:        structure to save restore information
  * @cmnd:       CDB to send. Can be NULL if no new cmnd is needed
@@ -739,7 +739,7 @@ void scsi_eh_prep_cmnd(struct scsi_cmnd *scmd, struct scsi_eh_save *ses,
 EXPORT_SYMBOL(scsi_eh_prep_cmnd);
 
 /**
- * scsi_eh_restore_cmnd  - Restore a scsi command info as part of error recory
+ * scsi_eh_restore_cmnd  - Restore a scsi command info as part of error recovery
  * @scmd:       SCSI command structure to restore
  * @ses:        saved information from a coresponding call to scsi_eh_prep_cmnd
  *
@@ -762,7 +762,7 @@ void scsi_eh_restore_cmnd(struct scsi_cmnd* scmd, struct scsi_eh_save *ses)
 EXPORT_SYMBOL(scsi_eh_restore_cmnd);
 
 /**
- * scsi_send_eh_cmnd  - submit a scsi command as part of error recory
+ * scsi_send_eh_cmnd  - submit a scsi command as part of error recovery
  * @scmd:       SCSI command structure to hijack
  * @cmnd:       CDB to send
  * @cmnd_size:  size in bytes of @cmnd
diff --git a/drivers/scsi/scsi_lib.c b/drivers/scsi/scsi_lib.c
index 5dfd7495d1a..62ddfd31d4c 100644
--- a/drivers/scsi/scsi_lib.c
+++ b/drivers/scsi/scsi_lib.c
@@ -2348,10 +2348,14 @@ EXPORT_SYMBOL(scsi_device_quiesce);
  *
  *	Must be called with user context, may sleep.
  */
-void
-scsi_device_resume(struct scsi_device *sdev)
+void scsi_device_resume(struct scsi_device *sdev)
 {
-	if(scsi_device_set_state(sdev, SDEV_RUNNING))
+	/* check if the device state was mutated prior to resume, and if
+	 * so assume the state is being managed elsewhere (for example
+	 * device deleted during suspend)
+	 */
+	if (sdev->sdev_state != SDEV_QUIESCE ||
+	    scsi_device_set_state(sdev, SDEV_RUNNING))
 		return;
 	scsi_run_queue(sdev->request_queue);
 }
diff --git a/drivers/scsi/scsi_pm.c b/drivers/scsi/scsi_pm.c
index c4670642d02..f661a41fa4c 100644
--- a/drivers/scsi/scsi_pm.c
+++ b/drivers/scsi/scsi_pm.c
@@ -97,7 +97,7 @@ static int scsi_bus_prepare(struct device *dev)
 {
 	if (scsi_is_sdev_device(dev)) {
 		/* sd probing uses async_schedule.  Wait until it finishes. */
-		async_synchronize_full();
+		async_synchronize_full_domain(&scsi_sd_probe_domain);
 
 	} else if (scsi_is_host_device(dev)) {
 		/* Wait until async scanning is finished */
diff --git a/drivers/scsi/scsi_priv.h b/drivers/scsi/scsi_priv.h
index be4fa6d179b..07ce3f51701 100644
--- a/drivers/scsi/scsi_priv.h
+++ b/drivers/scsi/scsi_priv.h
@@ -163,6 +163,8 @@ static inline int scsi_autopm_get_host(struct Scsi_Host *h) { return 0; }
 static inline void scsi_autopm_put_host(struct Scsi_Host *h) {}
 #endif /* CONFIG_PM_RUNTIME */
 
+extern struct list_head scsi_sd_probe_domain;
+
 /* 
  * internal scsi timeout functions: for use by mid-layer and transport
  * classes.
diff --git a/drivers/scsi/scsi_transport_fc.c b/drivers/scsi/scsi_transport_fc.c
index 80fbe2ac0b4..579760420d5 100644
--- a/drivers/scsi/scsi_transport_fc.c
+++ b/drivers/scsi/scsi_transport_fc.c
@@ -2808,17 +2808,20 @@ fc_remote_port_add(struct Scsi_Host *shost, int channel,
 						  FC_RPORT_DEVLOSS_PENDING |
 						  FC_RPORT_DEVLOSS_CALLBK_DONE);
 
+				spin_unlock_irqrestore(shost->host_lock, flags);
+
 				/* if target, initiate a scan */
 				if (rport->scsi_target_id != -1) {
+					scsi_target_unblock(&rport->dev);
+
+					spin_lock_irqsave(shost->host_lock,
+							  flags);
 					rport->flags |= FC_RPORT_SCAN_PENDING;
 					scsi_queue_work(shost,
 							&rport->scan_work);
 					spin_unlock_irqrestore(shost->host_lock,
 							flags);
-					scsi_target_unblock(&rport->dev);
-				} else
-					spin_unlock_irqrestore(shost->host_lock,
-							flags);
+				}
 
 				fc_bsg_goose_queue(rport);
 
@@ -2876,16 +2879,17 @@ fc_remote_port_add(struct Scsi_Host *shost, int channel,
 			if (fci->f->dd_fcrport_size)
 				memset(rport->dd_data, 0,
 						fci->f->dd_fcrport_size);
+			spin_unlock_irqrestore(shost->host_lock, flags);
+
+			if (ids->roles & FC_PORT_ROLE_FCP_TARGET) {
+				scsi_target_unblock(&rport->dev);
 
-			if (rport->roles & FC_PORT_ROLE_FCP_TARGET) {
 				/* initiate a scan of the target */
+				spin_lock_irqsave(shost->host_lock, flags);
 				rport->flags |= FC_RPORT_SCAN_PENDING;
 				scsi_queue_work(shost, &rport->scan_work);
 				spin_unlock_irqrestore(shost->host_lock, flags);
-				scsi_target_unblock(&rport->dev);
-			} else
-				spin_unlock_irqrestore(shost->host_lock, flags);
-
+			}
 			return rport;
 		}
 	}
@@ -3083,12 +3087,12 @@ fc_remote_port_rolechg(struct fc_rport  *rport, u32 roles)
 		/* ensure any stgt delete functions are done */
 		fc_flush_work(shost);
 
+		scsi_target_unblock(&rport->dev);
 		/* initiate a scan of the target */
 		spin_lock_irqsave(shost->host_lock, flags);
 		rport->flags |= FC_RPORT_SCAN_PENDING;
 		scsi_queue_work(shost, &rport->scan_work);
 		spin_unlock_irqrestore(shost->host_lock, flags);
-		scsi_target_unblock(&rport->dev);
 	}
 }
 EXPORT_SYMBOL(fc_remote_port_rolechg);
diff --git a/drivers/scsi/scsi_transport_spi.c b/drivers/scsi/scsi_transport_spi.c
index a2715c31e75..cf08071a9b6 100644
--- a/drivers/scsi/scsi_transport_spi.c
+++ b/drivers/scsi/scsi_transport_spi.c
@@ -1010,10 +1010,10 @@ spi_dv_device(struct scsi_device *sdev)
 	u8 *buffer;
 	const int len = SPI_MAX_ECHO_BUFFER_SIZE*2;
 
-	if (unlikely(scsi_device_get(sdev)))
+	if (unlikely(spi_dv_in_progress(starget)))
 		return;
 
-	if (unlikely(spi_dv_in_progress(starget)))
+	if (unlikely(scsi_device_get(sdev)))
 		return;
 	spi_dv_in_progress(starget) = 1;
 
diff --git a/drivers/scsi/sd.c b/drivers/scsi/sd.c
index 5ba5c2a9e8e..6f0a4c612b3 100644
--- a/drivers/scsi/sd.c
+++ b/drivers/scsi/sd.c
@@ -65,6 +65,7 @@
 #include <scsi/scsicam.h>
 
 #include "sd.h"
+#include "scsi_priv.h"
 #include "scsi_logging.h"
 
 MODULE_AUTHOR("Eric Youngdale");
@@ -2722,7 +2723,7 @@ static int sd_probe(struct device *dev)
 	dev_set_drvdata(dev, sdkp);
 
 	get_device(&sdkp->dev);	/* prevent release before async_schedule */
-	async_schedule(sd_probe_async, sdkp);
+	async_schedule_domain(sd_probe_async, sdkp, &scsi_sd_probe_domain);
 
 	return 0;
 
@@ -2756,7 +2757,7 @@ static int sd_remove(struct device *dev)
 	sdkp = dev_get_drvdata(dev);
 	scsi_autopm_get_device(sdkp->device);
 
-	async_synchronize_full();
+	async_synchronize_full_domain(&scsi_sd_probe_domain);
 	blk_queue_prep_rq(sdkp->device->request_queue, scsi_prep_fn);
 	blk_queue_unprep_rq(sdkp->device->request_queue, NULL);
 	device_del(&sdkp->dev);
diff --git a/drivers/scsi/sg.c b/drivers/scsi/sg.c
index eacd46bb36b..9c5c5f2b396 100644
--- a/drivers/scsi/sg.c
+++ b/drivers/scsi/sg.c
@@ -104,7 +104,7 @@ static int scatter_elem_sz_prev = SG_SCATTER_SZ;
 static int sg_add(struct device *, struct class_interface *);
 static void sg_remove(struct device *, struct class_interface *);
 
-static DEFINE_MUTEX(sg_mutex);
+static DEFINE_SPINLOCK(sg_open_exclusive_lock);
 
 static DEFINE_IDR(sg_index_idr);
 static DEFINE_RWLOCK(sg_index_lock);	/* Also used to lock
@@ -137,13 +137,15 @@ typedef struct sg_request {	/* SG_MAX_QUEUE requests outstanding per file */
 	char res_used;		/* 1 -> using reserve buffer, 0 -> not ... */
 	char orphan;		/* 1 -> drop on sight, 0 -> normal */
 	char sg_io_owned;	/* 1 -> packet belongs to SG_IO */
-	volatile char done;	/* 0->before bh, 1->before read, 2->read */
+	/* done protected by rq_list_lock */
+	char done;		/* 0->before bh, 1->before read, 2->read */
 	struct request *rq;
 	struct bio *bio;
 	struct execute_work ew;
 } Sg_request;
 
 typedef struct sg_fd {		/* holds the state of a file descriptor */
+	/* sfd_siblings is protected by sg_index_lock */
 	struct list_head sfd_siblings;
 	struct sg_device *parentdp;	/* owning device */
 	wait_queue_head_t read_wait;	/* queue read until command done */
@@ -157,7 +159,6 @@ typedef struct sg_fd {		/* holds the state of a file descriptor */
 	Sg_request req_arr[SG_MAX_QUEUE];	/* used as singly-linked list */
 	char low_dma;		/* as in parent but possibly overridden to 1 */
 	char force_packid;	/* 1 -> pack_id input to read(), 0 -> ignored */
-	volatile char closed;	/* 1 -> fd closed but request(s) outstanding */
 	char cmd_q;		/* 1 -> allow command queuing, 0 -> don't */
 	char next_cmd_len;	/* 0 -> automatic (def), >0 -> use on next write() */
 	char keep_orphan;	/* 0 -> drop orphan (def), 1 -> keep for read() */
@@ -171,9 +172,11 @@ typedef struct sg_device { /* holds the state of each scsi generic device */
 	wait_queue_head_t o_excl_wait;	/* queue open() when O_EXCL in use */
 	int sg_tablesize;	/* adapter's max scatter-gather table size */
 	u32 index;		/* device index number */
+	/* sfds is protected by sg_index_lock */
 	struct list_head sfds;
 	volatile char detached;	/* 0->attached, 1->detached pending removal */
-	volatile char exclude;	/* opened for exclusive access */
+	/* exclude protected by sg_open_exclusive_lock */
+	char exclude;		/* opened for exclusive access */
 	char sgdebug;		/* 0->off, 1->sense, 9->dump dev, 10-> all devs */
 	struct gendisk *disk;
 	struct cdev * cdev;	/* char_dev [sysfs: /sys/cdev/major/sg<n>] */
@@ -221,6 +224,38 @@ static int sg_allow_access(struct file *filp, unsigned char *cmd)
 	return blk_verify_command(cmd, filp->f_mode & FMODE_WRITE);
 }
 
+static int get_exclude(Sg_device *sdp)
+{
+	unsigned long flags;
+	int ret;
+
+	spin_lock_irqsave(&sg_open_exclusive_lock, flags);
+	ret = sdp->exclude;
+	spin_unlock_irqrestore(&sg_open_exclusive_lock, flags);
+	return ret;
+}
+
+static int set_exclude(Sg_device *sdp, char val)
+{
+	unsigned long flags;
+
+	spin_lock_irqsave(&sg_open_exclusive_lock, flags);
+	sdp->exclude = val;
+	spin_unlock_irqrestore(&sg_open_exclusive_lock, flags);
+	return val;
+}
+
+static int sfds_list_empty(Sg_device *sdp)
+{
+	unsigned long flags;
+	int ret;
+
+	read_lock_irqsave(&sg_index_lock, flags);
+	ret = list_empty(&sdp->sfds);
+	read_unlock_irqrestore(&sg_index_lock, flags);
+	return ret;
+}
+
 static int
 sg_open(struct inode *inode, struct file *filp)
 {
@@ -232,7 +267,6 @@ sg_open(struct inode *inode, struct file *filp)
 	int res;
 	int retval;
 
-	mutex_lock(&sg_mutex);
 	nonseekable_open(inode, filp);
 	SCSI_LOG_TIMEOUT(3, printk("sg_open: dev=%d, flags=0x%x\n", dev, flags));
 	sdp = sg_get_dev(dev);
@@ -264,25 +298,22 @@ sg_open(struct inode *inode, struct file *filp)
 			retval = -EPERM; /* Can't lock it with read only access */
 			goto error_out;
 		}
-		if (!list_empty(&sdp->sfds) && (flags & O_NONBLOCK)) {
+		if (!sfds_list_empty(sdp) && (flags & O_NONBLOCK)) {
 			retval = -EBUSY;
 			goto error_out;
 		}
-		res = 0;
-		__wait_event_interruptible(sdp->o_excl_wait,
-					   ((!list_empty(&sdp->sfds) || sdp->exclude) ? 0 : (sdp->exclude = 1)), res);
+		res = wait_event_interruptible(sdp->o_excl_wait,
+					   ((!sfds_list_empty(sdp) || get_exclude(sdp)) ? 0 : set_exclude(sdp, 1)));
 		if (res) {
 			retval = res;	/* -ERESTARTSYS because signal hit process */
 			goto error_out;
 		}
-	} else if (sdp->exclude) {	/* some other fd has an exclusive lock on dev */
+	} else if (get_exclude(sdp)) {	/* some other fd has an exclusive lock on dev */
 		if (flags & O_NONBLOCK) {
 			retval = -EBUSY;
 			goto error_out;
 		}
-		res = 0;
-		__wait_event_interruptible(sdp->o_excl_wait, (!sdp->exclude),
-					   res);
+		res = wait_event_interruptible(sdp->o_excl_wait, !get_exclude(sdp));
 		if (res) {
 			retval = res;	/* -ERESTARTSYS because signal hit process */
 			goto error_out;
@@ -292,7 +323,7 @@ sg_open(struct inode *inode, struct file *filp)
 		retval = -ENODEV;
 		goto error_out;
 	}
-	if (list_empty(&sdp->sfds)) {	/* no existing opens on this device */
+	if (sfds_list_empty(sdp)) {	/* no existing opens on this device */
 		sdp->sgdebug = 0;
 		q = sdp->device->request_queue;
 		sdp->sg_tablesize = queue_max_segments(q);
@@ -301,7 +332,7 @@ sg_open(struct inode *inode, struct file *filp)
 		filp->private_data = sfp;
 	else {
 		if (flags & O_EXCL) {
-			sdp->exclude = 0;	/* undo if error */
+			set_exclude(sdp, 0);	/* undo if error */
 			wake_up_interruptible(&sdp->o_excl_wait);
 		}
 		retval = -ENOMEM;
@@ -317,7 +348,6 @@ sdp_put:
 sg_put:
 	if (sdp)
 		sg_put_dev(sdp);
-	mutex_unlock(&sg_mutex);
 	return retval;
 }
 
@@ -332,9 +362,7 @@ sg_release(struct inode *inode, struct file *filp)
 		return -ENXIO;
 	SCSI_LOG_TIMEOUT(3, printk("sg_release: %s\n", sdp->disk->disk_name));
 
-	sfp->closed = 1;
-
-	sdp->exclude = 0;
+	set_exclude(sdp, 0);
 	wake_up_interruptible(&sdp->o_excl_wait);
 
 	scsi_autopm_put_device(sdp->device);
@@ -398,19 +426,14 @@ sg_read(struct file *filp, char __user *buf, size_t count, loff_t * ppos)
 			retval = -EAGAIN;
 			goto free_old_hdr;
 		}
-		while (1) {
-			retval = 0; /* following macro beats race condition */
-			__wait_event_interruptible(sfp->read_wait,
-				(sdp->detached ||
-				(srp = sg_get_rq_mark(sfp, req_pack_id))), 
-				retval);
-			if (sdp->detached) {
-				retval = -ENODEV;
-				goto free_old_hdr;
-			}
-			if (0 == retval)
-				break;
-
+		retval = wait_event_interruptible(sfp->read_wait,
+			(sdp->detached ||
+			(srp = sg_get_rq_mark(sfp, req_pack_id))));
+		if (sdp->detached) {
+			retval = -ENODEV;
+			goto free_old_hdr;
+		}
+		if (retval) {
 			/* -ERESTARTSYS as signal hit process */
 			goto free_old_hdr;
 		}
@@ -771,7 +794,18 @@ sg_common_write(Sg_fd * sfp, Sg_request * srp,
 	return 0;
 }
 
-static int
+static int srp_done(Sg_fd *sfp, Sg_request *srp)
+{
+	unsigned long flags;
+	int ret;
+
+	read_lock_irqsave(&sfp->rq_list_lock, flags);
+	ret = srp->done;
+	read_unlock_irqrestore(&sfp->rq_list_lock, flags);
+	return ret;
+}
+
+static long
 sg_ioctl(struct file *filp, unsigned int cmd_in, unsigned long arg)
 {
 	void __user *p = (void __user *)arg;
@@ -791,40 +825,30 @@ sg_ioctl(struct file *filp, unsigned int cmd_in, unsigned long arg)
 
 	switch (cmd_in) {
 	case SG_IO:
-		{
-			int blocking = 1;	/* ignore O_NONBLOCK flag */
-
-			if (sdp->detached)
-				return -ENODEV;
-			if (!scsi_block_when_processing_errors(sdp->device))
-				return -ENXIO;
-			if (!access_ok(VERIFY_WRITE, p, SZ_SG_IO_HDR))
-				return -EFAULT;
-			result =
-			    sg_new_write(sfp, filp, p, SZ_SG_IO_HDR,
-					 blocking, read_only, 1, &srp);
-			if (result < 0)
-				return result;
-			while (1) {
-				result = 0;	/* following macro to beat race condition */
-				__wait_event_interruptible(sfp->read_wait,
-					(srp->done || sdp->detached),
-					result);
-				if (sdp->detached)
-					return -ENODEV;
-				write_lock_irq(&sfp->rq_list_lock);
-				if (srp->done) {
-					srp->done = 2;
-					write_unlock_irq(&sfp->rq_list_lock);
-					break;
-				}
-				srp->orphan = 1;
-				write_unlock_irq(&sfp->rq_list_lock);
-				return result;	/* -ERESTARTSYS because signal hit process */
-			}
+		if (sdp->detached)
+			return -ENODEV;
+		if (!scsi_block_when_processing_errors(sdp->device))
+			return -ENXIO;
+		if (!access_ok(VERIFY_WRITE, p, SZ_SG_IO_HDR))
+			return -EFAULT;
+		result = sg_new_write(sfp, filp, p, SZ_SG_IO_HDR,
+				 1, read_only, 1, &srp);
+		if (result < 0)
+			return result;
+		result = wait_event_interruptible(sfp->read_wait,
+			(srp_done(sfp, srp) || sdp->detached));
+		if (sdp->detached)
+			return -ENODEV;
+		write_lock_irq(&sfp->rq_list_lock);
+		if (srp->done) {
+			srp->done = 2;
+			write_unlock_irq(&sfp->rq_list_lock);
 			result = sg_new_read(sfp, p, SZ_SG_IO_HDR, srp);
 			return (result < 0) ? result : 0;
 		}
+		srp->orphan = 1;
+		write_unlock_irq(&sfp->rq_list_lock);
+		return result;	/* -ERESTARTSYS because signal hit process */
 	case SG_SET_TIMEOUT:
 		result = get_user(val, ip);
 		if (result)
@@ -1091,18 +1115,6 @@ sg_ioctl(struct file *filp, unsigned int cmd_in, unsigned long arg)
 	}
 }
 
-static long
-sg_unlocked_ioctl(struct file *filp, unsigned int cmd_in, unsigned long arg)
-{
-	int ret;
-
-	mutex_lock(&sg_mutex);
-	ret = sg_ioctl(filp, cmd_in, arg);
-	mutex_unlock(&sg_mutex);
-
-	return ret;
-}
-
 #ifdef CONFIG_COMPAT
 static long sg_compat_ioctl(struct file *filp, unsigned int cmd_in, unsigned long arg)
 {
@@ -1136,8 +1148,11 @@ sg_poll(struct file *filp, poll_table * wait)
 	int count = 0;
 	unsigned long iflags;
 
-	if ((!(sfp = (Sg_fd *) filp->private_data)) || (!(sdp = sfp->parentdp))
-	    || sfp->closed)
+	sfp = filp->private_data;
+	if (!sfp)
+		return POLLERR;
+	sdp = sfp->parentdp;
+	if (!sdp)
 		return POLLERR;
 	poll_wait(filp, &sfp->read_wait, wait);
 	read_lock_irqsave(&sfp->rq_list_lock, iflags);
@@ -1347,7 +1362,7 @@ static const struct file_operations sg_fops = {
 	.read = sg_read,
 	.write = sg_write,
 	.poll = sg_poll,
-	.unlocked_ioctl = sg_unlocked_ioctl,
+	.unlocked_ioctl = sg_ioctl,
 #ifdef CONFIG_COMPAT
 	.compat_ioctl = sg_compat_ioctl,
 #endif
@@ -2312,7 +2327,7 @@ struct sg_proc_leaf {
 	const struct file_operations * fops;
 };
 
-static struct sg_proc_leaf sg_proc_leaf_arr[] = {
+static const struct sg_proc_leaf sg_proc_leaf_arr[] = {
 	{"allow_dio", &adio_fops},
 	{"debug", &debug_fops},
 	{"def_reserved_size", &dressz_fops},
@@ -2332,7 +2347,7 @@ sg_proc_init(void)
 	if (!sg_proc_sgp)
 		return 1;
 	for (k = 0; k < num_leaves; ++k) {
-		struct sg_proc_leaf *leaf = &sg_proc_leaf_arr[k];
+		const struct sg_proc_leaf *leaf = &sg_proc_leaf_arr[k];
 		umode_t mask = leaf->fops->write ? S_IRUGO | S_IWUSR : S_IRUGO;
 		proc_create(leaf->name, mask, sg_proc_sgp, leaf->fops);
 	}
@@ -2533,9 +2548,9 @@ static void sg_proc_debug_helper(struct seq_file *s, Sg_device * sdp)
 			   fp->reserve.bufflen,
 			   (int) fp->reserve.k_use_sg,
 			   (int) fp->low_dma);
-		seq_printf(s, "   cmd_q=%d f_packid=%d k_orphan=%d closed=%d\n",
+		seq_printf(s, "   cmd_q=%d f_packid=%d k_orphan=%d closed=0\n",
 			   (int) fp->cmd_q, (int) fp->force_packid,
-			   (int) fp->keep_orphan, (int) fp->closed);
+			   (int) fp->keep_orphan);
 		for (m = 0, srp = fp->headrp;
 				srp != NULL;
 				++m, srp = srp->nextrp) {
@@ -2612,7 +2627,7 @@ static int sg_proc_seq_show_debug(struct seq_file *s, void *v)
 			     scsidp->lun,
 			     scsidp->host->hostt->emulated);
 		seq_printf(s, " sg_tablesize=%d excl=%d\n",
-			   sdp->sg_tablesize, sdp->exclude);
+			   sdp->sg_tablesize, get_exclude(sdp));
 		sg_proc_debug_helper(s, sdp);
 	}
 	read_unlock_irqrestore(&sg_index_lock, iflags);
diff --git a/drivers/scsi/sim710.c b/drivers/scsi/sim710.c
index 8ac6ce792b6..a318264a4ba 100644
--- a/drivers/scsi/sim710.c
+++ b/drivers/scsi/sim710.c
@@ -17,7 +17,7 @@
  *  Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
  *----------------------------------------------------------------------------
  *
- * MCA card detection code by Trent McNair.
+ * MCA card detection code by Trent McNair. (now deleted)
  * Fixes to not explicitly nul bss data from Xavier Bestel.
  * Some multiboard fixes from Rolf Eike Beer.
  * Auto probing of EISA config space from Trevor Hemsley.
@@ -32,7 +32,6 @@
 #include <linux/blkdev.h>
 #include <linux/device.h>
 #include <linux/init.h>
-#include <linux/mca.h>
 #include <linux/eisa.h>
 #include <linux/interrupt.h>
 #include <scsi/scsi_host.h>
@@ -43,7 +42,7 @@
 #include "53c700.h"
 
 
-/* Must be enough for both EISA and MCA */
+/* Must be enough for EISA */
 #define MAX_SLOTS 8
 static __u8 __initdata id_array[MAX_SLOTS] = { [0 ... MAX_SLOTS-1] = 7 };
 
@@ -89,7 +88,7 @@ param_setup(char *str)
 __setup("sim710=", param_setup);
 
 static struct scsi_host_template sim710_driver_template = {
-	.name			= "LSI (Symbios) 710 MCA/EISA",
+	.name			= "LSI (Symbios) 710 EISA",
 	.proc_name		= "sim710",
 	.this_id		= 7,
 	.module			= THIS_MODULE,
@@ -169,114 +168,6 @@ sim710_device_remove(struct device *dev)
 	return 0;
 }
 
-#ifdef CONFIG_MCA
-
-/* CARD ID 01BB and 01BA use the same pos values */
-#define MCA_01BB_IO_PORTS { 0x0000, 0x0000, 0x0800, 0x0C00, 0x1000, 0x1400, \
-			    0x1800, 0x1C00, 0x2000, 0x2400, 0x2800, \
-			    0x2C00, 0x3000, 0x3400, 0x3800, 0x3C00, \
-			    0x4000, 0x4400, 0x4800, 0x4C00, 0x5000  }
-
-#define MCA_01BB_IRQS { 3, 5, 11, 14 }
-
-/* CARD ID 004f */
-#define MCA_004F_IO_PORTS { 0x0000, 0x0200, 0x0300, 0x0400, 0x0500,  0x0600 }
-#define MCA_004F_IRQS { 5, 9, 14 }
-
-static short sim710_mca_id_table[] = { 0x01bb, 0x01ba, 0x004f, 0};
-
-static __init int
-sim710_mca_probe(struct device *dev)
-{
-	struct mca_device *mca_dev = to_mca_device(dev);
-	int slot = mca_dev->slot;
-	int pos[3];
-	unsigned int base;
-	int irq_vector;
-	short id = sim710_mca_id_table[mca_dev->index];
-	static int io_004f_by_pos[] = MCA_004F_IO_PORTS;
-	static int irq_004f_by_pos[] = MCA_004F_IRQS;
-	static int io_01bb_by_pos[] = MCA_01BB_IO_PORTS;
-	static int irq_01bb_by_pos[] = MCA_01BB_IRQS;
-	char *name;
-	int clock;
-
-	pos[0] = mca_device_read_stored_pos(mca_dev, 2);
-	pos[1] = mca_device_read_stored_pos(mca_dev, 3);
-	pos[2] = mca_device_read_stored_pos(mca_dev, 4);
-
-	/*
-	 * 01BB & 01BA port base by bits 7,6,5,4,3,2 in pos[2]
-	 *
-	 *    000000  <disabled>   001010  0x2800
-	 *    000001  <invalid>    001011  0x2C00
-	 *    000010  0x0800       001100  0x3000
-	 *    000011  0x0C00       001101  0x3400
-	 *    000100  0x1000       001110  0x3800
-	 *    000101  0x1400       001111  0x3C00
-	 *    000110  0x1800       010000  0x4000
-	 *    000111  0x1C00       010001  0x4400
-	 *    001000  0x2000       010010  0x4800
-	 *    001001  0x2400       010011  0x4C00
-	 *                         010100  0x5000
-	 *
-	 * 00F4 port base by bits 3,2,1 in pos[0]
-	 *
-	 *    000  <disabled>      001    0x200
-	 *    010  0x300           011    0x400
-	 *    100  0x500           101    0x600
-	 *
-	 * 01BB & 01BA IRQ is specified in pos[0] bits 7 and 6:
-	 *
-	 *    00   3               10   11
-	 *    01   5               11   14
-	 *
-	 * 00F4 IRQ specified by bits 6,5,4 in pos[0]
-	 *
-	 *    100   5              101    9
-	 *    110   14
-	 */
-
-	if (id == 0x01bb || id == 0x01ba) {
-		base = io_01bb_by_pos[(pos[2] & 0xFC) >> 2];
-		irq_vector =
-			irq_01bb_by_pos[((pos[0] & 0xC0) >> 6)];
-
-		clock = 50;
-		if (id == 0x01bb)
-			name = "NCR 3360/3430 SCSI SubSystem";
-		else
-			name = "NCR Dual SIOP SCSI Host Adapter Board";
-	} else if ( id == 0x004f ) {
-		base = io_004f_by_pos[((pos[0] & 0x0E) >> 1)];
-		irq_vector =
-			irq_004f_by_pos[((pos[0] & 0x70) >> 4) - 4];
-		clock = 50;
-		name = "NCR 53c710 SCSI Host Adapter Board";
-	} else {
-		return -ENODEV;
-	}
-	mca_device_set_name(mca_dev, name);
-	mca_device_set_claim(mca_dev, 1);
-	base = mca_device_transform_ioport(mca_dev, base);
-	irq_vector = mca_device_transform_irq(mca_dev, irq_vector);
-
-	return sim710_probe_common(dev, base, irq_vector, clock,
-				   0, id_array[slot]);
-}
-
-static struct mca_driver sim710_mca_driver = {
-	.id_table		= sim710_mca_id_table,
-	.driver = {
-		.name		= "sim710",
-		.bus		= &mca_bus_type,
-		.probe		= sim710_mca_probe,
-		.remove		= __devexit_p(sim710_device_remove),
-	},
-};
-
-#endif /* CONFIG_MCA */
-
 #ifdef CONFIG_EISA
 static struct eisa_device_id sim710_eisa_ids[] = {
 	{ "CPQ4410" },
@@ -344,10 +235,6 @@ static int __init sim710_init(void)
 		param_setup(sim710);
 #endif
 
-#ifdef CONFIG_MCA
-	err = mca_register_driver(&sim710_mca_driver);
-#endif
-
 #ifdef CONFIG_EISA
 	err = eisa_driver_register(&sim710_eisa_driver);
 #endif
@@ -361,11 +248,6 @@ static int __init sim710_init(void)
 
 static void __exit sim710_exit(void)
 {
-#ifdef CONFIG_MCA
-	if (MCA_bus)
-		mca_unregister_driver(&sim710_mca_driver);
-#endif
-
 #ifdef CONFIG_EISA
 	eisa_driver_unregister(&sim710_eisa_driver);
 #endif
diff --git a/drivers/scsi/st.h b/drivers/scsi/st.h
index ea35632b986..b548923785e 100644
--- a/drivers/scsi/st.h
+++ b/drivers/scsi/st.h
@@ -35,8 +35,8 @@ struct st_request {
 /* The tape buffer descriptor. */
 struct st_buffer {
 	unsigned char dma;	/* DMA-able buffer */
-	unsigned char do_dio;   /* direct i/o set up? */
 	unsigned char cleared;  /* internal buffer cleared after open? */
+	unsigned short do_dio;  /* direct i/o set up? */
 	int buffer_size;
 	int buffer_blocks;
 	int buffer_bytes;
diff --git a/drivers/scsi/storvsc_drv.c b/drivers/scsi/storvsc_drv.c
index 83a1972a199..528d52beaa1 100644
--- a/drivers/scsi/storvsc_drv.c
+++ b/drivers/scsi/storvsc_drv.c
@@ -785,12 +785,22 @@ static void storvsc_command_completion(struct storvsc_cmd_request *cmd_request)
 	/*
 	 * If there is an error; offline the device since all
 	 * error recovery strategies would have already been
-	 * deployed on the host side.
+	 * deployed on the host side. However, if the command
+	 * were a pass-through command deal with it appropriately.
 	 */
-	if (vm_srb->srb_status == SRB_STATUS_ERROR)
-		scmnd->result = DID_TARGET_FAILURE << 16;
-	else
-		scmnd->result = vm_srb->scsi_status;
+	scmnd->result = vm_srb->scsi_status;
+
+	if (vm_srb->srb_status == SRB_STATUS_ERROR) {
+		switch (scmnd->cmnd[0]) {
+		case ATA_16:
+		case ATA_12:
+			set_host_byte(scmnd, DID_PASSTHROUGH);
+			break;
+		default:
+			set_host_byte(scmnd, DID_TARGET_FAILURE);
+		}
+	}
+
 
 	/*
 	 * If the LUN is invalid; remove the device.
diff --git a/drivers/scsi/ufs/ufshcd.c b/drivers/scsi/ufs/ufshcd.c
index 52b96e8bf92..4e010b72781 100644
--- a/drivers/scsi/ufs/ufshcd.c
+++ b/drivers/scsi/ufs/ufshcd.c
@@ -1032,11 +1032,11 @@ static int ufshcd_initialize_hba(struct ufs_hba *hba)
 		return -EIO;
 
 	/* Configure UTRL and UTMRL base address registers */
-	writel(hba->utrdl_dma_addr,
-	       (hba->mmio_base + REG_UTP_TRANSFER_REQ_LIST_BASE_L));
 	writel(lower_32_bits(hba->utrdl_dma_addr),
+	       (hba->mmio_base + REG_UTP_TRANSFER_REQ_LIST_BASE_L));
+	writel(upper_32_bits(hba->utrdl_dma_addr),
 	       (hba->mmio_base + REG_UTP_TRANSFER_REQ_LIST_BASE_H));
-	writel(hba->utmrdl_dma_addr,
+	writel(lower_32_bits(hba->utmrdl_dma_addr),
 	       (hba->mmio_base + REG_UTP_TASK_REQ_LIST_BASE_L));
 	writel(upper_32_bits(hba->utmrdl_dma_addr),
 	       (hba->mmio_base + REG_UTP_TASK_REQ_LIST_BASE_H));
@@ -1160,7 +1160,7 @@ static int ufshcd_task_req_compl(struct ufs_hba *hba, u32 index)
 		task_result = be32_to_cpu(task_rsp_upiup->header.dword_1);
 		task_result = ((task_result & MASK_TASK_RESPONSE) >> 8);
 
-		if (task_result != UPIU_TASK_MANAGEMENT_FUNC_COMPL ||
+		if (task_result != UPIU_TASK_MANAGEMENT_FUNC_COMPL &&
 		    task_result != UPIU_TASK_MANAGEMENT_FUNC_SUCCEEDED)
 			task_result = FAILED;
 	} else {
diff --git a/drivers/scsi/virtio_scsi.c b/drivers/scsi/virtio_scsi.c
index efccd72c4a3..1b384311726 100644
--- a/drivers/scsi/virtio_scsi.c
+++ b/drivers/scsi/virtio_scsi.c
@@ -175,7 +175,8 @@ static void virtscsi_complete_free(void *buf)
 
 	if (cmd->comp)
 		complete_all(cmd->comp);
-	mempool_free(cmd, virtscsi_cmd_pool);
+	else
+		mempool_free(cmd, virtscsi_cmd_pool);
 }
 
 static void virtscsi_ctrl_done(struct virtqueue *vq)
@@ -311,21 +312,22 @@ out:
 static int virtscsi_tmf(struct virtio_scsi *vscsi, struct virtio_scsi_cmd *cmd)
 {
 	DECLARE_COMPLETION_ONSTACK(comp);
-	int ret;
+	int ret = FAILED;
 
 	cmd->comp = &comp;
-	ret = virtscsi_kick_cmd(vscsi, vscsi->ctrl_vq, cmd,
-			       sizeof cmd->req.tmf, sizeof cmd->resp.tmf,
-			       GFP_NOIO);
-	if (ret < 0)
-		return FAILED;
+	if (virtscsi_kick_cmd(vscsi, vscsi->ctrl_vq, cmd,
+			      sizeof cmd->req.tmf, sizeof cmd->resp.tmf,
+			      GFP_NOIO) < 0)
+		goto out;
 
 	wait_for_completion(&comp);
-	if (cmd->resp.tmf.response != VIRTIO_SCSI_S_OK &&
-	    cmd->resp.tmf.response != VIRTIO_SCSI_S_FUNCTION_SUCCEEDED)
-		return FAILED;
+	if (cmd->resp.tmf.response == VIRTIO_SCSI_S_OK ||
+	    cmd->resp.tmf.response == VIRTIO_SCSI_S_FUNCTION_SUCCEEDED)
+		ret = SUCCESS;
 
-	return SUCCESS;
+out:
+	mempool_free(cmd, virtscsi_cmd_pool);
+	return ret;
 }
 
 static int virtscsi_device_reset(struct scsi_cmnd *sc)
diff --git a/drivers/sh/clk/cpg.c b/drivers/sh/clk/cpg.c
index 91b6d52f74e..f0d015dd0fe 100644
--- a/drivers/sh/clk/cpg.c
+++ b/drivers/sh/clk/cpg.c
@@ -2,6 +2,7 @@
  * Helper routines for SuperH Clock Pulse Generator blocks (CPG).
  *
  *  Copyright (C) 2010  Magnus Damm
+ *  Copyright (C) 2010 - 2012  Paul Mundt
  *
  * 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
@@ -13,26 +14,44 @@
 #include <linux/io.h>
 #include <linux/sh_clk.h>
 
-static int sh_clk_mstp32_enable(struct clk *clk)
+static unsigned int sh_clk_read(struct clk *clk)
 {
-	iowrite32(ioread32(clk->mapped_reg) & ~(1 << clk->enable_bit),
-		  clk->mapped_reg);
+	if (clk->flags & CLK_ENABLE_REG_8BIT)
+		return ioread8(clk->mapped_reg);
+	else if (clk->flags & CLK_ENABLE_REG_16BIT)
+		return ioread16(clk->mapped_reg);
+
+	return ioread32(clk->mapped_reg);
+}
+
+static void sh_clk_write(int value, struct clk *clk)
+{
+	if (clk->flags & CLK_ENABLE_REG_8BIT)
+		iowrite8(value, clk->mapped_reg);
+	else if (clk->flags & CLK_ENABLE_REG_16BIT)
+		iowrite16(value, clk->mapped_reg);
+	else
+		iowrite32(value, clk->mapped_reg);
+}
+
+static int sh_clk_mstp_enable(struct clk *clk)
+{
+	sh_clk_write(sh_clk_read(clk) & ~(1 << clk->enable_bit), clk);
 	return 0;
 }
 
-static void sh_clk_mstp32_disable(struct clk *clk)
+static void sh_clk_mstp_disable(struct clk *clk)
 {
-	iowrite32(ioread32(clk->mapped_reg) | (1 << clk->enable_bit),
-		  clk->mapped_reg);
+	sh_clk_write(sh_clk_read(clk) | (1 << clk->enable_bit), clk);
 }
 
-static struct sh_clk_ops sh_clk_mstp32_clk_ops = {
-	.enable		= sh_clk_mstp32_enable,
-	.disable	= sh_clk_mstp32_disable,
+static struct sh_clk_ops sh_clk_mstp_clk_ops = {
+	.enable		= sh_clk_mstp_enable,
+	.disable	= sh_clk_mstp_disable,
 	.recalc		= followparent_recalc,
 };
 
-int __init sh_clk_mstp32_register(struct clk *clks, int nr)
+int __init sh_clk_mstp_register(struct clk *clks, int nr)
 {
 	struct clk *clkp;
 	int ret = 0;
@@ -40,7 +59,7 @@ int __init sh_clk_mstp32_register(struct clk *clks, int nr)
 
 	for (k = 0; !ret && (k < nr); k++) {
 		clkp = clks + k;
-		clkp->ops = &sh_clk_mstp32_clk_ops;
+		clkp->ops = &sh_clk_mstp_clk_ops;
 		ret |= clk_register(clkp);
 	}
 
@@ -72,7 +91,7 @@ static unsigned long sh_clk_div6_recalc(struct clk *clk)
 	clk_rate_table_build(clk, clk->freq_table, table->nr_divisors,
 			     table, NULL);
 
-	idx = ioread32(clk->mapped_reg) & 0x003f;
+	idx = sh_clk_read(clk) & 0x003f;
 
 	return clk->freq_table[idx].frequency;
 }
@@ -98,10 +117,10 @@ static int sh_clk_div6_set_parent(struct clk *clk, struct clk *parent)
 	if (ret < 0)
 		return ret;
 
-	value = ioread32(clk->mapped_reg) &
+	value = sh_clk_read(clk) &
 		~(((1 << clk->src_width) - 1) << clk->src_shift);
 
-	iowrite32(value | (i << clk->src_shift), clk->mapped_reg);
+	sh_clk_write(value | (i << clk->src_shift), clk);
 
 	/* Rebuild the frequency table */
 	clk_rate_table_build(clk, clk->freq_table, table->nr_divisors,
@@ -119,10 +138,10 @@ static int sh_clk_div6_set_rate(struct clk *clk, unsigned long rate)
 	if (idx < 0)
 		return idx;
 
-	value = ioread32(clk->mapped_reg);
+	value = sh_clk_read(clk);
 	value &= ~0x3f;
 	value |= idx;
-	iowrite32(value, clk->mapped_reg);
+	sh_clk_write(value, clk);
 	return 0;
 }
 
@@ -133,9 +152,9 @@ static int sh_clk_div6_enable(struct clk *clk)
 
 	ret = sh_clk_div6_set_rate(clk, clk->rate);
 	if (ret == 0) {
-		value = ioread32(clk->mapped_reg);
+		value = sh_clk_read(clk);
 		value &= ~0x100; /* clear stop bit to enable clock */
-		iowrite32(value, clk->mapped_reg);
+		sh_clk_write(value, clk);
 	}
 	return ret;
 }
@@ -144,10 +163,10 @@ static void sh_clk_div6_disable(struct clk *clk)
 {
 	unsigned long value;
 
-	value = ioread32(clk->mapped_reg);
+	value = sh_clk_read(clk);
 	value |= 0x100; /* stop clock */
 	value |= 0x3f; /* VDIV bits must be non-zero, overwrite divider */
-	iowrite32(value, clk->mapped_reg);
+	sh_clk_write(value, clk);
 }
 
 static struct sh_clk_ops sh_clk_div6_clk_ops = {
@@ -182,7 +201,7 @@ static int __init sh_clk_init_parent(struct clk *clk)
 		return -EINVAL;
 	}
 
-	val  = (ioread32(clk->mapped_reg) >> clk->src_shift);
+	val  = (sh_clk_read(clk) >> clk->src_shift);
 	val &= (1 << clk->src_width) - 1;
 
 	if (val >= clk->parent_num) {
@@ -252,7 +271,7 @@ static unsigned long sh_clk_div4_recalc(struct clk *clk)
 	clk_rate_table_build(clk, clk->freq_table, table->nr_divisors,
 			     table, &clk->arch_flags);
 
-	idx = (ioread32(clk->mapped_reg) >> clk->enable_bit) & 0x000f;
+	idx = (sh_clk_read(clk) >> clk->enable_bit) & 0x000f;
 
 	return clk->freq_table[idx].frequency;
 }
@@ -270,15 +289,15 @@ static int sh_clk_div4_set_parent(struct clk *clk, struct clk *parent)
 	 */
 
 	if (parent->flags & CLK_ENABLE_ON_INIT)
-		value = ioread32(clk->mapped_reg) & ~(1 << 7);
+		value = sh_clk_read(clk) & ~(1 << 7);
 	else
-		value = ioread32(clk->mapped_reg) | (1 << 7);
+		value = sh_clk_read(clk) | (1 << 7);
 
 	ret = clk_reparent(clk, parent);
 	if (ret < 0)
 		return ret;
 
-	iowrite32(value, clk->mapped_reg);
+	sh_clk_write(value, clk);
 
 	/* Rebiuld the frequency table */
 	clk_rate_table_build(clk, clk->freq_table, table->nr_divisors,
@@ -295,10 +314,10 @@ static int sh_clk_div4_set_rate(struct clk *clk, unsigned long rate)
 	if (idx < 0)
 		return idx;
 
-	value = ioread32(clk->mapped_reg);
+	value = sh_clk_read(clk);
 	value &= ~(0xf << clk->enable_bit);
 	value |= (idx << clk->enable_bit);
-	iowrite32(value, clk->mapped_reg);
+	sh_clk_write(value, clk);
 
 	if (d4t->kick)
 		d4t->kick(clk);
@@ -308,13 +327,13 @@ static int sh_clk_div4_set_rate(struct clk *clk, unsigned long rate)
 
 static int sh_clk_div4_enable(struct clk *clk)
 {
-	iowrite32(ioread32(clk->mapped_reg) & ~(1 << 8), clk->mapped_reg);
+	sh_clk_write(sh_clk_read(clk) & ~(1 << 8), clk);
 	return 0;
 }
 
 static void sh_clk_div4_disable(struct clk *clk)
 {
-	iowrite32(ioread32(clk->mapped_reg) | (1 << 8), clk->mapped_reg);
+	sh_clk_write(sh_clk_read(clk) | (1 << 8), clk);
 }
 
 static struct sh_clk_ops sh_clk_div4_clk_ops = {
diff --git a/drivers/sh/intc/dynamic.c b/drivers/sh/intc/dynamic.c
index 5fea1ee8799..14eb01ef5d7 100644
--- a/drivers/sh/intc/dynamic.c
+++ b/drivers/sh/intc/dynamic.c
@@ -55,11 +55,3 @@ void destroy_irq(unsigned int irq)
 {
 	irq_free_desc(irq);
 }
-
-void reserve_intc_vectors(struct intc_vect *vectors, unsigned int nr_vecs)
-{
-	int i;
-
-	for (i = 0; i < nr_vecs; i++)
-		irq_reserve_irq(evt2irq(vectors[i].vect));
-}
diff --git a/drivers/spi/Kconfig b/drivers/spi/Kconfig
index 00c024039c9..cd2fe350e72 100644
--- a/drivers/spi/Kconfig
+++ b/drivers/spi/Kconfig
@@ -311,7 +311,7 @@ config SPI_S3C24XX_FIQ
 
 config SPI_S3C64XX
 	tristate "Samsung S3C64XX series type SPI"
-	depends on (ARCH_S3C64XX || ARCH_S5P64X0 || ARCH_EXYNOS)
+	depends on (ARCH_S3C24XX || ARCH_S3C64XX || ARCH_S5P64X0 || ARCH_EXYNOS)
 	select S3C64XX_DMA if ARCH_S3C64XX
 	help
 	  SPI driver for Samsung S3C64XX and newer SoCs.
diff --git a/drivers/spi/spi-ath79.c b/drivers/spi/spi-ath79.c
index acc88b4d286..249077e5cc4 100644
--- a/drivers/spi/spi-ath79.c
+++ b/drivers/spi/spi-ath79.c
@@ -216,9 +216,6 @@ static __devinit int ath79_spi_probe(struct platform_device *pdev)
 	if (pdata) {
 		master->bus_num = pdata->bus_num;
 		master->num_chipselect = pdata->num_chipselect;
-	} else {
-		master->bus_num = -1;
-		master->num_chipselect = 1;
 	}
 
 	sp->bitbang.master = spi_master_get(master);
diff --git a/drivers/spi/spi-coldfire-qspi.c b/drivers/spi/spi-coldfire-qspi.c
index 6eee64a5d24..b2d4b9e4e01 100644
--- a/drivers/spi/spi-coldfire-qspi.c
+++ b/drivers/spi/spi-coldfire-qspi.c
@@ -25,12 +25,12 @@
 #include <linux/errno.h>
 #include <linux/platform_device.h>
 #include <linux/sched.h>
-#include <linux/workqueue.h>
 #include <linux/delay.h>
 #include <linux/io.h>
 #include <linux/clk.h>
 #include <linux/err.h>
 #include <linux/spi/spi.h>
+#include <linux/pm_runtime.h>
 
 #include <asm/coldfire.h>
 #include <asm/mcfsim.h>
@@ -78,10 +78,7 @@ struct mcfqspi {
 
 	wait_queue_head_t waitq;
 
-	struct work_struct work;
-	struct workqueue_struct *workq;
-	spinlock_t lock;
-	struct list_head msgq;
+	struct device *dev;
 };
 
 static void mcfqspi_wr_qmr(struct mcfqspi *mcfqspi, u16 val)
@@ -303,120 +300,80 @@ static void mcfqspi_transfer_msg16(struct mcfqspi *mcfqspi, unsigned count,
 	}
 }
 
-static void mcfqspi_work(struct work_struct *work)
+static int mcfqspi_transfer_one_message(struct spi_master *master,
+					 struct spi_message *msg)
 {
-	struct mcfqspi *mcfqspi = container_of(work, struct mcfqspi, work);
-	unsigned long flags;
-
-	spin_lock_irqsave(&mcfqspi->lock, flags);
-	while (!list_empty(&mcfqspi->msgq)) {
-		struct spi_message *msg;
-		struct spi_device *spi;
-		struct spi_transfer *xfer;
-		int status = 0;
-
-		msg = container_of(mcfqspi->msgq.next, struct spi_message,
-				   queue);
-
-		list_del_init(&msg->queue);
-		spin_unlock_irqrestore(&mcfqspi->lock, flags);
-
-		spi = msg->spi;
-
-		list_for_each_entry(xfer, &msg->transfers, transfer_list) {
-			bool cs_high = spi->mode & SPI_CS_HIGH;
-			u16 qmr = MCFQSPI_QMR_MSTR;
-
-			if (xfer->bits_per_word)
-				qmr |= xfer->bits_per_word << 10;
-			else
-				qmr |= spi->bits_per_word << 10;
-			if (spi->mode & SPI_CPHA)
-				qmr |= MCFQSPI_QMR_CPHA;
-			if (spi->mode & SPI_CPOL)
-				qmr |= MCFQSPI_QMR_CPOL;
-			if (xfer->speed_hz)
-				qmr |= mcfqspi_qmr_baud(xfer->speed_hz);
-			else
-				qmr |= mcfqspi_qmr_baud(spi->max_speed_hz);
-			mcfqspi_wr_qmr(mcfqspi, qmr);
-
-			mcfqspi_cs_select(mcfqspi, spi->chip_select, cs_high);
-
-			mcfqspi_wr_qir(mcfqspi, MCFQSPI_QIR_SPIFE);
-			if ((xfer->bits_per_word ? xfer->bits_per_word :
-						spi->bits_per_word) == 8)
-				mcfqspi_transfer_msg8(mcfqspi, xfer->len,
-						      xfer->tx_buf,
-						      xfer->rx_buf);
-			else
-				mcfqspi_transfer_msg16(mcfqspi, xfer->len / 2,
-						       xfer->tx_buf,
-						       xfer->rx_buf);
-			mcfqspi_wr_qir(mcfqspi, 0);
-
-			if (xfer->delay_usecs)
-				udelay(xfer->delay_usecs);
-			if (xfer->cs_change) {
-				if (!list_is_last(&xfer->transfer_list,
-						  &msg->transfers))
-					mcfqspi_cs_deselect(mcfqspi,
-							    spi->chip_select,
-							    cs_high);
-			} else {
-				if (list_is_last(&xfer->transfer_list,
-						 &msg->transfers))
-					mcfqspi_cs_deselect(mcfqspi,
-							    spi->chip_select,
-							    cs_high);
-			}
-			msg->actual_length += xfer->len;
+	struct mcfqspi *mcfqspi = spi_master_get_devdata(master);
+	struct spi_device *spi = msg->spi;
+	struct spi_transfer *t;
+	int status = 0;
+
+	list_for_each_entry(t, &msg->transfers, transfer_list) {
+		bool cs_high = spi->mode & SPI_CS_HIGH;
+		u16 qmr = MCFQSPI_QMR_MSTR;
+
+		if (t->bits_per_word)
+			qmr |= t->bits_per_word << 10;
+		else
+			qmr |= spi->bits_per_word << 10;
+		if (spi->mode & SPI_CPHA)
+			qmr |= MCFQSPI_QMR_CPHA;
+		if (spi->mode & SPI_CPOL)
+			qmr |= MCFQSPI_QMR_CPOL;
+		if (t->speed_hz)
+			qmr |= mcfqspi_qmr_baud(t->speed_hz);
+		else
+			qmr |= mcfqspi_qmr_baud(spi->max_speed_hz);
+		mcfqspi_wr_qmr(mcfqspi, qmr);
+
+		mcfqspi_cs_select(mcfqspi, spi->chip_select, cs_high);
+
+		mcfqspi_wr_qir(mcfqspi, MCFQSPI_QIR_SPIFE);
+		if ((t->bits_per_word ? t->bits_per_word :
+					spi->bits_per_word) == 8)
+			mcfqspi_transfer_msg8(mcfqspi, t->len, t->tx_buf,
+					t->rx_buf);
+		else
+			mcfqspi_transfer_msg16(mcfqspi, t->len / 2, t->tx_buf,
+					t->rx_buf);
+		mcfqspi_wr_qir(mcfqspi, 0);
+
+		if (t->delay_usecs)
+			udelay(t->delay_usecs);
+		if (t->cs_change) {
+			if (!list_is_last(&t->transfer_list, &msg->transfers))
+				mcfqspi_cs_deselect(mcfqspi, spi->chip_select,
+						cs_high);
+		} else {
+			if (list_is_last(&t->transfer_list, &msg->transfers))
+				mcfqspi_cs_deselect(mcfqspi, spi->chip_select,
+						cs_high);
 		}
-		msg->status = status;
-		msg->complete(msg->context);
-
-		spin_lock_irqsave(&mcfqspi->lock, flags);
+		msg->actual_length += t->len;
 	}
-	spin_unlock_irqrestore(&mcfqspi->lock, flags);
+	msg->status = status;
+	spi_finalize_current_message(master);
+
+	return status;
+
 }
 
-static int mcfqspi_transfer(struct spi_device *spi, struct spi_message *msg)
+static int mcfqspi_prepare_transfer_hw(struct spi_master *master)
 {
-	struct mcfqspi *mcfqspi;
-	struct spi_transfer *xfer;
-	unsigned long flags;
-
-	mcfqspi = spi_master_get_devdata(spi->master);
-
-	list_for_each_entry(xfer, &msg->transfers, transfer_list) {
-		if (xfer->bits_per_word && ((xfer->bits_per_word < 8)
-					|| (xfer->bits_per_word > 16))) {
-			dev_dbg(&spi->dev,
-				"%d bits per word is not supported\n",
-				xfer->bits_per_word);
-			goto fail;
-		}
-		if (xfer->speed_hz) {
-			u32 real_speed = MCFQSPI_BUSCLK /
-				mcfqspi_qmr_baud(xfer->speed_hz);
-			if (real_speed != xfer->speed_hz)
-				dev_dbg(&spi->dev,
-					"using speed %d instead of %d\n",
-					real_speed, xfer->speed_hz);
-		}
-	}
-	msg->status = -EINPROGRESS;
-	msg->actual_length = 0;
+	struct mcfqspi *mcfqspi = spi_master_get_devdata(master);
 
-	spin_lock_irqsave(&mcfqspi->lock, flags);
-	list_add_tail(&msg->queue, &mcfqspi->msgq);
-	queue_work(mcfqspi->workq, &mcfqspi->work);
-	spin_unlock_irqrestore(&mcfqspi->lock, flags);
+	pm_runtime_get_sync(mcfqspi->dev);
+
+	return 0;
+}
+
+static int mcfqspi_unprepare_transfer_hw(struct spi_master *master)
+{
+	struct mcfqspi *mcfqspi = spi_master_get_devdata(master);
+
+	pm_runtime_put_sync(mcfqspi->dev);
 
 	return 0;
-fail:
-	msg->status = -EINVAL;
-	return -EINVAL;
 }
 
 static int mcfqspi_setup(struct spi_device *spi)
@@ -502,21 +459,10 @@ static int __devinit mcfqspi_probe(struct platform_device *pdev)
 	}
 	clk_enable(mcfqspi->clk);
 
-	mcfqspi->workq = create_singlethread_workqueue(dev_name(master->dev.parent));
-	if (!mcfqspi->workq) {
-		dev_dbg(&pdev->dev, "create_workqueue failed\n");
-		status = -ENOMEM;
-		goto fail4;
-	}
-	INIT_WORK(&mcfqspi->work, mcfqspi_work);
-	spin_lock_init(&mcfqspi->lock);
-	INIT_LIST_HEAD(&mcfqspi->msgq);
-	init_waitqueue_head(&mcfqspi->waitq);
-
 	pdata = pdev->dev.platform_data;
 	if (!pdata) {
 		dev_dbg(&pdev->dev, "platform data is missing\n");
-		goto fail5;
+		goto fail4;
 	}
 	master->bus_num = pdata->bus_num;
 	master->num_chipselect = pdata->num_chipselect;
@@ -525,28 +471,33 @@ static int __devinit mcfqspi_probe(struct platform_device *pdev)
 	status = mcfqspi_cs_setup(mcfqspi);
 	if (status) {
 		dev_dbg(&pdev->dev, "error initializing cs_control\n");
-		goto fail5;
+		goto fail4;
 	}
 
+	init_waitqueue_head(&mcfqspi->waitq);
+	mcfqspi->dev = &pdev->dev;
+
 	master->mode_bits = SPI_CS_HIGH | SPI_CPOL | SPI_CPHA;
 	master->setup = mcfqspi_setup;
-	master->transfer = mcfqspi_transfer;
+	master->transfer_one_message = mcfqspi_transfer_one_message;
+	master->prepare_transfer_hardware = mcfqspi_prepare_transfer_hw;
+	master->unprepare_transfer_hardware = mcfqspi_unprepare_transfer_hw;
 
 	platform_set_drvdata(pdev, master);
 
 	status = spi_register_master(master);
 	if (status) {
 		dev_dbg(&pdev->dev, "spi_register_master failed\n");
-		goto fail6;
+		goto fail5;
 	}
+	pm_runtime_enable(mcfqspi->dev);
+
 	dev_info(&pdev->dev, "Coldfire QSPI bus driver\n");
 
 	return 0;
 
-fail6:
-	mcfqspi_cs_teardown(mcfqspi);
 fail5:
-	destroy_workqueue(mcfqspi->workq);
+	mcfqspi_cs_teardown(mcfqspi);
 fail4:
 	clk_disable(mcfqspi->clk);
 	clk_put(mcfqspi->clk);
@@ -570,12 +521,12 @@ static int __devexit mcfqspi_remove(struct platform_device *pdev)
 	struct mcfqspi *mcfqspi = spi_master_get_devdata(master);
 	struct resource *res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
 
+	pm_runtime_disable(mcfqspi->dev);
 	/* disable the hardware (set the baud rate to 0) */
 	mcfqspi_wr_qmr(mcfqspi, MCFQSPI_QMR_MSTR);
 
 	platform_set_drvdata(pdev, NULL);
 	mcfqspi_cs_teardown(mcfqspi);
-	destroy_workqueue(mcfqspi->workq);
 	clk_disable(mcfqspi->clk);
 	clk_put(mcfqspi->clk);
 	free_irq(mcfqspi->irq, mcfqspi);
@@ -587,11 +538,13 @@ static int __devexit mcfqspi_remove(struct platform_device *pdev)
 	return 0;
 }
 
-#ifdef CONFIG_PM
-
+#ifdef CONFIG_PM_SLEEP
 static int mcfqspi_suspend(struct device *dev)
 {
-	struct mcfqspi *mcfqspi = platform_get_drvdata(to_platform_device(dev));
+	struct spi_master *master = spi_master_get(dev_get_drvdata(dev));
+	struct mcfqspi *mcfqspi = spi_master_get_devdata(master);
+
+	spi_master_suspend(master);
 
 	clk_disable(mcfqspi->clk);
 
@@ -600,27 +553,47 @@ static int mcfqspi_suspend(struct device *dev)
 
 static int mcfqspi_resume(struct device *dev)
 {
-	struct mcfqspi *mcfqspi = platform_get_drvdata(to_platform_device(dev));
+	struct spi_master *master = spi_master_get(dev_get_drvdata(dev));
+	struct mcfqspi *mcfqspi = spi_master_get_devdata(master);
+
+	spi_master_resume(master);
 
 	clk_enable(mcfqspi->clk);
 
 	return 0;
 }
+#endif
 
-static struct dev_pm_ops mcfqspi_dev_pm_ops = {
-	.suspend	= mcfqspi_suspend,
-	.resume		= mcfqspi_resume,
-};
+#ifdef CONFIG_PM_RUNTIME
+static int mcfqspi_runtime_suspend(struct device *dev)
+{
+	struct mcfqspi *mcfqspi = platform_get_drvdata(to_platform_device(dev));
 
-#define	MCFQSPI_DEV_PM_OPS	(&mcfqspi_dev_pm_ops)
-#else
-#define	MCFQSPI_DEV_PM_OPS	NULL
+	clk_disable(mcfqspi->clk);
+
+	return 0;
+}
+
+static int mcfqspi_runtime_resume(struct device *dev)
+{
+	struct mcfqspi *mcfqspi = platform_get_drvdata(to_platform_device(dev));
+
+	clk_enable(mcfqspi->clk);
+
+	return 0;
+}
 #endif
 
+static const struct dev_pm_ops mcfqspi_pm = {
+	SET_SYSTEM_SLEEP_PM_OPS(mcfqspi_suspend, mcfqspi_resume)
+	SET_RUNTIME_PM_OPS(mcfqspi_runtime_suspend, mcfqspi_runtime_resume,
+			NULL)
+};
+
 static struct platform_driver mcfqspi_driver = {
 	.driver.name	= DRIVER_NAME,
 	.driver.owner	= THIS_MODULE,
-	.driver.pm	= MCFQSPI_DEV_PM_OPS,
+	.driver.pm	= &mcfqspi_pm,
 	.probe		= mcfqspi_probe,
 	.remove		= __devexit_p(mcfqspi_remove),
 };
diff --git a/drivers/spi/spi-dw-pci.c b/drivers/spi/spi-dw-pci.c
index 14f7cc9523f..ff81abbb306 100644
--- a/drivers/spi/spi-dw-pci.c
+++ b/drivers/spi/spi-dw-pci.c
@@ -164,18 +164,7 @@ static struct pci_driver dw_spi_driver = {
 	.resume	=	spi_resume,
 };
 
-static int __init mrst_spi_init(void)
-{
-	return pci_register_driver(&dw_spi_driver);
-}
-
-static void __exit mrst_spi_exit(void)
-{
-	pci_unregister_driver(&dw_spi_driver);
-}
-
-module_init(mrst_spi_init);
-module_exit(mrst_spi_exit);
+module_pci_driver(dw_spi_driver);
 
 MODULE_AUTHOR("Feng Tang <feng.tang@intel.com>");
 MODULE_DESCRIPTION("PCI interface driver for DW SPI Core");
diff --git a/drivers/spi/spi-ep93xx.c b/drivers/spi/spi-ep93xx.c
index e8055073e84..f97f1d24880 100644
--- a/drivers/spi/spi-ep93xx.c
+++ b/drivers/spi/spi-ep93xx.c
@@ -76,7 +76,6 @@
  * @clk: clock for the controller
  * @regs_base: pointer to ioremap()'d registers
  * @sspdr_phys: physical address of the SSPDR register
- * @irq: IRQ number used by the driver
  * @min_rate: minimum clock rate (in Hz) supported by the controller
  * @max_rate: maximum clock rate (in Hz) supported by the controller
  * @running: is the queue running
@@ -114,7 +113,6 @@ struct ep93xx_spi {
 	struct clk			*clk;
 	void __iomem			*regs_base;
 	unsigned long			sspdr_phys;
-	int				irq;
 	unsigned long			min_rate;
 	unsigned long			max_rate;
 	bool				running;
@@ -1031,6 +1029,7 @@ static int __devinit ep93xx_spi_probe(struct platform_device *pdev)
 	struct ep93xx_spi_info *info;
 	struct ep93xx_spi *espi;
 	struct resource *res;
+	int irq;
 	int error;
 
 	info = pdev->dev.platform_data;
@@ -1070,8 +1069,8 @@ static int __devinit ep93xx_spi_probe(struct platform_device *pdev)
 	espi->min_rate = clk_get_rate(espi->clk) / (254 * 256);
 	espi->pdev = pdev;
 
-	espi->irq = platform_get_irq(pdev, 0);
-	if (espi->irq < 0) {
+	irq = platform_get_irq(pdev, 0);
+	if (irq < 0) {
 		error = -EBUSY;
 		dev_err(&pdev->dev, "failed to get irq resources\n");
 		goto fail_put_clock;
@@ -1084,26 +1083,20 @@ static int __devinit ep93xx_spi_probe(struct platform_device *pdev)
 		goto fail_put_clock;
 	}
 
-	res = request_mem_region(res->start, resource_size(res), pdev->name);
-	if (!res) {
-		dev_err(&pdev->dev, "unable to request iomem resources\n");
-		error = -EBUSY;
-		goto fail_put_clock;
-	}
-
 	espi->sspdr_phys = res->start + SSPDR;
-	espi->regs_base = ioremap(res->start, resource_size(res));
+
+	espi->regs_base = devm_request_and_ioremap(&pdev->dev, res);
 	if (!espi->regs_base) {
 		dev_err(&pdev->dev, "failed to map resources\n");
 		error = -ENODEV;
-		goto fail_free_mem;
+		goto fail_put_clock;
 	}
 
-	error = request_irq(espi->irq, ep93xx_spi_interrupt, 0,
-			    "ep93xx-spi", espi);
+	error = devm_request_irq(&pdev->dev, irq, ep93xx_spi_interrupt,
+				0, "ep93xx-spi", espi);
 	if (error) {
 		dev_err(&pdev->dev, "failed to request irq\n");
-		goto fail_unmap_regs;
+		goto fail_put_clock;
 	}
 
 	if (info->use_dma && ep93xx_spi_setup_dma(espi))
@@ -1128,7 +1121,7 @@ static int __devinit ep93xx_spi_probe(struct platform_device *pdev)
 	}
 
 	dev_info(&pdev->dev, "EP93xx SPI Controller at 0x%08lx irq %d\n",
-		 (unsigned long)res->start, espi->irq);
+		 (unsigned long)res->start, irq);
 
 	return 0;
 
@@ -1136,11 +1129,6 @@ fail_free_queue:
 	destroy_workqueue(espi->wq);
 fail_free_dma:
 	ep93xx_spi_release_dma(espi);
-	free_irq(espi->irq, espi);
-fail_unmap_regs:
-	iounmap(espi->regs_base);
-fail_free_mem:
-	release_mem_region(res->start, resource_size(res));
 fail_put_clock:
 	clk_put(espi->clk);
 fail_release_master:
@@ -1154,7 +1142,6 @@ static int __devexit ep93xx_spi_remove(struct platform_device *pdev)
 {
 	struct spi_master *master = platform_get_drvdata(pdev);
 	struct ep93xx_spi *espi = spi_master_get_devdata(master);
-	struct resource *res;
 
 	spin_lock_irq(&espi->lock);
 	espi->running = false;
@@ -1180,10 +1167,6 @@ static int __devexit ep93xx_spi_remove(struct platform_device *pdev)
 	spin_unlock_irq(&espi->lock);
 
 	ep93xx_spi_release_dma(espi);
-	free_irq(espi->irq, espi);
-	iounmap(espi->regs_base);
-	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
-	release_mem_region(res->start, resource_size(res));
 	clk_put(espi->clk);
 	platform_set_drvdata(pdev, NULL);
 
diff --git a/drivers/spi/spi-fsl-espi.c b/drivers/spi/spi-fsl-espi.c
index 7523a2429d0..27bdc47b525 100644
--- a/drivers/spi/spi-fsl-espi.c
+++ b/drivers/spi/spi-fsl-espi.c
@@ -17,7 +17,6 @@
 #include <linux/mm.h>
 #include <linux/of.h>
 #include <linux/of_platform.h>
-#include <linux/of_spi.h>
 #include <linux/interrupt.h>
 #include <linux/err.h>
 #include <sysdev/fsl_soc.h>
diff --git a/drivers/spi/spi-fsl-lib.c b/drivers/spi/spi-fsl-lib.c
index 2674fad7f68..1503574b215 100644
--- a/drivers/spi/spi-fsl-lib.c
+++ b/drivers/spi/spi-fsl-lib.c
@@ -22,7 +22,7 @@
 #include <linux/dma-mapping.h>
 #include <linux/mm.h>
 #include <linux/of_platform.h>
-#include <linux/of_spi.h>
+#include <linux/spi/spi.h>
 #include <sysdev/fsl_soc.h>
 
 #include "spi-fsl-lib.h"
diff --git a/drivers/spi/spi-fsl-spi.c b/drivers/spi/spi-fsl-spi.c
index 5f748c0d96b..6a62934ca74 100644
--- a/drivers/spi/spi-fsl-spi.c
+++ b/drivers/spi/spi-fsl-spi.c
@@ -933,7 +933,7 @@ err:
 
 static void fsl_spi_cs_control(struct spi_device *spi, bool on)
 {
-	struct device *dev = spi->dev.parent;
+	struct device *dev = spi->dev.parent->parent;
 	struct mpc8xxx_spi_probe_info *pinfo = to_of_pinfo(dev->platform_data);
 	u16 cs = spi->chip_select;
 	int gpio = pinfo->gpios[cs];
diff --git a/drivers/spi/spi-imx.c b/drivers/spi/spi-imx.c
index 570f22053be..47877d68761 100644
--- a/drivers/spi/spi-imx.c
+++ b/drivers/spi/spi-imx.c
@@ -37,6 +37,7 @@
 #include <linux/of.h>
 #include <linux/of_device.h>
 #include <linux/of_gpio.h>
+#include <linux/pinctrl/consumer.h>
 
 #include <mach/spi.h>
 
@@ -85,7 +86,8 @@ struct spi_imx_data {
 	struct completion xfer_done;
 	void __iomem *base;
 	int irq;
-	struct clk *clk;
+	struct clk *clk_per;
+	struct clk *clk_ipg;
 	unsigned long spi_clk;
 
 	unsigned int count;
@@ -758,6 +760,7 @@ static int __devinit spi_imx_probe(struct platform_device *pdev)
 	struct spi_master *master;
 	struct spi_imx_data *spi_imx;
 	struct resource *res;
+	struct pinctrl *pinctrl;
 	int i, ret, num_cs;
 
 	if (!np && !mxc_platform_info) {
@@ -845,15 +848,28 @@ static int __devinit spi_imx_probe(struct platform_device *pdev)
 		goto out_iounmap;
 	}
 
-	spi_imx->clk = clk_get(&pdev->dev, NULL);
-	if (IS_ERR(spi_imx->clk)) {
-		dev_err(&pdev->dev, "unable to get clock\n");
-		ret = PTR_ERR(spi_imx->clk);
+	pinctrl = devm_pinctrl_get_select_default(&pdev->dev);
+	if (IS_ERR(pinctrl)) {
+		ret = PTR_ERR(pinctrl);
 		goto out_free_irq;
 	}
 
-	clk_enable(spi_imx->clk);
-	spi_imx->spi_clk = clk_get_rate(spi_imx->clk);
+	spi_imx->clk_ipg = devm_clk_get(&pdev->dev, "ipg");
+	if (IS_ERR(spi_imx->clk_ipg)) {
+		ret = PTR_ERR(spi_imx->clk_ipg);
+		goto out_free_irq;
+	}
+
+	spi_imx->clk_per = devm_clk_get(&pdev->dev, "per");
+	if (IS_ERR(spi_imx->clk_per)) {
+		ret = PTR_ERR(spi_imx->clk_per);
+		goto out_free_irq;
+	}
+
+	clk_prepare_enable(spi_imx->clk_per);
+	clk_prepare_enable(spi_imx->clk_ipg);
+
+	spi_imx->spi_clk = clk_get_rate(spi_imx->clk_per);
 
 	spi_imx->devtype_data->reset(spi_imx);
 
@@ -871,8 +887,8 @@ static int __devinit spi_imx_probe(struct platform_device *pdev)
 	return ret;
 
 out_clk_put:
-	clk_disable(spi_imx->clk);
-	clk_put(spi_imx->clk);
+	clk_disable_unprepare(spi_imx->clk_per);
+	clk_disable_unprepare(spi_imx->clk_ipg);
 out_free_irq:
 	free_irq(spi_imx->irq, spi_imx);
 out_iounmap:
@@ -900,8 +916,8 @@ static int __devexit spi_imx_remove(struct platform_device *pdev)
 	spi_bitbang_stop(&spi_imx->bitbang);
 
 	writel(0, spi_imx->base + MXC_CSPICTRL);
-	clk_disable(spi_imx->clk);
-	clk_put(spi_imx->clk);
+	clk_disable_unprepare(spi_imx->clk_per);
+	clk_disable_unprepare(spi_imx->clk_ipg);
 	free_irq(spi_imx->irq, spi_imx);
 	iounmap(spi_imx->base);
 
diff --git a/drivers/spi/spi-lm70llp.c b/drivers/spi/spi-lm70llp.c
index 933eb9d9ddd..0759b5db988 100644
--- a/drivers/spi/spi-lm70llp.c
+++ b/drivers/spi/spi-lm70llp.c
@@ -219,9 +219,6 @@ static void spi_lm70llp_attach(struct parport *p)
 	}
 	pp = spi_master_get_devdata(master);
 
-	master->bus_num = -1;	/* dynamic alloc of a bus number */
-	master->num_chipselect = 1;
-
 	/*
 	 * SPI and bitbang hookup.
 	 */
diff --git a/drivers/spi/spi-mpc52xx.c b/drivers/spi/spi-mpc52xx.c
index 57633d96345..cb3a3830b0a 100644
--- a/drivers/spi/spi-mpc52xx.c
+++ b/drivers/spi/spi-mpc52xx.c
@@ -433,7 +433,6 @@ static int __devinit mpc52xx_spi_probe(struct platform_device *op)
 		goto err_alloc;
 	}
 
-	master->bus_num = -1;
 	master->setup = mpc52xx_spi_setup;
 	master->transfer = mpc52xx_spi_transfer;
 	master->mode_bits = SPI_CPOL | SPI_CPHA | SPI_LSB_FIRST;
@@ -479,8 +478,6 @@ static int __devinit mpc52xx_spi_probe(struct platform_device *op)
 			gpio_direction_output(gpio_cs, 1);
 			ms->gpio_cs[i] = gpio_cs;
 		}
-	} else {
-		master->num_chipselect = 1;
 	}
 
 	spin_lock_init(&ms->lock);
diff --git a/drivers/spi/spi-omap2-mcspi.c b/drivers/spi/spi-omap2-mcspi.c
index bb9274c2526..46ef5fe51db 100644
--- a/drivers/spi/spi-omap2-mcspi.c
+++ b/drivers/spi/spi-omap2-mcspi.c
@@ -44,9 +44,7 @@
 #include <plat/mcspi.h>
 
 #define OMAP2_MCSPI_MAX_FREQ		48000000
-
-/* OMAP2 has 3 SPI controllers, while OMAP3 has 4 */
-#define OMAP2_MCSPI_MAX_CTRL 		4
+#define SPI_AUTOSUSPEND_TIMEOUT		2000
 
 #define OMAP2_MCSPI_REVISION		0x00
 #define OMAP2_MCSPI_SYSSTATUS		0x14
@@ -111,19 +109,25 @@ struct omap2_mcspi_dma {
 #define DMA_MIN_BYTES			160
 
 
+/*
+ * Used for context save and restore, structure members to be updated whenever
+ * corresponding registers are modified.
+ */
+struct omap2_mcspi_regs {
+	u32 modulctrl;
+	u32 wakeupenable;
+	struct list_head cs;
+};
+
 struct omap2_mcspi {
-	struct work_struct	work;
-	/* lock protects queue and registers */
-	spinlock_t		lock;
-	struct list_head	msg_queue;
 	struct spi_master	*master;
 	/* Virtual base address of the controller */
 	void __iomem		*base;
 	unsigned long		phys;
 	/* SPI1 has 4 channels, while SPI2 has 2 */
 	struct omap2_mcspi_dma	*dma_channels;
-	struct  device		*dev;
-	struct workqueue_struct *wq;
+	struct device		*dev;
+	struct omap2_mcspi_regs ctx;
 };
 
 struct omap2_mcspi_cs {
@@ -135,17 +139,6 @@ struct omap2_mcspi_cs {
 	u32			chconf0;
 };
 
-/* used for context save and restore, structure members to be updated whenever
- * corresponding registers are modified.
- */
-struct omap2_mcspi_regs {
-	u32 modulctrl;
-	u32 wakeupenable;
-	struct list_head cs;
-};
-
-static struct omap2_mcspi_regs omap2_mcspi_ctx[OMAP2_MCSPI_MAX_CTRL];
-
 #define MOD_REG_BIT(val, mask, set) do { \
 	if (set) \
 		val |= mask; \
@@ -236,9 +229,12 @@ static void omap2_mcspi_force_cs(struct spi_device *spi, int cs_active)
 
 static void omap2_mcspi_set_master_mode(struct spi_master *master)
 {
+	struct omap2_mcspi	*mcspi = spi_master_get_devdata(master);
+	struct omap2_mcspi_regs	*ctx = &mcspi->ctx;
 	u32 l;
 
-	/* setup when switching from (reset default) slave mode
+	/*
+	 * Setup when switching from (reset default) slave mode
 	 * to single-channel master mode
 	 */
 	l = mcspi_read_reg(master, OMAP2_MCSPI_MODULCTRL);
@@ -247,29 +243,26 @@ static void omap2_mcspi_set_master_mode(struct spi_master *master)
 	MOD_REG_BIT(l, OMAP2_MCSPI_MODULCTRL_SINGLE, 1);
 	mcspi_write_reg(master, OMAP2_MCSPI_MODULCTRL, l);
 
-	omap2_mcspi_ctx[master->bus_num - 1].modulctrl = l;
+	ctx->modulctrl = l;
 }
 
 static void omap2_mcspi_restore_ctx(struct omap2_mcspi *mcspi)
 {
-	struct spi_master *spi_cntrl;
-	struct omap2_mcspi_cs *cs;
-	spi_cntrl = mcspi->master;
+	struct spi_master	*spi_cntrl = mcspi->master;
+	struct omap2_mcspi_regs	*ctx = &mcspi->ctx;
+	struct omap2_mcspi_cs	*cs;
 
 	/* McSPI: context restore */
-	mcspi_write_reg(spi_cntrl, OMAP2_MCSPI_MODULCTRL,
-			omap2_mcspi_ctx[spi_cntrl->bus_num - 1].modulctrl);
-
-	mcspi_write_reg(spi_cntrl, OMAP2_MCSPI_WAKEUPENABLE,
-			omap2_mcspi_ctx[spi_cntrl->bus_num - 1].wakeupenable);
+	mcspi_write_reg(spi_cntrl, OMAP2_MCSPI_MODULCTRL, ctx->modulctrl);
+	mcspi_write_reg(spi_cntrl, OMAP2_MCSPI_WAKEUPENABLE, ctx->wakeupenable);
 
-	list_for_each_entry(cs, &omap2_mcspi_ctx[spi_cntrl->bus_num - 1].cs,
-			node)
+	list_for_each_entry(cs, &ctx->cs, node)
 		__raw_writel(cs->chconf0, cs->base + OMAP2_MCSPI_CHCONF0);
 }
 static void omap2_mcspi_disable_clocks(struct omap2_mcspi *mcspi)
 {
-	pm_runtime_put_sync(mcspi->dev);
+	pm_runtime_mark_last_busy(mcspi->dev);
+	pm_runtime_put_autosuspend(mcspi->dev);
 }
 
 static int omap2_mcspi_enable_clocks(struct omap2_mcspi *mcspi)
@@ -277,6 +270,23 @@ static int omap2_mcspi_enable_clocks(struct omap2_mcspi *mcspi)
 	return pm_runtime_get_sync(mcspi->dev);
 }
 
+static int omap2_prepare_transfer(struct spi_master *master)
+{
+	struct omap2_mcspi *mcspi = spi_master_get_devdata(master);
+
+	pm_runtime_get_sync(mcspi->dev);
+	return 0;
+}
+
+static int omap2_unprepare_transfer(struct spi_master *master)
+{
+	struct omap2_mcspi *mcspi = spi_master_get_devdata(master);
+
+	pm_runtime_mark_last_busy(mcspi->dev);
+	pm_runtime_put_autosuspend(mcspi->dev);
+	return 0;
+}
+
 static int mcspi_wait_for_reg_bit(void __iomem *reg, unsigned long bit)
 {
 	unsigned long timeout;
@@ -777,7 +787,8 @@ static int omap2_mcspi_request_dma(struct spi_device *spi)
 static int omap2_mcspi_setup(struct spi_device *spi)
 {
 	int			ret;
-	struct omap2_mcspi	*mcspi;
+	struct omap2_mcspi	*mcspi = spi_master_get_devdata(spi->master);
+	struct omap2_mcspi_regs	*ctx = &mcspi->ctx;
 	struct omap2_mcspi_dma	*mcspi_dma;
 	struct omap2_mcspi_cs	*cs = spi->controller_state;
 
@@ -787,11 +798,10 @@ static int omap2_mcspi_setup(struct spi_device *spi)
 		return -EINVAL;
 	}
 
-	mcspi = spi_master_get_devdata(spi->master);
 	mcspi_dma = &mcspi->dma_channels[spi->chip_select];
 
 	if (!cs) {
-		cs = kzalloc(sizeof *cs, GFP_KERNEL);
+		cs = devm_kzalloc(&spi->dev , sizeof *cs, GFP_KERNEL);
 		if (!cs)
 			return -ENOMEM;
 		cs->base = mcspi->base + spi->chip_select * 0x14;
@@ -799,8 +809,7 @@ static int omap2_mcspi_setup(struct spi_device *spi)
 		cs->chconf0 = 0;
 		spi->controller_state = cs;
 		/* Link this to context save list */
-		list_add_tail(&cs->node,
-			&omap2_mcspi_ctx[mcspi->master->bus_num - 1].cs);
+		list_add_tail(&cs->node, &ctx->cs);
 	}
 
 	if (mcspi_dma->dma_rx_channel == -1
@@ -833,7 +842,6 @@ static void omap2_mcspi_cleanup(struct spi_device *spi)
 		cs = spi->controller_state;
 		list_del(&cs->node);
 
-		kfree(spi->controller_state);
 	}
 
 	if (spi->chip_select < spi->master->num_chipselect) {
@@ -850,144 +858,122 @@ static void omap2_mcspi_cleanup(struct spi_device *spi)
 	}
 }
 
-static void omap2_mcspi_work(struct work_struct *work)
+static void omap2_mcspi_work(struct omap2_mcspi *mcspi, struct spi_message *m)
 {
-	struct omap2_mcspi	*mcspi;
-
-	mcspi = container_of(work, struct omap2_mcspi, work);
-
-	if (omap2_mcspi_enable_clocks(mcspi) < 0)
-		return;
-
-	spin_lock_irq(&mcspi->lock);
 
 	/* We only enable one channel at a time -- the one whose message is
-	 * at the head of the queue -- although this controller would gladly
+	 * -- although this controller would gladly
 	 * arbitrate among multiple channels.  This corresponds to "single
 	 * channel" master mode.  As a side effect, we need to manage the
 	 * chipselect with the FORCE bit ... CS != channel enable.
 	 */
-	while (!list_empty(&mcspi->msg_queue)) {
-		struct spi_message		*m;
-		struct spi_device		*spi;
-		struct spi_transfer		*t = NULL;
-		int				cs_active = 0;
-		struct omap2_mcspi_cs		*cs;
-		struct omap2_mcspi_device_config *cd;
-		int				par_override = 0;
-		int				status = 0;
-		u32				chconf;
-
-		m = container_of(mcspi->msg_queue.next, struct spi_message,
-				 queue);
-
-		list_del_init(&m->queue);
-		spin_unlock_irq(&mcspi->lock);
-
-		spi = m->spi;
-		cs = spi->controller_state;
-		cd = spi->controller_data;
 
-		omap2_mcspi_set_enable(spi, 1);
-		list_for_each_entry(t, &m->transfers, transfer_list) {
-			if (t->tx_buf == NULL && t->rx_buf == NULL && t->len) {
-				status = -EINVAL;
-				break;
-			}
-			if (par_override || t->speed_hz || t->bits_per_word) {
-				par_override = 1;
-				status = omap2_mcspi_setup_transfer(spi, t);
-				if (status < 0)
-					break;
-				if (!t->speed_hz && !t->bits_per_word)
-					par_override = 0;
-			}
+	struct spi_device		*spi;
+	struct spi_transfer		*t = NULL;
+	int				cs_active = 0;
+	struct omap2_mcspi_cs		*cs;
+	struct omap2_mcspi_device_config *cd;
+	int				par_override = 0;
+	int				status = 0;
+	u32				chconf;
 
-			if (!cs_active) {
-				omap2_mcspi_force_cs(spi, 1);
-				cs_active = 1;
-			}
+	spi = m->spi;
+	cs = spi->controller_state;
+	cd = spi->controller_data;
 
-			chconf = mcspi_cached_chconf0(spi);
-			chconf &= ~OMAP2_MCSPI_CHCONF_TRM_MASK;
-			chconf &= ~OMAP2_MCSPI_CHCONF_TURBO;
+	omap2_mcspi_set_enable(spi, 1);
+	list_for_each_entry(t, &m->transfers, transfer_list) {
+		if (t->tx_buf == NULL && t->rx_buf == NULL && t->len) {
+			status = -EINVAL;
+			break;
+		}
+		if (par_override || t->speed_hz || t->bits_per_word) {
+			par_override = 1;
+			status = omap2_mcspi_setup_transfer(spi, t);
+			if (status < 0)
+				break;
+			if (!t->speed_hz && !t->bits_per_word)
+				par_override = 0;
+		}
 
-			if (t->tx_buf == NULL)
-				chconf |= OMAP2_MCSPI_CHCONF_TRM_RX_ONLY;
-			else if (t->rx_buf == NULL)
-				chconf |= OMAP2_MCSPI_CHCONF_TRM_TX_ONLY;
-
-			if (cd && cd->turbo_mode && t->tx_buf == NULL) {
-				/* Turbo mode is for more than one word */
-				if (t->len > ((cs->word_len + 7) >> 3))
-					chconf |= OMAP2_MCSPI_CHCONF_TURBO;
-			}
+		if (!cs_active) {
+			omap2_mcspi_force_cs(spi, 1);
+			cs_active = 1;
+		}
 
-			mcspi_write_chconf0(spi, chconf);
+		chconf = mcspi_cached_chconf0(spi);
+		chconf &= ~OMAP2_MCSPI_CHCONF_TRM_MASK;
+		chconf &= ~OMAP2_MCSPI_CHCONF_TURBO;
 
-			if (t->len) {
-				unsigned	count;
+		if (t->tx_buf == NULL)
+			chconf |= OMAP2_MCSPI_CHCONF_TRM_RX_ONLY;
+		else if (t->rx_buf == NULL)
+			chconf |= OMAP2_MCSPI_CHCONF_TRM_TX_ONLY;
 
-				/* RX_ONLY mode needs dummy data in TX reg */
-				if (t->tx_buf == NULL)
-					__raw_writel(0, cs->base
-							+ OMAP2_MCSPI_TX0);
+		if (cd && cd->turbo_mode && t->tx_buf == NULL) {
+			/* Turbo mode is for more than one word */
+			if (t->len > ((cs->word_len + 7) >> 3))
+				chconf |= OMAP2_MCSPI_CHCONF_TURBO;
+		}
 
-				if (m->is_dma_mapped || t->len >= DMA_MIN_BYTES)
-					count = omap2_mcspi_txrx_dma(spi, t);
-				else
-					count = omap2_mcspi_txrx_pio(spi, t);
-				m->actual_length += count;
+		mcspi_write_chconf0(spi, chconf);
 
-				if (count != t->len) {
-					status = -EIO;
-					break;
-				}
-			}
+		if (t->len) {
+			unsigned	count;
 
-			if (t->delay_usecs)
-				udelay(t->delay_usecs);
+			/* RX_ONLY mode needs dummy data in TX reg */
+			if (t->tx_buf == NULL)
+				__raw_writel(0, cs->base
+						+ OMAP2_MCSPI_TX0);
+
+			if (m->is_dma_mapped || t->len >= DMA_MIN_BYTES)
+				count = omap2_mcspi_txrx_dma(spi, t);
+			else
+				count = omap2_mcspi_txrx_pio(spi, t);
+			m->actual_length += count;
 
-			/* ignore the "leave it on after last xfer" hint */
-			if (t->cs_change) {
-				omap2_mcspi_force_cs(spi, 0);
-				cs_active = 0;
+			if (count != t->len) {
+				status = -EIO;
+				break;
 			}
 		}
 
-		/* Restore defaults if they were overriden */
-		if (par_override) {
-			par_override = 0;
-			status = omap2_mcspi_setup_transfer(spi, NULL);
-		}
+		if (t->delay_usecs)
+			udelay(t->delay_usecs);
 
-		if (cs_active)
+		/* ignore the "leave it on after last xfer" hint */
+		if (t->cs_change) {
 			omap2_mcspi_force_cs(spi, 0);
+			cs_active = 0;
+		}
+	}
+	/* Restore defaults if they were overriden */
+	if (par_override) {
+		par_override = 0;
+		status = omap2_mcspi_setup_transfer(spi, NULL);
+	}
 
-		omap2_mcspi_set_enable(spi, 0);
-
-		m->status = status;
-		m->complete(m->context);
+	if (cs_active)
+		omap2_mcspi_force_cs(spi, 0);
 
-		spin_lock_irq(&mcspi->lock);
-	}
+	omap2_mcspi_set_enable(spi, 0);
 
-	spin_unlock_irq(&mcspi->lock);
+	m->status = status;
 
-	omap2_mcspi_disable_clocks(mcspi);
 }
 
-static int omap2_mcspi_transfer(struct spi_device *spi, struct spi_message *m)
+static int omap2_mcspi_transfer_one_message(struct spi_master *master,
+						struct spi_message *m)
 {
 	struct omap2_mcspi	*mcspi;
-	unsigned long		flags;
 	struct spi_transfer	*t;
 
+	mcspi = spi_master_get_devdata(master);
 	m->actual_length = 0;
 	m->status = 0;
 
 	/* reject invalid messages and transfers */
-	if (list_empty(&m->transfers) || !m->complete)
+	if (list_empty(&m->transfers))
 		return -EINVAL;
 	list_for_each_entry(t, &m->transfers, transfer_list) {
 		const void	*tx_buf = t->tx_buf;
@@ -999,7 +985,7 @@ static int omap2_mcspi_transfer(struct spi_device *spi, struct spi_message *m)
 				|| (t->bits_per_word &&
 					(  t->bits_per_word < 4
 					|| t->bits_per_word > 32))) {
-			dev_dbg(&spi->dev, "transfer: %d Hz, %d %s%s, %d bpw\n",
+			dev_dbg(mcspi->dev, "transfer: %d Hz, %d %s%s, %d bpw\n",
 					t->speed_hz,
 					len,
 					tx_buf ? "tx" : "",
@@ -1008,7 +994,7 @@ static int omap2_mcspi_transfer(struct spi_device *spi, struct spi_message *m)
 			return -EINVAL;
 		}
 		if (t->speed_hz && t->speed_hz < (OMAP2_MCSPI_MAX_FREQ >> 15)) {
-			dev_dbg(&spi->dev, "speed_hz %d below minimum %d Hz\n",
+			dev_dbg(mcspi->dev, "speed_hz %d below minimum %d Hz\n",
 				t->speed_hz,
 				OMAP2_MCSPI_MAX_FREQ >> 15);
 			return -EINVAL;
@@ -1018,51 +1004,46 @@ static int omap2_mcspi_transfer(struct spi_device *spi, struct spi_message *m)
 			continue;
 
 		if (tx_buf != NULL) {
-			t->tx_dma = dma_map_single(&spi->dev, (void *) tx_buf,
+			t->tx_dma = dma_map_single(mcspi->dev, (void *) tx_buf,
 					len, DMA_TO_DEVICE);
-			if (dma_mapping_error(&spi->dev, t->tx_dma)) {
-				dev_dbg(&spi->dev, "dma %cX %d bytes error\n",
+			if (dma_mapping_error(mcspi->dev, t->tx_dma)) {
+				dev_dbg(mcspi->dev, "dma %cX %d bytes error\n",
 						'T', len);
 				return -EINVAL;
 			}
 		}
 		if (rx_buf != NULL) {
-			t->rx_dma = dma_map_single(&spi->dev, rx_buf, t->len,
+			t->rx_dma = dma_map_single(mcspi->dev, rx_buf, t->len,
 					DMA_FROM_DEVICE);
-			if (dma_mapping_error(&spi->dev, t->rx_dma)) {
-				dev_dbg(&spi->dev, "dma %cX %d bytes error\n",
+			if (dma_mapping_error(mcspi->dev, t->rx_dma)) {
+				dev_dbg(mcspi->dev, "dma %cX %d bytes error\n",
 						'R', len);
 				if (tx_buf != NULL)
-					dma_unmap_single(&spi->dev, t->tx_dma,
+					dma_unmap_single(mcspi->dev, t->tx_dma,
 							len, DMA_TO_DEVICE);
 				return -EINVAL;
 			}
 		}
 	}
 
-	mcspi = spi_master_get_devdata(spi->master);
-
-	spin_lock_irqsave(&mcspi->lock, flags);
-	list_add_tail(&m->queue, &mcspi->msg_queue);
-	queue_work(mcspi->wq, &mcspi->work);
-	spin_unlock_irqrestore(&mcspi->lock, flags);
-
+	omap2_mcspi_work(mcspi, m);
+	spi_finalize_current_message(master);
 	return 0;
 }
 
 static int __init omap2_mcspi_master_setup(struct omap2_mcspi *mcspi)
 {
 	struct spi_master	*master = mcspi->master;
-	u32			tmp;
-	int ret = 0;
+	struct omap2_mcspi_regs	*ctx = &mcspi->ctx;
+	int			ret = 0;
 
 	ret = omap2_mcspi_enable_clocks(mcspi);
 	if (ret < 0)
 		return ret;
 
-	tmp = OMAP2_MCSPI_WAKEUPENABLE_WKEN;
-	mcspi_write_reg(master, OMAP2_MCSPI_WAKEUPENABLE, tmp);
-	omap2_mcspi_ctx[master->bus_num - 1].wakeupenable = tmp;
+	mcspi_write_reg(master, OMAP2_MCSPI_WAKEUPENABLE,
+				OMAP2_MCSPI_WAKEUPENABLE_WKEN);
+	ctx->wakeupenable = OMAP2_MCSPI_WAKEUPENABLE_WKEN;
 
 	omap2_mcspi_set_master_mode(master);
 	omap2_mcspi_disable_clocks(mcspi);
@@ -1102,14 +1083,13 @@ static const struct of_device_id omap_mcspi_of_match[] = {
 };
 MODULE_DEVICE_TABLE(of, omap_mcspi_of_match);
 
-static int __init omap2_mcspi_probe(struct platform_device *pdev)
+static int __devinit omap2_mcspi_probe(struct platform_device *pdev)
 {
 	struct spi_master	*master;
 	struct omap2_mcspi_platform_config *pdata;
 	struct omap2_mcspi	*mcspi;
 	struct resource		*r;
 	int			status = 0, i;
-	char			wq_name[20];
 	u32			regs_offset = 0;
 	static int		bus_num = 1;
 	struct device_node	*node = pdev->dev.of_node;
@@ -1125,7 +1105,9 @@ static int __init omap2_mcspi_probe(struct platform_device *pdev)
 	master->mode_bits = SPI_CPOL | SPI_CPHA | SPI_CS_HIGH;
 
 	master->setup = omap2_mcspi_setup;
-	master->transfer = omap2_mcspi_transfer;
+	master->prepare_transfer_hardware = omap2_prepare_transfer;
+	master->unprepare_transfer_hardware = omap2_unprepare_transfer;
+	master->transfer_one_message = omap2_mcspi_transfer_one_message;
 	master->cleanup = omap2_mcspi_cleanup;
 	master->dev.of_node = node;
 
@@ -1150,13 +1132,6 @@ static int __init omap2_mcspi_probe(struct platform_device *pdev)
 	mcspi = spi_master_get_devdata(master);
 	mcspi->master = master;
 
-	sprintf(wq_name, "omap2_mcspi/%d", master->bus_num);
-	mcspi->wq = alloc_workqueue(wq_name, WQ_MEM_RECLAIM, 1);
-	if (mcspi->wq == NULL) {
-		status = -ENOMEM;
-		goto free_master;
-	}
-
 	r = platform_get_resource(pdev, IORESOURCE_MEM, 0);
 	if (r == NULL) {
 		status = -ENODEV;
@@ -1166,32 +1141,24 @@ static int __init omap2_mcspi_probe(struct platform_device *pdev)
 	r->start += regs_offset;
 	r->end += regs_offset;
 	mcspi->phys = r->start;
-	if (!request_mem_region(r->start, resource_size(r),
-				dev_name(&pdev->dev))) {
-		status = -EBUSY;
-		goto free_master;
-	}
 
-	mcspi->base = ioremap(r->start, resource_size(r));
+	mcspi->base = devm_request_and_ioremap(&pdev->dev, r);
 	if (!mcspi->base) {
 		dev_dbg(&pdev->dev, "can't ioremap MCSPI\n");
 		status = -ENOMEM;
-		goto release_region;
+		goto free_master;
 	}
 
 	mcspi->dev = &pdev->dev;
-	INIT_WORK(&mcspi->work, omap2_mcspi_work);
 
-	spin_lock_init(&mcspi->lock);
-	INIT_LIST_HEAD(&mcspi->msg_queue);
-	INIT_LIST_HEAD(&omap2_mcspi_ctx[master->bus_num - 1].cs);
+	INIT_LIST_HEAD(&mcspi->ctx.cs);
 
 	mcspi->dma_channels = kcalloc(master->num_chipselect,
 			sizeof(struct omap2_mcspi_dma),
 			GFP_KERNEL);
 
 	if (mcspi->dma_channels == NULL)
-		goto unmap_io;
+		goto free_master;
 
 	for (i = 0; i < master->num_chipselect; i++) {
 		char dma_ch_name[14];
@@ -1224,6 +1191,8 @@ static int __init omap2_mcspi_probe(struct platform_device *pdev)
 	if (status < 0)
 		goto dma_chnl_free;
 
+	pm_runtime_use_autosuspend(&pdev->dev);
+	pm_runtime_set_autosuspend_delay(&pdev->dev, SPI_AUTOSUSPEND_TIMEOUT);
 	pm_runtime_enable(&pdev->dev);
 
 	if (status || omap2_mcspi_master_setup(mcspi) < 0)
@@ -1241,23 +1210,17 @@ disable_pm:
 	pm_runtime_disable(&pdev->dev);
 dma_chnl_free:
 	kfree(mcspi->dma_channels);
-unmap_io:
-	iounmap(mcspi->base);
-release_region:
-	release_mem_region(r->start, resource_size(r));
 free_master:
 	kfree(master);
 	platform_set_drvdata(pdev, NULL);
 	return status;
 }
 
-static int __exit omap2_mcspi_remove(struct platform_device *pdev)
+static int __devexit omap2_mcspi_remove(struct platform_device *pdev)
 {
 	struct spi_master	*master;
 	struct omap2_mcspi	*mcspi;
 	struct omap2_mcspi_dma	*dma_channels;
-	struct resource		*r;
-	void __iomem *base;
 
 	master = dev_get_drvdata(&pdev->dev);
 	mcspi = spi_master_get_devdata(master);
@@ -1265,14 +1228,9 @@ static int __exit omap2_mcspi_remove(struct platform_device *pdev)
 
 	omap2_mcspi_disable_clocks(mcspi);
 	pm_runtime_disable(&pdev->dev);
-	r = platform_get_resource(pdev, IORESOURCE_MEM, 0);
-	release_mem_region(r->start, resource_size(r));
 
-	base = mcspi->base;
 	spi_unregister_master(master);
-	iounmap(base);
 	kfree(dma_channels);
-	destroy_workqueue(mcspi->wq);
 	platform_set_drvdata(pdev, NULL);
 
 	return 0;
@@ -1291,13 +1249,12 @@ static int omap2_mcspi_resume(struct device *dev)
 {
 	struct spi_master	*master = dev_get_drvdata(dev);
 	struct omap2_mcspi	*mcspi = spi_master_get_devdata(master);
-	struct omap2_mcspi_cs *cs;
+	struct omap2_mcspi_regs	*ctx = &mcspi->ctx;
+	struct omap2_mcspi_cs	*cs;
 
 	omap2_mcspi_enable_clocks(mcspi);
-	list_for_each_entry(cs, &omap2_mcspi_ctx[master->bus_num - 1].cs,
-			    node) {
+	list_for_each_entry(cs, &ctx->cs, node) {
 		if ((cs->chconf0 & OMAP2_MCSPI_CHCONF_FORCE) == 0) {
-
 			/*
 			 * We need to toggle CS state for OMAP take this
 			 * change in account.
@@ -1327,21 +1284,9 @@ static struct platform_driver omap2_mcspi_driver = {
 		.pm =		&omap2_mcspi_pm_ops,
 		.of_match_table = omap_mcspi_of_match,
 	},
-	.remove =	__exit_p(omap2_mcspi_remove),
+	.probe =	omap2_mcspi_probe,
+	.remove =	__devexit_p(omap2_mcspi_remove),
 };
 
-
-static int __init omap2_mcspi_init(void)
-{
-	return platform_driver_probe(&omap2_mcspi_driver, omap2_mcspi_probe);
-}
-subsys_initcall(omap2_mcspi_init);
-
-static void __exit omap2_mcspi_exit(void)
-{
-	platform_driver_unregister(&omap2_mcspi_driver);
-
-}
-module_exit(omap2_mcspi_exit);
-
+module_platform_driver(omap2_mcspi_driver);
 MODULE_LICENSE("GPL");
diff --git a/drivers/spi/spi-orion.c b/drivers/spi/spi-orion.c
index e496f799b7a..dfd04e91fa6 100644
--- a/drivers/spi/spi-orion.c
+++ b/drivers/spi/spi-orion.c
@@ -16,8 +16,8 @@
 #include <linux/err.h>
 #include <linux/io.h>
 #include <linux/spi/spi.h>
-#include <linux/spi/orion_spi.h>
 #include <linux/module.h>
+#include <linux/clk.h>
 #include <asm/unaligned.h>
 
 #define DRIVER_NAME			"orion_spi"
@@ -46,6 +46,7 @@ struct orion_spi {
 	unsigned int		max_speed;
 	unsigned int		min_speed;
 	struct orion_spi_info	*spi_info;
+	struct clk              *clk;
 };
 
 static struct workqueue_struct *orion_spi_wq;
@@ -104,7 +105,7 @@ static int orion_spi_baudrate_set(struct spi_device *spi, unsigned int speed)
 
 	orion_spi = spi_master_get_devdata(spi->master);
 
-	tclk_hz = orion_spi->spi_info->tclk;
+	tclk_hz = clk_get_rate(orion_spi->clk);
 
 	/*
 	 * the supported rates are: 4,6,8...30
@@ -450,6 +451,7 @@ static int __init orion_spi_probe(struct platform_device *pdev)
 	struct orion_spi *spi;
 	struct resource *r;
 	struct orion_spi_info *spi_info;
+	unsigned long tclk_hz;
 	int status = 0;
 
 	spi_info = pdev->dev.platform_data;
@@ -476,19 +478,28 @@ static int __init orion_spi_probe(struct platform_device *pdev)
 	spi->master = master;
 	spi->spi_info = spi_info;
 
-	spi->max_speed = DIV_ROUND_UP(spi_info->tclk, 4);
-	spi->min_speed = DIV_ROUND_UP(spi_info->tclk, 30);
+	spi->clk = clk_get(&pdev->dev, NULL);
+	if (IS_ERR(spi->clk)) {
+		status = PTR_ERR(spi->clk);
+		goto out;
+	}
+
+	clk_prepare(spi->clk);
+	clk_enable(spi->clk);
+	tclk_hz = clk_get_rate(spi->clk);
+	spi->max_speed = DIV_ROUND_UP(tclk_hz, 4);
+	spi->min_speed = DIV_ROUND_UP(tclk_hz, 30);
 
 	r = platform_get_resource(pdev, IORESOURCE_MEM, 0);
 	if (r == NULL) {
 		status = -ENODEV;
-		goto out;
+		goto out_rel_clk;
 	}
 
 	if (!request_mem_region(r->start, resource_size(r),
 				dev_name(&pdev->dev))) {
 		status = -EBUSY;
-		goto out;
+		goto out_rel_clk;
 	}
 	spi->base = ioremap(r->start, SZ_1K);
 
@@ -508,7 +519,9 @@ static int __init orion_spi_probe(struct platform_device *pdev)
 
 out_rel_mem:
 	release_mem_region(r->start, resource_size(r));
-
+out_rel_clk:
+	clk_disable_unprepare(spi->clk);
+	clk_put(spi->clk);
 out:
 	spi_master_put(master);
 	return status;
@@ -526,6 +539,9 @@ static int __exit orion_spi_remove(struct platform_device *pdev)
 
 	cancel_work_sync(&spi->work);
 
+	clk_disable_unprepare(spi->clk);
+	clk_put(spi->clk);
+
 	r = platform_get_resource(pdev, IORESOURCE_MEM, 0);
 	release_mem_region(r->start, resource_size(r));
 
diff --git a/drivers/spi/spi-ppc4xx.c b/drivers/spi/spi-ppc4xx.c
index 98ec53285fc..75ac9d48ef4 100644
--- a/drivers/spi/spi-ppc4xx.c
+++ b/drivers/spi/spi-ppc4xx.c
@@ -30,7 +30,6 @@
 #include <linux/errno.h>
 #include <linux/wait.h>
 #include <linux/of_platform.h>
-#include <linux/of_spi.h>
 #include <linux/of_gpio.h>
 #include <linux/interrupt.h>
 #include <linux/delay.h>
@@ -467,9 +466,6 @@ static int __init spi_ppc4xx_of_probe(struct platform_device *op)
 	bbp->master->setup = spi_ppc4xx_setup;
 	bbp->master->cleanup = spi_ppc4xx_cleanup;
 
-	/* Allocate bus num dynamically. */
-	bbp->master->bus_num = -1;
-
 	/* the spi->mode bits understood by this driver: */
 	bbp->master->mode_bits =
 		SPI_CPHA | SPI_CPOL | SPI_CS_HIGH | SPI_LSB_FIRST;
diff --git a/drivers/spi/spi-pxa2xx-pci.c b/drivers/spi/spi-pxa2xx-pci.c
index 3fb44afe27b..9f6ba34b172 100644
--- a/drivers/spi/spi-pxa2xx-pci.c
+++ b/drivers/spi/spi-pxa2xx-pci.c
@@ -164,17 +164,7 @@ static struct pci_driver ce4100_spi_driver = {
 	.remove         = __devexit_p(ce4100_spi_remove),
 };
 
-static int __init ce4100_spi_init(void)
-{
-	return pci_register_driver(&ce4100_spi_driver);
-}
-module_init(ce4100_spi_init);
-
-static void __exit ce4100_spi_exit(void)
-{
-	pci_unregister_driver(&ce4100_spi_driver);
-}
-module_exit(ce4100_spi_exit);
+module_pci_driver(ce4100_spi_driver);
 
 MODULE_DESCRIPTION("CE4100 PCI-SPI glue code for PXA's driver");
 MODULE_LICENSE("GPL v2");
diff --git a/drivers/spi/spi-rspi.c b/drivers/spi/spi-rspi.c
index 354f170eab9..4894bde4bbf 100644
--- a/drivers/spi/spi-rspi.c
+++ b/drivers/spi/spi-rspi.c
@@ -31,7 +31,11 @@
 #include <linux/platform_device.h>
 #include <linux/io.h>
 #include <linux/clk.h>
+#include <linux/dmaengine.h>
+#include <linux/dma-mapping.h>
+#include <linux/sh_dma.h>
 #include <linux/spi/spi.h>
+#include <linux/spi/rspi.h>
 
 #define RSPI_SPCR		0x00
 #define RSPI_SSLP		0x01
@@ -141,6 +145,16 @@ struct rspi_data {
 	spinlock_t lock;
 	struct clk *clk;
 	unsigned char spsr;
+
+	/* for dmaengine */
+	struct sh_dmae_slave dma_tx;
+	struct sh_dmae_slave dma_rx;
+	struct dma_chan *chan_tx;
+	struct dma_chan *chan_rx;
+	int irq;
+
+	unsigned dma_width_16bit:1;
+	unsigned dma_callbacked:1;
 };
 
 static void rspi_write8(struct rspi_data *rspi, u8 data, u16 offset)
@@ -265,11 +279,125 @@ static int rspi_send_pio(struct rspi_data *rspi, struct spi_message *mesg,
 	return 0;
 }
 
-static int rspi_receive_pio(struct rspi_data *rspi, struct spi_message *mesg,
-			    struct spi_transfer *t)
+static void rspi_dma_complete(void *arg)
+{
+	struct rspi_data *rspi = arg;
+
+	rspi->dma_callbacked = 1;
+	wake_up_interruptible(&rspi->wait);
+}
+
+static int rspi_dma_map_sg(struct scatterlist *sg, void *buf, unsigned len,
+			   struct dma_chan *chan,
+			   enum dma_transfer_direction dir)
+{
+	sg_init_table(sg, 1);
+	sg_set_buf(sg, buf, len);
+	sg_dma_len(sg) = len;
+	return dma_map_sg(chan->device->dev, sg, 1, dir);
+}
+
+static void rspi_dma_unmap_sg(struct scatterlist *sg, struct dma_chan *chan,
+			      enum dma_transfer_direction dir)
+{
+	dma_unmap_sg(chan->device->dev, sg, 1, dir);
+}
+
+static void rspi_memory_to_8bit(void *buf, const void *data, unsigned len)
+{
+	u16 *dst = buf;
+	const u8 *src = data;
+
+	while (len) {
+		*dst++ = (u16)(*src++);
+		len--;
+	}
+}
+
+static void rspi_memory_from_8bit(void *buf, const void *data, unsigned len)
+{
+	u8 *dst = buf;
+	const u16 *src = data;
+
+	while (len) {
+		*dst++ = (u8)*src++;
+		len--;
+	}
+}
+
+static int rspi_send_dma(struct rspi_data *rspi, struct spi_transfer *t)
+{
+	struct scatterlist sg;
+	void *buf = NULL;
+	struct dma_async_tx_descriptor *desc;
+	unsigned len;
+	int ret = 0;
+
+	if (rspi->dma_width_16bit) {
+		/*
+		 * If DMAC bus width is 16-bit, the driver allocates a dummy
+		 * buffer. And, the driver converts original data into the
+		 * DMAC data as the following format:
+		 *  original data: 1st byte, 2nd byte ...
+		 *  DMAC data:     1st byte, dummy, 2nd byte, dummy ...
+		 */
+		len = t->len * 2;
+		buf = kmalloc(len, GFP_KERNEL);
+		if (!buf)
+			return -ENOMEM;
+		rspi_memory_to_8bit(buf, t->tx_buf, t->len);
+	} else {
+		len = t->len;
+		buf = (void *)t->tx_buf;
+	}
+
+	if (!rspi_dma_map_sg(&sg, buf, len, rspi->chan_tx, DMA_TO_DEVICE)) {
+		ret = -EFAULT;
+		goto end_nomap;
+	}
+	desc = dmaengine_prep_slave_sg(rspi->chan_tx, &sg, 1, DMA_TO_DEVICE,
+				       DMA_PREP_INTERRUPT | DMA_CTRL_ACK);
+	if (!desc) {
+		ret = -EIO;
+		goto end;
+	}
+
+	/*
+	 * DMAC needs SPTIE, but if SPTIE is set, this IRQ routine will be
+	 * called. So, this driver disables the IRQ while DMA transfer.
+	 */
+	disable_irq(rspi->irq);
+
+	rspi_write8(rspi, rspi_read8(rspi, RSPI_SPCR) | SPCR_TXMD, RSPI_SPCR);
+	rspi_enable_irq(rspi, SPCR_SPTIE);
+	rspi->dma_callbacked = 0;
+
+	desc->callback = rspi_dma_complete;
+	desc->callback_param = rspi;
+	dmaengine_submit(desc);
+	dma_async_issue_pending(rspi->chan_tx);
+
+	ret = wait_event_interruptible_timeout(rspi->wait,
+					       rspi->dma_callbacked, HZ);
+	if (ret > 0 && rspi->dma_callbacked)
+		ret = 0;
+	else if (!ret)
+		ret = -ETIMEDOUT;
+	rspi_disable_irq(rspi, SPCR_SPTIE);
+
+	enable_irq(rspi->irq);
+
+end:
+	rspi_dma_unmap_sg(&sg, rspi->chan_tx, DMA_TO_DEVICE);
+end_nomap:
+	if (rspi->dma_width_16bit)
+		kfree(buf);
+
+	return ret;
+}
+
+static void rspi_receive_init(struct rspi_data *rspi)
 {
-	int remain = t->len;
-	u8 *data;
 	unsigned char spsr;
 
 	spsr = rspi_read8(rspi, RSPI_SPSR);
@@ -278,6 +406,15 @@ static int rspi_receive_pio(struct rspi_data *rspi, struct spi_message *mesg,
 	if (spsr & SPSR_OVRF)
 		rspi_write8(rspi, rspi_read8(rspi, RSPI_SPSR) & ~SPSR_OVRF,
 			    RSPI_SPCR);
+}
+
+static int rspi_receive_pio(struct rspi_data *rspi, struct spi_message *mesg,
+			    struct spi_transfer *t)
+{
+	int remain = t->len;
+	u8 *data;
+
+	rspi_receive_init(rspi);
 
 	data = (u8 *)t->rx_buf;
 	while (remain > 0) {
@@ -307,6 +444,120 @@ static int rspi_receive_pio(struct rspi_data *rspi, struct spi_message *mesg,
 	return 0;
 }
 
+static int rspi_receive_dma(struct rspi_data *rspi, struct spi_transfer *t)
+{
+	struct scatterlist sg, sg_dummy;
+	void *dummy = NULL, *rx_buf = NULL;
+	struct dma_async_tx_descriptor *desc, *desc_dummy;
+	unsigned len;
+	int ret = 0;
+
+	if (rspi->dma_width_16bit) {
+		/*
+		 * If DMAC bus width is 16-bit, the driver allocates a dummy
+		 * buffer. And, finally the driver converts the DMAC data into
+		 * actual data as the following format:
+		 *  DMAC data:   1st byte, dummy, 2nd byte, dummy ...
+		 *  actual data: 1st byte, 2nd byte ...
+		 */
+		len = t->len * 2;
+		rx_buf = kmalloc(len, GFP_KERNEL);
+		if (!rx_buf)
+			return -ENOMEM;
+	 } else {
+		len = t->len;
+		rx_buf = t->rx_buf;
+	}
+
+	/* prepare dummy transfer to generate SPI clocks */
+	dummy = kzalloc(len, GFP_KERNEL);
+	if (!dummy) {
+		ret = -ENOMEM;
+		goto end_nomap;
+	}
+	if (!rspi_dma_map_sg(&sg_dummy, dummy, len, rspi->chan_tx,
+			     DMA_TO_DEVICE)) {
+		ret = -EFAULT;
+		goto end_nomap;
+	}
+	desc_dummy = dmaengine_prep_slave_sg(rspi->chan_tx, &sg_dummy, 1,
+			DMA_TO_DEVICE, DMA_PREP_INTERRUPT | DMA_CTRL_ACK);
+	if (!desc_dummy) {
+		ret = -EIO;
+		goto end_dummy_mapped;
+	}
+
+	/* prepare receive transfer */
+	if (!rspi_dma_map_sg(&sg, rx_buf, len, rspi->chan_rx,
+			     DMA_FROM_DEVICE)) {
+		ret = -EFAULT;
+		goto end_dummy_mapped;
+
+	}
+	desc = dmaengine_prep_slave_sg(rspi->chan_rx, &sg, 1, DMA_FROM_DEVICE,
+				       DMA_PREP_INTERRUPT | DMA_CTRL_ACK);
+	if (!desc) {
+		ret = -EIO;
+		goto end;
+	}
+
+	rspi_receive_init(rspi);
+
+	/*
+	 * DMAC needs SPTIE, but if SPTIE is set, this IRQ routine will be
+	 * called. So, this driver disables the IRQ while DMA transfer.
+	 */
+	disable_irq(rspi->irq);
+
+	rspi_write8(rspi, rspi_read8(rspi, RSPI_SPCR) & ~SPCR_TXMD, RSPI_SPCR);
+	rspi_enable_irq(rspi, SPCR_SPTIE | SPCR_SPRIE);
+	rspi->dma_callbacked = 0;
+
+	desc->callback = rspi_dma_complete;
+	desc->callback_param = rspi;
+	dmaengine_submit(desc);
+	dma_async_issue_pending(rspi->chan_rx);
+
+	desc_dummy->callback = NULL;	/* No callback */
+	dmaengine_submit(desc_dummy);
+	dma_async_issue_pending(rspi->chan_tx);
+
+	ret = wait_event_interruptible_timeout(rspi->wait,
+					       rspi->dma_callbacked, HZ);
+	if (ret > 0 && rspi->dma_callbacked)
+		ret = 0;
+	else if (!ret)
+		ret = -ETIMEDOUT;
+	rspi_disable_irq(rspi, SPCR_SPTIE | SPCR_SPRIE);
+
+	enable_irq(rspi->irq);
+
+end:
+	rspi_dma_unmap_sg(&sg, rspi->chan_rx, DMA_FROM_DEVICE);
+end_dummy_mapped:
+	rspi_dma_unmap_sg(&sg_dummy, rspi->chan_tx, DMA_TO_DEVICE);
+end_nomap:
+	if (rspi->dma_width_16bit) {
+		if (!ret)
+			rspi_memory_from_8bit(t->rx_buf, rx_buf, t->len);
+		kfree(rx_buf);
+	}
+	kfree(dummy);
+
+	return ret;
+}
+
+static int rspi_is_dma(struct rspi_data *rspi, struct spi_transfer *t)
+{
+	if (t->tx_buf && rspi->chan_tx)
+		return 1;
+	/* If the module receives data by DMAC, it also needs TX DMAC */
+	if (t->rx_buf && rspi->chan_tx && rspi->chan_rx)
+		return 1;
+
+	return 0;
+}
+
 static void rspi_work(struct work_struct *work)
 {
 	struct rspi_data *rspi = container_of(work, struct rspi_data, ws);
@@ -325,12 +576,18 @@ static void rspi_work(struct work_struct *work)
 
 		list_for_each_entry(t, &mesg->transfers, transfer_list) {
 			if (t->tx_buf) {
-				ret = rspi_send_pio(rspi, mesg, t);
+				if (rspi_is_dma(rspi, t))
+					ret = rspi_send_dma(rspi, t);
+				else
+					ret = rspi_send_pio(rspi, mesg, t);
 				if (ret < 0)
 					goto error;
 			}
 			if (t->rx_buf) {
-				ret = rspi_receive_pio(rspi, mesg, t);
+				if (rspi_is_dma(rspi, t))
+					ret = rspi_receive_dma(rspi, t);
+				else
+					ret = rspi_receive_pio(rspi, mesg, t);
 				if (ret < 0)
 					goto error;
 			}
@@ -406,11 +663,58 @@ static irqreturn_t rspi_irq(int irq, void *_sr)
 	return ret;
 }
 
+static bool rspi_filter(struct dma_chan *chan, void *filter_param)
+{
+	chan->private = filter_param;
+	return true;
+}
+
+static void __devinit rspi_request_dma(struct rspi_data *rspi,
+				       struct platform_device *pdev)
+{
+	struct rspi_plat_data *rspi_pd = pdev->dev.platform_data;
+	dma_cap_mask_t mask;
+
+	if (!rspi_pd)
+		return;
+
+	rspi->dma_width_16bit = rspi_pd->dma_width_16bit;
+
+	/* If the module receives data by DMAC, it also needs TX DMAC */
+	if (rspi_pd->dma_rx_id && rspi_pd->dma_tx_id) {
+		dma_cap_zero(mask);
+		dma_cap_set(DMA_SLAVE, mask);
+		rspi->dma_rx.slave_id = rspi_pd->dma_rx_id;
+		rspi->chan_rx = dma_request_channel(mask, rspi_filter,
+						    &rspi->dma_rx);
+		if (rspi->chan_rx)
+			dev_info(&pdev->dev, "Use DMA when rx.\n");
+	}
+	if (rspi_pd->dma_tx_id) {
+		dma_cap_zero(mask);
+		dma_cap_set(DMA_SLAVE, mask);
+		rspi->dma_tx.slave_id = rspi_pd->dma_tx_id;
+		rspi->chan_tx = dma_request_channel(mask, rspi_filter,
+						    &rspi->dma_tx);
+		if (rspi->chan_tx)
+			dev_info(&pdev->dev, "Use DMA when tx\n");
+	}
+}
+
+static void __devexit rspi_release_dma(struct rspi_data *rspi)
+{
+	if (rspi->chan_tx)
+		dma_release_channel(rspi->chan_tx);
+	if (rspi->chan_rx)
+		dma_release_channel(rspi->chan_rx);
+}
+
 static int __devexit rspi_remove(struct platform_device *pdev)
 {
 	struct rspi_data *rspi = dev_get_drvdata(&pdev->dev);
 
 	spi_unregister_master(rspi->master);
+	rspi_release_dma(rspi);
 	free_irq(platform_get_irq(pdev, 0), rspi);
 	clk_put(rspi->clk);
 	iounmap(rspi->addr);
@@ -483,6 +787,9 @@ static int __devinit rspi_probe(struct platform_device *pdev)
 		goto error3;
 	}
 
+	rspi->irq = irq;
+	rspi_request_dma(rspi, pdev);
+
 	ret = spi_register_master(master);
 	if (ret < 0) {
 		dev_err(&pdev->dev, "spi_register_master error.\n");
@@ -494,6 +801,7 @@ static int __devinit rspi_probe(struct platform_device *pdev)
 	return 0;
 
 error4:
+	rspi_release_dma(rspi);
 	free_irq(irq, rspi);
 error3:
 	clk_put(rspi->clk);
diff --git a/drivers/spi/spi-sirf.c b/drivers/spi/spi-sirf.c
index 52fe495bb32..ecc3d9763d1 100644
--- a/drivers/spi/spi-sirf.c
+++ b/drivers/spi/spi-sirf.c
@@ -19,7 +19,7 @@
 #include <linux/of_gpio.h>
 #include <linux/spi/spi.h>
 #include <linux/spi/spi_bitbang.h>
-#include <linux/pinctrl/pinmux.h>
+#include <linux/pinctrl/consumer.h>
 
 #define DRIVER_NAME "sirfsoc_spi"
 
@@ -127,7 +127,7 @@ struct sirfsoc_spi {
 	void __iomem *base;
 	u32 ctrl_freq;  /* SPI controller clock speed */
 	struct clk *clk;
-	struct pinmux *pmx;
+	struct pinctrl *p;
 
 	/* rx & tx bufs from the spi_transfer */
 	const void *tx;
@@ -560,17 +560,15 @@ static int __devinit spi_sirfsoc_probe(struct platform_device *pdev)
 	master->bus_num = pdev->id;
 	sspi->bitbang.master->dev.of_node = pdev->dev.of_node;
 
-	sspi->pmx = pinmux_get(&pdev->dev, NULL);
-	ret = IS_ERR(sspi->pmx);
+	sspi->p = pinctrl_get_select_default(&pdev->dev);
+	ret = IS_ERR(sspi->p);
 	if (ret)
 		goto free_master;
 
-	pinmux_enable(sspi->pmx);
-
 	sspi->clk = clk_get(&pdev->dev, NULL);
 	if (IS_ERR(sspi->clk)) {
 		ret = -EINVAL;
-		goto free_pmx;
+		goto free_pin;
 	}
 	clk_enable(sspi->clk);
 	sspi->ctrl_freq = clk_get_rate(sspi->clk);
@@ -598,9 +596,8 @@ static int __devinit spi_sirfsoc_probe(struct platform_device *pdev)
 free_clk:
 	clk_disable(sspi->clk);
 	clk_put(sspi->clk);
-free_pmx:
-	pinmux_disable(sspi->pmx);
-	pinmux_put(sspi->pmx);
+free_pin:
+	pinctrl_put(sspi->p);
 free_master:
 	spi_master_put(master);
 err_cs:
@@ -623,8 +620,7 @@ static int  __devexit spi_sirfsoc_remove(struct platform_device *pdev)
 	}
 	clk_disable(sspi->clk);
 	clk_put(sspi->clk);
-	pinmux_disable(sspi->pmx);
-	pinmux_put(sspi->pmx);
+	pinctrl_put(sspi->p);
 	spi_master_put(master);
 	return 0;
 }
diff --git a/drivers/spi/spi-topcliff-pch.c b/drivers/spi/spi-topcliff-pch.c
index ec47d3bdfd1..cd56dcf4632 100644
--- a/drivers/spi/spi-topcliff-pch.c
+++ b/drivers/spi/spi-topcliff-pch.c
@@ -1438,7 +1438,6 @@ static int __devinit pch_spi_pd_probe(struct platform_device *plat_dev)
 		plat_dev->id, data->io_remap_addr);
 
 	/* initialize members of SPI master */
-	master->bus_num = -1;
 	master->num_chipselect = PCH_MAX_CS;
 	master->setup = pch_spi_setup;
 	master->transfer = pch_spi_transfer;
@@ -1779,7 +1778,7 @@ static struct pci_driver pch_spi_pcidev_driver = {
 	.name = "pch_spi",
 	.id_table = pch_spi_pcidev_id,
 	.probe = pch_spi_probe,
-	.remove = pch_spi_remove,
+	.remove = __devexit_p(pch_spi_remove),
 	.suspend = pch_spi_suspend,
 	.resume = pch_spi_resume,
 };
diff --git a/drivers/spi/spi.c b/drivers/spi/spi.c
index 3d8f662e4fe..1041cb83d67 100644
--- a/drivers/spi/spi.c
+++ b/drivers/spi/spi.c
@@ -2,6 +2,7 @@
  * SPI init/core code
  *
  * Copyright (C) 2005 David Brownell
+ * Copyright (C) 2008 Secret Lab Technologies 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
@@ -19,15 +20,16 @@
  */
 
 #include <linux/kernel.h>
+#include <linux/kmod.h>
 #include <linux/device.h>
 #include <linux/init.h>
 #include <linux/cache.h>
 #include <linux/mutex.h>
 #include <linux/of_device.h>
+#include <linux/of_irq.h>
 #include <linux/slab.h>
 #include <linux/mod_devicetable.h>
 #include <linux/spi/spi.h>
-#include <linux/of_spi.h>
 #include <linux/pm_runtime.h>
 #include <linux/export.h>
 #include <linux/sched.h>
@@ -530,7 +532,7 @@ static void spi_pump_messages(struct kthread_work *work)
 	/* Lock queue and check for queue work */
 	spin_lock_irqsave(&master->queue_lock, flags);
 	if (list_empty(&master->queue) || !master->running) {
-		if (master->busy) {
+		if (master->busy && master->unprepare_transfer_hardware) {
 			ret = master->unprepare_transfer_hardware(master);
 			if (ret) {
 				spin_unlock_irqrestore(&master->queue_lock, flags);
@@ -560,7 +562,7 @@ static void spi_pump_messages(struct kthread_work *work)
 		master->busy = true;
 	spin_unlock_irqrestore(&master->queue_lock, flags);
 
-	if (!was_busy) {
+	if (!was_busy && master->prepare_transfer_hardware) {
 		ret = master->prepare_transfer_hardware(master);
 		if (ret) {
 			dev_err(&master->dev,
@@ -798,6 +800,94 @@ err_init_queue:
 
 /*-------------------------------------------------------------------------*/
 
+#if defined(CONFIG_OF) && !defined(CONFIG_SPARC)
+/**
+ * of_register_spi_devices() - Register child devices onto the SPI bus
+ * @master:	Pointer to spi_master device
+ *
+ * Registers an spi_device for each child node of master node which has a 'reg'
+ * property.
+ */
+static void of_register_spi_devices(struct spi_master *master)
+{
+	struct spi_device *spi;
+	struct device_node *nc;
+	const __be32 *prop;
+	int rc;
+	int len;
+
+	if (!master->dev.of_node)
+		return;
+
+	for_each_child_of_node(master->dev.of_node, nc) {
+		/* Alloc an spi_device */
+		spi = spi_alloc_device(master);
+		if (!spi) {
+			dev_err(&master->dev, "spi_device alloc error for %s\n",
+				nc->full_name);
+			spi_dev_put(spi);
+			continue;
+		}
+
+		/* Select device driver */
+		if (of_modalias_node(nc, spi->modalias,
+				     sizeof(spi->modalias)) < 0) {
+			dev_err(&master->dev, "cannot find modalias for %s\n",
+				nc->full_name);
+			spi_dev_put(spi);
+			continue;
+		}
+
+		/* Device address */
+		prop = of_get_property(nc, "reg", &len);
+		if (!prop || len < sizeof(*prop)) {
+			dev_err(&master->dev, "%s has no 'reg' property\n",
+				nc->full_name);
+			spi_dev_put(spi);
+			continue;
+		}
+		spi->chip_select = be32_to_cpup(prop);
+
+		/* Mode (clock phase/polarity/etc.) */
+		if (of_find_property(nc, "spi-cpha", NULL))
+			spi->mode |= SPI_CPHA;
+		if (of_find_property(nc, "spi-cpol", NULL))
+			spi->mode |= SPI_CPOL;
+		if (of_find_property(nc, "spi-cs-high", NULL))
+			spi->mode |= SPI_CS_HIGH;
+
+		/* Device speed */
+		prop = of_get_property(nc, "spi-max-frequency", &len);
+		if (!prop || len < sizeof(*prop)) {
+			dev_err(&master->dev, "%s has no 'spi-max-frequency' property\n",
+				nc->full_name);
+			spi_dev_put(spi);
+			continue;
+		}
+		spi->max_speed_hz = be32_to_cpup(prop);
+
+		/* IRQ */
+		spi->irq = irq_of_parse_and_map(nc, 0);
+
+		/* Store a pointer to the node in the device structure */
+		of_node_get(nc);
+		spi->dev.of_node = nc;
+
+		/* Register the new device */
+		request_module(spi->modalias);
+		rc = spi_add_device(spi);
+		if (rc) {
+			dev_err(&master->dev, "spi_device register error %s\n",
+				nc->full_name);
+			spi_dev_put(spi);
+		}
+
+	}
+}
+#else
+static void of_register_spi_devices(struct spi_master *master) { }
+#endif
+
 static void spi_master_release(struct device *dev)
 {
 	struct spi_master *master;
@@ -846,6 +936,8 @@ struct spi_master *spi_alloc_master(struct device *dev, unsigned size)
 		return NULL;
 
 	device_initialize(&master->dev);
+	master->bus_num = -1;
+	master->num_chipselect = 1;
 	master->dev.class = &spi_master_class;
 	master->dev.parent = get_device(dev);
 	spi_master_set_devdata(master, &master[1]);
diff --git a/drivers/ssb/b43_pci_bridge.c b/drivers/ssb/b43_pci_bridge.c
index bad7ba517a1..f551e537614 100644
--- a/drivers/ssb/b43_pci_bridge.c
+++ b/drivers/ssb/b43_pci_bridge.c
@@ -29,6 +29,8 @@ static const struct pci_device_id b43_pci_bridge_tbl[] = {
 	{ PCI_DEVICE(PCI_VENDOR_ID_BROADCOM, 0x4319) },
 	{ PCI_DEVICE(PCI_VENDOR_ID_BROADCOM, 0x4320) },
 	{ PCI_DEVICE(PCI_VENDOR_ID_BROADCOM, 0x4321) },
+	{ PCI_DEVICE(PCI_VENDOR_ID_BROADCOM, 0x4322) },
+	{ PCI_DEVICE(PCI_VENDOR_ID_BROADCOM, 43222) },
 	{ PCI_DEVICE(PCI_VENDOR_ID_BROADCOM, 0x4324) },
 	{ PCI_DEVICE(PCI_VENDOR_ID_BROADCOM, 0x4325) },
 	{ PCI_DEVICE(PCI_VENDOR_ID_BROADCOM, 0x4328) },
diff --git a/drivers/ssb/pci.c b/drivers/ssb/pci.c
index ed4124469a3..e9d94968f39 100644
--- a/drivers/ssb/pci.c
+++ b/drivers/ssb/pci.c
@@ -178,6 +178,18 @@ err_pci:
 #define SPEX(_outvar, _offset, _mask, _shift) \
 	SPEX16(_outvar, _offset, _mask, _shift)
 
+#define SPEX_ARRAY8(_field, _offset, _mask, _shift)	\
+	do {	\
+		SPEX(_field[0], _offset +  0, _mask, _shift);	\
+		SPEX(_field[1], _offset +  2, _mask, _shift);	\
+		SPEX(_field[2], _offset +  4, _mask, _shift);	\
+		SPEX(_field[3], _offset +  6, _mask, _shift);	\
+		SPEX(_field[4], _offset +  8, _mask, _shift);	\
+		SPEX(_field[5], _offset + 10, _mask, _shift);	\
+		SPEX(_field[6], _offset + 12, _mask, _shift);	\
+		SPEX(_field[7], _offset + 14, _mask, _shift);	\
+	} while (0)
+
 
 static inline u8 ssb_crc8(u8 crc, u8 data)
 {
@@ -360,8 +372,9 @@ 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(country_code, SSB_SPROM1_BINF, SSB_SPROM1_BINF_CCODE,
-	     SSB_SPROM1_BINF_CCODE_SHIFT);
+	if (out->revision == 1)
+		SPEX(country_code, SSB_SPROM1_BINF, SSB_SPROM1_BINF_CCODE,
+		     SSB_SPROM1_BINF_CCODE_SHIFT);
 	SPEX(ant_available_a, SSB_SPROM1_BINF, SSB_SPROM1_BINF_ANTA,
 	     SSB_SPROM1_BINF_ANTA_SHIFT);
 	SPEX(ant_available_bg, SSB_SPROM1_BINF, SSB_SPROM1_BINF_ANTBG,
@@ -387,6 +400,8 @@ static void sprom_extract_r123(struct ssb_sprom *out, const u16 *in)
 	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);
 
 	/* Extract the antenna gain values. */
 	out->antenna_gain.a0 = r123_extract_antgain(out->revision, in,
@@ -455,14 +470,17 @@ static void sprom_extract_r45(struct ssb_sprom *out, const u16 *in)
 	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);
 	if (out->revision == 4) {
-		SPEX(country_code, SSB_SPROM4_CCODE, 0xFFFF, 0);
+		SPEX(alpha2[0], SSB_SPROM4_CCODE, 0xff00, 8);
+		SPEX(alpha2[1], SSB_SPROM4_CCODE, 0x00ff, 0);
 		SPEX(boardflags_lo, SSB_SPROM4_BFLLO, 0xFFFF, 0);
 		SPEX(boardflags_hi, SSB_SPROM4_BFLHI, 0xFFFF, 0);
 		SPEX(boardflags2_lo, SSB_SPROM4_BFL2LO, 0xFFFF, 0);
 		SPEX(boardflags2_hi, SSB_SPROM4_BFL2HI, 0xFFFF, 0);
 	} else {
-		SPEX(country_code, SSB_SPROM5_CCODE, 0xFFFF, 0);
+		SPEX(alpha2[0], SSB_SPROM5_CCODE, 0xff00, 8);
+		SPEX(alpha2[1], SSB_SPROM5_CCODE, 0x00ff, 0);
 		SPEX(boardflags_lo, SSB_SPROM5_BFLLO, 0xFFFF, 0);
 		SPEX(boardflags_hi, SSB_SPROM5_BFLHI, 0xFFFF, 0);
 		SPEX(boardflags2_lo, SSB_SPROM5_BFL2LO, 0xFFFF, 0);
@@ -525,7 +543,9 @@ static void sprom_extract_r8(struct ssb_sprom *out, const u16 *in)
 		v = in[SPOFF(SSB_SPROM8_IL0MAC) + i];
 		*(((__be16 *)out->il0mac) + i) = cpu_to_be16(v);
 	}
-	SPEX(country_code, SSB_SPROM8_CCODE, 0xFFFF, 0);
+	SPEX(board_rev, SSB_SPROM8_BOARDREV, 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);
 	SPEX(boardflags_hi, SSB_SPROM8_BFLHI, 0xFFFF, 0);
 	SPEX(boardflags2_lo, SSB_SPROM8_BFL2LO, 0xFFFF, 0);
@@ -655,6 +675,63 @@ static void sprom_extract_r8(struct ssb_sprom *out, const u16 *in)
 	SPEX(fem.ghz5.antswlut, SSB_SPROM8_FEM5G,
 		SSB_SROM8_FEM_ANTSWLUT, SSB_SROM8_FEM_ANTSWLUT_SHIFT);
 
+	SPEX(leddc_on_time, SSB_SPROM8_LEDDC, SSB_SPROM8_LEDDC_ON,
+	     SSB_SPROM8_LEDDC_ON_SHIFT);
+	SPEX(leddc_off_time, SSB_SPROM8_LEDDC, SSB_SPROM8_LEDDC_OFF,
+	     SSB_SPROM8_LEDDC_OFF_SHIFT);
+
+	SPEX(txchain, SSB_SPROM8_TXRXC, SSB_SPROM8_TXRXC_TXCHAIN,
+	     SSB_SPROM8_TXRXC_TXCHAIN_SHIFT);
+	SPEX(rxchain, SSB_SPROM8_TXRXC, SSB_SPROM8_TXRXC_RXCHAIN,
+	     SSB_SPROM8_TXRXC_RXCHAIN_SHIFT);
+	SPEX(antswitch, SSB_SPROM8_TXRXC, SSB_SPROM8_TXRXC_SWITCH,
+	     SSB_SPROM8_TXRXC_SWITCH_SHIFT);
+
+	SPEX(opo, SSB_SPROM8_OFDM2GPO, 0x00ff, 0);
+
+	SPEX_ARRAY8(mcs2gpo, SSB_SPROM8_2G_MCSPO, ~0, 0);
+	SPEX_ARRAY8(mcs5gpo, SSB_SPROM8_5G_MCSPO, ~0, 0);
+	SPEX_ARRAY8(mcs5glpo, SSB_SPROM8_5GL_MCSPO, ~0, 0);
+	SPEX_ARRAY8(mcs5ghpo, SSB_SPROM8_5GH_MCSPO, ~0, 0);
+
+	SPEX(rawtempsense, SSB_SPROM8_RAWTS, SSB_SPROM8_RAWTS_RAWTEMP,
+	     SSB_SPROM8_RAWTS_RAWTEMP_SHIFT);
+	SPEX(measpower, SSB_SPROM8_RAWTS, SSB_SPROM8_RAWTS_MEASPOWER,
+	     SSB_SPROM8_RAWTS_MEASPOWER_SHIFT);
+	SPEX(tempsense_slope, SSB_SPROM8_OPT_CORRX,
+	     SSB_SPROM8_OPT_CORRX_TEMP_SLOPE,
+	     SSB_SPROM8_OPT_CORRX_TEMP_SLOPE_SHIFT);
+	SPEX(tempcorrx, SSB_SPROM8_OPT_CORRX, SSB_SPROM8_OPT_CORRX_TEMPCORRX,
+	     SSB_SPROM8_OPT_CORRX_TEMPCORRX_SHIFT);
+	SPEX(tempsense_option, SSB_SPROM8_OPT_CORRX,
+	     SSB_SPROM8_OPT_CORRX_TEMP_OPTION,
+	     SSB_SPROM8_OPT_CORRX_TEMP_OPTION_SHIFT);
+	SPEX(freqoffset_corr, SSB_SPROM8_HWIQ_IQSWP,
+	     SSB_SPROM8_HWIQ_IQSWP_FREQ_CORR,
+	     SSB_SPROM8_HWIQ_IQSWP_FREQ_CORR_SHIFT);
+	SPEX(iqcal_swp_dis, SSB_SPROM8_HWIQ_IQSWP,
+	     SSB_SPROM8_HWIQ_IQSWP_IQCAL_SWP,
+	     SSB_SPROM8_HWIQ_IQSWP_IQCAL_SWP_SHIFT);
+	SPEX(hw_iqcal_en, SSB_SPROM8_HWIQ_IQSWP, SSB_SPROM8_HWIQ_IQSWP_HW_IQCAL,
+	     SSB_SPROM8_HWIQ_IQSWP_HW_IQCAL_SHIFT);
+
+	SPEX(bw40po, SSB_SPROM8_BW40PO, ~0, 0);
+	SPEX(cddpo, SSB_SPROM8_CDDPO, ~0, 0);
+	SPEX(stbcpo, SSB_SPROM8_STBCPO, ~0, 0);
+	SPEX(bwduppo, SSB_SPROM8_BWDUPPO, ~0, 0);
+
+	SPEX(tempthresh, SSB_SPROM8_THERMAL, SSB_SPROM8_THERMAL_TRESH,
+	     SSB_SPROM8_THERMAL_TRESH_SHIFT);
+	SPEX(tempoffset, SSB_SPROM8_THERMAL, SSB_SPROM8_THERMAL_OFFSET,
+	     SSB_SPROM8_THERMAL_OFFSET_SHIFT);
+	SPEX(phycal_tempdelta, SSB_SPROM8_TEMPDELTA,
+	     SSB_SPROM8_TEMPDELTA_PHYCAL,
+	     SSB_SPROM8_TEMPDELTA_PHYCAL_SHIFT);
+	SPEX(temps_period, SSB_SPROM8_TEMPDELTA, SSB_SPROM8_TEMPDELTA_PERIOD,
+	     SSB_SPROM8_TEMPDELTA_PERIOD_SHIFT);
+	SPEX(temps_hysteresis, SSB_SPROM8_TEMPDELTA,
+	     SSB_SPROM8_TEMPDELTA_HYSTERESIS,
+	     SSB_SPROM8_TEMPDELTA_HYSTERESIS_SHIFT);
 	sprom_extract_r458(out, in);
 
 	/* TODO - get remaining rev 8 stuff needed */
@@ -784,7 +861,6 @@ static void ssb_pci_get_boardinfo(struct ssb_bus *bus,
 {
 	bi->vendor = bus->host_pci->subsystem_vendor;
 	bi->type = bus->host_pci->subsystem_device;
-	bi->rev = bus->host_pci->revision;
 }
 
 int ssb_pci_get_invariants(struct ssb_bus *bus,
diff --git a/drivers/staging/Kconfig b/drivers/staging/Kconfig
index 97d412d9145..05e33c70075 100644
--- a/drivers/staging/Kconfig
+++ b/drivers/staging/Kconfig
@@ -24,8 +24,6 @@ menuconfig STAGING
 
 if STAGING
 
-source "drivers/staging/serial/Kconfig"
-
 source "drivers/staging/et131x/Kconfig"
 
 source "drivers/staging/slicoss/Kconfig"
@@ -68,14 +66,10 @@ source "drivers/staging/octeon/Kconfig"
 
 source "drivers/staging/serqt_usb2/Kconfig"
 
-source "drivers/staging/quatech_usb2/Kconfig"
-
 source "drivers/staging/vt6655/Kconfig"
 
 source "drivers/staging/vt6656/Kconfig"
 
-source "drivers/staging/vme/Kconfig"
-
 source "drivers/staging/sep/Kconfig"
 
 source "drivers/staging/iio/Kconfig"
@@ -116,12 +110,12 @@ source "drivers/staging/cptm1217/Kconfig"
 
 source "drivers/staging/ste_rmi4/Kconfig"
 
-source "drivers/staging/mei/Kconfig"
-
 source "drivers/staging/nvec/Kconfig"
 
 source "drivers/staging/media/Kconfig"
 
+source "drivers/staging/net/Kconfig"
+
 source "drivers/staging/omapdrm/Kconfig"
 
 source "drivers/staging/android/Kconfig"
@@ -132,4 +126,10 @@ source "drivers/staging/ramster/Kconfig"
 
 source "drivers/staging/ozwpan/Kconfig"
 
+source "drivers/staging/ccg/Kconfig"
+
+source "drivers/staging/ipack/Kconfig"
+
+source "drivers/staging/gdm72xx/Kconfig"
+
 endif # STAGING
diff --git a/drivers/staging/Makefile b/drivers/staging/Makefile
index ffe7d44374e..a987b3ad380 100644
--- a/drivers/staging/Makefile
+++ b/drivers/staging/Makefile
@@ -3,8 +3,8 @@
 # fix for build system bug...
 obj-$(CONFIG_STAGING)		+= staging.o
 
-obj-y				+= serial/
 obj-y				+= media/
+obj-y				+= net/
 obj-$(CONFIG_ET131X)		+= et131x/
 obj-$(CONFIG_SLICOSS)		+= slicoss/
 obj-$(CONFIG_USBIP_CORE)	+= usbip/
@@ -25,11 +25,11 @@ obj-$(CONFIG_TRANZPORT)		+= frontier/
 obj-$(CONFIG_IDE_PHISON)	+= phison/
 obj-$(CONFIG_LINE6_USB)		+= line6/
 obj-$(CONFIG_USB_SERIAL_QUATECH2)	+= serqt_usb2/
-obj-$(CONFIG_USB_SERIAL_QUATECH_USB2)	+= quatech_usb2/
 obj-$(CONFIG_OCTEON_ETHERNET)	+= octeon/
 obj-$(CONFIG_VT6655)		+= vt6655/
 obj-$(CONFIG_VT6656)		+= vt6656/
 obj-$(CONFIG_VME_BUS)		+= vme/
+obj-$(CONFIG_IPACK_BUS)		+= ipack/
 obj-$(CONFIG_DX_SEP)            += sep/
 obj-$(CONFIG_IIO)		+= iio/
 obj-$(CONFIG_ZRAM)		+= zram/
@@ -50,10 +50,11 @@ obj-$(CONFIG_FT1000)		+= ft1000/
 obj-$(CONFIG_SPEAKUP)		+= speakup/
 obj-$(CONFIG_TOUCHSCREEN_CLEARPAD_TM1217)	+= cptm1217/
 obj-$(CONFIG_TOUCHSCREEN_SYNAPTICS_I2C_RMI4)	+= ste_rmi4/
-obj-$(CONFIG_INTEL_MEI)		+= mei/
 obj-$(CONFIG_MFD_NVEC)		+= nvec/
 obj-$(CONFIG_DRM_OMAP)		+= omapdrm/
 obj-$(CONFIG_ANDROID)		+= android/
 obj-$(CONFIG_PHONE)		+= telephony/
 obj-$(CONFIG_RAMSTER)		+= ramster/
 obj-$(CONFIG_USB_WPAN_HCD)	+= ozwpan/
+obj-$(CONFIG_USB_G_CCG)		+= ccg/
+obj-$(CONFIG_WIMAX_GDM72XX)	+= gdm72xx/
diff --git a/drivers/staging/android/Kconfig b/drivers/staging/android/Kconfig
index eb1dee26bda..0e16b594460 100644
--- a/drivers/staging/android/Kconfig
+++ b/drivers/staging/android/Kconfig
@@ -25,17 +25,9 @@ config ANDROID_LOGGER
 	tristate "Android log driver"
 	default n
 
-config ANDROID_PERSISTENT_RAM
-	bool
-	depends on HAVE_MEMBLOCK
-	select REED_SOLOMON
-	select REED_SOLOMON_ENC8
-	select REED_SOLOMON_DEC8
-
 config ANDROID_RAM_CONSOLE
 	bool "Android RAM buffer console"
-	depends on !S390 && !UML && HAVE_MEMBLOCK
-	select ANDROID_PERSISTENT_RAM
+	depends on !S390 && !UML && HAVE_MEMBLOCK && PSTORE_RAM=y
 	default n
 
 config ANDROID_TIMED_OUTPUT
@@ -53,33 +45,14 @@ config ANDROID_LOW_MEMORY_KILLER
 	---help---
 	  Register processes to be killed when memory is low
 
-source "drivers/staging/android/switch/Kconfig"
-
-config ANDROID_INTF_ALARM
+config ANDROID_INTF_ALARM_DEV
 	bool "Android alarm driver"
 	depends on RTC_CLASS
 	default n
 	help
 	  Provides non-wakeup and rtc backed wakeup alarms based on rtc or
 	  elapsed realtime, and a non-wakeup alarm on the monotonic clock.
-	  Also provides an interface to set the wall time which must be used
-	  for elapsed realtime to work.
-
-config ANDROID_INTF_ALARM_DEV
-	bool "Android alarm device"
-	depends on ANDROID_INTF_ALARM
-	default y
-	help
-	  Exports the alarm interface to user-space.
-
-config ANDROID_ALARM_OLDDRV_COMPAT
-	bool "Android Alarm compatability with old drivers"
-	depends on ANDROID_INTF_ALARM
-	default n
-	help
-	  Provides preprocessor alias to aid compatability with
-	  older out-of-tree drivers that use the Android Alarm
-	  in-kernel API. This will be removed eventually.
+	  Also exports the alarm interface to user-space.
 
 endif # if ANDROID
 
diff --git a/drivers/staging/android/Makefile b/drivers/staging/android/Makefile
index 9b6c9ed91f6..98711e2b2af 100644
--- a/drivers/staging/android/Makefile
+++ b/drivers/staging/android/Makefile
@@ -1,11 +1,8 @@
 obj-$(CONFIG_ANDROID_BINDER_IPC)	+= binder.o
 obj-$(CONFIG_ASHMEM)			+= ashmem.o
 obj-$(CONFIG_ANDROID_LOGGER)		+= logger.o
-obj-$(CONFIG_ANDROID_PERSISTENT_RAM)	+= persistent_ram.o
 obj-$(CONFIG_ANDROID_RAM_CONSOLE)	+= ram_console.o
 obj-$(CONFIG_ANDROID_TIMED_OUTPUT)	+= timed_output.o
 obj-$(CONFIG_ANDROID_TIMED_GPIO)	+= timed_gpio.o
 obj-$(CONFIG_ANDROID_LOW_MEMORY_KILLER)	+= lowmemorykiller.o
-obj-$(CONFIG_ANDROID_SWITCH)		+= switch/
-obj-$(CONFIG_ANDROID_INTF_ALARM)	+= alarm.o
 obj-$(CONFIG_ANDROID_INTF_ALARM_DEV)	+= alarm-dev.o
diff --git a/drivers/staging/android/alarm-dev.c b/drivers/staging/android/alarm-dev.c
index 03efb34cbe2..53ce6ecf390 100644
--- a/drivers/staging/android/alarm-dev.c
+++ b/drivers/staging/android/alarm-dev.c
@@ -22,19 +22,9 @@
 #include <linux/sched.h>
 #include <linux/spinlock.h>
 #include <linux/uaccess.h>
+#include <linux/alarmtimer.h>
 #include "android_alarm.h"
 
-/* XXX - Hack out wakelocks, while they are out of tree */
-struct wake_lock {
-	int i;
-};
-#define wake_lock(x)
-#define wake_lock_timeout(x, y)
-#define wake_unlock(x)
-#define WAKE_LOCK_SUSPEND 0
-#define wake_lock_init(x, y, z) ((x)->i = 1)
-#define wake_lock_destroy(x)
-
 #define ANDROID_ALARM_PRINT_INFO (1U << 0)
 #define ANDROID_ALARM_PRINT_IO (1U << 1)
 #define ANDROID_ALARM_PRINT_INT (1U << 2)
@@ -54,19 +44,65 @@ module_param_named(debug_mask, debug_mask, int, S_IRUGO | S_IWUSR | S_IWGRP);
 	ANDROID_ALARM_RTC_WAKEUP_MASK | \
 	ANDROID_ALARM_ELAPSED_REALTIME_WAKEUP_MASK)
 
-/* support old usespace code */
+/* support old userspace code */
 #define ANDROID_ALARM_SET_OLD               _IOW('a', 2, time_t) /* set alarm */
 #define ANDROID_ALARM_SET_AND_WAIT_OLD      _IOW('a', 3, time_t)
 
 static int alarm_opened;
 static DEFINE_SPINLOCK(alarm_slock);
-static struct wake_lock alarm_wake_lock;
+static struct wakeup_source alarm_wake_lock;
 static DECLARE_WAIT_QUEUE_HEAD(alarm_wait_queue);
 static uint32_t alarm_pending;
 static uint32_t alarm_enabled;
 static uint32_t wait_pending;
 
-static struct android_alarm alarms[ANDROID_ALARM_TYPE_COUNT];
+struct devalarm {
+	union {
+		struct hrtimer hrt;
+		struct alarm alrm;
+	} u;
+	enum android_alarm_type type;
+};
+
+static struct devalarm alarms[ANDROID_ALARM_TYPE_COUNT];
+
+
+static int is_wakeup(enum android_alarm_type type)
+{
+	if (type == ANDROID_ALARM_RTC_WAKEUP ||
+			type == ANDROID_ALARM_ELAPSED_REALTIME_WAKEUP)
+		return 1;
+	return 0;
+}
+
+
+static void devalarm_start(struct devalarm *alrm, ktime_t exp)
+{
+	if (is_wakeup(alrm->type))
+		alarm_start(&alrm->u.alrm, exp);
+	else
+		hrtimer_start(&alrm->u.hrt, exp, HRTIMER_MODE_ABS);
+}
+
+
+static int devalarm_try_to_cancel(struct devalarm *alrm)
+{
+	int ret;
+	if (is_wakeup(alrm->type))
+		ret = alarm_try_to_cancel(&alrm->u.alrm);
+	else
+		ret = hrtimer_try_to_cancel(&alrm->u.hrt);
+	return ret;
+}
+
+static void devalarm_cancel(struct devalarm *alrm)
+{
+	if (is_wakeup(alrm->type))
+		alarm_cancel(&alrm->u.alrm);
+	else
+		hrtimer_cancel(&alrm->u.hrt);
+}
+
 
 static long alarm_ioctl(struct file *file, unsigned int cmd, unsigned long arg)
 {
@@ -75,6 +111,8 @@ static long alarm_ioctl(struct file *file, unsigned int cmd, unsigned long arg)
 	struct timespec new_alarm_time;
 	struct timespec new_rtc_time;
 	struct timespec tmp_time;
+	struct rtc_time new_rtc_tm;
+	struct rtc_device *rtc_dev;
 	enum android_alarm_type alarm_type = ANDROID_ALARM_IOCTL_TO_TYPE(cmd);
 	uint32_t alarm_type_mask = 1U << alarm_type;
 
@@ -101,11 +139,11 @@ static long alarm_ioctl(struct file *file, unsigned int cmd, unsigned long arg)
 	case ANDROID_ALARM_CLEAR(0):
 		spin_lock_irqsave(&alarm_slock, flags);
 		pr_alarm(IO, "alarm %d clear\n", alarm_type);
-		android_alarm_try_to_cancel(&alarms[alarm_type]);
+		devalarm_try_to_cancel(&alarms[alarm_type]);
 		if (alarm_pending) {
 			alarm_pending &= ~alarm_type_mask;
 			if (!alarm_pending && !wait_pending)
-				wake_unlock(&alarm_wake_lock);
+				__pm_relax(&alarm_wake_lock);
 		}
 		alarm_enabled &= ~alarm_type_mask;
 		spin_unlock_irqrestore(&alarm_slock, flags);
@@ -132,8 +170,7 @@ from_old_alarm_set:
 		pr_alarm(IO, "alarm %d set %ld.%09ld\n", alarm_type,
 			new_alarm_time.tv_sec, new_alarm_time.tv_nsec);
 		alarm_enabled |= alarm_type_mask;
-		android_alarm_start_range(&alarms[alarm_type],
-			timespec_to_ktime(new_alarm_time),
+		devalarm_start(&alarms[alarm_type],
 			timespec_to_ktime(new_alarm_time));
 		spin_unlock_irqrestore(&alarm_slock, flags);
 		if (ANDROID_ALARM_BASE_CMD(cmd) != ANDROID_ALARM_SET_AND_WAIT(0)
@@ -144,7 +181,7 @@ from_old_alarm_set:
 		spin_lock_irqsave(&alarm_slock, flags);
 		pr_alarm(IO, "alarm wait\n");
 		if (!alarm_pending && wait_pending) {
-			wake_unlock(&alarm_wake_lock);
+			__pm_relax(&alarm_wake_lock);
 			wait_pending = 0;
 		}
 		spin_unlock_irqrestore(&alarm_slock, flags);
@@ -163,7 +200,13 @@ from_old_alarm_set:
 			rv = -EFAULT;
 			goto err1;
 		}
-		rv = android_alarm_set_rtc(new_rtc_time);
+		rtc_time_to_tm(new_rtc_time.tv_sec, &new_rtc_tm);
+		rtc_dev = alarmtimer_get_rtcdev();
+		rv = do_settimeofday(&new_rtc_time);
+		if (rv < 0)
+			goto err1;
+		if (rtc_dev)
+			rv = rtc_set_time(rtc_dev, &new_rtc_tm);
 		spin_lock_irqsave(&alarm_slock, flags);
 		alarm_pending |= ANDROID_ALARM_TIME_CHANGE_MASK;
 		wake_up(&alarm_wait_queue);
@@ -179,8 +222,7 @@ from_old_alarm_set:
 			break;
 		case ANDROID_ALARM_ELAPSED_REALTIME_WAKEUP:
 		case ANDROID_ALARM_ELAPSED_REALTIME:
-			tmp_time =
-				ktime_to_timespec(alarm_get_elapsed_realtime());
+			get_monotonic_boottime(&tmp_time);
 			break;
 		case ANDROID_ALARM_TYPE_COUNT:
 		case ANDROID_ALARM_SYSTEMTIME:
@@ -224,14 +266,14 @@ static int alarm_release(struct inode *inode, struct file *file)
 				alarm_enabled &= ~alarm_type_mask;
 			}
 			spin_unlock_irqrestore(&alarm_slock, flags);
-			android_alarm_cancel(&alarms[i]);
+			devalarm_cancel(&alarms[i]);
 			spin_lock_irqsave(&alarm_slock, flags);
 		}
 		if (alarm_pending | wait_pending) {
 			if (alarm_pending)
 				pr_alarm(INFO, "alarm_release: clear "
 					"pending alarms %x\n", alarm_pending);
-			wake_unlock(&alarm_wake_lock);
+			__pm_relax(&alarm_wake_lock);
 			wait_pending = 0;
 			alarm_pending = 0;
 		}
@@ -241,15 +283,15 @@ static int alarm_release(struct inode *inode, struct file *file)
 	return 0;
 }
 
-static void alarm_triggered(struct android_alarm *alarm)
+static void devalarm_triggered(struct devalarm *alarm)
 {
 	unsigned long flags;
 	uint32_t alarm_type_mask = 1U << alarm->type;
 
-	pr_alarm(INT, "alarm_triggered type %d\n", alarm->type);
+	pr_alarm(INT, "devalarm_triggered type %d\n", alarm->type);
 	spin_lock_irqsave(&alarm_slock, flags);
 	if (alarm_enabled & alarm_type_mask) {
-		wake_lock_timeout(&alarm_wake_lock, 5 * HZ);
+		__pm_wakeup_event(&alarm_wake_lock, 5000); /* 5secs */
 		alarm_enabled &= ~alarm_type_mask;
 		alarm_pending |= alarm_type_mask;
 		wake_up(&alarm_wait_queue);
@@ -257,6 +299,25 @@ static void alarm_triggered(struct android_alarm *alarm)
 	spin_unlock_irqrestore(&alarm_slock, flags);
 }
 
+
+static enum hrtimer_restart devalarm_hrthandler(struct hrtimer *hrt)
+{
+	struct devalarm *devalrm = container_of(hrt, struct devalarm, u.hrt);
+
+	devalarm_triggered(devalrm);
+	return HRTIMER_NORESTART;
+}
+
+static enum alarmtimer_restart devalarm_alarmhandler(struct alarm *alrm,
+							ktime_t now)
+{
+	struct devalarm *devalrm = container_of(alrm, struct devalarm, u.alrm);
+
+	devalarm_triggered(devalrm);
+	return ALARMTIMER_NORESTART;
+}
+
+
 static const struct file_operations alarm_fops = {
 	.owner = THIS_MODULE,
 	.unlocked_ioctl = alarm_ioctl,
@@ -279,17 +340,31 @@ static int __init alarm_dev_init(void)
 	if (err)
 		return err;
 
-	for (i = 0; i < ANDROID_ALARM_TYPE_COUNT; i++)
-		android_alarm_init(&alarms[i], i, alarm_triggered);
-	wake_lock_init(&alarm_wake_lock, WAKE_LOCK_SUSPEND, "alarm");
+	alarm_init(&alarms[ANDROID_ALARM_RTC_WAKEUP].u.alrm,
+			ALARM_REALTIME, devalarm_alarmhandler);
+	hrtimer_init(&alarms[ANDROID_ALARM_RTC].u.hrt,
+			CLOCK_REALTIME, HRTIMER_MODE_ABS);
+	alarm_init(&alarms[ANDROID_ALARM_ELAPSED_REALTIME_WAKEUP].u.alrm,
+			ALARM_BOOTTIME, devalarm_alarmhandler);
+	hrtimer_init(&alarms[ANDROID_ALARM_ELAPSED_REALTIME].u.hrt,
+			CLOCK_BOOTTIME, HRTIMER_MODE_ABS);
+	hrtimer_init(&alarms[ANDROID_ALARM_SYSTEMTIME].u.hrt,
+			CLOCK_MONOTONIC, HRTIMER_MODE_ABS);
+
+	for (i = 0; i < ANDROID_ALARM_TYPE_COUNT; i++) {
+		alarms[i].type = i;
+		if (!is_wakeup(i))
+			alarms[i].u.hrt.function = devalarm_hrthandler;
+	}
 
+	wakeup_source_init(&alarm_wake_lock, "alarm");
 	return 0;
 }
 
 static void  __exit alarm_dev_exit(void)
 {
 	misc_deregister(&alarm_device);
-	wake_lock_destroy(&alarm_wake_lock);
+	wakeup_source_trash(&alarm_wake_lock);
 }
 
 module_init(alarm_dev_init);
diff --git a/drivers/staging/android/alarm.c b/drivers/staging/android/alarm.c
deleted file mode 100644
index c68950b9e08..00000000000
--- a/drivers/staging/android/alarm.c
+++ /dev/null
@@ -1,601 +0,0 @@
-/* drivers/rtc/alarm.c
- *
- * Copyright (C) 2007-2009 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/time.h>
-#include <linux/module.h>
-#include <linux/device.h>
-#include <linux/miscdevice.h>
-#include <linux/platform_device.h>
-#include <linux/rtc.h>
-#include <linux/sched.h>
-#include <linux/spinlock.h>
-#include "android_alarm.h"
-
-/* XXX - Hack out wakelocks, while they are out of tree */
-struct wake_lock {
-	int i;
-};
-#define wake_lock(x)
-#define wake_lock_timeout(x, y)
-#define wake_unlock(x)
-#define WAKE_LOCK_SUSPEND 0
-#define wake_lock_init(x, y, z) ((x)->i = 1)
-#define wake_lock_destroy(x)
-
-#define ANDROID_ALARM_PRINT_ERROR (1U << 0)
-#define ANDROID_ALARM_PRINT_INIT_STATUS (1U << 1)
-#define ANDROID_ALARM_PRINT_TSET (1U << 2)
-#define ANDROID_ALARM_PRINT_CALL (1U << 3)
-#define ANDROID_ALARM_PRINT_SUSPEND (1U << 4)
-#define ANDROID_ALARM_PRINT_INT (1U << 5)
-#define ANDROID_ALARM_PRINT_FLOW (1U << 6)
-
-static int debug_mask = ANDROID_ALARM_PRINT_ERROR | \
-			ANDROID_ALARM_PRINT_INIT_STATUS;
-module_param_named(debug_mask, debug_mask, int, S_IRUGO | S_IWUSR | S_IWGRP);
-
-#define pr_alarm(debug_level_mask, args...) \
-	do { \
-		if (debug_mask & ANDROID_ALARM_PRINT_##debug_level_mask) { \
-			pr_info(args); \
-		} \
-	} while (0)
-
-#define ANDROID_ALARM_WAKEUP_MASK ( \
-	ANDROID_ALARM_RTC_WAKEUP_MASK | \
-	ANDROID_ALARM_ELAPSED_REALTIME_WAKEUP_MASK)
-
-/* support old usespace code */
-#define ANDROID_ALARM_SET_OLD               _IOW('a', 2, time_t) /* set alarm */
-#define ANDROID_ALARM_SET_AND_WAIT_OLD      _IOW('a', 3, time_t)
-
-struct alarm_queue {
-	struct rb_root alarms;
-	struct rb_node *first;
-	struct hrtimer timer;
-	ktime_t delta;
-	bool stopped;
-	ktime_t stopped_time;
-};
-
-static struct rtc_device *alarm_rtc_dev;
-static DEFINE_SPINLOCK(alarm_slock);
-static DEFINE_MUTEX(alarm_setrtc_mutex);
-static struct wake_lock alarm_rtc_wake_lock;
-static struct platform_device *alarm_platform_dev;
-struct alarm_queue alarms[ANDROID_ALARM_TYPE_COUNT];
-static bool suspended;
-
-static void update_timer_locked(struct alarm_queue *base, bool head_removed)
-{
-	struct android_alarm *alarm;
-	bool is_wakeup = base == &alarms[ANDROID_ALARM_RTC_WAKEUP] ||
-			base == &alarms[ANDROID_ALARM_ELAPSED_REALTIME_WAKEUP];
-
-	if (base->stopped) {
-		pr_alarm(FLOW, "changed alarm while setting the wall time\n");
-		return;
-	}
-
-	if (is_wakeup && !suspended && head_removed)
-		wake_unlock(&alarm_rtc_wake_lock);
-
-	if (!base->first)
-		return;
-
-	alarm = container_of(base->first, struct android_alarm, node);
-
-	pr_alarm(FLOW, "selected alarm, type %d, func %pF at %lld\n",
-		alarm->type, alarm->function, ktime_to_ns(alarm->expires));
-
-	if (is_wakeup && suspended) {
-		pr_alarm(FLOW, "changed alarm while suspened\n");
-		wake_lock_timeout(&alarm_rtc_wake_lock, 1 * HZ);
-		return;
-	}
-
-	hrtimer_try_to_cancel(&base->timer);
-	base->timer.node.expires = ktime_add(base->delta, alarm->expires);
-	base->timer._softexpires = ktime_add(base->delta, alarm->softexpires);
-	hrtimer_start_expires(&base->timer, HRTIMER_MODE_ABS);
-}
-
-static void alarm_enqueue_locked(struct android_alarm *alarm)
-{
-	struct alarm_queue *base = &alarms[alarm->type];
-	struct rb_node **link = &base->alarms.rb_node;
-	struct rb_node *parent = NULL;
-	struct android_alarm *entry;
-	int leftmost = 1;
-	bool was_first = false;
-
-	pr_alarm(FLOW, "added alarm, type %d, func %pF at %lld\n",
-		alarm->type, alarm->function, ktime_to_ns(alarm->expires));
-
-	if (base->first == &alarm->node) {
-		base->first = rb_next(&alarm->node);
-		was_first = true;
-	}
-	if (!RB_EMPTY_NODE(&alarm->node)) {
-		rb_erase(&alarm->node, &base->alarms);
-		RB_CLEAR_NODE(&alarm->node);
-	}
-
-	while (*link) {
-		parent = *link;
-		entry = rb_entry(parent, struct android_alarm, node);
-		/*
-		* We dont care about collisions. Nodes with
-		* the same expiry time stay together.
-		*/
-		if (alarm->expires.tv64 < entry->expires.tv64) {
-			link = &(*link)->rb_left;
-		} else {
-			link = &(*link)->rb_right;
-			leftmost = 0;
-		}
-	}
-	if (leftmost)
-		base->first = &alarm->node;
-	if (leftmost || was_first)
-		update_timer_locked(base, was_first);
-
-	rb_link_node(&alarm->node, parent, link);
-	rb_insert_color(&alarm->node, &base->alarms);
-}
-
-/**
- * android_alarm_init - initialize an alarm
- * @alarm:	the alarm to be initialized
- * @type:	the alarm type to be used
- * @function:	alarm callback function
- */
-void android_alarm_init(struct android_alarm *alarm,
-	enum android_alarm_type type, void (*function)(struct android_alarm *))
-{
-	RB_CLEAR_NODE(&alarm->node);
-	alarm->type = type;
-	alarm->function = function;
-
-	pr_alarm(FLOW, "created alarm, type %d, func %pF\n", type, function);
-}
-
-
-/**
- * android_alarm_start_range - (re)start an alarm
- * @alarm:	the alarm to be added
- * @start:	earliest expiry time
- * @end:	expiry time
- */
-void android_alarm_start_range(struct android_alarm *alarm, ktime_t start,
-								ktime_t end)
-{
-	unsigned long flags;
-
-	spin_lock_irqsave(&alarm_slock, flags);
-	alarm->softexpires = start;
-	alarm->expires = end;
-	alarm_enqueue_locked(alarm);
-	spin_unlock_irqrestore(&alarm_slock, flags);
-}
-
-/**
- * android_alarm_try_to_cancel - try to deactivate an alarm
- * @alarm:	alarm to stop
- *
- * Returns:
- *  0 when the alarm was not active
- *  1 when the alarm was active
- * -1 when the alarm may currently be excuting the callback function and
- *    cannot be stopped (it may also be inactive)
- */
-int android_alarm_try_to_cancel(struct android_alarm *alarm)
-{
-	struct alarm_queue *base = &alarms[alarm->type];
-	unsigned long flags;
-	bool first = false;
-	int ret = 0;
-
-	spin_lock_irqsave(&alarm_slock, flags);
-	if (!RB_EMPTY_NODE(&alarm->node)) {
-		pr_alarm(FLOW, "canceled alarm, type %d, func %pF at %lld\n",
-			alarm->type, alarm->function,
-			ktime_to_ns(alarm->expires));
-		ret = 1;
-		if (base->first == &alarm->node) {
-			base->first = rb_next(&alarm->node);
-			first = true;
-		}
-		rb_erase(&alarm->node, &base->alarms);
-		RB_CLEAR_NODE(&alarm->node);
-		if (first)
-			update_timer_locked(base, true);
-	} else
-		pr_alarm(FLOW, "tried to cancel alarm, type %d, func %pF\n",
-			alarm->type, alarm->function);
-	spin_unlock_irqrestore(&alarm_slock, flags);
-	if (!ret && hrtimer_callback_running(&base->timer))
-		ret = -1;
-	return ret;
-}
-
-/**
- * android_alarm_cancel - cancel an alarm and wait for the handler to finish.
- * @alarm:	the alarm to be cancelled
- *
- * Returns:
- *  0 when the alarm was not active
- *  1 when the alarm was active
- */
-int android_alarm_cancel(struct android_alarm *alarm)
-{
-	for (;;) {
-		int ret = android_alarm_try_to_cancel(alarm);
-		if (ret >= 0)
-			return ret;
-		cpu_relax();
-	}
-}
-
-/**
- * alarm_set_rtc - set the kernel and rtc walltime
- * @new_time:	timespec value containing the new time
- */
-int android_alarm_set_rtc(struct timespec new_time)
-{
-	int i;
-	int ret;
-	unsigned long flags;
-	struct rtc_time rtc_new_rtc_time;
-	struct timespec tmp_time;
-
-	rtc_time_to_tm(new_time.tv_sec, &rtc_new_rtc_time);
-
-	pr_alarm(TSET, "set rtc %ld %ld - rtc %02d:%02d:%02d %02d/%02d/%04d\n",
-		new_time.tv_sec, new_time.tv_nsec,
-		rtc_new_rtc_time.tm_hour, rtc_new_rtc_time.tm_min,
-		rtc_new_rtc_time.tm_sec, rtc_new_rtc_time.tm_mon + 1,
-		rtc_new_rtc_time.tm_mday,
-		rtc_new_rtc_time.tm_year + 1900);
-
-	mutex_lock(&alarm_setrtc_mutex);
-	spin_lock_irqsave(&alarm_slock, flags);
-	wake_lock(&alarm_rtc_wake_lock);
-	getnstimeofday(&tmp_time);
-	for (i = 0; i < ANDROID_ALARM_SYSTEMTIME; i++) {
-		hrtimer_try_to_cancel(&alarms[i].timer);
-		alarms[i].stopped = true;
-		alarms[i].stopped_time = timespec_to_ktime(tmp_time);
-	}
-	alarms[ANDROID_ALARM_ELAPSED_REALTIME_WAKEUP].delta =
-		alarms[ANDROID_ALARM_ELAPSED_REALTIME].delta =
-		ktime_sub(alarms[ANDROID_ALARM_ELAPSED_REALTIME].delta,
-			timespec_to_ktime(timespec_sub(tmp_time, new_time)));
-	spin_unlock_irqrestore(&alarm_slock, flags);
-	ret = do_settimeofday(&new_time);
-	spin_lock_irqsave(&alarm_slock, flags);
-	for (i = 0; i < ANDROID_ALARM_SYSTEMTIME; i++) {
-		alarms[i].stopped = false;
-		update_timer_locked(&alarms[i], false);
-	}
-	spin_unlock_irqrestore(&alarm_slock, flags);
-	if (ret < 0) {
-		pr_alarm(ERROR, "alarm_set_rtc: Failed to set time\n");
-		goto err;
-	}
-	if (!alarm_rtc_dev) {
-		pr_alarm(ERROR,
-			"alarm_set_rtc: no RTC, time will be lost on reboot\n");
-		goto err;
-	}
-	ret = rtc_set_time(alarm_rtc_dev, &rtc_new_rtc_time);
-	if (ret < 0)
-		pr_alarm(ERROR, "alarm_set_rtc: "
-			"Failed to set RTC, time will be lost on reboot\n");
-err:
-	wake_unlock(&alarm_rtc_wake_lock);
-	mutex_unlock(&alarm_setrtc_mutex);
-	return ret;
-}
-
-/**
- * alarm_get_elapsed_realtime - get the elapsed real time in ktime_t format
- *
- * returns the time in ktime_t format
- */
-ktime_t alarm_get_elapsed_realtime(void)
-{
-	ktime_t now;
-	unsigned long flags;
-	struct alarm_queue *base = &alarms[ANDROID_ALARM_ELAPSED_REALTIME];
-
-	spin_lock_irqsave(&alarm_slock, flags);
-	now = base->stopped ? base->stopped_time : ktime_get_real();
-	now = ktime_sub(now, base->delta);
-	spin_unlock_irqrestore(&alarm_slock, flags);
-	return now;
-}
-
-static enum hrtimer_restart alarm_timer_triggered(struct hrtimer *timer)
-{
-	struct alarm_queue *base;
-	struct android_alarm *alarm;
-	unsigned long flags;
-	ktime_t now;
-
-	spin_lock_irqsave(&alarm_slock, flags);
-
-	base = container_of(timer, struct alarm_queue, timer);
-	now = base->stopped ? base->stopped_time : hrtimer_cb_get_time(timer);
-	now = ktime_sub(now, base->delta);
-
-	pr_alarm(INT, "alarm_timer_triggered type %ld at %lld\n",
-		base - alarms, ktime_to_ns(now));
-
-	while (base->first) {
-		alarm = container_of(base->first, struct android_alarm, node);
-		if (alarm->softexpires.tv64 > now.tv64) {
-			pr_alarm(FLOW, "don't call alarm, %pF, %lld (s %lld)\n",
-				alarm->function, ktime_to_ns(alarm->expires),
-				ktime_to_ns(alarm->softexpires));
-			break;
-		}
-		base->first = rb_next(&alarm->node);
-		rb_erase(&alarm->node, &base->alarms);
-		RB_CLEAR_NODE(&alarm->node);
-		pr_alarm(CALL, "call alarm, type %d, func %pF, %lld (s %lld)\n",
-			alarm->type, alarm->function,
-			ktime_to_ns(alarm->expires),
-			ktime_to_ns(alarm->softexpires));
-		spin_unlock_irqrestore(&alarm_slock, flags);
-		alarm->function(alarm);
-		spin_lock_irqsave(&alarm_slock, flags);
-	}
-	if (!base->first)
-		pr_alarm(FLOW, "no more alarms of type %ld\n", base - alarms);
-	update_timer_locked(base, true);
-	spin_unlock_irqrestore(&alarm_slock, flags);
-	return HRTIMER_NORESTART;
-}
-
-static void alarm_triggered_func(void *p)
-{
-	struct rtc_device *rtc = alarm_rtc_dev;
-	if (!(rtc->irq_data & RTC_AF))
-		return;
-	pr_alarm(INT, "rtc alarm triggered\n");
-	wake_lock_timeout(&alarm_rtc_wake_lock, 1 * HZ);
-}
-
-static int alarm_suspend(struct platform_device *pdev, pm_message_t state)
-{
-	int                 err = 0;
-	unsigned long       flags;
-	struct rtc_wkalrm   rtc_alarm;
-	struct rtc_time     rtc_current_rtc_time;
-	unsigned long       rtc_current_time;
-	unsigned long       rtc_alarm_time;
-	struct timespec     rtc_delta;
-	struct timespec     wall_time;
-	struct alarm_queue *wakeup_queue = NULL;
-	struct alarm_queue *tmp_queue = NULL;
-
-	pr_alarm(SUSPEND, "alarm_suspend(%p, %d)\n", pdev, state.event);
-
-	spin_lock_irqsave(&alarm_slock, flags);
-	suspended = true;
-	spin_unlock_irqrestore(&alarm_slock, flags);
-
-	hrtimer_cancel(&alarms[ANDROID_ALARM_RTC_WAKEUP].timer);
-	hrtimer_cancel(&alarms[
-			ANDROID_ALARM_ELAPSED_REALTIME_WAKEUP].timer);
-
-	tmp_queue = &alarms[ANDROID_ALARM_RTC_WAKEUP];
-	if (tmp_queue->first)
-		wakeup_queue = tmp_queue;
-	tmp_queue = &alarms[ANDROID_ALARM_ELAPSED_REALTIME_WAKEUP];
-	if (tmp_queue->first && (!wakeup_queue ||
-				hrtimer_get_expires(&tmp_queue->timer).tv64 <
-				hrtimer_get_expires(&wakeup_queue->timer).tv64))
-		wakeup_queue = tmp_queue;
-	if (wakeup_queue) {
-		rtc_read_time(alarm_rtc_dev, &rtc_current_rtc_time);
-		getnstimeofday(&wall_time);
-		rtc_tm_to_time(&rtc_current_rtc_time, &rtc_current_time);
-		set_normalized_timespec(&rtc_delta,
-					wall_time.tv_sec - rtc_current_time,
-					wall_time.tv_nsec);
-
-		rtc_alarm_time = timespec_sub(ktime_to_timespec(
-			hrtimer_get_expires(&wakeup_queue->timer)),
-			rtc_delta).tv_sec;
-
-		rtc_time_to_tm(rtc_alarm_time, &rtc_alarm.time);
-		rtc_alarm.enabled = 1;
-		rtc_set_alarm(alarm_rtc_dev, &rtc_alarm);
-		rtc_read_time(alarm_rtc_dev, &rtc_current_rtc_time);
-		rtc_tm_to_time(&rtc_current_rtc_time, &rtc_current_time);
-		pr_alarm(SUSPEND,
-			"rtc alarm set at %ld, now %ld, rtc delta %ld.%09ld\n",
-			rtc_alarm_time, rtc_current_time,
-			rtc_delta.tv_sec, rtc_delta.tv_nsec);
-		if (rtc_current_time + 1 >= rtc_alarm_time) {
-			pr_alarm(SUSPEND, "alarm about to go off\n");
-			memset(&rtc_alarm, 0, sizeof(rtc_alarm));
-			rtc_alarm.enabled = 0;
-			rtc_set_alarm(alarm_rtc_dev, &rtc_alarm);
-
-			spin_lock_irqsave(&alarm_slock, flags);
-			suspended = false;
-			wake_lock_timeout(&alarm_rtc_wake_lock, 2 * HZ);
-			update_timer_locked(&alarms[ANDROID_ALARM_RTC_WAKEUP],
-									false);
-			update_timer_locked(&alarms[
-				ANDROID_ALARM_ELAPSED_REALTIME_WAKEUP], false);
-			err = -EBUSY;
-			spin_unlock_irqrestore(&alarm_slock, flags);
-		}
-	}
-	return err;
-}
-
-static int alarm_resume(struct platform_device *pdev)
-{
-	struct rtc_wkalrm alarm;
-	unsigned long       flags;
-
-	pr_alarm(SUSPEND, "alarm_resume(%p)\n", pdev);
-
-	memset(&alarm, 0, sizeof(alarm));
-	alarm.enabled = 0;
-	rtc_set_alarm(alarm_rtc_dev, &alarm);
-
-	spin_lock_irqsave(&alarm_slock, flags);
-	suspended = false;
-	update_timer_locked(&alarms[ANDROID_ALARM_RTC_WAKEUP], false);
-	update_timer_locked(&alarms[ANDROID_ALARM_ELAPSED_REALTIME_WAKEUP],
-									false);
-	spin_unlock_irqrestore(&alarm_slock, flags);
-
-	return 0;
-}
-
-static struct rtc_task alarm_rtc_task = {
-	.func = alarm_triggered_func
-};
-
-static int rtc_alarm_add_device(struct device *dev,
-				struct class_interface *class_intf)
-{
-	int err;
-	struct rtc_device *rtc = to_rtc_device(dev);
-
-	mutex_lock(&alarm_setrtc_mutex);
-
-	if (alarm_rtc_dev) {
-		err = -EBUSY;
-		goto err1;
-	}
-
-	alarm_platform_dev =
-		platform_device_register_simple("alarm", -1, NULL, 0);
-	if (IS_ERR(alarm_platform_dev)) {
-		err = PTR_ERR(alarm_platform_dev);
-		goto err2;
-	}
-	err = rtc_irq_register(rtc, &alarm_rtc_task);
-	if (err)
-		goto err3;
-	alarm_rtc_dev = rtc;
-	pr_alarm(INIT_STATUS, "using rtc device, %s, for alarms", rtc->name);
-	mutex_unlock(&alarm_setrtc_mutex);
-
-	return 0;
-
-err3:
-	platform_device_unregister(alarm_platform_dev);
-err2:
-err1:
-	mutex_unlock(&alarm_setrtc_mutex);
-	return err;
-}
-
-static void rtc_alarm_remove_device(struct device *dev,
-				    struct class_interface *class_intf)
-{
-	if (dev == &alarm_rtc_dev->dev) {
-		pr_alarm(INIT_STATUS, "lost rtc device for alarms");
-		rtc_irq_unregister(alarm_rtc_dev, &alarm_rtc_task);
-		platform_device_unregister(alarm_platform_dev);
-		alarm_rtc_dev = NULL;
-	}
-}
-
-static struct class_interface rtc_alarm_interface = {
-	.add_dev = &rtc_alarm_add_device,
-	.remove_dev = &rtc_alarm_remove_device,
-};
-
-static struct platform_driver alarm_driver = {
-	.suspend = alarm_suspend,
-	.resume = alarm_resume,
-	.driver = {
-		.name = "alarm"
-	}
-};
-
-static int __init alarm_late_init(void)
-{
-	unsigned long   flags;
-	struct timespec tmp_time, system_time;
-
-	/* this needs to run after the rtc is read at boot */
-	spin_lock_irqsave(&alarm_slock, flags);
-	/* We read the current rtc and system time so we can later calulate
-	 * elasped realtime to be (boot_systemtime + rtc - boot_rtc) ==
-	 * (rtc - (boot_rtc - boot_systemtime))
-	 */
-	getnstimeofday(&tmp_time);
-	ktime_get_ts(&system_time);
-	alarms[ANDROID_ALARM_ELAPSED_REALTIME_WAKEUP].delta =
-		alarms[ANDROID_ALARM_ELAPSED_REALTIME].delta =
-			timespec_to_ktime(timespec_sub(tmp_time, system_time));
-
-	spin_unlock_irqrestore(&alarm_slock, flags);
-	return 0;
-}
-
-static int __init alarm_driver_init(void)
-{
-	int err;
-	int i;
-
-	for (i = 0; i < ANDROID_ALARM_SYSTEMTIME; i++) {
-		hrtimer_init(&alarms[i].timer,
-				CLOCK_REALTIME, HRTIMER_MODE_ABS);
-		alarms[i].timer.function = alarm_timer_triggered;
-	}
-	hrtimer_init(&alarms[ANDROID_ALARM_SYSTEMTIME].timer,
-		     CLOCK_MONOTONIC, HRTIMER_MODE_ABS);
-	alarms[ANDROID_ALARM_SYSTEMTIME].timer.function = alarm_timer_triggered;
-	err = platform_driver_register(&alarm_driver);
-	if (err < 0)
-		goto err1;
-	wake_lock_init(&alarm_rtc_wake_lock, WAKE_LOCK_SUSPEND, "alarm_rtc");
-	rtc_alarm_interface.class = rtc_class;
-	err = class_interface_register(&rtc_alarm_interface);
-	if (err < 0)
-		goto err2;
-
-	return 0;
-
-err2:
-	wake_lock_destroy(&alarm_rtc_wake_lock);
-	platform_driver_unregister(&alarm_driver);
-err1:
-	return err;
-}
-
-static void  __exit alarm_exit(void)
-{
-	class_interface_unregister(&rtc_alarm_interface);
-	wake_lock_destroy(&alarm_rtc_wake_lock);
-	platform_driver_unregister(&alarm_driver);
-}
-
-late_initcall(alarm_late_init);
-module_init(alarm_driver_init);
-module_exit(alarm_exit);
-
diff --git a/drivers/staging/android/android_alarm.h b/drivers/staging/android/android_alarm.h
index 6eecbde2ef6..d0cafd63719 100644
--- a/drivers/staging/android/android_alarm.h
+++ b/drivers/staging/android/android_alarm.h
@@ -33,65 +33,6 @@ enum android_alarm_type {
 	/* ANDROID_ALARM_TIME_CHANGE = 16 */
 };
 
-#ifdef __KERNEL__
-
-#include <linux/ktime.h>
-#include <linux/rbtree.h>
-
-/*
- * The alarm interface is similar to the hrtimer interface but adds support
- * for wakeup from suspend. It also adds an elapsed realtime clock that can
- * be used for periodic timers that need to keep runing while the system is
- * suspended and not be disrupted when the wall time is set.
- */
-
-/**
- * struct alarm - the basic alarm structure
- * @node:	red black tree node for time ordered insertion
- * @type:	alarm type. rtc/elapsed-realtime/systemtime, wakeup/non-wakeup.
- * @softexpires: the absolute earliest expiry time of the alarm.
- * @expires:	the absolute expiry time.
- * @function:	alarm expiry callback function
- *
- * The alarm structure must be initialized by alarm_init()
- *
- */
-
-struct android_alarm {
-	struct rb_node		node;
-	enum android_alarm_type type;
-	ktime_t			softexpires;
-	ktime_t			expires;
-	void			(*function)(struct android_alarm *);
-};
-
-void android_alarm_init(struct android_alarm *alarm,
-	enum android_alarm_type type, void (*function)(struct android_alarm *));
-void android_alarm_start_range(struct android_alarm *alarm, ktime_t start,
-								ktime_t end);
-int android_alarm_try_to_cancel(struct android_alarm *alarm);
-int android_alarm_cancel(struct android_alarm *alarm);
-ktime_t alarm_get_elapsed_realtime(void);
-
-/* set rtc while preserving elapsed realtime */
-int android_alarm_set_rtc(const struct timespec ts);
-
-#ifdef CONFIG_ANDROID_ALARM_OLDDRV_COMPAT
-/*
- * Some older drivers depend on the old API,
- * so provide compatability macros for now.
- */
-#define alarm android_alarm
-#define alarm_init(x, y, z) android_alarm_init(x, y, z)
-#define alarm_start_range(x, y, z) android_alarm_start_range(x, y, z)
-#define alarm_try_to_cancel(x) android_alarm_try_to_cancel(x)
-#define alarm_cancel(x) android_alarm_cancel(x)
-#define alarm_set_rtc(x) android_alarm_set_rtc(x)
-#endif
-
-
-#endif
-
 enum android_alarm_return_flags {
 	ANDROID_ALARM_RTC_WAKEUP_MASK = 1U << ANDROID_ALARM_RTC_WAKEUP,
 	ANDROID_ALARM_RTC_MASK = 1U << ANDROID_ALARM_RTC,
diff --git a/drivers/staging/android/ashmem.c b/drivers/staging/android/ashmem.c
index 9f1f27e7c86..e84dbecd099 100644
--- a/drivers/staging/android/ashmem.c
+++ b/drivers/staging/android/ashmem.c
@@ -19,6 +19,7 @@
 #include <linux/module.h>
 #include <linux/file.h>
 #include <linux/fs.h>
+#include <linux/falloc.h>
 #include <linux/miscdevice.h>
 #include <linux/security.h>
 #include <linux/mm.h>
@@ -269,7 +270,7 @@ out:
 	return ret;
 }
 
-static inline unsigned long calc_vm_may_flags(unsigned long prot)
+static inline vm_flags_t calc_vm_may_flags(unsigned long prot)
 {
 	return _calc_vm_trans(prot, PROT_READ,  VM_MAYREAD) |
 	       _calc_vm_trans(prot, PROT_WRITE, VM_MAYWRITE) |
@@ -363,11 +364,12 @@ static int ashmem_shrink(struct shrinker *s, struct shrink_control *sc)
 
 	mutex_lock(&ashmem_mutex);
 	list_for_each_entry_safe(range, next, &ashmem_lru_list, lru) {
-		struct inode *inode = range->asma->file->f_dentry->d_inode;
 		loff_t start = range->pgstart * PAGE_SIZE;
-		loff_t end = (range->pgend + 1) * PAGE_SIZE - 1;
+		loff_t end = (range->pgend + 1) * PAGE_SIZE;
 
-		vmtruncate_range(inode, start, end);
+		do_fallocate(range->asma->file,
+				FALLOC_FL_PUNCH_HOLE | FALLOC_FL_KEEP_SIZE,
+				start, end - start);
 		range->purged = ASHMEM_WAS_PURGED;
 		lru_del(range);
 
diff --git a/drivers/staging/android/binder.h b/drivers/staging/android/binder.h
index 25ab6f2759e..2f7d195d8b1 100644
--- a/drivers/staging/android/binder.h
+++ b/drivers/staging/android/binder.h
@@ -53,17 +53,17 @@ struct flat_binder_object {
 
 	/* 8 bytes of data. */
 	union {
-		void		*binder;	/* local object */
+		void __user	*binder;	/* local object */
 		signed long	handle;		/* remote object */
 	};
 
 	/* extra data associated with local object */
-	void			*cookie;
+	void __user		*cookie;
 };
 
 /*
  * On 64-bit platforms where user code may run in 32-bits the driver must
- * translate the buffer (and local binder) addresses apropriately.
+ * translate the buffer (and local binder) addresses appropriately.
  */
 
 struct binder_write_read {
@@ -139,9 +139,9 @@ struct binder_transaction_data {
 	union {
 		struct {
 			/* transaction data */
-			const void	*buffer;
+			const void __user	*buffer;
 			/* offsets from buffer to flat_binder_object structs */
-			const void	*offsets;
+			const void __user	*offsets;
 		} ptr;
 		uint8_t	buf[8];
 	} data;
diff --git a/drivers/staging/android/logger.c b/drivers/staging/android/logger.c
index ea69b6a77da..b2e71c6fd17 100644
--- a/drivers/staging/android/logger.c
+++ b/drivers/staging/android/logger.c
@@ -25,6 +25,7 @@
 #include <linux/poll.h>
 #include <linux/slab.h>
 #include <linux/time.h>
+#include <linux/vmalloc.h>
 #include "logger.h"
 
 #include <asm/ioctls.h>
@@ -45,8 +46,12 @@ struct logger_log {
 	size_t			w_off;	/* current write head offset */
 	size_t			head;	/* new readers start here */
 	size_t			size;	/* size of the log */
+	struct list_head	logs;	/* list of log channels (myself)*/
 };
 
+static LIST_HEAD(log_list);
+
+
 /*
  * struct logger_reader - a logging device open for reading
  *
@@ -60,9 +65,9 @@ struct logger_reader {
 };
 
 /* logger_offset - returns index 'n' into the log via (optimized) modulus */
-size_t logger_offset(struct logger_log *log, size_t n)
+static size_t logger_offset(struct logger_log *log, size_t n)
 {
-	return n & (log->size-1);
+	return n & (log->size - 1);
 }
 
 
@@ -348,7 +353,7 @@ static ssize_t do_write_log_from_user(struct logger_log *log,
  * writev(), and aio_write(). Writes are our fast path, and we try to optimize
  * them above all else.
  */
-ssize_t logger_aio_write(struct kiocb *iocb, const struct iovec *iov,
+static ssize_t logger_aio_write(struct kiocb *iocb, const struct iovec *iov,
 			 unsigned long nr_segs, loff_t ppos)
 {
 	struct logger_log *log = file_get_log(iocb->ki_filp);
@@ -408,7 +413,15 @@ ssize_t logger_aio_write(struct kiocb *iocb, const struct iovec *iov,
 	return ret;
 }
 
-static struct logger_log *get_log_from_minor(int);
+static struct logger_log *get_log_from_minor(int minor)
+{
+	struct logger_log *log;
+
+	list_for_each_entry(log, &log_list, logs)
+		if (log->misc.minor == minor)
+			return log;
+	return NULL;
+}
 
 /*
  * logger_open - the log's open() file operation
@@ -565,80 +578,84 @@ static const struct file_operations logger_fops = {
 };
 
 /*
- * Defines a log structure with name 'NAME' and a size of 'SIZE' bytes, which
- * must be a power of two, greater than LOGGER_ENTRY_MAX_LEN, and less than
- * LONG_MAX minus LOGGER_ENTRY_MAX_LEN.
+ * Log size must be a power of two, greater than LOGGER_ENTRY_MAX_LEN,
+ * and less than LONG_MAX minus LOGGER_ENTRY_MAX_LEN.
  */
-#define DEFINE_LOGGER_DEVICE(VAR, NAME, SIZE) \
-static unsigned char _buf_ ## VAR[SIZE]; \
-static struct logger_log VAR = { \
-	.buffer = _buf_ ## VAR, \
-	.misc = { \
-		.minor = MISC_DYNAMIC_MINOR, \
-		.name = NAME, \
-		.fops = &logger_fops, \
-		.parent = NULL, \
-	}, \
-	.wq = __WAIT_QUEUE_HEAD_INITIALIZER(VAR .wq), \
-	.readers = LIST_HEAD_INIT(VAR .readers), \
-	.mutex = __MUTEX_INITIALIZER(VAR .mutex), \
-	.w_off = 0, \
-	.head = 0, \
-	.size = SIZE, \
-};
+static int __init create_log(char *log_name, int size)
+{
+	int ret = 0;
+	struct logger_log *log;
+	unsigned char *buffer;
 
-DEFINE_LOGGER_DEVICE(log_main, LOGGER_LOG_MAIN, 256*1024)
-DEFINE_LOGGER_DEVICE(log_events, LOGGER_LOG_EVENTS, 256*1024)
-DEFINE_LOGGER_DEVICE(log_radio, LOGGER_LOG_RADIO, 256*1024)
-DEFINE_LOGGER_DEVICE(log_system, LOGGER_LOG_SYSTEM, 256*1024)
+	buffer = vmalloc(size);
+	if (buffer == NULL)
+		return -ENOMEM;
 
-static struct logger_log *get_log_from_minor(int minor)
-{
-	if (log_main.misc.minor == minor)
-		return &log_main;
-	if (log_events.misc.minor == minor)
-		return &log_events;
-	if (log_radio.misc.minor == minor)
-		return &log_radio;
-	if (log_system.misc.minor == minor)
-		return &log_system;
-	return NULL;
-}
+	log = kzalloc(sizeof(struct logger_log), GFP_KERNEL);
+	if (log == NULL) {
+		ret = -ENOMEM;
+		goto out_free_buffer;
+	}
+	log->buffer = buffer;
 
-static int __init init_log(struct logger_log *log)
-{
-	int ret;
+	log->misc.minor = MISC_DYNAMIC_MINOR;
+	log->misc.name = kstrdup(log_name, GFP_KERNEL);
+	if (log->misc.name == NULL) {
+		ret = -ENOMEM;
+		goto out_free_log;
+	}
+
+	log->misc.fops = &logger_fops;
+	log->misc.parent = NULL;
 
+	init_waitqueue_head(&log->wq);
+	INIT_LIST_HEAD(&log->readers);
+	mutex_init(&log->mutex);
+	log->w_off = 0;
+	log->head = 0;
+	log->size = size;
+
+	INIT_LIST_HEAD(&log->logs);
+	list_add_tail(&log->logs, &log_list);
+
+	/* finally, initialize the misc device for this log */
 	ret = misc_register(&log->misc);
 	if (unlikely(ret)) {
 		printk(KERN_ERR "logger: failed to register misc "
 		       "device for log '%s'!\n", log->misc.name);
-		return ret;
+		goto out_free_log;
 	}
 
 	printk(KERN_INFO "logger: created %luK log '%s'\n",
 	       (unsigned long) log->size >> 10, log->misc.name);
 
 	return 0;
+
+out_free_log:
+	kfree(log);
+
+out_free_buffer:
+	vfree(buffer);
+	return ret;
 }
 
 static int __init logger_init(void)
 {
 	int ret;
 
-	ret = init_log(&log_main);
+	ret = create_log(LOGGER_LOG_MAIN, 256*1024);
 	if (unlikely(ret))
 		goto out;
 
-	ret = init_log(&log_events);
+	ret = create_log(LOGGER_LOG_EVENTS, 256*1024);
 	if (unlikely(ret))
 		goto out;
 
-	ret = init_log(&log_radio);
+	ret = create_log(LOGGER_LOG_RADIO, 256*1024);
 	if (unlikely(ret))
 		goto out;
 
-	ret = init_log(&log_system);
+	ret = create_log(LOGGER_LOG_SYSTEM, 256*1024);
 	if (unlikely(ret))
 		goto out;
 
diff --git a/drivers/staging/android/persistent_ram.c b/drivers/staging/android/persistent_ram.c
deleted file mode 100644
index 8d8c1e33e0f..00000000000
--- a/drivers/staging/android/persistent_ram.c
+++ /dev/null
@@ -1,473 +0,0 @@
-/*
- * 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/device.h>
-#include <linux/err.h>
-#include <linux/errno.h>
-#include <linux/kernel.h>
-#include <linux/init.h>
-#include <linux/io.h>
-#include <linux/list.h>
-#include <linux/memblock.h>
-#include <linux/rslib.h>
-#include <linux/slab.h>
-#include <linux/vmalloc.h>
-#include "persistent_ram.h"
-
-struct persistent_ram_buffer {
-	uint32_t    sig;
-	atomic_t    start;
-	atomic_t    size;
-	uint8_t     data[0];
-};
-
-#define PERSISTENT_RAM_SIG (0x43474244) /* DBGC */
-
-static __initdata LIST_HEAD(persistent_ram_list);
-
-static inline size_t buffer_size(struct persistent_ram_zone *prz)
-{
-	return atomic_read(&prz->buffer->size);
-}
-
-static inline size_t buffer_start(struct persistent_ram_zone *prz)
-{
-	return atomic_read(&prz->buffer->start);
-}
-
-/* increase and wrap the start pointer, returning the old value */
-static inline size_t buffer_start_add(struct persistent_ram_zone *prz, size_t a)
-{
-	int old;
-	int new;
-
-	do {
-		old = atomic_read(&prz->buffer->start);
-		new = old + a;
-		while (unlikely(new > prz->buffer_size))
-			new -= prz->buffer_size;
-	} while (atomic_cmpxchg(&prz->buffer->start, old, new) != old);
-
-	return old;
-}
-
-/* increase the size counter until it hits the max size */
-static inline void buffer_size_add(struct persistent_ram_zone *prz, size_t a)
-{
-	size_t old;
-	size_t new;
-
-	if (atomic_read(&prz->buffer->size) == prz->buffer_size)
-		return;
-
-	do {
-		old = atomic_read(&prz->buffer->size);
-		new = old + a;
-		if (new > prz->buffer_size)
-			new = prz->buffer_size;
-	} while (atomic_cmpxchg(&prz->buffer->size, old, new) != old);
-}
-
-/* increase the size counter, retuning an error if it hits the max size */
-static inline ssize_t buffer_size_add_clamp(struct persistent_ram_zone *prz,
-	size_t a)
-{
-	size_t old;
-	size_t new;
-
-	do {
-		old = atomic_read(&prz->buffer->size);
-		new = old + a;
-		if (new > prz->buffer_size)
-			return -ENOMEM;
-	} while (atomic_cmpxchg(&prz->buffer->size, old, new) != old);
-
-	return 0;
-}
-
-static void notrace persistent_ram_encode_rs8(struct persistent_ram_zone *prz,
-	uint8_t *data, size_t len, uint8_t *ecc)
-{
-	int i;
-	uint16_t par[prz->ecc_size];
-
-	/* Initialize the parity buffer */
-	memset(par, 0, sizeof(par));
-	encode_rs8(prz->rs_decoder, data, len, par, 0);
-	for (i = 0; i < prz->ecc_size; i++)
-		ecc[i] = par[i];
-}
-
-static int persistent_ram_decode_rs8(struct persistent_ram_zone *prz,
-	void *data, size_t len, uint8_t *ecc)
-{
-	int i;
-	uint16_t par[prz->ecc_size];
-
-	for (i = 0; i < prz->ecc_size; i++)
-		par[i] = ecc[i];
-	return decode_rs8(prz->rs_decoder, data, par, len,
-				NULL, 0, NULL, 0, NULL);
-}
-
-static void notrace persistent_ram_update_ecc(struct persistent_ram_zone *prz,
-	unsigned int start, unsigned int count)
-{
-	struct persistent_ram_buffer *buffer = prz->buffer;
-	uint8_t *buffer_end = buffer->data + prz->buffer_size;
-	uint8_t *block;
-	uint8_t *par;
-	int ecc_block_size = prz->ecc_block_size;
-	int ecc_size = prz->ecc_size;
-	int size = prz->ecc_block_size;
-
-	if (!prz->ecc)
-		return;
-
-	block = buffer->data + (start & ~(ecc_block_size - 1));
-	par = prz->par_buffer + (start / ecc_block_size) * prz->ecc_size;
-
-	do {
-		if (block + ecc_block_size > buffer_end)
-			size = buffer_end - block;
-		persistent_ram_encode_rs8(prz, block, size, par);
-		block += ecc_block_size;
-		par += ecc_size;
-	} while (block < buffer->data + start + count);
-}
-
-static void persistent_ram_update_header_ecc(struct persistent_ram_zone *prz)
-{
-	struct persistent_ram_buffer *buffer = prz->buffer;
-
-	if (!prz->ecc)
-		return;
-
-	persistent_ram_encode_rs8(prz, (uint8_t *)buffer, sizeof(*buffer),
-				  prz->par_header);
-}
-
-static void persistent_ram_ecc_old(struct persistent_ram_zone *prz)
-{
-	struct persistent_ram_buffer *buffer = prz->buffer;
-	uint8_t *block;
-	uint8_t *par;
-
-	if (!prz->ecc)
-		return;
-
-	block = buffer->data;
-	par = prz->par_buffer;
-	while (block < buffer->data + buffer_size(prz)) {
-		int numerr;
-		int size = prz->ecc_block_size;
-		if (block + size > buffer->data + prz->buffer_size)
-			size = buffer->data + prz->buffer_size - block;
-		numerr = persistent_ram_decode_rs8(prz, block, size, par);
-		if (numerr > 0) {
-			pr_devel("persistent_ram: error in block %p, %d\n",
-			       block, numerr);
-			prz->corrected_bytes += numerr;
-		} else if (numerr < 0) {
-			pr_devel("persistent_ram: uncorrectable error in block %p\n",
-				block);
-			prz->bad_blocks++;
-		}
-		block += prz->ecc_block_size;
-		par += prz->ecc_size;
-	}
-}
-
-static int persistent_ram_init_ecc(struct persistent_ram_zone *prz,
-	size_t buffer_size)
-{
-	int numerr;
-	struct persistent_ram_buffer *buffer = prz->buffer;
-	int ecc_blocks;
-
-	if (!prz->ecc)
-		return 0;
-
-	prz->ecc_block_size = 128;
-	prz->ecc_size = 16;
-	prz->ecc_symsize = 8;
-	prz->ecc_poly = 0x11d;
-
-	ecc_blocks = DIV_ROUND_UP(prz->buffer_size, prz->ecc_block_size);
-	prz->buffer_size -= (ecc_blocks + 1) * prz->ecc_size;
-
-	if (prz->buffer_size > buffer_size) {
-		pr_err("persistent_ram: invalid size %zu, non-ecc datasize %zu\n",
-		       buffer_size, prz->buffer_size);
-		return -EINVAL;
-	}
-
-	prz->par_buffer = buffer->data + prz->buffer_size;
-	prz->par_header = prz->par_buffer + ecc_blocks * prz->ecc_size;
-
-	/*
-	 * first consecutive root is 0
-	 * primitive element to generate roots = 1
-	 */
-	prz->rs_decoder = init_rs(prz->ecc_symsize, prz->ecc_poly, 0, 1,
-				  prz->ecc_size);
-	if (prz->rs_decoder == NULL) {
-		pr_info("persistent_ram: init_rs failed\n");
-		return -EINVAL;
-	}
-
-	prz->corrected_bytes = 0;
-	prz->bad_blocks = 0;
-
-	numerr = persistent_ram_decode_rs8(prz, buffer, sizeof(*buffer),
-					   prz->par_header);
-	if (numerr > 0) {
-		pr_info("persistent_ram: error in header, %d\n", numerr);
-		prz->corrected_bytes += numerr;
-	} else if (numerr < 0) {
-		pr_info("persistent_ram: uncorrectable error in header\n");
-		prz->bad_blocks++;
-	}
-
-	return 0;
-}
-
-ssize_t persistent_ram_ecc_string(struct persistent_ram_zone *prz,
-	char *str, size_t len)
-{
-	ssize_t ret;
-
-	if (prz->corrected_bytes || prz->bad_blocks)
-		ret = snprintf(str, len, ""
-			"\n%d Corrected bytes, %d unrecoverable blocks\n",
-			prz->corrected_bytes, prz->bad_blocks);
-	else
-		ret = snprintf(str, len, "\nNo errors detected\n");
-
-	return ret;
-}
-
-static void notrace persistent_ram_update(struct persistent_ram_zone *prz,
-	const void *s, unsigned int start, unsigned int count)
-{
-	struct persistent_ram_buffer *buffer = prz->buffer;
-	memcpy(buffer->data + start, s, count);
-	persistent_ram_update_ecc(prz, start, count);
-}
-
-static void __init
-persistent_ram_save_old(struct persistent_ram_zone *prz)
-{
-	struct persistent_ram_buffer *buffer = prz->buffer;
-	size_t size = buffer_size(prz);
-	size_t start = buffer_start(prz);
-	char *dest;
-
-	persistent_ram_ecc_old(prz);
-
-	dest = kmalloc(size, GFP_KERNEL);
-	if (dest == NULL) {
-		pr_err("persistent_ram: failed to allocate buffer\n");
-		return;
-	}
-
-	prz->old_log = dest;
-	prz->old_log_size = size;
-	memcpy(prz->old_log, &buffer->data[start], size - start);
-	memcpy(prz->old_log + size - start, &buffer->data[0], start);
-}
-
-int notrace persistent_ram_write(struct persistent_ram_zone *prz,
-	const void *s, unsigned int count)
-{
-	int rem;
-	int c = count;
-	size_t start;
-
-	if (unlikely(c > prz->buffer_size)) {
-		s += c - prz->buffer_size;
-		c = prz->buffer_size;
-	}
-
-	buffer_size_add_clamp(prz, c);
-
-	start = buffer_start_add(prz, c);
-
-	rem = prz->buffer_size - start;
-	if (unlikely(rem < c)) {
-		persistent_ram_update(prz, s, start, rem);
-		s += rem;
-		c -= rem;
-		start = 0;
-	}
-	persistent_ram_update(prz, s, start, c);
-
-	persistent_ram_update_header_ecc(prz);
-
-	return count;
-}
-
-size_t persistent_ram_old_size(struct persistent_ram_zone *prz)
-{
-	return prz->old_log_size;
-}
-
-void *persistent_ram_old(struct persistent_ram_zone *prz)
-{
-	return prz->old_log;
-}
-
-void persistent_ram_free_old(struct persistent_ram_zone *prz)
-{
-	kfree(prz->old_log);
-	prz->old_log = NULL;
-	prz->old_log_size = 0;
-}
-
-static int persistent_ram_buffer_map(phys_addr_t start, phys_addr_t size,
-		struct persistent_ram_zone *prz)
-{
-	struct page **pages;
-	phys_addr_t page_start;
-	unsigned int page_count;
-	pgprot_t prot;
-	unsigned int i;
-
-	page_start = start - offset_in_page(start);
-	page_count = DIV_ROUND_UP(size + offset_in_page(start), PAGE_SIZE);
-
-	prot = pgprot_noncached(PAGE_KERNEL);
-
-	pages = kmalloc(sizeof(struct page *) * page_count, GFP_KERNEL);
-	if (!pages) {
-		pr_err("%s: Failed to allocate array for %u pages\n", __func__,
-			page_count);
-		return -ENOMEM;
-	}
-
-	for (i = 0; i < page_count; i++) {
-		phys_addr_t addr = page_start + i * PAGE_SIZE;
-		pages[i] = pfn_to_page(addr >> PAGE_SHIFT);
-	}
-	prz->vaddr = vmap(pages, page_count, VM_MAP, prot);
-	kfree(pages);
-	if (!prz->vaddr) {
-		pr_err("%s: Failed to map %u pages\n", __func__, page_count);
-		return -ENOMEM;
-	}
-
-	prz->buffer = prz->vaddr + offset_in_page(start);
-	prz->buffer_size = size - sizeof(struct persistent_ram_buffer);
-
-	return 0;
-}
-
-static int __init persistent_ram_buffer_init(const char *name,
-		struct persistent_ram_zone *prz)
-{
-	int i;
-	struct persistent_ram *ram;
-	struct persistent_ram_descriptor *desc;
-	phys_addr_t start;
-
-	list_for_each_entry(ram, &persistent_ram_list, node) {
-		start = ram->start;
-		for (i = 0; i < ram->num_descs; i++) {
-			desc = &ram->descs[i];
-			if (!strcmp(desc->name, name))
-				return persistent_ram_buffer_map(start,
-						desc->size, prz);
-			start += desc->size;
-		}
-	}
-
-	return -EINVAL;
-}
-
-static  __init
-struct persistent_ram_zone *__persistent_ram_init(struct device *dev, bool ecc)
-{
-	struct persistent_ram_zone *prz;
-	int ret = -ENOMEM;
-
-	prz = kzalloc(sizeof(struct persistent_ram_zone), GFP_KERNEL);
-	if (!prz) {
-		pr_err("persistent_ram: failed to allocate persistent ram zone\n");
-		goto err;
-	}
-
-	INIT_LIST_HEAD(&prz->node);
-
-	ret = persistent_ram_buffer_init(dev_name(dev), prz);
-	if (ret) {
-		pr_err("persistent_ram: failed to initialize buffer\n");
-		goto err;
-	}
-
-	prz->ecc = ecc;
-	ret = persistent_ram_init_ecc(prz, prz->buffer_size);
-	if (ret)
-		goto err;
-
-	if (prz->buffer->sig == PERSISTENT_RAM_SIG) {
-		if (buffer_size(prz) > prz->buffer_size ||
-		    buffer_start(prz) > buffer_size(prz))
-			pr_info("persistent_ram: found existing invalid buffer,"
-				" size %ld, start %ld\n",
-			       buffer_size(prz), buffer_start(prz));
-		else {
-			pr_info("persistent_ram: found existing buffer,"
-				" size %ld, start %ld\n",
-			       buffer_size(prz), buffer_start(prz));
-			persistent_ram_save_old(prz);
-		}
-	} else {
-		pr_info("persistent_ram: no valid data in buffer"
-			" (sig = 0x%08x)\n", prz->buffer->sig);
-	}
-
-	prz->buffer->sig = PERSISTENT_RAM_SIG;
-	atomic_set(&prz->buffer->start, 0);
-	atomic_set(&prz->buffer->size, 0);
-
-	return prz;
-err:
-	kfree(prz);
-	return ERR_PTR(ret);
-}
-
-struct persistent_ram_zone * __init
-persistent_ram_init_ringbuffer(struct device *dev, bool ecc)
-{
-	return __persistent_ram_init(dev, ecc);
-}
-
-int __init persistent_ram_early_init(struct persistent_ram *ram)
-{
-	int ret;
-
-	ret = memblock_reserve(ram->start, ram->size);
-	if (ret) {
-		pr_err("Failed to reserve persistent memory from %08lx-%08lx\n",
-			(long)ram->start, (long)(ram->start + ram->size - 1));
-		return ret;
-	}
-
-	list_add_tail(&ram->node, &persistent_ram_list);
-
-	pr_info("Initialized persistent memory from %08lx-%08lx\n",
-		(long)ram->start, (long)(ram->start + ram->size - 1));
-
-	return 0;
-}
diff --git a/drivers/staging/android/persistent_ram.h b/drivers/staging/android/persistent_ram.h
deleted file mode 100644
index f41e2086c64..00000000000
--- a/drivers/staging/android/persistent_ram.h
+++ /dev/null
@@ -1,78 +0,0 @@
-/*
- * Copyright (C) 2011 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.
- *
- */
-
-#ifndef __LINUX_PERSISTENT_RAM_H__
-#define __LINUX_PERSISTENT_RAM_H__
-
-#include <linux/device.h>
-#include <linux/kernel.h>
-#include <linux/list.h>
-#include <linux/types.h>
-
-struct persistent_ram_buffer;
-
-struct persistent_ram_descriptor {
-	const char	*name;
-	phys_addr_t	size;
-};
-
-struct persistent_ram {
-	phys_addr_t	start;
-	phys_addr_t	size;
-
-	int					num_descs;
-	struct persistent_ram_descriptor	*descs;
-
-	struct list_head node;
-};
-
-struct persistent_ram_zone {
-	struct list_head node;
-	void *vaddr;
-	struct persistent_ram_buffer *buffer;
-	size_t buffer_size;
-
-	/* ECC correction */
-	bool ecc;
-	char *par_buffer;
-	char *par_header;
-	struct rs_control *rs_decoder;
-	int corrected_bytes;
-	int bad_blocks;
-	int ecc_block_size;
-	int ecc_size;
-	int ecc_symsize;
-	int ecc_poly;
-
-	char *old_log;
-	size_t old_log_size;
-	size_t old_log_footer_size;
-	bool early;
-};
-
-int persistent_ram_early_init(struct persistent_ram *ram);
-
-struct persistent_ram_zone *persistent_ram_init_ringbuffer(struct device *dev,
-		bool ecc);
-
-int persistent_ram_write(struct persistent_ram_zone *prz, const void *s,
-	unsigned int count);
-
-size_t persistent_ram_old_size(struct persistent_ram_zone *prz);
-void *persistent_ram_old(struct persistent_ram_zone *prz);
-void persistent_ram_free_old(struct persistent_ram_zone *prz);
-ssize_t persistent_ram_ecc_string(struct persistent_ram_zone *prz,
-	char *str, size_t len);
-
-#endif
diff --git a/drivers/staging/android/ram_console.c b/drivers/staging/android/ram_console.c
index ce140ffc54e..82323bb1d1a 100644
--- a/drivers/staging/android/ram_console.c
+++ b/drivers/staging/android/ram_console.c
@@ -21,7 +21,7 @@
 #include <linux/string.h>
 #include <linux/uaccess.h>
 #include <linux/io.h>
-#include "persistent_ram.h"
+#include <linux/pstore_ram.h>
 #include "ram_console.h"
 
 static struct persistent_ram_zone *ram_console_zone;
diff --git a/drivers/staging/android/switch/Kconfig b/drivers/staging/android/switch/Kconfig
deleted file mode 100644
index 36846f62f4b..00000000000
--- a/drivers/staging/android/switch/Kconfig
+++ /dev/null
@@ -1,11 +0,0 @@
-menuconfig ANDROID_SWITCH
-	tristate "Android Switch class support"
-	help
-	  Say Y here to enable Android switch class support. This allows
-	  monitoring switches by userspace via sysfs and uevent.
-
-config ANDROID_SWITCH_GPIO
-	tristate "Android GPIO Switch support"
-	depends on GENERIC_GPIO && ANDROID_SWITCH
-	help
-	  Say Y here to enable GPIO based switch support.
diff --git a/drivers/staging/android/switch/Makefile b/drivers/staging/android/switch/Makefile
deleted file mode 100644
index d76bfdcedfa..00000000000
--- a/drivers/staging/android/switch/Makefile
+++ /dev/null
@@ -1,4 +0,0 @@
-# Android Switch Class Driver
-obj-$(CONFIG_ANDROID_SWITCH)		+= switch_class.o
-obj-$(CONFIG_ANDROID_SWITCH_GPIO)	+= switch_gpio.o
-
diff --git a/drivers/staging/android/switch/switch.h b/drivers/staging/android/switch/switch.h
deleted file mode 100644
index 4fcb3109875..00000000000
--- a/drivers/staging/android/switch/switch.h
+++ /dev/null
@@ -1,53 +0,0 @@
-/*
- *  Switch class driver
- *
- * Copyright (C) 2008 Google, Inc.
- * Author: Mike Lockwood <lockwood@android.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.
- *
-*/
-
-#ifndef __LINUX_SWITCH_H__
-#define __LINUX_SWITCH_H__
-
-struct switch_dev {
-	const char	*name;
-	struct device	*dev;
-	int		index;
-	int		state;
-
-	ssize_t	(*print_name)(struct switch_dev *sdev, char *buf);
-	ssize_t	(*print_state)(struct switch_dev *sdev, char *buf);
-};
-
-struct gpio_switch_platform_data {
-	const char *name;
-	unsigned	gpio;
-
-	/* if NULL, switch_dev.name will be printed */
-	const char *name_on;
-	const char *name_off;
-	/* if NULL, "0" or "1" will be printed */
-	const char *state_on;
-	const char *state_off;
-};
-
-extern int switch_dev_register(struct switch_dev *sdev);
-extern void switch_dev_unregister(struct switch_dev *sdev);
-
-static inline int switch_get_state(struct switch_dev *sdev)
-{
-	return sdev->state;
-}
-
-extern void switch_set_state(struct switch_dev *sdev, int state);
-
-#endif /* __LINUX_SWITCH_H__ */
diff --git a/drivers/staging/android/switch/switch_class.c b/drivers/staging/android/switch/switch_class.c
deleted file mode 100644
index 74680446fc6..00000000000
--- a/drivers/staging/android/switch/switch_class.c
+++ /dev/null
@@ -1,174 +0,0 @@
-/*
- * switch_class.c
- *
- * Copyright (C) 2008 Google, Inc.
- * Author: Mike Lockwood <lockwood@android.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/types.h>
-#include <linux/init.h>
-#include <linux/device.h>
-#include <linux/fs.h>
-#include <linux/err.h>
-#include "switch.h"
-
-struct class *switch_class;
-static atomic_t device_count;
-
-static ssize_t state_show(struct device *dev, struct device_attribute *attr,
-		char *buf)
-{
-	struct switch_dev *sdev = (struct switch_dev *)
-		dev_get_drvdata(dev);
-
-	if (sdev->print_state) {
-		int ret = sdev->print_state(sdev, buf);
-		if (ret >= 0)
-			return ret;
-	}
-	return sprintf(buf, "%d\n", sdev->state);
-}
-
-static ssize_t name_show(struct device *dev, struct device_attribute *attr,
-		char *buf)
-{
-	struct switch_dev *sdev = (struct switch_dev *)
-		dev_get_drvdata(dev);
-
-	if (sdev->print_name) {
-		int ret = sdev->print_name(sdev, buf);
-		if (ret >= 0)
-			return ret;
-	}
-	return sprintf(buf, "%s\n", sdev->name);
-}
-
-static DEVICE_ATTR(state, S_IRUGO | S_IWUSR, state_show, NULL);
-static DEVICE_ATTR(name, S_IRUGO | S_IWUSR, name_show, NULL);
-
-void switch_set_state(struct switch_dev *sdev, int state)
-{
-	char name_buf[120];
-	char state_buf[120];
-	char *prop_buf;
-	char *envp[3];
-	int env_offset = 0;
-	int length;
-
-	if (sdev->state != state) {
-		sdev->state = state;
-
-		prop_buf = (char *)get_zeroed_page(GFP_KERNEL);
-		if (prop_buf) {
-			length = name_show(sdev->dev, NULL, prop_buf);
-			if (length > 0) {
-				if (prop_buf[length - 1] == '\n')
-					prop_buf[length - 1] = 0;
-				snprintf(name_buf, sizeof(name_buf),
-					"SWITCH_NAME=%s", prop_buf);
-				envp[env_offset++] = name_buf;
-			}
-			length = state_show(sdev->dev, NULL, prop_buf);
-			if (length > 0) {
-				if (prop_buf[length - 1] == '\n')
-					prop_buf[length - 1] = 0;
-				snprintf(state_buf, sizeof(state_buf),
-					"SWITCH_STATE=%s", prop_buf);
-				envp[env_offset++] = state_buf;
-			}
-			envp[env_offset] = NULL;
-			kobject_uevent_env(&sdev->dev->kobj, KOBJ_CHANGE, envp);
-			free_page((unsigned long)prop_buf);
-		} else {
-			printk(KERN_ERR "out of memory in switch_set_state\n");
-			kobject_uevent(&sdev->dev->kobj, KOBJ_CHANGE);
-		}
-	}
-}
-EXPORT_SYMBOL_GPL(switch_set_state);
-
-static int create_switch_class(void)
-{
-	if (!switch_class) {
-		switch_class = class_create(THIS_MODULE, "switch");
-		if (IS_ERR(switch_class))
-			return PTR_ERR(switch_class);
-		atomic_set(&device_count, 0);
-	}
-
-	return 0;
-}
-
-int switch_dev_register(struct switch_dev *sdev)
-{
-	int ret;
-
-	if (!switch_class) {
-		ret = create_switch_class();
-		if (ret < 0)
-			return ret;
-	}
-
-	sdev->index = atomic_inc_return(&device_count);
-	sdev->dev = device_create(switch_class, NULL,
-		MKDEV(0, sdev->index), NULL, sdev->name);
-	if (IS_ERR(sdev->dev))
-		return PTR_ERR(sdev->dev);
-
-	ret = device_create_file(sdev->dev, &dev_attr_state);
-	if (ret < 0)
-		goto err_create_file_1;
-	ret = device_create_file(sdev->dev, &dev_attr_name);
-	if (ret < 0)
-		goto err_create_file_2;
-
-	dev_set_drvdata(sdev->dev, sdev);
-	sdev->state = 0;
-	return 0;
-
-err_create_file_2:
-	device_remove_file(sdev->dev, &dev_attr_state);
-err_create_file_1:
-	device_destroy(switch_class, MKDEV(0, sdev->index));
-	printk(KERN_ERR "switch: Failed to register driver %s\n", sdev->name);
-
-	return ret;
-}
-EXPORT_SYMBOL_GPL(switch_dev_register);
-
-void switch_dev_unregister(struct switch_dev *sdev)
-{
-	device_remove_file(sdev->dev, &dev_attr_name);
-	device_remove_file(sdev->dev, &dev_attr_state);
-	device_destroy(switch_class, MKDEV(0, sdev->index));
-	dev_set_drvdata(sdev->dev, NULL);
-}
-EXPORT_SYMBOL_GPL(switch_dev_unregister);
-
-static int __init switch_class_init(void)
-{
-	return create_switch_class();
-}
-
-static void __exit switch_class_exit(void)
-{
-	class_destroy(switch_class);
-}
-
-module_init(switch_class_init);
-module_exit(switch_class_exit);
-
-MODULE_AUTHOR("Mike Lockwood <lockwood@android.com>");
-MODULE_DESCRIPTION("Switch class driver");
-MODULE_LICENSE("GPL");
diff --git a/drivers/staging/android/switch/switch_gpio.c b/drivers/staging/android/switch/switch_gpio.c
deleted file mode 100644
index 38b2c2f6004..00000000000
--- a/drivers/staging/android/switch/switch_gpio.c
+++ /dev/null
@@ -1,172 +0,0 @@
-/*
- * switch_gpio.c
- *
- * Copyright (C) 2008 Google, Inc.
- * Author: Mike Lockwood <lockwood@android.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/kernel.h>
-#include <linux/init.h>
-#include <linux/interrupt.h>
-#include <linux/platform_device.h>
-#include <linux/slab.h>
-#include <linux/workqueue.h>
-#include <linux/gpio.h>
-#include "switch.h"
-
-struct gpio_switch_data {
-	struct switch_dev sdev;
-	unsigned gpio;
-	const char *name_on;
-	const char *name_off;
-	const char *state_on;
-	const char *state_off;
-	int irq;
-	struct work_struct work;
-};
-
-static void gpio_switch_work(struct work_struct *work)
-{
-	int state;
-	struct gpio_switch_data	*data =
-		container_of(work, struct gpio_switch_data, work);
-
-	state = gpio_get_value(data->gpio);
-	switch_set_state(&data->sdev, state);
-}
-
-static irqreturn_t gpio_irq_handler(int irq, void *dev_id)
-{
-	struct gpio_switch_data *switch_data =
-	    (struct gpio_switch_data *)dev_id;
-
-	schedule_work(&switch_data->work);
-	return IRQ_HANDLED;
-}
-
-static ssize_t switch_gpio_print_state(struct switch_dev *sdev, char *buf)
-{
-	struct gpio_switch_data	*switch_data =
-		container_of(sdev, struct gpio_switch_data, sdev);
-	const char *state;
-	if (switch_get_state(sdev))
-		state = switch_data->state_on;
-	else
-		state = switch_data->state_off;
-
-	if (state)
-		return sprintf(buf, "%s\n", state);
-	return -1;
-}
-
-static int gpio_switch_probe(struct platform_device *pdev)
-{
-	struct gpio_switch_platform_data *pdata = pdev->dev.platform_data;
-	struct gpio_switch_data *switch_data;
-	int ret = 0;
-
-	if (!pdata)
-		return -EBUSY;
-
-	switch_data = kzalloc(sizeof(struct gpio_switch_data), GFP_KERNEL);
-	if (!switch_data)
-		return -ENOMEM;
-
-	switch_data->sdev.name = pdata->name;
-	switch_data->gpio = pdata->gpio;
-	switch_data->name_on = pdata->name_on;
-	switch_data->name_off = pdata->name_off;
-	switch_data->state_on = pdata->state_on;
-	switch_data->state_off = pdata->state_off;
-	switch_data->sdev.print_state = switch_gpio_print_state;
-
-	ret = switch_dev_register(&switch_data->sdev);
-	if (ret < 0)
-		goto err_switch_dev_register;
-
-	ret = gpio_request(switch_data->gpio, pdev->name);
-	if (ret < 0)
-		goto err_request_gpio;
-
-	ret = gpio_direction_input(switch_data->gpio);
-	if (ret < 0)
-		goto err_set_gpio_input;
-
-	INIT_WORK(&switch_data->work, gpio_switch_work);
-
-	switch_data->irq = gpio_to_irq(switch_data->gpio);
-	if (switch_data->irq < 0) {
-		ret = switch_data->irq;
-		goto err_detect_irq_num_failed;
-	}
-
-	ret = request_irq(switch_data->irq, gpio_irq_handler,
-			  IRQF_TRIGGER_LOW, pdev->name, switch_data);
-	if (ret < 0)
-		goto err_request_irq;
-
-	/* Perform initial detection */
-	gpio_switch_work(&switch_data->work);
-
-	return 0;
-
-err_request_irq:
-err_detect_irq_num_failed:
-err_set_gpio_input:
-	gpio_free(switch_data->gpio);
-err_request_gpio:
-	switch_dev_unregister(&switch_data->sdev);
-err_switch_dev_register:
-	kfree(switch_data);
-
-	return ret;
-}
-
-static int __devexit gpio_switch_remove(struct platform_device *pdev)
-{
-	struct gpio_switch_data *switch_data = platform_get_drvdata(pdev);
-
-	cancel_work_sync(&switch_data->work);
-	gpio_free(switch_data->gpio);
-	switch_dev_unregister(&switch_data->sdev);
-	kfree(switch_data);
-
-	return 0;
-}
-
-static struct platform_driver gpio_switch_driver = {
-	.probe		= gpio_switch_probe,
-	.remove		= __devexit_p(gpio_switch_remove),
-	.driver		= {
-		.name	= "switch-gpio",
-		.owner	= THIS_MODULE,
-	},
-};
-
-static int __init gpio_switch_init(void)
-{
-	return platform_driver_register(&gpio_switch_driver);
-}
-
-static void __exit gpio_switch_exit(void)
-{
-	platform_driver_unregister(&gpio_switch_driver);
-}
-
-module_init(gpio_switch_init);
-module_exit(gpio_switch_exit);
-
-MODULE_AUTHOR("Mike Lockwood <lockwood@android.com>");
-MODULE_DESCRIPTION("GPIO Switch driver");
-MODULE_LICENSE("GPL");
diff --git a/drivers/staging/android/timed_output.c b/drivers/staging/android/timed_output.c
index f373422308e..38d930cadad 100644
--- a/drivers/staging/android/timed_output.c
+++ b/drivers/staging/android/timed_output.c
@@ -99,6 +99,7 @@ EXPORT_SYMBOL_GPL(timed_output_dev_register);
 
 void timed_output_dev_unregister(struct timed_output_dev *tdev)
 {
+	tdev->enable(tdev, 0);
 	device_remove_file(tdev->dev, &dev_attr_enable);
 	device_destroy(timed_output_class, MKDEV(0, tdev->index));
 	dev_set_drvdata(tdev->dev, NULL);
diff --git a/drivers/staging/asus_oled/asus_oled.c b/drivers/staging/asus_oled/asus_oled.c
index 83549d9cfef..510d7963921 100644
--- a/drivers/staging/asus_oled/asus_oled.c
+++ b/drivers/staging/asus_oled/asus_oled.c
@@ -782,20 +782,20 @@ static int __init asus_oled_init(void)
 	oled_class = class_create(THIS_MODULE, ASUS_OLED_UNDERSCORE_NAME);
 
 	if (IS_ERR(oled_class)) {
-		err("Error creating " ASUS_OLED_UNDERSCORE_NAME " class");
+		printk(KERN_ERR "Error creating " ASUS_OLED_UNDERSCORE_NAME " class\n");
 		return PTR_ERR(oled_class);
 	}
 
 	retval = class_create_file(oled_class, &class_attr_version.attr);
 	if (retval) {
-		err("Error creating class version file");
+		printk(KERN_ERR "Error creating class version file\n");
 		goto error;
 	}
 
 	retval = usb_register(&oled_driver);
 
 	if (retval) {
-		err("usb_register failed. Error number %d", retval);
+		printk(KERN_ERR "usb_register failed. Error number %d\n", retval);
 		goto error;
 	}
 
diff --git a/drivers/staging/bcm/Adapter.h b/drivers/staging/bcm/Adapter.h
index 20cca24ff5f..aa51d17be5a 100644
--- a/drivers/staging/bcm/Adapter.h
+++ b/drivers/staging/bcm/Adapter.h
@@ -7,168 +7,145 @@
 #define MAX_FRAGMENTEDIP_CLASSIFICATION_ENTRIES 256
 #include "Debug.h"
 
-struct _LEADER
-{
-	USHORT 	Vcid;
-	USHORT 	PLength;
-	UCHAR  	Status;
+struct _LEADER {
+	USHORT	Vcid;
+	USHORT	PLength;
+	UCHAR	Status;
 	UCHAR	Unused[3];
-}__attribute__((packed));
-typedef struct _LEADER LEADER,*PLEADER;
+} __packed;
+typedef struct _LEADER LEADER, *PLEADER;
 
-struct _PACKETTOSEND
-{
+struct _PACKETTOSEND {
 	LEADER	Leader;
 	UCHAR	ucPayload;
-}__attribute__((packed));
+} __packed;
 typedef struct _PACKETTOSEND PACKETTOSEND, *PPACKETTOSEND;
 
-
-struct _CONTROL_PACKET
-{
+struct _CONTROL_PACKET {
 	PVOID	ControlBuff;
 	UINT	ControlBuffLen;
-	struct _CONTROL_PACKET* next;
-}__attribute__((packed));
-typedef struct _CONTROL_PACKET CONTROL_PACKET,*PCONTROL_PACKET;
-
+	struct _CONTROL_PACKET *next;
+} __packed;
+typedef struct _CONTROL_PACKET CONTROL_PACKET, *PCONTROL_PACKET;
 
-struct link_request
-{
+struct link_request {
 	LEADER	Leader;
 	UCHAR	szData[4];
-}__attribute__((packed));
+} __packed;
 typedef struct link_request LINK_REQUEST, *PLINK_REQUEST;
 
-
-//classification extension is added
-typedef struct _ADD_CONNECTION
-{
-    ULONG 		SrcIpAddressCount;
-    ULONG 		SrcIpAddress[MAX_CONNECTIONS];
-    ULONG 		SrcIpMask[MAX_CONNECTIONS];
-
-    ULONG 		DestIpAddressCount;
-    ULONG 		DestIpAddress[MAX_CONNECTIONS];
-    ULONG 		DestIpMask[MAX_CONNECTIONS];
-
-    USHORT 		SrcPortBegin;
-    USHORT 		SrcPortEnd;
-
-    USHORT 		DestPortBegin;
-    USHORT 		DestPortEnd;
-
-    UCHAR 		SrcTOS;
-    UCHAR 		SrcProtocol;
-} ADD_CONNECTION,*PADD_CONNECTION;
-
-
-typedef struct _CLASSIFICATION_RULE
-{
-	UCHAR		ucIPSrcAddrLen;
-	UCHAR       ucIPSrcAddr[32];
-	UCHAR		ucIPDestAddrLen;
-	UCHAR       ucIPDestAddr[32];
-	UCHAR		ucSrcPortRangeLen;
-	UCHAR		ucSrcPortRange[4];
-	UCHAR		ucDestPortRangeLen;
-	UCHAR		ucDestPortRange[4];
-	USHORT		usVcid;
-} CLASSIFICATION_RULE,*PCLASSIFICATION_RULE;
-
-typedef struct _CLASSIFICATION_ONLY
-{
-    USHORT 		usVcid;
-    ULONG  		DestIpAddress;
-    ULONG  		DestIpMask;
-    USHORT 		usPortLo;
-    USHORT 		usPortHi;
-    BOOLEAN		bIpVersion;
-    UCHAR		ucDestinationAddress[16];
+/* classification extension is added */
+typedef struct _ADD_CONNECTION {
+	ULONG	SrcIpAddressCount;
+	ULONG	SrcIpAddress[MAX_CONNECTIONS];
+	ULONG	SrcIpMask[MAX_CONNECTIONS];
+
+	ULONG	DestIpAddressCount;
+	ULONG	DestIpAddress[MAX_CONNECTIONS];
+	ULONG	DestIpMask[MAX_CONNECTIONS];
+
+	USHORT	SrcPortBegin;
+	USHORT	SrcPortEnd;
+
+	USHORT	DestPortBegin;
+	USHORT	DestPortEnd;
+
+	UCHAR	SrcTOS;
+	UCHAR	SrcProtocol;
+} ADD_CONNECTION, *PADD_CONNECTION;
+
+typedef struct _CLASSIFICATION_RULE {
+	UCHAR	ucIPSrcAddrLen;
+	UCHAR	ucIPSrcAddr[32];
+	UCHAR	ucIPDestAddrLen;
+	UCHAR	ucIPDestAddr[32];
+	UCHAR	ucSrcPortRangeLen;
+	UCHAR	ucSrcPortRange[4];
+	UCHAR	ucDestPortRangeLen;
+	UCHAR	ucDestPortRange[4];
+	USHORT	usVcid;
+} CLASSIFICATION_RULE, *PCLASSIFICATION_RULE;
+
+typedef struct _CLASSIFICATION_ONLY {
+	USHORT	usVcid;
+	ULONG	DestIpAddress;
+	ULONG	DestIpMask;
+	USHORT	usPortLo;
+	USHORT	usPortHi;
+	BOOLEAN	bIpVersion;
+	UCHAR	ucDestinationAddress[16];
 } CLASSIFICATION_ONLY, *PCLASSIFICATION_ONLY;
 
-
 #define MAX_IP_RANGE_LENGTH 4
 #define MAX_PORT_RANGE 4
 #define MAX_PROTOCOL_LENGTH   32
 #define IPV6_ADDRESS_SIZEINBYTES 0x10
 
-typedef union _U_IP_ADDRESS
-{
-    struct
-	{
-		ULONG				ulIpv4Addr[MAX_IP_RANGE_LENGTH];//Source Ip Address Range
-		ULONG               ulIpv4Mask[MAX_IP_RANGE_LENGTH];//Source Ip Mask Address Range
+typedef union _U_IP_ADDRESS {
+	struct {
+		ULONG ulIpv4Addr[MAX_IP_RANGE_LENGTH]; /* Source Ip Address Range */
+		ULONG ulIpv4Mask[MAX_IP_RANGE_LENGTH]; /* Source Ip Mask Address Range */
 	};
-	struct
-	{
-		ULONG				ulIpv6Addr[MAX_IP_RANGE_LENGTH * 4];//Source Ip Address Range
-		ULONG               ulIpv6Mask[MAX_IP_RANGE_LENGTH * 4];//Source Ip Mask Address Range
-
+	struct {
+		ULONG ulIpv6Addr[MAX_IP_RANGE_LENGTH * 4]; /* Source Ip Address Range */
+		ULONG ulIpv6Mask[MAX_IP_RANGE_LENGTH * 4]; /* Source Ip Mask Address Range */
 	};
-	struct
-	{
-		UCHAR				ucIpv4Address[MAX_IP_RANGE_LENGTH * IP_LENGTH_OF_ADDRESS];
-		UCHAR				ucIpv4Mask[MAX_IP_RANGE_LENGTH * IP_LENGTH_OF_ADDRESS];
+	struct {
+		UCHAR ucIpv4Address[MAX_IP_RANGE_LENGTH * IP_LENGTH_OF_ADDRESS];
+		UCHAR ucIpv4Mask[MAX_IP_RANGE_LENGTH * IP_LENGTH_OF_ADDRESS];
 	};
-	struct
-	{
-		UCHAR				ucIpv6Address[MAX_IP_RANGE_LENGTH * IPV6_ADDRESS_SIZEINBYTES];
-		UCHAR				ucIpv6Mask[MAX_IP_RANGE_LENGTH * IPV6_ADDRESS_SIZEINBYTES];
+	struct {
+		UCHAR ucIpv6Address[MAX_IP_RANGE_LENGTH * IPV6_ADDRESS_SIZEINBYTES];
+		UCHAR ucIpv6Mask[MAX_IP_RANGE_LENGTH * IPV6_ADDRESS_SIZEINBYTES];
 	};
-}U_IP_ADDRESS;
+} U_IP_ADDRESS;
 struct _packet_info;
 
-typedef struct _S_HDR_SUPRESSION_CONTEXTINFO
-{
+typedef struct _S_HDR_SUPRESSION_CONTEXTINFO {
+	UCHAR ucaHdrSupressionInBuf[MAX_PHS_LENGTHS]; /* Intermediate buffer to accumulate pkt Header for PHS */
+	UCHAR ucaHdrSupressionOutBuf[MAX_PHS_LENGTHS + PHSI_LEN]; /* Intermediate buffer containing pkt Header after PHS */
+} S_HDR_SUPRESSION_CONTEXTINFO;
+
+typedef struct _S_CLASSIFIER_RULE {
+	ULONG		ulSFID;
+	UCHAR		ucReserved[2];
+	B_UINT16	uiClassifierRuleIndex;
+	BOOLEAN		bUsed;
+	USHORT		usVCID_Value;
+	B_UINT8		u8ClassifierRulePriority; /* This field detemines the Classifier Priority */
+	U_IP_ADDRESS	stSrcIpAddress;
+	UCHAR		ucIPSourceAddressLength; /* Ip Source Address Length */
 
-	UCHAR      ucaHdrSupressionInBuf[MAX_PHS_LENGTHS]; //Intermediate buffer to accumulate pkt Header for PHS
-	UCHAR      ucaHdrSupressionOutBuf[MAX_PHS_LENGTHS + PHSI_LEN]; //Intermediate buffer containing pkt Header after PHS
+	U_IP_ADDRESS	stDestIpAddress;
+	UCHAR		ucIPDestinationAddressLength; /* Ip Destination Address Length */
+	UCHAR		ucIPTypeOfServiceLength; /* Type of service Length */
+	UCHAR		ucTosLow; /* Tos Low */
+	UCHAR		ucTosHigh; /* Tos High */
+	UCHAR		ucTosMask; /* Tos Mask */
 
-}S_HDR_SUPRESSION_CONTEXTINFO;
+	UCHAR		ucProtocolLength; /* protocol Length */
+	UCHAR		ucProtocol[MAX_PROTOCOL_LENGTH]; /* protocol Length */
+	USHORT		usSrcPortRangeLo[MAX_PORT_RANGE];
+	USHORT		usSrcPortRangeHi[MAX_PORT_RANGE];
+	UCHAR		ucSrcPortRangeLength;
 
+	USHORT		usDestPortRangeLo[MAX_PORT_RANGE];
+	USHORT		usDestPortRangeHi[MAX_PORT_RANGE];
+	UCHAR		ucDestPortRangeLength;
 
-typedef struct _S_CLASSIFIER_RULE
-{
-	ULONG			ulSFID;
-	UCHAR           ucReserved[2];
-	B_UINT16        uiClassifierRuleIndex;
-	BOOLEAN			bUsed;
-	USHORT			usVCID_Value;
-	B_UINT8         u8ClassifierRulePriority; //This field detemines the Classifier Priority
-	U_IP_ADDRESS	stSrcIpAddress;
-	UCHAR           ucIPSourceAddressLength;//Ip Source Address Length
-
-	U_IP_ADDRESS    stDestIpAddress;
-    UCHAR           ucIPDestinationAddressLength;//Ip Destination Address Length
-    UCHAR           ucIPTypeOfServiceLength;//Type of service Length
-    UCHAR           ucTosLow;//Tos Low
-    UCHAR           ucTosHigh;//Tos High
-	UCHAR           ucTosMask;//Tos Mask
-
-    UCHAR           ucProtocolLength;//protocol Length
-    UCHAR           ucProtocol[MAX_PROTOCOL_LENGTH];//protocol Length
-    USHORT			usSrcPortRangeLo[MAX_PORT_RANGE];
-	USHORT			usSrcPortRangeHi[MAX_PORT_RANGE];
-    UCHAR           ucSrcPortRangeLength;
-
-    USHORT			usDestPortRangeLo[MAX_PORT_RANGE];
-	USHORT			usDestPortRangeHi[MAX_PORT_RANGE];
-    UCHAR           ucDestPortRangeLength;
-
-	BOOLEAN			bProtocolValid;
-	BOOLEAN			bTOSValid;
-	BOOLEAN			bDestIpValid;
-	BOOLEAN			bSrcIpValid;
-
-	//For IPv6 Addressing
-	UCHAR			ucDirection;
-	BOOLEAN         bIpv6Protocol;
-	UINT32          u32PHSRuleID;
-	S_PHS_RULE 	    sPhsRule;
-	UCHAR			u8AssociatedPHSI;
-
-	//Classification fields for ETH CS
+	BOOLEAN		bProtocolValid;
+	BOOLEAN		bTOSValid;
+	BOOLEAN		bDestIpValid;
+	BOOLEAN		bSrcIpValid;
+
+	/* For IPv6 Addressing */
+	UCHAR		ucDirection;
+	BOOLEAN		bIpv6Protocol;
+	UINT32		u32PHSRuleID;
+	S_PHS_RULE	sPhsRule;
+	UCHAR		u8AssociatedPHSI;
+
+	/* Classification fields for ETH CS */
 	UCHAR		ucEthCSSrcMACLen;
 	UCHAR		au8EThCSSrcMAC[MAC_ADDRESS_SIZE];
 	UCHAR		au8EThCSSrcMACMask[MAC_ADDRESS_SIZE];
@@ -180,71 +157,67 @@ typedef struct _S_CLASSIFIER_RULE
 	UCHAR		usUserPriority[2];
 	USHORT		usVLANID;
 	USHORT		usValidityBitMap;
-}S_CLASSIFIER_RULE;
-//typedef struct _S_CLASSIFIER_RULE S_CLASSIFIER_RULE;
-
-typedef struct _S_FRAGMENTED_PACKET_INFO
-{
-	BOOLEAN				bUsed;
-	ULONG		        ulSrcIpAddress;
-	USHORT 				usIpIdentification;
-	S_CLASSIFIER_RULE 	*pstMatchedClassifierEntry;
-	BOOLEAN				bOutOfOrderFragment;
-}S_FRAGMENTED_PACKET_INFO,*PS_FRAGMENTED_PACKET_INFO;
-
-struct _packet_info
-{
-	//classification extension Rule
-   	ULONG			ulSFID;
-   	USHORT			usVCID_Value;
-	UINT			uiThreshold;
-	// This field determines the priority of the SF Queues
-	B_UINT8     	u8TrafficPriority;
-
-	BOOLEAN			bValid;
-   	BOOLEAN     	bActive;
-	BOOLEAN			bActivateRequestSent;
-
-	B_UINT8			u8QueueType;//BE or rtPS
-
-	UINT			uiMaxBucketSize;//maximum size of the bucket for the queue
-	UINT			uiCurrentQueueDepthOnTarget;
-	UINT			uiCurrentBytesOnHost;
-	UINT			uiCurrentPacketsOnHost;
-	UINT			uiDroppedCountBytes;
-	UINT			uiDroppedCountPackets;
-	UINT			uiSentBytes;
-	UINT			uiSentPackets;
-	UINT			uiCurrentDrainRate;
-	UINT			uiThisPeriodSentBytes;
+} S_CLASSIFIER_RULE;
+/* typedef struct _S_CLASSIFIER_RULE S_CLASSIFIER_RULE; */
+
+typedef struct _S_FRAGMENTED_PACKET_INFO {
+	BOOLEAN			bUsed;
+	ULONG			ulSrcIpAddress;
+	USHORT			usIpIdentification;
+	S_CLASSIFIER_RULE	*pstMatchedClassifierEntry;
+	BOOLEAN			bOutOfOrderFragment;
+} S_FRAGMENTED_PACKET_INFO, *PS_FRAGMENTED_PACKET_INFO;
+
+struct _packet_info {
+	/* classification extension Rule */
+	ULONG		ulSFID;
+	USHORT		usVCID_Value;
+	UINT		uiThreshold;
+	/* This field determines the priority of the SF Queues */
+	B_UINT8		u8TrafficPriority;
+
+	BOOLEAN		bValid;
+	BOOLEAN		bActive;
+	BOOLEAN		bActivateRequestSent;
+
+	B_UINT8		u8QueueType; /* BE or rtPS */
+
+	UINT		uiMaxBucketSize; /* maximum size of the bucket for the queue */
+	UINT		uiCurrentQueueDepthOnTarget;
+	UINT		uiCurrentBytesOnHost;
+	UINT		uiCurrentPacketsOnHost;
+	UINT		uiDroppedCountBytes;
+	UINT		uiDroppedCountPackets;
+	UINT		uiSentBytes;
+	UINT		uiSentPackets;
+	UINT		uiCurrentDrainRate;
+	UINT		uiThisPeriodSentBytes;
 	LARGE_INTEGER	liDrainCalculated;
-	UINT			uiCurrentTokenCount;
-	LARGE_INTEGER 	liLastUpdateTokenAt;
-	UINT			uiMaxAllowedRate;
-	UINT			NumOfPacketsSent;
-	UCHAR			ucDirection;
-	USHORT			usCID;
+	UINT		uiCurrentTokenCount;
+	LARGE_INTEGER	liLastUpdateTokenAt;
+	UINT		uiMaxAllowedRate;
+	UINT		NumOfPacketsSent;
+	UCHAR		ucDirection;
+	USHORT		usCID;
 	S_MIBS_EXTSERVICEFLOW_PARAMETERS	stMibsExtServiceFlowTable;
-	UINT			uiCurrentRxRate;
-	UINT			uiThisPeriodRxBytes;
-	UINT			uiTotalRxBytes;
-	UINT			uiTotalTxBytes;
-	UINT			uiPendedLast;
-	UCHAR			ucIpVersion;
-
-	union
-	{
-		struct
-		{
-			struct sk_buff*	   FirstTxQueue;
-			struct sk_buff*	   LastTxQueue;
+	UINT		uiCurrentRxRate;
+	UINT		uiThisPeriodRxBytes;
+	UINT		uiTotalRxBytes;
+	UINT		uiTotalTxBytes;
+	UINT		uiPendedLast;
+	UCHAR		ucIpVersion;
+
+	union {
+		struct {
+			struct sk_buff *FirstTxQueue;
+			struct sk_buff *LastTxQueue;
 		};
-		struct
-		{
-			struct sk_buff*	   ControlHead;
-			struct sk_buff*	   ControlTail;
+		struct {
+			struct sk_buff *ControlHead;
+			struct sk_buff *ControlTail;
 		};
 	};
+
 	BOOLEAN		bProtocolValid;
 	BOOLEAN		bTOSValid;
 	BOOLEAN		bDestIpValid;
@@ -255,226 +228,209 @@ struct _packet_info
 	BOOLEAN		bAuthorizedSet;
 	BOOLEAN		bClassifierPriority;
 	UCHAR		ucServiceClassName[MAX_CLASS_NAME_LENGTH];
-	BOOLEAN     bHeaderSuppressionEnabled;
+	BOOLEAN		bHeaderSuppressionEnabled;
 	spinlock_t	SFQueueLock;
-	void 		*pstSFIndication;
+	void		*pstSFIndication;
 	struct timeval	stLastUpdateTokenAt;
-	atomic_t 	uiPerSFTxResourceCount;
+	atomic_t	uiPerSFTxResourceCount;
 	UINT		uiMaxLatency;
-	UCHAR 	bIPCSSupport;
-	UCHAR 	bEthCSSupport;
+	UCHAR		bIPCSSupport;
+	UCHAR		bEthCSSupport;
 };
 typedef struct _packet_info PacketInfo;
 
-
-typedef struct _PER_TARANG_DATA
-{
-    struct _PER_TARANG_DATA * next;
-    struct _MINI_ADAPTER * Adapter;
-    struct sk_buff*     RxAppControlHead;
-    struct sk_buff*     RxAppControlTail;
-    volatile INT        AppCtrlQueueLen;
-    BOOLEAN             MacTracingEnabled;
-	BOOLEAN				bApplicationToExit;
-	S_MIBS_DROPPED_APP_CNTRL_MESSAGES stDroppedAppCntrlMsgs;
-	ULONG 				RxCntrlMsgBitMask;
+typedef struct _PER_TARANG_DATA {
+	struct _PER_TARANG_DATA	*next;
+	struct _MINI_ADAPTER	*Adapter;
+	struct sk_buff		*RxAppControlHead;
+	struct sk_buff		*RxAppControlTail;
+	int			AppCtrlQueueLen;
+	BOOLEAN			MacTracingEnabled;
+	BOOLEAN			bApplicationToExit;
+	S_MIBS_DROPPED_APP_CNTRL_MESSAGES	stDroppedAppCntrlMsgs;
+	ULONG			RxCntrlMsgBitMask;
 } PER_TARANG_DATA, *PPER_TARANG_DATA;
 
-
 #ifdef REL_4_1
-typedef struct _TARGET_PARAMS
-{
-      B_UINT32 m_u32CfgVersion;
-
-      // Scanning Related Params
-      B_UINT32 m_u32CenterFrequency;
-      B_UINT32 m_u32BandAScan;
-      B_UINT32 m_u32BandBScan;
-      B_UINT32 m_u32BandCScan;
-
-      // QoS Params
-      B_UINT32 m_u32minGrantsize;	// size of minimum grant is 0 or 6
-      B_UINT32 m_u32PHSEnable;
-
-      // HO Params
-      B_UINT32 m_u32HoEnable;
-      B_UINT32 m_u32HoReserved1;
-      B_UINT32 m_u32HoReserved2;
-
-	// Power Control Params
-      B_UINT32 m_u32MimoEnable;
-      B_UINT32 m_u32SecurityEnable;
+typedef struct _TARGET_PARAMS {
+	B_UINT32 m_u32CfgVersion;
+
+	/* Scanning Related Params */
+	B_UINT32 m_u32CenterFrequency;
+	B_UINT32 m_u32BandAScan;
+	B_UINT32 m_u32BandBScan;
+	B_UINT32 m_u32BandCScan;
+
+	/* QoS Params */
+	B_UINT32 m_u32minGrantsize;	/* size of minimum grant is 0 or 6 */
+	B_UINT32 m_u32PHSEnable;
+
+	/* HO Params */
+	B_UINT32 m_u32HoEnable;
+	B_UINT32 m_u32HoReserved1;
+	B_UINT32 m_u32HoReserved2;
+
+	/* Power Control Params */
+	B_UINT32 m_u32MimoEnable;
+	B_UINT32 m_u32SecurityEnable;
 	/*
-     * bit 1: 1 Idlemode enable;
-     * bit 2: 1 Sleepmode Enable
-     */
-      B_UINT32 m_u32PowerSavingModesEnable;
-	  /* PowerSaving Mode Options:
-	     bit 0 = 1: CPE mode - to keep pcmcia if alive;
-	     bit 1 = 1: CINR reporing in Idlemode Msg
-	     bit 2 = 1: Default PSC Enable in sleepmode*/
-      B_UINT32 m_u32PowerSavingModeOptions;
-
-      B_UINT32 m_u32ArqEnable;
-
-      // From Version #3, the HARQ section renamed as general
-      B_UINT32 m_u32HarqEnable;
-       // EEPROM Param Location
-       B_UINT32  m_u32EEPROMFlag;
-       /* BINARY TYPE - 4th MSByte:
-        * Interface Type -  3rd MSByte:
-        * Vendor Type - 2nd MSByte
-        */
-       // Unused - LSByte
-      B_UINT32   m_u32Customize;
-      B_UINT32   m_u32ConfigBW;  /* In Hz */
-      B_UINT32   m_u32ShutDownTimer;
-
-
-      B_UINT32  m_u32RadioParameter;
-      B_UINT32  m_u32PhyParameter1;
-      B_UINT32  m_u32PhyParameter2;
-      B_UINT32  m_u32PhyParameter3;
+	 * bit 1: 1 Idlemode enable;
+	 * bit 2: 1 Sleepmode Enable
+	 */
+	B_UINT32 m_u32PowerSavingModesEnable;
+	/* PowerSaving Mode Options:
+	 * bit 0 = 1: CPE mode - to keep pcmcia if alive;
+	 * bit 1 = 1: CINR reporing in Idlemode Msg
+	 * bit 2 = 1: Default PSC Enable in sleepmode
+	 */
+	B_UINT32 m_u32PowerSavingModeOptions;
+
+	B_UINT32 m_u32ArqEnable;
+
+	/* From Version #3, the HARQ section renamed as general */
+	B_UINT32 m_u32HarqEnable;
+	/* EEPROM Param Location */
+	B_UINT32 m_u32EEPROMFlag;
+	/* BINARY TYPE - 4th MSByte:
+	 * Interface Type -  3rd MSByte:
+	 * Vendor Type - 2nd MSByte
+	 */
+	/* Unused - LSByte */
+	B_UINT32 m_u32Customize;
+	B_UINT32 m_u32ConfigBW;  /* In Hz */
+	B_UINT32 m_u32ShutDownTimer;
+	B_UINT32 m_u32RadioParameter;
+	B_UINT32 m_u32PhyParameter1;
+	B_UINT32 m_u32PhyParameter2;
+	B_UINT32 m_u32PhyParameter3;
 
 	/* in eval mode only;
-     * lower 16bits = basic cid for testing;
-     * then bit 16 is test cqich,
-     * bit 17  test init rang;
-     * bit 18 test periodic rang
-     * bit 19 is test harq ack/nack
-     */
-    B_UINT32	  m_u32TestOptions;
-
+	 * lower 16bits = basic cid for testing;
+	 * then bit 16 is test cqich,
+	 * bit 17  test init rang;
+	 * bit 18 test periodic rang
+	 * bit 19 is test harq ack/nack
+	 */
+	B_UINT32 m_u32TestOptions;
 	B_UINT32 m_u32MaxMACDataperDLFrame;
 	B_UINT32 m_u32MaxMACDataperULFrame;
-
 	B_UINT32 m_u32Corr2MacFlags;
 
-    //adding driver params.
-    B_UINT32 HostDrvrConfig1;
-    B_UINT32 HostDrvrConfig2;
-    B_UINT32 HostDrvrConfig3;
-    B_UINT32 HostDrvrConfig4;
-    B_UINT32 HostDrvrConfig5;
-    B_UINT32 HostDrvrConfig6;
-    B_UINT32 m_u32SegmentedPUSCenable;
-
-	//	BAMC enable - but 4.x does not support this feature
-	//	This is added just to sync 4.x and 5.x CFGs
+	/* adding driver params. */
+	B_UINT32 HostDrvrConfig1;
+	B_UINT32 HostDrvrConfig2;
+	B_UINT32 HostDrvrConfig3;
+	B_UINT32 HostDrvrConfig4;
+	B_UINT32 HostDrvrConfig5;
+	B_UINT32 HostDrvrConfig6;
+	B_UINT32 m_u32SegmentedPUSCenable;
+
+	/* BAMC enable - but 4.x does not support this feature
+	 * This is added just to sync 4.x and 5.x CFGs
+	 */
 	B_UINT32 m_u32BandAMCEnable;
 } STARGETPARAMS, *PSTARGETPARAMS;
 #endif
 
-typedef struct _STTARGETDSXBUFFER
-{
-    ULONG ulTargetDsxBuffer;
-    B_UINT16 tid;
-    BOOLEAN valid;
-}STTARGETDSXBUFFER, *PSTTARGETDSXBUFFER;
+typedef struct _STTARGETDSXBUFFER {
+	ULONG		ulTargetDsxBuffer;
+	B_UINT16	tid;
+	BOOLEAN		valid;
+} STTARGETDSXBUFFER, *PSTTARGETDSXBUFFER;
 
-typedef INT (*FP_FLASH_WRITE)(struct _MINI_ADAPTER*,UINT,PVOID);
+typedef int (*FP_FLASH_WRITE)(struct _MINI_ADAPTER *, UINT, PVOID);
 
-typedef INT (*FP_FLASH_WRITE_STATUS)(struct _MINI_ADAPTER*,UINT,PVOID);
+typedef int (*FP_FLASH_WRITE_STATUS)(struct _MINI_ADAPTER *, UINT, PVOID);
 
-/**
-Driver adapter data structure
-*/
-struct _MINI_ADAPTER
-{
-	struct _MINI_ADAPTER *next;
+/*
+ * Driver adapter data structure
+ */
+struct _MINI_ADAPTER {
+	struct _MINI_ADAPTER	*next;
 	struct net_device	*dev;
 	u32			msg_enable;
-
-	CHAR                *caDsxReqResp;
+	CHAR			*caDsxReqResp;
 	atomic_t		ApplicationRunning;
-	volatile INT		CtrlQueueLen;
-	atomic_t            	AppCtrlQueueLen;
-	BOOLEAN             	AppCtrlQueueOverFlow;
+	BOOLEAN			AppCtrlQueueOverFlow;
 	atomic_t		CurrentApplicationCount;
-	atomic_t 		RegisteredApplicationCount;
-	BOOLEAN		  	LinkUpStatus;
-	BOOLEAN		    	TimerActive;
+	atomic_t		RegisteredApplicationCount;
+	BOOLEAN			LinkUpStatus;
+	BOOLEAN			TimerActive;
 	u32			StatisticsPointer;
 	struct sk_buff		*RxControlHead;
 	struct sk_buff		*RxControlTail;
-
 	struct semaphore	RxAppControlQueuelock;
 	struct semaphore	fw_download_sema;
-
-	PPER_TARANG_DATA    pTarangs;
-	spinlock_t			control_queue_lock;
+	PPER_TARANG_DATA	pTarangs;
+	spinlock_t		control_queue_lock;
 	wait_queue_head_t	process_read_wait_queue;
 
-	// the pointer to the first packet we have queued in send
-	// deserialized miniport support variables
-	atomic_t		    TotalPacketCount;
-	atomic_t		    TxPktAvail;
-
-	// this to keep track of the Tx and Rx MailBox Registers.
-	atomic_t		    CurrNumFreeTxDesc;
-	// to keep track the no of byte received
-	USHORT				PrevNumRecvDescs;
-	USHORT				CurrNumRecvDescs;
-	UINT				u32TotalDSD;
-	PacketInfo		    PackInfo[NO_OF_QUEUES];
+	/* the pointer to the first packet we have queued in send
+	 * deserialized miniport support variables
+	 */
+	atomic_t		TotalPacketCount;
+	atomic_t		TxPktAvail;
+
+	/* this to keep track of the Tx and Rx MailBox Registers. */
+	atomic_t		CurrNumFreeTxDesc;
+	/* to keep track the no of byte received */
+	USHORT			PrevNumRecvDescs;
+	USHORT			CurrNumRecvDescs;
+	UINT			u32TotalDSD;
+	PacketInfo		PackInfo[NO_OF_QUEUES];
 	S_CLASSIFIER_RULE	astClassifierTable[MAX_CLASSIFIERS];
-	BOOLEAN			    TransferMode;
+	BOOLEAN			TransferMode;
 
 	/*************** qos ******************/
-	BOOLEAN			    bETHCSEnabled;
-
-	ULONG			    BEBucketSize;
-	ULONG			    rtPSBucketSize;
-	UCHAR			    LinkStatus;
-	BOOLEAN			    AutoLinkUp;
-	BOOLEAN			    AutoSyncup;
-
-	int				major;
-	int				minor;
-	wait_queue_head_t 	tx_packet_wait_queue;
-	wait_queue_head_t 	process_rx_cntrlpkt;
-	atomic_t			process_waiting;
-	BOOLEAN 			fw_download_done;
-
-	char 				*txctlpacket[MAX_CNTRL_PKTS];
-	atomic_t			cntrlpktCnt ;
-	atomic_t			index_app_read_cntrlpkt;
-	atomic_t			index_wr_txcntrlpkt;
-	atomic_t			index_rd_txcntrlpkt;
-	UINT				index_datpkt;
-	struct semaphore 	rdmwrmsync;
+	BOOLEAN			bETHCSEnabled;
+	ULONG			BEBucketSize;
+	ULONG			rtPSBucketSize;
+	UCHAR			LinkStatus;
+	BOOLEAN			AutoLinkUp;
+	BOOLEAN			AutoSyncup;
+
+	int			major;
+	int			minor;
+	wait_queue_head_t	tx_packet_wait_queue;
+	wait_queue_head_t	process_rx_cntrlpkt;
+	atomic_t		process_waiting;
+	BOOLEAN			fw_download_done;
+
+	char			*txctlpacket[MAX_CNTRL_PKTS];
+	atomic_t		cntrlpktCnt ;
+	atomic_t		index_app_read_cntrlpkt;
+	atomic_t		index_wr_txcntrlpkt;
+	atomic_t		index_rd_txcntrlpkt;
+	UINT			index_datpkt;
+	struct semaphore	rdmwrmsync;
 
 	STTARGETDSXBUFFER	astTargetDsxBuffer[MAX_TARGET_DSX_BUFFERS];
 	ULONG			ulFreeTargetBufferCnt;
-	ULONG              	ulCurrentTargetBuffer;
-	ULONG              	ulTotalTargetBuffersAvailable;
-
-	unsigned long 		chip_id;
-
-	wait_queue_head_t 	lowpower_mode_wait_queue;
-
+	ULONG			ulCurrentTargetBuffer;
+	ULONG			ulTotalTargetBuffersAvailable;
+	unsigned long		chip_id;
+	wait_queue_head_t	lowpower_mode_wait_queue;
 	BOOLEAN			bFlashBoot;
 	BOOLEAN			bBinDownloaded;
 	BOOLEAN			bCfgDownloaded;
 	BOOLEAN			bSyncUpRequestSent;
 	USHORT			usBestEffortQueueIndex;
-
-	wait_queue_head_t 	ioctl_fw_dnld_wait_queue;
-	BOOLEAN				waiting_to_fw_download_done;
-	pid_t				fw_download_process_pid;
+	wait_queue_head_t	ioctl_fw_dnld_wait_queue;
+	BOOLEAN			waiting_to_fw_download_done;
+	pid_t			fw_download_process_pid;
 	PSTARGETPARAMS		pstargetparams;
-	BOOLEAN				device_removed;
-	BOOLEAN				DeviceAccess;
-	BOOLEAN				bIsAutoCorrectEnabled;
-	BOOLEAN				bDDRInitDone;
-	INT				DDRSetting;
-	ULONG				ulPowerSaveMode;
-	spinlock_t			txtransmitlock;
-	B_UINT8				txtransmit_running;
+	BOOLEAN			device_removed;
+	BOOLEAN			DeviceAccess;
+	BOOLEAN			bIsAutoCorrectEnabled;
+	BOOLEAN			bDDRInitDone;
+	int			DDRSetting;
+	ULONG			ulPowerSaveMode;
+	spinlock_t		txtransmitlock;
+	B_UINT8			txtransmit_running;
 	/* Thread for control packet handling */
-	struct task_struct 	*control_packet_handler;
+	struct task_struct	*control_packet_handler;
 	/* thread for transmitting packets. */
-	struct task_struct 	*transmit_packet_thread;
+	struct task_struct	*transmit_packet_thread;
 
 	/* LED Related Structures */
 	LED_INFO_STRUCT		LEDInfo;
@@ -482,39 +438,38 @@ struct _MINI_ADAPTER
 	/* Driver State for LED Blinking */
 	LedEventInfo_t		DriverState;
 	/* Interface Specific */
-	PVOID				pvInterfaceAdapter;
-	int (*bcm_file_download)( PVOID,
-                        struct file *,
-                        unsigned int);
-	int (*bcm_file_readback_from_chip)( PVOID,
-                        struct file *,
-                        unsigned int);
-	INT (*interface_rdm)(PVOID,
-			UINT ,
-			PVOID ,
-			INT);
-	INT (*interface_wrm)(PVOID,
-			UINT ,
-			PVOID ,
-			INT);
+	PVOID			pvInterfaceAdapter;
+	int (*bcm_file_download)(PVOID,
+				struct file *,
+				unsigned int);
+	int (*bcm_file_readback_from_chip)(PVOID,
+					struct file *,
+					unsigned int);
+	int (*interface_rdm)(PVOID,
+			UINT,
+			PVOID,
+			int);
+	int (*interface_wrm)(PVOID,
+			UINT,
+			PVOID,
+			int);
 	int (*interface_transmit)(PVOID, PVOID , UINT);
 	BOOLEAN			IdleMode;
 	BOOLEAN			bDregRequestSentInIdleMode;
 	BOOLEAN			bTriedToWakeUpFromlowPowerMode;
 	BOOLEAN			bShutStatus;
 	BOOLEAN			bWakeUpDevice;
-	unsigned int	usIdleModePattern;
-	//BOOLEAN			bTriedToWakeUpFromShutdown;
+	unsigned int		usIdleModePattern;
+	/* BOOLEAN			bTriedToWakeUpFromShutdown; */
 	BOOLEAN			bLinkDownRequested;
-
-	int 			downloadDDR;
-	PHS_DEVICE_EXTENSION stBCMPhsContext;
-	S_HDR_SUPRESSION_CONTEXTINFO	stPhsTxContextInfo;
+	int			downloadDDR;
+	PHS_DEVICE_EXTENSION	stBCMPhsContext;
+	S_HDR_SUPRESSION_CONTEXTINFO stPhsTxContextInfo;
 	uint8_t			ucaPHSPktRestoreBuf[2048];
 	uint8_t			bPHSEnabled;
 	BOOLEAN			AutoFirmDld;
-	BOOLEAN         bMipsConfig;
-	BOOLEAN         bDPLLConfig;
+	BOOLEAN			bMipsConfig;
+	BOOLEAN			bDPLLConfig;
 	UINT32			aTxPktSizeHist[MIBS_MAX_HIST_ENTRIES];
 	UINT32			aRxPktSizeHist[MIBS_MAX_HIST_ENTRIES];
 	S_FRAGMENTED_PACKET_INFO astFragmentedPktClassifierTable[MAX_FRAGMENTEDIP_CLASSIFICATION_ENTRIES];
@@ -527,108 +482,101 @@ struct _MINI_ADAPTER
 	BOOLEAN			bStatusWrite;
 	UINT			uiNVMDSDSize;
 	UINT			uiVendorExtnFlag;
-	 //it will always represent chosen DSD at any point of time.
-	 // Generally it is Active DSD but in case of NVM RD/WR it might be different.
+	/* it will always represent chosen DSD at any point of time.
+	 * Generally it is Active DSD but in case of NVM RD/WR it might be different.
+	 */
 	UINT			ulFlashCalStart;
-	ULONG                   ulFlashControlSectionStart;
-	ULONG                   ulFlashWriteSize;
-	ULONG                   ulFlashID;
-	FP_FLASH_WRITE          fpFlashWrite;
-	FP_FLASH_WRITE_STATUS   fpFlashWriteWithStatusCheck;
-
+	ULONG			ulFlashControlSectionStart;
+	ULONG			ulFlashWriteSize;
+	ULONG			ulFlashID;
+	FP_FLASH_WRITE		fpFlashWrite;
+	FP_FLASH_WRITE_STATUS	fpFlashWriteWithStatusCheck;
 
 	struct semaphore	NVMRdmWrmLock;
+	struct device		*pstCreatedClassDevice;
 
-	struct device *pstCreatedClassDevice;
-
-//	BOOLEAN				InterfaceUpStatus;
-	PFLASH2X_CS_INFO psFlash2xCSInfo;
-	PFLASH_CS_INFO psFlashCSInfo ;
+	/*	BOOLEAN				InterfaceUpStatus; */
+	PFLASH2X_CS_INFO	psFlash2xCSInfo;
+	PFLASH_CS_INFO		psFlashCSInfo;
 	PFLASH2X_VENDORSPECIFIC_INFO psFlash2xVendorInfo;
-	UINT uiFlashBaseAdd; //Flash start address
-	UINT uiActiveISOOffset; //Active ISO offset chosen before f/w download
-	FLASH2X_SECTION_VAL eActiveISO; //Active ISO section val
-	FLASH2X_SECTION_VAL eActiveDSD;	//Active DSD val chosen before f/w download
-	UINT uiActiveDSDOffsetAtFwDld;  //For accessing Active DSD chosen before f/w download
-	UINT uiFlashLayoutMajorVersion ;
-	UINT uiFlashLayoutMinorVersion;
-	BOOLEAN bAllDSDWriteAllow ;
-	BOOLEAN bSigCorrupted ;
-	//this should be set who so ever want to change the Headers. after Wrtie it should be reset immediately.
-	BOOLEAN bHeaderChangeAllowed ;
-	INT SelectedChip ;
-	BOOLEAN bEndPointHalted;
-	//while bFlashRawRead will be true, Driver  ignore map lay out and consider flash as of without any map.
-	BOOLEAN bFlashRawRead;
-	BOOLEAN			bPreparingForLowPowerMode ;
-	BOOLEAN bDoSuspend ;
-	UINT syscfgBefFwDld ;
-	BOOLEAN StopAllXaction ;
-	UINT32	liTimeSinceLastNetEntry; //Used to Support extended CAPI requirements from
+	UINT			uiFlashBaseAdd; /* Flash start address */
+	UINT			uiActiveISOOffset; /* Active ISO offset chosen before f/w download */
+	FLASH2X_SECTION_VAL	eActiveISO; /* Active ISO section val */
+	FLASH2X_SECTION_VAL	eActiveDSD;	/* Active DSD val chosen before f/w download */
+	UINT			uiActiveDSDOffsetAtFwDld;  /* For accessing Active DSD chosen before f/w download */
+	UINT			uiFlashLayoutMajorVersion;
+	UINT			uiFlashLayoutMinorVersion;
+	BOOLEAN			bAllDSDWriteAllow;
+	BOOLEAN			bSigCorrupted;
+	/* this should be set who so ever want to change the Headers. after Wrtie it should be reset immediately. */
+	BOOLEAN			bHeaderChangeAllowed;
+	int			SelectedChip;
+	BOOLEAN			bEndPointHalted;
+	/* while bFlashRawRead will be true, Driver  ignore map lay out and consider flash as of without any map. */
+	BOOLEAN			bFlashRawRead;
+	BOOLEAN			bPreparingForLowPowerMode;
+	BOOLEAN			bDoSuspend;
+	UINT			syscfgBefFwDld;
+	BOOLEAN			StopAllXaction;
+	UINT32			liTimeSinceLastNetEntry; /* Used to Support extended CAPI requirements from */
 	struct semaphore	LowPowerModeSync;
-	ULONG	liDrainCalculated;
-	UINT gpioBitMap;
-
-    S_BCM_DEBUG_STATE stDebugState;
-
+	ULONG			liDrainCalculated;
+	UINT			gpioBitMap;
+	S_BCM_DEBUG_STATE	stDebugState;
 };
 typedef struct _MINI_ADAPTER MINI_ADAPTER, *PMINI_ADAPTER;
 
-#define GET_BCM_ADAPTER(net_dev)	netdev_priv(net_dev)
+#define GET_BCM_ADAPTER(net_dev) netdev_priv(net_dev)
 
 struct _ETH_HEADER_STRUC {
-    UCHAR       au8DestinationAddress[6];
-    UCHAR       au8SourceAddress[6];
-    USHORT      u16Etype;
-}__attribute__((packed));
+	UCHAR	au8DestinationAddress[6];
+	UCHAR	au8SourceAddress[6];
+	USHORT	u16Etype;
+} __packed;
 typedef struct _ETH_HEADER_STRUC ETH_HEADER_STRUC, *PETH_HEADER_STRUC;
 
+typedef struct FirmwareInfo {
+	void	__user *pvMappedFirmwareAddress;
+	ULONG	u32FirmwareLength;
+	ULONG	u32StartingAddress;
+} __packed FIRMWARE_INFO, *PFIRMWARE_INFO;
 
-typedef struct FirmwareInfo
-{
-	void __user *	pvMappedFirmwareAddress;
-	ULONG		u32FirmwareLength;
-	ULONG		u32StartingAddress;
-}__attribute__((packed)) FIRMWARE_INFO, *PFIRMWARE_INFO;
-
-// holds the value of net_device structure..
+/* holds the value of net_device structure.. */
 extern struct net_device *gblpnetdev;
-typedef struct _cntl_pkt{
-	PMINI_ADAPTER 	Adapter;
-	PLEADER 		PLeader;
-}cntl_pkt;
+typedef struct _cntl_pkt {
+	PMINI_ADAPTER	Adapter;
+	PLEADER		PLeader;
+} cntl_pkt;
 typedef LINK_REQUEST CONTROL_MESSAGE;
 
-typedef struct _DDR_SETTING
-{
+typedef struct _DDR_SETTING {
 	UINT ulRegAddress;
 	UINT ulRegValue;
-}DDR_SETTING, *PDDR_SETTING;
+} DDR_SETTING, *PDDR_SETTING;
 typedef DDR_SETTING DDR_SET_NODE, *PDDR_SET_NODE;
-INT
-InitAdapter(PMINI_ADAPTER psAdapter);
-
-// =====================================================================
-// Beceem vendor request codes for EP0
-// =====================================================================
+int InitAdapter(PMINI_ADAPTER psAdapter);
 
-#define BCM_REQUEST_READ  0x2
-#define BCM_REQUEST_WRITE 0x1
-#define EP2_MPS_REG  0x0F0110A0
-#define EP2_MPS 	 0x40
+/* =====================================================================
+ * Beceem vendor request codes for EP0
+ * =====================================================================
+ */
 
-#define EP2_CFG_REG  0x0F0110A8
-#define EP2_CFG_INT  0x27
-#define EP2_CFG_BULK 0x25
+#define BCM_REQUEST_READ	0x2
+#define BCM_REQUEST_WRITE	0x1
+#define EP2_MPS_REG		0x0F0110A0
+#define EP2_MPS			0x40
 
-#define EP4_MPS_REG  0x0F0110F0
-#define EP4_MPS      0x8C
+#define EP2_CFG_REG	0x0F0110A8
+#define EP2_CFG_INT	0x27
+#define EP2_CFG_BULK	0x25
 
-#define EP4_CFG_REG  0x0F0110F8
+#define EP4_MPS_REG	0x0F0110F0
+#define EP4_MPS		0x8C
 
-#define ISO_MPS_REG  0x0F0110C8
-#define ISO_MPS      0x00000000
+#define EP4_CFG_REG	0x0F0110F8
 
+#define ISO_MPS_REG	0x0F0110C8
+#define ISO_MPS		0x00000000
 
 #define EP1 0
 #define EP2 1
@@ -637,12 +585,9 @@ InitAdapter(PMINI_ADAPTER psAdapter);
 #define EP5 4
 #define EP6 5
 
-
-typedef enum eInterface_setting
-{
+typedef enum eInterface_setting {
 	DEFAULT_SETTING_0  = 0,
 	ALTERNATE_SETTING_1 = 1,
-}INTERFACE_SETTING;
-
-#endif	//__ADAPTER_H__
+} INTERFACE_SETTING;
 
+#endif	/* __ADAPTER_H__ */
diff --git a/drivers/staging/bcm/DDRInit.c b/drivers/staging/bcm/DDRInit.c
index 1c7db81a1ee..2b46f4d4ef0 100644
--- a/drivers/staging/bcm/DDRInit.c
+++ b/drivers/staging/bcm/DDRInit.c
@@ -1115,20 +1115,20 @@ int download_ddr_settings(PMINI_ADAPTER Adapter)
 	    {
 	        case DDR_80_MHZ:
 				psDDRSetting = asT3LP_DDRSetting80MHz;
-                RegCount = (sizeof(asT3LP_DDRSetting80MHz)/sizeof(DDR_SET_NODE));
+                RegCount = ARRAY_SIZE(asT3LP_DDRSetting80MHz);
 				RegCount -= T3LP_SKIP_CLOCK_PROGRAM_DUMP_80MHZ ;
                 psDDRSetting += T3LP_SKIP_CLOCK_PROGRAM_DUMP_80MHZ;
 			break;
 		    case DDR_100_MHZ:
 				psDDRSetting = asT3LP_DDRSetting100MHz;
-			    RegCount = (sizeof(asT3LP_DDRSetting100MHz)/sizeof(DDR_SET_NODE));
+			    RegCount = ARRAY_SIZE(asT3LP_DDRSetting100MHz);
 				RegCount -= T3LP_SKIP_CLOCK_PROGRAM_DUMP_100MHZ ;
                 psDDRSetting += T3LP_SKIP_CLOCK_PROGRAM_DUMP_100MHZ;
 			    break;
 		     case DDR_133_MHZ:
 				bOverrideSelfRefresh = TRUE;
 				psDDRSetting = asT3LP_DDRSetting133MHz;
-			    RegCount = (sizeof(asT3LP_DDRSetting133MHz)/sizeof(DDR_SET_NODE));
+			    RegCount = ARRAY_SIZE(asT3LP_DDRSetting133MHz);
 				RegCount -= T3LP_SKIP_CLOCK_PROGRAM_DUMP_133MHZ ;
 		        psDDRSetting += T3LP_SKIP_CLOCK_PROGRAM_DUMP_133MHZ;
 				break;
@@ -1146,20 +1146,20 @@ int download_ddr_settings(PMINI_ADAPTER Adapter)
 	    {
 	        case DDR_80_MHZ:
 				psDDRSetting = asT3LPB_DDRSetting80MHz;
-                RegCount=(sizeof(asT3LPB_DDRSetting80MHz)/sizeof(DDR_SET_NODE));
+                RegCount=ARRAY_SIZE(asT3LPB_DDRSetting80MHz);
 				RegCount -= T3LPB_SKIP_CLOCK_PROGRAM_DUMP_80MHZ ;
                 psDDRSetting += T3LPB_SKIP_CLOCK_PROGRAM_DUMP_80MHZ;
 			break;
 		    case DDR_100_MHZ:
 				psDDRSetting = asT3LPB_DDRSetting100MHz;
-			    RegCount = (sizeof(asT3LPB_DDRSetting100MHz)/sizeof(DDR_SET_NODE));
+			    RegCount = ARRAY_SIZE(asT3LPB_DDRSetting100MHz);
 				RegCount -= T3LPB_SKIP_CLOCK_PROGRAM_DUMP_100MHZ ;
                 psDDRSetting += T3LPB_SKIP_CLOCK_PROGRAM_DUMP_100MHZ;
 			    break;
 		     case DDR_133_MHZ:
 				bOverrideSelfRefresh = TRUE;
 				psDDRSetting = asT3LPB_DDRSetting133MHz;
-			    RegCount = (sizeof(asT3LPB_DDRSetting133MHz)/sizeof(DDR_SET_NODE));
+			    RegCount = ARRAY_SIZE(asT3LPB_DDRSetting133MHz);
 				RegCount -= T3LPB_SKIP_CLOCK_PROGRAM_DUMP_133MHZ ;
 		        psDDRSetting += T3LPB_SKIP_CLOCK_PROGRAM_DUMP_133MHZ;
 				break;
@@ -1167,7 +1167,7 @@ int download_ddr_settings(PMINI_ADAPTER Adapter)
 			case DDR_160_MHZ:
 					bOverrideSelfRefresh = TRUE;
 					psDDRSetting = asT3LPB_DDRSetting160MHz;
-					RegCount = sizeof(asT3LPB_DDRSetting160MHz)/sizeof(DDR_SET_NODE);
+					RegCount = ARRAY_SIZE(asT3LPB_DDRSetting160MHz);
 					RegCount -= T3LPB_SKIP_CLOCK_PROGRAM_DUMP_160MHZ;
 					psDDRSetting += T3LPB_SKIP_CLOCK_PROGRAM_DUMP_160MHZ;
 
@@ -1181,19 +1181,19 @@ int download_ddr_settings(PMINI_ADAPTER Adapter)
 	    {
 	        case DDR_80_MHZ:
 				psDDRSetting = asT3_DDRSetting80MHz;
-                RegCount = (sizeof(asT3_DDRSetting80MHz)/sizeof(DDR_SET_NODE));
+                RegCount = ARRAY_SIZE(asT3_DDRSetting80MHz);
 				RegCount-=T3_SKIP_CLOCK_PROGRAM_DUMP_80MHZ ;
                 psDDRSetting += T3_SKIP_CLOCK_PROGRAM_DUMP_80MHZ;
 			break;
 		    case DDR_100_MHZ:
 				psDDRSetting = asT3_DDRSetting100MHz;
-			    RegCount = (sizeof(asT3_DDRSetting100MHz)/sizeof(DDR_SET_NODE));
+			    RegCount = ARRAY_SIZE(asT3_DDRSetting100MHz);
 				RegCount-=T3_SKIP_CLOCK_PROGRAM_DUMP_100MHZ ;
                 psDDRSetting += T3_SKIP_CLOCK_PROGRAM_DUMP_100MHZ;
 			    break;
 		     case DDR_133_MHZ:
 				psDDRSetting = asT3_DDRSetting133MHz;
-			    RegCount = (sizeof(asT3_DDRSetting133MHz)/sizeof(DDR_SET_NODE));
+			    RegCount = ARRAY_SIZE(asT3_DDRSetting133MHz);
 				RegCount-=T3_SKIP_CLOCK_PROGRAM_DUMP_133MHZ ;
 		        psDDRSetting += T3_SKIP_CLOCK_PROGRAM_DUMP_133MHZ ;
 				break;
@@ -1207,20 +1207,20 @@ int download_ddr_settings(PMINI_ADAPTER Adapter)
 		    {
 		        case DDR_80_MHZ:
 					psDDRSetting = asT3B_DDRSetting80MHz;
-                    RegCount = (sizeof(asT3B_DDRSetting80MHz)/sizeof(DDR_SET_NODE));
+                    RegCount = ARRAY_SIZE(asT3B_DDRSetting80MHz);
                     RegCount -= T3B_SKIP_CLOCK_PROGRAM_DUMP_80MHZ ;
                     psDDRSetting += T3B_SKIP_CLOCK_PROGRAM_DUMP_80MHZ;
 			        break;
 		        case DDR_100_MHZ:
 					psDDRSetting = asT3B_DDRSetting100MHz;
-			        RegCount = (sizeof(asT3B_DDRSetting100MHz)/sizeof(DDR_SET_NODE));
+			        RegCount = ARRAY_SIZE(asT3B_DDRSetting100MHz);
                     RegCount -= T3B_SKIP_CLOCK_PROGRAM_DUMP_100MHZ ;
                     psDDRSetting += T3B_SKIP_CLOCK_PROGRAM_DUMP_100MHZ;
 			        break;
 		        case DDR_133_MHZ:
 					bOverrideSelfRefresh = TRUE;
 					psDDRSetting = asT3B_DDRSetting133MHz;
-			        RegCount = (sizeof(asT3B_DDRSetting133MHz)/sizeof(DDR_SET_NODE));
+			        RegCount = ARRAY_SIZE(asT3B_DDRSetting133MHz);
 	                RegCount -= T3B_SKIP_CLOCK_PROGRAM_DUMP_133MHZ ;
 		            psDDRSetting += T3B_SKIP_CLOCK_PROGRAM_DUMP_133MHZ;
 					break;
diff --git a/drivers/staging/bcm/IPv6Protocol.c b/drivers/staging/bcm/IPv6Protocol.c
index 5b4fd372ec3..1da21642c18 100644
--- a/drivers/staging/bcm/IPv6Protocol.c
+++ b/drivers/staging/bcm/IPv6Protocol.c
@@ -1,51 +1,52 @@
 #include "headers.h"
 
-static BOOLEAN MatchSrcIpv6Address(S_CLASSIFIER_RULE *pstClassifierRule,IPV6Header *pstIpv6Header);
-static BOOLEAN MatchDestIpv6Address(S_CLASSIFIER_RULE *pstClassifierRule,IPV6Header *pstIpv6Header);
+static BOOLEAN MatchSrcIpv6Address(S_CLASSIFIER_RULE *pstClassifierRule,
+	IPV6Header *pstIpv6Header);
+static BOOLEAN MatchDestIpv6Address(S_CLASSIFIER_RULE *pstClassifierRule,
+	IPV6Header *pstIpv6Header);
 static VOID DumpIpv6Header(IPV6Header *pstIpv6Header);
 
-static UCHAR * GetNextIPV6ChainedHeader(UCHAR **ppucPayload,UCHAR *pucNextHeader,BOOLEAN *bParseDone,USHORT *pusPayloadLength)
+static UCHAR *GetNextIPV6ChainedHeader(UCHAR **ppucPayload,
+	UCHAR *pucNextHeader, BOOLEAN *bParseDone, USHORT *pusPayloadLength)
 {
 	UCHAR *pucRetHeaderPtr = NULL;
 	UCHAR *pucPayloadPtr = NULL;
 	USHORT  usNextHeaderOffset = 0 ;
-    PMINI_ADAPTER Adapter = GET_BCM_ADAPTER(gblpnetdev);
+	PMINI_ADAPTER Adapter = GET_BCM_ADAPTER(gblpnetdev);
 
-	if((NULL == ppucPayload) || (*pusPayloadLength == 0) || (*bParseDone))
-	{
+	if ((ppucPayload == NULL) || (*pusPayloadLength == 0) ||
+		(*bParseDone)) {
 		*bParseDone = TRUE;
 		return NULL;
-
 	}
 
 	pucRetHeaderPtr = *ppucPayload;
 	pucPayloadPtr = *ppucPayload;
 
-	if(!pucRetHeaderPtr || !pucPayloadPtr)
-	{
+	if (!pucRetHeaderPtr || !pucPayloadPtr) {
 		*bParseDone = TRUE;
 		return NULL;
 	}
 
-	//Get the Nextt Header Type
+	/* Get the Nextt Header Type */
 	*bParseDone = FALSE;
 
 
-
-	switch(*pucNextHeader)
-	{
+	switch (*pucNextHeader) {
 	case IPV6HDR_TYPE_HOPBYHOP:
 		{
 
-			BCM_DEBUG_PRINT( Adapter,DBG_TYPE_TX, IPV6_DBG, DBG_LVL_ALL, "\nIPv6 HopByHop Header");
-			usNextHeaderOffset+=sizeof(IPV6HopByHopOptionsHeader);
+			BCM_DEBUG_PRINT(Adapter, DBG_TYPE_TX, IPV6_DBG,
+					DBG_LVL_ALL, "\nIPv6 HopByHop Header");
+			usNextHeaderOffset += sizeof(IPV6HopByHopOptionsHeader);
 		}
 		break;
 
 	case IPV6HDR_TYPE_ROUTING:
 		{
 			IPV6RoutingHeader *pstIpv6RoutingHeader;
-			BCM_DEBUG_PRINT( Adapter,DBG_TYPE_TX, IPV6_DBG, DBG_LVL_ALL, "\nIPv6 Routing Header");
+			BCM_DEBUG_PRINT(Adapter, DBG_TYPE_TX, IPV6_DBG,
+					DBG_LVL_ALL, "\nIPv6 Routing Header");
 			pstIpv6RoutingHeader = (IPV6RoutingHeader *)pucPayloadPtr;
 			usNextHeaderOffset += sizeof(IPV6RoutingHeader);
 			usNextHeaderOffset += pstIpv6RoutingHeader->ucNumAddresses * IPV6_ADDRESS_SIZEINBYTES;
@@ -54,8 +55,10 @@ static UCHAR * GetNextIPV6ChainedHeader(UCHAR **ppucPayload,UCHAR *pucNextHeader
 		break;
 	case IPV6HDR_TYPE_FRAGMENTATION:
 		{
-			BCM_DEBUG_PRINT( Adapter,DBG_TYPE_TX, IPV6_DBG, DBG_LVL_ALL, "\nIPv6 Fragmentation Header");
-			usNextHeaderOffset+= sizeof(IPV6FragmentHeader);
+			BCM_DEBUG_PRINT(Adapter, DBG_TYPE_TX, IPV6_DBG,
+					DBG_LVL_ALL,
+					"\nIPv6 Fragmentation Header");
+			usNextHeaderOffset += sizeof(IPV6FragmentHeader);
 
 		}
 		break;
@@ -63,9 +66,11 @@ static UCHAR * GetNextIPV6ChainedHeader(UCHAR **ppucPayload,UCHAR *pucNextHeader
 		{
 			IPV6DestOptionsHeader *pstIpv6DestOptsHdr = (IPV6DestOptionsHeader *)pucPayloadPtr;
 			int nTotalOptions = pstIpv6DestOptsHdr->ucHdrExtLen;
-			BCM_DEBUG_PRINT( Adapter,DBG_TYPE_TX, IPV6_DBG, DBG_LVL_ALL, "\nIPv6 DestOpts Header Header");
-			usNextHeaderOffset+= sizeof(IPV6DestOptionsHeader);
-			usNextHeaderOffset+= nTotalOptions * IPV6_DESTOPTS_HDR_OPTIONSIZE ;
+			BCM_DEBUG_PRINT(Adapter, DBG_TYPE_TX, IPV6_DBG,
+					DBG_LVL_ALL,
+					"\nIPv6 DestOpts Header Header");
+			usNextHeaderOffset += sizeof(IPV6DestOptionsHeader);
+			usNextHeaderOffset += nTotalOptions * IPV6_DESTOPTS_HDR_OPTIONSIZE ;
 
 		}
 		break;
@@ -73,36 +78,43 @@ static UCHAR * GetNextIPV6ChainedHeader(UCHAR **ppucPayload,UCHAR *pucNextHeader
 		{
 			IPV6AuthenticationHeader *pstIpv6AuthHdr = (IPV6AuthenticationHeader *)pucPayloadPtr;
 			int nHdrLen = pstIpv6AuthHdr->ucLength;
-			BCM_DEBUG_PRINT( Adapter,DBG_TYPE_TX, IPV6_DBG, DBG_LVL_ALL, "\nIPv6 Authentication Header");
-			usNextHeaderOffset+= nHdrLen * 4;
+			BCM_DEBUG_PRINT(Adapter, DBG_TYPE_TX, IPV6_DBG,
+					DBG_LVL_ALL,
+					"\nIPv6 Authentication Header");
+			usNextHeaderOffset += nHdrLen * 4;
 		}
 		break;
 	case IPV6HDR_TYPE_ENCRYPTEDSECURITYPAYLOAD:
 		{
-			BCM_DEBUG_PRINT( Adapter,DBG_TYPE_TX, IPV6_DBG, DBG_LVL_ALL, "\nIPv6 Encrypted Security Payload Header");
+			BCM_DEBUG_PRINT(Adapter, DBG_TYPE_TX, IPV6_DBG,
+					DBG_LVL_ALL,
+					"\nIPv6 Encrypted Security Payload Header");
 			*bParseDone = TRUE;
 
 		}
 		break;
 	case IPV6_ICMP_HDR_TYPE:
 		{
-			BCM_DEBUG_PRINT( Adapter,DBG_TYPE_TX, IPV6_DBG, DBG_LVL_ALL, " ICMP Header");
+			BCM_DEBUG_PRINT(Adapter, DBG_TYPE_TX, IPV6_DBG,
+					DBG_LVL_ALL, "\nICMP Header");
 			*bParseDone = TRUE;
 		}
 		break;
 	case TCP_HEADER_TYPE:
 		{
-			BCM_DEBUG_PRINT( Adapter,DBG_TYPE_TX, IPV6_DBG, DBG_LVL_ALL, " \nTCP Header");
+			BCM_DEBUG_PRINT(Adapter, DBG_TYPE_TX, IPV6_DBG,
+					DBG_LVL_ALL, "\nTCP Header");
 			*bParseDone = TRUE;
 		}
 		break;
 	case UDP_HEADER_TYPE:
 		{
-			BCM_DEBUG_PRINT( Adapter,DBG_TYPE_TX, IPV6_DBG, DBG_LVL_ALL, " \nUDP Header");
+			BCM_DEBUG_PRINT(Adapter, DBG_TYPE_TX, IPV6_DBG,
+					DBG_LVL_ALL, "\nUDP Header");
 			*bParseDone = TRUE;
 		}
 		break;
-	default :
+	default:
 		{
 			*bParseDone = TRUE;
 
@@ -112,53 +124,49 @@ static UCHAR * GetNextIPV6ChainedHeader(UCHAR **ppucPayload,UCHAR *pucNextHeader
 
 	}
 
-	if(*bParseDone == FALSE)
-	{
-		if(*pusPayloadLength <= usNextHeaderOffset)
-		{
+	if (*bParseDone == FALSE) {
+		if (*pusPayloadLength <= usNextHeaderOffset) {
 			*bParseDone = TRUE;
-		}
-		else
-		{
+		} else {
 			*pucNextHeader = *pucPayloadPtr;
-			pucPayloadPtr+=usNextHeaderOffset;
-			(*pusPayloadLength)-=usNextHeaderOffset;
+			pucPayloadPtr += usNextHeaderOffset;
+			(*pusPayloadLength) -= usNextHeaderOffset;
 		}
 
 	}
 
-
-
 	*ppucPayload = pucPayloadPtr;
 	return pucRetHeaderPtr;
 }
 
 
-static UCHAR GetIpv6ProtocolPorts(UCHAR *pucPayload,USHORT *pusSrcPort,USHORT *pusDestPort,USHORT usPayloadLength,UCHAR ucNextHeader)
+static UCHAR GetIpv6ProtocolPorts(UCHAR *pucPayload, USHORT *pusSrcPort,
+	USHORT *pusDestPort, USHORT usPayloadLength, UCHAR ucNextHeader)
 {
 	UCHAR *pIpv6HdrScanContext = pucPayload;
 	BOOLEAN bDone = FALSE;
-	UCHAR ucHeaderType =0;
+	UCHAR ucHeaderType = 0;
 	UCHAR *pucNextHeader = NULL;
-    PMINI_ADAPTER Adapter = GET_BCM_ADAPTER(gblpnetdev);
+	PMINI_ADAPTER Adapter = GET_BCM_ADAPTER(gblpnetdev);
 
-	if( !pucPayload || (usPayloadLength == 0))
-	{
+	if (!pucPayload || (usPayloadLength == 0))
 		return 0;
-	}
 
 	*pusSrcPort = *pusDestPort = 0;
 	ucHeaderType = ucNextHeader;
-	while(!bDone)
-	{
-		pucNextHeader = GetNextIPV6ChainedHeader(&pIpv6HdrScanContext,&ucHeaderType,&bDone,&usPayloadLength);
-		if(bDone)
-		{
-			if((ucHeaderType==TCP_HEADER_TYPE) || (ucHeaderType == UDP_HEADER_TYPE))
-			{
-				 *pusSrcPort=*((PUSHORT)(pucNextHeader));
-				 *pusDestPort=*((PUSHORT)(pucNextHeader+2));
-				 BCM_DEBUG_PRINT( Adapter,DBG_TYPE_TX, IPV6_DBG, DBG_LVL_ALL, " \nProtocol Ports - Src Port :0x%x Dest Port : 0x%x",ntohs(*pusSrcPort),ntohs(*pusDestPort));
+	while (!bDone) {
+		pucNextHeader = GetNextIPV6ChainedHeader(&pIpv6HdrScanContext,
+					&ucHeaderType, &bDone, &usPayloadLength);
+		if (bDone) {
+			if ((ucHeaderType == TCP_HEADER_TYPE) ||
+				(ucHeaderType == UDP_HEADER_TYPE)) {
+				*pusSrcPort = *((PUSHORT)(pucNextHeader));
+				*pusDestPort = *((PUSHORT)(pucNextHeader+2));
+				BCM_DEBUG_PRINT(Adapter, DBG_TYPE_TX, IPV6_DBG,
+						DBG_LVL_ALL,
+						"\nProtocol Ports - Src Port :0x%x Dest Port : 0x%x",
+						ntohs(*pusSrcPort),
+						ntohs(*pusDestPort));
 			}
 			break;
 
@@ -168,92 +176,111 @@ static UCHAR GetIpv6ProtocolPorts(UCHAR *pucPayload,USHORT *pusSrcPort,USHORT *p
 }
 
 
-
-USHORT	IpVersion6(PMINI_ADAPTER Adapter, /**< Pointer to the driver control structure */
-					PVOID pcIpHeader, /**<Pointer to the IP Hdr of the packet*/
-					S_CLASSIFIER_RULE *pstClassifierRule )
+/*
+ * Arg 1 PMINI_ADAPTER Adapter is a pointer ot the driver contorl structure
+ * Arg 2 PVOID pcIpHeader is a pointer to the IP header of the packet
+ */
+USHORT	IpVersion6(PMINI_ADAPTER Adapter, PVOID pcIpHeader,
+					S_CLASSIFIER_RULE *pstClassifierRule)
 {
 	USHORT	ushDestPort = 0;
 	USHORT	ushSrcPort = 0;
-	UCHAR   ucNextProtocolAboveIP =0;
+	UCHAR   ucNextProtocolAboveIP = 0;
 	IPV6Header *pstIpv6Header = NULL;
 	BOOLEAN bClassificationSucceed = FALSE;
 
-	BCM_DEBUG_PRINT( Adapter,DBG_TYPE_TX, IPV6_DBG, DBG_LVL_ALL, "IpVersion6 ==========>\n");
+	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_TX, IPV6_DBG,
+			DBG_LVL_ALL, "IpVersion6 ==========>\n");
 
 	pstIpv6Header = (IPV6Header *)pcIpHeader;
 
 	DumpIpv6Header(pstIpv6Header);
 
-	//Try to get the next higher layer protocol and the Ports Nos if TCP or UDP
+	/*
+	 * Try to get the next higher layer protocol
+	 * and the Ports Nos if TCP or UDP
+	 */
 	ucNextProtocolAboveIP = GetIpv6ProtocolPorts((UCHAR *)(pcIpHeader + sizeof(IPV6Header)),
 							&ushSrcPort,
 							&ushDestPort,
 							pstIpv6Header->usPayloadLength,
 							pstIpv6Header->ucNextHeader);
 
-	do
-	{
-		if(0 == pstClassifierRule->ucDirection)
-		{
-			//cannot be processed for classification.
-		   // it is a down link connection
+	do {
+		if (pstClassifierRule->ucDirection == 0) {
+			/*
+			 * cannot be processed for classification.
+			 * it is a down link connection
+			 */
 			break;
 		}
 
-		if(!pstClassifierRule->bIpv6Protocol)
-		{
-			//We are looking for Ipv6 Classifiers . Lets ignore this classifier and try the next one.
+		if (!pstClassifierRule->bIpv6Protocol) {
+			/*
+			 * We are looking for Ipv6 Classifiers
+			 * Lets ignore this classifier and try the next one
+			 */
 			break;
 		}
 
-		bClassificationSucceed=MatchSrcIpv6Address(pstClassifierRule,pstIpv6Header);
-        if(!bClassificationSucceed)
-            break;
+		bClassificationSucceed = MatchSrcIpv6Address(pstClassifierRule,
+								pstIpv6Header);
+		if (!bClassificationSucceed)
+			break;
 
-        bClassificationSucceed=MatchDestIpv6Address(pstClassifierRule,pstIpv6Header);
-        if(!bClassificationSucceed)
-            break;
+		bClassificationSucceed = MatchDestIpv6Address(pstClassifierRule,
+								pstIpv6Header);
+		if (!bClassificationSucceed)
+			break;
 
-		//Match the protocol type.For IPv6 the next protocol at end of Chain of IPv6 prot headers
-		bClassificationSucceed=MatchProtocol(pstClassifierRule,ucNextProtocolAboveIP);
-        if(!bClassificationSucceed)
-            break;
-        BCM_DEBUG_PRINT( Adapter,DBG_TYPE_TX, IPV6_DBG, DBG_LVL_ALL, "\nIPv6 Protocol Matched");
+		/*
+		 * Match the protocol type.
+		 * For IPv6 the next protocol at end of
+		 * Chain of IPv6 prot headers
+		 */
+		bClassificationSucceed = MatchProtocol(pstClassifierRule,
+							ucNextProtocolAboveIP);
+		if (!bClassificationSucceed)
+			break;
 
-		if((ucNextProtocolAboveIP == TCP_HEADER_TYPE) || (ucNextProtocolAboveIP == UDP_HEADER_TYPE))
-		{
-			//Match Src Port
-			BCM_DEBUG_PRINT( Adapter,DBG_TYPE_TX, IPV6_DBG, DBG_LVL_ALL, "\nIPv6 Source Port:%x\n",ntohs(ushSrcPort));
-			bClassificationSucceed=MatchSrcPort(pstClassifierRule,ntohs(ushSrcPort));
-			if(!bClassificationSucceed)
+		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_TX, IPV6_DBG,
+				DBG_LVL_ALL, "\nIPv6 Protocol Matched");
+
+		if ((ucNextProtocolAboveIP == TCP_HEADER_TYPE) ||
+			(ucNextProtocolAboveIP == UDP_HEADER_TYPE)) {
+			/* Match Src Port */
+			BCM_DEBUG_PRINT(Adapter, DBG_TYPE_TX, IPV6_DBG,
+					DBG_LVL_ALL, "\nIPv6 Source Port:%x\n",
+					ntohs(ushSrcPort));
+			bClassificationSucceed = MatchSrcPort(pstClassifierRule,
+							ntohs(ushSrcPort));
+			if (!bClassificationSucceed)
 				break;
 
-			BCM_DEBUG_PRINT( Adapter,DBG_TYPE_TX, IPV6_DBG, DBG_LVL_ALL, "\nIPv6 Src Port Matched");
+			BCM_DEBUG_PRINT(Adapter, DBG_TYPE_TX, IPV6_DBG,
+					DBG_LVL_ALL, "\nIPv6 Src Port Matched");
 
-			//Match Dest Port
-			BCM_DEBUG_PRINT( Adapter,DBG_TYPE_TX, IPV6_DBG, DBG_LVL_ALL, "\nIPv6 Destination Port:%x\n",ntohs(ushDestPort));
-			bClassificationSucceed=MatchDestPort(pstClassifierRule,ntohs(ushDestPort));
-			if(!bClassificationSucceed)
+			/* Match Dest Port */
+			BCM_DEBUG_PRINT(Adapter, DBG_TYPE_TX, IPV6_DBG,
+					DBG_LVL_ALL, "\nIPv6 Destination Port:%x\n",
+					ntohs(ushDestPort));
+			bClassificationSucceed = MatchDestPort(pstClassifierRule,
+							ntohs(ushDestPort));
+			if (!bClassificationSucceed)
 				break;
-			BCM_DEBUG_PRINT( Adapter,DBG_TYPE_TX, IPV6_DBG, DBG_LVL_ALL, "\nIPv6 Dest Port Matched");
+			BCM_DEBUG_PRINT(Adapter, DBG_TYPE_TX, IPV6_DBG,
+					DBG_LVL_ALL, "\nIPv6 Dest Port Matched");
 		}
-	}while(0);
+	} while (0);
 
-	if(TRUE==bClassificationSucceed)
-	{
+	if (bClassificationSucceed == TRUE) {
 		INT iMatchedSFQueueIndex = 0;
-		iMatchedSFQueueIndex = SearchSfid(Adapter,pstClassifierRule->ulSFID);
-		if(iMatchedSFQueueIndex >= NO_OF_QUEUES)
-		{
+		iMatchedSFQueueIndex = SearchSfid(Adapter, pstClassifierRule->ulSFID);
+		if (iMatchedSFQueueIndex >= NO_OF_QUEUES) {
 			bClassificationSucceed = FALSE;
-		}
-		else
-		{
-			if(FALSE == Adapter->PackInfo[iMatchedSFQueueIndex].bActive)
-			{
+		} else {
+			if (Adapter->PackInfo[iMatchedSFQueueIndex].bActive == FALSE)
 				bClassificationSucceed = FALSE;
-			}
 		}
 	}
 
@@ -261,52 +288,55 @@ USHORT	IpVersion6(PMINI_ADAPTER Adapter, /**< Pointer to the driver control stru
 }
 
 
-static BOOLEAN MatchSrcIpv6Address(S_CLASSIFIER_RULE *pstClassifierRule,IPV6Header *pstIpv6Header)
+static BOOLEAN MatchSrcIpv6Address(S_CLASSIFIER_RULE *pstClassifierRule,
+	IPV6Header *pstIpv6Header)
 {
-	UINT uiLoopIndex=0;
-	UINT  uiIpv6AddIndex=0;
-	UINT  uiIpv6AddrNoLongWords = 4;
+	UINT uiLoopIndex = 0;
+	UINT uiIpv6AddIndex = 0;
+	UINT uiIpv6AddrNoLongWords = 4;
 	ULONG aulSrcIP[4];
-    PMINI_ADAPTER Adapter = GET_BCM_ADAPTER(gblpnetdev);
+	PMINI_ADAPTER Adapter = GET_BCM_ADAPTER(gblpnetdev);
 	/*
-	//This is the no. of Src Addresses ie Range of IP Addresses contained
-	//in the classifier rule for which we need to match
-	*/
+	 * This is the no. of Src Addresses ie Range of IP Addresses contained
+	 * in the classifier rule for which we need to match
+	 */
 	UINT  uiCountIPSrcAddresses = (UINT)pstClassifierRule->ucIPSourceAddressLength;
 
 
-	if(0 == uiCountIPSrcAddresses)
+	if (uiCountIPSrcAddresses == 0)
 		return TRUE;
 
 
-	//First Convert the Ip Address in the packet to Host Endian order
-	for(uiIpv6AddIndex=0;uiIpv6AddIndex<uiIpv6AddrNoLongWords;uiIpv6AddIndex++)
-	{
-		aulSrcIP[uiIpv6AddIndex]=ntohl(pstIpv6Header->ulSrcIpAddress[uiIpv6AddIndex]);
-	}
+	/* First Convert the Ip Address in the packet to Host Endian order */
+	for (uiIpv6AddIndex = 0; uiIpv6AddIndex < uiIpv6AddrNoLongWords; uiIpv6AddIndex++)
+		aulSrcIP[uiIpv6AddIndex] = ntohl(pstIpv6Header->ulSrcIpAddress[uiIpv6AddIndex]);
 
-	for(uiLoopIndex=0;uiLoopIndex<uiCountIPSrcAddresses;uiLoopIndex+=uiIpv6AddrNoLongWords)
-	{
-		BCM_DEBUG_PRINT( Adapter,DBG_TYPE_TX, IPV6_DBG, DBG_LVL_ALL, "\n Src Ipv6 Address In Received Packet : \n ");
+	for (uiLoopIndex = 0; uiLoopIndex < uiCountIPSrcAddresses; uiLoopIndex += uiIpv6AddrNoLongWords) {
+		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_TX, IPV6_DBG, DBG_LVL_ALL,
+				"\n Src Ipv6 Address In Received Packet :\n ");
 		DumpIpv6Address(aulSrcIP);
-		BCM_DEBUG_PRINT( Adapter,DBG_TYPE_TX, IPV6_DBG, DBG_LVL_ALL, "\n Src Ipv6 Mask In Classifier Rule: \n");
+		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_TX, IPV6_DBG, DBG_LVL_ALL,
+				"\n Src Ipv6 Mask In Classifier Rule:\n");
 		DumpIpv6Address(&pstClassifierRule->stSrcIpAddress.ulIpv6Mask[uiLoopIndex]);
-		BCM_DEBUG_PRINT( Adapter,DBG_TYPE_TX, IPV6_DBG, DBG_LVL_ALL, "\n Src Ipv6 Address In Classifier Rule : \n");
+		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_TX, IPV6_DBG, DBG_LVL_ALL,
+				"\n Src Ipv6 Address In Classifier Rule :\n");
 		DumpIpv6Address(&pstClassifierRule->stSrcIpAddress.ulIpv6Addr[uiLoopIndex]);
 
-		for(uiIpv6AddIndex=0;uiIpv6AddIndex<uiIpv6AddrNoLongWords;uiIpv6AddIndex++)
-		{
-			if((pstClassifierRule->stSrcIpAddress.ulIpv6Mask[uiLoopIndex+uiIpv6AddIndex] & aulSrcIP[uiIpv6AddIndex])
-				!= pstClassifierRule->stSrcIpAddress.ulIpv6Addr[uiLoopIndex+uiIpv6AddIndex])
-			{
-				//Match failed for current Ipv6 Address.Try next Ipv6 Address
+		for (uiIpv6AddIndex = 0; uiIpv6AddIndex < uiIpv6AddrNoLongWords; uiIpv6AddIndex++) {
+			if ((pstClassifierRule->stSrcIpAddress.ulIpv6Mask[uiLoopIndex+uiIpv6AddIndex] & aulSrcIP[uiIpv6AddIndex])
+				!= pstClassifierRule->stSrcIpAddress.ulIpv6Addr[uiLoopIndex+uiIpv6AddIndex]) {
+				/*
+				 * Match failed for current Ipv6 Address
+				 * Try next Ipv6 Address
+				 */
 				break;
 			}
 
-			if(uiIpv6AddIndex ==  uiIpv6AddrNoLongWords-1)
-			{
-				//Match Found
-				BCM_DEBUG_PRINT( Adapter,DBG_TYPE_TX, IPV6_DBG, DBG_LVL_ALL, "Ipv6 Src Ip Address Matched\n");
+			if (uiIpv6AddIndex ==  uiIpv6AddrNoLongWords-1) {
+				/* Match Found */
+				BCM_DEBUG_PRINT(Adapter, DBG_TYPE_TX, IPV6_DBG,
+						DBG_LVL_ALL,
+						"Ipv6 Src Ip Address Matched\n");
 				return TRUE;
 			}
 		}
@@ -314,52 +344,56 @@ static BOOLEAN MatchSrcIpv6Address(S_CLASSIFIER_RULE *pstClassifierRule,IPV6Head
 	return FALSE;
 }
 
-static BOOLEAN MatchDestIpv6Address(S_CLASSIFIER_RULE *pstClassifierRule,IPV6Header *pstIpv6Header)
+static BOOLEAN MatchDestIpv6Address(S_CLASSIFIER_RULE *pstClassifierRule,
+	IPV6Header *pstIpv6Header)
 {
-	UINT uiLoopIndex=0;
-	UINT  uiIpv6AddIndex=0;
-	UINT  uiIpv6AddrNoLongWords = 4;
+	UINT uiLoopIndex = 0;
+	UINT uiIpv6AddIndex = 0;
+	UINT uiIpv6AddrNoLongWords = 4;
 	ULONG aulDestIP[4];
-    PMINI_ADAPTER Adapter = GET_BCM_ADAPTER(gblpnetdev);
+	PMINI_ADAPTER Adapter = GET_BCM_ADAPTER(gblpnetdev);
 	/*
-	//This is the no. of Destination Addresses ie Range of IP Addresses contained
-	//in the classifier rule for which we need to match
-	*/
+	 * This is the no. of Destination Addresses
+	 * ie Range of IP Addresses contained in the classifier rule
+	 * for which we need to match
+	 */
 	UINT  uiCountIPDestinationAddresses = (UINT)pstClassifierRule->ucIPDestinationAddressLength;
 
 
-	if(0 == uiCountIPDestinationAddresses)
+	if (uiCountIPDestinationAddresses == 0)
 		return TRUE;
 
 
-	//First Convert the Ip Address in the packet to Host Endian order
-	for(uiIpv6AddIndex=0;uiIpv6AddIndex<uiIpv6AddrNoLongWords;uiIpv6AddIndex++)
-	{
-		aulDestIP[uiIpv6AddIndex]=ntohl(pstIpv6Header->ulDestIpAddress[uiIpv6AddIndex]);
-	}
+	/* First Convert the Ip Address in the packet to Host Endian order */
+	for (uiIpv6AddIndex = 0; uiIpv6AddIndex < uiIpv6AddrNoLongWords; uiIpv6AddIndex++)
+		aulDestIP[uiIpv6AddIndex] = ntohl(pstIpv6Header->ulDestIpAddress[uiIpv6AddIndex]);
 
-	for(uiLoopIndex=0;uiLoopIndex<uiCountIPDestinationAddresses;uiLoopIndex+=uiIpv6AddrNoLongWords)
-	{
-		BCM_DEBUG_PRINT( Adapter,DBG_TYPE_TX, IPV6_DBG, DBG_LVL_ALL, "\n Destination Ipv6 Address In Received Packet : \n ");
+	for (uiLoopIndex = 0; uiLoopIndex < uiCountIPDestinationAddresses; uiLoopIndex += uiIpv6AddrNoLongWords) {
+		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_TX, IPV6_DBG, DBG_LVL_ALL,
+				"\n Destination Ipv6 Address In Received Packet :\n ");
 		DumpIpv6Address(aulDestIP);
-		BCM_DEBUG_PRINT( Adapter,DBG_TYPE_TX, IPV6_DBG, DBG_LVL_ALL, "\n Destination Ipv6 Mask In Classifier Rule: \n");
+		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_TX, IPV6_DBG, DBG_LVL_ALL,
+				"\n Destination Ipv6 Mask In Classifier Rule :\n");
 		DumpIpv6Address(&pstClassifierRule->stDestIpAddress.ulIpv6Mask[uiLoopIndex]);
-		BCM_DEBUG_PRINT( Adapter,DBG_TYPE_TX, IPV6_DBG, DBG_LVL_ALL, "\n Destination Ipv6 Address In Classifier Rule : \n");
+		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_TX, IPV6_DBG, DBG_LVL_ALL,
+				"\n Destination Ipv6 Address In Classifier Rule :\n");
 		DumpIpv6Address(&pstClassifierRule->stDestIpAddress.ulIpv6Addr[uiLoopIndex]);
 
-		for(uiIpv6AddIndex=0;uiIpv6AddIndex<uiIpv6AddrNoLongWords;uiIpv6AddIndex++)
-		{
-			if((pstClassifierRule->stDestIpAddress.ulIpv6Mask[uiLoopIndex+uiIpv6AddIndex] & aulDestIP[uiIpv6AddIndex])
-				!= pstClassifierRule->stDestIpAddress.ulIpv6Addr[uiLoopIndex+uiIpv6AddIndex])
-			{
-				//Match failed for current Ipv6 Address.Try next Ipv6 Address
+		for (uiIpv6AddIndex = 0; uiIpv6AddIndex < uiIpv6AddrNoLongWords; uiIpv6AddIndex++) {
+			if ((pstClassifierRule->stDestIpAddress.ulIpv6Mask[uiLoopIndex+uiIpv6AddIndex] & aulDestIP[uiIpv6AddIndex])
+				!= pstClassifierRule->stDestIpAddress.ulIpv6Addr[uiLoopIndex+uiIpv6AddIndex]) {
+				/*
+				 * Match failed for current Ipv6 Address.
+				 * Try next Ipv6 Address
+				 */
 				break;
 			}
 
-			if(uiIpv6AddIndex ==  uiIpv6AddrNoLongWords-1)
-			{
-				//Match Found
-				BCM_DEBUG_PRINT( Adapter,DBG_TYPE_TX, IPV6_DBG, DBG_LVL_ALL, "Ipv6 Destination Ip Address Matched\n");
+			if (uiIpv6AddIndex ==  uiIpv6AddrNoLongWords-1) {
+				/* Match Found */
+				BCM_DEBUG_PRINT(Adapter, DBG_TYPE_TX, IPV6_DBG,
+						DBG_LVL_ALL,
+						"Ipv6 Destination Ip Address Matched\n");
 				return TRUE;
 			}
 		}
@@ -371,11 +405,11 @@ static BOOLEAN MatchDestIpv6Address(S_CLASSIFIER_RULE *pstClassifierRule,IPV6Hea
 VOID DumpIpv6Address(ULONG *puIpv6Address)
 {
 	UINT uiIpv6AddrNoLongWords = 4;
-	UINT  uiIpv6AddIndex=0;
-    PMINI_ADAPTER Adapter = GET_BCM_ADAPTER(gblpnetdev);
-	for(uiIpv6AddIndex=0;uiIpv6AddIndex<uiIpv6AddrNoLongWords;uiIpv6AddIndex++)
-	{
-		BCM_DEBUG_PRINT( Adapter,DBG_TYPE_TX, IPV6_DBG, DBG_LVL_ALL, ":%lx",puIpv6Address[uiIpv6AddIndex]);
+	UINT uiIpv6AddIndex = 0;
+	PMINI_ADAPTER Adapter = GET_BCM_ADAPTER(gblpnetdev);
+	for (uiIpv6AddIndex = 0; uiIpv6AddIndex < uiIpv6AddrNoLongWords; uiIpv6AddIndex++) {
+		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_TX, IPV6_DBG, DBG_LVL_ALL,
+				":%lx", puIpv6Address[uiIpv6AddIndex]);
 	}
 
 }
@@ -383,22 +417,35 @@ VOID DumpIpv6Address(ULONG *puIpv6Address)
 static VOID DumpIpv6Header(IPV6Header *pstIpv6Header)
 {
 	UCHAR ucVersion;
-	UCHAR  ucPrio ;
-    PMINI_ADAPTER Adapter = GET_BCM_ADAPTER(gblpnetdev);
-	BCM_DEBUG_PRINT( Adapter,DBG_TYPE_TX, IPV6_DBG, DBG_LVL_ALL, "----Ipv6 Header---");
+	UCHAR ucPrio;
+	PMINI_ADAPTER Adapter = GET_BCM_ADAPTER(gblpnetdev);
+	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_TX, IPV6_DBG, DBG_LVL_ALL,
+			"----Ipv6 Header---");
 	ucVersion = pstIpv6Header->ucVersionPrio & 0xf0;
-	BCM_DEBUG_PRINT( Adapter,DBG_TYPE_TX, IPV6_DBG, DBG_LVL_ALL, "Version : %x \n",ucVersion);
+	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_TX, IPV6_DBG, DBG_LVL_ALL,
+			"Version : %x\n", ucVersion);
 	ucPrio = pstIpv6Header->ucVersionPrio & 0x0f;
-	BCM_DEBUG_PRINT( Adapter,DBG_TYPE_TX, IPV6_DBG, DBG_LVL_ALL, "Priority : %x \n",ucPrio);
-	//BCM_DEBUG_PRINT( Adapter,DBG_TYPE_TX, IPV6_DBG, DBG_LVL_ALL, "Flow Label : %x \n",(pstIpv6Header->ucVersionPrio &0xf0);
-	BCM_DEBUG_PRINT( Adapter,DBG_TYPE_TX, IPV6_DBG, DBG_LVL_ALL, "Payload Length : %x \n",ntohs(pstIpv6Header->usPayloadLength));
-	BCM_DEBUG_PRINT( Adapter,DBG_TYPE_TX, IPV6_DBG, DBG_LVL_ALL, "Next Header : %x \n",pstIpv6Header->ucNextHeader);
-	BCM_DEBUG_PRINT( Adapter,DBG_TYPE_TX, IPV6_DBG, DBG_LVL_ALL, "Hop Limit : %x \n",pstIpv6Header->ucHopLimit);
-	BCM_DEBUG_PRINT( Adapter,DBG_TYPE_TX, IPV6_DBG, DBG_LVL_ALL, "Src Address :\n");
+	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_TX, IPV6_DBG, DBG_LVL_ALL,
+			"Priority : %x\n", ucPrio);
+	/*
+	 * BCM_DEBUG_PRINT( Adapter,DBG_TYPE_TX, IPV6_DBG, DBG_LVL_ALL,
+	 * "Flow Label : %x\n",(pstIpv6Header->ucVersionPrio &0xf0);
+	 */
+	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_TX, IPV6_DBG, DBG_LVL_ALL,
+			"Payload Length : %x\n",
+			ntohs(pstIpv6Header->usPayloadLength));
+	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_TX, IPV6_DBG, DBG_LVL_ALL,
+			"Next Header : %x\n", pstIpv6Header->ucNextHeader);
+	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_TX, IPV6_DBG, DBG_LVL_ALL,
+			"Hop Limit : %x\n", pstIpv6Header->ucHopLimit);
+	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_TX, IPV6_DBG, DBG_LVL_ALL,
+			"Src Address :\n");
 	DumpIpv6Address(pstIpv6Header->ulSrcIpAddress);
-	BCM_DEBUG_PRINT( Adapter,DBG_TYPE_TX, IPV6_DBG, DBG_LVL_ALL, "Dest Address :\n");
+	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_TX, IPV6_DBG, DBG_LVL_ALL,
+			"Dest Address :\n");
 	DumpIpv6Address(pstIpv6Header->ulDestIpAddress);
-	BCM_DEBUG_PRINT( Adapter,DBG_TYPE_TX, IPV6_DBG, DBG_LVL_ALL, "----Ipv6 Header End---");
+	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_TX, IPV6_DBG, DBG_LVL_ALL,
+			"----Ipv6 Header End---");
 
 
 }
diff --git a/drivers/staging/bcm/Misc.c b/drivers/staging/bcm/Misc.c
index c7725e141fd..8223a6913fc 100644
--- a/drivers/staging/bcm/Misc.c
+++ b/drivers/staging/bcm/Misc.c
@@ -835,7 +835,7 @@ int reset_card_proc(PMINI_ADAPTER ps_adapter)
 	Bcm_kill_all_URBs(psIntfAdapter);
 	/* Reset the UMA-B Device */
 	if (ps_adapter->chip_id >= T3LPB) {
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0, "Reseting UMA-B\n");
+		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0, "Resetting UMA-B\n");
 		retval = usb_reset_device(psIntfAdapter->udev);
 		psIntfAdapter->psAdapter->StopAllXaction = FALSE;
 
diff --git a/drivers/staging/ccg/Kconfig b/drivers/staging/ccg/Kconfig
new file mode 100644
index 00000000000..ff05e52392b
--- /dev/null
+++ b/drivers/staging/ccg/Kconfig
@@ -0,0 +1,20 @@
+if USB_GADGET
+
+config USB_G_CCG
+	tristate "Configurable Composite Gadget (STAGING)"
+	depends on STAGING && !USB_ZERO && !USB_ZERO_HNPTEST && !USB_AUDIO && !GADGET_UAC1 && !USB_ETH && !USB_ETH_RNDIS && !USB_ETH_EEM && !USB_G_NCM && !USB_GADGETFS && !USB_FUNCTIONFS && !USB_FUNCTIONFS_ETH && !USB_FUNCTIONFS_RNDIS && !USB_FUNCTIONFS_GENERIC && !USB_FILE_STORAGE && !USB_FILE_STORAGE_TEST && !USB_MASS_STORAGE && !USB_G_SERIAL && !USB_MIDI_GADGET && !USB_G_PRINTER && !USB_CDC_COMPOSITE && !USB_G_NOKIA && !USB_G_ACM_MS && !USB_G_MULTI && !USB_G_MULTI_RNDIS && !USB_G_MULTI_CDC && !USB_G_HID && !USB_G_DBGP && !USB_G_WEBCAM
+	help
+	  The Configurable Composite Gadget supports multiple USB
+	  functions: acm, mass storage, rndis and FunctionFS.
+	  Each function can be configured and enabled/disabled
+	  dynamically from userspace through a sysfs interface.
+
+	  In order to compile this (either as a module or built-in),
+	  "USB Gadget Drivers" and anything under it must not be
+	  selected compiled-in in
+	  Device Drivers->USB Support->USB Gadget Support.
+	  However, you can say "M" there, if you do, the
+	  Configurable Composite Gadget can be compiled "M" only
+	  or not at all.
+
+endif # USB_GADGET
diff --git a/drivers/staging/ccg/Makefile b/drivers/staging/ccg/Makefile
new file mode 100644
index 00000000000..693da637a6d
--- /dev/null
+++ b/drivers/staging/ccg/Makefile
@@ -0,0 +1,4 @@
+g_ccg-y				:= ccg.o
+ccflags-y			+= -Idrivers/usb/gadget
+
+obj-$(CONFIG_USB_G_CCG)		+= g_ccg.o
diff --git a/drivers/staging/ccg/TODO b/drivers/staging/ccg/TODO
new file mode 100644
index 00000000000..18612fe7017
--- /dev/null
+++ b/drivers/staging/ccg/TODO
@@ -0,0 +1,6 @@
+TODO:
+	- change configuration interface from sysfs to configfs
+
+Please send patches to Greg Kroah-Hartmann <gregkh@linuxfoundation.org>,
+Andrzej Pietrasiewicz <andrzej.p@samsung.com>, and
+Cc: Mike Lockwood <lockwood@android.com>
diff --git a/drivers/staging/ccg/ccg.c b/drivers/staging/ccg/ccg.c
new file mode 100644
index 00000000000..a5b36a97598
--- /dev/null
+++ b/drivers/staging/ccg/ccg.c
@@ -0,0 +1,1299 @@
+/*
+ * Configurable Composite Gadget
+ *
+ * Initially contributed as "Android Composite Gdaget" by:
+ *
+ * Copyright (C) 2008 Google, Inc.
+ * Author: Mike Lockwood <lockwood@android.com>
+ *         Benoit Goby <benoit@android.com>
+ *
+ * Tailoring it to become a generic Configurable Composite Gadget is
+ *
+ * Copyright (C) 2012 Samsung Electronics
+ * Author: Andrzej Pietrasiewicz <andrzej.p@samsung.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/init.h>
+#include <linux/module.h>
+#include <linux/fs.h>
+#include <linux/delay.h>
+#include <linux/kernel.h>
+#include <linux/utsname.h>
+#include <linux/platform_device.h>
+
+#include <linux/usb/ch9.h>
+#include <linux/usb/composite.h>
+#include <linux/usb/gadget.h>
+
+#include "gadget_chips.h"
+
+/*
+ * Kbuild is not very cooperative with respect to linking separately
+ * compiled library objects into one module.  So for now we won't use
+ * separate compilation ... ensuring init/exit sections work to shrink
+ * the runtime footprint, and giving us at least some parts of what
+ * a "gcc --combine ... part1.c part2.c part3.c ... " build would.
+ */
+#include "../../usb/gadget/usbstring.c"
+#include "../../usb/gadget/config.c"
+#include "../../usb/gadget/epautoconf.c"
+#include "../../usb/gadget/composite.c"
+
+#include "../../usb/gadget/f_mass_storage.c"
+#include "../../usb/gadget/u_serial.c"
+#include "../../usb/gadget/f_acm.c"
+#define USB_ETH_RNDIS y
+#include "../../usb/gadget/f_rndis.c"
+#include "../../usb/gadget/rndis.c"
+#include "../../usb/gadget/u_ether.c"
+#include "../../usb/gadget/f_fs.c"
+
+MODULE_AUTHOR("Mike Lockwood, Andrzej Pietrasiewicz");
+MODULE_DESCRIPTION("Configurable Composite USB Gadget");
+MODULE_LICENSE("GPL");
+MODULE_VERSION("1.0");
+
+static const char longname[] = "Configurable Composite Gadget";
+
+/* Default vendor and product IDs, overridden by userspace */
+#define VENDOR_ID		0x1d6b /* Linux Foundation */
+#define PRODUCT_ID		0x0107
+#define GFS_MAX_DEVS		10
+
+struct ccg_usb_function {
+	char *name;
+	void *config;
+
+	struct device *dev;
+	char *dev_name;
+	struct device_attribute **attributes;
+
+	/* for ccg_dev.enabled_functions */
+	struct list_head enabled_list;
+
+	/* Optional: initialization during gadget bind */
+	int (*init)(struct ccg_usb_function *, struct usb_composite_dev *);
+	/* Optional: cleanup during gadget unbind */
+	void (*cleanup)(struct ccg_usb_function *);
+
+	int (*bind_config)(struct ccg_usb_function *,
+			   struct usb_configuration *);
+
+	/* Optional: called when the configuration is removed */
+	void (*unbind_config)(struct ccg_usb_function *,
+			      struct usb_configuration *);
+	/* Optional: handle ctrl requests before the device is configured */
+	int (*ctrlrequest)(struct ccg_usb_function *,
+			   struct usb_composite_dev *,
+			   const struct usb_ctrlrequest *);
+};
+
+struct ffs_obj {
+	const char *name;
+	bool mounted;
+	bool desc_ready;
+	bool used;
+	struct ffs_data *ffs_data;
+};
+
+struct ccg_dev {
+	struct ccg_usb_function **functions;
+	struct list_head enabled_functions;
+	struct usb_composite_dev *cdev;
+	struct device *dev;
+
+	bool enabled;
+	struct mutex mutex;
+	bool connected;
+	bool sw_connected;
+	struct work_struct work;
+
+	unsigned int max_func_num;
+	unsigned int func_num;
+	struct ffs_obj ffs_tab[GFS_MAX_DEVS];
+};
+
+static struct class *ccg_class;
+static struct ccg_dev *_ccg_dev;
+static int ccg_bind_config(struct usb_configuration *c);
+static void ccg_unbind_config(struct usb_configuration *c);
+
+static char func_names_buf[256];
+
+static struct usb_device_descriptor device_desc = {
+	.bLength              = sizeof(device_desc),
+	.bDescriptorType      = USB_DT_DEVICE,
+	.bcdUSB               = __constant_cpu_to_le16(0x0200),
+	.bDeviceClass         = USB_CLASS_PER_INTERFACE,
+	.idVendor             = __constant_cpu_to_le16(VENDOR_ID),
+	.idProduct            = __constant_cpu_to_le16(PRODUCT_ID),
+	.bcdDevice            = __constant_cpu_to_le16(0xffff),
+	.bNumConfigurations   = 1,
+};
+
+static struct usb_configuration ccg_config_driver = {
+	.label		= "ccg",
+	.unbind		= ccg_unbind_config,
+	.bConfigurationValue = 1,
+	.bmAttributes	= USB_CONFIG_ATT_ONE | USB_CONFIG_ATT_SELFPOWER,
+	.bMaxPower	= 0xFA, /* 500ma */
+};
+
+static void ccg_work(struct work_struct *data)
+{
+	struct ccg_dev *dev = container_of(data, struct ccg_dev, work);
+	struct usb_composite_dev *cdev = dev->cdev;
+	static char *disconnected[2] = { "USB_STATE=DISCONNECTED", NULL };
+	static char *connected[2]    = { "USB_STATE=CONNECTED", NULL };
+	static char *configured[2]   = { "USB_STATE=CONFIGURED", NULL };
+	char **uevent_envp = NULL;
+	unsigned long flags;
+
+	spin_lock_irqsave(&cdev->lock, flags);
+	if (cdev->config)
+		uevent_envp = configured;
+	else if (dev->connected != dev->sw_connected)
+		uevent_envp = dev->connected ? connected : disconnected;
+	dev->sw_connected = dev->connected;
+	spin_unlock_irqrestore(&cdev->lock, flags);
+
+	if (uevent_envp) {
+		kobject_uevent_env(&dev->dev->kobj, KOBJ_CHANGE, uevent_envp);
+		pr_info("%s: sent uevent %s\n", __func__, uevent_envp[0]);
+	} else {
+		pr_info("%s: did not send uevent (%d %d %p)\n", __func__,
+			 dev->connected, dev->sw_connected, cdev->config);
+	}
+}
+
+
+/*-------------------------------------------------------------------------*/
+/* Supported functions initialization */
+
+static struct ffs_obj *functionfs_find_dev(struct ccg_dev *dev,
+					   const char *dev_name)
+{
+	int i;
+
+	for (i = 0; i < dev->max_func_num; i++)
+		if (strcmp(dev->ffs_tab[i].name, dev_name) == 0)
+			return &dev->ffs_tab[i];
+
+	return NULL;
+}
+
+static bool functionfs_all_ready(struct ccg_dev *dev)
+{
+	int i;
+
+	for (i = 0; i < dev->max_func_num; i++)
+		if (dev->ffs_tab[i].used && !dev->ffs_tab[i].desc_ready)
+			return false;
+
+	return true;
+}
+
+static int functionfs_ready_callback(struct ffs_data *ffs)
+{
+	struct ffs_obj *ffs_obj;
+	int ret;
+
+	mutex_lock(&_ccg_dev->mutex);
+
+	ffs_obj = ffs->private_data;
+	if (!ffs_obj) {
+		ret = -EINVAL;
+		goto done;
+	}
+	if (WARN_ON(ffs_obj->desc_ready)) {
+		ret = -EBUSY;
+		goto done;
+	}
+	ffs_obj->ffs_data = ffs;
+
+	if (functionfs_all_ready(_ccg_dev)) {
+		ret = -EBUSY;
+		goto done;
+	}
+	ffs_obj->desc_ready = true;
+
+done:
+	mutex_unlock(&_ccg_dev->mutex);
+	return ret;
+}
+
+static void reset_usb(struct ccg_dev *dev)
+{
+	/* Cancel pending control requests */
+	usb_ep_dequeue(dev->cdev->gadget->ep0, dev->cdev->req);
+	usb_remove_config(dev->cdev, &ccg_config_driver);
+	dev->enabled = false;
+	usb_gadget_disconnect(dev->cdev->gadget);
+}
+
+static void functionfs_closed_callback(struct ffs_data *ffs)
+{
+	struct ffs_obj *ffs_obj;
+
+	mutex_lock(&_ccg_dev->mutex);
+
+	ffs_obj = ffs->private_data;
+	if (!ffs_obj)
+		goto done;
+
+	ffs_obj->desc_ready = false;
+
+	if (_ccg_dev->enabled)
+		reset_usb(_ccg_dev);
+
+done:
+	mutex_unlock(&_ccg_dev->mutex);
+}
+
+static void *functionfs_acquire_dev_callback(const char *dev_name)
+{
+	struct ffs_obj *ffs_dev;
+
+	mutex_lock(&_ccg_dev->mutex);
+
+	ffs_dev = functionfs_find_dev(_ccg_dev, dev_name);
+	if (!ffs_dev) {
+		ffs_dev = ERR_PTR(-ENODEV);
+		goto done;
+	}
+
+	if (ffs_dev->mounted) {
+		ffs_dev = ERR_PTR(-EBUSY);
+		goto done;
+	}
+	ffs_dev->mounted = true;
+
+done:
+	mutex_unlock(&_ccg_dev->mutex);
+	return ffs_dev;
+}
+
+static void functionfs_release_dev_callback(struct ffs_data *ffs_data)
+{
+	struct ffs_obj *ffs_dev;
+
+	mutex_lock(&_ccg_dev->mutex);
+
+	ffs_dev = ffs_data->private_data;
+	if (ffs_dev)
+		ffs_dev->mounted = false;
+
+	mutex_unlock(&_ccg_dev->mutex);
+}
+
+static int functionfs_function_init(struct ccg_usb_function *f,
+				struct usb_composite_dev *cdev)
+{
+	return functionfs_init();
+}
+
+static void functionfs_function_cleanup(struct ccg_usb_function *f)
+{
+	functionfs_cleanup();
+}
+
+static int functionfs_function_bind_config(struct ccg_usb_function *f,
+					   struct usb_configuration *c)
+{
+	struct ccg_dev *dev = _ccg_dev;
+	int i, ret;
+
+	for (i = dev->max_func_num; i--; ) {
+		if (!dev->ffs_tab[i].used)
+			continue;
+		ret = functionfs_bind(dev->ffs_tab[i].ffs_data, c->cdev);
+		if (unlikely(ret < 0)) {
+			while (++i < dev->max_func_num)
+				functionfs_unbind(dev->ffs_tab[i].ffs_data);
+			return ret;
+		}
+	}
+
+	for (i = dev->max_func_num; i--; ) {
+		if (!dev->ffs_tab[i].used)
+			continue;
+		ret = functionfs_bind_config(c->cdev, c,
+					     dev->ffs_tab[i].ffs_data);
+		if (unlikely(ret < 0))
+			return ret;
+	}
+
+	return 0;
+}
+
+static void functionfs_function_unbind_config(struct ccg_usb_function *f,
+					      struct usb_configuration *c)
+{
+	struct ccg_dev *dev = _ccg_dev;
+	int i;
+
+	for (i = dev->max_func_num; i--; )
+		if (dev->ffs_tab[i].ffs_data)
+			functionfs_unbind(dev->ffs_tab[i].ffs_data);
+}
+
+static ssize_t functionfs_user_functions_show(struct device *_dev,
+					      struct device_attribute *attr,
+					      char *buf)
+{
+	struct ccg_dev *dev = _ccg_dev;
+	char *buff = buf;
+	int i;
+
+	mutex_lock(&dev->mutex);
+
+	for (i = 0; i < dev->max_func_num; i++)
+		buff += snprintf(buff, PAGE_SIZE + buf - buff, "%s,",
+				 dev->ffs_tab[i].name);
+
+	mutex_unlock(&dev->mutex);
+
+	if (buff != buf)
+		*(buff - 1) = '\n';
+	return buff - buf;
+}
+
+static ssize_t functionfs_user_functions_store(struct device *_dev,
+					       struct device_attribute *attr,
+					       const char *buff, size_t size)
+{
+	struct ccg_dev *dev = _ccg_dev;
+	char *name, *b;
+	ssize_t ret = size;
+	int i;
+
+	buff = skip_spaces(buff);
+	if (!*buff)
+		return -EINVAL;
+
+	mutex_lock(&dev->mutex);
+
+	if (dev->enabled) {
+		ret = -EBUSY;
+		goto end;
+	}
+
+	for (i = 0; i < dev->max_func_num; i++)
+		if (dev->ffs_tab[i].mounted) {
+			ret = -EBUSY;
+			goto end;
+		}
+
+	strlcpy(func_names_buf, buff, sizeof(func_names_buf));
+	b = strim(func_names_buf);
+
+	/* replace the list of functions */
+	dev->max_func_num = 0;
+	while (b) {
+		name = strsep(&b, ",");
+		if (dev->max_func_num == GFS_MAX_DEVS) {
+			ret = -ENOSPC;
+			goto end;
+		}
+		if (functionfs_find_dev(dev, name)) {
+			ret = -EEXIST;
+			continue;
+		}
+		dev->ffs_tab[dev->max_func_num++].name = name;
+	}
+
+end:
+	mutex_unlock(&dev->mutex);
+	return ret;
+}
+
+static DEVICE_ATTR(user_functions, S_IRUGO | S_IWUSR,
+		   functionfs_user_functions_show,
+		   functionfs_user_functions_store);
+
+static ssize_t functionfs_max_user_functions_show(struct device *_dev,
+						  struct device_attribute *attr,
+						  char *buf)
+{
+	return sprintf(buf, "%d", GFS_MAX_DEVS);
+}
+
+static DEVICE_ATTR(max_user_functions, S_IRUGO,
+		   functionfs_max_user_functions_show, NULL);
+
+static struct device_attribute *functionfs_function_attributes[] = {
+	&dev_attr_user_functions,
+	&dev_attr_max_user_functions,
+	NULL
+};
+
+static struct ccg_usb_function functionfs_function = {
+	.name		= "fs",
+	.init		= functionfs_function_init,
+	.cleanup	= functionfs_function_cleanup,
+	.bind_config	= functionfs_function_bind_config,
+	.unbind_config  = functionfs_function_unbind_config,
+	.attributes	= functionfs_function_attributes,
+};
+
+#define MAX_ACM_INSTANCES 4
+struct acm_function_config {
+	int instances;
+};
+
+static int
+acm_function_init(struct ccg_usb_function *f, struct usb_composite_dev *cdev)
+{
+	f->config = kzalloc(sizeof(struct acm_function_config), GFP_KERNEL);
+	if (!f->config)
+		return -ENOMEM;
+
+	return gserial_setup(cdev->gadget, MAX_ACM_INSTANCES);
+}
+
+static void acm_function_cleanup(struct ccg_usb_function *f)
+{
+	gserial_cleanup();
+	kfree(f->config);
+	f->config = NULL;
+}
+
+static int
+acm_function_bind_config(struct ccg_usb_function *f,
+		struct usb_configuration *c)
+{
+	int i;
+	int ret = 0;
+	struct acm_function_config *config = f->config;
+
+	for (i = 0; i < config->instances; i++) {
+		ret = acm_bind_config(c, i);
+		if (ret) {
+			pr_err("Could not bind acm%u config\n", i);
+			break;
+		}
+	}
+
+	return ret;
+}
+
+static ssize_t acm_instances_show(struct device *dev,
+		struct device_attribute *attr, char *buf)
+{
+	struct ccg_usb_function *f = dev_get_drvdata(dev);
+	struct acm_function_config *config = f->config;
+	return sprintf(buf, "%d\n", config->instances);
+}
+
+static ssize_t acm_instances_store(struct device *dev,
+		struct device_attribute *attr, const char *buf, size_t size)
+{
+	struct ccg_usb_function *f = dev_get_drvdata(dev);
+	struct acm_function_config *config = f->config;
+	int value;
+	int ret = 0;
+
+	ret = kstrtoint(buf, 10, &value);
+	if (ret)
+		return ret;
+
+	if (value > MAX_ACM_INSTANCES)
+		return -EINVAL;
+
+	config->instances = value;
+
+	return size;
+}
+
+static DEVICE_ATTR(instances, S_IRUGO | S_IWUSR, acm_instances_show,
+						 acm_instances_store);
+static struct device_attribute *acm_function_attributes[] = {
+	&dev_attr_instances,
+	NULL
+};
+
+static struct ccg_usb_function acm_function = {
+	.name		= "acm",
+	.init		= acm_function_init,
+	.cleanup	= acm_function_cleanup,
+	.bind_config	= acm_function_bind_config,
+	.attributes	= acm_function_attributes,
+};
+
+struct rndis_function_config {
+	u8      ethaddr[ETH_ALEN];
+	u32     vendorID;
+	char	manufacturer[256];
+	/* "Wireless" RNDIS; auto-detected by Windows */
+	bool	wceis;
+};
+
+static int rndis_function_init(struct ccg_usb_function *f,
+			       struct usb_composite_dev *cdev)
+{
+	f->config = kzalloc(sizeof(struct rndis_function_config), GFP_KERNEL);
+	if (!f->config)
+		return -ENOMEM;
+	return 0;
+}
+
+static void rndis_function_cleanup(struct ccg_usb_function *f)
+{
+	kfree(f->config);
+	f->config = NULL;
+}
+
+static int rndis_function_bind_config(struct ccg_usb_function *f,
+				      struct usb_configuration *c)
+{
+	int ret;
+	struct rndis_function_config *rndis = f->config;
+
+	if (!rndis) {
+		pr_err("%s: rndis_pdata\n", __func__);
+		return -1;
+	}
+
+	pr_info("%s MAC: %02X:%02X:%02X:%02X:%02X:%02X\n", __func__,
+		rndis->ethaddr[0], rndis->ethaddr[1], rndis->ethaddr[2],
+		rndis->ethaddr[3], rndis->ethaddr[4], rndis->ethaddr[5]);
+
+	ret = gether_setup_name(c->cdev->gadget, rndis->ethaddr, "rndis");
+	if (ret) {
+		pr_err("%s: gether_setup failed\n", __func__);
+		return ret;
+	}
+
+	if (rndis->wceis) {
+		/* "Wireless" RNDIS; auto-detected by Windows */
+		rndis_iad_descriptor.bFunctionClass =
+						USB_CLASS_WIRELESS_CONTROLLER;
+		rndis_iad_descriptor.bFunctionSubClass = 0x01;
+		rndis_iad_descriptor.bFunctionProtocol = 0x03;
+		rndis_control_intf.bInterfaceClass =
+						USB_CLASS_WIRELESS_CONTROLLER;
+		rndis_control_intf.bInterfaceSubClass =	 0x01;
+		rndis_control_intf.bInterfaceProtocol =	 0x03;
+	}
+
+	return rndis_bind_config_vendor(c, rndis->ethaddr, rndis->vendorID,
+					   rndis->manufacturer);
+}
+
+static void rndis_function_unbind_config(struct ccg_usb_function *f,
+						struct usb_configuration *c)
+{
+	gether_cleanup();
+}
+
+static ssize_t rndis_manufacturer_show(struct device *dev,
+		struct device_attribute *attr, char *buf)
+{
+	struct ccg_usb_function *f = dev_get_drvdata(dev);
+	struct rndis_function_config *config = f->config;
+	return sprintf(buf, "%s\n", config->manufacturer);
+}
+
+static ssize_t rndis_manufacturer_store(struct device *dev,
+		struct device_attribute *attr, const char *buf, size_t size)
+{
+	struct ccg_usb_function *f = dev_get_drvdata(dev);
+	struct rndis_function_config *config = f->config;
+
+	if (size >= sizeof(config->manufacturer))
+		return -EINVAL;
+	memcpy(config->manufacturer, buf, size);
+	config->manufacturer[size] = 0;
+
+	return size;
+}
+
+static DEVICE_ATTR(manufacturer, S_IRUGO | S_IWUSR, rndis_manufacturer_show,
+						    rndis_manufacturer_store);
+
+static ssize_t rndis_wceis_show(struct device *dev,
+		struct device_attribute *attr, char *buf)
+{
+	struct ccg_usb_function *f = dev_get_drvdata(dev);
+	struct rndis_function_config *config = f->config;
+	return sprintf(buf, "%d\n", config->wceis);
+}
+
+static ssize_t rndis_wceis_store(struct device *dev,
+		struct device_attribute *attr, const char *buf, size_t size)
+{
+	struct ccg_usb_function *f = dev_get_drvdata(dev);
+	struct rndis_function_config *config = f->config;
+	int value;
+	int ret;
+
+	ret = kstrtoint(buf, 10, &value);
+	if (ret)
+		return ret;
+
+	config->wceis = value;
+
+	return size;
+}
+
+static DEVICE_ATTR(wceis, S_IRUGO | S_IWUSR, rndis_wceis_show,
+					     rndis_wceis_store);
+
+static ssize_t rndis_ethaddr_show(struct device *dev,
+		struct device_attribute *attr, char *buf)
+{
+	struct ccg_usb_function *f = dev_get_drvdata(dev);
+	struct rndis_function_config *rndis = f->config;
+	return sprintf(buf, "%02x:%02x:%02x:%02x:%02x:%02x\n",
+		rndis->ethaddr[0], rndis->ethaddr[1], rndis->ethaddr[2],
+		rndis->ethaddr[3], rndis->ethaddr[4], rndis->ethaddr[5]);
+}
+
+static ssize_t rndis_ethaddr_store(struct device *dev,
+		struct device_attribute *attr, const char *buf, size_t size)
+{
+	struct ccg_usb_function *f = dev_get_drvdata(dev);
+	struct rndis_function_config *rndis = f->config;
+	unsigned char tmp[6];
+
+	if (sscanf(buf, "%hhx:%hhx:%hhx:%hhx:%hhx:%hhx",
+		   tmp + 0, tmp + 1, tmp + 2, tmp + 3, tmp + 4, tmp + 5) !=
+	    ETH_ALEN)
+		return -EINVAL;
+
+	memcpy(rndis->ethaddr, tmp, ETH_ALEN);
+
+	return ETH_ALEN;
+
+}
+
+static DEVICE_ATTR(ethaddr, S_IRUGO | S_IWUSR, rndis_ethaddr_show,
+					       rndis_ethaddr_store);
+
+static ssize_t rndis_vendorID_show(struct device *dev,
+		struct device_attribute *attr, char *buf)
+{
+	struct ccg_usb_function *f = dev_get_drvdata(dev);
+	struct rndis_function_config *config = f->config;
+	return sprintf(buf, "%04x\n", config->vendorID);
+}
+
+static ssize_t rndis_vendorID_store(struct device *dev,
+		struct device_attribute *attr, const char *buf, size_t size)
+{
+	struct ccg_usb_function *f = dev_get_drvdata(dev);
+	struct rndis_function_config *config = f->config;
+	int value;
+	int ret;
+
+	ret = kstrtou32(buf, 16, &value);
+	if (ret)
+		return ret;
+
+	config->vendorID = value;
+
+	return size;
+}
+
+static DEVICE_ATTR(vendorID, S_IRUGO | S_IWUSR, rndis_vendorID_show,
+						rndis_vendorID_store);
+
+static struct device_attribute *rndis_function_attributes[] = {
+	&dev_attr_manufacturer,
+	&dev_attr_wceis,
+	&dev_attr_ethaddr,
+	&dev_attr_vendorID,
+	NULL
+};
+
+static struct ccg_usb_function rndis_function = {
+	.name		= "rndis",
+	.init		= rndis_function_init,
+	.cleanup	= rndis_function_cleanup,
+	.bind_config	= rndis_function_bind_config,
+	.unbind_config	= rndis_function_unbind_config,
+	.attributes	= rndis_function_attributes,
+};
+
+static int mass_storage_function_init(struct ccg_usb_function *f,
+					struct usb_composite_dev *cdev)
+{
+	struct fsg_config fsg;
+	struct fsg_common *common;
+	int err;
+
+	memset(&fsg, 0, sizeof fsg);
+	fsg.nluns = 1;
+	fsg.luns[0].removable = 1;
+	fsg.vendor_name = iManufacturer;
+	fsg.product_name = iProduct;
+
+	common = fsg_common_init(NULL, cdev, &fsg);
+	if (IS_ERR(common))
+		return PTR_ERR(common);
+
+	err = sysfs_create_link(&f->dev->kobj,
+				&common->luns[0].dev.kobj,
+				"lun");
+	if (err) {
+		fsg_common_put(common);
+		return err;
+	}
+
+	f->config = common;
+	return 0;
+}
+
+static void mass_storage_function_cleanup(struct ccg_usb_function *f)
+{
+	fsg_common_put(f->config);
+	f->config = NULL;
+}
+
+static int mass_storage_function_bind_config(struct ccg_usb_function *f,
+					     struct usb_configuration *c)
+{
+	struct fsg_common *common = f->config;
+	return fsg_bind_config(c->cdev, c, common);
+}
+
+static struct ccg_usb_function mass_storage_function = {
+	.name		= "mass_storage",
+	.init		= mass_storage_function_init,
+	.cleanup	= mass_storage_function_cleanup,
+	.bind_config	= mass_storage_function_bind_config,
+};
+
+static struct ccg_usb_function *supported_functions[] = {
+	&functionfs_function,
+	&acm_function,
+	&rndis_function,
+	&mass_storage_function,
+	NULL
+};
+
+
+static int ccg_init_functions(struct ccg_usb_function **functions,
+				  struct usb_composite_dev *cdev)
+{
+	struct ccg_dev *dev = _ccg_dev;
+	struct ccg_usb_function *f;
+	struct device_attribute **attrs;
+	struct device_attribute *attr;
+	int err;
+	int index = 0;
+
+	for (; (f = *functions++); index++) {
+		f->dev_name = kasprintf(GFP_KERNEL, "f_%s", f->name);
+		if (!f->dev_name) {
+			pr_err("%s: Failed to alloc name %s", __func__,
+			       f->name);
+			err = -ENOMEM;
+			goto err_alloc;
+		}
+		f->dev = device_create(ccg_class, dev->dev,
+				MKDEV(0, index), f, f->dev_name);
+		if (IS_ERR(f->dev)) {
+			pr_err("%s: Failed to create dev %s", __func__,
+							f->dev_name);
+			err = PTR_ERR(f->dev);
+			f->dev = NULL;
+			goto err_create;
+		}
+
+		if (f->init) {
+			err = f->init(f, cdev);
+			if (err) {
+				pr_err("%s: Failed to init %s", __func__,
+								f->name);
+				goto err_out;
+			}
+		}
+
+		attrs = f->attributes;
+		if (attrs) {
+			while ((attr = *attrs++) && !err)
+				err = device_create_file(f->dev, attr);
+		}
+		if (err) {
+			pr_err("%s: Failed to create function %s attributes",
+					__func__, f->name);
+			goto err_uninit;
+		}
+	}
+	return 0;
+
+err_uninit:
+	if (f->cleanup)
+		f->cleanup(f);
+err_out:
+	device_destroy(ccg_class, f->dev->devt);
+	f->dev = NULL;
+err_create:
+	kfree(f->dev_name);
+err_alloc:
+	return err;
+}
+
+static void ccg_cleanup_functions(struct ccg_usb_function **functions)
+{
+	struct ccg_usb_function *f;
+
+	while (*functions) {
+		f = *functions++;
+
+		if (f->dev) {
+			if (f->cleanup)
+				f->cleanup(f);
+			device_destroy(ccg_class, f->dev->devt);
+			kfree(f->dev_name);
+		}
+	}
+}
+
+static int ccg_bind_enabled_functions(struct ccg_dev *dev,
+				      struct usb_configuration *c)
+{
+	struct ccg_usb_function *f;
+	int ret;
+
+	list_for_each_entry(f, &dev->enabled_functions, enabled_list) {
+		ret = f->bind_config(f, c);
+		if (ret) {
+			pr_err("%s: %s failed", __func__, f->name);
+			return ret;
+		}
+	}
+	return 0;
+}
+
+static void ccg_unbind_enabled_functions(struct ccg_dev *dev,
+					 struct usb_configuration *c)
+{
+	struct ccg_usb_function *f;
+
+	list_for_each_entry(f, &dev->enabled_functions, enabled_list)
+		if (f->unbind_config)
+			f->unbind_config(f, c);
+}
+
+static int ccg_enable_function(struct ccg_dev *dev, char *name)
+{
+	struct ccg_usb_function **functions = dev->functions;
+	struct ccg_usb_function *f;
+	while ((f = *functions++)) {
+		if (!strcmp(name, f->name)) {
+			list_add_tail(&f->enabled_list,
+						&dev->enabled_functions);
+			return 0;
+		}
+	}
+	return -EINVAL;
+}
+
+/*-------------------------------------------------------------------------*/
+/* /sys/class/ccg_usb/ccg%d/ interface */
+
+static ssize_t
+functions_show(struct device *pdev, struct device_attribute *attr, char *buf)
+{
+	struct ccg_dev *dev = dev_get_drvdata(pdev);
+	struct ccg_usb_function *f;
+	char *buff = buf;
+	int i;
+
+	mutex_lock(&dev->mutex);
+
+	list_for_each_entry(f, &dev->enabled_functions, enabled_list)
+		buff += sprintf(buff, "%s,", f->name);
+	for (i = 0; i < dev->max_func_num; i++)
+		if (dev->ffs_tab[i].used)
+			buff += sprintf(buff, "%s", dev->ffs_tab[i].name);
+
+	mutex_unlock(&dev->mutex);
+
+	if (buff != buf)
+		*(buff-1) = '\n';
+	return buff - buf;
+}
+
+static ssize_t
+functions_store(struct device *pdev, struct device_attribute *attr,
+			       const char *buff, size_t size)
+{
+	struct ccg_dev *dev = dev_get_drvdata(pdev);
+	char *name;
+	char buf[256], *b;
+	int err, i;
+	bool functionfs_enabled;
+
+	buff = skip_spaces(buff);
+	if (!*buff)
+		return -EINVAL;
+
+	mutex_lock(&dev->mutex);
+
+	if (dev->enabled) {
+		mutex_unlock(&dev->mutex);
+		return -EBUSY;
+	}
+
+	INIT_LIST_HEAD(&dev->enabled_functions);
+	functionfs_enabled = false;
+	for (i = 0; i < dev->max_func_num; i++)
+		dev->ffs_tab[i].used = false;
+
+	strlcpy(buf, buff, sizeof(buf));
+	b = strim(buf);
+
+	while (b) {
+		struct ffs_obj *user_func;
+
+		name = strsep(&b, ",");
+		/* handle FunctionFS implicitly */
+		if (!strcmp(name, functionfs_function.name)) {
+			pr_err("ccg_usb: Cannot explicitly enable '%s'", name);
+			continue;
+		}
+		user_func = functionfs_find_dev(dev, name);
+		if (user_func)
+			name = functionfs_function.name;
+		err = 0;
+		if (!user_func || !functionfs_enabled)
+			err = ccg_enable_function(dev, name);
+		if (err)
+			pr_err("ccg_usb: Cannot enable '%s'", name);
+		else if (user_func) {
+			user_func->used = true;
+			dev->func_num++;
+			functionfs_enabled = true;
+		}
+	}
+
+	mutex_unlock(&dev->mutex);
+
+	return size;
+}
+
+static ssize_t enable_show(struct device *pdev, struct device_attribute *attr,
+			   char *buf)
+{
+	struct ccg_dev *dev = dev_get_drvdata(pdev);
+	return sprintf(buf, "%d\n", dev->enabled);
+}
+
+static ssize_t enable_store(struct device *pdev, struct device_attribute *attr,
+			    const char *buff, size_t size)
+{
+	struct ccg_dev *dev = dev_get_drvdata(pdev);
+	struct usb_composite_dev *cdev = dev->cdev;
+	int enabled = 0;
+
+	mutex_lock(&dev->mutex);
+	sscanf(buff, "%d", &enabled);
+	if (enabled && dev->func_num && !functionfs_all_ready(dev)) {
+		mutex_unlock(&dev->mutex);
+		return -ENODEV;
+	}
+
+	if (enabled && !dev->enabled) {
+		int ret;
+
+		cdev->next_string_id = 0;
+		/*
+		 * Update values in composite driver's copy of
+		 * device descriptor.
+		 */
+		cdev->desc.bDeviceClass = device_desc.bDeviceClass;
+		cdev->desc.bDeviceSubClass = device_desc.bDeviceSubClass;
+		cdev->desc.bDeviceProtocol = device_desc.bDeviceProtocol;
+		cdev->desc.idVendor = idVendor;
+		cdev->desc.idProduct = idProduct;
+		cdev->desc.bcdDevice = bcdDevice;
+
+		usb_add_config(cdev, &ccg_config_driver, ccg_bind_config);
+		dev->enabled = true;
+		ret = usb_gadget_connect(cdev->gadget);
+		if (ret) {
+			dev->enabled = false;
+			usb_remove_config(cdev, &ccg_config_driver);
+		}
+	} else if (!enabled && dev->enabled) {
+		reset_usb(dev);
+	} else {
+		pr_err("ccg_usb: already %s\n",
+			dev->enabled ? "enabled" : "disabled");
+	}
+
+	mutex_unlock(&dev->mutex);
+	return size;
+}
+
+static ssize_t state_show(struct device *pdev, struct device_attribute *attr,
+			   char *buf)
+{
+	struct ccg_dev *dev = dev_get_drvdata(pdev);
+	struct usb_composite_dev *cdev = dev->cdev;
+	char *state = "DISCONNECTED";
+	unsigned long flags;
+
+	if (!cdev)
+		goto out;
+
+	spin_lock_irqsave(&cdev->lock, flags);
+	if (cdev->config)
+		state = "CONFIGURED";
+	else if (dev->connected)
+		state = "CONNECTED";
+	spin_unlock_irqrestore(&cdev->lock, flags);
+out:
+	return sprintf(buf, "%s\n", state);
+}
+
+#define DESCRIPTOR_ATTR(field, format_string)				\
+static ssize_t								\
+field ## _show(struct device *dev, struct device_attribute *attr,	\
+		char *buf)						\
+{									\
+	return sprintf(buf, format_string, device_desc.field);		\
+}									\
+static ssize_t								\
+field ## _store(struct device *dev, struct device_attribute *attr,	\
+		const char *buf, size_t size)				\
+{									\
+	int value;							\
+	if (sscanf(buf, format_string, &value) == 1) {			\
+		device_desc.field = value;				\
+		return size;						\
+	}								\
+	return -1;							\
+}									\
+static DEVICE_ATTR(field, S_IRUGO | S_IWUSR, field ## _show, field ## _store);
+
+DESCRIPTOR_ATTR(bDeviceClass, "%d\n")
+DESCRIPTOR_ATTR(bDeviceSubClass, "%d\n")
+DESCRIPTOR_ATTR(bDeviceProtocol, "%d\n")
+
+static DEVICE_ATTR(functions, S_IRUGO | S_IWUSR, functions_show,
+						 functions_store);
+static DEVICE_ATTR(enable, S_IRUGO | S_IWUSR, enable_show, enable_store);
+static DEVICE_ATTR(state, S_IRUGO, state_show, NULL);
+
+static struct device_attribute *ccg_usb_attributes[] = {
+	&dev_attr_bDeviceClass,
+	&dev_attr_bDeviceSubClass,
+	&dev_attr_bDeviceProtocol,
+	&dev_attr_functions,
+	&dev_attr_enable,
+	&dev_attr_state,
+	NULL
+};
+
+/*-------------------------------------------------------------------------*/
+/* Composite driver */
+
+static int ccg_bind_config(struct usb_configuration *c)
+{
+	struct ccg_dev *dev = _ccg_dev;
+	int ret = 0;
+
+	ret = ccg_bind_enabled_functions(dev, c);
+	if (ret)
+		return ret;
+
+	return 0;
+}
+
+static void ccg_unbind_config(struct usb_configuration *c)
+{
+	struct ccg_dev *dev = _ccg_dev;
+
+	ccg_unbind_enabled_functions(dev, c);
+
+	usb_ep_autoconfig_reset(dev->cdev->gadget);
+}
+
+static int ccg_bind(struct usb_composite_dev *cdev)
+{
+	struct ccg_dev *dev = _ccg_dev;
+	struct usb_gadget	*gadget = cdev->gadget;
+	int			gcnum, ret;
+
+	/*
+	 * Start disconnected. Userspace will connect the gadget once
+	 * it is done configuring the functions.
+	 */
+	usb_gadget_disconnect(gadget);
+
+	ret = ccg_init_functions(dev->functions, cdev);
+	if (ret)
+		return ret;
+
+	gcnum = usb_gadget_controller_number(gadget);
+	if (gcnum >= 0)
+		device_desc.bcdDevice = cpu_to_le16(0x0200 + gcnum);
+	else {
+		pr_warning("%s: controller '%s' not recognized\n",
+			longname, gadget->name);
+		device_desc.bcdDevice = __constant_cpu_to_le16(0x9999);
+	}
+
+	usb_gadget_set_selfpowered(gadget);
+	dev->cdev = cdev;
+
+	return 0;
+}
+
+static int ccg_usb_unbind(struct usb_composite_dev *cdev)
+{
+	struct ccg_dev *dev = _ccg_dev;
+
+	cancel_work_sync(&dev->work);
+	ccg_cleanup_functions(dev->functions);
+	return 0;
+}
+
+static struct usb_composite_driver ccg_usb_driver = {
+	.name		= "configurable_usb",
+	.dev		= &device_desc,
+	.unbind		= ccg_usb_unbind,
+	.needs_serial	= true,
+	.iManufacturer	= "Linux Foundation",
+	.iProduct	= longname,
+	.iSerialNumber	= "1234567890123456",
+};
+
+static int ccg_setup(struct usb_gadget *gadget, const struct usb_ctrlrequest *c)
+{
+	struct ccg_dev		*dev = _ccg_dev;
+	struct usb_composite_dev	*cdev = get_gadget_data(gadget);
+	struct usb_request		*req = cdev->req;
+	struct ccg_usb_function	*f;
+	int value = -EOPNOTSUPP;
+	unsigned long flags;
+
+	req->zero = 0;
+	req->complete = composite_setup_complete;
+	req->length = 0;
+	gadget->ep0->driver_data = cdev;
+
+	list_for_each_entry(f, &dev->enabled_functions, enabled_list) {
+		if (f->ctrlrequest) {
+			value = f->ctrlrequest(f, cdev, c);
+			if (value >= 0)
+				break;
+		}
+	}
+
+	if (value < 0)
+		value = composite_setup(gadget, c);
+
+	spin_lock_irqsave(&cdev->lock, flags);
+	if (!dev->connected) {
+		dev->connected = 1;
+		schedule_work(&dev->work);
+	} else if (c->bRequest == USB_REQ_SET_CONFIGURATION &&
+						cdev->config) {
+		schedule_work(&dev->work);
+	}
+	spin_unlock_irqrestore(&cdev->lock, flags);
+
+	return value;
+}
+
+static void ccg_disconnect(struct usb_gadget *gadget)
+{
+	struct ccg_dev *dev = _ccg_dev;
+	struct usb_composite_dev *cdev = get_gadget_data(gadget);
+	unsigned long flags;
+
+	composite_disconnect(gadget);
+
+	spin_lock_irqsave(&cdev->lock, flags);
+	dev->connected = 0;
+	schedule_work(&dev->work);
+	spin_unlock_irqrestore(&cdev->lock, flags);
+}
+
+static int ccg_create_device(struct ccg_dev *dev)
+{
+	struct device_attribute **attrs = ccg_usb_attributes;
+	struct device_attribute *attr;
+	int err;
+
+	dev->dev = device_create(ccg_class, NULL, MKDEV(0, 0), NULL, "ccg0");
+	if (IS_ERR(dev->dev))
+		return PTR_ERR(dev->dev);
+
+	dev_set_drvdata(dev->dev, dev);
+
+	while ((attr = *attrs++)) {
+		err = device_create_file(dev->dev, attr);
+		if (err) {
+			device_destroy(ccg_class, dev->dev->devt);
+			return err;
+		}
+	}
+	return 0;
+}
+
+
+static int __init init(void)
+{
+	struct ccg_dev *dev;
+	int err;
+
+	ccg_class = class_create(THIS_MODULE, "ccg_usb");
+	if (IS_ERR(ccg_class))
+		return PTR_ERR(ccg_class);
+
+	dev = kzalloc(sizeof(*dev), GFP_KERNEL);
+	if (!dev)
+		return -ENOMEM;
+
+	dev->functions = supported_functions;
+	INIT_LIST_HEAD(&dev->enabled_functions);
+	INIT_WORK(&dev->work, ccg_work);
+	mutex_init(&dev->mutex);
+
+	err = ccg_create_device(dev);
+	if (err) {
+		class_destroy(ccg_class);
+		kfree(dev);
+		return err;
+	}
+
+	_ccg_dev = dev;
+
+	/* Override composite driver functions */
+	composite_driver.setup = ccg_setup;
+	composite_driver.disconnect = ccg_disconnect;
+
+	err = usb_composite_probe(&ccg_usb_driver, ccg_bind);
+	if (err) {
+		class_destroy(ccg_class);
+		kfree(dev);
+	}
+
+	return err;
+}
+module_init(init);
+
+static void __exit cleanup(void)
+{
+	usb_composite_unregister(&ccg_usb_driver);
+	class_destroy(ccg_class);
+	kfree(_ccg_dev);
+	_ccg_dev = NULL;
+}
+module_exit(cleanup);
diff --git a/drivers/staging/ccg/sysfs-class-ccg_usb b/drivers/staging/ccg/sysfs-class-ccg_usb
new file mode 100644
index 00000000000..dd12a332fb0
--- /dev/null
+++ b/drivers/staging/ccg/sysfs-class-ccg_usb
@@ -0,0 +1,158 @@
+What:		/sys/class/ccg_usb
+Date:		May 2012
+KernelVersion:	3.4
+Contact:	linux-usb@vger.kernel.org
+Description:
+		The ccg_usb/ class subdirectory belongs to ccg
+		USB gadget.
+
+What:		/sys/class/ccg_usb/ccgX
+Date:		May 2012
+KernelVersion:	3.4
+Contact:	linux-usb@vger.kernel.org
+Description:
+		The /sys/class/ccg_usb/ccg{0,1,2,3...} class
+		subdirectories correspond to each ccg gadget device;
+		at the time of this writing there is only ccg0 and it
+		represents the ccg gadget.
+
+What:		/sys/class/ccg_usb/ccgX/functions
+Date:		May 2012
+KernelVersion:	3.4
+Contact:	linux-usb@vger.kernel.org
+Description:
+		A comma-separated list of USB function names to be activated
+		in this ccg gadget. It includes both the functions provided
+		in-kernel by the ccg gadget and the functions provided from
+		userspace through FunctionFS.
+
+What:		/sys/class/ccg_usb/ccgX/enable
+Date:		May 2012
+KernelVersion:	3.4
+Contact:	linux-usb@vger.kernel.org
+Description:
+		A flag activating/deactivating the ccg usb gadget.
+
+What:		/sys/class/ccg_usb/ccgX/state
+Date:		May 2012
+KernelVersion:	3.4
+Contact:	linux-usb@vger.kernel.org
+Description:
+		Configurable usb gadget state:
+
+		DISCONNECTED
+		CONNECTED
+		CONFIGURED
+
+What:		/sys/class/ccg_usb/ccgX/f_acm/
+Date:		May 2012
+KernelVersion:	3.4
+Contact:	linux-usb@vger.kernel.org
+Description:
+		The /sys/class/ccg_usb/ccgX/f_acm subdirectory
+		corresponds to the gadget's USB CDC serial (ACM) function
+		driver.
+
+What:		/sys/class/ccg_usb/ccgX/f_acm/instances
+Date:		May 2012
+KernelVersion:	3.4
+Contact:	linux-usb@vger.kernel.org
+Description:
+		Maximum number of the /dev/ttyGS<X> interface the driver uses.
+
+What:		/sys/class/ccg_usb/ccgX/f_fs
+Date:		May 2012
+KernelVersion:	3.4
+Contact:	linux-usb@vger.kernel.org
+Description:
+		The /sys/class/ccg_usb/ccgX/f_fs subdirectory
+		corresponds to the gadget's FunctionFS driver.
+
+What:		/sys/class/ccg_usb/ccgX/f_fs/user_functions
+Date:		May 2012
+KernelVersion:	3.4
+Contact:	linux-usb@vger.kernel.org
+Description:
+		A comma-separeted list of USB function names to be supported
+		from userspace. No other userspace FunctionFS functions can
+		be supported than listed here. However, the actual activation
+		of these functions is still done through
+		/sys/class/ccg_usb/ccgX/functions, where it is possible
+		to specify any subset (including maximum and empty) of
+		/sys/class/ccg_usb/ccgX/f_fs/user_functions.
+
+What:		/sys/class/ccg_usb/ccgX/f_fs/max_user_functions
+Date:		May 2012
+KernelVersion:	3.4
+Contact:	linux-usb@vger.kernel.org
+Description:
+		Maximum number of USB functions to be supported from userspace.
+
+What:		/sys/class/ccg_usb/ccgX/f_rndis
+Date:		May 2012
+KernelVersion:	3.4
+Contact:	linux-usb@vger.kernel.org
+Description:
+		The /sys/class/ccg_usb/ccgX/f_rndis subdirectory
+		corresponds to the gadget's RNDIS driver.
+
+What:		/sys/class/ccg_usb/ccgX/f_rndis/manufacturer
+Date:		May 2012
+KernelVersion:	3.4
+Contact:	linux-usb@vger.kernel.org
+Description:
+		RNDIS Ethernet port manufacturer string.
+
+What:		/sys/class/ccg_usb/ccgX/f_rndis/wceis
+Date:		May 2012
+KernelVersion:	3.4
+Contact:	linux-usb@vger.kernel.org
+Description:
+		RNDIS Ethernet port wireless flag.
+
+What:		/sys/class/ccg_usb/ccgX/f_rndis/ethaddr
+Date:		May 2012
+KernelVersion:	3.4
+Contact:	linux-usb@vger.kernel.org
+Description:
+		RNDIS Ethernet port Ethernet address.
+
+What:		/sys/class/ccg_usb/ccgX/f_rndis/vendorID
+Date:		May 2012
+KernelVersion:	3.4
+Contact:	linux-usb@vger.kernel.org
+Description:
+		RNDIS Ethernet port vendor ID.
+
+What:		/sys/class/ccg_usb/ccgX/f_mass_storage
+Date:		May 2012
+KernelVersion:	3.4
+Contact:	linux-usb@vger.kernel.org
+Description:
+		The /sys/class/ccg_usb/ccgX/f_mass_storage subdirectory
+		corresponds to the gadget's USB mass storage driver.
+
+What:		/sys/class/ccg_usb/ccgX/f_mass_storage/lun
+Date:		May 2012
+KernelVersion:	3.4
+Contact:	linux-usb@vger.kernel.org
+Description:
+		The /sys/class/ccg_usb/ccgX/f_mass_storage/lun
+		subdirectory corresponds to the gadget's USB mass storage
+		driver and its underlying storage.
+
+What:		/sys/class/ccg_usb/ccgX/f_mass_storage/lun
+Date:		May 2012
+KernelVersion:	3.4
+Contact:	linux-usb@vger.kernel.org
+Description:
+		The /sys/class/ccg_usb/ccgX/f_mass_storage/lun
+		subdirectory corresponds to the gadget's USB mass storage
+		driver and its underlying storage.
+
+What:		/sys/class/ccg_usb/ccgX/f_mass_storage/lun/file
+Date:		May 2012
+KernelVersion:	3.4
+Contact:	linux-usb@vger.kernel.org
+Description:
+		Gadget's USB mass storage underlying file.
diff --git a/drivers/staging/comedi/Kconfig b/drivers/staging/comedi/Kconfig
index 12c691d9090..3bbe3fd103f 100644
--- a/drivers/staging/comedi/Kconfig
+++ b/drivers/staging/comedi/Kconfig
@@ -1,6 +1,5 @@
 config COMEDI
 	tristate "Data acquisition support (comedi)"
-	default N
 	depends on m
 	depends on BROKEN || FRV || M32R || MN10300 || SUPERH || TILE || X86
 	---help---
@@ -14,10 +13,29 @@ config COMEDI_DEBUG
 	  This is an option for use by developers; most people should
 	  say N here. This enables comedi core and driver debugging.
 
+config COMEDI_DEFAULT_BUF_SIZE_KB
+	int "Comedi default initial asynchronous buffer size in KiB"
+	default "2048"
+	depends on COMEDI != n
+	---help---
+	  This is the default asynchronous buffer size which is used for
+	  commands running in the background in kernel space.  This
+	  defaults to 2048 KiB of memory so that a 16 channel card
+	  running at 10 kHz has of 2-4 seconds of buffer.
+
+config COMEDI_DEFAULT_BUF_MAXSIZE_KB
+	int "Comedi default maximum asynchronous buffer size in KiB"
+	default "20480"
+	depends on COMEDI != n
+	---help---
+	  This is the default maximum asynchronous buffer size which can
+	  be requested by a userspace program without root privileges.
+	  This is set to 20480 KiB so that a fast I/O card with 16
+	  channels running at 100 kHz has 2-4 seconds of buffer.
+
 menuconfig COMEDI_MISC_DRIVERS
 	tristate "Comedi misc drivers"
 	depends on COMEDI
-	default N
 	---help---
 	  Enable comedi misc drivers to be built
 
@@ -35,7 +53,6 @@ config COMEDI_KCOMEDILIB
 config COMEDI_BOND
 	tristate "Device bonding support"
 	depends on COMEDI_KCOMEDILIB
-	default N
 	---help---
 	  Enable support for a driver to 'bond' (merge) multiple subdevices
 	  from multiple devices together as one.
@@ -46,7 +63,6 @@ config COMEDI_BOND
 config COMEDI_TEST
 	tristate "Fake waveform generator support"
 	select COMEDI_FC
-	default N
 	---help---
 	  Enable support for the fake waveform generator.
 	  This driver is mainly for testing purposes, but can also be used to
@@ -58,7 +74,6 @@ config COMEDI_TEST
 
 config COMEDI_PARPORT
 	tristate "Parallel port support"
-	default N
 	---help---
 	  Enable support for the standard parallel port.
 	  A cheap and easy way to get a few more digital I/O lines. Steal
@@ -70,7 +85,6 @@ config COMEDI_PARPORT
 
 config COMEDI_SERIAL2002
 	tristate "Driver for serial connected hardware"
-	default N
 	---help---
 	  Enable support for serial connected hardware
 
@@ -79,7 +93,6 @@ config COMEDI_SERIAL2002
 
 config COMEDI_SKEL
 	tristate "Comedi skeleton driver"
-	default N
 	---help---
 	  Build the Skeleton driver, an example for driver writers
 
@@ -91,7 +104,6 @@ endif # COMEDI_MISC_DRIVERS
 menuconfig COMEDI_ISA_DRIVERS
 	tristate "Comedi ISA and PC/104 drivers"
 	depends on COMEDI && ISA
-	default N
 	---help---
 	  Enable comedi ISA and PC/104 drivers to be built
 
@@ -103,7 +115,6 @@ if COMEDI_ISA_DRIVERS && ISA
 
 config COMEDI_ACL7225B
 	tristate "ADlink NuDAQ ACL-7225b and compatibles support"
-	default N
 	---help---
 	  Enable support for ADlink NuDAQ ACL-7225b and compatibles,
 	  ADlink ACL-7225b (acl7225b), ICP P16R16DIO (p16r16dio)
@@ -113,7 +124,6 @@ config COMEDI_ACL7225B
 
 config COMEDI_PCL711
 	tristate "Advantech PCL-711/711b and ADlink ACL-8112 ISA card support"
-	default N
 	---help---
 	  Enable support for Advantech PCL-711 and 711b, ADlink ACL-8112
 
@@ -123,7 +133,6 @@ config COMEDI_PCL711
 config COMEDI_PCL724
 	tristate "Advantech PCL-722/724/731 and ADlink ACL-7122/7124/PET-48DIO"
 	select COMEDI_8255
-	default N
 	---help---
 	  Enable support for Advantech PCL-724, PCL-722, PCL-731 and
 	  ADlink ACL-7122, ACL-7124, PET-48DIO ISA cards
@@ -133,7 +142,6 @@ config COMEDI_PCL724
 
 config COMEDI_PCL725
 	tristate "Advantech PCL-725 and compatible ISA card support"
-	default N
 	---help---
 	  Enable support for Advantech PCL-725 and compatible ISA cards.
 
@@ -142,7 +150,6 @@ config COMEDI_PCL725
 
 config COMEDI_PCL726
 	tristate "Advantech PCL-726 and compatible ISA card support"
-	default N
 	---help---
 	  Enable support for Advantech PCL-726 and compatible ISA cards.
 
@@ -151,7 +158,6 @@ config COMEDI_PCL726
 
 config COMEDI_PCL730
 	tristate "Advantech PCL-730 and ADlink ACL-7130 ISA card support"
-	default N
 	---help---
 	  Enable support for Advantech PCL-730, ICP ISO-730 and ADlink
 	  ACL-7130 ISA cards
@@ -162,7 +168,6 @@ config COMEDI_PCL730
 config COMEDI_PCL812
 	tristate "Advantech PCL-812/813 and ADlink ACL-8112/8113/8113/8216"
 	depends on VIRT_TO_BUS
-	default N
 	---help---
 	  Enable support for Advantech PCL-812/PG, PCL-813/B, ADLink
 	  ACL-8112DG/HG/PG, ACL-8113, ACL-8216, ICP DAS A-821PGH/PGL/PGL-NDA,
@@ -174,7 +179,6 @@ config COMEDI_PCL812
 config COMEDI_PCL816
 	tristate "Advantech PCL-814 and PCL-816 ISA card support"
 	depends on VIRT_TO_BUS
-	default N
 	---help---
 	  Enable support for Advantech PCL-814 and PCL-816 ISA cards
 
@@ -184,7 +188,6 @@ config COMEDI_PCL816
 config COMEDI_PCL818
 	tristate "Advantech PCL-718 and PCL-818 ISA card support"
 	depends on VIRT_TO_BUS
-	default N
 	---help---
 	  Enable support for Advantech PCL-818 ISA cards
 	  PCL-818L, PCL-818H, PCL-818HD, PCL-818HG, PCL-818 and PCL-718
@@ -195,7 +198,6 @@ config COMEDI_PCL818
 config COMEDI_PCM3724
 	tristate "Advantech PCM-3724 PC/104 card support"
 	select COMEDI_8255
-	default N
 	---help---
 	  Enable support for Advantech PCM-3724 PC/104 cards.
 
@@ -204,16 +206,43 @@ config COMEDI_PCM3724
 
 config COMEDI_PCM3730
 	tristate "Advantech PCM-3730 and clone PC/104 board support"
-	default N
 	---help---
 	  Enable support for Advantech PCM-3730 and clone PC/104 boards
 
 	  To compile this driver as a module, choose M here: the module will be
 	  called pcm3730.
 
+config COMEDI_AMPLC_DIO200_ISA
+	tristate "Amplicon PC212E/PC214E/PC215E/PC218E/PC272E"
+	select COMEDI_AMPLC_DIO200
+	depends on COMEDI_ISA_DRIVERS
+	---help---
+	  Enable support for Amplicon PC212E, PC214E, PC215E, PC218E and
+	  PC272E ISA DIO boards
+
+	  To compile this driver as a module, choose M here: the module will be
+	  called amplc_dio200.
+
+config COMEDI_AMPLC_PC236_ISA
+	tristate "Amplicon PC36AT DIO board support"
+	select COMEDI_AMPLC_PC236
+	---help---
+	  Enable support for Amplicon PC36AT ISA DIO board.
+
+	  To compile this driver as a module, choose M here: the module will be
+	  called amplc_pc236.
+
+config COMEDI_AMPLC_PC263_ISA
+	tristate "Amplicon PC263 relay board support"
+	select COMEDI_AMPLC_PC263
+	---help---
+	  Enable support for Amplicon PC263 ISA relay board.
+
+	  To compile this driver as a module, choose M here: the module will be
+	  called amplc_pc263.
+
 config COMEDI_RTI800
 	tristate "Analog Devices RTI-800/815 ISA card support"
-	default N
 	---help---
 	  Enable support for Analog Devices RTI-800/815 ISA cards
 
@@ -222,7 +251,6 @@ config COMEDI_RTI800
 
 config COMEDI_RTI802
 	tristate "Analog Devices RTI-802 ISA card support"
-	default N
 	---help---
 	  Enable support for Analog Devices RTI-802 ISA cards
 
@@ -233,18 +261,29 @@ config COMEDI_DAS16M1
 	tristate "MeasurementComputing CIO-DAS16/M1DAS-16 ISA card support"
 	select COMEDI_8255
 	select COMEDI_FC
-	default N
 	---help---
 	  Enable support for Measurement Computing CIO-DAS16/M1 ISA cards.
 
 	  To compile this driver as a module, choose M here: the module will be
 	  called das16m1.
 
+config COMEDI_DAS08_ISA
+	tristate "DAS-08 compatible ISA and PC/104 card support"
+	select COMEDI_DAS08
+	---help---
+	  Enable support for Keithley Metrabyte/ComputerBoards DAS08
+	  and compatible ISA and PC/104 cards:
+	  Keithley Metrabyte/ComputerBoards DAS08, DAS08-PGM, DAS08-PGH,
+	  DAS08-PGL, DAS08-AOH, DAS08-AOL, DAS08-AOM, DAS08/JR-AO,
+	  DAS08/JR-16-AO, PC104-DAS08, DAS08/JR/16.
+
+	  To compile this driver as a module, choose M here: the module will be
+	  called das08.
+
 config COMEDI_DAS16
 	tristate "DAS-16 compatible ISA and PC/104 card support"
 	select COMEDI_8255
 	select COMEDI_FC
-	default N
 	---help---
 	  Enable support for Keithley Metrabyte/ComputerBoards DAS16
 	  and compatible ISA and PC/104 cards:
@@ -261,7 +300,6 @@ config COMEDI_DAS16
 config COMEDI_DAS800
 	tristate "DAS800 and compatible ISA card support"
 	select COMEDI_FC
-	default N
 	---help---
 	  Enable support for Keithley Metrabyte DAS800 and compatible ISA cards
 	  Keithley Metrabyte DAS-800, DAS-801, DAS-802
@@ -275,7 +313,6 @@ config COMEDI_DAS1800
 	tristate "DAS1800 and compatible ISA card support"
 	depends on VIRT_TO_BUS
 	select COMEDI_FC
-	default N
 	---help---
 	  Enable support for DAS1800 and compatible ISA cards
 	  Keithley Metrabyte DAS-1701ST, DAS-1701ST-DA, DAS-1701/AO,
@@ -289,7 +326,6 @@ config COMEDI_DAS1800
 
 config COMEDI_DAS6402
 	tristate "DAS6402 and compatible ISA card support"
-	default N
 	---help---
 	  Enable support for DAS6402 and compatible ISA cards
 	  Computerboards, Keithley Metrabyte DAS6402 and compatibles
@@ -299,7 +335,6 @@ config COMEDI_DAS6402
 
 config COMEDI_DT2801
 	tristate "Data Translation DT2801 ISA card support"
-	default N
 	---help---
 	  Enable support for Data Translation DT2801 ISA cards
 
@@ -308,7 +343,6 @@ config COMEDI_DT2801
 
 config COMEDI_DT2811
 	tristate "Data Translation DT2811 ISA card support"
-	default N
 	---help---
 	  Enable support for Data Translation DT2811 ISA cards
 
@@ -317,7 +351,6 @@ config COMEDI_DT2811
 
 config COMEDI_DT2814
 	tristate "Data Translation DT2814 ISA card support"
-	default N
 	---help---
 	  Enable support for Data Translation DT2814 ISA cards
 
@@ -326,7 +359,6 @@ config COMEDI_DT2814
 
 config COMEDI_DT2815
 	tristate "Data Translation DT2815 ISA card support"
-	default N
 	---help---
 	  Enable support for Data Translation DT2815 ISA cards
 
@@ -335,7 +367,6 @@ config COMEDI_DT2815
 
 config COMEDI_DT2817
 	tristate "Data Translation DT2817 ISA card support"
-	default N
 	---help---
 	  Enable support for Data Translation DT2817 ISA cards
 
@@ -346,7 +377,6 @@ config COMEDI_DT282X
 	tristate "Data Translation DT2821 series and DT-EZ ISA card support"
 	select COMEDI_FC
 	depends on VIRT_TO_BUS
-	default N
 	---help---
 	  Enable support for Data Translation DT2821 series including DT-EZ
 	  DT2821, DT2821-F-16SE, DT2821-F-8DI, DT2821-G-16SE, DT2821-G-8DI,
@@ -358,7 +388,6 @@ config COMEDI_DT282X
 
 config COMEDI_DMM32AT
 	tristate "Diamond Systems MM-32-AT PC/104 board support"
-	default N
 	---help---
 	  Enable support for Diamond Systems MM-32-AT PC/104 boards
 
@@ -367,7 +396,6 @@ config COMEDI_DMM32AT
 
 config COMEDI_FL512
 	tristate "FL512 ISA card support"
-	default N
 	---help---
 	  Enable support for FL512 ISA card
 
@@ -377,7 +405,6 @@ config COMEDI_FL512
 config COMEDI_AIO_AIO12_8
 	tristate "I/O Products PC/104 AIO12-8 Analog I/O Board support"
 	select COMEDI_8255
-	default N
 	---help---
 	  Enable support for I/O Products PC/104 AIO12-8 Analog I/O Board
 
@@ -386,7 +413,6 @@ config COMEDI_AIO_AIO12_8
 
 config COMEDI_AIO_IIRO_16
 	tristate "I/O Products PC/104 IIRO16 Board support"
-	default N
 	---help---
 	  Enable support for I/O Products PC/104 IIRO16 Relay And Isolated
 	  Input Board
@@ -396,7 +422,6 @@ config COMEDI_AIO_IIRO_16
 
 config COMEDI_C6XDIGIO
 	tristate "Mechatronic Systems Inc. C6x_DIGIO DSP daughter card support"
-	default N
 	---help---
 	  Enable support for Mechatronic Systems Inc. C6x_DIGIO DSP daughter
 	  card
@@ -406,7 +431,6 @@ config COMEDI_C6XDIGIO
 
 config COMEDI_MPC624
 	tristate "Micro/sys MPC-624 PC/104 board support"
-	default N
 	---help---
 	  Enable support for Micro/sys MPC-624 PC/104 board
 
@@ -415,7 +439,6 @@ config COMEDI_MPC624
 
 config COMEDI_ADQ12B
 	tristate "MicroAxial ADQ12-B data acquisition and control card support"
-	default N
 	---help---
 	  Enable MicroAxial ADQ12-B daq and control card support.
 
@@ -426,7 +449,6 @@ config COMEDI_NI_AT_A2150
 	tristate "NI AT-A2150 ISA card support"
 	depends on COMEDI_NI_COMMON
 	depends on VIRT_TO_BUS
-	default N
 	---help---
 	  Enable support for National Instruments AT-A2150 cards
 
@@ -436,7 +458,6 @@ config COMEDI_NI_AT_A2150
 config COMEDI_NI_AT_AO
 	tristate "NI AT-AO-6/10 EISA card support"
 	depends on COMEDI_NI_COMMON
-	default N
 	---help---
 	  Enable support for National Instruments AT-AO-6/10 cards
 
@@ -447,7 +468,6 @@ config COMEDI_NI_ATMIO
 	tristate "NI AT-MIO E series ISA-PNP card support"
 	depends on ISAPNP && COMEDI_NI_TIO && COMEDI_NI_COMMON
 	select COMEDI_8255
-	default N
 	---help---
 	  Enable support for National Instruments AT-MIO E series cards
 	  National Instruments AT-MIO-16E-1 (ni_atmio),
@@ -461,7 +481,6 @@ config COMEDI_NI_ATMIO16D
 	tristate "NI AT-MIO16/AT-MIO16D series ISA-PNP card support"
 	depends on ISAPNP && COMEDI_NI_COMMON
 	select COMEDI_8255
-	default N
 	---help---
 	  Enable support for National Instruments AT-MIO16/AT-MIO16D cards.
 
@@ -470,7 +489,6 @@ config COMEDI_NI_ATMIO16D
 
 config COMEDI_PCMAD
 	tristate "Winsystems PCM-A/D12 and PCM-A/D16 PC/104 board support"
-	default N
 	---help---
 	  Enable support for Winsystems PCM-A/D12 and PCM-A/D16 PC/104 boards.
 
@@ -479,7 +497,6 @@ config COMEDI_PCMAD
 
 config COMEDI_PCMDA12
 	tristate "Winsystems PCM-D/A-12 8-channel AO PC/104 board support"
-	default N
 	---help---
 	  Enable support for Winsystems PCM-D/A-12 8-channel AO PC/104 boards.
 	  Note that the board is not ISA-PNP capable and thus needs the I/O
@@ -490,7 +507,6 @@ config COMEDI_PCMDA12
 
 config COMEDI_PCMMIO
 	tristate "Winsystems PCM-MIO PC/104 board support"
-	default N
 	---help---
 	  Enable support for Winsystems PCM-MIO multifunction PC/104 boards.
 
@@ -499,7 +515,6 @@ config COMEDI_PCMMIO
 
 config COMEDI_PCMUIO
 	tristate "Winsystems PCM-UIO48A and PCM-UIO96A PC/104 board support"
-	default N
 	---help---
 	  Enable support for PCM-UIO48A and PCM-UIO96A PC/104 boards.
 
@@ -508,7 +523,6 @@ config COMEDI_PCMUIO
 
 config COMEDI_MULTIQ3
 	tristate "Quanser Consulting MultiQ-3 ISA card support"
-	default N
 	---help---
 	  Enable support for Quanser Consulting MultiQ-3 ISA cards
 
@@ -517,7 +531,6 @@ config COMEDI_MULTIQ3
 
 config COMEDI_POC
 	tristate "Generic driver for very simple devices"
-	default N
 	---help---
 	  Enable generic support for very simple / POC (Piece of Crap) boards,
 	  Keithley Metrabyte DAC-02 (dac02), Advantech PCL-733 (pcl733) and
@@ -531,7 +544,6 @@ endif # COMEDI_ISA_DRIVERS
 menuconfig COMEDI_PCI_DRIVERS
 	tristate "Comedi PCI drivers"
 	depends on COMEDI && PCI
-	default N
 	---help---
 	  Enable comedi PCI drivers to be built
 
@@ -544,7 +556,6 @@ if COMEDI_PCI_DRIVERS && PCI
 config COMEDI_ADDI_APCI_035
 	tristate "ADDI-DATA APCI_035 support"
 	depends on VIRT_TO_BUS
-	default N
 	---help---
 	  Enable support for ADDI-DATA APCI_035 cards
 
@@ -554,7 +565,6 @@ config COMEDI_ADDI_APCI_035
 config COMEDI_ADDI_APCI_1032
 	tristate "ADDI-DATA APCI_1032 support"
 	depends on VIRT_TO_BUS
-	default N
 	---help---
 	  Enable support for ADDI-DATA APCI_1032 cards
 
@@ -564,7 +574,6 @@ config COMEDI_ADDI_APCI_1032
 config COMEDI_ADDI_APCI_1500
 	tristate "ADDI-DATA APCI_1500 support"
 	depends on VIRT_TO_BUS
-	default N
 	---help---
 	  Enable support for ADDI-DATA APCI_1500 cards
 
@@ -574,7 +583,6 @@ config COMEDI_ADDI_APCI_1500
 config COMEDI_ADDI_APCI_1516
 	tristate "ADDI-DATA APCI_1516 support"
 	depends on VIRT_TO_BUS
-	default N
 	---help---
 	  Enable support for ADDI-DATA APCI_1516 cards
 
@@ -584,7 +592,6 @@ config COMEDI_ADDI_APCI_1516
 config COMEDI_ADDI_APCI_1564
 	tristate "ADDI-DATA APCI_1564 support"
 	depends on VIRT_TO_BUS
-	default N
 	---help---
 	  Enable support for ADDI-DATA APCI_1564 cards
 
@@ -594,7 +601,6 @@ config COMEDI_ADDI_APCI_1564
 config COMEDI_ADDI_APCI_16XX
 	tristate "ADDI-DATA APCI_16xx support"
 	depends on VIRT_TO_BUS
-	default N
 	---help---
 	  Enable support for ADDI-DATA APCI_16xx cards
 
@@ -604,7 +610,6 @@ config COMEDI_ADDI_APCI_16XX
 config COMEDI_ADDI_APCI_2016
 	tristate "ADDI-DATA APCI_2016 support"
 	depends on VIRT_TO_BUS
-	default N
 	---help---
 	  Enable support for ADDI-DATA APCI_2016 cards
 
@@ -614,7 +619,6 @@ config COMEDI_ADDI_APCI_2016
 config COMEDI_ADDI_APCI_2032
 	tristate "ADDI-DATA APCI_2032 support"
 	depends on VIRT_TO_BUS
-	default N
 	---help---
 	  Enable support for ADDI-DATA APCI_2032 cards
 
@@ -624,7 +628,6 @@ config COMEDI_ADDI_APCI_2032
 config COMEDI_ADDI_APCI_2200
 	tristate "ADDI-DATA APCI_2200 support"
 	depends on VIRT_TO_BUS
-	default N
 	---help---
 	  Enable support for ADDI-DATA APCI_2200 cards
 
@@ -635,7 +638,6 @@ config COMEDI_ADDI_APCI_3001
 	tristate "ADDI-DATA APCI_3001 support"
 	depends on VIRT_TO_BUS
 	select COMEDI_FC
-	default N
 	---help---
 	  Enable support for ADDI-DATA APCI_3001 cards
 
@@ -646,7 +648,6 @@ config COMEDI_ADDI_APCI_3120
 	tristate "ADDI-DATA APCI_3520 support"
 	depends on VIRT_TO_BUS
 	select COMEDI_FC
-	default N
 	---help---
 	  Enable support for ADDI-DATA APCI_3520 cards
 
@@ -656,7 +657,6 @@ config COMEDI_ADDI_APCI_3120
 config COMEDI_ADDI_APCI_3501
 	tristate "ADDI-DATA APCI_3501 support"
 	depends on VIRT_TO_BUS
-	default N
 	---help---
 	  Enable support for ADDI-DATA APCI_3501 cards
 
@@ -666,7 +666,6 @@ config COMEDI_ADDI_APCI_3501
 config COMEDI_ADDI_APCI_3XXX
 	tristate "ADDI-DATA APCI_3xxx support"
 	depends on VIRT_TO_BUS
-	default N
 	---help---
 	  Enable support for ADDI-DATA APCI_3xxx cards
 
@@ -676,7 +675,6 @@ config COMEDI_ADDI_APCI_3XXX
 config COMEDI_ADL_PCI6208
 	tristate "ADLink PCI-6208A support"
 	select COMEDI_8255
-	default N
 	---help---
 	  Enable support for ADLink PCI-6208A cards
 
@@ -685,7 +683,6 @@ config COMEDI_ADL_PCI6208
 
 config COMEDI_ADL_PCI7230
 	tristate "ADLink PCI-7230 digital io board support"
-	default N
 	---help---
 	  Enable support for ADlink PCI-7230 digital io board support
 
@@ -694,7 +691,6 @@ config COMEDI_ADL_PCI7230
 
 config COMEDI_ADL_PCI7296
 	tristate "ADLink PCI-7296 96 ch. digital io board support"
-	default N
 	---help---
 	  Enable support for ADlink PCI-7296 96 ch. digital io board support
 
@@ -703,7 +699,6 @@ config COMEDI_ADL_PCI7296
 
 config COMEDI_ADL_PCI7432
 	tristate "ADLink PCI-7432 64 ch. isolated digital io board support"
-	default N
 	---help---
 	  Enable support for ADlink PCI-7432 64 ch. isolated digital io board
 
@@ -712,7 +707,6 @@ config COMEDI_ADL_PCI7432
 
 config COMEDI_ADL_PCI8164
 	tristate "ADLink PCI-8164 4 Axes Motion Control board support"
-	default N
 	---help---
 	  Enable support for ADlink PCI-8164 4 Axes Motion Control board
 
@@ -722,7 +716,6 @@ config COMEDI_ADL_PCI8164
 config COMEDI_ADL_PCI9111
 	tristate "ADLink PCI-9111HR support"
 	select COMEDI_FC
-	default N
 	---help---
 	  Enable support for ADlink PCI9111 cards
 
@@ -733,7 +726,6 @@ config COMEDI_ADL_PCI9118
 	tristate "ADLink PCI-9118DG, PCI-9118HG, PCI-9118HR support"
 	select COMEDI_FC
 	depends on VIRT_TO_BUS
-	default N
 	---help---
 	  Enable support for ADlink PCI-9118DG, PCI-9118HG, PCI-9118HR cards
 
@@ -742,7 +734,6 @@ config COMEDI_ADL_PCI9118
 
 config COMEDI_ADV_PCI1710
 	tristate "Advantech PCI-171x, PCI-1720 and PCI-1731 support"
-	default N
 	---help---
 	  Enable support for Advantech PCI-1710, PCI-1710HG, PCI-1711,
 	  PCI-1713, PCI-1720 and PCI-1731
@@ -752,7 +743,6 @@ config COMEDI_ADV_PCI1710
 
 config COMEDI_ADV_PCI1723
 	tristate "Advantech PCI-1723 support"
-	default N
 	---help---
 	  Enable support for Advantech PCI-1723 cards
 
@@ -762,7 +752,6 @@ config COMEDI_ADV_PCI1723
 config COMEDI_ADV_PCI_DIO
 	tristate "Advantech PCI DIO card support"
 	select COMEDI_8255
-	default N
 	---help---
 	  Enable support for Advantech PCI DIO cards
 	  PCI-1730, PCI-1733, PCI-1734, PCI-1735U, PCI-1736UP, PCI-1739U,
@@ -772,31 +761,29 @@ config COMEDI_ADV_PCI_DIO
 	  To compile this driver as a module, choose M here: the module will be
 	  called adv_pci_dio.
 
-config COMEDI_AMPLC_DIO200
-	tristate "Amplicon PC272E and PCI272 DIO board support"
-	select COMEDI_8255
-	default N
+config COMEDI_AMPLC_DIO200_PCI
+	tristate "Amplicon PCI215 and PCI272 DIO board support"
+	select COMEDI_AMPLC_DIO200
 	---help---
-	  Enable support for Amplicon PC272E and PCI272 DIO boards
+	  Enable support for Amplicon PCI215 and PCI272 DIO boards.
 
 	  To compile this driver as a module, choose M here: the module will be
 	  called amplc_dio200.
 
-config COMEDI_AMPLC_PC236
-	tristate "Amplicon PC36AT and PCI236 DIO board support"
-	select COMEDI_8255
-	default N
+config COMEDI_AMPLC_PC236_PCI
+	tristate "Amplicon PCI236 DIO board support"
+	select COMEDI_AMPLC_PC236
 	---help---
-	  Enable support for Amplicon PC36AT and PCI236 DIO boards
+	  Enable support for Amplicon PCI236 DIO board.
 
 	  To compile this driver as a module, choose M here: the module will be
 	  called amplc_pc236.
 
-config COMEDI_AMPLC_PC263
-	tristate "Amplicon PC263 and PCI263 relay board support"
-	default N
+config COMEDI_AMPLC_PC263_PCI
+	tristate "Amplicon PCI263 relay board support"
+	select COMEDI_AMPLC_PC263
 	---help---
-	  Enable support for Amplicon PC263 and PCI263 relay boards
+	  Enable support for Amplicon PCI263 relay board.
 
 	  To compile this driver as a module, choose M here: the module will be
 	  called amplc_pc263.
@@ -804,7 +791,6 @@ config COMEDI_AMPLC_PC263
 config COMEDI_AMPLC_PCI224
 	tristate "Amplicon PCI224 and PCI234 support"
 	select COMEDI_FC
-	default N
 	---help---
 	  Enable support for Amplicon PCI224 and PCI234 AO boards
 
@@ -814,7 +800,6 @@ config COMEDI_AMPLC_PCI224
 config COMEDI_AMPLC_PCI230
 	tristate "Amplicon PCI230 and PCI260 support"
 	select COMEDI_8255
-	default N
 	---help---
 	  Enable support for Amplicon PCI230 and PCI260 Multifunction I/O
 	  boards
@@ -824,16 +809,23 @@ config COMEDI_AMPLC_PCI230
 
 config COMEDI_CONTEC_PCI_DIO
 	tristate "Contec PIO1616L digital I/O board support"
-	default N
 	---help---
 	  Enable support for the Contec PIO1616L digital I/O board
 
 	  To compile this driver as a module, choose M here: the module will be
 	  called contec_pci_dio.
 
+config COMEDI_DAS08_PCI
+	tristate "DAS-08 PCI support"
+	select COMEDI_DAS08
+	---help---
+	  Enable support for PCI DAS-08 cards.
+
+	  To compile this driver as a module, choose M here: the module will be
+	  called das08.
+
 config COMEDI_DT3000
 	tristate "Data Translation DT3000 series support"
-	default N
 	---help---
 	  Enable support for Data Translation DT3000 series
 	  DT3001, DT3001-PGL, DT3002, DT3003, DT3003-PGL, DT3004, DT3005 and
@@ -844,7 +836,6 @@ config COMEDI_DT3000
 
 config COMEDI_DYNA_PCI10XX
 	tristate "Dynalog PCI DAQ series support"
-	default N
 	---help---
 	  Enable support for Dynalog PCI DAQ series
 	  PCI-1050
@@ -854,7 +845,6 @@ config COMEDI_DYNA_PCI10XX
 
 config COMEDI_UNIOXX5
 	tristate "Fastwel UNIOxx-5 analog and digital io board support"
-	default N
 	---help---
 	  Enable support for Fastwel UNIOxx-5 (analog and digital i/o) boards
 
@@ -864,7 +854,6 @@ config COMEDI_UNIOXX5
 config COMEDI_GSC_HPDI
 	tristate "General Standards PCI-HPDI32 / PMC-HPDI32 support"
 	select COMEDI_FC
-	default N
 	---help---
 	  Enable support for General Standards Corporation high speed parallel
 	  digital interface rs485 boards PCI-HPDI32 and PMC-HPDI32.
@@ -875,7 +864,6 @@ config COMEDI_GSC_HPDI
 
 config COMEDI_ICP_MULTI
 	tristate "Inova ICP_MULTI support"
-	default N
 	---help---
 	  Enable support for Inova ICP_MULTI card
 
@@ -884,7 +872,6 @@ config COMEDI_ICP_MULTI
 
 config COMEDI_II_PCI20KC
 	tristate "Intelligent Instruments PCI-20001C carrier support"
-	default N
 	---help---
 	  Enable support for Intelligent Instruments PCI-20001C carrier
 	  PCI-20001, PCI-20006 and PCI-20341
@@ -895,7 +882,6 @@ config COMEDI_II_PCI20KC
 config COMEDI_DAQBOARD2000
 	tristate "IOtech DAQboard/2000 support"
 	select COMEDI_8255
-	default N
 	---help---
 	  Enable support for the IOtech DAQboard/2000
 
@@ -904,7 +890,6 @@ config COMEDI_DAQBOARD2000
 
 config COMEDI_JR3_PCI
 	tristate "JR3/PCI force sensor board support"
-	default N
 	---help---
 	  Enable support for JR3/PCI force sensor boards
 
@@ -913,7 +898,6 @@ config COMEDI_JR3_PCI
 
 config COMEDI_KE_COUNTER
 	tristate "Kolter-Electronic PCI Counter 1 card support"
-	default N
 	---help---
 	  Enable support for Kolter-Electronic PCI Counter 1 cards
 
@@ -924,7 +908,6 @@ config COMEDI_CB_PCIDAS64
 	tristate "MeasurementComputing PCI-DAS 64xx, 60xx, and 4020 support"
 	select COMEDI_8255
 	select COMEDI_FC
-	default N
 	---help---
 	  Enable support for ComputerBoards/MeasurementComputing PCI-DAS 64xx,
 	  60xx, and 4020 series with the PLX 9080 PCI controller
@@ -936,7 +919,6 @@ config COMEDI_CB_PCIDAS
 	tristate "MeasurementComputing PCI-DAS support"
 	select COMEDI_8255
 	select COMEDI_FC
-	default N
 	---help---
 	  Enable support for ComputerBoards/MeasurementComputing PCI-DAS with
 	  AMCC S5933 PCIcontroller: PCI-DAS1602/16, PCI-DAS1602/16jr,
@@ -949,7 +931,6 @@ config COMEDI_CB_PCIDAS
 config COMEDI_CB_PCIDDA
 	tristate "MeasurementComputing PCI-DDA series support"
 	select COMEDI_8255
-	default N
 	---help---
 	  Enable support for ComputerBoards/MeasurementComputing PCI-DDA
 	  series: PCI-DDA08/12, PCI-DDA04/12, PCI-DDA02/12, PCI-DDA08/16,
@@ -961,7 +942,6 @@ config COMEDI_CB_PCIDDA
 config COMEDI_CB_PCIDIO
 	tristate "MeasurementComputing PCI-DIO series support"
 	select COMEDI_8255
-	default N
 	---help---
 	  Enable support for ComputerBoards/MeasurementComputing PCI-DIO series
 	  PCI-DIO24, PCI-DIO24H and PCI-DIO48H
@@ -972,7 +952,6 @@ config COMEDI_CB_PCIDIO
 config COMEDI_CB_PCIMDAS
 	tristate "MeasurementComputing PCIM-DAS1602/16 support"
 	select COMEDI_8255
-	default N
 	---help---
 	  Enable support for ComputerBoards/MeasurementComputing PCI Migration
 	  series PCIM-DAS1602/16
@@ -983,7 +962,6 @@ config COMEDI_CB_PCIMDAS
 config COMEDI_CB_PCIMDDA
 	tristate "MeasurementComputing PCIM-DDA06-16 support"
 	select COMEDI_8255
-	default N
 	---help---
 	  Enable support for ComputerBoards/MeasurementComputing PCIM-DDA06-16
 
@@ -992,7 +970,6 @@ config COMEDI_CB_PCIMDDA
 
 config COMEDI_ME4000
 	tristate "Meilhaus ME-4000 support"
-	default N
 	---help---
 	  Enable support for Meilhaus PCI data acquisition cards
 	  ME-4650, ME-4670i, ME-4680, ME-4680i and ME-4680is
@@ -1002,7 +979,6 @@ config COMEDI_ME4000
 
 config COMEDI_ME_DAQ
 	tristate "Meilhaus ME-2000i, ME-2600i, ME-3000vm1 support"
-	default N
 	---help---
 	  Enable support for Meilhaus PCI data acquisition cards
 	  ME-2000i, ME-2600i and ME-3000vm1
@@ -1013,7 +989,6 @@ config COMEDI_ME_DAQ
 config COMEDI_NI_6527
 	tristate "NI 6527 support"
 	depends on COMEDI_MITE
-	default N
 	---help---
 	  Enable support for the National Instruments 6527 PCI card
 
@@ -1023,7 +998,6 @@ config COMEDI_NI_6527
 config COMEDI_NI_65XX
 	tristate "NI 65xx static dio PCI card support"
 	depends on COMEDI_MITE
-	default N
 	---help---
 	  Enable support for National Instruments 65xx static dio boards.
 	  Supported devices: National Instruments PCI-6509 (ni_65xx),
@@ -1037,7 +1011,6 @@ config COMEDI_NI_65XX
 config COMEDI_NI_660X
 	tristate "NI 660x counter/timer PCI card support"
 	depends on COMEDI_NI_TIO && COMEDI_NI_COMMON
-	default N
 	---help---
 	  Enable support for National Instruments PCI-6601 (ni_660x), PCI-6602,
 	  PXI-6602 and PXI-6608.
@@ -1048,7 +1021,6 @@ config COMEDI_NI_660X
 config COMEDI_NI_670X
 	tristate "NI 670x PCI card support"
 	depends on COMEDI_MITE
-	default N
 	---help---
 	  Enable support for National Instruments PCI-6703 and PCI-6704
 
@@ -1059,7 +1031,6 @@ config COMEDI_NI_PCIDIO
 	tristate "NI PCI-DIO32HS, PCI-DIO96, PCI-6533, PCI-6503 support"
 	depends on COMEDI_MITE
 	select COMEDI_8255
-	default N
 	---help---
 	  Enable support for National Instruments PCI-DIO-32HS, PXI-6533,
 	  PCI-DIO-96, PCI-DIO-96B, PXI-6508, PCI-6503, PCI-6503B, PCI-6503X,
@@ -1075,7 +1046,6 @@ config COMEDI_NI_PCIMIO
 	depends on COMEDI_NI_TIO && COMEDI_NI_COMMON
 	select COMEDI_8255
 	select COMEDI_FC
-	default N
 	---help---
 	  Enable support for National Instruments PCI-MIO-E series and M series
 	  (all boards): PCI-MIO-16XE-10, PXI-6030E, PCI-MIO-16E-1,
@@ -1094,7 +1064,6 @@ config COMEDI_NI_PCIMIO
 config COMEDI_RTD520
 	tristate "Real Time Devices PCI4520/DM7520 support"
 	select COMEDI_8255
-	default N
 	---help---
 	  Enable support for Real Time Devices PCI4520/DM7520
 
@@ -1103,7 +1072,6 @@ config COMEDI_RTD520
 
 config COMEDI_S526
 	tristate "Sensoray s526 support"
-	default N
 	---help---
 	  Enable support for Sensoray s526
 
@@ -1113,7 +1081,6 @@ config COMEDI_S526
 config COMEDI_S626
 	tristate "Sensoray 626 support"
 	select COMEDI_FC
-	default N
 	---help---
 	  Enable support for Sensoray 626
 
@@ -1122,7 +1089,6 @@ config COMEDI_S626
 
 config COMEDI_SSV_DNP
 	tristate "SSV Embedded Systems DIL/Net-PC support"
-	default N
 	---help---
 	  Enable support for SSV Embedded Systems DIL/Net-PC
 
@@ -1134,7 +1100,6 @@ endif # COMEDI_PCI_DRIVERS
 menuconfig COMEDI_PCMCIA_DRIVERS
 	tristate "Comedi PCMCIA drivers"
 	depends on COMEDI && (PCMCIA || PCCARD)
-	default N
 	---help---
 	  Enable comedi PCMCIA and PCCARD drivers to be built
 
@@ -1146,7 +1111,6 @@ if COMEDI_PCMCIA_DRIVERS && PCMCIA
 
 config COMEDI_CB_DAS16_CS
 	tristate "CB DAS16 series PCMCIA support"
-	default N
 	---help---
 	  Enable support for the ComputerBoards/MeasurementComputing PCMCIA
 	  cards DAS16/16, PCM-DAS16D/12 and PCM-DAS16s/16
@@ -1157,7 +1121,6 @@ config COMEDI_CB_DAS16_CS
 config COMEDI_DAS08_CS
 	tristate "CB DAS08 PCMCIA support"
 	select COMEDI_DAS08
-	default N
 	---help---
 	  Enable support for the ComputerBoards/MeasurementComputing DAS-08
 	  PCMCIA card
@@ -1168,7 +1131,6 @@ config COMEDI_DAS08_CS
 config COMEDI_NI_DAQ_700_CS
 	tristate "NI DAQCard-700 PCMCIA support"
 	depends on COMEDI_NI_COMMON
-	default N
 	---help---
 	  Enable support for the National Instruments PCMCIA DAQCard-700 DIO
 
@@ -1179,7 +1141,6 @@ config COMEDI_NI_DAQ_DIO24_CS
 	tristate "NI DAQ-Card DIO-24 PCMCIA support"
 	depends on COMEDI_NI_COMMON
 	select COMEDI_8255
-	default N
 	---help---
 	  Enable support for the National Instruments PCMCIA DAQ-Card DIO-24
 
@@ -1189,7 +1150,6 @@ config COMEDI_NI_DAQ_DIO24_CS
 config COMEDI_NI_LABPC_CS
 	tristate "NI DAQCard-1200 PCMCIA support"
 	depends on COMEDI_NI_LABPC
-	default N
 	---help---
 	  Enable support for the National Instruments PCMCIA DAQCard-1200
 
@@ -1201,7 +1161,6 @@ config COMEDI_NI_MIO_CS
 	depends on COMEDI_NI_TIO && COMEDI_NI_COMMON
 	select COMEDI_8255
 	select COMEDI_FC
-	default N
 	---help---
 	  Enable support for the National Instruments PCMCIA DAQCard E series
 	  DAQCard-ai-16xe-50, DAQCard-ai-16e-4, DAQCard-6062E, DAQCard-6024E
@@ -1212,7 +1171,6 @@ config COMEDI_NI_MIO_CS
 
 config COMEDI_QUATECH_DAQP_CS
 	tristate "Quatech DAQP PCMCIA data capture card support"
-	default N
 	---help---
 	  Enable support for the Quatech DAQP PCMCIA data capture cards
 	  DAQP-208 and DAQP-308
@@ -1225,7 +1183,6 @@ endif # COMEDI_PCMCIA_DRIVERS
 menuconfig COMEDI_USB_DRIVERS
 	tristate "Comedi USB drivers"
 	depends on COMEDI && USB
-	default N
 	---help---
 	  Enable comedi USB drivers to be built
 
@@ -1237,7 +1194,6 @@ if COMEDI_USB_DRIVERS && USB
 
 config COMEDI_DT9812
 	tristate "DataTranslation DT9812 USB module support"
-	default N
 	---help---
 	  Enable support for the Data Translation DT9812 USB module
 
@@ -1246,7 +1202,6 @@ config COMEDI_DT9812
 
 config COMEDI_USBDUX
 	tristate "ITL USB-DUX-D support"
-	default N
 	---help---
 	  Enable support for the Incite Technology Ltd USB-DUX-D Board
 
@@ -1256,7 +1211,6 @@ config COMEDI_USBDUX
 config COMEDI_USBDUXFAST
 	tristate "ITL USB-DUXfast support"
 	select COMEDI_FC
-	default N
 	---help---
 	  Enable support for the Incite Technology Ltd USB-DUXfast Board
 
@@ -1266,7 +1220,6 @@ config COMEDI_USBDUXFAST
 config COMEDI_USBDUXSIGMA
 	tristate "ITL USB-DUXsigma support"
 	select COMEDI_FC
-	default N
 	---help---
 	  Enable support for the Incite Technology Ltd USB-DUXsigma Board
 
@@ -1275,7 +1228,6 @@ config COMEDI_USBDUXSIGMA
 
 config COMEDI_VMK80XX
 	tristate "Velleman VM110/VM140 USB Board support"
-	default N
 	---help---
 	  Build the Velleman USB Board Low-Level Driver supporting the
 	  K8055/K8061 aka VM110/VM140 devices
@@ -1288,7 +1240,6 @@ endif # COMEDI_USB_DRIVERS
 menuconfig COMEDI_NI_COMMON
 	tristate "Comedi National Instruments card support"
 	depends on COMEDI
-	default N
 	---help---
 	  Enable comedi support for National Instruments cards.
 	  Modules in this section are used by many comedi NI drivers.
@@ -1302,7 +1253,6 @@ if COMEDI_NI_COMMON
 config COMEDI_MITE
 	tristate "NI Mite PCI interface chip support"
 	depends on PCI
-	default N
 	---help---
 	  Enable support for National Instruments Mite PCI interface chip
 
@@ -1312,7 +1262,6 @@ config COMEDI_MITE
 config COMEDI_NI_TIO
 	tristate "NI general purpose counter support"
 	depends on COMEDI_MITE
-	default N
 	---help---
 	  Enable support for National Instruments general purpose counters.
 	  This module is not used directly by end-users. Rather, it
@@ -1328,7 +1277,6 @@ config COMEDI_NI_LABPC
 	select COMEDI_8255
 	select COMEDI_FC
 	depends on VIRT_TO_BUS
-	default N
 	---help---
 	  Enable support for National Instruments Lab-PC and compatibles
 	  Lab-PC-1200, Lab-PC-1200AI, Lab-PC+ and PCI-1200.
@@ -1343,7 +1291,6 @@ endif # COMEDI_NI_COMMON
 config COMEDI_8255
 	tristate "Generic 8255 support"
 	depends on COMEDI
-	default N
 	---help---
 	  Enable generic 8255 support.
 
@@ -1357,24 +1304,9 @@ config COMEDI_8255
 	  To compile this driver as a module, choose M here: the module will be
 	  called 8255.
 
-config COMEDI_DAS08
-	tristate "DAS-08 compatible support"
-	depends on COMEDI
-	select COMEDI_8255
-	default N
-	---help---
-	  Enable support for DAS08 and compatible ISA, PC/104 and PCI cards.
-
-	  Note that PCMCIA DAS08 cards are not directly supported by this
-	  driver, and need a separate driver as a wrapper.
-
-	  To compile this driver as a module, choose M here: the module will be
-	  called das08.
-
 config COMEDI_FC
 	tristate "Comedi shared functions for low-level driver support"
 	depends on COMEDI
-	default N
 	---help---
 	  Enable support for shared functions for low-level drivers.
 	  This module is not used directly by end-users. Rather, it
@@ -1382,3 +1314,22 @@ config COMEDI_FC
 
 	  To compile this driver as a module, choose M here: the module will be
 	  called comedi_fc.
+
+config COMEDI_AMPLC_DIO200
+	tristate
+	depends on COMEDI
+	select COMEDI_8255
+
+config COMEDI_AMPLC_PC236
+	tristate
+	depends on COMEDI
+	select COMEDI_8255
+
+config COMEDI_AMPLC_PC263
+	tristate
+	depends on COMEDI
+
+config COMEDI_DAS08
+	tristate
+	depends on COMEDI
+	select COMEDI_8255
diff --git a/drivers/staging/comedi/comedi.h b/drivers/staging/comedi/comedi.h
index 14ea35ac015..8ea55aef10a 100644
--- a/drivers/staging/comedi/comedi.h
+++ b/drivers/staging/comedi/comedi.h
@@ -465,7 +465,7 @@
 /* only relevant to kernel modules. */
 
 #define COMEDI_CB_EOS		1	/* end of scan */
-#define COMEDI_CB_EOA		2	/* end of acquisition */
+#define COMEDI_CB_EOA		2	/* end of acquisition/output */
 #define COMEDI_CB_BLOCK		4	/* data has arrived:
 					 * wakes up read() / write() */
 #define COMEDI_CB_EOBUF		8	/* DEPRECATED: end of buffer */
diff --git a/drivers/staging/comedi/comedi_fops.c b/drivers/staging/comedi/comedi_fops.c
index 9bcf87ae4c0..76776571ed9 100644
--- a/drivers/staging/comedi/comedi_fops.c
+++ b/drivers/staging/comedi/comedi_fops.c
@@ -58,14 +58,35 @@ MODULE_LICENSE("GPL");
 #ifdef CONFIG_COMEDI_DEBUG
 int comedi_debug;
 EXPORT_SYMBOL(comedi_debug);
-module_param(comedi_debug, int, 0644);
+module_param(comedi_debug, int, S_IRUGO | S_IWUSR);
+MODULE_PARM_DESC(comedi_debug,
+		 "enable comedi core and driver debugging if non-zero (default 0)"
+		);
 #endif
 
 bool comedi_autoconfig = 1;
-module_param(comedi_autoconfig, bool, 0444);
+module_param(comedi_autoconfig, bool, S_IRUGO);
+MODULE_PARM_DESC(comedi_autoconfig,
+		 "enable drivers to auto-configure comedi devices (default 1)");
 
 static int comedi_num_legacy_minors;
-module_param(comedi_num_legacy_minors, int, 0444);
+module_param(comedi_num_legacy_minors, int, S_IRUGO);
+MODULE_PARM_DESC(comedi_num_legacy_minors,
+		 "number of comedi minor devices to reserve for non-auto-configured devices (default 0)"
+		);
+
+unsigned int comedi_default_buf_size_kb = CONFIG_COMEDI_DEFAULT_BUF_SIZE_KB;
+module_param(comedi_default_buf_size_kb, uint, S_IRUGO | S_IWUSR);
+MODULE_PARM_DESC(comedi_default_buf_size_kb,
+		 "default asynchronous buffer size in KiB (default "
+		 __MODULE_STRING(CONFIG_COMEDI_DEFAULT_BUF_SIZE_KB) ")");
+
+unsigned int comedi_default_buf_maxsize_kb
+	= CONFIG_COMEDI_DEFAULT_BUF_MAXSIZE_KB;
+module_param(comedi_default_buf_maxsize_kb, uint, S_IRUGO | S_IWUSR);
+MODULE_PARM_DESC(comedi_default_buf_maxsize_kb,
+		 "default maximum size of asynchronous buffer in KiB (default "
+		 __MODULE_STRING(CONFIG_COMEDI_DEFAULT_BUF_MAXSIZE_KB) ")");
 
 static DEFINE_SPINLOCK(comedi_file_info_table_lock);
 static struct comedi_device_file_info
@@ -108,15 +129,283 @@ static int do_cancel(struct comedi_device *dev, struct comedi_subdevice *s);
 static int comedi_fasync(int fd, struct file *file, int on);
 
 static int is_device_busy(struct comedi_device *dev);
+
 static int resize_async_buffer(struct comedi_device *dev,
 			       struct comedi_subdevice *s,
-			       struct comedi_async *async, unsigned new_size);
+			       struct comedi_async *async, unsigned new_size)
+{
+	int retval;
+
+	if (new_size > async->max_bufsize)
+		return -EPERM;
+
+	if (s->busy) {
+		DPRINTK("subdevice is busy, cannot resize buffer\n");
+		return -EBUSY;
+	}
+	if (async->mmap_count) {
+		DPRINTK("subdevice is mmapped, cannot resize buffer\n");
+		return -EBUSY;
+	}
+
+	if (!async->prealloc_buf)
+		return -EINVAL;
+
+	/* make sure buffer is an integral number of pages
+	 * (we round up) */
+	new_size = (new_size + PAGE_SIZE - 1) & PAGE_MASK;
+
+	retval = comedi_buf_alloc(dev, s, new_size);
+	if (retval < 0)
+		return retval;
+
+	if (s->buf_change) {
+		retval = s->buf_change(dev, s, new_size);
+		if (retval < 0)
+			return retval;
+	}
+
+	DPRINTK("comedi%i subd %d buffer resized to %i bytes\n",
+		dev->minor, (int)(s - dev->subdevices), async->prealloc_bufsz);
+	return 0;
+}
+
+/* sysfs attribute files */
+
+static const unsigned bytes_per_kibi = 1024;
+
+static ssize_t show_max_read_buffer_kb(struct device *dev,
+				       struct device_attribute *attr, char *buf)
+{
+	ssize_t retval;
+	struct comedi_device_file_info *info = dev_get_drvdata(dev);
+	unsigned max_buffer_size_kb = 0;
+	struct comedi_subdevice *const read_subdevice =
+	    comedi_get_read_subdevice(info);
+
+	mutex_lock(&info->device->mutex);
+	if (read_subdevice &&
+	    (read_subdevice->subdev_flags & SDF_CMD_READ) &&
+	    read_subdevice->async) {
+		max_buffer_size_kb = read_subdevice->async->max_bufsize /
+		    bytes_per_kibi;
+	}
+	retval = snprintf(buf, PAGE_SIZE, "%i\n", max_buffer_size_kb);
+	mutex_unlock(&info->device->mutex);
+
+	return retval;
+}
+
+static ssize_t store_max_read_buffer_kb(struct device *dev,
+					struct device_attribute *attr,
+					const char *buf, size_t count)
+{
+	struct comedi_device_file_info *info = dev_get_drvdata(dev);
+	unsigned int new_max_size_kb;
+	unsigned int new_max_size;
+	int ret;
+	struct comedi_subdevice *const read_subdevice =
+	    comedi_get_read_subdevice(info);
+
+	ret = kstrtouint(buf, 10, &new_max_size_kb);
+	if (ret)
+		return ret;
+	if (new_max_size_kb > (UINT_MAX / bytes_per_kibi))
+		return -EINVAL;
+	new_max_size = new_max_size_kb * bytes_per_kibi;
+
+	mutex_lock(&info->device->mutex);
+	if (read_subdevice == NULL ||
+	    (read_subdevice->subdev_flags & SDF_CMD_READ) == 0 ||
+	    read_subdevice->async == NULL) {
+		mutex_unlock(&info->device->mutex);
+		return -EINVAL;
+	}
+	read_subdevice->async->max_bufsize = new_max_size;
+	mutex_unlock(&info->device->mutex);
+
+	return count;
+}
+
+static ssize_t show_read_buffer_kb(struct device *dev,
+				   struct device_attribute *attr, char *buf)
+{
+	ssize_t retval;
+	struct comedi_device_file_info *info = dev_get_drvdata(dev);
+	unsigned buffer_size_kb = 0;
+	struct comedi_subdevice *const read_subdevice =
+	    comedi_get_read_subdevice(info);
+
+	mutex_lock(&info->device->mutex);
+	if (read_subdevice &&
+	    (read_subdevice->subdev_flags & SDF_CMD_READ) &&
+	    read_subdevice->async) {
+		buffer_size_kb = read_subdevice->async->prealloc_bufsz /
+		    bytes_per_kibi;
+	}
+	retval = snprintf(buf, PAGE_SIZE, "%i\n", buffer_size_kb);
+	mutex_unlock(&info->device->mutex);
+
+	return retval;
+}
+
+static ssize_t store_read_buffer_kb(struct device *dev,
+				    struct device_attribute *attr,
+				    const char *buf, size_t count)
+{
+	struct comedi_device_file_info *info = dev_get_drvdata(dev);
+	unsigned int new_size_kb;
+	unsigned int new_size;
+	int retval;
+	int ret;
+	struct comedi_subdevice *const read_subdevice =
+	    comedi_get_read_subdevice(info);
+
+	ret = kstrtouint(buf, 10, &new_size_kb);
+	if (ret)
+		return ret;
+	if (new_size_kb > (UINT_MAX / bytes_per_kibi))
+		return -EINVAL;
+	new_size = new_size_kb * bytes_per_kibi;
+
+	mutex_lock(&info->device->mutex);
+	if (read_subdevice == NULL ||
+	    (read_subdevice->subdev_flags & SDF_CMD_READ) == 0 ||
+	    read_subdevice->async == NULL) {
+		mutex_unlock(&info->device->mutex);
+		return -EINVAL;
+	}
+	retval = resize_async_buffer(info->device, read_subdevice,
+				     read_subdevice->async, new_size);
+	mutex_unlock(&info->device->mutex);
+
+	if (retval < 0)
+		return retval;
+	return count;
+}
+
+static ssize_t show_max_write_buffer_kb(struct device *dev,
+					struct device_attribute *attr,
+					char *buf)
+{
+	ssize_t retval;
+	struct comedi_device_file_info *info = dev_get_drvdata(dev);
+	unsigned max_buffer_size_kb = 0;
+	struct comedi_subdevice *const write_subdevice =
+	    comedi_get_write_subdevice(info);
+
+	mutex_lock(&info->device->mutex);
+	if (write_subdevice &&
+	    (write_subdevice->subdev_flags & SDF_CMD_WRITE) &&
+	    write_subdevice->async) {
+		max_buffer_size_kb = write_subdevice->async->max_bufsize /
+		    bytes_per_kibi;
+	}
+	retval = snprintf(buf, PAGE_SIZE, "%i\n", max_buffer_size_kb);
+	mutex_unlock(&info->device->mutex);
+
+	return retval;
+}
+
+static ssize_t store_max_write_buffer_kb(struct device *dev,
+					 struct device_attribute *attr,
+					 const char *buf, size_t count)
+{
+	struct comedi_device_file_info *info = dev_get_drvdata(dev);
+	unsigned int new_max_size_kb;
+	unsigned int new_max_size;
+	int ret;
+	struct comedi_subdevice *const write_subdevice =
+	    comedi_get_write_subdevice(info);
+
+	ret = kstrtouint(buf, 10, &new_max_size_kb);
+	if (ret)
+		return ret;
+	if (new_max_size_kb > (UINT_MAX / bytes_per_kibi))
+		return -EINVAL;
+	new_max_size = new_max_size_kb * bytes_per_kibi;
+
+	mutex_lock(&info->device->mutex);
+	if (write_subdevice == NULL ||
+	    (write_subdevice->subdev_flags & SDF_CMD_WRITE) == 0 ||
+	    write_subdevice->async == NULL) {
+		mutex_unlock(&info->device->mutex);
+		return -EINVAL;
+	}
+	write_subdevice->async->max_bufsize = new_max_size;
+	mutex_unlock(&info->device->mutex);
+
+	return count;
+}
+
+static ssize_t show_write_buffer_kb(struct device *dev,
+				    struct device_attribute *attr, char *buf)
+{
+	ssize_t retval;
+	struct comedi_device_file_info *info = dev_get_drvdata(dev);
+	unsigned buffer_size_kb = 0;
+	struct comedi_subdevice *const write_subdevice =
+	    comedi_get_write_subdevice(info);
+
+	mutex_lock(&info->device->mutex);
+	if (write_subdevice &&
+	    (write_subdevice->subdev_flags & SDF_CMD_WRITE) &&
+	    write_subdevice->async) {
+		buffer_size_kb = write_subdevice->async->prealloc_bufsz /
+		    bytes_per_kibi;
+	}
+	retval = snprintf(buf, PAGE_SIZE, "%i\n", buffer_size_kb);
+	mutex_unlock(&info->device->mutex);
 
-/* declarations for sysfs attribute files */
-static struct device_attribute dev_attr_max_read_buffer_kb;
-static struct device_attribute dev_attr_read_buffer_kb;
-static struct device_attribute dev_attr_max_write_buffer_kb;
-static struct device_attribute dev_attr_write_buffer_kb;
+	return retval;
+}
+
+static ssize_t store_write_buffer_kb(struct device *dev,
+				     struct device_attribute *attr,
+				     const char *buf, size_t count)
+{
+	struct comedi_device_file_info *info = dev_get_drvdata(dev);
+	unsigned int new_size_kb;
+	unsigned int new_size;
+	int retval;
+	int ret;
+	struct comedi_subdevice *const write_subdevice =
+	    comedi_get_write_subdevice(info);
+
+	ret = kstrtouint(buf, 10, &new_size_kb);
+	if (ret)
+		return ret;
+	if (new_size_kb > (UINT_MAX / bytes_per_kibi))
+		return -EINVAL;
+	new_size = ((uint64_t) new_size_kb) * bytes_per_kibi;
+
+	mutex_lock(&info->device->mutex);
+	if (write_subdevice == NULL ||
+	    (write_subdevice->subdev_flags & SDF_CMD_WRITE) == 0 ||
+	    write_subdevice->async == NULL) {
+		mutex_unlock(&info->device->mutex);
+		return -EINVAL;
+	}
+	retval = resize_async_buffer(info->device, write_subdevice,
+				     write_subdevice->async, new_size);
+	mutex_unlock(&info->device->mutex);
+
+	if (retval < 0)
+		return retval;
+	return count;
+}
+
+static struct device_attribute comedi_dev_attrs[] = {
+	__ATTR(max_read_buffer_kb, S_IRUGO | S_IWUSR,
+		show_max_read_buffer_kb, store_max_read_buffer_kb),
+	__ATTR(read_buffer_kb, S_IRUGO | S_IWUSR | S_IWGRP,
+		show_read_buffer_kb, store_read_buffer_kb),
+	__ATTR(max_write_buffer_kb, S_IRUGO | S_IWUSR,
+		show_max_write_buffer_kb, store_max_write_buffer_kb),
+	__ATTR(write_buffer_kb, S_IRUGO | S_IWUSR | S_IWGRP,
+		show_write_buffer_kb, store_write_buffer_kb),
+	__ATTR_NULL
+};
 
 static long comedi_unlocked_ioctl(struct file *file, unsigned int cmd,
 				  unsigned long arg)
@@ -280,7 +569,7 @@ static int do_devconfig_ioctl(struct comedi_device *dev,
 	if (ret == 0) {
 		if (!try_module_get(dev->driver->module)) {
 			comedi_device_detach(dev);
-			return -ENOSYS;
+			ret = -ENOSYS;
 		}
 	}
 
@@ -1545,7 +1834,7 @@ done:
 	return retval;
 }
 
-static unsigned int comedi_poll(struct file *file, poll_table * wait)
+static unsigned int comedi_poll(struct file *file, poll_table *wait)
 {
 	unsigned int mask = 0;
 	const unsigned minor = iminor(file->f_dentry->d_inode);
@@ -2054,6 +2343,8 @@ static int __init comedi_init(void)
 		return PTR_ERR(comedi_class);
 	}
 
+	comedi_class->dev_attrs = comedi_dev_attrs;
+
 	/* XXX requires /proc interface */
 	comedi_proc_init();
 
@@ -2192,11 +2483,9 @@ static void comedi_device_cleanup(struct comedi_device *dev)
 
 int comedi_alloc_board_minor(struct device *hardware_device)
 {
-	unsigned long flags;
 	struct comedi_device_file_info *info;
 	struct device *csdev;
 	unsigned i;
-	int retval;
 
 	info = kzalloc(sizeof(struct comedi_device_file_info), GFP_KERNEL);
 	if (info == NULL)
@@ -2206,15 +2495,16 @@ int comedi_alloc_board_minor(struct device *hardware_device)
 		kfree(info);
 		return -ENOMEM;
 	}
+	info->hardware_device = hardware_device;
 	comedi_device_init(info->device);
-	spin_lock_irqsave(&comedi_file_info_table_lock, flags);
+	spin_lock(&comedi_file_info_table_lock);
 	for (i = 0; i < COMEDI_NUM_BOARD_MINORS; ++i) {
 		if (comedi_file_info_table[i] == NULL) {
 			comedi_file_info_table[i] = info;
 			break;
 		}
 	}
-	spin_unlock_irqrestore(&comedi_file_info_table_lock, flags);
+	spin_unlock(&comedi_file_info_table_lock);
 	if (i == COMEDI_NUM_BOARD_MINORS) {
 		comedi_device_cleanup(info->device);
 		kfree(info->device);
@@ -2230,55 +2520,19 @@ int comedi_alloc_board_minor(struct device *hardware_device)
 	if (!IS_ERR(csdev))
 		info->device->class_dev = csdev;
 	dev_set_drvdata(csdev, info);
-	retval = device_create_file(csdev, &dev_attr_max_read_buffer_kb);
-	if (retval) {
-		printk(KERN_ERR
-		       "comedi: "
-		       "failed to create sysfs attribute file \"%s\".\n",
-		       dev_attr_max_read_buffer_kb.attr.name);
-		comedi_free_board_minor(i);
-		return retval;
-	}
-	retval = device_create_file(csdev, &dev_attr_read_buffer_kb);
-	if (retval) {
-		printk(KERN_ERR
-		       "comedi: "
-		       "failed to create sysfs attribute file \"%s\".\n",
-		       dev_attr_read_buffer_kb.attr.name);
-		comedi_free_board_minor(i);
-		return retval;
-	}
-	retval = device_create_file(csdev, &dev_attr_max_write_buffer_kb);
-	if (retval) {
-		printk(KERN_ERR
-		       "comedi: "
-		       "failed to create sysfs attribute file \"%s\".\n",
-		       dev_attr_max_write_buffer_kb.attr.name);
-		comedi_free_board_minor(i);
-		return retval;
-	}
-	retval = device_create_file(csdev, &dev_attr_write_buffer_kb);
-	if (retval) {
-		printk(KERN_ERR
-		       "comedi: "
-		       "failed to create sysfs attribute file \"%s\".\n",
-		       dev_attr_write_buffer_kb.attr.name);
-		comedi_free_board_minor(i);
-		return retval;
-	}
+
 	return i;
 }
 
 void comedi_free_board_minor(unsigned minor)
 {
-	unsigned long flags;
 	struct comedi_device_file_info *info;
 
 	BUG_ON(minor >= COMEDI_NUM_BOARD_MINORS);
-	spin_lock_irqsave(&comedi_file_info_table_lock, flags);
+	spin_lock(&comedi_file_info_table_lock);
 	info = comedi_file_info_table[minor];
 	comedi_file_info_table[minor] = NULL;
-	spin_unlock_irqrestore(&comedi_file_info_table_lock, flags);
+	spin_unlock(&comedi_file_info_table_lock);
 
 	if (info) {
 		struct comedi_device *dev = info->device;
@@ -2294,14 +2548,29 @@ void comedi_free_board_minor(unsigned minor)
 	}
 }
 
+int comedi_find_board_minor(struct device *hardware_device)
+{
+	int minor;
+	struct comedi_device_file_info *info;
+
+	for (minor = 0; minor < COMEDI_NUM_BOARD_MINORS; minor++) {
+		spin_lock(&comedi_file_info_table_lock);
+		info = comedi_file_info_table[minor];
+		if (info && info->hardware_device == hardware_device) {
+			spin_unlock(&comedi_file_info_table_lock);
+			return minor;
+		}
+		spin_unlock(&comedi_file_info_table_lock);
+	}
+	return -ENODEV;
+}
+
 int comedi_alloc_subdevice_minor(struct comedi_device *dev,
 				 struct comedi_subdevice *s)
 {
-	unsigned long flags;
 	struct comedi_device_file_info *info;
 	struct device *csdev;
 	unsigned i;
-	int retval;
 
 	info = kmalloc(sizeof(struct comedi_device_file_info), GFP_KERNEL);
 	if (info == NULL)
@@ -2309,14 +2578,14 @@ int comedi_alloc_subdevice_minor(struct comedi_device *dev,
 	info->device = dev;
 	info->read_subdevice = s;
 	info->write_subdevice = s;
-	spin_lock_irqsave(&comedi_file_info_table_lock, flags);
+	spin_lock(&comedi_file_info_table_lock);
 	for (i = COMEDI_FIRST_SUBDEVICE_MINOR; i < COMEDI_NUM_MINORS; ++i) {
 		if (comedi_file_info_table[i] == NULL) {
 			comedi_file_info_table[i] = info;
 			break;
 		}
 	}
-	spin_unlock_irqrestore(&comedi_file_info_table_lock, flags);
+	spin_unlock(&comedi_file_info_table_lock);
 	if (i == COMEDI_NUM_MINORS) {
 		kfree(info);
 		printk(KERN_ERR
@@ -2331,48 +2600,12 @@ int comedi_alloc_subdevice_minor(struct comedi_device *dev,
 	if (!IS_ERR(csdev))
 		s->class_dev = csdev;
 	dev_set_drvdata(csdev, info);
-	retval = device_create_file(csdev, &dev_attr_max_read_buffer_kb);
-	if (retval) {
-		printk(KERN_ERR
-		       "comedi: "
-		       "failed to create sysfs attribute file \"%s\".\n",
-		       dev_attr_max_read_buffer_kb.attr.name);
-		comedi_free_subdevice_minor(s);
-		return retval;
-	}
-	retval = device_create_file(csdev, &dev_attr_read_buffer_kb);
-	if (retval) {
-		printk(KERN_ERR
-		       "comedi: "
-		       "failed to create sysfs attribute file \"%s\".\n",
-		       dev_attr_read_buffer_kb.attr.name);
-		comedi_free_subdevice_minor(s);
-		return retval;
-	}
-	retval = device_create_file(csdev, &dev_attr_max_write_buffer_kb);
-	if (retval) {
-		printk(KERN_ERR
-		       "comedi: "
-		       "failed to create sysfs attribute file \"%s\".\n",
-		       dev_attr_max_write_buffer_kb.attr.name);
-		comedi_free_subdevice_minor(s);
-		return retval;
-	}
-	retval = device_create_file(csdev, &dev_attr_write_buffer_kb);
-	if (retval) {
-		printk(KERN_ERR
-		       "comedi: "
-		       "failed to create sysfs attribute file \"%s\".\n",
-		       dev_attr_write_buffer_kb.attr.name);
-		comedi_free_subdevice_minor(s);
-		return retval;
-	}
+
 	return i;
 }
 
 void comedi_free_subdevice_minor(struct comedi_subdevice *s)
 {
-	unsigned long flags;
 	struct comedi_device_file_info *info;
 
 	if (s == NULL)
@@ -2383,10 +2616,10 @@ void comedi_free_subdevice_minor(struct comedi_subdevice *s)
 	BUG_ON(s->minor >= COMEDI_NUM_MINORS);
 	BUG_ON(s->minor < COMEDI_FIRST_SUBDEVICE_MINOR);
 
-	spin_lock_irqsave(&comedi_file_info_table_lock, flags);
+	spin_lock(&comedi_file_info_table_lock);
 	info = comedi_file_info_table[s->minor];
 	comedi_file_info_table[s->minor] = NULL;
-	spin_unlock_irqrestore(&comedi_file_info_table_lock, flags);
+	spin_unlock(&comedi_file_info_table_lock);
 
 	if (s->class_dev) {
 		device_destroy(comedi_class, MKDEV(COMEDI_MAJOR, s->minor));
@@ -2397,310 +2630,12 @@ void comedi_free_subdevice_minor(struct comedi_subdevice *s)
 
 struct comedi_device_file_info *comedi_get_device_file_info(unsigned minor)
 {
-	unsigned long flags;
 	struct comedi_device_file_info *info;
 
 	BUG_ON(minor >= COMEDI_NUM_MINORS);
-	spin_lock_irqsave(&comedi_file_info_table_lock, flags);
+	spin_lock(&comedi_file_info_table_lock);
 	info = comedi_file_info_table[minor];
-	spin_unlock_irqrestore(&comedi_file_info_table_lock, flags);
+	spin_unlock(&comedi_file_info_table_lock);
 	return info;
 }
 EXPORT_SYMBOL_GPL(comedi_get_device_file_info);
-
-static int resize_async_buffer(struct comedi_device *dev,
-			       struct comedi_subdevice *s,
-			       struct comedi_async *async, unsigned new_size)
-{
-	int retval;
-
-	if (new_size > async->max_bufsize)
-		return -EPERM;
-
-	if (s->busy) {
-		DPRINTK("subdevice is busy, cannot resize buffer\n");
-		return -EBUSY;
-	}
-	if (async->mmap_count) {
-		DPRINTK("subdevice is mmapped, cannot resize buffer\n");
-		return -EBUSY;
-	}
-
-	if (!async->prealloc_buf)
-		return -EINVAL;
-
-	/* make sure buffer is an integral number of pages
-	 * (we round up) */
-	new_size = (new_size + PAGE_SIZE - 1) & PAGE_MASK;
-
-	retval = comedi_buf_alloc(dev, s, new_size);
-	if (retval < 0)
-		return retval;
-
-	if (s->buf_change) {
-		retval = s->buf_change(dev, s, new_size);
-		if (retval < 0)
-			return retval;
-	}
-
-	DPRINTK("comedi%i subd %d buffer resized to %i bytes\n",
-		dev->minor, (int)(s - dev->subdevices), async->prealloc_bufsz);
-	return 0;
-}
-
-/* sysfs attribute files */
-
-static const unsigned bytes_per_kibi = 1024;
-
-static ssize_t show_max_read_buffer_kb(struct device *dev,
-				       struct device_attribute *attr, char *buf)
-{
-	ssize_t retval;
-	struct comedi_device_file_info *info = dev_get_drvdata(dev);
-	unsigned max_buffer_size_kb = 0;
-	struct comedi_subdevice *const read_subdevice =
-	    comedi_get_read_subdevice(info);
-
-	mutex_lock(&info->device->mutex);
-	if (read_subdevice &&
-	    (read_subdevice->subdev_flags & SDF_CMD_READ) &&
-	    read_subdevice->async) {
-		max_buffer_size_kb = read_subdevice->async->max_bufsize /
-		    bytes_per_kibi;
-	}
-	retval = snprintf(buf, PAGE_SIZE, "%i\n", max_buffer_size_kb);
-	mutex_unlock(&info->device->mutex);
-
-	return retval;
-}
-
-static ssize_t store_max_read_buffer_kb(struct device *dev,
-					struct device_attribute *attr,
-					const char *buf, size_t count)
-{
-	struct comedi_device_file_info *info = dev_get_drvdata(dev);
-	unsigned int new_max_size_kb;
-	unsigned int new_max_size;
-	int ret;
-	struct comedi_subdevice *const read_subdevice =
-	    comedi_get_read_subdevice(info);
-
-	ret = kstrtouint(buf, 10, &new_max_size_kb);
-	if (ret)
-		return ret;
-	if (new_max_size_kb > (UINT_MAX / bytes_per_kibi))
-		return -EINVAL;
-	new_max_size = new_max_size_kb * bytes_per_kibi;
-
-	mutex_lock(&info->device->mutex);
-	if (read_subdevice == NULL ||
-	    (read_subdevice->subdev_flags & SDF_CMD_READ) == 0 ||
-	    read_subdevice->async == NULL) {
-		mutex_unlock(&info->device->mutex);
-		return -EINVAL;
-	}
-	read_subdevice->async->max_bufsize = new_max_size;
-	mutex_unlock(&info->device->mutex);
-
-	return count;
-}
-
-static struct device_attribute dev_attr_max_read_buffer_kb = {
-	.attr = {
-		 .name = "max_read_buffer_kb",
-		 .mode = S_IRUGO | S_IWUSR},
-	.show = &show_max_read_buffer_kb,
-	.store = &store_max_read_buffer_kb
-};
-
-static ssize_t show_read_buffer_kb(struct device *dev,
-				   struct device_attribute *attr, char *buf)
-{
-	ssize_t retval;
-	struct comedi_device_file_info *info = dev_get_drvdata(dev);
-	unsigned buffer_size_kb = 0;
-	struct comedi_subdevice *const read_subdevice =
-	    comedi_get_read_subdevice(info);
-
-	mutex_lock(&info->device->mutex);
-	if (read_subdevice &&
-	    (read_subdevice->subdev_flags & SDF_CMD_READ) &&
-	    read_subdevice->async) {
-		buffer_size_kb = read_subdevice->async->prealloc_bufsz /
-		    bytes_per_kibi;
-	}
-	retval = snprintf(buf, PAGE_SIZE, "%i\n", buffer_size_kb);
-	mutex_unlock(&info->device->mutex);
-
-	return retval;
-}
-
-static ssize_t store_read_buffer_kb(struct device *dev,
-				    struct device_attribute *attr,
-				    const char *buf, size_t count)
-{
-	struct comedi_device_file_info *info = dev_get_drvdata(dev);
-	unsigned int new_size_kb;
-	unsigned int new_size;
-	int retval;
-	int ret;
-	struct comedi_subdevice *const read_subdevice =
-	    comedi_get_read_subdevice(info);
-
-	ret = kstrtouint(buf, 10, &new_size_kb);
-	if (ret)
-		return ret;
-	if (new_size_kb > (UINT_MAX / bytes_per_kibi))
-		return -EINVAL;
-	new_size = new_size_kb * bytes_per_kibi;
-
-	mutex_lock(&info->device->mutex);
-	if (read_subdevice == NULL ||
-	    (read_subdevice->subdev_flags & SDF_CMD_READ) == 0 ||
-	    read_subdevice->async == NULL) {
-		mutex_unlock(&info->device->mutex);
-		return -EINVAL;
-	}
-	retval = resize_async_buffer(info->device, read_subdevice,
-				     read_subdevice->async, new_size);
-	mutex_unlock(&info->device->mutex);
-
-	if (retval < 0)
-		return retval;
-	return count;
-}
-
-static struct device_attribute dev_attr_read_buffer_kb = {
-	.attr = {
-		 .name = "read_buffer_kb",
-		 .mode = S_IRUGO | S_IWUSR | S_IWGRP},
-	.show = &show_read_buffer_kb,
-	.store = &store_read_buffer_kb
-};
-
-static ssize_t show_max_write_buffer_kb(struct device *dev,
-					struct device_attribute *attr,
-					char *buf)
-{
-	ssize_t retval;
-	struct comedi_device_file_info *info = dev_get_drvdata(dev);
-	unsigned max_buffer_size_kb = 0;
-	struct comedi_subdevice *const write_subdevice =
-	    comedi_get_write_subdevice(info);
-
-	mutex_lock(&info->device->mutex);
-	if (write_subdevice &&
-	    (write_subdevice->subdev_flags & SDF_CMD_WRITE) &&
-	    write_subdevice->async) {
-		max_buffer_size_kb = write_subdevice->async->max_bufsize /
-		    bytes_per_kibi;
-	}
-	retval = snprintf(buf, PAGE_SIZE, "%i\n", max_buffer_size_kb);
-	mutex_unlock(&info->device->mutex);
-
-	return retval;
-}
-
-static ssize_t store_max_write_buffer_kb(struct device *dev,
-					 struct device_attribute *attr,
-					 const char *buf, size_t count)
-{
-	struct comedi_device_file_info *info = dev_get_drvdata(dev);
-	unsigned int new_max_size_kb;
-	unsigned int new_max_size;
-	int ret;
-	struct comedi_subdevice *const write_subdevice =
-	    comedi_get_write_subdevice(info);
-
-	ret = kstrtouint(buf, 10, &new_max_size_kb);
-	if (ret)
-		return ret;
-	if (new_max_size_kb > (UINT_MAX / bytes_per_kibi))
-		return -EINVAL;
-	new_max_size = new_max_size_kb * bytes_per_kibi;
-
-	mutex_lock(&info->device->mutex);
-	if (write_subdevice == NULL ||
-	    (write_subdevice->subdev_flags & SDF_CMD_WRITE) == 0 ||
-	    write_subdevice->async == NULL) {
-		mutex_unlock(&info->device->mutex);
-		return -EINVAL;
-	}
-	write_subdevice->async->max_bufsize = new_max_size;
-	mutex_unlock(&info->device->mutex);
-
-	return count;
-}
-
-static struct device_attribute dev_attr_max_write_buffer_kb = {
-	.attr = {
-		 .name = "max_write_buffer_kb",
-		 .mode = S_IRUGO | S_IWUSR},
-	.show = &show_max_write_buffer_kb,
-	.store = &store_max_write_buffer_kb
-};
-
-static ssize_t show_write_buffer_kb(struct device *dev,
-				    struct device_attribute *attr, char *buf)
-{
-	ssize_t retval;
-	struct comedi_device_file_info *info = dev_get_drvdata(dev);
-	unsigned buffer_size_kb = 0;
-	struct comedi_subdevice *const write_subdevice =
-	    comedi_get_write_subdevice(info);
-
-	mutex_lock(&info->device->mutex);
-	if (write_subdevice &&
-	    (write_subdevice->subdev_flags & SDF_CMD_WRITE) &&
-	    write_subdevice->async) {
-		buffer_size_kb = write_subdevice->async->prealloc_bufsz /
-		    bytes_per_kibi;
-	}
-	retval = snprintf(buf, PAGE_SIZE, "%i\n", buffer_size_kb);
-	mutex_unlock(&info->device->mutex);
-
-	return retval;
-}
-
-static ssize_t store_write_buffer_kb(struct device *dev,
-				     struct device_attribute *attr,
-				     const char *buf, size_t count)
-{
-	struct comedi_device_file_info *info = dev_get_drvdata(dev);
-	unsigned int new_size_kb;
-	unsigned int new_size;
-	int retval;
-	int ret;
-	struct comedi_subdevice *const write_subdevice =
-	    comedi_get_write_subdevice(info);
-
-	ret = kstrtouint(buf, 10, &new_size_kb);
-	if (ret)
-		return ret;
-	if (new_size_kb > (UINT_MAX / bytes_per_kibi))
-		return -EINVAL;
-	new_size = ((uint64_t) new_size_kb) * bytes_per_kibi;
-
-	mutex_lock(&info->device->mutex);
-	if (write_subdevice == NULL ||
-	    (write_subdevice->subdev_flags & SDF_CMD_WRITE) == 0 ||
-	    write_subdevice->async == NULL) {
-		mutex_unlock(&info->device->mutex);
-		return -EINVAL;
-	}
-	retval = resize_async_buffer(info->device, write_subdevice,
-				     write_subdevice->async, new_size);
-	mutex_unlock(&info->device->mutex);
-
-	if (retval < 0)
-		return retval;
-	return count;
-}
-
-static struct device_attribute dev_attr_write_buffer_kb = {
-	.attr = {
-		 .name = "write_buffer_kb",
-		 .mode = S_IRUGO | S_IWUSR | S_IWGRP},
-	.show = &show_write_buffer_kb,
-	.store = &store_write_buffer_kb
-};
diff --git a/drivers/staging/comedi/comedidev.h b/drivers/staging/comedi/comedidev.h
index 7a0d4bcbc35..134be93eaa6 100644
--- a/drivers/staging/comedi/comedidev.h
+++ b/drivers/staging/comedi/comedidev.h
@@ -180,13 +180,18 @@ struct comedi_async {
 			unsigned int x);
 };
 
+struct pci_dev;
+struct usb_interface;
+
 struct comedi_driver {
 	struct comedi_driver *next;
 
 	const char *driver_name;
 	struct module *module;
 	int (*attach) (struct comedi_device *, struct comedi_devconfig *);
-	int (*detach) (struct comedi_device *);
+	void (*detach) (struct comedi_device *);
+	int (*attach_pci) (struct comedi_device *, struct pci_dev *);
+	int (*attach_usb) (struct comedi_device *, struct usb_interface *);
 
 	/* number of elements in board_name and board_id arrays */
 	unsigned int num_names;
@@ -230,10 +235,16 @@ struct comedi_device {
 	void (*close) (struct comedi_device *dev);
 };
 
+static inline const void *comedi_board(struct comedi_device *dev)
+{
+	return dev->board_ptr;
+}
+
 struct comedi_device_file_info {
 	struct comedi_device *device;
 	struct comedi_subdevice *read_subdevice;
 	struct comedi_subdevice *write_subdevice;
+	struct device *hardware_device;
 };
 
 #ifdef CONFIG_COMEDI_DEBUG
@@ -287,6 +298,56 @@ int comedi_device_attach(struct comedi_device *dev,
 int comedi_driver_register(struct comedi_driver *);
 int comedi_driver_unregister(struct comedi_driver *);
 
+/**
+ * module_comedi_driver() - Helper macro for registering a comedi driver
+ * @__comedi_driver: comedi_driver struct
+ *
+ * Helper macro for comedi drivers which do not do anything special in module
+ * init/exit. This eliminates a lot of boilerplate. Each module may only use
+ * this macro once, and calling it replaces module_init() and module_exit().
+ */
+#define module_comedi_driver(__comedi_driver) \
+	module_driver(__comedi_driver, comedi_driver_register, \
+			comedi_driver_unregister)
+
+struct pci_driver;
+
+int comedi_pci_driver_register(struct comedi_driver *, struct pci_driver *);
+void comedi_pci_driver_unregister(struct comedi_driver *, struct pci_driver *);
+
+/**
+ * module_comedi_pci_driver() - Helper macro for registering a comedi PCI driver
+ * @__comedi_driver: comedi_driver struct
+ * @__pci_driver: pci_driver struct
+ *
+ * Helper macro for comedi PCI drivers which do not do anything special
+ * in module init/exit. This eliminates a lot of boilerplate. Each
+ * module may only use this macro once, and calling it replaces
+ * module_init() and module_exit()
+ */
+#define module_comedi_pci_driver(__comedi_driver, __pci_driver) \
+	module_driver(__comedi_driver, comedi_pci_driver_register, \
+			comedi_pci_driver_unregister, &(__pci_driver))
+
+struct usb_driver;
+
+int comedi_usb_driver_register(struct comedi_driver *, struct usb_driver *);
+void comedi_usb_driver_unregister(struct comedi_driver *, struct usb_driver *);
+
+/**
+ * module_comedi_usb_driver() - Helper macro for registering a comedi USB driver
+ * @__comedi_driver: comedi_driver struct
+ * @__usb_driver: usb_driver struct
+ *
+ * Helper macro for comedi USB drivers which do not do anything special
+ * in module init/exit. This eliminates a lot of boilerplate. Each
+ * module may only use this macro once, and calling it replaces
+ * module_init() and module_exit()
+ */
+#define module_comedi_usb_driver(__comedi_driver, __usb_driver) \
+	module_driver(__comedi_driver, comedi_usb_driver_register, \
+			comedi_usb_driver_unregister, &(__usb_driver))
+
 void init_polling(void);
 void cleanup_polling(void);
 void start_polling(struct comedi_device *);
@@ -456,11 +517,12 @@ static inline void *comedi_aux_data(int options[], int n)
 int comedi_alloc_subdevice_minor(struct comedi_device *dev,
 				 struct comedi_subdevice *s);
 void comedi_free_subdevice_minor(struct comedi_subdevice *s);
-int comedi_pci_auto_config(struct pci_dev *pcidev, const char *board_name);
+int comedi_pci_auto_config(struct pci_dev *pcidev,
+			   struct comedi_driver *driver);
 void comedi_pci_auto_unconfig(struct pci_dev *pcidev);
-struct usb_device;		/* forward declaration */
-int comedi_usb_auto_config(struct usb_device *usbdev, const char *board_name);
-void comedi_usb_auto_unconfig(struct usb_device *usbdev);
+int comedi_usb_auto_config(struct usb_interface *intf,
+			   struct comedi_driver *driver);
+void comedi_usb_auto_unconfig(struct usb_interface *intf);
 
 #ifdef CONFIG_COMEDI_PCI_DRIVERS
 #define CONFIG_COMEDI_PCI
diff --git a/drivers/staging/comedi/drivers.c b/drivers/staging/comedi/drivers.c
index bf185e2807d..1c3d6386ea3 100644
--- a/drivers/staging/comedi/drivers.c
+++ b/drivers/staging/comedi/drivers.c
@@ -106,6 +106,26 @@ void comedi_device_detach(struct comedi_device *dev)
 	__comedi_device_detach(dev);
 }
 
+/* do a little post-config cleanup */
+/* called with module refcount incremented, decrements it */
+static int comedi_device_postconfig(struct comedi_device *dev)
+{
+	int ret = postconfig(dev);
+	module_put(dev->driver->module);
+	if (ret < 0) {
+		__comedi_device_detach(dev);
+		return ret;
+	}
+	if (!dev->board_name) {
+		printk(KERN_WARNING "BUG: dev->board_name=<%p>\n",
+		       dev->board_name);
+		dev->board_name = "BUG";
+	}
+	smp_wmb();
+	dev->attached = 1;
+	return 0;
+}
+
 int comedi_device_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 {
 	struct comedi_driver *driv;
@@ -121,59 +141,36 @@ int comedi_device_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 		}
 		if (driv->num_names) {
 			dev->board_ptr = comedi_recognize(driv, it->board_name);
-			if (dev->board_ptr == NULL) {
-				module_put(driv->module);
-				continue;
-			}
-		} else {
-			if (strcmp(driv->driver_name, it->board_name)) {
-				module_put(driv->module);
+			if (dev->board_ptr)
+				break;
+		} else if (strcmp(driv->driver_name, it->board_name))
+			break;
+		module_put(driv->module);
+	}
+	if (driv == NULL) {
+		/*  recognize has failed if we get here */
+		/*  report valid board names before returning error */
+		for (driv = comedi_drivers; driv; driv = driv->next) {
+			if (!try_module_get(driv->module)) {
+				printk(KERN_INFO
+				       "comedi: failed to increment module count\n");
 				continue;
 			}
+			comedi_report_boards(driv);
+			module_put(driv->module);
 		}
-		/* initialize dev->driver here so
-		 * comedi_error() can be called from attach */
-		dev->driver = driv;
-		ret = driv->attach(dev, it);
-		if (ret < 0) {
-			module_put(dev->driver->module);
-			__comedi_device_detach(dev);
-			return ret;
-		}
-		goto attached;
+		return -EIO;
 	}
-
-	/*  recognize has failed if we get here */
-	/*  report valid board names before returning error */
-	for (driv = comedi_drivers; driv; driv = driv->next) {
-		if (!try_module_get(driv->module)) {
-			printk(KERN_INFO
-			       "comedi: failed to increment module count\n");
-			continue;
-		}
-		comedi_report_boards(driv);
-		module_put(driv->module);
-	}
-	return -EIO;
-
-attached:
-	/* do a little post-config cleanup */
-	ret = postconfig(dev);
-	module_put(dev->driver->module);
+	/* initialize dev->driver here so
+	 * comedi_error() can be called from attach */
+	dev->driver = driv;
+	ret = driv->attach(dev, it);
 	if (ret < 0) {
+		module_put(dev->driver->module);
 		__comedi_device_detach(dev);
 		return ret;
 	}
-
-	if (!dev->board_name) {
-		printk(KERN_WARNING "BUG: dev->board_name=<%p>\n",
-		       dev->board_name);
-		dev->board_name = "BUG";
-	}
-	smp_wmb();
-	dev->attached = 1;
-
-	return 0;
+	return comedi_device_postconfig(dev);
 }
 
 int comedi_driver_register(struct comedi_driver *driver)
@@ -242,6 +239,8 @@ static int postconfig(struct comedi_device *dev)
 			s->len_chanlist = 1;
 
 		if (s->do_cmd) {
+			unsigned int buf_size;
+
 			BUG_ON((s->subdev_flags & (SDF_CMD_READ |
 						   SDF_CMD_WRITE)) == 0);
 			BUG_ON(!s->do_cmdtest);
@@ -257,19 +256,20 @@ static int postconfig(struct comedi_device *dev)
 			async->subdevice = s;
 			s->async = async;
 
-#define DEFAULT_BUF_MAXSIZE (64*1024)
-#define DEFAULT_BUF_SIZE (64*1024)
-
-			async->max_bufsize = DEFAULT_BUF_MAXSIZE;
+			async->max_bufsize =
+				comedi_default_buf_maxsize_kb * 1024;
+			buf_size = comedi_default_buf_size_kb * 1024;
+			if (buf_size > async->max_bufsize)
+				buf_size = async->max_bufsize;
 
 			async->prealloc_buf = NULL;
 			async->prealloc_bufsz = 0;
-			if (comedi_buf_alloc(dev, s, DEFAULT_BUF_SIZE) < 0) {
+			if (comedi_buf_alloc(dev, s, buf_size) < 0) {
 				printk(KERN_INFO "Buffer allocation failed\n");
 				return -ENOMEM;
 			}
 			if (s->buf_change) {
-				ret = s->buf_change(dev, s, DEFAULT_BUF_SIZE);
+				ret = s->buf_change(dev, s, buf_size);
 				if (ret < 0)
 					return ret;
 			}
@@ -814,67 +814,102 @@ void comedi_reset_async_buf(struct comedi_async *async)
 	async->events = 0;
 }
 
-static int comedi_auto_config(struct device *hardware_device,
-			      const char *board_name, const int *options,
-			      unsigned num_options)
+static int
+comedi_auto_config_helper(struct device *hardware_device,
+			  struct comedi_driver *driver,
+			  int (*attach_wrapper) (struct comedi_device *,
+						 void *), void *context)
 {
-	struct comedi_devconfig it;
 	int minor;
 	struct comedi_device_file_info *dev_file_info;
-	int retval;
-	unsigned *private_data = NULL;
+	struct comedi_device *comedi_dev;
+	int ret;
 
-	if (!comedi_autoconfig) {
-		dev_set_drvdata(hardware_device, NULL);
+	if (!comedi_autoconfig)
 		return 0;
-	}
 
 	minor = comedi_alloc_board_minor(hardware_device);
 	if (minor < 0)
 		return minor;
 
-	private_data = kmalloc(sizeof(unsigned), GFP_KERNEL);
-	if (private_data == NULL) {
-		retval = -ENOMEM;
-		goto cleanup;
+	dev_file_info = comedi_get_device_file_info(minor);
+	comedi_dev = dev_file_info->device;
+
+	mutex_lock(&comedi_dev->mutex);
+	if (comedi_dev->attached)
+		ret = -EBUSY;
+	else if (!try_module_get(driver->module)) {
+		printk(KERN_INFO "comedi: failed to increment module count\n");
+		ret = -EIO;
+	} else {
+		/* set comedi_dev->driver here for attach wrapper */
+		comedi_dev->driver = driver;
+		ret = (*attach_wrapper)(comedi_dev, context);
+		if (ret < 0) {
+			module_put(driver->module);
+			__comedi_device_detach(comedi_dev);
+		} else {
+			ret = comedi_device_postconfig(comedi_dev);
+		}
 	}
-	*private_data = minor;
-	dev_set_drvdata(hardware_device, private_data);
+	mutex_unlock(&comedi_dev->mutex);
 
-	dev_file_info = comedi_get_device_file_info(minor);
+	if (ret < 0)
+		comedi_free_board_minor(minor);
+	return ret;
+}
+
+static int comedi_auto_config_wrapper(struct comedi_device *dev, void *context)
+{
+	struct comedi_devconfig *it = context;
+	struct comedi_driver *driv = dev->driver;
+
+	if (driv->num_names) {
+		/* look for generic board entry matching driver name, which
+		 * has already been copied to it->board_name */
+		dev->board_ptr = comedi_recognize(driv, it->board_name);
+		if (dev->board_ptr == NULL) {
+			printk(KERN_WARNING
+			       "comedi: auto config failed to find board entry"
+			       " '%s' for driver '%s'\n", it->board_name,
+			       driv->driver_name);
+			comedi_report_boards(driv);
+			return -EINVAL;
+		}
+	}
+	return driv->attach(dev, it);
+}
+
+static int comedi_auto_config(struct device *hardware_device,
+			      struct comedi_driver *driver, const int *options,
+			      unsigned num_options)
+{
+	struct comedi_devconfig it;
 
 	memset(&it, 0, sizeof(it));
-	strncpy(it.board_name, board_name, COMEDI_NAMELEN);
+	strncpy(it.board_name, driver->driver_name, COMEDI_NAMELEN);
 	it.board_name[COMEDI_NAMELEN - 1] = '\0';
 	BUG_ON(num_options > COMEDI_NDEVCONFOPTS);
 	memcpy(it.options, options, num_options * sizeof(int));
-
-	mutex_lock(&dev_file_info->device->mutex);
-	retval = comedi_device_attach(dev_file_info->device, &it);
-	mutex_unlock(&dev_file_info->device->mutex);
-
-cleanup:
-	if (retval < 0) {
-		kfree(private_data);
-		comedi_free_board_minor(minor);
-	}
-	return retval;
+	return comedi_auto_config_helper(hardware_device, driver,
+					 comedi_auto_config_wrapper, &it);
 }
 
 static void comedi_auto_unconfig(struct device *hardware_device)
 {
-	unsigned *minor = (unsigned *)dev_get_drvdata(hardware_device);
-	if (minor == NULL)
-		return;
-
-	BUG_ON(*minor >= COMEDI_NUM_BOARD_MINORS);
+	int minor;
 
-	comedi_free_board_minor(*minor);
-	dev_set_drvdata(hardware_device, NULL);
-	kfree(minor);
+	if (hardware_device == NULL)
+		return;
+	minor = comedi_find_board_minor(hardware_device);
+	if (minor < 0)
+		return;
+	BUG_ON(minor >= COMEDI_NUM_BOARD_MINORS);
+	comedi_free_board_minor(minor);
 }
 
-int comedi_pci_auto_config(struct pci_dev *pcidev, const char *board_name)
+static int comedi_old_pci_auto_config(struct pci_dev *pcidev,
+				      struct comedi_driver *driver)
 {
 	int options[2];
 
@@ -883,9 +918,30 @@ int comedi_pci_auto_config(struct pci_dev *pcidev, const char *board_name)
 	/*  pci slot */
 	options[1] = PCI_SLOT(pcidev->devfn);
 
-	return comedi_auto_config(&pcidev->dev, board_name,
+	return comedi_auto_config(&pcidev->dev, driver,
 				  options, ARRAY_SIZE(options));
 }
+
+static int comedi_pci_attach_wrapper(struct comedi_device *dev, void *pcidev)
+{
+	return dev->driver->attach_pci(dev, pcidev);
+}
+
+static int comedi_new_pci_auto_config(struct pci_dev *pcidev,
+				      struct comedi_driver *driver)
+{
+	return comedi_auto_config_helper(&pcidev->dev, driver,
+					 comedi_pci_attach_wrapper, pcidev);
+}
+
+int comedi_pci_auto_config(struct pci_dev *pcidev, struct comedi_driver *driver)
+{
+
+	if (driver->attach_pci)
+		return comedi_new_pci_auto_config(pcidev, driver);
+	else
+		return comedi_old_pci_auto_config(pcidev, driver);
+}
 EXPORT_SYMBOL_GPL(comedi_pci_auto_config);
 
 void comedi_pci_auto_unconfig(struct pci_dev *pcidev)
@@ -894,16 +950,96 @@ void comedi_pci_auto_unconfig(struct pci_dev *pcidev)
 }
 EXPORT_SYMBOL_GPL(comedi_pci_auto_unconfig);
 
-int comedi_usb_auto_config(struct usb_device *usbdev, const char *board_name)
+int comedi_pci_driver_register(struct comedi_driver *comedi_driver,
+		struct pci_driver *pci_driver)
+{
+	int ret;
+
+	ret = comedi_driver_register(comedi_driver);
+	if (ret < 0)
+		return ret;
+
+	/* FIXME: Remove this test after auditing all comedi pci drivers */
+	if (!pci_driver->name)
+		pci_driver->name = comedi_driver->driver_name;
+
+	ret = pci_register_driver(pci_driver);
+	if (ret < 0) {
+		comedi_driver_unregister(comedi_driver);
+		return ret;
+	}
+
+	return 0;
+}
+EXPORT_SYMBOL_GPL(comedi_pci_driver_register);
+
+void comedi_pci_driver_unregister(struct comedi_driver *comedi_driver,
+		struct pci_driver *pci_driver)
+{
+	pci_unregister_driver(pci_driver);
+	comedi_driver_unregister(comedi_driver);
+}
+EXPORT_SYMBOL_GPL(comedi_pci_driver_unregister);
+
+static int comedi_old_usb_auto_config(struct usb_interface *intf,
+				      struct comedi_driver *driver)
+{
+	return comedi_auto_config(&intf->dev, driver, NULL, 0);
+}
+
+static int comedi_usb_attach_wrapper(struct comedi_device *dev, void *intf)
+{
+	return dev->driver->attach_usb(dev, intf);
+}
+
+static int comedi_new_usb_auto_config(struct usb_interface *intf,
+				      struct comedi_driver *driver)
+{
+	return comedi_auto_config_helper(&intf->dev, driver,
+					 comedi_usb_attach_wrapper, intf);
+}
+
+int comedi_usb_auto_config(struct usb_interface *intf,
+			   struct comedi_driver *driver)
 {
-	BUG_ON(usbdev == NULL);
-	return comedi_auto_config(&usbdev->dev, board_name, NULL, 0);
+	BUG_ON(intf == NULL);
+	if (driver->attach_usb)
+		return comedi_new_usb_auto_config(intf, driver);
+	else
+		return comedi_old_usb_auto_config(intf, driver);
 }
 EXPORT_SYMBOL_GPL(comedi_usb_auto_config);
 
-void comedi_usb_auto_unconfig(struct usb_device *usbdev)
+void comedi_usb_auto_unconfig(struct usb_interface *intf)
 {
-	BUG_ON(usbdev == NULL);
-	comedi_auto_unconfig(&usbdev->dev);
+	BUG_ON(intf == NULL);
+	comedi_auto_unconfig(&intf->dev);
 }
 EXPORT_SYMBOL_GPL(comedi_usb_auto_unconfig);
+
+int comedi_usb_driver_register(struct comedi_driver *comedi_driver,
+		struct usb_driver *usb_driver)
+{
+	int ret;
+
+	ret = comedi_driver_register(comedi_driver);
+	if (ret < 0)
+		return ret;
+
+	ret = usb_register(usb_driver);
+	if (ret < 0) {
+		comedi_driver_unregister(comedi_driver);
+		return ret;
+	}
+
+	return 0;
+}
+EXPORT_SYMBOL_GPL(comedi_usb_driver_register);
+
+void comedi_usb_driver_unregister(struct comedi_driver *comedi_driver,
+		struct usb_driver *usb_driver)
+{
+	usb_deregister(usb_driver);
+	comedi_driver_unregister(comedi_driver);
+}
+EXPORT_SYMBOL_GPL(comedi_usb_driver_unregister);
diff --git a/drivers/staging/comedi/drivers/8255.c b/drivers/staging/comedi/drivers/8255.c
index 6c26ac887ee..27e39e4eb6b 100644
--- a/drivers/staging/comedi/drivers/8255.c
+++ b/drivers/staging/comedi/drivers/8255.c
@@ -107,31 +107,6 @@ struct subdev_8255_struct {
 #define CALLBACK_FUNC	(((struct subdev_8255_struct *)s->private)->cb_func)
 #define subdevpriv	((struct subdev_8255_struct *)s->private)
 
-static int dev_8255_attach(struct comedi_device *dev,
-			   struct comedi_devconfig *it);
-static int dev_8255_detach(struct comedi_device *dev);
-static struct comedi_driver driver_8255 = {
-	.driver_name = "8255",
-	.module = THIS_MODULE,
-	.attach = dev_8255_attach,
-	.detach = dev_8255_detach,
-};
-
-static int __init driver_8255_init_module(void)
-{
-	return comedi_driver_register(&driver_8255);
-}
-
-static void __exit driver_8255_cleanup_module(void)
-{
-	comedi_driver_unregister(&driver_8255);
-}
-
-module_init(driver_8255_init_module);
-module_exit(driver_8255_cleanup_module);
-
-static void do_config(struct comedi_device *dev, struct comedi_subdevice *s);
-
 void subdev_8255_interrupt(struct comedi_device *dev,
 			   struct comedi_subdevice *s)
 {
@@ -185,6 +160,23 @@ static int subdev_8255_insn(struct comedi_device *dev,
 	return 2;
 }
 
+static void do_config(struct comedi_device *dev, struct comedi_subdevice *s)
+{
+	int config;
+
+	config = CR_CW;
+	/* 1 in io_bits indicates output, 1 in config indicates input */
+	if (!(s->io_bits & 0x0000ff))
+		config |= CR_A_IO;
+	if (!(s->io_bits & 0x00ff00))
+		config |= CR_B_IO;
+	if (!(s->io_bits & 0x0f0000))
+		config |= CR_C_LO_IO;
+	if (!(s->io_bits & 0xf00000))
+		config |= CR_C_HI_IO;
+	CALLBACK_FUNC(1, _8255_CR, config, CALLBACK_ARG);
+}
+
 static int subdev_8255_insn_config(struct comedi_device *dev,
 				   struct comedi_subdevice *s,
 				   struct comedi_insn *insn, unsigned int *data)
@@ -222,23 +214,6 @@ static int subdev_8255_insn_config(struct comedi_device *dev,
 	return 1;
 }
 
-static void do_config(struct comedi_device *dev, struct comedi_subdevice *s)
-{
-	int config;
-
-	config = CR_CW;
-	/* 1 in io_bits indicates output, 1 in config indicates input */
-	if (!(s->io_bits & 0x0000ff))
-		config |= CR_A_IO;
-	if (!(s->io_bits & 0x00ff00))
-		config |= CR_B_IO;
-	if (!(s->io_bits & 0x0f0000))
-		config |= CR_C_LO_IO;
-	if (!(s->io_bits & 0xf00000))
-		config |= CR_C_HI_IO;
-	CALLBACK_FUNC(1, _8255_CR, config, CALLBACK_ARG);
-}
-
 static int subdev_8255_cmdtest(struct comedi_device *dev,
 			       struct comedi_subdevice *s,
 			       struct comedi_cmd *cmd)
@@ -442,14 +417,12 @@ static int dev_8255_attach(struct comedi_device *dev,
 	return 0;
 }
 
-static int dev_8255_detach(struct comedi_device *dev)
+static void dev_8255_detach(struct comedi_device *dev)
 {
 	int i;
 	unsigned long iobase;
 	struct comedi_subdevice *s;
 
-	printk(KERN_INFO "comedi%d: 8255: remove\n", dev->minor);
-
 	for (i = 0; i < dev->n_subdevices; i++) {
 		s = dev->subdevices + i;
 		if (s->type != COMEDI_SUBD_UNUSED) {
@@ -458,10 +431,16 @@ static int dev_8255_detach(struct comedi_device *dev)
 		}
 		subdev_8255_cleanup(dev, s);
 	}
-
-	return 0;
 }
 
+static struct comedi_driver dev_8255_driver = {
+	.driver_name	= "8255",
+	.module		= THIS_MODULE,
+	.attach		= dev_8255_attach,
+	.detach		= dev_8255_detach,
+};
+module_comedi_driver(dev_8255_driver);
+
 MODULE_AUTHOR("Comedi http://www.comedi.org");
 MODULE_DESCRIPTION("Comedi low-level driver");
 MODULE_LICENSE("GPL");
diff --git a/drivers/staging/comedi/drivers/acl7225b.c b/drivers/staging/comedi/drivers/acl7225b.c
index 9def2250bb8..4e4fc418655 100644
--- a/drivers/staging/comedi/drivers/acl7225b.c
+++ b/drivers/staging/comedi/drivers/acl7225b.c
@@ -22,46 +22,13 @@ Devices: [Adlink] ACL-7225b (acl7225b), [ICP] P16R16DIO (p16r16dio)
 #define ACL7225_DI_LO  2	/* Digital input low byte (DI0-DI7) */
 #define ACL7225_DI_HI  3	/* Digital input high byte (DI8-DI15) */
 
-static int acl7225b_attach(struct comedi_device *dev,
-			   struct comedi_devconfig *it);
-static int acl7225b_detach(struct comedi_device *dev);
-
 struct boardtype {
 	const char *name;	/*  driver name */
 	int io_range;		/*  len of I/O space */
 };
 
-static const struct boardtype boardtypes[] = {
-	{"acl7225b", ACL7225_SIZE,},
-	{"p16r16dio", P16R16DIO_SIZE,},
-};
-
-#define n_boardtypes (sizeof(boardtypes)/sizeof(struct boardtype))
 #define this_board ((const struct boardtype *)dev->board_ptr)
 
-static struct comedi_driver driver_acl7225b = {
-	.driver_name = "acl7225b",
-	.module = THIS_MODULE,
-	.attach = acl7225b_attach,
-	.detach = acl7225b_detach,
-	.board_name = &boardtypes[0].name,
-	.num_names = n_boardtypes,
-	.offset = sizeof(struct boardtype),
-};
-
-static int __init driver_acl7225b_init_module(void)
-{
-	return comedi_driver_register(&driver_acl7225b);
-}
-
-static void __exit driver_acl7225b_cleanup_module(void)
-{
-	comedi_driver_unregister(&driver_acl7225b);
-}
-
-module_init(driver_acl7225b_init_module);
-module_exit(driver_acl7225b_cleanup_module);
-
 static int acl7225b_do_insn(struct comedi_device *dev,
 			    struct comedi_subdevice *s,
 			    struct comedi_insn *insn, unsigned int *data)
@@ -152,16 +119,28 @@ static int acl7225b_attach(struct comedi_device *dev,
 	return 0;
 }
 
-static int acl7225b_detach(struct comedi_device *dev)
+static void acl7225b_detach(struct comedi_device *dev)
 {
-	printk(KERN_INFO "comedi%d: acl7225b: remove\n", dev->minor);
-
 	if (dev->iobase)
 		release_region(dev->iobase, this_board->io_range);
-
-	return 0;
 }
 
+static const struct boardtype boardtypes[] = {
+	{ "acl7225b", ACL7225_SIZE, },
+	{ "p16r16dio", P16R16DIO_SIZE, },
+};
+
+static struct comedi_driver acl7225b_driver = {
+	.driver_name	= "acl7225b",
+	.module		= THIS_MODULE,
+	.attach		= acl7225b_attach,
+	.detach		= acl7225b_detach,
+	.board_name	= &boardtypes[0].name,
+	.num_names	= ARRAY_SIZE(boardtypes),
+	.offset		= sizeof(struct boardtype),
+};
+module_comedi_driver(acl7225b_driver);
+
 MODULE_AUTHOR("Comedi http://www.comedi.org");
 MODULE_DESCRIPTION("Comedi low-level driver");
 MODULE_LICENSE("GPL");
diff --git a/drivers/staging/comedi/drivers/addi-data/addi_common.c b/drivers/staging/comedi/drivers/addi-data/addi_common.c
index ca5bd9b8704..44aaf8351ba 100644
--- a/drivers/staging/comedi/drivers/addi-data/addi_common.c
+++ b/drivers/staging/comedi/drivers/addi-data/addi_common.c
@@ -224,2318 +224,1213 @@ MODULE_DEVICE_TABLE(pci, addi_apci_tbl);
 
 static const struct addi_board boardtypes[] = {
 #ifdef CONFIG_APCI_3120
-	{"apci3120",
-			APCI3120_BOARD_VENDOR_ID,
-			0x818D,
-			AMCC_OP_REG_SIZE,
-			APCI3120_ADDRESS_RANGE,
-			8,
-			0,
-			ADDIDATA_NO_EEPROM,
-			NULL,
-			16,
-			8,
-			16,
-			8,
-			0xffff,
-			0x3fff,
-			&range_apci3120_ai,
-			&range_apci3120_ao,
-			4,
-			4,
-			0x0f,
-			0,
-			NULL,
-			1,
-			1,
-			1,
-			10000,
-			100000,
-			v_APCI3120_Interrupt,
-			i_APCI3120_Reset,
-			i_APCI3120_InsnConfigAnalogInput,
-			i_APCI3120_InsnReadAnalogInput,
-			NULL,
-			NULL,
-			i_APCI3120_CommandTestAnalogInput,
-			i_APCI3120_CommandAnalogInput,
-			i_APCI3120_StopCyclicAcquisition,
-			NULL,
-			i_APCI3120_InsnWriteAnalogOutput,
-			NULL,
-			NULL,
-			i_APCI3120_InsnReadDigitalInput,
-			NULL,
-			i_APCI3120_InsnBitsDigitalInput,
-			i_APCI3120_InsnConfigDigitalOutput,
-			i_APCI3120_InsnWriteDigitalOutput,
-			i_APCI3120_InsnBitsDigitalOutput,
-			NULL,
-			i_APCI3120_InsnConfigTimer,
-			i_APCI3120_InsnWriteTimer,
-			i_APCI3120_InsnReadTimer,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-		NULL},
+	{
+		.pc_DriverName		= "apci3120",
+		.i_VendorId		= APCI3120_BOARD_VENDOR_ID,
+		.i_DeviceId		= 0x818D,
+		.i_IorangeBase0		= AMCC_OP_REG_SIZE,
+		.i_IorangeBase1		= APCI3120_ADDRESS_RANGE,
+		.i_IorangeBase2		= 8,
+		.i_PCIEeprom		= ADDIDATA_NO_EEPROM,
+		.i_NbrAiChannel		= 16,
+		.i_NbrAiChannelDiff	= 8,
+		.i_AiChannelList	= 16,
+		.i_NbrAoChannel		= 8,
+		.i_AiMaxdata		= 0xffff,
+		.i_AoMaxdata		= 0x3fff,
+		.pr_AiRangelist		= &range_apci3120_ai,
+		.pr_AoRangelist		= &range_apci3120_ao,
+		.i_NbrDiChannel		= 4,
+		.i_NbrDoChannel		= 4,
+		.i_DoMaxdata		= 0x0f,
+		.i_Dma			= 1,
+		.i_Timer		= 1,
+		.b_AvailableConvertUnit	= 1,
+		.ui_MinAcquisitiontimeNs = 10000,
+		.ui_MinDelaytimeNs	= 100000,
+		.interrupt		= v_APCI3120_Interrupt,
+		.reset			= i_APCI3120_Reset,
+		.ai_config		= i_APCI3120_InsnConfigAnalogInput,
+		.ai_read		= i_APCI3120_InsnReadAnalogInput,
+		.ai_cmdtest		= i_APCI3120_CommandTestAnalogInput,
+		.ai_cmd			= i_APCI3120_CommandAnalogInput,
+		.ai_cancel		= i_APCI3120_StopCyclicAcquisition,
+		.ao_write		= i_APCI3120_InsnWriteAnalogOutput,
+		.di_read		= i_APCI3120_InsnReadDigitalInput,
+		.di_bits		= i_APCI3120_InsnBitsDigitalInput,
+		.do_config		= i_APCI3120_InsnConfigDigitalOutput,
+		.do_write		= i_APCI3120_InsnWriteDigitalOutput,
+		.do_bits		= i_APCI3120_InsnBitsDigitalOutput,
+		.timer_config		= i_APCI3120_InsnConfigTimer,
+		.timer_write		= i_APCI3120_InsnWriteTimer,
+		.timer_read		= i_APCI3120_InsnReadTimer,
+	},
 #endif
 #ifdef CONFIG_APCI_1032
-	{"apci1032",
-			APCI1032_BOARD_VENDOR_ID,
-			0x1003,
-			4,
-			APCI1032_ADDRESS_RANGE,
-			0,
-			0,
-			ADDIDATA_EEPROM,
-			ADDIDATA_93C76,
-			0,
-			0,
-			0,
-			0,
-			0,
-			0,
-			NULL,
-			NULL,
-			32,
-			0,
-			0,
-			0,
-			NULL,
-			0,
-			0,
-			0,
-			0,
-			0,
-			v_APCI1032_Interrupt,
-			i_APCI1032_Reset,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			i_APCI1032_ConfigDigitalInput,
-			i_APCI1032_Read1DigitalInput,
-			NULL,
-			i_APCI1032_ReadMoreDigitalInput,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-		NULL},
+	{
+		.pc_DriverName		= "apci1032",
+		.i_VendorId		= APCI1032_BOARD_VENDOR_ID,
+		.i_DeviceId		= 0x1003,
+		.i_IorangeBase0		= 4,
+		.i_IorangeBase1		= APCI1032_ADDRESS_RANGE,
+		.i_PCIEeprom		= ADDIDATA_EEPROM,
+		.pc_EepromChip		= ADDIDATA_93C76,
+		.i_NbrDiChannel		= 32,
+		.interrupt		= v_APCI1032_Interrupt,
+		.reset			= i_APCI1032_Reset,
+		.di_config		= i_APCI1032_ConfigDigitalInput,
+		.di_read		= i_APCI1032_Read1DigitalInput,
+		.di_bits		= i_APCI1032_ReadMoreDigitalInput,
+	},
 #endif
 #ifdef CONFIG_APCI_1516
-	{"apci1516",
-			APCI1516_BOARD_VENDOR_ID,
-			0x1001,
-			128,
-			APCI1516_ADDRESS_RANGE,
-			32,
-			0,
-			ADDIDATA_EEPROM,
-			ADDIDATA_S5920,
-			0,
-			0,
-			0,
-			0,
-			0,
-			0,
-			NULL,
-			NULL,
-			8,
-			8,
-			0,
-			0,
-			NULL,
-			0,
-			1,
-			0,
-			0,
-			0,
-			NULL,
-			i_APCI1516_Reset,
-			NULL, NULL,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			i_APCI1516_Read1DigitalInput,
-			NULL,
-			i_APCI1516_ReadMoreDigitalInput,
-			i_APCI1516_ConfigDigitalOutput,
-			i_APCI1516_WriteDigitalOutput,
-			i_APCI1516_ReadDigitalOutput,
-			NULL,
-			i_APCI1516_ConfigWatchdog,
-			i_APCI1516_StartStopWriteWatchdog,
-			i_APCI1516_ReadWatchdog,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-		NULL},
+	{
+		.pc_DriverName		= "apci1516",
+		.i_VendorId		= APCI1516_BOARD_VENDOR_ID,
+		.i_DeviceId		= 0x1001,
+		.i_IorangeBase0		= 128,
+		.i_IorangeBase1		= APCI1516_ADDRESS_RANGE,
+		.i_IorangeBase2		= 32,
+		.i_PCIEeprom		= ADDIDATA_EEPROM,
+		.pc_EepromChip		= ADDIDATA_S5920,
+		.i_NbrDiChannel		= 8,
+		.i_NbrDoChannel		= 8,
+		.i_Timer		= 1,
+		.reset			= i_APCI1516_Reset,
+		.di_read		= i_APCI1516_Read1DigitalInput,
+		.di_bits		= i_APCI1516_ReadMoreDigitalInput,
+		.do_config		= i_APCI1516_ConfigDigitalOutput,
+		.do_write		= i_APCI1516_WriteDigitalOutput,
+		.do_bits		= i_APCI1516_ReadDigitalOutput,
+		.timer_config		= i_APCI1516_ConfigWatchdog,
+		.timer_write		= i_APCI1516_StartStopWriteWatchdog,
+		.timer_read		= i_APCI1516_ReadWatchdog,
+	},
 #endif
 #ifdef CONFIG_APCI_2016
-	{"apci2016",
-			APCI2016_BOARD_VENDOR_ID,
-			0x1002,
-			128,
-			APCI2016_ADDRESS_RANGE,
-			32,
-			0,
-			ADDIDATA_EEPROM,
-			ADDIDATA_S5920,
-			0,
-			0,
-			0,
-			0,
-			0,
-			0,
-			NULL,
-			NULL,
-			0,
-			16,
-			0,
-			0,
-			NULL,
-			0,
-			1,
-			0,
-			0,
-			0,
-			NULL,
-			i_APCI2016_Reset,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			i_APCI2016_ConfigDigitalOutput,
-			i_APCI2016_WriteDigitalOutput,
-			i_APCI2016_BitsDigitalOutput,
-			NULL,
-			i_APCI2016_ConfigWatchdog,
-			i_APCI2016_StartStopWriteWatchdog,
-			i_APCI2016_ReadWatchdog,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-		NULL},
+	{
+		.pc_DriverName		= "apci2016",
+		.i_VendorId		= APCI2016_BOARD_VENDOR_ID,
+		.i_DeviceId		= 0x1002,
+		.i_IorangeBase0		= 128,
+		.i_IorangeBase1		= APCI2016_ADDRESS_RANGE,
+		.i_IorangeBase2		= 32,
+		.i_PCIEeprom		= ADDIDATA_EEPROM,
+		.pc_EepromChip		= ADDIDATA_S5920,
+		.i_NbrDoChannel		= 16,
+		.i_Timer		= 1,
+		.reset			= i_APCI2016_Reset,
+		.do_config		= i_APCI2016_ConfigDigitalOutput,
+		.do_write		= i_APCI2016_WriteDigitalOutput,
+		.do_bits		= i_APCI2016_BitsDigitalOutput,
+		.timer_config		= i_APCI2016_ConfigWatchdog,
+		.timer_write		= i_APCI2016_StartStopWriteWatchdog,
+		.timer_read		= i_APCI2016_ReadWatchdog,
+	},
 #endif
 #ifdef CONFIG_APCI_2032
-	{"apci2032",
-			APCI2032_BOARD_VENDOR_ID,
-			0x1004,
-			4,
-			APCI2032_ADDRESS_RANGE,
-			0,
-			0,
-			ADDIDATA_EEPROM,
-			ADDIDATA_93C76,
-			0,
-			0,
-			0,
-			0,
-			0,
-			0,
-			NULL,
-			NULL,
-			0,
-			32,
-			0xffffffff,
-			0,
-			NULL,
-			0,
-			1,
-			0,
-			0,
-			0,
-			v_APCI2032_Interrupt,
-			i_APCI2032_Reset,
-			NULL, NULL,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			i_APCI2032_ConfigDigitalOutput,
-			i_APCI2032_WriteDigitalOutput,
-			i_APCI2032_ReadDigitalOutput,
-			i_APCI2032_ReadInterruptStatus,
-			i_APCI2032_ConfigWatchdog,
-			i_APCI2032_StartStopWriteWatchdog,
-			i_APCI2032_ReadWatchdog,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-		NULL},
+	{
+		.pc_DriverName		= "apci2032",
+		.i_VendorId		= APCI2032_BOARD_VENDOR_ID,
+		.i_DeviceId		= 0x1004,
+		.i_IorangeBase0		= 4,
+		.i_IorangeBase1		= APCI2032_ADDRESS_RANGE,
+		.i_PCIEeprom		= ADDIDATA_EEPROM,
+		.pc_EepromChip		= ADDIDATA_93C76,
+		.i_NbrDoChannel		= 32,
+		.i_DoMaxdata		= 0xffffffff,
+		.i_Timer		= 1,
+		.interrupt		= v_APCI2032_Interrupt,
+		.reset			= i_APCI2032_Reset,
+		.do_config		= i_APCI2032_ConfigDigitalOutput,
+		.do_write		= i_APCI2032_WriteDigitalOutput,
+		.do_bits		= i_APCI2032_ReadDigitalOutput,
+		.do_read		= i_APCI2032_ReadInterruptStatus,
+		.timer_config		= i_APCI2032_ConfigWatchdog,
+		.timer_write		= i_APCI2032_StartStopWriteWatchdog,
+		.timer_read		= i_APCI2032_ReadWatchdog,
+	},
 #endif
 #ifdef CONFIG_APCI_2200
-	{"apci2200",
-			APCI2200_BOARD_VENDOR_ID,
-			0x1005,
-			4,
-			APCI2200_ADDRESS_RANGE,
-			0,
-			0,
-			ADDIDATA_EEPROM,
-			ADDIDATA_93C76,
-			0,
-			0,
-			0,
-			0,
-			0,
-			0,
-			NULL,
-			NULL,
-			8,
-			16,
-			0,
-			0,
-			NULL,
-			0,
-			1,
-			0,
-			0,
-			0,
-			NULL,
-			i_APCI2200_Reset,
-			NULL, NULL,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			i_APCI2200_Read1DigitalInput,
-			NULL,
-			i_APCI2200_ReadMoreDigitalInput,
-			i_APCI2200_ConfigDigitalOutput,
-			i_APCI2200_WriteDigitalOutput,
-			i_APCI2200_ReadDigitalOutput,
-			NULL,
-			i_APCI2200_ConfigWatchdog,
-			i_APCI2200_StartStopWriteWatchdog,
-			i_APCI2200_ReadWatchdog,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-		NULL},
+	{
+		.pc_DriverName		= "apci2200",
+		.i_VendorId		= APCI2200_BOARD_VENDOR_ID,
+		.i_DeviceId		= 0x1005,
+		.i_IorangeBase0		= 4,
+		.i_IorangeBase1		= APCI2200_ADDRESS_RANGE,
+		.i_PCIEeprom		= ADDIDATA_EEPROM,
+		.pc_EepromChip		= ADDIDATA_93C76,
+		.i_NbrDiChannel		= 8,
+		.i_NbrDoChannel		= 16,
+		.i_Timer		= 1,
+		.reset			= i_APCI2200_Reset,
+		.di_read		= i_APCI2200_Read1DigitalInput,
+		.di_bits		= i_APCI2200_ReadMoreDigitalInput,
+		.do_config		= i_APCI2200_ConfigDigitalOutput,
+		.do_write		= i_APCI2200_WriteDigitalOutput,
+		.do_bits		= i_APCI2200_ReadDigitalOutput,
+		.timer_config		= i_APCI2200_ConfigWatchdog,
+		.timer_write		= i_APCI2200_StartStopWriteWatchdog,
+		.timer_read		= i_APCI2200_ReadWatchdog,
+	},
 #endif
 #ifdef CONFIG_APCI_1564
-	{"apci1564",
-			APCI1564_BOARD_VENDOR_ID,
-			0x1006,
-			128,
-			APCI1564_ADDRESS_RANGE,
-			0,
-			0,
-			ADDIDATA_EEPROM,
-			ADDIDATA_93C76,
-			0,
-			0,
-			0,
-			0,
-			0,
-			0,
-			NULL,
-			NULL,
-			32,
-			32,
-			0xffffffff,
-			0,
-			NULL,
-			0,
-			1,
-			0,
-			0,
-			0,
-			v_APCI1564_Interrupt,
-			i_APCI1564_Reset,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			i_APCI1564_ConfigDigitalInput,
-			i_APCI1564_Read1DigitalInput,
-			NULL,
-			i_APCI1564_ReadMoreDigitalInput,
-			i_APCI1564_ConfigDigitalOutput,
-			i_APCI1564_WriteDigitalOutput,
-			i_APCI1564_ReadDigitalOutput,
-			i_APCI1564_ReadInterruptStatus,
-			i_APCI1564_ConfigTimerCounterWatchdog,
-			i_APCI1564_StartStopWriteTimerCounterWatchdog,
-			i_APCI1564_ReadTimerCounterWatchdog,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-		NULL},
+	{
+		.pc_DriverName		= "apci1564",
+		.i_VendorId		= APCI1564_BOARD_VENDOR_ID,
+		.i_DeviceId		= 0x1006,
+		.i_IorangeBase0		= 128,
+		.i_IorangeBase1		= APCI1564_ADDRESS_RANGE,
+		.i_PCIEeprom		= ADDIDATA_EEPROM,
+		.pc_EepromChip		= ADDIDATA_93C76,
+		.i_NbrDiChannel		= 32,
+		.i_NbrDoChannel		= 32,
+		.i_DoMaxdata		= 0xffffffff,
+		.i_Timer		= 1,
+		.interrupt		= v_APCI1564_Interrupt,
+		.reset			= i_APCI1564_Reset,
+		.di_config		= i_APCI1564_ConfigDigitalInput,
+		.di_read		= i_APCI1564_Read1DigitalInput,
+		.di_bits		= i_APCI1564_ReadMoreDigitalInput,
+		.do_config		= i_APCI1564_ConfigDigitalOutput,
+		.do_write		= i_APCI1564_WriteDigitalOutput,
+		.do_bits		= i_APCI1564_ReadDigitalOutput,
+		.do_read		= i_APCI1564_ReadInterruptStatus,
+		.timer_config		= i_APCI1564_ConfigTimerCounterWatchdog,
+		.timer_write		= i_APCI1564_StartStopWriteTimerCounterWatchdog,
+		.timer_read		= i_APCI1564_ReadTimerCounterWatchdog,
+	},
 #endif
 #ifdef CONFIG_APCI_1500
-	{"apci1500",
-			APCI1500_BOARD_VENDOR_ID,
-			0x80fc,
-			128,
-			APCI1500_ADDRESS_RANGE,
-			4,
-			0,
-			ADDIDATA_NO_EEPROM,
-			NULL,
-			0,
-			0,
-			0,
-			0,
-			0,
-			0,
-			NULL,
-			NULL,
-			16,
-			16,
-			0xffff,
-			0,
-			NULL,
-			0,
-			1,
-			0,
-			0,
-			0,
-			v_APCI1500_Interrupt,
-			i_APCI1500_Reset,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			i_APCI1500_ConfigDigitalInputEvent,
-			i_APCI1500_Initialisation,
-			i_APCI1500_StartStopInputEvent,
-			i_APCI1500_ReadMoreDigitalInput,
-			i_APCI1500_ConfigDigitalOutputErrorInterrupt,
-			i_APCI1500_WriteDigitalOutput,
-			i_APCI1500_ConfigureInterrupt,
-			NULL,
-			i_APCI1500_ConfigCounterTimerWatchdog,
-			i_APCI1500_StartStopTriggerTimerCounterWatchdog,
-			i_APCI1500_ReadInterruptMask,
-			i_APCI1500_ReadCounterTimerWatchdog,
-			NULL,
-			NULL,
-			NULL,
-		NULL},
+	{
+		.pc_DriverName		= "apci1500",
+		.i_VendorId		= APCI1500_BOARD_VENDOR_ID,
+		.i_DeviceId		= 0x80fc,
+		.i_IorangeBase0		= 128,
+		.i_IorangeBase1		= APCI1500_ADDRESS_RANGE,
+		.i_IorangeBase2		= 4,
+		.i_PCIEeprom		= ADDIDATA_NO_EEPROM,
+		.i_NbrDiChannel		= 16,
+		.i_NbrDoChannel		= 16,
+		.i_DoMaxdata		= 0xffff,
+		.i_Timer		= 1,
+		.interrupt		= v_APCI1500_Interrupt,
+		.reset			= i_APCI1500_Reset,
+		.di_config		= i_APCI1500_ConfigDigitalInputEvent,
+		.di_read		= i_APCI1500_Initialisation,
+		.di_write		= i_APCI1500_StartStopInputEvent,
+		.di_bits		= i_APCI1500_ReadMoreDigitalInput,
+		.do_config		= i_APCI1500_ConfigDigitalOutputErrorInterrupt,
+		.do_write		= i_APCI1500_WriteDigitalOutput,
+		.do_bits		= i_APCI1500_ConfigureInterrupt,
+		.timer_config		= i_APCI1500_ConfigCounterTimerWatchdog,
+		.timer_write		= i_APCI1500_StartStopTriggerTimerCounterWatchdog,
+		.timer_read		= i_APCI1500_ReadInterruptMask,
+		.timer_bits		= i_APCI1500_ReadCounterTimerWatchdog,
+	},
 #endif
 #ifdef CONFIG_APCI_3001
-	{"apci3001",
-			APCI3120_BOARD_VENDOR_ID,
-			0x828D,
-			AMCC_OP_REG_SIZE,
-			APCI3120_ADDRESS_RANGE,
-			8,
-			0,
-			ADDIDATA_NO_EEPROM,
-			NULL,
-			16,
-			8,
-			16,
-			0,
-			0xfff,
-			0,
-			&range_apci3120_ai,
-			NULL,
-			4,
-			4,
-			0x0f,
-			0,
-			NULL,
-			1,
-			1,
-			1,
-			10000,
-			100000,
-			v_APCI3120_Interrupt,
-			i_APCI3120_Reset,
-			i_APCI3120_InsnConfigAnalogInput,
-			i_APCI3120_InsnReadAnalogInput,
-			NULL,
-			NULL,
-			i_APCI3120_CommandTestAnalogInput,
-			i_APCI3120_CommandAnalogInput,
-			i_APCI3120_StopCyclicAcquisition,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			i_APCI3120_InsnReadDigitalInput,
-			NULL,
-			i_APCI3120_InsnBitsDigitalInput,
-			i_APCI3120_InsnConfigDigitalOutput,
-			i_APCI3120_InsnWriteDigitalOutput,
-			i_APCI3120_InsnBitsDigitalOutput,
-			NULL,
-			i_APCI3120_InsnConfigTimer,
-			i_APCI3120_InsnWriteTimer,
-			i_APCI3120_InsnReadTimer,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-		NULL},
+	{
+		.pc_DriverName		= "apci3001",
+		.i_VendorId		= APCI3120_BOARD_VENDOR_ID,
+		.i_DeviceId		= 0x828D,
+		.i_IorangeBase0		= AMCC_OP_REG_SIZE,
+		.i_IorangeBase1		= APCI3120_ADDRESS_RANGE,
+		.i_IorangeBase2		= 8,
+		.i_PCIEeprom		= ADDIDATA_NO_EEPROM,
+		.i_NbrAiChannel		= 16,
+		.i_NbrAiChannelDiff	= 8,
+		.i_AiChannelList	= 16,
+		.i_AiMaxdata		= 0xfff,
+		.pr_AiRangelist		= &range_apci3120_ai,
+		.i_NbrDiChannel		= 4,
+		.i_NbrDoChannel		= 4,
+		.i_DoMaxdata		= 0x0f,
+		.i_Dma			= 1,
+		.i_Timer		= 1,
+		.b_AvailableConvertUnit	= 1,
+		.ui_MinAcquisitiontimeNs = 10000,
+		.ui_MinDelaytimeNs	= 100000,
+		.interrupt		= v_APCI3120_Interrupt,
+		.reset			= i_APCI3120_Reset,
+		.ai_config		= i_APCI3120_InsnConfigAnalogInput,
+		.ai_read		= i_APCI3120_InsnReadAnalogInput,
+		.ai_cmdtest		= i_APCI3120_CommandTestAnalogInput,
+		.ai_cmd			= i_APCI3120_CommandAnalogInput,
+		.ai_cancel		= i_APCI3120_StopCyclicAcquisition,
+		.di_read		= i_APCI3120_InsnReadDigitalInput,
+		.di_bits		= i_APCI3120_InsnBitsDigitalInput,
+		.do_config		= i_APCI3120_InsnConfigDigitalOutput,
+		.do_write		= i_APCI3120_InsnWriteDigitalOutput,
+		.do_bits		= i_APCI3120_InsnBitsDigitalOutput,
+		.timer_config		= i_APCI3120_InsnConfigTimer,
+		.timer_write		= i_APCI3120_InsnWriteTimer,
+		.timer_read		= i_APCI3120_InsnReadTimer,
+	},
 #endif
 #ifdef CONFIG_APCI_3501
-	{"apci3501",
-			APCI3501_BOARD_VENDOR_ID,
-			0x3001,
-			64,
-			APCI3501_ADDRESS_RANGE,
-			0,
-			0,
-			ADDIDATA_EEPROM,
-			ADDIDATA_S5933,
-			0,
-			0,
-			0,
-			8,
-			0,
-			16383,
-			NULL,
-			&range_apci3501_ao,
-			2,
-			2,
-			0x3,
-			0,
-			NULL,
-			0,
-			1,
-			0,
-			0,
-			0,
-			v_APCI3501_Interrupt,
-			i_APCI3501_Reset,
-			NULL, NULL,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			i_APCI3501_ConfigAnalogOutput,
-			i_APCI3501_WriteAnalogOutput,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			i_APCI3501_ReadDigitalInput,
-			i_APCI3501_ConfigDigitalOutput,
-			i_APCI3501_WriteDigitalOutput,
-			i_APCI3501_ReadDigitalOutput,
-			NULL,
-			i_APCI3501_ConfigTimerCounterWatchdog,
-			i_APCI3501_StartStopWriteTimerCounterWatchdog,
-			i_APCI3501_ReadTimerCounterWatchdog,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-		NULL},
+	{
+		.pc_DriverName		= "apci3501",
+		.i_VendorId		= APCI3501_BOARD_VENDOR_ID,
+		.i_DeviceId		= 0x3001,
+		.i_IorangeBase0		= 64,
+		.i_IorangeBase1		= APCI3501_ADDRESS_RANGE,
+		.i_PCIEeprom		= ADDIDATA_EEPROM,
+		.pc_EepromChip		= ADDIDATA_S5933,
+		.i_AoMaxdata		= 16383,
+		.pr_AoRangelist		= &range_apci3501_ao,
+		.i_NbrDiChannel		= 2,
+		.i_NbrDoChannel		= 2,
+		.i_DoMaxdata		= 0x3,
+		.i_Timer		= 1,
+		.interrupt		= v_APCI3501_Interrupt,
+		.reset			= i_APCI3501_Reset,
+		.ao_config		= i_APCI3501_ConfigAnalogOutput,
+		.ao_write		= i_APCI3501_WriteAnalogOutput,
+		.di_bits		= i_APCI3501_ReadDigitalInput,
+		.do_config		= i_APCI3501_ConfigDigitalOutput,
+		.do_write		= i_APCI3501_WriteDigitalOutput,
+		.do_bits		= i_APCI3501_ReadDigitalOutput,
+		.timer_config		= i_APCI3501_ConfigTimerCounterWatchdog,
+		.timer_write		= i_APCI3501_StartStopWriteTimerCounterWatchdog,
+		.timer_read		= i_APCI3501_ReadTimerCounterWatchdog,
+	},
 #endif
 #ifdef CONFIG_APCI_035
-	{"apci035",
-			APCI035_BOARD_VENDOR_ID,
-			0x0300,
-			127,
-			APCI035_ADDRESS_RANGE,
-			0,
-			0,
-			1,
-			ADDIDATA_S5920,
-			16,
-			8,
-			16,
-			0,
-			0xff,
-			0,
-			&range_apci035_ai,
-			NULL,
-			0,
-			0,
-			0,
-			0,
-			NULL,
-			0,
-			1,
-			0,
-			10000,
-			100000,
-			v_APCI035_Interrupt,
-			i_APCI035_Reset,
-			i_APCI035_ConfigAnalogInput,
-			i_APCI035_ReadAnalogInput,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			i_APCI035_ConfigTimerWatchdog,
-			i_APCI035_StartStopWriteTimerWatchdog,
-			i_APCI035_ReadTimerWatchdog,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-		NULL},
+	{
+		.pc_DriverName		= "apci035",
+		.i_VendorId		= APCI035_BOARD_VENDOR_ID,
+		.i_DeviceId		= 0x0300,
+		.i_IorangeBase0		= 127,
+		.i_IorangeBase1		= APCI035_ADDRESS_RANGE,
+		.i_PCIEeprom		= 1,
+		.pc_EepromChip		= ADDIDATA_S5920,
+		.i_NbrAiChannel		= 16,
+		.i_NbrAiChannelDiff	= 8,
+		.i_AiChannelList	= 16,
+		.i_AiMaxdata		= 0xff,
+		.pr_AiRangelist		= &range_apci035_ai,
+		.i_Timer		= 1,
+		.ui_MinAcquisitiontimeNs = 10000,
+		.ui_MinDelaytimeNs	= 100000,
+		.interrupt		= v_APCI035_Interrupt,
+		.reset			= i_APCI035_Reset,
+		.ai_config		= i_APCI035_ConfigAnalogInput,
+		.ai_read		= i_APCI035_ReadAnalogInput,
+		.timer_config		= i_APCI035_ConfigTimerWatchdog,
+		.timer_write		= i_APCI035_StartStopWriteTimerWatchdog,
+		.timer_read		= i_APCI035_ReadTimerWatchdog,
+	},
 #endif
 #ifdef CONFIG_APCI_3200
-	{"apci3200",
-			APCI3200_BOARD_VENDOR_ID,
-			0x3000,
-			128,
-			256,
-			4,
-			4,
-			ADDIDATA_EEPROM,
-			ADDIDATA_S5920,
-			16,
-			8,
-			16,
-			0,
-			0x3ffff,
-			0,
-			&range_apci3200_ai,
-			NULL,
-			4,
-			4,
-			0,
-			0,
-			NULL,
-			0,
-			0,
-			0,
-			10000,
-			100000,
-			v_APCI3200_Interrupt,
-			i_APCI3200_Reset,
-			i_APCI3200_ConfigAnalogInput,
-			i_APCI3200_ReadAnalogInput,
-			i_APCI3200_InsnWriteReleaseAnalogInput,
-			i_APCI3200_InsnBits_AnalogInput_Test,
-			i_APCI3200_CommandTestAnalogInput,
-			i_APCI3200_CommandAnalogInput,
-			i_APCI3200_StopCyclicAcquisition,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			i_APCI3200_ReadDigitalInput,
-			i_APCI3200_ConfigDigitalOutput,
-			i_APCI3200_WriteDigitalOutput,
-			i_APCI3200_ReadDigitalOutput,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-		NULL},
+	{
+		.pc_DriverName		= "apci3200",
+		.i_VendorId		= APCI3200_BOARD_VENDOR_ID,
+		.i_DeviceId		= 0x3000,
+		.i_IorangeBase0		= 128,
+		.i_IorangeBase1		= 256,
+		.i_IorangeBase2		= 4,
+		.i_IorangeBase3		= 4,
+		.i_PCIEeprom		= ADDIDATA_EEPROM,
+		.pc_EepromChip		= ADDIDATA_S5920,
+		.i_NbrAiChannel		= 16,
+		.i_NbrAiChannelDiff	= 8,
+		.i_AiChannelList	= 16,
+		.i_AiMaxdata		= 0x3ffff,
+		.pr_AiRangelist		= &range_apci3200_ai,
+		.i_NbrDiChannel		= 4,
+		.i_NbrDoChannel		= 4,
+		.ui_MinAcquisitiontimeNs = 10000,
+		.ui_MinDelaytimeNs	= 100000,
+		.interrupt		= v_APCI3200_Interrupt,
+		.reset			= i_APCI3200_Reset,
+		.ai_config		= i_APCI3200_ConfigAnalogInput,
+		.ai_read		= i_APCI3200_ReadAnalogInput,
+		.ai_write		= i_APCI3200_InsnWriteReleaseAnalogInput,
+		.ai_bits		= i_APCI3200_InsnBits_AnalogInput_Test,
+		.ai_cmdtest		= i_APCI3200_CommandTestAnalogInput,
+		.ai_cmd			= i_APCI3200_CommandAnalogInput,
+		.ai_cancel		= i_APCI3200_StopCyclicAcquisition,
+		.di_bits		= i_APCI3200_ReadDigitalInput,
+		.do_config		= i_APCI3200_ConfigDigitalOutput,
+		.do_write		= i_APCI3200_WriteDigitalOutput,
+		.do_bits		= i_APCI3200_ReadDigitalOutput,
+	},
 #endif
 #ifdef CONFIG_APCI_3300
 	/* Begin JK	.20.10.2004 = APCI-3300 integration */
-	{"apci3300",
-			APCI3200_BOARD_VENDOR_ID,
-			0x3007,
-			128,
-			256,
-			4,
-			4,
-			ADDIDATA_EEPROM,
-			ADDIDATA_S5920,
-			0,
-			8,
-			8,
-			0,
-			0x3ffff,
-			0,
-			&range_apci3300_ai,
-			NULL,
-			4,
-			4,
-			0,
-			0,
-			NULL,
-			0,
-			0,
-			0,
-			10000,
-			100000,
-			v_APCI3200_Interrupt,
-			i_APCI3200_Reset,
-			i_APCI3200_ConfigAnalogInput,
-			i_APCI3200_ReadAnalogInput,
-			i_APCI3200_InsnWriteReleaseAnalogInput,
-			i_APCI3200_InsnBits_AnalogInput_Test,
-			i_APCI3200_CommandTestAnalogInput,
-			i_APCI3200_CommandAnalogInput,
-			i_APCI3200_StopCyclicAcquisition,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			i_APCI3200_ReadDigitalInput,
-			i_APCI3200_ConfigDigitalOutput,
-			i_APCI3200_WriteDigitalOutput,
-			i_APCI3200_ReadDigitalOutput,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-		NULL},
+	{
+		.pc_DriverName		= "apci3300",
+		.i_VendorId		= APCI3200_BOARD_VENDOR_ID,
+		.i_DeviceId		= 0x3007,
+		.i_IorangeBase0		= 128,
+		.i_IorangeBase1		= 256,
+		.i_IorangeBase2		= 4,
+		.i_IorangeBase3		= 4,
+		.i_PCIEeprom		= ADDIDATA_EEPROM,
+		.pc_EepromChip		= ADDIDATA_S5920,
+		.i_NbrAiChannelDiff	= 8,
+		.i_AiChannelList	= 8,
+		.i_AiMaxdata		= 0x3ffff,
+		.pr_AiRangelist		= &range_apci3300_ai,
+		.i_NbrDiChannel		= 4,
+		.i_NbrDoChannel		= 4,
+		.ui_MinAcquisitiontimeNs = 10000,
+		.ui_MinDelaytimeNs	= 100000,
+		.interrupt		= v_APCI3200_Interrupt,
+		.reset			= i_APCI3200_Reset,
+		.ai_config		= i_APCI3200_ConfigAnalogInput,
+		.ai_read		= i_APCI3200_ReadAnalogInput,
+		.ai_write		= i_APCI3200_InsnWriteReleaseAnalogInput,
+		.ai_bits		= i_APCI3200_InsnBits_AnalogInput_Test,
+		.ai_cmdtest		= i_APCI3200_CommandTestAnalogInput,
+		.ai_cmd			= i_APCI3200_CommandAnalogInput,
+		.ai_cancel		= i_APCI3200_StopCyclicAcquisition,
+		.di_bits		= i_APCI3200_ReadDigitalInput,
+		.do_config		= i_APCI3200_ConfigDigitalOutput,
+		.do_write		= i_APCI3200_WriteDigitalOutput,
+		.do_bits		= i_APCI3200_ReadDigitalOutput,
+	},
 #endif
 #ifdef CONFIG_APCI_1710
-	{"apci1710", APCI1710_BOARD_VENDOR_ID, APCI1710_BOARD_DEVICE_ID,
-			128,
-			8,
-			256,
-			0,
-			ADDIDATA_NO_EEPROM,
-			NULL,
-			0,
-			0,
-			0,
-			0,
-			0,
-			0,
-			NULL,
-			NULL,
-			0,
-			0,
-			0,
-			0,
-			NULL,
-			0,
-			0,
-			0,
-			0,
-			0,
-			v_APCI1710_Interrupt,
-			i_APCI1710_Reset,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-		NULL},
+	{
+		.pc_DriverName		= "apci1710",
+		.i_VendorId		= APCI1710_BOARD_VENDOR_ID,
+		.i_DeviceId		= APCI1710_BOARD_DEVICE_ID,
+		.i_IorangeBase0		= 128,
+		.i_IorangeBase1		= 8,
+		.i_IorangeBase2		= 256,
+		.i_PCIEeprom		= ADDIDATA_NO_EEPROM,
+		.interrupt		= v_APCI1710_Interrupt,
+		.reset			= i_APCI1710_Reset,
+	},
 #endif
 #ifdef CONFIG_APCI_16XX
-	{"apci1648",
-			PCI_VENDOR_ID_ADDIDATA,
-			0x1009,
-			128,
-			0,
-			0,
-			0,
-			ADDIDATA_NO_EEPROM,
-			NULL,
-			0,
-			0,
-			0,
-			0,
-			0,
-			0,
-			NULL,
-			NULL,
-			0,
-			0,
-			0,
-			48,
-			&range_apci16xx_ttl,
-			0,
-			0,
-			0,
-			0,
-			0,
-			NULL,
-			i_APCI16XX_Reset,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			i_APCI16XX_InsnConfigInitTTLIO,
-			i_APCI16XX_InsnBitsReadTTLIO,
-			i_APCI16XX_InsnReadTTLIOAllPortValue,
-		i_APCI16XX_InsnBitsWriteTTLIO},
-
-	{"apci1696",
-			PCI_VENDOR_ID_ADDIDATA,
-			0x100A,
-			128,
-			0,
-			0,
-			0,
-			ADDIDATA_NO_EEPROM,
-			NULL,
-			0,
-			0,
-			0,
-			0,
-			0,
-			0,
-			NULL,
-			NULL,
-			0,
-			0,
-			0,
-			96,
-			&range_apci16xx_ttl,
-			0,
-			0,
-			0,
-			0,
-			0,
-			NULL,
-			i_APCI16XX_Reset,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			i_APCI16XX_InsnConfigInitTTLIO,
-			i_APCI16XX_InsnBitsReadTTLIO,
-			i_APCI16XX_InsnReadTTLIOAllPortValue,
-		i_APCI16XX_InsnBitsWriteTTLIO},
+	{
+		.pc_DriverName		= "apci1648",
+		.i_VendorId		= PCI_VENDOR_ID_ADDIDATA,
+		.i_DeviceId		= 0x1009,
+		.i_IorangeBase0		= 128,
+		.i_PCIEeprom		= ADDIDATA_NO_EEPROM,
+		.i_NbrTTLChannel	= 48,
+		.pr_TTLRangelist	= &range_apci16xx_ttl,
+		.reset			= i_APCI16XX_Reset,
+		.ttl_config		= i_APCI16XX_InsnConfigInitTTLIO,
+		.ttl_bits		= i_APCI16XX_InsnBitsReadTTLIO,
+		.ttl_read		= i_APCI16XX_InsnReadTTLIOAllPortValue,
+		.ttl_write		= i_APCI16XX_InsnBitsWriteTTLIO,
+	}, {
+		.pc_DriverName		= "apci1696",
+		.i_VendorId		= PCI_VENDOR_ID_ADDIDATA,
+		.i_DeviceId		= 0x100A,
+		.i_IorangeBase0		= 128,
+		.i_PCIEeprom		= ADDIDATA_NO_EEPROM,
+		.i_NbrTTLChannel	= 96,
+		.pr_TTLRangelist	= &range_apci16xx_ttl,
+		.reset			= i_APCI16XX_Reset,
+		.ttl_config		= i_APCI16XX_InsnConfigInitTTLIO,
+		.ttl_bits		= i_APCI16XX_InsnBitsReadTTLIO,
+		.ttl_read		= i_APCI16XX_InsnReadTTLIOAllPortValue,
+		.ttl_write		= i_APCI16XX_InsnBitsWriteTTLIO,
+	},
 #endif
 #ifdef CONFIG_APCI_3XXX
-	{"apci3000-16",
-			PCI_VENDOR_ID_ADDIDATA,
-			0x3010,
-			256,
-			256,
-			256,
-			256,
-			ADDIDATA_NO_EEPROM,
-			ADDIDATA_9054,
-			16,
-			8,
-			16,
-			0,
-			4095,
-			0,
-			&range_apci3XXX_ai,
-			NULL,
-			0,
-			0,
-			0,
-			24,
-			&range_apci3XXX_ttl,
-			0,
-			0,
-			6,
-			10000,
-			0,
-			v_APCI3XXX_Interrupt,
-			i_APCI3XXX_Reset,
-			i_APCI3XXX_InsnConfigAnalogInput,
-			i_APCI3XXX_InsnReadAnalogInput,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			i_APCI3XXX_InsnConfigInitTTLIO,
-			i_APCI3XXX_InsnBitsTTLIO,
-			i_APCI3XXX_InsnReadTTLIO,
-		i_APCI3XXX_InsnWriteTTLIO},
-
-	{"apci3000-8",
-			PCI_VENDOR_ID_ADDIDATA,
-			0x300F,
-			256,
-			256,
-			256,
-			256,
-			ADDIDATA_NO_EEPROM,
-			ADDIDATA_9054,
-			8,
-			4,
-			8,
-			0,
-			4095,
-			0,
-			&range_apci3XXX_ai,
-			NULL,
-			0,
-			0,
-			0,
-			24,
-			&range_apci3XXX_ttl,
-			0,
-			0,
-			6,
-			10000,
-			0,
-			v_APCI3XXX_Interrupt,
-			i_APCI3XXX_Reset,
-			i_APCI3XXX_InsnConfigAnalogInput,
-			i_APCI3XXX_InsnReadAnalogInput,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			i_APCI3XXX_InsnConfigInitTTLIO,
-			i_APCI3XXX_InsnBitsTTLIO,
-			i_APCI3XXX_InsnReadTTLIO,
-		i_APCI3XXX_InsnWriteTTLIO},
-
-	{"apci3000-4",
-			PCI_VENDOR_ID_ADDIDATA,
-			0x300E,
-			256,
-			256,
-			256,
-			256,
-			ADDIDATA_NO_EEPROM,
-			ADDIDATA_9054,
-			4,
-			2,
-			4,
-			0,
-			4095,
-			0,
-			&range_apci3XXX_ai,
-			NULL,
-			0,
-			0,
-			0,
-			24,
-			&range_apci3XXX_ttl,
-			0,
-			0,
-			6,
-			10000,
-			0,
-			v_APCI3XXX_Interrupt,
-			i_APCI3XXX_Reset,
-			i_APCI3XXX_InsnConfigAnalogInput,
-			i_APCI3XXX_InsnReadAnalogInput,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			i_APCI3XXX_InsnConfigInitTTLIO,
-			i_APCI3XXX_InsnBitsTTLIO,
-			i_APCI3XXX_InsnReadTTLIO,
-		i_APCI3XXX_InsnWriteTTLIO},
-
-	{"apci3006-16",
-			PCI_VENDOR_ID_ADDIDATA,
-			0x3013,
-			256,
-			256,
-			256,
-			256,
-			ADDIDATA_NO_EEPROM,
-			ADDIDATA_9054,
-			16,
-			8,
-			16,
-			0,
-			65535,
-			0,
-			&range_apci3XXX_ai,
-			NULL,
-			0,
-			0,
-			0,
-			24,
-			&range_apci3XXX_ttl,
-			0,
-			0,
-			6,
-			10000,
-			0,
-			v_APCI3XXX_Interrupt,
-			i_APCI3XXX_Reset,
-			i_APCI3XXX_InsnConfigAnalogInput,
-			i_APCI3XXX_InsnReadAnalogInput,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			i_APCI3XXX_InsnConfigInitTTLIO,
-			i_APCI3XXX_InsnBitsTTLIO,
-			i_APCI3XXX_InsnReadTTLIO,
-		i_APCI3XXX_InsnWriteTTLIO},
-
-	{"apci3006-8",
-			PCI_VENDOR_ID_ADDIDATA,
-			0x3014,
-			256,
-			256,
-			256,
-			256,
-			ADDIDATA_NO_EEPROM,
-			ADDIDATA_9054,
-			8,
-			4,
-			8,
-			0,
-			65535,
-			0,
-			&range_apci3XXX_ai,
-			NULL,
-			0,
-			0,
-			0,
-			24,
-			&range_apci3XXX_ttl,
-			0,
-			0,
-			6,
-			10000,
-			0,
-			v_APCI3XXX_Interrupt,
-			i_APCI3XXX_Reset,
-			i_APCI3XXX_InsnConfigAnalogInput,
-			i_APCI3XXX_InsnReadAnalogInput,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			i_APCI3XXX_InsnConfigInitTTLIO,
-			i_APCI3XXX_InsnBitsTTLIO,
-			i_APCI3XXX_InsnReadTTLIO,
-		i_APCI3XXX_InsnWriteTTLIO},
-
-	{"apci3006-4",
-			PCI_VENDOR_ID_ADDIDATA,
-			0x3015,
-			256,
-			256,
-			256,
-			256,
-			ADDIDATA_NO_EEPROM,
-			ADDIDATA_9054,
-			4,
-			2,
-			4,
-			0,
-			65535,
-			0,
-			&range_apci3XXX_ai,
-			NULL,
-			0,
-			0,
-			0,
-			24,
-			&range_apci3XXX_ttl,
-			0,
-			0,
-			6,
-			10000,
-			0,
-			v_APCI3XXX_Interrupt,
-			i_APCI3XXX_Reset,
-			i_APCI3XXX_InsnConfigAnalogInput,
-			i_APCI3XXX_InsnReadAnalogInput,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			i_APCI3XXX_InsnConfigInitTTLIO,
-			i_APCI3XXX_InsnBitsTTLIO,
-			i_APCI3XXX_InsnReadTTLIO,
-		i_APCI3XXX_InsnWriteTTLIO},
-
-	{"apci3010-16",
-			PCI_VENDOR_ID_ADDIDATA,
-			0x3016,
-			256,
-			256,
-			256,
-			256,
-			ADDIDATA_NO_EEPROM,
-			ADDIDATA_9054,
-			16,
-			8,
-			16,
-			0,
-			4095,
-			0,
-			&range_apci3XXX_ai,
-			NULL,
-			4,
-			4,
-			1,
-			24,
-			&range_apci3XXX_ttl,
-			0,
-			0,
-			6,
-			5000,
-			0,
-			v_APCI3XXX_Interrupt,
-			i_APCI3XXX_Reset,
-			i_APCI3XXX_InsnConfigAnalogInput,
-			i_APCI3XXX_InsnReadAnalogInput,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			i_APCI3XXX_InsnReadDigitalInput,
-			NULL,
-			i_APCI3XXX_InsnBitsDigitalInput,
-			NULL,
-			i_APCI3XXX_InsnWriteDigitalOutput,
-			i_APCI3XXX_InsnBitsDigitalOutput,
-			i_APCI3XXX_InsnReadDigitalOutput,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			i_APCI3XXX_InsnConfigInitTTLIO,
-			i_APCI3XXX_InsnBitsTTLIO,
-			i_APCI3XXX_InsnReadTTLIO,
-		i_APCI3XXX_InsnWriteTTLIO},
-
-	{"apci3010-8",
-			PCI_VENDOR_ID_ADDIDATA,
-			0x3017,
-			256,
-			256,
-			256,
-			256,
-			ADDIDATA_NO_EEPROM,
-			ADDIDATA_9054,
-			8,
-			4,
-			8,
-			0,
-			4095,
-			0,
-			&range_apci3XXX_ai,
-			NULL,
-			4,
-			4,
-			1,
-			24,
-			&range_apci3XXX_ttl,
-			0,
-			0,
-			6,
-			5000,
-			0,
-			v_APCI3XXX_Interrupt,
-			i_APCI3XXX_Reset,
-			i_APCI3XXX_InsnConfigAnalogInput,
-			i_APCI3XXX_InsnReadAnalogInput,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			i_APCI3XXX_InsnReadDigitalInput,
-			NULL,
-			i_APCI3XXX_InsnBitsDigitalInput,
-			NULL,
-			i_APCI3XXX_InsnWriteDigitalOutput,
-			i_APCI3XXX_InsnBitsDigitalOutput,
-			i_APCI3XXX_InsnReadDigitalOutput,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			i_APCI3XXX_InsnConfigInitTTLIO,
-			i_APCI3XXX_InsnBitsTTLIO,
-			i_APCI3XXX_InsnReadTTLIO,
-		i_APCI3XXX_InsnWriteTTLIO},
-
-	{"apci3010-4",
-			PCI_VENDOR_ID_ADDIDATA,
-			0x3018,
-			256,
-			256,
-			256,
-			256,
-			ADDIDATA_NO_EEPROM,
-			ADDIDATA_9054,
-			4,
-			2,
-			4,
-			0,
-			4095,
-			0,
-			&range_apci3XXX_ai,
-			NULL,
-			4,
-			4,
-			1,
-			24,
-			&range_apci3XXX_ttl,
-			0,
-			0,
-			6,
-			5000,
-			0,
-			v_APCI3XXX_Interrupt,
-			i_APCI3XXX_Reset,
-			i_APCI3XXX_InsnConfigAnalogInput,
-			i_APCI3XXX_InsnReadAnalogInput,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			i_APCI3XXX_InsnReadDigitalInput,
-			NULL,
-			i_APCI3XXX_InsnBitsDigitalInput,
-			NULL,
-			i_APCI3XXX_InsnWriteDigitalOutput,
-			i_APCI3XXX_InsnBitsDigitalOutput,
-			i_APCI3XXX_InsnReadDigitalOutput,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			i_APCI3XXX_InsnConfigInitTTLIO,
-			i_APCI3XXX_InsnBitsTTLIO,
-			i_APCI3XXX_InsnReadTTLIO,
-		i_APCI3XXX_InsnWriteTTLIO},
-
-	{"apci3016-16",
-			PCI_VENDOR_ID_ADDIDATA,
-			0x3019,
-			256,
-			256,
-			256,
-			256,
-			ADDIDATA_NO_EEPROM,
-			ADDIDATA_9054,
-			16,
-			8,
-			16,
-			0,
-			65535,
-			0,
-			&range_apci3XXX_ai,
-			NULL,
-			4,
-			4,
-			1,
-			24,
-			&range_apci3XXX_ttl,
-			0,
-			0,
-			6,
-			5000,
-			0,
-			v_APCI3XXX_Interrupt,
-			i_APCI3XXX_Reset,
-			i_APCI3XXX_InsnConfigAnalogInput,
-			i_APCI3XXX_InsnReadAnalogInput,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			i_APCI3XXX_InsnReadDigitalInput,
-			NULL,
-			i_APCI3XXX_InsnBitsDigitalInput,
-			NULL,
-			i_APCI3XXX_InsnWriteDigitalOutput,
-			i_APCI3XXX_InsnBitsDigitalOutput,
-			i_APCI3XXX_InsnReadDigitalOutput,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			i_APCI3XXX_InsnConfigInitTTLIO,
-			i_APCI3XXX_InsnBitsTTLIO,
-			i_APCI3XXX_InsnReadTTLIO,
-		i_APCI3XXX_InsnWriteTTLIO},
-
-	{"apci3016-8",
-			PCI_VENDOR_ID_ADDIDATA,
-			0x301A,
-			256,
-			256,
-			256,
-			256,
-			ADDIDATA_NO_EEPROM,
-			ADDIDATA_9054,
-			8,
-			4,
-			8,
-			0,
-			65535,
-			0,
-			&range_apci3XXX_ai,
-			NULL,
-			4,
-			4,
-			1,
-			24,
-			&range_apci3XXX_ttl,
-			0,
-			0,
-			6,
-			5000,
-			0,
-			v_APCI3XXX_Interrupt,
-			i_APCI3XXX_Reset,
-			i_APCI3XXX_InsnConfigAnalogInput,
-			i_APCI3XXX_InsnReadAnalogInput,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			i_APCI3XXX_InsnReadDigitalInput,
-			NULL,
-			i_APCI3XXX_InsnBitsDigitalInput,
-			NULL,
-			i_APCI3XXX_InsnWriteDigitalOutput,
-			i_APCI3XXX_InsnBitsDigitalOutput,
-			i_APCI3XXX_InsnReadDigitalOutput,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			i_APCI3XXX_InsnConfigInitTTLIO,
-			i_APCI3XXX_InsnBitsTTLIO,
-			i_APCI3XXX_InsnReadTTLIO,
-		i_APCI3XXX_InsnWriteTTLIO},
-
-	{"apci3016-4",
-			PCI_VENDOR_ID_ADDIDATA,
-			0x301B,
-			256,
-			256,
-			256,
-			256,
-			ADDIDATA_NO_EEPROM,
-			ADDIDATA_9054,
-			4,
-			2,
-			4,
-			0,
-			65535,
-			0,
-			&range_apci3XXX_ai,
-			NULL,
-			4,
-			4,
-			1,
-			24,
-			&range_apci3XXX_ttl,
-			0,
-			0,
-			6,
-			5000,
-			0,
-			v_APCI3XXX_Interrupt,
-			i_APCI3XXX_Reset,
-			i_APCI3XXX_InsnConfigAnalogInput,
-			i_APCI3XXX_InsnReadAnalogInput,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			i_APCI3XXX_InsnReadDigitalInput,
-			NULL,
-			i_APCI3XXX_InsnBitsDigitalInput,
-			NULL,
-			i_APCI3XXX_InsnWriteDigitalOutput,
-			i_APCI3XXX_InsnBitsDigitalOutput,
-			i_APCI3XXX_InsnReadDigitalOutput,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			i_APCI3XXX_InsnConfigInitTTLIO,
-			i_APCI3XXX_InsnBitsTTLIO,
-			i_APCI3XXX_InsnReadTTLIO,
-		i_APCI3XXX_InsnWriteTTLIO},
-
-	{"apci3100-16-4",
-			PCI_VENDOR_ID_ADDIDATA,
-			0x301C,
-			256,
-			256,
-			256,
-			256,
-			ADDIDATA_NO_EEPROM,
-			ADDIDATA_9054,
-			16,
-			8,
-			16,
-			4,
-			4095,
-			4095,
-			&range_apci3XXX_ai,
-			&range_apci3XXX_ao,
-			0,
-			0,
-			0,
-			24,
-			&range_apci3XXX_ttl,
-			0,
-			0,
-			6,
-			10000,
-			0,
-			v_APCI3XXX_Interrupt,
-			i_APCI3XXX_Reset,
-			i_APCI3XXX_InsnConfigAnalogInput,
-			i_APCI3XXX_InsnReadAnalogInput,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			i_APCI3XXX_InsnWriteAnalogOutput,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			i_APCI3XXX_InsnConfigInitTTLIO,
-			i_APCI3XXX_InsnBitsTTLIO,
-			i_APCI3XXX_InsnReadTTLIO,
-		i_APCI3XXX_InsnWriteTTLIO},
-
-	{"apci3100-8-4",
-			PCI_VENDOR_ID_ADDIDATA,
-			0x301D,
-			256,
-			256,
-			256,
-			256,
-			ADDIDATA_NO_EEPROM,
-			ADDIDATA_9054,
-			8,
-			4,
-			8,
-			4,
-			4095,
-			4095,
-			&range_apci3XXX_ai,
-			&range_apci3XXX_ao,
-			0,
-			0,
-			0,
-			24,
-			&range_apci3XXX_ttl,
-			0,
-			0,
-			6,
-			10000,
-			0,
-			v_APCI3XXX_Interrupt,
-			i_APCI3XXX_Reset,
-			i_APCI3XXX_InsnConfigAnalogInput,
-			i_APCI3XXX_InsnReadAnalogInput,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			i_APCI3XXX_InsnWriteAnalogOutput,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			i_APCI3XXX_InsnConfigInitTTLIO,
-			i_APCI3XXX_InsnBitsTTLIO,
-			i_APCI3XXX_InsnReadTTLIO,
-		i_APCI3XXX_InsnWriteTTLIO},
-
-	{"apci3106-16-4",
-			PCI_VENDOR_ID_ADDIDATA,
-			0x301E,
-			256,
-			256,
-			256,
-			256,
-			ADDIDATA_NO_EEPROM,
-			ADDIDATA_9054,
-			16,
-			8,
-			16,
-			4,
-			65535,
-			4095,
-			&range_apci3XXX_ai,
-			&range_apci3XXX_ao,
-			0,
-			0,
-			0,
-			24,
-			&range_apci3XXX_ttl,
-			0,
-			0,
-			6,
-			10000,
-			0,
-			v_APCI3XXX_Interrupt,
-			i_APCI3XXX_Reset,
-			i_APCI3XXX_InsnConfigAnalogInput,
-			i_APCI3XXX_InsnReadAnalogInput,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			i_APCI3XXX_InsnWriteAnalogOutput,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			i_APCI3XXX_InsnConfigInitTTLIO,
-			i_APCI3XXX_InsnBitsTTLIO,
-			i_APCI3XXX_InsnReadTTLIO,
-		i_APCI3XXX_InsnWriteTTLIO},
-
-	{"apci3106-8-4",
-			PCI_VENDOR_ID_ADDIDATA,
-			0x301F,
-			256,
-			256,
-			256,
-			256,
-			ADDIDATA_NO_EEPROM,
-			ADDIDATA_9054,
-			8,
-			4,
-			8,
-			4,
-			65535,
-			4095,
-			&range_apci3XXX_ai,
-			&range_apci3XXX_ao,
-			0,
-			0,
-			0,
-			24,
-			&range_apci3XXX_ttl,
-			0,
-			0,
-			6,
-			10000,
-			0,
-			v_APCI3XXX_Interrupt,
-			i_APCI3XXX_Reset,
-			i_APCI3XXX_InsnConfigAnalogInput,
-			i_APCI3XXX_InsnReadAnalogInput,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			i_APCI3XXX_InsnWriteAnalogOutput,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			i_APCI3XXX_InsnConfigInitTTLIO,
-			i_APCI3XXX_InsnBitsTTLIO,
-			i_APCI3XXX_InsnReadTTLIO,
-		i_APCI3XXX_InsnWriteTTLIO},
-
-	{"apci3110-16-4",
-			PCI_VENDOR_ID_ADDIDATA,
-			0x3020,
-			256,
-			256,
-			256,
-			256,
-			ADDIDATA_NO_EEPROM,
-			ADDIDATA_9054,
-			16,
-			8,
-			16,
-			4,
-			4095,
-			4095,
-			&range_apci3XXX_ai,
-			&range_apci3XXX_ao,
-			4,
-			4,
-			1,
-			24,
-			&range_apci3XXX_ttl,
-			0,
-			0,
-			6,
-			5000,
-			0,
-			v_APCI3XXX_Interrupt,
-			i_APCI3XXX_Reset,
-			i_APCI3XXX_InsnConfigAnalogInput,
-			i_APCI3XXX_InsnReadAnalogInput,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			i_APCI3XXX_InsnWriteAnalogOutput,
-			NULL,
-			NULL,
-			i_APCI3XXX_InsnReadDigitalInput,
-			NULL,
-			i_APCI3XXX_InsnBitsDigitalInput,
-			NULL,
-			i_APCI3XXX_InsnWriteDigitalOutput,
-			i_APCI3XXX_InsnBitsDigitalOutput,
-			i_APCI3XXX_InsnReadDigitalOutput,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			i_APCI3XXX_InsnConfigInitTTLIO,
-			i_APCI3XXX_InsnBitsTTLIO,
-			i_APCI3XXX_InsnReadTTLIO,
-		i_APCI3XXX_InsnWriteTTLIO},
-
-	{"apci3110-8-4",
-			PCI_VENDOR_ID_ADDIDATA,
-			0x3021,
-			256,
-			256,
-			256,
-			256,
-			ADDIDATA_NO_EEPROM,
-			ADDIDATA_9054,
-			8,
-			4,
-			8,
-			4,
-			4095,
-			4095,
-			&range_apci3XXX_ai,
-			&range_apci3XXX_ao,
-			4,
-			4,
-			1,
-			24,
-			&range_apci3XXX_ttl,
-			0,
-			0,
-			6,
-			5000,
-			0,
-			v_APCI3XXX_Interrupt,
-			i_APCI3XXX_Reset,
-			i_APCI3XXX_InsnConfigAnalogInput,
-			i_APCI3XXX_InsnReadAnalogInput,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			i_APCI3XXX_InsnWriteAnalogOutput,
-			NULL,
-			NULL,
-			i_APCI3XXX_InsnReadDigitalInput,
-			NULL,
-			i_APCI3XXX_InsnBitsDigitalInput,
-			NULL,
-			i_APCI3XXX_InsnWriteDigitalOutput,
-			i_APCI3XXX_InsnBitsDigitalOutput,
-			i_APCI3XXX_InsnReadDigitalOutput,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			i_APCI3XXX_InsnConfigInitTTLIO,
-			i_APCI3XXX_InsnBitsTTLIO,
-			i_APCI3XXX_InsnReadTTLIO,
-		i_APCI3XXX_InsnWriteTTLIO},
-
-	{"apci3116-16-4",
-			PCI_VENDOR_ID_ADDIDATA,
-			0x3022,
-			256,
-			256,
-			256,
-			256,
-			ADDIDATA_NO_EEPROM,
-			ADDIDATA_9054,
-			16,
-			8,
-			16,
-			4,
-			65535,
-			4095,
-			&range_apci3XXX_ai,
-			&range_apci3XXX_ao,
-			4,
-			4,
-			1,
-			24,
-			&range_apci3XXX_ttl,
-			0,
-			0,
-			6,
-			5000,
-			0,
-			v_APCI3XXX_Interrupt,
-			i_APCI3XXX_Reset,
-			i_APCI3XXX_InsnConfigAnalogInput,
-			i_APCI3XXX_InsnReadAnalogInput,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			i_APCI3XXX_InsnWriteAnalogOutput,
-			NULL,
-			NULL,
-			i_APCI3XXX_InsnReadDigitalInput,
-			NULL,
-			i_APCI3XXX_InsnBitsDigitalInput,
-			NULL,
-			i_APCI3XXX_InsnWriteDigitalOutput,
-			i_APCI3XXX_InsnBitsDigitalOutput,
-			i_APCI3XXX_InsnReadDigitalOutput,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			i_APCI3XXX_InsnConfigInitTTLIO,
-			i_APCI3XXX_InsnBitsTTLIO,
-			i_APCI3XXX_InsnReadTTLIO,
-		i_APCI3XXX_InsnWriteTTLIO},
-
-	{"apci3116-8-4",
-			PCI_VENDOR_ID_ADDIDATA,
-			0x3023,
-			256,
-			256,
-			256,
-			256,
-			ADDIDATA_NO_EEPROM,
-			ADDIDATA_9054,
-			8,
-			4,
-			8,
-			4,
-			65535,
-			4095,
-			&range_apci3XXX_ai,
-			&range_apci3XXX_ao,
-			4,
-			4,
-			1,
-			24,
-			&range_apci3XXX_ttl,
-			0,
-			0,
-			6,
-			5000,
-			0,
-			v_APCI3XXX_Interrupt,
-			i_APCI3XXX_Reset,
-			i_APCI3XXX_InsnConfigAnalogInput,
-			i_APCI3XXX_InsnReadAnalogInput,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			i_APCI3XXX_InsnWriteAnalogOutput,
-			NULL,
-			NULL,
-			i_APCI3XXX_InsnReadDigitalInput,
-			NULL,
-			i_APCI3XXX_InsnBitsDigitalInput,
-			NULL,
-			i_APCI3XXX_InsnWriteDigitalOutput,
-			i_APCI3XXX_InsnBitsDigitalOutput,
-			i_APCI3XXX_InsnReadDigitalOutput,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			i_APCI3XXX_InsnConfigInitTTLIO,
-			i_APCI3XXX_InsnBitsTTLIO,
-			i_APCI3XXX_InsnReadTTLIO,
-		i_APCI3XXX_InsnWriteTTLIO},
-
-	{"apci3003",
-			PCI_VENDOR_ID_ADDIDATA,
-			0x300B,
-			256,
-			256,
-			256,
-			256,
-			ADDIDATA_NO_EEPROM,
-			ADDIDATA_9054,
-			0,
-			4,
-			4,
-			0,
-			65535,
-			0,
-			&range_apci3XXX_ai,
-			NULL,
-			4,
-			4,
-			1,
-			0,
-			NULL,
-			0,
-			0,
-			7,
-			2500,
-			0,
-			v_APCI3XXX_Interrupt,
-			i_APCI3XXX_Reset,
-			i_APCI3XXX_InsnConfigAnalogInput,
-			i_APCI3XXX_InsnReadAnalogInput,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			i_APCI3XXX_InsnReadDigitalInput,
-			NULL,
-			i_APCI3XXX_InsnBitsDigitalInput,
-			NULL,
-			i_APCI3XXX_InsnWriteDigitalOutput,
-			i_APCI3XXX_InsnBitsDigitalOutput,
-			i_APCI3XXX_InsnReadDigitalOutput,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-		NULL},
-
-	{"apci3002-16",
-			PCI_VENDOR_ID_ADDIDATA,
-			0x3002,
-			256,
-			256,
-			256,
-			256,
-			ADDIDATA_NO_EEPROM,
-			ADDIDATA_9054,
-			0,
-			16,
-			16,
-			0,
-			65535,
-			0,
-			&range_apci3XXX_ai,
-			NULL,
-			4,
-			4,
-			1,
-			0,
-			NULL,
-			0,
-			0,
-			6,
-			5000,
-			0,
-			v_APCI3XXX_Interrupt,
-			i_APCI3XXX_Reset,
-			i_APCI3XXX_InsnConfigAnalogInput,
-			i_APCI3XXX_InsnReadAnalogInput,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			i_APCI3XXX_InsnReadDigitalInput,
-			NULL,
-			i_APCI3XXX_InsnBitsDigitalInput,
-			NULL,
-			i_APCI3XXX_InsnWriteDigitalOutput,
-			i_APCI3XXX_InsnBitsDigitalOutput,
-			i_APCI3XXX_InsnReadDigitalOutput,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-		NULL},
-
-	{"apci3002-8",
-			PCI_VENDOR_ID_ADDIDATA,
-			0x3003,
-			256,
-			256,
-			256,
-			256,
-			ADDIDATA_NO_EEPROM,
-			ADDIDATA_9054,
-			0,
-			8,
-			8,
-			0,
-			65535,
-			0,
-			&range_apci3XXX_ai,
-			NULL,
-			4,
-			4,
-			1,
-			0,
-			NULL,
-			0,
-			0,
-			6,
-			5000,
-			0,
-			v_APCI3XXX_Interrupt,
-			i_APCI3XXX_Reset,
-			i_APCI3XXX_InsnConfigAnalogInput,
-			i_APCI3XXX_InsnReadAnalogInput,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			i_APCI3XXX_InsnReadDigitalInput,
-			NULL,
-			i_APCI3XXX_InsnBitsDigitalInput,
-			NULL,
-			i_APCI3XXX_InsnWriteDigitalOutput,
-			i_APCI3XXX_InsnBitsDigitalOutput,
-			i_APCI3XXX_InsnReadDigitalOutput,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-		NULL},
-
-	{"apci3002-4",
-			PCI_VENDOR_ID_ADDIDATA,
-			0x3004,
-			256,
-			256,
-			256,
-			256,
-			ADDIDATA_NO_EEPROM,
-			ADDIDATA_9054,
-			0,
-			4,
-			4,
-			0,
-			65535,
-			0,
-			&range_apci3XXX_ai,
-			NULL,
-			4,
-			4,
-			1,
-			0,
-			NULL,
-			0,
-			0,
-			6,
-			5000,
-			0,
-			v_APCI3XXX_Interrupt,
-			i_APCI3XXX_Reset,
-			i_APCI3XXX_InsnConfigAnalogInput,
-			i_APCI3XXX_InsnReadAnalogInput,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			i_APCI3XXX_InsnReadDigitalInput,
-			NULL,
-			i_APCI3XXX_InsnBitsDigitalInput,
-			NULL,
-			i_APCI3XXX_InsnWriteDigitalOutput,
-			i_APCI3XXX_InsnBitsDigitalOutput,
-			i_APCI3XXX_InsnReadDigitalOutput,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-		NULL},
-
-	{"apci3500",
-			PCI_VENDOR_ID_ADDIDATA,
-			0x3024,
-			256,
-			256,
-			256,
-			256,
-			ADDIDATA_NO_EEPROM,
-			ADDIDATA_9054,
-			0,
-			0,
-			0,
-			4,
-			0,
-			4095,
-			NULL,
-			&range_apci3XXX_ao,
-			0,
-			0,
-			0,
-			24,
-			&range_apci3XXX_ttl,
-			0,
-			0,
-			0,
-			0,
-			0,
-			v_APCI3XXX_Interrupt,
-			i_APCI3XXX_Reset,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			i_APCI3XXX_InsnWriteAnalogOutput,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			NULL,
-			i_APCI3XXX_InsnConfigInitTTLIO,
-			i_APCI3XXX_InsnBitsTTLIO,
-			i_APCI3XXX_InsnReadTTLIO,
-		i_APCI3XXX_InsnWriteTTLIO},
+	{
+		.pc_DriverName		= "apci3000-16",
+		.i_VendorId		= PCI_VENDOR_ID_ADDIDATA,
+		.i_DeviceId		= 0x3010,
+		.i_IorangeBase0		= 256,
+		.i_IorangeBase1		= 256,
+		.i_IorangeBase2		= 256,
+		.i_IorangeBase3		= 256,
+		.i_PCIEeprom		= ADDIDATA_NO_EEPROM,
+		.pc_EepromChip		= ADDIDATA_9054,
+		.i_NbrAiChannel		= 16,
+		.i_NbrAiChannelDiff	= 8,
+		.i_AiChannelList	= 16,
+		.i_AiMaxdata		= 4095,
+		.pr_AiRangelist		= &range_apci3XXX_ai,
+		.i_NbrTTLChannel	= 24,
+		.pr_TTLRangelist	= &range_apci3XXX_ttl,
+		.b_AvailableConvertUnit	= 6,
+		.ui_MinAcquisitiontimeNs = 10000,
+		.interrupt		= v_APCI3XXX_Interrupt,
+		.reset			= i_APCI3XXX_Reset,
+		.ai_config		= i_APCI3XXX_InsnConfigAnalogInput,
+		.ai_read		= i_APCI3XXX_InsnReadAnalogInput,
+		.ttl_config		= i_APCI3XXX_InsnConfigInitTTLIO,
+		.ttl_bits		= i_APCI3XXX_InsnBitsTTLIO,
+		.ttl_read		= i_APCI3XXX_InsnReadTTLIO,
+		.ttl_write		= i_APCI3XXX_InsnWriteTTLIO,
+	}, {
+		.pc_DriverName		= "apci3000-8",
+		.i_VendorId		= PCI_VENDOR_ID_ADDIDATA,
+		.i_DeviceId		= 0x300F,
+		.i_IorangeBase0		= 256,
+		.i_IorangeBase1		= 256,
+		.i_IorangeBase2		= 256,
+		.i_IorangeBase3		= 256,
+		.i_PCIEeprom		= ADDIDATA_NO_EEPROM,
+		.pc_EepromChip		= ADDIDATA_9054,
+		.i_NbrAiChannel		= 8,
+		.i_NbrAiChannelDiff	= 4,
+		.i_AiChannelList	= 8,
+		.i_AiMaxdata		= 4095,
+		.pr_AiRangelist		= &range_apci3XXX_ai,
+		.i_NbrTTLChannel	= 24,
+		.pr_TTLRangelist	= &range_apci3XXX_ttl,
+		.b_AvailableConvertUnit	= 6,
+		.ui_MinAcquisitiontimeNs = 10000,
+		.interrupt		= v_APCI3XXX_Interrupt,
+		.reset			= i_APCI3XXX_Reset,
+		.ai_config		= i_APCI3XXX_InsnConfigAnalogInput,
+		.ai_read		= i_APCI3XXX_InsnReadAnalogInput,
+		.ttl_config		= i_APCI3XXX_InsnConfigInitTTLIO,
+		.ttl_bits		= i_APCI3XXX_InsnBitsTTLIO,
+		.ttl_read		= i_APCI3XXX_InsnReadTTLIO,
+		.ttl_write		= i_APCI3XXX_InsnWriteTTLIO,
+	}, {
+		.pc_DriverName		= "apci3000-4",
+		.i_VendorId		= PCI_VENDOR_ID_ADDIDATA,
+		.i_DeviceId		= 0x300E,
+		.i_IorangeBase0		= 256,
+		.i_IorangeBase1		= 256,
+		.i_IorangeBase2		= 256,
+		.i_IorangeBase3		= 256,
+		.i_PCIEeprom		= ADDIDATA_NO_EEPROM,
+		.pc_EepromChip		= ADDIDATA_9054,
+		.i_NbrAiChannel		= 4,
+		.i_NbrAiChannelDiff	= 2,
+		.i_AiChannelList	= 4,
+		.i_AiMaxdata		= 4095,
+		.pr_AiRangelist		= &range_apci3XXX_ai,
+		.i_NbrTTLChannel	= 24,
+		.pr_TTLRangelist	= &range_apci3XXX_ttl,
+		.b_AvailableConvertUnit	= 6,
+		.ui_MinAcquisitiontimeNs = 10000,
+		.interrupt		= v_APCI3XXX_Interrupt,
+		.reset			= i_APCI3XXX_Reset,
+		.ai_config		= i_APCI3XXX_InsnConfigAnalogInput,
+		.ai_read		= i_APCI3XXX_InsnReadAnalogInput,
+		.ttl_config		= i_APCI3XXX_InsnConfigInitTTLIO,
+		.ttl_bits		= i_APCI3XXX_InsnBitsTTLIO,
+		.ttl_read		= i_APCI3XXX_InsnReadTTLIO,
+		.ttl_write		= i_APCI3XXX_InsnWriteTTLIO,
+	}, {
+		.pc_DriverName		= "apci3006-16",
+		.i_VendorId		= PCI_VENDOR_ID_ADDIDATA,
+		.i_DeviceId		= 0x3013,
+		.i_IorangeBase0		= 256,
+		.i_IorangeBase1		= 256,
+		.i_IorangeBase2		= 256,
+		.i_IorangeBase3		= 256,
+		.i_PCIEeprom		= ADDIDATA_NO_EEPROM,
+		.pc_EepromChip		= ADDIDATA_9054,
+		.i_NbrAiChannel		= 16,
+		.i_NbrAiChannelDiff	= 8,
+		.i_AiChannelList	= 16,
+		.i_AiMaxdata		= 65535,
+		.pr_AiRangelist		= &range_apci3XXX_ai,
+		.i_NbrTTLChannel	= 24,
+		.pr_TTLRangelist	= &range_apci3XXX_ttl,
+		.b_AvailableConvertUnit	= 6,
+		.ui_MinAcquisitiontimeNs = 10000,
+		.interrupt		= v_APCI3XXX_Interrupt,
+		.reset			= i_APCI3XXX_Reset,
+		.ai_config		= i_APCI3XXX_InsnConfigAnalogInput,
+		.ai_read		= i_APCI3XXX_InsnReadAnalogInput,
+		.ttl_config		= i_APCI3XXX_InsnConfigInitTTLIO,
+		.ttl_bits		= i_APCI3XXX_InsnBitsTTLIO,
+		.ttl_read		= i_APCI3XXX_InsnReadTTLIO,
+		.ttl_write		= i_APCI3XXX_InsnWriteTTLIO,
+	}, {
+		.pc_DriverName		= "apci3006-8",
+		.i_VendorId		= PCI_VENDOR_ID_ADDIDATA,
+		.i_DeviceId		= 0x3014,
+		.i_IorangeBase0		= 256,
+		.i_IorangeBase1		= 256,
+		.i_IorangeBase2		= 256,
+		.i_IorangeBase3		= 256,
+		.i_PCIEeprom		= ADDIDATA_NO_EEPROM,
+		.pc_EepromChip		= ADDIDATA_9054,
+		.i_NbrAiChannel		= 8,
+		.i_NbrAiChannelDiff	= 4,
+		.i_AiChannelList	= 8,
+		.i_AiMaxdata		= 65535,
+		.pr_AiRangelist		= &range_apci3XXX_ai,
+		.i_NbrTTLChannel	= 24,
+		.pr_TTLRangelist	= &range_apci3XXX_ttl,
+		.b_AvailableConvertUnit	= 6,
+		.ui_MinAcquisitiontimeNs = 10000,
+		.interrupt		= v_APCI3XXX_Interrupt,
+		.reset			= i_APCI3XXX_Reset,
+		.ai_config		= i_APCI3XXX_InsnConfigAnalogInput,
+		.ai_read		= i_APCI3XXX_InsnReadAnalogInput,
+		.ttl_config		= i_APCI3XXX_InsnConfigInitTTLIO,
+		.ttl_bits		= i_APCI3XXX_InsnBitsTTLIO,
+		.ttl_read		= i_APCI3XXX_InsnReadTTLIO,
+		.ttl_write		= i_APCI3XXX_InsnWriteTTLIO,
+	}, {
+		.pc_DriverName		= "apci3006-4",
+		.i_VendorId		= PCI_VENDOR_ID_ADDIDATA,
+		.i_DeviceId		= 0x3015,
+		.i_IorangeBase0		= 256,
+		.i_IorangeBase1		= 256,
+		.i_IorangeBase2		= 256,
+		.i_IorangeBase3		= 256,
+		.i_PCIEeprom		= ADDIDATA_NO_EEPROM,
+		.pc_EepromChip		= ADDIDATA_9054,
+		.i_NbrAiChannel		= 4,
+		.i_NbrAiChannelDiff	= 2,
+		.i_AiChannelList	= 4,
+		.i_AiMaxdata		= 65535,
+		.pr_AiRangelist		= &range_apci3XXX_ai,
+		.i_NbrTTLChannel	= 24,
+		.pr_TTLRangelist	= &range_apci3XXX_ttl,
+		.b_AvailableConvertUnit	= 6,
+		.ui_MinAcquisitiontimeNs = 10000,
+		.interrupt		= v_APCI3XXX_Interrupt,
+		.reset			= i_APCI3XXX_Reset,
+		.ai_config		= i_APCI3XXX_InsnConfigAnalogInput,
+		.ai_read		= i_APCI3XXX_InsnReadAnalogInput,
+		.ttl_config		= i_APCI3XXX_InsnConfigInitTTLIO,
+		.ttl_bits		= i_APCI3XXX_InsnBitsTTLIO,
+		.ttl_read		= i_APCI3XXX_InsnReadTTLIO,
+		.ttl_write		= i_APCI3XXX_InsnWriteTTLIO,
+	}, {
+		.pc_DriverName		= "apci3010-16",
+		.i_VendorId		= PCI_VENDOR_ID_ADDIDATA,
+		.i_DeviceId		= 0x3016,
+		.i_IorangeBase0		= 256,
+		.i_IorangeBase1		= 256,
+		.i_IorangeBase2		= 256,
+		.i_IorangeBase3		= 256,
+		.i_PCIEeprom		= ADDIDATA_NO_EEPROM,
+		.pc_EepromChip		= ADDIDATA_9054,
+		.i_NbrAiChannel		= 16,
+		.i_NbrAiChannelDiff	= 8,
+		.i_AiChannelList	= 16,
+		.i_AiMaxdata		= 4095,
+		.pr_AiRangelist		= &range_apci3XXX_ai,
+		.i_NbrDiChannel		= 4,
+		.i_NbrDoChannel		= 4,
+		.i_DoMaxdata		= 1,
+		.i_NbrTTLChannel	= 24,
+		.pr_TTLRangelist	= &range_apci3XXX_ttl,
+		.b_AvailableConvertUnit	= 6,
+		.ui_MinAcquisitiontimeNs = 5000,
+		.interrupt		= v_APCI3XXX_Interrupt,
+		.reset			= i_APCI3XXX_Reset,
+		.ai_config		= i_APCI3XXX_InsnConfigAnalogInput,
+		.ai_read		= i_APCI3XXX_InsnReadAnalogInput,
+		.di_read		= i_APCI3XXX_InsnReadDigitalInput,
+		.di_bits		= i_APCI3XXX_InsnBitsDigitalInput,
+		.do_write		= i_APCI3XXX_InsnWriteDigitalOutput,
+		.do_bits		= i_APCI3XXX_InsnBitsDigitalOutput,
+		.do_read		= i_APCI3XXX_InsnReadDigitalOutput,
+		.ttl_config		= i_APCI3XXX_InsnConfigInitTTLIO,
+		.ttl_bits		= i_APCI3XXX_InsnBitsTTLIO,
+		.ttl_read		= i_APCI3XXX_InsnReadTTLIO,
+		.ttl_write		= i_APCI3XXX_InsnWriteTTLIO,
+	}, {
+		.pc_DriverName		= "apci3010-8",
+		.i_VendorId		= PCI_VENDOR_ID_ADDIDATA,
+		.i_DeviceId		= 0x3017,
+		.i_IorangeBase0		= 256,
+		.i_IorangeBase1		= 256,
+		.i_IorangeBase2		= 256,
+		.i_IorangeBase3		= 256,
+		.i_PCIEeprom		= ADDIDATA_NO_EEPROM,
+		.pc_EepromChip		= ADDIDATA_9054,
+		.i_NbrAiChannel		= 8,
+		.i_NbrAiChannelDiff	= 4,
+		.i_AiChannelList	= 8,
+		.i_AiMaxdata		= 4095,
+		.pr_AiRangelist		= &range_apci3XXX_ai,
+		.i_NbrDiChannel		= 4,
+		.i_NbrDoChannel		= 4,
+		.i_DoMaxdata		= 1,
+		.i_NbrTTLChannel	= 24,
+		.pr_TTLRangelist	= &range_apci3XXX_ttl,
+		.b_AvailableConvertUnit	= 6,
+		.ui_MinAcquisitiontimeNs = 5000,
+		.interrupt		= v_APCI3XXX_Interrupt,
+		.reset			= i_APCI3XXX_Reset,
+		.ai_config		= i_APCI3XXX_InsnConfigAnalogInput,
+		.ai_read		= i_APCI3XXX_InsnReadAnalogInput,
+		.di_read		= i_APCI3XXX_InsnReadDigitalInput,
+		.di_bits		= i_APCI3XXX_InsnBitsDigitalInput,
+		.do_write		= i_APCI3XXX_InsnWriteDigitalOutput,
+		.do_bits		= i_APCI3XXX_InsnBitsDigitalOutput,
+		.do_read		= i_APCI3XXX_InsnReadDigitalOutput,
+		.ttl_config		= i_APCI3XXX_InsnConfigInitTTLIO,
+		.ttl_bits		= i_APCI3XXX_InsnBitsTTLIO,
+		.ttl_read		= i_APCI3XXX_InsnReadTTLIO,
+		.ttl_write		= i_APCI3XXX_InsnWriteTTLIO,
+	}, {
+		.pc_DriverName		= "apci3010-4",
+		.i_VendorId		= PCI_VENDOR_ID_ADDIDATA,
+		.i_DeviceId		= 0x3018,
+		.i_IorangeBase0		= 256,
+		.i_IorangeBase1		= 256,
+		.i_IorangeBase2		= 256,
+		.i_IorangeBase3		= 256,
+		.i_PCIEeprom		= ADDIDATA_NO_EEPROM,
+		.pc_EepromChip		= ADDIDATA_9054,
+		.i_NbrAiChannel		= 4,
+		.i_NbrAiChannelDiff	= 2,
+		.i_AiChannelList	= 4,
+		.i_AiMaxdata		= 4095,
+		.pr_AiRangelist		= &range_apci3XXX_ai,
+		.i_NbrDiChannel		= 4,
+		.i_NbrDoChannel		= 4,
+		.i_DoMaxdata		= 1,
+		.i_NbrTTLChannel	= 24,
+		.pr_TTLRangelist	= &range_apci3XXX_ttl,
+		.b_AvailableConvertUnit	= 6,
+		.ui_MinAcquisitiontimeNs = 5000,
+		.interrupt		= v_APCI3XXX_Interrupt,
+		.reset			= i_APCI3XXX_Reset,
+		.ai_config		= i_APCI3XXX_InsnConfigAnalogInput,
+		.ai_read		= i_APCI3XXX_InsnReadAnalogInput,
+		.di_read		= i_APCI3XXX_InsnReadDigitalInput,
+		.di_bits		= i_APCI3XXX_InsnBitsDigitalInput,
+		.do_write		= i_APCI3XXX_InsnWriteDigitalOutput,
+		.do_bits		= i_APCI3XXX_InsnBitsDigitalOutput,
+		.do_read		= i_APCI3XXX_InsnReadDigitalOutput,
+		.ttl_config		= i_APCI3XXX_InsnConfigInitTTLIO,
+		.ttl_bits		= i_APCI3XXX_InsnBitsTTLIO,
+		.ttl_read		= i_APCI3XXX_InsnReadTTLIO,
+		.ttl_write		= i_APCI3XXX_InsnWriteTTLIO,
+	}, {
+		.pc_DriverName		= "apci3016-16",
+		.i_VendorId		= PCI_VENDOR_ID_ADDIDATA,
+		.i_DeviceId		= 0x3019,
+		.i_IorangeBase0		= 256,
+		.i_IorangeBase1		= 256,
+		.i_IorangeBase2		= 256,
+		.i_IorangeBase3		= 256,
+		.i_PCIEeprom		= ADDIDATA_NO_EEPROM,
+		.pc_EepromChip		= ADDIDATA_9054,
+		.i_NbrAiChannel		= 16,
+		.i_NbrAiChannelDiff	= 8,
+		.i_AiChannelList	= 16,
+		.i_AiMaxdata		= 65535,
+		.pr_AiRangelist		= &range_apci3XXX_ai,
+		.i_NbrDiChannel		= 4,
+		.i_NbrDoChannel		= 4,
+		.i_DoMaxdata		= 1,
+		.i_NbrTTLChannel	= 24,
+		.pr_TTLRangelist	= &range_apci3XXX_ttl,
+		.b_AvailableConvertUnit	= 6,
+		.ui_MinAcquisitiontimeNs = 5000,
+		.interrupt		= v_APCI3XXX_Interrupt,
+		.reset			= i_APCI3XXX_Reset,
+		.ai_config		= i_APCI3XXX_InsnConfigAnalogInput,
+		.ai_read		= i_APCI3XXX_InsnReadAnalogInput,
+		.di_read		= i_APCI3XXX_InsnReadDigitalInput,
+		.di_bits		= i_APCI3XXX_InsnBitsDigitalInput,
+		.do_write		= i_APCI3XXX_InsnWriteDigitalOutput,
+		.do_bits		= i_APCI3XXX_InsnBitsDigitalOutput,
+		.do_read		= i_APCI3XXX_InsnReadDigitalOutput,
+		.ttl_config		= i_APCI3XXX_InsnConfigInitTTLIO,
+		.ttl_bits		= i_APCI3XXX_InsnBitsTTLIO,
+		.ttl_read		= i_APCI3XXX_InsnReadTTLIO,
+		.ttl_write		= i_APCI3XXX_InsnWriteTTLIO,
+	}, {
+		.pc_DriverName		= "apci3016-8",
+		.i_VendorId		= PCI_VENDOR_ID_ADDIDATA,
+		.i_DeviceId		= 0x301A,
+		.i_IorangeBase0		= 256,
+		.i_IorangeBase1		= 256,
+		.i_IorangeBase2		= 256,
+		.i_IorangeBase3		= 256,
+		.i_PCIEeprom		= ADDIDATA_NO_EEPROM,
+		.pc_EepromChip		= ADDIDATA_9054,
+		.i_NbrAiChannel		= 8,
+		.i_NbrAiChannelDiff	= 4,
+		.i_AiChannelList	= 8,
+		.i_AiMaxdata		= 65535,
+		.pr_AiRangelist		= &range_apci3XXX_ai,
+		.i_NbrDiChannel		= 4,
+		.i_NbrDoChannel		= 4,
+		.i_DoMaxdata		= 1,
+		.i_NbrTTLChannel	= 24,
+		.pr_TTLRangelist	= &range_apci3XXX_ttl,
+		.b_AvailableConvertUnit	= 6,
+		.ui_MinAcquisitiontimeNs = 5000,
+		.interrupt		= v_APCI3XXX_Interrupt,
+		.reset			= i_APCI3XXX_Reset,
+		.ai_config		= i_APCI3XXX_InsnConfigAnalogInput,
+		.ai_read		= i_APCI3XXX_InsnReadAnalogInput,
+		.di_read		= i_APCI3XXX_InsnReadDigitalInput,
+		.di_bits		= i_APCI3XXX_InsnBitsDigitalInput,
+		.do_write		= i_APCI3XXX_InsnWriteDigitalOutput,
+		.do_bits		= i_APCI3XXX_InsnBitsDigitalOutput,
+		.do_read		= i_APCI3XXX_InsnReadDigitalOutput,
+		.ttl_config		= i_APCI3XXX_InsnConfigInitTTLIO,
+		.ttl_bits		= i_APCI3XXX_InsnBitsTTLIO,
+		.ttl_read		= i_APCI3XXX_InsnReadTTLIO,
+		.ttl_write		= i_APCI3XXX_InsnWriteTTLIO,
+	}, {
+		.pc_DriverName		= "apci3016-4",
+		.i_VendorId		= PCI_VENDOR_ID_ADDIDATA,
+		.i_DeviceId		= 0x301B,
+		.i_IorangeBase0		= 256,
+		.i_IorangeBase1		= 256,
+		.i_IorangeBase2		= 256,
+		.i_IorangeBase3		= 256,
+		.i_PCIEeprom		= ADDIDATA_NO_EEPROM,
+		.pc_EepromChip		= ADDIDATA_9054,
+		.i_NbrAiChannel		= 4,
+		.i_NbrAiChannelDiff	= 2,
+		.i_AiChannelList	= 4,
+		.i_AiMaxdata		= 65535,
+		.pr_AiRangelist		= &range_apci3XXX_ai,
+		.i_NbrDiChannel		= 4,
+		.i_NbrDoChannel		= 4,
+		.i_DoMaxdata		= 1,
+		.i_NbrTTLChannel	= 24,
+		.pr_TTLRangelist	= &range_apci3XXX_ttl,
+		.b_AvailableConvertUnit	= 6,
+		.ui_MinAcquisitiontimeNs = 5000,
+		.interrupt		= v_APCI3XXX_Interrupt,
+		.reset			= i_APCI3XXX_Reset,
+		.ai_config		= i_APCI3XXX_InsnConfigAnalogInput,
+		.ai_read		= i_APCI3XXX_InsnReadAnalogInput,
+		.di_read		= i_APCI3XXX_InsnReadDigitalInput,
+		.di_bits		= i_APCI3XXX_InsnBitsDigitalInput,
+		.do_write		= i_APCI3XXX_InsnWriteDigitalOutput,
+		.do_bits		= i_APCI3XXX_InsnBitsDigitalOutput,
+		.do_read		= i_APCI3XXX_InsnReadDigitalOutput,
+		.ttl_config		= i_APCI3XXX_InsnConfigInitTTLIO,
+		.ttl_bits		= i_APCI3XXX_InsnBitsTTLIO,
+		.ttl_read		= i_APCI3XXX_InsnReadTTLIO,
+		.ttl_write		= i_APCI3XXX_InsnWriteTTLIO,
+	}, {
+		.pc_DriverName		= "apci3100-16-4",
+		.i_VendorId		= PCI_VENDOR_ID_ADDIDATA,
+		.i_DeviceId		= 0x301C,
+		.i_IorangeBase0		= 256,
+		.i_IorangeBase1		= 256,
+		.i_IorangeBase2		= 256,
+		.i_IorangeBase3		= 256,
+		.i_PCIEeprom		= ADDIDATA_NO_EEPROM,
+		.pc_EepromChip		= ADDIDATA_9054,
+		.i_NbrAiChannel		= 16,
+		.i_NbrAiChannelDiff	= 8,
+		.i_AiChannelList	= 16,
+		.i_NbrAoChannel		= 4,
+		.i_AiMaxdata		= 4095,
+		.i_AoMaxdata		= 4095,
+		.pr_AiRangelist		= &range_apci3XXX_ai,
+		.pr_AoRangelist		= &range_apci3XXX_ao,
+		.i_NbrTTLChannel	= 24,
+		.pr_TTLRangelist	= &range_apci3XXX_ttl,
+		.b_AvailableConvertUnit	= 6,
+		.ui_MinAcquisitiontimeNs = 10000,
+		.interrupt		= v_APCI3XXX_Interrupt,
+		.reset			= i_APCI3XXX_Reset,
+		.ai_config		= i_APCI3XXX_InsnConfigAnalogInput,
+		.ai_read		= i_APCI3XXX_InsnReadAnalogInput,
+		.ao_write		= i_APCI3XXX_InsnWriteAnalogOutput,
+		.ttl_config		= i_APCI3XXX_InsnConfigInitTTLIO,
+		.ttl_bits		= i_APCI3XXX_InsnBitsTTLIO,
+		.ttl_read		= i_APCI3XXX_InsnReadTTLIO,
+		.ttl_write		= i_APCI3XXX_InsnWriteTTLIO,
+	}, {
+		.pc_DriverName		= "apci3100-8-4",
+		.i_VendorId		= PCI_VENDOR_ID_ADDIDATA,
+		.i_DeviceId		= 0x301D,
+		.i_IorangeBase0		= 256,
+		.i_IorangeBase1		= 256,
+		.i_IorangeBase2		= 256,
+		.i_IorangeBase3		= 256,
+		.i_PCIEeprom		= ADDIDATA_NO_EEPROM,
+		.pc_EepromChip		= ADDIDATA_9054,
+		.i_NbrAiChannel		= 8,
+		.i_NbrAiChannelDiff	= 4,
+		.i_AiChannelList	= 8,
+		.i_NbrAoChannel		= 4,
+		.i_AiMaxdata		= 4095,
+		.i_AoMaxdata		= 4095,
+		.pr_AiRangelist		= &range_apci3XXX_ai,
+		.pr_AoRangelist		= &range_apci3XXX_ao,
+		.i_NbrTTLChannel	= 24,
+		.pr_TTLRangelist	= &range_apci3XXX_ttl,
+		.b_AvailableConvertUnit	= 6,
+		.ui_MinAcquisitiontimeNs = 10000,
+		.interrupt		= v_APCI3XXX_Interrupt,
+		.reset			= i_APCI3XXX_Reset,
+		.ai_config		= i_APCI3XXX_InsnConfigAnalogInput,
+		.ai_read		= i_APCI3XXX_InsnReadAnalogInput,
+		.ao_write		= i_APCI3XXX_InsnWriteAnalogOutput,
+		.ttl_config		= i_APCI3XXX_InsnConfigInitTTLIO,
+		.ttl_bits		= i_APCI3XXX_InsnBitsTTLIO,
+		.ttl_read		= i_APCI3XXX_InsnReadTTLIO,
+		.ttl_write		= i_APCI3XXX_InsnWriteTTLIO,
+	}, {
+		.pc_DriverName		= "apci3106-16-4",
+		.i_VendorId		= PCI_VENDOR_ID_ADDIDATA,
+		.i_DeviceId		= 0x301E,
+		.i_IorangeBase0		= 256,
+		.i_IorangeBase1		= 256,
+		.i_IorangeBase2		= 256,
+		.i_IorangeBase3		= 256,
+		.i_PCIEeprom		= ADDIDATA_NO_EEPROM,
+		.pc_EepromChip		= ADDIDATA_9054,
+		.i_NbrAiChannel		= 16,
+		.i_NbrAiChannelDiff	= 8,
+		.i_AiChannelList	= 16,
+		.i_NbrAoChannel		= 4,
+		.i_AiMaxdata		= 65535,
+		.i_AoMaxdata		= 4095,
+		.pr_AiRangelist		= &range_apci3XXX_ai,
+		.pr_AoRangelist		= &range_apci3XXX_ao,
+		.i_NbrTTLChannel	= 24,
+		.pr_TTLRangelist	= &range_apci3XXX_ttl,
+		.b_AvailableConvertUnit	= 6,
+		.ui_MinAcquisitiontimeNs = 10000,
+		.interrupt		= v_APCI3XXX_Interrupt,
+		.reset			= i_APCI3XXX_Reset,
+		.ai_config		= i_APCI3XXX_InsnConfigAnalogInput,
+		.ai_read		= i_APCI3XXX_InsnReadAnalogInput,
+		.ao_write		= i_APCI3XXX_InsnWriteAnalogOutput,
+		.ttl_config		= i_APCI3XXX_InsnConfigInitTTLIO,
+		.ttl_bits		= i_APCI3XXX_InsnBitsTTLIO,
+		.ttl_read		= i_APCI3XXX_InsnReadTTLIO,
+		.ttl_write		= i_APCI3XXX_InsnWriteTTLIO,
+	}, {
+		.pc_DriverName		= "apci3106-8-4",
+		.i_VendorId		= PCI_VENDOR_ID_ADDIDATA,
+		.i_DeviceId		= 0x301F,
+		.i_IorangeBase0		= 256,
+		.i_IorangeBase1		= 256,
+		.i_IorangeBase2		= 256,
+		.i_IorangeBase3		= 256,
+		.i_PCIEeprom		= ADDIDATA_NO_EEPROM,
+		.pc_EepromChip		= ADDIDATA_9054,
+		.i_NbrAiChannel		= 8,
+		.i_NbrAiChannelDiff	= 4,
+		.i_AiChannelList	= 8,
+		.i_NbrAoChannel		= 4,
+		.i_AiMaxdata		= 65535,
+		.i_AoMaxdata		= 4095,
+		.pr_AiRangelist		= &range_apci3XXX_ai,
+		.pr_AoRangelist		= &range_apci3XXX_ao,
+		.i_NbrTTLChannel	= 24,
+		.pr_TTLRangelist	= &range_apci3XXX_ttl,
+		.b_AvailableConvertUnit	= 6,
+		.ui_MinAcquisitiontimeNs = 10000,
+		.interrupt		= v_APCI3XXX_Interrupt,
+		.reset			= i_APCI3XXX_Reset,
+		.ai_config		= i_APCI3XXX_InsnConfigAnalogInput,
+		.ai_read		= i_APCI3XXX_InsnReadAnalogInput,
+		.ao_write		= i_APCI3XXX_InsnWriteAnalogOutput,
+		.ttl_config		= i_APCI3XXX_InsnConfigInitTTLIO,
+		.ttl_bits		= i_APCI3XXX_InsnBitsTTLIO,
+		.ttl_read		= i_APCI3XXX_InsnReadTTLIO,
+		.ttl_write		= i_APCI3XXX_InsnWriteTTLIO,
+	}, {
+		.pc_DriverName		= "apci3110-16-4",
+		.i_VendorId		= PCI_VENDOR_ID_ADDIDATA,
+		.i_DeviceId		= 0x3020,
+		.i_IorangeBase0		= 256,
+		.i_IorangeBase1		= 256,
+		.i_IorangeBase2		= 256,
+		.i_IorangeBase3		= 256,
+		.i_PCIEeprom		= ADDIDATA_NO_EEPROM,
+		.pc_EepromChip		= ADDIDATA_9054,
+		.i_NbrAiChannel		= 16,
+		.i_NbrAiChannelDiff	= 8,
+		.i_AiChannelList	= 16,
+		.i_NbrAoChannel		= 4,
+		.i_AiMaxdata		= 4095,
+		.i_AoMaxdata		= 4095,
+		.pr_AiRangelist		= &range_apci3XXX_ai,
+		.pr_AoRangelist		= &range_apci3XXX_ao,
+		.i_NbrDiChannel		= 4,
+		.i_NbrDoChannel		= 4,
+		.i_DoMaxdata		= 1,
+		.i_NbrTTLChannel	= 24,
+		.pr_TTLRangelist	= &range_apci3XXX_ttl,
+		.b_AvailableConvertUnit	= 6,
+		.ui_MinAcquisitiontimeNs = 5000,
+		.interrupt		= v_APCI3XXX_Interrupt,
+		.reset			= i_APCI3XXX_Reset,
+		.ai_config		= i_APCI3XXX_InsnConfigAnalogInput,
+		.ai_read		= i_APCI3XXX_InsnReadAnalogInput,
+		.ao_write		= i_APCI3XXX_InsnWriteAnalogOutput,
+		.di_read		= i_APCI3XXX_InsnReadDigitalInput,
+		.di_bits		= i_APCI3XXX_InsnBitsDigitalInput,
+		.do_write		= i_APCI3XXX_InsnWriteDigitalOutput,
+		.do_bits		= i_APCI3XXX_InsnBitsDigitalOutput,
+		.do_read		= i_APCI3XXX_InsnReadDigitalOutput,
+		.ttl_config		= i_APCI3XXX_InsnConfigInitTTLIO,
+		.ttl_bits		= i_APCI3XXX_InsnBitsTTLIO,
+		.ttl_read		= i_APCI3XXX_InsnReadTTLIO,
+		.ttl_write		= i_APCI3XXX_InsnWriteTTLIO,
+	}, {
+		.pc_DriverName		= "apci3110-8-4",
+		.i_VendorId		= PCI_VENDOR_ID_ADDIDATA,
+		.i_DeviceId		= 0x3021,
+		.i_IorangeBase0		= 256,
+		.i_IorangeBase1		= 256,
+		.i_IorangeBase2		= 256,
+		.i_IorangeBase3		= 256,
+		.i_PCIEeprom		= ADDIDATA_NO_EEPROM,
+		.pc_EepromChip		= ADDIDATA_9054,
+		.i_NbrAiChannel		= 8,
+		.i_NbrAiChannelDiff	= 4,
+		.i_AiChannelList	= 8,
+		.i_NbrAoChannel		= 4,
+		.i_AiMaxdata		= 4095,
+		.i_AoMaxdata		= 4095,
+		.pr_AiRangelist		= &range_apci3XXX_ai,
+		.pr_AoRangelist		= &range_apci3XXX_ao,
+		.i_NbrDiChannel		= 4,
+		.i_NbrDoChannel		= 4,
+		.i_DoMaxdata		= 1,
+		.i_NbrTTLChannel	= 24,
+		.pr_TTLRangelist	= &range_apci3XXX_ttl,
+		.b_AvailableConvertUnit	= 6,
+		.ui_MinAcquisitiontimeNs = 5000,
+		.interrupt		= v_APCI3XXX_Interrupt,
+		.reset			= i_APCI3XXX_Reset,
+		.ai_config		= i_APCI3XXX_InsnConfigAnalogInput,
+		.ai_read		= i_APCI3XXX_InsnReadAnalogInput,
+		.ao_write		= i_APCI3XXX_InsnWriteAnalogOutput,
+		.di_read		= i_APCI3XXX_InsnReadDigitalInput,
+		.di_bits		= i_APCI3XXX_InsnBitsDigitalInput,
+		.do_write		= i_APCI3XXX_InsnWriteDigitalOutput,
+		.do_bits		= i_APCI3XXX_InsnBitsDigitalOutput,
+		.do_read		= i_APCI3XXX_InsnReadDigitalOutput,
+		.ttl_config		= i_APCI3XXX_InsnConfigInitTTLIO,
+		.ttl_bits		= i_APCI3XXX_InsnBitsTTLIO,
+		.ttl_read		= i_APCI3XXX_InsnReadTTLIO,
+		.ttl_write		= i_APCI3XXX_InsnWriteTTLIO,
+	}, {
+		.pc_DriverName		= "apci3116-16-4",
+		.i_VendorId		= PCI_VENDOR_ID_ADDIDATA,
+		.i_DeviceId		= 0x3022,
+		.i_IorangeBase0		= 256,
+		.i_IorangeBase1		= 256,
+		.i_IorangeBase2		= 256,
+		.i_IorangeBase3		= 256,
+		.i_PCIEeprom		= ADDIDATA_NO_EEPROM,
+		.pc_EepromChip		= ADDIDATA_9054,
+		.i_NbrAiChannel		= 16,
+		.i_NbrAiChannelDiff	= 8,
+		.i_AiChannelList	= 16,
+		.i_NbrAoChannel		= 4,
+		.i_AiMaxdata		= 65535,
+		.i_AoMaxdata		= 4095,
+		.pr_AiRangelist		= &range_apci3XXX_ai,
+		.pr_AoRangelist		= &range_apci3XXX_ao,
+		.i_NbrDiChannel		= 4,
+		.i_NbrDoChannel		= 4,
+		.i_DoMaxdata		= 1,
+		.i_NbrTTLChannel	= 24,
+		.pr_TTLRangelist	= &range_apci3XXX_ttl,
+		.b_AvailableConvertUnit	= 6,
+		.ui_MinAcquisitiontimeNs = 5000,
+		.interrupt		= v_APCI3XXX_Interrupt,
+		.reset			= i_APCI3XXX_Reset,
+		.ai_config		= i_APCI3XXX_InsnConfigAnalogInput,
+		.ai_read		= i_APCI3XXX_InsnReadAnalogInput,
+		.ao_write		= i_APCI3XXX_InsnWriteAnalogOutput,
+		.di_read		= i_APCI3XXX_InsnReadDigitalInput,
+		.di_bits		= i_APCI3XXX_InsnBitsDigitalInput,
+		.do_write		= i_APCI3XXX_InsnWriteDigitalOutput,
+		.do_bits		= i_APCI3XXX_InsnBitsDigitalOutput,
+		.do_read		= i_APCI3XXX_InsnReadDigitalOutput,
+		.ttl_config		= i_APCI3XXX_InsnConfigInitTTLIO,
+		.ttl_bits		= i_APCI3XXX_InsnBitsTTLIO,
+		.ttl_read		= i_APCI3XXX_InsnReadTTLIO,
+		.ttl_write		= i_APCI3XXX_InsnWriteTTLIO,
+	}, {
+		.pc_DriverName		= "apci3116-8-4",
+		.i_VendorId		= PCI_VENDOR_ID_ADDIDATA,
+		.i_DeviceId		= 0x3023,
+		.i_IorangeBase0		= 256,
+		.i_IorangeBase1		= 256,
+		.i_IorangeBase2		= 256,
+		.i_IorangeBase3		= 256,
+		.i_PCIEeprom		= ADDIDATA_NO_EEPROM,
+		.pc_EepromChip		= ADDIDATA_9054,
+		.i_NbrAiChannel		= 8,
+		.i_NbrAiChannelDiff	= 4,
+		.i_AiChannelList	= 8,
+		.i_NbrAoChannel		= 4,
+		.i_AiMaxdata		= 65535,
+		.i_AoMaxdata		= 4095,
+		.pr_AiRangelist		= &range_apci3XXX_ai,
+		.pr_AoRangelist		= &range_apci3XXX_ao,
+		.i_NbrDiChannel		= 4,
+		.i_NbrDoChannel		= 4,
+		.i_DoMaxdata		= 1,
+		.i_NbrTTLChannel	= 24,
+		.pr_TTLRangelist	= &range_apci3XXX_ttl,
+		.b_AvailableConvertUnit	= 6,
+		.ui_MinAcquisitiontimeNs = 5000,
+		.interrupt		= v_APCI3XXX_Interrupt,
+		.reset			= i_APCI3XXX_Reset,
+		.ai_config		= i_APCI3XXX_InsnConfigAnalogInput,
+		.ai_read		= i_APCI3XXX_InsnReadAnalogInput,
+		.ao_write		= i_APCI3XXX_InsnWriteAnalogOutput,
+		.di_read		= i_APCI3XXX_InsnReadDigitalInput,
+		.di_bits		= i_APCI3XXX_InsnBitsDigitalInput,
+		.do_write		= i_APCI3XXX_InsnWriteDigitalOutput,
+		.do_bits		= i_APCI3XXX_InsnBitsDigitalOutput,
+		.do_read		= i_APCI3XXX_InsnReadDigitalOutput,
+		.ttl_config		= i_APCI3XXX_InsnConfigInitTTLIO,
+		.ttl_bits		= i_APCI3XXX_InsnBitsTTLIO,
+		.ttl_read		= i_APCI3XXX_InsnReadTTLIO,
+		.ttl_write		= i_APCI3XXX_InsnWriteTTLIO,
+	}, {
+		.pc_DriverName		= "apci3003",
+		.i_VendorId		= PCI_VENDOR_ID_ADDIDATA,
+		.i_DeviceId		= 0x300B,
+		.i_IorangeBase0		= 256,
+		.i_IorangeBase1		= 256,
+		.i_IorangeBase2		= 256,
+		.i_IorangeBase3		= 256,
+		.i_PCIEeprom		= ADDIDATA_NO_EEPROM,
+		.pc_EepromChip		= ADDIDATA_9054,
+		.i_NbrAiChannelDiff	= 4,
+		.i_AiChannelList	= 4,
+		.i_AiMaxdata		= 65535,
+		.pr_AiRangelist		= &range_apci3XXX_ai,
+		.i_NbrDiChannel		= 4,
+		.i_NbrDoChannel		= 4,
+		.i_DoMaxdata		= 1,
+		.b_AvailableConvertUnit	= 7,
+		.ui_MinAcquisitiontimeNs = 2500,
+		.interrupt		= v_APCI3XXX_Interrupt,
+		.reset			= i_APCI3XXX_Reset,
+		.ai_config		= i_APCI3XXX_InsnConfigAnalogInput,
+		.ai_read		= i_APCI3XXX_InsnReadAnalogInput,
+		.di_read		= i_APCI3XXX_InsnReadDigitalInput,
+		.di_bits		= i_APCI3XXX_InsnBitsDigitalInput,
+		.do_write		= i_APCI3XXX_InsnWriteDigitalOutput,
+		.do_bits		= i_APCI3XXX_InsnBitsDigitalOutput,
+		.do_read		= i_APCI3XXX_InsnReadDigitalOutput,
+	}, {
+		.pc_DriverName		= "apci3002-16",
+		.i_VendorId		= PCI_VENDOR_ID_ADDIDATA,
+		.i_DeviceId		= 0x3002,
+		.i_IorangeBase0		= 256,
+		.i_IorangeBase1		= 256,
+		.i_IorangeBase2		= 256,
+		.i_IorangeBase3		= 256,
+		.i_PCIEeprom		= ADDIDATA_NO_EEPROM,
+		.pc_EepromChip		= ADDIDATA_9054,
+		.i_NbrAiChannelDiff	= 16,
+		.i_AiChannelList	= 16,
+		.i_AiMaxdata		= 65535,
+		.pr_AiRangelist		= &range_apci3XXX_ai,
+		.i_NbrDiChannel		= 4,
+		.i_NbrDoChannel		= 4,
+		.i_DoMaxdata		= 1,
+		.b_AvailableConvertUnit	= 6,
+		.ui_MinAcquisitiontimeNs = 5000,
+		.interrupt		= v_APCI3XXX_Interrupt,
+		.reset			= i_APCI3XXX_Reset,
+		.ai_config		= i_APCI3XXX_InsnConfigAnalogInput,
+		.ai_read		= i_APCI3XXX_InsnReadAnalogInput,
+		.di_read		= i_APCI3XXX_InsnReadDigitalInput,
+		.di_bits		= i_APCI3XXX_InsnBitsDigitalInput,
+		.do_write		= i_APCI3XXX_InsnWriteDigitalOutput,
+		.do_bits		= i_APCI3XXX_InsnBitsDigitalOutput,
+		.do_read		= i_APCI3XXX_InsnReadDigitalOutput,
+	}, {
+		.pc_DriverName		= "apci3002-8",
+		.i_VendorId		= PCI_VENDOR_ID_ADDIDATA,
+		.i_DeviceId		= 0x3003,
+		.i_IorangeBase0		= 256,
+		.i_IorangeBase1		= 256,
+		.i_IorangeBase2		= 256,
+		.i_IorangeBase3		= 256,
+		.i_PCIEeprom		= ADDIDATA_NO_EEPROM,
+		.pc_EepromChip		= ADDIDATA_9054,
+		.i_NbrAiChannelDiff	= 8,
+		.i_AiChannelList	= 8,
+		.i_AiMaxdata		= 65535,
+		.pr_AiRangelist		= &range_apci3XXX_ai,
+		.i_NbrDiChannel		= 4,
+		.i_NbrDoChannel		= 4,
+		.i_DoMaxdata		= 1,
+		.b_AvailableConvertUnit	= 6,
+		.ui_MinAcquisitiontimeNs = 5000,
+		.interrupt		= v_APCI3XXX_Interrupt,
+		.reset			= i_APCI3XXX_Reset,
+		.ai_config		= i_APCI3XXX_InsnConfigAnalogInput,
+		.ai_read		= i_APCI3XXX_InsnReadAnalogInput,
+		.di_read		= i_APCI3XXX_InsnReadDigitalInput,
+		.di_bits		= i_APCI3XXX_InsnBitsDigitalInput,
+		.do_write		= i_APCI3XXX_InsnWriteDigitalOutput,
+		.do_bits		= i_APCI3XXX_InsnBitsDigitalOutput,
+		.do_read		= i_APCI3XXX_InsnReadDigitalOutput,
+	}, {
+		.pc_DriverName		= "apci3002-4",
+		.i_VendorId		= PCI_VENDOR_ID_ADDIDATA,
+		.i_DeviceId		= 0x3004,
+		.i_IorangeBase0		= 256,
+		.i_IorangeBase1		= 256,
+		.i_IorangeBase2		= 256,
+		.i_IorangeBase3		= 256,
+		.i_PCIEeprom		= ADDIDATA_NO_EEPROM,
+		.pc_EepromChip		= ADDIDATA_9054,
+		.i_NbrAiChannelDiff	= 4,
+		.i_AiChannelList	= 4,
+		.i_AiMaxdata		= 65535,
+		.pr_AiRangelist		= &range_apci3XXX_ai,
+		.i_NbrDiChannel		= 4,
+		.i_NbrDoChannel		= 4,
+		.i_DoMaxdata		= 1,
+		.b_AvailableConvertUnit	= 6,
+		.ui_MinAcquisitiontimeNs = 5000,
+		.interrupt		= v_APCI3XXX_Interrupt,
+		.reset			= i_APCI3XXX_Reset,
+		.ai_config		= i_APCI3XXX_InsnConfigAnalogInput,
+		.ai_read		= i_APCI3XXX_InsnReadAnalogInput,
+		.di_read		= i_APCI3XXX_InsnReadDigitalInput,
+		.di_bits		= i_APCI3XXX_InsnBitsDigitalInput,
+		.do_write		= i_APCI3XXX_InsnWriteDigitalOutput,
+		.do_bits		= i_APCI3XXX_InsnBitsDigitalOutput,
+		.do_read		= i_APCI3XXX_InsnReadDigitalOutput,
+	}, {
+		.pc_DriverName		= "apci3500",
+		.i_VendorId		= PCI_VENDOR_ID_ADDIDATA,
+		.i_DeviceId		= 0x3024,
+		.i_IorangeBase0		= 256,
+		.i_IorangeBase1		= 256,
+		.i_IorangeBase2		= 256,
+		.i_IorangeBase3		= 256,
+		.i_PCIEeprom		= ADDIDATA_NO_EEPROM,
+		.pc_EepromChip		= ADDIDATA_9054,
+		.i_NbrAoChannel		= 4,
+		.i_AoMaxdata		= 4095,
+		.pr_AoRangelist		= &range_apci3XXX_ao,
+		.i_NbrTTLChannel	= 24,
+		.pr_TTLRangelist	= &range_apci3XXX_ttl,
+		.interrupt		= v_APCI3XXX_Interrupt,
+		.reset			= i_APCI3XXX_Reset,
+		.ao_write		= i_APCI3XXX_InsnWriteAnalogOutput,
+		.ttl_config		= i_APCI3XXX_InsnConfigInitTTLIO,
+		.ttl_bits		= i_APCI3XXX_InsnBitsTTLIO,
+		.ttl_read		= i_APCI3XXX_InsnReadTTLIO,
+		.ttl_write		= i_APCI3XXX_InsnWriteTTLIO,
+	},
 #endif
 };
 
-#define n_boardtypes (sizeof(boardtypes)/sizeof(struct addi_board))
-
 static struct comedi_driver driver_addi = {
 	.driver_name = ADDIDATA_DRIVER_NAME,
 	.module = THIS_MODULE,
 	.attach = i_ADDI_Attach,
 	.detach = i_ADDI_Detach,
-	.num_names = n_boardtypes,
+	.num_names = ARRAY_SIZE(boardtypes),
 	.board_name = &boardtypes[0].pc_DriverName,
 	.offset = sizeof(struct addi_board),
 };
@@ -2543,7 +1438,7 @@ static struct comedi_driver driver_addi = {
 static int __devinit driver_addi_pci_probe(struct pci_dev *dev,
 					   const struct pci_device_id *ent)
 {
-	return comedi_pci_auto_config(dev, driver_addi.driver_name);
+	return comedi_pci_auto_config(dev, &driver_addi);
 }
 
 static void __devexit driver_addi_pci_remove(struct pci_dev *dev)
@@ -2821,16 +1716,13 @@ static int i_ADDI_Attach(struct comedi_device *dev, struct comedi_devconfig *it)
 			/* Set the initialisation flag */
 			devpriv->b_AiInitialisation = 1;
 
-			s->insn_config =
-				this_board->i_hwdrv_InsnConfigAnalogInput;
-			s->insn_read = this_board->i_hwdrv_InsnReadAnalogInput;
-			s->insn_write =
-				this_board->i_hwdrv_InsnWriteAnalogInput;
-			s->insn_bits = this_board->i_hwdrv_InsnBitsAnalogInput;
-			s->do_cmdtest =
-				this_board->i_hwdrv_CommandTestAnalogInput;
-			s->do_cmd = this_board->i_hwdrv_CommandAnalogInput;
-			s->cancel = this_board->i_hwdrv_CancelAnalogInput;
+			s->insn_config = this_board->ai_config;
+			s->insn_read = this_board->ai_read;
+			s->insn_write = this_board->ai_write;
+			s->insn_bits = this_board->ai_bits;
+			s->do_cmdtest = this_board->ai_cmdtest;
+			s->do_cmd = this_board->ai_cmd;
+			s->cancel = this_board->ai_cancel;
 
 		} else {
 			s->type = COMEDI_SUBD_UNUSED;
@@ -2846,10 +1738,8 @@ static int i_ADDI_Attach(struct comedi_device *dev, struct comedi_devconfig *it)
 			s->len_chanlist =
 				devpriv->s_EeParameters.i_NbrAoChannel;
 			s->range_table = this_board->pr_AoRangelist;
-			s->insn_config =
-				this_board->i_hwdrv_InsnConfigAnalogOutput;
-			s->insn_write =
-				this_board->i_hwdrv_InsnWriteAnalogOutput;
+			s->insn_config = this_board->ao_config;
+			s->insn_write = this_board->ao_write;
 		} else {
 			s->type = COMEDI_SUBD_UNUSED;
 		}
@@ -2864,12 +1754,10 @@ static int i_ADDI_Attach(struct comedi_device *dev, struct comedi_devconfig *it)
 				devpriv->s_EeParameters.i_NbrDiChannel;
 			s->range_table = &range_digital;
 			s->io_bits = 0;	/* all bits input */
-			s->insn_config =
-				this_board->i_hwdrv_InsnConfigDigitalInput;
-			s->insn_read = this_board->i_hwdrv_InsnReadDigitalInput;
-			s->insn_write =
-				this_board->i_hwdrv_InsnWriteDigitalInput;
-			s->insn_bits = this_board->i_hwdrv_InsnBitsDigitalInput;
+			s->insn_config = this_board->di_config;
+			s->insn_read = this_board->di_read;
+			s->insn_write = this_board->di_write;
+			s->insn_bits = this_board->di_bits;
 		} else {
 			s->type = COMEDI_SUBD_UNUSED;
 		}
@@ -2886,13 +1774,11 @@ static int i_ADDI_Attach(struct comedi_device *dev, struct comedi_devconfig *it)
 			s->range_table = &range_digital;
 			s->io_bits = 0xf;	/* all bits output */
 
-			s->insn_config = this_board->i_hwdrv_InsnConfigDigitalOutput;	/* for digital output memory.. */
-			s->insn_write =
-				this_board->i_hwdrv_InsnWriteDigitalOutput;
-			s->insn_bits =
-				this_board->i_hwdrv_InsnBitsDigitalOutput;
-			s->insn_read =
-				this_board->i_hwdrv_InsnReadDigitalOutput;
+			/* insn_config - for digital output memory */
+			s->insn_config = this_board->do_config;
+			s->insn_write = this_board->do_write;
+			s->insn_bits = this_board->do_bits;
+			s->insn_read = this_board->do_read;
 		} else {
 			s->type = COMEDI_SUBD_UNUSED;
 		}
@@ -2907,10 +1793,10 @@ static int i_ADDI_Attach(struct comedi_device *dev, struct comedi_devconfig *it)
 			s->len_chanlist = 1;
 			s->range_table = &range_digital;
 
-			s->insn_write = this_board->i_hwdrv_InsnWriteTimer;
-			s->insn_read = this_board->i_hwdrv_InsnReadTimer;
-			s->insn_config = this_board->i_hwdrv_InsnConfigTimer;
-			s->insn_bits = this_board->i_hwdrv_InsnBitsTimer;
+			s->insn_write = this_board->timer_write;
+			s->insn_read = this_board->timer_read;
+			s->insn_config = this_board->timer_config;
+			s->insn_bits = this_board->timer_bits;
 		} else {
 			s->type = COMEDI_SUBD_UNUSED;
 		}
@@ -2926,10 +1812,10 @@ static int i_ADDI_Attach(struct comedi_device *dev, struct comedi_devconfig *it)
 			s->io_bits = 0;	/* all bits input */
 			s->len_chanlist = this_board->i_NbrTTLChannel;
 			s->range_table = &range_digital;
-			s->insn_config = this_board->i_hwdr_ConfigInitTTLIO;
-			s->insn_bits = this_board->i_hwdr_ReadTTLIOBits;
-			s->insn_read = this_board->i_hwdr_ReadTTLIOAllPortValue;
-			s->insn_write = this_board->i_hwdr_WriteTTLIOChlOnOff;
+			s->insn_config = this_board->ttl_config;
+			s->insn_bits = this_board->ttl_bits;
+			s->insn_read = this_board->ttl_read;
+			s->insn_write = this_board->ttl_write;
 		} else {
 			s->type = COMEDI_SUBD_UNUSED;
 		}
@@ -2953,50 +1839,22 @@ static int i_ADDI_Attach(struct comedi_device *dev, struct comedi_devconfig *it)
 	return 0;
 }
 
-/*
-+----------------------------------------------------------------------------+
-| Function name     : static int i_ADDI_Detach(struct comedi_device *dev)           |
-|                                        									 |
-|                                            						         |
-+----------------------------------------------------------------------------+
-| Task              : Deallocates resources of the addi_common driver        |
-|			  Free the DMA buffers, unregister irq.				     |
-|                     										                 |
-+----------------------------------------------------------------------------+
-| Input Parameters  : struct comedi_device *dev									 |
-|                     														 |
-|                                                 					         |
-+----------------------------------------------------------------------------+
-| Return Value      : 0             					                     |
-|                    													     |
-+----------------------------------------------------------------------------+
-*/
-
-static int i_ADDI_Detach(struct comedi_device *dev)
+static void i_ADDI_Detach(struct comedi_device *dev)
 {
-
 	if (dev->private) {
-		if (devpriv->b_ValidDriver) {
+		if (devpriv->b_ValidDriver)
 			i_ADDI_Reset(dev);
-		}
-
-		if (dev->irq) {
+		if (dev->irq)
 			free_irq(dev->irq, dev);
-		}
-
-		if ((this_board->pc_EepromChip == NULL)
-			|| (strcmp(this_board->pc_EepromChip,
-					ADDIDATA_9054) != 0)) {
-			if (devpriv->allocated) {
+		if ((this_board->pc_EepromChip == NULL) ||
+		    (strcmp(this_board->pc_EepromChip, ADDIDATA_9054) != 0)) {
+			if (devpriv->allocated)
 				i_pci_card_free(devpriv->amcc);
-			}
-
 			if (devpriv->ul_DmaBufferVirtual[0]) {
 				free_pages((unsigned long)devpriv->
 					ul_DmaBufferVirtual[0],
 					devpriv->ui_DmaBufferPages[0]);
 			}
-
 			if (devpriv->ul_DmaBufferVirtual[1]) {
 				free_pages((unsigned long)devpriv->
 					ul_DmaBufferVirtual[1],
@@ -3004,20 +1862,14 @@ static int i_ADDI_Detach(struct comedi_device *dev)
 			}
 		} else {
 			iounmap(devpriv->dw_AiBase);
-
-			if (devpriv->allocated) {
+			if (devpriv->allocated)
 				i_pci_card_free(devpriv->amcc);
-			}
 		}
-
 		if (pci_list_builded) {
-			/* v_pci_card_list_cleanup(PCI_VENDOR_ID_AMCC); */
 			v_pci_card_list_cleanup(this_board->i_VendorId);
 			pci_list_builded = 0;
 		}
 	}
-
-	return 0;
 }
 
 /*
@@ -3041,7 +1893,7 @@ static int i_ADDI_Detach(struct comedi_device *dev)
 static int i_ADDI_Reset(struct comedi_device *dev)
 {
 
-	this_board->i_hwdrv_Reset(dev);
+	this_board->reset(dev);
 	return 0;
 }
 
@@ -3067,7 +1919,7 @@ static int i_ADDI_Reset(struct comedi_device *dev)
 static irqreturn_t v_ADDI_Interrupt(int irq, void *d)
 {
 	struct comedi_device *dev = d;
-	this_board->v_hwdrv_Interrupt(irq, d);
+	this_board->interrupt(irq, d);
 	return IRQ_RETVAL(1);
 }
 
diff --git a/drivers/staging/comedi/drivers/addi-data/addi_common.h b/drivers/staging/comedi/drivers/addi-data/addi_common.h
index c6980b7dfea..2c3f34703dd 100644
--- a/drivers/staging/comedi/drivers/addi-data/addi_common.h
+++ b/drivers/staging/comedi/drivers/addi-data/addi_common.h
@@ -94,111 +94,72 @@ struct addi_board {
 	unsigned int ui_MinDelaytimeNs;	/*  Minimum Delay in Nano secs */
 
 	/* interrupt and reset */
-	void (*v_hwdrv_Interrupt)(int irq, void *d);
-	int (*i_hwdrv_Reset)(struct comedi_device *dev);
+	void (*interrupt)(int irq, void *d);
+	int (*reset)(struct comedi_device *);
 
 	/* Subdevice functions */
 
 	/* ANALOG INPUT */
-	int (*i_hwdrv_InsnConfigAnalogInput)(struct comedi_device *dev,
-					     struct comedi_subdevice *s,
-					     struct comedi_insn *insn,
-					     unsigned int *data);
-	int (*i_hwdrv_InsnReadAnalogInput)(struct comedi_device *dev,
-					    struct comedi_subdevice *s,
-					    struct comedi_insn *insn,
-					    unsigned int *data);
-	int (*i_hwdrv_InsnWriteAnalogInput)(struct comedi_device *dev,
-					    struct comedi_subdevice *s,
-					    struct comedi_insn *insn,
-					    unsigned int *data);
-	int (*i_hwdrv_InsnBitsAnalogInput)(struct comedi_device *dev,
-					   struct comedi_subdevice *s,
-					   struct comedi_insn *insn,
-					   unsigned int *data);
-	int (*i_hwdrv_CommandTestAnalogInput)(struct comedi_device *dev,
-					      struct comedi_subdevice *s,
-					      struct comedi_cmd *cmd);
-	int (*i_hwdrv_CommandAnalogInput)(struct comedi_device *dev,
-					  struct comedi_subdevice *s);
-	int (*i_hwdrv_CancelAnalogInput)(struct comedi_device *dev,
-					 struct comedi_subdevice *s);
+	int (*ai_config)(struct comedi_device *, struct comedi_subdevice *,
+			 struct comedi_insn *, unsigned int *);
+	int (*ai_read)(struct comedi_device *, struct comedi_subdevice *,
+		       struct comedi_insn *, unsigned int *);
+	int (*ai_write)(struct comedi_device *, struct comedi_subdevice *,
+			struct comedi_insn *, unsigned int *);
+	int (*ai_bits)(struct comedi_device *, struct comedi_subdevice *,
+		       struct comedi_insn *, unsigned int *);
+	int (*ai_cmdtest)(struct comedi_device *, struct comedi_subdevice *,
+			  struct comedi_cmd *);
+	int (*ai_cmd)(struct comedi_device *, struct comedi_subdevice *);
+	int (*ai_cancel)(struct comedi_device *, struct comedi_subdevice *);
 
 	/* Analog Output */
-	int (*i_hwdrv_InsnConfigAnalogOutput)(struct comedi_device *dev,
-					      struct comedi_subdevice *s,
-					      struct comedi_insn *insn,
-					      unsigned int *data);
-	int (*i_hwdrv_InsnWriteAnalogOutput)(struct comedi_device *dev,
-					     struct comedi_subdevice *s,
-					     struct comedi_insn *insn,
-					     unsigned int *data);
-	int (*i_hwdrv_InsnBitsAnalogOutput)(struct comedi_device *dev,
-					    struct comedi_subdevice *s,
-					    struct comedi_insn *insn,
-					    unsigned int *data);
+	int (*ao_config)(struct comedi_device *, struct comedi_subdevice *,
+			 struct comedi_insn *, unsigned int *);
+	int (*ao_write)(struct comedi_device *, struct comedi_subdevice *,
+			struct comedi_insn *, unsigned int *);
+	int (*ao_bits)(struct comedi_device *, struct comedi_subdevice *,
+		       struct comedi_insn *, unsigned int *);
 
 	/* Digital Input */
-	int (*i_hwdrv_InsnConfigDigitalInput) (struct comedi_device *dev,
-					       struct comedi_subdevice *s,
-					       struct comedi_insn *insn,
-					       unsigned int *data);
-	int (*i_hwdrv_InsnReadDigitalInput) (struct comedi_device *dev,
-					     struct comedi_subdevice *s,
-					     struct comedi_insn *insn,
-					     unsigned int *data);
-	int (*i_hwdrv_InsnWriteDigitalInput) (struct comedi_device *dev,
-					      struct comedi_subdevice *s,
-					      struct comedi_insn *insn,
-					      unsigned int *data);
-	int (*i_hwdrv_InsnBitsDigitalInput) (struct comedi_device *dev,
-					     struct comedi_subdevice *s,
-					     struct comedi_insn *insn,
-					     unsigned int *data);
+	int (*di_config)(struct comedi_device *, struct comedi_subdevice *,
+			 struct comedi_insn *, unsigned int *);
+	int (*di_read)(struct comedi_device *, struct comedi_subdevice *,
+		       struct comedi_insn *, unsigned int *);
+	int (*di_write)(struct comedi_device *, struct comedi_subdevice *,
+			struct comedi_insn *, unsigned int *);
+	int (*di_bits)(struct comedi_device *, struct comedi_subdevice *,
+		       struct comedi_insn *, unsigned int *);
 
 	/* Digital Output */
-	int (*i_hwdrv_InsnConfigDigitalOutput)(struct comedi_device *dev,
-					       struct comedi_subdevice *s,
-					       struct comedi_insn *insn,
-					       unsigned int *data);
-	int (*i_hwdrv_InsnWriteDigitalOutput)(struct comedi_device *dev,
-					      struct comedi_subdevice *s,
-					      struct comedi_insn *insn,
-					      unsigned int *data);
-	int (*i_hwdrv_InsnBitsDigitalOutput)(struct comedi_device *dev,
-					     struct comedi_subdevice *s,
-					     struct comedi_insn *insn,
-					     unsigned int *data);
-	int (*i_hwdrv_InsnReadDigitalOutput)(struct comedi_device *dev,
-					     struct comedi_subdevice *s,
-					     struct comedi_insn *insn,
-					     unsigned int *data);
+	int (*do_config)(struct comedi_device *, struct comedi_subdevice *,
+			 struct comedi_insn *, unsigned int *);
+	int (*do_write)(struct comedi_device *, struct comedi_subdevice *,
+			struct comedi_insn *, unsigned int *);
+	int (*do_bits)(struct comedi_device *, struct comedi_subdevice *,
+		       struct comedi_insn *, unsigned int *);
+	int (*do_read)(struct comedi_device *, struct comedi_subdevice *,
+		       struct comedi_insn *, unsigned int *);
 
 	/* TIMER */
-	int (*i_hwdrv_InsnConfigTimer)(struct comedi_device *dev,
-				       struct comedi_subdevice *s,
-				       struct comedi_insn *insn, unsigned int *data);
-	int (*i_hwdrv_InsnWriteTimer)(struct comedi_device *dev,
-				      struct comedi_subdevice *s, struct comedi_insn *insn,
-				      unsigned int *data);
-	int (*i_hwdrv_InsnReadTimer)(struct comedi_device *dev, struct comedi_subdevice *s,
-				     struct comedi_insn *insn, unsigned int *data);
-	int (*i_hwdrv_InsnBitsTimer)(struct comedi_device *dev, struct comedi_subdevice *s,
-				     struct comedi_insn *insn, unsigned int *data);
+	int (*timer_config)(struct comedi_device *, struct comedi_subdevice *,
+			    struct comedi_insn *, unsigned int *);
+	int (*timer_write)(struct comedi_device *, struct comedi_subdevice *,
+			   struct comedi_insn *, unsigned int *);
+	int (*timer_read)(struct comedi_device *, struct comedi_subdevice *,
+			  struct comedi_insn *, unsigned int *);
+	int (*timer_bits)(struct comedi_device *, struct comedi_subdevice *,
+			  struct comedi_insn *, unsigned int *);
 
 	/* TTL IO */
-	int (*i_hwdr_ConfigInitTTLIO)(struct comedi_device *dev,
-				      struct comedi_subdevice *s, struct comedi_insn *insn,
-				      unsigned int *data);
-	int (*i_hwdr_ReadTTLIOBits)(struct comedi_device *dev, struct comedi_subdevice *s,
-				    struct comedi_insn *insn, unsigned int *data);
-	int (*i_hwdr_ReadTTLIOAllPortValue)(struct comedi_device *dev,
-					    struct comedi_subdevice *s,
-					    struct comedi_insn *insn,
-					    unsigned int *data);
-	int (*i_hwdr_WriteTTLIOChlOnOff)(struct comedi_device *dev,
-					 struct comedi_subdevice *s,
-					 struct comedi_insn *insn, unsigned int *data);
+	int (*ttl_config)(struct comedi_device *, struct comedi_subdevice *,
+			  struct comedi_insn *, unsigned int *);
+	int (*ttl_bits)(struct comedi_device *, struct comedi_subdevice *,
+			struct comedi_insn *, unsigned int *);
+	int (*ttl_read)(struct comedi_device *, struct comedi_subdevice *,
+			struct comedi_insn *, unsigned int *);
+	int (*ttl_write)(struct comedi_device *, struct comedi_subdevice *,
+			 struct comedi_insn *, unsigned int *);
 };
 
 /* MODULE INFO STRUCTURE */
@@ -455,7 +416,7 @@ static unsigned short pci_list_builded;	/* set to 1 when list of card is known *
 
 /* Function declarations */
 static int i_ADDI_Attach(struct comedi_device *dev, struct comedi_devconfig *it);
-static int i_ADDI_Detach(struct comedi_device *dev);
+static void i_ADDI_Detach(struct comedi_device *dev);
 static int i_ADDI_Reset(struct comedi_device *dev);
 
 static irqreturn_t v_ADDI_Interrupt(int irq, void *d);
diff --git a/drivers/staging/comedi/drivers/addi-data/hwdrv_apci3120.c b/drivers/staging/comedi/drivers/addi-data/hwdrv_apci3120.c
index e886ced4978..ffe390c6da8 100644
--- a/drivers/staging/comedi/drivers/addi-data/hwdrv_apci3120.c
+++ b/drivers/staging/comedi/drivers/addi-data/hwdrv_apci3120.c
@@ -156,7 +156,7 @@ int i_APCI3120_InsnReadAnalogInput(struct comedi_device *dev, struct comedi_subd
 	} else
 		us_ConvertTiming = (unsigned short) (devpriv->ui_EocEosConversionTime / 1000);	/*  nano to useconds */
 
-	/*  this_board->i_hwdrv_InsnReadAnalogInput(dev,us_ConvertTiming,insn->n,&insn->chanspec,data,insn->unused[0]); */
+	/*  this_board->ai_read(dev,us_ConvertTiming,insn->n,&insn->chanspec,data,insn->unused[0]); */
 
 	/*  Clear software registers */
 	devpriv->b_TimerSelectMode = 0;
@@ -670,7 +670,7 @@ int i_APCI3120_CommandAnalogInput(struct comedi_device *dev, struct comedi_subde
 			/*  mode 1 */
 
 			devpriv->ui_AiTimer0 = cmd->convert_arg;	/*  timer constant in nano seconds */
-			/* return this_board->i_hwdrv_CommandAnalogInput(1,dev,s); */
+			/* return this_board->ai_cmd(1,dev,s); */
 			return i_APCI3120_CyclicAnalogInput(1, dev, s);
 		}
 
@@ -680,7 +680,7 @@ int i_APCI3120_CommandAnalogInput(struct comedi_device *dev, struct comedi_subde
 		/*  mode 2 */
 		devpriv->ui_AiTimer1 = cmd->scan_begin_arg;
 		devpriv->ui_AiTimer0 = cmd->convert_arg;	/*  variable changed timer2 to timer0 */
-		/* return this_board->i_hwdrv_CommandAnalogInput(2,dev,s); */
+		/* return this_board->ai_cmd(2,dev,s); */
 		return i_APCI3120_CyclicAnalogInput(2, dev, s);
 	}
 	return -1;
@@ -1922,7 +1922,7 @@ int i_APCI3120_InsnConfigTimer(struct comedi_device *dev, struct comedi_subdevic
 
 	ui_Timervalue2 = data[1] / 1000;	/*  convert nano seconds  to u seconds */
 
-	/* this_board->i_hwdrv_InsnConfigTimer(dev, ui_Timervalue2,(unsigned char)data[0]); */
+	/* this_board->timer_config(dev, ui_Timervalue2,(unsigned char)data[0]); */
 	us_TmpValue = (unsigned short) inw(devpriv->iobase + APCI3120_RD_STATUS);
 
 /*
@@ -2092,7 +2092,7 @@ int i_APCI3120_InsnWriteTimer(struct comedi_device *dev, struct comedi_subdevice
 			ui_Timervalue2 = 0;
 	}
 
-	/* this_board->i_hwdrv_InsnWriteTimer(dev,data[0],ui_Timervalue2); */
+	/* this_board->timer_write(dev,data[0],ui_Timervalue2); */
 
 	switch (data[0]) {
 	case APCI3120_START:
@@ -2260,7 +2260,7 @@ int i_APCI3120_InsnReadTimer(struct comedi_device *dev, struct comedi_subdevice
 		comedi_error(dev, "\nread:timer2  not configured ");
 	}
 
-	/* this_board->i_hwdrv_InsnReadTimer(dev,data); */
+	/* this_board->timer_read(dev,data); */
 	if (devpriv->b_Timer2Mode == APCI3120_TIMER) {
 
 		/* Read the LOW unsigned short of Timer 2 register */
@@ -2331,7 +2331,7 @@ int i_APCI3120_InsnReadDigitalInput(struct comedi_device *dev,
 
 	ui_Chan = CR_CHAN(insn->chanspec);	/*  channel specified */
 
-	/* this_board->i_hwdrv_InsnReadDigitalInput(dev,ui_Chan,data); */
+	/* this_board->di_read(dev,ui_Chan,data); */
 	if (ui_Chan <= 3) {
 		ui_TmpValue = (unsigned int) inw(devpriv->iobase + APCI3120_RD_STATUS);
 
@@ -2379,7 +2379,7 @@ int i_APCI3120_InsnBitsDigitalInput(struct comedi_device *dev, struct comedi_sub
 	*****/
 
 	*data = (ui_TmpValue >> 8) & 0xf;
-	/* this_board->i_hwdrv_InsnBitsDigitalInput(dev,data); */
+	/* this_board->di_bits(dev,data); */
 	return insn->n;
 }
 
@@ -2595,7 +2595,7 @@ int i_APCI3120_InsnWriteAnalogOutput(struct comedi_device *dev,
 	ui_Range = CR_RANGE(insn->chanspec);
 	ui_Channel = CR_CHAN(insn->chanspec);
 
-	/* this_board->i_hwdrv_InsnWriteAnalogOutput(dev, ui_Range, ui_Channel,data[0]); */
+	/* this_board->ao_write(dev, ui_Range, ui_Channel,data[0]); */
 	if (ui_Range) {		/*  if 1 then unipolar */
 
 		if (data[0] != 0)
diff --git a/drivers/staging/comedi/drivers/adl_pci6208.c b/drivers/staging/comedi/drivers/adl_pci6208.c
index 4fc9e852021..de8c68af321 100644
--- a/drivers/staging/comedi/drivers/adl_pci6208.c
+++ b/drivers/staging/comedi/drivers/adl_pci6208.c
@@ -54,8 +54,6 @@ References:
 #include "../comedidev.h"
 #include "comedi_pci.h"
 
-#define PCI6208_DRIVER_NAME	"adl_pci6208"
-
 /* Board descriptions */
 struct pci6208_board {
 	const char *name;
@@ -85,17 +83,6 @@ static const struct pci6208_board pci6208_boards[] = {
 	 }
 };
 
-/* This is used by modprobe to translate PCI IDs to drivers.  Should
- * only be used for PCI and ISA-PnP devices */
-static DEFINE_PCI_DEVICE_TABLE(pci6208_pci_table) = {
-	/* { PCI_VENDOR_ID_ADLINK, 0x6208, PCI_ANY_ID, PCI_ANY_ID, 0, 0, 0 }, */
-	/* { PCI_VENDOR_ID_ADLINK, 0x6208, PCI_ANY_ID, PCI_ANY_ID, 0, 0, 0 }, */
-	{ PCI_DEVICE(PCI_VENDOR_ID_ADLINK, 0x6208) },
-	{ 0 }
-};
-
-MODULE_DEVICE_TABLE(pci, pci6208_pci_table);
-
 /* Will be initialized in pci6208_find device(). */
 #define thisboard ((const struct pci6208_board *)dev->board_ptr)
 
@@ -107,157 +94,6 @@ struct pci6208_private {
 
 #define devpriv ((struct pci6208_private *)dev->private)
 
-static int pci6208_attach(struct comedi_device *dev,
-			  struct comedi_devconfig *it);
-static int pci6208_detach(struct comedi_device *dev);
-
-static struct comedi_driver driver_pci6208 = {
-	.driver_name = PCI6208_DRIVER_NAME,
-	.module = THIS_MODULE,
-	.attach = pci6208_attach,
-	.detach = pci6208_detach,
-};
-
-static int __devinit driver_pci6208_pci_probe(struct pci_dev *dev,
-					      const struct pci_device_id *ent)
-{
-	return comedi_pci_auto_config(dev, driver_pci6208.driver_name);
-}
-
-static void __devexit driver_pci6208_pci_remove(struct pci_dev *dev)
-{
-	comedi_pci_auto_unconfig(dev);
-}
-
-static struct pci_driver driver_pci6208_pci_driver = {
-	.id_table = pci6208_pci_table,
-	.probe = &driver_pci6208_pci_probe,
-	.remove = __devexit_p(&driver_pci6208_pci_remove)
-};
-
-static int __init driver_pci6208_init_module(void)
-{
-	int retval;
-
-	retval = comedi_driver_register(&driver_pci6208);
-	if (retval < 0)
-		return retval;
-
-	driver_pci6208_pci_driver.name = (char *)driver_pci6208.driver_name;
-	return pci_register_driver(&driver_pci6208_pci_driver);
-}
-
-static void __exit driver_pci6208_cleanup_module(void)
-{
-	pci_unregister_driver(&driver_pci6208_pci_driver);
-	comedi_driver_unregister(&driver_pci6208);
-}
-
-module_init(driver_pci6208_init_module);
-module_exit(driver_pci6208_cleanup_module);
-
-static int pci6208_find_device(struct comedi_device *dev, int bus, int slot);
-static int
-pci6208_pci_setup(struct pci_dev *pci_dev, unsigned long *io_base_ptr,
-		  int dev_minor);
-
-/*read/write functions*/
-static int pci6208_ao_winsn(struct comedi_device *dev,
-			    struct comedi_subdevice *s,
-			    struct comedi_insn *insn, unsigned int *data);
-static int pci6208_ao_rinsn(struct comedi_device *dev,
-			    struct comedi_subdevice *s,
-			    struct comedi_insn *insn, unsigned int *data);
-/* static int pci6208_dio_insn_bits (struct comedi_device *dev,
- *					struct comedi_subdevice *s, */
-/* struct comedi_insn *insn,unsigned int *data); */
-/* static int pci6208_dio_insn_config(struct comedi_device *dev,
- *					struct comedi_subdevice *s, */
-/* struct comedi_insn *insn,unsigned int *data); */
-
-/*
- * Attach is called by the Comedi core to configure the driver
- * for a particular board.  If you specified a board_name array
- * in the driver structure, dev->board_ptr contains that
- * address.
- */
-static int pci6208_attach(struct comedi_device *dev,
-			  struct comedi_devconfig *it)
-{
-	struct comedi_subdevice *s;
-	int retval;
-	unsigned long io_base;
-
-	printk(KERN_INFO "comedi%d: pci6208: ", dev->minor);
-
-	retval = alloc_private(dev, sizeof(struct pci6208_private));
-	if (retval < 0)
-		return retval;
-
-	retval = pci6208_find_device(dev, it->options[0], it->options[1]);
-	if (retval < 0)
-		return retval;
-
-	retval = pci6208_pci_setup(devpriv->pci_dev, &io_base, dev->minor);
-	if (retval < 0)
-		return retval;
-
-	dev->iobase = io_base;
-	dev->board_name = thisboard->name;
-
-/*
- * Allocate the subdevice structures.  alloc_subdevice() is a
- * convenient macro defined in comedidev.h.
- */
-	if (alloc_subdevices(dev, 2) < 0)
-		return -ENOMEM;
-
-	s = dev->subdevices + 0;
-	/* analog output subdevice */
-	s->type = COMEDI_SUBD_AO;
-	s->subdev_flags = SDF_WRITABLE;	/* anything else to add here?? */
-	s->n_chan = thisboard->ao_chans;
-	s->maxdata = 0xffff;	/* 16-bit DAC */
-	s->range_table = &range_bipolar10;	/* this needs to be checked. */
-	s->insn_write = pci6208_ao_winsn;
-	s->insn_read = pci6208_ao_rinsn;
-
-	/* s=dev->subdevices+1; */
-	/* digital i/o subdevice */
-	/* s->type=COMEDI_SUBD_DIO; */
-	/* s->subdev_flags=SDF_READABLE|SDF_WRITABLE; */
-	/* s->n_chan=16; */
-	/* s->maxdata=1; */
-	/* s->range_table=&range_digital; */
-	/* s->insn_bits = pci6208_dio_insn_bits; */
-	/* s->insn_config = pci6208_dio_insn_config; */
-
-	printk(KERN_INFO "attached\n");
-
-	return 1;
-}
-
-/*
- * _detach is called to deconfigure a device.  It should deallocate
- * resources.
- * This function is also called when _attach() fails, so it should be
- * careful not to release resources that were not necessarily
- * allocated by _attach().  dev->private and dev->subdevices are
- * deallocated automatically by the core.
- */
-static int pci6208_detach(struct comedi_device *dev)
-{
-	printk(KERN_INFO "comedi%d: pci6208: remove\n", dev->minor);
-
-	if (devpriv && devpriv->pci_dev) {
-		if (dev->iobase)
-			comedi_pci_disable(devpriv->pci_dev);
-		pci_dev_put(devpriv->pci_dev);
-	}
-
-	return 0;
-}
-
 static int pci6208_ao_winsn(struct comedi_device *dev,
 			    struct comedi_subdevice *s,
 			    struct comedi_insn *insn, unsigned int *data)
@@ -410,7 +246,7 @@ pci6208_pci_setup(struct pci_dev *pci_dev, unsigned long *io_base_ptr,
 	unsigned long io_base, io_range, lcr_io_base, lcr_io_range;
 
 	/*  Enable PCI device and request regions */
-	if (comedi_pci_enable(pci_dev, PCI6208_DRIVER_NAME) < 0) {
+	if (comedi_pci_enable(pci_dev, "adl_pci6208") < 0) {
 		printk(KERN_ERR "comedi%d: Failed to enable PCI device "
 			"and request regions\n",
 			dev_minor);
@@ -442,6 +278,103 @@ pci6208_pci_setup(struct pci_dev *pci_dev, unsigned long *io_base_ptr,
 	return 0;
 }
 
+static int pci6208_attach(struct comedi_device *dev,
+			  struct comedi_devconfig *it)
+{
+	struct comedi_subdevice *s;
+	int retval;
+	unsigned long io_base;
+
+	printk(KERN_INFO "comedi%d: pci6208: ", dev->minor);
+
+	retval = alloc_private(dev, sizeof(struct pci6208_private));
+	if (retval < 0)
+		return retval;
+
+	retval = pci6208_find_device(dev, it->options[0], it->options[1]);
+	if (retval < 0)
+		return retval;
+
+	retval = pci6208_pci_setup(devpriv->pci_dev, &io_base, dev->minor);
+	if (retval < 0)
+		return retval;
+
+	dev->iobase = io_base;
+	dev->board_name = thisboard->name;
+
+	if (alloc_subdevices(dev, 2) < 0)
+		return -ENOMEM;
+
+	s = dev->subdevices + 0;
+	/* analog output subdevice */
+	s->type = COMEDI_SUBD_AO;
+	s->subdev_flags = SDF_WRITABLE;	/* anything else to add here?? */
+	s->n_chan = thisboard->ao_chans;
+	s->maxdata = 0xffff;	/* 16-bit DAC */
+	s->range_table = &range_bipolar10;	/* this needs to be checked. */
+	s->insn_write = pci6208_ao_winsn;
+	s->insn_read = pci6208_ao_rinsn;
+
+	/* s=dev->subdevices+1; */
+	/* digital i/o subdevice */
+	/* s->type=COMEDI_SUBD_DIO; */
+	/* s->subdev_flags=SDF_READABLE|SDF_WRITABLE; */
+	/* s->n_chan=16; */
+	/* s->maxdata=1; */
+	/* s->range_table=&range_digital; */
+	/* s->insn_bits = pci6208_dio_insn_bits; */
+	/* s->insn_config = pci6208_dio_insn_config; */
+
+	printk(KERN_INFO "attached\n");
+
+	return 1;
+}
+
+static void pci6208_detach(struct comedi_device *dev)
+{
+	if (devpriv && devpriv->pci_dev) {
+		if (dev->iobase)
+			comedi_pci_disable(devpriv->pci_dev);
+		pci_dev_put(devpriv->pci_dev);
+	}
+}
+
+static struct comedi_driver adl_pci6208_driver = {
+	.driver_name	= "adl_pci6208",
+	.module		= THIS_MODULE,
+	.attach		= pci6208_attach,
+	.detach		= pci6208_detach,
+};
+
+static int __devinit adl_pci6208_pci_probe(struct pci_dev *dev,
+					   const struct pci_device_id *ent)
+{
+	return comedi_pci_auto_config(dev, &adl_pci6208_driver);
+}
+
+static void __devexit adl_pci6208_pci_remove(struct pci_dev *dev)
+{
+	comedi_pci_auto_unconfig(dev);
+}
+
+/* This is used by modprobe to translate PCI IDs to drivers.  Should
+ * only be used for PCI and ISA-PnP devices */
+static DEFINE_PCI_DEVICE_TABLE(adl_pci6208_pci_table) = {
+	/* { PCI_VENDOR_ID_ADLINK, 0x6208, PCI_ANY_ID, PCI_ANY_ID, 0, 0, 0 }, */
+	/* { PCI_VENDOR_ID_ADLINK, 0x6208, PCI_ANY_ID, PCI_ANY_ID, 0, 0, 0 }, */
+	{ PCI_DEVICE(PCI_VENDOR_ID_ADLINK, 0x6208) },
+	{ 0 }
+};
+MODULE_DEVICE_TABLE(pci, adl_pci6208_pci_table);
+
+static struct pci_driver adl_pci6208_pci_driver = {
+	.name		= "adl_pci6208",
+	.id_table	= adl_pci6208_pci_table,
+	.probe		= adl_pci6208_pci_probe,
+	.remove		= __devexit_p(adl_pci6208_pci_remove),
+};
+module_comedi_pci_driver(adl_pci6208_driver, adl_pci6208_pci_driver);
+
 MODULE_AUTHOR("Comedi http://www.comedi.org");
 MODULE_DESCRIPTION("Comedi low-level driver");
 MODULE_LICENSE("GPL");
diff --git a/drivers/staging/comedi/drivers/adl_pci7230.c b/drivers/staging/comedi/drivers/adl_pci7230.c
index 20d570554fa..e8053bc825f 100644
--- a/drivers/staging/comedi/drivers/adl_pci7230.c
+++ b/drivers/staging/comedi/drivers/adl_pci7230.c
@@ -43,13 +43,6 @@ Configuration Options:
 
 #define PCI_DEVICE_ID_PCI7230 0x7230
 
-static DEFINE_PCI_DEVICE_TABLE(adl_pci7230_pci_table) = {
-	{ PCI_DEVICE(PCI_VENDOR_ID_ADLINK, PCI_DEVICE_ID_PCI7230) },
-	{0}
-};
-
-MODULE_DEVICE_TABLE(pci, adl_pci7230_pci_table);
-
 struct adl_pci7230_private {
 	int data;
 	struct pci_dev *pci_dev;
@@ -57,27 +50,36 @@ struct adl_pci7230_private {
 
 #define devpriv ((struct adl_pci7230_private *)dev->private)
 
-static int adl_pci7230_attach(struct comedi_device *dev,
-	struct comedi_devconfig *it);
-static int adl_pci7230_detach(struct comedi_device *dev);
-static struct comedi_driver driver_adl_pci7230 = {
-	.driver_name = "adl_pci7230",
-	.module = THIS_MODULE,
-	.attach = adl_pci7230_attach,
-	.detach = adl_pci7230_detach,
-};
+static int adl_pci7230_do_insn_bits(struct comedi_device *dev,
+	struct comedi_subdevice *s,
+	struct comedi_insn *insn,
+	unsigned int *data)
+{
+	if (insn->n != 2)
+		return -EINVAL;
+
+	if (data[0]) {
+		s->state &= ~data[0];
+		s->state |= (data[0] & data[1]);
+
+		outl((s->state  << 16) & 0xffffffff, dev->iobase + PCI7230_DO);
+	}
 
-/* Digital IO */
+	return 2;
+}
 
 static int adl_pci7230_di_insn_bits(struct comedi_device *dev,
 	struct comedi_subdevice *s,
 	struct comedi_insn *insn,
-	unsigned int *data);
+	unsigned int *data)
+{
+	if (insn->n != 2)
+		return -EINVAL;
 
-static int adl_pci7230_do_insn_bits(struct comedi_device *dev,
-	struct comedi_subdevice *s,
-	struct comedi_insn *insn,
-	unsigned int *data);
+	data[1] = inl(dev->iobase + PCI7230_DI) & 0xffffffff;
+
+	return 2;
+}
 
 static int adl_pci7230_attach(struct comedi_device *dev,
 	struct comedi_devconfig *it)
@@ -148,89 +150,46 @@ static int adl_pci7230_attach(struct comedi_device *dev,
 	return 1;
 }
 
-static int adl_pci7230_detach(struct comedi_device *dev)
+static void adl_pci7230_detach(struct comedi_device *dev)
 {
-	printk(KERN_DEBUG "comedi%d: pci7230: remove\n", dev->minor);
-
 	if (devpriv && devpriv->pci_dev) {
 		if (dev->iobase)
 			comedi_pci_disable(devpriv->pci_dev);
 		pci_dev_put(devpriv->pci_dev);
 	}
-
-	return 0;
-}
-
-static int adl_pci7230_do_insn_bits(struct comedi_device *dev,
-	struct comedi_subdevice *s,
-	struct comedi_insn *insn,
-	unsigned int *data)
-{
-	if (insn->n != 2)
-		return -EINVAL;
-
-	if (data[0]) {
-		s->state &= ~data[0];
-		s->state |= (data[0] & data[1]);
-
-		outl((s->state  << 16) & 0xffffffff, dev->iobase + PCI7230_DO);
-	}
-
-	return 2;
 }
 
-static int adl_pci7230_di_insn_bits(struct comedi_device *dev,
-	struct comedi_subdevice *s,
-	struct comedi_insn *insn,
-	unsigned int *data)
-{
-	if (insn->n != 2)
-		return -EINVAL;
-
-	data[1] = inl(dev->iobase + PCI7230_DI) & 0xffffffff;
-
-	return 2;
-}
+static struct comedi_driver adl_pci7230_driver = {
+	.driver_name	= "adl_pci7230",
+	.module		= THIS_MODULE,
+	.attach		= adl_pci7230_attach,
+	.detach		= adl_pci7230_detach,
+};
 
-static int __devinit driver_adl_pci7230_pci_probe(struct pci_dev *dev,
-						  const struct pci_device_id
-						  *ent)
+static int __devinit adl_pci7230_pci_probe(struct pci_dev *dev,
+					   const struct pci_device_id *ent)
 {
-	return comedi_pci_auto_config(dev, driver_adl_pci7230.driver_name);
+	return comedi_pci_auto_config(dev, &adl_pci7230_driver);
 }
 
-static void __devexit driver_adl_pci7230_pci_remove(struct pci_dev *dev)
+static void __devexit adl_pci7230_pci_remove(struct pci_dev *dev)
 {
 	comedi_pci_auto_unconfig(dev);
 }
 
-static struct pci_driver driver_adl_pci7230_pci_driver = {
-	.id_table = adl_pci7230_pci_table,
-	.probe = &driver_adl_pci7230_pci_probe,
-	.remove = __devexit_p(&driver_adl_pci7230_pci_remove)
+static DEFINE_PCI_DEVICE_TABLE(adl_pci7230_pci_table) = {
+	{ PCI_DEVICE(PCI_VENDOR_ID_ADLINK, PCI_DEVICE_ID_PCI7230) },
+	{ 0 }
 };
+MODULE_DEVICE_TABLE(pci, adl_pci7230_pci_table);
 
-static int __init driver_adl_pci7230_init_module(void)
-{
-	int retval;
-
-	retval = comedi_driver_register(&driver_adl_pci7230);
-	if (retval < 0)
-		return retval;
-
-	driver_adl_pci7230_pci_driver.name =
-	    (char *)driver_adl_pci7230.driver_name;
-	return pci_register_driver(&driver_adl_pci7230_pci_driver);
-}
-
-static void __exit driver_adl_pci7230_cleanup_module(void)
-{
-	pci_unregister_driver(&driver_adl_pci7230_pci_driver);
-	comedi_driver_unregister(&driver_adl_pci7230);
-}
-
-module_init(driver_adl_pci7230_init_module);
-module_exit(driver_adl_pci7230_cleanup_module);
+static struct pci_driver adl_pci7230_pci_driver = {
+	.name		= "adl_pci7230",
+	.id_table	= adl_pci7230_pci_table,
+	.probe		= adl_pci7230_pci_probe,
+	.remove		= __devexit_p(adl_pci7230_pci_remove),
+};
+module_comedi_pci_driver(adl_pci7230_driver, adl_pci7230_pci_driver);
 
 MODULE_AUTHOR("Comedi http://www.comedi.org");
 MODULE_DESCRIPTION("Comedi low-level driver");
diff --git a/drivers/staging/comedi/drivers/adl_pci7296.c b/drivers/staging/comedi/drivers/adl_pci7296.c
index 9a2320537bd..b4dae3b7598 100644
--- a/drivers/staging/comedi/drivers/adl_pci7296.c
+++ b/drivers/staging/comedi/drivers/adl_pci7296.c
@@ -48,13 +48,6 @@ Configuration Options:
 
 #define PCI_DEVICE_ID_PCI7296 0x7296
 
-static DEFINE_PCI_DEVICE_TABLE(adl_pci7296_pci_table) = {
-	{ PCI_DEVICE(PCI_VENDOR_ID_ADLINK, PCI_DEVICE_ID_PCI7296) },
-	{0}
-};
-
-MODULE_DEVICE_TABLE(pci, adl_pci7296_pci_table);
-
 struct adl_pci7296_private {
 	int data;
 	struct pci_dev *pci_dev;
@@ -63,16 +56,6 @@ struct adl_pci7296_private {
 #define devpriv ((struct adl_pci7296_private *)dev->private)
 
 static int adl_pci7296_attach(struct comedi_device *dev,
-			      struct comedi_devconfig *it);
-static int adl_pci7296_detach(struct comedi_device *dev);
-static struct comedi_driver driver_adl_pci7296 = {
-	.driver_name = "adl_pci7296",
-	.module = THIS_MODULE,
-	.attach = adl_pci7296_attach,
-	.detach = adl_pci7296_detach,
-};
-
-static int adl_pci7296_attach(struct comedi_device *dev,
 			      struct comedi_devconfig *it)
 {
 	struct pci_dev *pcidev = NULL;
@@ -151,66 +134,52 @@ static int adl_pci7296_attach(struct comedi_device *dev,
 	return -EIO;
 }
 
-static int adl_pci7296_detach(struct comedi_device *dev)
+static void adl_pci7296_detach(struct comedi_device *dev)
 {
-	printk(KERN_INFO "comedi%d: pci7432: remove\n", dev->minor);
-
 	if (devpriv && devpriv->pci_dev) {
 		if (dev->iobase)
 			comedi_pci_disable(devpriv->pci_dev);
 		pci_dev_put(devpriv->pci_dev);
 	}
-	/*  detach four 8255 digital io subdevices */
 	if (dev->subdevices) {
 		subdev_8255_cleanup(dev, dev->subdevices + 0);
 		subdev_8255_cleanup(dev, dev->subdevices + 1);
 		subdev_8255_cleanup(dev, dev->subdevices + 2);
 		subdev_8255_cleanup(dev, dev->subdevices + 3);
-
 	}
-
-	return 0;
 }
 
-static int __devinit driver_adl_pci7296_pci_probe(struct pci_dev *dev,
-						  const struct pci_device_id
-						  *ent)
+static struct comedi_driver adl_pci7296_driver = {
+	.driver_name	= "adl_pci7296",
+	.module		= THIS_MODULE,
+	.attach		= adl_pci7296_attach,
+	.detach		= adl_pci7296_detach,
+};
+
+static int __devinit adl_pci7296_pci_probe(struct pci_dev *dev,
+					   const struct pci_device_id *ent)
 {
-	return comedi_pci_auto_config(dev, driver_adl_pci7296.driver_name);
+	return comedi_pci_auto_config(dev, &adl_pci7296_driver);
 }
 
-static void __devexit driver_adl_pci7296_pci_remove(struct pci_dev *dev)
+static void __devexit adl_pci7296_pci_remove(struct pci_dev *dev)
 {
 	comedi_pci_auto_unconfig(dev);
 }
 
-static struct pci_driver driver_adl_pci7296_pci_driver = {
-	.id_table = adl_pci7296_pci_table,
-	.probe = &driver_adl_pci7296_pci_probe,
-	.remove = __devexit_p(&driver_adl_pci7296_pci_remove)
+static DEFINE_PCI_DEVICE_TABLE(adl_pci7296_pci_table) = {
+	{ PCI_DEVICE(PCI_VENDOR_ID_ADLINK, PCI_DEVICE_ID_PCI7296) },
+	{ 0 }
 };
+MODULE_DEVICE_TABLE(pci, adl_pci7296_pci_table);
 
-static int __init driver_adl_pci7296_init_module(void)
-{
-	int retval;
-
-	retval = comedi_driver_register(&driver_adl_pci7296);
-	if (retval < 0)
-		return retval;
-
-	driver_adl_pci7296_pci_driver.name =
-	    (char *)driver_adl_pci7296.driver_name;
-	return pci_register_driver(&driver_adl_pci7296_pci_driver);
-}
-
-static void __exit driver_adl_pci7296_cleanup_module(void)
-{
-	pci_unregister_driver(&driver_adl_pci7296_pci_driver);
-	comedi_driver_unregister(&driver_adl_pci7296);
-}
-
-module_init(driver_adl_pci7296_init_module);
-module_exit(driver_adl_pci7296_cleanup_module);
+static struct pci_driver adl_pci7296_pci_driver = {
+	.name		= "adl_pci7296",
+	.id_table	= adl_pci7296_pci_table,
+	.probe		= adl_pci7296_pci_probe,
+	.remove		= __devexit_p(adl_pci7296_pci_remove),
+};
+module_comedi_pci_driver(adl_pci7296_driver, adl_pci7296_pci_driver);
 
 MODULE_AUTHOR("Comedi http://www.comedi.org");
 MODULE_DESCRIPTION("Comedi low-level driver");
diff --git a/drivers/staging/comedi/drivers/adl_pci7432.c b/drivers/staging/comedi/drivers/adl_pci7432.c
index 86ee2197604..9cbfb61a447 100644
--- a/drivers/staging/comedi/drivers/adl_pci7432.c
+++ b/drivers/staging/comedi/drivers/adl_pci7432.c
@@ -43,13 +43,6 @@ Configuration Options:
 
 #define PCI_DEVICE_ID_PCI7432 0x7432
 
-static DEFINE_PCI_DEVICE_TABLE(adl_pci7432_pci_table) = {
-	{ PCI_DEVICE(PCI_VENDOR_ID_ADLINK, PCI_DEVICE_ID_PCI7432) },
-	{0}
-};
-
-MODULE_DEVICE_TABLE(pci, adl_pci7432_pci_table);
-
 struct adl_pci7432_private {
 	int data;
 	struct pci_dev *pci_dev;
@@ -57,29 +50,44 @@ struct adl_pci7432_private {
 
 #define devpriv ((struct adl_pci7432_private *)dev->private)
 
-static int adl_pci7432_attach(struct comedi_device *dev,
-			      struct comedi_devconfig *it);
-static int adl_pci7432_detach(struct comedi_device *dev);
-static struct comedi_driver driver_adl_pci7432 = {
-	.driver_name = "adl_pci7432",
-	.module = THIS_MODULE,
-	.attach = adl_pci7432_attach,
-	.detach = adl_pci7432_detach,
-};
+static int adl_pci7432_do_insn_bits(struct comedi_device *dev,
+				    struct comedi_subdevice *s,
+				    struct comedi_insn *insn,
+				    unsigned int *data)
+{
+	printk(KERN_DEBUG "comedi: pci7432_do_insn_bits called\n");
+	printk(KERN_DEBUG "comedi: data0: %8x data1: %8x\n", data[0], data[1]);
+
+	if (insn->n != 2)
+		return -EINVAL;
 
-/* Digital IO */
+	if (data[0]) {
+		s->state &= ~data[0];
+		s->state |= (data[0] & data[1]);
+
+		printk(KERN_DEBUG "comedi: out: %8x on iobase %4lx\n", s->state,
+		       dev->iobase + PCI7432_DO);
+		outl(s->state & 0xffffffff, dev->iobase + PCI7432_DO);
+	}
+	return 2;
+}
 
 static int adl_pci7432_di_insn_bits(struct comedi_device *dev,
 				    struct comedi_subdevice *s,
 				    struct comedi_insn *insn,
-				    unsigned int *data);
+				    unsigned int *data)
+{
+	printk(KERN_DEBUG "comedi: pci7432_di_insn_bits called\n");
+	printk(KERN_DEBUG "comedi: data0: %8x data1: %8x\n", data[0], data[1]);
 
-static int adl_pci7432_do_insn_bits(struct comedi_device *dev,
-				    struct comedi_subdevice *s,
-				    struct comedi_insn *insn,
-				    unsigned int *data);
+	if (insn->n != 2)
+		return -EINVAL;
+
+	data[1] = inl(dev->iobase + PCI7432_DI) & 0xffffffff;
+	printk(KERN_DEBUG "comedi: data1 %8x\n", data[1]);
 
-/*            */
+	return 2;
+}
 
 static int adl_pci7432_attach(struct comedi_device *dev,
 			      struct comedi_devconfig *it)
@@ -153,97 +161,46 @@ static int adl_pci7432_attach(struct comedi_device *dev,
 	return -EIO;
 }
 
-static int adl_pci7432_detach(struct comedi_device *dev)
+static void adl_pci7432_detach(struct comedi_device *dev)
 {
-	printk(KERN_INFO "comedi%d: pci7432: remove\n", dev->minor);
-
 	if (devpriv && devpriv->pci_dev) {
 		if (dev->iobase)
 			comedi_pci_disable(devpriv->pci_dev);
 		pci_dev_put(devpriv->pci_dev);
 	}
-
-	return 0;
 }
 
-static int adl_pci7432_do_insn_bits(struct comedi_device *dev,
-				    struct comedi_subdevice *s,
-				    struct comedi_insn *insn,
-				    unsigned int *data)
-{
-	printk(KERN_DEBUG "comedi: pci7432_do_insn_bits called\n");
-	printk(KERN_DEBUG "comedi: data0: %8x data1: %8x\n", data[0], data[1]);
-
-	if (insn->n != 2)
-		return -EINVAL;
-
-	if (data[0]) {
-		s->state &= ~data[0];
-		s->state |= (data[0] & data[1]);
-
-		printk(KERN_DEBUG "comedi: out: %8x on iobase %4lx\n", s->state,
-		       dev->iobase + PCI7432_DO);
-		outl(s->state & 0xffffffff, dev->iobase + PCI7432_DO);
-	}
-	return 2;
-}
-
-static int adl_pci7432_di_insn_bits(struct comedi_device *dev,
-				    struct comedi_subdevice *s,
-				    struct comedi_insn *insn,
-				    unsigned int *data)
-{
-	printk(KERN_DEBUG "comedi: pci7432_di_insn_bits called\n");
-	printk(KERN_DEBUG "comedi: data0: %8x data1: %8x\n", data[0], data[1]);
-
-	if (insn->n != 2)
-		return -EINVAL;
-
-	data[1] = inl(dev->iobase + PCI7432_DI) & 0xffffffff;
-	printk(KERN_DEBUG "comedi: data1 %8x\n", data[1]);
-
-	return 2;
-}
+static struct comedi_driver adl_pci7432_driver = {
+	.driver_name	= "adl_pci7432",
+	.module		= THIS_MODULE,
+	.attach		= adl_pci7432_attach,
+	.detach		= adl_pci7432_detach,
+};
 
-static int __devinit driver_adl_pci7432_pci_probe(struct pci_dev *dev,
-						  const struct pci_device_id
-						  *ent)
+static int __devinit adl_pci7432_pci_probe(struct pci_dev *dev,
+					   const struct pci_device_id *ent)
 {
-	return comedi_pci_auto_config(dev, driver_adl_pci7432.driver_name);
+	return comedi_pci_auto_config(dev, &adl_pci7432_driver);
 }
 
-static void __devexit driver_adl_pci7432_pci_remove(struct pci_dev *dev)
+static void __devexit adl_pci7432_pci_remove(struct pci_dev *dev)
 {
 	comedi_pci_auto_unconfig(dev);
 }
 
-static struct pci_driver driver_adl_pci7432_pci_driver = {
-	.id_table = adl_pci7432_pci_table,
-	.probe = &driver_adl_pci7432_pci_probe,
-	.remove = __devexit_p(&driver_adl_pci7432_pci_remove)
+static DEFINE_PCI_DEVICE_TABLE(adl_pci7432_pci_table) = {
+	{ PCI_DEVICE(PCI_VENDOR_ID_ADLINK, PCI_DEVICE_ID_PCI7432) },
+	{ 0 }
 };
+MODULE_DEVICE_TABLE(pci, adl_pci7432_pci_table);
 
-static int __init driver_adl_pci7432_init_module(void)
-{
-	int retval;
-
-	retval = comedi_driver_register(&driver_adl_pci7432);
-	if (retval < 0)
-		return retval;
-
-	driver_adl_pci7432_pci_driver.name =
-	    (char *)driver_adl_pci7432.driver_name;
-	return pci_register_driver(&driver_adl_pci7432_pci_driver);
-}
-
-static void __exit driver_adl_pci7432_cleanup_module(void)
-{
-	pci_unregister_driver(&driver_adl_pci7432_pci_driver);
-	comedi_driver_unregister(&driver_adl_pci7432);
-}
-
-module_init(driver_adl_pci7432_init_module);
-module_exit(driver_adl_pci7432_cleanup_module);
+static struct pci_driver adl_pci7432_pci_driver = {
+	.name		= "adl_pci7432",
+	.id_table	= adl_pci7432_pci_table,
+	.probe		= adl_pci7432_pci_probe,
+	.remove		= __devexit_p(adl_pci7432_pci_remove),
+};
+module_comedi_pci_driver(adl_pci7432_driver, adl_pci7432_pci_driver);
 
 MODULE_AUTHOR("Comedi http://www.comedi.org");
 MODULE_DESCRIPTION("Comedi low-level driver");
diff --git a/drivers/staging/comedi/drivers/adl_pci8164.c b/drivers/staging/comedi/drivers/adl_pci8164.c
index 3b83d65bc1b..409ef13ad09 100644
--- a/drivers/staging/comedi/drivers/adl_pci8164.c
+++ b/drivers/staging/comedi/drivers/adl_pci8164.c
@@ -56,13 +56,6 @@ Configuration Options:
 
 #define PCI_DEVICE_ID_PCI8164 0x8164
 
-static DEFINE_PCI_DEVICE_TABLE(adl_pci8164_pci_table) = {
-	{ PCI_DEVICE(PCI_VENDOR_ID_ADLINK, PCI_DEVICE_ID_PCI8164) },
-	{0}
-};
-
-MODULE_DEVICE_TABLE(pci, adl_pci8164_pci_table);
-
 struct adl_pci8164_private {
 	int data;
 	struct pci_dev *pci_dev;
@@ -70,159 +63,6 @@ struct adl_pci8164_private {
 
 #define devpriv ((struct adl_pci8164_private *)dev->private)
 
-static int adl_pci8164_attach(struct comedi_device *dev,
-			      struct comedi_devconfig *it);
-static int adl_pci8164_detach(struct comedi_device *dev);
-static struct comedi_driver driver_adl_pci8164 = {
-	.driver_name = "adl_pci8164",
-	.module = THIS_MODULE,
-	.attach = adl_pci8164_attach,
-	.detach = adl_pci8164_detach,
-};
-
-static int adl_pci8164_insn_read_msts(struct comedi_device *dev,
-				      struct comedi_subdevice *s,
-				      struct comedi_insn *insn,
-				      unsigned int *data);
-
-static int adl_pci8164_insn_read_ssts(struct comedi_device *dev,
-				      struct comedi_subdevice *s,
-				      struct comedi_insn *insn,
-				      unsigned int *data);
-
-static int adl_pci8164_insn_read_buf0(struct comedi_device *dev,
-				      struct comedi_subdevice *s,
-				      struct comedi_insn *insn,
-				      unsigned int *data);
-
-static int adl_pci8164_insn_read_buf1(struct comedi_device *dev,
-				      struct comedi_subdevice *s,
-				      struct comedi_insn *insn,
-				      unsigned int *data);
-
-static int adl_pci8164_insn_write_cmd(struct comedi_device *dev,
-				      struct comedi_subdevice *s,
-				      struct comedi_insn *insn,
-				      unsigned int *data);
-
-static int adl_pci8164_insn_write_otp(struct comedi_device *dev,
-				      struct comedi_subdevice *s,
-				      struct comedi_insn *insn,
-				      unsigned int *data);
-
-static int adl_pci8164_insn_write_buf0(struct comedi_device *dev,
-				       struct comedi_subdevice *s,
-				       struct comedi_insn *insn,
-				       unsigned int *data);
-
-static int adl_pci8164_insn_write_buf1(struct comedi_device *dev,
-				       struct comedi_subdevice *s,
-				       struct comedi_insn *insn,
-				       unsigned int *data);
-
-static int adl_pci8164_attach(struct comedi_device *dev,
-			      struct comedi_devconfig *it)
-{
-	struct pci_dev *pcidev = NULL;
-	struct comedi_subdevice *s;
-	int bus, slot;
-
-	printk(KERN_INFO "comedi: attempt to attach...\n");
-	printk(KERN_INFO "comedi%d: adl_pci8164\n", dev->minor);
-
-	dev->board_name = "pci8164";
-	bus = it->options[0];
-	slot = it->options[1];
-
-	if (alloc_private(dev, sizeof(struct adl_pci8164_private)) < 0)
-		return -ENOMEM;
-
-	if (alloc_subdevices(dev, 4) < 0)
-		return -ENOMEM;
-
-	for_each_pci_dev(pcidev) {
-		if (pcidev->vendor == PCI_VENDOR_ID_ADLINK &&
-		    pcidev->device == PCI_DEVICE_ID_PCI8164) {
-			if (bus || slot) {
-				/* requested particular bus/slot */
-				if (pcidev->bus->number != bus
-					|| PCI_SLOT(pcidev->devfn) != slot)
-					continue;
-			}
-			devpriv->pci_dev = pcidev;
-			if (comedi_pci_enable(pcidev, "adl_pci8164") < 0) {
-				printk(KERN_ERR "comedi%d: Failed to enable "
-				"PCI device and request regions\n", dev->minor);
-				return -EIO;
-			}
-			dev->iobase = pci_resource_start(pcidev, 2);
-			printk(KERN_DEBUG "comedi: base addr %4lx\n",
-				   dev->iobase);
-
-			s = dev->subdevices + 0;
-			s->type = COMEDI_SUBD_PROC;
-			s->subdev_flags = SDF_READABLE | SDF_WRITABLE;
-			s->n_chan = 4;
-			s->maxdata = 0xffff;
-			s->len_chanlist = 4;
-			/* s->range_table = &range_axis; */
-			s->insn_read = adl_pci8164_insn_read_msts;
-			s->insn_write = adl_pci8164_insn_write_cmd;
-
-			s = dev->subdevices + 1;
-			s->type = COMEDI_SUBD_PROC;
-			s->subdev_flags = SDF_READABLE | SDF_WRITABLE;
-			s->n_chan = 4;
-			s->maxdata = 0xffff;
-			s->len_chanlist = 4;
-			/* s->range_table = &range_axis; */
-			s->insn_read = adl_pci8164_insn_read_ssts;
-			s->insn_write = adl_pci8164_insn_write_otp;
-
-			s = dev->subdevices + 2;
-			s->type = COMEDI_SUBD_PROC;
-			s->subdev_flags = SDF_READABLE | SDF_WRITABLE;
-			s->n_chan = 4;
-			s->maxdata = 0xffff;
-			s->len_chanlist = 4;
-			/* s->range_table = &range_axis; */
-			s->insn_read = adl_pci8164_insn_read_buf0;
-			s->insn_write = adl_pci8164_insn_write_buf0;
-
-			s = dev->subdevices + 3;
-			s->type = COMEDI_SUBD_PROC;
-			s->subdev_flags = SDF_READABLE | SDF_WRITABLE;
-			s->n_chan = 4;
-			s->maxdata = 0xffff;
-			s->len_chanlist = 4;
-			/* s->range_table = &range_axis; */
-			s->insn_read = adl_pci8164_insn_read_buf1;
-			s->insn_write = adl_pci8164_insn_write_buf1;
-
-			printk(KERN_INFO "comedi: attached\n");
-
-			return 1;
-		}
-	}
-
-	printk(KERN_ERR "comedi%d: no supported board found!"
-		   "(req. bus/slot : %d/%d)\n", dev->minor, bus, slot);
-	return -EIO;
-}
-
-static int adl_pci8164_detach(struct comedi_device *dev)
-{
-	printk(KERN_INFO "comedi%d: pci8164: remove\n", dev->minor);
-
-	if (devpriv && devpriv->pci_dev) {
-		if (dev->iobase)
-			comedi_pci_disable(devpriv->pci_dev);
-		pci_dev_put(devpriv->pci_dev);
-	}
-
-	return 0;
-}
-
 /*
  all the read commands are the same except for the addition a constant
  * const to the data for inw()
@@ -384,45 +224,136 @@ static int adl_pci8164_insn_write_buf1(struct comedi_device *dev,
 	return 2;
 }
 
-static int __devinit driver_adl_pci8164_pci_probe(struct pci_dev *dev,
-						  const struct pci_device_id
-						  *ent)
+static int adl_pci8164_attach(struct comedi_device *dev,
+			      struct comedi_devconfig *it)
 {
-	return comedi_pci_auto_config(dev, driver_adl_pci8164.driver_name);
+	struct pci_dev *pcidev = NULL;
+	struct comedi_subdevice *s;
+	int bus, slot;
+
+	printk(KERN_INFO "comedi: attempt to attach...\n");
+	printk(KERN_INFO "comedi%d: adl_pci8164\n", dev->minor);
+
+	dev->board_name = "pci8164";
+	bus = it->options[0];
+	slot = it->options[1];
+
+	if (alloc_private(dev, sizeof(struct adl_pci8164_private)) < 0)
+		return -ENOMEM;
+
+	if (alloc_subdevices(dev, 4) < 0)
+		return -ENOMEM;
+
+	for_each_pci_dev(pcidev) {
+		if (pcidev->vendor == PCI_VENDOR_ID_ADLINK &&
+		    pcidev->device == PCI_DEVICE_ID_PCI8164) {
+			if (bus || slot) {
+				/* requested particular bus/slot */
+				if (pcidev->bus->number != bus
+					|| PCI_SLOT(pcidev->devfn) != slot)
+					continue;
+			}
+			devpriv->pci_dev = pcidev;
+			if (comedi_pci_enable(pcidev, "adl_pci8164") < 0) {
+				printk(KERN_ERR "comedi%d: Failed to enable "
+				"PCI device and request regions\n", dev->minor);
+				return -EIO;
+			}
+			dev->iobase = pci_resource_start(pcidev, 2);
+			printk(KERN_DEBUG "comedi: base addr %4lx\n",
+				   dev->iobase);
+
+			s = dev->subdevices + 0;
+			s->type = COMEDI_SUBD_PROC;
+			s->subdev_flags = SDF_READABLE | SDF_WRITABLE;
+			s->n_chan = 4;
+			s->maxdata = 0xffff;
+			s->len_chanlist = 4;
+			/* s->range_table = &range_axis; */
+			s->insn_read = adl_pci8164_insn_read_msts;
+			s->insn_write = adl_pci8164_insn_write_cmd;
+
+			s = dev->subdevices + 1;
+			s->type = COMEDI_SUBD_PROC;
+			s->subdev_flags = SDF_READABLE | SDF_WRITABLE;
+			s->n_chan = 4;
+			s->maxdata = 0xffff;
+			s->len_chanlist = 4;
+			/* s->range_table = &range_axis; */
+			s->insn_read = adl_pci8164_insn_read_ssts;
+			s->insn_write = adl_pci8164_insn_write_otp;
+
+			s = dev->subdevices + 2;
+			s->type = COMEDI_SUBD_PROC;
+			s->subdev_flags = SDF_READABLE | SDF_WRITABLE;
+			s->n_chan = 4;
+			s->maxdata = 0xffff;
+			s->len_chanlist = 4;
+			/* s->range_table = &range_axis; */
+			s->insn_read = adl_pci8164_insn_read_buf0;
+			s->insn_write = adl_pci8164_insn_write_buf0;
+
+			s = dev->subdevices + 3;
+			s->type = COMEDI_SUBD_PROC;
+			s->subdev_flags = SDF_READABLE | SDF_WRITABLE;
+			s->n_chan = 4;
+			s->maxdata = 0xffff;
+			s->len_chanlist = 4;
+			/* s->range_table = &range_axis; */
+			s->insn_read = adl_pci8164_insn_read_buf1;
+			s->insn_write = adl_pci8164_insn_write_buf1;
+
+			printk(KERN_INFO "comedi: attached\n");
+
+			return 1;
+		}
+	}
+
+	printk(KERN_ERR "comedi%d: no supported board found!"
+		   "(req. bus/slot : %d/%d)\n", dev->minor, bus, slot);
+	return -EIO;
 }
 
-static void __devexit driver_adl_pci8164_pci_remove(struct pci_dev *dev)
+static void adl_pci8164_detach(struct comedi_device *dev)
 {
-	comedi_pci_auto_unconfig(dev);
+	if (devpriv && devpriv->pci_dev) {
+		if (dev->iobase)
+			comedi_pci_disable(devpriv->pci_dev);
+		pci_dev_put(devpriv->pci_dev);
+	}
 }
 
-static struct pci_driver driver_adl_pci8164_pci_driver = {
-	.id_table = adl_pci8164_pci_table,
-	.probe = &driver_adl_pci8164_pci_probe,
-	.remove = __devexit_p(&driver_adl_pci8164_pci_remove)
+static struct comedi_driver adl_pci8164_driver = {
+	.driver_name	= "adl_pci8164",
+	.module		= THIS_MODULE,
+	.attach		= adl_pci8164_attach,
+	.detach		= adl_pci8164_detach,
 };
 
-static int __init driver_adl_pci8164_init_module(void)
+static int __devinit adl_pci8164_pci_probe(struct pci_dev *dev,
+					   const struct pci_device_id *ent)
 {
-	int retval;
-
-	retval = comedi_driver_register(&driver_adl_pci8164);
-	if (retval < 0)
-		return retval;
-
-	driver_adl_pci8164_pci_driver.name =
-	    (char *)driver_adl_pci8164.driver_name;
-	return pci_register_driver(&driver_adl_pci8164_pci_driver);
+	return comedi_pci_auto_config(dev, &adl_pci8164_driver);
 }
 
-static void __exit driver_adl_pci8164_cleanup_module(void)
+static void __devexit adl_pci8164_pci_remove(struct pci_dev *dev)
 {
-	pci_unregister_driver(&driver_adl_pci8164_pci_driver);
-	comedi_driver_unregister(&driver_adl_pci8164);
+	comedi_pci_auto_unconfig(dev);
 }
 
-module_init(driver_adl_pci8164_init_module);
-module_exit(driver_adl_pci8164_cleanup_module);
+static DEFINE_PCI_DEVICE_TABLE(adl_pci8164_pci_table) = {
+	{ PCI_DEVICE(PCI_VENDOR_ID_ADLINK, PCI_DEVICE_ID_PCI8164) },
+	{0}
+};
+MODULE_DEVICE_TABLE(pci, adl_pci8164_pci_table);
+
+static struct pci_driver adl_pci8164_pci_driver = {
+	.name		= "adl_pci8164",
+	.id_table	= adl_pci8164_pci_table,
+	.probe		= adl_pci8164_pci_probe,
+	.remove		= __devexit_p(adl_pci8164_pci_remove),
+};
+module_comedi_pci_driver(adl_pci8164_driver, adl_pci8164_pci_driver);
 
 MODULE_AUTHOR("Comedi http://www.comedi.org");
 MODULE_DESCRIPTION("Comedi low-level driver");
diff --git a/drivers/staging/comedi/drivers/adl_pci9111.c b/drivers/staging/comedi/drivers/adl_pci9111.c
index 2a9bd88a4ab..ccfb1a52154 100644
--- a/drivers/staging/comedi/drivers/adl_pci9111.c
+++ b/drivers/staging/comedi/drivers/adl_pci9111.c
@@ -289,16 +289,6 @@ TODO:
 			PCI9111_IO_BASE+PCI9111_REGISTER_8254_COUNTER_2); \
 	} while (0)
 
-/*  Function prototypes */
-
-static int pci9111_attach(struct comedi_device *dev,
-			  struct comedi_devconfig *it);
-static int pci9111_detach(struct comedi_device *dev);
-static void pci9111_ai_munge(struct comedi_device *dev,
-			     struct comedi_subdevice *s, void *data,
-			     unsigned int num_bytes,
-			     unsigned int start_chan_index);
-
 static const struct comedi_lrange pci9111_hr_ai_range = {
 	5,
 	{
@@ -310,14 +300,6 @@ static const struct comedi_lrange pci9111_hr_ai_range = {
 	 }
 };
 
-static DEFINE_PCI_DEVICE_TABLE(pci9111_pci_table) = {
-	{ PCI_DEVICE(PCI_VENDOR_ID_ADLINK, PCI9111_HR_DEVICE_ID) },
-	/* { PCI_DEVICE(PCI_VENDOR_ID_ADLINK, PCI9111_HG_DEVICE_ID) }, */
-	{ 0 }
-};
-
-MODULE_DEVICE_TABLE(pci, pci9111_pci_table);
-
 /*  */
 /*  Board specification structure */
 /*  */
@@ -354,51 +336,6 @@ static const struct pci9111_board pci9111_boards[] = {
 #define pci9111_board_nbr \
 	(sizeof(pci9111_boards)/sizeof(struct pci9111_board))
 
-static struct comedi_driver pci9111_driver = {
-	.driver_name = PCI9111_DRIVER_NAME,
-	.module = THIS_MODULE,
-	.attach = pci9111_attach,
-	.detach = pci9111_detach,
-};
-
-static int __devinit pci9111_driver_pci_probe(struct pci_dev *dev,
-					      const struct pci_device_id *ent)
-{
-	return comedi_pci_auto_config(dev, pci9111_driver.driver_name);
-}
-
-static void __devexit pci9111_driver_pci_remove(struct pci_dev *dev)
-{
-	comedi_pci_auto_unconfig(dev);
-}
-
-static struct pci_driver pci9111_driver_pci_driver = {
-	.id_table = pci9111_pci_table,
-	.probe = &pci9111_driver_pci_probe,
-	.remove = __devexit_p(&pci9111_driver_pci_remove)
-};
-
-static int __init pci9111_driver_init_module(void)
-{
-	int retval;
-
-	retval = comedi_driver_register(&pci9111_driver);
-	if (retval < 0)
-		return retval;
-
-	pci9111_driver_pci_driver.name = (char *)pci9111_driver.driver_name;
-	return pci_register_driver(&pci9111_driver_pci_driver);
-}
-
-static void __exit pci9111_driver_cleanup_module(void)
-{
-	pci_unregister_driver(&pci9111_driver_pci_driver);
-	comedi_driver_unregister(&pci9111_driver);
-}
-
-module_init(pci9111_driver_init_module);
-module_exit(pci9111_driver_cleanup_module);
-
 /*  Private data structure */
 
 struct pci9111_private_data {
@@ -1445,31 +1382,54 @@ found:
 	return 0;
 }
 
-/*  Detach */
-
-static int pci9111_detach(struct comedi_device *dev)
+static void pci9111_detach(struct comedi_device *dev)
 {
-	/*  Reset device */
-
 	if (dev->private != NULL) {
 		if (dev_private->is_valid)
 			pci9111_reset(dev);
-
 	}
-	/*  Release previously allocated irq */
-
 	if (dev->irq != 0)
 		free_irq(dev->irq, dev);
-
 	if (dev_private != NULL && dev_private->pci_device != NULL) {
 		if (dev->iobase)
 			comedi_pci_disable(dev_private->pci_device);
 		pci_dev_put(dev_private->pci_device);
 	}
+}
 
-	return 0;
+static struct comedi_driver adl_pci9111_driver = {
+	.driver_name	= "adl_pci9111",
+	.module		= THIS_MODULE,
+	.attach		= pci9111_attach,
+	.detach		= pci9111_detach,
+};
+
+static int __devinit pci9111_pci_probe(struct pci_dev *dev,
+				       const struct pci_device_id *ent)
+{
+	return comedi_pci_auto_config(dev, &adl_pci9111_driver);
 }
 
+static void __devexit pci9111_pci_remove(struct pci_dev *dev)
+{
+	comedi_pci_auto_unconfig(dev);
+}
+
+static DEFINE_PCI_DEVICE_TABLE(pci9111_pci_table) = {
+	{ PCI_DEVICE(PCI_VENDOR_ID_ADLINK, PCI9111_HR_DEVICE_ID) },
+	/* { PCI_DEVICE(PCI_VENDOR_ID_ADLINK, PCI9111_HG_DEVICE_ID) }, */
+	{ 0 }
+};
+MODULE_DEVICE_TABLE(pci, pci9111_pci_table);
+
+static struct pci_driver adl_pci9111_pci_driver = {
+	.name		= "adl_pci9111",
+	.id_table	= pci9111_pci_table,
+	.probe		= pci9111_pci_probe,
+	.remove		= __devexit_p(pci9111_pci_remove),
+};
+module_comedi_pci_driver(adl_pci9111_driver, adl_pci9111_pci_driver);
+
 MODULE_AUTHOR("Comedi http://www.comedi.org");
 MODULE_DESCRIPTION("Comedi low-level driver");
 MODULE_LICENSE("GPL");
diff --git a/drivers/staging/comedi/drivers/adl_pci9118.c b/drivers/staging/comedi/drivers/adl_pci9118.c
index f17654e44ae..78645863297 100644
--- a/drivers/staging/comedi/drivers/adl_pci9118.c
+++ b/drivers/staging/comedi/drivers/adl_pci9118.c
@@ -221,10 +221,6 @@ static const struct comedi_lrange range_pci9118hg = { 8, {
 					 * of BIP/UNI ranges
 					 */
 
-static int pci9118_attach(struct comedi_device *dev,
-			  struct comedi_devconfig *it);
-static int pci9118_detach(struct comedi_device *dev);
-
 struct boardtype {
 	const char *name;		/* board name */
 	int vendor_id;			/* PCI vendor a device ID of card */
@@ -252,81 +248,6 @@ struct boardtype {
 
 };
 
-static DEFINE_PCI_DEVICE_TABLE(pci9118_pci_table) = {
-	{ PCI_DEVICE(PCI_VENDOR_ID_AMCC, 0x80d9) },
-	{ 0 }
-};
-
-MODULE_DEVICE_TABLE(pci, pci9118_pci_table);
-
-static const struct boardtype boardtypes[] = {
-	{"pci9118dg", PCI_VENDOR_ID_AMCC, 0x80d9,
-	 AMCC_OP_REG_SIZE, IORANGE_9118,
-	 16, 8, 256, PCI9118_CHANLEN, 2, 0x0fff, 0x0fff,
-	 &range_pci9118dg_hr, &range_bipolar10,
-	 3000, 12, 512},
-	{"pci9118hg", PCI_VENDOR_ID_AMCC, 0x80d9,
-	 AMCC_OP_REG_SIZE, IORANGE_9118,
-	 16, 8, 256, PCI9118_CHANLEN, 2, 0x0fff, 0x0fff,
-	 &range_pci9118hg, &range_bipolar10,
-	 3000, 12, 512},
-	{"pci9118hr", PCI_VENDOR_ID_AMCC, 0x80d9,
-	 AMCC_OP_REG_SIZE, IORANGE_9118,
-	 16, 8, 256, PCI9118_CHANLEN, 2, 0xffff, 0x0fff,
-	 &range_pci9118dg_hr, &range_bipolar10,
-	 10000, 40, 512},
-};
-
-#define n_boardtypes (sizeof(boardtypes)/sizeof(struct boardtype))
-
-static struct comedi_driver driver_pci9118 = {
-	.driver_name = "adl_pci9118",
-	.module = THIS_MODULE,
-	.attach = pci9118_attach,
-	.detach = pci9118_detach,
-	.num_names = n_boardtypes,
-	.board_name = &boardtypes[0].name,
-	.offset = sizeof(struct boardtype),
-};
-
-static int __devinit driver_pci9118_pci_probe(struct pci_dev *dev,
-					      const struct pci_device_id *ent)
-{
-	return comedi_pci_auto_config(dev, driver_pci9118.driver_name);
-}
-
-static void __devexit driver_pci9118_pci_remove(struct pci_dev *dev)
-{
-	comedi_pci_auto_unconfig(dev);
-}
-
-static struct pci_driver driver_pci9118_pci_driver = {
-	.id_table = pci9118_pci_table,
-	.probe = &driver_pci9118_pci_probe,
-	.remove = __devexit_p(&driver_pci9118_pci_remove)
-};
-
-static int __init driver_pci9118_init_module(void)
-{
-	int retval;
-
-	retval = comedi_driver_register(&driver_pci9118);
-	if (retval < 0)
-		return retval;
-
-	driver_pci9118_pci_driver.name = (char *)driver_pci9118.driver_name;
-	return pci_register_driver(&driver_pci9118_pci_driver);
-}
-
-static void __exit driver_pci9118_cleanup_module(void)
-{
-	pci_unregister_driver(&driver_pci9118_pci_driver);
-	comedi_driver_unregister(&driver_pci9118);
-}
-
-module_init(driver_pci9118_init_module);
-module_exit(driver_pci9118_cleanup_module);
-
 struct pci9118_private {
 	unsigned long iobase_a;	/* base+size for AMCC chip */
 	unsigned int master;	/* master capable */
@@ -2190,9 +2111,6 @@ static int pci9118_reset(struct comedi_device *dev)
 	return 0;
 }
 
-/*
-==============================================================================
-*/
 static int pci9118_attach(struct comedi_device *dev,
 			  struct comedi_devconfig *it)
 {
@@ -2435,10 +2353,7 @@ static int pci9118_attach(struct comedi_device *dev,
 	return 0;
 }
 
-/*
-==============================================================================
-*/
-static int pci9118_detach(struct comedi_device *dev)
+static void pci9118_detach(struct comedi_device *dev)
 {
 	if (dev->private) {
 		if (devpriv->valid)
@@ -2458,13 +2373,100 @@ static int pci9118_detach(struct comedi_device *dev)
 			free_pages((unsigned long)devpriv->dmabuf_virt[1],
 				   devpriv->dmabuf_pages[1]);
 	}
+}
 
-	return 0;
+static const struct boardtype boardtypes[] = {
+	{
+		.name		= "pci9118dg",
+		.vendor_id	= PCI_VENDOR_ID_AMCC,
+		.device_id	= 0x80d9,
+		.iorange_amcc	= AMCC_OP_REG_SIZE,
+		.iorange_9118	= IORANGE_9118,
+		.n_aichan	= 16,
+		.n_aichand	= 8,
+		.mux_aichan	= 256,
+		.n_aichanlist	= PCI9118_CHANLEN,
+		.n_aochan	= 2,
+		.ai_maxdata	= 0x0fff,
+		.ao_maxdata	= 0x0fff,
+		.rangelist_ai	= &range_pci9118dg_hr,
+		.rangelist_ao	= &range_bipolar10,
+		.ai_ns_min	= 3000,
+		.ai_pacer_min	= 12,
+		.half_fifo_size	= 512,
+	}, {
+		.name		= "pci9118hg",
+		.vendor_id	= PCI_VENDOR_ID_AMCC,
+		.device_id	= 0x80d9,
+		.iorange_amcc	= AMCC_OP_REG_SIZE,
+		.iorange_9118	= IORANGE_9118,
+		.n_aichan	= 16,
+		.n_aichand	= 8,
+		.mux_aichan	= 256,
+		.n_aichanlist	= PCI9118_CHANLEN,
+		.n_aochan	= 2,
+		.ai_maxdata	= 0x0fff,
+		.ao_maxdata	= 0x0fff,
+		.rangelist_ai	= &range_pci9118hg,
+		.rangelist_ao	= &range_bipolar10,
+		.ai_ns_min	= 3000,
+		.ai_pacer_min	= 12,
+		.half_fifo_size	= 512,
+	}, {
+		.name		= "pci9118hr",
+		.vendor_id	= PCI_VENDOR_ID_AMCC,
+		.device_id	= 0x80d9,
+		.iorange_amcc	= AMCC_OP_REG_SIZE,
+		.iorange_9118	= IORANGE_9118,
+		.n_aichan	= 16,
+		.n_aichand	= 8,
+		.mux_aichan	= 256,
+		.n_aichanlist	= PCI9118_CHANLEN,
+		.n_aochan	= 2,
+		.ai_maxdata	= 0xffff,
+		.ao_maxdata	= 0x0fff,
+		.rangelist_ai	= &range_pci9118dg_hr,
+		.rangelist_ao	= &range_bipolar10,
+		.ai_ns_min	= 10000,
+		.ai_pacer_min	= 40,
+		.half_fifo_size	= 512,
+	},
+};
+
+static struct comedi_driver adl_pci9118_driver = {
+	.driver_name	= "adl_pci9118",
+	.module		= THIS_MODULE,
+	.attach		= pci9118_attach,
+	.detach		= pci9118_detach,
+	.num_names	= ARRAY_SIZE(boardtypes),
+	.board_name	= &boardtypes[0].name,
+	.offset		= sizeof(struct boardtype),
+};
+
+static int __devinit adl_pci9118_pci_probe(struct pci_dev *dev,
+					   const struct pci_device_id *ent)
+{
+	return comedi_pci_auto_config(dev, &adl_pci9118_driver);
 }
 
-/*
-==============================================================================
-*/
+static void __devexit adl_pci9118_pci_remove(struct pci_dev *dev)
+{
+	comedi_pci_auto_unconfig(dev);
+}
+
+static DEFINE_PCI_DEVICE_TABLE(adl_pci9118_pci_table) = {
+	{ PCI_DEVICE(PCI_VENDOR_ID_AMCC, 0x80d9) },
+	{ 0 }
+};
+MODULE_DEVICE_TABLE(pci, adl_pci9118_pci_table);
+
+static struct pci_driver adl_pci9118_pci_driver = {
+	.name		= "adl_pci9118",
+	.id_table	= adl_pci9118_pci_table,
+	.probe		= adl_pci9118_pci_probe,
+	.remove		= __devexit_p(adl_pci9118_pci_remove),
+};
+module_comedi_pci_driver(adl_pci9118_driver, adl_pci9118_pci_driver);
 
 MODULE_AUTHOR("Comedi http://www.comedi.org");
 MODULE_DESCRIPTION("Comedi low-level driver");
diff --git a/drivers/staging/comedi/drivers/adq12b.c b/drivers/staging/comedi/drivers/adq12b.c
index 5361f318b01..7d585a12050 100644
--- a/drivers/staging/comedi/drivers/adq12b.c
+++ b/drivers/staging/comedi/drivers/adq12b.c
@@ -125,24 +125,6 @@ struct adq12b_board {
 	int do_chans;
 };
 
-static const struct adq12b_board adq12b_boards[] = {
-	{
-	 .name = "adq12b",
-	 .ai_se_chans = 16,
-	 .ai_diff_chans = 8,
-	 .ai_bits = 12,
-	 .di_chans = 5,
-	 .do_chans = 8}
-/* potentially, more adq-based deviced will be added */
-/*,
-	.name = "adq12b",
-	.ai_chans = 16,  // this is just for reference, hardcoded again later
-	.ai_bits = 12,
-	.di_chans = 8,
-	.do_chans = 5
-	}*/
-};
-
 #define thisboard ((const struct adq12b_board *)dev->board_ptr)
 
 struct adq12b_private {
@@ -156,41 +138,88 @@ struct adq12b_private {
 #define devpriv ((struct adq12b_private *)dev->private)
 
 /*
- * The struct comedi_driver structure tells the Comedi core module
- * which functions to call to configure/deconfigure (attach/detach)
- * the board, and also about the kernel module that contains
- * the device code.
+ * "instructions" read/write data in "one-shot" or "software-triggered"
+ * mode.
  */
-static int adq12b_attach(struct comedi_device *dev,
-			 struct comedi_devconfig *it);
-static int adq12b_detach(struct comedi_device *dev);
-
-static struct comedi_driver driver_adq12b = {
-	.driver_name = "adq12b",
-	.module = THIS_MODULE,
-	.attach = adq12b_attach,
-	.detach = adq12b_detach,
-	.board_name = &adq12b_boards[0].name,
-	.offset = sizeof(struct adq12b_board),
-	.num_names = ARRAY_SIZE(adq12b_boards),
-};
 
 static int adq12b_ai_rinsn(struct comedi_device *dev,
 			   struct comedi_subdevice *s, struct comedi_insn *insn,
-			   unsigned int *data);
+			   unsigned int *data)
+{
+	int n, i;
+	int range, channel;
+	unsigned char hi, lo, status;
+
+	/* change channel and range only if it is different from the previous */
+	range = CR_RANGE(insn->chanspec);
+	channel = CR_CHAN(insn->chanspec);
+	if (channel != devpriv->last_channel || range != devpriv->last_range) {
+		outb((range << 4) | channel, dev->iobase + ADQ12B_CTREG);
+		udelay(50);	/* wait for the mux to settle */
+	}
+
+	/* trigger conversion */
+	status = inb(dev->iobase + ADQ12B_ADLOW);
+
+	/* convert n samples */
+	for (n = 0; n < insn->n; n++) {
+
+		/* wait for end of conversion */
+		i = 0;
+		do {
+			/* udelay(1); */
+			status = inb(dev->iobase + ADQ12B_STINR);
+			status = status & ADQ12B_EOC;
+		} while (status == 0 && ++i < TIMEOUT);
+		/* } while (++i < 10); */
+
+		/* read data */
+		hi = inb(dev->iobase + ADQ12B_ADHIG);
+		lo = inb(dev->iobase + ADQ12B_ADLOW);
+
+		/* printk("debug: chan=%d range=%d status=%d hi=%d lo=%d\n",
+		       channel, range, status,  hi, lo); */
+		data[n] = (hi << 8) | lo;
+
+	}
+
+	/* return the number of samples read/written */
+	return n;
+}
+
 static int adq12b_di_insn_bits(struct comedi_device *dev,
 			       struct comedi_subdevice *s,
-			       struct comedi_insn *insn, unsigned int *data);
+			       struct comedi_insn *insn, unsigned int *data)
+{
+
+	/* only bits 0-4 have information about digital inputs */
+	data[1] = (inb(dev->iobase + ADQ12B_STINR) & (0x1f));
+
+	return 2;
+}
+
 static int adq12b_do_insn_bits(struct comedi_device *dev,
 			       struct comedi_subdevice *s,
-			       struct comedi_insn *insn, unsigned int *data);
+			       struct comedi_insn *insn, unsigned int *data)
+{
+	int channel;
+
+	for (channel = 0; channel < 8; channel++)
+		if (((data[0] >> channel) & 0x01) != 0)
+			outb((((data[1] >> channel) & 0x01) << 3) | channel,
+			     dev->iobase + ADQ12B_OUTBR);
+
+	/* store information to retrieve when asked for reading */
+	if (data[0]) {
+		devpriv->digital_state &= ~data[0];
+		devpriv->digital_state |= (data[0] & data[1]);
+	}
+
+	data[1] = devpriv->digital_state;
+
+	return 2;
+}
 
-/*
- * Attach is called by the Comedi core to configure the driver
- * for a particular board.  If you specified a board_name array
- * in the driver structure, dev->board_ptr contains that
- * address.
- */
 static int adq12b_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 {
 	struct comedi_subdevice *s;
@@ -295,125 +324,34 @@ static int adq12b_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 	return 0;
 }
 
-/*
- * _detach is called to deconfigure a device.  It should deallocate
- * resources.
- * This function is also called when _attach() fails, so it should be
- * careful not to release resources that were not necessarily
- * allocated by _attach().  dev->private and dev->subdevices are
- * deallocated automatically by the core.
- */
-static int adq12b_detach(struct comedi_device *dev)
+static void adq12b_detach(struct comedi_device *dev)
 {
 	if (dev->iobase)
 		release_region(dev->iobase, ADQ12B_SIZE);
-
 	kfree(devpriv);
-
-	printk(KERN_INFO "comedi%d: adq12b: removed\n", dev->minor);
-
-	return 0;
-}
-
-/*
- * "instructions" read/write data in "one-shot" or "software-triggered"
- * mode.
- */
-
-static int adq12b_ai_rinsn(struct comedi_device *dev,
-			   struct comedi_subdevice *s, struct comedi_insn *insn,
-			   unsigned int *data)
-{
-	int n, i;
-	int range, channel;
-	unsigned char hi, lo, status;
-
-	/* change channel and range only if it is different from the previous */
-	range = CR_RANGE(insn->chanspec);
-	channel = CR_CHAN(insn->chanspec);
-	if (channel != devpriv->last_channel || range != devpriv->last_range) {
-		outb((range << 4) | channel, dev->iobase + ADQ12B_CTREG);
-		udelay(50);	/* wait for the mux to settle */
-	}
-
-	/* trigger conversion */
-	status = inb(dev->iobase + ADQ12B_ADLOW);
-
-	/* convert n samples */
-	for (n = 0; n < insn->n; n++) {
-
-		/* wait for end of conversion */
-		i = 0;
-		do {
-			/* udelay(1); */
-			status = inb(dev->iobase + ADQ12B_STINR);
-			status = status & ADQ12B_EOC;
-		} while (status == 0 && ++i < TIMEOUT);
-		/* } while (++i < 10); */
-
-		/* read data */
-		hi = inb(dev->iobase + ADQ12B_ADHIG);
-		lo = inb(dev->iobase + ADQ12B_ADLOW);
-
-		/* printk("debug: chan=%d range=%d status=%d hi=%d lo=%d\n",
-		       channel, range, status,  hi, lo); */
-		data[n] = (hi << 8) | lo;
-
-	}
-
-	/* return the number of samples read/written */
-	return n;
-}
-
-static int adq12b_di_insn_bits(struct comedi_device *dev,
-			       struct comedi_subdevice *s,
-			       struct comedi_insn *insn, unsigned int *data)
-{
-
-	/* only bits 0-4 have information about digital inputs */
-	data[1] = (inb(dev->iobase + ADQ12B_STINR) & (0x1f));
-
-	return 2;
 }
 
-static int adq12b_do_insn_bits(struct comedi_device *dev,
-			       struct comedi_subdevice *s,
-			       struct comedi_insn *insn, unsigned int *data)
-{
-	int channel;
-
-	for (channel = 0; channel < 8; channel++)
-		if (((data[0] >> channel) & 0x01) != 0)
-			outb((((data[1] >> channel) & 0x01) << 3) | channel,
-			     dev->iobase + ADQ12B_OUTBR);
-
-	/* store information to retrieve when asked for reading */
-	if (data[0]) {
-		devpriv->digital_state &= ~data[0];
-		devpriv->digital_state |= (data[0] & data[1]);
-	}
-
-	data[1] = devpriv->digital_state;
-
-	return 2;
-}
-
-/*
- * A convenient macro that defines init_module() and cleanup_module(),
- * as necessary.
- */
-static int __init driver_adq12b_init_module(void)
-{
-	return comedi_driver_register(&driver_adq12b);
-}
-
-static void __exit driver_adq12b_cleanup_module(void)
-{
-	comedi_driver_unregister(&driver_adq12b);
-}
+static const struct adq12b_board adq12b_boards[] = {
+	{
+		.name		= "adq12b",
+		.ai_se_chans	= 16,
+		.ai_diff_chans	= 8,
+		.ai_bits	= 12,
+		.di_chans	= 5,
+		.do_chans	= 8,
+	},
+};
 
-module_init(driver_adq12b_init_module);
-module_exit(driver_adq12b_cleanup_module);
+static struct comedi_driver adq12b_driver = {
+	.driver_name	= "adq12b",
+	.module		= THIS_MODULE,
+	.attach		= adq12b_attach,
+	.detach		= adq12b_detach,
+	.board_name	= &adq12b_boards[0].name,
+	.offset		= sizeof(struct adq12b_board),
+	.num_names	= ARRAY_SIZE(adq12b_boards),
+};
+module_comedi_driver(adq12b_driver);
 
 MODULE_AUTHOR("Comedi http://www.comedi.org");
 MODULE_DESCRIPTION("Comedi low-level driver");
diff --git a/drivers/staging/comedi/drivers/adv_pci1710.c b/drivers/staging/comedi/drivers/adv_pci1710.c
index 8318c82a555..de8c98cfe36 100644
--- a/drivers/staging/comedi/drivers/adv_pci1710.c
+++ b/drivers/staging/comedi/drivers/adv_pci1710.c
@@ -191,10 +191,6 @@ static const struct comedi_lrange range_pci171x_da = { 2, {
 							   }
 };
 
-static int pci1710_attach(struct comedi_device *dev,
-			  struct comedi_devconfig *it);
-static int pci1710_detach(struct comedi_device *dev);
-
 struct boardtype {
 	const char *name;	/*  board name */
 	int device_id;
@@ -216,17 +212,6 @@ struct boardtype {
 	unsigned int fifo_half_size;	/*  size of FIFO/2 */
 };
 
-static DEFINE_PCI_DEVICE_TABLE(pci1710_pci_table) = {
-	{ PCI_DEVICE(PCI_VENDOR_ID_ADVANTECH, 0x1710) },
-	{ PCI_DEVICE(PCI_VENDOR_ID_ADVANTECH, 0x1711) },
-	{ PCI_DEVICE(PCI_VENDOR_ID_ADVANTECH, 0x1713) },
-	{ PCI_DEVICE(PCI_VENDOR_ID_ADVANTECH, 0x1720) },
-	{ PCI_DEVICE(PCI_VENDOR_ID_ADVANTECH, 0x1731) },
-	{ 0 }
-};
-
-MODULE_DEVICE_TABLE(pci, pci1710_pci_table);
-
 static const struct boardtype boardtypes[] = {
 	{"pci1710", 0x1710,
 	 IORANGE_171x, 1, TYPE_PCI171X,
@@ -264,18 +249,6 @@ static const struct boardtype boardtypes[] = {
 	{.name = DRV_NAME},
 };
 
-#define n_boardtypes (sizeof(boardtypes)/sizeof(struct boardtype))
-
-static struct comedi_driver driver_pci1710 = {
-	.driver_name = DRV_NAME,
-	.module = THIS_MODULE,
-	.attach = pci1710_attach,
-	.detach = pci1710_detach,
-	.num_names = n_boardtypes,
-	.board_name = &boardtypes[0].name,
-	.offset = sizeof(struct boardtype),
-};
-
 struct pci1710_private {
 	struct pci_dev *pcidev;	/*  ptr to PCI device */
 	char valid;		/*  card is usable */
@@ -676,7 +649,9 @@ static void interrupt_pci1710_every_sample(void *d)
 			     s->async->buf_int_count, s->async->buf_int_ptr,
 			     s->async->buf_user_count, s->async->buf_user_ptr);
 			DPRINTK("adv_pci1710 EDBG: EOS2\n");
-			if ((!devpriv->neverending_ai) && (devpriv->ai_act_scan >= devpriv->ai_scans)) {	/*  all data sampled */
+			if ((!devpriv->neverending_ai) &&
+			    (devpriv->ai_act_scan >= devpriv->ai_scans)) {
+				/*  all data sampled */
 				pci171x_ai_cancel(dev, s);
 				s->async->events |= COMEDI_CB_EOA;
 				comedi_event(dev, s);
@@ -804,8 +779,8 @@ static irqreturn_t interrupt_service_pci1710(int irq, void *d)
 		irq);
 	if (!dev->attached)	/*  is device attached? */
 		return IRQ_NONE;	/*  no, exit */
-
-	if (!(inw(dev->iobase + PCI171x_STATUS) & Status_IRQ))	/*  is this interrupt from our board? */
+	/*  is this interrupt from our board? */
+	if (!(inw(dev->iobase + PCI171x_STATUS) & Status_IRQ))
 		return IRQ_NONE;	/*  no, exit */
 
 	DPRINTK("adv_pci1710 EDBG: interrupt_service_pci1710() ST: %4x\n",
@@ -814,7 +789,7 @@ static irqreturn_t interrupt_service_pci1710(int irq, void *d)
 	if (devpriv->ai_et) {	/*  Switch from initial TRIG_EXT to TRIG_xxx. */
 		devpriv->ai_et = 0;
 		devpriv->CntrlReg &= Control_CNT0;
-		devpriv->CntrlReg |= Control_SW;	/*  set software trigger */
+		devpriv->CntrlReg |= Control_SW; /* set software trigger */
 		outw(devpriv->CntrlReg, dev->iobase + PCI171x_CONTROL);
 		devpriv->CntrlReg = devpriv->ai_et_CntrlReg;
 		outb(0, dev->iobase + PCI171x_CLRFIFO);
@@ -865,7 +840,8 @@ static int pci171x_ai_docmd_and_mode(int mode, struct comedi_device *dev,
 	devpriv->neverending_ai = 0;
 
 	devpriv->CntrlReg &= Control_CNT0;
-	if ((devpriv->ai_flags & TRIG_WAKE_EOS)) {	/*  don't we want wake up every scan?            devpriv->ai_eos=1; */
+	/*  don't we want wake up every scan?  devpriv->ai_eos=1; */
+	if ((devpriv->ai_flags & TRIG_WAKE_EOS)) {
 		devpriv->ai_eos = 1;
 	} else {
 		devpriv->CntrlReg |= Control_ONEFH;
@@ -982,13 +958,13 @@ static int pci171x_ai_cmdtest(struct comedi_device *dev,
 #ifdef PCI171X_EXTDEBUG
 		pci171x_cmdtest_out(1, cmd);
 #endif
-		DPRINTK
-		    ("adv_pci1710 EDBG: BGN: pci171x_ai_cmdtest(...) err=%d ret=1\n",
-		     err);
+		DPRINTK(
+		"adv_pci1710 EDBG: BGN: pci171x_ai_cmdtest(...) err=%d ret=1\n",
+		err);
 		return 1;
 	}
 
-	/* step 2: make sure trigger sources are unique and mutually compatible */
+	/* step2: make sure trigger srcs are unique and mutually compatible */
 
 	if (cmd->start_src != TRIG_NOW && cmd->start_src != TRIG_EXT) {
 		cmd->start_src = TRIG_NOW;
@@ -1015,9 +991,9 @@ static int pci171x_ai_cmdtest(struct comedi_device *dev,
 #ifdef PCI171X_EXTDEBUG
 		pci171x_cmdtest_out(2, cmd);
 #endif
-		DPRINTK
-		    ("adv_pci1710 EDBG: BGN: pci171x_ai_cmdtest(...) err=%d ret=2\n",
-		     err);
+		DPRINTK(
+		"adv_pci1710 EDBG: BGN: pci171x_ai_cmdtest(...) err=%d ret=2\n",
+		err);
 		return 2;
 	}
 
@@ -1065,9 +1041,9 @@ static int pci171x_ai_cmdtest(struct comedi_device *dev,
 #ifdef PCI171X_EXTDEBUG
 		pci171x_cmdtest_out(3, cmd);
 #endif
-		DPRINTK
-		    ("adv_pci1710 EDBG: BGN: pci171x_ai_cmdtest(...) err=%d ret=3\n",
-		     err);
+		DPRINTK(
+		"adv_pci1710 EDBG: BGN: pci171x_ai_cmdtest(...) err=%d ret=3\n",
+		err);
 		return 3;
 	}
 
@@ -1160,48 +1136,41 @@ static int check_channel_list(struct comedi_device *dev,
 		return 0;
 	}
 
-	if (n_chan > 1) {
-		chansegment[0] = chanlist[0];	/*  first channel is every time ok */
-		for (i = 1, seglen = 1; i < n_chan; i++, seglen++) {	/*  build part of chanlist */
-			/*  printk("%d. %d %d\n",i,CR_CHAN(chanlist[i]),CR_RANGE(chanlist[i])); */
-			if (chanlist[0] == chanlist[i])
-				break;	/*  we detect loop, this must by finish */
-			if (CR_CHAN(chanlist[i]) & 1)	/*  odd channel cann't by differencial */
-				if (CR_AREF(chanlist[i]) == AREF_DIFF) {
-					comedi_error(dev,
-						     "Odd channel can't be differential input!\n");
-					return 0;
-				}
-			nowmustbechan =
-			    (CR_CHAN(chansegment[i - 1]) + 1) % s->n_chan;
-			if (CR_AREF(chansegment[i - 1]) == AREF_DIFF)
-				nowmustbechan = (nowmustbechan + 1) % s->n_chan;
-			if (nowmustbechan != CR_CHAN(chanlist[i])) {	/*  channel list isn't continuous :-( */
-				printk
-				    ("channel list must be continuous! chanlist[%i]=%d but must be %d or %d!\n",
-				     i, CR_CHAN(chanlist[i]), nowmustbechan,
-				     CR_CHAN(chanlist[0]));
-				return 0;
-			}
-			chansegment[i] = chanlist[i];	/*  well, this is next correct channel in list */
+	if (n_chan == 1)
+		return 1; /* seglen=1 */
+
+	chansegment[0] = chanlist[0]; /*  first channel is every time ok */
+	for (i = 1, seglen = 1; i < n_chan; i++, seglen++) {
+		if (chanlist[0] == chanlist[i])
+			break;	/*  we detected a loop, stop */
+		if ((CR_CHAN(chanlist[i]) & 1) &&
+		    (CR_AREF(chanlist[i]) == AREF_DIFF)) {
+			comedi_error(dev, "Odd channel cannot be differential input!\n");
+			return 0;
 		}
+		nowmustbechan = (CR_CHAN(chansegment[i - 1]) + 1) % s->n_chan;
+		if (CR_AREF(chansegment[i - 1]) == AREF_DIFF)
+			nowmustbechan = (nowmustbechan + 1) % s->n_chan;
+		if (nowmustbechan != CR_CHAN(chanlist[i])) {
+			printk("channel list must be continuous! chanlist[%i]=%d but must be %d or %d!\n",
+			       i, CR_CHAN(chanlist[i]), nowmustbechan,
+			       CR_CHAN(chanlist[0]));
+			return 0;
+		}
+		chansegment[i] = chanlist[i]; /* next correct channel in list */
+	}
 
-		for (i = 0, segpos = 0; i < n_chan; i++) {	/*  check whole chanlist */
-			/* printk("%d %d=%d %d\n",CR_CHAN(chansegment[i%seglen]),CR_RANGE(chansegment[i%seglen]),CR_CHAN(chanlist[i]),CR_RANGE(chanlist[i])); */
-			if (chanlist[i] != chansegment[i % seglen]) {
-				printk
-				    ("bad channel, reference or range number! chanlist[%i]=%d,%d,%d and not %d,%d,%d!\n",
-				     i, CR_CHAN(chansegment[i]),
-				     CR_RANGE(chansegment[i]),
-				     CR_AREF(chansegment[i]),
-				     CR_CHAN(chanlist[i % seglen]),
-				     CR_RANGE(chanlist[i % seglen]),
-				     CR_AREF(chansegment[i % seglen]));
-				return 0;	/*  chan/gain list is strange */
-			}
+	for (i = 0, segpos = 0; i < n_chan; i++) {
+		if (chanlist[i] != chansegment[i % seglen]) {
+			printk("bad channel, reference or range number! chanlist[%i]=%d,%d,%d and not %d,%d,%d!\n",
+			       i, CR_CHAN(chansegment[i]),
+			       CR_RANGE(chansegment[i]),
+			       CR_AREF(chansegment[i]),
+			       CR_CHAN(chanlist[i % seglen]),
+			       CR_RANGE(chanlist[i % seglen]),
+			       CR_AREF(chansegment[i % seglen]));
+			return 0;
 		}
-	} else {
-		seglen = 1;
 	}
 	return seglen;
 }
@@ -1221,14 +1190,14 @@ static void setup_channel_list(struct comedi_device *dev,
 	DPRINTK("SegLen: %d\n", seglen);
 	for (i = 0; i < seglen; i++) {	/*  store range list to card */
 		chanprog = muxonechan[CR_CHAN(chanlist[i])];
-		outw(chanprog, dev->iobase + PCI171x_MUX);	/* select channel */
+		outw(chanprog, dev->iobase + PCI171x_MUX); /* select channel */
 		range = this_board->rangecode_ai[CR_RANGE(chanlist[i])];
 		if (CR_AREF(chanlist[i]) == AREF_DIFF)
 			range |= 0x0020;
-		outw(range, dev->iobase + PCI171x_RANGE);	/* select gain */
+		outw(range, dev->iobase + PCI171x_RANGE); /* select gain */
 #ifdef PCI171x_PARANOIDCHECK
 		devpriv->act_chanlist[i] =
-		    (CR_CHAN(chanlist[i]) << 12) & 0xf000;
+			(CR_CHAN(chanlist[i]) << 12) & 0xf000;
 #endif
 		DPRINTK("GS: %2d. [%4x]=%4x %4x\n", i, chanprog, range,
 			devpriv->act_chanlist[i]);
@@ -1236,13 +1205,14 @@ static void setup_channel_list(struct comedi_device *dev,
 #ifdef PCI171x_PARANOIDCHECK
 	for ( ; i < n_chan; i++) { /* store remainder of channel list */
 		devpriv->act_chanlist[i] =
-		    (CR_CHAN(chanlist[i]) << 12) & 0xf000;
+			(CR_CHAN(chanlist[i]) << 12) & 0xf000;
 	}
 #endif
 
 	devpriv->ai_et_MuxVal =
-	    CR_CHAN(chanlist[0]) | (CR_CHAN(chanlist[seglen - 1]) << 8);
-	outw(devpriv->ai_et_MuxVal, dev->iobase + PCI171x_MUX);	/* select channel interval to scan */
+		CR_CHAN(chanlist[0]) | (CR_CHAN(chanlist[seglen - 1]) << 8);
+	/* select channel interval to scan */
+	outw(devpriv->ai_et_MuxVal, dev->iobase + PCI171x_MUX);
 	DPRINTK("MUX: %4x L%4x.H%4x\n",
 		CR_CHAN(chanlist[0]) | (CR_CHAN(chanlist[seglen - 1]) << 8),
 		CR_CHAN(chanlist[0]), CR_CHAN(chanlist[seglen - 1]));
@@ -1365,9 +1335,6 @@ static int pci1710_reset(struct comedi_device *dev)
 	DPRINTK("adv_pci1710 EDBG: END: pci1710_reset(...)\n");
 }
 
-/*
-==============================================================================
-*/
 static int pci1710_attach(struct comedi_device *dev,
 			  struct comedi_devconfig *it)
 {
@@ -1398,13 +1365,13 @@ static int pci1710_attach(struct comedi_device *dev,
 	while (NULL != (pcidev = pci_get_device(PCI_VENDOR_ID_ADVANTECH,
 						PCI_ANY_ID, pcidev))) {
 		if (strcmp(this_board->name, DRV_NAME) == 0) {
-			for (i = 0; i < n_boardtypes; ++i) {
+			for (i = 0; i < ARRAY_SIZE(boardtypes); ++i) {
 				if (pcidev->device == boardtypes[i].device_id) {
 					board_index = i;
 					break;
 				}
 			}
-			if (i == n_boardtypes)
+			if (i == ARRAY_SIZE(boardtypes))
 				continue;
 		} else {
 			if (pcidev->device != boardtypes[board_index].device_id)
@@ -1584,12 +1551,8 @@ static int pci1710_attach(struct comedi_device *dev,
 	return 0;
 }
 
-/*
-==============================================================================
-*/
-static int pci1710_detach(struct comedi_device *dev)
+static void pci1710_detach(struct comedi_device *dev)
 {
-
 	if (dev->private) {
 		if (devpriv->valid)
 			pci1710_reset(dev);
@@ -1598,57 +1561,49 @@ static int pci1710_detach(struct comedi_device *dev)
 		if (devpriv->pcidev) {
 			if (dev->iobase)
 				comedi_pci_disable(devpriv->pcidev);
-
 			pci_dev_put(devpriv->pcidev);
 		}
 	}
-
-	return 0;
 }
 
-/*
-==============================================================================
-*/
-static int __devinit driver_pci1710_pci_probe(struct pci_dev *dev,
-					      const struct pci_device_id *ent)
+static struct comedi_driver adv_pci1710_driver = {
+	.driver_name	= "adv_pci1710",
+	.module		= THIS_MODULE,
+	.attach		= pci1710_attach,
+	.detach		= pci1710_detach,
+	.num_names	= ARRAY_SIZE(boardtypes),
+	.board_name	= &boardtypes[0].name,
+	.offset		= sizeof(struct boardtype),
+};
+
+static int __devinit adv_pci1710_pci_probe(struct pci_dev *dev,
+					   const struct pci_device_id *ent)
 {
-	return comedi_pci_auto_config(dev, driver_pci1710.driver_name);
+	return comedi_pci_auto_config(dev, &adv_pci1710_driver);
 }
 
-static void __devexit driver_pci1710_pci_remove(struct pci_dev *dev)
+static void __devexit adv_pci1710_pci_remove(struct pci_dev *dev)
 {
 	comedi_pci_auto_unconfig(dev);
 }
 
-static struct pci_driver driver_pci1710_pci_driver = {
-	.id_table = pci1710_pci_table,
-	.probe = &driver_pci1710_pci_probe,
-	.remove = __devexit_p(&driver_pci1710_pci_remove)
+static DEFINE_PCI_DEVICE_TABLE(adv_pci1710_pci_table) = {
+	{ PCI_DEVICE(PCI_VENDOR_ID_ADVANTECH, 0x1710) },
+	{ PCI_DEVICE(PCI_VENDOR_ID_ADVANTECH, 0x1711) },
+	{ PCI_DEVICE(PCI_VENDOR_ID_ADVANTECH, 0x1713) },
+	{ PCI_DEVICE(PCI_VENDOR_ID_ADVANTECH, 0x1720) },
+	{ PCI_DEVICE(PCI_VENDOR_ID_ADVANTECH, 0x1731) },
+	{ 0 }
 };
+MODULE_DEVICE_TABLE(pci, adv_pci1710_pci_table);
 
-static int __init driver_pci1710_init_module(void)
-{
-	int retval;
-
-	retval = comedi_driver_register(&driver_pci1710);
-	if (retval < 0)
-		return retval;
-
-	driver_pci1710_pci_driver.name = (char *)driver_pci1710.driver_name;
-	return pci_register_driver(&driver_pci1710_pci_driver);
-}
-
-static void __exit driver_pci1710_cleanup_module(void)
-{
-	pci_unregister_driver(&driver_pci1710_pci_driver);
-	comedi_driver_unregister(&driver_pci1710);
-}
-
-module_init(driver_pci1710_init_module);
-module_exit(driver_pci1710_cleanup_module);
-/*
-==============================================================================
-*/
+static struct pci_driver adv_pci1710_pci_driver = {
+	.name		= "adv_pci1710",
+	.id_table	= adv_pci1710_pci_table,
+	.probe		= adv_pci1710_pci_probe,
+	.remove		= __devexit_p(adv_pci1710_pci_remove),
+};
+module_comedi_pci_driver(adv_pci1710_driver, adv_pci1710_pci_driver);
 
 MODULE_AUTHOR("Comedi http://www.comedi.org");
 MODULE_DESCRIPTION("Comedi low-level driver");
diff --git a/drivers/staging/comedi/drivers/adv_pci1723.c b/drivers/staging/comedi/drivers/adv_pci1723.c
index 29455a8e88b..336addcbce3 100644
--- a/drivers/staging/comedi/drivers/adv_pci1723.c
+++ b/drivers/staging/comedi/drivers/adv_pci1723.c
@@ -150,36 +150,6 @@ static const struct pci1723_board boardtypes[] = {
 	 },
 };
 
-/*
- * This is used by modprobe to translate PCI IDs to drivers.
- * Should only be used for PCI and ISA-PnP devices
- */
-static DEFINE_PCI_DEVICE_TABLE(pci1723_pci_table) = {
-	{ PCI_DEVICE(PCI_VENDOR_ID_ADVANTECH, 0x1723) },
-	{ 0 }
-};
-
-MODULE_DEVICE_TABLE(pci, pci1723_pci_table);
-
-/*
- * The struct comedi_driver structure tells the Comedi core module
- * which functions to call to configure/deconfigure (attach/detach)
- * the board, and also about the kernel module that contains
- * the device code.
- */
-static int pci1723_attach(struct comedi_device *dev,
-			  struct comedi_devconfig *it);
-static int pci1723_detach(struct comedi_device *dev);
-
-#define n_boardtypes (sizeof(boardtypes)/sizeof(struct pci1723_board))
-
-static struct comedi_driver driver_pci1723 = {
-	.driver_name = "adv_pci1723",
-	.module = THIS_MODULE,
-	.attach = pci1723_attach,
-	.detach = pci1723_detach,
-};
-
 /* This structure is for data unique to this hardware driver. */
 struct pci1723_private {
 	int valid;		/* card is usable; */
@@ -319,10 +289,6 @@ static int pci1723_dio_insn_bits(struct comedi_device *dev,
 	return 2;
 }
 
-/*
- * Attach is called by the Comedi core to configure the driver
- * for a pci1723 board.
- */
 static int pci1723_attach(struct comedi_device *dev,
 			  struct comedi_devconfig *it)
 {
@@ -465,73 +431,50 @@ static int pci1723_attach(struct comedi_device *dev,
 	return 0;
 }
 
-/*
- * _detach is called to deconfigure a device.  It should deallocate
- * resources.
- * This function is also called when _attach() fails, so it should be
- * careful not to release resources that were not necessarily
- * allocated by _attach().  dev->private and dev->subdevices are
- * deallocated automatically by the core.
- */
-static int pci1723_detach(struct comedi_device *dev)
+static void pci1723_detach(struct comedi_device *dev)
 {
-	printk(KERN_ERR "comedi%d: pci1723: remove\n", dev->minor);
-
 	if (dev->private) {
 		if (devpriv->valid)
 			pci1723_reset(dev);
-
 		if (devpriv->pcidev) {
 			if (dev->iobase)
 				comedi_pci_disable(devpriv->pcidev);
 			pci_dev_put(devpriv->pcidev);
 		}
 	}
-
-	return 0;
 }
 
-/*
- * A convenient macro that defines init_module() and cleanup_module(),
- * as necessary.
- */
-static int __devinit driver_pci1723_pci_probe(struct pci_dev *dev,
-					      const struct pci_device_id *ent)
+static struct comedi_driver adv_pci1723_driver = {
+	.driver_name	= "adv_pci1723",
+	.module		= THIS_MODULE,
+	.attach		= pci1723_attach,
+	.detach		= pci1723_detach,
+};
+
+static int __devinit adv_pci1723_pci_probe(struct pci_dev *dev,
+					   const struct pci_device_id *ent)
 {
-	return comedi_pci_auto_config(dev, driver_pci1723.driver_name);
+	return comedi_pci_auto_config(dev, &adv_pci1723_driver);
 }
 
-static void __devexit driver_pci1723_pci_remove(struct pci_dev *dev)
+static void __devexit adv_pci1723_pci_remove(struct pci_dev *dev)
 {
 	comedi_pci_auto_unconfig(dev);
 }
 
-static struct pci_driver driver_pci1723_pci_driver = {
-	.id_table = pci1723_pci_table,
-	.probe = &driver_pci1723_pci_probe,
-	.remove = __devexit_p(&driver_pci1723_pci_remove)
+static DEFINE_PCI_DEVICE_TABLE(adv_pci1723_pci_table) = {
+	{ PCI_DEVICE(PCI_VENDOR_ID_ADVANTECH, 0x1723) },
+	{ 0 }
 };
+MODULE_DEVICE_TABLE(pci, adv_pci1723_pci_table);
 
-static int __init driver_pci1723_init_module(void)
-{
-	int retval;
-
-	retval = comedi_driver_register(&driver_pci1723);
-	if (retval < 0)
-		return retval;
-
-	driver_pci1723_pci_driver.name = (char *)driver_pci1723.driver_name;
-	return pci_register_driver(&driver_pci1723_pci_driver);
-}
-
-static void __exit driver_pci1723_cleanup_module(void)
-{
-	pci_unregister_driver(&driver_pci1723_pci_driver);
-	comedi_driver_unregister(&driver_pci1723);
-}
-
-module_init(driver_pci1723_init_module);
-module_exit(driver_pci1723_cleanup_module);
+static struct pci_driver adv_pci1723_pci_driver = {
+	.name		= "adv_pci1723",
+	.id_table	= adv_pci1723_pci_table,
+	.probe		= adv_pci1723_pci_probe,
+	.remove		= __devexit_p(adv_pci1723_pci_remove),
+};
+module_comedi_pci_driver(adv_pci1723_driver, adv_pci1723_pci_driver);
 
 MODULE_AUTHOR("Comedi http://www.comedi.org");
 MODULE_DESCRIPTION("Comedi low-level driver");
diff --git a/drivers/staging/comedi/drivers/adv_pci_dio.c b/drivers/staging/comedi/drivers/adv_pci_dio.c
index 7af068f4a74..43a32dc12cd 100644
--- a/drivers/staging/comedi/drivers/adv_pci_dio.c
+++ b/drivers/staging/comedi/drivers/adv_pci_dio.c
@@ -237,10 +237,6 @@ enum hw_io_access {
 
 #define OMBCMD_RETRY	0x03	/* 3 times try request before error */
 
-static int pci_dio_attach(struct comedi_device *dev,
-			  struct comedi_devconfig *it);
-static int pci_dio_detach(struct comedi_device *dev);
-
 struct diosubd_data {
 	int chans;		/*  num of chans */
 	int addr;		/*  PCI address ofset */
@@ -263,26 +259,6 @@ struct dio_boardtype {
 	enum hw_io_access io_access;
 };
 
-static DEFINE_PCI_DEVICE_TABLE(pci_dio_pci_table) = {
-	{ PCI_DEVICE(PCI_VENDOR_ID_ADVANTECH, 0x1730) },
-	{ PCI_DEVICE(PCI_VENDOR_ID_ADVANTECH, 0x1733) },
-	{ PCI_DEVICE(PCI_VENDOR_ID_ADVANTECH, 0x1734) },
-	{ PCI_DEVICE(PCI_VENDOR_ID_ADVANTECH, 0x1735) },
-	{ PCI_DEVICE(PCI_VENDOR_ID_ADVANTECH, 0x1736) },
-	{ PCI_DEVICE(PCI_VENDOR_ID_ADVANTECH, 0x1739) },
-	{ PCI_DEVICE(PCI_VENDOR_ID_ADVANTECH, 0x1750) },
-	{ PCI_DEVICE(PCI_VENDOR_ID_ADVANTECH, 0x1751) },
-	{ PCI_DEVICE(PCI_VENDOR_ID_ADVANTECH, 0x1752) },
-	{ PCI_DEVICE(PCI_VENDOR_ID_ADVANTECH, 0x1753) },
-	{ PCI_DEVICE(PCI_VENDOR_ID_ADVANTECH, 0x1754) },
-	{ PCI_DEVICE(PCI_VENDOR_ID_ADVANTECH, 0x1756) },
-	{ PCI_DEVICE(PCI_VENDOR_ID_ADVANTECH, 0x1760) },
-	{ PCI_DEVICE(PCI_VENDOR_ID_ADVANTECH, 0x1762) },
-	{ 0 }
-};
-
-MODULE_DEVICE_TABLE(pci, pci_dio_pci_table);
-
 static const struct dio_boardtype boardtypes[] = {
 	{"pci1730", PCI_VENDOR_ID_ADVANTECH, 0x1730, PCIDIO_MAINREG,
 	 TYPE_PCI1730,
@@ -406,15 +382,6 @@ static const struct dio_boardtype boardtypes[] = {
 	 IO_16b}
 };
 
-#define n_boardtypes (sizeof(boardtypes)/sizeof(struct dio_boardtype))
-
-static struct comedi_driver driver_pci_dio = {
-	.driver_name = "adv_pci_dio",
-	.module = THIS_MODULE,
-	.attach = pci_dio_attach,
-	.detach = pci_dio_detach
-};
-
 struct pci_dio_private {
 	struct pci_dio_private *prev;	/*  previous private struct */
 	struct pci_dio_private *next;	/*  next private struct */
@@ -1116,9 +1083,6 @@ static int CheckAndAllocCard(struct comedi_device *dev,
 	return 1;
 }
 
-/*
-==============================================================================
-*/
 static int pci_dio_attach(struct comedi_device *dev,
 			  struct comedi_devconfig *it)
 {
@@ -1134,7 +1098,7 @@ static int pci_dio_attach(struct comedi_device *dev,
 
 	for_each_pci_dev(pcidev) {
 		/*  loop through cards supported by this driver */
-		for (i = 0; i < n_boardtypes; ++i) {
+		for (i = 0; i < ARRAY_SIZE(boardtypes); ++i) {
 			if (boardtypes[i].vendor_id != pcidev->vendor)
 				continue;
 			if (boardtypes[i].device_id != pcidev->device)
@@ -1162,7 +1126,7 @@ static int pci_dio_attach(struct comedi_device *dev,
 		return -EIO;
 	}
 
-	if (comedi_pci_enable(pcidev, driver_pci_dio.driver_name)) {
+	if (comedi_pci_enable(pcidev, dev->driver->driver_name)) {
 		dev_err(dev->hw_dev, "Error: Can't enable PCI device and request regions!\n");
 		return -EIO;
 	}
@@ -1246,10 +1210,7 @@ static int pci_dio_attach(struct comedi_device *dev,
 	return 0;
 }
 
-/*
-==============================================================================
-*/
-static int pci_dio_detach(struct comedi_device *dev)
+static void pci_dio_detach(struct comedi_device *dev)
 {
 	int i, j;
 	struct comedi_subdevice *s;
@@ -1258,20 +1219,14 @@ static int pci_dio_detach(struct comedi_device *dev)
 	if (dev->private) {
 		if (devpriv->valid)
 			pci_dio_reset(dev);
-
-
-		/* This shows the silliness of using this kind of
-		 * scheme for numbering subdevices.  Don't do it.  --ds */
 		subdev = 0;
 		for (i = 0; i < MAX_DI_SUBDEVS; i++) {
 			if (this_board->sdi[i].chans)
 				subdev++;
-
 		}
 		for (i = 0; i < MAX_DO_SUBDEVS; i++) {
 			if (this_board->sdo[i].chans)
 				subdev++;
-
 		}
 		for (i = 0; i < MAX_DIO_SUBDEVG; i++) {
 			for (j = 0; j < this_board->sdio[i].regs; j++) {
@@ -1280,82 +1235,73 @@ static int pci_dio_detach(struct comedi_device *dev)
 				subdev++;
 			}
 		}
-
 		if (this_board->boardid.chans)
 			subdev++;
-
 		for (i = 0; i < MAX_8254_SUBDEVS; i++)
 			if (this_board->s8254[i].chans)
 				subdev++;
-
 		for (i = 0; i < dev->n_subdevices; i++) {
 			s = dev->subdevices + i;
 			s->private = NULL;
 		}
-
 		if (devpriv->pcidev) {
 			if (dev->iobase)
 				comedi_pci_disable(devpriv->pcidev);
-
 			pci_dev_put(devpriv->pcidev);
 		}
-
 		if (devpriv->prev)
 			devpriv->prev->next = devpriv->next;
 		else
 			pci_priv = devpriv->next;
-
 		if (devpriv->next)
 			devpriv->next->prev = devpriv->prev;
-
 	}
-
-	return 0;
 }
 
-/*
-==============================================================================
-*/
-static int __devinit driver_pci_dio_pci_probe(struct pci_dev *dev,
-					      const struct pci_device_id *ent)
+static struct comedi_driver adv_pci_dio_driver = {
+	.driver_name	= "adv_pci_dio",
+	.module		= THIS_MODULE,
+	.attach		= pci_dio_attach,
+	.detach		= pci_dio_detach
+};
+
+static int __devinit adv_pci_dio_pci_probe(struct pci_dev *dev,
+					   const struct pci_device_id *ent)
 {
-	return comedi_pci_auto_config(dev, driver_pci_dio.driver_name);
+	return comedi_pci_auto_config(dev, &adv_pci_dio_driver);
 }
 
-static void __devexit driver_pci_dio_pci_remove(struct pci_dev *dev)
+static void __devexit adv_pci_dio_pci_remove(struct pci_dev *dev)
 {
 	comedi_pci_auto_unconfig(dev);
 }
 
-static struct pci_driver driver_pci_dio_pci_driver = {
-	.id_table = pci_dio_pci_table,
-	.probe = &driver_pci_dio_pci_probe,
-	.remove = __devexit_p(&driver_pci_dio_pci_remove)
+static DEFINE_PCI_DEVICE_TABLE(adv_pci_dio_pci_table) = {
+	{ PCI_DEVICE(PCI_VENDOR_ID_ADVANTECH, 0x1730) },
+	{ PCI_DEVICE(PCI_VENDOR_ID_ADVANTECH, 0x1733) },
+	{ PCI_DEVICE(PCI_VENDOR_ID_ADVANTECH, 0x1734) },
+	{ PCI_DEVICE(PCI_VENDOR_ID_ADVANTECH, 0x1735) },
+	{ PCI_DEVICE(PCI_VENDOR_ID_ADVANTECH, 0x1736) },
+	{ PCI_DEVICE(PCI_VENDOR_ID_ADVANTECH, 0x1739) },
+	{ PCI_DEVICE(PCI_VENDOR_ID_ADVANTECH, 0x1750) },
+	{ PCI_DEVICE(PCI_VENDOR_ID_ADVANTECH, 0x1751) },
+	{ PCI_DEVICE(PCI_VENDOR_ID_ADVANTECH, 0x1752) },
+	{ PCI_DEVICE(PCI_VENDOR_ID_ADVANTECH, 0x1753) },
+	{ PCI_DEVICE(PCI_VENDOR_ID_ADVANTECH, 0x1754) },
+	{ PCI_DEVICE(PCI_VENDOR_ID_ADVANTECH, 0x1756) },
+	{ PCI_DEVICE(PCI_VENDOR_ID_ADVANTECH, 0x1760) },
+	{ PCI_DEVICE(PCI_VENDOR_ID_ADVANTECH, 0x1762) },
+	{ 0 }
 };
+MODULE_DEVICE_TABLE(pci, adv_pci_dio_pci_table);
 
-static int __init driver_pci_dio_init_module(void)
-{
-	int retval;
-
-	retval = comedi_driver_register(&driver_pci_dio);
-	if (retval < 0)
-		return retval;
-
-	driver_pci_dio_pci_driver.name = (char *)driver_pci_dio.driver_name;
-	return pci_register_driver(&driver_pci_dio_pci_driver);
-}
-
-static void __exit driver_pci_dio_cleanup_module(void)
-{
-	pci_unregister_driver(&driver_pci_dio_pci_driver);
-	comedi_driver_unregister(&driver_pci_dio);
-}
-
-module_init(driver_pci_dio_init_module);
-module_exit(driver_pci_dio_cleanup_module);
-/*
-==============================================================================
-*/
+static struct pci_driver adv_pci_dio_pci_driver = {
+	.name		= "adv_pci_dio",
+	.id_table	= adv_pci_dio_pci_table,
+	.probe		= adv_pci_dio_pci_probe,
+	.remove		= __devexit_p(adv_pci_dio_pci_remove),
+};
+module_comedi_pci_driver(adv_pci_dio_driver, adv_pci_dio_pci_driver);
 
 MODULE_AUTHOR("Comedi http://www.comedi.org");
 MODULE_DESCRIPTION("Comedi low-level driver");
diff --git a/drivers/staging/comedi/drivers/aio_aio12_8.c b/drivers/staging/comedi/drivers/aio_aio12_8.c
index b0f98e5e4bf..64d82bc4ffe 100644
--- a/drivers/staging/comedi/drivers/aio_aio12_8.c
+++ b/drivers/staging/comedi/drivers/aio_aio12_8.c
@@ -209,36 +209,23 @@ static int aio_aio12_8_attach(struct comedi_device *dev,
 	return 0;
 }
 
-static int aio_aio12_8_detach(struct comedi_device *dev)
+static void aio_aio12_8_detach(struct comedi_device *dev)
 {
 	subdev_8255_cleanup(dev, &dev->subdevices[2]);
 	if (dev->iobase)
 		release_region(dev->iobase, 24);
-	return 0;
 }
 
-static struct comedi_driver driver_aio_aio12_8 = {
-	.driver_name = "aio_aio12_8",
-	.module = THIS_MODULE,
-	.attach = aio_aio12_8_attach,
-	.detach = aio_aio12_8_detach,
-	.board_name = &board_types[0].name,
-	.num_names = 1,
-	.offset = sizeof(struct aio12_8_boardtype),
+static struct comedi_driver aio_aio12_8_driver = {
+	.driver_name	= "aio_aio12_8",
+	.module		= THIS_MODULE,
+	.attach		= aio_aio12_8_attach,
+	.detach		= aio_aio12_8_detach,
+	.board_name	= &board_types[0].name,
+	.num_names	= ARRAY_SIZE(board_types),
+	.offset		= sizeof(struct aio12_8_boardtype),
 };
-
-static int __init driver_aio_aio12_8_init_module(void)
-{
-	return comedi_driver_register(&driver_aio_aio12_8);
-}
-
-static void __exit driver_aio_aio12_8_cleanup_module(void)
-{
-	comedi_driver_unregister(&driver_aio_aio12_8);
-}
-
-module_init(driver_aio_aio12_8_init_module);
-module_exit(driver_aio_aio12_8_cleanup_module);
+module_comedi_driver(aio_aio12_8_driver);
 
 MODULE_AUTHOR("Comedi http://www.comedi.org");
 MODULE_DESCRIPTION("Comedi low-level driver");
diff --git a/drivers/staging/comedi/drivers/aio_iiro_16.c b/drivers/staging/comedi/drivers/aio_iiro_16.c
index 160b0a0f4f1..04f6f94b1f4 100644
--- a/drivers/staging/comedi/drivers/aio_iiro_16.c
+++ b/drivers/staging/comedi/drivers/aio_iiro_16.c
@@ -67,30 +67,41 @@ struct aio_iiro_16_private {
 
 #define	devpriv	((struct aio_iiro_16_private *) dev->private)
 
-static int aio_iiro_16_attach(struct comedi_device *dev,
-			      struct comedi_devconfig *it);
-
-static int aio_iiro_16_detach(struct comedi_device *dev);
-
-static struct comedi_driver driver_aio_iiro_16 = {
-	.driver_name = "aio_iiro_16",
-	.module = THIS_MODULE,
-	.attach = aio_iiro_16_attach,
-	.detach = aio_iiro_16_detach,
-	.board_name = &aio_iiro_16_boards[0].name,
-	.offset = sizeof(struct aio_iiro_16_board),
-	.num_names = ARRAY_SIZE(aio_iiro_16_boards),
-};
+static int aio_iiro_16_dio_insn_bits_write(struct comedi_device *dev,
+					   struct comedi_subdevice *s,
+					   struct comedi_insn *insn,
+					   unsigned int *data)
+{
+	if (insn->n != 2)
+		return -EINVAL;
+
+	if (data[0]) {
+		s->state &= ~data[0];
+		s->state |= data[0] & data[1];
+		outb(s->state & 0xff, dev->iobase + AIO_IIRO_16_RELAY_0_7);
+		outb((s->state >> 8) & 0xff,
+		     dev->iobase + AIO_IIRO_16_RELAY_8_15);
+	}
+
+	data[1] = s->state;
+
+	return 2;
+}
 
 static int aio_iiro_16_dio_insn_bits_read(struct comedi_device *dev,
 					  struct comedi_subdevice *s,
 					  struct comedi_insn *insn,
-					  unsigned int *data);
+					  unsigned int *data)
+{
+	if (insn->n != 2)
+		return -EINVAL;
 
-static int aio_iiro_16_dio_insn_bits_write(struct comedi_device *dev,
-					   struct comedi_subdevice *s,
-					   struct comedi_insn *insn,
-					   unsigned int *data);
+	data[1] = 0;
+	data[1] |= inb(dev->iobase + AIO_IIRO_16_INPUT_0_7);
+	data[1] |= inb(dev->iobase + AIO_IIRO_16_INPUT_8_15) << 8;
+
+	return 2;
+}
 
 static int aio_iiro_16_attach(struct comedi_device *dev,
 			      struct comedi_devconfig *it)
@@ -138,64 +149,22 @@ static int aio_iiro_16_attach(struct comedi_device *dev,
 	return 1;
 }
 
-static int aio_iiro_16_detach(struct comedi_device *dev)
+static void aio_iiro_16_detach(struct comedi_device *dev)
 {
-	printk(KERN_INFO "comedi%d: aio_iiro_16: remove\n", dev->minor);
-
 	if (dev->iobase)
 		release_region(dev->iobase, AIO_IIRO_16_SIZE);
-
-	return 0;
-}
-
-static int aio_iiro_16_dio_insn_bits_write(struct comedi_device *dev,
-					   struct comedi_subdevice *s,
-					   struct comedi_insn *insn,
-					   unsigned int *data)
-{
-	if (insn->n != 2)
-		return -EINVAL;
-
-	if (data[0]) {
-		s->state &= ~data[0];
-		s->state |= data[0] & data[1];
-		outb(s->state & 0xff, dev->iobase + AIO_IIRO_16_RELAY_0_7);
-		outb((s->state >> 8) & 0xff,
-		     dev->iobase + AIO_IIRO_16_RELAY_8_15);
-	}
-
-	data[1] = s->state;
-
-	return 2;
-}
-
-static int aio_iiro_16_dio_insn_bits_read(struct comedi_device *dev,
-					  struct comedi_subdevice *s,
-					  struct comedi_insn *insn,
-					  unsigned int *data)
-{
-	if (insn->n != 2)
-		return -EINVAL;
-
-	data[1] = 0;
-	data[1] |= inb(dev->iobase + AIO_IIRO_16_INPUT_0_7);
-	data[1] |= inb(dev->iobase + AIO_IIRO_16_INPUT_8_15) << 8;
-
-	return 2;
-}
-
-static int __init driver_aio_iiro_16_init_module(void)
-{
-	return comedi_driver_register(&driver_aio_iiro_16);
 }
 
-static void __exit driver_aio_iiro_16_cleanup_module(void)
-{
-	comedi_driver_unregister(&driver_aio_iiro_16);
-}
-
-module_init(driver_aio_iiro_16_init_module);
-module_exit(driver_aio_iiro_16_cleanup_module);
+static struct comedi_driver aio_iiro_16_driver = {
+	.driver_name	= "aio_iiro_16",
+	.module		= THIS_MODULE,
+	.attach		= aio_iiro_16_attach,
+	.detach		= aio_iiro_16_detach,
+	.board_name	= &aio_iiro_16_boards[0].name,
+	.offset		= sizeof(struct aio_iiro_16_board),
+	.num_names	= ARRAY_SIZE(aio_iiro_16_boards),
+};
+module_comedi_driver(aio_iiro_16_driver);
 
 MODULE_AUTHOR("Comedi http://www.comedi.org");
 MODULE_DESCRIPTION("Comedi low-level driver");
diff --git a/drivers/staging/comedi/drivers/amplc_dio200.c b/drivers/staging/comedi/drivers/amplc_dio200.c
index 566cc441145..c9c5d97b3ca 100644
--- a/drivers/staging/comedi/drivers/amplc_dio200.c
+++ b/drivers/staging/comedi/drivers/amplc_dio200.c
@@ -217,6 +217,14 @@ order they appear in the channel list.
 
 #define DIO200_DRIVER_NAME	"amplc_dio200"
 
+#ifdef CONFIG_COMEDI_AMPLC_DIO200_ISA_MODULE
+#define CONFIG_COMEDI_AMPLC_DIO200_ISA
+#endif
+
+#ifdef CONFIG_COMEDI_AMPLC_DIO200_PCI_MODULE
+#define CONFIG_COMEDI_AMPLC_DIO200_PCI
+#endif
+
 /* PCI IDs */
 #define PCI_VENDOR_ID_AMPLICON 0x14dc
 #define PCI_DEVICE_ID_AMPLICON_PCI272 0x000a
@@ -274,10 +282,14 @@ enum dio200_model {
 };
 
 enum dio200_layout {
+#ifdef CONFIG_COMEDI_AMPLC_DIO200_ISA
 	pc212_layout,
 	pc214_layout,
+#endif
 	pc215_layout,
+#ifdef CONFIG_COMEDI_AMPLC_DIO200_ISA
 	pc218_layout,
+#endif
 	pc272_layout
 };
 
@@ -290,6 +302,7 @@ struct dio200_board {
 };
 
 static const struct dio200_board dio200_boards[] = {
+#ifdef CONFIG_COMEDI_AMPLC_DIO200_ISA
 	{
 	 .name = "pc212e",
 	 .bustype = isa_bustype,
@@ -308,15 +321,6 @@ static const struct dio200_board dio200_boards[] = {
 	 .model = pc215e_model,
 	 .layout = pc215_layout,
 	 },
-#ifdef CONFIG_COMEDI_PCI
-	{
-	 .name = "pci215",
-	 .devid = PCI_DEVICE_ID_AMPLICON_PCI215,
-	 .bustype = pci_bustype,
-	 .model = pci215_model,
-	 .layout = pc215_layout,
-	 },
-#endif
 	{
 	 .name = "pc218e",
 	 .bustype = isa_bustype,
@@ -329,7 +333,15 @@ static const struct dio200_board dio200_boards[] = {
 	 .model = pc272e_model,
 	 .layout = pc272_layout,
 	 },
-#ifdef CONFIG_COMEDI_PCI
+#endif
+#ifdef CONFIG_COMEDI_AMPLC_DIO200_PCI
+	{
+	 .name = "pci215",
+	 .devid = PCI_DEVICE_ID_AMPLICON_PCI215,
+	 .bustype = pci_bustype,
+	 .model = pci215_model,
+	 .layout = pc215_layout,
+	 },
 	{
 	 .name = "pci272",
 	 .devid = PCI_DEVICE_ID_AMPLICON_PCI272,
@@ -337,8 +349,6 @@ static const struct dio200_board dio200_boards[] = {
 	 .model = pci272_model,
 	 .layout = pc272_layout,
 	 },
-#endif
-#ifdef CONFIG_COMEDI_PCI
 	{
 	 .name = DIO200_DRIVER_NAME,
 	 .devid = PCI_DEVICE_ID_INVALID,
@@ -367,6 +377,7 @@ struct dio200_layout_struct {
 };
 
 static const struct dio200_layout_struct dio200_layouts[] = {
+#ifdef CONFIG_COMEDI_AMPLC_DIO200_ISA
 	[pc212_layout] = {
 			  .n_subdevs = 6,
 			  .sdtype = {sd_8255, sd_8254, sd_8254, sd_8254,
@@ -385,6 +396,7 @@ static const struct dio200_layout_struct dio200_layouts[] = {
 			  .has_int_sce = 0,
 			  .has_clk_gat_sce = 0,
 			  },
+#endif
 	[pc215_layout] = {
 			  .n_subdevs = 5,
 			  .sdtype = {sd_8255, sd_8255, sd_8254,
@@ -394,6 +406,7 @@ static const struct dio200_layout_struct dio200_layouts[] = {
 			  .has_int_sce = 1,
 			  .has_clk_gat_sce = 1,
 			  },
+#ifdef CONFIG_COMEDI_AMPLC_DIO200_ISA
 	[pc218_layout] = {
 			  .n_subdevs = 7,
 			  .sdtype = {sd_8254, sd_8254, sd_8255, sd_8254,
@@ -405,6 +418,7 @@ static const struct dio200_layout_struct dio200_layouts[] = {
 			  .has_int_sce = 1,
 			  .has_clk_gat_sce = 1,
 			  },
+#endif
 	[pc272_layout] = {
 			  .n_subdevs = 4,
 			  .sdtype = {sd_8255, sd_8255, sd_8255,
@@ -419,7 +433,7 @@ static const struct dio200_layout_struct dio200_layouts[] = {
  * PCI driver table.
  */
 
-#ifdef CONFIG_COMEDI_PCI
+#ifdef CONFIG_COMEDI_AMPLC_DIO200_PCI
 static DEFINE_PCI_DEVICE_TABLE(dio200_pci_table) = {
 	{ PCI_DEVICE(PCI_VENDOR_ID_AMPLICON, PCI_DEVICE_ID_AMPLICON_PCI215) },
 	{ PCI_DEVICE(PCI_VENDOR_ID_AMPLICON, PCI_DEVICE_ID_AMPLICON_PCI272) },
@@ -427,7 +441,7 @@ static DEFINE_PCI_DEVICE_TABLE(dio200_pci_table) = {
 };
 
 MODULE_DEVICE_TABLE(pci, dio200_pci_table);
-#endif /* CONFIG_COMEDI_PCI */
+#endif /* CONFIG_COMEDI_AMPLC_DIO200_PCI */
 
 /*
  * Useful for shorthand access to the particular board structure
@@ -441,7 +455,7 @@ MODULE_DEVICE_TABLE(pci, dio200_pci_table);
    feel free to suggest moving the variable to the struct comedi_device struct.
  */
 struct dio200_private {
-#ifdef CONFIG_COMEDI_PCI
+#ifdef CONFIG_COMEDI_AMPLC_DIO200_PCI
 	struct pci_dev *pci_dev;	/* PCI device */
 #endif
 	int intr_sd;
@@ -479,7 +493,7 @@ struct dio200_subdev_intr {
  */
 static int dio200_attach(struct comedi_device *dev,
 			 struct comedi_devconfig *it);
-static int dio200_detach(struct comedi_device *dev);
+static void dio200_detach(struct comedi_device *dev);
 static struct comedi_driver driver_amplc_dio200 = {
 	.driver_name = DIO200_DRIVER_NAME,
 	.module = THIS_MODULE,
@@ -490,12 +504,12 @@ static struct comedi_driver driver_amplc_dio200 = {
 	.num_names = ARRAY_SIZE(dio200_boards),
 };
 
-#ifdef CONFIG_COMEDI_PCI
+#ifdef CONFIG_COMEDI_AMPLC_DIO200_PCI
 static int __devinit driver_amplc_dio200_pci_probe(struct pci_dev *dev,
 						   const struct pci_device_id
 						   *ent)
 {
-	return comedi_pci_auto_config(dev, driver_amplc_dio200.driver_name);
+	return comedi_pci_auto_config(dev, &driver_amplc_dio200);
 }
 
 static void __devexit driver_amplc_dio200_pci_remove(struct pci_dev *dev)
@@ -549,7 +563,7 @@ module_exit(driver_amplc_dio200_cleanup_module);
  * This function looks for a PCI device matching the requested board name,
  * bus and slot.
  */
-#ifdef CONFIG_COMEDI_PCI
+#ifdef CONFIG_COMEDI_AMPLC_DIO200_PCI
 static int
 dio200_find_pci(struct comedi_device *dev, int bus, int slot,
 		struct pci_dev **pci_dev_p)
@@ -611,6 +625,7 @@ dio200_find_pci(struct comedi_device *dev, int bus, int slot,
  * This function checks and requests an I/O region, reporting an error
  * if there is a conflict.
  */
+#ifdef CONFIG_COMEDI_AMPLC_DIO200_ISA
 static int
 dio200_request_region(unsigned minor, unsigned long from, unsigned long extent)
 {
@@ -621,6 +636,7 @@ dio200_request_region(unsigned minor, unsigned long from, unsigned long extent)
 	}
 	return 0;
 }
+#endif
 
 /*
  * 'insn_bits' function for an 'INTERRUPT' subdevice.
@@ -1332,7 +1348,7 @@ static int dio200_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 	struct comedi_subdevice *s;
 	unsigned long iobase = 0;
 	unsigned int irq = 0;
-#ifdef CONFIG_COMEDI_PCI
+#ifdef CONFIG_COMEDI_AMPLC_DIO200_PCI
 	struct pci_dev *pci_dev = NULL;
 	int bus = 0, slot = 0;
 #endif
@@ -1354,12 +1370,14 @@ static int dio200_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 
 	/* Process options. */
 	switch (thisboard->bustype) {
+#ifdef CONFIG_COMEDI_AMPLC_DIO200_ISA
 	case isa_bustype:
 		iobase = it->options[0];
 		irq = it->options[1];
 		share_irq = 0;
 		break;
-#ifdef CONFIG_COMEDI_PCI
+#endif
+#ifdef CONFIG_COMEDI_AMPLC_DIO200_PCI
 	case pci_bustype:
 		bus = it->options[0];
 		slot = it->options[1];
@@ -1382,7 +1400,7 @@ static int dio200_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 	devpriv->intr_sd = -1;
 
 	/* Enable device and reserve I/O spaces. */
-#ifdef CONFIG_COMEDI_PCI
+#ifdef CONFIG_COMEDI_AMPLC_DIO200_PCI
 	if (pci_dev) {
 		ret = comedi_pci_enable(pci_dev, DIO200_DRIVER_NAME);
 		if (ret < 0) {
@@ -1396,9 +1414,11 @@ static int dio200_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 	} else
 #endif
 	{
+#ifdef CONFIG_COMEDI_AMPLC_DIO200_ISA
 		ret = dio200_request_region(dev->minor, iobase, DIO200_IO_SIZE);
 		if (ret < 0)
 			return ret;
+#endif
 	}
 	dev->iobase = iobase;
 
@@ -1474,12 +1494,19 @@ static int dio200_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 	}
 
 	printk(KERN_INFO "comedi%d: %s ", dev->minor, dev->board_name);
-	if (thisboard->bustype == isa_bustype) {
+	switch (thisboard->bustype) {
+#ifdef CONFIG_COMEDI_AMPLC_DIO200_ISA
+	case isa_bustype:
 		printk("(base %#lx) ", iobase);
-	} else {
-#ifdef CONFIG_COMEDI_PCI
+		break;
+#endif
+#ifdef CONFIG_COMEDI_AMPLC_DIO200_PCI
+	case pci_bustype:
 		printk("(pci %s) ", pci_name(pci_dev));
+		break;
 #endif
+	default:
+		break;
 	}
 	if (irq)
 		printk("(irq %u%s) ", irq, (dev->irq ? "" : " UNAVAILABLE"));
@@ -1491,22 +1518,11 @@ static int dio200_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 	return 1;
 }
 
-/*
- * _detach is called to deconfigure a device.  It should deallocate
- * resources.
- * This function is also called when _attach() fails, so it should be
- * careful not to release resources that were not necessarily
- * allocated by _attach().  dev->private and dev->subdevices are
- * deallocated automatically by the core.
- */
-static int dio200_detach(struct comedi_device *dev)
+static void dio200_detach(struct comedi_device *dev)
 {
 	const struct dio200_layout_struct *layout;
 	unsigned n;
 
-	printk(KERN_DEBUG "comedi%d: %s: detach\n", dev->minor,
-	       DIO200_DRIVER_NAME);
-
 	if (dev->irq)
 		free_irq(dev->irq, dev);
 	if (dev->subdevices) {
@@ -1529,7 +1545,7 @@ static int dio200_detach(struct comedi_device *dev)
 		}
 	}
 	if (devpriv) {
-#ifdef CONFIG_COMEDI_PCI
+#ifdef CONFIG_COMEDI_AMPLC_DIO200_PCI
 		if (devpriv->pci_dev) {
 			if (dev->iobase)
 				comedi_pci_disable(devpriv->pci_dev);
@@ -1537,15 +1553,12 @@ static int dio200_detach(struct comedi_device *dev)
 		} else
 #endif
 		{
+#ifdef CONFIG_COMEDI_AMPLC_DIO200_ISA
 			if (dev->iobase)
 				release_region(dev->iobase, DIO200_IO_SIZE);
+#endif
 		}
 	}
-	if (dev->board_name)
-		printk(KERN_INFO "comedi%d: %s removed\n",
-		       dev->minor, dev->board_name);
-
-	return 0;
 }
 
 MODULE_AUTHOR("Comedi http://www.comedi.org");
diff --git a/drivers/staging/comedi/drivers/amplc_pc236.c b/drivers/staging/comedi/drivers/amplc_pc236.c
index 7972cadd403..57ba3228b1a 100644
--- a/drivers/staging/comedi/drivers/amplc_pc236.c
+++ b/drivers/staging/comedi/drivers/amplc_pc236.c
@@ -63,6 +63,14 @@ unused.
 
 #define PC236_DRIVER_NAME	"amplc_pc236"
 
+#ifdef CONFIG_COMEDI_AMPLC_PC236_ISA_MODULE
+#define CONFIG_COMEDI_AMPLC_PC236_ISA
+#endif
+
+#ifdef CONFIG_COMEDI_AMPLC_PC236_PCI_MODULE
+#define CONFIG_COMEDI_AMPLC_PC236_PCI
+#endif
+
 /* PCI236 PCI configuration register information */
 #define PCI_VENDOR_ID_AMPLICON 0x14dc
 #define PCI_DEVICE_ID_AMPLICON_PCI236 0x0009
@@ -106,13 +114,15 @@ struct pc236_board {
 	enum pc236_model model;
 };
 static const struct pc236_board pc236_boards[] = {
+#ifdef CONFIG_COMEDI_AMPLC_PC236_ISA
 	{
 	 .name = "pc36at",
 	 .fancy_name = "PC36AT",
 	 .bustype = isa_bustype,
 	 .model = pc36at_model,
 	 },
-#ifdef CONFIG_COMEDI_PCI
+#endif
+#ifdef CONFIG_COMEDI_AMPLC_PC236_PCI
 	{
 	 .name = "pci236",
 	 .fancy_name = "PCI236",
@@ -120,8 +130,6 @@ static const struct pc236_board pc236_boards[] = {
 	 .bustype = pci_bustype,
 	 .model = pci236_model,
 	 },
-#endif
-#ifdef CONFIG_COMEDI_PCI
 	{
 	 .name = PC236_DRIVER_NAME,
 	 .fancy_name = PC236_DRIVER_NAME,
@@ -132,14 +140,14 @@ static const struct pc236_board pc236_boards[] = {
 #endif
 };
 
-#ifdef CONFIG_COMEDI_PCI
+#ifdef CONFIG_COMEDI_AMPLC_PC236_PCI
 static DEFINE_PCI_DEVICE_TABLE(pc236_pci_table) = {
 	{ PCI_DEVICE(PCI_VENDOR_ID_AMPLICON, PCI_DEVICE_ID_AMPLICON_PCI236) },
 	{0}
 };
 
 MODULE_DEVICE_TABLE(pci, pc236_pci_table);
-#endif /* CONFIG_COMEDI_PCI */
+#endif /* CONFIG_COMEDI_AMPLC_PC236_PCI */
 
 /*
  * Useful for shorthand access to the particular board structure
@@ -151,7 +159,7 @@ MODULE_DEVICE_TABLE(pci, pc236_pci_table);
    feel free to suggest moving the variable to the struct comedi_device struct.
  */
 struct pc236_private {
-#ifdef CONFIG_COMEDI_PCI
+#ifdef CONFIG_COMEDI_AMPLC_PC236_PCI
 	/* PCI device */
 	struct pci_dev *pci_dev;
 	unsigned long lcr_iobase; /* PLX PCI9052 config registers in PCIBAR1 */
@@ -168,7 +176,7 @@ struct pc236_private {
  * the device code.
  */
 static int pc236_attach(struct comedi_device *dev, struct comedi_devconfig *it);
-static int pc236_detach(struct comedi_device *dev);
+static void pc236_detach(struct comedi_device *dev);
 static struct comedi_driver driver_amplc_pc236 = {
 	.driver_name = PC236_DRIVER_NAME,
 	.module = THIS_MODULE,
@@ -179,12 +187,12 @@ static struct comedi_driver driver_amplc_pc236 = {
 	.num_names = ARRAY_SIZE(pc236_boards),
 };
 
-#ifdef CONFIG_COMEDI_PCI
+#ifdef CONFIG_COMEDI_AMPLC_PC236_PCI
 static int __devinit driver_amplc_pc236_pci_probe(struct pci_dev *dev,
 						  const struct pci_device_id
 						  *ent)
 {
-	return comedi_pci_auto_config(dev, driver_amplc_pc236.driver_name);
+	return comedi_pci_auto_config(dev, &driver_amplc_pc236);
 }
 
 static void __devexit driver_amplc_pc236_pci_remove(struct pci_dev *dev)
@@ -234,8 +242,10 @@ module_init(driver_amplc_pc236_init_module);
 module_exit(driver_amplc_pc236_cleanup_module);
 #endif
 
+#ifdef CONFIG_COMEDI_AMPLC_PC236_ISA
 static int pc236_request_region(unsigned minor, unsigned long from,
 				unsigned long extent);
+#endif
 static void pc236_intr_disable(struct comedi_device *dev);
 static void pc236_intr_enable(struct comedi_device *dev);
 static int pc236_intr_check(struct comedi_device *dev);
@@ -255,7 +265,7 @@ static irqreturn_t pc236_interrupt(int irq, void *d);
  * This function looks for a PCI device matching the requested board name,
  * bus and slot.
  */
-#ifdef CONFIG_COMEDI_PCI
+#ifdef CONFIG_COMEDI_AMPLC_PC236_PCI
 static int
 pc236_find_pci(struct comedi_device *dev, int bus, int slot,
 	       struct pci_dev **pci_dev_p)
@@ -324,7 +334,7 @@ static int pc236_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 	struct comedi_subdevice *s;
 	unsigned long iobase = 0;
 	unsigned int irq = 0;
-#ifdef CONFIG_COMEDI_PCI
+#ifdef CONFIG_COMEDI_AMPLC_PC236_PCI
 	struct pci_dev *pci_dev = NULL;
 	int bus = 0, slot = 0;
 #endif
@@ -345,12 +355,14 @@ static int pc236_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 	}
 	/* Process options. */
 	switch (thisboard->bustype) {
+#ifdef CONFIG_COMEDI_AMPLC_PC236_ISA
 	case isa_bustype:
 		iobase = it->options[0];
 		irq = it->options[1];
 		share_irq = 0;
 		break;
-#ifdef CONFIG_COMEDI_PCI
+#endif
+#ifdef CONFIG_COMEDI_AMPLC_PC236_PCI
 	case pci_bustype:
 		bus = it->options[0];
 		slot = it->options[1];
@@ -361,7 +373,7 @@ static int pc236_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 			return ret;
 		devpriv->pci_dev = pci_dev;
 		break;
-#endif /* CONFIG_COMEDI_PCI */
+#endif
 	default:
 		printk(KERN_ERR
 		       "comedi%d: %s: BUG! cannot determine board type!\n",
@@ -376,7 +388,7 @@ static int pc236_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 	dev->board_name = thisboard->name;
 
 	/* Enable device and reserve I/O spaces. */
-#ifdef CONFIG_COMEDI_PCI
+#ifdef CONFIG_COMEDI_AMPLC_PC236_PCI
 	if (pci_dev) {
 
 		ret = comedi_pci_enable(pci_dev, PC236_DRIVER_NAME);
@@ -392,9 +404,11 @@ static int pc236_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 	} else
 #endif
 	{
+#ifdef CONFIG_COMEDI_AMPLC_PC236_ISA
 		ret = pc236_request_region(dev->minor, iobase, PC236_IO_SIZE);
 		if (ret < 0)
 			return ret;
+#endif
 	}
 	dev->iobase = iobase;
 
@@ -439,12 +453,19 @@ static int pc236_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 		}
 	}
 	printk(KERN_INFO "comedi%d: %s ", dev->minor, dev->board_name);
-	if (thisboard->bustype == isa_bustype) {
+	switch (thisboard->bustype) {
+#ifdef CONFIG_COMEDI_AMPLC_PC236_ISA
+	case isa_bustype:
 		printk("(base %#lx) ", iobase);
-	} else {
-#ifdef CONFIG_COMEDI_PCI
+		break;
+#endif
+#ifdef CONFIG_COMEDI_AMPLC_PC236_PCI
+	case pci_bustype:
 		printk("(pci %s) ", pci_name(pci_dev));
+		break;
 #endif
+	default:
+		break;
 	}
 	if (irq)
 		printk("(irq %u%s) ", irq, (dev->irq ? "" : " UNAVAILABLE"));
@@ -456,27 +477,16 @@ static int pc236_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 	return 1;
 }
 
-/*
- * _detach is called to deconfigure a device.  It should deallocate
- * resources.
- * This function is also called when _attach() fails, so it should be
- * careful not to release resources that were not necessarily
- * allocated by _attach().  dev->private and dev->subdevices are
- * deallocated automatically by the core.
- */
-static int pc236_detach(struct comedi_device *dev)
+static void pc236_detach(struct comedi_device *dev)
 {
-	printk(KERN_DEBUG "comedi%d: %s: detach\n", dev->minor,
-	       PC236_DRIVER_NAME);
 	if (devpriv)
 		pc236_intr_disable(dev);
-
 	if (dev->irq)
 		free_irq(dev->irq, dev);
 	if (dev->subdevices)
 		subdev_8255_cleanup(dev, dev->subdevices + 0);
 	if (devpriv) {
-#ifdef CONFIG_COMEDI_PCI
+#ifdef CONFIG_COMEDI_AMPLC_PC236_PCI
 		if (devpriv->pci_dev) {
 			if (dev->iobase)
 				comedi_pci_disable(devpriv->pci_dev);
@@ -484,21 +494,19 @@ static int pc236_detach(struct comedi_device *dev)
 		} else
 #endif
 		{
+#ifdef CONFIG_COMEDI_AMPLC_PC236_ISA
 			if (dev->iobase)
 				release_region(dev->iobase, PC236_IO_SIZE);
+#endif
 		}
 	}
-	if (dev->board_name) {
-		printk(KERN_INFO "comedi%d: %s removed\n",
-		       dev->minor, dev->board_name);
-	}
-	return 0;
 }
 
 /*
  * This function checks and requests an I/O region, reporting an error
  * if there is a conflict.
  */
+#ifdef CONFIG_COMEDI_AMPLC_PC236_ISA
 static int pc236_request_region(unsigned minor, unsigned long from,
 				unsigned long extent)
 {
@@ -509,6 +517,7 @@ static int pc236_request_region(unsigned minor, unsigned long from,
 	}
 	return 0;
 }
+#endif
 
 /*
  * This function is called to mark the interrupt as disabled (no command
@@ -521,7 +530,7 @@ static void pc236_intr_disable(struct comedi_device *dev)
 
 	spin_lock_irqsave(&dev->spinlock, flags);
 	devpriv->enable_irq = 0;
-#ifdef CONFIG_COMEDI_PCI
+#ifdef CONFIG_COMEDI_AMPLC_PC236_PCI
 	if (devpriv->lcr_iobase)
 		outl(PCI236_INTR_DISABLE, devpriv->lcr_iobase + PLX9052_INTCSR);
 #endif
@@ -539,7 +548,7 @@ static void pc236_intr_enable(struct comedi_device *dev)
 
 	spin_lock_irqsave(&dev->spinlock, flags);
 	devpriv->enable_irq = 1;
-#ifdef CONFIG_COMEDI_PCI
+#ifdef CONFIG_COMEDI_AMPLC_PC236_PCI
 	if (devpriv->lcr_iobase)
 		outl(PCI236_INTR_ENABLE, devpriv->lcr_iobase + PLX9052_INTCSR);
 #endif
@@ -561,7 +570,7 @@ static int pc236_intr_check(struct comedi_device *dev)
 	spin_lock_irqsave(&dev->spinlock, flags);
 	if (devpriv->enable_irq) {
 		retval = 1;
-#ifdef CONFIG_COMEDI_PCI
+#ifdef CONFIG_COMEDI_AMPLC_PC236_PCI
 		if (devpriv->lcr_iobase) {
 			if ((inl(devpriv->lcr_iobase + PLX9052_INTCSR)
 			     & PLX9052_INTCSR_LI1STAT_MASK)
diff --git a/drivers/staging/comedi/drivers/amplc_pc263.c b/drivers/staging/comedi/drivers/amplc_pc263.c
index 191ac0d23ce..974d7450051 100644
--- a/drivers/staging/comedi/drivers/amplc_pc263.c
+++ b/drivers/staging/comedi/drivers/amplc_pc263.c
@@ -50,6 +50,14 @@ The state of the outputs can be read.
 
 #define PC263_DRIVER_NAME	"amplc_pc263"
 
+#ifdef CONFIG_COMEDI_AMPLC_PC263_ISA_MODULE
+#define CONFIG_COMEDI_AMPLC_PC263_ISA
+#endif
+
+#ifdef CONFIG_COMEDI_AMPLC_PC263_PCI_MODULE
+#define CONFIG_COMEDI_AMPLC_PC263_PCI
+#endif
+
 /* PCI263 PCI configuration register information */
 #define PCI_VENDOR_ID_AMPLICON 0x14dc
 #define PCI_DEVICE_ID_AMPLICON_PCI263 0x000c
@@ -73,13 +81,15 @@ struct pc263_board {
 	enum pc263_model model;
 };
 static const struct pc263_board pc263_boards[] = {
+#ifdef CONFIG_COMEDI_AMPLC_PC263_ISA
 	{
 	 .name = "pc263",
 	 .fancy_name = "PC263",
 	 .bustype = isa_bustype,
 	 .model = pc263_model,
 	 },
-#ifdef CONFIG_COMEDI_PCI
+#endif
+#ifdef CONFIG_COMEDI_AMPLC_PC263_PCI
 	{
 	 .name = "pci263",
 	 .fancy_name = "PCI263",
@@ -87,8 +97,6 @@ static const struct pc263_board pc263_boards[] = {
 	 .bustype = pci_bustype,
 	 .model = pci263_model,
 	 },
-#endif
-#ifdef CONFIG_COMEDI_PCI
 	{
 	 .name = PC263_DRIVER_NAME,
 	 .fancy_name = PC263_DRIVER_NAME,
@@ -99,14 +107,14 @@ static const struct pc263_board pc263_boards[] = {
 #endif
 };
 
-#ifdef CONFIG_COMEDI_PCI
+#ifdef CONFIG_COMEDI_AMPLC_PC263_PCI
 static DEFINE_PCI_DEVICE_TABLE(pc263_pci_table) = {
 	{ PCI_DEVICE(PCI_VENDOR_ID_AMPLICON, PCI_DEVICE_ID_AMPLICON_PCI263) },
 	{0}
 };
 
 MODULE_DEVICE_TABLE(pci, pc263_pci_table);
-#endif /* CONFIG_COMEDI_PCI */
+#endif /* CONFIG_COMEDI_AMPLC_PC263_PCI */
 
 /*
  * Useful for shorthand access to the particular board structure
@@ -117,14 +125,14 @@ MODULE_DEVICE_TABLE(pci, pc263_pci_table);
    several hardware drivers keep similar information in this structure,
    feel free to suggest moving the variable to the struct comedi_device struct.
 */
-#ifdef CONFIG_COMEDI_PCI
+#ifdef CONFIG_COMEDI_AMPLC_PC263_PCI
 struct pc263_private {
 	/* PCI device. */
 	struct pci_dev *pci_dev;
 };
 
 #define devpriv ((struct pc263_private *)dev->private)
-#endif /* CONFIG_COMEDI_PCI */
+#endif /* CONFIG_COMEDI_AMPLC_PC263_PCI */
 
 /*
  * The struct comedi_driver structure tells the Comedi core module
@@ -133,7 +141,7 @@ struct pc263_private {
  * the device code.
  */
 static int pc263_attach(struct comedi_device *dev, struct comedi_devconfig *it);
-static int pc263_detach(struct comedi_device *dev);
+static void pc263_detach(struct comedi_device *dev);
 static struct comedi_driver driver_amplc_pc263 = {
 	.driver_name = PC263_DRIVER_NAME,
 	.module = THIS_MODULE,
@@ -144,8 +152,10 @@ static struct comedi_driver driver_amplc_pc263 = {
 	.num_names = ARRAY_SIZE(pc263_boards),
 };
 
+#ifdef CONFIG_COMEDI_AMPLC_PC263_ISA
 static int pc263_request_region(unsigned minor, unsigned long from,
 				unsigned long extent);
+#endif
 static int pc263_dio_insn_bits(struct comedi_device *dev,
 			       struct comedi_subdevice *s,
 			       struct comedi_insn *insn, unsigned int *data);
@@ -157,7 +167,7 @@ static int pc263_dio_insn_config(struct comedi_device *dev,
  * This function looks for a PCI device matching the requested board name,
  * bus and slot.
  */
-#ifdef CONFIG_COMEDI_PCI
+#ifdef CONFIG_COMEDI_AMPLC_PC263_PCI
 static int
 pc263_find_pci(struct comedi_device *dev, int bus, int slot,
 	       struct pci_dev **pci_dev_p)
@@ -225,7 +235,7 @@ static int pc263_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 {
 	struct comedi_subdevice *s;
 	unsigned long iobase = 0;
-#ifdef CONFIG_COMEDI_PCI
+#ifdef CONFIG_COMEDI_AMPLC_PC263_PCI
 	struct pci_dev *pci_dev = NULL;
 	int bus = 0, slot = 0;
 #endif
@@ -237,7 +247,7 @@ static int pc263_attach(struct comedi_device *dev, struct comedi_devconfig *it)
  * Allocate the private structure area.  alloc_private() is a
  * convenient macro defined in comedidev.h.
  */
-#ifdef CONFIG_COMEDI_PCI
+#ifdef CONFIG_COMEDI_AMPLC_PC263_PCI
 	ret = alloc_private(dev, sizeof(struct pc263_private));
 	if (ret < 0) {
 		printk(KERN_ERR "comedi%d: error! out of memory!\n",
@@ -247,10 +257,12 @@ static int pc263_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 #endif
 	/* Process options. */
 	switch (thisboard->bustype) {
+#ifdef CONFIG_COMEDI_AMPLC_PC263_ISA
 	case isa_bustype:
 		iobase = it->options[0];
 		break;
-#ifdef CONFIG_COMEDI_PCI
+#endif
+#ifdef CONFIG_COMEDI_AMPLC_PC263_PCI
 	case pci_bustype:
 		bus = it->options[0];
 		slot = it->options[1];
@@ -260,7 +272,7 @@ static int pc263_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 			return ret;
 		devpriv->pci_dev = pci_dev;
 		break;
-#endif /* CONFIG_COMEDI_PCI */
+#endif
 	default:
 		printk(KERN_ERR
 		       "comedi%d: %s: BUG! cannot determine board type!\n",
@@ -275,7 +287,7 @@ static int pc263_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 	dev->board_name = thisboard->name;
 
 	/* Enable device and reserve I/O spaces. */
-#ifdef CONFIG_COMEDI_PCI
+#ifdef CONFIG_COMEDI_AMPLC_PC263_PCI
 	if (pci_dev) {
 		ret = comedi_pci_enable(pci_dev, PC263_DRIVER_NAME);
 		if (ret < 0) {
@@ -289,9 +301,11 @@ static int pc263_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 	} else
 #endif
 	{
+#ifdef CONFIG_COMEDI_AMPLC_PC263_ISA
 		ret = pc263_request_region(dev->minor, iobase, PC263_IO_SIZE);
 		if (ret < 0)
 			return ret;
+#endif
 	}
 	dev->iobase = iobase;
 
@@ -322,12 +336,18 @@ static int pc263_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 	s->state = s->state | (inb(dev->iobase) << 8);
 
 	printk(KERN_INFO "comedi%d: %s ", dev->minor, dev->board_name);
-	if (thisboard->bustype == isa_bustype) {
+	switch (thisboard->bustype) {
+#ifdef CONFIG_COMEDI_AMPLC_PC263_ISA
+	case isa_bustype:
 		printk("(base %#lx) ", iobase);
-	} else {
-#ifdef CONFIG_COMEDI_PCI
+		break;
+#endif
+#ifdef CONFIG_COMEDI_AMPLC_PC263_PCI
 		printk("(pci %s) ", pci_name(pci_dev));
+		break;
 #endif
+	default:
+		break;
 	}
 
 	printk("attached\n");
@@ -335,23 +355,13 @@ static int pc263_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 	return 1;
 }
 
-/*
- * _detach is called to deconfigure a device.  It should deallocate
- * resources.
- * This function is also called when _attach() fails, so it should be
- * careful not to release resources that were not necessarily
- * allocated by _attach().  dev->private and dev->subdevices are
- * deallocated automatically by the core.
- */
-static int pc263_detach(struct comedi_device *dev)
+static void pc263_detach(struct comedi_device *dev)
 {
-	printk(KERN_DEBUG "comedi%d: %s: detach\n", dev->minor,
-	       PC263_DRIVER_NAME);
-
-#ifdef CONFIG_COMEDI_PCI
-	if (devpriv) {
+#ifdef CONFIG_COMEDI_AMPLC_PC263_PCI
+	if (devpriv)
 #endif
-#ifdef CONFIG_COMEDI_PCI
+	{
+#ifdef CONFIG_COMEDI_AMPLC_PC263_PCI
 		if (devpriv->pci_dev) {
 			if (dev->iobase)
 				comedi_pci_disable(devpriv->pci_dev);
@@ -359,21 +369,19 @@ static int pc263_detach(struct comedi_device *dev)
 		} else
 #endif
 		{
+#ifdef CONFIG_COMEDI_AMPLC_PC263_ISA
 			if (dev->iobase)
 				release_region(dev->iobase, PC263_IO_SIZE);
+#endif
 		}
 	}
-	if (dev->board_name) {
-		printk(KERN_INFO "comedi%d: %s removed\n",
-		       dev->minor, dev->board_name);
-	}
-	return 0;
 }
 
 /*
  * This function checks and requests an I/O region, reporting an error
  * if there is a conflict.
  */
+#ifdef CONFIG_COMEDI_AMPLC_PC263_ISA
 static int pc263_request_region(unsigned minor, unsigned long from,
 				unsigned long extent)
 {
@@ -384,6 +392,7 @@ static int pc263_request_region(unsigned minor, unsigned long from,
 	}
 	return 0;
 }
+#endif
 
 /* DIO devices are slightly special.  Although it is possible to
  * implement the insn_read/insn_write interface, it is much more
@@ -429,12 +438,12 @@ static int pc263_dio_insn_config(struct comedi_device *dev,
  * A convenient macro that defines init_module() and cleanup_module(),
  * as necessary.
  */
-#ifdef CONFIG_COMEDI_PCI
+#ifdef CONFIG_COMEDI_AMPLC_PC263_PCI
 static int __devinit driver_amplc_pc263_pci_probe(struct pci_dev *dev,
 						  const struct pci_device_id
 						  *ent)
 {
-	return comedi_pci_auto_config(dev, driver_amplc_pc263.driver_name);
+	return comedi_pci_auto_config(dev, &driver_amplc_pc263);
 }
 
 static void __devexit driver_amplc_pc263_pci_remove(struct pci_dev *dev)
diff --git a/drivers/staging/comedi/drivers/amplc_pci224.c b/drivers/staging/comedi/drivers/amplc_pci224.c
index b278917cec2..fbf19cae874 100644
--- a/drivers/staging/comedi/drivers/amplc_pci224.c
+++ b/drivers/staging/comedi/drivers/amplc_pci224.c
@@ -380,18 +380,6 @@ static const struct pci224_board pci224_boards[] = {
 };
 
 /*
- * PCI driver table.
- */
-
-static DEFINE_PCI_DEVICE_TABLE(pci224_pci_table) = {
-	{ PCI_DEVICE(PCI_VENDOR_ID_AMPLICON, PCI_DEVICE_ID_AMPLICON_PCI224) },
-	{ PCI_DEVICE(PCI_VENDOR_ID_AMPLICON, PCI_DEVICE_ID_AMPLICON_PCI234) },
-	{0}
-};
-
-MODULE_DEVICE_TABLE(pci, pci224_pci_table);
-
-/*
  * Useful for shorthand access to the particular board structure
  */
 #define thisboard ((struct pci224_board *)dev->board_ptr)
@@ -422,65 +410,6 @@ struct pci224_private {
 #define devpriv ((struct pci224_private *)dev->private)
 
 /*
- * The struct comedi_driver structure tells the Comedi core module
- * which functions to call to configure/deconfigure (attach/detach)
- * the board, and also about the kernel module that contains
- * the device code.
- */
-static int pci224_attach(struct comedi_device *dev,
-			 struct comedi_devconfig *it);
-static int pci224_detach(struct comedi_device *dev);
-static struct comedi_driver driver_amplc_pci224 = {
-	.driver_name = DRIVER_NAME,
-	.module = THIS_MODULE,
-	.attach = pci224_attach,
-	.detach = pci224_detach,
-	.board_name = &pci224_boards[0].name,
-	.offset = sizeof(struct pci224_board),
-	.num_names = ARRAY_SIZE(pci224_boards),
-};
-
-static int __devinit driver_amplc_pci224_pci_probe(struct pci_dev *dev,
-						   const struct pci_device_id
-						   *ent)
-{
-	return comedi_pci_auto_config(dev, driver_amplc_pci224.driver_name);
-}
-
-static void __devexit driver_amplc_pci224_pci_remove(struct pci_dev *dev)
-{
-	comedi_pci_auto_unconfig(dev);
-}
-
-static struct pci_driver driver_amplc_pci224_pci_driver = {
-	.id_table = pci224_pci_table,
-	.probe = &driver_amplc_pci224_pci_probe,
-	.remove = __devexit_p(&driver_amplc_pci224_pci_remove)
-};
-
-static int __init driver_amplc_pci224_init_module(void)
-{
-	int retval;
-
-	retval = comedi_driver_register(&driver_amplc_pci224);
-	if (retval < 0)
-		return retval;
-
-	driver_amplc_pci224_pci_driver.name =
-	    (char *)driver_amplc_pci224.driver_name;
-	return pci_register_driver(&driver_amplc_pci224_pci_driver);
-}
-
-static void __exit driver_amplc_pci224_cleanup_module(void)
-{
-	pci_unregister_driver(&driver_amplc_pci224_pci_driver);
-	comedi_driver_unregister(&driver_amplc_pci224);
-}
-
-module_init(driver_amplc_pci224_init_module);
-module_exit(driver_amplc_pci224_cleanup_module);
-
-/*
  * Called from the 'insn_write' function to perform a single write.
  */
 static void
@@ -1312,6 +1241,20 @@ static irqreturn_t pci224_interrupt(int irq, void *d)
 }
 
 /*
+ * This function looks for a board matching the supplied PCI device.
+ */
+static const struct pci224_board
+*pci224_find_pci_board(struct pci_dev *pci_dev)
+{
+	int i;
+
+	for (i = 0; i < ARRAY_SIZE(pci224_boards); i++)
+		if (pci_dev->device == pci224_boards[i].devid)
+			return &pci224_boards[i];
+	return NULL;
+}
+
+/*
  * This function looks for a PCI device matching the requested board name,
  * bus and slot.
  */
@@ -1336,17 +1279,12 @@ pci224_find_pci(struct comedi_device *dev, int bus, int slot,
 		}
 		if (thisboard->model == any_model) {
 			/* Match any supported model. */
-			int i;
-
-			for (i = 0; i < ARRAY_SIZE(pci224_boards); i++) {
-				if (pci_dev->device == pci224_boards[i].devid) {
-					/* Change board_ptr to matched board. */
-					dev->board_ptr = &pci224_boards[i];
-					break;
-				}
-			}
-			if (i == ARRAY_SIZE(pci224_boards))
+			const struct pci224_board *board_ptr;
+			board_ptr = pci224_find_pci_board(pci_dev);
+			if (board_ptr == NULL)
 				continue;
+			/* Change board_ptr to matched board. */
+			dev->board_ptr = board_ptr;
 		} else {
 			/* Match specific model name. */
 			if (thisboard->devid != pci_dev->device)
@@ -1370,35 +1308,16 @@ pci224_find_pci(struct comedi_device *dev, int bus, int slot,
 }
 
 /*
- * Attach is called by the Comedi core to configure the driver
- * for a particular board.  If you specified a board_name array
- * in the driver structure, dev->board_ptr contains that
- * address.
+ * Common part of attach and attach_pci.
  */
-static int pci224_attach(struct comedi_device *dev, struct comedi_devconfig *it)
+static int pci224_attach_common(struct comedi_device *dev,
+				struct pci_dev *pci_dev, int *options)
 {
 	struct comedi_subdevice *s;
-	struct pci_dev *pci_dev;
 	unsigned int irq;
-	int bus = 0, slot = 0;
 	unsigned n;
 	int ret;
 
-	printk(KERN_DEBUG "comedi%d: %s: attach\n", dev->minor, DRIVER_NAME);
-
-	bus = it->options[0];
-	slot = it->options[1];
-	ret = alloc_private(dev, sizeof(struct pci224_private));
-	if (ret < 0) {
-		printk(KERN_ERR "comedi%d: error! out of memory!\n",
-		       dev->minor);
-		return ret;
-	}
-
-	ret = pci224_find_pci(dev, bus, slot, &pci_dev);
-	if (ret < 0)
-		return ret;
-
 	devpriv->pci_dev = pci_dev;
 	ret = comedi_pci_enable(pci_dev, DRIVER_NAME);
 	if (ret < 0) {
@@ -1483,24 +1402,26 @@ static int pci224_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 		if (!s->range_table_list)
 			return -ENOMEM;
 
-		for (n = 2; n < 3 + s->n_chan; n++) {
-			if (it->options[n] < 0 || it->options[n] > 1) {
-				printk(KERN_WARNING "comedi%d: %s: warning! "
-				       "bad options[%u]=%d\n",
-				       dev->minor, DRIVER_NAME, n,
-				       it->options[n]);
+		if (options) {
+			for (n = 2; n < 3 + s->n_chan; n++) {
+				if (options[n] < 0 || options[n] > 1) {
+					printk(KERN_WARNING
+					       "comedi%d: %s: warning! bad options[%u]=%d\n",
+					       dev->minor, DRIVER_NAME, n,
+					       options[n]);
+				}
 			}
 		}
 		for (n = 0; n < s->n_chan; n++) {
-			if (n < COMEDI_NDEVCONFOPTS - 3 &&
-			    it->options[3 + n] == 1) {
-				if (it->options[2] == 1)
+			if (n < COMEDI_NDEVCONFOPTS - 3 && options &&
+			    options[3 + n] == 1) {
+				if (options[2] == 1)
 					range_table_list[n] = &range_pci234_ext;
 				else
 					range_table_list[n] = &range_bipolar5;
 
 			} else {
-				if (it->options[2] == 1) {
+				if (options && options[2] == 1) {
 					range_table_list[n] =
 					    &range_pci234_ext2;
 				} else {
@@ -1511,14 +1432,14 @@ static int pci224_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 		devpriv->hwrange = hwrange_pci234;
 	} else {
 		/* PCI224 range options. */
-		if (it->options[2] == 1) {
+		if (options && options[2] == 1) {
 			s->range_table = &range_pci224_external;
 			devpriv->hwrange = hwrange_pci224_external;
 		} else {
-			if (it->options[2] != 0) {
+			if (options && options[2] != 0) {
 				printk(KERN_WARNING "comedi%d: %s: warning! "
 				       "bad options[2]=%d\n",
-				       dev->minor, DRIVER_NAME, it->options[2]);
+				       dev->minor, DRIVER_NAME, options[2]);
 			}
 			s->range_table = &range_pci224_internal;
 			devpriv->hwrange = hwrange_pci224_internal;
@@ -1552,21 +1473,59 @@ static int pci224_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 	return 1;
 }
 
-/*
- * _detach is called to deconfigure a device.  It should deallocate
- * resources.
- * This function is also called when _attach() fails, so it should be
- * careful not to release resources that were not necessarily
- * allocated by _attach().  dev->private and dev->subdevices are
- * deallocated automatically by the core.
- */
-static int pci224_detach(struct comedi_device *dev)
+static int pci224_attach(struct comedi_device *dev, struct comedi_devconfig *it)
+{
+	struct pci_dev *pci_dev;
+	int bus, slot;
+	int ret;
+
+	printk(KERN_DEBUG "comedi%d: %s: attach\n", dev->minor, DRIVER_NAME);
+
+	bus = it->options[0];
+	slot = it->options[1];
+	ret = alloc_private(dev, sizeof(struct pci224_private));
+	if (ret < 0) {
+		printk(KERN_ERR "comedi%d: error! out of memory!\n",
+		       dev->minor);
+		return ret;
+	}
+
+	ret = pci224_find_pci(dev, bus, slot, &pci_dev);
+	if (ret < 0)
+		return ret;
+
+	return pci224_attach_common(dev, pci_dev, it->options);
+}
+
+static int
+pci224_attach_pci(struct comedi_device *dev, struct pci_dev *pci_dev)
 {
-	printk(KERN_DEBUG "comedi%d: %s: detach\n", dev->minor, DRIVER_NAME);
+	int ret;
+
+	printk(KERN_DEBUG "comedi%d: %s: attach_pci %s\n", dev->minor,
+	       DRIVER_NAME, pci_name(pci_dev));
+
+	ret = alloc_private(dev, sizeof(struct pci224_private));
+	if (ret < 0) {
+		printk(KERN_ERR "comedi%d: error! out of memory!\n",
+		       dev->minor);
+		return ret;
+	}
 
+	dev->board_ptr = pci224_find_pci_board(pci_dev);
+	if (dev->board_ptr == NULL) {
+		printk(KERN_ERR
+		       "comedi%d: %s: BUG! cannot determine board type!\n",
+		       dev->minor, DRIVER_NAME);
+		return -EINVAL;
+	}
+	return pci224_attach_common(dev, pci_dev, NULL);
+}
+
+static void pci224_detach(struct comedi_device *dev)
+{
 	if (dev->irq)
 		free_irq(dev->irq, dev);
-
 	if (dev->subdevices) {
 		struct comedi_subdevice *s;
 
@@ -1581,18 +1540,49 @@ static int pci224_detach(struct comedi_device *dev)
 		if (devpriv->pci_dev) {
 			if (dev->iobase)
 				comedi_pci_disable(devpriv->pci_dev);
-
 			pci_dev_put(devpriv->pci_dev);
 		}
 	}
-	if (dev->board_name) {
-		printk(KERN_INFO "comedi%d: %s removed\n",
-		       dev->minor, dev->board_name);
-	}
+}
 
-	return 0;
+static struct comedi_driver amplc_pci224_driver = {
+	.driver_name	= "amplc_pci224",
+	.module		= THIS_MODULE,
+	.attach		= pci224_attach,
+	.detach		= pci224_detach,
+	.attach_pci	= pci224_attach_pci,
+	.board_name	= &pci224_boards[0].name,
+	.offset		= sizeof(struct pci224_board),
+	.num_names	= ARRAY_SIZE(pci224_boards),
+};
+
+static int __devinit amplc_pci224_pci_probe(struct pci_dev *dev,
+						   const struct pci_device_id
+						   *ent)
+{
+	return comedi_pci_auto_config(dev, &amplc_pci224_driver);
 }
 
+static void __devexit amplc_pci224_pci_remove(struct pci_dev *dev)
+{
+	comedi_pci_auto_unconfig(dev);
+}
+
+static DEFINE_PCI_DEVICE_TABLE(amplc_pci224_pci_table) = {
+	{ PCI_DEVICE(PCI_VENDOR_ID_AMPLICON, PCI_DEVICE_ID_AMPLICON_PCI224) },
+	{ PCI_DEVICE(PCI_VENDOR_ID_AMPLICON, PCI_DEVICE_ID_AMPLICON_PCI234) },
+	{ 0 }
+};
+MODULE_DEVICE_TABLE(pci, amplc_pci224_pci_table);
+
+static struct pci_driver amplc_pci224_pci_driver = {
+	.name		= "amplc_pci224",
+	.id_table	= amplc_pci224_pci_table,
+	.probe		= amplc_pci224_pci_probe,
+	.remove		= __devexit_p(amplc_pci224_pci_remove),
+};
+module_comedi_pci_driver(amplc_pci224_driver, amplc_pci224_pci_driver);
+
 MODULE_AUTHOR("Comedi http://www.comedi.org");
 MODULE_DESCRIPTION("Comedi low-level driver");
 MODULE_LICENSE("GPL");
diff --git a/drivers/staging/comedi/drivers/amplc_pci230.c b/drivers/staging/comedi/drivers/amplc_pci230.c
index 538979551c8..d4c80b1281f 100644
--- a/drivers/staging/comedi/drivers/amplc_pci230.c
+++ b/drivers/staging/comedi/drivers/amplc_pci230.c
@@ -500,13 +500,6 @@ static const struct pci230_board pci230_boards[] = {
 	 },
 };
 
-static DEFINE_PCI_DEVICE_TABLE(pci230_pci_table) = {
-	{ PCI_DEVICE(PCI_VENDOR_ID_AMPLICON, PCI_DEVICE_ID_PCI230) },
-	{ PCI_DEVICE(PCI_VENDOR_ID_AMPLICON, PCI_DEVICE_ID_PCI260) },
-	{0}
-};
-
-MODULE_DEVICE_TABLE(pci, pci230_pci_table);
 /*
  * Useful for shorthand access to the particular board structure
  */
@@ -595,65 +588,6 @@ static const struct comedi_lrange pci230_ao_range = { 2, {
 /* PCI230 daccon bipolar flag for each analogue output range. */
 static const unsigned char pci230_ao_bipolar[2] = { 0, 1 };
 
-/*
- * The struct comedi_driver structure tells the Comedi core module
- * which functions to call to configure/deconfigure (attach/detach)
- * the board, and also about the kernel module that contains
- * the device code.
- */
-static int pci230_attach(struct comedi_device *dev,
-			 struct comedi_devconfig *it);
-static int pci230_detach(struct comedi_device *dev);
-static struct comedi_driver driver_amplc_pci230 = {
-	.driver_name = "amplc_pci230",
-	.module = THIS_MODULE,
-	.attach = pci230_attach,
-	.detach = pci230_detach,
-	.board_name = &pci230_boards[0].name,
-	.offset = sizeof(pci230_boards[0]),
-	.num_names = ARRAY_SIZE(pci230_boards),
-};
-
-static int __devinit driver_amplc_pci230_pci_probe(struct pci_dev *dev,
-						   const struct pci_device_id
-						   *ent)
-{
-	return comedi_pci_auto_config(dev, driver_amplc_pci230.driver_name);
-}
-
-static void __devexit driver_amplc_pci230_pci_remove(struct pci_dev *dev)
-{
-	comedi_pci_auto_unconfig(dev);
-}
-
-static struct pci_driver driver_amplc_pci230_pci_driver = {
-	.id_table = pci230_pci_table,
-	.probe = &driver_amplc_pci230_pci_probe,
-	.remove = __devexit_p(&driver_amplc_pci230_pci_remove)
-};
-
-static int __init driver_amplc_pci230_init_module(void)
-{
-	int retval;
-
-	retval = comedi_driver_register(&driver_amplc_pci230);
-	if (retval < 0)
-		return retval;
-
-	driver_amplc_pci230_pci_driver.name =
-	    (char *)driver_amplc_pci230.driver_name;
-	return pci_register_driver(&driver_amplc_pci230_pci_driver);
-}
-
-static void __exit driver_amplc_pci230_cleanup_module(void)
-{
-	pci_unregister_driver(&driver_amplc_pci230_pci_driver);
-	comedi_driver_unregister(&driver_amplc_pci230);
-}
-
-module_init(driver_amplc_pci230_init_module);
-module_exit(driver_amplc_pci230_cleanup_module);
-
 static int pci230_ai_rinsn(struct comedi_device *dev,
 			   struct comedi_subdevice *s, struct comedi_insn *insn,
 			   unsigned int *data);
@@ -1003,35 +937,19 @@ static int pci230_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 	return 1;
 }
 
-/*
- * _detach is called to deconfigure a device.  It should deallocate
- * resources.
- * This function is also called when _attach() fails, so it should be
- * careful not to release resources that were not necessarily
- * allocated by _attach().  dev->private and dev->subdevices are
- * deallocated automatically by the core.
- */
-static int pci230_detach(struct comedi_device *dev)
+static void pci230_detach(struct comedi_device *dev)
 {
-	printk("comedi%d: amplc_pci230: remove\n", dev->minor);
-
 	if (dev->subdevices && thisboard->have_dio)
-		/* Clean up dio subdevice. */
 		subdev_8255_cleanup(dev, dev->subdevices + 2);
-
 	if (dev->irq)
 		free_irq(dev->irq, dev);
-
 	if (devpriv) {
 		if (devpriv->pci_dev) {
 			if (dev->iobase)
 				comedi_pci_disable(devpriv->pci_dev);
-
 			pci_dev_put(devpriv->pci_dev);
 		}
 	}
-
-	return 0;
 }
 
 static int get_resources(struct comedi_device *dev, unsigned int res_mask,
@@ -3048,6 +2966,42 @@ static int pci230_ai_cancel(struct comedi_device *dev,
 	return 0;
 }
 
+static struct comedi_driver amplc_pci230_driver = {
+	.driver_name	= "amplc_pci230",
+	.module		= THIS_MODULE,
+	.attach		= pci230_attach,
+	.detach		= pci230_detach,
+	.board_name	= &pci230_boards[0].name,
+	.offset		= sizeof(pci230_boards[0]),
+	.num_names	= ARRAY_SIZE(pci230_boards),
+};
+
+static int __devinit amplc_pci230_pci_probe(struct pci_dev *dev,
+					    const struct pci_device_id *ent)
+{
+	return comedi_pci_auto_config(dev, &amplc_pci230_driver);
+}
+
+static void __devexit amplc_pci230_pci_remove(struct pci_dev *dev)
+{
+	comedi_pci_auto_unconfig(dev);
+}
+
+static DEFINE_PCI_DEVICE_TABLE(amplc_pci230_pci_table) = {
+	{ PCI_DEVICE(PCI_VENDOR_ID_AMPLICON, PCI_DEVICE_ID_PCI230) },
+	{ PCI_DEVICE(PCI_VENDOR_ID_AMPLICON, PCI_DEVICE_ID_PCI260) },
+	{ 0 }
+};
+MODULE_DEVICE_TABLE(pci, amplc_pci230_pci_table);
+
+static struct pci_driver amplc_pci230_pci_driver = {
+	.name		= "amplc_pci230",
+	.id_table	= amplc_pci230_pci_table,
+	.probe		= amplc_pci230_pci_probe,
+	.remove		= __devexit_p(amplc_pci230_pci_remove)
+};
+module_comedi_pci_driver(amplc_pci230_driver, amplc_pci230_pci_driver);
+
 MODULE_AUTHOR("Comedi http://www.comedi.org");
 MODULE_DESCRIPTION("Comedi low-level driver");
 MODULE_LICENSE("GPL");
diff --git a/drivers/staging/comedi/drivers/c6xdigio.c b/drivers/staging/comedi/drivers/c6xdigio.c
index 11cdaf2a5aa..fb9951a746a 100644
--- a/drivers/staging/comedi/drivers/c6xdigio.c
+++ b/drivers/staging/comedi/drivers/c6xdigio.c
@@ -97,16 +97,6 @@ union encvaluetype {
 
 #define C6XDIGIO_TIME_OUT 20
 
-static int c6xdigio_attach(struct comedi_device *dev,
-			   struct comedi_devconfig *it);
-static int c6xdigio_detach(struct comedi_device *dev);
-struct comedi_driver driver_c6xdigio = {
-	.driver_name = "c6xdigio",
-	.module = THIS_MODULE,
-	.attach = c6xdigio_attach,
-	.detach = c6xdigio_detach,
-};
-
 static void C6X_pwmInit(unsigned long baseAddr)
 {
 	int timeout = 0;
@@ -407,10 +397,6 @@ static void board_init(struct comedi_device *dev)
 
 }
 
-/* static void board_halt(struct comedi_device *dev) { */
-/* C6X_pwmInit(dev->iobase); */
-/* } */
-
 /*
    options[0] - I/O port
    options[1] - irq
@@ -500,36 +486,22 @@ static int c6xdigio_attach(struct comedi_device *dev,
 	return 0;
 }
 
-static int c6xdigio_detach(struct comedi_device *dev)
+static void c6xdigio_detach(struct comedi_device *dev)
 {
-	/* board_halt(dev);  may not need this */
-
-	printk(KERN_DEBUG "comedi%d: c6xdigio: remove\n", dev->minor);
-
 	if (dev->iobase)
 		release_region(dev->iobase, C6XDIGIO_SIZE);
-
-	/*  Not using IRQ so I am not sure if I need this */
 	if (dev->irq)
 		free_irq(dev->irq, dev);
-
 	pnp_unregister_driver(&c6xdigio_pnp_driver);
-
-	return 0;
-}
-
-static int __init driver_c6xdigio_init_module(void)
-{
-	return comedi_driver_register(&driver_c6xdigio);
 }
 
-static void __exit driver_c6xdigio_cleanup_module(void)
-{
-	comedi_driver_unregister(&driver_c6xdigio);
-}
-
-module_init(driver_c6xdigio_init_module);
-module_exit(driver_c6xdigio_cleanup_module);
+static struct comedi_driver c6xdigio_driver = {
+	.driver_name	= "c6xdigio",
+	.module		= THIS_MODULE,
+	.attach		= c6xdigio_attach,
+	.detach		= c6xdigio_detach,
+};
+module_comedi_driver(c6xdigio_driver);
 
 MODULE_AUTHOR("Comedi http://www.comedi.org");
 MODULE_DESCRIPTION("Comedi low-level driver");
diff --git a/drivers/staging/comedi/drivers/cb_das16_cs.c b/drivers/staging/comedi/drivers/cb_das16_cs.c
index 49404f49f7b..35159235a1b 100644
--- a/drivers/staging/comedi/drivers/cb_das16_cs.c
+++ b/drivers/staging/comedi/drivers/cb_das16_cs.c
@@ -91,7 +91,7 @@ struct das16cs_private {
 
 static int das16cs_attach(struct comedi_device *dev,
 			  struct comedi_devconfig *it);
-static int das16cs_detach(struct comedi_device *dev);
+static void das16cs_detach(struct comedi_device *dev);
 static struct comedi_driver driver_das16cs = {
 	.driver_name = "cb_das16_cs",
 	.module = THIS_MODULE,
@@ -255,15 +255,10 @@ static int das16cs_attach(struct comedi_device *dev,
 	return 1;
 }
 
-static int das16cs_detach(struct comedi_device *dev)
+static void das16cs_detach(struct comedi_device *dev)
 {
-	dev_dbg(dev->hw_dev, "comedi%d: das16cs: remove\n", dev->minor);
-
 	if (dev->irq)
 		free_irq(dev->irq, dev);
-
-
-	return 0;
 }
 
 static irqreturn_t das16cs_interrupt(int irq, void *d)
diff --git a/drivers/staging/comedi/drivers/cb_pcidas.c b/drivers/staging/comedi/drivers/cb_pcidas.c
index 7e4ffcfdac6..ee9e084bb96 100644
--- a/drivers/staging/comedi/drivers/cb_pcidas.c
+++ b/drivers/staging/comedi/drivers/cb_pcidas.c
@@ -405,20 +405,6 @@ static const struct cb_pcidas_board cb_pcidas_boards[] = {
 	 },
 };
 
-static DEFINE_PCI_DEVICE_TABLE(cb_pcidas_pci_table) = {
-	{ PCI_DEVICE(PCI_VENDOR_ID_CB, 0x0001) },
-	{ PCI_DEVICE(PCI_VENDOR_ID_CB, 0x000f) },
-	{ PCI_DEVICE(PCI_VENDOR_ID_CB, 0x0010) },
-	{ PCI_DEVICE(PCI_VENDOR_ID_CB, 0x0019) },
-	{ PCI_DEVICE(PCI_VENDOR_ID_CB, 0x001c) },
-	{ PCI_DEVICE(PCI_VENDOR_ID_CB, 0x004c) },
-	{ PCI_DEVICE(PCI_VENDOR_ID_CB, 0x001a) },
-	{ PCI_DEVICE(PCI_VENDOR_ID_CB, 0x001b) },
-	{ 0 }
-};
-
-MODULE_DEVICE_TABLE(pci, cb_pcidas_pci_table);
-
 /*
  * Useful for shorthand access to the particular board structure
  */
@@ -462,22 +448,6 @@ struct cb_pcidas_private {
  */
 #define devpriv ((struct cb_pcidas_private *)dev->private)
 
-/*
- * The struct comedi_driver structure tells the Comedi core module
- * which functions to call to configure/deconfigure (attach/detach)
- * the board, and also about the kernel module that contains
- * the device code.
- */
-static int cb_pcidas_attach(struct comedi_device *dev,
-			    struct comedi_devconfig *it);
-static int cb_pcidas_detach(struct comedi_device *dev);
-static struct comedi_driver driver_cb_pcidas = {
-	.driver_name = "cb_pcidas",
-	.module = THIS_MODULE,
-	.attach = cb_pcidas_attach,
-	.detach = cb_pcidas_detach,
-};
-
 static int cb_pcidas_ai_rinsn(struct comedi_device *dev,
 			      struct comedi_subdevice *s,
 			      struct comedi_insn *insn, unsigned int *data);
@@ -756,26 +726,12 @@ found:
 	return 1;
 }
 
-/*
- * cb_pcidas_detach is called to deconfigure a device.  It should deallocate
- * resources.
- * This function is also called when _attach() fails, so it should be
- * careful not to release resources that were not necessarily
- * allocated by _attach().  dev->private and dev->subdevices are
- * deallocated automatically by the core.
- */
-static int cb_pcidas_detach(struct comedi_device *dev)
+static void cb_pcidas_detach(struct comedi_device *dev)
 {
-
 	if (devpriv) {
 		if (devpriv->s5933_config) {
-			/*  disable and clear interrupts on amcc s5933 */
 			outl(INTCSR_INBOX_INTR_STATUS,
 			     devpriv->s5933_config + AMCC_OP_REG_INTCSR);
-#ifdef CB_PCIDAS_DEBUG
-			dev_dbg(dev->hw_dev, "detaching, incsr is 0x%x\n",
-				inl(devpriv->s5933_config + AMCC_OP_REG_INTCSR));
-#endif
 		}
 	}
 	if (dev->irq)
@@ -787,8 +743,6 @@ static int cb_pcidas_detach(struct comedi_device *dev)
 			comedi_pci_disable(devpriv->pci_dev);
 		pci_dev_put(devpriv->pci_dev);
 	}
-
-	return 0;
 }
 
 /*
@@ -1918,47 +1872,44 @@ static int nvram_read(struct comedi_device *dev, unsigned int address,
 	return 0;
 }
 
-/*
- * A convenient macro that defines init_module() and cleanup_module(),
- * as necessary.
- */
-static int __devinit driver_cb_pcidas_pci_probe(struct pci_dev *dev,
-						const struct pci_device_id *ent)
+static struct comedi_driver cb_pcidas_driver = {
+	.driver_name	= "cb_pcidas",
+	.module		= THIS_MODULE,
+	.attach		= cb_pcidas_attach,
+	.detach		= cb_pcidas_detach,
+};
+
+static int __devinit cb_pcidas_pci_probe(struct pci_dev *dev,
+					 const struct pci_device_id *ent)
 {
-	return comedi_pci_auto_config(dev, driver_cb_pcidas.driver_name);
+	return comedi_pci_auto_config(dev, &cb_pcidas_driver);
 }
 
-static void __devexit driver_cb_pcidas_pci_remove(struct pci_dev *dev)
+static void __devexit cb_pcidas_pci_remove(struct pci_dev *dev)
 {
 	comedi_pci_auto_unconfig(dev);
 }
 
-static struct pci_driver driver_cb_pcidas_pci_driver = {
-	.id_table = cb_pcidas_pci_table,
-	.probe = &driver_cb_pcidas_pci_probe,
-	.remove = __devexit_p(&driver_cb_pcidas_pci_remove)
+static DEFINE_PCI_DEVICE_TABLE(cb_pcidas_pci_table) = {
+	{ PCI_DEVICE(PCI_VENDOR_ID_CB, 0x0001) },
+	{ PCI_DEVICE(PCI_VENDOR_ID_CB, 0x000f) },
+	{ PCI_DEVICE(PCI_VENDOR_ID_CB, 0x0010) },
+	{ PCI_DEVICE(PCI_VENDOR_ID_CB, 0x0019) },
+	{ PCI_DEVICE(PCI_VENDOR_ID_CB, 0x001c) },
+	{ PCI_DEVICE(PCI_VENDOR_ID_CB, 0x004c) },
+	{ PCI_DEVICE(PCI_VENDOR_ID_CB, 0x001a) },
+	{ PCI_DEVICE(PCI_VENDOR_ID_CB, 0x001b) },
+	{ 0 }
 };
+MODULE_DEVICE_TABLE(pci, cb_pcidas_pci_table);
 
-static int __init driver_cb_pcidas_init_module(void)
-{
-	int retval;
-
-	retval = comedi_driver_register(&driver_cb_pcidas);
-	if (retval < 0)
-		return retval;
-
-	driver_cb_pcidas_pci_driver.name = (char *)driver_cb_pcidas.driver_name;
-	return pci_register_driver(&driver_cb_pcidas_pci_driver);
-}
-
-static void __exit driver_cb_pcidas_cleanup_module(void)
-{
-	pci_unregister_driver(&driver_cb_pcidas_pci_driver);
-	comedi_driver_unregister(&driver_cb_pcidas);
-}
-
-module_init(driver_cb_pcidas_init_module);
-module_exit(driver_cb_pcidas_cleanup_module);
+static struct pci_driver cb_pcidas_pci_driver = {
+	.name		= "cb_pcidas",
+	.id_table	= cb_pcidas_pci_table,
+	.probe		= cb_pcidas_pci_probe,
+	.remove		= __devexit_p(cb_pcidas_pci_remove)
+};
+module_comedi_pci_driver(cb_pcidas_driver, cb_pcidas_pci_driver);
 
 MODULE_AUTHOR("Comedi http://www.comedi.org");
 MODULE_DESCRIPTION("Comedi low-level driver");
diff --git a/drivers/staging/comedi/drivers/cb_pcidas64.c b/drivers/staging/comedi/drivers/cb_pcidas64.c
index 915157d4780..9d0b8754ff5 100644
--- a/drivers/staging/comedi/drivers/cb_pcidas64.c
+++ b/drivers/staging/comedi/drivers/cb_pcidas64.c
@@ -1026,31 +1026,6 @@ static const struct pcidas64_board pcidas64_boards[] = {
 #endif
 };
 
-static DEFINE_PCI_DEVICE_TABLE(pcidas64_pci_table) = {
-	{ PCI_DEVICE(PCI_VENDOR_ID_COMPUTERBOARDS, 0x001d) },
-	{ PCI_DEVICE(PCI_VENDOR_ID_COMPUTERBOARDS, 0x001e) },
-	{ PCI_DEVICE(PCI_VENDOR_ID_COMPUTERBOARDS, 0x0035) },
-	{ PCI_DEVICE(PCI_VENDOR_ID_COMPUTERBOARDS, 0x0036) },
-	{ PCI_DEVICE(PCI_VENDOR_ID_COMPUTERBOARDS, 0x0037) },
-	{ PCI_DEVICE(PCI_VENDOR_ID_COMPUTERBOARDS, 0x0052) },
-	{ PCI_DEVICE(PCI_VENDOR_ID_COMPUTERBOARDS, 0x005d) },
-	{ PCI_DEVICE(PCI_VENDOR_ID_COMPUTERBOARDS, 0x005e) },
-	{ PCI_DEVICE(PCI_VENDOR_ID_COMPUTERBOARDS, 0x005f) },
-	{ PCI_DEVICE(PCI_VENDOR_ID_COMPUTERBOARDS, 0x0061) },
-	{ PCI_DEVICE(PCI_VENDOR_ID_COMPUTERBOARDS, 0x0062) },
-	{ PCI_DEVICE(PCI_VENDOR_ID_COMPUTERBOARDS, 0x0063) },
-	{ PCI_DEVICE(PCI_VENDOR_ID_COMPUTERBOARDS, 0x0064) },
-	{ PCI_DEVICE(PCI_VENDOR_ID_COMPUTERBOARDS, 0x0066) },
-	{ PCI_DEVICE(PCI_VENDOR_ID_COMPUTERBOARDS, 0x0067) },
-	{ PCI_DEVICE(PCI_VENDOR_ID_COMPUTERBOARDS, 0x0068) },
-	{ PCI_DEVICE(PCI_VENDOR_ID_COMPUTERBOARDS, 0x006f) },
-	{ PCI_DEVICE(PCI_VENDOR_ID_COMPUTERBOARDS, 0x0078) },
-	{ PCI_DEVICE(PCI_VENDOR_ID_COMPUTERBOARDS, 0x0079) },
-	{ 0 }
-};
-
-MODULE_DEVICE_TABLE(pci, pcidas64_pci_table);
-
 static inline struct pcidas64_board *board(const struct comedi_device *dev)
 {
 	return (struct pcidas64_board *)dev->board_ptr;
@@ -1127,21 +1102,6 @@ static inline struct pcidas64_private *priv(struct comedi_device *dev)
 	return dev->private;
 }
 
-/*
- * The comedi_driver structure tells the Comedi core module
- * which functions to call to configure/deconfigure (attach/detach)
- * the board, and also about the kernel module that contains
- * the device code.
- */
-static int attach(struct comedi_device *dev, struct comedi_devconfig *it);
-static int detach(struct comedi_device *dev);
-static struct comedi_driver driver_cb_pcidas = {
-	.driver_name = "cb_pcidas64",
-	.module = THIS_MODULE,
-	.attach = attach,
-	.detach = detach,
-};
-
 static int ai_rinsn(struct comedi_device *dev, struct comedi_subdevice *s,
 		    struct comedi_insn *insn, unsigned int *data);
 static int ai_config_insn(struct comedi_device *dev, struct comedi_subdevice *s,
@@ -1216,44 +1176,6 @@ static unsigned int get_ao_divisor(unsigned int ns, unsigned int flags);
 static void load_ao_dma(struct comedi_device *dev,
 			const struct comedi_cmd *cmd);
 
-static int __devinit driver_cb_pcidas_pci_probe(struct pci_dev *dev,
-						const struct pci_device_id *ent)
-{
-	return comedi_pci_auto_config(dev, driver_cb_pcidas.driver_name);
-}
-
-static void __devexit driver_cb_pcidas_pci_remove(struct pci_dev *dev)
-{
-	comedi_pci_auto_unconfig(dev);
-}
-
-static struct pci_driver driver_cb_pcidas_pci_driver = {
-	.id_table = pcidas64_pci_table,
-	.probe = &driver_cb_pcidas_pci_probe,
-	.remove = __devexit_p(&driver_cb_pcidas_pci_remove)
-};
-
-static int __init driver_cb_pcidas_init_module(void)
-{
-	int retval;
-
-	retval = comedi_driver_register(&driver_cb_pcidas);
-	if (retval < 0)
-		return retval;
-
-	driver_cb_pcidas_pci_driver.name = (char *)driver_cb_pcidas.driver_name;
-	return pci_register_driver(&driver_cb_pcidas_pci_driver);
-}
-
-static void __exit driver_cb_pcidas_cleanup_module(void)
-{
-	pci_unregister_driver(&driver_cb_pcidas_pci_driver);
-	comedi_driver_unregister(&driver_cb_pcidas);
-}
-
-module_init(driver_cb_pcidas_init_module);
-module_exit(driver_cb_pcidas_cleanup_module);
-
 static unsigned int ai_range_bits_6xxx(const struct comedi_device *dev,
 				       unsigned int range_index)
 {
@@ -1781,7 +1703,7 @@ static int attach(struct comedi_device *dev, struct comedi_devconfig *it)
 	dev_dbg(dev->hw_dev, "Found %s on bus %i, slot %i\n", board(dev)->name,
 		pcidev->bus->number, PCI_SLOT(pcidev->devfn));
 
-	if (comedi_pci_enable(pcidev, driver_cb_pcidas.driver_name)) {
+	if (comedi_pci_enable(pcidev, dev->driver->driver_name)) {
 		dev_warn(dev->hw_dev, "failed to enable PCI device and request regions\n");
 		return -EIO;
 	}
@@ -1868,15 +1790,7 @@ static int attach(struct comedi_device *dev, struct comedi_devconfig *it)
 	return 0;
 }
 
-/*
- * _detach is called to deconfigure a device.  It should deallocate
- * resources.
- * This function is also called when _attach() fails, so it should be
- * careful not to release resources that were not necessarily
- * allocated by _attach().  dev->private and dev->subdevices are
- * deallocated automatically by the core.
- */
-static int detach(struct comedi_device *dev)
+static void detach(struct comedi_device *dev)
 {
 	unsigned int i;
 
@@ -1938,8 +1852,6 @@ static int detach(struct comedi_device *dev)
 	}
 	if (dev->subdevices)
 		subdev_8255_cleanup(dev, dev->subdevices + 4);
-
-	return 0;
 }
 
 static int ai_rinsn(struct comedi_device *dev, struct comedi_subdevice *s,
@@ -4315,6 +4227,56 @@ static void i2c_write(struct comedi_device *dev, unsigned int address,
 	i2c_stop(dev);
 }
 
+static struct comedi_driver cb_pcidas64_driver = {
+	.driver_name	= "cb_pcidas64",
+	.module		= THIS_MODULE,
+	.attach		= attach,
+	.detach		= detach,
+};
+
+static int __devinit cb_pcidas64_pci_probe(struct pci_dev *dev,
+					   const struct pci_device_id *ent)
+{
+	return comedi_pci_auto_config(dev, &cb_pcidas64_driver);
+}
+
+static void __devexit cb_pcidas64_pci_remove(struct pci_dev *dev)
+{
+	comedi_pci_auto_unconfig(dev);
+}
+
+static DEFINE_PCI_DEVICE_TABLE(cb_pcidas64_pci_table) = {
+	{ PCI_DEVICE(PCI_VENDOR_ID_COMPUTERBOARDS, 0x001d) },
+	{ PCI_DEVICE(PCI_VENDOR_ID_COMPUTERBOARDS, 0x001e) },
+	{ PCI_DEVICE(PCI_VENDOR_ID_COMPUTERBOARDS, 0x0035) },
+	{ PCI_DEVICE(PCI_VENDOR_ID_COMPUTERBOARDS, 0x0036) },
+	{ PCI_DEVICE(PCI_VENDOR_ID_COMPUTERBOARDS, 0x0037) },
+	{ PCI_DEVICE(PCI_VENDOR_ID_COMPUTERBOARDS, 0x0052) },
+	{ PCI_DEVICE(PCI_VENDOR_ID_COMPUTERBOARDS, 0x005d) },
+	{ PCI_DEVICE(PCI_VENDOR_ID_COMPUTERBOARDS, 0x005e) },
+	{ PCI_DEVICE(PCI_VENDOR_ID_COMPUTERBOARDS, 0x005f) },
+	{ PCI_DEVICE(PCI_VENDOR_ID_COMPUTERBOARDS, 0x0061) },
+	{ PCI_DEVICE(PCI_VENDOR_ID_COMPUTERBOARDS, 0x0062) },
+	{ PCI_DEVICE(PCI_VENDOR_ID_COMPUTERBOARDS, 0x0063) },
+	{ PCI_DEVICE(PCI_VENDOR_ID_COMPUTERBOARDS, 0x0064) },
+	{ PCI_DEVICE(PCI_VENDOR_ID_COMPUTERBOARDS, 0x0066) },
+	{ PCI_DEVICE(PCI_VENDOR_ID_COMPUTERBOARDS, 0x0067) },
+	{ PCI_DEVICE(PCI_VENDOR_ID_COMPUTERBOARDS, 0x0068) },
+	{ PCI_DEVICE(PCI_VENDOR_ID_COMPUTERBOARDS, 0x006f) },
+	{ PCI_DEVICE(PCI_VENDOR_ID_COMPUTERBOARDS, 0x0078) },
+	{ PCI_DEVICE(PCI_VENDOR_ID_COMPUTERBOARDS, 0x0079) },
+	{ 0 }
+};
+MODULE_DEVICE_TABLE(pci, cb_pcidas64_pci_table);
+
+static struct pci_driver cb_pcidas64_pci_driver = {
+	.name		= "cb_pcidas64",
+	.id_table	= cb_pcidas64_pci_table,
+	.probe		= cb_pcidas64_pci_probe,
+	.remove		= __devexit_p(cb_pcidas64_pci_remove),
+};
+module_comedi_pci_driver(cb_pcidas64_driver, cb_pcidas64_pci_driver);
+
 MODULE_AUTHOR("Comedi http://www.comedi.org");
 MODULE_DESCRIPTION("Comedi low-level driver");
 MODULE_LICENSE("GPL");
diff --git a/drivers/staging/comedi/drivers/cb_pcidda.c b/drivers/staging/comedi/drivers/cb_pcidda.c
index abba220a767..25ebca11ead 100644
--- a/drivers/staging/comedi/drivers/cb_pcidda.c
+++ b/drivers/staging/comedi/drivers/cb_pcidda.c
@@ -51,9 +51,12 @@ Please report success/failure with other different cards to
 #include "comedi_pci.h"
 #include "8255.h"
 
-#define PCI_VENDOR_ID_CB	0x1307	/*  PCI vendor number of ComputerBoards */
+
+/* PCI vendor number of ComputerBoards */
+#define PCI_VENDOR_ID_CB        0x1307
 #define EEPROM_SIZE	128	/*  number of entries in eeprom */
-#define MAX_AO_CHANNELS 8	/*  maximum number of ao channels for supported boards */
+/* maximum number of ao channels for supported boards */
+#define MAX_AO_CHANNELS 8
 
 /* PCI-DDA base addresses */
 #define DIGITALIO_BADRINDEX	2
@@ -94,20 +97,26 @@ Please report success/failure with other different cards to
 
 #define DACALIBRATION1	4	/*  D/A CALIBRATION REGISTER 1 */
 /* write bits */
-#define	SERIAL_IN_BIT	0x1	/*  serial data input for eeprom, caldacs, reference dac */
+/* serial data input for eeprom, caldacs, reference dac */
+#define SERIAL_IN_BIT   0x1
 #define	CAL_CHANNEL_MASK	(0x7 << 1)
 #define	CAL_CHANNEL_BITS(channel)	(((channel) << 1) & CAL_CHANNEL_MASK)
 /* read bits */
 #define	CAL_COUNTER_MASK	0x1f
-#define	CAL_COUNTER_OVERFLOW_BIT	0x20	/*  calibration counter overflow status bit */
-#define	AO_BELOW_REF_BIT	0x40	/*  analog output is less than reference dac voltage */
+/* calibration counter overflow status bit */
+#define CAL_COUNTER_OVERFLOW_BIT        0x20
+/* analog output is less than reference dac voltage */
+#define AO_BELOW_REF_BIT        0x40
 #define	SERIAL_OUT_BIT	0x80	/*  serial data out, for reading from eeprom */
 
 #define DACALIBRATION2	6	/*  D/A CALIBRATION REGISTER 2 */
 #define	SELECT_EEPROM_BIT	0x1	/*  send serial data in to eeprom */
-#define	DESELECT_REF_DAC_BIT	0x2	/*  don't send serial data to MAX542 reference dac */
-#define	DESELECT_CALDAC_BIT(n)	(0x4 << (n))	/*  don't send serial data to caldac n */
-#define	DUMMY_BIT	0x40	/*  manual says to set this bit with no explanation */
+/* don't send serial data to MAX542 reference dac */
+#define DESELECT_REF_DAC_BIT    0x2
+/* don't send serial data to caldac n */
+#define DESELECT_CALDAC_BIT(n)  (0x4 << (n))
+/* manual says to set this bit with no explanation */
+#define DUMMY_BIT       0x40
 
 #define DADATA	8		/*  FIRST D/A DATA REGISTER (0) */
 
@@ -195,26 +204,17 @@ static const struct cb_pcidda_board cb_pcidda_boards[] = {
 	 },
 };
 
-static DEFINE_PCI_DEVICE_TABLE(cb_pcidda_pci_table) = {
-	{ PCI_DEVICE(PCI_VENDOR_ID_CB, 0x0020) },
-	{ PCI_DEVICE(PCI_VENDOR_ID_CB, 0x0021) },
-	{ PCI_DEVICE(PCI_VENDOR_ID_CB, 0x0022) },
-	{ PCI_DEVICE(PCI_VENDOR_ID_CB, 0x0023) },
-	{ PCI_DEVICE(PCI_VENDOR_ID_CB, 0x0024) },
-	{ PCI_DEVICE(PCI_VENDOR_ID_CB, 0x0025) },
-	{ 0 }
-};
-
-MODULE_DEVICE_TABLE(pci, cb_pcidda_pci_table);
-
 /*
  * Useful for shorthand access to the particular board structure
  */
 #define thisboard ((const struct cb_pcidda_board *)dev->board_ptr)
 
-/* this structure is for data unique to this hardware driver.  If
-   several hardware drivers keep similar information in this structure,
-   feel free to suggest moving the variable to the struct comedi_device struct.  */
+/*
+ * this structure is for data unique to this hardware driver.  If
+ * several hardware drivers keep similar information in this structure,
+ * feel free to suggest moving the variable to the struct comedi_device
+ * struct.
+ */
 struct cb_pcidda_private {
 	int data;
 
@@ -227,8 +227,10 @@ struct cb_pcidda_private {
 	/* unsigned long control_status; */
 	/* unsigned long adc_fifo; */
 
-	unsigned int dac_cal1_bits;	/*  bits last written to da calibration register 1 */
-	unsigned int ao_range[MAX_AO_CHANNELS];	/*  current range settings for output channels */
+	/* bits last written to da calibration register 1 */
+	unsigned int dac_cal1_bits;
+	/* current range settings for output channels */
+	unsigned int ao_range[MAX_AO_CHANNELS];
 	u16 eeprom_data[EEPROM_SIZE];	/*  software copy of board's eeprom */
 };
 
@@ -238,9 +240,6 @@ struct cb_pcidda_private {
  */
 #define devpriv ((struct cb_pcidda_private *)dev->private)
 
-static int cb_pcidda_attach(struct comedi_device *dev,
-			    struct comedi_devconfig *it);
-static int cb_pcidda_detach(struct comedi_device *dev);
 /* static int cb_pcidda_ai_rinsn(struct comedi_device *dev,struct comedi_subdevice *s,struct comedi_insn *insn,unsigned int *data); */
 static int cb_pcidda_ao_winsn(struct comedi_device *dev,
 			      struct comedi_subdevice *s,
@@ -259,19 +258,6 @@ static void cb_pcidda_calibrate(struct comedi_device *dev, unsigned int channel,
 				unsigned int range);
 
 /*
- * The struct comedi_driver structure tells the Comedi core module
- * which functions to call to configure/deconfigure (attach/detach)
- * the board, and also about the kernel module that contains
- * the device code.
- */
-static struct comedi_driver driver_cb_pcidda = {
-	.driver_name = "cb_pcidda",
-	.module = THIS_MODULE,
-	.attach = cb_pcidda_attach,
-	.detach = cb_pcidda_detach,
-};
-
-/*
  * Attach is called by the Comedi core to configure the driver
  * for a particular board.
  */
@@ -377,7 +363,8 @@ found:
 	dev_dbg(dev->hw_dev, "eeprom:\n");
 	for (index = 0; index < EEPROM_SIZE; index++) {
 		devpriv->eeprom_data[index] = cb_pcidda_read_eeprom(dev, index);
-		dev_dbg(dev->hw_dev, "%i:0x%x\n", index, devpriv->eeprom_data[index]);
+		dev_dbg(dev->hw_dev, "%i:0x%x\n", index,
+			devpriv->eeprom_data[index]);
 	}
 
 	/*  set calibrations dacs */
@@ -387,19 +374,8 @@ found:
 	return 1;
 }
 
-/*
- * _detach is called to deconfigure a device.  It should deallocate
- * resources.
- * This function is also called when _attach() fails, so it should be
- * careful not to release resources that were not necessarily
- * allocated by _attach().  dev->private and dev->subdevices are
- * deallocated automatically by the core.
- */
-static int cb_pcidda_detach(struct comedi_device *dev)
+static void cb_pcidda_detach(struct comedi_device *dev)
 {
-/*
- * Deallocate the I/O ports.
- */
 	if (devpriv) {
 		if (devpriv->pci_dev) {
 			if (devpriv->dac)
@@ -407,13 +383,10 @@ static int cb_pcidda_detach(struct comedi_device *dev)
 			pci_dev_put(devpriv->pci_dev);
 		}
 	}
-	/*  cleanup 8255 */
 	if (dev->subdevices) {
 		subdev_8255_cleanup(dev, dev->subdevices + 1);
 		subdev_8255_cleanup(dev, dev->subdevices + 2);
 	}
-
-	return 0;
 }
 
 /*
@@ -484,7 +457,10 @@ static int cb_pcidda_ai_cmdtest(struct comedi_device *dev,
 	if (err)
 		return 1;
 
-	/* step 2: make sure trigger sources are unique and mutually compatible */
+	/*
+	 * step 2: make sure trigger sources are unique and mutually
+	 * compatible
+	 */
 
 	/* note that mutual compatibility is not an issue here */
 	if (cmd->scan_begin_src != TRIG_TIMER
@@ -696,8 +672,10 @@ static unsigned int cb_pcidda_read_eeprom(struct comedi_device *dev,
 	unsigned int i;
 	unsigned int cal2_bits;
 	unsigned int value;
-	const int max_num_caldacs = 4;	/*  one caldac for every two dac channels */
-	const int read_instruction = 0x6;	/*  bits to send to tell eeprom we want to read */
+	/* one caldac for every two dac channels */
+	const int max_num_caldacs = 4;
+	/* bits to send to tell eeprom we want to read */
+	const int read_instruction = 0x6;
 	const int instruction_length = 3;
 	const int address_length = 8;
 
@@ -729,9 +707,11 @@ static void cb_pcidda_write_caldac(struct comedi_device *dev,
 {
 	unsigned int cal2_bits;
 	unsigned int i;
-	const int num_channel_bits = 3;	/*  caldacs use 3 bit channel specification */
+	/* caldacs use 3 bit channel specification */
+	const int num_channel_bits = 3;
 	const int num_caldac_bits = 8;	/*  8 bit calibration dacs */
-	const int max_num_caldacs = 4;	/*  one caldac for every two dac channels */
+	/* one caldac for every two dac channels */
+	const int max_num_caldacs = 4;
 
 	/* write 3 bit channel */
 	cb_pcidda_serial_out(dev, channel, num_channel_bits);
@@ -790,14 +770,20 @@ static unsigned int offset_eeprom_address(unsigned int ao_channel,
 	return 0x7 + 2 * range + 12 * ao_channel;
 }
 
-/* returns eeprom address that provides gain calibration for given ao channel and range */
+/*
+ * returns eeprom address that provides gain calibration for given ao
+ * channel and range
+ */
 static unsigned int gain_eeprom_address(unsigned int ao_channel,
 					unsigned int range)
 {
 	return 0x8 + 2 * range + 12 * ao_channel;
 }
 
-/* returns upper byte of eeprom entry, which gives the coarse adjustment values */
+/*
+ * returns upper byte of eeprom entry, which gives the coarse adjustment
+ * values
+ */
 static unsigned int eeprom_coarse_byte(unsigned int word)
 {
 	return (word >> 8) & 0xff;
@@ -815,7 +801,7 @@ static void cb_pcidda_calibrate(struct comedi_device *dev, unsigned int channel,
 {
 	unsigned int coarse_offset, fine_offset, coarse_gain, fine_gain;
 
-	/*  remember range so we can tell when we need to readjust calibration */
+	/* remember range so we can tell when we need to readjust calibration */
 	devpriv->ao_range[channel] = range;
 
 	/*  get values from eeprom data */
@@ -843,47 +829,42 @@ static void cb_pcidda_calibrate(struct comedi_device *dev, unsigned int channel,
 			       fine_gain_channel(channel), fine_gain);
 }
 
-/*
- * A convenient macro that defines init_module() and cleanup_module(),
- * as necessary.
- */
-static int __devinit driver_cb_pcidda_pci_probe(struct pci_dev *dev,
-						const struct pci_device_id *ent)
+static struct comedi_driver cb_pcidda_driver = {
+	.driver_name	= "cb_pcidda",
+	.module		= THIS_MODULE,
+	.attach		= cb_pcidda_attach,
+	.detach		= cb_pcidda_detach,
+};
+
+static int __devinit cb_pcidda_pci_probe(struct pci_dev *dev,
+					 const struct pci_device_id *ent)
 {
-	return comedi_pci_auto_config(dev, driver_cb_pcidda.driver_name);
+	return comedi_pci_auto_config(dev, &cb_pcidda_driver);
 }
 
-static void __devexit driver_cb_pcidda_pci_remove(struct pci_dev *dev)
+static void __devexit cb_pcidda_pci_remove(struct pci_dev *dev)
 {
 	comedi_pci_auto_unconfig(dev);
 }
 
-static struct pci_driver driver_cb_pcidda_pci_driver = {
-	.id_table = cb_pcidda_pci_table,
-	.probe = &driver_cb_pcidda_pci_probe,
-	.remove = __devexit_p(&driver_cb_pcidda_pci_remove)
+static DEFINE_PCI_DEVICE_TABLE(cb_pcidda_pci_table) = {
+	{ PCI_DEVICE(PCI_VENDOR_ID_CB, 0x0020) },
+	{ PCI_DEVICE(PCI_VENDOR_ID_CB, 0x0021) },
+	{ PCI_DEVICE(PCI_VENDOR_ID_CB, 0x0022) },
+	{ PCI_DEVICE(PCI_VENDOR_ID_CB, 0x0023) },
+	{ PCI_DEVICE(PCI_VENDOR_ID_CB, 0x0024) },
+	{ PCI_DEVICE(PCI_VENDOR_ID_CB, 0x0025) },
+	{ 0 }
 };
+MODULE_DEVICE_TABLE(pci, cb_pcidda_pci_table);
 
-static int __init driver_cb_pcidda_init_module(void)
-{
-	int retval;
-
-	retval = comedi_driver_register(&driver_cb_pcidda);
-	if (retval < 0)
-		return retval;
-
-	driver_cb_pcidda_pci_driver.name = (char *)driver_cb_pcidda.driver_name;
-	return pci_register_driver(&driver_cb_pcidda_pci_driver);
-}
-
-static void __exit driver_cb_pcidda_cleanup_module(void)
-{
-	pci_unregister_driver(&driver_cb_pcidda_pci_driver);
-	comedi_driver_unregister(&driver_cb_pcidda);
-}
-
-module_init(driver_cb_pcidda_init_module);
-module_exit(driver_cb_pcidda_cleanup_module);
+static struct pci_driver cb_pcidda_pci_driver = {
+	.name		= "cb_pcidda",
+	.id_table	= cb_pcidda_pci_table,
+	.probe		= cb_pcidda_pci_probe,
+	.remove		= __devexit_p(cb_pcidda_pci_remove),
+};
+module_comedi_pci_driver(cb_pcidda_driver, cb_pcidda_pci_driver);
 
 MODULE_AUTHOR("Comedi http://www.comedi.org");
 MODULE_DESCRIPTION("Comedi low-level driver");
diff --git a/drivers/staging/comedi/drivers/cb_pcidio.c b/drivers/staging/comedi/drivers/cb_pcidio.c
index 8f3215239a1..713132c8dbb 100644
--- a/drivers/staging/comedi/drivers/cb_pcidio.c
+++ b/drivers/staging/comedi/drivers/cb_pcidio.c
@@ -86,19 +86,6 @@ static const struct pcidio_board pcidio_boards[] = {
 	 },
 };
 
-/* This is used by modprobe to translate PCI IDs to drivers.  Should
- * only be used for PCI and ISA-PnP devices */
-/* Please add your PCI vendor ID to comedidev.h, and it will be forwarded
- * upstream. */
-static DEFINE_PCI_DEVICE_TABLE(pcidio_pci_table) = {
-	{ PCI_DEVICE(PCI_VENDOR_ID_CB, 0x0028) },
-	{ PCI_DEVICE(PCI_VENDOR_ID_CB, 0x0014) },
-	{ PCI_DEVICE(PCI_VENDOR_ID_CB, 0x000b) },
-	{ 0 }
-};
-
-MODULE_DEVICE_TABLE(pci, pcidio_pci_table);
-
 /*
  * Useful for shorthand access to the particular board structure
  */
@@ -125,59 +112,6 @@ struct pcidio_private {
  */
 #define devpriv ((struct pcidio_private *)dev->private)
 
-/*
- * The struct comedi_driver structure tells the Comedi core module
- * which functions to call to configure/deconfigure (attach/detach)
- * the board, and also about the kernel module that contains
- * the device code.
- */
-static int pcidio_attach(struct comedi_device *dev,
-			 struct comedi_devconfig *it);
-static int pcidio_detach(struct comedi_device *dev);
-static struct comedi_driver driver_cb_pcidio = {
-	.driver_name = "cb_pcidio",
-	.module = THIS_MODULE,
-	.attach = pcidio_attach,
-	.detach = pcidio_detach,
-
-/* It is not necessary to implement the following members if you are
- * writing a driver for a ISA PnP or PCI card */
-
-	/* Most drivers will support multiple types of boards by
-	 * having an array of board structures.  These were defined
-	 * in pcidio_boards[] above.  Note that the element 'name'
-	 * was first in the structure -- Comedi uses this fact to
-	 * extract the name of the board without knowing any details
-	 * about the structure except for its length.
-	 * When a device is attached (by comedi_config), the name
-	 * of the device is given to Comedi, and Comedi tries to
-	 * match it by going through the list of board names.  If
-	 * there is a match, the address of the pointer is put
-	 * into dev->board_ptr and driver->attach() is called.
-	 *
-	 * Note that these are not necessary if you can determine
-	 * the type of board in software.  ISA PnP, PCI, and PCMCIA
-	 * devices are such boards.
-	 */
-
-/* The following fields should NOT be initialized if you are dealing
- * with PCI devices
- *
- *	.board_name = pcidio_boards,
- *	.offset = sizeof(struct pcidio_board),
- *	.num_names = sizeof(pcidio_boards) / sizeof(structpcidio_board),
- */
-
-};
-
-/*------------------------------- FUNCTIONS -----------------------------------*/
-
-/*
- * Attach is called by the Comedi core to configure the driver
- * for a particular board.  If you specified a board_name array
- * in the driver structure, dev->board_ptr contains that
- * address.
- */
 static int pcidio_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 {
 	struct pci_dev *pcidev = NULL;
@@ -261,15 +195,7 @@ found:
 	return 1;
 }
 
-/*
- * _detach is called to deconfigure a device.  It should deallocate
- * resources.
- * This function is also called when _attach() fails, so it should be
- * careful not to release resources that were not necessarily
- * allocated by _attach().  dev->private and dev->subdevices are
- * deallocated automatically by the core.
- */
-static int pcidio_detach(struct comedi_device *dev)
+static void pcidio_detach(struct comedi_device *dev)
 {
 	if (devpriv) {
 		if (devpriv->pci_dev) {
@@ -283,50 +209,41 @@ static int pcidio_detach(struct comedi_device *dev)
 		for (i = 0; i < thisboard->n_8255; i++)
 			subdev_8255_cleanup(dev, dev->subdevices + i);
 	}
-	return 0;
 }
 
-/*
- * A convenient macro that defines init_module() and cleanup_module(),
- * as necessary.
- */
-static int __devinit driver_cb_pcidio_pci_probe(struct pci_dev *dev,
+static struct comedi_driver cb_pcidio_driver = {
+	.driver_name	= "cb_pcidio",
+	.module		= THIS_MODULE,
+	.attach		= pcidio_attach,
+	.detach		= pcidio_detach,
+};
+
+static int __devinit cb_pcidio_pci_probe(struct pci_dev *dev,
 						const struct pci_device_id *ent)
 {
-	return comedi_pci_auto_config(dev, driver_cb_pcidio.driver_name);
+	return comedi_pci_auto_config(dev, &cb_pcidio_driver);
 }
 
-static void __devexit driver_cb_pcidio_pci_remove(struct pci_dev *dev)
+static void __devexit cb_pcidio_pci_remove(struct pci_dev *dev)
 {
 	comedi_pci_auto_unconfig(dev);
 }
 
-static struct pci_driver driver_cb_pcidio_pci_driver = {
-	.id_table = pcidio_pci_table,
-	.probe = &driver_cb_pcidio_pci_probe,
-	.remove = __devexit_p(&driver_cb_pcidio_pci_remove)
+static DEFINE_PCI_DEVICE_TABLE(cb_pcidio_pci_table) = {
+	{ PCI_DEVICE(PCI_VENDOR_ID_CB, 0x0028) },
+	{ PCI_DEVICE(PCI_VENDOR_ID_CB, 0x0014) },
+	{ PCI_DEVICE(PCI_VENDOR_ID_CB, 0x000b) },
+	{ 0 }
 };
+MODULE_DEVICE_TABLE(pci, cb_pcidio_pci_table);
 
-static int __init driver_cb_pcidio_init_module(void)
-{
-	int retval;
-
-	retval = comedi_driver_register(&driver_cb_pcidio);
-	if (retval < 0)
-		return retval;
-
-	driver_cb_pcidio_pci_driver.name = (char *)driver_cb_pcidio.driver_name;
-	return pci_register_driver(&driver_cb_pcidio_pci_driver);
-}
-
-static void __exit driver_cb_pcidio_cleanup_module(void)
-{
-	pci_unregister_driver(&driver_cb_pcidio_pci_driver);
-	comedi_driver_unregister(&driver_cb_pcidio);
-}
-
-module_init(driver_cb_pcidio_init_module);
-module_exit(driver_cb_pcidio_cleanup_module);
+static struct pci_driver cb_pcidio_pci_driver = {
+	.name		= "cb_pcidio",
+	.id_table	= cb_pcidio_pci_table,
+	.probe		= cb_pcidio_pci_probe,
+	.remove		= __devexit_p(cb_pcidio_pci_remove),
+};
+module_comedi_pci_driver(cb_pcidio_driver, cb_pcidio_pci_driver);
 
 MODULE_AUTHOR("Comedi http://www.comedi.org");
 MODULE_DESCRIPTION("Comedi low-level driver");
diff --git a/drivers/staging/comedi/drivers/cb_pcimdas.c b/drivers/staging/comedi/drivers/cb_pcimdas.c
index 8ba694263bd..5f834d02ec2 100644
--- a/drivers/staging/comedi/drivers/cb_pcimdas.c
+++ b/drivers/staging/comedi/drivers/cb_pcimdas.c
@@ -123,15 +123,6 @@ static const struct cb_pcimdas_board cb_pcimdas_boards[] = {
 	 },
 };
 
-/* This is used by modprobe to translate PCI IDs to drivers.  Should
- * only be used for PCI and ISA-PnP devices */
-static DEFINE_PCI_DEVICE_TABLE(cb_pcimdas_pci_table) = {
-	{ PCI_DEVICE(PCI_VENDOR_ID_COMPUTERBOARDS, 0x0056) },
-	{ 0 }
-};
-
-MODULE_DEVICE_TABLE(pci, cb_pcimdas_pci_table);
-
 #define N_BOARDS 1		/*  Max number of boards supported */
 
 /*
@@ -139,9 +130,12 @@ MODULE_DEVICE_TABLE(pci, cb_pcimdas_pci_table);
  */
 #define thisboard ((const struct cb_pcimdas_board *)dev->board_ptr)
 
-/* this structure is for data unique to this hardware driver.  If
-   several hardware drivers keep similar information in this structure,
-   feel free to suggest moving the variable to the struct comedi_device struct.  */
+/*
+ * this structure is for data unique to this hardware driver.  If
+ * several hardware drivers keep similar information in this structure,
+ * feel free to suggest moving the variable to the struct comedi_device
+ * struct.
+ */
 struct cb_pcimdas_private {
 	int data;
 
@@ -172,22 +166,6 @@ struct cb_pcimdas_private {
  */
 #define devpriv ((struct cb_pcimdas_private *)dev->private)
 
-/*
- * The struct comedi_driver structure tells the Comedi core module
- * which functions to call to configure/deconfigure (attach/detach)
- * the board, and also about the kernel module that contains
- * the device code.
- */
-static int cb_pcimdas_attach(struct comedi_device *dev,
-			     struct comedi_devconfig *it);
-static int cb_pcimdas_detach(struct comedi_device *dev);
-static struct comedi_driver driver_cb_pcimdas = {
-	.driver_name = "cb_pcimdas",
-	.module = THIS_MODULE,
-	.attach = cb_pcimdas_attach,
-	.detach = cb_pcimdas_detach,
-};
-
 static int cb_pcimdas_ai_rinsn(struct comedi_device *dev,
 			       struct comedi_subdevice *s,
 			       struct comedi_insn *insn, unsigned int *data);
@@ -317,7 +295,8 @@ found:
 	s->subdev_flags = SDF_WRITABLE;
 	s->n_chan = thisboard->ao_nchan;
 	s->maxdata = 1 << thisboard->ao_bits;
-	s->range_table = &range_unknown;	/* ranges are hardware settable, but not software readable. */
+	/* ranges are hardware settable, but not software readable. */
+	s->range_table = &range_unknown;
 	s->insn_write = &cb_pcimdas_ao_winsn;
 	s->insn_read = &cb_pcimdas_ao_rinsn;
 
@@ -331,29 +310,8 @@ found:
 	return 1;
 }
 
-/*
- * _detach is called to deconfigure a device.  It should deallocate
- * resources.
- * This function is also called when _attach() fails, so it should be
- * careful not to release resources that were not necessarily
- * allocated by _attach().  dev->private and dev->subdevices are
- * deallocated automatically by the core.
- */
-static int cb_pcimdas_detach(struct comedi_device *dev)
+static void cb_pcimdas_detach(struct comedi_device *dev)
 {
-	if (devpriv) {
-		dev_dbg(dev->hw_dev, "devpriv->BADR0 = 0x%lx\n",
-			devpriv->BADR0);
-		dev_dbg(dev->hw_dev, "devpriv->BADR1 = 0x%lx\n",
-			devpriv->BADR1);
-		dev_dbg(dev->hw_dev, "devpriv->BADR2 = 0x%lx\n",
-			devpriv->BADR2);
-		dev_dbg(dev->hw_dev, "devpriv->BADR3 = 0x%lx\n",
-			devpriv->BADR3);
-		dev_dbg(dev->hw_dev, "devpriv->BADR4 = 0x%lx\n",
-			devpriv->BADR4);
-	}
-
 	if (dev->irq)
 		free_irq(dev->irq, dev);
 	if (devpriv) {
@@ -363,8 +321,6 @@ static int cb_pcimdas_detach(struct comedi_device *dev)
 			pci_dev_put(devpriv->pci_dev);
 		}
 	}
-
-	return 0;
 }
 
 /*
@@ -402,7 +358,10 @@ static int cb_pcimdas_ai_rinsn(struct comedi_device *dev,
 	outb(0x01, devpriv->BADR3 + 6);	/* set bursting off, conversions on */
 	outb(0x00, devpriv->BADR3 + 7);	/* set range to 10V. UP/BP is controlled by a switch on the board */
 
-	/*  write channel limits to multiplexer, set Low (bits 0-3) and High (bits 4-7) channels to chan. */
+	/*
+	 * write channel limits to multiplexer, set Low (bits 0-3) and
+	 * High (bits 4-7) channels to chan.
+	 */
 	chanlims = chan | (chan << 4);
 	outb(chanlims, devpriv->BADR3 + 0);
 
@@ -479,49 +438,37 @@ static int cb_pcimdas_ao_rinsn(struct comedi_device *dev,
 	return i;
 }
 
-/*
- * A convenient macro that defines init_module() and cleanup_module(),
- * as necessary.
- */
-static int __devinit driver_cb_pcimdas_pci_probe(struct pci_dev *dev,
-						 const struct pci_device_id
-						 *ent)
+static struct comedi_driver cb_pcimdas_driver = {
+	.driver_name	= "cb_pcimdas",
+	.module		= THIS_MODULE,
+	.attach		= cb_pcimdas_attach,
+	.detach		= cb_pcimdas_detach,
+};
+
+static int __devinit cb_pcimdas_pci_probe(struct pci_dev *dev,
+					  const struct pci_device_id *ent)
 {
-	return comedi_pci_auto_config(dev, driver_cb_pcimdas.driver_name);
+	return comedi_pci_auto_config(dev, &cb_pcimdas_driver);
 }
 
-static void __devexit driver_cb_pcimdas_pci_remove(struct pci_dev *dev)
+static void __devexit cb_pcimdas_pci_remove(struct pci_dev *dev)
 {
 	comedi_pci_auto_unconfig(dev);
 }
 
-static struct pci_driver driver_cb_pcimdas_pci_driver = {
-	.id_table = cb_pcimdas_pci_table,
-	.probe = &driver_cb_pcimdas_pci_probe,
-	.remove = __devexit_p(&driver_cb_pcimdas_pci_remove)
+static DEFINE_PCI_DEVICE_TABLE(cb_pcimdas_pci_table) = {
+	{ PCI_DEVICE(PCI_VENDOR_ID_COMPUTERBOARDS, 0x0056) },
+	{ 0 }
 };
+MODULE_DEVICE_TABLE(pci, cb_pcimdas_pci_table);
 
-static int __init driver_cb_pcimdas_init_module(void)
-{
-	int retval;
-
-	retval = comedi_driver_register(&driver_cb_pcimdas);
-	if (retval < 0)
-		return retval;
-
-	driver_cb_pcimdas_pci_driver.name =
-	    (char *)driver_cb_pcimdas.driver_name;
-	return pci_register_driver(&driver_cb_pcimdas_pci_driver);
-}
-
-static void __exit driver_cb_pcimdas_cleanup_module(void)
-{
-	pci_unregister_driver(&driver_cb_pcimdas_pci_driver);
-	comedi_driver_unregister(&driver_cb_pcimdas);
-}
-
-module_init(driver_cb_pcimdas_init_module);
-module_exit(driver_cb_pcimdas_cleanup_module);
+static struct pci_driver cb_pcimdas_pci_driver = {
+	.name		= "cb_pcimdas",
+	.id_table	= cb_pcimdas_pci_table,
+	.probe		= cb_pcimdas_pci_probe,
+	.remove		= __devexit_p(cb_pcimdas_pci_remove),
+};
+module_comedi_pci_driver(cb_pcimdas_driver, cb_pcimdas_pci_driver);
 
 MODULE_AUTHOR("Comedi http://www.comedi.org");
 MODULE_DESCRIPTION("Comedi low-level driver");
diff --git a/drivers/staging/comedi/drivers/cb_pcimdda.c b/drivers/staging/comedi/drivers/cb_pcimdda.c
index 40bddfa2222..b339685e234 100644
--- a/drivers/staging/comedi/drivers/cb_pcimdda.c
+++ b/drivers/staging/comedi/drivers/cb_pcimdda.c
@@ -140,17 +140,6 @@ static const struct board_struct boards[] = {
 #define REG_SZ (thisboard->reg_sz)
 #define REGS_BADRINDEX (thisboard->regs_badrindex)
 
-/* This is used by modprobe to translate PCI IDs to drivers.  Should
- * only be used for PCI and ISA-PnP devices */
-/* Please add your PCI vendor ID to comedidev.h, and it will be forwarded
- * upstream. */
-static DEFINE_PCI_DEVICE_TABLE(pci_table) = {
-	{ PCI_DEVICE(PCI_VENDOR_ID_COMPUTERBOARDS, PCI_ID_PCIM_DDA06_16) },
-	{0}
-};
-
-MODULE_DEVICE_TABLE(pci, pci_table);
-
 /*
  * this structure is for data unique to this hardware driver.  If
  * several hardware drivers keep similar information in this structure,
@@ -161,7 +150,6 @@ struct board_private_struct {
 	unsigned long registers;	/* set by probe */
 	unsigned long dio_registers;
 	char attached_to_8255;	/* boolean */
-	char attached_successfully;	/* boolean */
 	/* would be useful for a PCI device */
 	struct pci_dev *pci_dev;
 
@@ -177,66 +165,6 @@ struct board_private_struct {
  */
 #define devpriv ((struct board_private_struct *)dev->private)
 
-/*
- * The struct comedi_driver structure tells the Comedi core module
- * which functions to call to configure/deconfigure (attach/detach)
- * the board, and also about the kernel module that contains
- * the device code.
- */
-static int attach(struct comedi_device *dev, struct comedi_devconfig *it);
-static int detach(struct comedi_device *dev);
-static struct comedi_driver cb_pcimdda_driver = {
-	.driver_name = "cb_pcimdda",
-	.module = THIS_MODULE,
-	.attach = attach,
-	.detach = detach,
-};
-
-MODULE_AUTHOR("Calin A. Culianu <calin@rtlab.org>");
-MODULE_DESCRIPTION("Comedi low-level driver for the Computerboards PCIM-DDA "
-		   "series.  Currently only supports PCIM-DDA06-16 (which "
-		   "also happens to be the only board in this series. :) ) ");
-MODULE_LICENSE("GPL");
-static int __devinit cb_pcimdda_driver_pci_probe(struct pci_dev *dev,
-						 const struct pci_device_id
-						 *ent)
-{
-	return comedi_pci_auto_config(dev, cb_pcimdda_driver.driver_name);
-}
-
-static void __devexit cb_pcimdda_driver_pci_remove(struct pci_dev *dev)
-{
-	comedi_pci_auto_unconfig(dev);
-}
-
-static struct pci_driver cb_pcimdda_driver_pci_driver = {
-	.id_table = pci_table,
-	.probe = &cb_pcimdda_driver_pci_probe,
-	.remove = __devexit_p(&cb_pcimdda_driver_pci_remove)
-};
-
-static int __init cb_pcimdda_driver_init_module(void)
-{
-	int retval;
-
-	retval = comedi_driver_register(&cb_pcimdda_driver);
-	if (retval < 0)
-		return retval;
-
-	cb_pcimdda_driver_pci_driver.name =
-	    (char *)cb_pcimdda_driver.driver_name;
-	return pci_register_driver(&cb_pcimdda_driver_pci_driver);
-}
-
-static void __exit cb_pcimdda_driver_cleanup_module(void)
-{
-	pci_unregister_driver(&cb_pcimdda_driver_pci_driver);
-	comedi_driver_unregister(&cb_pcimdda_driver);
-}
-
-module_init(cb_pcimdda_driver_init_module);
-module_exit(cb_pcimdda_driver_cleanup_module);
-
 static int ao_winsn(struct comedi_device *dev, struct comedi_subdevice *s,
 		    struct comedi_insn *insn, unsigned int *data);
 static int ao_rinsn(struct comedi_device *dev, struct comedi_subdevice *s,
@@ -354,44 +282,24 @@ static int attach(struct comedi_device *dev, struct comedi_devconfig *it)
 		s->type = COMEDI_SUBD_UNUSED;
 	}
 
-	devpriv->attached_successfully = 1;
-
 	printk("attached\n");
 
 	return 1;
 }
 
-/*
- * _detach is called to deconfigure a device.  It should deallocate
- * resources.
- * This function is also called when _attach() fails, so it should be
- * careful not to release resources that were not necessarily
- * allocated by _attach().  dev->private and dev->subdevices are
- * deallocated automatically by the core.
- */
-static int detach(struct comedi_device *dev)
+static void detach(struct comedi_device *dev)
 {
 	if (devpriv) {
-
 		if (dev->subdevices && devpriv->attached_to_8255) {
-			/* de-register us from the 8255 driver */
 			subdev_8255_cleanup(dev, dev->subdevices + 2);
 			devpriv->attached_to_8255 = 0;
 		}
-
 		if (devpriv->pci_dev) {
 			if (devpriv->registers)
 				comedi_pci_disable(devpriv->pci_dev);
 			pci_dev_put(devpriv->pci_dev);
 		}
-
-		if (devpriv->attached_successfully && thisboard)
-			printk("comedi%d: %s: detached\n", dev->minor,
-			       thisboard->name);
-
 	}
-
-	return 0;
 }
 
 static int ao_winsn(struct comedi_device *dev, struct comedi_subdevice *s,
@@ -515,3 +423,41 @@ static int probe(struct comedi_device *dev, const struct comedi_devconfig *it)
 	       "card found at the requested position\n");
 	return -ENODEV;
 }
+
+static struct comedi_driver cb_pcimdda_driver = {
+	.driver_name	= "cb_pcimdda",
+	.module		= THIS_MODULE,
+	.attach		= attach,
+	.detach		= detach,
+};
+
+static int __devinit cb_pcimdda_pci_probe(struct pci_dev *dev,
+					  const struct pci_device_id *ent)
+{
+	return comedi_pci_auto_config(dev, &cb_pcimdda_driver);
+}
+
+static void __devexit cb_pcimdda_pci_remove(struct pci_dev *dev)
+{
+	comedi_pci_auto_unconfig(dev);
+}
+
+static DEFINE_PCI_DEVICE_TABLE(cb_pcimdda_pci_table) = {
+	{ PCI_DEVICE(PCI_VENDOR_ID_COMPUTERBOARDS, PCI_ID_PCIM_DDA06_16) },
+	{ 0 }
+};
+MODULE_DEVICE_TABLE(pci, cb_pcimdda_pci_table);
+
+static struct pci_driver cb_pcimdda_driver_pci_driver = {
+	.name		= "cb_pcimdda",
+	.id_table	= cb_pcimdda_pci_table,
+	.probe		= cb_pcimdda_pci_probe,
+	.remove		= __devexit_p(cb_pcimdda_pci_remove),
+};
+module_comedi_pci_driver(cb_pcimdda_driver, cb_pcimdda_driver_pci_driver);
+
+MODULE_AUTHOR("Calin A. Culianu <calin@rtlab.org>");
+MODULE_DESCRIPTION("Comedi low-level driver for the Computerboards PCIM-DDA "
+		   "series.  Currently only supports PCIM-DDA06-16 (which "
+		   "also happens to be the only board in this series. :) ) ");
+MODULE_LICENSE("GPL");
diff --git a/drivers/staging/comedi/drivers/comedi_bond.c b/drivers/staging/comedi/drivers/comedi_bond.c
index d8aefb23d6b..29412de06c3 100644
--- a/drivers/staging/comedi/drivers/comedi_bond.c
+++ b/drivers/staging/comedi/drivers/comedi_bond.c
@@ -60,7 +60,6 @@ Configuration Options:
 #define MAX_CHANS 256
 
 #define MODULE_NAME "comedi_bond"
-MODULE_LICENSE("GPL");
 #ifndef STR
 #  define STR1(x) #x
 #  define STR(x) STR1(x)
@@ -79,10 +78,6 @@ MODULE_PARM_DESC(debug, "If true, print extra cryptic debugging output useful"
 	} while (0)
 #define WARNING(x...)  printk(KERN_WARNING MODULE_NAME ": WARNING: "x)
 #define ERROR(x...)  printk(KERN_ERR MODULE_NAME ": INTERNAL ERROR: "x)
-MODULE_AUTHOR("Calin A. Culianu");
-MODULE_DESCRIPTION(MODULE_NAME "A driver for COMEDI to bond multiple COMEDI "
-		   "devices together as one.  In the words of John Lennon: "
-		   "'And the world will live as one...'");
 
 /*
  * Board descriptions for two imaginary boards.  Describing the
@@ -93,12 +88,6 @@ struct BondingBoard {
 	const char *name;
 };
 
-static const struct BondingBoard bondingBoards[] = {
-	{
-	 .name = MODULE_NAME,
-	 },
-};
-
 /*
  * Useful for shorthand access to the particular board structure
  */
@@ -133,129 +122,6 @@ struct Private {
  */
 #define devpriv ((struct Private *)dev->private)
 
-/*
- * The struct comedi_driver structure tells the Comedi core module
- * which functions to call to configure/deconfigure (attach/detach)
- * the board, and also about the kernel module that contains
- * the device code.
- */
-static int bonding_attach(struct comedi_device *dev,
-			  struct comedi_devconfig *it);
-static int bonding_detach(struct comedi_device *dev);
-/** Build Private array of all devices.. */
-static int doDevConfig(struct comedi_device *dev, struct comedi_devconfig *it);
-static void doDevUnconfig(struct comedi_device *dev);
-/* Ugly implementation of realloc that always copies memory around -- I'm lazy,
- * what can I say?  I like to do wasteful memcopies.. :) */
-static void *Realloc(const void *ptr, size_t len, size_t old_len);
-
-static struct comedi_driver driver_bonding = {
-	.driver_name = MODULE_NAME,
-	.module = THIS_MODULE,
-	.attach = bonding_attach,
-	.detach = bonding_detach,
-	/* It is not necessary to implement the following members if you are
-	 * writing a driver for a ISA PnP or PCI card */
-	/* Most drivers will support multiple types of boards by
-	 * having an array of board structures.  These were defined
-	 * in skel_boards[] above.  Note that the element 'name'
-	 * was first in the structure -- Comedi uses this fact to
-	 * extract the name of the board without knowing any details
-	 * about the structure except for its length.
-	 * When a device is attached (by comedi_config), the name
-	 * of the device is given to Comedi, and Comedi tries to
-	 * match it by going through the list of board names.  If
-	 * there is a match, the address of the pointer is put
-	 * into dev->board_ptr and driver->attach() is called.
-	 *
-	 * Note that these are not necessary if you can determine
-	 * the type of board in software.  ISA PnP, PCI, and PCMCIA
-	 * devices are such boards.
-	 */
-	.board_name = &bondingBoards[0].name,
-	.offset = sizeof(struct BondingBoard),
-	.num_names = ARRAY_SIZE(bondingBoards),
-};
-
-static int bonding_dio_insn_bits(struct comedi_device *dev,
-				 struct comedi_subdevice *s,
-				 struct comedi_insn *insn, unsigned int *data);
-static int bonding_dio_insn_config(struct comedi_device *dev,
-				   struct comedi_subdevice *s,
-				   struct comedi_insn *insn,
-				   unsigned int *data);
-
-/*
- * Attach is called by the Comedi core to configure the driver
- * for a particular board.  If you specified a board_name array
- * in the driver structure, dev->board_ptr contains that
- * address.
- */
-static int bonding_attach(struct comedi_device *dev,
-			  struct comedi_devconfig *it)
-{
-	struct comedi_subdevice *s;
-
-	LOG_MSG("comedi%d\n", dev->minor);
-
-	/*
-	 * Allocate the private structure area.  alloc_private() is a
-	 * convenient macro defined in comedidev.h.
-	 */
-	if (alloc_private(dev, sizeof(struct Private)) < 0)
-		return -ENOMEM;
-
-	/*
-	 * Setup our bonding from config params.. sets up our Private struct..
-	 */
-	if (!doDevConfig(dev, it))
-		return -EINVAL;
-
-	/*
-	 * Initialize dev->board_name.  Note that we can use the "thisboard"
-	 * macro now, since we just initialized it in the last line.
-	 */
-	dev->board_name = devpriv->name;
-
-	/*
-	 * Allocate the subdevice structures.  alloc_subdevice() is a
-	 * convenient macro defined in comedidev.h.
-	 */
-	if (alloc_subdevices(dev, 1) < 0)
-		return -ENOMEM;
-
-	s = dev->subdevices + 0;
-	s->type = COMEDI_SUBD_DIO;
-	s->subdev_flags = SDF_READABLE | SDF_WRITABLE;
-	s->n_chan = devpriv->nchans;
-	s->maxdata = 1;
-	s->range_table = &range_digital;
-	s->insn_bits = bonding_dio_insn_bits;
-	s->insn_config = bonding_dio_insn_config;
-
-	LOG_MSG("attached with %u DIO channels coming from %u different "
-		"subdevices all bonded together.  "
-		"John Lennon would be proud!\n",
-		devpriv->nchans, devpriv->ndevs);
-
-	return 1;
-}
-
-/*
- * _detach is called to deconfigure a device.  It should deallocate
- * resources.
- * This function is also called when _attach() fails, so it should be
- * careful not to release resources that were not necessarily
- * allocated by _attach().  dev->private and dev->subdevices are
- * deallocated automatically by the core.
- */
-static int bonding_detach(struct comedi_device *dev)
-{
-	LOG_MSG("comedi%d: remove\n", dev->minor);
-	doDevUnconfig(dev);
-	return 0;
-}
-
 /* DIO devices are slightly special.  Although it is possible to
  * implement the insn_read/insn_write interface, it is much more
  * useful to applications if you implement the insn_bits interface.
@@ -466,7 +332,57 @@ static int doDevConfig(struct comedi_device *dev, struct comedi_devconfig *it)
 	return 1;
 }
 
-static void doDevUnconfig(struct comedi_device *dev)
+static int bonding_attach(struct comedi_device *dev,
+			  struct comedi_devconfig *it)
+{
+	struct comedi_subdevice *s;
+
+	LOG_MSG("comedi%d\n", dev->minor);
+
+	/*
+	 * Allocate the private structure area.  alloc_private() is a
+	 * convenient macro defined in comedidev.h.
+	 */
+	if (alloc_private(dev, sizeof(struct Private)) < 0)
+		return -ENOMEM;
+
+	/*
+	 * Setup our bonding from config params.. sets up our Private struct..
+	 */
+	if (!doDevConfig(dev, it))
+		return -EINVAL;
+
+	/*
+	 * Initialize dev->board_name.  Note that we can use the "thisboard"
+	 * macro now, since we just initialized it in the last line.
+	 */
+	dev->board_name = devpriv->name;
+
+	/*
+	 * Allocate the subdevice structures.  alloc_subdevice() is a
+	 * convenient macro defined in comedidev.h.
+	 */
+	if (alloc_subdevices(dev, 1) < 0)
+		return -ENOMEM;
+
+	s = dev->subdevices + 0;
+	s->type = COMEDI_SUBD_DIO;
+	s->subdev_flags = SDF_READABLE | SDF_WRITABLE;
+	s->n_chan = devpriv->nchans;
+	s->maxdata = 1;
+	s->range_table = &range_digital;
+	s->insn_bits = bonding_dio_insn_bits;
+	s->insn_config = bonding_dio_insn_config;
+
+	LOG_MSG("attached with %u DIO channels coming from %u different "
+		"subdevices all bonded together.  "
+		"John Lennon would be proud!\n",
+		devpriv->nchans, devpriv->ndevs);
+
+	return 1;
+}
+
+static void bonding_detach(struct comedi_device *dev)
 {
 	unsigned long devs_closed = 0;
 
@@ -490,15 +406,25 @@ static void doDevUnconfig(struct comedi_device *dev)
 	}
 }
 
-static int __init init(void)
-{
-	return comedi_driver_register(&driver_bonding);
-}
+static const struct BondingBoard bondingBoards[] = {
+	{
+		.name		= "comedi_bond",
+	},
+};
 
-static void __exit cleanup(void)
-{
-	comedi_driver_unregister(&driver_bonding);
-}
+static struct comedi_driver bonding_driver = {
+	.driver_name	= "comedi_bond",
+	.module		= THIS_MODULE,
+	.attach		= bonding_attach,
+	.detach		= bonding_detach,
+	.board_name	= &bondingBoards[0].name,
+	.offset		= sizeof(struct BondingBoard),
+	.num_names	= ARRAY_SIZE(bondingBoards),
+};
+module_comedi_driver(bonding_driver);
 
-module_init(init);
-module_exit(cleanup);
+MODULE_AUTHOR("Calin A. Culianu");
+MODULE_DESCRIPTION(MODULE_NAME "A driver for COMEDI to bond multiple COMEDI "
+		   "devices together as one.  In the words of John Lennon: "
+		   "'And the world will live as one...'");
+MODULE_LICENSE("GPL");
diff --git a/drivers/staging/comedi/drivers/comedi_parport.c b/drivers/staging/comedi/drivers/comedi_parport.c
index 21d834dd92b..bff5dcd7610 100644
--- a/drivers/staging/comedi/drivers/comedi_parport.c
+++ b/drivers/staging/comedi/drivers/comedi_parport.c
@@ -91,29 +91,6 @@ pin, which can be used to wake up tasks.
 #define PARPORT_B 1
 #define PARPORT_C 2
 
-static int parport_attach(struct comedi_device *dev,
-			  struct comedi_devconfig *it);
-static int parport_detach(struct comedi_device *dev);
-static struct comedi_driver driver_parport = {
-	.driver_name = "comedi_parport",
-	.module = THIS_MODULE,
-	.attach = parport_attach,
-	.detach = parport_detach,
-};
-
-static int __init driver_parport_init_module(void)
-{
-	return comedi_driver_register(&driver_parport);
-}
-
-static void __exit driver_parport_cleanup_module(void)
-{
-	comedi_driver_unregister(&driver_parport);
-}
-
-module_init(driver_parport_init_module);
-module_exit(driver_parport_cleanup_module);
-
 struct parport_private {
 	unsigned int a_data;
 	unsigned int c_data;
@@ -395,19 +372,22 @@ static int parport_attach(struct comedi_device *dev,
 	return 1;
 }
 
-static int parport_detach(struct comedi_device *dev)
+static void parport_detach(struct comedi_device *dev)
 {
-	printk(KERN_INFO "comedi%d: parport: remove\n", dev->minor);
-
 	if (dev->iobase)
 		release_region(dev->iobase, PARPORT_SIZE);
-
 	if (dev->irq)
 		free_irq(dev->irq, dev);
-
-	return 0;
 }
 
+static struct comedi_driver parport_driver = {
+	.driver_name	= "comedi_parport",
+	.module		= THIS_MODULE,
+	.attach		= parport_attach,
+	.detach		= parport_detach,
+};
+module_comedi_driver(parport_driver);
+
 MODULE_AUTHOR("Comedi http://www.comedi.org");
 MODULE_DESCRIPTION("Comedi low-level driver");
 MODULE_LICENSE("GPL");
diff --git a/drivers/staging/comedi/drivers/comedi_test.c b/drivers/staging/comedi/drivers/comedi_test.c
index a804742b802..873e37450bb 100644
--- a/drivers/staging/comedi/drivers/comedi_test.c
+++ b/drivers/staging/comedi/drivers/comedi_test.c
@@ -67,15 +67,6 @@ struct waveform_board {
 
 #define N_CHANS 8
 
-static const struct waveform_board waveform_boards[] = {
-	{
-	 .name = "comedi_test",
-	 .ai_chans = N_CHANS,
-	 .ai_bits = 16,
-	 .have_dio = 0,
-	 },
-};
-
 #define thisboard ((const struct waveform_board *)dev->board_ptr)
 
 /* Data unique to this driver */
@@ -94,54 +85,6 @@ struct waveform_private {
 };
 #define devpriv ((struct waveform_private *)dev->private)
 
-static int waveform_attach(struct comedi_device *dev,
-			   struct comedi_devconfig *it);
-static int waveform_detach(struct comedi_device *dev);
-static struct comedi_driver driver_waveform = {
-	.driver_name = "comedi_test",
-	.module = THIS_MODULE,
-	.attach = waveform_attach,
-	.detach = waveform_detach,
-	.board_name = &waveform_boards[0].name,
-	.offset = sizeof(struct waveform_board),
-	.num_names = ARRAY_SIZE(waveform_boards),
-};
-
-static int __init driver_waveform_init_module(void)
-{
-	return comedi_driver_register(&driver_waveform);
-}
-
-static void __exit driver_waveform_cleanup_module(void)
-{
-	comedi_driver_unregister(&driver_waveform);
-}
-
-module_init(driver_waveform_init_module);
-module_exit(driver_waveform_cleanup_module);
-
-static int waveform_ai_cmdtest(struct comedi_device *dev,
-			       struct comedi_subdevice *s,
-			       struct comedi_cmd *cmd);
-static int waveform_ai_cmd(struct comedi_device *dev,
-			   struct comedi_subdevice *s);
-static int waveform_ai_cancel(struct comedi_device *dev,
-			      struct comedi_subdevice *s);
-static int waveform_ai_insn_read(struct comedi_device *dev,
-				 struct comedi_subdevice *s,
-				 struct comedi_insn *insn, unsigned int *data);
-static int waveform_ao_insn_write(struct comedi_device *dev,
-				  struct comedi_subdevice *s,
-				  struct comedi_insn *insn, unsigned int *data);
-static short fake_sawtooth(struct comedi_device *dev, unsigned int range,
-			   unsigned long current_time);
-static short fake_squarewave(struct comedi_device *dev, unsigned int range,
-			     unsigned long current_time);
-static short fake_flatline(struct comedi_device *dev, unsigned int range,
-			   unsigned long current_time);
-static short fake_waveform(struct comedi_device *dev, unsigned int channel,
-			   unsigned int range, unsigned long current_time);
-
 /* 1000 nanosec in a microsec */
 static const int nano_per_micro = 1000;
 
@@ -154,6 +97,78 @@ static const struct comedi_lrange waveform_ai_ranges = {
 	 }
 };
 
+static short fake_sawtooth(struct comedi_device *dev, unsigned int range_index,
+			   unsigned long current_time)
+{
+	struct comedi_subdevice *s = dev->read_subdev;
+	unsigned int offset = s->maxdata / 2;
+	u64 value;
+	const struct comedi_krange *krange =
+	    &s->range_table->range[range_index];
+	u64 binary_amplitude;
+
+	binary_amplitude = s->maxdata;
+	binary_amplitude *= devpriv->uvolt_amplitude;
+	do_div(binary_amplitude, krange->max - krange->min);
+
+	current_time %= devpriv->usec_period;
+	value = current_time;
+	value *= binary_amplitude * 2;
+	do_div(value, devpriv->usec_period);
+	value -= binary_amplitude;	/* get rid of sawtooth's dc offset */
+
+	return offset + value;
+}
+
+static short fake_squarewave(struct comedi_device *dev,
+			     unsigned int range_index,
+			     unsigned long current_time)
+{
+	struct comedi_subdevice *s = dev->read_subdev;
+	unsigned int offset = s->maxdata / 2;
+	u64 value;
+	const struct comedi_krange *krange =
+	    &s->range_table->range[range_index];
+	current_time %= devpriv->usec_period;
+
+	value = s->maxdata;
+	value *= devpriv->uvolt_amplitude;
+	do_div(value, krange->max - krange->min);
+
+	if (current_time < devpriv->usec_period / 2)
+		value *= -1;
+
+	return offset + value;
+}
+
+static short fake_flatline(struct comedi_device *dev, unsigned int range_index,
+			   unsigned long current_time)
+{
+	return dev->read_subdev->maxdata / 2;
+}
+
+/* generates a different waveform depending on what channel is read */
+static short fake_waveform(struct comedi_device *dev, unsigned int channel,
+			   unsigned int range, unsigned long current_time)
+{
+	enum {
+		SAWTOOTH_CHAN,
+		SQUARE_CHAN,
+	};
+	switch (channel) {
+	case SAWTOOTH_CHAN:
+		return fake_sawtooth(dev, range, current_time);
+		break;
+	case SQUARE_CHAN:
+		return fake_squarewave(dev, range, current_time);
+		break;
+	default:
+		break;
+	}
+
+	return fake_flatline(dev, range, current_time);
+}
+
 /*
    This is the background routine used to generate arbitrary data.
    It should run in the background; therefore it is scheduled by
@@ -217,84 +232,6 @@ static void waveform_ai_interrupt(unsigned long arg)
 	comedi_event(dev, dev->read_subdev);
 }
 
-static int waveform_attach(struct comedi_device *dev,
-			   struct comedi_devconfig *it)
-{
-	struct comedi_subdevice *s;
-	int amplitude = it->options[0];
-	int period = it->options[1];
-	int i;
-
-	dev->board_name = thisboard->name;
-
-	if (alloc_private(dev, sizeof(struct waveform_private)) < 0)
-		return -ENOMEM;
-
-	/* set default amplitude and period */
-	if (amplitude <= 0)
-		amplitude = 1000000;	/* 1 volt */
-	if (period <= 0)
-		period = 100000;	/* 0.1 sec */
-
-	devpriv->uvolt_amplitude = amplitude;
-	devpriv->usec_period = period;
-
-	dev->n_subdevices = 2;
-	if (alloc_subdevices(dev, dev->n_subdevices) < 0)
-		return -ENOMEM;
-
-	s = dev->subdevices + 0;
-	dev->read_subdev = s;
-	/* analog input subdevice */
-	s->type = COMEDI_SUBD_AI;
-	s->subdev_flags = SDF_READABLE | SDF_GROUND | SDF_CMD_READ;
-	s->n_chan = thisboard->ai_chans;
-	s->maxdata = (1 << thisboard->ai_bits) - 1;
-	s->range_table = &waveform_ai_ranges;
-	s->len_chanlist = s->n_chan * 2;
-	s->insn_read = waveform_ai_insn_read;
-	s->do_cmd = waveform_ai_cmd;
-	s->do_cmdtest = waveform_ai_cmdtest;
-	s->cancel = waveform_ai_cancel;
-
-	s = dev->subdevices + 1;
-	dev->write_subdev = s;
-	/* analog output subdevice (loopback) */
-	s->type = COMEDI_SUBD_AO;
-	s->subdev_flags = SDF_WRITEABLE | SDF_GROUND;
-	s->n_chan = thisboard->ai_chans;
-	s->maxdata = (1 << thisboard->ai_bits) - 1;
-	s->range_table = &waveform_ai_ranges;
-	s->len_chanlist = s->n_chan * 2;
-	s->insn_write = waveform_ao_insn_write;
-	s->do_cmd = NULL;
-	s->do_cmdtest = NULL;
-	s->cancel = NULL;
-
-	/* Our default loopback value is just a 0V flatline */
-	for (i = 0; i < s->n_chan; i++)
-		devpriv->ao_loopbacks[i] = s->maxdata / 2;
-
-	init_timer(&(devpriv->timer));
-	devpriv->timer.function = waveform_ai_interrupt;
-	devpriv->timer.data = (unsigned long)dev;
-
-	printk(KERN_INFO "comedi%d: comedi_test: "
-	       "%i microvolt, %li microsecond waveform attached\n", dev->minor,
-	       devpriv->uvolt_amplitude, devpriv->usec_period);
-	return 1;
-}
-
-static int waveform_detach(struct comedi_device *dev)
-{
-	printk("comedi%d: comedi_test: remove\n", dev->minor);
-
-	if (dev->private)
-		waveform_ai_cancel(dev, dev->read_subdev);
-
-	return 0;
-}
-
 static int waveform_ai_cmdtest(struct comedi_device *dev,
 			       struct comedi_subdevice *s,
 			       struct comedi_cmd *cmd)
@@ -465,78 +402,6 @@ static int waveform_ai_cancel(struct comedi_device *dev,
 	return 0;
 }
 
-static short fake_sawtooth(struct comedi_device *dev, unsigned int range_index,
-			   unsigned long current_time)
-{
-	struct comedi_subdevice *s = dev->read_subdev;
-	unsigned int offset = s->maxdata / 2;
-	u64 value;
-	const struct comedi_krange *krange =
-	    &s->range_table->range[range_index];
-	u64 binary_amplitude;
-
-	binary_amplitude = s->maxdata;
-	binary_amplitude *= devpriv->uvolt_amplitude;
-	do_div(binary_amplitude, krange->max - krange->min);
-
-	current_time %= devpriv->usec_period;
-	value = current_time;
-	value *= binary_amplitude * 2;
-	do_div(value, devpriv->usec_period);
-	value -= binary_amplitude;	/* get rid of sawtooth's dc offset */
-
-	return offset + value;
-}
-
-static short fake_squarewave(struct comedi_device *dev,
-			     unsigned int range_index,
-			     unsigned long current_time)
-{
-	struct comedi_subdevice *s = dev->read_subdev;
-	unsigned int offset = s->maxdata / 2;
-	u64 value;
-	const struct comedi_krange *krange =
-	    &s->range_table->range[range_index];
-	current_time %= devpriv->usec_period;
-
-	value = s->maxdata;
-	value *= devpriv->uvolt_amplitude;
-	do_div(value, krange->max - krange->min);
-
-	if (current_time < devpriv->usec_period / 2)
-		value *= -1;
-
-	return offset + value;
-}
-
-static short fake_flatline(struct comedi_device *dev, unsigned int range_index,
-			   unsigned long current_time)
-{
-	return dev->read_subdev->maxdata / 2;
-}
-
-/* generates a different waveform depending on what channel is read */
-static short fake_waveform(struct comedi_device *dev, unsigned int channel,
-			   unsigned int range, unsigned long current_time)
-{
-	enum {
-		SAWTOOTH_CHAN,
-		SQUARE_CHAN,
-	};
-	switch (channel) {
-	case SAWTOOTH_CHAN:
-		return fake_sawtooth(dev, range, current_time);
-		break;
-	case SQUARE_CHAN:
-		return fake_squarewave(dev, range, current_time);
-		break;
-	default:
-		break;
-	}
-
-	return fake_flatline(dev, range, current_time);
-}
-
 static int waveform_ai_insn_read(struct comedi_device *dev,
 				 struct comedi_subdevice *s,
 				 struct comedi_insn *insn, unsigned int *data)
@@ -561,6 +426,100 @@ static int waveform_ao_insn_write(struct comedi_device *dev,
 	return insn->n;
 }
 
+static int waveform_attach(struct comedi_device *dev,
+			   struct comedi_devconfig *it)
+{
+	struct comedi_subdevice *s;
+	int amplitude = it->options[0];
+	int period = it->options[1];
+	int i;
+
+	dev->board_name = thisboard->name;
+
+	if (alloc_private(dev, sizeof(struct waveform_private)) < 0)
+		return -ENOMEM;
+
+	/* set default amplitude and period */
+	if (amplitude <= 0)
+		amplitude = 1000000;	/* 1 volt */
+	if (period <= 0)
+		period = 100000;	/* 0.1 sec */
+
+	devpriv->uvolt_amplitude = amplitude;
+	devpriv->usec_period = period;
+
+	dev->n_subdevices = 2;
+	if (alloc_subdevices(dev, dev->n_subdevices) < 0)
+		return -ENOMEM;
+
+	s = dev->subdevices + 0;
+	dev->read_subdev = s;
+	/* analog input subdevice */
+	s->type = COMEDI_SUBD_AI;
+	s->subdev_flags = SDF_READABLE | SDF_GROUND | SDF_CMD_READ;
+	s->n_chan = thisboard->ai_chans;
+	s->maxdata = (1 << thisboard->ai_bits) - 1;
+	s->range_table = &waveform_ai_ranges;
+	s->len_chanlist = s->n_chan * 2;
+	s->insn_read = waveform_ai_insn_read;
+	s->do_cmd = waveform_ai_cmd;
+	s->do_cmdtest = waveform_ai_cmdtest;
+	s->cancel = waveform_ai_cancel;
+
+	s = dev->subdevices + 1;
+	dev->write_subdev = s;
+	/* analog output subdevice (loopback) */
+	s->type = COMEDI_SUBD_AO;
+	s->subdev_flags = SDF_WRITEABLE | SDF_GROUND;
+	s->n_chan = thisboard->ai_chans;
+	s->maxdata = (1 << thisboard->ai_bits) - 1;
+	s->range_table = &waveform_ai_ranges;
+	s->len_chanlist = s->n_chan * 2;
+	s->insn_write = waveform_ao_insn_write;
+	s->do_cmd = NULL;
+	s->do_cmdtest = NULL;
+	s->cancel = NULL;
+
+	/* Our default loopback value is just a 0V flatline */
+	for (i = 0; i < s->n_chan; i++)
+		devpriv->ao_loopbacks[i] = s->maxdata / 2;
+
+	init_timer(&(devpriv->timer));
+	devpriv->timer.function = waveform_ai_interrupt;
+	devpriv->timer.data = (unsigned long)dev;
+
+	printk(KERN_INFO "comedi%d: comedi_test: "
+	       "%i microvolt, %li microsecond waveform attached\n", dev->minor,
+	       devpriv->uvolt_amplitude, devpriv->usec_period);
+	return 1;
+}
+
+static void waveform_detach(struct comedi_device *dev)
+{
+	if (dev->private)
+		waveform_ai_cancel(dev, dev->read_subdev);
+}
+
+static const struct waveform_board waveform_boards[] = {
+	{
+		.name		= "comedi_test",
+		.ai_chans	= N_CHANS,
+		.ai_bits	= 16,
+		.have_dio	= 0,
+	},
+};
+
+static struct comedi_driver waveform_driver = {
+	.driver_name	= "comedi_test",
+	.module		= THIS_MODULE,
+	.attach		= waveform_attach,
+	.detach		= waveform_detach,
+	.board_name	= &waveform_boards[0].name,
+	.offset		= sizeof(struct waveform_board),
+	.num_names	= ARRAY_SIZE(waveform_boards),
+};
+module_comedi_driver(waveform_driver);
+
 MODULE_AUTHOR("Comedi http://www.comedi.org");
 MODULE_DESCRIPTION("Comedi low-level driver");
 MODULE_LICENSE("GPL");
diff --git a/drivers/staging/comedi/drivers/contec_pci_dio.c b/drivers/staging/comedi/drivers/contec_pci_dio.c
index e3659bd6e85..b8bac80f2ba 100644
--- a/drivers/staging/comedi/drivers/contec_pci_dio.c
+++ b/drivers/staging/comedi/drivers/contec_pci_dio.c
@@ -56,13 +56,6 @@ static const struct contec_board contec_boards[] = {
 };
 
 #define PCI_DEVICE_ID_PIO1616L 0x8172
-static DEFINE_PCI_DEVICE_TABLE(contec_pci_table) = {
-	{ PCI_DEVICE(PCI_VENDOR_ID_CONTEC, PCI_DEVICE_ID_PIO1616L),
-		.driver_data = PIO1616L },
-	{0}
-};
-
-MODULE_DEVICE_TABLE(pci, contec_pci_table);
 
 #define thisboard ((const struct contec_board *)dev->board_ptr)
 
@@ -75,30 +68,42 @@ struct contec_private {
 
 #define devpriv ((struct contec_private *)dev->private)
 
-static int contec_attach(struct comedi_device *dev,
-			 struct comedi_devconfig *it);
-static int contec_detach(struct comedi_device *dev);
-static struct comedi_driver driver_contec = {
-	.driver_name = "contec_pci_dio",
-	.module = THIS_MODULE,
-	.attach = contec_attach,
-	.detach = contec_detach,
-};
+static int contec_do_insn_bits(struct comedi_device *dev,
+			       struct comedi_subdevice *s,
+			       struct comedi_insn *insn, unsigned int *data)
+{
+
+	dev_dbg(dev->hw_dev, "contec_do_insn_bits called\n");
+	dev_dbg(dev->hw_dev, "data: %d %d\n", data[0], data[1]);
+
+	if (insn->n != 2)
+		return -EINVAL;
+
+	if (data[0]) {
+		s->state &= ~data[0];
+		s->state |= data[0] & data[1];
+		dev_dbg(dev->hw_dev, "out: %d on %lx\n", s->state,
+			dev->iobase + thisboard->out_offs);
+		outw(s->state, dev->iobase + thisboard->out_offs);
+	}
+	return 2;
+}
 
-/* Classic digital IO */
 static int contec_di_insn_bits(struct comedi_device *dev,
 			       struct comedi_subdevice *s,
-			       struct comedi_insn *insn, unsigned int *data);
-static int contec_do_insn_bits(struct comedi_device *dev,
-			       struct comedi_subdevice *s,
-			       struct comedi_insn *insn, unsigned int *data);
+			       struct comedi_insn *insn, unsigned int *data)
+{
+
+	dev_dbg(dev->hw_dev, "contec_di_insn_bits called\n");
+	dev_dbg(dev->hw_dev, "data: %d %d\n", data[0], data[1]);
 
-#if 0
-static int contec_cmdtest(struct comedi_device *dev, struct comedi_subdevice *s,
-			  struct comedi_cmd *cmd);
+	if (insn->n != 2)
+		return -EINVAL;
 
-static int contec_ns_to_timer(unsigned int *ns, int round);
-#endif
+	data[1] = inw(dev->iobase + thisboard->in_offs);
+
+	return 2;
+}
 
 static int contec_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 {
@@ -164,107 +169,47 @@ static int contec_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 	return -EIO;
 }
 
-static int contec_detach(struct comedi_device *dev)
+static void contec_detach(struct comedi_device *dev)
 {
-	printk("comedi%d: contec: remove\n", dev->minor);
-
 	if (devpriv && devpriv->pci_dev) {
 		if (dev->iobase)
 			comedi_pci_disable(devpriv->pci_dev);
 		pci_dev_put(devpriv->pci_dev);
 	}
-
-	return 0;
-}
-
-#if 0
-static int contec_cmdtest(struct comedi_device *dev, struct comedi_subdevice *s,
-			  struct comedi_cmd *cmd)
-{
-	printk("contec_cmdtest called\n");
-	return 0;
-}
-
-static int contec_ns_to_timer(unsigned int *ns, int round)
-{
-	return *ns;
-}
-#endif
-
-static int contec_do_insn_bits(struct comedi_device *dev,
-			       struct comedi_subdevice *s,
-			       struct comedi_insn *insn, unsigned int *data)
-{
-
-	dev_dbg(dev->hw_dev, "contec_do_insn_bits called\n");
-	dev_dbg(dev->hw_dev, "data: %d %d\n", data[0], data[1]);
-
-	if (insn->n != 2)
-		return -EINVAL;
-
-	if (data[0]) {
-		s->state &= ~data[0];
-		s->state |= data[0] & data[1];
-		dev_dbg(dev->hw_dev, "out: %d on %lx\n", s->state,
-			dev->iobase + thisboard->out_offs);
-		outw(s->state, dev->iobase + thisboard->out_offs);
-	}
-	return 2;
 }
 
-static int contec_di_insn_bits(struct comedi_device *dev,
-			       struct comedi_subdevice *s,
-			       struct comedi_insn *insn, unsigned int *data)
-{
-
-	dev_dbg(dev->hw_dev, "contec_di_insn_bits called\n");
-	dev_dbg(dev->hw_dev, "data: %d %d\n", data[0], data[1]);
-
-	if (insn->n != 2)
-		return -EINVAL;
-
-	data[1] = inw(dev->iobase + thisboard->in_offs);
-
-	return 2;
-}
+static struct comedi_driver contec_pci_dio_driver = {
+	.driver_name	= "contec_pci_dio",
+	.module		= THIS_MODULE,
+	.attach		= contec_attach,
+	.detach		= contec_detach,
+};
 
-static int __devinit driver_contec_pci_probe(struct pci_dev *dev,
-					     const struct pci_device_id *ent)
+static int __devinit contec_pci_dio_pci_probe(struct pci_dev *dev,
+					      const struct pci_device_id *ent)
 {
-	return comedi_pci_auto_config(dev, driver_contec.driver_name);
+	return comedi_pci_auto_config(dev, &contec_pci_dio_driver);
 }
 
-static void __devexit driver_contec_pci_remove(struct pci_dev *dev)
+static void __devexit contec_pci_dio_pci_remove(struct pci_dev *dev)
 {
 	comedi_pci_auto_unconfig(dev);
 }
 
-static struct pci_driver driver_contec_pci_driver = {
-	.id_table = contec_pci_table,
-	.probe = &driver_contec_pci_probe,
-	.remove = __devexit_p(&driver_contec_pci_remove)
+static DEFINE_PCI_DEVICE_TABLE(contec_pci_dio_pci_table) = {
+	{ PCI_DEVICE(PCI_VENDOR_ID_CONTEC, PCI_DEVICE_ID_PIO1616L),
+		.driver_data = PIO1616L },
+	{ 0 }
 };
+MODULE_DEVICE_TABLE(pci, contec_pci_dio_pci_table);
 
-static int __init driver_contec_init_module(void)
-{
-	int retval;
-
-	retval = comedi_driver_register(&driver_contec);
-	if (retval < 0)
-		return retval;
-
-	driver_contec_pci_driver.name = (char *)driver_contec.driver_name;
-	return pci_register_driver(&driver_contec_pci_driver);
-}
-
-static void __exit driver_contec_cleanup_module(void)
-{
-	pci_unregister_driver(&driver_contec_pci_driver);
-	comedi_driver_unregister(&driver_contec);
-}
-
-module_init(driver_contec_init_module);
-module_exit(driver_contec_cleanup_module);
+static struct pci_driver contec_pci_dio_pci_driver = {
+	.name		= "contec_pci_dio",
+	.id_table	= contec_pci_dio_pci_table,
+	.probe		= contec_pci_dio_pci_probe,
+	.remove		= __devexit_p(contec_pci_dio_pci_remove),
+};
+module_comedi_pci_driver(contec_pci_dio_driver, contec_pci_dio_pci_driver);
 
 MODULE_AUTHOR("Comedi http://www.comedi.org");
 MODULE_DESCRIPTION("Comedi low-level driver");
diff --git a/drivers/staging/comedi/drivers/daqboard2000.c b/drivers/staging/comedi/drivers/daqboard2000.c
index e61c6a8f285..696b58ca2e5 100644
--- a/drivers/staging/comedi/drivers/daqboard2000.c
+++ b/drivers/staging/comedi/drivers/daqboard2000.c
@@ -301,17 +301,6 @@ struct daqboard2000_hw {
 #define DAQBOARD2000_PosRefDacSelect             0x0100
 #define DAQBOARD2000_NegRefDacSelect             0x0000
 
-static int daqboard2000_attach(struct comedi_device *dev,
-			       struct comedi_devconfig *it);
-static int daqboard2000_detach(struct comedi_device *dev);
-
-static struct comedi_driver driver_daqboard2000 = {
-	.driver_name = "daqboard2000",
-	.module = THIS_MODULE,
-	.attach = daqboard2000_attach,
-	.detach = daqboard2000_detach,
-};
-
 struct daq200_boardtype {
 	const char *name;
 	int id;
@@ -321,16 +310,8 @@ static const struct daq200_boardtype boardtypes[] = {
 	{"ids4", DAQBOARD2000_SUBSYSTEM_IDS4},
 };
 
-#define n_boardtypes (sizeof(boardtypes)/sizeof(struct daq200_boardtype))
 #define this_board ((const struct daq200_boardtype *)dev->board_ptr)
 
-static DEFINE_PCI_DEVICE_TABLE(daqboard2000_pci_table) = {
-	{ PCI_DEVICE(0x1616, 0x0409) },
-	{0}
-};
-
-MODULE_DEVICE_TABLE(pci, daqboard2000_pci_table);
-
 struct daqboard2000_private {
 	enum {
 		card_daqboard_2000
@@ -412,9 +393,12 @@ static int daqboard2000_ai_insn_read(struct comedi_device *dev,
 	    DAQBOARD2000_AcqResetScanListFifo |
 	    DAQBOARD2000_AcqResetResultsFifo | DAQBOARD2000_AcqResetConfigPipe;
 
-	/* If pacer clock is not set to some high value (> 10 us), we
-	   risk multiple samples to be put into the result FIFO. */
-	fpga->acqPacerClockDivLow = 1000000;	/* 1 second, should be long enough */
+	/*
+	 * If pacer clock is not set to some high value (> 10 us), we
+	 * risk multiple samples to be put into the result FIFO.
+	 */
+	/* 1 second, should be long enough */
+	fpga->acqPacerClockDivLow = 1000000;
 	fpga->acqPacerClockDivHigh = 0;
 
 	gain = CR_RANGE(insn->chanspec);
@@ -761,7 +745,7 @@ static int daqboard2000_attach(struct comedi_device *dev,
 		devpriv->pci_dev = card;
 		id = ((u32) card->
 		      subsystem_device << 16) | card->subsystem_vendor;
-		for (i = 0; i < n_boardtypes; i++) {
+		for (i = 0; i < ARRAY_SIZE(boardtypes); i++) {
 			if (boardtypes[i].id == id) {
 				dev_dbg(dev->hw_dev, "%s\n",
 					boardtypes[i].name);
@@ -852,14 +836,12 @@ out:
 	return result;
 }
 
-static int daqboard2000_detach(struct comedi_device *dev)
+static void daqboard2000_detach(struct comedi_device *dev)
 {
 	if (dev->subdevices)
 		subdev_8255_cleanup(dev, dev->subdevices + 2);
-
 	if (dev->irq)
 		free_irq(dev->irq, dev);
-
 	if (devpriv) {
 		if (devpriv->daq)
 			iounmap(devpriv->daq);
@@ -871,48 +853,39 @@ static int daqboard2000_detach(struct comedi_device *dev)
 			pci_dev_put(devpriv->pci_dev);
 		}
 	}
-	return 0;
 }
 
-static int __devinit driver_daqboard2000_pci_probe(struct pci_dev *dev,
-						   const struct pci_device_id
-						   *ent)
+static struct comedi_driver daqboard2000_driver = {
+	.driver_name	= "daqboard2000",
+	.module		= THIS_MODULE,
+	.attach		= daqboard2000_attach,
+	.detach		= daqboard2000_detach,
+};
+
+static int __devinit daqboard2000_pci_probe(struct pci_dev *dev,
+					    const struct pci_device_id *ent)
 {
-	return comedi_pci_auto_config(dev, driver_daqboard2000.driver_name);
+	return comedi_pci_auto_config(dev, &daqboard2000_driver);
 }
 
-static void __devexit driver_daqboard2000_pci_remove(struct pci_dev *dev)
+static void __devexit daqboard2000_pci_remove(struct pci_dev *dev)
 {
 	comedi_pci_auto_unconfig(dev);
 }
 
-static struct pci_driver driver_daqboard2000_pci_driver = {
-	.id_table = daqboard2000_pci_table,
-	.probe = &driver_daqboard2000_pci_probe,
-	.remove = __devexit_p(&driver_daqboard2000_pci_remove)
+static DEFINE_PCI_DEVICE_TABLE(daqboard2000_pci_table) = {
+	{ PCI_DEVICE(0x1616, 0x0409) },
+	{ 0 }
 };
+MODULE_DEVICE_TABLE(pci, daqboard2000_pci_table);
 
-static int __init driver_daqboard2000_init_module(void)
-{
-	int retval;
-
-	retval = comedi_driver_register(&driver_daqboard2000);
-	if (retval < 0)
-		return retval;
-
-	driver_daqboard2000_pci_driver.name =
-	    (char *)driver_daqboard2000.driver_name;
-	return pci_register_driver(&driver_daqboard2000_pci_driver);
-}
-
-static void __exit driver_daqboard2000_cleanup_module(void)
-{
-	pci_unregister_driver(&driver_daqboard2000_pci_driver);
-	comedi_driver_unregister(&driver_daqboard2000);
-}
-
-module_init(driver_daqboard2000_init_module);
-module_exit(driver_daqboard2000_cleanup_module);
+static struct pci_driver daqboard2000_pci_driver = {
+	.name		= "daqboard2000",
+	.id_table	= daqboard2000_pci_table,
+	.probe		= daqboard2000_pci_probe,
+	.remove		= __devexit_p(daqboard2000_pci_remove),
+};
+module_comedi_pci_driver(daqboard2000_driver, daqboard2000_pci_driver);
 
 MODULE_AUTHOR("Comedi http://www.comedi.org");
 MODULE_DESCRIPTION("Comedi low-level driver");
diff --git a/drivers/staging/comedi/drivers/das08.c b/drivers/staging/comedi/drivers/das08.c
index c2dd0ed36a7..1f319435e23 100644
--- a/drivers/staging/comedi/drivers/das08.c
+++ b/drivers/staging/comedi/drivers/das08.c
@@ -61,6 +61,20 @@
 
 #define DRV_NAME "das08"
 
+#ifdef CONFIG_COMEDI_DAS08_ISA_MODULE
+#define CONFIG_COMEDI_DAS08_ISA
+#endif
+#ifdef CONFIG_COMEDI_DAS08_PCI_MODULE
+#define CONFIG_COMEDI_DAS08_PCI
+#endif
+#ifdef CONFIG_COMEDI_DAS08_CS_MODULE
+#define CONFIG_COMEDI_DAS08_CS
+#endif
+
+#if defined(CONFIG_COMEDI_DAS08_ISA) || defined(CONFIG_COMEDI_DAS08_PCI)
+#define DO_COMEDI_DRIVER_REGISTER
+#endif
+
 #define PCI_VENDOR_ID_COMPUTERBOARDS 0x1307
 #define PCI_DEVICE_ID_PCIDAS08 0x29
 #define PCIDAS08_SIZE 0x54
@@ -160,6 +174,7 @@ static int das08_di_rbits(struct comedi_device *dev, struct comedi_subdevice *s,
 			  struct comedi_insn *insn, unsigned int *data);
 static int das08_do_wbits(struct comedi_device *dev, struct comedi_subdevice *s,
 			  struct comedi_insn *insn, unsigned int *data);
+#ifdef CONFIG_COMEDI_DAS08_ISA
 static int das08jr_di_rbits(struct comedi_device *dev,
 			    struct comedi_subdevice *s,
 			    struct comedi_insn *insn, unsigned int *data);
@@ -172,6 +187,7 @@ static int das08jr_ao_winsn(struct comedi_device *dev,
 static int das08ao_ao_winsn(struct comedi_device *dev,
 			    struct comedi_subdevice *s,
 			    struct comedi_insn *insn, unsigned int *data);
+#endif
 static void i8254_set_mode_low(unsigned int base, int channel,
 			       unsigned int mode);
 
@@ -253,7 +269,9 @@ static const int *const das08_gainlists[] = {
 	das08_pgm_gainlist,
 };
 
+#ifdef DO_COMEDI_DRIVER_REGISTER
 static const struct das08_board_struct das08_boards[] = {
+#ifdef CONFIG_COMEDI_DAS08_ISA
 	{
 	 .name = "isa-das08",	/*  cio-das08.pdf */
 	 .bustype = isa,
@@ -395,25 +413,6 @@ static const struct das08_board_struct das08_boards[] = {
 	 .i8254_offset = 0x04,
 	 .iosize = 16,		/*  unchecked */
 	 },
-#ifdef CONFIG_COMEDI_PCI
-	{
-	 .name = "das08",	/*  pci-das08 */
-	 .id = PCI_DEVICE_ID_PCIDAS08,
-	 .bustype = pci,
-	 .ai = das08_ai_rinsn,
-	 .ai_nbits = 12,
-	 .ai_pg = das08_bipolar5,
-	 .ai_encoding = das08_encode12,
-	 .ao = NULL,
-	 .ao_nbits = 0,
-	 .di = das08_di_rbits,
-	 .do_ = das08_do_wbits,
-	 .do_nchan = 4,
-	 .i8255_offset = 0,
-	 .i8254_offset = 4,
-	 .iosize = 8,
-	 },
-#endif
 	{
 	 .name = "pc104-das08",
 	 .bustype = pc104,
@@ -462,9 +461,30 @@ static const struct das08_board_struct das08_boards[] = {
 	 .name = "das08-pga-g2",	/*  a KM board */
 	 },
 #endif
+#endif /* CONFIG_COMEDI_DAS08_ISA */
+#ifdef CONFIG_COMEDI_DAS08_PCI
+	{
+	 .name = "das08",	/*  pci-das08 */
+	 .id = PCI_DEVICE_ID_PCIDAS08,
+	 .bustype = pci,
+	 .ai = das08_ai_rinsn,
+	 .ai_nbits = 12,
+	 .ai_pg = das08_bipolar5,
+	 .ai_encoding = das08_encode12,
+	 .ao = NULL,
+	 .ao_nbits = 0,
+	 .di = das08_di_rbits,
+	 .do_ = das08_do_wbits,
+	 .do_nchan = 4,
+	 .i8255_offset = 0,
+	 .i8254_offset = 4,
+	 .iosize = 8,
+	 },
+#endif /* CONFIG_COMEDI_DAS08_PCI */
 };
+#endif /* DO_COMEDI_DRIVER_REGISTER */
 
-#ifdef CONFIG_COMEDI_PCMCIA
+#ifdef CONFIG_COMEDI_DAS08_CS
 struct das08_board_struct das08_cs_boards[NUM_DAS08_CS_BOARDS] = {
 	{
 	 .name = "pcm-das08",
@@ -504,7 +524,7 @@ struct das08_board_struct das08_cs_boards[NUM_DAS08_CS_BOARDS] = {
 };
 #endif
 
-#ifdef CONFIG_COMEDI_PCI
+#ifdef CONFIG_COMEDI_DAS08_PCI
 static DEFINE_PCI_DEVICE_TABLE(das08_pci_table) = {
 	{ PCI_DEVICE(PCI_VENDOR_ID_COMPUTERBOARDS, PCI_DEVICE_ID_PCIDAS08) },
 	{0}
@@ -619,6 +639,7 @@ static int das08_do_wbits(struct comedi_device *dev, struct comedi_subdevice *s,
 	return 2;
 }
 
+#ifdef CONFIG_COMEDI_DAS08_ISA
 static int das08jr_di_rbits(struct comedi_device *dev,
 			    struct comedi_subdevice *s,
 			    struct comedi_insn *insn, unsigned int *data)
@@ -628,7 +649,9 @@ static int das08jr_di_rbits(struct comedi_device *dev,
 
 	return 2;
 }
+#endif
 
+#ifdef CONFIG_COMEDI_DAS08_ISA
 static int das08jr_do_wbits(struct comedi_device *dev,
 			    struct comedi_subdevice *s,
 			    struct comedi_insn *insn, unsigned int *data)
@@ -643,7 +666,9 @@ static int das08jr_do_wbits(struct comedi_device *dev,
 
 	return 2;
 }
+#endif
 
+#ifdef CONFIG_COMEDI_DAS08_ISA
 static int das08jr_ao_winsn(struct comedi_device *dev,
 			    struct comedi_subdevice *s,
 			    struct comedi_insn *insn, unsigned int *data)
@@ -672,6 +697,7 @@ static int das08jr_ao_winsn(struct comedi_device *dev,
 
 	return n;
 }
+#endif
 
 /*
  *
@@ -679,6 +705,7 @@ static int das08jr_ao_winsn(struct comedi_device *dev,
  * a different method to force an update.
  *
  */
+#ifdef CONFIG_COMEDI_DAS08_ISA
 static int das08ao_ao_winsn(struct comedi_device *dev,
 			    struct comedi_subdevice *s,
 			    struct comedi_insn *insn, unsigned int *data)
@@ -707,6 +734,7 @@ static int das08ao_ao_winsn(struct comedi_device *dev,
 
 	return n;
 }
+#endif
 
 static unsigned int i8254_read_channel_low(unsigned int base, int chan)
 {
@@ -842,6 +870,7 @@ static int das08_counter_config(struct comedi_device *dev,
 	return 2;
 }
 
+#ifdef DO_COMEDI_DRIVER_REGISTER
 static int das08_attach(struct comedi_device *dev, struct comedi_devconfig *it);
 
 static struct comedi_driver driver_das08 = {
@@ -853,6 +882,7 @@ static struct comedi_driver driver_das08 = {
 	.num_names = sizeof(das08_boards) / sizeof(struct das08_board_struct),
 	.offset = sizeof(struct das08_board_struct),
 };
+#endif
 
 int das08_common_attach(struct comedi_device *dev, unsigned long iobase)
 {
@@ -972,11 +1002,12 @@ int das08_common_attach(struct comedi_device *dev, unsigned long iobase)
 }
 EXPORT_SYMBOL_GPL(das08_common_attach);
 
+#ifdef DO_COMEDI_DRIVER_REGISTER
 static int das08_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 {
 	int ret;
 	unsigned long iobase;
-#ifdef CONFIG_COMEDI_PCI
+#ifdef CONFIG_COMEDI_DAS08_PCI
 	unsigned long pci_iobase = 0;
 	struct pci_dev *pdev = NULL;
 #endif
@@ -986,9 +1017,9 @@ static int das08_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 		return ret;
 
 	printk(KERN_INFO "comedi%d: das08: ", dev->minor);
+#ifdef CONFIG_COMEDI_DAS08_PCI
 	/*  deal with a pci board */
 	if (thisboard->bustype == pci) {
-#ifdef CONFIG_COMEDI_PCI
 		if (it->options[0] || it->options[1]) {
 			printk("bus %i slot %i ",
 			       it->options[0], it->options[1]);
@@ -1037,32 +1068,26 @@ static int das08_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 		/* Enable local interrupt 1 and pci interrupt */
 		outw(INTR1_ENABLE | PCI_INTR_ENABLE, pci_iobase + INTCSR);
 #endif
-#else /* CONFIG_COMEDI_PCI */
-		printk(KERN_ERR "this driver has not been built with PCI support.\n");
-		return -EINVAL;
-#endif /* CONFIG_COMEDI_PCI */
-	} else {
+	} else
+#endif /* CONFIG_COMEDI_DAS08_PCI */
+	{
 		iobase = it->options[0];
 	}
 	printk(KERN_INFO "\n");
 
 	return das08_common_attach(dev, iobase);
 }
+#endif /* DO_COMEDI_DRIVER_REGISTER */
 
-
-int das08_common_detach(struct comedi_device *dev)
+void das08_common_detach(struct comedi_device *dev)
 {
-	printk(KERN_INFO "comedi%d: das08: remove\n", dev->minor);
-
 	if (dev->subdevices)
 		subdev_8255_cleanup(dev, dev->subdevices + 4);
-
-	/*  deallocate ioports for non-pcmcia, non-pci boards */
 	if ((thisboard->bustype != pcmcia) && (thisboard->bustype != pci)) {
 		if (dev->iobase)
 			release_region(dev->iobase, thisboard->iosize);
 	}
-#ifdef CONFIG_COMEDI_PCI
+#ifdef CONFIG_COMEDI_DAS08_PCI
 	if (devpriv) {
 		if (devpriv->pdev) {
 			if (devpriv->pci_iobase)
@@ -1072,16 +1097,14 @@ int das08_common_detach(struct comedi_device *dev)
 		}
 	}
 #endif
-
-	return 0;
 }
 EXPORT_SYMBOL_GPL(das08_common_detach);
 
-#ifdef CONFIG_COMEDI_PCI
+#ifdef CONFIG_COMEDI_DAS08_PCI
 static int __devinit driver_das08_pci_probe(struct pci_dev *dev,
 					    const struct pci_device_id *ent)
 {
-	return comedi_pci_auto_config(dev, driver_das08.driver_name);
+	return comedi_pci_auto_config(dev, &driver_das08);
 }
 
 static void __devexit driver_das08_pci_remove(struct pci_dev *dev)
@@ -1094,43 +1117,38 @@ static struct pci_driver driver_das08_pci_driver = {
 	.probe = &driver_das08_pci_probe,
 	.remove = __devexit_p(&driver_das08_pci_remove)
 };
+#endif /* CONFIG_COMEDI_DAS08_PCI */
 
 static int __init driver_das08_init_module(void)
 {
-	int retval;
+	int retval = 0;
 
+#ifdef DO_COMEDI_DRIVER_REGISTER
 	retval = comedi_driver_register(&driver_das08);
 	if (retval < 0)
 		return retval;
-
+#endif
+#ifdef CONFIG_COMEDI_DAS08_PCI
 	driver_das08_pci_driver.name = (char *)driver_das08.driver_name;
-	return pci_register_driver(&driver_das08_pci_driver);
+	retval = pci_register_driver(&driver_das08_pci_driver);
+#endif
+	return retval;
 }
 
 static void __exit driver_das08_cleanup_module(void)
 {
+#ifdef CONFIG_COMEDI_DAS08_PCI
 	pci_unregister_driver(&driver_das08_pci_driver);
+#endif
+#ifdef DO_COMEDI_DRIVER_REGISTER
 	comedi_driver_unregister(&driver_das08);
+#endif
 }
 
 module_init(driver_das08_init_module);
 module_exit(driver_das08_cleanup_module);
-#else
-static int __init driver_das08_init_module(void)
-{
-	return comedi_driver_register(&driver_das08);
-}
-
-static void __exit driver_das08_cleanup_module(void)
-{
-	comedi_driver_unregister(&driver_das08);
-}
-
-module_init(driver_das08_init_module);
-module_exit(driver_das08_cleanup_module);
-#endif
 
-#ifdef CONFIG_COMEDI_PCMCIA
+#ifdef CONFIG_COMEDI_DAS08_CS
 EXPORT_SYMBOL_GPL(das08_cs_boards);
 #endif
 
diff --git a/drivers/staging/comedi/drivers/das08.h b/drivers/staging/comedi/drivers/das08.h
index 2a30d764ddf..0b92f24b261 100644
--- a/drivers/staging/comedi/drivers/das08.h
+++ b/drivers/staging/comedi/drivers/das08.h
@@ -74,6 +74,6 @@ struct das08_private_struct {
 extern struct das08_board_struct das08_cs_boards[NUM_DAS08_CS_BOARDS];
 
 int das08_common_attach(struct comedi_device *dev, unsigned long iobase);
-int das08_common_detach(struct comedi_device *dev);
+void das08_common_detach(struct comedi_device *dev);
 
 #endif /* _DAS08_H */
diff --git a/drivers/staging/comedi/drivers/das16.c b/drivers/staging/comedi/drivers/das16.c
index e7905bac92d..998444c1ba3 100644
--- a/drivers/staging/comedi/drivers/das16.c
+++ b/drivers/staging/comedi/drivers/das16.c
@@ -339,38 +339,6 @@ struct munge_info {
 	unsigned have_byte:1;
 };
 
-static int das16_ao_winsn(struct comedi_device *dev, struct comedi_subdevice *s,
-			  struct comedi_insn *insn, unsigned int *data);
-static int das16_do_wbits(struct comedi_device *dev, struct comedi_subdevice *s,
-			  struct comedi_insn *insn, unsigned int *data);
-static int das16_di_rbits(struct comedi_device *dev, struct comedi_subdevice *s,
-			  struct comedi_insn *insn, unsigned int *data);
-static int das16_ai_rinsn(struct comedi_device *dev, struct comedi_subdevice *s,
-			  struct comedi_insn *insn, unsigned int *data);
-
-static int das16_cmd_test(struct comedi_device *dev, struct comedi_subdevice *s,
-			  struct comedi_cmd *cmd);
-static int das16_cmd_exec(struct comedi_device *dev,
-			  struct comedi_subdevice *s);
-static int das16_cancel(struct comedi_device *dev, struct comedi_subdevice *s);
-static void das16_ai_munge(struct comedi_device *dev,
-			   struct comedi_subdevice *s, void *array,
-			   unsigned int num_bytes,
-			   unsigned int start_chan_index);
-
-static void das16_reset(struct comedi_device *dev);
-static irqreturn_t das16_dma_interrupt(int irq, void *d);
-static void das16_timer_interrupt(unsigned long arg);
-static void das16_interrupt(struct comedi_device *dev);
-
-static unsigned int das16_set_pacer(struct comedi_device *dev, unsigned int ns,
-				    int flags);
-static int das1600_mode_detect(struct comedi_device *dev);
-static unsigned int das16_suggest_transfer_size(struct comedi_device *dev,
-						struct comedi_cmd cmd);
-
-static void reg_dump(struct comedi_device *dev);
-
 struct das16_board {
 	const char *name;
 	void *ai;
@@ -389,344 +357,6 @@ struct das16_board {
 	unsigned int id;
 };
 
-static const struct das16_board das16_boards[] = {
-	{
-	 .name = "das-16",
-	 .ai = das16_ai_rinsn,
-	 .ai_nbits = 12,
-	 .ai_speed = 15000,
-	 .ai_pg = das16_pg_none,
-	 .ao = das16_ao_winsn,
-	 .ao_nbits = 12,
-	 .di = das16_di_rbits,
-	 .do_ = das16_do_wbits,
-	 .i8255_offset = 0x10,
-	 .i8254_offset = 0x0c,
-	 .size = 0x14,
-	 .id = 0x00,
-	 },
-	{
-	 .name = "das-16g",
-	 .ai = das16_ai_rinsn,
-	 .ai_nbits = 12,
-	 .ai_speed = 15000,
-	 .ai_pg = das16_pg_none,
-	 .ao = das16_ao_winsn,
-	 .ao_nbits = 12,
-	 .di = das16_di_rbits,
-	 .do_ = das16_do_wbits,
-	 .i8255_offset = 0x10,
-	 .i8254_offset = 0x0c,
-	 .size = 0x14,
-	 .id = 0x00,
-	 },
-	{
-	 .name = "das-16f",
-	 .ai = das16_ai_rinsn,
-	 .ai_nbits = 12,
-	 .ai_speed = 8500,
-	 .ai_pg = das16_pg_none,
-	 .ao = das16_ao_winsn,
-	 .ao_nbits = 12,
-	 .di = das16_di_rbits,
-	 .do_ = das16_do_wbits,
-	 .i8255_offset = 0x10,
-	 .i8254_offset = 0x0c,
-	 .size = 0x14,
-	 .id = 0x00,
-	 },
-	{
-	 .name = "cio-das16",	/*  cio-das16.pdf */
-	 .ai = das16_ai_rinsn,
-	 .ai_nbits = 12,
-	 .ai_speed = 20000,
-	 .ai_pg = das16_pg_none,
-	 .ao = das16_ao_winsn,
-	 .ao_nbits = 12,
-	 .di = das16_di_rbits,
-	 .do_ = das16_do_wbits,
-	 .i8255_offset = 0x10,
-	 .i8254_offset = 0x0c,
-	 .size = 0x14,
-	 .id = 0x80,
-	 },
-	{
-	 .name = "cio-das16/f",	/*  das16.pdf */
-	 .ai = das16_ai_rinsn,
-	 .ai_nbits = 12,
-	 .ai_speed = 10000,
-	 .ai_pg = das16_pg_none,
-	 .ao = das16_ao_winsn,
-	 .ao_nbits = 12,
-	 .di = das16_di_rbits,
-	 .do_ = das16_do_wbits,
-	 .i8255_offset = 0x10,
-	 .i8254_offset = 0x0c,
-	 .size = 0x14,
-	 .id = 0x80,
-	 },
-	{
-	 .name = "cio-das16/jr",	/*  cio-das16jr.pdf */
-	 .ai = das16_ai_rinsn,
-	 .ai_nbits = 12,
-	 .ai_speed = 7692,
-	 .ai_pg = das16_pg_16jr,
-	 .ao = NULL,
-	 .di = das16_di_rbits,
-	 .do_ = das16_do_wbits,
-	 .i8255_offset = 0,
-	 .i8254_offset = 0x0c,
-	 .size = 0x10,
-	 .id = 0x00,
-	 },
-	{
-	 .name = "pc104-das16jr",	/*  pc104-das16jr_xx.pdf */
-	 .ai = das16_ai_rinsn,
-	 .ai_nbits = 12,
-	 .ai_speed = 3300,
-	 .ai_pg = das16_pg_16jr,
-	 .ao = NULL,
-	 .di = das16_di_rbits,
-	 .do_ = das16_do_wbits,
-	 .i8255_offset = 0,
-	 .i8254_offset = 0x0c,
-	 .size = 0x10,
-	 .id = 0x00,
-	 },
-	{
-	 .name = "cio-das16jr/16",	/*  cio-das16jr_16.pdf */
-	 .ai = das16_ai_rinsn,
-	 .ai_nbits = 16,
-	 .ai_speed = 10000,
-	 .ai_pg = das16_pg_16jr_16,
-	 .ao = NULL,
-	 .di = das16_di_rbits,
-	 .do_ = das16_do_wbits,
-	 .i8255_offset = 0,
-	 .i8254_offset = 0x0c,
-	 .size = 0x10,
-	 .id = 0x00,
-	 },
-	{
-	 .name = "pc104-das16jr/16",	/*  pc104-das16jr_xx.pdf */
-	 .ai = das16_ai_rinsn,
-	 .ai_nbits = 16,
-	 .ai_speed = 10000,
-	 .ai_pg = das16_pg_16jr_16,
-	 .ao = NULL,
-	 .di = das16_di_rbits,
-	 .do_ = das16_do_wbits,
-	 .i8255_offset = 0,
-	 .i8254_offset = 0x0c,
-	 .size = 0x10,
-	 .id = 0x00,
-	 },
-	{
-	 .name = "das-1201",	/*  4924.pdf (keithley user's manual) */
-	 .ai = das16_ai_rinsn,
-	 .ai_nbits = 12,
-	 .ai_speed = 20000,
-	 .ai_pg = das16_pg_none,
-	 .ao = NULL,
-	 .di = das16_di_rbits,
-	 .do_ = das16_do_wbits,
-	 .i8255_offset = 0x400,
-	 .i8254_offset = 0x0c,
-	 .size = 0x408,
-	 .id = 0x20,
-	 },
-	{
-	 .name = "das-1202",	/*  4924.pdf (keithley user's manual) */
-	 .ai = das16_ai_rinsn,
-	 .ai_nbits = 12,
-	 .ai_speed = 10000,
-	 .ai_pg = das16_pg_none,
-	 .ao = NULL,
-	 .di = das16_di_rbits,
-	 .do_ = das16_do_wbits,
-	 .i8255_offset = 0x400,
-	 .i8254_offset = 0x0c,
-	 .size = 0x408,
-	 .id = 0x20,
-	 },
-	{
-	/*  4919.pdf and 4922.pdf (keithley user's manual) */
-	 .name = "das-1401",
-	 .ai = das16_ai_rinsn,
-	 .ai_nbits = 12,
-	 .ai_speed = 10000,
-	 .ai_pg = das16_pg_1601,
-	 .ao = NULL,
-	 .di = das16_di_rbits,
-	 .do_ = das16_do_wbits,
-	 .i8255_offset = 0x0,
-	 .i8254_offset = 0x0c,
-	 .size = 0x408,
-	 .id = 0xc0   /*  4919.pdf says id bits are 0xe0, 4922.pdf says 0xc0 */
-	 },
-	{
-	/*  4919.pdf and 4922.pdf (keithley user's manual) */
-	 .name = "das-1402",
-	 .ai = das16_ai_rinsn,
-	 .ai_nbits = 12,
-	 .ai_speed = 10000,
-	 .ai_pg = das16_pg_1602,
-	 .ao = NULL,
-	 .di = das16_di_rbits,
-	 .do_ = das16_do_wbits,
-	 .i8255_offset = 0x0,
-	 .i8254_offset = 0x0c,
-	 .size = 0x408,
-	 .id = 0xc0   /*  4919.pdf says id bits are 0xe0, 4922.pdf says 0xc0 */
-	 },
-	{
-	 .name = "das-1601",	/*  4919.pdf */
-	 .ai = das16_ai_rinsn,
-	 .ai_nbits = 12,
-	 .ai_speed = 10000,
-	 .ai_pg = das16_pg_1601,
-	 .ao = das16_ao_winsn,
-	 .ao_nbits = 12,
-	 .di = das16_di_rbits,
-	 .do_ = das16_do_wbits,
-	 .i8255_offset = 0x400,
-	 .i8254_offset = 0x0c,
-	 .size = 0x408,
-	 .id = 0xc0},
-	{
-	 .name = "das-1602",	/*  4919.pdf */
-	 .ai = das16_ai_rinsn,
-	 .ai_nbits = 12,
-	 .ai_speed = 10000,
-	 .ai_pg = das16_pg_1602,
-	 .ao = das16_ao_winsn,
-	 .ao_nbits = 12,
-	 .di = das16_di_rbits,
-	 .do_ = das16_do_wbits,
-	 .i8255_offset = 0x400,
-	 .i8254_offset = 0x0c,
-	 .size = 0x408,
-	 .id = 0xc0},
-	{
-	 .name = "cio-das1401/12",	/*  cio-das1400_series.pdf */
-	 .ai = das16_ai_rinsn,
-	 .ai_nbits = 12,
-	 .ai_speed = 6250,
-	 .ai_pg = das16_pg_1601,
-	 .ao = NULL,
-	 .di = das16_di_rbits,
-	 .do_ = das16_do_wbits,
-	 .i8255_offset = 0,
-	 .i8254_offset = 0x0c,
-	 .size = 0x408,
-	 .id = 0xc0},
-	{
-	 .name = "cio-das1402/12",	/*  cio-das1400_series.pdf */
-	 .ai = das16_ai_rinsn,
-	 .ai_nbits = 12,
-	 .ai_speed = 6250,
-	 .ai_pg = das16_pg_1602,
-	 .ao = NULL,
-	 .di = das16_di_rbits,
-	 .do_ = das16_do_wbits,
-	 .i8255_offset = 0,
-	 .i8254_offset = 0x0c,
-	 .size = 0x408,
-	 .id = 0xc0},
-	{
-	 .name = "cio-das1402/16",	/*  cio-das1400_series.pdf */
-	 .ai = das16_ai_rinsn,
-	 .ai_nbits = 16,
-	 .ai_speed = 10000,
-	 .ai_pg = das16_pg_1602,
-	 .ao = NULL,
-	 .di = das16_di_rbits,
-	 .do_ = das16_do_wbits,
-	 .i8255_offset = 0,
-	 .i8254_offset = 0x0c,
-	 .size = 0x408,
-	 .id = 0xc0},
-	{
-	 .name = "cio-das1601/12",	/*  cio-das160x-1x.pdf */
-	 .ai = das16_ai_rinsn,
-	 .ai_nbits = 12,
-	 .ai_speed = 6250,
-	 .ai_pg = das16_pg_1601,
-	 .ao = das16_ao_winsn,
-	 .ao_nbits = 12,
-	 .di = das16_di_rbits,
-	 .do_ = das16_do_wbits,
-	 .i8255_offset = 0x400,
-	 .i8254_offset = 0x0c,
-	 .size = 0x408,
-	 .id = 0xc0},
-	{
-	 .name = "cio-das1602/12",	/*  cio-das160x-1x.pdf */
-	 .ai = das16_ai_rinsn,
-	 .ai_nbits = 12,
-	 .ai_speed = 10000,
-	 .ai_pg = das16_pg_1602,
-	 .ao = das16_ao_winsn,
-	 .ao_nbits = 12,
-	 .di = das16_di_rbits,
-	 .do_ = das16_do_wbits,
-	 .i8255_offset = 0x400,
-	 .i8254_offset = 0x0c,
-	 .size = 0x408,
-	 .id = 0xc0},
-	{
-	 .name = "cio-das1602/16",	/*  cio-das160x-1x.pdf */
-	 .ai = das16_ai_rinsn,
-	 .ai_nbits = 16,
-	 .ai_speed = 10000,
-	 .ai_pg = das16_pg_1602,
-	 .ao = das16_ao_winsn,
-	 .ao_nbits = 12,
-	 .di = das16_di_rbits,
-	 .do_ = das16_do_wbits,
-	 .i8255_offset = 0x400,
-	 .i8254_offset = 0x0c,
-	 .size = 0x408,
-	 .id = 0xc0},
-	{
-	 .name = "cio-das16/330",	/*  ? */
-	 .ai = das16_ai_rinsn,
-	 .ai_nbits = 12,
-	 .ai_speed = 3030,
-	 .ai_pg = das16_pg_16jr,
-	 .ao = NULL,
-	 .di = das16_di_rbits,
-	 .do_ = das16_do_wbits,
-	 .i8255_offset = 0,
-	 .i8254_offset = 0x0c,
-	 .size = 0x14,
-	 .id = 0xf0},
-#if 0
-	{
-	 .name = "das16/330i",	/*  ? */
-	 },
-	{
-	 .name = "das16/jr/ctr5",	/*  ? */
-	 },
-	{
-	/*  cio-das16_m1_16.pdf, this board is a bit quirky, no dma */
-	 .name = "cio-das16/m1/16",
-	 },
-#endif
-};
-
-static int das16_attach(struct comedi_device *dev, struct comedi_devconfig *it);
-static int das16_detach(struct comedi_device *dev);
-static struct comedi_driver driver_das16 = {
-	.driver_name = "das16",
-	.module = THIS_MODULE,
-	.attach = das16_attach,
-	.detach = das16_detach,
-	.board_name = &das16_boards[0].name,
-	.num_names = ARRAY_SIZE(das16_boards),
-	.offset = sizeof(das16_boards[0]),
-};
-
 #define DAS16_TIMEOUT 1000
 
 /* Period for timer interrupt in jiffies.  It's a function
@@ -926,6 +556,62 @@ static int das16_cmd_test(struct comedi_device *dev, struct comedi_subdevice *s,
 	return 0;
 }
 
+/* utility function that suggests a dma transfer size in bytes */
+static unsigned int das16_suggest_transfer_size(struct comedi_device *dev,
+						struct comedi_cmd cmd)
+{
+	unsigned int size;
+	unsigned int freq;
+
+	/* if we are using timer interrupt, we don't care how long it
+	 * will take to complete transfer since it will be interrupted
+	 * by timer interrupt */
+	if (devpriv->timer_mode)
+		return DAS16_DMA_SIZE;
+
+	/* otherwise, we are relying on dma terminal count interrupt,
+	 * so pick a reasonable size */
+	if (cmd.convert_src == TRIG_TIMER)
+		freq = 1000000000 / cmd.convert_arg;
+	else if (cmd.scan_begin_src == TRIG_TIMER)
+		freq = (1000000000 / cmd.scan_begin_arg) * cmd.chanlist_len;
+	/*  return some default value */
+	else
+		freq = 0xffffffff;
+
+	if (cmd.flags & TRIG_WAKE_EOS) {
+		size = sample_size * cmd.chanlist_len;
+	} else {
+		/*  make buffer fill in no more than 1/3 second */
+		size = (freq / 3) * sample_size;
+	}
+
+	/*  set a minimum and maximum size allowed */
+	if (size > DAS16_DMA_SIZE)
+		size = DAS16_DMA_SIZE - DAS16_DMA_SIZE % sample_size;
+	else if (size < sample_size)
+		size = sample_size;
+
+	if (cmd.stop_src == TRIG_COUNT && size > devpriv->adc_byte_count)
+		size = devpriv->adc_byte_count;
+
+	return size;
+}
+
+static unsigned int das16_set_pacer(struct comedi_device *dev, unsigned int ns,
+				    int rounding_flags)
+{
+	i8253_cascade_ns_to_timer_2div(devpriv->clockbase, &(devpriv->divisor1),
+				       &(devpriv->divisor2), &ns,
+				       rounding_flags & TRIG_ROUND_MASK);
+
+	/* Write the values of ctr1 and ctr2 into counters 1 and 2 */
+	i8254_load(dev->iobase + DAS16_CNTR0_DATA, 0, 1, devpriv->divisor1, 2);
+	i8254_load(dev->iobase + DAS16_CNTR0_DATA, 0, 2, devpriv->divisor2, 2);
+
+	return ns;
+}
+
 static int das16_cmd_exec(struct comedi_device *dev, struct comedi_subdevice *s)
 {
 	struct comedi_async *async = s->async;
@@ -1170,34 +856,6 @@ static int das16_ao_winsn(struct comedi_device *dev, struct comedi_subdevice *s,
 	return i;
 }
 
-static irqreturn_t das16_dma_interrupt(int irq, void *d)
-{
-	int status;
-	struct comedi_device *dev = d;
-
-	status = inb(dev->iobase + DAS16_STATUS);
-
-	if ((status & DAS16_INT) == 0) {
-		DEBUG_PRINT("spurious interrupt\n");
-		return IRQ_NONE;
-	}
-
-	/* clear interrupt */
-	outb(0x00, dev->iobase + DAS16_STATUS);
-	das16_interrupt(dev);
-	return IRQ_HANDLED;
-}
-
-static void das16_timer_interrupt(unsigned long arg)
-{
-	struct comedi_device *dev = (struct comedi_device *)arg;
-
-	das16_interrupt(dev);
-
-	if (devpriv->timer_running)
-		mod_timer(&devpriv->timer, jiffies + timer_period());
-}
-
 /* the pc104-das16jr (at least) has problems if the dma
 	transfer is interrupted in the middle of transferring
 	a 16 bit sample, so this function takes care to get
@@ -1309,18 +967,32 @@ static void das16_interrupt(struct comedi_device *dev)
 	cfc_handle_events(dev, s);
 }
 
-static unsigned int das16_set_pacer(struct comedi_device *dev, unsigned int ns,
-				    int rounding_flags)
+static irqreturn_t das16_dma_interrupt(int irq, void *d)
 {
-	i8253_cascade_ns_to_timer_2div(devpriv->clockbase, &(devpriv->divisor1),
-				       &(devpriv->divisor2), &ns,
-				       rounding_flags & TRIG_ROUND_MASK);
+	int status;
+	struct comedi_device *dev = d;
 
-	/* Write the values of ctr1 and ctr2 into counters 1 and 2 */
-	i8254_load(dev->iobase + DAS16_CNTR0_DATA, 0, 1, devpriv->divisor1, 2);
-	i8254_load(dev->iobase + DAS16_CNTR0_DATA, 0, 2, devpriv->divisor2, 2);
+	status = inb(dev->iobase + DAS16_STATUS);
 
-	return ns;
+	if ((status & DAS16_INT) == 0) {
+		DEBUG_PRINT("spurious interrupt\n");
+		return IRQ_NONE;
+	}
+
+	/* clear interrupt */
+	outb(0x00, dev->iobase + DAS16_STATUS);
+	das16_interrupt(dev);
+	return IRQ_HANDLED;
+}
+
+static void das16_timer_interrupt(unsigned long arg)
+{
+	struct comedi_device *dev = (struct comedi_device *)arg;
+
+	das16_interrupt(dev);
+
+	if (devpriv->timer_running)
+		mod_timer(&devpriv->timer, jiffies + timer_period());
 }
 
 static void reg_dump(struct comedi_device *dev)
@@ -1394,6 +1066,22 @@ static int das1600_mode_detect(struct comedi_device *dev)
 	return 0;
 }
 
+static void das16_ai_munge(struct comedi_device *dev,
+			   struct comedi_subdevice *s, void *array,
+			   unsigned int num_bytes,
+			   unsigned int start_chan_index)
+{
+	unsigned int i, num_samples = num_bytes / sizeof(short);
+	short *data = array;
+
+	for (i = 0; i < num_samples; i++) {
+		data[i] = le16_to_cpu(data[i]);
+		if (thisboard->ai_nbits == 12)
+			data[i] = (data[i] >> 4) & 0xfff;
+
+	}
+}
+
 /*
  *
  * Options list:
@@ -1402,7 +1090,6 @@ static int das1600_mode_detect(struct comedi_device *dev)
  *   2  DMA
  *   3  Clock speed (in MHz)
  */
-
 static int das16_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 {
 	struct comedi_subdevice *s;
@@ -1675,15 +1362,11 @@ static int das16_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 	return 0;
 }
 
-static int das16_detach(struct comedi_device *dev)
+static void das16_detach(struct comedi_device *dev)
 {
-	printk(KERN_INFO "comedi%d: das16: remove\n", dev->minor);
-
 	das16_reset(dev);
-
 	if (dev->subdevices)
 		subdev_8255_cleanup(dev, dev->subdevices + 4);
-
 	if (devpriv) {
 		int i;
 		for (i = 0; i < 2; i++) {
@@ -1698,10 +1381,8 @@ static int das16_detach(struct comedi_device *dev)
 		kfree(devpriv->user_ai_range_table);
 		kfree(devpriv->user_ao_range_table);
 	}
-
 	if (dev->irq)
 		free_irq(dev->irq, dev);
-
 	if (dev->iobase) {
 		if (thisboard->size < 0x400) {
 			release_region(dev->iobase, thisboard->size);
@@ -1711,80 +1392,318 @@ static int das16_detach(struct comedi_device *dev)
 				       thisboard->size & 0x3ff);
 		}
 	}
-
-	return 0;
 }
 
-static int __init driver_das16_init_module(void)
-{
-	return comedi_driver_register(&driver_das16);
-}
-
-static void __exit driver_das16_cleanup_module(void)
-{
-	comedi_driver_unregister(&driver_das16);
-}
-
-module_init(driver_das16_init_module);
-module_exit(driver_das16_cleanup_module);
-
-/* utility function that suggests a dma transfer size in bytes */
-static unsigned int das16_suggest_transfer_size(struct comedi_device *dev,
-						struct comedi_cmd cmd)
-{
-	unsigned int size;
-	unsigned int freq;
-
-	/* if we are using timer interrupt, we don't care how long it
-	 * will take to complete transfer since it will be interrupted
-	 * by timer interrupt */
-	if (devpriv->timer_mode)
-		return DAS16_DMA_SIZE;
-
-	/* otherwise, we are relying on dma terminal count interrupt,
-	 * so pick a reasonable size */
-	if (cmd.convert_src == TRIG_TIMER)
-		freq = 1000000000 / cmd.convert_arg;
-	else if (cmd.scan_begin_src == TRIG_TIMER)
-		freq = (1000000000 / cmd.scan_begin_arg) * cmd.chanlist_len;
-	/*  return some default value */
-	else
-		freq = 0xffffffff;
-
-	if (cmd.flags & TRIG_WAKE_EOS) {
-		size = sample_size * cmd.chanlist_len;
-	} else {
-		/*  make buffer fill in no more than 1/3 second */
-		size = (freq / 3) * sample_size;
-	}
-
-	/*  set a minimum and maximum size allowed */
-	if (size > DAS16_DMA_SIZE)
-		size = DAS16_DMA_SIZE - DAS16_DMA_SIZE % sample_size;
-	else if (size < sample_size)
-		size = sample_size;
-
-	if (cmd.stop_src == TRIG_COUNT && size > devpriv->adc_byte_count)
-		size = devpriv->adc_byte_count;
-
-	return size;
-}
-
-static void das16_ai_munge(struct comedi_device *dev,
-			   struct comedi_subdevice *s, void *array,
-			   unsigned int num_bytes,
-			   unsigned int start_chan_index)
-{
-	unsigned int i, num_samples = num_bytes / sizeof(short);
-	short *data = array;
-
-	for (i = 0; i < num_samples; i++) {
-		data[i] = le16_to_cpu(data[i]);
-		if (thisboard->ai_nbits == 12)
-			data[i] = (data[i] >> 4) & 0xfff;
+static const struct das16_board das16_boards[] = {
+	{
+		.name		= "das-16",
+		.ai		= das16_ai_rinsn,
+		.ai_nbits	= 12,
+		.ai_speed	= 15000,
+		.ai_pg		= das16_pg_none,
+		.ao		= das16_ao_winsn,
+		.ao_nbits	= 12,
+		.di		= das16_di_rbits,
+		.do_		= das16_do_wbits,
+		.i8255_offset	= 0x10,
+		.i8254_offset	= 0x0c,
+		.size		= 0x14,
+		.id		= 0x00,
+	}, {
+		.name		= "das-16g",
+		.ai		= das16_ai_rinsn,
+		.ai_nbits	= 12,
+		.ai_speed	= 15000,
+		.ai_pg		= das16_pg_none,
+		.ao		= das16_ao_winsn,
+		.ao_nbits	= 12,
+		.di		= das16_di_rbits,
+		.do_		= das16_do_wbits,
+		.i8255_offset	= 0x10,
+		.i8254_offset	= 0x0c,
+		.size		= 0x14,
+		.id		= 0x00,
+	}, {
+		.name		= "das-16f",
+		.ai		= das16_ai_rinsn,
+		.ai_nbits	= 12,
+		.ai_speed	= 8500,
+		.ai_pg		= das16_pg_none,
+		.ao		= das16_ao_winsn,
+		.ao_nbits	= 12,
+		.di		= das16_di_rbits,
+		.do_		= das16_do_wbits,
+		.i8255_offset	= 0x10,
+		.i8254_offset	= 0x0c,
+		.size		= 0x14,
+		.id		= 0x00,
+	}, {
+		.name		= "cio-das16",
+		.ai		= das16_ai_rinsn,
+		.ai_nbits	= 12,
+		.ai_speed	= 20000,
+		.ai_pg		= das16_pg_none,
+		.ao		= das16_ao_winsn,
+		.ao_nbits	= 12,
+		.di		= das16_di_rbits,
+		.do_		= das16_do_wbits,
+		.i8255_offset	= 0x10,
+		.i8254_offset	= 0x0c,
+		.size		= 0x14,
+		.id		= 0x80,
+	}, {
+		.name		= "cio-das16/f",
+		.ai		= das16_ai_rinsn,
+		.ai_nbits	= 12,
+		.ai_speed	= 10000,
+		.ai_pg		= das16_pg_none,
+		.ao		= das16_ao_winsn,
+		.ao_nbits	= 12,
+		.di		= das16_di_rbits,
+		.do_		= das16_do_wbits,
+		.i8255_offset	= 0x10,
+		.i8254_offset	= 0x0c,
+		.size		= 0x14,
+		.id		= 0x80,
+	}, {
+		.name		= "cio-das16/jr",
+		.ai		= das16_ai_rinsn,
+		.ai_nbits	= 12,
+		.ai_speed	= 7692,
+		.ai_pg		= das16_pg_16jr,
+		.ao		= NULL,
+		.di		= das16_di_rbits,
+		.do_		= das16_do_wbits,
+		.i8255_offset	= 0,
+		.i8254_offset	= 0x0c,
+		.size		= 0x10,
+		.id		= 0x00,
+	}, {
+		.name		= "pc104-das16jr",
+		.ai		= das16_ai_rinsn,
+		.ai_nbits	= 12,
+		.ai_speed	= 3300,
+		.ai_pg		= das16_pg_16jr,
+		.ao		= NULL,
+		.di		= das16_di_rbits,
+		.do_		= das16_do_wbits,
+		.i8255_offset	= 0,
+		.i8254_offset	= 0x0c,
+		.size		= 0x10,
+		.id		= 0x00,
+	}, {
+		.name		= "cio-das16jr/16",
+		.ai		= das16_ai_rinsn,
+		.ai_nbits	= 16,
+		.ai_speed	= 10000,
+		.ai_pg		= das16_pg_16jr_16,
+		.ao		= NULL,
+		.di		= das16_di_rbits,
+		.do_		= das16_do_wbits,
+		.i8255_offset	= 0,
+		.i8254_offset	= 0x0c,
+		.size		= 0x10,
+		.id		= 0x00,
+	}, {
+		.name		= "pc104-das16jr/16",
+		.ai		= das16_ai_rinsn,
+		.ai_nbits	= 16,
+		.ai_speed	= 10000,
+		.ai_pg		= das16_pg_16jr_16,
+		.ao		= NULL,
+		.di		= das16_di_rbits,
+		.do_		= das16_do_wbits,
+		.i8255_offset	= 0,
+		.i8254_offset	= 0x0c,
+		.size		= 0x10,
+		.id		= 0x00,
+	}, {
+		.name		= "das-1201",
+		.ai		= das16_ai_rinsn,
+		.ai_nbits	= 12,
+		.ai_speed	= 20000,
+		.ai_pg		= das16_pg_none,
+		.ao		= NULL,
+		.di		= das16_di_rbits,
+		.do_		= das16_do_wbits,
+		.i8255_offset	= 0x400,
+		.i8254_offset	= 0x0c,
+		.size		= 0x408,
+		.id		= 0x20,
+	}, {
+		.name		= "das-1202",
+		.ai		= das16_ai_rinsn,
+		.ai_nbits	= 12,
+		.ai_speed	= 10000,
+		.ai_pg		= das16_pg_none,
+		.ao		= NULL,
+		.di		= das16_di_rbits,
+		.do_		= das16_do_wbits,
+		.i8255_offset	= 0x400,
+		.i8254_offset	= 0x0c,
+		.size		= 0x408,
+		.id		= 0x20,
+	}, {
+		.name		= "das-1401",
+		.ai		= das16_ai_rinsn,
+		.ai_nbits	= 12,
+		.ai_speed	= 10000,
+		.ai_pg		= das16_pg_1601,
+		.ao		= NULL,
+		.di		= das16_di_rbits,
+		.do_		= das16_do_wbits,
+		.i8255_offset	= 0x0,
+		.i8254_offset	= 0x0c,
+		.size		= 0x408,
+		.id		= 0xc0,
+	}, {
+		.name		= "das-1402",
+		.ai		= das16_ai_rinsn,
+		.ai_nbits	= 12,
+		.ai_speed	= 10000,
+		.ai_pg		= das16_pg_1602,
+		.ao		= NULL,
+		.di		= das16_di_rbits,
+		.do_		= das16_do_wbits,
+		.i8255_offset	= 0x0,
+		.i8254_offset	= 0x0c,
+		.size		= 0x408,
+		.id		= 0xc0,
+	}, {
+		.name		= "das-1601",
+		.ai		= das16_ai_rinsn,
+		.ai_nbits	= 12,
+		.ai_speed	= 10000,
+		.ai_pg		= das16_pg_1601,
+		.ao		= das16_ao_winsn,
+		.ao_nbits	= 12,
+		.di		= das16_di_rbits,
+		.do_		= das16_do_wbits,
+		.i8255_offset	= 0x400,
+		.i8254_offset	= 0x0c,
+		.size		= 0x408,
+		.id		= 0xc0,
+	}, {
+		.name		= "das-1602",
+		.ai		= das16_ai_rinsn,
+		.ai_nbits	= 12,
+		.ai_speed	= 10000,
+		.ai_pg		= das16_pg_1602,
+		.ao		= das16_ao_winsn,
+		.ao_nbits	= 12,
+		.di		= das16_di_rbits,
+		.do_		= das16_do_wbits,
+		.i8255_offset	= 0x400,
+		.i8254_offset	= 0x0c,
+		.size		= 0x408,
+		.id		= 0xc0,
+	}, {
+		.name		= "cio-das1401/12",
+		.ai		= das16_ai_rinsn,
+		.ai_nbits	= 12,
+		.ai_speed	= 6250,
+		.ai_pg		= das16_pg_1601,
+		.ao		= NULL,
+		.di		= das16_di_rbits,
+		.do_		= das16_do_wbits,
+		.i8255_offset	= 0,
+		.i8254_offset	= 0x0c,
+		.size		= 0x408,
+		.id		= 0xc0,
+	}, {
+		.name		= "cio-das1402/12",
+		.ai		= das16_ai_rinsn,
+		.ai_nbits	= 12,
+		.ai_speed	= 6250,
+		.ai_pg		= das16_pg_1602,
+		.ao		= NULL,
+		.di		= das16_di_rbits,
+		.do_		= das16_do_wbits,
+		.i8255_offset	= 0,
+		.i8254_offset	= 0x0c,
+		.size		= 0x408,
+		.id		= 0xc0,
+	}, {
+		.name		= "cio-das1402/16",
+		.ai		= das16_ai_rinsn,
+		.ai_nbits	= 16,
+		.ai_speed	= 10000,
+		.ai_pg		= das16_pg_1602,
+		.ao		= NULL,
+		.di		= das16_di_rbits,
+		.do_		= das16_do_wbits,
+		.i8255_offset	= 0,
+		.i8254_offset	= 0x0c,
+		.size		= 0x408,
+		.id		= 0xc0,
+	}, {
+		.name		= "cio-das1601/12",
+		.ai		= das16_ai_rinsn,
+		.ai_nbits	= 12,
+		.ai_speed	= 6250,
+		.ai_pg		= das16_pg_1601,
+		.ao		= das16_ao_winsn,
+		.ao_nbits	= 12,
+		.di		= das16_di_rbits,
+		.do_		= das16_do_wbits,
+		.i8255_offset	= 0x400,
+		.i8254_offset	= 0x0c,
+		.size		= 0x408,
+		.id		= 0xc0,
+	}, {
+		.name		= "cio-das1602/12",
+		.ai		= das16_ai_rinsn,
+		.ai_nbits	= 12,
+		.ai_speed	= 10000,
+		.ai_pg		= das16_pg_1602,
+		.ao		= das16_ao_winsn,
+		.ao_nbits	= 12,
+		.di		= das16_di_rbits,
+		.do_		= das16_do_wbits,
+		.i8255_offset	= 0x400,
+		.i8254_offset	= 0x0c,
+		.size		= 0x408,
+		.id		= 0xc0,
+	}, {
+		.name		= "cio-das1602/16",
+		.ai		= das16_ai_rinsn,
+		.ai_nbits	= 16,
+		.ai_speed	= 10000,
+		.ai_pg		= das16_pg_1602,
+		.ao		= das16_ao_winsn,
+		.ao_nbits	= 12,
+		.di		= das16_di_rbits,
+		.do_		= das16_do_wbits,
+		.i8255_offset	= 0x400,
+		.i8254_offset	= 0x0c,
+		.size		= 0x408,
+		.id		= 0xc0,
+	}, {
+		.name		= "cio-das16/330",
+		.ai		= das16_ai_rinsn,
+		.ai_nbits	= 12,
+		.ai_speed	= 3030,
+		.ai_pg		= das16_pg_16jr,
+		.ao		= NULL,
+		.di		= das16_di_rbits,
+		.do_		= das16_do_wbits,
+		.i8255_offset	= 0,
+		.i8254_offset	= 0x0c,
+		.size		= 0x14,
+		.id		= 0xf0,
+	},
+};
 
-	}
-}
+static struct comedi_driver das16_driver = {
+	.driver_name	= "das16",
+	.module		= THIS_MODULE,
+	.attach		= das16_attach,
+	.detach		= das16_detach,
+	.board_name	= &das16_boards[0].name,
+	.num_names	= ARRAY_SIZE(das16_boards),
+	.offset		= sizeof(das16_boards[0]),
+};
+module_comedi_driver(das16_driver);
 
 MODULE_AUTHOR("Comedi http://www.comedi.org");
 MODULE_DESCRIPTION("Comedi low-level driver");
diff --git a/drivers/staging/comedi/drivers/das16m1.c b/drivers/staging/comedi/drivers/das16m1.c
index 5376e718e3d..d2e1490cd80 100644
--- a/drivers/staging/comedi/drivers/das16m1.c
+++ b/drivers/staging/comedi/drivers/das16m1.c
@@ -132,57 +132,11 @@ static const struct comedi_lrange range_das16m1 = { 9,
 	 }
 };
 
-static int das16m1_do_wbits(struct comedi_device *dev,
-			    struct comedi_subdevice *s,
-			    struct comedi_insn *insn, unsigned int *data);
-static int das16m1_di_rbits(struct comedi_device *dev,
-			    struct comedi_subdevice *s,
-			    struct comedi_insn *insn, unsigned int *data);
-static int das16m1_ai_rinsn(struct comedi_device *dev,
-			    struct comedi_subdevice *s,
-			    struct comedi_insn *insn, unsigned int *data);
-
-static int das16m1_cmd_test(struct comedi_device *dev,
-			    struct comedi_subdevice *s, struct comedi_cmd *cmd);
-static int das16m1_cmd_exec(struct comedi_device *dev,
-			    struct comedi_subdevice *s);
-static int das16m1_cancel(struct comedi_device *dev,
-			  struct comedi_subdevice *s);
-
-static int das16m1_poll(struct comedi_device *dev, struct comedi_subdevice *s);
-static irqreturn_t das16m1_interrupt(int irq, void *d);
-static void das16m1_handler(struct comedi_device *dev, unsigned int status);
-
-static unsigned int das16m1_set_pacer(struct comedi_device *dev,
-				      unsigned int ns, int round_flag);
-
-static int das16m1_irq_bits(unsigned int irq);
-
 struct das16m1_board {
 	const char *name;
 	unsigned int ai_speed;
 };
 
-static const struct das16m1_board das16m1_boards[] = {
-	{
-	 .name = "cio-das16/m1",	/*  CIO-DAS16_M1.pdf */
-	 .ai_speed = 1000,	/*  1MHz max speed */
-	 },
-};
-
-static int das16m1_attach(struct comedi_device *dev,
-			  struct comedi_devconfig *it);
-static int das16m1_detach(struct comedi_device *dev);
-static struct comedi_driver driver_das16m1 = {
-	.driver_name = "das16m1",
-	.module = THIS_MODULE,
-	.attach = das16m1_attach,
-	.detach = das16m1_detach,
-	.board_name = &das16m1_boards[0].name,
-	.num_names = ARRAY_SIZE(das16m1_boards),
-	.offset = sizeof(das16m1_boards[0]),
-};
-
 struct das16m1_private_struct {
 	unsigned int control_state;
 	volatile unsigned int adc_count;	/*  number of samples completed */
@@ -198,22 +152,17 @@ struct das16m1_private_struct {
 #define devpriv ((struct das16m1_private_struct *)(dev->private))
 #define thisboard ((const struct das16m1_board *)(dev->board_ptr))
 
-static int __init driver_das16m1_init_module(void)
+static inline short munge_sample(short data)
 {
-	return comedi_driver_register(&driver_das16m1);
+	return (data >> 4) & 0xfff;
 }
 
-static void __exit driver_das16m1_cleanup_module(void)
+static void munge_sample_array(short *array, unsigned int num_elements)
 {
-	comedi_driver_unregister(&driver_das16m1);
-}
-
-module_init(driver_das16m1_init_module);
-module_exit(driver_das16m1_cleanup_module);
+	unsigned int i;
 
-static inline short munge_sample(short data)
-{
-	return (data >> 4) & 0xfff;
+	for (i = 0; i < num_elements; i++)
+		array[i] = munge_sample(array[i]);
 }
 
 static int das16m1_cmd_test(struct comedi_device *dev,
@@ -340,6 +289,25 @@ static int das16m1_cmd_test(struct comedi_device *dev,
 	return 0;
 }
 
+/* This function takes a time in nanoseconds and sets the     *
+ * 2 pacer clocks to the closest frequency possible. It also  *
+ * returns the actual sampling period.                        */
+static unsigned int das16m1_set_pacer(struct comedi_device *dev,
+				      unsigned int ns, int rounding_flags)
+{
+	i8253_cascade_ns_to_timer_2div(DAS16M1_XTAL, &(devpriv->divisor1),
+				       &(devpriv->divisor2), &ns,
+				       rounding_flags & TRIG_ROUND_MASK);
+
+	/* Write the values of ctr1 and ctr2 into counters 1 and 2 */
+	i8254_load(dev->iobase + DAS16M1_8254_SECOND, 0, 1, devpriv->divisor1,
+		   2);
+	i8254_load(dev->iobase + DAS16M1_8254_SECOND, 0, 2, devpriv->divisor2,
+		   2);
+
+	return ns;
+}
+
 static int das16m1_cmd_exec(struct comedi_device *dev,
 			    struct comedi_subdevice *s)
 {
@@ -484,57 +452,6 @@ static int das16m1_do_wbits(struct comedi_device *dev,
 	return 2;
 }
 
-static int das16m1_poll(struct comedi_device *dev, struct comedi_subdevice *s)
-{
-	unsigned long flags;
-	unsigned int status;
-
-	/*  prevent race with interrupt handler */
-	spin_lock_irqsave(&dev->spinlock, flags);
-	status = inb(dev->iobase + DAS16M1_CS);
-	das16m1_handler(dev, status);
-	spin_unlock_irqrestore(&dev->spinlock, flags);
-
-	return s->async->buf_write_count - s->async->buf_read_count;
-}
-
-static irqreturn_t das16m1_interrupt(int irq, void *d)
-{
-	int status;
-	struct comedi_device *dev = d;
-
-	if (dev->attached == 0) {
-		comedi_error(dev, "premature interrupt");
-		return IRQ_HANDLED;
-	}
-	/*  prevent race with comedi_poll() */
-	spin_lock(&dev->spinlock);
-
-	status = inb(dev->iobase + DAS16M1_CS);
-
-	if ((status & (IRQDATA | OVRUN)) == 0) {
-		comedi_error(dev, "spurious interrupt");
-		spin_unlock(&dev->spinlock);
-		return IRQ_NONE;
-	}
-
-	das16m1_handler(dev, status);
-
-	/* clear interrupt */
-	outb(0, dev->iobase + DAS16M1_CLEAR_INTR);
-
-	spin_unlock(&dev->spinlock);
-	return IRQ_HANDLED;
-}
-
-static void munge_sample_array(short *array, unsigned int num_elements)
-{
-	unsigned int i;
-
-	for (i = 0; i < num_elements; i++)
-		array[i] = munge_sample(array[i]);
-}
-
 static void das16m1_handler(struct comedi_device *dev, unsigned int status)
 {
 	struct comedi_subdevice *s;
@@ -596,23 +513,47 @@ static void das16m1_handler(struct comedi_device *dev, unsigned int status)
 
 }
 
-/* This function takes a time in nanoseconds and sets the     *
- * 2 pacer clocks to the closest frequency possible. It also  *
- * returns the actual sampling period.                        */
-static unsigned int das16m1_set_pacer(struct comedi_device *dev,
-				      unsigned int ns, int rounding_flags)
+static int das16m1_poll(struct comedi_device *dev, struct comedi_subdevice *s)
 {
-	i8253_cascade_ns_to_timer_2div(DAS16M1_XTAL, &(devpriv->divisor1),
-				       &(devpriv->divisor2), &ns,
-				       rounding_flags & TRIG_ROUND_MASK);
+	unsigned long flags;
+	unsigned int status;
 
-	/* Write the values of ctr1 and ctr2 into counters 1 and 2 */
-	i8254_load(dev->iobase + DAS16M1_8254_SECOND, 0, 1, devpriv->divisor1,
-		   2);
-	i8254_load(dev->iobase + DAS16M1_8254_SECOND, 0, 2, devpriv->divisor2,
-		   2);
+	/*  prevent race with interrupt handler */
+	spin_lock_irqsave(&dev->spinlock, flags);
+	status = inb(dev->iobase + DAS16M1_CS);
+	das16m1_handler(dev, status);
+	spin_unlock_irqrestore(&dev->spinlock, flags);
 
-	return ns;
+	return s->async->buf_write_count - s->async->buf_read_count;
+}
+
+static irqreturn_t das16m1_interrupt(int irq, void *d)
+{
+	int status;
+	struct comedi_device *dev = d;
+
+	if (dev->attached == 0) {
+		comedi_error(dev, "premature interrupt");
+		return IRQ_HANDLED;
+	}
+	/*  prevent race with comedi_poll() */
+	spin_lock(&dev->spinlock);
+
+	status = inb(dev->iobase + DAS16M1_CS);
+
+	if ((status & (IRQDATA | OVRUN)) == 0) {
+		comedi_error(dev, "spurious interrupt");
+		spin_unlock(&dev->spinlock);
+		return IRQ_NONE;
+	}
+
+	das16m1_handler(dev, status);
+
+	/* clear interrupt */
+	outb(0, dev->iobase + DAS16M1_CLEAR_INTR);
+
+	spin_unlock(&dev->spinlock);
+	return IRQ_HANDLED;
 }
 
 static int das16m1_irq_bits(unsigned int irq)
@@ -656,7 +597,6 @@ static int das16m1_irq_bits(unsigned int irq)
  *   0  I/O base
  *   1  IRQ
  */
-
 static int das16m1_attach(struct comedi_device *dev,
 			  struct comedi_devconfig *it)
 {
@@ -673,12 +613,12 @@ static int das16m1_attach(struct comedi_device *dev,
 
 	dev->board_name = thisboard->name;
 
-	if (!request_region(iobase, DAS16M1_SIZE, driver_das16m1.driver_name)) {
+	if (!request_region(iobase, DAS16M1_SIZE, dev->driver->driver_name)) {
 		comedi_error(dev, "I/O port conflict\n");
 		return -EIO;
 	}
 	if (!request_region(iobase + DAS16M1_82C55, DAS16M1_SIZE2,
-			    driver_das16m1.driver_name)) {
+			    dev->driver->driver_name)) {
 		release_region(iobase, DAS16M1_SIZE);
 		comedi_error(dev, "I/O port conflict\n");
 		return -EIO;
@@ -690,7 +630,7 @@ static int das16m1_attach(struct comedi_device *dev,
 	/*  make sure it is valid */
 	if (das16m1_irq_bits(irq) >= 0) {
 		ret = request_irq(irq, das16m1_interrupt, 0,
-				  driver_das16m1.driver_name, dev);
+				  dev->driver->driver_name, dev);
 		if (ret < 0)
 			return ret;
 		dev->irq = irq;
@@ -763,25 +703,36 @@ static int das16m1_attach(struct comedi_device *dev,
 	return 0;
 }
 
-static int das16m1_detach(struct comedi_device *dev)
+static void das16m1_detach(struct comedi_device *dev)
 {
-
-/* das16m1_reset(dev); */
-
 	if (dev->subdevices)
 		subdev_8255_cleanup(dev, dev->subdevices + 3);
-
 	if (dev->irq)
 		free_irq(dev->irq, dev);
-
 	if (dev->iobase) {
 		release_region(dev->iobase, DAS16M1_SIZE);
 		release_region(dev->iobase + DAS16M1_82C55, DAS16M1_SIZE2);
 	}
-
-	return 0;
 }
 
+static const struct das16m1_board das16m1_boards[] = {
+	{
+		.name		= "cio-das16/m1",	/*  CIO-DAS16_M1.pdf */
+		.ai_speed	= 1000,			/*  1MHz max speed */
+	},
+};
+
+static struct comedi_driver das16m1_driver = {
+	.driver_name	= "das16m1",
+	.module		= THIS_MODULE,
+	.attach		= das16m1_attach,
+	.detach		= das16m1_detach,
+	.board_name	= &das16m1_boards[0].name,
+	.num_names	= ARRAY_SIZE(das16m1_boards),
+	.offset		= sizeof(das16m1_boards[0]),
+};
+module_comedi_driver(das16m1_driver);
+
 MODULE_AUTHOR("Comedi http://www.comedi.org");
 MODULE_DESCRIPTION("Comedi low-level driver");
 MODULE_LICENSE("GPL");
diff --git a/drivers/staging/comedi/drivers/das1800.c b/drivers/staging/comedi/drivers/das1800.c
index 99ada5a53b9..2ac344354c1 100644
--- a/drivers/staging/comedi/drivers/das1800.c
+++ b/drivers/staging/comedi/drivers/das1800.c
@@ -183,9 +183,6 @@ enum {
 	das1802hr, das1802hr_da, das1801hc, das1802hc, das1801ao, das1802ao
 };
 
-static int das1800_attach(struct comedi_device *dev,
-			  struct comedi_devconfig *it);
-static int das1800_detach(struct comedi_device *dev);
 static int das1800_probe(struct comedi_device *dev);
 static int das1800_cancel(struct comedi_device *dev,
 			  struct comedi_subdevice *s);
@@ -518,33 +515,6 @@ static const struct comedi_lrange range_ao_2 = {
 };
 */
 
-static struct comedi_driver driver_das1800 = {
-	.driver_name = "das1800",
-	.module = THIS_MODULE,
-	.attach = das1800_attach,
-	.detach = das1800_detach,
-	.num_names = ARRAY_SIZE(das1800_boards),
-	.board_name = &das1800_boards[0].name,
-	.offset = sizeof(struct das1800_board),
-};
-
-/*
- * A convenient macro that defines init_module() and cleanup_module(),
- * as necessary.
- */
-static int __init driver_das1800_init_module(void)
-{
-	return comedi_driver_register(&driver_das1800);
-}
-
-static void __exit driver_das1800_cleanup_module(void)
-{
-	comedi_driver_unregister(&driver_das1800);
-}
-
-module_init(driver_das1800_init_module);
-module_exit(driver_das1800_cleanup_module);
-
 static int das1800_init_dma(struct comedi_device *dev, unsigned int dma0,
 			    unsigned int dma1)
 {
@@ -579,7 +549,7 @@ static int das1800_init_dma(struct comedi_device *dev, unsigned int dma0,
 			return -EINVAL;
 			break;
 		}
-		if (request_dma(dma0, driver_das1800.driver_name)) {
+		if (request_dma(dma0, dev->driver->driver_name)) {
 			dev_err(dev->hw_dev, "failed to allocate dma channel %i\n",
 				dma0);
 			return -EINVAL;
@@ -587,7 +557,7 @@ static int das1800_init_dma(struct comedi_device *dev, unsigned int dma0,
 		devpriv->dma0 = dma0;
 		devpriv->dma_current = dma0;
 		if (dma1) {
-			if (request_dma(dma1, driver_das1800.driver_name)) {
+			if (request_dma(dma1, dev->driver->driver_name)) {
 				dev_err(dev->hw_dev, "failed to allocate dma channel %i\n",
 					dma1);
 				return -EINVAL;
@@ -633,7 +603,7 @@ static int das1800_attach(struct comedi_device *dev,
 		return -ENOMEM;
 
 	printk(KERN_DEBUG "comedi%d: %s: io 0x%lx", dev->minor,
-	       driver_das1800.driver_name, iobase);
+	       dev->driver->driver_name, iobase);
 	if (irq) {
 		printk(KERN_CONT ", irq %u", irq);
 		if (dma0) {
@@ -650,7 +620,7 @@ static int das1800_attach(struct comedi_device *dev,
 	}
 
 	/* check if io addresses are available */
-	if (!request_region(iobase, DAS1800_SIZE, driver_das1800.driver_name)) {
+	if (!request_region(iobase, DAS1800_SIZE, dev->driver->driver_name)) {
 		printk
 		    (" I/O port conflict: failed to allocate ports 0x%lx to 0x%lx\n",
 		     iobase, iobase + DAS1800_SIZE - 1);
@@ -671,7 +641,7 @@ static int das1800_attach(struct comedi_device *dev,
 	if (thisboard->ao_ability == 2) {
 		iobase2 = iobase + IOBASE2;
 		if (!request_region(iobase2, DAS1800_SIZE,
-				    driver_das1800.driver_name)) {
+				    dev->driver->driver_name)) {
 			printk
 			    (" I/O port conflict: failed to allocate ports 0x%lx to 0x%lx\n",
 			     iobase2, iobase2 + DAS1800_SIZE - 1);
@@ -683,7 +653,7 @@ static int das1800_attach(struct comedi_device *dev,
 	/* grab our IRQ */
 	if (irq) {
 		if (request_irq(irq, das1800_interrupt, 0,
-				driver_das1800.driver_name, dev)) {
+				dev->driver->driver_name, dev)) {
 			dev_dbg(dev->hw_dev, "unable to allocate irq %u\n",
 				irq);
 			return -EINVAL;
@@ -797,9 +767,8 @@ static int das1800_attach(struct comedi_device *dev,
 	return 0;
 };
 
-static int das1800_detach(struct comedi_device *dev)
+static void das1800_detach(struct comedi_device *dev)
 {
-	/* only free stuff if it has been allocated by _attach */
 	if (dev->iobase)
 		release_region(dev->iobase, DAS1800_SIZE);
 	if (dev->irq)
@@ -814,11 +783,6 @@ static int das1800_detach(struct comedi_device *dev)
 		kfree(devpriv->ai_buf0);
 		kfree(devpriv->ai_buf1);
 	}
-
-	dev_dbg(dev->hw_dev, "comedi%d: %s: remove\n", dev->minor,
-		driver_das1800.driver_name);
-
-	return 0;
 };
 
 /* probes and checks das-1800 series board type
@@ -1811,6 +1775,17 @@ static unsigned int suggest_transfer_size(struct comedi_cmd *cmd)
 	return size;
 }
 
+static struct comedi_driver das1800_driver = {
+	.driver_name	= "das1800",
+	.module		= THIS_MODULE,
+	.attach		= das1800_attach,
+	.detach		= das1800_detach,
+	.num_names	= ARRAY_SIZE(das1800_boards),
+	.board_name	= &das1800_boards[0].name,
+	.offset		= sizeof(struct das1800_board),
+};
+module_comedi_driver(das1800_driver);
+
 MODULE_AUTHOR("Comedi http://www.comedi.org");
 MODULE_DESCRIPTION("Comedi low-level driver");
 MODULE_LICENSE("GPL");
diff --git a/drivers/staging/comedi/drivers/das6402.c b/drivers/staging/comedi/drivers/das6402.c
index f25684145e8..881f392d1db 100644
--- a/drivers/staging/comedi/drivers/das6402.c
+++ b/drivers/staging/comedi/drivers/das6402.c
@@ -99,29 +99,6 @@ This driver has suffered bitrot.
 #define	C2 0x80
 #define	RWLH 0x30
 
-static int das6402_attach(struct comedi_device *dev,
-			  struct comedi_devconfig *it);
-static int das6402_detach(struct comedi_device *dev);
-static struct comedi_driver driver_das6402 = {
-	.driver_name = "das6402",
-	.module = THIS_MODULE,
-	.attach = das6402_attach,
-	.detach = das6402_detach,
-};
-
-static int __init driver_das6402_init_module(void)
-{
-	return comedi_driver_register(&driver_das6402);
-}
-
-static void __exit driver_das6402_cleanup_module(void)
-{
-	comedi_driver_unregister(&driver_das6402);
-}
-
-module_init(driver_das6402_init_module);
-module_exit(driver_das6402_cleanup_module);
-
 struct das6402_private {
 	int ai_bytes_to_read;
 
@@ -130,7 +107,14 @@ struct das6402_private {
 #define devpriv ((struct das6402_private *)dev->private)
 
 static void das6402_ai_fifo_dregs(struct comedi_device *dev,
-				  struct comedi_subdevice *s);
+				  struct comedi_subdevice *s)
+{
+	while (1) {
+		if (!(inb(dev->iobase + 8) & 0x01))
+			return;
+		comedi_buf_put(s->async, inw(dev->iobase));
+	}
+}
 
 static void das6402_setcounter(struct comedi_device *dev)
 {
@@ -209,16 +193,6 @@ static void das6402_ai_fifo_read(struct comedi_device *dev, short *data, int n)
 }
 #endif
 
-static void das6402_ai_fifo_dregs(struct comedi_device *dev,
-				  struct comedi_subdevice *s)
-{
-	while (1) {
-		if (!(inb(dev->iobase + 8) & 0x01))
-			return;
-		comedi_buf_put(s->async, inw(dev->iobase));
-	}
-}
-
 static int das6402_ai_cancel(struct comedi_device *dev,
 			     struct comedi_subdevice *s)
 {
@@ -300,16 +274,6 @@ static int board_init(struct comedi_device *dev)
 	return 0;
 }
 
-static int das6402_detach(struct comedi_device *dev)
-{
-	if (dev->irq)
-		free_irq(dev->irq, dev);
-	if (dev->iobase)
-		release_region(dev->iobase, DAS6402_SIZE);
-
-	return 0;
-}
-
 static int das6402_attach(struct comedi_device *dev,
 			  struct comedi_devconfig *it)
 {
@@ -363,6 +327,22 @@ static int das6402_attach(struct comedi_device *dev,
 	return 0;
 }
 
+static void das6402_detach(struct comedi_device *dev)
+{
+	if (dev->irq)
+		free_irq(dev->irq, dev);
+	if (dev->iobase)
+		release_region(dev->iobase, DAS6402_SIZE);
+}
+
+static struct comedi_driver das6402_driver = {
+	.driver_name	= "das6402",
+	.module		= THIS_MODULE,
+	.attach		= das6402_attach,
+	.detach		= das6402_detach,
+};
+module_comedi_driver(das6402_driver)
+
 MODULE_AUTHOR("Comedi http://www.comedi.org");
 MODULE_DESCRIPTION("Comedi low-level driver");
 MODULE_LICENSE("GPL");
diff --git a/drivers/staging/comedi/drivers/das800.c b/drivers/staging/comedi/drivers/das800.c
index 6e347b40fe6..a3a54e1c5c9 100644
--- a/drivers/staging/comedi/drivers/das800.c
+++ b/drivers/staging/comedi/drivers/das800.c
@@ -245,7 +245,7 @@ struct das800_private {
 
 static int das800_attach(struct comedi_device *dev,
 			 struct comedi_devconfig *it);
-static int das800_detach(struct comedi_device *dev);
+static void das800_detach(struct comedi_device *dev);
 static int das800_cancel(struct comedi_device *dev, struct comedi_subdevice *s);
 
 static struct comedi_driver driver_das800 = {
@@ -556,16 +556,12 @@ static int das800_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 	return 0;
 };
 
-static int das800_detach(struct comedi_device *dev)
+static void das800_detach(struct comedi_device *dev)
 {
-	dev_info(dev->hw_dev, "comedi%d: das800: remove\n", dev->minor);
-
-	/* only free stuff if it has been allocated by _attach */
 	if (dev->iobase)
 		release_region(dev->iobase, DAS800_SIZE);
 	if (dev->irq)
 		free_irq(dev->irq, dev);
-	return 0;
 };
 
 static int das800_cancel(struct comedi_device *dev, struct comedi_subdevice *s)
diff --git a/drivers/staging/comedi/drivers/dmm32at.c b/drivers/staging/comedi/drivers/dmm32at.c
index 2b4e6e6eb82..83828903db4 100644
--- a/drivers/staging/comedi/drivers/dmm32at.c
+++ b/drivers/staging/comedi/drivers/dmm32at.c
@@ -224,7 +224,7 @@ struct dmm32at_private {
  */
 static int dmm32at_attach(struct comedi_device *dev,
 			  struct comedi_devconfig *it);
-static int dmm32at_detach(struct comedi_device *dev);
+static void dmm32at_detach(struct comedi_device *dev);
 static struct comedi_driver driver_dmm32at = {
 	.driver_name = "dmm32at",
 	.module = THIS_MODULE,
@@ -450,23 +450,12 @@ static int dmm32at_attach(struct comedi_device *dev,
 
 }
 
-/*
- * _detach is called to deconfigure a device.  It should deallocate
- * resources.
- * This function is also called when _attach() fails, so it should be
- * careful not to release resources that were not necessarily
- * allocated by _attach().  dev->private and dev->subdevices are
- * deallocated automatically by the core.
- */
-static int dmm32at_detach(struct comedi_device *dev)
+static void dmm32at_detach(struct comedi_device *dev)
 {
-	printk(KERN_INFO "comedi%d: dmm32at: remove\n", dev->minor);
 	if (dev->irq)
 		free_irq(dev->irq, dev);
 	if (dev->iobase)
 		release_region(dev->iobase, DMM32AT_MEMSIZE);
-
-	return 0;
 }
 
 /*
diff --git a/drivers/staging/comedi/drivers/dt2801.c b/drivers/staging/comedi/drivers/dt2801.c
index b85c8366a39..625bd617a8e 100644
--- a/drivers/staging/comedi/drivers/dt2801.c
+++ b/drivers/staging/comedi/drivers/dt2801.c
@@ -88,29 +88,6 @@ Configuration options:
 #define DT2801_STATUS		1
 #define DT2801_CMD		1
 
-static int dt2801_attach(struct comedi_device *dev,
-			 struct comedi_devconfig *it);
-static int dt2801_detach(struct comedi_device *dev);
-static struct comedi_driver driver_dt2801 = {
-	.driver_name = "dt2801",
-	.module = THIS_MODULE,
-	.attach = dt2801_attach,
-	.detach = dt2801_detach,
-};
-
-static int __init driver_dt2801_init_module(void)
-{
-	return comedi_driver_register(&driver_dt2801);
-}
-
-static void __exit driver_dt2801_cleanup_module(void)
-{
-	comedi_driver_unregister(&driver_dt2801);
-}
-
-module_init(driver_dt2801_init_module);
-module_exit(driver_dt2801_cleanup_module);
-
 #if 0
 /* ignore 'defined but not used' warning */
 static const struct comedi_lrange range_dt2801_ai_pgh_bipolar = { 4, {
@@ -258,22 +235,6 @@ struct dt2801_private {
 
 #define devpriv ((struct dt2801_private *)dev->private)
 
-static int dt2801_ai_insn_read(struct comedi_device *dev,
-			       struct comedi_subdevice *s,
-			       struct comedi_insn *insn, unsigned int *data);
-static int dt2801_ao_insn_read(struct comedi_device *dev,
-			       struct comedi_subdevice *s,
-			       struct comedi_insn *insn, unsigned int *data);
-static int dt2801_ao_insn_write(struct comedi_device *dev,
-				struct comedi_subdevice *s,
-				struct comedi_insn *insn, unsigned int *data);
-static int dt2801_dio_insn_bits(struct comedi_device *dev,
-				struct comedi_subdevice *s,
-				struct comedi_insn *insn, unsigned int *data);
-static int dt2801_dio_insn_config(struct comedi_device *dev,
-				  struct comedi_subdevice *s,
-				  struct comedi_insn *insn, unsigned int *data);
-
 /* These are the low-level routines:
    writecommand: write a command to the board
    writedata: write data byte
@@ -503,6 +464,123 @@ static const struct comedi_lrange *ai_range_lkup(int type, int opt)
 	return &range_unknown;
 }
 
+static int dt2801_error(struct comedi_device *dev, int stat)
+{
+	if (stat < 0) {
+		if (stat == -ETIME)
+			printk("dt2801: timeout\n");
+		else
+			printk("dt2801: error %d\n", stat);
+		return stat;
+	}
+	printk("dt2801: error status 0x%02x, resetting...\n", stat);
+
+	dt2801_reset(dev);
+	dt2801_reset(dev);
+
+	return -EIO;
+}
+
+static int dt2801_ai_insn_read(struct comedi_device *dev,
+			       struct comedi_subdevice *s,
+			       struct comedi_insn *insn, unsigned int *data)
+{
+	int d;
+	int stat;
+	int i;
+
+	for (i = 0; i < insn->n; i++) {
+		stat = dt2801_writecmd(dev, DT_C_READ_ADIM);
+		dt2801_writedata(dev, CR_RANGE(insn->chanspec));
+		dt2801_writedata(dev, CR_CHAN(insn->chanspec));
+		stat = dt2801_readdata2(dev, &d);
+
+		if (stat != 0)
+			return dt2801_error(dev, stat);
+
+		data[i] = d;
+	}
+
+	return i;
+}
+
+static int dt2801_ao_insn_read(struct comedi_device *dev,
+			       struct comedi_subdevice *s,
+			       struct comedi_insn *insn, unsigned int *data)
+{
+	data[0] = devpriv->ao_readback[CR_CHAN(insn->chanspec)];
+
+	return 1;
+}
+
+static int dt2801_ao_insn_write(struct comedi_device *dev,
+				struct comedi_subdevice *s,
+				struct comedi_insn *insn, unsigned int *data)
+{
+	dt2801_writecmd(dev, DT_C_WRITE_DAIM);
+	dt2801_writedata(dev, CR_CHAN(insn->chanspec));
+	dt2801_writedata2(dev, data[0]);
+
+	devpriv->ao_readback[CR_CHAN(insn->chanspec)] = data[0];
+
+	return 1;
+}
+
+static int dt2801_dio_insn_bits(struct comedi_device *dev,
+				struct comedi_subdevice *s,
+				struct comedi_insn *insn, unsigned int *data)
+{
+	int which = 0;
+
+	if (s == dev->subdevices + 4)
+		which = 1;
+
+	if (insn->n != 2)
+		return -EINVAL;
+	if (data[0]) {
+		s->state &= ~data[0];
+		s->state |= (data[0] & data[1]);
+		dt2801_writecmd(dev, DT_C_WRITE_DIG);
+		dt2801_writedata(dev, which);
+		dt2801_writedata(dev, s->state);
+	}
+	dt2801_writecmd(dev, DT_C_READ_DIG);
+	dt2801_writedata(dev, which);
+	dt2801_readdata(dev, data + 1);
+
+	return 2;
+}
+
+static int dt2801_dio_insn_config(struct comedi_device *dev,
+				  struct comedi_subdevice *s,
+				  struct comedi_insn *insn, unsigned int *data)
+{
+	int which = 0;
+
+	if (s == dev->subdevices + 4)
+		which = 1;
+
+	/* configure */
+	switch (data[0]) {
+	case INSN_CONFIG_DIO_OUTPUT:
+		s->io_bits = 0xff;
+		dt2801_writecmd(dev, DT_C_SET_DIGOUT);
+		break;
+	case INSN_CONFIG_DIO_INPUT:
+		s->io_bits = 0;
+		dt2801_writecmd(dev, DT_C_SET_DIGIN);
+		break;
+	case INSN_CONFIG_DIO_QUERY:
+		data[1] = s->io_bits ? COMEDI_OUTPUT : COMEDI_INPUT;
+		return insn->n;
+	default:
+		return -EINVAL;
+	}
+	dt2801_writedata(dev, which);
+
+	return 1;
+}
+
 /*
    options:
 	[0] - i/o base
@@ -615,130 +693,19 @@ out:
 	return ret;
 }
 
-static int dt2801_detach(struct comedi_device *dev)
+static void dt2801_detach(struct comedi_device *dev)
 {
 	if (dev->iobase)
 		release_region(dev->iobase, DT2801_IOSIZE);
-
-	return 0;
-}
-
-static int dt2801_error(struct comedi_device *dev, int stat)
-{
-	if (stat < 0) {
-		if (stat == -ETIME)
-			printk("dt2801: timeout\n");
-		else
-			printk("dt2801: error %d\n", stat);
-		return stat;
-	}
-	printk("dt2801: error status 0x%02x, resetting...\n", stat);
-
-	dt2801_reset(dev);
-	dt2801_reset(dev);
-
-	return -EIO;
-}
-
-static int dt2801_ai_insn_read(struct comedi_device *dev,
-			       struct comedi_subdevice *s,
-			       struct comedi_insn *insn, unsigned int *data)
-{
-	int d;
-	int stat;
-	int i;
-
-	for (i = 0; i < insn->n; i++) {
-		stat = dt2801_writecmd(dev, DT_C_READ_ADIM);
-		dt2801_writedata(dev, CR_RANGE(insn->chanspec));
-		dt2801_writedata(dev, CR_CHAN(insn->chanspec));
-		stat = dt2801_readdata2(dev, &d);
-
-		if (stat != 0)
-			return dt2801_error(dev, stat);
-
-		data[i] = d;
-	}
-
-	return i;
-}
-
-static int dt2801_ao_insn_read(struct comedi_device *dev,
-			       struct comedi_subdevice *s,
-			       struct comedi_insn *insn, unsigned int *data)
-{
-	data[0] = devpriv->ao_readback[CR_CHAN(insn->chanspec)];
-
-	return 1;
-}
-
-static int dt2801_ao_insn_write(struct comedi_device *dev,
-				struct comedi_subdevice *s,
-				struct comedi_insn *insn, unsigned int *data)
-{
-	dt2801_writecmd(dev, DT_C_WRITE_DAIM);
-	dt2801_writedata(dev, CR_CHAN(insn->chanspec));
-	dt2801_writedata2(dev, data[0]);
-
-	devpriv->ao_readback[CR_CHAN(insn->chanspec)] = data[0];
-
-	return 1;
-}
-
-static int dt2801_dio_insn_bits(struct comedi_device *dev,
-				struct comedi_subdevice *s,
-				struct comedi_insn *insn, unsigned int *data)
-{
-	int which = 0;
-
-	if (s == dev->subdevices + 4)
-		which = 1;
-
-	if (insn->n != 2)
-		return -EINVAL;
-	if (data[0]) {
-		s->state &= ~data[0];
-		s->state |= (data[0] & data[1]);
-		dt2801_writecmd(dev, DT_C_WRITE_DIG);
-		dt2801_writedata(dev, which);
-		dt2801_writedata(dev, s->state);
-	}
-	dt2801_writecmd(dev, DT_C_READ_DIG);
-	dt2801_writedata(dev, which);
-	dt2801_readdata(dev, data + 1);
-
-	return 2;
 }
 
-static int dt2801_dio_insn_config(struct comedi_device *dev,
-				  struct comedi_subdevice *s,
-				  struct comedi_insn *insn, unsigned int *data)
-{
-	int which = 0;
-
-	if (s == dev->subdevices + 4)
-		which = 1;
-
-	/* configure */
-	switch (data[0]) {
-	case INSN_CONFIG_DIO_OUTPUT:
-		s->io_bits = 0xff;
-		dt2801_writecmd(dev, DT_C_SET_DIGOUT);
-		break;
-	case INSN_CONFIG_DIO_INPUT:
-		s->io_bits = 0;
-		dt2801_writecmd(dev, DT_C_SET_DIGIN);
-		break;
-	case INSN_CONFIG_DIO_QUERY:
-		data[1] = s->io_bits ? COMEDI_OUTPUT : COMEDI_INPUT;
-		return insn->n;
-	default:
-		return -EINVAL;
-	}
-	dt2801_writedata(dev, which);
-
-	return 1;
-}
+static struct comedi_driver dt2801_driver = {
+	.driver_name	= "dt2801",
+	.module		= THIS_MODULE,
+	.attach		= dt2801_attach,
+	.detach		= dt2801_detach,
+};
+module_comedi_driver(dt2801_driver);
 
 MODULE_AUTHOR("Comedi http://www.comedi.org");
 MODULE_DESCRIPTION("Comedi low-level driver");
diff --git a/drivers/staging/comedi/drivers/dt2811.c b/drivers/staging/comedi/drivers/dt2811.c
index 0131d5225b5..106ffea30b9 100644
--- a/drivers/staging/comedi/drivers/dt2811.c
+++ b/drivers/staging/comedi/drivers/dt2811.c
@@ -211,61 +211,8 @@ struct dt2811_board {
 	const struct comedi_lrange *unip_5;
 };
 
-static const struct dt2811_board boardtypes[] = {
-	{"dt2811-pgh",
-	 &range_dt2811_pgh_ai_5_bipolar,
-	 &range_dt2811_pgh_ai_2_5_bipolar,
-	 &range_dt2811_pgh_ai_5_unipolar,
-	 },
-	{"dt2811-pgl",
-	 &range_dt2811_pgl_ai_5_bipolar,
-	 &range_dt2811_pgl_ai_2_5_bipolar,
-	 &range_dt2811_pgl_ai_5_unipolar,
-	 },
-};
-
 #define this_board ((const struct dt2811_board *)dev->board_ptr)
 
-static int dt2811_attach(struct comedi_device *dev,
-			 struct comedi_devconfig *it);
-static int dt2811_detach(struct comedi_device *dev);
-static struct comedi_driver driver_dt2811 = {
-	.driver_name = "dt2811",
-	.module = THIS_MODULE,
-	.attach = dt2811_attach,
-	.detach = dt2811_detach,
-	.board_name = &boardtypes[0].name,
-	.num_names = ARRAY_SIZE(boardtypes),
-	.offset = sizeof(struct dt2811_board),
-};
-
-static int __init driver_dt2811_init_module(void)
-{
-	return comedi_driver_register(&driver_dt2811);
-}
-
-static void __exit driver_dt2811_cleanup_module(void)
-{
-	comedi_driver_unregister(&driver_dt2811);
-}
-
-module_init(driver_dt2811_init_module);
-module_exit(driver_dt2811_cleanup_module);
-
-static int dt2811_ai_insn(struct comedi_device *dev, struct comedi_subdevice *s,
-			  struct comedi_insn *insn, unsigned int *data);
-static int dt2811_ao_insn(struct comedi_device *dev, struct comedi_subdevice *s,
-			  struct comedi_insn *insn, unsigned int *data);
-static int dt2811_ao_insn_read(struct comedi_device *dev,
-			       struct comedi_subdevice *s,
-			       struct comedi_insn *insn, unsigned int *data);
-static int dt2811_di_insn_bits(struct comedi_device *dev,
-			       struct comedi_subdevice *s,
-			       struct comedi_insn *insn, unsigned int *data);
-static int dt2811_do_insn_bits(struct comedi_device *dev,
-			       struct comedi_subdevice *s,
-			       struct comedi_insn *insn, unsigned int *data);
-
 enum { card_2811_pgh, card_2811_pgl };
 
 struct dt2811_private {
@@ -317,6 +264,120 @@ static irqreturn_t dt2811_interrupt(int irq, void *d)
 }
 #endif
 
+static int dt2811_ai_insn(struct comedi_device *dev, struct comedi_subdevice *s,
+			  struct comedi_insn *insn, unsigned int *data)
+{
+	int chan = CR_CHAN(insn->chanspec);
+	int timeout = DT2811_TIMEOUT;
+	int i;
+
+	for (i = 0; i < insn->n; i++) {
+		outb(chan, dev->iobase + DT2811_ADGCR);
+
+		while (timeout
+		       && inb(dev->iobase + DT2811_ADCSR) & DT2811_ADBUSY)
+			timeout--;
+		if (!timeout)
+			return -ETIME;
+
+		data[i] = inb(dev->iobase + DT2811_ADDATLO);
+		data[i] |= inb(dev->iobase + DT2811_ADDATHI) << 8;
+		data[i] &= 0xfff;
+	}
+
+	return i;
+}
+
+#if 0
+/* Wow.  This is code from the Comedi stone age.  But it hasn't been
+ * replaced, so I'll let it stay. */
+int dt2811_adtrig(kdev_t minor, comedi_adtrig *adtrig)
+{
+	struct comedi_device *dev = comedi_devices + minor;
+
+	if (adtrig->n < 1)
+		return 0;
+	dev->curadchan = adtrig->chan;
+	switch (dev->i_admode) {
+	case COMEDI_MDEMAND:
+		dev->ntrig = adtrig->n - 1;
+		/* not necessary */
+		/*printk("dt2811: AD soft trigger\n"); */
+		/*outb(DT2811_CLRERROR|DT2811_INTENB,
+			dev->iobase+DT2811_ADCSR); */
+		outb(dev->curadchan, dev->iobase + DT2811_ADGCR);
+		do_gettimeofday(&trigtime);
+		break;
+	case COMEDI_MCONTS:
+		dev->ntrig = adtrig->n;
+		break;
+	}
+
+	return 0;
+}
+#endif
+
+static int dt2811_ao_insn(struct comedi_device *dev, struct comedi_subdevice *s,
+			  struct comedi_insn *insn, unsigned int *data)
+{
+	int i;
+	int chan;
+
+	chan = CR_CHAN(insn->chanspec);
+
+	for (i = 0; i < insn->n; i++) {
+		outb(data[i] & 0xff, dev->iobase + DT2811_DADAT0LO + 2 * chan);
+		outb((data[i] >> 8) & 0xff,
+		     dev->iobase + DT2811_DADAT0HI + 2 * chan);
+		devpriv->ao_readback[chan] = data[i];
+	}
+
+	return i;
+}
+
+static int dt2811_ao_insn_read(struct comedi_device *dev,
+			       struct comedi_subdevice *s,
+			       struct comedi_insn *insn, unsigned int *data)
+{
+	int i;
+	int chan;
+
+	chan = CR_CHAN(insn->chanspec);
+
+	for (i = 0; i < insn->n; i++)
+		data[i] = devpriv->ao_readback[chan];
+
+	return i;
+}
+
+static int dt2811_di_insn_bits(struct comedi_device *dev,
+			       struct comedi_subdevice *s,
+			       struct comedi_insn *insn, unsigned int *data)
+{
+	if (insn->n != 2)
+		return -EINVAL;
+
+	data[1] = inb(dev->iobase + DT2811_DIO);
+
+	return 2;
+}
+
+static int dt2811_do_insn_bits(struct comedi_device *dev,
+			       struct comedi_subdevice *s,
+			       struct comedi_insn *insn, unsigned int *data)
+{
+	if (insn->n != 2)
+		return -EINVAL;
+
+	s->state &= ~data[0];
+	s->state |= data[0] & data[1];
+	outb(s->state, dev->iobase + DT2811_DIO);
+
+	data[1] = s->state;
+
+	return 2;
+}
+
 /*
   options[0]   Board base address
   options[1]   IRQ
@@ -337,7 +398,6 @@ static irqreturn_t dt2811_interrupt(int irq, void *d)
 		 1 == bipolar 2.5V  (-2.5V -- +2.5V)
 		 2 == unipolar 5V  (0V -- +5V)
 */
-
 static int dt2811_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 {
 	/* int i, irq; */
@@ -511,131 +571,38 @@ static int dt2811_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 	return 0;
 }
 
-static int dt2811_detach(struct comedi_device *dev)
+static void dt2811_detach(struct comedi_device *dev)
 {
-	printk(KERN_INFO "comedi%d: dt2811: remove\n", dev->minor);
-
 	if (dev->irq)
 		free_irq(dev->irq, dev);
 	if (dev->iobase)
 		release_region(dev->iobase, DT2811_SIZE);
-
-	return 0;
-}
-
-static int dt2811_ai_insn(struct comedi_device *dev, struct comedi_subdevice *s,
-			  struct comedi_insn *insn, unsigned int *data)
-{
-	int chan = CR_CHAN(insn->chanspec);
-	int timeout = DT2811_TIMEOUT;
-	int i;
-
-	for (i = 0; i < insn->n; i++) {
-		outb(chan, dev->iobase + DT2811_ADGCR);
-
-		while (timeout
-		       && inb(dev->iobase + DT2811_ADCSR) & DT2811_ADBUSY)
-			timeout--;
-		if (!timeout)
-			return -ETIME;
-
-		data[i] = inb(dev->iobase + DT2811_ADDATLO);
-		data[i] |= inb(dev->iobase + DT2811_ADDATHI) << 8;
-		data[i] &= 0xfff;
-	}
-
-	return i;
 }
 
-#if 0
-/* Wow.  This is code from the Comedi stone age.  But it hasn't been
- * replaced, so I'll let it stay. */
-int dt2811_adtrig(kdev_t minor, comedi_adtrig *adtrig)
-{
-	struct comedi_device *dev = comedi_devices + minor;
-
-	if (adtrig->n < 1)
-		return 0;
-	dev->curadchan = adtrig->chan;
-	switch (dev->i_admode) {
-	case COMEDI_MDEMAND:
-		dev->ntrig = adtrig->n - 1;
-		/* not necessary */
-		/*printk("dt2811: AD soft trigger\n"); */
-		/*outb(DT2811_CLRERROR|DT2811_INTENB,
-			dev->iobase+DT2811_ADCSR); */
-		outb(dev->curadchan, dev->iobase + DT2811_ADGCR);
-		do_gettimeofday(&trigtime);
-		break;
-	case COMEDI_MCONTS:
-		dev->ntrig = adtrig->n;
-		break;
-	}
-
-	return 0;
-}
-#endif
-
-static int dt2811_ao_insn(struct comedi_device *dev, struct comedi_subdevice *s,
-			  struct comedi_insn *insn, unsigned int *data)
-{
-	int i;
-	int chan;
-
-	chan = CR_CHAN(insn->chanspec);
-
-	for (i = 0; i < insn->n; i++) {
-		outb(data[i] & 0xff, dev->iobase + DT2811_DADAT0LO + 2 * chan);
-		outb((data[i] >> 8) & 0xff,
-		     dev->iobase + DT2811_DADAT0HI + 2 * chan);
-		devpriv->ao_readback[chan] = data[i];
-	}
-
-	return i;
-}
-
-static int dt2811_ao_insn_read(struct comedi_device *dev,
-			       struct comedi_subdevice *s,
-			       struct comedi_insn *insn, unsigned int *data)
-{
-	int i;
-	int chan;
-
-	chan = CR_CHAN(insn->chanspec);
-
-	for (i = 0; i < insn->n; i++)
-		data[i] = devpriv->ao_readback[chan];
-
-	return i;
-}
-
-static int dt2811_di_insn_bits(struct comedi_device *dev,
-			       struct comedi_subdevice *s,
-			       struct comedi_insn *insn, unsigned int *data)
-{
-	if (insn->n != 2)
-		return -EINVAL;
-
-	data[1] = inb(dev->iobase + DT2811_DIO);
-
-	return 2;
-}
-
-static int dt2811_do_insn_bits(struct comedi_device *dev,
-			       struct comedi_subdevice *s,
-			       struct comedi_insn *insn, unsigned int *data)
-{
-	if (insn->n != 2)
-		return -EINVAL;
-
-	s->state &= ~data[0];
-	s->state |= data[0] & data[1];
-	outb(s->state, dev->iobase + DT2811_DIO);
-
-	data[1] = s->state;
+static const struct dt2811_board boardtypes[] = {
+	{
+		.name		= "dt2811-pgh",
+		.bip_5		= &range_dt2811_pgh_ai_5_bipolar,
+		.bip_2_5	= &range_dt2811_pgh_ai_2_5_bipolar,
+		.unip_5		= &range_dt2811_pgh_ai_5_unipolar,
+	}, {
+		.name		= "dt2811-pgl",
+		.bip_5		= &range_dt2811_pgl_ai_5_bipolar,
+		.bip_2_5	= &range_dt2811_pgl_ai_2_5_bipolar,
+		.unip_5		= &range_dt2811_pgl_ai_5_unipolar,
+	},
+};
 
-	return 2;
-}
+static struct comedi_driver dt2811_driver = {
+	.driver_name	= "dt2811",
+	.module		= THIS_MODULE,
+	.attach		= dt2811_attach,
+	.detach		= dt2811_detach,
+	.board_name	= &boardtypes[0].name,
+	.num_names	= ARRAY_SIZE(boardtypes),
+	.offset		= sizeof(struct dt2811_board),
+};
+module_comedi_driver(dt2811_driver);
 
 MODULE_AUTHOR("Comedi http://www.comedi.org");
 MODULE_DESCRIPTION("Comedi low-level driver");
diff --git a/drivers/staging/comedi/drivers/dt2814.c b/drivers/staging/comedi/drivers/dt2814.c
index 1c6248cf592..fa4ade61be5 100644
--- a/drivers/staging/comedi/drivers/dt2814.c
+++ b/drivers/staging/comedi/drivers/dt2814.c
@@ -60,31 +60,6 @@ addition, the clock does not seem to be very accurate.
 #define DT2814_ENB 0x10
 #define DT2814_CHANMASK 0x0f
 
-static int dt2814_attach(struct comedi_device *dev,
-			 struct comedi_devconfig *it);
-static int dt2814_detach(struct comedi_device *dev);
-static struct comedi_driver driver_dt2814 = {
-	.driver_name = "dt2814",
-	.module = THIS_MODULE,
-	.attach = dt2814_attach,
-	.detach = dt2814_detach,
-};
-
-static int __init driver_dt2814_init_module(void)
-{
-	return comedi_driver_register(&driver_dt2814);
-}
-
-static void __exit driver_dt2814_cleanup_module(void)
-{
-	comedi_driver_unregister(&driver_dt2814);
-}
-
-module_init(driver_dt2814_init_module);
-module_exit(driver_dt2814_cleanup_module);
-
-static irqreturn_t dt2814_interrupt(int irq, void *dev);
-
 struct dt2814_private {
 
 	int ntrig;
@@ -260,6 +235,45 @@ static int dt2814_ai_cmd(struct comedi_device *dev, struct comedi_subdevice *s)
 
 }
 
+static irqreturn_t dt2814_interrupt(int irq, void *d)
+{
+	int lo, hi;
+	struct comedi_device *dev = d;
+	struct comedi_subdevice *s;
+	int data;
+
+	if (!dev->attached) {
+		comedi_error(dev, "spurious interrupt");
+		return IRQ_HANDLED;
+	}
+
+	s = dev->subdevices + 0;
+
+	hi = inb(dev->iobase + DT2814_DATA);
+	lo = inb(dev->iobase + DT2814_DATA);
+
+	data = (hi << 4) | (lo >> 4);
+
+	if (!(--devpriv->ntrig)) {
+		int i;
+
+		outb(0, dev->iobase + DT2814_CSR);
+		/* note: turning off timed mode triggers another
+		   sample. */
+
+		for (i = 0; i < DT2814_TIMEOUT; i++) {
+			if (inb(dev->iobase + DT2814_CSR) & DT2814_FINISH)
+				break;
+		}
+		inb(dev->iobase + DT2814_DATA);
+		inb(dev->iobase + DT2814_DATA);
+
+		s->async->events |= COMEDI_CB_EOA;
+	}
+	comedi_event(dev, s);
+	return IRQ_HANDLED;
+}
+
 static int dt2814_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 {
 	int i, irq;
@@ -347,57 +361,21 @@ static int dt2814_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 	return 0;
 }
 
-static int dt2814_detach(struct comedi_device *dev)
+static void dt2814_detach(struct comedi_device *dev)
 {
-	printk(KERN_INFO "comedi%d: dt2814: remove\n", dev->minor);
-
 	if (dev->irq)
 		free_irq(dev->irq, dev);
-
 	if (dev->iobase)
 		release_region(dev->iobase, DT2814_SIZE);
-
-	return 0;
 }
 
-static irqreturn_t dt2814_interrupt(int irq, void *d)
-{
-	int lo, hi;
-	struct comedi_device *dev = d;
-	struct comedi_subdevice *s;
-	int data;
-
-	if (!dev->attached) {
-		comedi_error(dev, "spurious interrupt");
-		return IRQ_HANDLED;
-	}
-
-	s = dev->subdevices + 0;
-
-	hi = inb(dev->iobase + DT2814_DATA);
-	lo = inb(dev->iobase + DT2814_DATA);
-
-	data = (hi << 4) | (lo >> 4);
-
-	if (!(--devpriv->ntrig)) {
-		int i;
-
-		outb(0, dev->iobase + DT2814_CSR);
-		/* note: turning off timed mode triggers another
-		   sample. */
-
-		for (i = 0; i < DT2814_TIMEOUT; i++) {
-			if (inb(dev->iobase + DT2814_CSR) & DT2814_FINISH)
-				break;
-		}
-		inb(dev->iobase + DT2814_DATA);
-		inb(dev->iobase + DT2814_DATA);
-
-		s->async->events |= COMEDI_CB_EOA;
-	}
-	comedi_event(dev, s);
-	return IRQ_HANDLED;
-}
+static struct comedi_driver dt2814_driver = {
+	.driver_name	= "dt2814",
+	.module		= THIS_MODULE,
+	.attach		= dt2814_attach,
+	.detach		= dt2814_detach,
+};
+module_comedi_driver(dt2814_driver);
 
 MODULE_AUTHOR("Comedi http://www.comedi.org");
 MODULE_DESCRIPTION("Comedi low-level driver");
diff --git a/drivers/staging/comedi/drivers/dt2815.c b/drivers/staging/comedi/drivers/dt2815.c
index 4155da43fd5..bbab712be4b 100644
--- a/drivers/staging/comedi/drivers/dt2815.c
+++ b/drivers/staging/comedi/drivers/dt2815.c
@@ -72,31 +72,6 @@ static const struct comedi_lrange
 #define DT2815_DATA 0
 #define DT2815_STATUS 1
 
-static int dt2815_attach(struct comedi_device *dev,
-			 struct comedi_devconfig *it);
-static int dt2815_detach(struct comedi_device *dev);
-static struct comedi_driver driver_dt2815 = {
-	.driver_name = "dt2815",
-	.module = THIS_MODULE,
-	.attach = dt2815_attach,
-	.detach = dt2815_detach,
-};
-
-static int __init driver_dt2815_init_module(void)
-{
-	return comedi_driver_register(&driver_dt2815);
-}
-
-static void __exit driver_dt2815_cleanup_module(void)
-{
-	comedi_driver_unregister(&driver_dt2815);
-}
-
-module_init(driver_dt2815_init_module);
-module_exit(driver_dt2815_cleanup_module);
-
-static void dt2815_free_resources(struct comedi_device *dev);
-
 struct dt2815_private {
 
 	const struct comedi_lrange *range_type_list[8];
@@ -252,20 +227,19 @@ static int dt2815_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 	return 0;
 }
 
-static void dt2815_free_resources(struct comedi_device *dev)
+static void dt2815_detach(struct comedi_device *dev)
 {
 	if (dev->iobase)
 		release_region(dev->iobase, DT2815_SIZE);
 }
 
-static int dt2815_detach(struct comedi_device *dev)
-{
-	printk(KERN_INFO "comedi%d: dt2815: remove\n", dev->minor);
-
-	dt2815_free_resources(dev);
-
-	return 0;
-}
+static struct comedi_driver dt2815_driver = {
+	.driver_name	= "dt2815",
+	.module		= THIS_MODULE,
+	.attach		= dt2815_attach,
+	.detach		= dt2815_detach,
+};
+module_comedi_driver(dt2815_driver);
 
 MODULE_AUTHOR("Comedi http://www.comedi.org");
 MODULE_DESCRIPTION("Comedi low-level driver");
diff --git a/drivers/staging/comedi/drivers/dt2817.c b/drivers/staging/comedi/drivers/dt2817.c
index 99c1584153d..1ee10e7bf1d 100644
--- a/drivers/staging/comedi/drivers/dt2817.c
+++ b/drivers/staging/comedi/drivers/dt2817.c
@@ -47,29 +47,6 @@ Configuration options:
 #define DT2817_CR 0
 #define DT2817_DATA 1
 
-static int dt2817_attach(struct comedi_device *dev,
-			 struct comedi_devconfig *it);
-static int dt2817_detach(struct comedi_device *dev);
-static struct comedi_driver driver_dt2817 = {
-	.driver_name = "dt2817",
-	.module = THIS_MODULE,
-	.attach = dt2817_attach,
-	.detach = dt2817_detach,
-};
-
-static int __init driver_dt2817_init_module(void)
-{
-	return comedi_driver_register(&driver_dt2817);
-}
-
-static void __exit driver_dt2817_cleanup_module(void)
-{
-	comedi_driver_unregister(&driver_dt2817);
-}
-
-module_init(driver_dt2817_init_module);
-module_exit(driver_dt2817_cleanup_module);
-
 static int dt2817_dio_insn_config(struct comedi_device *dev,
 				  struct comedi_subdevice *s,
 				  struct comedi_insn *insn, unsigned int *data)
@@ -182,16 +159,20 @@ static int dt2817_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 	return 0;
 }
 
-static int dt2817_detach(struct comedi_device *dev)
+static void dt2817_detach(struct comedi_device *dev)
 {
-	printk(KERN_INFO "comedi%d: dt2817: remove\n", dev->minor);
-
 	if (dev->iobase)
 		release_region(dev->iobase, DT2817_SIZE);
-
-	return 0;
 }
 
+static struct comedi_driver dt2817_driver = {
+	.driver_name	= "dt2817",
+	.module		= THIS_MODULE,
+	.attach		= dt2817_attach,
+	.detach		= dt2817_detach,
+};
+module_comedi_driver(dt2817_driver);
+
 MODULE_AUTHOR("Comedi http://www.comedi.org");
 MODULE_DESCRIPTION("Comedi low-level driver");
 MODULE_LICENSE("GPL");
diff --git a/drivers/staging/comedi/drivers/dt282x.c b/drivers/staging/comedi/drivers/dt282x.c
index 95ebc267bb7..736d8facaee 100644
--- a/drivers/staging/comedi/drivers/dt282x.c
+++ b/drivers/staging/comedi/drivers/dt282x.c
@@ -221,136 +221,6 @@ struct dt282x_board {
 	int dabits;
 };
 
-static const struct dt282x_board boardtypes[] = {
-	{.name = "dt2821",
-	 .adbits = 12,
-	 .adchan_se = 16,
-	 .adchan_di = 8,
-	 .ai_speed = 20000,
-	 .ispgl = 0,
-	 .dachan = 2,
-	 .dabits = 12,
-	 },
-	{.name = "dt2821-f",
-	 .adbits = 12,
-	 .adchan_se = 16,
-	 .adchan_di = 8,
-	 .ai_speed = 6500,
-	 .ispgl = 0,
-	 .dachan = 2,
-	 .dabits = 12,
-	 },
-	{.name = "dt2821-g",
-	 .adbits = 12,
-	 .adchan_se = 16,
-	 .adchan_di = 8,
-	 .ai_speed = 4000,
-	 .ispgl = 0,
-	 .dachan = 2,
-	 .dabits = 12,
-	 },
-	{.name = "dt2823",
-	 .adbits = 16,
-	 .adchan_se = 0,
-	 .adchan_di = 4,
-	 .ai_speed = 10000,
-	 .ispgl = 0,
-	 .dachan = 2,
-	 .dabits = 16,
-	 },
-	{.name = "dt2824-pgh",
-	 .adbits = 12,
-	 .adchan_se = 16,
-	 .adchan_di = 8,
-	 .ai_speed = 20000,
-	 .ispgl = 0,
-	 .dachan = 0,
-	 .dabits = 0,
-	 },
-	{.name = "dt2824-pgl",
-	 .adbits = 12,
-	 .adchan_se = 16,
-	 .adchan_di = 8,
-	 .ai_speed = 20000,
-	 .ispgl = 1,
-	 .dachan = 0,
-	 .dabits = 0,
-	 },
-	{.name = "dt2825",
-	 .adbits = 12,
-	 .adchan_se = 16,
-	 .adchan_di = 8,
-	 .ai_speed = 20000,
-	 .ispgl = 1,
-	 .dachan = 2,
-	 .dabits = 12,
-	 },
-	{.name = "dt2827",
-	 .adbits = 16,
-	 .adchan_se = 0,
-	 .adchan_di = 4,
-	 .ai_speed = 10000,
-	 .ispgl = 0,
-	 .dachan = 2,
-	 .dabits = 12,
-	 },
-	{.name = "dt2828",
-	 .adbits = 12,
-	 .adchan_se = 4,
-	 .adchan_di = 0,
-	 .ai_speed = 10000,
-	 .ispgl = 0,
-	 .dachan = 2,
-	 .dabits = 12,
-	 },
-	{.name = "dt2829",
-	 .adbits = 16,
-	 .adchan_se = 8,
-	 .adchan_di = 0,
-	 .ai_speed = 33250,
-	 .ispgl = 0,
-	 .dachan = 2,
-	 .dabits = 16,
-	 },
-	{.name = "dt21-ez",
-	 .adbits = 12,
-	 .adchan_se = 16,
-	 .adchan_di = 8,
-	 .ai_speed = 10000,
-	 .ispgl = 0,
-	 .dachan = 2,
-	 .dabits = 12,
-	 },
-	{.name = "dt23-ez",
-	 .adbits = 16,
-	 .adchan_se = 16,
-	 .adchan_di = 8,
-	 .ai_speed = 10000,
-	 .ispgl = 0,
-	 .dachan = 0,
-	 .dabits = 0,
-	 },
-	{.name = "dt24-ez",
-	 .adbits = 12,
-	 .adchan_se = 16,
-	 .adchan_di = 8,
-	 .ai_speed = 10000,
-	 .ispgl = 0,
-	 .dachan = 0,
-	 .dabits = 0,
-	 },
-	{.name = "dt24-ez-pgl",
-	 .adbits = 12,
-	 .adchan_se = 16,
-	 .adchan_di = 8,
-	 .ai_speed = 10000,
-	 .ispgl = 1,
-	 .dachan = 0,
-	 .dabits = 0,
-	 },
-};
-
-#define n_boardtypes (sizeof(boardtypes)/sizeof(struct dt282x_board))
 #define this_board ((const struct dt282x_board *)dev->board_ptr)
 
 struct dt282x_private {
@@ -411,33 +281,6 @@ struct dt282x_private {
 			b					\
 	} while (0)
 
-static int dt282x_attach(struct comedi_device *dev,
-			 struct comedi_devconfig *it);
-static int dt282x_detach(struct comedi_device *dev);
-static struct comedi_driver driver_dt282x = {
-	.driver_name = "dt282x",
-	.module = THIS_MODULE,
-	.attach = dt282x_attach,
-	.detach = dt282x_detach,
-	.board_name = &boardtypes[0].name,
-	.num_names = n_boardtypes,
-	.offset = sizeof(struct dt282x_board),
-};
-
-static int __init driver_dt282x_init_module(void)
-{
-	return comedi_driver_register(&driver_dt282x);
-}
-
-static void __exit driver_dt282x_cleanup_module(void)
-{
-	comedi_driver_unregister(&driver_dt282x);
-}
-
-module_init(driver_dt282x_init_module);
-module_exit(driver_dt282x_cleanup_module);
-
-static void free_resources(struct comedi_device *dev);
 static int prep_ai_dma(struct comedi_device *dev, int chan, int size);
 static int prep_ao_dma(struct comedi_device *dev, int chan, int size);
 static int dt282x_ai_cancel(struct comedi_device *dev,
@@ -1271,6 +1114,52 @@ enum {  /* i/o base, irq, dma channels */
 	opt_ai_range, opt_ao0_range, opt_ao1_range,	/* range */
 };
 
+static int dt282x_grab_dma(struct comedi_device *dev, int dma1, int dma2)
+{
+	int ret;
+
+	devpriv->usedma = 0;
+
+	if (!dma1 && !dma2) {
+		printk(KERN_ERR " (no dma)");
+		return 0;
+	}
+
+	if (dma1 == dma2 || dma1 < 5 || dma2 < 5 || dma1 > 7 || dma2 > 7)
+		return -EINVAL;
+
+	if (dma2 < dma1) {
+		int i;
+		i = dma1;
+		dma1 = dma2;
+		dma2 = i;
+	}
+
+	ret = request_dma(dma1, "dt282x A");
+	if (ret)
+		return -EBUSY;
+	devpriv->dma[0].chan = dma1;
+
+	ret = request_dma(dma2, "dt282x B");
+	if (ret)
+		return -EBUSY;
+	devpriv->dma[1].chan = dma2;
+
+	devpriv->dma_maxsize = PAGE_SIZE;
+	devpriv->dma[0].buf = (void *)__get_free_page(GFP_KERNEL | GFP_DMA);
+	devpriv->dma[1].buf = (void *)__get_free_page(GFP_KERNEL | GFP_DMA);
+	if (!devpriv->dma[0].buf || !devpriv->dma[1].buf) {
+		printk(KERN_ERR " can't get DMA memory");
+		return -ENOMEM;
+	}
+
+	printk(KERN_INFO " (dma=%d,%d)", dma1, dma2);
+
+	devpriv->usedma = 1;
+
+	return 0;
+}
+
 /*
    options:
    0	i/o base
@@ -1442,7 +1331,7 @@ static int dt282x_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 	return 0;
 }
 
-static void free_resources(struct comedi_device *dev)
+static void dt282x_detach(struct comedi_device *dev)
 {
 	if (dev->irq)
 		free_irq(dev->irq, dev);
@@ -1460,60 +1349,146 @@ static void free_resources(struct comedi_device *dev)
 	}
 }
 
-static int dt282x_detach(struct comedi_device *dev)
-{
-	printk(KERN_INFO "comedi%d: dt282x: remove\n", dev->minor);
-
-	free_resources(dev);
-
-	return 0;
-}
-
-static int dt282x_grab_dma(struct comedi_device *dev, int dma1, int dma2)
-{
-	int ret;
-
-	devpriv->usedma = 0;
-
-	if (!dma1 && !dma2) {
-		printk(KERN_ERR " (no dma)");
-		return 0;
-	}
-
-	if (dma1 == dma2 || dma1 < 5 || dma2 < 5 || dma1 > 7 || dma2 > 7)
-		return -EINVAL;
-
-	if (dma2 < dma1) {
-		int i;
-		i = dma1;
-		dma1 = dma2;
-		dma2 = i;
-	}
-
-	ret = request_dma(dma1, "dt282x A");
-	if (ret)
-		return -EBUSY;
-	devpriv->dma[0].chan = dma1;
-
-	ret = request_dma(dma2, "dt282x B");
-	if (ret)
-		return -EBUSY;
-	devpriv->dma[1].chan = dma2;
-
-	devpriv->dma_maxsize = PAGE_SIZE;
-	devpriv->dma[0].buf = (void *)__get_free_page(GFP_KERNEL | GFP_DMA);
-	devpriv->dma[1].buf = (void *)__get_free_page(GFP_KERNEL | GFP_DMA);
-	if (!devpriv->dma[0].buf || !devpriv->dma[1].buf) {
-		printk(KERN_ERR " can't get DMA memory");
-		return -ENOMEM;
-	}
-
-	printk(KERN_INFO " (dma=%d,%d)", dma1, dma2);
-
-	devpriv->usedma = 1;
+static const struct dt282x_board boardtypes[] = {
+	{
+		.name		= "dt2821",
+		.adbits		= 12,
+		.adchan_se	= 16,
+		.adchan_di	= 8,
+		.ai_speed	= 20000,
+		.ispgl		= 0,
+		.dachan		= 2,
+		.dabits		= 12,
+	}, {
+		.name		= "dt2821-f",
+		.adbits		= 12,
+		.adchan_se	= 16,
+		.adchan_di	= 8,
+		.ai_speed	= 6500,
+		.ispgl		= 0,
+		.dachan		= 2,
+		.dabits		= 12,
+	}, {
+		.name		= "dt2821-g",
+		.adbits		= 12,
+		.adchan_se	= 16,
+		.adchan_di	= 8,
+		.ai_speed	= 4000,
+		.ispgl		= 0,
+		.dachan		= 2,
+		.dabits		= 12,
+	}, {
+		.name		= "dt2823",
+		.adbits		= 16,
+		.adchan_se	= 0,
+		.adchan_di	= 4,
+		.ai_speed	= 10000,
+		.ispgl		= 0,
+		.dachan		= 2,
+		.dabits		= 16,
+	}, {
+		.name		= "dt2824-pgh",
+		.adbits		= 12,
+		.adchan_se	= 16,
+		.adchan_di	= 8,
+		.ai_speed	= 20000,
+		.ispgl		= 0,
+		.dachan		= 0,
+		.dabits		= 0,
+	}, {
+		.name		= "dt2824-pgl",
+		.adbits		= 12,
+		.adchan_se	= 16,
+		.adchan_di	= 8,
+		.ai_speed	= 20000,
+		.ispgl		= 1,
+		.dachan		= 0,
+		.dabits		= 0,
+	}, {
+		.name		= "dt2825",
+		.adbits		= 12,
+		.adchan_se	= 16,
+		.adchan_di	= 8,
+		.ai_speed	= 20000,
+		.ispgl		= 1,
+		.dachan		= 2,
+		.dabits		= 12,
+	}, {
+		.name		= "dt2827",
+		.adbits		= 16,
+		.adchan_se	= 0,
+		.adchan_di	= 4,
+		.ai_speed	= 10000,
+		.ispgl		= 0,
+		.dachan		= 2,
+		.dabits		= 12,
+	}, {
+		.name		= "dt2828",
+		.adbits		= 12,
+		.adchan_se	= 4,
+		.adchan_di	= 0,
+		.ai_speed	= 10000,
+		.ispgl		= 0,
+		.dachan		= 2,
+		.dabits		= 12,
+	}, {
+		.name		= "dt2829",
+		.adbits		= 16,
+		.adchan_se	= 8,
+		.adchan_di	= 0,
+		.ai_speed	= 33250,
+		.ispgl		= 0,
+		.dachan		= 2,
+		.dabits		= 16,
+	}, {
+		.name		= "dt21-ez",
+		.adbits		= 12,
+		.adchan_se	= 16,
+		.adchan_di	= 8,
+		.ai_speed	= 10000,
+		.ispgl		= 0,
+		.dachan		= 2,
+		.dabits		= 12,
+	}, {
+		.name		= "dt23-ez",
+		.adbits		= 16,
+		.adchan_se	= 16,
+		.adchan_di	= 8,
+		.ai_speed	= 10000,
+		.ispgl		= 0,
+		.dachan		= 0,
+		.dabits		= 0,
+	}, {
+		.name		= "dt24-ez",
+		.adbits		= 12,
+		.adchan_se	= 16,
+		.adchan_di	= 8,
+		.ai_speed	= 10000,
+		.ispgl		= 0,
+		.dachan		= 0,
+		.dabits		= 0,
+	}, {
+		.name		= "dt24-ez-pgl",
+		.adbits		= 12,
+		.adchan_se	= 16,
+		.adchan_di	= 8,
+		.ai_speed	= 10000,
+		.ispgl		= 1,
+		.dachan		= 0,
+		.dabits		= 0,
+	},
+};
 
-	return 0;
-}
+static struct comedi_driver dt282x_driver = {
+	.driver_name	= "dt282x",
+	.module		= THIS_MODULE,
+	.attach		= dt282x_attach,
+	.detach		= dt282x_detach,
+	.board_name	= &boardtypes[0].name,
+	.num_names	= ARRAY_SIZE(boardtypes),
+	.offset		= sizeof(struct dt282x_board),
+};
+module_comedi_driver(dt282x_driver);
 
 MODULE_AUTHOR("Comedi http://www.comedi.org");
 MODULE_DESCRIPTION("Comedi low-level driver");
diff --git a/drivers/staging/comedi/drivers/dt3000.c b/drivers/staging/comedi/drivers/dt3000.c
index 0a7979e5299..0d273269b57 100644
--- a/drivers/staging/comedi/drivers/dt3000.c
+++ b/drivers/staging/comedi/drivers/dt3000.c
@@ -164,19 +164,6 @@ static const struct dt3k_boardtype dt3k_boardtypes[] = {
 #define n_dt3k_boards sizeof(dt3k_boardtypes)/sizeof(struct dt3k_boardtype)
 #define this_board ((const struct dt3k_boardtype *)dev->board_ptr)
 
-static DEFINE_PCI_DEVICE_TABLE(dt3k_pci_table) = {
-	{ PCI_DEVICE(PCI_VENDOR_ID_DT, 0x0022) },
-	{ PCI_DEVICE(PCI_VENDOR_ID_DT, 0x0027) },
-	{ PCI_DEVICE(PCI_VENDOR_ID_DT, 0x0023) },
-	{ PCI_DEVICE(PCI_VENDOR_ID_DT, 0x0024) },
-	{ PCI_DEVICE(PCI_VENDOR_ID_DT, 0x0028) },
-	{ PCI_DEVICE(PCI_VENDOR_ID_DT, 0x0025) },
-	{ PCI_DEVICE(PCI_VENDOR_ID_DT, 0x0026) },
-	{ 0 }
-};
-
-MODULE_DEVICE_TABLE(pci, dt3k_pci_table);
-
 #define DT3000_SIZE		(4*0x1000)
 
 /* dual-ported RAM location definitions */
@@ -276,54 +263,6 @@ struct dt3k_private {
 
 #define devpriv ((struct dt3k_private *)dev->private)
 
-static int dt3000_attach(struct comedi_device *dev,
-			 struct comedi_devconfig *it);
-static int dt3000_detach(struct comedi_device *dev);
-static struct comedi_driver driver_dt3000 = {
-	.driver_name = "dt3000",
-	.module = THIS_MODULE,
-	.attach = dt3000_attach,
-	.detach = dt3000_detach,
-};
-
-static int __devinit driver_dt3000_pci_probe(struct pci_dev *dev,
-					     const struct pci_device_id *ent)
-{
-	return comedi_pci_auto_config(dev, driver_dt3000.driver_name);
-}
-
-static void __devexit driver_dt3000_pci_remove(struct pci_dev *dev)
-{
-	comedi_pci_auto_unconfig(dev);
-}
-
-static struct pci_driver driver_dt3000_pci_driver = {
-	.id_table = dt3k_pci_table,
-	.probe = &driver_dt3000_pci_probe,
-	.remove = __devexit_p(&driver_dt3000_pci_remove)
-};
-
-static int __init driver_dt3000_init_module(void)
-{
-	int retval;
-
-	retval = comedi_driver_register(&driver_dt3000);
-	if (retval < 0)
-		return retval;
-
-	driver_dt3000_pci_driver.name = (char *)driver_dt3000.driver_name;
-	return pci_register_driver(&driver_dt3000_pci_driver);
-}
-
-static void __exit driver_dt3000_cleanup_module(void)
-{
-	pci_unregister_driver(&driver_dt3000_pci_driver);
-	comedi_driver_unregister(&driver_dt3000);
-}
-
-module_init(driver_dt3000_init_module);
-module_exit(driver_dt3000_cleanup_module);
-
 static void dt3k_ai_empty_fifo(struct comedi_device *dev,
 			       struct comedi_subdevice *s);
 static int dt3k_ns_to_timer(unsigned int timer_base, unsigned int *arg,
@@ -841,7 +780,77 @@ static int dt3k_mem_insn_read(struct comedi_device *dev,
 	return i;
 }
 
-static int dt_pci_probe(struct comedi_device *dev, int bus, int slot);
+static int setup_pci(struct comedi_device *dev)
+{
+	resource_size_t addr;
+	int ret;
+
+	ret = comedi_pci_enable(devpriv->pci_dev, "dt3000");
+	if (ret < 0)
+		return ret;
+
+	addr = pci_resource_start(devpriv->pci_dev, 0);
+	devpriv->phys_addr = addr;
+	devpriv->io_addr = ioremap(devpriv->phys_addr, DT3000_SIZE);
+	if (!devpriv->io_addr)
+		return -ENOMEM;
+#if DEBUG
+	printk("0x%08llx mapped to %p, ",
+	       (unsigned long long)devpriv->phys_addr, devpriv->io_addr);
+#endif
+
+	return 0;
+}
+
+static struct pci_dev *dt_pci_find_device(struct pci_dev *from, int *board)
+{
+	int i;
+
+	for (from = pci_get_device(PCI_VENDOR_ID_DT, PCI_ANY_ID, from);
+	     from != NULL;
+	     from = pci_get_device(PCI_VENDOR_ID_DT, PCI_ANY_ID, from)) {
+		for (i = 0; i < n_dt3k_boards; i++) {
+			if (from->device == dt3k_boardtypes[i].device_id) {
+				*board = i;
+				return from;
+			}
+		}
+		printk
+		    ("unknown Data Translation PCI device found with device_id=0x%04x\n",
+		     from->device);
+	}
+	*board = -1;
+	return from;
+}
+
+static int dt_pci_probe(struct comedi_device *dev, int bus, int slot)
+{
+	int board;
+	int ret;
+	struct pci_dev *pcidev;
+
+	pcidev = NULL;
+	while ((pcidev = dt_pci_find_device(pcidev, &board)) != NULL) {
+		if ((bus == 0 && slot == 0) ||
+		    (pcidev->bus->number == bus &&
+		     PCI_SLOT(pcidev->devfn) == slot)) {
+			break;
+		}
+	}
+	devpriv->pci_dev = pcidev;
+
+	if (board >= 0)
+		dev->board_ptr = dt3k_boardtypes + board;
+
+	if (!devpriv->pci_dev)
+		return 0;
+
+	ret = setup_pci(dev);
+	if (ret < 0)
+		return ret;
+
+	return 1;
+}
 
 static int dt3000_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 {
@@ -935,11 +944,10 @@ static int dt3000_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 	return 0;
 }
 
-static int dt3000_detach(struct comedi_device *dev)
+static void dt3000_detach(struct comedi_device *dev)
 {
 	if (dev->irq)
 		free_irq(dev->irq, dev);
-
 	if (devpriv) {
 		if (devpriv->pci_dev) {
 			if (devpriv->phys_addr)
@@ -949,85 +957,45 @@ static int dt3000_detach(struct comedi_device *dev)
 		if (devpriv->io_addr)
 			iounmap(devpriv->io_addr);
 	}
-	/* XXX */
-
-	return 0;
 }
 
-static struct pci_dev *dt_pci_find_device(struct pci_dev *from, int *board);
-static int setup_pci(struct comedi_device *dev);
+static struct comedi_driver dt3000_driver = {
+	.driver_name	= "dt3000",
+	.module		= THIS_MODULE,
+	.attach		= dt3000_attach,
+	.detach		= dt3000_detach,
+};
 
-static int dt_pci_probe(struct comedi_device *dev, int bus, int slot)
+static int __devinit dt3000_pci_probe(struct pci_dev *dev,
+				      const struct pci_device_id *ent)
 {
-	int board;
-	int ret;
-	struct pci_dev *pcidev;
-
-	pcidev = NULL;
-	while ((pcidev = dt_pci_find_device(pcidev, &board)) != NULL) {
-		if ((bus == 0 && slot == 0) ||
-		    (pcidev->bus->number == bus &&
-		     PCI_SLOT(pcidev->devfn) == slot)) {
-			break;
-		}
-	}
-	devpriv->pci_dev = pcidev;
-
-	if (board >= 0)
-		dev->board_ptr = dt3k_boardtypes + board;
-
-	if (!devpriv->pci_dev)
-		return 0;
-
-	ret = setup_pci(dev);
-	if (ret < 0)
-		return ret;
-
-	return 1;
+	return comedi_pci_auto_config(dev, &dt3000_driver);
 }
 
-static int setup_pci(struct comedi_device *dev)
+static void __devexit dt3000_pci_remove(struct pci_dev *dev)
 {
-	resource_size_t addr;
-	int ret;
-
-	ret = comedi_pci_enable(devpriv->pci_dev, "dt3000");
-	if (ret < 0)
-		return ret;
-
-	addr = pci_resource_start(devpriv->pci_dev, 0);
-	devpriv->phys_addr = addr;
-	devpriv->io_addr = ioremap(devpriv->phys_addr, DT3000_SIZE);
-	if (!devpriv->io_addr)
-		return -ENOMEM;
-#if DEBUG
-	printk("0x%08llx mapped to %p, ",
-	       (unsigned long long)devpriv->phys_addr, devpriv->io_addr);
-#endif
-
-	return 0;
+	comedi_pci_auto_unconfig(dev);
 }
 
-static struct pci_dev *dt_pci_find_device(struct pci_dev *from, int *board)
-{
-	int i;
+static DEFINE_PCI_DEVICE_TABLE(dt3000_pci_table) = {
+	{ PCI_DEVICE(PCI_VENDOR_ID_DT, 0x0022) },
+	{ PCI_DEVICE(PCI_VENDOR_ID_DT, 0x0027) },
+	{ PCI_DEVICE(PCI_VENDOR_ID_DT, 0x0023) },
+	{ PCI_DEVICE(PCI_VENDOR_ID_DT, 0x0024) },
+	{ PCI_DEVICE(PCI_VENDOR_ID_DT, 0x0028) },
+	{ PCI_DEVICE(PCI_VENDOR_ID_DT, 0x0025) },
+	{ PCI_DEVICE(PCI_VENDOR_ID_DT, 0x0026) },
+	{ 0 }
+};
+MODULE_DEVICE_TABLE(pci, dt3000_pci_table);
 
-	for (from = pci_get_device(PCI_VENDOR_ID_DT, PCI_ANY_ID, from);
-	     from != NULL;
-	     from = pci_get_device(PCI_VENDOR_ID_DT, PCI_ANY_ID, from)) {
-		for (i = 0; i < n_dt3k_boards; i++) {
-			if (from->device == dt3k_boardtypes[i].device_id) {
-				*board = i;
-				return from;
-			}
-		}
-		printk
-		    ("unknown Data Translation PCI device found with device_id=0x%04x\n",
-		     from->device);
-	}
-	*board = -1;
-	return from;
-}
+static struct pci_driver dt3000_pci_driver = {
+	.name		= "dt3000",
+	.id_table	= dt3000_pci_table,
+	.probe		= dt3000_pci_probe,
+	.remove		= __devexit_p(dt3000_pci_remove),
+};
+module_comedi_pci_driver(dt3000_driver, dt3000_pci_driver);
 
 MODULE_AUTHOR("Comedi http://www.comedi.org");
 MODULE_DESCRIPTION("Comedi low-level driver");
diff --git a/drivers/staging/comedi/drivers/dt9812.c b/drivers/staging/comedi/drivers/dt9812.c
index e86ab586289..22cda5c76ce 100644
--- a/drivers/staging/comedi/drivers/dt9812.c
+++ b/drivers/staging/comedi/drivers/dt9812.c
@@ -196,7 +196,7 @@ struct dt9812_flash_data {
 };
 
 #define DT9812_MAX_NUM_MULTI_BYTE_RDS  \
-    ((DT9812_MAX_WRITE_CMD_PIPE_SIZE - 4 - 1) / sizeof(u8))
+	((DT9812_MAX_WRITE_CMD_PIPE_SIZE - 4 - 1) / sizeof(u8))
 
 struct dt9812_read_multi {
 	u8 count;
@@ -209,8 +209,8 @@ struct dt9812_write_byte {
 };
 
 #define DT9812_MAX_NUM_MULTI_BYTE_WRTS  \
-    ((DT9812_MAX_WRITE_CMD_PIPE_SIZE - 4 - 1) / \
-      sizeof(struct dt9812_write_byte))
+	((DT9812_MAX_WRITE_CMD_PIPE_SIZE - 4 - 1) / \
+	 sizeof(struct dt9812_write_byte))
 
 struct dt9812_write_multi {
 	u8 count;
@@ -224,7 +224,8 @@ struct dt9812_rmw_byte {
 };
 
 #define DT9812_MAX_NUM_MULTI_BYTE_RMWS  \
-    ((DT9812_MAX_WRITE_CMD_PIPE_SIZE - 4 - 1) / sizeof(struct dt9812_rmw_byte))
+	((DT9812_MAX_WRITE_CMD_PIPE_SIZE - 4 - 1) / \
+	 sizeof(struct dt9812_rmw_byte))
 
 struct dt9812_rmw_multi {
 	u8 count;
@@ -365,7 +366,7 @@ static int dt9812_read_info(struct usb_dt9812 *dev, int offset, void *buf,
 }
 
 static int dt9812_read_multiple_registers(struct usb_dt9812 *dev, int reg_count,
-					  u8 * address, u8 * value)
+					  u8 *address, u8 *value)
 {
 	struct dt9812_usb_cmd cmd;
 	int i, count, retval;
@@ -391,8 +392,8 @@ static int dt9812_read_multiple_registers(struct usb_dt9812 *dev, int reg_count,
 }
 
 static int dt9812_write_multiple_registers(struct usb_dt9812 *dev,
-					   int reg_count, u8 * address,
-					   u8 * value)
+					   int reg_count, u8 *address,
+					   u8 *value)
 {
 	struct dt9812_usb_cmd cmd;
 	int i, count, retval;
@@ -430,7 +431,7 @@ static int dt9812_rmw_multiple_registers(struct usb_dt9812 *dev, int reg_count,
 	return retval;
 }
 
-static int dt9812_digital_in(struct slot_dt9812 *slot, u8 * bits)
+static int dt9812_digital_in(struct slot_dt9812 *slot, u8 *bits)
 {
 	int result = -ENODEV;
 
@@ -476,7 +477,7 @@ static int dt9812_digital_out(struct slot_dt9812 *slot, u8 bits)
 	return result;
 }
 
-static int dt9812_digital_out_shadow(struct slot_dt9812 *slot, u8 * bits)
+static int dt9812_digital_out_shadow(struct slot_dt9812 *slot, u8 *bits)
 {
 	int result = -ENODEV;
 
@@ -547,12 +548,12 @@ static void dt9812_configure_gain(struct usb_dt9812 *dev,
 		rmw->or_value = F020_MASK_ADC0CF_AMP0GN2;
 		break;
 	default:
-		err("Illegal gain %d\n", gain);
+		dev_err(&dev->interface->dev, "Illegal gain %d\n", gain);
 
 	}
 }
 
-static int dt9812_analog_in(struct slot_dt9812 *slot, int channel, u16 * value,
+static int dt9812_analog_in(struct slot_dt9812 *slot, int channel, u16 *value,
 			    enum dt9812_gain gain)
 {
 	struct dt9812_rmw_byte rmw[3];
@@ -619,7 +620,7 @@ exit:
 }
 
 static int dt9812_analog_out_shadow(struct slot_dt9812 *slot, int channel,
-				    u16 * value)
+				    u16 *value)
 {
 	int result = -ENODEV;
 
@@ -715,7 +716,7 @@ static int dt9812_probe(struct usb_interface *interface,
 	iface_desc = interface->cur_altsetting;
 
 	if (iface_desc->desc.bNumEndpoints != 5) {
-		err("Wrong number of endpints.");
+		dev_err(&interface->dev, "Wrong number of endpoints.\n");
 		retval = -ENODEV;
 		goto error;
 	}
@@ -781,22 +782,22 @@ static int dt9812_probe(struct usb_interface *interface,
 	}
 
 	if (dt9812_read_info(dev, 1, &dev->vendor, sizeof(dev->vendor)) != 0) {
-		err("Failed to read vendor.");
+		dev_err(&interface->dev, "Failed to read vendor.\n");
 		retval = -ENODEV;
 		goto error;
 	}
 	if (dt9812_read_info(dev, 3, &dev->product, sizeof(dev->product)) != 0) {
-		err("Failed to read product.");
+		dev_err(&interface->dev, "Failed to read product.\n");
 		retval = -ENODEV;
 		goto error;
 	}
 	if (dt9812_read_info(dev, 5, &dev->device, sizeof(dev->device)) != 0) {
-		err("Failed to read device.");
+		dev_err(&interface->dev, "Failed to read device.\n");
 		retval = -ENODEV;
 		goto error;
 	}
 	if (dt9812_read_info(dev, 7, &dev->serial, sizeof(dev->serial)) != 0) {
-		err("Failed to read serial.");
+		dev_err(&interface->dev, "Failed to read serial.\n");
 		retval = -ENODEV;
 		goto error;
 	}
@@ -1110,9 +1111,9 @@ static int dt9812_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 	return 0;
 }
 
-static int dt9812_detach(struct comedi_device *dev)
+static void dt9812_detach(struct comedi_device *dev)
 {
-	return 0;
+	/* Nothing to cleanup */
 }
 
 static struct comedi_driver dt9812_comedi_driver = {
@@ -1146,7 +1147,9 @@ static int __init usb_dt9812_init(void)
 	result = comedi_driver_register(&dt9812_comedi_driver);
 	if (result) {
 		usb_deregister(&dt9812_usb_driver);
-		err("comedi_driver_register failed. Error number %d", result);
+		printk(KERN_ERR KBUILD_MODNAME
+			": comedi_driver_register failed. Error number %d\n",
+			result);
 	}
 
 	return result;
diff --git a/drivers/staging/comedi/drivers/dyna_pci10xx.c b/drivers/staging/comedi/drivers/dyna_pci10xx.c
index da8a2bf3165..b0cec7b1b0c 100644
--- a/drivers/staging/comedi/drivers/dyna_pci10xx.c
+++ b/drivers/staging/comedi/drivers/dyna_pci10xx.c
@@ -48,17 +48,6 @@
 
 static DEFINE_MUTEX(start_stop_sem);
 
-static DEFINE_PCI_DEVICE_TABLE(dyna_pci10xx_pci_table) = {
-	{ PCI_DEVICE(PCI_VENDOR_ID_DYNALOG, 0x1050) },
-	{ 0 }
-};
-
-MODULE_DEVICE_TABLE(pci, dyna_pci10xx_pci_table);
-
-static int dyna_pci10xx_attach(struct comedi_device *dev,
-			  struct comedi_devconfig *it);
-static int dyna_pci10xx_detach(struct comedi_device *dev);
-
 static const struct comedi_lrange range_pci1050_ai = { 3, {
 							  BIP_RANGE(10),
 							  BIP_RANGE(5),
@@ -113,16 +102,6 @@ static const struct boardtype boardtypes[] = {
 	{.name = DRV_NAME},
 };
 
-static struct comedi_driver driver_dyna_pci10xx = {
-	.driver_name = DRV_NAME,
-	.module = THIS_MODULE,
-	.attach = dyna_pci10xx_attach,
-	.detach = dyna_pci10xx_detach,
-	.board_name = &boardtypes[0].name,
-	.offset = sizeof(struct boardtype),
-	.num_names = ARRAY_SIZE(boardtypes),
-};
-
 struct dyna_pci10xx_private {
 	struct pci_dev *pci_dev;	/*  ptr to PCI device */
 	char valid;			/*  card is usable */
@@ -408,54 +387,48 @@ found:
 	return 1;
 }
 
-static int dyna_pci10xx_detach(struct comedi_device *dev)
+static void dyna_pci10xx_detach(struct comedi_device *dev)
 {
 	if (devpriv && devpriv->pci_dev) {
 		comedi_pci_disable(devpriv->pci_dev);
 		mutex_destroy(&devpriv->mutex);
 	}
-
-	return 0;
 }
 
-static int __devinit driver_dyna_pci10xx_pci_probe(struct pci_dev *dev,
-					      const struct pci_device_id *ent)
+static struct comedi_driver dyna_pci10xx_driver = {
+	.driver_name	= "dyna_pci10xx",
+	.module		= THIS_MODULE,
+	.attach		= dyna_pci10xx_attach,
+	.detach		= dyna_pci10xx_detach,
+	.board_name	= &boardtypes[0].name,
+	.offset		= sizeof(struct boardtype),
+	.num_names	= ARRAY_SIZE(boardtypes),
+};
+
+static int __devinit dyna_pci10xx_pci_probe(struct pci_dev *dev,
+					    const struct pci_device_id *ent)
 {
-	return comedi_pci_auto_config(dev, driver_dyna_pci10xx.driver_name);
+	return comedi_pci_auto_config(dev, &dyna_pci10xx_driver);
 }
 
-static void __devexit driver_dyna_pci10xx_pci_remove(struct pci_dev *dev)
+static void __devexit dyna_pci10xx_pci_remove(struct pci_dev *dev)
 {
 	comedi_pci_auto_unconfig(dev);
 }
 
-static struct pci_driver driver_dyna_pci10xx_pci_driver = {
-	.id_table = dyna_pci10xx_pci_table,
-	.probe = &driver_dyna_pci10xx_pci_probe,
-	.remove = __devexit_p(&driver_dyna_pci10xx_pci_remove)
+static DEFINE_PCI_DEVICE_TABLE(dyna_pci10xx_pci_table) = {
+	{ PCI_DEVICE(PCI_VENDOR_ID_DYNALOG, 0x1050) },
+	{ 0 }
 };
+MODULE_DEVICE_TABLE(pci, dyna_pci10xx_pci_table);
 
-static int __init driver_dyna_pci10xx_init_module(void)
-{
-	int retval;
-
-	retval = comedi_driver_register(&driver_dyna_pci10xx);
-	if (retval < 0)
-		return retval;
-
-	driver_dyna_pci10xx_pci_driver.name =
-		(char *)driver_dyna_pci10xx.driver_name;
-	return pci_register_driver(&driver_dyna_pci10xx_pci_driver);
-}
-
-static void __exit driver_dyna_pci10xx_cleanup_module(void)
-{
-	pci_unregister_driver(&driver_dyna_pci10xx_pci_driver);
-	comedi_driver_unregister(&driver_dyna_pci10xx);
-}
-
-module_init(driver_dyna_pci10xx_init_module);
-module_exit(driver_dyna_pci10xx_cleanup_module);
+static struct pci_driver dyna_pci10xx_pci_driver = {
+	.name		= "dyna_pci10xx",
+	.id_table	= dyna_pci10xx_pci_table,
+	.probe		= dyna_pci10xx_pci_probe,
+	.remove		= __devexit_p(dyna_pci10xx_pci_remove),
+};
+module_comedi_pci_driver(dyna_pci10xx_driver, dyna_pci10xx_pci_driver);
 
 MODULE_LICENSE("GPL");
 MODULE_AUTHOR("Prashant Shah <pshah.mumbai@gmail.com>");
diff --git a/drivers/staging/comedi/drivers/fl512.c b/drivers/staging/comedi/drivers/fl512.c
index 7f49add60b2..d23814450b4 100644
--- a/drivers/staging/comedi/drivers/fl512.c
+++ b/drivers/staging/comedi/drivers/fl512.c
@@ -42,38 +42,6 @@ static const struct comedi_lrange range_fl512 = { 4, {
 						      }
 };
 
-static int fl512_attach(struct comedi_device *dev, struct comedi_devconfig *it);
-static int fl512_detach(struct comedi_device *dev);
-
-static struct comedi_driver driver_fl512 = {
-	.driver_name = "fl512",
-	.module = THIS_MODULE,
-	.attach = fl512_attach,
-	.detach = fl512_detach,
-};
-
-static int __init driver_fl512_init_module(void)
-{
-	return comedi_driver_register(&driver_fl512);
-}
-
-static void __exit driver_fl512_cleanup_module(void)
-{
-	comedi_driver_unregister(&driver_fl512);
-}
-
-module_init(driver_fl512_init_module);
-module_exit(driver_fl512_cleanup_module);
-
-static int fl512_ai_insn(struct comedi_device *dev,
-			 struct comedi_subdevice *s, struct comedi_insn *insn,
-			 unsigned int *data);
-static int fl512_ao_insn(struct comedi_device *dev, struct comedi_subdevice *s,
-			 struct comedi_insn *insn, unsigned int *data);
-static int fl512_ao_insn_readback(struct comedi_device *dev,
-				  struct comedi_subdevice *s,
-				  struct comedi_insn *insn, unsigned int *data);
-
 /*
  * fl512_ai_insn : this is the analog input function
  */
@@ -140,9 +108,6 @@ static int fl512_ao_insn_readback(struct comedi_device *dev,
 	return n;
 }
 
-/*
- * start attach
- */
 static int fl512_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 {
 	unsigned long iobase;
@@ -209,14 +174,20 @@ static int fl512_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 	return 1;
 }
 
-static int fl512_detach(struct comedi_device *dev)
+static void fl512_detach(struct comedi_device *dev)
 {
 	if (dev->iobase)
 		release_region(dev->iobase, FL512_SIZE);
-	printk(KERN_INFO "comedi%d: fl512: dummy i detach\n", dev->minor);
-	return 0;
 }
 
+static struct comedi_driver fl512_driver = {
+	.driver_name	= "fl512",
+	.module		= THIS_MODULE,
+	.attach		= fl512_attach,
+	.detach		= fl512_detach,
+};
+module_comedi_driver(fl512_driver);
+
 MODULE_AUTHOR("Comedi http://www.comedi.org");
 MODULE_DESCRIPTION("Comedi low-level driver");
 MODULE_LICENSE("GPL");
diff --git a/drivers/staging/comedi/drivers/gsc_hpdi.c b/drivers/staging/comedi/drivers/gsc_hpdi.c
index bc020dea141..8aece08bd0d 100644
--- a/drivers/staging/comedi/drivers/gsc_hpdi.c
+++ b/drivers/staging/comedi/drivers/gsc_hpdi.c
@@ -53,8 +53,6 @@ support could be added to this driver.
 #include "plx9080.h"
 #include "comedi_fc.h"
 
-static int hpdi_attach(struct comedi_device *dev, struct comedi_devconfig *it);
-static int hpdi_detach(struct comedi_device *dev);
 static void abort_dma(struct comedi_device *dev, unsigned int channel);
 static int hpdi_cmd(struct comedi_device *dev, struct comedi_subdevice *s);
 static int hpdi_cmd_test(struct comedi_device *dev, struct comedi_subdevice *s,
@@ -287,15 +285,6 @@ static const struct hpdi_board hpdi_boards[] = {
 #endif
 };
 
-static DEFINE_PCI_DEVICE_TABLE(hpdi_pci_table) = {
-	{
-	PCI_VENDOR_ID_PLX, PCI_DEVICE_ID_PLX_9080, PCI_VENDOR_ID_PLX,
-		    0x2400, 0, 0, 0}, {
-	0}
-};
-
-MODULE_DEVICE_TABLE(pci, hpdi_pci_table);
-
 static inline struct hpdi_board *board(const struct comedi_device *dev)
 {
 	return (struct hpdi_board *)dev->board_ptr;
@@ -338,51 +327,6 @@ static inline struct hpdi_private *priv(struct comedi_device *dev)
 	return dev->private;
 }
 
-static struct comedi_driver driver_hpdi = {
-	.driver_name = "gsc_hpdi",
-	.module = THIS_MODULE,
-	.attach = hpdi_attach,
-	.detach = hpdi_detach,
-};
-
-static int __devinit driver_hpdi_pci_probe(struct pci_dev *dev,
-					   const struct pci_device_id *ent)
-{
-	return comedi_pci_auto_config(dev, driver_hpdi.driver_name);
-}
-
-static void __devexit driver_hpdi_pci_remove(struct pci_dev *dev)
-{
-	comedi_pci_auto_unconfig(dev);
-}
-
-static struct pci_driver driver_hpdi_pci_driver = {
-	.id_table = hpdi_pci_table,
-	.probe = &driver_hpdi_pci_probe,
-	.remove = __devexit_p(&driver_hpdi_pci_remove)
-};
-
-static int __init driver_hpdi_init_module(void)
-{
-	int retval;
-
-	retval = comedi_driver_register(&driver_hpdi);
-	if (retval < 0)
-		return retval;
-
-	driver_hpdi_pci_driver.name = (char *)driver_hpdi.driver_name;
-	return pci_register_driver(&driver_hpdi_pci_driver);
-}
-
-static void __exit driver_hpdi_cleanup_module(void)
-{
-	pci_unregister_driver(&driver_hpdi_pci_driver);
-	comedi_driver_unregister(&driver_hpdi);
-}
-
-module_init(driver_hpdi_init_module);
-module_exit(driver_hpdi_cleanup_module);
-
 static int dio_config_insn(struct comedi_device *dev,
 			   struct comedi_subdevice *s, struct comedi_insn *insn,
 			   unsigned int *data)
@@ -645,7 +589,7 @@ static int hpdi_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 	       "gsc_hpdi: found %s on bus %i, slot %i\n", board(dev)->name,
 	       pcidev->bus->number, PCI_SLOT(pcidev->devfn));
 
-	if (comedi_pci_enable(pcidev, driver_hpdi.driver_name)) {
+	if (comedi_pci_enable(pcidev, dev->driver->driver_name)) {
 		printk(KERN_WARNING
 		       " failed enable PCI device and request regions\n");
 		return -EIO;
@@ -679,7 +623,7 @@ static int hpdi_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 
 	/*  get irq */
 	if (request_irq(pcidev->irq, handle_interrupt, IRQF_SHARED,
-			driver_hpdi.driver_name, dev)) {
+			dev->driver->driver_name, dev)) {
 		printk(KERN_WARNING
 		       " unable to allocate irq %u\n", pcidev->irq);
 		return -EINVAL;
@@ -720,12 +664,10 @@ static int hpdi_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 	return init_hpdi(dev);
 }
 
-static int hpdi_detach(struct comedi_device *dev)
+static void hpdi_detach(struct comedi_device *dev)
 {
 	unsigned int i;
 
-	printk(KERN_WARNING "comedi%d: gsc_hpdi: remove\n", dev->minor);
-
 	if (dev->irq)
 		free_irq(dev->irq, dev);
 	if ((priv(dev)) && (priv(dev)->hw_dev)) {
@@ -758,7 +700,6 @@ static int hpdi_detach(struct comedi_device *dev)
 			comedi_pci_disable(priv(dev)->hw_dev);
 		pci_dev_put(priv(dev)->hw_dev);
 	}
-	return 0;
 }
 
 static int dio_config_block_size(struct comedi_device *dev, unsigned int *data)
@@ -1113,6 +1054,39 @@ static int hpdi_cancel(struct comedi_device *dev, struct comedi_subdevice *s)
 	return 0;
 }
 
+static struct comedi_driver gsc_hpdi_driver = {
+	.driver_name	= "gsc_hpdi",
+	.module		= THIS_MODULE,
+	.attach		= hpdi_attach,
+	.detach		= hpdi_detach,
+};
+
+static int __devinit gsc_hpdi_pci_probe(struct pci_dev *dev,
+					const struct pci_device_id *ent)
+{
+	return comedi_pci_auto_config(dev, &gsc_hpdi_driver);
+}
+
+static void __devexit gsc_hpdi_pci_remove(struct pci_dev *dev)
+{
+	comedi_pci_auto_unconfig(dev);
+}
+
+static DEFINE_PCI_DEVICE_TABLE(gsc_hpdi_pci_table) = {
+	{ PCI_VENDOR_ID_PLX, PCI_DEVICE_ID_PLX_9080, PCI_VENDOR_ID_PLX,
+		    0x2400, 0, 0, 0},
+	{ 0 }
+};
+MODULE_DEVICE_TABLE(pci, gsc_hpdi_pci_table);
+
+static struct pci_driver gsc_hpdi_pci_driver = {
+	.name		= "gsc_hpdi",
+	.id_table	= gsc_hpdi_pci_table,
+	.probe		= gsc_hpdi_pci_probe,
+	.remove		= __devexit_p(gsc_hpdi_pci_remove)
+};
+module_comedi_pci_driver(gsc_hpdi_driver, gsc_hpdi_pci_driver);
+
 MODULE_AUTHOR("Comedi http://www.comedi.org");
 MODULE_DESCRIPTION("Comedi low-level driver");
 MODULE_LICENSE("GPL");
diff --git a/drivers/staging/comedi/drivers/icp_multi.c b/drivers/staging/comedi/drivers/icp_multi.c
index 126550f3c02..fdc596fb099 100644
--- a/drivers/staging/comedi/drivers/icp_multi.c
+++ b/drivers/staging/comedi/drivers/icp_multi.c
@@ -121,15 +121,6 @@ static const char range_codes_analog[] = { 0x00, 0x20, 0x10, 0x30 };
 
 /*
 ==============================================================================
-	Forward declarations
-==============================================================================
-*/
-static int icp_multi_attach(struct comedi_device *dev,
-			    struct comedi_devconfig *it);
-static int icp_multi_detach(struct comedi_device *dev);
-
-/*
-==============================================================================
 	Data & Structure declarations
 ==============================================================================
 */
@@ -154,50 +145,6 @@ struct boardtype {
 	const struct comedi_lrange *rangelist_ao;	/*  rangelist for D/A */
 };
 
-static const struct boardtype boardtypes[] = {
-	{"icp_multi",		/*  Driver name */
-	 DEVICE_ID,		/*  PCI device ID */
-	 IORANGE_ICP_MULTI,	/*  I/O range length */
-	 1,			/*  1=Card supports interrupts */
-	 TYPE_ICP_MULTI,	/*  Card type = ICP MULTI */
-	 16,			/*  Num of A/D channels */
-	 8,			/*  Num of A/D channels in diff mode */
-	 4,			/*  Num of D/A channels */
-	 16,			/*  Num of digital inputs */
-	 8,			/*  Num of digital outputs */
-	 4,			/*  Num of counters */
-	 0x0fff,		/*  Resolution of A/D */
-	 0x0fff,		/*  Resolution of D/A */
-	 &range_analog,		/*  Rangelist for A/D */
-	 range_codes_analog,	/*  Range codes for programming */
-	 &range_analog},	/*  Rangelist for D/A */
-};
-
-#define n_boardtypes (sizeof(boardtypes)/sizeof(struct boardtype))
-
-static struct comedi_driver driver_icp_multi = {
-	.driver_name = "icp_multi",
-	.module = THIS_MODULE,
-	.attach = icp_multi_attach,
-	.detach = icp_multi_detach,
-	.num_names = n_boardtypes,
-	.board_name = &boardtypes[0].name,
-	.offset = sizeof(struct boardtype),
-};
-
-static int __init driver_icp_multi_init_module(void)
-{
-	return comedi_driver_register(&driver_icp_multi);
-}
-
-static void __exit driver_icp_multi_cleanup_module(void)
-{
-	comedi_driver_unregister(&driver_icp_multi);
-}
-
-module_init(driver_icp_multi_init_module);
-module_exit(driver_icp_multi_cleanup_module);
-
 struct icp_multi_private {
 	struct pcilst_struct *card;	/*  pointer to card */
 	char valid;		/*  card is usable */
@@ -222,25 +169,81 @@ struct icp_multi_private {
 
 /*
 ==============================================================================
-	More forward declarations
+
+Name:	setup_channel_list
+
+Description:
+	This function sets the appropriate channel selection,
+	differential input mode and range bits in the ADC Command/
+	Status register.
+
+Parameters:
+	struct comedi_device *dev	Pointer to current service structure
+	struct comedi_subdevice *s	Pointer to current subdevice structure
+	unsigned int *chanlist	Pointer to packed channel list
+	unsigned int n_chan	Number of channels to scan
+
+Returns:Void
+
 ==============================================================================
 */
-
-#if 0
-static int check_channel_list(struct comedi_device *dev,
-			      struct comedi_subdevice *s,
-			      unsigned int *chanlist, unsigned int n_chan);
-#endif
 static void setup_channel_list(struct comedi_device *dev,
 			       struct comedi_subdevice *s,
-			       unsigned int *chanlist, unsigned int n_chan);
-static int icp_multi_reset(struct comedi_device *dev);
+			       unsigned int *chanlist, unsigned int n_chan)
+{
+	unsigned int i, range, chanprog;
+	unsigned int diff;
 
-/*
-==============================================================================
-	Functions
-==============================================================================
-*/
+#ifdef ICP_MULTI_EXTDEBUG
+	printk(KERN_DEBUG
+	       "icp multi EDBG:  setup_channel_list(...,%d)\n", n_chan);
+#endif
+	devpriv->act_chanlist_len = n_chan;
+	devpriv->act_chanlist_pos = 0;
+
+	for (i = 0; i < n_chan; i++) {
+		/*  Get channel */
+		chanprog = CR_CHAN(chanlist[i]);
+
+		/*  Determine if it is a differential channel (Bit 15  = 1) */
+		if (CR_AREF(chanlist[i]) == AREF_DIFF) {
+			diff = 1;
+			chanprog &= 0x0007;
+		} else {
+			diff = 0;
+			chanprog &= 0x000f;
+		}
+
+		/*  Clear channel, range and input mode bits
+		 *  in A/D command/status register */
+		devpriv->AdcCmdStatus &= 0xf00f;
+
+		/*  Set channel number and differential mode status bit */
+		if (diff) {
+			/*  Set channel number, bits 9-11 & mode, bit 6 */
+			devpriv->AdcCmdStatus |= (chanprog << 9);
+			devpriv->AdcCmdStatus |= ADC_DI;
+		} else
+			/*  Set channel number, bits 8-11 */
+			devpriv->AdcCmdStatus |= (chanprog << 8);
+
+		/*  Get range for current channel */
+		range = this_board->rangecode[CR_RANGE(chanlist[i])];
+		/*  Set range. bits 4-5 */
+		devpriv->AdcCmdStatus |= range;
+
+		/* Output channel, range, mode to ICP Multi */
+		writew(devpriv->AdcCmdStatus,
+		       devpriv->io_addr + ICP_MULTI_ADC_CSR);
+
+#ifdef ICP_MULTI_EXTDEBUG
+		printk(KERN_DEBUG
+		       "GS: %2d. [%4x]=%4x %4x\n", i, chanprog, range,
+		       devpriv->act_chanlist[i]);
+#endif
+	}
+
+}
 
 /*
 ==============================================================================
@@ -764,84 +767,6 @@ static int check_channel_list(struct comedi_device *dev,
 /*
 ==============================================================================
 
-Name:	setup_channel_list
-
-Description:
-	This function sets the appropriate channel selection,
-	differential input mode and range bits in the ADC Command/
-	Status register.
-
-Parameters:
-	struct comedi_device *dev	Pointer to current service structure
-	struct comedi_subdevice *s	Pointer to current subdevice structure
-	unsigned int *chanlist	Pointer to packed channel list
-	unsigned int n_chan	Number of channels to scan
-
-Returns:Void
-
-==============================================================================
-*/
-static void setup_channel_list(struct comedi_device *dev,
-			       struct comedi_subdevice *s,
-			       unsigned int *chanlist, unsigned int n_chan)
-{
-	unsigned int i, range, chanprog;
-	unsigned int diff;
-
-#ifdef ICP_MULTI_EXTDEBUG
-	printk(KERN_DEBUG
-	       "icp multi EDBG:  setup_channel_list(...,%d)\n", n_chan);
-#endif
-	devpriv->act_chanlist_len = n_chan;
-	devpriv->act_chanlist_pos = 0;
-
-	for (i = 0; i < n_chan; i++) {
-		/*  Get channel */
-		chanprog = CR_CHAN(chanlist[i]);
-
-		/*  Determine if it is a differential channel (Bit 15  = 1) */
-		if (CR_AREF(chanlist[i]) == AREF_DIFF) {
-			diff = 1;
-			chanprog &= 0x0007;
-		} else {
-			diff = 0;
-			chanprog &= 0x000f;
-		}
-
-		/*  Clear channel, range and input mode bits
-		 *  in A/D command/status register */
-		devpriv->AdcCmdStatus &= 0xf00f;
-
-		/*  Set channel number and differential mode status bit */
-		if (diff) {
-			/*  Set channel number, bits 9-11 & mode, bit 6 */
-			devpriv->AdcCmdStatus |= (chanprog << 9);
-			devpriv->AdcCmdStatus |= ADC_DI;
-		} else
-			/*  Set channel number, bits 8-11 */
-			devpriv->AdcCmdStatus |= (chanprog << 8);
-
-		/*  Get range for current channel */
-		range = this_board->rangecode[CR_RANGE(chanlist[i])];
-		/*  Set range. bits 4-5 */
-		devpriv->AdcCmdStatus |= range;
-
-		/* Output channel, range, mode to ICP Multi */
-		writew(devpriv->AdcCmdStatus,
-		       devpriv->io_addr + ICP_MULTI_ADC_CSR);
-
-#ifdef ICP_MULTI_EXTDEBUG
-		printk(KERN_DEBUG
-		       "GS: %2d. [%4x]=%4x %4x\n", i, chanprog, range,
-		       devpriv->act_chanlist[i]);
-#endif
-	}
-
-}
-
-/*
-==============================================================================
-
 Name:	icp_multi_reset
 
 Description:
@@ -897,23 +822,6 @@ static int icp_multi_reset(struct comedi_device *dev)
 	return 0;
 }
 
-/*
-==============================================================================
-
-Name:	icp_multi_attach
-
-Description:
-	This function sets up all the appropriate data for the current
-	device.
-
-Parameters:
-	struct comedi_device *dev	Pointer to current device structure
-	struct comedi_devconfig *it	Pointer to current device configuration
-
-Returns:int	0 = success
-
-==============================================================================
-*/
 static int icp_multi_attach(struct comedi_device *dev,
 			    struct comedi_devconfig *it)
 {
@@ -1099,44 +1007,53 @@ static int icp_multi_attach(struct comedi_device *dev,
 	return 0;
 }
 
-/*
-==============================================================================
-
-Name:	icp_multi_detach
-
-Description:
-	This function releases all the resources used by the current
-	device.
-
-Parameters:
-	struct comedi_device *dev	Pointer to current device structure
-
-Returns:int	0 = success
-
-==============================================================================
-*/
-static int icp_multi_detach(struct comedi_device *dev)
+static void icp_multi_detach(struct comedi_device *dev)
 {
-
 	if (dev->private)
 		if (devpriv->valid)
 			icp_multi_reset(dev);
-
 	if (dev->irq)
 		free_irq(dev->irq, dev);
-
 	if (dev->private && devpriv->io_addr)
 		iounmap(devpriv->io_addr);
-
 	if (dev->private && devpriv->card)
 		pci_card_free(devpriv->card);
-
 	if (--pci_list_builded == 0)
 		pci_card_list_cleanup(PCI_VENDOR_ID_ICP);
-
-	return 0;
 }
 
+static const struct boardtype boardtypes[] = {
+	{
+		.name		= "icp_multi",
+		.device_id	= DEVICE_ID,
+		.iorange	= IORANGE_ICP_MULTI,
+		.have_irq	= 1,
+		.cardtype	= TYPE_ICP_MULTI,
+		.n_aichan	= 16,
+		.n_aichand	= 8,
+		.n_aochan	= 4,
+		.n_dichan	= 16,
+		.n_dochan	= 8,
+		.n_ctrs		= 4,
+		.ai_maxdata	= 0x0fff,
+		.ao_maxdata	= 0x0fff,
+		.rangelist_ai	= &range_analog,
+		.rangecode	= range_codes_analog,
+		.rangelist_ao	= &range_analog,
+	},
+};
+
+static struct comedi_driver icp_multi_driver = {
+	.driver_name	= "icp_multi",
+	.module		= THIS_MODULE,
+	.attach		= icp_multi_attach,
+	.detach		= icp_multi_detach,
+	.num_names	= ARRAY_SIZE(boardtypes),
+	.board_name	= &boardtypes[0].name,
+	.offset		= sizeof(struct boardtype),
+};
+module_comedi_driver(icp_multi_driver);
+
 MODULE_AUTHOR("Comedi http://www.comedi.org");
 MODULE_DESCRIPTION("Comedi low-level driver");
 MODULE_LICENSE("GPL");
diff --git a/drivers/staging/comedi/drivers/ii_pci20kc.c b/drivers/staging/comedi/drivers/ii_pci20kc.c
index e4711ef5471..f0a579a04a7 100644
--- a/drivers/staging/comedi/drivers/ii_pci20kc.c
+++ b/drivers/staging/comedi/drivers/ii_pci20kc.c
@@ -159,17 +159,6 @@ struct pci20xxx_private {
 #define devpriv ((struct pci20xxx_private *)dev->private)
 #define CHAN (CR_CHAN(it->chanlist[0]))
 
-static int pci20xxx_attach(struct comedi_device *dev,
-			   struct comedi_devconfig *it);
-static int pci20xxx_detach(struct comedi_device *dev);
-
-static struct comedi_driver driver_pci20xxx = {
-	.driver_name = "ii_pci20kc",
-	.module = THIS_MODULE,
-	.attach = pci20xxx_attach,
-	.detach = pci20xxx_detach,
-};
-
 static int pci20006_init(struct comedi_device *dev, struct comedi_subdevice *s,
 			 int opt0, int opt1);
 static int pci20341_init(struct comedi_device *dev, struct comedi_subdevice *s,
@@ -275,11 +264,9 @@ static int pci20xxx_attach(struct comedi_device *dev,
 	return 1;
 }
 
-static int pci20xxx_detach(struct comedi_device *dev)
+static void pci20xxx_detach(struct comedi_device *dev)
 {
-	printk(KERN_INFO "comedi%d: pci20xxx: remove\n", dev->minor);
-
-	return 0;
+	/* Nothing to cleanup */
 }
 
 /* pci20006m */
@@ -666,18 +653,13 @@ static unsigned int pci20xxx_di(struct comedi_device *dev,
 }
 #endif
 
-static int __init driver_pci20xxx_init_module(void)
-{
-	return comedi_driver_register(&driver_pci20xxx);
-}
-
-static void __exit driver_pci20xxx_cleanup_module(void)
-{
-	comedi_driver_unregister(&driver_pci20xxx);
-}
-
-module_init(driver_pci20xxx_init_module);
-module_exit(driver_pci20xxx_cleanup_module);
+static struct comedi_driver pci20xxx_driver = {
+	.driver_name	= "ii_pci20kc",
+	.module		= THIS_MODULE,
+	.attach		= pci20xxx_attach,
+	.detach		= pci20xxx_detach,
+};
+module_comedi_driver(pci20xxx_driver);
 
 MODULE_AUTHOR("Comedi http://www.comedi.org");
 MODULE_DESCRIPTION("Comedi low-level driver");
diff --git a/drivers/staging/comedi/drivers/jr3_pci.c b/drivers/staging/comedi/drivers/jr3_pci.c
index 6a79ba10630..d536a11edb9 100644
--- a/drivers/staging/comedi/drivers/jr3_pci.c
+++ b/drivers/staging/comedi/drivers/jr3_pci.c
@@ -59,28 +59,6 @@ Devices: [JR3] PCI force sensor board (jr3_pci)
 #define PCI_DEVICE_ID_JR3_3_CHANNEL 0x3113
 #define PCI_DEVICE_ID_JR3_4_CHANNEL 0x3114
 
-static int jr3_pci_attach(struct comedi_device *dev,
-			  struct comedi_devconfig *it);
-static int jr3_pci_detach(struct comedi_device *dev);
-
-static struct comedi_driver driver_jr3_pci = {
-	.driver_name = "jr3_pci",
-	.module = THIS_MODULE,
-	.attach = jr3_pci_attach,
-	.detach = jr3_pci_detach,
-};
-
-static DEFINE_PCI_DEVICE_TABLE(jr3_pci_pci_table) = {
-	{ PCI_DEVICE(PCI_VENDOR_ID_JR3, PCI_DEVICE_ID_JR3_1_CHANNEL) },
-	{ PCI_DEVICE(PCI_VENDOR_ID_JR3, PCI_DEVICE_ID_JR3_1_CHANNEL_NEW) },
-	{ PCI_DEVICE(PCI_VENDOR_ID_JR3, PCI_DEVICE_ID_JR3_2_CHANNEL) },
-	{ PCI_DEVICE(PCI_VENDOR_ID_JR3, PCI_DEVICE_ID_JR3_3_CHANNEL) },
-	{ PCI_DEVICE(PCI_VENDOR_ID_JR3, PCI_DEVICE_ID_JR3_4_CHANNEL) },
-	{0}
-};
-
-MODULE_DEVICE_TABLE(pci, jr3_pci_pci_table);
-
 struct jr3_pci_dev_private {
 
 	struct pci_dev *pci_dev;
@@ -948,9 +926,7 @@ out:
 	return result;
 }
 
-MODULE_FIRMWARE("comedi/jr3pci.idm");
-
-static int jr3_pci_detach(struct comedi_device *dev)
+static void jr3_pci_detach(struct comedi_device *dev)
 {
 	int i;
 	struct jr3_pci_dev_private *devpriv = dev->private;
@@ -962,56 +938,52 @@ static int jr3_pci_detach(struct comedi_device *dev)
 			for (i = 0; i < devpriv->n_channels; i++)
 				kfree(dev->subdevices[i].private);
 		}
-
 		if (devpriv->iobase)
 			iounmap((void *)devpriv->iobase);
 		if (devpriv->pci_enabled)
 			comedi_pci_disable(devpriv->pci_dev);
-
 		if (devpriv->pci_dev)
 			pci_dev_put(devpriv->pci_dev);
 	}
-	return 0;
 }
 
-static int __devinit driver_jr3_pci_pci_probe(struct pci_dev *dev,
-					      const struct pci_device_id *ent)
+static struct comedi_driver jr3_pci_driver = {
+	.driver_name	= "jr3_pci",
+	.module		= THIS_MODULE,
+	.attach		= jr3_pci_attach,
+	.detach		= jr3_pci_detach,
+};
+
+static int __devinit jr3_pci_pci_probe(struct pci_dev *dev,
+				       const struct pci_device_id *ent)
 {
-	return comedi_pci_auto_config(dev, driver_jr3_pci.driver_name);
+	return comedi_pci_auto_config(dev, &jr3_pci_driver);
 }
 
-static void __devexit driver_jr3_pci_pci_remove(struct pci_dev *dev)
+static void __devexit jr3_pci_pci_remove(struct pci_dev *dev)
 {
 	comedi_pci_auto_unconfig(dev);
 }
 
-static struct pci_driver driver_jr3_pci_pci_driver = {
-	.id_table = jr3_pci_pci_table,
-	.probe = &driver_jr3_pci_pci_probe,
-	.remove = __devexit_p(&driver_jr3_pci_pci_remove)
+static DEFINE_PCI_DEVICE_TABLE(jr3_pci_pci_table) = {
+	{ PCI_DEVICE(PCI_VENDOR_ID_JR3, PCI_DEVICE_ID_JR3_1_CHANNEL) },
+	{ PCI_DEVICE(PCI_VENDOR_ID_JR3, PCI_DEVICE_ID_JR3_1_CHANNEL_NEW) },
+	{ PCI_DEVICE(PCI_VENDOR_ID_JR3, PCI_DEVICE_ID_JR3_2_CHANNEL) },
+	{ PCI_DEVICE(PCI_VENDOR_ID_JR3, PCI_DEVICE_ID_JR3_3_CHANNEL) },
+	{ PCI_DEVICE(PCI_VENDOR_ID_JR3, PCI_DEVICE_ID_JR3_4_CHANNEL) },
+	{ 0 }
 };
+MODULE_DEVICE_TABLE(pci, jr3_pci_pci_table);
 
-static int __init driver_jr3_pci_init_module(void)
-{
-	int retval;
-
-	retval = comedi_driver_register(&driver_jr3_pci);
-	if (retval < 0)
-		return retval;
-
-	driver_jr3_pci_pci_driver.name = (char *)driver_jr3_pci.driver_name;
-	return pci_register_driver(&driver_jr3_pci_pci_driver);
-}
-
-static void __exit driver_jr3_pci_cleanup_module(void)
-{
-	pci_unregister_driver(&driver_jr3_pci_pci_driver);
-	comedi_driver_unregister(&driver_jr3_pci);
-}
-
-module_init(driver_jr3_pci_init_module);
-module_exit(driver_jr3_pci_cleanup_module);
+static struct pci_driver jr3_pci_pci_driver = {
+	.name		= "jr3_pci",
+	.id_table	= jr3_pci_pci_table,
+	.probe		= jr3_pci_pci_probe,
+	.remove		= __devexit_p(jr3_pci_pci_remove),
+};
+module_comedi_pci_driver(jr3_pci_driver, jr3_pci_pci_driver);
 
 MODULE_AUTHOR("Comedi http://www.comedi.org");
 MODULE_DESCRIPTION("Comedi low-level driver");
 MODULE_LICENSE("GPL");
+MODULE_FIRMWARE("comedi/jr3pci.idm");
diff --git a/drivers/staging/comedi/drivers/ke_counter.c b/drivers/staging/comedi/drivers/ke_counter.c
index 4e9e9a07865..09d191844bf 100644
--- a/drivers/staging/comedi/drivers/ke_counter.c
+++ b/drivers/staging/comedi/drivers/ke_counter.c
@@ -46,18 +46,6 @@ Kolter Electronic PCI Counter Card.
 #define PCI_VENDOR_ID_KOLTER    0x1001
 #define CNT_CARD_DEVICE_ID      0x0014
 
-/*-- function prototypes ----------------------------------------------------*/
-
-static int cnt_attach(struct comedi_device *dev, struct comedi_devconfig *it);
-static int cnt_detach(struct comedi_device *dev);
-
-static DEFINE_PCI_DEVICE_TABLE(cnt_pci_table) = {
-	{ PCI_DEVICE(PCI_VENDOR_ID_KOLTER, CNT_CARD_DEVICE_ID) },
-	{0}
-};
-
-MODULE_DEVICE_TABLE(pci, cnt_pci_table);
-
 /*-- board specification structure ------------------------------------------*/
 
 struct cnt_board_struct {
@@ -87,51 +75,6 @@ struct cnt_device_private {
 
 #define devpriv ((struct cnt_device_private *)dev->private)
 
-static struct comedi_driver cnt_driver = {
-	.driver_name = CNT_DRIVER_NAME,
-	.module = THIS_MODULE,
-	.attach = cnt_attach,
-	.detach = cnt_detach,
-};
-
-static int __devinit cnt_driver_pci_probe(struct pci_dev *dev,
-					  const struct pci_device_id *ent)
-{
-	return comedi_pci_auto_config(dev, cnt_driver.driver_name);
-}
-
-static void __devexit cnt_driver_pci_remove(struct pci_dev *dev)
-{
-	comedi_pci_auto_unconfig(dev);
-}
-
-static struct pci_driver cnt_driver_pci_driver = {
-	.id_table = cnt_pci_table,
-	.probe = &cnt_driver_pci_probe,
-	.remove = __devexit_p(&cnt_driver_pci_remove)
-};
-
-static int __init cnt_driver_init_module(void)
-{
-	int retval;
-
-	retval = comedi_driver_register(&cnt_driver);
-	if (retval < 0)
-		return retval;
-
-	cnt_driver_pci_driver.name = (char *)cnt_driver.driver_name;
-	return pci_register_driver(&cnt_driver_pci_driver);
-}
-
-static void __exit cnt_driver_cleanup_module(void)
-{
-	pci_unregister_driver(&cnt_driver_pci_driver);
-	comedi_driver_unregister(&cnt_driver);
-}
-
-module_init(cnt_driver_init_module);
-module_exit(cnt_driver_cleanup_module);
-
 /*-- counter write ----------------------------------------------------------*/
 
 /* This should be used only for resetting the counters; maybe it is better
@@ -181,8 +124,6 @@ static int cnt_rinsn(struct comedi_device *dev,
 	return 1;
 }
 
-/*-- attach -----------------------------------------------------------------*/
-
 static int cnt_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 {
 	struct comedi_subdevice *subdevice;
@@ -278,20 +219,47 @@ found:
 	return 0;
 }
 
-/*-- detach -----------------------------------------------------------------*/
-
-static int cnt_detach(struct comedi_device *dev)
+static void cnt_detach(struct comedi_device *dev)
 {
 	if (devpriv && devpriv->pcidev) {
 		if (dev->iobase)
 			comedi_pci_disable(devpriv->pcidev);
 		pci_dev_put(devpriv->pcidev);
 	}
-	printk(KERN_INFO "comedi%d: " CNT_DRIVER_NAME " remove\n",
-	       dev->minor);
-	return 0;
 }
 
+static struct comedi_driver ke_counter_driver = {
+	.driver_name	= "ke_counter",
+	.module		= THIS_MODULE,
+	.attach		= cnt_attach,
+	.detach		= cnt_detach,
+};
+
+static int __devinit ke_counter_pci_probe(struct pci_dev *dev,
+					  const struct pci_device_id *ent)
+{
+	return comedi_pci_auto_config(dev, &ke_counter_driver);
+}
+
+static void __devexit ke_counter_pci_remove(struct pci_dev *dev)
+{
+	comedi_pci_auto_unconfig(dev);
+}
+
+static DEFINE_PCI_DEVICE_TABLE(ke_counter_pci_table) = {
+	{ PCI_DEVICE(PCI_VENDOR_ID_KOLTER, CNT_CARD_DEVICE_ID) },
+	{ 0 }
+};
+MODULE_DEVICE_TABLE(pci, ke_counter_pci_table);
+
+static struct pci_driver ke_counter_pci_driver = {
+	.name		= "ke_counter",
+	.id_table	= ke_counter_pci_table,
+	.probe		= ke_counter_pci_probe,
+	.remove		= __devexit_p(ke_counter_pci_remove),
+};
+module_comedi_pci_driver(ke_counter_driver, ke_counter_pci_driver);
+
 MODULE_AUTHOR("Comedi http://www.comedi.org");
 MODULE_DESCRIPTION("Comedi low-level driver");
 MODULE_LICENSE("GPL");
diff --git a/drivers/staging/comedi/drivers/me4000.c b/drivers/staging/comedi/drivers/me4000.c
index b0bc6bb877a..8ca1b54600d 100644
--- a/drivers/staging/comedi/drivers/me4000.c
+++ b/drivers/staging/comedi/drivers/me4000.c
@@ -65,30 +65,6 @@ broken.
 #include "me4000_fw.h"
 #endif
 
-/*=============================================================================
-  PCI device table.
-  This is used by modprobe to translate PCI IDs to drivers.
-  ===========================================================================*/
-
-static DEFINE_PCI_DEVICE_TABLE(me4000_pci_table) = {
-	{ PCI_DEVICE(PCI_VENDOR_ID_MEILHAUS, 0x4650) },
-	{ PCI_DEVICE(PCI_VENDOR_ID_MEILHAUS, 0x4660) },
-	{ PCI_DEVICE(PCI_VENDOR_ID_MEILHAUS, 0x4661) },
-	{ PCI_DEVICE(PCI_VENDOR_ID_MEILHAUS, 0x4662) },
-	{ PCI_DEVICE(PCI_VENDOR_ID_MEILHAUS, 0x4663) },
-	{ PCI_DEVICE(PCI_VENDOR_ID_MEILHAUS, 0x4670) },
-	{ PCI_DEVICE(PCI_VENDOR_ID_MEILHAUS, 0x4671) },
-	{ PCI_DEVICE(PCI_VENDOR_ID_MEILHAUS, 0x4672) },
-	{ PCI_DEVICE(PCI_VENDOR_ID_MEILHAUS, 0x4673) },
-	{ PCI_DEVICE(PCI_VENDOR_ID_MEILHAUS, 0x4680) },
-	{ PCI_DEVICE(PCI_VENDOR_ID_MEILHAUS, 0x4681) },
-	{ PCI_DEVICE(PCI_VENDOR_ID_MEILHAUS, 0x4682) },
-	{ PCI_DEVICE(PCI_VENDOR_ID_MEILHAUS, 0x4683) },
-	{ 0 }
-};
-
-MODULE_DEVICE_TABLE(pci, me4000_pci_table);
-
 static const struct me4000_board me4000_boards[] = {
 	{"ME-4650", 0x4650, {0, 0}, {16, 0, 0, 0}, {4}, {0} },
 
@@ -113,22 +89,8 @@ static const struct me4000_board me4000_boards[] = {
 #define ME4000_BOARD_VERSIONS (ARRAY_SIZE(me4000_boards) - 1)
 
 /*-----------------------------------------------------------------------------
-  Comedi function prototypes
-  ---------------------------------------------------------------------------*/
-static int me4000_attach(struct comedi_device *dev,
-			 struct comedi_devconfig *it);
-static int me4000_detach(struct comedi_device *dev);
-static struct comedi_driver driver_me4000 = {
-	.driver_name = "me4000",
-	.module = THIS_MODULE,
-	.attach = me4000_attach,
-	.detach = me4000_detach,
-};
-
-/*-----------------------------------------------------------------------------
   Meilhaus function prototypes
   ---------------------------------------------------------------------------*/
-static int me4000_probe(struct comedi_device *dev, struct comedi_devconfig *it);
 static int get_registers(struct comedi_device *dev, struct pci_dev *pci_dev_p);
 static int init_board_info(struct comedi_device *dev,
 			   struct pci_dev *pci_dev_p);
@@ -139,76 +101,10 @@ static int init_cnt_context(struct comedi_device *dev);
 static int xilinx_download(struct comedi_device *dev);
 static int reset_board(struct comedi_device *dev);
 
-static int me4000_dio_insn_bits(struct comedi_device *dev,
-				struct comedi_subdevice *s,
-				struct comedi_insn *insn, unsigned int *data);
-
-static int me4000_dio_insn_config(struct comedi_device *dev,
-				  struct comedi_subdevice *s,
-				  struct comedi_insn *insn, unsigned int *data);
-
-static int cnt_reset(struct comedi_device *dev, unsigned int channel);
-
-static int cnt_config(struct comedi_device *dev,
-		      unsigned int channel, unsigned int mode);
-
-static int me4000_cnt_insn_config(struct comedi_device *dev,
-				  struct comedi_subdevice *s,
-				  struct comedi_insn *insn, unsigned int *data);
-
-static int me4000_cnt_insn_write(struct comedi_device *dev,
-				 struct comedi_subdevice *s,
-				 struct comedi_insn *insn, unsigned int *data);
-
-static int me4000_cnt_insn_read(struct comedi_device *dev,
-				struct comedi_subdevice *s,
-				struct comedi_insn *insn, unsigned int *data);
-
-static int me4000_ai_insn_read(struct comedi_device *dev,
-			       struct comedi_subdevice *subdevice,
-			       struct comedi_insn *insn, unsigned int *data);
-
-static int me4000_ai_cancel(struct comedi_device *dev,
-			    struct comedi_subdevice *s);
-
-static int ai_check_chanlist(struct comedi_device *dev,
-			     struct comedi_subdevice *s,
-			     struct comedi_cmd *cmd);
-
-static int ai_round_cmd_args(struct comedi_device *dev,
-			     struct comedi_subdevice *s,
-			     struct comedi_cmd *cmd,
-			     unsigned int *init_ticks,
-			     unsigned int *scan_ticks,
-			     unsigned int *chan_ticks);
-
-static int ai_prepare(struct comedi_device *dev,
-		      struct comedi_subdevice *s,
-		      struct comedi_cmd *cmd,
-		      unsigned int init_ticks,
-		      unsigned int scan_ticks, unsigned int chan_ticks);
-
 static int ai_write_chanlist(struct comedi_device *dev,
 			     struct comedi_subdevice *s,
 			     struct comedi_cmd *cmd);
 
-static irqreturn_t me4000_ai_isr(int irq, void *dev_id);
-
-static int me4000_ai_do_cmd_test(struct comedi_device *dev,
-				 struct comedi_subdevice *s,
-				 struct comedi_cmd *cmd);
-
-static int me4000_ai_do_cmd(struct comedi_device *dev,
-			    struct comedi_subdevice *s);
-
-static int me4000_ao_insn_write(struct comedi_device *dev,
-				struct comedi_subdevice *s,
-				struct comedi_insn *insn, unsigned int *data);
-
-static int me4000_ao_insn_read(struct comedi_device *dev,
-			       struct comedi_subdevice *s,
-			       struct comedi_insn *insn, unsigned int *data);
-
 /*-----------------------------------------------------------------------------
   Meilhaus inline functions
   ---------------------------------------------------------------------------*/
@@ -262,130 +158,6 @@ static const struct comedi_lrange me4000_ao_range = {
 	 }
 };
 
-static int me4000_attach(struct comedi_device *dev, struct comedi_devconfig *it)
-{
-	struct comedi_subdevice *s;
-	int result;
-
-	CALL_PDEBUG("In me4000_attach()\n");
-
-	result = me4000_probe(dev, it);
-	if (result)
-		return result;
-
-	/*
-	 * Allocate the subdevice structures.  alloc_subdevice() is a
-	 * convenient macro defined in comedidev.h.  It relies on
-	 * n_subdevices being set correctly.
-	 */
-	if (alloc_subdevices(dev, 4) < 0)
-		return -ENOMEM;
-
-    /*=========================================================================
-      Analog input subdevice
-      ========================================================================*/
-
-	s = dev->subdevices + 0;
-
-	if (thisboard->ai.count) {
-		s->type = COMEDI_SUBD_AI;
-		s->subdev_flags =
-		    SDF_READABLE | SDF_COMMON | SDF_GROUND | SDF_DIFF;
-		s->n_chan = thisboard->ai.count;
-		s->maxdata = 0xFFFF;	/*  16 bit ADC */
-		s->len_chanlist = ME4000_AI_CHANNEL_LIST_COUNT;
-		s->range_table = &me4000_ai_range;
-		s->insn_read = me4000_ai_insn_read;
-
-		if (info->irq > 0) {
-			if (request_irq(info->irq, me4000_ai_isr,
-					IRQF_SHARED, "ME-4000", dev)) {
-				printk
-				    ("comedi%d: me4000: me4000_attach(): "
-				     "Unable to allocate irq\n", dev->minor);
-			} else {
-				dev->read_subdev = s;
-				s->subdev_flags |= SDF_CMD_READ;
-				s->cancel = me4000_ai_cancel;
-				s->do_cmdtest = me4000_ai_do_cmd_test;
-				s->do_cmd = me4000_ai_do_cmd;
-			}
-		} else {
-			printk(KERN_WARNING
-			       "comedi%d: me4000: me4000_attach(): "
-			       "No interrupt available\n", dev->minor);
-		}
-	} else {
-		s->type = COMEDI_SUBD_UNUSED;
-	}
-
-    /*=========================================================================
-      Analog output subdevice
-      ========================================================================*/
-
-	s = dev->subdevices + 1;
-
-	if (thisboard->ao.count) {
-		s->type = COMEDI_SUBD_AO;
-		s->subdev_flags = SDF_WRITEABLE | SDF_COMMON | SDF_GROUND;
-		s->n_chan = thisboard->ao.count;
-		s->maxdata = 0xFFFF;	/*  16 bit DAC */
-		s->range_table = &me4000_ao_range;
-		s->insn_write = me4000_ao_insn_write;
-		s->insn_read = me4000_ao_insn_read;
-	} else {
-		s->type = COMEDI_SUBD_UNUSED;
-	}
-
-    /*=========================================================================
-      Digital I/O subdevice
-      ========================================================================*/
-
-	s = dev->subdevices + 2;
-
-	if (thisboard->dio.count) {
-		s->type = COMEDI_SUBD_DIO;
-		s->subdev_flags = SDF_READABLE | SDF_WRITABLE;
-		s->n_chan = thisboard->dio.count * 8;
-		s->maxdata = 1;
-		s->range_table = &range_digital;
-		s->insn_bits = me4000_dio_insn_bits;
-		s->insn_config = me4000_dio_insn_config;
-	} else {
-		s->type = COMEDI_SUBD_UNUSED;
-	}
-
-	/*
-	 * Check for optoisolated ME-4000 version. If one the first
-	 * port is a fixed output port and the second is a fixed input port.
-	 */
-	if (!me4000_inl(dev, info->dio_context.dir_reg)) {
-		s->io_bits |= 0xFF;
-		me4000_outl(dev, ME4000_DIO_CTRL_BIT_MODE_0,
-			    info->dio_context.dir_reg);
-	}
-
-    /*=========================================================================
-      Counter subdevice
-      ========================================================================*/
-
-	s = dev->subdevices + 3;
-
-	if (thisboard->cnt.count) {
-		s->type = COMEDI_SUBD_COUNTER;
-		s->subdev_flags = SDF_READABLE | SDF_WRITABLE;
-		s->n_chan = thisboard->cnt.count;
-		s->maxdata = 0xFFFF;	/*  16 bit counters */
-		s->insn_read = me4000_cnt_insn_read;
-		s->insn_write = me4000_cnt_insn_write;
-		s->insn_config = me4000_cnt_insn_config;
-	} else {
-		s->type = COMEDI_SUBD_UNUSED;
-	}
-
-	return 0;
-}
-
 static int me4000_probe(struct comedi_device *dev, struct comedi_devconfig *it)
 {
 	struct pci_dev *pci_device = NULL;
@@ -920,22 +692,6 @@ static int reset_board(struct comedi_device *dev)
 	return 0;
 }
 
-static int me4000_detach(struct comedi_device *dev)
-{
-	CALL_PDEBUG("In me4000_detach()\n");
-
-	if (info) {
-		if (info->pci_dev_p) {
-			reset_board(dev);
-			if (info->plx_regbase)
-				comedi_pci_disable(info->pci_dev_p);
-			pci_dev_put(info->pci_dev_p);
-		}
-	}
-
-	return 0;
-}
-
 /*=============================================================================
   Analog input section
   ===========================================================================*/
@@ -2424,43 +2180,185 @@ static int me4000_cnt_insn_write(struct comedi_device *dev,
 	return 1;
 }
 
-static int __devinit driver_me4000_pci_probe(struct pci_dev *dev,
-					     const struct pci_device_id *ent)
+static int me4000_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 {
-	return comedi_pci_auto_config(dev, driver_me4000.driver_name);
+	struct comedi_subdevice *s;
+	int result;
+
+	CALL_PDEBUG("In me4000_attach()\n");
+
+	result = me4000_probe(dev, it);
+	if (result)
+		return result;
+
+	/*
+	 * Allocate the subdevice structures.  alloc_subdevice() is a
+	 * convenient macro defined in comedidev.h.  It relies on
+	 * n_subdevices being set correctly.
+	 */
+	if (alloc_subdevices(dev, 4) < 0)
+		return -ENOMEM;
+
+    /*=========================================================================
+      Analog input subdevice
+      ========================================================================*/
+
+	s = dev->subdevices + 0;
+
+	if (thisboard->ai.count) {
+		s->type = COMEDI_SUBD_AI;
+		s->subdev_flags =
+		    SDF_READABLE | SDF_COMMON | SDF_GROUND | SDF_DIFF;
+		s->n_chan = thisboard->ai.count;
+		s->maxdata = 0xFFFF;	/*  16 bit ADC */
+		s->len_chanlist = ME4000_AI_CHANNEL_LIST_COUNT;
+		s->range_table = &me4000_ai_range;
+		s->insn_read = me4000_ai_insn_read;
+
+		if (info->irq > 0) {
+			if (request_irq(info->irq, me4000_ai_isr,
+					IRQF_SHARED, "ME-4000", dev)) {
+				printk
+				    ("comedi%d: me4000: me4000_attach(): "
+				     "Unable to allocate irq\n", dev->minor);
+			} else {
+				dev->read_subdev = s;
+				s->subdev_flags |= SDF_CMD_READ;
+				s->cancel = me4000_ai_cancel;
+				s->do_cmdtest = me4000_ai_do_cmd_test;
+				s->do_cmd = me4000_ai_do_cmd;
+			}
+		} else {
+			printk(KERN_WARNING
+			       "comedi%d: me4000: me4000_attach(): "
+			       "No interrupt available\n", dev->minor);
+		}
+	} else {
+		s->type = COMEDI_SUBD_UNUSED;
+	}
+
+    /*=========================================================================
+      Analog output subdevice
+      ========================================================================*/
+
+	s = dev->subdevices + 1;
+
+	if (thisboard->ao.count) {
+		s->type = COMEDI_SUBD_AO;
+		s->subdev_flags = SDF_WRITEABLE | SDF_COMMON | SDF_GROUND;
+		s->n_chan = thisboard->ao.count;
+		s->maxdata = 0xFFFF;	/*  16 bit DAC */
+		s->range_table = &me4000_ao_range;
+		s->insn_write = me4000_ao_insn_write;
+		s->insn_read = me4000_ao_insn_read;
+	} else {
+		s->type = COMEDI_SUBD_UNUSED;
+	}
+
+    /*=========================================================================
+      Digital I/O subdevice
+      ========================================================================*/
+
+	s = dev->subdevices + 2;
+
+	if (thisboard->dio.count) {
+		s->type = COMEDI_SUBD_DIO;
+		s->subdev_flags = SDF_READABLE | SDF_WRITABLE;
+		s->n_chan = thisboard->dio.count * 8;
+		s->maxdata = 1;
+		s->range_table = &range_digital;
+		s->insn_bits = me4000_dio_insn_bits;
+		s->insn_config = me4000_dio_insn_config;
+	} else {
+		s->type = COMEDI_SUBD_UNUSED;
+	}
+
+	/*
+	 * Check for optoisolated ME-4000 version. If one the first
+	 * port is a fixed output port and the second is a fixed input port.
+	 */
+	if (!me4000_inl(dev, info->dio_context.dir_reg)) {
+		s->io_bits |= 0xFF;
+		me4000_outl(dev, ME4000_DIO_CTRL_BIT_MODE_0,
+			    info->dio_context.dir_reg);
+	}
+
+    /*=========================================================================
+      Counter subdevice
+      ========================================================================*/
+
+	s = dev->subdevices + 3;
+
+	if (thisboard->cnt.count) {
+		s->type = COMEDI_SUBD_COUNTER;
+		s->subdev_flags = SDF_READABLE | SDF_WRITABLE;
+		s->n_chan = thisboard->cnt.count;
+		s->maxdata = 0xFFFF;	/*  16 bit counters */
+		s->insn_read = me4000_cnt_insn_read;
+		s->insn_write = me4000_cnt_insn_write;
+		s->insn_config = me4000_cnt_insn_config;
+	} else {
+		s->type = COMEDI_SUBD_UNUSED;
+	}
+
+	return 0;
 }
 
-static void __devexit driver_me4000_pci_remove(struct pci_dev *dev)
+static void me4000_detach(struct comedi_device *dev)
 {
-	comedi_pci_auto_unconfig(dev);
+	if (info) {
+		if (info->pci_dev_p) {
+			reset_board(dev);
+			if (info->plx_regbase)
+				comedi_pci_disable(info->pci_dev_p);
+			pci_dev_put(info->pci_dev_p);
+		}
+	}
 }
 
-static struct pci_driver driver_me4000_pci_driver = {
-	.id_table = me4000_pci_table,
-	.probe = &driver_me4000_pci_probe,
-	.remove = __devexit_p(&driver_me4000_pci_remove)
+static struct comedi_driver me4000_driver = {
+	.driver_name	= "me4000",
+	.module		= THIS_MODULE,
+	.attach		= me4000_attach,
+	.detach		= me4000_detach,
 };
 
-static int __init driver_me4000_init_module(void)
+static int __devinit me4000_pci_probe(struct pci_dev *dev,
+				      const struct pci_device_id *ent)
 {
-	int retval;
-
-	retval = comedi_driver_register(&driver_me4000);
-	if (retval < 0)
-		return retval;
-
-	driver_me4000_pci_driver.name = (char *)driver_me4000.driver_name;
-	return pci_register_driver(&driver_me4000_pci_driver);
+	return comedi_pci_auto_config(dev, &me4000_driver);
 }
 
-static void __exit driver_me4000_cleanup_module(void)
+static void __devexit me4000_pci_remove(struct pci_dev *dev)
 {
-	pci_unregister_driver(&driver_me4000_pci_driver);
-	comedi_driver_unregister(&driver_me4000);
+	comedi_pci_auto_unconfig(dev);
 }
 
-module_init(driver_me4000_init_module);
-module_exit(driver_me4000_cleanup_module);
+static DEFINE_PCI_DEVICE_TABLE(me4000_pci_table) = {
+	{ PCI_DEVICE(PCI_VENDOR_ID_MEILHAUS, 0x4650) },
+	{ PCI_DEVICE(PCI_VENDOR_ID_MEILHAUS, 0x4660) },
+	{ PCI_DEVICE(PCI_VENDOR_ID_MEILHAUS, 0x4661) },
+	{ PCI_DEVICE(PCI_VENDOR_ID_MEILHAUS, 0x4662) },
+	{ PCI_DEVICE(PCI_VENDOR_ID_MEILHAUS, 0x4663) },
+	{ PCI_DEVICE(PCI_VENDOR_ID_MEILHAUS, 0x4670) },
+	{ PCI_DEVICE(PCI_VENDOR_ID_MEILHAUS, 0x4671) },
+	{ PCI_DEVICE(PCI_VENDOR_ID_MEILHAUS, 0x4672) },
+	{ PCI_DEVICE(PCI_VENDOR_ID_MEILHAUS, 0x4673) },
+	{ PCI_DEVICE(PCI_VENDOR_ID_MEILHAUS, 0x4680) },
+	{ PCI_DEVICE(PCI_VENDOR_ID_MEILHAUS, 0x4681) },
+	{ PCI_DEVICE(PCI_VENDOR_ID_MEILHAUS, 0x4682) },
+	{ PCI_DEVICE(PCI_VENDOR_ID_MEILHAUS, 0x4683) },
+	{ 0 }
+};
+MODULE_DEVICE_TABLE(pci, me4000_pci_table);
+
+static struct pci_driver me4000_pci_driver = {
+	.name		= "me4000",
+	.id_table	= me4000_pci_table,
+	.probe		= me4000_pci_probe,
+	.remove		= __devexit_p(me4000_pci_remove),
+};
+module_comedi_pci_driver(me4000_driver, me4000_pci_driver);
 
 MODULE_AUTHOR("Comedi http://www.comedi.org");
 MODULE_DESCRIPTION("Comedi low-level driver");
diff --git a/drivers/staging/comedi/drivers/me_daq.c b/drivers/staging/comedi/drivers/me_daq.c
index 8b812e41c52..ffe251250e6 100644
--- a/drivers/staging/comedi/drivers/me_daq.c
+++ b/drivers/staging/comedi/drivers/me_daq.c
@@ -146,10 +146,6 @@ from http://www.comedi.org
 #define ME_COUNTER_STARTDATA_B		0x0022	/* - | W */
 #define ME_COUNTER_VALUE_B		0x0022	/* R | - */
 
-/* Function prototypes */
-static int me_attach(struct comedi_device *dev, struct comedi_devconfig *it);
-static int me_detach(struct comedi_device *dev);
-
 static const struct comedi_lrange me2000_ai_range = {
 	8,
 	{
@@ -187,14 +183,6 @@ static const struct comedi_lrange me2600_ao_range = {
 	 }
 };
 
-static DEFINE_PCI_DEVICE_TABLE(me_pci_table) = {
-	{ PCI_DEVICE(PCI_VENDOR_ID_MEILHAUS, ME2600_DEVICE_ID) },
-	{ PCI_DEVICE(PCI_VENDOR_ID_MEILHAUS, ME2000_DEVICE_ID) },
-	{0}
-};
-
-MODULE_DEVICE_TABLE(pci, me_pci_table);
-
 /* Board specification structure */
 struct me_board {
 	const char *name;	/* driver name */
@@ -247,51 +235,6 @@ static const struct me_board me_boards[] = {
 
 #define me_board_nbr (sizeof(me_boards)/sizeof(struct me_board))
 
-static struct comedi_driver me_driver = {
-	.driver_name = ME_DRIVER_NAME,
-	.module = THIS_MODULE,
-	.attach = me_attach,
-	.detach = me_detach,
-};
-
-static int __devinit me_driver_pci_probe(struct pci_dev *dev,
-					 const struct pci_device_id *ent)
-{
-	return comedi_pci_auto_config(dev, me_driver.driver_name);
-}
-
-static void __devexit me_driver_pci_remove(struct pci_dev *dev)
-{
-	comedi_pci_auto_unconfig(dev);
-}
-
-static struct pci_driver me_driver_pci_driver = {
-	.id_table = me_pci_table,
-	.probe = &me_driver_pci_probe,
-	.remove = __devexit_p(&me_driver_pci_remove)
-};
-
-static int __init me_driver_init_module(void)
-{
-	int retval;
-
-	retval = comedi_driver_register(&me_driver);
-	if (retval < 0)
-		return retval;
-
-	me_driver_pci_driver.name = (char *)me_driver.driver_name;
-	return pci_register_driver(&me_driver_pci_driver);
-}
-
-static void __exit me_driver_cleanup_module(void)
-{
-	pci_unregister_driver(&me_driver_pci_driver);
-	comedi_driver_unregister(&me_driver);
-}
-
-module_init(me_driver_init_module);
-module_exit(me_driver_cleanup_module);
-
 /* Private data structure */
 struct me_private_data {
 	struct pci_dev *pci_device;
@@ -669,12 +612,6 @@ static int me_reset(struct comedi_device *dev)
 	return 0;
 }
 
-/*
- * Attach
- *
- * - Register PCI device
- * - Declare device driver capability
- */
 static int me_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 {
 	struct pci_dev *pci_device = NULL;
@@ -869,8 +806,7 @@ found:
 	return 0;
 }
 
-/* Detach */
-static int me_detach(struct comedi_device *dev)
+static void me_detach(struct comedi_device *dev)
 {
 	if (dev_private) {
 		if (dev_private->me_regbase) {
@@ -882,13 +818,44 @@ static int me_detach(struct comedi_device *dev)
 		if (dev_private->pci_device) {
 			if (dev_private->plx_regbase_size)
 				comedi_pci_disable(dev_private->pci_device);
-
 			pci_dev_put(dev_private->pci_device);
 		}
 	}
-	return 0;
 }
 
+static struct comedi_driver me_daq_driver = {
+	.driver_name	= "me_daq",
+	.module		= THIS_MODULE,
+	.attach		= me_attach,
+	.detach		= me_detach,
+};
+
+static int __devinit me_daq_pci_probe(struct pci_dev *dev,
+				      const struct pci_device_id *ent)
+{
+	return comedi_pci_auto_config(dev, &me_daq_driver);
+}
+
+static void __devexit me_daq_pci_remove(struct pci_dev *dev)
+{
+	comedi_pci_auto_unconfig(dev);
+}
+
+static DEFINE_PCI_DEVICE_TABLE(me_daq_pci_table) = {
+	{ PCI_DEVICE(PCI_VENDOR_ID_MEILHAUS, ME2600_DEVICE_ID) },
+	{ PCI_DEVICE(PCI_VENDOR_ID_MEILHAUS, ME2000_DEVICE_ID) },
+	{ 0 }
+};
+MODULE_DEVICE_TABLE(pci, me_daq_pci_table);
+
+static struct pci_driver me_daq_pci_driver = {
+	.name		= "me_daq",
+	.id_table	= me_daq_pci_table,
+	.probe		= me_daq_pci_probe,
+	.remove		= __devexit_p(me_daq_pci_remove),
+};
+module_comedi_pci_driver(me_daq_driver, me_daq_pci_driver);
+
 MODULE_AUTHOR("Comedi http://www.comedi.org");
 MODULE_DESCRIPTION("Comedi low-level driver");
 MODULE_LICENSE("GPL");
diff --git a/drivers/staging/comedi/drivers/mite.h b/drivers/staging/comedi/drivers/mite.h
index 999551f54c2..83f1b27a472 100644
--- a/drivers/staging/comedi/drivers/mite.h
+++ b/drivers/staging/comedi/drivers/mite.h
@@ -66,9 +66,9 @@ struct mite_struct {
 
 	struct pci_dev *pcidev;
 	resource_size_t mite_phys_addr;
-	void *mite_io_addr;
+	void __iomem *mite_io_addr;
 	resource_size_t daq_phys_addr;
-	void *daq_io_addr;
+	void __iomem *daq_io_addr;
 
 	struct mite_channel channels[MAX_MITE_DMA_CHANNELS];
 	short channel_allocated[MAX_MITE_DMA_CHANNELS];
diff --git a/drivers/staging/comedi/drivers/mpc624.c b/drivers/staging/comedi/drivers/mpc624.c
index dd09a6d46e5..4304e864a4d 100644
--- a/drivers/staging/comedi/drivers/mpc624.c
+++ b/drivers/staging/comedi/drivers/mpc624.c
@@ -148,131 +148,6 @@ static const struct comedi_lrange range_mpc624_bipolar10 = {
 	 }
 };
 
-/* -------------------------------------------------------------------------- */
-static int mpc624_attach(struct comedi_device *dev,
-			 struct comedi_devconfig *it);
-static int mpc624_detach(struct comedi_device *dev);
-/* -------------------------------------------------------------------------- */
-static struct comedi_driver driver_mpc624 = {
-	.driver_name = "mpc624",
-	.module = THIS_MODULE,
-	.attach = mpc624_attach,
-	.detach = mpc624_detach
-};
-
-/* -------------------------------------------------------------------------- */
-static int mpc624_ai_rinsn(struct comedi_device *dev,
-			   struct comedi_subdevice *s, struct comedi_insn *insn,
-			   unsigned int *data);
-/* -------------------------------------------------------------------------- */
-static int mpc624_attach(struct comedi_device *dev, struct comedi_devconfig *it)
-{
-	struct comedi_subdevice *s;
-	unsigned long iobase;
-
-	iobase = it->options[0];
-	printk(KERN_INFO "comedi%d: mpc624 [0x%04lx, ", dev->minor, iobase);
-	if (request_region(iobase, MPC624_SIZE, "mpc624") == NULL) {
-		printk(KERN_ERR "I/O port(s) in use\n");
-		return -EIO;
-	}
-
-	dev->iobase = iobase;
-	dev->board_name = "mpc624";
-
-	/*  Private structure initialization */
-	if (alloc_private(dev, sizeof(struct skel_private)) < 0)
-		return -ENOMEM;
-
-	switch (it->options[1]) {
-	case 0:
-		devpriv->ulConvertionRate = MPC624_SPEED_3_52_kHz;
-		printk(KERN_INFO "3.52 kHz, ");
-		break;
-	case 1:
-		devpriv->ulConvertionRate = MPC624_SPEED_1_76_kHz;
-		printk(KERN_INFO "1.76 kHz, ");
-		break;
-	case 2:
-		devpriv->ulConvertionRate = MPC624_SPEED_880_Hz;
-		printk(KERN_INFO "880 Hz, ");
-		break;
-	case 3:
-		devpriv->ulConvertionRate = MPC624_SPEED_440_Hz;
-		printk(KERN_INFO "440 Hz, ");
-		break;
-	case 4:
-		devpriv->ulConvertionRate = MPC624_SPEED_220_Hz;
-		printk(KERN_INFO "220 Hz, ");
-		break;
-	case 5:
-		devpriv->ulConvertionRate = MPC624_SPEED_110_Hz;
-		printk(KERN_INFO "110 Hz, ");
-		break;
-	case 6:
-		devpriv->ulConvertionRate = MPC624_SPEED_55_Hz;
-		printk(KERN_INFO "55 Hz, ");
-		break;
-	case 7:
-		devpriv->ulConvertionRate = MPC624_SPEED_27_5_Hz;
-		printk(KERN_INFO "27.5 Hz, ");
-		break;
-	case 8:
-		devpriv->ulConvertionRate = MPC624_SPEED_13_75_Hz;
-		printk(KERN_INFO "13.75 Hz, ");
-		break;
-	case 9:
-		devpriv->ulConvertionRate = MPC624_SPEED_6_875_Hz;
-		printk(KERN_INFO "6.875 Hz, ");
-		break;
-	default:
-		printk
-		    (KERN_ERR "illegal conversion rate setting!"
-			" Valid numbers are 0..9. Using 9 => 6.875 Hz, ");
-		devpriv->ulConvertionRate = MPC624_SPEED_3_52_kHz;
-	}
-
-	/*  Subdevices structures */
-	if (alloc_subdevices(dev, 1) < 0)
-		return -ENOMEM;
-
-	s = dev->subdevices + 0;
-	s->type = COMEDI_SUBD_AI;
-	s->subdev_flags = SDF_READABLE | SDF_DIFF;
-	s->n_chan = 8;
-	switch (it->options[1]) {
-	default:
-		s->maxdata = 0x3FFFFFFF;
-		printk(KERN_INFO "30 bit, ");
-	}
-
-	switch (it->options[1]) {
-	case 0:
-		s->range_table = &range_mpc624_bipolar1;
-		printk(KERN_INFO "1.01V]: ");
-		break;
-	default:
-		s->range_table = &range_mpc624_bipolar10;
-		printk(KERN_INFO "10.1V]: ");
-	}
-	s->len_chanlist = 1;
-	s->insn_read = mpc624_ai_rinsn;
-
-	printk(KERN_INFO "attached\n");
-
-	return 1;
-}
-
-static int mpc624_detach(struct comedi_device *dev)
-{
-	printk(KERN_INFO "comedi%d: mpc624: remove\n", dev->minor);
-
-	if (dev->iobase)
-		release_region(dev->iobase, MPC624_SIZE);
-
-	return 0;
-}
-
 /* Timeout 200ms */
 #define TIMEOUT 200
 
@@ -406,18 +281,117 @@ static int mpc624_ai_rinsn(struct comedi_device *dev,
 	return n;
 }
 
-static int __init driver_mpc624_init_module(void)
+static int mpc624_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 {
-	return comedi_driver_register(&driver_mpc624);
+	struct comedi_subdevice *s;
+	unsigned long iobase;
+
+	iobase = it->options[0];
+	printk(KERN_INFO "comedi%d: mpc624 [0x%04lx, ", dev->minor, iobase);
+	if (request_region(iobase, MPC624_SIZE, "mpc624") == NULL) {
+		printk(KERN_ERR "I/O port(s) in use\n");
+		return -EIO;
+	}
+
+	dev->iobase = iobase;
+	dev->board_name = "mpc624";
+
+	/*  Private structure initialization */
+	if (alloc_private(dev, sizeof(struct skel_private)) < 0)
+		return -ENOMEM;
+
+	switch (it->options[1]) {
+	case 0:
+		devpriv->ulConvertionRate = MPC624_SPEED_3_52_kHz;
+		printk(KERN_INFO "3.52 kHz, ");
+		break;
+	case 1:
+		devpriv->ulConvertionRate = MPC624_SPEED_1_76_kHz;
+		printk(KERN_INFO "1.76 kHz, ");
+		break;
+	case 2:
+		devpriv->ulConvertionRate = MPC624_SPEED_880_Hz;
+		printk(KERN_INFO "880 Hz, ");
+		break;
+	case 3:
+		devpriv->ulConvertionRate = MPC624_SPEED_440_Hz;
+		printk(KERN_INFO "440 Hz, ");
+		break;
+	case 4:
+		devpriv->ulConvertionRate = MPC624_SPEED_220_Hz;
+		printk(KERN_INFO "220 Hz, ");
+		break;
+	case 5:
+		devpriv->ulConvertionRate = MPC624_SPEED_110_Hz;
+		printk(KERN_INFO "110 Hz, ");
+		break;
+	case 6:
+		devpriv->ulConvertionRate = MPC624_SPEED_55_Hz;
+		printk(KERN_INFO "55 Hz, ");
+		break;
+	case 7:
+		devpriv->ulConvertionRate = MPC624_SPEED_27_5_Hz;
+		printk(KERN_INFO "27.5 Hz, ");
+		break;
+	case 8:
+		devpriv->ulConvertionRate = MPC624_SPEED_13_75_Hz;
+		printk(KERN_INFO "13.75 Hz, ");
+		break;
+	case 9:
+		devpriv->ulConvertionRate = MPC624_SPEED_6_875_Hz;
+		printk(KERN_INFO "6.875 Hz, ");
+		break;
+	default:
+		printk
+		    (KERN_ERR "illegal conversion rate setting!"
+			" Valid numbers are 0..9. Using 9 => 6.875 Hz, ");
+		devpriv->ulConvertionRate = MPC624_SPEED_3_52_kHz;
+	}
+
+	/*  Subdevices structures */
+	if (alloc_subdevices(dev, 1) < 0)
+		return -ENOMEM;
+
+	s = dev->subdevices + 0;
+	s->type = COMEDI_SUBD_AI;
+	s->subdev_flags = SDF_READABLE | SDF_DIFF;
+	s->n_chan = 8;
+	switch (it->options[1]) {
+	default:
+		s->maxdata = 0x3FFFFFFF;
+		printk(KERN_INFO "30 bit, ");
+	}
+
+	switch (it->options[1]) {
+	case 0:
+		s->range_table = &range_mpc624_bipolar1;
+		printk(KERN_INFO "1.01V]: ");
+		break;
+	default:
+		s->range_table = &range_mpc624_bipolar10;
+		printk(KERN_INFO "10.1V]: ");
+	}
+	s->len_chanlist = 1;
+	s->insn_read = mpc624_ai_rinsn;
+
+	printk(KERN_INFO "attached\n");
+
+	return 1;
 }
 
-static void __exit driver_mpc624_cleanup_module(void)
+static void mpc624_detach(struct comedi_device *dev)
 {
-	comedi_driver_unregister(&driver_mpc624);
+	if (dev->iobase)
+		release_region(dev->iobase, MPC624_SIZE);
 }
 
-module_init(driver_mpc624_init_module);
-module_exit(driver_mpc624_cleanup_module);
+static struct comedi_driver mpc624_driver = {
+	.driver_name	= "mpc624",
+	.module		= THIS_MODULE,
+	.attach		= mpc624_attach,
+	.detach		= mpc624_detach
+};
+module_comedi_driver(mpc624_driver);
 
 MODULE_AUTHOR("Comedi http://www.comedi.org");
 MODULE_DESCRIPTION("Comedi low-level driver");
diff --git a/drivers/staging/comedi/drivers/mpc8260cpm.c b/drivers/staging/comedi/drivers/mpc8260cpm.c
index 5f6816a3fe8..364470e4458 100644
--- a/drivers/staging/comedi/drivers/mpc8260cpm.c
+++ b/drivers/staging/comedi/drivers/mpc8260cpm.c
@@ -46,75 +46,6 @@ struct mpc8260cpm_private {
 
 #define devpriv ((struct mpc8260cpm_private *)dev->private)
 
-static int mpc8260cpm_attach(struct comedi_device *dev,
-			     struct comedi_devconfig *it);
-static int mpc8260cpm_detach(struct comedi_device *dev);
-static struct comedi_driver driver_mpc8260cpm = {
-	.driver_name = "mpc8260cpm",
-	.module = THIS_MODULE,
-	.attach = mpc8260cpm_attach,
-	.detach = mpc8260cpm_detach,
-};
-
-static int __init driver_mpc8260cpm_init_module(void)
-{
-	return comedi_driver_register(&driver_mpc8260cpm);
-}
-
-static void __exit driver_mpc8260cpm_cleanup_module(void)
-{
-	comedi_driver_unregister(&driver_mpc8260cpm);
-}
-
-module_init(driver_mpc8260cpm_init_module);
-module_exit(driver_mpc8260cpm_cleanup_module);
-
-static int mpc8260cpm_dio_config(struct comedi_device *dev,
-				 struct comedi_subdevice *s,
-				 struct comedi_insn *insn, unsigned int *data);
-static int mpc8260cpm_dio_bits(struct comedi_device *dev,
-			       struct comedi_subdevice *s,
-			       struct comedi_insn *insn, unsigned int *data);
-
-static int mpc8260cpm_attach(struct comedi_device *dev,
-			     struct comedi_devconfig *it)
-{
-	struct comedi_subdevice *s;
-	int i;
-
-	printk("comedi%d: mpc8260cpm: ", dev->minor);
-
-	dev->board_ptr = mpc8260cpm_boards + dev->board;
-
-	dev->board_name = thisboard->name;
-
-	if (alloc_private(dev, sizeof(struct mpc8260cpm_private)) < 0)
-		return -ENOMEM;
-
-	if (alloc_subdevices(dev, 4) < 0)
-		return -ENOMEM;
-
-	for (i = 0; i < 4; i++) {
-		s = dev->subdevices + i;
-		s->type = COMEDI_SUBD_DIO;
-		s->subdev_flags = SDF_READABLE | SDF_WRITABLE;
-		s->n_chan = 32;
-		s->maxdata = 1;
-		s->range_table = &range_digital;
-		s->insn_config = mpc8260cpm_dio_config;
-		s->insn_bits = mpc8260cpm_dio_bits;
-	}
-
-	return 1;
-}
-
-static int mpc8260cpm_detach(struct comedi_device *dev)
-{
-	printk("comedi%d: mpc8260cpm: remove\n", dev->minor);
-
-	return 0;
-}
-
 static unsigned long *cpm_pdat(int port)
 {
 	switch (port) {
@@ -184,3 +115,48 @@ static int mpc8260cpm_dio_bits(struct comedi_device *dev,
 
 	return 2;
 }
+
+static int mpc8260cpm_attach(struct comedi_device *dev,
+			     struct comedi_devconfig *it)
+{
+	struct comedi_subdevice *s;
+	int i;
+
+	printk("comedi%d: mpc8260cpm: ", dev->minor);
+
+	dev->board_ptr = mpc8260cpm_boards + dev->board;
+
+	dev->board_name = thisboard->name;
+
+	if (alloc_private(dev, sizeof(struct mpc8260cpm_private)) < 0)
+		return -ENOMEM;
+
+	if (alloc_subdevices(dev, 4) < 0)
+		return -ENOMEM;
+
+	for (i = 0; i < 4; i++) {
+		s = dev->subdevices + i;
+		s->type = COMEDI_SUBD_DIO;
+		s->subdev_flags = SDF_READABLE | SDF_WRITABLE;
+		s->n_chan = 32;
+		s->maxdata = 1;
+		s->range_table = &range_digital;
+		s->insn_config = mpc8260cpm_dio_config;
+		s->insn_bits = mpc8260cpm_dio_bits;
+	}
+
+	return 1;
+}
+
+static void mpc8260cpm_detach(struct comedi_device *dev)
+{
+	/* Nothing to cleanup */
+}
+
+static struct comedi_driver mpc8260cpm_driver = {
+	.driver_name	= "mpc8260cpm",
+	.module		= THIS_MODULE,
+	.attach		= mpc8260cpm_attach,
+	.detach		= mpc8260cpm_detach,
+};
+module_comedi_driver(mpc8260cpm_driver);
diff --git a/drivers/staging/comedi/drivers/multiq3.c b/drivers/staging/comedi/drivers/multiq3.c
index dace902d3bc..e951e73d66f 100644
--- a/drivers/staging/comedi/drivers/multiq3.c
+++ b/drivers/staging/comedi/drivers/multiq3.c
@@ -83,29 +83,6 @@ Devices: [Quanser Consulting] MultiQ-3 (multiq3)
 
 #define MULTIQ3_TIMEOUT 30
 
-static int multiq3_attach(struct comedi_device *dev,
-			  struct comedi_devconfig *it);
-static int multiq3_detach(struct comedi_device *dev);
-static struct comedi_driver driver_multiq3 = {
-	.driver_name = "multiq3",
-	.module = THIS_MODULE,
-	.attach = multiq3_attach,
-	.detach = multiq3_detach,
-};
-
-static int __init driver_multiq3_init_module(void)
-{
-	return comedi_driver_register(&driver_multiq3);
-}
-
-static void __exit driver_multiq3_cleanup_module(void)
-{
-	comedi_driver_unregister(&driver_multiq3);
-}
-
-module_init(driver_multiq3_init_module);
-module_exit(driver_multiq3_cleanup_module);
-
 struct multiq3_private {
 	unsigned int ao_readback[2];
 };
@@ -338,18 +315,22 @@ static int multiq3_attach(struct comedi_device *dev,
 	return 0;
 }
 
-static int multiq3_detach(struct comedi_device *dev)
+static void multiq3_detach(struct comedi_device *dev)
 {
-	printk(KERN_INFO "comedi%d: multiq3: remove\n", dev->minor);
-
 	if (dev->iobase)
 		release_region(dev->iobase, MULTIQ3_SIZE);
 	if (dev->irq)
 		free_irq(dev->irq, dev);
-
-	return 0;
 }
 
+static struct comedi_driver multiq3_driver = {
+	.driver_name	= "multiq3",
+	.module		= THIS_MODULE,
+	.attach		= multiq3_attach,
+	.detach		= multiq3_detach,
+};
+module_comedi_driver(multiq3_driver);
+
 MODULE_AUTHOR("Comedi http://www.comedi.org");
 MODULE_DESCRIPTION("Comedi low-level driver");
 MODULE_LICENSE("GPL");
diff --git a/drivers/staging/comedi/drivers/ni_6527.c b/drivers/staging/comedi/drivers/ni_6527.c
index 54741c9e1af..b02aa0efcd8 100644
--- a/drivers/staging/comedi/drivers/ni_6527.c
+++ b/drivers/staging/comedi/drivers/ni_6527.c
@@ -78,7 +78,7 @@ Updated: Sat, 25 Jan 2003 13:24:40 -0800
 
 static int ni6527_attach(struct comedi_device *dev,
 			 struct comedi_devconfig *it);
-static int ni6527_detach(struct comedi_device *dev);
+static void ni6527_detach(struct comedi_device *dev);
 static struct comedi_driver driver_ni6527 = {
 	.driver_name = "ni6527",
 	.module = THIS_MODULE,
@@ -449,19 +449,15 @@ static int ni6527_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 	return 0;
 }
 
-static int ni6527_detach(struct comedi_device *dev)
+static void ni6527_detach(struct comedi_device *dev)
 {
 	if (devpriv && devpriv->mite && devpriv->mite->daq_io_addr)
 		writeb(0x00,
 		       devpriv->mite->daq_io_addr + Master_Interrupt_Control);
-
 	if (dev->irq)
 		free_irq(dev->irq, dev);
-
 	if (devpriv && devpriv->mite)
 		mite_unsetup(devpriv->mite);
-
-	return 0;
 }
 
 static int ni6527_find_device(struct comedi_device *dev, int bus, int slot)
@@ -493,7 +489,7 @@ static int ni6527_find_device(struct comedi_device *dev, int bus, int slot)
 static int __devinit driver_ni6527_pci_probe(struct pci_dev *dev,
 					     const struct pci_device_id *ent)
 {
-	return comedi_pci_auto_config(dev, driver_ni6527.driver_name);
+	return comedi_pci_auto_config(dev, &driver_ni6527);
 }
 
 static void __devexit driver_ni6527_pci_remove(struct pci_dev *dev)
diff --git a/drivers/staging/comedi/drivers/ni_65xx.c b/drivers/staging/comedi/drivers/ni_65xx.c
index 403fc0997d3..0d27a9323bc 100644
--- a/drivers/staging/comedi/drivers/ni_65xx.c
+++ b/drivers/staging/comedi/drivers/ni_65xx.c
@@ -111,7 +111,7 @@ static inline unsigned Filter_Enable(unsigned port)
 
 static int ni_65xx_attach(struct comedi_device *dev,
 			  struct comedi_devconfig *it);
-static int ni_65xx_detach(struct comedi_device *dev);
+static void ni_65xx_detach(struct comedi_device *dev);
 static struct comedi_driver driver_ni_65xx = {
 	.driver_name = "ni_65xx",
 	.module = THIS_MODULE,
@@ -784,7 +784,7 @@ static int ni_65xx_attach(struct comedi_device *dev,
 	return 0;
 }
 
-static int ni_65xx_detach(struct comedi_device *dev)
+static void ni_65xx_detach(struct comedi_device *dev)
 {
 	if (private(dev) && private(dev)->mite
 	    && private(dev)->mite->daq_io_addr) {
@@ -792,10 +792,8 @@ static int ni_65xx_detach(struct comedi_device *dev)
 		       private(dev)->mite->daq_io_addr +
 		       Master_Interrupt_Control);
 	}
-
 	if (dev->irq)
 		free_irq(dev->irq, dev);
-
 	if (private(dev)) {
 		unsigned i;
 		for (i = 0; i < dev->n_subdevices; ++i) {
@@ -805,7 +803,6 @@ static int ni_65xx_detach(struct comedi_device *dev)
 		if (private(dev)->mite)
 			mite_unsetup(private(dev)->mite);
 	}
-	return 0;
 }
 
 static int ni_65xx_find_device(struct comedi_device *dev, int bus, int slot)
@@ -837,7 +834,7 @@ static int ni_65xx_find_device(struct comedi_device *dev, int bus, int slot)
 static int __devinit driver_ni_65xx_pci_probe(struct pci_dev *dev,
 					      const struct pci_device_id *ent)
 {
-	return comedi_pci_auto_config(dev, driver_ni_65xx.driver_name);
+	return comedi_pci_auto_config(dev, &driver_ni_65xx);
 }
 
 static void __devexit driver_ni_65xx_pci_remove(struct pci_dev *dev)
diff --git a/drivers/staging/comedi/drivers/ni_660x.c b/drivers/staging/comedi/drivers/ni_660x.c
index 35f3a474982..8c40730e296 100644
--- a/drivers/staging/comedi/drivers/ni_660x.c
+++ b/drivers/staging/comedi/drivers/ni_660x.c
@@ -458,7 +458,7 @@ static inline const struct ni_660x_board *board(struct comedi_device *dev)
 
 static int ni_660x_attach(struct comedi_device *dev,
 			  struct comedi_devconfig *it);
-static int ni_660x_detach(struct comedi_device *dev);
+static void ni_660x_detach(struct comedi_device *dev);
 static void init_tio_chip(struct comedi_device *dev, int chipset);
 static void ni_660x_select_pfi_output(struct comedi_device *dev,
 				      unsigned pfi_channel,
@@ -474,7 +474,7 @@ static struct comedi_driver driver_ni_660x = {
 static int __devinit driver_ni_660x_pci_probe(struct pci_dev *dev,
 					      const struct pci_device_id *ent)
 {
-	return comedi_pci_auto_config(dev, driver_ni_660x.driver_name);
+	return comedi_pci_auto_config(dev, &driver_ni_660x);
 }
 
 static void __devexit driver_ni_660x_pci_remove(struct pci_dev *dev)
@@ -761,7 +761,7 @@ static inline void ni_660x_write_register(struct comedi_device *dev,
 					  unsigned chip_index, unsigned bits,
 					  enum NI_660x_Register reg)
 {
-	void *const write_address =
+	void __iomem *write_address =
 	    private(dev)->mite->daq_io_addr + GPCT_OFFSET[chip_index] +
 	    registerData[reg].offset;
 
@@ -784,7 +784,7 @@ static inline unsigned ni_660x_read_register(struct comedi_device *dev,
 					     unsigned chip_index,
 					     enum NI_660x_Register reg)
 {
-	void *const read_address =
+	void __iomem *read_address =
 	    private(dev)->mite->daq_io_addr + GPCT_OFFSET[chip_index] +
 	    registerData[reg].offset;
 
@@ -1188,14 +1188,10 @@ static int ni_660x_attach(struct comedi_device *dev,
 	return 0;
 }
 
-static int ni_660x_detach(struct comedi_device *dev)
+static void ni_660x_detach(struct comedi_device *dev)
 {
-	printk(KERN_INFO "comedi%d: ni_660x: remove\n", dev->minor);
-
-	/* Free irq */
 	if (dev->irq)
 		free_irq(dev->irq, dev);
-
 	if (dev->private) {
 		if (private(dev)->counter_dev)
 			ni_gpct_device_destroy(private(dev)->counter_dev);
@@ -1204,7 +1200,6 @@ static int ni_660x_detach(struct comedi_device *dev)
 			mite_unsetup(private(dev)->mite);
 		}
 	}
-	return 0;
 }
 
 static int
diff --git a/drivers/staging/comedi/drivers/ni_670x.c b/drivers/staging/comedi/drivers/ni_670x.c
index d8d91f90060..a9cf94fd0c3 100644
--- a/drivers/staging/comedi/drivers/ni_670x.c
+++ b/drivers/staging/comedi/drivers/ni_670x.c
@@ -111,7 +111,7 @@ struct ni_670x_private {
 
 static int ni_670x_attach(struct comedi_device *dev,
 			  struct comedi_devconfig *it);
-static int ni_670x_detach(struct comedi_device *dev);
+static void ni_670x_detach(struct comedi_device *dev);
 
 static struct comedi_driver driver_ni_670x = {
 	.driver_name = "ni_670x",
@@ -123,7 +123,7 @@ static struct comedi_driver driver_ni_670x = {
 static int __devinit driver_ni_670x_pci_probe(struct pci_dev *dev,
 					      const struct pci_device_id *ent)
 {
-	return comedi_pci_auto_config(dev, driver_ni_670x.driver_name);
+	return comedi_pci_auto_config(dev, &driver_ni_670x);
 }
 
 static void __devexit driver_ni_670x_pci_remove(struct pci_dev *dev)
@@ -249,19 +249,13 @@ static int ni_670x_attach(struct comedi_device *dev,
 	return 1;
 }
 
-static int ni_670x_detach(struct comedi_device *dev)
+static void ni_670x_detach(struct comedi_device *dev)
 {
-	printk(KERN_INFO "comedi%d: ni_670x: remove\n", dev->minor);
-
 	kfree(dev->subdevices[0].range_table_list);
-
 	if (dev->private && devpriv->mite)
 		mite_unsetup(devpriv->mite);
-
 	if (dev->irq)
 		free_irq(dev->irq, dev);
-
-	return 0;
 }
 
 static int ni_670x_ao_winsn(struct comedi_device *dev,
diff --git a/drivers/staging/comedi/drivers/ni_at_a2150.c b/drivers/staging/comedi/drivers/ni_at_a2150.c
index c25e44c1905..ae896a09415 100644
--- a/drivers/staging/comedi/drivers/ni_at_a2150.c
+++ b/drivers/staging/comedi/drivers/ni_at_a2150.c
@@ -171,46 +171,13 @@ struct a2150_private {
 
 #define devpriv ((struct a2150_private *)dev->private)
 
-static int a2150_attach(struct comedi_device *dev, struct comedi_devconfig *it);
-static int a2150_detach(struct comedi_device *dev);
 static int a2150_cancel(struct comedi_device *dev, struct comedi_subdevice *s);
 
-static struct comedi_driver driver_a2150 = {
-	.driver_name = "ni_at_a2150",
-	.module = THIS_MODULE,
-	.attach = a2150_attach,
-	.detach = a2150_detach,
-};
-
-static irqreturn_t a2150_interrupt(int irq, void *d);
-static int a2150_ai_cmdtest(struct comedi_device *dev,
-			    struct comedi_subdevice *s, struct comedi_cmd *cmd);
-static int a2150_ai_cmd(struct comedi_device *dev, struct comedi_subdevice *s);
-static int a2150_ai_rinsn(struct comedi_device *dev, struct comedi_subdevice *s,
-			  struct comedi_insn *insn, unsigned int *data);
 static int a2150_get_timing(struct comedi_device *dev, unsigned int *period,
 			    int flags);
-static int a2150_probe(struct comedi_device *dev);
 static int a2150_set_chanlist(struct comedi_device *dev,
 			      unsigned int start_channel,
 			      unsigned int num_channels);
-/*
- * A convenient macro that defines init_module() and cleanup_module(),
- * as necessary.
- */
-static int __init driver_a2150_init_module(void)
-{
-	return comedi_driver_register(&driver_a2150);
-}
-
-static void __exit driver_a2150_cleanup_module(void)
-{
-	comedi_driver_unregister(&driver_a2150);
-}
-
-module_init(driver_a2150_init_module);
-module_exit(driver_a2150_cleanup_module);
-
 #ifdef A2150_DEBUG
 
 static void ni_dump_regs(struct comedi_device *dev)
@@ -331,161 +298,6 @@ static irqreturn_t a2150_interrupt(int irq, void *d)
 	return IRQ_HANDLED;
 }
 
-/* probes board type, returns offset */
-static int a2150_probe(struct comedi_device *dev)
-{
-	int status = inw(dev->iobase + STATUS_REG);
-	return ID_BITS(status);
-}
-
-static int a2150_attach(struct comedi_device *dev, struct comedi_devconfig *it)
-{
-	struct comedi_subdevice *s;
-	unsigned long iobase = it->options[0];
-	unsigned int irq = it->options[1];
-	unsigned int dma = it->options[2];
-	static const int timeout = 2000;
-	int i;
-
-	printk("comedi%d: %s: io 0x%lx", dev->minor, driver_a2150.driver_name,
-	       iobase);
-	if (irq) {
-		printk(", irq %u", irq);
-	} else {
-		printk(", no irq");
-	}
-	if (dma) {
-		printk(", dma %u", dma);
-	} else {
-		printk(", no dma");
-	}
-	printk("\n");
-
-	/* allocate and initialize dev->private */
-	if (alloc_private(dev, sizeof(struct a2150_private)) < 0)
-		return -ENOMEM;
-
-	if (iobase == 0) {
-		printk(" io base address required\n");
-		return -EINVAL;
-	}
-
-	/* check if io addresses are available */
-	if (!request_region(iobase, A2150_SIZE, driver_a2150.driver_name)) {
-		printk(" I/O port conflict\n");
-		return -EIO;
-	}
-	dev->iobase = iobase;
-
-	/* grab our IRQ */
-	if (irq) {
-		/*  check that irq is supported */
-		if (irq < 3 || irq == 8 || irq == 13 || irq > 15) {
-			printk(" invalid irq line %u\n", irq);
-			return -EINVAL;
-		}
-		if (request_irq(irq, a2150_interrupt, 0,
-				driver_a2150.driver_name, dev)) {
-			printk("unable to allocate irq %u\n", irq);
-			return -EINVAL;
-		}
-		devpriv->irq_dma_bits |= IRQ_LVL_BITS(irq);
-		dev->irq = irq;
-	}
-	/*  initialize dma */
-	if (dma) {
-		if (dma == 4 || dma > 7) {
-			printk(" invalid dma channel %u\n", dma);
-			return -EINVAL;
-		}
-		if (request_dma(dma, driver_a2150.driver_name)) {
-			printk(" failed to allocate dma channel %u\n", dma);
-			return -EINVAL;
-		}
-		devpriv->dma = dma;
-		devpriv->dma_buffer =
-		    kmalloc(A2150_DMA_BUFFER_SIZE, GFP_KERNEL | GFP_DMA);
-		if (devpriv->dma_buffer == NULL)
-			return -ENOMEM;
-
-		disable_dma(dma);
-		set_dma_mode(dma, DMA_MODE_READ);
-
-		devpriv->irq_dma_bits |= DMA_CHAN_BITS(dma);
-	}
-
-	dev->board_ptr = a2150_boards + a2150_probe(dev);
-	dev->board_name = thisboard->name;
-
-	if (alloc_subdevices(dev, 1) < 0)
-		return -ENOMEM;
-
-	/* analog input subdevice */
-	s = dev->subdevices + 0;
-	dev->read_subdev = s;
-	s->type = COMEDI_SUBD_AI;
-	s->subdev_flags = SDF_READABLE | SDF_GROUND | SDF_OTHER | SDF_CMD_READ;
-	s->n_chan = 4;
-	s->len_chanlist = 4;
-	s->maxdata = 0xffff;
-	s->range_table = &range_a2150;
-	s->do_cmd = a2150_ai_cmd;
-	s->do_cmdtest = a2150_ai_cmdtest;
-	s->insn_read = a2150_ai_rinsn;
-	s->cancel = a2150_cancel;
-
-	/* need to do this for software counting of completed conversions, to
-	 * prevent hardware count from stopping acquisition */
-	outw(HW_COUNT_DISABLE, dev->iobase + I8253_MODE_REG);
-
-	/*  set card's irq and dma levels */
-	outw(devpriv->irq_dma_bits, dev->iobase + IRQ_DMA_CNTRL_REG);
-
-	/*  reset and sync adc clock circuitry */
-	outw_p(DPD_BIT | APD_BIT, dev->iobase + CONFIG_REG);
-	outw_p(DPD_BIT, dev->iobase + CONFIG_REG);
-	/*  initialize configuration register */
-	devpriv->config_bits = 0;
-	outw(devpriv->config_bits, dev->iobase + CONFIG_REG);
-	/*  wait until offset calibration is done, then enable analog inputs */
-	for (i = 0; i < timeout; i++) {
-		if ((DCAL_BIT & inw(dev->iobase + STATUS_REG)) == 0)
-			break;
-		udelay(1000);
-	}
-	if (i == timeout) {
-		printk
-		    (" timed out waiting for offset calibration to complete\n");
-		return -ETIME;
-	}
-	devpriv->config_bits |= ENABLE0_BIT | ENABLE1_BIT;
-	outw(devpriv->config_bits, dev->iobase + CONFIG_REG);
-
-	return 0;
-};
-
-static int a2150_detach(struct comedi_device *dev)
-{
-	printk("comedi%d: %s: remove\n", dev->minor, driver_a2150.driver_name);
-
-	/* only free stuff if it has been allocated by _attach */
-	if (dev->iobase) {
-		/*  put board in power-down mode */
-		outw(APD_BIT | DPD_BIT, dev->iobase + CONFIG_REG);
-		release_region(dev->iobase, A2150_SIZE);
-	}
-
-	if (dev->irq)
-		free_irq(dev->irq, dev);
-	if (devpriv) {
-		if (devpriv->dma)
-			free_dma(devpriv->dma);
-		kfree(devpriv->dma_buffer);
-	}
-
-	return 0;
-};
-
 static int a2150_cancel(struct comedi_device *dev, struct comedi_subdevice *s)
 {
 	/*  disable dma on card */
@@ -539,7 +351,10 @@ static int a2150_ai_cmdtest(struct comedi_device *dev,
 	if (err)
 		return 1;
 
-	/* step 2: make sure trigger sources are unique and mutually compatible */
+	/*
+	 * step 2: make sure trigger sources are unique and mutually
+	 * compatible
+	 */
 
 	if (cmd->start_src != TRIG_NOW && cmd->start_src != TRIG_EXT)
 		err++;
@@ -771,7 +586,10 @@ static int a2150_ai_rinsn(struct comedi_device *dev, struct comedi_subdevice *s,
 	/*  start acquisition for soft trigger */
 	outw(0, dev->iobase + FIFO_START_REG);
 
-	/* there is a 35.6 sample delay for data to get through the antialias filter */
+	/*
+	 * there is a 35.6 sample delay for data to get through the
+	 * antialias filter
+	 */
 	for (n = 0; n < filter_delay; n++) {
 		for (i = 0; i < timeout; i++) {
 			if (inw(dev->iobase + STATUS_REG) & FNE_BIT)
@@ -812,8 +630,10 @@ static int a2150_ai_rinsn(struct comedi_device *dev, struct comedi_subdevice *s,
 	return n;
 }
 
-/* sets bits in devpriv->clock_bits to nearest approximation of requested period,
- * adjusts requested period to actual timing. */
+/*
+ * sets bits in devpriv->clock_bits to nearest approximation of requested
+ * period, adjusts requested period to actual timing.
+ */
 static int a2150_get_timing(struct comedi_device *dev, unsigned int *period,
 			    int flags)
 {
@@ -920,6 +740,162 @@ static int a2150_set_chanlist(struct comedi_device *dev,
 	return 0;
 }
 
+/* probes board type, returns offset */
+static int a2150_probe(struct comedi_device *dev)
+{
+	int status = inw(dev->iobase + STATUS_REG);
+	return ID_BITS(status);
+}
+
+static int a2150_attach(struct comedi_device *dev, struct comedi_devconfig *it)
+{
+	struct comedi_subdevice *s;
+	unsigned long iobase = it->options[0];
+	unsigned int irq = it->options[1];
+	unsigned int dma = it->options[2];
+	static const int timeout = 2000;
+	int i;
+
+	printk("comedi%d: %s: io 0x%lx", dev->minor, dev->driver->driver_name,
+	       iobase);
+	if (irq) {
+		printk(", irq %u", irq);
+	} else {
+		printk(", no irq");
+	}
+	if (dma) {
+		printk(", dma %u", dma);
+	} else {
+		printk(", no dma");
+	}
+	printk("\n");
+
+	/* allocate and initialize dev->private */
+	if (alloc_private(dev, sizeof(struct a2150_private)) < 0)
+		return -ENOMEM;
+
+	if (iobase == 0) {
+		printk(" io base address required\n");
+		return -EINVAL;
+	}
+
+	/* check if io addresses are available */
+	if (!request_region(iobase, A2150_SIZE, dev->driver->driver_name)) {
+		printk(" I/O port conflict\n");
+		return -EIO;
+	}
+	dev->iobase = iobase;
+
+	/* grab our IRQ */
+	if (irq) {
+		/*  check that irq is supported */
+		if (irq < 3 || irq == 8 || irq == 13 || irq > 15) {
+			printk(" invalid irq line %u\n", irq);
+			return -EINVAL;
+		}
+		if (request_irq(irq, a2150_interrupt, 0,
+				dev->driver->driver_name, dev)) {
+			printk("unable to allocate irq %u\n", irq);
+			return -EINVAL;
+		}
+		devpriv->irq_dma_bits |= IRQ_LVL_BITS(irq);
+		dev->irq = irq;
+	}
+	/*  initialize dma */
+	if (dma) {
+		if (dma == 4 || dma > 7) {
+			printk(" invalid dma channel %u\n", dma);
+			return -EINVAL;
+		}
+		if (request_dma(dma, dev->driver->driver_name)) {
+			printk(" failed to allocate dma channel %u\n", dma);
+			return -EINVAL;
+		}
+		devpriv->dma = dma;
+		devpriv->dma_buffer =
+		    kmalloc(A2150_DMA_BUFFER_SIZE, GFP_KERNEL | GFP_DMA);
+		if (devpriv->dma_buffer == NULL)
+			return -ENOMEM;
+
+		disable_dma(dma);
+		set_dma_mode(dma, DMA_MODE_READ);
+
+		devpriv->irq_dma_bits |= DMA_CHAN_BITS(dma);
+	}
+
+	dev->board_ptr = a2150_boards + a2150_probe(dev);
+	dev->board_name = thisboard->name;
+
+	if (alloc_subdevices(dev, 1) < 0)
+		return -ENOMEM;
+
+	/* analog input subdevice */
+	s = dev->subdevices + 0;
+	dev->read_subdev = s;
+	s->type = COMEDI_SUBD_AI;
+	s->subdev_flags = SDF_READABLE | SDF_GROUND | SDF_OTHER | SDF_CMD_READ;
+	s->n_chan = 4;
+	s->len_chanlist = 4;
+	s->maxdata = 0xffff;
+	s->range_table = &range_a2150;
+	s->do_cmd = a2150_ai_cmd;
+	s->do_cmdtest = a2150_ai_cmdtest;
+	s->insn_read = a2150_ai_rinsn;
+	s->cancel = a2150_cancel;
+
+	/* need to do this for software counting of completed conversions, to
+	 * prevent hardware count from stopping acquisition */
+	outw(HW_COUNT_DISABLE, dev->iobase + I8253_MODE_REG);
+
+	/*  set card's irq and dma levels */
+	outw(devpriv->irq_dma_bits, dev->iobase + IRQ_DMA_CNTRL_REG);
+
+	/*  reset and sync adc clock circuitry */
+	outw_p(DPD_BIT | APD_BIT, dev->iobase + CONFIG_REG);
+	outw_p(DPD_BIT, dev->iobase + CONFIG_REG);
+	/*  initialize configuration register */
+	devpriv->config_bits = 0;
+	outw(devpriv->config_bits, dev->iobase + CONFIG_REG);
+	/*  wait until offset calibration is done, then enable analog inputs */
+	for (i = 0; i < timeout; i++) {
+		if ((DCAL_BIT & inw(dev->iobase + STATUS_REG)) == 0)
+			break;
+		udelay(1000);
+	}
+	if (i == timeout) {
+		printk
+		    (" timed out waiting for offset calibration to complete\n");
+		return -ETIME;
+	}
+	devpriv->config_bits |= ENABLE0_BIT | ENABLE1_BIT;
+	outw(devpriv->config_bits, dev->iobase + CONFIG_REG);
+
+	return 0;
+};
+
+static void a2150_detach(struct comedi_device *dev)
+{
+	if (dev->iobase) {
+		outw(APD_BIT | DPD_BIT, dev->iobase + CONFIG_REG);
+		release_region(dev->iobase, A2150_SIZE);
+	}
+	if (dev->irq)
+		free_irq(dev->irq, dev);
+	if (devpriv) {
+		if (devpriv->dma)
+			free_dma(devpriv->dma);
+		kfree(devpriv->dma_buffer);
+	}
+};
+
+static struct comedi_driver ni_at_a2150_driver = {
+	.driver_name	= "ni_at_a2150",
+	.module		= THIS_MODULE,
+	.attach		= a2150_attach,
+	.detach		= a2150_detach,
+};
+module_comedi_driver(ni_at_a2150_driver);
+
 MODULE_AUTHOR("Comedi http://www.comedi.org");
 MODULE_DESCRIPTION("Comedi low-level driver");
 MODULE_LICENSE("GPL");
diff --git a/drivers/staging/comedi/drivers/ni_at_ao.c b/drivers/staging/comedi/drivers/ni_at_ao.c
index 138dcc2275a..c43dd8ada1d 100644
--- a/drivers/staging/comedi/drivers/ni_at_ao.c
+++ b/drivers/staging/comedi/drivers/ni_at_ao.c
@@ -157,17 +157,6 @@ struct atao_board {
 	int n_ao_chans;
 };
 
-static const struct atao_board atao_boards[] = {
-	{
-	 .name = "ai-ao-6",
-	 .n_ao_chans = 6,
-	 },
-	{
-	 .name = "ai-ao-10",
-	 .n_ao_chans = 10,
-	 },
-};
-
 #define thisboard ((struct atao_board *)dev->board_ptr)
 
 struct atao_private {
@@ -182,133 +171,6 @@ struct atao_private {
 
 #define devpriv ((struct atao_private *)dev->private)
 
-static int atao_attach(struct comedi_device *dev, struct comedi_devconfig *it);
-static int atao_detach(struct comedi_device *dev);
-static struct comedi_driver driver_atao = {
-	.driver_name = "ni_at_ao",
-	.module = THIS_MODULE,
-	.attach = atao_attach,
-	.detach = atao_detach,
-	.board_name = &atao_boards[0].name,
-	.offset = sizeof(struct atao_board),
-	.num_names = ARRAY_SIZE(atao_boards),
-};
-
-static int __init driver_atao_init_module(void)
-{
-	return comedi_driver_register(&driver_atao);
-}
-
-static void __exit driver_atao_cleanup_module(void)
-{
-	comedi_driver_unregister(&driver_atao);
-}
-
-module_init(driver_atao_init_module);
-module_exit(driver_atao_cleanup_module);
-
-static void atao_reset(struct comedi_device *dev);
-
-static int atao_ao_winsn(struct comedi_device *dev, struct comedi_subdevice *s,
-			 struct comedi_insn *insn, unsigned int *data);
-static int atao_ao_rinsn(struct comedi_device *dev, struct comedi_subdevice *s,
-			 struct comedi_insn *insn, unsigned int *data);
-static int atao_dio_insn_bits(struct comedi_device *dev,
-			      struct comedi_subdevice *s,
-			      struct comedi_insn *insn, unsigned int *data);
-static int atao_dio_insn_config(struct comedi_device *dev,
-				struct comedi_subdevice *s,
-				struct comedi_insn *insn, unsigned int *data);
-static int atao_calib_insn_read(struct comedi_device *dev,
-				struct comedi_subdevice *s,
-				struct comedi_insn *insn, unsigned int *data);
-static int atao_calib_insn_write(struct comedi_device *dev,
-				 struct comedi_subdevice *s,
-				 struct comedi_insn *insn, unsigned int *data);
-
-static int atao_attach(struct comedi_device *dev, struct comedi_devconfig *it)
-{
-	struct comedi_subdevice *s;
-	unsigned long iobase;
-	int ao_unipolar;
-
-	iobase = it->options[0];
-	if (iobase == 0)
-		iobase = 0x1c0;
-	ao_unipolar = it->options[3];
-
-	printk(KERN_INFO "comedi%d: ni_at_ao: 0x%04lx", dev->minor, iobase);
-
-	if (!request_region(iobase, ATAO_SIZE, "ni_at_ao")) {
-		printk(" I/O port conflict\n");
-		return -EIO;
-	}
-	dev->iobase = iobase;
-
-	/* dev->board_ptr = atao_probe(dev); */
-
-	dev->board_name = thisboard->name;
-
-	if (alloc_private(dev, sizeof(struct atao_private)) < 0)
-		return -ENOMEM;
-
-	if (alloc_subdevices(dev, 4) < 0)
-		return -ENOMEM;
-
-	s = dev->subdevices + 0;
-	/* analog output subdevice */
-	s->type = COMEDI_SUBD_AO;
-	s->subdev_flags = SDF_WRITABLE;
-	s->n_chan = thisboard->n_ao_chans;
-	s->maxdata = (1 << 12) - 1;
-	if (ao_unipolar)
-		s->range_table = &range_unipolar10;
-	else
-		s->range_table = &range_bipolar10;
-	s->insn_write = &atao_ao_winsn;
-	s->insn_read = &atao_ao_rinsn;
-
-	s = dev->subdevices + 1;
-	/* digital i/o subdevice */
-	s->type = COMEDI_SUBD_DIO;
-	s->subdev_flags = SDF_READABLE | SDF_WRITABLE;
-	s->n_chan = 8;
-	s->maxdata = 1;
-	s->range_table = &range_digital;
-	s->insn_bits = atao_dio_insn_bits;
-	s->insn_config = atao_dio_insn_config;
-
-	s = dev->subdevices + 2;
-	/* caldac subdevice */
-	s->type = COMEDI_SUBD_CALIB;
-	s->subdev_flags = SDF_WRITABLE | SDF_INTERNAL;
-	s->n_chan = 21;
-	s->maxdata = 0xff;
-	s->insn_read = atao_calib_insn_read;
-	s->insn_write = atao_calib_insn_write;
-
-	s = dev->subdevices + 3;
-	/* eeprom subdevice */
-	/* s->type=COMEDI_SUBD_EEPROM; */
-	s->type = COMEDI_SUBD_UNUSED;
-
-	atao_reset(dev);
-
-	printk(KERN_INFO "\n");
-
-	return 0;
-}
-
-static int atao_detach(struct comedi_device *dev)
-{
-	printk(KERN_INFO "comedi%d: atao: remove\n", dev->minor);
-
-	if (dev->iobase)
-		release_region(dev->iobase, ATAO_SIZE);
-
-	return 0;
-}
-
 static void atao_reset(struct comedi_device *dev)
 {
 	/* This is the reset sequence described in the manual */
@@ -471,6 +333,106 @@ static int atao_calib_insn_write(struct comedi_device *dev,
 	return insn->n;
 }
 
+static int atao_attach(struct comedi_device *dev, struct comedi_devconfig *it)
+{
+	struct comedi_subdevice *s;
+	unsigned long iobase;
+	int ao_unipolar;
+
+	iobase = it->options[0];
+	if (iobase == 0)
+		iobase = 0x1c0;
+	ao_unipolar = it->options[3];
+
+	printk(KERN_INFO "comedi%d: ni_at_ao: 0x%04lx", dev->minor, iobase);
+
+	if (!request_region(iobase, ATAO_SIZE, "ni_at_ao")) {
+		printk(" I/O port conflict\n");
+		return -EIO;
+	}
+	dev->iobase = iobase;
+
+	/* dev->board_ptr = atao_probe(dev); */
+
+	dev->board_name = thisboard->name;
+
+	if (alloc_private(dev, sizeof(struct atao_private)) < 0)
+		return -ENOMEM;
+
+	if (alloc_subdevices(dev, 4) < 0)
+		return -ENOMEM;
+
+	s = dev->subdevices + 0;
+	/* analog output subdevice */
+	s->type = COMEDI_SUBD_AO;
+	s->subdev_flags = SDF_WRITABLE;
+	s->n_chan = thisboard->n_ao_chans;
+	s->maxdata = (1 << 12) - 1;
+	if (ao_unipolar)
+		s->range_table = &range_unipolar10;
+	else
+		s->range_table = &range_bipolar10;
+	s->insn_write = &atao_ao_winsn;
+	s->insn_read = &atao_ao_rinsn;
+
+	s = dev->subdevices + 1;
+	/* digital i/o subdevice */
+	s->type = COMEDI_SUBD_DIO;
+	s->subdev_flags = SDF_READABLE | SDF_WRITABLE;
+	s->n_chan = 8;
+	s->maxdata = 1;
+	s->range_table = &range_digital;
+	s->insn_bits = atao_dio_insn_bits;
+	s->insn_config = atao_dio_insn_config;
+
+	s = dev->subdevices + 2;
+	/* caldac subdevice */
+	s->type = COMEDI_SUBD_CALIB;
+	s->subdev_flags = SDF_WRITABLE | SDF_INTERNAL;
+	s->n_chan = 21;
+	s->maxdata = 0xff;
+	s->insn_read = atao_calib_insn_read;
+	s->insn_write = atao_calib_insn_write;
+
+	s = dev->subdevices + 3;
+	/* eeprom subdevice */
+	/* s->type=COMEDI_SUBD_EEPROM; */
+	s->type = COMEDI_SUBD_UNUSED;
+
+	atao_reset(dev);
+
+	printk(KERN_INFO "\n");
+
+	return 0;
+}
+
+static void atao_detach(struct comedi_device *dev)
+{
+	if (dev->iobase)
+		release_region(dev->iobase, ATAO_SIZE);
+}
+
+static const struct atao_board atao_boards[] = {
+	{
+		.name		= "ai-ao-6",
+		.n_ao_chans	= 6,
+	}, {
+		.name		= "ai-ao-10",
+		.n_ao_chans	= 10,
+	},
+};
+
+static struct comedi_driver ni_at_ao_driver = {
+	.driver_name	= "ni_at_ao",
+	.module		= THIS_MODULE,
+	.attach		= atao_attach,
+	.detach		= atao_detach,
+	.board_name	= &atao_boards[0].name,
+	.offset		= sizeof(struct atao_board),
+	.num_names	= ARRAY_SIZE(atao_boards),
+};
+module_comedi_driver(ni_at_ao_driver);
+
 MODULE_AUTHOR("Comedi http://www.comedi.org");
 MODULE_DESCRIPTION("Comedi low-level driver");
 MODULE_LICENSE("GPL");
diff --git a/drivers/staging/comedi/drivers/ni_atmio.c b/drivers/staging/comedi/drivers/ni_atmio.c
index 647c228abfb..6448373878e 100644
--- a/drivers/staging/comedi/drivers/ni_atmio.c
+++ b/drivers/staging/comedi/drivers/ni_atmio.c
@@ -343,49 +343,8 @@ static struct pnp_device_id device_ids[] = {
 
 MODULE_DEVICE_TABLE(pnp, device_ids);
 
-static int ni_atmio_attach(struct comedi_device *dev,
-			   struct comedi_devconfig *it);
-static int ni_atmio_detach(struct comedi_device *dev);
-static struct comedi_driver driver_atmio = {
-	.driver_name = "ni_atmio",
-	.module = THIS_MODULE,
-	.attach = ni_atmio_attach,
-	.detach = ni_atmio_detach,
-};
-
-static int __init driver_atmio_init_module(void)
-{
-	return comedi_driver_register(&driver_atmio);
-}
-
-static void __exit driver_atmio_cleanup_module(void)
-{
-	comedi_driver_unregister(&driver_atmio);
-}
-
-module_init(driver_atmio_init_module);
-module_exit(driver_atmio_cleanup_module);
-
 #include "ni_mio_common.c"
 
-static int ni_getboardtype(struct comedi_device *dev);
-
-/* clean up allocated resources */
-static int ni_atmio_detach(struct comedi_device *dev)
-{
-	mio_common_detach(dev);
-
-	if (dev->iobase)
-		release_region(dev->iobase, NI_SIZE);
-	if (dev->irq)
-		free_irq(dev->irq, dev);
-
-	if (devpriv->isapnp_dev)
-		pnp_device_detach(devpriv->isapnp_dev);
-
-	return 0;
-}
-
 static int ni_isapnp_find_board(struct pnp_dev **dev)
 {
 	struct pnp_dev *isapnp_dev = NULL;
@@ -424,6 +383,26 @@ static int ni_isapnp_find_board(struct pnp_dev **dev)
 	return 0;
 }
 
+static int ni_getboardtype(struct comedi_device *dev)
+{
+	int device_id = ni_read_eeprom(dev, 511);
+	int i;
+
+	for (i = 0; i < n_ni_boards; i++) {
+		if (ni_boards[i].device_id == device_id)
+			return i;
+
+	}
+	if (device_id == 255)
+		printk(" can't find board\n");
+	 else if (device_id == 0)
+		printk(" EEPROM read error (?) or device not found\n");
+	 else
+		printk(" unknown device ID %d -- contact author\n", device_id);
+
+	return -1;
+}
+
 static int ni_atmio_attach(struct comedi_device *dev,
 			   struct comedi_devconfig *it)
 {
@@ -518,22 +497,21 @@ static int ni_atmio_attach(struct comedi_device *dev,
 	return 0;
 }
 
-static int ni_getboardtype(struct comedi_device *dev)
+static void ni_atmio_detach(struct comedi_device *dev)
 {
-	int device_id = ni_read_eeprom(dev, 511);
-	int i;
-
-	for (i = 0; i < n_ni_boards; i++) {
-		if (ni_boards[i].device_id == device_id)
-			return i;
-
-	}
-	if (device_id == 255)
-		printk(" can't find board\n");
-	 else if (device_id == 0)
-		printk(" EEPROM read error (?) or device not found\n");
-	 else
-		printk(" unknown device ID %d -- contact author\n", device_id);
-
-	return -1;
+	mio_common_detach(dev);
+	if (dev->iobase)
+		release_region(dev->iobase, NI_SIZE);
+	if (dev->irq)
+		free_irq(dev->irq, dev);
+	if (devpriv->isapnp_dev)
+		pnp_device_detach(devpriv->isapnp_dev);
 }
+
+static struct comedi_driver ni_atmio_driver = {
+	.driver_name	= "ni_atmio",
+	.module		= THIS_MODULE,
+	.attach		= ni_atmio_attach,
+	.detach		= ni_atmio_detach,
+};
+module_comedi_driver(ni_atmio_driver);
diff --git a/drivers/staging/comedi/drivers/ni_atmio16d.c b/drivers/staging/comedi/drivers/ni_atmio16d.c
index 285b933551a..4f614532674 100644
--- a/drivers/staging/comedi/drivers/ni_atmio16d.c
+++ b/drivers/staging/comedi/drivers/ni_atmio16d.c
@@ -110,60 +110,8 @@ struct atmio16_board_t {
 	int has_8255;
 };
 
-static const struct atmio16_board_t atmio16_boards[] = {
-	{
-	 .name = "atmio16",
-	 .has_8255 = 0,
-	 },
-	{
-	 .name = "atmio16d",
-	 .has_8255 = 1,
-	 },
-};
-
-#define n_atmio16_boards ARRAY_SIZE(atmio16_boards)
-
 #define boardtype ((const struct atmio16_board_t *)dev->board_ptr)
 
-/* function prototypes */
-static int atmio16d_attach(struct comedi_device *dev,
-			   struct comedi_devconfig *it);
-static int atmio16d_detach(struct comedi_device *dev);
-static irqreturn_t atmio16d_interrupt(int irq, void *d);
-static int atmio16d_ai_cmdtest(struct comedi_device *dev,
-			       struct comedi_subdevice *s,
-			       struct comedi_cmd *cmd);
-static int atmio16d_ai_cmd(struct comedi_device *dev,
-			   struct comedi_subdevice *s);
-static int atmio16d_ai_cancel(struct comedi_device *dev,
-			      struct comedi_subdevice *s);
-static void reset_counters(struct comedi_device *dev);
-static void reset_atmio16d(struct comedi_device *dev);
-
-/* main driver struct */
-static struct comedi_driver driver_atmio16d = {
-	.driver_name = "atmio16",
-	.module = THIS_MODULE,
-	.attach = atmio16d_attach,
-	.detach = atmio16d_detach,
-	.board_name = &atmio16_boards[0].name,
-	.num_names = n_atmio16_boards,
-	.offset = sizeof(struct atmio16_board_t),
-};
-
-static int __init driver_atmio16d_init_module(void)
-{
-	return comedi_driver_register(&driver_atmio16d);
-}
-
-static void __exit driver_atmio16d_cleanup_module(void)
-{
-	comedi_driver_unregister(&driver_atmio16d);
-}
-
-module_init(driver_atmio16d_init_module);
-module_exit(driver_atmio16d_cleanup_module);
-
 /* range structs */
 static const struct comedi_lrange range_atmio16d_ai_10_bipolar = { 4, {
 								       BIP_RANGE
@@ -881,24 +829,38 @@ static int atmio16d_attach(struct comedi_device *dev,
 	return 0;
 }
 
-static int atmio16d_detach(struct comedi_device *dev)
+static void atmio16d_detach(struct comedi_device *dev)
 {
-	printk(KERN_INFO "comedi%d: atmio16d: remove\n", dev->minor);
-
 	if (dev->subdevices && boardtype->has_8255)
 		subdev_8255_cleanup(dev, dev->subdevices + 3);
-
 	if (dev->irq)
 		free_irq(dev->irq, dev);
-
 	reset_atmio16d(dev);
-
 	if (dev->iobase)
 		release_region(dev->iobase, ATMIO16D_SIZE);
-
-	return 0;
 }
 
+static const struct atmio16_board_t atmio16_boards[] = {
+	{
+		.name		= "atmio16",
+		.has_8255	= 0,
+	}, {
+		.name		= "atmio16d",
+		.has_8255	= 1,
+	},
+};
+
+static struct comedi_driver atmio16d_driver = {
+	.driver_name	= "atmio16",
+	.module		= THIS_MODULE,
+	.attach		= atmio16d_attach,
+	.detach		= atmio16d_detach,
+	.board_name	= &atmio16_boards[0].name,
+	.num_names	= ARRAY_SIZE(atmio16_boards),
+	.offset		= sizeof(struct atmio16_board_t),
+};
+module_comedi_driver(atmio16d_driver);
+
 MODULE_AUTHOR("Comedi http://www.comedi.org");
 MODULE_DESCRIPTION("Comedi low-level driver");
 MODULE_LICENSE("GPL");
diff --git a/drivers/staging/comedi/drivers/ni_daq_700.c b/drivers/staging/comedi/drivers/ni_daq_700.c
index e2420123db8..75764e8d27e 100644
--- a/drivers/staging/comedi/drivers/ni_daq_700.c
+++ b/drivers/staging/comedi/drivers/ni_daq_700.c
@@ -57,7 +57,7 @@ static struct pcmcia_device *pcmcia_cur_dev;
 
 static int dio700_attach(struct comedi_device *dev,
 			 struct comedi_devconfig *it);
-static int dio700_detach(struct comedi_device *dev);
+static void dio700_detach(struct comedi_device *dev);
 
 enum dio700_bustype { pcmcia_bustype };
 
@@ -419,19 +419,14 @@ static int dio700_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 	return 0;
 };
 
-static int dio700_detach(struct comedi_device *dev)
+static void dio700_detach(struct comedi_device *dev)
 {
-	printk(KERN_ERR "comedi%d: ni_daq_700: cs-remove\n", dev->minor);
-
 	if (dev->subdevices)
 		subdev_700_cleanup(dev, dev->subdevices + 0);
-
 	if (thisboard->bustype != pcmcia_bustype && dev->iobase)
 		release_region(dev->iobase, DIO700_SIZE);
 	if (dev->irq)
 		free_irq(dev->irq, dev);
-
-	return 0;
 };
 
 static void dio700_config(struct pcmcia_device *link);
@@ -472,18 +467,12 @@ static int dio700_cs_attach(struct pcmcia_device *link)
 
 static void dio700_cs_detach(struct pcmcia_device *link)
 {
-
-	printk(KERN_INFO "ni_daq_700: cs-detach!\n");
-
-	dev_dbg(&link->dev, "dio700_cs_detach\n");
-
 	((struct local_info_t *)link->priv)->stop = 1;
 	dio700_release(link);
 
 	/* This points to the parent struct local_info_t struct */
 	kfree(link->priv);
-
-}				/* dio700_cs_detach */
+}
 
 static int dio700_pcmcia_config_loop(struct pcmcia_device *p_dev,
 				void *priv_data)
diff --git a/drivers/staging/comedi/drivers/ni_daq_dio24.c b/drivers/staging/comedi/drivers/ni_daq_dio24.c
index c0423a8c3e3..493a2278863 100644
--- a/drivers/staging/comedi/drivers/ni_daq_dio24.c
+++ b/drivers/staging/comedi/drivers/ni_daq_dio24.c
@@ -57,7 +57,7 @@ static struct pcmcia_device *pcmcia_cur_dev;
 #define DIO24_SIZE 4		/*  size of io region used by board */
 
 static int dio24_attach(struct comedi_device *dev, struct comedi_devconfig *it);
-static int dio24_detach(struct comedi_device *dev);
+static void dio24_detach(struct comedi_device *dev);
 
 enum dio24_bustype { pcmcia_bustype };
 
@@ -168,19 +168,14 @@ static int dio24_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 	return 0;
 };
 
-static int dio24_detach(struct comedi_device *dev)
+static void dio24_detach(struct comedi_device *dev)
 {
-	dev_info(dev->hw_dev, "comedi%d: ni_daq_dio24: remove\n", dev->minor);
-
 	if (dev->subdevices)
 		subdev_8255_cleanup(dev, dev->subdevices + 0);
-
 	if (thisboard->bustype != pcmcia_bustype && dev->iobase)
 		release_region(dev->iobase, DIO24_SIZE);
 	if (dev->irq)
 		free_irq(dev->irq, dev);
-
-	return 0;
 };
 
 static void dio24_config(struct pcmcia_device *link);
@@ -221,18 +216,12 @@ static int dio24_cs_attach(struct pcmcia_device *link)
 
 static void dio24_cs_detach(struct pcmcia_device *link)
 {
-
-	printk(KERN_INFO "ni_daq_dio24: HOLA SOY YO - cs-detach!\n");
-
-	dev_dbg(&link->dev, "dio24_cs_detach\n");
-
 	((struct local_info_t *)link->priv)->stop = 1;
 	dio24_release(link);
 
 	/* This points to the parent local_info_t struct */
 	kfree(link->priv);
-
-}				/* dio24_cs_detach */
+}
 
 static int dio24_pcmcia_config_loop(struct pcmcia_device *p_dev,
 				void *priv_data)
diff --git a/drivers/staging/comedi/drivers/ni_labpc.c b/drivers/staging/comedi/drivers/ni_labpc.c
index 721b2be2250..53349777246 100644
--- a/drivers/staging/comedi/drivers/ni_labpc.c
+++ b/drivers/staging/comedi/drivers/ni_labpc.c
@@ -805,13 +805,10 @@ static int labpc_find_device(struct comedi_device *dev, int bus, int slot)
 }
 #endif
 
-int labpc_common_detach(struct comedi_device *dev)
+void labpc_common_detach(struct comedi_device *dev)
 {
-	printk(KERN_ERR "comedi%d: ni_labpc: detach\n", dev->minor);
-
 	if (dev->subdevices)
 		subdev_8255_cleanup(dev, dev->subdevices + 2);
-
 #ifdef CONFIG_ISA_DMA_API
 	/* only free stuff if it has been allocated by _attach */
 	kfree(devpriv->dma_buffer);
@@ -826,8 +823,6 @@ int labpc_common_detach(struct comedi_device *dev)
 	if (devpriv->mite)
 		mite_unsetup(devpriv->mite);
 #endif
-
-	return 0;
 };
 EXPORT_SYMBOL_GPL(labpc_common_detach);
 
@@ -2141,7 +2136,7 @@ static void write_caldac(struct comedi_device *dev, unsigned int channel,
 static int __devinit driver_labpc_pci_probe(struct pci_dev *dev,
 					    const struct pci_device_id *ent)
 {
-	return comedi_pci_auto_config(dev, driver_labpc.driver_name);
+	return comedi_pci_auto_config(dev, &driver_labpc);
 }
 
 static void __devexit driver_labpc_pci_remove(struct pci_dev *dev)
diff --git a/drivers/staging/comedi/drivers/ni_labpc.h b/drivers/staging/comedi/drivers/ni_labpc.h
index 422cee58a96..e052ed3ba54 100644
--- a/drivers/staging/comedi/drivers/ni_labpc.h
+++ b/drivers/staging/comedi/drivers/ni_labpc.h
@@ -103,7 +103,7 @@ struct labpc_private {
 
 int labpc_common_attach(struct comedi_device *dev, unsigned long iobase,
 			unsigned int irq, unsigned int dma);
-int labpc_common_detach(struct comedi_device *dev);
+void labpc_common_detach(struct comedi_device *dev);
 
 extern const int labpc_1200_is_unipolar[];
 extern const int labpc_1200_ai_gain_bits[];
diff --git a/drivers/staging/comedi/drivers/ni_labpc_cs.c b/drivers/staging/comedi/drivers/ni_labpc_cs.c
index ff3840544dd..dbb61b6b3ed 100644
--- a/drivers/staging/comedi/drivers/ni_labpc_cs.c
+++ b/drivers/staging/comedi/drivers/ni_labpc_cs.c
@@ -188,21 +188,13 @@ static int labpc_cs_attach(struct pcmcia_device *link)
 
 static void labpc_cs_detach(struct pcmcia_device *link)
 {
-	dev_dbg(&link->dev, "labpc_cs_detach\n");
-
-	/*
-	   If the device is currently configured and active, we won't
-	   actually delete it yet.  Instead, it is marked so that when
-	   the release() function is called, that will trigger a proper
-	   detach().
-	 */
 	((struct local_info_t *)link->priv)->stop = 1;
 	labpc_release(link);
 
 	/* This points to the parent local_info_t struct (may be null) */
 	kfree(link->priv);
 
-}				/* labpc_cs_detach */
+}
 
 static int labpc_pcmcia_config_loop(struct pcmcia_device *p_dev,
 				void *priv_data)
diff --git a/drivers/staging/comedi/drivers/ni_mio_cs.c b/drivers/staging/comedi/drivers/ni_mio_cs.c
index 53ec24bb6dc..b85765d266c 100644
--- a/drivers/staging/comedi/drivers/ni_mio_cs.c
+++ b/drivers/staging/comedi/drivers/ni_mio_cs.c
@@ -227,7 +227,7 @@ static uint16_t mio_cs_win_in(struct comedi_device *dev, int addr)
 
 static int mio_cs_attach(struct comedi_device *dev,
 			 struct comedi_devconfig *it);
-static int mio_cs_detach(struct comedi_device *dev);
+static void mio_cs_detach(struct comedi_device *dev);
 static struct comedi_driver driver_ni_mio_cs = {
 	.driver_name = "ni_mio_cs",
 	.module = THIS_MODULE,
@@ -240,18 +240,11 @@ static struct comedi_driver driver_ni_mio_cs = {
 static int ni_getboardtype(struct comedi_device *dev,
 			   struct pcmcia_device *link);
 
-/* clean up allocated resources */
-/* called when driver is removed */
-static int mio_cs_detach(struct comedi_device *dev)
+static void mio_cs_detach(struct comedi_device *dev)
 {
 	mio_common_detach(dev);
-
-	/* PCMCIA layer frees the IO region */
-
 	if (dev->irq)
 		free_irq(dev->irq, dev);
-
-	return 0;
 }
 
 static void mio_cs_config(struct pcmcia_device *link);
@@ -276,8 +269,6 @@ static void cs_release(struct pcmcia_device *link)
 
 static void cs_detach(struct pcmcia_device *link)
 {
-	DPRINTK("cs_detach(link=%p)\n", link);
-
 	cs_release(link);
 }
 
diff --git a/drivers/staging/comedi/drivers/ni_pcidio.c b/drivers/staging/comedi/drivers/ni_pcidio.c
index 1df8fcbcd10..37b700830e2 100644
--- a/drivers/staging/comedi/drivers/ni_pcidio.c
+++ b/drivers/staging/comedi/drivers/ni_pcidio.c
@@ -293,18 +293,9 @@ enum FPGA_Control_Bits {
 #define IntEn (TransferReady|CountExpired|Waited|PrimaryTC|SecondaryTC)
 #endif
 
-static int nidio_attach(struct comedi_device *dev, struct comedi_devconfig *it);
-static int nidio_detach(struct comedi_device *dev);
 static int ni_pcidio_cancel(struct comedi_device *dev,
 			    struct comedi_subdevice *s);
 
-static struct comedi_driver driver_pcidio = {
-	.driver_name = "ni_pcidio",
-	.module = THIS_MODULE,
-	.attach = nidio_attach,
-	.detach = nidio_detach,
-};
-
 struct nidio_board {
 
 	int dev_id;
@@ -381,22 +372,6 @@ static const struct nidio_board nidio_boards[] = {
 #define n_nidio_boards ARRAY_SIZE(nidio_boards)
 #define this_board ((const struct nidio_board *)dev->board_ptr)
 
-static DEFINE_PCI_DEVICE_TABLE(ni_pcidio_pci_table) = {
-	{PCI_DEVICE(PCI_VENDOR_ID_NI, 0x1150)},
-	{PCI_DEVICE(PCI_VENDOR_ID_NI, 0x1320)},
-	{PCI_DEVICE(PCI_VENDOR_ID_NI, 0x12b0)},
-	{PCI_DEVICE(PCI_VENDOR_ID_NI, 0x0160)},
-	{PCI_DEVICE(PCI_VENDOR_ID_NI, 0x1630)},
-	{PCI_DEVICE(PCI_VENDOR_ID_NI, 0x13c0)},
-	{PCI_DEVICE(PCI_VENDOR_ID_NI, 0x0400)},
-	{PCI_DEVICE(PCI_VENDOR_ID_NI, 0x1250)},
-	{PCI_DEVICE(PCI_VENDOR_ID_NI, 0x17d0)},
-	{PCI_DEVICE(PCI_VENDOR_ID_NI, 0x1800)},
-	{0}
-};
-
-MODULE_DEVICE_TABLE(pci, ni_pcidio_pci_table);
-
 struct nidio96_private {
 	struct mite_struct *mite;
 	int boardtype;
@@ -414,7 +389,6 @@ static int ni_pcidio_cmdtest(struct comedi_device *dev,
 static int ni_pcidio_cmd(struct comedi_device *dev, struct comedi_subdevice *s);
 static int ni_pcidio_inttrig(struct comedi_device *dev,
 			     struct comedi_subdevice *s, unsigned int trignum);
-static int nidio_find_device(struct comedi_device *dev, int bus, int slot);
 static int ni_pcidio_ns_to_timer(int *nanosec, int round_mode);
 static int setup_mite_dma(struct comedi_device *dev,
 			  struct comedi_subdevice *s);
@@ -1205,6 +1179,33 @@ static int pci_6534_upload_firmware(struct comedi_device *dev, int options[])
 	return 0;
 }
 
+static int nidio_find_device(struct comedi_device *dev, int bus, int slot)
+{
+	struct mite_struct *mite;
+	int i;
+
+	for (mite = mite_devices; mite; mite = mite->next) {
+		if (mite->used)
+			continue;
+		if (bus || slot) {
+			if (bus != mite->pcidev->bus->number ||
+			    slot != PCI_SLOT(mite->pcidev->devfn))
+				continue;
+		}
+		for (i = 0; i < n_nidio_boards; i++) {
+			if (mite_device_id(mite) == nidio_boards[i].dev_id) {
+				dev->board_ptr = nidio_boards + i;
+				devpriv->mite = mite;
+
+				return 0;
+			}
+		}
+	}
+	printk(KERN_WARNING "no device found\n");
+	mite_list_devices();
+	return -EIO;
+}
+
 static int nidio_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 {
 	struct comedi_subdevice *s;
@@ -1306,7 +1307,7 @@ static int nidio_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 	return 0;
 }
 
-static int nidio_detach(struct comedi_device *dev)
+static void nidio_detach(struct comedi_device *dev)
 {
 	int i;
 
@@ -1314,10 +1315,8 @@ static int nidio_detach(struct comedi_device *dev)
 		for (i = 0; i < this_board->n_8255; i++)
 			subdev_8255_cleanup(dev, dev->subdevices + i);
 	}
-
 	if (dev->irq)
 		free_irq(dev->irq, dev);
-
 	if (devpriv) {
 		if (devpriv->di_mite_ring) {
 			mite_free_ring(devpriv->di_mite_ring);
@@ -1326,73 +1325,48 @@ static int nidio_detach(struct comedi_device *dev)
 		if (devpriv->mite)
 			mite_unsetup(devpriv->mite);
 	}
-	return 0;
 }
 
-static int nidio_find_device(struct comedi_device *dev, int bus, int slot)
-{
-	struct mite_struct *mite;
-	int i;
-
-	for (mite = mite_devices; mite; mite = mite->next) {
-		if (mite->used)
-			continue;
-		if (bus || slot) {
-			if (bus != mite->pcidev->bus->number ||
-			    slot != PCI_SLOT(mite->pcidev->devfn))
-				continue;
-		}
-		for (i = 0; i < n_nidio_boards; i++) {
-			if (mite_device_id(mite) == nidio_boards[i].dev_id) {
-				dev->board_ptr = nidio_boards + i;
-				devpriv->mite = mite;
-
-				return 0;
-			}
-		}
-	}
-	printk(KERN_WARNING "no device found\n");
-	mite_list_devices();
-	return -EIO;
-}
+static struct comedi_driver ni_pcidio_driver = {
+	.driver_name	= "ni_pcidio",
+	.module		= THIS_MODULE,
+	.attach		= nidio_attach,
+	.detach		= nidio_detach,
+};
 
-static int __devinit driver_pcidio_pci_probe(struct pci_dev *dev,
-					     const struct pci_device_id *ent)
+static int __devinit ni_pcidio_pci_probe(struct pci_dev *dev,
+					 const struct pci_device_id *ent)
 {
-	return comedi_pci_auto_config(dev, driver_pcidio.driver_name);
+	return comedi_pci_auto_config(dev, &ni_pcidio_driver);
 }
 
-static void __devexit driver_pcidio_pci_remove(struct pci_dev *dev)
+static void __devexit ni_pcidio_pci_remove(struct pci_dev *dev)
 {
 	comedi_pci_auto_unconfig(dev);
 }
 
-static struct pci_driver driver_pcidio_pci_driver = {
-	.id_table = ni_pcidio_pci_table,
-	.probe = &driver_pcidio_pci_probe,
-	.remove = __devexit_p(&driver_pcidio_pci_remove)
+static DEFINE_PCI_DEVICE_TABLE(ni_pcidio_pci_table) = {
+	{ PCI_DEVICE(PCI_VENDOR_ID_NI, 0x1150) },
+	{ PCI_DEVICE(PCI_VENDOR_ID_NI, 0x1320) },
+	{ PCI_DEVICE(PCI_VENDOR_ID_NI, 0x12b0) },
+	{ PCI_DEVICE(PCI_VENDOR_ID_NI, 0x0160) },
+	{ PCI_DEVICE(PCI_VENDOR_ID_NI, 0x1630) },
+	{ PCI_DEVICE(PCI_VENDOR_ID_NI, 0x13c0) },
+	{ PCI_DEVICE(PCI_VENDOR_ID_NI, 0x0400) },
+	{ PCI_DEVICE(PCI_VENDOR_ID_NI, 0x1250) },
+	{ PCI_DEVICE(PCI_VENDOR_ID_NI, 0x17d0) },
+	{ PCI_DEVICE(PCI_VENDOR_ID_NI, 0x1800) },
+	{ 0 }
 };
+MODULE_DEVICE_TABLE(pci, ni_pcidio_pci_table);
 
-static int __init driver_pcidio_init_module(void)
-{
-	int retval;
-
-	retval = comedi_driver_register(&driver_pcidio);
-	if (retval < 0)
-		return retval;
-
-	driver_pcidio_pci_driver.name = (char *)driver_pcidio.driver_name;
-	return pci_register_driver(&driver_pcidio_pci_driver);
-}
-
-static void __exit driver_pcidio_cleanup_module(void)
-{
-	pci_unregister_driver(&driver_pcidio_pci_driver);
-	comedi_driver_unregister(&driver_pcidio);
-}
-
-module_init(driver_pcidio_init_module);
-module_exit(driver_pcidio_cleanup_module);
+static struct pci_driver ni_pcidio_pci_driver = {
+	.name		= "ni_pcidio",
+	.id_table	= ni_pcidio_pci_table,
+	.probe		= ni_pcidio_pci_probe,
+	.remove		= __devexit_p(ni_pcidio_pci_remove),
+};
+module_comedi_pci_driver(ni_pcidio_driver, ni_pcidio_pci_driver);
 
 MODULE_AUTHOR("Comedi http://www.comedi.org");
 MODULE_DESCRIPTION("Comedi low-level driver");
diff --git a/drivers/staging/comedi/drivers/ni_pcimio.c b/drivers/staging/comedi/drivers/ni_pcimio.c
index 27baefa32b1..3974c0d98d2 100644
--- a/drivers/staging/comedi/drivers/ni_pcimio.c
+++ b/drivers/staging/comedi/drivers/ni_pcimio.c
@@ -129,66 +129,6 @@ Bugs:
 
 #define DRV_NAME "ni_pcimio"
 
-/* The following two tables must be in the same order */
-static DEFINE_PCI_DEVICE_TABLE(ni_pci_table) = {
-	{PCI_DEVICE(PCI_VENDOR_ID_NI, 0x0162)},
-	{PCI_DEVICE(PCI_VENDOR_ID_NI, 0x1170)},
-	{PCI_DEVICE(PCI_VENDOR_ID_NI, 0x1180)},
-	{PCI_DEVICE(PCI_VENDOR_ID_NI, 0x1190)},
-	{PCI_DEVICE(PCI_VENDOR_ID_NI, 0x11b0)},
-	{PCI_DEVICE(PCI_VENDOR_ID_NI, 0x11c0)},
-	{PCI_DEVICE(PCI_VENDOR_ID_NI, 0x11d0)},
-	{PCI_DEVICE(PCI_VENDOR_ID_NI, 0x1270)},
-	{PCI_DEVICE(PCI_VENDOR_ID_NI, 0x1330)},
-	{PCI_DEVICE(PCI_VENDOR_ID_NI, 0x1340)},
-	{PCI_DEVICE(PCI_VENDOR_ID_NI, 0x1350)},
-	{PCI_DEVICE(PCI_VENDOR_ID_NI, 0x14e0)},
-	{PCI_DEVICE(PCI_VENDOR_ID_NI, 0x14f0)},
-	{PCI_DEVICE(PCI_VENDOR_ID_NI, 0x1580)},
-	{PCI_DEVICE(PCI_VENDOR_ID_NI, 0x15b0)},
-	{PCI_DEVICE(PCI_VENDOR_ID_NI, 0x1880)},
-	{PCI_DEVICE(PCI_VENDOR_ID_NI, 0x1870)},
-	{PCI_DEVICE(PCI_VENDOR_ID_NI, 0x18b0)},
-	{PCI_DEVICE(PCI_VENDOR_ID_NI, 0x18c0)},
-	{PCI_DEVICE(PCI_VENDOR_ID_NI, 0x2410)},
-	{PCI_DEVICE(PCI_VENDOR_ID_NI, 0x2420)},
-	{PCI_DEVICE(PCI_VENDOR_ID_NI, 0x2430)},
-	{PCI_DEVICE(PCI_VENDOR_ID_NI, 0x2890)},
-	{PCI_DEVICE(PCI_VENDOR_ID_NI, 0x28c0)},
-	{PCI_DEVICE(PCI_VENDOR_ID_NI, 0x2a60)},
-	{PCI_DEVICE(PCI_VENDOR_ID_NI, 0x2a70)},
-	{PCI_DEVICE(PCI_VENDOR_ID_NI, 0x2a80)},
-	{PCI_DEVICE(PCI_VENDOR_ID_NI, 0x2ab0)},
-	{PCI_DEVICE(PCI_VENDOR_ID_NI, 0x2b80)},
-	{PCI_DEVICE(PCI_VENDOR_ID_NI, 0x2b90)},
-	{PCI_DEVICE(PCI_VENDOR_ID_NI, 0x2c80)},
-	{PCI_DEVICE(PCI_VENDOR_ID_NI, 0x2ca0)},
-	{PCI_DEVICE(PCI_VENDOR_ID_NI, 0x70aa)},
-	{PCI_DEVICE(PCI_VENDOR_ID_NI, 0x70ab)},
-	{PCI_DEVICE(PCI_VENDOR_ID_NI, 0x70ac)},
-	{PCI_DEVICE(PCI_VENDOR_ID_NI, 0x70af)},
-	{PCI_DEVICE(PCI_VENDOR_ID_NI, 0x70b0)},
-	{PCI_DEVICE(PCI_VENDOR_ID_NI, 0x70b4)},
-	{PCI_DEVICE(PCI_VENDOR_ID_NI, 0x70b6)},
-	{PCI_DEVICE(PCI_VENDOR_ID_NI, 0x70b7)},
-	{PCI_DEVICE(PCI_VENDOR_ID_NI, 0x70b8)},
-	{PCI_DEVICE(PCI_VENDOR_ID_NI, 0x70bc)},
-	{PCI_DEVICE(PCI_VENDOR_ID_NI, 0x70bd)},
-	{PCI_DEVICE(PCI_VENDOR_ID_NI, 0x70bf)},
-	{PCI_DEVICE(PCI_VENDOR_ID_NI, 0x70c0)},
-	{PCI_DEVICE(PCI_VENDOR_ID_NI, 0x70f2)},
-	{PCI_DEVICE(PCI_VENDOR_ID_NI, 0x710d)},
-	{PCI_DEVICE(PCI_VENDOR_ID_NI, 0x716c)},
-	{PCI_DEVICE(PCI_VENDOR_ID_NI, 0x716d)},
-	{PCI_DEVICE(PCI_VENDOR_ID_NI, 0x717f)},
-	{PCI_DEVICE(PCI_VENDOR_ID_NI, 0x71bc)},
-	{PCI_DEVICE(PCI_VENDOR_ID_NI, 0x717d)},
-	{PCI_DEVICE(PCI_VENDOR_ID_NI, 0x72e8)},
-	{0}
-};
-
-MODULE_DEVICE_TABLE(pci, ni_pci_table);
-
 /* These are not all the possible ao ranges for 628x boards.
  They can do OFFSET +- REFERENCE where OFFSET can be
  0V, 5V, APFI<0,1>, or AO<0...3> and RANGE can
@@ -1250,54 +1190,6 @@ static const struct ni_board_struct ni_boards[] = {
 
 #define n_pcimio_boards ARRAY_SIZE(ni_boards)
 
-static int pcimio_attach(struct comedi_device *dev,
-			 struct comedi_devconfig *it);
-static int pcimio_detach(struct comedi_device *dev);
-static struct comedi_driver driver_pcimio = {
-	.driver_name = DRV_NAME,
-	.module = THIS_MODULE,
-	.attach = pcimio_attach,
-	.detach = pcimio_detach,
-};
-
-static int __devinit driver_pcimio_pci_probe(struct pci_dev *dev,
-					     const struct pci_device_id *ent)
-{
-	return comedi_pci_auto_config(dev, driver_pcimio.driver_name);
-}
-
-static void __devexit driver_pcimio_pci_remove(struct pci_dev *dev)
-{
-	comedi_pci_auto_unconfig(dev);
-}
-
-static struct pci_driver driver_pcimio_pci_driver = {
-	.id_table = ni_pci_table,
-	.probe = &driver_pcimio_pci_probe,
-	.remove = __devexit_p(&driver_pcimio_pci_remove)
-};
-
-static int __init driver_pcimio_init_module(void)
-{
-	int retval;
-
-	retval = comedi_driver_register(&driver_pcimio);
-	if (retval < 0)
-		return retval;
-
-	driver_pcimio_pci_driver.name = (char *)driver_pcimio.driver_name;
-	return pci_register_driver(&driver_pcimio_pci_driver);
-}
-
-static void __exit driver_pcimio_cleanup_module(void)
-{
-	pci_unregister_driver(&driver_pcimio_pci_driver);
-	comedi_driver_unregister(&driver_pcimio);
-}
-
-module_init(driver_pcimio_init_module);
-module_exit(driver_pcimio_cleanup_module);
-
 struct ni_private {
 NI_PRIVATE_COMMON};
 #define devpriv ((struct ni_private *)dev->private)
@@ -1681,13 +1573,11 @@ static void init_6143(struct comedi_device *dev)
 	ni_writew(devpriv->ai_calib_source, Calibration_Channel_6143);
 }
 
-/* cleans up allocated resources */
-static int pcimio_detach(struct comedi_device *dev)
+static void pcimio_detach(struct comedi_device *dev)
 {
 	mio_common_detach(dev);
 	if (dev->irq)
 		free_irq(dev->irq, dev);
-
 	if (dev->private) {
 		mite_free_ring(devpriv->ai_mite_ring);
 		mite_free_ring(devpriv->ao_mite_ring);
@@ -1697,8 +1587,6 @@ static int pcimio_detach(struct comedi_device *dev)
 		if (devpriv->mite)
 			mite_unsetup(devpriv->mite);
 	}
-
-	return 0;
 }
 
 static int pcimio_attach(struct comedi_device *dev, struct comedi_devconfig *it)
@@ -1874,6 +1762,90 @@ static int pcimio_dio_change(struct comedi_device *dev,
 	return 0;
 }
 
+static struct comedi_driver ni_pcimio_driver = {
+	.driver_name	= "ni_pcimio",
+	.module		= THIS_MODULE,
+	.attach		= pcimio_attach,
+	.detach		= pcimio_detach,
+};
+
+static int __devinit ni_pcimio_pci_probe(struct pci_dev *dev,
+					 const struct pci_device_id *ent)
+{
+	return comedi_pci_auto_config(dev, &ni_pcimio_driver);
+}
+
+static void __devexit ni_pcimio_pci_remove(struct pci_dev *dev)
+{
+	comedi_pci_auto_unconfig(dev);
+}
+
+static DEFINE_PCI_DEVICE_TABLE(ni_pcimio_pci_table) = {
+	{ PCI_DEVICE(PCI_VENDOR_ID_NI, 0x0162) },
+	{ PCI_DEVICE(PCI_VENDOR_ID_NI, 0x1170) },
+	{ PCI_DEVICE(PCI_VENDOR_ID_NI, 0x1180) },
+	{ PCI_DEVICE(PCI_VENDOR_ID_NI, 0x1190) },
+	{ PCI_DEVICE(PCI_VENDOR_ID_NI, 0x11b0) },
+	{ PCI_DEVICE(PCI_VENDOR_ID_NI, 0x11c0) },
+	{ PCI_DEVICE(PCI_VENDOR_ID_NI, 0x11d0) },
+	{ PCI_DEVICE(PCI_VENDOR_ID_NI, 0x1270) },
+	{ PCI_DEVICE(PCI_VENDOR_ID_NI, 0x1330) },
+	{ PCI_DEVICE(PCI_VENDOR_ID_NI, 0x1340) },
+	{ PCI_DEVICE(PCI_VENDOR_ID_NI, 0x1350) },
+	{ PCI_DEVICE(PCI_VENDOR_ID_NI, 0x14e0) },
+	{ PCI_DEVICE(PCI_VENDOR_ID_NI, 0x14f0) },
+	{ PCI_DEVICE(PCI_VENDOR_ID_NI, 0x1580) },
+	{ PCI_DEVICE(PCI_VENDOR_ID_NI, 0x15b0) },
+	{ PCI_DEVICE(PCI_VENDOR_ID_NI, 0x1880) },
+	{ PCI_DEVICE(PCI_VENDOR_ID_NI, 0x1870) },
+	{ PCI_DEVICE(PCI_VENDOR_ID_NI, 0x18b0) },
+	{ PCI_DEVICE(PCI_VENDOR_ID_NI, 0x18c0) },
+	{ PCI_DEVICE(PCI_VENDOR_ID_NI, 0x2410) },
+	{ PCI_DEVICE(PCI_VENDOR_ID_NI, 0x2420) },
+	{ PCI_DEVICE(PCI_VENDOR_ID_NI, 0x2430) },
+	{ PCI_DEVICE(PCI_VENDOR_ID_NI, 0x2890) },
+	{ PCI_DEVICE(PCI_VENDOR_ID_NI, 0x28c0) },
+	{ PCI_DEVICE(PCI_VENDOR_ID_NI, 0x2a60) },
+	{ PCI_DEVICE(PCI_VENDOR_ID_NI, 0x2a70) },
+	{ PCI_DEVICE(PCI_VENDOR_ID_NI, 0x2a80) },
+	{ PCI_DEVICE(PCI_VENDOR_ID_NI, 0x2ab0) },
+	{ PCI_DEVICE(PCI_VENDOR_ID_NI, 0x2b80) },
+	{ PCI_DEVICE(PCI_VENDOR_ID_NI, 0x2b90) },
+	{ PCI_DEVICE(PCI_VENDOR_ID_NI, 0x2c80) },
+	{ PCI_DEVICE(PCI_VENDOR_ID_NI, 0x2ca0) },
+	{ PCI_DEVICE(PCI_VENDOR_ID_NI, 0x70aa) },
+	{ PCI_DEVICE(PCI_VENDOR_ID_NI, 0x70ab) },
+	{ PCI_DEVICE(PCI_VENDOR_ID_NI, 0x70ac) },
+	{ PCI_DEVICE(PCI_VENDOR_ID_NI, 0x70af) },
+	{ PCI_DEVICE(PCI_VENDOR_ID_NI, 0x70b0) },
+	{ PCI_DEVICE(PCI_VENDOR_ID_NI, 0x70b4) },
+	{ PCI_DEVICE(PCI_VENDOR_ID_NI, 0x70b6) },
+	{ PCI_DEVICE(PCI_VENDOR_ID_NI, 0x70b7) },
+	{ PCI_DEVICE(PCI_VENDOR_ID_NI, 0x70b8) },
+	{ PCI_DEVICE(PCI_VENDOR_ID_NI, 0x70bc) },
+	{ PCI_DEVICE(PCI_VENDOR_ID_NI, 0x70bd) },
+	{ PCI_DEVICE(PCI_VENDOR_ID_NI, 0x70bf) },
+	{ PCI_DEVICE(PCI_VENDOR_ID_NI, 0x70c0) },
+	{ PCI_DEVICE(PCI_VENDOR_ID_NI, 0x70f2) },
+	{ PCI_DEVICE(PCI_VENDOR_ID_NI, 0x710d) },
+	{ PCI_DEVICE(PCI_VENDOR_ID_NI, 0x716c) },
+	{ PCI_DEVICE(PCI_VENDOR_ID_NI, 0x716d) },
+	{ PCI_DEVICE(PCI_VENDOR_ID_NI, 0x717f) },
+	{ PCI_DEVICE(PCI_VENDOR_ID_NI, 0x71bc) },
+	{ PCI_DEVICE(PCI_VENDOR_ID_NI, 0x717d) },
+	{ PCI_DEVICE(PCI_VENDOR_ID_NI, 0x72e8) },
+	{ 0 }
+};
+MODULE_DEVICE_TABLE(pci, ni_pcimio_pci_table);
+
+static struct pci_driver ni_pcimio_pci_driver = {
+	.name		= "ni_pcimio",
+	.id_table	= ni_pcimio_pci_table,
+	.probe		= ni_pcimio_pci_probe,
+	.remove		= __devexit_p(ni_pcimio_pci_remove)
+};
+module_comedi_pci_driver(ni_pcimio_driver, ni_pcimio_pci_driver)
+
 MODULE_AUTHOR("Comedi http://www.comedi.org");
 MODULE_DESCRIPTION("Comedi low-level driver");
 MODULE_LICENSE("GPL");
diff --git a/drivers/staging/comedi/drivers/ni_tio_internal.h b/drivers/staging/comedi/drivers/ni_tio_internal.h
index c4ca5378583..5e00212aa02 100644
--- a/drivers/staging/comedi/drivers/ni_tio_internal.h
+++ b/drivers/staging/comedi/drivers/ni_tio_internal.h
@@ -362,8 +362,8 @@ static inline enum ni_gpct_register NITIO_Gi_ABZ_Reg(int counter_index)
 	return 0;
 }
 
-static inline enum ni_gpct_register NITIO_Gi_Interrupt_Acknowledge_Reg(int
-								       counter_index)
+static inline enum ni_gpct_register NITIO_Gi_Interrupt_Acknowledge_Reg(
+	int counter_index)
 {
 	switch (counter_index) {
 	case 0:
@@ -407,8 +407,8 @@ static inline enum ni_gpct_register NITIO_Gi_Status_Reg(int counter_index)
 	return 0;
 }
 
-static inline enum ni_gpct_register NITIO_Gi_Interrupt_Enable_Reg(int
-								  counter_index)
+static inline enum ni_gpct_register NITIO_Gi_Interrupt_Enable_Reg(
+	int counter_index)
 {
 	switch (counter_index) {
 	case 0:
@@ -472,15 +472,22 @@ enum Gi_Counting_Mode_Reg_Bits {
 	Gi_Index_Phase_LowA_HighB = 0x1 << Gi_Index_Phase_Bitshift,
 	Gi_Index_Phase_HighA_LowB = 0x2 << Gi_Index_Phase_Bitshift,
 	Gi_Index_Phase_HighA_HighB = 0x3 << Gi_Index_Phase_Bitshift,
-	Gi_HW_Arm_Enable_Bit = 0x80,	/* from m-series example code, not documented in 660x register level manual */
-	Gi_660x_HW_Arm_Select_Mask = 0x7 << Gi_HW_Arm_Select_Shift,	/* from m-series example code, not documented in 660x register level manual */
+	/* from m-series example code, not documented in 660x register level
+	 * manual */
+	Gi_HW_Arm_Enable_Bit = 0x80,
+	/* from m-series example code, not documented in 660x register level
+	 * manual */
+	Gi_660x_HW_Arm_Select_Mask = 0x7 << Gi_HW_Arm_Select_Shift,
 	Gi_660x_Prescale_X8_Bit = 0x1000,
 	Gi_M_Series_Prescale_X8_Bit = 0x2000,
 	Gi_M_Series_HW_Arm_Select_Mask = 0x1f << Gi_HW_Arm_Select_Shift,
-	/* must be set for clocks over 40MHz, which includes synchronous counting and quadrature modes */
+	/* must be set for clocks over 40MHz, which includes synchronous
+	 * counting and quadrature modes */
 	Gi_660x_Alternate_Sync_Bit = 0x2000,
 	Gi_M_Series_Alternate_Sync_Bit = 0x4000,
-	Gi_660x_Prescale_X2_Bit = 0x4000,	/* from m-series example code, not documented in 660x register level manual */
+	/* from m-series example code, not documented in 660x register level
+	 * manual */
+	Gi_660x_Prescale_X2_Bit = 0x4000,
 	Gi_M_Series_Prescale_X2_Bit = 0x8000,
 };
 
@@ -503,7 +510,8 @@ enum Gi_Mode_Bits {
 	Gi_Level_Gating_Bits = 0x1,
 	Gi_Rising_Edge_Gating_Bits = 0x2,
 	Gi_Falling_Edge_Gating_Bits = 0x3,
-	Gi_Gate_On_Both_Edges_Bit = 0x4,	/* used in conjunction with rising edge gating mode */
+	Gi_Gate_On_Both_Edges_Bit = 0x4,	/* used in conjunction with
+						 * rising edge gating mode */
 	Gi_Trigger_Mode_for_Edge_Gate_Mask = 0x18,
 	Gi_Edge_Gate_Starts_Stops_Bits = 0x0,
 	Gi_Edge_Gate_Stops_Starts_Bits = 0x8,
@@ -686,11 +694,10 @@ static inline unsigned Gi_Gate_Interrupt_Enable_Bit(unsigned counter_index)
 {
 	unsigned bit;
 
-	if (counter_index % 2) {
+	if (counter_index % 2)
 		bit = G1_Gate_Interrupt_Enable_Bit;
-	} else {
+	else
 		bit = G0_Gate_Interrupt_Enable_Bit;
-	}
 	return bit;
 }
 
@@ -748,8 +755,9 @@ static inline void ni_tio_set_bits_transient(struct ni_gpct *counter,
 }
 
 /* ni_tio_set_bits( ) is for safely writing to registers whose bits may be
-twiddled in interrupt context, or whose software copy may be read in interrupt context.
-*/
+ * twiddled in interrupt context, or whose software copy may be read in
+ * interrupt context.
+ */
 static inline void ni_tio_set_bits(struct ni_gpct *counter,
 				   enum ni_gpct_register register_index,
 				   unsigned bit_mask, unsigned bit_values)
diff --git a/drivers/staging/comedi/drivers/pcl711.c b/drivers/staging/comedi/drivers/pcl711.c
index b44386a6b63..2e7753f988a 100644
--- a/drivers/staging/comedi/drivers/pcl711.c
+++ b/drivers/staging/comedi/drivers/pcl711.c
@@ -148,42 +148,8 @@ struct pcl711_board {
 	const struct comedi_lrange *ai_range_type;
 };
 
-static const struct pcl711_board boardtypes[] = {
-	{"pcl711", 0, 0, 0, 5, 8, 1, 0, &range_bipolar5},
-	{"pcl711b", 1, 0, 0, 5, 8, 1, 7, &range_pcl711b_ai},
-	{"acl8112hg", 0, 1, 0, 12, 16, 2, 15, &range_acl8112hg_ai},
-	{"acl8112dg", 0, 1, 1, 9, 16, 2, 15, &range_acl8112dg_ai},
-};
-
-#define n_boardtypes (sizeof(boardtypes)/sizeof(struct pcl711_board))
 #define this_board ((const struct pcl711_board *)dev->board_ptr)
 
-static int pcl711_attach(struct comedi_device *dev,
-			 struct comedi_devconfig *it);
-static int pcl711_detach(struct comedi_device *dev);
-static struct comedi_driver driver_pcl711 = {
-	.driver_name = "pcl711",
-	.module = THIS_MODULE,
-	.attach = pcl711_attach,
-	.detach = pcl711_detach,
-	.board_name = &boardtypes[0].name,
-	.num_names = n_boardtypes,
-	.offset = sizeof(struct pcl711_board),
-};
-
-static int __init driver_pcl711_init_module(void)
-{
-	return comedi_driver_register(&driver_pcl711);
-}
-
-static void __exit driver_pcl711_cleanup_module(void)
-{
-	comedi_driver_unregister(&driver_pcl711);
-}
-
-module_init(driver_pcl711_init_module);
-module_exit(driver_pcl711_cleanup_module);
-
 struct pcl711_private {
 
 	int board;
@@ -513,21 +479,6 @@ static int pcl711_do_insn_bits(struct comedi_device *dev,
 	return 2;
 }
 
-/*  Free any resources that we have claimed  */
-static int pcl711_detach(struct comedi_device *dev)
-{
-	printk(KERN_INFO "comedi%d: pcl711: remove\n", dev->minor);
-
-	if (dev->irq)
-		free_irq(dev->irq, dev);
-
-	if (dev->iobase)
-		release_region(dev->iobase, PCL711_SIZE);
-
-	return 0;
-}
-
-/*  Initialization */
 static int pcl711_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 {
 	int ret;
@@ -640,6 +591,32 @@ static int pcl711_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 	return 0;
 }
 
+static void pcl711_detach(struct comedi_device *dev)
+{
+	if (dev->irq)
+		free_irq(dev->irq, dev);
+	if (dev->iobase)
+		release_region(dev->iobase, PCL711_SIZE);
+}
+
+static const struct pcl711_board boardtypes[] = {
+	{ "pcl711", 0, 0, 0, 5, 8, 1, 0, &range_bipolar5 },
+	{ "pcl711b", 1, 0, 0, 5, 8, 1, 7, &range_pcl711b_ai },
+	{ "acl8112hg", 0, 1, 0, 12, 16, 2, 15, &range_acl8112hg_ai },
+	{ "acl8112dg", 0, 1, 1, 9, 16, 2, 15, &range_acl8112dg_ai },
+};
+
+static struct comedi_driver pcl711_driver = {
+	.driver_name	= "pcl711",
+	.module		= THIS_MODULE,
+	.attach		= pcl711_attach,
+	.detach		= pcl711_detach,
+	.board_name	= &boardtypes[0].name,
+	.num_names	= ARRAY_SIZE(boardtypes),
+	.offset		= sizeof(struct pcl711_board),
+};
+module_comedi_driver(pcl711_driver);
+
 MODULE_AUTHOR("Comedi http://www.comedi.org");
 MODULE_DESCRIPTION("Comedi low-level driver");
 MODULE_LICENSE("GPL");
diff --git a/drivers/staging/comedi/drivers/pcl724.c b/drivers/staging/comedi/drivers/pcl724.c
index 61b075db66e..1f66fe1c7d5 100644
--- a/drivers/staging/comedi/drivers/pcl724.c
+++ b/drivers/staging/comedi/drivers/pcl724.c
@@ -56,10 +56,6 @@ See the source for configuration details.
 
 /* #define PCL724_IRQ   1  no IRQ support now */
 
-static int pcl724_attach(struct comedi_device *dev,
-			 struct comedi_devconfig *it);
-static int pcl724_detach(struct comedi_device *dev);
-
 struct pcl724_board {
 
 	const char *name;	/*  board name */
@@ -71,41 +67,8 @@ struct pcl724_board {
 	char is_pet48;
 };
 
-static const struct pcl724_board boardtypes[] = {
-	{"pcl724", 24, 1, 0x00fc, PCL724_SIZE, 0, 0,},
-	{"pcl722", 144, 6, 0x00fc, PCL722_SIZE, 1, 0,},
-	{"pcl731", 48, 2, 0x9cfc, PCL731_SIZE, 0, 0,},
-	{"acl7122", 144, 6, 0x9ee8, PCL722_SIZE, 1, 0,},
-	{"acl7124", 24, 1, 0x00fc, PCL724_SIZE, 0, 0,},
-	{"pet48dio", 48, 2, 0x9eb8, PET48_SIZE, 0, 1,},
-};
-
-#define n_boardtypes (sizeof(boardtypes)/sizeof(struct pcl724_board))
 #define this_board ((const struct pcl724_board *)dev->board_ptr)
 
-static struct comedi_driver driver_pcl724 = {
-	.driver_name = "pcl724",
-	.module = THIS_MODULE,
-	.attach = pcl724_attach,
-	.detach = pcl724_detach,
-	.board_name = &boardtypes[0].name,
-	.num_names = n_boardtypes,
-	.offset = sizeof(struct pcl724_board),
-};
-
-static int __init driver_pcl724_init_module(void)
-{
-	return comedi_driver_register(&driver_pcl724);
-}
-
-static void __exit driver_pcl724_cleanup_module(void)
-{
-	comedi_driver_unregister(&driver_pcl724);
-}
-
-module_init(driver_pcl724_init_module);
-module_exit(driver_pcl724_cleanup_module);
-
 static int subdev_8255_cb(int dir, int port, int data, unsigned long arg)
 {
 	unsigned long iobase = arg;
@@ -214,25 +177,39 @@ static int pcl724_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 	return 0;
 }
 
-static int pcl724_detach(struct comedi_device *dev)
+static void pcl724_detach(struct comedi_device *dev)
 {
 	int i;
 
-	/* printk("comedi%d: pcl724: remove\n",dev->minor); */
-
 	for (i = 0; i < dev->n_subdevices; i++)
 		subdev_8255_cleanup(dev, dev->subdevices + i);
-
 #ifdef PCL724_IRQ
 	if (dev->irq)
 		free_irq(dev->irq, dev);
 #endif
-
 	release_region(dev->iobase, this_board->io_range);
-
-	return 0;
 }
 
+static const struct pcl724_board boardtypes[] = {
+	{ "pcl724", 24, 1, 0x00fc, PCL724_SIZE, 0, 0, },
+	{ "pcl722", 144, 6, 0x00fc, PCL722_SIZE, 1, 0, },
+	{ "pcl731", 48, 2, 0x9cfc, PCL731_SIZE, 0, 0, },
+	{ "acl7122", 144, 6, 0x9ee8, PCL722_SIZE, 1, 0, },
+	{ "acl7124", 24, 1, 0x00fc, PCL724_SIZE, 0, 0, },
+	{ "pet48dio", 48, 2, 0x9eb8, PET48_SIZE, 0, 1, },
+};
+
+static struct comedi_driver pcl724_driver = {
+	.driver_name	= "pcl724",
+	.module		= THIS_MODULE,
+	.attach		= pcl724_attach,
+	.detach		= pcl724_detach,
+	.board_name	= &boardtypes[0].name,
+	.num_names	= ARRAY_SIZE(boardtypes),
+	.offset		= sizeof(struct pcl724_board),
+};
+module_comedi_driver(pcl724_driver);
+
 MODULE_AUTHOR("Comedi http://www.comedi.org");
 MODULE_DESCRIPTION("Comedi low-level driver");
 MODULE_LICENSE("GPL");
diff --git a/drivers/staging/comedi/drivers/pcl725.c b/drivers/staging/comedi/drivers/pcl725.c
index 24b223ca439..83a6fa53ddd 100644
--- a/drivers/staging/comedi/drivers/pcl725.c
+++ b/drivers/staging/comedi/drivers/pcl725.c
@@ -20,29 +20,6 @@ Devices: [Advantech] PCL-725 (pcl725)
 #define PCL725_DO 0
 #define PCL725_DI 1
 
-static int pcl725_attach(struct comedi_device *dev,
-			 struct comedi_devconfig *it);
-static int pcl725_detach(struct comedi_device *dev);
-static struct comedi_driver driver_pcl725 = {
-	.driver_name = "pcl725",
-	.module = THIS_MODULE,
-	.attach = pcl725_attach,
-	.detach = pcl725_detach,
-};
-
-static int __init driver_pcl725_init_module(void)
-{
-	return comedi_driver_register(&driver_pcl725);
-}
-
-static void __exit driver_pcl725_cleanup_module(void)
-{
-	comedi_driver_unregister(&driver_pcl725);
-}
-
-module_init(driver_pcl725_init_module);
-module_exit(driver_pcl725_cleanup_module);
-
 static int pcl725_do_insn(struct comedi_device *dev, struct comedi_subdevice *s,
 			  struct comedi_insn *insn, unsigned int *data)
 {
@@ -112,16 +89,20 @@ static int pcl725_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 	return 0;
 }
 
-static int pcl725_detach(struct comedi_device *dev)
+static void pcl725_detach(struct comedi_device *dev)
 {
-	printk(KERN_INFO "comedi%d: pcl725: remove\n", dev->minor);
-
 	if (dev->iobase)
 		release_region(dev->iobase, PCL725_SIZE);
-
-	return 0;
 }
 
+static struct comedi_driver pcl725_driver = {
+	.driver_name	= "pcl725",
+	.module		= THIS_MODULE,
+	.attach		= pcl725_attach,
+	.detach		= pcl725_detach,
+};
+module_comedi_driver(pcl725_driver);
+
 MODULE_AUTHOR("Comedi http://www.comedi.org");
 MODULE_DESCRIPTION("Comedi low-level driver");
 MODULE_LICENSE("GPL");
diff --git a/drivers/staging/comedi/drivers/pcl726.c b/drivers/staging/comedi/drivers/pcl726.c
index 897cd808eeb..d25c30c694e 100644
--- a/drivers/staging/comedi/drivers/pcl726.c
+++ b/drivers/staging/comedi/drivers/pcl726.c
@@ -111,10 +111,6 @@ static const struct comedi_lrange *const rangelist_728[] = {
 	&range_4_20mA, &range_0_20mA
 };
 
-static int pcl726_attach(struct comedi_device *dev,
-			 struct comedi_devconfig *it);
-static int pcl726_detach(struct comedi_device *dev);
-
 struct pcl726_board {
 
 	const char *name;	/*  driver name */
@@ -149,32 +145,8 @@ static const struct pcl726_board boardtypes[] = {
 	 &rangelist_728[0],},
 };
 
-#define n_boardtypes (sizeof(boardtypes)/sizeof(struct pcl726_board))
 #define this_board ((const struct pcl726_board *)dev->board_ptr)
 
-static struct comedi_driver driver_pcl726 = {
-	.driver_name = "pcl726",
-	.module = THIS_MODULE,
-	.attach = pcl726_attach,
-	.detach = pcl726_detach,
-	.board_name = &boardtypes[0].name,
-	.num_names = n_boardtypes,
-	.offset = sizeof(struct pcl726_board),
-};
-
-static int __init driver_pcl726_init_module(void)
-{
-	return comedi_driver_register(&driver_pcl726);
-}
-
-static void __exit driver_pcl726_cleanup_module(void)
-{
-	comedi_driver_unregister(&driver_pcl726);
-}
-
-module_init(driver_pcl726_init_module);
-module_exit(driver_pcl726_cleanup_module);
-
 struct pcl726_private {
 
 	int bipolar[12];
@@ -378,21 +350,27 @@ static int pcl726_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 	return 0;
 }
 
-static int pcl726_detach(struct comedi_device *dev)
+static void pcl726_detach(struct comedi_device *dev)
 {
-/* printk("comedi%d: pcl726: remove\n",dev->minor); */
-
 #ifdef ACL6126_IRQ
 	if (dev->irq)
 		free_irq(dev->irq, dev);
 #endif
-
 	if (dev->iobase)
 		release_region(dev->iobase, this_board->io_range);
-
-	return 0;
 }
 
+static struct comedi_driver pcl726_driver = {
+	.driver_name	= "pcl726",
+	.module		= THIS_MODULE,
+	.attach		= pcl726_attach,
+	.detach		= pcl726_detach,
+	.board_name	= &boardtypes[0].name,
+	.num_names	= ARRAY_SIZE(boardtypes),
+	.offset		= sizeof(struct pcl726_board),
+};
+module_comedi_driver(pcl726_driver);
+
 MODULE_AUTHOR("Comedi http://www.comedi.org");
 MODULE_DESCRIPTION("Comedi low-level driver");
 MODULE_LICENSE("GPL");
diff --git a/drivers/staging/comedi/drivers/pcl730.c b/drivers/staging/comedi/drivers/pcl730.c
index c9682d614e0..e11704added 100644
--- a/drivers/staging/comedi/drivers/pcl730.c
+++ b/drivers/staging/comedi/drivers/pcl730.c
@@ -26,48 +26,14 @@ The ACL-7130 card have an 8254 timer/counter not supported by this driver.
 #define PCL730_DIO_LO	2	/* TTL Digital I/O low byte (D0-D7) */
 #define PCL730_DIO_HI	3	/* TTL Digital I/O high byte (D8-D15) */
 
-static int pcl730_attach(struct comedi_device *dev,
-			 struct comedi_devconfig *it);
-static int pcl730_detach(struct comedi_device *dev);
-
 struct pcl730_board {
 
 	const char *name;	/*  board name */
 	unsigned int io_range;	/*  len of I/O space */
 };
 
-static const struct pcl730_board boardtypes[] = {
-	{"pcl730", PCL730_SIZE,},
-	{"iso730", PCL730_SIZE,},
-	{"acl7130", ACL7130_SIZE,},
-};
-
-#define n_boardtypes (sizeof(boardtypes)/sizeof(struct pcl730_board))
 #define this_board ((const struct pcl730_board *)dev->board_ptr)
 
-static struct comedi_driver driver_pcl730 = {
-	.driver_name = "pcl730",
-	.module = THIS_MODULE,
-	.attach = pcl730_attach,
-	.detach = pcl730_detach,
-	.board_name = &boardtypes[0].name,
-	.num_names = n_boardtypes,
-	.offset = sizeof(struct pcl730_board),
-};
-
-static int __init driver_pcl730_init_module(void)
-{
-	return comedi_driver_register(&driver_pcl730);
-}
-
-static void __exit driver_pcl730_cleanup_module(void)
-{
-	comedi_driver_unregister(&driver_pcl730);
-}
-
-module_init(driver_pcl730_init_module);
-module_exit(driver_pcl730_cleanup_module);
-
 static int pcl730_do_insn(struct comedi_device *dev, struct comedi_subdevice *s,
 			  struct comedi_insn *insn, unsigned int *data)
 {
@@ -168,16 +134,29 @@ static int pcl730_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 	return 0;
 }
 
-static int pcl730_detach(struct comedi_device *dev)
+static void pcl730_detach(struct comedi_device *dev)
 {
-	printk(KERN_INFO "comedi%d: pcl730: remove\n", dev->minor);
-
 	if (dev->iobase)
 		release_region(dev->iobase, this_board->io_range);
-
-	return 0;
 }
 
+static const struct pcl730_board boardtypes[] = {
+	{ "pcl730", PCL730_SIZE, },
+	{ "iso730", PCL730_SIZE, },
+	{ "acl7130", ACL7130_SIZE, },
+};
+
+static struct comedi_driver pcl730_driver = {
+	.driver_name	= "pcl730",
+	.module		= THIS_MODULE,
+	.attach		= pcl730_attach,
+	.detach		= pcl730_detach,
+	.board_name	= &boardtypes[0].name,
+	.num_names	= ARRAY_SIZE(boardtypes),
+	.offset		= sizeof(struct pcl730_board),
+};
+module_comedi_driver(pcl730_driver);
+
 MODULE_AUTHOR("Comedi http://www.comedi.org");
 MODULE_DESCRIPTION("Comedi low-level driver");
 MODULE_LICENSE("GPL");
diff --git a/drivers/staging/comedi/drivers/pcl812.c b/drivers/staging/comedi/drivers/pcl812.c
index 6fc74645af2..51f4ca9f792 100644
--- a/drivers/staging/comedi/drivers/pcl812.c
+++ b/drivers/staging/comedi/drivers/pcl812.c
@@ -316,10 +316,6 @@ static const struct comedi_lrange range_a821pgh_ai = { 4, {
 							   }
 };
 
-static int pcl812_attach(struct comedi_device *dev,
-			 struct comedi_devconfig *it);
-static int pcl812_detach(struct comedi_device *dev);
-
 struct pcl812_board {
 
 	const char *name;	/*  board name */
@@ -340,89 +336,8 @@ struct pcl812_board {
 	unsigned char haveMPC508;	/*  1=board use MPC508A multiplexor */
 };
 
-static const struct pcl812_board boardtypes[] = {
-	{"pcl812", boardPCL812, 16, 0, 2, 16, 16, 0x0fff,
-	 33000, 500, &range_bipolar10, &range_unipolar5,
-	 0xdcfc, 0x0a, PCLx1x_IORANGE, 0},
-	{"pcl812pg", boardPCL812PG, 16, 0, 2, 16, 16, 0x0fff,
-	 33000, 500, &range_pcl812pg_ai, &range_unipolar5,
-	 0xdcfc, 0x0a, PCLx1x_IORANGE, 0},
-	{"acl8112pg", boardPCL812PG, 16, 0, 2, 16, 16, 0x0fff,
-	 10000, 500, &range_pcl812pg_ai, &range_unipolar5,
-	 0xdcfc, 0x0a, PCLx1x_IORANGE, 0},
-	{"acl8112dg", boardACL8112, 16, 8, 2, 16, 16, 0x0fff,
-	 10000, 500, &range_acl8112dg_ai, &range_unipolar5,
-	 0xdcfc, 0x0a, PCLx1x_IORANGE, 1},
-	{"acl8112hg", boardACL8112, 16, 8, 2, 16, 16, 0x0fff,
-	 10000, 500, &range_acl8112hg_ai, &range_unipolar5,
-	 0xdcfc, 0x0a, PCLx1x_IORANGE, 1},
-	{"a821pgl", boardA821, 16, 8, 1, 16, 16, 0x0fff,
-	 10000, 500, &range_pcl813b_ai, &range_unipolar5,
-	 0x000c, 0x00, PCLx1x_IORANGE, 0},
-	{"a821pglnda", boardA821, 16, 8, 0, 0, 0, 0x0fff,
-	 10000, 500, &range_pcl813b_ai, NULL,
-	 0x000c, 0x00, PCLx1x_IORANGE, 0},
-	{"a821pgh", boardA821, 16, 8, 1, 16, 16, 0x0fff,
-	 10000, 500, &range_a821pgh_ai, &range_unipolar5,
-	 0x000c, 0x00, PCLx1x_IORANGE, 0},
-	{"a822pgl", boardACL8112, 16, 8, 2, 16, 16, 0x0fff,
-	 10000, 500, &range_acl8112dg_ai, &range_unipolar5,
-	 0xdcfc, 0x0a, PCLx1x_IORANGE, 0},
-	{"a822pgh", boardACL8112, 16, 8, 2, 16, 16, 0x0fff,
-	 10000, 500, &range_acl8112hg_ai, &range_unipolar5,
-	 0xdcfc, 0x0a, PCLx1x_IORANGE, 0},
-	{"a823pgl", boardACL8112, 16, 8, 2, 16, 16, 0x0fff,
-	 8000, 500, &range_acl8112dg_ai, &range_unipolar5,
-	 0xdcfc, 0x0a, PCLx1x_IORANGE, 0},
-	{"a823pgh", boardACL8112, 16, 8, 2, 16, 16, 0x0fff,
-	 8000, 500, &range_acl8112hg_ai, &range_unipolar5,
-	 0xdcfc, 0x0a, PCLx1x_IORANGE, 0},
-	{"pcl813", boardPCL813, 32, 0, 0, 0, 0, 0x0fff,
-	 0, 0, &range_pcl813b_ai, NULL,
-	 0x0000, 0x00, PCLx1x_IORANGE, 0},
-	{"pcl813b", boardPCL813B, 32, 0, 0, 0, 0, 0x0fff,
-	 0, 0, &range_pcl813b_ai, NULL,
-	 0x0000, 0x00, PCLx1x_IORANGE, 0},
-	{"acl8113", boardACL8113, 32, 0, 0, 0, 0, 0x0fff,
-	 0, 0, &range_acl8113_1_ai, NULL,
-	 0x0000, 0x00, PCLx1x_IORANGE, 0},
-	{"iso813", boardISO813, 32, 0, 0, 0, 0, 0x0fff,
-	 0, 0, &range_iso813_1_ai, NULL,
-	 0x0000, 0x00, PCLx1x_IORANGE, 0},
-	{"acl8216", boardACL8216, 16, 8, 2, 16, 16, 0xffff,
-	 10000, 500, &range_pcl813b2_ai, &range_unipolar5,
-	 0xdcfc, 0x0a, PCLx1x_IORANGE, 1},
-	{"a826pg", boardACL8216, 16, 8, 2, 16, 16, 0xffff,
-	 10000, 500, &range_pcl813b2_ai, &range_unipolar5,
-	 0xdcfc, 0x0a, PCLx1x_IORANGE, 0},
-};
-
-#define n_boardtypes (sizeof(boardtypes)/sizeof(struct pcl812_board))
 #define this_board ((const struct pcl812_board *)dev->board_ptr)
 
-static struct comedi_driver driver_pcl812 = {
-	.driver_name = "pcl812",
-	.module = THIS_MODULE,
-	.attach = pcl812_attach,
-	.detach = pcl812_detach,
-	.board_name = &boardtypes[0].name,
-	.num_names = n_boardtypes,
-	.offset = sizeof(struct pcl812_board),
-};
-
-static int __init driver_pcl812_init_module(void)
-{
-	return comedi_driver_register(&driver_pcl812);
-}
-
-static void __exit driver_pcl812_cleanup_module(void)
-{
-	comedi_driver_unregister(&driver_pcl812);
-}
-
-module_init(driver_pcl812_init_module);
-module_exit(driver_pcl812_cleanup_module);
-
 struct pcl812_private {
 
 	unsigned char valid;	/*  =1 device is OK */
@@ -1356,9 +1271,6 @@ static void pcl812_reset(struct comedi_device *dev)
 #endif
 }
 
-/*
-==============================================================================
-*/
 static int pcl812_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 {
 	int ret, subdev;
@@ -1702,19 +1614,79 @@ no_dma:
 	return 0;
 }
 
-/*
-==============================================================================
- */
-static int pcl812_detach(struct comedi_device *dev)
+static void pcl812_detach(struct comedi_device *dev)
 {
-
-#ifdef PCL812_EXTDEBUG
-	printk(KERN_DEBUG "comedi%d: pcl812: remove\n", dev->minor);
-#endif
 	free_resources(dev);
-	return 0;
 }
 
+static const struct pcl812_board boardtypes[] = {
+	{"pcl812", boardPCL812, 16, 0, 2, 16, 16, 0x0fff,
+	 33000, 500, &range_bipolar10, &range_unipolar5,
+	 0xdcfc, 0x0a, PCLx1x_IORANGE, 0},
+	{"pcl812pg", boardPCL812PG, 16, 0, 2, 16, 16, 0x0fff,
+	 33000, 500, &range_pcl812pg_ai, &range_unipolar5,
+	 0xdcfc, 0x0a, PCLx1x_IORANGE, 0},
+	{"acl8112pg", boardPCL812PG, 16, 0, 2, 16, 16, 0x0fff,
+	 10000, 500, &range_pcl812pg_ai, &range_unipolar5,
+	 0xdcfc, 0x0a, PCLx1x_IORANGE, 0},
+	{"acl8112dg", boardACL8112, 16, 8, 2, 16, 16, 0x0fff,
+	 10000, 500, &range_acl8112dg_ai, &range_unipolar5,
+	 0xdcfc, 0x0a, PCLx1x_IORANGE, 1},
+	{"acl8112hg", boardACL8112, 16, 8, 2, 16, 16, 0x0fff,
+	 10000, 500, &range_acl8112hg_ai, &range_unipolar5,
+	 0xdcfc, 0x0a, PCLx1x_IORANGE, 1},
+	{"a821pgl", boardA821, 16, 8, 1, 16, 16, 0x0fff,
+	 10000, 500, &range_pcl813b_ai, &range_unipolar5,
+	 0x000c, 0x00, PCLx1x_IORANGE, 0},
+	{"a821pglnda", boardA821, 16, 8, 0, 0, 0, 0x0fff,
+	 10000, 500, &range_pcl813b_ai, NULL,
+	 0x000c, 0x00, PCLx1x_IORANGE, 0},
+	{"a821pgh", boardA821, 16, 8, 1, 16, 16, 0x0fff,
+	 10000, 500, &range_a821pgh_ai, &range_unipolar5,
+	 0x000c, 0x00, PCLx1x_IORANGE, 0},
+	{"a822pgl", boardACL8112, 16, 8, 2, 16, 16, 0x0fff,
+	 10000, 500, &range_acl8112dg_ai, &range_unipolar5,
+	 0xdcfc, 0x0a, PCLx1x_IORANGE, 0},
+	{"a822pgh", boardACL8112, 16, 8, 2, 16, 16, 0x0fff,
+	 10000, 500, &range_acl8112hg_ai, &range_unipolar5,
+	 0xdcfc, 0x0a, PCLx1x_IORANGE, 0},
+	{"a823pgl", boardACL8112, 16, 8, 2, 16, 16, 0x0fff,
+	 8000, 500, &range_acl8112dg_ai, &range_unipolar5,
+	 0xdcfc, 0x0a, PCLx1x_IORANGE, 0},
+	{"a823pgh", boardACL8112, 16, 8, 2, 16, 16, 0x0fff,
+	 8000, 500, &range_acl8112hg_ai, &range_unipolar5,
+	 0xdcfc, 0x0a, PCLx1x_IORANGE, 0},
+	{"pcl813", boardPCL813, 32, 0, 0, 0, 0, 0x0fff,
+	 0, 0, &range_pcl813b_ai, NULL,
+	 0x0000, 0x00, PCLx1x_IORANGE, 0},
+	{"pcl813b", boardPCL813B, 32, 0, 0, 0, 0, 0x0fff,
+	 0, 0, &range_pcl813b_ai, NULL,
+	 0x0000, 0x00, PCLx1x_IORANGE, 0},
+	{"acl8113", boardACL8113, 32, 0, 0, 0, 0, 0x0fff,
+	 0, 0, &range_acl8113_1_ai, NULL,
+	 0x0000, 0x00, PCLx1x_IORANGE, 0},
+	{"iso813", boardISO813, 32, 0, 0, 0, 0, 0x0fff,
+	 0, 0, &range_iso813_1_ai, NULL,
+	 0x0000, 0x00, PCLx1x_IORANGE, 0},
+	{"acl8216", boardACL8216, 16, 8, 2, 16, 16, 0xffff,
+	 10000, 500, &range_pcl813b2_ai, &range_unipolar5,
+	 0xdcfc, 0x0a, PCLx1x_IORANGE, 1},
+	{"a826pg", boardACL8216, 16, 8, 2, 16, 16, 0xffff,
+	 10000, 500, &range_pcl813b2_ai, &range_unipolar5,
+	 0xdcfc, 0x0a, PCLx1x_IORANGE, 0},
+};
+
+static struct comedi_driver pcl812_driver = {
+	.driver_name	= "pcl812",
+	.module		= THIS_MODULE,
+	.attach		= pcl812_attach,
+	.detach		= pcl812_detach,
+	.board_name	= &boardtypes[0].name,
+	.num_names	= ARRAY_SIZE(boardtypes),
+	.offset		= sizeof(struct pcl812_board),
+};
+module_comedi_driver(pcl812_driver);
+
 MODULE_AUTHOR("Comedi http://www.comedi.org");
 MODULE_DESCRIPTION("Comedi low-level driver");
 MODULE_LICENSE("GPL");
diff --git a/drivers/staging/comedi/drivers/pcl816.c b/drivers/staging/comedi/drivers/pcl816.c
index 96cd7ec2ad5..cc67b6d4605 100644
--- a/drivers/staging/comedi/drivers/pcl816.c
+++ b/drivers/staging/comedi/drivers/pcl816.c
@@ -125,63 +125,14 @@ struct pcl816_board {
 	int i8254_osc_base;	/*  1/frequency of on board oscilator in ns */
 };
 
-static const struct pcl816_board boardtypes[] = {
-	{"pcl816", 8, 16, 10000, 1, 16, 16, &range_pcl816,
-	 &range_pcl816, PCLx1x_RANGE,
-	 0x00fc,		/*  IRQ mask */
-	 0x0a,			/*  DMA mask */
-	 0xffff,		/*  16-bit card */
-	 0xffff,		/*  D/A maxdata */
-	 1024,
-	 1,			/*  ao chan list */
-	 100},
-	{"pcl814b", 8, 16, 10000, 1, 16, 16, &range_pcl816,
-	 &range_pcl816, PCLx1x_RANGE,
-	 0x00fc,
-	 0x0a,
-	 0x3fff,		/* 14 bit card */
-	 0x3fff,
-	 1024,
-	 1,
-	 100},
-};
-
-#define n_boardtypes (sizeof(boardtypes)/sizeof(struct pcl816_board))
 #define devpriv ((struct pcl816_private *)dev->private)
 #define this_board ((const struct pcl816_board *)dev->board_ptr)
 
-static int pcl816_attach(struct comedi_device *dev,
-			 struct comedi_devconfig *it);
-static int pcl816_detach(struct comedi_device *dev);
-
 #ifdef unused
 static int RTC_lock;	/* RTC lock */
 static int RTC_timer_lock;	/* RTC int lock */
 #endif
 
-static struct comedi_driver driver_pcl816 = {
-	.driver_name = "pcl816",
-	.module = THIS_MODULE,
-	.attach = pcl816_attach,
-	.detach = pcl816_detach,
-	.board_name = &boardtypes[0].name,
-	.num_names = n_boardtypes,
-	.offset = sizeof(struct pcl816_board),
-};
-
-static int __init driver_pcl816_init_module(void)
-{
-	return comedi_driver_register(&driver_pcl816);
-}
-
-static void __exit driver_pcl816_cleanup_module(void)
-{
-	comedi_driver_unregister(&driver_pcl816);
-}
-
-module_init(driver_pcl816_init_module);
-module_exit(driver_pcl816_cleanup_module);
-
 struct pcl816_private {
 
 	unsigned int dma;	/*  used DMA, 0=don't use DMA */
@@ -1075,46 +1026,6 @@ static int set_rtc_irq_bit(unsigned char bit)
 }
 #endif
 
-/*
-==============================================================================
-  Free any resources that we have claimed
-*/
-static void free_resources(struct comedi_device *dev)
-{
-	/* printk("free_resource()\n"); */
-	if (dev->private) {
-		pcl816_ai_cancel(dev, devpriv->sub_ai);
-		pcl816_reset(dev);
-		if (devpriv->dma)
-			free_dma(devpriv->dma);
-		if (devpriv->dmabuf[0])
-			free_pages(devpriv->dmabuf[0], devpriv->dmapages[0]);
-		if (devpriv->dmabuf[1])
-			free_pages(devpriv->dmabuf[1], devpriv->dmapages[1]);
-#ifdef unused
-		if (devpriv->rtc_irq)
-			free_irq(devpriv->rtc_irq, dev);
-		if ((devpriv->dma_rtc) && (RTC_lock == 1)) {
-			if (devpriv->rtc_iobase)
-				release_region(devpriv->rtc_iobase,
-					       devpriv->rtc_iosize);
-		}
-#endif
-	}
-
-	if (dev->irq)
-		free_irq(dev->irq, dev);
-	if (dev->iobase)
-		release_region(dev->iobase, this_board->io_range);
-	/* printk("free_resource() end\n"); */
-}
-
-/*
-==============================================================================
-
-   Initialization
-
-*/
 static int pcl816_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 {
 	int ret;
@@ -1340,21 +1251,69 @@ case COMEDI_SUBD_DO:
 	return 0;
 }
 
-/*
-==============================================================================
-  Removes device
- */
-static int pcl816_detach(struct comedi_device *dev)
+static void pcl816_detach(struct comedi_device *dev)
 {
-	DEBUG(printk(KERN_INFO "comedi%d: pcl816: remove\n", dev->minor);)
-	    free_resources(dev);
+	if (dev->private) {
+		pcl816_ai_cancel(dev, devpriv->sub_ai);
+		pcl816_reset(dev);
+		if (devpriv->dma)
+			free_dma(devpriv->dma);
+		if (devpriv->dmabuf[0])
+			free_pages(devpriv->dmabuf[0], devpriv->dmapages[0]);
+		if (devpriv->dmabuf[1])
+			free_pages(devpriv->dmabuf[1], devpriv->dmapages[1]);
+#ifdef unused
+		if (devpriv->rtc_irq)
+			free_irq(devpriv->rtc_irq, dev);
+		if ((devpriv->dma_rtc) && (RTC_lock == 1)) {
+			if (devpriv->rtc_iobase)
+				release_region(devpriv->rtc_iobase,
+					       devpriv->rtc_iosize);
+		}
+#endif
+	}
+	if (dev->irq)
+		free_irq(dev->irq, dev);
+	if (dev->iobase)
+		release_region(dev->iobase, this_board->io_range);
 #ifdef unused
 	if (devpriv->dma_rtc)
 		RTC_lock--;
 #endif
-	return 0;
 }
 
+static const struct pcl816_board boardtypes[] = {
+	{"pcl816", 8, 16, 10000, 1, 16, 16, &range_pcl816,
+	 &range_pcl816, PCLx1x_RANGE,
+	 0x00fc,		/*  IRQ mask */
+	 0x0a,			/*  DMA mask */
+	 0xffff,		/*  16-bit card */
+	 0xffff,		/*  D/A maxdata */
+	 1024,
+	 1,			/*  ao chan list */
+	 100},
+	{"pcl814b", 8, 16, 10000, 1, 16, 16, &range_pcl816,
+	 &range_pcl816, PCLx1x_RANGE,
+	 0x00fc,
+	 0x0a,
+	 0x3fff,		/* 14 bit card */
+	 0x3fff,
+	 1024,
+	 1,
+	 100},
+};
+
+static struct comedi_driver pcl816_driver = {
+	.driver_name	= "pcl816",
+	.module		= THIS_MODULE,
+	.attach		= pcl816_attach,
+	.detach		= pcl816_detach,
+	.board_name	= &boardtypes[0].name,
+	.num_names	= ARRAY_SIZE(boardtypes),
+	.offset		= sizeof(struct pcl816_board),
+};
+module_comedi_driver(pcl816_driver);
+
 MODULE_AUTHOR("Comedi http://www.comedi.org");
 MODULE_DESCRIPTION("Comedi low-level driver");
 MODULE_LICENSE("GPL");
diff --git a/drivers/staging/comedi/drivers/pcl818.c b/drivers/staging/comedi/drivers/pcl818.c
index 7344a53a81c..1406c9720f5 100644
--- a/drivers/staging/comedi/drivers/pcl818.c
+++ b/drivers/staging/comedi/drivers/pcl818.c
@@ -247,10 +247,6 @@ static const struct comedi_lrange range718_bipolar0_5 =
 static const struct comedi_lrange range718_unipolar2 = { 1, {UNI_RANGE(2),} };
 static const struct comedi_lrange range718_unipolar1 = { 1, {BIP_RANGE(1),} };
 
-static int pcl818_attach(struct comedi_device *dev,
-			 struct comedi_devconfig *it);
-static int pcl818_detach(struct comedi_device *dev);
-
 #ifdef unused
 static int RTC_lock;	/* RTC lock */
 static int RTC_timer_lock;	/* RTC int lock */
@@ -277,56 +273,6 @@ struct pcl818_board {
 	int is_818;
 };
 
-static const struct pcl818_board boardtypes[] = {
-	{"pcl818l", 4, 16, 8, 25000, 1, 16, 16, &range_pcl818l_l_ai,
-	 &range_unipolar5, PCLx1x_RANGE, 0x00fc,
-	 0x0a, 0xfff, 0xfff, 0, 1},
-	{"pcl818h", 9, 16, 8, 10000, 1, 16, 16, &range_pcl818h_ai,
-	 &range_unipolar5, PCLx1x_RANGE, 0x00fc,
-	 0x0a, 0xfff, 0xfff, 0, 1},
-	{"pcl818hd", 9, 16, 8, 10000, 1, 16, 16, &range_pcl818h_ai,
-	 &range_unipolar5, PCLx1x_RANGE, 0x00fc,
-	 0x0a, 0xfff, 0xfff, 1, 1},
-	{"pcl818hg", 12, 16, 8, 10000, 1, 16, 16, &range_pcl818hg_ai,
-	 &range_unipolar5, PCLx1x_RANGE, 0x00fc,
-	 0x0a, 0xfff, 0xfff, 1, 1},
-	{"pcl818", 9, 16, 8, 10000, 2, 16, 16, &range_pcl818h_ai,
-	 &range_unipolar5, PCLx1x_RANGE, 0x00fc,
-	 0x0a, 0xfff, 0xfff, 0, 1},
-	{"pcl718", 1, 16, 8, 16000, 2, 16, 16, &range_unipolar5,
-	 &range_unipolar5, PCLx1x_RANGE, 0x00fc,
-	 0x0a, 0xfff, 0xfff, 0, 0},
-	/* pcm3718 */
-	{"pcm3718", 9, 16, 8, 10000, 0, 16, 16, &range_pcl818h_ai,
-	 &range_unipolar5, PCLx1x_RANGE, 0x00fc,
-	 0x0a, 0xfff, 0xfff, 0, 1 /* XXX ? */ },
-};
-
-#define n_boardtypes (sizeof(boardtypes)/sizeof(struct pcl818_board))
-
-static struct comedi_driver driver_pcl818 = {
-	.driver_name = "pcl818",
-	.module = THIS_MODULE,
-	.attach = pcl818_attach,
-	.detach = pcl818_detach,
-	.board_name = &boardtypes[0].name,
-	.num_names = n_boardtypes,
-	.offset = sizeof(struct pcl818_board),
-};
-
-static int __init driver_pcl818_init_module(void)
-{
-	return comedi_driver_register(&driver_pcl818);
-}
-
-static void __exit driver_pcl818_cleanup_module(void)
-{
-	comedi_driver_unregister(&driver_pcl818);
-}
-
-module_init(driver_pcl818_init_module);
-module_exit(driver_pcl818_cleanup_module);
-
 struct pcl818_private {
 
 	unsigned int dma;	/*  used DMA, 0=don't use DMA */
@@ -1688,48 +1634,6 @@ static int rtc_setfreq_irq(int freq)
 }
 #endif
 
-/*
-==============================================================================
-  Free any resources that we have claimed
-*/
-static void free_resources(struct comedi_device *dev)
-{
-	/* printk("free_resource()\n"); */
-	if (dev->private) {
-		pcl818_ai_cancel(dev, devpriv->sub_ai);
-		pcl818_reset(dev);
-		if (devpriv->dma)
-			free_dma(devpriv->dma);
-		if (devpriv->dmabuf[0])
-			free_pages(devpriv->dmabuf[0], devpriv->dmapages[0]);
-		if (devpriv->dmabuf[1])
-			free_pages(devpriv->dmabuf[1], devpriv->dmapages[1]);
-#ifdef unused
-		if (devpriv->rtc_irq)
-			free_irq(devpriv->rtc_irq, dev);
-		if ((devpriv->dma_rtc) && (RTC_lock == 1)) {
-			if (devpriv->rtc_iobase)
-				release_region(devpriv->rtc_iobase,
-					       devpriv->rtc_iosize);
-		}
-		if (devpriv->dma_rtc)
-			RTC_lock--;
-#endif
-	}
-
-	if (dev->irq)
-		free_irq(dev->irq, dev);
-	if (dev->iobase)
-		release_region(dev->iobase, devpriv->io_range);
-	/* printk("free_resource() end\n"); */
-}
-
-/*
-==============================================================================
-
-   Initialization
-
-*/
 static int pcl818_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 {
 	int ret;
@@ -2020,17 +1924,71 @@ no_dma:
 	return 0;
 }
 
-/*
-==============================================================================
-  Removes device
- */
-static int pcl818_detach(struct comedi_device *dev)
+static void pcl818_detach(struct comedi_device *dev)
 {
-	/*   printk("comedi%d: pcl818: remove\n", dev->minor); */
-	free_resources(dev);
-	return 0;
+	if (dev->private) {
+		pcl818_ai_cancel(dev, devpriv->sub_ai);
+		pcl818_reset(dev);
+		if (devpriv->dma)
+			free_dma(devpriv->dma);
+		if (devpriv->dmabuf[0])
+			free_pages(devpriv->dmabuf[0], devpriv->dmapages[0]);
+		if (devpriv->dmabuf[1])
+			free_pages(devpriv->dmabuf[1], devpriv->dmapages[1]);
+#ifdef unused
+		if (devpriv->rtc_irq)
+			free_irq(devpriv->rtc_irq, dev);
+		if ((devpriv->dma_rtc) && (RTC_lock == 1)) {
+			if (devpriv->rtc_iobase)
+				release_region(devpriv->rtc_iobase,
+					       devpriv->rtc_iosize);
+		}
+		if (devpriv->dma_rtc)
+			RTC_lock--;
+#endif
+	}
+	if (dev->irq)
+		free_irq(dev->irq, dev);
+	if (dev->iobase)
+		release_region(dev->iobase, devpriv->io_range);
 }
 
+static const struct pcl818_board boardtypes[] = {
+	{"pcl818l", 4, 16, 8, 25000, 1, 16, 16, &range_pcl818l_l_ai,
+	 &range_unipolar5, PCLx1x_RANGE, 0x00fc,
+	 0x0a, 0xfff, 0xfff, 0, 1},
+	{"pcl818h", 9, 16, 8, 10000, 1, 16, 16, &range_pcl818h_ai,
+	 &range_unipolar5, PCLx1x_RANGE, 0x00fc,
+	 0x0a, 0xfff, 0xfff, 0, 1},
+	{"pcl818hd", 9, 16, 8, 10000, 1, 16, 16, &range_pcl818h_ai,
+	 &range_unipolar5, PCLx1x_RANGE, 0x00fc,
+	 0x0a, 0xfff, 0xfff, 1, 1},
+	{"pcl818hg", 12, 16, 8, 10000, 1, 16, 16, &range_pcl818hg_ai,
+	 &range_unipolar5, PCLx1x_RANGE, 0x00fc,
+	 0x0a, 0xfff, 0xfff, 1, 1},
+	{"pcl818", 9, 16, 8, 10000, 2, 16, 16, &range_pcl818h_ai,
+	 &range_unipolar5, PCLx1x_RANGE, 0x00fc,
+	 0x0a, 0xfff, 0xfff, 0, 1},
+	{"pcl718", 1, 16, 8, 16000, 2, 16, 16, &range_unipolar5,
+	 &range_unipolar5, PCLx1x_RANGE, 0x00fc,
+	 0x0a, 0xfff, 0xfff, 0, 0},
+	/* pcm3718 */
+	{"pcm3718", 9, 16, 8, 10000, 0, 16, 16, &range_pcl818h_ai,
+	 &range_unipolar5, PCLx1x_RANGE, 0x00fc,
+	 0x0a, 0xfff, 0xfff, 0, 1 /* XXX ? */ },
+};
+
+static struct comedi_driver pcl818_driver = {
+	.driver_name	= "pcl818",
+	.module		= THIS_MODULE,
+	.attach		= pcl818_attach,
+	.detach		= pcl818_detach,
+	.board_name	= &boardtypes[0].name,
+	.num_names	= ARRAY_SIZE(boardtypes),
+	.offset		= sizeof(struct pcl818_board),
+};
+module_comedi_driver(pcl818_driver);
+
 MODULE_AUTHOR("Comedi http://www.comedi.org");
 MODULE_DESCRIPTION("Comedi low-level driver");
 MODULE_LICENSE("GPL");
diff --git a/drivers/staging/comedi/drivers/pcm3724.c b/drivers/staging/comedi/drivers/pcm3724.c
index f5c0bd17684..7492b8f1d49 100644
--- a/drivers/staging/comedi/drivers/pcm3724.c
+++ b/drivers/staging/comedi/drivers/pcm3724.c
@@ -62,10 +62,6 @@ Copy/pasted/hacked from pcm724.c
 #define CR_A_MODE(a)	((a)<<5)
 #define CR_CW		0x80
 
-static int pcm3724_attach(struct comedi_device *dev,
-			  struct comedi_devconfig *it);
-static int pcm3724_detach(struct comedi_device *dev);
-
 struct pcm3724_board {
 	const char *name;	/*  driver name */
 	int dio;		/*  num of DIO */
@@ -80,36 +76,8 @@ struct priv_pcm3724 {
 	int dio_2;
 };
 
-static const struct pcm3724_board boardtypes[] = {
-	{"pcm3724", 48, 2, 0x00fc, PCM3724_SIZE,},
-};
-
-#define n_boardtypes (sizeof(boardtypes)/sizeof(struct pcm3724_board))
 #define this_board ((const struct pcm3724_board *)dev->board_ptr)
 
-static struct comedi_driver driver_pcm3724 = {
-	.driver_name = "pcm3724",
-	.module = THIS_MODULE,
-	.attach = pcm3724_attach,
-	.detach = pcm3724_detach,
-	.board_name = &boardtypes[0].name,
-	.num_names = n_boardtypes,
-	.offset = sizeof(struct pcm3724_board),
-};
-
-static int __init driver_pcm3724_init_module(void)
-{
-	return comedi_driver_register(&driver_pcm3724);
-}
-
-static void __exit driver_pcm3724_cleanup_module(void)
-{
-	comedi_driver_unregister(&driver_pcm3724);
-}
-
-module_init(driver_pcm3724_init_module);
-module_exit(driver_pcm3724_cleanup_module);
-
 /* (setq c-basic-offset 8) */
 
 static int subdev_8255_cb(int dir, int port, int data, unsigned long arg)
@@ -305,7 +273,7 @@ static int pcm3724_attach(struct comedi_device *dev,
 	return 0;
 }
 
-static int pcm3724_detach(struct comedi_device *dev)
+static void pcm3724_detach(struct comedi_device *dev)
 {
 	int i;
 
@@ -315,10 +283,23 @@ static int pcm3724_detach(struct comedi_device *dev)
 	}
 	if (dev->iobase)
 		release_region(dev->iobase, this_board->io_range);
-
-	return 0;
 }
 
+static const struct pcm3724_board boardtypes[] = {
+	{ "pcm3724", 48, 2, 0x00fc, PCM3724_SIZE, },
+};
+
+static struct comedi_driver pcm3724_driver = {
+	.driver_name	= "pcm3724",
+	.module		= THIS_MODULE,
+	.attach		= pcm3724_attach,
+	.detach		= pcm3724_detach,
+	.board_name	= &boardtypes[0].name,
+	.num_names	= ARRAY_SIZE(boardtypes),
+	.offset		= sizeof(struct pcm3724_board),
+};
+module_comedi_driver(pcm3724_driver);
+
 MODULE_AUTHOR("Comedi http://www.comedi.org");
 MODULE_DESCRIPTION("Comedi low-level driver");
 MODULE_LICENSE("GPL");
diff --git a/drivers/staging/comedi/drivers/pcm3730.c b/drivers/staging/comedi/drivers/pcm3730.c
index bada6b236ff..f8d1c644daf 100644
--- a/drivers/staging/comedi/drivers/pcm3730.c
+++ b/drivers/staging/comedi/drivers/pcm3730.c
@@ -28,29 +28,6 @@ Configuration options:
 #define PCM3730_DIB 2
 #define PCM3730_DIC 3
 
-static int pcm3730_attach(struct comedi_device *dev,
-			  struct comedi_devconfig *it);
-static int pcm3730_detach(struct comedi_device *dev);
-static struct comedi_driver driver_pcm3730 = {
-	.driver_name = "pcm3730",
-	.module = THIS_MODULE,
-	.attach = pcm3730_attach,
-	.detach = pcm3730_detach,
-};
-
-static int __init driver_pcm3730_init_module(void)
-{
-	return comedi_driver_register(&driver_pcm3730);
-}
-
-static void __exit driver_pcm3730_cleanup_module(void)
-{
-	comedi_driver_unregister(&driver_pcm3730);
-}
-
-module_init(driver_pcm3730_init_module);
-module_exit(driver_pcm3730_cleanup_module);
-
 static int pcm3730_do_insn_bits(struct comedi_device *dev,
 				struct comedi_subdevice *s,
 				struct comedi_insn *insn, unsigned int *data)
@@ -156,16 +133,20 @@ static int pcm3730_attach(struct comedi_device *dev,
 	return 0;
 }
 
-static int pcm3730_detach(struct comedi_device *dev)
+static void pcm3730_detach(struct comedi_device *dev)
 {
-	printk(KERN_INFO "comedi%d: pcm3730: remove\n", dev->minor);
-
 	if (dev->iobase)
 		release_region(dev->iobase, PCM3730_SIZE);
-
-	return 0;
 }
 
+static struct comedi_driver pcm3730_driver = {
+	.driver_name	= "pcm3730",
+	.module		= THIS_MODULE,
+	.attach		= pcm3730_attach,
+	.detach		= pcm3730_detach,
+};
+module_comedi_driver(pcm3730_driver);
+
 MODULE_AUTHOR("Comedi http://www.comedi.org");
 MODULE_DESCRIPTION("Comedi low-level driver");
 MODULE_LICENSE("GPL");
diff --git a/drivers/staging/comedi/drivers/pcmad.c b/drivers/staging/comedi/drivers/pcmad.c
index 23b3d777340..1ec7d5cb346 100644
--- a/drivers/staging/comedi/drivers/pcmad.c
+++ b/drivers/staging/comedi/drivers/pcmad.c
@@ -57,19 +57,8 @@ struct pcmad_board_struct {
 	const char *name;
 	int n_ai_bits;
 };
-static const struct pcmad_board_struct pcmad_boards[] = {
-	{
-	 .name = "pcmad12",
-	 .n_ai_bits = 12,
-	 },
-	{
-	 .name = "pcmad16",
-	 .n_ai_bits = 16,
-	 },
-};
 
 #define this_board ((const struct pcmad_board_struct *)(dev->board_ptr))
-#define n_pcmad_boards ARRAY_SIZE(pcmad_boards)
 
 struct pcmad_priv_struct {
 	int differential;
@@ -77,31 +66,6 @@ struct pcmad_priv_struct {
 };
 #define devpriv ((struct pcmad_priv_struct *)dev->private)
 
-static int pcmad_attach(struct comedi_device *dev, struct comedi_devconfig *it);
-static int pcmad_detach(struct comedi_device *dev);
-static struct comedi_driver driver_pcmad = {
-	.driver_name = "pcmad",
-	.module = THIS_MODULE,
-	.attach = pcmad_attach,
-	.detach = pcmad_detach,
-	.board_name = &pcmad_boards[0].name,
-	.num_names = n_pcmad_boards,
-	.offset = sizeof(pcmad_boards[0]),
-};
-
-static int __init driver_pcmad_init_module(void)
-{
-	return comedi_driver_register(&driver_pcmad);
-}
-
-static void __exit driver_pcmad_cleanup_module(void)
-{
-	comedi_driver_unregister(&driver_pcmad);
-}
-
-module_init(driver_pcmad_init_module);
-module_exit(driver_pcmad_cleanup_module);
-
 #define TIMEOUT	100
 
 static int pcmad_ai_insn_read(struct comedi_device *dev,
@@ -175,19 +139,34 @@ static int pcmad_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 	return 0;
 }
 
-static int pcmad_detach(struct comedi_device *dev)
+static void pcmad_detach(struct comedi_device *dev)
 {
-	printk(KERN_INFO "comedi%d: pcmad: remove\n", dev->minor);
-
 	if (dev->irq)
 		free_irq(dev->irq, dev);
-
 	if (dev->iobase)
 		release_region(dev->iobase, PCMAD_SIZE);
-
-	return 0;
 }
 
+static const struct pcmad_board_struct pcmad_boards[] = {
+	{
+		.name		= "pcmad12",
+		.n_ai_bits	= 12,
+	}, {
+		.name		= "pcmad16",
+		.n_ai_bits	= 16,
+	},
+};
+static struct comedi_driver pcmad_driver = {
+	.driver_name	= "pcmad",
+	.module		= THIS_MODULE,
+	.attach		= pcmad_attach,
+	.detach		= pcmad_detach,
+	.board_name	= &pcmad_boards[0].name,
+	.num_names	= ARRAY_SIZE(pcmad_boards),
+	.offset		= sizeof(pcmad_boards[0]),
+};
+module_comedi_driver(pcmad_driver);
+
 MODULE_AUTHOR("Comedi http://www.comedi.org");
 MODULE_DESCRIPTION("Comedi low-level driver");
 MODULE_LICENSE("GPL");
diff --git a/drivers/staging/comedi/drivers/pcmda12.c b/drivers/staging/comedi/drivers/pcmda12.c
index 0e9ffa28d74..4786148b4fd 100644
--- a/drivers/staging/comedi/drivers/pcmda12.c
+++ b/drivers/staging/comedi/drivers/pcmda12.c
@@ -80,12 +80,6 @@ static const struct comedi_lrange pcmda12_ranges = {
 	 }
 };
 
-static const struct pcmda12_board pcmda12_boards[] = {
-	{
-	 .name = "pcmda12",
-	 },
-};
-
 /*
  * Useful for shorthand access to the particular board structure
  */
@@ -99,137 +93,6 @@ struct pcmda12_private {
 
 #define devpriv ((struct pcmda12_private *)(dev->private))
 
-/*
- * The struct comedi_driver structure tells the Comedi core module
- * which functions to call to configure/deconfigure (attach/detach)
- * the board, and also about the kernel module that contains
- * the device code.
- */
-static int pcmda12_attach(struct comedi_device *dev,
-			  struct comedi_devconfig *it);
-static int pcmda12_detach(struct comedi_device *dev);
-
-static void zero_chans(struct comedi_device *dev);
-
-static struct comedi_driver driver = {
-	.driver_name = "pcmda12",
-	.module = THIS_MODULE,
-	.attach = pcmda12_attach,
-	.detach = pcmda12_detach,
-/* It is not necessary to implement the following members if you are
- * writing a driver for a ISA PnP or PCI card */
-	/* Most drivers will support multiple types of boards by
-	 * having an array of board structures.  These were defined
-	 * in pcmda12_boards[] above.  Note that the element 'name'
-	 * was first in the structure -- Comedi uses this fact to
-	 * extract the name of the board without knowing any details
-	 * about the structure except for its length.
-	 * When a device is attached (by comedi_config), the name
-	 * of the device is given to Comedi, and Comedi tries to
-	 * match it by going through the list of board names.  If
-	 * there is a match, the address of the pointer is put
-	 * into dev->board_ptr and driver->attach() is called.
-	 *
-	 * Note that these are not necessary if you can determine
-	 * the type of board in software.  ISA PnP, PCI, and PCMCIA
-	 * devices are such boards.
-	 */
-	.board_name = &pcmda12_boards[0].name,
-	.offset = sizeof(struct pcmda12_board),
-	.num_names = ARRAY_SIZE(pcmda12_boards),
-};
-
-static int ao_winsn(struct comedi_device *dev, struct comedi_subdevice *s,
-		    struct comedi_insn *insn, unsigned int *data);
-static int ao_rinsn(struct comedi_device *dev, struct comedi_subdevice *s,
-		    struct comedi_insn *insn, unsigned int *data);
-
-/*
- * Attach is called by the Comedi core to configure the driver
- * for a particular board.  If you specified a board_name array
- * in the driver structure, dev->board_ptr contains that
- * address.
- */
-static int pcmda12_attach(struct comedi_device *dev,
-			  struct comedi_devconfig *it)
-{
-	struct comedi_subdevice *s;
-	unsigned long iobase;
-
-	iobase = it->options[0];
-	printk(KERN_INFO
-	       "comedi%d: %s: io: %lx %s ", dev->minor, driver.driver_name,
-	       iobase, it->options[1] ? "simultaneous xfer mode enabled" : "");
-
-	if (!request_region(iobase, IOSIZE, driver.driver_name)) {
-		printk("I/O port conflict\n");
-		return -EIO;
-	}
-	dev->iobase = iobase;
-
-/*
- * Initialize dev->board_name.  Note that we can use the "thisboard"
- * macro now, since we just initialized it in the last line.
- */
-	dev->board_name = thisboard->name;
-
-/*
- * Allocate the private structure area.  alloc_private() is a
- * convenient macro defined in comedidev.h.
- */
-	if (alloc_private(dev, sizeof(struct pcmda12_private)) < 0) {
-		printk(KERN_ERR "cannot allocate private data structure\n");
-		return -ENOMEM;
-	}
-
-	devpriv->simultaneous_xfer_mode = it->options[1];
-
-	/*
-	 * Allocate the subdevice structures.  alloc_subdevice() is a
-	 * convenient macro defined in comedidev.h.
-	 *
-	 * Allocate 2 subdevs (32 + 16 DIO lines) or 3 32 DIO subdevs for the
-	 * 96-channel version of the board.
-	 */
-	if (alloc_subdevices(dev, 1) < 0) {
-		printk(KERN_ERR "cannot allocate subdevice data structures\n");
-		return -ENOMEM;
-	}
-
-	s = dev->subdevices;
-	s->private = NULL;
-	s->maxdata = (0x1 << BITS) - 1;
-	s->range_table = &pcmda12_ranges;
-	s->type = COMEDI_SUBD_AO;
-	s->subdev_flags = SDF_READABLE | SDF_WRITABLE;
-	s->n_chan = CHANS;
-	s->insn_write = &ao_winsn;
-	s->insn_read = &ao_rinsn;
-
-	zero_chans(dev);	/* clear out all the registers, basically */
-
-	printk(KERN_INFO "attached\n");
-
-	return 1;
-}
-
-/*
- * _detach is called to deconfigure a device.  It should deallocate
- * resources.
- * This function is also called when _attach() fails, so it should be
- * careful not to release resources that were not necessarily
- * allocated by _attach().  dev->private and dev->subdevices are
- * deallocated automatically by the core.
- */
-static int pcmda12_detach(struct comedi_device *dev)
-{
-	printk(KERN_INFO
-	       "comedi%d: %s: remove\n", dev->minor, driver.driver_name);
-	if (dev->iobase)
-		release_region(dev->iobase, IOSIZE);
-	return 0;
-}
-
 static void zero_chans(struct comedi_device *dev)
 {				/* sets up an
 				   ASIC chip to defaults */
@@ -301,22 +164,91 @@ static int ao_rinsn(struct comedi_device *dev, struct comedi_subdevice *s,
 	return i;
 }
 
+static int pcmda12_attach(struct comedi_device *dev,
+			  struct comedi_devconfig *it)
+{
+	struct comedi_subdevice *s;
+	unsigned long iobase;
+
+	iobase = it->options[0];
+	printk(KERN_INFO
+	       "comedi%d: %s: io: %lx %s ", dev->minor, dev->driver->driver_name,
+	       iobase, it->options[1] ? "simultaneous xfer mode enabled" : "");
+
+	if (!request_region(iobase, IOSIZE, dev->driver->driver_name)) {
+		printk("I/O port conflict\n");
+		return -EIO;
+	}
+	dev->iobase = iobase;
+
 /*
- * A convenient macro that defines init_module() and cleanup_module(),
- * as necessary.
+ * Initialize dev->board_name.  Note that we can use the "thisboard"
+ * macro now, since we just initialized it in the last line.
  */
-static int __init driver_init_module(void)
-{
-	return comedi_driver_register(&driver);
+	dev->board_name = thisboard->name;
+
+/*
+ * Allocate the private structure area.  alloc_private() is a
+ * convenient macro defined in comedidev.h.
+ */
+	if (alloc_private(dev, sizeof(struct pcmda12_private)) < 0) {
+		printk(KERN_ERR "cannot allocate private data structure\n");
+		return -ENOMEM;
+	}
+
+	devpriv->simultaneous_xfer_mode = it->options[1];
+
+	/*
+	 * Allocate the subdevice structures.  alloc_subdevice() is a
+	 * convenient macro defined in comedidev.h.
+	 *
+	 * Allocate 2 subdevs (32 + 16 DIO lines) or 3 32 DIO subdevs for the
+	 * 96-channel version of the board.
+	 */
+	if (alloc_subdevices(dev, 1) < 0) {
+		printk(KERN_ERR "cannot allocate subdevice data structures\n");
+		return -ENOMEM;
+	}
+
+	s = dev->subdevices;
+	s->private = NULL;
+	s->maxdata = (0x1 << BITS) - 1;
+	s->range_table = &pcmda12_ranges;
+	s->type = COMEDI_SUBD_AO;
+	s->subdev_flags = SDF_READABLE | SDF_WRITABLE;
+	s->n_chan = CHANS;
+	s->insn_write = &ao_winsn;
+	s->insn_read = &ao_rinsn;
+
+	zero_chans(dev);	/* clear out all the registers, basically */
+
+	printk(KERN_INFO "attached\n");
+
+	return 1;
 }
 
-static void __exit driver_cleanup_module(void)
+static void pcmda12_detach(struct comedi_device *dev)
 {
-	comedi_driver_unregister(&driver);
+	if (dev->iobase)
+		release_region(dev->iobase, IOSIZE);
 }
 
-module_init(driver_init_module);
-module_exit(driver_cleanup_module);
+static const struct pcmda12_board pcmda12_boards[] = {
+	{
+		.name	= "pcmda12",
+	},
+};
+
+static struct comedi_driver pcmda12_driver = {
+	.driver_name	= "pcmda12",
+	.module		= THIS_MODULE,
+	.attach		= pcmda12_attach,
+	.detach		= pcmda12_detach,
+	.board_name	= &pcmda12_boards[0].name,
+	.offset		= sizeof(struct pcmda12_board),
+	.num_names	= ARRAY_SIZE(pcmda12_boards),
+};
+module_comedi_driver(pcmda12_driver);
 
 MODULE_AUTHOR("Comedi http://www.comedi.org");
 MODULE_DESCRIPTION("Comedi low-level driver");
diff --git a/drivers/staging/comedi/drivers/pcmmio.c b/drivers/staging/comedi/drivers/pcmmio.c
index eddac00e4e2..efed168d2ba 100644
--- a/drivers/staging/comedi/drivers/pcmmio.c
+++ b/drivers/staging/comedi/drivers/pcmmio.c
@@ -145,13 +145,6 @@ Configuration Options:
 #define PAGE_ENAB 2
 #define PAGE_INT_ID 3
 
-static int ai_rinsn(struct comedi_device *, struct comedi_subdevice *,
-		    struct comedi_insn *, unsigned int *);
-static int ao_rinsn(struct comedi_device *, struct comedi_subdevice *,
-		    struct comedi_insn *, unsigned int *);
-static int ao_winsn(struct comedi_device *, struct comedi_subdevice *,
-		    struct comedi_insn *, unsigned int *);
-
 /*
  * Board descriptions for two imaginary boards.  Describing the
  * boards in this way is optional, and completely driver-dependent.
@@ -190,23 +183,6 @@ static const struct comedi_lrange ranges_ao = {
 	  RANGE(-2.5, 2.5), RANGE(-2.5, 7.5)}
 };
 
-static const struct pcmmio_board pcmmio_boards[] = {
-	{
-	 .name = "pcmmio",
-	 .dio_num_asics = 1,
-	 .dio_num_ports = 6,
-	 .total_iosize = 32,
-	 .ai_bits = 16,
-	 .ao_bits = 16,
-	 .n_ai_chans = 16,
-	 .n_ao_chans = 8,
-	 .ai_range_table = &ranges_ai,
-	 .ao_range_table = &ranges_ao,
-	 .ai_rinsn = ai_rinsn,
-	 .ao_rinsn = ao_rinsn,
-	 .ao_winsn = ao_winsn},
-};
-
 /*
  * Useful for shorthand access to the particular board structure
  */
@@ -293,312 +269,6 @@ struct pcmmio_private {
  */
 #define devpriv ((struct pcmmio_private *)dev->private)
 #define subpriv ((struct pcmmio_subdev_private *)s->private)
-/*
- * The struct comedi_driver structure tells the Comedi core module
- * which functions to call to configure/deconfigure (attach/detach)
- * the board, and also about the kernel module that contains
- * the device code.
- */
-static int pcmmio_attach(struct comedi_device *dev,
-			 struct comedi_devconfig *it);
-static int pcmmio_detach(struct comedi_device *dev);
-
-static struct comedi_driver driver = {
-	.driver_name = "pcmmio",
-	.module = THIS_MODULE,
-	.attach = pcmmio_attach,
-	.detach = pcmmio_detach,
-/* It is not necessary to implement the following members if you are
- * writing a driver for a ISA PnP or PCI card */
-	/* Most drivers will support multiple types of boards by
-	 * having an array of board structures.  These were defined
-	 * in pcmmio_boards[] above.  Note that the element 'name'
-	 * was first in the structure -- Comedi uses this fact to
-	 * extract the name of the board without knowing any details
-	 * about the structure except for its length.
-	 * When a device is attached (by comedi_config), the name
-	 * of the device is given to Comedi, and Comedi tries to
-	 * match it by going through the list of board names.  If
-	 * there is a match, the address of the pointer is put
-	 * into dev->board_ptr and driver->attach() is called.
-	 *
-	 * Note that these are not necessary if you can determine
-	 * the type of board in software.  ISA PnP, PCI, and PCMCIA
-	 * devices are such boards.
-	 */
-	.board_name = &pcmmio_boards[0].name,
-	.offset = sizeof(struct pcmmio_board),
-	.num_names = ARRAY_SIZE(pcmmio_boards),
-};
-
-static int pcmmio_dio_insn_bits(struct comedi_device *dev,
-				struct comedi_subdevice *s,
-				struct comedi_insn *insn, unsigned int *data);
-static int pcmmio_dio_insn_config(struct comedi_device *dev,
-				  struct comedi_subdevice *s,
-				  struct comedi_insn *insn, unsigned int *data);
-
-static irqreturn_t interrupt_pcmmio(int irq, void *d);
-static void pcmmio_stop_intr(struct comedi_device *, struct comedi_subdevice *);
-static int pcmmio_cancel(struct comedi_device *dev, struct comedi_subdevice *s);
-static int pcmmio_cmd(struct comedi_device *dev, struct comedi_subdevice *s);
-static int pcmmio_cmdtest(struct comedi_device *dev, struct comedi_subdevice *s,
-			  struct comedi_cmd *cmd);
-
-/* some helper functions to deal with specifics of this device's registers */
-/* sets up/clears ASIC chips to defaults */
-static void init_asics(struct comedi_device *dev);
-static void switch_page(struct comedi_device *dev, int asic, int page);
-#ifdef notused
-static void lock_port(struct comedi_device *dev, int asic, int port);
-static void unlock_port(struct comedi_device *dev, int asic, int port);
-#endif
-
-/*
- * Attach is called by the Comedi core to configure the driver
- * for a particular board.  If you specified a board_name array
- * in the driver structure, dev->board_ptr contains that
- * address.
- */
-static int pcmmio_attach(struct comedi_device *dev, struct comedi_devconfig *it)
-{
-	struct comedi_subdevice *s;
-	int sdev_no, chans_left, n_dio_subdevs, n_subdevs, port, asic,
-	    thisasic_chanct = 0;
-	unsigned long iobase;
-	unsigned int irq[MAX_ASICS];
-
-	iobase = it->options[0];
-	irq[0] = it->options[1];
-
-	printk(KERN_INFO "comedi%d: %s: io: %lx attaching...\n", dev->minor,
-			driver.driver_name, iobase);
-
-	dev->iobase = iobase;
-
-	if (!iobase || !request_region(iobase,
-				       thisboard->total_iosize,
-				       driver.driver_name)) {
-		printk(KERN_ERR "comedi%d: I/O port conflict\n", dev->minor);
-		return -EIO;
-	}
-
-/*
- * Initialize dev->board_name.  Note that we can use the "thisboard"
- * macro now, since we just initialized it in the last line.
- */
-	dev->board_name = thisboard->name;
-
-/*
- * Allocate the private structure area.  alloc_private() is a
- * convenient macro defined in comedidev.h.
- */
-	if (alloc_private(dev, sizeof(struct pcmmio_private)) < 0) {
-		printk(KERN_ERR "comedi%d: cannot allocate private data structure\n",
-				dev->minor);
-		return -ENOMEM;
-	}
-
-	for (asic = 0; asic < MAX_ASICS; ++asic) {
-		devpriv->asics[asic].num = asic;
-		devpriv->asics[asic].iobase =
-		    dev->iobase + 16 + asic * ASIC_IOSIZE;
-		/*
-		 * this gets actually set at the end of this function when we
-		 * request_irqs
-		 */
-		devpriv->asics[asic].irq = 0;
-		spin_lock_init(&devpriv->asics[asic].spinlock);
-	}
-
-	chans_left = CHANS_PER_ASIC * thisboard->dio_num_asics;
-	n_dio_subdevs = CALC_N_DIO_SUBDEVS(chans_left);
-	n_subdevs = n_dio_subdevs + 2;
-	devpriv->sprivs =
-	    kcalloc(n_subdevs, sizeof(struct pcmmio_subdev_private),
-		    GFP_KERNEL);
-	if (!devpriv->sprivs) {
-		printk(KERN_ERR "comedi%d: cannot allocate subdevice private data structures\n",
-				dev->minor);
-		return -ENOMEM;
-	}
-	/*
-	 * Allocate the subdevice structures.  alloc_subdevice() is a
-	 * convenient macro defined in comedidev.h.
-	 *
-	 * Allocate 1 AI + 1 AO + 2 DIO subdevs (24 lines per DIO)
-	 */
-	if (alloc_subdevices(dev, n_subdevs) < 0) {
-		printk(KERN_ERR "comedi%d: cannot allocate subdevice data structures\n",
-				dev->minor);
-		return -ENOMEM;
-	}
-
-	/* First, AI */
-	sdev_no = 0;
-	s = dev->subdevices + sdev_no;
-	s->private = devpriv->sprivs + sdev_no;
-	s->maxdata = (1 << thisboard->ai_bits) - 1;
-	s->range_table = thisboard->ai_range_table;
-	s->subdev_flags = SDF_READABLE | SDF_GROUND | SDF_DIFF;
-	s->type = COMEDI_SUBD_AI;
-	s->n_chan = thisboard->n_ai_chans;
-	s->len_chanlist = s->n_chan;
-	s->insn_read = thisboard->ai_rinsn;
-	subpriv->iobase = dev->iobase + 0;
-	/* initialize the resource enable register by clearing it */
-	outb(0, subpriv->iobase + 3);
-	outb(0, subpriv->iobase + 4 + 3);
-
-	/* Next, AO */
-	++sdev_no;
-	s = dev->subdevices + sdev_no;
-	s->private = devpriv->sprivs + sdev_no;
-	s->maxdata = (1 << thisboard->ao_bits) - 1;
-	s->range_table = thisboard->ao_range_table;
-	s->subdev_flags = SDF_READABLE;
-	s->type = COMEDI_SUBD_AO;
-	s->n_chan = thisboard->n_ao_chans;
-	s->len_chanlist = s->n_chan;
-	s->insn_read = thisboard->ao_rinsn;
-	s->insn_write = thisboard->ao_winsn;
-	subpriv->iobase = dev->iobase + 8;
-	/* initialize the resource enable register by clearing it */
-	outb(0, subpriv->iobase + 3);
-	outb(0, subpriv->iobase + 4 + 3);
-
-	++sdev_no;
-	port = 0;
-	asic = 0;
-	for (; sdev_no < (int)dev->n_subdevices; ++sdev_no) {
-		int byte_no;
-
-		s = dev->subdevices + sdev_no;
-		s->private = devpriv->sprivs + sdev_no;
-		s->maxdata = 1;
-		s->range_table = &range_digital;
-		s->subdev_flags = SDF_READABLE | SDF_WRITABLE;
-		s->type = COMEDI_SUBD_DIO;
-		s->insn_bits = pcmmio_dio_insn_bits;
-		s->insn_config = pcmmio_dio_insn_config;
-		s->n_chan = min(chans_left, MAX_CHANS_PER_SUBDEV);
-		subpriv->dio.intr.asic = -1;
-		subpriv->dio.intr.first_chan = -1;
-		subpriv->dio.intr.asic_chan = -1;
-		subpriv->dio.intr.num_asic_chans = -1;
-		subpriv->dio.intr.active = 0;
-		s->len_chanlist = 1;
-
-		/* save the ioport address for each 'port' of 8 channels in the
-		   subdevice */
-		for (byte_no = 0; byte_no < PORTS_PER_SUBDEV; ++byte_no, ++port) {
-			if (port >= PORTS_PER_ASIC) {
-				port = 0;
-				++asic;
-				thisasic_chanct = 0;
-			}
-			subpriv->iobases[byte_no] =
-			    devpriv->asics[asic].iobase + port;
-
-			if (thisasic_chanct <
-			    CHANS_PER_PORT * INTR_PORTS_PER_ASIC
-			    && subpriv->dio.intr.asic < 0) {
-				/*
-				 * this is an interrupt subdevice,
-				 * so setup the struct
-				 */
-				subpriv->dio.intr.asic = asic;
-				subpriv->dio.intr.active = 0;
-				subpriv->dio.intr.stop_count = 0;
-				subpriv->dio.intr.first_chan = byte_no * 8;
-				subpriv->dio.intr.asic_chan = thisasic_chanct;
-				subpriv->dio.intr.num_asic_chans =
-				    s->n_chan - subpriv->dio.intr.first_chan;
-				s->cancel = pcmmio_cancel;
-				s->do_cmd = pcmmio_cmd;
-				s->do_cmdtest = pcmmio_cmdtest;
-				s->len_chanlist =
-				    subpriv->dio.intr.num_asic_chans;
-			}
-			thisasic_chanct += CHANS_PER_PORT;
-		}
-		spin_lock_init(&subpriv->dio.intr.spinlock);
-
-		chans_left -= s->n_chan;
-
-		if (!chans_left) {
-			/*
-			 * reset the asic to our first asic,
-			 * to do intr subdevs
-			 */
-			asic = 0;
-			port = 0;
-		}
-
-	}
-
-	init_asics(dev);	/* clear out all the registers, basically */
-
-	for (asic = 0; irq[0] && asic < MAX_ASICS; ++asic) {
-		if (irq[asic]
-		    && request_irq(irq[asic], interrupt_pcmmio,
-				   IRQF_SHARED, thisboard->name, dev)) {
-			int i;
-			/* unroll the allocated irqs.. */
-			for (i = asic - 1; i >= 0; --i) {
-				free_irq(irq[i], dev);
-				devpriv->asics[i].irq = irq[i] = 0;
-			}
-			irq[asic] = 0;
-		}
-		devpriv->asics[asic].irq = irq[asic];
-	}
-
-	dev->irq = irq[0];	/*
-				 * grr.. wish comedi dev struct supported
-				 * multiple irqs..
-				 */
-
-	if (irq[0]) {
-		printk(KERN_DEBUG "comedi%d: irq: %u\n", dev->minor, irq[0]);
-		if (thisboard->dio_num_asics == 2 && irq[1])
-			printk(KERN_DEBUG "comedi%d: second ASIC irq: %u\n",
-					dev->minor, irq[1]);
-	} else {
-		printk(KERN_INFO "comedi%d: (IRQ mode disabled)\n", dev->minor);
-	}
-
-	printk(KERN_INFO "comedi%d: attached\n", dev->minor);
-
-	return 1;
-}
-
-/*
- * _detach is called to deconfigure a device.  It should deallocate
- * resources.
- * This function is also called when _attach() fails, so it should be
- * careful not to release resources that were not necessarily
- * allocated by _attach().  dev->private and dev->subdevices are
- * deallocated automatically by the core.
- */
-static int pcmmio_detach(struct comedi_device *dev)
-{
-	int i;
-
-	printk(KERN_INFO "comedi%d: %s: remove\n", dev->minor, driver.driver_name);
-	if (dev->iobase)
-		release_region(dev->iobase, thisboard->total_iosize);
-
-	for (i = 0; i < MAX_ASICS; ++i) {
-		if (devpriv && devpriv->asics[i].irq)
-			free_irq(devpriv->asics[i].irq, dev);
-	}
-
-	if (devpriv && devpriv->sprivs)
-		kfree(devpriv->sprivs);
-
-	return 0;
-}
 
 /* DIO devices are slightly special.  Although it is possible to
  * implement the insn_read/insn_write interface, it is much more
@@ -667,7 +337,7 @@ static int pcmmio_dio_insn_bits(struct comedi_device *dev,
 		}
 #ifdef DAMMIT_ITS_BROKEN
 		/* DEBUG */
-		printk("data_out_byte %02x\n", (unsigned)byte);
+		printk(KERN_DEBUG "data_out_byte %02x\n", (unsigned)byte);
 #endif
 		/* save the digital input lines for this byte.. */
 		s->state |= ((unsigned int)byte) << offset;
@@ -751,6 +421,21 @@ static int pcmmio_dio_insn_config(struct comedi_device *dev,
 	return insn->n;
 }
 
+static void switch_page(struct comedi_device *dev, int asic, int page)
+{
+	if (asic < 0 || asic >= thisboard->dio_num_asics)
+		return;		/* paranoia */
+	if (page < 0 || page >= NUM_PAGES)
+		return;		/* more paranoia */
+
+	devpriv->asics[asic].pagelock &= ~REG_PAGE_MASK;
+	devpriv->asics[asic].pagelock |= page << REG_PAGE_BITOFFSET;
+
+	/* now write out the shadow register */
+	outb(devpriv->asics[asic].pagelock,
+	     devpriv->asics[asic].iobase + REG_PAGELOCK);
+}
+
 static void init_asics(struct comedi_device *dev)
 {				/* sets up an
 				   ASIC chip to defaults */
@@ -788,21 +473,6 @@ static void init_asics(struct comedi_device *dev)
 	}
 }
 
-static void switch_page(struct comedi_device *dev, int asic, int page)
-{
-	if (asic < 0 || asic >= thisboard->dio_num_asics)
-		return;		/* paranoia */
-	if (page < 0 || page >= NUM_PAGES)
-		return;		/* more paranoia */
-
-	devpriv->asics[asic].pagelock &= ~REG_PAGE_MASK;
-	devpriv->asics[asic].pagelock |= page << REG_PAGE_BITOFFSET;
-
-	/* now write out the shadow register */
-	outb(devpriv->asics[asic].pagelock,
-	     devpriv->asics[asic].iobase + REG_PAGELOCK);
-}
-
 #ifdef notused
 static void lock_port(struct comedi_device *dev, int asic, int port)
 {
@@ -831,6 +501,27 @@ static void unlock_port(struct comedi_device *dev, int asic, int port)
 }
 #endif /* notused */
 
+static void pcmmio_stop_intr(struct comedi_device *dev,
+			     struct comedi_subdevice *s)
+{
+	int nports, firstport, asic, port;
+
+	asic = subpriv->dio.intr.asic;
+	if (asic < 0)
+		return;		/* not an interrupt subdev */
+
+	subpriv->dio.intr.enabled_mask = 0;
+	subpriv->dio.intr.active = 0;
+	s->async->inttrig = 0;
+	nports = subpriv->dio.intr.num_asic_chans / CHANS_PER_PORT;
+	firstport = subpriv->dio.intr.asic_chan / CHANS_PER_PORT;
+	switch_page(dev, asic, PAGE_ENAB);
+	for (port = firstport; port < firstport + nports; ++port) {
+		/* disable all intrs for this subdev.. */
+		outb(0, devpriv->asics[asic].iobase + REG_ENAB0 + port);
+	}
+}
+
 static irqreturn_t interrupt_pcmmio(int irq, void *d)
 {
 	int asic, got1 = 0;
@@ -991,27 +682,6 @@ static irqreturn_t interrupt_pcmmio(int irq, void *d)
 	return IRQ_HANDLED;
 }
 
-static void pcmmio_stop_intr(struct comedi_device *dev,
-			     struct comedi_subdevice *s)
-{
-	int nports, firstport, asic, port;
-
-	asic = subpriv->dio.intr.asic;
-	if (asic < 0)
-		return;		/* not an interrupt subdev */
-
-	subpriv->dio.intr.enabled_mask = 0;
-	subpriv->dio.intr.active = 0;
-	s->async->inttrig = 0;
-	nports = subpriv->dio.intr.num_asic_chans / CHANS_PER_PORT;
-	firstport = subpriv->dio.intr.asic_chan / CHANS_PER_PORT;
-	switch_page(dev, asic, PAGE_ENAB);
-	for (port = firstport; port < firstport + nports; ++port) {
-		/* disable all intrs for this subdev.. */
-		outb(0, devpriv->asics[asic].iobase + REG_ENAB0 + port);
-	}
-}
-
 static int pcmmio_start_intr(struct comedi_device *dev,
 			     struct comedi_subdevice *s)
 {
@@ -1340,22 +1010,261 @@ static int ao_winsn(struct comedi_device *dev, struct comedi_subdevice *s,
 	return n;
 }
 
+static int pcmmio_attach(struct comedi_device *dev, struct comedi_devconfig *it)
+{
+	struct comedi_subdevice *s;
+	int sdev_no, chans_left, n_dio_subdevs, n_subdevs, port, asic,
+	    thisasic_chanct = 0;
+	unsigned long iobase;
+	unsigned int irq[MAX_ASICS];
+
+	iobase = it->options[0];
+	irq[0] = it->options[1];
+
+	printk(KERN_INFO "comedi%d: %s: io: %lx attaching...\n", dev->minor,
+			dev->driver->driver_name, iobase);
+
+	dev->iobase = iobase;
+
+	if (!iobase || !request_region(iobase,
+				       thisboard->total_iosize,
+				       dev->driver->driver_name)) {
+		printk(KERN_ERR "comedi%d: I/O port conflict\n", dev->minor);
+		return -EIO;
+	}
+
 /*
- * A convenient macro that defines init_module() and cleanup_module(),
- * as necessary.
+ * Initialize dev->board_name.  Note that we can use the "thisboard"
+ * macro now, since we just initialized it in the last line.
  */
-static int __init driver_init_module(void)
-{
-	return comedi_driver_register(&driver);
+	dev->board_name = thisboard->name;
+
+/*
+ * Allocate the private structure area.  alloc_private() is a
+ * convenient macro defined in comedidev.h.
+ */
+	if (alloc_private(dev, sizeof(struct pcmmio_private)) < 0) {
+		printk(KERN_ERR "comedi%d: cannot allocate private data structure\n",
+				dev->minor);
+		return -ENOMEM;
+	}
+
+	for (asic = 0; asic < MAX_ASICS; ++asic) {
+		devpriv->asics[asic].num = asic;
+		devpriv->asics[asic].iobase =
+		    dev->iobase + 16 + asic * ASIC_IOSIZE;
+		/*
+		 * this gets actually set at the end of this function when we
+		 * request_irqs
+		 */
+		devpriv->asics[asic].irq = 0;
+		spin_lock_init(&devpriv->asics[asic].spinlock);
+	}
+
+	chans_left = CHANS_PER_ASIC * thisboard->dio_num_asics;
+	n_dio_subdevs = CALC_N_DIO_SUBDEVS(chans_left);
+	n_subdevs = n_dio_subdevs + 2;
+	devpriv->sprivs =
+	    kcalloc(n_subdevs, sizeof(struct pcmmio_subdev_private),
+		    GFP_KERNEL);
+	if (!devpriv->sprivs) {
+		printk(KERN_ERR "comedi%d: cannot allocate subdevice private data structures\n",
+				dev->minor);
+		return -ENOMEM;
+	}
+	/*
+	 * Allocate the subdevice structures.  alloc_subdevice() is a
+	 * convenient macro defined in comedidev.h.
+	 *
+	 * Allocate 1 AI + 1 AO + 2 DIO subdevs (24 lines per DIO)
+	 */
+	if (alloc_subdevices(dev, n_subdevs) < 0) {
+		printk(KERN_ERR "comedi%d: cannot allocate subdevice data structures\n",
+				dev->minor);
+		return -ENOMEM;
+	}
+
+	/* First, AI */
+	sdev_no = 0;
+	s = dev->subdevices + sdev_no;
+	s->private = devpriv->sprivs + sdev_no;
+	s->maxdata = (1 << thisboard->ai_bits) - 1;
+	s->range_table = thisboard->ai_range_table;
+	s->subdev_flags = SDF_READABLE | SDF_GROUND | SDF_DIFF;
+	s->type = COMEDI_SUBD_AI;
+	s->n_chan = thisboard->n_ai_chans;
+	s->len_chanlist = s->n_chan;
+	s->insn_read = thisboard->ai_rinsn;
+	subpriv->iobase = dev->iobase + 0;
+	/* initialize the resource enable register by clearing it */
+	outb(0, subpriv->iobase + 3);
+	outb(0, subpriv->iobase + 4 + 3);
+
+	/* Next, AO */
+	++sdev_no;
+	s = dev->subdevices + sdev_no;
+	s->private = devpriv->sprivs + sdev_no;
+	s->maxdata = (1 << thisboard->ao_bits) - 1;
+	s->range_table = thisboard->ao_range_table;
+	s->subdev_flags = SDF_READABLE;
+	s->type = COMEDI_SUBD_AO;
+	s->n_chan = thisboard->n_ao_chans;
+	s->len_chanlist = s->n_chan;
+	s->insn_read = thisboard->ao_rinsn;
+	s->insn_write = thisboard->ao_winsn;
+	subpriv->iobase = dev->iobase + 8;
+	/* initialize the resource enable register by clearing it */
+	outb(0, subpriv->iobase + 3);
+	outb(0, subpriv->iobase + 4 + 3);
+
+	++sdev_no;
+	port = 0;
+	asic = 0;
+	for (; sdev_no < (int)dev->n_subdevices; ++sdev_no) {
+		int byte_no;
+
+		s = dev->subdevices + sdev_no;
+		s->private = devpriv->sprivs + sdev_no;
+		s->maxdata = 1;
+		s->range_table = &range_digital;
+		s->subdev_flags = SDF_READABLE | SDF_WRITABLE;
+		s->type = COMEDI_SUBD_DIO;
+		s->insn_bits = pcmmio_dio_insn_bits;
+		s->insn_config = pcmmio_dio_insn_config;
+		s->n_chan = min(chans_left, MAX_CHANS_PER_SUBDEV);
+		subpriv->dio.intr.asic = -1;
+		subpriv->dio.intr.first_chan = -1;
+		subpriv->dio.intr.asic_chan = -1;
+		subpriv->dio.intr.num_asic_chans = -1;
+		subpriv->dio.intr.active = 0;
+		s->len_chanlist = 1;
+
+		/* save the ioport address for each 'port' of 8 channels in the
+		   subdevice */
+		for (byte_no = 0; byte_no < PORTS_PER_SUBDEV; ++byte_no, ++port) {
+			if (port >= PORTS_PER_ASIC) {
+				port = 0;
+				++asic;
+				thisasic_chanct = 0;
+			}
+			subpriv->iobases[byte_no] =
+			    devpriv->asics[asic].iobase + port;
+
+			if (thisasic_chanct <
+			    CHANS_PER_PORT * INTR_PORTS_PER_ASIC
+			    && subpriv->dio.intr.asic < 0) {
+				/*
+				 * this is an interrupt subdevice,
+				 * so setup the struct
+				 */
+				subpriv->dio.intr.asic = asic;
+				subpriv->dio.intr.active = 0;
+				subpriv->dio.intr.stop_count = 0;
+				subpriv->dio.intr.first_chan = byte_no * 8;
+				subpriv->dio.intr.asic_chan = thisasic_chanct;
+				subpriv->dio.intr.num_asic_chans =
+				    s->n_chan - subpriv->dio.intr.first_chan;
+				s->cancel = pcmmio_cancel;
+				s->do_cmd = pcmmio_cmd;
+				s->do_cmdtest = pcmmio_cmdtest;
+				s->len_chanlist =
+				    subpriv->dio.intr.num_asic_chans;
+			}
+			thisasic_chanct += CHANS_PER_PORT;
+		}
+		spin_lock_init(&subpriv->dio.intr.spinlock);
+
+		chans_left -= s->n_chan;
+
+		if (!chans_left) {
+			/*
+			 * reset the asic to our first asic,
+			 * to do intr subdevs
+			 */
+			asic = 0;
+			port = 0;
+		}
+
+	}
+
+	init_asics(dev);	/* clear out all the registers, basically */
+
+	for (asic = 0; irq[0] && asic < MAX_ASICS; ++asic) {
+		if (irq[asic]
+		    && request_irq(irq[asic], interrupt_pcmmio,
+				   IRQF_SHARED, thisboard->name, dev)) {
+			int i;
+			/* unroll the allocated irqs.. */
+			for (i = asic - 1; i >= 0; --i) {
+				free_irq(irq[i], dev);
+				devpriv->asics[i].irq = irq[i] = 0;
+			}
+			irq[asic] = 0;
+		}
+		devpriv->asics[asic].irq = irq[asic];
+	}
+
+	dev->irq = irq[0];	/*
+				 * grr.. wish comedi dev struct supported
+				 * multiple irqs..
+				 */
+
+	if (irq[0]) {
+		printk(KERN_DEBUG "comedi%d: irq: %u\n", dev->minor, irq[0]);
+		if (thisboard->dio_num_asics == 2 && irq[1])
+			printk(KERN_DEBUG "comedi%d: second ASIC irq: %u\n",
+					dev->minor, irq[1]);
+	} else {
+		printk(KERN_INFO "comedi%d: (IRQ mode disabled)\n", dev->minor);
+	}
+
+	printk(KERN_INFO "comedi%d: attached\n", dev->minor);
+
+	return 1;
 }
 
-static void __exit driver_cleanup_module(void)
+static void pcmmio_detach(struct comedi_device *dev)
 {
-	comedi_driver_unregister(&driver);
+	int i;
+
+	if (dev->iobase)
+		release_region(dev->iobase, thisboard->total_iosize);
+	for (i = 0; i < MAX_ASICS; ++i) {
+		if (devpriv && devpriv->asics[i].irq)
+			free_irq(devpriv->asics[i].irq, dev);
+	}
+	if (devpriv && devpriv->sprivs)
+		kfree(devpriv->sprivs);
 }
 
-module_init(driver_init_module);
-module_exit(driver_cleanup_module);
+static const struct pcmmio_board pcmmio_boards[] = {
+	{
+		.name		= "pcmmio",
+		.dio_num_asics	= 1,
+		.dio_num_ports	= 6,
+		.total_iosize	= 32,
+		.ai_bits	= 16,
+		.ao_bits	= 16,
+		.n_ai_chans	= 16,
+		.n_ao_chans	= 8,
+		.ai_range_table	= &ranges_ai,
+		.ao_range_table	= &ranges_ao,
+		.ai_rinsn	= ai_rinsn,
+		.ao_rinsn	= ao_rinsn,
+		.ao_winsn	= ao_winsn
+	},
+};
+
+static struct comedi_driver pcmmio_driver = {
+	.driver_name	= "pcmmio",
+	.module		= THIS_MODULE,
+	.attach		= pcmmio_attach,
+	.detach		= pcmmio_detach,
+	.board_name	= &pcmmio_boards[0].name,
+	.offset		= sizeof(struct pcmmio_board),
+	.num_names	= ARRAY_SIZE(pcmmio_boards),
+};
+module_comedi_driver(pcmmio_driver);
 
 MODULE_AUTHOR("Comedi http://www.comedi.org");
 MODULE_DESCRIPTION("Comedi low-level driver");
diff --git a/drivers/staging/comedi/drivers/pcmuio.c b/drivers/staging/comedi/drivers/pcmuio.c
index 661ba2e0389..623381d50da 100644
--- a/drivers/staging/comedi/drivers/pcmuio.c
+++ b/drivers/staging/comedi/drivers/pcmuio.c
@@ -155,19 +155,6 @@ struct pcmuio_board {
 	const int num_ports;
 };
 
-static const struct pcmuio_board pcmuio_boards[] = {
-	{
-	 .name = "pcmuio48",
-	 .num_asics = 1,
-	 .num_ports = 6,
-	 },
-	{
-	 .name = "pcmuio96",
-	 .num_asics = 2,
-	 .num_ports = 12,
-	 },
-};
-
 /*
  * Useful for shorthand access to the particular board structure
  */
@@ -218,262 +205,6 @@ struct pcmuio_private {
  */
 #define devpriv ((struct pcmuio_private *)dev->private)
 #define subpriv ((struct pcmuio_subdev_private *)s->private)
-/*
- * The struct comedi_driver structure tells the Comedi core module
- * which functions to call to configure/deconfigure (attach/detach)
- * the board, and also about the kernel module that contains
- * the device code.
- */
-static int pcmuio_attach(struct comedi_device *dev,
-			 struct comedi_devconfig *it);
-static int pcmuio_detach(struct comedi_device *dev);
-
-static struct comedi_driver driver = {
-	.driver_name = "pcmuio",
-	.module = THIS_MODULE,
-	.attach = pcmuio_attach,
-	.detach = pcmuio_detach,
-/* It is not necessary to implement the following members if you are
- * writing a driver for a ISA PnP or PCI card */
-	/* Most drivers will support multiple types of boards by
-	 * having an array of board structures.  These were defined
-	 * in pcmuio_boards[] above.  Note that the element 'name'
-	 * was first in the structure -- Comedi uses this fact to
-	 * extract the name of the board without knowing any details
-	 * about the structure except for its length.
-	 * When a device is attached (by comedi_config), the name
-	 * of the device is given to Comedi, and Comedi tries to
-	 * match it by going through the list of board names.  If
-	 * there is a match, the address of the pointer is put
-	 * into dev->board_ptr and driver->attach() is called.
-	 *
-	 * Note that these are not necessary if you can determine
-	 * the type of board in software.  ISA PnP, PCI, and PCMCIA
-	 * devices are such boards.
-	 */
-	.board_name = &pcmuio_boards[0].name,
-	.offset = sizeof(struct pcmuio_board),
-	.num_names = ARRAY_SIZE(pcmuio_boards),
-};
-
-static int pcmuio_dio_insn_bits(struct comedi_device *dev,
-				struct comedi_subdevice *s,
-				struct comedi_insn *insn, unsigned int *data);
-static int pcmuio_dio_insn_config(struct comedi_device *dev,
-				  struct comedi_subdevice *s,
-				  struct comedi_insn *insn, unsigned int *data);
-
-static irqreturn_t interrupt_pcmuio(int irq, void *d);
-static void pcmuio_stop_intr(struct comedi_device *, struct comedi_subdevice *);
-static int pcmuio_cancel(struct comedi_device *dev, struct comedi_subdevice *s);
-static int pcmuio_cmd(struct comedi_device *dev, struct comedi_subdevice *s);
-static int pcmuio_cmdtest(struct comedi_device *dev, struct comedi_subdevice *s,
-			  struct comedi_cmd *cmd);
-
-/* some helper functions to deal with specifics of this device's registers */
-static void init_asics(struct comedi_device *dev);	/* sets up/clears ASIC chips to defaults */
-static void switch_page(struct comedi_device *dev, int asic, int page);
-#ifdef notused
-static void lock_port(struct comedi_device *dev, int asic, int port);
-static void unlock_port(struct comedi_device *dev, int asic, int port);
-#endif
-
-/*
- * Attach is called by the Comedi core to configure the driver
- * for a particular board.  If you specified a board_name array
- * in the driver structure, dev->board_ptr contains that
- * address.
- */
-static int pcmuio_attach(struct comedi_device *dev, struct comedi_devconfig *it)
-{
-	struct comedi_subdevice *s;
-	int sdev_no, chans_left, n_subdevs, port, asic, thisasic_chanct = 0;
-	unsigned long iobase;
-	unsigned int irq[MAX_ASICS];
-
-	iobase = it->options[0];
-	irq[0] = it->options[1];
-	irq[1] = it->options[2];
-
-	dev_dbg(dev->hw_dev, "comedi%d: %s: io: %lx attached\n", dev->minor,
-		driver.driver_name, iobase);
-
-	dev->iobase = iobase;
-
-	if (!iobase || !request_region(iobase,
-				       thisboard->num_asics * ASIC_IOSIZE,
-				       driver.driver_name)) {
-		dev_err(dev->hw_dev, "I/O port conflict\n");
-		return -EIO;
-	}
-
-/*
- * Initialize dev->board_name.  Note that we can use the "thisboard"
- * macro now, since we just initialized it in the last line.
- */
-	dev->board_name = thisboard->name;
-
-/*
- * Allocate the private structure area.  alloc_private() is a
- * convenient macro defined in comedidev.h.
- */
-	if (alloc_private(dev, sizeof(struct pcmuio_private)) < 0) {
-		dev_warn(dev->hw_dev, "cannot allocate private data structure\n");
-		return -ENOMEM;
-	}
-
-	for (asic = 0; asic < MAX_ASICS; ++asic) {
-		devpriv->asics[asic].num = asic;
-		devpriv->asics[asic].iobase = dev->iobase + asic * ASIC_IOSIZE;
-		devpriv->asics[asic].irq = 0;	/* this gets actually set at the end of
-						   this function when we
-						   request_irqs */
-		spin_lock_init(&devpriv->asics[asic].spinlock);
-	}
-
-	chans_left = CHANS_PER_ASIC * thisboard->num_asics;
-	n_subdevs = CALC_N_SUBDEVS(chans_left);
-	devpriv->sprivs =
-	    kcalloc(n_subdevs, sizeof(struct pcmuio_subdev_private),
-		    GFP_KERNEL);
-	if (!devpriv->sprivs) {
-		dev_warn(dev->hw_dev, "cannot allocate subdevice private data structures\n");
-		return -ENOMEM;
-	}
-	/*
-	 * Allocate the subdevice structures.  alloc_subdevice() is a
-	 * convenient macro defined in comedidev.h.
-	 *
-	 * Allocate 2 subdevs (32 + 16 DIO lines) or 3 32 DIO subdevs for the
-	 * 96-channel version of the board.
-	 */
-	if (alloc_subdevices(dev, n_subdevs) < 0) {
-		dev_dbg(dev->hw_dev, "cannot allocate subdevice data structures\n");
-		return -ENOMEM;
-	}
-
-	port = 0;
-	asic = 0;
-	for (sdev_no = 0; sdev_no < (int)dev->n_subdevices; ++sdev_no) {
-		int byte_no;
-
-		s = dev->subdevices + sdev_no;
-		s->private = devpriv->sprivs + sdev_no;
-		s->maxdata = 1;
-		s->range_table = &range_digital;
-		s->subdev_flags = SDF_READABLE | SDF_WRITABLE;
-		s->type = COMEDI_SUBD_DIO;
-		s->insn_bits = pcmuio_dio_insn_bits;
-		s->insn_config = pcmuio_dio_insn_config;
-		s->n_chan = min(chans_left, MAX_CHANS_PER_SUBDEV);
-		subpriv->intr.asic = -1;
-		subpriv->intr.first_chan = -1;
-		subpriv->intr.asic_chan = -1;
-		subpriv->intr.num_asic_chans = -1;
-		subpriv->intr.active = 0;
-		s->len_chanlist = 1;
-
-		/* save the ioport address for each 'port' of 8 channels in the
-		   subdevice */
-		for (byte_no = 0; byte_no < PORTS_PER_SUBDEV; ++byte_no, ++port) {
-			if (port >= PORTS_PER_ASIC) {
-				port = 0;
-				++asic;
-				thisasic_chanct = 0;
-			}
-			subpriv->iobases[byte_no] =
-			    devpriv->asics[asic].iobase + port;
-
-			if (thisasic_chanct <
-			    CHANS_PER_PORT * INTR_PORTS_PER_ASIC
-			    && subpriv->intr.asic < 0) {
-				/* this is an interrupt subdevice, so setup the struct */
-				subpriv->intr.asic = asic;
-				subpriv->intr.active = 0;
-				subpriv->intr.stop_count = 0;
-				subpriv->intr.first_chan = byte_no * 8;
-				subpriv->intr.asic_chan = thisasic_chanct;
-				subpriv->intr.num_asic_chans =
-				    s->n_chan - subpriv->intr.first_chan;
-				dev->read_subdev = s;
-				s->subdev_flags |= SDF_CMD_READ;
-				s->cancel = pcmuio_cancel;
-				s->do_cmd = pcmuio_cmd;
-				s->do_cmdtest = pcmuio_cmdtest;
-				s->len_chanlist = subpriv->intr.num_asic_chans;
-			}
-			thisasic_chanct += CHANS_PER_PORT;
-		}
-		spin_lock_init(&subpriv->intr.spinlock);
-
-		chans_left -= s->n_chan;
-
-		if (!chans_left) {
-			asic = 0;	/* reset the asic to our first asic, to do intr subdevs */
-			port = 0;
-		}
-
-	}
-
-	init_asics(dev);	/* clear out all the registers, basically */
-
-	for (asic = 0; irq[0] && asic < MAX_ASICS; ++asic) {
-		if (irq[asic]
-		    && request_irq(irq[asic], interrupt_pcmuio,
-				   IRQF_SHARED, thisboard->name, dev)) {
-			int i;
-			/* unroll the allocated irqs.. */
-			for (i = asic - 1; i >= 0; --i) {
-				free_irq(irq[i], dev);
-				devpriv->asics[i].irq = irq[i] = 0;
-			}
-			irq[asic] = 0;
-		}
-		devpriv->asics[asic].irq = irq[asic];
-	}
-
-	dev->irq = irq[0];	/* grr.. wish comedi dev struct supported multiple
-				   irqs.. */
-
-	if (irq[0]) {
-		dev_dbg(dev->hw_dev, "irq: %u\n", irq[0]);
-		if (irq[1] && thisboard->num_asics == 2)
-			dev_dbg(dev->hw_dev, "second ASIC irq: %u\n", irq[1]);
-	} else {
-		dev_dbg(dev->hw_dev, "(IRQ mode disabled)\n");
-	}
-
-
-	return 1;
-}
-
-/*
- * _detach is called to deconfigure a device.  It should deallocate
- * resources.
- * This function is also called when _attach() fails, so it should be
- * careful not to release resources that were not necessarily
- * allocated by _attach().  dev->private and dev->subdevices are
- * deallocated automatically by the core.
- */
-static int pcmuio_detach(struct comedi_device *dev)
-{
-	int i;
-
-	dev_dbg(dev->hw_dev, "comedi%d: %s: remove\n", dev->minor,
-		driver.driver_name);
-	if (dev->iobase)
-		release_region(dev->iobase, ASIC_IOSIZE * thisboard->num_asics);
-
-	for (i = 0; i < MAX_ASICS; ++i) {
-		if (devpriv->asics[i].irq)
-			free_irq(devpriv->asics[i].irq, dev);
-	}
-
-	if (devpriv && devpriv->sprivs)
-		kfree(devpriv->sprivs);
-
-	return 0;
-}
 
 /* DIO devices are slightly special.  Although it is possible to
  * implement the insn_read/insn_write interface, it is much more
@@ -621,6 +352,21 @@ static int pcmuio_dio_insn_config(struct comedi_device *dev,
 	return insn->n;
 }
 
+static void switch_page(struct comedi_device *dev, int asic, int page)
+{
+	if (asic < 0 || asic >= thisboard->num_asics)
+		return;		/* paranoia */
+	if (page < 0 || page >= NUM_PAGES)
+		return;		/* more paranoia */
+
+	devpriv->asics[asic].pagelock &= ~REG_PAGE_MASK;
+	devpriv->asics[asic].pagelock |= page << REG_PAGE_BITOFFSET;
+
+	/* now write out the shadow register */
+	outb(devpriv->asics[asic].pagelock,
+	     dev->iobase + ASIC_IOSIZE * asic + REG_PAGELOCK);
+}
+
 static void init_asics(struct comedi_device *dev)
 {				/* sets up an
 				   ASIC chip to defaults */
@@ -658,21 +404,6 @@ static void init_asics(struct comedi_device *dev)
 	}
 }
 
-static void switch_page(struct comedi_device *dev, int asic, int page)
-{
-	if (asic < 0 || asic >= thisboard->num_asics)
-		return;		/* paranoia */
-	if (page < 0 || page >= NUM_PAGES)
-		return;		/* more paranoia */
-
-	devpriv->asics[asic].pagelock &= ~REG_PAGE_MASK;
-	devpriv->asics[asic].pagelock |= page << REG_PAGE_BITOFFSET;
-
-	/* now write out the shadow register */
-	outb(devpriv->asics[asic].pagelock,
-	     dev->iobase + ASIC_IOSIZE * asic + REG_PAGELOCK);
-}
-
 #ifdef notused
 static void lock_port(struct comedi_device *dev, int asic, int port)
 {
@@ -700,6 +431,27 @@ static void unlock_port(struct comedi_device *dev, int asic, int port)
 }
 #endif /* notused */
 
+static void pcmuio_stop_intr(struct comedi_device *dev,
+			     struct comedi_subdevice *s)
+{
+	int nports, firstport, asic, port;
+
+	asic = subpriv->intr.asic;
+	if (asic < 0)
+		return;		/* not an interrupt subdev */
+
+	subpriv->intr.enabled_mask = 0;
+	subpriv->intr.active = 0;
+	s->async->inttrig = 0;
+	nports = subpriv->intr.num_asic_chans / CHANS_PER_PORT;
+	firstport = subpriv->intr.asic_chan / CHANS_PER_PORT;
+	switch_page(dev, asic, PAGE_ENAB);
+	for (port = firstport; port < firstport + nports; ++port) {
+		/* disable all intrs for this subdev.. */
+		outb(0, devpriv->asics[asic].iobase + REG_ENAB0 + port);
+	}
+}
+
 static irqreturn_t interrupt_pcmuio(int irq, void *d)
 {
 	int asic, got1 = 0;
@@ -852,27 +604,6 @@ static irqreturn_t interrupt_pcmuio(int irq, void *d)
 	return IRQ_HANDLED;
 }
 
-static void pcmuio_stop_intr(struct comedi_device *dev,
-			     struct comedi_subdevice *s)
-{
-	int nports, firstport, asic, port;
-
-	asic = subpriv->intr.asic;
-	if (asic < 0)
-		return;		/* not an interrupt subdev */
-
-	subpriv->intr.enabled_mask = 0;
-	subpriv->intr.active = 0;
-	s->async->inttrig = 0;
-	nports = subpriv->intr.num_asic_chans / CHANS_PER_PORT;
-	firstport = subpriv->intr.asic_chan / CHANS_PER_PORT;
-	switch_page(dev, asic, PAGE_ENAB);
-	for (port = firstport; port < firstport + nports; ++port) {
-		/* disable all intrs for this subdev.. */
-		outb(0, devpriv->asics[asic].iobase + REG_ENAB0 + port);
-	}
-}
-
 static int pcmuio_start_intr(struct comedi_device *dev,
 			     struct comedi_subdevice *s)
 {
@@ -1014,22 +745,205 @@ pcmuio_cmdtest(struct comedi_device *dev, struct comedi_subdevice *s,
 	return comedi_pcm_cmdtest(dev, s, cmd);
 }
 
+static int pcmuio_attach(struct comedi_device *dev, struct comedi_devconfig *it)
+{
+	struct comedi_subdevice *s;
+	int sdev_no, chans_left, n_subdevs, port, asic, thisasic_chanct = 0;
+	unsigned long iobase;
+	unsigned int irq[MAX_ASICS];
+
+	iobase = it->options[0];
+	irq[0] = it->options[1];
+	irq[1] = it->options[2];
+
+	dev_dbg(dev->hw_dev, "comedi%d: %s: io: %lx attached\n", dev->minor,
+		dev->driver->driver_name, iobase);
+
+	dev->iobase = iobase;
+
+	if (!iobase || !request_region(iobase,
+				       thisboard->num_asics * ASIC_IOSIZE,
+				       dev->driver->driver_name)) {
+		dev_err(dev->hw_dev, "I/O port conflict\n");
+		return -EIO;
+	}
+
 /*
- * A convenient macro that defines init_module() and cleanup_module(),
- * as necessary.
+ * Initialize dev->board_name.  Note that we can use the "thisboard"
+ * macro now, since we just initialized it in the last line.
  */
-static int __init driver_init_module(void)
-{
-	return comedi_driver_register(&driver);
+	dev->board_name = thisboard->name;
+
+/*
+ * Allocate the private structure area.  alloc_private() is a
+ * convenient macro defined in comedidev.h.
+ */
+	if (alloc_private(dev, sizeof(struct pcmuio_private)) < 0) {
+		dev_warn(dev->hw_dev, "cannot allocate private data structure\n");
+		return -ENOMEM;
+	}
+
+	for (asic = 0; asic < MAX_ASICS; ++asic) {
+		devpriv->asics[asic].num = asic;
+		devpriv->asics[asic].iobase = dev->iobase + asic * ASIC_IOSIZE;
+		devpriv->asics[asic].irq = 0;	/* this gets actually set at the end of
+						   this function when we
+						   request_irqs */
+		spin_lock_init(&devpriv->asics[asic].spinlock);
+	}
+
+	chans_left = CHANS_PER_ASIC * thisboard->num_asics;
+	n_subdevs = CALC_N_SUBDEVS(chans_left);
+	devpriv->sprivs =
+	    kcalloc(n_subdevs, sizeof(struct pcmuio_subdev_private),
+		    GFP_KERNEL);
+	if (!devpriv->sprivs) {
+		dev_warn(dev->hw_dev, "cannot allocate subdevice private data structures\n");
+		return -ENOMEM;
+	}
+	/*
+	 * Allocate the subdevice structures.  alloc_subdevice() is a
+	 * convenient macro defined in comedidev.h.
+	 *
+	 * Allocate 2 subdevs (32 + 16 DIO lines) or 3 32 DIO subdevs for the
+	 * 96-channel version of the board.
+	 */
+	if (alloc_subdevices(dev, n_subdevs) < 0) {
+		dev_dbg(dev->hw_dev, "cannot allocate subdevice data structures\n");
+		return -ENOMEM;
+	}
+
+	port = 0;
+	asic = 0;
+	for (sdev_no = 0; sdev_no < (int)dev->n_subdevices; ++sdev_no) {
+		int byte_no;
+
+		s = dev->subdevices + sdev_no;
+		s->private = devpriv->sprivs + sdev_no;
+		s->maxdata = 1;
+		s->range_table = &range_digital;
+		s->subdev_flags = SDF_READABLE | SDF_WRITABLE;
+		s->type = COMEDI_SUBD_DIO;
+		s->insn_bits = pcmuio_dio_insn_bits;
+		s->insn_config = pcmuio_dio_insn_config;
+		s->n_chan = min(chans_left, MAX_CHANS_PER_SUBDEV);
+		subpriv->intr.asic = -1;
+		subpriv->intr.first_chan = -1;
+		subpriv->intr.asic_chan = -1;
+		subpriv->intr.num_asic_chans = -1;
+		subpriv->intr.active = 0;
+		s->len_chanlist = 1;
+
+		/* save the ioport address for each 'port' of 8 channels in the
+		   subdevice */
+		for (byte_no = 0; byte_no < PORTS_PER_SUBDEV; ++byte_no, ++port) {
+			if (port >= PORTS_PER_ASIC) {
+				port = 0;
+				++asic;
+				thisasic_chanct = 0;
+			}
+			subpriv->iobases[byte_no] =
+			    devpriv->asics[asic].iobase + port;
+
+			if (thisasic_chanct <
+			    CHANS_PER_PORT * INTR_PORTS_PER_ASIC
+			    && subpriv->intr.asic < 0) {
+				/* this is an interrupt subdevice, so setup the struct */
+				subpriv->intr.asic = asic;
+				subpriv->intr.active = 0;
+				subpriv->intr.stop_count = 0;
+				subpriv->intr.first_chan = byte_no * 8;
+				subpriv->intr.asic_chan = thisasic_chanct;
+				subpriv->intr.num_asic_chans =
+				    s->n_chan - subpriv->intr.first_chan;
+				dev->read_subdev = s;
+				s->subdev_flags |= SDF_CMD_READ;
+				s->cancel = pcmuio_cancel;
+				s->do_cmd = pcmuio_cmd;
+				s->do_cmdtest = pcmuio_cmdtest;
+				s->len_chanlist = subpriv->intr.num_asic_chans;
+			}
+			thisasic_chanct += CHANS_PER_PORT;
+		}
+		spin_lock_init(&subpriv->intr.spinlock);
+
+		chans_left -= s->n_chan;
+
+		if (!chans_left) {
+			asic = 0;	/* reset the asic to our first asic, to do intr subdevs */
+			port = 0;
+		}
+
+	}
+
+	init_asics(dev);	/* clear out all the registers, basically */
+
+	for (asic = 0; irq[0] && asic < MAX_ASICS; ++asic) {
+		if (irq[asic]
+		    && request_irq(irq[asic], interrupt_pcmuio,
+				   IRQF_SHARED, thisboard->name, dev)) {
+			int i;
+			/* unroll the allocated irqs.. */
+			for (i = asic - 1; i >= 0; --i) {
+				free_irq(irq[i], dev);
+				devpriv->asics[i].irq = irq[i] = 0;
+			}
+			irq[asic] = 0;
+		}
+		devpriv->asics[asic].irq = irq[asic];
+	}
+
+	dev->irq = irq[0];	/* grr.. wish comedi dev struct supported multiple
+				   irqs.. */
+
+	if (irq[0]) {
+		dev_dbg(dev->hw_dev, "irq: %u\n", irq[0]);
+		if (irq[1] && thisboard->num_asics == 2)
+			dev_dbg(dev->hw_dev, "second ASIC irq: %u\n", irq[1]);
+	} else {
+		dev_dbg(dev->hw_dev, "(IRQ mode disabled)\n");
+	}
+
+
+	return 1;
 }
 
-static void __exit driver_cleanup_module(void)
+static void pcmuio_detach(struct comedi_device *dev)
 {
-	comedi_driver_unregister(&driver);
+	int i;
+
+	if (dev->iobase)
+		release_region(dev->iobase, ASIC_IOSIZE * thisboard->num_asics);
+	for (i = 0; i < MAX_ASICS; ++i) {
+		if (devpriv->asics[i].irq)
+			free_irq(devpriv->asics[i].irq, dev);
+	}
+	if (devpriv && devpriv->sprivs)
+		kfree(devpriv->sprivs);
 }
 
-module_init(driver_init_module);
-module_exit(driver_cleanup_module);
+static const struct pcmuio_board pcmuio_boards[] = {
+	{
+		.name		= "pcmuio48",
+		.num_asics	= 1,
+		.num_ports	= 6,
+	}, {
+		.name		= "pcmuio96",
+		.num_asics	= 2,
+		.num_ports	= 12,
+	},
+};
+
+static struct comedi_driver pcmuio_driver = {
+	.driver_name	= "pcmuio",
+	.module		= THIS_MODULE,
+	.attach		= pcmuio_attach,
+	.detach		= pcmuio_detach,
+	.board_name	= &pcmuio_boards[0].name,
+	.offset		= sizeof(struct pcmuio_board),
+	.num_names	= ARRAY_SIZE(pcmuio_boards),
+};
+module_comedi_driver(pcmuio_driver);
 
 MODULE_AUTHOR("Comedi http://www.comedi.org");
 MODULE_DESCRIPTION("Comedi low-level driver");
diff --git a/drivers/staging/comedi/drivers/poc.c b/drivers/staging/comedi/drivers/poc.c
index 831a576c24a..e7120480687 100644
--- a/drivers/staging/comedi/drivers/poc.c
+++ b/drivers/staging/comedi/drivers/poc.c
@@ -41,20 +41,6 @@ Configuration options:
 
 #include <linux/ioport.h>
 
-static int poc_attach(struct comedi_device *dev, struct comedi_devconfig *it);
-static int poc_detach(struct comedi_device *dev);
-static int readback_insn(struct comedi_device *dev, struct comedi_subdevice *s,
-			 struct comedi_insn *insn, unsigned int *data);
-
-static int dac02_ao_winsn(struct comedi_device *dev, struct comedi_subdevice *s,
-			  struct comedi_insn *insn, unsigned int *data);
-static int pcl733_insn_bits(struct comedi_device *dev,
-			    struct comedi_subdevice *s,
-			    struct comedi_insn *insn, unsigned int *data);
-static int pcl734_insn_bits(struct comedi_device *dev,
-			    struct comedi_subdevice *s,
-			    struct comedi_insn *insn, unsigned int *data);
-
 struct boarddef_struct {
 	const char *name;
 	unsigned int iosize;
@@ -70,108 +56,9 @@ struct boarddef_struct {
 			 struct comedi_insn *, unsigned int *);
 	const struct comedi_lrange *range;
 };
-static const struct boarddef_struct boards[] = {
-	{
-	 .name = "dac02",
-	 .iosize = 8,
-	 /*      .setup = dac02_setup, */
-	 .type = COMEDI_SUBD_AO,
-	 .n_chan = 2,
-	 .n_bits = 12,
-	 .winsn = dac02_ao_winsn,
-	 .rinsn = readback_insn,
-	 .range = &range_unknown,
-	 },
-	{
-	 .name = "pcl733",
-	 .iosize = 4,
-	 .type = COMEDI_SUBD_DI,
-	 .n_chan = 32,
-	 .n_bits = 1,
-	 .insnbits = pcl733_insn_bits,
-	 .range = &range_digital,
-	 },
-	{
-	 .name = "pcl734",
-	 .iosize = 4,
-	 .type = COMEDI_SUBD_DO,
-	 .n_chan = 32,
-	 .n_bits = 1,
-	 .insnbits = pcl734_insn_bits,
-	 .range = &range_digital,
-	 },
-};
 
-#define n_boards ARRAY_SIZE(boards)
 #define this_board ((const struct boarddef_struct *)dev->board_ptr)
 
-static struct comedi_driver driver_poc = {
-	.driver_name = "poc",
-	.module = THIS_MODULE,
-	.attach = poc_attach,
-	.detach = poc_detach,
-	.board_name = &boards[0].name,
-	.num_names = n_boards,
-	.offset = sizeof(boards[0]),
-};
-
-static int poc_attach(struct comedi_device *dev, struct comedi_devconfig *it)
-{
-	struct comedi_subdevice *s;
-	unsigned long iobase;
-	unsigned int iosize;
-
-	iobase = it->options[0];
-	printk(KERN_INFO "comedi%d: poc: using %s iobase 0x%lx\n", dev->minor,
-	       this_board->name, iobase);
-
-	dev->board_name = this_board->name;
-
-	if (iobase == 0) {
-		printk(KERN_ERR "io base address required\n");
-		return -EINVAL;
-	}
-
-	iosize = this_board->iosize;
-	/* check if io addresses are available */
-	if (!request_region(iobase, iosize, "dac02")) {
-		printk(KERN_ERR "I/O port conflict: failed to allocate ports "
-			"0x%lx to 0x%lx\n", iobase, iobase + iosize - 1);
-		return -EIO;
-	}
-	dev->iobase = iobase;
-
-	if (alloc_subdevices(dev, 1) < 0)
-		return -ENOMEM;
-	if (alloc_private(dev, sizeof(unsigned int) * this_board->n_chan) < 0)
-		return -ENOMEM;
-
-	/* analog output subdevice */
-	s = dev->subdevices + 0;
-	s->type = this_board->type;
-	s->n_chan = this_board->n_chan;
-	s->maxdata = (1 << this_board->n_bits) - 1;
-	s->range_table = this_board->range;
-	s->insn_write = this_board->winsn;
-	s->insn_read = this_board->rinsn;
-	s->insn_bits = this_board->insnbits;
-	if (s->type == COMEDI_SUBD_AO || s->type == COMEDI_SUBD_DO)
-		s->subdev_flags = SDF_WRITABLE;
-
-	return 0;
-}
-
-static int poc_detach(struct comedi_device *dev)
-{
-	/* only free stuff if it has been allocated by _attach */
-	if (dev->iobase)
-		release_region(dev->iobase, this_board->iosize);
-
-	printk(KERN_INFO "comedi%d: dac02: remove\n", dev->minor);
-
-	return 0;
-}
-
 static int readback_insn(struct comedi_device *dev, struct comedi_subdevice *s,
 			 struct comedi_insn *insn, unsigned int *data)
 {
@@ -248,18 +135,98 @@ static int pcl734_insn_bits(struct comedi_device *dev,
 	return 2;
 }
 
-static int __init driver_poc_init_module(void)
+static int poc_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 {
-	return comedi_driver_register(&driver_poc);
+	struct comedi_subdevice *s;
+	unsigned long iobase;
+	unsigned int iosize;
+
+	iobase = it->options[0];
+	printk(KERN_INFO "comedi%d: poc: using %s iobase 0x%lx\n", dev->minor,
+	       this_board->name, iobase);
+
+	dev->board_name = this_board->name;
+
+	if (iobase == 0) {
+		printk(KERN_ERR "io base address required\n");
+		return -EINVAL;
+	}
+
+	iosize = this_board->iosize;
+	/* check if io addresses are available */
+	if (!request_region(iobase, iosize, "dac02")) {
+		printk(KERN_ERR "I/O port conflict: failed to allocate ports "
+			"0x%lx to 0x%lx\n", iobase, iobase + iosize - 1);
+		return -EIO;
+	}
+	dev->iobase = iobase;
+
+	if (alloc_subdevices(dev, 1) < 0)
+		return -ENOMEM;
+	if (alloc_private(dev, sizeof(unsigned int) * this_board->n_chan) < 0)
+		return -ENOMEM;
+
+	/* analog output subdevice */
+	s = dev->subdevices + 0;
+	s->type = this_board->type;
+	s->n_chan = this_board->n_chan;
+	s->maxdata = (1 << this_board->n_bits) - 1;
+	s->range_table = this_board->range;
+	s->insn_write = this_board->winsn;
+	s->insn_read = this_board->rinsn;
+	s->insn_bits = this_board->insnbits;
+	if (s->type == COMEDI_SUBD_AO || s->type == COMEDI_SUBD_DO)
+		s->subdev_flags = SDF_WRITABLE;
+
+	return 0;
 }
 
-static void __exit driver_poc_cleanup_module(void)
+static void poc_detach(struct comedi_device *dev)
 {
-	comedi_driver_unregister(&driver_poc);
+	if (dev->iobase)
+		release_region(dev->iobase, this_board->iosize);
 }
 
-module_init(driver_poc_init_module);
-module_exit(driver_poc_cleanup_module);
+static const struct boarddef_struct boards[] = {
+	{
+		.name		= "dac02",
+		.iosize		= 8,
+		/* .setup	= dac02_setup, */
+		.type		= COMEDI_SUBD_AO,
+		.n_chan		= 2,
+		.n_bits		= 12,
+		.winsn		= dac02_ao_winsn,
+		.rinsn		= readback_insn,
+		.range		= &range_unknown,
+	}, {
+		.name		= "pcl733",
+		.iosize		= 4,
+		.type		= COMEDI_SUBD_DI,
+		.n_chan		= 32,
+		.n_bits		= 1,
+		.insnbits	= pcl733_insn_bits,
+		.range		= &range_digital,
+	}, {
+		.name		= "pcl734",
+		.iosize		= 4,
+		.type		= COMEDI_SUBD_DO,
+		.n_chan		= 32,
+		.n_bits		= 1,
+		.insnbits	= pcl734_insn_bits,
+		.range		= &range_digital,
+	},
+};
+
+static struct comedi_driver poc_driver = {
+	.driver_name	= "poc",
+	.module		= THIS_MODULE,
+	.attach		= poc_attach,
+	.detach		= poc_detach,
+	.board_name	= &boards[0].name,
+	.num_names	= ARRAY_SIZE(boards),
+	.offset		= sizeof(boards[0]),
+};
+module_comedi_driver(poc_driver);
 
 MODULE_AUTHOR("Comedi http://www.comedi.org");
 MODULE_DESCRIPTION("Comedi low-level driver");
diff --git a/drivers/staging/comedi/drivers/quatech_daqp_cs.c b/drivers/staging/comedi/drivers/quatech_daqp_cs.c
index e0bb73445dd..2f130b3095e 100644
--- a/drivers/staging/comedi/drivers/quatech_daqp_cs.c
+++ b/drivers/staging/comedi/drivers/quatech_daqp_cs.c
@@ -181,7 +181,7 @@ static const struct comedi_lrange range_daqp_ao = { 1, {BIP_RANGE(5)} };
 /* comedi interface code */
 
 static int daqp_attach(struct comedi_device *dev, struct comedi_devconfig *it);
-static int daqp_detach(struct comedi_device *dev);
+static void daqp_detach(struct comedi_device *dev);
 static struct comedi_driver driver_daqp = {
 	.driver_name = "quatech_daqp_cs",
 	.module = THIS_MODULE,
@@ -922,15 +922,9 @@ static int daqp_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 	return 1;
 }
 
-/* daqp_detach (called from comedi_comdig) does nothing. If the PCMCIA
- * card is removed, daqp_cs_detach() is called by the pcmcia subsystem.
- */
-
-static int daqp_detach(struct comedi_device *dev)
+static void daqp_detach(struct comedi_device *dev)
 {
-	printk(KERN_INFO "comedi%d: detaching daqp\n", dev->minor);
-
-	return 0;
+	/* Nothing to cleanup */
 }
 
 /*====================================================================
@@ -1010,8 +1004,6 @@ static void daqp_cs_detach(struct pcmcia_device *link)
 {
 	struct local_info_t *dev = link->priv;
 
-	dev_dbg(&link->dev, "daqp_cs_detach\n");
-
 	dev->stop = 1;
 	daqp_cs_release(link);
 
@@ -1019,7 +1011,7 @@ static void daqp_cs_detach(struct pcmcia_device *link)
 	dev_table[dev->table_index] = NULL;
 	kfree(dev);
 
-}				/* daqp_cs_detach */
+}
 
 static int daqp_pcmcia_config_loop(struct pcmcia_device *p_dev, void *priv_data)
 {
diff --git a/drivers/staging/comedi/drivers/rtd520.c b/drivers/staging/comedi/drivers/rtd520.c
index 13844196050..1678a0ccb8c 100644
--- a/drivers/staging/comedi/drivers/rtd520.c
+++ b/drivers/staging/comedi/drivers/rtd520.c
@@ -328,14 +328,6 @@ static const struct rtdBoard rtd520Boards[] = {
 	 },
 };
 
-static DEFINE_PCI_DEVICE_TABLE(rtd520_pci_table) = {
-	{ PCI_DEVICE(PCI_VENDOR_ID_RTD, 0x7520) },
-	{ PCI_DEVICE(PCI_VENDOR_ID_RTD, 0x4520) },
-	{ 0 }
-};
-
-MODULE_DEVICE_TABLE(pci, rtd520_pci_table);
-
 /*
  * Useful for shorthand access to the particular board structure
  */
@@ -347,13 +339,13 @@ MODULE_DEVICE_TABLE(pci, rtd520_pci_table);
 */
 struct rtdPrivate {
 	/* memory mapped board structures */
-	void *las0;
-	void *las1;
-	void *lcfg;
+	void __iomem *las0;
+	void __iomem *las1;
+	void __iomem *lcfg;
 
 	unsigned long intCount;	/* interrupt count */
 	long aiCount;		/* total transfer size (samples) */
-	int transCount;		/* # to tranfer data. 0->1/2FIFO */
+	int transCount;		/* # to transfer data. 0->1/2FIFO */
 	int flags;		/* flag event modes */
 
 	/* PCI device info */
@@ -377,8 +369,11 @@ struct rtdPrivate {
 	u8 utcCtrl[4];		/* crtl mode for 3 utc + read back */
 	u8 dioStatus;		/* could be read back (dio0Ctrl) */
 #ifdef USE_DMA
-	/* Always DMA 1/2 FIFO.  Buffer (dmaBuff?) is (at least) twice that size.
-	   After transferring, interrupt processes 1/2 FIFO and passes to comedi */
+	/*
+	 * Always DMA 1/2 FIFO.  Buffer (dmaBuff?) is (at least) twice that
+	 * size.  After transferring, interrupt processes 1/2 FIFO and
+	 * passes to comedi
+	 */
 	s16 dma0Offset;		/* current processing offset (0, 1/2) */
 	uint16_t *dma0Buff[DMA_CHAIN_COUNT];	/* DMA buffers (for ADC) */
 	dma_addr_t dma0BuffPhysAddr[DMA_CHAIN_COUNT];	/* physical addresses */
@@ -581,7 +576,8 @@ struct rtdPrivate {
 
 /* User output N source select (write only) */
 #define RtdUsrOutSource(dev, n, v) \
-	writel(v, devpriv->las0+((n <= 0) ? LAS0_UOUT0_SELECT : LAS0_UOUT1_SELECT))
+	writel(v, devpriv->las0+((n <= 0) ? LAS0_UOUT0_SELECT : \
+				LAS0_UOUT1_SELECT))
 
 /* Digital IO */
 #define RtdDio0Read(dev) \
@@ -608,7 +604,8 @@ struct rtdPrivate {
 /* Write one data value (sign + 12bit + marker bits) */
 /* Note: matches what DMA would put.  Actual value << 3 */
 #define RtdDacFifoPut(dev, n, v) \
-	writew((v), devpriv->las1 + (((n) == 0) ? LAS1_DAC1_FIFO : LAS1_DAC2_FIFO))
+	writew((v), devpriv->las1 + (((n) == 0) ? LAS1_DAC1_FIFO : \
+				LAS1_DAC2_FIFO))
 
 /* Start single DAC conversion */
 #define RtdDacUpdate(dev, n) \
@@ -625,7 +622,8 @@ struct rtdPrivate {
 
 /* Reset DAC FIFO */
 #define RtdDacClearFifo(dev, n) \
-	writel(0, devpriv->las0+(((n) == 0) ? LAS0_DAC1_RESET : LAS0_DAC2_RESET))
+	writel(0, devpriv->las0+(((n) == 0) ? LAS0_DAC1_RESET : \
+				LAS0_DAC2_RESET))
 
 /* Set source for DMA 0 (write only, shadow?) */
 #define RtdDma0Source(dev, n) \
@@ -705,22 +703,6 @@ struct rtdPrivate {
 #define RtdDma1Status(dev) \
 	readb(devpriv->lcfg+LCFG_DMACSR1)
 
-/*
- * The struct comedi_driver structure tells the Comedi core module
- * which functions to call to configure/deconfigure (attac/detach)
- * the board, and also about the kernel module that contains
- * the device code.
- */
-static int rtd_attach(struct comedi_device *dev, struct comedi_devconfig *it);
-static int rtd_detach(struct comedi_device *dev);
-
-static struct comedi_driver rtd520Driver = {
-	.driver_name = DRV_NAME,
-	.module = THIS_MODULE,
-	.attach = rtd_attach,
-	.detach = rtd_detach,
-};
-
 static int rtd_ai_rinsn(struct comedi_device *dev, struct comedi_subdevice *s,
 			struct comedi_insn *insn, unsigned int *data);
 static int rtd_ao_winsn(struct comedi_device *dev, struct comedi_subdevice *s,
@@ -737,7 +719,10 @@ static int rtd_ai_cmdtest(struct comedi_device *dev, struct comedi_subdevice *s,
 			  struct comedi_cmd *cmd);
 static int rtd_ai_cmd(struct comedi_device *dev, struct comedi_subdevice *s);
 static int rtd_ai_cancel(struct comedi_device *dev, struct comedi_subdevice *s);
-/* static int rtd_ai_poll (struct comedi_device *dev,struct comedi_subdevice *s); */
+/*
+ * static int rtd_ai_poll(struct comedi_device *dev,
+ *			  struct comedi_subdevice *s);
+ */
 static int rtd_ns_to_timer(unsigned int *ns, int roundMode);
 static irqreturn_t rtd_interrupt(int irq, void *d);
 static int rtd520_probe_fifo_depth(struct comedi_device *dev);
@@ -857,7 +842,9 @@ static int rtd_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 			DPRINTK("rtd520: PCI latency = %d\n", pci_latency);
 		}
 
-		/* Undocumented EPLD version (doesn't match RTD driver results) */
+		/*
+		 * Undocumented EPLD version (doesn't match RTD driver results)
+		 */
 		/*DPRINTK ("rtd520: Reading epld from %p\n",
 		   devpriv->las0+0);
 		   epld_version = readl (devpriv->las0+0);
@@ -970,9 +957,11 @@ static int rtd_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 #ifdef USE_DMA
 	if (dev->irq > 0) {
 		printk("( DMA buff=%d )\n", DMA_CHAIN_COUNT);
-		/* The PLX9080 has 2 DMA controllers, but there could be 4 sources:
-		   ADC, digital, DAC1, and DAC2.  Since only the ADC supports cmd mode
-		   right now, this isn't an issue (yet) */
+		/*
+		 * The PLX9080 has 2 DMA controllers, but there could be
+		 * 4 sources: ADC, digital, DAC1, and DAC2.  Since only the
+		 * ADC supports cmd mode right now, this isn't an issue (yet)
+		 */
 		devpriv->dma0Offset = 0;
 
 		for (index = 0; index < DMA_CHAIN_COUNT; index++) {
@@ -988,10 +977,14 @@ static int rtd_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 			}
 			/*DPRINTK ("buff[%d] @ %p virtual, %x PCI\n",
 			   index,
-			   devpriv->dma0Buff[index], devpriv->dma0BuffPhysAddr[index]); */
+			   devpriv->dma0Buff[index],
+			   devpriv->dma0BuffPhysAddr[index]); */
 		}
 
-		/* setup DMA descriptor ring (use cpu_to_le32 for byte ordering?) */
+		/*
+		 * setup DMA descriptor ring (use cpu_to_le32 for byte
+		 * ordering?)
+		 */
 		devpriv->dma0Chain =
 		    pci_alloc_consistent(devpriv->pci_dev,
 					 sizeof(struct plx_dma_desc) *
@@ -1088,30 +1081,12 @@ static int rtd_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 #endif
 }
 
-/*
- * _detach is called to deconfigure a device.  It should deallocate
- * resources.
- * This function is also called when _attach() fails, so it should be
- * careful not to release resources that were not necessarily
- * allocated by _attach().  dev->private and dev->subdevices are
- * deallocated automatically by the core.
- */
-static int rtd_detach(struct comedi_device *dev)
+static void rtd_detach(struct comedi_device *dev)
 {
 #ifdef USE_DMA
 	int index;
 #endif
 
-	DPRINTK("comedi%d: rtd520: removing (%ld ints)\n",
-		dev->minor, (devpriv ? devpriv->intCount : 0L));
-	if (devpriv && devpriv->lcfg) {
-		DPRINTK
-		    ("(int status 0x%x, overrun status 0x%x, fifo status 0x%x)...\n",
-		     0xffff & RtdInterruptStatus(dev),
-		     0xffff & RtdInterruptOverrunStatus(dev),
-		     (0xffff & RtdFifoStatus(dev)) ^ 0x6666);
-	}
-
 	if (devpriv) {
 		/* Shut down any board ops by resetting it */
 #ifdef USE_DMA
@@ -1148,37 +1123,24 @@ static int rtd_detach(struct comedi_device *dev)
 			devpriv->dma0Chain = NULL;
 		}
 #endif /* USE_DMA */
-
-		/* release IRQ */
 		if (dev->irq) {
-			/* disable interrupt controller */
 			RtdPlxInterruptWrite(dev, RtdPlxInterruptRead(dev)
 					     & ~(ICS_PLIE | ICS_DMA0_E |
 						 ICS_DMA1_E));
 			free_irq(dev->irq, dev);
 		}
-
-		/* release all regions that were allocated */
 		if (devpriv->las0)
 			iounmap(devpriv->las0);
-
 		if (devpriv->las1)
 			iounmap(devpriv->las1);
-
 		if (devpriv->lcfg)
 			iounmap(devpriv->lcfg);
-
 		if (devpriv->pci_dev) {
 			if (devpriv->got_regions)
 				comedi_pci_disable(devpriv->pci_dev);
-
 			pci_dev_put(devpriv->pci_dev);
 		}
 	}
-
-	printk(KERN_INFO "comedi%d: rtd520: removed.\n", dev->minor);
-
-	return 0;
 }
 
 /*
@@ -1278,7 +1240,8 @@ static int rtd520_probe_fifo_depth(struct comedi_device *dev)
 		}
 	}
 	if (i == limit) {
-		printk(KERN_INFO "\ncomedi: %s: failed to probe fifo size.\n", DRV_NAME);
+		printk(KERN_INFO "\ncomedi: %s: failed to probe fifo size.\n",
+		       DRV_NAME);
 		return -EIO;
 	}
 	RtdAdcClearFifo(dev);
@@ -1378,9 +1341,10 @@ static int ai_read_n(struct comedi_device *dev, struct comedi_subdevice *s,
 		d = RtdAdcFifoGet(dev);	/* get 2s comp value */
 
 		d = d >> 3;	/* low 3 bits are marker lines */
-		if (CHAN_ARRAY_TEST(devpriv->chanBipolar, s->async->cur_chan))
-			sample = d + 2048;	/* convert to comedi unsigned data */
-		else
+		if (CHAN_ARRAY_TEST(devpriv->chanBipolar, s->async->cur_chan)) {
+			/* convert to comedi unsigned data */
+			sample = d + 2048;
+		} else
 			sample = d;
 
 		if (!comedi_buf_put(s->async, sample))
@@ -1406,9 +1370,10 @@ static int ai_read_dregs(struct comedi_device *dev, struct comedi_subdevice *s)
 		}
 
 		d = d >> 3;	/* low 3 bits are marker lines */
-		if (CHAN_ARRAY_TEST(devpriv->chanBipolar, s->async->cur_chan))
-			sample = d + 2048;	/* convert to comedi unsigned data */
-		else
+		if (CHAN_ARRAY_TEST(devpriv->chanBipolar, s->async->cur_chan)) {
+			/* convert to comedi unsigned data */
+			sample = d + 2048;
+		} else
 			sample = d;
 
 		if (!comedi_buf_put(s->async, sample))
@@ -1525,7 +1490,9 @@ static int ai_process_dma(struct comedi_device *dev, struct comedi_subdevice *s)
 	comedi_buf_memcpy_to(s->async, 0, dp, n);
 	comedi_buf_write_free(s->async, n);
 
-	/* always at least 1 scan -- 1/2 FIFO is larger than our max scan list */
+	/*
+	 * always at least 1 scan -- 1/2 FIFO is larger than our max scan list
+	 */
 	s->async->events |= COMEDI_CB_BLOCK | COMEDI_CB_EOS;
 
 	if (++devpriv->dma0Offset >= DMA_CHAIN_COUNT) {	/* next buffer */
@@ -1989,7 +1956,7 @@ static int rtd_ai_cmd(struct comedi_device *dev, struct comedi_subdevice *s)
 			    (TRANS_TARGET_PERIOD * cmd->chanlist_len) /
 			    cmd->scan_begin_arg;
 			if (devpriv->transCount < cmd->chanlist_len) {
-				/* tranfer after each scan (and avoid 0) */
+				/* transfer after each scan (and avoid 0) */
 				devpriv->transCount = cmd->chanlist_len;
 			} else {	/* make a multiple of scan length */
 				devpriv->transCount =
@@ -2005,12 +1972,12 @@ static int rtd_ai_cmd(struct comedi_device *dev, struct comedi_subdevice *s)
 			devpriv->transCount = 0;
 			devpriv->flags &= ~SEND_EOS;
 		} else {
-			/* interrupt for each tranfer */
+			/* interrupt for each transfer */
 			RtdAboutCounter(dev, devpriv->transCount - 1);
 		}
 
 		DPRINTK
-		    ("rtd520: scanLen=%d tranferCount=%d fifoLen=%d\n  scanTime(ns)=%d flags=0x%x\n",
+		    ("rtd520: scanLen=%d transferCount=%d fifoLen=%d\n  scanTime(ns)=%d flags=0x%x\n",
 		     cmd->chanlist_len, devpriv->transCount, devpriv->fifoLen,
 		     cmd->scan_begin_arg, devpriv->flags);
 	} else {		/* unknown timing, just use 1/2 FIFO */
@@ -2348,47 +2315,38 @@ static int rtd_dio_insn_config(struct comedi_device *dev,
 	return 1;
 }
 
-/*
- * A convenient macro that defines init_module() and cleanup_module(),
- * as necessary.
- */
-static int __devinit rtd520Driver_pci_probe(struct pci_dev *dev,
-					    const struct pci_device_id *ent)
+static struct comedi_driver rtd520_driver = {
+	.driver_name	= "rtd520",
+	.module		= THIS_MODULE,
+	.attach		= rtd_attach,
+	.detach		= rtd_detach,
+};
+
+static int __devinit rtd520_pci_probe(struct pci_dev *dev,
+				      const struct pci_device_id *ent)
 {
-	return comedi_pci_auto_config(dev, rtd520Driver.driver_name);
+	return comedi_pci_auto_config(dev, &rtd520_driver);
 }
 
-static void __devexit rtd520Driver_pci_remove(struct pci_dev *dev)
+static void __devexit rtd520_pci_remove(struct pci_dev *dev)
 {
 	comedi_pci_auto_unconfig(dev);
 }
 
-static struct pci_driver rtd520Driver_pci_driver = {
-	.id_table = rtd520_pci_table,
-	.probe = &rtd520Driver_pci_probe,
-	.remove = __devexit_p(&rtd520Driver_pci_remove)
+static DEFINE_PCI_DEVICE_TABLE(rtd520_pci_table) = {
+	{ PCI_DEVICE(PCI_VENDOR_ID_RTD, 0x7520) },
+	{ PCI_DEVICE(PCI_VENDOR_ID_RTD, 0x4520) },
+	{ 0 }
 };
+MODULE_DEVICE_TABLE(pci, rtd520_pci_table);
 
-static int __init rtd520Driver_init_module(void)
-{
-	int retval;
-
-	retval = comedi_driver_register(&rtd520Driver);
-	if (retval < 0)
-		return retval;
-
-	rtd520Driver_pci_driver.name = (char *)rtd520Driver.driver_name;
-	return pci_register_driver(&rtd520Driver_pci_driver);
-}
-
-static void __exit rtd520Driver_cleanup_module(void)
-{
-	pci_unregister_driver(&rtd520Driver_pci_driver);
-	comedi_driver_unregister(&rtd520Driver);
-}
-
-module_init(rtd520Driver_init_module);
-module_exit(rtd520Driver_cleanup_module);
+static struct pci_driver rtd520_pci_driver = {
+	.name		= "rtd520",
+	.id_table	= rtd520_pci_table,
+	.probe		= rtd520_pci_probe,
+	.remove		= __devexit_p(rtd520_pci_remove),
+};
+module_comedi_pci_driver(rtd520_driver, rtd520_pci_driver);
 
 MODULE_AUTHOR("Comedi http://www.comedi.org");
 MODULE_DESCRIPTION("Comedi low-level driver");
diff --git a/drivers/staging/comedi/drivers/rti800.c b/drivers/staging/comedi/drivers/rti800.c
index 72042b81831..f0eb52a7788 100644
--- a/drivers/staging/comedi/drivers/rti800.c
+++ b/drivers/staging/comedi/drivers/rti800.c
@@ -138,39 +138,8 @@ struct rti800_board {
 	int has_ao;
 };
 
-static const struct rti800_board boardtypes[] = {
-	{"rti800", 0},
-	{"rti815", 1},
-};
-
 #define this_board ((const struct rti800_board *)dev->board_ptr)
 
-static int rti800_attach(struct comedi_device *dev,
-			 struct comedi_devconfig *it);
-static int rti800_detach(struct comedi_device *dev);
-static struct comedi_driver driver_rti800 = {
-	.driver_name = "rti800",
-	.module = THIS_MODULE,
-	.attach = rti800_attach,
-	.detach = rti800_detach,
-	.num_names = ARRAY_SIZE(boardtypes),
-	.board_name = &boardtypes[0].name,
-	.offset = sizeof(struct rti800_board),
-};
-
-static int __init driver_rti800_init_module(void)
-{
-	return comedi_driver_register(&driver_rti800);
-}
-
-static void __exit driver_rti800_cleanup_module(void)
-{
-	comedi_driver_unregister(&driver_rti800);
-}
-
-module_init(driver_rti800_init_module);
-module_exit(driver_rti800_cleanup_module);
-
 static irqreturn_t rti800_interrupt(int irq, void *dev);
 
 struct rti800_private {
@@ -474,19 +443,30 @@ static int rti800_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 	return 0;
 }
 
-static int rti800_detach(struct comedi_device *dev)
+static void rti800_detach(struct comedi_device *dev)
 {
-	printk(KERN_INFO "comedi%d: rti800: remove\n", dev->minor);
-
 	if (dev->iobase)
 		release_region(dev->iobase, RTI800_SIZE);
-
 	if (dev->irq)
 		free_irq(dev->irq, dev);
-
-	return 0;
 }
 
+static const struct rti800_board boardtypes[] = {
+	{ "rti800", 0 },
+	{ "rti815", 1 },
+};
+
+static struct comedi_driver rti800_driver = {
+	.driver_name	= "rti800",
+	.module		= THIS_MODULE,
+	.attach		= rti800_attach,
+	.detach		= rti800_detach,
+	.num_names	= ARRAY_SIZE(boardtypes),
+	.board_name	= &boardtypes[0].name,
+	.offset		= sizeof(struct rti800_board),
+};
+module_comedi_driver(rti800_driver);
+
 MODULE_AUTHOR("Comedi http://www.comedi.org");
 MODULE_DESCRIPTION("Comedi low-level driver");
 MODULE_LICENSE("GPL");
diff --git a/drivers/staging/comedi/drivers/rti802.c b/drivers/staging/comedi/drivers/rti802.c
index f59cb11590f..09da5c21858 100644
--- a/drivers/staging/comedi/drivers/rti802.c
+++ b/drivers/staging/comedi/drivers/rti802.c
@@ -47,29 +47,6 @@ Configuration Options:
 #define RTI802_DATALOW 1
 #define RTI802_DATAHIGH 2
 
-static int rti802_attach(struct comedi_device *dev,
-			 struct comedi_devconfig *it);
-static int rti802_detach(struct comedi_device *dev);
-static struct comedi_driver driver_rti802 = {
-	.driver_name = "rti802",
-	.module = THIS_MODULE,
-	.attach = rti802_attach,
-	.detach = rti802_detach,
-};
-
-static int __init driver_rti802_init_module(void)
-{
-	return comedi_driver_register(&driver_rti802);
-}
-
-static void __exit driver_rti802_cleanup_module(void)
-{
-	comedi_driver_unregister(&driver_rti802);
-}
-
-module_init(driver_rti802_init_module);
-module_exit(driver_rti802_cleanup_module);
-
 struct rti802_private {
 	enum {
 		dac_2comp, dac_straight
@@ -152,16 +129,20 @@ static int rti802_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 	return 0;
 }
 
-static int rti802_detach(struct comedi_device *dev)
+static void rti802_detach(struct comedi_device *dev)
 {
-	printk(KERN_INFO "comedi%d: rti802: remove\n", dev->minor);
-
 	if (dev->iobase)
 		release_region(dev->iobase, RTI802_SIZE);
-
-	return 0;
 }
 
+static struct comedi_driver rti802_driver = {
+	.driver_name	= "rti802",
+	.module		= THIS_MODULE,
+	.attach		= rti802_attach,
+	.detach		= rti802_detach,
+};
+module_comedi_driver(rti802_driver);
+
 MODULE_AUTHOR("Comedi http://www.comedi.org");
 MODULE_DESCRIPTION("Comedi low-level driver");
 MODULE_LICENSE("GPL");
diff --git a/drivers/staging/comedi/drivers/s526.c b/drivers/staging/comedi/drivers/s526.c
index 2b34daedc3d..7a56434eb22 100644
--- a/drivers/staging/comedi/drivers/s526.c
+++ b/drivers/staging/comedi/drivers/s526.c
@@ -230,287 +230,6 @@ struct s526_private {
  */
 #define devpriv ((struct s526_private *)dev->private)
 
-/*
- * The struct comedi_driver structure tells the Comedi core module
- * which functions to call to configure/deconfigure (attach/detach)
- * the board, and also about the kernel module that contains
- * the device code.
- */
-static int s526_attach(struct comedi_device *dev, struct comedi_devconfig *it);
-static int s526_detach(struct comedi_device *dev);
-static struct comedi_driver driver_s526 = {
-	.driver_name = "s526",
-	.module = THIS_MODULE,
-	.attach = s526_attach,
-	.detach = s526_detach,
-/* It is not necessary to implement the following members if you are
- * writing a driver for a ISA PnP or PCI card */
-	/* Most drivers will support multiple types of boards by
-	 * having an array of board structures.  These were defined
-	 * in s526_boards[] above.  Note that the element 'name'
-	 * was first in the structure -- Comedi uses this fact to
-	 * extract the name of the board without knowing any details
-	 * about the structure except for its length.
-	 * When a device is attached (by comedi_config), the name
-	 * of the device is given to Comedi, and Comedi tries to
-	 * match it by going through the list of board names.  If
-	 * there is a match, the address of the pointer is put
-	 * into dev->board_ptr and driver->attach() is called.
-	 *
-	 * Note that these are not necessary if you can determine
-	 * the type of board in software.  ISA PnP, PCI, and PCMCIA
-	 * devices are such boards.
-	 */
-	.board_name = &s526_boards[0].name,
-	.offset = sizeof(struct s526_board),
-	.num_names = ARRAY_SIZE(s526_boards),
-};
-
-static int s526_gpct_rinsn(struct comedi_device *dev,
-			   struct comedi_subdevice *s, struct comedi_insn *insn,
-			   unsigned int *data);
-static int s526_gpct_insn_config(struct comedi_device *dev,
-				 struct comedi_subdevice *s,
-				 struct comedi_insn *insn, unsigned int *data);
-static int s526_gpct_winsn(struct comedi_device *dev,
-			   struct comedi_subdevice *s, struct comedi_insn *insn,
-			   unsigned int *data);
-static int s526_ai_insn_config(struct comedi_device *dev,
-			       struct comedi_subdevice *s,
-			       struct comedi_insn *insn, unsigned int *data);
-static int s526_ai_rinsn(struct comedi_device *dev, struct comedi_subdevice *s,
-			 struct comedi_insn *insn, unsigned int *data);
-static int s526_ao_winsn(struct comedi_device *dev, struct comedi_subdevice *s,
-			 struct comedi_insn *insn, unsigned int *data);
-static int s526_ao_rinsn(struct comedi_device *dev, struct comedi_subdevice *s,
-			 struct comedi_insn *insn, unsigned int *data);
-static int s526_dio_insn_bits(struct comedi_device *dev,
-			      struct comedi_subdevice *s,
-			      struct comedi_insn *insn, unsigned int *data);
-static int s526_dio_insn_config(struct comedi_device *dev,
-				struct comedi_subdevice *s,
-				struct comedi_insn *insn, unsigned int *data);
-
-/*
- * Attach is called by the Comedi core to configure the driver
- * for a particular board.  If you specified a board_name array
- * in the driver structure, dev->board_ptr contains that
- * address.
- */
-static int s526_attach(struct comedi_device *dev, struct comedi_devconfig *it)
-{
-	struct comedi_subdevice *s;
-	int iobase;
-	int i, n;
-/* short value; */
-/* int subdev_channel = 0; */
-	union cmReg cmReg;
-
-	printk(KERN_INFO "comedi%d: s526: ", dev->minor);
-
-	iobase = it->options[0];
-	if (!iobase || !request_region(iobase, S526_IOSIZE, thisboard->name)) {
-		comedi_error(dev, "I/O port conflict");
-		return -EIO;
-	}
-	dev->iobase = iobase;
-
-	printk("iobase=0x%lx\n", dev->iobase);
-
-	/*** make it a little quieter, exw, 8/29/06
-	for (i = 0; i < S526_NUM_PORTS; i++) {
-		printk("0x%02x: 0x%04x\n", ADDR_REG(s526_ports[i]),
-				inw(ADDR_REG(s526_ports[i])));
-	}
-	***/
-
-/*
- * Initialize dev->board_name.  Note that we can use the "thisboard"
- * macro now, since we just initialized it in the last line.
- */
-	dev->board_ptr = &s526_boards[0];
-
-	dev->board_name = thisboard->name;
-
-/*
- * Allocate the private structure area.  alloc_private() is a
- * convenient macro defined in comedidev.h.
- */
-	if (alloc_private(dev, sizeof(struct s526_private)) < 0)
-		return -ENOMEM;
-
-/*
- * Allocate the subdevice structures.  alloc_subdevice() is a
- * convenient macro defined in comedidev.h.
- */
-	dev->n_subdevices = 4;
-	if (alloc_subdevices(dev, dev->n_subdevices) < 0)
-		return -ENOMEM;
-
-	s = dev->subdevices + 0;
-	/* GENERAL-PURPOSE COUNTER/TIME (GPCT) */
-	s->type = COMEDI_SUBD_COUNTER;
-	s->subdev_flags = SDF_READABLE | SDF_WRITABLE | SDF_LSAMPL;
-	/* KG: What does SDF_LSAMPL (see multiq3.c) mean? */
-	s->n_chan = thisboard->gpct_chans;
-	s->maxdata = 0x00ffffff;	/* 24 bit counter */
-	s->insn_read = s526_gpct_rinsn;
-	s->insn_config = s526_gpct_insn_config;
-	s->insn_write = s526_gpct_winsn;
-
-	/* Command are not implemented yet, however they are necessary to
-	   allocate the necessary memory for the comedi_async struct (used
-	   to trigger the GPCT in case of pulsegenerator function */
-	/* s->do_cmd = s526_gpct_cmd; */
-	/* s->do_cmdtest = s526_gpct_cmdtest; */
-	/* s->cancel = s526_gpct_cancel; */
-
-	s = dev->subdevices + 1;
-	/* dev->read_subdev=s; */
-	/* analog input subdevice */
-	s->type = COMEDI_SUBD_AI;
-	/* we support differential */
-	s->subdev_flags = SDF_READABLE | SDF_DIFF;
-	/* channels 0 to 7 are the regular differential inputs */
-	/* channel 8 is "reference 0" (+10V), channel 9 is "reference 1" (0V) */
-	s->n_chan = 10;
-	s->maxdata = 0xffff;
-	s->range_table = &range_bipolar10;
-	s->len_chanlist = 16;	/* This is the maximum chanlist length that
-				   the board can handle */
-	s->insn_read = s526_ai_rinsn;
-	s->insn_config = s526_ai_insn_config;
-
-	s = dev->subdevices + 2;
-	/* analog output subdevice */
-	s->type = COMEDI_SUBD_AO;
-	s->subdev_flags = SDF_WRITABLE;
-	s->n_chan = 4;
-	s->maxdata = 0xffff;
-	s->range_table = &range_bipolar10;
-	s->insn_write = s526_ao_winsn;
-	s->insn_read = s526_ao_rinsn;
-
-	s = dev->subdevices + 3;
-	/* digital i/o subdevice */
-	if (thisboard->have_dio) {
-		s->type = COMEDI_SUBD_DIO;
-		s->subdev_flags = SDF_READABLE | SDF_WRITABLE;
-		s->n_chan = 8;
-		s->maxdata = 1;
-		s->range_table = &range_digital;
-		s->insn_bits = s526_dio_insn_bits;
-		s->insn_config = s526_dio_insn_config;
-	} else {
-		s->type = COMEDI_SUBD_UNUSED;
-	}
-
-	printk(KERN_INFO "attached\n");
-
-	return 1;
-
-#if 0
-	/*  Example of Counter Application */
-	/* One-shot (software trigger) */
-	cmReg.reg.coutSource = 0;	/*  out RCAP */
-	cmReg.reg.coutPolarity = 1;	/*  Polarity inverted */
-	cmReg.reg.autoLoadResetRcap = 1;/*  Auto load 0:disabled, 1:enabled */
-	cmReg.reg.hwCtEnableSource = 3;	/*  NOT RCAP */
-	cmReg.reg.ctEnableCtrl = 2;	/*  Hardware */
-	cmReg.reg.clockSource = 2;	/*  Internal */
-	cmReg.reg.countDir = 1;	/*  Down */
-	cmReg.reg.countDirCtrl = 1;	/*  Software */
-	cmReg.reg.outputRegLatchCtrl = 0;	/*  latch on read */
-	cmReg.reg.preloadRegSel = 0;	/*  PR0 */
-	cmReg.reg.reserved = 0;
-
-	outw(cmReg.value, ADDR_CHAN_REG(REG_C0M, subdev_channel));
-
-	outw(0x0001, ADDR_CHAN_REG(REG_C0H, subdev_channel));
-	outw(0x3C68, ADDR_CHAN_REG(REG_C0L, subdev_channel));
-
-	/*  Reset the counter */
-	outw(0x8000, ADDR_CHAN_REG(REG_C0C, subdev_channel));
-	/*  Load the counter from PR0 */
-	outw(0x4000, ADDR_CHAN_REG(REG_C0C, subdev_channel));
-	/*  Reset RCAP (fires one-shot) */
-	outw(0x0008, ADDR_CHAN_REG(REG_C0C, subdev_channel));
-
-#else
-
-	/*  Set Counter Mode Register */
-	cmReg.reg.coutSource = 0;	/*  out RCAP */
-	cmReg.reg.coutPolarity = 0;	/*  Polarity inverted */
-	cmReg.reg.autoLoadResetRcap = 0;	/*  Auto load disabled */
-	cmReg.reg.hwCtEnableSource = 2;	/*  NOT RCAP */
-	cmReg.reg.ctEnableCtrl = 1;	/*  1: Software,  >1 : Hardware */
-	cmReg.reg.clockSource = 3;	/*  x4 */
-	cmReg.reg.countDir = 0;	/*  up */
-	cmReg.reg.countDirCtrl = 0;	/*  quadrature */
-	cmReg.reg.outputRegLatchCtrl = 0;	/*  latch on read */
-	cmReg.reg.preloadRegSel = 0;	/*  PR0 */
-	cmReg.reg.reserved = 0;
-
-	n = 0;
-	printk(KERN_INFO "Mode reg=0x%04x, 0x%04lx\n",
-		cmReg.value, ADDR_CHAN_REG(REG_C0M, n));
-	outw(cmReg.value, ADDR_CHAN_REG(REG_C0M, n));
-	udelay(1000);
-	printk(KERN_INFO "Read back mode reg=0x%04x\n",
-		inw(ADDR_CHAN_REG(REG_C0M, n)));
-
-	/*  Load the pre-load register high word */
-/* value = (short) (0x55); */
-/* outw(value, ADDR_CHAN_REG(REG_C0H, n)); */
-
-	/*  Load the pre-load register low word */
-/* value = (short)(0xaa55); */
-/* outw(value, ADDR_CHAN_REG(REG_C0L, n)); */
-
-	/*  Write the Counter Control Register */
-/* outw(value, ADDR_CHAN_REG(REG_C0C, 0)); */
-
-	/*  Reset the counter if it is software preload */
-	if (cmReg.reg.autoLoadResetRcap == 0) {
-		/*  Reset the counter */
-		outw(0x8000, ADDR_CHAN_REG(REG_C0C, n));
-		/*  Load the counter from PR0 */
-		outw(0x4000, ADDR_CHAN_REG(REG_C0C, n));
-	}
-
-	outw(cmReg.value, ADDR_CHAN_REG(REG_C0M, n));
-	udelay(1000);
-	printk(KERN_INFO "Read back mode reg=0x%04x\n",
-			inw(ADDR_CHAN_REG(REG_C0M, n)));
-
-#endif
-	printk(KERN_INFO "Current registres:\n");
-
-	for (i = 0; i < S526_NUM_PORTS; i++) {
-		printk(KERN_INFO "0x%02lx: 0x%04x\n",
-			ADDR_REG(s526_ports[i]), inw(ADDR_REG(s526_ports[i])));
-	}
-	return 1;
-}
-
-/*
- * _detach is called to deconfigure a device.  It should deallocate
- * resources.
- * This function is also called when _attach() fails, so it should be
- * careful not to release resources that were not necessarily
- * allocated by _attach().  dev->private and dev->subdevices are
- * deallocated automatically by the core.
- */
-static int s526_detach(struct comedi_device *dev)
-{
-	printk(KERN_INFO "comedi%d: s526: remove\n", dev->minor);
-
-	if (dev->iobase > 0)
-		release_region(dev->iobase, S526_IOSIZE);
-
-	return 0;
-}
-
 static int s526_gpct_rinsn(struct comedi_device *dev,
 			   struct comedi_subdevice *s, struct comedi_insn *insn,
 			   unsigned int *data)
@@ -1023,22 +742,218 @@ static int s526_dio_insn_config(struct comedi_device *dev,
 	return 1;
 }
 
+static int s526_attach(struct comedi_device *dev, struct comedi_devconfig *it)
+{
+	struct comedi_subdevice *s;
+	int iobase;
+	int i, n;
+/* short value; */
+/* int subdev_channel = 0; */
+	union cmReg cmReg;
+
+	printk(KERN_INFO "comedi%d: s526: ", dev->minor);
+
+	iobase = it->options[0];
+	if (!iobase || !request_region(iobase, S526_IOSIZE, thisboard->name)) {
+		comedi_error(dev, "I/O port conflict");
+		return -EIO;
+	}
+	dev->iobase = iobase;
+
+	printk("iobase=0x%lx\n", dev->iobase);
+
+	/*** make it a little quieter, exw, 8/29/06
+	for (i = 0; i < S526_NUM_PORTS; i++) {
+		printk("0x%02x: 0x%04x\n", ADDR_REG(s526_ports[i]),
+				inw(ADDR_REG(s526_ports[i])));
+	}
+	***/
+
 /*
- * A convenient macro that defines init_module() and cleanup_module(),
- * as necessary.
+ * Initialize dev->board_name.  Note that we can use the "thisboard"
+ * macro now, since we just initialized it in the last line.
  */
-static int __init driver_s526_init_module(void)
-{
-	return comedi_driver_register(&driver_s526);
+	dev->board_ptr = &s526_boards[0];
+
+	dev->board_name = thisboard->name;
+
+/*
+ * Allocate the private structure area.  alloc_private() is a
+ * convenient macro defined in comedidev.h.
+ */
+	if (alloc_private(dev, sizeof(struct s526_private)) < 0)
+		return -ENOMEM;
+
+/*
+ * Allocate the subdevice structures.  alloc_subdevice() is a
+ * convenient macro defined in comedidev.h.
+ */
+	dev->n_subdevices = 4;
+	if (alloc_subdevices(dev, dev->n_subdevices) < 0)
+		return -ENOMEM;
+
+	s = dev->subdevices + 0;
+	/* GENERAL-PURPOSE COUNTER/TIME (GPCT) */
+	s->type = COMEDI_SUBD_COUNTER;
+	s->subdev_flags = SDF_READABLE | SDF_WRITABLE | SDF_LSAMPL;
+	/* KG: What does SDF_LSAMPL (see multiq3.c) mean? */
+	s->n_chan = thisboard->gpct_chans;
+	s->maxdata = 0x00ffffff;	/* 24 bit counter */
+	s->insn_read = s526_gpct_rinsn;
+	s->insn_config = s526_gpct_insn_config;
+	s->insn_write = s526_gpct_winsn;
+
+	/* Command are not implemented yet, however they are necessary to
+	   allocate the necessary memory for the comedi_async struct (used
+	   to trigger the GPCT in case of pulsegenerator function */
+	/* s->do_cmd = s526_gpct_cmd; */
+	/* s->do_cmdtest = s526_gpct_cmdtest; */
+	/* s->cancel = s526_gpct_cancel; */
+
+	s = dev->subdevices + 1;
+	/* dev->read_subdev=s; */
+	/* analog input subdevice */
+	s->type = COMEDI_SUBD_AI;
+	/* we support differential */
+	s->subdev_flags = SDF_READABLE | SDF_DIFF;
+	/* channels 0 to 7 are the regular differential inputs */
+	/* channel 8 is "reference 0" (+10V), channel 9 is "reference 1" (0V) */
+	s->n_chan = 10;
+	s->maxdata = 0xffff;
+	s->range_table = &range_bipolar10;
+	s->len_chanlist = 16;	/* This is the maximum chanlist length that
+				   the board can handle */
+	s->insn_read = s526_ai_rinsn;
+	s->insn_config = s526_ai_insn_config;
+
+	s = dev->subdevices + 2;
+	/* analog output subdevice */
+	s->type = COMEDI_SUBD_AO;
+	s->subdev_flags = SDF_WRITABLE;
+	s->n_chan = 4;
+	s->maxdata = 0xffff;
+	s->range_table = &range_bipolar10;
+	s->insn_write = s526_ao_winsn;
+	s->insn_read = s526_ao_rinsn;
+
+	s = dev->subdevices + 3;
+	/* digital i/o subdevice */
+	if (thisboard->have_dio) {
+		s->type = COMEDI_SUBD_DIO;
+		s->subdev_flags = SDF_READABLE | SDF_WRITABLE;
+		s->n_chan = 8;
+		s->maxdata = 1;
+		s->range_table = &range_digital;
+		s->insn_bits = s526_dio_insn_bits;
+		s->insn_config = s526_dio_insn_config;
+	} else {
+		s->type = COMEDI_SUBD_UNUSED;
+	}
+
+	printk(KERN_INFO "attached\n");
+
+	return 1;
+
+#if 0
+	/*  Example of Counter Application */
+	/* One-shot (software trigger) */
+	cmReg.reg.coutSource = 0;	/*  out RCAP */
+	cmReg.reg.coutPolarity = 1;	/*  Polarity inverted */
+	cmReg.reg.autoLoadResetRcap = 1;/*  Auto load 0:disabled, 1:enabled */
+	cmReg.reg.hwCtEnableSource = 3;	/*  NOT RCAP */
+	cmReg.reg.ctEnableCtrl = 2;	/*  Hardware */
+	cmReg.reg.clockSource = 2;	/*  Internal */
+	cmReg.reg.countDir = 1;	/*  Down */
+	cmReg.reg.countDirCtrl = 1;	/*  Software */
+	cmReg.reg.outputRegLatchCtrl = 0;	/*  latch on read */
+	cmReg.reg.preloadRegSel = 0;	/*  PR0 */
+	cmReg.reg.reserved = 0;
+
+	outw(cmReg.value, ADDR_CHAN_REG(REG_C0M, subdev_channel));
+
+	outw(0x0001, ADDR_CHAN_REG(REG_C0H, subdev_channel));
+	outw(0x3C68, ADDR_CHAN_REG(REG_C0L, subdev_channel));
+
+	/*  Reset the counter */
+	outw(0x8000, ADDR_CHAN_REG(REG_C0C, subdev_channel));
+	/*  Load the counter from PR0 */
+	outw(0x4000, ADDR_CHAN_REG(REG_C0C, subdev_channel));
+	/*  Reset RCAP (fires one-shot) */
+	outw(0x0008, ADDR_CHAN_REG(REG_C0C, subdev_channel));
+
+#else
+
+	/*  Set Counter Mode Register */
+	cmReg.reg.coutSource = 0;	/*  out RCAP */
+	cmReg.reg.coutPolarity = 0;	/*  Polarity inverted */
+	cmReg.reg.autoLoadResetRcap = 0;	/*  Auto load disabled */
+	cmReg.reg.hwCtEnableSource = 2;	/*  NOT RCAP */
+	cmReg.reg.ctEnableCtrl = 1;	/*  1: Software,  >1 : Hardware */
+	cmReg.reg.clockSource = 3;	/*  x4 */
+	cmReg.reg.countDir = 0;	/*  up */
+	cmReg.reg.countDirCtrl = 0;	/*  quadrature */
+	cmReg.reg.outputRegLatchCtrl = 0;	/*  latch on read */
+	cmReg.reg.preloadRegSel = 0;	/*  PR0 */
+	cmReg.reg.reserved = 0;
+
+	n = 0;
+	printk(KERN_INFO "Mode reg=0x%04x, 0x%04lx\n",
+		cmReg.value, ADDR_CHAN_REG(REG_C0M, n));
+	outw(cmReg.value, ADDR_CHAN_REG(REG_C0M, n));
+	udelay(1000);
+	printk(KERN_INFO "Read back mode reg=0x%04x\n",
+		inw(ADDR_CHAN_REG(REG_C0M, n)));
+
+	/*  Load the pre-load register high word */
+/* value = (short) (0x55); */
+/* outw(value, ADDR_CHAN_REG(REG_C0H, n)); */
+
+	/*  Load the pre-load register low word */
+/* value = (short)(0xaa55); */
+/* outw(value, ADDR_CHAN_REG(REG_C0L, n)); */
+
+	/*  Write the Counter Control Register */
+/* outw(value, ADDR_CHAN_REG(REG_C0C, 0)); */
+
+	/*  Reset the counter if it is software preload */
+	if (cmReg.reg.autoLoadResetRcap == 0) {
+		/*  Reset the counter */
+		outw(0x8000, ADDR_CHAN_REG(REG_C0C, n));
+		/*  Load the counter from PR0 */
+		outw(0x4000, ADDR_CHAN_REG(REG_C0C, n));
+	}
+
+	outw(cmReg.value, ADDR_CHAN_REG(REG_C0M, n));
+	udelay(1000);
+	printk(KERN_INFO "Read back mode reg=0x%04x\n",
+			inw(ADDR_CHAN_REG(REG_C0M, n)));
+
+#endif
+	printk(KERN_INFO "Current registres:\n");
+
+	for (i = 0; i < S526_NUM_PORTS; i++) {
+		printk(KERN_INFO "0x%02lx: 0x%04x\n",
+			ADDR_REG(s526_ports[i]), inw(ADDR_REG(s526_ports[i])));
+	}
+	return 1;
 }
 
-static void __exit driver_s526_cleanup_module(void)
+static void s526_detach(struct comedi_device *dev)
 {
-	comedi_driver_unregister(&driver_s526);
+	if (dev->iobase > 0)
+		release_region(dev->iobase, S526_IOSIZE);
 }
 
-module_init(driver_s526_init_module);
-module_exit(driver_s526_cleanup_module);
+static struct comedi_driver s526_driver = {
+	.driver_name	= "s526",
+	.module		= THIS_MODULE,
+	.attach		= s526_attach,
+	.detach		= s526_detach,
+	.board_name	= &s526_boards[0].name,
+	.offset		= sizeof(struct s526_board),
+	.num_names	= ARRAY_SIZE(s526_boards),
+};
+module_comedi_driver(s526_driver);
 
 MODULE_AUTHOR("Comedi http://www.comedi.org");
 MODULE_DESCRIPTION("Comedi low-level driver");
diff --git a/drivers/staging/comedi/drivers/s626.c b/drivers/staging/comedi/drivers/s626.c
index 23fc64b9988..7beb8f6853a 100644
--- a/drivers/staging/comedi/drivers/s626.c
+++ b/drivers/staging/comedi/drivers/s626.c
@@ -79,12 +79,17 @@ INSN_CONFIG instructions:
 #include "comedi_fc.h"
 #include "s626.h"
 
-MODULE_AUTHOR("Gianluca Palli <gpalli@deis.unibo.it>");
-MODULE_DESCRIPTION("Sensoray 626 Comedi driver module");
-MODULE_LICENSE("GPL");
+#define PCI_VENDOR_ID_S626 0x1131
+#define PCI_DEVICE_ID_S626 0x7146
+#define PCI_SUBVENDOR_ID_S626 0x6000
+#define PCI_SUBDEVICE_ID_S626 0x0272
 
 struct s626_board {
 	const char *name;
+	int vendor_id;
+	int device_id;
+	int subvendor_id;
+	int subdevice_id;
 	int ai_chans;
 	int ai_bits;
 	int ao_chans;
@@ -97,6 +102,10 @@ struct s626_board {
 static const struct s626_board s626_boards[] = {
 	{
 	 .name = "s626",
+	 .vendor_id = PCI_VENDOR_ID_S626,
+	 .device_id = PCI_DEVICE_ID_S626,
+	 .subvendor_id = PCI_SUBVENDOR_ID_S626,
+	 .subdevice_id = PCI_SUBDEVICE_ID_S626,
 	 .ai_chans = S626_ADC_CHANNELS,
 	 .ai_bits = 14,
 	 .ao_chans = S626_DAC_CHANNELS,
@@ -108,30 +117,6 @@ static const struct s626_board s626_boards[] = {
 };
 
 #define thisboard ((const struct s626_board *)dev->board_ptr)
-#define PCI_VENDOR_ID_S626 0x1131
-#define PCI_DEVICE_ID_S626 0x7146
-
-/*
- * For devices with vendor:device id == 0x1131:0x7146 you must specify
- * also subvendor:subdevice ids, because otherwise it will conflict with
- * Philips SAA7146 media/dvb based cards.
- */
-static DEFINE_PCI_DEVICE_TABLE(s626_pci_table) = {
-	{PCI_VENDOR_ID_S626, PCI_DEVICE_ID_S626, 0x6000, 0x0272, 0, 0, 0},
-	{0}
-};
-
-MODULE_DEVICE_TABLE(pci, s626_pci_table);
-
-static int s626_attach(struct comedi_device *dev, struct comedi_devconfig *it);
-static int s626_detach(struct comedi_device *dev);
-
-static struct comedi_driver driver_s626 = {
-	.driver_name = "s626",
-	.module = THIS_MODULE,
-	.attach = s626_attach,
-	.detach = s626_detach,
-};
 
 struct s626_private {
 	struct pci_dev *pdev;
@@ -224,44 +209,6 @@ static struct dio_private *dio_private_word[]={
 #define devpriv ((struct s626_private *)dev->private)
 #define diopriv ((struct dio_private *)s->private)
 
-static int __devinit driver_s626_pci_probe(struct pci_dev *dev,
-					   const struct pci_device_id *ent)
-{
-	return comedi_pci_auto_config(dev, driver_s626.driver_name);
-}
-
-static void __devexit driver_s626_pci_remove(struct pci_dev *dev)
-{
-	comedi_pci_auto_unconfig(dev);
-}
-
-static struct pci_driver driver_s626_pci_driver = {
-	.id_table = s626_pci_table,
-	.probe = &driver_s626_pci_probe,
-	.remove = __devexit_p(&driver_s626_pci_remove)
-};
-
-static int __init driver_s626_init_module(void)
-{
-	int retval;
-
-	retval = comedi_driver_register(&driver_s626);
-	if (retval < 0)
-		return retval;
-
-	driver_s626_pci_driver.name = (char *)driver_s626.driver_name;
-	return pci_register_driver(&driver_s626_pci_driver);
-}
-
-static void __exit driver_s626_cleanup_module(void)
-{
-	pci_unregister_driver(&driver_s626_pci_driver);
-	comedi_driver_unregister(&driver_s626);
-}
-
-module_init(driver_s626_init_module);
-module_exit(driver_s626_cleanup_module);
-
 /* ioctl routines */
 static int s626_ai_insn_config(struct comedi_device *dev,
 			       struct comedi_subdevice *s,
@@ -554,17 +501,17 @@ static int s626_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 	resource_size_t resourceStart;
 	dma_addr_t appdma;
 	struct comedi_subdevice *s;
-	const struct pci_device_id *ids;
 	struct pci_dev *pdev = NULL;
 
 	if (alloc_private(dev, sizeof(struct s626_private)) < 0)
 		return -ENOMEM;
 
-	for (i = 0; i < (ARRAY_SIZE(s626_pci_table) - 1) && !pdev; i++) {
-		ids = &s626_pci_table[i];
+	for (i = 0; i < ARRAY_SIZE(s626_boards) && !pdev; i++) {
 		do {
-			pdev = pci_get_subsys(ids->vendor, ids->device,
-					      ids->subvendor, ids->subdevice,
+			pdev = pci_get_subsys(s626_boards[i].vendor_id,
+					      s626_boards[i].device_id,
+					      s626_boards[i].subvendor_id,
+					      s626_boards[i].subdevice_id,
 					      pdev);
 
 			if ((it->options[0] || it->options[1]) && pdev) {
@@ -1365,7 +1312,7 @@ static irqreturn_t s626_irq_handler(int irq, void *d)
 	return IRQ_HANDLED;
 }
 
-static int s626_detach(struct comedi_device *dev)
+static void s626_detach(struct comedi_device *dev)
 {
 	if (devpriv) {
 		/* stop ai_command */
@@ -1389,20 +1336,14 @@ static int s626_detach(struct comedi_device *dev)
 
 		if (dev->irq)
 			free_irq(dev->irq, dev);
-
 		if (devpriv->base_addr)
 			iounmap(devpriv->base_addr);
-
 		if (devpriv->pdev) {
 			if (devpriv->got_regions)
 				comedi_pci_disable(devpriv->pdev);
 			pci_dev_put(devpriv->pdev);
 		}
 	}
-
-	DEBUG("s626_detach: S626 detached!\n");
-
-	return 0;
 }
 
 /*
@@ -3367,3 +3308,45 @@ static void CountersInit(struct comedi_device *dev)
 	DEBUG("CountersInit: counters initialized\n");
 
 }
+
+static struct comedi_driver s626_driver = {
+	.driver_name	= "s626",
+	.module		= THIS_MODULE,
+	.attach		= s626_attach,
+	.detach		= s626_detach,
+};
+
+static int __devinit s626_pci_probe(struct pci_dev *dev,
+				    const struct pci_device_id *ent)
+{
+	return comedi_pci_auto_config(dev, &s626_driver);
+}
+
+static void __devexit s626_pci_remove(struct pci_dev *dev)
+{
+	comedi_pci_auto_unconfig(dev);
+}
+
+/*
+ * For devices with vendor:device id == 0x1131:0x7146 you must specify
+ * also subvendor:subdevice ids, because otherwise it will conflict with
+ * Philips SAA7146 media/dvb based cards.
+ */
+static DEFINE_PCI_DEVICE_TABLE(s626_pci_table) = {
+	{ PCI_VENDOR_ID_S626, PCI_DEVICE_ID_S626,
+		PCI_SUBVENDOR_ID_S626, PCI_SUBDEVICE_ID_S626, 0, 0, 0 },
+	{ 0 }
+};
+MODULE_DEVICE_TABLE(pci, s626_pci_table);
+
+static struct pci_driver s626_pci_driver = {
+	.name		= "s626",
+	.id_table	= s626_pci_table,
+	.probe		= s626_pci_probe,
+	.remove		= __devexit_p(s626_pci_remove),
+};
+module_comedi_pci_driver(s626_driver, s626_pci_driver);
+
+MODULE_AUTHOR("Gianluca Palli <gpalli@deis.unibo.it>");
+MODULE_DESCRIPTION("Sensoray 626 Comedi driver module");
+MODULE_LICENSE("GPL");
diff --git a/drivers/staging/comedi/drivers/serial2002.c b/drivers/staging/comedi/drivers/serial2002.c
index d880c2f6fbc..6342bc5ddb3 100644
--- a/drivers/staging/comedi/drivers/serial2002.c
+++ b/drivers/staging/comedi/drivers/serial2002.c
@@ -43,20 +43,10 @@ Status: in development
 #include <linux/serial.h>
 #include <linux/poll.h>
 
-/*
- * Board descriptions for two imaginary boards.  Describing the
- * boards in this way is optional, and completely driver-dependent.
- * Some drivers use arrays such as this, other do not.
- */
 struct serial2002_board {
 	const char *name;
 };
 
-static const struct serial2002_board serial2002_boards[] = {
-	{
-	 .name = "serial2002"}
-};
-
 /*
  * Useful for shorthand access to the particular board structure
  */
@@ -89,35 +79,6 @@ struct serial2002_private {
  */
 #define devpriv ((struct serial2002_private *)dev->private)
 
-static int serial2002_attach(struct comedi_device *dev,
-			     struct comedi_devconfig *it);
-static int serial2002_detach(struct comedi_device *dev);
-struct comedi_driver driver_serial2002 = {
-	.driver_name = "serial2002",
-	.module = THIS_MODULE,
-	.attach = serial2002_attach,
-	.detach = serial2002_detach,
-	.board_name = &serial2002_boards[0].name,
-	.offset = sizeof(struct serial2002_board),
-	.num_names = ARRAY_SIZE(serial2002_boards),
-};
-
-static int serial2002_di_rinsn(struct comedi_device *dev,
-			       struct comedi_subdevice *s,
-			       struct comedi_insn *insn, unsigned int *data);
-static int serial2002_do_winsn(struct comedi_device *dev,
-			       struct comedi_subdevice *s,
-			       struct comedi_insn *insn, unsigned int *data);
-static int serial2002_ai_rinsn(struct comedi_device *dev,
-			       struct comedi_subdevice *s,
-			       struct comedi_insn *insn, unsigned int *data);
-static int serial2002_ao_winsn(struct comedi_device *dev,
-			       struct comedi_subdevice *s,
-			       struct comedi_insn *insn, unsigned int *data);
-static int serial2002_ao_rinsn(struct comedi_device *dev,
-			       struct comedi_subdevice *s,
-			       struct comedi_insn *insn, unsigned int *data);
-
 struct serial_data {
 	enum { is_invalid, is_digital, is_channel } kind;
 	int index;
@@ -887,32 +848,34 @@ static int serial2002_attach(struct comedi_device *dev,
 	return 1;
 }
 
-static int serial2002_detach(struct comedi_device *dev)
+static void serial2002_detach(struct comedi_device *dev)
 {
 	struct comedi_subdevice *s;
 	int i;
 
-	dev_dbg(dev->hw_dev, "comedi%d: remove\n", dev->minor);
 	for (i = 0; i < 5; i++) {
 		s = &dev->subdevices[i];
 		kfree(s->maxdata_list);
 		kfree(s->range_table_list);
 	}
-	return 0;
-}
-
-static int __init driver_serial2002_init_module(void)
-{
-	return comedi_driver_register(&driver_serial2002);
 }
 
-static void __exit driver_serial2002_cleanup_module(void)
-{
-	comedi_driver_unregister(&driver_serial2002);
-}
+static const struct serial2002_board serial2002_boards[] = {
+	{
+		.name	= "serial2002"
+	},
+};
 
-module_init(driver_serial2002_init_module);
-module_exit(driver_serial2002_cleanup_module);
+static struct comedi_driver serial2002_driver = {
+	.driver_name	= "serial2002",
+	.module		= THIS_MODULE,
+	.attach		= serial2002_attach,
+	.detach		= serial2002_detach,
+	.board_name	= &serial2002_boards[0].name,
+	.offset		= sizeof(struct serial2002_board),
+	.num_names	= ARRAY_SIZE(serial2002_boards),
+};
+module_comedi_driver(serial2002_driver);
 
 MODULE_AUTHOR("Comedi http://www.comedi.org");
 MODULE_DESCRIPTION("Comedi low-level driver");
diff --git a/drivers/staging/comedi/drivers/skel.c b/drivers/staging/comedi/drivers/skel.c
index ed69008f0d3..7d13ffa7f4f 100644
--- a/drivers/staging/comedi/drivers/skel.c
+++ b/drivers/staging/comedi/drivers/skel.c
@@ -156,7 +156,7 @@ struct skel_private {
  * the device code.
  */
 static int skel_attach(struct comedi_device *dev, struct comedi_devconfig *it);
-static int skel_detach(struct comedi_device *dev);
+static void skel_detach(struct comedi_device *dev);
 static struct comedi_driver driver_skel = {
 	.driver_name = "dummy",
 	.module = THIS_MODULE,
@@ -295,11 +295,8 @@ static int skel_attach(struct comedi_device *dev, struct comedi_devconfig *it)
  * allocated by _attach().  dev->private and dev->subdevices are
  * deallocated automatically by the core.
  */
-static int skel_detach(struct comedi_device *dev)
+static void skel_detach(struct comedi_device *dev)
 {
-	pr_info("comedi%d: skel: remove\n", dev->minor);
-
-	return 0;
 }
 
 /*
@@ -623,7 +620,7 @@ static int skel_dio_insn_config(struct comedi_device *dev,
 static int __devinit driver_skel_pci_probe(struct pci_dev *dev,
 					   const struct pci_device_id *ent)
 {
-	return comedi_pci_auto_config(dev, driver_skel.driver_name);
+	return comedi_pci_auto_config(dev, &driver_skel);
 }
 
 static void __devexit driver_skel_pci_remove(struct pci_dev *dev)
diff --git a/drivers/staging/comedi/drivers/ssv_dnp.c b/drivers/staging/comedi/drivers/ssv_dnp.c
index 526de2efa12..16c4f5a757b 100644
--- a/drivers/staging/comedi/drivers/ssv_dnp.c
+++ b/drivers/staging/comedi/drivers/ssv_dnp.c
@@ -59,16 +59,6 @@ struct dnp_board {
 	int have_dio;
 };
 
-/* We only support one DNP 'board' variant at the moment */
-static const struct dnp_board dnp_boards[] = {
-{
-	 .name = "dnp-1486",
-	 .ai_chans = 16,
-	 .ai_bits = 12,
-	 .have_dio = 1,
-	 },
-};
-
 /* Useful for shorthand access to the particular board structure ----------- */
 #define thisboard ((const struct dnp_board *)dev->board_ptr)
 
@@ -81,136 +71,6 @@ struct dnp_private_data {
 #define devpriv ((dnp_private *)dev->private)
 
 /* ------------------------------------------------------------------------- */
-/* The struct comedi_driver structure tells the Comedi core module which     */
-/* functions to call to configure/deconfigure (attach/detach) the board, and */
-/* also about the kernel module that contains the device code.               */
-/*                                                                           */
-/* In the following section we define the API of this driver.                */
-/* ------------------------------------------------------------------------- */
-
-static int dnp_attach(struct comedi_device *dev, struct comedi_devconfig *it);
-static int dnp_detach(struct comedi_device *dev);
-
-static struct comedi_driver driver_dnp = {
-	.driver_name = "ssv_dnp",
-	.module = THIS_MODULE,
-	.attach = dnp_attach,
-	.detach = dnp_detach,
-	.board_name = &dnp_boards[0].name,
-	/* only necessary for non-PnP devs   */
-	.offset = sizeof(struct dnp_board),   /* like ISA-PnP, PCI or PCMCIA */
-	.num_names = ARRAY_SIZE(dnp_boards),
-};
-
-static int __init driver_dnp_init_module(void)
-{
-	return comedi_driver_register(&driver_dnp);
-}
-
-static void __exit driver_dnp_cleanup_module(void)
-{
-	comedi_driver_unregister(&driver_dnp);
-}
-
-module_init(driver_dnp_init_module);
-module_exit(driver_dnp_cleanup_module);
-
-static int dnp_dio_insn_bits(struct comedi_device *dev,
-			     struct comedi_subdevice *s,
-			     struct comedi_insn *insn, unsigned int *data);
-
-static int dnp_dio_insn_config(struct comedi_device *dev,
-			       struct comedi_subdevice *s,
-			       struct comedi_insn *insn, unsigned int *data);
-
-/* ------------------------------------------------------------------------- */
-/* Attach is called by comedi core to configure the driver for a particular  */
-/* board. If you specified a board_name array in the driver structure,       */
-/* dev->board_ptr contains that address.                                     */
-/* ------------------------------------------------------------------------- */
-
-static int dnp_attach(struct comedi_device *dev, struct comedi_devconfig *it)
-{
-
-	struct comedi_subdevice *s;
-
-	printk(KERN_INFO "comedi%d: dnp: ", dev->minor);
-
-	/* Autoprobing: this should find out which board we have. Currently  */
-	/* only the 1486 board is supported and autoprobing is not           */
-	/* implemented :-)                                                   */
-	/* dev->board_ptr = dnp_probe(dev); */
-
-	/* Initialize the name of the board.                                 */
-	/* We can use the "thisboard" macro now.                             */
-	dev->board_name = thisboard->name;
-
-	/* Allocate the private structure area. alloc_private() is a         */
-	/* convenient macro defined in comedidev.h.                          */
-	if (alloc_private(dev, sizeof(struct dnp_private_data)) < 0)
-		return -ENOMEM;
-
-	/* Allocate the subdevice structures. alloc_subdevice() is a         */
-	/* convenient macro defined in comedidev.h.                          */
-
-	if (alloc_subdevices(dev, 1) < 0)
-		return -ENOMEM;
-
-	s = dev->subdevices + 0;
-	/* digital i/o subdevice                                             */
-	s->type = COMEDI_SUBD_DIO;
-	s->subdev_flags = SDF_READABLE | SDF_WRITABLE;
-	s->n_chan = 20;
-	s->maxdata = 1;
-	s->range_table = &range_digital;
-	s->insn_bits = dnp_dio_insn_bits;
-	s->insn_config = dnp_dio_insn_config;
-
-	printk("attached\n");
-
-	/* We use the I/O ports 0x22,0x23 and 0xa3-0xa9, which are always
-	 * allocated for the primary 8259, so we don't need to allocate them
-	 * ourselves. */
-
-	/* configure all ports as input (default)                            */
-	outb(PAMR, CSCIR);
-	outb(0x00, CSCDR);
-	outb(PBMR, CSCIR);
-	outb(0x00, CSCDR);
-	outb(PCMR, CSCIR);
-	outb((inb(CSCDR) & 0xAA), CSCDR);
-
-	return 1;
-
-}
-
-/* ------------------------------------------------------------------------- */
-/* detach is called to deconfigure a device. It should deallocate the        */
-/* resources. This function is also called when _attach() fails, so it       */
-/* should be careful not to release resources that were not necessarily      */
-/* allocated by _attach(). dev->private and dev->subdevices are              */
-/* deallocated automatically by the core.                                    */
-/* ------------------------------------------------------------------------- */
-
-static int dnp_detach(struct comedi_device *dev)
-{
-
-	/* configure all ports as input (default)                            */
-	outb(PAMR, CSCIR);
-	outb(0x00, CSCDR);
-	outb(PBMR, CSCIR);
-	outb(0x00, CSCDR);
-	outb(PCMR, CSCIR);
-	outb((inb(CSCDR) & 0xAA), CSCDR);
-
-	/* announce that we are finished                                     */
-	printk(KERN_INFO "comedi%d: dnp: remove\n", dev->minor);
-
-	return 0;
-
-}
-
-/* ------------------------------------------------------------------------- */
 /* The insn_bits interface allows packed reading/writing of DIO channels.    */
 /* The comedi core can convert between insn_bits and insn_read/write, so you */
 /* are able to use these instructions as well.                               */
@@ -326,6 +186,89 @@ static int dnp_dio_insn_config(struct comedi_device *dev,
 
 }
 
+static int dnp_attach(struct comedi_device *dev, struct comedi_devconfig *it)
+{
+	struct comedi_subdevice *s;
+
+	printk(KERN_INFO "comedi%d: dnp: ", dev->minor);
+
+	/* Autoprobing: this should find out which board we have. Currently  */
+	/* only the 1486 board is supported and autoprobing is not           */
+	/* implemented :-)                                                   */
+	/* dev->board_ptr = dnp_probe(dev); */
+
+	/* Initialize the name of the board.                                 */
+	/* We can use the "thisboard" macro now.                             */
+	dev->board_name = thisboard->name;
+
+	/* Allocate the private structure area. alloc_private() is a         */
+	/* convenient macro defined in comedidev.h.                          */
+	if (alloc_private(dev, sizeof(struct dnp_private_data)) < 0)
+		return -ENOMEM;
+
+	/* Allocate the subdevice structures. alloc_subdevice() is a         */
+	/* convenient macro defined in comedidev.h.                          */
+
+	if (alloc_subdevices(dev, 1) < 0)
+		return -ENOMEM;
+
+	s = dev->subdevices + 0;
+	/* digital i/o subdevice                                             */
+	s->type = COMEDI_SUBD_DIO;
+	s->subdev_flags = SDF_READABLE | SDF_WRITABLE;
+	s->n_chan = 20;
+	s->maxdata = 1;
+	s->range_table = &range_digital;
+	s->insn_bits = dnp_dio_insn_bits;
+	s->insn_config = dnp_dio_insn_config;
+
+	printk("attached\n");
+
+	/* We use the I/O ports 0x22,0x23 and 0xa3-0xa9, which are always
+	 * allocated for the primary 8259, so we don't need to allocate them
+	 * ourselves. */
+
+	/* configure all ports as input (default)                            */
+	outb(PAMR, CSCIR);
+	outb(0x00, CSCDR);
+	outb(PBMR, CSCIR);
+	outb(0x00, CSCDR);
+	outb(PCMR, CSCIR);
+	outb((inb(CSCDR) & 0xAA), CSCDR);
+
+	return 1;
+}
+
+static void dnp_detach(struct comedi_device *dev)
+{
+	outb(PAMR, CSCIR);
+	outb(0x00, CSCDR);
+	outb(PBMR, CSCIR);
+	outb(0x00, CSCDR);
+	outb(PCMR, CSCIR);
+	outb((inb(CSCDR) & 0xAA), CSCDR);
+}
+
+static const struct dnp_board dnp_boards[] = {
+	{
+		.name		= "dnp-1486",
+		.ai_chans	= 16,
+		.ai_bits	= 12,
+		.have_dio	= 1,
+	},
+};
+
+static struct comedi_driver dnp_driver = {
+	.driver_name	= "ssv_dnp",
+	.module		= THIS_MODULE,
+	.attach		= dnp_attach,
+	.detach		= dnp_detach,
+	.board_name	= &dnp_boards[0].name,
+	.offset		= sizeof(struct dnp_board),
+	.num_names	= ARRAY_SIZE(dnp_boards),
+};
+module_comedi_driver(dnp_driver);
+
 MODULE_AUTHOR("Comedi http://www.comedi.org");
 MODULE_DESCRIPTION("Comedi low-level driver");
 MODULE_LICENSE("GPL");
diff --git a/drivers/staging/comedi/drivers/unioxx5.c b/drivers/staging/comedi/drivers/unioxx5.c
index f45824f0d86..d5f1f22aa70 100644
--- a/drivers/staging/comedi/drivers/unioxx5.c
+++ b/drivers/staging/comedi/drivers/unioxx5.c
@@ -83,96 +83,188 @@ struct unioxx5_subd_priv {
 	unsigned char usp_prev_cn_val[3];	/* previous channel value */
 };
 
-static int unioxx5_attach(struct comedi_device *dev,
-			  struct comedi_devconfig *it);
-static int unioxx5_subdev_write(struct comedi_device *dev,
-				struct comedi_subdevice *subdev,
-				struct comedi_insn *insn, unsigned int *data);
-static int unioxx5_subdev_read(struct comedi_device *dev,
-			       struct comedi_subdevice *subdev,
-			       struct comedi_insn *insn, unsigned int *data);
-static int unioxx5_insn_config(struct comedi_device *dev,
-			       struct comedi_subdevice *subdev,
-			       struct comedi_insn *insn, unsigned int *data);
-static int unioxx5_detach(struct comedi_device *dev);
-static int __unioxx5_subdev_init(struct comedi_subdevice *subdev,
-				 int subdev_iobase, int minor);
-static int __unioxx5_digital_write(struct unioxx5_subd_priv *usp,
-				   unsigned int *data, int channel, int minor);
-static int __unioxx5_digital_read(struct unioxx5_subd_priv *usp,
-				  unsigned int *data, int channel, int minor);
-/* static void __unioxx5_digital_config(struct unioxx5_subd_priv* usp, int mode); */
-static int __unioxx5_analog_write(struct unioxx5_subd_priv *usp,
-				  unsigned int *data, int channel, int minor);
-static int __unioxx5_analog_read(struct unioxx5_subd_priv *usp,
-				 unsigned int *data, int channel, int minor);
-static int __unioxx5_define_chan_offset(int chan_num);
-static void __unioxx5_analog_config(struct unioxx5_subd_priv *usp, int channel);
+static int __unioxx5_define_chan_offset(int chan_num)
+{
 
-static struct comedi_driver unioxx5_driver = {
-	.driver_name = DRIVER_NAME,
-	.module = THIS_MODULE,
-	.attach = unioxx5_attach,
-	.detach = unioxx5_detach
-};
+	if (chan_num < 0 || chan_num > 23)
+		return -1;
 
-static int __init unioxx5_driver_init_module(void)
+	return (chan_num >> 3) + 1;
+}
+
+#if 0				/* not used? */
+static void __unioxx5_digital_config(struct unioxx5_subd_priv *usp, int mode)
 {
-	return comedi_driver_register(&unioxx5_driver);
+	int i, mask;
+
+	mask = (mode == ALL_2_OUTPUT) ? 0xFF : 0x00;
+	printk("COMEDI: mode = %d\n", mask);
+
+	outb(1, usp->usp_iobase + 0);
+
+	for (i = 0; i < 3; i++)
+		outb(mask, usp->usp_iobase + i);
+
+	outb(0, usp->usp_iobase + 0);
 }
+#endif
 
-static void __exit unioxx5_driver_cleanup_module(void)
+/* configure channels for analog i/o (even to output, odd to input) */
+static void __unioxx5_analog_config(struct unioxx5_subd_priv *usp, int channel)
 {
-	comedi_driver_unregister(&unioxx5_driver);
+	int chan_a, chan_b, conf, channel_offset;
+
+	channel_offset = __unioxx5_define_chan_offset(channel);
+	conf = usp->usp_prev_cn_val[channel_offset - 1];
+	chan_a = chan_b = 1;
+
+	/* setting channel A and channel B mask */
+	if (channel % 2 == 0) {
+		chan_a <<= channel & 0x07;
+		chan_b <<= (channel + 1) & 0x07;
+	} else {
+		chan_a <<= (channel - 1) & 0x07;
+		chan_b <<= channel & 0x07;
+	}
+
+	conf |= chan_a;		/* even channel ot output */
+	conf &= ~chan_b;	/* odd channel to input */
+
+	outb(1, usp->usp_iobase + 0);
+	outb(conf, usp->usp_iobase + channel_offset);
+	outb(0, usp->usp_iobase + 0);
+
+	usp->usp_prev_cn_val[channel_offset - 1] = conf;
 }
 
-module_init(unioxx5_driver_init_module);
-module_exit(unioxx5_driver_cleanup_module);
+static int __unioxx5_digital_read(struct unioxx5_subd_priv *usp,
+				  unsigned int *data, int channel, int minor)
+{
+	int channel_offset, mask = 1 << (channel & 0x07);
 
-static int unioxx5_attach(struct comedi_device *dev,
-			  struct comedi_devconfig *it)
+	channel_offset = __unioxx5_define_chan_offset(channel);
+	if (channel_offset < 0) {
+		printk(KERN_ERR
+		       "comedi%d: undefined channel %d. channel range is 0 .. 23\n",
+		       minor, channel);
+		return 0;
+	}
+
+	*data = inb(usp->usp_iobase + channel_offset);
+	*data &= mask;
+
+	/* correct the read value to 0 or 1 */
+	if (channel_offset > 1)
+		channel -= 2 << channel_offset;
+	*data >>= channel;
+	return 1;
+}
+
+static int __unioxx5_analog_read(struct unioxx5_subd_priv *usp,
+				 unsigned int *data, int channel, int minor)
 {
-	int iobase, i, n_subd;
-	int id, num, ba;
+	int module_no, read_ch;
+	char control;
 
-	iobase = it->options[0];
+	module_no = channel / 2;
+	read_ch = channel % 2;	/* depend on type of channel (A or B) */
 
-	dev->board_name = DRIVER_NAME;
-	dev->iobase = iobase;
-	iobase += UNIOXX5_SUBDEV_BASE;
+	/* defining if given module can work on input */
+	if (usp->usp_module_type[module_no] & MODULE_OUTPUT_MASK) {
+		printk(KERN_ERR
+		       "comedi%d: module in position %d with id 0x%02x is for output only",
+		       minor, module_no, usp->usp_module_type[module_no]);
+		return 0;
+	}
 
-	/* defining number of subdevices and getting they types (it must be 'g01')  */
-	for (i = n_subd = 0, ba = iobase; i < 4; i++, ba += UNIOXX5_SUBDEV_ODDS) {
-		id = inb(ba + 0xE);
-		num = inb(ba + 0xF);
+	__unioxx5_analog_config(usp, channel);
+	/* sends module number to card(1 .. 12) */
+	outb(module_no + 1, usp->usp_iobase + 5);
+	outb('V', usp->usp_iobase + 6);	/* sends to module (V)erify command */
+	control = inb(usp->usp_iobase);	/* get control register byte */
 
-		if (id != 'g' || num != 1)
-			continue;
+	/* waits while reading four bytes will be allowed */
+	while (!((control = inb(usp->usp_iobase + 0)) & Rx4CA))
+		;
 
-		n_subd++;
+	/* if four bytes readding error occurs - return 0(false) */
+	if ((control & Rx4CA_ERR_MASK)) {
+		printk("COMEDI: 4 bytes error\n");
+		return 0;
 	}
 
-	/* unioxx5 can has from two to four subdevices */
-	if (n_subd < 2) {
+	if (read_ch)
+		*data = inw(usp->usp_iobase + 6);	/* channel B */
+	else
+		*data = inw(usp->usp_iobase + 4);	/* channel A */
+
+	return 1;
+}
+
+static int __unioxx5_digital_write(struct unioxx5_subd_priv *usp,
+				   unsigned int *data, int channel, int minor)
+{
+	int channel_offset, val;
+	int mask = 1 << (channel & 0x07);
+
+	channel_offset = __unioxx5_define_chan_offset(channel);
+	if (channel_offset < 0) {
 		printk(KERN_ERR
-		       "your card must has at least 2 'g01' subdevices\n");
-		return -1;
+		       "comedi%d: undefined channel %d. channel range is 0 .. 23\n",
+		       minor, channel);
+		return 0;
 	}
 
-	if (alloc_subdevices(dev, n_subd) < 0) {
-		printk(KERN_ERR "out of memory\n");
-		return -ENOMEM;
+	/* getting previous written value */
+	val = usp->usp_prev_wr_val[channel_offset - 1];
+
+	if (*data)
+		val |= mask;
+	else
+		val &= ~mask;
+
+	outb(val, usp->usp_iobase + channel_offset);
+	/* saving new written value */
+	usp->usp_prev_wr_val[channel_offset - 1] = val;
+
+	return 1;
+}
+
+static int __unioxx5_analog_write(struct unioxx5_subd_priv *usp,
+				  unsigned int *data, int channel, int minor)
+{
+	int module, i;
+
+	module = channel / 2;	/* definig module number(0 .. 11) */
+	i = (channel % 2) << 1;	/* depends on type of channel (A or B) */
+
+	/* defining if given module can work on output */
+	if (!(usp->usp_module_type[module] & MODULE_OUTPUT_MASK)) {
+		printk(KERN_ERR
+		       "comedi%d: module in position %d with id 0x%0x is for input only!\n",
+		       minor, module, usp->usp_module_type[module]);
+		return 0;
 	}
 
-	/* initializing each of for same subdevices */
-	for (i = 0; i < n_subd; i++, iobase += UNIOXX5_SUBDEV_ODDS) {
-		if (__unioxx5_subdev_init(&dev->subdevices[i], iobase,
-					  dev->minor) < 0)
-			return -1;
+	__unioxx5_analog_config(usp, channel);
+	/* saving minor byte */
+	usp->usp_extra_data[module][i++] = (unsigned char)(*data & 0x00FF);
+	/* saving major byte */
+	usp->usp_extra_data[module][i] = (unsigned char)((*data & 0xFF00) >> 8);
+
+	/* while(!((inb(usp->usp_iobase + 0)) & TxBE)); */
+	/* sending module number to card(1 .. 12) */
+	outb(module + 1, usp->usp_iobase + 5);
+	outb('W', usp->usp_iobase + 6);	/* sends (W)rite command to module */
+
+	/* sending for bytes to module(one byte per cycle iteration) */
+	for (i = 0; i < 4; i++) {
+		while (!((inb(usp->usp_iobase + 0)) & TxBE))
+			;	/* waits while writting will be allowed */
+		outb(usp->usp_extra_data[module][i], usp->usp_iobase + 6);
 	}
 
-	printk(KERN_INFO "attached\n");
-	return 0;
+	return 1;
 }
 
 static int unioxx5_subdev_read(struct comedi_device *dev,
@@ -275,22 +367,6 @@ static int unioxx5_insn_config(struct comedi_device *dev,
 	return 0;
 }
 
-static int unioxx5_detach(struct comedi_device *dev)
-{
-	int i;
-	struct comedi_subdevice *subdev;
-	struct unioxx5_subd_priv *usp;
-
-	for (i = 0; i < dev->n_subdevices; i++) {
-		subdev = &dev->subdevices[i];
-		usp = subdev->private;
-		release_region(usp->usp_iobase, UNIOXX5_SIZE);
-		kfree(subdev->private);
-	}
-
-	return 0;
-}
-
 /* initializing subdevice with given address */
 static int __unioxx5_subdev_init(struct comedi_subdevice *subdev,
 				 int subdev_iobase, int minor)
@@ -362,196 +438,73 @@ static int __unioxx5_subdev_init(struct comedi_subdevice *subdev,
 	return 0;
 }
 
-static int __unioxx5_digital_write(struct unioxx5_subd_priv *usp,
-				   unsigned int *data, int channel, int minor)
+static int unioxx5_attach(struct comedi_device *dev,
+			  struct comedi_devconfig *it)
 {
-	int channel_offset, val;
-	int mask = 1 << (channel & 0x07);
-
-	channel_offset = __unioxx5_define_chan_offset(channel);
-	if (channel_offset < 0) {
-		printk(KERN_ERR
-		       "comedi%d: undefined channel %d. channel range is 0 .. 23\n",
-		       minor, channel);
-		return 0;
-	}
-
-	/* getting previous written value */
-	val = usp->usp_prev_wr_val[channel_offset - 1];
+	int iobase, i, n_subd;
+	int id, num, ba;
 
-	if (*data)
-		val |= mask;
-	else
-		val &= ~mask;
+	iobase = it->options[0];
 
-	outb(val, usp->usp_iobase + channel_offset);
-	/* saving new written value */
-	usp->usp_prev_wr_val[channel_offset - 1] = val;
+	dev->board_name = DRIVER_NAME;
+	dev->iobase = iobase;
+	iobase += UNIOXX5_SUBDEV_BASE;
 
-	return 1;
-}
+	/* defining number of subdevices and getting they types (it must be 'g01')  */
+	for (i = n_subd = 0, ba = iobase; i < 4; i++, ba += UNIOXX5_SUBDEV_ODDS) {
+		id = inb(ba + 0xE);
+		num = inb(ba + 0xF);
 
-/* function for digital reading */
-static int __unioxx5_digital_read(struct unioxx5_subd_priv *usp,
-				  unsigned int *data, int channel, int minor)
-{
-	int channel_offset, mask = 1 << (channel & 0x07);
+		if (id != 'g' || num != 1)
+			continue;
 
-	channel_offset = __unioxx5_define_chan_offset(channel);
-	if (channel_offset < 0) {
-		printk(KERN_ERR
-		       "comedi%d: undefined channel %d. channel range is 0 .. 23\n",
-		       minor, channel);
-		return 0;
+		n_subd++;
 	}
 
-	*data = inb(usp->usp_iobase + channel_offset);
-	*data &= mask;
-
-	if (channel_offset > 1)
-		channel -= 2 << channel_offset;	/* this operation is created for correct readed value to 0 or 1 */
-	*data >>= channel;
-	return 1;
-}
-
-#if 0				/* not used? */
-static void __unioxx5_digital_config(struct unioxx5_subd_priv *usp, int mode)
-{
-	int i, mask;
-
-	mask = (mode == ALL_2_OUTPUT) ? 0xFF : 0x00;
-	printk("COMEDI: mode = %d\n", mask);
-
-	outb(1, usp->usp_iobase + 0);
-
-	for (i = 0; i < 3; i++)
-		outb(mask, usp->usp_iobase + i);
-
-	outb(0, usp->usp_iobase + 0);
-}
-#endif
-
-static int __unioxx5_analog_write(struct unioxx5_subd_priv *usp,
-				  unsigned int *data, int channel, int minor)
-{
-	int module, i;
-
-	module = channel / 2;	/* definig module number(0 .. 11) */
-	i = (channel % 2) << 1;	/* depends on type of channel (A or B) */
-
-	/* defining if given module can work on output */
-	if (!(usp->usp_module_type[module] & MODULE_OUTPUT_MASK)) {
+	/* unioxx5 can has from two to four subdevices */
+	if (n_subd < 2) {
 		printk(KERN_ERR
-		       "comedi%d: module in position %d with id 0x%0x is for input only!\n",
-		       minor, module, usp->usp_module_type[module]);
-		return 0;
-	}
-
-	__unioxx5_analog_config(usp, channel);
-	/* saving minor byte */
-	usp->usp_extra_data[module][i++] = (unsigned char)(*data & 0x00FF);
-	/* saving major byte */
-	usp->usp_extra_data[module][i] = (unsigned char)((*data & 0xFF00) >> 8);
-
-	/* while(!((inb(usp->usp_iobase + 0)) & TxBE)); */
-	/* sending module number to card(1 .. 12) */
-	outb(module + 1, usp->usp_iobase + 5);
-	outb('W', usp->usp_iobase + 6);	/* sends (W)rite command to module */
-
-	/* sending for bytes to module(one byte per cycle iteration) */
-	for (i = 0; i < 4; i++) {
-		while (!((inb(usp->usp_iobase + 0)) & TxBE))
-			;	/* waits while writting will be allowed */
-		outb(usp->usp_extra_data[module][i], usp->usp_iobase + 6);
+		       "your card must has at least 2 'g01' subdevices\n");
+		return -1;
 	}
 
-	return 1;
-}
-
-static int __unioxx5_analog_read(struct unioxx5_subd_priv *usp,
-				 unsigned int *data, int channel, int minor)
-{
-	int module_no, read_ch;
-	char control;
-
-	module_no = channel / 2;
-	read_ch = channel % 2;	/* depend on type of channel (A or B) */
-
-	/* defining if given module can work on input */
-	if (usp->usp_module_type[module_no] & MODULE_OUTPUT_MASK) {
-		printk(KERN_ERR
-		       "comedi%d: module in position %d with id 0x%02x is for output only",
-		       minor, module_no, usp->usp_module_type[module_no]);
-		return 0;
+	if (alloc_subdevices(dev, n_subd) < 0) {
+		printk(KERN_ERR "out of memory\n");
+		return -ENOMEM;
 	}
 
-	__unioxx5_analog_config(usp, channel);
-	/* sends module number to card(1 .. 12) */
-	outb(module_no + 1, usp->usp_iobase + 5);
-	outb('V', usp->usp_iobase + 6);	/* sends to module (V)erify command */
-	control = inb(usp->usp_iobase);	/* get control register byte */
-
-	/* waits while reading four bytes will be allowed */
-	while (!((control = inb(usp->usp_iobase + 0)) & Rx4CA))
-		;
-
-	/* if four bytes readding error occurs - return 0(false) */
-	if ((control & Rx4CA_ERR_MASK)) {
-		printk("COMEDI: 4 bytes error\n");
-		return 0;
+	/* initializing each of for same subdevices */
+	for (i = 0; i < n_subd; i++, iobase += UNIOXX5_SUBDEV_ODDS) {
+		if (__unioxx5_subdev_init(&dev->subdevices[i], iobase,
+					  dev->minor) < 0)
+			return -1;
 	}
 
-	if (read_ch)
-		*data = inw(usp->usp_iobase + 6);	/* channel B */
-	else
-		*data = inw(usp->usp_iobase + 4);	/* channel A */
-
-	return 1;
+	printk(KERN_INFO "attached\n");
+	return 0;
 }
 
-/* configure channels for analog i/o (even to output, odd to input) */
-static void __unioxx5_analog_config(struct unioxx5_subd_priv *usp, int channel)
+static void unioxx5_detach(struct comedi_device *dev)
 {
-	int chan_a, chan_b, conf, channel_offset;
-
-	channel_offset = __unioxx5_define_chan_offset(channel);
-	conf = usp->usp_prev_cn_val[channel_offset - 1];
-	chan_a = chan_b = 1;
+	int i;
+	struct comedi_subdevice *subdev;
+	struct unioxx5_subd_priv *usp;
 
-	/* setting channel A and channel B mask */
-	if (channel % 2 == 0) {
-		chan_a <<= channel & 0x07;
-		chan_b <<= (channel + 1) & 0x07;
-	} else {
-		chan_a <<= (channel - 1) & 0x07;
-		chan_b <<= channel & 0x07;
+	for (i = 0; i < dev->n_subdevices; i++) {
+		subdev = &dev->subdevices[i];
+		usp = subdev->private;
+		release_region(usp->usp_iobase, UNIOXX5_SIZE);
+		kfree(subdev->private);
 	}
-
-	conf |= chan_a;		/* even channel ot output */
-	conf &= ~chan_b;	/* odd channel to input */
-
-	outb(1, usp->usp_iobase + 0);
-	outb(conf, usp->usp_iobase + channel_offset);
-	outb(0, usp->usp_iobase + 0);
-
-	usp->usp_prev_cn_val[channel_offset - 1] = conf;
 }
 
-/*                                                    *\
- * this function defines if the given channel number  *
- * enters in default numeric interspace(from 0 to 23) *
- * and it returns address offset for usage needed     *
- * channel.                                           *
-\*                                                    */
-
-static int __unioxx5_define_chan_offset(int chan_num)
-{
-
-	if (chan_num < 0 || chan_num > 23)
-		return -1;
-
-	return (chan_num >> 3) + 1;
-}
+static struct comedi_driver unioxx5_driver = {
+	.driver_name	= DRIVER_NAME,
+	.module		= THIS_MODULE,
+	.attach		= unioxx5_attach,
+	.detach		= unioxx5_detach,
+};
+module_comedi_driver(unioxx5_driver);
 
 MODULE_AUTHOR("Comedi http://www.comedi.org");
 MODULE_DESCRIPTION("Comedi low-level driver");
diff --git a/drivers/staging/comedi/drivers/usbdux.c b/drivers/staging/comedi/drivers/usbdux.c
index bf62e0dd6f6..13d9fd3efcf 100644
--- a/drivers/staging/comedi/drivers/usbdux.c
+++ b/drivers/staging/comedi/drivers/usbdux.c
@@ -317,6 +317,8 @@ static struct usbduxsub usbduxsub[NUMUSBDUX];
 
 static DEFINE_SEMAPHORE(start_stop_sem);
 
+static struct comedi_driver driver_usbdux;	/* see below for initializer */
+
 /*
  * Stops the data acquision
  * It should be safe to call this function from any context
@@ -2304,11 +2306,11 @@ static void usbdux_firmware_request_complete_handler(const struct firmware *fw,
 						     void *context)
 {
 	struct usbduxsub *usbduxsub_tmp = context;
-	struct usb_device *usbdev = usbduxsub_tmp->usbdev;
+	struct usb_interface *uinterf = usbduxsub_tmp->interface;
 	int ret;
 
 	if (fw == NULL) {
-		dev_err(&usbdev->dev,
+		dev_err(&uinterf->dev,
 			"Firmware complete handler without firmware!\n");
 		return;
 	}
@@ -2320,11 +2322,11 @@ static void usbdux_firmware_request_complete_handler(const struct firmware *fw,
 	ret = firmwareUpload(usbduxsub_tmp, fw->data, fw->size);
 
 	if (ret) {
-		dev_err(&usbdev->dev,
+		dev_err(&uinterf->dev,
 			"Could not upload firmware (err=%d)\n", ret);
 		goto out;
 	}
-	comedi_usb_auto_config(usbdev, BOARDNAME);
+	comedi_usb_auto_config(uinterf, &driver_usbdux);
  out:
 	release_firmware(fw);
 }
@@ -2606,7 +2608,7 @@ static void usbduxsub_disconnect(struct usb_interface *intf)
 		dev_err(&intf->dev, "comedi_: BUG! called with wrong ptr!!!\n");
 		return;
 	}
-	comedi_usb_auto_unconfig(udev);
+	comedi_usb_auto_unconfig(intf);
 	down(&start_stop_sem);
 	down(&usbduxsub_tmp->sem);
 	tidy_up(usbduxsub_tmp);
@@ -2615,46 +2617,21 @@ static void usbduxsub_disconnect(struct usb_interface *intf)
 	dev_dbg(&intf->dev, "comedi_: disconnected from the usb\n");
 }
 
-/* is called when comedi-config is called */
-static int usbdux_attach(struct comedi_device *dev, struct comedi_devconfig *it)
+/* common part of attach and attach_usb */
+static int usbdux_attach_common(struct comedi_device *dev,
+				struct usbduxsub *udev,
+				void *aux_data, int aux_len)
 {
 	int ret;
-	int index;
-	int i;
-	struct usbduxsub *udev;
-
 	struct comedi_subdevice *s = NULL;
-	dev->private = NULL;
-
-	down(&start_stop_sem);
-	/* find a valid device which has been detected by the probe function of
-	 * the usb */
-	index = -1;
-	for (i = 0; i < NUMUSBDUX; i++) {
-		if ((usbduxsub[i].probed) && (!usbduxsub[i].attached)) {
-			index = i;
-			break;
-		}
-	}
 
-	if (index < 0) {
-		printk(KERN_ERR "comedi%d: usbdux: error: attach failed, no "
-		       "usbdux devs connected to the usb bus.\n", dev->minor);
-		up(&start_stop_sem);
-		return -ENODEV;
-	}
-
-	udev = &usbduxsub[index];
 	down(&udev->sem);
 	/* pointer back to the corresponding comedi device */
 	udev->comedidev = dev;
 
 	/* trying to upload the firmware into the chip */
-	if (comedi_aux_data(it->options, 0) &&
-	    it->options[COMEDI_DEVCONF_AUX_DATA_LENGTH]) {
-		firmwareUpload(udev, comedi_aux_data(it->options, 0),
-			       it->options[COMEDI_DEVCONF_AUX_DATA_LENGTH]);
-	}
+	if (aux_data)
+		firmwareUpload(udev, aux_data, aux_len);
 
 	dev->board_name = BOARDNAME;
 
@@ -2673,13 +2650,9 @@ static int usbdux_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 		dev_err(&udev->interface->dev,
 			"comedi%d: error alloc space for subdev\n", dev->minor);
 		up(&udev->sem);
-		up(&start_stop_sem);
 		return ret;
 	}
 
-	dev_info(&udev->interface->dev,
-		 "comedi%d: usb-device %d is attached to comedi.\n",
-		 dev->minor, index);
 	/* private structure is also simply the usb-structure */
 	dev->private = udev;
 
@@ -2776,44 +2749,91 @@ static int usbdux_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 
 	up(&udev->sem);
 
-	up(&start_stop_sem);
-
 	dev_info(&udev->interface->dev, "comedi%d: attached to usbdux.\n",
 		 dev->minor);
 
 	return 0;
 }
 
-static int usbdux_detach(struct comedi_device *dev)
+/* is called when comedi-config is called */
+static int usbdux_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 {
-	struct usbduxsub *usbduxsub_tmp;
+	int ret;
+	int index;
+	int i;
+	void *aux_data;
+	int aux_len;
 
-	if (!dev) {
-		printk(KERN_ERR
-		       "comedi?: usbdux: detach without dev variable...\n");
-		return -EFAULT;
+	dev->private = NULL;
+
+	aux_data = comedi_aux_data(it->options, 0);
+	aux_len = it->options[COMEDI_DEVCONF_AUX_DATA_LENGTH];
+	if (aux_data == NULL)
+		aux_len = 0;
+	else if (aux_len == 0)
+		aux_data = NULL;
+
+	down(&start_stop_sem);
+	/* find a valid device which has been detected by the probe function of
+	 * the usb */
+	index = -1;
+	for (i = 0; i < NUMUSBDUX; i++) {
+		if ((usbduxsub[i].probed) && (!usbduxsub[i].attached)) {
+			index = i;
+			break;
+		}
 	}
 
-	usbduxsub_tmp = dev->private;
-	if (!usbduxsub_tmp) {
+	if (index < 0) {
 		printk(KERN_ERR
-		       "comedi?: usbdux: detach without ptr to usbduxsub[]\n");
-		return -EFAULT;
-	}
+		       "comedi%d: usbdux: error: attach failed, no usbdux devs connected to the usb bus.\n",
+		       dev->minor);
+		ret = -ENODEV;
+	} else
+		ret = usbdux_attach_common(dev, &usbduxsub[index],
+					   aux_data, aux_len);
+	up(&start_stop_sem);
+	return ret;
+}
 
-	dev_dbg(&usbduxsub_tmp->interface->dev, "comedi%d: detach usb device\n",
-		dev->minor);
+/* is called from comedi_usb_auto_config() */
+static int usbdux_attach_usb(struct comedi_device *dev,
+			     struct usb_interface *uinterf)
+{
+	int ret;
+	struct usbduxsub *this_usbduxsub;
 
-	down(&usbduxsub_tmp->sem);
-	/* Don't allow detach to free the private structure */
-	/* It's one entry of of usbduxsub[] */
 	dev->private = NULL;
-	usbduxsub_tmp->attached = 0;
-	usbduxsub_tmp->comedidev = NULL;
-	dev_dbg(&usbduxsub_tmp->interface->dev,
-		"comedi%d: detach: successfully removed\n", dev->minor);
-	up(&usbduxsub_tmp->sem);
-	return 0;
+
+	down(&start_stop_sem);
+	this_usbduxsub = usb_get_intfdata(uinterf);
+	if (!this_usbduxsub || !this_usbduxsub->probed) {
+		printk(KERN_ERR
+		       "comedi%d: usbdux: error: attach_usb failed, not connected\n",
+		       dev->minor);
+		ret = -ENODEV;
+	} else if (this_usbduxsub->attached) {
+		printk(KERN_ERR
+		       "comedi%d: usbdux: error: attach_usb failed, already attached\n",
+		       dev->minor);
+		ret = -ENODEV;
+	} else
+		ret = usbdux_attach_common(dev, this_usbduxsub, NULL, 0);
+	up(&start_stop_sem);
+	return ret;
+}
+
+static void usbdux_detach(struct comedi_device *dev)
+{
+	struct usbduxsub *usb = dev->private;
+
+	if (usb) {
+		down(&usb->sem);
+		dev->private = NULL;
+		usb->attached = 0;
+		usb->comedidev = NULL;
+		up(&usb->sem);
+	}
 }
 
 /* main driver struct */
@@ -2822,6 +2842,7 @@ static struct comedi_driver driver_usbdux = {
 	.module = THIS_MODULE,
 	.attach = usbdux_attach,
 	.detach = usbdux_detach,
+	.attach_usb = usbdux_attach_usb,
 };
 
 /* Table with the USB-devices: just now only testing IDs */
diff --git a/drivers/staging/comedi/drivers/usbduxfast.c b/drivers/staging/comedi/drivers/usbduxfast.c
index 2a8e725b785..7b1d21a6fc5 100644
--- a/drivers/staging/comedi/drivers/usbduxfast.c
+++ b/drivers/staging/comedi/drivers/usbduxfast.c
@@ -201,6 +201,8 @@ static struct usbduxfastsub_s usbduxfastsub[NUMUSBDUXFAST];
 
 static DEFINE_SEMAPHORE(start_stop_sem);
 
+static struct comedi_driver driver_usbduxfast;	/* see below for initializer */
+
 /*
  * bulk transfers to usbduxfast
  */
@@ -453,14 +455,15 @@ static int usbduxfastsub_start(struct usbduxfastsub_s *udfs)
 	/* 7f92 to zero */
 	local_transfer_buffer[0] = 0;
 	/* bRequest, "Firmware" */
-	ret = usb_control_msg(udfs->usbdev, usb_sndctrlpipe(udfs->usbdev, 0), USBDUXFASTSUB_FIRMWARE,
-				VENDOR_DIR_OUT,	/* bmRequestType */
-				USBDUXFASTSUB_CPUCS,	/* Value */
-				0x0000,	/* Index */
-				/* address of the transfer buffer */
-				local_transfer_buffer,
-				1,	/* Length */
-				EZTIMEOUT);	/* Timeout */
+	ret = usb_control_msg(udfs->usbdev, usb_sndctrlpipe(udfs->usbdev, 0),
+			      USBDUXFASTSUB_FIRMWARE,
+			      VENDOR_DIR_OUT,	  /* bmRequestType */
+			      USBDUXFASTSUB_CPUCS,    /* Value */
+			      0x0000,	/* Index */
+			      /* address of the transfer buffer */
+			      local_transfer_buffer,
+			      1,      /* Length */
+			      EZTIMEOUT);    /* Timeout */
 	if (ret < 0) {
 		printk("comedi_: usbduxfast_: control msg failed (start)\n");
 		return ret;
@@ -477,7 +480,8 @@ static int usbduxfastsub_stop(struct usbduxfastsub_s *udfs)
 	/* 7f92 to one */
 	local_transfer_buffer[0] = 1;
 	/* bRequest, "Firmware" */
-	ret = usb_control_msg(udfs->usbdev, usb_sndctrlpipe(udfs->usbdev, 0), USBDUXFASTSUB_FIRMWARE,
+	ret = usb_control_msg(udfs->usbdev, usb_sndctrlpipe(udfs->usbdev, 0),
+			      USBDUXFASTSUB_FIRMWARE,
 			      VENDOR_DIR_OUT,	/* bmRequestType */
 			      USBDUXFASTSUB_CPUCS,	/* Value */
 			      0x0000,	/* Index */
@@ -504,14 +508,15 @@ static int usbduxfastsub_upload(struct usbduxfastsub_s *udfs,
 	       startAddr, local_transfer_buffer[0]);
 #endif
 	/* brequest, firmware */
-	ret = usb_control_msg(udfs->usbdev, usb_sndctrlpipe(udfs->usbdev, 0), USBDUXFASTSUB_FIRMWARE,
-				VENDOR_DIR_OUT,	/* bmRequestType */
-				startAddr,	/* value */
-				0x0000,	/* index */
-				/* our local safe buffer */
-				local_transfer_buffer,
-				len,	/* length */
-				EZTIMEOUT);	/* timeout */
+	ret = usb_control_msg(udfs->usbdev, usb_sndctrlpipe(udfs->usbdev, 0),
+			      USBDUXFASTSUB_FIRMWARE,
+			      VENDOR_DIR_OUT,	/* bmRequestType */
+			      startAddr,	/* value */
+			      0x0000,	 /* index */
+			      /* our local safe buffer */
+			      local_transfer_buffer,
+			      len,	/* length */
+			      EZTIMEOUT);      /* timeout */
 
 #ifdef CONFIG_COMEDI_DEBUG
 	printk(KERN_DEBUG "comedi_: usbduxfast: result=%d\n", ret);
@@ -1440,7 +1445,7 @@ static void usbduxfast_firmware_request_complete_handler(const struct firmware
 							 *fw, void *context)
 {
 	struct usbduxfastsub_s *usbduxfastsub_tmp = context;
-	struct usb_device *usbdev = usbduxfastsub_tmp->usbdev;
+	struct usb_interface *uinterf = usbduxfastsub_tmp->interface;
 	int ret;
 
 	if (fw == NULL)
@@ -1453,12 +1458,12 @@ static void usbduxfast_firmware_request_complete_handler(const struct firmware
 	ret = firmwareUpload(usbduxfastsub_tmp, fw->data, fw->size);
 
 	if (ret) {
-		dev_err(&usbdev->dev,
+		dev_err(&uinterf->dev,
 			"Could not upload firmware (err=%d)\n", ret);
 		goto out;
 	}
 
-	comedi_usb_auto_config(usbdev, BOARDNAME);
+	comedi_usb_auto_config(uinterf, &driver_usbduxfast);
  out:
 	release_firmware(fw);
 }
@@ -1606,7 +1611,7 @@ static void usbduxfastsub_disconnect(struct usb_interface *intf)
 		return;
 	}
 
-	comedi_usb_auto_unconfig(udev);
+	comedi_usb_auto_unconfig(intf);
 
 	down(&start_stop_sem);
 	down(&udfs->sem);
@@ -1721,43 +1726,19 @@ static int usbduxfast_attach(struct comedi_device *dev,
 	return 0;
 }
 
-static int usbduxfast_detach(struct comedi_device *dev)
+static void usbduxfast_detach(struct comedi_device *dev)
 {
-	struct usbduxfastsub_s *udfs;
-
-	if (!dev) {
-		printk(KERN_ERR "comedi?: usbduxfast: detach without dev "
-		       "variable...\n");
-		return -EFAULT;
-	}
-#ifdef CONFIG_COMEDI_DEBUG
-	printk(KERN_DEBUG "comedi%d: usbduxfast: detach usb device\n",
-	       dev->minor);
-#endif
-
-	udfs = dev->private;
-	if (!udfs) {
-		printk(KERN_ERR "comedi?: usbduxfast: detach without ptr to "
-		       "usbduxfastsub[]\n");
-		return -EFAULT;
+	struct usbduxfastsub_s *usb = dev->private;
+
+	if (usb) {
+		down(&usb->sem);
+		down(&start_stop_sem);
+		dev->private = NULL;
+		usb->attached = 0;
+		usb->comedidev = NULL;
+		up(&start_stop_sem);
+		up(&usb->sem);
 	}
-
-	down(&udfs->sem);
-	down(&start_stop_sem);
-	/*
-	 * Don't allow detach to free the private structure
-	 * It's one entry of of usbduxfastsub[]
-	 */
-	dev->private = NULL;
-	udfs->attached = 0;
-	udfs->comedidev = NULL;
-#ifdef CONFIG_COMEDI_DEBUG
-	printk(KERN_DEBUG "comedi%d: usbduxfast: detach: successfully "
-	       "removed\n", dev->minor);
-#endif
-	up(&start_stop_sem);
-	up(&udfs->sem);
-	return 0;
 }
 
 /*
diff --git a/drivers/staging/comedi/drivers/usbduxsigma.c b/drivers/staging/comedi/drivers/usbduxsigma.c
index 63c9b6dbc31..465afbdf406 100644
--- a/drivers/staging/comedi/drivers/usbduxsigma.c
+++ b/drivers/staging/comedi/drivers/usbduxsigma.c
@@ -267,6 +267,8 @@ static struct usbduxsub usbduxsub[NUMUSBDUX];
 
 static DEFINE_SEMAPHORE(start_stop_sem);
 
+static struct comedi_driver driver_usbduxsigma;	/* see below for initializer */
+
 /*
  * Stops the data acquision
  * It should be safe to call this function from any context
@@ -2312,11 +2314,11 @@ static void usbdux_firmware_request_complete_handler(const struct firmware *fw,
 						     void *context)
 {
 	struct usbduxsub *usbduxsub_tmp = context;
-	struct usb_device *usbdev = usbduxsub_tmp->usbdev;
+	struct usb_interface *uinterf = usbduxsub_tmp->interface;
 	int ret;
 
 	if (fw == NULL) {
-		dev_err(&usbdev->dev,
+		dev_err(&uinterf->dev,
 			"Firmware complete handler without firmware!\n");
 		return;
 	}
@@ -2328,11 +2330,11 @@ static void usbdux_firmware_request_complete_handler(const struct firmware *fw,
 	ret = firmwareUpload(usbduxsub_tmp, fw->data, fw->size);
 
 	if (ret) {
-		dev_err(&usbdev->dev,
+		dev_err(&uinterf->dev,
 			"Could not upload firmware (err=%d)\n", ret);
 		goto out;
 	}
-	comedi_usb_auto_config(usbdev, BOARDNAME);
+	comedi_usb_auto_config(uinterf, &driver_usbduxsigma);
 out:
 	release_firmware(fw);
 }
@@ -2623,7 +2625,7 @@ static void usbduxsigma_disconnect(struct usb_interface *intf)
 	if (usbduxsub_tmp->ao_cmd_running)
 		/* we are still running a command */
 		usbdux_ao_stop(usbduxsub_tmp, 1);
-	comedi_usb_auto_unconfig(udev);
+	comedi_usb_auto_unconfig(intf);
 	down(&start_stop_sem);
 	down(&usbduxsub_tmp->sem);
 	tidy_up(usbduxsub_tmp);
@@ -2799,37 +2801,17 @@ static int usbduxsigma_attach(struct comedi_device *dev,
 	return 0;
 }
 
-static int usbduxsigma_detach(struct comedi_device *dev)
+static void usbduxsigma_detach(struct comedi_device *dev)
 {
-	struct usbduxsub *usbduxsub_tmp;
+	struct usbduxsub *usb = dev->private;
 
-	if (!dev) {
-		printk(KERN_ERR
-		       "comedi? usbduxsigma detach: dev=NULL\n");
-		return -EFAULT;
+	if (usb) {
+		down(&usb->sem);
+		dev->private = NULL;
+		usb->attached = 0;
+		usb->comedidev = NULL;
+		up(&usb->sem);
 	}
-
-	usbduxsub_tmp = dev->private;
-	if (!usbduxsub_tmp) {
-		printk(KERN_ERR
-		       "comedi?: usbduxsigma detach: private=NULL\n");
-		return -EFAULT;
-	}
-
-	dev_dbg(&usbduxsub_tmp->interface->dev,
-		"comedi%d: detach usb device\n",
-		dev->minor);
-
-	down(&usbduxsub_tmp->sem);
-	/* Don't allow detach to free the private structure */
-	/* It's one entry of of usbduxsub[] */
-	dev->private = NULL;
-	usbduxsub_tmp->attached = 0;
-	usbduxsub_tmp->comedidev = NULL;
-	dev_info(&usbduxsub_tmp->interface->dev,
-		"comedi%d: successfully detached.\n", dev->minor);
-	up(&usbduxsub_tmp->sem);
-	return 0;
 }
 
 /* main driver struct */
diff --git a/drivers/staging/comedi/drivers/vmk80xx.c b/drivers/staging/comedi/drivers/vmk80xx.c
index 3d13ca6e167..baee8d76763 100644
--- a/drivers/staging/comedi/drivers/vmk80xx.c
+++ b/drivers/staging/comedi/drivers/vmk80xx.c
@@ -151,27 +151,12 @@ MODULE_DEVICE_TABLE(usb, vmk80xx_id_table);
 #define URB_RCV_FLAG            (1 << 0)
 #define URB_SND_FLAG            (1 << 1)
 
-#define CONFIG_VMK80XX_DEBUG
-#undef CONFIG_VMK80XX_DEBUG
-
-#ifdef CONFIG_VMK80XX_DEBUG
-static int dbgvm = 1;
-#else
-static int dbgvm;
-#endif
-
 #ifdef CONFIG_COMEDI_DEBUG
 static int dbgcm = 1;
 #else
 static int dbgcm;
 #endif
 
-#define dbgvm(fmt, arg...)                     \
-do {                                           \
-	if (dbgvm)                             \
-		printk(KERN_DEBUG fmt, ##arg); \
-} while (0)
-
 #define dbgcm(fmt, arg...)                     \
 do {                                           \
 	if (dbgcm)                             \
@@ -247,13 +232,13 @@ static struct vmk80xx_usb vmb[VMK80XX_MAX_BOARDS];
 
 static DEFINE_MUTEX(glb_mutex);
 
+static struct comedi_driver driver_vmk80xx;	/* see below for initializer */
+
 static void vmk80xx_tx_callback(struct urb *urb)
 {
 	struct vmk80xx_usb *dev = urb->context;
 	int stat = urb->status;
 
-	dbgvm("vmk80xx: %s\n", __func__);
-
 	if (stat && !(stat == -ENOENT
 		      || stat == -ECONNRESET || stat == -ESHUTDOWN))
 		dbgcm("comedi#: vmk80xx: %s - nonzero urb status (%d)\n",
@@ -272,8 +257,6 @@ static void vmk80xx_rx_callback(struct urb *urb)
 	struct vmk80xx_usb *dev = urb->context;
 	int stat = urb->status;
 
-	dbgvm("vmk80xx: %s\n", __func__);
-
 	switch (stat) {
 	case 0:
 		break;
@@ -295,7 +278,9 @@ resubmit:
 		if (!usb_submit_urb(urb, GFP_KERNEL))
 			goto exit;
 
-		err("comedi#: vmk80xx: %s - submit urb failed\n", __func__);
+		dev_err(&urb->dev->dev,
+			"comedi#: vmk80xx: %s - submit urb failed\n",
+			__func__);
 
 		usb_unanchor_urb(urb);
 	}
@@ -312,8 +297,6 @@ static int vmk80xx_check_data_link(struct vmk80xx_usb *dev)
 	unsigned char tx[1];
 	unsigned char rx[2];
 
-	dbgvm("vmk80xx: %s\n", __func__);
-
 	tx_pipe = usb_sndbulkpipe(dev->udev, 0x01);
 	rx_pipe = usb_rcvbulkpipe(dev->udev, 0x81);
 
@@ -338,8 +321,6 @@ static void vmk80xx_read_eeprom(struct vmk80xx_usb *dev, int flag)
 	unsigned char rx[64];
 	int cnt;
 
-	dbgvm("vmk80xx: %s\n", __func__);
-
 	tx_pipe = usb_sndbulkpipe(dev->udev, 0x01);
 	rx_pipe = usb_rcvbulkpipe(dev->udev, 0x81);
 
@@ -367,8 +348,6 @@ static int vmk80xx_reset_device(struct vmk80xx_usb *dev)
 	int ival;
 	size_t size;
 
-	dbgvm("vmk80xx: %s\n", __func__);
-
 	urb = usb_alloc_urb(0, GFP_KERNEL);
 	if (!urb)
 		return -ENOMEM;
@@ -406,8 +385,6 @@ static void vmk80xx_build_int_urb(struct urb *urb, int flag)
 	void (*callback) (struct urb *);
 	int ival;
 
-	dbgvm("vmk80xx: %s\n", __func__);
-
 	if (flag & URB_RCV_FLAG) {
 		rx_addr = dev->ep_rx->bEndpointAddress;
 		pipe = usb_rcvintpipe(dev->udev, rx_addr);
@@ -435,8 +412,6 @@ static void vmk80xx_do_bulk_msg(struct vmk80xx_usb *dev)
 	unsigned int rx_pipe;
 	size_t size;
 
-	dbgvm("vmk80xx: %s\n", __func__);
-
 	set_bit(TRANS_IN_BUSY, &dev->flags);
 	set_bit(TRANS_OUT_BUSY, &dev->flags);
 
@@ -464,8 +439,6 @@ static int vmk80xx_read_packet(struct vmk80xx_usb *dev)
 	struct urb *urb;
 	int retval;
 
-	dbgvm("vmk80xx: %s\n", __func__);
-
 	if (!dev->intf)
 		return -ENODEV;
 
@@ -512,8 +485,6 @@ static int vmk80xx_write_packet(struct vmk80xx_usb *dev, int cmd)
 	struct urb *urb;
 	int retval;
 
-	dbgvm("vmk80xx: %s\n", __func__);
-
 	if (!dev->intf)
 		return -ENODEV;
 
@@ -588,8 +559,6 @@ static int vmk80xx_ai_rinsn(struct comedi_device *cdev,
 	int reg[2];
 	int n;
 
-	dbgvm("vmk80xx: %s\n", __func__);
-
 	n = rudimentary_check(dev, DIR_IN);
 	if (n)
 		return n;
@@ -641,8 +610,6 @@ static int vmk80xx_ao_winsn(struct comedi_device *cdev,
 	int reg;
 	int n;
 
-	dbgvm("vmk80xx: %s\n", __func__);
-
 	n = rudimentary_check(dev, DIR_OUT);
 	if (n)
 		return n;
@@ -686,8 +653,6 @@ static int vmk80xx_ao_rinsn(struct comedi_device *cdev,
 	int reg;
 	int n;
 
-	dbgvm("vmk80xx: %s\n", __func__);
-
 	n = rudimentary_check(dev, DIR_IN);
 	if (n)
 		return n;
@@ -720,8 +685,6 @@ static int vmk80xx_di_bits(struct comedi_device *cdev,
 	int reg;
 	int retval;
 
-	dbgvm("vmk80xx: %s\n", __func__);
-
 	retval = rudimentary_check(dev, DIR_IN);
 	if (retval)
 		return retval;
@@ -766,8 +729,6 @@ static int vmk80xx_di_rinsn(struct comedi_device *cdev,
 	int inp;
 	int n;
 
-	dbgvm("vmk80xx: %s\n", __func__);
-
 	n = rudimentary_check(dev, DIR_IN);
 	if (n)
 		return n;
@@ -813,8 +774,6 @@ static int vmk80xx_do_winsn(struct comedi_device *cdev,
 	int cmd;
 	int n;
 
-	dbgvm("vmk80xx: %s\n", __func__);
-
 	n = rudimentary_check(dev, DIR_OUT);
 	if (n)
 		return n;
@@ -861,8 +820,6 @@ static int vmk80xx_do_rinsn(struct comedi_device *cdev,
 	int reg;
 	int n;
 
-	dbgvm("vmk80xx: %s\n", __func__);
-
 	n = rudimentary_check(dev, DIR_IN);
 	if (n)
 		return n;
@@ -895,8 +852,6 @@ static int vmk80xx_do_bits(struct comedi_device *cdev,
 	int dir, reg, cmd;
 	int retval;
 
-	dbgvm("vmk80xx: %s\n", __func__);
-
 	dir = 0;
 
 	if (data[0])
@@ -962,8 +917,6 @@ static int vmk80xx_cnt_rinsn(struct comedi_device *cdev,
 	int reg[2];
 	int n;
 
-	dbgvm("vmk80xx: %s\n", __func__);
-
 	n = rudimentary_check(dev, DIR_IN);
 	if (n)
 		return n;
@@ -1012,18 +965,16 @@ static int vmk80xx_cnt_cinsn(struct comedi_device *cdev,
 	int reg;
 	int n;
 
-	dbgvm("vmk80xx: %s\n", __func__);
-
 	n = rudimentary_check(dev, DIR_OUT);
 	if (n)
 		return n;
 
-	down(&dev->limit_sem);
-
 	insn_cmd = data[0];
 	if (insn_cmd != INSN_CONFIG_RESET && insn_cmd != GPCT_RESET)
 		return -EINVAL;
 
+	down(&dev->limit_sem);
+
 	chan = CR_CHAN(insn->chanspec);
 
 	if (dev->board.model == VMK8055_MODEL) {
@@ -1060,8 +1011,6 @@ static int vmk80xx_cnt_winsn(struct comedi_device *cdev,
 	int cmd;
 	int n;
 
-	dbgvm("vmk80xx: %s\n", __func__);
-
 	n = rudimentary_check(dev, DIR_OUT);
 	if (n)
 		return n;
@@ -1106,8 +1055,6 @@ static int vmk80xx_pwm_rinsn(struct comedi_device *cdev,
 	int reg[2];
 	int n;
 
-	dbgvm("vmk80xx: %s\n", __func__);
-
 	n = rudimentary_check(dev, DIR_IN);
 	if (n)
 		return n;
@@ -1141,8 +1088,6 @@ static int vmk80xx_pwm_winsn(struct comedi_device *cdev,
 	int cmd;
 	int n;
 
-	dbgvm("vmk80xx: %s\n", __func__);
-
 	n = rudimentary_check(dev, DIR_OUT);
 	if (n)
 		return n;
@@ -1191,8 +1136,6 @@ static int vmk80xx_attach(struct comedi_device *cdev,
 	struct comedi_subdevice *s;
 	int minor;
 
-	dbgvm("vmk80xx: %s\n", __func__);
-
 	mutex_lock(&glb_mutex);
 
 	for (i = 0; i < VMK80XX_MAX_BOARDS; i++)
@@ -1307,34 +1250,16 @@ static int vmk80xx_attach(struct comedi_device *cdev,
 	return 0;
 }
 
-static int vmk80xx_detach(struct comedi_device *cdev)
+static void vmk80xx_detach(struct comedi_device *dev)
 {
-	struct vmk80xx_usb *dev;
-	int minor;
-
-	dbgvm("vmk80xx: %s\n", __func__);
-
-	if (!cdev)
-		return -EFAULT;
-
-	dev = cdev->private;
-	if (!dev)
-		return -EFAULT;
+	struct vmk80xx_usb *usb = dev->private;
 
-	down(&dev->limit_sem);
-
-	cdev->private = NULL;
-	dev->attached = 0;
-
-	minor = cdev->minor;
-
-	printk(KERN_INFO
-	       "comedi%d: vmk80xx: board #%d [%s] detached from comedi\n",
-	       minor, dev->count, dev->board.name);
-
-	up(&dev->limit_sem);
-
-	return 0;
+	if (usb) {
+		down(&usb->limit_sem);
+		dev->private = NULL;
+		usb->attached = 0;
+		up(&usb->limit_sem);
+	}
 }
 
 static int vmk80xx_probe(struct usb_interface *intf,
@@ -1346,8 +1271,6 @@ static int vmk80xx_probe(struct usb_interface *intf,
 	struct usb_endpoint_descriptor *ep_desc;
 	size_t size;
 
-	dbgvm("vmk80xx: %s\n", __func__);
-
 	mutex_lock(&glb_mutex);
 
 	for (i = 0; i < VMK80XX_MAX_BOARDS; i++)
@@ -1482,7 +1405,7 @@ static int vmk80xx_probe(struct usb_interface *intf,
 
 	mutex_unlock(&glb_mutex);
 
-	comedi_usb_auto_config(dev->udev, BOARDNAME);
+	comedi_usb_auto_config(intf, &driver_vmk80xx);
 
 	return 0;
 error:
@@ -1495,12 +1418,10 @@ static void vmk80xx_disconnect(struct usb_interface *intf)
 {
 	struct vmk80xx_usb *dev = usb_get_intfdata(intf);
 
-	dbgvm("vmk80xx: %s\n", __func__);
-
 	if (!dev)
 		return;
 
-	comedi_usb_auto_unconfig(dev->udev);
+	comedi_usb_auto_unconfig(intf);
 
 	mutex_lock(&glb_mutex);
 	down(&dev->limit_sem);
diff --git a/drivers/staging/comedi/internal.h b/drivers/staging/comedi/internal.h
index 434ce343336..7ed20a04eef 100644
--- a/drivers/staging/comedi/internal.h
+++ b/drivers/staging/comedi/internal.h
@@ -1,5 +1,5 @@
 /*
- * various internal comedi functions
+ * various internal comedi stuff
  */
 int do_rangeinfo_ioctl(struct comedi_device *dev,
 		       struct comedi_rangeinfo __user *arg);
@@ -7,6 +7,10 @@ int insn_inval(struct comedi_device *dev, struct comedi_subdevice *s,
 	       struct comedi_insn *insn, unsigned int *data);
 int comedi_alloc_board_minor(struct device *hardware_device);
 void comedi_free_board_minor(unsigned minor);
+int comedi_find_board_minor(struct device *hardware_device);
 void comedi_reset_async_buf(struct comedi_async *async);
 int comedi_buf_alloc(struct comedi_device *dev, struct comedi_subdevice *s,
 		     unsigned long new_size);
+
+extern unsigned int comedi_default_buf_size_kb;
+extern unsigned int comedi_default_buf_maxsize_kb;
diff --git a/drivers/staging/et131x/et131x.c b/drivers/staging/et131x/et131x.c
index 886f5650444..5b11c5e3622 100644
--- a/drivers/staging/et131x/et131x.c
+++ b/drivers/staging/et131x/et131x.c
@@ -1710,7 +1710,8 @@ static int et131x_mdio_read(struct mii_bus *bus, int phy_addr, int reg)
 		return value;
 }
 
-static int et131x_mdio_write(struct mii_bus *bus, int phy_addr, int reg, u16 value)
+static int et131x_mdio_write(struct mii_bus *bus, int phy_addr,
+			     int reg, u16 value)
 {
 	struct net_device *netdev = bus->priv;
 	struct et131x_adapter *adapter = netdev_priv(netdev);
@@ -4013,7 +4014,7 @@ static int et131x_pci_init(struct et131x_adapter *adapter,
 		dev_err(&pdev->dev, "Missing PCIe capabilities\n");
 		goto err_out;
 	}
-		
+
 	/* Let's set up the PORT LOGIC Register.  First we need to know what
 	 * the max_payload_size is
 	 */
@@ -4060,7 +4061,7 @@ static int et131x_pci_init(struct et131x_adapter *adapter,
 		goto err_out;
 	}
 
-	ctl = (ctl & ~PCI_EXP_DEVCTL_READRQ) | ( 0x04 << 12);
+	ctl = (ctl & ~PCI_EXP_DEVCTL_READRQ) | (0x04 << 12);
 
 	if (pci_write_config_word(pdev, cap + PCI_EXP_DEVCTL, ctl)) {
 		dev_err(&pdev->dev,
@@ -4824,7 +4825,8 @@ static int et131x_open(struct net_device *netdev)
 	adapter->error_timer.data = (unsigned long)adapter;
 	add_timer(&adapter->error_timer);
 
-	result = request_irq(irq, et131x_isr, IRQF_SHARED, netdev->name, netdev);
+	result = request_irq(irq, et131x_isr,
+			     IRQF_SHARED, netdev->name, netdev);
 	if (result) {
 		dev_err(&pdev->dev, "could not register IRQ %d\n", irq);
 		return result;
diff --git a/drivers/staging/frontier/alphatrack.c b/drivers/staging/frontier/alphatrack.c
index 3bf0f40e97f..acbb2cc510f 100644
--- a/drivers/staging/frontier/alphatrack.c
+++ b/drivers/staging/frontier/alphatrack.c
@@ -333,8 +333,8 @@ static int usb_alphatrack_open(struct inode *inode, struct file *file)
 	interface = usb_find_interface(&usb_alphatrack_driver, subminor);
 
 	if (!interface) {
-		err("%s - error, can't find device for minor %d\n",
-		    __func__, subminor);
+		printk(KERN_ERR "%s - error, can't find device for minor %d\n",
+		       __func__, subminor);
 		retval = -ENODEV;
 		goto unlock_disconnect_exit;
 	}
@@ -494,7 +494,8 @@ static ssize_t usb_alphatrack_read(struct file *file, char __user *buffer,
 	/* verify that the device wasn't unplugged */
 	if (dev->intf == NULL) {
 		retval = -ENODEV;
-		err("No device or device unplugged %d\n", retval);
+		printk(KERN_ERR "%s: No device or device unplugged %d\n",
+		       __func__, retval);
 		goto unlock_exit;
 	}
 
@@ -564,7 +565,8 @@ static ssize_t usb_alphatrack_write(struct file *file,
 	/* verify that the device wasn't unplugged */
 	if (dev->intf == NULL) {
 		retval = -ENODEV;
-		err("No device or device unplugged %d\n", retval);
+		printk(KERN_ERR "%s: No device or device unplugged %d\n",
+		       __func__, retval);
 		goto unlock_exit;
 	}
 
@@ -599,7 +601,7 @@ static ssize_t usb_alphatrack_write(struct file *file,
 	}
 
 	if (dev->interrupt_out_endpoint == NULL) {
-		err("Endpoint should not be be null!\n");
+		dev_err(&dev->intf->dev, "Endpoint should not be be null!\n");
 		goto unlock_exit;
 	}
 
@@ -619,7 +621,8 @@ static ssize_t usb_alphatrack_write(struct file *file,
 	retval = usb_submit_urb(dev->interrupt_out_urb, GFP_KERNEL);
 	if (retval) {
 		dev->interrupt_out_busy = 0;
-		err("Couldn't submit interrupt_out_urb %d\n", retval);
+		dev_err(&dev->intf->dev,
+			"Couldn't submit interrupt_out_urb %d\n", retval);
 		atomic_dec(&dev->writes_pending);
 		goto unlock_exit;
 	}
diff --git a/drivers/staging/frontier/tranzport.c b/drivers/staging/frontier/tranzport.c
index 29e99bbcae4..376706f1c71 100644
--- a/drivers/staging/frontier/tranzport.c
+++ b/drivers/staging/frontier/tranzport.c
@@ -353,7 +353,7 @@ static int usb_tranzport_open(struct inode *inode, struct file *file)
 	interface = usb_find_interface(&usb_tranzport_driver, subminor);
 
 	if (!interface) {
-		err("%s - error, can't find device for minor %d\n",
+		printk(KERN_ERR "%s - error, can't find device for minor %d\n",
 			__func__, subminor);
 		retval = -ENODEV;
 		goto unlock_disconnect_exit;
@@ -517,9 +517,11 @@ static ssize_t usb_tranzport_read(struct file *file, char __user *buffer,
 		goto exit;
 	}
 
-	/* verify that the device wasn't unplugged */ if (dev->intf == NULL) {
+	/* verify that the device wasn't unplugged */
+	if (dev->intf == NULL) {
 		retval = -ENODEV;
-		err("No device or device unplugged %d\n", retval);
+		printk(KERN_ERR "%s: No device or device unplugged %d\n",
+			__func__, retval);
 		goto unlock_exit;
 	}
 
@@ -691,7 +693,8 @@ static ssize_t usb_tranzport_write(struct file *file,
 	/* verify that the device wasn't unplugged */
 	if (dev->intf == NULL) {
 		retval = -ENODEV;
-		err("No device or device unplugged %d\n", retval);
+		printk(KERN_ERR "%s: No device or device unplugged %d\n",
+			__func__, retval);
 		goto unlock_exit;
 	}
 
@@ -726,7 +729,7 @@ static ssize_t usb_tranzport_write(struct file *file,
 	}
 
 	if (dev->interrupt_out_endpoint == NULL) {
-		err("Endpoint should not be be null!\n");
+		dev_err(&dev->intf->dev, "Endpoint should not be be null!\n");
 		goto unlock_exit;
 	}
 
@@ -746,7 +749,8 @@ static ssize_t usb_tranzport_write(struct file *file,
 	retval = usb_submit_urb(dev->interrupt_out_urb, GFP_KERNEL);
 	if (retval) {
 		dev->interrupt_out_busy = 0;
-		err("Couldn't submit interrupt_out_urb %d\n", retval);
+		dev_err(&dev->intf->dev,
+			"Couldn't submit interrupt_out_urb %d\n", retval);
 		goto unlock_exit;
 	}
 	retval = bytes_to_write;
diff --git a/drivers/staging/gdm72xx/Kconfig b/drivers/staging/gdm72xx/Kconfig
new file mode 100644
index 00000000000..3c18efe3136
--- /dev/null
+++ b/drivers/staging/gdm72xx/Kconfig
@@ -0,0 +1,46 @@
+#
+# GCT GDM72xx WiMAX driver configuration
+#
+
+menuconfig WIMAX_GDM72XX
+	tristate "GCT GDM72xx WiMAX support"
+	depends on NET
+	help
+	  Support for the GCT GDM72xx WiMAX chip
+
+if WIMAX_GDM72XX
+
+config WIMAX_GDM72XX_QOS
+	bool "Enable QoS support"
+	default n
+
+config WIMAX_GDM72XX_K_MODE
+	bool "Enable K mode"
+	default n
+
+config WIMAX_GDM72XX_WIMAX2
+	bool "Enable WIMAX2 support"
+	default n
+
+choice
+	prompt "Select interface"
+
+config WIMAX_GDM72XX_USB
+	bool "USB interface"
+	depends on USB
+
+config WIMAX_GDM72XX_SDIO
+	bool "SDIO interface"
+	depends on MMC
+
+endchoice
+
+if WIMAX_GDM72XX_USB
+
+config WIMAX_GDM72XX_USB_PM
+	bool "Enable power managerment support"
+	depends on USB_SUSPEND
+
+endif # WIMAX_GDM72XX_USB
+
+endif # WIMAX_GDM72XX
diff --git a/drivers/staging/gdm72xx/Makefile b/drivers/staging/gdm72xx/Makefile
new file mode 100644
index 00000000000..35da7b90b19
--- /dev/null
+++ b/drivers/staging/gdm72xx/Makefile
@@ -0,0 +1,6 @@
+obj-$(CONFIG_WIMAX_GDM72XX) := gdmwm.o
+
+gdmwm-y += gdm_wimax.o netlink_k.o
+gdmwm-$(CONFIG_WIMAX_GDM72XX_QOS) += gdm_qos.o
+gdmwm-$(CONFIG_WIMAX_GDM72XX_SDIO) += gdm_sdio.o sdio_boot.o
+gdmwm-$(CONFIG_WIMAX_GDM72XX_USB) += gdm_usb.o usb_boot.o
diff --git a/drivers/staging/gdm72xx/TODO b/drivers/staging/gdm72xx/TODO
new file mode 100644
index 00000000000..30ac01ab972
--- /dev/null
+++ b/drivers/staging/gdm72xx/TODO
@@ -0,0 +1,5 @@
+TODO:
+- Replace kernel_thread with kthread in gdm_usb.c
+- Replace hard-coded firmware paths with request_firmware in
+  sdio_boot.c and usb_boot.c
+- Clean up coding style to meet kernel standard.
diff --git a/drivers/staging/gdm72xx/gdm_qos.c b/drivers/staging/gdm72xx/gdm_qos.c
new file mode 100644
index 00000000000..0217680ec54
--- /dev/null
+++ b/drivers/staging/gdm72xx/gdm_qos.c
@@ -0,0 +1,460 @@
+/*
+ * Copyright (c) 2012 GCT Semiconductor, Inc. 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
+ * 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/version.h>
+#include <linux/etherdevice.h>
+#include <asm/byteorder.h>
+
+#include <linux/ip.h>
+#include <linux/tcp.h>
+#include <linux/if_ether.h>
+
+#include "gdm_wimax.h"
+#include "hci.h"
+#include "gdm_qos.h"
+
+#define B2H(x)		__be16_to_cpu(x)
+
+#undef dprintk
+#define dprintk(fmt, args ...) printk(KERN_DEBUG "[QoS] " fmt, ## args)
+#undef wprintk
+#define wprintk(fmt, args ...) \
+	printk(KERN_WARNING "[QoS WARNING] " fmt, ## args)
+#undef eprintk
+#define eprintk(fmt, args ...) printk(KERN_ERR "[QoS ERROR] " fmt, ## args)
+
+
+#define MAX_FREE_LIST_CNT		32
+static struct {
+	struct list_head head;
+	int cnt;
+	spinlock_t lock;
+} qos_free_list;
+
+static void init_qos_entry_list(void)
+{
+	qos_free_list.cnt = 0;
+	INIT_LIST_HEAD(&qos_free_list.head);
+	spin_lock_init(&qos_free_list.lock);
+}
+
+static void *alloc_qos_entry(void)
+{
+	struct qos_entry_s *entry;
+	unsigned long flags;
+
+	spin_lock_irqsave(&qos_free_list.lock, flags);
+	if (qos_free_list.cnt) {
+		entry = list_entry(qos_free_list.head.prev, struct qos_entry_s,
+					list);
+		list_del(&entry->list);
+		qos_free_list.cnt--;
+		spin_unlock_irqrestore(&qos_free_list.lock, flags);
+		return entry;
+	}
+	spin_unlock_irqrestore(&qos_free_list.lock, flags);
+
+	entry = kmalloc(sizeof(struct qos_entry_s), GFP_ATOMIC);
+	return entry;
+}
+
+static void free_qos_entry(void *entry)
+{
+	struct qos_entry_s *qentry = (struct qos_entry_s *) entry;
+	unsigned long flags;
+
+	spin_lock_irqsave(&qos_free_list.lock, flags);
+	if (qos_free_list.cnt < MAX_FREE_LIST_CNT) {
+		list_add(&qentry->list, &qos_free_list.head);
+		qos_free_list.cnt++;
+		spin_unlock_irqrestore(&qos_free_list.lock, flags);
+		return;
+	}
+	spin_unlock_irqrestore(&qos_free_list.lock, flags);
+
+	kfree(entry);
+}
+
+static void free_qos_entry_list(struct list_head *free_list)
+{
+	struct qos_entry_s *entry, *n;
+	int total_free = 0;
+
+	list_for_each_entry_safe(entry, n, free_list, list) {
+		list_del(&entry->list);
+		kfree(entry);
+		total_free++;
+	}
+
+	dprintk("%s: total_free_cnt=%d\n", __func__, total_free);
+}
+
+void gdm_qos_init(void *nic_ptr)
+{
+	struct nic *nic = nic_ptr;
+	struct qos_cb_s *qcb = &nic->qos;
+	int i;
+
+	for (i = 0 ; i < QOS_MAX; i++) {
+		INIT_LIST_HEAD(&qcb->qos_list[i]);
+		qcb->csr[i].QoSBufCount = 0;
+		qcb->csr[i].Enabled = 0;
+	}
+
+	qcb->qos_list_cnt = 0;
+	qcb->qos_null_idx = QOS_MAX-1;
+	qcb->qos_limit_size = 255;
+
+	spin_lock_init(&qcb->qos_lock);
+
+	init_qos_entry_list();
+}
+
+void gdm_qos_release_list(void *nic_ptr)
+{
+	struct nic *nic = nic_ptr;
+	struct qos_cb_s *qcb = &nic->qos;
+	unsigned long flags;
+	struct qos_entry_s *entry, *n;
+	struct list_head free_list;
+	int i;
+
+	INIT_LIST_HEAD(&free_list);
+
+	spin_lock_irqsave(&qcb->qos_lock, flags);
+
+	for (i = 0; i < QOS_MAX; i++) {
+		qcb->csr[i].QoSBufCount = 0;
+		qcb->csr[i].Enabled = 0;
+	}
+
+	qcb->qos_list_cnt = 0;
+	qcb->qos_null_idx = QOS_MAX-1;
+
+	for (i = 0; i < QOS_MAX; i++) {
+		list_for_each_entry_safe(entry, n, &qcb->qos_list[i], list) {
+			list_move_tail(&entry->list, &free_list);
+		}
+	}
+	spin_unlock_irqrestore(&qcb->qos_lock, flags);
+	free_qos_entry_list(&free_list);
+}
+
+static u32 chk_ipv4_rule(struct gdm_wimax_csr_s *csr, u8 *Stream, u8 *port)
+{
+	int i;
+
+	if (csr->ClassifierRuleEnable&IPTYPEOFSERVICE) {
+		if (((Stream[1] & csr->IPToSMask) < csr->IPToSLow) ||
+		((Stream[1] & csr->IPToSMask) > csr->IPToSHigh))
+			return 1;
+	}
+
+	if (csr->ClassifierRuleEnable&PROTOCOL) {
+		if (Stream[9] != csr->Protocol)
+			return 1;
+	}
+
+	if (csr->ClassifierRuleEnable&IPMASKEDSRCADDRESS) {
+		for (i = 0; i < 4; i++) {
+			if ((Stream[12 + i] & csr->IPSrcAddrMask[i]) !=
+			(csr->IPSrcAddr[i] & csr->IPSrcAddrMask[i]))
+				return 1;
+		}
+	}
+
+	if (csr->ClassifierRuleEnable&IPMASKEDDSTADDRESS) {
+		for (i = 0; i < 4; i++) {
+			if ((Stream[16 + i] & csr->IPDstAddrMask[i]) !=
+			(csr->IPDstAddr[i] & csr->IPDstAddrMask[i]))
+				return 1;
+		}
+	}
+
+	if (csr->ClassifierRuleEnable&PROTOCOLSRCPORTRANGE) {
+		i = ((port[0]<<8)&0xff00)+port[1];
+		if ((i < csr->SrcPortLow) || (i > csr->SrcPortHigh))
+			return 1;
+	}
+
+	if (csr->ClassifierRuleEnable&PROTOCOLDSTPORTRANGE) {
+		i = ((port[2]<<8)&0xff00)+port[3];
+		if ((i < csr->DstPortLow) || (i > csr->DstPortHigh))
+			return 1;
+	}
+
+	return 0;
+}
+
+static u32 get_qos_index(struct nic *nic, u8* iph, u8* tcpudph)
+{
+	u32	IP_Ver, Header_Len, i;
+	struct qos_cb_s *qcb = &nic->qos;
+
+	if (iph == NULL || tcpudph == NULL)
+		return -1;
+
+	IP_Ver = (iph[0]>>4)&0xf;
+	Header_Len = iph[0]&0xf;
+
+	if (IP_Ver == 4) {
+		for (i = 0; i < QOS_MAX; i++) {
+			if (qcb->csr[i].Enabled) {
+				if (qcb->csr[i].ClassifierRuleEnable) {
+					if (chk_ipv4_rule(&qcb->csr[i], iph,
+					tcpudph) == 0)
+						return i;
+				}
+			}
+		}
+	}
+
+	return -1;
+}
+
+static u32 extract_qos_list(struct nic *nic, struct list_head *head)
+{
+	struct qos_cb_s *qcb = &nic->qos;
+	struct qos_entry_s *entry;
+	int i;
+
+	INIT_LIST_HEAD(head);
+
+	for (i = 0; i < QOS_MAX; i++) {
+		if (qcb->csr[i].Enabled) {
+			if (qcb->csr[i].QoSBufCount < qcb->qos_limit_size) {
+				if (!list_empty(&qcb->qos_list[i])) {
+					entry = list_entry(
+					qcb->qos_list[i].prev,
+					struct qos_entry_s, list);
+					list_move_tail(&entry->list, head);
+					qcb->csr[i].QoSBufCount++;
+
+					if (!list_empty(&qcb->qos_list[i]))
+						wprintk("QoS Index(%d) "
+							"is piled!!\n", i);
+				}
+			}
+		}
+	}
+
+	return 0;
+}
+
+static void send_qos_list(struct nic *nic, struct list_head *head)
+{
+	struct qos_entry_s *entry, *n;
+
+	list_for_each_entry_safe(entry, n, head, list) {
+		list_del(&entry->list);
+		free_qos_entry(entry);
+		gdm_wimax_send_tx(entry->skb, entry->dev);
+	}
+}
+
+int gdm_qos_send_hci_pkt(struct sk_buff *skb, struct net_device *dev)
+{
+	struct nic *nic = netdev_priv(dev);
+	int index;
+	struct qos_cb_s *qcb = &nic->qos;
+	unsigned long flags;
+	struct ethhdr *ethh = (struct ethhdr *) (skb->data + HCI_HEADER_SIZE);
+	struct iphdr *iph = (struct iphdr *) ((char *) ethh + ETH_HLEN);
+	struct tcphdr *tcph;
+	struct qos_entry_s *entry = NULL;
+	struct list_head send_list;
+	int ret = 0;
+
+	tcph = (struct tcphdr *) iph + iph->ihl*4;
+
+	if (B2H(ethh->h_proto) == ETH_P_IP) {
+		if (qcb->qos_list_cnt && !qos_free_list.cnt) {
+			entry = alloc_qos_entry();
+			entry->skb = skb;
+			entry->dev = dev;
+			dprintk("qcb->qos_list_cnt=%d\n", qcb->qos_list_cnt);
+		}
+
+		spin_lock_irqsave(&qcb->qos_lock, flags);
+		if (qcb->qos_list_cnt) {
+			index = get_qos_index(nic, (u8 *)iph, (u8 *) tcph);
+			if (index == -1)
+				index = qcb->qos_null_idx;
+
+			if (!entry) {
+				entry = alloc_qos_entry();
+				entry->skb = skb;
+				entry->dev = dev;
+			}
+
+			list_add_tail(&entry->list, &qcb->qos_list[index]);
+			extract_qos_list(nic, &send_list);
+			spin_unlock_irqrestore(&qcb->qos_lock, flags);
+			send_qos_list(nic, &send_list);
+			goto out;
+		}
+		spin_unlock_irqrestore(&qcb->qos_lock, flags);
+		if (entry)
+			free_qos_entry(entry);
+	}
+
+	ret = gdm_wimax_send_tx(skb, dev);
+out:
+	return ret;
+}
+
+static u32 get_csr(struct qos_cb_s *qcb, u32 SFID, int mode)
+{
+	int i;
+
+	for (i = 0; i < qcb->qos_list_cnt; i++) {
+		if (qcb->csr[i].SFID == SFID)
+			return i;
+	}
+
+	if (mode) {
+		for (i = 0; i < QOS_MAX; i++) {
+			if (qcb->csr[i].Enabled == 0) {
+				qcb->csr[i].Enabled = 1;
+				qcb->qos_list_cnt++;
+				return i;
+			}
+		}
+	}
+	return -1;
+}
+
+#define QOS_CHANGE_DEL	0xFC
+#define QOS_ADD		0xFD
+#define QOS_REPORT	0xFE
+
+void gdm_recv_qos_hci_packet(void *nic_ptr, u8 *buf, int size)
+{
+	struct nic *nic = nic_ptr;
+	u32 i, SFID, index, pos;
+	u8 subCmdEvt;
+	u8 len;
+	struct qos_cb_s *qcb = &nic->qos;
+	struct qos_entry_s *entry, *n;
+	struct list_head send_list;
+	struct list_head free_list;
+	unsigned long flags;
+
+	subCmdEvt = (u8)buf[4];
+
+	if (subCmdEvt == QOS_REPORT) {
+		len = (u8)buf[5];
+
+		spin_lock_irqsave(&qcb->qos_lock, flags);
+		for (i = 0; i < qcb->qos_list_cnt; i++) {
+			SFID = ((buf[(i*5)+6]<<24)&0xff000000);
+			SFID += ((buf[(i*5)+7]<<16)&0xff0000);
+			SFID += ((buf[(i*5)+8]<<8)&0xff00);
+			SFID += (buf[(i*5)+9]);
+			index = get_csr(qcb, SFID, 0);
+			if (index == -1) {
+				spin_unlock_irqrestore(&qcb->qos_lock, flags);
+				eprintk("QoS ERROR: No SF\n");
+				return;
+			}
+			qcb->csr[index].QoSBufCount = buf[(i*5)+10];
+		}
+
+		extract_qos_list(nic, &send_list);
+		spin_unlock_irqrestore(&qcb->qos_lock, flags);
+		send_qos_list(nic, &send_list);
+		return;
+	} else if (subCmdEvt == QOS_ADD) {
+		pos = 5;
+		len = (u8)buf[pos++];
+
+		SFID = ((buf[pos++]<<24)&0xff000000);
+		SFID += ((buf[pos++]<<16)&0xff0000);
+		SFID += ((buf[pos++]<<8)&0xff00);
+		SFID += (buf[pos++]);
+
+		index = get_csr(qcb, SFID, 1);
+		if (index == -1) {
+			eprintk("QoS ERROR: csr Update Error\n");
+			return;
+		}
+
+		dprintk("QOS_ADD SFID = 0x%x, index=%d\n", SFID, index);
+
+		spin_lock_irqsave(&qcb->qos_lock, flags);
+		qcb->csr[index].SFID = SFID;
+		qcb->csr[index].ClassifierRuleEnable = ((buf[pos++]<<8)&0xff00);
+		qcb->csr[index].ClassifierRuleEnable += buf[pos++];
+		if (qcb->csr[index].ClassifierRuleEnable == 0)
+			qcb->qos_null_idx = index;
+		qcb->csr[index].IPToSMask = buf[pos++];
+		qcb->csr[index].IPToSLow = buf[pos++];
+		qcb->csr[index].IPToSHigh = buf[pos++];
+		qcb->csr[index].Protocol = buf[pos++];
+		qcb->csr[index].IPSrcAddrMask[0] = buf[pos++];
+		qcb->csr[index].IPSrcAddrMask[1] = buf[pos++];
+		qcb->csr[index].IPSrcAddrMask[2] = buf[pos++];
+		qcb->csr[index].IPSrcAddrMask[3] = buf[pos++];
+		qcb->csr[index].IPSrcAddr[0] = buf[pos++];
+		qcb->csr[index].IPSrcAddr[1] = buf[pos++];
+		qcb->csr[index].IPSrcAddr[2] = buf[pos++];
+		qcb->csr[index].IPSrcAddr[3] = buf[pos++];
+		qcb->csr[index].IPDstAddrMask[0] = buf[pos++];
+		qcb->csr[index].IPDstAddrMask[1] = buf[pos++];
+		qcb->csr[index].IPDstAddrMask[2] = buf[pos++];
+		qcb->csr[index].IPDstAddrMask[3] = buf[pos++];
+		qcb->csr[index].IPDstAddr[0] = buf[pos++];
+		qcb->csr[index].IPDstAddr[1] = buf[pos++];
+		qcb->csr[index].IPDstAddr[2] = buf[pos++];
+		qcb->csr[index].IPDstAddr[3] = buf[pos++];
+		qcb->csr[index].SrcPortLow = ((buf[pos++]<<8)&0xff00);
+		qcb->csr[index].SrcPortLow += buf[pos++];
+		qcb->csr[index].SrcPortHigh = ((buf[pos++]<<8)&0xff00);
+		qcb->csr[index].SrcPortHigh += buf[pos++];
+		qcb->csr[index].DstPortLow = ((buf[pos++]<<8)&0xff00);
+		qcb->csr[index].DstPortLow += buf[pos++];
+		qcb->csr[index].DstPortHigh = ((buf[pos++]<<8)&0xff00);
+		qcb->csr[index].DstPortHigh += buf[pos++];
+
+		qcb->qos_limit_size = 254/qcb->qos_list_cnt;
+		spin_unlock_irqrestore(&qcb->qos_lock, flags);
+	} else if (subCmdEvt == QOS_CHANGE_DEL) {
+		pos = 5;
+		len = (u8)buf[pos++];
+		SFID = ((buf[pos++]<<24)&0xff000000);
+		SFID += ((buf[pos++]<<16)&0xff0000);
+		SFID += ((buf[pos++]<<8)&0xff00);
+		SFID += (buf[pos++]);
+		index = get_csr(qcb, SFID, 1);
+		if (index == -1) {
+			eprintk("QoS ERROR: Wrong index(%d)\n", index);
+			return;
+		}
+
+		dprintk("QOS_CHANGE_DEL SFID = 0x%x, index=%d\n", SFID, index);
+
+		INIT_LIST_HEAD(&free_list);
+
+		spin_lock_irqsave(&qcb->qos_lock, flags);
+		qcb->csr[index].Enabled = 0;
+		qcb->qos_list_cnt--;
+		qcb->qos_limit_size = 254/qcb->qos_list_cnt;
+
+		list_for_each_entry_safe(entry, n, &qcb->qos_list[index],
+					list) {
+			list_move_tail(&entry->list, &free_list);
+		}
+		spin_unlock_irqrestore(&qcb->qos_lock, flags);
+		free_qos_entry_list(&free_list);
+	}
+}
diff --git a/drivers/staging/gdm72xx/gdm_qos.h b/drivers/staging/gdm72xx/gdm_qos.h
new file mode 100644
index 00000000000..33f2bd4cee3
--- /dev/null
+++ b/drivers/staging/gdm72xx/gdm_qos.h
@@ -0,0 +1,93 @@
+/*
+ * Copyright (c) 2012 GCT Semiconductor, Inc. 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
+ * 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.
+ */
+
+#if !defined(GDM_QOS_H_20090403)
+#define GDM_QOS_H_20090403
+
+#include <linux/types.h>
+#include <linux/usb.h>
+#include <linux/list.h>
+
+#define BOOLEAN	u8
+
+#define QOS_MAX						16
+#define IPTYPEOFSERVICE				0x8000
+#define	PROTOCOL				0x4000
+#define	IPMASKEDSRCADDRESS			0x2000
+#define	IPMASKEDDSTADDRESS			0x1000
+#define	PROTOCOLSRCPORTRANGE		0x800
+#define	PROTOCOLDSTPORTRANGE		0x400
+#define	DSTMACADDR					0x200
+#define	SRCMACADDR					0x100
+#define	ETHERTYPE					0x80
+#define	IEEE802_1DUSERPRIORITY		0x40
+#define	IEEE802_1QVLANID			0x10
+
+struct gdm_wimax_csr_s {
+	/*	union{
+		U16 all;
+		struct _CS_CLASSIFIER_RULE_ENABLE{
+			IPTypeOfService:1,
+			Protocol:1,
+			IPMaskedSrcAddress:1,
+			IPMaskedDstAddress:1,
+			ProtocolSrcPortRange:1,
+			ProtocolDstPortRange:1,
+			DstMacAddr:1,
+			SrcMacAddr:1,
+			Ethertype:1,
+			IEEE802_1DUserPriority:1,
+			IEEE802_1QVLANID:1,
+			Reserved:5;
+		} fields;
+	} */
+	BOOLEAN		Enabled;
+	u32			SFID;
+	u8			QoSBufCount;
+	u16		ClassifierRuleEnable;
+	u8			IPToSLow;
+	u8			IPToSHigh;
+	u8			IPToSMask;
+	u8			Protocol;
+	u8			IPSrcAddr[16];
+	u8			IPSrcAddrMask[16];
+	u8			IPDstAddr[16];
+	u8			IPDstAddrMask[16];
+	u16		SrcPortLow;
+	u16		SrcPortHigh;
+	u16		DstPortLow;
+	u16		DstPortHigh;
+};
+
+struct qos_entry_s {
+	struct list_head list;
+	struct sk_buff *skb;
+	struct net_device *dev;
+
+};
+
+struct qos_cb_s {
+	struct list_head	qos_list[QOS_MAX];
+	u32			qos_list_cnt;
+	u32			qos_null_idx;
+	struct gdm_wimax_csr_s	csr[QOS_MAX];
+	spinlock_t	qos_lock;
+	u32			qos_limit_size;
+};
+
+void gdm_qos_init(void *nic_ptr);
+void gdm_qos_release_list(void *nic_ptr);
+int gdm_qos_send_hci_pkt(struct sk_buff *skb, struct net_device *dev);
+void gdm_recv_qos_hci_packet(void *nic_ptr, u8 *buf, int size);
+
+#endif
diff --git a/drivers/staging/gdm72xx/gdm_sdio.c b/drivers/staging/gdm72xx/gdm_sdio.c
new file mode 100644
index 00000000000..e1a3dd2fc4a
--- /dev/null
+++ b/drivers/staging/gdm72xx/gdm_sdio.c
@@ -0,0 +1,754 @@
+/*
+ * Copyright (c) 2012 GCT Semiconductor, Inc. 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
+ * 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/init.h>
+#include <linux/kernel.h>
+#include <linux/version.h>
+
+#include <linux/mmc/core.h>
+#include <linux/mmc/card.h>
+#include <linux/mmc/sdio_func.h>
+#include <linux/mmc/sdio_ids.h>
+
+#include "gdm_sdio.h"
+#include "gdm_wimax.h"
+#include "sdio_boot.h"
+#include "hci.h"
+
+#define TYPE_A_HEADER_SIZE	4
+#define TYPE_A_LOOKAHEAD_SIZE	16
+
+#define MAX_NR_RX_BUF	4
+
+#define SDU_TX_BUF_SIZE	2048
+#define TX_BUF_SIZE		2048
+#define TX_CHUNK_SIZE	(2048 - TYPE_A_HEADER_SIZE)
+#define RX_BUF_SIZE		(25*1024)
+
+#define TX_HZ	2000
+#define TX_INTERVAL	(1000000/TX_HZ)
+
+/*#define DEBUG*/
+
+static int init_sdio(struct sdiowm_dev *sdev);
+static void release_sdio(struct sdiowm_dev *sdev);
+
+#ifdef DEBUG
+static void hexdump(char *title, u8 *data, int len)
+{
+	int i;
+
+	printk(KERN_DEBUG "%s: length = %d\n", title, len);
+	for (i = 0; i < len; i++) {
+		printk(KERN_DEBUG "%02x ", data[i]);
+		if ((i & 0xf) == 0xf)
+			printk(KERN_DEBUG "\n");
+	}
+	printk(KERN_DEBUG "\n");
+}
+#endif
+
+static struct sdio_tx *alloc_tx_struct(struct tx_cxt *tx)
+{
+	struct sdio_tx *t = NULL;
+
+	t = kmalloc(sizeof(*t), GFP_ATOMIC);
+	if (t == NULL)
+		goto out;
+
+	memset(t, 0, sizeof(*t));
+
+	t->buf = kmalloc(TX_BUF_SIZE, GFP_ATOMIC);
+	if (t->buf == NULL)
+		goto out;
+
+	t->tx_cxt = tx;
+
+	return t;
+out:
+	if (t) {
+		kfree(t->buf);
+		kfree(t);
+	}
+	return NULL;
+}
+
+static void free_tx_struct(struct sdio_tx *t)
+{
+	if (t) {
+		kfree(t->buf);
+		kfree(t);
+	}
+}
+
+static struct sdio_rx *alloc_rx_struct(struct rx_cxt *rx)
+{
+	struct sdio_rx *r = NULL;
+
+	r = kmalloc(sizeof(*r), GFP_ATOMIC);
+	if (r == NULL)
+		goto out;
+
+	memset(r, 0, sizeof(*r));
+
+	r->rx_cxt = rx;
+
+	return r;
+out:
+	kfree(r);
+	return NULL;
+}
+
+static void free_rx_struct(struct sdio_rx *r)
+{
+	kfree(r);
+}
+
+/* Before this function is called, spin lock should be locked. */
+static struct sdio_tx *get_tx_struct(struct tx_cxt *tx, int *no_spc)
+{
+	struct sdio_tx *t;
+
+	if (list_empty(&tx->free_list))
+		return NULL;
+
+	t = list_entry(tx->free_list.prev, struct sdio_tx, list);
+	list_del(&t->list);
+
+	*no_spc = list_empty(&tx->free_list) ? 1 : 0;
+
+	return t;
+}
+
+/* Before this function is called, spin lock should be locked. */
+static void put_tx_struct(struct tx_cxt *tx, struct sdio_tx *t)
+{
+	list_add_tail(&t->list, &tx->free_list);
+}
+
+/* Before this function is called, spin lock should be locked. */
+static struct sdio_rx *get_rx_struct(struct rx_cxt *rx)
+{
+	struct sdio_rx *r;
+
+	if (list_empty(&rx->free_list))
+		return NULL;
+
+	r = list_entry(rx->free_list.prev, struct sdio_rx, list);
+	list_del(&r->list);
+
+	return r;
+}
+
+/* Before this function is called, spin lock should be locked. */
+static void put_rx_struct(struct rx_cxt *rx, struct sdio_rx *r)
+{
+	list_add_tail(&r->list, &rx->free_list);
+}
+
+static int init_sdio(struct sdiowm_dev *sdev)
+{
+	int ret = 0, i;
+	struct tx_cxt	*tx = &sdev->tx;
+	struct rx_cxt	*rx = &sdev->rx;
+	struct sdio_tx	*t;
+	struct sdio_rx	*r;
+
+	INIT_LIST_HEAD(&tx->free_list);
+	INIT_LIST_HEAD(&tx->sdu_list);
+	INIT_LIST_HEAD(&tx->hci_list);
+
+	spin_lock_init(&tx->lock);
+
+	tx->sdu_buf = kmalloc(SDU_TX_BUF_SIZE, GFP_KERNEL);
+	if (tx->sdu_buf == NULL) {
+		printk(KERN_ERR "Failed to allocate SDU tx buffer.\n");
+		goto fail;
+	}
+
+	for (i = 0; i < MAX_NR_SDU_BUF; i++) {
+		t = alloc_tx_struct(tx);
+		if (t == NULL) {
+			ret = -ENOMEM;
+			goto fail;
+		}
+		list_add(&t->list, &tx->free_list);
+	}
+
+	INIT_LIST_HEAD(&rx->free_list);
+	INIT_LIST_HEAD(&rx->req_list);
+
+	spin_lock_init(&rx->lock);
+
+	for (i = 0; i < MAX_NR_RX_BUF; i++) {
+		r = alloc_rx_struct(rx);
+		if (r == NULL) {
+			ret = -ENOMEM;
+			goto fail;
+		}
+		list_add(&r->list, &rx->free_list);
+	}
+
+	rx->rx_buf = kmalloc(RX_BUF_SIZE, GFP_KERNEL);
+	if (rx->rx_buf == NULL) {
+		printk(KERN_ERR "Failed to allocate rx buffer.\n");
+		goto fail;
+	}
+
+	return 0;
+
+fail:
+	release_sdio(sdev);
+	return ret;
+}
+
+static void release_sdio(struct sdiowm_dev *sdev)
+{
+	struct tx_cxt	*tx = &sdev->tx;
+	struct rx_cxt	*rx = &sdev->rx;
+	struct sdio_tx	*t, *t_next;
+	struct sdio_rx	*r, *r_next;
+
+	kfree(tx->sdu_buf);
+
+	list_for_each_entry_safe(t, t_next, &tx->free_list, list) {
+		list_del(&t->list);
+		free_tx_struct(t);
+	}
+
+	list_for_each_entry_safe(t, t_next, &tx->sdu_list, list) {
+		list_del(&t->list);
+		free_tx_struct(t);
+	}
+
+	list_for_each_entry_safe(t, t_next, &tx->hci_list, list) {
+		list_del(&t->list);
+		free_tx_struct(t);
+	}
+
+	kfree(rx->rx_buf);
+
+	list_for_each_entry_safe(r, r_next, &rx->free_list, list) {
+		list_del(&r->list);
+		free_rx_struct(r);
+	}
+
+	list_for_each_entry_safe(r, r_next, &rx->req_list, list) {
+		list_del(&r->list);
+		free_rx_struct(r);
+	}
+}
+
+static void send_sdio_pkt(struct sdio_func *func, u8 *data, int len)
+{
+	int n, blocks, ret, remain;
+
+	sdio_claim_host(func);
+
+	blocks = len / func->cur_blksize;
+	n = blocks * func->cur_blksize;
+	if (blocks) {
+		ret = sdio_memcpy_toio(func, 0, data, n);
+		if (ret < 0) {
+			if (ret != -ENOMEDIUM)
+				printk(KERN_ERR "gdmwms: %s error: ret = %d\n",
+					__func__, ret);
+			goto end_io;
+		}
+	}
+
+	remain = len - n;
+	remain = (remain + 3) & ~3;
+
+	if (remain) {
+		ret = sdio_memcpy_toio(func, 0, data + n, remain);
+		if (ret < 0) {
+			if (ret != -ENOMEDIUM)
+				printk(KERN_ERR "gdmwms: %s error: ret = %d\n",
+					__func__, ret);
+			goto end_io;
+		}
+	}
+
+end_io:
+	sdio_release_host(func);
+}
+
+static void send_sdu(struct sdio_func *func, struct tx_cxt *tx)
+{
+	struct list_head *l, *next;
+	struct hci_s *hci;
+	struct sdio_tx *t;
+	int pos, len, i, estlen, aggr_num = 0, aggr_len;
+	u8 *buf;
+	unsigned long flags;
+
+	spin_lock_irqsave(&tx->lock, flags);
+
+	pos = TYPE_A_HEADER_SIZE + HCI_HEADER_SIZE;
+	list_for_each_entry(t, &tx->sdu_list, list) {
+		estlen = ((t->len + 3) & ~3) + 4;
+		if ((pos + estlen) > SDU_TX_BUF_SIZE)
+			break;
+
+		aggr_num++;
+		memcpy(tx->sdu_buf + pos, t->buf, t->len);
+		memset(tx->sdu_buf + pos + t->len, 0, estlen - t->len);
+		pos += estlen;
+	}
+	aggr_len = pos;
+
+	hci = (struct hci_s *)(tx->sdu_buf + TYPE_A_HEADER_SIZE);
+	hci->cmd_evt = H2B(WIMAX_TX_SDU_AGGR);
+	hci->length = H2B(aggr_len - TYPE_A_HEADER_SIZE - HCI_HEADER_SIZE);
+
+	spin_unlock_irqrestore(&tx->lock, flags);
+
+#ifdef DEBUG
+	hexdump("sdio_send", tx->sdu_buf + TYPE_A_HEADER_SIZE,
+		aggr_len - TYPE_A_HEADER_SIZE);
+#endif
+
+	for (pos = TYPE_A_HEADER_SIZE; pos < aggr_len; pos += TX_CHUNK_SIZE) {
+		len = aggr_len - pos;
+		len = len > TX_CHUNK_SIZE ? TX_CHUNK_SIZE : len;
+		buf = tx->sdu_buf + pos - TYPE_A_HEADER_SIZE;
+
+		buf[0] = len & 0xff;
+		buf[1] = (len >> 8) & 0xff;
+		buf[2] = (len >> 16) & 0xff;
+		buf[3] = (pos + len) >= aggr_len ? 0 : 1;
+		send_sdio_pkt(func, buf, len + TYPE_A_HEADER_SIZE);
+	}
+
+	spin_lock_irqsave(&tx->lock, flags);
+
+	for (l = tx->sdu_list.next, i = 0; i < aggr_num; i++, l = next) {
+		next = l->next;
+		t = list_entry(l, struct sdio_tx, list);
+		if (t->callback)
+			t->callback(t->cb_data);
+
+		list_del(l);
+		put_tx_struct(t->tx_cxt, t);
+	}
+
+	do_gettimeofday(&tx->sdu_stamp);
+	spin_unlock_irqrestore(&tx->lock, flags);
+}
+
+static void send_hci(struct sdio_func *func, struct tx_cxt *tx,
+			struct sdio_tx *t)
+{
+	unsigned long flags;
+
+#ifdef DEBUG
+	hexdump("sdio_send", t->buf + TYPE_A_HEADER_SIZE,
+		t->len - TYPE_A_HEADER_SIZE);
+#endif
+	send_sdio_pkt(func, t->buf, t->len);
+
+	spin_lock_irqsave(&tx->lock, flags);
+	if (t->callback)
+		t->callback(t->cb_data);
+	free_tx_struct(t);
+	spin_unlock_irqrestore(&tx->lock, flags);
+}
+
+static void do_tx(struct work_struct *work)
+{
+	struct sdiowm_dev *sdev = container_of(work, struct sdiowm_dev, ws);
+	struct sdio_func *func = sdev->func;
+	struct tx_cxt *tx = &sdev->tx;
+	struct sdio_tx *t = NULL;
+	struct timeval now, *before;
+	int is_sdu = 0;
+	long diff;
+	unsigned long flags;
+
+	spin_lock_irqsave(&tx->lock, flags);
+	if (!tx->can_send) {
+		spin_unlock_irqrestore(&tx->lock, flags);
+		return;
+	}
+
+	if (!list_empty(&tx->hci_list)) {
+		t = list_entry(tx->hci_list.next, struct sdio_tx, list);
+		list_del(&t->list);
+		is_sdu = 0;
+	} else if (!tx->stop_sdu_tx && !list_empty(&tx->sdu_list)) {
+		do_gettimeofday(&now);
+		before = &tx->sdu_stamp;
+
+		diff = (now.tv_sec - before->tv_sec) * 1000000 +
+			(now.tv_usec - before->tv_usec);
+		if (diff >= 0 && diff < TX_INTERVAL) {
+			schedule_work(&sdev->ws);
+			spin_unlock_irqrestore(&tx->lock, flags);
+			return;
+		}
+		is_sdu = 1;
+	}
+
+	if (!is_sdu && t == NULL) {
+		spin_unlock_irqrestore(&tx->lock, flags);
+		return;
+	}
+
+	tx->can_send = 0;
+
+	spin_unlock_irqrestore(&tx->lock, flags);
+
+	if (is_sdu)
+		send_sdu(func, tx);
+	else
+		send_hci(func, tx, t);
+}
+
+static int gdm_sdio_send(void *priv_dev, void *data, int len,
+			void (*cb)(void *data), void *cb_data)
+{
+	struct sdiowm_dev *sdev = priv_dev;
+	struct tx_cxt *tx = &sdev->tx;
+	struct sdio_tx *t;
+	u8 *pkt = data;
+	int no_spc = 0;
+	u16 cmd_evt;
+	unsigned long flags;
+
+	BUG_ON(len > TX_BUF_SIZE - TYPE_A_HEADER_SIZE);
+
+	spin_lock_irqsave(&tx->lock, flags);
+
+	cmd_evt = (pkt[0] << 8) | pkt[1];
+	if (cmd_evt == WIMAX_TX_SDU) {
+		t = get_tx_struct(tx, &no_spc);
+		if (t == NULL) {
+			/* This case must not happen. */
+			spin_unlock_irqrestore(&tx->lock, flags);
+			return -ENOSPC;
+		}
+		list_add_tail(&t->list, &tx->sdu_list);
+
+		memcpy(t->buf, data, len);
+
+		t->len = len;
+		t->callback = cb;
+		t->cb_data = cb_data;
+	} else {
+		t = alloc_tx_struct(tx);
+		if (t == NULL) {
+			spin_unlock_irqrestore(&tx->lock, flags);
+			return -ENOMEM;
+		}
+		list_add_tail(&t->list, &tx->hci_list);
+
+		t->buf[0] = len & 0xff;
+		t->buf[1] = (len >> 8) & 0xff;
+		t->buf[2] = (len >> 16) & 0xff;
+		t->buf[3] = 2;
+		memcpy(t->buf + TYPE_A_HEADER_SIZE, data, len);
+
+		t->len = len + TYPE_A_HEADER_SIZE;
+		t->callback = cb;
+		t->cb_data = cb_data;
+	}
+
+	if (tx->can_send)
+		schedule_work(&sdev->ws);
+
+	spin_unlock_irqrestore(&tx->lock, flags);
+
+	if (no_spc)
+		return -ENOSPC;
+
+	return 0;
+}
+
+/*
+ * Handle the HCI, WIMAX_SDU_TX_FLOW.
+ */
+static int control_sdu_tx_flow(struct sdiowm_dev *sdev, u8 *hci_data, int len)
+{
+	struct tx_cxt *tx = &sdev->tx;
+	u16 cmd_evt;
+	unsigned long flags;
+
+	spin_lock_irqsave(&tx->lock, flags);
+
+	cmd_evt = (hci_data[0] << 8) | (hci_data[1]);
+	if (cmd_evt != WIMAX_SDU_TX_FLOW)
+		goto out;
+
+	if (hci_data[4] == 0) {
+#ifdef DEBUG
+		printk(KERN_DEBUG "WIMAX ==> STOP SDU TX\n");
+#endif
+		tx->stop_sdu_tx = 1;
+	} else if (hci_data[4] == 1) {
+#ifdef DEBUG
+		printk(KERN_DEBUG "WIMAX ==> START SDU TX\n");
+#endif
+		tx->stop_sdu_tx = 0;
+		if (tx->can_send)
+			schedule_work(&sdev->ws);
+		/*
+		 * If free buffer for sdu tx doesn't exist, then tx queue
+		 * should not be woken. For this reason, don't pass the command,
+		 * START_SDU_TX.
+		 */
+		if (list_empty(&tx->free_list))
+			len = 0;
+	}
+
+out:
+	spin_unlock_irqrestore(&tx->lock, flags);
+	return len;
+}
+
+static void gdm_sdio_irq(struct sdio_func *func)
+{
+	struct phy_dev *phy_dev = sdio_get_drvdata(func);
+	struct sdiowm_dev *sdev = phy_dev->priv_dev;
+	struct tx_cxt *tx = &sdev->tx;
+	struct rx_cxt *rx = &sdev->rx;
+	struct sdio_rx *r;
+	unsigned long flags;
+	u8 val, hdr[TYPE_A_LOOKAHEAD_SIZE], *buf;
+	u32 len, blocks, n;
+	int ret, remain;
+
+	/* Check interrupt */
+	val = sdio_readb(func, 0x13, &ret);
+	if (val & 0x01)
+		sdio_writeb(func, 0x01, 0x13, &ret);	/* clear interrupt */
+	else
+		return;
+
+	ret = sdio_memcpy_fromio(func, hdr, 0x0, TYPE_A_LOOKAHEAD_SIZE);
+	if (ret) {
+		printk(KERN_ERR "Cannot read from function %d\n", func->num);
+		goto done;
+	}
+
+	len = (hdr[2] << 16) | (hdr[1] << 8) | hdr[0];
+	if (len > (RX_BUF_SIZE - TYPE_A_HEADER_SIZE)) {
+		printk(KERN_ERR "Too big Type-A size: %d\n", len);
+		goto done;
+	}
+
+	if (hdr[3] == 1) {	/* Ack */
+#ifdef DEBUG
+		u32 *ack_seq = (u32 *)&hdr[4];
+#endif
+		spin_lock_irqsave(&tx->lock, flags);
+		tx->can_send = 1;
+
+		if (!list_empty(&tx->sdu_list) || !list_empty(&tx->hci_list))
+			schedule_work(&sdev->ws);
+		spin_unlock_irqrestore(&tx->lock, flags);
+#ifdef DEBUG
+		printk(KERN_DEBUG "Ack... %0x\n", ntohl(*ack_seq));
+#endif
+		goto done;
+	}
+
+	memcpy(rx->rx_buf, hdr + TYPE_A_HEADER_SIZE,
+			TYPE_A_LOOKAHEAD_SIZE - TYPE_A_HEADER_SIZE);
+
+	buf = rx->rx_buf + TYPE_A_LOOKAHEAD_SIZE - TYPE_A_HEADER_SIZE;
+	remain = len - TYPE_A_LOOKAHEAD_SIZE + TYPE_A_HEADER_SIZE;
+	if (remain <= 0)
+		goto end_io;
+
+	blocks = remain / func->cur_blksize;
+
+	if (blocks) {
+		n = blocks * func->cur_blksize;
+		ret = sdio_memcpy_fromio(func, buf, 0x0, n);
+		if (ret) {
+			printk(KERN_ERR "Cannot read from function %d\n",
+				func->num);
+			goto done;
+		}
+		buf += n;
+		remain -= n;
+	}
+
+	if (remain) {
+		ret = sdio_memcpy_fromio(func, buf, 0x0, remain);
+		if (ret) {
+			printk(KERN_ERR "Cannot read from function %d\n",
+				func->num);
+			goto done;
+		}
+	}
+
+end_io:
+#ifdef DEBUG
+	hexdump("sdio_receive", rx->rx_buf, len);
+#endif
+	len = control_sdu_tx_flow(sdev, rx->rx_buf, len);
+
+	spin_lock_irqsave(&rx->lock, flags);
+
+	if (!list_empty(&rx->req_list)) {
+		r = list_entry(rx->req_list.next, struct sdio_rx, list);
+		spin_unlock_irqrestore(&rx->lock, flags);
+		if (r->callback)
+			r->callback(r->cb_data, rx->rx_buf, len);
+		spin_lock_irqsave(&rx->lock, flags);
+		list_del(&r->list);
+		put_rx_struct(rx, r);
+	}
+
+	spin_unlock_irqrestore(&rx->lock, flags);
+
+done:
+	sdio_writeb(func, 0x00, 0x10, &ret);	/* PCRRT */
+	if (!phy_dev->netdev)
+		register_wimax_device(phy_dev, &func->dev);
+}
+
+static int gdm_sdio_receive(void *priv_dev,
+				void (*cb)(void *cb_data, void *data, int len),
+				void *cb_data)
+{
+	struct sdiowm_dev *sdev = priv_dev;
+	struct rx_cxt *rx = &sdev->rx;
+	struct sdio_rx *r;
+	unsigned long flags;
+
+	spin_lock_irqsave(&rx->lock, flags);
+	r = get_rx_struct(rx);
+	if (r == NULL) {
+		spin_unlock_irqrestore(&rx->lock, flags);
+		return -ENOMEM;
+	}
+
+	r->callback = cb;
+	r->cb_data = cb_data;
+
+	list_add_tail(&r->list, &rx->req_list);
+	spin_unlock_irqrestore(&rx->lock, flags);
+
+	return 0;
+}
+
+static int sdio_wimax_probe(struct sdio_func *func,
+				const struct sdio_device_id *id)
+{
+	int ret;
+	struct phy_dev *phy_dev = NULL;
+	struct sdiowm_dev *sdev = NULL;
+
+	printk(KERN_INFO "Found GDM SDIO VID = 0x%04x PID = 0x%04x...\n",
+			func->vendor, func->device);
+	printk(KERN_INFO "GCT WiMax driver version %s\n", DRIVER_VERSION);
+
+	sdio_claim_host(func);
+	sdio_enable_func(func);
+	sdio_claim_irq(func, gdm_sdio_irq);
+
+	ret = sdio_boot(func);
+	if (ret)
+		return ret;
+
+	phy_dev = kmalloc(sizeof(*phy_dev), GFP_KERNEL);
+	if (phy_dev == NULL) {
+		ret = -ENOMEM;
+		goto out;
+	}
+	sdev = kmalloc(sizeof(*sdev), GFP_KERNEL);
+	if (sdev == NULL) {
+		ret = -ENOMEM;
+		goto out;
+	}
+
+	memset(phy_dev, 0, sizeof(*phy_dev));
+	memset(sdev, 0, sizeof(*sdev));
+
+	phy_dev->priv_dev = (void *)sdev;
+	phy_dev->send_func = gdm_sdio_send;
+	phy_dev->rcv_func = gdm_sdio_receive;
+
+	ret = init_sdio(sdev);
+	if (sdev < 0)
+		goto out;
+
+	sdev->func = func;
+
+	sdio_writeb(func, 1, 0x14, &ret);	/* Enable interrupt */
+	sdio_release_host(func);
+
+	INIT_WORK(&sdev->ws, do_tx);
+
+	sdio_set_drvdata(func, phy_dev);
+out:
+	if (ret) {
+		kfree(phy_dev);
+		kfree(sdev);
+	}
+
+	return ret;
+}
+
+static void sdio_wimax_remove(struct sdio_func *func)
+{
+	struct phy_dev *phy_dev = sdio_get_drvdata(func);
+	struct sdiowm_dev *sdev = phy_dev->priv_dev;
+
+	if (phy_dev->netdev)
+		unregister_wimax_device(phy_dev);
+	sdio_claim_host(func);
+	sdio_release_irq(func);
+	sdio_disable_func(func);
+	sdio_release_host(func);
+	release_sdio(sdev);
+
+	kfree(sdev);
+	kfree(phy_dev);
+}
+
+static const struct sdio_device_id sdio_wimax_ids[] = {
+	{ SDIO_DEVICE(0x0296, 0x5347) },
+	{0}
+};
+
+MODULE_DEVICE_TABLE(sdio, sdio_wimax_ids);
+
+static struct sdio_driver sdio_wimax_driver = {
+	.probe		= sdio_wimax_probe,
+	.remove		= sdio_wimax_remove,
+	.name		= "sdio_wimax",
+	.id_table	= sdio_wimax_ids,
+};
+
+static int __init sdio_gdm_wimax_init(void)
+{
+	return sdio_register_driver(&sdio_wimax_driver);
+}
+
+static void __exit sdio_gdm_wimax_exit(void)
+{
+	sdio_unregister_driver(&sdio_wimax_driver);
+}
+
+module_init(sdio_gdm_wimax_init);
+module_exit(sdio_gdm_wimax_exit);
+
+MODULE_VERSION(DRIVER_VERSION);
+MODULE_DESCRIPTION("GCT WiMax SDIO Device Driver");
+MODULE_AUTHOR("Ethan Park");
+MODULE_LICENSE("GPL");
diff --git a/drivers/staging/gdm72xx/gdm_sdio.h b/drivers/staging/gdm72xx/gdm_sdio.h
new file mode 100644
index 00000000000..216e98f31ba
--- /dev/null
+++ b/drivers/staging/gdm72xx/gdm_sdio.h
@@ -0,0 +1,72 @@
+/*
+ * Copyright (c) 2012 GCT Semiconductor, Inc. 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
+ * 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.
+ */
+
+#ifndef __GDM_SDIO_H__
+#define __GDM_SDIO_H__
+
+#include <linux/types.h>
+#include <linux/time.h>
+
+#define MAX_NR_SDU_BUF  64
+
+struct sdio_tx {
+	struct list_head	list;
+	struct tx_cxt		*tx_cxt;
+
+	u8	*buf;
+	int	len;
+
+	void (*callback)(void *cb_data);
+	void *cb_data;
+};
+
+struct tx_cxt {
+	struct list_head	free_list;
+	struct list_head	sdu_list;
+	struct list_head	hci_list;
+	struct timeval		sdu_stamp;
+
+	u8	*sdu_buf;
+
+	spinlock_t			lock;
+	int	can_send;
+	int stop_sdu_tx;
+};
+
+struct sdio_rx {
+	struct list_head	list;
+	struct rx_cxt		*rx_cxt;
+
+	void (*callback)(void *cb_data, void *data, int len);
+	void *cb_data;
+};
+
+struct rx_cxt {
+	struct list_head	free_list;
+	struct list_head	req_list;
+
+	u8		*rx_buf;
+
+	spinlock_t			lock;
+};
+
+struct sdiowm_dev {
+	struct sdio_func	*func;
+
+	struct tx_cxt	tx;
+	struct rx_cxt	rx;
+
+	struct work_struct	ws;
+};
+
+#endif /* __GDM_SDIO_H__ */
diff --git a/drivers/staging/gdm72xx/gdm_usb.c b/drivers/staging/gdm72xx/gdm_usb.c
new file mode 100644
index 00000000000..1e9dc0d9036
--- /dev/null
+++ b/drivers/staging/gdm72xx/gdm_usb.c
@@ -0,0 +1,798 @@
+/*
+ * Copyright (c) 2012 GCT Semiconductor, Inc. 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
+ * 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/version.h>
+#include <linux/kernel.h>
+#include <linux/usb.h>
+#include <asm/byteorder.h>
+
+#include "gdm_usb.h"
+#include "gdm_wimax.h"
+#include "usb_boot.h"
+#include "hci.h"
+
+#include "usb_ids.h"
+
+MODULE_DEVICE_TABLE(usb, id_table);
+
+#define TX_BUF_SIZE	2048
+#if defined(CONFIG_WIMAX_GDM72XX_WIMAX2)
+#define RX_BUF_SIZE	(128*1024)	/* For packet aggregation */
+#else
+#define RX_BUF_SIZE	2048
+#endif
+
+#define GDM7205_PADDING		256
+
+#define H2B(x)		__cpu_to_be16(x)
+#define B2H(x)		__be16_to_cpu(x)
+#define DB2H(x)		__be32_to_cpu(x)
+
+#define DOWNLOAD_CONF_VALUE		0x21
+
+#ifdef CONFIG_WIMAX_GDM72XX_K_MODE
+
+static DECLARE_WAIT_QUEUE_HEAD(k_wait);
+static LIST_HEAD(k_list);
+static DEFINE_SPINLOCK(k_lock);
+static int k_mode_stop;
+
+#define K_WAIT_TIME	(2 * HZ / 100)
+
+#endif /* CONFIG_WIMAX_GDM72XX_K_MODE */
+
+static int init_usb(struct usbwm_dev *udev);
+static void release_usb(struct usbwm_dev *udev);
+
+/*#define DEBUG */
+#ifdef DEBUG
+static void hexdump(char *title, u8 *data, int len)
+{
+	int i;
+
+	printk(KERN_DEBUG "%s: length = %d\n", title, len);
+	for (i = 0; i < len; i++) {
+		printk(KERN_DEBUG "%02x ", data[i]);
+		if ((i & 0xf) == 0xf)
+			printk(KERN_DEBUG "\n");
+	}
+	printk(KERN_DEBUG "\n");
+}
+#endif
+
+static struct usb_tx *alloc_tx_struct(struct tx_cxt *tx)
+{
+	struct usb_tx *t = NULL;
+
+	t = kmalloc(sizeof(*t), GFP_ATOMIC);
+	if (t == NULL)
+		goto out;
+
+	memset(t, 0, sizeof(*t));
+
+	t->urb = usb_alloc_urb(0, GFP_ATOMIC);
+	t->buf = kmalloc(TX_BUF_SIZE, GFP_ATOMIC);
+	if (t->urb == NULL || t->buf == NULL)
+		goto out;
+
+	t->tx_cxt = tx;
+
+	return t;
+out:
+	if (t) {
+		usb_free_urb(t->urb);
+		kfree(t->buf);
+		kfree(t);
+	}
+	return NULL;
+}
+
+static void free_tx_struct(struct usb_tx *t)
+{
+	if (t) {
+		usb_free_urb(t->urb);
+		kfree(t->buf);
+		kfree(t);
+	}
+}
+
+static struct usb_rx *alloc_rx_struct(struct rx_cxt *rx)
+{
+	struct usb_rx *r = NULL;
+
+	r = kmalloc(sizeof(*r), GFP_ATOMIC);
+	if (r == NULL)
+		goto out;
+
+	memset(r, 0, sizeof(*r));
+
+	r->urb = usb_alloc_urb(0, GFP_ATOMIC);
+	r->buf = kmalloc(RX_BUF_SIZE, GFP_ATOMIC);
+	if (r->urb == NULL || r->buf == NULL)
+		goto out;
+
+	r->rx_cxt = rx;
+	return r;
+out:
+	if (r) {
+		usb_free_urb(r->urb);
+		kfree(r->buf);
+		kfree(r);
+	}
+	return NULL;
+}
+
+static void free_rx_struct(struct usb_rx *r)
+{
+	if (r) {
+		usb_free_urb(r->urb);
+		kfree(r->buf);
+		kfree(r);
+	}
+}
+
+/* Before this function is called, spin lock should be locked. */
+static struct usb_tx *get_tx_struct(struct tx_cxt *tx, int *no_spc)
+{
+	struct usb_tx *t;
+
+	if (list_empty(&tx->free_list)) {
+		*no_spc = 1;
+		return NULL;
+	}
+
+	t = list_entry(tx->free_list.next, struct usb_tx, list);
+	list_del(&t->list);
+
+	*no_spc = list_empty(&tx->free_list) ? 1 : 0;
+
+	return t;
+}
+
+/* Before this function is called, spin lock should be locked. */
+static void put_tx_struct(struct tx_cxt *tx, struct usb_tx *t)
+{
+	list_add_tail(&t->list, &tx->free_list);
+}
+
+/* Before this function is called, spin lock should be locked. */
+static struct usb_rx *get_rx_struct(struct rx_cxt *rx)
+{
+	struct usb_rx *r;
+
+	if (list_empty(&rx->free_list)) {
+		r = alloc_rx_struct(rx);
+		if (r == NULL)
+			return NULL;
+
+		list_add(&r->list, &rx->free_list);
+	}
+
+	r = list_entry(rx->free_list.next, struct usb_rx, list);
+	list_del(&r->list);
+	list_add_tail(&r->list, &rx->used_list);
+
+	return r;
+}
+
+/* Before this function is called, spin lock should be locked. */
+static void put_rx_struct(struct rx_cxt *rx, struct usb_rx *r)
+{
+	list_del(&r->list);
+	list_add(&r->list, &rx->free_list);
+}
+
+static int init_usb(struct usbwm_dev *udev)
+{
+	int ret = 0, i;
+	struct tx_cxt	*tx = &udev->tx;
+	struct rx_cxt	*rx = &udev->rx;
+	struct usb_tx	*t;
+	struct usb_rx	*r;
+
+	INIT_LIST_HEAD(&tx->free_list);
+	INIT_LIST_HEAD(&tx->sdu_list);
+	INIT_LIST_HEAD(&tx->hci_list);
+#if defined(CONFIG_WIMAX_GDM72XX_USB_PM) || defined(CONFIG_WIMAX_GDM72XX_K_MODE)
+	INIT_LIST_HEAD(&tx->pending_list);
+#endif
+
+	INIT_LIST_HEAD(&rx->free_list);
+	INIT_LIST_HEAD(&rx->used_list);
+
+	spin_lock_init(&tx->lock);
+	spin_lock_init(&rx->lock);
+
+	for (i = 0; i < MAX_NR_SDU_BUF; i++) {
+		t = alloc_tx_struct(tx);
+		if (t == NULL) {
+			ret = -ENOMEM;
+			goto fail;
+		}
+		list_add(&t->list, &tx->free_list);
+	}
+
+	r = alloc_rx_struct(rx);
+	if (r == NULL) {
+		ret = -ENOMEM;
+		goto fail;
+	}
+
+	list_add(&r->list, &rx->free_list);
+	return ret;
+
+fail:
+	release_usb(udev);
+	return ret;
+}
+
+static void release_usb(struct usbwm_dev *udev)
+{
+	struct tx_cxt	*tx = &udev->tx;
+	struct rx_cxt	*rx = &udev->rx;
+	struct usb_tx	*t, *t_next;
+	struct usb_rx	*r, *r_next;
+
+	list_for_each_entry_safe(t, t_next, &tx->sdu_list, list) {
+		list_del(&t->list);
+		free_tx_struct(t);
+	}
+
+	list_for_each_entry_safe(t, t_next, &tx->hci_list, list) {
+		list_del(&t->list);
+		free_tx_struct(t);
+	}
+
+	list_for_each_entry_safe(t, t_next, &tx->free_list, list) {
+		list_del(&t->list);
+		free_tx_struct(t);
+	}
+
+	list_for_each_entry_safe(r, r_next, &rx->free_list, list) {
+		list_del(&r->list);
+		free_rx_struct(r);
+	}
+
+	list_for_each_entry_safe(r, r_next, &rx->used_list, list) {
+		list_del(&r->list);
+		free_rx_struct(r);
+	}
+}
+
+static void gdm_usb_send_complete(struct urb *urb)
+{
+	struct usb_tx *t = urb->context;
+	struct tx_cxt *tx = t->tx_cxt;
+	u8 *pkt = t->buf;
+	u16 cmd_evt;
+	unsigned long flags;
+
+	/* Completion by usb_unlink_urb */
+	if (urb->status == -ECONNRESET)
+		return;
+
+	spin_lock_irqsave(&tx->lock, flags);
+
+	if (t->callback)
+		t->callback(t->cb_data);
+
+	/* Delete from sdu list or hci list. */
+	list_del(&t->list);
+
+	cmd_evt = (pkt[0] << 8) | pkt[1];
+	if (cmd_evt == WIMAX_TX_SDU)
+		put_tx_struct(tx, t);
+	else
+		free_tx_struct(t);
+
+	spin_unlock_irqrestore(&tx->lock, flags);
+}
+
+static int gdm_usb_send(void *priv_dev, void *data, int len,
+			void (*cb)(void *data), void *cb_data)
+{
+	struct usbwm_dev *udev = priv_dev;
+	struct usb_device *usbdev = udev->usbdev;
+	struct tx_cxt *tx = &udev->tx;
+	struct usb_tx *t;
+	int padding = udev->padding;
+	int no_spc = 0, ret;
+	u8 *pkt = data;
+	u16 cmd_evt;
+	unsigned long flags;
+
+	if (!udev->usbdev) {
+		printk(KERN_ERR "%s: No such device\n", __func__);
+		return -ENODEV;
+	}
+
+	BUG_ON(len > TX_BUF_SIZE - padding - 1);
+
+	spin_lock_irqsave(&tx->lock, flags);
+
+	cmd_evt = (pkt[0] << 8) | pkt[1];
+	if (cmd_evt == WIMAX_TX_SDU) {
+		t = get_tx_struct(tx, &no_spc);
+		if (t == NULL) {
+			/* This case must not happen. */
+			spin_unlock_irqrestore(&tx->lock, flags);
+			return -ENOSPC;
+		}
+		list_add_tail(&t->list, &tx->sdu_list);
+	} else {
+		t = alloc_tx_struct(tx);
+		if (t == NULL) {
+			spin_unlock_irqrestore(&tx->lock, flags);
+			return -ENOMEM;
+		}
+		list_add_tail(&t->list, &tx->hci_list);
+	}
+
+	memcpy(t->buf + padding, data, len);
+	t->callback = cb;
+	t->cb_data = cb_data;
+
+	/*
+	 * In some cases, USB Module of WiMax is blocked when data size is
+	 * the multiple of 512. So, increment length by one in that case.
+	 */
+	if ((len % 512) == 0)
+		len++;
+
+	usb_fill_bulk_urb(t->urb,
+			usbdev,
+			usb_sndbulkpipe(usbdev, 1),
+			t->buf,
+			len + padding,
+			gdm_usb_send_complete,
+			t);
+
+#ifdef DEBUG
+	hexdump("usb_send", t->buf, len + padding);
+#endif
+#ifdef CONFIG_WIMAX_GDM72XX_USB_PM
+	if (usbdev->state & USB_STATE_SUSPENDED) {
+		list_add_tail(&t->p_list, &tx->pending_list);
+		schedule_work(&udev->pm_ws);
+		goto out;
+	}
+#endif /* CONFIG_WIMAX_GDM72XX_USB_PM */
+
+#ifdef CONFIG_WIMAX_GDM72XX_K_MODE
+	if (udev->bw_switch) {
+		list_add_tail(&t->p_list, &tx->pending_list);
+		goto out;
+	} else if (cmd_evt == WIMAX_SCAN) {
+		struct rx_cxt *rx;
+		struct usb_rx *r;
+
+		rx = &udev->rx;
+
+		list_for_each_entry(r, &rx->used_list, list)
+			usb_unlink_urb(r->urb);
+		udev->bw_switch = 1;
+
+		spin_lock(&k_lock);
+		list_add_tail(&udev->list, &k_list);
+		spin_unlock(&k_lock);
+
+		wake_up(&k_wait);
+	}
+#endif /* CONFIG_WIMAX_GDM72XX_K_MODE */
+
+	ret = usb_submit_urb(t->urb, GFP_ATOMIC);
+	if (ret)
+		goto send_fail;
+
+#ifdef CONFIG_WIMAX_GDM72XX_USB_PM
+	usb_mark_last_busy(usbdev);
+#endif /* CONFIG_WIMAX_GDM72XX_USB_PM */
+
+#if defined(CONFIG_WIMAX_GDM72XX_USB_PM) || defined(CONFIG_WIMAX_GDM72XX_K_MODE)
+out:
+#endif
+	spin_unlock_irqrestore(&tx->lock, flags);
+
+	if (no_spc)
+		return -ENOSPC;
+
+	return 0;
+
+send_fail:
+	t->callback = NULL;
+	gdm_usb_send_complete(t->urb);
+	spin_unlock_irqrestore(&tx->lock, flags);
+	return ret;
+}
+
+static void gdm_usb_rcv_complete(struct urb *urb)
+{
+	struct usb_rx *r = urb->context;
+	struct rx_cxt *rx = r->rx_cxt;
+	struct usbwm_dev *udev = container_of(r->rx_cxt, struct usbwm_dev, rx);
+	struct tx_cxt *tx = &udev->tx;
+	struct usb_tx *t;
+	u16 cmd_evt;
+	unsigned long flags;
+
+#ifdef CONFIG_WIMAX_GDM72XX_USB_PM
+	struct usb_device *dev = urb->dev;
+#endif
+
+	/* Completion by usb_unlink_urb */
+	if (urb->status == -ECONNRESET)
+		return;
+
+	spin_lock_irqsave(&tx->lock, flags);
+
+	if (!urb->status) {
+		cmd_evt = (r->buf[0] << 8) | (r->buf[1]);
+#ifdef DEBUG
+		hexdump("usb_receive", r->buf, urb->actual_length);
+#endif
+		if (cmd_evt == WIMAX_SDU_TX_FLOW) {
+			if (r->buf[4] == 0) {
+#ifdef DEBUG
+				printk(KERN_DEBUG "WIMAX ==> STOP SDU TX\n");
+#endif
+				list_for_each_entry(t, &tx->sdu_list, list)
+					usb_unlink_urb(t->urb);
+			} else if (r->buf[4] == 1) {
+#ifdef DEBUG
+				printk(KERN_DEBUG "WIMAX ==> START SDU TX\n");
+#endif
+				list_for_each_entry(t, &tx->sdu_list, list) {
+					usb_submit_urb(t->urb, GFP_ATOMIC);
+				}
+				/*
+				 * If free buffer for sdu tx doesn't
+				 * exist, then tx queue should not be
+				 * woken. For this reason, don't pass
+				 * the command, START_SDU_TX.
+				 */
+				if (list_empty(&tx->free_list))
+					urb->actual_length = 0;
+			}
+		}
+	}
+
+	if (!urb->status && r->callback)
+		r->callback(r->cb_data, r->buf, urb->actual_length);
+
+	spin_lock(&rx->lock);
+	put_rx_struct(rx, r);
+	spin_unlock(&rx->lock);
+
+	spin_unlock_irqrestore(&tx->lock, flags);
+
+#ifdef CONFIG_WIMAX_GDM72XX_USB_PM
+	usb_mark_last_busy(dev);
+#endif
+}
+
+static int gdm_usb_receive(void *priv_dev,
+			void (*cb)(void *cb_data, void *data, int len),
+			void *cb_data)
+{
+	struct usbwm_dev *udev = priv_dev;
+	struct usb_device *usbdev = udev->usbdev;
+	struct rx_cxt *rx = &udev->rx;
+	struct usb_rx *r;
+	unsigned long flags;
+
+	if (!udev->usbdev) {
+		printk(KERN_ERR "%s: No such device\n", __func__);
+		return -ENODEV;
+	}
+
+	spin_lock_irqsave(&rx->lock, flags);
+	r = get_rx_struct(rx);
+	spin_unlock_irqrestore(&rx->lock, flags);
+
+	if (r == NULL)
+		return -ENOMEM;
+
+	r->callback = cb;
+	r->cb_data = cb_data;
+
+	usb_fill_bulk_urb(r->urb,
+			usbdev,
+			usb_rcvbulkpipe(usbdev, 0x82),
+			r->buf,
+			RX_BUF_SIZE,
+			gdm_usb_rcv_complete,
+			r);
+
+	return usb_submit_urb(r->urb, GFP_ATOMIC);
+}
+
+#ifdef CONFIG_WIMAX_GDM72XX_USB_PM
+static void do_pm_control(struct work_struct *work)
+{
+	struct usbwm_dev *udev = container_of(work, struct usbwm_dev, pm_ws);
+	struct tx_cxt *tx = &udev->tx;
+	int ret;
+	unsigned long flags;
+
+	ret = usb_autopm_get_interface(udev->intf);
+	if (!ret)
+		usb_autopm_put_interface(udev->intf);
+
+	spin_lock_irqsave(&tx->lock, flags);
+	if (!(udev->usbdev->state & USB_STATE_SUSPENDED)
+		&& (!list_empty(&tx->hci_list) || !list_empty(&tx->sdu_list))) {
+		struct usb_tx *t, *temp;
+
+		list_for_each_entry_safe(t, temp, &tx->pending_list, p_list) {
+			list_del(&t->p_list);
+			ret =  usb_submit_urb(t->urb, GFP_ATOMIC);
+
+			if (ret) {
+				t->callback = NULL;
+				gdm_usb_send_complete(t->urb);
+			}
+		}
+	}
+	spin_unlock_irqrestore(&tx->lock, flags);
+}
+#endif /* CONFIG_WIMAX_GDM72XX_USB_PM */
+
+static int gdm_usb_probe(struct usb_interface *intf,
+				const struct usb_device_id *id)
+{
+	int ret = 0;
+	u8 bConfigurationValue;
+	struct phy_dev *phy_dev = NULL;
+	struct usbwm_dev *udev = NULL;
+	u16 idVendor, idProduct, bcdDevice;
+
+	struct usb_device *usbdev = interface_to_usbdev(intf);
+
+	usb_get_dev(usbdev);
+	bConfigurationValue = usbdev->actconfig->desc.bConfigurationValue;
+
+	/*USB description is set up with Little-Endian*/
+	idVendor = L2H(usbdev->descriptor.idVendor);
+	idProduct = L2H(usbdev->descriptor.idProduct);
+	bcdDevice = L2H(usbdev->descriptor.bcdDevice);
+
+	printk(KERN_INFO "Found GDM USB VID = 0x%04x PID = 0x%04x...\n",
+		idVendor, idProduct);
+	printk(KERN_INFO "GCT WiMax driver version %s\n", DRIVER_VERSION);
+
+
+	if (idProduct == EMERGENCY_PID) {
+		ret = usb_emergency(usbdev);
+		goto out;
+	}
+
+	/* Support for EEPROM bootloader */
+	if (bConfigurationValue == DOWNLOAD_CONF_VALUE ||
+		idProduct & B_DOWNLOAD) {
+		ret = usb_boot(usbdev, bcdDevice);
+		goto out;
+	}
+
+	phy_dev = kmalloc(sizeof(*phy_dev), GFP_KERNEL);
+	if (phy_dev == NULL) {
+		ret = -ENOMEM;
+		goto out;
+	}
+	udev = kmalloc(sizeof(*udev), GFP_KERNEL);
+	if (udev == NULL) {
+		ret = -ENOMEM;
+		goto out;
+	}
+
+	memset(phy_dev, 0, sizeof(*phy_dev));
+	memset(udev, 0, sizeof(*udev));
+
+	if (idProduct == 0x7205 || idProduct == 0x7206)
+		udev->padding = GDM7205_PADDING;
+	else
+		udev->padding = 0;
+
+	phy_dev->priv_dev = (void *)udev;
+	phy_dev->send_func = gdm_usb_send;
+	phy_dev->rcv_func = gdm_usb_receive;
+
+	ret = init_usb(udev);
+	if (ret < 0)
+		goto out;
+
+	udev->usbdev = usbdev;
+
+#ifdef CONFIG_WIMAX_GDM72XX_USB_PM
+	udev->intf = intf;
+
+	intf->needs_remote_wakeup = 1;
+	device_init_wakeup(&intf->dev, 1);
+
+	pm_runtime_set_autosuspend_delay(&usbdev->dev, 10 * 1000); /* msec */
+
+	INIT_WORK(&udev->pm_ws, do_pm_control);
+#endif /* CONFIG_WIMAX_GDM72XX_USB_PM */
+
+	ret = register_wimax_device(phy_dev, &intf->dev);
+
+out:
+	if (ret) {
+		kfree(phy_dev);
+		kfree(udev);
+	}
+	usb_set_intfdata(intf, phy_dev);
+	return ret;
+}
+
+static void gdm_usb_disconnect(struct usb_interface *intf)
+{
+	u8 bConfigurationValue;
+	struct phy_dev *phy_dev;
+	struct usbwm_dev *udev;
+	u16 idProduct;
+	struct usb_device *usbdev = interface_to_usbdev(intf);
+
+	bConfigurationValue = usbdev->actconfig->desc.bConfigurationValue;
+	phy_dev = usb_get_intfdata(intf);
+
+	/*USB description is set up with Little-Endian*/
+	idProduct = L2H(usbdev->descriptor.idProduct);
+
+	if (idProduct != EMERGENCY_PID &&
+			bConfigurationValue != DOWNLOAD_CONF_VALUE &&
+			(idProduct & B_DOWNLOAD) == 0) {
+		udev = phy_dev->priv_dev;
+		udev->usbdev = NULL;
+
+		unregister_wimax_device(phy_dev);
+		release_usb(udev);
+		kfree(udev);
+		kfree(phy_dev);
+	}
+
+	usb_put_dev(usbdev);
+}
+
+#ifdef CONFIG_WIMAX_GDM72XX_USB_PM
+static int gdm_suspend(struct usb_interface *intf, pm_message_t pm_msg)
+{
+	struct phy_dev *phy_dev;
+	struct usbwm_dev *udev;
+	struct rx_cxt *rx;
+	struct usb_rx *r;
+
+	phy_dev = usb_get_intfdata(intf);
+	udev = phy_dev->priv_dev;
+	rx = &udev->rx;
+
+	list_for_each_entry(r, &rx->used_list, list)
+		usb_unlink_urb(r->urb);
+
+	return 0;
+}
+
+static int gdm_resume(struct usb_interface *intf)
+{
+	struct phy_dev *phy_dev;
+	struct usbwm_dev *udev;
+	struct rx_cxt *rx;
+	struct usb_rx *r;
+
+	phy_dev = usb_get_intfdata(intf);
+	udev = phy_dev->priv_dev;
+	rx = &udev->rx;
+
+	list_for_each_entry(r, &rx->used_list, list)
+		usb_submit_urb(r->urb, GFP_ATOMIC);
+
+	return 0;
+}
+
+#endif /* CONFIG_WIMAX_GDM72XX_USB_PM */
+
+#ifdef CONFIG_WIMAX_GDM72XX_K_MODE
+static int k_mode_thread(void *arg)
+{
+	struct usbwm_dev *udev;
+	struct tx_cxt *tx;
+	struct rx_cxt *rx;
+	struct usb_tx *t, *temp;
+	struct usb_rx *r;
+	unsigned long flags, flags2, expire;
+	int ret;
+
+	daemonize("k_mode_wimax");
+
+	while (!k_mode_stop) {
+
+		spin_lock_irqsave(&k_lock, flags2);
+		while (!list_empty(&k_list)) {
+
+			udev = list_entry(k_list.next, struct usbwm_dev, list);
+			tx = &udev->tx;
+			rx = &udev->rx;
+
+			list_del(&udev->list);
+			spin_unlock_irqrestore(&k_lock, flags2);
+
+			expire = jiffies + K_WAIT_TIME;
+			while (jiffies < expire)
+				schedule_timeout(K_WAIT_TIME);
+
+			list_for_each_entry(r, &rx->used_list, list)
+				usb_submit_urb(r->urb, GFP_ATOMIC);
+
+			spin_lock_irqsave(&tx->lock, flags);
+
+			list_for_each_entry_safe(t, temp, &tx->pending_list,
+						p_list) {
+				list_del(&t->p_list);
+				ret = usb_submit_urb(t->urb, GFP_ATOMIC);
+
+				if (ret) {
+					t->callback = NULL;
+					gdm_usb_send_complete(t->urb);
+				}
+			}
+
+			udev->bw_switch = 0;
+			spin_unlock_irqrestore(&tx->lock, flags);
+
+			spin_lock_irqsave(&k_lock, flags2);
+		}
+		spin_unlock_irqrestore(&k_lock, flags2);
+
+		interruptible_sleep_on(&k_wait);
+	}
+	return 0;
+}
+#endif /* CONFIG_WIMAX_GDM72XX_K_MODE */
+
+static struct usb_driver gdm_usb_driver = {
+	.name = "gdm_wimax",
+	.probe = gdm_usb_probe,
+	.disconnect = gdm_usb_disconnect,
+	.id_table = id_table,
+#ifdef CONFIG_WIMAX_GDM72XX_USB_PM
+	.supports_autosuspend = 1,
+	.suspend = gdm_suspend,
+	.resume = gdm_resume,
+	.reset_resume = gdm_resume,
+#endif
+};
+
+static int __init usb_gdm_wimax_init(void)
+{
+#ifdef CONFIG_WIMAX_GDM72XX_K_MODE
+	kernel_thread(k_mode_thread, NULL, CLONE_KERNEL);
+#endif /* CONFIG_WIMAX_GDM72XX_K_MODE */
+	return usb_register(&gdm_usb_driver);
+}
+
+static void __exit usb_gdm_wimax_exit(void)
+{
+#ifdef CONFIG_WIMAX_GDM72XX_K_MODE
+	k_mode_stop = 1;
+	wake_up(&k_wait);
+#endif
+	usb_deregister(&gdm_usb_driver);
+}
+
+module_init(usb_gdm_wimax_init);
+module_exit(usb_gdm_wimax_exit);
+
+MODULE_VERSION(DRIVER_VERSION);
+MODULE_DESCRIPTION("GCT WiMax Device Driver");
+MODULE_AUTHOR("Ethan Park");
+MODULE_LICENSE("GPL");
diff --git a/drivers/staging/gdm72xx/gdm_usb.h b/drivers/staging/gdm72xx/gdm_usb.h
new file mode 100644
index 00000000000..ecb891f6a59
--- /dev/null
+++ b/drivers/staging/gdm72xx/gdm_usb.h
@@ -0,0 +1,85 @@
+/*
+ * Copyright (c) 2012 GCT Semiconductor, Inc. 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
+ * 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.
+ */
+
+#ifndef __GDM_USB_H__
+#define __GDM_USB_H__
+
+#include <linux/types.h>
+#include <linux/usb.h>
+#include <linux/list.h>
+
+#define B_DIFF_DL_DRV		(1<<4)
+#define B_DOWNLOAD			(1 << 5)
+#define MAX_NR_SDU_BUF		64
+
+struct usb_tx {
+	struct list_head	list;
+#if defined(CONFIG_WIMAX_GDM72XX_USB_PM) || defined(CONFIG_WIMAX_GDM72XX_K_MODE)
+	struct list_head	p_list;
+#endif
+	struct tx_cxt		*tx_cxt;
+
+	struct urb	*urb;
+	u8			*buf;
+
+	void (*callback)(void *cb_data);
+	void *cb_data;
+};
+
+struct tx_cxt {
+	struct list_head	free_list;
+	struct list_head	sdu_list;
+	struct list_head	hci_list;
+#if defined(CONFIG_WIMAX_GDM72XX_USB_PM) || defined(CONFIG_WIMAX_GDM72XX_K_MODE)
+	struct list_head	pending_list;
+#endif
+
+	spinlock_t			lock;
+};
+
+struct usb_rx {
+	struct list_head	list;
+	struct rx_cxt		*rx_cxt;
+
+	struct urb	*urb;
+	u8			*buf;
+
+	void (*callback)(void *cb_data, void *data, int len);
+	void *cb_data;
+};
+
+struct rx_cxt {
+	struct list_head	free_list;
+	struct list_head	used_list;
+	spinlock_t			lock;
+};
+
+struct usbwm_dev {
+	struct usb_device	*usbdev;
+#ifdef CONFIG_WIMAX_GDM72XX_USB_PM
+	struct work_struct	pm_ws;
+
+	struct usb_interface	*intf;
+#endif
+#ifdef CONFIG_WIMAX_GDM72XX_K_MODE
+	int bw_switch;
+	struct list_head	list;
+#endif
+
+	struct tx_cxt	tx;
+	struct rx_cxt	rx;
+
+	int padding;
+};
+
+#endif /* __GDM_USB_H__ */
diff --git a/drivers/staging/gdm72xx/gdm_wimax.c b/drivers/staging/gdm72xx/gdm_wimax.c
new file mode 100644
index 00000000000..f1936b92533
--- /dev/null
+++ b/drivers/staging/gdm72xx/gdm_wimax.c
@@ -0,0 +1,1026 @@
+/*
+ * Copyright (c) 2012 GCT Semiconductor, Inc. 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
+ * 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/version.h>
+#include <linux/etherdevice.h>
+#include <asm/byteorder.h>
+#include <linux/ip.h>
+#include <linux/ipv6.h>
+#include <linux/udp.h>
+#include <linux/in.h>
+
+#include "gdm_wimax.h"
+#include "hci.h"
+#include "wm_ioctl.h"
+#include "netlink_k.h"
+
+#define gdm_wimax_send(n, d, l)	\
+	(n->phy_dev->send_func)(n->phy_dev->priv_dev, d, l, NULL, NULL)
+#define gdm_wimax_send_with_cb(n, d, l, c, b)	\
+	(n->phy_dev->send_func)(n->phy_dev->priv_dev, d, l, c, b)
+#define gdm_wimax_rcv_with_cb(n, c, b)	\
+	(n->phy_dev->rcv_func)(n->phy_dev->priv_dev, c, b)
+
+#define EVT_MAX_SIZE	2048
+
+struct evt_entry {
+	struct list_head list;
+	struct net_device *dev;
+	char evt_data[EVT_MAX_SIZE];
+	int	 size;
+};
+
+static void __gdm_wimax_event_send(struct work_struct *work);
+static inline struct evt_entry *alloc_event_entry(void);
+static inline void free_event_entry(struct evt_entry *e);
+static struct evt_entry *get_event_entry(void);
+static void put_event_entry(struct evt_entry *e);
+
+static struct {
+	int ref_cnt;
+	struct sock *sock;
+	struct list_head evtq;
+	spinlock_t evt_lock;
+
+	struct list_head freeq;
+	struct work_struct ws;
+} wm_event;
+
+static u8 gdm_wimax_macaddr[6] = {0x00, 0x0a, 0x3b, 0xf0, 0x01, 0x30};
+
+static void gdm_wimax_ind_fsm_update(struct net_device *dev, struct fsm_s *fsm);
+static void gdm_wimax_ind_if_updown(struct net_device *dev, int if_up);
+
+#if defined(DEBUG_SDU)
+static void printk_hex(u8 *buf, u32 size)
+{
+	int i;
+
+	for (i = 0; i < size; i++) {
+		if (i && i % 16 == 0)
+			printk(KERN_DEBUG "\n%02x ", *buf++);
+		else
+			printk(KERN_DEBUG "%02x ", *buf++);
+	}
+
+	printk(KERN_DEBUG "\n");
+}
+
+static const char *get_protocol_name(u16 protocol)
+{
+	static char buf[32];
+	const char *name = "-";
+
+	switch (protocol) {
+	case ETH_P_ARP:
+		name = "ARP";
+		break;
+	case ETH_P_IP:
+		name = "IP";
+		break;
+	case ETH_P_IPV6:
+		name = "IPv6";
+		break;
+	}
+
+	sprintf(buf, "0x%04x(%s)", protocol, name);
+	return buf;
+}
+
+static const char *get_ip_protocol_name(u8 ip_protocol)
+{
+	static char buf[32];
+	const char *name = "-";
+
+	switch (ip_protocol) {
+	case IPPROTO_TCP:
+		name = "TCP";
+		break;
+	case IPPROTO_UDP:
+		name = "UDP";
+		break;
+	case IPPROTO_ICMP:
+		name = "ICMP";
+		break;
+	}
+
+	sprintf(buf, "%u(%s)", ip_protocol, name);
+	return buf;
+}
+
+static const char *get_port_name(u16 port)
+{
+	static char buf[32];
+	const char *name = "-";
+
+	switch (port) {
+	case 67:
+		name = "DHCP-Server";
+		break;
+	case 68:
+		name = "DHCP-Client";
+		break;
+	case 69:
+		name = "TFTP";
+		break;
+	}
+
+	sprintf(buf, "%u(%s)", port, name);
+	return buf;
+}
+
+static void dump_eth_packet(const char *title, u8 *data, int len)
+{
+	struct iphdr *ih = NULL;
+	struct udphdr *uh = NULL;
+	u16 protocol = 0;
+	u8 ip_protocol = 0;
+	u16 port = 0;
+
+	protocol = (data[12]<<8) | data[13];
+	ih = (struct iphdr *) (data+ETH_HLEN);
+
+	if (protocol == ETH_P_IP) {
+		uh = (struct udphdr *) ((char *)ih + sizeof(struct iphdr));
+		ip_protocol = ih->protocol;
+		port = ntohs(uh->dest);
+	} else if (protocol == ETH_P_IPV6) {
+		struct ipv6hdr *i6h = (struct ipv6hdr *) data;
+		uh = (struct udphdr *) ((char *)i6h + sizeof(struct ipv6hdr));
+		ip_protocol = i6h->nexthdr;
+		port = ntohs(uh->dest);
+	}
+
+	printk(KERN_DEBUG "[%s] len=%d, %s, %s, %s\n",
+		title, len,
+		get_protocol_name(protocol),
+		get_ip_protocol_name(ip_protocol),
+		get_port_name(port));
+
+	#if 1
+	if (!(data[0] == 0xff && data[1] == 0xff)) {
+		if (protocol == ETH_P_IP) {
+			printk(KERN_DEBUG "     src=%u.%u.%u.%u\n",
+				NIPQUAD(ih->saddr));
+		} else if (protocol == ETH_P_IPV6) {
+			#ifdef NIP6
+			printk(KERN_DEBUG "     src=%x:%x:%x:%x:%x:%x:%x:%x\n",
+				NIP6(ih->saddr));
+			#else
+			printk(KERN_DEBUG "     src=%pI6\n", &ih->saddr);
+			#endif
+		}
+	}
+	#endif
+
+	#if (DUMP_PACKET & DUMP_SDU_ALL)
+	printk_hex(data, len);
+	#else
+		#if (DUMP_PACKET & DUMP_SDU_ARP)
+		if (protocol == ETH_P_ARP)
+			printk_hex(data, len);
+		#endif
+		#if (DUMP_PACKET & DUMP_SDU_IP)
+		if (protocol == ETH_P_IP || protocol == ETH_P_IPV6)
+			printk_hex(data, len);
+		#else
+			#if (DUMP_PACKET & DUMP_SDU_IP_TCP)
+			if (ip_protocol == IPPROTO_TCP)
+				printk_hex(data, len);
+			#endif
+			#if (DUMP_PACKET & DUMP_SDU_IP_UDP)
+			if (ip_protocol == IPPROTO_UDP)
+				printk_hex(data, len);
+			#endif
+			#if (DUMP_PACKET & DUMP_SDU_IP_ICMP)
+			if (ip_protocol == IPPROTO_ICMP)
+				printk_hex(data, len);
+			#endif
+		#endif
+	#endif
+}
+#endif
+
+
+static inline int gdm_wimax_header(struct sk_buff **pskb)
+{
+	u16 buf[HCI_HEADER_SIZE / sizeof(u16)];
+	struct sk_buff *skb = *pskb;
+	int ret = 0;
+
+	if (unlikely(skb_headroom(skb) < HCI_HEADER_SIZE)) {
+		struct sk_buff *skb2;
+
+		skb2 = skb_realloc_headroom(skb, HCI_HEADER_SIZE);
+		if (skb2 == NULL)
+			return -ENOMEM;
+		if (skb->sk)
+			skb_set_owner_w(skb2, skb->sk);
+		kfree_skb(skb);
+		skb = skb2;
+	}
+
+	skb_push(skb, HCI_HEADER_SIZE);
+	buf[0] = H2B(WIMAX_TX_SDU);
+	buf[1] = H2B(skb->len - HCI_HEADER_SIZE);
+	memcpy(skb->data, buf, HCI_HEADER_SIZE);
+
+	*pskb = skb;
+	return ret;
+}
+
+static void gdm_wimax_event_rcv(struct net_device *dev, u16 type, void *msg,
+				int len)
+{
+	struct nic *nic = netdev_priv(dev);
+
+	#if defined(DEBUG_HCI)
+	u8 *buf = (u8 *) msg;
+	u16 hci_cmd =  (buf[0]<<8) | buf[1];
+	u16 hci_len = (buf[2]<<8) | buf[3];
+	printk(KERN_DEBUG "H=>D: 0x%04x(%d)\n", hci_cmd, hci_len);
+	#endif
+
+	gdm_wimax_send(nic, msg, len);
+}
+
+static int gdm_wimax_event_init(void)
+{
+	if (wm_event.ref_cnt == 0) {
+		wm_event.sock = netlink_init(NETLINK_WIMAX,
+						gdm_wimax_event_rcv);
+		INIT_LIST_HEAD(&wm_event.evtq);
+		INIT_LIST_HEAD(&wm_event.freeq);
+		INIT_WORK(&wm_event.ws, __gdm_wimax_event_send);
+		spin_lock_init(&wm_event.evt_lock);
+	}
+
+	if (wm_event.sock) {
+		wm_event.ref_cnt++;
+		return 0;
+	}
+
+	printk(KERN_ERR "Creating WiMax Event netlink is failed\n");
+	return -1;
+}
+
+static void gdm_wimax_event_exit(void)
+{
+	if (wm_event.sock && --wm_event.ref_cnt == 0) {
+		struct evt_entry *e, *temp;
+		unsigned long flags;
+
+		spin_lock_irqsave(&wm_event.evt_lock, flags);
+
+		list_for_each_entry_safe(e, temp, &wm_event.evtq, list) {
+			list_del(&e->list);
+			free_event_entry(e);
+		}
+		list_for_each_entry_safe(e, temp, &wm_event.freeq, list) {
+			list_del(&e->list);
+			free_event_entry(e);
+		}
+
+		spin_unlock_irqrestore(&wm_event.evt_lock, flags);
+		netlink_exit(wm_event.sock);
+		wm_event.sock = NULL;
+	}
+}
+
+static inline struct evt_entry *alloc_event_entry(void)
+{
+	return kmalloc(sizeof(struct evt_entry), GFP_ATOMIC);
+}
+
+static inline void free_event_entry(struct evt_entry *e)
+{
+	kfree(e);
+}
+
+static struct evt_entry *get_event_entry(void)
+{
+	struct evt_entry *e;
+
+	if (list_empty(&wm_event.freeq))
+		e = alloc_event_entry();
+	else {
+		e = list_entry(wm_event.freeq.next, struct evt_entry, list);
+		list_del(&e->list);
+	}
+
+	return e;
+}
+
+static void put_event_entry(struct evt_entry *e)
+{
+	BUG_ON(!e);
+
+	list_add_tail(&e->list, &wm_event.freeq);
+}
+
+static void __gdm_wimax_event_send(struct work_struct *work)
+{
+	int idx;
+	unsigned long flags;
+	struct evt_entry *e;
+
+	spin_lock_irqsave(&wm_event.evt_lock, flags);
+
+	while (!list_empty(&wm_event.evtq)) {
+		e = list_entry(wm_event.evtq.next, struct evt_entry, list);
+		spin_unlock_irqrestore(&wm_event.evt_lock, flags);
+
+		sscanf(e->dev->name, "wm%d", &idx);
+		netlink_send(wm_event.sock, idx, 0, e->evt_data, e->size);
+
+		spin_lock_irqsave(&wm_event.evt_lock, flags);
+		list_del(&e->list);
+		put_event_entry(e);
+	}
+
+	spin_unlock_irqrestore(&wm_event.evt_lock, flags);
+}
+
+static int gdm_wimax_event_send(struct net_device *dev, char *buf, int size)
+{
+	struct evt_entry *e;
+	unsigned long flags;
+
+	#if defined(DEBUG_HCI)
+	u16 hci_cmd =  ((u8)buf[0]<<8) | (u8)buf[1];
+	u16 hci_len = ((u8)buf[2]<<8) | (u8)buf[3];
+	printk(KERN_DEBUG "D=>H: 0x%04x(%d)\n", hci_cmd, hci_len);
+	#endif
+
+	spin_lock_irqsave(&wm_event.evt_lock, flags);
+
+	e = get_event_entry();
+	if (!e) {
+		printk(KERN_ERR "%s: No memory for event\n", __func__);
+		spin_unlock_irqrestore(&wm_event.evt_lock, flags);
+		return -ENOMEM;
+	}
+
+	e->dev = dev;
+	e->size = size;
+	memcpy(e->evt_data, buf, size);
+
+	list_add_tail(&e->list, &wm_event.evtq);
+	spin_unlock_irqrestore(&wm_event.evt_lock, flags);
+
+	schedule_work(&wm_event.ws);
+
+	return 0;
+}
+
+static void tx_complete(void *arg)
+{
+	struct nic *nic = arg;
+
+	if (netif_queue_stopped(nic->netdev))
+		netif_wake_queue(nic->netdev);
+}
+
+int gdm_wimax_send_tx(struct sk_buff *skb, struct net_device *dev)
+{
+	int ret = 0;
+	struct nic *nic = netdev_priv(dev);
+
+	ret = gdm_wimax_send_with_cb(nic, skb->data, skb->len, tx_complete,
+					nic);
+	if (ret == -ENOSPC) {
+		netif_stop_queue(dev);
+		ret = 0;
+	}
+
+	if (ret) {
+		skb_pull(skb, HCI_HEADER_SIZE);
+		return ret;
+	}
+
+	nic->stats.tx_packets++;
+	nic->stats.tx_bytes += skb->len - HCI_HEADER_SIZE;
+	kfree_skb(skb);
+	return ret;
+}
+
+static int gdm_wimax_tx(struct sk_buff *skb, struct net_device *dev)
+{
+	int ret = 0;
+	struct nic *nic = netdev_priv(dev);
+	struct fsm_s *fsm = (struct fsm_s *) nic->sdk_data[SIOC_DATA_FSM].buf;
+
+	#if defined(DEBUG_SDU)
+	dump_eth_packet("TX", skb->data, skb->len);
+	#endif
+
+	ret = gdm_wimax_header(&skb);
+	if (ret < 0) {
+		skb_pull(skb, HCI_HEADER_SIZE);
+		return ret;
+	}
+
+	#if !defined(LOOPBACK_TEST)
+	if (!fsm)
+		printk(KERN_ERR "ASSERTION ERROR: fsm is NULL!!\n");
+	else if (fsm->m_status != M_CONNECTED) {
+		printk(KERN_EMERG "ASSERTION ERROR: Device is NOT ready. status=%d\n",
+			fsm->m_status);
+		kfree_skb(skb);
+		return 0;
+	}
+	#endif
+
+#if defined(CONFIG_WIMAX_GDM72XX_QOS)
+	ret = gdm_qos_send_hci_pkt(skb, dev);
+#else
+	ret = gdm_wimax_send_tx(skb, dev);
+#endif
+	return ret;
+}
+
+static int gdm_wimax_set_config(struct net_device *dev, struct ifmap *map)
+{
+	if (dev->flags & IFF_UP)
+		return -EBUSY;
+
+	return 0;
+}
+
+static void __gdm_wimax_set_mac_addr(struct net_device *dev, char *mac_addr)
+{
+	u16 hci_pkt_buf[32 / sizeof(u16)];
+	u8 *pkt = (u8 *) &hci_pkt_buf[0];
+	struct nic *nic = netdev_priv(dev);
+
+	/* Since dev is registered as a ethernet device,
+	 * ether_setup has made dev->addr_len to be ETH_ALEN
+	 */
+	memcpy(dev->dev_addr, mac_addr, dev->addr_len);
+
+	/* Let lower layer know of this change by sending
+	 * SetInformation(MAC Address)
+	 */
+	hci_pkt_buf[0] = H2B(WIMAX_SET_INFO);	/* cmd_evt */
+	hci_pkt_buf[1] = H2B(8);			/* size */
+	pkt[4] = 0; /* T */
+	pkt[5] = 6; /* L */
+	memcpy(pkt + 6, mac_addr, dev->addr_len); /* V */
+
+	gdm_wimax_send(nic, pkt, HCI_HEADER_SIZE + 8);
+}
+
+/* A driver function */
+static int gdm_wimax_set_mac_addr(struct net_device *dev, void *p)
+{
+	struct sockaddr *addr = p;
+
+	if (netif_running(dev))
+		return -EBUSY;
+
+	if (!is_valid_ether_addr(addr->sa_data))
+		return -EADDRNOTAVAIL;
+
+	__gdm_wimax_set_mac_addr(dev, addr->sa_data);
+
+	return 0;
+}
+
+static struct net_device_stats *gdm_wimax_stats(struct net_device *dev)
+{
+	struct nic *nic = netdev_priv(dev);
+
+	return &nic->stats;
+}
+
+static int gdm_wimax_open(struct net_device *dev)
+{
+	struct nic *nic = netdev_priv(dev);
+	struct fsm_s *fsm = (struct fsm_s *) nic->sdk_data[SIOC_DATA_FSM].buf;
+
+	netif_start_queue(dev);
+
+	if (fsm && fsm->m_status != M_INIT)
+		gdm_wimax_ind_if_updown(dev, 1);
+	return 0;
+}
+
+static int gdm_wimax_close(struct net_device *dev)
+{
+	struct nic *nic = netdev_priv(dev);
+	struct fsm_s *fsm = (struct fsm_s *) nic->sdk_data[SIOC_DATA_FSM].buf;
+
+	netif_stop_queue(dev);
+
+	if (fsm && fsm->m_status != M_INIT)
+		gdm_wimax_ind_if_updown(dev, 0);
+	return 0;
+}
+
+static void kdelete(void **buf)
+{
+	if (buf && *buf) {
+		kfree(*buf);
+		*buf = NULL;
+	}
+}
+
+static int gdm_wimax_ioctl_get_data(struct data_s *dst, struct data_s *src)
+{
+	int size;
+
+	size = dst->size < src->size ? dst->size : src->size;
+
+	dst->size = size;
+	if (src->size) {
+		if (!dst->buf)
+			return -EINVAL;
+		if (copy_to_user(dst->buf, src->buf, size))
+			return -EFAULT;
+	}
+	return 0;
+}
+
+static int gdm_wimax_ioctl_set_data(struct data_s *dst, struct data_s *src)
+{
+	if (!src->size) {
+		dst->size = 0;
+		return 0;
+	}
+
+	if (!src->buf)
+		return -EINVAL;
+
+	if (!(dst->buf && dst->size == src->size)) {
+		kdelete(&dst->buf);
+		dst->buf = kmalloc(src->size, GFP_KERNEL);
+		if (dst->buf == NULL)
+			return -ENOMEM;
+	}
+
+	if (copy_from_user(dst->buf, src->buf, src->size)) {
+		kdelete(&dst->buf);
+		return -EFAULT;
+	}
+	dst->size = src->size;
+	return 0;
+}
+
+static void gdm_wimax_cleanup_ioctl(struct net_device *dev)
+{
+	struct nic *nic = netdev_priv(dev);
+	int i;
+
+	for (i = 0; i < SIOC_DATA_MAX; i++)
+		kdelete(&nic->sdk_data[i].buf);
+}
+
+static void gdm_update_fsm(struct net_device *dev, struct fsm_s *new_fsm)
+{
+	struct nic *nic = netdev_priv(dev);
+	struct fsm_s *cur_fsm =
+		(struct fsm_s *) nic->sdk_data[SIOC_DATA_FSM].buf;
+
+	if (!cur_fsm)
+		return;
+
+	if (cur_fsm->m_status != new_fsm->m_status ||
+		cur_fsm->c_status != new_fsm->c_status) {
+		if (new_fsm->m_status == M_CONNECTED)
+			netif_carrier_on(dev);
+		else if (cur_fsm->m_status == M_CONNECTED) {
+			netif_carrier_off(dev);
+			#if defined(CONFIG_WIMAX_GDM72XX_QOS)
+			gdm_qos_release_list(nic);
+			#endif
+		}
+		gdm_wimax_ind_fsm_update(dev, new_fsm);
+	}
+}
+
+static int gdm_wimax_ioctl(struct net_device *dev, struct ifreq *ifr, int cmd)
+{
+	struct wm_req_s *req = (struct wm_req_s *) ifr;
+	struct nic *nic = netdev_priv(dev);
+	int ret;
+
+	if (cmd != SIOCWMIOCTL)
+		return -EOPNOTSUPP;
+
+	switch (req->cmd) {
+	case SIOCG_DATA:
+	case SIOCS_DATA:
+		if (req->data_id >= SIOC_DATA_MAX) {
+			printk(KERN_ERR
+				"%s error: data-index(%d) is invalid!!\n",
+				__func__, req->data_id);
+			return -EOPNOTSUPP;
+		}
+		if (req->cmd == SIOCG_DATA) {
+			ret = gdm_wimax_ioctl_get_data(&req->data,
+						&nic->sdk_data[req->data_id]);
+			if (ret < 0)
+				return ret;
+		} else if (req->cmd == SIOCS_DATA) {
+			if (req->data_id == SIOC_DATA_FSM) {
+				/*NOTE: gdm_update_fsm should be called
+				before gdm_wimax_ioctl_set_data is called*/
+				gdm_update_fsm(dev,
+						(struct fsm_s *) req->data.buf);
+			}
+			ret = gdm_wimax_ioctl_set_data(
+				&nic->sdk_data[req->data_id], &req->data);
+			if (ret < 0)
+				return ret;
+		}
+		break;
+	default:
+		printk(KERN_ERR "%s: %x unknown ioctl\n", __func__, cmd);
+		return -EOPNOTSUPP;
+	}
+
+	return 0;
+}
+
+static void gdm_wimax_prepare_device(struct net_device *dev)
+{
+	struct nic *nic = netdev_priv(dev);
+	u16 buf[32 / sizeof(u16)];
+	struct hci_s *hci = (struct hci_s *) buf;
+	u16 len = 0;
+	u32 val = 0;
+
+	#define BIT_MULTI_CS	0
+	#define BIT_WIMAX		1
+	#define BIT_QOS			2
+	#define BIT_AGGREGATION	3
+
+	/* GetInformation mac address */
+	len = 0;
+	hci->cmd_evt = H2B(WIMAX_GET_INFO);
+	hci->data[len++] = TLV_T(T_MAC_ADDRESS);
+	hci->length = H2B(len);
+	gdm_wimax_send(nic, hci, HCI_HEADER_SIZE+len);
+
+	val = (1<<BIT_WIMAX) | (1<<BIT_MULTI_CS);
+	#if defined(CONFIG_WIMAX_GDM72XX_QOS)
+	val |= (1<<BIT_QOS);
+	#endif
+	#if defined(CONFIG_WIMAX_GDM72XX_WIMAX2)
+	val |= (1<<BIT_AGGREGATION);
+	#endif
+
+	/* Set capability */
+	len = 0;
+	hci->cmd_evt = H2B(WIMAX_SET_INFO);
+	hci->data[len++] = TLV_T(T_CAPABILITY);
+	hci->data[len++] = TLV_L(T_CAPABILITY);
+	val = DH2B(val);
+	memcpy(&hci->data[len], &val, TLV_L(T_CAPABILITY));
+	len += TLV_L(T_CAPABILITY);
+	hci->length = H2B(len);
+	gdm_wimax_send(nic, hci, HCI_HEADER_SIZE+len);
+
+	printk(KERN_INFO "GDM WiMax Set CAPABILITY: 0x%08X\n", DB2H(val));
+}
+
+static int gdm_wimax_hci_get_tlv(u8 *buf, u8 *T, u16 *L, u8 **V)
+{
+	#define __U82U16(b) ((u16)((u8 *)(b))[0] | ((u16)((u8 *)(b))[1] << 8))
+	int next_pos;
+
+	*T = buf[0];
+	if (buf[1] == 0x82) {
+		*L = B2H(__U82U16(&buf[2]));
+		next_pos = 1/*type*/+3/*len*/;
+	} else {
+		*L = buf[1];
+		next_pos = 1/*type*/+1/*len*/;
+	}
+	*V = &buf[next_pos];
+
+	next_pos += *L/*length of val*/;
+	return next_pos;
+}
+
+static int gdm_wimax_get_prepared_info(struct net_device *dev, char *buf,
+					int len)
+{
+	u8 T, *V;
+	u16 L;
+	u16 cmd_evt, cmd_len;
+	int pos = HCI_HEADER_SIZE;
+
+	cmd_evt = B2H(*(u16 *)&buf[0]);
+	cmd_len = B2H(*(u16 *)&buf[2]);
+
+	if (len < cmd_len + HCI_HEADER_SIZE) {
+		printk(KERN_ERR "%s: invalid length [%d/%d]\n", __func__,
+			cmd_len + HCI_HEADER_SIZE, len);
+		return -1;
+	}
+
+	if (cmd_evt == WIMAX_GET_INFO_RESULT) {
+		if (cmd_len < 2) {
+			printk(KERN_ERR "%s: len is too short [%x/%d]\n",
+				__func__, cmd_evt, len);
+			return -1;
+		}
+
+		pos += gdm_wimax_hci_get_tlv(&buf[pos], &T, &L, &V);
+		if (T == TLV_T(T_MAC_ADDRESS)) {
+			if (L != dev->addr_len) {
+				printk(KERN_ERR
+					"%s Invalid inofrmation result T/L "
+					"[%x/%d]\n", __func__, T, L);
+				return -1;
+			}
+			printk(KERN_INFO
+				"MAC change [%02x:%02x:%02x:%02x:%02x:%02x]"
+				"->[%02x:%02x:%02x:%02x:%02x:%02x]\n",
+				dev->dev_addr[0], dev->dev_addr[1],
+				dev->dev_addr[2], dev->dev_addr[3],
+				dev->dev_addr[4], dev->dev_addr[5],
+				V[0], V[1], V[2], V[3], V[4], V[5]);
+			memcpy(dev->dev_addr, V, dev->addr_len);
+			return 1;
+		}
+	}
+
+	gdm_wimax_event_send(dev, buf, len);
+	return 0;
+}
+
+static void gdm_wimax_netif_rx(struct net_device *dev, char *buf, int len)
+{
+	struct nic *nic = netdev_priv(dev);
+	struct sk_buff *skb;
+	int ret;
+
+	#if defined(DEBUG_SDU)
+	dump_eth_packet("RX", buf, len);
+	#endif
+
+	skb = dev_alloc_skb(len + 2);
+	if (!skb) {
+		printk(KERN_ERR "%s: dev_alloc_skb failed!\n", __func__);
+		return;
+	}
+	skb_reserve(skb, 2);
+
+	nic->stats.rx_packets++;
+	nic->stats.rx_bytes += len;
+
+	memcpy(skb_put(skb, len), buf, len);
+
+	skb->dev = dev;
+	skb->protocol = eth_type_trans(skb, dev); /* what will happen? */
+
+	ret = in_interrupt() ? netif_rx(skb) : netif_rx_ni(skb);
+	if (ret == NET_RX_DROP)
+		printk(KERN_ERR "%s skb dropped\n", __func__);
+}
+
+static void gdm_wimax_transmit_aggr_pkt(struct net_device *dev, char *buf,
+					int len)
+{
+	#define HCI_PADDING_BYTE	4
+	#define HCI_RESERVED_BYTE	4
+	struct hci_s *hci;
+	int length;
+
+	while (len > 0) {
+		hci = (struct hci_s *) buf;
+
+		if (B2H(hci->cmd_evt) != WIMAX_RX_SDU) {
+			printk(KERN_ERR "Wrong cmd_evt(0x%04X)\n",
+				B2H(hci->cmd_evt));
+			break;
+		}
+
+		length = B2H(hci->length);
+		gdm_wimax_netif_rx(dev, hci->data, length);
+
+		if (length & 0x3) {
+			/* Add padding size */
+			length += HCI_PADDING_BYTE - (length & 0x3);
+		}
+
+		length += HCI_HEADER_SIZE + HCI_RESERVED_BYTE;
+		len -= length;
+		buf += length;
+	}
+}
+
+static void gdm_wimax_transmit_pkt(struct net_device *dev, char *buf, int len)
+{
+	#if defined(CONFIG_WIMAX_GDM72XX_QOS)
+	struct nic *nic = netdev_priv(dev);
+	#endif
+	u16 cmd_evt, cmd_len;
+
+	/* This code is added for certain rx packet to be ignored. */
+	if (len == 0)
+		return;
+
+	cmd_evt = B2H(*(u16 *)&buf[0]);
+	cmd_len = B2H(*(u16 *)&buf[2]);
+
+	if (len < cmd_len + HCI_HEADER_SIZE) {
+		if (len)
+			printk(KERN_ERR "%s: invalid length [%d/%d]\n",
+				__func__, cmd_len + HCI_HEADER_SIZE, len);
+		return;
+	}
+
+	switch (cmd_evt) {
+	case WIMAX_RX_SDU_AGGR:
+		gdm_wimax_transmit_aggr_pkt(dev, &buf[HCI_HEADER_SIZE],
+						cmd_len);
+		break;
+	case WIMAX_RX_SDU:
+		gdm_wimax_netif_rx(dev, &buf[HCI_HEADER_SIZE], cmd_len);
+		break;
+	#if defined(CONFIG_WIMAX_GDM72XX_QOS)
+	case WIMAX_EVT_MODEM_REPORT:
+		gdm_recv_qos_hci_packet(nic, buf, len);
+		break;
+	#endif
+	case WIMAX_SDU_TX_FLOW:
+		if (buf[4] == 0) {
+			if (!netif_queue_stopped(dev))
+				netif_stop_queue(dev);
+		} else if (buf[4] == 1) {
+			if (netif_queue_stopped(dev))
+				netif_wake_queue(dev);
+		}
+		break;
+	default:
+		gdm_wimax_event_send(dev, buf, len);
+		break;
+	}
+}
+
+static void gdm_wimax_ind_fsm_update(struct net_device *dev, struct fsm_s *fsm)
+{
+	u16 buf[32 / sizeof(u16)];
+	u8 *hci_pkt_buf = (u8 *)&buf[0];
+
+	/* Indicate updating fsm */
+	buf[0] = H2B(WIMAX_FSM_UPDATE);
+	buf[1] = H2B(sizeof(struct fsm_s));
+	memcpy(&hci_pkt_buf[HCI_HEADER_SIZE], fsm, sizeof(struct fsm_s));
+
+	gdm_wimax_event_send(dev, hci_pkt_buf,
+				HCI_HEADER_SIZE + sizeof(struct fsm_s));
+}
+
+static void gdm_wimax_ind_if_updown(struct net_device *dev, int if_up)
+{
+	u16 buf[32 / sizeof(u16)];
+	struct hci_s *hci = (struct hci_s *) buf;
+	unsigned char up_down;
+
+	up_down = if_up ? WIMAX_IF_UP : WIMAX_IF_DOWN;
+
+	/* Indicate updating fsm */
+	hci->cmd_evt = H2B(WIMAX_IF_UPDOWN);
+	hci->length = H2B(sizeof(up_down));
+	hci->data[0] = up_down;
+
+	gdm_wimax_event_send(dev, (char *)hci, HCI_HEADER_SIZE+sizeof(up_down));
+}
+
+static void rx_complete(void *arg, void *data, int len)
+{
+	struct nic *nic = arg;
+
+	gdm_wimax_transmit_pkt(nic->netdev, data, len);
+	gdm_wimax_rcv_with_cb(nic, rx_complete, nic);
+}
+
+static void prepare_rx_complete(void *arg, void *data, int len)
+{
+	struct nic *nic = arg;
+	int ret;
+
+	ret = gdm_wimax_get_prepared_info(nic->netdev, data, len);
+	if (ret == 1)
+		gdm_wimax_rcv_with_cb(nic, rx_complete, nic);
+	else {
+		if (ret < 0)
+			printk(KERN_ERR "get_prepared_info failed(%d)\n", ret);
+		gdm_wimax_rcv_with_cb(nic, prepare_rx_complete, nic);
+		#if 0
+		/* Re-prepare WiMax device */
+		gdm_wimax_prepare_device(nic->netdev);
+		#endif
+	}
+}
+
+static void start_rx_proc(struct nic *nic)
+{
+	gdm_wimax_rcv_with_cb(nic, prepare_rx_complete, nic);
+}
+
+static struct net_device_ops gdm_netdev_ops = {
+	.ndo_open				= gdm_wimax_open,
+	.ndo_stop				= gdm_wimax_close,
+	.ndo_set_config			= gdm_wimax_set_config,
+	.ndo_start_xmit			= gdm_wimax_tx,
+	.ndo_get_stats			= gdm_wimax_stats,
+	.ndo_set_mac_address	= gdm_wimax_set_mac_addr,
+	.ndo_do_ioctl			= gdm_wimax_ioctl,
+};
+
+int register_wimax_device(struct phy_dev *phy_dev, struct device *pdev)
+{
+	struct nic *nic = NULL;
+	struct net_device *dev;
+	int ret;
+
+	dev = (struct net_device *)alloc_netdev(sizeof(*nic),
+						"wm%d", ether_setup);
+
+	if (dev == NULL) {
+		printk(KERN_ERR "alloc_etherdev failed\n");
+		return -ENOMEM;
+	}
+
+	SET_NETDEV_DEV(dev, pdev);
+	dev->mtu = 1400;
+	dev->netdev_ops = &gdm_netdev_ops;
+	dev->flags &= ~IFF_MULTICAST;
+	memcpy(dev->dev_addr, gdm_wimax_macaddr, sizeof(gdm_wimax_macaddr));
+
+	nic = netdev_priv(dev);
+	memset(nic, 0, sizeof(*nic));
+
+	nic->netdev = dev;
+	nic->phy_dev = phy_dev;
+	phy_dev->netdev = dev;
+
+	/* event socket init */
+	ret = gdm_wimax_event_init();
+	if (ret < 0) {
+		printk(KERN_ERR "Cannot create event.\n");
+		goto cleanup;
+	}
+
+	ret = register_netdev(dev);
+	if (ret)
+		goto cleanup;
+
+	#if defined(LOOPBACK_TEST)
+	netif_start_queue(dev);
+	netif_carrier_on(dev);
+	#else
+	netif_carrier_off(dev);
+	#endif
+
+#ifdef CONFIG_WIMAX_GDM72XX_QOS
+	gdm_qos_init(nic);
+#endif
+
+	start_rx_proc(nic);
+
+	/* Prepare WiMax device */
+	gdm_wimax_prepare_device(dev);
+
+	return 0;
+
+cleanup:
+	printk(KERN_ERR "register_netdev failed\n");
+	free_netdev(dev);
+	return ret;
+}
+
+void unregister_wimax_device(struct phy_dev *phy_dev)
+{
+	struct nic *nic = netdev_priv(phy_dev->netdev);
+	struct fsm_s *fsm = (struct fsm_s *) nic->sdk_data[SIOC_DATA_FSM].buf;
+
+	if (fsm)
+		fsm->m_status = M_INIT;
+	unregister_netdev(nic->netdev);
+
+	gdm_wimax_event_exit();
+
+#if defined(CONFIG_WIMAX_GDM72XX_QOS)
+	gdm_qos_release_list(nic);
+#endif
+
+	gdm_wimax_cleanup_ioctl(phy_dev->netdev);
+
+	free_netdev(nic->netdev);
+}
diff --git a/drivers/staging/gdm72xx/gdm_wimax.h b/drivers/staging/gdm72xx/gdm_wimax.h
new file mode 100644
index 00000000000..023e6492e33
--- /dev/null
+++ b/drivers/staging/gdm72xx/gdm_wimax.h
@@ -0,0 +1,92 @@
+/*
+ * Copyright (c) 2012 GCT Semiconductor, Inc. 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
+ * 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.
+ */
+
+#ifndef __GDM_WIMAX_H__
+#define __GDM_WIMAX_H__
+
+#include <linux/netdevice.h>
+#include <linux/version.h>
+#include <linux/types.h>
+#include "wm_ioctl.h"
+#if defined(CONFIG_WIMAX_GDM72XX_QOS)
+#include "gdm_qos.h"
+#endif
+
+#define DRIVER_VERSION		"3.2.3"
+
+/*#define ETH_P_IP	0x0800 */
+/*#define ETH_P_ARP	0x0806 */
+/*#define ETH_P_IPV6	0x86DD */
+
+#define H2L(x)		__cpu_to_le16(x)
+#define L2H(x)		__le16_to_cpu(x)
+#define DH2L(x)		__cpu_to_le32(x)
+#define DL2H(x)		__le32_to_cpu(x)
+
+#define H2B(x)		__cpu_to_be16(x)
+#define B2H(x)		__be16_to_cpu(x)
+#define DH2B(x)		__cpu_to_be32(x)
+#define DB2H(x)		__be32_to_cpu(x)
+
+struct phy_dev {
+	void	*priv_dev;
+	struct net_device	*netdev;
+
+	int	(*send_func)(void *priv_dev, void *data, int len,
+			void (*cb)(void *cb_data), void *cb_data);
+	int	(*rcv_func)(void *priv_dev,
+			void (*cb)(void *cb_data, void *data, int len),
+			void *cb_data);
+};
+
+struct nic {
+	struct net_device	*netdev;
+	struct phy_dev		*phy_dev;
+
+	struct net_device_stats	stats;
+
+	struct data_s	sdk_data[SIOC_DATA_MAX];
+
+#if defined(CONFIG_WIMAX_GDM72XX_QOS)
+	struct qos_cb_s	qos;
+#endif
+
+};
+
+
+#if 0
+#define dprintk(fmt, args ...)	printk(KERN_DEBUG " [GDM] " fmt, ## args)
+#else
+#define dprintk(...)
+#endif
+
+/*#define DEBUG_SDU */
+#if defined(DEBUG_SDU)
+#define DUMP_SDU_ALL		(1<<0)
+#define DUMP_SDU_ARP		(1<<1)
+#define DUMP_SDU_IP			(1<<2)
+#define DUMP_SDU_IP_TCP		(1<<8)
+#define DUMP_SDU_IP_UDP		(1<<9)
+#define DUMP_SDU_IP_ICMP	(1<<10)
+#define DUMP_PACKET			(DUMP_SDU_ALL)
+#endif
+
+/*#define DEBUG_HCI */
+
+/*#define LOOPBACK_TEST */
+
+extern int register_wimax_device(struct phy_dev *phy_dev, struct device *pdev);
+extern int gdm_wimax_send_tx(struct sk_buff *skb, struct net_device *dev);
+extern void unregister_wimax_device(struct phy_dev *phy_dev);
+
+#endif
diff --git a/drivers/staging/gdm72xx/hci.h b/drivers/staging/gdm72xx/hci.h
new file mode 100644
index 00000000000..0e0676622f1
--- /dev/null
+++ b/drivers/staging/gdm72xx/hci.h
@@ -0,0 +1,218 @@
+/*
+ * Copyright (c) 2012 GCT Semiconductor, Inc. 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
+ * 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.
+ */
+
+#ifndef HCI_H_20080801
+#define HCI_H_20080801
+
+#define HCI_HEADER_SIZE		4
+#define HCI_VALUE_OFFS		(HCI_HEADER_SIZE)
+#define HCI_MAX_PACKET		2048
+#define HCI_MAX_PARAM		(HCI_MAX_PACKET-HCI_HEADER_SIZE)
+#define HCI_MAX_TLV			32
+
+/* CMD-EVT */
+
+/* Category 0 */
+#define WIMAX_RESET				0x0000
+#define WIMAX_SET_INFO			0x0001
+#define WIMAX_GET_INFO			0x0002
+#define WIMAX_GET_INFO_RESULT	0x8003
+#define WIMAX_RADIO_OFF			0x0004
+#define WIMAX_RADIO_ON			0x0006
+#define WIMAX_WIMAX_RESET		0x0007	/* Is this still here */
+
+/* Category 1 */
+#define WIMAX_NET_ENTRY			0x0100
+#define WIMAX_NET_DISCONN		0x0102
+#define WIMAX_ENTER_SLEEP		0x0103
+#define WIMAX_EXIT_SLEEP		0x0104
+#define WIMAX_ENTER_IDLE		0x0105
+#define WIMAX_EXIT_IDLE			0x0106
+#define WIMAX_MODE_CHANGE		0x8108
+#define WIMAX_HANDOVER			0x8109	/* obsolete */
+
+#define WIMAX_SCAN				0x010d
+#define WIMAX_SCAN_COMPLETE		0x810e
+#define WIMAX_SCAN_RESULT		0x810f
+
+#define WIMAX_CONNECT			0x0110
+#define WIMAX_CONNECT_START		0x8111
+#define WIMAX_CONNECT_COMPLETE	0x8112
+#define WIMAX_ASSOC_START		0x8113
+#define WIMAX_ASSOC_COMPLETE	0x8114
+#define WIMAX_DISCONN_IND		0x8115
+#define WIMAX_ENTRY_IND			0x8116
+#define WIMAX_HO_START			0x8117
+#define WIMAX_HO_COMPLETE		0x8118
+#define WIMAX_RADIO_STATE_IND	0x8119
+#define WIMAX_IP_RENEW_IND		0x811a
+
+#define WIMAX_DISCOVER_NSP			0x011d
+#define WIMAX_DISCOVER_NSP_RESULT	0x811e
+
+#define WIMAX_SDU_TX_FLOW		0x8125
+
+/* Category 2 */
+#define WIMAX_TX_EAP		0x0200
+#define WIMAX_RX_EAP		0x8201
+#define WIMAX_TX_SDU		0x0202
+#define WIMAX_RX_SDU		0x8203
+#define WIMAX_RX_SDU_AGGR	0x8204
+#define WIMAX_TX_SDU_AGGR	0x0205
+
+/* Category 3 */
+#define WIMAX_DM_CMD				0x030a
+#define WIMAX_DM_RSP				0x830b
+
+#define WIMAX_CLI_CMD				0x030c
+#define WIMAX_CLI_RSP				0x830d
+
+#define WIMAX_DL_IMAGE				0x0310
+#define WIMAX_DL_IMAGE_STATUS		0x8311
+#define WIMAX_UL_IMAGE				0x0312
+#define WIMAX_UL_IMAGE_RESULT		0x8313
+#define WIMAX_UL_IMAGE_STATUS		0x0314
+
+#define WIMAX_EVT_MODEM_REPORT		0x8325
+
+/* Category 0xF */
+#define WIMAX_FSM_UPDATE			0x8F01
+#define WIMAX_IF_UPDOWN				0x8F02
+	#define WIMAX_IF_UP				1
+	#define WIMAX_IF_DOWN			2
+
+/* WIMAX mode */
+#define W_NULL				0
+#define W_STANDBY			1
+#define W_OOZ				2
+#define W_AWAKE				3
+#define W_IDLE				4
+#define W_SLEEP				5
+#define W_WAIT				6
+
+#define W_NET_ENTRY_RNG		0x80
+#define W_NET_ENTRY_SBC		0x81
+#define W_NET_ENTRY_PKM		0x82
+#define W_NET_ENTRY_REG		0x83
+#define W_NET_ENTRY_DSX		0x84
+
+#define W_NET_ENTRY_RNG_FAIL	0x1100100
+#define W_NET_ENTRY_SBC_FAIL	0x1100200
+#define W_NET_ENTRY_PKM_FAIL	0x1102000
+#define W_NET_ENTRY_REG_FAIL	0x1103000
+#define W_NET_ENTRY_DSX_FAIL	0x1104000
+
+/* Scan Type */
+#define W_SCAN_ALL_CHANNEL				0
+#define W_SCAN_ALL_SUBSCRIPTION			1
+#define W_SCAN_SPECIFIED_SUBSCRIPTION	2
+
+/*
+ * TLV
+ *
+ * [31:31] indicates the type is composite.
+ * [30:16] is the length of the type. 0 length means length is variable.
+ * [15:0] is the actual type.
+ *
+ */
+#define TLV_L(x)		(((x) >> 16) & 0xff)
+#define TLV_T(x)			((x) & 0xff)
+#define TLV_COMPOSITE(x)	((x) >> 31)
+
+/* GENERAL */
+#define T_MAC_ADDRESS			(0x00	| (6 << 16))
+#define T_BSID				(0x01	| (6 << 16))
+#define T_MSK				(0x02	| (64 << 16))
+#define T_RSSI_THRSHLD			(0x03	| (1 << 16))
+#define T_FREQUENCY			(0x04	| (4 << 16))
+#define T_CONN_CS_TYPE			(0x05	| (1 << 16))
+#define T_HOST_IP_VER			(0x06	| (1 << 16))
+#define T_STBY_SCAN_INTERVAL		(0x07	| (4  << 16))
+#define T_OOZ_SCAN_INTERVAL		(0x08	| (4 << 16))
+#define T_IMEI				(0x09	| (8 << 16))
+#define T_PID				(0x0a	| (12 << 16))
+
+#define T_CAPABILITY			(0x1a	| (4 << 16))
+#define T_RELEASE_NUMBER		(0x1b	| (4 << 16))
+#define T_DRIVER_REVISION		(0x1c	| (4 << 16))
+#define T_FW_REVISION			(0x1d	| (4 << 16))
+#define T_MAC_HW_REVISION		(0x1e	| (4 << 16))
+#define T_PHY_HW_REVISION		(0x1f	| (4 << 16))
+
+/* HANDOVER */
+#define T_SCAN_INTERVAL		(0x20	| (1 << 16))
+
+#define T_RSC_RETAIN_TIME		(0x2f	| (2 << 16))
+
+/* SLEEP */
+#define T_TYPE1_ISW			(0x40	| (1 << 16))
+
+#define T_SLP_START_TO			(0x4a	| (2 << 16))
+
+/* IDLE */
+#define T_IDLE_MODE_TO			(0x50	| (2 << 16))
+
+#define T_IDLE_START_TO		(0x54	| (2 << 16))
+
+/* MONITOR */
+#define T_RSSI				(0x60	| (1 << 16))
+#define T_CINR				(0x61	| (1 << 16))
+#define T_TX_POWER			(0x6a   | (1 << 16))
+#define T_CUR_FREQ			(0x7f	| (4 << 16))
+
+
+/* WIMAX */
+#define T_MAX_SUBSCRIPTION		(0xa1	| (1 << 16))
+#define T_MAX_SF			(0xa2	| (1 << 16))
+#define T_PHY_TYPE			(0xa3	| (1 << 16))
+#define T_PKM				(0xa4	| (1 << 16))
+#define T_AUTH_POLICY			(0xa5	| (1 << 16))
+#define T_CS_TYPE			(0xa6	| (2 << 16))
+#define T_VENDOR_NAME			(0xa7	| (0 << 16))
+#define T_MOD_NAME			(0xa8	| (0 << 16))
+#define T_PACKET_FILTER		(0xa9	| (1 << 16))
+#define T_NSP_CHANGE_COUNT		(0xaa	| (4 << 16))
+#define T_RADIO_STATE			(0xab	| (1 << 16))
+#define T_URI_CONTACT_TYPE		(0xac	| (1 << 16))
+#define T_URI_TEXT			(0xad	| (0 << 16))
+#define T_URI				(0xae	| (0 << 16))
+#define T_ENABLE_AUTH			(0xaf	| (1 << 16))
+#define T_TIMEOUT			(0xb0   | (2 << 16))
+#define T_RUN_MODE			(0xb1	| (1 << 16))
+#define T_OMADMT_VER			(0xb2	| (4 << 16))
+/* This is measured in seconds from 00:00:00 GMT January 1, 1970. */
+#define T_RTC_TIME			(0xb3	| (4 << 16))
+#define T_CERT_STATUS			(0xb4	| (4 << 16))
+#define T_CERT_MASK			(0xb5	| (4 << 16))
+#define T_EMSK				(0xb6	| (64 << 16))
+
+/* Subscription TLV */
+#define T_SUBSCRIPTION_LIST		(0xd1	| (0 << 16) | (1 << 31))
+#define T_H_NSPID			(0xd2	| (3 << 16))
+#define T_NSP_NAME			(0xd3	| (0 << 16))
+#define T_SUBSCRIPTION_NAME		(0xd4	| (0 << 16))
+#define T_SUBSCRIPTION_FLAG		(0xd5	| (2 << 16))
+#define T_V_NSPID			(0xd6	| (3 << 16))
+#define T_NAP_ID			(0xd7	| (3 << 16))
+#define T_PREAMBLES			(0xd8	| (15 << 16))
+#define T_BW				(0xd9	| (4 << 16))
+#define T_FFTSIZE			(0xda	| (4 << 16))
+#define T_DUPLEX_MODE			(0xdb	| (4 << 16))
+
+struct hci_s {
+	unsigned short cmd_evt;
+	unsigned short length;
+	unsigned char  data[0];
+} __packed;
+
+#endif
diff --git a/drivers/staging/gdm72xx/netlink_k.c b/drivers/staging/gdm72xx/netlink_k.c
new file mode 100644
index 00000000000..292af0f7f45
--- /dev/null
+++ b/drivers/staging/gdm72xx/netlink_k.c
@@ -0,0 +1,150 @@
+/*
+ * Copyright (c) 2012 GCT Semiconductor, Inc. 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
+ * 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/version.h>
+#include <linux/module.h>
+#include <linux/etherdevice.h>
+#include <linux/netlink.h>
+#include <asm/byteorder.h>
+#include <net/sock.h>
+
+#if !defined(NLMSG_HDRLEN)
+#define NLMSG_HDRLEN	 ((int) NLMSG_ALIGN(sizeof(struct nlmsghdr)))
+#endif
+
+#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_DATA(nlh) \
+	((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_MAX_MSG_LEN			8096
+
+#if defined(DEFINE_MUTEX)
+static DEFINE_MUTEX(netlink_mutex);
+#else
+static struct semaphore netlink_mutex;
+#define mutex_lock(x)		down(x)
+#define mutex_unlock(x)		up(x)
+#endif
+
+static void (*rcv_cb)(struct net_device *dev, u16 type, void *msg, int len);
+
+static void netlink_rcv_cb(struct sk_buff *skb)
+{
+	struct nlmsghdr *nlh;
+	struct net_device *dev;
+	u32 mlen;
+	void *msg;
+	int ifindex;
+
+	if (skb->len >= NLMSG_SPACE(0)) {
+		nlh = (struct nlmsghdr *)skb->data;
+
+		if (skb->len < nlh->nlmsg_len ||
+		nlh->nlmsg_len > ND_MAX_MSG_LEN) {
+			printk(KERN_ERR "Invalid length (%d,%d)\n", skb->len,
+				nlh->nlmsg_len);
+			return;
+		}
+
+		memcpy(&ifindex, ND_NLMSG_IFIDX(nlh), ND_IFINDEX_LEN);
+		msg = ND_NLMSG_DATA(nlh);
+		mlen = ND_NLMSG_R_LEN(nlh);
+
+		if (rcv_cb) {
+			dev = dev_get_by_index(&init_net, ifindex);
+			if (dev) {
+				rcv_cb(dev, nlh->nlmsg_type, msg, mlen);
+				dev_put(dev);
+			} else
+				printk(KERN_ERR "dev_get_by_index(%d) "
+					"is not found.\n", ifindex);
+		} else
+			printk(KERN_ERR "Unregistered Callback\n");
+	}
+}
+
+static void netlink_rcv(struct sk_buff *skb)
+{
+	mutex_lock(&netlink_mutex);
+	netlink_rcv_cb(skb);
+	mutex_unlock(&netlink_mutex);
+}
+
+struct sock *netlink_init(int unit, void (*cb)(struct net_device *dev, u16 type,
+						void *msg, int len))
+{
+	struct sock *sock;
+
+#if !defined(DEFINE_MUTEX)
+	init_MUTEX(&netlink_mutex);
+#endif
+
+	sock = netlink_kernel_create(&init_net, unit, 0, netlink_rcv, NULL,
+					THIS_MODULE);
+
+	if (sock)
+		rcv_cb = cb;
+
+	return sock;
+}
+
+void netlink_exit(struct sock *sock)
+{
+	sock_release(sock->sk_socket);
+}
+
+int netlink_send(struct sock *sock, int group, u16 type, void *msg, int len)
+{
+	static u32 seq;
+	struct sk_buff *skb = NULL;
+	struct nlmsghdr *nlh;
+	int ret = 0;
+
+	if (group > ND_MAX_GROUP) {
+		printk(KERN_ERR "Group %d is invalied.\n", group);
+		printk(KERN_ERR "Valid group is 0 ~ %d.\n", ND_MAX_GROUP);
+		return -EINVAL;
+	}
+
+	skb = alloc_skb(NLMSG_SPACE(len), GFP_ATOMIC);
+	if (!skb) {
+		printk(KERN_ERR "netlink_broadcast ret=%d\n", ret);
+		return -ENOMEM;
+	}
+
+	seq++;
+	nlh = NLMSG_PUT(skb, 0, seq, type, len);
+	memcpy(NLMSG_DATA(nlh), msg, len);
+
+	NETLINK_CB(skb).pid = 0;
+	NETLINK_CB(skb).dst_group = 0;
+
+	ret = netlink_broadcast(sock, skb, 0, group+1, GFP_ATOMIC);
+
+	if (!ret)
+		return len;
+	else {
+		if (ret != -ESRCH) {
+			printk(KERN_ERR "netlink_broadcast g=%d, t=%d, l=%d, r=%d\n",
+				group, type, len, ret);
+		}
+		ret = 0;
+	}
+
+nlmsg_failure:
+	return ret;
+}
diff --git a/drivers/staging/gdm72xx/netlink_k.h b/drivers/staging/gdm72xx/netlink_k.h
new file mode 100644
index 00000000000..1dffaa6156e
--- /dev/null
+++ b/drivers/staging/gdm72xx/netlink_k.h
@@ -0,0 +1,24 @@
+/*
+ * Copyright (c) 2012 GCT Semiconductor, Inc. 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
+ * 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.
+ */
+
+#if !defined(NETLINK_H_20081202)
+#define NETLINK_H_20081202
+#include <linux/netdevice.h>
+#include <net/sock.h>
+
+struct sock *netlink_init(int unit,
+	void (*cb)(struct net_device *dev, u16 type, void *msg, int len));
+void netlink_exit(struct sock *sock);
+int netlink_send(struct sock *sock, int group, u16 type, void *msg, int len);
+
+#endif
diff --git a/drivers/staging/gdm72xx/sdio_boot.c b/drivers/staging/gdm72xx/sdio_boot.c
new file mode 100644
index 00000000000..6ff4dc37252
--- /dev/null
+++ b/drivers/staging/gdm72xx/sdio_boot.c
@@ -0,0 +1,159 @@
+/*
+ * Copyright (c) 2012 GCT Semiconductor, Inc. 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
+ * 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/version.h>
+#include <linux/kernel.h>
+#include <linux/init.h>
+#include <linux/mm.h>
+#include <linux/uaccess.h>
+#include <linux/fs.h>
+#include <linux/sched.h>
+#include <linux/slab.h>
+
+#include <linux/mmc/core.h>
+#include <linux/mmc/card.h>
+#include <linux/mmc/sdio_func.h>
+
+#include "gdm_sdio.h"
+
+#define TYPE_A_HEADER_SIZE	4
+#define TYPE_A_LOOKAHEAD_SIZE   16
+#define YMEM0_SIZE			0x8000	/* 32kbytes */
+#define DOWNLOAD_SIZE		(YMEM0_SIZE - TYPE_A_HEADER_SIZE)
+
+#define KRN_PATH	"/lib/firmware/gdm72xx/gdmskrn.bin"
+#define RFS_PATH	"/lib/firmware/gdm72xx/gdmsrfs.bin"
+
+static u8 *tx_buf;
+
+static int ack_ready(struct sdio_func *func)
+{
+	unsigned long start = jiffies;
+	u8 val;
+	int ret;
+
+	while ((jiffies - start) < HZ) {
+		val = sdio_readb(func, 0x13, &ret);
+		if (val & 0x01)
+			return 1;
+		schedule();
+	}
+
+	return 0;
+}
+
+static int download_image(struct sdio_func *func, char *img_name)
+{
+	int ret = 0, len, size, pno;
+	struct file *filp = NULL;
+	struct inode *inode = NULL;
+	u8 *buf = tx_buf;
+	loff_t pos = 0;
+
+	filp = filp_open(img_name, O_RDONLY | O_LARGEFILE, 0);
+	if (IS_ERR(filp)) {
+		printk(KERN_ERR "Can't find %s.\n", img_name);
+		return -ENOENT;
+	}
+
+	if (filp->f_dentry)
+		inode = filp->f_dentry->d_inode;
+	if (!inode || !S_ISREG(inode->i_mode)) {
+		printk(KERN_ERR "Invalid file type: %s\n", img_name);
+		ret = -EINVAL;
+		goto out;
+	}
+
+	size = i_size_read(inode->i_mapping->host);
+	if (size <= 0) {
+		printk(KERN_ERR "Unable to find file size: %s\n", img_name);
+		ret = size;
+		goto out;
+	}
+
+	pno = 0;
+	while ((len = filp->f_op->read(filp, buf + TYPE_A_HEADER_SIZE,
+					DOWNLOAD_SIZE, &pos))) {
+		if (len < 0) {
+			ret = -1;
+			goto out;
+		}
+
+		buf[0] = len & 0xff;
+		buf[1] = (len >> 8) & 0xff;
+		buf[2] = (len >> 16) & 0xff;
+
+		if (pos >= size)	/* The last packet */
+			buf[3] = 2;
+		else
+			buf[3] = 0;
+
+		ret = sdio_memcpy_toio(func, 0, buf, len + TYPE_A_HEADER_SIZE);
+		if (ret < 0) {
+			printk(KERN_ERR "gdmwm: send image error: "
+				"packet number = %d ret = %d\n", pno, ret);
+			goto out;
+		}
+		if (buf[3] == 2)	/* The last packet */
+			break;
+		if (!ack_ready(func)) {
+			ret = -EIO;
+			printk(KERN_ERR "gdmwm: Ack is not ready.\n");
+			goto out;
+		}
+		ret = sdio_memcpy_fromio(func, buf, 0, TYPE_A_LOOKAHEAD_SIZE);
+		if (ret < 0) {
+			printk(KERN_ERR "gdmwm: receive ack error: "
+				"packet number = %d ret = %d\n", pno, ret);
+			goto out;
+		}
+		sdio_writeb(func, 0x01, 0x13, &ret);
+		sdio_writeb(func, 0x00, 0x10, &ret);	/* PCRRT */
+
+		pno++;
+	}
+out:
+	filp_close(filp, current->files);
+	return ret;
+}
+
+int sdio_boot(struct sdio_func *func)
+{
+	static mm_segment_t fs;
+	int ret;
+
+	tx_buf = kmalloc(YMEM0_SIZE, GFP_KERNEL);
+	if (tx_buf == NULL) {
+		printk(KERN_ERR "Error: kmalloc: %s %d\n", __func__, __LINE__);
+		return -ENOMEM;
+	}
+
+	fs = get_fs();
+	set_fs(get_ds());
+
+	ret = download_image(func, KRN_PATH);
+	if (ret)
+		goto restore_fs;
+	printk(KERN_INFO "GCT: Kernel download success.\n");
+
+	ret = download_image(func, RFS_PATH);
+	if (ret)
+		goto restore_fs;
+	printk(KERN_INFO "GCT: Filesystem download success.\n");
+
+restore_fs:
+	set_fs(fs);
+	kfree(tx_buf);
+	return ret;
+}
diff --git a/drivers/staging/gdm72xx/sdio_boot.h b/drivers/staging/gdm72xx/sdio_boot.h
new file mode 100644
index 00000000000..373ac28063c
--- /dev/null
+++ b/drivers/staging/gdm72xx/sdio_boot.h
@@ -0,0 +1,21 @@
+/*
+ * Copyright (c) 2012 GCT Semiconductor, Inc. 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
+ * 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.
+ */
+
+#ifndef __SDIO_BOOT_H__
+#define __SDIO_BOOT_H__
+
+struct sdio_func;
+
+extern int sdio_boot(struct sdio_func *func);
+
+#endif /* __SDIO_BOOT_H__ */
diff --git a/drivers/staging/gdm72xx/usb_boot.c b/drivers/staging/gdm72xx/usb_boot.c
new file mode 100644
index 00000000000..5a0e030220d
--- /dev/null
+++ b/drivers/staging/gdm72xx/usb_boot.c
@@ -0,0 +1,404 @@
+/*
+ * Copyright (c) 2012 GCT Semiconductor, Inc. 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
+ * 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/uaccess.h>
+#include <linux/module.h>
+#include <linux/version.h>
+#include <linux/kernel.h>
+#include <linux/mm.h>
+#include <linux/usb.h>
+#include <linux/unistd.h>
+#include <linux/slab.h>
+
+#include <asm/byteorder.h>
+#include "gdm_usb.h"
+#include "usb_boot.h"
+
+#define DN_KERNEL_MAGIC_NUMBER		0x10760001
+#define DN_ROOTFS_MAGIC_NUMBER		0x10760002
+
+#define DOWNLOAD_SIZE	1024
+
+#define DH2B(x)		__cpu_to_be32(x)
+#define DL2H(x)		__le32_to_cpu(x)
+
+#define MIN(a, b)	((a) > (b) ? (b) : (a))
+
+#define MAX_IMG_CNT		16
+#define UIMG_PATH		"/lib/firmware/gdm72xx/gdmuimg.bin"
+#define KERN_PATH		"/lib/firmware/gdm72xx/zImage"
+#define FS_PATH			"/lib/firmware/gdm72xx/ramdisk.jffs2"
+
+struct dn_header {
+	u32	magic_num;
+	u32	file_size;
+};
+
+struct img_header {
+	u32		magic_code;
+	u32		count;
+	u32		len;
+	u32		offset[MAX_IMG_CNT];
+	char	hostname[32];
+	char	date[32];
+};
+
+struct fw_info {
+	u32		id;
+	u32		len;
+	u32		kernel_len;
+	u32		rootfs_len;
+	u32		kernel_offset;
+	u32		rootfs_offset;
+	u32		fw_ver;
+	u32		mac_ver;
+	char	hostname[32];
+	char	userid[16];
+	char	date[32];
+	char	user_desc[128];
+};
+
+static void array_le32_to_cpu(u32 *arr, int num)
+{
+	int i;
+	for (i = 0; i < num; i++, arr++)
+		*arr = DL2H(*arr);
+}
+
+static u8 *tx_buf;
+
+static int gdm_wibro_send(struct usb_device *usbdev, void *data, int len)
+{
+	int ret;
+	int actual;
+
+	ret = usb_bulk_msg(usbdev, usb_sndbulkpipe(usbdev, 1), data, len,
+			&actual, 1000);
+
+	if (ret < 0) {
+		printk(KERN_ERR "Error : usb_bulk_msg ( result = %d )\n", ret);
+		return ret;
+	}
+	return 0;
+}
+
+static int gdm_wibro_recv(struct usb_device *usbdev, void *data, int len)
+{
+	int ret;
+	int actual;
+
+	ret = usb_bulk_msg(usbdev, usb_rcvbulkpipe(usbdev, 2), data, len,
+			&actual, 5000);
+
+	if (ret < 0) {
+		printk(KERN_ERR "Error : usb_bulk_msg(recv) ( result = %d )\n",
+			ret);
+		return ret;
+	}
+	return 0;
+}
+
+static int download_image(struct usb_device *usbdev, struct file *filp,
+				loff_t *pos, u32 img_len, u32 magic_num)
+{
+	struct dn_header h;
+	int ret = 0;
+	u32 size;
+	int len, readn;
+
+	size = (img_len + DOWNLOAD_SIZE - 1) & ~(DOWNLOAD_SIZE - 1);
+	h.magic_num = DH2B(magic_num);
+	h.file_size = DH2B(size);
+
+	ret = gdm_wibro_send(usbdev, &h, sizeof(h));
+	if (ret < 0)
+		goto out;
+
+	readn = 0;
+	while ((len = filp->f_op->read(filp, tx_buf, DOWNLOAD_SIZE, pos))) {
+
+		if (len < 0) {
+			ret = -1;
+			goto out;
+		}
+		readn += len;
+
+		ret = gdm_wibro_send(usbdev, tx_buf, DOWNLOAD_SIZE);
+		if (ret < 0)
+			goto out;
+		if (readn >= img_len)
+			break;
+	}
+
+	if (readn < img_len) {
+		printk(KERN_ERR "gdmwm: Cannot read to the requested size. "
+			"Read = %d Requested = %d\n", readn, img_len);
+		ret = -EIO;
+	}
+out:
+
+	return ret;
+}
+
+int usb_boot(struct usb_device *usbdev, u16 pid)
+{
+	int i, ret = 0;
+	struct file *filp = NULL;
+	struct inode *inode = NULL;
+	static mm_segment_t fs;
+	struct img_header hdr;
+	struct fw_info fw_info;
+	loff_t pos = 0;
+	char *img_name = UIMG_PATH;
+	int len;
+
+	tx_buf = kmalloc(DOWNLOAD_SIZE, GFP_KERNEL);
+	if (tx_buf == NULL) {
+		printk(KERN_ERR "Error: kmalloc\n");
+		return -ENOMEM;
+	}
+
+	fs = get_fs();
+	set_fs(get_ds());
+
+	filp = filp_open(img_name, O_RDONLY | O_LARGEFILE, 0);
+	if (IS_ERR(filp)) {
+		printk(KERN_ERR "Can't find %s.\n", img_name);
+		set_fs(fs);
+		ret = -ENOENT;
+		goto restore_fs;
+	}
+
+	if (filp->f_dentry)
+		inode = filp->f_dentry->d_inode;
+	if (!inode || !S_ISREG(inode->i_mode)) {
+		printk(KERN_ERR "Invalid file type: %s\n", img_name);
+		ret = -EINVAL;
+		goto out;
+	}
+
+	len = filp->f_op->read(filp, (u8 *)&hdr, sizeof(hdr), &pos);
+	if (len != sizeof(hdr)) {
+		printk(KERN_ERR "gdmwm: Cannot read the image info.\n");
+		ret = -EIO;
+		goto out;
+	}
+
+	array_le32_to_cpu((u32 *)&hdr, 19);
+#if 0
+	if (hdr.magic_code != 0x10767fff) {
+		printk(KERN_ERR "gdmwm: Invalid magic code 0x%08x\n",
+			hdr.magic_code);
+		ret = -EINVAL;
+		goto out;
+	}
+#endif
+	if (hdr.count > MAX_IMG_CNT) {
+		printk(KERN_ERR "gdmwm: Too many images. %d\n", hdr.count);
+		ret = -EINVAL;
+		goto out;
+	}
+
+	for (i = 0; i < hdr.count; i++) {
+		if (hdr.offset[i] > hdr.len) {
+			printk(KERN_ERR "gdmwm: Invalid offset. "
+				"Entry = %d Offset = 0x%08x "
+				"Image length = 0x%08x\n",
+				i, hdr.offset[i], hdr.len);
+			ret = -EINVAL;
+			goto out;
+		}
+
+		pos = hdr.offset[i];
+		len = filp->f_op->read(filp, (u8 *)&fw_info, sizeof(fw_info),
+					&pos);
+		if (len != sizeof(fw_info)) {
+			printk(KERN_ERR "gdmwm: Cannot read the FW info.\n");
+			ret = -EIO;
+			goto out;
+		}
+
+		array_le32_to_cpu((u32 *)&fw_info, 8);
+#if 0
+		if ((fw_info.id & 0xfffff000) != 0x10767000) {
+			printk(KERN_ERR "gdmwm: Invalid FW id. 0x%08x\n",
+				fw_info.id);
+			ret = -EIO;
+			goto out;
+		}
+#endif
+
+		if ((fw_info.id & 0xffff) != pid)
+			continue;
+
+		pos = hdr.offset[i] + fw_info.kernel_offset;
+		ret = download_image(usbdev, filp, &pos, fw_info.kernel_len,
+				DN_KERNEL_MAGIC_NUMBER);
+		if (ret < 0)
+			goto out;
+		printk(KERN_INFO "GCT: Kernel download success.\n");
+
+		pos = hdr.offset[i] + fw_info.rootfs_offset;
+		ret = download_image(usbdev, filp, &pos, fw_info.rootfs_len,
+				DN_ROOTFS_MAGIC_NUMBER);
+		if (ret < 0)
+			goto out;
+		printk(KERN_INFO "GCT: Filesystem download success.\n");
+
+		break;
+	}
+
+	if (i == hdr.count) {
+		printk(KERN_ERR "Firmware for gsk%x is not installed.\n", pid);
+		ret = -EINVAL;
+	}
+out:
+	filp_close(filp, current->files);
+
+restore_fs:
+	set_fs(fs);
+	kfree(tx_buf);
+	return ret;
+}
+
+/*#define GDM7205_PADDING		256 */
+#define DOWNLOAD_CHUCK			2048
+#define KERNEL_TYPE_STRING		"linux"
+#define FS_TYPE_STRING			"rootfs"
+
+static int em_wait_ack(struct usb_device *usbdev, int send_zlp)
+{
+	int ack;
+	int ret = -1;
+
+	if (send_zlp) {
+		/*Send ZLP*/
+		ret = gdm_wibro_send(usbdev, NULL, 0);
+		if (ret < 0)
+			goto out;
+	}
+
+	/*Wait for ACK*/
+	ret = gdm_wibro_recv(usbdev, &ack, sizeof(ack));
+	if (ret < 0)
+		goto out;
+out:
+	return ret;
+}
+
+static int em_download_image(struct usb_device *usbdev, char *path,
+				char *type_string)
+{
+	struct file *filp;
+	struct inode *inode;
+	static mm_segment_t fs;
+	char *buf = NULL;
+	loff_t pos = 0;
+	int ret = 0;
+	int len, readn = 0;
+	#if defined(GDM7205_PADDING)
+	const int pad_size = GDM7205_PADDING;
+	#else
+	const int pad_size = 0;
+	#endif
+
+	fs = get_fs();
+	set_fs(get_ds());
+
+	filp = filp_open(path, O_RDONLY | O_LARGEFILE, 0);
+	if (IS_ERR(filp)) {
+		printk(KERN_ERR "Can't find %s.\n", path);
+		set_fs(fs);
+		ret = -ENOENT;
+		goto restore_fs;
+	}
+
+	if (filp->f_dentry) {
+		inode = filp->f_dentry->d_inode;
+		if (!inode || !S_ISREG(inode->i_mode)) {
+			printk(KERN_ERR "Invalid file type: %s\n", path);
+			ret = -EINVAL;
+			goto out;
+		}
+	}
+
+	buf = kmalloc(DOWNLOAD_CHUCK + pad_size, GFP_KERNEL);
+	if (buf == NULL) {
+		printk(KERN_ERR "Error: kmalloc\n");
+		return -ENOMEM;
+	}
+
+	strcpy(buf+pad_size, type_string);
+	ret = gdm_wibro_send(usbdev, buf, strlen(type_string)+pad_size);
+	if (ret < 0)
+		goto out;
+
+	while ((len = filp->f_op->read(filp, buf+pad_size, DOWNLOAD_CHUCK,
+					&pos))) {
+		if (len < 0) {
+			ret = -1;
+			goto out;
+		}
+		readn += len;
+
+		ret = gdm_wibro_send(usbdev, buf, len+pad_size);
+		if (ret < 0)
+			goto out;
+
+		ret = em_wait_ack(usbdev, ((len+pad_size) % 512 == 0));
+		if (ret < 0)
+			goto out;
+	}
+
+	ret = em_wait_ack(usbdev, 1);
+	if (ret < 0)
+		goto out;
+
+out:
+	filp_close(filp, current->files);
+
+restore_fs:
+	set_fs(fs);
+
+	kfree(buf);
+
+	return ret;
+}
+
+static int em_fw_reset(struct usb_device *usbdev)
+{
+	int ret;
+
+	/*Send ZLP*/
+	ret = gdm_wibro_send(usbdev, NULL, 0);
+	return ret;
+}
+
+int usb_emergency(struct usb_device *usbdev)
+{
+	int ret;
+
+	ret = em_download_image(usbdev, KERN_PATH, KERNEL_TYPE_STRING);
+	if (ret < 0)
+		goto out;
+	printk(KERN_INFO "GCT Emergency: Kernel download success.\n");
+
+	ret = em_download_image(usbdev, FS_PATH, FS_TYPE_STRING);
+	if (ret < 0)
+		goto out;
+	printk(KERN_INFO "GCT Emergency: Filesystem download success.\n");
+
+	ret = em_fw_reset(usbdev);
+out:
+	return ret;
+}
diff --git a/drivers/staging/gdm72xx/usb_boot.h b/drivers/staging/gdm72xx/usb_boot.h
new file mode 100644
index 00000000000..c715cd3cd30
--- /dev/null
+++ b/drivers/staging/gdm72xx/usb_boot.h
@@ -0,0 +1,22 @@
+/*
+ * Copyright (c) 2012 GCT Semiconductor, Inc. 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
+ * 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.
+ */
+
+#ifndef __USB_BOOT_H__
+#define __USB_BOOT_H__
+
+struct usb_device;
+
+extern int usb_boot(struct usb_device *usbdev, u16 pid);
+extern int usb_emergency(struct usb_device *usbdev);
+
+#endif /* __USB_BOOT_H__ */
diff --git a/drivers/staging/gdm72xx/usb_ids.h b/drivers/staging/gdm72xx/usb_ids.h
new file mode 100644
index 00000000000..b34616b7203
--- /dev/null
+++ b/drivers/staging/gdm72xx/usb_ids.h
@@ -0,0 +1,82 @@
+/*
+ * Copyright (c) 2012 GCT Semiconductor, Inc. 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
+ * 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.
+ */
+
+#ifndef __USB_IDS_H__
+#define __USB_IDS_H__
+
+/*You can replace vendor-ID as yours.*/
+#define GCT_VID			0x1076
+
+/*You can replace product-ID as yours.*/
+#define GCT_PID1			0x7e00
+#define GCT_PID2			0x7f00
+
+#define USB_DEVICE_ID_MATCH_DEVICE_INTERFACE	\
+	(USB_DEVICE_ID_MATCH_DEVICE | USB_DEVICE_ID_MATCH_INT_CLASS)
+
+#define USB_DEVICE_INTF(vend, prod, intf)	\
+	.match_flags = USB_DEVICE_ID_MATCH_DEVICE_INTERFACE,	\
+	.idVendor = (vend), .idProduct = (prod), .bInterfaceClass = (intf)
+
+#define EMERGENCY_PID		0x720f
+#define BL_PID_MASK			0xffc0
+
+#define USB_DEVICE_BOOTLOADER(vid, pid)	\
+	{USB_DEVICE((vid), ((pid)&BL_PID_MASK)|B_DOWNLOAD)},	\
+	{USB_DEVICE((vid), ((pid)&BL_PID_MASK)|B_DOWNLOAD|B_DIFF_DL_DRV)}
+
+#define USB_DEVICE_CDC_DATA(vid, pid)	\
+	{USB_DEVICE_INTF((vid), (pid), USB_CLASS_CDC_DATA)}
+
+static const struct usb_device_id id_table[] = {
+	USB_DEVICE_BOOTLOADER(GCT_VID, GCT_PID1),
+	USB_DEVICE_CDC_DATA(GCT_VID, GCT_PID1),
+	USB_DEVICE_CDC_DATA(GCT_VID, GCT_PID1+0x1),
+	USB_DEVICE_CDC_DATA(GCT_VID, GCT_PID1+0x2),
+	USB_DEVICE_CDC_DATA(GCT_VID, GCT_PID1+0x3),
+	USB_DEVICE_CDC_DATA(GCT_VID, GCT_PID1+0x4),
+	USB_DEVICE_CDC_DATA(GCT_VID, GCT_PID1+0x5),
+	USB_DEVICE_CDC_DATA(GCT_VID, GCT_PID1+0x6),
+	USB_DEVICE_CDC_DATA(GCT_VID, GCT_PID1+0x7),
+	USB_DEVICE_CDC_DATA(GCT_VID, GCT_PID1+0x8),
+	USB_DEVICE_CDC_DATA(GCT_VID, GCT_PID1+0x9),
+	USB_DEVICE_CDC_DATA(GCT_VID, GCT_PID1+0xa),
+	USB_DEVICE_CDC_DATA(GCT_VID, GCT_PID1+0xb),
+	USB_DEVICE_CDC_DATA(GCT_VID, GCT_PID1+0xc),
+	USB_DEVICE_CDC_DATA(GCT_VID, GCT_PID1+0xd),
+	USB_DEVICE_CDC_DATA(GCT_VID, GCT_PID1+0xe),
+	USB_DEVICE_CDC_DATA(GCT_VID, GCT_PID1+0xf),
+
+	USB_DEVICE_BOOTLOADER(GCT_VID, GCT_PID2),
+	USB_DEVICE_CDC_DATA(GCT_VID, GCT_PID2),
+	USB_DEVICE_CDC_DATA(GCT_VID, GCT_PID2+0x1),
+	USB_DEVICE_CDC_DATA(GCT_VID, GCT_PID2+0x2),
+	USB_DEVICE_CDC_DATA(GCT_VID, GCT_PID2+0x3),
+	USB_DEVICE_CDC_DATA(GCT_VID, GCT_PID2+0x4),
+	USB_DEVICE_CDC_DATA(GCT_VID, GCT_PID2+0x5),
+	USB_DEVICE_CDC_DATA(GCT_VID, GCT_PID2+0x6),
+	USB_DEVICE_CDC_DATA(GCT_VID, GCT_PID2+0x7),
+	USB_DEVICE_CDC_DATA(GCT_VID, GCT_PID2+0x8),
+	USB_DEVICE_CDC_DATA(GCT_VID, GCT_PID2+0x9),
+	USB_DEVICE_CDC_DATA(GCT_VID, GCT_PID2+0xa),
+	USB_DEVICE_CDC_DATA(GCT_VID, GCT_PID2+0xb),
+	USB_DEVICE_CDC_DATA(GCT_VID, GCT_PID2+0xc),
+	USB_DEVICE_CDC_DATA(GCT_VID, GCT_PID2+0xd),
+	USB_DEVICE_CDC_DATA(GCT_VID, GCT_PID2+0xe),
+	USB_DEVICE_CDC_DATA(GCT_VID, GCT_PID2+0xf),
+
+	{USB_DEVICE(GCT_VID, EMERGENCY_PID)},
+	{ }
+};
+
+#endif /* __USB_IDS_H__ */
diff --git a/drivers/staging/gdm72xx/wm_ioctl.h b/drivers/staging/gdm72xx/wm_ioctl.h
new file mode 100644
index 00000000000..9f46e06f230
--- /dev/null
+++ b/drivers/staging/gdm72xx/wm_ioctl.h
@@ -0,0 +1,97 @@
+/*
+ * Copyright (c) 2012 GCT Semiconductor, Inc. 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
+ * 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.
+ */
+
+#if !defined(WM_IOCTL_H_20080714)
+#define WM_IOCTL_H_20080714
+#if !defined(__KERNEL__)
+#include <net/if.h>
+#endif
+
+#define NETLINK_WIMAX	31
+
+#define SIOCWMIOCTL			SIOCDEVPRIVATE
+
+#define SIOCG_DATA			0x8D10
+#define SIOCS_DATA			0x8D11
+
+enum {
+	SIOC_DATA_FSM,
+	SIOC_DATA_NETLIST,
+	SIOC_DATA_CONNNSP,
+	SIOC_DATA_CONNCOMP,
+	SIOC_DATA_PROFILEID,
+
+	SIOC_DATA_END
+};
+
+#define SIOC_DATA_MAX			16
+
+/* FSM */
+enum {
+	M_INIT = 0,
+	M_OPEN_OFF,
+	M_OPEN_ON,
+	M_SCAN,
+	M_CONNECTING,
+	M_CONNECTED,
+	M_FSM_END,
+
+	C_INIT = 0,
+	C_CONNSTART,
+	C_ASSOCSTART,
+	C_RNG,
+	C_SBC,
+	C_AUTH,
+	C_REG,
+	C_DSX,
+	C_ASSOCCOMPLETE,
+	C_CONNCOMPLETE,
+	C_FSM_END,
+
+	D_INIT = 0,
+	D_READY,
+	D_LISTEN,
+	D_IPACQUISITION,
+
+	END_FSM
+};
+
+struct fsm_s {
+	int		m_status;	/*main status*/
+	int		c_status;	/*connection status*/
+	int		d_status;	/*oma-dm status*/
+};
+
+struct data_s {
+	int		size;
+	void	*buf;
+};
+
+struct wm_req_s {
+	union {
+		char	ifrn_name[IFNAMSIZ];
+	} ifr_ifrn;
+
+	unsigned short	cmd;
+
+	unsigned short	data_id;
+	struct data_s	data;
+
+/* NOTE: sizeof(struct wm_req_s) must be less than sizeof(struct ifreq). */
+};
+
+#ifndef ifr_name
+#define ifr_name	ifr_ifrn.ifrn_name
+#endif
+
+#endif
diff --git a/drivers/staging/iio/Documentation/device.txt b/drivers/staging/iio/Documentation/device.txt
index 8926f2448cc..0338c7cd0a8 100644
--- a/drivers/staging/iio/Documentation/device.txt
+++ b/drivers/staging/iio/Documentation/device.txt
@@ -8,7 +8,7 @@ The crucial structure for device drivers in iio is iio_dev.
 
 First allocate one using:
 
-struct iio_dev *indio_dev = iio_allocate_device(sizeof(struct chip_state));
+struct iio_dev *indio_dev = iio_device_alloc(sizeof(struct chip_state));
 where chip_state is a structure of local state data for this instance of
 the chip.
 
@@ -78,4 +78,4 @@ be registered afterwards (otherwise the whole parentage of devices
 gets confused)
 
 On remove, iio_device_unregister(indio_dev) will remove the device from
-the core, and iio_free_device will clean up.
+the core, and iio_device_free will clean up.
diff --git a/drivers/staging/iio/Documentation/generic_buffer.c b/drivers/staging/iio/Documentation/generic_buffer.c
index 69a05b9456d..bf553356fda 100644
--- a/drivers/staging/iio/Documentation/generic_buffer.c
+++ b/drivers/staging/iio/Documentation/generic_buffer.c
@@ -60,9 +60,9 @@ void print2byte(int input, struct iio_channel_info *info)
 	/* First swap if incorrect endian */
 
 	if (info->be)
-		input = be16toh((uint_16t)input);
+		input = be16toh((uint16_t)input);
 	else
-		input = le16toh((uint_16t)input);
+		input = le16toh((uint16_t)input);
 
 	/* shift before conversion to avoid sign extension
 	   of left aligned data */
diff --git a/drivers/staging/iio/Documentation/iio_event_monitor.c b/drivers/staging/iio/Documentation/iio_event_monitor.c
index 0d21a277305..22275845fb1 100644
--- a/drivers/staging/iio/Documentation/iio_event_monitor.c
+++ b/drivers/staging/iio/Documentation/iio_event_monitor.c
@@ -27,7 +27,7 @@
 #include <fcntl.h>
 #include <sys/ioctl.h>
 #include "iio_utils.h"
-#include "../events.h"
+#include <linux/iio/events.h>
 
 static const char * const iio_chan_type_name_spec[] = {
 	[IIO_VOLTAGE] = "voltage",
diff --git a/drivers/staging/iio/Documentation/light/sysfs-bus-iio-light-tsl2583 b/drivers/staging/iio/Documentation/light/sysfs-bus-iio-light-tsl2583
new file mode 100755
index 00000000000..470f7ad9c07
--- /dev/null
+++ b/drivers/staging/iio/Documentation/light/sysfs-bus-iio-light-tsl2583
@@ -0,0 +1,6 @@
+What:		/sys/bus/iio/devices/device[n]/in_illuminance0_calibrate
+KernelVersion:	2.6.37
+Contact:	linux-iio@vger.kernel.org
+Description:
+		This property causes an internal calibration of the als gain trim
+		value which is later used in calculating illuminance in lux.
diff --git a/drivers/staging/iio/Documentation/light/sysfs-bus-iio-light-tsl2x7x b/drivers/staging/iio/Documentation/light/sysfs-bus-iio-light-tsl2x7x
new file mode 100755
index 00000000000..b2798b258bf
--- /dev/null
+++ b/drivers/staging/iio/Documentation/light/sysfs-bus-iio-light-tsl2x7x
@@ -0,0 +1,13 @@
+What:		/sys/bus/iio/devices/device[n]/in_illuminance0_calibrate
+KernelVersion:	3.3-rc1
+Contact:	linux-iio@vger.kernel.org
+Description:
+		Causes an internal calibration of the als gain trim
+		value which is later used in calculating illuminance in lux.
+
+What:		/sys/bus/iio/devices/device[n]/in_proximity0_calibrate
+KernelVersion:	3.3-rc1
+Contact:	linux-iio@vger.kernel.org
+Description:
+		Causes a recalculation and adjustment to the
+		proximity_thresh_rising_value.
diff --git a/drivers/staging/iio/Documentation/sysfs-bus-iio b/drivers/staging/iio/Documentation/sysfs-bus-iio
deleted file mode 100644
index 46a995d6d26..00000000000
--- a/drivers/staging/iio/Documentation/sysfs-bus-iio
+++ /dev/null
@@ -1,741 +0,0 @@
-What:		/sys/bus/iio/devices/iio:deviceX
-KernelVersion:	2.6.35
-Contact:	linux-iio@vger.kernel.org
-Description:
-		Hardware chip or device accessed by one communication port.
-		Corresponds to a grouping of sensor channels. X is the IIO
-		index of the device.
-
-What:		/sys/bus/iio/devices/triggerX
-KernelVersion:	2.6.35
-Contact:	linux-iio@vger.kernel.org
-Description:
-		An event driven driver of data capture to an in kernel buffer.
-		May be provided by a device driver that also has an IIO device
-		based on hardware generated events (e.g. data ready) or
-		provided by a separate driver for other hardware (e.g.
-		periodic timer, GPIO or high resolution timer).
-		Contains trigger type specific elements. These do not
-		generalize well and hence are not documented in this file.
-		X is the IIO index of the trigger.
-
-What:		/sys/bus/iio/devices/iio:deviceX/buffer
-KernelVersion:	2.6.35
-Contact:	linux-iio@vger.kernel.org
-Description:
-		Directory of attributes relating to the buffer for the device.
-
-What:		/sys/bus/iio/devices/iio:deviceX/name
-KernelVersion:	2.6.35
-Contact:	linux-iio@vger.kernel.org
-Description:
-		Description of the physical chip / device for device X.
-		Typically a part number.
-
-What:		/sys/bus/iio/devices/iio:deviceX/sampling_frequency
-What:		/sys/bus/iio/devices/iio:deviceX/buffer/sampling_frequency
-What:		/sys/bus/iio/devices/triggerX/sampling_frequency
-KernelVersion:	2.6.35
-Contact:	linux-iio@vger.kernel.org
-Description:
-		Some devices have internal clocks.  This parameter sets the
-		resulting sampling frequency.  In many devices this
-		parameter has an effect on input filters etc rather than
-		simply controlling when the input is sampled.  As this
-		effects datardy triggers, hardware buffers and the sysfs
-		direct access interfaces, it may be found in any of the
-		relevant directories.  If it effects all of the above
-		then it is to be found in the base device directory.
-
-What:		/sys/bus/iio/devices/iio:deviceX/sampling_frequency_available
-What:		/sys/.../iio:deviceX/buffer/sampling_frequency_available
-What:		/sys/bus/iio/devices/triggerX/sampling_frequency_available
-KernelVersion:	2.6.35
-Contact:	linux-iio@vger.kernel.org
-Description:
-		When the internal sampling clock can only take a small
-		discrete set of values, this file lists those available.
-
-What:		/sys/bus/iio/devices/iio:deviceX/oversampling_ratio
-KernelVersion:	2.6.38
-Contact:	linux-iio@vger.kernel.org
-Description:
-		Hardware dependent ADC oversampling. Controls the sampling ratio
-		of the digital filter if available.
-
-What:		/sys/bus/iio/devices/iio:deviceX/oversampling_ratio_available
-KernelVersion:	2.6.38
-Contact:	linux-iio@vger.kernel.org
-Description:
-		Hardware dependent values supported by the oversampling filter.
-
-What:		/sys/bus/iio/devices/iio:deviceX/in_voltageY_raw
-What:		/sys/bus/iio/devices/iio:deviceX/in_voltageY_supply_raw
-KernelVersion:	2.6.35
-Contact:	linux-iio@vger.kernel.org
-Description:
-		Raw (unscaled no bias removal etc) voltage measurement from
-		channel Y. In special cases where the channel does not
-		correspond to externally available input one of the named
-		versions may be used. The number must always be specified and
-		unique to allow association with event codes. Units after
-		application of scale and offset are microvolts.
-
-What:		/sys/bus/iio/devices/iio:deviceX/in_voltageY-voltageZ_raw
-KernelVersion:	2.6.35
-Contact:	linux-iio@vger.kernel.org
-Description:
-		Raw (unscaled) differential voltage measurement equivalent to
-		channel Y - channel Z where these channel numbers apply to the
-		physically equivalent inputs when non differential readings are
-		separately available. In differential only parts, then all that
-		is required is a consistent labeling.  Units after application
-		of scale and offset are microvolts.
-
-What:		/sys/bus/iio/devices/iio:deviceX/in_capacitanceY_raw
-KernelVersion:	3.2
-Contact:	linux-iio@vger.kernel.org
-Description:
-		Raw capacitance measurement from channel Y. Units after
-		application of scale and offset are nanofarads.
-
-What:		/sys/.../iio:deviceX/in_capacitanceY-in_capacitanceZ_raw
-KernelVersion:	3.2
-Contact:	linux-iio@vger.kernel.org
-Description:
-		Raw differential capacitance measurement equivalent to
-		channel Y - channel Z where these channel numbers apply to the
-		physically equivalent inputs when non differential readings are
-		separately available. In differential only parts, then all that
-		is required is a consistent labeling.  Units after application
-		of scale and offset are nanofarads..
-
-What:		/sys/bus/iio/devices/iio:deviceX/in_temp_raw
-What:		/sys/bus/iio/devices/iio:deviceX/in_tempX_raw
-What:		/sys/bus/iio/devices/iio:deviceX/in_temp_x_raw
-What:		/sys/bus/iio/devices/iio:deviceX/in_temp_y_raw
-What:		/sys/bus/iio/devices/iio:deviceX/in_temp_z_raw
-KernelVersion:	2.6.35
-Contact:	linux-iio@vger.kernel.org
-Description:
-		Raw (unscaled no bias removal etc) temperature measurement.
-		It an axis is specified it generally means that the temperature
-		sensor is associated with one part of a compound device (e.g.
-		a gyroscope axis). Units after application of scale and offset
-		are milli degrees Celsuis.
-
-What:		/sys/bus/iio/devices/iio:deviceX/in_tempX_input
-KernelVersion:	2.6.38
-Contact:	linux-iio@vger.kernel.org
-Description:
-		Scaled temperature measurement in milli degrees Celsius.
-
-What:		/sys/bus/iio/devices/iio:deviceX/in_accel_x_raw
-What:		/sys/bus/iio/devices/iio:deviceX/in_accel_y_raw
-What:		/sys/bus/iio/devices/iio:deviceX/in_accel_z_raw
-KernelVersion:	2.6.35
-Contact:	linux-iio@vger.kernel.org
-Description:
-		Acceleration in direction x, y or z (may be arbitrarily assigned
-		but should match other such assignments on device).
-		Has all of the equivalent parameters as per voltageY. Units
-		after application of scale and offset are m/s^2.
-
-What:		/sys/bus/iio/devices/iio:deviceX/in_anglvel_x_raw
-What:		/sys/bus/iio/devices/iio:deviceX/in_anglvel_y_raw
-What:		/sys/bus/iio/devices/iio:deviceX/in_anglvel_z_raw
-KernelVersion:	2.6.35
-Contact:	linux-iio@vger.kernel.org
-Description:
-		Angular velocity about axis x, y or z (may be arbitrarily
-		assigned) Data converted by application of offset then scale to
-		radians per second. Has all the equivalent parameters as
-		per voltageY. Units after application of scale and offset are
-		radians per second.
-
-What:		/sys/bus/iio/devices/iio:deviceX/in_incli_x_raw
-What:		/sys/bus/iio/devices/iio:deviceX/in_incli_y_raw
-What:		/sys/bus/iio/devices/iio:deviceX/in_incli_z_raw
-KernelVersion:	2.6.35
-Contact:	linux-iio@vger.kernel.org
-Description:
-		Inclination raw reading about axis x, y or z (may be
-		arbitrarily assigned). Data converted by application of offset
-		and scale to Degrees.
-
-What:		/sys/bus/iio/devices/iio:deviceX/in_magn_x_raw
-What:		/sys/bus/iio/devices/iio:deviceX/in_magn_y_raw
-What:		/sys/bus/iio/devices/iio:deviceX/in_magn_z_raw
-KernelVersion:	2.6.35
-Contact:	linux-iio@vger.kernel.org
-Description:
-		Magnetic field along axis x, y or z (may be arbitrarily
-		assigned).  Data converted by application of offset
-		then scale to Gauss.
-
-What:		/sys/bus/iio/devices/iio:deviceX/in_accel_x_peak_raw
-What:		/sys/bus/iio/devices/iio:deviceX/in_accel_y_peak_raw
-What:		/sys/bus/iio/devices/iio:deviceX/in_accel_z_peak_raw
-KernelVersion:	2.6.36
-Contact:	linux-iio@vger.kernel.org
-Description:
-		Highest value since some reset condition.  These
-		attributes allow access to this and are otherwise
-		the direct equivalent of the <type>Y[_name]_raw attributes.
-
-What:		/sys/bus/iio/devices/iio:deviceX/in_accel_xyz_squared_peak_raw
-KernelVersion:	2.6.36
-Contact:	linux-iio@vger.kernel.org
-Description:
-		A computed peak value based on the sum squared magnitude of
-		the underlying value in the specified directions.
-
-What:		/sys/bus/iio/devices/iio:deviceX/in_accel_offset
-What:		/sys/bus/iio/devices/iio:deviceX/in_accel_x_offset
-What:		/sys/bus/iio/devices/iio:deviceX/in_accel_y_offset
-What:		/sys/bus/iio/devices/iio:deviceX/in_accel_z_offset
-What:		/sys/bus/iio/devices/iio:deviceX/in_voltageY_offset
-What:		/sys/bus/iio/devices/iio:deviceX/in_voltage_offset
-What:		/sys/bus/iio/devices/iio:deviceX/in_tempY_offset
-What:		/sys/bus/iio/devices/iio:deviceX/in_temp_offset
-KernelVersion:	2.6.35
-Contact:	linux-iio@vger.kernel.org
-Description:
-		If known for a device, offset to be added to <type>[Y]_raw prior
-		to scaling by <type>[Y]_scale in order to obtain value in the
-		<type> units as specified in <type>[y]_raw documentation.
-		Not present if the offset is always 0 or unknown. If Y or
-		axis <x|y|z> is not present, then the offset applies to all
-		in channels of <type>.
-		May be writable if a variable offset can be applied on the
-		device. Note that this is different to calibbias which
-		is for devices (or drivers) that apply offsets to compensate
-		for variation between different instances of the part, typically
-		adjusted by using some hardware supported calibration procedure.
-		Calibbias is applied internally, offset is applied in userspace
-		to the _raw output.
-
-What:		/sys/bus/iio/devices/iio:deviceX/in_voltageY_scale
-What:		/sys/bus/iio/devices/iio:deviceX/in_voltageY_supply_scale
-What:		/sys/bus/iio/devices/iio:deviceX/in_voltage_scale
-What:		/sys/bus/iio/devices/iio:deviceX/out_voltageY_scale
-What:		/sys/bus/iio/devices/iio:deviceX/in_accel_scale
-What:		/sys/bus/iio/devices/iio:deviceX/in_accel_peak_scale
-What:		/sys/bus/iio/devices/iio:deviceX/in_anglvel_scale
-What:		/sys/bus/iio/devices/iio:deviceX/in_magn_scale
-What:		/sys/bus/iio/devices/iio:deviceX/in_magn_x_scale
-What:		/sys/bus/iio/devices/iio:deviceX/in_magn_y_scale
-What:		/sys/bus/iio/devices/iio:deviceX/in_magn_z_scale
-KernelVersion:	2.6.35
-Contact:	linux-iio@vger.kernel.org
-Description:
-		If known for a device, scale to be applied to <type>Y[_name]_raw
-		post addition of <type>[Y][_name]_offset in order to obtain the
-		measured value in <type> units as specified in
-		<type>[Y][_name]_raw documentation..  If shared across all in
-		channels then Y and <x|y|z> are not present and the value is
-		called <type>[Y][_name]_scale. The peak modifier means this
-		value is applied to <type>Y[_name]_peak_raw values.
-
-What:		/sys/bus/iio/devices/iio:deviceX/in_accel_x_calibbias
-What:		/sys/bus/iio/devices/iio:deviceX/in_accel_y_calibbias
-What:		/sys/bus/iio/devices/iio:deviceX/in_accel_z_calibbias
-What:		/sys/bus/iio/devices/iio:deviceX/in_anglvel_x_calibbias
-What:		/sys/bus/iio/devices/iio:deviceX/in_anglvel_y_calibbias
-What:		/sys/bus/iio/devices/iio:deviceX/in_anglvel_z_calibbias
-KernelVersion:	2.6.35
-Contact:	linux-iio@vger.kernel.org
-Description:
-		Hardware applied calibration offset. (assumed to fix production
-		inaccuracies).
-
-What		/sys/bus/iio/devices/iio:deviceX/in_voltageY_calibscale
-What		/sys/bus/iio/devices/iio:deviceX/in_voltageY_supply_calibscale
-What		/sys/bus/iio/devices/iio:deviceX/in_voltage_calibscale
-What		/sys/bus/iio/devices/iio:deviceX/in_accel_x_calibscale
-What		/sys/bus/iio/devices/iio:deviceX/in_accel_y_calibscale
-What		/sys/bus/iio/devices/iio:deviceX/in_accel_z_calibscale
-What		/sys/bus/iio/devices/iio:deviceX/in_anglvel_x_calibscale
-What		/sys/bus/iio/devices/iio:deviceX/in_anglvel_y_calibscale
-What		/sys/bus/iio/devices/iio:deviceX/in_anglvel_z_calibscale
-KernelVersion:	2.6.35
-Contact:	linux-iio@vger.kernel.org
-Description:
-		Hardware applied calibration scale factor. (assumed to fix
-		production inaccuracies).  If shared across all channels,
-		<type>_calibscale is used.
-
-What:		/sys/bus/iio/devices/iio:deviceX/in_accel_scale_available
-What:		/sys/.../iio:deviceX/in_voltageX_scale_available
-What:		/sys/.../iio:deviceX/in_voltage-voltage_scale_available
-What:		/sys/.../iio:deviceX/out_voltageX_scale_available
-What:		/sys/.../iio:deviceX/in_capacitance_scale_available
-KernelVersion:	2.635
-Contact:	linux-iio@vger.kernel.org
-Description:
-		If a discrete set of scale values are available, they
-		are listed in this attribute.
-
-What:		/sys/.../in_accel_filter_low_pass_3db_frequency
-What:		/sys/.../in_magn_filter_low_pass_3db_frequency
-What:		/sys/.../in_anglvel_filter_low_pass_3db_frequency
-KernelVersion:	3.2
-Contact:	linux-iio@vger.kernel.org
-Description:
-		If a known or controllable low pass filter is applied
-		to the underlying data channel, then this parameter
-		gives the 3dB frequency of the filter in Hz.
-
-What:		/sys/bus/iio/devices/iio:deviceX/out_voltageY_raw
-KernelVersion:	2.6.37
-Contact:	linux-iio@vger.kernel.org
-Description:
-		Raw (unscaled, no bias etc.) output voltage for
-		channel Y.  The number must always be specified and
-		unique if the output corresponds to a single channel.
-
-What:		/sys/bus/iio/devices/iio:deviceX/out_voltageY&Z_raw
-KernelVersion:	2.6.37
-Contact:	linux-iio@vger.kernel.org
-Description:
-		Raw (unscaled, no bias etc.) output voltage for an aggregate of
-		channel Y, channel Z, etc.  This interface is available in cases
-		where a single output sets the value for multiple channels
-		simultaneously.
-
-What:		/sys/bus/iio/devices/iio:deviceX/out_voltageY_powerdown_mode
-What:		/sys/bus/iio/devices/iio:deviceX/out_voltage_powerdown_mode
-KernelVersion:	2.6.38
-Contact:	linux-iio@vger.kernel.org
-Description:
-		Specifies the output powerdown mode.
-		DAC output stage is disconnected from the amplifier and
-		1kohm_to_gnd: connected to ground via an 1kOhm resistor
-		100kohm_to_gnd: connected to ground via an 100kOhm resistor
-		three_state: left floating
-		For a list of available output power down options read
-		outX_powerdown_mode_available. If Y is not present the
-		mode is shared across all outputs.
-
-What:		/sys/.../iio:deviceX/out_votlageY_powerdown_mode_available
-What:		/sys/.../iio:deviceX/out_voltage_powerdown_mode_available
-KernelVersion:	2.6.38
-Contact:	linux-iio@vger.kernel.org
-Description:
-		Lists all available output power down modes.
-		If Y is not present the mode is shared across all outputs.
-
-What:		/sys/bus/iio/devices/iio:deviceX/out_voltageY_powerdown
-What:		/sys/bus/iio/devices/iio:deviceX/out_voltage_powerdown
-KernelVersion:	2.6.38
-Contact:	linux-iio@vger.kernel.org
-Description:
-		Writing 1 causes output Y to enter the power down mode specified
-		by the corresponding outY_powerdown_mode. Clearing returns to
-		normal operation. Y may be suppressed if all outputs are
-		controlled together.
-
-What:		/sys/bus/iio/devices/iio:deviceX/events
-KernelVersion:	2.6.35
-Contact:	linux-iio@vger.kernel.org
-Description:
-		Configuration of which hardware generated events are passed up
-		to user-space.
-
-What:		/sys/.../iio:deviceX/events/in_accel_x_thresh_rising_en
-What:		/sys/.../iio:deviceX/events/in_accel_x_thresh_falling_en
-What:		/sys/.../iio:deviceX/events/in_accel_y_thresh_rising_en
-What:		/sys/.../iio:deviceX/events/in_accel_y_thresh_falling_en
-What:		/sys/.../iio:deviceX/events/in_accel_z_thresh_rising_en
-What:		/sys/.../iio:deviceX/events/in_accel_z_thresh_falling_en
-What:		/sys/.../iio:deviceX/events/in_anglvel_x_thresh_rising_en
-What:		/sys/.../iio:deviceX/events/in_anglvel_x_thresh_falling_en
-What:		/sys/.../iio:deviceX/events/in_anglvel_y_thresh_rising_en
-What:		/sys/.../iio:deviceX/events/in_anglvel_y_thresh_falling_en
-What:		/sys/.../iio:deviceX/events/in_anglvel_z_thresh_rising_en
-What:		/sys/.../iio:deviceX/events/in_anglvel_z_thresh_falling_en
-What:		/sys/.../iio:deviceX/events/in_magn_x_thresh_rising_en
-What:		/sys/.../iio:deviceX/events/in_magn_x_thresh_falling_en
-What:		/sys/.../iio:deviceX/events/in_magn_y_thresh_rising_en
-What:		/sys/.../iio:deviceX/events/in_magn_y_thresh_falling_en
-What:		/sys/.../iio:deviceX/events/in_magn_z_thresh_rising_en
-What:		/sys/.../iio:deviceX/events/in_magn_z_thresh_falling_en
-What:		/sys/.../iio:deviceX/events/in_voltageY_supply_thresh_rising_en
-What:		/sys/.../iio:deviceX/events/in_voltageY_supply_thresh_falling_en
-What:		/sys/.../iio:deviceX/events/in_voltageY_thresh_rising_en
-What:		/sys/.../iio:deviceX/events/in_voltageY_thresh_falling_en
-What:		/sys/.../iio:deviceX/events/in_tempY_thresh_rising_en
-What:		/sys/.../iio:deviceX/events/in_tempY_thresh_falling_en
-KernelVersion:	2.6.37
-Contact:	linux-iio@vger.kernel.org
-Description:
-		Event generated when channel passes a threshold in the specified
-		(_rising|_falling) direction. If the direction is not specified,
-		then either the device will report an event which ever direction
-		a single threshold value is passed in (e.g.
-		<type>[Y][_name]_<raw|input>_thresh_value) or
-		<type>[Y][_name]_<raw|input>_thresh_rising_value and
-		<type>[Y][_name]_<raw|input>_thresh_falling_value may take
-		different values, but the device can only enable both thresholds
-		or neither.
-		Note the driver will assume the last p events requested are
-		to be enabled where p is however many it supports (which may
-		vary depending on the exact set requested. So if you want to be
-		sure you have set what you think you have, check the contents of
-		these attributes after everything is configured. Drivers may
-		have to buffer any parameters so that they are consistent when
-		a given event type is enabled a future point (and not those for
-		whatever event was previously enabled).
-
-What:		/sys/.../iio:deviceX/events/in_accel_x_roc_rising_en
-What:		/sys/.../iio:deviceX/events/in_accel_x_roc_falling_en
-What:		/sys/.../iio:deviceX/events/in_accel_y_roc_rising_en
-What:		/sys/.../iio:deviceX/events/in_accel_y_roc_falling_en
-What:		/sys/.../iio:deviceX/events/in_accel_z_roc_rising_en
-What:		/sys/.../iio:deviceX/events/in_accel_z_roc_falling_en
-What:		/sys/.../iio:deviceX/events/in_anglvel_x_roc_rising_en
-What:		/sys/.../iio:deviceX/events/in_anglvel_x_roc_falling_en
-What:		/sys/.../iio:deviceX/events/in_anglvel_y_roc_rising_en
-What:		/sys/.../iio:deviceX/events/in_anglvel_y_roc_falling_en
-What:		/sys/.../iio:deviceX/events/in_anglvel_z_roc_rising_en
-What:		/sys/.../iio:deviceX/events/in_anglvel_z_roc_falling_en
-What:		/sys/.../iio:deviceX/events/in_magn_x_roc_rising_en
-What:		/sys/.../iio:deviceX/events/in_magn_x_roc_falling_en
-What:		/sys/.../iio:deviceX/events/in_magn_y_roc_rising_en
-What:		/sys/.../iio:deviceX/events/in_magn_y_roc_falling_en
-What:		/sys/.../iio:deviceX/events/in_magn_z_roc_rising_en
-What:		/sys/.../iio:deviceX/events/in_magn_z_roc_falling_en
-What:		/sys/.../iio:deviceX/events/in_voltageY_supply_roc_rising_en
-What:		/sys/.../iio:deviceX/events/in_voltageY_supply_roc_falling_en
-What:		/sys/.../iio:deviceX/events/in_voltageY_roc_rising_en
-What:		/sys/.../iio:deviceX/events/in_voltageY_roc_falling_en
-What:		/sys/.../iio:deviceX/events/in_tempY_roc_rising_en
-What:		/sys/.../iio:deviceX/events/in_tempY_roc_falling_en
-KernelVersion:	2.6.37
-Contact:	linux-iio@vger.kernel.org
-Description:
-		Event generated when channel passes a threshold on the rate of
-		change (1st differential) in the specified (_rising|_falling)
-		direction. If the direction is not specified, then either the
-		device will report an event which ever direction a single
-		threshold value is passed in (e.g.
-		<type>[Y][_name]_<raw|input>_roc_value) or
-		<type>[Y][_name]_<raw|input>_roc_rising_value and
-		<type>[Y][_name]_<raw|input>_roc_falling_value may take
-		different values, but the device can only enable both rate of
-		change thresholds or neither.
-		Note the driver will assume the last p events requested are
-		to be enabled where p is however many it supports (which may
-		vary depending on the exact set requested. So if you want to be
-		sure you have set what you think you have, check the contents of
-		these attributes after everything is configured. Drivers may
-		have to buffer any parameters so that they are consistent when
-		a given event type is enabled a future point (and not those for
-		whatever event was previously enabled).
-
-What:		/sys/.../events/in_accel_x_raw_thresh_rising_value
-What:		/sys/.../events/in_accel_x_raw_thresh_falling_value
-What:		/sys/.../events/in_accel_y_raw_thresh_rising_value
-What:		/sys/.../events/in_accel_y_raw_thresh_falling_value
-What:		/sys/.../events/in_accel_z_raw_thresh_rising_value
-What:		/sys/.../events/in_accel_z_raw_thresh_falling_value
-What:		/sys/.../events/in_anglvel_x_raw_thresh_rising_value
-What:		/sys/.../events/in_anglvel_x_raw_thresh_falling_value
-What:		/sys/.../events/in_anglvel_y_raw_thresh_rising_value
-What:		/sys/.../events/in_anglvel_y_raw_thresh_falling_value
-What:		/sys/.../events/in_anglvel_z_raw_thresh_rising_value
-What:		/sys/.../events/in_anglvel_z_raw_thresh_falling_value
-What:		/sys/.../events/in_magn_x_raw_thresh_rising_value
-What:		/sys/.../events/in_magn_x_raw_thresh_falling_value
-What:		/sys/.../events/in_magn_y_raw_thresh_rising_value
-What:		/sys/.../events/in_magn_y_raw_thresh_falling_value
-What:		/sys/.../events/in_magn_z_raw_thresh_rising_value
-What:		/sys/.../events/in_magn_z_raw_thresh_falling_value
-What:		/sys/.../events/in_voltageY_supply_raw_thresh_rising_value
-What:		/sys/.../events/in_voltageY_supply_raw_thresh_falling_value
-What:		/sys/.../events/in_voltageY_raw_thresh_falling_value
-What:		/sys/.../events/in_voltageY_raw_thresh_falling_value
-What:		/sys/.../events/in_tempY_raw_thresh_falling_value
-What:		/sys/.../events/in_tempY_raw_thresh_falling_value
-KernelVersion:	2.6.37
-Contact:	linux-iio@vger.kernel.org
-Description:
-		Specifies the value of threshold that the device is comparing
-		against for the events enabled by
-		<type>Y[_name]_thresh[_rising|falling]_en.
-		If separate attributes exist for the two directions, but
-		direction is not specified for this attribute, then a single
-		threshold value applies to both directions.
-		The raw or input element of the name indicates whether the
-		value is in raw device units or in processed units (as _raw
-		and _input do on sysfs direct channel read attributes).
-
-What:		/sys/.../events/in_accel_x_raw_roc_rising_value
-What:		/sys/.../events/in_accel_x_raw_roc_falling_value
-What:		/sys/.../events/in_accel_y_raw_roc_rising_value
-What:		/sys/.../events/in_accel_y_raw_roc_falling_value
-What:		/sys/.../events/in_accel_z_raw_roc_rising_value
-What:		/sys/.../events/in_accel_z_raw_roc_falling_value
-What:		/sys/.../events/in_anglvel_x_raw_roc_rising_value
-What:		/sys/.../events/in_anglvel_x_raw_roc_falling_value
-What:		/sys/.../events/in_anglvel_y_raw_roc_rising_value
-What:		/sys/.../events/in_anglvel_y_raw_roc_falling_value
-What:		/sys/.../events/in_anglvel_z_raw_roc_rising_value
-What:		/sys/.../events/in_anglvel_z_raw_roc_falling_value
-What:		/sys/.../events/in_magn_x_raw_roc_rising_value
-What:		/sys/.../events/in_magn_x_raw_roc_falling_value
-What:		/sys/.../events/in_magn_y_raw_roc_rising_value
-What:		/sys/.../events/in_magn_y_raw_roc_falling_value
-What:		/sys/.../events/in_magn_z_raw_roc_rising_value
-What:		/sys/.../events/in_magn_z_raw_roc_falling_value
-What:		/sys/.../events/in_voltageY_supply_raw_roc_rising_value
-What:		/sys/.../events/in_voltageY_supply_raw_roc_falling_value
-What:		/sys/.../events/in_voltageY_raw_roc_falling_value
-What:		/sys/.../events/in_voltageY_raw_roc_falling_value
-What:		/sys/.../events/in_tempY_raw_roc_falling_value
-What:		/sys/.../events/in_tempY_raw_roc_falling_value
-KernelVersion:	2.6.37
-Contact:	linux-iio@vger.kernel.org
-Description:
-		Specifies the value of rate of change threshold that the
-		device is comparing against for the events enabled by
-		<type>[Y][_name]_roc[_rising|falling]_en.
-		If separate attributes exist for the two directions,
-		but direction is not specified for this attribute,
-		then a single threshold value applies to both directions.
-		The raw or input element of the name indicates whether the
-		value is in raw device units or in processed units (as _raw
-		and _input do on sysfs direct channel read attributes).
-
-What:		/sys/.../events/in_accel_x_thresh_rising_period
-What:		/sys/.../events/in_accel_x_thresh_falling_period
-hat:		/sys/.../events/in_accel_x_roc_rising_period
-What:		/sys/.../events/in_accel_x_roc_falling_period
-What:		/sys/.../events/in_accel_y_thresh_rising_period
-What:		/sys/.../events/in_accel_y_thresh_falling_period
-What:		/sys/.../events/in_accel_y_roc_rising_period
-What:		/sys/.../events/in_accel_y_roc_falling_period
-What:		/sys/.../events/in_accel_z_thresh_rising_period
-What:		/sys/.../events/in_accel_z_thresh_falling_period
-What:		/sys/.../events/in_accel_z_roc_rising_period
-What:		/sys/.../events/in_accel_z_roc_falling_period
-What:		/sys/.../events/in_anglvel_x_thresh_rising_period
-What:		/sys/.../events/in_anglvel_x_thresh_falling_period
-What:		/sys/.../events/in_anglvel_x_roc_rising_period
-What:		/sys/.../events/in_anglvel_x_roc_falling_period
-What:		/sys/.../events/in_anglvel_y_thresh_rising_period
-What:		/sys/.../events/in_anglvel_y_thresh_falling_period
-What:		/sys/.../events/in_anglvel_y_roc_rising_period
-What:		/sys/.../events/in_anglvel_y_roc_falling_period
-What:		/sys/.../events/in_anglvel_z_thresh_rising_period
-What:		/sys/.../events/in_anglvel_z_thresh_falling_period
-What:		/sys/.../events/in_anglvel_z_roc_rising_period
-What:		/sys/.../events/in_anglvel_z_roc_falling_period
-What:		/sys/.../events/in_magn_x_thresh_rising_period
-What:		/sys/.../events/in_magn_x_thresh_falling_period
-What:		/sys/.../events/in_magn_x_roc_rising_period
-What:		/sys/.../events/in_magn_x_roc_falling_period
-What:		/sys/.../events/in_magn_y_thresh_rising_period
-What:		/sys/.../events/in_magn_y_thresh_falling_period
-What:		/sys/.../events/in_magn_y_roc_rising_period
-What:		/sys/.../events/in_magn_y_roc_falling_period
-What:		/sys/.../events/in_magn_z_thresh_rising_period
-What:		/sys/.../events/in_magn_z_thresh_falling_period
-What:		/sys/.../events/in_magn_z_roc_rising_period
-What:		/sys/.../events/in_magn_z_roc_falling_period
-What:		/sys/.../events/in_voltageY_supply_thresh_rising_period
-What:		/sys/.../events/in_voltageY_supply_thresh_falling_period
-What:		/sys/.../events/in_voltageY_supply_roc_rising_period
-What:		/sys/.../events/in_voltageY_supply_roc_falling_period
-What:		/sys/.../events/in_voltageY_thresh_rising_period
-What:		/sys/.../events/in_voltageY_thresh_falling_period
-What:		/sys/.../events/in_voltageY_roc_rising_period
-What:		/sys/.../events/in_voltageY_roc_falling_period
-What:		/sys/.../events/in_tempY_thresh_rising_period
-What:		/sys/.../events/in_tempY_thresh_falling_period
-What:		/sys/.../events/in_tempY_roc_rising_period
-What:		/sys/.../events/in_tempY_roc_falling_period
-What:		/sys/.../events/in_accel_x&y&z_mag_falling_period
-KernelVersion:	2.6.37
-Contact:	linux-iio@vger.kernel.org
-Description:
-		Period of time (in seconds) for which the condition must be
-		met before an event is generated. If direction is not
-		specified then this period applies to both directions.
-
-What:		/sys/.../iio:deviceX/events/in_accel_mag_en
-What:		/sys/.../iio:deviceX/events/in_accel_mag_rising_en
-What:		/sys/.../iio:deviceX/events/in_accel_mag_falling_en
-What:		/sys/.../iio:deviceX/events/in_accel_x_mag_en
-What:		/sys/.../iio:deviceX/events/in_accel_x_mag_rising_en
-What:		/sys/.../iio:deviceX/events/in_accel_x_mag_falling_en
-What:		/sys/.../iio:deviceX/events/in_accel_y_mag_en
-What:		/sys/.../iio:deviceX/events/in_accel_y_mag_rising_en
-What:		/sys/.../iio:deviceX/events/in_accel_y_mag_falling_en
-What:		/sys/.../iio:deviceX/events/in_accel_z_mag_en
-What:		/sys/.../iio:deviceX/events/in_accel_z_mag_rising_en
-What:		/sys/.../iio:deviceX/events/in_accel_z_mag_falling_en
-What:		/sys/.../iio:deviceX/events/in_accel_x&y&z_mag_rising_en
-What:		/sys/.../iio:deviceX/events/in_accel_x&y&z_mag_falling_en
-KernelVersion:	2.6.37
-Contact:	linux-iio@vger.kernel.org
-Description:
-		Similar to in_accel_x_thresh[_rising|_falling]_en, but here the
-		magnitude of the channel is compared to the threshold, not its
-		signed value.
-
-What:		/sys/.../events/in_accel_raw_mag_value
-What:		/sys/.../events/in_accel_x_raw_mag_rising_value
-What:		/sys/.../events/in_accel_y_raw_mag_rising_value
-What:		/sys/.../events/in_accel_z_raw_mag_rising_value
-KernelVersion:	2.6.37
-Contact:	linux-iio@vger.kernel.org
-Description:
-		The value to which the magnitude of the channel is compared. If
-		number or direction is not specified, applies to all channels of
-		this type.
-
-What:		/sys/bus/iio/devices/iio:deviceX/trigger/current_trigger
-KernelVersion:	2.6.35
-Contact:	linux-iio@vger.kernel.org
-Description:
-		The name of the trigger source being used, as per string given
-		in /sys/class/iio/triggerY/name.
-
-What:		/sys/bus/iio/devices/iio:deviceX/buffer/length
-KernelVersion:	2.6.35
-Contact:	linux-iio@vger.kernel.org
-Description:
-		Number of scans contained by the buffer.
-
-What:		/sys/bus/iio/devices/iio:deviceX/buffer/bytes_per_datum
-KernelVersion:	2.6.37
-Contact:	linux-iio@vger.kernel.org
-Description:
-		Bytes per scan.  Due to alignment fun, the scan may be larger
-		than implied directly by the scan_element parameters.
-
-What:		/sys/bus/iio/devices/iio:deviceX/buffer/enable
-KernelVersion:	2.6.35
-Contact:	linux-iio@vger.kernel.org
-Description:
-		Actually start the buffer capture up.  Will start trigger
-		if first device and appropriate.
-
-What:		/sys/bus/iio/devices/iio:deviceX/buffer/scan_elements
-KernelVersion:	2.6.37
-Contact:	linux-iio@vger.kernel.org
-Description:
-		Directory containing interfaces for elements that will be
-		captured for a single triggered sample set in the buffer.
-
-What:		/sys/.../buffer/scan_elements/in_accel_x_en
-What:		/sys/.../buffer/scan_elements/in_accel_y_en
-What:		/sys/.../buffer/scan_elements/in_accel_z_en
-What:		/sys/.../buffer/scan_elements/in_anglvel_x_en
-What:		/sys/.../buffer/scan_elements/in_anglvel_y_en
-What:		/sys/.../buffer/scan_elements/in_anglvel_z_en
-What:		/sys/.../buffer/scan_elements/in_magn_x_en
-What:		/sys/.../buffer/scan_elements/in_magn_y_en
-What:		/sys/.../buffer/scan_elements/in_magn_z_en
-What:		/sys/.../buffer/scan_elements/in_timestamp_en
-What:		/sys/.../buffer/scan_elements/in_voltageY_supply_en
-What:		/sys/.../buffer/scan_elements/in_voltageY_en
-What:		/sys/.../buffer/scan_elements/in_voltageY-voltageZ_en
-What:		/sys/.../buffer/scan_elements/in_incli_x_en
-What:		/sys/.../buffer/scan_elements/in_incli_y_en
-KernelVersion:	2.6.37
-Contact:	linux-iio@vger.kernel.org
-Description:
-		Scan element control for triggered data capture.
-
-What:		/sys/.../buffer/scan_elements/in_accel_type
-What:		/sys/.../buffer/scan_elements/in_anglvel_type
-What:		/sys/.../buffer/scan_elements/in_magn_type
-What:		/sys/.../buffer/scan_elements/in_incli_type
-What:		/sys/.../buffer/scan_elements/in_voltageY_type
-What:		/sys/.../buffer/scan_elements/in_voltage-in_type
-What:		/sys/.../buffer/scan_elements/in_voltageY_supply_type
-What:		/sys/.../buffer/scan_elements/in_timestamp_type
-KernelVersion:	2.6.37
-Contact:	linux-iio@vger.kernel.org
-Description:
-		Description of the scan element data storage within the buffer
-		and hence the form in which it is read from user-space.
-		Form is [be|le]:[s|u]bits/storagebits[>>shift].
-		be or le specifies big or little endian. s or u specifies if
-		signed (2's complement) or unsigned. bits is the number of bits
-		of data and storagebits is the space (after padding) that it
-		occupies in the buffer. shift if specified, is the shift that
-		needs to be applied prior to masking out unused bits. Some
-		devices put their data in the middle of the transferred elements
-		with additional information on both sides.  Note that some
-		devices will have additional information in the unused bits
-		so to get a clean value, the bits value must be used to mask
-		the buffer output value appropriately.  The storagebits value
-		also specifies the data alignment.  So s48/64>>2 will be a
-		signed 48 bit integer stored in a 64 bit location aligned to
-		a a64 bit boundary. To obtain the clean value, shift right 2
-		and apply a mask to zero the top 16 bits of the result.
-		For other storage combinations this attribute will be extended
-		appropriately.
-
-What:		/sys/.../buffer/scan_elements/in_accel_type_available
-KernelVersion:	2.6.37
-Contact:	linux-iio@vger.kernel.org
-Description:
-		If the type parameter can take one of a small set of values,
-		this attribute lists them.
-
-What:		/sys/.../buffer/scan_elements/in_voltageY_index
-What:		/sys/.../buffer/scan_elements/in_voltageY_supply_index
-What:		/sys/.../buffer/scan_elements/in_accel_x_index
-What:		/sys/.../buffer/scan_elements/in_accel_y_index
-What:		/sys/.../buffer/scan_elements/in_accel_z_index
-What:		/sys/.../buffer/scan_elements/in_anglvel_x_index
-What:		/sys/.../buffer/scan_elements/in_anglvel_y_index
-What:		/sys/.../buffer/scan_elements/in_anglvel_z_index
-What:		/sys/.../buffer/scan_elements/in_magn_x_index
-What:		/sys/.../buffer/scan_elements/in_magn_y_index
-What:		/sys/.../buffer/scan_elements/in_magn_z_index
-What:		/sys/.../buffer/scan_elements/in_incli_x_index
-What:		/sys/.../buffer/scan_elements/in_incli_y_index
-What:		/sys/.../buffer/scan_elements/in_timestamp_index
-KernelVersion:	2.6.37
-Contact:	linux-iio@vger.kernel.org
-Description:
-		A single positive integer specifying the position of this
-		scan element in the buffer. Note these are not dependent on
-		what is enabled and may not be contiguous. Thus for user-space
-		to establish the full layout these must be used in conjunction
-		with all _en attributes to establish which channels are present,
-		and the relevant _type attributes to establish the data storage
-		format.
-
-What:		/sys/.../iio:deviceX/in_anglvel_z_quadrature_correction_raw
-KernelVersion:	2.6.38
-Contact:	linux-iio@vger.kernel.org
-Description:
-		This attribute is used to read the amount of quadrature error
-		present in the device at a given time.
-
-What:		/sys/.../iio:deviceX/ac_excitation_en
-KernelVersion:	3.1.0
-Contact:	linux-iio@vger.kernel.org
-Description:
-		This attribute, if available, is used to enable the AC
-		excitation mode found on some converters. In ac excitation mode,
-		the polarity of the excitation voltage is reversed on
-		alternate cycles, to eliminate DC errors.
-
-What:		/sys/.../iio:deviceX/bridge_switch_en
-KernelVersion:	3.1.0
-Contact:	linux-iio@vger.kernel.org
-Description:
-		This attribute, if available, is used to close or open the
-		bridge power down switch found on some converters.
-		In bridge applications, such as strain gauges and load cells,
-		the bridge itself consumes the majority of the current in the
-		system. To minimize the current consumption of the system,
-		the bridge can be disconnected (when it is not being used
-		using the bridge_switch_en attribute.
diff --git a/drivers/staging/iio/Documentation/sysfs-bus-iio-ad7192 b/drivers/staging/iio/Documentation/sysfs-bus-iio-ad7192
new file mode 100644
index 00000000000..1c35c507cc0
--- /dev/null
+++ b/drivers/staging/iio/Documentation/sysfs-bus-iio-ad7192
@@ -0,0 +1,20 @@
+What:		/sys/.../iio:deviceX/ac_excitation_en
+KernelVersion:	3.1.0
+Contact:	linux-iio@vger.kernel.org
+Description:
+		This attribute, if available, is used to enable the AC
+		excitation mode found on some converters. In ac excitation mode,
+		the polarity of the excitation voltage is reversed on
+		alternate cycles, to eliminate DC errors.
+
+What:		/sys/.../iio:deviceX/bridge_switch_en
+KernelVersion:	3.1.0
+Contact:	linux-iio@vger.kernel.org
+Description:
+		This attribute, if available, is used to close or open the
+		bridge power down switch found on some converters.
+		In bridge applications, such as strain gauges and load cells,
+		the bridge itself consumes the majority of the current in the
+		system. To minimize the current consumption of the system,
+		the bridge can be disconnected (when it is not being used
+		using the bridge_switch_en attribute.
diff --git a/drivers/staging/iio/Documentation/sysfs-bus-iio-dds b/drivers/staging/iio/Documentation/sysfs-bus-iio-dds
index ffdd5478a35..ee8c509c673 100644
--- a/drivers/staging/iio/Documentation/sysfs-bus-iio-dds
+++ b/drivers/staging/iio/Documentation/sysfs-bus-iio-dds
@@ -1,83 +1,86 @@
 
-What:		/sys/bus/iio/devices/.../ddsX_freqY
+What:		/sys/bus/iio/devices/.../out_altvoltageX_frequencyY
 KernelVersion:	2.6.37
 Contact:	linux-iio@vger.kernel.org
 Description:
 		Stores frequency into tuning word Y.
-		There will be more than one ddsX_freqY file, which allows for
-		pin controlled FSK Frequency Shift Keying
-		(ddsX_pincontrol_freq_en is active) or the user can control
-		the desired active tuning word by writing Y to the
-		ddsX_freqsymbol file.
+		There will be more than one out_altvoltageX_frequencyY file,
+		which allows for pin controlled FSK Frequency Shift Keying
+		(out_altvoltageX_pincontrol_frequency_en is active) or the user
+		can control the desired active tuning word by writing Y to the
+		out_altvoltageX_frequencysymbol file.
 
-What:		/sys/bus/iio/devices/.../ddsX_freqY_scale
+What:		/sys/bus/iio/devices/.../out_altvoltageX_frequencyY_scale
 KernelVersion:	2.6.37
 Contact:	linux-iio@vger.kernel.org
 Description:
-		Scale to be applied to ddsX_freqY in order to obtain the
-		desired value in Hz. If shared across all frequency registers
-		Y is not present. It is also possible X is not present if
-		shared across all channels.
+		Scale to be applied to out_altvoltageX_frequencyY in order to
+		obtain the desired value in Hz. If shared across all frequency
+		registers Y is not present. It is also possible X is not present
+		if shared across all channels.
 
-What:		/sys/bus/iio/devices/.../ddsX_freqsymbol
+What:		/sys/bus/iio/devices/.../out_altvoltageX_frequencysymbol
 KernelVersion:	2.6.37
 Contact:	linux-iio@vger.kernel.org
 Description:
 		Specifies the active output frequency tuning word. The value
-		corresponds to the Y in ddsX_freqY. To exit this mode the user
-		can write ddsX_pincontrol_freq_en or ddsX_out_enable file.
+		corresponds to the Y in out_altvoltageX_frequencyY.
+		To exit this mode the user can write
+		out_altvoltageX_pincontrol_frequency_en or
+		out_altvoltageX_out_enable file.
 
-What:		/sys/bus/iio/devices/.../ddsX_phaseY
+What:		/sys/bus/iio/devices/.../out_altvoltageX_phaseY
 KernelVersion:	2.6.37
 Contact:	linux-iio@vger.kernel.org
 Description:
 		Stores phase into Y.
-		There will be more than one ddsX_phaseY file, which allows for
-		pin controlled PSK Phase Shift Keying
-		(ddsX_pincontrol_phase_en is active) or the user can
+		There will be more than one out_altvoltageX_phaseY file, which
+		allows for pin controlled PSK Phase Shift Keying
+		(out_altvoltageX_pincontrol_phase_en is active) or the user can
 		control the desired phase Y which is added to the phase
-		accumulator output by writing Y to the en_phase file.
+		accumulator output by writing Y to the phase_en file.
 
-What:		/sys/bus/iio/devices/.../ddsX_phaseY_scale
+What:		/sys/bus/iio/devices/.../out_altvoltageX_phaseY_scale
 KernelVersion:	2.6.37
 Contact:	linux-iio@vger.kernel.org
 Description:
-		Scale to be applied to ddsX_phaseY in order to obtain the
-		desired value in rad. If shared across all phase registers
+		Scale to be applied to out_altvoltageX_phaseY in order to obtain
+		the desired value in rad. If shared across all phase registers
 		Y is not present. It is also possible X is not present if
 		shared across all channels.
 
-What:		/sys/bus/iio/devices/.../ddsX_phasesymbol
+What:		/sys/bus/iio/devices/.../out_altvoltageX_phasesymbol
 KernelVersion:	2.6.37
 Contact:	linux-iio@vger.kernel.org
 Description:
 		Specifies the active phase Y which is added to the phase
 		accumulator output. The value corresponds to the Y in
-		ddsX_phaseY. To exit this mode the user can write
-		ddsX_pincontrol_phase_en or disable file.
+		out_altvoltageX_phaseY. To exit this mode the user can write
+		out_altvoltageX_pincontrol_phase_en or disable file.
 
-What:		/sys/bus/iio/devices/.../ddsX_pincontrol_en
-What:		/sys/bus/iio/devices/.../ddsX_pincontrol_freq_en
-What:		/sys/bus/iio/devices/.../ddsX_pincontrol_phase_en
+What:		/sys/bus/iio/devices/.../out_altvoltageX_pincontrol_en
+What:		/sys/bus/iio/devices/.../out_altvoltageX_pincontrol_frequency_en
+What:		/sys/bus/iio/devices/.../out_altvoltageX_pincontrol_phase_en
 KernelVersion:	2.6.37
 Contact:	linux-iio@vger.kernel.org
 Description:
-		ddsX_pincontrol_en: Both, the active frequency and phase is
-		controlled by the respective phase and frequency control inputs.
-		In case the device in question allows to independent controls,
-		then there are dedicated files (ddsX_pincontrol_freq_en,
-		ddsX_pincontrol_phase_en).
+		out_altvoltageX_pincontrol_en: Both, the active frequency and
+		phase is controlled by the respective phase and frequency
+		control inputs. In case the device in features independent
+		controls, then there are dedicated files
+		(out_altvoltageX_pincontrol_frequency_en,
+		out_altvoltageX_pincontrol_phase_en).
 
-What:		/sys/bus/iio/devices/.../ddsX_out_enable
-What:		/sys/bus/iio/devices/.../ddsX_outY_enable
+What:		/sys/bus/iio/devices/.../out_altvoltageX_out_enable
+What:		/sys/bus/iio/devices/.../out_altvoltageX_outY_enable
 KernelVersion:	2.6.37
 Contact:	linux-iio@vger.kernel.org
 Description:
-		ddsX_outY_enable controls signal generation on output Y of
-		channel X. Y may be suppressed if all channels are
+		out_altvoltageX_outY_enable controls signal generation on
+		output Y of channel X. Y may be suppressed if all channels are
 		controlled together.
 
-What:		/sys/bus/iio/devices/.../ddsX_outY_wavetype
+What:		/sys/bus/iio/devices/.../out_altvoltageX_outY_wavetype
 KernelVersion:	2.6.37
 Contact:	linux-iio@vger.kernel.org
 Description:
@@ -86,7 +89,7 @@ Description:
 		For a list of available output waveform options read
 		available_output_modes.
 
-What:		/sys/bus/iio/devices/.../ddsX_outY_wavetype_available
+What:		/sys/bus/iio/devices/.../out_altvoltageX_outY_wavetype_available
 KernelVersion:	2.6.37
 Contact:	linux-iio@vger.kernel.org
 Description:
diff --git a/drivers/staging/iio/Documentation/sysfs-bus-iio-light b/drivers/staging/iio/Documentation/sysfs-bus-iio-light
index edbf470e4e3..715c74dcb53 100644
--- a/drivers/staging/iio/Documentation/sysfs-bus-iio-light
+++ b/drivers/staging/iio/Documentation/sysfs-bus-iio-light
@@ -26,7 +26,7 @@ Description:
 		Hardware dependent list of possible values supported for the
 		adc_resolution of the given sensor.
 
-What:		/sys/bus/iio/devices/device[n]/illuminance0[_input|_raw]
+What:		/sys/bus/iio/devices/device[n]/in_illuminance0[_input|_raw]
 KernelVersion:	2.6.35
 Contact:	linux-iio@vger.kernel.org
 Description:
@@ -45,7 +45,7 @@ Description:
 		do this calculation manually by reading the infrared sensor
 		value and doing the negation in sw.
 
-What:		/sys/bus/iio/devices/device[n]/proximity[_input|_raw]
+What:		/sys/bus/iio/devices/device[n]/in_proximity[_input|_raw]
 KernelVersion:	2.6.37
 Contact:	linux-iio@vger.kernel.org
 Description:
@@ -63,23 +63,22 @@ Description:
 		and if expressed in SI units, should include _input. If this
 		value is not in SI units, then it should include _raw.
 
-What:		/sys/bus/iio/devices/device[n]/illuminance0_target
+What:		/sys/bus/iio/devices/device[n]/in_illuminance0_target
 KernelVersion:	2.6.37
 Contact:	linux-iio@vger.kernel.org
 Description:
 		This property gets/sets the last known external
 		lux measurement used in/for calibration.
 
-What:		/sys/bus/iio/devices/device[n]/illuminance0_integration_time
+What:		/sys/bus/iio/devices/device[n]/in_illuminance0_integration_time
 KernelVersion:	2.6.37
 Contact:	linux-iio@vger.kernel.org
 Description:
 		This property gets/sets the sensors ADC analog integration time.
 
-What:		/sys/bus/iio/devices/device[n]/illuminance0_calibscale
+What:		/sys/bus/iio/devices/device[n]/in_illuminance0_lux_table
 KernelVersion:	2.6.37
 Contact:	linux-iio@vger.kernel.org
 Description:
-		Hardware or software applied calibration scale factor assumed
-		to account for attenuation due to industrial design (glass
-		filters or aperture holes).
+		This property gets/sets the table of coefficients
+		used in calculating illuminance in lux.
diff --git a/drivers/staging/iio/Documentation/trigger.txt b/drivers/staging/iio/Documentation/trigger.txt
index fc2012ebc10..75cc37ff1ed 100644
--- a/drivers/staging/iio/Documentation/trigger.txt
+++ b/drivers/staging/iio/Documentation/trigger.txt
@@ -5,7 +5,7 @@ an IIO device.  Whilst this can create device specific complexities
 such triggers are registered with the core in the same way as
 stand-alone triggers.
 
-struct iio_trig *trig = iio_allocate_trigger("<trigger format string>", ...);
+struct iio_trig *trig = iio_trigger_alloc("<trigger format string>", ...);
 
 allocates a trigger structure.  The key elements to then fill in within
 a driver are:
diff --git a/drivers/staging/iio/Kconfig b/drivers/staging/iio/Kconfig
index fe158671888..3c8e5ec26ac 100644
--- a/drivers/staging/iio/Kconfig
+++ b/drivers/staging/iio/Kconfig
@@ -1,16 +1,9 @@
 #
 # Industrial I/O subsytem configuration
 #
+menu "IIO staging drivers"
+	depends on IIO
 
-menuconfig IIO
-	tristate "Industrial I/O support"
-	depends on GENERIC_HARDIRQS
-	help
-	  The industrial I/O subsystem provides a unified framework for
-	  drivers for many different types of embedded sensors using a
-	  number of different physical interfaces (i2c, spi, etc). See
-	  drivers/staging/iio/Documentation for more information.
-if IIO
 config IIO_ST_HWMON
 	tristate "Hwmon driver that uses channels specified via iio maps"
 	depends on HWMON
@@ -19,12 +12,6 @@ config IIO_ST_HWMON
 	  map allows IIO devices to provide  basic hwmon functionality
 	  for those channels specified in the map.
 
-config IIO_BUFFER
-	bool "Enable buffer support within IIO"
-	help
-	  Provide core support for various buffer based data
-	  acquisition methods.
-
 if IIO_BUFFER
 
 config IIO_SW_RING
@@ -36,39 +23,14 @@ config IIO_SW_RING
 	  with the intention that some devices would be able to write
 	  in interrupt context.
 
-config IIO_KFIFO_BUF
-	select IIO_TRIGGER
-	tristate "Industrial I/O buffering based on kfifo"
-	help
-	  A simple fifo based on kfifo.  Use this if you want a fifo
-	  rather than a ring buffer. Note that this currently provides
-	  no buffer events so it is up to userspace to work out how
-	  often to read from the buffer.
-
 endif # IIO_BUFFER
 
-config IIO_TRIGGER
-	boolean "Enable triggered sampling support"
-	help
-	  Provides IIO core support for triggers.  Currently these
-	  are used to initialize capture of samples to push into
-	  ring buffers.  The triggers are effectively a 'capture
-	  data now' interrupt.
-
-config IIO_CONSUMERS_PER_TRIGGER
-       int "Maximum number of consumers per trigger"
-       depends on IIO_TRIGGER
-       default "2"
-       help
-	This value controls the maximum number of consumers that a
-	given trigger may handle. Default is 2.
-
 source "drivers/staging/iio/accel/Kconfig"
 source "drivers/staging/iio/adc/Kconfig"
 source "drivers/staging/iio/addac/Kconfig"
 source "drivers/staging/iio/cdc/Kconfig"
 source "drivers/staging/iio/dac/Kconfig"
-source "drivers/staging/iio/dds/Kconfig"
+source "drivers/staging/iio/frequency/Kconfig"
 source "drivers/staging/iio/gyro/Kconfig"
 source "drivers/staging/iio/impedance-analyzer/Kconfig"
 source "drivers/staging/iio/imu/Kconfig"
@@ -104,4 +66,4 @@ config IIO_SIMPLE_DUMMY_BUFFER
 
 endif # IIO_SIMPLE_DUMMY
 
-endif # IIO
+endmenu
diff --git a/drivers/staging/iio/Makefile b/drivers/staging/iio/Makefile
index 5075291dda7..6a46d5afb38 100644
--- a/drivers/staging/iio/Makefile
+++ b/drivers/staging/iio/Makefile
@@ -2,13 +2,7 @@
 # Makefile for the industrial I/O core.
 #
 
-obj-$(CONFIG_IIO) += industrialio.o
-industrialio-y := industrialio-core.o industrialio-event.o inkern.o
-industrialio-$(CONFIG_IIO_BUFFER) += industrialio-buffer.o
-industrialio-$(CONFIG_IIO_TRIGGER) += industrialio-trigger.o
-
 obj-$(CONFIG_IIO_SW_RING) += ring_sw.o
-obj-$(CONFIG_IIO_KFIFO_BUF) += kfifo_buf.o
 
 obj-$(CONFIG_IIO_SIMPLE_DUMMY) += iio_dummy.o
 iio_dummy-y := iio_simple_dummy.o
@@ -24,7 +18,7 @@ obj-y += adc/
 obj-y += addac/
 obj-y += cdc/
 obj-y += dac/
-obj-y += dds/
+obj-y += frequency/
 obj-y += gyro/
 obj-y += impedance-analyzer/
 obj-y += imu/
diff --git a/drivers/staging/iio/TODO b/drivers/staging/iio/TODO
index d1ad35e24ab..cf3f9489b9d 100644
--- a/drivers/staging/iio/TODO
+++ b/drivers/staging/iio/TODO
@@ -67,7 +67,7 @@ e-mailing the normal IIO list (see below).
 
 Documentation
 1) Lots of cleanup and expansion.
-2) Some device require indvidual docs.
+2) Some device require individual docs.
 
 Contact: Jonathan Cameron <jic23@cam.ac.uk>.
 Mailing list: linux-iio@vger.kernel.org
diff --git a/drivers/staging/iio/accel/adis16201_core.c b/drivers/staging/iio/accel/adis16201_core.c
index d439e45d07f..02b340919c0 100644
--- a/drivers/staging/iio/accel/adis16201_core.c
+++ b/drivers/staging/iio/accel/adis16201_core.c
@@ -15,9 +15,9 @@
 #include <linux/sysfs.h>
 #include <linux/module.h>
 
-#include "../iio.h"
-#include "../sysfs.h"
-#include "../buffer.h"
+#include <linux/iio/iio.h>
+#include <linux/iio/sysfs.h>
+#include <linux/iio/buffer.h>
 
 #include "adis16201.h"
 
@@ -171,7 +171,7 @@ static ssize_t adis16201_write_reset(struct device *dev,
 	ret = strtobool(buf, &res);
 	if (ret || !res)
 		return ret;
-	return adis16201_reset(dev_get_drvdata(dev));
+	return adis16201_reset(dev_to_iio_dev(dev));
 }
 
 int adis16201_set_irq(struct iio_dev *indio_dev, bool enable)
@@ -298,7 +298,7 @@ static int adis16201_read_raw(struct iio_dev *indio_dev,
 	s16 val16;
 
 	switch (mask) {
-	case 0:
+	case IIO_CHAN_INFO_RAW:
 		mutex_lock(&indio_dev->mlock);
 		addr = adis16201_addresses[chan->address][0];
 		ret = adis16201_spi_read_reg_16(indio_dev, addr, &val16);
@@ -406,39 +406,104 @@ static int adis16201_write_raw(struct iio_dev *indio_dev,
 }
 
 static struct iio_chan_spec adis16201_channels[] = {
-	IIO_CHAN(IIO_VOLTAGE, 0, 1, 0, "supply", 0, 0,
+	{
+		.type = IIO_VOLTAGE,
+		.indexed = 1,
+		.channel = 0,
+		.extend_name = "supply",
+		.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT |
 		 IIO_CHAN_INFO_SCALE_SEPARATE_BIT,
-		 in_supply, ADIS16201_SCAN_SUPPLY,
-		 IIO_ST('u', 12, 16, 0), 0),
-	IIO_CHAN(IIO_TEMP, 0, 1, 0, NULL, 0, 0,
+		.address = in_supply,
+		.scan_index = ADIS16201_SCAN_SUPPLY,
+		.scan_type = {
+			.sign = 'u',
+			.realbits = 12,
+			.storagebits = 16,
+		},
+	}, {
+		.type = IIO_TEMP,
+		.indexed = 1,
+		.channel = 0,
+		.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT |
 		 IIO_CHAN_INFO_SCALE_SEPARATE_BIT |
 		 IIO_CHAN_INFO_OFFSET_SEPARATE_BIT,
-		 temp, ADIS16201_SCAN_TEMP,
-		 IIO_ST('u', 12, 16, 0), 0),
-	IIO_CHAN(IIO_ACCEL, 1, 0, 0, NULL, 0, IIO_MOD_X,
+		.address = temp,
+		.scan_index = ADIS16201_SCAN_TEMP,
+		.scan_type = {
+			.sign = 'u',
+			.realbits = 12,
+			.storagebits = 16,
+		},
+	}, {
+		.type = IIO_ACCEL,
+		.modified = 1,
+		.channel2 = IIO_MOD_X,
+		.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT |
 		 IIO_CHAN_INFO_SCALE_SHARED_BIT |
 		 IIO_CHAN_INFO_CALIBBIAS_SEPARATE_BIT,
-		 accel_x, ADIS16201_SCAN_ACC_X,
-		 IIO_ST('s', 14, 16, 0), 0),
-	IIO_CHAN(IIO_ACCEL, 1, 0, 0, NULL, 0, IIO_MOD_Y,
+		.address = accel_x,
+		.scan_index = ADIS16201_SCAN_ACC_X,
+		.scan_type = {
+			.sign = 's',
+			.realbits = 14,
+			.storagebits = 16,
+		},
+	}, {
+		.type = IIO_ACCEL,
+		.modified = 1,
+		.channel2 = IIO_MOD_Y,
+		.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT |
 		 IIO_CHAN_INFO_SCALE_SHARED_BIT |
 		 IIO_CHAN_INFO_CALIBBIAS_SEPARATE_BIT,
-		 accel_y, ADIS16201_SCAN_ACC_Y,
-		 IIO_ST('s', 14, 16, 0), 0),
-	IIO_CHAN(IIO_VOLTAGE, 0, 1, 0, NULL, 1, 0,
+		.address = accel_y,
+		.scan_index = ADIS16201_SCAN_ACC_Y,
+		.scan_type = {
+			.sign = 's',
+			.realbits = 14,
+			.storagebits = 16,
+		},
+	}, {
+		.type = IIO_VOLTAGE,
+		.indexed = 1,
+		.channel = 1,
+		.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT |
 		 IIO_CHAN_INFO_SCALE_SEPARATE_BIT,
-		 in_aux, ADIS16201_SCAN_AUX_ADC,
-		 IIO_ST('u', 12, 16, 0), 0),
-	IIO_CHAN(IIO_INCLI, 1, 0, 0, NULL, 0, IIO_MOD_X,
+		.address = in_aux,
+		.scan_index = ADIS16201_SCAN_AUX_ADC,
+		.scan_type = {
+			.sign = 'u',
+			.realbits = 12,
+			.storagebits = 16,
+		},
+	}, {
+		.type = IIO_INCLI,
+		.modified = 1,
+		.channel2 = IIO_MOD_X,
+		.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT |
 		 IIO_CHAN_INFO_SCALE_SHARED_BIT |
 		 IIO_CHAN_INFO_CALIBBIAS_SEPARATE_BIT,
-		 incli_x, ADIS16201_SCAN_INCLI_X,
-		 IIO_ST('s', 14, 16, 0), 0),
-	IIO_CHAN(IIO_INCLI, 1, 0, 0, NULL, 0, IIO_MOD_Y,
+		.address = incli_x,
+		.scan_index = ADIS16201_SCAN_INCLI_X,
+		.scan_type = {
+			.sign = 's',
+			.realbits = 14,
+			.storagebits = 16,
+		},
+	}, {
+		.type = IIO_INCLI,
+		.modified = 1,
+		.channel2 = IIO_MOD_Y,
+		.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT |
 		 IIO_CHAN_INFO_SCALE_SHARED_BIT |
 		 IIO_CHAN_INFO_CALIBBIAS_SEPARATE_BIT,
-		 incli_y, ADIS16201_SCAN_INCLI_Y,
-		 IIO_ST('s', 14, 16, 0), 0),
+		.address = incli_y,
+		.scan_index = ADIS16201_SCAN_INCLI_Y,
+		.scan_type = {
+			.sign = 's',
+			.realbits = 14,
+			.storagebits = 16,
+		},
+	},
 	IIO_CHAN_SOFT_TIMESTAMP(7)
 };
 
@@ -467,7 +532,7 @@ static int __devinit adis16201_probe(struct spi_device *spi)
 	struct iio_dev *indio_dev;
 
 	/* setup the industrialio driver allocated elements */
-	indio_dev = iio_allocate_device(sizeof(*st));
+	indio_dev = iio_device_alloc(sizeof(*st));
 	if (indio_dev == NULL) {
 		ret = -ENOMEM;
 		goto error_ret;
@@ -522,7 +587,7 @@ error_uninitialize_ring:
 error_unreg_ring_funcs:
 	adis16201_unconfigure_ring(indio_dev);
 error_free_dev:
-	iio_free_device(indio_dev);
+	iio_device_free(indio_dev);
 error_ret:
 	return ret;
 }
@@ -535,7 +600,7 @@ static int adis16201_remove(struct spi_device *spi)
 	adis16201_remove_trigger(indio_dev);
 	iio_buffer_unregister(indio_dev);
 	adis16201_unconfigure_ring(indio_dev);
-	iio_free_device(indio_dev);
+	iio_device_free(indio_dev);
 
 	return 0;
 }
diff --git a/drivers/staging/iio/accel/adis16201_ring.c b/drivers/staging/iio/accel/adis16201_ring.c
index 97f9e6b159d..247602a8e54 100644
--- a/drivers/staging/iio/accel/adis16201_ring.c
+++ b/drivers/staging/iio/accel/adis16201_ring.c
@@ -5,9 +5,9 @@
 #include <linux/spi/spi.h>
 #include <linux/slab.h>
 
-#include "../iio.h"
+#include <linux/iio/iio.h>
 #include "../ring_sw.h"
-#include "../trigger_consumer.h"
+#include <linux/iio/trigger_consumer.h>
 #include "adis16201.h"
 
 
@@ -66,9 +66,8 @@ static irqreturn_t adis16201_trigger_handler(int irq, void *p)
 
 	int i = 0;
 	s16 *data;
-	size_t datasize = ring->access->get_bytes_per_datum(ring);
 
-	data = kmalloc(datasize, GFP_KERNEL);
+	data = kmalloc(indio_dev->scan_bytes, GFP_KERNEL);
 	if (data == NULL) {
 		dev_err(&st->us->dev, "memory alloc failed in ring bh");
 		return -ENOMEM;
@@ -81,7 +80,7 @@ static irqreturn_t adis16201_trigger_handler(int irq, void *p)
 			data[i] = be16_to_cpup((__be16 *)&(st->rx[i*2]));
 
 	/* Guaranteed to be aligned with 8 byte boundary */
-	if (ring->scan_timestamp)
+	if (indio_dev->scan_timestamp)
 		*((s64 *)(data + ((i + 3)/4)*4)) = pf->timestamp;
 
 	ring->access->store_to(ring, (u8 *)data, pf->timestamp);
diff --git a/drivers/staging/iio/accel/adis16201_trigger.c b/drivers/staging/iio/accel/adis16201_trigger.c
index bce505e716d..96fdabbac20 100644
--- a/drivers/staging/iio/accel/adis16201_trigger.c
+++ b/drivers/staging/iio/accel/adis16201_trigger.c
@@ -3,8 +3,8 @@
 #include <linux/spi/spi.h>
 #include <linux/export.h>
 
-#include "../iio.h"
-#include "../trigger.h"
+#include <linux/iio/iio.h>
+#include <linux/iio/trigger.h>
 #include "adis16201.h"
 
 /**
@@ -29,7 +29,7 @@ int adis16201_probe_trigger(struct iio_dev *indio_dev)
 	int ret;
 	struct adis16201_state *st = iio_priv(indio_dev);
 
-	st->trig = iio_allocate_trigger("adis16201-dev%d", indio_dev->id);
+	st->trig = iio_trigger_alloc("adis16201-dev%d", indio_dev->id);
 	if (st->trig == NULL) {
 		ret = -ENOMEM;
 		goto error_ret;
@@ -56,7 +56,7 @@ int adis16201_probe_trigger(struct iio_dev *indio_dev)
 error_free_irq:
 	free_irq(st->us->irq, st->trig);
 error_free_trig:
-	iio_free_trigger(st->trig);
+	iio_trigger_free(st->trig);
 error_ret:
 	return ret;
 }
@@ -67,5 +67,5 @@ void adis16201_remove_trigger(struct iio_dev *indio_dev)
 
 	iio_trigger_unregister(state->trig);
 	free_irq(state->us->irq, state->trig);
-	iio_free_trigger(state->trig);
+	iio_trigger_free(state->trig);
 }
diff --git a/drivers/staging/iio/accel/adis16203_core.c b/drivers/staging/iio/accel/adis16203_core.c
index 1a5140f9e3f..15d46bfd1b4 100644
--- a/drivers/staging/iio/accel/adis16203_core.c
+++ b/drivers/staging/iio/accel/adis16203_core.c
@@ -15,9 +15,9 @@
 #include <linux/sysfs.h>
 #include <linux/module.h>
 
-#include "../iio.h"
-#include "../sysfs.h"
-#include "../buffer.h"
+#include <linux/iio/iio.h>
+#include <linux/iio/sysfs.h>
+#include <linux/iio/buffer.h>
 
 #include "adis16203.h"
 
@@ -182,7 +182,7 @@ static ssize_t adis16203_write_reset(struct device *dev,
 		struct device_attribute *attr,
 		const char *buf, size_t len)
 {
-	struct iio_dev *indio_dev = dev_get_drvdata(dev);
+	struct iio_dev *indio_dev = dev_to_iio_dev(dev);
 	if (len < 1)
 		return -EINVAL;
 	switch (buf[0]) {
@@ -305,7 +305,7 @@ static int adis16203_read_raw(struct iio_dev *indio_dev,
 	u8 addr;
 	s16 val16;
 	switch (mask) {
-	case 0:
+	case IIO_CHAN_INFO_RAW:
 		mutex_lock(&indio_dev->mlock);
 		addr = adis16203_addresses[chan->address][0];
 		ret = adis16203_spi_read_reg_16(indio_dev, addr, &val16);
@@ -372,29 +372,75 @@ static int adis16203_read_raw(struct iio_dev *indio_dev,
 }
 
 static struct iio_chan_spec adis16203_channels[] = {
-	IIO_CHAN(IIO_VOLTAGE, 0, 1, 0, "supply", 0, 0,
-		 IIO_CHAN_INFO_SCALE_SEPARATE_BIT,
-		 in_supply, ADIS16203_SCAN_SUPPLY,
-		 IIO_ST('u', 12, 16, 0), 0),
-	IIO_CHAN(IIO_VOLTAGE, 0, 1, 0, NULL, 1, 0,
-		 IIO_CHAN_INFO_SCALE_SEPARATE_BIT,
-		 in_aux, ADIS16203_SCAN_AUX_ADC,
-		 IIO_ST('u', 12, 16, 0), 0),
-	IIO_CHAN(IIO_INCLI, 1, 0, 0, NULL, 0, IIO_MOD_X,
-		 IIO_CHAN_INFO_SCALE_SHARED_BIT |
-		 IIO_CHAN_INFO_CALIBBIAS_SEPARATE_BIT,
-		 incli_x, ADIS16203_SCAN_INCLI_X,
-		 IIO_ST('s', 14, 16, 0), 0),
-	/* Fixme: Not what it appears to be - see data sheet */
-	IIO_CHAN(IIO_INCLI, 1, 0, 0, NULL, 0, IIO_MOD_Y,
-		 IIO_CHAN_INFO_SCALE_SHARED_BIT,
-		 incli_y, ADIS16203_SCAN_INCLI_Y,
-		 IIO_ST('s', 14, 16, 0), 0),
-	IIO_CHAN(IIO_TEMP, 0, 1, 0, NULL, 0, 0,
-		 IIO_CHAN_INFO_SCALE_SEPARATE_BIT |
-		 IIO_CHAN_INFO_OFFSET_SEPARATE_BIT,
-		 temp, ADIS16203_SCAN_TEMP,
-		 IIO_ST('u', 12, 16, 0), 0),
+	{
+		.type = IIO_VOLTAGE,
+		.indexed = 1,
+		.channel = 0,
+		.extend_name = "supply",
+		.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT |
+		IIO_CHAN_INFO_SCALE_SEPARATE_BIT,
+		.address = in_supply,
+		.scan_index = ADIS16203_SCAN_SUPPLY,
+		.scan_type = {
+			.sign = 'u',
+			.realbits = 12,
+			.storagebits = 16,
+		},
+	}, {
+		.type = IIO_VOLTAGE,
+		.indexed = 1,
+		.channel = 1,
+		.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT |
+		IIO_CHAN_INFO_SCALE_SEPARATE_BIT,
+		.address = in_aux,
+		.scan_index = ADIS16203_SCAN_AUX_ADC,
+		.scan_type = {
+			.sign = 'u',
+			.realbits = 12,
+			.storagebits = 16,
+		},
+	}, {
+		.type = IIO_INCLI,
+		.modified = 1,
+		.channel2 = IIO_MOD_X,
+		.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT |
+		IIO_CHAN_INFO_SCALE_SHARED_BIT |
+		IIO_CHAN_INFO_CALIBBIAS_SEPARATE_BIT,
+		.address = incli_x,
+		.scan_index = ADIS16203_SCAN_INCLI_X,
+		.scan_type = {
+			.sign = 's',
+			.realbits = 14,
+			.storagebits = 16,
+		},
+	}, { /* Fixme: Not what it appears to be - see data sheet */
+		.type = IIO_INCLI,
+		.modified = 1,
+		.channel2 = IIO_MOD_Y,
+		.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT |
+		IIO_CHAN_INFO_SCALE_SHARED_BIT,
+		.address = incli_y,
+		.scan_index = ADIS16203_SCAN_INCLI_Y,
+		.scan_type = {
+			.sign = 's',
+			.realbits = 14,
+			.storagebits = 16,
+		},
+	}, {
+		.type = IIO_TEMP,
+		.indexed = 1,
+		.channel = 0,
+		.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT |
+		IIO_CHAN_INFO_SCALE_SEPARATE_BIT |
+		IIO_CHAN_INFO_OFFSET_SEPARATE_BIT,
+		.address = temp,
+		.scan_index = ADIS16203_SCAN_TEMP,
+		.scan_type = {
+			.sign = 'u',
+			.realbits = 12,
+			.storagebits = 16,
+		},
+	},
 	IIO_CHAN_SOFT_TIMESTAMP(5),
 };
 
@@ -423,7 +469,7 @@ static int __devinit adis16203_probe(struct spi_device *spi)
 	struct adis16203_state *st;
 
 	/* setup the industrialio driver allocated elements */
-	indio_dev = iio_allocate_device(sizeof(*st));
+	indio_dev = iio_device_alloc(sizeof(*st));
 	if (indio_dev == NULL) {
 		ret = -ENOMEM;
 		goto error_ret;
@@ -477,7 +523,7 @@ error_uninitialize_ring:
 error_unreg_ring_funcs:
 	adis16203_unconfigure_ring(indio_dev);
 error_free_dev:
-	iio_free_device(indio_dev);
+	iio_device_free(indio_dev);
 error_ret:
 	return ret;
 }
@@ -490,7 +536,7 @@ static int adis16203_remove(struct spi_device *spi)
 	adis16203_remove_trigger(indio_dev);
 	iio_buffer_unregister(indio_dev);
 	adis16203_unconfigure_ring(indio_dev);
-	iio_free_device(indio_dev);
+	iio_device_free(indio_dev);
 
 	return 0;
 }
diff --git a/drivers/staging/iio/accel/adis16203_ring.c b/drivers/staging/iio/accel/adis16203_ring.c
index 6a8963db4f6..7bbd2c2bbd1 100644
--- a/drivers/staging/iio/accel/adis16203_ring.c
+++ b/drivers/staging/iio/accel/adis16203_ring.c
@@ -5,20 +5,19 @@
 #include <linux/spi/spi.h>
 #include <linux/slab.h>
 
-#include "../iio.h"
+#include <linux/iio/iio.h>
 #include "../ring_sw.h"
-#include "../trigger_consumer.h"
+#include <linux/iio/trigger_consumer.h>
 #include "adis16203.h"
 
 /**
  * adis16203_read_ring_data() read data registers which will be placed into ring
- * @dev: device associated with child of actual device (iio_dev or iio_trig)
+ * @indio_dev: the IIO device
  * @rx: somewhere to pass back the value read
  **/
-static int adis16203_read_ring_data(struct device *dev, u8 *rx)
+static int adis16203_read_ring_data(struct iio_dev *indio_dev, u8 *rx)
 {
 	struct spi_message msg;
-	struct iio_dev *indio_dev = dev_get_drvdata(dev);
 	struct adis16203_state *st = iio_priv(indio_dev);
 	struct spi_transfer xfers[ADIS16203_OUTPUTS + 1];
 	int ret;
@@ -66,22 +65,21 @@ static irqreturn_t adis16203_trigger_handler(int irq, void *p)
 
 	int i = 0;
 	s16 *data;
-	size_t datasize = ring->access->get_bytes_per_datum(ring);
 
-	data = kmalloc(datasize, GFP_KERNEL);
+	data = kmalloc(indio_dev->scan_bytes, GFP_KERNEL);
 	if (data == NULL) {
 		dev_err(&st->us->dev, "memory alloc failed in ring bh");
 		return -ENOMEM;
 	}
 
 	if (!bitmap_empty(indio_dev->active_scan_mask, indio_dev->masklength) &&
-	    adis16203_read_ring_data(&indio_dev->dev, st->rx) >= 0)
+	    adis16203_read_ring_data(indio_dev, st->rx) >= 0)
 		for (; i < bitmap_weight(indio_dev->active_scan_mask,
 					 indio_dev->masklength); i++)
 			data[i] = be16_to_cpup((__be16 *)&(st->rx[i*2]));
 
 	/* Guaranteed to be aligned with 8 byte boundary */
-	if (ring->scan_timestamp)
+	if (indio_dev->scan_timestamp)
 		*((s64 *)(data + ((i + 3)/4)*4)) = pf->timestamp;
 
 	ring->access->store_to(ring,
diff --git a/drivers/staging/iio/accel/adis16203_trigger.c b/drivers/staging/iio/accel/adis16203_trigger.c
index 24bcb8e15c5..b8a04073d6d 100644
--- a/drivers/staging/iio/accel/adis16203_trigger.c
+++ b/drivers/staging/iio/accel/adis16203_trigger.c
@@ -3,8 +3,8 @@
 #include <linux/spi/spi.h>
 #include <linux/export.h>
 
-#include "../iio.h"
-#include "../trigger.h"
+#include <linux/iio/iio.h>
+#include <linux/iio/trigger.h>
 #include "adis16203.h"
 
 /**
@@ -29,7 +29,7 @@ int adis16203_probe_trigger(struct iio_dev *indio_dev)
 	int ret;
 	struct adis16203_state *st = iio_priv(indio_dev);
 
-	st->trig = iio_allocate_trigger("adis16203-dev%d", indio_dev->id);
+	st->trig = iio_trigger_alloc("adis16203-dev%d", indio_dev->id);
 	if (st->trig == NULL) {
 		ret = -ENOMEM;
 		goto error_ret;
@@ -58,7 +58,7 @@ int adis16203_probe_trigger(struct iio_dev *indio_dev)
 error_free_irq:
 	free_irq(st->us->irq, st->trig);
 error_free_trig:
-	iio_free_trigger(st->trig);
+	iio_trigger_free(st->trig);
 error_ret:
 	return ret;
 }
@@ -69,5 +69,5 @@ void adis16203_remove_trigger(struct iio_dev *indio_dev)
 
 	iio_trigger_unregister(st->trig);
 	free_irq(st->us->irq, st->trig);
-	iio_free_trigger(st->trig);
+	iio_trigger_free(st->trig);
 }
diff --git a/drivers/staging/iio/accel/adis16204_core.c b/drivers/staging/iio/accel/adis16204_core.c
index fa89364b841..ac9d95e4ea4 100644
--- a/drivers/staging/iio/accel/adis16204_core.c
+++ b/drivers/staging/iio/accel/adis16204_core.c
@@ -18,9 +18,9 @@
 #include <linux/list.h>
 #include <linux/module.h>
 
-#include "../iio.h"
-#include "../sysfs.h"
-#include "../buffer.h"
+#include <linux/iio/iio.h>
+#include <linux/iio/sysfs.h>
+#include <linux/iio/buffer.h>
 
 #include "adis16204.h"
 
@@ -173,7 +173,7 @@ static ssize_t adis16204_read_14bit_signed(struct device *dev,
 		struct device_attribute *attr,
 		char *buf)
 {
-	struct iio_dev *indio_dev = dev_get_drvdata(dev);
+	struct iio_dev *indio_dev = dev_to_iio_dev(dev);
 	struct iio_dev_attr *this_attr = to_iio_dev_attr(attr);
 	s16 val = 0;
 	ssize_t ret;
@@ -211,7 +211,7 @@ static ssize_t adis16204_write_reset(struct device *dev,
 		struct device_attribute *attr,
 		const char *buf, size_t len)
 {
-	struct iio_dev *indio_dev = dev_get_drvdata(dev);
+	struct iio_dev *indio_dev = dev_to_iio_dev(dev);
 
 	if (len < 1)
 		return -EINVAL;
@@ -342,7 +342,7 @@ static int adis16204_read_raw(struct iio_dev *indio_dev,
 	int addrind;
 
 	switch (mask) {
-	case 0:
+	case IIO_CHAN_INFO_RAW:
 		mutex_lock(&indio_dev->mlock);
 		addr = adis16204_addresses[chan->address][0];
 		ret = adis16204_spi_read_reg_16(indio_dev, addr, &val16);
@@ -444,31 +444,78 @@ static int adis16204_write_raw(struct iio_dev *indio_dev,
 }
 
 static struct iio_chan_spec adis16204_channels[] = {
-	IIO_CHAN(IIO_VOLTAGE, 0, 0, 0, "supply", 0, 0,
-		 IIO_CHAN_INFO_SCALE_SEPARATE_BIT,
-		 in_supply, ADIS16204_SCAN_SUPPLY,
-		 IIO_ST('u', 12, 16, 0), 0),
-	IIO_CHAN(IIO_VOLTAGE, 0, 1, 0, NULL, 1, 0,
-		 IIO_CHAN_INFO_SCALE_SEPARATE_BIT,
-		 in_aux, ADIS16204_SCAN_AUX_ADC,
-		 IIO_ST('u', 12, 16, 0), 0),
-	IIO_CHAN(IIO_TEMP, 0, 1, 0, NULL, 0, 0,
-		 IIO_CHAN_INFO_SCALE_SEPARATE_BIT |
-		 IIO_CHAN_INFO_OFFSET_SEPARATE_BIT,
-		 temp, ADIS16204_SCAN_TEMP,
-		 IIO_ST('u', 12, 16, 0), 0),
-	IIO_CHAN(IIO_ACCEL, 1, 0, 0, NULL, 0, IIO_MOD_X,
-		 IIO_CHAN_INFO_SCALE_SEPARATE_BIT |
-		 IIO_CHAN_INFO_CALIBBIAS_SEPARATE_BIT |
-		 IIO_CHAN_INFO_PEAK_SEPARATE_BIT,
-		 accel_x, ADIS16204_SCAN_ACC_X,
-		 IIO_ST('s', 14, 16, 0), 0),
-	IIO_CHAN(IIO_ACCEL, 1, 0, 0, NULL, 0, IIO_MOD_Y,
-		 IIO_CHAN_INFO_SCALE_SEPARATE_BIT |
-		 IIO_CHAN_INFO_CALIBBIAS_SEPARATE_BIT |
-		 IIO_CHAN_INFO_PEAK_SEPARATE_BIT,
-		 accel_y, ADIS16204_SCAN_ACC_Y,
-		 IIO_ST('s', 14, 16, 0), 0),
+	{
+		.type = IIO_VOLTAGE,
+		.indexed = 1, /* Note was not previously indexed */
+		.channel = 0,
+		.extend_name = "supply",
+		.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT |
+		IIO_CHAN_INFO_SCALE_SEPARATE_BIT,
+		.address = in_supply,
+		.scan_index = ADIS16204_SCAN_SUPPLY,
+		.scan_type = {
+			.sign = 'u',
+			.realbits = 12,
+			.storagebits = 16,
+		},
+	}, {
+		.type = IIO_VOLTAGE,
+		.indexed = 1,
+		.channel = 1,
+		.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT |
+		IIO_CHAN_INFO_SCALE_SEPARATE_BIT,
+		.address = in_aux,
+		.scan_index = ADIS16204_SCAN_AUX_ADC,
+		.scan_type = {
+			.sign = 'u',
+			.realbits = 12,
+			.storagebits = 16,
+		},
+	}, {
+		.type = IIO_TEMP,
+		.indexed = 1,
+		.channel = 0,
+		.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT |
+		IIO_CHAN_INFO_SCALE_SEPARATE_BIT |
+		IIO_CHAN_INFO_OFFSET_SEPARATE_BIT,
+		.address = temp,
+		.scan_index = ADIS16204_SCAN_TEMP,
+		.scan_type = {
+			.sign = 'u',
+			.realbits = 12,
+			.storagebits = 16,
+		},
+	}, {
+		.type = IIO_ACCEL,
+		.modified = 1,
+		.channel2 = IIO_MOD_X,
+		.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT |
+		IIO_CHAN_INFO_SCALE_SEPARATE_BIT |
+		IIO_CHAN_INFO_CALIBBIAS_SEPARATE_BIT |
+		IIO_CHAN_INFO_PEAK_SEPARATE_BIT,
+		.address = accel_x,
+		.scan_index = ADIS16204_SCAN_ACC_X,
+		.scan_type = {
+			.sign = 's',
+			.realbits = 14,
+			.storagebits = 16,
+		},
+	}, {
+		.type = IIO_ACCEL,
+		.modified = 1,
+		.channel2 = IIO_MOD_Y,
+		.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT |
+		IIO_CHAN_INFO_SCALE_SEPARATE_BIT |
+		IIO_CHAN_INFO_CALIBBIAS_SEPARATE_BIT |
+		IIO_CHAN_INFO_PEAK_SEPARATE_BIT,
+		.address = accel_y,
+		.scan_index = ADIS16204_SCAN_ACC_Y,
+		.scan_type = {
+			.sign = 's',
+			.realbits = 14,
+			.storagebits = 16,
+		},
+	},
 	IIO_CHAN_SOFT_TIMESTAMP(5),
 };
 
@@ -498,7 +545,7 @@ static int __devinit adis16204_probe(struct spi_device *spi)
 	struct iio_dev *indio_dev;
 
 	/* setup the industrialio driver allocated elements */
-	indio_dev = iio_allocate_device(sizeof(*st));
+	indio_dev = iio_device_alloc(sizeof(*st));
 	if (indio_dev == NULL) {
 		ret = -ENOMEM;
 		goto error_ret;
@@ -551,7 +598,7 @@ error_uninitialize_ring:
 error_unreg_ring_funcs:
 	adis16204_unconfigure_ring(indio_dev);
 error_free_dev:
-	iio_free_device(indio_dev);
+	iio_device_free(indio_dev);
 error_ret:
 	return ret;
 }
@@ -564,7 +611,7 @@ static int adis16204_remove(struct spi_device *spi)
 	adis16204_remove_trigger(indio_dev);
 	iio_buffer_unregister(indio_dev);
 	adis16204_unconfigure_ring(indio_dev);
-	iio_free_device(indio_dev);
+	iio_device_free(indio_dev);
 
 	return 0;
 }
diff --git a/drivers/staging/iio/accel/adis16204_ring.c b/drivers/staging/iio/accel/adis16204_ring.c
index 5c8ab733886..f73518bc658 100644
--- a/drivers/staging/iio/accel/adis16204_ring.c
+++ b/drivers/staging/iio/accel/adis16204_ring.c
@@ -5,20 +5,19 @@
 #include <linux/spi/spi.h>
 #include <linux/slab.h>
 
-#include "../iio.h"
+#include <linux/iio/iio.h>
 #include "../ring_sw.h"
-#include "../trigger_consumer.h"
+#include <linux/iio/trigger_consumer.h>
 #include "adis16204.h"
 
 /**
  * adis16204_read_ring_data() read data registers which will be placed into ring
- * @dev: device associated with child of actual device (iio_dev or iio_trig)
+ * @indio_dev: the IIO device
  * @rx: somewhere to pass back the value read
  **/
-static int adis16204_read_ring_data(struct device *dev, u8 *rx)
+static int adis16204_read_ring_data(struct iio_dev *indio_dev, u8 *rx)
 {
 	struct spi_message msg;
-	struct iio_dev *indio_dev = dev_get_drvdata(dev);
 	struct adis16204_state *st = iio_priv(indio_dev);
 	struct spi_transfer xfers[ADIS16204_OUTPUTS + 1];
 	int ret;
@@ -63,22 +62,21 @@ static irqreturn_t adis16204_trigger_handler(int irq, void *p)
 	struct iio_buffer *ring = indio_dev->buffer;
 	int i = 0;
 	s16 *data;
-	size_t datasize = ring->access->get_bytes_per_datum(ring);
 
-	data = kmalloc(datasize, GFP_KERNEL);
+	data = kmalloc(indio_dev->scan_bytes, GFP_KERNEL);
 	if (data == NULL) {
 		dev_err(&st->us->dev, "memory alloc failed in ring bh");
 		return -ENOMEM;
 	}
 
 	if (!bitmap_empty(indio_dev->active_scan_mask, indio_dev->masklength) &&
-	    adis16204_read_ring_data(&indio_dev->dev, st->rx) >= 0)
+	    adis16204_read_ring_data(indio_dev, st->rx) >= 0)
 		for (; i < bitmap_weight(indio_dev->active_scan_mask,
 					 indio_dev->masklength); i++)
 			data[i] = be16_to_cpup((__be16 *)&(st->rx[i*2]));
 
 	/* Guaranteed to be aligned with 8 byte boundary */
-	if (ring->scan_timestamp)
+	if (indio_dev->scan_timestamp)
 		*((s64 *)(data + ((i + 3)/4)*4)) = pf->timestamp;
 
 	ring->access->store_to(ring, (u8 *)data, pf->timestamp);
diff --git a/drivers/staging/iio/accel/adis16204_trigger.c b/drivers/staging/iio/accel/adis16204_trigger.c
index 6e542af02c0..408a1682368 100644
--- a/drivers/staging/iio/accel/adis16204_trigger.c
+++ b/drivers/staging/iio/accel/adis16204_trigger.c
@@ -3,8 +3,8 @@
 #include <linux/spi/spi.h>
 #include <linux/export.h>
 
-#include "../iio.h"
-#include "../trigger.h"
+#include <linux/iio/iio.h>
+#include <linux/iio/trigger.h>
 #include "adis16204.h"
 
 /**
@@ -29,7 +29,7 @@ int adis16204_probe_trigger(struct iio_dev *indio_dev)
 	int ret;
 	struct adis16204_state *st = iio_priv(indio_dev);
 
-	st->trig = iio_allocate_trigger("adis16204-dev%d", indio_dev->id);
+	st->trig = iio_trigger_alloc("adis16204-dev%d", indio_dev->id);
 	if (st->trig == NULL) {
 		ret = -ENOMEM;
 		goto error_ret;
@@ -58,7 +58,7 @@ int adis16204_probe_trigger(struct iio_dev *indio_dev)
 error_free_irq:
 	free_irq(st->us->irq, st->trig);
 error_free_trig:
-	iio_free_trigger(st->trig);
+	iio_trigger_free(st->trig);
 error_ret:
 	return ret;
 }
@@ -69,5 +69,5 @@ void adis16204_remove_trigger(struct iio_dev *indio_dev)
 
 	iio_trigger_unregister(state->trig);
 	free_irq(state->us->irq, state->trig);
-	iio_free_trigger(state->trig);
+	iio_trigger_free(state->trig);
 }
diff --git a/drivers/staging/iio/accel/adis16209_core.c b/drivers/staging/iio/accel/adis16209_core.c
index a98715f6bd6..f6fd0d31d4f 100644
--- a/drivers/staging/iio/accel/adis16209_core.c
+++ b/drivers/staging/iio/accel/adis16209_core.c
@@ -16,9 +16,9 @@
 #include <linux/list.h>
 #include <linux/module.h>
 
-#include "../iio.h"
-#include "../sysfs.h"
-#include "../buffer.h"
+#include <linux/iio/iio.h>
+#include <linux/iio/sysfs.h>
+#include <linux/iio/buffer.h>
 
 #include "adis16209.h"
 
@@ -157,7 +157,7 @@ static ssize_t adis16209_write_reset(struct device *dev,
 		struct device_attribute *attr,
 		const char *buf, size_t len)
 {
-	struct iio_dev *indio_dev = dev_get_drvdata(dev);
+	struct iio_dev *indio_dev = dev_to_iio_dev(dev);
 
 	if (len < 1)
 		return -EINVAL;
@@ -331,7 +331,7 @@ static int adis16209_read_raw(struct iio_dev *indio_dev,
 	s16 val16;
 
 	switch (mask) {
-	case 0:
+	case IIO_CHAN_INFO_RAW:
 		mutex_lock(&indio_dev->mlock);
 		addr = adis16209_addresses[chan->address][0];
 		ret = adis16209_spi_read_reg_16(indio_dev, addr, &val16);
@@ -408,41 +408,114 @@ static int adis16209_read_raw(struct iio_dev *indio_dev,
 }
 
 static struct iio_chan_spec adis16209_channels[] = {
-	IIO_CHAN(IIO_VOLTAGE, 0, 1, 0, NULL, 0, 0,
-		 IIO_CHAN_INFO_SCALE_SEPARATE_BIT,
-		 in_supply, ADIS16209_SCAN_SUPPLY,
-		 IIO_ST('u', 14, 16, 0), 0),
-	IIO_CHAN(IIO_TEMP, 0, 1, 0, NULL, 0, 0,
-		 IIO_CHAN_INFO_SCALE_SEPARATE_BIT |
-		 IIO_CHAN_INFO_OFFSET_SEPARATE_BIT,
-		 temp, ADIS16209_SCAN_TEMP,
-		 IIO_ST('u', 12, 16, 0), 0),
-	IIO_CHAN(IIO_ACCEL, 1, 0, 0, NULL, 0, IIO_MOD_X,
-		 IIO_CHAN_INFO_SCALE_SHARED_BIT |
-		 IIO_CHAN_INFO_CALIBBIAS_SEPARATE_BIT,
-		 accel_x, ADIS16209_SCAN_ACC_X,
-		 IIO_ST('s', 14, 16, 0), 0),
-	IIO_CHAN(IIO_ACCEL, 1, 0, 0, NULL, 0, IIO_MOD_Y,
-		 IIO_CHAN_INFO_SCALE_SHARED_BIT |
-		 IIO_CHAN_INFO_CALIBBIAS_SEPARATE_BIT,
-		 accel_y, ADIS16209_SCAN_ACC_Y,
-		 IIO_ST('s', 14, 16, 0), 0),
-	IIO_CHAN(IIO_VOLTAGE, 0, 1, 0, NULL, 1, 0,
-		 IIO_CHAN_INFO_SCALE_SEPARATE_BIT,
-		 in_aux, ADIS16209_SCAN_AUX_ADC,
-		 IIO_ST('u', 12, 16, 0), 0),
-	IIO_CHAN(IIO_INCLI, 1, 0, 0, NULL, 0, IIO_MOD_X,
-		 IIO_CHAN_INFO_SCALE_SHARED_BIT,
-		 incli_x, ADIS16209_SCAN_INCLI_X,
-		 IIO_ST('s', 14, 16, 0), 0),
-	IIO_CHAN(IIO_INCLI, 1, 0, 0, NULL, 0, IIO_MOD_Y,
-		 IIO_CHAN_INFO_SCALE_SHARED_BIT,
-		 incli_y, ADIS16209_SCAN_INCLI_Y,
-		 IIO_ST('s', 14, 16, 0), 0),
-	IIO_CHAN(IIO_ROT, 0, 1, 0, NULL, 0, IIO_MOD_X,
-		    0,
-		    rot, ADIS16209_SCAN_ROT,
-		    IIO_ST('s', 14, 16, 0), 0),
+	{
+		.type = IIO_VOLTAGE,
+		.indexed = 1,
+		.channel = 0,
+		.extend_name = "supply",
+		.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT |
+		IIO_CHAN_INFO_SCALE_SEPARATE_BIT,
+		.address = in_supply,
+		.scan_index = ADIS16209_SCAN_SUPPLY,
+		.scan_type = {
+			.sign = 'u',
+			.realbits = 14,
+			.storagebits = 16,
+		},
+	}, {
+		.type = IIO_TEMP,
+		.indexed = 0,
+		.channel = 0,
+		.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT |
+		IIO_CHAN_INFO_SCALE_SEPARATE_BIT |
+		IIO_CHAN_INFO_OFFSET_SEPARATE_BIT,
+		.address = temp,
+		.scan_index = ADIS16209_SCAN_TEMP,
+		.scan_type = {
+			.sign = 'u',
+			.realbits = 12,
+			.storagebits = 16,
+		},
+	}, {
+		.type = IIO_ACCEL,
+		.modified = 1,
+		.channel2 = IIO_MOD_X,
+		.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT |
+		IIO_CHAN_INFO_SCALE_SHARED_BIT |
+		IIO_CHAN_INFO_CALIBBIAS_SEPARATE_BIT,
+		.address = accel_x,
+		.scan_index = ADIS16209_SCAN_ACC_X,
+		.scan_type = {
+			.sign = 's',
+			.realbits = 14,
+			.storagebits = 16,
+		},
+	}, {
+		.type = IIO_ACCEL,
+		.modified = 1,
+		.channel2 = IIO_MOD_Y,
+		.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT |
+		IIO_CHAN_INFO_SCALE_SHARED_BIT |
+		IIO_CHAN_INFO_CALIBBIAS_SEPARATE_BIT,
+		.address = accel_y,
+		.scan_index = ADIS16209_SCAN_ACC_Y,
+		.scan_type = {
+			.sign = 's',
+			.realbits = 14,
+			.storagebits = 16,
+		},
+	}, {
+		.type = IIO_VOLTAGE,
+		.indexed = 1,
+		.channel = 1,
+		.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT |
+		IIO_CHAN_INFO_SCALE_SEPARATE_BIT,
+		.address = in_aux,
+		.scan_index = ADIS16209_SCAN_AUX_ADC,
+		.scan_type = {
+			.sign = 'u',
+			.realbits = 12,
+			.storagebits = 16,
+		},
+	}, {
+		.type = IIO_INCLI,
+		.modified = 1,
+		.channel2 = IIO_MOD_X,
+		.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT |
+		IIO_CHAN_INFO_SCALE_SHARED_BIT,
+		.address = incli_x,
+		.scan_index = ADIS16209_SCAN_INCLI_X,
+		.scan_type = {
+			.sign = 's',
+			.realbits = 14,
+			.storagebits = 16,
+		},
+	}, {
+		.type = IIO_INCLI,
+		.modified = 1,
+		.channel2 = IIO_MOD_Y,
+		.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT |
+		IIO_CHAN_INFO_SCALE_SHARED_BIT,
+		.address = incli_y,
+		.scan_index = ADIS16209_SCAN_INCLI_Y,
+		.scan_type = {
+			.sign = 's',
+			.realbits = 14,
+			.storagebits = 16,
+		},
+	}, {
+		.type = IIO_ROT,
+		.modified = 1,
+		.channel2 = IIO_MOD_X,
+		.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT,
+		.address = rot,
+		.scan_index = ADIS16209_SCAN_ROT,
+		.scan_type = {
+			.sign = 's',
+			.realbits = 14,
+			.storagebits = 16,
+		},
+	},
 	IIO_CHAN_SOFT_TIMESTAMP(8)
 };
 
@@ -471,7 +544,7 @@ static int __devinit adis16209_probe(struct spi_device *spi)
 	struct iio_dev *indio_dev;
 
 	/* setup the industrialio driver allocated elements */
-	indio_dev = iio_allocate_device(sizeof(*st));
+	indio_dev = iio_device_alloc(sizeof(*st));
 	if (indio_dev == NULL) {
 		ret = -ENOMEM;
 		goto error_ret;
@@ -524,7 +597,7 @@ error_uninitialize_ring:
 error_unreg_ring_funcs:
 	adis16209_unconfigure_ring(indio_dev);
 error_free_dev:
-	iio_free_device(indio_dev);
+	iio_device_free(indio_dev);
 error_ret:
 	return ret;
 }
@@ -539,7 +612,7 @@ static int adis16209_remove(struct spi_device *spi)
 	adis16209_remove_trigger(indio_dev);
 	iio_buffer_unregister(indio_dev);
 	adis16209_unconfigure_ring(indio_dev);
-	iio_free_device(indio_dev);
+	iio_device_free(indio_dev);
 
 	return 0;
 }
diff --git a/drivers/staging/iio/accel/adis16209_ring.c b/drivers/staging/iio/accel/adis16209_ring.c
index 57254b6b38b..090607504c9 100644
--- a/drivers/staging/iio/accel/adis16209_ring.c
+++ b/drivers/staging/iio/accel/adis16209_ring.c
@@ -5,20 +5,19 @@
 #include <linux/spi/spi.h>
 #include <linux/slab.h>
 
-#include "../iio.h"
+#include <linux/iio/iio.h>
 #include "../ring_sw.h"
-#include "../trigger_consumer.h"
+#include <linux/iio/trigger_consumer.h>
 #include "adis16209.h"
 
 /**
  * adis16209_read_ring_data() read data registers which will be placed into ring
- * @dev: device associated with child of actual device (iio_dev or iio_trig)
+ * @indio_dev: the IIO device
  * @rx: somewhere to pass back the value read
  **/
-static int adis16209_read_ring_data(struct device *dev, u8 *rx)
+static int adis16209_read_ring_data(struct iio_dev *indio_dev, u8 *rx)
 {
 	struct spi_message msg;
-	struct iio_dev *indio_dev = dev_get_drvdata(dev);
 	struct adis16209_state *st = iio_priv(indio_dev);
 	struct spi_transfer xfers[ADIS16209_OUTPUTS + 1];
 	int ret;
@@ -61,25 +60,23 @@ static irqreturn_t adis16209_trigger_handler(int irq, void *p)
 	struct iio_dev *indio_dev = pf->indio_dev;
 	struct adis16209_state *st = iio_priv(indio_dev);
 	struct iio_buffer *ring = indio_dev->buffer;
-
 	int i = 0;
 	s16 *data;
-	size_t datasize = ring->access->get_bytes_per_datum(ring);
 
-	data = kmalloc(datasize , GFP_KERNEL);
+	data = kmalloc(indio_dev->scan_bytes, GFP_KERNEL);
 	if (data == NULL) {
 		dev_err(&st->us->dev, "memory alloc failed in ring bh");
 		return -ENOMEM;
 	}
 
 	if (!bitmap_empty(indio_dev->active_scan_mask, indio_dev->masklength) &&
-	    adis16209_read_ring_data(&indio_dev->dev, st->rx) >= 0)
+	    adis16209_read_ring_data(indio_dev, st->rx) >= 0)
 		for (; i < bitmap_weight(indio_dev->active_scan_mask,
 					 indio_dev->masklength); i++)
 			data[i] = be16_to_cpup((__be16 *)&(st->rx[i*2]));
 
 	/* Guaranteed to be aligned with 8 byte boundary */
-	if (ring->scan_timestamp)
+	if (indio_dev->scan_timestamp)
 		*((s64 *)(data + ((i + 3)/4)*4)) = pf->timestamp;
 
 	ring->access->store_to(ring, (u8 *)data, pf->timestamp);
diff --git a/drivers/staging/iio/accel/adis16209_trigger.c b/drivers/staging/iio/accel/adis16209_trigger.c
index c5d82c1a55d..2ad93dcaf40 100644
--- a/drivers/staging/iio/accel/adis16209_trigger.c
+++ b/drivers/staging/iio/accel/adis16209_trigger.c
@@ -3,8 +3,8 @@
 #include <linux/spi/spi.h>
 #include <linux/export.h>
 
-#include "../iio.h"
-#include "../trigger.h"
+#include <linux/iio/iio.h>
+#include <linux/iio/trigger.h>
 #include "adis16209.h"
 
 /**
@@ -38,7 +38,7 @@ int adis16209_probe_trigger(struct iio_dev *indio_dev)
 	int ret;
 	struct adis16209_state *st = iio_priv(indio_dev);
 
-	st->trig = iio_allocate_trigger("adis16209-dev%d", indio_dev->id);
+	st->trig = iio_trigger_alloc("adis16209-dev%d", indio_dev->id);
 	if (st->trig == NULL) {
 		ret = -ENOMEM;
 		goto error_ret;
@@ -66,7 +66,7 @@ int adis16209_probe_trigger(struct iio_dev *indio_dev)
 error_free_irq:
 	free_irq(st->us->irq, st->trig);
 error_free_trig:
-	iio_free_trigger(st->trig);
+	iio_trigger_free(st->trig);
 error_ret:
 	return ret;
 }
@@ -77,5 +77,5 @@ void adis16209_remove_trigger(struct iio_dev *indio_dev)
 
 	iio_trigger_unregister(st->trig);
 	free_irq(st->us->irq, st->trig);
-	iio_free_trigger(st->trig);
+	iio_trigger_free(st->trig);
 }
diff --git a/drivers/staging/iio/accel/adis16220_core.c b/drivers/staging/iio/accel/adis16220_core.c
index 51a852d4548..6a9ac898cb0 100644
--- a/drivers/staging/iio/accel/adis16220_core.c
+++ b/drivers/staging/iio/accel/adis16220_core.c
@@ -15,8 +15,8 @@
 #include <linux/sysfs.h>
 #include <linux/module.h>
 
-#include "../iio.h"
-#include "../sysfs.h"
+#include <linux/iio/iio.h>
+#include <linux/iio/sysfs.h>
 
 #include "adis16220.h"
 
@@ -145,7 +145,7 @@ static ssize_t adis16220_read_16bit(struct device *dev,
 		char *buf)
 {
 	struct iio_dev_attr *this_attr = to_iio_dev_attr(attr);
-	struct iio_dev *indio_dev = dev_get_drvdata(dev);
+	struct iio_dev *indio_dev = dev_to_iio_dev(dev);
 	ssize_t ret;
 	s16 val = 0;
 
@@ -164,7 +164,7 @@ static ssize_t adis16220_write_16bit(struct device *dev,
 		const char *buf,
 		size_t len)
 {
-	struct iio_dev *indio_dev = dev_get_drvdata(dev);
+	struct iio_dev *indio_dev = dev_to_iio_dev(dev);
 	struct iio_dev_attr *this_attr = to_iio_dev_attr(attr);
 	int ret;
 	u16 val;
@@ -208,7 +208,7 @@ static ssize_t adis16220_write_reset(struct device *dev,
 		struct device_attribute *attr,
 		const char *buf, size_t len)
 {
-	struct iio_dev *indio_dev = dev_get_drvdata(dev);
+	struct iio_dev *indio_dev = dev_to_iio_dev(dev);
 	bool val;
 	int ret;
 
@@ -228,7 +228,7 @@ static ssize_t adis16220_write_capture(struct device *dev,
 		struct device_attribute *attr,
 		const char *buf, size_t len)
 {
-	struct iio_dev *indio_dev = dev_get_drvdata(dev);
+	struct iio_dev *indio_dev = dev_to_iio_dev(dev);
 	bool val;
 	int ret;
 
@@ -393,7 +393,7 @@ static ssize_t adis16220_accel_bin_read(struct file *filp, struct kobject *kobj,
 					size_t count)
 {
 	struct device *dev = container_of(kobj, struct device, kobj);
-	struct iio_dev *indio_dev = dev_get_drvdata(dev);
+	struct iio_dev *indio_dev = dev_to_iio_dev(dev);
 
 	return adis16220_capture_buffer_read(indio_dev, buf,
 					off, count,
@@ -415,7 +415,7 @@ static ssize_t adis16220_adc1_bin_read(struct file *filp, struct kobject *kobj,
 				size_t count)
 {
 	struct device *dev = container_of(kobj, struct device, kobj);
-	struct iio_dev *indio_dev = dev_get_drvdata(dev);
+	struct iio_dev *indio_dev = dev_to_iio_dev(dev);
 
 	return adis16220_capture_buffer_read(indio_dev, buf,
 					off, count,
@@ -437,7 +437,7 @@ static ssize_t adis16220_adc2_bin_read(struct file *filp, struct kobject *kobj,
 				size_t count)
 {
 	struct device *dev = container_of(kobj, struct device, kobj);
-	struct iio_dev *indio_dev = dev_get_drvdata(dev);
+	struct iio_dev *indio_dev = dev_to_iio_dev(dev);
 
 	return adis16220_capture_buffer_read(indio_dev, buf,
 					off, count,
@@ -507,7 +507,7 @@ static int adis16220_read_raw(struct iio_dev *indio_dev,
 	u8 bits;
 
 	switch (mask) {
-	case 0:
+	case IIO_CHAN_INFO_RAW:
 		addrind = 0;
 		break;
 	case IIO_CHAN_INFO_OFFSET:
@@ -575,11 +575,13 @@ static const struct iio_chan_spec adis16220_channels[] = {
 		.indexed = 1,
 		.channel = 0,
 		.extend_name = "supply",
-		.info_mask = IIO_CHAN_INFO_SCALE_SEPARATE_BIT,
+		.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT |
+			     IIO_CHAN_INFO_SCALE_SEPARATE_BIT,
 		.address = in_supply,
 	}, {
 		.type = IIO_ACCEL,
-		.info_mask = IIO_CHAN_INFO_OFFSET_SEPARATE_BIT |
+		.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT |
+			     IIO_CHAN_INFO_OFFSET_SEPARATE_BIT |
 			     IIO_CHAN_INFO_SCALE_SEPARATE_BIT |
 			     IIO_CHAN_INFO_PEAK_SEPARATE_BIT,
 		.address = accel,
@@ -587,20 +589,23 @@ static const struct iio_chan_spec adis16220_channels[] = {
 		.type = IIO_TEMP,
 		.indexed = 1,
 		.channel = 0,
-		.info_mask = IIO_CHAN_INFO_OFFSET_SEPARATE_BIT |
+		.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT |
+			     IIO_CHAN_INFO_OFFSET_SEPARATE_BIT |
 			     IIO_CHAN_INFO_SCALE_SEPARATE_BIT,
 		.address = temp,
 	}, {
 		.type = IIO_VOLTAGE,
 		.indexed = 1,
 		.channel = 1,
-		.info_mask = IIO_CHAN_INFO_OFFSET_SEPARATE_BIT |
+		.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT |
+			     IIO_CHAN_INFO_OFFSET_SEPARATE_BIT |
 			     IIO_CHAN_INFO_SCALE_SEPARATE_BIT,
 		.address = in_1,
 	}, {
 		.type = IIO_VOLTAGE,
 		.indexed = 1,
 		.channel = 2,
+		.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT,
 		.address = in_2,
 	}
 };
@@ -629,7 +634,7 @@ static int __devinit adis16220_probe(struct spi_device *spi)
 	struct iio_dev *indio_dev;
 
 	/* setup the industrialio driver allocated elements */
-	indio_dev = iio_allocate_device(sizeof(*st));
+	indio_dev = iio_device_alloc(sizeof(*st));
 	if (indio_dev == NULL) {
 		ret = -ENOMEM;
 		goto error_ret;
@@ -680,7 +685,7 @@ error_rm_accel_bin:
 error_unregister_dev:
 	iio_device_unregister(indio_dev);
 error_free_dev:
-	iio_free_device(indio_dev);
+	iio_device_free(indio_dev);
 error_ret:
 	return ret;
 }
@@ -695,7 +700,7 @@ static int adis16220_remove(struct spi_device *spi)
 	sysfs_remove_bin_file(&indio_dev->dev.kobj, &adc1_bin);
 	sysfs_remove_bin_file(&indio_dev->dev.kobj, &accel_bin);
 	iio_device_unregister(indio_dev);
-	iio_free_device(indio_dev);
+	iio_device_free(indio_dev);
 
 	return 0;
 }
diff --git a/drivers/staging/iio/accel/adis16240_core.c b/drivers/staging/iio/accel/adis16240_core.c
index 17f77fef7f2..8b15eaea338 100644
--- a/drivers/staging/iio/accel/adis16240_core.c
+++ b/drivers/staging/iio/accel/adis16240_core.c
@@ -19,9 +19,9 @@
 #include <linux/list.h>
 #include <linux/module.h>
 
-#include "../iio.h"
-#include "../sysfs.h"
-#include "../buffer.h"
+#include <linux/iio/iio.h>
+#include <linux/iio/sysfs.h>
+#include <linux/iio/buffer.h>
 
 #include "adis16240.h"
 
@@ -154,7 +154,7 @@ static ssize_t adis16240_spi_read_signed(struct device *dev,
 		char *buf,
 		unsigned bits)
 {
-	struct iio_dev *indio_dev = dev_get_drvdata(dev);
+	struct iio_dev *indio_dev = dev_to_iio_dev(dev);
 	int ret;
 	s16 val = 0;
 	unsigned shift = 16 - bits;
@@ -177,7 +177,7 @@ static ssize_t adis16240_read_12bit_signed(struct device *dev,
 		char *buf)
 {
 	ssize_t ret;
-	struct iio_dev *indio_dev = dev_get_drvdata(dev);
+	struct iio_dev *indio_dev = dev_to_iio_dev(dev);
 
 	/* Take the iio_dev status lock */
 	mutex_lock(&indio_dev->mlock);
@@ -203,7 +203,7 @@ static ssize_t adis16240_write_reset(struct device *dev,
 		struct device_attribute *attr,
 		const char *buf, size_t len)
 {
-	struct iio_dev *indio_dev = dev_get_drvdata(dev);
+	struct iio_dev *indio_dev = dev_to_iio_dev(dev);
 
 	if (len < 1)
 		return -EINVAL;
@@ -365,7 +365,7 @@ static int adis16240_read_raw(struct iio_dev *indio_dev,
 	s16 val16;
 
 	switch (mask) {
-	case 0:
+	case IIO_CHAN_INFO_RAW:
 		mutex_lock(&indio_dev->mlock);
 		addr = adis16240_addresses[chan->address][0];
 		ret = adis16240_spi_read_reg_16(indio_dev, addr, &val16);
@@ -468,33 +468,88 @@ static int adis16240_write_raw(struct iio_dev *indio_dev,
 }
 
 static struct iio_chan_spec adis16240_channels[] = {
-	IIO_CHAN(IIO_VOLTAGE, 0, 1, 0, "supply", 0, 0,
-		 IIO_CHAN_INFO_SCALE_SEPARATE_BIT,
-		 in_supply, ADIS16240_SCAN_SUPPLY,
-		 IIO_ST('u', 10, 16, 0), 0),
-	IIO_CHAN(IIO_VOLTAGE, 0, 1, 0, NULL, 1, 0,
-		 0,
-		 in_aux, ADIS16240_SCAN_AUX_ADC,
-		 IIO_ST('u', 10, 16, 0), 0),
-	IIO_CHAN(IIO_ACCEL, 1, 0, 0, NULL, 0, IIO_MOD_X,
-		 IIO_CHAN_INFO_SCALE_SHARED_BIT |
-		 IIO_CHAN_INFO_CALIBBIAS_SEPARATE_BIT,
-		 accel_x, ADIS16240_SCAN_ACC_X,
-		 IIO_ST('s', 10, 16, 0), 0),
-	IIO_CHAN(IIO_ACCEL, 1, 0, 0, NULL, 0, IIO_MOD_Y,
-		 IIO_CHAN_INFO_SCALE_SHARED_BIT |
-		 IIO_CHAN_INFO_CALIBBIAS_SEPARATE_BIT,
-		 accel_y, ADIS16240_SCAN_ACC_Y,
-		 IIO_ST('s', 10, 16, 0), 0),
-	IIO_CHAN(IIO_ACCEL, 1, 0, 0, NULL, 0, IIO_MOD_Z,
-		 IIO_CHAN_INFO_SCALE_SHARED_BIT |
-		 IIO_CHAN_INFO_CALIBBIAS_SEPARATE_BIT,
-		 accel_z, ADIS16240_SCAN_ACC_Z,
-		 IIO_ST('s', 10, 16, 0), 0),
-	IIO_CHAN(IIO_TEMP, 0, 1, 0, NULL, 0, 0,
-		 IIO_CHAN_INFO_SCALE_SEPARATE_BIT,
-		 temp, ADIS16240_SCAN_TEMP,
-		 IIO_ST('u', 10, 16, 0), 0),
+	{
+		.type = IIO_VOLTAGE,
+		.indexed = 1,
+		.channel = 0,
+		.extend_name = "supply",
+		.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT |
+		IIO_CHAN_INFO_SCALE_SEPARATE_BIT,
+		.address = in_supply,
+		.scan_index = ADIS16240_SCAN_SUPPLY,
+		.scan_type = {
+			.sign = 'u',
+			.realbits = 10,
+			.storagebits = 16,
+		},
+	}, {
+		.type = IIO_VOLTAGE,
+		.indexed = 1,
+		.channel = 1,
+		.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT,
+		.address = in_aux,
+		.scan_index = ADIS16240_SCAN_AUX_ADC,
+		.scan_type = {
+			.sign = 'u',
+			.realbits = 10,
+			.storagebits = 16,
+		},
+	}, {
+		.type = IIO_ACCEL,
+		.modified = 1,
+		.channel2 = IIO_MOD_X,
+		.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT |
+		IIO_CHAN_INFO_SCALE_SHARED_BIT |
+		IIO_CHAN_INFO_CALIBBIAS_SEPARATE_BIT,
+		.address = accel_x,
+		.scan_index = ADIS16240_SCAN_ACC_X,
+		.scan_type = {
+			.sign = 's',
+			.realbits = 10,
+			.storagebits = 16,
+		},
+	}, {
+		.type = IIO_ACCEL,
+		.modified = 1,
+		.channel2 = IIO_MOD_Y,
+		.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT |
+		IIO_CHAN_INFO_SCALE_SHARED_BIT |
+		IIO_CHAN_INFO_CALIBBIAS_SEPARATE_BIT,
+		.address = accel_y,
+		.scan_index = ADIS16240_SCAN_ACC_Y,
+		.scan_type = {
+			.sign = 's',
+			.realbits = 10,
+			.storagebits = 16,
+		},
+	}, {
+		.type = IIO_ACCEL,
+		.modified = 1,
+		.channel2 = IIO_MOD_Z,
+		.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT |
+		IIO_CHAN_INFO_SCALE_SHARED_BIT |
+		IIO_CHAN_INFO_CALIBBIAS_SEPARATE_BIT,
+		.address = accel_z,
+		.scan_index = ADIS16240_SCAN_ACC_Z,
+		.scan_type = {
+			.sign = 's',
+			.realbits = 10,
+			.storagebits = 16,
+		},
+	}, {
+		.type = IIO_TEMP,
+		.indexed = 1,
+		.channel = 0,
+		.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT |
+		IIO_CHAN_INFO_SCALE_SEPARATE_BIT,
+		.address = temp,
+		.scan_index = ADIS16240_SCAN_TEMP,
+		.scan_type = {
+			.sign = 'u',
+			.realbits = 10,
+			.storagebits = 16,
+		},
+	},
 	IIO_CHAN_SOFT_TIMESTAMP(6)
 };
 
@@ -523,7 +578,7 @@ static int __devinit adis16240_probe(struct spi_device *spi)
 	struct iio_dev *indio_dev;
 
 	/* setup the industrialio driver allocated elements */
-	indio_dev = iio_allocate_device(sizeof(*st));
+	indio_dev = iio_device_alloc(sizeof(*st));
 	if (indio_dev == NULL) {
 		ret = -ENOMEM;
 		goto error_ret;
@@ -576,7 +631,7 @@ error_uninitialize_ring:
 error_unreg_ring_funcs:
 	adis16240_unconfigure_ring(indio_dev);
 error_free_dev:
-	iio_free_device(indio_dev);
+	iio_device_free(indio_dev);
 error_ret:
 	return ret;
 }
@@ -592,7 +647,7 @@ static int adis16240_remove(struct spi_device *spi)
 	adis16240_remove_trigger(indio_dev);
 	iio_buffer_unregister(indio_dev);
 	adis16240_unconfigure_ring(indio_dev);
-	iio_free_device(indio_dev);
+	iio_device_free(indio_dev);
 
 	return 0;
 }
diff --git a/drivers/staging/iio/accel/adis16240_ring.c b/drivers/staging/iio/accel/adis16240_ring.c
index 43ba84e993a..86a2a4757ea 100644
--- a/drivers/staging/iio/accel/adis16240_ring.c
+++ b/drivers/staging/iio/accel/adis16240_ring.c
@@ -5,20 +5,19 @@
 #include <linux/spi/spi.h>
 #include <linux/slab.h>
 
-#include "../iio.h"
+#include <linux/iio/iio.h>
 #include "../ring_sw.h"
-#include "../trigger_consumer.h"
+#include <linux/iio/trigger_consumer.h>
 #include "adis16240.h"
 
 /**
  * adis16240_read_ring_data() read data registers which will be placed into ring
- * @dev: device associated with child of actual device (iio_dev or iio_trig)
+ * @indio_dev: the IIO device
  * @rx: somewhere to pass back the value read
  **/
-static int adis16240_read_ring_data(struct device *dev, u8 *rx)
+static int adis16240_read_ring_data(struct iio_dev *indio_dev, u8 *rx)
 {
 	struct spi_message msg;
-	struct iio_dev *indio_dev = dev_get_drvdata(dev);
 	struct adis16240_state *st = iio_priv(indio_dev);
 	struct spi_transfer xfers[ADIS16240_OUTPUTS + 1];
 	int ret;
@@ -61,22 +60,21 @@ static irqreturn_t adis16240_trigger_handler(int irq, void *p)
 
 	int i = 0;
 	s16 *data;
-	size_t datasize = ring->access->get_bytes_per_datum(ring);
 
-	data = kmalloc(datasize, GFP_KERNEL);
+	data = kmalloc(indio_dev->scan_bytes, GFP_KERNEL);
 	if (data == NULL) {
 		dev_err(&st->us->dev, "memory alloc failed in ring bh");
 		return -ENOMEM;
 	}
 
 	if (!bitmap_empty(indio_dev->active_scan_mask, indio_dev->masklength) &&
-	    adis16240_read_ring_data(&indio_dev->dev, st->rx) >= 0)
+	    adis16240_read_ring_data(indio_dev, st->rx) >= 0)
 		for (; i < bitmap_weight(indio_dev->active_scan_mask,
 					 indio_dev->masklength); i++)
 			data[i] = be16_to_cpup((__be16 *)&(st->rx[i*2]));
 
 	/* Guaranteed to be aligned with 8 byte boundary */
-	if (ring->scan_timestamp)
+	if (indio_dev->scan_timestamp)
 		*((s64 *)(data + ((i + 3)/4)*4)) = pf->timestamp;
 
 	ring->access->store_to(ring, (u8 *)data, pf->timestamp);
diff --git a/drivers/staging/iio/accel/adis16240_trigger.c b/drivers/staging/iio/accel/adis16240_trigger.c
index 8e0ce568e64..fa90a22b143 100644
--- a/drivers/staging/iio/accel/adis16240_trigger.c
+++ b/drivers/staging/iio/accel/adis16240_trigger.c
@@ -3,8 +3,8 @@
 #include <linux/spi/spi.h>
 #include <linux/export.h>
 
-#include "../iio.h"
-#include "../trigger.h"
+#include <linux/iio/iio.h>
+#include <linux/iio/trigger.h>
 #include "adis16240.h"
 
 /**
@@ -38,7 +38,7 @@ int adis16240_probe_trigger(struct iio_dev *indio_dev)
 	int ret;
 	struct adis16240_state *st = iio_priv(indio_dev);
 
-	st->trig = iio_allocate_trigger("adis16240-dev%d", indio_dev->id);
+	st->trig = iio_trigger_alloc("adis16240-dev%d", indio_dev->id);
 	if (st->trig == NULL) {
 		ret = -ENOMEM;
 		goto error_ret;
@@ -67,7 +67,7 @@ int adis16240_probe_trigger(struct iio_dev *indio_dev)
 error_free_irq:
 	free_irq(st->us->irq, st->trig);
 error_free_trig:
-	iio_free_trigger(st->trig);
+	iio_trigger_free(st->trig);
 error_ret:
 	return ret;
 }
@@ -78,5 +78,5 @@ void adis16240_remove_trigger(struct iio_dev *indio_dev)
 
 	iio_trigger_unregister(st->trig);
 	free_irq(st->us->irq, st->trig);
-	iio_free_trigger(st->trig);
+	iio_trigger_free(st->trig);
 }
diff --git a/drivers/staging/iio/accel/kxsd9.c b/drivers/staging/iio/accel/kxsd9.c
index d13d7215ff6..8cf7cd943c9 100644
--- a/drivers/staging/iio/accel/kxsd9.c
+++ b/drivers/staging/iio/accel/kxsd9.c
@@ -23,8 +23,8 @@
 #include <linux/slab.h>
 #include <linux/module.h>
 
-#include "../iio.h"
-#include "../sysfs.h"
+#include <linux/iio/iio.h>
+#include <linux/iio/sysfs.h>
 
 #define KXSD9_REG_X		0x00
 #define KXSD9_REG_Y		0x02
@@ -158,7 +158,7 @@ static int kxsd9_read_raw(struct iio_dev *indio_dev,
 	struct kxsd9_state *st = iio_priv(indio_dev);
 
 	switch (mask) {
-	case 0:
+	case IIO_CHAN_INFO_RAW:
 		ret = kxsd9_read(indio_dev, chan->address);
 		if (ret < 0)
 			goto error_ret;
@@ -181,7 +181,8 @@ error_ret:
 		.type = IIO_ACCEL,					\
 		.modified = 1,						\
 		.channel2 = IIO_MOD_##axis,				\
-		.info_mask = IIO_CHAN_INFO_SCALE_SHARED_BIT,		\
+		.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT |		\
+			IIO_CHAN_INFO_SCALE_SHARED_BIT,			\
 		.address = KXSD9_REG_##axis,				\
 	}
 
@@ -189,6 +190,7 @@ static struct iio_chan_spec kxsd9_channels[] = {
 	KXSD9_ACCEL_CHAN(X), KXSD9_ACCEL_CHAN(Y), KXSD9_ACCEL_CHAN(Z),
 	{
 		.type = IIO_VOLTAGE,
+		.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT,
 		.indexed = 1,
 		.address = KXSD9_REG_AUX,
 	}
@@ -226,7 +228,7 @@ static int __devinit kxsd9_probe(struct spi_device *spi)
 	struct kxsd9_state *st;
 	int ret = 0;
 
-	indio_dev = iio_allocate_device(sizeof(*st));
+	indio_dev = iio_device_alloc(sizeof(*st));
 	if (indio_dev == NULL) {
 		ret = -ENOMEM;
 		goto error_ret;
@@ -254,7 +256,7 @@ static int __devinit kxsd9_probe(struct spi_device *spi)
 	return 0;
 
 error_free_dev:
-	iio_free_device(indio_dev);
+	iio_device_free(indio_dev);
 error_ret:
 	return ret;
 }
@@ -262,7 +264,7 @@ error_ret:
 static int __devexit kxsd9_remove(struct spi_device *spi)
 {
 	iio_device_unregister(spi_get_drvdata(spi));
-	iio_free_device(spi_get_drvdata(spi));
+	iio_device_free(spi_get_drvdata(spi));
 
 	return 0;
 }
diff --git a/drivers/staging/iio/accel/lis3l02dq_core.c b/drivers/staging/iio/accel/lis3l02dq_core.c
index 376da513796..9d263484fb8 100644
--- a/drivers/staging/iio/accel/lis3l02dq_core.c
+++ b/drivers/staging/iio/accel/lis3l02dq_core.c
@@ -23,10 +23,10 @@
 #include <linux/sysfs.h>
 #include <linux/module.h>
 
-#include "../iio.h"
-#include "../sysfs.h"
-#include "../events.h"
-#include "../buffer.h"
+#include <linux/iio/iio.h>
+#include <linux/iio/sysfs.h>
+#include <linux/iio/events.h>
+#include <linux/iio/buffer.h>
 
 #include "lis3l02dq.h"
 
@@ -257,7 +257,7 @@ static int lis3l02dq_read_raw(struct iio_dev *indio_dev,
 	u8 reg;
 
 	switch (mask) {
-	case 0:
+	case IIO_CHAN_INFO_RAW:
 		/* Take the iio_dev status lock */
 		mutex_lock(&indio_dev->mlock);
 		if (indio_dev->currentmode == INDIO_BUFFER_TRIGGERED) {
@@ -297,7 +297,7 @@ static ssize_t lis3l02dq_read_frequency(struct device *dev,
 					struct device_attribute *attr,
 					char *buf)
 {
-	struct iio_dev *indio_dev = dev_get_drvdata(dev);
+	struct iio_dev *indio_dev = dev_to_iio_dev(dev);
 	int ret, len = 0;
 	s8 t;
 	ret = lis3l02dq_spi_read_reg_8(indio_dev,
@@ -328,7 +328,7 @@ static ssize_t lis3l02dq_write_frequency(struct device *dev,
 					 const char *buf,
 					 size_t len)
 {
-	struct iio_dev *indio_dev = dev_get_drvdata(dev);
+	struct iio_dev *indio_dev = dev_to_iio_dev(dev);
 	unsigned long val;
 	int ret;
 	u8 t;
@@ -513,7 +513,8 @@ static irqreturn_t lis3l02dq_event_handler(int irq, void *private)
 }
 
 #define LIS3L02DQ_INFO_MASK				\
-	(IIO_CHAN_INFO_SCALE_SHARED_BIT |		\
+	(IIO_CHAN_INFO_RAW_SEPARATE_BIT |		\
+	 IIO_CHAN_INFO_SCALE_SHARED_BIT |		\
 	 IIO_CHAN_INFO_CALIBSCALE_SEPARATE_BIT |	\
 	 IIO_CHAN_INFO_CALIBBIAS_SEPARATE_BIT)
 
@@ -521,13 +522,26 @@ static irqreturn_t lis3l02dq_event_handler(int irq, void *private)
 	(IIO_EV_BIT(IIO_EV_TYPE_THRESH, IIO_EV_DIR_RISING) |	\
 	 IIO_EV_BIT(IIO_EV_TYPE_THRESH, IIO_EV_DIR_FALLING))
 
+#define LIS3L02DQ_CHAN(index, mod)				\
+	{							\
+		.type = IIO_ACCEL,				\
+		.modified = 1,					\
+		.channel2 = mod,				\
+		.info_mask =  LIS3L02DQ_INFO_MASK,		\
+		.address = index,				\
+		.scan_index = index,				\
+		.scan_type = {					\
+			.sign = 's',				\
+			.realbits = 12,				\
+			.storagebits = 16,			\
+		},						\
+		.event_mask = LIS3L02DQ_EVENT_MASK,		\
+	 }
+
 static struct iio_chan_spec lis3l02dq_channels[] = {
-	IIO_CHAN(IIO_ACCEL, 1, 0, 0, NULL, 0, IIO_MOD_X, LIS3L02DQ_INFO_MASK,
-		 0, 0, IIO_ST('s', 12, 16, 0), LIS3L02DQ_EVENT_MASK),
-	IIO_CHAN(IIO_ACCEL, 1, 0, 0, NULL, 0, IIO_MOD_Y, LIS3L02DQ_INFO_MASK,
-		 1, 1, IIO_ST('s', 12, 16, 0), LIS3L02DQ_EVENT_MASK),
-	IIO_CHAN(IIO_ACCEL, 1, 0, 0, NULL, 0, IIO_MOD_Z, LIS3L02DQ_INFO_MASK,
-		 2, 2, IIO_ST('s', 12, 16, 0), LIS3L02DQ_EVENT_MASK),
+	LIS3L02DQ_CHAN(0, IIO_MOD_X),
+	LIS3L02DQ_CHAN(1, IIO_MOD_Y),
+	LIS3L02DQ_CHAN(2, IIO_MOD_Z),
 	IIO_CHAN_SOFT_TIMESTAMP(3)
 };
 
@@ -666,7 +680,7 @@ static int __devinit lis3l02dq_probe(struct spi_device *spi)
 	struct lis3l02dq_state *st;
 	struct iio_dev *indio_dev;
 
-	indio_dev = iio_allocate_device(sizeof *st);
+	indio_dev = iio_device_alloc(sizeof *st);
 	if (indio_dev == NULL) {
 		ret = -ENOMEM;
 		goto error_ret;
@@ -724,7 +738,7 @@ static int __devinit lis3l02dq_probe(struct spi_device *spi)
 	return 0;
 
 error_remove_trigger:
-	if (indio_dev->modes & INDIO_BUFFER_TRIGGERED)
+	if (spi->irq && gpio_is_valid(irq_to_gpio(spi->irq)))
 		lis3l02dq_remove_trigger(indio_dev);
 error_free_interrupt:
 	if (spi->irq && gpio_is_valid(irq_to_gpio(spi->irq)) > 0)
@@ -734,7 +748,7 @@ error_uninitialize_buffer:
 error_unreg_buffer_funcs:
 	lis3l02dq_unconfigure_buffer(indio_dev);
 error_free_dev:
-	iio_free_device(indio_dev);
+	iio_device_free(indio_dev);
 error_ret:
 	return ret;
 }
@@ -789,7 +803,7 @@ static int lis3l02dq_remove(struct spi_device *spi)
 	iio_buffer_unregister(indio_dev);
 	lis3l02dq_unconfigure_buffer(indio_dev);
 
-	iio_free_device(indio_dev);
+	iio_device_free(indio_dev);
 err_ret:
 	return ret;
 }
diff --git a/drivers/staging/iio/accel/lis3l02dq_ring.c b/drivers/staging/iio/accel/lis3l02dq_ring.c
index 0fc3973f32a..51b00dfc046 100644
--- a/drivers/staging/iio/accel/lis3l02dq_ring.c
+++ b/drivers/staging/iio/accel/lis3l02dq_ring.c
@@ -6,11 +6,11 @@
 #include <linux/slab.h>
 #include <linux/export.h>
 
-#include "../iio.h"
+#include <linux/iio/iio.h>
 #include "../ring_sw.h"
-#include "../kfifo_buf.h"
-#include "../trigger.h"
-#include "../trigger_consumer.h"
+#include <linux/iio/kfifo_buf.h>
+#include <linux/iio/trigger.h>
+#include <linux/iio/trigger_consumer.h>
 #include "lis3l02dq.h"
 
 /**
@@ -137,9 +137,9 @@ static irqreturn_t lis3l02dq_trigger_handler(int irq, void *p)
 	struct iio_dev *indio_dev = pf->indio_dev;
 	struct iio_buffer *buffer = indio_dev->buffer;
 	int len = 0;
-	size_t datasize = buffer->access->get_bytes_per_datum(buffer);
-	char *data = kmalloc(datasize, GFP_KERNEL);
+	char *data;
 
+	data = kmalloc(indio_dev->scan_bytes, GFP_KERNEL);
 	if (data == NULL) {
 		dev_err(indio_dev->dev.parent,
 			"memory alloc failed in buffer bh");
@@ -150,7 +150,7 @@ static irqreturn_t lis3l02dq_trigger_handler(int irq, void *p)
 		len = lis3l02dq_get_buffer_element(indio_dev, data);
 
 	  /* Guaranteed to be aligned with 8 byte boundary */
-	if (buffer->scan_timestamp)
+	if (indio_dev->scan_timestamp)
 		*(s64 *)(((phys_addr_t)data + len
 				+ sizeof(s64) - 1) & ~(sizeof(s64) - 1))
 			= pf->timestamp;
@@ -163,12 +163,11 @@ static irqreturn_t lis3l02dq_trigger_handler(int irq, void *p)
 
 /* Caller responsible for locking as necessary. */
 static int
-__lis3l02dq_write_data_ready_config(struct device *dev, bool state)
+__lis3l02dq_write_data_ready_config(struct iio_dev *indio_dev, bool state)
 {
 	int ret;
 	u8 valold;
 	bool currentlyset;
-	struct iio_dev *indio_dev = dev_get_drvdata(dev);
 	struct lis3l02dq_state *st = iio_priv(indio_dev);
 
 /* Get the current event mask register */
@@ -236,7 +235,7 @@ static int lis3l02dq_data_rdy_trigger_set_state(struct iio_trigger *trig,
 	int ret = 0;
 	u8 t;
 
-	__lis3l02dq_write_data_ready_config(&indio_dev->dev, state);
+	__lis3l02dq_write_data_ready_config(indio_dev, state);
 	if (state == false) {
 		/*
 		 * A possible quirk with the handler is currently worked around
@@ -286,7 +285,7 @@ int lis3l02dq_probe_trigger(struct iio_dev *indio_dev)
 	int ret;
 	struct lis3l02dq_state *st = iio_priv(indio_dev);
 
-	st->trig = iio_allocate_trigger("lis3l02dq-dev%d", indio_dev->id);
+	st->trig = iio_trigger_alloc("lis3l02dq-dev%d", indio_dev->id);
 	if (!st->trig) {
 		ret = -ENOMEM;
 		goto error_ret;
@@ -302,7 +301,7 @@ int lis3l02dq_probe_trigger(struct iio_dev *indio_dev)
 	return 0;
 
 error_free_trig:
-	iio_free_trigger(st->trig);
+	iio_trigger_free(st->trig);
 error_ret:
 	return ret;
 }
@@ -312,7 +311,7 @@ void lis3l02dq_remove_trigger(struct iio_dev *indio_dev)
 	struct lis3l02dq_state *st = iio_priv(indio_dev);
 
 	iio_trigger_unregister(st->trig);
-	iio_free_trigger(st->trig);
+	iio_trigger_free(st->trig);
 }
 
 void lis3l02dq_unconfigure_buffer(struct iio_dev *indio_dev)
diff --git a/drivers/staging/iio/accel/sca3000_core.c b/drivers/staging/iio/accel/sca3000_core.c
index 49764fb7181..6ec5c204ff1 100644
--- a/drivers/staging/iio/accel/sca3000_core.c
+++ b/drivers/staging/iio/accel/sca3000_core.c
@@ -18,10 +18,10 @@
 #include <linux/spi/spi.h>
 #include <linux/sysfs.h>
 #include <linux/module.h>
-#include "../iio.h"
-#include "../sysfs.h"
-#include "../events.h"
-#include "../buffer.h"
+#include <linux/iio/iio.h>
+#include <linux/iio/sysfs.h>
+#include <linux/iio/events.h>
+#include <linux/iio/buffer.h>
 
 #include "sca3000.h"
 
@@ -241,7 +241,7 @@ error_ret:
 static int sca3000_check_status(struct device *dev)
 {
 	int ret;
-	struct iio_dev *indio_dev = dev_get_drvdata(dev);
+	struct iio_dev *indio_dev = dev_to_iio_dev(dev);
 	struct sca3000_state *st = iio_priv(indio_dev);
 
 	mutex_lock(&st->lock);
@@ -268,7 +268,7 @@ static ssize_t sca3000_show_rev(struct device *dev,
 				char *buf)
 {
 	int len = 0, ret;
-	struct iio_dev *indio_dev = dev_get_drvdata(dev);
+	struct iio_dev *indio_dev = dev_to_iio_dev(dev);
 	struct sca3000_state *st = iio_priv(indio_dev);
 
 	mutex_lock(&st->lock);
@@ -296,7 +296,7 @@ sca3000_show_available_measurement_modes(struct device *dev,
 					 struct device_attribute *attr,
 					 char *buf)
 {
-	struct iio_dev *indio_dev = dev_get_drvdata(dev);
+	struct iio_dev *indio_dev = dev_to_iio_dev(dev);
 	struct sca3000_state *st = iio_priv(indio_dev);
 	int len = 0;
 
@@ -328,7 +328,7 @@ sca3000_show_measurement_mode(struct device *dev,
 			      struct device_attribute *attr,
 			      char *buf)
 {
-	struct iio_dev *indio_dev = dev_get_drvdata(dev);
+	struct iio_dev *indio_dev = dev_to_iio_dev(dev);
 	struct sca3000_state *st = iio_priv(indio_dev);
 	int len = 0, ret;
 
@@ -379,7 +379,7 @@ sca3000_store_measurement_mode(struct device *dev,
 			       const char *buf,
 			       size_t len)
 {
-	struct iio_dev *indio_dev = dev_get_drvdata(dev);
+	struct iio_dev *indio_dev = dev_to_iio_dev(dev);
 	struct sca3000_state *st = iio_priv(indio_dev);
 	int ret;
 	u8 mask = 0x03;
@@ -429,17 +429,31 @@ static IIO_DEVICE_ATTR(measurement_mode, S_IRUGO | S_IWUSR,
 static IIO_DEVICE_ATTR(revision, S_IRUGO, sca3000_show_rev, NULL, 0);
 
 #define SCA3000_INFO_MASK			\
-	IIO_CHAN_INFO_SCALE_SHARED_BIT
+	IIO_CHAN_INFO_RAW_SEPARATE_BIT | IIO_CHAN_INFO_SCALE_SHARED_BIT
 #define SCA3000_EVENT_MASK					\
 	(IIO_EV_BIT(IIO_EV_TYPE_MAG, IIO_EV_DIR_RISING))
 
+#define SCA3000_CHAN(index, mod)				\
+	{							\
+		.type = IIO_ACCEL,				\
+		.modified = 1,					\
+		.channel2 = mod,				\
+		.info_mask = SCA3000_INFO_MASK,			\
+		.address = index,				\
+		.scan_index = index,				\
+		.scan_type = {					\
+			.sign = 's',				\
+			.realbits = 11,				\
+			.storagebits = 16,			\
+			.shift = 5,				\
+		},						\
+		.event_mask = SCA3000_EVENT_MASK,		\
+	 }
+
 static struct iio_chan_spec sca3000_channels[] = {
-	IIO_CHAN(IIO_ACCEL, 1, 0, 0, NULL, 0, IIO_MOD_X, SCA3000_INFO_MASK,
-		 0, 0, IIO_ST('s', 11, 16, 5), SCA3000_EVENT_MASK),
-	IIO_CHAN(IIO_ACCEL, 1, 0, 0, NULL, 0, IIO_MOD_Y, SCA3000_INFO_MASK,
-		 1, 1, IIO_ST('s', 11, 16, 5), SCA3000_EVENT_MASK),
-	IIO_CHAN(IIO_ACCEL, 1, 0, 0, NULL, 0, IIO_MOD_Z, SCA3000_INFO_MASK,
-		 2, 2, IIO_ST('s', 11, 16, 5), SCA3000_EVENT_MASK),
+	SCA3000_CHAN(0, IIO_MOD_X),
+	SCA3000_CHAN(1, IIO_MOD_Y),
+	SCA3000_CHAN(2, IIO_MOD_Z),
 };
 
 static u8 sca3000_addresses[3][3] = {
@@ -462,7 +476,7 @@ static int sca3000_read_raw(struct iio_dev *indio_dev,
 	u8 address;
 
 	switch (mask) {
-	case 0:
+	case IIO_CHAN_INFO_RAW:
 		mutex_lock(&st->lock);
 		if (st->mo_det_use_count) {
 			mutex_unlock(&st->lock);
@@ -503,7 +517,7 @@ static ssize_t sca3000_read_av_freq(struct device *dev,
 			     struct device_attribute *attr,
 			     char *buf)
 {
-	struct iio_dev *indio_dev = dev_get_drvdata(dev);
+	struct iio_dev *indio_dev = dev_to_iio_dev(dev);
 	struct sca3000_state *st = iio_priv(indio_dev);
 	int len = 0, ret, val;
 
@@ -574,7 +588,7 @@ static ssize_t sca3000_read_frequency(struct device *dev,
 			       struct device_attribute *attr,
 			       char *buf)
 {
-	struct iio_dev *indio_dev = dev_get_drvdata(dev);
+	struct iio_dev *indio_dev = dev_to_iio_dev(dev);
 	struct sca3000_state *st = iio_priv(indio_dev);
 	int ret, len = 0, base_freq = 0, val;
 
@@ -616,7 +630,7 @@ static ssize_t sca3000_set_frequency(struct device *dev,
 			      const char *buf,
 			      size_t len)
 {
-	struct iio_dev *indio_dev = dev_get_drvdata(dev);
+	struct iio_dev *indio_dev = dev_to_iio_dev(dev);
 	struct sca3000_state *st = iio_priv(indio_dev);
 	int ret, base_freq = 0;
 	int ctrlval;
@@ -676,7 +690,7 @@ static ssize_t sca3000_read_temp(struct device *dev,
 				 struct device_attribute *attr,
 				 char *buf)
 {
-	struct iio_dev *indio_dev = dev_get_drvdata(dev);
+	struct iio_dev *indio_dev = dev_to_iio_dev(dev);
 	struct sca3000_state *st = iio_priv(indio_dev);
 	int ret;
 	int val;
@@ -897,7 +911,7 @@ static ssize_t sca3000_query_free_fall_mode(struct device *dev,
 					    char *buf)
 {
 	int ret, len;
-	struct iio_dev *indio_dev = dev_get_drvdata(dev);
+	struct iio_dev *indio_dev = dev_to_iio_dev(dev);
 	struct sca3000_state *st = iio_priv(indio_dev);
 	int val;
 
@@ -925,7 +939,7 @@ static ssize_t sca3000_set_free_fall_mode(struct device *dev,
 					  const char *buf,
 					  size_t len)
 {
-	struct iio_dev *indio_dev = dev_get_drvdata(dev);
+	struct iio_dev *indio_dev = dev_to_iio_dev(dev);
 	struct sca3000_state *st = iio_priv(indio_dev);
 	long val;
 	int ret;
@@ -1131,7 +1145,7 @@ static int __devinit sca3000_probe(struct spi_device *spi)
 	struct sca3000_state *st;
 	struct iio_dev *indio_dev;
 
-	indio_dev = iio_allocate_device(sizeof(*st));
+	indio_dev = iio_device_alloc(sizeof(*st));
 	if (indio_dev == NULL) {
 		ret = -ENOMEM;
 		goto error_ret;
@@ -1195,7 +1209,7 @@ error_unregister_ring:
 error_unregister_dev:
 	iio_device_unregister(indio_dev);
 error_free_dev:
-	iio_free_device(indio_dev);
+	iio_device_free(indio_dev);
 
 error_ret:
 	return ret;
@@ -1233,7 +1247,7 @@ static int sca3000_remove(struct spi_device *spi)
 	iio_device_unregister(indio_dev);
 	iio_buffer_unregister(indio_dev);
 	sca3000_unconfigure_ring(indio_dev);
-	iio_free_device(indio_dev);
+	iio_device_free(indio_dev);
 
 	return 0;
 }
diff --git a/drivers/staging/iio/accel/sca3000_ring.c b/drivers/staging/iio/accel/sca3000_ring.c
index 6b824a11f7f..b7e1a002630 100644
--- a/drivers/staging/iio/accel/sca3000_ring.c
+++ b/drivers/staging/iio/accel/sca3000_ring.c
@@ -18,9 +18,9 @@
 #include <linux/sched.h>
 #include <linux/poll.h>
 
-#include "../iio.h"
-#include "../sysfs.h"
-#include "../buffer.h"
+#include <linux/iio/iio.h>
+#include <linux/iio/sysfs.h>
+#include <linux/iio/buffer.h>
 #include "../ring_hw.h"
 #include "sca3000.h"
 
@@ -157,7 +157,7 @@ static ssize_t sca3000_query_ring_int(struct device *dev,
 {
 	struct iio_dev_attr *this_attr = to_iio_dev_attr(attr);
 	int ret, val;
-	struct iio_dev *indio_dev = dev_get_drvdata(dev);
+	struct iio_dev *indio_dev = dev_to_iio_dev(dev);
 	struct sca3000_state *st = iio_priv(indio_dev);
 
 	mutex_lock(&st->lock);
@@ -178,7 +178,7 @@ static ssize_t sca3000_set_ring_int(struct device *dev,
 				      const char *buf,
 				      size_t len)
 {
-	struct iio_dev *indio_dev = dev_get_drvdata(dev);
+	struct iio_dev *indio_dev = dev_to_iio_dev(dev);
 	struct sca3000_state *st = iio_priv(indio_dev);
 	struct iio_dev_attr *this_attr = to_iio_dev_attr(attr);
 	long val;
@@ -219,7 +219,7 @@ static ssize_t sca3000_show_buffer_scale(struct device *dev,
 					 struct device_attribute *attr,
 					 char *buf)
 {
-	struct iio_dev *indio_dev = dev_get_drvdata(dev);
+	struct iio_dev *indio_dev = dev_to_iio_dev(dev);
 	struct sca3000_state *st = iio_priv(indio_dev);
 
 	return sprintf(buf, "0.%06d\n", 4*st->info->scale);
diff --git a/drivers/staging/iio/adc/Kconfig b/drivers/staging/iio/adc/Kconfig
index 592eabd85f3..2490dd25093 100644
--- a/drivers/staging/iio/adc/Kconfig
+++ b/drivers/staging/iio/adc/Kconfig
@@ -25,7 +25,7 @@ config AD7606
 	depends on GPIOLIB
 	select IIO_BUFFER
 	select IIO_TRIGGER
-	select IIO_SW_RING
+	select IIO_KFIFO_BUF
 	help
 	  Say yes here to build support for Analog Devices:
 	  ad7606, ad7606-6, ad7606-4 analog to digital converters (ADC).
@@ -63,7 +63,7 @@ config AD799X_RING_BUFFER
 	bool "Analog Devices AD799x: use ring buffer"
 	depends on AD799X
 	select IIO_BUFFER
-	select IIO_SW_RING
+	select IIO_KFIFO_BUF
 	help
 	  Say yes here to include ring buffer support in the AD799X
 	  ADC driver.
@@ -72,7 +72,7 @@ config AD7476
 	tristate "Analog Devices AD7475/6/7/8 AD7466/7/8 and AD7495 ADC driver"
 	depends on SPI
 	select IIO_BUFFER
-	select IIO_SW_RING
+	select IIO_KFIFO_BUF
 	select IIO_TRIGGER
 	help
 	  Say yes here to build support for Analog Devices
@@ -87,7 +87,7 @@ config AD7887
 	tristate "Analog Devices AD7887 ADC driver"
 	depends on SPI
 	select IIO_BUFFER
-	select IIO_SW_RING
+	select IIO_KFIFO_BUF
 	select IIO_TRIGGER
 	help
 	  Say yes here to build support for Analog Devices
@@ -113,7 +113,7 @@ config AD7793
 	tristate "Analog Devices AD7792 AD7793 ADC driver"
 	depends on SPI
 	select IIO_BUFFER
-	select IIO_SW_RING
+	select IIO_KFIFO_BUF
 	select IIO_TRIGGER
 	help
 	  Say yes here to build support for Analog Devices
@@ -135,7 +135,7 @@ config AD7192
 	tristate "Analog Devices AD7190 AD7192 AD7195 ADC driver"
 	depends on SPI
 	select IIO_BUFFER
-	select IIO_SW_RING
+	select IIO_KFIFO_BUF
 	select IIO_TRIGGER
 	help
 	  Say yes here to build support for Analog Devices AD7190,
@@ -195,11 +195,20 @@ config MAX1363_RING_BUFFER
 
 config LPC32XX_ADC
 	tristate "NXP LPC32XX ADC"
-	depends on ARCH_LPC32XX && !TOUCHSCREEN_LPC32XX
+	depends on ARCH_LPC32XX
 	help
 	  Say yes here to build support for the integrated ADC inside the
 	  LPC32XX SoC. Note that this feature uses the same hardware as the
-	  touchscreen driver, so you can only select one of the two drivers
-	  (lpc32xx_adc or lpc32xx_ts). Provides direct access via sysfs.
+	  touchscreen driver, so you should either select only one of the two
+	  drivers (lpc32xx_adc or lpc32xx_ts) or, in the OpenFirmware case,
+	  activate only one via device tree selection.  Provides direct access
+	  via sysfs.
+
+config SPEAR_ADC
+	tristate "ST SPEAr ADC"
+	depends on PLAT_SPEAR
+	help
+	  Say yes here to build support for the integrated ADC inside the
+	  ST SPEAr SoC. Provides direct access via sysfs.
 
 endmenu
diff --git a/drivers/staging/iio/adc/Makefile b/drivers/staging/iio/adc/Makefile
index f83ab9551d8..14e98b62b70 100644
--- a/drivers/staging/iio/adc/Makefile
+++ b/drivers/staging/iio/adc/Makefile
@@ -38,3 +38,4 @@ obj-$(CONFIG_ADT7310) += adt7310.o
 obj-$(CONFIG_ADT7410) += adt7410.o
 obj-$(CONFIG_AD7280) += ad7280a.o
 obj-$(CONFIG_LPC32XX_ADC) += lpc32xx_adc.o
+obj-$(CONFIG_SPEAR_ADC) += spear_adc.o
diff --git a/drivers/staging/iio/adc/ad7192.c b/drivers/staging/iio/adc/ad7192.c
index 9fd6d63d299..5eaeaf1f0ae 100644
--- a/drivers/staging/iio/adc/ad7192.c
+++ b/drivers/staging/iio/adc/ad7192.c
@@ -1,7 +1,7 @@
 /*
  * AD7190 AD7192 AD7195 SPI ADC driver
  *
- * Copyright 2011 Analog Devices Inc.
+ * Copyright 2011-2012 Analog Devices Inc.
  *
  * Licensed under the GPL-2.
  */
@@ -17,12 +17,12 @@
 #include <linux/sched.h>
 #include <linux/delay.h>
 
-#include "../iio.h"
-#include "../sysfs.h"
-#include "../buffer.h"
-#include "../ring_sw.h"
-#include "../trigger.h"
-#include "../trigger_consumer.h"
+#include <linux/iio/iio.h>
+#include <linux/iio/sysfs.h>
+#include <linux/iio/buffer.h>
+#include <linux/iio/kfifo_buf.h>
+#include <linux/iio/trigger.h>
+#include <linux/iio/trigger_consumer.h>
 
 #include "ad7192.h"
 
@@ -456,31 +456,19 @@ out:
 static int ad7192_ring_preenable(struct iio_dev *indio_dev)
 {
 	struct ad7192_state *st = iio_priv(indio_dev);
-	struct iio_buffer *ring = indio_dev->buffer;
-	size_t d_size;
 	unsigned channel;
+	int ret;
 
 	if (bitmap_empty(indio_dev->active_scan_mask, indio_dev->masklength))
 		return -EINVAL;
 
+	ret = iio_sw_buffer_preenable(indio_dev);
+	if (ret < 0)
+		return ret;
+
 	channel = find_first_bit(indio_dev->active_scan_mask,
 				 indio_dev->masklength);
 
-	d_size = bitmap_weight(indio_dev->active_scan_mask,
-			       indio_dev->masklength) *
-		 indio_dev->channels[0].scan_type.storagebits / 8;
-
-	if (ring->scan_timestamp) {
-		d_size += sizeof(s64);
-
-		if (d_size % sizeof(s64))
-			d_size += sizeof(s64) - (d_size % sizeof(s64));
-	}
-
-	if (indio_dev->buffer->access->set_bytes_per_datum)
-		indio_dev->buffer->access->
-			set_bytes_per_datum(indio_dev->buffer, d_size);
-
 	st->mode  = (st->mode & ~AD7192_MODE_SEL(-1)) |
 		    AD7192_MODE_SEL(AD7192_MODE_CONT);
 	st->conf  = (st->conf & ~AD7192_CONF_CHAN(-1)) |
@@ -533,7 +521,7 @@ static irqreturn_t ad7192_trigger_handler(int irq, void *p)
 				  indio_dev->channels[0].scan_type.realbits/8);
 
 	/* Guaranteed to be aligned with 8 byte boundary */
-	if (ring->scan_timestamp)
+	if (indio_dev->scan_timestamp)
 		dat64[1] = pf->timestamp;
 
 	ring->access->store_to(ring, (u8 *)dat64, pf->timestamp);
@@ -556,7 +544,7 @@ static int ad7192_register_ring_funcs_and_init(struct iio_dev *indio_dev)
 {
 	int ret;
 
-	indio_dev->buffer = iio_sw_rb_allocate(indio_dev);
+	indio_dev->buffer = iio_kfifo_allocate(indio_dev);
 	if (!indio_dev->buffer) {
 		ret = -ENOMEM;
 		goto error_ret;
@@ -569,7 +557,7 @@ static int ad7192_register_ring_funcs_and_init(struct iio_dev *indio_dev)
 						 indio_dev->id);
 	if (indio_dev->pollfunc == NULL) {
 		ret = -ENOMEM;
-		goto error_deallocate_sw_rb;
+		goto error_deallocate_kfifo;
 	}
 
 	/* Ring buffer functions - here trigger setup related */
@@ -579,8 +567,8 @@ static int ad7192_register_ring_funcs_and_init(struct iio_dev *indio_dev)
 	indio_dev->modes |= INDIO_BUFFER_TRIGGERED;
 	return 0;
 
-error_deallocate_sw_rb:
-	iio_sw_rb_free(indio_dev->buffer);
+error_deallocate_kfifo:
+	iio_kfifo_free(indio_dev->buffer);
 error_ret:
 	return ret;
 }
@@ -588,7 +576,7 @@ error_ret:
 static void ad7192_ring_cleanup(struct iio_dev *indio_dev)
 {
 	iio_dealloc_pollfunc(indio_dev->pollfunc);
-	iio_sw_rb_free(indio_dev->buffer);
+	iio_kfifo_free(indio_dev->buffer);
 }
 
 /**
@@ -616,7 +604,7 @@ static int ad7192_probe_trigger(struct iio_dev *indio_dev)
 	struct ad7192_state *st = iio_priv(indio_dev);
 	int ret;
 
-	st->trig = iio_allocate_trigger("%s-dev%d",
+	st->trig = iio_trigger_alloc("%s-dev%d",
 					spi_get_device_id(st->spi)->name,
 					indio_dev->id);
 	if (st->trig == NULL) {
@@ -649,7 +637,7 @@ static int ad7192_probe_trigger(struct iio_dev *indio_dev)
 error_free_irq:
 	free_irq(st->spi->irq, indio_dev);
 error_free_trig:
-	iio_free_trigger(st->trig);
+	iio_trigger_free(st->trig);
 error_ret:
 	return ret;
 }
@@ -660,14 +648,14 @@ static void ad7192_remove_trigger(struct iio_dev *indio_dev)
 
 	iio_trigger_unregister(st->trig);
 	free_irq(st->spi->irq, indio_dev);
-	iio_free_trigger(st->trig);
+	iio_trigger_free(st->trig);
 }
 
 static ssize_t ad7192_read_frequency(struct device *dev,
 		struct device_attribute *attr,
 		char *buf)
 {
-	struct iio_dev *indio_dev = dev_get_drvdata(dev);
+	struct iio_dev *indio_dev = dev_to_iio_dev(dev);
 	struct ad7192_state *st = iio_priv(indio_dev);
 
 	return sprintf(buf, "%d\n", st->mclk /
@@ -679,7 +667,7 @@ static ssize_t ad7192_write_frequency(struct device *dev,
 		const char *buf,
 		size_t len)
 {
-	struct iio_dev *indio_dev = dev_get_drvdata(dev);
+	struct iio_dev *indio_dev = dev_to_iio_dev(dev);
 	struct ad7192_state *st = iio_priv(indio_dev);
 	unsigned long lval;
 	int div, ret;
@@ -718,7 +706,7 @@ static IIO_DEV_ATTR_SAMP_FREQ(S_IWUSR | S_IRUGO,
 static ssize_t ad7192_show_scale_available(struct device *dev,
 			struct device_attribute *attr, char *buf)
 {
-	struct iio_dev *indio_dev = dev_get_drvdata(dev);
+	struct iio_dev *indio_dev = dev_to_iio_dev(dev);
 	struct ad7192_state *st = iio_priv(indio_dev);
 	int i, len = 0;
 
@@ -742,7 +730,7 @@ static ssize_t ad7192_show_ac_excitation(struct device *dev,
 		struct device_attribute *attr,
 		char *buf)
 {
-	struct iio_dev *indio_dev = dev_get_drvdata(dev);
+	struct iio_dev *indio_dev = dev_to_iio_dev(dev);
 	struct ad7192_state *st = iio_priv(indio_dev);
 
 	return sprintf(buf, "%d\n", !!(st->mode & AD7192_MODE_ACX));
@@ -752,7 +740,7 @@ static ssize_t ad7192_show_bridge_switch(struct device *dev,
 		struct device_attribute *attr,
 		char *buf)
 {
-	struct iio_dev *indio_dev = dev_get_drvdata(dev);
+	struct iio_dev *indio_dev = dev_to_iio_dev(dev);
 	struct ad7192_state *st = iio_priv(indio_dev);
 
 	return sprintf(buf, "%d\n", !!(st->gpocon & AD7192_GPOCON_BPDSW));
@@ -763,7 +751,7 @@ static ssize_t ad7192_set(struct device *dev,
 		const char *buf,
 		size_t len)
 {
-	struct iio_dev *indio_dev = dev_get_drvdata(dev);
+	struct iio_dev *indio_dev = dev_to_iio_dev(dev);
 	struct ad7192_state *st = iio_priv(indio_dev);
 	struct iio_dev_attr *this_attr = to_iio_dev_attr(attr);
 	int ret;
@@ -849,7 +837,7 @@ static int ad7192_read_raw(struct iio_dev *indio_dev,
 	bool unipolar = !!(st->conf & AD7192_CONF_UNIPOLAR);
 
 	switch (m) {
-	case 0:
+	case IIO_CHAN_INFO_RAW:
 		mutex_lock(&indio_dev->mlock);
 		if (iio_buffer_enabled(indio_dev))
 			ret = -EBUSY;
@@ -981,7 +969,8 @@ static const struct iio_info ad7195_info = {
 	  .extend_name = _name,						\
 	  .channel = _chan,						\
 	  .channel2 = _chan2,						\
-	  .info_mask = IIO_CHAN_INFO_SCALE_SHARED_BIT,		\
+	  .info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT |			\
+	  IIO_CHAN_INFO_SCALE_SHARED_BIT,				\
 	  .address = _address,						\
 	  .scan_index = _si,						\
 	  .scan_type =  IIO_ST('s', 24, 32, 0)}
@@ -990,7 +979,8 @@ static const struct iio_info ad7195_info = {
 	{ .type = IIO_VOLTAGE,						\
 	  .indexed = 1,							\
 	  .channel = _chan,						\
-	  .info_mask = IIO_CHAN_INFO_SCALE_SHARED_BIT,		\
+	  .info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT |			\
+	  IIO_CHAN_INFO_SCALE_SHARED_BIT,				\
 	  .address = _address,						\
 	  .scan_index = _si,						\
 	  .scan_type =  IIO_ST('s', 24, 32, 0)}
@@ -999,7 +989,8 @@ static const struct iio_info ad7195_info = {
 	{ .type = IIO_TEMP,						\
 	  .indexed = 1,							\
 	  .channel = _chan,						\
-	  .info_mask = IIO_CHAN_INFO_SCALE_SEPARATE_BIT,		\
+	  .info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT |			\
+	  IIO_CHAN_INFO_SCALE_SEPARATE_BIT,				\
 	  .address = _address,						\
 	  .scan_index = _si,						\
 	  .scan_type =  IIO_ST('s', 24, 32, 0)}
@@ -1033,7 +1024,7 @@ static int __devinit ad7192_probe(struct spi_device *spi)
 		return -ENODEV;
 	}
 
-	indio_dev = iio_allocate_device(sizeof(*st));
+	indio_dev = iio_device_alloc(sizeof(*st));
 	if (indio_dev == NULL)
 		return -ENOMEM;
 
@@ -1114,7 +1105,7 @@ error_put_reg:
 	if (!IS_ERR(st->reg))
 		regulator_put(st->reg);
 
-	iio_free_device(indio_dev);
+	iio_device_free(indio_dev);
 
 	return ret;
 }
diff --git a/drivers/staging/iio/adc/ad7280a.c b/drivers/staging/iio/adc/ad7280a.c
index 7dbd6812c24..cfc39a70312 100644
--- a/drivers/staging/iio/adc/ad7280a.c
+++ b/drivers/staging/iio/adc/ad7280a.c
@@ -16,9 +16,9 @@
 #include <linux/interrupt.h>
 #include <linux/module.h>
 
-#include "../iio.h"
-#include "../sysfs.h"
-#include "../events.h"
+#include <linux/iio/iio.h>
+#include <linux/iio/sysfs.h>
+#include <linux/iio/events.h>
 
 #include "ad7280a.h"
 
@@ -384,7 +384,7 @@ static ssize_t ad7280_show_balance_sw(struct device *dev,
 					struct device_attribute *attr,
 					char *buf)
 {
-	struct iio_dev *indio_dev = dev_get_drvdata(dev);
+	struct iio_dev *indio_dev = dev_to_iio_dev(dev);
 	struct ad7280_state *st = iio_priv(indio_dev);
 	struct iio_dev_attr *this_attr = to_iio_dev_attr(attr);
 
@@ -398,7 +398,7 @@ static ssize_t ad7280_store_balance_sw(struct device *dev,
 					 const char *buf,
 					 size_t len)
 {
-	struct iio_dev *indio_dev = dev_get_drvdata(dev);
+	struct iio_dev *indio_dev = dev_to_iio_dev(dev);
 	struct ad7280_state *st = iio_priv(indio_dev);
 	struct iio_dev_attr *this_attr = to_iio_dev_attr(attr);
 	bool readin;
@@ -429,7 +429,7 @@ static ssize_t ad7280_show_balance_timer(struct device *dev,
 					struct device_attribute *attr,
 					char *buf)
 {
-	struct iio_dev *indio_dev = dev_get_drvdata(dev);
+	struct iio_dev *indio_dev = dev_to_iio_dev(dev);
 	struct ad7280_state *st = iio_priv(indio_dev);
 	struct iio_dev_attr *this_attr = to_iio_dev_attr(attr);
 	int ret;
@@ -453,7 +453,7 @@ static ssize_t ad7280_store_balance_timer(struct device *dev,
 					 const char *buf,
 					 size_t len)
 {
-	struct iio_dev *indio_dev = dev_get_drvdata(dev);
+	struct iio_dev *indio_dev = dev_to_iio_dev(dev);
 	struct ad7280_state *st = iio_priv(indio_dev);
 	struct iio_dev_attr *this_attr = to_iio_dev_attr(attr);
 	unsigned long val;
@@ -508,6 +508,7 @@ static int ad7280_channel_init(struct ad7280_state *st)
 			}
 			st->channels[cnt].indexed = 1;
 			st->channels[cnt].info_mask =
+				IIO_CHAN_INFO_RAW_SEPARATE_BIT |
 				IIO_CHAN_INFO_SCALE_SHARED_BIT;
 			st->channels[cnt].address =
 				AD7280A_DEVADDR(dev) << 8 | ch;
@@ -524,7 +525,9 @@ static int ad7280_channel_init(struct ad7280_state *st)
 	st->channels[cnt].channel2 = dev * 6;
 	st->channels[cnt].address = AD7280A_ALL_CELLS;
 	st->channels[cnt].indexed = 1;
-	st->channels[cnt].info_mask = IIO_CHAN_INFO_SCALE_SHARED_BIT;
+	st->channels[cnt].info_mask =
+		IIO_CHAN_INFO_RAW_SEPARATE_BIT |
+		IIO_CHAN_INFO_SCALE_SHARED_BIT;
 	st->channels[cnt].scan_index = cnt;
 	st->channels[cnt].scan_type.sign = 'u';
 	st->channels[cnt].scan_type.realbits = 32;
@@ -596,7 +599,7 @@ static ssize_t ad7280_read_channel_config(struct device *dev,
 					struct device_attribute *attr,
 					char *buf)
 {
-	struct iio_dev *indio_dev = dev_get_drvdata(dev);
+	struct iio_dev *indio_dev = dev_to_iio_dev(dev);
 	struct ad7280_state *st = iio_priv(indio_dev);
 	struct iio_dev_attr *this_attr = to_iio_dev_attr(attr);
 	unsigned val;
@@ -626,7 +629,7 @@ static ssize_t ad7280_write_channel_config(struct device *dev,
 					 const char *buf,
 					 size_t len)
 {
-	struct iio_dev *indio_dev = dev_get_drvdata(dev);
+	struct iio_dev *indio_dev = dev_to_iio_dev(dev);
 	struct ad7280_state *st = iio_priv(indio_dev);
 	struct iio_dev_attr *this_attr = to_iio_dev_attr(attr);
 
@@ -788,7 +791,7 @@ static int ad7280_read_raw(struct iio_dev *indio_dev,
 	int ret;
 
 	switch (m) {
-	case 0:
+	case IIO_CHAN_INFO_RAW:
 		mutex_lock(&indio_dev->mlock);
 		if (chan->address == AD7280A_ALL_CELLS)
 			ret = ad7280_read_all_channels(st, st->scan_cnt, NULL);
@@ -836,7 +839,7 @@ static int __devinit ad7280_probe(struct spi_device *spi)
 	int ret;
 	const unsigned short tACQ_ns[4] = {465, 1010, 1460, 1890};
 	const unsigned short nAVG[4] = {1, 2, 4, 8};
-	struct iio_dev *indio_dev = iio_allocate_device(sizeof(*st));
+	struct iio_dev *indio_dev = iio_device_alloc(sizeof(*st));
 
 	if (indio_dev == NULL)
 		return -ENOMEM;
@@ -942,7 +945,7 @@ error_free_channels:
 	kfree(st->channels);
 
 error_free_device:
-	iio_free_device(indio_dev);
+	iio_device_free(indio_dev);
 
 	return ret;
 }
@@ -961,7 +964,7 @@ static int __devexit ad7280_remove(struct spi_device *spi)
 
 	kfree(st->channels);
 	kfree(st->iio_attr);
-	iio_free_device(indio_dev);
+	iio_device_free(indio_dev);
 
 	return 0;
 }
diff --git a/drivers/staging/iio/adc/ad7291.c b/drivers/staging/iio/adc/ad7291.c
index 81d6b6128cb..029b39c0ba6 100644
--- a/drivers/staging/iio/adc/ad7291.c
+++ b/drivers/staging/iio/adc/ad7291.c
@@ -17,9 +17,9 @@
 #include <linux/regulator/consumer.h>
 #include <linux/err.h>
 
-#include "../iio.h"
-#include "../sysfs.h"
-#include "../events.h"
+#include <linux/iio/iio.h>
+#include <linux/iio/sysfs.h>
+#include <linux/iio/events.h>
 
 /*
  * Simplified handling
@@ -132,7 +132,7 @@ static ssize_t ad7291_store_reset(struct device *dev,
 		const char *buf,
 		size_t len)
 {
-	struct iio_dev *indio_dev = dev_get_drvdata(dev);
+	struct iio_dev *indio_dev = dev_to_iio_dev(dev);
 	struct ad7291_chip_info *chip = iio_priv(indio_dev);
 
 	return ad7291_i2c_write(chip, AD7291_COMMAND,
@@ -214,7 +214,7 @@ static inline ssize_t ad7291_show_hyst(struct device *dev,
 		struct device_attribute *attr,
 		char *buf)
 {
-	struct iio_dev *indio_dev = dev_get_drvdata(dev);
+	struct iio_dev *indio_dev = dev_to_iio_dev(dev);
 	struct ad7291_chip_info *chip = iio_priv(indio_dev);
 	struct iio_dev_attr *this_attr = to_iio_dev_attr(attr);
 	u16 data;
@@ -232,7 +232,7 @@ static inline ssize_t ad7291_set_hyst(struct device *dev,
 				      const char *buf,
 				      size_t len)
 {
-	struct iio_dev *indio_dev = dev_get_drvdata(dev);
+	struct iio_dev *indio_dev = dev_to_iio_dev(dev);
 	struct ad7291_chip_info *chip = iio_priv(indio_dev);
 	struct iio_dev_attr *this_attr = to_iio_dev_attr(attr);
 	u16 data;
@@ -461,7 +461,7 @@ static int ad7291_read_raw(struct iio_dev *indio_dev,
 	s16 signval;
 
 	switch (mask) {
-	case 0:
+	case IIO_CHAN_INFO_RAW:
 		switch (chan->type) {
 		case IIO_VOLTAGE:
 			mutex_lock(&chip->state_lock);
@@ -536,7 +536,8 @@ static int ad7291_read_raw(struct iio_dev *indio_dev,
 #define AD7291_VOLTAGE_CHAN(_chan)					\
 {									\
 	.type = IIO_VOLTAGE,						\
-	.info_mask = IIO_CHAN_INFO_SCALE_SHARED_BIT,			\
+	.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT |			\
+	IIO_CHAN_INFO_SCALE_SHARED_BIT,					\
 	.indexed = 1,							\
 	.channel = _chan,						\
 	.event_mask = IIO_EV_BIT(IIO_EV_TYPE_THRESH, IIO_EV_DIR_RISING)|\
@@ -554,7 +555,8 @@ static const struct iio_chan_spec ad7291_channels[] = {
 	AD7291_VOLTAGE_CHAN(7),
 	{
 		.type = IIO_TEMP,
-		.info_mask = IIO_CHAN_INFO_AVERAGE_RAW_SEPARATE_BIT |
+		.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT |
+				IIO_CHAN_INFO_AVERAGE_RAW_SEPARATE_BIT |
 				IIO_CHAN_INFO_SCALE_SEPARATE_BIT,
 		.indexed = 1,
 		.channel = 0,
@@ -585,7 +587,7 @@ static int __devinit ad7291_probe(struct i2c_client *client,
 	struct iio_dev *indio_dev;
 	int ret = 0, voltage_uv = 0;
 
-	indio_dev = iio_allocate_device(sizeof(*chip));
+	indio_dev = iio_device_alloc(sizeof(*chip));
 	if (indio_dev == NULL) {
 		ret = -ENOMEM;
 		goto error_ret;
@@ -667,7 +669,7 @@ error_put_reg:
 	if (!IS_ERR(chip->reg))
 		regulator_put(chip->reg);
 
-	iio_free_device(indio_dev);
+	iio_device_free(indio_dev);
 error_ret:
 	return ret;
 }
@@ -687,7 +689,7 @@ static int __devexit ad7291_remove(struct i2c_client *client)
 		regulator_put(chip->reg);
 	}
 
-	iio_free_device(indio_dev);
+	iio_device_free(indio_dev);
 
 	return 0;
 }
diff --git a/drivers/staging/iio/adc/ad7298.h b/drivers/staging/iio/adc/ad7298.h
index a0e5dea415e..5051a7e4d4f 100644
--- a/drivers/staging/iio/adc/ad7298.h
+++ b/drivers/staging/iio/adc/ad7298.h
@@ -38,7 +38,6 @@ struct ad7298_platform_data {
 struct ad7298_state {
 	struct spi_device		*spi;
 	struct regulator		*reg;
-	size_t				d_size;
 	u16				int_vref_mv;
 	unsigned			ext_ref;
 	struct spi_transfer		ring_xfer[10];
diff --git a/drivers/staging/iio/adc/ad7298_core.c b/drivers/staging/iio/adc/ad7298_core.c
index 8dd6aa9cf92..c90f2b3e661 100644
--- a/drivers/staging/iio/adc/ad7298_core.c
+++ b/drivers/staging/iio/adc/ad7298_core.c
@@ -16,40 +16,51 @@
 #include <linux/delay.h>
 #include <linux/module.h>
 
-#include "../iio.h"
-#include "../sysfs.h"
-#include "../buffer.h"
+#include <linux/iio/iio.h>
+#include <linux/iio/sysfs.h>
+#include <linux/iio/buffer.h>
 
 #include "ad7298.h"
 
+#define AD7298_V_CHAN(index)						\
+	{								\
+		.type = IIO_VOLTAGE,					\
+		.indexed = 1,						\
+		.channel = index,					\
+		.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT |		\
+		IIO_CHAN_INFO_SCALE_SHARED_BIT,				\
+		.address = index,					\
+		.scan_index = index,					\
+		.scan_type = {						\
+			.sign = 'u',					\
+			.realbits = 12,					\
+			.storagebits = 16,				\
+		},							\
+	}
+
 static struct iio_chan_spec ad7298_channels[] = {
-	IIO_CHAN(IIO_TEMP, 0, 1, 0, NULL, 0, 0,
-		 IIO_CHAN_INFO_SCALE_SEPARATE_BIT,
-		 9, AD7298_CH_TEMP, IIO_ST('s', 32, 32, 0), 0),
-	IIO_CHAN(IIO_VOLTAGE, 0, 1, 0, NULL, 0, 0,
-		 IIO_CHAN_INFO_SCALE_SHARED_BIT,
-		 0, 0, IIO_ST('u', 12, 16, 0), 0),
-	IIO_CHAN(IIO_VOLTAGE, 0, 1, 0, NULL, 1, 0,
-		 IIO_CHAN_INFO_SCALE_SHARED_BIT,
-		 1, 1, IIO_ST('u', 12, 16, 0), 0),
-	IIO_CHAN(IIO_VOLTAGE, 0, 1, 0, NULL, 2, 0,
-		 IIO_CHAN_INFO_SCALE_SHARED_BIT,
-		 2, 2, IIO_ST('u', 12, 16, 0), 0),
-	IIO_CHAN(IIO_VOLTAGE, 0, 1, 0, NULL, 3, 0,
-		 IIO_CHAN_INFO_SCALE_SHARED_BIT,
-		 3, 3, IIO_ST('u', 12, 16, 0), 0),
-	IIO_CHAN(IIO_VOLTAGE, 0, 1, 0, NULL, 4, 0,
-		 IIO_CHAN_INFO_SCALE_SHARED_BIT,
-		 4, 4, IIO_ST('u', 12, 16, 0), 0),
-	IIO_CHAN(IIO_VOLTAGE, 0, 1, 0, NULL, 5, 0,
-		 IIO_CHAN_INFO_SCALE_SHARED_BIT,
-		 5, 5, IIO_ST('u', 12, 16, 0), 0),
-	IIO_CHAN(IIO_VOLTAGE, 0, 1, 0, NULL, 6, 0,
-		 IIO_CHAN_INFO_SCALE_SHARED_BIT,
-		 6, 6, IIO_ST('u', 12, 16, 0), 0),
-	IIO_CHAN(IIO_VOLTAGE, 0, 1, 0, NULL, 7, 0,
-		 IIO_CHAN_INFO_SCALE_SHARED_BIT,
-		 7, 7, IIO_ST('u', 12, 16, 0), 0),
+	{
+		.type = IIO_TEMP,
+		.indexed = 1,
+		.channel = 0,
+		.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT |
+		IIO_CHAN_INFO_SCALE_SEPARATE_BIT,
+		.address = 9,
+		.scan_index = AD7298_CH_TEMP,
+		.scan_type = {
+			.sign = 's',
+			.realbits = 32,
+			.storagebits = 32,
+		},
+	},
+	AD7298_V_CHAN(0),
+	AD7298_V_CHAN(1),
+	AD7298_V_CHAN(2),
+	AD7298_V_CHAN(3),
+	AD7298_V_CHAN(4),
+	AD7298_V_CHAN(5),
+	AD7298_V_CHAN(6),
+	AD7298_V_CHAN(7),
 	IIO_CHAN_SOFT_TIMESTAMP(8),
 };
 
@@ -121,7 +132,7 @@ static int ad7298_read_raw(struct iio_dev *indio_dev,
 	unsigned int scale_uv;
 
 	switch (m) {
-	case 0:
+	case IIO_CHAN_INFO_RAW:
 		mutex_lock(&indio_dev->mlock);
 		if (indio_dev->currentmode == INDIO_BUFFER_TRIGGERED) {
 			ret = -EBUSY;
@@ -168,7 +179,7 @@ static int __devinit ad7298_probe(struct spi_device *spi)
 	struct ad7298_platform_data *pdata = spi->dev.platform_data;
 	struct ad7298_state *st;
 	int ret;
-	struct iio_dev *indio_dev = iio_allocate_device(sizeof(*st));
+	struct iio_dev *indio_dev = iio_device_alloc(sizeof(*st));
 
 	if (indio_dev == NULL)
 		return -ENOMEM;
@@ -241,7 +252,7 @@ error_disable_reg:
 error_put_reg:
 	if (!IS_ERR(st->reg))
 		regulator_put(st->reg);
-	iio_free_device(indio_dev);
+	iio_device_free(indio_dev);
 
 	return ret;
 }
@@ -258,7 +269,7 @@ static int __devexit ad7298_remove(struct spi_device *spi)
 		regulator_disable(st->reg);
 		regulator_put(st->reg);
 	}
-	iio_free_device(indio_dev);
+	iio_device_free(indio_dev);
 
 	return 0;
 }
diff --git a/drivers/staging/iio/adc/ad7298_ring.c b/drivers/staging/iio/adc/ad7298_ring.c
index feeb0eeba59..908a3e5609d 100644
--- a/drivers/staging/iio/adc/ad7298_ring.c
+++ b/drivers/staging/iio/adc/ad7298_ring.c
@@ -1,7 +1,7 @@
 /*
  * AD7298 SPI ADC driver
  *
- * Copyright 2011 Analog Devices Inc.
+ * Copyright 2011-2012 Analog Devices Inc.
  *
  * Licensed under the GPL-2.
  */
@@ -11,10 +11,10 @@
 #include <linux/slab.h>
 #include <linux/spi/spi.h>
 
-#include "../iio.h"
-#include "../buffer.h"
-#include "../ring_sw.h"
-#include "../trigger_consumer.h"
+#include <linux/iio/iio.h>
+#include <linux/iio/buffer.h>
+#include <linux/iio/kfifo_buf.h>
+#include <linux/iio/trigger_consumer.h>
 
 #include "ad7298.h"
 
@@ -28,25 +28,17 @@
 static int ad7298_ring_preenable(struct iio_dev *indio_dev)
 {
 	struct ad7298_state *st = iio_priv(indio_dev);
-	struct iio_buffer *ring = indio_dev->buffer;
-	size_t d_size;
 	int i, m;
 	unsigned short command;
-	int scan_count = bitmap_weight(indio_dev->active_scan_mask,
-				       indio_dev->masklength);
-	d_size = scan_count * (AD7298_STORAGE_BITS / 8);
-
-	if (ring->scan_timestamp) {
-		d_size += sizeof(s64);
-
-		if (d_size % sizeof(s64))
-			d_size += sizeof(s64) - (d_size % sizeof(s64));
-	}
+	int scan_count, ret;
 
-	if (ring->access->set_bytes_per_datum)
-		ring->access->set_bytes_per_datum(ring, d_size);
+	ret = iio_sw_buffer_preenable(indio_dev);
+	if (ret < 0)
+		return ret;
 
-	st->d_size = d_size;
+	/* Now compute overall size */
+	scan_count = bitmap_weight(indio_dev->active_scan_mask,
+				   indio_dev->masklength);
 
 	command = AD7298_WRITE | st->ext_ref;
 
@@ -100,9 +92,9 @@ static irqreturn_t ad7298_trigger_handler(int irq, void *p)
 	if (b_sent)
 		return b_sent;
 
-	if (ring->scan_timestamp) {
+	if (indio_dev->scan_timestamp) {
 		time_ns = iio_get_time_ns();
-		memcpy((u8 *)buf + st->d_size - sizeof(s64),
+		memcpy((u8 *)buf + indio_dev->scan_bytes - sizeof(s64),
 			&time_ns, sizeof(time_ns));
 	}
 
@@ -126,7 +118,7 @@ int ad7298_register_ring_funcs_and_init(struct iio_dev *indio_dev)
 {
 	int ret;
 
-	indio_dev->buffer = iio_sw_rb_allocate(indio_dev);
+	indio_dev->buffer = iio_kfifo_allocate(indio_dev);
 	if (!indio_dev->buffer) {
 		ret = -ENOMEM;
 		goto error_ret;
@@ -140,7 +132,7 @@ int ad7298_register_ring_funcs_and_init(struct iio_dev *indio_dev)
 
 	if (indio_dev->pollfunc == NULL) {
 		ret = -ENOMEM;
-		goto error_deallocate_sw_rb;
+		goto error_deallocate_kfifo;
 	}
 
 	/* Ring buffer functions - here trigger setup related */
@@ -151,8 +143,8 @@ int ad7298_register_ring_funcs_and_init(struct iio_dev *indio_dev)
 	indio_dev->modes |= INDIO_BUFFER_TRIGGERED;
 	return 0;
 
-error_deallocate_sw_rb:
-	iio_sw_rb_free(indio_dev->buffer);
+error_deallocate_kfifo:
+	iio_kfifo_free(indio_dev->buffer);
 error_ret:
 	return ret;
 }
@@ -160,5 +152,5 @@ error_ret:
 void ad7298_ring_cleanup(struct iio_dev *indio_dev)
 {
 	iio_dealloc_pollfunc(indio_dev->pollfunc);
-	iio_sw_rb_free(indio_dev->buffer);
+	iio_kfifo_free(indio_dev->buffer);
 }
diff --git a/drivers/staging/iio/adc/ad7476.h b/drivers/staging/iio/adc/ad7476.h
index 27f696c75cc..b1dd9317fe1 100644
--- a/drivers/staging/iio/adc/ad7476.h
+++ b/drivers/staging/iio/adc/ad7476.h
@@ -27,7 +27,6 @@ struct ad7476_state {
 	struct spi_device		*spi;
 	const struct ad7476_chip_info	*chip_info;
 	struct regulator		*reg;
-	size_t				d_size;
 	u16				int_vref_mv;
 	struct spi_transfer		xfer;
 	struct spi_message		msg;
diff --git a/drivers/staging/iio/adc/ad7476_core.c b/drivers/staging/iio/adc/ad7476_core.c
index 0c064d1c392..be1c260cf16 100644
--- a/drivers/staging/iio/adc/ad7476_core.c
+++ b/drivers/staging/iio/adc/ad7476_core.c
@@ -15,9 +15,9 @@
 #include <linux/err.h>
 #include <linux/module.h>
 
-#include "../iio.h"
-#include "../sysfs.h"
-#include "../buffer.h"
+#include <linux/iio/iio.h>
+#include <linux/iio/sysfs.h>
+#include <linux/iio/buffer.h>
 
 #include "ad7476.h"
 
@@ -43,7 +43,7 @@ static int ad7476_read_raw(struct iio_dev *indio_dev,
 	unsigned int scale_uv;
 
 	switch (m) {
-	case 0:
+	case IIO_CHAN_INFO_RAW:
 		mutex_lock(&indio_dev->mlock);
 		if (iio_buffer_enabled(indio_dev))
 			ret = -EBUSY;
@@ -66,53 +66,51 @@ static int ad7476_read_raw(struct iio_dev *indio_dev,
 	return -EINVAL;
 }
 
+#define AD7476_CHAN(bits)					\
+	{							\
+	.type = IIO_VOLTAGE,					\
+	.indexed = 1,						\
+	.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT |		\
+	IIO_CHAN_INFO_SCALE_SHARED_BIT,				\
+	.scan_type = {						\
+		.sign = 'u',					\
+		.realbits = bits,				\
+		.storagebits = 16,				\
+		.shift = 12 - bits,				\
+	},							\
+}
+
 static const struct ad7476_chip_info ad7476_chip_info_tbl[] = {
 	[ID_AD7466] = {
-		.channel[0] = IIO_CHAN(IIO_VOLTAGE, 0, 1, 0, NULL, 0, 0,
-				       IIO_CHAN_INFO_SCALE_SHARED_BIT,
-				       0, 0, IIO_ST('u', 12, 16, 0), 0),
+		.channel[0] = AD7476_CHAN(12),
 		.channel[1] = IIO_CHAN_SOFT_TIMESTAMP(1),
 	},
 	[ID_AD7467] = {
-		.channel[0] = IIO_CHAN(IIO_VOLTAGE, 0, 1, 0, NULL, 0, 0,
-				       IIO_CHAN_INFO_SCALE_SHARED_BIT,
-				       0, 0, IIO_ST('u', 10, 16, 2), 0),
+		.channel[0] = AD7476_CHAN(10),
 		.channel[1] = IIO_CHAN_SOFT_TIMESTAMP(1),
 	},
 	[ID_AD7468] = {
-		.channel[0] = IIO_CHAN(IIO_VOLTAGE, 0, 1 , 0, NULL, 0, 0,
-				       IIO_CHAN_INFO_SCALE_SHARED_BIT,
-				       0, 0, IIO_ST('u', 8, 16, 4), 0),
+		.channel[0] = AD7476_CHAN(8),
 		.channel[1] = IIO_CHAN_SOFT_TIMESTAMP(1),
 	},
 	[ID_AD7475] = {
-		.channel[0] = IIO_CHAN(IIO_VOLTAGE, 0, 1, 0, NULL, 0, 0,
-				       IIO_CHAN_INFO_SCALE_SHARED_BIT,
-				       0, 0, IIO_ST('u', 12, 16, 0), 0),
+		.channel[0] = AD7476_CHAN(12),
 		.channel[1] = IIO_CHAN_SOFT_TIMESTAMP(1),
 	},
 	[ID_AD7476] = {
-		.channel[0] = IIO_CHAN(IIO_VOLTAGE, 0, 1, 0, NULL, 0, 0,
-				       IIO_CHAN_INFO_SCALE_SHARED_BIT,
-				       0, 0, IIO_ST('u', 12, 16, 0), 0),
+		.channel[0] = AD7476_CHAN(12),
 		.channel[1] = IIO_CHAN_SOFT_TIMESTAMP(1),
 	},
 	[ID_AD7477] = {
-		.channel[0] = IIO_CHAN(IIO_VOLTAGE, 0, 1, 0, NULL, 0, 0,
-				       IIO_CHAN_INFO_SCALE_SHARED_BIT,
-				       0, 0, IIO_ST('u', 10, 16, 2), 0),
+		.channel[0] = AD7476_CHAN(10),
 		.channel[1] = IIO_CHAN_SOFT_TIMESTAMP(1),
 	},
 	[ID_AD7478] = {
-		.channel[0] = IIO_CHAN(IIO_VOLTAGE, 0, 1, 0, NULL, 0, 0,
-				       IIO_CHAN_INFO_SCALE_SHARED_BIT,
-				       0, 0, IIO_ST('u', 8, 16, 4), 0),
+		.channel[0] = AD7476_CHAN(8),
 		.channel[1] = IIO_CHAN_SOFT_TIMESTAMP(1),
 	},
 	[ID_AD7495] = {
-		.channel[0] = IIO_CHAN(IIO_VOLTAGE, 0, 1, 0, NULL, 0, 0,
-				       IIO_CHAN_INFO_SCALE_SHARED_BIT,
-				       0, 0, IIO_ST('u', 12, 16, 0), 0),
+		.channel[0] = AD7476_CHAN(12),
 		.channel[1] = IIO_CHAN_SOFT_TIMESTAMP(1),
 		.int_vref_mv = 2500,
 	},
@@ -130,7 +128,7 @@ static int __devinit ad7476_probe(struct spi_device *spi)
 	struct iio_dev *indio_dev;
 	int ret, voltage_uv = 0;
 
-	indio_dev = iio_allocate_device(sizeof(*st));
+	indio_dev = iio_device_alloc(sizeof(*st));
 	if (indio_dev == NULL) {
 		ret = -ENOMEM;
 		goto error_ret;
@@ -200,7 +198,7 @@ error_disable_reg:
 error_put_reg:
 	if (!IS_ERR(st->reg))
 		regulator_put(st->reg);
-	iio_free_device(indio_dev);
+	iio_device_free(indio_dev);
 
 error_ret:
 	return ret;
@@ -218,7 +216,7 @@ static int ad7476_remove(struct spi_device *spi)
 		regulator_disable(st->reg);
 		regulator_put(st->reg);
 	}
-	iio_free_device(indio_dev);
+	iio_device_free(indio_dev);
 
 	return 0;
 }
diff --git a/drivers/staging/iio/adc/ad7476_ring.c b/drivers/staging/iio/adc/ad7476_ring.c
index d6af6c05ce1..383611b0576 100644
--- a/drivers/staging/iio/adc/ad7476_ring.c
+++ b/drivers/staging/iio/adc/ad7476_ring.c
@@ -1,5 +1,5 @@
 /*
- * Copyright 2010 Analog Devices Inc.
+ * Copyright 2010-2012 Analog Devices Inc.
  * Copyright (C) 2008 Jonathan Cameron
  *
  * Licensed under the GPL-2 or later.
@@ -13,43 +13,13 @@
 #include <linux/slab.h>
 #include <linux/spi/spi.h>
 
-#include "../iio.h"
-#include "../buffer.h"
-#include "../ring_sw.h"
-#include "../trigger_consumer.h"
+#include <linux/iio/iio.h>
+#include <linux/iio/buffer.h>
+#include <linux/iio/kfifo_buf.h>
+#include <linux/iio/trigger_consumer.h>
 
 #include "ad7476.h"
 
-/**
- * ad7476_ring_preenable() setup the parameters of the ring before enabling
- *
- * The complex nature of the setting of the number of bytes per datum is due
- * to this driver currently ensuring that the timestamp is stored at an 8
- * byte boundary.
- **/
-static int ad7476_ring_preenable(struct iio_dev *indio_dev)
-{
-	struct ad7476_state *st = iio_priv(indio_dev);
-	struct iio_buffer *ring = indio_dev->buffer;
-
-	st->d_size = bitmap_weight(indio_dev->active_scan_mask,
-				   indio_dev->masklength) *
-		st->chip_info->channel[0].scan_type.storagebits / 8;
-
-	if (ring->scan_timestamp) {
-		st->d_size += sizeof(s64);
-
-		if (st->d_size % sizeof(s64))
-			st->d_size += sizeof(s64) - (st->d_size % sizeof(s64));
-	}
-
-	if (indio_dev->buffer->access->set_bytes_per_datum)
-		indio_dev->buffer->access->
-			set_bytes_per_datum(indio_dev->buffer, st->d_size);
-
-	return 0;
-}
-
 static irqreturn_t ad7476_trigger_handler(int irq, void  *p)
 {
 	struct iio_poll_func *pf = p;
@@ -59,7 +29,7 @@ static irqreturn_t ad7476_trigger_handler(int irq, void  *p)
 	__u8 *rxbuf;
 	int b_sent;
 
-	rxbuf = kzalloc(st->d_size, GFP_KERNEL);
+	rxbuf = kzalloc(indio_dev->scan_bytes, GFP_KERNEL);
 	if (rxbuf == NULL)
 		return -ENOMEM;
 
@@ -70,8 +40,8 @@ static irqreturn_t ad7476_trigger_handler(int irq, void  *p)
 
 	time_ns = iio_get_time_ns();
 
-	if (indio_dev->buffer->scan_timestamp)
-		memcpy(rxbuf + st->d_size - sizeof(s64),
+	if (indio_dev->scan_timestamp)
+		memcpy(rxbuf + indio_dev->scan_bytes - sizeof(s64),
 			&time_ns, sizeof(time_ns));
 
 	indio_dev->buffer->access->store_to(indio_dev->buffer, rxbuf, time_ns);
@@ -83,7 +53,7 @@ done:
 }
 
 static const struct iio_buffer_setup_ops ad7476_ring_setup_ops = {
-	.preenable = &ad7476_ring_preenable,
+	.preenable = &iio_sw_buffer_preenable,
 	.postenable = &iio_triggered_buffer_postenable,
 	.predisable = &iio_triggered_buffer_predisable,
 };
@@ -93,7 +63,7 @@ int ad7476_register_ring_funcs_and_init(struct iio_dev *indio_dev)
 	struct ad7476_state *st = iio_priv(indio_dev);
 	int ret = 0;
 
-	indio_dev->buffer = iio_sw_rb_allocate(indio_dev);
+	indio_dev->buffer = iio_kfifo_allocate(indio_dev);
 	if (!indio_dev->buffer) {
 		ret = -ENOMEM;
 		goto error_ret;
@@ -108,7 +78,7 @@ int ad7476_register_ring_funcs_and_init(struct iio_dev *indio_dev)
 				     indio_dev->id);
 	if (indio_dev->pollfunc == NULL) {
 		ret = -ENOMEM;
-		goto error_deallocate_sw_rb;
+		goto error_deallocate_kfifo;
 	}
 
 	/* Ring buffer functions - here trigger setup related */
@@ -119,8 +89,8 @@ int ad7476_register_ring_funcs_and_init(struct iio_dev *indio_dev)
 	indio_dev->modes |= INDIO_BUFFER_TRIGGERED;
 	return 0;
 
-error_deallocate_sw_rb:
-	iio_sw_rb_free(indio_dev->buffer);
+error_deallocate_kfifo:
+	iio_kfifo_free(indio_dev->buffer);
 error_ret:
 	return ret;
 }
@@ -128,5 +98,5 @@ error_ret:
 void ad7476_ring_cleanup(struct iio_dev *indio_dev)
 {
 	iio_dealloc_pollfunc(indio_dev->pollfunc);
-	iio_sw_rb_free(indio_dev->buffer);
+	iio_kfifo_free(indio_dev->buffer);
 }
diff --git a/drivers/staging/iio/adc/ad7606_core.c b/drivers/staging/iio/adc/ad7606_core.c
index 97e8d3d4471..10ab6dc823b 100644
--- a/drivers/staging/iio/adc/ad7606_core.c
+++ b/drivers/staging/iio/adc/ad7606_core.c
@@ -18,9 +18,9 @@
 #include <linux/sched.h>
 #include <linux/module.h>
 
-#include "../iio.h"
-#include "../sysfs.h"
-#include "../buffer.h"
+#include <linux/iio/iio.h>
+#include <linux/iio/sysfs.h>
+#include <linux/iio/buffer.h>
 
 #include "ad7606.h"
 
@@ -88,7 +88,7 @@ static int ad7606_read_raw(struct iio_dev *indio_dev,
 	unsigned int scale_uv;
 
 	switch (m) {
-	case 0:
+	case IIO_CHAN_INFO_RAW:
 		mutex_lock(&indio_dev->mlock);
 		if (iio_buffer_enabled(indio_dev))
 			ret = -EBUSY;
@@ -113,7 +113,7 @@ static int ad7606_read_raw(struct iio_dev *indio_dev,
 static ssize_t ad7606_show_range(struct device *dev,
 			struct device_attribute *attr, char *buf)
 {
-	struct iio_dev *indio_dev = dev_get_drvdata(dev);
+	struct iio_dev *indio_dev = dev_to_iio_dev(dev);
 	struct ad7606_state *st = iio_priv(indio_dev);
 
 	return sprintf(buf, "%u\n", st->range);
@@ -122,7 +122,7 @@ static ssize_t ad7606_show_range(struct device *dev,
 static ssize_t ad7606_store_range(struct device *dev,
 		struct device_attribute *attr, const char *buf, size_t count)
 {
-	struct iio_dev *indio_dev = dev_get_drvdata(dev);
+	struct iio_dev *indio_dev = dev_to_iio_dev(dev);
 	struct ad7606_state *st = iio_priv(indio_dev);
 	unsigned long lval;
 
@@ -147,7 +147,7 @@ static IIO_CONST_ATTR(in_voltage_range_available, "5000 10000");
 static ssize_t ad7606_show_oversampling_ratio(struct device *dev,
 			struct device_attribute *attr, char *buf)
 {
-	struct iio_dev *indio_dev = dev_get_drvdata(dev);
+	struct iio_dev *indio_dev = dev_to_iio_dev(dev);
 	struct ad7606_state *st = iio_priv(indio_dev);
 
 	return sprintf(buf, "%u\n", st->oversampling);
@@ -168,7 +168,7 @@ static int ad7606_oversampling_get_index(unsigned val)
 static ssize_t ad7606_store_oversampling_ratio(struct device *dev,
 		struct device_attribute *attr, const char *buf, size_t count)
 {
-	struct iio_dev *indio_dev = dev_get_drvdata(dev);
+	struct iio_dev *indio_dev = dev_to_iio_dev(dev);
 	struct ad7606_state *st = iio_priv(indio_dev);
 	unsigned long lval;
 	int ret;
@@ -229,14 +229,15 @@ static const struct attribute_group ad7606_attribute_group_range = {
 	.attrs = ad7606_attributes_range,
 };
 
-#define AD7606_CHANNEL(num)				\
-	{						\
-		.type = IIO_VOLTAGE,			\
-		.indexed = 1,				\
-		.channel = num,				\
-		.address = num,				\
-		.scan_index = num,			\
-		.scan_type = IIO_ST('s', 16, 16, 0),	\
+#define AD7606_CHANNEL(num)					\
+	{							\
+		.type = IIO_VOLTAGE,				\
+		.indexed = 1,					\
+		.channel = num,					\
+		.address = num,					\
+		.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT,	\
+		.scan_index = num,				\
+		.scan_type = IIO_ST('s', 16, 16, 0),		\
 	}
 
 static struct iio_chan_spec ad7606_8_channels[] = {
@@ -460,7 +461,7 @@ struct iio_dev *ad7606_probe(struct device *dev, int irq,
 	struct ad7606_platform_data *pdata = dev->platform_data;
 	struct ad7606_state *st;
 	int ret;
-	struct iio_dev *indio_dev = iio_allocate_device(sizeof(*st));
+	struct iio_dev *indio_dev = iio_device_alloc(sizeof(*st));
 
 	if (indio_dev == NULL) {
 		ret = -ENOMEM;
@@ -559,7 +560,7 @@ error_disable_reg:
 error_put_reg:
 	if (!IS_ERR(st->reg))
 		regulator_put(st->reg);
-	iio_free_device(indio_dev);
+	iio_device_free(indio_dev);
 error_ret:
 	return ERR_PTR(ret);
 }
@@ -579,7 +580,7 @@ int ad7606_remove(struct iio_dev *indio_dev, int irq)
 	}
 
 	ad7606_free_gpios(st);
-	iio_free_device(indio_dev);
+	iio_device_free(indio_dev);
 
 	return 0;
 }
diff --git a/drivers/staging/iio/adc/ad7606_par.c b/drivers/staging/iio/adc/ad7606_par.c
index bb152a8e8c9..a53faafec07 100644
--- a/drivers/staging/iio/adc/ad7606_par.c
+++ b/drivers/staging/iio/adc/ad7606_par.c
@@ -12,7 +12,7 @@
 #include <linux/err.h>
 #include <linux/io.h>
 
-#include "../iio.h"
+#include <linux/iio/iio.h>
 #include "ad7606.h"
 
 static int ad7606_par16_read_block(struct device *dev,
diff --git a/drivers/staging/iio/adc/ad7606_ring.c b/drivers/staging/iio/adc/ad7606_ring.c
index 1ef9fbcaf2d..24ce8fc7164 100644
--- a/drivers/staging/iio/adc/ad7606_ring.c
+++ b/drivers/staging/iio/adc/ad7606_ring.c
@@ -1,5 +1,5 @@
 /*
- * Copyright 2011 Analog Devices Inc.
+ * Copyright 2011-2012 Analog Devices Inc.
  *
  * Licensed under the GPL-2.
  *
@@ -11,10 +11,10 @@
 #include <linux/kernel.h>
 #include <linux/slab.h>
 
-#include "../iio.h"
-#include "../buffer.h"
-#include "../ring_sw.h"
-#include "../trigger_consumer.h"
+#include <linux/iio/iio.h>
+#include <linux/iio/buffer.h>
+#include <linux/iio/kfifo_buf.h>
+#include <linux/iio/trigger_consumer.h>
 
 #include "ad7606.h"
 
@@ -51,8 +51,7 @@ static void ad7606_poll_bh_to_ring(struct work_struct *work_s)
 	__u8 *buf;
 	int ret;
 
-	buf = kzalloc(ring->access->get_bytes_per_datum(ring),
-		      GFP_KERNEL);
+	buf = kzalloc(indio_dev->scan_bytes, GFP_KERNEL);
 	if (buf == NULL)
 		return;
 
@@ -82,9 +81,8 @@ static void ad7606_poll_bh_to_ring(struct work_struct *work_s)
 
 	time_ns = iio_get_time_ns();
 
-	if (ring->scan_timestamp)
-		*((s64 *)(buf + ring->access->get_bytes_per_datum(ring) -
-			  sizeof(s64))) = time_ns;
+	if (indio_dev->scan_timestamp)
+		*((s64 *)(buf + indio_dev->scan_bytes - sizeof(s64))) = time_ns;
 
 	ring->access->store_to(indio_dev->buffer, buf, time_ns);
 done:
@@ -104,7 +102,7 @@ int ad7606_register_ring_funcs_and_init(struct iio_dev *indio_dev)
 	struct ad7606_state *st = iio_priv(indio_dev);
 	int ret;
 
-	indio_dev->buffer = iio_sw_rb_allocate(indio_dev);
+	indio_dev->buffer = iio_kfifo_allocate(indio_dev);
 	if (!indio_dev->buffer) {
 		ret = -ENOMEM;
 		goto error_ret;
@@ -119,13 +117,13 @@ int ad7606_register_ring_funcs_and_init(struct iio_dev *indio_dev)
 						 indio_dev->id);
 	if (indio_dev->pollfunc == NULL) {
 		ret = -ENOMEM;
-		goto error_deallocate_sw_rb;
+		goto error_deallocate_kfifo;
 	}
 
 	/* Ring buffer functions - here trigger setup related */
 
 	indio_dev->setup_ops = &ad7606_ring_setup_ops;
-	indio_dev->buffer->scan_timestamp = true ;
+	indio_dev->buffer->scan_timestamp = true;
 
 	INIT_WORK(&st->poll_work, &ad7606_poll_bh_to_ring);
 
@@ -133,8 +131,8 @@ int ad7606_register_ring_funcs_and_init(struct iio_dev *indio_dev)
 	indio_dev->modes |= INDIO_BUFFER_TRIGGERED;
 	return 0;
 
-error_deallocate_sw_rb:
-	iio_sw_rb_free(indio_dev->buffer);
+error_deallocate_kfifo:
+	iio_kfifo_free(indio_dev->buffer);
 error_ret:
 	return ret;
 }
@@ -142,5 +140,5 @@ error_ret:
 void ad7606_ring_cleanup(struct iio_dev *indio_dev)
 {
 	iio_dealloc_pollfunc(indio_dev->pollfunc);
-	iio_sw_rb_free(indio_dev->buffer);
+	iio_kfifo_free(indio_dev->buffer);
 }
diff --git a/drivers/staging/iio/adc/ad7606_spi.c b/drivers/staging/iio/adc/ad7606_spi.c
index 237f1c44d29..099d347da52 100644
--- a/drivers/staging/iio/adc/ad7606_spi.c
+++ b/drivers/staging/iio/adc/ad7606_spi.c
@@ -11,7 +11,7 @@
 #include <linux/types.h>
 #include <linux/err.h>
 
-#include "../iio.h"
+#include <linux/iio/iio.h>
 #include "ad7606.h"
 
 #define MAX_SPI_FREQ_HZ		23500000	/* VDRIVE above 4.75 V */
diff --git a/drivers/staging/iio/adc/ad7780.c b/drivers/staging/iio/adc/ad7780.c
index a13e58c814e..1ece2ac8de5 100644
--- a/drivers/staging/iio/adc/ad7780.c
+++ b/drivers/staging/iio/adc/ad7780.c
@@ -18,8 +18,8 @@
 #include <linux/gpio.h>
 #include <linux/module.h>
 
-#include "../iio.h"
-#include "../sysfs.h"
+#include <linux/iio/iio.h>
+#include <linux/iio/sysfs.h>
 
 #include "ad7780.h"
 
@@ -94,7 +94,7 @@ static int ad7780_read_raw(struct iio_dev *indio_dev,
 	unsigned long scale_uv;
 
 	switch (m) {
-	case 0:
+	case IIO_CHAN_INFO_RAW:
 		mutex_lock(&indio_dev->mlock);
 		ret = ad7780_read(st, &smpl);
 		mutex_unlock(&indio_dev->mlock);
@@ -126,14 +126,34 @@ static int ad7780_read_raw(struct iio_dev *indio_dev,
 
 static const struct ad7780_chip_info ad7780_chip_info_tbl[] = {
 	[ID_AD7780] = {
-		.channel = IIO_CHAN(IIO_VOLTAGE, 0, 1, 0, NULL, 0, 0,
-				    IIO_CHAN_INFO_SCALE_SHARED_BIT,
-				    0, 0, IIO_ST('s', 24, 32, 8), 0),
+		.channel = {
+			.type = IIO_VOLTAGE,
+			.indexed = 1,
+			.channel = 0,
+			.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT |
+			IIO_CHAN_INFO_SCALE_SHARED_BIT,
+			.scan_type = {
+				.sign = 's',
+				.realbits = 24,
+				.storagebits = 32,
+				.shift = 8,
+			},
+		},
 	},
 	[ID_AD7781] = {
-		.channel = IIO_CHAN(IIO_VOLTAGE, 0, 1, 0, NULL, 0, 0,
-				    IIO_CHAN_INFO_SCALE_SHARED_BIT,
-				    0, 0, IIO_ST('s', 20, 32, 12), 0),
+		.channel = {
+			.type = IIO_VOLTAGE,
+			.indexed = 1,
+			.channel = 0,
+			.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT |
+			IIO_CHAN_INFO_SCALE_SHARED_BIT,
+			.scan_type = {
+				.sign = 's',
+				.realbits = 20,
+				.storagebits = 32,
+				.shift = 12,
+			},
+		},
 	},
 };
 
@@ -167,7 +187,7 @@ static int __devinit ad7780_probe(struct spi_device *spi)
 		return -ENODEV;
 	}
 
-	indio_dev = iio_allocate_device(sizeof(*st));
+	indio_dev = iio_device_alloc(sizeof(*st));
 	if (indio_dev == NULL)
 		return -ENOMEM;
 
@@ -245,7 +265,7 @@ error_put_reg:
 	if (!IS_ERR(st->reg))
 		regulator_put(st->reg);
 
-	iio_free_device(indio_dev);
+	iio_device_free(indio_dev);
 
 	return ret;
 }
@@ -262,7 +282,7 @@ static int ad7780_remove(struct spi_device *spi)
 		regulator_disable(st->reg);
 		regulator_put(st->reg);
 	}
-	iio_free_device(indio_dev);
+	iio_device_free(indio_dev);
 
 	return 0;
 }
diff --git a/drivers/staging/iio/adc/ad7793.c b/drivers/staging/iio/adc/ad7793.c
index 84ecde1ad04..b36556fa295 100644
--- a/drivers/staging/iio/adc/ad7793.c
+++ b/drivers/staging/iio/adc/ad7793.c
@@ -1,7 +1,7 @@
 /*
  * AD7792/AD7793 SPI ADC driver
  *
- * Copyright 2011 Analog Devices Inc.
+ * Copyright 2011-2012 Analog Devices Inc.
  *
  * Licensed under the GPL-2.
  */
@@ -18,12 +18,12 @@
 #include <linux/delay.h>
 #include <linux/module.h>
 
-#include "../iio.h"
-#include "../sysfs.h"
-#include "../buffer.h"
-#include "../ring_sw.h"
-#include "../trigger.h"
-#include "../trigger_consumer.h"
+#include <linux/iio/iio.h>
+#include <linux/iio/sysfs.h>
+#include <linux/iio/buffer.h>
+#include <linux/iio/kfifo_buf.h>
+#include <linux/iio/trigger.h>
+#include <linux/iio/trigger_consumer.h>
 
 #include "ad7793.h"
 
@@ -319,31 +319,18 @@ out:
 static int ad7793_ring_preenable(struct iio_dev *indio_dev)
 {
 	struct ad7793_state *st = iio_priv(indio_dev);
-	struct iio_buffer *ring = indio_dev->buffer;
-	size_t d_size;
 	unsigned channel;
+	int ret;
 
 	if (bitmap_empty(indio_dev->active_scan_mask, indio_dev->masklength))
 		return -EINVAL;
+	ret = iio_sw_buffer_preenable(indio_dev);
+	if (ret < 0)
+		return ret;
 
 	channel = find_first_bit(indio_dev->active_scan_mask,
 				 indio_dev->masklength);
 
-	d_size = bitmap_weight(indio_dev->active_scan_mask,
-			       indio_dev->masklength) *
-		indio_dev->channels[0].scan_type.storagebits / 8;
-
-	if (ring->scan_timestamp) {
-		d_size += sizeof(s64);
-
-		if (d_size % sizeof(s64))
-			d_size += sizeof(s64) - (d_size % sizeof(s64));
-	}
-
-	if (indio_dev->buffer->access->set_bytes_per_datum)
-		indio_dev->buffer->access->
-			set_bytes_per_datum(indio_dev->buffer, d_size);
-
 	st->mode  = (st->mode & ~AD7793_MODE_SEL(-1)) |
 		    AD7793_MODE_SEL(AD7793_MODE_CONT);
 	st->conf  = (st->conf & ~AD7793_CONF_CHAN(-1)) |
@@ -399,7 +386,7 @@ static irqreturn_t ad7793_trigger_handler(int irq, void *p)
 				  indio_dev->channels[0].scan_type.realbits/8);
 
 	/* Guaranteed to be aligned with 8 byte boundary */
-	if (ring->scan_timestamp)
+	if (indio_dev->scan_timestamp)
 		dat64[1] = pf->timestamp;
 
 	ring->access->store_to(ring, (u8 *)dat64, pf->timestamp);
@@ -422,7 +409,7 @@ static int ad7793_register_ring_funcs_and_init(struct iio_dev *indio_dev)
 {
 	int ret;
 
-	indio_dev->buffer = iio_sw_rb_allocate(indio_dev);
+	indio_dev->buffer = iio_kfifo_allocate(indio_dev);
 	if (!indio_dev->buffer) {
 		ret = -ENOMEM;
 		goto error_ret;
@@ -435,7 +422,7 @@ static int ad7793_register_ring_funcs_and_init(struct iio_dev *indio_dev)
 						 indio_dev->id);
 	if (indio_dev->pollfunc == NULL) {
 		ret = -ENOMEM;
-		goto error_deallocate_sw_rb;
+		goto error_deallocate_kfifo;
 	}
 
 	/* Ring buffer functions - here trigger setup related */
@@ -445,8 +432,8 @@ static int ad7793_register_ring_funcs_and_init(struct iio_dev *indio_dev)
 	indio_dev->modes |= INDIO_BUFFER_TRIGGERED;
 	return 0;
 
-error_deallocate_sw_rb:
-	iio_sw_rb_free(indio_dev->buffer);
+error_deallocate_kfifo:
+	iio_kfifo_free(indio_dev->buffer);
 error_ret:
 	return ret;
 }
@@ -454,7 +441,7 @@ error_ret:
 static void ad7793_ring_cleanup(struct iio_dev *indio_dev)
 {
 	iio_dealloc_pollfunc(indio_dev->pollfunc);
-	iio_sw_rb_free(indio_dev->buffer);
+	iio_kfifo_free(indio_dev->buffer);
 }
 
 /**
@@ -482,7 +469,7 @@ static int ad7793_probe_trigger(struct iio_dev *indio_dev)
 	struct ad7793_state *st = iio_priv(indio_dev);
 	int ret;
 
-	st->trig = iio_allocate_trigger("%s-dev%d",
+	st->trig = iio_trigger_alloc("%s-dev%d",
 					spi_get_device_id(st->spi)->name,
 					indio_dev->id);
 	if (st->trig == NULL) {
@@ -516,7 +503,7 @@ static int ad7793_probe_trigger(struct iio_dev *indio_dev)
 error_free_irq:
 	free_irq(st->spi->irq, indio_dev);
 error_free_trig:
-	iio_free_trigger(st->trig);
+	iio_trigger_free(st->trig);
 error_ret:
 	return ret;
 }
@@ -527,7 +514,7 @@ static void ad7793_remove_trigger(struct iio_dev *indio_dev)
 
 	iio_trigger_unregister(st->trig);
 	free_irq(st->spi->irq, indio_dev);
-	iio_free_trigger(st->trig);
+	iio_trigger_free(st->trig);
 }
 
 static const u16 sample_freq_avail[16] = {0, 470, 242, 123, 62, 50, 39, 33, 19,
@@ -537,7 +524,7 @@ static ssize_t ad7793_read_frequency(struct device *dev,
 		struct device_attribute *attr,
 		char *buf)
 {
-	struct iio_dev *indio_dev = dev_get_drvdata(dev);
+	struct iio_dev *indio_dev = dev_to_iio_dev(dev);
 	struct ad7793_state *st = iio_priv(indio_dev);
 
 	return sprintf(buf, "%d\n",
@@ -549,7 +536,7 @@ static ssize_t ad7793_write_frequency(struct device *dev,
 		const char *buf,
 		size_t len)
 {
-	struct iio_dev *indio_dev = dev_get_drvdata(dev);
+	struct iio_dev *indio_dev = dev_to_iio_dev(dev);
 	struct ad7793_state *st = iio_priv(indio_dev);
 	long lval;
 	int i, ret;
@@ -591,7 +578,7 @@ static IIO_CONST_ATTR_SAMP_FREQ_AVAIL(
 static ssize_t ad7793_show_scale_available(struct device *dev,
 			struct device_attribute *attr, char *buf)
 {
-	struct iio_dev *indio_dev = dev_get_drvdata(dev);
+	struct iio_dev *indio_dev = dev_to_iio_dev(dev);
 	struct ad7793_state *st = iio_priv(indio_dev);
 	int i, len = 0;
 
@@ -630,7 +617,7 @@ static int ad7793_read_raw(struct iio_dev *indio_dev,
 	bool unipolar = !!(st->conf & AD7793_CONF_UNIPOLAR);
 
 	switch (m) {
-	case 0:
+	case IIO_CHAN_INFO_RAW:
 		mutex_lock(&indio_dev->mlock);
 		if (iio_buffer_enabled(indio_dev))
 			ret = -EBUSY;
@@ -760,7 +747,8 @@ static const struct ad7793_chip_info ad7793_chip_info_tbl[] = {
 			.channel = 0,
 			.channel2 = 0,
 			.address = AD7793_CH_AIN1P_AIN1M,
-			.info_mask = IIO_CHAN_INFO_SCALE_SHARED_BIT,
+			.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT |
+			IIO_CHAN_INFO_SCALE_SHARED_BIT,
 			.scan_index = 0,
 			.scan_type = IIO_ST('s', 24, 32, 0)
 		},
@@ -771,7 +759,8 @@ static const struct ad7793_chip_info ad7793_chip_info_tbl[] = {
 			.channel = 1,
 			.channel2 = 1,
 			.address = AD7793_CH_AIN2P_AIN2M,
-			.info_mask = IIO_CHAN_INFO_SCALE_SHARED_BIT,
+			.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT |
+			IIO_CHAN_INFO_SCALE_SHARED_BIT,
 			.scan_index = 1,
 			.scan_type = IIO_ST('s', 24, 32, 0)
 		},
@@ -782,7 +771,8 @@ static const struct ad7793_chip_info ad7793_chip_info_tbl[] = {
 			.channel = 2,
 			.channel2 = 2,
 			.address = AD7793_CH_AIN3P_AIN3M,
-			.info_mask = IIO_CHAN_INFO_SCALE_SHARED_BIT,
+			.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT |
+			IIO_CHAN_INFO_SCALE_SHARED_BIT,
 			.scan_index = 2,
 			.scan_type = IIO_ST('s', 24, 32, 0)
 		},
@@ -794,7 +784,8 @@ static const struct ad7793_chip_info ad7793_chip_info_tbl[] = {
 			.channel = 2,
 			.channel2 = 2,
 			.address = AD7793_CH_AIN1M_AIN1M,
-			.info_mask = IIO_CHAN_INFO_SCALE_SHARED_BIT,
+			.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT |
+			IIO_CHAN_INFO_SCALE_SHARED_BIT,
 			.scan_index = 2,
 			.scan_type = IIO_ST('s', 24, 32, 0)
 		},
@@ -803,7 +794,8 @@ static const struct ad7793_chip_info ad7793_chip_info_tbl[] = {
 			.indexed = 1,
 			.channel = 0,
 			.address = AD7793_CH_TEMP,
-			.info_mask = IIO_CHAN_INFO_SCALE_SEPARATE_BIT,
+			.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT |
+			IIO_CHAN_INFO_SCALE_SEPARATE_BIT,
 			.scan_index = 4,
 			.scan_type = IIO_ST('s', 24, 32, 0),
 		},
@@ -813,7 +805,8 @@ static const struct ad7793_chip_info ad7793_chip_info_tbl[] = {
 			.indexed = 1,
 			.channel = 4,
 			.address = AD7793_CH_AVDD_MONITOR,
-			.info_mask = IIO_CHAN_INFO_SCALE_SEPARATE_BIT,
+			.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT |
+			IIO_CHAN_INFO_SCALE_SEPARATE_BIT,
 			.scan_index = 5,
 			.scan_type = IIO_ST('s', 24, 32, 0),
 		},
@@ -827,7 +820,8 @@ static const struct ad7793_chip_info ad7793_chip_info_tbl[] = {
 			.channel = 0,
 			.channel2 = 0,
 			.address = AD7793_CH_AIN1P_AIN1M,
-			.info_mask = IIO_CHAN_INFO_SCALE_SHARED_BIT,
+			.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT |
+			IIO_CHAN_INFO_SCALE_SHARED_BIT,
 			.scan_index = 0,
 			.scan_type = IIO_ST('s', 16, 32, 0)
 		},
@@ -838,7 +832,8 @@ static const struct ad7793_chip_info ad7793_chip_info_tbl[] = {
 			.channel = 1,
 			.channel2 = 1,
 			.address = AD7793_CH_AIN2P_AIN2M,
-			.info_mask = IIO_CHAN_INFO_SCALE_SHARED_BIT,
+			.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT |
+			IIO_CHAN_INFO_SCALE_SHARED_BIT,
 			.scan_index = 1,
 			.scan_type = IIO_ST('s', 16, 32, 0)
 		},
@@ -849,7 +844,8 @@ static const struct ad7793_chip_info ad7793_chip_info_tbl[] = {
 			.channel = 2,
 			.channel2 = 2,
 			.address = AD7793_CH_AIN3P_AIN3M,
-			.info_mask = IIO_CHAN_INFO_SCALE_SHARED_BIT,
+			.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT |
+			IIO_CHAN_INFO_SCALE_SHARED_BIT,
 			.scan_index = 2,
 			.scan_type = IIO_ST('s', 16, 32, 0)
 		},
@@ -861,7 +857,8 @@ static const struct ad7793_chip_info ad7793_chip_info_tbl[] = {
 			.channel = 2,
 			.channel2 = 2,
 			.address = AD7793_CH_AIN1M_AIN1M,
-			.info_mask = IIO_CHAN_INFO_SCALE_SHARED_BIT,
+			.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT |
+			IIO_CHAN_INFO_SCALE_SHARED_BIT,
 			.scan_index = 2,
 			.scan_type = IIO_ST('s', 16, 32, 0)
 		},
@@ -870,7 +867,8 @@ static const struct ad7793_chip_info ad7793_chip_info_tbl[] = {
 			.indexed = 1,
 			.channel = 0,
 			.address = AD7793_CH_TEMP,
-			.info_mask = IIO_CHAN_INFO_SCALE_SEPARATE_BIT,
+			.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT |
+			IIO_CHAN_INFO_SCALE_SEPARATE_BIT,
 			.scan_index = 4,
 			.scan_type = IIO_ST('s', 16, 32, 0),
 		},
@@ -880,7 +878,8 @@ static const struct ad7793_chip_info ad7793_chip_info_tbl[] = {
 			.indexed = 1,
 			.channel = 4,
 			.address = AD7793_CH_AVDD_MONITOR,
-			.info_mask = IIO_CHAN_INFO_SCALE_SEPARATE_BIT,
+			.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT |
+			IIO_CHAN_INFO_SCALE_SEPARATE_BIT,
 			.scan_index = 5,
 			.scan_type = IIO_ST('s', 16, 32, 0),
 		},
@@ -905,7 +904,7 @@ static int __devinit ad7793_probe(struct spi_device *spi)
 		return -ENODEV;
 	}
 
-	indio_dev = iio_allocate_device(sizeof(*st));
+	indio_dev = iio_device_alloc(sizeof(*st));
 	if (indio_dev == NULL)
 		return -ENOMEM;
 
@@ -989,7 +988,7 @@ error_put_reg:
 	if (!IS_ERR(st->reg))
 		regulator_put(st->reg);
 
-	iio_free_device(indio_dev);
+	iio_device_free(indio_dev);
 
 	return ret;
 }
@@ -1009,7 +1008,7 @@ static int ad7793_remove(struct spi_device *spi)
 		regulator_put(st->reg);
 	}
 
-	iio_free_device(indio_dev);
+	iio_device_free(indio_dev);
 
 	return 0;
 }
diff --git a/drivers/staging/iio/adc/ad7816.c b/drivers/staging/iio/adc/ad7816.c
index 52b720e2b03..5356b091b08 100644
--- a/drivers/staging/iio/adc/ad7816.c
+++ b/drivers/staging/iio/adc/ad7816.c
@@ -16,9 +16,9 @@
 #include <linux/spi/spi.h>
 #include <linux/module.h>
 
-#include "../iio.h"
-#include "../sysfs.h"
-#include "../events.h"
+#include <linux/iio/iio.h>
+#include <linux/iio/sysfs.h>
+#include <linux/iio/events.h>
 
 /*
  * AD7816 config masks
@@ -113,7 +113,7 @@ static ssize_t ad7816_show_mode(struct device *dev,
 		struct device_attribute *attr,
 		char *buf)
 {
-	struct iio_dev *indio_dev = dev_get_drvdata(dev);
+	struct iio_dev *indio_dev = dev_to_iio_dev(dev);
 	struct ad7816_chip_info *chip = iio_priv(indio_dev);
 
 	if (chip->mode)
@@ -127,7 +127,7 @@ static ssize_t ad7816_store_mode(struct device *dev,
 		const char *buf,
 		size_t len)
 {
-	struct iio_dev *indio_dev = dev_get_drvdata(dev);
+	struct iio_dev *indio_dev = dev_to_iio_dev(dev);
 	struct ad7816_chip_info *chip = iio_priv(indio_dev);
 
 	if (strcmp(buf, "full")) {
@@ -159,7 +159,7 @@ static ssize_t ad7816_show_channel(struct device *dev,
 		struct device_attribute *attr,
 		char *buf)
 {
-	struct iio_dev *indio_dev = dev_get_drvdata(dev);
+	struct iio_dev *indio_dev = dev_to_iio_dev(dev);
 	struct ad7816_chip_info *chip = iio_priv(indio_dev);
 
 	return sprintf(buf, "%d\n", chip->channel_id);
@@ -170,7 +170,7 @@ static ssize_t ad7816_store_channel(struct device *dev,
 		const char *buf,
 		size_t len)
 {
-	struct iio_dev *indio_dev = dev_get_drvdata(dev);
+	struct iio_dev *indio_dev = dev_to_iio_dev(dev);
 	struct ad7816_chip_info *chip = iio_priv(indio_dev);
 	unsigned long data;
 	int ret;
@@ -208,7 +208,7 @@ static ssize_t ad7816_show_value(struct device *dev,
 		struct device_attribute *attr,
 		char *buf)
 {
-	struct iio_dev *indio_dev = dev_get_drvdata(dev);
+	struct iio_dev *indio_dev = dev_to_iio_dev(dev);
 	struct ad7816_chip_info *chip = iio_priv(indio_dev);
 	u16 data;
 	s8 value;
@@ -263,7 +263,7 @@ static ssize_t ad7816_show_oti(struct device *dev,
 		struct device_attribute *attr,
 		char *buf)
 {
-	struct iio_dev *indio_dev = dev_get_drvdata(dev);
+	struct iio_dev *indio_dev = dev_to_iio_dev(dev);
 	struct ad7816_chip_info *chip = iio_priv(indio_dev);
 	int value;
 
@@ -284,7 +284,7 @@ static inline ssize_t ad7816_set_oti(struct device *dev,
 		const char *buf,
 		size_t len)
 {
-	struct iio_dev *indio_dev = dev_get_drvdata(dev);
+	struct iio_dev *indio_dev = dev_to_iio_dev(dev);
 	struct ad7816_chip_info *chip = iio_priv(indio_dev);
 	long value;
 	u8 data;
@@ -354,7 +354,7 @@ static int __devinit ad7816_probe(struct spi_device *spi_dev)
 		return -EINVAL;
 	}
 
-	indio_dev = iio_allocate_device(sizeof(*chip));
+	indio_dev = iio_device_alloc(sizeof(*chip));
 	if (indio_dev == NULL) {
 		ret = -ENOMEM;
 		goto error_ret;
@@ -426,7 +426,7 @@ error_free_gpio_convert:
 error_free_gpio_rdwr:
 	gpio_free(chip->rdwr_pin);
 error_free_device:
-	iio_free_device(indio_dev);
+	iio_device_free(indio_dev);
 error_ret:
 	return ret;
 }
@@ -443,7 +443,7 @@ static int __devexit ad7816_remove(struct spi_device *spi_dev)
 	gpio_free(chip->busy_pin);
 	gpio_free(chip->convert_pin);
 	gpio_free(chip->rdwr_pin);
-	iio_free_device(indio_dev);
+	iio_device_free(indio_dev);
 
 	return 0;
 }
diff --git a/drivers/staging/iio/adc/ad7887.h b/drivers/staging/iio/adc/ad7887.h
index bc53b653212..2e09e54fc9c 100644
--- a/drivers/staging/iio/adc/ad7887.h
+++ b/drivers/staging/iio/adc/ad7887.h
@@ -63,7 +63,6 @@ struct ad7887_state {
 	struct spi_device		*spi;
 	const struct ad7887_chip_info	*chip_info;
 	struct regulator		*reg;
-	size_t				d_size;
 	u16				int_vref_mv;
 	struct spi_transfer		xfer[4];
 	struct spi_message		msg[3];
diff --git a/drivers/staging/iio/adc/ad7887_core.c b/drivers/staging/iio/adc/ad7887_core.c
index e9bbc3eed15..7186074deeb 100644
--- a/drivers/staging/iio/adc/ad7887_core.c
+++ b/drivers/staging/iio/adc/ad7887_core.c
@@ -15,9 +15,9 @@
 #include <linux/err.h>
 #include <linux/module.h>
 
-#include "../iio.h"
-#include "../sysfs.h"
-#include "../buffer.h"
+#include <linux/iio/iio.h>
+#include <linux/iio/sysfs.h>
+#include <linux/iio/buffer.h>
 
 
 #include "ad7887.h"
@@ -42,7 +42,7 @@ static int ad7887_read_raw(struct iio_dev *indio_dev,
 	unsigned int scale_uv;
 
 	switch (m) {
-	case 0:
+	case IIO_CHAN_INFO_RAW:
 		mutex_lock(&indio_dev->mlock);
 		if (iio_buffer_enabled(indio_dev))
 			ret = -EBUSY;
@@ -75,7 +75,8 @@ static const struct ad7887_chip_info ad7887_chip_info_tbl[] = {
 			.type = IIO_VOLTAGE,
 			.indexed = 1,
 			.channel = 1,
-			.info_mask = IIO_CHAN_INFO_SCALE_SHARED_BIT,
+			.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT |
+			IIO_CHAN_INFO_SCALE_SHARED_BIT,
 			.address = 1,
 			.scan_index = 1,
 			.scan_type = IIO_ST('u', 12, 16, 0),
@@ -84,7 +85,8 @@ static const struct ad7887_chip_info ad7887_chip_info_tbl[] = {
 			.type = IIO_VOLTAGE,
 			.indexed = 1,
 			.channel = 0,
-			.info_mask = IIO_CHAN_INFO_SCALE_SHARED_BIT,
+			.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT |
+			IIO_CHAN_INFO_SCALE_SHARED_BIT,
 			.address = 0,
 			.scan_index = 0,
 			.scan_type = IIO_ST('u', 12, 16, 0),
@@ -104,7 +106,7 @@ static int __devinit ad7887_probe(struct spi_device *spi)
 	struct ad7887_platform_data *pdata = spi->dev.platform_data;
 	struct ad7887_state *st;
 	int ret, voltage_uv = 0;
-	struct iio_dev *indio_dev = iio_allocate_device(sizeof(*st));
+	struct iio_dev *indio_dev = iio_device_alloc(sizeof(*st));
 
 	if (indio_dev == NULL)
 		return -ENOMEM;
@@ -220,7 +222,7 @@ error_disable_reg:
 error_put_reg:
 	if (!IS_ERR(st->reg))
 		regulator_put(st->reg);
-	iio_free_device(indio_dev);
+	iio_device_free(indio_dev);
 
 	return ret;
 }
@@ -237,7 +239,7 @@ static int ad7887_remove(struct spi_device *spi)
 		regulator_disable(st->reg);
 		regulator_put(st->reg);
 	}
-	iio_free_device(indio_dev);
+	iio_device_free(indio_dev);
 
 	return 0;
 }
diff --git a/drivers/staging/iio/adc/ad7887_ring.c b/drivers/staging/iio/adc/ad7887_ring.c
index d1809079b63..fd91384db89 100644
--- a/drivers/staging/iio/adc/ad7887_ring.c
+++ b/drivers/staging/iio/adc/ad7887_ring.c
@@ -1,5 +1,5 @@
 /*
- * Copyright 2010-2011 Analog Devices Inc.
+ * Copyright 2010-2012 Analog Devices Inc.
  * Copyright (C) 2008 Jonathan Cameron
  *
  * Licensed under the GPL-2.
@@ -12,10 +12,10 @@
 #include <linux/slab.h>
 #include <linux/spi/spi.h>
 
-#include "../iio.h"
-#include "../buffer.h"
-#include "../ring_sw.h"
-#include "../trigger_consumer.h"
+#include <linux/iio/iio.h>
+#include <linux/iio/buffer.h>
+#include <linux/iio/kfifo_buf.h>
+#include <linux/iio/trigger_consumer.h>
 
 #include "ad7887.h"
 
@@ -29,22 +29,11 @@
 static int ad7887_ring_preenable(struct iio_dev *indio_dev)
 {
 	struct ad7887_state *st = iio_priv(indio_dev);
-	struct iio_buffer *ring = indio_dev->buffer;
-
-	st->d_size = bitmap_weight(indio_dev->active_scan_mask,
-				   indio_dev->masklength) *
-		st->chip_info->channel[0].scan_type.storagebits / 8;
-
-	if (ring->scan_timestamp) {
-		st->d_size += sizeof(s64);
-
-		if (st->d_size % sizeof(s64))
-			st->d_size += sizeof(s64) - (st->d_size % sizeof(s64));
-	}
+	int ret;
 
-	if (indio_dev->buffer->access->set_bytes_per_datum)
-		indio_dev->buffer->access->
-			set_bytes_per_datum(indio_dev->buffer, st->d_size);
+	ret = iio_sw_buffer_preenable(indio_dev);
+	if (ret < 0)
+		return ret;
 
 	/* We know this is a single long so can 'cheat' */
 	switch (*indio_dev->active_scan_mask) {
@@ -83,7 +72,6 @@ static irqreturn_t ad7887_trigger_handler(int irq, void *p)
 	struct iio_poll_func *pf = p;
 	struct iio_dev *indio_dev = pf->indio_dev;
 	struct ad7887_state *st = iio_priv(indio_dev);
-	struct iio_buffer *ring = indio_dev->buffer;
 	s64 time_ns;
 	__u8 *buf;
 	int b_sent;
@@ -92,7 +80,7 @@ static irqreturn_t ad7887_trigger_handler(int irq, void *p)
 					   indio_dev->masklength) *
 		st->chip_info->channel[0].scan_type.storagebits / 8;
 
-	buf = kzalloc(st->d_size, GFP_KERNEL);
+	buf = kzalloc(indio_dev->scan_bytes, GFP_KERNEL);
 	if (buf == NULL)
 		return -ENOMEM;
 
@@ -103,8 +91,8 @@ static irqreturn_t ad7887_trigger_handler(int irq, void *p)
 	time_ns = iio_get_time_ns();
 
 	memcpy(buf, st->data, bytes);
-	if (ring->scan_timestamp)
-		memcpy(buf + st->d_size - sizeof(s64),
+	if (indio_dev->scan_timestamp)
+		memcpy(buf + indio_dev->scan_bytes - sizeof(s64),
 		       &time_ns, sizeof(time_ns));
 
 	indio_dev->buffer->access->store_to(indio_dev->buffer, buf, time_ns);
@@ -126,7 +114,7 @@ int ad7887_register_ring_funcs_and_init(struct iio_dev *indio_dev)
 {
 	int ret;
 
-	indio_dev->buffer = iio_sw_rb_allocate(indio_dev);
+	indio_dev->buffer = iio_kfifo_allocate(indio_dev);
 	if (!indio_dev->buffer) {
 		ret = -ENOMEM;
 		goto error_ret;
@@ -139,7 +127,7 @@ int ad7887_register_ring_funcs_and_init(struct iio_dev *indio_dev)
 						 indio_dev->id);
 	if (indio_dev->pollfunc == NULL) {
 		ret = -ENOMEM;
-		goto error_deallocate_sw_rb;
+		goto error_deallocate_kfifo;
 	}
 	/* Ring buffer functions - here trigger setup related */
 	indio_dev->setup_ops = &ad7887_ring_setup_ops;
@@ -148,8 +136,8 @@ int ad7887_register_ring_funcs_and_init(struct iio_dev *indio_dev)
 	indio_dev->modes |= INDIO_BUFFER_TRIGGERED;
 	return 0;
 
-error_deallocate_sw_rb:
-	iio_sw_rb_free(indio_dev->buffer);
+error_deallocate_kfifo:
+	iio_kfifo_free(indio_dev->buffer);
 error_ret:
 	return ret;
 }
@@ -157,5 +145,5 @@ error_ret:
 void ad7887_ring_cleanup(struct iio_dev *indio_dev)
 {
 	iio_dealloc_pollfunc(indio_dev->pollfunc);
-	iio_sw_rb_free(indio_dev->buffer);
+	iio_kfifo_free(indio_dev->buffer);
 }
diff --git a/drivers/staging/iio/adc/ad799x.h b/drivers/staging/iio/adc/ad799x.h
index 356f690a76f..99f8abe9731 100644
--- a/drivers/staging/iio/adc/ad799x.h
+++ b/drivers/staging/iio/adc/ad799x.h
@@ -104,7 +104,6 @@ struct ad799x_chip_info {
 struct ad799x_state {
 	struct i2c_client		*client;
 	const struct ad799x_chip_info	*chip_info;
-	size_t				d_size;
 	struct iio_trigger		*trig;
 	struct regulator		*reg;
 	u16				int_vref_mv;
diff --git a/drivers/staging/iio/adc/ad799x_core.c b/drivers/staging/iio/adc/ad799x_core.c
index a8458669350..80e0c6e25a9 100644
--- a/drivers/staging/iio/adc/ad799x_core.c
+++ b/drivers/staging/iio/adc/ad799x_core.c
@@ -33,10 +33,10 @@
 #include <linux/err.h>
 #include <linux/module.h>
 
-#include "../iio.h"
-#include "../sysfs.h"
-#include "../events.h"
-#include "../buffer.h"
+#include <linux/iio/iio.h>
+#include <linux/iio/sysfs.h>
+#include <linux/iio/events.h>
+#include <linux/iio/buffer.h>
 
 #include "ad799x.h"
 
@@ -148,7 +148,7 @@ static int ad799x_read_raw(struct iio_dev *indio_dev,
 	unsigned int scale_uv;
 
 	switch (m) {
-	case 0:
+	case IIO_CHAN_INFO_RAW:
 		mutex_lock(&indio_dev->mlock);
 		if (iio_buffer_enabled(indio_dev))
 			ret = -EBUSY;
@@ -182,7 +182,7 @@ static ssize_t ad799x_read_frequency(struct device *dev,
 					struct device_attribute *attr,
 					char *buf)
 {
-	struct iio_dev *indio_dev = dev_get_drvdata(dev);
+	struct iio_dev *indio_dev = dev_to_iio_dev(dev);
 	struct ad799x_state *st = iio_priv(indio_dev);
 
 	int ret;
@@ -201,7 +201,7 @@ static ssize_t ad799x_write_frequency(struct device *dev,
 					 const char *buf,
 					 size_t len)
 {
-	struct iio_dev *indio_dev = dev_get_drvdata(dev);
+	struct iio_dev *indio_dev = dev_to_iio_dev(dev);
 	struct ad799x_state *st = iio_priv(indio_dev);
 
 	long val;
@@ -294,7 +294,7 @@ static ssize_t ad799x_read_channel_config(struct device *dev,
 					struct device_attribute *attr,
 					char *buf)
 {
-	struct iio_dev *indio_dev = dev_get_drvdata(dev);
+	struct iio_dev *indio_dev = dev_to_iio_dev(dev);
 	struct ad799x_state *st = iio_priv(indio_dev);
 	struct iio_dev_attr *this_attr = to_iio_dev_attr(attr);
 
@@ -312,7 +312,7 @@ static ssize_t ad799x_write_channel_config(struct device *dev,
 					 const char *buf,
 					 size_t len)
 {
-	struct iio_dev *indio_dev = dev_get_drvdata(dev);
+	struct iio_dev *indio_dev = dev_to_iio_dev(dev);
 	struct ad799x_state *st = iio_priv(indio_dev);
 	struct iio_dev_attr *this_attr = to_iio_dev_attr(attr);
 
@@ -454,6 +454,7 @@ static const struct ad799x_chip_info ad799x_chip_info_tbl[] = {
 				.type = IIO_VOLTAGE,
 				.indexed = 1,
 				.channel = 0,
+				.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT,
 				.scan_index = 0,
 				.scan_type = IIO_ST('u', 12, 16, 0),
 			},
@@ -461,6 +462,7 @@ static const struct ad799x_chip_info ad799x_chip_info_tbl[] = {
 				.type = IIO_VOLTAGE,
 				.indexed = 1,
 				.channel = 1,
+				.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT,
 				.scan_index = 1,
 				.scan_type = IIO_ST('u', 12, 16, 0),
 			},
@@ -468,6 +470,7 @@ static const struct ad799x_chip_info ad799x_chip_info_tbl[] = {
 				.type = IIO_VOLTAGE,
 				.indexed = 1,
 				.channel = 2,
+				.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT,
 				.scan_index = 2,
 				.scan_type = IIO_ST('u', 12, 16, 0),
 			},
@@ -475,6 +478,7 @@ static const struct ad799x_chip_info ad799x_chip_info_tbl[] = {
 				.type = IIO_VOLTAGE,
 				.indexed = 1,
 				.channel = 3,
+				.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT,
 				.scan_index = 3,
 				.scan_type = IIO_ST('u', 12, 16, 0),
 			},
@@ -490,6 +494,7 @@ static const struct ad799x_chip_info ad799x_chip_info_tbl[] = {
 				.type = IIO_VOLTAGE,
 				.indexed = 1,
 				.channel = 0,
+				.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT,
 				.scan_index = 0,
 				.scan_type = IIO_ST('u', 10, 16, 2),
 			},
@@ -497,6 +502,7 @@ static const struct ad799x_chip_info ad799x_chip_info_tbl[] = {
 				.type = IIO_VOLTAGE,
 				.indexed = 1,
 				.channel = 1,
+				.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT,
 				.scan_index = 1,
 				.scan_type = IIO_ST('u', 10, 16, 2),
 			},
@@ -504,6 +510,7 @@ static const struct ad799x_chip_info ad799x_chip_info_tbl[] = {
 				.type = IIO_VOLTAGE,
 				.indexed = 1,
 				.channel = 2,
+				.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT,
 				.scan_index = 2,
 				.scan_type = IIO_ST('u', 10, 16, 2),
 			},
@@ -511,6 +518,7 @@ static const struct ad799x_chip_info ad799x_chip_info_tbl[] = {
 				.type = IIO_VOLTAGE,
 				.indexed = 1,
 				.channel = 3,
+				.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT,
 				.scan_index = 3,
 				.scan_type = IIO_ST('u', 10, 16, 2),
 			},
@@ -526,6 +534,7 @@ static const struct ad799x_chip_info ad799x_chip_info_tbl[] = {
 				.type = IIO_VOLTAGE,
 				.indexed = 1,
 				.channel = 0,
+				.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT,
 				.scan_index = 0,
 				.scan_type = IIO_ST('u', 8, 16, 4),
 			},
@@ -533,6 +542,7 @@ static const struct ad799x_chip_info ad799x_chip_info_tbl[] = {
 				.type = IIO_VOLTAGE,
 				.indexed = 1,
 				.channel = 1,
+				.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT,
 				.scan_index = 1,
 				.scan_type = IIO_ST('u', 8, 16, 4),
 			},
@@ -540,6 +550,7 @@ static const struct ad799x_chip_info ad799x_chip_info_tbl[] = {
 				.type = IIO_VOLTAGE,
 				.indexed = 1,
 				.channel = 2,
+				.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT,
 				.scan_index = 2,
 				.scan_type = IIO_ST('u', 8, 16, 4),
 			},
@@ -547,6 +558,7 @@ static const struct ad799x_chip_info ad799x_chip_info_tbl[] = {
 				.type = IIO_VOLTAGE,
 				.indexed = 1,
 				.channel = 3,
+				.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT,
 				.scan_index = 3,
 				.scan_type = IIO_ST('u', 8, 16, 4),
 			},
@@ -562,6 +574,7 @@ static const struct ad799x_chip_info ad799x_chip_info_tbl[] = {
 				.type = IIO_VOLTAGE,
 				.indexed = 1,
 				.channel = 0,
+				.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT,
 				.scan_index = 0,
 				.scan_type = IIO_ST('u', 12, 16, 0),
 				.event_mask = AD799X_EV_MASK,
@@ -570,6 +583,7 @@ static const struct ad799x_chip_info ad799x_chip_info_tbl[] = {
 				.type = IIO_VOLTAGE,
 				.indexed = 1,
 				.channel = 1,
+				.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT,
 				.scan_index = 1,
 				.scan_type = IIO_ST('u', 12, 16, 0),
 				.event_mask = AD799X_EV_MASK,
@@ -587,6 +601,7 @@ static const struct ad799x_chip_info ad799x_chip_info_tbl[] = {
 				.type = IIO_VOLTAGE,
 				.indexed = 1,
 				.channel = 0,
+				.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT,
 				.scan_index = 0,
 				.scan_type = IIO_ST('u', 10, 16, 2),
 				.event_mask = AD799X_EV_MASK,
@@ -596,6 +611,7 @@ static const struct ad799x_chip_info ad799x_chip_info_tbl[] = {
 				.indexed = 1,
 				.channel = 1,
 				.scan_index = 1,
+				.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT,
 				.scan_type = IIO_ST('u', 10, 16, 2),
 				.event_mask = AD799X_EV_MASK,
 			},
@@ -603,6 +619,7 @@ static const struct ad799x_chip_info ad799x_chip_info_tbl[] = {
 				.type = IIO_VOLTAGE,
 				.indexed = 1,
 				.channel = 2,
+				.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT,
 				.scan_index = 2,
 				.scan_type = IIO_ST('u', 10, 16, 2),
 				.event_mask = AD799X_EV_MASK,
@@ -611,6 +628,7 @@ static const struct ad799x_chip_info ad799x_chip_info_tbl[] = {
 				.type = IIO_VOLTAGE,
 				.indexed = 1,
 				.channel = 3,
+				.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT,
 				.scan_index = 3,
 				.scan_type = IIO_ST('u', 10, 16, 2),
 				.event_mask = AD799X_EV_MASK,
@@ -628,6 +646,7 @@ static const struct ad799x_chip_info ad799x_chip_info_tbl[] = {
 				.type = IIO_VOLTAGE,
 				.indexed = 1,
 				.channel = 0,
+				.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT,
 				.scan_index = 0,
 				.scan_type = IIO_ST('u', 12, 16, 0),
 				.event_mask = AD799X_EV_MASK,
@@ -636,6 +655,7 @@ static const struct ad799x_chip_info ad799x_chip_info_tbl[] = {
 				.type = IIO_VOLTAGE,
 				.indexed = 1,
 				.channel = 1,
+				.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT,
 				.scan_index = 1,
 				.scan_type = IIO_ST('u', 12, 16, 0),
 				.event_mask = AD799X_EV_MASK,
@@ -644,6 +664,7 @@ static const struct ad799x_chip_info ad799x_chip_info_tbl[] = {
 				.type = IIO_VOLTAGE,
 				.indexed = 1,
 				.channel = 2,
+				.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT,
 				.scan_index = 2,
 				.scan_type = IIO_ST('u', 12, 16, 0),
 				.event_mask = AD799X_EV_MASK,
@@ -652,6 +673,7 @@ static const struct ad799x_chip_info ad799x_chip_info_tbl[] = {
 				.type = IIO_VOLTAGE,
 				.indexed = 1,
 				.channel = 3,
+				.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT,
 				.scan_index = 3,
 				.scan_type = IIO_ST('u', 12, 16, 0),
 				.event_mask = AD799X_EV_MASK,
@@ -669,6 +691,7 @@ static const struct ad799x_chip_info ad799x_chip_info_tbl[] = {
 				.type = IIO_VOLTAGE,
 				.indexed = 1,
 				.channel = 0,
+				.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT,
 				.scan_index = 0,
 				.scan_type = IIO_ST('u', 10, 16, 2),
 				.event_mask = AD799X_EV_MASK,
@@ -677,6 +700,7 @@ static const struct ad799x_chip_info ad799x_chip_info_tbl[] = {
 				.type = IIO_VOLTAGE,
 				.indexed = 1,
 				.channel = 1,
+				.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT,
 				.scan_index = 1,
 				.scan_type = IIO_ST('u', 10, 16, 2),
 				.event_mask = AD799X_EV_MASK,
@@ -685,6 +709,7 @@ static const struct ad799x_chip_info ad799x_chip_info_tbl[] = {
 				.type = IIO_VOLTAGE,
 				.indexed = 1,
 				.channel = 2,
+				.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT,
 				.scan_index = 2,
 				.scan_type = IIO_ST('u', 10, 16, 2),
 				.event_mask = AD799X_EV_MASK,
@@ -693,6 +718,7 @@ static const struct ad799x_chip_info ad799x_chip_info_tbl[] = {
 				.type = IIO_VOLTAGE,
 				.indexed = 1,
 				.channel = 3,
+				.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT,
 				.scan_index = 3,
 				.scan_type = IIO_ST('u', 10, 16, 2),
 				.event_mask = AD799X_EV_MASK,
@@ -701,6 +727,7 @@ static const struct ad799x_chip_info ad799x_chip_info_tbl[] = {
 				.type = IIO_VOLTAGE,
 				.indexed = 1,
 				.channel = 4,
+				.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT,
 				.scan_index = 4,
 				.scan_type = IIO_ST('u', 10, 16, 2),
 			},
@@ -708,6 +735,7 @@ static const struct ad799x_chip_info ad799x_chip_info_tbl[] = {
 				.type = IIO_VOLTAGE,
 				.indexed = 1,
 				.channel = 5,
+				.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT,
 				.scan_index = 5,
 				.scan_type = IIO_ST('u', 10, 16, 2),
 			},
@@ -715,6 +743,7 @@ static const struct ad799x_chip_info ad799x_chip_info_tbl[] = {
 				.type = IIO_VOLTAGE,
 				.indexed = 1,
 				.channel = 6,
+				.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT,
 				.scan_index = 6,
 				.scan_type = IIO_ST('u', 10, 16, 2),
 			},
@@ -722,6 +751,7 @@ static const struct ad799x_chip_info ad799x_chip_info_tbl[] = {
 				.type = IIO_VOLTAGE,
 				.indexed = 1,
 				.channel = 7,
+				.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT,
 				.scan_index = 7,
 				.scan_type = IIO_ST('u', 10, 16, 2),
 			},
@@ -738,6 +768,7 @@ static const struct ad799x_chip_info ad799x_chip_info_tbl[] = {
 				.type = IIO_VOLTAGE,
 				.indexed = 1,
 				.channel = 0,
+				.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT,
 				.scan_index = 0,
 				.scan_type = IIO_ST('u', 12, 16, 0),
 				.event_mask = AD799X_EV_MASK,
@@ -746,6 +777,7 @@ static const struct ad799x_chip_info ad799x_chip_info_tbl[] = {
 				.type = IIO_VOLTAGE,
 				.indexed = 1,
 				.channel = 1,
+				.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT,
 				.scan_index = 1,
 				.scan_type = IIO_ST('u', 12, 16, 0),
 				.event_mask = AD799X_EV_MASK,
@@ -754,6 +786,7 @@ static const struct ad799x_chip_info ad799x_chip_info_tbl[] = {
 				.type = IIO_VOLTAGE,
 				.indexed = 1,
 				.channel = 2,
+				.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT,
 				.scan_index = 2,
 				.scan_type = IIO_ST('u', 12, 16, 0),
 				.event_mask = AD799X_EV_MASK,
@@ -762,6 +795,7 @@ static const struct ad799x_chip_info ad799x_chip_info_tbl[] = {
 				.type = IIO_VOLTAGE,
 				.indexed = 1,
 				.channel = 3,
+				.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT,
 				.scan_index = 3,
 				.scan_type = IIO_ST('u', 12, 16, 0),
 				.event_mask = AD799X_EV_MASK,
@@ -770,6 +804,7 @@ static const struct ad799x_chip_info ad799x_chip_info_tbl[] = {
 				.type = IIO_VOLTAGE,
 				.indexed = 1,
 				.channel = 4,
+				.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT,
 				.scan_index = 4,
 				.scan_type = IIO_ST('u', 12, 16, 0),
 			},
@@ -777,6 +812,7 @@ static const struct ad799x_chip_info ad799x_chip_info_tbl[] = {
 				.type = IIO_VOLTAGE,
 				.indexed = 1,
 				.channel = 5,
+				.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT,
 				.scan_index = 5,
 				.scan_type = IIO_ST('u', 12, 16, 0),
 			},
@@ -784,6 +820,7 @@ static const struct ad799x_chip_info ad799x_chip_info_tbl[] = {
 				.type = IIO_VOLTAGE,
 				.indexed = 1,
 				.channel = 6,
+				.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT,
 				.scan_index = 6,
 				.scan_type = IIO_ST('u', 12, 16, 0),
 			},
@@ -791,6 +828,7 @@ static const struct ad799x_chip_info ad799x_chip_info_tbl[] = {
 				.type = IIO_VOLTAGE,
 				.indexed = 1,
 				.channel = 7,
+				.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT,
 				.scan_index = 7,
 				.scan_type = IIO_ST('u', 12, 16, 0),
 			},
@@ -809,7 +847,7 @@ static int __devinit ad799x_probe(struct i2c_client *client,
 	int ret;
 	struct ad799x_platform_data *pdata = client->dev.platform_data;
 	struct ad799x_state *st;
-	struct iio_dev *indio_dev = iio_allocate_device(sizeof(*st));
+	struct iio_dev *indio_dev = iio_device_alloc(sizeof(*st));
 
 	if (indio_dev == NULL)
 		return -ENOMEM;
@@ -882,7 +920,7 @@ error_disable_reg:
 error_put_reg:
 	if (!IS_ERR(st->reg))
 		regulator_put(st->reg);
-	iio_free_device(indio_dev);
+	iio_device_free(indio_dev);
 
 	return ret;
 }
@@ -902,7 +940,7 @@ static __devexit int ad799x_remove(struct i2c_client *client)
 		regulator_disable(st->reg);
 		regulator_put(st->reg);
 	}
-	iio_free_device(indio_dev);
+	iio_device_free(indio_dev);
 
 	return 0;
 }
diff --git a/drivers/staging/iio/adc/ad799x_ring.c b/drivers/staging/iio/adc/ad799x_ring.c
index 069765cab27..1c7ff4423db 100644
--- a/drivers/staging/iio/adc/ad799x_ring.c
+++ b/drivers/staging/iio/adc/ad799x_ring.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (C) 2010 Michael Hennerich, Analog Devices Inc.
+ * Copyright (C) 2010-2012 Michael Hennerich, Analog Devices Inc.
  * Copyright (C) 2008-2010 Jonathan Cameron
  *
  * This program is free software; you can redistribute it and/or modify
@@ -16,10 +16,10 @@
 #include <linux/i2c.h>
 #include <linux/bitops.h>
 
-#include "../iio.h"
-#include "../buffer.h"
-#include "../ring_sw.h"
-#include "../trigger_consumer.h"
+#include <linux/iio/iio.h>
+#include <linux/iio/buffer.h>
+#include <linux/iio/kfifo_buf.h>
+#include <linux/iio/trigger_consumer.h>
 
 #include "ad799x.h"
 
@@ -32,9 +32,7 @@
  **/
 static int ad799x_ring_preenable(struct iio_dev *indio_dev)
 {
-	struct iio_buffer *ring = indio_dev->buffer;
 	struct ad799x_state *st = iio_priv(indio_dev);
-
 	/*
 	 * Need to figure out the current mode based upon the requested
 	 * scan mask in iio_dev
@@ -43,21 +41,7 @@ static int ad799x_ring_preenable(struct iio_dev *indio_dev)
 	if (st->id == ad7997 || st->id == ad7998)
 		ad7997_8_set_scan_mode(st, *indio_dev->active_scan_mask);
 
-	st->d_size = bitmap_weight(indio_dev->active_scan_mask,
-				   indio_dev->masklength) * 2;
-
-	if (ring->scan_timestamp) {
-		st->d_size += sizeof(s64);
-
-		if (st->d_size % sizeof(s64))
-			st->d_size += sizeof(s64) - (st->d_size % sizeof(s64));
-	}
-
-	if (indio_dev->buffer->access->set_bytes_per_datum)
-		indio_dev->buffer->access->
-			set_bytes_per_datum(indio_dev->buffer, st->d_size);
-
-	return 0;
+	return iio_sw_buffer_preenable(indio_dev);
 }
 
 /**
@@ -78,7 +62,7 @@ static irqreturn_t ad799x_trigger_handler(int irq, void *p)
 	int b_sent;
 	u8 cmd;
 
-	rxbuf = kmalloc(st->d_size, GFP_KERNEL);
+	rxbuf = kmalloc(indio_dev->scan_bytes, GFP_KERNEL);
 	if (rxbuf == NULL)
 		goto out;
 
@@ -111,8 +95,8 @@ static irqreturn_t ad799x_trigger_handler(int irq, void *p)
 
 	time_ns = iio_get_time_ns();
 
-	if (ring->scan_timestamp)
-		memcpy(rxbuf + st->d_size - sizeof(s64),
+	if (indio_dev->scan_timestamp)
+		memcpy(rxbuf + indio_dev->scan_bytes - sizeof(s64),
 			&time_ns, sizeof(time_ns));
 
 	ring->access->store_to(indio_dev->buffer, rxbuf, time_ns);
@@ -136,7 +120,7 @@ int ad799x_register_ring_funcs_and_init(struct iio_dev *indio_dev)
 {
 	int ret = 0;
 
-	indio_dev->buffer = iio_sw_rb_allocate(indio_dev);
+	indio_dev->buffer = iio_kfifo_allocate(indio_dev);
 	if (!indio_dev->buffer) {
 		ret = -ENOMEM;
 		goto error_ret;
@@ -150,7 +134,7 @@ int ad799x_register_ring_funcs_and_init(struct iio_dev *indio_dev)
 						 indio_dev->id);
 	if (indio_dev->pollfunc == NULL) {
 		ret = -ENOMEM;
-		goto error_deallocate_sw_rb;
+		goto error_deallocate_kfifo;
 	}
 
 	/* Ring buffer functions - here trigger setup related */
@@ -161,8 +145,8 @@ int ad799x_register_ring_funcs_and_init(struct iio_dev *indio_dev)
 	indio_dev->modes |= INDIO_BUFFER_TRIGGERED;
 	return 0;
 
-error_deallocate_sw_rb:
-	iio_sw_rb_free(indio_dev->buffer);
+error_deallocate_kfifo:
+	iio_kfifo_free(indio_dev->buffer);
 error_ret:
 	return ret;
 }
@@ -170,5 +154,5 @@ error_ret:
 void ad799x_ring_cleanup(struct iio_dev *indio_dev)
 {
 	iio_dealloc_pollfunc(indio_dev->pollfunc);
-	iio_sw_rb_free(indio_dev->buffer);
+	iio_kfifo_free(indio_dev->buffer);
 }
diff --git a/drivers/staging/iio/adc/adt7310.c b/drivers/staging/iio/adc/adt7310.c
index caf57c1169b..e5f1ed7f869 100644
--- a/drivers/staging/iio/adc/adt7310.c
+++ b/drivers/staging/iio/adc/adt7310.c
@@ -15,9 +15,9 @@
 #include <linux/spi/spi.h>
 #include <linux/module.h>
 
-#include "../iio.h"
-#include "../sysfs.h"
-#include "../events.h"
+#include <linux/iio/iio.h>
+#include <linux/iio/sysfs.h>
+#include <linux/iio/events.h>
 /*
  * ADT7310 registers definition
  */
@@ -175,7 +175,7 @@ static ssize_t adt7310_show_mode(struct device *dev,
 		struct device_attribute *attr,
 		char *buf)
 {
-	struct iio_dev *dev_info = dev_get_drvdata(dev);
+	struct iio_dev *dev_info = dev_to_iio_dev(dev);
 	struct adt7310_chip_info *chip = iio_priv(dev_info);
 	u8 config;
 
@@ -198,7 +198,7 @@ static ssize_t adt7310_store_mode(struct device *dev,
 		const char *buf,
 		size_t len)
 {
-	struct iio_dev *dev_info = dev_get_drvdata(dev);
+	struct iio_dev *dev_info = dev_to_iio_dev(dev);
 	struct adt7310_chip_info *chip = iio_priv(dev_info);
 	u16 config;
 	int ret;
@@ -242,7 +242,7 @@ static ssize_t adt7310_show_resolution(struct device *dev,
 		struct device_attribute *attr,
 		char *buf)
 {
-	struct iio_dev *dev_info = dev_get_drvdata(dev);
+	struct iio_dev *dev_info = dev_to_iio_dev(dev);
 	struct adt7310_chip_info *chip = iio_priv(dev_info);
 	int ret;
 	int bits;
@@ -264,7 +264,7 @@ static ssize_t adt7310_store_resolution(struct device *dev,
 		const char *buf,
 		size_t len)
 {
-	struct iio_dev *dev_info = dev_get_drvdata(dev);
+	struct iio_dev *dev_info = dev_to_iio_dev(dev);
 	struct adt7310_chip_info *chip = iio_priv(dev_info);
 	unsigned long data;
 	u16 config;
@@ -300,7 +300,7 @@ static ssize_t adt7310_show_id(struct device *dev,
 		struct device_attribute *attr,
 		char *buf)
 {
-	struct iio_dev *dev_info = dev_get_drvdata(dev);
+	struct iio_dev *dev_info = dev_to_iio_dev(dev);
 	struct adt7310_chip_info *chip = iio_priv(dev_info);
 	u8 id;
 	int ret;
@@ -350,7 +350,7 @@ static ssize_t adt7310_show_value(struct device *dev,
 		struct device_attribute *attr,
 		char *buf)
 {
-	struct iio_dev *dev_info = dev_get_drvdata(dev);
+	struct iio_dev *dev_info = dev_to_iio_dev(dev);
 	struct adt7310_chip_info *chip = iio_priv(dev_info);
 	u8 status;
 	u16 data;
@@ -424,7 +424,7 @@ static ssize_t adt7310_show_event_mode(struct device *dev,
 		struct device_attribute *attr,
 		char *buf)
 {
-	struct iio_dev *dev_info = dev_get_drvdata(dev);
+	struct iio_dev *dev_info = dev_to_iio_dev(dev);
 	struct adt7310_chip_info *chip = iio_priv(dev_info);
 	int ret;
 
@@ -443,7 +443,7 @@ static ssize_t adt7310_set_event_mode(struct device *dev,
 		const char *buf,
 		size_t len)
 {
-	struct iio_dev *dev_info = dev_get_drvdata(dev);
+	struct iio_dev *dev_info = dev_to_iio_dev(dev);
 	struct adt7310_chip_info *chip = iio_priv(dev_info);
 	u16 config;
 	int ret;
@@ -476,7 +476,7 @@ static ssize_t adt7310_show_fault_queue(struct device *dev,
 		struct device_attribute *attr,
 		char *buf)
 {
-	struct iio_dev *dev_info = dev_get_drvdata(dev);
+	struct iio_dev *dev_info = dev_to_iio_dev(dev);
 	struct adt7310_chip_info *chip = iio_priv(dev_info);
 	int ret;
 
@@ -492,7 +492,7 @@ static ssize_t adt7310_set_fault_queue(struct device *dev,
 		const char *buf,
 		size_t len)
 {
-	struct iio_dev *dev_info = dev_get_drvdata(dev);
+	struct iio_dev *dev_info = dev_to_iio_dev(dev);
 	struct adt7310_chip_info *chip = iio_priv(dev_info);
 	unsigned long data;
 	int ret;
@@ -522,7 +522,7 @@ static inline ssize_t adt7310_show_t_bound(struct device *dev,
 		u8 bound_reg,
 		char *buf)
 {
-	struct iio_dev *dev_info = dev_get_drvdata(dev);
+	struct iio_dev *dev_info = dev_to_iio_dev(dev);
 	struct adt7310_chip_info *chip = iio_priv(dev_info);
 	u16 data;
 	int ret;
@@ -540,7 +540,7 @@ static inline ssize_t adt7310_set_t_bound(struct device *dev,
 		const char *buf,
 		size_t len)
 {
-	struct iio_dev *dev_info = dev_get_drvdata(dev);
+	struct iio_dev *dev_info = dev_to_iio_dev(dev);
 	struct adt7310_chip_info *chip = iio_priv(dev_info);
 	long tmp1, tmp2;
 	u16 data;
@@ -660,7 +660,7 @@ static ssize_t adt7310_show_t_hyst(struct device *dev,
 		struct device_attribute *attr,
 		char *buf)
 {
-	struct iio_dev *dev_info = dev_get_drvdata(dev);
+	struct iio_dev *dev_info = dev_to_iio_dev(dev);
 	struct adt7310_chip_info *chip = iio_priv(dev_info);
 	int ret;
 	u8 t_hyst;
@@ -677,7 +677,7 @@ static inline ssize_t adt7310_set_t_hyst(struct device *dev,
 		const char *buf,
 		size_t len)
 {
-	struct iio_dev *dev_info = dev_get_drvdata(dev);
+	struct iio_dev *dev_info = dev_to_iio_dev(dev);
 	struct adt7310_chip_info *chip = iio_priv(dev_info);
 	int ret;
 	unsigned long data;
@@ -753,7 +753,7 @@ static int __devinit adt7310_probe(struct spi_device *spi_dev)
 	unsigned long *adt7310_platform_data = spi_dev->dev.platform_data;
 	unsigned long irq_flags;
 
-	indio_dev = iio_allocate_device(sizeof(*chip));
+	indio_dev = iio_device_alloc(sizeof(*chip));
 	if (indio_dev == NULL) {
 		ret = -ENOMEM;
 		goto error_ret;
@@ -833,7 +833,7 @@ error_unreg_int_irq:
 error_unreg_ct_irq:
 	free_irq(spi_dev->irq, indio_dev);
 error_free_dev:
-	iio_free_device(indio_dev);
+	iio_device_free(indio_dev);
 error_ret:
 	return ret;
 }
@@ -849,7 +849,7 @@ static int __devexit adt7310_remove(struct spi_device *spi_dev)
 		free_irq(adt7310_platform_data[0], indio_dev);
 	if (spi_dev->irq)
 		free_irq(spi_dev->irq, indio_dev);
-	iio_free_device(indio_dev);
+	iio_device_free(indio_dev);
 
 	return 0;
 }
diff --git a/drivers/staging/iio/adc/adt7410.c b/drivers/staging/iio/adc/adt7410.c
index dff3e8ca2d7..917b6921e24 100644
--- a/drivers/staging/iio/adc/adt7410.c
+++ b/drivers/staging/iio/adc/adt7410.c
@@ -15,9 +15,9 @@
 #include <linux/i2c.h>
 #include <linux/module.h>
 
-#include "../iio.h"
-#include "../sysfs.h"
-#include "../events.h"
+#include <linux/iio/iio.h>
+#include <linux/iio/sysfs.h>
+#include <linux/iio/events.h>
 
 /*
  * ADT7410 registers definition
@@ -144,7 +144,7 @@ static ssize_t adt7410_show_mode(struct device *dev,
 		struct device_attribute *attr,
 		char *buf)
 {
-	struct iio_dev *dev_info = dev_get_drvdata(dev);
+	struct iio_dev *dev_info = dev_to_iio_dev(dev);
 	struct adt7410_chip_info *chip = iio_priv(dev_info);
 	u8 config;
 
@@ -167,7 +167,7 @@ static ssize_t adt7410_store_mode(struct device *dev,
 		const char *buf,
 		size_t len)
 {
-	struct iio_dev *dev_info = dev_get_drvdata(dev);
+	struct iio_dev *dev_info = dev_to_iio_dev(dev);
 	struct adt7410_chip_info *chip = iio_priv(dev_info);
 	u16 config;
 	int ret;
@@ -211,7 +211,7 @@ static ssize_t adt7410_show_resolution(struct device *dev,
 		struct device_attribute *attr,
 		char *buf)
 {
-	struct iio_dev *dev_info = dev_get_drvdata(dev);
+	struct iio_dev *dev_info = dev_to_iio_dev(dev);
 	struct adt7410_chip_info *chip = iio_priv(dev_info);
 	int ret;
 	int bits;
@@ -233,7 +233,7 @@ static ssize_t adt7410_store_resolution(struct device *dev,
 		const char *buf,
 		size_t len)
 {
-	struct iio_dev *dev_info = dev_get_drvdata(dev);
+	struct iio_dev *dev_info = dev_to_iio_dev(dev);
 	struct adt7410_chip_info *chip = iio_priv(dev_info);
 	unsigned long data;
 	u16 config;
@@ -269,7 +269,7 @@ static ssize_t adt7410_show_id(struct device *dev,
 		struct device_attribute *attr,
 		char *buf)
 {
-	struct iio_dev *dev_info = dev_get_drvdata(dev);
+	struct iio_dev *dev_info = dev_to_iio_dev(dev);
 	struct adt7410_chip_info *chip = iio_priv(dev_info);
 	u8 id;
 	int ret;
@@ -319,7 +319,7 @@ static ssize_t adt7410_show_value(struct device *dev,
 		struct device_attribute *attr,
 		char *buf)
 {
-	struct iio_dev *dev_info = dev_get_drvdata(dev);
+	struct iio_dev *dev_info = dev_to_iio_dev(dev);
 	struct adt7410_chip_info *chip = iio_priv(dev_info);
 	u8 status;
 	u16 data;
@@ -392,7 +392,7 @@ static ssize_t adt7410_show_event_mode(struct device *dev,
 		struct device_attribute *attr,
 		char *buf)
 {
-	struct iio_dev *dev_info = dev_get_drvdata(dev);
+	struct iio_dev *dev_info = dev_to_iio_dev(dev);
 	struct adt7410_chip_info *chip = iio_priv(dev_info);
 	int ret;
 
@@ -411,7 +411,7 @@ static ssize_t adt7410_set_event_mode(struct device *dev,
 		const char *buf,
 		size_t len)
 {
-	struct iio_dev *dev_info = dev_get_drvdata(dev);
+	struct iio_dev *dev_info = dev_to_iio_dev(dev);
 	struct adt7410_chip_info *chip = iio_priv(dev_info);
 	u16 config;
 	int ret;
@@ -444,7 +444,7 @@ static ssize_t adt7410_show_fault_queue(struct device *dev,
 		struct device_attribute *attr,
 		char *buf)
 {
-	struct iio_dev *dev_info = dev_get_drvdata(dev);
+	struct iio_dev *dev_info = dev_to_iio_dev(dev);
 	struct adt7410_chip_info *chip = iio_priv(dev_info);
 	int ret;
 
@@ -460,7 +460,7 @@ static ssize_t adt7410_set_fault_queue(struct device *dev,
 		const char *buf,
 		size_t len)
 {
-	struct iio_dev *dev_info = dev_get_drvdata(dev);
+	struct iio_dev *dev_info = dev_to_iio_dev(dev);
 	struct adt7410_chip_info *chip = iio_priv(dev_info);
 	unsigned long data;
 	int ret;
@@ -490,7 +490,7 @@ static inline ssize_t adt7410_show_t_bound(struct device *dev,
 		u8 bound_reg,
 		char *buf)
 {
-	struct iio_dev *dev_info = dev_get_drvdata(dev);
+	struct iio_dev *dev_info = dev_to_iio_dev(dev);
 	struct adt7410_chip_info *chip = iio_priv(dev_info);
 	u16 data;
 	int ret;
@@ -508,7 +508,7 @@ static inline ssize_t adt7410_set_t_bound(struct device *dev,
 		const char *buf,
 		size_t len)
 {
-	struct iio_dev *dev_info = dev_get_drvdata(dev);
+	struct iio_dev *dev_info = dev_to_iio_dev(dev);
 	struct adt7410_chip_info *chip = iio_priv(dev_info);
 	long tmp1, tmp2;
 	u16 data;
@@ -628,7 +628,7 @@ static ssize_t adt7410_show_t_hyst(struct device *dev,
 		struct device_attribute *attr,
 		char *buf)
 {
-	struct iio_dev *dev_info = dev_get_drvdata(dev);
+	struct iio_dev *dev_info = dev_to_iio_dev(dev);
 	struct adt7410_chip_info *chip = iio_priv(dev_info);
 	int ret;
 	u8 t_hyst;
@@ -645,7 +645,7 @@ static inline ssize_t adt7410_set_t_hyst(struct device *dev,
 		const char *buf,
 		size_t len)
 {
-	struct iio_dev *dev_info = dev_get_drvdata(dev);
+	struct iio_dev *dev_info = dev_to_iio_dev(dev);
 	struct adt7410_chip_info *chip = iio_priv(dev_info);
 	int ret;
 	unsigned long data;
@@ -721,7 +721,7 @@ static int __devinit adt7410_probe(struct i2c_client *client,
 	int ret = 0;
 	unsigned long *adt7410_platform_data = client->dev.platform_data;
 
-	indio_dev = iio_allocate_device(sizeof(*chip));
+	indio_dev = iio_device_alloc(sizeof(*chip));
 	if (indio_dev == NULL) {
 		ret = -ENOMEM;
 		goto error_ret;
@@ -797,7 +797,7 @@ error_unreg_int_irq:
 error_unreg_ct_irq:
 	free_irq(client->irq, indio_dev);
 error_free_dev:
-	iio_free_device(indio_dev);
+	iio_device_free(indio_dev);
 error_ret:
 	return ret;
 }
@@ -812,7 +812,7 @@ static int __devexit adt7410_remove(struct i2c_client *client)
 		free_irq(adt7410_platform_data[0], indio_dev);
 	if (client->irq)
 		free_irq(client->irq, indio_dev);
-	iio_free_device(indio_dev);
+	iio_device_free(indio_dev);
 
 	return 0;
 }
diff --git a/drivers/staging/iio/adc/lpc32xx_adc.c b/drivers/staging/iio/adc/lpc32xx_adc.c
index dfc9033843a..9690306d1f8 100644
--- a/drivers/staging/iio/adc/lpc32xx_adc.c
+++ b/drivers/staging/iio/adc/lpc32xx_adc.c
@@ -30,9 +30,10 @@
 #include <linux/clk.h>
 #include <linux/err.h>
 #include <linux/completion.h>
+#include <linux/of.h>
 
-#include "../iio.h"
-#include "../sysfs.h"
+#include <linux/iio/iio.h>
+#include <linux/iio/sysfs.h>
 
 /*
  * LPC32XX registers definitions
@@ -73,7 +74,7 @@ static int lpc32xx_read_raw(struct iio_dev *indio_dev,
 {
 	struct lpc32xx_adc_info *info = iio_priv(indio_dev);
 
-	if (mask == 0) {
+	if (mask == IIO_CHAN_INFO_RAW) {
 		mutex_lock(&indio_dev->mlock);
 		clk_enable(info->clk);
 		/* Measurement setup */
@@ -98,12 +99,13 @@ static const struct iio_info lpc32xx_adc_iio_info = {
 	.driver_module = THIS_MODULE,
 };
 
-#define LPC32XX_ADC_CHANNEL(_index) {		\
-	.type = IIO_VOLTAGE,			\
-	.indexed = 1,				\
-	.channel = _index,			\
-	.address = AD_IN * _index,		\
-	.scan_index = _index,			\
+#define LPC32XX_ADC_CHANNEL(_index) {			\
+	.type = IIO_VOLTAGE,				\
+	.indexed = 1,					\
+	.channel = _index,				\
+	.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT,	\
+	.address = AD_IN * _index,			\
+	.scan_index = _index,				\
 }
 
 static struct iio_chan_spec lpc32xx_adc_iio_channels[] = {
@@ -139,7 +141,7 @@ static int __devinit lpc32xx_adc_probe(struct platform_device *pdev)
 		goto errout1;
 	}
 
-	iodev = iio_allocate_device(sizeof(struct lpc32xx_adc_info));
+	iodev = iio_device_alloc(sizeof(struct lpc32xx_adc_info));
 	if (!iodev) {
 		dev_err(&pdev->dev, "failed allocating iio device\n");
 		retval = -ENOMEM;
@@ -200,7 +202,7 @@ errout4:
 errout3:
 	iounmap(info->adc_base);
 errout2:
-	iio_free_device(iodev);
+	iio_device_free(iodev);
 errout1:
 	return retval;
 }
@@ -216,17 +218,26 @@ static int __devexit lpc32xx_adc_remove(struct platform_device *pdev)
 	platform_set_drvdata(pdev, NULL);
 	clk_put(info->clk);
 	iounmap(info->adc_base);
-	iio_free_device(iodev);
+	iio_device_free(iodev);
 
 	return 0;
 }
 
+#ifdef CONFIG_OF
+static const struct of_device_id lpc32xx_adc_match[] = {
+	{ .compatible = "nxp,lpc3220-adc" },
+	{},
+};
+MODULE_DEVICE_TABLE(of, lpc32xx_adc_match);
+#endif
+
 static struct platform_driver lpc32xx_adc_driver = {
 	.probe		= lpc32xx_adc_probe,
 	.remove		= __devexit_p(lpc32xx_adc_remove),
 	.driver		= {
 		.name	= MOD_NAME,
 		.owner	= THIS_MODULE,
+		.of_match_table = of_match_ptr(lpc32xx_adc_match),
 	},
 };
 
diff --git a/drivers/staging/iio/adc/max1363_core.c b/drivers/staging/iio/adc/max1363_core.c
index cf3e2ca7e31..6799ce23a39 100644
--- a/drivers/staging/iio/adc/max1363_core.c
+++ b/drivers/staging/iio/adc/max1363_core.c
@@ -32,10 +32,11 @@
 #include <linux/err.h>
 #include <linux/module.h>
 
-#include "../iio.h"
-#include "../sysfs.h"
-#include "../events.h"
-#include "../buffer.h"
+#include <linux/iio/iio.h>
+#include <linux/iio/sysfs.h>
+#include <linux/iio/events.h>
+#include <linux/iio/buffer.h>
+#include <linux/iio/driver.h>
 
 #include "max1363.h"
 
@@ -248,7 +249,7 @@ static int max1363_read_raw(struct iio_dev *indio_dev,
 	struct max1363_state *st = iio_priv(indio_dev);
 	int ret;
 	switch (m) {
-	case 0:
+	case IIO_CHAN_INFO_RAW:
 		ret = max1363_read_single_chan(indio_dev, chan, val, m);
 		if (ret < 0)
 			return ret;
@@ -281,7 +282,8 @@ static const enum max1363_modes max1363_mode_list[] = {
 #define MAX1363_EV_M						\
 	(IIO_EV_BIT(IIO_EV_TYPE_THRESH, IIO_EV_DIR_RISING)	\
 	 | IIO_EV_BIT(IIO_EV_TYPE_THRESH, IIO_EV_DIR_FALLING))
-#define MAX1363_INFO_MASK IIO_CHAN_INFO_SCALE_SHARED_BIT
+#define MAX1363_INFO_MASK (IIO_CHAN_INFO_RAW_SEPARATE_BIT |	\
+			   IIO_CHAN_INFO_SCALE_SHARED_BIT)
 #define MAX1363_CHAN_U(num, addr, si, bits, evmask)			\
 	{								\
 		.type = IIO_VOLTAGE,					\
@@ -497,7 +499,7 @@ static ssize_t max1363_monitor_show_freq(struct device *dev,
 					struct device_attribute *attr,
 					char *buf)
 {
-	struct max1363_state *st = iio_priv(dev_get_drvdata(dev));
+	struct max1363_state *st = iio_priv(dev_to_iio_dev(dev));
 	return sprintf(buf, "%d\n", max1363_monitor_speeds[st->monitor_speed]);
 }
 
@@ -506,7 +508,7 @@ static ssize_t max1363_monitor_store_freq(struct device *dev,
 					const char *buf,
 					size_t len)
 {
-	struct iio_dev *indio_dev = dev_get_drvdata(dev);
+	struct iio_dev *indio_dev = dev_to_iio_dev(dev);
 	struct max1363_state *st = iio_priv(indio_dev);
 	int i, ret;
 	unsigned long val;
@@ -830,6 +832,7 @@ static struct attribute_group max1363_event_attribute_group = {
 static const struct iio_info max1238_info = {
 	.read_raw = &max1363_read_raw,
 	.driver_module = THIS_MODULE,
+	.update_scan_mode = &max1363_update_scan_mode,
 };
 
 static const struct iio_info max1363_info = {
@@ -1284,11 +1287,14 @@ static int __devinit max1363_probe(struct i2c_client *client,
 	if (ret)
 		goto error_put_reg;
 
-	indio_dev = iio_allocate_device(sizeof(struct max1363_state));
+	indio_dev = iio_device_alloc(sizeof(struct max1363_state));
 	if (indio_dev == NULL) {
 		ret = -ENOMEM;
 		goto error_disable_reg;
 	}
+	ret = iio_map_array_register(indio_dev, client->dev.platform_data);
+	if (ret < 0)
+		goto error_free_device;
 	st = iio_priv(indio_dev);
 	st->reg = reg;
 	/* this is only used for device removal purposes */
@@ -1299,7 +1305,7 @@ static int __devinit max1363_probe(struct i2c_client *client,
 
 	ret = max1363_alloc_scan_masks(indio_dev);
 	if (ret)
-		goto error_free_device;
+		goto error_unregister_map;
 
 	/* Estabilish that the iio_dev is a child of the i2c device */
 	indio_dev->dev.parent = &client->dev;
@@ -1349,8 +1355,10 @@ error_cleanup_ring:
 	max1363_ring_cleanup(indio_dev);
 error_free_available_scan_masks:
 	kfree(indio_dev->available_scan_masks);
+error_unregister_map:
+	iio_map_array_unregister(indio_dev, client->dev.platform_data);
 error_free_device:
-	iio_free_device(indio_dev);
+	iio_device_free(indio_dev);
 error_disable_reg:
 	regulator_disable(reg);
 error_put_reg:
@@ -1375,7 +1383,8 @@ static int max1363_remove(struct i2c_client *client)
 		regulator_disable(reg);
 		regulator_put(reg);
 	}
-	iio_free_device(indio_dev);
+	iio_map_array_unregister(indio_dev, client->dev.platform_data);
+	iio_device_free(indio_dev);
 
 	return 0;
 }
diff --git a/drivers/staging/iio/adc/max1363_ring.c b/drivers/staging/iio/adc/max1363_ring.c
index d0a60a38293..b3020130012 100644
--- a/drivers/staging/iio/adc/max1363_ring.c
+++ b/drivers/staging/iio/adc/max1363_ring.c
@@ -14,10 +14,10 @@
 #include <linux/i2c.h>
 #include <linux/bitops.h>
 
-#include "../iio.h"
-#include "../buffer.h"
+#include <linux/iio/iio.h>
+#include <linux/iio/buffer.h>
 #include "../ring_sw.h"
-#include "../trigger_consumer.h"
+#include <linux/iio/trigger_consumer.h>
 
 #include "max1363.h"
 
@@ -54,7 +54,7 @@ static irqreturn_t max1363_trigger_handler(int irq, void *p)
 		d_size = numvals*2;
 	else
 		d_size = numvals;
-	if (indio_dev->buffer->scan_timestamp) {
+	if (indio_dev->scan_timestamp) {
 		d_size += sizeof(s64);
 		if (d_size % sizeof(s64))
 			d_size += sizeof(s64) - (d_size % sizeof(s64));
@@ -78,7 +78,7 @@ static irqreturn_t max1363_trigger_handler(int irq, void *p)
 
 	time_ns = iio_get_time_ns();
 
-	if (indio_dev->buffer->scan_timestamp)
+	if (indio_dev->scan_timestamp)
 		memcpy(rxbuf + d_size - sizeof(s64), &time_ns, sizeof(time_ns));
 	iio_push_to_buffer(indio_dev->buffer, rxbuf, time_ns);
 
diff --git a/drivers/staging/iio/adc/spear_adc.c b/drivers/staging/iio/adc/spear_adc.c
new file mode 100644
index 00000000000..64d630e6fe2
--- /dev/null
+++ b/drivers/staging/iio/adc/spear_adc.c
@@ -0,0 +1,448 @@
+/*
+ * ST SPEAr ADC driver
+ *
+ * Copyright 2012 Stefan Roese <sr@denx.de>
+ *
+ * Licensed under the GPL-2.
+ */
+
+#include <linux/module.h>
+#include <linux/platform_device.h>
+#include <linux/interrupt.h>
+#include <linux/device.h>
+#include <linux/kernel.h>
+#include <linux/slab.h>
+#include <linux/io.h>
+#include <linux/clk.h>
+#include <linux/err.h>
+#include <linux/completion.h>
+#include <linux/of.h>
+#include <linux/of_address.h>
+
+#include <linux/iio/iio.h>
+#include <linux/iio/sysfs.h>
+
+/*
+ * SPEAR registers definitions
+ */
+
+#define SCAN_RATE_LO(x)		((x) & 0xFFFF)
+#define SCAN_RATE_HI(x)		(((x) >> 0x10) & 0xFFFF)
+#define CLK_LOW(x)		(((x) & 0xf) << 0)
+#define CLK_HIGH(x)		(((x) & 0xf) << 4)
+
+/* Bit definitions for SPEAR_ADC_STATUS */
+#define START_CONVERSION	(1 << 0)
+#define CHANNEL_NUM(x)		((x) << 1)
+#define ADC_ENABLE		(1 << 4)
+#define AVG_SAMPLE(x)		((x) << 5)
+#define VREF_INTERNAL		(1 << 9)
+
+#define DATA_MASK		0x03ff
+#define DATA_BITS		10
+
+#define MOD_NAME "spear-adc"
+
+#define ADC_CHANNEL_NUM		8
+
+#define CLK_MIN			2500000
+#define CLK_MAX			20000000
+
+struct adc_regs_spear3xx {
+	u32 status;
+	u32 average;
+	u32 scan_rate;
+	u32 clk;	/* Not avail for 1340 & 1310 */
+	u32 ch_ctrl[ADC_CHANNEL_NUM];
+	u32 ch_data[ADC_CHANNEL_NUM];
+};
+
+struct chan_data {
+	u32 lsb;
+	u32 msb;
+};
+
+struct adc_regs_spear6xx {
+	u32 status;
+	u32 pad[2];
+	u32 clk;
+	u32 ch_ctrl[ADC_CHANNEL_NUM];
+	struct chan_data ch_data[ADC_CHANNEL_NUM];
+	u32 scan_rate_lo;
+	u32 scan_rate_hi;
+	struct chan_data average;
+};
+
+struct spear_adc_info {
+	struct device_node *np;
+	struct adc_regs_spear3xx __iomem *adc_base_spear3xx;
+	struct adc_regs_spear6xx __iomem *adc_base_spear6xx;
+	struct clk *clk;
+	struct completion completion;
+	u32 current_clk;
+	u32 sampling_freq;
+	u32 avg_samples;
+	u32 vref_external;
+	u32 value;
+};
+
+/*
+ * Functions to access some SPEAr ADC register. Abstracted into
+ * static inline functions, because of different register offsets
+ * on different SoC variants (SPEAr300 vs SPEAr600 etc).
+ */
+static void spear_adc_set_status(struct spear_adc_info *info, u32 val)
+{
+	__raw_writel(val, &info->adc_base_spear6xx->status);
+}
+
+static void spear_adc_set_clk(struct spear_adc_info *info, u32 val)
+{
+	u32 clk_high, clk_low, count;
+	u32 apb_clk = clk_get_rate(info->clk);
+
+	count = (apb_clk + val - 1) / val;
+	clk_low = count / 2;
+	clk_high = count - clk_low;
+	info->current_clk = apb_clk / count;
+
+	__raw_writel(CLK_LOW(clk_low) | CLK_HIGH(clk_high),
+		     &info->adc_base_spear6xx->clk);
+}
+
+static void spear_adc_set_ctrl(struct spear_adc_info *info, int n,
+			       u32 val)
+{
+	__raw_writel(val, &info->adc_base_spear6xx->ch_ctrl[n]);
+}
+
+static u32 spear_adc_get_average(struct spear_adc_info *info)
+{
+	if (of_device_is_compatible(info->np, "st,spear600-adc")) {
+		return __raw_readl(&info->adc_base_spear6xx->average.msb) &
+			DATA_MASK;
+	} else {
+		return __raw_readl(&info->adc_base_spear3xx->average) &
+			DATA_MASK;
+	}
+}
+
+static void spear_adc_set_scanrate(struct spear_adc_info *info, u32 rate)
+{
+	if (of_device_is_compatible(info->np, "st,spear600-adc")) {
+		__raw_writel(SCAN_RATE_LO(rate),
+			     &info->adc_base_spear6xx->scan_rate_lo);
+		__raw_writel(SCAN_RATE_HI(rate),
+			     &info->adc_base_spear6xx->scan_rate_hi);
+	} else {
+		__raw_writel(rate, &info->adc_base_spear3xx->scan_rate);
+	}
+}
+
+static int spear_read_raw(struct iio_dev *indio_dev,
+			  struct iio_chan_spec const *chan,
+			  int *val,
+			  int *val2,
+			  long mask)
+{
+	struct spear_adc_info *info = iio_priv(indio_dev);
+	u32 scale_mv;
+	u32 status;
+
+	switch (mask) {
+	case IIO_CHAN_INFO_RAW:
+		mutex_lock(&indio_dev->mlock);
+
+		status = CHANNEL_NUM(chan->channel) |
+			AVG_SAMPLE(info->avg_samples) |
+			START_CONVERSION | ADC_ENABLE;
+		if (info->vref_external == 0)
+			status |= VREF_INTERNAL;
+
+		spear_adc_set_status(info, status);
+		wait_for_completion(&info->completion); /* set by ISR */
+		*val = info->value;
+
+		mutex_unlock(&indio_dev->mlock);
+
+		return IIO_VAL_INT;
+
+	case IIO_CHAN_INFO_SCALE:
+		scale_mv = (info->vref_external * 1000) >> DATA_BITS;
+		*val =  scale_mv / 1000;
+		*val2 = (scale_mv % 1000) * 1000;
+		return IIO_VAL_INT_PLUS_MICRO;
+	}
+
+	return -EINVAL;
+}
+
+#define SPEAR_ADC_CHAN(idx) {				\
+	.type = IIO_VOLTAGE,				\
+	.indexed = 1,					\
+	.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT |	\
+	IIO_CHAN_INFO_SCALE_SHARED_BIT,			\
+	.channel = idx,					\
+	.scan_type = {					\
+		.sign = 'u',				\
+		.storagebits = 16,			\
+	},						\
+}
+
+static struct iio_chan_spec spear_adc_iio_channels[] = {
+	SPEAR_ADC_CHAN(0),
+	SPEAR_ADC_CHAN(1),
+	SPEAR_ADC_CHAN(2),
+	SPEAR_ADC_CHAN(3),
+	SPEAR_ADC_CHAN(4),
+	SPEAR_ADC_CHAN(5),
+	SPEAR_ADC_CHAN(6),
+	SPEAR_ADC_CHAN(7),
+};
+
+static irqreturn_t spear_adc_isr(int irq, void *dev_id)
+{
+	struct spear_adc_info *info = (struct spear_adc_info *)dev_id;
+
+	/* Read value to clear IRQ */
+	info->value = spear_adc_get_average(info);
+	complete(&info->completion);
+
+	return IRQ_HANDLED;
+}
+
+static int spear_adc_configure(struct spear_adc_info *info)
+{
+	int i;
+
+	/* Reset ADC core */
+	spear_adc_set_status(info, 0);
+	__raw_writel(0, &info->adc_base_spear6xx->clk);
+	for (i = 0; i < 8; i++)
+		spear_adc_set_ctrl(info, i, 0);
+	spear_adc_set_scanrate(info, 0);
+
+	spear_adc_set_clk(info, info->sampling_freq);
+
+	return 0;
+}
+
+static ssize_t spear_adc_read_frequency(struct device *dev,
+					struct device_attribute *attr,
+					char *buf)
+{
+	struct iio_dev *indio_dev = dev_to_iio_dev(dev);
+	struct spear_adc_info *info = iio_priv(indio_dev);
+
+	return sprintf(buf, "%d\n", info->current_clk);
+}
+
+static ssize_t spear_adc_write_frequency(struct device *dev,
+					 struct device_attribute *attr,
+					 const char *buf,
+					 size_t len)
+{
+	struct iio_dev *indio_dev = dev_to_iio_dev(dev);
+	struct spear_adc_info *info = iio_priv(indio_dev);
+	u32 clk_high, clk_low, count;
+	u32 apb_clk = clk_get_rate(info->clk);
+	unsigned long lval;
+	int ret;
+
+	ret = kstrtoul(buf, 10, &lval);
+	if (ret)
+		return ret;
+
+	mutex_lock(&indio_dev->mlock);
+
+	if ((lval < CLK_MIN) || (lval > CLK_MAX)) {
+		ret = -EINVAL;
+		goto out;
+	}
+
+	count = (apb_clk + lval - 1) / lval;
+	clk_low = count / 2;
+	clk_high = count - clk_low;
+	info->current_clk = apb_clk / count;
+	spear_adc_set_clk(info, lval);
+
+out:
+	mutex_unlock(&indio_dev->mlock);
+
+	return ret ? ret : len;
+}
+
+static IIO_DEV_ATTR_SAMP_FREQ(S_IWUSR | S_IRUGO,
+			      spear_adc_read_frequency,
+			      spear_adc_write_frequency);
+
+static struct attribute *spear_attributes[] = {
+	&iio_dev_attr_sampling_frequency.dev_attr.attr,
+	NULL
+};
+
+static const struct attribute_group spear_attribute_group = {
+	.attrs = spear_attributes,
+};
+
+static const struct iio_info spear_adc_iio_info = {
+	.read_raw = &spear_read_raw,
+	.attrs = &spear_attribute_group,
+	.driver_module = THIS_MODULE,
+};
+
+static int __devinit spear_adc_probe(struct platform_device *pdev)
+{
+	struct device_node *np = pdev->dev.of_node;
+	struct device *dev = &pdev->dev;
+	struct spear_adc_info *info;
+	struct iio_dev *iodev = NULL;
+	int ret = -ENODEV;
+	int irq;
+
+	iodev = iio_device_alloc(sizeof(struct spear_adc_info));
+	if (!iodev) {
+		dev_err(dev, "failed allocating iio device\n");
+		ret = -ENOMEM;
+		goto errout1;
+	}
+
+	info = iio_priv(iodev);
+	info->np = np;
+
+	/*
+	 * SPEAr600 has a different register layout than other SPEAr SoC's
+	 * (e.g. SPEAr3xx). Let's provide two register base addresses
+	 * to support multi-arch kernels.
+	 */
+	info->adc_base_spear6xx = of_iomap(np, 0);
+	if (!info->adc_base_spear6xx) {
+		dev_err(dev, "failed mapping memory\n");
+		ret = -ENOMEM;
+		goto errout2;
+	}
+	info->adc_base_spear3xx =
+		(struct adc_regs_spear3xx *)info->adc_base_spear6xx;
+
+	info->clk = clk_get(dev, NULL);
+	if (IS_ERR(info->clk)) {
+		dev_err(dev, "failed getting clock\n");
+		goto errout3;
+	}
+
+	ret = clk_prepare(info->clk);
+	if (ret) {
+		dev_err(dev, "failed preparing clock\n");
+		goto errout4;
+	}
+
+	ret = clk_enable(info->clk);
+	if (ret) {
+		dev_err(dev, "failed enabling clock\n");
+		goto errout5;
+	}
+
+	irq = platform_get_irq(pdev, 0);
+	if ((irq < 0) || (irq >= NR_IRQS)) {
+		dev_err(dev, "failed getting interrupt resource\n");
+		ret = -EINVAL;
+		goto errout6;
+	}
+
+	ret = devm_request_irq(dev, irq, spear_adc_isr, 0, MOD_NAME, info);
+	if (ret < 0) {
+		dev_err(dev, "failed requesting interrupt\n");
+		goto errout6;
+	}
+
+	if (of_property_read_u32(np, "sampling-frequency",
+				 &info->sampling_freq)) {
+		dev_err(dev, "sampling-frequency missing in DT\n");
+		ret = -EINVAL;
+		goto errout6;
+	}
+
+	/*
+	 * Optional avg_samples defaults to 0, resulting in single data
+	 * conversion
+	 */
+	of_property_read_u32(np, "average-samples", &info->avg_samples);
+
+	/*
+	 * Optional vref_external defaults to 0, resulting in internal vref
+	 * selection
+	 */
+	of_property_read_u32(np, "vref-external", &info->vref_external);
+
+	spear_adc_configure(info);
+
+	platform_set_drvdata(pdev, iodev);
+
+	init_completion(&info->completion);
+
+	iodev->name = MOD_NAME;
+	iodev->dev.parent = dev;
+	iodev->info = &spear_adc_iio_info;
+	iodev->modes = INDIO_DIRECT_MODE;
+	iodev->channels = spear_adc_iio_channels;
+	iodev->num_channels = ARRAY_SIZE(spear_adc_iio_channels);
+
+	ret = iio_device_register(iodev);
+	if (ret)
+		goto errout6;
+
+	dev_info(dev, "SPEAR ADC driver loaded, IRQ %d\n", irq);
+
+	return 0;
+
+errout6:
+	clk_disable(info->clk);
+errout5:
+	clk_unprepare(info->clk);
+errout4:
+	clk_put(info->clk);
+errout3:
+	iounmap(info->adc_base_spear6xx);
+errout2:
+	iio_device_free(iodev);
+errout1:
+	return ret;
+}
+
+static int __devexit spear_adc_remove(struct platform_device *pdev)
+{
+	struct iio_dev *iodev = platform_get_drvdata(pdev);
+	struct spear_adc_info *info = iio_priv(iodev);
+
+	iio_device_unregister(iodev);
+	platform_set_drvdata(pdev, NULL);
+	clk_disable(info->clk);
+	clk_unprepare(info->clk);
+	clk_put(info->clk);
+	iounmap(info->adc_base_spear6xx);
+	iio_device_free(iodev);
+
+	return 0;
+}
+
+static const struct of_device_id spear_adc_dt_ids[] = {
+	{ .compatible = "st,spear600-adc", },
+	{ /* sentinel */ }
+};
+MODULE_DEVICE_TABLE(of, spear_adc_dt_ids);
+
+static struct platform_driver spear_adc_driver = {
+	.probe		= spear_adc_probe,
+	.remove		= __devexit_p(spear_adc_remove),
+	.driver		= {
+		.name	= MOD_NAME,
+		.owner	= THIS_MODULE,
+		.of_match_table = of_match_ptr(spear_adc_dt_ids),
+	},
+};
+
+module_platform_driver(spear_adc_driver);
+
+MODULE_AUTHOR("Stefan Roese <sr@denx.de>");
+MODULE_DESCRIPTION("SPEAr ADC driver");
+MODULE_LICENSE("GPL");
diff --git a/drivers/staging/iio/addac/adt7316.c b/drivers/staging/iio/addac/adt7316.c
index fd6a4544405..8fb014a046a 100644
--- a/drivers/staging/iio/addac/adt7316.c
+++ b/drivers/staging/iio/addac/adt7316.c
@@ -19,9 +19,9 @@
 #include <linux/rtc.h>
 #include <linux/module.h>
 
-#include "../iio.h"
-#include "../events.h"
-#include "../sysfs.h"
+#include <linux/iio/iio.h>
+#include <linux/iio/events.h>
+#include <linux/iio/sysfs.h>
 #include "adt7316.h"
 
 /*
@@ -220,7 +220,7 @@ static ssize_t adt7316_show_enabled(struct device *dev,
 		struct device_attribute *attr,
 		char *buf)
 {
-	struct iio_dev *dev_info = dev_get_drvdata(dev);
+	struct iio_dev *dev_info = dev_to_iio_dev(dev);
 	struct adt7316_chip_info *chip = iio_priv(dev_info);
 
 	return sprintf(buf, "%d\n", !!(chip->config1 & ADT7316_EN));
@@ -252,7 +252,7 @@ static ssize_t adt7316_store_enabled(struct device *dev,
 		const char *buf,
 		size_t len)
 {
-	struct iio_dev *dev_info = dev_get_drvdata(dev);
+	struct iio_dev *dev_info = dev_to_iio_dev(dev);
 	struct adt7316_chip_info *chip = iio_priv(dev_info);
 	int enable;
 
@@ -276,7 +276,7 @@ static ssize_t adt7316_show_select_ex_temp(struct device *dev,
 		struct device_attribute *attr,
 		char *buf)
 {
-	struct iio_dev *dev_info = dev_get_drvdata(dev);
+	struct iio_dev *dev_info = dev_to_iio_dev(dev);
 	struct adt7316_chip_info *chip = iio_priv(dev_info);
 
 	if ((chip->id & ID_FAMILY_MASK) != ID_ADT75XX)
@@ -290,7 +290,7 @@ static ssize_t adt7316_store_select_ex_temp(struct device *dev,
 		const char *buf,
 		size_t len)
 {
-	struct iio_dev *dev_info = dev_get_drvdata(dev);
+	struct iio_dev *dev_info = dev_to_iio_dev(dev);
 	struct adt7316_chip_info *chip = iio_priv(dev_info);
 	u8 config1;
 	int ret;
@@ -320,7 +320,7 @@ static ssize_t adt7316_show_mode(struct device *dev,
 		struct device_attribute *attr,
 		char *buf)
 {
-	struct iio_dev *dev_info = dev_get_drvdata(dev);
+	struct iio_dev *dev_info = dev_to_iio_dev(dev);
 	struct adt7316_chip_info *chip = iio_priv(dev_info);
 
 	if (chip->config2 & ADT7316_AD_SINGLE_CH_MODE)
@@ -334,7 +334,7 @@ static ssize_t adt7316_store_mode(struct device *dev,
 		const char *buf,
 		size_t len)
 {
-	struct iio_dev *dev_info = dev_get_drvdata(dev);
+	struct iio_dev *dev_info = dev_to_iio_dev(dev);
 	struct adt7316_chip_info *chip = iio_priv(dev_info);
 	u8 config2;
 	int ret;
@@ -370,7 +370,7 @@ static ssize_t adt7316_show_ad_channel(struct device *dev,
 		struct device_attribute *attr,
 		char *buf)
 {
-	struct iio_dev *dev_info = dev_get_drvdata(dev);
+	struct iio_dev *dev_info = dev_to_iio_dev(dev);
 	struct adt7316_chip_info *chip = iio_priv(dev_info);
 
 	if (!(chip->config2 & ADT7316_AD_SINGLE_CH_MODE))
@@ -409,7 +409,7 @@ static ssize_t adt7316_store_ad_channel(struct device *dev,
 		const char *buf,
 		size_t len)
 {
-	struct iio_dev *dev_info = dev_get_drvdata(dev);
+	struct iio_dev *dev_info = dev_to_iio_dev(dev);
 	struct adt7316_chip_info *chip = iio_priv(dev_info);
 	u8 config2;
 	unsigned long data = 0;
@@ -455,7 +455,7 @@ static ssize_t adt7316_show_all_ad_channels(struct device *dev,
 		struct device_attribute *attr,
 		char *buf)
 {
-	struct iio_dev *dev_info = dev_get_drvdata(dev);
+	struct iio_dev *dev_info = dev_to_iio_dev(dev);
 	struct adt7316_chip_info *chip = iio_priv(dev_info);
 
 	if (!(chip->config2 & ADT7316_AD_SINGLE_CH_MODE))
@@ -477,7 +477,7 @@ static ssize_t adt7316_show_disable_averaging(struct device *dev,
 		struct device_attribute *attr,
 		char *buf)
 {
-	struct iio_dev *dev_info = dev_get_drvdata(dev);
+	struct iio_dev *dev_info = dev_to_iio_dev(dev);
 	struct adt7316_chip_info *chip = iio_priv(dev_info);
 
 	return sprintf(buf, "%d\n",
@@ -489,7 +489,7 @@ static ssize_t adt7316_store_disable_averaging(struct device *dev,
 		const char *buf,
 		size_t len)
 {
-	struct iio_dev *dev_info = dev_get_drvdata(dev);
+	struct iio_dev *dev_info = dev_to_iio_dev(dev);
 	struct adt7316_chip_info *chip = iio_priv(dev_info);
 	u8 config2;
 	int ret;
@@ -516,7 +516,7 @@ static ssize_t adt7316_show_enable_smbus_timeout(struct device *dev,
 		struct device_attribute *attr,
 		char *buf)
 {
-	struct iio_dev *dev_info = dev_get_drvdata(dev);
+	struct iio_dev *dev_info = dev_to_iio_dev(dev);
 	struct adt7316_chip_info *chip = iio_priv(dev_info);
 
 	return sprintf(buf, "%d\n",
@@ -528,7 +528,7 @@ static ssize_t adt7316_store_enable_smbus_timeout(struct device *dev,
 		const char *buf,
 		size_t len)
 {
-	struct iio_dev *dev_info = dev_get_drvdata(dev);
+	struct iio_dev *dev_info = dev_to_iio_dev(dev);
 	struct adt7316_chip_info *chip = iio_priv(dev_info);
 	u8 config2;
 	int ret;
@@ -557,7 +557,7 @@ static ssize_t adt7316_store_reset(struct device *dev,
 		const char *buf,
 		size_t len)
 {
-	struct iio_dev *dev_info = dev_get_drvdata(dev);
+	struct iio_dev *dev_info = dev_to_iio_dev(dev);
 	struct adt7316_chip_info *chip = iio_priv(dev_info);
 	u8 config2;
 	int ret;
@@ -580,7 +580,7 @@ static ssize_t adt7316_show_powerdown(struct device *dev,
 		struct device_attribute *attr,
 		char *buf)
 {
-	struct iio_dev *dev_info = dev_get_drvdata(dev);
+	struct iio_dev *dev_info = dev_to_iio_dev(dev);
 	struct adt7316_chip_info *chip = iio_priv(dev_info);
 
 	return sprintf(buf, "%d\n", !!(chip->config1 & ADT7316_PD));
@@ -591,7 +591,7 @@ static ssize_t adt7316_store_powerdown(struct device *dev,
 		const char *buf,
 		size_t len)
 {
-	struct iio_dev *dev_info = dev_get_drvdata(dev);
+	struct iio_dev *dev_info = dev_to_iio_dev(dev);
 	struct adt7316_chip_info *chip = iio_priv(dev_info);
 	u8 config1;
 	int ret;
@@ -618,7 +618,7 @@ static ssize_t adt7316_show_fast_ad_clock(struct device *dev,
 		struct device_attribute *attr,
 		char *buf)
 {
-	struct iio_dev *dev_info = dev_get_drvdata(dev);
+	struct iio_dev *dev_info = dev_to_iio_dev(dev);
 	struct adt7316_chip_info *chip = iio_priv(dev_info);
 
 	return sprintf(buf, "%d\n", !!(chip->config3 & ADT7316_ADCLK_22_5));
@@ -629,7 +629,7 @@ static ssize_t adt7316_store_fast_ad_clock(struct device *dev,
 		const char *buf,
 		size_t len)
 {
-	struct iio_dev *dev_info = dev_get_drvdata(dev);
+	struct iio_dev *dev_info = dev_to_iio_dev(dev);
 	struct adt7316_chip_info *chip = iio_priv(dev_info);
 	u8 config3;
 	int ret;
@@ -656,7 +656,7 @@ static ssize_t adt7316_show_da_high_resolution(struct device *dev,
 		struct device_attribute *attr,
 		char *buf)
 {
-	struct iio_dev *dev_info = dev_get_drvdata(dev);
+	struct iio_dev *dev_info = dev_to_iio_dev(dev);
 	struct adt7316_chip_info *chip = iio_priv(dev_info);
 
 	if (chip->config3 & ADT7316_DA_HIGH_RESOLUTION) {
@@ -674,7 +674,7 @@ static ssize_t adt7316_store_da_high_resolution(struct device *dev,
 		const char *buf,
 		size_t len)
 {
-	struct iio_dev *dev_info = dev_get_drvdata(dev);
+	struct iio_dev *dev_info = dev_to_iio_dev(dev);
 	struct adt7316_chip_info *chip = iio_priv(dev_info);
 	u8 config3;
 	int ret;
@@ -708,7 +708,7 @@ static ssize_t adt7316_show_AIN_internal_Vref(struct device *dev,
 		struct device_attribute *attr,
 		char *buf)
 {
-	struct iio_dev *dev_info = dev_get_drvdata(dev);
+	struct iio_dev *dev_info = dev_to_iio_dev(dev);
 	struct adt7316_chip_info *chip = iio_priv(dev_info);
 
 	if ((chip->id & ID_FAMILY_MASK) != ID_ADT75XX)
@@ -723,7 +723,7 @@ static ssize_t adt7316_store_AIN_internal_Vref(struct device *dev,
 		const char *buf,
 		size_t len)
 {
-	struct iio_dev *dev_info = dev_get_drvdata(dev);
+	struct iio_dev *dev_info = dev_to_iio_dev(dev);
 	struct adt7316_chip_info *chip = iio_priv(dev_info);
 	u8 config3;
 	int ret;
@@ -755,7 +755,7 @@ static ssize_t adt7316_show_enable_prop_DACA(struct device *dev,
 		struct device_attribute *attr,
 		char *buf)
 {
-	struct iio_dev *dev_info = dev_get_drvdata(dev);
+	struct iio_dev *dev_info = dev_to_iio_dev(dev);
 	struct adt7316_chip_info *chip = iio_priv(dev_info);
 
 	return sprintf(buf, "%d\n",
@@ -767,7 +767,7 @@ static ssize_t adt7316_store_enable_prop_DACA(struct device *dev,
 		const char *buf,
 		size_t len)
 {
-	struct iio_dev *dev_info = dev_get_drvdata(dev);
+	struct iio_dev *dev_info = dev_to_iio_dev(dev);
 	struct adt7316_chip_info *chip = iio_priv(dev_info);
 	u8 config3;
 	int ret;
@@ -794,7 +794,7 @@ static ssize_t adt7316_show_enable_prop_DACB(struct device *dev,
 		struct device_attribute *attr,
 		char *buf)
 {
-	struct iio_dev *dev_info = dev_get_drvdata(dev);
+	struct iio_dev *dev_info = dev_to_iio_dev(dev);
 	struct adt7316_chip_info *chip = iio_priv(dev_info);
 
 	return sprintf(buf, "%d\n",
@@ -806,7 +806,7 @@ static ssize_t adt7316_store_enable_prop_DACB(struct device *dev,
 		const char *buf,
 		size_t len)
 {
-	struct iio_dev *dev_info = dev_get_drvdata(dev);
+	struct iio_dev *dev_info = dev_to_iio_dev(dev);
 	struct adt7316_chip_info *chip = iio_priv(dev_info);
 	u8 config3;
 	int ret;
@@ -833,7 +833,7 @@ static ssize_t adt7316_show_DAC_2Vref_ch_mask(struct device *dev,
 		struct device_attribute *attr,
 		char *buf)
 {
-	struct iio_dev *dev_info = dev_get_drvdata(dev);
+	struct iio_dev *dev_info = dev_to_iio_dev(dev);
 	struct adt7316_chip_info *chip = iio_priv(dev_info);
 
 	return sprintf(buf, "0x%x\n",
@@ -845,7 +845,7 @@ static ssize_t adt7316_store_DAC_2Vref_ch_mask(struct device *dev,
 		const char *buf,
 		size_t len)
 {
-	struct iio_dev *dev_info = dev_get_drvdata(dev);
+	struct iio_dev *dev_info = dev_to_iio_dev(dev);
 	struct adt7316_chip_info *chip = iio_priv(dev_info);
 	u8 dac_config;
 	unsigned long data = 0;
@@ -876,7 +876,7 @@ static ssize_t adt7316_show_DAC_update_mode(struct device *dev,
 		struct device_attribute *attr,
 		char *buf)
 {
-	struct iio_dev *dev_info = dev_get_drvdata(dev);
+	struct iio_dev *dev_info = dev_to_iio_dev(dev);
 	struct adt7316_chip_info *chip = iio_priv(dev_info);
 
 	if (!(chip->config3 & ADT7316_DA_EN_VIA_DAC_LDCA))
@@ -900,7 +900,7 @@ static ssize_t adt7316_store_DAC_update_mode(struct device *dev,
 		const char *buf,
 		size_t len)
 {
-	struct iio_dev *dev_info = dev_get_drvdata(dev);
+	struct iio_dev *dev_info = dev_to_iio_dev(dev);
 	struct adt7316_chip_info *chip = iio_priv(dev_info);
 	u8 dac_config;
 	unsigned long data;
@@ -934,7 +934,7 @@ static ssize_t adt7316_show_all_DAC_update_modes(struct device *dev,
 		struct device_attribute *attr,
 		char *buf)
 {
-	struct iio_dev *dev_info = dev_get_drvdata(dev);
+	struct iio_dev *dev_info = dev_to_iio_dev(dev);
 	struct adt7316_chip_info *chip = iio_priv(dev_info);
 
 	if (chip->config3 & ADT7316_DA_EN_VIA_DAC_LDCA)
@@ -955,7 +955,7 @@ static ssize_t adt7316_store_update_DAC(struct device *dev,
 		const char *buf,
 		size_t len)
 {
-	struct iio_dev *dev_info = dev_get_drvdata(dev);
+	struct iio_dev *dev_info = dev_to_iio_dev(dev);
 	struct adt7316_chip_info *chip = iio_priv(dev_info);
 	u8 ldac_config;
 	unsigned long data;
@@ -994,7 +994,7 @@ static ssize_t adt7316_show_DA_AB_Vref_bypass(struct device *dev,
 		struct device_attribute *attr,
 		char *buf)
 {
-	struct iio_dev *dev_info = dev_get_drvdata(dev);
+	struct iio_dev *dev_info = dev_to_iio_dev(dev);
 	struct adt7316_chip_info *chip = iio_priv(dev_info);
 
 	if ((chip->id & ID_FAMILY_MASK) == ID_ADT75XX)
@@ -1009,7 +1009,7 @@ static ssize_t adt7316_store_DA_AB_Vref_bypass(struct device *dev,
 		const char *buf,
 		size_t len)
 {
-	struct iio_dev *dev_info = dev_get_drvdata(dev);
+	struct iio_dev *dev_info = dev_to_iio_dev(dev);
 	struct adt7316_chip_info *chip = iio_priv(dev_info);
 	u8 dac_config;
 	int ret;
@@ -1039,7 +1039,7 @@ static ssize_t adt7316_show_DA_CD_Vref_bypass(struct device *dev,
 		struct device_attribute *attr,
 		char *buf)
 {
-	struct iio_dev *dev_info = dev_get_drvdata(dev);
+	struct iio_dev *dev_info = dev_to_iio_dev(dev);
 	struct adt7316_chip_info *chip = iio_priv(dev_info);
 
 	if ((chip->id & ID_FAMILY_MASK) == ID_ADT75XX)
@@ -1054,7 +1054,7 @@ static ssize_t adt7316_store_DA_CD_Vref_bypass(struct device *dev,
 		const char *buf,
 		size_t len)
 {
-	struct iio_dev *dev_info = dev_get_drvdata(dev);
+	struct iio_dev *dev_info = dev_to_iio_dev(dev);
 	struct adt7316_chip_info *chip = iio_priv(dev_info);
 	u8 dac_config;
 	int ret;
@@ -1084,7 +1084,7 @@ static ssize_t adt7316_show_DAC_internal_Vref(struct device *dev,
 		struct device_attribute *attr,
 		char *buf)
 {
-	struct iio_dev *dev_info = dev_get_drvdata(dev);
+	struct iio_dev *dev_info = dev_to_iio_dev(dev);
 	struct adt7316_chip_info *chip = iio_priv(dev_info);
 
 	if ((chip->id & ID_FAMILY_MASK) == ID_ADT75XX)
@@ -1101,7 +1101,7 @@ static ssize_t adt7316_store_DAC_internal_Vref(struct device *dev,
 		const char *buf,
 		size_t len)
 {
-	struct iio_dev *dev_info = dev_get_drvdata(dev);
+	struct iio_dev *dev_info = dev_to_iio_dev(dev);
 	struct adt7316_chip_info *chip = iio_priv(dev_info);
 	u8 ldac_config;
 	unsigned long data;
@@ -1220,7 +1220,7 @@ static ssize_t adt7316_show_VDD(struct device *dev,
 		struct device_attribute *attr,
 		char *buf)
 {
-	struct iio_dev *dev_info = dev_get_drvdata(dev);
+	struct iio_dev *dev_info = dev_to_iio_dev(dev);
 	struct adt7316_chip_info *chip = iio_priv(dev_info);
 
 	return adt7316_show_ad(chip, ADT7316_AD_SINGLE_CH_VDD, buf);
@@ -1231,7 +1231,7 @@ static ssize_t adt7316_show_in_temp(struct device *dev,
 		struct device_attribute *attr,
 		char *buf)
 {
-	struct iio_dev *dev_info = dev_get_drvdata(dev);
+	struct iio_dev *dev_info = dev_to_iio_dev(dev);
 	struct adt7316_chip_info *chip = iio_priv(dev_info);
 
 	return adt7316_show_ad(chip, ADT7316_AD_SINGLE_CH_IN, buf);
@@ -1243,7 +1243,7 @@ static ssize_t adt7316_show_ex_temp_AIN1(struct device *dev,
 		struct device_attribute *attr,
 		char *buf)
 {
-	struct iio_dev *dev_info = dev_get_drvdata(dev);
+	struct iio_dev *dev_info = dev_to_iio_dev(dev);
 	struct adt7316_chip_info *chip = iio_priv(dev_info);
 
 	return adt7316_show_ad(chip, ADT7316_AD_SINGLE_CH_EX, buf);
@@ -1256,7 +1256,7 @@ static ssize_t adt7316_show_AIN2(struct device *dev,
 		struct device_attribute *attr,
 		char *buf)
 {
-	struct iio_dev *dev_info = dev_get_drvdata(dev);
+	struct iio_dev *dev_info = dev_to_iio_dev(dev);
 	struct adt7316_chip_info *chip = iio_priv(dev_info);
 
 	return adt7316_show_ad(chip, ADT7516_AD_SINGLE_CH_AIN2, buf);
@@ -1267,7 +1267,7 @@ static ssize_t adt7316_show_AIN3(struct device *dev,
 		struct device_attribute *attr,
 		char *buf)
 {
-	struct iio_dev *dev_info = dev_get_drvdata(dev);
+	struct iio_dev *dev_info = dev_to_iio_dev(dev);
 	struct adt7316_chip_info *chip = iio_priv(dev_info);
 
 	return adt7316_show_ad(chip, ADT7516_AD_SINGLE_CH_AIN3, buf);
@@ -1278,7 +1278,7 @@ static ssize_t adt7316_show_AIN4(struct device *dev,
 		struct device_attribute *attr,
 		char *buf)
 {
-	struct iio_dev *dev_info = dev_get_drvdata(dev);
+	struct iio_dev *dev_info = dev_to_iio_dev(dev);
 	struct adt7316_chip_info *chip = iio_priv(dev_info);
 
 	return adt7316_show_ad(chip, ADT7516_AD_SINGLE_CH_AIN4, buf);
@@ -1330,7 +1330,7 @@ static ssize_t adt7316_show_in_temp_offset(struct device *dev,
 		struct device_attribute *attr,
 		char *buf)
 {
-	struct iio_dev *dev_info = dev_get_drvdata(dev);
+	struct iio_dev *dev_info = dev_to_iio_dev(dev);
 	struct adt7316_chip_info *chip = iio_priv(dev_info);
 
 	return adt7316_show_temp_offset(chip, ADT7316_IN_TEMP_OFFSET, buf);
@@ -1341,7 +1341,7 @@ static ssize_t adt7316_store_in_temp_offset(struct device *dev,
 		const char *buf,
 		size_t len)
 {
-	struct iio_dev *dev_info = dev_get_drvdata(dev);
+	struct iio_dev *dev_info = dev_to_iio_dev(dev);
 	struct adt7316_chip_info *chip = iio_priv(dev_info);
 
 	return adt7316_store_temp_offset(chip, ADT7316_IN_TEMP_OFFSET, buf, len);
@@ -1355,7 +1355,7 @@ static ssize_t adt7316_show_ex_temp_offset(struct device *dev,
 		struct device_attribute *attr,
 		char *buf)
 {
-	struct iio_dev *dev_info = dev_get_drvdata(dev);
+	struct iio_dev *dev_info = dev_to_iio_dev(dev);
 	struct adt7316_chip_info *chip = iio_priv(dev_info);
 
 	return adt7316_show_temp_offset(chip, ADT7316_EX_TEMP_OFFSET, buf);
@@ -1366,7 +1366,7 @@ static ssize_t adt7316_store_ex_temp_offset(struct device *dev,
 		const char *buf,
 		size_t len)
 {
-	struct iio_dev *dev_info = dev_get_drvdata(dev);
+	struct iio_dev *dev_info = dev_to_iio_dev(dev);
 	struct adt7316_chip_info *chip = iio_priv(dev_info);
 
 	return adt7316_store_temp_offset(chip, ADT7316_EX_TEMP_OFFSET, buf, len);
@@ -1380,7 +1380,7 @@ static ssize_t adt7316_show_in_analog_temp_offset(struct device *dev,
 		struct device_attribute *attr,
 		char *buf)
 {
-	struct iio_dev *dev_info = dev_get_drvdata(dev);
+	struct iio_dev *dev_info = dev_to_iio_dev(dev);
 	struct adt7316_chip_info *chip = iio_priv(dev_info);
 
 	return adt7316_show_temp_offset(chip,
@@ -1392,7 +1392,7 @@ static ssize_t adt7316_store_in_analog_temp_offset(struct device *dev,
 		const char *buf,
 		size_t len)
 {
-	struct iio_dev *dev_info = dev_get_drvdata(dev);
+	struct iio_dev *dev_info = dev_to_iio_dev(dev);
 	struct adt7316_chip_info *chip = iio_priv(dev_info);
 
 	return adt7316_store_temp_offset(chip,
@@ -1407,7 +1407,7 @@ static ssize_t adt7316_show_ex_analog_temp_offset(struct device *dev,
 		struct device_attribute *attr,
 		char *buf)
 {
-	struct iio_dev *dev_info = dev_get_drvdata(dev);
+	struct iio_dev *dev_info = dev_to_iio_dev(dev);
 	struct adt7316_chip_info *chip = iio_priv(dev_info);
 
 	return adt7316_show_temp_offset(chip,
@@ -1419,7 +1419,7 @@ static ssize_t adt7316_store_ex_analog_temp_offset(struct device *dev,
 		const char *buf,
 		size_t len)
 {
-	struct iio_dev *dev_info = dev_get_drvdata(dev);
+	struct iio_dev *dev_info = dev_to_iio_dev(dev);
 	struct adt7316_chip_info *chip = iio_priv(dev_info);
 
 	return adt7316_store_temp_offset(chip,
@@ -1504,7 +1504,7 @@ static ssize_t adt7316_show_DAC_A(struct device *dev,
 		struct device_attribute *attr,
 		char *buf)
 {
-	struct iio_dev *dev_info = dev_get_drvdata(dev);
+	struct iio_dev *dev_info = dev_to_iio_dev(dev);
 	struct adt7316_chip_info *chip = iio_priv(dev_info);
 
 	return adt7316_show_DAC(chip, 0, buf);
@@ -1515,7 +1515,7 @@ static ssize_t adt7316_store_DAC_A(struct device *dev,
 		const char *buf,
 		size_t len)
 {
-	struct iio_dev *dev_info = dev_get_drvdata(dev);
+	struct iio_dev *dev_info = dev_to_iio_dev(dev);
 	struct adt7316_chip_info *chip = iio_priv(dev_info);
 
 	return adt7316_store_DAC(chip, 0, buf, len);
@@ -1528,7 +1528,7 @@ static ssize_t adt7316_show_DAC_B(struct device *dev,
 		struct device_attribute *attr,
 		char *buf)
 {
-	struct iio_dev *dev_info = dev_get_drvdata(dev);
+	struct iio_dev *dev_info = dev_to_iio_dev(dev);
 	struct adt7316_chip_info *chip = iio_priv(dev_info);
 
 	return adt7316_show_DAC(chip, 1, buf);
@@ -1539,7 +1539,7 @@ static ssize_t adt7316_store_DAC_B(struct device *dev,
 		const char *buf,
 		size_t len)
 {
-	struct iio_dev *dev_info = dev_get_drvdata(dev);
+	struct iio_dev *dev_info = dev_to_iio_dev(dev);
 	struct adt7316_chip_info *chip = iio_priv(dev_info);
 
 	return adt7316_store_DAC(chip, 1, buf, len);
@@ -1552,7 +1552,7 @@ static ssize_t adt7316_show_DAC_C(struct device *dev,
 		struct device_attribute *attr,
 		char *buf)
 {
-	struct iio_dev *dev_info = dev_get_drvdata(dev);
+	struct iio_dev *dev_info = dev_to_iio_dev(dev);
 	struct adt7316_chip_info *chip = iio_priv(dev_info);
 
 	return adt7316_show_DAC(chip, 2, buf);
@@ -1563,7 +1563,7 @@ static ssize_t adt7316_store_DAC_C(struct device *dev,
 		const char *buf,
 		size_t len)
 {
-	struct iio_dev *dev_info = dev_get_drvdata(dev);
+	struct iio_dev *dev_info = dev_to_iio_dev(dev);
 	struct adt7316_chip_info *chip = iio_priv(dev_info);
 
 	return adt7316_store_DAC(chip, 2, buf, len);
@@ -1576,7 +1576,7 @@ static ssize_t adt7316_show_DAC_D(struct device *dev,
 		struct device_attribute *attr,
 		char *buf)
 {
-	struct iio_dev *dev_info = dev_get_drvdata(dev);
+	struct iio_dev *dev_info = dev_to_iio_dev(dev);
 	struct adt7316_chip_info *chip = iio_priv(dev_info);
 
 	return adt7316_show_DAC(chip, 3, buf);
@@ -1587,7 +1587,7 @@ static ssize_t adt7316_store_DAC_D(struct device *dev,
 		const char *buf,
 		size_t len)
 {
-	struct iio_dev *dev_info = dev_get_drvdata(dev);
+	struct iio_dev *dev_info = dev_to_iio_dev(dev);
 	struct adt7316_chip_info *chip = iio_priv(dev_info);
 
 	return adt7316_store_DAC(chip, 3, buf, len);
@@ -1600,7 +1600,7 @@ static ssize_t adt7316_show_device_id(struct device *dev,
 		struct device_attribute *attr,
 		char *buf)
 {
-	struct iio_dev *dev_info = dev_get_drvdata(dev);
+	struct iio_dev *dev_info = dev_to_iio_dev(dev);
 	struct adt7316_chip_info *chip = iio_priv(dev_info);
 	u8 id;
 	int ret;
@@ -1618,7 +1618,7 @@ static ssize_t adt7316_show_manufactorer_id(struct device *dev,
 		struct device_attribute *attr,
 		char *buf)
 {
-	struct iio_dev *dev_info = dev_get_drvdata(dev);
+	struct iio_dev *dev_info = dev_to_iio_dev(dev);
 	struct adt7316_chip_info *chip = iio_priv(dev_info);
 	u8 id;
 	int ret;
@@ -1637,7 +1637,7 @@ static ssize_t adt7316_show_device_rev(struct device *dev,
 		struct device_attribute *attr,
 		char *buf)
 {
-	struct iio_dev *dev_info = dev_get_drvdata(dev);
+	struct iio_dev *dev_info = dev_to_iio_dev(dev);
 	struct adt7316_chip_info *chip = iio_priv(dev_info);
 	u8 rev;
 	int ret;
@@ -1655,7 +1655,7 @@ static ssize_t adt7316_show_bus_type(struct device *dev,
 		struct device_attribute *attr,
 		char *buf)
 {
-	struct iio_dev *dev_info = dev_get_drvdata(dev);
+	struct iio_dev *dev_info = dev_to_iio_dev(dev);
 	struct adt7316_chip_info *chip = iio_priv(dev_info);
 	u8 stat;
 	int ret;
@@ -1841,7 +1841,7 @@ static ssize_t adt7316_show_int_mask(struct device *dev,
 		struct device_attribute *attr,
 		char *buf)
 {
-	struct iio_dev *dev_info = dev_get_drvdata(dev);
+	struct iio_dev *dev_info = dev_to_iio_dev(dev);
 	struct adt7316_chip_info *chip = iio_priv(dev_info);
 
 	return sprintf(buf, "0x%x\n", chip->int_mask);
@@ -1855,7 +1855,7 @@ static ssize_t adt7316_set_int_mask(struct device *dev,
 		const char *buf,
 		size_t len)
 {
-	struct iio_dev *dev_info = dev_get_drvdata(dev);
+	struct iio_dev *dev_info = dev_to_iio_dev(dev);
 	struct adt7316_chip_info *chip = iio_priv(dev_info);
 	unsigned long data;
 	int ret;
@@ -1895,7 +1895,7 @@ static inline ssize_t adt7316_show_ad_bound(struct device *dev,
 		char *buf)
 {
 	struct iio_dev_attr *this_attr = to_iio_dev_attr(attr);
-	struct iio_dev *dev_info = dev_get_drvdata(dev);
+	struct iio_dev *dev_info = dev_to_iio_dev(dev);
 	struct adt7316_chip_info *chip = iio_priv(dev_info);
 	u8 val;
 	int data;
@@ -1926,7 +1926,7 @@ static inline ssize_t adt7316_set_ad_bound(struct device *dev,
 		size_t len)
 {
 	struct iio_dev_attr *this_attr = to_iio_dev_attr(attr);
-	struct iio_dev *dev_info = dev_get_drvdata(dev);
+	struct iio_dev *dev_info = dev_to_iio_dev(dev);
 	struct adt7316_chip_info *chip = iio_priv(dev_info);
 	long data;
 	u8 val;
@@ -1965,7 +1965,7 @@ static ssize_t adt7316_show_int_enabled(struct device *dev,
 		struct device_attribute *attr,
 		char *buf)
 {
-	struct iio_dev *dev_info = dev_get_drvdata(dev);
+	struct iio_dev *dev_info = dev_to_iio_dev(dev);
 	struct adt7316_chip_info *chip = iio_priv(dev_info);
 
 	return sprintf(buf, "%d\n", !!(chip->config1 & ADT7316_INT_EN));
@@ -1976,7 +1976,7 @@ static ssize_t adt7316_set_int_enabled(struct device *dev,
 		const char *buf,
 		size_t len)
 {
-	struct iio_dev *dev_info = dev_get_drvdata(dev);
+	struct iio_dev *dev_info = dev_to_iio_dev(dev);
 	struct adt7316_chip_info *chip = iio_priv(dev_info);
 	u8 config1;
 	int ret;
@@ -2133,7 +2133,7 @@ int __devinit adt7316_probe(struct device *dev, struct adt7316_bus *bus,
 	unsigned short *adt7316_platform_data = dev->platform_data;
 	int ret = 0;
 
-	indio_dev = iio_allocate_device(sizeof(*chip));
+	indio_dev = iio_device_alloc(sizeof(*chip));
 	if (indio_dev == NULL) {
 		ret = -ENOMEM;
 		goto error_ret;
@@ -2210,7 +2210,7 @@ int __devinit adt7316_probe(struct device *dev, struct adt7316_bus *bus,
 error_unreg_irq:
 	free_irq(chip->bus.irq, indio_dev);
 error_free_dev:
-	iio_free_device(indio_dev);
+	iio_device_free(indio_dev);
 error_ret:
 	return ret;
 }
@@ -2224,7 +2224,7 @@ int __devexit adt7316_remove(struct device *dev)
 	iio_device_unregister(indio_dev);
 	if (chip->bus.irq)
 		free_irq(chip->bus.irq, indio_dev);
-	iio_free_device(indio_dev);
+	iio_device_free(indio_dev);
 
 	return 0;
 }
diff --git a/drivers/staging/iio/buffer.h b/drivers/staging/iio/buffer.h
deleted file mode 100644
index df2046dcb62..00000000000
--- a/drivers/staging/iio/buffer.h
+++ /dev/null
@@ -1,193 +0,0 @@
-/* The industrial I/O core - generic buffer interfaces.
- *
- * Copyright (c) 2008 Jonathan Cameron
- *
- * 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 _IIO_BUFFER_GENERIC_H_
-#define _IIO_BUFFER_GENERIC_H_
-#include <linux/sysfs.h>
-#include "iio.h"
-
-#ifdef CONFIG_IIO_BUFFER
-
-struct iio_buffer;
-
-/**
- * struct iio_buffer_access_funcs - access functions for buffers.
- * @store_to:		actually store stuff to the buffer
- * @read_first_n:	try to get a specified number of bytes (must exist)
- * @request_update:	if a parameter change has been marked, update underlying
- *			storage.
- * @get_bytes_per_datum:get current bytes per datum
- * @set_bytes_per_datum:set number of bytes per datum
- * @get_length:		get number of datums in buffer
- * @set_length:		set number of datums in buffer
- *
- * The purpose of this structure is to make the buffer element
- * modular as event for a given driver, different usecases may require
- * different buffer designs (space efficiency vs speed for example).
- *
- * It is worth noting that a given buffer implementation may only support a
- * small proportion of these functions.  The core code 'should' cope fine with
- * any of them not existing.
- **/
-struct iio_buffer_access_funcs {
-	int (*store_to)(struct iio_buffer *buffer, u8 *data, s64 timestamp);
-	int (*read_first_n)(struct iio_buffer *buffer,
-			    size_t n,
-			    char __user *buf);
-
-	int (*request_update)(struct iio_buffer *buffer);
-
-	int (*get_bytes_per_datum)(struct iio_buffer *buffer);
-	int (*set_bytes_per_datum)(struct iio_buffer *buffer, size_t bpd);
-	int (*get_length)(struct iio_buffer *buffer);
-	int (*set_length)(struct iio_buffer *buffer, int length);
-};
-
-/**
- * struct iio_buffer - general buffer structure
- * @length:		[DEVICE] number of datums in buffer
- * @bytes_per_datum:	[DEVICE] size of individual datum including timestamp
- * @scan_el_attrs:	[DRIVER] control of scan elements if that scan mode
- *			control method is used
- * @scan_mask:		[INTERN] bitmask used in masking scan mode elements
- * @scan_index_timestamp:[INTERN] cache of the index to the timestamp
- * @scan_timestamp:	[INTERN] does the scan mode include a timestamp
- * @access:		[DRIVER] buffer access functions associated with the
- *			implementation.
- * @scan_el_dev_attr_list:[INTERN] list of scan element related attributes.
- * @scan_el_group:	[DRIVER] attribute group for those attributes not
- *			created from the iio_chan_info array.
- * @pollq:		[INTERN] wait queue to allow for polling on the buffer.
- * @stufftoread:	[INTERN] flag to indicate new data.
- * @demux_list:		[INTERN] list of operations required to demux the scan.
- * @demux_bounce:	[INTERN] buffer for doing gather from incoming scan.
- **/
-struct iio_buffer {
-	int					length;
-	int					bytes_per_datum;
-	struct attribute_group			*scan_el_attrs;
-	long					*scan_mask;
-	bool					scan_timestamp;
-	unsigned				scan_index_timestamp;
-	const struct iio_buffer_access_funcs	*access;
-	struct list_head			scan_el_dev_attr_list;
-	struct attribute_group			scan_el_group;
-	wait_queue_head_t			pollq;
-	bool					stufftoread;
-	const struct attribute_group *attrs;
-	struct list_head			demux_list;
-	unsigned char				*demux_bounce;
-};
-
-/**
- * iio_buffer_init() - Initialize the buffer structure
- * @buffer: buffer to be initialized
- **/
-void iio_buffer_init(struct iio_buffer *buffer);
-
-/**
- * __iio_update_buffer() - update common elements of buffers
- * @buffer:		buffer that is the event source
- * @bytes_per_datum:	size of individual datum including timestamp
- * @length:		number of datums in buffer
- **/
-static inline void __iio_update_buffer(struct iio_buffer *buffer,
-				       int bytes_per_datum, int length)
-{
-	buffer->bytes_per_datum = bytes_per_datum;
-	buffer->length = length;
-}
-
-int iio_scan_mask_query(struct iio_dev *indio_dev,
-			struct iio_buffer *buffer, int bit);
-
-/**
- * iio_scan_mask_set() - set particular bit in the scan mask
- * @buffer: the buffer whose scan mask we are interested in
- * @bit: the bit to be set.
- **/
-int iio_scan_mask_set(struct iio_dev *indio_dev,
-		      struct iio_buffer *buffer, int bit);
-
-/**
- * iio_push_to_buffer() - push to a registered buffer.
- * @buffer:		IIO buffer structure for device
- * @scan:		Full scan.
- * @timestamp:
- */
-int iio_push_to_buffer(struct iio_buffer *buffer, unsigned char *data,
-		       s64 timestamp);
-
-int iio_update_demux(struct iio_dev *indio_dev);
-
-/**
- * iio_buffer_register() - register the buffer with IIO core
- * @indio_dev: device with the buffer to be registered
- **/
-int iio_buffer_register(struct iio_dev *indio_dev,
-			const struct iio_chan_spec *channels,
-			int num_channels);
-
-/**
- * iio_buffer_unregister() - unregister the buffer from IIO core
- * @indio_dev: the device with the buffer to be unregistered
- **/
-void iio_buffer_unregister(struct iio_dev *indio_dev);
-
-/**
- * iio_buffer_read_length() - attr func to get number of datums in the buffer
- **/
-ssize_t iio_buffer_read_length(struct device *dev,
-			       struct device_attribute *attr,
-			       char *buf);
-/**
- * iio_buffer_write_length() - attr func to set number of datums in the buffer
- **/
-ssize_t iio_buffer_write_length(struct device *dev,
-			      struct device_attribute *attr,
-			      const char *buf,
-			      size_t len);
-/**
- * iio_buffer_store_enable() - attr to turn the buffer on
- **/
-ssize_t iio_buffer_store_enable(struct device *dev,
-				struct device_attribute *attr,
-				const char *buf,
-				size_t len);
-/**
- * iio_buffer_show_enable() - attr to see if the buffer is on
- **/
-ssize_t iio_buffer_show_enable(struct device *dev,
-			       struct device_attribute *attr,
-			       char *buf);
-#define IIO_BUFFER_LENGTH_ATTR DEVICE_ATTR(length, S_IRUGO | S_IWUSR,	\
-					   iio_buffer_read_length,	\
-					   iio_buffer_write_length)
-
-#define IIO_BUFFER_ENABLE_ATTR DEVICE_ATTR(enable, S_IRUGO | S_IWUSR,	\
-					   iio_buffer_show_enable,	\
-					   iio_buffer_store_enable)
-
-int iio_sw_buffer_preenable(struct iio_dev *indio_dev);
-
-#else /* CONFIG_IIO_BUFFER */
-
-static inline int iio_buffer_register(struct iio_dev *indio_dev,
-					   struct iio_chan_spec *channels,
-					   int num_channels)
-{
-	return 0;
-}
-
-static inline void iio_buffer_unregister(struct iio_dev *indio_dev)
-{};
-
-#endif /* CONFIG_IIO_BUFFER */
-
-#endif /* _IIO_BUFFER_GENERIC_H_ */
diff --git a/drivers/staging/iio/cdc/ad7150.c b/drivers/staging/iio/cdc/ad7150.c
index e4a08dc9b6f..a16d1a22db0 100644
--- a/drivers/staging/iio/cdc/ad7150.c
+++ b/drivers/staging/iio/cdc/ad7150.c
@@ -13,9 +13,9 @@
 #include <linux/i2c.h>
 #include <linux/module.h>
 
-#include "../iio.h"
-#include "../sysfs.h"
-#include "../events.h"
+#include <linux/iio/iio.h>
+#include <linux/iio/sysfs.h>
+#include <linux/iio/events.h>
 /*
  * AD7150 registers definition
  */
@@ -104,7 +104,7 @@ static int ad7150_read_raw(struct iio_dev *indio_dev,
 	struct ad7150_chip_info *chip = iio_priv(indio_dev);
 
 	switch (mask) {
-	case 0:
+	case IIO_CHAN_INFO_RAW:
 		ret = i2c_smbus_read_word_data(chip->client,
 					ad7150_addresses[chan->channel][0]);
 		if (ret < 0)
@@ -341,7 +341,7 @@ static ssize_t ad7150_show_timeout(struct device *dev,
 				   struct device_attribute *attr,
 				   char *buf)
 {
-	struct iio_dev *indio_dev = dev_get_drvdata(dev);
+	struct iio_dev *indio_dev = dev_to_iio_dev(dev);
 	struct ad7150_chip_info *chip = iio_priv(indio_dev);
 	struct iio_dev_attr *this_attr = to_iio_dev_attr(attr);
 	u8 value;
@@ -370,7 +370,7 @@ static ssize_t ad7150_store_timeout(struct device *dev,
 		const char *buf,
 		size_t len)
 {
-	struct iio_dev *indio_dev = dev_get_drvdata(dev);
+	struct iio_dev *indio_dev = dev_to_iio_dev(dev);
 	struct ad7150_chip_info *chip = iio_priv(indio_dev);
 	struct iio_dev_attr *this_attr = to_iio_dev_attr(attr);
 	int chan = IIO_EVENT_CODE_EXTRACT_CHAN(this_attr->address);
@@ -429,7 +429,8 @@ static const struct iio_chan_spec ad7150_channels[] = {
 		.type = IIO_CAPACITANCE,
 		.indexed = 1,
 		.channel = 0,
-		.info_mask = IIO_CHAN_INFO_AVERAGE_RAW_SEPARATE_BIT,
+		.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT |
+		IIO_CHAN_INFO_AVERAGE_RAW_SEPARATE_BIT,
 		.event_mask =
 		IIO_EV_BIT(IIO_EV_TYPE_THRESH, IIO_EV_DIR_RISING) |
 		IIO_EV_BIT(IIO_EV_TYPE_THRESH, IIO_EV_DIR_FALLING) |
@@ -441,7 +442,8 @@ static const struct iio_chan_spec ad7150_channels[] = {
 		.type = IIO_CAPACITANCE,
 		.indexed = 1,
 		.channel = 1,
-		.info_mask = IIO_CHAN_INFO_AVERAGE_RAW_SEPARATE_BIT,
+		.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT |
+		IIO_CHAN_INFO_AVERAGE_RAW_SEPARATE_BIT,
 		.event_mask =
 		IIO_EV_BIT(IIO_EV_TYPE_THRESH, IIO_EV_DIR_RISING) |
 		IIO_EV_BIT(IIO_EV_TYPE_THRESH, IIO_EV_DIR_FALLING) |
@@ -556,7 +558,7 @@ static int __devinit ad7150_probe(struct i2c_client *client,
 	struct ad7150_chip_info *chip;
 	struct iio_dev *indio_dev;
 
-	indio_dev = iio_allocate_device(sizeof(*chip));
+	indio_dev = iio_device_alloc(sizeof(*chip));
 	if (indio_dev == NULL) {
 		ret = -ENOMEM;
 		goto error_ret;
@@ -619,7 +621,7 @@ error_free_irq:
 	if (client->irq)
 		free_irq(client->irq, indio_dev);
 error_free_dev:
-	iio_free_device(indio_dev);
+	iio_device_free(indio_dev);
 error_ret:
 	return ret;
 }
@@ -635,7 +637,7 @@ static int __devexit ad7150_remove(struct i2c_client *client)
 	if (client->dev.platform_data)
 		free_irq(*(unsigned int *)client->dev.platform_data, indio_dev);
 
-	iio_free_device(indio_dev);
+	iio_device_free(indio_dev);
 
 	return 0;
 }
diff --git a/drivers/staging/iio/cdc/ad7152.c b/drivers/staging/iio/cdc/ad7152.c
index fdb83c35e6d..98c3015116a 100644
--- a/drivers/staging/iio/cdc/ad7152.c
+++ b/drivers/staging/iio/cdc/ad7152.c
@@ -15,8 +15,8 @@
 #include <linux/module.h>
 #include <linux/delay.h>
 
-#include "../iio.h"
-#include "../sysfs.h"
+#include <linux/iio/iio.h>
+#include <linux/iio/sysfs.h>
 
 /*
  * TODO: Check compliance of calibbias with abi (units)
@@ -97,7 +97,7 @@ static inline ssize_t ad7152_start_calib(struct device *dev,
 					 size_t len,
 					 u8 regval)
 {
-	struct iio_dev *indio_dev = dev_get_drvdata(dev);
+	struct iio_dev *indio_dev = dev_to_iio_dev(dev);
 	struct ad7152_chip_info *chip = iio_priv(indio_dev);
 	struct iio_dev_attr *this_attr = to_iio_dev_attr(attr);
 	bool doit;
@@ -169,7 +169,7 @@ static ssize_t ad7152_show_filter_rate_setup(struct device *dev,
 		struct device_attribute *attr,
 		char *buf)
 {
-	struct iio_dev *indio_dev = dev_get_drvdata(dev);
+	struct iio_dev *indio_dev = dev_to_iio_dev(dev);
 	struct ad7152_chip_info *chip = iio_priv(indio_dev);
 
 	return sprintf(buf, "%d\n",
@@ -181,7 +181,7 @@ static ssize_t ad7152_store_filter_rate_setup(struct device *dev,
 		const char *buf,
 		size_t len)
 {
-	struct iio_dev *indio_dev = dev_get_drvdata(dev);
+	struct iio_dev *indio_dev = dev_to_iio_dev(dev);
 	struct ad7152_chip_info *chip = iio_priv(indio_dev);
 	u8 data;
 	int ret, i;
@@ -329,7 +329,7 @@ static int ad7152_read_raw(struct iio_dev *indio_dev,
 	mutex_lock(&indio_dev->mlock);
 
 	switch (mask) {
-	case 0:
+	case IIO_CHAN_INFO_RAW:
 		/* First set whether in differential mode */
 
 		regval = chip->setup[chan->channel];
@@ -436,7 +436,8 @@ static const struct iio_chan_spec ad7152_channels[] = {
 		.type = IIO_CAPACITANCE,
 		.indexed = 1,
 		.channel = 0,
-		.info_mask = IIO_CHAN_INFO_CALIBSCALE_SEPARATE_BIT |
+		.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT |
+		IIO_CHAN_INFO_CALIBSCALE_SEPARATE_BIT |
 		IIO_CHAN_INFO_CALIBBIAS_SEPARATE_BIT |
 		IIO_CHAN_INFO_SCALE_SEPARATE_BIT,
 	}, {
@@ -445,14 +446,16 @@ static const struct iio_chan_spec ad7152_channels[] = {
 		.indexed = 1,
 		.channel = 0,
 		.channel2 = 2,
-		.info_mask = IIO_CHAN_INFO_CALIBSCALE_SEPARATE_BIT |
+		.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT |
+		IIO_CHAN_INFO_CALIBSCALE_SEPARATE_BIT |
 		IIO_CHAN_INFO_CALIBBIAS_SEPARATE_BIT |
 		IIO_CHAN_INFO_SCALE_SEPARATE_BIT,
 	}, {
 		.type = IIO_CAPACITANCE,
 		.indexed = 1,
 		.channel = 1,
-		.info_mask = IIO_CHAN_INFO_CALIBSCALE_SEPARATE_BIT |
+		.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT |
+		IIO_CHAN_INFO_CALIBSCALE_SEPARATE_BIT |
 		IIO_CHAN_INFO_CALIBBIAS_SEPARATE_BIT |
 		IIO_CHAN_INFO_SCALE_SEPARATE_BIT,
 	}, {
@@ -461,7 +464,8 @@ static const struct iio_chan_spec ad7152_channels[] = {
 		.indexed = 1,
 		.channel = 1,
 		.channel2 = 3,
-		.info_mask = IIO_CHAN_INFO_CALIBSCALE_SEPARATE_BIT |
+		.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT |
+		IIO_CHAN_INFO_CALIBSCALE_SEPARATE_BIT |
 		IIO_CHAN_INFO_CALIBBIAS_SEPARATE_BIT |
 		IIO_CHAN_INFO_SCALE_SEPARATE_BIT,
 	}
@@ -477,7 +481,7 @@ static int __devinit ad7152_probe(struct i2c_client *client,
 	struct ad7152_chip_info *chip;
 	struct iio_dev *indio_dev;
 
-	indio_dev = iio_allocate_device(sizeof(*chip));
+	indio_dev = iio_device_alloc(sizeof(*chip));
 	if (indio_dev == NULL) {
 		ret = -ENOMEM;
 		goto error_ret;
@@ -509,7 +513,7 @@ static int __devinit ad7152_probe(struct i2c_client *client,
 	return 0;
 
 error_free_dev:
-	iio_free_device(indio_dev);
+	iio_device_free(indio_dev);
 error_ret:
 	return ret;
 }
@@ -519,7 +523,7 @@ static int __devexit ad7152_remove(struct i2c_client *client)
 	struct iio_dev *indio_dev = i2c_get_clientdata(client);
 
 	iio_device_unregister(indio_dev);
-	iio_free_device(indio_dev);
+	iio_device_free(indio_dev);
 
 	return 0;
 }
diff --git a/drivers/staging/iio/cdc/ad7746.c b/drivers/staging/iio/cdc/ad7746.c
index 40b8512cbc3..754e11e8719 100644
--- a/drivers/staging/iio/cdc/ad7746.c
+++ b/drivers/staging/iio/cdc/ad7746.c
@@ -16,8 +16,8 @@
 #include <linux/module.h>
 #include <linux/stat.h>
 
-#include "../iio.h"
-#include "../sysfs.h"
+#include <linux/iio/iio.h>
+#include <linux/iio/sysfs.h>
 
 #include "ad7746.h"
 
@@ -123,7 +123,8 @@ static const struct iio_chan_spec ad7746_channels[] = {
 		.type = IIO_VOLTAGE,
 		.indexed = 1,
 		.channel = 0,
-		.info_mask = IIO_CHAN_INFO_SCALE_SHARED_BIT,
+		.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT |
+		IIO_CHAN_INFO_SCALE_SHARED_BIT,
 		.address = AD7746_REG_VT_DATA_HIGH << 8 |
 			AD7746_VTSETUP_VTMD_EXT_VIN,
 	},
@@ -132,7 +133,8 @@ static const struct iio_chan_spec ad7746_channels[] = {
 		.indexed = 1,
 		.channel = 1,
 		.extend_name = "supply",
-		.info_mask = IIO_CHAN_INFO_SCALE_SHARED_BIT,
+		.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT |
+		IIO_CHAN_INFO_SCALE_SHARED_BIT,
 		.address = AD7746_REG_VT_DATA_HIGH << 8 |
 			AD7746_VTSETUP_VTMD_VDD_MON,
 	},
@@ -140,7 +142,7 @@ static const struct iio_chan_spec ad7746_channels[] = {
 		.type = IIO_TEMP,
 		.indexed = 1,
 		.channel = 0,
-		.processed_val = IIO_PROCESSED,
+		.info_mask = IIO_CHAN_INFO_PROCESSED_SEPARATE_BIT,
 		.address = AD7746_REG_VT_DATA_HIGH << 8 |
 			AD7746_VTSETUP_VTMD_INT_TEMP,
 	},
@@ -148,7 +150,7 @@ static const struct iio_chan_spec ad7746_channels[] = {
 		.type = IIO_TEMP,
 		.indexed = 1,
 		.channel = 1,
-		.processed_val = IIO_PROCESSED,
+		.info_mask = IIO_CHAN_INFO_PROCESSED_SEPARATE_BIT,
 		.address = AD7746_REG_VT_DATA_HIGH << 8 |
 			AD7746_VTSETUP_VTMD_EXT_TEMP,
 	},
@@ -156,7 +158,8 @@ static const struct iio_chan_spec ad7746_channels[] = {
 		.type = IIO_CAPACITANCE,
 		.indexed = 1,
 		.channel = 0,
-		.info_mask = IIO_CHAN_INFO_CALIBSCALE_SEPARATE_BIT |
+		.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT |
+		IIO_CHAN_INFO_CALIBSCALE_SEPARATE_BIT |
 		IIO_CHAN_INFO_CALIBBIAS_SHARED_BIT |
 		IIO_CHAN_INFO_OFFSET_SEPARATE_BIT |
 		IIO_CHAN_INFO_SCALE_SHARED_BIT,
@@ -168,7 +171,8 @@ static const struct iio_chan_spec ad7746_channels[] = {
 		.indexed = 1,
 		.channel = 0,
 		.channel2 = 2,
-		.info_mask = IIO_CHAN_INFO_CALIBSCALE_SEPARATE_BIT |
+		.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT |
+		IIO_CHAN_INFO_CALIBSCALE_SEPARATE_BIT |
 		IIO_CHAN_INFO_CALIBBIAS_SHARED_BIT |
 		IIO_CHAN_INFO_OFFSET_SEPARATE_BIT |
 		IIO_CHAN_INFO_SCALE_SHARED_BIT,
@@ -179,7 +183,8 @@ static const struct iio_chan_spec ad7746_channels[] = {
 		.type = IIO_CAPACITANCE,
 		.indexed = 1,
 		.channel = 1,
-		.info_mask = IIO_CHAN_INFO_CALIBSCALE_SEPARATE_BIT |
+		.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT |
+		IIO_CHAN_INFO_CALIBSCALE_SEPARATE_BIT |
 		IIO_CHAN_INFO_CALIBBIAS_SHARED_BIT |
 		IIO_CHAN_INFO_OFFSET_SEPARATE_BIT |
 		IIO_CHAN_INFO_SCALE_SHARED_BIT,
@@ -192,7 +197,8 @@ static const struct iio_chan_spec ad7746_channels[] = {
 		.indexed = 1,
 		.channel = 1,
 		.channel2 = 3,
-		.info_mask = IIO_CHAN_INFO_CALIBSCALE_SEPARATE_BIT |
+		.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT |
+		IIO_CHAN_INFO_CALIBSCALE_SEPARATE_BIT |
 		IIO_CHAN_INFO_CALIBBIAS_SHARED_BIT |
 		IIO_CHAN_INFO_OFFSET_SEPARATE_BIT |
 		IIO_CHAN_INFO_SCALE_SHARED_BIT,
@@ -280,7 +286,7 @@ static inline ssize_t ad7746_start_calib(struct device *dev,
 					 size_t len,
 					 u8 regval)
 {
-	struct iio_dev *indio_dev = dev_get_drvdata(dev);
+	struct iio_dev *indio_dev = dev_to_iio_dev(dev);
 	struct ad7746_chip_info *chip = iio_priv(indio_dev);
 	bool doit;
 	int ret, timeout = 10;
@@ -319,7 +325,7 @@ static ssize_t ad7746_start_offset_calib(struct device *dev,
 					 const char *buf,
 					 size_t len)
 {
-	struct iio_dev *indio_dev = dev_get_drvdata(dev);
+	struct iio_dev *indio_dev = dev_to_iio_dev(dev);
 	int ret = ad7746_select_channel(indio_dev,
 			      &ad7746_channels[to_iio_dev_attr(attr)->address]);
 	if (ret < 0)
@@ -334,7 +340,7 @@ static ssize_t ad7746_start_gain_calib(struct device *dev,
 				       const char *buf,
 				       size_t len)
 {
-	struct iio_dev *indio_dev = dev_get_drvdata(dev);
+	struct iio_dev *indio_dev = dev_to_iio_dev(dev);
 	int ret = ad7746_select_channel(indio_dev,
 			      &ad7746_channels[to_iio_dev_attr(attr)->address]);
 	if (ret < 0)
@@ -359,7 +365,7 @@ static ssize_t ad7746_show_cap_filter_rate_setup(struct device *dev,
 		struct device_attribute *attr,
 		char *buf)
 {
-	struct iio_dev *indio_dev = dev_get_drvdata(dev);
+	struct iio_dev *indio_dev = dev_to_iio_dev(dev);
 	struct ad7746_chip_info *chip = iio_priv(indio_dev);
 
 	return sprintf(buf, "%d\n", ad7746_cap_filter_rate_table[
@@ -371,7 +377,7 @@ static ssize_t ad7746_store_cap_filter_rate_setup(struct device *dev,
 		const char *buf,
 		size_t len)
 {
-	struct iio_dev *indio_dev = dev_get_drvdata(dev);
+	struct iio_dev *indio_dev = dev_to_iio_dev(dev);
 	struct ad7746_chip_info *chip = iio_priv(indio_dev);
 	u8 data;
 	int ret, i;
@@ -399,7 +405,7 @@ static ssize_t ad7746_show_vt_filter_rate_setup(struct device *dev,
 		struct device_attribute *attr,
 		char *buf)
 {
-	struct iio_dev *indio_dev = dev_get_drvdata(dev);
+	struct iio_dev *indio_dev = dev_to_iio_dev(dev);
 	struct ad7746_chip_info *chip = iio_priv(indio_dev);
 
 	return sprintf(buf, "%d\n", ad7746_vt_filter_rate_table[
@@ -411,7 +417,7 @@ static ssize_t ad7746_store_vt_filter_rate_setup(struct device *dev,
 		const char *buf,
 		size_t len)
 {
-	struct iio_dev *indio_dev = dev_get_drvdata(dev);
+	struct iio_dev *indio_dev = dev_to_iio_dev(dev);
 	struct ad7746_chip_info *chip = iio_priv(indio_dev);
 	u8 data;
 	int ret, i;
@@ -572,7 +578,8 @@ static int ad7746_read_raw(struct iio_dev *indio_dev,
 	mutex_lock(&indio_dev->mlock);
 
 	switch (mask) {
-	case 0:
+	case IIO_CHAN_INFO_RAW:
+	case IIO_CHAN_INFO_PROCESSED:
 		ret = ad7746_select_channel(indio_dev, chan);
 		if (ret < 0)
 			goto out;
@@ -696,7 +703,7 @@ static int __devinit ad7746_probe(struct i2c_client *client,
 	int ret = 0;
 	unsigned char regval = 0;
 
-	indio_dev = iio_allocate_device(sizeof(*chip));
+	indio_dev = iio_device_alloc(sizeof(*chip));
 	if (indio_dev == NULL) {
 		ret = -ENOMEM;
 		goto error_ret;
@@ -756,7 +763,7 @@ static int __devinit ad7746_probe(struct i2c_client *client,
 	return 0;
 
 error_free_dev:
-	iio_free_device(indio_dev);
+	iio_device_free(indio_dev);
 error_ret:
 	return ret;
 }
@@ -766,7 +773,7 @@ static int __devexit ad7746_remove(struct i2c_client *client)
 	struct iio_dev *indio_dev = i2c_get_clientdata(client);
 
 	iio_device_unregister(indio_dev);
-	iio_free_device(indio_dev);
+	iio_device_free(indio_dev);
 
 	return 0;
 }
diff --git a/drivers/staging/iio/consumer.h b/drivers/staging/iio/consumer.h
deleted file mode 100644
index 36a060cd3a2..00000000000
--- a/drivers/staging/iio/consumer.h
+++ /dev/null
@@ -1,96 +0,0 @@
-/*
- * Industrial I/O in kernel consumer interface
- *
- * Copyright (c) 2011 Jonathan Cameron
- *
- * 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 _IIO_INKERN_CONSUMER_H_
-#define _IIO_INKERN_CONSUMER_H
-#include "types.h"
-
-struct iio_dev;
-struct iio_chan_spec;
-
-/**
- * struct iio_channel - everything needed for a consumer to use a channel
- * @indio_dev:		Device on which the channel exists.
- * @channel:		Full description of the channel.
- */
-struct iio_channel {
-	struct iio_dev *indio_dev;
-	const struct iio_chan_spec *channel;
-};
-
-/**
- * iio_channel_get() - get description of all that is needed to access channel.
- * @name:		Unique name of the device as provided in the iio_map
- *			with which the desired provider to consumer mapping
- *			was registered.
- * @consumer_channel:	Unique name to identify the channel on the consumer
- *			side. This typically describes the channels use within
- *			the consumer. E.g. 'battery_voltage'
- */
-struct iio_channel *iio_st_channel_get(const char *name,
-				       const char *consumer_channel);
-
-/**
- * iio_st_channel_release() - release channels obtained via iio_st_channel_get
- * @chan:		The channel to be released.
- */
-void iio_st_channel_release(struct iio_channel *chan);
-
-/**
- * iio_st_channel_get_all() - get all channels associated with a client
- * @name:		name of consumer device.
- *
- * Returns an array of iio_channel structures terminated with one with
- * null iio_dev pointer.
- * This function is used by fairly generic consumers to get all the
- * channels registered as having this consumer.
- */
-struct iio_channel *iio_st_channel_get_all(const char *name);
-
-/**
- * iio_st_channel_release_all() - reverse iio_st_get_all
- * @chan:		Array of channels to be released.
- */
-void iio_st_channel_release_all(struct iio_channel *chan);
-
-/**
- * iio_st_read_channel_raw() - read from a given channel
- * @channel:		The channel being queried.
- * @val:		Value read back.
- *
- * Note raw reads from iio channels are in adc counts and hence
- * scale will need to be applied if standard units required.
- */
-int iio_st_read_channel_raw(struct iio_channel *chan,
-			    int *val);
-
-/**
- * iio_st_get_channel_type() - get the type of a channel
- * @channel:		The channel being queried.
- * @type:		The type of the channel.
- *
- * returns the enum iio_chan_type of the channel
- */
-int iio_st_get_channel_type(struct iio_channel *channel,
-			    enum iio_chan_type *type);
-
-/**
- * iio_st_read_channel_scale() - read the scale value for a channel
- * @channel:		The channel being queried.
- * @val:		First part of value read back.
- * @val2:		Second part of value read back.
- *
- * Note returns a description of what is in val and val2, such
- * as IIO_VAL_INT_PLUS_MICRO telling us we have a value of val
- * + val2/1e6
- */
-int iio_st_read_channel_scale(struct iio_channel *chan, int *val,
-			      int *val2);
-
-#endif
diff --git a/drivers/staging/iio/dac/Kconfig b/drivers/staging/iio/dac/Kconfig
index a57803a5d1a..a626f03871e 100644
--- a/drivers/staging/iio/dac/Kconfig
+++ b/drivers/staging/iio/dac/Kconfig
@@ -56,12 +56,12 @@ config AD5624R_SPI
 	  AD5664R converters (DAC). This driver uses the common SPI interface.
 
 config AD5446
-	tristate "Analog Devices AD5444/6, AD5620/40/60 and AD5542A/12A DAC SPI driver"
+	tristate "Analog Devices AD5446 and similar single channel DACs driver"
 	depends on SPI
 	help
 	  Say yes here to build support for Analog Devices AD5444, AD5446,
-	  AD5512A, AD5542A, AD5543, AD5553, AD5601, AD5611, AD5620, AD5621,
-	  AD5640, AD5660, AD5662 DACs.
+	  AD5512A, AD5541A, AD5542A, AD5543, AD5553, AD5601, AD5611, AD5620,
+	  AD5621, AD5640, AD5660, AD5662 DACs.
 
 	  To compile this driver as a module, choose M here: the
 	  module will be called ad5446.
diff --git a/drivers/staging/iio/dac/ad5064.c b/drivers/staging/iio/dac/ad5064.c
index 06b162745a3..047148aa66b 100644
--- a/drivers/staging/iio/dac/ad5064.c
+++ b/drivers/staging/iio/dac/ad5064.c
@@ -16,8 +16,8 @@
 #include <linux/sysfs.h>
 #include <linux/regulator/consumer.h>
 
-#include "../iio.h"
-#include "../sysfs.h"
+#include <linux/iio/iio.h>
+#include <linux/iio/sysfs.h>
 #include "dac.h"
 
 #define AD5064_MAX_DAC_CHANNELS			8
@@ -144,14 +144,14 @@ static const char ad5064_powerdown_modes[][15] = {
 };
 
 static ssize_t ad5064_read_powerdown_mode_available(struct iio_dev *indio_dev,
-	const struct iio_chan_spec *chan, char *buf)
+	uintptr_t private, const struct iio_chan_spec *chan, char *buf)
 {
 	return sprintf(buf, "%s %s %s\n", ad5064_powerdown_modes[1],
 		ad5064_powerdown_modes[2], ad5064_powerdown_modes[3]);
 }
 
 static ssize_t ad5064_read_powerdown_mode(struct iio_dev *indio_dev,
-	const struct iio_chan_spec *chan, char *buf)
+	uintptr_t private, const struct iio_chan_spec *chan, char *buf)
 {
 	struct ad5064_state *st = iio_priv(indio_dev);
 
@@ -160,7 +160,8 @@ static ssize_t ad5064_read_powerdown_mode(struct iio_dev *indio_dev,
 }
 
 static ssize_t ad5064_write_powerdown_mode(struct iio_dev *indio_dev,
-	const struct iio_chan_spec *chan, const char *buf, size_t len)
+	uintptr_t private, const struct iio_chan_spec *chan, const char *buf,
+	size_t len)
 {
 	struct ad5064_state *st = iio_priv(indio_dev);
 	unsigned int mode, i;
@@ -187,7 +188,7 @@ static ssize_t ad5064_write_powerdown_mode(struct iio_dev *indio_dev,
 }
 
 static ssize_t ad5064_read_dac_powerdown(struct iio_dev *indio_dev,
-	const struct iio_chan_spec *chan, char *buf)
+	uintptr_t private, const struct iio_chan_spec *chan, char *buf)
 {
 	struct ad5064_state *st = iio_priv(indio_dev);
 
@@ -195,7 +196,8 @@ static ssize_t ad5064_read_dac_powerdown(struct iio_dev *indio_dev,
 }
 
 static ssize_t ad5064_write_dac_powerdown(struct iio_dev *indio_dev,
-	const struct iio_chan_spec *chan, const char *buf, size_t len)
+	 uintptr_t private, const struct iio_chan_spec *chan, const char *buf,
+	 size_t len)
 {
 	struct ad5064_state *st = iio_priv(indio_dev);
 	bool pwr_down;
@@ -235,7 +237,7 @@ static int ad5064_read_raw(struct iio_dev *indio_dev,
 	int scale_uv;
 
 	switch (m) {
-	case 0:
+	case IIO_CHAN_INFO_RAW:
 		*val = st->dac_cache[chan->channel];
 		return IIO_VAL_INT;
 	case IIO_CHAN_INFO_SCALE:
@@ -260,7 +262,7 @@ static int ad5064_write_raw(struct iio_dev *indio_dev,
 	int ret;
 
 	switch (mask) {
-	case 0:
+	case IIO_CHAN_INFO_RAW:
 		if (val > (1 << chan->scan_type.realbits) || val < 0)
 			return -EINVAL;
 
@@ -308,7 +310,8 @@ static struct iio_chan_spec_ext_info ad5064_ext_info[] = {
 	.indexed = 1,						\
 	.output = 1,						\
 	.channel = (chan),					\
-	.info_mask = IIO_CHAN_INFO_SCALE_SEPARATE_BIT,	\
+	.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT |		\
+	IIO_CHAN_INFO_SCALE_SEPARATE_BIT,			\
 	.address = AD5064_ADDR_DAC(chan),			\
 	.scan_type = IIO_ST('u', (bits), 16, 20 - (bits)),	\
 	.ext_info = ad5064_ext_info,				\
@@ -442,7 +445,7 @@ static int __devinit ad5064_probe(struct spi_device *spi)
 	unsigned int i;
 	int ret;
 
-	indio_dev = iio_allocate_device(sizeof(*st));
+	indio_dev = iio_device_alloc(sizeof(*st));
 	if (indio_dev == NULL)
 		return  -ENOMEM;
 
@@ -499,7 +502,7 @@ error_free_reg:
 	if (!st->use_internal_vref)
 		regulator_bulk_free(ad5064_num_vref(st), st->vref_reg);
 error_free:
-	iio_free_device(indio_dev);
+	iio_device_free(indio_dev);
 
 	return ret;
 }
@@ -517,7 +520,7 @@ static int __devexit ad5064_remove(struct spi_device *spi)
 		regulator_bulk_free(ad5064_num_vref(st), st->vref_reg);
 	}
 
-	iio_free_device(indio_dev);
+	iio_device_free(indio_dev);
 
 	return 0;
 }
diff --git a/drivers/staging/iio/dac/ad5360.c b/drivers/staging/iio/dac/ad5360.c
index cec3693b50a..38660efca78 100644
--- a/drivers/staging/iio/dac/ad5360.c
+++ b/drivers/staging/iio/dac/ad5360.c
@@ -16,8 +16,8 @@
 #include <linux/sysfs.h>
 #include <linux/regulator/consumer.h>
 
-#include "../iio.h"
-#include "../sysfs.h"
+#include <linux/iio/iio.h>
+#include <linux/iio/sysfs.h>
 #include "dac.h"
 
 #define AD5360_CMD(x)				((x) << 22)
@@ -103,7 +103,8 @@ enum ad5360_type {
 	.type = IIO_VOLTAGE,					\
 	.indexed = 1,						\
 	.output = 1,						\
-	.info_mask = IIO_CHAN_INFO_SCALE_SEPARATE_BIT |	\
+	.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT |		\
+		IIO_CHAN_INFO_SCALE_SEPARATE_BIT |		\
 		IIO_CHAN_INFO_OFFSET_SEPARATE_BIT |		\
 		IIO_CHAN_INFO_CALIBSCALE_SEPARATE_BIT |	\
 		IIO_CHAN_INFO_CALIBBIAS_SEPARATE_BIT,	\
@@ -250,7 +251,7 @@ static ssize_t ad5360_read_dac_powerdown(struct device *dev,
 					   struct device_attribute *attr,
 					   char *buf)
 {
-	struct iio_dev *indio_dev = dev_get_drvdata(dev);
+	struct iio_dev *indio_dev = dev_to_iio_dev(dev);
 	struct ad5360_state *st = iio_priv(indio_dev);
 
 	return sprintf(buf, "%d\n", (bool)(st->ctrl & AD5360_SF_CTRL_PWR_DOWN));
@@ -278,7 +279,7 @@ static int ad5360_update_ctrl(struct iio_dev *indio_dev, unsigned int set,
 static ssize_t ad5360_write_dac_powerdown(struct device *dev,
 	struct device_attribute *attr, const char *buf, size_t len)
 {
-	struct iio_dev *indio_dev = dev_get_drvdata(dev);
+	struct iio_dev *indio_dev = dev_to_iio_dev(dev);
 	bool pwr_down;
 	int ret;
 
@@ -319,7 +320,7 @@ static int ad5360_write_raw(struct iio_dev *indio_dev,
 	unsigned int ofs_index;
 
 	switch (mask) {
-	case 0:
+	case IIO_CHAN_INFO_RAW:
 		if (val >= max_val || val < 0)
 			return -EINVAL;
 
@@ -376,7 +377,7 @@ static int ad5360_read_raw(struct iio_dev *indio_dev,
 	int ret;
 
 	switch (m) {
-	case 0:
+	case IIO_CHAN_INFO_RAW:
 		ret = ad5360_read(indio_dev, AD5360_READBACK_X1A,
 			chan->address);
 		if (ret < 0)
@@ -464,7 +465,7 @@ static int __devinit ad5360_probe(struct spi_device *spi)
 	unsigned int i;
 	int ret;
 
-	indio_dev = iio_allocate_device(sizeof(*st));
+	indio_dev = iio_device_alloc(sizeof(*st));
 	if (indio_dev == NULL) {
 		dev_err(&spi->dev, "Failed to allocate iio device\n");
 		return  -ENOMEM;
@@ -519,7 +520,7 @@ error_free_reg:
 error_free_channels:
 	kfree(indio_dev->channels);
 error_free:
-	iio_free_device(indio_dev);
+	iio_device_free(indio_dev);
 
 	return ret;
 }
@@ -536,7 +537,7 @@ static int __devexit ad5360_remove(struct spi_device *spi)
 	regulator_bulk_disable(st->chip_info->num_vrefs, st->vref_reg);
 	regulator_bulk_free(st->chip_info->num_vrefs, st->vref_reg);
 
-	iio_free_device(indio_dev);
+	iio_device_free(indio_dev);
 
 	return 0;
 }
diff --git a/drivers/staging/iio/dac/ad5380.c b/drivers/staging/iio/dac/ad5380.c
index 4c50716fa80..370d2842190 100644
--- a/drivers/staging/iio/dac/ad5380.c
+++ b/drivers/staging/iio/dac/ad5380.c
@@ -18,8 +18,8 @@
 #include <linux/regmap.h>
 #include <linux/regulator/consumer.h>
 
-#include "../iio.h"
-#include "../sysfs.h"
+#include <linux/iio/iio.h>
+#include <linux/iio/sysfs.h>
 #include "dac.h"
 
 
@@ -85,7 +85,8 @@ enum ad5380_type {
 	.type = IIO_VOLTAGE,					\
 	.indexed = 1,						\
 	.output = 1,						\
-	.info_mask = IIO_CHAN_INFO_SCALE_SHARED_BIT |		\
+	.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT |		\
+		IIO_CHAN_INFO_SCALE_SHARED_BIT |		\
 		IIO_CHAN_INFO_CALIBSCALE_SEPARATE_BIT |		\
 		IIO_CHAN_INFO_CALIBBIAS_SEPARATE_BIT,		\
 	.scan_type = IIO_ST('u', (_bits), 16, 14 - (_bits))	\
@@ -167,7 +168,7 @@ static const struct ad5380_chip_info ad5380_chip_info_tbl[] = {
 static ssize_t ad5380_read_dac_powerdown(struct device *dev,
 	struct device_attribute *attr, char *buf)
 {
-	struct iio_dev *indio_dev = dev_get_drvdata(dev);
+	struct iio_dev *indio_dev = dev_to_iio_dev(dev);
 	struct ad5380_state *st = iio_priv(indio_dev);
 
 	return sprintf(buf, "%d\n", st->pwr_down);
@@ -176,7 +177,7 @@ static ssize_t ad5380_read_dac_powerdown(struct device *dev,
 static ssize_t ad5380_write_dac_powerdown(struct device *dev,
 	struct device_attribute *attr, const char *buf, size_t len)
 {
-	struct iio_dev *indio_dev = dev_get_drvdata(dev);
+	struct iio_dev *indio_dev = dev_to_iio_dev(dev);
 	struct ad5380_state *st = iio_priv(indio_dev);
 	bool pwr_down;
 	int ret;
@@ -212,7 +213,7 @@ static const char ad5380_powerdown_modes[][15] = {
 static ssize_t ad5380_read_powerdown_mode(struct device *dev,
 	struct device_attribute *attr, char *buf)
 {
-	struct iio_dev *indio_dev = dev_get_drvdata(dev);
+	struct iio_dev *indio_dev = dev_to_iio_dev(dev);
 	struct ad5380_state *st = iio_priv(indio_dev);
 	unsigned int mode;
 	int ret;
@@ -229,7 +230,7 @@ static ssize_t ad5380_read_powerdown_mode(struct device *dev,
 static ssize_t ad5380_write_powerdown_mode(struct device *dev,
 	struct device_attribute *attr, const char *buf, size_t len)
 {
-	struct iio_dev *indio_dev = dev_get_drvdata(dev);
+	struct iio_dev *indio_dev = dev_to_iio_dev(dev);
 	struct ad5380_state *st = iio_priv(indio_dev);
 	unsigned int i;
 	int ret;
@@ -292,7 +293,7 @@ static int ad5380_write_raw(struct iio_dev *indio_dev,
 	struct ad5380_state *st = iio_priv(indio_dev);
 
 	switch (info) {
-	case 0:
+	case IIO_CHAN_INFO_RAW:
 	case IIO_CHAN_INFO_CALIBSCALE:
 		if (val >= max_val || val < 0)
 			return -EINVAL;
@@ -322,7 +323,7 @@ static int ad5380_read_raw(struct iio_dev *indio_dev,
 	int ret;
 
 	switch (info) {
-	case 0:
+	case IIO_CHAN_INFO_RAW:
 	case IIO_CHAN_INFO_CALIBSCALE:
 		ret = regmap_read(st->regmap, ad5380_info_to_reg(chan, info),
 					val);
@@ -388,7 +389,7 @@ static int __devinit ad5380_probe(struct device *dev, struct regmap *regmap,
 	unsigned int ctrl = 0;
 	int ret;
 
-	indio_dev = iio_allocate_device(sizeof(*st));
+	indio_dev = iio_device_alloc(sizeof(*st));
 	if (indio_dev == NULL) {
 		dev_err(dev, "Failed to allocate iio device\n");
 		ret = -ENOMEM;
@@ -454,7 +455,7 @@ error_free_reg:
 
 	kfree(indio_dev->channels);
 error_free:
-	iio_free_device(indio_dev);
+	iio_device_free(indio_dev);
 error_regmap_exit:
 	regmap_exit(regmap);
 
@@ -476,7 +477,7 @@ static int __devexit ad5380_remove(struct device *dev)
 	}
 
 	regmap_exit(st->regmap);
-	iio_free_device(indio_dev);
+	iio_device_free(indio_dev);
 
 	return 0;
 }
diff --git a/drivers/staging/iio/dac/ad5421.c b/drivers/staging/iio/dac/ad5421.c
index 0b040b20469..ffbd4c234f5 100644
--- a/drivers/staging/iio/dac/ad5421.c
+++ b/drivers/staging/iio/dac/ad5421.c
@@ -16,9 +16,9 @@
 #include <linux/slab.h>
 #include <linux/sysfs.h>
 
-#include "../iio.h"
-#include "../sysfs.h"
-#include "../events.h"
+#include <linux/iio/iio.h>
+#include <linux/iio/sysfs.h>
+#include <linux/iio/events.h>
 #include "dac.h"
 #include "ad5421.h"
 
@@ -87,7 +87,8 @@ static const struct iio_chan_spec ad5421_channels[] = {
 		.indexed = 1,
 		.output = 1,
 		.channel = 0,
-		.info_mask = IIO_CHAN_INFO_SCALE_SHARED_BIT |
+		.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT |
+			IIO_CHAN_INFO_SCALE_SHARED_BIT |
 			IIO_CHAN_INFO_OFFSET_SHARED_BIT |
 			IIO_CHAN_INFO_CALIBSCALE_SEPARATE_BIT |
 			IIO_CHAN_INFO_CALIBBIAS_SEPARATE_BIT,
@@ -304,7 +305,7 @@ static int ad5421_read_raw(struct iio_dev *indio_dev,
 		return -EINVAL;
 
 	switch (m) {
-	case 0:
+	case IIO_CHAN_INFO_RAW:
 		ret = ad5421_read(indio_dev, AD5421_REG_DAC_DATA);
 		if (ret < 0)
 			return ret;
@@ -340,7 +341,7 @@ static int ad5421_write_raw(struct iio_dev *indio_dev,
 	const unsigned int max_val = 1 << 16;
 
 	switch (mask) {
-	case 0:
+	case IIO_CHAN_INFO_RAW:
 		if (val >= max_val || val < 0)
 			return -EINVAL;
 
@@ -456,7 +457,7 @@ static int __devinit ad5421_probe(struct spi_device *spi)
 	struct ad5421_state *st;
 	int ret;
 
-	indio_dev = iio_allocate_device(sizeof(*st));
+	indio_dev = iio_device_alloc(sizeof(*st));
 	if (indio_dev == NULL) {
 		dev_err(&spi->dev, "Failed to allocate iio device\n");
 		return  -ENOMEM;
@@ -511,7 +512,7 @@ error_free_irq:
 	if (spi->irq)
 		free_irq(spi->irq, indio_dev);
 error_free:
-	iio_free_device(indio_dev);
+	iio_device_free(indio_dev);
 
 	return ret;
 }
@@ -523,7 +524,7 @@ static int __devexit ad5421_remove(struct spi_device *spi)
 	iio_device_unregister(indio_dev);
 	if (spi->irq)
 		free_irq(spi->irq, indio_dev);
-	iio_free_device(indio_dev);
+	iio_device_free(indio_dev);
 
 	return 0;
 }
diff --git a/drivers/staging/iio/dac/ad5446.c b/drivers/staging/iio/dac/ad5446.c
index 633ffbb2181..daa65b384c1 100644
--- a/drivers/staging/iio/dac/ad5446.c
+++ b/drivers/staging/iio/dac/ad5446.c
@@ -18,229 +18,212 @@
 #include <linux/err.h>
 #include <linux/module.h>
 
-#include "../iio.h"
-#include "../sysfs.h"
+#include <linux/iio/iio.h>
+#include <linux/iio/sysfs.h>
 #include "dac.h"
 
 #include "ad5446.h"
 
-static void ad5446_store_sample(struct ad5446_state *st, unsigned val)
+static int ad5446_write(struct ad5446_state *st, unsigned val)
 {
-	st->data.d16 = cpu_to_be16(AD5446_LOAD | val);
+	__be16 data = cpu_to_be16(val);
+	return spi_write(st->spi, &data, sizeof(data));
 }
 
-static void ad5542_store_sample(struct ad5446_state *st, unsigned val)
+static int ad5660_write(struct ad5446_state *st, unsigned val)
 {
-	st->data.d16 = cpu_to_be16(val);
-}
+	uint8_t data[3];
 
-static void ad5620_store_sample(struct ad5446_state *st, unsigned val)
-{
-	st->data.d16 = cpu_to_be16(AD5620_LOAD | val);
-}
+	data[0] = (val >> 16) & 0xFF;
+	data[1] = (val >> 8) & 0xFF;
+	data[2] = val & 0xFF;
 
-static void ad5660_store_sample(struct ad5446_state *st, unsigned val)
-{
-	val |= AD5660_LOAD;
-	st->data.d24[0] = (val >> 16) & 0xFF;
-	st->data.d24[1] = (val >> 8) & 0xFF;
-	st->data.d24[2] = val & 0xFF;
+	return spi_write(st->spi, data, sizeof(data));
 }
 
-static void ad5620_store_pwr_down(struct ad5446_state *st, unsigned mode)
-{
-	st->data.d16 = cpu_to_be16(mode << 14);
-}
+static const char * const ad5446_powerdown_modes[] = {
+	"", "1kohm_to_gnd", "100kohm_to_gnd", "three_state"
+};
 
-static void ad5660_store_pwr_down(struct ad5446_state *st, unsigned mode)
+static ssize_t ad5446_read_powerdown_mode_available(struct iio_dev *indio_dev,
+	uintptr_t private, const struct iio_chan_spec *chan, char *buf)
 {
-	unsigned val = mode << 16;
-
-	st->data.d24[0] = (val >> 16) & 0xFF;
-	st->data.d24[1] = (val >> 8) & 0xFF;
-	st->data.d24[2] = val & 0xFF;
+	return sprintf(buf, "%s %s %s\n", ad5446_powerdown_modes[1],
+		ad5446_powerdown_modes[2], ad5446_powerdown_modes[3]);
 }
 
-static ssize_t ad5446_write_powerdown_mode(struct device *dev,
-				       struct device_attribute *attr,
-				       const char *buf, size_t len)
+static ssize_t ad5446_write_powerdown_mode(struct iio_dev *indio_dev,
+					    uintptr_t private,
+					    const struct iio_chan_spec *chan,
+					    const char *buf, size_t len)
 {
-	struct iio_dev *indio_dev = dev_get_drvdata(dev);
 	struct ad5446_state *st = iio_priv(indio_dev);
+	int i;
 
-	if (sysfs_streq(buf, "1kohm_to_gnd"))
-		st->pwr_down_mode = MODE_PWRDWN_1k;
-	else if (sysfs_streq(buf, "100kohm_to_gnd"))
-		st->pwr_down_mode = MODE_PWRDWN_100k;
-	else if (sysfs_streq(buf, "three_state"))
-		st->pwr_down_mode = MODE_PWRDWN_TRISTATE;
-	else
+	for (i = 1; i < ARRAY_SIZE(ad5446_powerdown_modes); i++) {
+		if (sysfs_streq(buf, ad5446_powerdown_modes[i])) {
+			st->pwr_down_mode = i;
+			break;
+		}
+	}
+
+	if (i == ARRAY_SIZE(ad5446_powerdown_modes))
 		return -EINVAL;
 
 	return len;
 }
 
-static ssize_t ad5446_read_powerdown_mode(struct device *dev,
-				      struct device_attribute *attr, char *buf)
+static ssize_t ad5446_read_powerdown_mode(struct iio_dev *indio_dev,
+					   uintptr_t private,
+					   const struct iio_chan_spec *chan,
+					   char *buf)
 {
-	struct iio_dev *indio_dev = dev_get_drvdata(dev);
 	struct ad5446_state *st = iio_priv(indio_dev);
 
-	char mode[][15] = {"", "1kohm_to_gnd", "100kohm_to_gnd", "three_state"};
-
-	return sprintf(buf, "%s\n", mode[st->pwr_down_mode]);
+	return sprintf(buf, "%s\n", ad5446_powerdown_modes[st->pwr_down_mode]);
 }
 
-static ssize_t ad5446_read_dac_powerdown(struct device *dev,
-					   struct device_attribute *attr,
+static ssize_t ad5446_read_dac_powerdown(struct iio_dev *indio_dev,
+					   uintptr_t private,
+					   const struct iio_chan_spec *chan,
 					   char *buf)
 {
-	struct iio_dev *indio_dev = dev_get_drvdata(dev);
 	struct ad5446_state *st = iio_priv(indio_dev);
 
 	return sprintf(buf, "%d\n", st->pwr_down);
 }
 
-static ssize_t ad5446_write_dac_powerdown(struct device *dev,
-					    struct device_attribute *attr,
+static ssize_t ad5446_write_dac_powerdown(struct iio_dev *indio_dev,
+					    uintptr_t private,
+					    const struct iio_chan_spec *chan,
 					    const char *buf, size_t len)
 {
-	struct iio_dev *indio_dev = dev_get_drvdata(dev);
 	struct ad5446_state *st = iio_priv(indio_dev);
-	unsigned long readin;
+	unsigned int shift;
+	unsigned int val;
+	bool powerdown;
 	int ret;
 
-	ret = strict_strtol(buf, 10, &readin);
+	ret = strtobool(buf, &powerdown);
 	if (ret)
 		return ret;
 
-	if (readin > 1)
-		ret = -EINVAL;
-
 	mutex_lock(&indio_dev->mlock);
-	st->pwr_down = readin;
+	st->pwr_down = powerdown;
 
-	if (st->pwr_down)
-		st->chip_info->store_pwr_down(st, st->pwr_down_mode);
-	else
-		st->chip_info->store_sample(st, st->cached_val);
+	if (st->pwr_down) {
+		shift = chan->scan_type.realbits + chan->scan_type.shift;
+		val = st->pwr_down_mode << shift;
+	} else {
+		val = st->cached_val;
+	}
 
-	ret = spi_sync(st->spi, &st->msg);
+	ret = st->chip_info->write(st, val);
 	mutex_unlock(&indio_dev->mlock);
 
 	return ret ? ret : len;
 }
 
-static IIO_DEVICE_ATTR(out_voltage_powerdown_mode, S_IRUGO | S_IWUSR,
-			ad5446_read_powerdown_mode,
-			ad5446_write_powerdown_mode, 0);
-
-static IIO_CONST_ATTR(out_voltage_powerdown_mode_available,
-			"1kohm_to_gnd 100kohm_to_gnd three_state");
-
-static IIO_DEVICE_ATTR(out_voltage0_powerdown, S_IRUGO | S_IWUSR,
-			ad5446_read_dac_powerdown,
-			ad5446_write_dac_powerdown, 0);
-
-static struct attribute *ad5446_attributes[] = {
-	&iio_dev_attr_out_voltage0_powerdown.dev_attr.attr,
-	&iio_dev_attr_out_voltage_powerdown_mode.dev_attr.attr,
-	&iio_const_attr_out_voltage_powerdown_mode_available.dev_attr.attr,
-	NULL,
-};
-
-static const struct attribute_group ad5446_attribute_group = {
-	.attrs = ad5446_attributes,
+static const struct iio_chan_spec_ext_info ad5064_ext_info_powerdown[] = {
+	{
+		.name = "powerdown",
+		.read = ad5446_read_dac_powerdown,
+		.write = ad5446_write_dac_powerdown,
+	}, {
+		.name = "powerdown_mode",
+		.read = ad5446_read_powerdown_mode,
+		.write = ad5446_write_powerdown_mode,
+	}, {
+		.name = "powerdown_mode_available",
+		.shared = true,
+		.read = ad5446_read_powerdown_mode_available,
+	},
+	{ },
 };
 
-#define AD5446_CHANNEL(bits, storage, shift) { \
+#define _AD5446_CHANNEL(bits, storage, shift, ext) { \
 	.type = IIO_VOLTAGE, \
 	.indexed = 1, \
 	.output = 1, \
 	.channel = 0, \
-	.info_mask = IIO_CHAN_INFO_SCALE_SHARED_BIT, \
-	.scan_type = IIO_ST('u', (bits), (storage), (shift)) \
+	.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT | \
+	IIO_CHAN_INFO_SCALE_SHARED_BIT,	\
+	.scan_type = IIO_ST('u', (bits), (storage), (shift)), \
+	.ext_info = (ext), \
 }
 
+#define AD5446_CHANNEL(bits, storage, shift) \
+	_AD5446_CHANNEL(bits, storage, shift, NULL)
+
+#define AD5446_CHANNEL_POWERDOWN(bits, storage, shift) \
+	_AD5446_CHANNEL(bits, storage, shift, ad5064_ext_info_powerdown)
+
 static const struct ad5446_chip_info ad5446_chip_info_tbl[] = {
 	[ID_AD5444] = {
 		.channel = AD5446_CHANNEL(12, 16, 2),
-		.store_sample = ad5446_store_sample,
+		.write = ad5446_write,
 	},
 	[ID_AD5446] = {
 		.channel = AD5446_CHANNEL(14, 16, 0),
-		.store_sample = ad5446_store_sample,
+		.write = ad5446_write,
 	},
 	[ID_AD5541A] = {
 		.channel = AD5446_CHANNEL(16, 16, 0),
-		.store_sample = ad5542_store_sample,
-	},
-	[ID_AD5542A] = {
-		.channel = AD5446_CHANNEL(16, 16, 0),
-		.store_sample = ad5542_store_sample,
-	},
-	[ID_AD5543] = {
-		.channel = AD5446_CHANNEL(16, 16, 0),
-		.store_sample = ad5542_store_sample,
+		.write = ad5446_write,
 	},
 	[ID_AD5512A] = {
 		.channel = AD5446_CHANNEL(12, 16, 4),
-		.store_sample = ad5542_store_sample,
+		.write = ad5446_write,
 	},
 	[ID_AD5553] = {
 		.channel = AD5446_CHANNEL(14, 16, 0),
-		.store_sample = ad5542_store_sample,
+		.write = ad5446_write,
 	},
 	[ID_AD5601] = {
-		.channel = AD5446_CHANNEL(8, 16, 6),
-		.store_sample = ad5542_store_sample,
-		.store_pwr_down = ad5620_store_pwr_down,
+		.channel = AD5446_CHANNEL_POWERDOWN(8, 16, 6),
+		.write = ad5446_write,
 	},
 	[ID_AD5611] = {
-		.channel = AD5446_CHANNEL(10, 16, 4),
-		.store_sample = ad5542_store_sample,
-		.store_pwr_down = ad5620_store_pwr_down,
+		.channel = AD5446_CHANNEL_POWERDOWN(10, 16, 4),
+		.write = ad5446_write,
 	},
 	[ID_AD5621] = {
-		.channel = AD5446_CHANNEL(12, 16, 2),
-		.store_sample = ad5542_store_sample,
-		.store_pwr_down = ad5620_store_pwr_down,
+		.channel = AD5446_CHANNEL_POWERDOWN(12, 16, 2),
+		.write = ad5446_write,
 	},
 	[ID_AD5620_2500] = {
-		.channel = AD5446_CHANNEL(12, 16, 2),
+		.channel = AD5446_CHANNEL_POWERDOWN(12, 16, 2),
 		.int_vref_mv = 2500,
-		.store_sample = ad5620_store_sample,
-		.store_pwr_down = ad5620_store_pwr_down,
+		.write = ad5446_write,
 	},
 	[ID_AD5620_1250] = {
-		.channel = AD5446_CHANNEL(12, 16, 2),
+		.channel = AD5446_CHANNEL_POWERDOWN(12, 16, 2),
 		.int_vref_mv = 1250,
-		.store_sample = ad5620_store_sample,
-		.store_pwr_down = ad5620_store_pwr_down,
+		.write = ad5446_write,
 	},
 	[ID_AD5640_2500] = {
-		.channel = AD5446_CHANNEL(14, 16, 0),
+		.channel = AD5446_CHANNEL_POWERDOWN(14, 16, 0),
 		.int_vref_mv = 2500,
-		.store_sample = ad5620_store_sample,
-		.store_pwr_down = ad5620_store_pwr_down,
+		.write = ad5446_write,
 	},
 	[ID_AD5640_1250] = {
-		.channel = AD5446_CHANNEL(14, 16, 0),
+		.channel = AD5446_CHANNEL_POWERDOWN(14, 16, 0),
 		.int_vref_mv = 1250,
-		.store_sample = ad5620_store_sample,
-		.store_pwr_down = ad5620_store_pwr_down,
+		.write = ad5446_write,
 	},
 	[ID_AD5660_2500] = {
-		.channel = AD5446_CHANNEL(16, 16, 0),
+		.channel = AD5446_CHANNEL_POWERDOWN(16, 16, 0),
 		.int_vref_mv = 2500,
-		.store_sample = ad5660_store_sample,
-		.store_pwr_down = ad5660_store_pwr_down,
+		.write = ad5660_write,
 	},
 	[ID_AD5660_1250] = {
-		.channel = AD5446_CHANNEL(16, 16, 0),
+		.channel = AD5446_CHANNEL_POWERDOWN(16, 16, 0),
 		.int_vref_mv = 1250,
-		.store_sample = ad5660_store_sample,
-		.store_pwr_down = ad5660_store_pwr_down,
+		.write = ad5660_write,
+	},
+	[ID_AD5662] = {
+		.channel = AD5446_CHANNEL_POWERDOWN(16, 16, 0),
+		.write = ad5660_write,
 	},
 };
 
@@ -254,6 +237,9 @@ static int ad5446_read_raw(struct iio_dev *indio_dev,
 	unsigned long scale_uv;
 
 	switch (m) {
+	case IIO_CHAN_INFO_RAW:
+		*val = st->cached_val;
+		return IIO_VAL_INT;
 	case IIO_CHAN_INFO_SCALE:
 		scale_uv = (st->vref_mv * 1000) >> chan->scan_type.realbits;
 		*val =  scale_uv / 1000;
@@ -271,18 +257,18 @@ static int ad5446_write_raw(struct iio_dev *indio_dev,
 			       long mask)
 {
 	struct ad5446_state *st = iio_priv(indio_dev);
-	int ret;
+	int ret = 0;
 
 	switch (mask) {
-	case 0:
+	case IIO_CHAN_INFO_RAW:
 		if (val >= (1 << chan->scan_type.realbits) || val < 0)
 			return -EINVAL;
 
 		val <<= chan->scan_type.shift;
 		mutex_lock(&indio_dev->mlock);
 		st->cached_val = val;
-		st->chip_info->store_sample(st, val);
-		ret = spi_sync(st->spi, &st->msg);
+		if (!st->pwr_down)
+			ret = st->chip_info->write(st, val);
 		mutex_unlock(&indio_dev->mlock);
 		break;
 	default:
@@ -295,13 +281,6 @@ static int ad5446_write_raw(struct iio_dev *indio_dev,
 static const struct iio_info ad5446_info = {
 	.read_raw = ad5446_read_raw,
 	.write_raw = ad5446_write_raw,
-	.attrs = &ad5446_attribute_group,
-	.driver_module = THIS_MODULE,
-};
-
-static const struct iio_info ad5446_info_no_pwr_down = {
-	.read_raw = ad5446_read_raw,
-	.write_raw = ad5446_write_raw,
 	.driver_module = THIS_MODULE,
 };
 
@@ -321,7 +300,7 @@ static int __devinit ad5446_probe(struct spi_device *spi)
 		voltage_uv = regulator_get_voltage(reg);
 	}
 
-	indio_dev = iio_allocate_device(sizeof(*st));
+	indio_dev = iio_device_alloc(sizeof(*st));
 	if (indio_dev == NULL) {
 		ret = -ENOMEM;
 		goto error_disable_reg;
@@ -337,38 +316,17 @@ static int __devinit ad5446_probe(struct spi_device *spi)
 	/* Establish that the iio_dev is a child of the spi device */
 	indio_dev->dev.parent = &spi->dev;
 	indio_dev->name = spi_get_device_id(spi)->name;
-	if (st->chip_info->store_pwr_down)
-		indio_dev->info = &ad5446_info;
-	else
-		indio_dev->info = &ad5446_info_no_pwr_down;
+	indio_dev->info = &ad5446_info;
 	indio_dev->modes = INDIO_DIRECT_MODE;
 	indio_dev->channels = &st->chip_info->channel;
 	indio_dev->num_channels = 1;
 
-	/* Setup default message */
-
-	st->xfer.tx_buf = &st->data;
-	st->xfer.len = st->chip_info->channel.scan_type.storagebits / 8;
-
-	spi_message_init(&st->msg);
-	spi_message_add_tail(&st->xfer, &st->msg);
-
-	switch (spi_get_device_id(spi)->driver_data) {
-	case ID_AD5620_2500:
-	case ID_AD5620_1250:
-	case ID_AD5640_2500:
-	case ID_AD5640_1250:
-	case ID_AD5660_2500:
-	case ID_AD5660_1250:
+	if (st->chip_info->int_vref_mv)
 		st->vref_mv = st->chip_info->int_vref_mv;
-		break;
-	default:
-		if (voltage_uv)
-			st->vref_mv = voltage_uv / 1000;
-		else
-			dev_warn(&spi->dev,
-				 "reference voltage unspecified\n");
-	}
+	else if (voltage_uv)
+		st->vref_mv = voltage_uv / 1000;
+	else
+		dev_warn(&spi->dev, "reference voltage unspecified\n");
 
 	ret = iio_device_register(indio_dev);
 	if (ret)
@@ -377,7 +335,7 @@ static int __devinit ad5446_probe(struct spi_device *spi)
 	return 0;
 
 error_free_device:
-	iio_free_device(indio_dev);
+	iio_device_free(indio_dev);
 error_disable_reg:
 	if (!IS_ERR(reg))
 		regulator_disable(reg);
@@ -398,7 +356,7 @@ static int ad5446_remove(struct spi_device *spi)
 		regulator_disable(st->reg);
 		regulator_put(st->reg);
 	}
-	iio_free_device(indio_dev);
+	iio_device_free(indio_dev);
 
 	return 0;
 }
@@ -408,8 +366,8 @@ static const struct spi_device_id ad5446_id[] = {
 	{"ad5446", ID_AD5446},
 	{"ad5512a", ID_AD5512A},
 	{"ad5541a", ID_AD5541A},
-	{"ad5542a", ID_AD5542A},
-	{"ad5543", ID_AD5543},
+	{"ad5542a", ID_AD5541A}, /* ad5541a and ad5542a are compatible */
+	{"ad5543", ID_AD5541A}, /* ad5541a and ad5543 are compatible */
 	{"ad5553", ID_AD5553},
 	{"ad5601", ID_AD5601},
 	{"ad5611", ID_AD5611},
@@ -420,6 +378,7 @@ static const struct spi_device_id ad5446_id[] = {
 	{"ad5640-1250", ID_AD5640_1250},
 	{"ad5660-2500", ID_AD5660_2500},
 	{"ad5660-1250", ID_AD5660_1250},
+	{"ad5662", ID_AD5662},
 	{}
 };
 MODULE_DEVICE_TABLE(spi, ad5446_id);
diff --git a/drivers/staging/iio/dac/ad5446.h b/drivers/staging/iio/dac/ad5446.h
index 4ea3476fb06..dfd68ce7427 100644
--- a/drivers/staging/iio/dac/ad5446.h
+++ b/drivers/staging/iio/dac/ad5446.h
@@ -34,43 +34,30 @@
  * @spi:		spi_device
  * @chip_info:		chip model specific constants, available modes etc
  * @reg:		supply regulator
- * @poll_work:		bottom half of polling interrupt handler
  * @vref_mv:		actual reference voltage used
- * @xfer:		default spi transfer
- * @msg:		default spi message
- * @data:		spi transmit buffer
  */
 
 struct ad5446_state {
 	struct spi_device		*spi;
 	const struct ad5446_chip_info	*chip_info;
 	struct regulator		*reg;
-	struct work_struct		poll_work;
 	unsigned short			vref_mv;
 	unsigned			cached_val;
 	unsigned			pwr_down_mode;
 	unsigned			pwr_down;
-	struct spi_transfer		xfer;
-	struct spi_message		msg;
-	union {
-		unsigned short		d16;
-		unsigned char		d24[3];
-	} data;
 };
 
 /**
  * struct ad5446_chip_info - chip specific information
  * @channel:		channel spec for the DAC
  * @int_vref_mv:	AD5620/40/60: the internal reference voltage
- * @store_sample:	chip specific helper function to store the datum
- * @store_sample:	chip specific helper function to store the powerpown cmd
+ * @write:		chip specific helper function to write to the register
  */
 
 struct ad5446_chip_info {
 	struct iio_chan_spec	channel;
 	u16			int_vref_mv;
-	void (*store_sample)	(struct ad5446_state *st, unsigned val);
-	void (*store_pwr_down)	(struct ad5446_state *st, unsigned mode);
+	int			(*write)(struct ad5446_state *st, unsigned val);
 };
 
 /**
@@ -85,8 +72,6 @@ enum ad5446_supported_device_ids {
 	ID_AD5444,
 	ID_AD5446,
 	ID_AD5541A,
-	ID_AD5542A,
-	ID_AD5543,
 	ID_AD5512A,
 	ID_AD5553,
 	ID_AD5601,
@@ -98,6 +83,7 @@ enum ad5446_supported_device_ids {
 	ID_AD5640_1250,
 	ID_AD5660_2500,
 	ID_AD5660_1250,
+	ID_AD5662,
 };
 
 #endif /* IIO_DAC_AD5446_H_ */
diff --git a/drivers/staging/iio/dac/ad5504.c b/drivers/staging/iio/dac/ad5504.c
index bc17205fe72..019cf15cd24 100644
--- a/drivers/staging/iio/dac/ad5504.c
+++ b/drivers/staging/iio/dac/ad5504.c
@@ -16,9 +16,9 @@
 #include <linux/regulator/consumer.h>
 #include <linux/module.h>
 
-#include "../iio.h"
-#include "../sysfs.h"
-#include "../events.h"
+#include <linux/iio/iio.h>
+#include <linux/iio/sysfs.h>
+#include <linux/iio/events.h>
 #include "dac.h"
 #include "ad5504.h"
 
@@ -27,7 +27,8 @@
 	.indexed = 1, \
 	.output = 1, \
 	.channel = (_chan), \
-	.info_mask = IIO_CHAN_INFO_SCALE_SHARED_BIT, \
+	.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT | \
+		     IIO_CHAN_INFO_SCALE_SHARED_BIT, \
 	.address = AD5504_ADDR_DAC(_chan), \
 	.scan_type = IIO_ST('u', 12, 16, 0), \
 }
@@ -81,7 +82,7 @@ static int ad5504_read_raw(struct iio_dev *indio_dev,
 	int ret;
 
 	switch (m) {
-	case 0:
+	case IIO_CHAN_INFO_RAW:
 		ret = ad5504_spi_read(st->spi, chan->address);
 		if (ret < 0)
 			return ret;
@@ -109,7 +110,7 @@ static int ad5504_write_raw(struct iio_dev *indio_dev,
 	int ret;
 
 	switch (mask) {
-	case 0:
+	case IIO_CHAN_INFO_RAW:
 		if (val >= (1 << chan->scan_type.realbits) || val < 0)
 			return -EINVAL;
 
@@ -124,7 +125,7 @@ static int ad5504_write_raw(struct iio_dev *indio_dev,
 static ssize_t ad5504_read_powerdown_mode(struct device *dev,
 				      struct device_attribute *attr, char *buf)
 {
-	struct iio_dev *indio_dev = dev_get_drvdata(dev);
+	struct iio_dev *indio_dev = dev_to_iio_dev(dev);
 	struct ad5504_state *st = iio_priv(indio_dev);
 
 	const char mode[][14] = {"20kohm_to_gnd", "three_state"};
@@ -136,7 +137,7 @@ static ssize_t ad5504_write_powerdown_mode(struct device *dev,
 				       struct device_attribute *attr,
 				       const char *buf, size_t len)
 {
-	struct iio_dev *indio_dev = dev_get_drvdata(dev);
+	struct iio_dev *indio_dev = dev_to_iio_dev(dev);
 	struct ad5504_state *st = iio_priv(indio_dev);
 	int ret;
 
@@ -154,7 +155,7 @@ static ssize_t ad5504_read_dac_powerdown(struct device *dev,
 					   struct device_attribute *attr,
 					   char *buf)
 {
-	struct iio_dev *indio_dev = dev_get_drvdata(dev);
+	struct iio_dev *indio_dev = dev_to_iio_dev(dev);
 	struct ad5504_state *st = iio_priv(indio_dev);
 	struct iio_dev_attr *this_attr = to_iio_dev_attr(attr);
 
@@ -168,7 +169,7 @@ static ssize_t ad5504_write_dac_powerdown(struct device *dev,
 {
 	long readin;
 	int ret;
-	struct iio_dev *indio_dev = dev_get_drvdata(dev);
+	struct iio_dev *indio_dev = dev_to_iio_dev(dev);
 	struct ad5504_state *st = iio_priv(indio_dev);
 	struct iio_dev_attr *this_attr = to_iio_dev_attr(attr);
 
@@ -287,7 +288,7 @@ static int __devinit ad5504_probe(struct spi_device *spi)
 	struct regulator *reg;
 	int ret, voltage_uv = 0;
 
-	indio_dev = iio_allocate_device(sizeof(*st));
+	indio_dev = iio_device_alloc(sizeof(*st));
 	if (indio_dev == NULL) {
 		ret = -ENOMEM;
 		goto error_ret;
@@ -350,7 +351,7 @@ error_put_reg:
 	if (!IS_ERR(reg))
 		regulator_put(reg);
 
-	iio_free_device(indio_dev);
+	iio_device_free(indio_dev);
 error_ret:
 	return ret;
 }
@@ -368,7 +369,7 @@ static int __devexit ad5504_remove(struct spi_device *spi)
 		regulator_disable(st->reg);
 		regulator_put(st->reg);
 	}
-	iio_free_device(indio_dev);
+	iio_device_free(indio_dev);
 
 	return 0;
 }
diff --git a/drivers/staging/iio/dac/ad5624r_spi.c b/drivers/staging/iio/dac/ad5624r_spi.c
index 10c7484366e..42ff644ac43 100644
--- a/drivers/staging/iio/dac/ad5624r_spi.c
+++ b/drivers/staging/iio/dac/ad5624r_spi.c
@@ -16,8 +16,8 @@
 #include <linux/regulator/consumer.h>
 #include <linux/module.h>
 
-#include "../iio.h"
-#include "../sysfs.h"
+#include <linux/iio/iio.h>
+#include <linux/iio/sysfs.h>
 #include "dac.h"
 #include "ad5624r.h"
 
@@ -26,7 +26,8 @@
 	.indexed = 1, \
 	.output = 1, \
 	.channel = (_chan), \
-	.info_mask = IIO_CHAN_INFO_SCALE_SHARED_BIT, \
+	.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT | \
+		     IIO_CHAN_INFO_SCALE_SHARED_BIT, \
 	.address = (_chan), \
 	.scan_type = IIO_ST('u', (_bits), 16, 16 - (_bits)), \
 }
@@ -122,7 +123,7 @@ static int ad5624r_write_raw(struct iio_dev *indio_dev,
 	int ret;
 
 	switch (mask) {
-	case 0:
+	case IIO_CHAN_INFO_RAW:
 		if (val >= (1 << chan->scan_type.realbits) || val < 0)
 			return -EINVAL;
 
@@ -140,7 +141,7 @@ static int ad5624r_write_raw(struct iio_dev *indio_dev,
 static ssize_t ad5624r_read_powerdown_mode(struct device *dev,
 				      struct device_attribute *attr, char *buf)
 {
-	struct iio_dev *indio_dev = dev_get_drvdata(dev);
+	struct iio_dev *indio_dev = dev_to_iio_dev(dev);
 	struct ad5624r_state *st = iio_priv(indio_dev);
 
 	char mode[][15] = {"", "1kohm_to_gnd", "100kohm_to_gnd", "three_state"};
@@ -152,7 +153,7 @@ static ssize_t ad5624r_write_powerdown_mode(struct device *dev,
 				       struct device_attribute *attr,
 				       const char *buf, size_t len)
 {
-	struct iio_dev *indio_dev = dev_get_drvdata(dev);
+	struct iio_dev *indio_dev = dev_to_iio_dev(dev);
 	struct ad5624r_state *st = iio_priv(indio_dev);
 	int ret;
 
@@ -172,7 +173,7 @@ static ssize_t ad5624r_read_dac_powerdown(struct device *dev,
 					   struct device_attribute *attr,
 					   char *buf)
 {
-	struct iio_dev *indio_dev = dev_get_drvdata(dev);
+	struct iio_dev *indio_dev = dev_to_iio_dev(dev);
 	struct ad5624r_state *st = iio_priv(indio_dev);
 	struct iio_dev_attr *this_attr = to_iio_dev_attr(attr);
 
@@ -186,7 +187,7 @@ static ssize_t ad5624r_write_dac_powerdown(struct device *dev,
 {
 	long readin;
 	int ret;
-	struct iio_dev *indio_dev = dev_get_drvdata(dev);
+	struct iio_dev *indio_dev = dev_to_iio_dev(dev);
 	struct ad5624r_state *st = iio_priv(indio_dev);
 	struct iio_dev_attr *this_attr = to_iio_dev_attr(attr);
 
@@ -255,7 +256,7 @@ static int __devinit ad5624r_probe(struct spi_device *spi)
 	struct iio_dev *indio_dev;
 	int ret, voltage_uv = 0;
 
-	indio_dev = iio_allocate_device(sizeof(*st));
+	indio_dev = iio_device_alloc(sizeof(*st));
 	if (indio_dev == NULL) {
 		ret = -ENOMEM;
 		goto error_ret;
@@ -305,7 +306,7 @@ error_disable_reg:
 error_put_reg:
 	if (!IS_ERR(st->reg))
 		regulator_put(st->reg);
-	iio_free_device(indio_dev);
+	iio_device_free(indio_dev);
 error_ret:
 
 	return ret;
@@ -321,7 +322,7 @@ static int __devexit ad5624r_remove(struct spi_device *spi)
 		regulator_disable(st->reg);
 		regulator_put(st->reg);
 	}
-	iio_free_device(indio_dev);
+	iio_device_free(indio_dev);
 
 	return 0;
 }
diff --git a/drivers/staging/iio/dac/ad5686.c b/drivers/staging/iio/dac/ad5686.c
index 2415a6e60c7..c1e903ebc7b 100644
--- a/drivers/staging/iio/dac/ad5686.c
+++ b/drivers/staging/iio/dac/ad5686.c
@@ -16,8 +16,8 @@
 #include <linux/sysfs.h>
 #include <linux/regulator/consumer.h>
 
-#include "../iio.h"
-#include "../sysfs.h"
+#include <linux/iio/iio.h>
+#include <linux/iio/sysfs.h>
 #include "dac.h"
 
 #define AD5686_DAC_CHANNELS			4
@@ -98,7 +98,8 @@ enum ad5686_supported_device_ids {
 		.indexed = 1,					\
 		.output = 1,					\
 		.channel = chan,				\
-		.info_mask = IIO_CHAN_INFO_SCALE_SHARED_BIT,	\
+		.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT |	\
+		IIO_CHAN_INFO_SCALE_SHARED_BIT,			\
 		.address = AD5686_ADDR_DAC(chan),			\
 		.scan_type = IIO_ST('u', bits, 16, shift)	\
 }
@@ -172,7 +173,7 @@ static int ad5686_spi_read(struct ad5686_state *st, u8 addr)
 static ssize_t ad5686_read_powerdown_mode(struct device *dev,
 				      struct device_attribute *attr, char *buf)
 {
-	struct iio_dev *indio_dev = dev_get_drvdata(dev);
+	struct iio_dev *indio_dev = dev_to_iio_dev(dev);
 	struct ad5686_state *st = iio_priv(indio_dev);
 	struct iio_dev_attr *this_attr = to_iio_dev_attr(attr);
 
@@ -186,7 +187,7 @@ static ssize_t ad5686_write_powerdown_mode(struct device *dev,
 				       struct device_attribute *attr,
 				       const char *buf, size_t len)
 {
-	struct iio_dev *indio_dev = dev_get_drvdata(dev);
+	struct iio_dev *indio_dev = dev_to_iio_dev(dev);
 	struct ad5686_state *st = iio_priv(indio_dev);
 	struct iio_dev_attr *this_attr = to_iio_dev_attr(attr);
 	unsigned mode;
@@ -210,7 +211,7 @@ static ssize_t ad5686_read_dac_powerdown(struct device *dev,
 					   struct device_attribute *attr,
 					   char *buf)
 {
-	struct iio_dev *indio_dev = dev_get_drvdata(dev);
+	struct iio_dev *indio_dev = dev_to_iio_dev(dev);
 	struct ad5686_state *st = iio_priv(indio_dev);
 	struct iio_dev_attr *this_attr = to_iio_dev_attr(attr);
 
@@ -224,7 +225,7 @@ static ssize_t ad5686_write_dac_powerdown(struct device *dev,
 {
 	bool readin;
 	int ret;
-	struct iio_dev *indio_dev = dev_get_drvdata(dev);
+	struct iio_dev *indio_dev = dev_to_iio_dev(dev);
 	struct ad5686_state *st = iio_priv(indio_dev);
 	struct iio_dev_attr *this_attr = to_iio_dev_attr(attr);
 
@@ -296,7 +297,7 @@ static int ad5686_read_raw(struct iio_dev *indio_dev,
 	int ret;
 
 	switch (m) {
-	case 0:
+	case IIO_CHAN_INFO_RAW:
 		mutex_lock(&indio_dev->mlock);
 		ret = ad5686_spi_read(st, chan->address);
 		mutex_unlock(&indio_dev->mlock);
@@ -326,7 +327,7 @@ static int ad5686_write_raw(struct iio_dev *indio_dev,
 	int ret;
 
 	switch (mask) {
-	case 0:
+	case IIO_CHAN_INFO_RAW:
 		if (val > (1 << chan->scan_type.realbits) || val < 0)
 			return -EINVAL;
 
@@ -358,7 +359,7 @@ static int __devinit ad5686_probe(struct spi_device *spi)
 	struct iio_dev *indio_dev;
 	int ret, regdone = 0, voltage_uv = 0;
 
-	indio_dev = iio_allocate_device(sizeof(*st));
+	indio_dev = iio_device_alloc(sizeof(*st));
 	if (indio_dev == NULL)
 		return  -ENOMEM;
 
@@ -410,7 +411,7 @@ error_put_reg:
 	if (!IS_ERR(st->reg))
 		regulator_put(st->reg);
 
-	iio_free_device(indio_dev);
+	iio_device_free(indio_dev);
 
 	return ret;
 }
@@ -425,7 +426,7 @@ static int __devexit ad5686_remove(struct spi_device *spi)
 		regulator_disable(st->reg);
 		regulator_put(st->reg);
 	}
-	iio_free_device(indio_dev);
+	iio_device_free(indio_dev);
 
 	return 0;
 }
diff --git a/drivers/staging/iio/dac/ad5764.c b/drivers/staging/iio/dac/ad5764.c
index f73a7307949..03dbd937b08 100644
--- a/drivers/staging/iio/dac/ad5764.c
+++ b/drivers/staging/iio/dac/ad5764.c
@@ -16,8 +16,8 @@
 #include <linux/sysfs.h>
 #include <linux/regulator/consumer.h>
 
-#include "../iio.h"
-#include "../sysfs.h"
+#include <linux/iio/iio.h>
+#include <linux/iio/sysfs.h>
 #include "dac.h"
 
 #define AD5764_REG_SF_NOP			0x0
@@ -79,7 +79,8 @@ enum ad5764_type {
 	.output = 1,						\
 	.channel = (_chan),					\
 	.address = (_chan),					\
-	.info_mask = IIO_CHAN_INFO_OFFSET_SHARED_BIT |		\
+	.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT |		\
+		IIO_CHAN_INFO_OFFSET_SHARED_BIT |		\
 		IIO_CHAN_INFO_SCALE_SEPARATE_BIT |		\
 		IIO_CHAN_INFO_CALIBSCALE_SEPARATE_BIT |		\
 		IIO_CHAN_INFO_CALIBBIAS_SEPARATE_BIT,		\
@@ -188,7 +189,7 @@ static int ad5764_write_raw(struct iio_dev *indio_dev,
 	unsigned int reg;
 
 	switch (info) {
-	case 0:
+	case IIO_CHAN_INFO_RAW:
 		if (val >= max_val || val < 0)
 			return -EINVAL;
 		val <<= chan->scan_type.shift;
@@ -228,7 +229,7 @@ static int ad5764_read_raw(struct iio_dev *indio_dev,
 	int ret;
 
 	switch (info) {
-	case 0:
+	case IIO_CHAN_INFO_RAW:
 		reg = AD5764_REG_DATA(chan->address);
 		ret = ad5764_read(indio_dev, reg, val);
 		if (ret < 0)
@@ -280,7 +281,7 @@ static int __devinit ad5764_probe(struct spi_device *spi)
 	struct ad5764_state *st;
 	int ret;
 
-	indio_dev = iio_allocate_device(sizeof(*st));
+	indio_dev = iio_device_alloc(sizeof(*st));
 	if (indio_dev == NULL) {
 		dev_err(&spi->dev, "Failed to allocate iio device\n");
 		return -ENOMEM;
@@ -335,7 +336,7 @@ error_free_reg:
 	if (st->chip_info->int_vref == 0)
 		regulator_bulk_free(ARRAY_SIZE(st->vref_reg), st->vref_reg);
 error_free:
-	iio_free_device(indio_dev);
+	iio_device_free(indio_dev);
 
 	return ret;
 }
@@ -352,7 +353,7 @@ static int __devexit ad5764_remove(struct spi_device *spi)
 		regulator_bulk_free(ARRAY_SIZE(st->vref_reg), st->vref_reg);
 	}
 
-	iio_free_device(indio_dev);
+	iio_device_free(indio_dev);
 
 	return 0;
 }
diff --git a/drivers/staging/iio/dac/ad5791.c b/drivers/staging/iio/dac/ad5791.c
index ac45636a8d7..13d8b5bb1ce 100644
--- a/drivers/staging/iio/dac/ad5791.c
+++ b/drivers/staging/iio/dac/ad5791.c
@@ -17,8 +17,8 @@
 #include <linux/regulator/consumer.h>
 #include <linux/module.h>
 
-#include "../iio.h"
-#include "../sysfs.h"
+#include <linux/iio/iio.h>
+#include <linux/iio/sysfs.h>
 #include "dac.h"
 #include "ad5791.h"
 
@@ -78,7 +78,8 @@ static int ad5791_spi_read(struct spi_device *spi, u8 addr, u32 *val)
 	.indexed = 1,					\
 	.address = AD5791_ADDR_DAC0,			\
 	.channel = 0,					\
-	.info_mask = IIO_CHAN_INFO_SCALE_SHARED_BIT | \
+	.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT |	\
+		IIO_CHAN_INFO_SCALE_SHARED_BIT |	\
 		IIO_CHAN_INFO_OFFSET_SHARED_BIT,	\
 	.scan_type = IIO_ST('u', bits, 24, shift)	\
 }
@@ -93,7 +94,7 @@ static const struct iio_chan_spec ad5791_channels[] = {
 static ssize_t ad5791_read_powerdown_mode(struct device *dev,
 				      struct device_attribute *attr, char *buf)
 {
-	struct iio_dev *indio_dev = dev_get_drvdata(dev);
+	struct iio_dev *indio_dev = dev_to_iio_dev(dev);
 	struct ad5791_state *st = iio_priv(indio_dev);
 
 	const char mode[][14] = {"6kohm_to_gnd", "three_state"};
@@ -105,7 +106,7 @@ static ssize_t ad5791_write_powerdown_mode(struct device *dev,
 				       struct device_attribute *attr,
 				       const char *buf, size_t len)
 {
-	struct iio_dev *indio_dev = dev_get_drvdata(dev);
+	struct iio_dev *indio_dev = dev_to_iio_dev(dev);
 	struct ad5791_state *st = iio_priv(indio_dev);
 	int ret;
 
@@ -123,7 +124,7 @@ static ssize_t ad5791_read_dac_powerdown(struct device *dev,
 					   struct device_attribute *attr,
 					   char *buf)
 {
-	struct iio_dev *indio_dev = dev_get_drvdata(dev);
+	struct iio_dev *indio_dev = dev_to_iio_dev(dev);
 	struct ad5791_state *st = iio_priv(indio_dev);
 
 	return sprintf(buf, "%d\n", st->pwr_down);
@@ -135,7 +136,7 @@ static ssize_t ad5791_write_dac_powerdown(struct device *dev,
 {
 	long readin;
 	int ret;
-	struct iio_dev *indio_dev = dev_get_drvdata(dev);
+	struct iio_dev *indio_dev = dev_to_iio_dev(dev);
 	struct ad5791_state *st = iio_priv(indio_dev);
 
 	ret = strict_strtol(buf, 10, &readin);
@@ -231,7 +232,7 @@ static int ad5791_read_raw(struct iio_dev *indio_dev,
 	int ret;
 
 	switch (m) {
-	case 0:
+	case IIO_CHAN_INFO_RAW:
 		ret = ad5791_spi_read(st->spi, chan->address, val);
 		if (ret)
 			return ret;
@@ -263,7 +264,7 @@ static int ad5791_write_raw(struct iio_dev *indio_dev,
 	struct ad5791_state *st = iio_priv(indio_dev);
 
 	switch (mask) {
-	case 0:
+	case IIO_CHAN_INFO_RAW:
 		val &= AD5791_RES_MASK(chan->scan_type.realbits);
 		val <<= chan->scan_type.shift;
 
@@ -288,7 +289,7 @@ static int __devinit ad5791_probe(struct spi_device *spi)
 	struct ad5791_state *st;
 	int ret, pos_voltage_uv = 0, neg_voltage_uv = 0;
 
-	indio_dev = iio_allocate_device(sizeof(*st));
+	indio_dev = iio_device_alloc(sizeof(*st));
 	if (indio_dev == NULL) {
 		ret = -ENOMEM;
 		goto error_ret;
@@ -368,7 +369,7 @@ error_put_reg_neg:
 error_put_reg_pos:
 	if (!IS_ERR(st->reg_vdd))
 		regulator_put(st->reg_vdd);
-	iio_free_device(indio_dev);
+	iio_device_free(indio_dev);
 error_ret:
 
 	return ret;
@@ -389,7 +390,7 @@ static int __devexit ad5791_remove(struct spi_device *spi)
 		regulator_disable(st->reg_vss);
 		regulator_put(st->reg_vss);
 	}
-	iio_free_device(indio_dev);
+	iio_device_free(indio_dev);
 
 	return 0;
 }
diff --git a/drivers/staging/iio/dac/max517.c b/drivers/staging/iio/dac/max517.c
index 41483c72cec..5287cad1f3a 100644
--- a/drivers/staging/iio/dac/max517.c
+++ b/drivers/staging/iio/dac/max517.c
@@ -25,8 +25,8 @@
 #include <linux/i2c.h>
 #include <linux/err.h>
 
-#include "../iio.h"
-#include "../sysfs.h"
+#include <linux/iio/iio.h>
+#include <linux/iio/sysfs.h>
 #include "dac.h"
 
 #include "max517.h"
@@ -59,7 +59,7 @@ static ssize_t max517_set_value(struct device *dev,
 				 struct device_attribute *attr,
 				 const char *buf, size_t count, int channel)
 {
-	struct iio_dev *indio_dev = dev_get_drvdata(dev);
+	struct iio_dev *indio_dev = dev_to_iio_dev(dev);
 	struct max517_data *data = iio_priv(indio_dev);
 	struct i2c_client *client = data->client;
 	u8 outbuf[4]; /* 1x or 2x command + value */
@@ -128,7 +128,7 @@ static ssize_t max517_show_scale(struct device *dev,
 				struct device_attribute *attr,
 				char *buf, int channel)
 {
-	struct iio_dev *indio_dev = dev_get_drvdata(dev);
+	struct iio_dev *indio_dev = dev_to_iio_dev(dev);
 	struct max517_data *data = iio_priv(indio_dev);
 	/* Corresponds to Vref / 2^(bits) */
 	unsigned int scale_uv = (data->vref_mv[channel - 1] * 1000) >> 8;
@@ -218,7 +218,7 @@ static int max517_probe(struct i2c_client *client,
 	struct max517_platform_data *platform_data = client->dev.platform_data;
 	int err;
 
-	indio_dev = iio_allocate_device(sizeof(*data));
+	indio_dev = iio_device_alloc(sizeof(*data));
 	if (indio_dev == NULL) {
 		err = -ENOMEM;
 		goto exit;
@@ -257,14 +257,15 @@ static int max517_probe(struct i2c_client *client,
 	return 0;
 
 exit_free_device:
-	iio_free_device(indio_dev);
+	iio_device_free(indio_dev);
 exit:
 	return err;
 }
 
 static int max517_remove(struct i2c_client *client)
 {
-	iio_free_device(i2c_get_clientdata(client));
+	iio_device_unregister(i2c_get_clientdata(client));
+	iio_device_free(i2c_get_clientdata(client));
 
 	return 0;
 }
diff --git a/drivers/staging/iio/dds/dds.h b/drivers/staging/iio/dds/dds.h
deleted file mode 100644
index d8ac3a93baf..00000000000
--- a/drivers/staging/iio/dds/dds.h
+++ /dev/null
@@ -1,110 +0,0 @@
-/*
- * dds.h - sysfs attributes associated with DDS devices
- *
- * Copyright (c) 2010 Analog Devices Inc.
- *
- * Licensed under the GPL-2 or later.
- */
-
-/**
- * /sys/bus/iio/devices/.../ddsX_freqY
- */
-
-#define IIO_DEV_ATTR_FREQ(_channel, _num, _mode, _show, _store, _addr)	\
-	IIO_DEVICE_ATTR(dds##_channel##_freq##_num,			\
-			_mode, _show, _store, _addr)
-
-/**
- * /sys/bus/iio/devices/.../ddsX_freqY_scale
- */
-
-#define IIO_CONST_ATTR_FREQ_SCALE(_channel, _string)			\
-	IIO_CONST_ATTR(dds##_channel##_freq_scale, _string)
-
-/**
- * /sys/bus/iio/devices/.../ddsX_freqsymbol
- */
-
-#define IIO_DEV_ATTR_FREQSYMBOL(_channel, _mode, _show, _store, _addr)	\
-	IIO_DEVICE_ATTR(dds##_channel##_freqsymbol,			\
-			_mode, _show, _store, _addr);
-
-/**
- * /sys/bus/iio/devices/.../ddsX_phaseY
- */
-
-#define IIO_DEV_ATTR_PHASE(_channel, _num, _mode, _show, _store, _addr)	\
-	IIO_DEVICE_ATTR(dds##_channel##_phase##_num,			\
-			_mode, _show, _store, _addr)
-
-/**
- * /sys/bus/iio/devices/.../ddsX_phaseY_scale
- */
-
-#define IIO_CONST_ATTR_PHASE_SCALE(_channel, _string)			\
-	IIO_CONST_ATTR(dds##_channel##_phase_scale, _string)
-
-/**
- * /sys/bus/iio/devices/.../ddsX_phasesymbol
- */
-
-#define IIO_DEV_ATTR_PHASESYMBOL(_channel, _mode, _show, _store, _addr)	\
-	IIO_DEVICE_ATTR(dds##_channel##_phasesymbol,			\
-			_mode, _show, _store, _addr);
-
-/**
- * /sys/bus/iio/devices/.../ddsX_pincontrol_en
- */
-
-#define IIO_DEV_ATTR_PINCONTROL_EN(_channel, _mode, _show, _store, _addr)\
-	IIO_DEVICE_ATTR(dds##_channel##_pincontrol_en,			\
-			_mode, _show, _store, _addr);
-
-/**
- * /sys/bus/iio/devices/.../ddsX_pincontrol_freq_en
- */
-
-#define IIO_DEV_ATTR_PINCONTROL_FREQ_EN(_channel, _mode, _show, _store, _addr)\
-	IIO_DEVICE_ATTR(dds##_channel##_pincontrol_freq_en,		\
-			_mode, _show, _store, _addr);
-
-/**
- * /sys/bus/iio/devices/.../ddsX_pincontrol_phase_en
- */
-
-#define IIO_DEV_ATTR_PINCONTROL_PHASE_EN(_channel, _mode, _show, _store, _addr)\
-	IIO_DEVICE_ATTR(dds##_channel##_pincontrol_phase_en,		\
-			_mode, _show, _store, _addr);
-
-/**
- * /sys/bus/iio/devices/.../ddsX_out_enable
- */
-
-#define IIO_DEV_ATTR_OUT_ENABLE(_channel, _mode, _show, _store, _addr)	\
-	IIO_DEVICE_ATTR(dds##_channel##_out_enable,			\
-			_mode, _show, _store, _addr);
-
-/**
- * /sys/bus/iio/devices/.../ddsX_outY_enable
- */
-
-#define IIO_DEV_ATTR_OUTY_ENABLE(_channel, _output,			\
-			_mode, _show, _store, _addr)			\
-	IIO_DEVICE_ATTR(dds##_channel##_out##_output##_enable,		\
-			_mode, _show, _store, _addr);
-
-/**
- * /sys/bus/iio/devices/.../ddsX_outY_wavetype
- */
-
-#define IIO_DEV_ATTR_OUT_WAVETYPE(_channel, _output, _store, _addr)	\
-	IIO_DEVICE_ATTR(dds##_channel##_out##_output##_wavetype,	\
-			S_IWUSR, NULL, _store, _addr);
-
-/**
- * /sys/bus/iio/devices/.../ddsX_outY_wavetype_available
- */
-
-#define IIO_CONST_ATTR_OUT_WAVETYPES_AVAILABLE(_channel, _output, _modes)\
-	IIO_CONST_ATTR(dds##_channel##_out##_output##_wavetype_available,\
-			_modes);
diff --git a/drivers/staging/iio/driver.h b/drivers/staging/iio/driver.h
deleted file mode 100644
index a4f8b2e05af..00000000000
--- a/drivers/staging/iio/driver.h
+++ /dev/null
@@ -1,34 +0,0 @@
-/*
- * Industrial I/O in kernel access map interface.
- *
- * Copyright (c) 2011 Jonathan Cameron
- *
- * 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 _IIO_INKERN_H_
-#define _IIO_INKERN_H_
-
-struct iio_map;
-
-/**
- * iio_map_array_register() - tell the core about inkernel consumers
- * @indio_dev:	provider device
- * @map:	array of mappings specifying association of channel with client
- */
-int iio_map_array_register(struct iio_dev *indio_dev,
-			   struct iio_map *map);
-
-/**
- * iio_map_array_unregister() - tell the core to remove consumer mappings
- * @indio_dev:	provider device
- * @map:	array of mappings to remove. Note these must have same memory
- *		addresses as those originally added not just equal parameter
- *		values.
- */
-int iio_map_array_unregister(struct iio_dev *indio_dev,
-			     struct iio_map *map);
-
-#endif
diff --git a/drivers/staging/iio/events.h b/drivers/staging/iio/events.h
deleted file mode 100644
index c25f0e3c92e..00000000000
--- a/drivers/staging/iio/events.h
+++ /dev/null
@@ -1,105 +0,0 @@
-/* The industrial I/O - event passing to userspace
- *
- * Copyright (c) 2008-2011 Jonathan Cameron
- *
- * 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 _IIO_EVENTS_H_
-#define _IIO_EVENTS_H_
-
-#include <linux/ioctl.h>
-#include <linux/types.h>
-#include "types.h"
-
-/**
- * struct iio_event_data - The actual event being pushed to userspace
- * @id:		event identifier
- * @timestamp:	best estimate of time of event occurrence (often from
- *		the interrupt handler)
- */
-struct iio_event_data {
-	__u64	id;
-	__s64	timestamp;
-};
-
-#define IIO_GET_EVENT_FD_IOCTL _IOR('i', 0x90, int)
-
-enum iio_event_type {
-	IIO_EV_TYPE_THRESH,
-	IIO_EV_TYPE_MAG,
-	IIO_EV_TYPE_ROC,
-	IIO_EV_TYPE_THRESH_ADAPTIVE,
-	IIO_EV_TYPE_MAG_ADAPTIVE,
-};
-
-enum iio_event_direction {
-	IIO_EV_DIR_EITHER,
-	IIO_EV_DIR_RISING,
-	IIO_EV_DIR_FALLING,
-};
-
-/**
- * IIO_EVENT_CODE() - create event identifier
- * @chan_type:	Type of the channel. Should be one of enum iio_chan_type.
- * @diff:	Whether the event is for an differential channel or not.
- * @modifier:	Modifier for the channel. Should be one of enum iio_modifier.
- * @direction:	Direction of the event. One of enum iio_event_direction.
- * @type:	Type of the event. Should be one enum iio_event_type.
- * @chan:	Channel number for non-differential channels.
- * @chan1:	First channel number for differential channels.
- * @chan2:	Second channel number for differential channels.
- */
-
-#define IIO_EVENT_CODE(chan_type, diff, modifier, direction,		\
-		       type, chan, chan1, chan2)			\
-	(((u64)type << 56) | ((u64)diff << 55) |			\
-	 ((u64)direction << 48) | ((u64)modifier << 40) |		\
-	 ((u64)chan_type << 32) | (((u16)chan2) << 16) | ((u16)chan1) | \
-	 ((u16)chan))
-
-
-#define IIO_EV_DIR_MAX 4
-#define IIO_EV_BIT(type, direction)			\
-	(1 << (type*IIO_EV_DIR_MAX + direction))
-
-/**
- * IIO_MOD_EVENT_CODE() - create event identifier for modified channels
- * @chan_type:	Type of the channel. Should be one of enum iio_chan_type.
- * @number:	Channel number.
- * @modifier:	Modifier for the channel. Should be one of enum iio_modifier.
- * @type:	Type of the event. Should be one enum iio_event_type.
- * @direction:	Direction of the event. One of enum iio_event_direction.
- */
-
-#define IIO_MOD_EVENT_CODE(chan_type, number, modifier,		\
-			   type, direction)				\
-	IIO_EVENT_CODE(chan_type, 0, modifier, direction, type, number, 0, 0)
-
-/**
- * IIO_UNMOD_EVENT_CODE() - create event identifier for unmodified channels
- * @chan_type:	Type of the channel. Should be one of enum iio_chan_type.
- * @number:	Channel number.
- * @type:	Type of the event. Should be one enum iio_event_type.
- * @direction:	Direction of the event. One of enum iio_event_direction.
- */
-
-#define IIO_UNMOD_EVENT_CODE(chan_type, number, type, direction)	\
-	IIO_EVENT_CODE(chan_type, 0, 0, direction, type, number, 0, 0)
-
-#define IIO_EVENT_CODE_EXTRACT_TYPE(mask) ((mask >> 56) & 0xFF)
-
-#define IIO_EVENT_CODE_EXTRACT_DIR(mask) ((mask >> 48) & 0xCF)
-
-#define IIO_EVENT_CODE_EXTRACT_CHAN_TYPE(mask) ((mask >> 32) & 0xFF)
-
-/* Event code number extraction depends on which type of event we have.
- * Perhaps review this function in the future*/
-#define IIO_EVENT_CODE_EXTRACT_CHAN(mask) ((__s16)(mask & 0xFFFF))
-#define IIO_EVENT_CODE_EXTRACT_CHAN2(mask) ((__s16)(((mask) >> 16) & 0xFFFF))
-
-#define IIO_EVENT_CODE_EXTRACT_MODIFIER(mask) ((mask >> 40) & 0xFF)
-#define IIO_EVENT_CODE_EXTRACT_DIFF(mask) (((mask) >> 55) & 0x1)
-
-#endif
diff --git a/drivers/staging/iio/dds/Kconfig b/drivers/staging/iio/frequency/Kconfig
index 93b7141b2c1..93b7141b2c1 100644
--- a/drivers/staging/iio/dds/Kconfig
+++ b/drivers/staging/iio/frequency/Kconfig
diff --git a/drivers/staging/iio/dds/Makefile b/drivers/staging/iio/frequency/Makefile
index 147746176b9..147746176b9 100644
--- a/drivers/staging/iio/dds/Makefile
+++ b/drivers/staging/iio/frequency/Makefile
diff --git a/drivers/staging/iio/dds/ad5930.c b/drivers/staging/iio/frequency/ad5930.c
index 9c32d1beae2..2d541d0eebe 100644
--- a/drivers/staging/iio/dds/ad5930.c
+++ b/drivers/staging/iio/frequency/ad5930.c
@@ -16,8 +16,8 @@
 #include <linux/sysfs.h>
 #include <linux/module.h>
 
-#include "../iio.h"
-#include "../sysfs.h"
+#include <linux/iio/iio.h>
+#include <linux/iio/sysfs.h>
 
 #define DRV_NAME "ad5930"
 
@@ -48,7 +48,7 @@ static ssize_t ad5930_set_parameter(struct device *dev,
 	struct spi_transfer xfer;
 	int ret;
 	struct ad5903_config *config = (struct ad5903_config *)buf;
-	struct iio_dev *idev = dev_get_drvdata(dev);
+	struct iio_dev *idev = dev_to_iio_dev(dev);
 	struct ad5930_state *st = iio_priv(idev);
 
 	config->control = (config->control & ~value_mask);
@@ -97,7 +97,7 @@ static int __devinit ad5930_probe(struct spi_device *spi)
 	struct iio_dev *idev;
 	int ret = 0;
 
-	idev = iio_allocate_device(sizeof(*st));
+	idev = iio_device_alloc(sizeof(*st));
 	if (idev == NULL) {
 		ret = -ENOMEM;
 		goto error_ret;
@@ -122,7 +122,7 @@ static int __devinit ad5930_probe(struct spi_device *spi)
 	return 0;
 
 error_free_dev:
-	iio_free_device(idev);
+	iio_device_free(idev);
 error_ret:
 	return ret;
 }
@@ -130,7 +130,7 @@ error_ret:
 static int __devexit ad5930_remove(struct spi_device *spi)
 {
 	iio_device_unregister(spi_get_drvdata(spi));
-	iio_free_device(spi_get_drvdata(spi));
+	iio_device_free(spi_get_drvdata(spi));
 
 	return 0;
 }
diff --git a/drivers/staging/iio/dds/ad9832.c b/drivers/staging/iio/frequency/ad9832.c
index 2ccf25dd928..fed39404e34 100644
--- a/drivers/staging/iio/dds/ad9832.c
+++ b/drivers/staging/iio/frequency/ad9832.c
@@ -16,8 +16,8 @@
 #include <linux/module.h>
 #include <asm/div64.h>
 
-#include "../iio.h"
-#include "../sysfs.h"
+#include <linux/iio/iio.h>
+#include <linux/iio/sysfs.h>
 #include "dds.h"
 
 #include "ad9832.h"
@@ -77,7 +77,7 @@ static ssize_t ad9832_write(struct device *dev,
 		const char *buf,
 		size_t len)
 {
-	struct iio_dev *indio_dev = dev_get_drvdata(dev);
+	struct iio_dev *indio_dev = dev_to_iio_dev(dev);
 	struct ad9832_state *st = iio_priv(indio_dev);
 	struct iio_dev_attr *this_attr = to_iio_dev_attr(attr);
 	int ret;
@@ -177,18 +177,18 @@ static IIO_DEV_ATTR_OUT_ENABLE(0, S_IWUSR, NULL,
 				ad9832_write, AD9832_OUTPUT_EN);
 
 static struct attribute *ad9832_attributes[] = {
-	&iio_dev_attr_dds0_freq0.dev_attr.attr,
-	&iio_dev_attr_dds0_freq1.dev_attr.attr,
-	&iio_const_attr_dds0_freq_scale.dev_attr.attr,
-	&iio_dev_attr_dds0_phase0.dev_attr.attr,
-	&iio_dev_attr_dds0_phase1.dev_attr.attr,
-	&iio_dev_attr_dds0_phase2.dev_attr.attr,
-	&iio_dev_attr_dds0_phase3.dev_attr.attr,
-	&iio_const_attr_dds0_phase_scale.dev_attr.attr,
-	&iio_dev_attr_dds0_pincontrol_en.dev_attr.attr,
-	&iio_dev_attr_dds0_freqsymbol.dev_attr.attr,
-	&iio_dev_attr_dds0_phasesymbol.dev_attr.attr,
-	&iio_dev_attr_dds0_out_enable.dev_attr.attr,
+	&iio_dev_attr_out_altvoltage0_frequency0.dev_attr.attr,
+	&iio_dev_attr_out_altvoltage0_frequency1.dev_attr.attr,
+	&iio_const_attr_out_altvoltage0_frequency_scale.dev_attr.attr,
+	&iio_dev_attr_out_altvoltage0_phase0.dev_attr.attr,
+	&iio_dev_attr_out_altvoltage0_phase1.dev_attr.attr,
+	&iio_dev_attr_out_altvoltage0_phase2.dev_attr.attr,
+	&iio_dev_attr_out_altvoltage0_phase3.dev_attr.attr,
+	&iio_const_attr_out_altvoltage0_phase_scale.dev_attr.attr,
+	&iio_dev_attr_out_altvoltage0_pincontrol_en.dev_attr.attr,
+	&iio_dev_attr_out_altvoltage0_frequencysymbol.dev_attr.attr,
+	&iio_dev_attr_out_altvoltage0_phasesymbol.dev_attr.attr,
+	&iio_dev_attr_out_altvoltage0_out_enable.dev_attr.attr,
 	NULL,
 };
 
@@ -221,7 +221,7 @@ static int __devinit ad9832_probe(struct spi_device *spi)
 			goto error_put_reg;
 	}
 
-	indio_dev = iio_allocate_device(sizeof(*st));
+	indio_dev = iio_device_alloc(sizeof(*st));
 	if (indio_dev == NULL) {
 		ret = -ENOMEM;
 		goto error_disable_reg;
@@ -313,7 +313,7 @@ static int __devinit ad9832_probe(struct spi_device *spi)
 	return 0;
 
 error_free_device:
-	iio_free_device(indio_dev);
+	iio_device_free(indio_dev);
 error_disable_reg:
 	if (!IS_ERR(reg))
 		regulator_disable(reg);
@@ -334,7 +334,7 @@ static int __devexit ad9832_remove(struct spi_device *spi)
 		regulator_disable(st->reg);
 		regulator_put(st->reg);
 	}
-	iio_free_device(indio_dev);
+	iio_device_free(indio_dev);
 
 	return 0;
 }
diff --git a/drivers/staging/iio/dds/ad9832.h b/drivers/staging/iio/frequency/ad9832.h
index c5b701f8aab..c5b701f8aab 100644
--- a/drivers/staging/iio/dds/ad9832.h
+++ b/drivers/staging/iio/frequency/ad9832.h
diff --git a/drivers/staging/iio/dds/ad9834.c b/drivers/staging/iio/frequency/ad9834.c
index 38a2de08626..1b2dc741d2c 100644
--- a/drivers/staging/iio/dds/ad9834.c
+++ b/drivers/staging/iio/frequency/ad9834.c
@@ -19,8 +19,8 @@
 #include <linux/module.h>
 #include <asm/div64.h>
 
-#include "../iio.h"
-#include "../sysfs.h"
+#include <linux/iio/iio.h>
+#include <linux/iio/sysfs.h>
 #include "dds.h"
 
 #include "ad9834.h"
@@ -66,7 +66,7 @@ static ssize_t ad9834_write(struct device *dev,
 		const char *buf,
 		size_t len)
 {
-	struct iio_dev *indio_dev = dev_get_drvdata(dev);
+	struct iio_dev *indio_dev = dev_to_iio_dev(dev);
 	struct ad9834_state *st = iio_priv(indio_dev);
 	struct iio_dev_attr *this_attr = to_iio_dev_attr(attr);
 	int ret;
@@ -145,7 +145,7 @@ static ssize_t ad9834_store_wavetype(struct device *dev,
 				 const char *buf,
 				 size_t len)
 {
-	struct iio_dev *indio_dev = dev_get_drvdata(dev);
+	struct iio_dev *indio_dev = dev_to_iio_dev(dev);
 	struct ad9834_state *st = iio_priv(indio_dev);
 	struct iio_dev_attr *this_attr = to_iio_dev_attr(attr);
 	int ret = 0;
@@ -203,7 +203,7 @@ static ssize_t ad9834_show_out0_wavetype_available(struct device *dev,
 						struct device_attribute *attr,
 						char *buf)
 {
-	struct iio_dev *indio_dev = dev_get_drvdata(dev);
+	struct iio_dev *indio_dev = dev_to_iio_dev(dev);
 	struct ad9834_state *st = iio_priv(indio_dev);
 	char *str;
 
@@ -218,14 +218,14 @@ static ssize_t ad9834_show_out0_wavetype_available(struct device *dev,
 }
 
 
-static IIO_DEVICE_ATTR(dds0_out0_wavetype_available, S_IRUGO,
+static IIO_DEVICE_ATTR(out_altvoltage0_out0_wavetype_available, S_IRUGO,
 		       ad9834_show_out0_wavetype_available, NULL, 0);
 
 static ssize_t ad9834_show_out1_wavetype_available(struct device *dev,
 						struct device_attribute *attr,
 						char *buf)
 {
-	struct iio_dev *indio_dev = dev_get_drvdata(dev);
+	struct iio_dev *indio_dev = dev_to_iio_dev(dev);
 	struct ad9834_state *st = iio_priv(indio_dev);
 	char *str;
 
@@ -237,7 +237,7 @@ static ssize_t ad9834_show_out1_wavetype_available(struct device *dev,
 	return sprintf(buf, "%s\n", str);
 }
 
-static IIO_DEVICE_ATTR(dds0_out1_wavetype_available, S_IRUGO,
+static IIO_DEVICE_ATTR(out_altvoltage0_out1_wavetype_available, S_IRUGO,
 		       ad9834_show_out1_wavetype_available, NULL, 0);
 
 /**
@@ -263,36 +263,36 @@ static IIO_DEV_ATTR_OUT_WAVETYPE(0, 0, ad9834_store_wavetype, 0);
 static IIO_DEV_ATTR_OUT_WAVETYPE(0, 1, ad9834_store_wavetype, 1);
 
 static struct attribute *ad9834_attributes[] = {
-	&iio_dev_attr_dds0_freq0.dev_attr.attr,
-	&iio_dev_attr_dds0_freq1.dev_attr.attr,
-	&iio_const_attr_dds0_freq_scale.dev_attr.attr,
-	&iio_dev_attr_dds0_phase0.dev_attr.attr,
-	&iio_dev_attr_dds0_phase1.dev_attr.attr,
-	&iio_const_attr_dds0_phase_scale.dev_attr.attr,
-	&iio_dev_attr_dds0_pincontrol_en.dev_attr.attr,
-	&iio_dev_attr_dds0_freqsymbol.dev_attr.attr,
-	&iio_dev_attr_dds0_phasesymbol.dev_attr.attr,
-	&iio_dev_attr_dds0_out_enable.dev_attr.attr,
-	&iio_dev_attr_dds0_out1_enable.dev_attr.attr,
-	&iio_dev_attr_dds0_out0_wavetype.dev_attr.attr,
-	&iio_dev_attr_dds0_out1_wavetype.dev_attr.attr,
-	&iio_dev_attr_dds0_out0_wavetype_available.dev_attr.attr,
-	&iio_dev_attr_dds0_out1_wavetype_available.dev_attr.attr,
+	&iio_dev_attr_out_altvoltage0_frequency0.dev_attr.attr,
+	&iio_dev_attr_out_altvoltage0_frequency1.dev_attr.attr,
+	&iio_const_attr_out_altvoltage0_frequency_scale.dev_attr.attr,
+	&iio_dev_attr_out_altvoltage0_phase0.dev_attr.attr,
+	&iio_dev_attr_out_altvoltage0_phase1.dev_attr.attr,
+	&iio_const_attr_out_altvoltage0_phase_scale.dev_attr.attr,
+	&iio_dev_attr_out_altvoltage0_pincontrol_en.dev_attr.attr,
+	&iio_dev_attr_out_altvoltage0_frequencysymbol.dev_attr.attr,
+	&iio_dev_attr_out_altvoltage0_phasesymbol.dev_attr.attr,
+	&iio_dev_attr_out_altvoltage0_out_enable.dev_attr.attr,
+	&iio_dev_attr_out_altvoltage0_out1_enable.dev_attr.attr,
+	&iio_dev_attr_out_altvoltage0_out0_wavetype.dev_attr.attr,
+	&iio_dev_attr_out_altvoltage0_out1_wavetype.dev_attr.attr,
+	&iio_dev_attr_out_altvoltage0_out0_wavetype_available.dev_attr.attr,
+	&iio_dev_attr_out_altvoltage0_out1_wavetype_available.dev_attr.attr,
 	NULL,
 };
 
 static struct attribute *ad9833_attributes[] = {
-	&iio_dev_attr_dds0_freq0.dev_attr.attr,
-	&iio_dev_attr_dds0_freq1.dev_attr.attr,
-	&iio_const_attr_dds0_freq_scale.dev_attr.attr,
-	&iio_dev_attr_dds0_phase0.dev_attr.attr,
-	&iio_dev_attr_dds0_phase1.dev_attr.attr,
-	&iio_const_attr_dds0_phase_scale.dev_attr.attr,
-	&iio_dev_attr_dds0_freqsymbol.dev_attr.attr,
-	&iio_dev_attr_dds0_phasesymbol.dev_attr.attr,
-	&iio_dev_attr_dds0_out_enable.dev_attr.attr,
-	&iio_dev_attr_dds0_out0_wavetype.dev_attr.attr,
-	&iio_dev_attr_dds0_out0_wavetype_available.dev_attr.attr,
+	&iio_dev_attr_out_altvoltage0_frequency0.dev_attr.attr,
+	&iio_dev_attr_out_altvoltage0_frequency1.dev_attr.attr,
+	&iio_const_attr_out_altvoltage0_frequency_scale.dev_attr.attr,
+	&iio_dev_attr_out_altvoltage0_phase0.dev_attr.attr,
+	&iio_dev_attr_out_altvoltage0_phase1.dev_attr.attr,
+	&iio_const_attr_out_altvoltage0_phase_scale.dev_attr.attr,
+	&iio_dev_attr_out_altvoltage0_frequencysymbol.dev_attr.attr,
+	&iio_dev_attr_out_altvoltage0_phasesymbol.dev_attr.attr,
+	&iio_dev_attr_out_altvoltage0_out_enable.dev_attr.attr,
+	&iio_dev_attr_out_altvoltage0_out0_wavetype.dev_attr.attr,
+	&iio_dev_attr_out_altvoltage0_out0_wavetype_available.dev_attr.attr,
 	NULL,
 };
 
@@ -334,7 +334,7 @@ static int __devinit ad9834_probe(struct spi_device *spi)
 			goto error_put_reg;
 	}
 
-	indio_dev = iio_allocate_device(sizeof(*st));
+	indio_dev = iio_device_alloc(sizeof(*st));
 	if (indio_dev == NULL) {
 		ret = -ENOMEM;
 		goto error_disable_reg;
@@ -414,7 +414,7 @@ static int __devinit ad9834_probe(struct spi_device *spi)
 	return 0;
 
 error_free_device:
-	iio_free_device(indio_dev);
+	iio_device_free(indio_dev);
 error_disable_reg:
 	if (!IS_ERR(reg))
 		regulator_disable(reg);
@@ -434,7 +434,7 @@ static int __devexit ad9834_remove(struct spi_device *spi)
 		regulator_disable(st->reg);
 		regulator_put(st->reg);
 	}
-	iio_free_device(indio_dev);
+	iio_device_free(indio_dev);
 
 	return 0;
 }
diff --git a/drivers/staging/iio/dds/ad9834.h b/drivers/staging/iio/frequency/ad9834.h
index ed5ed8d0007..ed5ed8d0007 100644
--- a/drivers/staging/iio/dds/ad9834.h
+++ b/drivers/staging/iio/frequency/ad9834.h
diff --git a/drivers/staging/iio/dds/ad9850.c b/drivers/staging/iio/frequency/ad9850.c
index f4f731bb219..74abee054ac 100644
--- a/drivers/staging/iio/dds/ad9850.c
+++ b/drivers/staging/iio/frequency/ad9850.c
@@ -16,8 +16,8 @@
 #include <linux/sysfs.h>
 #include <linux/module.h>
 
-#include "../iio.h"
-#include "../sysfs.h"
+#include <linux/iio/iio.h>
+#include <linux/iio/sysfs.h>
 
 #define DRV_NAME "ad9850"
 
@@ -43,7 +43,7 @@ static ssize_t ad9850_set_parameter(struct device *dev,
 	struct spi_transfer xfer;
 	int ret;
 	struct ad9850_config *config = (struct ad9850_config *)buf;
-	struct iio_dev *idev = dev_get_drvdata(dev);
+	struct iio_dev *idev = dev_to_iio_dev(dev);
 	struct ad9850_state *st = iio_priv(idev);
 
 	xfer.len = len;
@@ -83,7 +83,7 @@ static int __devinit ad9850_probe(struct spi_device *spi)
 	struct iio_dev *idev;
 	int ret = 0;
 
-	idev = iio_allocate_device(sizeof(*st));
+	idev = iio_device_alloc(sizeof(*st));
 	if (idev == NULL) {
 		ret = -ENOMEM;
 		goto error_ret;
@@ -108,7 +108,7 @@ static int __devinit ad9850_probe(struct spi_device *spi)
 	return 0;
 
 error_free_dev:
-	iio_free_device(idev);
+	iio_device_free(idev);
 error_ret:
 	return ret;
 }
@@ -116,7 +116,7 @@ error_ret:
 static int __devexit ad9850_remove(struct spi_device *spi)
 {
 	iio_device_unregister(spi_get_drvdata(spi));
-	iio_free_device(spi_get_drvdata(spi));
+	iio_device_free(spi_get_drvdata(spi));
 
 	return 0;
 }
diff --git a/drivers/staging/iio/dds/ad9852.c b/drivers/staging/iio/frequency/ad9852.c
index 554266c615a..fd9d14a413a 100644
--- a/drivers/staging/iio/dds/ad9852.c
+++ b/drivers/staging/iio/frequency/ad9852.c
@@ -16,8 +16,8 @@
 #include <linux/sysfs.h>
 #include <linux/module.h>
 
-#include "../iio.h"
-#include "../sysfs.h"
+#include <linux/iio/iio.h>
+#include <linux/iio/sysfs.h>
 
 #define DRV_NAME "ad9852"
 
@@ -71,7 +71,7 @@ static ssize_t ad9852_set_parameter(struct device *dev,
 	struct spi_transfer xfer;
 	int ret;
 	struct ad9852_config *config = (struct ad9852_config *)buf;
-	struct iio_dev *idev = dev_get_drvdata(dev);
+	struct iio_dev *idev = dev_to_iio_dev(dev);
 	struct ad9852_state *st = iio_priv(idev);
 
 	xfer.len = 3;
@@ -232,7 +232,7 @@ static int __devinit ad9852_probe(struct spi_device *spi)
 	struct iio_dev *idev;
 	int ret = 0;
 
-	idev = iio_allocate_device(sizeof(*st));
+	idev = iio_device_alloc(sizeof(*st));
 	if (idev == NULL) {
 		ret = -ENOMEM;
 		goto error_ret;
@@ -258,7 +258,7 @@ static int __devinit ad9852_probe(struct spi_device *spi)
 	return 0;
 
 error_free_dev:
-	iio_free_device(idev);
+	iio_device_free(idev);
 
 error_ret:
 	return ret;
@@ -267,7 +267,7 @@ error_ret:
 static int __devexit ad9852_remove(struct spi_device *spi)
 {
 	iio_device_unregister(spi_get_drvdata(spi));
-	iio_free_device(spi_get_drvdata(spi));
+	iio_device_free(spi_get_drvdata(spi));
 
 	return 0;
 }
diff --git a/drivers/staging/iio/dds/ad9910.c b/drivers/staging/iio/frequency/ad9910.c
index 3985766d6f8..5a7ba305b75 100644
--- a/drivers/staging/iio/dds/ad9910.c
+++ b/drivers/staging/iio/frequency/ad9910.c
@@ -16,8 +16,8 @@
 #include <linux/sysfs.h>
 #include <linux/module.h>
 
-#include "../iio.h"
-#include "../sysfs.h"
+#include <linux/iio/iio.h>
+#include <linux/iio/sysfs.h>
 
 #define DRV_NAME "ad9910"
 
@@ -123,7 +123,7 @@ static ssize_t ad9910_set_parameter(struct device *dev,
 	struct spi_transfer xfer;
 	int ret;
 	struct ad9910_config *config = (struct ad9910_config *)buf;
-	struct iio_dev *idev = dev_get_drvdata(dev);
+	struct iio_dev *idev = dev_to_iio_dev(dev);
 	struct ad9910_state *st = iio_priv(idev);
 
 	xfer.len = 5;
@@ -367,7 +367,7 @@ static int __devinit ad9910_probe(struct spi_device *spi)
 	struct iio_dev *idev;
 	int ret = 0;
 
-	idev = iio_allocate_device(sizeof(*st));
+	idev = iio_device_alloc(sizeof(*st));
 	if (idev == NULL) {
 		ret = -ENOMEM;
 		goto error_ret;
@@ -392,7 +392,7 @@ static int __devinit ad9910_probe(struct spi_device *spi)
 	return 0;
 
 error_free_dev:
-	iio_free_device(idev);
+	iio_device_free(idev);
 error_ret:
 	return ret;
 }
@@ -400,7 +400,7 @@ error_ret:
 static int __devexit ad9910_remove(struct spi_device *spi)
 {
 	iio_device_unregister(spi_get_drvdata(spi));
-	iio_free_device(spi_get_drvdata(spi));
+	iio_device_free(spi_get_drvdata(spi));
 
 	return 0;
 }
diff --git a/drivers/staging/iio/dds/ad9951.c b/drivers/staging/iio/frequency/ad9951.c
index 4d150048002..ba6f49ff09a 100644
--- a/drivers/staging/iio/dds/ad9951.c
+++ b/drivers/staging/iio/frequency/ad9951.c
@@ -16,8 +16,8 @@
 #include <linux/sysfs.h>
 #include <linux/module.h>
 
-#include "../iio.h"
-#include "../sysfs.h"
+#include <linux/iio/iio.h>
+#include <linux/iio/sysfs.h>
 
 #define DRV_NAME "ad9951"
 
@@ -64,7 +64,7 @@ static ssize_t ad9951_set_parameter(struct device *dev,
 	struct spi_transfer xfer;
 	int ret;
 	struct ad9951_config *config = (struct ad9951_config *)buf;
-	struct iio_dev *idev = dev_get_drvdata(dev);
+	struct iio_dev *idev = dev_to_iio_dev(dev);
 	struct ad9951_state *st = iio_priv(idev);
 
 	xfer.len = 3;
@@ -176,7 +176,7 @@ static int __devinit ad9951_probe(struct spi_device *spi)
 	struct iio_dev *idev;
 	int ret = 0;
 
-	idev = iio_allocate_device(sizeof(*st));
+	idev = iio_device_alloc(sizeof(*st));
 	if (idev == NULL) {
 		ret = -ENOMEM;
 		goto error_ret;
@@ -202,7 +202,7 @@ static int __devinit ad9951_probe(struct spi_device *spi)
 	return 0;
 
 error_free_dev:
-	iio_free_device(idev);
+	iio_device_free(idev);
 
 error_ret:
 	return ret;
@@ -211,7 +211,7 @@ error_ret:
 static int __devexit ad9951_remove(struct spi_device *spi)
 {
 	iio_device_unregister(spi_get_drvdata(spi));
-	iio_free_device(spi_get_drvdata(spi));
+	iio_device_free(spi_get_drvdata(spi));
 
 	return 0;
 }
diff --git a/drivers/staging/iio/frequency/dds.h b/drivers/staging/iio/frequency/dds.h
new file mode 100644
index 00000000000..c3342f6e052
--- /dev/null
+++ b/drivers/staging/iio/frequency/dds.h
@@ -0,0 +1,110 @@
+/*
+ * dds.h - sysfs attributes associated with DDS devices
+ *
+ * Copyright (c) 2010 Analog Devices Inc.
+ *
+ * Licensed under the GPL-2 or later.
+ */
+
+/**
+ * /sys/bus/iio/devices/.../out_altvoltageX_frequencyY
+ */
+
+#define IIO_DEV_ATTR_FREQ(_channel, _num, _mode, _show, _store, _addr)	\
+	IIO_DEVICE_ATTR(out_altvoltage##_channel##_frequency##_num,	\
+			_mode, _show, _store, _addr)
+
+/**
+ * /sys/bus/iio/devices/.../out_altvoltageX_frequencyY_scale
+ */
+
+#define IIO_CONST_ATTR_FREQ_SCALE(_channel, _string)			\
+	IIO_CONST_ATTR(out_altvoltage##_channel##_frequency_scale, _string)
+
+/**
+ * /sys/bus/iio/devices/.../out_altvoltageX_frequencysymbol
+ */
+
+#define IIO_DEV_ATTR_FREQSYMBOL(_channel, _mode, _show, _store, _addr)	\
+	IIO_DEVICE_ATTR(out_altvoltage##_channel##_frequencysymbol,	\
+			_mode, _show, _store, _addr);
+
+/**
+ * /sys/bus/iio/devices/.../out_altvoltageX_phaseY
+ */
+
+#define IIO_DEV_ATTR_PHASE(_channel, _num, _mode, _show, _store, _addr)	\
+	IIO_DEVICE_ATTR(out_altvoltage##_channel##_phase##_num,		\
+			_mode, _show, _store, _addr)
+
+/**
+ * /sys/bus/iio/devices/.../out_altvoltageX_phaseY_scale
+ */
+
+#define IIO_CONST_ATTR_PHASE_SCALE(_channel, _string)			\
+	IIO_CONST_ATTR(out_altvoltage##_channel##_phase_scale, _string)
+
+/**
+ * /sys/bus/iio/devices/.../out_altvoltageX_phasesymbol
+ */
+
+#define IIO_DEV_ATTR_PHASESYMBOL(_channel, _mode, _show, _store, _addr)	\
+	IIO_DEVICE_ATTR(out_altvoltage##_channel##_phasesymbol,		\
+			_mode, _show, _store, _addr);
+
+/**
+ * /sys/bus/iio/devices/.../out_altvoltageX_pincontrol_en
+ */
+
+#define IIO_DEV_ATTR_PINCONTROL_EN(_channel, _mode, _show, _store, _addr)\
+	IIO_DEVICE_ATTR(out_altvoltage##_channel##_pincontrol_en,	\
+			_mode, _show, _store, _addr);
+
+/**
+ * /sys/bus/iio/devices/.../out_altvoltageX_pincontrol_frequency_en
+ */
+
+#define IIO_DEV_ATTR_PINCONTROL_FREQ_EN(_channel, _mode, _show, _store, _addr)\
+	IIO_DEVICE_ATTR(out_altvoltage##_channel##_pincontrol_frequency_en,\
+			_mode, _show, _store, _addr);
+
+/**
+ * /sys/bus/iio/devices/.../out_altvoltageX_pincontrol_phase_en
+ */
+
+#define IIO_DEV_ATTR_PINCONTROL_PHASE_EN(_channel, _mode, _show, _store, _addr)\
+	IIO_DEVICE_ATTR(out_altvoltage##_channel##_pincontrol_phase_en,	\
+			_mode, _show, _store, _addr);
+
+/**
+ * /sys/bus/iio/devices/.../out_altvoltageX_out_enable
+ */
+
+#define IIO_DEV_ATTR_OUT_ENABLE(_channel, _mode, _show, _store, _addr)	\
+	IIO_DEVICE_ATTR(out_altvoltage##_channel##_out_enable,		\
+			_mode, _show, _store, _addr);
+
+/**
+ * /sys/bus/iio/devices/.../out_altvoltageX_outY_enable
+ */
+
+#define IIO_DEV_ATTR_OUTY_ENABLE(_channel, _output,			\
+			_mode, _show, _store, _addr)			\
+	IIO_DEVICE_ATTR(out_altvoltage##_channel##_out##_output##_enable,\
+			_mode, _show, _store, _addr);
+
+/**
+ * /sys/bus/iio/devices/.../out_altvoltageX_outY_wavetype
+ */
+
+#define IIO_DEV_ATTR_OUT_WAVETYPE(_channel, _output, _store, _addr)	\
+	IIO_DEVICE_ATTR(out_altvoltage##_channel##_out##_output##_wavetype,\
+			S_IWUSR, NULL, _store, _addr);
+
+/**
+ * /sys/bus/iio/devices/.../out_altvoltageX_outY_wavetype_available
+ */
+
+#define IIO_CONST_ATTR_OUT_WAVETYPES_AVAILABLE(_channel, _output, _modes)\
+	IIO_CONST_ATTR(							\
+	out_altvoltage##_channel##_out##_output##_wavetype_available, _modes);
diff --git a/drivers/staging/iio/gyro/adis16060_core.c b/drivers/staging/iio/gyro/adis16060_core.c
index 02cc23420b9..9931e2060e1 100644
--- a/drivers/staging/iio/gyro/adis16060_core.c
+++ b/drivers/staging/iio/gyro/adis16060_core.c
@@ -15,8 +15,8 @@
 #include <linux/slab.h>
 #include <linux/sysfs.h>
 
-#include "../iio.h"
-#include "../sysfs.h"
+#include <linux/iio/iio.h>
+#include <linux/iio/sysfs.h>
 
 #define ADIS16060_GYRO		0x20 /* Measure Angular Rate (Gyro) */
 #define ADIS16060_TEMP_OUT	0x10 /* Measure Temperature */
@@ -85,7 +85,7 @@ static int adis16060_read_raw(struct iio_dev *indio_dev,
 	int ret;
 
 	switch (mask) {
-	case 0:
+	case IIO_CHAN_INFO_RAW:
 		/* Take the iio_dev status lock */
 		mutex_lock(&indio_dev->mlock);
 		ret = adis16060_spi_write(indio_dev, chan->address);
@@ -120,22 +120,26 @@ static const struct iio_chan_spec adis16060_channels[] = {
 		.type = IIO_ANGL_VEL,
 		.modified = 1,
 		.channel2 = IIO_MOD_Z,
+		.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT,
 		.address = ADIS16060_GYRO,
 	}, {
 		.type = IIO_VOLTAGE,
 		.indexed = 1,
 		.channel = 0,
+		.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT,
 		.address = ADIS16060_AIN1,
 	}, {
 		.type = IIO_VOLTAGE,
 		.indexed = 1,
 		.channel = 1,
+		.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT,
 		.address = ADIS16060_AIN2,
 	}, {
 		.type = IIO_TEMP,
 		.indexed = 1,
 		.channel = 0,
-		.info_mask = IIO_CHAN_INFO_OFFSET_SEPARATE_BIT |
+		.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT |
+		IIO_CHAN_INFO_OFFSET_SEPARATE_BIT |
 		IIO_CHAN_INFO_SCALE_SEPARATE_BIT,
 		.address = ADIS16060_TEMP_OUT,
 	}
@@ -148,7 +152,7 @@ static int __devinit adis16060_r_probe(struct spi_device *spi)
 	struct iio_dev *indio_dev;
 
 	/* setup the industrialio driver allocated elements */
-	indio_dev = iio_allocate_device(sizeof(*st));
+	indio_dev = iio_device_alloc(sizeof(*st));
 	if (indio_dev == NULL) {
 		ret = -ENOMEM;
 		goto error_ret;
@@ -174,7 +178,7 @@ static int __devinit adis16060_r_probe(struct spi_device *spi)
 	return 0;
 
 error_free_dev:
-	iio_free_device(indio_dev);
+	iio_device_free(indio_dev);
 error_ret:
 	return ret;
 }
@@ -183,7 +187,7 @@ error_ret:
 static int adis16060_r_remove(struct spi_device *spi)
 {
 	iio_device_unregister(spi_get_drvdata(spi));
-	iio_free_device(spi_get_drvdata(spi));
+	iio_device_free(spi_get_drvdata(spi));
 
 	return 0;
 }
diff --git a/drivers/staging/iio/gyro/adis16080_core.c b/drivers/staging/iio/gyro/adis16080_core.c
index 1815490db8b..11f1dccd7a0 100644
--- a/drivers/staging/iio/gyro/adis16080_core.c
+++ b/drivers/staging/iio/gyro/adis16080_core.c
@@ -14,8 +14,8 @@
 #include <linux/sysfs.h>
 #include <linux/module.h>
 
-#include "../iio.h"
-#include "../sysfs.h"
+#include <linux/iio/iio.h>
+#include <linux/iio/sysfs.h>
 
 #define ADIS16080_DIN_GYRO   (0 << 10) /* Gyroscope output */
 #define ADIS16080_DIN_TEMP   (1 << 10) /* Temperature output */
@@ -87,7 +87,7 @@ static int adis16080_read_raw(struct iio_dev *indio_dev,
 
 	mutex_lock(&indio_dev->mlock);
 	switch (mask) {
-	case 0:
+	case IIO_CHAN_INFO_RAW:
 		ret = adis16080_spi_write(indio_dev,
 					  chan->address |
 					  ADIS16080_DIN_WRITE);
@@ -110,21 +110,25 @@ static const struct iio_chan_spec adis16080_channels[] = {
 		.type = IIO_ANGL_VEL,
 		.modified = 1,
 		.channel2 = IIO_MOD_Z,
+		.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT,
 		.address = ADIS16080_DIN_GYRO,
 	}, {
 		.type = IIO_VOLTAGE,
 		.indexed = 1,
 		.channel = 0,
+		.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT,
 		.address = ADIS16080_DIN_AIN1,
 	}, {
 		.type = IIO_VOLTAGE,
 		.indexed = 1,
 		.channel = 1,
+		.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT,
 		.address = ADIS16080_DIN_AIN2,
 	}, {
 		.type = IIO_TEMP,
 		.indexed = 1,
 		.channel = 0,
+		.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT,
 		.address = ADIS16080_DIN_TEMP,
 	}
 };
@@ -141,7 +145,7 @@ static int __devinit adis16080_probe(struct spi_device *spi)
 	struct iio_dev *indio_dev;
 
 	/* setup the industrialio driver allocated elements */
-	indio_dev = iio_allocate_device(sizeof(*st));
+	indio_dev = iio_device_alloc(sizeof(*st));
 	if (indio_dev == NULL) {
 		ret = -ENOMEM;
 		goto error_ret;
@@ -167,7 +171,7 @@ static int __devinit adis16080_probe(struct spi_device *spi)
 	return 0;
 
 error_free_dev:
-	iio_free_device(indio_dev);
+	iio_device_free(indio_dev);
 error_ret:
 	return ret;
 }
@@ -176,7 +180,7 @@ error_ret:
 static int adis16080_remove(struct spi_device *spi)
 {
 	iio_device_unregister(spi_get_drvdata(spi));
-	iio_free_device(spi_get_drvdata(spi));
+	iio_device_free(spi_get_drvdata(spi));
 
 	return 0;
 }
diff --git a/drivers/staging/iio/gyro/adis16130_core.c b/drivers/staging/iio/gyro/adis16130_core.c
index 947eb86f05d..bf61cd0b5bb 100644
--- a/drivers/staging/iio/gyro/adis16130_core.c
+++ b/drivers/staging/iio/gyro/adis16130_core.c
@@ -16,8 +16,8 @@
 #include <linux/list.h>
 #include <linux/module.h>
 
-#include "../iio.h"
-#include "../sysfs.h"
+#include <linux/iio/iio.h>
+#include <linux/iio/sysfs.h>
 
 #define ADIS16130_CON         0x0
 #define ADIS16130_CON_RD      (1 << 6)
@@ -100,11 +100,13 @@ static const struct iio_chan_spec adis16130_channels[] = {
 		.type = IIO_ANGL_VEL,
 		.modified = 1,
 		.channel2 = IIO_MOD_Z,
+		.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT,
 		.address = ADIS16130_RATEDATA,
 	}, {
 		.type = IIO_TEMP,
 		.indexed = 1,
 		.channel = 0,
+		.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT,
 		.address = ADIS16130_TEMPDATA,
 	}
 };
@@ -121,7 +123,7 @@ static int __devinit adis16130_probe(struct spi_device *spi)
 	struct iio_dev *indio_dev;
 
 	/* setup the industrialio driver allocated elements */
-	indio_dev = iio_allocate_device(sizeof(*st));
+	indio_dev = iio_device_alloc(sizeof(*st));
 	if (indio_dev == NULL) {
 		ret = -ENOMEM;
 		goto error_ret;
@@ -145,7 +147,7 @@ static int __devinit adis16130_probe(struct spi_device *spi)
 	return 0;
 
 error_free_dev:
-	iio_free_device(indio_dev);
+	iio_device_free(indio_dev);
 
 error_ret:
 	return ret;
@@ -155,7 +157,7 @@ error_ret:
 static int adis16130_remove(struct spi_device *spi)
 {
 	iio_device_unregister(spi_get_drvdata(spi));
-	iio_free_device(spi_get_drvdata(spi));
+	iio_device_free(spi_get_drvdata(spi));
 
 	return 0;
 }
diff --git a/drivers/staging/iio/gyro/adis16260_core.c b/drivers/staging/iio/gyro/adis16260_core.c
index 8f6af47e955..ec765f955f8 100644
--- a/drivers/staging/iio/gyro/adis16260_core.c
+++ b/drivers/staging/iio/gyro/adis16260_core.c
@@ -18,9 +18,9 @@
 #include <linux/list.h>
 #include <linux/module.h>
 
-#include "../iio.h"
-#include "../sysfs.h"
-#include "../buffer.h"
+#include <linux/iio/iio.h>
+#include <linux/iio/sysfs.h>
+#include <linux/iio/buffer.h>
 
 #include "adis16260.h"
 
@@ -149,7 +149,7 @@ static ssize_t adis16260_read_frequency_available(struct device *dev,
 						  struct device_attribute *attr,
 						  char *buf)
 {
-	struct iio_dev *indio_dev = dev_get_drvdata(dev);
+	struct iio_dev *indio_dev = dev_to_iio_dev(dev);
 	struct adis16260_state *st = iio_priv(indio_dev);
 	if (spi_get_device_id(st->us)->driver_data)
 		return sprintf(buf, "%s\n", "0.129 ~ 256");
@@ -161,7 +161,7 @@ static ssize_t adis16260_read_frequency(struct device *dev,
 		struct device_attribute *attr,
 		char *buf)
 {
-	struct iio_dev *indio_dev = dev_get_drvdata(dev);
+	struct iio_dev *indio_dev = dev_to_iio_dev(dev);
 	struct adis16260_state *st = iio_priv(indio_dev);
 	int ret, len = 0;
 	u16 t;
@@ -186,7 +186,7 @@ static ssize_t adis16260_write_frequency(struct device *dev,
 		const char *buf,
 		size_t len)
 {
-	struct iio_dev *indio_dev = dev_get_drvdata(dev);
+	struct iio_dev *indio_dev = dev_to_iio_dev(dev);
 	struct adis16260_state *st = iio_priv(indio_dev);
 	long val;
 	int ret;
@@ -237,7 +237,7 @@ static ssize_t adis16260_write_reset(struct device *dev,
 		struct device_attribute *attr,
 		const char *buf, size_t len)
 {
-	struct iio_dev *indio_dev = dev_get_drvdata(dev);
+	struct iio_dev *indio_dev = dev_to_iio_dev(dev);
 	if (len < 1)
 		return -EINVAL;
 	switch (buf[0]) {
@@ -389,30 +389,76 @@ enum adis16260_channel {
 };
 #define ADIS16260_GYRO_CHANNEL_SET(axis, mod)				\
 	struct iio_chan_spec adis16260_channels_##axis[] = {		\
-		IIO_CHAN(IIO_ANGL_VEL, 1, 0, 0, NULL, 0, mod,		\
-			 IIO_CHAN_INFO_CALIBBIAS_SEPARATE_BIT |	\
-			 IIO_CHAN_INFO_CALIBSCALE_SEPARATE_BIT |	\
-			 IIO_CHAN_INFO_SCALE_SEPARATE_BIT,		\
-			 gyro, ADIS16260_SCAN_GYRO,			\
-			 IIO_ST('s', 14, 16, 0), 0),			\
-		IIO_CHAN(IIO_ANGL, 1, 0, 0, NULL, 0, mod,		\
-			 0,						\
-			 angle, ADIS16260_SCAN_ANGL,			\
-			 IIO_ST('u', 14, 16, 0), 0),			\
-		IIO_CHAN(IIO_TEMP, 0, 1, 0, NULL, 0, 0,			\
-			 IIO_CHAN_INFO_OFFSET_SEPARATE_BIT |		\
-			 IIO_CHAN_INFO_SCALE_SEPARATE_BIT,		\
-			 temp, ADIS16260_SCAN_TEMP,			\
-			 IIO_ST('u', 12, 16, 0), 0),			\
-		IIO_CHAN(IIO_VOLTAGE, 0, 1, 0, "supply", 0, 0,		\
-			 IIO_CHAN_INFO_SCALE_SEPARATE_BIT,		\
-			 in_supply, ADIS16260_SCAN_SUPPLY,		\
-			 IIO_ST('u', 12, 16, 0), 0),			\
-		IIO_CHAN(IIO_VOLTAGE, 0, 1, 0, NULL, 1, 0,		\
-			 IIO_CHAN_INFO_SCALE_SEPARATE_BIT,		\
-			 in_aux, ADIS16260_SCAN_AUX_ADC,		\
-			 IIO_ST('u', 12, 16, 0), 0),			\
-		IIO_CHAN_SOFT_TIMESTAMP(5)				\
+		{							\
+			.type = IIO_ANGL_VEL,				\
+			.modified = 1,					\
+			.channel2 = mod,				\
+			.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT |	\
+			IIO_CHAN_INFO_CALIBBIAS_SEPARATE_BIT |		\
+			IIO_CHAN_INFO_CALIBSCALE_SEPARATE_BIT |		\
+			IIO_CHAN_INFO_SCALE_SEPARATE_BIT,		\
+			.address = gyro,				\
+			.scan_index = ADIS16260_SCAN_GYRO,		\
+			.scan_type = {					\
+				.sign = 's',				\
+				.realbits = 14,				\
+				.storagebits = 16,			\
+			},						\
+		}, {							\
+			.type = IIO_ANGL,				\
+			.modified = 1,					\
+			.channel2 = mod,				\
+			.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT,	\
+			.address = angle,				\
+			.scan_index = ADIS16260_SCAN_ANGL,		\
+			.scan_type = {					\
+				.sign = 'u',				\
+				.realbits = 14,				\
+				.storagebits = 16,			\
+			},						\
+		}, {							\
+			.type = IIO_TEMP,				\
+			.indexed = 1,					\
+			.channel = 0,					\
+			.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT |	\
+			IIO_CHAN_INFO_OFFSET_SEPARATE_BIT |		\
+			IIO_CHAN_INFO_SCALE_SEPARATE_BIT,		\
+			.address = temp,				\
+			.scan_index = ADIS16260_SCAN_TEMP,		\
+			.scan_type = {					\
+				.sign = 'u',				\
+				.realbits = 12,				\
+				.storagebits = 16,			\
+			},						\
+		}, {							\
+			.type = IIO_VOLTAGE,				\
+			.indexed = 1,					\
+			.channel = 0,					\
+			.extend_name = "supply",			\
+			.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT |	\
+			IIO_CHAN_INFO_SCALE_SEPARATE_BIT,		\
+			.address = in_supply,				\
+			.scan_index = ADIS16260_SCAN_SUPPLY,		\
+			.scan_type = {					\
+				.sign = 'u',				\
+				.realbits = 12,				\
+				.storagebits = 16,			\
+			},						\
+		}, {							\
+			.type = IIO_VOLTAGE,				\
+			.indexed = 1,					\
+			.channel = 1,					\
+			.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT |	\
+			IIO_CHAN_INFO_SCALE_SEPARATE_BIT,		\
+			.address = in_aux,				\
+			.scan_index = ADIS16260_SCAN_AUX_ADC,		\
+			.scan_type = {					\
+				.sign = 'u',				\
+				.realbits = 12,				\
+				.storagebits = 16,			\
+			},						\
+		},							\
+		IIO_CHAN_SOFT_TIMESTAMP(5),				\
 	}
 
 static const ADIS16260_GYRO_CHANNEL_SET(x, IIO_MOD_X);
@@ -440,7 +486,7 @@ static int adis16260_read_raw(struct iio_dev *indio_dev,
 	s16 val16;
 
 	switch (mask) {
-	case 0:
+	case IIO_CHAN_INFO_RAW:
 		mutex_lock(&indio_dev->mlock);
 		addr = adis16260_addresses[chan->address][0];
 		ret = adis16260_spi_read_reg_16(indio_dev, addr, &val16);
@@ -581,7 +627,7 @@ static int __devinit adis16260_probe(struct spi_device *spi)
 	struct iio_dev *indio_dev;
 
 	/* setup the industrialio driver allocated elements */
-	indio_dev = iio_allocate_device(sizeof(*st));
+	indio_dev = iio_device_alloc(sizeof(*st));
 	if (indio_dev == NULL) {
 		ret = -ENOMEM;
 		goto error_ret;
@@ -666,7 +712,7 @@ error_uninitialize_ring:
 error_unreg_ring_funcs:
 	adis16260_unconfigure_ring(indio_dev);
 error_free_dev:
-	iio_free_device(indio_dev);
+	iio_device_free(indio_dev);
 error_ret:
 	return ret;
 }
@@ -687,7 +733,7 @@ static int adis16260_remove(struct spi_device *spi)
 	adis16260_remove_trigger(indio_dev);
 	iio_buffer_unregister(indio_dev);
 	adis16260_unconfigure_ring(indio_dev);
-	iio_free_device(indio_dev);
+	iio_device_free(indio_dev);
 
 err_ret:
 	return ret;
diff --git a/drivers/staging/iio/gyro/adis16260_ring.c b/drivers/staging/iio/gyro/adis16260_ring.c
index 711f15122a0..0fe2d9dfb6c 100644
--- a/drivers/staging/iio/gyro/adis16260_ring.c
+++ b/drivers/staging/iio/gyro/adis16260_ring.c
@@ -5,20 +5,19 @@
 #include <linux/spi/spi.h>
 #include <linux/slab.h>
 
-#include "../iio.h"
+#include <linux/iio/iio.h>
 #include "../ring_sw.h"
-#include "../trigger_consumer.h"
+#include <linux/iio/trigger_consumer.h>
 #include "adis16260.h"
 
 /**
  * adis16260_read_ring_data() read data registers which will be placed into ring
- * @dev: device associated with child of actual device (iio_dev or iio_trig)
+ * @indio_dev: the IIO device
  * @rx: somewhere to pass back the value read
  **/
-static int adis16260_read_ring_data(struct device *dev, u8 *rx)
+static int adis16260_read_ring_data(struct iio_dev *indio_dev, u8 *rx)
 {
 	struct spi_message msg;
-	struct iio_dev *indio_dev = dev_get_drvdata(dev);
 	struct adis16260_state *st = iio_priv(indio_dev);
 	struct spi_transfer xfers[ADIS16260_OUTPUTS + 1];
 	int ret;
@@ -66,22 +65,21 @@ static irqreturn_t adis16260_trigger_handler(int irq, void *p)
 	struct iio_buffer *ring = indio_dev->buffer;
 	int i = 0;
 	s16 *data;
-	size_t datasize = ring->access->get_bytes_per_datum(ring);
 
-	data = kmalloc(datasize , GFP_KERNEL);
+	data = kmalloc(indio_dev->scan_bytes, GFP_KERNEL);
 	if (data == NULL) {
 		dev_err(&st->us->dev, "memory alloc failed in ring bh");
 		return -ENOMEM;
 	}
 
 	if (!bitmap_empty(indio_dev->active_scan_mask, indio_dev->masklength) &&
-	    adis16260_read_ring_data(&indio_dev->dev, st->rx) >= 0)
+	    adis16260_read_ring_data(indio_dev, st->rx) >= 0)
 		for (; i < bitmap_weight(indio_dev->active_scan_mask,
 					 indio_dev->masklength); i++)
 			data[i] = be16_to_cpup((__be16 *)&(st->rx[i*2]));
 
 	/* Guaranteed to be aligned with 8 byte boundary */
-	if (ring->scan_timestamp)
+	if (indio_dev->scan_timestamp)
 		*((s64 *)(data + ((i + 3)/4)*4)) = pf->timestamp;
 
 	ring->access->store_to(ring, (u8 *)data, pf->timestamp);
diff --git a/drivers/staging/iio/gyro/adis16260_trigger.c b/drivers/staging/iio/gyro/adis16260_trigger.c
index 8299cd18d70..034559e4d5b 100644
--- a/drivers/staging/iio/gyro/adis16260_trigger.c
+++ b/drivers/staging/iio/gyro/adis16260_trigger.c
@@ -3,8 +3,8 @@
 #include <linux/spi/spi.h>
 #include <linux/export.h>
 
-#include "../iio.h"
-#include "../trigger.h"
+#include <linux/iio/iio.h>
+#include <linux/iio/trigger.h>
 #include "adis16260.h"
 
 /**
@@ -29,7 +29,7 @@ int adis16260_probe_trigger(struct iio_dev *indio_dev)
 	int ret;
 	struct adis16260_state *st = iio_priv(indio_dev);
 
-	st->trig = iio_allocate_trigger("%s-dev%d",
+	st->trig = iio_trigger_alloc("%s-dev%d",
 					spi_get_device_id(st->us)->name,
 					indio_dev->id);
 	if (st->trig == NULL) {
@@ -60,7 +60,7 @@ int adis16260_probe_trigger(struct iio_dev *indio_dev)
 error_free_irq:
 	free_irq(st->us->irq, st->trig);
 error_free_trig:
-	iio_free_trigger(st->trig);
+	iio_trigger_free(st->trig);
 error_ret:
 	return ret;
 }
@@ -71,5 +71,5 @@ void adis16260_remove_trigger(struct iio_dev *indio_dev)
 
 	iio_trigger_unregister(st->trig);
 	free_irq(st->us->irq, st->trig);
-	iio_free_trigger(st->trig);
+	iio_trigger_free(st->trig);
 }
diff --git a/drivers/staging/iio/gyro/adxrs450.h b/drivers/staging/iio/gyro/adxrs450.h
index af0c870100b..f8cf21f0294 100644
--- a/drivers/staging/iio/gyro/adxrs450.h
+++ b/drivers/staging/iio/gyro/adxrs450.h
@@ -49,7 +49,7 @@ enum {
  * @us:			actual spi_device
  * @buf_lock:		mutex to protect tx and rx
  * @tx:			transmit buffer
- * @rx:			recieve buffer
+ * @rx:			receive buffer
  **/
 struct adxrs450_state {
 	struct spi_device	*us;
diff --git a/drivers/staging/iio/gyro/adxrs450_core.c b/drivers/staging/iio/gyro/adxrs450_core.c
index 15e2496f70c..6513119b1e9 100644
--- a/drivers/staging/iio/gyro/adxrs450_core.c
+++ b/drivers/staging/iio/gyro/adxrs450_core.c
@@ -18,8 +18,8 @@
 #include <linux/list.h>
 #include <linux/module.h>
 
-#include "../iio.h"
-#include "../sysfs.h"
+#include <linux/iio/iio.h>
+#include <linux/iio/sysfs.h>
 
 #include "adxrs450.h"
 
@@ -265,7 +265,7 @@ static int adxrs450_read_raw(struct iio_dev *indio_dev,
 	s16 t;
 
 	switch (mask) {
-	case 0:
+	case IIO_CHAN_INFO_RAW:
 		switch (chan->type) {
 		case IIO_ANGL_VEL:
 			ret = adxrs450_spi_sensor_data(indio_dev, &t);
@@ -329,14 +329,16 @@ static const struct iio_chan_spec adxrs450_channels[2][2] = {
 			.type = IIO_ANGL_VEL,
 			.modified = 1,
 			.channel2 = IIO_MOD_Z,
-			.info_mask = IIO_CHAN_INFO_CALIBBIAS_SEPARATE_BIT |
+			.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT |
+			IIO_CHAN_INFO_CALIBBIAS_SEPARATE_BIT |
 			IIO_CHAN_INFO_QUADRATURE_CORRECTION_RAW_SEPARATE_BIT |
 			IIO_CHAN_INFO_SCALE_SEPARATE_BIT,
 		}, {
 			.type = IIO_TEMP,
 			.indexed = 1,
 			.channel = 0,
-			.info_mask = IIO_CHAN_INFO_SCALE_SEPARATE_BIT,
+			.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT |
+			IIO_CHAN_INFO_SCALE_SEPARATE_BIT,
 		}
 	},
 	[ID_ADXRS453] = {
@@ -344,13 +346,15 @@ static const struct iio_chan_spec adxrs450_channels[2][2] = {
 			.type = IIO_ANGL_VEL,
 			.modified = 1,
 			.channel2 = IIO_MOD_Z,
-			.info_mask = IIO_CHAN_INFO_SCALE_SEPARATE_BIT |
+			.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT |
+			IIO_CHAN_INFO_SCALE_SEPARATE_BIT |
 			IIO_CHAN_INFO_QUADRATURE_CORRECTION_RAW_SEPARATE_BIT,
 		}, {
 			.type = IIO_TEMP,
 			.indexed = 1,
 			.channel = 0,
-			.info_mask = IIO_CHAN_INFO_SCALE_SEPARATE_BIT,
+			.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT |
+			IIO_CHAN_INFO_SCALE_SEPARATE_BIT,
 		}
 	},
 };
@@ -368,7 +372,7 @@ static int __devinit adxrs450_probe(struct spi_device *spi)
 	struct iio_dev *indio_dev;
 
 	/* setup the industrialio driver allocated elements */
-	indio_dev = iio_allocate_device(sizeof(*st));
+	indio_dev = iio_device_alloc(sizeof(*st));
 	if (indio_dev == NULL) {
 		ret = -ENOMEM;
 		goto error_ret;
@@ -399,7 +403,7 @@ static int __devinit adxrs450_probe(struct spi_device *spi)
 error_initial:
 	iio_device_unregister(indio_dev);
 error_free_dev:
-	iio_free_device(indio_dev);
+	iio_device_free(indio_dev);
 
 error_ret:
 	return ret;
@@ -408,7 +412,7 @@ error_ret:
 static int adxrs450_remove(struct spi_device *spi)
 {
 	iio_device_unregister(spi_get_drvdata(spi));
-	iio_free_device(spi_get_drvdata(spi));
+	iio_device_free(spi_get_drvdata(spi));
 
 	return 0;
 }
diff --git a/drivers/staging/iio/iio.h b/drivers/staging/iio/iio.h
deleted file mode 100644
index b9cd454f69e..00000000000
--- a/drivers/staging/iio/iio.h
+++ /dev/null
@@ -1,471 +0,0 @@
-
-/* The industrial I/O core
- *
- * Copyright (c) 2008 Jonathan Cameron
- *
- * 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 _INDUSTRIAL_IO_H_
-#define _INDUSTRIAL_IO_H_
-
-#include <linux/device.h>
-#include <linux/cdev.h>
-#include "types.h"
-/* IIO TODO LIST */
-/*
- * Provide means of adjusting timer accuracy.
- * Currently assumes nano seconds.
- */
-
-enum iio_data_type {
-	IIO_RAW,
-	IIO_PROCESSED,
-};
-
-/* Could add the raw attributes as well - allowing buffer only devices */
-enum iio_chan_info_enum {
-	/* 0 is reserved for raw attributes */
-	IIO_CHAN_INFO_SCALE = 1,
-	IIO_CHAN_INFO_OFFSET,
-	IIO_CHAN_INFO_CALIBSCALE,
-	IIO_CHAN_INFO_CALIBBIAS,
-	IIO_CHAN_INFO_PEAK,
-	IIO_CHAN_INFO_PEAK_SCALE,
-	IIO_CHAN_INFO_QUADRATURE_CORRECTION_RAW,
-	IIO_CHAN_INFO_AVERAGE_RAW,
-	IIO_CHAN_INFO_LOW_PASS_FILTER_3DB_FREQUENCY,
-};
-
-#define IIO_CHAN_INFO_SHARED_BIT(type) BIT(type*2)
-#define IIO_CHAN_INFO_SEPARATE_BIT(type) BIT(type*2 + 1)
-
-#define IIO_CHAN_INFO_SCALE_SEPARATE_BIT		\
-	IIO_CHAN_INFO_SEPARATE_BIT(IIO_CHAN_INFO_SCALE)
-#define IIO_CHAN_INFO_SCALE_SHARED_BIT			\
-	IIO_CHAN_INFO_SHARED_BIT(IIO_CHAN_INFO_SCALE)
-#define IIO_CHAN_INFO_OFFSET_SEPARATE_BIT			\
-	IIO_CHAN_INFO_SEPARATE_BIT(IIO_CHAN_INFO_OFFSET)
-#define IIO_CHAN_INFO_OFFSET_SHARED_BIT			\
-	IIO_CHAN_INFO_SHARED_BIT(IIO_CHAN_INFO_OFFSET)
-#define IIO_CHAN_INFO_CALIBSCALE_SEPARATE_BIT			\
-	IIO_CHAN_INFO_SEPARATE_BIT(IIO_CHAN_INFO_CALIBSCALE)
-#define IIO_CHAN_INFO_CALIBSCALE_SHARED_BIT			\
-	IIO_CHAN_INFO_SHARED_BIT(IIO_CHAN_INFO_CALIBSCALE)
-#define IIO_CHAN_INFO_CALIBBIAS_SEPARATE_BIT			\
-	IIO_CHAN_INFO_SEPARATE_BIT(IIO_CHAN_INFO_CALIBBIAS)
-#define IIO_CHAN_INFO_CALIBBIAS_SHARED_BIT			\
-	IIO_CHAN_INFO_SHARED_BIT(IIO_CHAN_INFO_CALIBBIAS)
-#define IIO_CHAN_INFO_PEAK_SEPARATE_BIT			\
-	IIO_CHAN_INFO_SEPARATE_BIT(IIO_CHAN_INFO_PEAK)
-#define IIO_CHAN_INFO_PEAK_SHARED_BIT			\
-	IIO_CHAN_INFO_SHARED_BIT(IIO_CHAN_INFO_PEAK)
-#define IIO_CHAN_INFO_PEAKSCALE_SEPARATE_BIT			\
-	IIO_CHAN_INFO_SEPARATE_BIT(IIO_CHAN_INFO_PEAKSCALE)
-#define IIO_CHAN_INFO_PEAKSCALE_SHARED_BIT			\
-	IIO_CHAN_INFO_SHARED_BIT(IIO_CHAN_INFO_PEAKSCALE)
-#define IIO_CHAN_INFO_QUADRATURE_CORRECTION_RAW_SEPARATE_BIT	\
-	IIO_CHAN_INFO_SEPARATE_BIT(				\
-		IIO_CHAN_INFO_QUADRATURE_CORRECTION_RAW)
-#define IIO_CHAN_INFO_QUADRATURE_CORRECTION_RAW_SHARED_BIT	\
-	IIO_CHAN_INFO_SHARED_BIT(				\
-		IIO_CHAN_INFO_QUADRATURE_CORRECTION_RAW)
-#define IIO_CHAN_INFO_AVERAGE_RAW_SEPARATE_BIT			\
-	IIO_CHAN_INFO_SEPARATE_BIT(IIO_CHAN_INFO_AVERAGE_RAW)
-#define IIO_CHAN_INFO_AVERAGE_RAW_SHARED_BIT			\
-	IIO_CHAN_INFO_SHARED_BIT(IIO_CHAN_INFO_AVERAGE_RAW)
-#define IIO_CHAN_INFO_LOW_PASS_FILTER_3DB_FREQUENCY_SHARED_BIT \
-	IIO_CHAN_INFO_SHARED_BIT(			       \
-		IIO_CHAN_INFO_LOW_PASS_FILTER_3DB_FREQUENCY)
-#define IIO_CHAN_INFO_LOW_PASS_FILTER_3DB_FREQUENCY_SEPARATE_BIT \
-	IIO_CHAN_INFO_SEPARATE_BIT(			       \
-		IIO_CHAN_INFO_LOW_PASS_FILTER_3DB_FREQUENCY)
-
-enum iio_endian {
-	IIO_CPU,
-	IIO_BE,
-	IIO_LE,
-};
-
-struct iio_chan_spec;
-struct iio_dev;
-
-/**
- * struct iio_chan_spec_ext_info - Extended channel info attribute
- * @name:	Info attribute name
- * @shared:	Whether this attribute is shared between all channels.
- * @read:	Read callback for this info attribute, may be NULL.
- * @write:	Write callback for this info attribute, may be NULL.
- */
-struct iio_chan_spec_ext_info {
-	const char *name;
-	bool shared;
-	ssize_t (*read)(struct iio_dev *, struct iio_chan_spec const *,
-			char *buf);
-	ssize_t (*write)(struct iio_dev *, struct iio_chan_spec const *,
-			const char *buf, size_t len);
-};
-
-/**
- * struct iio_chan_spec - specification of a single channel
- * @type:		What type of measurement is the channel making.
- * @channel:		What number or name do we wish to assign the channel.
- * @channel2:		If there is a second number for a differential
- *			channel then this is it. If modified is set then the
- *			value here specifies the modifier.
- * @address:		Driver specific identifier.
- * @scan_index:	Monotonic index to give ordering in scans when read
- *			from a buffer.
- * @scan_type:		Sign:		's' or 'u' to specify signed or unsigned
- *			realbits:	Number of valid bits of data
- *			storage_bits:	Realbits + padding
- *			shift:		Shift right by this before masking out
- *					realbits.
- *			endianness:	little or big endian
- * @info_mask:		What information is to be exported about this channel.
- *			This includes calibbias, scale etc.
- * @event_mask:	What events can this channel produce.
- * @ext_info:		Array of extended info attributes for this channel.
- *			The array is NULL terminated, the last element should
- *			have it's name field set to NULL.
- * @extend_name:	Allows labeling of channel attributes with an
- *			informative name. Note this has no effect codes etc,
- *			unlike modifiers.
- * @datasheet_name:	A name used in in kernel mapping of channels. It should
- *			correspond to the first name that the channel is referred
- *			to by in the datasheet (e.g. IND), or the nearest
- *			possible compound name (e.g. IND-INC).
- * @processed_val:	Flag to specify the data access attribute should be
- *			*_input rather than *_raw.
- * @modified:		Does a modifier apply to this channel. What these are
- *			depends on the channel type.  Modifier is set in
- *			channel2. Examples are IIO_MOD_X for axial sensors about
- *			the 'x' axis.
- * @indexed:		Specify the channel has a numerical index. If not,
- *			the value in channel will be suppressed for attribute
- *			but not for event codes. Typically set it to 0 when
- *			the index is false.
- * @differential:	Channel is differential.
- */
-struct iio_chan_spec {
-	enum iio_chan_type	type;
-	int			channel;
-	int			channel2;
-	unsigned long		address;
-	int			scan_index;
-	struct {
-		char	sign;
-		u8	realbits;
-		u8	storagebits;
-		u8	shift;
-		enum iio_endian endianness;
-	} scan_type;
-	long			info_mask;
-	long			event_mask;
-	const struct iio_chan_spec_ext_info *ext_info;
-	char			*extend_name;
-	const char		*datasheet_name;
-	unsigned		processed_val:1;
-	unsigned		modified:1;
-	unsigned		indexed:1;
-	unsigned		output:1;
-	unsigned		differential:1;
-};
-
-#define IIO_ST(si, rb, sb, sh)						\
-	{ .sign = si, .realbits = rb, .storagebits = sb, .shift = sh }
-
-/* Macro assumes input channels */
-#define IIO_CHAN(_type, _mod, _indexed, _proc, _name, _chan, _chan2, \
-		 _inf_mask, _address, _si, _stype, _event_mask)		\
-	{ .type = _type,						\
-	  .output = 0,							\
-	  .modified = _mod,						\
-	  .indexed = _indexed,						\
-	  .processed_val = _proc,					\
-	  .extend_name = _name,						\
-	  .channel = _chan,						\
-	  .channel2 = _chan2,						\
-	  .info_mask = _inf_mask,					\
-	  .address = _address,						\
-	  .scan_index = _si,						\
-	  .scan_type = _stype,						\
-	  .event_mask = _event_mask }
-
-#define IIO_CHAN_SOFT_TIMESTAMP(_si)					\
-	{ .type = IIO_TIMESTAMP, .channel = -1,				\
-			.scan_index = _si, .scan_type = IIO_ST('s', 64, 64, 0) }
-
-/**
- * iio_get_time_ns() - utility function to get a time stamp for events etc
- **/
-static inline s64 iio_get_time_ns(void)
-{
-	struct timespec ts;
-	/*
-	 * calls getnstimeofday.
-	 * If hrtimers then up to ns accurate, if not microsecond.
-	 */
-	ktime_get_real_ts(&ts);
-
-	return timespec_to_ns(&ts);
-}
-
-/* Device operating modes */
-#define INDIO_DIRECT_MODE		0x01
-#define INDIO_BUFFER_TRIGGERED		0x02
-#define INDIO_BUFFER_HARDWARE		0x08
-
-#define INDIO_ALL_BUFFER_MODES					\
-	(INDIO_BUFFER_TRIGGERED | INDIO_BUFFER_HARDWARE)
-
-struct iio_trigger; /* forward declaration */
-struct iio_dev;
-
-/**
- * struct iio_info - constant information about device
- * @driver_module:	module structure used to ensure correct
- *			ownership of chrdevs etc
- * @event_attrs:	event control attributes
- * @attrs:		general purpose device attributes
- * @read_raw:		function to request a value from the device.
- *			mask specifies which value. Note 0 means a reading of
- *			the channel in question.  Return value will specify the
- *			type of value returned by the device. val and val2 will
- *			contain the elements making up the returned value.
- * @write_raw:		function to write a value to the device.
- *			Parameters are the same as for read_raw.
- * @write_raw_get_fmt:	callback function to query the expected
- *			format/precision. If not set by the driver, write_raw
- *			returns IIO_VAL_INT_PLUS_MICRO.
- * @read_event_config:	find out if the event is enabled.
- * @write_event_config:	set if the event is enabled.
- * @read_event_value:	read a value associated with the event. Meaning
- *			is event dependant. event_code specifies which event.
- * @write_event_value:	write the value associated with the event.
- *			Meaning is event dependent.
- * @validate_trigger:	function to validate the trigger when the
- *			current trigger gets changed.
- **/
-struct iio_info {
-	struct module			*driver_module;
-	struct attribute_group		*event_attrs;
-	const struct attribute_group	*attrs;
-
-	int (*read_raw)(struct iio_dev *indio_dev,
-			struct iio_chan_spec const *chan,
-			int *val,
-			int *val2,
-			long mask);
-
-	int (*write_raw)(struct iio_dev *indio_dev,
-			 struct iio_chan_spec const *chan,
-			 int val,
-			 int val2,
-			 long mask);
-
-	int (*write_raw_get_fmt)(struct iio_dev *indio_dev,
-			 struct iio_chan_spec const *chan,
-			 long mask);
-
-	int (*read_event_config)(struct iio_dev *indio_dev,
-				 u64 event_code);
-
-	int (*write_event_config)(struct iio_dev *indio_dev,
-				  u64 event_code,
-				  int state);
-
-	int (*read_event_value)(struct iio_dev *indio_dev,
-				u64 event_code,
-				int *val);
-	int (*write_event_value)(struct iio_dev *indio_dev,
-				 u64 event_code,
-				 int val);
-	int (*validate_trigger)(struct iio_dev *indio_dev,
-				struct iio_trigger *trig);
-	int (*update_scan_mode)(struct iio_dev *indio_dev,
-				const unsigned long *scan_mask);
-	int (*debugfs_reg_access)(struct iio_dev *indio_dev,
-				  unsigned reg, unsigned writeval,
-				  unsigned *readval);
-};
-
-/**
- * struct iio_buffer_setup_ops - buffer setup related callbacks
- * @preenable:		[DRIVER] function to run prior to marking buffer enabled
- * @postenable:		[DRIVER] function to run after marking buffer enabled
- * @predisable:		[DRIVER] function to run prior to marking buffer
- *			disabled
- * @postdisable:	[DRIVER] function to run after marking buffer disabled
- */
-struct iio_buffer_setup_ops {
-	int				(*preenable)(struct iio_dev *);
-	int				(*postenable)(struct iio_dev *);
-	int				(*predisable)(struct iio_dev *);
-	int				(*postdisable)(struct iio_dev *);
-};
-
-/**
- * struct iio_dev - industrial I/O device
- * @id:			[INTERN] used to identify device internally
- * @modes:		[DRIVER] operating modes supported by device
- * @currentmode:	[DRIVER] current operating mode
- * @dev:		[DRIVER] device structure, should be assigned a parent
- *			and owner
- * @event_interface:	[INTERN] event chrdevs associated with interrupt lines
- * @buffer:		[DRIVER] any buffer present
- * @mlock:		[INTERN] lock used to prevent simultaneous device state
- *			changes
- * @available_scan_masks: [DRIVER] optional array of allowed bitmasks
- * @masklength:		[INTERN] the length of the mask established from
- *			channels
- * @active_scan_mask:	[INTERN] union of all scan masks requested by buffers
- * @trig:		[INTERN] current device trigger (buffer modes)
- * @pollfunc:		[DRIVER] function run on trigger being received
- * @channels:		[DRIVER] channel specification structure table
- * @num_channels:	[DRIVER] number of chanels specified in @channels.
- * @channel_attr_list:	[INTERN] keep track of automatically created channel
- *			attributes
- * @chan_attr_group:	[INTERN] group for all attrs in base directory
- * @name:		[DRIVER] name of the device.
- * @info:		[DRIVER] callbacks and constant info from driver
- * @info_exist_lock:	[INTERN] lock to prevent use during removal
- * @chrdev:		[INTERN] associated character device
- * @groups:		[INTERN] attribute groups
- * @groupcounter:	[INTERN] index of next attribute group
- * @flags:		[INTERN] file ops related flags including busy flag.
- * @debugfs_dentry:	[INTERN] device specific debugfs dentry.
- * @cached_reg_addr:	[INTERN] cached register address for debugfs reads.
- */
-struct iio_dev {
-	int				id;
-
-	int				modes;
-	int				currentmode;
-	struct device			dev;
-
-	struct iio_event_interface	*event_interface;
-
-	struct iio_buffer		*buffer;
-	struct mutex			mlock;
-
-	const unsigned long		*available_scan_masks;
-	unsigned			masklength;
-	const unsigned long		*active_scan_mask;
-	struct iio_trigger		*trig;
-	struct iio_poll_func		*pollfunc;
-
-	struct iio_chan_spec const	*channels;
-	int				num_channels;
-
-	struct list_head		channel_attr_list;
-	struct attribute_group		chan_attr_group;
-	const char			*name;
-	const struct iio_info		*info;
-	struct mutex			info_exist_lock;
-	const struct iio_buffer_setup_ops	*setup_ops;
-	struct cdev			chrdev;
-#define IIO_MAX_GROUPS 6
-	const struct attribute_group	*groups[IIO_MAX_GROUPS + 1];
-	int				groupcounter;
-
-	unsigned long			flags;
-#if defined(CONFIG_DEBUG_FS)
-	struct dentry			*debugfs_dentry;
-	unsigned			cached_reg_addr;
-#endif
-};
-
-/**
- * iio_find_channel_from_si() - get channel from its scan index
- * @indio_dev:		device
- * @si:			scan index to match
- */
-const struct iio_chan_spec
-*iio_find_channel_from_si(struct iio_dev *indio_dev, int si);
-
-/**
- * iio_device_register() - register a device with the IIO subsystem
- * @indio_dev:		Device structure filled by the device driver
- **/
-int iio_device_register(struct iio_dev *indio_dev);
-
-/**
- * iio_device_unregister() - unregister a device from the IIO subsystem
- * @indio_dev:		Device structure representing the device.
- **/
-void iio_device_unregister(struct iio_dev *indio_dev);
-
-/**
- * iio_push_event() - try to add event to the list for userspace reading
- * @indio_dev:		IIO device structure
- * @ev_code:		What event
- * @timestamp:		When the event occurred
- **/
-int iio_push_event(struct iio_dev *indio_dev, u64 ev_code, s64 timestamp);
-
-extern struct bus_type iio_bus_type;
-
-/**
- * iio_put_device() - reference counted deallocation of struct device
- * @dev: the iio_device containing the device
- **/
-static inline void iio_put_device(struct iio_dev *indio_dev)
-{
-	if (indio_dev)
-		put_device(&indio_dev->dev);
-};
-
-/* Can we make this smaller? */
-#define IIO_ALIGN L1_CACHE_BYTES
-/**
- * iio_allocate_device() - allocate an iio_dev from a driver
- * @sizeof_priv: Space to allocate for private structure.
- **/
-struct iio_dev *iio_allocate_device(int sizeof_priv);
-
-static inline void *iio_priv(const struct iio_dev *indio_dev)
-{
-	return (char *)indio_dev + ALIGN(sizeof(struct iio_dev), IIO_ALIGN);
-}
-
-static inline struct iio_dev *iio_priv_to_dev(void *priv)
-{
-	return (struct iio_dev *)((char *)priv -
-				  ALIGN(sizeof(struct iio_dev), IIO_ALIGN));
-}
-
-/**
- * iio_free_device() - free an iio_dev from a driver
- * @dev: the iio_dev associated with the device
- **/
-void iio_free_device(struct iio_dev *indio_dev);
-
-/**
- * iio_buffer_enabled() - helper function to test if the buffer is enabled
- * @indio_dev:		IIO device info structure for device
- **/
-static inline bool iio_buffer_enabled(struct iio_dev *indio_dev)
-{
-	return indio_dev->currentmode
-		& (INDIO_BUFFER_TRIGGERED | INDIO_BUFFER_HARDWARE);
-};
-
-/**
- * iio_get_debugfs_dentry() - helper function to get the debugfs_dentry
- * @indio_dev:		IIO device info structure for device
- **/
-#if defined(CONFIG_DEBUG_FS)
-static inline struct dentry *iio_get_debugfs_dentry(struct iio_dev *indio_dev)
-{
-	return indio_dev->debugfs_dentry;
-};
-#else
-static inline struct dentry *iio_get_debugfs_dentry(struct iio_dev *indio_dev)
-{
-	return NULL;
-};
-#endif
-
-#endif /* _INDUSTRIAL_IO_H_ */
diff --git a/drivers/staging/iio/iio_dummy_evgen.c b/drivers/staging/iio/iio_dummy_evgen.c
index f39f346bf04..0cd4fe916bf 100644
--- a/drivers/staging/iio/iio_dummy_evgen.c
+++ b/drivers/staging/iio/iio_dummy_evgen.c
@@ -22,8 +22,8 @@
 #include <linux/sysfs.h>
 
 #include "iio_dummy_evgen.h"
-#include "iio.h"
-#include "sysfs.h"
+#include <linux/iio/iio.h>
+#include <linux/iio/sysfs.h>
 
 /* Fiddly bit of faking and irq without hardware */
 #define IIO_EVENTGEN_NO 10
diff --git a/drivers/staging/iio/iio_hwmon.c b/drivers/staging/iio/iio_hwmon.c
index a603a5f51f9..b03554fee44 100644
--- a/drivers/staging/iio/iio_hwmon.c
+++ b/drivers/staging/iio/iio_hwmon.c
@@ -14,8 +14,8 @@
 #include <linux/platform_device.h>
 #include <linux/hwmon.h>
 #include <linux/hwmon-sysfs.h>
-#include "consumer.h"
-#include "types.h"
+#include <linux/iio/consumer.h>
+#include <linux/iio/types.h>
 
 /**
  * struct iio_hwmon_state - device instance state
diff --git a/drivers/staging/iio/iio_simple_dummy.c b/drivers/staging/iio/iio_simple_dummy.c
index e3a94572bb4..310411911ed 100644
--- a/drivers/staging/iio/iio_simple_dummy.c
+++ b/drivers/staging/iio/iio_simple_dummy.c
@@ -19,10 +19,10 @@
 #include <linux/module.h>
 #include <linux/moduleparam.h>
 
-#include "iio.h"
-#include "sysfs.h"
-#include "events.h"
-#include "buffer.h"
+#include <linux/iio/iio.h>
+#include <linux/iio/sysfs.h>
+#include <linux/iio/events.h>
+#include <linux/iio/buffer.h>
 #include "iio_simple_dummy.h"
 
 /*
@@ -73,6 +73,12 @@ static struct iio_chan_spec iio_dummy_channels[] = {
 		/* What other information is available? */
 		.info_mask =
 		/*
+		 * in_voltage0_raw
+		 * Raw (unscaled no bias removal etc) measurement
+		 * from the device.
+		 */
+		IIO_CHAN_INFO_RAW_SEPARATE_BIT |
+		/*
 		 * in_voltage0_offset
 		 * Offset for userspace to apply prior to scale
 		 * when converting to standard units (microvolts)
@@ -114,6 +120,12 @@ static struct iio_chan_spec iio_dummy_channels[] = {
 		.channel2 = 2,
 		.info_mask =
 		/*
+		 * in_voltage1-voltage2_raw
+		 * Raw (unscaled no bias removal etc) measurement
+		 * from the device.
+		 */
+		IIO_CHAN_INFO_RAW_SEPARATE_BIT |
+		/*
 		 * in_voltage-voltage_scale
 		 * Shared version of scale - shared by differential
 		 * input channels of type IIO_VOLTAGE.
@@ -135,6 +147,7 @@ static struct iio_chan_spec iio_dummy_channels[] = {
 		.channel = 3,
 		.channel2 = 4,
 		.info_mask =
+		IIO_CHAN_INFO_RAW_SEPARATE_BIT |
 		IIO_CHAN_INFO_SCALE_SHARED_BIT,
 		.scan_index = diffvoltage3m4,
 		.scan_type = {
@@ -154,6 +167,7 @@ static struct iio_chan_spec iio_dummy_channels[] = {
 		/* Channel 2 is use for modifiers */
 		.channel2 = IIO_MOD_X,
 		.info_mask =
+		IIO_CHAN_INFO_RAW_SEPARATE_BIT |
 		/*
 		 * Internal bias correction value. Applied
 		 * by the hardware or driver prior to userspace
@@ -177,6 +191,7 @@ static struct iio_chan_spec iio_dummy_channels[] = {
 	/* DAC channel out_voltage0_raw */
 	{
 		.type = IIO_VOLTAGE,
+		.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT,
 		.output = 1,
 		.indexed = 1,
 		.channel = 0,
@@ -203,7 +218,7 @@ static int iio_dummy_read_raw(struct iio_dev *indio_dev,
 
 	mutex_lock(&st->lock);
 	switch (mask) {
-	case 0: /* magic value - channel value read */
+	case IIO_CHAN_INFO_RAW: /* magic value - channel value read */
 		switch (chan->type) {
 		case IIO_VOLTAGE:
 			if (chan->output) {
@@ -290,7 +305,7 @@ static int iio_dummy_write_raw(struct iio_dev *indio_dev,
 	struct iio_dummy_state *st = iio_priv(indio_dev);
 
 	switch (mask) {
-	case 0:
+	case IIO_CHAN_INFO_RAW:
 		if (chan->output == 0)
 			return -EINVAL;
 
@@ -377,7 +392,7 @@ static int __devinit iio_dummy_probe(int index)
 	 * It also has a region (accessed by iio_priv()
 	 * for chip specific state information.
 	 */
-	indio_dev = iio_allocate_device(sizeof(*st));
+	indio_dev = iio_device_alloc(sizeof(*st));
 	if (indio_dev == NULL) {
 		ret = -ENOMEM;
 		goto error_ret;
@@ -455,9 +470,7 @@ error_unconfigure_buffer:
 error_unregister_events:
 	iio_simple_dummy_events_unregister(indio_dev);
 error_free_device:
-	/* Note free device should only be called, before registration
-	 * has succeeded. */
-	iio_free_device(indio_dev);
+	iio_device_free(indio_dev);
 error_ret:
 	return ret;
 }
@@ -494,7 +507,7 @@ static int iio_dummy_remove(int index)
 		goto error_ret;
 
 	/* Free all structures */
-	iio_free_device(indio_dev);
+	iio_device_free(indio_dev);
 
 error_ret:
 	return ret;
diff --git a/drivers/staging/iio/iio_simple_dummy_buffer.c b/drivers/staging/iio/iio_simple_dummy_buffer.c
index bb4daf74436..fdfc8739095 100644
--- a/drivers/staging/iio/iio_simple_dummy_buffer.c
+++ b/drivers/staging/iio/iio_simple_dummy_buffer.c
@@ -18,9 +18,9 @@
 #include <linux/irq.h>
 #include <linux/bitmap.h>
 
-#include "iio.h"
-#include "trigger_consumer.h"
-#include "kfifo_buf.h"
+#include <linux/iio/iio.h>
+#include <linux/iio/trigger_consumer.h>
+#include <linux/iio/kfifo_buf.h>
 
 #include "iio_simple_dummy.h"
 
@@ -37,7 +37,7 @@ static const s16 fakedata[] = {
  * @irq: the interrupt number
  * @p: private data - always a pointer to the poll func.
  *
- * This is the guts of buffered capture. On a trigger event occuring,
+ * This is the guts of buffered capture. On a trigger event occurring,
  * if the pollfunc is attached then this handler is called as a threaded
  * interrupt (and hence may sleep). It is responsible for grabbing data
  * from the device and pushing it into the associated buffer.
@@ -48,12 +48,9 @@ static irqreturn_t iio_simple_dummy_trigger_h(int irq, void *p)
 	struct iio_dev *indio_dev = pf->indio_dev;
 	struct iio_buffer *buffer = indio_dev->buffer;
 	int len = 0;
-	/*
-	 * The datasize is obtained from the buffer. It was stored when
-	 * the preenable setup function was called.
-	 */
-	size_t datasize = buffer->access->get_bytes_per_datum(buffer);
-	u16 *data = kmalloc(datasize, GFP_KERNEL);
+	u16 *data;
+
+	data = kmalloc(indio_dev->scan_bytes, GFP_KERNEL);
 	if (data == NULL)
 		return -ENOMEM;
 
@@ -64,7 +61,7 @@ static irqreturn_t iio_simple_dummy_trigger_h(int irq, void *p)
 		 *   up a fast read.  The capture will consist of all of them.
 		 *   Hence we just call the grab data function and fill the
 		 *   buffer without processing.
-		 * sofware scans: can be considered to be random access
+		 * software scans: can be considered to be random access
 		 *   so efficient reading is just a case of minimal bus
 		 *   transactions.
 		 * software culled hardware scans:
@@ -87,7 +84,7 @@ static irqreturn_t iio_simple_dummy_trigger_h(int irq, void *p)
 		}
 	}
 	/* Store a timestampe at an 8 byte boundary */
-	if (buffer->scan_timestamp)
+	if (indio_dev->scan_timestamp)
 		*(s64 *)(((phys_addr_t)data + len
 				+ sizeof(s64) - 1) & ~(sizeof(s64) - 1))
 			= iio_get_time_ns();
diff --git a/drivers/staging/iio/iio_simple_dummy_events.c b/drivers/staging/iio/iio_simple_dummy_events.c
index 449c7a5ece8..317b77465db 100644
--- a/drivers/staging/iio/iio_simple_dummy_events.c
+++ b/drivers/staging/iio/iio_simple_dummy_events.c
@@ -12,9 +12,9 @@
 #include <linux/interrupt.h>
 #include <linux/irq.h>
 
-#include "iio.h"
-#include "sysfs.h"
-#include "events.h"
+#include <linux/iio/iio.h>
+#include <linux/iio/sysfs.h>
+#include <linux/iio/events.h>
 #include "iio_simple_dummy.h"
 
 /* Evgen 'fakes' interrupt events for this example */
@@ -122,7 +122,7 @@ int iio_simple_dummy_write_event_value(struct iio_dev *indio_dev,
  * @private: pointer to device instance state.
  *
  * This handler is responsible for querying the device to find out what
- * event occured and for then pushing that event towards userspace.
+ * event occurred and for then pushing that event towards userspace.
  * Here only one event occurs so we push that directly on with locally
  * grabbed timestamp.
  */
diff --git a/drivers/staging/iio/impedance-analyzer/ad5933.c b/drivers/staging/iio/impedance-analyzer/ad5933.c
index cd82b56d58a..a8e51bc0443 100644
--- a/drivers/staging/iio/impedance-analyzer/ad5933.c
+++ b/drivers/staging/iio/impedance-analyzer/ad5933.c
@@ -19,9 +19,9 @@
 #include <linux/module.h>
 #include <asm/div64.h>
 
-#include "../iio.h"
-#include "../sysfs.h"
-#include "../buffer.h"
+#include <linux/iio/iio.h>
+#include <linux/iio/sysfs.h>
+#include <linux/iio/buffer.h>
 #include "../ring_sw.h"
 
 #include "ad5933.h"
@@ -109,15 +109,46 @@ static struct ad5933_platform_data ad5933_default_pdata  = {
 };
 
 static struct iio_chan_spec ad5933_channels[] = {
-	IIO_CHAN(IIO_TEMP, 0, 1, 1, NULL, 0, 0, 0,
-		 0, AD5933_REG_TEMP_DATA, IIO_ST('s', 14, 16, 0), 0),
-	/* Ring Channels */
-	IIO_CHAN(IIO_VOLTAGE, 0, 1, 0, "real_raw", 0, 0,
-		 IIO_CHAN_INFO_SCALE_SEPARATE_BIT,
-		 AD5933_REG_REAL_DATA, 0, IIO_ST('s', 16, 16, 0), 0),
-	IIO_CHAN(IIO_VOLTAGE, 0, 1, 0, "imag_raw", 0, 0,
-		 IIO_CHAN_INFO_SCALE_SEPARATE_BIT,
-		 AD5933_REG_IMAG_DATA, 1, IIO_ST('s', 16, 16, 0), 0),
+	{
+		.type = IIO_TEMP,
+		.indexed = 1,
+		.channel = 0,
+		.info_mask = IIO_CHAN_INFO_PROCESSED_SEPARATE_BIT,
+		.address = AD5933_REG_TEMP_DATA,
+		.scan_type = {
+			.sign = 's',
+			.realbits = 14,
+			.storagebits = 16,
+		},
+	}, { /* Ring Channels */
+		.type = IIO_VOLTAGE,
+		.indexed = 1,
+		.channel = 0,
+		.extend_name = "real_raw",
+		.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT |
+		IIO_CHAN_INFO_SCALE_SEPARATE_BIT,
+		.address = AD5933_REG_REAL_DATA,
+		.scan_index = 0,
+		.scan_type = {
+			.sign = 's',
+			.realbits = 16,
+			.storagebits = 16,
+		},
+	}, {
+		.type = IIO_VOLTAGE,
+		.indexed = 1,
+		.channel = 0,
+		.extend_name = "imag_raw",
+		.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT |
+		IIO_CHAN_INFO_SCALE_SEPARATE_BIT,
+		.address = AD5933_REG_IMAG_DATA,
+		.scan_index = 1,
+		.scan_type = {
+			.sign = 's',
+			.realbits = 16,
+			.storagebits = 16,
+		},
+	},
 };
 
 static int ad5933_i2c_write(struct i2c_client *client,
@@ -260,7 +291,7 @@ static ssize_t ad5933_show_frequency(struct device *dev,
 					struct device_attribute *attr,
 					char *buf)
 {
-	struct iio_dev *indio_dev = dev_get_drvdata(dev);
+	struct iio_dev *indio_dev = dev_to_iio_dev(dev);
 	struct ad5933_state *st = iio_priv(indio_dev);
 	struct iio_dev_attr *this_attr = to_iio_dev_attr(attr);
 	int ret;
@@ -289,7 +320,7 @@ static ssize_t ad5933_store_frequency(struct device *dev,
 					 const char *buf,
 					 size_t len)
 {
-	struct iio_dev *indio_dev = dev_get_drvdata(dev);
+	struct iio_dev *indio_dev = dev_to_iio_dev(dev);
 	struct ad5933_state *st = iio_priv(indio_dev);
 	struct iio_dev_attr *this_attr = to_iio_dev_attr(attr);
 	long val;
@@ -323,7 +354,7 @@ static ssize_t ad5933_show(struct device *dev,
 					struct device_attribute *attr,
 					char *buf)
 {
-	struct iio_dev *indio_dev = dev_get_drvdata(dev);
+	struct iio_dev *indio_dev = dev_to_iio_dev(dev);
 	struct ad5933_state *st = iio_priv(indio_dev);
 	struct iio_dev_attr *this_attr = to_iio_dev_attr(attr);
 	int ret = 0, len = 0;
@@ -366,7 +397,7 @@ static ssize_t ad5933_store(struct device *dev,
 					 const char *buf,
 					 size_t len)
 {
-	struct iio_dev *indio_dev = dev_get_drvdata(dev);
+	struct iio_dev *indio_dev = dev_to_iio_dev(dev);
 	struct ad5933_state *st = iio_priv(indio_dev);
 	struct iio_dev_attr *this_attr = to_iio_dev_attr(attr);
 	long val;
@@ -495,7 +526,8 @@ static int ad5933_read_raw(struct iio_dev *indio_dev,
 
 	mutex_lock(&indio_dev->mlock);
 	switch (m) {
-	case 0:
+	case IIO_CHAN_INFO_RAW:
+	case IIO_CHAN_INFO_PROCESSED:
 		if (iio_buffer_enabled(indio_dev)) {
 			ret = -EBUSY;
 			goto out;
@@ -537,19 +569,14 @@ static const struct iio_info ad5933_info = {
 static int ad5933_ring_preenable(struct iio_dev *indio_dev)
 {
 	struct ad5933_state *st = iio_priv(indio_dev);
-	size_t d_size;
 	int ret;
 
 	if (bitmap_empty(indio_dev->active_scan_mask, indio_dev->masklength))
 		return -EINVAL;
 
-	d_size = bitmap_weight(indio_dev->active_scan_mask,
-			       indio_dev->masklength) *
-		 ad5933_channels[1].scan_type.storagebits / 8;
-
-	if (indio_dev->buffer->access->set_bytes_per_datum)
-		indio_dev->buffer->access->
-			set_bytes_per_datum(indio_dev->buffer, d_size);
+	ret = iio_sw_buffer_preenable(indio_dev);
+	if (ret < 0)
+		return ret;
 
 	ret = ad5933_reset(st);
 	if (ret < 0)
@@ -678,7 +705,7 @@ static int __devinit ad5933_probe(struct i2c_client *client,
 	int ret, voltage_uv = 0;
 	struct ad5933_platform_data *pdata = client->dev.platform_data;
 	struct ad5933_state *st;
-	struct iio_dev *indio_dev = iio_allocate_device(sizeof(*st));
+	struct iio_dev *indio_dev = iio_device_alloc(sizeof(*st));
 	if (indio_dev == NULL)
 		return -ENOMEM;
 
@@ -757,7 +784,7 @@ error_put_reg:
 	if (!IS_ERR(st->reg))
 		regulator_put(st->reg);
 
-	iio_free_device(indio_dev);
+	iio_device_free(indio_dev);
 
 	return ret;
 }
@@ -774,7 +801,7 @@ static __devexit int ad5933_remove(struct i2c_client *client)
 		regulator_disable(st->reg);
 		regulator_put(st->reg);
 	}
-	iio_free_device(indio_dev);
+	iio_device_free(indio_dev);
 
 	return 0;
 }
diff --git a/drivers/staging/iio/imu/adis16400_core.c b/drivers/staging/iio/imu/adis16400_core.c
index e73ad7818d8..1f6bd854e95 100644
--- a/drivers/staging/iio/imu/adis16400_core.c
+++ b/drivers/staging/iio/imu/adis16400_core.c
@@ -26,9 +26,9 @@
 #include <linux/list.h>
 #include <linux/module.h>
 
-#include "../iio.h"
-#include "../sysfs.h"
-#include "../buffer.h"
+#include <linux/iio/iio.h>
+#include <linux/iio/sysfs.h>
+#include <linux/iio/buffer.h>
 #include "adis16400.h"
 
 enum adis16400_chip_variant {
@@ -179,7 +179,7 @@ static ssize_t adis16400_read_frequency(struct device *dev,
 		struct device_attribute *attr,
 		char *buf)
 {
-	struct iio_dev *indio_dev = dev_get_drvdata(dev);
+	struct iio_dev *indio_dev = dev_to_iio_dev(dev);
 	int ret, len = 0;
 	ret = adis16400_get_freq(indio_dev);
 	if (ret < 0)
@@ -225,7 +225,7 @@ static ssize_t adis16400_write_frequency(struct device *dev,
 		const char *buf,
 		size_t len)
 {
-	struct iio_dev *indio_dev = dev_get_drvdata(dev);
+	struct iio_dev *indio_dev = dev_to_iio_dev(dev);
 	struct adis16400_state *st = iio_priv(indio_dev);
 	long val;
 	int ret;
@@ -279,7 +279,7 @@ static ssize_t adis16400_write_reset(struct device *dev,
 	if (ret < 0)
 		return ret;
 	if (val) {
-		ret = adis16400_reset(dev_get_drvdata(dev));
+		ret = adis16400_reset(dev_to_iio_dev(dev));
 		if (ret < 0)
 			return ret;
 	}
@@ -545,7 +545,7 @@ static int adis16400_read_raw(struct iio_dev *indio_dev,
 	s16 val16;
 
 	switch (mask) {
-	case 0:
+	case IIO_CHAN_INFO_RAW:
 		mutex_lock(&indio_dev->mlock);
 		ret = adis16400_spi_read_reg_16(indio_dev,
 				adis16400_addresses[chan->address][0],
@@ -635,7 +635,8 @@ static struct iio_chan_spec adis16400_channels[] = {
 		.indexed = 1,
 		.channel = 0,
 		.extend_name = "supply",
-		.info_mask = IIO_CHAN_INFO_SCALE_SEPARATE_BIT,
+		.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT |
+		IIO_CHAN_INFO_SCALE_SEPARATE_BIT,
 		.address = in_supply,
 		.scan_index = ADIS16400_SCAN_SUPPLY,
 		.scan_type = IIO_ST('u', 14, 16, 0)
@@ -643,7 +644,8 @@ static struct iio_chan_spec adis16400_channels[] = {
 		.type = IIO_ANGL_VEL,
 		.modified = 1,
 		.channel2 = IIO_MOD_X,
-		.info_mask = IIO_CHAN_INFO_CALIBBIAS_SEPARATE_BIT |
+		.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT |
+		IIO_CHAN_INFO_CALIBBIAS_SEPARATE_BIT |
 		IIO_CHAN_INFO_SCALE_SHARED_BIT |
 		IIO_CHAN_INFO_LOW_PASS_FILTER_3DB_FREQUENCY_SHARED_BIT,
 		.address = gyro_x,
@@ -653,7 +655,8 @@ static struct iio_chan_spec adis16400_channels[] = {
 		.type = IIO_ANGL_VEL,
 		.modified = 1,
 		.channel2 = IIO_MOD_Y,
-		.info_mask = IIO_CHAN_INFO_CALIBBIAS_SEPARATE_BIT |
+		.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT |
+		IIO_CHAN_INFO_CALIBBIAS_SEPARATE_BIT |
 		IIO_CHAN_INFO_SCALE_SHARED_BIT |
 		IIO_CHAN_INFO_LOW_PASS_FILTER_3DB_FREQUENCY_SHARED_BIT,
 		.address = gyro_y,
@@ -663,7 +666,8 @@ static struct iio_chan_spec adis16400_channels[] = {
 		.type = IIO_ANGL_VEL,
 		.modified = 1,
 		.channel2 = IIO_MOD_Z,
-		.info_mask = IIO_CHAN_INFO_CALIBBIAS_SEPARATE_BIT |
+		.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT |
+		IIO_CHAN_INFO_CALIBBIAS_SEPARATE_BIT |
 		IIO_CHAN_INFO_SCALE_SHARED_BIT |
 		IIO_CHAN_INFO_LOW_PASS_FILTER_3DB_FREQUENCY_SHARED_BIT,
 		.address = gyro_z,
@@ -673,7 +677,8 @@ static struct iio_chan_spec adis16400_channels[] = {
 		.type = IIO_ACCEL,
 		.modified = 1,
 		.channel2 = IIO_MOD_X,
-		.info_mask = IIO_CHAN_INFO_CALIBBIAS_SEPARATE_BIT |
+		.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT |
+		IIO_CHAN_INFO_CALIBBIAS_SEPARATE_BIT |
 		IIO_CHAN_INFO_SCALE_SHARED_BIT |
 		IIO_CHAN_INFO_LOW_PASS_FILTER_3DB_FREQUENCY_SHARED_BIT,
 		.address = accel_x,
@@ -683,7 +688,8 @@ static struct iio_chan_spec adis16400_channels[] = {
 		.type = IIO_ACCEL,
 		.modified = 1,
 		.channel2 = IIO_MOD_Y,
-		.info_mask = IIO_CHAN_INFO_CALIBBIAS_SEPARATE_BIT |
+		.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT |
+		IIO_CHAN_INFO_CALIBBIAS_SEPARATE_BIT |
 		IIO_CHAN_INFO_SCALE_SHARED_BIT |
 		IIO_CHAN_INFO_LOW_PASS_FILTER_3DB_FREQUENCY_SHARED_BIT,
 		.address = accel_y,
@@ -693,7 +699,8 @@ static struct iio_chan_spec adis16400_channels[] = {
 		.type = IIO_ACCEL,
 		.modified = 1,
 		.channel2 = IIO_MOD_Z,
-		.info_mask = IIO_CHAN_INFO_CALIBBIAS_SEPARATE_BIT |
+		.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT |
+		IIO_CHAN_INFO_CALIBBIAS_SEPARATE_BIT |
 		IIO_CHAN_INFO_SCALE_SHARED_BIT |
 		IIO_CHAN_INFO_LOW_PASS_FILTER_3DB_FREQUENCY_SHARED_BIT,
 		.address = accel_z,
@@ -703,7 +710,8 @@ static struct iio_chan_spec adis16400_channels[] = {
 		.type = IIO_MAGN,
 		.modified = 1,
 		.channel2 = IIO_MOD_X,
-		.info_mask = IIO_CHAN_INFO_SCALE_SHARED_BIT |
+		.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT |
+		IIO_CHAN_INFO_SCALE_SHARED_BIT |
 		IIO_CHAN_INFO_LOW_PASS_FILTER_3DB_FREQUENCY_SHARED_BIT,
 		.address = magn_x,
 		.scan_index = ADIS16400_SCAN_MAGN_X,
@@ -712,7 +720,8 @@ static struct iio_chan_spec adis16400_channels[] = {
 		.type = IIO_MAGN,
 		.modified = 1,
 		.channel2 = IIO_MOD_Y,
-		.info_mask = IIO_CHAN_INFO_SCALE_SHARED_BIT |
+		.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT |
+		IIO_CHAN_INFO_SCALE_SHARED_BIT |
 		IIO_CHAN_INFO_LOW_PASS_FILTER_3DB_FREQUENCY_SHARED_BIT,
 		.address = magn_y,
 		.scan_index = ADIS16400_SCAN_MAGN_Y,
@@ -721,7 +730,8 @@ static struct iio_chan_spec adis16400_channels[] = {
 		.type = IIO_MAGN,
 		.modified = 1,
 		.channel2 = IIO_MOD_Z,
-		.info_mask = IIO_CHAN_INFO_SCALE_SHARED_BIT |
+		.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT |
+		IIO_CHAN_INFO_SCALE_SHARED_BIT |
 		IIO_CHAN_INFO_LOW_PASS_FILTER_3DB_FREQUENCY_SHARED_BIT,
 		.address = magn_z,
 		.scan_index = ADIS16400_SCAN_MAGN_Z,
@@ -730,7 +740,8 @@ static struct iio_chan_spec adis16400_channels[] = {
 		.type = IIO_TEMP,
 		.indexed = 1,
 		.channel = 0,
-		.info_mask = IIO_CHAN_INFO_OFFSET_SEPARATE_BIT |
+		.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT |
+		IIO_CHAN_INFO_OFFSET_SEPARATE_BIT |
 		IIO_CHAN_INFO_SCALE_SEPARATE_BIT,
 		.address = temp,
 		.scan_index = ADIS16400_SCAN_TEMP,
@@ -739,7 +750,8 @@ static struct iio_chan_spec adis16400_channels[] = {
 		.type = IIO_VOLTAGE,
 		.indexed = 1,
 		.channel = 1,
-		.info_mask = IIO_CHAN_INFO_SCALE_SEPARATE_BIT,
+		.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT |
+		IIO_CHAN_INFO_SCALE_SEPARATE_BIT,
 		.address = in1,
 		.scan_index = ADIS16400_SCAN_ADC_0,
 		.scan_type = IIO_ST('s', 12, 16, 0),
@@ -753,7 +765,8 @@ static struct iio_chan_spec adis16350_channels[] = {
 		.indexed = 1,
 		.channel = 0,
 		.extend_name = "supply",
-		.info_mask = IIO_CHAN_INFO_SCALE_SEPARATE_BIT,
+		.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT |
+		IIO_CHAN_INFO_SCALE_SEPARATE_BIT,
 		.address = in_supply,
 		.scan_index = ADIS16400_SCAN_SUPPLY,
 		.scan_type = IIO_ST('u', 12, 16, 0)
@@ -761,7 +774,8 @@ static struct iio_chan_spec adis16350_channels[] = {
 		.type = IIO_ANGL_VEL,
 		.modified = 1,
 		.channel2 = IIO_MOD_X,
-		.info_mask = IIO_CHAN_INFO_CALIBBIAS_SEPARATE_BIT |
+		.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT |
+		IIO_CHAN_INFO_CALIBBIAS_SEPARATE_BIT |
 		IIO_CHAN_INFO_SCALE_SHARED_BIT |
 		IIO_CHAN_INFO_LOW_PASS_FILTER_3DB_FREQUENCY_SHARED_BIT,
 		.address = gyro_x,
@@ -771,7 +785,8 @@ static struct iio_chan_spec adis16350_channels[] = {
 		.type = IIO_ANGL_VEL,
 		.modified = 1,
 		.channel2 = IIO_MOD_Y,
-		.info_mask = IIO_CHAN_INFO_CALIBBIAS_SEPARATE_BIT |
+		.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT |
+		IIO_CHAN_INFO_CALIBBIAS_SEPARATE_BIT |
 		IIO_CHAN_INFO_SCALE_SHARED_BIT |
 		IIO_CHAN_INFO_LOW_PASS_FILTER_3DB_FREQUENCY_SHARED_BIT,
 		.address = gyro_y,
@@ -781,17 +796,19 @@ static struct iio_chan_spec adis16350_channels[] = {
 		.type = IIO_ANGL_VEL,
 		.modified = 1,
 		.channel2 = IIO_MOD_Z,
-		.info_mask = IIO_CHAN_INFO_CALIBBIAS_SEPARATE_BIT |
+		.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT |
+		IIO_CHAN_INFO_CALIBBIAS_SEPARATE_BIT |
 		IIO_CHAN_INFO_SCALE_SHARED_BIT |
 		IIO_CHAN_INFO_LOW_PASS_FILTER_3DB_FREQUENCY_SHARED_BIT,
 		.address = gyro_z,
 		.scan_index = ADIS16400_SCAN_GYRO_Z,
 		.scan_type = IIO_ST('s', 14, 16, 0),
 	}, {
-	.type = IIO_ACCEL,
+		.type = IIO_ACCEL,
 		.modified = 1,
 		.channel2 = IIO_MOD_X,
-		.info_mask = IIO_CHAN_INFO_CALIBBIAS_SEPARATE_BIT |
+		.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT |
+		IIO_CHAN_INFO_CALIBBIAS_SEPARATE_BIT |
 		IIO_CHAN_INFO_SCALE_SHARED_BIT |
 		IIO_CHAN_INFO_LOW_PASS_FILTER_3DB_FREQUENCY_SHARED_BIT,
 		.address = accel_x,
@@ -801,7 +818,8 @@ static struct iio_chan_spec adis16350_channels[] = {
 		.type = IIO_ACCEL,
 		.modified = 1,
 		.channel2 = IIO_MOD_Y,
-		.info_mask = IIO_CHAN_INFO_CALIBBIAS_SEPARATE_BIT |
+		.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT |
+		IIO_CHAN_INFO_CALIBBIAS_SEPARATE_BIT |
 		IIO_CHAN_INFO_SCALE_SHARED_BIT |
 		IIO_CHAN_INFO_LOW_PASS_FILTER_3DB_FREQUENCY_SHARED_BIT,
 		.address = accel_y,
@@ -811,7 +829,8 @@ static struct iio_chan_spec adis16350_channels[] = {
 		.type = IIO_ACCEL,
 		.modified = 1,
 		.channel2 = IIO_MOD_Z,
-		.info_mask = IIO_CHAN_INFO_CALIBBIAS_SEPARATE_BIT |
+		.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT |
+		IIO_CHAN_INFO_CALIBBIAS_SEPARATE_BIT |
 		IIO_CHAN_INFO_SCALE_SHARED_BIT |
 		IIO_CHAN_INFO_LOW_PASS_FILTER_3DB_FREQUENCY_SHARED_BIT,
 		.address = accel_z,
@@ -822,7 +841,8 @@ static struct iio_chan_spec adis16350_channels[] = {
 		.indexed = 1,
 		.channel = 0,
 		.extend_name = "x",
-		.info_mask = IIO_CHAN_INFO_OFFSET_SEPARATE_BIT |
+		.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT |
+		IIO_CHAN_INFO_OFFSET_SEPARATE_BIT |
 		IIO_CHAN_INFO_SCALE_SEPARATE_BIT |
 		IIO_CHAN_INFO_LOW_PASS_FILTER_3DB_FREQUENCY_SHARED_BIT,
 		.address = temp0,
@@ -833,7 +853,8 @@ static struct iio_chan_spec adis16350_channels[] = {
 		.indexed = 1,
 		.channel = 1,
 		.extend_name = "y",
-		.info_mask = IIO_CHAN_INFO_OFFSET_SEPARATE_BIT |
+		.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT |
+		IIO_CHAN_INFO_OFFSET_SEPARATE_BIT |
 		IIO_CHAN_INFO_SCALE_SEPARATE_BIT |
 		IIO_CHAN_INFO_LOW_PASS_FILTER_3DB_FREQUENCY_SHARED_BIT,
 		.address = temp1,
@@ -844,7 +865,8 @@ static struct iio_chan_spec adis16350_channels[] = {
 		.indexed = 1,
 		.channel = 2,
 		.extend_name = "z",
-		.info_mask = IIO_CHAN_INFO_OFFSET_SEPARATE_BIT |
+		.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT |
+		IIO_CHAN_INFO_OFFSET_SEPARATE_BIT |
 		IIO_CHAN_INFO_SCALE_SEPARATE_BIT,
 		.address = temp2,
 		.scan_index = ADIS16350_SCAN_TEMP_Z,
@@ -853,7 +875,8 @@ static struct iio_chan_spec adis16350_channels[] = {
 		.type = IIO_VOLTAGE,
 		.indexed = 1,
 		.channel = 1,
-		.info_mask = IIO_CHAN_INFO_SCALE_SEPARATE_BIT,
+		.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT |
+		IIO_CHAN_INFO_SCALE_SEPARATE_BIT,
 		.address = in1,
 		.scan_index = ADIS16350_SCAN_ADC_0,
 		.scan_type = IIO_ST('s', 12, 16, 0),
@@ -867,7 +890,8 @@ static struct iio_chan_spec adis16300_channels[] = {
 		.indexed = 1,
 		.channel = 0,
 		.extend_name = "supply",
-		.info_mask = IIO_CHAN_INFO_SCALE_SEPARATE_BIT,
+		.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT |
+		IIO_CHAN_INFO_SCALE_SEPARATE_BIT,
 		.address = in_supply,
 		.scan_index = ADIS16400_SCAN_SUPPLY,
 		.scan_type = IIO_ST('u', 12, 16, 0)
@@ -875,7 +899,8 @@ static struct iio_chan_spec adis16300_channels[] = {
 		.type = IIO_ANGL_VEL,
 		.modified = 1,
 		.channel2 = IIO_MOD_X,
-		.info_mask = IIO_CHAN_INFO_CALIBBIAS_SEPARATE_BIT |
+		.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT |
+		IIO_CHAN_INFO_CALIBBIAS_SEPARATE_BIT |
 		IIO_CHAN_INFO_SCALE_SHARED_BIT |
 		IIO_CHAN_INFO_LOW_PASS_FILTER_3DB_FREQUENCY_SHARED_BIT,
 		.address = gyro_x,
@@ -885,7 +910,8 @@ static struct iio_chan_spec adis16300_channels[] = {
 		.type = IIO_ACCEL,
 		.modified = 1,
 		.channel2 = IIO_MOD_X,
-		.info_mask = IIO_CHAN_INFO_CALIBBIAS_SEPARATE_BIT |
+		.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT |
+		IIO_CHAN_INFO_CALIBBIAS_SEPARATE_BIT |
 		IIO_CHAN_INFO_SCALE_SHARED_BIT |
 		IIO_CHAN_INFO_LOW_PASS_FILTER_3DB_FREQUENCY_SHARED_BIT,
 		.address = accel_x,
@@ -895,7 +921,8 @@ static struct iio_chan_spec adis16300_channels[] = {
 		.type = IIO_ACCEL,
 		.modified = 1,
 		.channel2 = IIO_MOD_Y,
-		.info_mask = IIO_CHAN_INFO_CALIBBIAS_SEPARATE_BIT |
+		.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT |
+		IIO_CHAN_INFO_CALIBBIAS_SEPARATE_BIT |
 		IIO_CHAN_INFO_SCALE_SHARED_BIT |
 		IIO_CHAN_INFO_LOW_PASS_FILTER_3DB_FREQUENCY_SHARED_BIT,
 		.address = accel_y,
@@ -905,7 +932,8 @@ static struct iio_chan_spec adis16300_channels[] = {
 		.type = IIO_ACCEL,
 		.modified = 1,
 		.channel2 = IIO_MOD_Z,
-		.info_mask = IIO_CHAN_INFO_CALIBBIAS_SEPARATE_BIT |
+		.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT |
+		IIO_CHAN_INFO_CALIBBIAS_SEPARATE_BIT |
 		IIO_CHAN_INFO_SCALE_SHARED_BIT |
 		IIO_CHAN_INFO_LOW_PASS_FILTER_3DB_FREQUENCY_SHARED_BIT,
 		.address = accel_z,
@@ -915,7 +943,8 @@ static struct iio_chan_spec adis16300_channels[] = {
 		.type = IIO_TEMP,
 		.indexed = 1,
 		.channel = 0,
-		.info_mask = IIO_CHAN_INFO_OFFSET_SEPARATE_BIT |
+		.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT |
+		IIO_CHAN_INFO_OFFSET_SEPARATE_BIT |
 		IIO_CHAN_INFO_SCALE_SEPARATE_BIT,
 		.address = temp,
 		.scan_index = ADIS16400_SCAN_TEMP,
@@ -924,7 +953,8 @@ static struct iio_chan_spec adis16300_channels[] = {
 		.type = IIO_VOLTAGE,
 		.indexed = 1,
 		.channel = 1,
-		.info_mask = IIO_CHAN_INFO_SCALE_SEPARATE_BIT,
+		.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT |
+		IIO_CHAN_INFO_SCALE_SEPARATE_BIT,
 		.address = in1,
 		.scan_index = ADIS16350_SCAN_ADC_0,
 		.scan_type = IIO_ST('s', 12, 16, 0),
@@ -932,7 +962,8 @@ static struct iio_chan_spec adis16300_channels[] = {
 		.type = IIO_INCLI,
 		.modified = 1,
 		.channel2 = IIO_MOD_X,
-		.info_mask = IIO_CHAN_INFO_SCALE_SHARED_BIT,
+		.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT |
+		IIO_CHAN_INFO_SCALE_SHARED_BIT,
 		.address = incli_x,
 		.scan_index = ADIS16300_SCAN_INCLI_X,
 		.scan_type = IIO_ST('s', 13, 16, 0),
@@ -940,7 +971,8 @@ static struct iio_chan_spec adis16300_channels[] = {
 		.type = IIO_INCLI,
 		.modified = 1,
 		.channel2 = IIO_MOD_Y,
-		.info_mask = IIO_CHAN_INFO_SCALE_SHARED_BIT,
+		.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT |
+		IIO_CHAN_INFO_SCALE_SHARED_BIT,
 		.address = incli_y,
 		.scan_index = ADIS16300_SCAN_INCLI_Y,
 		.scan_type = IIO_ST('s', 13, 16, 0),
@@ -953,7 +985,8 @@ static const struct iio_chan_spec adis16334_channels[] = {
 		.type = IIO_ANGL_VEL,
 		.modified = 1,
 		.channel2 = IIO_MOD_X,
-		.info_mask = IIO_CHAN_INFO_CALIBBIAS_SEPARATE_BIT |
+		.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT |
+		IIO_CHAN_INFO_CALIBBIAS_SEPARATE_BIT |
 		IIO_CHAN_INFO_SCALE_SHARED_BIT |
 		IIO_CHAN_INFO_LOW_PASS_FILTER_3DB_FREQUENCY_SHARED_BIT,
 		.address = gyro_x,
@@ -963,7 +996,8 @@ static const struct iio_chan_spec adis16334_channels[] = {
 		.type = IIO_ANGL_VEL,
 		.modified = 1,
 		.channel2 = IIO_MOD_Y,
-		.info_mask = IIO_CHAN_INFO_CALIBBIAS_SEPARATE_BIT |
+		.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT |
+		IIO_CHAN_INFO_CALIBBIAS_SEPARATE_BIT |
 		IIO_CHAN_INFO_SCALE_SHARED_BIT |
 		IIO_CHAN_INFO_LOW_PASS_FILTER_3DB_FREQUENCY_SHARED_BIT,
 		.address = gyro_y,
@@ -973,7 +1007,8 @@ static const struct iio_chan_spec adis16334_channels[] = {
 		.type = IIO_ANGL_VEL,
 		.modified = 1,
 		.channel2 = IIO_MOD_Z,
-		.info_mask = IIO_CHAN_INFO_CALIBBIAS_SEPARATE_BIT |
+		.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT |
+		IIO_CHAN_INFO_CALIBBIAS_SEPARATE_BIT |
 		IIO_CHAN_INFO_SCALE_SHARED_BIT |
 		IIO_CHAN_INFO_LOW_PASS_FILTER_3DB_FREQUENCY_SHARED_BIT,
 		.address = gyro_z,
@@ -983,7 +1018,8 @@ static const struct iio_chan_spec adis16334_channels[] = {
 		.type = IIO_ACCEL,
 		.modified = 1,
 		.channel2 = IIO_MOD_X,
-		.info_mask = IIO_CHAN_INFO_CALIBBIAS_SEPARATE_BIT |
+		.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT |
+		IIO_CHAN_INFO_CALIBBIAS_SEPARATE_BIT |
 		IIO_CHAN_INFO_SCALE_SHARED_BIT |
 		IIO_CHAN_INFO_LOW_PASS_FILTER_3DB_FREQUENCY_SHARED_BIT,
 		.address = accel_x,
@@ -993,7 +1029,8 @@ static const struct iio_chan_spec adis16334_channels[] = {
 		.type = IIO_ACCEL,
 		.modified = 1,
 		.channel2 = IIO_MOD_Y,
-		.info_mask = IIO_CHAN_INFO_CALIBBIAS_SEPARATE_BIT |
+		.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT |
+		IIO_CHAN_INFO_CALIBBIAS_SEPARATE_BIT |
 		IIO_CHAN_INFO_SCALE_SHARED_BIT |
 		IIO_CHAN_INFO_LOW_PASS_FILTER_3DB_FREQUENCY_SHARED_BIT,
 		.address = accel_y,
@@ -1003,7 +1040,8 @@ static const struct iio_chan_spec adis16334_channels[] = {
 		.type = IIO_ACCEL,
 		.modified = 1,
 		.channel2 = IIO_MOD_Z,
-		.info_mask = IIO_CHAN_INFO_CALIBBIAS_SEPARATE_BIT |
+		.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT |
+		IIO_CHAN_INFO_CALIBBIAS_SEPARATE_BIT |
 		IIO_CHAN_INFO_SCALE_SHARED_BIT |
 		IIO_CHAN_INFO_LOW_PASS_FILTER_3DB_FREQUENCY_SHARED_BIT,
 		.address = accel_z,
@@ -1013,7 +1051,8 @@ static const struct iio_chan_spec adis16334_channels[] = {
 		.type = IIO_TEMP,
 		.indexed = 1,
 		.channel = 0,
-		.info_mask = IIO_CHAN_INFO_CALIBBIAS_SEPARATE_BIT |
+		.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT |
+		IIO_CHAN_INFO_CALIBBIAS_SEPARATE_BIT |
 		IIO_CHAN_INFO_SCALE_SHARED_BIT,
 		.address = accel_z,
 		.scan_index = ADIS16400_SCAN_ACC_Z,
@@ -1122,7 +1161,7 @@ static int __devinit adis16400_probe(struct spi_device *spi)
 {
 	int ret;
 	struct adis16400_state *st;
-	struct iio_dev *indio_dev = iio_allocate_device(sizeof(*st));
+	struct iio_dev *indio_dev = iio_device_alloc(sizeof(*st));
 	if (indio_dev == NULL) {
 		ret =  -ENOMEM;
 		goto error_ret;
@@ -1172,14 +1211,14 @@ static int __devinit adis16400_probe(struct spi_device *spi)
 	return 0;
 
 error_remove_trigger:
-	if (indio_dev->modes & INDIO_BUFFER_TRIGGERED)
+	if (spi->irq)
 		adis16400_remove_trigger(indio_dev);
 error_uninitialize_ring:
 	iio_buffer_unregister(indio_dev);
 error_unreg_ring_funcs:
 	adis16400_unconfigure_ring(indio_dev);
 error_free_dev:
-	iio_free_device(indio_dev);
+	iio_device_free(indio_dev);
 error_ret:
 	return ret;
 }
@@ -1198,7 +1237,7 @@ static int adis16400_remove(struct spi_device *spi)
 	adis16400_remove_trigger(indio_dev);
 	iio_buffer_unregister(indio_dev);
 	adis16400_unconfigure_ring(indio_dev);
-	iio_free_device(indio_dev);
+	iio_device_free(indio_dev);
 
 	return 0;
 
diff --git a/drivers/staging/iio/imu/adis16400_ring.c b/drivers/staging/iio/imu/adis16400_ring.c
index 8daa038b23e..809e2c4270d 100644
--- a/drivers/staging/iio/imu/adis16400_ring.c
+++ b/drivers/staging/iio/imu/adis16400_ring.c
@@ -6,20 +6,19 @@
 #include <linux/bitops.h>
 #include <linux/export.h>
 
-#include "../iio.h"
+#include <linux/iio/iio.h>
 #include "../ring_sw.h"
-#include "../trigger_consumer.h"
+#include <linux/iio/trigger_consumer.h>
 #include "adis16400.h"
 
 /**
  * adis16400_spi_read_burst() - read all data registers
- * @dev: device associated with child of actual device (iio_dev or iio_trig)
+ * @indio_dev: the IIO device
  * @rx: somewhere to pass back the value read (min size is 24 bytes)
  **/
-static int adis16400_spi_read_burst(struct device *dev, u8 *rx)
+static int adis16400_spi_read_burst(struct iio_dev *indio_dev, u8 *rx)
 {
 	struct spi_message msg;
-	struct iio_dev *indio_dev = dev_get_drvdata(dev);
 	struct adis16400_state *st = iio_priv(indio_dev);
 	u32 old_speed_hz = st->us->max_speed_hz;
 	int ret;
@@ -71,9 +70,8 @@ static const u16 read_all_tx_array[] = {
 	cpu_to_be16(ADIS16400_READ_REG(ADIS16400_AUX_ADC)),
 };
 
-static int adis16350_spi_read_all(struct device *dev, u8 *rx)
+static int adis16350_spi_read_all(struct iio_dev *indio_dev, u8 *rx)
 {
-	struct iio_dev *indio_dev = dev_get_drvdata(dev);
 	struct adis16400_state *st = iio_priv(indio_dev);
 
 	struct spi_message msg;
@@ -119,12 +117,12 @@ static irqreturn_t adis16400_trigger_handler(int irq, void *p)
 	struct iio_buffer *ring = indio_dev->buffer;
 	int i = 0, j, ret = 0;
 	s16 *data;
-	size_t datasize = ring->access->get_bytes_per_datum(ring);
+
 	/* Asumption that long is enough for maximum channels */
 	unsigned long mask = *indio_dev->active_scan_mask;
 	int scan_count = bitmap_weight(indio_dev->active_scan_mask,
 				       indio_dev->masklength);
-	data = kmalloc(datasize , GFP_KERNEL);
+	data = kmalloc(indio_dev->scan_bytes, GFP_KERNEL);
 	if (data == NULL) {
 		dev_err(&st->us->dev, "memory alloc failed in ring bh");
 		return -ENOMEM;
@@ -132,13 +130,13 @@ static irqreturn_t adis16400_trigger_handler(int irq, void *p)
 
 	if (scan_count) {
 		if (st->variant->flags & ADIS16400_NO_BURST) {
-			ret = adis16350_spi_read_all(&indio_dev->dev, st->rx);
+			ret = adis16350_spi_read_all(indio_dev, st->rx);
 			if (ret < 0)
 				goto err;
 			for (; i < scan_count; i++)
 				data[i]	= *(s16 *)(st->rx + i*2);
 		} else {
-			ret = adis16400_spi_read_burst(&indio_dev->dev, st->rx);
+			ret = adis16400_spi_read_burst(indio_dev, st->rx);
 			if (ret < 0)
 				goto err;
 			for (; i < scan_count; i++) {
diff --git a/drivers/staging/iio/imu/adis16400_trigger.c b/drivers/staging/iio/imu/adis16400_trigger.c
index 5bf00075752..42a678e92fc 100644
--- a/drivers/staging/iio/imu/adis16400_trigger.c
+++ b/drivers/staging/iio/imu/adis16400_trigger.c
@@ -3,8 +3,8 @@
 #include <linux/spi/spi.h>
 #include <linux/export.h>
 
-#include "../iio.h"
-#include "../trigger.h"
+#include <linux/iio/iio.h>
+#include <linux/iio/trigger.h>
 #include "adis16400.h"
 
 /**
@@ -29,7 +29,7 @@ int adis16400_probe_trigger(struct iio_dev *indio_dev)
 	int ret;
 	struct adis16400_state *st = iio_priv(indio_dev);
 
-	st->trig = iio_allocate_trigger("%s-dev%d",
+	st->trig = iio_trigger_alloc("%s-dev%d",
 					indio_dev->name,
 					indio_dev->id);
 	if (st->trig == NULL) {
@@ -59,7 +59,7 @@ int adis16400_probe_trigger(struct iio_dev *indio_dev)
 error_free_irq:
 	free_irq(st->us->irq, st->trig);
 error_free_trig:
-	iio_free_trigger(st->trig);
+	iio_trigger_free(st->trig);
 error_ret:
 	return ret;
 }
@@ -70,5 +70,5 @@ void adis16400_remove_trigger(struct iio_dev *indio_dev)
 
 	iio_trigger_unregister(st->trig);
 	free_irq(st->us->irq, st->trig);
-	iio_free_trigger(st->trig);
+	iio_trigger_free(st->trig);
 }
diff --git a/drivers/staging/iio/kfifo_buf.h b/drivers/staging/iio/kfifo_buf.h
deleted file mode 100644
index 9f7da016af0..00000000000
--- a/drivers/staging/iio/kfifo_buf.h
+++ /dev/null
@@ -1,8 +0,0 @@
-
-#include <linux/kfifo.h>
-#include "iio.h"
-#include "buffer.h"
-
-struct iio_buffer *iio_kfifo_allocate(struct iio_dev *indio_dev);
-void iio_kfifo_free(struct iio_buffer *r);
-
diff --git a/drivers/staging/iio/light/Kconfig b/drivers/staging/iio/light/Kconfig
index e7e9159d989..4bed30eac3e 100644
--- a/drivers/staging/iio/light/Kconfig
+++ b/drivers/staging/iio/light/Kconfig
@@ -4,15 +4,26 @@
 menu "Light sensors"
 
 config SENSORS_ISL29018
-        tristate "ISL 29018 light and proximity sensor"
-        depends on I2C
-        default n
-        help
-         If you say yes here you get support for ambient light sensing and
-         proximity infrared sensing from Intersil ISL29018.
-         This driver will provide the measurements of ambient light intensity
-         in lux, proximity infrared sensing and normal infrared sensing.
-         Data from sensor is accessible via sysfs.
+	tristate "ISL 29018 light and proximity sensor"
+	depends on I2C
+	select REGMAP_I2C
+	default n
+	help
+	 If you say yes here you get support for ambient light sensing and
+	 proximity infrared sensing from Intersil ISL29018.
+	 This driver will provide the measurements of ambient light intensity
+	 in lux, proximity infrared sensing and normal infrared sensing.
+	 Data from sensor is accessible via sysfs.
+
+config SENSORS_ISL29028
+	tristate "Intersil ISL29028 Concurrent Light and Proximity Sensor"
+	depends on I2C
+	select REGMAP_I2C
+	help
+	 Provides driver for the Intersil's ISL29028 device.
+	 This driver supports the sysfs interface to get the ALS, IR intensity,
+	 Proximity value via iio. The ISL29028 provides the concurrent sensing
+	 of ambient light and proximity.
 
 config SENSORS_TSL2563
 	tristate "TAOS TSL2560, TSL2561, TSL2562 and TSL2563 ambient light sensors"
@@ -31,4 +42,12 @@ config TSL2583
 	 Provides support for the TAOS tsl2580, tsl2581 and tsl2583 devices.
 	 Access ALS data via iio, sysfs.
 
+config TSL2x7x
+	tristate "TAOS TSL/TMD2x71 and TSL/TMD2x72 Family of light and proximity sensors"
+	depends on I2C
+	help
+	 Support for: tsl2571, tsl2671, tmd2671, tsl2771, tmd2771, tsl2572, tsl2672,
+	 tmd2672, tsl2772, tmd2772 devices.
+	 Provides iio_events and direct access via sysfs.
+
 endmenu
diff --git a/drivers/staging/iio/light/Makefile b/drivers/staging/iio/light/Makefile
index 3011fbfa8dc..141af1eb164 100644
--- a/drivers/staging/iio/light/Makefile
+++ b/drivers/staging/iio/light/Makefile
@@ -4,4 +4,6 @@
 
 obj-$(CONFIG_SENSORS_TSL2563)	+= tsl2563.o
 obj-$(CONFIG_SENSORS_ISL29018)	+= isl29018.o
+obj-$(CONFIG_SENSORS_ISL29028)	+= isl29028.o
 obj-$(CONFIG_TSL2583)	+= tsl2583.o
+obj-$(CONFIG_TSL2x7x)	+= tsl2x7x_core.o
diff --git a/drivers/staging/iio/light/isl29018.c b/drivers/staging/iio/light/isl29018.c
index 38ec52b65df..0abbf18d610 100644
--- a/drivers/staging/iio/light/isl29018.c
+++ b/drivers/staging/iio/light/isl29018.c
@@ -26,9 +26,11 @@
 #include <linux/err.h>
 #include <linux/mutex.h>
 #include <linux/delay.h>
+#include <linux/regmap.h>
 #include <linux/slab.h>
-#include "../iio.h"
-#include "../sysfs.h"
+#include <linux/iio/iio.h>
+#include <linux/iio/sysfs.h>
+
 #define CONVERSION_TIME_MS		100
 
 #define ISL29018_REG_ADD_COMMAND1	0x00
@@ -51,49 +53,22 @@
 
 #define ISL29018_REG_ADD_DATA_LSB	0x02
 #define ISL29018_REG_ADD_DATA_MSB	0x03
-#define ISL29018_MAX_REGS		(ISL29018_REG_ADD_DATA_MSB+1)
 
 #define ISL29018_REG_TEST		0x08
 #define ISL29018_TEST_SHIFT		0
 #define ISL29018_TEST_MASK		(0xFF << ISL29018_TEST_SHIFT)
 
 struct isl29018_chip {
-	struct i2c_client	*client;
+	struct device		*dev;
+	struct regmap		*regmap;
 	struct mutex		lock;
 	unsigned int		lux_scale;
 	unsigned int		range;
 	unsigned int		adc_bit;
 	int			prox_scheme;
-	u8			reg_cache[ISL29018_MAX_REGS];
 };
 
-static int isl29018_write_data(struct i2c_client *client, u8 reg,
-			u8 val, u8 mask, u8 shift)
-{
-	u8 regval = val;
-	int ret;
-	struct isl29018_chip *chip = iio_priv(i2c_get_clientdata(client));
-
-	/* don't cache or mask REG_TEST */
-	if (reg < ISL29018_MAX_REGS) {
-		regval = chip->reg_cache[reg];
-		regval &= ~mask;
-		regval |= val << shift;
-	}
-
-	ret = i2c_smbus_write_byte_data(client, reg, regval);
-	if (ret) {
-		dev_err(&client->dev, "Write to device fails status %x\n", ret);
-	} else {
-		/* don't update cache on err */
-		if (reg < ISL29018_MAX_REGS)
-			chip->reg_cache[reg] = regval;
-	}
-
-	return ret;
-}
-
-static int isl29018_set_range(struct i2c_client *client, unsigned long range,
+static int isl29018_set_range(struct isl29018_chip *chip, unsigned long range,
 		unsigned int *new_range)
 {
 	static const unsigned long supp_ranges[] = {1000, 4000, 16000, 64000};
@@ -109,11 +84,11 @@ static int isl29018_set_range(struct i2c_client *client, unsigned long range,
 	if (i >= ARRAY_SIZE(supp_ranges))
 		return -EINVAL;
 
-	return isl29018_write_data(client, ISL29018_REG_ADD_COMMANDII,
-			i, COMMANDII_RANGE_MASK, COMMANDII_RANGE_SHIFT);
+	return regmap_update_bits(chip->regmap, ISL29018_REG_ADD_COMMANDII,
+			COMMANDII_RANGE_MASK, i << COMMANDII_RANGE_SHIFT);
 }
 
-static int isl29018_set_resolution(struct i2c_client *client,
+static int isl29018_set_resolution(struct isl29018_chip *chip,
 			unsigned long adcbit, unsigned int *conf_adc_bit)
 {
 	static const unsigned long supp_adcbit[] = {16, 12, 8, 4};
@@ -129,48 +104,49 @@ static int isl29018_set_resolution(struct i2c_client *client,
 	if (i >= ARRAY_SIZE(supp_adcbit))
 		return -EINVAL;
 
-	return isl29018_write_data(client, ISL29018_REG_ADD_COMMANDII,
-			i, COMMANDII_RESOLUTION_MASK,
-			COMMANDII_RESOLUTION_SHIFT);
+	return regmap_update_bits(chip->regmap, ISL29018_REG_ADD_COMMANDII,
+			COMMANDII_RESOLUTION_MASK,
+			i << COMMANDII_RESOLUTION_SHIFT);
 }
 
-static int isl29018_read_sensor_input(struct i2c_client *client, int mode)
+static int isl29018_read_sensor_input(struct isl29018_chip *chip, int mode)
 {
 	int status;
-	int lsb;
-	int msb;
+	unsigned int lsb;
+	unsigned int msb;
 
 	/* Set mode */
-	status = isl29018_write_data(client, ISL29018_REG_ADD_COMMAND1,
-			mode, COMMMAND1_OPMODE_MASK, COMMMAND1_OPMODE_SHIFT);
+	status = regmap_write(chip->regmap, ISL29018_REG_ADD_COMMAND1,
+			mode << COMMMAND1_OPMODE_SHIFT);
 	if (status) {
-		dev_err(&client->dev, "Error in setting operating mode\n");
+		dev_err(chip->dev,
+			"Error in setting operating mode err %d\n", status);
 		return status;
 	}
 	msleep(CONVERSION_TIME_MS);
-	lsb = i2c_smbus_read_byte_data(client, ISL29018_REG_ADD_DATA_LSB);
-	if (lsb < 0) {
-		dev_err(&client->dev, "Error in reading LSB DATA\n");
-		return lsb;
+	status = regmap_read(chip->regmap, ISL29018_REG_ADD_DATA_LSB, &lsb);
+	if (status < 0) {
+		dev_err(chip->dev,
+			"Error in reading LSB DATA with err %d\n", status);
+		return status;
 	}
 
-	msb = i2c_smbus_read_byte_data(client, ISL29018_REG_ADD_DATA_MSB);
-	if (msb < 0) {
-		dev_err(&client->dev, "Error in reading MSB DATA\n");
-		return msb;
+	status = regmap_read(chip->regmap, ISL29018_REG_ADD_DATA_MSB, &msb);
+	if (status < 0) {
+		dev_err(chip->dev,
+			"Error in reading MSB DATA with error %d\n", status);
+		return status;
 	}
-	dev_vdbg(&client->dev, "MSB 0x%x and LSB 0x%x\n", msb, lsb);
+	dev_vdbg(chip->dev, "MSB 0x%x and LSB 0x%x\n", msb, lsb);
 
 	return (msb << 8) | lsb;
 }
 
-static int isl29018_read_lux(struct i2c_client *client, int *lux)
+static int isl29018_read_lux(struct isl29018_chip *chip, int *lux)
 {
 	int lux_data;
-	struct isl29018_chip *chip = iio_priv(i2c_get_clientdata(client));
 
-	lux_data = isl29018_read_sensor_input(client,
-				COMMMAND1_OPMODE_ALS_ONCE);
+	lux_data = isl29018_read_sensor_input(chip, COMMMAND1_OPMODE_ALS_ONCE);
 
 	if (lux_data < 0)
 		return lux_data;
@@ -180,11 +156,11 @@ static int isl29018_read_lux(struct i2c_client *client, int *lux)
 	return 0;
 }
 
-static int isl29018_read_ir(struct i2c_client *client, int *ir)
+static int isl29018_read_ir(struct isl29018_chip *chip, int *ir)
 {
 	int ir_data;
 
-	ir_data = isl29018_read_sensor_input(client, COMMMAND1_OPMODE_IR_ONCE);
+	ir_data = isl29018_read_sensor_input(chip, COMMMAND1_OPMODE_IR_ONCE);
 
 	if (ir_data < 0)
 		return ir_data;
@@ -194,7 +170,7 @@ static int isl29018_read_ir(struct i2c_client *client, int *ir)
 	return 0;
 }
 
-static int isl29018_read_proximity_ir(struct i2c_client *client, int scheme,
+static int isl29018_read_proximity_ir(struct isl29018_chip *chip, int scheme,
 		int *near_ir)
 {
 	int status;
@@ -202,14 +178,15 @@ static int isl29018_read_proximity_ir(struct i2c_client *client, int scheme,
 	int ir_data = -1;
 
 	/* Do proximity sensing with required scheme */
-	status = isl29018_write_data(client, ISL29018_REG_ADD_COMMANDII,
-			scheme, COMMANDII_SCHEME_MASK, COMMANDII_SCHEME_SHIFT);
+	status = regmap_update_bits(chip->regmap, ISL29018_REG_ADD_COMMANDII,
+			COMMANDII_SCHEME_MASK,
+			scheme << COMMANDII_SCHEME_SHIFT);
 	if (status) {
-		dev_err(&client->dev, "Error in setting operating mode\n");
+		dev_err(chip->dev, "Error in setting operating mode\n");
 		return status;
 	}
 
-	prox_data = isl29018_read_sensor_input(client,
+	prox_data = isl29018_read_sensor_input(chip,
 					COMMMAND1_OPMODE_PROX_ONCE);
 	if (prox_data < 0)
 		return prox_data;
@@ -219,8 +196,7 @@ static int isl29018_read_proximity_ir(struct i2c_client *client, int scheme,
 		return 0;
 	}
 
-	ir_data = isl29018_read_sensor_input(client,
-				COMMMAND1_OPMODE_IR_ONCE);
+	ir_data = isl29018_read_sensor_input(chip, COMMMAND1_OPMODE_IR_ONCE);
 
 	if (ir_data < 0)
 		return ir_data;
@@ -238,7 +214,7 @@ static int isl29018_read_proximity_ir(struct i2c_client *client, int scheme,
 static ssize_t show_range(struct device *dev,
 			struct device_attribute *attr, char *buf)
 {
-	struct iio_dev *indio_dev = dev_get_drvdata(dev);
+	struct iio_dev *indio_dev = dev_to_iio_dev(dev);
 	struct isl29018_chip *chip = iio_priv(indio_dev);
 
 	return sprintf(buf, "%u\n", chip->range);
@@ -247,9 +223,8 @@ static ssize_t show_range(struct device *dev,
 static ssize_t store_range(struct device *dev,
 		struct device_attribute *attr, const char *buf, size_t count)
 {
-	struct iio_dev *indio_dev = dev_get_drvdata(dev);
+	struct iio_dev *indio_dev = dev_to_iio_dev(dev);
 	struct isl29018_chip *chip = iio_priv(indio_dev);
-	struct i2c_client *client = chip->client;
 	int status;
 	unsigned long lval;
 	unsigned int new_range;
@@ -264,10 +239,11 @@ static ssize_t store_range(struct device *dev,
 	}
 
 	mutex_lock(&chip->lock);
-	status = isl29018_set_range(client, lval, &new_range);
+	status = isl29018_set_range(chip, lval, &new_range);
 	if (status < 0) {
 		mutex_unlock(&chip->lock);
-		dev_err(dev, "Error in setting max range\n");
+		dev_err(dev,
+			"Error in setting max range with err %d\n", status);
 		return status;
 	}
 	chip->range = new_range;
@@ -280,7 +256,7 @@ static ssize_t store_range(struct device *dev,
 static ssize_t show_resolution(struct device *dev,
 			struct device_attribute *attr, char *buf)
 {
-	struct iio_dev *indio_dev = dev_get_drvdata(dev);
+	struct iio_dev *indio_dev = dev_to_iio_dev(dev);
 	struct isl29018_chip *chip = iio_priv(indio_dev);
 
 	return sprintf(buf, "%u\n", chip->adc_bit);
@@ -289,9 +265,8 @@ static ssize_t show_resolution(struct device *dev,
 static ssize_t store_resolution(struct device *dev,
 		struct device_attribute *attr, const char *buf, size_t count)
 {
-	struct iio_dev *indio_dev = dev_get_drvdata(dev);
+	struct iio_dev *indio_dev = dev_to_iio_dev(dev);
 	struct isl29018_chip *chip = iio_priv(indio_dev);
-	struct i2c_client *client = chip->client;
 	int status;
 	unsigned long lval;
 	unsigned int new_adc_bit;
@@ -304,7 +279,7 @@ static ssize_t store_resolution(struct device *dev,
 	}
 
 	mutex_lock(&chip->lock);
-	status = isl29018_set_resolution(client, lval, &new_adc_bit);
+	status = isl29018_set_resolution(chip, lval, &new_adc_bit);
 	if (status < 0) {
 		mutex_unlock(&chip->lock);
 		dev_err(dev, "Error in setting resolution\n");
@@ -320,7 +295,7 @@ static ssize_t store_resolution(struct device *dev,
 static ssize_t show_prox_infrared_supression(struct device *dev,
 			struct device_attribute *attr, char *buf)
 {
-	struct iio_dev *indio_dev = dev_get_drvdata(dev);
+	struct iio_dev *indio_dev = dev_to_iio_dev(dev);
 	struct isl29018_chip *chip = iio_priv(indio_dev);
 
 	/* return the "proximity scheme" i.e. if the chip does on chip
@@ -331,7 +306,7 @@ static ssize_t show_prox_infrared_supression(struct device *dev,
 static ssize_t store_prox_infrared_supression(struct device *dev,
 		struct device_attribute *attr, const char *buf, size_t count)
 {
-	struct iio_dev *indio_dev = dev_get_drvdata(dev);
+	struct iio_dev *indio_dev = dev_to_iio_dev(dev);
 	struct isl29018_chip *chip = iio_priv(indio_dev);
 	unsigned long lval;
 
@@ -379,20 +354,20 @@ static int isl29018_read_raw(struct iio_dev *indio_dev,
 {
 	int ret = -EINVAL;
 	struct isl29018_chip *chip = iio_priv(indio_dev);
-	struct i2c_client *client = chip->client;
 
 	mutex_lock(&chip->lock);
 	switch (mask) {
-	case 0:
+	case IIO_CHAN_INFO_RAW:
+	case IIO_CHAN_INFO_PROCESSED:
 		switch (chan->type) {
 		case IIO_LIGHT:
-			ret = isl29018_read_lux(client, val);
+			ret = isl29018_read_lux(chip, val);
 			break;
 		case IIO_INTENSITY:
-			ret = isl29018_read_ir(client, val);
+			ret = isl29018_read_ir(chip, val);
 			break;
 		case IIO_PROXIMITY:
-			ret = isl29018_read_proximity_ir(client,
+			ret = isl29018_read_proximity_ir(chip,
 					chip->prox_scheme, val);
 			break;
 		default:
@@ -419,15 +394,17 @@ static const struct iio_chan_spec isl29018_channels[] = {
 		.type = IIO_LIGHT,
 		.indexed = 1,
 		.channel = 0,
-		.processed_val = IIO_PROCESSED,
-		.info_mask = IIO_CHAN_INFO_CALIBSCALE_SEPARATE_BIT,
+		.info_mask = IIO_CHAN_INFO_PROCESSED_SEPARATE_BIT |
+		IIO_CHAN_INFO_CALIBSCALE_SEPARATE_BIT,
 	}, {
 		.type = IIO_INTENSITY,
 		.modified = 1,
+		.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT,
 		.channel2 = IIO_MOD_LIGHT_IR,
 	}, {
 		/* Unindexed in current ABI.  But perhaps it should be. */
 		.type = IIO_PROXIMITY,
+		.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT,
 	}
 };
 
@@ -456,15 +433,12 @@ static const struct attribute_group isl29108_group = {
 	.attrs = isl29018_attributes,
 };
 
-static int isl29018_chip_init(struct i2c_client *client)
+static int isl29018_chip_init(struct isl29018_chip *chip)
 {
-	struct isl29018_chip *chip = iio_priv(i2c_get_clientdata(client));
 	int status;
 	int new_adc_bit;
 	unsigned int new_range;
 
-	memset(chip->reg_cache, 0, sizeof(chip->reg_cache));
-
 	/* Code added per Intersil Application Note 1534:
 	 *     When VDD sinks to approximately 1.8V or below, some of
 	 * the part's registers may change their state. When VDD
@@ -485,10 +459,9 @@ static int isl29018_chip_init(struct i2c_client *client)
 	 * the same thing EXCEPT the data sheet asks for a 1ms delay after
 	 * writing the CMD1 register.
 	 */
-	status = isl29018_write_data(client, ISL29018_REG_TEST, 0,
-				ISL29018_TEST_MASK, ISL29018_TEST_SHIFT);
+	status = regmap_write(chip->regmap, ISL29018_REG_TEST, 0x0);
 	if (status < 0) {
-		dev_err(&client->dev, "Failed to clear isl29018 TEST reg."
+		dev_err(chip->dev, "Failed to clear isl29018 TEST reg."
 					"(%d)\n", status);
 		return status;
 	}
@@ -497,10 +470,9 @@ static int isl29018_chip_init(struct i2c_client *client)
 	 * "Operating Mode" (COMMAND1) register is reprogrammed when
 	 * data is read from the device.
 	 */
-	status = isl29018_write_data(client, ISL29018_REG_ADD_COMMAND1, 0,
-				0xff, 0);
+	status = regmap_write(chip->regmap, ISL29018_REG_ADD_COMMAND1, 0);
 	if (status < 0) {
-		dev_err(&client->dev, "Failed to clear isl29018 CMD1 reg."
+		dev_err(chip->dev, "Failed to clear isl29018 CMD1 reg."
 					"(%d)\n", status);
 		return status;
 	}
@@ -508,13 +480,13 @@ static int isl29018_chip_init(struct i2c_client *client)
 	msleep(1);	/* per data sheet, page 10 */
 
 	/* set defaults */
-	status = isl29018_set_range(client, chip->range, &new_range);
+	status = isl29018_set_range(chip, chip->range, &new_range);
 	if (status < 0) {
-		dev_err(&client->dev, "Init of isl29018 fails\n");
+		dev_err(chip->dev, "Init of isl29018 fails\n");
 		return status;
 	}
 
-	status = isl29018_set_resolution(client, chip->adc_bit,
+	status = isl29018_set_resolution(chip, chip->adc_bit,
 						&new_adc_bit);
 
 	return 0;
@@ -527,6 +499,32 @@ static const struct iio_info isl29108_info = {
 	.write_raw = &isl29018_write_raw,
 };
 
+static bool is_volatile_reg(struct device *dev, unsigned int reg)
+{
+	switch (reg) {
+	case ISL29018_REG_ADD_DATA_LSB:
+	case ISL29018_REG_ADD_DATA_MSB:
+	case ISL29018_REG_ADD_COMMAND1:
+	case ISL29018_REG_TEST:
+		return true;
+	default:
+		return false;
+	}
+}
+
+/*
+ * isl29018_regmap_config: regmap configuration.
+ * Use RBTREE mechanism for caching.
+ */
+static const struct regmap_config isl29018_regmap_config = {
+	.reg_bits = 8,
+	.val_bits = 8,
+	.volatile_reg = is_volatile_reg,
+	.max_register = ISL29018_REG_TEST,
+	.num_reg_defaults_raw = ISL29018_REG_TEST + 1,
+	.cache_type = REGCACHE_RBTREE,
+};
+
 static int __devinit isl29018_probe(struct i2c_client *client,
 			 const struct i2c_device_id *id)
 {
@@ -534,7 +532,7 @@ static int __devinit isl29018_probe(struct i2c_client *client,
 	struct iio_dev *indio_dev;
 	int err;
 
-	indio_dev = iio_allocate_device(sizeof(*chip));
+	indio_dev = iio_device_alloc(sizeof(*chip));
 	if (indio_dev == NULL) {
 		dev_err(&client->dev, "iio allocation fails\n");
 		err = -ENOMEM;
@@ -543,7 +541,7 @@ static int __devinit isl29018_probe(struct i2c_client *client,
 	chip = iio_priv(indio_dev);
 
 	i2c_set_clientdata(client, indio_dev);
-	chip->client = client;
+	chip->dev = &client->dev;
 
 	mutex_init(&chip->lock);
 
@@ -551,7 +549,14 @@ static int __devinit isl29018_probe(struct i2c_client *client,
 	chip->range = 1000;
 	chip->adc_bit = 16;
 
-	err = isl29018_chip_init(client);
+	chip->regmap = devm_regmap_init_i2c(client, &isl29018_regmap_config);
+	if (IS_ERR(chip->regmap)) {
+		err = PTR_ERR(chip->regmap);
+		dev_err(chip->dev, "regmap initialization failed: %d\n", err);
+		goto exit;
+	}
+
+	err = isl29018_chip_init(chip);
 	if (err)
 		goto exit_iio_free;
 
@@ -569,7 +574,7 @@ static int __devinit isl29018_probe(struct i2c_client *client,
 
 	return 0;
 exit_iio_free:
-	iio_free_device(indio_dev);
+	iio_device_free(indio_dev);
 exit:
 	return err;
 }
@@ -580,7 +585,7 @@ static int __devexit isl29018_remove(struct i2c_client *client)
 
 	dev_dbg(&client->dev, "%s()\n", __func__);
 	iio_device_unregister(indio_dev);
-	iio_free_device(indio_dev);
+	iio_device_free(indio_dev);
 
 	return 0;
 }
@@ -593,7 +598,7 @@ static const struct i2c_device_id isl29018_id[] = {
 MODULE_DEVICE_TABLE(i2c, isl29018_id);
 
 static const struct of_device_id isl29018_of_match[] = {
-	{ .compatible = "invn,isl29018", },
+	{ .compatible = "isil,isl29018", },
 	{ },
 };
 MODULE_DEVICE_TABLE(of, isl29018_of_match);
diff --git a/drivers/staging/iio/light/isl29028.c b/drivers/staging/iio/light/isl29028.c
new file mode 100644
index 00000000000..33a4c3f94a1
--- /dev/null
+++ b/drivers/staging/iio/light/isl29028.c
@@ -0,0 +1,566 @@
+/*
+ * IIO driver for the light sensor ISL29028.
+ * ISL29028 is Concurrent Ambient Light and Proximity Sensor
+ *
+ * Copyright (c) 2012, 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/module.h>
+#include <linux/i2c.h>
+#include <linux/err.h>
+#include <linux/mutex.h>
+#include <linux/delay.h>
+#include <linux/slab.h>
+#include <linux/regmap.h>
+#include <linux/iio/iio.h>
+#include <linux/iio/sysfs.h>
+
+#define CONVERSION_TIME_MS		100
+
+#define ISL29028_REG_CONFIGURE		0x01
+
+#define CONFIGURE_ALS_IR_MODE_ALS	0
+#define CONFIGURE_ALS_IR_MODE_IR	BIT(0)
+#define CONFIGURE_ALS_IR_MODE_MASK	BIT(0)
+
+#define CONFIGURE_ALS_RANGE_LOW_LUX	0
+#define CONFIGURE_ALS_RANGE_HIGH_LUX	BIT(1)
+#define CONFIGURE_ALS_RANGE_MASK	BIT(1)
+
+#define CONFIGURE_ALS_DIS		0
+#define CONFIGURE_ALS_EN		BIT(2)
+#define CONFIGURE_ALS_EN_MASK		BIT(2)
+
+#define CONFIGURE_PROX_DRIVE		BIT(3)
+
+#define CONFIGURE_PROX_SLP_SH		4
+#define CONFIGURE_PROX_SLP_MASK		(7 << CONFIGURE_PROX_SLP_SH)
+
+#define CONFIGURE_PROX_EN		BIT(7)
+#define CONFIGURE_PROX_EN_MASK		BIT(7)
+
+#define ISL29028_REG_INTERRUPT		0x02
+
+#define ISL29028_REG_PROX_DATA		0x08
+#define ISL29028_REG_ALSIR_L		0x09
+#define ISL29028_REG_ALSIR_U		0x0A
+
+#define ISL29028_REG_TEST1_MODE		0x0E
+#define ISL29028_REG_TEST2_MODE		0x0F
+
+#define ISL29028_NUM_REGS		(ISL29028_REG_TEST2_MODE + 1)
+
+enum als_ir_mode {
+	MODE_NONE = 0,
+	MODE_ALS,
+	MODE_IR
+};
+
+struct isl29028_chip {
+	struct device		*dev;
+	struct mutex		lock;
+	struct regmap		*regmap;
+
+	unsigned int		prox_sampling;
+	bool			enable_prox;
+
+	int			lux_scale;
+	int			als_ir_mode;
+};
+
+static int isl29028_set_proxim_sampling(struct isl29028_chip *chip,
+			unsigned int sampling)
+{
+	static unsigned int prox_period[] = {800, 400, 200, 100, 75, 50, 12, 0};
+	int sel;
+	unsigned int period = DIV_ROUND_UP(1000, sampling);
+
+	for (sel = 0; sel < ARRAY_SIZE(prox_period); ++sel) {
+		if (period >= prox_period[sel])
+			break;
+	}
+	return regmap_update_bits(chip->regmap, ISL29028_REG_CONFIGURE,
+			CONFIGURE_PROX_SLP_MASK, sel << CONFIGURE_PROX_SLP_SH);
+}
+
+static int isl29028_enable_proximity(struct isl29028_chip *chip, bool enable)
+{
+	int ret;
+	int val = 0;
+
+	if (enable)
+		val = CONFIGURE_PROX_EN;
+	ret = regmap_update_bits(chip->regmap, ISL29028_REG_CONFIGURE,
+			CONFIGURE_PROX_EN_MASK, val);
+	if (ret < 0)
+		return ret;
+
+	/* Wait for conversion to be complete for first sample */
+	mdelay(DIV_ROUND_UP(1000, chip->prox_sampling));
+	return 0;
+}
+
+static int isl29028_set_als_scale(struct isl29028_chip *chip, int lux_scale)
+{
+	int val = (lux_scale == 2000) ? CONFIGURE_ALS_RANGE_HIGH_LUX :
+					CONFIGURE_ALS_RANGE_LOW_LUX;
+
+	return regmap_update_bits(chip->regmap, ISL29028_REG_CONFIGURE,
+		CONFIGURE_ALS_RANGE_MASK, val);
+}
+
+static int isl29028_set_als_ir_mode(struct isl29028_chip *chip,
+	enum als_ir_mode mode)
+{
+	int ret = 0;
+
+	switch (mode) {
+	case MODE_ALS:
+		ret = regmap_update_bits(chip->regmap, ISL29028_REG_CONFIGURE,
+			CONFIGURE_ALS_IR_MODE_MASK, CONFIGURE_ALS_IR_MODE_ALS);
+		if (ret < 0)
+			return ret;
+
+		ret = regmap_update_bits(chip->regmap, ISL29028_REG_CONFIGURE,
+			CONFIGURE_ALS_RANGE_MASK, CONFIGURE_ALS_RANGE_HIGH_LUX);
+		break;
+
+	case MODE_IR:
+		ret = regmap_update_bits(chip->regmap, ISL29028_REG_CONFIGURE,
+			CONFIGURE_ALS_IR_MODE_MASK, CONFIGURE_ALS_IR_MODE_IR);
+		break;
+
+	case MODE_NONE:
+		return regmap_update_bits(chip->regmap, ISL29028_REG_CONFIGURE,
+			CONFIGURE_ALS_EN_MASK, CONFIGURE_ALS_DIS);
+	}
+
+	if (ret < 0)
+		return ret;
+
+	/* Enable the ALS/IR */
+	ret = regmap_update_bits(chip->regmap, ISL29028_REG_CONFIGURE,
+			CONFIGURE_ALS_EN_MASK, CONFIGURE_ALS_EN);
+	if (ret < 0)
+		return ret;
+
+	/* Need to wait for conversion time if ALS/IR mode enabled */
+	mdelay(CONVERSION_TIME_MS);
+	return 0;
+}
+
+static int isl29028_read_als_ir(struct isl29028_chip *chip, int *als_ir)
+{
+	unsigned int lsb;
+	unsigned int msb;
+	int ret;
+
+	ret = regmap_read(chip->regmap, ISL29028_REG_ALSIR_L, &lsb);
+	if (ret < 0) {
+		dev_err(chip->dev,
+			"Error in reading register ALSIR_L err %d\n", ret);
+		return ret;
+	}
+
+	ret = regmap_read(chip->regmap, ISL29028_REG_ALSIR_U, &msb);
+	if (ret < 0) {
+		dev_err(chip->dev,
+			"Error in reading register ALSIR_U err %d\n", ret);
+		return ret;
+	}
+
+	*als_ir = ((msb & 0xF) << 8) | (lsb & 0xFF);
+	return 0;
+}
+
+static int isl29028_read_proxim(struct isl29028_chip *chip, int *prox)
+{
+	unsigned int data;
+	int ret;
+
+	ret = regmap_read(chip->regmap, ISL29028_REG_PROX_DATA, &data);
+	if (ret < 0) {
+		dev_err(chip->dev, "Error in reading register %d, error %d\n",
+				ISL29028_REG_PROX_DATA, ret);
+		return ret;
+	}
+	*prox = data;
+	return 0;
+}
+
+static int isl29028_proxim_get(struct isl29028_chip *chip, int *prox_data)
+{
+	int ret;
+
+	if (!chip->enable_prox) {
+		ret = isl29028_enable_proximity(chip, true);
+		if (ret < 0)
+			return ret;
+		chip->enable_prox = true;
+	}
+	return isl29028_read_proxim(chip, prox_data);
+}
+
+static int isl29028_als_get(struct isl29028_chip *chip, int *als_data)
+{
+	int ret;
+	int als_ir_data;
+
+	if (chip->als_ir_mode != MODE_ALS) {
+		ret = isl29028_set_als_ir_mode(chip, MODE_ALS);
+		if (ret < 0) {
+			dev_err(chip->dev,
+				"Error in enabling ALS mode err %d\n", ret);
+			return ret;
+		}
+		chip->als_ir_mode = MODE_ALS;
+	}
+
+	ret = isl29028_read_als_ir(chip, &als_ir_data);
+	if (ret < 0)
+		return ret;
+
+	/*
+	 * convert als data count to lux.
+	 * if lux_scale = 125,  lux = count * 0.031
+	 * if lux_scale = 2000, lux = count * 0.49
+	 */
+	if (chip->lux_scale == 125)
+		als_ir_data = (als_ir_data * 31) / 1000;
+	else
+		als_ir_data = (als_ir_data * 49) / 100;
+
+	*als_data = als_ir_data;
+	return 0;
+}
+
+static int isl29028_ir_get(struct isl29028_chip *chip, int *ir_data)
+{
+	int ret;
+
+	if (chip->als_ir_mode != MODE_IR) {
+		ret = isl29028_set_als_ir_mode(chip, MODE_IR);
+		if (ret < 0) {
+			dev_err(chip->dev,
+				"Error in enabling IR mode err %d\n", ret);
+			return ret;
+		}
+		chip->als_ir_mode = MODE_IR;
+	}
+	return isl29028_read_als_ir(chip, ir_data);
+}
+
+/* Channel IO */
+static int isl29028_write_raw(struct iio_dev *indio_dev,
+	     struct iio_chan_spec const *chan, int val, int val2, long mask)
+{
+	struct isl29028_chip *chip = iio_priv(indio_dev);
+	int ret = -EINVAL;
+
+	mutex_lock(&chip->lock);
+	switch (chan->type) {
+	case IIO_PROXIMITY:
+		if (mask != IIO_CHAN_INFO_SAMP_FREQ) {
+			dev_err(chip->dev,
+				"proximity: mask value 0x%08lx not supported\n",
+				mask);
+			break;
+		}
+		if (val < 1 || val > 100) {
+			dev_err(chip->dev,
+				"Samp_freq %d is not in range[1:100]\n", val);
+			break;
+		}
+		ret = isl29028_set_proxim_sampling(chip, val);
+		if (ret < 0) {
+			dev_err(chip->dev,
+				"Setting proximity samp_freq fail, err %d\n",
+				ret);
+			break;
+		}
+		chip->prox_sampling = val;
+		break;
+
+	case IIO_LIGHT:
+		if (mask != IIO_CHAN_INFO_SCALE) {
+			dev_err(chip->dev,
+				"light: mask value 0x%08lx not supported\n",
+				mask);
+			break;
+		}
+		if ((val != 125) && (val != 2000)) {
+			dev_err(chip->dev,
+				"lux scale %d is invalid [125, 2000]\n", val);
+			break;
+		}
+		ret = isl29028_set_als_scale(chip, val);
+		if (ret < 0) {
+			dev_err(chip->dev,
+				"Setting lux scale fail with error %d\n", ret);
+			break;
+		}
+		chip->lux_scale = val;
+		break;
+
+	default:
+		dev_err(chip->dev, "Unsupported channel type\n");
+		break;
+	}
+	mutex_unlock(&chip->lock);
+	return ret;
+}
+
+static int isl29028_read_raw(struct iio_dev *indio_dev,
+	     struct iio_chan_spec const *chan, int *val, int *val2, long mask)
+{
+	struct isl29028_chip *chip = iio_priv(indio_dev);
+	int ret = -EINVAL;
+
+	mutex_lock(&chip->lock);
+	switch (mask) {
+	case IIO_CHAN_INFO_RAW:
+	case IIO_CHAN_INFO_PROCESSED:
+		switch (chan->type) {
+		case IIO_LIGHT:
+			ret = isl29028_als_get(chip, val);
+			break;
+		case IIO_INTENSITY:
+			ret = isl29028_ir_get(chip, val);
+			break;
+		case IIO_PROXIMITY:
+			ret = isl29028_proxim_get(chip, val);
+			break;
+		default:
+			break;
+		}
+		if (ret < 0)
+			break;
+		ret = IIO_VAL_INT;
+		break;
+
+	case IIO_CHAN_INFO_SAMP_FREQ:
+		if (chan->type != IIO_PROXIMITY)
+			break;
+		*val = chip->prox_sampling;
+		ret = IIO_VAL_INT;
+		break;
+
+	case IIO_CHAN_INFO_SCALE:
+		if (chan->type != IIO_LIGHT)
+			break;
+		*val = chip->lux_scale;
+		ret = IIO_VAL_INT;
+		break;
+
+	default:
+		dev_err(chip->dev, "mask value 0x%08lx not supported\n", mask);
+		break;
+	}
+	mutex_unlock(&chip->lock);
+	return ret;
+}
+
+static IIO_CONST_ATTR(in_proximity_sampling_frequency_available,
+				"1, 3, 5, 10, 13, 20, 83, 100");
+static IIO_CONST_ATTR(in_illuminance_scale_available, "125, 2000");
+
+#define ISL29028_DEV_ATTR(name) (&iio_dev_attr_##name.dev_attr.attr)
+#define ISL29028_CONST_ATTR(name) (&iio_const_attr_##name.dev_attr.attr)
+static struct attribute *isl29028_attributes[] = {
+	ISL29028_CONST_ATTR(in_proximity_sampling_frequency_available),
+	ISL29028_CONST_ATTR(in_illuminance_scale_available),
+	NULL,
+};
+
+static const struct attribute_group isl29108_group = {
+	.attrs = isl29028_attributes,
+};
+
+static const struct iio_chan_spec isl29028_channels[] = {
+	{
+		.type = IIO_LIGHT,
+		.info_mask = IIO_CHAN_INFO_PROCESSED_SEPARATE_BIT |
+		IIO_CHAN_INFO_SCALE_SEPARATE_BIT,
+	}, {
+		.type = IIO_INTENSITY,
+		.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT,
+	}, {
+		.type = IIO_PROXIMITY,
+		.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT |
+		IIO_CHAN_INFO_SAMP_FREQ_SEPARATE_BIT,
+	}
+};
+
+static const struct iio_info isl29028_info = {
+	.attrs = &isl29108_group,
+	.driver_module = THIS_MODULE,
+	.read_raw = &isl29028_read_raw,
+	.write_raw = &isl29028_write_raw,
+};
+
+static int isl29028_chip_init(struct isl29028_chip *chip)
+{
+	int ret;
+
+	chip->enable_prox  = false;
+	chip->prox_sampling = 20;
+	chip->lux_scale = 2000;
+	chip->als_ir_mode = MODE_NONE;
+
+	ret = regmap_write(chip->regmap, ISL29028_REG_TEST1_MODE, 0x0);
+	if (ret < 0) {
+		dev_err(chip->dev, "%s(): write to reg %d failed, err = %d\n",
+			__func__, ISL29028_REG_TEST1_MODE, ret);
+		return ret;
+	}
+	ret = regmap_write(chip->regmap, ISL29028_REG_TEST2_MODE, 0x0);
+	if (ret < 0) {
+		dev_err(chip->dev, "%s(): write to reg %d failed, err = %d\n",
+			__func__, ISL29028_REG_TEST2_MODE, ret);
+		return ret;
+	}
+
+	ret = regmap_write(chip->regmap, ISL29028_REG_CONFIGURE, 0x0);
+	if (ret < 0) {
+		dev_err(chip->dev, "%s(): write to reg %d failed, err = %d\n",
+			__func__, ISL29028_REG_CONFIGURE, ret);
+		return ret;
+	}
+
+	ret = isl29028_set_proxim_sampling(chip, chip->prox_sampling);
+	if (ret < 0) {
+		dev_err(chip->dev, "%s(): setting the proximity, err = %d\n",
+			__func__, ret);
+		return ret;
+	}
+
+	ret = isl29028_set_als_scale(chip, chip->lux_scale);
+	if (ret < 0)
+		dev_err(chip->dev, "%s(): setting als scale failed, err = %d\n",
+			__func__, ret);
+	return ret;
+}
+
+static bool is_volatile_reg(struct device *dev, unsigned int reg)
+{
+	switch (reg) {
+	case ISL29028_REG_INTERRUPT:
+	case ISL29028_REG_PROX_DATA:
+	case ISL29028_REG_ALSIR_L:
+	case ISL29028_REG_ALSIR_U:
+		return true;
+	default:
+		return false;
+	}
+}
+
+static const struct regmap_config isl29028_regmap_config = {
+	.reg_bits = 8,
+	.val_bits = 8,
+	.volatile_reg = is_volatile_reg,
+	.max_register = ISL29028_NUM_REGS - 1,
+	.num_reg_defaults_raw = ISL29028_NUM_REGS,
+	.cache_type = REGCACHE_RBTREE,
+};
+
+static int __devinit isl29028_probe(struct i2c_client *client,
+	const struct i2c_device_id *id)
+{
+	struct isl29028_chip *chip;
+	struct iio_dev *indio_dev;
+	int ret;
+
+	indio_dev = iio_device_alloc(sizeof(*chip));
+	if (!indio_dev) {
+		dev_err(&client->dev, "iio allocation fails\n");
+		return -ENOMEM;
+	}
+
+	chip = iio_priv(indio_dev);
+
+	i2c_set_clientdata(client, indio_dev);
+	chip->dev = &client->dev;
+	mutex_init(&chip->lock);
+
+	chip->regmap = devm_regmap_init_i2c(client, &isl29028_regmap_config);
+	if (IS_ERR(chip->regmap)) {
+		ret = PTR_ERR(chip->regmap);
+		dev_err(chip->dev, "regmap initialization failed: %d\n", ret);
+		goto exit_iio_free;
+	}
+
+	ret = isl29028_chip_init(chip);
+	if (ret < 0) {
+		dev_err(chip->dev, "chip initialization failed: %d\n", ret);
+		goto exit_iio_free;
+	}
+
+	indio_dev->info = &isl29028_info;
+	indio_dev->channels = isl29028_channels;
+	indio_dev->num_channels = ARRAY_SIZE(isl29028_channels);
+	indio_dev->name = id->name;
+	indio_dev->dev.parent = &client->dev;
+	indio_dev->modes = INDIO_DIRECT_MODE;
+	ret = iio_device_register(indio_dev);
+	if (ret < 0) {
+		dev_err(chip->dev, "iio registration fails with error %d\n",
+			ret);
+		goto exit_iio_free;
+	}
+	return 0;
+
+exit_iio_free:
+	iio_device_free(indio_dev);
+	return ret;
+}
+
+static int __devexit isl29028_remove(struct i2c_client *client)
+{
+	struct iio_dev *indio_dev = i2c_get_clientdata(client);
+
+	iio_device_unregister(indio_dev);
+	iio_device_free(indio_dev);
+	return 0;
+}
+
+static const struct i2c_device_id isl29028_id[] = {
+	{"isl29028", 0},
+	{}
+};
+MODULE_DEVICE_TABLE(i2c, isl29028_id);
+
+static const struct of_device_id isl29028_of_match[] = {
+	{ .compatible = "isil,isl29028", },
+	{ },
+};
+MODULE_DEVICE_TABLE(of, isl29028_of_match);
+
+static struct i2c_driver isl29028_driver = {
+	.class	= I2C_CLASS_HWMON,
+	.driver  = {
+		.name = "isl29028",
+		.owner = THIS_MODULE,
+		.of_match_table = isl29028_of_match,
+	},
+	.probe	 = isl29028_probe,
+	.remove  = __devexit_p(isl29028_remove),
+	.id_table = isl29028_id,
+};
+
+module_i2c_driver(isl29028_driver);
+
+MODULE_DESCRIPTION("ISL29028 Ambient Light and Proximity Sensor driver");
+MODULE_LICENSE("GPL v2");
+MODULE_AUTHOR("Laxman Dewangan <ldewangan@nvidia.com>");
diff --git a/drivers/staging/iio/light/tsl2563.c b/drivers/staging/iio/light/tsl2563.c
index 546c95a4ea9..9d740be43a8 100644
--- a/drivers/staging/iio/light/tsl2563.c
+++ b/drivers/staging/iio/light/tsl2563.c
@@ -35,9 +35,9 @@
 #include <linux/err.h>
 #include <linux/slab.h>
 
-#include "../iio.h"
-#include "../sysfs.h"
-#include "../events.h"
+#include <linux/iio/iio.h>
+#include <linux/iio/sysfs.h>
+#include <linux/iio/events.h>
 #include "tsl2563.h"
 
 /* Use this many bits for fraction part. */
@@ -465,7 +465,7 @@ static int tsl2563_write_raw(struct iio_dev *indio_dev,
 {
 	struct tsl2563_chip *chip = iio_priv(indio_dev);
 
-	if (chan->channel == 0)
+	if (chan->channel == IIO_MOD_LIGHT_BOTH)
 		chip->calib0 = calib_from_sysfs(val);
 	else
 		chip->calib1 = calib_from_sysfs(val);
@@ -485,7 +485,8 @@ static int tsl2563_read_raw(struct iio_dev *indio_dev,
 
 	mutex_lock(&chip->lock);
 	switch (m) {
-	case 0:
+	case IIO_CHAN_INFO_RAW:
+	case IIO_CHAN_INFO_PROCESSED:
 		switch (chan->type) {
 		case IIO_LIGHT:
 			ret = tsl2563_get_adc(chip);
@@ -534,12 +535,14 @@ static const struct iio_chan_spec tsl2563_channels[] = {
 	{
 		.type = IIO_LIGHT,
 		.indexed = 1,
+		.info_mask = IIO_CHAN_INFO_PROCESSED_SEPARATE_BIT,
 		.channel = 0,
 	}, {
 		.type = IIO_INTENSITY,
 		.modified = 1,
 		.channel2 = IIO_MOD_LIGHT_BOTH,
-		.info_mask = IIO_CHAN_INFO_CALIBSCALE_SEPARATE_BIT,
+		.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT |
+		IIO_CHAN_INFO_CALIBSCALE_SEPARATE_BIT,
 		.event_mask = (IIO_EV_BIT(IIO_EV_TYPE_THRESH,
 					  IIO_EV_DIR_RISING) |
 			       IIO_EV_BIT(IIO_EV_TYPE_THRESH,
@@ -548,7 +551,8 @@ static const struct iio_chan_spec tsl2563_channels[] = {
 		.type = IIO_INTENSITY,
 		.modified = 1,
 		.channel2 = IIO_MOD_LIGHT_IR,
-		.info_mask = IIO_CHAN_INFO_CALIBSCALE_SEPARATE_BIT,
+		.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT |
+		IIO_CHAN_INFO_CALIBSCALE_SEPARATE_BIT,
 	}
 };
 
@@ -710,7 +714,7 @@ static int __devinit tsl2563_probe(struct i2c_client *client,
 	int err = 0;
 	u8 id = 0;
 
-	indio_dev = iio_allocate_device(sizeof(*chip));
+	indio_dev = iio_device_alloc(sizeof(*chip));
 	if (!indio_dev)
 		return -ENOMEM;
 
@@ -797,7 +801,7 @@ fail2:
 	if (client->irq)
 		free_irq(client->irq, indio_dev);
 fail1:
-	iio_free_device(indio_dev);
+	iio_device_free(indio_dev);
 	return err;
 }
 
@@ -818,7 +822,7 @@ static int tsl2563_remove(struct i2c_client *client)
 	if (client->irq)
 		free_irq(client->irq, indio_dev);
 
-	iio_free_device(indio_dev);
+	iio_device_free(indio_dev);
 
 	return 0;
 }
diff --git a/drivers/staging/iio/light/tsl2583.c b/drivers/staging/iio/light/tsl2583.c
index 8671d98e044..5e23ad5a30d 100644
--- a/drivers/staging/iio/light/tsl2583.c
+++ b/drivers/staging/iio/light/tsl2583.c
@@ -28,7 +28,7 @@
 #include <linux/unistd.h>
 #include <linux/slab.h>
 #include <linux/module.h>
-#include "../iio.h"
+#include <linux/iio/iio.h>
 
 #define TSL258X_MAX_DEVICE_REGS		32
 
@@ -483,7 +483,7 @@ static int taos_chip_off(struct iio_dev *indio_dev)
 static ssize_t taos_power_state_show(struct device *dev,
 	struct device_attribute *attr, char *buf)
 {
-	struct iio_dev *indio_dev = dev_get_drvdata(dev);
+	struct iio_dev *indio_dev = dev_to_iio_dev(dev);
 	struct tsl2583_chip *chip = iio_priv(indio_dev);
 
 	return sprintf(buf, "%d\n", chip->taos_chip_status);
@@ -492,7 +492,7 @@ static ssize_t taos_power_state_show(struct device *dev,
 static ssize_t taos_power_state_store(struct device *dev,
 	struct device_attribute *attr, const char *buf, size_t len)
 {
-	struct iio_dev *indio_dev = dev_get_drvdata(dev);
+	struct iio_dev *indio_dev = dev_to_iio_dev(dev);
 	unsigned long value;
 
 	if (strict_strtoul(buf, 0, &value))
@@ -509,7 +509,7 @@ static ssize_t taos_power_state_store(struct device *dev,
 static ssize_t taos_gain_show(struct device *dev,
 	struct device_attribute *attr, char *buf)
 {
-	struct iio_dev *indio_dev = dev_get_drvdata(dev);
+	struct iio_dev *indio_dev = dev_to_iio_dev(dev);
 	struct tsl2583_chip *chip = iio_priv(indio_dev);
 	char gain[4] = {0};
 
@@ -534,7 +534,7 @@ static ssize_t taos_gain_show(struct device *dev,
 static ssize_t taos_gain_store(struct device *dev,
 	struct device_attribute *attr, const char *buf, size_t len)
 {
-	struct iio_dev *indio_dev = dev_get_drvdata(dev);
+	struct iio_dev *indio_dev = dev_to_iio_dev(dev);
 	struct tsl2583_chip *chip = iio_priv(indio_dev);
 	unsigned long value;
 
@@ -571,7 +571,7 @@ static ssize_t taos_gain_available_show(struct device *dev,
 static ssize_t taos_als_time_show(struct device *dev,
 	struct device_attribute *attr, char *buf)
 {
-	struct iio_dev *indio_dev = dev_get_drvdata(dev);
+	struct iio_dev *indio_dev = dev_to_iio_dev(dev);
 	struct tsl2583_chip *chip = iio_priv(indio_dev);
 
 	return sprintf(buf, "%d\n", chip->taos_settings.als_time);
@@ -580,7 +580,7 @@ static ssize_t taos_als_time_show(struct device *dev,
 static ssize_t taos_als_time_store(struct device *dev,
 	struct device_attribute *attr, const char *buf, size_t len)
 {
-	struct iio_dev *indio_dev = dev_get_drvdata(dev);
+	struct iio_dev *indio_dev = dev_to_iio_dev(dev);
 	struct tsl2583_chip *chip = iio_priv(indio_dev);
 	unsigned long value;
 
@@ -608,7 +608,7 @@ static ssize_t taos_als_time_available_show(struct device *dev,
 static ssize_t taos_als_trim_show(struct device *dev,
 	struct device_attribute *attr, char *buf)
 {
-	struct iio_dev *indio_dev = dev_get_drvdata(dev);
+	struct iio_dev *indio_dev = dev_to_iio_dev(dev);
 	struct tsl2583_chip *chip = iio_priv(indio_dev);
 
 	return sprintf(buf, "%d\n", chip->taos_settings.als_gain_trim);
@@ -617,7 +617,7 @@ static ssize_t taos_als_trim_show(struct device *dev,
 static ssize_t taos_als_trim_store(struct device *dev,
 	struct device_attribute *attr, const char *buf, size_t len)
 {
-	struct iio_dev *indio_dev = dev_get_drvdata(dev);
+	struct iio_dev *indio_dev = dev_to_iio_dev(dev);
 	struct tsl2583_chip *chip = iio_priv(indio_dev);
 	unsigned long value;
 
@@ -633,7 +633,7 @@ static ssize_t taos_als_trim_store(struct device *dev,
 static ssize_t taos_als_cal_target_show(struct device *dev,
 	struct device_attribute *attr, char *buf)
 {
-	struct iio_dev *indio_dev = dev_get_drvdata(dev);
+	struct iio_dev *indio_dev = dev_to_iio_dev(dev);
 	struct tsl2583_chip *chip = iio_priv(indio_dev);
 
 	return sprintf(buf, "%d\n", chip->taos_settings.als_cal_target);
@@ -642,7 +642,7 @@ static ssize_t taos_als_cal_target_show(struct device *dev,
 static ssize_t taos_als_cal_target_store(struct device *dev,
 	struct device_attribute *attr, const char *buf, size_t len)
 {
-	struct iio_dev *indio_dev = dev_get_drvdata(dev);
+	struct iio_dev *indio_dev = dev_to_iio_dev(dev);
 	struct tsl2583_chip *chip = iio_priv(indio_dev);
 	unsigned long value;
 
@@ -660,7 +660,7 @@ static ssize_t taos_lux_show(struct device *dev, struct device_attribute *attr,
 {
 	int ret;
 
-	ret = taos_get_lux(dev_get_drvdata(dev));
+	ret = taos_get_lux(dev_to_iio_dev(dev));
 	if (ret < 0)
 		return ret;
 
@@ -670,7 +670,7 @@ static ssize_t taos_lux_show(struct device *dev, struct device_attribute *attr,
 static ssize_t taos_do_calibrate(struct device *dev,
 	struct device_attribute *attr, const char *buf, size_t len)
 {
-	struct iio_dev *indio_dev = dev_get_drvdata(dev);
+	struct iio_dev *indio_dev = dev_to_iio_dev(dev);
 	unsigned long value;
 
 	if (strict_strtoul(buf, 0, &value))
@@ -708,7 +708,7 @@ static ssize_t taos_luxtable_show(struct device *dev,
 static ssize_t taos_luxtable_store(struct device *dev,
 	struct device_attribute *attr, const char *buf, size_t len)
 {
-	struct iio_dev *indio_dev = dev_get_drvdata(dev);
+	struct iio_dev *indio_dev = dev_to_iio_dev(dev);
 	struct tsl2583_chip *chip = iio_priv(indio_dev);
 	int value[ARRAY_SIZE(taos_device_lux)*3 + 1];
 	int n;
@@ -815,7 +815,7 @@ static int __devinit taos_probe(struct i2c_client *clientp,
 		return -EOPNOTSUPP;
 	}
 
-	indio_dev = iio_allocate_device(sizeof(*chip));
+	indio_dev = iio_device_alloc(sizeof(*chip));
 	if (indio_dev == NULL) {
 		ret = -ENOMEM;
 		dev_err(&clientp->dev, "iio allocation failed\n");
@@ -879,7 +879,7 @@ static int __devinit taos_probe(struct i2c_client *clientp,
 	dev_info(&clientp->dev, "Light sensor found.\n");
 	return 0;
 fail1:
-	iio_free_device(indio_dev);
+	iio_device_free(indio_dev);
 fail2:
 	return ret;
 }
@@ -926,7 +926,7 @@ static SIMPLE_DEV_PM_OPS(taos_pm_ops, taos_suspend, taos_resume);
 static int __devexit taos_remove(struct i2c_client *client)
 {
 	iio_device_unregister(i2c_get_clientdata(client));
-	iio_free_device(i2c_get_clientdata(client));
+	iio_device_free(i2c_get_clientdata(client));
 
 	return 0;
 }
diff --git a/drivers/staging/iio/light/tsl2x7x.h b/drivers/staging/iio/light/tsl2x7x.h
new file mode 100755
index 00000000000..c4acf5ff179
--- /dev/null
+++ b/drivers/staging/iio/light/tsl2x7x.h
@@ -0,0 +1,100 @@
+/*
+ * Device driver for monitoring ambient light intensity (lux)
+ * and proximity (prox) within the TAOS TSL2X7X family of devices.
+ *
+ * Copyright (c) 2012, TAOS Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, 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 __TSL2X7X_H
+#define __TSL2X7X_H
+#include <linux/pm.h>
+
+/* Max number of segments allowable in LUX table */
+#define TSL2X7X_MAX_LUX_TABLE_SIZE		9
+#define MAX_DEFAULT_TABLE_BYTES (sizeof(int) * TSL2X7X_MAX_LUX_TABLE_SIZE)
+
+struct iio_dev;
+
+struct tsl2x7x_lux {
+	unsigned int ratio;
+	unsigned int ch0;
+	unsigned int ch1;
+};
+
+/**
+ * struct tsl2x7x_default_settings - power on defaults unless
+ *                                   overridden by platform data.
+ *  @als_time:              ALS Integration time - multiple of 50mS
+ *  @als_gain:              Index into the ALS gain table.
+ *  @als_gain_trim:         default gain trim to account for
+ *                          aperture effects.
+ *  @wait_time:             Time between PRX and ALS cycles
+ *                          in 2.7 periods
+ *  @prx_time:              5.2ms prox integration time -
+ *                          decrease in 2.7ms periods
+ *  @prx_gain:              Proximity gain index
+ *  @prox_config:           Prox configuration filters.
+ *  @als_cal_target:        Known external ALS reading for
+ *                          calibration.
+ *  @interrupts_en:         Enable/Disable - 0x00 = none, 0x10 = als,
+ *                                           0x20 = prx,  0x30 = bth
+ *  @persistence:           H/W Filters, Number of 'out of limits'
+ *                          ADC readings PRX/ALS.
+ *  @als_thresh_low:        CH0 'low' count to trigger interrupt.
+ *  @als_thresh_high:       CH0 'high' count to trigger interrupt.
+ *  @prox_thres_low:        Low threshold proximity detection.
+ *  @prox_thres_high:       High threshold proximity detection
+ *  @prox_pulse_count:      Number if proximity emitter pulses
+ *  @prox_max_samples_cal:  Used for prox cal.
+ */
+struct tsl2x7x_settings {
+	int als_time;
+	int als_gain;
+	int als_gain_trim;
+	int wait_time;
+	int prx_time;
+	int prox_gain;
+	int prox_config;
+	int als_cal_target;
+	u8  interrupts_en;
+	u8  persistence;
+	int als_thresh_low;
+	int als_thresh_high;
+	int prox_thres_low;
+	int prox_thres_high;
+	int prox_pulse_count;
+	int prox_max_samples_cal;
+};
+
+/**
+ * struct tsl2X7X_platform_data - Platform callback, glass and defaults
+ * @platform_power:            Suspend/resume platform callback
+ * @power_on:                  Power on callback
+ * @power_off:                 Power off callback
+ * @platform_lux_table:        Device specific glass coefficents
+ * @platform_default_settings: Device specific power on defaults
+ *
+ */
+struct tsl2X7X_platform_data {
+	int (*platform_power)(struct device *dev, pm_message_t);
+	int (*power_on)      (struct iio_dev *indio_dev);
+	int (*power_off)     (struct i2c_client *dev);
+	struct tsl2x7x_lux platform_lux_table[TSL2X7X_MAX_LUX_TABLE_SIZE];
+	struct tsl2x7x_settings *platform_default_settings;
+};
+
+#endif /* __TSL2X7X_H */
diff --git a/drivers/staging/iio/light/tsl2x7x_core.c b/drivers/staging/iio/light/tsl2x7x_core.c
new file mode 100755
index 00000000000..c3b05a1b3ea
--- /dev/null
+++ b/drivers/staging/iio/light/tsl2x7x_core.c
@@ -0,0 +1,2082 @@
+/*
+ * Device driver for monitoring ambient light intensity in (lux)
+ * and proximity detection (prox) within the TAOS TSL2X7X family of devices.
+ *
+ * Copyright (c) 2012, TAOS Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, 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/i2c.h>
+#include <linux/errno.h>
+#include <linux/delay.h>
+#include <linux/mutex.h>
+#include <linux/interrupt.h>
+#include <linux/slab.h>
+#include <linux/module.h>
+#include <linux/version.h>
+#include <linux/iio/events.h>
+#include <linux/iio/iio.h>
+#include <linux/iio/sysfs.h>
+#include "tsl2x7x.h"
+
+/* Cal defs*/
+#define PROX_STAT_CAL        0
+#define PROX_STAT_SAMP       1
+#define MAX_SAMPLES_CAL      200
+
+/* TSL2X7X Device ID */
+#define TRITON_ID    0x00
+#define SWORDFISH_ID 0x30
+#define HALIBUT_ID   0x20
+
+/* Lux calculation constants */
+#define TSL2X7X_LUX_CALC_OVER_FLOW     65535
+
+/* TAOS Register definitions - note:
+ * depending on device, some of these register are not used and the
+ * register address is benign.
+ */
+/* 2X7X register offsets */
+#define TSL2X7X_MAX_CONFIG_REG         16
+
+/* Device Registers and Masks */
+#define TSL2X7X_CNTRL                  0x00
+#define TSL2X7X_ALS_TIME               0X01
+#define TSL2X7X_PRX_TIME               0x02
+#define TSL2X7X_WAIT_TIME              0x03
+#define TSL2X7X_ALS_MINTHRESHLO        0X04
+#define TSL2X7X_ALS_MINTHRESHHI        0X05
+#define TSL2X7X_ALS_MAXTHRESHLO        0X06
+#define TSL2X7X_ALS_MAXTHRESHHI        0X07
+#define TSL2X7X_PRX_MINTHRESHLO        0X08
+#define TSL2X7X_PRX_MINTHRESHHI        0X09
+#define TSL2X7X_PRX_MAXTHRESHLO        0X0A
+#define TSL2X7X_PRX_MAXTHRESHHI        0X0B
+#define TSL2X7X_PERSISTENCE            0x0C
+#define TSL2X7X_PRX_CONFIG             0x0D
+#define TSL2X7X_PRX_COUNT              0x0E
+#define TSL2X7X_GAIN                   0x0F
+#define TSL2X7X_NOTUSED                0x10
+#define TSL2X7X_REVID                  0x11
+#define TSL2X7X_CHIPID                 0x12
+#define TSL2X7X_STATUS                 0x13
+#define TSL2X7X_ALS_CHAN0LO            0x14
+#define TSL2X7X_ALS_CHAN0HI            0x15
+#define TSL2X7X_ALS_CHAN1LO            0x16
+#define TSL2X7X_ALS_CHAN1HI            0x17
+#define TSL2X7X_PRX_LO                 0x18
+#define TSL2X7X_PRX_HI                 0x19
+
+/* tsl2X7X cmd reg masks */
+#define TSL2X7X_CMD_REG                0x80
+#define TSL2X7X_CMD_SPL_FN             0x60
+
+#define TSL2X7X_CMD_PROX_INT_CLR       0X05
+#define TSL2X7X_CMD_ALS_INT_CLR        0x06
+#define TSL2X7X_CMD_PROXALS_INT_CLR    0X07
+
+/* tsl2X7X cntrl reg masks */
+#define TSL2X7X_CNTL_ADC_ENBL          0x02
+#define TSL2X7X_CNTL_PWR_ON            0x01
+
+/* tsl2X7X status reg masks */
+#define TSL2X7X_STA_ADC_VALID          0x01
+#define TSL2X7X_STA_PRX_VALID          0x02
+#define TSL2X7X_STA_ADC_PRX_VALID      (TSL2X7X_STA_ADC_VALID |\
+					TSL2X7X_STA_PRX_VALID)
+#define TSL2X7X_STA_ALS_INTR           0x10
+#define TSL2X7X_STA_PRX_INTR           0x20
+
+/* tsl2X7X cntrl reg masks */
+#define TSL2X7X_CNTL_REG_CLEAR         0x00
+#define TSL2X7X_CNTL_PROX_INT_ENBL     0X20
+#define TSL2X7X_CNTL_ALS_INT_ENBL      0X10
+#define TSL2X7X_CNTL_WAIT_TMR_ENBL     0X08
+#define TSL2X7X_CNTL_PROX_DET_ENBL     0X04
+#define TSL2X7X_CNTL_PWRON             0x01
+#define TSL2X7X_CNTL_ALSPON_ENBL       0x03
+#define TSL2X7X_CNTL_INTALSPON_ENBL    0x13
+#define TSL2X7X_CNTL_PROXPON_ENBL      0x0F
+#define TSL2X7X_CNTL_INTPROXPON_ENBL   0x2F
+
+/*Prox diode to use */
+#define TSL2X7X_DIODE0                 0x10
+#define TSL2X7X_DIODE1                 0x20
+#define TSL2X7X_DIODE_BOTH             0x30
+
+/* LED Power */
+#define TSL2X7X_mA100                  0x00
+#define TSL2X7X_mA50                   0x40
+#define TSL2X7X_mA25                   0x80
+#define TSL2X7X_mA13                   0xD0
+#define TSL2X7X_MAX_TIMER_CNT          (0xFF)
+
+/*Common device IIO EventMask */
+#define TSL2X7X_EVENT_MASK \
+		(IIO_EV_BIT(IIO_EV_TYPE_THRESH, IIO_EV_DIR_RISING) | \
+		IIO_EV_BIT(IIO_EV_TYPE_THRESH, IIO_EV_DIR_FALLING)),
+
+#define TSL2X7X_MIN_ITIME 3
+
+/* TAOS txx2x7x Device family members */
+enum {
+	tsl2571,
+	tsl2671,
+	tmd2671,
+	tsl2771,
+	tmd2771,
+	tsl2572,
+	tsl2672,
+	tmd2672,
+	tsl2772,
+	tmd2772
+};
+
+enum {
+	TSL2X7X_CHIP_UNKNOWN = 0,
+	TSL2X7X_CHIP_WORKING = 1,
+	TSL2X7X_CHIP_SUSPENDED = 2
+};
+
+struct tsl2x7x_parse_result {
+	int integer;
+	int fract;
+};
+
+/* Per-device data */
+struct tsl2x7x_als_info {
+	u16 als_ch0;
+	u16 als_ch1;
+	u16 lux;
+};
+
+struct tsl2x7x_prox_stat {
+	int min;
+	int max;
+	int mean;
+	unsigned long stddev;
+};
+
+struct tsl2x7x_chip_info {
+	int chan_table_elements;
+	struct iio_chan_spec		channel[4];
+	const struct iio_info		*info;
+};
+
+struct tsl2X7X_chip {
+	kernel_ulong_t id;
+	struct mutex prox_mutex;
+	struct mutex als_mutex;
+	struct i2c_client *client;
+	u16 prox_data;
+	struct tsl2x7x_als_info als_cur_info;
+	struct tsl2x7x_settings tsl2x7x_settings;
+	struct tsl2X7X_platform_data *pdata;
+	int als_time_scale;
+	int als_saturation;
+	int tsl2x7x_chip_status;
+	u8 tsl2x7x_config[TSL2X7X_MAX_CONFIG_REG];
+	const struct tsl2x7x_chip_info	*chip_info;
+	const struct iio_info *info;
+	s64 event_timestamp;
+	/* This structure is intentionally large to accommodate
+	 * updates via sysfs. */
+	/* Sized to 9 = max 8 segments + 1 termination segment */
+	struct tsl2x7x_lux tsl2x7x_device_lux[TSL2X7X_MAX_LUX_TABLE_SIZE];
+};
+
+/* Different devices require different coefficents */
+static const struct tsl2x7x_lux tsl2x71_lux_table[] = {
+	{ 14461,   611,   1211 },
+	{ 18540,   352,    623 },
+	{     0,     0,      0 },
+};
+
+static const struct tsl2x7x_lux tmd2x71_lux_table[] = {
+	{ 11635,   115,    256 },
+	{ 15536,    87,    179 },
+	{     0,     0,      0 },
+};
+
+static const struct tsl2x7x_lux tsl2x72_lux_table[] = {
+	{ 14013,   466,   917 },
+	{ 18222,   310,   552 },
+	{     0,     0,     0 },
+};
+
+static const struct tsl2x7x_lux tmd2x72_lux_table[] = {
+	{ 13218,   130,   262 },
+	{ 17592,   92,    169 },
+	{     0,     0,     0 },
+};
+
+static const struct tsl2x7x_lux *tsl2x7x_default_lux_table_group[] = {
+	[tsl2571] =	tsl2x71_lux_table,
+	[tsl2671] =	tsl2x71_lux_table,
+	[tmd2671] =	tmd2x71_lux_table,
+	[tsl2771] =	tsl2x71_lux_table,
+	[tmd2771] =	tmd2x71_lux_table,
+	[tsl2572] =	tsl2x72_lux_table,
+	[tsl2672] =	tsl2x72_lux_table,
+	[tmd2672] =	tmd2x72_lux_table,
+	[tsl2772] =	tsl2x72_lux_table,
+	[tmd2772] =	tmd2x72_lux_table,
+};
+
+static const struct tsl2x7x_settings tsl2x7x_default_settings = {
+	.als_time = 219, /* 101 ms */
+	.als_gain = 0,
+	.prx_time = 254, /* 5.4 ms */
+	.prox_gain = 1,
+	.wait_time = 245,
+	.prox_config = 0,
+	.als_gain_trim = 1000,
+	.als_cal_target = 150,
+	.als_thresh_low = 200,
+	.als_thresh_high = 256,
+	.persistence = 255,
+	.interrupts_en = 0,
+	.prox_thres_low  = 0,
+	.prox_thres_high = 512,
+	.prox_max_samples_cal = 30,
+	.prox_pulse_count = 8
+};
+
+static const s16 tsl2X7X_als_gainadj[] = {
+	1,
+	8,
+	16,
+	120
+};
+
+static const s16 tsl2X7X_prx_gainadj[] = {
+	1,
+	2,
+	4,
+	8
+};
+
+/* Channel variations */
+enum {
+	ALS,
+	PRX,
+	ALSPRX,
+	PRX2,
+	ALSPRX2,
+};
+
+static const u8 device_channel_config[] = {
+	ALS,
+	PRX,
+	PRX,
+	ALSPRX,
+	ALSPRX,
+	ALS,
+	PRX2,
+	PRX2,
+	ALSPRX2,
+	ALSPRX2
+};
+
+/**
+ * tsl2x7x_parse_buffer() - parse a decimal result from a buffer.
+ * @*buf:                   pointer to char buffer to parse
+ * @*result:                pointer to buffer to contain
+ *                          resulting interger / decimal as ints.
+ *
+ */
+static int
+tsl2x7x_parse_buffer(const char *buf, struct tsl2x7x_parse_result *result)
+{
+	int integer = 0, fract = 0, fract_mult = 100000;
+	bool integer_part = true, negative = false;
+
+	if (buf[0] == '-') {
+		negative = true;
+		buf++;
+	}
+
+	while (*buf) {
+		if ('0' <= *buf && *buf <= '9') {
+			if (integer_part)
+				integer = integer*10 + *buf - '0';
+			else {
+				fract += fract_mult*(*buf - '0');
+				if (fract_mult == 1)
+					break;
+				fract_mult /= 10;
+			}
+		} else if (*buf == '\n') {
+			if (*(buf + 1) == '\0')
+				break;
+			else
+				return -EINVAL;
+		} else if (*buf == '.') {
+			integer_part = false;
+		} else {
+			return -EINVAL;
+		}
+		buf++;
+	}
+	if (negative) {
+		if (integer)
+			integer = -integer;
+		else
+			fract = -fract;
+	}
+
+	result->integer = integer;
+	result->fract = fract;
+
+	return 0;
+}
+
+/**
+ * tsl2x7x_i2c_read() - Read a byte from a register.
+ * @client:	i2c client
+ * @reg:	device register to read from
+ * @*val:	pointer to location to store register contents.
+ *
+ */
+static int
+tsl2x7x_i2c_read(struct i2c_client *client, u8 reg, u8 *val)
+{
+	int ret = 0;
+
+	/* select register to write */
+	ret = i2c_smbus_write_byte(client, (TSL2X7X_CMD_REG | reg));
+	if (ret < 0) {
+		dev_err(&client->dev, "%s: failed to write register %x\n"
+				, __func__, reg);
+		return ret;
+	}
+
+	/* read the data */
+	ret = i2c_smbus_read_byte(client);
+	if (ret >= 0)
+		*val = (u8)ret;
+	else
+		dev_err(&client->dev, "%s: failed to read register %x\n"
+						, __func__, reg);
+
+	return ret;
+}
+
+/**
+ * tsl2x7x_get_lux() - Reads and calculates current lux value.
+ * @indio_dev:	pointer to IIO device
+ *
+ * The raw ch0 and ch1 values of the ambient light sensed in the last
+ * integration cycle are read from the device.
+ * Time scale factor array values are adjusted based on the integration time.
+ * The raw values are multiplied by a scale factor, and device gain is obtained
+ * using gain index. Limit checks are done next, then the ratio of a multiple
+ * of ch1 value, to the ch0 value, is calculated. Array tsl2x7x_device_lux[]
+ * is then scanned to find the first ratio value that is just above the ratio
+ * we just calculated. The ch0 and ch1 multiplier constants in the array are
+ * then used along with the time scale factor array values, to calculate the
+ * lux.
+ */
+static int tsl2x7x_get_lux(struct iio_dev *indio_dev)
+{
+	u16 ch0, ch1; /* separated ch0/ch1 data from device */
+	u32 lux; /* raw lux calculated from device data */
+	u64 lux64;
+	u32 ratio;
+	u8 buf[4];
+	struct tsl2x7x_lux *p;
+	struct tsl2X7X_chip *chip = iio_priv(indio_dev);
+	int i, ret;
+	u32 ch0lux = 0;
+	u32 ch1lux = 0;
+
+	if (mutex_trylock(&chip->als_mutex) == 0)
+		return chip->als_cur_info.lux; /* busy, so return LAST VALUE */
+
+	if (chip->tsl2x7x_chip_status != TSL2X7X_CHIP_WORKING) {
+		/* device is not enabled */
+		dev_err(&chip->client->dev, "%s: device is not enabled\n",
+				__func__);
+		ret = -EBUSY ;
+		goto out_unlock;
+	}
+
+	ret = tsl2x7x_i2c_read(chip->client,
+		(TSL2X7X_CMD_REG | TSL2X7X_STATUS), &buf[0]);
+	if (ret < 0) {
+		dev_err(&chip->client->dev,
+			"%s: Failed to read STATUS Reg\n", __func__);
+		goto out_unlock;
+	}
+	/* is data new & valid */
+	if (!(buf[0] & TSL2X7X_STA_ADC_VALID)) {
+		dev_err(&chip->client->dev,
+			"%s: data not valid yet\n", __func__);
+		ret = chip->als_cur_info.lux; /* return LAST VALUE */
+		goto out_unlock;
+	}
+
+	for (i = 0; i < 4; i++) {
+		ret = tsl2x7x_i2c_read(chip->client,
+			(TSL2X7X_CMD_REG | (TSL2X7X_ALS_CHAN0LO + i)),
+			&buf[i]);
+		if (ret < 0) {
+			dev_err(&chip->client->dev,
+				"%s: failed to read. err=%x\n", __func__, ret);
+			goto out_unlock;
+		}
+	}
+
+	/* clear any existing interrupt status */
+	ret = i2c_smbus_write_byte(chip->client,
+		(TSL2X7X_CMD_REG |
+				TSL2X7X_CMD_SPL_FN |
+				TSL2X7X_CMD_ALS_INT_CLR));
+	if (ret < 0) {
+		dev_err(&chip->client->dev,
+		"%s: i2c_write_command failed - err = %d\n",
+			__func__, ret);
+		goto out_unlock; /* have no data, so return failure */
+	}
+
+	/* extract ALS/lux data */
+	ch0 = le16_to_cpup((const __le16 *)&buf[0]);
+	ch1 = le16_to_cpup((const __le16 *)&buf[2]);
+
+	chip->als_cur_info.als_ch0 = ch0;
+	chip->als_cur_info.als_ch1 = ch1;
+
+	if ((ch0 >= chip->als_saturation) || (ch1 >= chip->als_saturation)) {
+		lux = TSL2X7X_LUX_CALC_OVER_FLOW;
+		goto return_max;
+	}
+
+	if (ch0 == 0) {
+		/* have no data, so return LAST VALUE */
+		ret = chip->als_cur_info.lux;
+		goto out_unlock;
+	}
+	/* calculate ratio */
+	ratio = (ch1 << 15) / ch0;
+	/* convert to unscaled lux using the pointer to the table */
+	p = (struct tsl2x7x_lux *) chip->tsl2x7x_device_lux;
+	while (p->ratio != 0 && p->ratio < ratio)
+		p++;
+
+	if (p->ratio == 0) {
+		lux = 0;
+	} else {
+		ch0lux = DIV_ROUND_UP((ch0 * p->ch0),
+			tsl2X7X_als_gainadj[chip->tsl2x7x_settings.als_gain]);
+		ch1lux = DIV_ROUND_UP((ch1 * p->ch1),
+			tsl2X7X_als_gainadj[chip->tsl2x7x_settings.als_gain]);
+		lux = ch0lux - ch1lux;
+	}
+
+	/* note: lux is 31 bit max at this point */
+	if (ch1lux > ch0lux) {
+		dev_dbg(&chip->client->dev, "ch1lux > ch0lux-return last value\n");
+		ret = chip->als_cur_info.lux;
+		goto out_unlock;
+	}
+
+	/* adjust for active time scale */
+	if (chip->als_time_scale == 0)
+		lux = 0;
+	else
+		lux = (lux + (chip->als_time_scale >> 1)) /
+			chip->als_time_scale;
+
+	/* adjust for active gain scale
+	 * The tsl2x7x_device_lux tables have a factor of 256 built-in.
+	 * User-specified gain provides a multiplier.
+	 * Apply user-specified gain before shifting right to retain precision.
+	 * Use 64 bits to avoid overflow on multiplication.
+	 * Then go back to 32 bits before division to avoid using div_u64().
+	 */
+
+	lux64 = lux;
+	lux64 = lux64 * chip->tsl2x7x_settings.als_gain_trim;
+	lux64 >>= 8;
+	lux = lux64;
+	lux = (lux + 500) / 1000;
+
+	if (lux > TSL2X7X_LUX_CALC_OVER_FLOW) /* check for overflow */
+		lux = TSL2X7X_LUX_CALC_OVER_FLOW;
+
+	/* Update the structure with the latest lux. */
+return_max:
+	chip->als_cur_info.lux = lux;
+	ret = lux;
+
+out_unlock:
+	mutex_unlock(&chip->als_mutex);
+
+	return ret;
+}
+
+/**
+ * tsl2x7x_get_prox() - Reads proximity data registers and updates
+ *                      chip->prox_data.
+ *
+ * @indio_dev:	pointer to IIO device
+ */
+static int tsl2x7x_get_prox(struct iio_dev *indio_dev)
+{
+	int i;
+	int ret;
+	u8 status;
+	u8 chdata[2];
+	struct tsl2X7X_chip *chip = iio_priv(indio_dev);
+
+	if (mutex_trylock(&chip->prox_mutex) == 0) {
+		dev_err(&chip->client->dev,
+			"%s: Can't get prox mutex\n", __func__);
+		return -EBUSY;
+	}
+
+	ret = tsl2x7x_i2c_read(chip->client,
+		(TSL2X7X_CMD_REG | TSL2X7X_STATUS), &status);
+	if (ret < 0) {
+		dev_err(&chip->client->dev,
+		"%s: i2c err=%d\n", __func__, ret);
+		goto prox_poll_err;
+	}
+
+	switch (chip->id) {
+	case tsl2571:
+	case tsl2671:
+	case tmd2671:
+	case tsl2771:
+	case tmd2771:
+		if (!(status & TSL2X7X_STA_ADC_VALID))
+			goto prox_poll_err;
+	break;
+	case tsl2572:
+	case tsl2672:
+	case tmd2672:
+	case tsl2772:
+	case tmd2772:
+		if (!(status & TSL2X7X_STA_PRX_VALID))
+			goto prox_poll_err;
+	break;
+	}
+
+	for (i = 0; i < 2; i++) {
+		ret = tsl2x7x_i2c_read(chip->client,
+			(TSL2X7X_CMD_REG |
+					(TSL2X7X_PRX_LO + i)), &chdata[i]);
+		if (ret < 0)
+			goto prox_poll_err;
+	}
+
+	chip->prox_data =
+			le16_to_cpup((const __le16 *)&chdata[0]);
+
+prox_poll_err:
+
+	mutex_unlock(&chip->prox_mutex);
+
+	return chip->prox_data;
+}
+
+/**
+ * tsl2x7x_defaults() - Populates the device nominal operating parameters
+ *                      with those provided by a 'platform' data struct or
+ *                      with prefined defaults.
+ *
+ * @chip:               pointer to device structure.
+ */
+static void tsl2x7x_defaults(struct tsl2X7X_chip *chip)
+{
+	/* If Operational settings defined elsewhere.. */
+	if (chip->pdata && chip->pdata->platform_default_settings != 0)
+		memcpy(&(chip->tsl2x7x_settings),
+			chip->pdata->platform_default_settings,
+			sizeof(tsl2x7x_default_settings));
+	else
+		memcpy(&(chip->tsl2x7x_settings),
+			&tsl2x7x_default_settings,
+			sizeof(tsl2x7x_default_settings));
+
+	/* Load up the proper lux table. */
+	if (chip->pdata && chip->pdata->platform_lux_table[0].ratio != 0)
+		memcpy(chip->tsl2x7x_device_lux,
+			chip->pdata->platform_lux_table,
+			sizeof(chip->pdata->platform_lux_table));
+	else
+		memcpy(chip->tsl2x7x_device_lux,
+		(struct tsl2x7x_lux *)tsl2x7x_default_lux_table_group[chip->id],
+				MAX_DEFAULT_TABLE_BYTES);
+}
+
+/**
+ * tsl2x7x_als_calibrate() -	Obtain single reading and calculate
+ *                              the als_gain_trim.
+ *
+ * @indio_dev:	pointer to IIO device
+ */
+static int tsl2x7x_als_calibrate(struct iio_dev *indio_dev)
+{
+	struct tsl2X7X_chip *chip = iio_priv(indio_dev);
+	u8 reg_val;
+	int gain_trim_val;
+	int ret;
+	int lux_val;
+
+	ret = i2c_smbus_write_byte(chip->client,
+			(TSL2X7X_CMD_REG | TSL2X7X_CNTRL));
+	if (ret < 0) {
+		dev_err(&chip->client->dev,
+		"%s: failed to write CNTRL register, ret=%d\n",
+		__func__, ret);
+		return ret;
+	}
+
+	reg_val = i2c_smbus_read_byte(chip->client);
+	if ((reg_val & (TSL2X7X_CNTL_ADC_ENBL | TSL2X7X_CNTL_PWR_ON))
+		!= (TSL2X7X_CNTL_ADC_ENBL | TSL2X7X_CNTL_PWR_ON)) {
+		dev_err(&chip->client->dev,
+			"%s: failed: ADC not enabled\n", __func__);
+		return -1;
+	}
+
+	ret = i2c_smbus_write_byte(chip->client,
+			(TSL2X7X_CMD_REG | TSL2X7X_CNTRL));
+	if (ret < 0) {
+		dev_err(&chip->client->dev,
+			"%s: failed to write ctrl reg: ret=%d\n",
+			__func__, ret);
+		return ret;
+	}
+
+	reg_val = i2c_smbus_read_byte(chip->client);
+	if ((reg_val & TSL2X7X_STA_ADC_VALID) != TSL2X7X_STA_ADC_VALID) {
+		dev_err(&chip->client->dev,
+			"%s: failed: STATUS - ADC not valid.\n", __func__);
+		return -ENODATA;
+	}
+
+	lux_val = tsl2x7x_get_lux(indio_dev);
+	if (lux_val < 0) {
+		dev_err(&chip->client->dev,
+		"%s: failed to get lux\n", __func__);
+		return lux_val;
+	}
+
+	gain_trim_val =  (((chip->tsl2x7x_settings.als_cal_target)
+			* chip->tsl2x7x_settings.als_gain_trim) / lux_val);
+	if ((gain_trim_val < 250) || (gain_trim_val > 4000))
+		return -ERANGE;
+
+	chip->tsl2x7x_settings.als_gain_trim = gain_trim_val;
+	dev_info(&chip->client->dev,
+		"%s als_calibrate completed\n", chip->client->name);
+
+	return (int) gain_trim_val;
+}
+
+static int tsl2x7x_chip_on(struct iio_dev *indio_dev)
+{
+	int i;
+	int ret = 0;
+	u8 *dev_reg;
+	u8 utmp;
+	int als_count;
+	int als_time;
+	struct tsl2X7X_chip *chip = iio_priv(indio_dev);
+	u8 reg_val = 0;
+
+	if (chip->pdata && chip->pdata->power_on)
+		chip->pdata->power_on(indio_dev);
+
+	/* Non calculated parameters */
+	chip->tsl2x7x_config[TSL2X7X_PRX_TIME] =
+			chip->tsl2x7x_settings.prx_time;
+	chip->tsl2x7x_config[TSL2X7X_WAIT_TIME] =
+			chip->tsl2x7x_settings.wait_time;
+	chip->tsl2x7x_config[TSL2X7X_PRX_CONFIG] =
+			chip->tsl2x7x_settings.prox_config;
+
+	chip->tsl2x7x_config[TSL2X7X_ALS_MINTHRESHLO] =
+		(chip->tsl2x7x_settings.als_thresh_low) & 0xFF;
+	chip->tsl2x7x_config[TSL2X7X_ALS_MINTHRESHHI] =
+		(chip->tsl2x7x_settings.als_thresh_low >> 8) & 0xFF;
+	chip->tsl2x7x_config[TSL2X7X_ALS_MAXTHRESHLO] =
+		(chip->tsl2x7x_settings.als_thresh_high) & 0xFF;
+	chip->tsl2x7x_config[TSL2X7X_ALS_MAXTHRESHHI] =
+		(chip->tsl2x7x_settings.als_thresh_high >> 8) & 0xFF;
+	chip->tsl2x7x_config[TSL2X7X_PERSISTENCE] =
+		chip->tsl2x7x_settings.persistence;
+
+	chip->tsl2x7x_config[TSL2X7X_PRX_COUNT] =
+			chip->tsl2x7x_settings.prox_pulse_count;
+	chip->tsl2x7x_config[TSL2X7X_PRX_MINTHRESHLO] =
+	chip->tsl2x7x_settings.prox_thres_low;
+	chip->tsl2x7x_config[TSL2X7X_PRX_MAXTHRESHLO] =
+			chip->tsl2x7x_settings.prox_thres_high;
+
+	/* and make sure we're not already on */
+	if (chip->tsl2x7x_chip_status == TSL2X7X_CHIP_WORKING) {
+		/* if forcing a register update - turn off, then on */
+		dev_info(&chip->client->dev, "device is already enabled\n");
+		return -EINVAL;
+	}
+
+	/* determine als integration regster */
+	als_count = (chip->tsl2x7x_settings.als_time * 100 + 135) / 270;
+	if (als_count == 0)
+		als_count = 1; /* ensure at least one cycle */
+
+	/* convert back to time (encompasses overrides) */
+	als_time = (als_count * 27 + 5) / 10;
+	chip->tsl2x7x_config[TSL2X7X_ALS_TIME] = 256 - als_count;
+
+	/* Set the gain based on tsl2x7x_settings struct */
+	chip->tsl2x7x_config[TSL2X7X_GAIN] =
+		(chip->tsl2x7x_settings.als_gain |
+			(TSL2X7X_mA100 | TSL2X7X_DIODE1)
+			| ((chip->tsl2x7x_settings.prox_gain) << 2));
+
+	/* set chip struct re scaling and saturation */
+	chip->als_saturation = als_count * 922; /* 90% of full scale */
+	chip->als_time_scale = (als_time + 25) / 50;
+
+	/* TSL2X7X Specific power-on / adc enable sequence
+	 * Power on the device 1st. */
+	utmp = TSL2X7X_CNTL_PWR_ON;
+	ret = i2c_smbus_write_byte_data(chip->client,
+		TSL2X7X_CMD_REG | TSL2X7X_CNTRL, utmp);
+	if (ret < 0) {
+		dev_err(&chip->client->dev,
+			"%s: failed on CNTRL reg.\n", __func__);
+		return ret;
+	}
+
+	/* Use the following shadow copy for our delay before enabling ADC.
+	 * Write all the registers. */
+	for (i = 0, dev_reg = chip->tsl2x7x_config;
+			i < TSL2X7X_MAX_CONFIG_REG; i++) {
+		ret = i2c_smbus_write_byte_data(chip->client,
+				TSL2X7X_CMD_REG + i, *dev_reg++);
+		if (ret < 0) {
+			dev_err(&chip->client->dev,
+			"%s: failed on write to reg %d.\n", __func__, i);
+			return ret;
+		}
+	}
+
+	mdelay(3);	/* Power-on settling time */
+
+	/* NOW enable the ADC
+	 * initialize the desired mode of operation */
+	utmp = TSL2X7X_CNTL_PWR_ON |
+			TSL2X7X_CNTL_ADC_ENBL |
+			TSL2X7X_CNTL_PROX_DET_ENBL;
+	ret = i2c_smbus_write_byte_data(chip->client,
+			TSL2X7X_CMD_REG | TSL2X7X_CNTRL, utmp);
+	if (ret < 0) {
+		dev_err(&chip->client->dev,
+			"%s: failed on 2nd CTRL reg.\n", __func__);
+		return ret;
+	}
+
+	chip->tsl2x7x_chip_status = TSL2X7X_CHIP_WORKING;
+
+	if (chip->tsl2x7x_settings.interrupts_en != 0) {
+		dev_info(&chip->client->dev, "Setting Up Interrupt(s)\n");
+
+		reg_val = TSL2X7X_CNTL_PWR_ON | TSL2X7X_CNTL_ADC_ENBL;
+		if ((chip->tsl2x7x_settings.interrupts_en == 0x20) ||
+			(chip->tsl2x7x_settings.interrupts_en == 0x30))
+			reg_val |= TSL2X7X_CNTL_PROX_DET_ENBL;
+
+		reg_val |= chip->tsl2x7x_settings.interrupts_en;
+		ret = i2c_smbus_write_byte_data(chip->client,
+			(TSL2X7X_CMD_REG | TSL2X7X_CNTRL), reg_val);
+		if (ret < 0)
+			dev_err(&chip->client->dev,
+				"%s: failed in tsl2x7x_IOCTL_INT_SET.\n",
+				__func__);
+
+		/* Clear out any initial interrupts  */
+		ret = i2c_smbus_write_byte(chip->client,
+			TSL2X7X_CMD_REG | TSL2X7X_CMD_SPL_FN |
+			TSL2X7X_CMD_PROXALS_INT_CLR);
+		if (ret < 0) {
+			dev_err(&chip->client->dev,
+				"%s: Failed to clear Int status\n",
+				__func__);
+		return ret;
+		}
+	}
+
+	return ret;
+}
+
+static int tsl2x7x_chip_off(struct iio_dev *indio_dev)
+{
+	int ret;
+	struct tsl2X7X_chip *chip = iio_priv(indio_dev);
+
+	/* turn device off */
+	chip->tsl2x7x_chip_status = TSL2X7X_CHIP_SUSPENDED;
+
+	ret = i2c_smbus_write_byte_data(chip->client,
+		TSL2X7X_CMD_REG | TSL2X7X_CNTRL, 0x00);
+
+	if (chip->pdata && chip->pdata->power_off)
+		chip->pdata->power_off(chip->client);
+
+	return ret;
+}
+
+/**
+ * tsl2x7x_invoke_change
+ * @indio_dev:	pointer to IIO device
+ *
+ * Obtain and lock both ALS and PROX resources,
+ * determine and save device state (On/Off),
+ * cycle device to implement updated parameter,
+ * put device back into proper state, and unlock
+ * resource.
+ */
+static
+int tsl2x7x_invoke_change(struct iio_dev *indio_dev)
+{
+	struct tsl2X7X_chip *chip = iio_priv(indio_dev);
+	int device_status = chip->tsl2x7x_chip_status;
+
+	mutex_lock(&chip->als_mutex);
+	mutex_lock(&chip->prox_mutex);
+
+	if (device_status == TSL2X7X_CHIP_WORKING)
+		tsl2x7x_chip_off(indio_dev);
+
+	tsl2x7x_chip_on(indio_dev);
+
+	if (device_status != TSL2X7X_CHIP_WORKING)
+		tsl2x7x_chip_off(indio_dev);
+
+	mutex_unlock(&chip->prox_mutex);
+	mutex_unlock(&chip->als_mutex);
+
+	return 0;
+}
+
+static
+void tsl2x7x_prox_calculate(int *data, int length,
+		struct tsl2x7x_prox_stat *statP)
+{
+	int i;
+	int sample_sum;
+	int tmp;
+
+	if (length == 0)
+		length = 1;
+
+	sample_sum = 0;
+	statP->min = INT_MAX;
+	statP->max = INT_MIN;
+	for (i = 0; i < length; i++) {
+		sample_sum += data[i];
+		statP->min = min(statP->min, data[i]);
+		statP->max = max(statP->max, data[i]);
+	}
+
+	statP->mean = sample_sum / length;
+	sample_sum = 0;
+	for (i = 0; i < length; i++) {
+		tmp = data[i] - statP->mean;
+		sample_sum += tmp * tmp;
+	}
+	statP->stddev = int_sqrt((long)sample_sum)/length;
+}
+
+/**
+ * tsl2x7x_prox_cal() - Calculates std. and sets thresholds.
+ * @indio_dev:	pointer to IIO device
+ *
+ * Calculates a standard deviation based on the samples,
+ * and sets the threshold accordingly.
+ */
+static void tsl2x7x_prox_cal(struct iio_dev *indio_dev)
+{
+	int prox_history[MAX_SAMPLES_CAL + 1];
+	int i;
+	struct tsl2x7x_prox_stat prox_stat_data[2];
+	struct tsl2x7x_prox_stat *calP;
+	struct tsl2X7X_chip *chip = iio_priv(indio_dev);
+	u8 tmp_irq_settings;
+	u8 current_state = chip->tsl2x7x_chip_status;
+
+	if (chip->tsl2x7x_settings.prox_max_samples_cal > MAX_SAMPLES_CAL) {
+		dev_err(&chip->client->dev,
+			"%s: max prox samples cal is too big: %d\n",
+			__func__, chip->tsl2x7x_settings.prox_max_samples_cal);
+		chip->tsl2x7x_settings.prox_max_samples_cal = MAX_SAMPLES_CAL;
+	}
+
+	/* have to stop to change settings */
+	tsl2x7x_chip_off(indio_dev);
+
+	/* Enable proximity detection save just in case prox not wanted yet*/
+	tmp_irq_settings = chip->tsl2x7x_settings.interrupts_en;
+	chip->tsl2x7x_settings.interrupts_en |= TSL2X7X_CNTL_PROX_INT_ENBL;
+
+	/*turn on device if not already on*/
+	tsl2x7x_chip_on(indio_dev);
+
+	/*gather the samples*/
+	for (i = 0; i < chip->tsl2x7x_settings.prox_max_samples_cal; i++) {
+		mdelay(15);
+		tsl2x7x_get_prox(indio_dev);
+		prox_history[i] = chip->prox_data;
+		dev_info(&chip->client->dev, "2 i=%d prox data= %d\n",
+			i, chip->prox_data);
+	}
+
+	tsl2x7x_chip_off(indio_dev);
+	calP = &prox_stat_data[PROX_STAT_CAL];
+	tsl2x7x_prox_calculate(prox_history,
+		chip->tsl2x7x_settings.prox_max_samples_cal, calP);
+	chip->tsl2x7x_settings.prox_thres_high = (calP->max << 1) - calP->mean;
+
+	dev_info(&chip->client->dev, " cal min=%d mean=%d max=%d\n",
+		calP->min, calP->mean, calP->max);
+	dev_info(&chip->client->dev,
+		"%s proximity threshold set to %d\n",
+		chip->client->name, chip->tsl2x7x_settings.prox_thres_high);
+
+	/* back to the way they were */
+	chip->tsl2x7x_settings.interrupts_en = tmp_irq_settings;
+	if (current_state == TSL2X7X_CHIP_WORKING)
+		tsl2x7x_chip_on(indio_dev);
+}
+
+static ssize_t tsl2x7x_power_state_show(struct device *dev,
+	struct device_attribute *attr, char *buf)
+{
+	struct tsl2X7X_chip *chip = iio_priv(dev_to_iio_dev(dev));
+
+	return snprintf(buf, PAGE_SIZE, "%d\n", chip->tsl2x7x_chip_status);
+}
+
+static ssize_t tsl2x7x_power_state_store(struct device *dev,
+	struct device_attribute *attr, const char *buf, size_t len)
+{
+	struct iio_dev *indio_dev = dev_to_iio_dev(dev);
+	bool value;
+
+	if (strtobool(buf, &value))
+		return -EINVAL;
+
+	if (value)
+		tsl2x7x_chip_on(indio_dev);
+	else
+		tsl2x7x_chip_off(indio_dev);
+
+	return len;
+}
+
+static ssize_t tsl2x7x_gain_available_show(struct device *dev,
+	struct device_attribute *attr, char *buf)
+{
+	struct tsl2X7X_chip *chip = iio_priv(dev_to_iio_dev(dev));
+
+	switch (chip->id) {
+	case tsl2571:
+	case tsl2671:
+	case tmd2671:
+	case tsl2771:
+	case tmd2771:
+		return snprintf(buf, PAGE_SIZE, "%s\n", "1 8 16 128");
+	break;
+	}
+
+	return snprintf(buf, PAGE_SIZE, "%s\n", "1 8 16 120");
+}
+
+static ssize_t tsl2x7x_prox_gain_available_show(struct device *dev,
+	struct device_attribute *attr, char *buf)
+{
+		return snprintf(buf, PAGE_SIZE, "%s\n", "1 2 4 8");
+}
+
+static ssize_t tsl2x7x_als_time_show(struct device *dev,
+	struct device_attribute *attr, char *buf)
+{
+	struct tsl2X7X_chip *chip = iio_priv(dev_to_iio_dev(dev));
+	int y, z;
+
+	y = (TSL2X7X_MAX_TIMER_CNT - (u8)chip->tsl2x7x_settings.als_time) + 1;
+	z = y * TSL2X7X_MIN_ITIME;
+	y /= 1000;
+	z %= 1000;
+
+	return snprintf(buf, PAGE_SIZE, "%d.%03d\n", y, z);
+}
+
+static ssize_t tsl2x7x_als_time_store(struct device *dev,
+	struct device_attribute *attr, const char *buf, size_t len)
+{
+	struct iio_dev *indio_dev = dev_to_iio_dev(dev);
+	struct tsl2X7X_chip *chip = iio_priv(indio_dev);
+	struct tsl2x7x_parse_result result;
+
+	result.integer = 0;
+	result.fract = 0;
+
+	tsl2x7x_parse_buffer(buf, &result);
+
+	result.fract /= 1000;
+	result.fract /= 3;
+	chip->tsl2x7x_settings.als_time =
+			(TSL2X7X_MAX_TIMER_CNT - (u8)result.fract);
+
+	dev_info(&chip->client->dev, "%s: als time = %d",
+		__func__, chip->tsl2x7x_settings.als_time);
+
+	tsl2x7x_invoke_change(indio_dev);
+
+	return IIO_VAL_INT_PLUS_MICRO;
+}
+
+static IIO_CONST_ATTR(in_illuminance0_integration_time_available,
+		".00272 - .696");
+
+static ssize_t tsl2x7x_als_cal_target_show(struct device *dev,
+	struct device_attribute *attr, char *buf)
+{
+	struct tsl2X7X_chip *chip = iio_priv(dev_to_iio_dev(dev));
+
+	return snprintf(buf, PAGE_SIZE, "%d\n",
+			chip->tsl2x7x_settings.als_cal_target);
+}
+
+static ssize_t tsl2x7x_als_cal_target_store(struct device *dev,
+	struct device_attribute *attr, const char *buf, size_t len)
+{
+	struct iio_dev *indio_dev = dev_to_iio_dev(dev);
+	struct tsl2X7X_chip *chip = iio_priv(indio_dev);
+	unsigned long value;
+
+	if (kstrtoul(buf, 0, &value))
+		return -EINVAL;
+
+	if (value)
+		chip->tsl2x7x_settings.als_cal_target = value;
+
+	tsl2x7x_invoke_change(indio_dev);
+
+	return len;
+}
+
+/* persistence settings */
+static ssize_t tsl2x7x_als_persistence_show(struct device *dev,
+	struct device_attribute *attr, char *buf)
+{
+	struct tsl2X7X_chip *chip = iio_priv(dev_to_iio_dev(dev));
+	int y, z, filter_delay;
+
+	/* Determine integration time */
+	y = (TSL2X7X_MAX_TIMER_CNT - (u8)chip->tsl2x7x_settings.als_time) + 1;
+	z = y * TSL2X7X_MIN_ITIME;
+	filter_delay = z * (chip->tsl2x7x_settings.persistence & 0x0F);
+	y = (filter_delay / 1000);
+	z = (filter_delay % 1000);
+
+	return snprintf(buf, PAGE_SIZE, "%d.%03d\n", y, z);
+}
+
+static ssize_t tsl2x7x_als_persistence_store(struct device *dev,
+	struct device_attribute *attr, const char *buf, size_t len)
+{
+	struct iio_dev *indio_dev = dev_to_iio_dev(dev);
+	struct tsl2X7X_chip *chip = iio_priv(indio_dev);
+	struct tsl2x7x_parse_result result;
+	int y, z, filter_delay;
+
+	result.integer = 0;
+	result.fract = 0;
+	tsl2x7x_parse_buffer(buf, &result);
+
+	result.fract /= 1000;
+	y = (TSL2X7X_MAX_TIMER_CNT - (u8)chip->tsl2x7x_settings.als_time) + 1;
+	z = y * TSL2X7X_MIN_ITIME;
+
+	filter_delay =
+		DIV_ROUND_UP(((result.integer * 1000) + result.fract), z);
+
+	chip->tsl2x7x_settings.persistence &= 0xF0;
+	chip->tsl2x7x_settings.persistence |= (filter_delay & 0x0F);
+
+	dev_info(&chip->client->dev, "%s: als persistence = %d",
+		__func__, filter_delay);
+
+	tsl2x7x_invoke_change(indio_dev);
+
+	return IIO_VAL_INT_PLUS_MICRO;
+}
+
+static ssize_t tsl2x7x_prox_persistence_show(struct device *dev,
+	struct device_attribute *attr, char *buf)
+{
+	struct tsl2X7X_chip *chip = iio_priv(dev_to_iio_dev(dev));
+	int y, z, filter_delay;
+
+	/* Determine integration time */
+	y = (TSL2X7X_MAX_TIMER_CNT - (u8)chip->tsl2x7x_settings.prx_time) + 1;
+	z = y * TSL2X7X_MIN_ITIME;
+	filter_delay = z * ((chip->tsl2x7x_settings.persistence & 0xF0) >> 4);
+	y = (filter_delay / 1000);
+	z = (filter_delay % 1000);
+
+	return snprintf(buf, PAGE_SIZE, "%d.%03d\n", y, z);
+}
+
+static ssize_t tsl2x7x_prox_persistence_store(struct device *dev,
+	struct device_attribute *attr, const char *buf, size_t len)
+{
+	struct iio_dev *indio_dev = dev_to_iio_dev(dev);
+	struct tsl2X7X_chip *chip = iio_priv(indio_dev);
+	struct tsl2x7x_parse_result result;
+	int y, z, filter_delay;
+
+	result.integer = 0;
+	result.fract = 0;
+	tsl2x7x_parse_buffer(buf, &result);
+
+	result.fract /= 1000;
+	y = (TSL2X7X_MAX_TIMER_CNT - (u8)chip->tsl2x7x_settings.prx_time) + 1;
+	z = y * TSL2X7X_MIN_ITIME;
+
+	filter_delay =
+		DIV_ROUND_UP(((result.integer * 1000) + result.fract), z);
+
+	chip->tsl2x7x_settings.persistence &= 0x0F;
+	chip->tsl2x7x_settings.persistence |= ((filter_delay << 4) & 0xF0);
+
+	dev_info(&chip->client->dev, "%s: prox persistence = %d",
+		__func__, filter_delay);
+
+	tsl2x7x_invoke_change(indio_dev);
+
+	return IIO_VAL_INT_PLUS_MICRO;
+}
+
+static ssize_t tsl2x7x_do_calibrate(struct device *dev,
+	struct device_attribute *attr, const char *buf, size_t len)
+{
+	struct iio_dev *indio_dev = dev_to_iio_dev(dev);
+	bool value;
+
+	if (strtobool(buf, &value))
+		return -EINVAL;
+
+	if (value)
+		tsl2x7x_als_calibrate(indio_dev);
+
+	tsl2x7x_invoke_change(indio_dev);
+
+	return len;
+}
+
+static ssize_t tsl2x7x_luxtable_show(struct device *dev,
+	struct device_attribute *attr, char *buf)
+{
+	struct tsl2X7X_chip *chip = iio_priv(dev_to_iio_dev(dev));
+	int i = 0;
+	int offset = 0;
+
+	while (i < (TSL2X7X_MAX_LUX_TABLE_SIZE * 3)) {
+		offset += snprintf(buf + offset, PAGE_SIZE, "%d,%d,%d,",
+			chip->tsl2x7x_device_lux[i].ratio,
+			chip->tsl2x7x_device_lux[i].ch0,
+			chip->tsl2x7x_device_lux[i].ch1);
+		if (chip->tsl2x7x_device_lux[i].ratio == 0) {
+			/* We just printed the first "0" entry.
+			 * Now get rid of the extra "," and break. */
+			offset--;
+			break;
+		}
+		i++;
+	}
+
+	offset += snprintf(buf + offset, PAGE_SIZE, "\n");
+	return offset;
+}
+
+static ssize_t tsl2x7x_luxtable_store(struct device *dev,
+	struct device_attribute *attr, const char *buf, size_t len)
+{
+	struct iio_dev *indio_dev = dev_to_iio_dev(dev);
+	struct tsl2X7X_chip *chip = iio_priv(indio_dev);
+	int value[ARRAY_SIZE(chip->tsl2x7x_device_lux)*3 + 1];
+	int n;
+
+	get_options(buf, ARRAY_SIZE(value), value);
+
+	/* We now have an array of ints starting at value[1], and
+	 * enumerated by value[0].
+	 * We expect each group of three ints is one table entry,
+	 * and the last table entry is all 0.
+	 */
+	n = value[0];
+	if ((n % 3) || n < 6 ||
+			n > ((ARRAY_SIZE(chip->tsl2x7x_device_lux) - 1) * 3)) {
+		dev_info(dev, "LUX TABLE INPUT ERROR 1 Value[0]=%d\n", n);
+		return -EINVAL;
+	}
+
+	if ((value[(n - 2)] | value[(n - 1)] | value[n]) != 0) {
+		dev_info(dev, "LUX TABLE INPUT ERROR 2 Value[0]=%d\n", n);
+		return -EINVAL;
+	}
+
+	if (chip->tsl2x7x_chip_status == TSL2X7X_CHIP_WORKING)
+		tsl2x7x_chip_off(indio_dev);
+
+	/* Zero out the table */
+	memset(chip->tsl2x7x_device_lux, 0, sizeof(chip->tsl2x7x_device_lux));
+	memcpy(chip->tsl2x7x_device_lux, &value[1], (value[0] * 4));
+
+	tsl2x7x_invoke_change(indio_dev);
+
+	return len;
+}
+
+static ssize_t tsl2x7x_do_prox_calibrate(struct device *dev,
+	struct device_attribute *attr, const char *buf, size_t len)
+{
+	struct iio_dev *indio_dev = dev_to_iio_dev(dev);
+	bool value;
+
+	if (strtobool(buf, &value))
+		return -EINVAL;
+
+	if (value)
+		tsl2x7x_prox_cal(indio_dev);
+
+	tsl2x7x_invoke_change(indio_dev);
+
+	return len;
+}
+
+static int tsl2x7x_read_interrupt_config(struct iio_dev *indio_dev,
+					 u64 event_code)
+{
+	struct tsl2X7X_chip *chip = iio_priv(indio_dev);
+	int ret;
+
+	if (IIO_EVENT_CODE_EXTRACT_CHAN_TYPE(event_code) == IIO_INTENSITY)
+		ret = !!(chip->tsl2x7x_settings.interrupts_en & 0x10);
+	else
+		ret = !!(chip->tsl2x7x_settings.interrupts_en & 0x20);
+
+	return ret;
+}
+
+static int tsl2x7x_write_interrupt_config(struct iio_dev *indio_dev,
+					  u64 event_code,
+					  int val)
+{
+	struct tsl2X7X_chip *chip = iio_priv(indio_dev);
+
+	if (IIO_EVENT_CODE_EXTRACT_CHAN_TYPE(event_code) == IIO_INTENSITY) {
+		if (val)
+			chip->tsl2x7x_settings.interrupts_en |= 0x10;
+		else
+			chip->tsl2x7x_settings.interrupts_en &= 0x20;
+	} else {
+		if (val)
+			chip->tsl2x7x_settings.interrupts_en |= 0x20;
+		else
+			chip->tsl2x7x_settings.interrupts_en &= 0x10;
+	}
+
+	tsl2x7x_invoke_change(indio_dev);
+
+	return 0;
+}
+
+static int tsl2x7x_write_thresh(struct iio_dev *indio_dev,
+				  u64 event_code,
+				  int val)
+{
+	struct tsl2X7X_chip *chip = iio_priv(indio_dev);
+
+	if (IIO_EVENT_CODE_EXTRACT_CHAN_TYPE(event_code) == IIO_INTENSITY) {
+		switch (IIO_EVENT_CODE_EXTRACT_DIR(event_code)) {
+		case IIO_EV_DIR_RISING:
+			chip->tsl2x7x_settings.als_thresh_high = val;
+			break;
+		case IIO_EV_DIR_FALLING:
+			chip->tsl2x7x_settings.als_thresh_low = val;
+			break;
+		default:
+			return -EINVAL;
+		}
+	} else {
+		switch (IIO_EVENT_CODE_EXTRACT_DIR(event_code)) {
+		case IIO_EV_DIR_RISING:
+			chip->tsl2x7x_settings.prox_thres_high = val;
+			break;
+		case IIO_EV_DIR_FALLING:
+			chip->tsl2x7x_settings.prox_thres_low = val;
+			break;
+		default:
+			return -EINVAL;
+		}
+	}
+
+	tsl2x7x_invoke_change(indio_dev);
+
+	return 0;
+}
+
+static int tsl2x7x_read_thresh(struct iio_dev *indio_dev,
+			       u64 event_code,
+			       int *val)
+{
+	struct tsl2X7X_chip *chip = iio_priv(indio_dev);
+
+	if (IIO_EVENT_CODE_EXTRACT_CHAN_TYPE(event_code) == IIO_INTENSITY) {
+		switch (IIO_EVENT_CODE_EXTRACT_DIR(event_code)) {
+		case IIO_EV_DIR_RISING:
+			*val = chip->tsl2x7x_settings.als_thresh_high;
+			break;
+		case IIO_EV_DIR_FALLING:
+			*val = chip->tsl2x7x_settings.als_thresh_low;
+			break;
+		default:
+			return -EINVAL;
+		}
+	} else {
+		switch (IIO_EVENT_CODE_EXTRACT_DIR(event_code)) {
+		case IIO_EV_DIR_RISING:
+			*val = chip->tsl2x7x_settings.prox_thres_high;
+			break;
+		case IIO_EV_DIR_FALLING:
+			*val = chip->tsl2x7x_settings.prox_thres_low;
+			break;
+		default:
+			return -EINVAL;
+		}
+	}
+
+	return 0;
+}
+
+static int tsl2x7x_read_raw(struct iio_dev *indio_dev,
+			    struct iio_chan_spec const *chan,
+			    int *val,
+			    int *val2,
+			    long mask)
+{
+	int ret = -EINVAL;
+	struct tsl2X7X_chip *chip = iio_priv(indio_dev);
+
+	switch (mask) {
+	case IIO_CHAN_INFO_PROCESSED:
+		switch (chan->type) {
+		case IIO_LIGHT:
+			tsl2x7x_get_lux(indio_dev);
+			*val = chip->als_cur_info.lux;
+			ret = IIO_VAL_INT;
+			break;
+		default:
+			return -EINVAL;
+			break;
+		}
+		break;
+	case IIO_CHAN_INFO_RAW:
+		switch (chan->type) {
+		case IIO_INTENSITY:
+			tsl2x7x_get_lux(indio_dev);
+			if (chan->channel == 0)
+				*val = chip->als_cur_info.als_ch0;
+			else
+				*val = chip->als_cur_info.als_ch1;
+			ret = IIO_VAL_INT;
+			break;
+		case IIO_PROXIMITY:
+			tsl2x7x_get_prox(indio_dev);
+			*val = chip->prox_data;
+			ret = IIO_VAL_INT;
+			break;
+		default:
+			return -EINVAL;
+			break;
+		}
+		break;
+	case IIO_CHAN_INFO_CALIBSCALE:
+		if (chan->type == IIO_LIGHT)
+			*val =
+			tsl2X7X_als_gainadj[chip->tsl2x7x_settings.als_gain];
+		else
+			*val =
+			tsl2X7X_prx_gainadj[chip->tsl2x7x_settings.prox_gain];
+		ret = IIO_VAL_INT;
+		break;
+	case IIO_CHAN_INFO_CALIBBIAS:
+		*val = chip->tsl2x7x_settings.als_gain_trim;
+		ret = IIO_VAL_INT;
+		break;
+
+	default:
+		ret = -EINVAL;
+	}
+
+	return ret;
+}
+
+static int tsl2x7x_write_raw(struct iio_dev *indio_dev,
+			       struct iio_chan_spec const *chan,
+			       int val,
+			       int val2,
+			       long mask)
+{
+	struct tsl2X7X_chip *chip = iio_priv(indio_dev);
+
+	switch (mask) {
+	case IIO_CHAN_INFO_CALIBSCALE:
+		if (chan->type == IIO_INTENSITY) {
+			switch (val) {
+			case 1:
+				chip->tsl2x7x_settings.als_gain = 0;
+				break;
+			case 8:
+				chip->tsl2x7x_settings.als_gain = 1;
+				break;
+			case 16:
+				chip->tsl2x7x_settings.als_gain = 2;
+				break;
+			case 120:
+				switch (chip->id) {
+				case tsl2572:
+				case tsl2672:
+				case tmd2672:
+				case tsl2772:
+				case tmd2772:
+					return -EINVAL;
+				break;
+				}
+				chip->tsl2x7x_settings.als_gain = 3;
+				break;
+			case 128:
+				switch (chip->id) {
+				case tsl2571:
+				case tsl2671:
+				case tmd2671:
+				case tsl2771:
+				case tmd2771:
+					return -EINVAL;
+				break;
+				}
+				chip->tsl2x7x_settings.als_gain = 3;
+				break;
+			default:
+				return -EINVAL;
+			}
+		} else {
+			switch (val) {
+			case 1:
+				chip->tsl2x7x_settings.prox_gain = 0;
+				break;
+			case 2:
+				chip->tsl2x7x_settings.prox_gain = 1;
+				break;
+			case 4:
+				chip->tsl2x7x_settings.prox_gain = 2;
+				break;
+			case 8:
+				chip->tsl2x7x_settings.prox_gain = 3;
+				break;
+			default:
+				return -EINVAL;
+			}
+		}
+		break;
+	case IIO_CHAN_INFO_CALIBBIAS:
+		chip->tsl2x7x_settings.als_gain_trim = val;
+		break;
+
+	default:
+		return -EINVAL;
+	}
+
+	tsl2x7x_invoke_change(indio_dev);
+
+	return 0;
+}
+
+static DEVICE_ATTR(power_state, S_IRUGO | S_IWUSR,
+		tsl2x7x_power_state_show, tsl2x7x_power_state_store);
+
+static DEVICE_ATTR(in_proximity0_calibscale_available, S_IRUGO,
+		tsl2x7x_prox_gain_available_show, NULL);
+
+static DEVICE_ATTR(in_illuminance0_calibscale_available, S_IRUGO,
+		tsl2x7x_gain_available_show, NULL);
+
+static DEVICE_ATTR(in_illuminance0_integration_time, S_IRUGO | S_IWUSR,
+		tsl2x7x_als_time_show, tsl2x7x_als_time_store);
+
+static DEVICE_ATTR(in_illuminance0_target_input, S_IRUGO | S_IWUSR,
+		tsl2x7x_als_cal_target_show, tsl2x7x_als_cal_target_store);
+
+static DEVICE_ATTR(in_illuminance0_calibrate, S_IWUSR, NULL,
+		tsl2x7x_do_calibrate);
+
+static DEVICE_ATTR(in_proximity0_calibrate, S_IWUSR, NULL,
+		tsl2x7x_do_prox_calibrate);
+
+static DEVICE_ATTR(in_illuminance0_lux_table, S_IRUGO | S_IWUSR,
+		tsl2x7x_luxtable_show, tsl2x7x_luxtable_store);
+
+static DEVICE_ATTR(in_intensity0_thresh_period, S_IRUGO | S_IWUSR,
+		tsl2x7x_als_persistence_show, tsl2x7x_als_persistence_store);
+
+static DEVICE_ATTR(in_proximity0_thresh_period, S_IRUGO | S_IWUSR,
+		tsl2x7x_prox_persistence_show, tsl2x7x_prox_persistence_store);
+
+/* Use the default register values to identify the Taos device */
+static int tsl2x7x_device_id(unsigned char *id, int target)
+{
+	switch (target) {
+	case tsl2571:
+	case tsl2671:
+	case tsl2771:
+		return ((*id & 0xf0) == TRITON_ID);
+	break;
+	case tmd2671:
+	case tmd2771:
+		return ((*id & 0xf0) == HALIBUT_ID);
+	break;
+	case tsl2572:
+	case tsl2672:
+	case tmd2672:
+	case tsl2772:
+	case tmd2772:
+		return ((*id & 0xf0) == SWORDFISH_ID);
+	break;
+	}
+
+	return -EINVAL;
+}
+
+static irqreturn_t tsl2x7x_event_handler(int irq, void *private)
+{
+	struct iio_dev *indio_dev = private;
+	struct tsl2X7X_chip *chip = iio_priv(indio_dev);
+	s64 timestamp = iio_get_time_ns();
+	int ret;
+	u8 value;
+
+	value = i2c_smbus_read_byte_data(chip->client,
+		TSL2X7X_CMD_REG | TSL2X7X_STATUS);
+
+	/* What type of interrupt do we need to process */
+	if (value & TSL2X7X_STA_PRX_INTR) {
+		tsl2x7x_get_prox(indio_dev); /* freshen data for ABI */
+		iio_push_event(indio_dev,
+			       IIO_UNMOD_EVENT_CODE(IIO_PROXIMITY,
+						    0,
+						    IIO_EV_TYPE_THRESH,
+						    IIO_EV_DIR_EITHER),
+						    timestamp);
+	}
+
+	if (value & TSL2X7X_STA_ALS_INTR) {
+		tsl2x7x_get_lux(indio_dev); /* freshen data for ABI */
+		iio_push_event(indio_dev,
+		       IIO_UNMOD_EVENT_CODE(IIO_LIGHT,
+					    0,
+					    IIO_EV_TYPE_THRESH,
+					    IIO_EV_DIR_EITHER),
+					    timestamp);
+	}
+	/* Clear interrupt now that we have handled it. */
+	ret = i2c_smbus_write_byte(chip->client,
+		TSL2X7X_CMD_REG | TSL2X7X_CMD_SPL_FN |
+		TSL2X7X_CMD_PROXALS_INT_CLR);
+	if (ret < 0)
+		dev_err(&chip->client->dev,
+			"%s: Failed to clear irq from event handler. err = %d\n",
+			__func__, ret);
+
+	return IRQ_HANDLED;
+}
+
+static struct attribute *tsl2x7x_ALS_device_attrs[] = {
+	&dev_attr_power_state.attr,
+	&dev_attr_in_illuminance0_calibscale_available.attr,
+	&dev_attr_in_illuminance0_integration_time.attr,
+	&iio_const_attr_in_illuminance0_integration_time_available\
+	.dev_attr.attr,
+	&dev_attr_in_illuminance0_target_input.attr,
+	&dev_attr_in_illuminance0_calibrate.attr,
+	&dev_attr_in_illuminance0_lux_table.attr,
+	NULL
+};
+
+static struct attribute *tsl2x7x_PRX_device_attrs[] = {
+	&dev_attr_power_state.attr,
+	&dev_attr_in_proximity0_calibrate.attr,
+	NULL
+};
+
+static struct attribute *tsl2x7x_ALSPRX_device_attrs[] = {
+	&dev_attr_power_state.attr,
+	&dev_attr_in_illuminance0_calibscale_available.attr,
+	&dev_attr_in_illuminance0_integration_time.attr,
+	&iio_const_attr_in_illuminance0_integration_time_available\
+	.dev_attr.attr,
+	&dev_attr_in_illuminance0_target_input.attr,
+	&dev_attr_in_illuminance0_calibrate.attr,
+	&dev_attr_in_illuminance0_lux_table.attr,
+	&dev_attr_in_proximity0_calibrate.attr,
+	NULL
+};
+
+static struct attribute *tsl2x7x_PRX2_device_attrs[] = {
+	&dev_attr_power_state.attr,
+	&dev_attr_in_proximity0_calibrate.attr,
+	&dev_attr_in_proximity0_calibscale_available.attr,
+	NULL
+};
+
+static struct attribute *tsl2x7x_ALSPRX2_device_attrs[] = {
+	&dev_attr_power_state.attr,
+	&dev_attr_in_illuminance0_calibscale_available.attr,
+	&dev_attr_in_illuminance0_integration_time.attr,
+	&iio_const_attr_in_illuminance0_integration_time_available\
+	.dev_attr.attr,
+	&dev_attr_in_illuminance0_target_input.attr,
+	&dev_attr_in_illuminance0_calibrate.attr,
+	&dev_attr_in_illuminance0_lux_table.attr,
+	&dev_attr_in_proximity0_calibrate.attr,
+	&dev_attr_in_proximity0_calibscale_available.attr,
+	NULL
+};
+
+static struct attribute *tsl2X7X_ALS_event_attrs[] = {
+	&dev_attr_in_intensity0_thresh_period.attr,
+	NULL,
+};
+static struct attribute *tsl2X7X_PRX_event_attrs[] = {
+	&dev_attr_in_proximity0_thresh_period.attr,
+	NULL,
+};
+
+static struct attribute *tsl2X7X_ALSPRX_event_attrs[] = {
+	&dev_attr_in_intensity0_thresh_period.attr,
+	&dev_attr_in_proximity0_thresh_period.attr,
+	NULL,
+};
+
+static const struct attribute_group tsl2X7X_device_attr_group_tbl[] = {
+	[ALS] = {
+		.attrs = tsl2x7x_ALS_device_attrs,
+	},
+	[PRX] = {
+		.attrs = tsl2x7x_PRX_device_attrs,
+	},
+	[ALSPRX] = {
+		.attrs = tsl2x7x_ALSPRX_device_attrs,
+	},
+	[PRX2] = {
+		.attrs = tsl2x7x_PRX2_device_attrs,
+	},
+	[ALSPRX2] = {
+		.attrs = tsl2x7x_ALSPRX2_device_attrs,
+	},
+};
+
+static struct attribute_group tsl2X7X_event_attr_group_tbl[] = {
+	[ALS] = {
+		.attrs = tsl2X7X_ALS_event_attrs,
+		.name = "events",
+	},
+	[PRX] = {
+		.attrs = tsl2X7X_PRX_event_attrs,
+		.name = "events",
+	},
+	[ALSPRX] = {
+		.attrs = tsl2X7X_ALSPRX_event_attrs,
+		.name = "events",
+	},
+};
+
+static const struct iio_info tsl2X7X_device_info[] = {
+	[ALS] = {
+		.attrs = &tsl2X7X_device_attr_group_tbl[ALS],
+		.event_attrs = &tsl2X7X_event_attr_group_tbl[ALS],
+		.driver_module = THIS_MODULE,
+		.read_raw = &tsl2x7x_read_raw,
+		.write_raw = &tsl2x7x_write_raw,
+		.read_event_value = &tsl2x7x_read_thresh,
+		.write_event_value = &tsl2x7x_write_thresh,
+		.read_event_config = &tsl2x7x_read_interrupt_config,
+		.write_event_config = &tsl2x7x_write_interrupt_config,
+	},
+	[PRX] = {
+		.attrs = &tsl2X7X_device_attr_group_tbl[PRX],
+		.event_attrs = &tsl2X7X_event_attr_group_tbl[PRX],
+		.driver_module = THIS_MODULE,
+		.read_raw = &tsl2x7x_read_raw,
+		.write_raw = &tsl2x7x_write_raw,
+		.read_event_value = &tsl2x7x_read_thresh,
+		.write_event_value = &tsl2x7x_write_thresh,
+		.read_event_config = &tsl2x7x_read_interrupt_config,
+		.write_event_config = &tsl2x7x_write_interrupt_config,
+	},
+	[ALSPRX] = {
+		.attrs = &tsl2X7X_device_attr_group_tbl[ALSPRX],
+		.event_attrs = &tsl2X7X_event_attr_group_tbl[ALSPRX],
+		.driver_module = THIS_MODULE,
+		.read_raw = &tsl2x7x_read_raw,
+		.write_raw = &tsl2x7x_write_raw,
+		.read_event_value = &tsl2x7x_read_thresh,
+		.write_event_value = &tsl2x7x_write_thresh,
+		.read_event_config = &tsl2x7x_read_interrupt_config,
+		.write_event_config = &tsl2x7x_write_interrupt_config,
+	},
+	[PRX2] = {
+		.attrs = &tsl2X7X_device_attr_group_tbl[PRX2],
+		.event_attrs = &tsl2X7X_event_attr_group_tbl[PRX],
+		.driver_module = THIS_MODULE,
+		.read_raw = &tsl2x7x_read_raw,
+		.write_raw = &tsl2x7x_write_raw,
+		.read_event_value = &tsl2x7x_read_thresh,
+		.write_event_value = &tsl2x7x_write_thresh,
+		.read_event_config = &tsl2x7x_read_interrupt_config,
+		.write_event_config = &tsl2x7x_write_interrupt_config,
+	},
+	[ALSPRX2] = {
+		.attrs = &tsl2X7X_device_attr_group_tbl[ALSPRX2],
+		.event_attrs = &tsl2X7X_event_attr_group_tbl[ALSPRX],
+		.driver_module = THIS_MODULE,
+		.read_raw = &tsl2x7x_read_raw,
+		.write_raw = &tsl2x7x_write_raw,
+		.read_event_value = &tsl2x7x_read_thresh,
+		.write_event_value = &tsl2x7x_write_thresh,
+		.read_event_config = &tsl2x7x_read_interrupt_config,
+		.write_event_config = &tsl2x7x_write_interrupt_config,
+	},
+};
+
+static const struct tsl2x7x_chip_info tsl2x7x_chip_info_tbl[] = {
+	[ALS] = {
+		.channel = {
+			{
+			.type = IIO_LIGHT,
+			.indexed = 1,
+			.channel = 0,
+			.info_mask = IIO_CHAN_INFO_PROCESSED_SEPARATE_BIT,
+			}, {
+			.type = IIO_INTENSITY,
+			.indexed = 1,
+			.channel = 0,
+			.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT |
+				IIO_CHAN_INFO_CALIBSCALE_SEPARATE_BIT |
+				IIO_CHAN_INFO_CALIBBIAS_SEPARATE_BIT,
+			.event_mask = TSL2X7X_EVENT_MASK
+			}, {
+			.type = IIO_INTENSITY,
+			.indexed = 1,
+			.channel = 1,
+			},
+		},
+	.chan_table_elements = 3,
+	.info = &tsl2X7X_device_info[ALS],
+	},
+	[PRX] = {
+		.channel = {
+			{
+			.type = IIO_PROXIMITY,
+			.indexed = 1,
+			.channel = 0,
+			.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT,
+			.event_mask = TSL2X7X_EVENT_MASK
+			},
+		},
+	.chan_table_elements = 1,
+	.info = &tsl2X7X_device_info[PRX],
+	},
+	[ALSPRX] = {
+		.channel = {
+			{
+			.type = IIO_LIGHT,
+			.indexed = 1,
+			.channel = 0,
+			.info_mask = IIO_CHAN_INFO_PROCESSED_SEPARATE_BIT
+			}, {
+			.type = IIO_INTENSITY,
+			.indexed = 1,
+			.channel = 0,
+			.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT |
+				IIO_CHAN_INFO_CALIBSCALE_SEPARATE_BIT |
+				IIO_CHAN_INFO_CALIBBIAS_SEPARATE_BIT,
+			.event_mask = TSL2X7X_EVENT_MASK
+			}, {
+			.type = IIO_INTENSITY,
+			.indexed = 1,
+			.channel = 1,
+			.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT,
+			}, {
+			.type = IIO_PROXIMITY,
+			.indexed = 1,
+			.channel = 0,
+			.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT,
+			.event_mask = TSL2X7X_EVENT_MASK
+			},
+		},
+	.chan_table_elements = 4,
+	.info = &tsl2X7X_device_info[ALSPRX],
+	},
+	[PRX2] = {
+		.channel = {
+			{
+			.type = IIO_PROXIMITY,
+			.indexed = 1,
+			.channel = 0,
+			.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT |
+				IIO_CHAN_INFO_CALIBSCALE_SEPARATE_BIT,
+			.event_mask = TSL2X7X_EVENT_MASK
+			},
+		},
+	.chan_table_elements = 1,
+	.info = &tsl2X7X_device_info[PRX2],
+	},
+	[ALSPRX2] = {
+		.channel = {
+			{
+			.type = IIO_LIGHT,
+			.indexed = 1,
+			.channel = 0,
+			.info_mask = IIO_CHAN_INFO_PROCESSED_SEPARATE_BIT,
+			}, {
+			.type = IIO_INTENSITY,
+			.indexed = 1,
+			.channel = 0,
+			.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT |
+				IIO_CHAN_INFO_CALIBSCALE_SEPARATE_BIT |
+				IIO_CHAN_INFO_CALIBBIAS_SEPARATE_BIT,
+			.event_mask = TSL2X7X_EVENT_MASK
+			}, {
+			.type = IIO_INTENSITY,
+			.indexed = 1,
+			.channel = 1,
+			.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT,
+			}, {
+			.type = IIO_PROXIMITY,
+			.indexed = 1,
+			.channel = 0,
+			.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT |
+				IIO_CHAN_INFO_CALIBSCALE_SEPARATE_BIT,
+			.event_mask = TSL2X7X_EVENT_MASK
+			},
+		},
+	.chan_table_elements = 4,
+	.info = &tsl2X7X_device_info[ALSPRX2],
+	},
+};
+
+static int __devinit tsl2x7x_probe(struct i2c_client *clientp,
+	const struct i2c_device_id *id)
+{
+	int ret;
+	unsigned char device_id;
+	struct iio_dev *indio_dev;
+	struct tsl2X7X_chip *chip;
+
+	indio_dev = iio_device_alloc(sizeof(*chip));
+	if (!indio_dev)
+		return -ENOMEM;
+
+	chip = iio_priv(indio_dev);
+	chip->client = clientp;
+	i2c_set_clientdata(clientp, indio_dev);
+
+	ret = tsl2x7x_i2c_read(chip->client,
+		TSL2X7X_CHIPID, &device_id);
+	if (ret < 0)
+		goto fail1;
+
+	if ((!tsl2x7x_device_id(&device_id, id->driver_data)) ||
+		(tsl2x7x_device_id(&device_id, id->driver_data) == -EINVAL)) {
+		dev_info(&chip->client->dev,
+				"%s: i2c device found does not match expected id\n",
+				__func__);
+		goto fail1;
+	}
+
+	ret = i2c_smbus_write_byte(clientp, (TSL2X7X_CMD_REG | TSL2X7X_CNTRL));
+	if (ret < 0) {
+		dev_err(&clientp->dev, "%s: write to cmd reg failed. err = %d\n",
+				__func__, ret);
+		goto fail1;
+	}
+
+	/* ALS and PROX functions can be invoked via user space poll
+	 * or H/W interrupt. If busy return last sample. */
+	mutex_init(&chip->als_mutex);
+	mutex_init(&chip->prox_mutex);
+
+	chip->tsl2x7x_chip_status = TSL2X7X_CHIP_UNKNOWN;
+	chip->pdata = clientp->dev.platform_data;
+	chip->id = id->driver_data;
+	chip->chip_info =
+		&tsl2x7x_chip_info_tbl[device_channel_config[id->driver_data]];
+
+	indio_dev->info = chip->chip_info->info;
+	indio_dev->dev.parent = &clientp->dev;
+	indio_dev->modes = INDIO_DIRECT_MODE;
+	indio_dev->name = chip->client->name;
+	indio_dev->channels = chip->chip_info->channel;
+	indio_dev->num_channels = chip->chip_info->chan_table_elements;
+
+	if (clientp->irq) {
+		ret = request_threaded_irq(clientp->irq,
+					   NULL,
+					   &tsl2x7x_event_handler,
+					   IRQF_TRIGGER_RISING | IRQF_ONESHOT,
+					   "TSL2X7X_event",
+					   indio_dev);
+		if (ret) {
+			dev_err(&clientp->dev,
+				"%s: irq request failed", __func__);
+			goto fail2;
+		}
+	}
+
+	/* Load up the defaults */
+	tsl2x7x_defaults(chip);
+	/* Make sure the chip is on */
+	tsl2x7x_chip_on(indio_dev);
+
+	ret = iio_device_register(indio_dev);
+	if (ret) {
+		dev_err(&clientp->dev,
+			"%s: iio registration failed\n", __func__);
+		goto fail1;
+	}
+
+	dev_info(&clientp->dev, "%s Light sensor found.\n", id->name);
+
+	return 0;
+
+fail1:
+	if (clientp->irq)
+		free_irq(clientp->irq, indio_dev);
+fail2:
+	iio_device_free(indio_dev);
+
+	return ret;
+}
+
+static int tsl2x7x_suspend(struct device *dev)
+{
+	struct iio_dev *indio_dev = dev_get_drvdata(dev);
+	struct tsl2X7X_chip *chip = iio_priv(indio_dev);
+	int ret = 0;
+
+	if (chip->tsl2x7x_chip_status == TSL2X7X_CHIP_WORKING) {
+		ret = tsl2x7x_chip_off(indio_dev);
+		chip->tsl2x7x_chip_status = TSL2X7X_CHIP_SUSPENDED;
+	}
+
+	if (chip->pdata && chip->pdata->platform_power) {
+		pm_message_t pmm = {PM_EVENT_SUSPEND};
+		chip->pdata->platform_power(dev, pmm);
+	}
+
+	return ret;
+}
+
+static int tsl2x7x_resume(struct device *dev)
+{
+	struct iio_dev *indio_dev = dev_get_drvdata(dev);
+	struct tsl2X7X_chip *chip = iio_priv(indio_dev);
+	int ret = 0;
+
+	if (chip->pdata && chip->pdata->platform_power) {
+		pm_message_t pmm = {PM_EVENT_RESUME};
+		chip->pdata->platform_power(dev, pmm);
+	}
+
+	if (chip->tsl2x7x_chip_status == TSL2X7X_CHIP_SUSPENDED)
+		ret = tsl2x7x_chip_on(indio_dev);
+
+	return ret;
+}
+
+static int __devexit tsl2x7x_remove(struct i2c_client *client)
+{
+	struct tsl2X7X_chip *chip = i2c_get_clientdata(client);
+	struct iio_dev *indio_dev = iio_priv_to_dev(chip);
+
+	tsl2x7x_chip_off(indio_dev);
+
+	iio_device_unregister(indio_dev);
+	if (client->irq)
+		free_irq(client->irq, chip->client->name);
+
+	iio_device_free(indio_dev);
+
+	return 0;
+}
+
+static struct i2c_device_id tsl2x7x_idtable[] = {
+	{ "tsl2571", tsl2571 },
+	{ "tsl2671", tsl2671 },
+	{ "tmd2671", tmd2671 },
+	{ "tsl2771", tsl2771 },
+	{ "tmd2771", tmd2771 },
+	{ "tsl2572", tsl2572 },
+	{ "tsl2672", tsl2672 },
+	{ "tmd2672", tmd2672 },
+	{ "tsl2772", tsl2772 },
+	{ "tmd2772", tmd2772 },
+	{}
+};
+
+MODULE_DEVICE_TABLE(i2c, tsl2x7x_idtable);
+
+static const struct dev_pm_ops tsl2x7x_pm_ops = {
+	.suspend = tsl2x7x_suspend,
+	.resume  = tsl2x7x_resume,
+};
+
+/* Driver definition */
+static struct i2c_driver tsl2x7x_driver = {
+	.driver = {
+		.name = "tsl2x7x",
+		.pm = &tsl2x7x_pm_ops,
+	},
+	.id_table = tsl2x7x_idtable,
+	.probe = tsl2x7x_probe,
+	.remove = __devexit_p(tsl2x7x_remove),
+};
+
+module_i2c_driver(tsl2x7x_driver);
+
+MODULE_AUTHOR("J. August Brenner<jbrenner@taosinc.com>");
+MODULE_DESCRIPTION("TAOS tsl2x7x ambient and proximity light sensor driver");
+MODULE_LICENSE("GPL");
diff --git a/drivers/staging/iio/machine.h b/drivers/staging/iio/machine.h
deleted file mode 100644
index 0b1f19bfdc4..00000000000
--- a/drivers/staging/iio/machine.h
+++ /dev/null
@@ -1,24 +0,0 @@
-/*
- * Industrial I/O in kernel access map definitions for board files.
- *
- * Copyright (c) 2011 Jonathan Cameron
- *
- * This program is free software; you can redistribute it and/or modify it
- * under the terms of the GNU General Public License version 2 as published by
- * the Free Software Foundation.
- */
-
-/**
- * struct iio_map - description of link between consumer and device channels
- * @adc_channel_label:	Label used to identify the channel on the provider.
- *			This is matched against the datasheet_name element
- *			of struct iio_chan_spec.
- * @consumer_dev_name:	Name to uniquely identify the consumer device.
- * @consumer_channel:	Unique name used to idenitify the channel on the
- *			consumer side.
- */
-struct iio_map {
-	const char *adc_channel_label;
-	const char *consumer_dev_name;
-	const char *consumer_channel;
-};
diff --git a/drivers/staging/iio/magnetometer/Kconfig b/drivers/staging/iio/magnetometer/Kconfig
index 722c4e13f71..b9d932595ba 100644
--- a/drivers/staging/iio/magnetometer/Kconfig
+++ b/drivers/staging/iio/magnetometer/Kconfig
@@ -15,13 +15,13 @@ config SENSORS_AK8975
 	  will be called ak8975.
 
 config SENSORS_HMC5843
-	tristate "Honeywell HMC5843 3-Axis Magnetometer"
+	tristate "Honeywell HMC5843/5883/5883L 3-Axis Magnetometer"
 	depends on I2C
 	help
-	  Say Y here to add support for the Honeywell HMC 5843 3-Axis
-	  Magnetometer (digital compass).
+	  Say Y here to add support for the Honeywell HMC5843, HMC5883 and
+	  HMC5883L 3-Axis Magnetometer (digital compass).
 
 	  To compile this driver as a module, choose M here: the module
-	  will be called hmc5843
+	  will be called hmc5843.
 
 endmenu
diff --git a/drivers/staging/iio/magnetometer/ak8975.c b/drivers/staging/iio/magnetometer/ak8975.c
index ebc2d0840ca..5834e4a70f8 100644
--- a/drivers/staging/iio/magnetometer/ak8975.c
+++ b/drivers/staging/iio/magnetometer/ak8975.c
@@ -30,8 +30,8 @@
 
 #include <linux/gpio.h>
 
-#include "../iio.h"
-#include "../sysfs.h"
+#include <linux/iio/iio.h>
+#include <linux/iio/sysfs.h>
 /*
  * Register definitions, as well as various shifts and masks to get at the
  * individual fields of the registers.
@@ -242,7 +242,7 @@ static int ak8975_setup(struct i2c_client *client)
 static ssize_t show_mode(struct device *dev, struct device_attribute *devattr,
 			 char *buf)
 {
-	struct iio_dev *indio_dev = dev_get_drvdata(dev);
+	struct iio_dev *indio_dev = dev_to_iio_dev(dev);
 	struct ak8975_data *data = iio_priv(indio_dev);
 
 	return sprintf(buf, "%u\n", data->mode);
@@ -255,7 +255,7 @@ static ssize_t show_mode(struct device *dev, struct device_attribute *devattr,
 static ssize_t store_mode(struct device *dev, struct device_attribute *devattr,
 			  const char *buf, size_t count)
 {
-	struct iio_dev *indio_dev = dev_get_drvdata(dev);
+	struct iio_dev *indio_dev = dev_to_iio_dev(dev);
 	struct ak8975_data *data = iio_priv(indio_dev);
 	struct i2c_client *client = data->client;
 	bool value;
@@ -431,7 +431,7 @@ static int ak8975_read_raw(struct iio_dev *indio_dev,
 	struct ak8975_data *data = iio_priv(indio_dev);
 
 	switch (mask) {
-	case 0:
+	case IIO_CHAN_INFO_RAW:
 		return ak8975_read_axis(indio_dev, chan->address, val);
 	case IIO_CHAN_INFO_SCALE:
 		*val = data->raw_to_gauss[chan->address];
@@ -445,7 +445,8 @@ static int ak8975_read_raw(struct iio_dev *indio_dev,
 		.type = IIO_MAGN,					\
 		.modified = 1,						\
 		.channel2 = IIO_MOD_##axis,				\
-		.info_mask = IIO_CHAN_INFO_SCALE_SEPARATE_BIT,	\
+		.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT |		\
+			     IIO_CHAN_INFO_SCALE_SEPARATE_BIT,		\
 		.address = index,					\
 	}
 
@@ -505,7 +506,7 @@ static int ak8975_probe(struct i2c_client *client,
 	}
 
 	/* Register with IIO */
-	indio_dev = iio_allocate_device(sizeof(*data));
+	indio_dev = iio_device_alloc(sizeof(*data));
 	if (indio_dev == NULL) {
 		err = -ENOMEM;
 		goto exit_gpio;
@@ -536,7 +537,7 @@ static int ak8975_probe(struct i2c_client *client,
 	return 0;
 
 exit_free_iio:
-	iio_free_device(indio_dev);
+	iio_device_free(indio_dev);
 exit_gpio:
 	if (gpio_is_valid(eoc_gpio))
 		gpio_free(eoc_gpio);
@@ -554,7 +555,7 @@ static int ak8975_remove(struct i2c_client *client)
 	if (gpio_is_valid(data->eoc_gpio))
 		gpio_free(data->eoc_gpio);
 
-	iio_free_device(indio_dev);
+	iio_device_free(indio_dev);
 
 	return 0;
 }
diff --git a/drivers/staging/iio/magnetometer/hmc5843.c b/drivers/staging/iio/magnetometer/hmc5843.c
index e00b416c4d3..c1fa09f0762 100644
--- a/drivers/staging/iio/magnetometer/hmc5843.c
+++ b/drivers/staging/iio/magnetometer/hmc5843.c
@@ -2,6 +2,8 @@
     Author: Shubhrajyoti Datta <shubhrajyoti@ti.com>
     Acknowledgement: Jonathan Cameron <jic23@cam.ac.uk> for valuable inputs.
 
+    Support for HMC5883 and HMC5883L by Peter Meerwald <pmeerw@pmeerw.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
@@ -22,10 +24,8 @@
 #include <linux/i2c.h>
 #include <linux/slab.h>
 #include <linux/types.h>
-#include "../iio.h"
-#include "../sysfs.h"
-
-#define HMC5843_I2C_ADDRESS			0x1E
+#include <linux/iio/iio.h>
+#include <linux/iio/sysfs.h>
 
 #define HMC5843_CONFIG_REG_A			0x00
 #define HMC5843_CONFIG_REG_B			0x01
@@ -36,110 +36,185 @@
 #define HMC5843_DATA_OUT_Y_LSB_REG		0x06
 #define HMC5843_DATA_OUT_Z_MSB_REG		0x07
 #define HMC5843_DATA_OUT_Z_LSB_REG		0x08
+/* Beware: Y and Z are exchanged on HMC5883 */
+#define HMC5883_DATA_OUT_Z_MSB_REG		0x05
+#define HMC5883_DATA_OUT_Z_LSB_REG		0x06
+#define HMC5883_DATA_OUT_Y_MSB_REG		0x07
+#define HMC5883_DATA_OUT_Y_LSB_REG		0x08
 #define HMC5843_STATUS_REG			0x09
 #define HMC5843_ID_REG_A			0x0A
 #define HMC5843_ID_REG_B			0x0B
 #define HMC5843_ID_REG_C			0x0C
 
+enum hmc5843_ids {
+	HMC5843_ID,
+	HMC5883_ID,
+	HMC5883L_ID,
+};
+
+/*
+ * Beware: identification of the HMC5883 is still "H43";
+ * I2C address is also unchanged
+ */
 #define HMC5843_ID_REG_LENGTH			0x03
 #define HMC5843_ID_STRING			"H43"
+#define HMC5843_I2C_ADDRESS			0x1E
 
 /*
- * Range settings in  (+-)Ga
- * */
-#define RANGE_GAIN_OFFSET			0x05
-
-#define	RANGE_0_7				0x00
-#define	RANGE_1_0				0x01 /* default */
-#define	RANGE_1_5				0x02
-#define	RANGE_2_0				0x03
-#define	RANGE_3_2				0x04
-#define	RANGE_3_8				0x05
-#define	RANGE_4_5				0x06
-#define	RANGE_6_5				0x07 /* Not recommended */
+ * Range gain settings in (+-)Ga
+ * Beware: HMC5843 and HMC5883 have different recommended sensor field
+ * ranges; default corresponds to +-1.0 Ga and +-1.3 Ga, respectively
+ */
+#define HMC5843_RANGE_GAIN_OFFSET		0x05
+#define HMC5843_RANGE_GAIN_DEFAULT		0x01
+#define HMC5843_RANGE_GAIN_MAX			0x07
 
 /*
  * Device status
  */
-#define	DATA_READY				0x01
-#define	DATA_OUTPUT_LOCK			0x02
-#define	VOLTAGE_REGULATOR_ENABLED		0x04
+#define HMC5843_DATA_READY			0x01
+#define HMC5843_DATA_OUTPUT_LOCK		0x02
+/* Does not exist on HMC5883, not used */
+#define HMC5843_VOLTAGE_REGULATOR_ENABLED	0x04
 
 /*
  * Mode register configuration
  */
-#define	MODE_CONVERSION_CONTINUOUS		0x00
-#define	MODE_CONVERSION_SINGLE			0x01
-#define	MODE_IDLE				0x02
-#define	MODE_SLEEP				0x03
-
-/* Minimum Data Output Rate in 1/10 Hz  */
-#define RATE_OFFSET				0x02
-#define RATE_BITMASK				0x1C
-#define	RATE_5					0x00
-#define	RATE_10					0x01
-#define	RATE_20					0x02
-#define	RATE_50					0x03
-#define	RATE_100				0x04
-#define	RATE_200				0x05
-#define	RATE_500				0x06
-#define	RATE_NOT_USED				0x07
+#define HMC5843_MODE_CONVERSION_CONTINUOUS	0x00
+#define HMC5843_MODE_CONVERSION_SINGLE		0x01
+#define HMC5843_MODE_IDLE			0x02
+#define HMC5843_MODE_SLEEP			0x03
+#define HMC5843_MODE_MASK			0x03
+
+/*
+ * HMC5843: Minimum data output rate
+ * HMC5883: Typical data output rate
+ */
+#define HMC5843_RATE_OFFSET			0x02
+#define HMC5843_RATE_BITMASK			0x1C
+#define HMC5843_RATE_NOT_USED			0x07
 
 /*
- * Device Configuration
+ * Device measurement configuration
  */
-#define	CONF_NORMAL				0x00
-#define	CONF_POSITIVE_BIAS			0x01
-#define	CONF_NEGATIVE_BIAS			0x02
-#define	CONF_NOT_USED				0x03
-#define	MEAS_CONF_MASK				0x03
+#define HMC5843_MEAS_CONF_NORMAL		0x00
+#define HMC5843_MEAS_CONF_POSITIVE_BIAS		0x01
+#define HMC5843_MEAS_CONF_NEGATIVE_BIAS		0x02
+#define HMC5843_MEAS_CONF_NOT_USED		0x03
+#define HMC5843_MEAS_CONF_MASK			0x03
 
-static int hmc5843_regval_to_nanoscale[] = {
+/*
+ * Scaling factors: 10000000/Gain
+ */
+static const int hmc5843_regval_to_nanoscale[] = {
 	6173, 7692, 10309, 12821, 18868, 21739, 25641, 35714
 };
 
-static const int regval_to_input_field_mg[] = {
-	700,
-	1000,
-	1500,
-	2000,
-	3200,
-	3800,
-	4500,
-	6500
+static const int hmc5883_regval_to_nanoscale[] = {
+	7812, 9766, 13021, 16287, 24096, 27701, 32573, 45662
 };
-static const char * const regval_to_samp_freq[] = {
-	"0.5",
-	"1",
-	"2",
-	"5",
-	"10",
-	"20",
-	"50",
+
+static const int hmc5883l_regval_to_nanoscale[] = {
+	7299, 9174, 12195, 15152, 22727, 25641, 30303, 43478
+};
+
+/*
+ * From the HMC5843 datasheet:
+ * Value	| Sensor input field range (Ga)	| Gain (counts/milli-Gauss)
+ * 0		| (+-)0.7			| 1620
+ * 1		| (+-)1.0			| 1300
+ * 2		| (+-)1.5			| 970
+ * 3		| (+-)2.0			| 780
+ * 4		| (+-)3.2			| 530
+ * 5		| (+-)3.8			| 460
+ * 6		| (+-)4.5			| 390
+ * 7		| (+-)6.5			| 280
+ *
+ * From the HMC5883 datasheet:
+ * Value	| Recommended sensor field range (Ga)	| Gain (counts/Gauss)
+ * 0		| (+-)0.9				| 1280
+ * 1		| (+-)1.2				| 1024
+ * 2		| (+-)1.9				| 768
+ * 3		| (+-)2.5				| 614
+ * 4		| (+-)4.0				| 415
+ * 5		| (+-)4.6				| 361
+ * 6		| (+-)5.5				| 307
+ * 7		| (+-)7.9				| 219
+ *
+ * From the HMC5883L datasheet:
+ * Value	| Recommended sensor field range (Ga)	| Gain (LSB/Gauss)
+ * 0		| (+-)0.88				| 1370
+ * 1		| (+-)1.3				| 1090
+ * 2		| (+-)1.9				| 820
+ * 3		| (+-)2.5				| 660
+ * 4		| (+-)4.0				| 440
+ * 5		| (+-)4.7				| 390
+ * 6		| (+-)5.6				| 330
+ * 7		| (+-)8.1				| 230
+ */
+static const int hmc5843_regval_to_input_field_mga[] = {
+	700, 1000, 1500, 2000, 3200, 3800, 4500, 6500
+};
+
+static const int hmc5883_regval_to_input_field_mga[] = {
+	900, 1200, 1900, 2500, 4000, 4600, 5500, 7900
+};
+
+static const int hmc5883l_regval_to_input_field_mga[] = {
+	880, 1300, 1900, 2500, 4000, 4700, 5600, 8100
+};
+
+/*
+ * From the datasheet:
+ * Value	| HMC5843		| HMC5883/HMC5883L
+ *		| Data output rate (Hz)	| Data output rate (Hz)
+ * 0		| 0.5			| 0.75
+ * 1		| 1			| 1.5
+ * 2		| 2			| 3
+ * 3		| 5			| 7.5
+ * 4		| 10 (default)		| 15
+ * 5		| 20			| 30
+ * 6		| 50			| 75
+ * 7		| Not used		| Not used
+ */
+static const char * const hmc5843_regval_to_sample_freq[] = {
+	"0.5", "1", "2", "5", "10", "20", "50",
+};
+
+static const char * const hmc5883_regval_to_sample_freq[] = {
+	"0.75", "1.5", "3", "7.5", "15", "30", "75",
 };
 
 /* Addresses to scan: 0x1E */
 static const unsigned short normal_i2c[] = { HMC5843_I2C_ADDRESS,
-							I2C_CLIENT_END };
+					     I2C_CLIENT_END };
+
+/* Describe chip variants */
+struct hmc5843_chip_info {
+	const struct iio_chan_spec *channels;
+	int num_channels;
+	const char * const *regval_to_sample_freq;
+	const int *regval_to_input_field_mga;
+	const int *regval_to_nanoscale;
+};
 
 /* Each client has this additional data */
 struct hmc5843_data {
 	struct mutex lock;
-	u8		rate;
-	u8		meas_conf;
-	u8		operating_mode;
-	u8		range;
+	u8 rate;
+	u8 meas_conf;
+	u8 operating_mode;
+	u8 range;
+	const struct hmc5843_chip_info *variant;
 };
 
-static void hmc5843_init_client(struct i2c_client *client);
-
+/* The lower two bits contain the current conversion mode */
 static s32 hmc5843_configure(struct i2c_client *client,
 				       u8 operating_mode)
 {
-	/* The lower two bits contain the current conversion mode */
 	return i2c_smbus_write_byte_data(client,
 					HMC5843_MODE_REG,
-					(operating_mode & 0x03));
+					operating_mode & HMC5843_MODE_MASK);
 }
 
 /* Return the measurement value from the specified channel */
@@ -153,7 +228,7 @@ static int hmc5843_read_measurement(struct iio_dev *indio_dev,
 
 	mutex_lock(&data->lock);
 	result = i2c_smbus_read_byte_data(client, HMC5843_STATUS_REG);
-	while (!(result & DATA_READY))
+	while (!(result & HMC5843_DATA_READY))
 		result = i2c_smbus_read_byte_data(client, HMC5843_STATUS_REG);
 
 	result = i2c_smbus_read_word_data(client, address);
@@ -161,30 +236,29 @@ static int hmc5843_read_measurement(struct iio_dev *indio_dev,
 	if (result < 0)
 		return -EINVAL;
 
-	*val	= (s16)swab16((u16)result);
+	*val = (s16)swab16((u16)result);
 	return IIO_VAL_INT;
 }
 
-
 /*
- * From the datasheet
+ * From the datasheet:
  * 0 - Continuous-Conversion Mode: In continuous-conversion mode, the
- * device continuously performs conversions and places the result in the
- * data register.
+ *     device continuously performs conversions and places the result in
+ *     the data register.
  *
- * 1 - Single-Conversion Mode : device performs a single measurement,
- *  sets RDY high and returned to sleep mode
+ * 1 - Single-Conversion Mode : Device performs a single measurement,
+ *     sets RDY high and returns to sleep mode.
  *
- * 2 - Idle Mode :  Device is placed in idle mode.
+ * 2 - Idle Mode : Device is placed in idle mode.
  *
- * 3 - Sleep Mode. Device is placed in sleep mode.
+ * 3 - Sleep Mode : Device is placed in sleep mode.
  *
  */
 static ssize_t hmc5843_show_operating_mode(struct device *dev,
 					struct device_attribute *attr,
 					char *buf)
 {
-	struct iio_dev *indio_dev = dev_get_drvdata(dev);
+	struct iio_dev *indio_dev = dev_to_iio_dev(dev);
 	struct hmc5843_data *data = iio_priv(indio_dev);
 	return sprintf(buf, "%d\n", data->operating_mode);
 }
@@ -194,21 +268,22 @@ static ssize_t hmc5843_set_operating_mode(struct device *dev,
 				const char *buf,
 				size_t count)
 {
-	struct iio_dev *indio_dev = dev_get_drvdata(dev);
+	struct iio_dev *indio_dev = dev_to_iio_dev(dev);
 	struct i2c_client *client = to_i2c_client(indio_dev->dev.parent);
 	struct hmc5843_data *data = iio_priv(indio_dev);
 	struct iio_dev_attr *this_attr = to_iio_dev_attr(attr);
 	unsigned long operating_mode = 0;
 	s32 status;
 	int error;
+
 	mutex_lock(&data->lock);
-	error = strict_strtoul(buf, 10, &operating_mode);
+	error = kstrtoul(buf, 10, &operating_mode);
 	if (error) {
 		count = error;
 		goto exit;
 	}
-	dev_dbg(dev, "set Conversion mode to %lu\n", operating_mode);
-	if (operating_mode > MODE_SLEEP) {
+	dev_dbg(dev, "set conversion mode to %lu\n", operating_mode);
+	if (operating_mode > HMC5843_MODE_SLEEP) {
 		count = -EINVAL;
 		goto exit;
 	}
@@ -225,6 +300,7 @@ exit:
 	mutex_unlock(&data->lock);
 	return count;
 }
+
 static IIO_DEVICE_ATTR(operating_mode,
 			S_IWUSR | S_IRUGO,
 			hmc5843_show_operating_mode,
@@ -234,25 +310,29 @@ static IIO_DEVICE_ATTR(operating_mode,
 /*
  * API for setting the measurement configuration to
  * Normal, Positive bias and Negative bias
- * From the datasheet
  *
- * Normal measurement configuration (default): In normal measurement
- * configuration the device follows normal measurement flow. Pins BP and BN
- * are left floating and high impedance.
+ * From the datasheet:
+ * 0 - Normal measurement configuration (default): In normal measurement
+ *     configuration the device follows normal measurement flow. Pins BP
+ *     and BN are left floating and high impedance.
  *
- * Positive bias configuration: In positive bias configuration, a positive
- * current is forced across the resistive load on pins BP and BN.
+ * 1 - Positive bias configuration: In positive bias configuration, a
+ *     positive current is forced across the resistive load on pins BP
+ *     and BN.
  *
- * Negative bias configuration. In negative bias configuration, a negative
- * current is forced across the resistive load on pins BP and BN.
+ * 2 - Negative bias configuration. In negative bias configuration, a
+ *     negative current is forced across the resistive load on pins BP
+ *     and BN.
  *
  */
 static s32 hmc5843_set_meas_conf(struct i2c_client *client,
 				      u8 meas_conf)
 {
-	struct hmc5843_data *data = i2c_get_clientdata(client);
+	struct iio_dev *indio_dev = i2c_get_clientdata(client);
+	struct hmc5843_data *data = iio_priv(indio_dev);
 	u8 reg_val;
-	reg_val = (meas_conf & MEAS_CONF_MASK) |  (data->rate << RATE_OFFSET);
+	reg_val = (meas_conf & HMC5843_MEAS_CONF_MASK) |
+		(data->rate << HMC5843_RATE_OFFSET);
 	return i2c_smbus_write_byte_data(client, HMC5843_CONFIG_REG_A, reg_val);
 }
 
@@ -260,7 +340,7 @@ static ssize_t hmc5843_show_measurement_configuration(struct device *dev,
 						struct device_attribute *attr,
 						char *buf)
 {
-	struct iio_dev *indio_dev = dev_get_drvdata(dev);
+	struct iio_dev *indio_dev = dev_to_iio_dev(dev);
 	struct hmc5843_data *data = iio_priv(indio_dev);
 	return sprintf(buf, "%d\n", data->meas_conf);
 }
@@ -270,16 +350,20 @@ static ssize_t hmc5843_set_measurement_configuration(struct device *dev,
 						const char *buf,
 						size_t count)
 {
-	struct iio_dev *indio_dev = dev_get_drvdata(dev);
+	struct iio_dev *indio_dev = dev_to_iio_dev(dev);
 	struct i2c_client *client = to_i2c_client(indio_dev->dev.parent);
-	struct hmc5843_data *data = i2c_get_clientdata(client);
+	struct hmc5843_data *data = iio_priv(indio_dev);
 	unsigned long meas_conf = 0;
-	int error = strict_strtoul(buf, 10, &meas_conf);
+	int error;
+
+	error = kstrtoul(buf, 10, &meas_conf);
 	if (error)
 		return error;
-	mutex_lock(&data->lock);
+	if (meas_conf >= HMC5843_MEAS_CONF_NOT_USED)
+		return -EINVAL;
 
-	dev_dbg(dev, "set mode to %lu\n", meas_conf);
+	mutex_lock(&data->lock);
+	dev_dbg(dev, "set measurement configuration to %lu\n", meas_conf);
 	if (hmc5843_set_meas_conf(client, meas_conf)) {
 		count = -EINVAL;
 		goto exit;
@@ -290,71 +374,85 @@ exit:
 	mutex_unlock(&data->lock);
 	return count;
 }
+
 static IIO_DEVICE_ATTR(meas_conf,
 			S_IWUSR | S_IRUGO,
 			hmc5843_show_measurement_configuration,
 			hmc5843_set_measurement_configuration,
 			0);
 
-/*
- * From Datasheet
- * The table shows the minimum data output
- * Value	| Minimum data output rate(Hz)
- * 0		| 0.5
- * 1		| 1
- * 2		| 2
- * 3		| 5
- * 4		| 10 (default)
- * 5		| 20
- * 6		| 50
- * 7		| Not used
- */
-static IIO_CONST_ATTR_SAMP_FREQ_AVAIL("0.5 1 2 5 10 20 50");
+static ssize_t hmc5843_show_sampling_frequencies_available(struct device *dev,
+						struct device_attribute *attr,
+						char *buf)
+{
+	struct iio_dev *indio_dev = dev_to_iio_dev(dev);
+	struct hmc5843_data *data = iio_priv(indio_dev);
+	ssize_t total_n = 0;
+	int i;
+
+	for (i = 0; i < HMC5843_RATE_NOT_USED; i++) {
+		ssize_t n = sprintf(buf, "%s ", data->variant->regval_to_sample_freq[i]);
+		buf += n;
+		total_n += n;
+	}
+	/* replace trailing space by newline */
+	buf[-1] = '\n';
+
+	return total_n;
+}
+
+static IIO_DEV_ATTR_SAMP_FREQ_AVAIL(hmc5843_show_sampling_frequencies_available);
 
 static s32 hmc5843_set_rate(struct i2c_client *client,
 				u8 rate)
 {
-	struct hmc5843_data *data = i2c_get_clientdata(client);
+	struct iio_dev *indio_dev = i2c_get_clientdata(client);
+	struct hmc5843_data *data = iio_priv(indio_dev);
 	u8 reg_val;
 
-	reg_val = (data->meas_conf) |  (rate << RATE_OFFSET);
-	if (rate >= RATE_NOT_USED) {
+	if (rate >= HMC5843_RATE_NOT_USED) {
 		dev_err(&client->dev,
-			"This data output rate is not supported\n");
+			"data output rate is not supported\n");
 		return -EINVAL;
 	}
+
+	reg_val = data->meas_conf | (rate << HMC5843_RATE_OFFSET);
 	return i2c_smbus_write_byte_data(client, HMC5843_CONFIG_REG_A, reg_val);
 }
 
-static ssize_t set_sampling_frequency(struct device *dev,
+static int hmc5843_check_sampling_frequency(struct hmc5843_data *data,
+						const char *buf)
+{
+	const char * const *samp_freq = data->variant->regval_to_sample_freq;
+	int i;
+
+	for (i = 0; i < HMC5843_RATE_NOT_USED; i++) {
+		if (sysfs_streq(buf, samp_freq[i]))
+			return i;
+	}
+
+	return -EINVAL;
+}
+
+static ssize_t hmc5843_set_sampling_frequency(struct device *dev,
 					struct device_attribute *attr,
 					const char *buf, size_t count)
 {
 
-	struct iio_dev *indio_dev = dev_get_drvdata(dev);
+	struct iio_dev *indio_dev = dev_to_iio_dev(dev);
 	struct i2c_client *client = to_i2c_client(indio_dev->dev.parent);
 	struct hmc5843_data *data = iio_priv(indio_dev);
-	unsigned long rate = 0;
-
-	if (strncmp(buf, "0.5" , 3) == 0)
-		rate = RATE_5;
-	else if (strncmp(buf, "1" , 1) == 0)
-		rate = RATE_10;
-	else if (strncmp(buf, "2", 1) == 0)
-		rate = RATE_20;
-	else if (strncmp(buf, "5", 1) == 0)
-		rate = RATE_50;
-	else if (strncmp(buf, "10", 2) == 0)
-		rate = RATE_100;
-	else if (strncmp(buf, "20" , 2) == 0)
-		rate = RATE_200;
-	else if (strncmp(buf, "50" , 2) == 0)
-		rate = RATE_500;
-	else
-		return -EINVAL;
+	int rate;
+
+	rate = hmc5843_check_sampling_frequency(data, buf);
+	if (rate < 0) {
+		dev_err(&client->dev,
+			"sampling frequency is not supported\n");
+		return rate;
+	}
 
 	mutex_lock(&data->lock);
-	dev_dbg(dev, "set rate to %lu\n", rate);
+	dev_dbg(dev, "set rate to %d\n", rate);
 	if (hmc5843_set_rate(client, rate)) {
 		count = -EINVAL;
 		goto exit;
@@ -366,89 +464,79 @@ exit:
 	return count;
 }
 
-static ssize_t show_sampling_frequency(struct device *dev,
+static ssize_t hmc5843_show_sampling_frequency(struct device *dev,
 			struct device_attribute *attr, char *buf)
 {
-	struct iio_dev *indio_dev = dev_get_drvdata(dev);
+	struct iio_dev *indio_dev = dev_to_iio_dev(dev);
 	struct i2c_client *client = to_i2c_client(indio_dev->dev.parent);
 	struct iio_dev_attr *this_attr = to_iio_dev_attr(attr);
+	struct hmc5843_data *data = iio_priv(indio_dev);
 	s32 rate;
 
-	rate = i2c_smbus_read_byte_data(client,  this_attr->address);
+	rate = i2c_smbus_read_byte_data(client, this_attr->address);
 	if (rate < 0)
 		return rate;
-	rate = (rate & RATE_BITMASK) >> RATE_OFFSET;
-	return sprintf(buf, "%s\n", regval_to_samp_freq[rate]);
+	rate = (rate & HMC5843_RATE_BITMASK) >> HMC5843_RATE_OFFSET;
+	return sprintf(buf, "%s\n", data->variant->regval_to_sample_freq[rate]);
 }
+
 static IIO_DEVICE_ATTR(sampling_frequency,
 			S_IWUSR | S_IRUGO,
-			show_sampling_frequency,
-			set_sampling_frequency,
+			hmc5843_show_sampling_frequency,
+			hmc5843_set_sampling_frequency,
 			HMC5843_CONFIG_REG_A);
 
-/*
- * From Datasheet
- *	Nominal gain settings
- * Value	| Sensor Input Field Range(Ga)	| Gain(counts/ milli-gauss)
- *0		|(+-)0.7			|1620
- *1		|(+-)1.0			|1300
- *2		|(+-)1.5			|970
- *3		|(+-)2.0			|780
- *4		|(+-)3.2			|530
- *5		|(+-)3.8			|460
- *6		|(+-)4.5			|390
- *7		|(+-)6.5			|280
- */
-static ssize_t show_range(struct device *dev,
+static ssize_t hmc5843_show_range_gain(struct device *dev,
 				struct device_attribute *attr,
 				char *buf)
 {
 	u8 range;
-	struct iio_dev *indio_dev = dev_get_drvdata(dev);
+	struct iio_dev *indio_dev = dev_to_iio_dev(dev);
 	struct hmc5843_data *data = iio_priv(indio_dev);
 
 	range = data->range;
-	return sprintf(buf, "%d\n", regval_to_input_field_mg[range]);
+	return sprintf(buf, "%d\n", data->variant->regval_to_input_field_mga[range]);
 }
 
-static ssize_t set_range(struct device *dev,
+static ssize_t hmc5843_set_range_gain(struct device *dev,
 			struct device_attribute *attr,
 			const char *buf,
 			size_t count)
 {
-	struct iio_dev *indio_dev = dev_get_drvdata(dev);
+	struct iio_dev *indio_dev = dev_to_iio_dev(dev);
 	struct i2c_client *client = to_i2c_client(indio_dev->dev.parent);
 	struct iio_dev_attr *this_attr = to_iio_dev_attr(attr);
 	struct hmc5843_data *data = iio_priv(indio_dev);
 	unsigned long range = 0;
 	int error;
+
 	mutex_lock(&data->lock);
-	error = strict_strtoul(buf, 10, &range);
+	error = kstrtoul(buf, 10, &range);
 	if (error) {
 		count = error;
 		goto exit;
 	}
 	dev_dbg(dev, "set range to %lu\n", range);
 
-	if (range > RANGE_6_5) {
+	if (range > HMC5843_RANGE_GAIN_MAX) {
 		count = -EINVAL;
 		goto exit;
 	}
 
 	data->range = range;
-	range = range << RANGE_GAIN_OFFSET;
+	range = range << HMC5843_RANGE_GAIN_OFFSET;
 	if (i2c_smbus_write_byte_data(client, this_attr->address, range))
 		count = -EINVAL;
 
 exit:
 	mutex_unlock(&data->lock);
 	return count;
-
 }
+
 static IIO_DEVICE_ATTR(in_magn_range,
 			S_IWUSR | S_IRUGO,
-			show_range,
-			set_range,
+			hmc5843_show_range_gain,
+			hmc5843_set_range_gain,
 			HMC5843_CONFIG_REG_B);
 
 static int hmc5843_read_raw(struct iio_dev *indio_dev,
@@ -459,13 +547,13 @@ static int hmc5843_read_raw(struct iio_dev *indio_dev,
 	struct hmc5843_data *data = iio_priv(indio_dev);
 
 	switch (mask) {
-	case 0:
+	case IIO_CHAN_INFO_RAW:
 		return hmc5843_read_measurement(indio_dev,
 						chan->address,
 						val);
 	case IIO_CHAN_INFO_SCALE:
 		*val = 0;
-		*val2 = hmc5843_regval_to_nanoscale[data->range];
+		*val2 = data->variant->regval_to_nanoscale[data->range];
 		return IIO_VAL_INT_PLUS_NANO;
 	};
 	return -EINVAL;
@@ -476,7 +564,8 @@ static int hmc5843_read_raw(struct iio_dev *indio_dev,
 		.type = IIO_MAGN,					\
 		.modified = 1,						\
 		.channel2 = IIO_MOD_##axis,				\
-		.info_mask = IIO_CHAN_INFO_SCALE_SHARED_BIT,		\
+		.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT |		\
+			     IIO_CHAN_INFO_SCALE_SHARED_BIT,		\
 		.address = add						\
 	}
 
@@ -486,12 +575,18 @@ static const struct iio_chan_spec hmc5843_channels[] = {
 	HMC5843_CHANNEL(Z, HMC5843_DATA_OUT_Z_MSB_REG),
 };
 
+static const struct iio_chan_spec hmc5883_channels[] = {
+	HMC5843_CHANNEL(X, HMC5843_DATA_OUT_X_MSB_REG),
+	HMC5843_CHANNEL(Y, HMC5883_DATA_OUT_Y_MSB_REG),
+	HMC5843_CHANNEL(Z, HMC5883_DATA_OUT_Z_MSB_REG),
+};
+
 static struct attribute *hmc5843_attributes[] = {
 	&iio_dev_attr_meas_conf.dev_attr.attr,
 	&iio_dev_attr_operating_mode.dev_attr.attr,
 	&iio_dev_attr_sampling_frequency.dev_attr.attr,
 	&iio_dev_attr_in_magn_range.dev_attr.attr,
-	&iio_const_attr_sampling_frequency_available.dev_attr.attr,
+	&iio_dev_attr_sampling_frequency_available.dev_attr.attr,
 	NULL
 };
 
@@ -499,6 +594,33 @@ static const struct attribute_group hmc5843_group = {
 	.attrs = hmc5843_attributes,
 };
 
+static const struct hmc5843_chip_info hmc5843_chip_info_tbl[] = {
+	[HMC5843_ID] = {
+		.channels = hmc5843_channels,
+		.num_channels = ARRAY_SIZE(hmc5843_channels),
+		.regval_to_sample_freq = hmc5843_regval_to_sample_freq,
+		.regval_to_input_field_mga =
+			hmc5843_regval_to_input_field_mga,
+		.regval_to_nanoscale = hmc5843_regval_to_nanoscale,
+	},
+	[HMC5883_ID] = {
+		.channels = hmc5883_channels,
+		.num_channels = ARRAY_SIZE(hmc5883_channels),
+		.regval_to_sample_freq = hmc5883_regval_to_sample_freq,
+		.regval_to_input_field_mga =
+			hmc5883_regval_to_input_field_mga,
+		.regval_to_nanoscale = hmc5883_regval_to_nanoscale,
+	},
+	[HMC5883L_ID] = {
+		.channels = hmc5883_channels,
+		.num_channels = ARRAY_SIZE(hmc5883_channels),
+		.regval_to_sample_freq = hmc5883_regval_to_sample_freq,
+		.regval_to_input_field_mga =
+			hmc5883l_regval_to_input_field_mga,
+		.regval_to_nanoscale = hmc5883l_regval_to_nanoscale,
+	},
+};
+
 static int hmc5843_detect(struct i2c_client *client,
 			  struct i2c_board_info *info)
 {
@@ -518,18 +640,23 @@ static int hmc5843_detect(struct i2c_client *client,
 	return 0;
 }
 
-/* Called when we have found a new HMC5843. */
-static void hmc5843_init_client(struct i2c_client *client)
+/* Called when we have found a new HMC58X3 */
+static void hmc5843_init_client(struct i2c_client *client,
+				const struct i2c_device_id *id)
 {
 	struct iio_dev *indio_dev = i2c_get_clientdata(client);
 	struct hmc5843_data *data = iio_priv(indio_dev);
 
+	data->variant = &hmc5843_chip_info_tbl[id->driver_data];
+	indio_dev->channels = data->variant->channels;
+	indio_dev->num_channels = data->variant->num_channels;
 	hmc5843_set_meas_conf(client, data->meas_conf);
 	hmc5843_set_rate(client, data->rate);
 	hmc5843_configure(client, data->operating_mode);
 	i2c_smbus_write_byte_data(client, HMC5843_CONFIG_REG_B, data->range);
 	mutex_init(&data->lock);
-	pr_info("HMC5843 initialized\n");
+
+	pr_info("%s initialized\n", id->name);
 }
 
 static const struct iio_info hmc5843_info = {
@@ -545,35 +672,34 @@ static int hmc5843_probe(struct i2c_client *client,
 	struct iio_dev *indio_dev;
 	int err = 0;
 
-	indio_dev = iio_allocate_device(sizeof(*data));
+	indio_dev = iio_device_alloc(sizeof(*data));
 	if (indio_dev == NULL) {
 		err = -ENOMEM;
 		goto exit;
 	}
-	data = iio_priv(indio_dev);
-	/* default settings at probe */
 
-	data->meas_conf = CONF_NORMAL;
-	data->range = RANGE_1_0;
-	data->operating_mode = MODE_CONVERSION_CONTINUOUS;
+	/* default settings at probe */
+	data = iio_priv(indio_dev);
+	data->meas_conf = HMC5843_MEAS_CONF_NORMAL;
+	data->range = HMC5843_RANGE_GAIN_DEFAULT;
+	data->operating_mode = HMC5843_MODE_CONVERSION_CONTINUOUS;
 
 	i2c_set_clientdata(client, indio_dev);
-
-	/* Initialize the HMC5843 chip */
-	hmc5843_init_client(client);
+	hmc5843_init_client(client, id);
 
 	indio_dev->info = &hmc5843_info;
 	indio_dev->name = id->name;
-	indio_dev->channels = hmc5843_channels;
-	indio_dev->num_channels = ARRAY_SIZE(hmc5843_channels);
 	indio_dev->dev.parent = &client->dev;
 	indio_dev->modes = INDIO_DIRECT_MODE;
+
 	err = iio_device_register(indio_dev);
 	if (err)
 		goto exit_free2;
+
 	return 0;
+
 exit_free2:
-	iio_free_device(indio_dev);
+	iio_device_free(indio_dev);
 exit:
 	return err;
 }
@@ -584,8 +710,8 @@ static int hmc5843_remove(struct i2c_client *client)
 
 	iio_device_unregister(indio_dev);
 	 /*  sleep mode to save power */
-	hmc5843_configure(client, MODE_SLEEP);
-	iio_free_device(indio_dev);
+	hmc5843_configure(client, HMC5843_MODE_SLEEP);
+	iio_device_free(indio_dev);
 
 	return 0;
 }
@@ -593,14 +719,18 @@ static int hmc5843_remove(struct i2c_client *client)
 #ifdef CONFIG_PM_SLEEP
 static int hmc5843_suspend(struct device *dev)
 {
-	hmc5843_configure(to_i2c_client(dev), MODE_SLEEP);
+	hmc5843_configure(to_i2c_client(dev), HMC5843_MODE_SLEEP);
 	return 0;
 }
 
 static int hmc5843_resume(struct device *dev)
 {
-	struct hmc5843_data *data = i2c_get_clientdata(to_i2c_client(dev));
-	hmc5843_configure(to_i2c_client(dev), data->operating_mode);
+	struct i2c_client *client = to_i2c_client(dev);
+	struct iio_dev *indio_dev = i2c_get_clientdata(client);
+	struct hmc5843_data *data = iio_priv(indio_dev);
+
+	hmc5843_configure(client, data->operating_mode);
+
 	return 0;
 }
 
@@ -611,7 +741,9 @@ static SIMPLE_DEV_PM_OPS(hmc5843_pm_ops, hmc5843_suspend, hmc5843_resume);
 #endif
 
 static const struct i2c_device_id hmc5843_id[] = {
-	{ "hmc5843", 0 },
+	{ "hmc5843", HMC5843_ID },
+	{ "hmc5883", HMC5883_ID },
+	{ "hmc5883l", HMC5883L_ID },
 	{ }
 };
 MODULE_DEVICE_TABLE(i2c, hmc5843_id);
@@ -630,5 +762,5 @@ static struct i2c_driver hmc5843_driver = {
 module_i2c_driver(hmc5843_driver);
 
 MODULE_AUTHOR("Shubhrajyoti Datta <shubhrajyoti@ti.com");
-MODULE_DESCRIPTION("HMC5843 driver");
+MODULE_DESCRIPTION("HMC5843/5883/5883L driver");
 MODULE_LICENSE("GPL");
diff --git a/drivers/staging/iio/meter/ade7753.c b/drivers/staging/iio/meter/ade7753.c
index 57baac6c0d4..f04ece7fbc2 100644
--- a/drivers/staging/iio/meter/ade7753.c
+++ b/drivers/staging/iio/meter/ade7753.c
@@ -18,8 +18,8 @@
 #include <linux/list.h>
 #include <linux/module.h>
 
-#include "../iio.h"
-#include "../sysfs.h"
+#include <linux/iio/iio.h>
+#include <linux/iio/sysfs.h>
 #include "meter.h"
 #include "ade7753.h"
 
@@ -28,7 +28,7 @@ static int ade7753_spi_write_reg_8(struct device *dev,
 				   u8 val)
 {
 	int ret;
-	struct iio_dev *indio_dev = dev_get_drvdata(dev);
+	struct iio_dev *indio_dev = dev_to_iio_dev(dev);
 	struct ade7753_state *st = iio_priv(indio_dev);
 
 	mutex_lock(&st->buf_lock);
@@ -46,7 +46,7 @@ static int ade7753_spi_write_reg_16(struct device *dev,
 		u16 value)
 {
 	int ret;
-	struct iio_dev *indio_dev = dev_get_drvdata(dev);
+	struct iio_dev *indio_dev = dev_to_iio_dev(dev);
 	struct ade7753_state *st = iio_priv(indio_dev);
 
 	mutex_lock(&st->buf_lock);
@@ -63,7 +63,7 @@ static int ade7753_spi_read_reg_8(struct device *dev,
 		u8 reg_address,
 		u8 *val)
 {
-	struct iio_dev *indio_dev = dev_get_drvdata(dev);
+	struct iio_dev *indio_dev = dev_to_iio_dev(dev);
 	struct ade7753_state *st = iio_priv(indio_dev);
 	ssize_t ret;
 
@@ -82,7 +82,7 @@ static int ade7753_spi_read_reg_16(struct device *dev,
 		u8 reg_address,
 		u16 *val)
 {
-	struct iio_dev *indio_dev = dev_get_drvdata(dev);
+	struct iio_dev *indio_dev = dev_to_iio_dev(dev);
 	struct ade7753_state *st = iio_priv(indio_dev);
 	ssize_t ret;
 
@@ -104,7 +104,7 @@ static int ade7753_spi_read_reg_24(struct device *dev,
 		u32 *val)
 {
 	struct spi_message msg;
-	struct iio_dev *indio_dev = dev_get_drvdata(dev);
+	struct iio_dev *indio_dev = dev_to_iio_dev(dev);
 	struct ade7753_state *st = iio_priv(indio_dev);
 	int ret;
 	struct spi_transfer xfers[] = {
@@ -416,7 +416,7 @@ static ssize_t ade7753_write_frequency(struct device *dev,
 		const char *buf,
 		size_t len)
 {
-	struct iio_dev *indio_dev = dev_get_drvdata(dev);
+	struct iio_dev *indio_dev = dev_to_iio_dev(dev);
 	struct ade7753_state *st = iio_priv(indio_dev);
 	unsigned long val;
 	int ret;
@@ -517,7 +517,7 @@ static int __devinit ade7753_probe(struct spi_device *spi)
 	struct iio_dev *indio_dev;
 
 	/* setup the industrialio driver allocated elements */
-	indio_dev = iio_allocate_device(sizeof(*st));
+	indio_dev = iio_device_alloc(sizeof(*st));
 	if (indio_dev == NULL) {
 		ret = -ENOMEM;
 		goto error_ret;
@@ -546,7 +546,7 @@ static int __devinit ade7753_probe(struct spi_device *spi)
 	return 0;
 
 error_free_dev:
-	iio_free_device(indio_dev);
+	iio_device_free(indio_dev);
 
 error_ret:
 	return ret;
@@ -564,7 +564,7 @@ static int ade7753_remove(struct spi_device *spi)
 	if (ret)
 		goto err_ret;
 
-	iio_free_device(indio_dev);
+	iio_device_free(indio_dev);
 err_ret:
 	return ret;
 }
diff --git a/drivers/staging/iio/meter/ade7754.c b/drivers/staging/iio/meter/ade7754.c
index 8d81c92007e..6cee28a5e87 100644
--- a/drivers/staging/iio/meter/ade7754.c
+++ b/drivers/staging/iio/meter/ade7754.c
@@ -18,8 +18,8 @@
 #include <linux/list.h>
 #include <linux/module.h>
 
-#include "../iio.h"
-#include "../sysfs.h"
+#include <linux/iio/iio.h>
+#include <linux/iio/sysfs.h>
 #include "meter.h"
 #include "ade7754.h"
 
@@ -28,7 +28,7 @@ static int ade7754_spi_write_reg_8(struct device *dev,
 		u8 val)
 {
 	int ret;
-	struct iio_dev *indio_dev = dev_get_drvdata(dev);
+	struct iio_dev *indio_dev = dev_to_iio_dev(dev);
 	struct ade7754_state *st = iio_priv(indio_dev);
 
 	mutex_lock(&st->buf_lock);
@@ -46,7 +46,7 @@ static int ade7754_spi_write_reg_16(struct device *dev,
 		u16 value)
 {
 	int ret;
-	struct iio_dev *indio_dev = dev_get_drvdata(dev);
+	struct iio_dev *indio_dev = dev_to_iio_dev(dev);
 	struct ade7754_state *st = iio_priv(indio_dev);
 
 	mutex_lock(&st->buf_lock);
@@ -63,7 +63,7 @@ static int ade7754_spi_read_reg_8(struct device *dev,
 		u8 reg_address,
 		u8 *val)
 {
-	struct iio_dev *indio_dev = dev_get_drvdata(dev);
+	struct iio_dev *indio_dev = dev_to_iio_dev(dev);
 	struct ade7754_state *st = iio_priv(indio_dev);
 	int ret;
 
@@ -82,7 +82,7 @@ static int ade7754_spi_read_reg_16(struct device *dev,
 		u8 reg_address,
 		u16 *val)
 {
-	struct iio_dev *indio_dev = dev_get_drvdata(dev);
+	struct iio_dev *indio_dev = dev_to_iio_dev(dev);
 	struct ade7754_state *st = iio_priv(indio_dev);
 	int ret;
 
@@ -104,7 +104,7 @@ static int ade7754_spi_read_reg_24(struct device *dev,
 		u32 *val)
 {
 	struct spi_message msg;
-	struct iio_dev *indio_dev = dev_get_drvdata(dev);
+	struct iio_dev *indio_dev = dev_to_iio_dev(dev);
 	struct ade7754_state *st = iio_priv(indio_dev);
 	int ret;
 	struct spi_transfer xfers[] = {
@@ -436,7 +436,7 @@ static ssize_t ade7754_write_frequency(struct device *dev,
 		const char *buf,
 		size_t len)
 {
-	struct iio_dev *indio_dev = dev_get_drvdata(dev);
+	struct iio_dev *indio_dev = dev_to_iio_dev(dev);
 	struct ade7754_state *st = iio_priv(indio_dev);
 	unsigned long val;
 	int ret;
@@ -540,7 +540,7 @@ static int __devinit ade7754_probe(struct spi_device *spi)
 	struct iio_dev *indio_dev;
 
 	/* setup the industrialio driver allocated elements */
-	indio_dev = iio_allocate_device(sizeof(*st));
+	indio_dev = iio_device_alloc(sizeof(*st));
 	if (indio_dev == NULL) {
 		ret = -ENOMEM;
 		goto error_ret;
@@ -568,7 +568,7 @@ static int __devinit ade7754_probe(struct spi_device *spi)
 	return 0;
 
 error_free_dev:
-	iio_free_device(indio_dev);
+	iio_device_free(indio_dev);
 
 error_ret:
 	return ret;
@@ -585,7 +585,7 @@ static int ade7754_remove(struct spi_device *spi)
 	if (ret)
 		goto err_ret;
 
-	iio_free_device(indio_dev);
+	iio_device_free(indio_dev);
 
 err_ret:
 	return ret;
diff --git a/drivers/staging/iio/meter/ade7758_core.c b/drivers/staging/iio/meter/ade7758_core.c
index dcb20294dfe..96d6114a31a 100644
--- a/drivers/staging/iio/meter/ade7758_core.c
+++ b/drivers/staging/iio/meter/ade7758_core.c
@@ -18,9 +18,9 @@
 #include <linux/list.h>
 #include <linux/module.h>
 
-#include "../iio.h"
-#include "../sysfs.h"
-#include "../buffer.h"
+#include <linux/iio/iio.h>
+#include <linux/iio/sysfs.h>
+#include <linux/iio/buffer.h>
 #include "meter.h"
 #include "ade7758.h"
 
@@ -29,7 +29,7 @@ int ade7758_spi_write_reg_8(struct device *dev,
 		u8 val)
 {
 	int ret;
-	struct iio_dev *indio_dev = dev_get_drvdata(dev);
+	struct iio_dev *indio_dev = dev_to_iio_dev(dev);
 	struct ade7758_state *st = iio_priv(indio_dev);
 
 	mutex_lock(&st->buf_lock);
@@ -48,7 +48,7 @@ static int ade7758_spi_write_reg_16(struct device *dev,
 {
 	int ret;
 	struct spi_message msg;
-	struct iio_dev *indio_dev = dev_get_drvdata(dev);
+	struct iio_dev *indio_dev = dev_to_iio_dev(dev);
 	struct ade7758_state *st = iio_priv(indio_dev);
 	struct spi_transfer xfers[] = {
 		{
@@ -77,7 +77,7 @@ static int ade7758_spi_write_reg_24(struct device *dev,
 {
 	int ret;
 	struct spi_message msg;
-	struct iio_dev *indio_dev = dev_get_drvdata(dev);
+	struct iio_dev *indio_dev = dev_to_iio_dev(dev);
 	struct ade7758_state *st = iio_priv(indio_dev);
 	struct spi_transfer xfers[] = {
 		{
@@ -106,7 +106,7 @@ int ade7758_spi_read_reg_8(struct device *dev,
 		u8 *val)
 {
 	struct spi_message msg;
-	struct iio_dev *indio_dev = dev_get_drvdata(dev);
+	struct iio_dev *indio_dev = dev_to_iio_dev(dev);
 	struct ade7758_state *st = iio_priv(indio_dev);
 	int ret;
 	struct spi_transfer xfers[] = {
@@ -149,7 +149,7 @@ static int ade7758_spi_read_reg_16(struct device *dev,
 		u16 *val)
 {
 	struct spi_message msg;
-	struct iio_dev *indio_dev = dev_get_drvdata(dev);
+	struct iio_dev *indio_dev = dev_to_iio_dev(dev);
 	struct ade7758_state *st = iio_priv(indio_dev);
 	int ret;
 	struct spi_transfer xfers[] = {
@@ -195,7 +195,7 @@ static int ade7758_spi_read_reg_24(struct device *dev,
 		u32 *val)
 {
 	struct spi_message msg;
-	struct iio_dev *indio_dev = dev_get_drvdata(dev);
+	struct iio_dev *indio_dev = dev_to_iio_dev(dev);
 	struct ade7758_state *st = iio_priv(indio_dev);
 	int ret;
 	struct spi_transfer xfers[] = {
@@ -534,7 +534,7 @@ static ssize_t ade7758_write_frequency(struct device *dev,
 		const char *buf,
 		size_t len)
 {
-	struct iio_dev *indio_dev = dev_get_drvdata(dev);
+	struct iio_dev *indio_dev = dev_to_iio_dev(dev);
 	unsigned long val;
 	int ret;
 	u8 reg, t;
@@ -662,66 +662,217 @@ static const struct attribute_group ade7758_attribute_group = {
 };
 
 static struct iio_chan_spec ade7758_channels[] = {
-	IIO_CHAN(IIO_VOLTAGE, 0, 1, 0, "raw", 0, 0,
-		IIO_CHAN_INFO_SCALE_SHARED_BIT,
-		AD7758_WT(AD7758_PHASE_A, AD7758_VOLTAGE),
-		0, IIO_ST('s', 24, 32, 0), 0),
-	IIO_CHAN(IIO_CURRENT, 0, 1, 0, "raw", 0, 0,
-		IIO_CHAN_INFO_SCALE_SHARED_BIT,
-		AD7758_WT(AD7758_PHASE_A, AD7758_CURRENT),
-		1, IIO_ST('s', 24, 32, 0), 0),
-	IIO_CHAN(IIO_POWER, 0, 1, 0, "apparent_raw", 0, 0,
-		IIO_CHAN_INFO_SCALE_SHARED_BIT,
-		AD7758_WT(AD7758_PHASE_A, AD7758_APP_PWR),
-		2, IIO_ST('s', 24, 32, 0), 0),
-	IIO_CHAN(IIO_POWER, 0, 1, 0, "active_raw", 0, 0,
-		IIO_CHAN_INFO_SCALE_SHARED_BIT,
-		AD7758_WT(AD7758_PHASE_A, AD7758_ACT_PWR),
-		3, IIO_ST('s', 24, 32, 0), 0),
-	IIO_CHAN(IIO_POWER, 0, 1, 0, "reactive_raw", 0, 0,
-		IIO_CHAN_INFO_SCALE_SHARED_BIT,
-		AD7758_WT(AD7758_PHASE_A, AD7758_REACT_PWR),
-		4, IIO_ST('s', 24, 32, 0), 0),
-	IIO_CHAN(IIO_VOLTAGE, 0, 1, 0, "raw", 1, 0,
-		IIO_CHAN_INFO_SCALE_SHARED_BIT,
-		AD7758_WT(AD7758_PHASE_B, AD7758_VOLTAGE),
-		5, IIO_ST('s', 24, 32, 0), 0),
-	IIO_CHAN(IIO_CURRENT, 0, 1, 0, "raw", 1, 0,
-		IIO_CHAN_INFO_SCALE_SHARED_BIT,
-		AD7758_WT(AD7758_PHASE_B, AD7758_CURRENT),
-		6, IIO_ST('s', 24, 32, 0), 0),
-	IIO_CHAN(IIO_POWER, 0, 1, 0, "apparent_raw", 1, 0,
-		IIO_CHAN_INFO_SCALE_SHARED_BIT,
-		AD7758_WT(AD7758_PHASE_B, AD7758_APP_PWR),
-		7, IIO_ST('s', 24, 32, 0), 0),
-	IIO_CHAN(IIO_POWER, 0, 1, 0, "active_raw", 1, 0,
-		IIO_CHAN_INFO_SCALE_SHARED_BIT,
-		AD7758_WT(AD7758_PHASE_B, AD7758_ACT_PWR),
-		8, IIO_ST('s', 24, 32, 0), 0),
-	IIO_CHAN(IIO_POWER, 0, 1, 0, "reactive_raw", 1, 0,
-		IIO_CHAN_INFO_SCALE_SHARED_BIT,
-		AD7758_WT(AD7758_PHASE_B, AD7758_REACT_PWR),
-		9, IIO_ST('s', 24, 32, 0), 0),
-	IIO_CHAN(IIO_VOLTAGE, 0, 1, 0, "raw", 2, 0,
-		IIO_CHAN_INFO_SCALE_SHARED_BIT,
-		AD7758_WT(AD7758_PHASE_C, AD7758_VOLTAGE),
-		10, IIO_ST('s', 24, 32, 0), 0),
-	IIO_CHAN(IIO_CURRENT, 0, 1, 0, "raw", 2, 0,
-		IIO_CHAN_INFO_SCALE_SHARED_BIT,
-		AD7758_WT(AD7758_PHASE_C, AD7758_CURRENT),
-		11, IIO_ST('s', 24, 32, 0), 0),
-	IIO_CHAN(IIO_POWER, 0, 1, 0, "apparent_raw", 2, 0,
-		IIO_CHAN_INFO_SCALE_SHARED_BIT,
-		AD7758_WT(AD7758_PHASE_C, AD7758_APP_PWR),
-		12, IIO_ST('s', 24, 32, 0), 0),
-	IIO_CHAN(IIO_POWER, 0, 1, 0, "active_raw", 2, 0,
-		IIO_CHAN_INFO_SCALE_SHARED_BIT,
-		AD7758_WT(AD7758_PHASE_C, AD7758_ACT_PWR),
-		13, IIO_ST('s', 24, 32, 0), 0),
-	IIO_CHAN(IIO_POWER, 0, 1, 0, "reactive_raw", 2, 0,
-		IIO_CHAN_INFO_SCALE_SHARED_BIT,
-		AD7758_WT(AD7758_PHASE_C, AD7758_REACT_PWR),
-		14, IIO_ST('s', 24, 32, 0), 0),
+	{
+		.type = IIO_VOLTAGE,
+		.indexed = 1,
+		.channel = 0,
+		.extend_name = "raw",
+		.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT |
+			     IIO_CHAN_INFO_SCALE_SHARED_BIT,
+		.address = AD7758_WT(AD7758_PHASE_A, AD7758_VOLTAGE),
+		.scan_index = 0,
+		.scan_type = {
+			.sign = 's',
+			.realbits = 24,
+			.storagebits = 32,
+		},
+	}, {
+		.type = IIO_CURRENT,
+		.indexed = 1,
+		.channel = 0,
+		.extend_name = "raw",
+		.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT |
+			     IIO_CHAN_INFO_SCALE_SHARED_BIT,
+		.address = AD7758_WT(AD7758_PHASE_A, AD7758_CURRENT),
+		.scan_index = 1,
+		.scan_type = {
+			.sign = 's',
+			.realbits = 24,
+			.storagebits = 32,
+		},
+	}, {
+		.type = IIO_POWER,
+		.indexed = 1,
+		.channel = 0,
+		.extend_name = "apparent_raw",
+		.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT |
+			     IIO_CHAN_INFO_SCALE_SHARED_BIT,
+		.address = AD7758_WT(AD7758_PHASE_A, AD7758_APP_PWR),
+		.scan_index = 2,
+		.scan_type = {
+			.sign = 's',
+			.realbits = 24,
+			.storagebits = 32,
+		},
+	}, {
+		.type = IIO_POWER,
+		.indexed = 1,
+		.channel = 0,
+		.extend_name = "active_raw",
+		.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT |
+			     IIO_CHAN_INFO_SCALE_SHARED_BIT,
+		.address = AD7758_WT(AD7758_PHASE_A, AD7758_ACT_PWR),
+		.scan_index = 3,
+		.scan_type = {
+			.sign = 's',
+			.realbits = 24,
+			.storagebits = 32,
+		},
+	}, {
+		.type = IIO_POWER,
+		.indexed = 1,
+		.channel = 0,
+		.extend_name = "reactive_raw",
+		.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT |
+			     IIO_CHAN_INFO_SCALE_SHARED_BIT,
+		.address = AD7758_WT(AD7758_PHASE_A, AD7758_REACT_PWR),
+		.scan_index = 4,
+		.scan_type = {
+			.sign = 's',
+			.realbits = 24,
+			.storagebits = 32,
+		},
+	}, {
+		.type = IIO_VOLTAGE,
+		.indexed = 1,
+		.channel = 1,
+		.extend_name = "raw",
+		.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT |
+			     IIO_CHAN_INFO_SCALE_SHARED_BIT,
+		.address = AD7758_WT(AD7758_PHASE_B, AD7758_VOLTAGE),
+		.scan_index = 5,
+		.scan_type = {
+			.sign = 's',
+			.realbits = 24,
+			.storagebits = 32,
+		},
+	}, {
+		.type = IIO_CURRENT,
+		.indexed = 1,
+		.channel = 1,
+		.extend_name = "raw",
+		.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT |
+			     IIO_CHAN_INFO_SCALE_SHARED_BIT,
+		.address = AD7758_WT(AD7758_PHASE_B, AD7758_CURRENT),
+		.scan_index = 6,
+		.scan_type = {
+			.sign = 's',
+			.realbits = 24,
+			.storagebits = 32,
+		},
+	}, {
+		.type = IIO_POWER,
+		.indexed = 1,
+		.channel = 1,
+		.extend_name = "apparent_raw",
+		.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT |
+			     IIO_CHAN_INFO_SCALE_SHARED_BIT,
+		.address = AD7758_WT(AD7758_PHASE_B, AD7758_APP_PWR),
+		.scan_index = 7,
+		.scan_type = {
+			.sign = 's',
+			.realbits = 24,
+			.storagebits = 32,
+		},
+	}, {
+		.type = IIO_POWER,
+		.indexed = 1,
+		.channel = 1,
+		.extend_name = "active_raw",
+		.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT |
+			     IIO_CHAN_INFO_SCALE_SHARED_BIT,
+		.address = AD7758_WT(AD7758_PHASE_B, AD7758_ACT_PWR),
+		.scan_index = 8,
+		.scan_type = {
+			.sign = 's',
+			.realbits = 24,
+			.storagebits = 32,
+		},
+	}, {
+		.type = IIO_POWER,
+		.indexed = 1,
+		.channel = 1,
+		.extend_name = "reactive_raw",
+		.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT |
+			     IIO_CHAN_INFO_SCALE_SHARED_BIT,
+		.address = AD7758_WT(AD7758_PHASE_B, AD7758_REACT_PWR),
+		.scan_index = 9,
+		.scan_type = {
+			.sign = 's',
+			.realbits = 24,
+			.storagebits = 32,
+		},
+	}, {
+		.type = IIO_VOLTAGE,
+		.indexed = 1,
+		.channel = 2,
+		.extend_name = "raw",
+		.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT |
+			     IIO_CHAN_INFO_SCALE_SHARED_BIT,
+		.address = AD7758_WT(AD7758_PHASE_C, AD7758_VOLTAGE),
+		.scan_index = 10,
+		.scan_type = {
+			.sign = 's',
+			.realbits = 24,
+			.storagebits = 32,
+		},
+	}, {
+		.type = IIO_CURRENT,
+		.indexed = 1,
+		.channel = 2,
+		.extend_name = "raw",
+		.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT |
+			     IIO_CHAN_INFO_SCALE_SHARED_BIT,
+		.address = AD7758_WT(AD7758_PHASE_C, AD7758_CURRENT),
+		.scan_index = 11,
+		.scan_type = {
+			.sign = 's',
+			.realbits = 24,
+			.storagebits = 32,
+		},
+	}, {
+		.type = IIO_POWER,
+		.indexed = 1,
+		.channel = 2,
+		.extend_name = "apparent_raw",
+		.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT |
+			     IIO_CHAN_INFO_SCALE_SHARED_BIT,
+		.address = AD7758_WT(AD7758_PHASE_C, AD7758_APP_PWR),
+		.scan_index = 12,
+		.scan_type = {
+			.sign = 's',
+			.realbits = 24,
+			.storagebits = 32,
+		},
+	}, {
+		.type = IIO_POWER,
+		.indexed = 1,
+		.channel = 2,
+		.extend_name = "active_raw",
+		.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT |
+			     IIO_CHAN_INFO_SCALE_SHARED_BIT,
+		.address = AD7758_WT(AD7758_PHASE_C, AD7758_ACT_PWR),
+		.scan_index = 13,
+		.scan_type = {
+			.sign = 's',
+			.realbits = 24,
+			.storagebits = 32,
+		},
+	}, {
+		.type = IIO_POWER,
+		.indexed = 1,
+		.channel = 2,
+		.extend_name = "reactive_raw",
+		.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT |
+			     IIO_CHAN_INFO_SCALE_SHARED_BIT,
+		.address = AD7758_WT(AD7758_PHASE_C, AD7758_REACT_PWR),
+		.scan_index = 14,
+		.scan_type = {
+			.sign = 's',
+			.realbits = 24,
+			.storagebits = 32,
+		},
+	},
 	IIO_CHAN_SOFT_TIMESTAMP(15),
 };
 
@@ -734,7 +885,7 @@ static int __devinit ade7758_probe(struct spi_device *spi)
 {
 	int i, ret;
 	struct ade7758_state *st;
-	struct iio_dev *indio_dev = iio_allocate_device(sizeof(*st));
+	struct iio_dev *indio_dev = iio_device_alloc(sizeof(*st));
 
 	if (indio_dev == NULL) {
 		ret = -ENOMEM;
@@ -800,7 +951,7 @@ static int __devinit ade7758_probe(struct spi_device *spi)
 	return 0;
 
 error_remove_trigger:
-	if (indio_dev->modes & INDIO_BUFFER_TRIGGERED)
+	if (spi->irq)
 		ade7758_remove_trigger(indio_dev);
 error_uninitialize_ring:
 	ade7758_uninitialize_ring(indio_dev);
@@ -811,7 +962,7 @@ error_free_tx:
 error_free_rx:
 	kfree(st->rx);
 error_free_dev:
-	iio_free_device(indio_dev);
+	iio_device_free(indio_dev);
 error_ret:
 	return ret;
 }
@@ -833,7 +984,7 @@ static int ade7758_remove(struct spi_device *spi)
 	kfree(st->tx);
 	kfree(st->rx);
 
-	iio_free_device(indio_dev);
+	iio_device_free(indio_dev);
 
 err_ret:
 	return ret;
diff --git a/drivers/staging/iio/meter/ade7758_ring.c b/drivers/staging/iio/meter/ade7758_ring.c
index c45b23bb122..92159f208d1 100644
--- a/drivers/staging/iio/meter/ade7758_ring.c
+++ b/drivers/staging/iio/meter/ade7758_ring.c
@@ -12,18 +12,17 @@
 #include <linux/slab.h>
 #include <asm/unaligned.h>
 
-#include "../iio.h"
+#include <linux/iio/iio.h>
 #include "../ring_sw.h"
-#include "../trigger_consumer.h"
+#include <linux/iio/trigger_consumer.h>
 #include "ade7758.h"
 
 /**
  * ade7758_spi_read_burst() - read data registers
- * @dev: device associated with child of actual device (iio_dev or iio_trig)
+ * @indio_dev: the IIO device
  **/
-static int ade7758_spi_read_burst(struct device *dev)
+static int ade7758_spi_read_burst(struct iio_dev *indio_dev)
 {
-	struct iio_dev *indio_dev = dev_get_drvdata(dev);
 	struct ade7758_state *st = iio_priv(indio_dev);
 	int ret;
 
@@ -68,11 +67,11 @@ static irqreturn_t ade7758_trigger_handler(int irq, void *p)
 	u32 *dat32 = (u32 *)dat64;
 
 	if (!bitmap_empty(indio_dev->active_scan_mask, indio_dev->masklength))
-		if (ade7758_spi_read_burst(&indio_dev->dev) >= 0)
+		if (ade7758_spi_read_burst(indio_dev) >= 0)
 			*dat32 = get_unaligned_be32(&st->rx_buf[5]) & 0xFFFFFF;
 
 	/* Guaranteed to be aligned with 8 byte boundary */
-	if (ring->scan_timestamp)
+	if (indio_dev->scan_timestamp)
 		dat64[1] = pf->timestamp;
 
 	ring->access->store_to(ring, (u8 *)dat64, pf->timestamp);
@@ -92,29 +91,19 @@ static irqreturn_t ade7758_trigger_handler(int irq, void *p)
 static int ade7758_ring_preenable(struct iio_dev *indio_dev)
 {
 	struct ade7758_state *st = iio_priv(indio_dev);
-	struct iio_buffer *ring = indio_dev->buffer;
-	size_t d_size;
 	unsigned channel;
+	int ret;
 
 	if (!bitmap_empty(indio_dev->active_scan_mask, indio_dev->masklength))
 		return -EINVAL;
 
+	ret = iio_sw_buffer_preenable(indio_dev);
+	if (ret < 0)
+		return ret;
+
 	channel = find_first_bit(indio_dev->active_scan_mask,
 				 indio_dev->masklength);
 
-	d_size = st->ade7758_ring_channels[channel].scan_type.storagebits / 8;
-
-	if (ring->scan_timestamp) {
-		d_size += sizeof(s64);
-
-		if (d_size % sizeof(s64))
-			d_size += sizeof(s64) - (d_size % sizeof(s64));
-	}
-
-	if (indio_dev->buffer->access->set_bytes_per_datum)
-		indio_dev->buffer->access->
-			set_bytes_per_datum(indio_dev->buffer, d_size);
-
 	ade7758_write_waveform_type(&indio_dev->dev,
 		st->ade7758_ring_channels[channel].address);
 
diff --git a/drivers/staging/iio/meter/ade7758_trigger.c b/drivers/staging/iio/meter/ade7758_trigger.c
index b6569c70665..f9c6a340092 100644
--- a/drivers/staging/iio/meter/ade7758_trigger.c
+++ b/drivers/staging/iio/meter/ade7758_trigger.c
@@ -11,8 +11,8 @@
 #include <linux/spi/spi.h>
 #include <linux/export.h>
 
-#include "../iio.h"
-#include "../trigger.h"
+#include <linux/iio/iio.h>
+#include <linux/iio/trigger.h>
 #include "ade7758.h"
 
 /**
@@ -63,7 +63,7 @@ int ade7758_probe_trigger(struct iio_dev *indio_dev)
 	struct ade7758_state *st = iio_priv(indio_dev);
 	int ret;
 
-	st->trig = iio_allocate_trigger("%s-dev%d",
+	st->trig = iio_trigger_alloc("%s-dev%d",
 					spi_get_device_id(st->us)->name,
 					indio_dev->id);
 	if (st->trig == NULL) {
@@ -94,7 +94,7 @@ int ade7758_probe_trigger(struct iio_dev *indio_dev)
 error_free_irq:
 	free_irq(st->us->irq, st->trig);
 error_free_trig:
-	iio_free_trigger(st->trig);
+	iio_trigger_free(st->trig);
 error_ret:
 	return ret;
 }
@@ -105,5 +105,5 @@ void ade7758_remove_trigger(struct iio_dev *indio_dev)
 
 	iio_trigger_unregister(st->trig);
 	free_irq(st->us->irq, st->trig);
-	iio_free_trigger(st->trig);
+	iio_trigger_free(st->trig);
 }
diff --git a/drivers/staging/iio/meter/ade7759.c b/drivers/staging/iio/meter/ade7759.c
index 0beab478dcd..b3f7e0fa961 100644
--- a/drivers/staging/iio/meter/ade7759.c
+++ b/drivers/staging/iio/meter/ade7759.c
@@ -18,8 +18,8 @@
 #include <linux/list.h>
 #include <linux/module.h>
 
-#include "../iio.h"
-#include "../sysfs.h"
+#include <linux/iio/iio.h>
+#include <linux/iio/sysfs.h>
 #include "meter.h"
 #include "ade7759.h"
 
@@ -28,7 +28,7 @@ static int ade7759_spi_write_reg_8(struct device *dev,
 		u8 val)
 {
 	int ret;
-	struct iio_dev *indio_dev = dev_get_drvdata(dev);
+	struct iio_dev *indio_dev = dev_to_iio_dev(dev);
 	struct ade7759_state *st = iio_priv(indio_dev);
 
 	mutex_lock(&st->buf_lock);
@@ -46,7 +46,7 @@ static int ade7759_spi_write_reg_16(struct device *dev,
 		u16 value)
 {
 	int ret;
-	struct iio_dev *indio_dev = dev_get_drvdata(dev);
+	struct iio_dev *indio_dev = dev_to_iio_dev(dev);
 	struct ade7759_state *st = iio_priv(indio_dev);
 
 	mutex_lock(&st->buf_lock);
@@ -63,7 +63,7 @@ static int ade7759_spi_read_reg_8(struct device *dev,
 		u8 reg_address,
 		u8 *val)
 {
-	struct iio_dev *indio_dev = dev_get_drvdata(dev);
+	struct iio_dev *indio_dev = dev_to_iio_dev(dev);
 	struct ade7759_state *st = iio_priv(indio_dev);
 	int ret;
 
@@ -82,7 +82,7 @@ static int ade7759_spi_read_reg_16(struct device *dev,
 		u8 reg_address,
 		u16 *val)
 {
-	struct iio_dev *indio_dev = dev_get_drvdata(dev);
+	struct iio_dev *indio_dev = dev_to_iio_dev(dev);
 	struct ade7759_state *st = iio_priv(indio_dev);
 	int ret;
 
@@ -104,7 +104,7 @@ static int ade7759_spi_read_reg_40(struct device *dev,
 		u64 *val)
 {
 	struct spi_message msg;
-	struct iio_dev *indio_dev = dev_get_drvdata(dev);
+	struct iio_dev *indio_dev = dev_to_iio_dev(dev);
 	struct ade7759_state *st = iio_priv(indio_dev);
 	int ret;
 	struct spi_transfer xfers[] = {
@@ -376,7 +376,7 @@ static ssize_t ade7759_write_frequency(struct device *dev,
 		const char *buf,
 		size_t len)
 {
-	struct iio_dev *indio_dev = dev_get_drvdata(dev);
+	struct iio_dev *indio_dev = dev_to_iio_dev(dev);
 	struct ade7759_state *st = iio_priv(indio_dev);
 	unsigned long val;
 	int ret;
@@ -463,7 +463,7 @@ static int __devinit ade7759_probe(struct spi_device *spi)
 	struct iio_dev *indio_dev;
 
 	/* setup the industrialio driver allocated elements */
-	indio_dev = iio_allocate_device(sizeof(*st));
+	indio_dev = iio_device_alloc(sizeof(*st));
 	if (indio_dev == NULL) {
 		ret = -ENOMEM;
 		goto error_ret;
@@ -491,7 +491,7 @@ static int __devinit ade7759_probe(struct spi_device *spi)
 	return 0;
 
 error_free_dev:
-	iio_free_device(indio_dev);
+	iio_device_free(indio_dev);
 error_ret:
 	return ret;
 }
@@ -507,7 +507,7 @@ static int ade7759_remove(struct spi_device *spi)
 	if (ret)
 		goto err_ret;
 
-	iio_free_device(indio_dev);
+	iio_device_free(indio_dev);
 
 err_ret:
 	return ret;
diff --git a/drivers/staging/iio/meter/ade7854-i2c.c b/drivers/staging/iio/meter/ade7854-i2c.c
index 1e1faa0479d..06090465fa5 100644
--- a/drivers/staging/iio/meter/ade7854-i2c.c
+++ b/drivers/staging/iio/meter/ade7854-i2c.c
@@ -12,7 +12,7 @@
 #include <linux/slab.h>
 #include <linux/module.h>
 
-#include "../iio.h"
+#include <linux/iio/iio.h>
 #include "ade7854.h"
 
 static int ade7854_i2c_write_reg_8(struct device *dev,
@@ -20,7 +20,7 @@ static int ade7854_i2c_write_reg_8(struct device *dev,
 		u8 value)
 {
 	int ret;
-	struct iio_dev *indio_dev = dev_get_drvdata(dev);
+	struct iio_dev *indio_dev = dev_to_iio_dev(dev);
 	struct ade7854_state *st = iio_priv(indio_dev);
 
 	mutex_lock(&st->buf_lock);
@@ -39,7 +39,7 @@ static int ade7854_i2c_write_reg_16(struct device *dev,
 		u16 value)
 {
 	int ret;
-	struct iio_dev *indio_dev = dev_get_drvdata(dev);
+	struct iio_dev *indio_dev = dev_to_iio_dev(dev);
 	struct ade7854_state *st = iio_priv(indio_dev);
 
 	mutex_lock(&st->buf_lock);
@@ -59,7 +59,7 @@ static int ade7854_i2c_write_reg_24(struct device *dev,
 		u32 value)
 {
 	int ret;
-	struct iio_dev *indio_dev = dev_get_drvdata(dev);
+	struct iio_dev *indio_dev = dev_to_iio_dev(dev);
 	struct ade7854_state *st = iio_priv(indio_dev);
 
 	mutex_lock(&st->buf_lock);
@@ -80,7 +80,7 @@ static int ade7854_i2c_write_reg_32(struct device *dev,
 		u32 value)
 {
 	int ret;
-	struct iio_dev *indio_dev = dev_get_drvdata(dev);
+	struct iio_dev *indio_dev = dev_to_iio_dev(dev);
 	struct ade7854_state *st = iio_priv(indio_dev);
 
 	mutex_lock(&st->buf_lock);
@@ -101,7 +101,7 @@ static int ade7854_i2c_read_reg_8(struct device *dev,
 		u16 reg_address,
 		u8 *val)
 {
-	struct iio_dev *indio_dev = dev_get_drvdata(dev);
+	struct iio_dev *indio_dev = dev_to_iio_dev(dev);
 	struct ade7854_state *st = iio_priv(indio_dev);
 	int ret;
 
@@ -127,7 +127,7 @@ static int ade7854_i2c_read_reg_16(struct device *dev,
 		u16 reg_address,
 		u16 *val)
 {
-	struct iio_dev *indio_dev = dev_get_drvdata(dev);
+	struct iio_dev *indio_dev = dev_to_iio_dev(dev);
 	struct ade7854_state *st = iio_priv(indio_dev);
 	int ret;
 
@@ -153,7 +153,7 @@ static int ade7854_i2c_read_reg_24(struct device *dev,
 		u16 reg_address,
 		u32 *val)
 {
-	struct iio_dev *indio_dev = dev_get_drvdata(dev);
+	struct iio_dev *indio_dev = dev_to_iio_dev(dev);
 	struct ade7854_state *st = iio_priv(indio_dev);
 	int ret;
 
@@ -179,7 +179,7 @@ static int ade7854_i2c_read_reg_32(struct device *dev,
 		u16 reg_address,
 		u32 *val)
 {
-	struct iio_dev *indio_dev = dev_get_drvdata(dev);
+	struct iio_dev *indio_dev = dev_to_iio_dev(dev);
 	struct ade7854_state *st = iio_priv(indio_dev);
 	int ret;
 
@@ -208,7 +208,7 @@ static int __devinit ade7854_i2c_probe(struct i2c_client *client,
 	struct ade7854_state *st;
 	struct iio_dev *indio_dev;
 
-	indio_dev = iio_allocate_device(sizeof(*st));
+	indio_dev = iio_device_alloc(sizeof(*st));
 	if (indio_dev == NULL)
 		return -ENOMEM;
 	st = iio_priv(indio_dev);
@@ -226,7 +226,7 @@ static int __devinit ade7854_i2c_probe(struct i2c_client *client,
 
 	ret = ade7854_probe(indio_dev, &client->dev);
 	if (ret)
-		iio_free_device(indio_dev);
+		iio_device_free(indio_dev);
 
 	return ret;
 }
diff --git a/drivers/staging/iio/meter/ade7854-spi.c b/drivers/staging/iio/meter/ade7854-spi.c
index 81121862c1b..9fb2f8bfca8 100644
--- a/drivers/staging/iio/meter/ade7854-spi.c
+++ b/drivers/staging/iio/meter/ade7854-spi.c
@@ -12,7 +12,7 @@
 #include <linux/slab.h>
 #include <linux/module.h>
 
-#include "../iio.h"
+#include <linux/iio/iio.h>
 #include "ade7854.h"
 
 static int ade7854_spi_write_reg_8(struct device *dev,
@@ -21,7 +21,7 @@ static int ade7854_spi_write_reg_8(struct device *dev,
 {
 	int ret;
 	struct spi_message msg;
-	struct iio_dev *indio_dev = dev_get_drvdata(dev);
+	struct iio_dev *indio_dev = dev_to_iio_dev(dev);
 	struct ade7854_state *st = iio_priv(indio_dev);
 	struct spi_transfer xfer = {
 		.tx_buf = st->tx,
@@ -49,7 +49,7 @@ static int ade7854_spi_write_reg_16(struct device *dev,
 {
 	int ret;
 	struct spi_message msg;
-	struct iio_dev *indio_dev = dev_get_drvdata(dev);
+	struct iio_dev *indio_dev = dev_to_iio_dev(dev);
 	struct ade7854_state *st = iio_priv(indio_dev);
 	struct spi_transfer xfer = {
 		.tx_buf = st->tx,
@@ -78,7 +78,7 @@ static int ade7854_spi_write_reg_24(struct device *dev,
 {
 	int ret;
 	struct spi_message msg;
-	struct iio_dev *indio_dev = dev_get_drvdata(dev);
+	struct iio_dev *indio_dev = dev_to_iio_dev(dev);
 	struct ade7854_state *st = iio_priv(indio_dev);
 	struct spi_transfer xfer = {
 		.tx_buf = st->tx,
@@ -108,7 +108,7 @@ static int ade7854_spi_write_reg_32(struct device *dev,
 {
 	int ret;
 	struct spi_message msg;
-	struct iio_dev *indio_dev = dev_get_drvdata(dev);
+	struct iio_dev *indio_dev = dev_to_iio_dev(dev);
 	struct ade7854_state *st = iio_priv(indio_dev);
 	struct spi_transfer xfer = {
 		.tx_buf = st->tx,
@@ -138,7 +138,7 @@ static int ade7854_spi_read_reg_8(struct device *dev,
 		u8 *val)
 {
 	struct spi_message msg;
-	struct iio_dev *indio_dev = dev_get_drvdata(dev);
+	struct iio_dev *indio_dev = dev_to_iio_dev(dev);
 	struct ade7854_state *st = iio_priv(indio_dev);
 	int ret;
 	struct spi_transfer xfers[] = {
@@ -180,7 +180,7 @@ static int ade7854_spi_read_reg_16(struct device *dev,
 		u16 *val)
 {
 	struct spi_message msg;
-	struct iio_dev *indio_dev = dev_get_drvdata(dev);
+	struct iio_dev *indio_dev = dev_to_iio_dev(dev);
 	struct ade7854_state *st = iio_priv(indio_dev);
 	int ret;
 	struct spi_transfer xfers[] = {
@@ -221,7 +221,7 @@ static int ade7854_spi_read_reg_24(struct device *dev,
 		u32 *val)
 {
 	struct spi_message msg;
-	struct iio_dev *indio_dev = dev_get_drvdata(dev);
+	struct iio_dev *indio_dev = dev_to_iio_dev(dev);
 	struct ade7854_state *st = iio_priv(indio_dev);
 	int ret;
 	struct spi_transfer xfers[] = {
@@ -263,7 +263,7 @@ static int ade7854_spi_read_reg_32(struct device *dev,
 		u32 *val)
 {
 	struct spi_message msg;
-	struct iio_dev *indio_dev = dev_get_drvdata(dev);
+	struct iio_dev *indio_dev = dev_to_iio_dev(dev);
 	struct ade7854_state *st = iio_priv(indio_dev);
 	int ret;
 	struct spi_transfer xfers[] = {
@@ -306,7 +306,7 @@ static int __devinit ade7854_spi_probe(struct spi_device *spi)
 	struct ade7854_state *st;
 	struct iio_dev *indio_dev;
 
-	indio_dev = iio_allocate_device(sizeof(*st));
+	indio_dev = iio_device_alloc(sizeof(*st));
 	if (indio_dev == NULL)
 		return -ENOMEM;
 	st = iio_priv(indio_dev);
@@ -325,7 +325,7 @@ static int __devinit ade7854_spi_probe(struct spi_device *spi)
 
 	ret = ade7854_probe(indio_dev, &spi->dev);
 	if (ret)
-		iio_free_device(indio_dev);
+		iio_device_free(indio_dev);
 
 	return 0;
 }
diff --git a/drivers/staging/iio/meter/ade7854.c b/drivers/staging/iio/meter/ade7854.c
index 49c01c5c1b5..c642da84842 100644
--- a/drivers/staging/iio/meter/ade7854.c
+++ b/drivers/staging/iio/meter/ade7854.c
@@ -17,8 +17,8 @@
 #include <linux/list.h>
 #include <linux/module.h>
 
-#include "../iio.h"
-#include "../sysfs.h"
+#include <linux/iio/iio.h>
+#include <linux/iio/sysfs.h>
 #include "meter.h"
 #include "ade7854.h"
 
@@ -28,7 +28,7 @@ static ssize_t ade7854_read_8bit(struct device *dev,
 {
 	int ret;
 	u8 val = 0;
-	struct iio_dev *indio_dev = dev_get_drvdata(dev);
+	struct iio_dev *indio_dev = dev_to_iio_dev(dev);
 	struct ade7854_state *st = iio_priv(indio_dev);
 	struct iio_dev_attr *this_attr = to_iio_dev_attr(attr);
 
@@ -45,7 +45,7 @@ static ssize_t ade7854_read_16bit(struct device *dev,
 {
 	int ret;
 	u16 val = 0;
-	struct iio_dev *indio_dev = dev_get_drvdata(dev);
+	struct iio_dev *indio_dev = dev_to_iio_dev(dev);
 	struct ade7854_state *st = iio_priv(indio_dev);
 	struct iio_dev_attr *this_attr = to_iio_dev_attr(attr);
 
@@ -62,7 +62,7 @@ static ssize_t ade7854_read_24bit(struct device *dev,
 {
 	int ret;
 	u32 val;
-	struct iio_dev *indio_dev = dev_get_drvdata(dev);
+	struct iio_dev *indio_dev = dev_to_iio_dev(dev);
 	struct ade7854_state *st = iio_priv(indio_dev);
 	struct iio_dev_attr *this_attr = to_iio_dev_attr(attr);
 
@@ -80,7 +80,7 @@ static ssize_t ade7854_read_32bit(struct device *dev,
 	int ret;
 	u32 val = 0;
 	struct iio_dev_attr *this_attr = to_iio_dev_attr(attr);
-	struct iio_dev *indio_dev = dev_get_drvdata(dev);
+	struct iio_dev *indio_dev = dev_to_iio_dev(dev);
 	struct ade7854_state *st = iio_priv(indio_dev);
 
 	ret = st->read_reg_32(dev, this_attr->address, &val);
@@ -96,7 +96,7 @@ static ssize_t ade7854_write_8bit(struct device *dev,
 		size_t len)
 {
 	struct iio_dev_attr *this_attr = to_iio_dev_attr(attr);
-	struct iio_dev *indio_dev = dev_get_drvdata(dev);
+	struct iio_dev *indio_dev = dev_to_iio_dev(dev);
 	struct ade7854_state *st = iio_priv(indio_dev);
 
 	int ret;
@@ -117,7 +117,7 @@ static ssize_t ade7854_write_16bit(struct device *dev,
 		size_t len)
 {
 	struct iio_dev_attr *this_attr = to_iio_dev_attr(attr);
-	struct iio_dev *indio_dev = dev_get_drvdata(dev);
+	struct iio_dev *indio_dev = dev_to_iio_dev(dev);
 	struct ade7854_state *st = iio_priv(indio_dev);
 
 	int ret;
@@ -138,7 +138,7 @@ static ssize_t ade7854_write_24bit(struct device *dev,
 		size_t len)
 {
 	struct iio_dev_attr *this_attr = to_iio_dev_attr(attr);
-	struct iio_dev *indio_dev = dev_get_drvdata(dev);
+	struct iio_dev *indio_dev = dev_to_iio_dev(dev);
 	struct ade7854_state *st = iio_priv(indio_dev);
 
 	int ret;
@@ -159,7 +159,7 @@ static ssize_t ade7854_write_32bit(struct device *dev,
 		size_t len)
 {
 	struct iio_dev_attr *this_attr = to_iio_dev_attr(attr);
-	struct iio_dev *indio_dev = dev_get_drvdata(dev);
+	struct iio_dev *indio_dev = dev_to_iio_dev(dev);
 	struct ade7854_state *st = iio_priv(indio_dev);
 
 	int ret;
@@ -176,7 +176,7 @@ error_ret:
 
 static int ade7854_reset(struct device *dev)
 {
-	struct iio_dev *indio_dev = dev_get_drvdata(dev);
+	struct iio_dev *indio_dev = dev_to_iio_dev(dev);
 	struct ade7854_state *st = iio_priv(indio_dev);
 	u16 val;
 
@@ -425,7 +425,7 @@ static IIO_DEV_ATTR_CVAHR(ade7854_read_32bit,
 
 static int ade7854_set_irq(struct device *dev, bool enable)
 {
-	struct iio_dev *indio_dev = dev_get_drvdata(dev);
+	struct iio_dev *indio_dev = dev_to_iio_dev(dev);
 	struct ade7854_state *st = iio_priv(indio_dev);
 
 	int ret;
@@ -581,7 +581,7 @@ int ade7854_probe(struct iio_dev *indio_dev, struct device *dev)
 error_unreg_dev:
 	iio_device_unregister(indio_dev);
 error_free_dev:
-	iio_free_device(indio_dev);
+	iio_device_free(indio_dev);
 
 	return ret;
 }
@@ -590,7 +590,7 @@ EXPORT_SYMBOL(ade7854_probe);
 int ade7854_remove(struct iio_dev *indio_dev)
 {
 	iio_device_unregister(indio_dev);
-	iio_free_device(indio_dev);
+	iio_device_free(indio_dev);
 
 	return 0;
 }
diff --git a/drivers/staging/iio/meter/meter.h b/drivers/staging/iio/meter/meter.h
index 6a3db142363..23e1b5f480a 100644
--- a/drivers/staging/iio/meter/meter.h
+++ b/drivers/staging/iio/meter/meter.h
@@ -1,4 +1,4 @@
-#include "../sysfs.h"
+#include <linux/iio/sysfs.h>
 
 /* metering ic types of attribute */
 
diff --git a/drivers/staging/iio/resolver/ad2s1200.c b/drivers/staging/iio/resolver/ad2s1200.c
index d8ce854c189..8b71eb0e16f 100644
--- a/drivers/staging/iio/resolver/ad2s1200.c
+++ b/drivers/staging/iio/resolver/ad2s1200.c
@@ -19,8 +19,8 @@
 #include <linux/gpio.h>
 #include <linux/module.h>
 
-#include "../iio.h"
-#include "../sysfs.h"
+#include <linux/iio/iio.h>
+#include <linux/iio/sysfs.h>
 
 #define DRV_NAME "ad2s1200"
 
@@ -85,10 +85,12 @@ static const struct iio_chan_spec ad2s1200_channels[] = {
 		.type = IIO_ANGL,
 		.indexed = 1,
 		.channel = 0,
+		.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT,
 	}, {
 		.type = IIO_ANGL_VEL,
 		.indexed = 1,
 		.channel = 0,
+		.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT,
 	}
 };
 
@@ -110,7 +112,7 @@ static int __devinit ad2s1200_probe(struct spi_device *spi)
 						DRV_NAME, pins[pn]);
 			goto error_ret;
 		}
-	indio_dev = iio_allocate_device(sizeof(*st));
+	indio_dev = iio_device_alloc(sizeof(*st));
 	if (indio_dev == NULL) {
 		ret = -ENOMEM;
 		goto error_ret;
@@ -140,7 +142,7 @@ static int __devinit ad2s1200_probe(struct spi_device *spi)
 	return 0;
 
 error_free_dev:
-	iio_free_device(indio_dev);
+	iio_device_free(indio_dev);
 error_ret:
 	for (--pn; pn >= 0; pn--)
 		gpio_free(pins[pn]);
@@ -150,7 +152,7 @@ error_ret:
 static int __devexit ad2s1200_remove(struct spi_device *spi)
 {
 	iio_device_unregister(spi_get_drvdata(spi));
-	iio_free_device(spi_get_drvdata(spi));
+	iio_device_free(spi_get_drvdata(spi));
 
 	return 0;
 }
diff --git a/drivers/staging/iio/resolver/ad2s1210.c b/drivers/staging/iio/resolver/ad2s1210.c
index c439fcf72be..f313859476c 100644
--- a/drivers/staging/iio/resolver/ad2s1210.c
+++ b/drivers/staging/iio/resolver/ad2s1210.c
@@ -18,8 +18,8 @@
 #include <linux/gpio.h>
 #include <linux/module.h>
 
-#include "../iio.h"
-#include "../sysfs.h"
+#include <linux/iio/iio.h>
+#include <linux/iio/sysfs.h>
 #include "ad2s1210.h"
 
 #define DRV_NAME "ad2s1210"
@@ -200,7 +200,7 @@ static ssize_t ad2s1210_store_softreset(struct device *dev,
 					const char *buf,
 					size_t len)
 {
-	struct ad2s1210_state *st = iio_priv(dev_get_drvdata(dev));
+	struct ad2s1210_state *st = iio_priv(dev_to_iio_dev(dev));
 	int ret;
 
 	mutex_lock(&st->lock);
@@ -214,7 +214,7 @@ static ssize_t ad2s1210_show_fclkin(struct device *dev,
 				    struct device_attribute *attr,
 				    char *buf)
 {
-	struct ad2s1210_state *st = iio_priv(dev_get_drvdata(dev));
+	struct ad2s1210_state *st = iio_priv(dev_to_iio_dev(dev));
 	return sprintf(buf, "%d\n", st->fclkin);
 }
 
@@ -223,7 +223,7 @@ static ssize_t ad2s1210_store_fclkin(struct device *dev,
 				     const char *buf,
 				     size_t len)
 {
-	struct ad2s1210_state *st = iio_priv(dev_get_drvdata(dev));
+	struct ad2s1210_state *st = iio_priv(dev_to_iio_dev(dev));
 	unsigned long fclkin;
 	int ret;
 
@@ -252,7 +252,7 @@ static ssize_t ad2s1210_show_fexcit(struct device *dev,
 				    struct device_attribute *attr,
 				    char *buf)
 {
-	struct ad2s1210_state *st = iio_priv(dev_get_drvdata(dev));
+	struct ad2s1210_state *st = iio_priv(dev_to_iio_dev(dev));
 	return sprintf(buf, "%d\n", st->fexcit);
 }
 
@@ -260,7 +260,7 @@ static ssize_t ad2s1210_store_fexcit(struct device *dev,
 				     struct device_attribute *attr,
 				     const char *buf, size_t len)
 {
-	struct ad2s1210_state *st = iio_priv(dev_get_drvdata(dev));
+	struct ad2s1210_state *st = iio_priv(dev_to_iio_dev(dev));
 	unsigned long fexcit;
 	int ret;
 
@@ -287,7 +287,7 @@ static ssize_t ad2s1210_show_control(struct device *dev,
 				     struct device_attribute *attr,
 				     char *buf)
 {
-	struct ad2s1210_state *st = iio_priv(dev_get_drvdata(dev));
+	struct ad2s1210_state *st = iio_priv(dev_to_iio_dev(dev));
 	int ret;
 	mutex_lock(&st->lock);
 	ret = ad2s1210_config_read(st, AD2S1210_REG_CONTROL);
@@ -299,7 +299,7 @@ static ssize_t ad2s1210_store_control(struct device *dev,
 			struct device_attribute *attr,
 			const char *buf, size_t len)
 {
-	struct ad2s1210_state *st = iio_priv(dev_get_drvdata(dev));
+	struct ad2s1210_state *st = iio_priv(dev_to_iio_dev(dev));
 	unsigned long udata;
 	unsigned char data;
 	int ret;
@@ -345,7 +345,7 @@ error_ret:
 static ssize_t ad2s1210_show_resolution(struct device *dev,
 			struct device_attribute *attr, char *buf)
 {
-	struct ad2s1210_state *st = iio_priv(dev_get_drvdata(dev));
+	struct ad2s1210_state *st = iio_priv(dev_to_iio_dev(dev));
 	return sprintf(buf, "%d\n", st->resolution);
 }
 
@@ -353,7 +353,7 @@ static ssize_t ad2s1210_store_resolution(struct device *dev,
 			struct device_attribute *attr,
 			const char *buf, size_t len)
 {
-	struct ad2s1210_state *st = iio_priv(dev_get_drvdata(dev));
+	struct ad2s1210_state *st = iio_priv(dev_to_iio_dev(dev));
 	unsigned char data;
 	unsigned long udata;
 	int ret;
@@ -403,7 +403,7 @@ error_ret:
 static ssize_t ad2s1210_show_fault(struct device *dev,
 			struct device_attribute *attr, char *buf)
 {
-	struct ad2s1210_state *st = iio_priv(dev_get_drvdata(dev));
+	struct ad2s1210_state *st = iio_priv(dev_to_iio_dev(dev));
 	int ret;
 
 	mutex_lock(&st->lock);
@@ -418,7 +418,7 @@ static ssize_t ad2s1210_clear_fault(struct device *dev,
 				    const char *buf,
 				    size_t len)
 {
-	struct ad2s1210_state *st = iio_priv(dev_get_drvdata(dev));
+	struct ad2s1210_state *st = iio_priv(dev_to_iio_dev(dev));
 	int ret;
 
 	mutex_lock(&st->lock);
@@ -441,7 +441,7 @@ static ssize_t ad2s1210_show_reg(struct device *dev,
 				 struct device_attribute *attr,
 				 char *buf)
 {
-	struct ad2s1210_state *st = iio_priv(dev_get_drvdata(dev));
+	struct ad2s1210_state *st = iio_priv(dev_to_iio_dev(dev));
 	struct iio_dev_attr *iattr = to_iio_dev_attr(attr);
 	int ret;
 
@@ -455,7 +455,7 @@ static ssize_t ad2s1210_show_reg(struct device *dev,
 static ssize_t ad2s1210_store_reg(struct device *dev,
 		struct device_attribute *attr, const char *buf, size_t len)
 {
-	struct ad2s1210_state *st = iio_priv(dev_get_drvdata(dev));
+	struct ad2s1210_state *st = iio_priv(dev_to_iio_dev(dev));
 	unsigned long data;
 	int ret;
 	struct iio_dev_attr *iattr = to_iio_dev_attr(attr);
@@ -580,10 +580,12 @@ static struct iio_chan_spec ad2s1210_channels[] = {
 		.type = IIO_ANGL,
 		.indexed = 1,
 		.channel = 0,
+		.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT,
 	}, {
 		.type = IIO_ANGL_VEL,
 		.indexed = 1,
 		.channel = 0,
+		.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT,
 	}
 };
 
@@ -688,7 +690,7 @@ static int __devinit ad2s1210_probe(struct spi_device *spi)
 	if (spi->dev.platform_data == NULL)
 		return -EINVAL;
 
-	indio_dev = iio_allocate_device(sizeof(*st));
+	indio_dev = iio_device_alloc(sizeof(*st));
 	if (indio_dev == NULL) {
 		ret = -ENOMEM;
 		goto error_ret;
@@ -729,7 +731,7 @@ static int __devinit ad2s1210_probe(struct spi_device *spi)
 error_free_gpios:
 	ad2s1210_free_gpios(st);
 error_free_dev:
-	iio_free_device(indio_dev);
+	iio_device_free(indio_dev);
 error_ret:
 	return ret;
 }
@@ -740,7 +742,7 @@ static int __devexit ad2s1210_remove(struct spi_device *spi)
 
 	iio_device_unregister(indio_dev);
 	ad2s1210_free_gpios(iio_priv(indio_dev));
-	iio_free_device(indio_dev);
+	iio_device_free(indio_dev);
 
 	return 0;
 }
diff --git a/drivers/staging/iio/resolver/ad2s90.c b/drivers/staging/iio/resolver/ad2s90.c
index 2a86f582ddf..a8057228dca 100644
--- a/drivers/staging/iio/resolver/ad2s90.c
+++ b/drivers/staging/iio/resolver/ad2s90.c
@@ -16,8 +16,8 @@
 #include <linux/sysfs.h>
 #include <linux/module.h>
 
-#include "../iio.h"
-#include "../sysfs.h"
+#include <linux/iio/iio.h>
+#include <linux/iio/sysfs.h>
 
 struct ad2s90_state {
 	struct mutex lock;
@@ -55,6 +55,7 @@ static const struct iio_chan_spec ad2s90_chan = {
 	.type = IIO_ANGL,
 	.indexed = 1,
 	.channel = 0,
+	.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT,
 };
 
 static int __devinit ad2s90_probe(struct spi_device *spi)
@@ -63,7 +64,7 @@ static int __devinit ad2s90_probe(struct spi_device *spi)
 	struct ad2s90_state *st;
 	int ret = 0;
 
-	indio_dev = iio_allocate_device(sizeof(*st));
+	indio_dev = iio_device_alloc(sizeof(*st));
 	if (indio_dev == NULL) {
 		ret = -ENOMEM;
 		goto error_ret;
@@ -92,7 +93,7 @@ static int __devinit ad2s90_probe(struct spi_device *spi)
 	return 0;
 
 error_free_dev:
-	iio_free_device(indio_dev);
+	iio_device_free(indio_dev);
 error_ret:
 	return ret;
 }
@@ -100,7 +101,7 @@ error_ret:
 static int __devexit ad2s90_remove(struct spi_device *spi)
 {
 	iio_device_unregister(spi_get_drvdata(spi));
-	iio_free_device(spi_get_drvdata(spi));
+	iio_device_free(spi_get_drvdata(spi));
 
 	return 0;
 }
diff --git a/drivers/staging/iio/ring_sw.c b/drivers/staging/iio/ring_sw.c
index b9945ec44fa..9358c6cb1c7 100644
--- a/drivers/staging/iio/ring_sw.c
+++ b/drivers/staging/iio/ring_sw.c
@@ -15,7 +15,7 @@
 #include <linux/sched.h>
 #include <linux/poll.h>
 #include "ring_sw.h"
-#include "trigger.h"
+#include <linux/iio/trigger.h>
 
 /**
  * struct iio_sw_ring_buffer - software ring buffer
diff --git a/drivers/staging/iio/ring_sw.h b/drivers/staging/iio/ring_sw.h
index 7556e212236..a5857aa7aef 100644
--- a/drivers/staging/iio/ring_sw.h
+++ b/drivers/staging/iio/ring_sw.h
@@ -23,7 +23,7 @@
 
 #ifndef _IIO_RING_SW_H_
 #define _IIO_RING_SW_H_
-#include "buffer.h"
+#include <linux/iio/buffer.h>
 
 struct iio_buffer *iio_sw_rb_allocate(struct iio_dev *indio_dev);
 void iio_sw_rb_free(struct iio_buffer *ring);
diff --git a/drivers/staging/iio/sysfs.h b/drivers/staging/iio/sysfs.h
deleted file mode 100644
index bfedb73b850..00000000000
--- a/drivers/staging/iio/sysfs.h
+++ /dev/null
@@ -1,117 +0,0 @@
-/* The industrial I/O core
- *
- *Copyright (c) 2008 Jonathan Cameron
- *
- * 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.
- *
- * General attributes
- */
-
-#ifndef _INDUSTRIAL_IO_SYSFS_H_
-#define _INDUSTRIAL_IO_SYSFS_H_
-
-struct iio_chan_spec;
-
-/**
- * struct iio_dev_attr - iio specific device attribute
- * @dev_attr:	underlying device attribute
- * @address:	associated register address
- * @l:		list head for maintaining list of dynamically created attrs.
- */
-struct iio_dev_attr {
-	struct device_attribute dev_attr;
-	u64 address;
-	struct list_head l;
-	struct iio_chan_spec const *c;
-};
-
-#define to_iio_dev_attr(_dev_attr)				\
-	container_of(_dev_attr, struct iio_dev_attr, dev_attr)
-
-ssize_t iio_read_const_attr(struct device *dev,
-			    struct device_attribute *attr,
-			    char *len);
-
-/**
- * struct iio_const_attr - constant device specific attribute
- *                         often used for things like available modes
- * @string:	attribute string
- * @dev_attr:	underlying device attribute
- */
-struct iio_const_attr {
-	const char *string;
-	struct device_attribute dev_attr;
-};
-
-#define to_iio_const_attr(_dev_attr) \
-	container_of(_dev_attr, struct iio_const_attr, dev_attr)
-
-/* Some attributes will be hard coded (device dependent) and not require an
-   address, in these cases pass a negative */
-#define IIO_ATTR(_name, _mode, _show, _store, _addr)		\
-	{ .dev_attr = __ATTR(_name, _mode, _show, _store),	\
-	  .address = _addr }
-
-#define IIO_DEVICE_ATTR(_name, _mode, _show, _store, _addr)	\
-	struct iio_dev_attr iio_dev_attr_##_name		\
-	= IIO_ATTR(_name, _mode, _show, _store, _addr)
-
-#define IIO_DEVICE_ATTR_NAMED(_vname, _name, _mode, _show, _store, _addr) \
-	struct iio_dev_attr iio_dev_attr_##_vname			\
-	= IIO_ATTR(_name, _mode, _show, _store, _addr)
-
-#define IIO_CONST_ATTR(_name, _string)					\
-	struct iio_const_attr iio_const_attr_##_name			\
-	= { .string = _string,						\
-	    .dev_attr = __ATTR(_name, S_IRUGO, iio_read_const_attr, NULL)}
-
-#define IIO_CONST_ATTR_NAMED(_vname, _name, _string)			\
-	struct iio_const_attr iio_const_attr_##_vname			\
-	= { .string = _string,						\
-	    .dev_attr = __ATTR(_name, S_IRUGO, iio_read_const_attr, NULL)}
-
-/* Generic attributes of onetype or another */
-/**
- * IIO_DEV_ATTR_RESET: resets the device
- **/
-#define IIO_DEV_ATTR_RESET(_store)			\
-	IIO_DEVICE_ATTR(reset, S_IWUSR, NULL, _store, 0)
-
-/**
- * IIO_DEV_ATTR_SAMP_FREQ - sets any internal clock frequency
- * @_mode: sysfs file mode/permissions
- * @_show: output method for the attribute
- * @_store: input method for the attribute
- **/
-#define IIO_DEV_ATTR_SAMP_FREQ(_mode, _show, _store)			\
-	IIO_DEVICE_ATTR(sampling_frequency, _mode, _show, _store, 0)
-
-/**
- * IIO_DEV_ATTR_SAMP_FREQ_AVAIL - list available sampling frequencies
- * @_show: output method for the attribute
- *
- * May be mode dependent on some devices
- **/
-#define IIO_DEV_ATTR_SAMP_FREQ_AVAIL(_show)				\
-	IIO_DEVICE_ATTR(sampling_frequency_available, S_IRUGO, _show, NULL, 0)
-/**
- * IIO_CONST_ATTR_AVAIL_SAMP_FREQ - list available sampling frequencies
- * @_string: frequency string for the attribute
- *
- * Constant version
- **/
-#define IIO_CONST_ATTR_SAMP_FREQ_AVAIL(_string)			\
-	IIO_CONST_ATTR(sampling_frequency_available, _string)
-
-#define IIO_DEV_ATTR_TEMP_RAW(_show)			\
-	IIO_DEVICE_ATTR(in_temp_raw, S_IRUGO, _show, NULL, 0)
-
-#define IIO_CONST_ATTR_TEMP_OFFSET(_string)		\
-	IIO_CONST_ATTR(in_temp_offset, _string)
-
-#define IIO_CONST_ATTR_TEMP_SCALE(_string)		\
-	IIO_CONST_ATTR(in_temp_scale, _string)
-
-#endif /* _INDUSTRIAL_IO_SYSFS_H_ */
diff --git a/drivers/staging/iio/trigger.h b/drivers/staging/iio/trigger.h
deleted file mode 100644
index 1cfca231db8..00000000000
--- a/drivers/staging/iio/trigger.h
+++ /dev/null
@@ -1,119 +0,0 @@
-/* The industrial I/O core, trigger handling functions
- *
- * Copyright (c) 2008 Jonathan Cameron
- *
- * 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/irq.h>
-#include <linux/module.h>
-
-#ifndef _IIO_TRIGGER_H_
-#define _IIO_TRIGGER_H_
-
-struct iio_subirq {
-	bool enabled;
-};
-
-/**
- * struct iio_trigger_ops - operations structure for an iio_trigger.
- * @owner:		used to monitor usage count of the trigger.
- * @set_trigger_state:	switch on/off the trigger on demand
- * @try_reenable:	function to reenable the trigger when the
- *			use count is zero (may be NULL)
- * @validate_device:	function to validate the device when the
- *			current trigger gets changed.
- *
- * This is typically static const within a driver and shared by
- * instances of a given device.
- **/
-struct iio_trigger_ops {
-	struct module			*owner;
-	int (*set_trigger_state)(struct iio_trigger *trig, bool state);
-	int (*try_reenable)(struct iio_trigger *trig);
-	int (*validate_device)(struct iio_trigger *trig,
-			       struct iio_dev *indio_dev);
-};
-
-
-/**
- * struct iio_trigger - industrial I/O trigger device
- *
- * @id:			[INTERN] unique id number
- * @name:		[DRIVER] unique name
- * @dev:		[DRIVER] associated device (if relevant)
- * @private_data:	[DRIVER] device specific data
- * @list:		[INTERN] used in maintenance of global trigger list
- * @alloc_list:		[DRIVER] used for driver specific trigger list
- * @use_count:		use count for the trigger
- * @subirq_chip:	[INTERN] associate 'virtual' irq chip.
- * @subirq_base:	[INTERN] base number for irqs provided by trigger.
- * @subirqs:		[INTERN] information about the 'child' irqs.
- * @pool:		[INTERN] bitmap of irqs currently in use.
- * @pool_lock:		[INTERN] protection of the irq pool.
- **/
-struct iio_trigger {
-	const struct iio_trigger_ops	*ops;
-	int				id;
-	const char			*name;
-	struct device			dev;
-
-	void				*private_data;
-	struct list_head		list;
-	struct list_head		alloc_list;
-	int use_count;
-
-	struct irq_chip			subirq_chip;
-	int				subirq_base;
-
-	struct iio_subirq subirqs[CONFIG_IIO_CONSUMERS_PER_TRIGGER];
-	unsigned long pool[BITS_TO_LONGS(CONFIG_IIO_CONSUMERS_PER_TRIGGER)];
-	struct mutex			pool_lock;
-};
-
-
-static inline struct iio_trigger *to_iio_trigger(struct device *d)
-{
-	return container_of(d, struct iio_trigger, dev);
-};
-
-static inline void iio_put_trigger(struct iio_trigger *trig)
-{
-	module_put(trig->ops->owner);
-	put_device(&trig->dev);
-};
-
-static inline void iio_get_trigger(struct iio_trigger *trig)
-{
-	get_device(&trig->dev);
-	__module_get(trig->ops->owner);
-};
-
-/**
- * iio_trigger_register() - register a trigger with the IIO core
- * @trig_info:	trigger to be registered
- **/
-int iio_trigger_register(struct iio_trigger *trig_info);
-
-/**
- * iio_trigger_unregister() - unregister a trigger from the core
- * @trig_info:	trigger to be unregistered
- **/
-void iio_trigger_unregister(struct iio_trigger *trig_info);
-
-/**
- * iio_trigger_poll() - called on a trigger occurring
- * @trig: trigger which occurred
- *
- * Typically called in relevant hardware interrupt handler.
- **/
-void iio_trigger_poll(struct iio_trigger *trig, s64 time);
-void iio_trigger_poll_chained(struct iio_trigger *trig, s64 time);
-
-irqreturn_t iio_trigger_generic_data_rdy_poll(int irq, void *private);
-
-__printf(1, 2) struct iio_trigger *iio_allocate_trigger(const char *fmt, ...);
-void iio_free_trigger(struct iio_trigger *trig);
-
-#endif /* _IIO_TRIGGER_H_ */
diff --git a/drivers/staging/iio/trigger/iio-trig-bfin-timer.c b/drivers/staging/iio/trigger/iio-trig-bfin-timer.c
index 665653d79f0..f85734d212b 100644
--- a/drivers/staging/iio/trigger/iio-trig-bfin-timer.c
+++ b/drivers/staging/iio/trigger/iio-trig-bfin-timer.c
@@ -15,8 +15,8 @@
 
 #include <asm/gptimers.h>
 
-#include "../iio.h"
-#include "../trigger.h"
+#include <linux/iio/iio.h>
+#include <linux/iio/trigger.h>
 
 struct bfin_timer {
 	unsigned short id, bit;
@@ -172,7 +172,7 @@ static int __devinit iio_bfin_tmr_trigger_probe(struct platform_device *pdev)
 	st->timer_num = ret;
 	st->t = &iio_bfin_timer_code[st->timer_num];
 
-	st->trig = iio_allocate_trigger("bfintmr%d", st->timer_num);
+	st->trig = iio_trigger_alloc("bfintmr%d", st->timer_num);
 	if (!st->trig) {
 		ret = -ENOMEM;
 		goto out1;
@@ -203,7 +203,7 @@ static int __devinit iio_bfin_tmr_trigger_probe(struct platform_device *pdev)
 out4:
 	iio_trigger_unregister(st->trig);
 out2:
-	iio_put_trigger(st->trig);
+	iio_trigger_put(st->trig);
 out1:
 	kfree(st);
 out:
@@ -217,7 +217,7 @@ static int __devexit iio_bfin_tmr_trigger_remove(struct platform_device *pdev)
 	disable_gptimers(st->t->bit);
 	free_irq(st->irq, st);
 	iio_trigger_unregister(st->trig);
-	iio_put_trigger(st->trig);
+	iio_trigger_put(st->trig);
 	kfree(st);
 
 	return 0;
diff --git a/drivers/staging/iio/trigger/iio-trig-gpio.c b/drivers/staging/iio/trigger/iio-trig-gpio.c
index a3465947235..90b26846fc6 100644
--- a/drivers/staging/iio/trigger/iio-trig-gpio.c
+++ b/drivers/staging/iio/trigger/iio-trig-gpio.c
@@ -22,8 +22,8 @@
 #include <linux/gpio.h>
 #include <linux/slab.h>
 
-#include "../iio.h"
-#include "../trigger.h"
+#include <linux/iio/iio.h>
+#include <linux/iio/trigger.h>
 
 static LIST_HEAD(iio_gpio_trigger_list);
 static DEFINE_MUTEX(iio_gpio_trigger_list_lock);
@@ -72,7 +72,7 @@ static int iio_gpio_trigger_probe(struct platform_device *pdev)
 
 		for (irq = irq_res->start; irq <= irq_res->end; irq++) {
 
-			trig = iio_allocate_trigger("irqtrig%d", irq);
+			trig = iio_trigger_alloc("irqtrig%d", irq);
 			if (!trig) {
 				ret = -ENOMEM;
 				goto error_free_completed_registrations;
@@ -114,7 +114,7 @@ error_release_irq:
 error_free_trig_info:
 	kfree(trig_info);
 error_put_trigger:
-	iio_put_trigger(trig);
+	iio_trigger_put(trig);
 error_free_completed_registrations:
 	/* The rest should have been added to the iio_gpio_trigger_list */
 	list_for_each_entry_safe(trig,
@@ -144,7 +144,7 @@ static int iio_gpio_trigger_remove(struct platform_device *pdev)
 		iio_trigger_unregister(trig);
 		free_irq(trig_info->irq, trig);
 		kfree(trig_info);
-		iio_put_trigger(trig);
+		iio_trigger_put(trig);
 	}
 	mutex_unlock(&iio_gpio_trigger_list_lock);
 
diff --git a/drivers/staging/iio/trigger/iio-trig-periodic-rtc.c b/drivers/staging/iio/trigger/iio-trig-periodic-rtc.c
index a80cf67bf84..9f2d055524a 100644
--- a/drivers/staging/iio/trigger/iio-trig-periodic-rtc.c
+++ b/drivers/staging/iio/trigger/iio-trig-periodic-rtc.c
@@ -16,8 +16,8 @@
 #include <linux/module.h>
 #include <linux/slab.h>
 #include <linux/rtc.h>
-#include "../iio.h"
-#include "../trigger.h"
+#include <linux/iio/iio.h>
+#include <linux/iio/trigger.h>
 
 static LIST_HEAD(iio_prtc_trigger_list);
 static DEFINE_MUTEX(iio_prtc_trigger_list_lock);
@@ -112,7 +112,7 @@ static int iio_trig_periodic_rtc_probe(struct platform_device *dev)
 	for (i = 0;; i++) {
 		if (pdata[i] == NULL)
 			break;
-		trig = iio_allocate_trigger("periodic%s", pdata[i]);
+		trig = iio_trigger_alloc("periodic%s", pdata[i]);
 		if (!trig) {
 			ret = -ENOMEM;
 			goto error_free_completed_registrations;
@@ -152,7 +152,7 @@ error_free_trig_info:
 	kfree(trig_info);
 error_put_trigger_and_remove_from_list:
 	list_del(&trig->alloc_list);
-	iio_put_trigger(trig);
+	iio_trigger_put(trig);
 error_free_completed_registrations:
 	list_for_each_entry_safe(trig,
 				 trig2,
diff --git a/drivers/staging/iio/trigger/iio-trig-sysfs.c b/drivers/staging/iio/trigger/iio-trig-sysfs.c
index 174dc65709d..552763bb3d4 100644
--- a/drivers/staging/iio/trigger/iio-trig-sysfs.c
+++ b/drivers/staging/iio/trigger/iio-trig-sysfs.c
@@ -11,8 +11,8 @@
 #include <linux/slab.h>
 #include <linux/list.h>
 
-#include "../iio.h"
-#include "../trigger.h"
+#include <linux/iio/iio.h>
+#include <linux/iio/trigger.h>
 
 struct iio_sysfs_trig {
 	struct iio_trigger *trig;
@@ -139,7 +139,7 @@ static int iio_sysfs_trigger_probe(int id)
 		goto out1;
 	}
 	t->id = id;
-	t->trig = iio_allocate_trigger("sysfstrig%d", id);
+	t->trig = iio_trigger_alloc("sysfstrig%d", id);
 	if (!t->trig) {
 		ret = -ENOMEM;
 		goto free_t;
@@ -158,7 +158,7 @@ static int iio_sysfs_trigger_probe(int id)
 	return 0;
 
 out2:
-	iio_put_trigger(t->trig);
+	iio_trigger_put(t->trig);
 free_t:
 	kfree(t);
 out1:
@@ -182,7 +182,7 @@ static int iio_sysfs_trigger_remove(int id)
 	}
 
 	iio_trigger_unregister(t->trig);
-	iio_free_trigger(t->trig);
+	iio_trigger_free(t->trig);
 
 	list_del(&t->l);
 	kfree(t);
diff --git a/drivers/staging/iio/trigger_consumer.h b/drivers/staging/iio/trigger_consumer.h
deleted file mode 100644
index 60d64b35694..00000000000
--- a/drivers/staging/iio/trigger_consumer.h
+++ /dev/null
@@ -1,52 +0,0 @@
-/* The industrial I/O core, trigger consumer functions
- *
- * Copyright (c) 2008-2011 Jonathan Cameron
- *
- * This program is free software; you can redistribute it and/or modify it
- * under the terms of the GNU General Public License version 2 as published by
- * the Free Software Foundation.
- */
-
-/**
- * struct iio_poll_func - poll function pair
- *
- * @indio_dev:			data specific to device (passed into poll func)
- * @h:				the function that is actually run on trigger
- * @thread:			threaded interrupt part
- * @type:			the type of interrupt (basically if oneshot)
- * @name:			name used to identify the trigger consumer.
- * @irq:			the corresponding irq as allocated from the
- *				trigger pool
- * @timestamp:			some devices need a timestamp grabbed as soon
- *				as possible after the trigger - hence handler
- *				passes it via here.
- **/
-struct iio_poll_func {
-	struct iio_dev *indio_dev;
-	irqreturn_t (*h)(int irq, void *p);
-	irqreturn_t (*thread)(int irq, void *p);
-	int type;
-	char *name;
-	int irq;
-	s64 timestamp;
-};
-
-
-struct iio_poll_func
-*iio_alloc_pollfunc(irqreturn_t (*h)(int irq, void *p),
-		    irqreturn_t (*thread)(int irq, void *p),
-		    int type,
-		    struct iio_dev *indio_dev,
-		    const char *fmt,
-		    ...);
-void iio_dealloc_pollfunc(struct iio_poll_func *pf);
-irqreturn_t iio_pollfunc_store_time(int irq, void *p);
-
-void iio_trigger_notify_done(struct iio_trigger *trig);
-
-/*
- * Two functions for common case where all that happens is a pollfunc
- * is attached and detached from a trigger
- */
-int iio_triggered_buffer_postenable(struct iio_dev *indio_dev);
-int iio_triggered_buffer_predisable(struct iio_dev *indio_dev);
diff --git a/drivers/staging/iio/types.h b/drivers/staging/iio/types.h
deleted file mode 100644
index 0c321366690..00000000000
--- a/drivers/staging/iio/types.h
+++ /dev/null
@@ -1,53 +0,0 @@
-/* industrial I/O data types needed both in and out of kernel
- *
- * Copyright (c) 2008 Jonathan Cameron
- *
- * 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 _IIO_TYPES_H_
-#define _IIO_TYPES_H_
-
-enum iio_chan_type {
-	/* real channel types */
-	IIO_VOLTAGE,
-	IIO_CURRENT,
-	IIO_POWER,
-	IIO_ACCEL,
-	IIO_ANGL_VEL,
-	IIO_MAGN,
-	IIO_LIGHT,
-	IIO_INTENSITY,
-	IIO_PROXIMITY,
-	IIO_TEMP,
-	IIO_INCLI,
-	IIO_ROT,
-	IIO_ANGL,
-	IIO_TIMESTAMP,
-	IIO_CAPACITANCE,
-};
-
-enum iio_modifier {
-	IIO_NO_MOD,
-	IIO_MOD_X,
-	IIO_MOD_Y,
-	IIO_MOD_Z,
-	IIO_MOD_X_AND_Y,
-	IIO_MOD_X_AND_Z,
-	IIO_MOD_Y_AND_Z,
-	IIO_MOD_X_AND_Y_AND_Z,
-	IIO_MOD_X_OR_Y,
-	IIO_MOD_X_OR_Z,
-	IIO_MOD_Y_OR_Z,
-	IIO_MOD_X_OR_Y_OR_Z,
-	IIO_MOD_LIGHT_BOTH,
-	IIO_MOD_LIGHT_IR,
-};
-
-#define IIO_VAL_INT 1
-#define IIO_VAL_INT_PLUS_MICRO 2
-#define IIO_VAL_INT_PLUS_NANO 3
-
-#endif /* _IIO_TYPES_H_ */
diff --git a/drivers/staging/ipack/Kconfig b/drivers/staging/ipack/Kconfig
new file mode 100644
index 00000000000..af321789ddd
--- /dev/null
+++ b/drivers/staging/ipack/Kconfig
@@ -0,0 +1,20 @@
+#
+# IPACK configuration.
+#
+
+menuconfig IPACK_BUS
+	tristate "IndustryPack bus support"
+	---help---
+	  If you say Y here you get support for the IndustryPack Framework
+	  for drivers for many types of boards that support this industrial
+	  bus. The IndustryPack Framework is a virtual bus allowing to
+	  communicate between carrier and mezzanine cards connected through
+	  this bus.
+
+if IPACK_BUS
+
+source "drivers/staging/ipack/bridges/Kconfig"
+
+source "drivers/staging/ipack/devices/Kconfig"
+
+endif # IPACK
diff --git a/drivers/staging/ipack/Makefile b/drivers/staging/ipack/Makefile
new file mode 100644
index 00000000000..85ff223616f
--- /dev/null
+++ b/drivers/staging/ipack/Makefile
@@ -0,0 +1,6 @@
+#
+# Makefile for the IPACK bridge device drivers.
+#
+obj-$(CONFIG_IPACK_BUS)		+= ipack.o
+obj-y				+= devices/
+obj-y				+= bridges/
diff --git a/drivers/staging/ipack/TODO b/drivers/staging/ipack/TODO
new file mode 100644
index 00000000000..3a45a53afd8
--- /dev/null
+++ b/drivers/staging/ipack/TODO
@@ -0,0 +1,46 @@
+				TODO
+				====
+Introduction
+============
+
+These drivers add support for IndustryPack devices: carrier and mezzanine
+boards.
+
+The ipack driver is just an abstraction of the bus providing the common
+operations between the two kind of boards.
+
+TODO
+====
+
+TPCI-200
+--------
+
+* It receives the name of the mezzanine plugged in each slot by SYSFS.
+  No autodetection supported yet, because the mezzanine driver could not be
+  loaded at the time that the tpci200 driver loads.
+
+* It has a linked list with the tpci200 devices it is managing. Get rid of it
+  and use driver_for_each_device() instead.
+
+IP-OCTAL
+--------
+
+* It has a linked list which saves the devices it is currently
+  managing. It should use the driver_for_each_device() function. It is not there
+  due to the impossibility of using container_of macro to recover the
+  corresponding "struct ipoctal" because the attribute "struct ipack_device" is
+  a pointer. This code should be refactored.
+
+Ipack
+-----
+
+* The structures and API exported can be improved a lot. For example, the
+  way to unregistering mezzanine devices, doing the mezzanine driver a call to
+  remove_device() to notify the carrier driver, or the opposite with the call to
+  the ipack_driver_ops' remove() function could be improved.
+
+
+Contact
+=======
+
+Contact: Samuel Iglesias Gonsalvez <siglesias@igalia.com>
\ No newline at end of file
diff --git a/drivers/staging/ipack/bridges/Kconfig b/drivers/staging/ipack/bridges/Kconfig
new file mode 100644
index 00000000000..97c837ea7a0
--- /dev/null
+++ b/drivers/staging/ipack/bridges/Kconfig
@@ -0,0 +1,8 @@
+config BOARD_TPCI200
+	tristate "TEWS TPCI-200 support for IndustryPack bus"
+	depends on IPACK_BUS
+	depends on PCI
+	help
+	  This driver supports the TEWS TPCI200 device for the IndustryPack bus.
+	default n
+
diff --git a/drivers/staging/ipack/bridges/Makefile b/drivers/staging/ipack/bridges/Makefile
new file mode 100644
index 00000000000..d8b76459300
--- /dev/null
+++ b/drivers/staging/ipack/bridges/Makefile
@@ -0,0 +1 @@
+obj-$(CONFIG_BOARD_TPCI200) += tpci200.o
diff --git a/drivers/staging/ipack/bridges/tpci200.c b/drivers/staging/ipack/bridges/tpci200.c
new file mode 100644
index 00000000000..ad287075023
--- /dev/null
+++ b/drivers/staging/ipack/bridges/tpci200.c
@@ -0,0 +1,1141 @@
+/**
+ * tpci200.c
+ *
+ * driver for the TEWS TPCI-200 device
+ * Copyright (c) 2009 Nicolas Serafini, EIC2 SA
+ * Copyright (c) 2010,2011 Samuel Iglesias Gonsalvez <siglesia@cern.ch>, CERN
+ * Copyright (c) 2012 Samuel Iglesias Gonsalvez <siglesias@igalia.com>, Igalia
+ *
+ * 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.
+ */
+
+#define pr_fmt(fmt) KBUILD_MODNAME ": " fmt
+
+#include <linux/module.h>
+#include "tpci200.h"
+
+static struct ipack_bus_ops tpci200_bus_ops;
+
+/* TPCI200 controls registers */
+static int control_reg[] = {
+	TPCI200_CONTROL_A_REG,
+	TPCI200_CONTROL_B_REG,
+	TPCI200_CONTROL_C_REG,
+	TPCI200_CONTROL_D_REG
+};
+
+/* Linked list to save the registered devices */
+static LIST_HEAD(tpci200_list);
+
+static int tpci200_slot_unregister(struct ipack_device *dev);
+
+static struct tpci200_board *check_slot(struct ipack_device *dev)
+{
+	struct tpci200_board *tpci200;
+	int found = 0;
+
+	if (dev == NULL) {
+		pr_info("Slot doesn't exist.\n");
+		return NULL;
+	}
+
+	list_for_each_entry(tpci200, &tpci200_list, list) {
+		if (tpci200->number == dev->bus_nr) {
+			found = 1;
+			break;
+		}
+	}
+
+	if (!found) {
+		pr_err("Carrier not found\n");
+		return NULL;
+	}
+
+	if (dev->slot >= TPCI200_NB_SLOT) {
+		pr_info("Slot [%d:%d] doesn't exist! Last tpci200 slot is %d.\n",
+			dev->bus_nr, dev->slot, TPCI200_NB_SLOT-1);
+		return NULL;
+	}
+
+	BUG_ON(tpci200->slots == NULL);
+	if (tpci200->slots[dev->slot].dev == NULL) {
+		pr_info("Slot [%d:%d] is not registered !\n", dev->bus_nr,
+			dev->slot);
+		return NULL;
+	}
+
+	return tpci200;
+}
+
+static inline unsigned char __tpci200_read8(void __iomem *address,
+					    unsigned long offset)
+{
+	return ioread8(address + (offset^1));
+}
+
+static inline unsigned short __tpci200_read16(void __iomem *address,
+					      unsigned long offset)
+{
+	return ioread16(address + offset);
+}
+
+static inline unsigned int __tpci200_read32(void __iomem *address,
+					    unsigned long offset)
+{
+	return swahw32(ioread32(address + offset));
+}
+
+static inline void __tpci200_write8(unsigned char value,
+				    void __iomem *address, unsigned long offset)
+{
+	iowrite8(value, address+(offset^1));
+}
+
+static inline void __tpci200_write16(unsigned short value,
+				     void __iomem *address,
+				     unsigned long offset)
+{
+	iowrite16(value, address+offset);
+}
+
+static inline void __tpci200_write32(unsigned int value,
+				     void __iomem *address,
+				     unsigned long offset)
+{
+	iowrite32(swahw32(value), address+offset);
+}
+
+static struct ipack_addr_space *get_slot_address_space(struct ipack_device *dev,
+						       int space)
+{
+	struct ipack_addr_space *addr;
+
+	switch (space) {
+	case IPACK_IO_SPACE:
+		addr = &dev->io_space;
+		break;
+	case IPACK_ID_SPACE:
+		addr = &dev->id_space;
+		break;
+	case IPACK_MEM_SPACE:
+		addr = &dev->mem_space;
+		break;
+	default:
+		pr_err("Slot [%d:%d] space number %d doesn't exist !\n",
+		       dev->bus_nr, dev->slot, space);
+		return NULL;
+		break;
+	}
+
+	if ((addr->size == 0) || (addr->address == NULL)) {
+		pr_err("Error, slot space not mapped !\n");
+		return NULL;
+	}
+
+	return addr;
+}
+
+static int tpci200_read8(struct ipack_device *dev, int space,
+			 unsigned long offset, unsigned char *value)
+{
+	struct ipack_addr_space *addr;
+	struct tpci200_board *tpci200;
+
+	tpci200 = check_slot(dev);
+	if (tpci200 == NULL)
+		return -EINVAL;
+
+	addr = get_slot_address_space(dev, space);
+	if (addr == NULL)
+		return -EINVAL;
+
+	if (offset >= addr->size) {
+		pr_err("Error, slot space offset error !\n");
+		return -EFAULT;
+	}
+
+	*value = __tpci200_read8(addr->address, offset);
+
+	return 0;
+}
+
+static int tpci200_read16(struct ipack_device *dev, int space,
+			  unsigned long offset, unsigned short *value)
+{
+	struct ipack_addr_space *addr;
+	struct tpci200_board *tpci200;
+
+	tpci200 = check_slot(dev);
+	if (tpci200 == NULL)
+		return -EINVAL;
+
+	addr = get_slot_address_space(dev, space);
+	if (addr == NULL)
+		return -EINVAL;
+
+	if ((offset+2) >= addr->size) {
+		pr_err("Error, slot space offset error !\n");
+		return -EFAULT;
+	}
+	*value = __tpci200_read16(addr->address, offset);
+
+	return 0;
+}
+
+static int tpci200_read32(struct ipack_device *dev, int space,
+			  unsigned long offset, unsigned int *value)
+{
+	struct ipack_addr_space *addr;
+	struct tpci200_board *tpci200;
+
+	tpci200 = check_slot(dev);
+	if (tpci200 == NULL)
+		return -EINVAL;
+
+	addr = get_slot_address_space(dev, space);
+	if (addr == NULL)
+		return -EINVAL;
+
+	if ((offset+4) >= addr->size) {
+		pr_err("Error, slot space offset error !\n");
+		return -EFAULT;
+	}
+
+	*value = __tpci200_read32(addr->address, offset);
+
+	return 0;
+}
+
+static int tpci200_write8(struct ipack_device *dev, int space,
+			  unsigned long offset, unsigned char value)
+{
+	struct ipack_addr_space *addr;
+	struct tpci200_board *tpci200;
+
+	tpci200 = check_slot(dev);
+	if (tpci200 == NULL)
+		return -EINVAL;
+
+	addr = get_slot_address_space(dev, space);
+	if (addr == NULL)
+		return -EINVAL;
+
+	if (offset >= addr->size) {
+		pr_err("Error, slot space offset error !\n");
+		return -EFAULT;
+	}
+
+	__tpci200_write8(value, addr->address, offset);
+
+	return 0;
+}
+
+static int tpci200_write16(struct ipack_device *dev, int space,
+			   unsigned long offset, unsigned short value)
+{
+	struct ipack_addr_space *addr;
+	struct tpci200_board *tpci200;
+
+	tpci200 = check_slot(dev);
+	if (tpci200 == NULL)
+		return -EINVAL;
+
+	addr = get_slot_address_space(dev, space);
+	if (addr == NULL)
+		return -EINVAL;
+
+	if ((offset+2) >= addr->size) {
+		pr_err("Error, slot space offset error !\n");
+		return -EFAULT;
+	}
+
+	__tpci200_write16(value, addr->address, offset);
+
+	return 0;
+}
+
+static int tpci200_write32(struct ipack_device *dev, int space,
+			   unsigned long offset, unsigned int value)
+{
+	struct ipack_addr_space *addr;
+	struct tpci200_board *tpci200;
+
+	tpci200 = check_slot(dev);
+	if (tpci200 == NULL)
+		return -EINVAL;
+
+	addr = get_slot_address_space(dev, space);
+	if (addr == NULL)
+		return -EINVAL;
+
+	if ((offset+4) >= addr->size) {
+		pr_err("Error, slot space offset error !\n");
+		return -EFAULT;
+	}
+
+	__tpci200_write32(value, addr->address, offset);
+
+	return 0;
+}
+
+static void tpci200_unregister(struct tpci200_board *tpci200)
+{
+	int i;
+
+	free_irq(tpci200->info->pdev->irq, (void *) tpci200);
+
+	pci_iounmap(tpci200->info->pdev, tpci200->info->interface_regs);
+	pci_iounmap(tpci200->info->pdev, tpci200->info->ioidint_space);
+	pci_iounmap(tpci200->info->pdev, tpci200->info->mem8_space);
+
+	pci_release_region(tpci200->info->pdev, TPCI200_IP_INTERFACE_BAR);
+	pci_release_region(tpci200->info->pdev, TPCI200_IO_ID_INT_SPACES_BAR);
+	pci_release_region(tpci200->info->pdev, TPCI200_MEM8_SPACE_BAR);
+
+	pci_disable_device(tpci200->info->pdev);
+	pci_dev_put(tpci200->info->pdev);
+
+	kfree(tpci200->info);
+
+	for (i = 0; i < TPCI200_NB_SLOT; i++) {
+		tpci200->slots[i].io_phys.address = NULL;
+		tpci200->slots[i].io_phys.size = 0;
+		tpci200->slots[i].id_phys.address = NULL;
+		tpci200->slots[i].id_phys.size = 0;
+		tpci200->slots[i].mem_phys.address = NULL;
+		tpci200->slots[i].mem_phys.size = 0;
+	}
+}
+
+static irqreturn_t tpci200_interrupt(int irq, void *dev_id)
+{
+	struct tpci200_board *tpci200 = (struct tpci200_board *) dev_id;
+	int i;
+	unsigned long flags;
+	unsigned short status_reg, reg_value;
+	unsigned short unhandled_ints = 0;
+	irqreturn_t ret = IRQ_NONE;
+
+	spin_lock_irqsave(&tpci200->info->access_lock, flags);
+
+	/* Read status register */
+	status_reg = readw(tpci200->info->interface_regs +
+			   TPCI200_STATUS_REG);
+
+	if (status_reg & TPCI200_SLOT_INT_MASK) {
+		unhandled_ints = status_reg & TPCI200_SLOT_INT_MASK;
+		/* callback to the IRQ handler for the corresponding slot */
+		for (i = 0; i < TPCI200_NB_SLOT; i++) {
+			if ((tpci200->slots[i].irq != NULL) &&
+			    (status_reg & ((TPCI200_A_INT0 | TPCI200_A_INT1) << (2*i)))) {
+
+				ret = tpci200->slots[i].irq->handler(tpci200->slots[i].irq->arg);
+
+				/* Dummy reads */
+				readw(tpci200->slots[i].dev->io_space.address +
+				      0xC0);
+				readw(tpci200->slots[i].dev->io_space.address +
+				      0xC2);
+
+				unhandled_ints &= ~(((TPCI200_A_INT0 | TPCI200_A_INT1) << (2*i)));
+			}
+		}
+	}
+	/* Interrupt not handled are disabled */
+	if (unhandled_ints) {
+		for (i = 0; i < TPCI200_NB_SLOT; i++) {
+			if (unhandled_ints & ((TPCI200_INT0_EN | TPCI200_INT1_EN) << (2*i))) {
+				pr_info("No registered ISR for slot [%d:%d]!. IRQ will be disabled.\n",
+					tpci200->number, i);
+				reg_value = readw(
+					tpci200->info->interface_regs +
+					control_reg[i]);
+				reg_value &=
+					~(TPCI200_INT0_EN | TPCI200_INT1_EN);
+				writew(reg_value,
+				       (tpci200->info->interface_regs +
+					control_reg[i]));
+			}
+		}
+	}
+
+	spin_unlock_irqrestore(&tpci200->info->access_lock, flags);
+	return ret;
+}
+
+#ifdef CONFIG_SYSFS
+
+static struct ipack_device *tpci200_slot_register(unsigned int tpci200_number,
+						  unsigned int slot_position)
+{
+	int found = 0;
+	struct ipack_device  *dev;
+	struct tpci200_board *tpci200;
+
+	list_for_each_entry(tpci200, &tpci200_list, list) {
+		if (tpci200->number == tpci200_number) {
+			found = 1;
+			break;
+		}
+	}
+
+	if (!found) {
+		pr_err("carrier board not found for the device\n");
+		return NULL;
+	}
+
+	if (slot_position >= TPCI200_NB_SLOT) {
+		pr_info("Slot [%d:%d] doesn't exist!\n", tpci200_number,
+			slot_position);
+		return NULL;
+	}
+
+	if (mutex_lock_interruptible(&tpci200->mutex))
+		return NULL;
+
+	if (tpci200->slots[slot_position].dev != NULL) {
+		pr_err("Slot [%d:%d] already installed !\n", tpci200_number,
+		       slot_position);
+		goto err_unlock;
+	}
+
+	/*
+	 * Give the same IRQ number as the slot number.
+	 * The TPCI200 has assigned his own two IRQ by PCI bus driver
+	 */
+	dev = ipack_device_register(tpci200->info->ipack_bus,
+				    slot_position, slot_position);
+	if (dev == NULL) {
+		pr_info("Slot [%d:%d] Unable to register an ipack device\n",
+			tpci200_number, slot_position);
+		goto err_unlock;
+	}
+
+	tpci200->slots[slot_position].dev = dev;
+	mutex_unlock(&tpci200->mutex);
+	return dev;
+
+err_unlock:
+	mutex_unlock(&tpci200->mutex);
+	return NULL;
+}
+
+static ssize_t tpci200_store_board(struct device *pdev, const char *buf,
+				   size_t count, int slot)
+{
+	struct tpci200_board *card = dev_get_drvdata(pdev);
+	struct ipack_device *dev = card->slots[slot].dev;
+
+	if (dev != NULL)
+		return -EBUSY;
+
+	dev = tpci200_slot_register(card->number, slot);
+	if (dev == NULL)
+		return -ENODEV;
+
+	return count;
+}
+
+static ssize_t tpci200_show_board(struct device *pdev, char *buf, int slot)
+{
+	struct tpci200_board *card = dev_get_drvdata(pdev);
+	struct ipack_device *dev = card->slots[slot].dev;
+
+	if (dev != NULL)
+		return snprintf(buf, PAGE_SIZE, "%s\n", dev_name(&dev->dev));
+	else
+		return snprintf(buf, PAGE_SIZE, "none\n");
+}
+
+static ssize_t tpci200_show_description(struct device *pdev,
+					struct device_attribute *attr,
+					char *buf)
+{
+	return snprintf(buf, PAGE_SIZE,
+			"TEWS tpci200 carrier PCI for Industry-pack mezzanines.\n");
+}
+
+static ssize_t tpci200_show_board_slot0(struct device *pdev,
+					struct device_attribute *attr,
+					char *buf)
+{
+	return tpci200_show_board(pdev, buf, 0);
+}
+
+static ssize_t tpci200_store_board_slot0(struct device *pdev,
+					 struct device_attribute *attr,
+					 const char *buf, size_t count)
+{
+	return tpci200_store_board(pdev, buf, count, 0);
+}
+
+static ssize_t tpci200_show_board_slot1(struct device *pdev,
+					struct device_attribute *attr,
+					char *buf)
+{
+	return tpci200_show_board(pdev, buf, 1);
+}
+
+static ssize_t tpci200_store_board_slot1(struct device *pdev,
+					 struct device_attribute *attr,
+					 const char *buf, size_t count)
+{
+	return tpci200_store_board(pdev, buf, count, 1);
+}
+
+static ssize_t tpci200_show_board_slot2(struct device *pdev,
+					struct device_attribute *attr,
+					char *buf)
+{
+	return tpci200_show_board(pdev, buf, 2);
+}
+
+static ssize_t tpci200_store_board_slot2(struct device *pdev,
+					 struct device_attribute *attr,
+					 const char *buf, size_t count)
+{
+	return tpci200_store_board(pdev, buf, count, 2);
+}
+
+
+static ssize_t tpci200_show_board_slot3(struct device *pdev,
+					struct device_attribute *attr,
+					char *buf)
+{
+	return tpci200_show_board(pdev, buf, 3);
+}
+
+static ssize_t tpci200_store_board_slot3(struct device *pdev,
+					 struct device_attribute *attr,
+					 const char *buf, size_t count)
+{
+	return tpci200_store_board(pdev, buf, count, 3);
+}
+
+/* Declaration of the device attributes for the TPCI200 */
+static DEVICE_ATTR(description, S_IRUGO,
+		   tpci200_show_description, NULL);
+static DEVICE_ATTR(board_slot0, S_IRUGO | S_IWUSR,
+		   tpci200_show_board_slot0, tpci200_store_board_slot0);
+static DEVICE_ATTR(board_slot1, S_IRUGO | S_IWUSR,
+		   tpci200_show_board_slot1, tpci200_store_board_slot1);
+static DEVICE_ATTR(board_slot2, S_IRUGO | S_IWUSR,
+		   tpci200_show_board_slot2, tpci200_store_board_slot2);
+static DEVICE_ATTR(board_slot3, S_IRUGO | S_IWUSR,
+		   tpci200_show_board_slot3, tpci200_store_board_slot3);
+
+static struct attribute *tpci200_attrs[] = {
+	&dev_attr_description.attr,
+	&dev_attr_board_slot0.attr,
+	&dev_attr_board_slot1.attr,
+	&dev_attr_board_slot2.attr,
+	&dev_attr_board_slot3.attr,
+	NULL,
+};
+
+static struct attribute_group tpci200_attr_group = {
+	.attrs = tpci200_attrs,
+};
+
+static int tpci200_create_sysfs_files(struct tpci200_board *card)
+{
+	return sysfs_create_group(&card->info->pdev->dev.kobj,
+				  &tpci200_attr_group);
+}
+
+static void tpci200_remove_sysfs_files(struct tpci200_board *card)
+{
+	sysfs_remove_group(&card->info->pdev->dev.kobj, &tpci200_attr_group);
+}
+
+#else
+
+static int tpci200_create_sysfs_files(struct tpci200_board *card)
+{
+	return 0;
+}
+
+static void tpci200_remove_sysfs_files(struct tpci200_board *card)
+{
+}
+
+#endif /* CONFIG_SYSFS */
+
+static int tpci200_register(struct tpci200_board *tpci200)
+{
+	int  i;
+	int res;
+	unsigned long ioidint_base;
+	unsigned long mem_base;
+	unsigned short slot_ctrl;
+
+	if (pci_enable_device(tpci200->info->pdev) < 0)
+		return -ENODEV;
+
+	if (tpci200_create_sysfs_files(tpci200) < 0) {
+		pr_err("failed creating sysfs files\n");
+		res = -EFAULT;
+		goto out_disable_pci;
+	}
+
+	/* Request IP interface register (Bar 2) */
+	res = pci_request_region(tpci200->info->pdev, TPCI200_IP_INTERFACE_BAR,
+				 "Carrier IP interface registers");
+	if (res) {
+		pr_err("(bn 0x%X, sn 0x%X) failed to allocate PCI resource for BAR 2 !",
+		       tpci200->info->pdev->bus->number,
+		       tpci200->info->pdev->devfn);
+		goto out_remove_sysfs;
+	}
+
+	/* Request IO ID INT space (Bar 3) */
+	res = pci_request_region(tpci200->info->pdev,
+				 TPCI200_IO_ID_INT_SPACES_BAR,
+				 "Carrier IO ID INT space");
+	if (res) {
+		pr_err("(bn 0x%X, sn 0x%X) failed to allocate PCI resource for BAR 3 !",
+		       tpci200->info->pdev->bus->number,
+		       tpci200->info->pdev->devfn);
+		goto out_release_ip_space;
+	}
+
+	/* Request MEM space (Bar 4) */
+	res = pci_request_region(tpci200->info->pdev, TPCI200_MEM8_SPACE_BAR,
+				 "Carrier MEM space");
+	if (res) {
+		pr_err("(bn 0x%X, sn 0x%X) failed to allocate PCI resource for BAR 4!",
+		       tpci200->info->pdev->bus->number,
+		       tpci200->info->pdev->devfn);
+		goto out_release_ioid_int_space;
+	}
+
+	/* Map internal tpci200 driver user space */
+	tpci200->info->interface_regs =
+		ioremap(pci_resource_start(tpci200->info->pdev,
+					   TPCI200_IP_INTERFACE_BAR),
+			TPCI200_IFACE_SIZE);
+	tpci200->info->ioidint_space =
+		ioremap(pci_resource_start(tpci200->info->pdev,
+					   TPCI200_IO_ID_INT_SPACES_BAR),
+			TPCI200_IOIDINT_SIZE);
+	tpci200->info->mem8_space =
+		ioremap(pci_resource_start(tpci200->info->pdev,
+					   TPCI200_MEM8_SPACE_BAR),
+			TPCI200_MEM8_SIZE);
+
+	spin_lock_init(&tpci200->info->access_lock);
+	ioidint_base = pci_resource_start(tpci200->info->pdev,
+					  TPCI200_IO_ID_INT_SPACES_BAR);
+	mem_base = pci_resource_start(tpci200->info->pdev,
+				      TPCI200_MEM8_SPACE_BAR);
+
+	/* Set the default parameters of the slot
+	 * INT0 disabled, level sensitive
+	 * INT1 disabled, level sensitive
+	 * error interrupt disabled
+	 * timeout interrupt disabled
+	 * recover time disabled
+	 * clock rate 8 MHz
+	 */
+	slot_ctrl = 0;
+
+	/* Set all slot physical address space */
+	for (i = 0; i < TPCI200_NB_SLOT; i++) {
+		tpci200->slots[i].io_phys.address =
+			(void __iomem *)ioidint_base +
+			TPCI200_IO_SPACE_OFF + TPCI200_IO_SPACE_GAP*i;
+		tpci200->slots[i].io_phys.size = TPCI200_IO_SPACE_SIZE;
+
+		tpci200->slots[i].id_phys.address =
+			(void __iomem *)ioidint_base +
+			TPCI200_ID_SPACE_OFF + TPCI200_ID_SPACE_GAP*i;
+		tpci200->slots[i].id_phys.size = TPCI200_ID_SPACE_SIZE;
+
+		tpci200->slots[i].mem_phys.address =
+			(void __iomem *)mem_base + TPCI200_MEM8_GAP*i;
+		tpci200->slots[i].mem_phys.size = TPCI200_MEM8_SIZE;
+
+		writew(slot_ctrl, (tpci200->info->interface_regs +
+				   control_reg[i]));
+	}
+
+	res = request_irq(tpci200->info->pdev->irq,
+			  tpci200_interrupt, IRQF_SHARED,
+			  KBUILD_MODNAME, (void *) tpci200);
+	if (res) {
+		pr_err("(bn 0x%X, sn 0x%X) unable to register IRQ !",
+		       tpci200->info->pdev->bus->number,
+		       tpci200->info->pdev->devfn);
+		tpci200_unregister(tpci200);
+		goto out_err;
+	}
+
+	return 0;
+
+out_release_ioid_int_space:
+	pci_release_region(tpci200->info->pdev, TPCI200_IO_ID_INT_SPACES_BAR);
+out_release_ip_space:
+	pci_release_region(tpci200->info->pdev, TPCI200_IP_INTERFACE_BAR);
+out_remove_sysfs:
+	tpci200_remove_sysfs_files(tpci200);
+out_disable_pci:
+	pci_disable_device(tpci200->info->pdev);
+out_err:
+	return res;
+}
+
+static int __tpci200_request_irq(struct tpci200_board *tpci200,
+				 struct ipack_device *dev)
+{
+	unsigned short slot_ctrl;
+
+	/* Set the default parameters of the slot
+	 * INT0 enabled, level sensitive
+	 * INT1 enabled, level sensitive
+	 * error interrupt disabled
+	 * timeout interrupt disabled
+	 * recover time disabled
+	 * clock rate 8 MHz
+	 */
+	slot_ctrl = TPCI200_INT0_EN | TPCI200_INT1_EN;
+	writew(slot_ctrl, (tpci200->info->interface_regs +
+			   control_reg[dev->slot]));
+
+	return 0;
+}
+
+static void __tpci200_free_irq(struct tpci200_board *tpci200,
+			       struct ipack_device *dev)
+{
+	unsigned short slot_ctrl;
+
+	/* Set the default parameters of the slot
+	 * INT0 disabled, level sensitive
+	 * INT1 disabled, level sensitive
+	 * error interrupt disabled
+	 * timeout interrupt disabled
+	 * recover time disabled
+	 * clock rate 8 MHz
+	 */
+	slot_ctrl = 0;
+	writew(slot_ctrl, (tpci200->info->interface_regs +
+			   control_reg[dev->slot]));
+}
+
+static int tpci200_free_irq(struct ipack_device *dev)
+{
+	int res;
+	struct slot_irq *slot_irq;
+	struct tpci200_board *tpci200;
+
+	tpci200 = check_slot(dev);
+	if (tpci200 == NULL) {
+		res = -EINVAL;
+		goto out;
+	}
+
+	if (mutex_lock_interruptible(&tpci200->mutex)) {
+		res = -ERESTARTSYS;
+		goto out;
+	}
+
+	if (tpci200->slots[dev->slot].irq == NULL) {
+		res = -EINVAL;
+		goto out_unlock;
+	}
+
+	__tpci200_free_irq(tpci200, dev);
+	slot_irq = tpci200->slots[dev->slot].irq;
+	tpci200->slots[dev->slot].irq = NULL;
+	kfree(slot_irq);
+
+out_unlock:
+	mutex_unlock(&tpci200->mutex);
+out:
+	return res;
+}
+
+static int tpci200_slot_unmap_space(struct ipack_device *dev, int space)
+{
+	int res;
+	struct ipack_addr_space *virt_addr_space;
+	struct tpci200_board *tpci200;
+
+	tpci200 = check_slot(dev);
+	if (tpci200 == NULL) {
+		res = -EINVAL;
+		goto out;
+	}
+
+	if (mutex_lock_interruptible(&tpci200->mutex)) {
+		res = -ERESTARTSYS;
+		goto out;
+	}
+
+	switch (space) {
+	case IPACK_IO_SPACE:
+		if (dev->io_space.address == NULL) {
+			pr_info("Slot [%d:%d] IO space not mapped !\n",
+				dev->bus_nr, dev->slot);
+			goto out_unlock;
+		}
+		virt_addr_space = &dev->io_space;
+		break;
+	case IPACK_ID_SPACE:
+		if (dev->id_space.address == NULL) {
+			pr_info("Slot [%d:%d] ID space not mapped !\n",
+				dev->bus_nr, dev->slot);
+			goto out_unlock;
+		}
+		virt_addr_space = &dev->id_space;
+		break;
+	case IPACK_MEM_SPACE:
+		if (dev->mem_space.address == NULL) {
+			pr_info("Slot [%d:%d] MEM space not mapped !\n",
+				dev->bus_nr, dev->slot);
+		goto out_unlock;
+		}
+		virt_addr_space = &dev->mem_space;
+		break;
+	default:
+		pr_err("Slot [%d:%d] space number %d doesn't exist !\n",
+		       dev->bus_nr, dev->slot, space);
+		res = -EINVAL;
+		goto out_unlock;
+		break;
+	}
+
+	iounmap(virt_addr_space->address);
+
+	virt_addr_space->address = NULL;
+	virt_addr_space->size = 0;
+out_unlock:
+	mutex_unlock(&tpci200->mutex);
+out:
+	return res;
+}
+
+static int tpci200_slot_unregister(struct ipack_device *dev)
+{
+	struct tpci200_board *tpci200;
+
+	if (dev == NULL)
+		return -ENODEV;
+
+	tpci200 = check_slot(dev);
+	if (tpci200 == NULL)
+		return -EINVAL;
+
+	tpci200_free_irq(dev);
+
+	if (mutex_lock_interruptible(&tpci200->mutex))
+		return -ERESTARTSYS;
+
+	ipack_device_unregister(dev);
+	tpci200->slots[dev->slot].dev = NULL;
+	mutex_unlock(&tpci200->mutex);
+
+	return 0;
+}
+
+static int tpci200_slot_map_space(struct ipack_device *dev,
+				  unsigned int memory_size, int space)
+{
+	int res;
+	unsigned int size_to_map;
+	void __iomem *phys_address;
+	struct ipack_addr_space *virt_addr_space;
+	struct tpci200_board *tpci200;
+
+	tpci200 = check_slot(dev);
+	if (tpci200 == NULL) {
+		res = -EINVAL;
+		goto out;
+	}
+
+	if (mutex_lock_interruptible(&tpci200->mutex)) {
+		res = -ERESTARTSYS;
+		goto out;
+	}
+
+	switch (space) {
+	case IPACK_IO_SPACE:
+		if (dev->io_space.address != NULL) {
+			pr_err("Slot [%d:%d] IO space already mapped !\n",
+			       tpci200->number, dev->slot);
+			res = -EINVAL;
+			goto out_unlock;
+		}
+		virt_addr_space = &dev->io_space;
+
+		phys_address = tpci200->slots[dev->slot].io_phys.address;
+		size_to_map = tpci200->slots[dev->slot].io_phys.size;
+		break;
+	case IPACK_ID_SPACE:
+		if (dev->id_space.address != NULL) {
+			pr_err("Slot [%d:%d] ID space already mapped !\n",
+			       tpci200->number, dev->slot);
+			res = -EINVAL;
+			goto out_unlock;
+		}
+		virt_addr_space = &dev->id_space;
+
+		phys_address = tpci200->slots[dev->slot].id_phys.address;
+		size_to_map = tpci200->slots[dev->slot].id_phys.size;
+		break;
+	case IPACK_MEM_SPACE:
+		if (dev->mem_space.address != NULL) {
+			pr_err("Slot [%d:%d] MEM space already mapped !\n",
+			       tpci200->number, dev->slot);
+			res = -EINVAL;
+			goto out_unlock;
+		}
+		virt_addr_space = &dev->mem_space;
+
+		if (memory_size > tpci200->slots[dev->slot].mem_phys.size) {
+			pr_err("Slot [%d:%d] request is 0x%X memory, only 0x%X available !\n",
+			       dev->bus_nr, dev->slot, memory_size,
+			       tpci200->slots[dev->slot].mem_phys.size);
+			res = -EINVAL;
+			goto out_unlock;
+		}
+
+		phys_address = tpci200->slots[dev->slot].mem_phys.address;
+		size_to_map = memory_size;
+		break;
+	default:
+		pr_err("Slot [%d:%d] space %d doesn't exist !\n",
+		       tpci200->number, dev->slot, space);
+		res = -EINVAL;
+		goto out_unlock;
+		break;
+	}
+
+	virt_addr_space->size = size_to_map;
+	virt_addr_space->address =
+		ioremap((unsigned long)phys_address, size_to_map);
+
+out_unlock:
+	mutex_unlock(&tpci200->mutex);
+out:
+	return res;
+}
+
+static int tpci200_request_irq(struct ipack_device *dev, int vector,
+			       int (*handler)(void *), void *arg)
+{
+	int res;
+	struct slot_irq *slot_irq;
+	struct tpci200_board *tpci200;
+
+	tpci200 = check_slot(dev);
+	if (tpci200 == NULL) {
+		res = -EINVAL;
+		goto out;
+	}
+
+	if (mutex_lock_interruptible(&tpci200->mutex)) {
+		res = -ERESTARTSYS;
+		goto out;
+	}
+
+	if (tpci200->slots[dev->slot].irq != NULL) {
+		pr_err("Slot [%d:%d] IRQ already registered !\n", dev->bus_nr,
+		       dev->slot);
+		res = -EINVAL;
+		goto out_unlock;
+	}
+
+	slot_irq = kzalloc(sizeof(struct slot_irq), GFP_KERNEL);
+	if (slot_irq == NULL) {
+		pr_err("Slot [%d:%d] unable to allocate memory for IRQ !\n",
+		       dev->bus_nr, dev->slot);
+		res = -ENOMEM;
+		goto out_unlock;
+	}
+
+	/*
+	 * WARNING: Setup Interrupt Vector in the IndustryPack device
+	 * before an IRQ request.
+	 * Read the User Manual of your IndustryPack device to know
+	 * where to write the vector in memory.
+	 */
+	slot_irq->vector = vector;
+	slot_irq->handler = handler;
+	slot_irq->arg = arg;
+	slot_irq->name = dev_name(&dev->dev);
+
+	tpci200->slots[dev->slot].irq = slot_irq;
+	res = __tpci200_request_irq(tpci200, dev);
+
+out_unlock:
+	mutex_unlock(&tpci200->mutex);
+out:
+	return res;
+}
+
+static void tpci200_slot_remove(struct tpci200_slot *slot)
+{
+	if ((slot->dev == NULL) ||
+	    (slot->dev->driver->ops->remove == NULL))
+		return;
+
+	slot->dev->driver->ops->remove(slot->dev);
+}
+
+static void tpci200_uninstall(struct tpci200_board *tpci200)
+{
+	int i;
+
+	for (i = 0; i < TPCI200_NB_SLOT; i++)
+		tpci200_slot_remove(&tpci200->slots[i]);
+
+	tpci200_unregister(tpci200);
+	kfree(tpci200->slots);
+}
+
+static struct ipack_bus_ops tpci200_bus_ops = {
+	.map_space = tpci200_slot_map_space,
+	.unmap_space = tpci200_slot_unmap_space,
+	.request_irq = tpci200_request_irq,
+	.free_irq = tpci200_free_irq,
+	.read8 = tpci200_read8,
+	.read16 = tpci200_read16,
+	.read32 = tpci200_read32,
+	.write8 = tpci200_write8,
+	.write16 = tpci200_write16,
+	.write32 = tpci200_write32,
+	.remove_device = tpci200_slot_unregister,
+};
+
+static int tpci200_install(struct tpci200_board *tpci200)
+{
+	int res;
+
+	tpci200->slots = kzalloc(
+		TPCI200_NB_SLOT * sizeof(struct tpci200_slot), GFP_KERNEL);
+	if (tpci200->slots == NULL) {
+		res = -ENOMEM;
+		goto out_err;
+	}
+
+	res = tpci200_register(tpci200);
+	if (res)
+		goto out_free;
+
+	mutex_init(&tpci200->mutex);
+	return 0;
+
+out_free:
+	kfree(tpci200->slots);
+	tpci200->slots = NULL;
+out_err:
+	return res;
+}
+
+static int tpci200_pciprobe(struct pci_dev *pdev,
+			    const struct pci_device_id *id)
+{
+	int ret;
+	struct tpci200_board *tpci200;
+
+	tpci200 = kzalloc(sizeof(struct tpci200_board), GFP_KERNEL);
+	if (!tpci200)
+		return -ENOMEM;
+
+	tpci200->info = kzalloc(sizeof(struct tpci200_infos), GFP_KERNEL);
+	if (!tpci200->info) {
+		kfree(tpci200);
+		return -ENOMEM;
+	}
+
+	/* Save struct pci_dev pointer */
+	tpci200->info->pdev = pdev;
+	tpci200->info->id_table = (struct pci_device_id *)id;
+
+	/* register the device and initialize it */
+	ret = tpci200_install(tpci200);
+	if (ret) {
+		pr_err("Error during tpci200 install !\n");
+		kfree(tpci200->info);
+		kfree(tpci200);
+		return -ENODEV;
+	}
+
+	/* Register the carrier in the industry pack bus driver */
+	tpci200->info->ipack_bus = ipack_bus_register(&pdev->dev,
+						      TPCI200_NB_SLOT,
+						      &tpci200_bus_ops);
+	if (!tpci200->info->ipack_bus) {
+		pr_err("error registering the carrier on ipack driver\n");
+		tpci200_uninstall(tpci200);
+		kfree(tpci200->info);
+		kfree(tpci200);
+		return -EFAULT;
+	}
+
+	/* save the bus number given by ipack to logging purpose */
+	tpci200->number = tpci200->info->ipack_bus->bus_nr;
+	dev_set_drvdata(&pdev->dev, tpci200);
+	/* add the registered device in an internal linked list */
+	list_add_tail(&tpci200->list, &tpci200_list);
+	return ret;
+}
+
+static void __tpci200_pci_remove(struct tpci200_board *tpci200)
+{
+	tpci200_uninstall(tpci200);
+	tpci200_remove_sysfs_files(tpci200);
+	list_del(&tpci200->list);
+	ipack_bus_unregister(tpci200->info->ipack_bus);
+	kfree(tpci200->info);
+	kfree(tpci200);
+}
+
+static void __devexit tpci200_pci_remove(struct pci_dev *dev)
+{
+	struct tpci200_board *tpci200, *next;
+
+	/* Search the registered device to uninstall it */
+	list_for_each_entry_safe(tpci200, next, &tpci200_list, list) {
+		if (tpci200->info->pdev == dev) {
+			__tpci200_pci_remove(tpci200);
+			break;
+		}
+	}
+}
+
+static struct pci_device_id tpci200_idtable[2] = {
+	{ TPCI200_VENDOR_ID, TPCI200_DEVICE_ID, TPCI200_SUBVENDOR_ID,
+	  TPCI200_SUBDEVICE_ID },
+	{ 0, },
+};
+
+static struct pci_driver tpci200_pci_drv = {
+	.name = "tpci200",
+	.id_table = tpci200_idtable,
+	.probe = tpci200_pciprobe,
+	.remove = __devexit_p(tpci200_pci_remove),
+};
+
+static int __init tpci200_drvr_init_module(void)
+{
+	return pci_register_driver(&tpci200_pci_drv);
+}
+
+static void __exit tpci200_drvr_exit_module(void)
+{
+	struct tpci200_board *tpci200, *next;
+
+	list_for_each_entry_safe(tpci200, next, &tpci200_list, list)
+		__tpci200_pci_remove(tpci200);
+
+	pci_unregister_driver(&tpci200_pci_drv);
+}
+
+MODULE_DESCRIPTION("TEWS TPCI-200 device driver");
+MODULE_LICENSE("GPL");
+module_init(tpci200_drvr_init_module);
+module_exit(tpci200_drvr_exit_module);
diff --git a/drivers/staging/ipack/bridges/tpci200.h b/drivers/staging/ipack/bridges/tpci200.h
new file mode 100644
index 00000000000..0b547eedddc
--- /dev/null
+++ b/drivers/staging/ipack/bridges/tpci200.h
@@ -0,0 +1,162 @@
+/**
+ * tpci200.h
+ *
+ * driver for the carrier TEWS TPCI-200
+ * Copyright (c) 2009 Nicolas Serafini, EIC2 SA
+ * Copyright (c) 2010,2011 Samuel Iglesias Gonsalvez <siglesia@cern.ch>, CERN
+ * Copyright (c) 2012 Samuel Iglesias Gonsalvez <siglesias@igalia.com>, Igalia
+ *
+ * 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.
+ */
+
+#ifndef _TPCI200_H_
+#define _TPCI200_H_
+
+#include <linux/version.h>
+#include <linux/limits.h>
+#include <linux/pci.h>
+#include <linux/spinlock.h>
+#include <linux/interrupt.h>
+#include <linux/swab.h>
+#include <linux/io.h>
+
+#include "../ipack.h"
+
+#define TPCI200_NB_SLOT               0x4
+#define TPCI200_NB_BAR                0x6
+
+#define TPCI200_VENDOR_ID             0x1498
+#define TPCI200_DEVICE_ID             0x30C8
+#define TPCI200_SUBVENDOR_ID          0x1498
+#define TPCI200_SUBDEVICE_ID          0x300A
+
+#define TPCI200_IP_INTERFACE_BAR      2
+#define TPCI200_IO_ID_INT_SPACES_BAR  3
+#define TPCI200_MEM16_SPACE_BAR       4
+#define TPCI200_MEM8_SPACE_BAR        5
+
+#define TPCI200_REVISION_REG          0x00
+#define TPCI200_CONTROL_A_REG         0x02
+#define TPCI200_CONTROL_B_REG         0x04
+#define TPCI200_CONTROL_C_REG         0x06
+#define TPCI200_CONTROL_D_REG         0x08
+#define TPCI200_RESET_REG             0x0A
+#define TPCI200_STATUS_REG            0x0C
+
+#define TPCI200_IFACE_SIZE            0x100
+
+#define TPCI200_IO_SPACE_OFF          0x0000
+#define TPCI200_IO_SPACE_GAP          0x0100
+#define TPCI200_IO_SPACE_SIZE         0x0080
+#define TPCI200_ID_SPACE_OFF          0x0080
+#define TPCI200_ID_SPACE_GAP          0x0100
+#define TPCI200_ID_SPACE_SIZE         0x0040
+#define TPCI200_INT_SPACE_OFF         0x00C0
+#define TPCI200_INT_SPACE_GAP         0x0100
+#define TPCI200_INT_SPACE_SIZE        0x0040
+#define TPCI200_IOIDINT_SIZE          0x0400
+
+#define TPCI200_MEM8_GAP              0x00400000
+#define TPCI200_MEM8_SIZE             0x00400000
+#define TPCI200_MEM16_GAP             0x00800000
+#define TPCI200_MEM16_SIZE            0x00800000
+
+#define TPCI200_INT0_EN               0x0040
+#define TPCI200_INT1_EN               0x0080
+#define TPCI200_INT0_EDGE             0x0010
+#define TPCI200_INT1_EDGE             0x0020
+#define TPCI200_ERR_INT_EN            0x0008
+#define TPCI200_TIME_INT_EN           0x0004
+#define TPCI200_RECOVER_EN            0x0002
+#define TPCI200_CLK32                 0x0001
+
+#define TPCI200_A_RESET               0x0001
+#define TPCI200_B_RESET               0x0002
+#define TPCI200_C_RESET               0x0004
+#define TPCI200_D_RESET               0x0008
+
+#define TPCI200_A_TIMEOUT             0x1000
+#define TPCI200_B_TIMEOUT             0x2000
+#define TPCI200_C_TIMEOUT             0x4000
+#define TPCI200_D_TIMEOUT             0x8000
+
+#define TPCI200_A_ERROR               0x0100
+#define TPCI200_B_ERROR               0x0200
+#define TPCI200_C_ERROR               0x0400
+#define TPCI200_D_ERROR               0x0800
+
+#define TPCI200_A_INT0                0x0001
+#define TPCI200_A_INT1                0x0002
+#define TPCI200_B_INT0                0x0004
+#define TPCI200_B_INT1                0x0008
+#define TPCI200_C_INT0                0x0010
+#define TPCI200_C_INT1                0x0020
+#define TPCI200_D_INT0                0x0040
+#define TPCI200_D_INT1                0x0080
+
+#define TPCI200_SLOT_INT_MASK         0x00FF
+
+#define VME_IOID_SPACE  "IOID"
+#define VME_MEM_SPACE  "MEM"
+
+/**
+ * struct slot_irq - slot IRQ definition.
+ * @vector	Vector number
+ * @handler	Handler called when IRQ arrives
+ * @arg		Handler argument
+ * @name	IRQ name
+ *
+ */
+struct slot_irq {
+	int		vector;
+	int		(*handler)(void *);
+	void		*arg;
+	const char	*name;
+};
+
+/**
+ * struct tpci200_slot - data specific to the tpci200 slot.
+ * @slot_id	Slot identification gived to external interface
+ * @irq		Slot IRQ infos
+ * @io_phys	IO physical base address register of the slot
+ * @id_phys	ID physical base address register of the slot
+ * @mem_phys	MEM physical base address register of the slot
+ *
+ */
+struct tpci200_slot {
+	struct ipack_device	*dev;
+	struct slot_irq		*irq;
+	struct ipack_addr_space io_phys;
+	struct ipack_addr_space id_phys;
+	struct ipack_addr_space mem_phys;
+};
+
+/**
+ * struct tpci200_infos - informations specific of the TPCI200 tpci200.
+ * @pci_dev		PCI device
+ * @interface_regs	Pointer to IP interface space (Bar 2)
+ * @ioidint_space	Pointer to IP ID, IO and INT space (Bar 3)
+ * @mem8_space		Pointer to MEM space (Bar 4)
+ * @access_lock		Mutex lock for simultaneous access
+ *
+ */
+struct tpci200_infos {
+	struct pci_dev			*pdev;
+	struct pci_device_id		*id_table;
+	void __iomem			*interface_regs;
+	void __iomem			*ioidint_space;
+	void __iomem			*mem8_space;
+	spinlock_t			access_lock;
+	struct ipack_bus_device		*ipack_bus;
+};
+struct tpci200_board {
+	struct list_head	list;
+	unsigned int		number;
+	struct mutex		mutex;
+	struct tpci200_slot	*slots;
+	struct tpci200_infos	*info;
+};
+
+#endif /* _TPCI200_H_ */
diff --git a/drivers/staging/ipack/devices/Kconfig b/drivers/staging/ipack/devices/Kconfig
new file mode 100644
index 00000000000..39f71888a58
--- /dev/null
+++ b/drivers/staging/ipack/devices/Kconfig
@@ -0,0 +1,7 @@
+config SERIAL_IPOCTAL
+	tristate "IndustryPack IP-OCTAL uart support"
+	depends on IPACK_BUS
+	help
+	  This driver supports the IPOCTAL serial port device for the IndustryPack bus.
+	default n
+
diff --git a/drivers/staging/ipack/devices/Makefile b/drivers/staging/ipack/devices/Makefile
new file mode 100644
index 00000000000..6de18bda4a9
--- /dev/null
+++ b/drivers/staging/ipack/devices/Makefile
@@ -0,0 +1 @@
+obj-$(CONFIG_SERIAL_IPOCTAL) += ipoctal.o
diff --git a/drivers/staging/ipack/devices/ipoctal.c b/drivers/staging/ipack/devices/ipoctal.c
new file mode 100644
index 00000000000..29f6fa841d2
--- /dev/null
+++ b/drivers/staging/ipack/devices/ipoctal.c
@@ -0,0 +1,901 @@
+/**
+ * ipoctal.c
+ *
+ * driver for the GE IP-OCTAL boards
+ * Copyright (c) 2009 Nicolas Serafini, EIC2 SA
+ * Copyright (c) 2010,2011 Samuel Iglesias Gonsalvez <siglesia@cern.ch>, CERN
+ * Copyright (c) 2012 Samuel Iglesias Gonsalvez <siglesias@igalia.com>, Igalia
+ *
+ * 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.
+ */
+
+#define pr_fmt(fmt) KBUILD_MODNAME ": " fmt
+
+#include <linux/device.h>
+#include <linux/module.h>
+#include <linux/moduleparam.h>
+#include <linux/interrupt.h>
+#include <linux/fs.h>
+#include <linux/delay.h>
+#include <linux/sched.h>
+#include <linux/tty.h>
+#include <linux/serial.h>
+#include <linux/tty_flip.h>
+#include <linux/slab.h>
+#include <linux/uaccess.h>
+#include <linux/atomic.h>
+#include "../ipack.h"
+#include "ipoctal.h"
+#include "scc2698.h"
+
+#define IP_OCTAL_MANUFACTURER_ID    0xF0
+#define IP_OCTAL_232_ID             0x22
+#define IP_OCTAL_422_ID             0x2A
+#define IP_OCTAL_485_ID             0x48
+
+#define IP_OCTAL_ID_SPACE_VECTOR    0x41
+#define IP_OCTAL_NB_BLOCKS          4
+
+static struct ipack_driver driver;
+static const struct tty_operations ipoctal_fops;
+
+struct ipoctal {
+	struct list_head		list;
+	struct ipack_device		*dev;
+	unsigned int			board_id;
+	struct scc2698_channel		*chan_regs;
+	struct scc2698_block		*block_regs;
+	struct ipoctal_stats		chan_stats[NR_CHANNELS];
+	char				*buffer[NR_CHANNELS];
+	unsigned int			nb_bytes[NR_CHANNELS];
+	unsigned int			count_wr[NR_CHANNELS];
+	struct ipoctal_config		chan_config[NR_CHANNELS];
+	wait_queue_head_t		queue[NR_CHANNELS];
+	unsigned short			error_flag[NR_CHANNELS];
+	spinlock_t			lock[NR_CHANNELS];
+	unsigned int			pointer_read[NR_CHANNELS];
+	unsigned int			pointer_write[NR_CHANNELS];
+	atomic_t			open[NR_CHANNELS];
+	unsigned char			write;
+	struct tty_port			tty_port[NR_CHANNELS];
+	struct tty_driver		*tty_drv;
+};
+
+/* Linked list to save the registered devices */
+static LIST_HEAD(ipoctal_list);
+
+static inline void ipoctal_write_io_reg(struct ipoctal *ipoctal,
+					unsigned char *dest,
+					unsigned char value)
+{
+	unsigned long offset;
+
+	offset = ((void __iomem *) dest) - ipoctal->dev->io_space.address;
+	ipoctal->dev->bus->ops->write8(ipoctal->dev, IPACK_IO_SPACE, offset,
+				       value);
+}
+
+static inline void ipoctal_write_cr_cmd(struct ipoctal *ipoctal,
+					unsigned char *dest,
+					unsigned char value)
+{
+	ipoctal_write_io_reg(ipoctal, dest, value);
+}
+
+static inline unsigned char ipoctal_read_io_reg(struct ipoctal *ipoctal,
+						unsigned char *src)
+{
+	unsigned long offset;
+	unsigned char value;
+
+	offset = ((void __iomem *) src) - ipoctal->dev->io_space.address;
+	ipoctal->dev->bus->ops->read8(ipoctal->dev, IPACK_IO_SPACE, offset,
+				      &value);
+	return value;
+}
+
+static struct ipoctal *ipoctal_find_board(struct tty_struct *tty)
+{
+	struct ipoctal *p;
+
+	list_for_each_entry(p, &ipoctal_list, list) {
+		if (tty->driver->major == p->tty_drv->major)
+			return p;
+	}
+
+	return NULL;
+}
+
+static int ipoctal_port_activate(struct tty_port *port, struct tty_struct *tty)
+{
+	struct ipoctal *ipoctal;
+	int channel = tty->index;
+
+	ipoctal = ipoctal_find_board(tty);
+
+	if (ipoctal == NULL) {
+		pr_err("Device not found. Major %d\n", tty->driver->major);
+		return -ENODEV;
+	}
+
+	ipoctal_write_io_reg(ipoctal, &ipoctal->chan_regs[channel].u.w.cr,
+			     CR_ENABLE_RX);
+	tty->driver_data = ipoctal;
+
+	return 0;
+}
+
+static int ipoctal_open(struct tty_struct *tty, struct file *file)
+{
+	int channel = tty->index;
+	int res;
+	struct ipoctal *ipoctal;
+
+	ipoctal = ipoctal_find_board(tty);
+
+	if (ipoctal == NULL) {
+		pr_err("Device not found. Major %d\n", tty->driver->major);
+		return -ENODEV;
+	}
+
+	if (atomic_read(&ipoctal->open[channel]))
+		return -EBUSY;
+
+	res = tty_port_open(&ipoctal->tty_port[channel], tty, file);
+	if (res)
+		return res;
+
+	atomic_inc(&ipoctal->open[channel]);
+	return 0;
+}
+
+static void ipoctal_reset_stats(struct ipoctal_stats *stats)
+{
+	stats->tx = 0;
+	stats->rx = 0;
+	stats->rcv_break = 0;
+	stats->framing_err = 0;
+	stats->overrun_err = 0;
+	stats->parity_err = 0;
+}
+
+static void ipoctal_free_channel(struct tty_struct *tty)
+{
+	int channel = tty->index;
+	struct ipoctal *ipoctal = tty->driver_data;
+
+	if (ipoctal == NULL)
+		return;
+
+	ipoctal_reset_stats(&ipoctal->chan_stats[channel]);
+	ipoctal->pointer_read[channel] = 0;
+	ipoctal->pointer_write[channel] = 0;
+	ipoctal->nb_bytes[channel] = 0;
+}
+
+static void ipoctal_close(struct tty_struct *tty, struct file *filp)
+{
+	int channel = tty->index;
+	struct ipoctal *ipoctal = tty->driver_data;
+
+	tty_port_close(&ipoctal->tty_port[channel], tty, filp);
+
+	if (atomic_dec_and_test(&ipoctal->open[channel]))
+		ipoctal_free_channel(tty);
+}
+
+static int ipoctal_get_icount(struct tty_struct *tty,
+			      struct serial_icounter_struct *icount)
+{
+	struct ipoctal *ipoctal = tty->driver_data;
+	int channel = tty->index;
+
+	icount->cts = 0;
+	icount->dsr = 0;
+	icount->rng = 0;
+	icount->dcd = 0;
+	icount->rx = ipoctal->chan_stats[channel].rx;
+	icount->tx = ipoctal->chan_stats[channel].tx;
+	icount->frame = ipoctal->chan_stats[channel].framing_err;
+	icount->parity = ipoctal->chan_stats[channel].parity_err;
+	icount->brk = ipoctal->chan_stats[channel].rcv_break;
+	return 0;
+}
+
+static int ipoctal_irq_handler(void *arg)
+{
+	unsigned int channel;
+	unsigned int block;
+	unsigned char isr;
+	unsigned char sr;
+	unsigned char isr_tx_rdy, isr_rx_rdy;
+	unsigned char value;
+	unsigned char flag;
+	struct tty_struct *tty;
+	struct ipoctal *ipoctal = (struct ipoctal *) arg;
+
+	/* Check all channels */
+	for (channel = 0; channel < NR_CHANNELS; channel++) {
+		/* If there is no client, skip the check */
+		if (!atomic_read(&ipoctal->open[channel]))
+			continue;
+
+		tty = tty_port_tty_get(&ipoctal->tty_port[channel]);
+		if (!tty)
+			continue;
+
+		/*
+		 * The HW is organized in pair of channels.
+		 * See which register we need to read from
+		 */
+		block = channel / 2;
+		isr = ipoctal_read_io_reg(ipoctal,
+					  &ipoctal->block_regs[block].u.r.isr);
+		sr = ipoctal_read_io_reg(ipoctal,
+					 &ipoctal->chan_regs[channel].u.r.sr);
+
+		if ((channel % 2) == 1) {
+			isr_tx_rdy = isr & ISR_TxRDY_B;
+			isr_rx_rdy = isr & ISR_RxRDY_FFULL_B;
+		} else {
+			isr_tx_rdy = isr & ISR_TxRDY_A;
+			isr_rx_rdy = isr & ISR_RxRDY_FFULL_A;
+		}
+
+		/* In case of RS-485, change from TX to RX when finishing TX.
+		 * Half-duplex.
+		 */
+		if ((ipoctal->board_id == IP_OCTAL_485_ID) &&
+		    (sr & SR_TX_EMPTY) &&
+		    (ipoctal->nb_bytes[channel] == 0)) {
+			ipoctal_write_io_reg(ipoctal,
+					     &ipoctal->chan_regs[channel].u.w.cr,
+					     CR_DISABLE_TX);
+			ipoctal_write_cr_cmd(ipoctal,
+					     &ipoctal->chan_regs[channel].u.w.cr,
+					     CR_CMD_NEGATE_RTSN);
+			ipoctal_write_io_reg(ipoctal,
+					     &ipoctal->chan_regs[channel].u.w.cr,
+					     CR_ENABLE_RX);
+			ipoctal->write = 1;
+			wake_up_interruptible(&ipoctal->queue[channel]);
+		}
+
+		/* RX data */
+		if (isr_rx_rdy && (sr & SR_RX_READY)) {
+			value = ipoctal_read_io_reg(ipoctal,
+						    &ipoctal->chan_regs[channel].u.r.rhr);
+			flag = TTY_NORMAL;
+
+			/* Error: count statistics */
+			if (sr & SR_ERROR) {
+				ipoctal_write_cr_cmd(ipoctal,
+						     &ipoctal->chan_regs[channel].u.w.cr,
+						     CR_CMD_RESET_ERR_STATUS);
+
+				if (sr & SR_OVERRUN_ERROR) {
+					ipoctal->error_flag[channel] |= UART_OVERRUN;
+					ipoctal->chan_stats[channel].overrun_err++;
+					/* Overrun doesn't affect the current character*/
+					tty_insert_flip_char(tty, 0, TTY_OVERRUN);
+				}
+				if (sr & SR_PARITY_ERROR) {
+					ipoctal->error_flag[channel] |= UART_PARITY;
+					ipoctal->chan_stats[channel].parity_err++;
+					flag = TTY_PARITY;
+				}
+				if (sr & SR_FRAMING_ERROR) {
+					ipoctal->error_flag[channel] |= UART_FRAMING;
+					ipoctal->chan_stats[channel].framing_err++;
+					flag = TTY_FRAME;
+				}
+				if (sr & SR_RECEIVED_BREAK) {
+					ipoctal->error_flag[channel] |= UART_BREAK;
+					ipoctal->chan_stats[channel].rcv_break++;
+					flag = TTY_BREAK;
+				}
+			}
+
+			tty_insert_flip_char(tty, value, flag);
+		}
+
+		/* TX of each character */
+		if (isr_tx_rdy && (sr & SR_TX_READY)) {
+			unsigned int *pointer_write =
+				&ipoctal->pointer_write[channel];
+
+			if (ipoctal->nb_bytes[channel] <= 0) {
+				ipoctal->nb_bytes[channel] = 0;
+				continue;
+			}
+			spin_lock(&ipoctal->lock[channel]);
+			value = ipoctal->buffer[channel][*pointer_write];
+			ipoctal_write_io_reg(ipoctal,
+					     &ipoctal->chan_regs[channel].u.w.thr,
+					     value);
+			ipoctal->chan_stats[channel].tx++;
+			ipoctal->count_wr[channel]++;
+			(*pointer_write)++;
+			*pointer_write = *pointer_write % PAGE_SIZE;
+			ipoctal->nb_bytes[channel]--;
+			spin_unlock(&ipoctal->lock[channel]);
+
+			if ((ipoctal->nb_bytes[channel] == 0) &&
+			    (waitqueue_active(&ipoctal->queue[channel]))) {
+
+				if (ipoctal->board_id != IP_OCTAL_485_ID) {
+					ipoctal->write = 1;
+					wake_up_interruptible(&ipoctal->queue[channel]);
+				}
+			}
+		}
+
+		tty_flip_buffer_push(tty);
+		tty_kref_put(tty);
+	}
+	return IRQ_HANDLED;
+}
+
+static int ipoctal_check_model(struct ipack_device *dev, unsigned char *id)
+{
+	unsigned char manufacturerID;
+	unsigned char board_id;
+
+	dev->bus->ops->read8(dev, IPACK_ID_SPACE,
+			IPACK_IDPROM_OFFSET_MANUFACTURER_ID, &manufacturerID);
+	if (manufacturerID != IP_OCTAL_MANUFACTURER_ID)
+		return -ENODEV;
+
+	dev->bus->ops->read8(dev, IPACK_ID_SPACE,
+			IPACK_IDPROM_OFFSET_MODEL, (unsigned char *)&board_id);
+
+	switch (board_id) {
+	case IP_OCTAL_232_ID:
+	case IP_OCTAL_422_ID:
+	case IP_OCTAL_485_ID:
+		*id = board_id;
+		break;
+	default:
+		return -ENODEV;
+	}
+
+	return 0;
+}
+
+static const struct tty_port_operations ipoctal_tty_port_ops = {
+	.dtr_rts = NULL,
+	.activate = ipoctal_port_activate,
+};
+
+static int ipoctal_inst_slot(struct ipoctal *ipoctal, unsigned int bus_nr,
+			     unsigned int slot, unsigned int vector)
+{
+	int res = 0;
+	int i;
+	struct tty_driver *tty;
+	char name[20];
+	unsigned char board_id;
+
+	res = ipoctal->dev->bus->ops->map_space(ipoctal->dev, 0,
+						IPACK_ID_SPACE);
+	if (res) {
+		pr_err("Unable to map slot [%d:%d] ID space!\n", bus_nr, slot);
+		return res;
+	}
+
+	res = ipoctal_check_model(ipoctal->dev, &board_id);
+	if (res) {
+		ipoctal->dev->bus->ops->unmap_space(ipoctal->dev,
+						    IPACK_ID_SPACE);
+		goto out_unregister_id_space;
+	}
+	ipoctal->board_id = board_id;
+
+	res = ipoctal->dev->bus->ops->map_space(ipoctal->dev, 0,
+						IPACK_IO_SPACE);
+	if (res) {
+		pr_err("Unable to map slot [%d:%d] IO space!\n", bus_nr, slot);
+		goto out_unregister_id_space;
+	}
+
+	res = ipoctal->dev->bus->ops->map_space(ipoctal->dev,
+					   0x8000, IPACK_MEM_SPACE);
+	if (res) {
+		pr_err("Unable to map slot [%d:%d] MEM space!\n", bus_nr, slot);
+		goto out_unregister_io_space;
+	}
+
+	/* Save the virtual address to access the registers easily */
+	ipoctal->chan_regs =
+		(struct scc2698_channel *) ipoctal->dev->io_space.address;
+	ipoctal->block_regs =
+		(struct scc2698_block *) ipoctal->dev->io_space.address;
+
+	/* Disable RX and TX before touching anything */
+	for (i = 0; i < NR_CHANNELS ; i++) {
+		ipoctal_write_io_reg(ipoctal, &ipoctal->chan_regs[i].u.w.cr,
+				     CR_DISABLE_RX | CR_DISABLE_TX);
+	}
+
+	for (i = 0; i < IP_OCTAL_NB_BLOCKS; i++) {
+		ipoctal_write_io_reg(ipoctal,
+				     &ipoctal->block_regs[i].u.w.acr,
+				     ACR_BRG_SET2);
+		ipoctal_write_io_reg(ipoctal,
+				     &ipoctal->block_regs[i].u.w.opcr,
+				     OPCR_MPP_OUTPUT | OPCR_MPOa_RTSN |
+				     OPCR_MPOb_RTSN);
+		ipoctal_write_io_reg(ipoctal,
+				     &ipoctal->block_regs[i].u.w.imr,
+				     IMR_TxRDY_A | IMR_RxRDY_FFULL_A |
+				     IMR_DELTA_BREAK_A | IMR_TxRDY_B |
+				     IMR_RxRDY_FFULL_B | IMR_DELTA_BREAK_B);
+	}
+
+	/*
+	 * IP-OCTAL has different addresses to copy its IRQ vector.
+	 * Depending of the carrier these addresses are accesible or not.
+	 * More info in the datasheet.
+	 */
+	ipoctal->dev->bus->ops->request_irq(ipoctal->dev, vector,
+				       ipoctal_irq_handler, ipoctal);
+	ipoctal->dev->bus->ops->write8(ipoctal->dev, IPACK_ID_SPACE, 0, vector);
+
+	/* Register the TTY device */
+
+	/* Each IP-OCTAL channel is a TTY port */
+	tty = alloc_tty_driver(NR_CHANNELS);
+
+	if (!tty) {
+		res = -ENOMEM;
+		goto out_unregister_slot_unmap;
+	}
+
+	/* Fill struct tty_driver with ipoctal data */
+	tty->owner = THIS_MODULE;
+	tty->driver_name = "ipoctal";
+	sprintf(name, "ipoctal.%d.%d.", bus_nr, slot);
+	tty->name = name;
+	tty->major = 0;
+
+	tty->minor_start = 0;
+	tty->type = TTY_DRIVER_TYPE_SERIAL;
+	tty->subtype = SERIAL_TYPE_NORMAL;
+	tty->flags = TTY_DRIVER_REAL_RAW | TTY_DRIVER_DYNAMIC_DEV;
+	tty->init_termios = tty_std_termios;
+	tty->init_termios.c_cflag = B9600 | CS8 | CREAD | HUPCL | CLOCAL;
+	tty->init_termios.c_ispeed = 9600;
+	tty->init_termios.c_ospeed = 9600;
+
+	tty_set_operations(tty, &ipoctal_fops);
+	res = tty_register_driver(tty);
+	if (res) {
+		pr_err("Can't register tty driver.\n");
+		put_tty_driver(tty);
+		goto out_unregister_slot_unmap;
+	}
+
+	/* Save struct tty_driver for use it when uninstalling the device */
+	ipoctal->tty_drv = tty;
+
+	for (i = 0; i < NR_CHANNELS; i++) {
+		tty_port_init(&ipoctal->tty_port[i]);
+		tty_port_alloc_xmit_buf(&ipoctal->tty_port[i]);
+		ipoctal->tty_port[i].ops = &ipoctal_tty_port_ops;
+
+		ipoctal_reset_stats(&ipoctal->chan_stats[i]);
+		ipoctal->nb_bytes[i] = 0;
+		init_waitqueue_head(&ipoctal->queue[i]);
+		ipoctal->error_flag[i] = UART_NOERROR;
+
+		spin_lock_init(&ipoctal->lock[i]);
+		ipoctal->pointer_read[i] = 0;
+		ipoctal->pointer_write[i] = 0;
+		ipoctal->nb_bytes[i] = 0;
+		tty_register_device(tty, i, NULL);
+
+		/*
+		 * Enable again the RX. TX will be enabled when
+		 * there is something to send
+		 */
+		ipoctal_write_io_reg(ipoctal, &ipoctal->chan_regs[i].u.w.cr,
+				     CR_ENABLE_RX);
+	}
+
+	return 0;
+
+out_unregister_slot_unmap:
+	ipoctal->dev->bus->ops->unmap_space(ipoctal->dev, IPACK_ID_SPACE);
+out_unregister_io_space:
+	ipoctal->dev->bus->ops->unmap_space(ipoctal->dev, IPACK_IO_SPACE);
+out_unregister_id_space:
+	ipoctal->dev->bus->ops->unmap_space(ipoctal->dev, IPACK_MEM_SPACE);
+	return res;
+}
+
+static inline int ipoctal_copy_write_buffer(struct ipoctal *ipoctal,
+					    unsigned int channel,
+					    const unsigned char *buf,
+					    int count)
+{
+	unsigned long flags;
+	int i;
+	unsigned int *pointer_read = &ipoctal->pointer_read[channel];
+
+	/* Copy the bytes from the user buffer to the internal one */
+	for (i = 0; i < count; i++) {
+		if (i <= (PAGE_SIZE - ipoctal->nb_bytes[channel])) {
+			spin_lock_irqsave(&ipoctal->lock[channel], flags);
+			ipoctal->tty_port[channel].xmit_buf[*pointer_read] = buf[i];
+			*pointer_read = (*pointer_read + 1) % PAGE_SIZE;
+			ipoctal->nb_bytes[channel]++;
+			spin_unlock_irqrestore(&ipoctal->lock[channel], flags);
+		} else {
+			break;
+		}
+	}
+	return i;
+}
+
+static int ipoctal_write(struct ipoctal *ipoctal, unsigned int channel,
+			 const unsigned char *buf, int count)
+{
+	ipoctal->nb_bytes[channel] = 0;
+	ipoctal->count_wr[channel] = 0;
+
+	ipoctal_copy_write_buffer(ipoctal, channel, buf, count);
+
+	ipoctal->error_flag[channel] = UART_NOERROR;
+
+	/* As the IP-OCTAL 485 only supports half duplex, do it manually */
+	if (ipoctal->board_id == IP_OCTAL_485_ID) {
+		ipoctal_write_io_reg(ipoctal,
+				     &ipoctal->chan_regs[channel].u.w.cr,
+				     CR_DISABLE_RX);
+		ipoctal_write_cr_cmd(ipoctal,
+				     &ipoctal->chan_regs[channel].u.w.cr,
+				     CR_CMD_ASSERT_RTSN);
+	}
+
+	/*
+	 * Send a packet and then disable TX to avoid failure after several send
+	 * operations
+	 */
+	ipoctal_write_io_reg(ipoctal,
+			     &ipoctal->chan_regs[channel].u.w.cr,
+			     CR_ENABLE_TX);
+	wait_event_interruptible(ipoctal->queue[channel], ipoctal->write);
+	ipoctal_write_io_reg(ipoctal,
+			     &ipoctal->chan_regs[channel].u.w.cr,
+			     CR_DISABLE_TX);
+
+	ipoctal->write = 0;
+	return ipoctal->count_wr[channel];
+}
+
+static int ipoctal_write_tty(struct tty_struct *tty,
+			     const unsigned char *buf, int count)
+{
+	unsigned int channel = tty->index;
+	struct ipoctal *ipoctal = tty->driver_data;
+
+	return ipoctal_write(ipoctal, channel, buf, count);
+}
+
+static int ipoctal_write_room(struct tty_struct *tty)
+{
+	int channel = tty->index;
+	struct ipoctal *ipoctal = tty->driver_data;
+
+	return PAGE_SIZE - ipoctal->nb_bytes[channel];
+}
+
+static int ipoctal_chars_in_buffer(struct tty_struct *tty)
+{
+	int channel = tty->index;
+	struct ipoctal *ipoctal = tty->driver_data;
+
+	return ipoctal->nb_bytes[channel];
+}
+
+static void ipoctal_set_termios(struct tty_struct *tty,
+				struct ktermios *old_termios)
+{
+	unsigned int cflag;
+	unsigned char mr1 = 0;
+	unsigned char mr2 = 0;
+	unsigned char csr = 0;
+	unsigned int channel = tty->index;
+	struct ipoctal *ipoctal = tty->driver_data;
+	speed_t baud;
+
+	cflag = tty->termios->c_cflag;
+
+	/* Disable and reset everything before change the setup */
+	ipoctal_write_io_reg(ipoctal, &ipoctal->chan_regs[channel].u.w.cr,
+			     CR_DISABLE_RX | CR_DISABLE_TX);
+	ipoctal_write_cr_cmd(ipoctal, &ipoctal->chan_regs[channel].u.w.cr,
+			     CR_CMD_RESET_RX);
+	ipoctal_write_cr_cmd(ipoctal, &ipoctal->chan_regs[channel].u.w.cr,
+			     CR_CMD_RESET_TX);
+	ipoctal_write_cr_cmd(ipoctal, &ipoctal->chan_regs[channel].u.w.cr,
+			     CR_CMD_RESET_ERR_STATUS);
+	ipoctal_write_cr_cmd(ipoctal, &ipoctal->chan_regs[channel].u.w.cr,
+			     CR_CMD_RESET_MR);
+
+	/* Set Bits per chars */
+	switch (cflag & CSIZE) {
+	case CS6:
+		mr1 |= MR1_CHRL_6_BITS;
+		break;
+	case CS7:
+		mr1 |= MR1_CHRL_7_BITS;
+		break;
+	case CS8:
+	default:
+		mr1 |= MR1_CHRL_8_BITS;
+		/* By default, select CS8 */
+		tty->termios->c_cflag = (cflag & ~CSIZE) | CS8;
+		break;
+	}
+
+	/* Set Parity */
+	if (cflag & PARENB)
+		if (cflag & PARODD)
+			mr1 |= MR1_PARITY_ON | MR1_PARITY_ODD;
+		else
+			mr1 |= MR1_PARITY_ON | MR1_PARITY_EVEN;
+	else
+		mr1 |= MR1_PARITY_OFF;
+
+	/* Mark or space parity is not supported */
+	tty->termios->c_cflag &= ~CMSPAR;
+
+	/* Set stop bits */
+	if (cflag & CSTOPB)
+		mr2 |= MR2_STOP_BITS_LENGTH_2;
+	else
+		mr2 |= MR2_STOP_BITS_LENGTH_1;
+
+	/* Set the flow control */
+	switch (ipoctal->board_id) {
+	case IP_OCTAL_232_ID:
+		if (cflag & CRTSCTS) {
+			mr1 |= MR1_RxRTS_CONTROL_ON;
+			mr2 |= MR2_TxRTS_CONTROL_OFF | MR2_CTS_ENABLE_TX_ON;
+			ipoctal->chan_config[channel].flow_control = 1;
+		} else {
+			mr1 |= MR1_RxRTS_CONTROL_OFF;
+			mr2 |= MR2_TxRTS_CONTROL_OFF | MR2_CTS_ENABLE_TX_OFF;
+			ipoctal->chan_config[channel].flow_control = 0;
+		}
+		break;
+	case IP_OCTAL_422_ID:
+		mr1 |= MR1_RxRTS_CONTROL_OFF;
+		mr2 |= MR2_TxRTS_CONTROL_OFF | MR2_CTS_ENABLE_TX_OFF;
+		ipoctal->chan_config[channel].flow_control = 0;
+		break;
+	case IP_OCTAL_485_ID:
+		mr1 |= MR1_RxRTS_CONTROL_OFF;
+		mr2 |= MR2_TxRTS_CONTROL_ON | MR2_CTS_ENABLE_TX_OFF;
+		ipoctal->chan_config[channel].flow_control = 0;
+		break;
+	default:
+		return;
+		break;
+	}
+
+	baud = tty_get_baud_rate(tty);
+	tty_termios_encode_baud_rate(tty->termios, baud, baud);
+
+	/* Set baud rate */
+	switch (tty->termios->c_ospeed) {
+	case 75:
+		csr |= TX_CLK_75 | RX_CLK_75;
+		break;
+	case 110:
+		csr |= TX_CLK_110 | RX_CLK_110;
+		break;
+	case 150:
+		csr |= TX_CLK_150 | RX_CLK_150;
+		break;
+	case 300:
+		csr |= TX_CLK_300 | RX_CLK_300;
+		break;
+	case 600:
+		csr |= TX_CLK_600 | RX_CLK_600;
+		break;
+	case 1200:
+		csr |= TX_CLK_1200 | RX_CLK_1200;
+		break;
+	case 1800:
+		csr |= TX_CLK_1800 | RX_CLK_1800;
+		break;
+	case 2000:
+		csr |= TX_CLK_2000 | RX_CLK_2000;
+		break;
+	case 2400:
+		csr |= TX_CLK_2400 | RX_CLK_2400;
+		break;
+	case 4800:
+		csr |= TX_CLK_4800  | RX_CLK_4800;
+		break;
+	case 9600:
+		csr |= TX_CLK_9600  | RX_CLK_9600;
+		break;
+	case 19200:
+		csr |= TX_CLK_19200 | RX_CLK_19200;
+		break;
+	case 38400:
+	default:
+		csr |= TX_CLK_38400 | RX_CLK_38400;
+		/* In case of default, we establish 38400 bps */
+		tty_termios_encode_baud_rate(tty->termios, 38400, 38400);
+		break;
+	}
+
+	mr1 |= MR1_ERROR_CHAR;
+	mr1 |= MR1_RxINT_RxRDY;
+
+	/* Write the control registers */
+	ipoctal_write_io_reg(ipoctal, &ipoctal->chan_regs[channel].u.w.mr, mr1);
+	ipoctal_write_io_reg(ipoctal, &ipoctal->chan_regs[channel].u.w.mr, mr2);
+	ipoctal_write_io_reg(ipoctal, &ipoctal->chan_regs[channel].u.w.csr, csr);
+
+	/* save the setup in the structure */
+	ipoctal->chan_config[channel].baud = tty_get_baud_rate(tty);
+	ipoctal->chan_config[channel].bits_per_char = cflag & CSIZE;
+	ipoctal->chan_config[channel].parity = cflag & PARENB;
+	ipoctal->chan_config[channel].stop_bits = cflag & CSTOPB;
+
+	/* Enable again the RX */
+	ipoctal_write_io_reg(ipoctal, &ipoctal->chan_regs[channel].u.w.cr,
+			     CR_ENABLE_RX);
+}
+
+static void ipoctal_hangup(struct tty_struct *tty)
+{
+	unsigned long flags;
+	int channel = tty->index;
+	struct ipoctal *ipoctal = tty->driver_data;
+
+	if (ipoctal == NULL)
+		return;
+
+	spin_lock_irqsave(&ipoctal->lock[channel], flags);
+	ipoctal->nb_bytes[channel] = 0;
+	ipoctal->pointer_read[channel] = 0;
+	ipoctal->pointer_write[channel] = 0;
+	spin_unlock_irqrestore(&ipoctal->lock[channel], flags);
+
+	tty_port_hangup(&ipoctal->tty_port[channel]);
+
+	ipoctal_write_io_reg(ipoctal, &ipoctal->chan_regs[channel].u.w.cr,
+			     CR_DISABLE_RX | CR_DISABLE_TX);
+	ipoctal_write_cr_cmd(ipoctal, &ipoctal->chan_regs[channel].u.w.cr,
+			     CR_CMD_RESET_RX);
+	ipoctal_write_cr_cmd(ipoctal, &ipoctal->chan_regs[channel].u.w.cr,
+			     CR_CMD_RESET_TX);
+	ipoctal_write_cr_cmd(ipoctal, &ipoctal->chan_regs[channel].u.w.cr,
+			     CR_CMD_RESET_ERR_STATUS);
+	ipoctal_write_cr_cmd(ipoctal, &ipoctal->chan_regs[channel].u.w.cr,
+			     CR_CMD_RESET_MR);
+
+	clear_bit(ASYNCB_INITIALIZED, &ipoctal->tty_port[channel].flags);
+	wake_up_interruptible(&ipoctal->tty_port[channel].open_wait);
+}
+
+static const struct tty_operations ipoctal_fops = {
+	.ioctl =		NULL,
+	.open =			ipoctal_open,
+	.close =		ipoctal_close,
+	.write =		ipoctal_write_tty,
+	.set_termios =		ipoctal_set_termios,
+	.write_room =		ipoctal_write_room,
+	.chars_in_buffer =	ipoctal_chars_in_buffer,
+	.get_icount =		ipoctal_get_icount,
+	.hangup =		ipoctal_hangup,
+};
+
+static int ipoctal_match(struct ipack_device *dev)
+{
+	int res;
+	unsigned char board_id;
+
+	if ((!dev->bus->ops) || (!dev->bus->ops->map_space) ||
+	    (!dev->bus->ops->unmap_space))
+		return 0;
+
+	res = dev->bus->ops->map_space(dev, 0, IPACK_ID_SPACE);
+	if (res)
+		return 0;
+
+	res = ipoctal_check_model(dev, &board_id);
+	dev->bus->ops->unmap_space(dev, IPACK_ID_SPACE);
+	if (!res)
+		return 1;
+
+	return 0;
+}
+
+static int ipoctal_probe(struct ipack_device *dev)
+{
+	int res;
+	struct ipoctal *ipoctal;
+
+	ipoctal = kzalloc(sizeof(struct ipoctal), GFP_KERNEL);
+	if (ipoctal == NULL)
+		return -ENOMEM;
+
+	ipoctal->dev = dev;
+	res = ipoctal_inst_slot(ipoctal, dev->bus_nr, dev->slot, dev->irq);
+	if (res)
+		goto out_uninst;
+
+	list_add_tail(&ipoctal->list, &ipoctal_list);
+	return 0;
+
+out_uninst:
+	kfree(ipoctal);
+	return res;
+}
+
+static void __ipoctal_remove(struct ipoctal *ipoctal)
+{
+	int i;
+
+	for (i = 0; i < NR_CHANNELS; i++) {
+		tty_unregister_device(ipoctal->tty_drv, i);
+		tty_port_free_xmit_buf(&ipoctal->tty_port[i]);
+	}
+
+	tty_unregister_driver(ipoctal->tty_drv);
+	put_tty_driver(ipoctal->tty_drv);
+
+	/* Tell the carrier board to free all the resources for this device */
+	if (ipoctal->dev->bus->ops->remove_device != NULL)
+		ipoctal->dev->bus->ops->remove_device(ipoctal->dev);
+
+	list_del(&ipoctal->list);
+	kfree(ipoctal);
+}
+
+static void ipoctal_remove(struct ipack_device *device)
+{
+	struct ipoctal *ipoctal, *next;
+
+	list_for_each_entry_safe(ipoctal, next, &ipoctal_list, list) {
+		if (ipoctal->dev == device)
+			__ipoctal_remove(ipoctal);
+	}
+}
+
+static struct ipack_driver_ops ipoctal_drv_ops = {
+	.match = ipoctal_match,
+	.probe = ipoctal_probe,
+	.remove = ipoctal_remove,
+};
+
+static int __init ipoctal_init(void)
+{
+	driver.ops = &ipoctal_drv_ops;
+	return ipack_driver_register(&driver, THIS_MODULE, KBUILD_MODNAME);
+}
+
+static void __exit ipoctal_exit(void)
+{
+	struct ipoctal *p, *next;
+
+	list_for_each_entry_safe(p, next, &ipoctal_list, list)
+		__ipoctal_remove(p);
+
+	ipack_driver_unregister(&driver);
+}
+
+MODULE_DESCRIPTION("IP-Octal 232, 422 and 485 device driver");
+MODULE_LICENSE("GPL");
+
+module_init(ipoctal_init);
+module_exit(ipoctal_exit);
diff --git a/drivers/staging/ipack/devices/ipoctal.h b/drivers/staging/ipack/devices/ipoctal.h
new file mode 100644
index 00000000000..266f3617069
--- /dev/null
+++ b/drivers/staging/ipack/devices/ipoctal.h
@@ -0,0 +1,80 @@
+/**
+ * ipoctal.h
+ *
+ * driver for the IPOCTAL boards
+ * Copyright (c) 2009 Nicolas Serafini, EIC2 SA
+ * Copyright (c) 2010,2011 Samuel Iglesias Gonsalvez <siglesia@cern.ch>, CERN
+ * Copyright (c) 2012 Samuel Iglesias Gonsalvez <siglesias@igalia.com>, Igalia
+ *
+ * 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.
+ */
+
+#ifndef _IPOCTAL_H
+#define _IPOCTAL_H_
+
+#define NR_CHANNELS		8
+#define IPOCTAL_MAX_BOARDS	16
+#define MAX_DEVICES		(NR_CHANNELS * IPOCTAL_MAX_BOARDS)
+#define RELEVANT_IFLAG(iflag) ((iflag) & (IGNBRK|BRKINT|IGNPAR|PARMRK|INPCK))
+
+/**
+ * enum uart_parity_e - UART supported parity.
+ */
+enum uart_parity_e {
+	UART_NONE  = 0,
+	UART_ODD   = 1,
+	UART_EVEN  = 2,
+};
+
+/**
+ * enum uart_error - UART error type
+ *
+ */
+enum uart_error	{
+	UART_NOERROR = 0,      /* No error during transmission */
+	UART_TIMEOUT = 1 << 0, /* Timeout error */
+	UART_OVERRUN = 1 << 1, /* Overrun error */
+	UART_PARITY  = 1 << 2, /* Parity error */
+	UART_FRAMING = 1 << 3, /* Framing error */
+	UART_BREAK   = 1 << 4, /* Received break */
+};
+
+/**
+ * struct ipoctal_config - Serial configuration
+ *
+ * @baud: Baud rate
+ * @stop_bits: Stop bits (1 or 2)
+ * @bits_per_char: data size in bits
+ * @parity
+ * @flow_control: Flow control management (RTS/CTS) (0 disabled, 1 enabled)
+ */
+struct ipoctal_config {
+	unsigned int baud;
+	unsigned int stop_bits;
+	unsigned int bits_per_char;
+	unsigned short parity;
+	unsigned int flow_control;
+};
+
+/**
+ * struct ipoctal_stats -- Stats since last reset
+ *
+ * @tx: Number of transmitted bytes
+ * @rx: Number of received bytes
+ * @overrun: Number of overrun errors
+ * @parity_err: Number of parity errors
+ * @framing_err: Number of framing errors
+ * @rcv_break: Number of break received
+ */
+struct ipoctal_stats {
+	unsigned long tx;
+	unsigned long rx;
+	unsigned long overrun_err;
+	unsigned long parity_err;
+	unsigned long framing_err;
+	unsigned long rcv_break;
+};
+
+#endif /* _IPOCTAL_H_ */
diff --git a/drivers/staging/ipack/devices/scc2698.h b/drivers/staging/ipack/devices/scc2698.h
new file mode 100644
index 00000000000..47f6269985f
--- /dev/null
+++ b/drivers/staging/ipack/devices/scc2698.h
@@ -0,0 +1,228 @@
+/*
+ * scc2698.h
+ *
+ * driver for the IPOCTAL boards
+ * Copyright (c) 2009 Nicolas Serafini, EIC2 SA
+ * Copyright (c) 2010,2011 Samuel Iglesias Gonsalvez <siglesia@cern.ch>, CERN
+ * Copyright (c) 2012 Samuel Iglesias Gonsalvez <siglesias@igalia.com>, Igalia
+ *
+ * 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.
+ */
+
+#ifndef SCC2698_H_
+#define SCC2698_H_
+
+/*
+ * struct scc2698_channel - Channel access to scc2698 IO
+ *
+ * dn value are only spacer.
+ *
+ */
+struct scc2698_channel {
+	union {
+		struct {
+			unsigned char d0, mr;  /* Mode register 1/2*/
+			unsigned char d1, sr;  /* Status register */
+			unsigned char d2, r1;  /* reserved */
+			unsigned char d3, rhr; /* Receive holding register (R) */
+			unsigned char junk[8]; /* other crap for block control */
+		} r; /* Read access */
+		struct {
+			unsigned char d0, mr;  /* Mode register 1/2 */
+			unsigned char d1, csr; /* Clock select register */
+			unsigned char d2, cr;  /* Command register */
+			unsigned char d3, thr; /* Transmit holding register */
+			unsigned char junk[8]; /* other crap for block control */
+		} w; /* Write access */
+	} u;
+};
+
+/*
+ * struct scc2698_block - Block access to scc2698 IO
+ *
+ * The scc2698 contain 4 block.
+ * Each block containt two channel a and b.
+ * dn value are only spacer.
+ *
+ */
+struct scc2698_block {
+	union {
+		struct {
+			unsigned char d0, mra;  /* Mode register 1/2 (a) */
+			unsigned char d1, sra;  /* Status register (a) */
+			unsigned char d2, r1;   /* reserved */
+			unsigned char d3, rhra; /* Receive holding register (a) */
+			unsigned char d4, ipcr; /* Input port change register of block */
+			unsigned char d5, isr;  /* Interrupt status register of block */
+			unsigned char d6, ctur; /* Counter timer upper register of block */
+			unsigned char d7, ctlr; /* Counter timer lower register of block */
+			unsigned char d8, mrb;  /* Mode register 1/2 (b) */
+			unsigned char d9, srb;  /* Status register (b) */
+			unsigned char da, r2;   /* reserved */
+			unsigned char db, rhrb; /* Receive holding register (b) */
+			unsigned char dc, r3;   /* reserved */
+			unsigned char dd, ip;   /* Input port register of block */
+			unsigned char de, ctg;  /* Start counter timer of block */
+			unsigned char df, cts;  /* Stop counter timer of block */
+		} r; /* Read access */
+		struct {
+			unsigned char d0, mra;  /* Mode register 1/2 (a) */
+			unsigned char d1, csra; /* Clock select register (a) */
+			unsigned char d2, cra;  /* Command register (a) */
+			unsigned char d3, thra; /* Transmit holding register (a) */
+			unsigned char d4, acr;  /* Auxiliary control register of block */
+			unsigned char d5, imr;  /* Interrupt mask register of block  */
+			unsigned char d6, ctu;  /* Counter timer upper register of block */
+			unsigned char d7, ctl;  /* Counter timer lower register of block */
+			unsigned char d8, mrb;  /* Mode register 1/2 (b) */
+			unsigned char d9, csrb; /* Clock select register (a) */
+			unsigned char da, crb;  /* Command register (b) */
+			unsigned char db, thrb; /* Transmit holding register (b) */
+			unsigned char dc, r1;   /* reserved */
+			unsigned char dd, opcr; /* Output port configuration register of block */
+			unsigned char de, r2;   /* reserved */
+			unsigned char df, r3;   /* reserved */
+		} w; /* Write access */
+	} u;
+} ;
+
+#define MR1_CHRL_5_BITS             (0x0 << 0)
+#define MR1_CHRL_6_BITS             (0x1 << 0)
+#define MR1_CHRL_7_BITS             (0x2 << 0)
+#define MR1_CHRL_8_BITS             (0x3 << 0)
+#define MR1_PARITY_EVEN             (0x1 << 2)
+#define MR1_PARITY_ODD              (0x0 << 2)
+#define MR1_PARITY_ON               (0x0 << 3)
+#define MR1_PARITY_FORCE            (0x1 << 3)
+#define MR1_PARITY_OFF              (0x2 << 3)
+#define MR1_PARITY_SPECIAL          (0x3 << 3)
+#define MR1_ERROR_CHAR              (0x0 << 5)
+#define MR1_ERROR_BLOCK             (0x1 << 5)
+#define MR1_RxINT_RxRDY             (0x0 << 6)
+#define MR1_RxINT_FFULL             (0x1 << 6)
+#define MR1_RxRTS_CONTROL_ON        (0x1 << 7)
+#define MR1_RxRTS_CONTROL_OFF       (0x0 << 7)
+
+#define MR2_STOP_BITS_LENGTH_1      (0x7 << 0)
+#define MR2_STOP_BITS_LENGTH_2      (0xF << 0)
+#define MR2_CTS_ENABLE_TX_ON        (0x1 << 4)
+#define MR2_CTS_ENABLE_TX_OFF       (0x0 << 4)
+#define MR2_TxRTS_CONTROL_ON        (0x1 << 5)
+#define MR2_TxRTS_CONTROL_OFF       (0x0 << 5)
+#define MR2_CH_MODE_NORMAL          (0x0 << 6)
+#define MR2_CH_MODE_ECHO            (0x1 << 6)
+#define MR2_CH_MODE_LOCAL           (0x2 << 6)
+#define MR2_CH_MODE_REMOTE          (0x3 << 6)
+
+#define CR_ENABLE_RX                (0x1 << 0)
+#define CR_DISABLE_RX               (0x1 << 1)
+#define CR_ENABLE_TX                (0x1 << 2)
+#define CR_DISABLE_TX               (0x1 << 3)
+#define CR_CMD_RESET_MR             (0x1 << 4)
+#define CR_CMD_RESET_RX             (0x2 << 4)
+#define CR_CMD_RESET_TX             (0x3 << 4)
+#define CR_CMD_RESET_ERR_STATUS     (0x4 << 4)
+#define CR_CMD_RESET_BREAK_CHANGE   (0x5 << 4)
+#define CR_CMD_START_BREAK          (0x6 << 4)
+#define CR_CMD_STOP_BREAK           (0x7 << 4)
+#define CR_CMD_ASSERT_RTSN          (0x8 << 4)
+#define CR_CMD_NEGATE_RTSN          (0x9 << 4)
+#define CR_CMD_SET_TIMEOUT_MODE     (0xA << 4)
+#define CR_CMD_DISABLE_TIMEOUT_MODE (0xC << 4)
+
+#define SR_RX_READY                 (0x1 << 0)
+#define SR_FIFO_FULL                (0x1 << 1)
+#define SR_TX_READY                 (0x1 << 2)
+#define SR_TX_EMPTY                 (0x1 << 3)
+#define SR_OVERRUN_ERROR            (0x1 << 4)
+#define SR_PARITY_ERROR             (0x1 << 5)
+#define SR_FRAMING_ERROR            (0x1 << 6)
+#define SR_RECEIVED_BREAK           (0x1 << 7)
+
+#define SR_ERROR                    (0xF0)
+
+#define ACR_DELTA_IP0_IRQ_EN        (0x1 << 0)
+#define ACR_DELTA_IP1_IRQ_EN        (0x1 << 1)
+#define ACR_DELTA_IP2_IRQ_EN        (0x1 << 2)
+#define ACR_DELTA_IP3_IRQ_EN        (0x1 << 3)
+#define ACR_CT_Mask                 (0x7 << 4)
+#define ACR_CExt                    (0x0 << 4)
+#define ACR_CTxCA                   (0x1 << 4)
+#define ACR_CTxCB                   (0x2 << 4)
+#define ACR_CClk16                  (0x3 << 4)
+#define ACR_TExt                    (0x4 << 4)
+#define ACR_TExt16                  (0x5 << 4)
+#define ACR_TClk                    (0x6 << 4)
+#define ACR_TClk16                  (0x7 << 4)
+#define ACR_BRG_SET1                (0x0 << 7)
+#define ACR_BRG_SET2                (0x1 << 7)
+
+#define TX_CLK_75                   (0x0 << 0)
+#define TX_CLK_110                  (0x1 << 0)
+#define TX_CLK_38400                (0x2 << 0)
+#define TX_CLK_150                  (0x3 << 0)
+#define TX_CLK_300                  (0x4 << 0)
+#define TX_CLK_600                  (0x5 << 0)
+#define TX_CLK_1200                 (0x6 << 0)
+#define TX_CLK_2000                 (0x7 << 0)
+#define TX_CLK_2400                 (0x8 << 0)
+#define TX_CLK_4800                 (0x9 << 0)
+#define TX_CLK_1800                 (0xA << 0)
+#define TX_CLK_9600                 (0xB << 0)
+#define TX_CLK_19200                (0xC << 0)
+#define RX_CLK_75                   (0x0 << 4)
+#define RX_CLK_110                  (0x1 << 4)
+#define RX_CLK_38400                (0x2 << 4)
+#define RX_CLK_150                  (0x3 << 4)
+#define RX_CLK_300                  (0x4 << 4)
+#define RX_CLK_600                  (0x5 << 4)
+#define RX_CLK_1200                 (0x6 << 4)
+#define RX_CLK_2000                 (0x7 << 4)
+#define RX_CLK_2400                 (0x8 << 4)
+#define RX_CLK_4800                 (0x9 << 4)
+#define RX_CLK_1800                 (0xA << 4)
+#define RX_CLK_9600                 (0xB << 4)
+#define RX_CLK_19200                (0xC << 4)
+
+#define OPCR_MPOa_RTSN              (0x0 << 0)
+#define OPCR_MPOa_C_TO              (0x1 << 0)
+#define OPCR_MPOa_TxC1X             (0x2 << 0)
+#define OPCR_MPOa_TxC16X            (0x3 << 0)
+#define OPCR_MPOa_RxC1X             (0x4 << 0)
+#define OPCR_MPOa_RxC16X            (0x5 << 0)
+#define OPCR_MPOa_TxRDY             (0x6 << 0)
+#define OPCR_MPOa_RxRDY_FF          (0x7 << 0)
+
+#define OPCR_MPOb_RTSN              (0x0 << 4)
+#define OPCR_MPOb_C_TO              (0x1 << 4)
+#define OPCR_MPOb_TxC1X             (0x2 << 4)
+#define OPCR_MPOb_TxC16X            (0x3 << 4)
+#define OPCR_MPOb_RxC1X             (0x4 << 4)
+#define OPCR_MPOb_RxC16X            (0x5 << 4)
+#define OPCR_MPOb_TxRDY             (0x6 << 4)
+#define OPCR_MPOb_RxRDY_FF          (0x7 << 4)
+
+#define OPCR_MPP_INPUT              (0x0 << 7)
+#define OPCR_MPP_OUTPUT             (0x1 << 7)
+
+#define IMR_TxRDY_A                 (0x1 << 0)
+#define IMR_RxRDY_FFULL_A           (0x1 << 1)
+#define IMR_DELTA_BREAK_A           (0x1 << 2)
+#define IMR_COUNTER_READY           (0x1 << 3)
+#define IMR_TxRDY_B                 (0x1 << 4)
+#define IMR_RxRDY_FFULL_B           (0x1 << 5)
+#define IMR_DELTA_BREAK_B           (0x1 << 6)
+#define IMR_INPUT_PORT_CHANGE       (0x1 << 7)
+
+#define ISR_TxRDY_A                 (0x1 << 0)
+#define ISR_RxRDY_FFULL_A           (0x1 << 1)
+#define ISR_DELTA_BREAK_A           (0x1 << 2)
+#define ISR_COUNTER_READY           (0x1 << 3)
+#define ISR_TxRDY_B                 (0x1 << 4)
+#define ISR_RxRDY_FFULL_B           (0x1 << 5)
+#define ISR_DELTA_BREAK_B           (0x1 << 6)
+#define ISR_INPUT_PORT_CHANGE       (0x1 << 7)
+
+#endif /* SCC2698_H_ */
diff --git a/drivers/staging/ipack/ipack.c b/drivers/staging/ipack/ipack.c
new file mode 100644
index 00000000000..2b4fa51bf16
--- /dev/null
+++ b/drivers/staging/ipack/ipack.c
@@ -0,0 +1,205 @@
+/*
+ * Industry-pack bus support functions.
+ *
+ * (C) 2011 Samuel Iglesias Gonsalvez <siglesia@cern.ch>, CERN
+ * (C) 2012 Samuel Iglesias Gonsalvez <siglesias@igalia.com>, Igalia
+ *
+ * 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.
+ */
+
+#define pr_fmt(fmt) KBUILD_MODNAME ": " fmt
+
+#include <linux/module.h>
+#include <linux/mutex.h>
+#include <linux/slab.h>
+#include "ipack.h"
+
+#define to_ipack_dev(device) container_of(device, struct ipack_device, dev)
+#define to_ipack_driver(drv) container_of(drv, struct ipack_driver, driver)
+
+/* used when allocating bus numbers */
+#define IPACK_MAXBUS              64
+
+static DEFINE_MUTEX(ipack_mutex);
+
+struct ipack_busmap {
+	unsigned long busmap[IPACK_MAXBUS / (8*sizeof(unsigned long))];
+};
+static struct ipack_busmap busmap;
+
+static void ipack_device_release(struct device *dev)
+{
+	struct ipack_device *device = to_ipack_dev(dev);
+	kfree(device);
+}
+
+static int ipack_bus_match(struct device *device, struct device_driver *driver)
+{
+	int ret;
+	struct ipack_device *dev = to_ipack_dev(device);
+	struct ipack_driver *drv = to_ipack_driver(driver);
+
+	if ((!drv->ops) || (!drv->ops->match))
+		return -EINVAL;
+
+	ret = drv->ops->match(dev);
+	if (ret)
+		dev->driver = drv;
+
+	return 0;
+}
+
+static int ipack_bus_probe(struct device *device)
+{
+	struct ipack_device *dev = to_ipack_dev(device);
+
+	if (!dev->driver->ops->probe)
+		return -EINVAL;
+
+	return dev->driver->ops->probe(dev);
+}
+
+static int ipack_bus_remove(struct device *device)
+{
+	struct ipack_device *dev = to_ipack_dev(device);
+
+	if (!dev->driver->ops->remove)
+		return -EINVAL;
+
+	dev->driver->ops->remove(dev);
+	return 0;
+}
+
+static struct bus_type ipack_bus_type = {
+	.name  = "ipack",
+	.probe = ipack_bus_probe,
+	.match = ipack_bus_match,
+	.remove = ipack_bus_remove,
+};
+
+static int ipack_assign_bus_number(void)
+{
+	int busnum;
+
+	mutex_lock(&ipack_mutex);
+	busnum = find_next_zero_bit(busmap.busmap, IPACK_MAXBUS, 1);
+
+	if (busnum >= IPACK_MAXBUS) {
+		pr_err("too many buses\n");
+		busnum = -1;
+		goto error_find_busnum;
+	}
+
+	set_bit(busnum, busmap.busmap);
+
+error_find_busnum:
+	mutex_unlock(&ipack_mutex);
+	return busnum;
+}
+
+struct ipack_bus_device *ipack_bus_register(struct device *parent, int slots,
+					    struct ipack_bus_ops *ops)
+{
+	int bus_nr;
+	struct ipack_bus_device *bus;
+
+	bus = kzalloc(sizeof(struct ipack_bus_device), GFP_KERNEL);
+	if (!bus)
+		return NULL;
+
+	bus_nr = ipack_assign_bus_number();
+	if (bus_nr < 0) {
+		kfree(bus);
+		return NULL;
+	}
+
+	bus->bus_nr = bus_nr;
+	bus->parent = parent;
+	bus->slots = slots;
+	bus->ops = ops;
+	return bus;
+}
+EXPORT_SYMBOL_GPL(ipack_bus_register);
+
+int ipack_bus_unregister(struct ipack_bus_device *bus)
+{
+	mutex_lock(&ipack_mutex);
+	clear_bit(bus->bus_nr, busmap.busmap);
+	mutex_unlock(&ipack_mutex);
+	kfree(bus);
+	return 0;
+}
+EXPORT_SYMBOL_GPL(ipack_bus_unregister);
+
+int ipack_driver_register(struct ipack_driver *edrv, struct module *owner,
+			  char *name)
+{
+	edrv->driver.owner = owner;
+	edrv->driver.name = name;
+	edrv->driver.bus = &ipack_bus_type;
+	return driver_register(&edrv->driver);
+}
+EXPORT_SYMBOL_GPL(ipack_driver_register);
+
+void ipack_driver_unregister(struct ipack_driver *edrv)
+{
+	driver_unregister(&edrv->driver);
+}
+EXPORT_SYMBOL_GPL(ipack_driver_unregister);
+
+struct ipack_device *ipack_device_register(struct ipack_bus_device *bus,
+					   int slot, int irqv)
+{
+	int ret;
+	struct ipack_device *dev;
+
+	dev = kzalloc(sizeof(struct ipack_device), GFP_KERNEL);
+	if (!dev)
+		return NULL;
+
+	dev->dev.bus = &ipack_bus_type;
+	dev->dev.release = ipack_device_release;
+	dev->dev.parent = bus->parent;
+	dev->slot = slot;
+	dev->bus_nr = bus->bus_nr;
+	dev->irq = irqv;
+	dev->bus = bus;
+	dev_set_name(&dev->dev,
+		     "ipack-dev.%u.%u", dev->bus_nr, dev->slot);
+
+	ret = device_register(&dev->dev);
+	if (ret < 0) {
+		pr_err("error registering the device.\n");
+		dev->driver->ops->remove(dev);
+		kfree(dev);
+		return NULL;
+	}
+
+	return dev;
+}
+EXPORT_SYMBOL_GPL(ipack_device_register);
+
+void ipack_device_unregister(struct ipack_device *dev)
+{
+	device_unregister(&dev->dev);
+}
+EXPORT_SYMBOL_GPL(ipack_device_unregister);
+
+static int __init ipack_init(void)
+{
+	return bus_register(&ipack_bus_type);
+}
+
+static void __exit ipack_exit(void)
+{
+	bus_unregister(&ipack_bus_type);
+}
+
+module_init(ipack_init);
+module_exit(ipack_exit);
+
+MODULE_AUTHOR("Samuel Iglesias Gonsalvez <siglesias@igalia.com>");
+MODULE_LICENSE("GPL");
+MODULE_DESCRIPTION("Industry-pack bus core");
diff --git a/drivers/staging/ipack/ipack.h b/drivers/staging/ipack/ipack.h
new file mode 100644
index 00000000000..8bc001e3ca4
--- /dev/null
+++ b/drivers/staging/ipack/ipack.h
@@ -0,0 +1,183 @@
+/*
+ * Industry-pack bus.
+ *
+ * (C) 2011 Samuel Iglesias Gonsalvez <siglesia@cern.ch>, CERN
+ * (C) 2012 Samuel Iglesias Gonsalvez <siglesias@igalia.com>, Igalia
+ *
+ * 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.
+ */
+
+#include <linux/device.h>
+
+#define IPACK_IDPROM_OFFSET_I			0x01
+#define IPACK_IDPROM_OFFSET_P			0x03
+#define IPACK_IDPROM_OFFSET_A			0x05
+#define IPACK_IDPROM_OFFSET_C			0x07
+#define IPACK_IDPROM_OFFSET_MANUFACTURER_ID	0x09
+#define IPACK_IDPROM_OFFSET_MODEL		0x0B
+#define IPACK_IDPROM_OFFSET_REVISION		0x0D
+#define IPACK_IDPROM_OFFSET_RESERVED		0x0F
+#define IPACK_IDPROM_OFFSET_DRIVER_ID_L		0x11
+#define IPACK_IDPROM_OFFSET_DRIVER_ID_H		0x13
+#define IPACK_IDPROM_OFFSET_NUM_BYTES		0x15
+#define IPACK_IDPROM_OFFSET_CRC			0x17
+
+struct ipack_bus_ops;
+struct ipack_driver;
+
+enum ipack_space {
+	IPACK_IO_SPACE    = 0,
+	IPACK_ID_SPACE    = 1,
+	IPACK_MEM_SPACE   = 2,
+};
+
+/**
+ *	struct ipack_addr_space - Virtual address space mapped for a specified type.
+ *
+ *	@address: virtual address
+ *	@size: size of the mapped space
+ */
+struct ipack_addr_space {
+	void __iomem *address;
+	unsigned int size;
+};
+
+/**
+ *	struct ipack_device
+ *
+ *	@bus_nr: IP bus number where the device is plugged
+ *	@slot: Slot where the device is plugged in the carrier board
+ *	@irq: IRQ vector
+ *	@driver: Pointer to the ipack_driver that manages the device
+ *	@bus: ipack_bus_device where the device is plugged to.
+ *	@id_space: Virtual address to ID space.
+ *	@io_space: Virtual address to IO space.
+ *	@mem_space: Virtual address to MEM space.
+ *	@dev: device in kernel representation.
+ *
+ * Warning: Direct access to mapped memory is possible but the endianness
+ * is not the same with PCI carrier or VME carrier. The endianness is managed
+ * by the carrier board throught bus->ops.
+ */
+struct ipack_device {
+	unsigned int bus_nr;
+	unsigned int slot;
+	unsigned int irq;
+	struct ipack_driver *driver;
+	struct ipack_bus_device *bus;
+	struct ipack_addr_space id_space;
+	struct ipack_addr_space io_space;
+	struct ipack_addr_space mem_space;
+	struct device dev;
+};
+
+/**
+ *	struct ipack_driver_ops -- callbacks to mezzanine driver for installing/removing one device
+ *
+ *	@match: Match function
+ *	@probe: Probe function
+ *	@remove: tell the driver that the carrier board wants to remove one device
+ */
+
+struct ipack_driver_ops {
+	int (*match) (struct ipack_device *dev);
+	int (*probe) (struct ipack_device *dev);
+	void (*remove) (struct ipack_device *dev);
+};
+
+/**
+ *	struct ipack_driver -- Specific data to each ipack board driver
+ *
+ *	@driver: Device driver kernel representation
+ *	@ops: Mezzanine driver operations specific for the ipack bus.
+ */
+struct ipack_driver {
+	struct device_driver driver;
+	struct ipack_driver_ops *ops;
+};
+
+/**
+ *	struct ipack_bus_ops - available operations on a bridge module
+ *
+ *	@map_space: map IP address space
+ *	@unmap_space: unmap IP address space
+ *	@request_irq: request IRQ
+ *	@free_irq: free IRQ
+ *	@read8: read unsigned char
+ *	@read16: read unsigned short
+ *	@read32: read unsigned int
+ *	@write8: read unsigned char
+ *	@write16: read unsigned short
+ *	@write32: read unsigned int
+ *	@remove_device: tell the bridge module that the device has been removed
+ */
+struct ipack_bus_ops {
+	int (*map_space) (struct ipack_device *dev, unsigned int memory_size, int space);
+	int (*unmap_space) (struct ipack_device *dev, int space);
+	int (*request_irq) (struct ipack_device *dev, int vector, int (*handler)(void *), void *arg);
+	int (*free_irq) (struct ipack_device *dev);
+	int (*read8) (struct ipack_device *dev, int space, unsigned long offset, unsigned char *value);
+	int (*read16) (struct ipack_device *dev, int space, unsigned long offset, unsigned short *value);
+	int (*read32) (struct ipack_device *dev, int space, unsigned long offset, unsigned int *value);
+	int (*write8) (struct ipack_device *dev, int space, unsigned long offset, unsigned char value);
+	int (*write16) (struct ipack_device *dev, int space, unsigned long offset, unsigned short value);
+	int (*write32) (struct ipack_device *dev, int space, unsigned long offset, unsigned int value);
+	int (*remove_device) (struct ipack_device *dev);
+};
+
+/**
+ *	struct ipack_bus_device
+ *
+ *	@dev: pointer to carrier device
+ *	@slots: number of slots available
+ *	@bus_nr: ipack bus number
+ *	@ops: bus operations for the mezzanine drivers
+ */
+struct ipack_bus_device {
+	struct device *parent;
+	int slots;
+	int bus_nr;
+	struct ipack_bus_ops *ops;
+};
+
+/**
+ *	ipack_bus_register -- register a new ipack bus
+ *
+ * @parent: pointer to the parent device, if any.
+ * @slots: number of slots available in the bus device.
+ * @ops: bus operations for the mezzanine drivers.
+ *
+ * The carrier board device should call this function to register itself as
+ * available bus device in ipack.
+ */
+struct ipack_bus_device *ipack_bus_register(struct device *parent, int slots,
+					    struct ipack_bus_ops *ops);
+
+/**
+ *	ipack_bus_unregister -- unregister an ipack bus
+ */
+int ipack_bus_unregister(struct ipack_bus_device *bus);
+
+/**
+ *	ipack_driver_register -- Register a new driver
+ *
+ * Called by a ipack driver to register itself as a driver
+ * that can manage ipack devices.
+ */
+int ipack_driver_register(struct ipack_driver *edrv, struct module *owner, char *name);
+void ipack_driver_unregister(struct ipack_driver *edrv);
+
+/**
+ *	ipack_device_register -- register a new mezzanine device
+ *
+ * @bus: ipack bus device it is plugged to.
+ * @slot: slot position in the bus device.
+ * @irqv: IRQ vector for the mezzanine.
+ *
+ * Register a new ipack device (mezzanine device). The call is done by
+ * the carrier device driver.
+ */
+struct ipack_device *ipack_device_register(struct ipack_bus_device *bus, int slot, int irqv);
+void ipack_device_unregister(struct ipack_device *dev);
diff --git a/drivers/staging/line6/config.h b/drivers/staging/line6/config.h
deleted file mode 100644
index f8a5149e3da..00000000000
--- a/drivers/staging/line6/config.h
+++ /dev/null
@@ -1,48 +0,0 @@
-/*
- * Line6 Linux USB driver - 0.8.0
- *
- * Copyright (C) 2004-2009 Markus Grabner (grabner@icg.tugraz.at)
- *
- *	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.
- *
- */
-
-#ifndef CONFIG_H
-#define CONFIG_H
-
-
-#ifdef CONFIG_USB_DEBUG
-#define DEBUG 1
-#endif
-
-
-/*
- * Development tools.
- */
-#define DO_DEBUG_MESSAGES    0
-#define DO_DUMP_URB_SEND     DO_DEBUG_MESSAGES
-#define DO_DUMP_URB_RECEIVE  DO_DEBUG_MESSAGES
-#define DO_DUMP_PCM_SEND     0
-#define DO_DUMP_PCM_RECEIVE  0
-#define DO_DUMP_MIDI_SEND    DO_DEBUG_MESSAGES
-#define DO_DUMP_MIDI_RECEIVE DO_DEBUG_MESSAGES
-#define DO_DUMP_ANY          (DO_DUMP_URB_SEND || DO_DUMP_URB_RECEIVE || \
-			      DO_DUMP_PCM_SEND || DO_DUMP_PCM_RECEIVE || \
-			      DO_DUMP_MIDI_SEND || DO_DUMP_MIDI_RECEIVE)
-#define CREATE_RAW_FILE      0
-
-#if DO_DEBUG_MESSAGES
-#define CHECKPOINT printk(KERN_INFO "line6usb: %s (%s:%d)\n", \
-			  __func__, __FILE__, __LINE__)
-#endif
-
-#if DO_DEBUG_MESSAGES
-#define DEBUG_MESSAGES(x) (x)
-#else
-#define DEBUG_MESSAGES(x)
-#endif
-
-
-#endif
diff --git a/drivers/staging/line6/driver.c b/drivers/staging/line6/driver.c
index e8023afd365..4513f78f112 100644
--- a/drivers/staging/line6/driver.c
+++ b/drivers/staging/line6/driver.c
@@ -92,15 +92,10 @@ const unsigned char line6_midi_id[] = {
 	Code to request version of POD, Variax interface
 	(and maybe other devices).
 */
-static const char line6_request_version0[] = {
+static const char line6_request_version[] = {
 	0xf0, 0x7e, 0x7f, 0x06, 0x01, 0xf7
 };
 
-/*
-	Copy of version request code with GFP_KERNEL flag for use in URB.
-*/
-static const char *line6_request_version;
-
 struct usb_line6 *line6_devices[LINE6_MAX_DEVICES];
 
 /**
@@ -336,8 +331,21 @@ int line6_send_raw_message_async(struct usb_line6 *line6, const char *buffer,
 */
 int line6_version_request_async(struct usb_line6 *line6)
 {
-	return line6_send_raw_message_async(line6, line6_request_version,
-					    sizeof(line6_request_version0));
+	char *buffer;
+	int retval;
+
+	buffer = kmalloc(sizeof(line6_request_version), GFP_ATOMIC);
+	if (buffer == NULL) {
+		dev_err(line6->ifcdev, "Out of memory");
+		return -ENOMEM;
+	}
+
+	memcpy(buffer, line6_request_version, sizeof(line6_request_version));
+
+	retval = line6_send_raw_message_async(line6, buffer,
+					      sizeof(line6_request_version));
+	kfree(buffer);
+	return retval;
 }
 
 /*
@@ -1292,69 +1300,7 @@ static struct usb_driver line6_driver = {
 	.id_table = line6_id_table,
 };
 
-/*
-	Module initialization.
-*/
-static int __init line6_init(void)
-{
-	int i, retval;
-
-	printk(KERN_INFO "%s driver version %s\n", DRIVER_NAME, DRIVER_VERSION);
-
-	for (i = LINE6_MAX_DEVICES; i--;)
-		line6_devices[i] = NULL;
-
-	retval = usb_register(&line6_driver);
-
-	if (retval) {
-		err("usb_register failed. Error number %d", retval);
-		return retval;
-	}
-
-	line6_request_version = kmalloc(sizeof(line6_request_version0),
-					GFP_KERNEL);
-
-	if (line6_request_version == NULL) {
-		err("Out of memory");
-		return -ENOMEM;
-	}
-
-	memcpy((char *)line6_request_version, line6_request_version0,
-	       sizeof(line6_request_version0));
-
-	return retval;
-}
-
-/*
-	Module cleanup.
-*/
-static void __exit line6_exit(void)
-{
-	int i;
-	struct usb_line6 *line6;
-	struct snd_line6_pcm *line6pcm;
-
-	/* stop all PCM channels */
-	for (i = LINE6_MAX_DEVICES; i--;) {
-		line6 = line6_devices[i];
-
-		if (line6 == NULL)
-			continue;
-
-		line6pcm = line6->line6pcm;
-
-		if (line6pcm == NULL)
-			continue;
-
-		line6_pcm_release(line6pcm, ~0);
-	}
-
-	usb_deregister(&line6_driver);
-	kfree(line6_request_version);
-}
-
-module_init(line6_init);
-module_exit(line6_exit);
+module_usb_driver(line6_driver);
 
 MODULE_AUTHOR(DRIVER_AUTHOR);
 MODULE_DESCRIPTION(DRIVER_DESC);
diff --git a/drivers/staging/line6/midi.c b/drivers/staging/line6/midi.c
index 13d02939c3c..50407294fbd 100644
--- a/drivers/staging/line6/midi.c
+++ b/drivers/staging/line6/midi.c
@@ -406,7 +406,7 @@ int line6_init_midi(struct usb_line6 *line6)
 
 	line6midi->line6 = line6;
 
-	switch(line6->product) {
+	switch (line6->product) {
 	case LINE6_DEVID_PODHD300:
 	case LINE6_DEVID_PODHD500:
 		line6midi->midi_mask_transmit = 1;
diff --git a/drivers/staging/line6/midibuf.c b/drivers/staging/line6/midibuf.c
index 7b532e5ce8b..836e8c847c5 100644
--- a/drivers/staging/line6/midibuf.c
+++ b/drivers/staging/line6/midibuf.c
@@ -64,7 +64,7 @@ int line6_midibuf_init(struct MidiBuffer *this, int size, int split)
 
 void line6_midibuf_status(struct MidiBuffer *this)
 {
-	printk(KERN_DEBUG "midibuf size=%d split=%d pos_read=%d pos_write=%d "
+	pr_debug("midibuf size=%d split=%d pos_read=%d pos_write=%d "
 	       "full=%d command_prev=%02x\n", this->size, this->split,
 	       this->pos_read, this->pos_write, this->full, this->command_prev);
 }
diff --git a/drivers/staging/line6/pcm.c b/drivers/staging/line6/pcm.c
index 90d2d4475cb..5e319e3ce68 100644
--- a/drivers/staging/line6/pcm.c
+++ b/drivers/staging/line6/pcm.c
@@ -99,7 +99,7 @@ int line6_pcm_acquire(struct snd_line6_pcm *line6pcm, int channels)
 	unsigned long flags_new = flags_old | channels;
 	unsigned long flags_final = flags_old;
 	int err = 0;
-	
+
 	line6pcm->prev_fbuf = NULL;
 
 	if (test_flags(flags_old, flags_new, LINE6_BITS_CAPTURE_BUFFER)) {
diff --git a/drivers/staging/line6/toneport.c b/drivers/staging/line6/toneport.c
index b754f69a29c..31b624b6342 100644
--- a/drivers/staging/line6/toneport.c
+++ b/drivers/staging/line6/toneport.c
@@ -168,7 +168,7 @@ static int toneport_send_cmd(struct usb_device *usbdev, int cmd1, int cmd2)
 			      cmd1, cmd2, NULL, 0, LINE6_TIMEOUT * HZ);
 
 	if (ret < 0) {
-		err("send failed (error %d)\n", ret);
+		dev_err(&usbdev->dev, "send failed (error %d)\n", ret);
 		return ret;
 	}
 
diff --git a/drivers/staging/media/as102/as102_drv.c b/drivers/staging/media/as102/as102_drv.c
index ea4f992de23..ac92eaf6c74 100644
--- a/drivers/staging/media/as102/as102_drv.c
+++ b/drivers/staging/media/as102/as102_drv.c
@@ -279,40 +279,8 @@ void as102_dvb_unregister(struct as102_dev_t *as102_dev)
 	pr_info("Unregistered device %s", as102_dev->name);
 }
 
-static int __init as102_driver_init(void)
-{
-	int ret;
-
-	/* register this driver with the low level subsystem */
-	ret = usb_register(&as102_usb_driver);
-	if (ret)
-		err("usb_register failed (ret = %d)", ret);
-
-	return ret;
-}
-
-/*
- * Mandatory function : Adds a special section to the module indicating
- * where initialisation function is defined
- */
-module_init(as102_driver_init);
-
-/**
- * as102_driver_exit - as102 driver exit point
- *
- * This function is called when device has to be removed.
- */
-static void __exit as102_driver_exit(void)
-{
-	/* deregister this driver with the low level bus subsystem */
-	usb_deregister(&as102_usb_driver);
-}
+module_usb_driver(as102_usb_driver);
 
-/*
- * required function for unload: Adds a special section to the module
- * indicating where unload function is defined
- */
-module_exit(as102_driver_exit);
 /* modinfo details */
 MODULE_DESCRIPTION(DRIVER_FULL_NAME);
 MODULE_LICENSE("GPL");
diff --git a/drivers/staging/media/as102/as102_fe.c b/drivers/staging/media/as102/as102_fe.c
index 5917657b9d0..9ce8c9daa2e 100644
--- a/drivers/staging/media/as102/as102_fe.c
+++ b/drivers/staging/media/as102/as102_fe.c
@@ -17,8 +17,6 @@
  * along with this program; if not, write to the Free Software
  * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
  */
-#include <linux/version.h>
-
 #include "as102_drv.h"
 #include "as10x_types.h"
 #include "as10x_cmd.h"
diff --git a/drivers/staging/media/as102/as102_fw.c b/drivers/staging/media/as102/as102_fw.c
index 1075fb1df0d..b9670ee41b4 100644
--- a/drivers/staging/media/as102/as102_fw.c
+++ b/drivers/staging/media/as102/as102_fw.c
@@ -230,11 +230,8 @@ int as102_fw_upload(struct as10x_bus_adapter_t *bus_adap)
 	pr_info("%s: firmware: %s loaded with success\n",
 		DRIVER_NAME, fw2);
 error:
-	/* free data buffer */
 	kfree(cmd_buf);
-	/* release firmware if needed */
-	if (firmware != NULL)
-		release_firmware(firmware);
+	release_firmware(firmware);
 
 	LEAVE();
 	return errno;
diff --git a/drivers/staging/media/as102/as102_usb_drv.c b/drivers/staging/media/as102/as102_usb_drv.c
index 0f6bfe7eccb..aaf1bc2ad1b 100644
--- a/drivers/staging/media/as102/as102_usb_drv.c
+++ b/drivers/staging/media/as102/as102_usb_drv.c
@@ -367,7 +367,7 @@ static int as102_usb_probe(struct usb_interface *intf,
 	ENTER();
 
 	/* This should never actually happen */
-	if ((sizeof(as102_usb_id_table) / sizeof(struct usb_device_id)) !=
+	if (ARRAY_SIZE(as102_usb_id_table) !=
 	    (sizeof(as102_device_names) / sizeof(const char *))) {
 		pr_err("Device names table invalid size");
 		return -EINVAL;
@@ -375,13 +375,12 @@ static int as102_usb_probe(struct usb_interface *intf,
 
 	as102_dev = kzalloc(sizeof(struct as102_dev_t), GFP_KERNEL);
 	if (as102_dev == NULL) {
-		err("%s: kzalloc failed", __func__);
+		dev_err(&intf->dev, "%s: kzalloc failed\n", __func__);
 		return -ENOMEM;
 	}
 
 	/* Assign the user-friendly device name */
-	for (i = 0; i < (sizeof(as102_usb_id_table) /
-			 sizeof(struct usb_device_id)); i++) {
+	for (i = 0; i < ARRAY_SIZE(as102_usb_id_table); i++) {
 		if (id == &as102_usb_id_table[i]) {
 			as102_dev->name = as102_device_names[i];
 			as102_dev->elna_cfg = as102_elna_cfg[i];
@@ -411,8 +410,9 @@ static int as102_usb_probe(struct usb_interface *intf,
 	ret = usb_register_dev(intf, &as102_usb_class_driver);
 	if (ret < 0) {
 		/* something prevented us from registering this driver */
-		err("%s: usb_register_dev() failed (errno = %d)",
-		    __func__, ret);
+		dev_err(&intf->dev,
+			"%s: usb_register_dev() failed (errno = %d)\n",
+			__func__, ret);
 		goto failed;
 	}
 
diff --git a/drivers/staging/media/as102/as102_usb_drv.h b/drivers/staging/media/as102/as102_usb_drv.h
index fc2884ab02a..1ad1ec52b11 100644
--- a/drivers/staging/media/as102/as102_usb_drv.h
+++ b/drivers/staging/media/as102/as102_usb_drv.h
@@ -17,8 +17,6 @@
  * along with this program; if not, write to the Free Software
  * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
  */
-#include <linux/version.h>
-
 #ifndef _AS102_USB_DRV_H_
 #define _AS102_USB_DRV_H_
 
diff --git a/drivers/staging/media/as102/as10x_cmd.c b/drivers/staging/media/as102/as10x_cmd.c
index 262bb94ad27..a73df10982d 100644
--- a/drivers/staging/media/as102/as10x_cmd.c
+++ b/drivers/staging/media/as102/as10x_cmd.c
@@ -31,7 +31,7 @@
  */
 int as10x_cmd_turn_on(struct as10x_bus_adapter_t *adap)
 {
-	int error;
+	int error = AS10X_CMD_ERROR;
 	struct as10x_cmd_t *pcmd, *prsp;
 
 	ENTER();
@@ -54,8 +54,6 @@ int as10x_cmd_turn_on(struct as10x_bus_adapter_t *adap)
 					    (uint8_t *) prsp,
 					    sizeof(prsp->body.turn_on.rsp) +
 					    HEADER_SIZE);
-	} else {
-		error = AS10X_CMD_ERROR;
 	}
 
 	if (error < 0)
@@ -77,7 +75,7 @@ out:
  */
 int as10x_cmd_turn_off(struct as10x_bus_adapter_t *adap)
 {
-	int error;
+	int error = AS10X_CMD_ERROR;
 	struct as10x_cmd_t *pcmd, *prsp;
 
 	ENTER();
@@ -99,8 +97,6 @@ int as10x_cmd_turn_off(struct as10x_bus_adapter_t *adap)
 			sizeof(pcmd->body.turn_off.req) + HEADER_SIZE,
 			(uint8_t *) prsp,
 			sizeof(prsp->body.turn_off.rsp) + HEADER_SIZE);
-	} else {
-		error = AS10X_CMD_ERROR;
 	}
 
 	if (error < 0)
@@ -124,7 +120,7 @@ out:
 int as10x_cmd_set_tune(struct as10x_bus_adapter_t *adap,
 		       struct as10x_tune_args *ptune)
 {
-	int error;
+	int error = AS10X_CMD_ERROR;
 	struct as10x_cmd_t *preq, *prsp;
 
 	ENTER();
@@ -159,8 +155,6 @@ int as10x_cmd_set_tune(struct as10x_bus_adapter_t *adap,
 					    (uint8_t *) prsp,
 					    sizeof(prsp->body.set_tune.rsp)
 					    + HEADER_SIZE);
-	} else {
-		error = AS10X_CMD_ERROR;
 	}
 
 	if (error < 0)
@@ -184,7 +178,7 @@ out:
 int as10x_cmd_get_tune_status(struct as10x_bus_adapter_t *adap,
 			      struct as10x_tune_status *pstatus)
 {
-	int error;
+	int error = AS10X_CMD_ERROR;
 	struct as10x_cmd_t  *preq, *prsp;
 
 	ENTER();
@@ -208,8 +202,6 @@ int as10x_cmd_get_tune_status(struct as10x_bus_adapter_t *adap,
 			sizeof(preq->body.get_tune_status.req) + HEADER_SIZE,
 			(uint8_t *) prsp,
 			sizeof(prsp->body.get_tune_status.rsp) + HEADER_SIZE);
-	} else {
-		error = AS10X_CMD_ERROR;
 	}
 
 	if (error < 0)
@@ -241,7 +233,7 @@ out:
  */
 int as10x_cmd_get_tps(struct as10x_bus_adapter_t *adap, struct as10x_tps *ptps)
 {
-	int error;
+	int error = AS10X_CMD_ERROR;
 	struct as10x_cmd_t *pcmd, *prsp;
 
 	ENTER();
@@ -266,8 +258,6 @@ int as10x_cmd_get_tps(struct as10x_bus_adapter_t *adap, struct as10x_tps *ptps)
 					    (uint8_t *) prsp,
 					    sizeof(prsp->body.get_tps.rsp) +
 					    HEADER_SIZE);
-	} else {
-		error = AS10X_CMD_ERROR;
 	}
 
 	if (error < 0)
@@ -305,7 +295,7 @@ out:
 int as10x_cmd_get_demod_stats(struct as10x_bus_adapter_t *adap,
 			      struct as10x_demod_stats *pdemod_stats)
 {
-	int error;
+	int error = AS10X_CMD_ERROR;
 	struct as10x_cmd_t *pcmd, *prsp;
 
 	ENTER();
@@ -330,8 +320,6 @@ int as10x_cmd_get_demod_stats(struct as10x_bus_adapter_t *adap,
 				(uint8_t *) prsp,
 				sizeof(prsp->body.get_demod_stats.rsp)
 				+ HEADER_SIZE);
-	} else {
-		error = AS10X_CMD_ERROR;
 	}
 
 	if (error < 0)
@@ -370,7 +358,7 @@ out:
 int as10x_cmd_get_impulse_resp(struct as10x_bus_adapter_t *adap,
 			       uint8_t *is_ready)
 {
-	int error;
+	int error = AS10X_CMD_ERROR;
 	struct as10x_cmd_t *pcmd, *prsp;
 
 	ENTER();
@@ -395,8 +383,6 @@ int as10x_cmd_get_impulse_resp(struct as10x_bus_adapter_t *adap,
 					(uint8_t *) prsp,
 					sizeof(prsp->body.get_impulse_rsp.rsp)
 					+ HEADER_SIZE);
-	} else {
-		error = AS10X_CMD_ERROR;
 	}
 
 	if (error < 0)
diff --git a/drivers/staging/media/dt3155v4l/dt3155v4l.c b/drivers/staging/media/dt3155v4l/dt3155v4l.c
index 280c84ec4cc..c365cdf714e 100644
--- a/drivers/staging/media/dt3155v4l/dt3155v4l.c
+++ b/drivers/staging/media/dt3155v4l/dt3155v4l.c
@@ -898,6 +898,10 @@ dt3155_probe(struct pci_dev *pdev, const struct pci_device_id *id)
 	INIT_LIST_HEAD(&pd->dmaq);
 	mutex_init(&pd->mux);
 	pd->vdev->lock = &pd->mux; /* for locking v4l2_file_operations */
+	/* Locking in file operations other than ioctl should be done
+	   by the driver, not the V4L2 core.
+	   This driver needs auditing so that this flag can be removed. */
+	set_bit(V4L2_FL_LOCK_ALL_FOPS, &pd->vdev->flags);
 	spin_lock_init(&pd->lock);
 	pd->csr2 = csr2_init;
 	pd->config = config_init;
diff --git a/drivers/staging/media/easycap/easycap_ioctl.c b/drivers/staging/media/easycap/easycap_ioctl.c
index 9413b37490c..3cee3cd986d 100644
--- a/drivers/staging/media/easycap/easycap_ioctl.c
+++ b/drivers/staging/media/easycap/easycap_ioctl.c
@@ -26,6 +26,7 @@
 /*****************************************************************************/
 
 #include "easycap.h"
+#include <linux/version.h>
 
 /*--------------------------------------------------------------------------*/
 /*
diff --git a/drivers/staging/media/easycap/easycap_main.c b/drivers/staging/media/easycap/easycap_main.c
index d0fe34afc2e..a1c45e4dcdc 100644
--- a/drivers/staging/media/easycap/easycap_main.c
+++ b/drivers/staging/media/easycap/easycap_main.c
@@ -700,214 +700,7 @@ static int videodev_release(struct video_device *pvideo_device)
 	JOM(4, "ending successfully\n");
 	return 0;
 }
-/*^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^*/
-/*****************************************************************************/
-/*--------------------------------------------------------------------------*/
-/*
- *  THIS FUNCTION IS CALLED FROM WITHIN easycap_usb_disconnect() AND IS
- *  PROTECTED BY SEMAPHORES SET AND CLEARED BY easycap_usb_disconnect().
- *
- *  BY THIS STAGE THE DEVICE HAS ALREADY BEEN PHYSICALLY UNPLUGGED, SO
- *  peasycap->pusb_device IS NO LONGER VALID.
- */
-/*---------------------------------------------------------------------------*/
-static void easycap_delete(struct kref *pkref)
-{
-	struct easycap *peasycap;
-	struct data_urb *pdata_urb;
-	struct list_head *plist_head, *plist_next;
-	int k, m, gone, kd;
-	int allocation_video_urb;
-	int allocation_video_page;
-	int allocation_video_struct;
-	int allocation_audio_urb;
-	int allocation_audio_page;
-	int allocation_audio_struct;
-	int registered_video, registered_audio;
-
-	peasycap = container_of(pkref, struct easycap, kref);
-	if (!peasycap) {
-		SAM("ERROR: peasycap is NULL: cannot perform deletions\n");
-		return;
-	}
-	kd = easycap_isdongle(peasycap);
-/*---------------------------------------------------------------------------*/
-/*
- *  FREE VIDEO.
- */
-/*---------------------------------------------------------------------------*/
-	if (peasycap->purb_video_head) {
-		m = 0;
-		list_for_each(plist_head, peasycap->purb_video_head) {
-			pdata_urb = list_entry(plist_head,
-						struct data_urb, list_head);
-			if (pdata_urb && pdata_urb->purb) {
-				usb_free_urb(pdata_urb->purb);
-				pdata_urb->purb = NULL;
-				peasycap->allocation_video_urb--;
-				m++;
-			}
-		}
-
-		JOM(4, "%i video urbs freed\n", m);
-/*---------------------------------------------------------------------------*/
-		JOM(4, "freeing video data_urb structures.\n");
-		m = 0;
-		list_for_each_safe(plist_head, plist_next,
-					peasycap->purb_video_head) {
-			pdata_urb = list_entry(plist_head,
-						struct data_urb, list_head);
-			if (pdata_urb) {
-				peasycap->allocation_video_struct -=
-						sizeof(struct data_urb);
-				kfree(pdata_urb);
-				m++;
-			}
-		}
-		JOM(4, "%i video data_urb structures freed\n", m);
-		JOM(4, "setting peasycap->purb_video_head=NULL\n");
-		peasycap->purb_video_head = NULL;
-	}
-/*---------------------------------------------------------------------------*/
-	JOM(4, "freeing video isoc buffers.\n");
-	m = 0;
-	for (k = 0;  k < VIDEO_ISOC_BUFFER_MANY;  k++) {
-		if (peasycap->video_isoc_buffer[k].pgo) {
-			free_pages((unsigned long)
-				   peasycap->video_isoc_buffer[k].pgo,
-					VIDEO_ISOC_ORDER);
-			peasycap->video_isoc_buffer[k].pgo = NULL;
-			peasycap->allocation_video_page -=
-						BIT(VIDEO_ISOC_ORDER);
-			m++;
-		}
-	}
-	JOM(4, "isoc video buffers freed: %i pages\n",
-			m * (0x01 << VIDEO_ISOC_ORDER));
-/*---------------------------------------------------------------------------*/
-	JOM(4, "freeing video field buffers.\n");
-	gone = 0;
-	for (k = 0;  k < FIELD_BUFFER_MANY;  k++) {
-		for (m = 0;  m < FIELD_BUFFER_SIZE/PAGE_SIZE;  m++) {
-			if (peasycap->field_buffer[k][m].pgo) {
-				free_page((unsigned long)
-					  peasycap->field_buffer[k][m].pgo);
-				peasycap->field_buffer[k][m].pgo = NULL;
-				peasycap->allocation_video_page -= 1;
-				gone++;
-			}
-		}
-	}
-	JOM(4, "video field buffers freed: %i pages\n", gone);
-/*---------------------------------------------------------------------------*/
-	JOM(4, "freeing video frame buffers.\n");
-	gone = 0;
-	for (k = 0;  k < FRAME_BUFFER_MANY;  k++) {
-		for (m = 0;  m < FRAME_BUFFER_SIZE/PAGE_SIZE;  m++) {
-			if (peasycap->frame_buffer[k][m].pgo) {
-				free_page((unsigned long)
-					  peasycap->frame_buffer[k][m].pgo);
-				peasycap->frame_buffer[k][m].pgo = NULL;
-				peasycap->allocation_video_page -= 1;
-				gone++;
-			}
-		}
-	}
-	JOM(4, "video frame buffers freed: %i pages\n", gone);
-/*---------------------------------------------------------------------------*/
-/*
- *  FREE AUDIO.
- */
-/*---------------------------------------------------------------------------*/
-	if (peasycap->purb_audio_head) {
-		JOM(4, "freeing audio urbs\n");
-		m = 0;
-		list_for_each(plist_head, (peasycap->purb_audio_head)) {
-			pdata_urb = list_entry(plist_head,
-					struct data_urb, list_head);
-			if (pdata_urb && pdata_urb->purb) {
-				usb_free_urb(pdata_urb->purb);
-				pdata_urb->purb = NULL;
-				peasycap->allocation_audio_urb--;
-				m++;
-			}
-		}
-		JOM(4, "%i audio urbs freed\n", m);
-/*---------------------------------------------------------------------------*/
-		JOM(4, "freeing audio data_urb structures.\n");
-		m = 0;
-		list_for_each_safe(plist_head, plist_next,
-					peasycap->purb_audio_head) {
-			pdata_urb = list_entry(plist_head,
-					struct data_urb, list_head);
-			if (pdata_urb) {
-				peasycap->allocation_audio_struct -=
-							sizeof(struct data_urb);
-				kfree(pdata_urb);
-				m++;
-			}
-		}
-		JOM(4, "%i audio data_urb structures freed\n", m);
-		JOM(4, "setting peasycap->purb_audio_head=NULL\n");
-		peasycap->purb_audio_head = NULL;
-	}
-/*---------------------------------------------------------------------------*/
-	JOM(4, "freeing audio isoc buffers.\n");
-	m = 0;
-	for (k = 0;  k < AUDIO_ISOC_BUFFER_MANY;  k++) {
-		if (peasycap->audio_isoc_buffer[k].pgo) {
-			free_pages((unsigned long)
-					(peasycap->audio_isoc_buffer[k].pgo),
-					AUDIO_ISOC_ORDER);
-			peasycap->audio_isoc_buffer[k].pgo = NULL;
-			peasycap->allocation_audio_page -=
-					BIT(AUDIO_ISOC_ORDER);
-			m++;
-		}
-	}
-	JOM(4, "easyoss_delete(): isoc audio buffers freed: %i pages\n",
-					m * (0x01 << AUDIO_ISOC_ORDER));
-/*---------------------------------------------------------------------------*/
-	JOM(4, "freeing easycap structure.\n");
-	allocation_video_urb    = peasycap->allocation_video_urb;
-	allocation_video_page   = peasycap->allocation_video_page;
-	allocation_video_struct = peasycap->allocation_video_struct;
-	registered_video        = peasycap->registered_video;
-	allocation_audio_urb    = peasycap->allocation_audio_urb;
-	allocation_audio_page   = peasycap->allocation_audio_page;
-	allocation_audio_struct = peasycap->allocation_audio_struct;
-	registered_audio        = peasycap->registered_audio;
-
-	if (0 <= kd && DONGLE_MANY > kd) {
-		if (mutex_lock_interruptible(&mutex_dongle)) {
-			SAY("ERROR: cannot down mutex_dongle\n");
-		} else {
-			JOM(4, "locked mutex_dongle\n");
-			easycapdc60_dongle[kd].peasycap = NULL;
-			mutex_unlock(&mutex_dongle);
-			JOM(4, "unlocked mutex_dongle\n");
-			JOT(4, "   null-->dongle[%i].peasycap\n", kd);
-			allocation_video_struct -= sizeof(struct easycap);
-		}
-	} else {
-		SAY("ERROR: cannot purge dongle[].peasycap");
-	}
 
-	kfree(peasycap);
-
-/*---------------------------------------------------------------------------*/
-	SAY("%8i=video urbs    after all deletions\n", allocation_video_urb);
-	SAY("%8i=video pages   after all deletions\n", allocation_video_page);
-	SAY("%8i=video structs after all deletions\n", allocation_video_struct);
-	SAY("%8i=video devices after all deletions\n", registered_video);
-	SAY("%8i=audio urbs    after all deletions\n", allocation_audio_urb);
-	SAY("%8i=audio pages   after all deletions\n", allocation_audio_page);
-	SAY("%8i=audio structs after all deletions\n", allocation_audio_struct);
-	SAY("%8i=audio devices after all deletions\n", registered_audio);
-
-	JOT(4, "ending.\n");
-	return;
-}
 /*****************************************************************************/
 static unsigned int easycap_poll(struct file *file, poll_table *wait)
 {
@@ -2842,6 +2635,813 @@ static void easycap_complete(struct urb *purb)
 	return;
 }
 
+static struct easycap *alloc_easycap(u8 bInterfaceNumber)
+{
+	struct easycap *peasycap;
+	int i;
+
+	peasycap = kzalloc(sizeof(struct easycap), GFP_KERNEL);
+	if (!peasycap) {
+		SAY("ERROR: Could not allocate peasycap\n");
+		return NULL;
+	}
+
+	if (mutex_lock_interruptible(&mutex_dongle)) {
+		SAY("ERROR: cannot lock mutex_dongle\n");
+		kfree(peasycap);
+		return NULL;
+	}
+
+	/* Find a free dongle in easycapdc60_dongle array */
+	for (i = 0; i < DONGLE_MANY; i++) {
+
+		if ((!easycapdc60_dongle[i].peasycap) &&
+		    (!mutex_is_locked(&easycapdc60_dongle[i].mutex_video)) &&
+		    (!mutex_is_locked(&easycapdc60_dongle[i].mutex_audio))) {
+
+			easycapdc60_dongle[i].peasycap = peasycap;
+			peasycap->isdongle = i;
+			JOM(8, "intf[%i]: peasycap-->easycap"
+				"_dongle[%i].peasycap\n",
+				bInterfaceNumber, i);
+			break;
+		}
+	}
+
+	mutex_unlock(&mutex_dongle);
+
+	if (i >= DONGLE_MANY) {
+		SAM("ERROR: too many dongles\n");
+		kfree(peasycap);
+		return NULL;
+	}
+
+	return peasycap;
+}
+
+static void free_easycap(struct easycap *peasycap)
+{
+	int allocation_video_urb;
+	int allocation_video_page;
+	int allocation_video_struct;
+	int allocation_audio_urb;
+	int allocation_audio_page;
+	int allocation_audio_struct;
+	int registered_video, registered_audio;
+	int kd;
+
+	JOM(4, "freeing easycap structure.\n");
+	allocation_video_urb    = peasycap->allocation_video_urb;
+	allocation_video_page   = peasycap->allocation_video_page;
+	allocation_video_struct = peasycap->allocation_video_struct;
+	registered_video        = peasycap->registered_video;
+	allocation_audio_urb    = peasycap->allocation_audio_urb;
+	allocation_audio_page   = peasycap->allocation_audio_page;
+	allocation_audio_struct = peasycap->allocation_audio_struct;
+	registered_audio        = peasycap->registered_audio;
+
+	kd = easycap_isdongle(peasycap);
+	if (0 <= kd && DONGLE_MANY > kd) {
+		if (mutex_lock_interruptible(&mutex_dongle)) {
+			SAY("ERROR: cannot down mutex_dongle\n");
+		} else {
+			JOM(4, "locked mutex_dongle\n");
+			easycapdc60_dongle[kd].peasycap = NULL;
+			mutex_unlock(&mutex_dongle);
+			JOM(4, "unlocked mutex_dongle\n");
+			JOT(4, "   null-->dongle[%i].peasycap\n", kd);
+			allocation_video_struct -= sizeof(struct easycap);
+		}
+	} else {
+		SAY("ERROR: cannot purge dongle[].peasycap");
+	}
+
+	/* Free device structure */
+	kfree(peasycap);
+
+	SAY("%8i=video urbs    after all deletions\n", allocation_video_urb);
+	SAY("%8i=video pages   after all deletions\n", allocation_video_page);
+	SAY("%8i=video structs after all deletions\n", allocation_video_struct);
+	SAY("%8i=video devices after all deletions\n", registered_video);
+	SAY("%8i=audio urbs    after all deletions\n", allocation_audio_urb);
+	SAY("%8i=audio pages   after all deletions\n", allocation_audio_page);
+	SAY("%8i=audio structs after all deletions\n", allocation_audio_struct);
+	SAY("%8i=audio devices after all deletions\n", registered_audio);
+}
+
+/*
+ * FIXME: Identify the appropriate pointer peasycap for interfaces
+ * 1 and 2. The address of peasycap->pusb_device is reluctantly used
+ * for this purpose.
+ */
+static struct easycap *get_easycap(struct usb_device *usbdev,
+				   u8 bInterfaceNumber)
+{
+	int i;
+	struct easycap *peasycap;
+
+	for (i = 0; i < DONGLE_MANY; i++) {
+		if (easycapdc60_dongle[i].peasycap->pusb_device == usbdev) {
+			peasycap = easycapdc60_dongle[i].peasycap;
+			JOT(8, "intf[%i]: dongle[%i].peasycap\n",
+					bInterfaceNumber, i);
+			break;
+		}
+	}
+	if (i >= DONGLE_MANY) {
+		SAY("ERROR: peasycap is unknown when probing interface %i\n",
+			bInterfaceNumber);
+		return NULL;
+	}
+	if (!peasycap) {
+		SAY("ERROR: peasycap is NULL when probing interface %i\n",
+			bInterfaceNumber);
+		return NULL;
+	}
+
+	return peasycap;
+}
+
+static void init_easycap(struct easycap *peasycap,
+			 struct usb_device *usbdev,
+			 struct usb_interface *intf,
+			 u8 bInterfaceNumber)
+{
+	/* Save usb_device and usb_interface */
+	peasycap->pusb_device = usbdev;
+	peasycap->pusb_interface = intf;
+
+	peasycap->minor = -1;
+	kref_init(&peasycap->kref);
+	JOM(8, "intf[%i]: after kref_init(..._video) "
+		"%i=peasycap->kref.refcount.counter\n",
+		bInterfaceNumber, peasycap->kref.refcount.counter);
+
+	/* module params */
+	peasycap->gain = (s8)clamp(easycap_gain, 0, 31);
+
+	init_waitqueue_head(&peasycap->wq_video);
+	init_waitqueue_head(&peasycap->wq_audio);
+	init_waitqueue_head(&peasycap->wq_trigger);
+
+	peasycap->allocation_video_struct = sizeof(struct easycap);
+
+	peasycap->microphone = false;
+
+	peasycap->video_interface = -1;
+	peasycap->video_altsetting_on = -1;
+	peasycap->video_altsetting_off = -1;
+	peasycap->video_endpointnumber = -1;
+	peasycap->video_isoc_maxframesize = -1;
+	peasycap->video_isoc_buffer_size = -1;
+
+	peasycap->audio_interface = -1;
+	peasycap->audio_altsetting_on = -1;
+	peasycap->audio_altsetting_off = -1;
+	peasycap->audio_endpointnumber = -1;
+	peasycap->audio_isoc_maxframesize = -1;
+	peasycap->audio_isoc_buffer_size = -1;
+
+	peasycap->frame_buffer_many = FRAME_BUFFER_MANY;
+
+	peasycap->ntsc = easycap_ntsc;
+	JOM(8, "defaulting initially to %s\n",
+		easycap_ntsc ? "NTSC" : "PAL");
+}
+
+static int populate_inputset(struct easycap *peasycap)
+{
+	struct inputset *inputset;
+	struct easycap_format *peasycap_format;
+	struct v4l2_pix_format *pix;
+	int m, i, k, mask, fmtidx;
+	s32 value;
+
+	inputset = peasycap->inputset;
+
+	fmtidx = peasycap->ntsc ? NTSC_M : PAL_BGHIN;
+
+	m = 0;
+	mask = 0;
+	for (i = 0; easycap_standard[i].mask != 0xffff; i++) {
+		if (fmtidx == easycap_standard[i].v4l2_standard.index) {
+			m++;
+			for (k = 0; k < INPUT_MANY; k++)
+				inputset[k].standard_offset = i;
+			mask = easycap_standard[i].mask;
+		}
+	}
+
+	if (m != 1) {
+		SAM("ERROR: inputset->standard_offset unpopulated, %i=m\n", m);
+		return -ENOENT;
+	}
+
+	peasycap_format = &easycap_format[0];
+	m = 0;
+	for (i = 0; peasycap_format->v4l2_format.fmt.pix.width; i++) {
+		pix = &peasycap_format->v4l2_format.fmt.pix;
+		if (((peasycap_format->mask & 0x0F) == (mask & 0x0F))
+			&& pix->field == V4L2_FIELD_NONE
+			&& pix->pixelformat == V4L2_PIX_FMT_UYVY
+			&& pix->width  == 640 && pix->height == 480) {
+			m++;
+			for (k = 0; k < INPUT_MANY; k++)
+				inputset[k].format_offset = i;
+			break;
+		}
+		peasycap_format++;
+	}
+	if (m != 1) {
+		SAM("ERROR: inputset[]->format_offset unpopulated\n");
+		return -ENOENT;
+	}
+
+	m = 0;
+	for (i = 0; easycap_control[i].id != 0xffffffff; i++) {
+		value = easycap_control[i].default_value;
+		if (V4L2_CID_BRIGHTNESS == easycap_control[i].id) {
+			m++;
+			for (k = 0; k < INPUT_MANY; k++)
+				inputset[k].brightness = value;
+		} else if (V4L2_CID_CONTRAST == easycap_control[i].id) {
+			m++;
+			for (k = 0; k < INPUT_MANY; k++)
+				inputset[k].contrast = value;
+		} else if (V4L2_CID_SATURATION == easycap_control[i].id) {
+			m++;
+			for (k = 0; k < INPUT_MANY; k++)
+				inputset[k].saturation = value;
+		} else if (V4L2_CID_HUE == easycap_control[i].id) {
+			m++;
+			for (k = 0; k < INPUT_MANY; k++)
+				inputset[k].hue = value;
+		}
+	}
+
+	if (m != 4) {
+		SAM("ERROR: inputset[]->brightness underpopulated\n");
+		return -ENOENT;
+	}
+
+	for (k = 0; k < INPUT_MANY; k++)
+		inputset[k].input = k;
+	JOM(4, "populated inputset[]\n");
+
+	return 0;
+}
+
+static int alloc_framebuffers(struct easycap *peasycap)
+{
+	int i, j;
+	void *pbuf;
+
+	JOM(4, "allocating %i frame buffers of size %li\n",
+			FRAME_BUFFER_MANY, (long int)FRAME_BUFFER_SIZE);
+	JOM(4, ".... each scattered over %li pages\n",
+			FRAME_BUFFER_SIZE/PAGE_SIZE);
+
+	for (i = 0; i < FRAME_BUFFER_MANY; i++) {
+		for (j = 0; j < FRAME_BUFFER_SIZE/PAGE_SIZE; j++) {
+			if (peasycap->frame_buffer[i][j].pgo)
+				SAM("attempting to reallocate framebuffers\n");
+			else {
+				pbuf = (void *)__get_free_page(GFP_KERNEL);
+				if (!pbuf) {
+					SAM("ERROR: Could not allocate "
+					"framebuffer %i page %i\n", i, j);
+					return -ENOMEM;
+				}
+				peasycap->allocation_video_page += 1;
+				peasycap->frame_buffer[i][j].pgo = pbuf;
+			}
+			peasycap->frame_buffer[i][j].pto =
+			    peasycap->frame_buffer[i][j].pgo;
+		}
+	}
+
+	peasycap->frame_fill = 0;
+	peasycap->frame_read = 0;
+	JOM(4, "allocation of frame buffers done: %i pages\n", i*j);
+
+	return 0;
+}
+
+static void free_framebuffers(struct easycap *peasycap)
+{
+	int k, m, gone;
+
+	JOM(4, "freeing video frame buffers.\n");
+	gone = 0;
+	for (k = 0;  k < FRAME_BUFFER_MANY;  k++) {
+		for (m = 0;  m < FRAME_BUFFER_SIZE/PAGE_SIZE;  m++) {
+			if (peasycap->frame_buffer[k][m].pgo) {
+				free_page((unsigned long)
+					peasycap->frame_buffer[k][m].pgo);
+				peasycap->frame_buffer[k][m].pgo = NULL;
+				peasycap->allocation_video_page -= 1;
+				gone++;
+			}
+		}
+	}
+	JOM(4, "video frame buffers freed: %i pages\n", gone);
+}
+
+static int alloc_fieldbuffers(struct easycap *peasycap)
+{
+	int i, j;
+	void *pbuf;
+
+	JOM(4, "allocating %i field buffers of size %li\n",
+			FIELD_BUFFER_MANY, (long int)FIELD_BUFFER_SIZE);
+	JOM(4, ".... each scattered over %li pages\n",
+			FIELD_BUFFER_SIZE/PAGE_SIZE);
+
+	for (i = 0; i < FIELD_BUFFER_MANY; i++) {
+		for (j = 0; j < FIELD_BUFFER_SIZE/PAGE_SIZE; j++) {
+			if (peasycap->field_buffer[i][j].pgo) {
+				SAM("ERROR: attempting to reallocate "
+					"fieldbuffers\n");
+			} else {
+				pbuf = (void *) __get_free_page(GFP_KERNEL);
+				if (!pbuf) {
+					SAM("ERROR: Could not allocate "
+					"fieldbuffer %i page %i\n", i, j);
+					return -ENOMEM;
+				}
+				peasycap->allocation_video_page += 1;
+				peasycap->field_buffer[i][j].pgo = pbuf;
+			}
+			peasycap->field_buffer[i][j].pto =
+				peasycap->field_buffer[i][j].pgo;
+		}
+		/* TODO: Hardcoded 0x0200 meaning? */
+		peasycap->field_buffer[i][0].kount = 0x0200;
+	}
+	peasycap->field_fill = 0;
+	peasycap->field_page = 0;
+	peasycap->field_read = 0;
+	JOM(4, "allocation of field buffers done:  %i pages\n", i*j);
+
+	return 0;
+}
+
+static void free_fieldbuffers(struct easycap *peasycap)
+{
+	int k, m, gone;
+
+	JOM(4, "freeing video field buffers.\n");
+	gone = 0;
+	for (k = 0;  k < FIELD_BUFFER_MANY;  k++) {
+		for (m = 0;  m < FIELD_BUFFER_SIZE/PAGE_SIZE;  m++) {
+			if (peasycap->field_buffer[k][m].pgo) {
+				free_page((unsigned long)
+					  peasycap->field_buffer[k][m].pgo);
+				peasycap->field_buffer[k][m].pgo = NULL;
+				peasycap->allocation_video_page -= 1;
+				gone++;
+			}
+		}
+	}
+	JOM(4, "video field buffers freed: %i pages\n", gone);
+}
+
+static int alloc_isocbuffers(struct easycap *peasycap)
+{
+	int i;
+	void *pbuf;
+
+	JOM(4, "allocating %i isoc video buffers of size %i\n",
+			VIDEO_ISOC_BUFFER_MANY,
+			peasycap->video_isoc_buffer_size);
+	JOM(4, ".... each occupying contiguous memory pages\n");
+
+	for (i = 0; i < VIDEO_ISOC_BUFFER_MANY; i++) {
+		pbuf = (void *)__get_free_pages(GFP_KERNEL,
+				VIDEO_ISOC_ORDER);
+		if (!pbuf) {
+			SAM("ERROR: Could not allocate isoc "
+				"video buffer %i\n", i);
+			return -ENOMEM;
+		}
+		peasycap->allocation_video_page += BIT(VIDEO_ISOC_ORDER);
+
+		peasycap->video_isoc_buffer[i].pgo = pbuf;
+		peasycap->video_isoc_buffer[i].pto =
+			pbuf + peasycap->video_isoc_buffer_size;
+		peasycap->video_isoc_buffer[i].kount = i;
+	}
+	JOM(4, "allocation of isoc video buffers done: %i pages\n",
+			i * (0x01 << VIDEO_ISOC_ORDER));
+	return 0;
+}
+
+static void free_isocbuffers(struct easycap *peasycap)
+{
+	int k, m;
+
+	JOM(4, "freeing video isoc buffers.\n");
+	m = 0;
+	for (k = 0;  k < VIDEO_ISOC_BUFFER_MANY;  k++) {
+		if (peasycap->video_isoc_buffer[k].pgo) {
+			free_pages((unsigned long)
+				   peasycap->video_isoc_buffer[k].pgo,
+					VIDEO_ISOC_ORDER);
+			peasycap->video_isoc_buffer[k].pgo = NULL;
+			peasycap->allocation_video_page -=
+						BIT(VIDEO_ISOC_ORDER);
+			m++;
+		}
+	}
+	JOM(4, "isoc video buffers freed: %i pages\n",
+			m * (0x01 << VIDEO_ISOC_ORDER));
+}
+
+static int create_video_urbs(struct easycap *peasycap)
+{
+	struct urb *purb;
+	struct data_urb *pdata_urb;
+	int i, j;
+
+	JOM(4, "allocating %i struct urb.\n", VIDEO_ISOC_BUFFER_MANY);
+	JOM(4, "using %i=peasycap->video_isoc_framesperdesc\n",
+			peasycap->video_isoc_framesperdesc);
+	JOM(4, "using %i=peasycap->video_isoc_maxframesize\n",
+			peasycap->video_isoc_maxframesize);
+	JOM(4, "using %i=peasycap->video_isoc_buffer_sizen",
+			peasycap->video_isoc_buffer_size);
+
+	for (i = 0; i < VIDEO_ISOC_BUFFER_MANY; i++) {
+		purb = usb_alloc_urb(peasycap->video_isoc_framesperdesc,
+				GFP_KERNEL);
+		if (!purb) {
+			SAM("ERROR: usb_alloc_urb returned NULL for buffer "
+				"%i\n", i);
+			return -ENOMEM;
+		}
+
+		peasycap->allocation_video_urb += 1;
+		pdata_urb = kzalloc(sizeof(struct data_urb), GFP_KERNEL);
+		if (!pdata_urb) {
+			SAM("ERROR: Could not allocate struct data_urb.\n");
+			return -ENOMEM;
+		}
+
+		peasycap->allocation_video_struct +=
+			sizeof(struct data_urb);
+
+		pdata_urb->purb = purb;
+		pdata_urb->isbuf = i;
+		pdata_urb->length = 0;
+		list_add_tail(&(pdata_urb->list_head),
+				peasycap->purb_video_head);
+
+		if (!i) {
+			JOM(4, "initializing video urbs thus:\n");
+			JOM(4, "  purb->interval = 1;\n");
+			JOM(4, "  purb->dev = peasycap->pusb_device;\n");
+			JOM(4, "  purb->pipe = usb_rcvisocpipe"
+					"(peasycap->pusb_device,%i);\n",
+					peasycap->video_endpointnumber);
+			JOM(4, "  purb->transfer_flags = URB_ISO_ASAP;\n");
+			JOM(4, "  purb->transfer_buffer = peasycap->"
+					"video_isoc_buffer[.].pgo;\n");
+			JOM(4, "  purb->transfer_buffer_length = %i;\n",
+					peasycap->video_isoc_buffer_size);
+			JOM(4, "  purb->complete = easycap_complete;\n");
+			JOM(4, "  purb->context = peasycap;\n");
+			JOM(4, "  purb->start_frame = 0;\n");
+			JOM(4, "  purb->number_of_packets = %i;\n",
+					peasycap->video_isoc_framesperdesc);
+			JOM(4, "  for (j = 0; j < %i; j++)\n",
+					peasycap->video_isoc_framesperdesc);
+			JOM(4, "    {\n");
+			JOM(4, "    purb->iso_frame_desc[j].offset = j*%i;\n",
+					peasycap->video_isoc_maxframesize);
+			JOM(4, "    purb->iso_frame_desc[j].length = %i;\n",
+					peasycap->video_isoc_maxframesize);
+			JOM(4, "    }\n");
+		}
+
+		purb->interval = 1;
+		purb->dev = peasycap->pusb_device;
+		purb->pipe = usb_rcvisocpipe(peasycap->pusb_device,
+				peasycap->video_endpointnumber);
+
+		purb->transfer_flags = URB_ISO_ASAP;
+		purb->transfer_buffer = peasycap->video_isoc_buffer[i].pgo;
+		purb->transfer_buffer_length =
+			peasycap->video_isoc_buffer_size;
+
+		purb->complete = easycap_complete;
+		purb->context = peasycap;
+		purb->start_frame = 0;
+		purb->number_of_packets = peasycap->video_isoc_framesperdesc;
+
+		for (j = 0; j < peasycap->video_isoc_framesperdesc; j++) {
+			purb->iso_frame_desc[j].offset =
+				j * peasycap->video_isoc_maxframesize;
+			purb->iso_frame_desc[j].length =
+				peasycap->video_isoc_maxframesize;
+		}
+	}
+	JOM(4, "allocation of %i struct urb done.\n", i);
+	return 0;
+}
+
+static void free_video_urbs(struct easycap *peasycap)
+{
+	struct list_head *plist_head, *plist_next;
+	struct data_urb *pdata_urb;
+	int m;
+
+	if (peasycap->purb_video_head) {
+		m = 0;
+		list_for_each(plist_head, peasycap->purb_video_head) {
+			pdata_urb = list_entry(plist_head,
+					struct data_urb, list_head);
+			if (pdata_urb && pdata_urb->purb) {
+				usb_free_urb(pdata_urb->purb);
+				pdata_urb->purb = NULL;
+				peasycap->allocation_video_urb--;
+				m++;
+			}
+		}
+
+		JOM(4, "%i video urbs freed\n", m);
+		JOM(4, "freeing video data_urb structures.\n");
+		m = 0;
+		list_for_each_safe(plist_head, plist_next,
+					peasycap->purb_video_head) {
+			pdata_urb = list_entry(plist_head,
+					struct data_urb, list_head);
+			if (pdata_urb) {
+				peasycap->allocation_video_struct -=
+					sizeof(struct data_urb);
+				kfree(pdata_urb);
+				m++;
+			}
+		}
+		JOM(4, "%i video data_urb structures freed\n", m);
+		JOM(4, "setting peasycap->purb_video_head=NULL\n");
+		peasycap->purb_video_head = NULL;
+	}
+}
+
+static int alloc_audio_buffers(struct easycap *peasycap)
+{
+	void *pbuf;
+	int k;
+
+	JOM(4, "allocating %i isoc audio buffers of size %i\n",
+		AUDIO_ISOC_BUFFER_MANY,
+		peasycap->audio_isoc_buffer_size);
+	JOM(4, ".... each occupying contiguous memory pages\n");
+
+	for (k = 0;  k < AUDIO_ISOC_BUFFER_MANY;  k++) {
+		pbuf = (void *)__get_free_pages(GFP_KERNEL, AUDIO_ISOC_ORDER);
+		if (!pbuf) {
+			SAM("ERROR: Could not allocate isoc audio buffer %i\n",
+			    k);
+				return -ENOMEM;
+		}
+		peasycap->allocation_audio_page += BIT(AUDIO_ISOC_ORDER);
+
+		peasycap->audio_isoc_buffer[k].pgo = pbuf;
+		peasycap->audio_isoc_buffer[k].pto =
+			pbuf + peasycap->audio_isoc_buffer_size;
+		peasycap->audio_isoc_buffer[k].kount = k;
+	}
+
+	JOM(4, "allocation of isoc audio buffers done.\n");
+	return 0;
+}
+
+static void free_audio_buffers(struct easycap *peasycap)
+{
+	int k, m;
+
+	JOM(4, "freeing audio isoc buffers.\n");
+	m = 0;
+	for (k = 0;  k < AUDIO_ISOC_BUFFER_MANY;  k++) {
+		if (peasycap->audio_isoc_buffer[k].pgo) {
+			free_pages((unsigned long)
+					(peasycap->audio_isoc_buffer[k].pgo),
+					AUDIO_ISOC_ORDER);
+			peasycap->audio_isoc_buffer[k].pgo = NULL;
+			peasycap->allocation_audio_page -=
+					BIT(AUDIO_ISOC_ORDER);
+			m++;
+		}
+	}
+	JOM(4, "easyoss_delete(): isoc audio buffers freed: %i pages\n",
+					m * (0x01 << AUDIO_ISOC_ORDER));
+}
+
+static int create_audio_urbs(struct easycap *peasycap)
+{
+	struct urb *purb;
+	struct data_urb *pdata_urb;
+	int k, j;
+
+	JOM(4, "allocating %i struct urb.\n", AUDIO_ISOC_BUFFER_MANY);
+	JOM(4, "using %i=peasycap->audio_isoc_framesperdesc\n",
+		peasycap->audio_isoc_framesperdesc);
+	JOM(4, "using %i=peasycap->audio_isoc_maxframesize\n",
+		peasycap->audio_isoc_maxframesize);
+	JOM(4, "using %i=peasycap->audio_isoc_buffer_size\n",
+		peasycap->audio_isoc_buffer_size);
+
+	for (k = 0;  k < AUDIO_ISOC_BUFFER_MANY; k++) {
+		purb = usb_alloc_urb(peasycap->audio_isoc_framesperdesc,
+				     GFP_KERNEL);
+		if (!purb) {
+			SAM("ERROR: usb_alloc_urb returned NULL for buffer "
+			     "%i\n", k);
+			return -ENOMEM;
+		}
+		peasycap->allocation_audio_urb += 1 ;
+		pdata_urb = kzalloc(sizeof(struct data_urb), GFP_KERNEL);
+		if (!pdata_urb) {
+			usb_free_urb(purb);
+			SAM("ERROR: Could not allocate struct data_urb.\n");
+			return -ENOMEM;
+		}
+		peasycap->allocation_audio_struct +=
+			sizeof(struct data_urb);
+
+		pdata_urb->purb = purb;
+		pdata_urb->isbuf = k;
+		pdata_urb->length = 0;
+		list_add_tail(&(pdata_urb->list_head),
+				peasycap->purb_audio_head);
+
+		if (!k) {
+			JOM(4, "initializing audio urbs thus:\n");
+			JOM(4, "  purb->interval = 1;\n");
+			JOM(4, "  purb->dev = peasycap->pusb_device;\n");
+			JOM(4, "  purb->pipe = usb_rcvisocpipe(peasycap->"
+				"pusb_device,%i);\n",
+				peasycap->audio_endpointnumber);
+			JOM(4, "  purb->transfer_flags = URB_ISO_ASAP;\n");
+			JOM(4, "  purb->transfer_buffer = "
+				"peasycap->audio_isoc_buffer[.].pgo;\n");
+			JOM(4, "  purb->transfer_buffer_length = %i;\n",
+				peasycap->audio_isoc_buffer_size);
+			JOM(4, "  purb->complete = easycap_alsa_complete;\n");
+			JOM(4, "  purb->context = peasycap;\n");
+			JOM(4, "  purb->start_frame = 0;\n");
+			JOM(4, "  purb->number_of_packets = %i;\n",
+				peasycap->audio_isoc_framesperdesc);
+			JOM(4, "  for (j = 0; j < %i; j++)\n",
+				peasycap->audio_isoc_framesperdesc);
+			JOM(4, "    {\n");
+			JOM(4, "    purb->iso_frame_desc[j].offset = j*%i;\n",
+				peasycap->audio_isoc_maxframesize);
+			JOM(4, "    purb->iso_frame_desc[j].length = %i;\n",
+				peasycap->audio_isoc_maxframesize);
+			JOM(4, "    }\n");
+		}
+
+		purb->interval = 1;
+		purb->dev = peasycap->pusb_device;
+		purb->pipe = usb_rcvisocpipe(peasycap->pusb_device,
+					     peasycap->audio_endpointnumber);
+		purb->transfer_flags = URB_ISO_ASAP;
+		purb->transfer_buffer = peasycap->audio_isoc_buffer[k].pgo;
+		purb->transfer_buffer_length =
+			peasycap->audio_isoc_buffer_size;
+		purb->complete = easycap_alsa_complete;
+		purb->context = peasycap;
+		purb->start_frame = 0;
+		purb->number_of_packets = peasycap->audio_isoc_framesperdesc;
+		for (j = 0;  j < peasycap->audio_isoc_framesperdesc; j++) {
+			purb->iso_frame_desc[j].offset =
+				j * peasycap->audio_isoc_maxframesize;
+			purb->iso_frame_desc[j].length =
+				peasycap->audio_isoc_maxframesize;
+		}
+	}
+	JOM(4, "allocation of %i struct urb done.\n", k);
+	return 0;
+}
+
+static void free_audio_urbs(struct easycap *peasycap)
+{
+	struct list_head *plist_head, *plist_next;
+	struct data_urb *pdata_urb;
+	int m;
+
+	if (peasycap->purb_audio_head) {
+		JOM(4, "freeing audio urbs\n");
+		m = 0;
+		list_for_each(plist_head, (peasycap->purb_audio_head)) {
+			pdata_urb = list_entry(plist_head,
+					struct data_urb, list_head);
+			if (pdata_urb && pdata_urb->purb) {
+				usb_free_urb(pdata_urb->purb);
+				pdata_urb->purb = NULL;
+				peasycap->allocation_audio_urb--;
+				m++;
+			}
+		}
+		JOM(4, "%i audio urbs freed\n", m);
+		JOM(4, "freeing audio data_urb structures.\n");
+		m = 0;
+		list_for_each_safe(plist_head, plist_next,
+					peasycap->purb_audio_head) {
+			pdata_urb = list_entry(plist_head,
+					struct data_urb, list_head);
+			if (pdata_urb) {
+				peasycap->allocation_audio_struct -=
+							sizeof(struct data_urb);
+				kfree(pdata_urb);
+				m++;
+			}
+		}
+		JOM(4, "%i audio data_urb structures freed\n", m);
+		JOM(4, "setting peasycap->purb_audio_head=NULL\n");
+		peasycap->purb_audio_head = NULL;
+	}
+}
+
+static void config_easycap(struct easycap *peasycap,
+			   u8 bInterfaceNumber,
+			   u8 bInterfaceClass,
+			   u8 bInterfaceSubClass)
+{
+	if ((USB_CLASS_VIDEO == bInterfaceClass) ||
+	    (USB_CLASS_VENDOR_SPEC == bInterfaceClass)) {
+		if (-1 == peasycap->video_interface) {
+			peasycap->video_interface = bInterfaceNumber;
+			JOM(4, "setting peasycap->video_interface=%i\n",
+				peasycap->video_interface);
+		} else {
+			if (peasycap->video_interface != bInterfaceNumber) {
+				SAM("ERROR: attempting to reset "
+				    "peasycap->video_interface\n");
+				SAM("...... continuing with "
+				    "%i=peasycap->video_interface\n",
+				    peasycap->video_interface);
+			}
+		}
+	} else if ((USB_CLASS_AUDIO == bInterfaceClass) &&
+		   (USB_SUBCLASS_AUDIOSTREAMING == bInterfaceSubClass)) {
+		if (-1 == peasycap->audio_interface) {
+			peasycap->audio_interface = bInterfaceNumber;
+			JOM(4, "setting peasycap->audio_interface=%i\n",
+				peasycap->audio_interface);
+		} else {
+			if (peasycap->audio_interface != bInterfaceNumber) {
+				SAM("ERROR: attempting to reset "
+				    "peasycap->audio_interface\n");
+				SAM("...... continuing with "
+				    "%i=peasycap->audio_interface\n",
+				    peasycap->audio_interface);
+			}
+		}
+	}
+}
+
+/*
+ * This function is called from within easycap_usb_disconnect() and is
+ * protected by semaphores set and cleared by easycap_usb_disconnect().
+ * By this stage the device has already been physically unplugged,
+ * so peasycap->pusb_device is no longer valid.
+ */
+static void easycap_delete(struct kref *pkref)
+{
+	struct easycap *peasycap;
+
+	peasycap = container_of(pkref, struct easycap, kref);
+	if (!peasycap) {
+		SAM("ERROR: peasycap is NULL: cannot perform deletions\n");
+		return;
+	}
+
+	/* Free video urbs */
+	free_video_urbs(peasycap);
+
+	/* Free video isoc buffers */
+	free_isocbuffers(peasycap);
+
+	/* Free video field buffers */
+	free_fieldbuffers(peasycap);
+
+	/* Free video frame buffers */
+	free_framebuffers(peasycap);
+
+	/* Free audio urbs */
+	free_audio_urbs(peasycap);
+
+	/* Free audio isoc buffers */
+	free_audio_buffers(peasycap);
+
+	free_easycap(peasycap);
+
+	JOT(4, "ending.\n");
+}
+
 static const struct v4l2_file_operations v4l2_fops = {
 	.owner		= THIS_MODULE,
 	.open		= easycap_open_noinode,
@@ -2850,6 +3450,36 @@ static const struct v4l2_file_operations v4l2_fops = {
 	.mmap		= easycap_mmap,
 };
 
+static int easycap_register_video(struct easycap *peasycap)
+{
+	/*
+	 * FIXME: This is believed to be harmless,
+	 * but may well be unnecessary or wrong.
+	 */
+	peasycap->video_device.v4l2_dev = NULL;
+
+	strcpy(&peasycap->video_device.name[0], "easycapdc60");
+	peasycap->video_device.fops = &v4l2_fops;
+	peasycap->video_device.minor = -1;
+	peasycap->video_device.release = (void *)(&videodev_release);
+
+	video_set_drvdata(&(peasycap->video_device), (void *)peasycap);
+
+	if (0 != (video_register_device(&(peasycap->video_device),
+					VFL_TYPE_GRABBER, -1))) {
+		videodev_release(&(peasycap->video_device));
+		return -ENODEV;
+	}
+
+	peasycap->registered_video++;
+
+	SAM("registered with videodev: %i=minor\n",
+	    peasycap->video_device.minor);
+	    peasycap->minor = peasycap->video_device.minor;
+
+	return 0;
+}
+
 /*
  * When the device is plugged, this function is called three times,
  * one for each interface.
@@ -2861,24 +3491,15 @@ static int easycap_usb_probe(struct usb_interface *intf,
 	struct usb_host_interface *alt;
 	struct usb_endpoint_descriptor *ep;
 	struct usb_interface_descriptor *interface;
-	struct urb *purb;
 	struct easycap *peasycap;
-	int ndong;
-	struct data_urb *pdata_urb;
-	int i, j, k, m, rc;
+	int i, j, rc;
 	u8 bInterfaceNumber;
 	u8 bInterfaceClass;
 	u8 bInterfaceSubClass;
-	void *pbuf;
 	int okalt[8], isokalt;
 	int okepn[8];
 	int okmps[8];
 	int maxpacketsize;
-	u16 mask;
-	s32 value;
-	struct easycap_format *peasycap_format;
-	int fmtidx;
-	struct inputset *inputset;
 
 	usbdev = interface_to_usbdev(intf);
 
@@ -2916,76 +3537,16 @@ static int easycap_usb_probe(struct usb_interface *intf,
 	 * interfaces 1 and 2 are probed.
 	 */
 	if (0 == bInterfaceNumber) {
-		peasycap = kzalloc(sizeof(struct easycap), GFP_KERNEL);
-		if (!peasycap) {
-			SAY("ERROR: Could not allocate peasycap\n");
-			return -ENOMEM;
-		}
-
-		/* Perform urgent initializations */
-		peasycap->minor = -1;
-		kref_init(&peasycap->kref);
-		JOM(8, "intf[%i]: after kref_init(..._video) "
-				"%i=peasycap->kref.refcount.counter\n",
-				bInterfaceNumber, peasycap->kref.refcount.counter);
-
-		/* module params */
-		peasycap->gain = (s8)clamp(easycap_gain, 0, 31);
-
-		init_waitqueue_head(&peasycap->wq_video);
-		init_waitqueue_head(&peasycap->wq_audio);
-		init_waitqueue_head(&peasycap->wq_trigger);
-
-		if (mutex_lock_interruptible(&mutex_dongle)) {
-			SAY("ERROR: cannot down mutex_dongle\n");
-			return -ERESTARTSYS;
-		}
-
-		for (ndong = 0; ndong < DONGLE_MANY; ndong++) {
-			if ((!easycapdc60_dongle[ndong].peasycap) &&
-					(!mutex_is_locked(&easycapdc60_dongle
-						[ndong].mutex_video)) &&
-					(!mutex_is_locked(&easycapdc60_dongle
-						[ndong].mutex_audio))) {
-				easycapdc60_dongle[ndong].peasycap = peasycap;
-				peasycap->isdongle = ndong;
-				JOM(8, "intf[%i]: peasycap-->easycap"
-						"_dongle[%i].peasycap\n",
-						bInterfaceNumber, ndong);
-				break;
-			}
-		}
-
-		if (DONGLE_MANY <= ndong) {
-			SAM("ERROR: too many dongles\n");
-			mutex_unlock(&mutex_dongle);
+		/*
+		 * Alloc structure and save it in a free slot in
+		 * easycapdc60_dongle array
+		 */
+		peasycap = alloc_easycap(bInterfaceNumber);
+		if (!peasycap)
 			return -ENOMEM;
-		}
-		mutex_unlock(&mutex_dongle);
-
-		peasycap->allocation_video_struct = sizeof(struct easycap);
-
-		/* and further initialize the structure */
-		peasycap->pusb_device = usbdev;
-		peasycap->pusb_interface = intf;
 
-		peasycap->microphone = false;
-
-		peasycap->video_interface = -1;
-		peasycap->video_altsetting_on = -1;
-		peasycap->video_altsetting_off = -1;
-		peasycap->video_endpointnumber = -1;
-		peasycap->video_isoc_maxframesize = -1;
-		peasycap->video_isoc_buffer_size = -1;
-
-		peasycap->audio_interface = -1;
-		peasycap->audio_altsetting_on = -1;
-		peasycap->audio_altsetting_off = -1;
-		peasycap->audio_endpointnumber = -1;
-		peasycap->audio_isoc_maxframesize = -1;
-		peasycap->audio_isoc_buffer_size = -1;
-
-		peasycap->frame_buffer_many = FRAME_BUFFER_MANY;
+		/* Perform basic struct initialization */
+		init_easycap(peasycap, usbdev, intf, bInterfaceNumber);
 
 		/* Dynamically fill in the available formats */
 		rc = easycap_video_fillin_formats();
@@ -2996,136 +3557,19 @@ static int easycap_usb_probe(struct usb_interface *intf,
 		JOM(4, "%i formats available\n", rc);
 
 		/* Populate easycap.inputset[] */
-		inputset = peasycap->inputset;
-		fmtidx = peasycap->ntsc ? NTSC_M : PAL_BGHIN;
-		m = 0;
-		mask = 0;
-		for (i = 0; 0xFFFF != easycap_standard[i].mask; i++) {
-			if (fmtidx == easycap_standard[i].v4l2_standard.index) {
-				m++;
-				for (k = 0; k < INPUT_MANY; k++)
-					inputset[k].standard_offset = i;
-
-				mask = easycap_standard[i].mask;
-			}
-		}
-		if (1 != m) {
-			SAM("ERROR: "
-			    "inputset->standard_offset unpopulated, %i=m\n", m);
-			return -ENOENT;
-		}
-
-		peasycap_format = &easycap_format[0];
-		m = 0;
-		for (i = 0; peasycap_format->v4l2_format.fmt.pix.width; i++) {
-			struct v4l2_pix_format *pix =
-				&peasycap_format->v4l2_format.fmt.pix;
-			if (((peasycap_format->mask & 0x0F) == (mask & 0x0F)) &&
-			    pix->field == V4L2_FIELD_NONE &&
-			    pix->pixelformat == V4L2_PIX_FMT_UYVY &&
-			    pix->width  == 640 && pix->height == 480) {
-				m++;
-				for (k = 0; k < INPUT_MANY; k++)
-					inputset[k].format_offset = i;
-				break;
-			}
-			peasycap_format++;
-		}
-		if (1 != m) {
-			SAM("ERROR: inputset[]->format_offset unpopulated\n");
-			return -ENOENT;
-		}
-
-		m = 0;
-		for (i = 0; 0xFFFFFFFF != easycap_control[i].id; i++) {
-			value = easycap_control[i].default_value;
-			if (V4L2_CID_BRIGHTNESS == easycap_control[i].id) {
-				m++;
-				for (k = 0; k < INPUT_MANY; k++)
-					inputset[k].brightness = value;
-			} else if (V4L2_CID_CONTRAST == easycap_control[i].id) {
-				m++;
-				for (k = 0; k < INPUT_MANY; k++)
-					inputset[k].contrast = value;
-			} else if (V4L2_CID_SATURATION == easycap_control[i].id) {
-				m++;
-				for (k = 0; k < INPUT_MANY; k++)
-					inputset[k].saturation = value;
-			} else if (V4L2_CID_HUE == easycap_control[i].id) {
-				m++;
-				for (k = 0; k < INPUT_MANY; k++)
-					inputset[k].hue = value;
-			}
-		}
-
-		if (4 != m) {
-			SAM("ERROR: inputset[]->brightness underpopulated\n");
-			return -ENOENT;
-		}
-		for (k = 0; k < INPUT_MANY; k++)
-			inputset[k].input = k;
-		JOM(4, "populated inputset[]\n");
+		rc = populate_inputset(peasycap);
+		if (rc < 0)
+			return rc;
 		JOM(4, "finished initialization\n");
 	} else {
-
-		/*
-		 * FIXME: Identify the appropriate pointer
-		 * peasycap for interfaces 1 and 2.
-		 * The address of peasycap->pusb_device
-		 * is reluctantly used for this purpose.
-		 */
-		for (ndong = 0; ndong < DONGLE_MANY; ndong++) {
-			if (usbdev == easycapdc60_dongle[ndong].peasycap->
-									pusb_device) {
-				peasycap = easycapdc60_dongle[ndong].peasycap;
-				JOT(8, "intf[%i]: dongle[%i].peasycap\n",
-						bInterfaceNumber, ndong);
-				break;
-			}
-		}
-		if (DONGLE_MANY <= ndong) {
-			SAY("ERROR: peasycap is unknown when probing interface %i\n",
-								bInterfaceNumber);
+		peasycap = get_easycap(usbdev, bInterfaceNumber);
+		if (!peasycap)
 			return -ENODEV;
-		}
-		if (!peasycap) {
-			SAY("ERROR: peasycap is NULL when probing interface %i\n",
-								bInterfaceNumber);
-			return -ENODEV;
-		}
 	}
 
-	if ((USB_CLASS_VIDEO == bInterfaceClass) ||
-	    (USB_CLASS_VENDOR_SPEC == bInterfaceClass)) {
-		if (-1 == peasycap->video_interface) {
-			peasycap->video_interface = bInterfaceNumber;
-			JOM(4, "setting peasycap->video_interface=%i\n",
-							peasycap->video_interface);
-		} else {
-			if (peasycap->video_interface != bInterfaceNumber) {
-				SAM("ERROR: attempting to reset "
-						"peasycap->video_interface\n");
-				SAM("...... continuing with "
-						"%i=peasycap->video_interface\n",
-						peasycap->video_interface);
-			}
-		}
-	} else if ((USB_CLASS_AUDIO == bInterfaceClass) &&
-		   (USB_SUBCLASS_AUDIOSTREAMING == bInterfaceSubClass)) {
-		if (-1 == peasycap->audio_interface) {
-			peasycap->audio_interface = bInterfaceNumber;
-			JOM(4, "setting peasycap->audio_interface=%i\n",
-							 peasycap->audio_interface);
-		} else {
-			if (peasycap->audio_interface != bInterfaceNumber) {
-				SAM("ERROR: attempting to reset "
-						"peasycap->audio_interface\n");
-				SAM("...... continuing with "
-						"%i=peasycap->audio_interface\n",
-						peasycap->audio_interface);
-			}
-		}
-	}
+	config_easycap(peasycap, bInterfaceNumber,
+				 bInterfaceClass,
+				 bInterfaceSubClass);
 
 	/*
 	 * Investigate all altsettings. This is done in detail
@@ -3368,173 +3812,23 @@ static int easycap_usb_probe(struct usb_interface *intf,
 		 */
 		INIT_LIST_HEAD(&(peasycap->urb_video_head));
 		peasycap->purb_video_head = &(peasycap->urb_video_head);
-		JOM(4, "allocating %i frame buffers of size %li\n",
-				FRAME_BUFFER_MANY, (long int)FRAME_BUFFER_SIZE);
-		JOM(4, ".... each scattered over %li pages\n",
-							FRAME_BUFFER_SIZE/PAGE_SIZE);
-
-		for (k = 0;  k < FRAME_BUFFER_MANY;  k++) {
-			for (m = 0;  m < FRAME_BUFFER_SIZE/PAGE_SIZE;  m++) {
-				if (peasycap->frame_buffer[k][m].pgo)
-					SAM("attempting to reallocate frame "
-									" buffers\n");
-				else {
-					pbuf = (void *)__get_free_page(GFP_KERNEL);
-					if (!pbuf) {
-						SAM("ERROR: Could not allocate frame "
-							"buffer %i page %i\n", k, m);
-						return -ENOMEM;
-					}
-
-					peasycap->allocation_video_page += 1;
-					peasycap->frame_buffer[k][m].pgo = pbuf;
-				}
-				peasycap->frame_buffer[k][m].pto =
-						peasycap->frame_buffer[k][m].pgo;
-			}
-		}
-
-		peasycap->frame_fill = 0;
-		peasycap->frame_read = 0;
-		JOM(4, "allocation of frame buffers done:  %i pages\n", k *
-									m);
-		JOM(4, "allocating %i field buffers of size %li\n",
-				FIELD_BUFFER_MANY, (long int)FIELD_BUFFER_SIZE);
-		JOM(4, ".... each scattered over %li pages\n",
-						FIELD_BUFFER_SIZE/PAGE_SIZE);
-
-		for (k = 0;  k < FIELD_BUFFER_MANY;  k++) {
-			for (m = 0;  m < FIELD_BUFFER_SIZE/PAGE_SIZE;  m++) {
-				if (peasycap->field_buffer[k][m].pgo) {
-					SAM("ERROR: attempting to reallocate "
-								"field buffers\n");
-				} else {
-					pbuf = (void *) __get_free_page(GFP_KERNEL);
-					if (!pbuf) {
-						SAM("ERROR: Could not allocate field"
-							" buffer %i page %i\n", k, m);
-						return -ENOMEM;
-					}
-
-					peasycap->allocation_video_page += 1;
-					peasycap->field_buffer[k][m].pgo = pbuf;
-				}
-				peasycap->field_buffer[k][m].pto =
-						peasycap->field_buffer[k][m].pgo;
-			}
-			peasycap->field_buffer[k][0].kount = 0x0200;
-		}
-		peasycap->field_fill = 0;
-		peasycap->field_page = 0;
-		peasycap->field_read = 0;
-		JOM(4, "allocation of field buffers done:  %i pages\n", k *
-									m);
-		JOM(4, "allocating %i isoc video buffers of size %i\n",
-						VIDEO_ISOC_BUFFER_MANY,
-						peasycap->video_isoc_buffer_size);
-		JOM(4, ".... each occupying contiguous memory pages\n");
-
-		for (k = 0;  k < VIDEO_ISOC_BUFFER_MANY; k++) {
-			pbuf = (void *)__get_free_pages(GFP_KERNEL,
-							VIDEO_ISOC_ORDER);
-			if (!pbuf) {
-				SAM("ERROR: Could not allocate isoc video buffer "
-									"%i\n", k);
-				return -ENOMEM;
-			}
-			peasycap->allocation_video_page +=
-						BIT(VIDEO_ISOC_ORDER);
 
-			peasycap->video_isoc_buffer[k].pgo = pbuf;
-			peasycap->video_isoc_buffer[k].pto =
-				pbuf + peasycap->video_isoc_buffer_size;
-			peasycap->video_isoc_buffer[k].kount = k;
-		}
-		JOM(4, "allocation of isoc video buffers done: %i pages\n",
-						k * (0x01 << VIDEO_ISOC_ORDER));
-
-		/* Allocate and initialize multiple struct usb */
-		JOM(4, "allocating %i struct urb.\n", VIDEO_ISOC_BUFFER_MANY);
-		JOM(4, "using %i=peasycap->video_isoc_framesperdesc\n",
-						peasycap->video_isoc_framesperdesc);
-		JOM(4, "using %i=peasycap->video_isoc_maxframesize\n",
-						peasycap->video_isoc_maxframesize);
-		JOM(4, "using %i=peasycap->video_isoc_buffer_sizen",
-						peasycap->video_isoc_buffer_size);
-
-		for (k = 0;  k < VIDEO_ISOC_BUFFER_MANY; k++) {
-			purb = usb_alloc_urb(peasycap->video_isoc_framesperdesc,
-									GFP_KERNEL);
-			if (!purb) {
-				SAM("ERROR: usb_alloc_urb returned NULL for buffer "
-									"%i\n", k);
-				return -ENOMEM;
-			}
+		rc = alloc_framebuffers(peasycap);
+		if (rc < 0)
+			return rc;
 
-			peasycap->allocation_video_urb += 1;
-			pdata_urb = kzalloc(sizeof(struct data_urb), GFP_KERNEL);
-			if (!pdata_urb) {
-				SAM("ERROR: Could not allocate struct data_urb.\n");
-				return -ENOMEM;
-			}
+		rc = alloc_fieldbuffers(peasycap);
+		if (rc < 0)
+			return rc;
 
-			peasycap->allocation_video_struct +=
-							sizeof(struct data_urb);
+		rc = alloc_isocbuffers(peasycap);
+		if (rc < 0)
+			return rc;
 
-			pdata_urb->purb = purb;
-			pdata_urb->isbuf = k;
-			pdata_urb->length = 0;
-			list_add_tail(&(pdata_urb->list_head),
-							peasycap->purb_video_head);
-
-			/* Initialize allocated urbs */
-			if (!k) {
-				JOM(4, "initializing video urbs thus:\n");
-				JOM(4, "  purb->interval = 1;\n");
-				JOM(4, "  purb->dev = peasycap->pusb_device;\n");
-				JOM(4, "  purb->pipe = usb_rcvisocpipe"
-						"(peasycap->pusb_device,%i);\n",
-						peasycap->video_endpointnumber);
-				JOM(4, "  purb->transfer_flags = URB_ISO_ASAP;\n");
-				JOM(4, "  purb->transfer_buffer = peasycap->"
-						"video_isoc_buffer[.].pgo;\n");
-				JOM(4, "  purb->transfer_buffer_length = %i;\n",
-						peasycap->video_isoc_buffer_size);
-				JOM(4, "  purb->complete = easycap_complete;\n");
-				JOM(4, "  purb->context = peasycap;\n");
-				JOM(4, "  purb->start_frame = 0;\n");
-				JOM(4, "  purb->number_of_packets = %i;\n",
-						peasycap->video_isoc_framesperdesc);
-				JOM(4, "  for (j = 0; j < %i; j++)\n",
-						peasycap->video_isoc_framesperdesc);
-				JOM(4, "    {\n");
-				JOM(4, "    purb->iso_frame_desc[j].offset = j*%i;\n",
-						peasycap->video_isoc_maxframesize);
-				JOM(4, "    purb->iso_frame_desc[j].length = %i;\n",
-						peasycap->video_isoc_maxframesize);
-				JOM(4, "    }\n");
-			}
-
-			purb->interval = 1;
-			purb->dev = peasycap->pusb_device;
-			purb->pipe = usb_rcvisocpipe(peasycap->pusb_device,
-						peasycap->video_endpointnumber);
-			purb->transfer_flags = URB_ISO_ASAP;
-			purb->transfer_buffer = peasycap->video_isoc_buffer[k].pgo;
-			purb->transfer_buffer_length =
-						peasycap->video_isoc_buffer_size;
-			purb->complete = easycap_complete;
-			purb->context = peasycap;
-			purb->start_frame = 0;
-			purb->number_of_packets = peasycap->video_isoc_framesperdesc;
-			for (j = 0;  j < peasycap->video_isoc_framesperdesc; j++) {
-				purb->iso_frame_desc[j].offset = j *
-						peasycap->video_isoc_maxframesize;
-				purb->iso_frame_desc[j].length =
-						peasycap->video_isoc_maxframesize;
-			}
-		}
-		JOM(4, "allocation of %i struct urb done.\n", k);
+		/* Allocate and initialize video urbs */
+		rc = create_video_urbs(peasycap);
+		if (rc < 0)
+			return rc;
 
 		/* Save pointer peasycap in this interface */
 		usb_set_intfdata(intf, peasycap);
@@ -3545,9 +3839,6 @@ static int easycap_usb_probe(struct usb_interface *intf,
 		 * because some udev rules triggers easycap_open()
 		 * immediately after registration, causing a clash.
 		 */
-		peasycap->ntsc = easycap_ntsc;
-		JOM(8, "defaulting initially to %s\n",
-			easycap_ntsc ? "NTSC" : "PAL");
 		rc = reset(peasycap);
 		if (rc) {
 			SAM("ERROR: reset() rc = %i\n", rc);
@@ -3562,32 +3853,12 @@ static int easycap_usb_probe(struct usb_interface *intf,
 		JOM(4, "registered device instance: %s\n",
 			peasycap->v4l2_device.name);
 
-		/*
-		 * FIXME: This is believed to be harmless,
-		 * but may well be unnecessary or wrong.
-		 */
-		peasycap->video_device.v4l2_dev = NULL;
-
-
-		strcpy(&peasycap->video_device.name[0], "easycapdc60");
-		peasycap->video_device.fops = &v4l2_fops;
-		peasycap->video_device.minor = -1;
-		peasycap->video_device.release = (void *)(&videodev_release);
-
-		video_set_drvdata(&(peasycap->video_device), (void *)peasycap);
-
-		if (0 != (video_register_device(&(peasycap->video_device),
-							VFL_TYPE_GRABBER, -1))) {
-			err("Not able to register with videodev");
-			videodev_release(&(peasycap->video_device));
+		rc = easycap_register_video(peasycap);
+		if (rc < 0) {
+			dev_err(&intf->dev,
+				"Not able to register with videodev\n");
 			return -ENODEV;
 		}
-
-		peasycap->registered_video++;
-		SAM("registered with videodev: %i=minor\n",
-						peasycap->video_device.minor);
-		peasycap->minor = peasycap->video_device.minor;
-
 		break;
 	}
 	/* 1: Audio control */
@@ -3710,109 +3981,14 @@ static int easycap_usb_probe(struct usb_interface *intf,
 		INIT_LIST_HEAD(&(peasycap->urb_audio_head));
 		peasycap->purb_audio_head = &(peasycap->urb_audio_head);
 
-		JOM(4, "allocating %i isoc audio buffers of size %i\n",
-			AUDIO_ISOC_BUFFER_MANY,
-			peasycap->audio_isoc_buffer_size);
-		JOM(4, ".... each occupying contiguous memory pages\n");
-
-		for (k = 0;  k < AUDIO_ISOC_BUFFER_MANY;  k++) {
-			pbuf = (void *)__get_free_pages(GFP_KERNEL,
-							AUDIO_ISOC_ORDER);
-			if (!pbuf) {
-				SAM("ERROR: Could not allocate isoc audio buffer "
-								"%i\n", k);
-				return -ENOMEM;
-			}
-			peasycap->allocation_audio_page +=
-						BIT(AUDIO_ISOC_ORDER);
-
-			peasycap->audio_isoc_buffer[k].pgo = pbuf;
-			peasycap->audio_isoc_buffer[k].pto = pbuf +
-			peasycap->audio_isoc_buffer_size;
-			peasycap->audio_isoc_buffer[k].kount = k;
-		}
-		JOM(4, "allocation of isoc audio buffers done.\n");
+		alloc_audio_buffers(peasycap);
+		if (rc < 0)
+			return rc;
 
 		/* Allocate and initialize urbs */
-		JOM(4, "allocating %i struct urb.\n", AUDIO_ISOC_BUFFER_MANY);
-		JOM(4, "using %i=peasycap->audio_isoc_framesperdesc\n",
-					peasycap->audio_isoc_framesperdesc);
-		JOM(4, "using %i=peasycap->audio_isoc_maxframesize\n",
-					peasycap->audio_isoc_maxframesize);
-		JOM(4, "using %i=peasycap->audio_isoc_buffer_size\n",
-					peasycap->audio_isoc_buffer_size);
-
-		for (k = 0;  k < AUDIO_ISOC_BUFFER_MANY; k++) {
-			purb = usb_alloc_urb(peasycap->audio_isoc_framesperdesc,
-								GFP_KERNEL);
-			if (!purb) {
-				SAM("ERROR: usb_alloc_urb returned NULL for buffer "
-								"%i\n", k);
-				return -ENOMEM;
-			}
-			peasycap->allocation_audio_urb += 1 ;
-			pdata_urb = kzalloc(sizeof(struct data_urb), GFP_KERNEL);
-			if (!pdata_urb) {
-				usb_free_urb(purb);
-				SAM("ERROR: Could not allocate struct data_urb.\n");
-				return -ENOMEM;
-			}
-			peasycap->allocation_audio_struct +=
-						sizeof(struct data_urb);
-
-			pdata_urb->purb = purb;
-			pdata_urb->isbuf = k;
-			pdata_urb->length = 0;
-			list_add_tail(&(pdata_urb->list_head),
-							peasycap->purb_audio_head);
-
-			if (!k) {
-				JOM(4, "initializing audio urbs thus:\n");
-				JOM(4, "  purb->interval = 1;\n");
-				JOM(4, "  purb->dev = peasycap->pusb_device;\n");
-				JOM(4, "  purb->pipe = usb_rcvisocpipe(peasycap->"
-						"pusb_device,%i);\n",
-						peasycap->audio_endpointnumber);
-				JOM(4, "  purb->transfer_flags = URB_ISO_ASAP;\n");
-				JOM(4, "  purb->transfer_buffer = "
-					"peasycap->audio_isoc_buffer[.].pgo;\n");
-				JOM(4, "  purb->transfer_buffer_length = %i;\n",
-					peasycap->audio_isoc_buffer_size);
-				JOM(4, "  purb->complete = easycap_alsa_complete;\n");
-				JOM(4, "  purb->context = peasycap;\n");
-				JOM(4, "  purb->start_frame = 0;\n");
-				JOM(4, "  purb->number_of_packets = %i;\n",
-						peasycap->audio_isoc_framesperdesc);
-				JOM(4, "  for (j = 0; j < %i; j++)\n",
-						peasycap->audio_isoc_framesperdesc);
-				JOM(4, "    {\n");
-				JOM(4, "    purb->iso_frame_desc[j].offset = j*%i;\n",
-					peasycap->audio_isoc_maxframesize);
-				JOM(4, "    purb->iso_frame_desc[j].length = %i;\n",
-					peasycap->audio_isoc_maxframesize);
-				JOM(4, "    }\n");
-			}
-
-			purb->interval = 1;
-			purb->dev = peasycap->pusb_device;
-			purb->pipe = usb_rcvisocpipe(peasycap->pusb_device,
-						peasycap->audio_endpointnumber);
-			purb->transfer_flags = URB_ISO_ASAP;
-			purb->transfer_buffer = peasycap->audio_isoc_buffer[k].pgo;
-			purb->transfer_buffer_length =
-						peasycap->audio_isoc_buffer_size;
-			purb->complete = easycap_alsa_complete;
-			purb->context = peasycap;
-			purb->start_frame = 0;
-			purb->number_of_packets = peasycap->audio_isoc_framesperdesc;
-			for (j = 0;  j < peasycap->audio_isoc_framesperdesc; j++) {
-				purb->iso_frame_desc[j].offset = j *
-						peasycap->audio_isoc_maxframesize;
-				purb->iso_frame_desc[j].length =
-						peasycap->audio_isoc_maxframesize;
-			}
-		}
-		JOM(4, "allocation of %i struct urb done.\n", k);
+		rc = create_audio_urbs(peasycap);
+		if (rc < 0)
+			return rc;
 
 		/* Save pointer peasycap in this interface */
 		usb_set_intfdata(intf, peasycap);
@@ -3822,7 +3998,8 @@ static int easycap_usb_probe(struct usb_interface *intf,
 
 		rc = easycap_alsa_probe(peasycap);
 		if (rc) {
-			err("easycap_alsa_probe() rc = %i\n", rc);
+			dev_err(&intf->dev, "easycap_alsa_probe() rc = %i\n",
+				rc);
 			return -ENODEV;
 		}
 
@@ -3841,15 +4018,13 @@ static int easycap_usb_probe(struct usb_interface *intf,
 	SAM("ends successfully for interface %i\n", bInterfaceNumber);
 	return 0;
 }
-/*****************************************************************************/
-/*---------------------------------------------------------------------------*/
+
 /*
- *  WHEN THIS FUNCTION IS CALLED THE EasyCAP HAS ALREADY BEEN PHYSICALLY
- *  UNPLUGGED.  HENCE peasycap->pusb_device IS NO LONGER VALID.
- *
- *  THIS FUNCTION AFFECTS ALSA.  BEWARE.
+ * When this function is called the device has already been
+ * physically unplugged.
+ * Hence, peasycap->pusb_device is no longer valid.
+ * This function affects alsa.
  */
-/*---------------------------------------------------------------------------*/
 static void easycap_usb_disconnect(struct usb_interface *pusb_interface)
 {
 	struct usb_host_interface *pusb_host_interface;
@@ -3874,6 +4049,7 @@ static void easycap_usb_disconnect(struct usb_interface *pusb_interface)
 	minor = pusb_interface->minor;
 	JOT(4, "intf[%i]: minor=%i\n", bInterfaceNumber, minor);
 
+	/* There is nothing to do for Interface Number 1 */
 	if (1 == bInterfaceNumber)
 		return;
 
@@ -3882,11 +4058,8 @@ static void easycap_usb_disconnect(struct usb_interface *pusb_interface)
 		SAY("ERROR: peasycap is NULL\n");
 		return;
 	}
-/*---------------------------------------------------------------------------*/
-/*
- *  IF THE WAIT QUEUES ARE NOT CLEARED A DEADLOCK IS POSSIBLE.  BEWARE.
-*/
-/*---------------------------------------------------------------------------*/
+
+	/* If the waitqueues are not cleared a deadlock is possible */
 	peasycap->video_eof = 1;
 	peasycap->audio_eof = 1;
 	wake_up_interruptible(&(peasycap->wq_video));
@@ -3902,15 +4075,14 @@ static void easycap_usb_disconnect(struct usb_interface *pusb_interface)
 	default:
 		break;
 	}
-/*--------------------------------------------------------------------------*/
-/*
- *  DEREGISTER
- *
- *  THIS PROCEDURE WILL BLOCK UNTIL easycap_poll(), VIDEO IOCTL AND AUDIO
- *  IOCTL ARE ALL UNLOCKED.  IF THIS IS NOT DONE AN Oops CAN OCCUR WHEN
- *  AN EasyCAP IS UNPLUGGED WHILE THE URBS ARE RUNNING.  BEWARE.
- */
-/*--------------------------------------------------------------------------*/
+
+	/*
+	 * Deregister
+	 * This procedure will block until easycap_poll(),
+	 * video and audio ioctl are all unlocked.
+	 * If this is not done an oops can occur when an easycap
+	 * is unplugged while the urbs are running.
+	 */
 	kd = easycap_isdongle(peasycap);
 	switch (bInterfaceNumber) {
 	case 0: {
@@ -3927,7 +4099,6 @@ static void easycap_usb_disconnect(struct usb_interface *pusb_interface)
 		} else {
 			SAY("ERROR: %i=kd is bad: cannot lock dongle\n", kd);
 		}
-/*---------------------------------------------------------------------------*/
 		if (!peasycap->v4l2_device.name[0]) {
 			SAM("ERROR: peasycap->v4l2_device.name is empty\n");
 			if (0 <= kd && DONGLE_MANY > kd)
@@ -3943,7 +4114,6 @@ static void easycap_usb_disconnect(struct usb_interface *pusb_interface)
 		JOM(4, "intf[%i]: video_unregister_device() minor=%i\n",
 				bInterfaceNumber, minor);
 		peasycap->registered_video--;
-/*^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^*/
 
 		if (0 <= kd && DONGLE_MANY > kd) {
 			mutex_unlock(&easycapdc60_dongle[kd].mutex_video);
@@ -3979,12 +4149,12 @@ static void easycap_usb_disconnect(struct usb_interface *pusb_interface)
 	default:
 		break;
 	}
-/*---------------------------------------------------------------------------*/
-/*
- *  CALL easycap_delete() IF NO REMAINING REFERENCES TO peasycap
- *  (ALSO WHEN ALSA HAS BEEN IN USE)
- */
-/*---------------------------------------------------------------------------*/
+
+	/*
+	 * If no remaining references to peasycap,
+	 * call easycap_delete.
+	 * (Also when alsa has been in use)
+	 */
 	if (!peasycap->kref.refcount.counter) {
 		SAM("ERROR: peasycap->kref.refcount.counter is zero "
 							"so cannot call kref_put()\n");
@@ -4019,17 +4189,11 @@ static void easycap_usb_disconnect(struct usb_interface *pusb_interface)
 		mutex_unlock(&easycapdc60_dongle[kd].mutex_video);
 		JOT(4, "unlocked dongle[%i].mutex_video\n", kd);
 	}
-/*---------------------------------------------------------------------------*/
 	JOM(4, "ends\n");
 	return;
 }
-/*****************************************************************************/
 
-/*---------------------------------------------------------------------------*/
-/*
- *  PARAMETERS APPLICABLE TO ENTIRE DRIVER, I.E. BOTH VIDEO AND AUDIO
- */
-/*---------------------------------------------------------------------------*/
+/* Devices supported by this driver */
 static struct usb_device_id easycap_usb_device_id_table[] = {
 	{USB_DEVICE(USB_EASYCAP_VENDOR_ID, USB_EASYCAP_PRODUCT_ID)},
 	{ }
@@ -4064,14 +4228,11 @@ static int __init easycap_module_init(void)
 
 	return rc;
 }
-/*****************************************************************************/
+
 static void __exit easycap_module_exit(void)
 {
 	usb_deregister(&easycap_usb_driver);
 }
-/*****************************************************************************/
 
 module_init(easycap_module_init);
 module_exit(easycap_module_exit);
-
-/*****************************************************************************/
diff --git a/drivers/staging/media/go7007/README b/drivers/staging/media/go7007/README
index 48f44763781..aeba1324a9c 100644
--- a/drivers/staging/media/go7007/README
+++ b/drivers/staging/media/go7007/README
@@ -6,6 +6,6 @@ Todo:
 	- testing?
 	- handle churn in v4l layer.
 
-Please send patchs to Greg Kroah-Hartman <greg@kroah.com> and Cc: Ross
+Please send patches to Greg Kroah-Hartman <greg@linuxfoundation.org> and Cc: Ross
 Cohen <rcohen@snurgle.org> as well.
 
diff --git a/drivers/staging/media/go7007/go7007-v4l2.c b/drivers/staging/media/go7007/go7007-v4l2.c
index 3ef4cd8b4de..c184ad30fbd 100644
--- a/drivers/staging/media/go7007/go7007-v4l2.c
+++ b/drivers/staging/media/go7007/go7007-v4l2.c
@@ -76,7 +76,6 @@ static void abort_queued(struct go7007 *go)
 
 static int go7007_streamoff(struct go7007 *go)
 {
-	int retval = -EINVAL;
 	unsigned long flags;
 
 	mutex_lock(&go->hw_lock);
@@ -87,7 +86,6 @@ static int go7007_streamoff(struct go7007 *go)
 		abort_queued(go);
 		spin_unlock_irqrestore(&go->spinlock, flags);
 		go7007_reset_encoder(go);
-		retval = 0;
 	}
 	mutex_unlock(&go->hw_lock);
 	return 0;
diff --git a/drivers/staging/media/go7007/go7007.txt b/drivers/staging/media/go7007/go7007.txt
index 9db1f3952fd..fcb3e235abb 100644
--- a/drivers/staging/media/go7007/go7007.txt
+++ b/drivers/staging/media/go7007/go7007.txt
@@ -87,7 +87,6 @@ kernel as built-in or modules:
 	CONFIG_SOUND             - Sound card support
 	CONFIG_SND               - Advanced Linux Sound Architecture
 	CONFIG_USB               - Support for Host-side USB
-	CONFIG_USB_DEVICEFS      - USB device filesystem
 	CONFIG_USB_EHCI_HCD      - EHCI HCD (USB 2.0) support
 
 Additionally, to use the example application, the following options need to
diff --git a/drivers/staging/media/go7007/s2250-loader.c b/drivers/staging/media/go7007/s2250-loader.c
index 4e132519e25..f1bd159ac19 100644
--- a/drivers/staging/media/go7007/s2250-loader.c
+++ b/drivers/staging/media/go7007/s2250-loader.c
@@ -160,32 +160,10 @@ static struct usb_driver s2250loader_driver = {
 	.id_table	= s2250loader_ids,
 };
 
-static int __init s2250loader_init(void)
-{
-	int r;
-	unsigned i = 0;
-
-	for (i = 0; i < MAX_DEVICES; i++)
-		s2250_dev_table[i] = NULL;
-
-	r = usb_register(&s2250loader_driver);
-	if (r) {
-		printk(KERN_ERR "usb_register failed. Error number %d\n", r);
-		return -1;
-	}
-
-	printk(KERN_INFO "s2250loader_init: driver registered\n");
-	return 0;
-}
-module_init(s2250loader_init);
-
-static void __exit s2250loader_cleanup(void)
-{
-	printk(KERN_INFO "s2250loader_cleanup\n");
-	usb_deregister(&s2250loader_driver);
-}
-module_exit(s2250loader_cleanup);
+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/lirc/lirc_imon.c b/drivers/staging/media/lirc/lirc_imon.c
index 5f7f8cd3a66..2944fde89f4 100644
--- a/drivers/staging/media/lirc/lirc_imon.c
+++ b/drivers/staging/media/lirc/lirc_imon.c
@@ -70,10 +70,6 @@ static ssize_t vfd_write(struct file *file, const char __user *buf,
 static int ir_open(void *data);
 static void ir_close(void *data);
 
-/* Driver init/exit prototypes */
-static int __init imon_init(void);
-static void __exit imon_exit(void);
-
 /*** G L O B A L S ***/
 #define IMON_DATA_BUF_SZ	35
 
@@ -209,8 +205,9 @@ static void deregister_from_lirc(struct imon_context *context)
 
 	retval = lirc_unregister_driver(minor);
 	if (retval)
-		err("%s: unable to deregister from lirc(%d)",
-			__func__, retval);
+		printk(KERN_ERR KBUILD_MODNAME
+		       ": %s: unable to deregister from lirc(%d)",
+		       __func__, retval);
 	else
 		printk(KERN_INFO MOD_NAME ": Deregistered iMON driver "
 		       "(minor:%d)\n", minor);
@@ -234,16 +231,18 @@ static int display_open(struct inode *inode, struct file *file)
 	subminor = iminor(inode);
 	interface = usb_find_interface(&imon_driver, subminor);
 	if (!interface) {
-		err("%s: could not find interface for minor %d",
-		    __func__, subminor);
+		printk(KERN_ERR KBUILD_MODNAME
+		       ": %s: could not find interface for minor %d\n",
+		       __func__, subminor);
 		retval = -ENODEV;
 		goto exit;
 	}
 	context = usb_get_intfdata(interface);
 
 	if (!context) {
-		err("%s: no context found for minor %d",
-					__func__, subminor);
+		dev_err(&interface->dev,
+			"%s: no context found for minor %d\n",
+			__func__, subminor);
 		retval = -ENODEV;
 		goto exit;
 	}
@@ -251,10 +250,12 @@ static int display_open(struct inode *inode, struct file *file)
 	mutex_lock(&context->ctx_lock);
 
 	if (!context->display) {
-		err("%s: display not supported by device", __func__);
+		dev_err(&interface->dev,
+			"%s: display not supported by device\n", __func__);
 		retval = -ENODEV;
 	} else if (context->display_isopen) {
-		err("%s: display port is already open", __func__);
+		dev_err(&interface->dev,
+			"%s: display port is already open\n", __func__);
 		retval = -EBUSY;
 	} else {
 		context->display_isopen = 1;
@@ -281,17 +282,20 @@ static int display_close(struct inode *inode, struct file *file)
 	context = file->private_data;
 
 	if (!context) {
-		err("%s: no context for device", __func__);
+		printk(KERN_ERR KBUILD_MODNAME
+		       "%s: no context for device\n", __func__);
 		return -ENODEV;
 	}
 
 	mutex_lock(&context->ctx_lock);
 
 	if (!context->display) {
-		err("%s: display not supported by device", __func__);
+		dev_err(&context->usbdev->dev,
+			"%s: display not supported by device\n", __func__);
 		retval = -ENODEV;
 	} else if (!context->display_isopen) {
-		err("%s: display is not open", __func__);
+		dev_err(&context->usbdev->dev,
+			"%s: display is not open\n", __func__);
 		retval = -EIO;
 	} else {
 		context->display_isopen = 0;
@@ -340,19 +344,23 @@ static int send_packet(struct imon_context *context)
 	retval = usb_submit_urb(context->tx_urb, GFP_KERNEL);
 	if (retval) {
 		atomic_set(&(context->tx.busy), 0);
-		err("%s: error submitting urb(%d)", __func__, retval);
+		dev_err(&context->usbdev->dev,
+			"%s: error submitting urb(%d)\n", __func__, retval);
 	} else {
 		/* Wait for transmission to complete (or abort) */
 		mutex_unlock(&context->ctx_lock);
 		retval = wait_for_completion_interruptible(
 				&context->tx.finished);
 		if (retval)
-			err("%s: task interrupted", __func__);
+			dev_err(&context->usbdev->dev,
+				"%s: task interrupted\n", __func__);
 		mutex_lock(&context->ctx_lock);
 
 		retval = context->tx.status;
 		if (retval)
-			err("%s: packet tx failed (%d)", __func__, retval);
+			dev_err(&context->usbdev->dev,
+				"%s: packet tx failed (%d)\n",
+				__func__, retval);
 	}
 
 	return retval;
@@ -383,20 +391,23 @@ static ssize_t vfd_write(struct file *file, const char __user *buf,
 
 	context = file->private_data;
 	if (!context) {
-		err("%s: no context for device", __func__);
+		printk(KERN_ERR KBUILD_MODNAME
+		       "%s: no context for device\n", __func__);
 		return -ENODEV;
 	}
 
 	mutex_lock(&context->ctx_lock);
 
 	if (!context->dev_present) {
-		err("%s: no iMON device present", __func__);
+		dev_err(&context->usbdev->dev,
+			"%s: no iMON device present\n", __func__);
 		retval = -ENODEV;
 		goto exit;
 	}
 
 	if (n_bytes <= 0 || n_bytes > IMON_DATA_BUF_SZ - 3) {
-		err("%s: invalid payload size", __func__);
+		dev_err(&context->usbdev->dev,
+			"%s: invalid payload size\n", __func__);
 		retval = -EINVAL;
 		goto exit;
 	}
@@ -425,8 +436,9 @@ static ssize_t vfd_write(struct file *file, const char __user *buf,
 
 		retval = send_packet(context);
 		if (retval) {
-			err("%s: send packet failed for packet #%d",
-					__func__, seq/2);
+			dev_err(&context->usbdev->dev,
+				"%s: send packet failed for packet #%d\n",
+				__func__, seq/2);
 			goto exit;
 		} else {
 			seq += 2;
@@ -441,7 +453,8 @@ static ssize_t vfd_write(struct file *file, const char __user *buf,
 		context->usb_tx_buf[7] = (unsigned char) seq;
 		retval = send_packet(context);
 		if (retval)
-			err("%s: send packet failed for packet #%d",
+			dev_err(&context->usbdev->dev,
+				"%s: send packet failed for packet #%d\n",
 					__func__, seq/2);
 	}
 
@@ -508,7 +521,8 @@ static void ir_close(void *data)
 
 	context = (struct imon_context *)data;
 	if (!context) {
-		err("%s: no context for device", __func__);
+		printk(KERN_ERR KBUILD_MODNAME
+		       "%s: no context for device\n", __func__);
 		return;
 	}
 
@@ -732,7 +746,7 @@ static int imon_probe(struct usb_interface *interface,
 
 	context = kzalloc(sizeof(struct imon_context), GFP_KERNEL);
 	if (!context) {
-		err("%s: kzalloc failed for context", __func__);
+		dev_err(dev, "%s: kzalloc failed for context\n", __func__);
 		alloc_status = 1;
 		goto alloc_status_switch;
 	}
@@ -797,7 +811,7 @@ static int imon_probe(struct usb_interface *interface,
 
 	/* Input endpoint is mandatory */
 	if (!ir_ep_found) {
-		err("%s: no valid input (IR) endpoint found.", __func__);
+		dev_err(dev, "%s: no valid input (IR) endpoint found.\n", __func__);
 		retval = -ENODEV;
 		alloc_status = 2;
 		goto alloc_status_switch;
@@ -814,30 +828,30 @@ static int imon_probe(struct usb_interface *interface,
 
 	driver = kzalloc(sizeof(struct lirc_driver), GFP_KERNEL);
 	if (!driver) {
-		err("%s: kzalloc failed for lirc_driver", __func__);
+		dev_err(dev, "%s: kzalloc failed for lirc_driver\n", __func__);
 		alloc_status = 2;
 		goto alloc_status_switch;
 	}
 	rbuf = kmalloc(sizeof(struct lirc_buffer), GFP_KERNEL);
 	if (!rbuf) {
-		err("%s: kmalloc failed for lirc_buffer", __func__);
+		dev_err(dev, "%s: kmalloc failed for lirc_buffer\n", __func__);
 		alloc_status = 3;
 		goto alloc_status_switch;
 	}
 	if (lirc_buffer_init(rbuf, BUF_CHUNK_SIZE, BUF_SIZE)) {
-		err("%s: lirc_buffer_init failed", __func__);
+		dev_err(dev, "%s: lirc_buffer_init failed\n", __func__);
 		alloc_status = 4;
 		goto alloc_status_switch;
 	}
 	rx_urb = usb_alloc_urb(0, GFP_KERNEL);
 	if (!rx_urb) {
-		err("%s: usb_alloc_urb failed for IR urb", __func__);
+		dev_err(dev, "%s: usb_alloc_urb failed for IR urb\n", __func__);
 		alloc_status = 5;
 		goto alloc_status_switch;
 	}
 	tx_urb = usb_alloc_urb(0, GFP_KERNEL);
 	if (!tx_urb) {
-		err("%s: usb_alloc_urb failed for display urb",
+		dev_err(dev, "%s: usb_alloc_urb failed for display urb\n",
 		    __func__);
 		alloc_status = 6;
 		goto alloc_status_switch;
@@ -865,7 +879,7 @@ static int imon_probe(struct usb_interface *interface,
 
 	lirc_minor = lirc_register_driver(driver);
 	if (lirc_minor < 0) {
-		err("%s: lirc_register_driver failed", __func__);
+		dev_err(dev, "%s: lirc_register_driver failed\n", __func__);
 		alloc_status = 7;
 		goto unlock;
 	} else
@@ -900,8 +914,8 @@ static int imon_probe(struct usb_interface *interface,
 	retval = usb_submit_urb(context->rx_urb, GFP_KERNEL);
 
 	if (retval) {
-		err("%s: usb_submit_urb failed for intf0 (%d)",
-		    __func__, retval);
+		dev_err(dev, "%s: usb_submit_urb failed for intf0 (%d)\n",
+			__func__, retval);
 		mutex_unlock(&context->ctx_lock);
 		goto exit;
 	}
diff --git a/drivers/staging/media/lirc/lirc_sasem.c b/drivers/staging/media/lirc/lirc_sasem.c
index 74421043b95..f4e4d9003f3 100644
--- a/drivers/staging/media/lirc/lirc_sasem.c
+++ b/drivers/staging/media/lirc/lirc_sasem.c
@@ -80,10 +80,6 @@ static ssize_t vfd_write(struct file *file, const char *buf,
 static int ir_open(void *data);
 static void ir_close(void *data);
 
-/* Driver init/exit prototypes */
-static int __init sasem_init(void);
-static void __exit sasem_exit(void);
-
 /*** G L O B A L S ***/
 #define SASEM_DATA_BUF_SZ	32
 
@@ -185,7 +181,7 @@ static void deregister_from_lirc(struct sasem_context *context)
 
 	retval = lirc_unregister_driver(minor);
 	if (retval)
-		err("%s: unable to deregister from lirc (%d)",
+		printk(KERN_ERR "%s: unable to deregister from lirc (%d)\n",
 			__func__, retval);
 	else
 		printk(KERN_INFO "Deregistered Sasem driver (minor:%d)\n",
@@ -210,16 +206,18 @@ static int vfd_open(struct inode *inode, struct file *file)
 	subminor = iminor(inode);
 	interface = usb_find_interface(&sasem_driver, subminor);
 	if (!interface) {
-		err("%s: could not find interface for minor %d",
-		    __func__, subminor);
+		printk(KERN_ERR KBUILD_MODNAME
+		       ": %s: could not find interface for minor %d\n",
+		       __func__, subminor);
 		retval = -ENODEV;
 		goto exit;
 	}
 	context = usb_get_intfdata(interface);
 
 	if (!context) {
-		err("%s: no context found for minor %d",
-					__func__, subminor);
+		dev_err(&interface->dev,
+			"%s: no context found for minor %d\n",
+			__func__, subminor);
 		retval = -ENODEV;
 		goto exit;
 	}
@@ -227,12 +225,13 @@ static int vfd_open(struct inode *inode, struct file *file)
 	mutex_lock(&context->ctx_lock);
 
 	if (context->vfd_isopen) {
-		err("%s: VFD port is already open", __func__);
+		dev_err(&interface->dev,
+			"%s: VFD port is already open", __func__);
 		retval = -EBUSY;
 	} else {
 		context->vfd_isopen = 1;
 		file->private_data = context;
-		printk(KERN_INFO "VFD port opened\n");
+		dev_info(&interface->dev, "VFD port opened\n");
 	}
 
 	mutex_unlock(&context->ctx_lock);
@@ -253,7 +252,8 @@ static long vfd_ioctl(struct file *file, unsigned cmd, unsigned long arg)
 	context = (struct sasem_context *) file->private_data;
 
 	if (!context) {
-		err("%s: no context for device", __func__);
+		printk(KERN_ERR KBUILD_MODNAME
+		       ": %s: no context for device\n", __func__);
 		return -ENODEV;
 	}
 
@@ -287,14 +287,15 @@ static int vfd_close(struct inode *inode, struct file *file)
 	context = (struct sasem_context *) file->private_data;
 
 	if (!context) {
-		err("%s: no context for device", __func__);
+		printk(KERN_ERR KBUILD_MODNAME
+		       ": %s: no context for device\n", __func__);
 		return -ENODEV;
 	}
 
 	mutex_lock(&context->ctx_lock);
 
 	if (!context->vfd_isopen) {
-		err("%s: VFD is not open", __func__);
+		dev_err(&context->dev->dev, "%s: VFD is not open\n", __func__);
 		retval = -EIO;
 	} else {
 		context->vfd_isopen = 0;
@@ -339,7 +340,8 @@ static int send_packet(struct sasem_context *context)
 	retval =  usb_submit_urb(context->tx_urb, GFP_KERNEL);
 	if (retval) {
 		atomic_set(&(context->tx.busy), 0);
-		err("%s: error submitting urb (%d)", __func__, retval);
+		dev_err(&context->dev->dev, "%s: error submitting urb (%d)\n",
+			__func__, retval);
 	} else {
 		/* Wait for transmission to complete (or abort) */
 		mutex_unlock(&context->ctx_lock);
@@ -348,7 +350,9 @@ static int send_packet(struct sasem_context *context)
 
 		retval = context->tx.status;
 		if (retval)
-			err("%s: packet tx failed (%d)", __func__, retval);
+			dev_err(&context->dev->dev,
+				"%s: packet tx failed (%d)\n",
+				__func__, retval);
 	}
 
 	return retval;
@@ -369,20 +373,23 @@ static ssize_t vfd_write(struct file *file, const char *buf,
 
 	context = (struct sasem_context *) file->private_data;
 	if (!context) {
-		err("%s: no context for device", __func__);
+		printk(KERN_ERR KBUILD_MODNAME
+		       ": %s: no context for device\n", __func__);
 		return -ENODEV;
 	}
 
 	mutex_lock(&context->ctx_lock);
 
 	if (!context->dev_present) {
-		err("%s: no Sasem device present", __func__);
+		printk(KERN_ERR KBUILD_MODNAME
+		       ": %s: no Sasem device present\n", __func__);
 		retval = -ENODEV;
 		goto exit;
 	}
 
 	if (n_bytes <= 0 || n_bytes > SASEM_DATA_BUF_SZ) {
-		err("%s: invalid payload size", __func__);
+		dev_err(&context->dev->dev, "%s: invalid payload size\n",
+			__func__);
 		retval = -EINVAL;
 		goto exit;
 	}
@@ -440,9 +447,9 @@ static ssize_t vfd_write(struct file *file, const char *buf,
 		}
 		retval = send_packet(context);
 		if (retval) {
-
-			err("%s: send packet failed for packet #%d",
-					__func__, i);
+			dev_err(&context->dev->dev,
+				"%s: send packet failed for packet #%d\n",
+				__func__, i);
 			goto exit;
 		}
 	}
@@ -492,7 +499,8 @@ static int ir_open(void *data)
 	mutex_lock(&context->ctx_lock);
 
 	if (context->ir_isopen) {
-		err("%s: IR port is already open", __func__);
+		dev_err(&context->dev->dev, "%s: IR port is already open\n",
+			__func__);
 		retval = -EBUSY;
 		goto exit;
 	}
@@ -506,8 +514,9 @@ static int ir_open(void *data)
 	retval = usb_submit_urb(context->rx_urb, GFP_KERNEL);
 
 	if (retval)
-		err("%s: usb_submit_urb failed for ir_open (%d)",
-		    __func__, retval);
+		dev_err(&context->dev->dev,
+			"%s: usb_submit_urb failed for ir_open (%d)\n",
+			__func__, retval);
 	else {
 		context->ir_isopen = 1;
 		printk(KERN_INFO "IR port opened\n");
@@ -529,7 +538,8 @@ static void ir_close(void *data)
 
 	context = (struct sasem_context *)data;
 	if (!context) {
-		err("%s: no context for device", __func__);
+		printk(KERN_ERR KBUILD_MODNAME
+		       ": %s: no context for device\n", __func__);
 		return;
 	}
 
@@ -687,7 +697,7 @@ static int sasem_probe(struct usb_interface *interface,
 	struct sasem_context *context = NULL;
 	int i;
 
-	printk(KERN_INFO "%s: found Sasem device\n", __func__);
+	dev_info(&interface->dev, "%s: found Sasem device\n", __func__);
 
 
 	dev = usb_get_dev(interface_to_usbdev(interface));
@@ -719,8 +729,8 @@ static int sasem_probe(struct usb_interface *interface,
 			rx_endpoint = ep;
 			ir_ep_found = 1;
 			if (debug)
-				printk(KERN_INFO "%s: found IR endpoint\n",
-				       __func__);
+				dev_info(&interface->dev,
+					"%s: found IR endpoint\n", __func__);
 
 		} else if (!vfd_ep_found &&
 			ep_dir == USB_DIR_OUT &&
@@ -729,22 +739,23 @@ static int sasem_probe(struct usb_interface *interface,
 			tx_endpoint = ep;
 			vfd_ep_found = 1;
 			if (debug)
-				printk(KERN_INFO "%s: found VFD endpoint\n",
-				       __func__);
+				dev_info(&interface->dev,
+					"%s: found VFD endpoint\n", __func__);
 		}
 	}
 
 	/* Input endpoint is mandatory */
 	if (!ir_ep_found) {
-
-		err("%s: no valid input (IR) endpoint found.", __func__);
+		dev_err(&interface->dev,
+			"%s: no valid input (IR) endpoint found.\n", __func__);
 		retval = -ENODEV;
 		goto exit;
 	}
 
 	if (!vfd_ep_found)
-		printk(KERN_INFO "%s: no valid output (VFD) endpoint found.\n",
-		       __func__);
+		dev_info(&interface->dev,
+			"%s: no valid output (VFD) endpoint found.\n",
+			__func__);
 
 
 	/* Allocate memory */
@@ -752,38 +763,44 @@ static int sasem_probe(struct usb_interface *interface,
 
 	context = kzalloc(sizeof(struct sasem_context), GFP_KERNEL);
 	if (!context) {
-		err("%s: kzalloc failed for context", __func__);
+		dev_err(&interface->dev,
+			"%s: kzalloc failed for context\n", __func__);
 		alloc_status = 1;
 		goto alloc_status_switch;
 	}
 	driver = kzalloc(sizeof(struct lirc_driver), GFP_KERNEL);
 	if (!driver) {
-		err("%s: kzalloc failed for lirc_driver", __func__);
+		dev_err(&interface->dev,
+			"%s: kzalloc failed for lirc_driver\n", __func__);
 		alloc_status = 2;
 		goto alloc_status_switch;
 	}
 	rbuf = kmalloc(sizeof(struct lirc_buffer), GFP_KERNEL);
 	if (!rbuf) {
-		err("%s: kmalloc failed for lirc_buffer", __func__);
+		dev_err(&interface->dev,
+			"%s: kmalloc failed for lirc_buffer\n", __func__);
 		alloc_status = 3;
 		goto alloc_status_switch;
 	}
 	if (lirc_buffer_init(rbuf, BUF_CHUNK_SIZE, BUF_SIZE)) {
-		err("%s: lirc_buffer_init failed", __func__);
+		dev_err(&interface->dev,
+			"%s: lirc_buffer_init failed\n", __func__);
 		alloc_status = 4;
 		goto alloc_status_switch;
 	}
 	rx_urb = usb_alloc_urb(0, GFP_KERNEL);
 	if (!rx_urb) {
-		err("%s: usb_alloc_urb failed for IR urb", __func__);
+		dev_err(&interface->dev,
+			"%s: usb_alloc_urb failed for IR urb\n", __func__);
 		alloc_status = 5;
 		goto alloc_status_switch;
 	}
 	if (vfd_ep_found) {
 		tx_urb = usb_alloc_urb(0, GFP_KERNEL);
 		if (!tx_urb) {
-			err("%s: usb_alloc_urb failed for VFD urb",
-			    __func__);
+			dev_err(&interface->dev,
+				"%s: usb_alloc_urb failed for VFD urb",
+				__func__);
 			alloc_status = 6;
 			goto alloc_status_switch;
 		}
@@ -807,7 +824,8 @@ static int sasem_probe(struct usb_interface *interface,
 
 	lirc_minor = lirc_register_driver(driver);
 	if (lirc_minor < 0) {
-		err("%s: lirc_register_driver failed", __func__);
+		dev_err(&interface->dev,
+			"%s: lirc_register_driver failed\n", __func__);
 		alloc_status = 7;
 		retval = lirc_minor;
 		goto unlock;
diff --git a/drivers/staging/media/lirc/lirc_ttusbir.c b/drivers/staging/media/lirc/lirc_ttusbir.c
index 7950887ff11..3bb865c0217 100644
--- a/drivers/staging/media/lirc/lirc_ttusbir.c
+++ b/drivers/staging/media/lirc/lirc_ttusbir.c
@@ -113,8 +113,9 @@ static int set_use_inc(void *data)
 	for (i = 0; i < num_urbs; i++) {
 		retval = usb_submit_urb(ttusbir->urb[i], GFP_KERNEL);
 		if (retval) {
-			err("%s: usb_submit_urb failed on urb %d",
-			    __func__, i);
+			dev_err(&ttusbir->interf->dev,
+				"%s: usb_submit_urb failed on urb %d\n",
+				__func__, i);
 			return retval;
 		}
 	}
@@ -278,7 +279,7 @@ static int probe(struct usb_interface *intf, const struct usb_device_id *id)
 	if (ttusbir->alt_setting != -1)
 		DPRINTK("alt setting: %d\n", ttusbir->alt_setting);
 	else {
-		err("Could not find alternate setting\n");
+		dev_err(&intf->dev, "Could not find alternate setting\n");
 		kfree(ttusbir);
 		return -EINVAL;
 	}
@@ -291,7 +292,7 @@ static int probe(struct usb_interface *intf, const struct usb_device_id *id)
 
 	/* Register as a LIRC driver */
 	if (lirc_buffer_init(&ttusbir->rbuf, sizeof(int), 256) < 0) {
-		err("Could not get memory for LIRC data buffer\n");
+		dev_err(&intf->dev, "Could not get memory for LIRC data buffer\n");
 		usb_set_intfdata(intf, NULL);
 		kfree(ttusbir);
 		return -ENOMEM;
@@ -310,7 +311,7 @@ static int probe(struct usb_interface *intf, const struct usb_device_id *id)
 	ttusbir->driver.features = LIRC_CAN_REC_MODE2;
 	ttusbir->minor = lirc_register_driver(&ttusbir->driver);
 	if (ttusbir->minor < 0) {
-		err("Error registering as LIRC driver\n");
+		dev_err(&intf->dev, "Error registering as LIRC driver\n");
 		usb_set_intfdata(intf, NULL);
 		lirc_buffer_free(&ttusbir->rbuf);
 		kfree(ttusbir);
@@ -321,7 +322,7 @@ static int probe(struct usb_interface *intf, const struct usb_device_id *id)
 	for (i = 0; i < num_urbs; i++) {
 		ttusbir->urb[i] = usb_alloc_urb(8, GFP_KERNEL);
 		if (!ttusbir->urb[i]) {
-			err("Could not allocate memory for the URB\n");
+			dev_err(&intf->dev, "Could not allocate memory for the URB\n");
 			for (j = i - 1; j >= 0; j--)
 				kfree(ttusbir->urb[j]);
 			lirc_buffer_free(&ttusbir->rbuf);
diff --git a/drivers/staging/mei/TODO b/drivers/staging/mei/TODO
deleted file mode 100644
index fc266018355..00000000000
--- a/drivers/staging/mei/TODO
+++ /dev/null
@@ -1,10 +0,0 @@
-TODO:
-	- Cleanup and split the timer function
-Upon Unstaging:
-	- move mei.h to include/linux/mei.h
-	- Documentation/ioctl/ioctl-number.txt
-	- move mei.txt under Documentation/mei/
-	- move mei-amt-version.c under Documentation/mei
-	- add hostprogs-y for mei-amt-version.c
-	- drop mei_version.h
-	- Updated MAINTAINERS
diff --git a/drivers/staging/mei/mei-amt-version.c b/drivers/staging/mei/mei-amt-version.c
deleted file mode 100644
index ac2a507be25..00000000000
--- a/drivers/staging/mei/mei-amt-version.c
+++ /dev/null
@@ -1,481 +0,0 @@
-/******************************************************************************
- * Intel Management Engine Interface (Intel MEI) Linux driver
- * Intel MEI Interface Header
- *
- * 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) 2012 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 Corporation.
- *	linux-mei@linux.intel.com
- *	http://www.intel.com
- *
- * BSD LICENSE
- *
- * Copyright(c) 2003 - 2012 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 <stdio.h>
-#include <stdlib.h>
-#include <string.h>
-#include <fcntl.h>
-#include <sys/ioctl.h>
-#include <unistd.h>
-#include <errno.h>
-#include <stdint.h>
-#include <stdbool.h>
-#include <bits/wordsize.h>
-#include "mei.h"
-
-/*****************************************************************************
- * Intel Management Engine Interface
- *****************************************************************************/
-
-#define mei_msg(_me, fmt, ARGS...) do {         \
-	if (_me->verbose)                       \
-		fprintf(stderr, fmt, ##ARGS);	\
-} while (0)
-
-#define mei_err(_me, fmt, ARGS...) do {         \
-	fprintf(stderr, "Error: " fmt, ##ARGS); \
-} while (0)
-
-struct mei {
-	uuid_le guid;
-	bool initialized;
-	bool verbose;
-	unsigned int buf_size;
-	unsigned char prot_ver;
-	int fd;
-};
-
-static void mei_deinit(struct mei *cl)
-{
-	if (cl->fd != -1)
-		close(cl->fd);
-	cl->fd = -1;
-	cl->buf_size = 0;
-	cl->prot_ver = 0;
-	cl->initialized = false;
-}
-
-static bool mei_init(struct mei *me, const uuid_le *guid,
-		unsigned char req_protocol_version, bool verbose)
-{
-	int result;
-	struct mei_client *cl;
-	struct mei_connect_client_data data;
-
-	mei_deinit(me);
-
-	me->verbose = verbose;
-
-	me->fd = open("/dev/mei", O_RDWR);
-	if (me->fd == -1) {
-		mei_err(me, "Cannot establish a handle to the Intel MEI driver\n");
-		goto err;
-	}
-	memcpy(&me->guid, guid, sizeof(*guid));
-	memset(&data, 0, sizeof(data));
-	me->initialized = true;
-
-	memcpy(&data.in_client_uuid, &me->guid, sizeof(me->guid));
-	result = ioctl(me->fd, IOCTL_MEI_CONNECT_CLIENT, &data);
-	if (result) {
-		mei_err(me, "IOCTL_MEI_CONNECT_CLIENT receive message. err=%d\n", result);
-		goto err;
-	}
-	cl = &data.out_client_properties;
-	mei_msg(me, "max_message_length %d\n", cl->max_msg_length);
-	mei_msg(me, "protocol_version %d\n", cl->protocol_version);
-
-	if ((req_protocol_version > 0) &&
-	     (cl->protocol_version != req_protocol_version)) {
-		mei_err(me, "Intel MEI protocol version not supported\n");
-		goto err;
-	}
-
-	me->buf_size = cl->max_msg_length;
-	me->prot_ver = cl->protocol_version;
-
-	return true;
-err:
-	mei_deinit(me);
-	return false;
-}
-
-static ssize_t mei_recv_msg(struct mei *me, unsigned char *buffer,
-			ssize_t len, unsigned long timeout)
-{
-	ssize_t rc;
-
-	mei_msg(me, "call read length = %zd\n", len);
-
-	rc = read(me->fd, buffer, len);
-	if (rc < 0) {
-		mei_err(me, "read failed with status %zd %s\n",
-				rc, strerror(errno));
-		mei_deinit(me);
-	} else {
-		mei_msg(me, "read succeeded with result %zd\n", rc);
-	}
-	return rc;
-}
-
-static ssize_t mei_send_msg(struct mei *me, const unsigned char *buffer,
-			ssize_t len, unsigned long timeout)
-{
-	struct timeval tv;
-	ssize_t written;
-	ssize_t rc;
-	fd_set set;
-
-	tv.tv_sec = timeout / 1000;
-	tv.tv_usec = (timeout % 1000) * 1000000;
-
-	mei_msg(me, "call write length = %zd\n", len);
-
-	written = write(me->fd, buffer, len);
-	if (written < 0) {
-		rc = -errno;
-		mei_err(me, "write failed with status %zd %s\n",
-			written, strerror(errno));
-		goto out;
-	}
-
-	FD_ZERO(&set);
-	FD_SET(me->fd, &set);
-	rc = select(me->fd + 1 , &set, NULL, NULL, &tv);
-	if (rc > 0 && FD_ISSET(me->fd, &set)) {
-		mei_msg(me, "write success\n");
-	} else if (rc == 0) {
-		mei_err(me, "write failed on timeout with status\n");
-		goto out;
-	} else { /* rc < 0 */
-		mei_err(me, "write failed on select with status %zd\n", rc);
-		goto out;
-	}
-
-	rc = written;
-out:
-	if (rc < 0)
-		mei_deinit(me);
-
-	return rc;
-}
-
-/***************************************************************************
- * Intel Advanced Management Technolgy ME Client
- ***************************************************************************/
-
-#define AMT_MAJOR_VERSION 1
-#define AMT_MINOR_VERSION 1
-
-#define AMT_STATUS_SUCCESS                0x0
-#define AMT_STATUS_INTERNAL_ERROR         0x1
-#define AMT_STATUS_NOT_READY              0x2
-#define AMT_STATUS_INVALID_AMT_MODE       0x3
-#define AMT_STATUS_INVALID_MESSAGE_LENGTH 0x4
-
-#define AMT_STATUS_HOST_IF_EMPTY_RESPONSE  0x4000
-#define AMT_STATUS_SDK_RESOURCES      0x1004
-
-
-#define AMT_BIOS_VERSION_LEN   65
-#define AMT_VERSIONS_NUMBER    50
-#define AMT_UNICODE_STRING_LEN 20
-
-struct amt_unicode_string {
-	uint16_t length;
-	char string[AMT_UNICODE_STRING_LEN];
-} __attribute__((packed));
-
-struct amt_version_type {
-	struct amt_unicode_string description;
-	struct amt_unicode_string version;
-} __attribute__((packed));
-
-struct amt_version {
-	uint8_t major;
-	uint8_t minor;
-} __attribute__((packed));
-
-struct amt_code_versions {
-	uint8_t bios[AMT_BIOS_VERSION_LEN];
-	uint32_t count;
-	struct amt_version_type versions[AMT_VERSIONS_NUMBER];
-} __attribute__((packed));
-
-/***************************************************************************
- * Intel Advanced Management Technolgy Host Interface
- ***************************************************************************/
-
-struct amt_host_if_msg_header {
-	struct amt_version version;
-	uint16_t _reserved;
-	uint32_t command;
-	uint32_t length;
-} __attribute__((packed));
-
-struct amt_host_if_resp_header {
-	struct amt_host_if_msg_header header;
-	uint32_t status;
-	unsigned char data[0];
-} __attribute__((packed));
-
-const uuid_le MEI_IAMTHIF = UUID_LE(0x12f80028, 0xb4b7, 0x4b2d,  \
-				0xac, 0xa8, 0x46, 0xe0, 0xff, 0x65, 0x81, 0x4c);
-
-#define AMT_HOST_IF_CODE_VERSIONS_REQUEST  0x0400001A
-#define AMT_HOST_IF_CODE_VERSIONS_RESPONSE 0x0480001A
-
-const struct amt_host_if_msg_header CODE_VERSION_REQ = {
-	.version = {AMT_MAJOR_VERSION, AMT_MINOR_VERSION},
-	._reserved = 0,
-	.command = AMT_HOST_IF_CODE_VERSIONS_REQUEST,
-	.length = 0
-};
-
-
-struct amt_host_if {
-	struct mei mei_cl;
-	unsigned long send_timeout;
-	bool initialized;
-};
-
-
-static bool amt_host_if_init(struct amt_host_if *acmd,
-		      unsigned long send_timeout, bool verbose)
-{
-	acmd->send_timeout = (send_timeout) ? send_timeout : 20000;
-	acmd->initialized = mei_init(&acmd->mei_cl, &MEI_IAMTHIF, 0, verbose);
-	return acmd->initialized;
-}
-
-static void amt_host_if_deinit(struct amt_host_if *acmd)
-{
-	mei_deinit(&acmd->mei_cl);
-	acmd->initialized = false;
-}
-
-static uint32_t amt_verify_code_versions(const struct amt_host_if_resp_header *resp)
-{
-	uint32_t status = AMT_STATUS_SUCCESS;
-	struct amt_code_versions *code_ver;
-	size_t code_ver_len;
-	uint32_t ver_type_cnt;
-	uint32_t len;
-	uint32_t i;
-
-	code_ver = (struct amt_code_versions *)resp->data;
-	/* length - sizeof(status) */
-	code_ver_len = resp->header.length - sizeof(uint32_t);
-	ver_type_cnt = code_ver_len -
-			sizeof(code_ver->bios) -
-			sizeof(code_ver->count);
-	if (code_ver->count != ver_type_cnt / sizeof(struct amt_version_type)) {
-		status = AMT_STATUS_INTERNAL_ERROR;
-		goto out;
-	}
-
-	for (i = 0; i < code_ver->count; i++) {
-		len = code_ver->versions[i].description.length;
-
-		if (len > AMT_UNICODE_STRING_LEN) {
-			status = AMT_STATUS_INTERNAL_ERROR;
-			goto out;
-		}
-
-		len = code_ver->versions[i].version.length;
-		if (code_ver->versions[i].version.string[len] != '\0' ||
-		    len != strlen(code_ver->versions[i].version.string)) {
-			status = AMT_STATUS_INTERNAL_ERROR;
-			goto out;
-		}
-	}
-out:
-	return status;
-}
-
-static uint32_t amt_verify_response_header(uint32_t command,
-				const struct amt_host_if_msg_header *resp_hdr,
-				uint32_t response_size)
-{
-	if (response_size < sizeof(struct amt_host_if_resp_header)) {
-		return AMT_STATUS_INTERNAL_ERROR;
-	} else if (response_size != (resp_hdr->length +
-				sizeof(struct amt_host_if_msg_header))) {
-		return AMT_STATUS_INTERNAL_ERROR;
-	} else if (resp_hdr->command != command) {
-		return AMT_STATUS_INTERNAL_ERROR;
-	} else if (resp_hdr->_reserved != 0) {
-		return AMT_STATUS_INTERNAL_ERROR;
-	} else if (resp_hdr->version.major != AMT_MAJOR_VERSION ||
-		   resp_hdr->version.minor < AMT_MINOR_VERSION) {
-		return AMT_STATUS_INTERNAL_ERROR;
-	}
-	return AMT_STATUS_SUCCESS;
-}
-
-static uint32_t amt_host_if_call(struct amt_host_if *acmd,
-			const unsigned char *command, ssize_t command_sz,
-			uint8_t **read_buf, uint32_t rcmd,
-			unsigned int expected_sz)
-{
-	uint32_t in_buf_sz;
-	uint32_t out_buf_sz;
-	ssize_t written;
-	uint32_t status;
-	struct amt_host_if_resp_header *msg_hdr;
-
-	in_buf_sz = acmd->mei_cl.buf_size;
-	*read_buf = (uint8_t *)malloc(sizeof(uint8_t) * in_buf_sz);
-	if (*read_buf == NULL)
-		return AMT_STATUS_SDK_RESOURCES;
-	memset(*read_buf, 0, in_buf_sz);
-	msg_hdr = (struct amt_host_if_resp_header *)*read_buf;
-
-	written = mei_send_msg(&acmd->mei_cl,
-				command, command_sz, acmd->send_timeout);
-	if (written != command_sz)
-		return AMT_STATUS_INTERNAL_ERROR;
-
-	out_buf_sz = mei_recv_msg(&acmd->mei_cl, *read_buf, in_buf_sz, 2000);
-	if (out_buf_sz <= 0)
-		return AMT_STATUS_HOST_IF_EMPTY_RESPONSE;
-
-	status = msg_hdr->status;
-	if (status != AMT_STATUS_SUCCESS)
-		return status;
-
-	status = amt_verify_response_header(rcmd,
-				&msg_hdr->header, out_buf_sz);
-	if (status != AMT_STATUS_SUCCESS)
-		return status;
-
-	if (expected_sz && expected_sz != out_buf_sz)
-		return AMT_STATUS_INTERNAL_ERROR;
-
-	return AMT_STATUS_SUCCESS;
-}
-
-
-static uint32_t amt_get_code_versions(struct amt_host_if *cmd,
-			       struct amt_code_versions *versions)
-{
-	struct amt_host_if_resp_header *response = NULL;
-	uint32_t status;
-
-	status = amt_host_if_call(cmd,
-			(const unsigned char *)&CODE_VERSION_REQ,
-			sizeof(CODE_VERSION_REQ),
-			(uint8_t **)&response,
-			AMT_HOST_IF_CODE_VERSIONS_RESPONSE, 0);
-
-	if (status != AMT_STATUS_SUCCESS)
-		goto out;
-
-	status = amt_verify_code_versions(response);
-	if (status != AMT_STATUS_SUCCESS)
-		goto out;
-
-	memcpy(versions, response->data, sizeof(struct amt_code_versions));
-out:
-	if (response != NULL)
-		free(response);
-
-	return status;
-}
-
-/************************** end of amt_host_if_command ***********************/
-int main(int argc, char **argv)
-{
-	struct amt_code_versions ver;
-	struct amt_host_if acmd;
-	unsigned int i;
-	uint32_t status;
-	int ret;
-	bool verbose;
-
-	verbose = (argc > 1 && strcmp(argv[1], "-v") == 0);
-
-	if (!amt_host_if_init(&acmd, 5000, verbose)) {
-		ret = 1;
-		goto out;
-	}
-
-	status = amt_get_code_versions(&acmd, &ver);
-
-	amt_host_if_deinit(&acmd);
-
-	switch (status) {
-	case AMT_STATUS_HOST_IF_EMPTY_RESPONSE:
-		printf("Intel AMT: DISABLED\n");
-		ret = 0;
-		break;
-	case AMT_STATUS_SUCCESS:
-		printf("Intel AMT: ENABLED\n");
-		for (i = 0; i < ver.count; i++) {
-			printf("%s:\t%s\n", ver.versions[i].description.string,
-				ver.versions[i].version.string);
-		}
-		ret = 0;
-		break;
-	default:
-		printf("An error has occurred\n");
-		ret = 1;
-		break;
-	}
-
-out:
-	return ret;
-}
diff --git a/drivers/staging/mei/mei.txt b/drivers/staging/mei/mei.txt
deleted file mode 100644
index 2785697da59..00000000000
--- a/drivers/staging/mei/mei.txt
+++ /dev/null
@@ -1,215 +0,0 @@
-Intel(R) Management Engine Interface (Intel(R) MEI)
-=======================
-
-Introduction
-=======================
-
-The Intel Management Engine (Intel ME) is an isolated and protected computing
-resource (Co-processor) residing inside certain Intel chipsets. The Intel ME
-provides support for computer/IT management features. The feature set
-depends on the Intel chipset SKU.
-
-The Intel Management Engine Interface (Intel MEI, previously known as HECI)
-is the interface between the Host and Intel ME. This interface is exposed
-to the host as a PCI device. The Intel MEI Driver is in charge of the
-communication channel between a host application and the Intel ME feature.
-
-Each Intel ME feature (Intel ME Client) is addressed by a GUID/UUID and
-each client has its own protocol. The protocol is message-based with a
-header and payload up to 512 bytes.
-
-Prominent usage of the Intel ME Interface is to communicate with Intel(R)
-Active Management Technology (Intel AMT)implemented in firmware running on
-the Intel ME.
-
-Intel AMT provides the ability to manage a host remotely out-of-band (OOB)
-even when the operating system running on the host processor has crashed or
-is in a sleep state.
-
-Some examples of Intel AMT usage are:
-   - Monitoring hardware state and platform components
-   - Remote power off/on (useful for green computing or overnight IT
-     maintenance)
-   - OS updates
-   - Storage of useful platform information such as software assets
-   - Built-in hardware KVM
-   - Selective network isolation of Ethernet and IP protocol flows based
-     on policies set by a remote management console
-   - IDE device redirection from remote management console
-
-Intel AMT (OOB) communication is based on SOAP (deprecated
-starting with Release 6.0) over HTTP/S or WS-Management protocol over
-HTTP/S that are received from a remote management console application.
-
-For more information about Intel AMT:
-http://software.intel.com/sites/manageability/AMT_Implementation_and_Reference_Guide
-
-Intel MEI Driver
-=======================
-
-The driver exposes a misc device called /dev/mei.
-
-An application maintains communication with an Intel ME feature while
-/dev/mei is open. The binding to a specific features is performed by calling
-MEI_CONNECT_CLIENT_IOCTL, which passes the desired UUID.
-The number of instances of an Intel ME feature that can be opened
-at the same time depends on the Intel ME feature, but most of the
-features allow only a single instance.
-
-The Intel AMT Host Interface (Intel AMTHI) feature supports multiple
-simultaneous user applications. Therefore, the Intel MEI driver handles
-this internally by maintaining request queues for the applications.
-
-The driver is oblivious to data that is passed between firmware feature
-and host application.
-
-Because some of the Intel ME features can change the system
-configuration, the driver by default allows only a privileged
-user to access it.
-
-A code snippet for an application communicating with
-Intel AMTHI client:
-	struct mei_connect_client_data data;
-	fd = open(MEI_DEVICE);
-
-	data.d.in_client_uuid = AMTHI_UUID;
-
-	ioctl(fd, IOCTL_MEI_CONNECT_CLIENT, &data);
-
-	printf("Ver=%d, MaxLen=%ld\n",
-			data.d.in_client_uuid.protocol_version,
-			data.d.in_client_uuid.max_msg_length);
-
-	[...]
-
-	write(fd, amthi_req_data, amthi_req_data_len);
-
-	[...]
-
-	read(fd, &amthi_res_data, amthi_res_data_len);
-
-	[...]
-	close(fd);
-
-IOCTL:
-======
-The Intel MEI Driver supports the following IOCTL command:
-	IOCTL_MEI_CONNECT_CLIENT	Connect to firmware Feature (client).
-
-	usage:
-		struct mei_connect_client_data clientData;
-		ioctl(fd, IOCTL_MEI_CONNECT_CLIENT, &clientData);
-
-	inputs:
-		mei_connect_client_data struct contain the following
-		input field:
-
-		in_client_uuid -	UUID of the FW Feature that needs
-					to connect to.
-	outputs:
-		out_client_properties - Client Properties: MTU and Protocol Version.
-
-	error returns:
-		EINVAL	Wrong IOCTL Number
-		ENODEV	Device or Connection is not initialized or ready.
-			(e.g. Wrong UUID)
-		ENOMEM	Unable to allocate memory to client internal data.
-		EFAULT	Fatal Error (e.g. Unable to access user input data)
-		EBUSY	Connection Already Open
-
-	Notes:
-        max_msg_length (MTU) in client properties describes the maximum
-        data that can be sent or received. (e.g. if MTU=2K, can send
-        requests up to bytes 2k and received responses upto 2k bytes).
-
-Intel ME Applications:
-==============
-
-1) Intel Local Management Service (Intel LMS)
-
-	Applications running locally on the platform communicate with Intel AMT Release
-	2.0 and later releases in the same way that network applications do via SOAP
-	over HTTP (deprecated starting with Release 6.0) or with WS-Management over
-	SOAP over HTTP. This means that some Intel AMT features can be accessed from a
-	local application using the same network interface as a remote application
-	communicating with Intel AMT over the network.
-
-	When a local application sends a message addressed to the local Intel AMT host
-	name, the Intel LMS, which listens for traffic directed to the host name,
-	intercepts the message and routes it to the Intel MEI.
-	For more information:
-	http://software.intel.com/sites/manageability/AMT_Implementation_and_Reference_Guide
-	Under "About Intel AMT" => "Local Access"
-
-	For downloading Intel LMS:
-	http://software.intel.com/en-us/articles/download-the-latest-intel-amt-open-source-drivers/
-
-	The Intel LMS opens a connection using the Intel MEI driver to the Intel LMS
-	firmware feature using a defined UUID and then communicates with the feature
-	using a protocol called Intel AMT Port Forwarding Protocol(Intel APF protocol).
-	The protocol is used to maintain multiple sessions with Intel AMT from a
-	single application.
-
-	See the protocol specification in the Intel AMT Software Development Kit(SDK)
-	http://software.intel.com/sites/manageability/AMT_Implementation_and_Reference_Guide
-	Under "SDK Resources" => "Intel(R) vPro(TM) Gateway(MPS)"
-	=> "Information for Intel(R) vPro(TM) Gateway Developers"
-	=> "Description of the Intel AMT Port Forwarding (APF)Protocol"
-
-  2) Intel AMT Remote configuration using a Local Agent
-	A Local Agent enables IT personnel to configure Intel AMT out-of-the-box
-	without requiring installing additional data to enable setup. The remote
-	configuration process may involve an ISV-developed remote configuration
-	agent that runs on the host.
-	For more information:
-	http://software.intel.com/sites/manageability/AMT_Implementation_and_Reference_Guide
-	Under "Setup and Configuration of Intel AMT" =>
-	"SDK Tools Supporting Setup and Configuration" =>
-	"Using the Local Agent Sample"
-
-	An open source Intel AMT configuration utility,	implementing a local agent
-	that accesses the Intel MEI driver, can be found here:
-	http://software.intel.com/en-us/articles/download-the-latest-intel-amt-open-source-drivers/
-
-
-Intel AMT OS Health Watchdog:
-=============================
-The Intel AMT Watchdog is an OS Health (Hang/Crash) watchdog.
-Whenever the OS hangs or crashes, Intel AMT will send an event
-to any subscriber to this event. This mechanism means that
-IT knows when a platform crashes even when there is a hard failure on the host.
-
-The Intel AMT Watchdog is composed of two parts:
-	1) Firmware feature - receives the heartbeats
-	   and sends an event when the heartbeats stop.
-	2) Intel MEI driver - connects to the watchdog feature, configures the
-	   watchdog and sends the heartbeats.
-
-The Intel MEI driver uses the kernel watchdog to configure the Intel AMT
-Watchdog and to send heartbeats to it. The default timeout of the
-watchdog is 120 seconds.
-
-If the Intel AMT Watchdog feature does not exist (i.e. the connection failed),
-the Intel MEI driver will disable the sending of heartbeats.
-
-Supported Chipsets:
-==================
-7 Series Chipset Family
-6 Series Chipset Family
-5 Series Chipset Family
-4 Series Chipset Family
-Mobile 4 Series Chipset Family
-ICH9
-82946GZ/GL
-82G35 Express
-82Q963/Q965
-82P965/G965
-Mobile PM965/GM965
-Mobile GME965/GLE960
-82Q35 Express
-82G33/G31/P35/P31 Express
-82Q33 Express
-82X38/X48 Express
-
----
-linux-mei@linux.intel.com
diff --git a/drivers/staging/net/Kconfig b/drivers/staging/net/Kconfig
new file mode 100644
index 00000000000..a64e56b1898
--- /dev/null
+++ b/drivers/staging/net/Kconfig
@@ -0,0 +1,38 @@
+if NETDEVICES
+
+if WAN
+
+config PC300
+	tristate "Cyclades-PC300 support (RS-232/V.35, X.21, T1/E1 boards)"
+	depends on HDLC && PCI && BROKEN
+	---help---
+	  This driver is broken because of struct tty_driver change.
+
+	  Driver for the Cyclades-PC300 synchronous communication boards.
+
+	  These boards provide synchronous serial interfaces to your
+	  Linux box (interfaces currently available are RS-232/V.35, X.21 and
+	  T1/E1). If you wish to support Multilink PPP, please select the
+	  option later and read the file README.mlppp provided by PC300
+	  package.
+
+	  To compile this as a module, choose M here: the module
+	  will be called pc300.
+
+	  If unsure, say N.
+
+config PC300_MLPPP
+	bool "Cyclades-PC300 MLPPP support"
+	depends on PC300 && PPP_MULTILINK && PPP_SYNC_TTY && HDLC_PPP
+	help
+	  Multilink PPP over the PC300 synchronous communication boards.
+
+comment "Cyclades-PC300 MLPPP support is disabled."
+	depends on HDLC && PC300 && (PPP=n || !PPP_MULTILINK || PPP_SYNC_TTY=n || !HDLC_PPP)
+
+comment "Refer to the file README.mlppp, provided by PC300 package."
+	depends on HDLC && PC300 && (PPP=n || !PPP_MULTILINK || PPP_SYNC_TTY=n || !HDLC_PPP)
+
+endif # WAN
+
+endif # NETDEVICES
diff --git a/drivers/staging/net/Makefile b/drivers/staging/net/Makefile
new file mode 100644
index 00000000000..0799c43d511
--- /dev/null
+++ b/drivers/staging/net/Makefile
@@ -0,0 +1,5 @@
+pc300-y				:= pc300_drv.o
+pc300-$(CONFIG_PC300_MLPPP)	+= pc300_tty.o
+pc300-objs			:= $(pc300-y)
+
+obj-$(CONFIG_PC300)		+= pc300.o
diff --git a/drivers/staging/net/TODO b/drivers/staging/net/TODO
new file mode 100644
index 00000000000..e3446f2ad7c
--- /dev/null
+++ b/drivers/staging/net/TODO
@@ -0,0 +1,5 @@
+PC300
+The driver is very broken and cannot work with the current TTY layer. It is
+inevitable to convert it to the new TTY API.
+
+If no one steps in to adopt the driver, it will be removed in the 3.7 release.
diff --git a/drivers/net/wan/pc300-falc-lh.h b/drivers/staging/net/pc300-falc-lh.h
index 01ed23ca76c..01ed23ca76c 100644
--- a/drivers/net/wan/pc300-falc-lh.h
+++ b/drivers/staging/net/pc300-falc-lh.h
diff --git a/drivers/net/wan/pc300.h b/drivers/staging/net/pc300.h
index 2e4f84f6cad..2e4f84f6cad 100644
--- a/drivers/net/wan/pc300.h
+++ b/drivers/staging/net/pc300.h
diff --git a/drivers/net/wan/pc300_drv.c b/drivers/staging/net/pc300_drv.c
index cb0f8d932b0..cb0f8d932b0 100644
--- a/drivers/net/wan/pc300_drv.c
+++ b/drivers/staging/net/pc300_drv.c
diff --git a/drivers/net/wan/pc300_tty.c b/drivers/staging/net/pc300_tty.c
index 4709f422856..4709f422856 100644
--- a/drivers/net/wan/pc300_tty.c
+++ b/drivers/staging/net/pc300_tty.c
diff --git a/drivers/staging/nvec/nvec.h b/drivers/staging/nvec/nvec.h
index a4c17b0e10c..ba6ed8f4e8a 100644
--- a/drivers/staging/nvec/nvec.h
+++ b/drivers/staging/nvec/nvec.h
@@ -42,7 +42,7 @@
  * enum nvec_event_size - The size of an event message
  * @NVEC_2BYTES: The message has one command byte and one data byte
  * @NVEC_3BYTES: The message has one command byte and two data bytes
- * @NVEC_VAR_SIZE: The message has one command byte, one count byte, and as
+ * @NVEC_VAR_SIZE: The message has one command byte, one count byte, and has
  *                 up to as many bytes as the number in the count byte. The
  *                 maximum is 32
  *
diff --git a/drivers/staging/octeon/ethernet-rx.c b/drivers/staging/octeon/ethernet-rx.c
index d91751f9ffe..34afc16bc49 100644
--- a/drivers/staging/octeon/ethernet-rx.c
+++ b/drivers/staging/octeon/ethernet-rx.c
@@ -163,7 +163,7 @@ static inline int cvm_oct_check_rcv_error(cvmx_wqe_t *work)
 		/*
 		 * We received a packet with either an alignment error
 		 * or a FCS error. This may be signalling that we are
-		 * running 10Mbps with GMXX_RXX_FRM_CTL[PRE_CHK}
+		 * running 10Mbps with GMXX_RXX_FRM_CTL[PRE_CHK]
 		 * off. If this is the case we need to parse the
 		 * packet to determine if we can remove a non spec
 		 * preamble and generate a correct packet.
diff --git a/drivers/staging/octeon/ethernet-tx.c b/drivers/staging/octeon/ethernet-tx.c
index 91a97b3e45c..5631dd9f820 100644
--- a/drivers/staging/octeon/ethernet-tx.c
+++ b/drivers/staging/octeon/ethernet-tx.c
@@ -62,7 +62,7 @@
  * You can define GET_SKBUFF_QOS() to override how the skbuff output
  * function determines which output queue is used. The default
  * implementation always uses the base queue for the port. If, for
- * example, you wanted to use the skb->priority fieid, define
+ * example, you wanted to use the skb->priority field, define
  * GET_SKBUFF_QOS as: #define GET_SKBUFF_QOS(skb) ((skb)->priority)
  */
 #ifndef GET_SKBUFF_QOS
@@ -165,8 +165,8 @@ int cvm_oct_xmit(struct sk_buff *skb, struct net_device *dev)
 #endif
 
 	/*
-	 * Prefetch the private data structure.  It is larger that one
-	 * cache line.
+	 * Prefetch the private data structure.  It is larger than the
+	 * one cache line.
 	 */
 	prefetch(priv);
 
@@ -291,8 +291,8 @@ int cvm_oct_xmit(struct sk_buff *skb, struct net_device *dev)
 	 * See if we can put this skb in the FPA pool. Any strange
 	 * behavior from the Linux networking stack will most likely
 	 * be caused by a bug in the following code. If some field is
-	 * in use by the network stack and get carried over when a
-	 * buffer is reused, bad thing may happen.  If in doubt and
+	 * in use by the network stack and gets carried over when a
+	 * buffer is reused, bad things may happen.  If in doubt and
 	 * you dont need the absolute best performance, disable the
 	 * define REUSE_SKBUFFS_WITHOUT_FREE. The reuse of buffers has
 	 * shown a 25% increase in performance under some loads.
@@ -345,7 +345,7 @@ int cvm_oct_xmit(struct sk_buff *skb, struct net_device *dev)
 	}
 	if (unlikely
 	    (skb->truesize !=
-	     sizeof(*skb) + skb_end_pointer(skb) - skb->head)) {
+	     sizeof(*skb) + skb_end_offset(skb))) {
 		/*
 		   printk("TX buffer truesize has been changed\n");
 		 */
diff --git a/drivers/staging/octeon/ethernet-util.h b/drivers/staging/octeon/ethernet-util.h
index 144fb99bf50..2da5ce17ead 100644
--- a/drivers/staging/octeon/ethernet-util.h
+++ b/drivers/staging/octeon/ethernet-util.h
@@ -38,7 +38,7 @@ static inline void *cvm_oct_get_buffer_ptr(union cvmx_buf_ptr packet_ptr)
 }
 
 /**
- * INTERFACE - convert IPD port to locgical interface
+ * INTERFACE - convert IPD port to logical interface
  * @ipd_port: Port to check
  *
  * Returns Logical interface
diff --git a/drivers/staging/octeon/ethernet.c b/drivers/staging/octeon/ethernet.c
index 60cba8194de..18f7a790f73 100644
--- a/drivers/staging/octeon/ethernet.c
+++ b/drivers/staging/octeon/ethernet.c
@@ -357,7 +357,7 @@ static void cvm_oct_common_set_multicast_list(struct net_device *dev)
 			/* Force accept multicast packets */
 			control.s.mcst = 2;
 		else
-			/* Force reject multicat packets */
+			/* Force reject multicast packets */
 			control.s.mcst = 1;
 
 		if (dev->flags & IFF_PROMISC)
diff --git a/drivers/staging/olpc_dcon/olpc_dcon.c b/drivers/staging/olpc_dcon/olpc_dcon.c
index 3d9199320d8..992275c0d87 100644
--- a/drivers/staging/olpc_dcon/olpc_dcon.c
+++ b/drivers/staging/olpc_dcon/olpc_dcon.c
@@ -71,8 +71,8 @@ static int dcon_hw_init(struct dcon_priv *dcon, int is_init)
 
 	ver = dcon_read(dcon, DCON_REG_ID);
 	if ((ver >> 8) != 0xDC) {
-		printk(KERN_ERR "olpc-dcon:  DCON ID not 0xDCxx: 0x%04x "
-				"instead.\n", ver);
+		printk(KERN_ERR "olpc-dcon:  DCON ID not 0xDCxx: 0x%04x instead.\n",
+			ver);
 		rc = -ENXIO;
 		goto err;
 	}
@@ -134,10 +134,10 @@ static int dcon_bus_stabilize(struct dcon_priv *dcon, int is_powered_down)
 power_up:
 	if (is_powered_down) {
 		x = 1;
-		x = olpc_ec_cmd(0x26, (unsigned char *) &x, 1, NULL, 0);
+		x = olpc_ec_cmd(0x26, (unsigned char *)&x, 1, NULL, 0);
 		if (x) {
-			printk(KERN_WARNING "olpc-dcon:  unable to force dcon "
-					"to power up: %d!\n", x);
+			printk(KERN_WARNING "olpc-dcon:  unable to force dcon to power up: %d!\n",
+				x);
 			return x;
 		}
 		msleep(10); /* we'll be conservative */
@@ -150,11 +150,10 @@ power_up:
 		x = dcon_read(dcon, DCON_REG_ID);
 	}
 	if (x < 0) {
-		printk(KERN_ERR "olpc-dcon:  unable to stabilize dcon's "
-				"smbus, reasserting power and praying.\n");
+		printk(KERN_ERR "olpc-dcon:  unable to stabilize dcon's smbus, reasserting power and praying.\n");
 		BUG_ON(olpc_board_at_least(olpc_board(0xc2)));
 		x = 0;
-		olpc_ec_cmd(0x26, (unsigned char *) &x, 1, NULL, 0);
+		olpc_ec_cmd(0x26, (unsigned char *)&x, 1, NULL, 0);
 		msleep(100);
 		is_powered_down = 1;
 		goto power_up;	/* argh, stupid hardware.. */
@@ -220,10 +219,10 @@ static void dcon_sleep(struct dcon_priv *dcon, bool sleep)
 
 	if (sleep) {
 		x = 0;
-		x = olpc_ec_cmd(0x26, (unsigned char *) &x, 1, NULL, 0);
+		x = olpc_ec_cmd(0x26, (unsigned char *)&x, 1, NULL, 0);
 		if (x)
-			printk(KERN_WARNING "olpc-dcon:  unable to force dcon "
-					"to power down: %d!\n", x);
+			printk(KERN_WARNING "olpc-dcon:  unable to force dcon to power down: %d!\n",
+				x);
 		else
 			dcon->asleep = sleep;
 	} else {
@@ -232,8 +231,8 @@ static void dcon_sleep(struct dcon_priv *dcon, bool sleep)
 			dcon->disp_mode |= MODE_BL_ENABLE;
 		x = dcon_bus_stabilize(dcon, 1);
 		if (x)
-			printk(KERN_WARNING "olpc-dcon:  unable to reinit dcon"
-					" hardware: %d!\n", x);
+			printk(KERN_WARNING "olpc-dcon:  unable to reinit dcon hardware: %d!\n",
+				x);
 		else
 			dcon->asleep = sleep;
 
@@ -304,7 +303,7 @@ static void dcon_source_switch(struct work_struct *work)
 
 	switch (source) {
 	case DCON_SOURCE_CPU:
-		printk("dcon_source_switch to CPU\n");
+		printk(KERN_INFO "dcon_source_switch to CPU\n");
 		/* Enable the scanline interrupt bit */
 		if (dcon_write(dcon, DCON_REG_MODE,
 				dcon->disp_mode | MODE_SCAN_INT))
@@ -599,7 +598,7 @@ static int dcon_fb_notifier(struct notifier_block *self,
 	struct fb_event *evdata = data;
 	struct dcon_priv *dcon = container_of(self, struct dcon_priv,
 			fbevent_nb);
-	int *blank = (int *) evdata->data;
+	int *blank = (int *)evdata->data;
 	if (((event != FB_EVENT_BLANK) && (event != FB_EVENT_CONBLANK)) ||
 			dcon->ignore_fb_events)
 		return 0;
diff --git a/drivers/staging/olpc_dcon/olpc_dcon_xo_1.c b/drivers/staging/olpc_dcon/olpc_dcon_xo_1.c
index cb6ce0cf92a..c87fdfac485 100644
--- a/drivers/staging/olpc_dcon/olpc_dcon_xo_1.c
+++ b/drivers/staging/olpc_dcon/olpc_dcon_xo_1.c
@@ -116,7 +116,7 @@ static int dcon_init_xo_1(struct dcon_priv *dcon)
 	cs5535_gpio_set(OLPC_GPIO_DCON_IRQ, GPIO_NEGATIVE_EDGE_STS);
 	cs5535_gpio_set(OLPC_GPIO_DCON_BLANK, GPIO_NEGATIVE_EDGE_STS);
 
-	/* FIXME:  Clear the posiitive status as well, just to be sure */
+	/* FIXME:  Clear the positive status as well, just to be sure */
 	cs5535_gpio_set(OLPC_GPIO_DCON_IRQ, GPIO_POSITIVE_EDGE_STS);
 	cs5535_gpio_set(OLPC_GPIO_DCON_BLANK, GPIO_POSITIVE_EDGE_STS);
 
diff --git a/drivers/staging/omapdrm/Makefile b/drivers/staging/omapdrm/Makefile
index d9cdc120d12..1ca0e0016de 100644
--- a/drivers/staging/omapdrm/Makefile
+++ b/drivers/staging/omapdrm/Makefile
@@ -13,6 +13,7 @@ omapdrm-y := omap_drv.o \
 	omap_fb.o \
 	omap_fbdev.o \
 	omap_gem.o \
+	omap_gem_dmabuf.o \
 	omap_dmm_tiler.o \
 	tcm-sita.o
 
diff --git a/drivers/staging/omapdrm/omap_crtc.c b/drivers/staging/omapdrm/omap_crtc.c
index 490a7f15604..8b864afb40b 100644
--- a/drivers/staging/omapdrm/omap_crtc.c
+++ b/drivers/staging/omapdrm/omap_crtc.c
@@ -36,12 +36,6 @@ struct omap_crtc {
 	struct drm_framebuffer *old_fb;
 };
 
-static void omap_crtc_gamma_set(struct drm_crtc *crtc,
-		u16 *red, u16 *green, u16 *blue, uint32_t start, uint32_t size)
-{
-	/* not supported.. at least not yet */
-}
-
 static void omap_crtc_destroy(struct drm_crtc *crtc)
 {
 	struct omap_crtc *omap_crtc = to_omap_crtc(crtc);
@@ -198,7 +192,6 @@ static int omap_crtc_page_flip_locked(struct drm_crtc *crtc,
 }
 
 static const struct drm_crtc_funcs omap_crtc_funcs = {
-	.gamma_set = omap_crtc_gamma_set,
 	.set_config = drm_crtc_helper_set_config,
 	.destroy = omap_crtc_destroy,
 	.page_flip = omap_crtc_page_flip_locked,
diff --git a/drivers/staging/omapdrm/omap_dmm_tiler.c b/drivers/staging/omapdrm/omap_dmm_tiler.c
index 1ecb6a73d79..9d83060e753 100644
--- a/drivers/staging/omapdrm/omap_dmm_tiler.c
+++ b/drivers/staging/omapdrm/omap_dmm_tiler.c
@@ -347,7 +347,7 @@ struct tiler_block *tiler_reserve_2d(enum tiler_fmt fmt, uint16_t w,
 	ret = tcm_reserve_2d(containers[fmt], w, h, align, &block->area);
 	if (ret) {
 		kfree(block);
-		return 0;
+		return ERR_PTR(-ENOMEM);
 	}
 
 	/* add to allocation list */
@@ -371,7 +371,7 @@ struct tiler_block *tiler_reserve_1d(size_t size)
 	if (tcm_reserve_1d(containers[TILFMT_PAGE], num_pages,
 				&block->area)) {
 		kfree(block);
-		return 0;
+		return ERR_PTR(-ENOMEM);
 	}
 
 	spin_lock(&omap_dmm->list_lock);
diff --git a/drivers/staging/omapdrm/omap_drv.c b/drivers/staging/omapdrm/omap_drv.c
index 620b8d54223..4beab9447ce 100644
--- a/drivers/staging/omapdrm/omap_drv.c
+++ b/drivers/staging/omapdrm/omap_drv.c
@@ -58,7 +58,7 @@ static void omap_fb_output_poll_changed(struct drm_device *dev)
 	}
 }
 
-static struct drm_mode_config_funcs omap_mode_config_funcs = {
+static const struct drm_mode_config_funcs omap_mode_config_funcs = {
 	.fb_create = omap_framebuffer_create,
 	.output_poll_changed = omap_fb_output_poll_changed,
 };
@@ -726,7 +726,7 @@ static void dev_irq_uninstall(struct drm_device *dev)
 	DBG("irq_uninstall: dev=%p", dev);
 }
 
-static struct vm_operations_struct omap_gem_vm_ops = {
+static const struct vm_operations_struct omap_gem_vm_ops = {
 	.fault = omap_gem_fault,
 	.open = drm_gem_vm_open,
 	.close = drm_gem_vm_close,
@@ -746,7 +746,7 @@ static const struct file_operations omapdriver_fops = {
 
 static struct drm_driver omap_drm_driver = {
 		.driver_features =
-				DRIVER_HAVE_IRQ | DRIVER_MODESET | DRIVER_GEM,
+				DRIVER_HAVE_IRQ | DRIVER_MODESET | DRIVER_GEM | DRIVER_PRIME,
 		.load = dev_load,
 		.unload = dev_unload,
 		.open = dev_open,
@@ -766,6 +766,10 @@ static struct drm_driver omap_drm_driver = {
 		.debugfs_init = omap_debugfs_init,
 		.debugfs_cleanup = omap_debugfs_cleanup,
 #endif
+		.prime_handle_to_fd = drm_gem_prime_handle_to_fd,
+		.prime_fd_to_handle = drm_gem_prime_fd_to_handle,
+		.gem_prime_export = omap_gem_prime_export,
+		.gem_prime_import = omap_gem_prime_import,
 		.gem_init_object = omap_gem_init_object,
 		.gem_free_object = omap_gem_free_object,
 		.gem_vm_ops = &omap_gem_vm_ops,
diff --git a/drivers/staging/omapdrm/omap_drv.h b/drivers/staging/omapdrm/omap_drv.h
index b7e0f077300..f238d574da0 100644
--- a/drivers/staging/omapdrm/omap_drv.h
+++ b/drivers/staging/omapdrm/omap_drv.h
@@ -138,6 +138,8 @@ int omap_gem_dumb_destroy(struct drm_file *file, struct drm_device *dev,
 int omap_gem_dumb_create(struct drm_file *file, struct drm_device *dev,
 		struct drm_mode_create_dumb *args);
 int omap_gem_mmap(struct file *filp, struct vm_area_struct *vma);
+int omap_gem_mmap_obj(struct drm_gem_object *obj,
+		struct vm_area_struct *vma);
 int omap_gem_fault(struct vm_area_struct *vma, struct vm_fault *vmf);
 int omap_gem_op_start(struct drm_gem_object *obj, enum omap_gem_op op);
 int omap_gem_op_finish(struct drm_gem_object *obj, enum omap_gem_op op);
@@ -145,12 +147,24 @@ int omap_gem_op_sync(struct drm_gem_object *obj, enum omap_gem_op op);
 int omap_gem_op_async(struct drm_gem_object *obj, enum omap_gem_op op,
 		void (*fxn)(void *arg), void *arg);
 int omap_gem_roll(struct drm_gem_object *obj, uint32_t roll);
+void omap_gem_cpu_sync(struct drm_gem_object *obj, int pgoff);
+void omap_gem_dma_sync(struct drm_gem_object *obj,
+		enum dma_data_direction dir);
 int omap_gem_get_paddr(struct drm_gem_object *obj,
 		dma_addr_t *paddr, bool remap);
 int omap_gem_put_paddr(struct drm_gem_object *obj);
+int omap_gem_get_pages(struct drm_gem_object *obj, struct page ***pages,
+		bool remap);
+int omap_gem_put_pages(struct drm_gem_object *obj);
+uint32_t omap_gem_flags(struct drm_gem_object *obj);
 uint64_t omap_gem_mmap_offset(struct drm_gem_object *obj);
 size_t omap_gem_mmap_size(struct drm_gem_object *obj);
 
+struct dma_buf * omap_gem_prime_export(struct drm_device *dev,
+		struct drm_gem_object *obj, int flags);
+struct drm_gem_object * omap_gem_prime_import(struct drm_device *dev,
+		struct dma_buf *buffer);
+
 static inline int align_pitch(int pitch, int width, int bpp)
 {
 	int bytespp = (bpp + 7) / 8;
diff --git a/drivers/staging/omapdrm/omap_fb.c b/drivers/staging/omapdrm/omap_fb.c
index 04b235b6724..74260f043ab 100644
--- a/drivers/staging/omapdrm/omap_fb.c
+++ b/drivers/staging/omapdrm/omap_fb.c
@@ -167,7 +167,7 @@ void omap_framebuffer_update_scanout(struct drm_framebuffer *fb, int x, int y,
 }
 
 /* Call for unpin 'a' (if not NULL), and pin 'b' (if not NULL).  Although
- * buffers to unpin are just just pushed to the unpin fifo so that the
+ * buffers to unpin are just pushed to the unpin fifo so that the
  * caller can defer unpin until vblank.
  *
  * Note if this fails (ie. something went very wrong!), all buffers are
@@ -197,8 +197,11 @@ int omap_framebuffer_replace(struct drm_framebuffer *a,
 			pa->paddr = 0;
 		}
 
-		if (pb && !ret)
+		if (pb && !ret) {
 			ret = omap_gem_get_paddr(pb->bo, &pb->paddr, true);
+			if (!ret)
+				omap_gem_dma_sync(pb->bo, DMA_TO_DEVICE);
+		}
 	}
 
 	if (ret) {
diff --git a/drivers/staging/omapdrm/omap_gem.c b/drivers/staging/omapdrm/omap_gem.c
index 921f058cc6a..3a0d035a9e0 100644
--- a/drivers/staging/omapdrm/omap_gem.c
+++ b/drivers/staging/omapdrm/omap_gem.c
@@ -207,13 +207,27 @@ static inline bool is_shmem(struct drm_gem_object *obj)
 	return obj->filp != NULL;
 }
 
+/**
+ * shmem buffers that are mapped cached can simulate coherency via using
+ * page faulting to keep track of dirty pages
+ */
+static inline bool is_cached_coherent(struct drm_gem_object *obj)
+{
+	struct omap_gem_object *omap_obj = to_omap_bo(obj);
+	return is_shmem(obj) &&
+		((omap_obj->flags & OMAP_BO_CACHE_MASK) == OMAP_BO_CACHED);
+}
+
 static DEFINE_SPINLOCK(sync_lock);
 
 /** ensure backing pages are allocated */
 static int omap_gem_attach_pages(struct drm_gem_object *obj)
 {
+	struct drm_device *dev = obj->dev;
 	struct omap_gem_object *omap_obj = to_omap_bo(obj);
 	struct page **pages;
+	int i, npages = obj->size >> PAGE_SHIFT;
+	dma_addr_t *addrs;
 
 	WARN_ON(omap_obj->pages);
 
@@ -231,16 +245,18 @@ static int omap_gem_attach_pages(struct drm_gem_object *obj)
 	 * DSS, GPU, etc. are not cache coherent:
 	 */
 	if (omap_obj->flags & (OMAP_BO_WC|OMAP_BO_UNCACHED)) {
-		int i, npages = obj->size >> PAGE_SHIFT;
-		dma_addr_t *addrs = kmalloc(npages * sizeof(addrs), GFP_KERNEL);
+		addrs = kmalloc(npages * sizeof(addrs), GFP_KERNEL);
 		for (i = 0; i < npages; i++) {
-			addrs[i] = dma_map_page(obj->dev->dev, pages[i],
+			addrs[i] = dma_map_page(dev->dev, pages[i],
 					0, PAGE_SIZE, DMA_BIDIRECTIONAL);
 		}
-		omap_obj->addrs = addrs;
+	} else {
+		addrs = kzalloc(npages * sizeof(addrs), GFP_KERNEL);
 	}
 
+	omap_obj->addrs = addrs;
 	omap_obj->pages = pages;
+
 	return 0;
 }
 
@@ -258,14 +274,21 @@ static void omap_gem_detach_pages(struct drm_gem_object *obj)
 			dma_unmap_page(obj->dev->dev, omap_obj->addrs[i],
 					PAGE_SIZE, DMA_BIDIRECTIONAL);
 		}
-		kfree(omap_obj->addrs);
-		omap_obj->addrs = NULL;
 	}
 
+	kfree(omap_obj->addrs);
+	omap_obj->addrs = NULL;
+
 	_drm_gem_put_pages(obj, omap_obj->pages, true, false);
 	omap_obj->pages = NULL;
 }
 
+/* get buffer flags */
+uint32_t omap_gem_flags(struct drm_gem_object *obj)
+{
+	return to_omap_bo(obj)->flags;
+}
+
 /** get mmap offset */
 static uint64_t mmap_offset(struct drm_gem_object *obj)
 {
@@ -330,6 +353,7 @@ static int fault_1d(struct drm_gem_object *obj,
 			vma->vm_start) >> PAGE_SHIFT;
 
 	if (omap_obj->pages) {
+		omap_gem_cpu_sync(obj, pgoff);
 		pfn = page_to_pfn(omap_obj->pages[pgoff]);
 	} else {
 		BUG_ON(!(omap_obj->flags & OMAP_BO_DMA));
@@ -504,7 +528,6 @@ fail:
 /** We override mainly to fix up some of the vm mapping flags.. */
 int omap_gem_mmap(struct file *filp, struct vm_area_struct *vma)
 {
-	struct omap_gem_object *omap_obj;
 	int ret;
 
 	ret = drm_gem_mmap(filp, vma);
@@ -513,8 +536,13 @@ int omap_gem_mmap(struct file *filp, struct vm_area_struct *vma)
 		return ret;
 	}
 
-	/* after drm_gem_mmap(), it is safe to access the obj */
-	omap_obj = to_omap_bo(vma->vm_private_data);
+	return omap_gem_mmap_obj(vma->vm_private_data, vma);
+}
+
+int omap_gem_mmap_obj(struct drm_gem_object *obj,
+		struct vm_area_struct *vma)
+{
+	struct omap_gem_object *omap_obj = to_omap_bo(obj);
 
 	vma->vm_flags &= ~VM_PFNMAP;
 	vma->vm_flags |= VM_MIXEDMAP;
@@ -524,12 +552,31 @@ int omap_gem_mmap(struct file *filp, struct vm_area_struct *vma)
 	} else if (omap_obj->flags & OMAP_BO_UNCACHED) {
 		vma->vm_page_prot = pgprot_noncached(vm_get_page_prot(vma->vm_flags));
 	} else {
+		/*
+		 * We do have some private objects, at least for scanout buffers
+		 * on hardware without DMM/TILER.  But these are allocated write-
+		 * combine
+		 */
+		if (WARN_ON(!obj->filp))
+			return -EINVAL;
+
+		/*
+		 * Shunt off cached objs to shmem file so they have their own
+		 * address_space (so unmap_mapping_range does what we want,
+		 * in particular in the case of mmap'd dmabufs)
+		 */
+		fput(vma->vm_file);
+		get_file(obj->filp);
+		vma->vm_pgoff = 0;
+		vma->vm_file  = obj->filp;
+
 		vma->vm_page_prot = vm_get_page_prot(vma->vm_flags);
 	}
 
-	return ret;
+	return 0;
 }
 
+
 /**
  * omap_gem_dumb_create	-	create a dumb buffer
  * @drm_file: our client file
@@ -639,6 +686,48 @@ fail:
 	return ret;
 }
 
+/* Sync the buffer for CPU access.. note pages should already be
+ * attached, ie. omap_gem_get_pages()
+ */
+void omap_gem_cpu_sync(struct drm_gem_object *obj, int pgoff)
+{
+	struct drm_device *dev = obj->dev;
+	struct omap_gem_object *omap_obj = to_omap_bo(obj);
+
+	if (is_cached_coherent(obj) && omap_obj->addrs[pgoff]) {
+		dma_unmap_page(dev->dev, omap_obj->addrs[pgoff],
+				PAGE_SIZE, DMA_BIDIRECTIONAL);
+		omap_obj->addrs[pgoff] = 0;
+	}
+}
+
+/* sync the buffer for DMA access */
+void omap_gem_dma_sync(struct drm_gem_object *obj,
+		enum dma_data_direction dir)
+{
+	struct drm_device *dev = obj->dev;
+	struct omap_gem_object *omap_obj = to_omap_bo(obj);
+
+	if (is_cached_coherent(obj)) {
+		int i, npages = obj->size >> PAGE_SHIFT;
+		struct page **pages = omap_obj->pages;
+		bool dirty = false;
+
+		for (i = 0; i < npages; i++) {
+			if (!omap_obj->addrs[i]) {
+				omap_obj->addrs[i] = dma_map_page(dev->dev, pages[i], 0,
+						PAGE_SIZE, DMA_BIDIRECTIONAL);
+				dirty = true;
+			}
+		}
+
+		if (dirty) {
+			unmap_mapping_range(obj->filp->f_mapping, 0,
+					omap_gem_mmap_size(obj), 1);
+		}
+	}
+}
+
 /* Get physical address for DMA.. if 'remap' is true, and the buffer is not
  * already contiguous, remap it to pin in physically contiguous memory.. (ie.
  * map in TILER)
@@ -703,6 +792,7 @@ int omap_gem_get_paddr(struct drm_gem_object *obj,
 		*paddr = omap_obj->paddr;
 	} else {
 		ret = -EINVAL;
+		goto fail;
 	}
 
 fail:
@@ -764,9 +854,27 @@ static int get_pages(struct drm_gem_object *obj, struct page ***pages)
 	return 0;
 }
 
-int omap_gem_get_pages(struct drm_gem_object *obj, struct page ***pages)
+/* if !remap, and we don't have pages backing, then fail, rather than
+ * increasing the pin count (which we don't really do yet anyways,
+ * because we don't support swapping pages back out).  And 'remap'
+ * might not be quite the right name, but I wanted to keep it working
+ * similarly to omap_gem_get_paddr().  Note though that mutex is not
+ * aquired if !remap (because this can be called in atomic ctxt),
+ * but probably omap_gem_get_paddr() should be changed to work in the
+ * same way.  If !remap, a matching omap_gem_put_pages() call is not
+ * required (and should not be made).
+ */
+int omap_gem_get_pages(struct drm_gem_object *obj, struct page ***pages,
+		bool remap)
 {
 	int ret;
+	if (!remap) {
+		struct omap_gem_object *omap_obj = to_omap_bo(obj);
+		if (!omap_obj->pages)
+			return -ENOMEM;
+		*pages = omap_obj->pages;
+		return 0;
+	}
 	mutex_lock(&obj->dev->struct_mutex);
 	ret = get_pages(obj, pages);
 	mutex_unlock(&obj->dev->struct_mutex);
diff --git a/drivers/staging/omapdrm/omap_gem_dmabuf.c b/drivers/staging/omapdrm/omap_gem_dmabuf.c
new file mode 100644
index 00000000000..42728e0cc19
--- /dev/null
+++ b/drivers/staging/omapdrm/omap_gem_dmabuf.c
@@ -0,0 +1,220 @@
+/*
+ * drivers/staging/omapdrm/omap_gem_dmabuf.c
+ *
+ * Copyright (C) 2011 Texas Instruments
+ * Author: Rob Clark <rob.clark@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 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 "omap_drv.h"
+
+#include <linux/dma-buf.h>
+
+static struct sg_table *omap_gem_map_dma_buf(
+		struct dma_buf_attachment *attachment,
+		enum dma_data_direction dir)
+{
+	struct drm_gem_object *obj = attachment->dmabuf->priv;
+	struct sg_table *sg;
+	dma_addr_t paddr;
+	int ret;
+
+	sg = kzalloc(sizeof(*sg), GFP_KERNEL);
+	if (!sg)
+		return ERR_PTR(-ENOMEM);
+
+	/* camera, etc, need physically contiguous.. but we need a
+	 * better way to know this..
+	 */
+	ret = omap_gem_get_paddr(obj, &paddr, true);
+	if (ret)
+		goto out;
+
+	ret = sg_alloc_table(sg, 1, GFP_KERNEL);
+	if (ret)
+		goto out;
+
+	sg_init_table(sg->sgl, 1);
+	sg_dma_len(sg->sgl) = obj->size;
+	sg_set_page(sg->sgl, pfn_to_page(PFN_DOWN(paddr)), obj->size, 0);
+	sg_dma_address(sg->sgl) = paddr;
+
+	/* this should be after _get_paddr() to ensure we have pages attached */
+	omap_gem_dma_sync(obj, dir);
+
+out:
+	if (ret)
+		return ERR_PTR(ret);
+	return sg;
+}
+
+static void omap_gem_unmap_dma_buf(struct dma_buf_attachment *attachment,
+		struct sg_table *sg, enum dma_data_direction dir)
+{
+	struct drm_gem_object *obj = attachment->dmabuf->priv;
+	omap_gem_put_paddr(obj);
+	sg_free_table(sg);
+	kfree(sg);
+}
+
+static void omap_gem_dmabuf_release(struct dma_buf *buffer)
+{
+	struct drm_gem_object *obj = buffer->priv;
+	/* release reference that was taken when dmabuf was exported
+	 * in omap_gem_prime_set()..
+	 */
+	drm_gem_object_unreference_unlocked(obj);
+}
+
+
+static int omap_gem_dmabuf_begin_cpu_access(struct dma_buf *buffer,
+		size_t start, size_t len, enum dma_data_direction dir)
+{
+	struct drm_gem_object *obj = buffer->priv;
+	struct page **pages;
+	if (omap_gem_flags(obj) & OMAP_BO_TILED) {
+		/* TODO we would need to pin at least part of the buffer to
+		 * get de-tiled view.  For now just reject it.
+		 */
+		return -ENOMEM;
+	}
+	/* make sure we have the pages: */
+	return omap_gem_get_pages(obj, &pages, true);
+}
+
+static void omap_gem_dmabuf_end_cpu_access(struct dma_buf *buffer,
+		size_t start, size_t len, enum dma_data_direction dir)
+{
+	struct drm_gem_object *obj = buffer->priv;
+	omap_gem_put_pages(obj);
+}
+
+
+static void *omap_gem_dmabuf_kmap_atomic(struct dma_buf *buffer,
+		unsigned long page_num)
+{
+	struct drm_gem_object *obj = buffer->priv;
+	struct page **pages;
+	omap_gem_get_pages(obj, &pages, false);
+	omap_gem_cpu_sync(obj, page_num);
+	return kmap_atomic(pages[page_num]);
+}
+
+static void omap_gem_dmabuf_kunmap_atomic(struct dma_buf *buffer,
+		unsigned long page_num, void *addr)
+{
+	kunmap_atomic(addr);
+}
+
+static void *omap_gem_dmabuf_kmap(struct dma_buf *buffer,
+		unsigned long page_num)
+{
+	struct drm_gem_object *obj = buffer->priv;
+	struct page **pages;
+	omap_gem_get_pages(obj, &pages, false);
+	omap_gem_cpu_sync(obj, page_num);
+	return kmap(pages[page_num]);
+}
+
+static void omap_gem_dmabuf_kunmap(struct dma_buf *buffer,
+		unsigned long page_num, void *addr)
+{
+	struct drm_gem_object *obj = buffer->priv;
+	struct page **pages;
+	omap_gem_get_pages(obj, &pages, false);
+	kunmap(pages[page_num]);
+}
+
+/*
+ * TODO maybe we can split up drm_gem_mmap to avoid duplicating
+ * some here.. or at least have a drm_dmabuf_mmap helper.
+ */
+static int omap_gem_dmabuf_mmap(struct dma_buf *buffer,
+		struct vm_area_struct *vma)
+{
+	struct drm_gem_object *obj = buffer->priv;
+	int ret = 0;
+
+	if (WARN_ON(!obj->filp))
+		return -EINVAL;
+
+	/* Check for valid size. */
+	if (omap_gem_mmap_size(obj) < vma->vm_end - vma->vm_start) {
+		ret = -EINVAL;
+		goto out_unlock;
+	}
+
+	if (!obj->dev->driver->gem_vm_ops) {
+		ret = -EINVAL;
+		goto out_unlock;
+	}
+
+	vma->vm_flags |= VM_RESERVED | VM_IO | VM_PFNMAP | VM_DONTEXPAND;
+	vma->vm_ops = obj->dev->driver->gem_vm_ops;
+	vma->vm_private_data = obj;
+	vma->vm_page_prot =  pgprot_writecombine(vm_get_page_prot(vma->vm_flags));
+
+	/* Take a ref for this mapping of the object, so that the fault
+	 * handler can dereference the mmap offset's pointer to the object.
+	 * This reference is cleaned up by the corresponding vm_close
+	 * (which should happen whether the vma was created by this call, or
+	 * by a vm_open due to mremap or partial unmap or whatever).
+	 */
+	vma->vm_ops->open(vma);
+
+out_unlock:
+
+	return omap_gem_mmap_obj(obj, vma);
+}
+
+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,
+		.begin_cpu_access = omap_gem_dmabuf_begin_cpu_access,
+		.end_cpu_access = omap_gem_dmabuf_end_cpu_access,
+		.kmap_atomic = omap_gem_dmabuf_kmap_atomic,
+		.kunmap_atomic = omap_gem_dmabuf_kunmap_atomic,
+		.kmap = omap_gem_dmabuf_kmap,
+		.kunmap = omap_gem_dmabuf_kunmap,
+		.mmap = omap_gem_dmabuf_mmap,
+};
+
+struct dma_buf * omap_gem_prime_export(struct drm_device *dev,
+		struct drm_gem_object *obj, int flags)
+{
+	return dma_buf_export(obj, &omap_dmabuf_ops, obj->size, 0600);
+}
+
+struct drm_gem_object * omap_gem_prime_import(struct drm_device *dev,
+		struct dma_buf *buffer)
+{
+	struct drm_gem_object *obj;
+
+	/* is this one of own objects? */
+	if (buffer->ops == &omap_dmabuf_ops) {
+		obj = buffer->priv;
+		/* is it from our device? */
+		if (obj->dev == dev) {
+			drm_gem_object_reference(obj);
+			return obj;
+		}
+	}
+
+	/*
+	 * TODO add support for importing buffers from other devices..
+	 * for now we don't need this but would be nice to add eventually
+	 */
+	return ERR_PTR(-EINVAL);
+}
diff --git a/drivers/staging/omapdrm/tcm-sita.c b/drivers/staging/omapdrm/tcm-sita.c
index 10d5ac3dae4..efb60951054 100644
--- a/drivers/staging/omapdrm/tcm-sita.c
+++ b/drivers/staging/omapdrm/tcm-sita.c
@@ -200,7 +200,7 @@ static s32 sita_reserve_1d(struct tcm *tcm, u32 num_slots,
  *
  * @param w	width
  * @param h	height
- * @param area	pointer to the area that will be populated with the reesrved
+ * @param area	pointer to the area that will be populated with the reserved
  *		area
  *
  * @return 0 on success, non-0 error value on failure.
diff --git a/drivers/staging/ozwpan/README b/drivers/staging/ozwpan/README
index bb1a69b9454..7c055ec9954 100644
--- a/drivers/staging/ozwpan/README
+++ b/drivers/staging/ozwpan/README
@@ -9,7 +9,7 @@ technology.
 
 To operate the driver must be bound to a suitable network interface. This can
 be done when the module is loaded (specifying the name of the network interface
-as a paramter - e.g. 'insmod ozwpan g_net_dev=go0') or can be bound after
+as a parameter - e.g. 'insmod ozwpan g_net_dev=go0') or can be bound after
 loading using an ioctl call. See the ozappif.h file and the ioctls
 OZ_IOCTL_ADD_BINDING and OZ_IOCTL_REMOVE_BINDING.
 
diff --git a/drivers/staging/ozwpan/ozappif.h b/drivers/staging/ozwpan/ozappif.h
index af027329387..1b59b0748c6 100644
--- a/drivers/staging/ozwpan/ozappif.h
+++ b/drivers/staging/ozwpan/ozappif.h
@@ -11,13 +11,13 @@
 #define OZ_IOCTL_MAGIC	0xf4
 
 struct oz_mac_addr {
-	unsigned char a[6];
+	__u8 a[6];
 };
 
 #define OZ_MAX_PDS	8
 
 struct oz_pd_list {
-	int count;
+	__u32 count;
 	struct oz_mac_addr addr[OZ_MAX_PDS];
 };
 
@@ -27,18 +27,10 @@ struct oz_binding_info {
 	char name[OZ_MAX_BINDING_LEN];
 };
 
-struct oz_test {
-	int action;
-};
-
 #define OZ_IOCTL_GET_PD_LIST	_IOR(OZ_IOCTL_MAGIC, 0, struct oz_pd_list)
 #define OZ_IOCTL_SET_ACTIVE_PD	_IOW(OZ_IOCTL_MAGIC, 1, struct oz_mac_addr)
 #define OZ_IOCTL_GET_ACTIVE_PD	_IOR(OZ_IOCTL_MAGIC, 2, struct oz_mac_addr)
-#define OZ_IOCTL_CLEAR_EVENTS	_IO(OZ_IOCTL_MAGIC, 3)
-#define OZ_IOCTL_GET_EVENTS	_IOR(OZ_IOCTL_MAGIC, 4, struct oz_evtlist)
 #define OZ_IOCTL_ADD_BINDING	_IOW(OZ_IOCTL_MAGIC, 5, struct oz_binding_info)
-#define OZ_IOCTL_TEST		_IOWR(OZ_IOCTL_MAGIC, 6, struct oz_test)
-#define OZ_IOCTL_SET_EVENT_MASK	_IOW(OZ_IOCTL_MAGIC, 7, unsigned long)
 #define OZ_IOCTL_REMOVE_BINDING	_IOW(OZ_IOCTL_MAGIC, 8, struct oz_binding_info)
 #define OZ_IOCTL_MAX		9
 
diff --git a/drivers/staging/ozwpan/ozcdev.c b/drivers/staging/ozwpan/ozcdev.c
index 1c380d68796..27325f74ecd 100644
--- a/drivers/staging/ozwpan/ozcdev.c
+++ b/drivers/staging/ozwpan/ozcdev.c
@@ -41,9 +41,6 @@ struct oz_serial_ctx {
 };
 /*------------------------------------------------------------------------------
  */
-int g_taction;
-/*------------------------------------------------------------------------------
- */
 static struct oz_cdev g_cdev;
 /*------------------------------------------------------------------------------
  * Context: process and softirq
@@ -276,20 +273,6 @@ long oz_cdev_ioctl(struct file *filp, unsigned int cmd, unsigned long arg)
 				return -EFAULT;
 		}
 		break;
-#ifdef WANT_EVENT_TRACE
-	case OZ_IOCTL_CLEAR_EVENTS:
-		oz_events_clear();
-		break;
-	case OZ_IOCTL_GET_EVENTS:
-		rc = oz_events_copy((void __user *)arg);
-		break;
-	case OZ_IOCTL_SET_EVENT_MASK:
-		if (copy_from_user(&g_evt_mask, (void __user *)arg,
-			sizeof(unsigned long))) {
-			return -EFAULT;
-		}
-		break;
-#endif /* WANT_EVENT_TRACE */
 	case OZ_IOCTL_ADD_BINDING:
 	case OZ_IOCTL_REMOVE_BINDING: {
 			struct oz_binding_info b;
diff --git a/drivers/staging/ozwpan/ozevent.c b/drivers/staging/ozwpan/ozevent.c
index 73703d3e96b..7f66b4f19b0 100644
--- a/drivers/staging/ozwpan/ozevent.c
+++ b/drivers/staging/ozwpan/ozevent.c
@@ -5,29 +5,46 @@
  */
 #include "ozconfig.h"
 #ifdef WANT_EVENT_TRACE
+#include <linux/module.h>
+#include <linux/debugfs.h>
 #include <linux/jiffies.h>
 #include <linux/uaccess.h>
 #include "oztrace.h"
 #include "ozevent.h"
+#include "ozappif.h"
 /*------------------------------------------------------------------------------
+ * Although the event mask is logically part of the oz_evtdev structure, it is
+ * needed outside of this file so define it seperately to avoid the need to
+ * export definition of struct oz_evtdev.
  */
-unsigned long g_evt_mask = 0xffffffff;
+u32 g_evt_mask;
 /*------------------------------------------------------------------------------
  */
 #define OZ_MAX_EVTS	2048	/* Must be power of 2 */
-DEFINE_SPINLOCK(g_eventlock);
-static int g_evt_in;
-static int g_evt_out;
-static int g_missed_events;
-static struct oz_event g_events[OZ_MAX_EVTS];
+struct oz_evtdev {
+	struct dentry *root_dir;
+	int evt_in;
+	int evt_out;
+	int missed_events;
+	int present;
+	atomic_t users;
+	spinlock_t lock;
+	struct oz_event evts[OZ_MAX_EVTS];
+};
+
+static struct oz_evtdev g_evtdev;
+
 /*------------------------------------------------------------------------------
  * Context: process
  */
 void oz_event_init(void)
 {
+	/* Because g_evtdev is static external all fields initally zero so no
+	 * need to reinitialised those.
+	 */
 	oz_trace("Event tracing initialized\n");
-	g_evt_in = g_evt_out = 0;
-	g_missed_events = 0;
+	spin_lock_init(&g_evtdev.lock);
+	atomic_set(&g_evtdev.users, 0);
 }
 /*------------------------------------------------------------------------------
  * Context: process
@@ -43,74 +60,136 @@ void oz_event_log2(u8 evt, u8 ctx1, u16 ctx2, void *ctx3, unsigned ctx4)
 {
 	unsigned long irqstate;
 	int ix;
-	spin_lock_irqsave(&g_eventlock, irqstate);
-	ix = (g_evt_in + 1) & (OZ_MAX_EVTS - 1);
-	if (ix != g_evt_out) {
-		struct oz_event *e = &g_events[g_evt_in];
+	spin_lock_irqsave(&g_evtdev.lock, irqstate);
+	ix = (g_evtdev.evt_in + 1) & (OZ_MAX_EVTS - 1);
+	if (ix != g_evtdev.evt_out) {
+		struct oz_event *e = &g_evtdev.evts[g_evtdev.evt_in];
 		e->jiffies = jiffies;
 		e->evt = evt;
 		e->ctx1 = ctx1;
 		e->ctx2 = ctx2;
-		e->ctx3 = ctx3;
+		e->ctx3 = (__u32)(unsigned long)ctx3;
 		e->ctx4 = ctx4;
-		g_evt_in = ix;
+		g_evtdev.evt_in = ix;
 	} else {
-		g_missed_events++;
+		g_evtdev.missed_events++;
 	}
-	spin_unlock_irqrestore(&g_eventlock, irqstate);
+	spin_unlock_irqrestore(&g_evtdev.lock, irqstate);
 }
 /*------------------------------------------------------------------------------
  * Context: process
  */
-int oz_events_copy(struct oz_evtlist __user *lst)
+static void oz_events_clear(struct oz_evtdev *dev)
 {
-	int first;
-	int ix;
-	struct hdr {
-		int count;
-		int missed;
-	} hdr;
-	ix = g_evt_out;
-	hdr.count = g_evt_in - ix;
-	if (hdr.count < 0)
-		hdr.count += OZ_MAX_EVTS;
-	if (hdr.count > OZ_EVT_LIST_SZ)
-		hdr.count = OZ_EVT_LIST_SZ;
-	hdr.missed = g_missed_events;
-	g_missed_events = 0;
-	if (copy_to_user((void __user *)lst, &hdr, sizeof(hdr)))
-		return -EFAULT;
-	first = OZ_MAX_EVTS - ix;
-	if (first > hdr.count)
-		first = hdr.count;
-	if (first) {
-		int sz = first*sizeof(struct oz_event);
-		void __user *p = (void __user *)lst->evts;
-		if (copy_to_user(p, &g_events[ix], sz))
-			return -EFAULT;
-		if (hdr.count > first) {
-			p = (void __user *)&lst->evts[first];
-			sz = (hdr.count-first)*sizeof(struct oz_event);
-			if (copy_to_user(p, g_events, sz))
-				return -EFAULT;
-		}
+	unsigned long irqstate;
+	oz_trace("Clearing events\n");
+	spin_lock_irqsave(&dev->lock, irqstate);
+	dev->evt_in = dev->evt_out = 0;
+	dev->missed_events = 0;
+	spin_unlock_irqrestore(&dev->lock, irqstate);
+}
+#ifdef CONFIG_DEBUG_FS
+/*------------------------------------------------------------------------------
+ * Context: process
+ */
+int oz_events_open(struct inode *inode, struct file *filp)
+{
+	oz_trace("oz_evt_open()\n");
+	oz_trace("Open flags: 0x%x\n", filp->f_flags);
+	if (atomic_add_return(1, &g_evtdev.users) == 1) {
+		oz_events_clear(&g_evtdev);
+		return nonseekable_open(inode, filp);
+	} else {
+		atomic_dec(&g_evtdev.users);
+		return -EBUSY;
 	}
-	ix += hdr.count;
-	if (ix >= OZ_MAX_EVTS)
-		ix -= OZ_MAX_EVTS;
-	g_evt_out = ix;
+}
+/*------------------------------------------------------------------------------
+ * Context: process
+ */
+int oz_events_release(struct inode *inode, struct file *filp)
+{
+	oz_events_clear(&g_evtdev);
+	atomic_dec(&g_evtdev.users);
+	g_evt_mask = 0;
+	oz_trace("oz_evt_release()\n");
 	return 0;
 }
 /*------------------------------------------------------------------------------
  * Context: process
  */
-void oz_events_clear(void)
+ssize_t oz_events_read(struct file *filp, char __user *buf, size_t count,
+		loff_t *fpos)
 {
-	unsigned long irqstate;
-	spin_lock_irqsave(&g_eventlock, irqstate);
-	g_evt_in = g_evt_out = 0;
-	g_missed_events = 0;
-	spin_unlock_irqrestore(&g_eventlock, irqstate);
+	struct oz_evtdev *dev = &g_evtdev;
+	int rc = 0;
+	int nb_evts = count / sizeof(struct oz_event);
+	int n;
+	int sz;
+
+	n = dev->evt_in - dev->evt_out;
+	if (n < 0)
+		n += OZ_MAX_EVTS;
+	if (nb_evts > n)
+		nb_evts = n;
+	if (nb_evts == 0)
+		goto out;
+	n = OZ_MAX_EVTS - dev->evt_out;
+	if (n > nb_evts)
+		n = nb_evts;
+	sz = n * sizeof(struct oz_event);
+	if (copy_to_user(buf, &dev->evts[dev->evt_out], sz)) {
+		rc = -EFAULT;
+		goto out;
+	}
+	if (n == nb_evts)
+		goto out2;
+	n = nb_evts - n;
+	if (copy_to_user(buf + sz, dev->evts, n * sizeof(struct oz_event))) {
+		rc = -EFAULT;
+		goto out;
+	}
+out2:
+	dev->evt_out = (dev->evt_out + nb_evts) & (OZ_MAX_EVTS - 1);
+	rc = nb_evts * sizeof(struct oz_event);
+out:
+	return rc;
 }
-#endif /* WANT_EVENT_TRACE */
+/*------------------------------------------------------------------------------
+ */
+const struct file_operations oz_events_fops = {
+	.owner =	THIS_MODULE,
+	.open =		oz_events_open,
+	.release =	oz_events_release,
+	.read =		oz_events_read,
+};
+/*------------------------------------------------------------------------------
+ * Context: process
+ */
+void oz_debugfs_init(void)
+{
+	struct dentry *parent;
 
+	parent = debugfs_create_dir("ozwpan", NULL);
+	if (parent  == NULL) {
+		oz_trace("Failed to create debugfs directory ozmo\n");
+		return;
+	} else {
+		g_evtdev.root_dir = parent;
+		if (debugfs_create_file("events", S_IRUSR, parent, NULL,
+						&oz_events_fops) == NULL)
+			oz_trace("Failed to create file ozmo/events\n");
+		if (debugfs_create_x32("event_mask", S_IRUSR | S_IWUSR, parent,
+							&g_evt_mask) == NULL)
+			oz_trace("Failed to create file ozmo/event_mask\n");
+	}
+}
+/*------------------------------------------------------------------------------
+ * Context: process
+ */
+void oz_debugfs_remove(void)
+{
+	debugfs_remove_recursive(g_evtdev.root_dir);
+}
+#endif /* CONFIG_DEBUG_FS */
+#endif /* WANT_EVENT_TRACE */
diff --git a/drivers/staging/ozwpan/ozevent.h b/drivers/staging/ozwpan/ozevent.h
index f033d014c6f..32f6f9859c4 100644
--- a/drivers/staging/ozwpan/ozevent.h
+++ b/drivers/staging/ozwpan/ozevent.h
@@ -9,23 +9,24 @@
 #include "ozeventdef.h"
 
 #ifdef WANT_EVENT_TRACE
-extern unsigned long g_evt_mask;
+extern u32 g_evt_mask;
 void oz_event_init(void);
 void oz_event_term(void);
 void oz_event_log2(u8 evt, u8 ctx1, u16 ctx2, void *ctx3, unsigned ctx4);
+void oz_debugfs_init(void);
+void oz_debugfs_remove(void);
 #define oz_event_log(__evt, __ctx1, __ctx2, __ctx3, __ctx4) \
 	do { \
 		if ((1<<(__evt)) & g_evt_mask) \
 			oz_event_log2(__evt, __ctx1, __ctx2, __ctx3, __ctx4); \
 	} while (0)
-int oz_events_copy(struct oz_evtlist __user *lst);
-void oz_events_clear(void);
+
 #else
 #define oz_event_init()
 #define oz_event_term()
 #define oz_event_log(__evt, __ctx1, __ctx2, __ctx3, __ctx4)
-#define oz_events_copy(__lst)
-#define oz_events_clear()
+#define oz_debugfs_init()
+#define oz_debugfs_remove()
 #endif /* WANT_EVENT_TRACE */
 
 #endif /* _OZEVENT_H */
diff --git a/drivers/staging/ozwpan/ozeventdef.h b/drivers/staging/ozwpan/ozeventdef.h
index a880288bab1..4b938981671 100644
--- a/drivers/staging/ozwpan/ozeventdef.h
+++ b/drivers/staging/ozwpan/ozeventdef.h
@@ -29,19 +29,12 @@
 #define OZ_EVT_DEBUG		20
 
 struct oz_event {
-	unsigned long jiffies;
-	unsigned char evt;
-	unsigned char ctx1;
-	unsigned short ctx2;
-	void *ctx3;
-	unsigned ctx4;
-};
-
-#define OZ_EVT_LIST_SZ	64
-struct oz_evtlist {
-	int count;
-	int missed;
-	struct oz_event evts[OZ_EVT_LIST_SZ];
+	__u32 jiffies;
+	__u8 evt;
+	__u8 ctx1;
+	__u16 ctx2;
+	__u32 ctx3;
+	__u32 ctx4;
 };
 
 #endif /* _OZEVENTDEF_H */
diff --git a/drivers/staging/ozwpan/ozhcd.c b/drivers/staging/ozwpan/ozhcd.c
index 750b14eb505..251f07c39a6 100644
--- a/drivers/staging/ozwpan/ozhcd.c
+++ b/drivers/staging/ozwpan/ozhcd.c
@@ -1416,7 +1416,7 @@ static void oz_process_ep0_urb(struct oz_hcd *ozhcd, struct urb *urb,
 			oz_trace("USB_REQ_SET_CONFIGURATION - req\n");
 			break;
 		case USB_REQ_GET_CONFIGURATION:
-			/* We short curcuit this case and reply directly since
+			/* We short circuit this case and reply directly since
 			 * we have the selected configuration number cached.
 			 */
 			oz_event_log(OZ_EVT_CTRL_LOCAL, setup->bRequest, 0, 0,
@@ -1432,7 +1432,7 @@ static void oz_process_ep0_urb(struct oz_hcd *ozhcd, struct urb *urb,
 			}
 			break;
 		case USB_REQ_GET_INTERFACE:
-			/* We short curcuit this case and reply directly since
+			/* We short circuit this case and reply directly since
 			 * we have the selected interface alternative cached.
 			 */
 			oz_event_log(OZ_EVT_CTRL_LOCAL, setup->bRequest, 0, 0,
@@ -1463,7 +1463,7 @@ static void oz_process_ep0_urb(struct oz_hcd *ozhcd, struct urb *urb,
 			rc = -ENOMEM;
 		} else {
 			/* Note: we are queuing the request after we have
-			 * submitted it to be tranmitted. If the request were
+			 * submitted it to be transmitted. If the request were
 			 * to complete before we queued it then it would not
 			 * be found in the queue. It seems impossible for
 			 * this to happen but if it did the request would
diff --git a/drivers/staging/ozwpan/ozmain.c b/drivers/staging/ozwpan/ozmain.c
index aaf2ccc0bcf..7579645d642 100644
--- a/drivers/staging/ozwpan/ozmain.c
+++ b/drivers/staging/ozwpan/ozmain.c
@@ -33,6 +33,9 @@ static int __init ozwpan_init(void)
 	oz_protocol_init(g_net_dev);
 	oz_app_enable(OZ_APPID_USB, 1);
 	oz_apps_init();
+#ifdef CONFIG_DEBUG_FS
+	oz_debugfs_init();
+#endif
 	return 0;
 }
 /*------------------------------------------------------------------------------
@@ -44,6 +47,9 @@ static void __exit ozwpan_exit(void)
 	oz_apps_term();
 	oz_cdev_deregister();
 	oz_event_term();
+#ifdef CONFIG_DEBUG_FS
+	oz_debugfs_remove();
+#endif
 }
 /*------------------------------------------------------------------------------
  */
@@ -53,6 +59,6 @@ module_exit(ozwpan_exit);
 
 MODULE_AUTHOR("Chris Kelly");
 MODULE_DESCRIPTION("Ozmo Devices USB over WiFi hcd driver");
-MODULE_VERSION("1.0.8");
+MODULE_VERSION("1.0.9");
 MODULE_LICENSE("GPL");
 
diff --git a/drivers/staging/ozwpan/ozusbsvc.c b/drivers/staging/ozwpan/ozusbsvc.c
index 9e74f960238..8fa7f256ad8 100644
--- a/drivers/staging/ozwpan/ozusbsvc.c
+++ b/drivers/staging/ozwpan/ozusbsvc.c
@@ -7,7 +7,7 @@
  * The implementation of this service is split into two parts the first of which
  * is protocol independent and the second contains protocol specific details.
  * This split is to allow alternative protocols to be defined.
- * The implemenation of this service uses ozhcd.c to implement a USB HCD.
+ * The implementation of this service uses ozhcd.c to implement a USB HCD.
  * -----------------------------------------------------------------------------
  */
 #include <linux/init.h>
diff --git a/drivers/staging/panel/panel.c b/drivers/staging/panel/panel.c
index 6183573f112..7365089a33e 100644
--- a/drivers/staging/panel/panel.c
+++ b/drivers/staging/panel/panel.c
@@ -754,7 +754,7 @@ static void lcd_backlight(int on)
 	if (lcd_bl_pin == PIN_NONE)
 		return;
 
-	/* The backlight is activated by seting the AUTOFEED line to +5V  */
+	/* The backlight is activated by setting the AUTOFEED line to +5V  */
 	spin_lock(&pprt_lock);
 	bits.bl = on;
 	panel_set_bits();
diff --git a/drivers/staging/quatech_usb2/Kconfig b/drivers/staging/quatech_usb2/Kconfig
deleted file mode 100644
index 1494f42f3da..00000000000
--- a/drivers/staging/quatech_usb2/Kconfig
+++ /dev/null
@@ -1,15 +0,0 @@
-config USB_SERIAL_QUATECH_USB2
-	tristate "USB Quatech xSU2-[14]00 USB Serial Driver"
-	depends on USB_SERIAL
-	help
-	  Say Y here if you want to use a Quatech USB2.0 to serial adaptor. This
-	  driver supports the SSU2-100, DSU2-100, DSU2-400, QSU2-100, QSU2-400,
-	  ESU2-400 and ESU2-100 USB2.0 to RS232 / 485 / 422 serial adaptors.
-
-	  Some hardware has an incorrect product string and announces itself as
-	  ESU-100 (which uses the serqt driver) even though it is an ESU2-100.
-	  Check the label on the bottom of your device.
-
-	  To compile this driver as a module, choose M here: the module will be
-	  called quatech_usb2 .
-
diff --git a/drivers/staging/quatech_usb2/Makefile b/drivers/staging/quatech_usb2/Makefile
deleted file mode 100644
index bcd1f890d16..00000000000
--- a/drivers/staging/quatech_usb2/Makefile
+++ /dev/null
@@ -1 +0,0 @@
-obj-$(CONFIG_USB_SERIAL_QUATECH_USB2)		+= quatech_usb2.o
diff --git a/drivers/staging/quatech_usb2/TODO b/drivers/staging/quatech_usb2/TODO
deleted file mode 100644
index 67f61dbe14a..00000000000
--- a/drivers/staging/quatech_usb2/TODO
+++ /dev/null
@@ -1,8 +0,0 @@
-Incomplete list of things that this driver does not yet implement completely or
-at all. some of these may not be possible to implement because the hardware
-support does not exist. Others may be possible, but the magic control codes to
-make them happen are unknown, and some may just need the driver support to
-implement them writing.
-
-* Mark/Space parity is not implemented (reported back correctly)
-* IXANY flow control mode is not implemented (flag ignored completely)
diff --git a/drivers/staging/quatech_usb2/quatech_usb2.c b/drivers/staging/quatech_usb2/quatech_usb2.c
deleted file mode 100644
index bb977e00cc8..00000000000
--- a/drivers/staging/quatech_usb2/quatech_usb2.c
+++ /dev/null
@@ -1,1976 +0,0 @@
-/*
- * Driver for Quatech Inc USB2.0 to serial adaptors. Largely unrelated to the
- * serqt_usb driver, based on a re-write of the vendor supplied serqt_usb2 code,
- * which is unrelated to the serqt_usb2 in the staging kernel
- */
-
-#include <linux/errno.h>
-#include <linux/init.h>
-#include <linux/slab.h>
-#include <linux/tty.h>
-#include <linux/tty_driver.h>
-#include <linux/tty_flip.h>
-#include <linux/module.h>
-#include <linux/serial.h>
-#include <linux/usb.h>
-#include <linux/usb/serial.h>
-#include <linux/uaccess.h>
-
-static bool debug;
-
-/* Version Information */
-#define DRIVER_VERSION "v2.00"
-#define DRIVER_AUTHOR "Tim Gobeli, Quatech, Inc"
-#define DRIVER_DESC "Quatech USB 2.0 to Serial Driver"
-
-/* vendor and device IDs */
-#define	USB_VENDOR_ID_QUATECH 0x061d	/* Quatech VID */
-#define QUATECH_SSU2_100 0xC120		/* RS232 single port */
-#define QUATECH_DSU2_100 0xC140		/* RS232 dual port */
-#define QUATECH_DSU2_400 0xC150		/* RS232/422/485 dual port */
-#define QUATECH_QSU2_100 0xC160		/* RS232 four port */
-#define QUATECH_QSU2_400 0xC170		/* RS232/422/485 four port */
-#define QUATECH_ESU2_100 0xC1A0		/* RS232 eight port */
-#define QUATECH_ESU2_400 0xC180		/* RS232/422/485 eight port */
-
-/* magic numbers go here, when we find out which ones are needed */
-
-#define QU2BOXPWRON 0x8000		/* magic number to turn FPGA power on */
-#define QU2BOX232 0x40			/* RS232 mode on MEI devices */
-#define QU2BOXSPD9600 0x60		/* set speed to 9600 baud */
-#define QT2_FIFO_DEPTH 1024			/* size of hardware fifos */
-#define QT2_TX_HEADER_LENGTH	5
-/* length of the header sent to the box with each write URB */
-
-/* directions for USB transfers */
-#define USBD_TRANSFER_DIRECTION_IN    0xc0
-#define USBD_TRANSFER_DIRECTION_OUT   0x40
-
-/* special Quatech command IDs. These are pushed down the
- USB control pipe to get the box on the end to do things */
-#define QT_SET_GET_DEVICE		0xc2
-#define QT_OPEN_CLOSE_CHANNEL		0xca
-/*#define QT_GET_SET_PREBUF_TRIG_LVL	0xcc
-#define QT_SET_ATF			0xcd*/
-#define QT2_GET_SET_REGISTER			0xc0
-#define QT2_GET_SET_UART			0xc1
-#define QT2_HW_FLOW_CONTROL_MASK		0xc5
-#define QT2_SW_FLOW_CONTROL_MASK		0xc6
-#define QT2_SW_FLOW_CONTROL_DISABLE		0xc7
-#define QT2_BREAK_CONTROL			0xc8
-#define QT2_STOP_RECEIVE			0xe0
-#define QT2_FLUSH_DEVICE			0xc4
-#define QT2_GET_SET_QMCR			0xe1
-
-/* sorts of flush we can do on */
-#define QT2_FLUSH_RX			0x00
-#define QT2_FLUSH_TX			0x01
-
-/* port setting constants, used to set up serial port speeds, flow
- * control and so on */
-#define QT2_SERIAL_MCR_DTR	0x01
-#define QT2_SERIAL_MCR_RTS	0x02
-#define QT2_SERIAL_MCR_LOOP	0x10
-
-#define QT2_SERIAL_MSR_CTS	0x10
-#define QT2_SERIAL_MSR_CD	0x80
-#define QT2_SERIAL_MSR_RI	0x40
-#define QT2_SERIAL_MSR_DSR	0x20
-#define QT2_SERIAL_MSR_MASK	0xf0
-
-#define QT2_SERIAL_8_DATA	0x03
-#define QT2_SERIAL_7_DATA	0x02
-#define QT2_SERIAL_6_DATA	0x01
-#define QT2_SERIAL_5_DATA	0x00
-
-#define QT2_SERIAL_ODD_PARITY	0x08
-#define QT2_SERIAL_EVEN_PARITY	0x18
-#define QT2_SERIAL_TWO_STOPB	0x04
-#define QT2_SERIAL_ONE_STOPB	0x00
-
-#define QT2_MAX_BAUD_RATE	921600
-#define QT2_MAX_BAUD_REMAINDER	4608
-
-#define QT2_SERIAL_LSR_OE	0x02
-#define QT2_SERIAL_LSR_PE	0x04
-#define QT2_SERIAL_LSR_FE	0x08
-#define QT2_SERIAL_LSR_BI	0x10
-
-/* value of Line Status Register when UART has completed
- * emptying data out on the line */
-#define QT2_LSR_TEMT     0x40
-
-/* register numbers on each UART, for use with  qt2_box_[get|set]_register*/
-#define  QT2_XMT_HOLD_REGISTER          0x00
-#define  QT2_XVR_BUFFER_REGISTER        0x00
-#define  QT2_FIFO_CONTROL_REGISTER      0x02
-#define  QT2_LINE_CONTROL_REGISTER      0x03
-#define  QT2_MODEM_CONTROL_REGISTER     0x04
-#define  QT2_LINE_STATUS_REGISTER       0x05
-#define  QT2_MODEM_STATUS_REGISTER      0x06
-
-/* handy macros for doing escape sequence parsing on data reads */
-#define THISCHAR	((unsigned char *)(urb->transfer_buffer))[i]
-#define NEXTCHAR	((unsigned char *)(urb->transfer_buffer))[i + 1]
-#define THIRDCHAR	((unsigned char *)(urb->transfer_buffer))[i + 2]
-#define FOURTHCHAR	((unsigned char *)(urb->transfer_buffer))[i + 3]
-#define FIFTHCHAR	((unsigned char *)(urb->transfer_buffer))[i + 4]
-
-static const struct usb_device_id quausb2_id_table[] = {
-	{USB_DEVICE(USB_VENDOR_ID_QUATECH, QUATECH_SSU2_100)},
-	{USB_DEVICE(USB_VENDOR_ID_QUATECH, QUATECH_DSU2_100)},
-	{USB_DEVICE(USB_VENDOR_ID_QUATECH, QUATECH_DSU2_400)},
-	{USB_DEVICE(USB_VENDOR_ID_QUATECH, QUATECH_QSU2_100)},
-	{USB_DEVICE(USB_VENDOR_ID_QUATECH, QUATECH_QSU2_400)},
-	{USB_DEVICE(USB_VENDOR_ID_QUATECH, QUATECH_ESU2_100)},
-	{USB_DEVICE(USB_VENDOR_ID_QUATECH, QUATECH_ESU2_400)},
-	{}	/* Terminating entry */
-};
-
-MODULE_DEVICE_TABLE(usb, quausb2_id_table);
-
-/* custom structures we need go here */
-static struct usb_driver quausb2_usb_driver = {
-	.name = "quatech-usb2-serial",
-	.probe = usb_serial_probe,
-	.disconnect = usb_serial_disconnect,
-	.id_table = quausb2_id_table,
-};
-
-/**
- * quatech2_port: Structure in which to keep all the messy stuff that this
- * driver needs alongside the usb_serial_port structure
- * @read_urb_busy: Flag indicating that port->read_urb is in use
- * @close_pending: flag indicating that this port is in the process of
- * being closed (and so no new reads / writes should be started).
- * @shadowLSR: Last received state of the line status register, holds the
- * value of the line status flags from the port
- * @shadowMSR: Last received state of the modem status register, holds
- * the value of the modem status received from the port
- * @rcv_flush: Flag indicating that a receive flush has occurred on
- * the hardware.
- * @xmit_flush: Flag indicating that a transmit flush has been processed by
- * the hardware.
- * @tx_pending_bytes: Number of bytes waiting to be sent. This total
- * includes the size (excluding header) of URBs that have been submitted but
- * have not yet been sent to to the device, and bytes that have been sent out
- * of the port but not yet reported sent by the "xmit_empty" messages (which
- * indicate the number of bytes sent each time they are received, despite the
- * misleading name).
- * - Starts at zero when port is initialised.
- * - is incremented by the size of the data to be written (no headers)
- * each time a write urb is dispatched.
- * - is decremented each time a "transmit empty" message is received
- * by the driver in the data stream.
- * @lock: Mutex to lock access to this structure when we need to ensure that
- * races don't occur to access bits of it.
- * @open_count: The number of uses of the port currently having
- * it open, i.e. the reference count.
- */
-struct quatech2_port {
-	int	magic;
-	bool	read_urb_busy;
-	bool	close_pending;
-	__u8	shadowLSR;
-	__u8	shadowMSR;
-	bool	rcv_flush;
-	bool	xmit_flush;
-	int	tx_pending_bytes;
-	struct mutex modelock;
-	int	open_count;
-
-	char	active;		/* someone has this device open */
-	unsigned char		*xfer_to_tty_buffer;
-	wait_queue_head_t	wait;
-	__u8	shadowLCR;	/* last LCR value received */
-	__u8	shadowMCR;	/* last MCR value received */
-	char	RxHolding;
-	struct semaphore	pend_xmit_sem;	/* locks this structure */
-	spinlock_t lock;
-};
-
-/**
- * Structure to hold device-wide internal status information
- * @param ReadBulkStopped The last bulk read attempt ended in tears
- * @param open_ports The number of serial ports currently in use on the box
- * @param current_port Pointer to the serial port structure of the port which
- * the read stream is currently directed to. Escape sequences in the read
- * stream will change this around as data arrives from different ports on the
- * box
- * @buffer_size: The max size buffer each URB can take, used to set the size of
- * the buffers allocated for writing to each port on the device (we need to
- * store this because it is known only to the endpoint, but used each time a
- * port is opened and a new buffer is allocated.
- */
-struct quatech2_dev {
-	bool	ReadBulkStopped;
-	char	open_ports;
-	struct usb_serial_port *current_port;
-	int	buffer_size;
-};
-
-/* structure which holds line and modem status flags */
-struct qt2_status_data {
-	__u8 line_status;
-	__u8 modem_status;
-};
-
-/* Function prototypes */
-static int qt2_boxpoweron(struct usb_serial *serial);
-static int qt2_boxsetQMCR(struct usb_serial *serial, __u16 Uart_Number,
-			__u8 QMCR_Value);
-static int port_paranoia_check(struct usb_serial_port *port,
-			const char *function);
-static int serial_paranoia_check(struct usb_serial *serial,
-			 const char *function);
-static inline struct quatech2_port *qt2_get_port_private(struct usb_serial_port
-			*port);
-static inline void qt2_set_port_private(struct usb_serial_port *port,
-			struct quatech2_port *data);
-static inline struct quatech2_dev *qt2_get_dev_private(struct usb_serial
-			*serial);
-static inline void qt2_set_dev_private(struct usb_serial *serial,
-			struct quatech2_dev *data);
-static int qt2_openboxchannel(struct usb_serial *serial, __u16
-			Uart_Number, struct qt2_status_data *pDeviceData);
-static int qt2_closeboxchannel(struct usb_serial *serial, __u16
-			Uart_Number);
-static int qt2_conf_uart(struct usb_serial *serial,  unsigned short Uart_Number,
-			 unsigned short divisor, unsigned char LCR);
-static void qt2_read_bulk_callback(struct urb *urb);
-static void qt2_write_bulk_callback(struct urb *urb);
-static void qt2_process_line_status(struct usb_serial_port *port,
-			      unsigned char LineStatus);
-static void qt2_process_modem_status(struct usb_serial_port *port,
-			       unsigned char ModemStatus);
-static void qt2_process_xmit_empty(struct usb_serial_port *port,
-	unsigned char fourth_char, unsigned char fifth_char);
-static void qt2_process_port_change(struct usb_serial_port *port,
-			      unsigned char New_Current_Port);
-static void qt2_process_rcv_flush(struct usb_serial_port *port);
-static void qt2_process_xmit_flush(struct usb_serial_port *port);
-static void qt2_process_rx_char(struct usb_serial_port *port,
-				unsigned char data);
-static int qt2_box_get_register(struct usb_serial *serial,
-		unsigned char uart_number, unsigned short register_num,
-		__u8 *pValue);
-static int qt2_box_set_register(struct usb_serial *serial,
-		unsigned short Uart_Number, unsigned short Register_Num,
-		unsigned short Value);
-static int qt2_boxsetuart(struct usb_serial *serial, unsigned short Uart_Number,
-		unsigned short default_divisor, unsigned char default_LCR);
-static int qt2_boxsethw_flowctl(struct usb_serial *serial,
-		unsigned int UartNumber, bool bSet);
-static int qt2_boxsetsw_flowctl(struct usb_serial *serial, __u16 UartNumber,
-		unsigned char stop_char,  unsigned char start_char);
-static int qt2_boxunsetsw_flowctl(struct usb_serial *serial, __u16 UartNumber);
-static int qt2_boxstoprx(struct usb_serial *serial, unsigned short uart_number,
-			 unsigned short stop);
-
-/* implementation functions, roughly in order of use, are here */
-static int qt2_calc_num_ports(struct usb_serial *serial)
-{
-	int num_ports;
-	int flag_as_400;
-	switch (serial->dev->descriptor.idProduct) {
-	case QUATECH_SSU2_100:
-		num_ports = 1;
-		break;
-
-	case QUATECH_DSU2_400:
-		flag_as_400 = true;
-	case QUATECH_DSU2_100:
-		num_ports = 2;
-	break;
-
-	case QUATECH_QSU2_400:
-		flag_as_400 = true;
-	case QUATECH_QSU2_100:
-		num_ports = 4;
-	break;
-
-	case QUATECH_ESU2_400:
-		flag_as_400 = true;
-	case QUATECH_ESU2_100:
-		num_ports = 8;
-	break;
-	default:
-	num_ports = 1;
-	break;
-	}
-	return num_ports;
-}
-
-static int qt2_attach(struct usb_serial *serial)
-{
-	struct usb_serial_port *port;
-	struct quatech2_port *qt2_port;	/* port-specific private data pointer */
-	struct quatech2_dev  *qt2_dev;	/* dev-specific private data pointer */
-	int i;
-	/* stuff for storing endpoint addresses now */
-	struct usb_endpoint_descriptor *endpoint;
-	struct usb_host_interface *iface_desc;
-	struct usb_serial_port *port0;	/* first port structure on device */
-
-	/* check how many endpoints there are on the device, for
-	 * sanity's sake */
-	dbg("%s(): Endpoints: %d bulk in, %d bulk out, %d interrupt in",
-			__func__, serial->num_bulk_in,
-			serial->num_bulk_out, serial->num_interrupt_in);
-	if ((serial->num_bulk_in != 1) || (serial->num_bulk_out != 1)) {
-		dbg("Device has wrong number of bulk endpoints!");
-		return -ENODEV;
-	}
-	iface_desc = serial->interface->cur_altsetting;
-
-	/* Set up per-device private data, storing extra data alongside
-	 * struct usb_serial */
-	qt2_dev = kzalloc(sizeof(*qt2_dev), GFP_KERNEL);
-	if (!qt2_dev) {
-		dbg("%s: kmalloc for quatech2_dev failed!",
-		    __func__);
-		return -ENOMEM;
-	}
-	qt2_dev->open_ports = 0;	/* no ports open */
-	qt2_set_dev_private(serial, qt2_dev);	/* store private data */
-
-	/* Now setup per port private data, which replaces all the things
-	 * that quatech added to standard kernel structures in their driver */
-	for (i = 0; i < serial->num_ports; i++) {
-		port = serial->port[i];
-		qt2_port = kzalloc(sizeof(*qt2_port), GFP_KERNEL);
-		if (!qt2_port) {
-			dbg("%s: kmalloc for quatech2_port (%d) failed!.",
-			    __func__, i);
-			return -ENOMEM;
-		}
-		/* initialise stuff in the structure */
-		qt2_port->open_count = 0;	/* port is not open */
-		spin_lock_init(&qt2_port->lock);
-		mutex_init(&qt2_port->modelock);
-		qt2_set_port_private(port, qt2_port);
-	}
-
-	/* gain access to port[0]'s structure because we want to store
-	 * device-level stuff in it */
-	if (serial_paranoia_check(serial, __func__))
-		return -ENODEV;
-	port0 = serial->port[0]; /* get the first port's device structure */
-
-	/* print endpoint addresses so we can check them later
-	 * by hand */
-	for (i = 0; i < iface_desc->desc.bNumEndpoints; ++i) {
-		endpoint = &iface_desc->endpoint[i].desc;
-		if ((endpoint->bEndpointAddress & 0x80) &&
-			((endpoint->bmAttributes & 3) == 0x02)) {
-			/* we found a bulk in endpoint */
-			dbg("found bulk in at %#.2x",
-				endpoint->bEndpointAddress);
-		}
-
-		if (((endpoint->bEndpointAddress & 0x80) == 0x00) &&
-			((endpoint->bmAttributes & 3) == 0x02)) {
-			/* we found a bulk out endpoint */
-			dbg("found bulk out at %#.2x",
-				endpoint->bEndpointAddress);
-			qt2_dev->buffer_size = endpoint->wMaxPacketSize;
-			/* max size of URB needs recording for the device */
-		}
-	}	/* end printing endpoint addresses */
-
-	/* switch on power to the hardware */
-	if (qt2_boxpoweron(serial) < 0) {
-		dbg("qt2_boxpoweron() failed");
-		goto startup_error;
-	}
-	/* set all ports to RS232 mode */
-	for (i = 0; i < serial->num_ports; ++i) {
-		if (qt2_boxsetQMCR(serial, i, QU2BOX232) < 0) {
-			dbg("qt2_boxsetQMCR() on port %d failed",
-				i);
-			goto startup_error;
-		}
-	}
-
-	return 0;
-
-startup_error:
-	for (i = 0; i < serial->num_ports; i++) {
-		port = serial->port[i];
-		qt2_port = qt2_get_port_private(port);
-		kfree(qt2_port);
-		qt2_set_port_private(port, NULL);
-	}
-	qt2_dev = qt2_get_dev_private(serial);
-	kfree(qt2_dev);
-	qt2_set_dev_private(serial, NULL);
-
-	dbg("Exit fail %s\n", __func__);
-	return -EIO;
-}
-
-static void qt2_release(struct usb_serial *serial)
-{
-	struct usb_serial_port *port;
-	struct quatech2_port *qt_port;
-	int i;
-
-	dbg("enterting %s", __func__);
-
-	for (i = 0; i < serial->num_ports; i++) {
-		port = serial->port[i];
-		if (!port)
-			continue;
-
-		qt_port = usb_get_serial_port_data(port);
-		kfree(qt_port);
-		usb_set_serial_port_data(port, NULL);
-	}
-}
-/* This function is called once per serial port on the device, when
- * that port is opened by a userspace application.
- * The tty_struct and the usb_serial_port belong to this port,
- * i.e. there are multiple ones for a multi-port device.
- * However the usb_serial_port structure has a back-pointer
- * to the parent usb_serial structure which belongs to the device,
- * so we can access either the device-wide information or
- * any other port's information (because there are also forward
- * pointers) via that pointer.
- * This is most helpful if the device shares resources (e.g. end
- * points) between different ports
- */
-int qt2_open(struct tty_struct *tty, struct usb_serial_port *port)
-{
-	struct usb_serial *serial;	/* device structure */
-	struct usb_serial_port *port0;	/* first port structure on device */
-	struct quatech2_port *port_extra;       /* extra data for this port */
-	struct quatech2_port *port0_extra;	/* extra data for first port */
-	struct quatech2_dev *dev_extra;		/* extra data for the device */
-	struct qt2_status_data ChannelData;
-	unsigned short default_divisor = QU2BOXSPD9600;
-	unsigned char  default_LCR = QT2_SERIAL_8_DATA;
-	int status;
-	int result;
-
-	if (port_paranoia_check(port, __func__))
-		return -ENODEV;
-
-	dbg("%s(): port %d", __func__, port->number);
-
-	serial = port->serial;	/* get the parent device structure */
-	if (serial_paranoia_check(serial, __func__)) {
-		dbg("usb_serial struct failed sanity check");
-		return -ENODEV;
-	}
-	dev_extra = qt2_get_dev_private(serial);
-	/* get the device private data */
-	if (dev_extra == NULL) {
-		dbg("device extra data pointer is null");
-		return -ENODEV;
-	}
-	port0 = serial->port[0]; /* get the first port's device structure */
-	if (port_paranoia_check(port0, __func__)) {
-		dbg("port0 usb_serial_port struct failed sanity check");
-		return -ENODEV;
-	}
-
-	port_extra = qt2_get_port_private(port);
-	port0_extra = qt2_get_port_private(port0);
-	if (port_extra == NULL || port0_extra == NULL) {
-		dbg("failed to get private data for port or port0");
-		return -ENODEV;
-	}
-
-	/* FIXME: are these needed?  Does it even do anything useful? */
-	/* get the modem and line status values from the UART */
-	status = qt2_openboxchannel(serial, port->number,
-			&ChannelData);
-	if (status < 0) {
-		dbg("qt2_openboxchannel on channel %d failed",
-		    port->number);
-		return status;
-	}
-	port_extra->shadowLSR = ChannelData.line_status &
-			(QT2_SERIAL_LSR_OE | QT2_SERIAL_LSR_PE |
-			QT2_SERIAL_LSR_FE | QT2_SERIAL_LSR_BI);
-	port_extra->shadowMSR = ChannelData.modem_status &
-			(QT2_SERIAL_MSR_CTS | QT2_SERIAL_MSR_DSR |
-			QT2_SERIAL_MSR_RI | QT2_SERIAL_MSR_CD);
-
-/*	port_extra->fifo_empty_flag = true;*/
-	dbg("qt2_openboxchannel on channel %d completed.",
-	    port->number);
-
-	/* Set Baud rate to default and turn off flow control here */
-	status = qt2_conf_uart(serial, port->number, default_divisor,
-				default_LCR);
-	if (status < 0) {
-		dbg("qt2_conf_uart() failed on channel %d",
-		    port->number);
-		return status;
-	}
-	dbg("qt2_conf_uart() completed on channel %d",
-		port->number);
-
-	/*
-	 * At this point we will need some end points to make further progress.
-	 * Handlily, the correct endpoint addresses have been filled out into
-	 * the usb_serial_port structure for us by the driver core, so we
-	 * already have access to them.
-	 * As there is only one bulk in and one bulk out end-point, these are in
-	 * port[0]'s structure, and the rest are uninitialised. Handily,
-	 * when we do a write to a port, we will use the same endpoint
-	 * regardless of the port, with a 5-byte header added on to
-	 * tell the box which port it should eventually come out of, so we only
-	 * need the one set of endpoints. We will have one URB per port for
-	 * writing, so that multiple ports can be writing at once.
-	 * Finally we need a bulk in URB to use for background reads from the
-	 * device, which will deal with uplink data from the box to host.
-	 */
-	dbg("port0 bulk in endpoint is %#.2x", port0->bulk_in_endpointAddress);
-	dbg("port0 bulk out endpoint is %#.2x",
-		port0->bulk_out_endpointAddress);
-
-	/* set up write_urb for bulk out transfers on this port. The USB
-	 * serial framework will have allocated a blank URB, buffer etc for
-	 * port0 when it put the endpoints there, but not for any of the other
-	 * ports on the device because there are no more endpoints. Thus we
-	 * have to allocate our own URBs for ports 1-7
-	 */
-	if (port->write_urb == NULL) {
-		dbg("port->write_urb == NULL, allocating one");
-		port->write_urb = usb_alloc_urb(0, GFP_KERNEL);
-		if (!port->write_urb) {
-			err("Allocating write URB failed");
-			return -ENOMEM;
-		}
-		/* buffer same size as port0 */
-		port->bulk_out_size = dev_extra->buffer_size;
-		port->bulk_out_buffer = kmalloc(port->bulk_out_size,
-						GFP_KERNEL);
-		if (!port->bulk_out_buffer) {
-			err("Couldn't allocate bulk_out_buffer");
-			return -ENOMEM;
-		}
-	}
-	if (serial->dev == NULL)
-		dbg("serial->dev == NULL");
-	dbg("port->bulk_out_size is %d", port->bulk_out_size);
-
-	usb_fill_bulk_urb(port->write_urb, serial->dev,
-			usb_sndbulkpipe(serial->dev,
-			port0->bulk_out_endpointAddress),
-			port->bulk_out_buffer,
-			port->bulk_out_size,
-			qt2_write_bulk_callback,
-			port);
-	port_extra->tx_pending_bytes = 0;
-
-	if (dev_extra->open_ports == 0) {
-		/* this is first port to be opened, so need the read URB
-		 * initialised for bulk in transfers (this is shared amongst
-		 * all the ports on the device) */
-		usb_fill_bulk_urb(port0->read_urb, serial->dev,
-			usb_rcvbulkpipe(serial->dev,
-			port0->bulk_in_endpointAddress),
-			port0->bulk_in_buffer,
-			port0->bulk_in_size,
-			qt2_read_bulk_callback, serial);
-		dbg("port0 bulk in URB initialised");
-
-		/* submit URB, i.e. start reading from device (async) */
-		dev_extra->ReadBulkStopped = false;
-		port_extra->read_urb_busy = true;
-		result = usb_submit_urb(port->read_urb, GFP_KERNEL);
-		if (result) {
-			dev_err(&port->dev,
-				 "%s(): Error %d submitting bulk in urb",
-				__func__, result);
-			port_extra->read_urb_busy = false;
-			dev_extra->ReadBulkStopped = true;
-		}
-
-		/* When the first port is opened, initialise the value of
-		 * current_port in dev_extra to this port, so it is set
-		 * to something. Once the box sends data it will send the
-		 * relevant escape sequences to get it to the right port anyway
-		 */
-		dev_extra->current_port = port;
-	}
-
-	/* initialize our wait queues */
-	init_waitqueue_head(&port_extra->wait);
-	/* increment the count of openings of this port by one */
-	port_extra->open_count++;
-
-	/* remember to store dev_extra, port_extra and port0_extra back again at
-	 * end !*/
-	qt2_set_port_private(port, port_extra);
-	qt2_set_port_private(serial->port[0], port0_extra);
-	qt2_set_dev_private(serial, dev_extra);
-
-	dev_extra->open_ports++; /* one more port opened */
-
-	return 0;
-}
-
-/* called when a port is closed by userspace. It won't be called, however,
- * until calls to chars_in_buffer() reveal that the port has completed
- * sending buffered data, and there is nothing else to do. Thus we don't have
- * to rely on forcing data through in this function. */
-/* Setting close_pending should keep new data from being written out,
- * once all the data in the enpoint buffers is moved out we won't get
- * any more. */
-/* BoxStopReceive would keep any more data from coming from a given
- * port, but isn't called by the vendor driver, although their comments
- * mention it. Should it be used here to stop the inbound data
- * flow?
- */
-static void qt2_close(struct usb_serial_port *port)
-{
-	/* time out value for flush loops */
-	unsigned long jift;
-	struct quatech2_port *port_extra;	/* extra data for this port */
-	struct usb_serial *serial;	/* device structure */
-	struct quatech2_dev *dev_extra; /* extra data for the device */
-	__u8  lsr_value = 0;	/* value of Line Status Register */
-	int status;	/* result of last USB comms function */
-
-	dbg("%s(): port %d", __func__, port->number);
-	serial = port->serial;	/* get the parent device structure */
-	dev_extra = qt2_get_dev_private(serial);
-	/* get the device private data */
-	port_extra = qt2_get_port_private(port); /* port private data */
-
-	/* we can now (and only now) stop reading data */
-	port_extra->close_pending = true;
-	dbg("%s(): port_extra->close_pending = true", __func__);
-	/* although the USB side is now empty, the UART itself may
-	 * still be pushing characters out over the line, so we have to
-	 * wait testing the actual line status until the lines change
-	 * indicating that the data is done transferring. */
-	/* FIXME: slow this polling down so it doesn't run the USB bus flat out
-	 * if it actually has to spend any time in this loop (which it normally
-	 * doesn't because the buffer is nearly empty) */
-	jift = jiffies + (10 * HZ);	/* 10 sec timeout */
-	do {
-		status = qt2_box_get_register(serial, port->number,
-			QT2_LINE_STATUS_REGISTER, &lsr_value);
-		if (status < 0) {
-			dbg("%s(): qt2_box_get_register failed", __func__);
-			break;
-		}
-		if ((lsr_value & QT2_LSR_TEMT)) {
-			dbg("UART done sending");
-			break;
-		}
-		schedule();
-	} while (jiffies <= jift);
-
-	status = qt2_closeboxchannel(serial, port->number);
-	if (status < 0)
-		dbg("%s(): port %d qt2_box_open_close_channel failed",
-			__func__, port->number);
-	/* to avoid leaking URBs, we should now free the write_urb for this
-	 * port and set the pointer to null so that next time the port is opened
-	 * a new URB is allocated. This avoids leaking URBs when the device is
-	 * removed */
-	usb_free_urb(port->write_urb);
-	kfree(port->bulk_out_buffer);
-	port->bulk_out_buffer = NULL;
-	port->bulk_out_size = 0;
-
-	/* decrement the count of openings of this port by one */
-	port_extra->open_count--;
-	/* one less overall open as well */
-	dev_extra->open_ports--;
-	dbg("%s(): Exit, dev_extra->open_ports  = %d", __func__,
-		dev_extra->open_ports);
-}
-
-/**
- * qt2_write - write bytes from the tty layer out to the USB device.
- * @buf: The data to be written, size at least count.
- * @count: The number of bytes requested for transmission.
- * @return The number of bytes actually accepted for transmission to the device.
- */
-static int qt2_write(struct tty_struct *tty, struct usb_serial_port *port,
-		const unsigned char *buf, int count)
-{
-	struct usb_serial *serial;	/* parent device struct */
-	__u8 header_array[5];	/* header used to direct writes to the correct
-	port on the device */
-	struct quatech2_port *port_extra;	/* extra data for this port */
-	int result;
-
-	serial = port->serial; /* get the parent device of the port */
-	port_extra = qt2_get_port_private(port); /* port extra info */
-	if (serial == NULL)
-		return -ENODEV;
-	dbg("%s(): port %d, requested to write %d bytes, %d already pending",
-		__func__, port->number, count, port_extra->tx_pending_bytes);
-
-	if (count <= 0)	{
-		dbg("%s(): write request of <= 0 bytes", __func__);
-		return 0;	/* no bytes written */
-	}
-
-	/* check if the write urb is already in use, i.e. data already being
-	 * sent to this port */
-	if ((port->write_urb->status == -EINPROGRESS)) {
-		/* Fifo hasn't been emptied since last write to this port */
-		dbg("%s(): already writing, port->write_urb->status == "
-			"-EINPROGRESS", __func__);
-		/* schedule_work(&port->work); commented in vendor driver */
-		return 0;
-	} else if (port_extra->tx_pending_bytes >= QT2_FIFO_DEPTH) {
-		/* buffer is full (==). > should not occur, but would indicate
-		 * that an overflow had occurred */
-		dbg("%s(): port transmit buffer is full!", __func__);
-		/* schedule_work(&port->work); commented in vendor driver */
-		return 0;
-	}
-
-	/* We must fill the first 5 bytes of anything we sent with a transmit
-	 * header which directes the data to the correct port. The maximum
-	 * size we can send out in one URB is port->bulk_out_size, which caps
-	 * the number of bytes of real data we can send in each write. As the
-	 * semantics of write allow us to write less than we were give, we cap
-	 * the maximum we will ever write to the device as 5 bytes less than
-	 * one URB's worth, by reducing the value of the count argument
-	 * appropriately*/
-	if (count > port->bulk_out_size - QT2_TX_HEADER_LENGTH) {
-		count = port->bulk_out_size - QT2_TX_HEADER_LENGTH;
-		dbg("%s(): write request bigger than urb, only accepting "
-			"%d bytes", __func__, count);
-	}
-	/* we must also ensure that the FIFO at the other end can cope with the
-	 * URB we send it, otherwise it will have problems. As above, we can
-	 * restrict the write size by just shrinking count.*/
-	if (count > (QT2_FIFO_DEPTH - port_extra->tx_pending_bytes)) {
-		count = QT2_FIFO_DEPTH - port_extra->tx_pending_bytes;
-		dbg("%s(): not enough room in buffer, only accepting %d bytes",
-			__func__, count);
-	}
-	/* now build the header for transmission */
-	header_array[0] = 0x1b;
-	header_array[1] = 0x1b;
-	header_array[2] = (__u8)port->number;
-	header_array[3] = (__u8)count;
-	header_array[4] = (__u8)count >> 8;
-	/* copy header into URB */
-	memcpy(port->write_urb->transfer_buffer, header_array,
-		QT2_TX_HEADER_LENGTH);
-	/* and actual data to write */
-	memcpy(port->write_urb->transfer_buffer + 5, buf, count);
-
-	dbg("%s(): first data byte to send = %#.2x", __func__, *buf);
-
-	/* set up our urb */
-	usb_fill_bulk_urb(port->write_urb, serial->dev,
-			usb_sndbulkpipe(serial->dev,
-			port->bulk_out_endpointAddress),
-			port->write_urb->transfer_buffer, count + 5,
-			(qt2_write_bulk_callback), port);
-	/* send the data out the bulk port */
-	result = usb_submit_urb(port->write_urb, GFP_ATOMIC);
-	if (result) {
-		/* error couldn't submit urb */
-		result = 0;	/* return 0 as nothing got written */
-		dbg("%s(): failed submitting write urb, error %d",
-			__func__, result);
-	} else {
-		port_extra->tx_pending_bytes += count;
-		result = count;	/* return number of bytes written, i.e. count */
-		dbg("%s(): submitted write urb, wrote %d bytes, "
-			"total pending bytes %d",
-			__func__, result, port_extra->tx_pending_bytes);
-	}
-	return result;
-}
-
-/* This is used by the next layer up to know how much space is available
- * in the buffer on the device. It is used on a device closure to avoid
- * calling close() until the buffer is reported to be empty.
- * The returned value must never go down by more than the number of bytes
- * written for correct behaviour further up the driver stack, i.e. if I call
- * it, then write 6 bytes, then call again I should get 6 less, or possibly
- * only 5 less if one was written in the meantime, etc. I should never get 7
- * less (or any bigger number) because I only wrote 6 bytes.
- */
-static int qt2_write_room(struct tty_struct *tty)
-{
-	struct usb_serial_port *port = tty->driver_data;
-		/* parent usb_serial_port pointer */
-	struct quatech2_port *port_extra;	/* extra data for this port */
-	int room = 0;
-	port_extra = qt2_get_port_private(port);
-
-	if (port_extra->close_pending == true) {
-		dbg("%s(): port_extra->close_pending == true", __func__);
-		return -ENODEV;
-	}
-	/* Q: how many bytes would a write() call actually succeed in writing
-	 * if it happened now?
-	 * A: one QT2_FIFO_DEPTH, less the number of bytes waiting to be sent
-	 * out of the port, unless this is more than the size of the
-	 * write_urb output buffer less the header, which is the maximum
-	 * size write we can do.
-
-	 * Most of the implementation of this is done when writes to the device
-	 * are started or terminate. When we send a write to the device, we
-	 * reduce the free space count by the size of the dispatched write.
-	 * When a "transmit empty" message comes back up the USB read stream,
-	 * we decrement the count by the number of bytes reported sent, thus
-	 * keeping track of the difference between sent and received bytes.
-	 */
-
-	room = (QT2_FIFO_DEPTH - port_extra->tx_pending_bytes);
-	/* space in FIFO */
-	if (room > port->bulk_out_size - QT2_TX_HEADER_LENGTH)
-		room = port->bulk_out_size - QT2_TX_HEADER_LENGTH;
-	/* if more than the URB can hold, then cap to that limit */
-
-	dbg("%s(): port %d: write room is %d", __func__, port->number, room);
-	return room;
-}
-
-static int qt2_chars_in_buffer(struct tty_struct *tty)
-{
-	struct usb_serial_port *port = tty->driver_data;
-	/* parent usb_serial_port pointer */
-	struct quatech2_port *port_extra;	/* extra data for this port */
-	port_extra = qt2_get_port_private(port);
-
-	dbg("%s(): port %d: chars_in_buffer = %d", __func__,
-		port->number, port_extra->tx_pending_bytes);
-	return port_extra->tx_pending_bytes;
-}
-
-/* called when userspace does an ioctl() on the device. Note that
- * TIOCMGET and TIOCMSET are filtered off to their own methods before they get
- * here, so we don't have to handle them.
- */
-static int qt2_ioctl(struct tty_struct *tty,
-		     unsigned int cmd, unsigned long arg)
-{
-	struct usb_serial_port *port = tty->driver_data;
-	struct usb_serial *serial = port->serial;
-	__u8 mcr_value;	/* Modem Control Register value */
-	__u8 msr_value; /* Modem Status Register value */
-	unsigned short prev_msr_value; /* Previous value of Modem Status
-	 * Register used to implement waiting for a line status change to
-	 * occur */
-	struct quatech2_port *port_extra;	/* extra data for this port */
-	DECLARE_WAITQUEUE(wait, current);
-	/* Declare a wait queue named "wait" */
-
-	unsigned int value;
-	unsigned int UartNumber;
-
-	if (serial == NULL)
-		return -ENODEV;
-	UartNumber = tty->index - serial->minor;
-	port_extra = qt2_get_port_private(port);
-
-	dbg("%s(): port %d, UartNumber %d, tty =0x%p", __func__,
-	    port->number, UartNumber, tty);
-
-	if (cmd == TIOCMBIS || cmd == TIOCMBIC) {
-		if (qt2_box_get_register(port->serial, UartNumber,
-			QT2_MODEM_CONTROL_REGISTER, &mcr_value) < 0)
-			return -ESPIPE;
-		if (copy_from_user(&value, (unsigned int *)arg,
-			sizeof(value)))
-			return -EFAULT;
-
-		switch (cmd) {
-		case TIOCMBIS:
-			if (value & TIOCM_RTS)
-				mcr_value |= QT2_SERIAL_MCR_RTS;
-			if (value & TIOCM_DTR)
-				mcr_value |= QT2_SERIAL_MCR_DTR;
-			if (value & TIOCM_LOOP)
-				mcr_value |= QT2_SERIAL_MCR_LOOP;
-		break;
-		case TIOCMBIC:
-			if (value & TIOCM_RTS)
-				mcr_value &= ~QT2_SERIAL_MCR_RTS;
-			if (value & TIOCM_DTR)
-				mcr_value &= ~QT2_SERIAL_MCR_DTR;
-			if (value & TIOCM_LOOP)
-				mcr_value &= ~QT2_SERIAL_MCR_LOOP;
-		break;
-		default:
-		break;
-		}	/* end of local switch on cmd */
-		if (qt2_box_set_register(port->serial,  UartNumber,
-		    QT2_MODEM_CONTROL_REGISTER, mcr_value) < 0) {
-			return -ESPIPE;
-		} else {
-			port_extra->shadowMCR = mcr_value;
-			return 0;
-		}
-	} else if (cmd == TIOCMIWAIT) {
-		dbg("%s() port %d, cmd == TIOCMIWAIT enter",
-			__func__, port->number);
-		prev_msr_value = port_extra->shadowMSR  & QT2_SERIAL_MSR_MASK;
-		barrier();
-		__set_current_state(TASK_INTERRUPTIBLE);
-		while (1) {
-			add_wait_queue(&port_extra->wait, &wait);
-			schedule();
-			dbg("%s(): port %d, cmd == TIOCMIWAIT here\n",
-				__func__, port->number);
-			remove_wait_queue(&port_extra->wait, &wait);
-			/* see if a signal woke us up */
-			if (signal_pending(current))
-				return -ERESTARTSYS;
-			set_current_state(TASK_INTERRUPTIBLE);
-			msr_value = port_extra->shadowMSR & QT2_SERIAL_MSR_MASK;
-			if (msr_value == prev_msr_value) {
-				__set_current_state(TASK_RUNNING);
-				return -EIO;  /* no change - error */
-			}
-			if ((arg & TIOCM_RNG &&
-				((prev_msr_value & QT2_SERIAL_MSR_RI) ==
-					(msr_value & QT2_SERIAL_MSR_RI))) ||
-				(arg & TIOCM_DSR &&
-				((prev_msr_value & QT2_SERIAL_MSR_DSR) ==
-					(msr_value & QT2_SERIAL_MSR_DSR))) ||
-				(arg & TIOCM_CD &&
-				((prev_msr_value & QT2_SERIAL_MSR_CD) ==
-					(msr_value & QT2_SERIAL_MSR_CD))) ||
-				(arg & TIOCM_CTS &&
-				((prev_msr_value & QT2_SERIAL_MSR_CTS) ==
-					(msr_value & QT2_SERIAL_MSR_CTS)))) {
-				__set_current_state(TASK_RUNNING);
-				return 0;
-			}
-		} /* end inifinite while */
-		/* FIXME: This while loop needs a way to break out if the device
-		 * is disconnected while a process is waiting for the MSR to
-		 * change, because once it's disconnected, it isn't going to
-		 * change state ... */
-	} else {
-		/* any other ioctls we don't know about come here */
-		dbg("%s(): No ioctl for that one. port = %d", __func__,
-			port->number);
-		return -ENOIOCTLCMD;
-	}
-}
-
-/* Called when the user wishes to change the port settings using the termios
- * userspace interface */
-static void qt2_set_termios(struct tty_struct *tty,
-	struct usb_serial_port *port, struct ktermios *old_termios)
-{
-	struct usb_serial *serial; /* parent serial device */
-	int baud, divisor, remainder;
-	unsigned char LCR_change_to = 0;
-	int status;
-	__u16 UartNumber;
-
-	dbg("%s(): port %d", __func__, port->number);
-
-	serial = port->serial;
-
-	UartNumber = port->number;
-
-	if (old_termios && !tty_termios_hw_change(old_termios, tty->termios))
-		return;
-
-	switch (tty->termios->c_cflag) {
-	case CS5:
-		LCR_change_to |= QT2_SERIAL_5_DATA;
-		break;
-	case CS6:
-		LCR_change_to |= QT2_SERIAL_6_DATA;
-		break;
-	case CS7:
-		LCR_change_to |= QT2_SERIAL_7_DATA;
-		break;
-	default:
-	case CS8:
-		LCR_change_to |= QT2_SERIAL_8_DATA;
-		break;
-	}
-
-	/* Parity stuff */
-	if (tty->termios->c_cflag & PARENB) {
-		if (tty->termios->c_cflag & PARODD)
-			LCR_change_to |= QT2_SERIAL_ODD_PARITY;
-		else
-			LCR_change_to |= QT2_SERIAL_EVEN_PARITY;
-	}
-	/* Because LCR_change_to is initialised to zero, we don't have to worry
-	 * about the case where PARENB is not set or clearing bits, because by
-	 * default all of them are cleared, turning parity off.
-	 * as we don't support mark/space parity, we should clear the
-	 * mark/space parity bit in c_cflag, so the caller can tell we have
-	 * ignored the request */
-	tty->termios->c_cflag &= ~CMSPAR;
-
-	if (tty->termios->c_cflag & CSTOPB)
-		LCR_change_to |= QT2_SERIAL_TWO_STOPB;
-	else
-		LCR_change_to |= QT2_SERIAL_ONE_STOPB;
-
-	/* Thats the LCR stuff, next we need to work out the divisor as the
-	 * LCR and the divisor are set together */
-	baud = tty_get_baud_rate(tty);
-	if (!baud) {
-		/* pick a default, any default... */
-		baud = 9600;
-	}
-	dbg("%s(): got baud = %d", __func__, baud);
-
-	divisor = QT2_MAX_BAUD_RATE / baud;
-	remainder = QT2_MAX_BAUD_RATE % baud;
-	/* Round to nearest divisor */
-	if (((remainder * 2) >= baud) && (baud != 110))
-		divisor++;
-	dbg("%s(): setting divisor = %d, QT2_MAX_BAUD_RATE = %d , LCR = %#.2x",
-	      __func__, divisor, QT2_MAX_BAUD_RATE, LCR_change_to);
-
-	status = qt2_boxsetuart(serial, UartNumber, (unsigned short) divisor,
-			    LCR_change_to);
-	if (status < 0)	{
-		dbg("qt2_boxsetuart() failed");
-		return;
-	} else {
-		/* now encode the baud rate we actually set, which may be
-		 * different to the request */
-		baud = QT2_MAX_BAUD_RATE / divisor;
-		tty_encode_baud_rate(tty, baud, baud);
-	}
-
-	/* Now determine flow control */
-	if (tty->termios->c_cflag & CRTSCTS) {
-		dbg("%s(): Enabling HW flow control port %d", __func__,
-		      port->number);
-		/* Enable  RTS/CTS flow control */
-		status = qt2_boxsethw_flowctl(serial, UartNumber, true);
-		if (status < 0) {
-			dbg("qt2_boxsethw_flowctl() failed");
-			return;
-		}
-	} else {
-		/* Disable RTS/CTS flow control */
-		dbg("%s(): disabling HW flow control port %d", __func__,
-			port->number);
-		status = qt2_boxsethw_flowctl(serial, UartNumber, false);
-		if (status < 0)	{
-			dbg("qt2_boxsethw_flowctl failed");
-			return;
-		}
-	}
-	/* if we are implementing XON/XOFF, set the start and stop character
-	 * in the device */
-	if (I_IXOFF(tty) || I_IXON(tty)) {
-		unsigned char stop_char  = STOP_CHAR(tty);
-		unsigned char start_char = START_CHAR(tty);
-		status = qt2_boxsetsw_flowctl(serial, UartNumber, stop_char,
-				start_char);
-		if (status < 0)
-			dbg("qt2_boxsetsw_flowctl (enabled) failed");
-	} else {
-		/* disable SW flow control */
-		status = qt2_boxunsetsw_flowctl(serial, UartNumber);
-		if (status < 0)
-			dbg("qt2_boxunsetsw_flowctl (disabling) failed");
-	}
-}
-
-static int qt2_tiocmget(struct tty_struct *tty)
-{
-	struct usb_serial_port *port = tty->driver_data;
-	struct usb_serial *serial = port->serial;
-
-	__u8 mcr_value;	/* Modem Control Register value */
-	__u8 msr_value;	/* Modem Status Register value */
-	unsigned int result = 0;
-	int status;
-	unsigned int UartNumber;
-
-	if (serial == NULL)
-		return -ENODEV;
-
-	dbg("%s(): port %d, tty =0x%p", __func__, port->number, tty);
-	UartNumber = tty->index - serial->minor;
-	dbg("UartNumber is %d", UartNumber);
-
-	status = qt2_box_get_register(port->serial, UartNumber,
-			QT2_MODEM_CONTROL_REGISTER,	&mcr_value);
-	if (status >= 0) {
-		status = qt2_box_get_register(port->serial,  UartNumber,
-				QT2_MODEM_STATUS_REGISTER, &msr_value);
-	}
-	if (status >= 0) {
-		result = ((mcr_value & QT2_SERIAL_MCR_DTR) ? TIOCM_DTR : 0)
-				/*DTR set */
-			| ((mcr_value & QT2_SERIAL_MCR_RTS)  ? TIOCM_RTS : 0)
-				/*RTS set */
-			| ((msr_value & QT2_SERIAL_MSR_CTS)  ? TIOCM_CTS : 0)
-				/* CTS set */
-			| ((msr_value & QT2_SERIAL_MSR_CD)  ? TIOCM_CAR : 0)
-				/*Carrier detect set */
-			| ((msr_value & QT2_SERIAL_MSR_RI)  ? TIOCM_RI : 0)
-				/* Ring indicator set */
-			| ((msr_value & QT2_SERIAL_MSR_DSR)  ? TIOCM_DSR : 0);
-				/* DSR set */
-		return result;
-	} else {
-		return -ESPIPE;
-	}
-}
-
-static int qt2_tiocmset(struct tty_struct *tty,
-		       unsigned int set, unsigned int clear)
-{
-	struct usb_serial_port *port = tty->driver_data;
-	struct usb_serial *serial = port->serial;
-	__u8 mcr_value;	/* Modem Control Register value */
-	int status;
-	unsigned int UartNumber;
-
-	if (serial == NULL)
-		return -ENODEV;
-
-	UartNumber = tty->index - serial->minor;
-	dbg("%s(): port %d, UartNumber %d", __func__, port->number, UartNumber);
-
-	status = qt2_box_get_register(port->serial, UartNumber,
-			QT2_MODEM_CONTROL_REGISTER, &mcr_value);
-	if (status < 0)
-		return -ESPIPE;
-
-	/* Turn off RTS, DTR and loopback, then only turn on what was asked
-	 * for */
-	mcr_value &= ~(QT2_SERIAL_MCR_RTS | QT2_SERIAL_MCR_DTR |
-			QT2_SERIAL_MCR_LOOP);
-	if (set & TIOCM_RTS)
-		mcr_value |= QT2_SERIAL_MCR_RTS;
-	if (set & TIOCM_DTR)
-		mcr_value |= QT2_SERIAL_MCR_DTR;
-	if (set & TIOCM_LOOP)
-		mcr_value |= QT2_SERIAL_MCR_LOOP;
-
-	status = qt2_box_set_register(port->serial, UartNumber,
-			QT2_MODEM_CONTROL_REGISTER, mcr_value);
-	if (status < 0)
-		return -ESPIPE;
-	else
-		return 0;
-}
-
-/** qt2_break - Turn BREAK on and off on the UARTs
- */
-static void qt2_break(struct tty_struct *tty, int break_state)
-{
-	struct usb_serial_port *port = tty->driver_data; /* parent port */
-	struct usb_serial *serial = port->serial;	/* parent device */
-	struct quatech2_port *port_extra;	/* extra data for this port */
-	__u16 break_value;
-	unsigned int result;
-
-	port_extra = qt2_get_port_private(port);
-	if (!serial) {
-		dbg("%s(): port %d: no serial object", __func__, port->number);
-		return;
-	}
-
-	if (break_state == -1)
-		break_value = 1;
-	else
-		break_value = 0;
-	dbg("%s(): port %d, break_value %d", __func__, port->number,
-		break_value);
-
-	mutex_lock(&port_extra->modelock);
-	if (!port_extra->open_count) {
-		dbg("%s(): port not open", __func__);
-		goto exit;
-	}
-
-	result = usb_control_msg(serial->dev, usb_sndctrlpipe(serial->dev, 0),
-				QT2_BREAK_CONTROL, 0x40, break_value,
-				port->number, NULL, 0, 300);
-exit:
-	mutex_unlock(&port_extra->modelock);
-	dbg("%s(): exit port %d", __func__, port->number);
-
-}
-/**
- * qt2_throttle: - stop reading new data from the port
- */
-static void qt2_throttle(struct tty_struct *tty)
-{
-	struct usb_serial_port *port = tty->driver_data;
-	struct usb_serial *serial = port->serial;
-	struct quatech2_port *port_extra;	/* extra data for this port */
-	dbg("%s(): port %d", __func__, port->number);
-
-	port_extra = qt2_get_port_private(port);
-	if (!serial) {
-		dbg("%s(): enter port %d no serial object", __func__,
-		      port->number);
-		return;
-	}
-
-	mutex_lock(&port_extra->modelock);	/* lock structure */
-	if (!port_extra->open_count) {
-		dbg("%s(): port not open", __func__);
-		goto exit;
-	}
-	/* Send command to box to stop receiving stuff. This will stop this
-	 * particular UART from filling the endpoint - in the multiport case the
-	 * FPGA UART will handle any flow control implemented, but for the single
-	 * port it's handed differently and we just quit submitting urbs
-	 */
-	if (serial->dev->descriptor.idProduct != QUATECH_SSU2_100)
-		qt2_boxstoprx(serial, port->number, 1);
-
-	port->throttled = 1;
-exit:
-	mutex_unlock(&port_extra->modelock);
-	dbg("%s(): port %d: setting port->throttled", __func__, port->number);
-	return;
-}
-
-/**
- * qt2_unthrottle: - start receiving data through the port again after being
- * throttled
- */
-static void qt2_unthrottle(struct tty_struct *tty)
-{
-	struct usb_serial_port *port = tty->driver_data;
-	struct usb_serial *serial = port->serial;
-	struct quatech2_port *port_extra;	/* extra data for this port */
-	struct usb_serial_port *port0;	/* first port structure on device */
-	struct quatech2_dev *dev_extra;		/* extra data for the device */
-
-	if (!serial) {
-		dbg("%s() enter port %d no serial object!", __func__,
-			port->number);
-		return;
-	}
-	dbg("%s(): enter port %d", __func__, port->number);
-	dev_extra = qt2_get_dev_private(serial);
-	port_extra = qt2_get_port_private(port);
-	port0 = serial->port[0]; /* get the first port's device structure */
-
-	mutex_lock(&port_extra->modelock);
-	if (!port_extra->open_count) {
-		dbg("%s(): port %d not open", __func__, port->number);
-		goto exit;
-	}
-
-	if (port->throttled != 0) {
-		dbg("%s(): port %d: unsetting port->throttled", __func__,
-		    port->number);
-		port->throttled = 0;
-		/* Send command to box to start receiving stuff */
-		if (serial->dev->descriptor.idProduct != QUATECH_SSU2_100) {
-			qt2_boxstoprx(serial,  port->number, 0);
-		} else if (dev_extra->ReadBulkStopped == true) {
-			usb_fill_bulk_urb(port0->read_urb, serial->dev,
-				usb_rcvbulkpipe(serial->dev,
-				port0->bulk_in_endpointAddress),
-				port0->bulk_in_buffer,
-				port0->bulk_in_size,
-				qt2_read_bulk_callback,
-				serial);
-		}
-	}
-exit:
-	mutex_unlock(&port_extra->modelock);
-	dbg("%s(): exit port %d", __func__, port->number);
-	return;
-}
-
-/* internal, private helper functions for the driver */
-
-/* Power up the FPGA in the box to get it working */
-static int qt2_boxpoweron(struct usb_serial *serial)
-{
-	int result;
-	__u8  Direcion;
-	unsigned int pipe;
-	Direcion = USBD_TRANSFER_DIRECTION_OUT;
-	pipe = usb_rcvctrlpipe(serial->dev, 0);
-	result = usb_control_msg(serial->dev, pipe, QT_SET_GET_DEVICE,
-				Direcion, QU2BOXPWRON, 0x00, NULL, 0x00,
-				5000);
-	return result;
-}
-
-/*
- * qt2_boxsetQMCR Issue a QT2_GET_SET_QMCR vendor-spcific request on the
- * default control pipe. If successful return the number of bytes written,
- * otherwise return a negative error number of the problem.
- */
-static int qt2_boxsetQMCR(struct usb_serial *serial, __u16 Uart_Number,
-			  __u8 QMCR_Value)
-{
-	int result;
-	__u16 PortSettings;
-
-	PortSettings = (__u16)(QMCR_Value);
-
-	dbg("%s(): Port = %d, PortSettings = 0x%x", __func__,
-			Uart_Number, PortSettings);
-
-	result = usb_control_msg(serial->dev, usb_sndctrlpipe(serial->dev, 0),
-				QT2_GET_SET_QMCR, 0x40, PortSettings,
-				(__u16)Uart_Number, NULL, 0, 5000);
-	return result;
-}
-
-static int port_paranoia_check(struct usb_serial_port *port,
-			       const char *function)
-{
-	if (!port) {
-		dbg("%s - port == NULL", function);
-		return -1;
-	}
-	if (!port->serial) {
-		dbg("%s - port->serial == NULL\n", function);
-		return -1;
-	}
-	return 0;
-}
-
-static int serial_paranoia_check(struct usb_serial *serial,
-				 const char *function)
-{
-	if (!serial) {
-		dbg("%s - serial == NULL\n", function);
-		return -1;
-	}
-
-	if (!serial->type) {
-		dbg("%s - serial->type == NULL!", function);
-		return -1;
-	}
-
-	return 0;
-}
-
-static inline struct quatech2_port *qt2_get_port_private(struct usb_serial_port
-		*port)
-{
-	return (struct quatech2_port *)usb_get_serial_port_data(port);
-}
-
-static inline void qt2_set_port_private(struct usb_serial_port *port,
-		struct quatech2_port *data)
-{
-	usb_set_serial_port_data(port, (void *)data);
-}
-
-static inline struct quatech2_dev *qt2_get_dev_private(struct usb_serial
-		*serial)
-{
-	return (struct quatech2_dev *)usb_get_serial_data(serial);
-}
-static inline void qt2_set_dev_private(struct usb_serial *serial,
-		struct quatech2_dev *data)
-{
-	usb_set_serial_data(serial, (void *)data);
-}
-
-static int qt2_openboxchannel(struct usb_serial *serial, __u16
-		Uart_Number, struct qt2_status_data *status)
-{
-	int result;
-	__u16 length;
-	__u8  Direcion;
-	unsigned int pipe;
-	length = sizeof(struct qt2_status_data);
-	Direcion = USBD_TRANSFER_DIRECTION_IN;
-	pipe = usb_rcvctrlpipe(serial->dev, 0);
-	result = usb_control_msg(serial->dev, pipe, QT_OPEN_CLOSE_CHANNEL,
-			Direcion, 0x00, Uart_Number, status, length, 5000);
-	return result;
-}
-static int qt2_closeboxchannel(struct usb_serial *serial, __u16 Uart_Number)
-{
-	int result;
-	__u8  direcion;
-	unsigned int pipe;
-	direcion = USBD_TRANSFER_DIRECTION_OUT;
-	pipe = usb_sndctrlpipe(serial->dev, 0);
-	result = usb_control_msg(serial->dev, pipe, QT_OPEN_CLOSE_CHANNEL,
-		  direcion, 0, Uart_Number, NULL, 0, 5000);
-	return result;
-}
-
-/* qt2_conf_uart Issue a SET_UART vendor-spcific request on the default
- * control pipe. If successful sets baud rate divisor and LCR value
- */
-static int qt2_conf_uart(struct usb_serial *serial,  unsigned short Uart_Number,
-		      unsigned short divisor, unsigned char LCR)
-{
-	int result;
-	unsigned short UartNumandLCR;
-
-	UartNumandLCR = (LCR << 8) + Uart_Number;
-
-	result = usb_control_msg(serial->dev, usb_sndctrlpipe(serial->dev, 0),
-				QT2_GET_SET_UART, 0x40, divisor, UartNumandLCR,
-				NULL, 0, 300);
-	return result;
-}
-
-/** @brief Callback for asynchronous submission of read URBs on bulk in
- * endpoints
- *
- * Registered in qt2_open_port(), used to deal with incomming data
- * from the box.
- */
-static void qt2_read_bulk_callback(struct urb *urb)
-{
-	/* Get the device pointer (struct usb_serial) back out of the URB */
-	struct usb_serial *serial = urb->context;
-	/* get the extra struct for the device */
-	struct quatech2_dev *dev_extra = qt2_get_dev_private(serial);
-	/* Get first port structure from the device */
-	struct usb_serial_port *port0 = serial->port[0];
-	/* Get the currently active port structure from serial struct */
-	struct usb_serial_port *active = dev_extra->current_port;
-	/* get the extra struct for port 0 */
-	struct quatech2_port *port0_extra = qt2_get_port_private(port0);
-	/* and for the currently active port */
-	struct quatech2_port *active_extra = qt2_get_port_private(active);
-	/* When we finally get to doing some tty stuff, we will need this */
-	struct tty_struct *tty_st;
-	unsigned int RxCount;	/* the length of the data to process */
-	unsigned int i;	/* loop counter over the data to process */
-	int result;	/* return value cache variable */
-	bool escapeflag;	/* flag set to true if this loop iteration is
-				 * parsing an escape sequence, rather than
-				 * ordinary data */
-	dbg("%s(): callback running, active port is %d", __func__,
-		active->number);
-
-	if (urb->status) {
-		/* read didn't go well */
-		dev_extra->ReadBulkStopped = true;
-		dbg("%s(): nonzero bulk read status received: %d",
-			__func__, urb->status);
-		return;
-	}
-
-	/* inline port_sofrint() here */
-	if (port_paranoia_check(port0, __func__) != 0) {
-		dbg("%s - port_paranoia_check on port0 failed, exiting\n",
-__func__);
-		return;
-	}
-	if (port_paranoia_check(active, __func__) != 0) {
-		dbg("%s - port_paranoia_check on current_port "
-			"failed, exiting", __func__);
-		return;
-	}
-
-/* This single callback function has to do for all the ports on
- * the device. Data being read up the USB can contain certain
- * escape sequences which are used to communicate out-of-band
- * information from the serial port in-band over the USB.
- * These escapes include sending modem and flow control line
- * status, and switching the port. The concept of a "Current Port"
- * is used, which is where data is going until a port change
- * escape seqence is received. This Current Port is kept between
- * callbacks so that when this function enters we know which the
- * currently active port is and can get to work right away without
- * the box having to send repeat escape sequences (anyway, how
- * would it know to do so?).
- */
-
-	if (active_extra->close_pending == true) {
-		/* We are closing , stop reading */
-		dbg("%s - (active->close_pending == true", __func__);
-		if (dev_extra->open_ports <= 0) {
-			/* If this is the only port left open - stop the
-			 * bulk read */
-			dev_extra->ReadBulkStopped = true;
-			dbg("%s - (ReadBulkStopped == true;", __func__);
-			return;
-		}
-	}
-
-	/*
-	 * RxHolding is asserted by throttle, if we assert it, we're not
-	 * receiving any more characters and let the box handle the flow
-	 * control
-	 */
-	if ((port0_extra->RxHolding == true) &&
-		    (serial->dev->descriptor.idProduct == QUATECH_SSU2_100)) {
-		/* single port device, input is already stopped, so we don't
-		 * need any more input data */
-		dev_extra->ReadBulkStopped = true;
-		return;
-	}
-	/* finally, we are in a situation where we might consider the data
-	 * that is contained within the URB, and what to do about it.
-	 * This is likely to involved communicating up to the TTY layer, so
-	 * we will need to get hold of the tty for the port we are currently
-	 * dealing with */
-
-	/* active is a usb_serial_port. It has a member port which is a
-	 * tty_port. From this we get a tty_struct pointer which is what we
-	 * actually wanted, and keep it on tty_st */
-	tty_st = tty_port_tty_get(&active->port);
-	if (!tty_st) {
-		dbg("%s - bad tty pointer - exiting", __func__);
-		return;
-	}
-	RxCount = urb->actual_length;	/* grab length of data handy */
-
-	if (RxCount) {
-		/* skip all this if no data to process */
-		for (i = 0; i < RxCount ; ++i) {
-			/* Look ahead code here -works on several bytes at onc*/
-			if ((i <= (RxCount - 3)) && (THISCHAR == 0x1b)
-				&& (NEXTCHAR == 0x1b)) {
-				/* we are in an escape sequence, type
-				 * determined by the 3rd char */
-				escapeflag = false;
-				switch (THIRDCHAR) {
-				case 0x00:
-					/* Line status change 4th byte must
-					 * follow */
-					if (i > (RxCount - 4)) {
-						dbg("Illegal escape sequences "
-						"in received data");
-						break;
-					}
-					qt2_process_line_status(active,
-						FOURTHCHAR);
-					i += 3;
-					escapeflag = true;
-					break;
-				case 0x01:
-					/* Modem status status change 4th byte
-					 * must follow */
-					if (i > (RxCount - 4)) {
-						dbg("Illegal escape sequences "
-						"in received data");
-						break;
-					}
-					qt2_process_modem_status(active,
-						FOURTHCHAR);
-					i += 3;
-					escapeflag = true;
-					break;
-				case 0x02:
-					/* xmit hold empty 4th byte
-					 * must follow */
-					if (i > (RxCount - 4)) {
-						dbg("Illegal escape sequences "
-						"in received data");
-						break;
-					}
-					qt2_process_xmit_empty(active,
-						FOURTHCHAR, FIFTHCHAR);
-					i += 4;
-					escapeflag = true;
-					break;
-				case 0x03:
-					/* Port number change 4th byte
-					 * must follow */
-					if (i > (RxCount - 4)) {
-						dbg("Illegal escape sequences "
-						"in received data");
-						break;
-					}
-					/* Port change. If port open push
-					 * current data up to tty layer */
-					if (active_extra->open_count > 0)
-						tty_flip_buffer_push(tty_st);
-
-					dbg("Port Change: new port = %d",
-						FOURTHCHAR);
-					qt2_process_port_change(active,
-						FOURTHCHAR);
-					i += 3;
-					escapeflag = true;
-					/* having changed port, the pointers for
-					 * the currently active port are all out
-					 * of date and need updating */
-					active = dev_extra->current_port;
-					active_extra =
-						qt2_get_port_private(active);
-					tty_st = tty_port_tty_get(
-						&active->port);
-					break;
-				case 0x04:
-					/* Recv flush 3rd byte must
-					 * follow */
-					if (i > (RxCount - 3)) {
-						dbg("Illegal escape sequences "
-							"in received data");
-						break;
-					}
-					qt2_process_rcv_flush(active);
-					i += 2;
-					escapeflag = true;
-					break;
-				case 0x05:
-					/* xmit flush 3rd byte must follow */
-					if (i > (RxCount - 3)) {
-						dbg("Illegal escape sequences "
-						"in received data");
-						break;
-					}
-					qt2_process_xmit_flush(active);
-					i += 2;
-					escapeflag = true;
-					break;
-				case 0xff:
-					dbg("No status sequence");
-					qt2_process_rx_char(active, THISCHAR);
-					qt2_process_rx_char(active, NEXTCHAR);
-					i += 2;
-					break;
-				default:
-					qt2_process_rx_char(active, THISCHAR);
-					i += 1;
-					break;
-				} /*end switch*/
-				if (escapeflag == true)
-					continue;
-				/* if we did an escape char, we don't need
-				 * to mess around pushing data through the
-				 * tty layer, and can go round again */
-			} /*endif*/
-			if (tty_st && urb->actual_length) {
-				tty_buffer_request_room(tty_st, 1);
-				tty_insert_flip_string(tty_st, &(
-						(unsigned char *)
-						(urb->transfer_buffer)
-					)[i], 1);
-			}
-		} /*endfor*/
-		tty_flip_buffer_push(tty_st);
-	} /*endif*/
-
-	/* at this point we have complete dealing with the data for this
-	 * callback. All we have to do now is to start the async read process
-	 * back off again. */
-
-	usb_fill_bulk_urb(port0->read_urb, serial->dev,
-		usb_rcvbulkpipe(serial->dev, port0->bulk_in_endpointAddress),
-		port0->bulk_in_buffer, port0->bulk_in_size,
-		qt2_read_bulk_callback, serial);
-	result = usb_submit_urb(port0->read_urb, GFP_ATOMIC);
-	if (result) {
-		dbg("%s(): failed resubmitting read urb, error %d",
-			__func__, result);
-	} else {
-		dbg("%s() successfully resubmitted read urb", __func__);
-		if (tty_st && RxCount) {
-			/* if some inbound data was processed, then
-			 * we need to push that through the tty layer
-			 */
-			tty_flip_buffer_push(tty_st);
-			tty_schedule_flip(tty_st);
-		}
-	}
-
-	/* cribbed from serqt_usb2 driver, but not sure which work needs
-	 * scheduling - port0 or currently active port? */
-	/* schedule_work(&port->work); */
-	dbg("%s() completed", __func__);
-	return;
-}
-
-/** @brief Callback for asynchronous submission of write URBs on bulk in
- * endpoints
- *
- * Registered in qt2_write(), used to deal with outgoing data
- * to the box.
- */
-static void qt2_write_bulk_callback(struct urb *urb)
-{
-	struct usb_serial_port *port = (struct usb_serial_port *)urb->context;
-	struct usb_serial *serial = port->serial;
-	dbg("%s(): port %d", __func__, port->number);
-	if (!serial) {
-		dbg("%s(): bad serial pointer, exiting", __func__);
-		return;
-	}
-	if (urb->status) {
-		dbg("%s(): nonzero write bulk status received: %d",
-			__func__, urb->status);
-		return;
-	}
-	/* FIXME What is supposed to be going on here?
-	 * does this actually do anything useful, and should it?
-	 */
-	/*port_softint((void *) serial); commented in vendor driver */
-	schedule_work(&port->work);
-	dbg("%s(): port %d exit", __func__, port->number);
-	return;
-}
-
-static void qt2_process_line_status(struct usb_serial_port *port,
-	unsigned char LineStatus)
-{
-	/* obtain the private structure for the port */
-	struct quatech2_port *port_extra = qt2_get_port_private(port);
-	port_extra->shadowLSR = LineStatus & (QT2_SERIAL_LSR_OE |
-		QT2_SERIAL_LSR_PE | QT2_SERIAL_LSR_FE | QT2_SERIAL_LSR_BI);
-}
-static void qt2_process_modem_status(struct usb_serial_port *port,
-	unsigned char ModemStatus)
-{
-	/* obtain the private structure for the port */
-	struct quatech2_port *port_extra = qt2_get_port_private(port);
-	port_extra->shadowMSR = ModemStatus;
-	wake_up_interruptible(&port_extra->wait);
-	/* this wakes up the otherwise indefinitely waiting code for
-	 * the TIOCMIWAIT ioctl, so that it can notice that
-	 * port_extra->shadowMSR has changed and the ioctl needs to return.
-	 */
-}
-
-static void qt2_process_xmit_empty(struct usb_serial_port *port,
-	unsigned char fourth_char, unsigned char fifth_char)
-{
-	int byte_count;
-	/* obtain the private structure for the port */
-	struct quatech2_port *port_extra = qt2_get_port_private(port);
-
-	byte_count = (int)(fifth_char * 16);
-	byte_count +=  (int)fourth_char;
-	/* byte_count indicates how many bytes the device has written out. This
-	 * message appears to occur regularly, and is used in the vendor driver
-	 * to keep track of the fill state of the port transmit buffer */
-	port_extra->tx_pending_bytes -= byte_count;
-	/* reduce the stored data queue length by the known number of bytes
-	 * sent */
-	dbg("port %d: %d bytes reported sent, %d still pending", port->number,
-			byte_count, port_extra->tx_pending_bytes);
-
-	/*port_extra->xmit_fifo_room_bytes = FIFO_DEPTH; ???*/
-}
-
-static void qt2_process_port_change(struct usb_serial_port *port,
-	unsigned char New_Current_Port)
-{
-	/* obtain the parent usb serial device structure */
-	struct usb_serial *serial = port->serial;
-	/* obtain the private structure for the device */
-	struct quatech2_dev *dev_extra = qt2_get_dev_private(serial);
-	dev_extra->current_port = serial->port[New_Current_Port];
-	/* what should I do with this? commented out in upstream
-	 * driver */
-	/*schedule_work(&port->work);*/
-}
-
-static void qt2_process_rcv_flush(struct usb_serial_port *port)
-{
-	/* obtain the private structure for the port */
-	struct quatech2_port *port_extra = qt2_get_port_private(port);
-	port_extra->rcv_flush = true;
-}
-static void qt2_process_xmit_flush(struct usb_serial_port *port)
-{
-	/* obtain the private structure for the port */
-	struct quatech2_port *port_extra = qt2_get_port_private(port);
-	port_extra->xmit_flush = true;
-}
-
-static void qt2_process_rx_char(struct usb_serial_port *port,
-	unsigned char data)
-{
-	/* get the tty_struct for this port */
-	struct tty_struct *tty = tty_port_tty_get(&(port->port));
-	/* get the URB with the data in to push */
-	struct urb *urb = port->serial->port[0]->read_urb;
-
-	if (tty && urb->actual_length) {
-		tty_buffer_request_room(tty, 1);
-		tty_insert_flip_string(tty, &data, 1);
-		/* should this be commented out here? */
-		/*tty_flip_buffer_push(tty);*/
-	}
-}
-
-/** @brief Retrieve the value of a register from the device
- *
- * Issues a GET_REGISTER vendor-spcific request over the USB control
- * pipe to obtain a value back from a specific register on a specific
- * UART
- * @param serial Serial device handle to access the device through
- * @param uart_number Which UART the value is wanted from
- * @param register_num Which register to read the value from
- * @param pValue Pointer to somewhere to put the retrieved value
- */
-static int qt2_box_get_register(struct usb_serial *serial,
-		unsigned char uart_number, unsigned short register_num,
-		__u8 *pValue)
-{
-	int result;
-	result = usb_control_msg(serial->dev, usb_rcvctrlpipe(serial->dev, 0),
-			QT2_GET_SET_REGISTER, 0xC0, register_num,
-			uart_number, (void *)pValue, sizeof(*pValue), 300);
-	return result;
-}
-
-/** qt2_box_set_register
- * Issue a SET_REGISTER vendor-specific request on the default control pipe
- */
-static int qt2_box_set_register(struct usb_serial *serial,
-		unsigned short Uart_Number, unsigned short Register_Num,
-		unsigned short Value)
-{
-	int result;
-	unsigned short reg_and_byte;
-
-	reg_and_byte = Value;
-	reg_and_byte = reg_and_byte << 8;
-	reg_and_byte = reg_and_byte + Register_Num;
-
-	result = usb_control_msg(serial->dev, usb_sndctrlpipe(serial->dev, 0),
-			QT2_GET_SET_REGISTER, 0x40, reg_and_byte,
-			Uart_Number, NULL, 0, 300);
-	return result;
-}
-
-/** qt2_boxsetuart - Issue a SET_UART vendor-spcific request on the default
- * control pipe. If successful sets baud rate divisor and LCR value.
- */
-static int qt2_boxsetuart(struct usb_serial *serial, unsigned short Uart_Number,
-		unsigned short default_divisor, unsigned char default_LCR)
-{
-	unsigned short UartNumandLCR;
-
-	UartNumandLCR = (default_LCR << 8) + Uart_Number;
-
-	return usb_control_msg(serial->dev, usb_sndctrlpipe(serial->dev, 0),
-			QT2_GET_SET_UART, 0x40, default_divisor, UartNumandLCR,
-			NULL, 0, 300);
-}
-
-/** qt2_boxsethw_flowctl - Turn hardware (RTS/CTS) flow control on and off for
- * a hardware UART.
- */
-static int qt2_boxsethw_flowctl(struct usb_serial *serial,
-		unsigned int UartNumber, bool bSet)
-{
-	__u8 MCR_Value = 0;
-	__u8 MSR_Value = 0;
-	__u16 MOUT_Value = 0;
-
-	if (bSet == true) {
-		MCR_Value =  QT2_SERIAL_MCR_RTS;
-		/* flow control, box will clear RTS line to prevent remote
-		 * device from transmitting more chars */
-	} else {
-		/* no flow control to remote device */
-		MCR_Value =  0;
-	}
-	MOUT_Value = MCR_Value << 8;
-
-	if (bSet == true) {
-		MSR_Value = QT2_SERIAL_MSR_CTS;
-		/* flow control on, box will inhibit tx data if CTS line is
-		 * asserted */
-	} else {
-		/* Box will not inhibit tx data due to CTS line */
-		MSR_Value = 0;
-	}
-	MOUT_Value |= MSR_Value;
-	return usb_control_msg(serial->dev, usb_sndctrlpipe(serial->dev, 0),
-			QT2_HW_FLOW_CONTROL_MASK, 0x40, MOUT_Value, UartNumber,
-			NULL, 0, 300);
-}
-
-/** qt2_boxsetsw_flowctl - Turn software (XON/XOFF) flow control on for
- * a hardware UART, and set the XON and XOFF characters.
- */
-static int qt2_boxsetsw_flowctl(struct usb_serial *serial, __u16 UartNumber,
-			unsigned char stop_char,  unsigned char start_char)
-{
-	__u16 nSWflowout;
-
-	nSWflowout = start_char << 8;
-	nSWflowout = (unsigned short)stop_char;
-	return usb_control_msg(serial->dev, usb_sndctrlpipe(serial->dev, 0),
-			QT2_SW_FLOW_CONTROL_MASK, 0x40, nSWflowout, UartNumber,
-			NULL, 0, 300);
-}
-
-/** qt2_boxunsetsw_flowctl - Turn software (XON/XOFF) flow control off for
- * a hardware UART.
- */
-static int qt2_boxunsetsw_flowctl(struct usb_serial *serial, __u16 UartNumber)
-{
-	return usb_control_msg(serial->dev, usb_sndctrlpipe(serial->dev, 0),
-			QT2_SW_FLOW_CONTROL_DISABLE, 0x40, 0, UartNumber, NULL,
-			0, 300);
-}
-
-/**
- * qt2_boxstoprx - Start and stop reception of data by the FPGA UART in
- * response to requests from the tty layer
- * @serial: pointer to the usb_serial structure for the parent device
- * @uart_number: which UART on the device we are addressing
- * @stop: Whether to start or stop data reception. Set to 1 to stop data being
- * received, and to 0 to start it being received.
- */
-static int qt2_boxstoprx(struct usb_serial *serial, unsigned short uart_number,
-		unsigned short stop)
-{
-	return usb_control_msg(serial->dev, usb_sndctrlpipe(serial->dev, 0),
-		QT2_STOP_RECEIVE, 0x40, stop, uart_number, NULL, 0, 300);
-}
-
-
-/*
- * last things in file: stuff to register this driver into the generic
- * USB serial framework.
- */
-
-static struct usb_serial_driver quatech2_device = {
-	.driver = {
-		.owner = THIS_MODULE,
-		.name = "quatech_usb2",
-	},
-	.description = DRIVER_DESC,
-	.id_table = quausb2_id_table,
-	.num_ports = 8,
-	.open = qt2_open,
-	.close = qt2_close,
-	.write = qt2_write,
-	.write_room = qt2_write_room,
-	.chars_in_buffer = qt2_chars_in_buffer,
-	.throttle = qt2_throttle,
-	.unthrottle = qt2_unthrottle,
-	.calc_num_ports = qt2_calc_num_ports,
-	.ioctl = qt2_ioctl,
-	.set_termios = qt2_set_termios,
-	.break_ctl = qt2_break,
-	.tiocmget = qt2_tiocmget,
-	.tiocmset = qt2_tiocmset,
-	.attach = qt2_attach,
-	.release = qt2_release,
-	.read_bulk_callback = qt2_read_bulk_callback,
-	.write_bulk_callback = qt2_write_bulk_callback,
-};
-
-static struct usb_serial_driver * const serial_drivers[] = {
-	&quatech2_device, NULL
-};
-
-module_usb_serial_driver(quausb2_usb_driver, serial_drivers);
-
-MODULE_AUTHOR(DRIVER_AUTHOR);
-MODULE_DESCRIPTION(DRIVER_DESC);
-MODULE_LICENSE("GPL");
-
-module_param(debug, bool, S_IRUGO | S_IWUSR);
-MODULE_PARM_DESC(debug, "Debug enabled or not");
diff --git a/drivers/staging/ramster/Kconfig b/drivers/staging/ramster/Kconfig
index 4af1f8d4b95..8349887827d 100644
--- a/drivers/staging/ramster/Kconfig
+++ b/drivers/staging/ramster/Kconfig
@@ -1,6 +1,6 @@
 config RAMSTER
 	bool "Cross-machine RAM capacity sharing, aka peer-to-peer tmem"
-	depends on (CLEANCACHE || FRONTSWAP) && CONFIGFS_FS=y && !ZCACHE && !XVMALLOC && !HIGHMEM
+	depends on (CLEANCACHE || FRONTSWAP) && CONFIGFS_FS=y && !ZCACHE && !XVMALLOC && !HIGHMEM && NET
 	select LZO_COMPRESS
 	select LZO_DECOMPRESS
 	default n
diff --git a/drivers/staging/ramster/cluster/tcp.c b/drivers/staging/ramster/cluster/tcp.c
index 3af1b2c51b7..d0a07d722b6 100644
--- a/drivers/staging/ramster/cluster/tcp.c
+++ b/drivers/staging/ramster/cluster/tcp.c
@@ -111,7 +111,7 @@ static struct socket *r2net_listen_sock;
  * r2net_wq.  teardown detaches the callbacks before destroying the workqueue.
  * quorum work is queued as sock containers are shutdown.. stop_listening
  * tears down all the node's sock containers, preventing future shutdowns
- * and queued quroum work, before canceling delayed quorum work and
+ * and queued quorum work, before canceling delayed quorum work and
  * destroying the work queue.
  */
 static struct workqueue_struct *r2net_wq;
@@ -660,7 +660,7 @@ out:
 
 /*
  * we register callbacks so we can queue work on events before calling
- * the original callbacks.  our callbacks our careful to test user_data
+ * the original callbacks.  our callbacks are careful to test user_data
  * to discover when they've reaced with r2net_unregister_callbacks().
  */
 static void r2net_register_callbacks(struct sock *sk,
@@ -2106,7 +2106,7 @@ static int r2net_open_listening_sock(__be32 addr, __be16 port)
 	r2net_listen_sock = sock;
 	INIT_WORK(&r2net_listen_work, r2net_accept_many);
 
-	sock->sk->sk_reuse = 1;
+	sock->sk->sk_reuse = SK_CAN_REUSE;
 	ret = sock->ops->bind(sock, (struct sockaddr *)&sin, sizeof(sin));
 	if (ret < 0) {
 		printk(KERN_ERR "ramster: Error %d while binding socket at "
diff --git a/drivers/staging/ramster/xvmalloc.c b/drivers/staging/ramster/xvmalloc.c
index 93ba8e9407a..44ceb0b823a 100644
--- a/drivers/staging/ramster/xvmalloc.c
+++ b/drivers/staging/ramster/xvmalloc.c
@@ -132,7 +132,7 @@ static u32 find_block(struct xv_pool *pool, u32 size,
 	if (!pool->flbitmap)
 		return 0;
 
-	/* Get freelist index correspoding to this size */
+	/* Get freelist index corresponding to this size */
 	slindex = get_index(size);
 	slbitmap = pool->slbitmap[slindex / BITS_PER_LONG];
 	slbitstart = slindex % BITS_PER_LONG;
diff --git a/drivers/staging/ramster/zcache-main.c b/drivers/staging/ramster/zcache-main.c
index 68b2e053a0e..4e7ef0e6b79 100644
--- a/drivers/staging/ramster/zcache-main.c
+++ b/drivers/staging/ramster/zcache-main.c
@@ -1331,7 +1331,7 @@ static ssize_t zv_max_mean_zsize_store(struct kobject *kobj,
  * when that limit is reached, further puts will be rejected (until
  * some pages have been flushed).  Note that, due to compression,
  * this number may exceed 100; it defaults to 75 and we set an
- * arbitary limit of 150.  A poor choice will almost certainly result
+ * arbitrary limit of 150.  A poor choice will almost certainly result
  * in OOM's, so this value should only be changed prudently.
  */
 static ssize_t zv_page_count_policy_percent_show(struct kobject *kobj,
@@ -2004,7 +2004,7 @@ int zcache_pampd_replace_in_obj(void *new_pampd, struct tmem_obj *obj)
  * Called by the message handler after a (still compressed) page has been
  * fetched from the remote machine in response to an "is_remote" tmem_get
  * or persistent tmem_localify.  For a tmem_get, "extra" is the address of
- * the page that is to be filled to succesfully resolve the tmem_get; for
+ * the page that is to be filled to successfully resolve the tmem_get; for
  * a (persistent) tmem_localify, "extra" is NULL (as the data is placed only
  * in the local zcache).  "data" points to "size" bytes of (compressed) data
  * passed in the message.  In the case of a persistent remote get, if
@@ -2095,7 +2095,7 @@ out:
 /*
  * Called on a remote persistent tmem_get to attempt to preallocate
  * local storage for the data contained in the remote persistent page.
- * If succesfully preallocated, returns the pampd, marked as remote and
+ * If successfully preallocated, returns the pampd, marked as remote and
  * in_transit.  Else returns NULL.  Note that the appropriate tmem data
  * structure must be locked.
  */
diff --git a/drivers/staging/rtl8187se/Makefile b/drivers/staging/rtl8187se/Makefile
index 72db504b23b..91d1aa2830c 100644
--- a/drivers/staging/rtl8187se/Makefile
+++ b/drivers/staging/rtl8187se/Makefile
@@ -10,7 +10,7 @@ ccflags-y += -DHIGH_POWER
 ccflags-y += -DSW_DIG
 ccflags-y += -DRATE_ADAPT
 
-#enable it for legacy power save, disable it for leisure  power save
+#enable it for legacy power save, disable it for leisure power save
 ccflags-y += -DENABLE_LPS
 
 
diff --git a/drivers/staging/rtl8187se/ieee80211/dot11d.c b/drivers/staging/rtl8187se/ieee80211/dot11d.c
index 309bb8bf287..0e93eb0735a 100644
--- a/drivers/staging/rtl8187se/ieee80211/dot11d.c
+++ b/drivers/staging/rtl8187se/ieee80211/dot11d.c
@@ -55,7 +55,7 @@ Dot11d_Reset(struct ieee80211_device *ieee)
 
 //
 //	Description:
-//		Update country IE from Beacon or Probe Resopnse
+//		Update country IE from Beacon or Probe Response
 //		and configure PHY for operation in the regulatory domain.
 //
 //	TODO:
diff --git a/drivers/staging/rtl8187se/ieee80211/ieee80211.h b/drivers/staging/rtl8187se/ieee80211/ieee80211.h
index 40dd715d9df..b94c48b2930 100644
--- a/drivers/staging/rtl8187se/ieee80211/ieee80211.h
+++ b/drivers/staging/rtl8187se/ieee80211/ieee80211.h
@@ -834,7 +834,7 @@ enum ieee80211_state {
 	/* the association procedure is sending AUTH request*/
 	IEEE80211_ASSOCIATING_AUTHENTICATING,
 
-	/* the association procedure has successfully authentcated
+	/* the association procedure has successfully authenticated
 	 * and is sending association request
 	 */
 	IEEE80211_ASSOCIATING_AUTHENTICATED,
@@ -934,7 +934,7 @@ struct ieee80211_device {
 			   * with RX of broad/multicast frames */
 
 	/* Fragmentation structures */
-	// each streaming contain a entry
+	/* each stream contains an entry */
 	struct ieee80211_frag_entry frag_cache[17][IEEE80211_FRAG_CACHE_LEN];
 	unsigned int frag_next_idx[17];
 	u16 fts; /* Fragmentation Threshold */
@@ -972,7 +972,7 @@ struct ieee80211_device {
 
 	int rate;       /* current rate */
 	int basic_rate;
-	//FIXME: pleace callback, see if redundant with softmac_features
+	//FIXME: please callback, see if redundant with softmac_features
 	short active_scan;
 
 	/* this contains flags for selectively enable softmac support */
@@ -1106,7 +1106,7 @@ struct ieee80211_device {
 
 	/* used instead of hard_start_xmit (not softmac_hard_start_xmit)
 	 * if the IEEE_SOFTMAC_TX_QUEUE feature is used to TX data
-	 * frames. I the option IEEE_SOFTMAC_SINGLE_QUEUE is also set
+	 * frames. If the option IEEE_SOFTMAC_SINGLE_QUEUE is also set
 	 * then also management frames are sent via this callback.
 	 * This function can't sleep.
 	 */
@@ -1124,7 +1124,7 @@ struct ieee80211_device {
 
 	/* ask to the driver to retune the radio .
 	 * This function can sleep. the driver should ensure
-	 * the radio has been swithced before return.
+	 * the radio has been switched before return.
 	 */
 	void (*set_chan)(struct net_device *dev,short ch);
 
@@ -1135,7 +1135,7 @@ struct ieee80211_device {
 	 * The syncro version is similar to the start_scan but
 	 * does not return until all channels has been scanned.
 	 * this is called in user context and should sleep,
-	 * it is called in a work_queue when swithcing to ad-hoc mode
+	 * it is called in a work_queue when switching to ad-hoc mode
 	 * or in behalf of iwlist scan when the card is associated
 	 * and root user ask for a scan.
 	 * the function stop_scan should stop both the syncro and
@@ -1196,7 +1196,7 @@ struct ieee80211_device {
 /* Generate probe requests */
 #define IEEE_SOFTMAC_PROBERQ (1<<4)
 
-/* Generate respones to probe requests */
+/* Generate response to probe requests */
 #define IEEE_SOFTMAC_PROBERS (1<<5)
 
 /* The ieee802.11 stack will manages the netif queue
diff --git a/drivers/staging/rtl8187se/ieee80211/ieee80211_softmac.c b/drivers/staging/rtl8187se/ieee80211/ieee80211_softmac.c
index 26bacb96d24..8173240dcf7 100644
--- a/drivers/staging/rtl8187se/ieee80211/ieee80211_softmac.c
+++ b/drivers/staging/rtl8187se/ieee80211/ieee80211_softmac.c
@@ -42,7 +42,7 @@ short ieee80211_is_shortslot(const struct ieee80211_network *net)
 	return net->capability & WLAN_CAPABILITY_SHORT_SLOT;
 }
 
-/* returns the total length needed for pleacing the RATE MFIE
+/* returns the total length needed for placing the RATE MFIE
  * tag and the EXTENDED RATE MFIE tag if needed.
  * It encludes two bytes per tag for the tag itself and its len
  */
@@ -60,7 +60,7 @@ unsigned int ieee80211_MFIE_rate_len(struct ieee80211_device *ieee)
 	return rate_len;
 }
 
-/* pleace the MFIE rate, tag to the memory (double) poined.
+/* place the MFIE rate, tag to the memory (double) poised.
  * Then it updates the pointer so that
  * it points after the new MFIE tag added.
  */
@@ -467,7 +467,7 @@ void ieee80211_softmac_scan_syncro(struct ieee80211_device *ieee)
 		 *    So we switch to IEEE80211_LINKED_SCANNING to remember
 		 *    that we are still logically linked (not interested in
 		 *    new network events, despite for updating the net list,
-		 *    but we are temporarly 'unlinked' as the driver shall
+		 *    but we are temporarily 'unlinked' as the driver shall
 		 *    not filter RX frames and the channel is changing.
 		 * So the only situation in witch are interested is to check
 		 * if the state become LINKED because of the #1 situation
@@ -530,7 +530,7 @@ void ieee80211_softmac_ips_scan_syncro(struct ieee80211_device *ieee)
                  *    So we switch to IEEE80211_LINKED_SCANNING to remember
                  *    that we are still logically linked (not interested in
                  *    new network events, despite for updating the net list,
-                 *    but we are temporarly 'unlinked' as the driver shall
+                 *    but we are temporarily 'unlinked' as the driver shall
                  *    not filter RX frames and the channel is changing.
                  * So the only situation in witch are interested is to check
                  * if the state become LINKED because of the #1 situation
@@ -1140,7 +1140,7 @@ void ieee80211_associate_abort(struct ieee80211_device *ieee)
 
 	ieee->associate_seq++;
 
-	/* don't scan, and avoid to have the RX path possibily
+	/* don't scan, and avoid to have the RX path possibly
 	 * try again to associate. Even do not react to AUTH or
 	 * ASSOC response. Just wait for the retry wq to be scheduled.
 	 * Here we will check if there are good nets to associate
@@ -1346,14 +1346,14 @@ inline void ieee80211_softmac_new_net(struct ieee80211_device *ieee, struct ieee
 		//printk("apset=%d apmatch=%d ssidset=%d ssidbroad=%d ssidmatch=%d\n",apset,apmatch,ssidset,ssidbroad,ssidmatch);
 
 		if (	/* if the user set the AP check if match.
-			 * if the network does not broadcast essid we check the user supplyed ANY essid
+			 * if the network does not broadcast essid we check the user supplied ANY essid
 			 * if the network does broadcast and the user does not set essid it is OK
 			 * if the network does broadcast and the user did set essid chech if essid match
 			 */
 				( apset && apmatch &&
 				  ((ssidset && ssidbroad && ssidmatch) || (ssidbroad && !ssidset) || (!ssidbroad && ssidset)) ) ||
 				/* if the ap is not set, check that the user set the bssid
-				 * and the network does bradcast and that those two bssid matches
+				 * and the network does broadcast and that those two bssid matches
 				 */
 				(!apset && ssidset && ssidbroad && ssidmatch)
 		   ){
@@ -1821,7 +1821,7 @@ ieee80211_rx_frame_softmac(struct ieee80211_device *ieee, struct sk_buff *skb,
 
 					while (left >= sizeof(struct ieee80211_info_element_hdr)) {
 						if (sizeof(struct ieee80211_info_element_hdr) + info_element->len > left) {
-							printk(KERN_WARNING "[re]associate reeponse error!");
+							printk(KERN_WARNING "[re]associate response error!");
 							return 1;
 						}
 						switch (info_element->id) {
@@ -1905,7 +1905,7 @@ associate_complete:
 							}
 						}else{
 							ieee->softmac_stats.rx_auth_rs_err++;
-							IEEE80211_DEBUG_MGMT("Authentication respose status code 0x%x",errcode);
+							IEEE80211_DEBUG_MGMT("Authentication response status code 0x%x",errcode);
 							ieee80211_associate_abort(ieee);
 						}
 
@@ -2184,15 +2184,15 @@ void ieee80211_start_ibss_wq(struct work_struct *work)
 
 	if(ieee->state == IEEE80211_NOLINK)
 		ieee->current_network.channel = 10;
-	/* if not then the state is not linked. Maybe the user swithced to
+	/* if not then the state is not linked. Maybe the user switched to
 	 * ad-hoc mode just after being in monitor mode, or just after
 	 * being very few time in managed mode (so the card have had no
 	 * time to scan all the chans..) or we have just run up the iface
 	 * after setting ad-hoc mode. So we have to give another try..
 	 * Here, in ibss mode, should be safe to do this without extra care
-	 * (in bss mode we had to make sure no-one tryed to associate when
+	 * (in bss mode we had to make sure no-one tried to associate when
 	 * we had just checked the ieee->state and we was going to start the
-	 * scan) beacause in ibss mode the ieee80211_new_net function, when
+	 * scan) because in ibss mode the ieee80211_new_net function, when
 	 * finds a good net, just set the ieee->state to IEEE80211_LINKED,
 	 * so, at worst, we waste a bit of time to initiate an unneeded syncro
 	 * scan, that will stop at the first round because it sees the state
@@ -2342,7 +2342,7 @@ void ieee80211_associate_retry_wq(struct work_struct *work)
 		goto exit;
 	/* until we do not set the state to IEEE80211_NOLINK
 	* there are no possibility to have someone else trying
-	* to start an association procdure (we get here with
+	* to start an association procedure (we get here with
 	* ieee->state = IEEE80211_ASSOCIATING).
 	* When we set the state to IEEE80211_NOLINK it is possible
 	* that the RX path run an attempt to associate, but
diff --git a/drivers/staging/rtl8187se/ieee80211/ieee80211_softmac_wx.c b/drivers/staging/rtl8187se/ieee80211/ieee80211_softmac_wx.c
index e46ff2ffa09..5d204906baf 100644
--- a/drivers/staging/rtl8187se/ieee80211/ieee80211_softmac_wx.c
+++ b/drivers/staging/rtl8187se/ieee80211/ieee80211_softmac_wx.c
@@ -362,7 +362,7 @@ int ieee80211_wx_set_essid(struct ieee80211_device *ieee,
 		ieee80211_stop_protocol(ieee);
 
 	/* this is just to be sure that the GET wx callback
-	 * has consisten infos. not needed otherwise
+	 * has consistent infos. not needed otherwise
 	 */
 	spin_lock_irqsave(&ieee->lock, flags);
 
diff --git a/drivers/staging/rtl8187se/ieee80211/ieee80211_tx.c b/drivers/staging/rtl8187se/ieee80211/ieee80211_tx.c
index 552115cd760..89ed86ef0d1 100644
--- a/drivers/staging/rtl8187se/ieee80211/ieee80211_tx.c
+++ b/drivers/staging/rtl8187se/ieee80211/ieee80211_tx.c
@@ -328,7 +328,7 @@ int ieee80211_rtl_xmit(struct sk_buff *skb,
 	//printk(KERN_WARNING "upper layer packet!\n");
 	spin_lock_irqsave(&ieee->lock, flags);
 
-	/* If there is no driver handler to take the TXB, dont' bother
+	/* If there is no driver handler to take the TXB, don't bother
 	 * creating it... */
 	if ((!ieee->hard_start_xmit && !(ieee->softmac_features & IEEE_SOFTMAC_TX_QUEUE))||
 	   ((!ieee->softmac_data_hard_start_xmit && (ieee->softmac_features & IEEE_SOFTMAC_TX_QUEUE)))) {
@@ -413,10 +413,7 @@ int ieee80211_rtl_xmit(struct sk_buff *skb,
 
 		/* Determine fragmentation size based on destination (multicast
 		* and broadcast are not fragmented) */
-//		if (is_multicast_ether_addr(dest) ||
-//		is_broadcast_ether_addr(dest)) {
-		if (is_multicast_ether_addr(header.addr1) ||
-		is_broadcast_ether_addr(header.addr1)) {
+		if (is_multicast_ether_addr(header.addr1)) {
 			frag_size = MAX_FRAG_THRESHOLD;
 			qos_ctl = QOS_CTL_NOTCONTAIN_ACK;
 		}
diff --git a/drivers/staging/rtl8187se/ieee80211/ieee80211_wx.c b/drivers/staging/rtl8187se/ieee80211/ieee80211_wx.c
index ca414a915a4..c7917b24425 100644
--- a/drivers/staging/rtl8187se/ieee80211/ieee80211_wx.c
+++ b/drivers/staging/rtl8187se/ieee80211/ieee80211_wx.c
@@ -363,7 +363,7 @@ int ieee80211_wx_set_encode(struct ieee80211_device *ieee,
 				       (*crypt)->priv);
 		sec.flags |= (1 << key);
 		/* This ensures a key will be activated if no key is
-		 * explicitely set */
+		 * explicitly set */
 		if (key == sec.active_key)
 			sec.flags |= SEC_ACTIVE_KEY;
 		ieee->tx_keyidx = key;//by wb 080312
diff --git a/drivers/staging/rtl8187se/r8180.h b/drivers/staging/rtl8187se/r8180.h
index a2c46ae4a40..2682afbac4f 100644
--- a/drivers/staging/rtl8187se/r8180.h
+++ b/drivers/staging/rtl8187se/r8180.h
@@ -11,7 +11,7 @@
 
    Parts of this driver are based on the Intel Pro Wireless 2100 GPL driver
 
-   We want to tanks the Authors of those projects and the Ndiswrapper
+   We want to thanks the Authors of those projects and the Ndiswrapper
    project Authors.
 */
 
@@ -514,12 +514,12 @@ typedef struct r8180_priv
 	bool bDefaultAntenna1;
 	u8 SignalStrength;
 	long Stats_SignalStrength;
-	long LastSignalStrengthInPercent; // In percentange, used for smoothing, e.g. Moving Average.
+	long LastSignalStrengthInPercent; // In percentage, used for smoothing, e.g. Moving Average.
 	u8	 SignalQuality; // in 0-100 index.
 	long Stats_SignalQuality;
 	long RecvSignalPower; // in dBm.
 	long Stats_RecvSignalPower;
-	u8	 LastRxPktAntenna;	// +by amy 080312 Antenn which received the lasted packet. 0: Aux, 1:Main. Added by Roger, 2008.01.25.
+	u8	 LastRxPktAntenna;	// +by amy 080312 Antenna which received the lasted packet. 0: Aux, 1:Main. Added by Roger, 2008.01.25.
 	u32 AdRxOkCnt;
 	long AdRxSignalStrength;
 	u8 CurrAntennaIndex;			// Index to current Antenna (both Tx and Rx).
@@ -530,7 +530,7 @@ typedef struct r8180_priv
 	long AdRxSsThreshold;			// Signal strength threshold to switch antenna.
 	long AdMaxRxSsThreshold;			// Max value of AdRxSsThreshold.
 	bool bAdSwitchedChecking;		// TRUE if we shall shall check Rx signal strength for last time switching antenna.
-	long AdRxSsBeforeSwitched;		// Rx signal strength before we swithed antenna.
+	long AdRxSsBeforeSwitched;		// Rx signal strength before we switched antenna.
 	struct timer_list SwAntennaDiversityTimer;
 //by amy for antenna
 //{by amy 080312
@@ -553,7 +553,7 @@ typedef struct r8180_priv
 	bool				bDigMechanism; // TRUE if DIG is enabled, FALSE ow.
 	bool				bRegHighPowerMechanism; // For High Power Mechanism. 061010, by rcnjko.
 	u32					FalseAlarmRegValue;
-	u8					RegDigOfdmFaUpTh; // Upper threhold of OFDM false alarm, which is used in DIG.
+	u8					RegDigOfdmFaUpTh; // Upper threshold of OFDM false alarm, which is used in DIG.
 	u8					DIG_NumberFallbackVote;
 	u8					DIG_NumberUpgradeVote;
 	// For HW antenna diversity, added by Roger, 2008.01.30.
diff --git a/drivers/staging/rtl8187se/r8180_core.c b/drivers/staging/rtl8187se/r8180_core.c
index 4fe52f6b003..fd22b75aea4 100644
--- a/drivers/staging/rtl8187se/r8180_core.c
+++ b/drivers/staging/rtl8187se/r8180_core.c
@@ -1329,7 +1329,7 @@ u16 N_DBPSOfRate(u16 DataRate)
 }
 
 /*
- * For Netgear case, they want good-looking singal strength.
+ * For Netgear case, they want good-looking signal strength.
  */
 long NetgearSignalStrengthTranslate(long LastSS, long CurrSS)
 {
@@ -1380,7 +1380,7 @@ long TranslateToDbm8185(u8 SignalStrengthIndex)
 
 /*
  * Perform signal smoothing for dynamic mechanism.
- * This is different with PerformSignalSmoothing8185 in smoothing fomula.
+ * This is different with PerformSignalSmoothing8185 in smoothing formula.
  * No dramatic adjustion is apply because dynamic mechanism need some degree
  * of correctness. Ported from 8187B.
  */
@@ -1535,7 +1535,7 @@ void rtl8180_rx(struct net_device *dev)
 			/* HW is probably passing several buggy frames
 			* without FD or LD flag set.
 			* Throw this garbage away to prevent skb
-			* memory exausting
+			* memory exhausting
 			*/
 			if (!priv->rx_skb_complete)
 				dev_kfree_skb_any(priv->rx_skb);
@@ -1648,14 +1648,14 @@ void rtl8180_rx(struct net_device *dev)
 			priv->Stats_SignalQuality = (long)(priv->Stats_SignalQuality * 5 + (long)priv->SignalQuality + 5) / 6;
 			priv->Stats_RecvSignalPower = (long)(priv->Stats_RecvSignalPower * 5 + priv->RecvSignalPower - 1) / 6;
 
-		/* Figure out which antenna that received the lasted packet. */
+		/* Figure out which antenna that received the last packet. */
 			priv->LastRxPktAntenna = Antenna ? 1 : 0; /* 0: aux, 1: main. */
 			SwAntennaDiversityRxOk8185(dev, priv->SignalStrength);
 		}
 
 		if (first) {
 			if (!priv->rx_skb_complete) {
-				/* seems that HW sometimes fails to reiceve and
+				/* seems that HW sometimes fails to receive and
 				   doesn't provide the last descriptor */
 				dev_kfree_skb_any(priv->rx_skb);
 				priv->stats.rxnolast++;
@@ -1672,7 +1672,7 @@ void rtl8180_rx(struct net_device *dev)
 			priv->rx_skb_complete = 0;
 			priv->rx_skb->dev = dev;
 		} else {
-			/* if we are here we should  have already RXed
+			/* if we are here we should have already RXed
 			* the first frame.
 			* If we get here and the skb is not allocated then
 			* we have just throw out garbage (skb not allocated)
@@ -1821,15 +1821,15 @@ rate) {
 /*
  * This is a rough attempt to TX a frame
  * This is called by the ieee 80211 stack to TX management frames.
- * If the ring is full packet are dropped (for data frame the queue
+ * If the ring is full packets are dropped (for data frame the queue
  * is stopped before this can happen). For this reason it is better
  * if the descriptors are larger than the largest management frame
- * we intend to TX: i'm unsure what the HW does if it will not found
+ * we intend to TX: i'm unsure what the HW does if it will not find
  * the last fragment of a frame because it has been dropped...
  * Since queues for Management and Data frames are different we
  * might use a different lock than tx_lock (for example mgmt_tx_lock)
  */
-/* these function may loops if invoked with 0 descriptors or 0 len buffer */
+/* these function may loop if invoked with 0 descriptors or 0 len buffer */
 int rtl8180_hard_start_xmit(struct sk_buff *skb, struct net_device *dev)
 {
 	struct r8180_priv *priv = (struct r8180_priv *)ieee80211_priv(dev);
@@ -2003,8 +2003,7 @@ short rtl8180_tx(struct net_device *dev, u8* txbuf, int len, int priority,
 	}
 
 		memcpy(&dest, frag_hdr->addr1, ETH_ALEN);
-		if (is_multicast_ether_addr(dest) ||
-				is_broadcast_ether_addr(dest)) {
+		if (is_multicast_ether_addr(dest)) {
 			Duration = 0;
 			RtsDur = 0;
 			bRTSEnable = 0;
@@ -2378,7 +2377,7 @@ void rtl8180_wmm_param_update(struct work_struct *work)
 				u8		u1bAIFS;
 				u32		u4bAcParam;
 				pAcParam = (PAC_PARAM)(&AcParam);
-				/* Retrive paramters to udpate. */
+				/* Retrieve paramters to update. */
 				u1bAIFS = pAcParam->f.AciAifsn.f.AIFSN * (((mode&IEEE_G) == IEEE_G) ? 9 : 20) + aSifsTime;
 				u4bAcParam = ((((u32)(pAcParam->f.TXOPLimit))<<AC_PARAM_TXOP_LIMIT_OFFSET)|
 					      (((u32)(pAcParam->f.Ecw.f.ECWmax))<<AC_PARAM_ECW_MAX_OFFSET)|
@@ -2414,7 +2413,7 @@ void rtl8180_wmm_param_update(struct work_struct *work)
 			u8		u1bAIFS;
 			u32		u4bAcParam;
 
-			/* Retrive paramters to udpate. */
+			/* Retrieve paramters to update. */
 			eACI = pAcParam->f.AciAifsn.f.ACI;
 			/* Mode G/A: slotTimeTimer = 9; Mode B: 20 */
 			u1bAIFS = pAcParam->f.AciAifsn.f.AIFSN * (((mode&IEEE_G) == IEEE_G) ? 9 : 20) + aSifsTime;
@@ -2700,7 +2699,7 @@ short rtl8180_init(struct net_device *dev)
 	priv->bTxPowerTrack = false;
 	priv->ThermalMeter = 0;
 	priv->FalseAlarmRegValue = 0;
-	priv->RegDigOfdmFaUpTh = 0xc; /* Upper threhold of OFDM false alarm, which is used in DIG. */
+	priv->RegDigOfdmFaUpTh = 0xc; /* Upper threshold of OFDM false alarm, which is used in DIG. */
 	priv->DIG_NumberFallbackVote = 0;
 	priv->DIG_NumberUpgradeVote = 0;
 	priv->LastSignalStrengthInPercent = 0;
@@ -2896,7 +2895,7 @@ short rtl8180_init(struct net_device *dev)
 		priv->chtxpwr_ofdm[i+1] = (word & 0xff00) >> 8;
 	}
 
-	/* 3Read crystal calibtration and thermal meter indication on 87SE. */
+	/* 3Read crystal calibration and thermal meter indication on 87SE. */
 	eeprom_93cx6_read(&eeprom, EEPROM_RSV>>1, &tmpu16);
 
 	/* Crystal calibration for Xin and Xout resp. */
@@ -3140,7 +3139,7 @@ void rtl8180_adapter_start(struct net_device *dev)
 
 	/*
 	 * The following is very strange. seems to be that 1 means test mode,
-	 * but we need to acknolwledges the nic when a packet is ready
+	 * but we need to acknowledges the nic when a packet is ready
 	 * although we set it to 0
 	 */
 
@@ -3971,7 +3970,7 @@ irqreturn_t rtl8180_interrupt(int irq, void *netdev, struct pt_regs *regs)
 	}
 
 	if (inta == 0xffff) {
-		/* HW disappared */
+		/* HW disappeared */
 		spin_unlock_irqrestore(&priv->irq_th_lock, flags);
 		return IRQ_HANDLED;
 	}
diff --git a/drivers/staging/rtl8187se/r8180_dm.c b/drivers/staging/rtl8187se/r8180_dm.c
index 4d7a5951486..b8f2ba010a0 100644
--- a/drivers/staging/rtl8187se/r8180_dm.c
+++ b/drivers/staging/rtl8187se/r8180_dm.c
@@ -2,7 +2,7 @@
 #include "r8180_hw.h"
 #include "r8180_93cx6.h"
 
- /*	Return TRUE if we shall perform High Power Mecahnism, FALSE otherwise. */
+ /*	Return TRUE if we shall perform High Power Mechanism, FALSE otherwise. */
 #define RATE_ADAPTIVE_TIMER_PERIOD      300
 
 bool CheckHighPower(struct net_device *dev)
@@ -105,7 +105,7 @@ void rtl8180_tx_pw_wq(struct work_struct *work)
 
 
 /*
- *	Return TRUE if we shall perform DIG Mecahnism, FALSE otherwise.
+ *	Return TRUE if we shall perform DIG Mechanism, FALSE otherwise.
  */
 bool CheckDig(struct net_device *dev)
 {
@@ -507,7 +507,7 @@ void StaRateAdaptive87SE(struct net_device *dev)
 	 *		and retry rate.
 	 * (3) Remove all Initial Gain Updates over OFDM rate. To avoid the complicated
 	 *		situation, Initial Gain Update is upon on DIG mechanism except CCK rate.
-	 * (4) Add the mehanism of trying to upgrade tx rate.
+	 * (4) Add the mechanism of trying to upgrade tx rate.
 	 * (5) Record the information of upping tx rate to avoid trying upping tx rate constantly.
 	 *
 	 */
@@ -528,7 +528,7 @@ void StaRateAdaptive87SE(struct net_device *dev)
 	if (priv->bTryuping == true) {
 		/* 2 For Test Upgrading mechanism
 		 * Note:
-		 *	Sometimes the throughput is upon on the capability bwtween the AP and NIC,
+		 *	Sometimes the throughput is upon on the capability between the AP and NIC,
 		 *	thus the low data rate does not improve the performance.
 		 *	We randomly upgrade the data rate and check if the retry rate is improved.
 		 */
@@ -704,7 +704,7 @@ void StaRateAdaptive87SE(struct net_device *dev)
 
 			/* 
 			 * The difference in throughput between 48Mbps and 36Mbps is 8M.
-			 * So, we must be carefully in this rate scale. Isaiah 2008-02-15.
+			 * So, we must be careful in this rate scale. Isaiah 2008-02-15.
 			 */
 			if (((priv->CurrentOperaRate == 72) || (priv->CurrentOperaRate == 48) || (priv->CurrentOperaRate == 36)) &&
 				(priv->FailTxRateCount > 2))
@@ -1009,7 +1009,7 @@ void SwAntennaDiversity(struct net_device *dev)
 			if (priv->AdCheckPeriod > priv->AdMaxCheckPeriod)
 				priv->AdCheckPeriod = priv->AdMaxCheckPeriod;
 
-			/* Wrong deceision => switch back. */
+			/* Wrong decision => switch back. */
 			SwitchAntenna(dev);
 		} else {
 		/* Rx Signal Strength is improved. */
@@ -1057,7 +1057,7 @@ void SwAntennaDiversity(struct net_device *dev)
 		}
 		/*
 		 * <Roger_Notes> We evaluate Rx signal strength ONLY when default antenna
-		 * didn't changed by HW evaluation.
+		 * didn't change by HW evaluation.
 		 * 2008.02.27.
 		 *
 		 * [TRC Dell Lab] SignalStrength is inaccuracy. Isaiah 2008-03-05
@@ -1098,7 +1098,7 @@ void SwAntennaDiversity(struct net_device *dev)
 	priv->AdAuxAntennaRxOkCnt = 0;
 }
 
- /*	Return TRUE if we shall perform Tx Power Tracking Mecahnism, FALSE otherwise. */
+ /*	Return TRUE if we shall perform Tx Power Tracking Mechanism, FALSE otherwise. */
 bool CheckTxPwrTracking(struct net_device *dev)
 {
 	struct r8180_priv *priv = (struct r8180_priv *)ieee80211_priv(dev);
diff --git a/drivers/staging/rtl8187se/r8180_rtl8225z2.c b/drivers/staging/rtl8187se/r8180_rtl8225z2.c
index ee5b867fd0d..d28c1d99608 100644
--- a/drivers/staging/rtl8187se/r8180_rtl8225z2.c
+++ b/drivers/staging/rtl8187se/r8180_rtl8225z2.c
@@ -190,7 +190,7 @@ static void rtl8225_SetTXPowerLevel(struct net_device *dev, short ch)
 		write_phy_cck(dev, 0x44 + i, power);
 	}
 
-	/* FIXME Is this delay really needeed ? */
+	/* FIXME Is this delay really needed ? */
 	force_pci_posting(dev);
 	mdelay(1);
 
@@ -479,7 +479,7 @@ s8 DbmToTxPwrIdx(struct r8180_priv *priv, WIRELESS_MODE WirelessMode,
 
 	/*
 	 * TRUE if we want to use a default implementation.
-	 * We shall set it to FALSE when we have exact translation formular
+	 * We shall set it to FALSE when we have exact translation formula
 	 * for target IC. 070622, by rcnjko.
 	 */
 	if (bUseDefault) {
diff --git a/drivers/staging/rtl8187se/r8180_wx.c b/drivers/staging/rtl8187se/r8180_wx.c
index 303ec691262..46ee6f47f52 100644
--- a/drivers/staging/rtl8187se/r8180_wx.c
+++ b/drivers/staging/rtl8187se/r8180_wx.c
@@ -13,7 +13,7 @@
 
 	Parts of this driver are based on the Intel Pro Wireless 2100 GPL driver.
 
-	We want to tanks the Authors of those projects and the Ndiswrapper
+	We want to thanks the Authors of those projects and the Ndiswrapper
 	project Authors.
 */
 
@@ -1181,7 +1181,7 @@ static iw_handler r8180_wx_handlers[] =	{
 		r8180_wx_set_wap,			/* SIOCSIWAP */
 		r8180_wx_get_wap,			/* SIOCGIWAP */
 		r8180_wx_set_mlme,			/* SIOCSIWMLME*/
-		dummy,					/* SIOCGIWAPLIST -- depricated */
+		dummy,					/* SIOCGIWAPLIST -- deprecated */
 		r8180_wx_set_scan,			/* SIOCSIWSCAN */
 		r8180_wx_get_scan,			/* SIOCGIWSCAN */
 		r8180_wx_set_essid,			/* SIOCSIWESSID */
@@ -1369,7 +1369,7 @@ static inline int is_same_network(struct ieee80211_network *src,
 			(dst->capability & WLAN_CAPABILITY_BSS)));
 }
 
-/* WB modefied to show signal to GUI on 18-01-2008 */
+/* WB modified to show signal to GUI on 18-01-2008 */
 static struct iw_statistics *r8180_get_wireless_stats(struct net_device *dev)
 {
 	struct r8180_priv *priv = ieee80211_priv(dev);
diff --git a/drivers/staging/rtl8187se/r8180_wx.h b/drivers/staging/rtl8187se/r8180_wx.h
index 735d03dceed..40819140311 100644
--- a/drivers/staging/rtl8187se/r8180_wx.h
+++ b/drivers/staging/rtl8187se/r8180_wx.h
@@ -7,7 +7,7 @@
 	Parts of this driver are based on the rtl8180 driver skeleton from Patric Schenke & Andres Salomon
 	Parts of this driver are based on the Intel Pro Wireless 2100 GPL driver
 
-	We want to tanks the Authors of such projects and the Ndiswrapper project Authors.
+	We want to thanks the Authors of such projects and the Ndiswrapper project Authors.
 */
 
 /* this file (will) contains wireless extension handlers*/
diff --git a/drivers/staging/rtl8187se/r8185b_init.c b/drivers/staging/rtl8187se/r8185b_init.c
index 4b0b830f9ab..914495783c0 100644
--- a/drivers/staging/rtl8187se/r8185b_init.c
+++ b/drivers/staging/rtl8187se/r8185b_init.c
@@ -1,22 +1,22 @@
-/*++
-Copyright (c) Realtek Semiconductor Corp. All rights reserved.
-
-Module Name:
-	r8185b_init.c
-
-Abstract:
-	Hardware Initialization and Hardware IO for RTL8185B
-
-Major Change History:
-	When		Who				What
-	----------	---------------		-------------------------------
-	2006-11-15    Xiong		Created
-
-Notes:
-	This file is ported from RTL8185B Windows driver.
-
-
---*/
+/*
+ * Copyright (c) Realtek Semiconductor Corp. All rights reserved.
+ *
+ * Module Name:
+ *	r8185b_init.c
+ *
+ * Abstract:
+ *	Hardware Initialization and Hardware IO for RTL8185B
+ *
+ * Major Change History:
+ *	When		Who				What
+ *	----------	---------------		-------------------------------
+ *	2006-11-15	Xiong			Created
+ *
+ * Notes:
+ *	This file is ported from RTL8185B Windows driver.
+ *
+ *
+ */
 
 /*--------------------------Include File------------------------------------*/
 #include <linux/spinlock.h>
@@ -25,155 +25,134 @@ Notes:
 #include "r8180_rtl8225.h" /* RTL8225 Radio frontend */
 #include "r8180_93cx6.h"   /* Card EEPROM */
 #include "r8180_wx.h"
-
 #include "ieee80211/dot11d.h"
-
-
 /* #define CONFIG_RTL8180_IO_MAP */
-
 #define TC_3W_POLL_MAX_TRY_CNT 5
+
 static u8 MAC_REG_TABLE[][2] =	{
-                        /*PAGA 0:	*/
-						/* 0x34(BRSR), 0xBE(RATE_FALLBACK_CTL), 0x1E0(ARFR) would set in HwConfigureRTL8185()	*/
-						/* 0x272(RFSW_CTRL), 0x1CE(AESMSK_QC) set in InitializeAdapter8185().					*/
-						/* 0x1F0~0x1F8  set in MacConfig_85BASIC()												*/
-						{0x08, 0xae}, {0x0a, 0x72}, {0x5b, 0x42},
-						{0x84, 0x88}, {0x85, 0x24}, {0x88, 0x54}, {0x8b, 0xb8}, {0x8c, 0x03},
-						{0x8d, 0x40}, {0x8e, 0x00}, {0x8f, 0x00}, {0x5b, 0x18}, {0x91, 0x03},
-						{0x94, 0x0F}, {0x95, 0x32},
-						{0x96, 0x00}, {0x97, 0x07}, {0xb4, 0x22}, {0xdb, 0x00},
-						{0xf0, 0x32}, {0xf1, 0x32}, {0xf2, 0x00}, {0xf3, 0x00}, {0xf4, 0x32},
-						{0xf5, 0x43}, {0xf6, 0x00}, {0xf7, 0x00}, {0xf8, 0x46}, {0xf9, 0xa4},
-						{0xfa, 0x00}, {0xfb, 0x00}, {0xfc, 0x96}, {0xfd, 0xa4}, {0xfe, 0x00},
-						{0xff, 0x00},
-
-						/*PAGE 1:	*/
-						/* For Flextronics system Logo PCIHCT failure:	*/
-				/* 0x1C4~0x1CD set no-zero value to avoid PCI configuration space 0x45[7]=1	*/
-						{0x5e, 0x01},
-						{0x58, 0x00}, {0x59, 0x00}, {0x5a, 0x04}, {0x5b, 0x00}, {0x60, 0x24},
-						{0x61, 0x97}, {0x62, 0xF0}, {0x63, 0x09}, {0x80, 0x0F}, {0x81, 0xFF},
-						{0x82, 0xFF}, {0x83, 0x03},
-						{0xC4, 0x22}, {0xC5, 0x22}, {0xC6, 0x22}, {0xC7, 0x22}, {0xC8, 0x22}, /* lzm add 080826 */
-						{0xC9, 0x22}, {0xCA, 0x22}, {0xCB, 0x22}, {0xCC, 0x22}, {0xCD, 0x22},/* lzm add 080826 */
-						{0xe2, 0x00},
-
-
-						/* PAGE 2: */
-						{0x5e, 0x02},
-						{0x0c, 0x04}, {0x4c, 0x30}, {0x4d, 0x08}, {0x50, 0x05}, {0x51, 0xf5},
-						{0x52, 0x04}, {0x53, 0xa0}, {0x54, 0xff}, {0x55, 0xff}, {0x56, 0xff},
-						{0x57, 0xff}, {0x58, 0x08}, {0x59, 0x08}, {0x5a, 0x08}, {0x5b, 0x08},
-						{0x60, 0x08}, {0x61, 0x08}, {0x62, 0x08}, {0x63, 0x08}, {0x64, 0x2f},
-						{0x8c, 0x3f}, {0x8d, 0x3f}, {0x8e, 0x3f},
-						{0x8f, 0x3f}, {0xc4, 0xff}, {0xc5, 0xff}, {0xc6, 0xff}, {0xc7, 0xff},
-						{0xc8, 0x00}, {0xc9, 0x00}, {0xca, 0x80}, {0xcb, 0x00},
-
-						/* PAGA 0: */
-						{0x5e, 0x00}, {0x9f, 0x03}
-			};
-
-
-static u8  ZEBRA_AGC[]	=	{
-			0,
-			0x7E, 0x7E, 0x7E, 0x7E, 0x7D, 0x7C, 0x7B, 0x7A, 0x79, 0x78, 0x77, 0x76, 0x75, 0x74, 0x73, 0x72,
-			0x71, 0x70, 0x6F, 0x6E, 0x6D, 0x6C, 0x6B, 0x6A, 0x69, 0x68, 0x67, 0x66, 0x65, 0x64, 0x63, 0x62,
-			0x48, 0x47, 0x46, 0x45, 0x44, 0x29, 0x28, 0x27, 0x26, 0x25, 0x24, 0x23, 0x22, 0x21, 0x08, 0x07,
-			0x06, 0x05, 0x04, 0x03, 0x02, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
-			0x0f, 0x0f, 0x0f, 0x0f, 0x0f, 0x0f, 0x0f, 0x0f, 0x0f, 0x0f, 0x10, 0x11, 0x12, 0x13, 0x15, 0x16,
-			0x17, 0x17, 0x18, 0x18, 0x19, 0x1a, 0x1a, 0x1b, 0x1b, 0x1c, 0x1c, 0x1d, 0x1d, 0x1d, 0x1e, 0x1e,
-			0x1f, 0x1f, 0x1f, 0x20, 0x20, 0x20, 0x20, 0x21, 0x21, 0x21, 0x22, 0x22, 0x22, 0x23, 0x23, 0x24,
-			0x24, 0x25, 0x25, 0x25, 0x26, 0x26, 0x27, 0x27, 0x2F, 0x2F, 0x2F, 0x2F, 0x2F, 0x2F, 0x2F, 0x2F
-			};
-
-static u32 ZEBRA_RF_RX_GAIN_TABLE[]	=	{
-			0x0096, 0x0076, 0x0056, 0x0036, 0x0016, 0x01f6, 0x01d6, 0x01b6,
-			0x0196, 0x0176, 0x00F7, 0x00D7, 0x00B7, 0x0097, 0x0077, 0x0057,
-			0x0037, 0x00FB, 0x00DB, 0x00BB, 0x00FF, 0x00E3, 0x00C3, 0x00A3,
-			0x0083, 0x0063, 0x0043, 0x0023, 0x0003, 0x01E3, 0x01C3, 0x01A3,
-			0x0183, 0x0163, 0x0143, 0x0123, 0x0103
+	/*PAGA 0:	*/
+	/* 0x34(BRSR), 0xBE(RATE_FALLBACK_CTL), 0x1E0(ARFR) would set in HwConfigureRTL8185() */
+	/* 0x272(RFSW_CTRL), 0x1CE(AESMSK_QC) set in InitializeAdapter8185(). */
+	/* 0x1F0~0x1F8  set in MacConfig_85BASIC() */
+	{0x08, 0xae}, {0x0a, 0x72}, {0x5b, 0x42},
+	{0x84, 0x88}, {0x85, 0x24}, {0x88, 0x54}, {0x8b, 0xb8}, {0x8c, 0x03},
+	{0x8d, 0x40}, {0x8e, 0x00}, {0x8f, 0x00}, {0x5b, 0x18}, {0x91, 0x03},
+	{0x94, 0x0F}, {0x95, 0x32},
+	{0x96, 0x00}, {0x97, 0x07}, {0xb4, 0x22}, {0xdb, 0x00},
+	{0xf0, 0x32}, {0xf1, 0x32}, {0xf2, 0x00}, {0xf3, 0x00}, {0xf4, 0x32},
+	{0xf5, 0x43}, {0xf6, 0x00}, {0xf7, 0x00}, {0xf8, 0x46}, {0xf9, 0xa4},
+	{0xfa, 0x00}, {0xfb, 0x00}, {0xfc, 0x96}, {0xfd, 0xa4}, {0xfe, 0x00},
+	{0xff, 0x00},
+
+	/*PAGE 1: */
+	/* For Flextronics system Logo PCIHCT failure: */
+	/* 0x1C4~0x1CD set no-zero value to avoid PCI configuration space 0x45[7]=1 */
+	{0x5e, 0x01},
+	{0x58, 0x00}, {0x59, 0x00}, {0x5a, 0x04}, {0x5b, 0x00}, {0x60, 0x24},
+	{0x61, 0x97}, {0x62, 0xF0}, {0x63, 0x09}, {0x80, 0x0F}, {0x81, 0xFF},
+	{0x82, 0xFF}, {0x83, 0x03},
+	{0xC4, 0x22}, {0xC5, 0x22}, {0xC6, 0x22}, {0xC7, 0x22}, {0xC8, 0x22}, /* lzm add 080826 */
+	{0xC9, 0x22}, {0xCA, 0x22}, {0xCB, 0x22}, {0xCC, 0x22}, {0xCD, 0x22}, /* lzm add 080826 */
+	{0xe2, 0x00},
+
+
+	/* PAGE 2: */
+	{0x5e, 0x02},
+	{0x0c, 0x04}, {0x4c, 0x30}, {0x4d, 0x08}, {0x50, 0x05}, {0x51, 0xf5},
+	{0x52, 0x04}, {0x53, 0xa0}, {0x54, 0xff}, {0x55, 0xff}, {0x56, 0xff},
+	{0x57, 0xff}, {0x58, 0x08}, {0x59, 0x08}, {0x5a, 0x08}, {0x5b, 0x08},
+	{0x60, 0x08}, {0x61, 0x08}, {0x62, 0x08}, {0x63, 0x08}, {0x64, 0x2f},
+	{0x8c, 0x3f}, {0x8d, 0x3f}, {0x8e, 0x3f},
+	{0x8f, 0x3f}, {0xc4, 0xff}, {0xc5, 0xff}, {0xc6, 0xff}, {0xc7, 0xff},
+	{0xc8, 0x00}, {0xc9, 0x00}, {0xca, 0x80}, {0xcb, 0x00},
+
+	/* PAGA 0: */
+	{0x5e, 0x00}, {0x9f, 0x03}
+	};
+
+
+static u8  ZEBRA_AGC[] = {
+	0,
+	0x7E, 0x7E, 0x7E, 0x7E, 0x7D, 0x7C, 0x7B, 0x7A, 0x79, 0x78, 0x77, 0x76, 0x75, 0x74, 0x73, 0x72,
+	0x71, 0x70, 0x6F, 0x6E, 0x6D, 0x6C, 0x6B, 0x6A, 0x69, 0x68, 0x67, 0x66, 0x65, 0x64, 0x63, 0x62,
+	0x48, 0x47, 0x46, 0x45, 0x44, 0x29, 0x28, 0x27, 0x26, 0x25, 0x24, 0x23, 0x22, 0x21, 0x08, 0x07,
+	0x06, 0x05, 0x04, 0x03, 0x02, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x0f, 0x0f, 0x0f, 0x0f, 0x0f, 0x0f, 0x0f, 0x0f, 0x0f, 0x0f, 0x10, 0x11, 0x12, 0x13, 0x15, 0x16,
+	0x17, 0x17, 0x18, 0x18, 0x19, 0x1a, 0x1a, 0x1b, 0x1b, 0x1c, 0x1c, 0x1d, 0x1d, 0x1d, 0x1e, 0x1e,
+	0x1f, 0x1f, 0x1f, 0x20, 0x20, 0x20, 0x20, 0x21, 0x21, 0x21, 0x22, 0x22, 0x22, 0x23, 0x23, 0x24,
+	0x24, 0x25, 0x25, 0x25, 0x26, 0x26, 0x27, 0x27, 0x2F, 0x2F, 0x2F, 0x2F, 0x2F, 0x2F, 0x2F, 0x2F
+	};
+
+static u32 ZEBRA_RF_RX_GAIN_TABLE[] = {
+	0x0096, 0x0076, 0x0056, 0x0036, 0x0016, 0x01f6, 0x01d6, 0x01b6,
+	0x0196, 0x0176, 0x00F7, 0x00D7, 0x00B7, 0x0097, 0x0077, 0x0057,
+	0x0037, 0x00FB, 0x00DB, 0x00BB, 0x00FF, 0x00E3, 0x00C3, 0x00A3,
+	0x0083, 0x0063, 0x0043, 0x0023, 0x0003, 0x01E3, 0x01C3, 0x01A3,
+	0x0183, 0x0163, 0x0143, 0x0123, 0x0103
 	};
 
-static u8 OFDM_CONFIG[]	=	{
-			/* OFDM reg0x06[7:0]=0xFF: Enable power saving mode in RX				*/
-			/* OFDM reg0x3C[4]=1'b1: Enable RX power saving mode					*/
-			/* ofdm 0x3a = 0x7b ,(original : 0xfb) For ECS shielding room TP test	*/
-
-			/* 0x00	*/
-			0x10, 0x0F, 0x0A, 0x0C, 0x14, 0xFA, 0xFF, 0x50,
-			0x00, 0x50, 0x00, 0x00, 0x00, 0x5C, 0x00, 0x00,
-			/* 0x10	*/
-			0x40, 0x00, 0x40, 0x00, 0x00, 0x00, 0xA8, 0x26,
-			0x32, 0x33, 0x06, 0xA5, 0x6F, 0x55, 0xC8, 0xBB,
-			/* 0x20	*/
-			0x0A, 0xE1, 0x2C, 0x4A, 0x86, 0x83, 0x34, 0x00,
-			0x4F, 0x24, 0x6F, 0xC2, 0x03, 0x40, 0x80, 0x00,
-			/* 0x30	*/
-			0xC0, 0xC1, 0x58, 0xF1, 0x00, 0xC4, 0x90, 0x3e,
-			0xD8, 0x3C, 0x7B, 0x10, 0x10
-		};
-
-/*	---------------------------------------------------------------
-	*	Hardware IO
-	*	the code is ported from Windows source code
-	----------------------------------------------------------------*/
-
-void
-PlatformIOWrite1Byte(
-	struct net_device *dev,
-	u32		offset,
-	u8		data
-	)
+static u8 OFDM_CONFIG[]	= {
+	/* OFDM reg0x06[7:0]=0xFF: Enable power saving mode in RX */
+	/* OFDM reg0x3C[4]=1'b1: Enable RX power saving mode */
+	/* ofdm 0x3a = 0x7b ,(original : 0xfb) For ECS shielding room TP test */
+	/* 0x00	*/
+	0x10, 0x0F, 0x0A, 0x0C, 0x14, 0xFA, 0xFF, 0x50,
+	0x00, 0x50, 0x00, 0x00, 0x00, 0x5C, 0x00, 0x00,
+	/* 0x10	*/
+	0x40, 0x00, 0x40, 0x00, 0x00, 0x00, 0xA8, 0x26,
+	0x32, 0x33, 0x06, 0xA5, 0x6F, 0x55, 0xC8, 0xBB,
+	/* 0x20	*/
+	0x0A, 0xE1, 0x2C, 0x4A, 0x86, 0x83, 0x34, 0x00,
+	0x4F, 0x24, 0x6F, 0xC2, 0x03, 0x40, 0x80, 0x00,
+	/* 0x30	*/
+	0xC0, 0xC1, 0x58, 0xF1, 0x00, 0xC4, 0x90, 0x3e,
+	0xD8, 0x3C, 0x7B, 0x10, 0x10
+	};
+
+	/*---------------------------------------------------------------
+	 *	Hardware IO
+	 *	the code is ported from Windows source code
+	 *---------------------------------------------------------------
+	 */
+
+void PlatformIOWrite1Byte(struct net_device *dev, u32 offset, u8 data)
 {
 	write_nic_byte(dev, offset, data);
-	read_nic_byte(dev, offset); /* To make sure write operation is completed, 2005.11.09, by rcnjko.	*/
-
+	read_nic_byte(dev, offset); /* To make sure write operation is completed, 2005.11.09, by rcnjko. */
 }
 
-void
-PlatformIOWrite2Byte(
-	struct net_device *dev,
-	u32		offset,
-	u16		data
-	)
+void PlatformIOWrite2Byte(struct net_device *dev, u32 offset, u16 data)
 {
 	write_nic_word(dev, offset, data);
 	read_nic_word(dev, offset); /* To make sure write operation is completed, 2005.11.09, by rcnjko. */
-
-
 }
+
 u8 PlatformIORead1Byte(struct net_device *dev, u32 offset);
 
-void
-PlatformIOWrite4Byte(
-	struct net_device *dev,
-	u32		offset,
-	u32		data
-	)
+void PlatformIOWrite4Byte(struct net_device *dev, u32 offset, u32 data)
 {
-/* {by amy 080312 */
-if (offset == PhyAddr)	{
-/* For Base Band configuration. */
+	if (offset == PhyAddr) {
+	/* For Base Band configuration. */
 		unsigned char	cmdByte;
 		unsigned long	dataBytes;
 		unsigned char	idx;
-		u8	u1bTmp;
+		u8		u1bTmp;
 
 		cmdByte = (u8)(data & 0x000000ff);
 		dataBytes = data>>8;
 
 		/*
-			071010, rcnjko:
-			The critical section is only BB read/write race condition.
-			Assumption:
-			1. We assume NO one will access BB at DIRQL, otherwise, system will crash for
-			acquiring the spinlock in such context.
-			2. PlatformIOWrite4Byte() MUST NOT be recursive.
-		*/
-/*		NdisAcquireSpinLock( &(pDevice->IoSpinLock) );	*/
-
-		for (idx = 0; idx < 30; idx++)	{ 
-		/* Make sure command bit is clear before access it.	*/
+		 *	071010, rcnjko:
+		 *	The critical section is only BB read/write race condition.
+		 *	Assumption:
+		 *	1. We assume NO one will access BB at DIRQL, otherwise, system will crash for
+		 *	acquiring the spinlock in such context.
+		 *	2. PlatformIOWrite4Byte() MUST NOT be recursive.
+		 */
+		/* NdisAcquireSpinLock( &(pDevice->IoSpinLock) ); */
+
+		for (idx = 0; idx < 30; idx++) {
+		/* Make sure command bit is clear before access it. */
 			u1bTmp = PlatformIORead1Byte(dev, PhyAddr);
 			if ((u1bTmp & BIT7) == 0)
 				break;
@@ -186,20 +165,14 @@ if (offset == PhyAddr)	{
 
 		write_nic_byte(dev, offset, cmdByte);
 
-/*		NdisReleaseSpinLock( &(pDevice->IoSpinLock) ); */
-	}
-/* by amy 080312} */
-	else	{
+		/* NdisReleaseSpinLock( &(pDevice->IoSpinLock) ); */
+	} else {
 		write_nic_dword(dev, offset, data);
 		read_nic_dword(dev, offset); /* To make sure write operation is completed, 2005.11.09, by rcnjko. */
 	}
 }
 
-u8
-PlatformIORead1Byte(
-	struct net_device *dev,
-	u32		offset
-	)
+u8 PlatformIORead1Byte(struct net_device *dev, u32 offset)
 {
 	u8	data = 0;
 
@@ -209,11 +182,7 @@ PlatformIORead1Byte(
 	return data;
 }
 
-u16
-PlatformIORead2Byte(
-	struct net_device *dev,
-	u32		offset
-	)
+u16 PlatformIORead2Byte(struct net_device *dev, u32 offset)
 {
 	u16	data = 0;
 
@@ -223,11 +192,7 @@ PlatformIORead2Byte(
 	return data;
 }
 
-u32
-PlatformIORead4Byte(
-	struct net_device *dev,
-	u32		offset
-	)
+u32 PlatformIORead4Byte(struct net_device *dev, u32 offset)
 {
 	u32	data = 0;
 
@@ -242,22 +207,19 @@ void SetOutputEnableOfRfPins(struct net_device *dev)
 	write_nic_word(dev, RFPinsEnable, 0x1bff);
 }
 
-static int
-HwHSSIThreeWire(
-	struct net_device *dev,
-	u8			*pDataBuf,
-	u8			nDataBufBitCnt,
-	int			bSI,
-	int			bWrite
-	)
+static int HwHSSIThreeWire(struct net_device *dev,
+			   u8 *pDataBuf,
+			   u8 nDataBufBitCnt,
+			   int bSI,
+			   int bWrite)
 {
 	int	bResult = 1;
 	u8	TryCnt;
 	u8	u1bTmp;
 
-	do	{
+	do {
 		/* Check if WE and RE are cleared. */
-		for (TryCnt = 0; TryCnt < TC_3W_POLL_MAX_TRY_CNT; TryCnt++)	{
+		for (TryCnt = 0; TryCnt < TC_3W_POLL_MAX_TRY_CNT; TryCnt++) {
 			u1bTmp = read_nic_byte(dev, SW_3W_CMD1);
 			if ((u1bTmp & (SW_3W_CMD1_RE|SW_3W_CMD1_WE)) == 0)
 				break;
@@ -275,15 +237,15 @@ HwHSSIThreeWire(
 		u1bTmp = read_nic_byte(dev, RF_SW_CONFIG);
 
 		if (bSI)
-			u1bTmp |=   RF_SW_CFG_SI;   /* reg08[1]=1 Serial Interface(SI)	*/
+			u1bTmp |=   RF_SW_CFG_SI; /* reg08[1]=1 Serial Interface(SI) */
 
 		else
-			u1bTmp &= ~RF_SW_CFG_SI;  /* reg08[1]=0 Parallel Interface(PI)	*/
+			u1bTmp &= ~RF_SW_CFG_SI; /* reg08[1]=0 Parallel Interface(PI) */
 
 
 		write_nic_byte(dev, RF_SW_CONFIG, u1bTmp);
 
-		if (bSI)	{
+		if (bSI) {
 			/* jong: HW SI read must set reg84[3]=0. */
 			u1bTmp = read_nic_byte(dev, RFPinsSelect);
 			u1bTmp &= ~BIT3;
@@ -291,14 +253,14 @@ HwHSSIThreeWire(
 		}
 		/*  Fill up data buffer for write operation. */
 
-		if (bWrite)	{
-			if (nDataBufBitCnt == 16)	{
+		if (bWrite) {
+			if (nDataBufBitCnt == 16) {
 				write_nic_word(dev, SW_3W_DB0, *((u16 *)pDataBuf));
-			}	else if (nDataBufBitCnt == 64)	{
+			} else if (nDataBufBitCnt == 64) {
 			/* RTL8187S shouldn't enter this case */
 				write_nic_dword(dev, SW_3W_DB0, *((u32 *)pDataBuf));
 				write_nic_dword(dev, SW_3W_DB1, *((u32 *)(pDataBuf + 4)));
-			}	else	{
+			} else {
 				int idx;
 				int ByteCnt = nDataBufBitCnt / 8;
 								/* printk("%d\n",nDataBufBitCnt); */
@@ -324,11 +286,11 @@ HwHSSIThreeWire(
 					write_nic_byte(dev, (SW_3W_DB0+idx), *(pDataBuf+idx));
 
 			}
-		}	else	{	/* read */
-			if (bSI)	{
-				/* SI - reg274[3:0] : RF register's Address	*/
+		} else {	/* read */
+			if (bSI) {
+				/* SI - reg274[3:0] : RF register's Address */
 				write_nic_word(dev, SW_3W_DB0, *((u16 *)pDataBuf));
-			}	else	{
+			} else {
 				/*  PI - reg274[15:12] : RF register's Address */
 				write_nic_word(dev, SW_3W_DB0, (*((u16 *)pDataBuf)) << 12);
 			}
@@ -343,7 +305,7 @@ HwHSSIThreeWire(
 
 
 		/* Check if DONE is set. */
-		for (TryCnt = 0; TryCnt < TC_3W_POLL_MAX_TRY_CNT; TryCnt++)	{
+		for (TryCnt = 0; TryCnt < TC_3W_POLL_MAX_TRY_CNT; TryCnt++) {
 			u1bTmp = read_nic_byte(dev, SW_3W_CMD1);
 			if ((u1bTmp & SW_3W_CMD1_DONE) != 0)
 				break;
@@ -353,12 +315,12 @@ HwHSSIThreeWire(
 
 		write_nic_byte(dev, SW_3W_CMD1, 0);
 
-		/* Read back data for read operation.	*/
-		if (bWrite == 0)	{
-			if (bSI)		{
+		/* Read back data for read operation. */
+		if (bWrite == 0) {
+			if (bSI) {
 				/* Serial Interface : reg363_362[11:0] */
 				*((u16 *)pDataBuf) = read_nic_word(dev, SI_DATA_READ) ;
-			}	else	{
+			} else {
 				/* Parallel Interface : reg361_360[11:0] */
 				*((u16 *)pDataBuf) = read_nic_word(dev, PI_DATA_READ);
 			}
@@ -366,13 +328,12 @@ HwHSSIThreeWire(
 			*((u16 *)pDataBuf) &= 0x0FFF;
 		}
 
-	}	while (0);
+	} while (0);
 
 	return bResult;
 }
 
-void
-RF_WriteReg(struct net_device *dev, u8 offset, u32 data)
+void RF_WriteReg(struct net_device *dev, u8 offset, u32 data)
 {
 	u32 data2Write;
 	u8 len;
@@ -400,11 +361,7 @@ u32 RF_ReadReg(struct net_device *dev, u8 offset)
 
 
 /* by Owen on 04/07/14 for writing BB register successfully */
-void
-WriteBBPortUchar(
-	struct net_device *dev,
-	u32		Data
-	)
+void WriteBBPortUchar(struct net_device *dev, u32 Data)
 {
 	/* u8	TimeoutCounter; */
 	u8	RegisterContent;
@@ -421,11 +378,7 @@ WriteBBPortUchar(
 	}
 }
 
-u8
-ReadBBPortUchar(
-	struct net_device *dev,
-	u32		addr
-	)
+u8 ReadBBPortUchar(struct net_device *dev, u32 addr)
 {
 	/*u8	TimeoutCounter; */
 	u8	RegisterContent;
@@ -435,66 +388,62 @@ ReadBBPortUchar(
 
 	return RegisterContent;
 }
-/* {by amy 080312 */
 /*
-	Description:
-	Perform Antenna settings with antenna diversity on 87SE.
-		Created by Roger, 2008.01.25.
-*/
-bool
-SetAntennaConfig87SE(
-	struct net_device *dev,
-	u8			DefaultAnt,		/* 0: Main, 1: Aux.			*/
-	bool		bAntDiversity	/* 1:Enable, 0: Disable.	*/
-)
+ *	Description:
+ *	Perform Antenna settings with antenna diversity on 87SE.
+ *		Created by Roger, 2008.01.25.
+ */
+bool SetAntennaConfig87SE(struct net_device *dev,
+			  u8   DefaultAnt, /* 0: Main, 1: Aux. */
+			  bool bAntDiversity) /* 1:Enable, 0: Disable. */
 {
 	struct r8180_priv *priv = (struct r8180_priv *)ieee80211_priv(dev);
 	bool   bAntennaSwitched = true;
 
 	/* printk("SetAntennaConfig87SE(): DefaultAnt(%d), bAntDiversity(%d)\n", DefaultAnt, bAntDiversity); */
 
-	/* Threshold for antenna diversity.	*/
+	/* Threshold for antenna diversity. */
 	write_phy_cck(dev, 0x0c, 0x09); /* Reg0c : 09 */
 
-	if (bAntDiversity)	{	/*	Enable Antenna Diversity.	*/
-		if (DefaultAnt == 1)	{	/* aux antenna			*/
+	if (bAntDiversity) {	/*	Enable Antenna Diversity. */
+		if (DefaultAnt == 1) {	/* aux antenna */
 
-			/* Mac register, aux antenna	*/
+			/* Mac register, aux antenna */
 			write_nic_byte(dev, ANTSEL, 0x00);
 
-			/* Config CCK RX antenna.		*/
+			/* Config CCK RX antenna. */
 			write_phy_cck(dev, 0x11, 0xbb); /* Reg11 : bb */
 			write_phy_cck(dev, 0x01, 0xc7); /* Reg01 : c7 */
 
-			/* Config OFDM RX antenna.	*/
-			write_phy_ofdm(dev, 0x0D, 0x54);	/* Reg0d : 54	*/
-			write_phy_ofdm(dev, 0x18, 0xb2);	/* Reg18 : b2		*/
-		}	else	{	/*  use main antenna	*/
-			/* Mac register, main antenna		*/
+			/* Config OFDM RX antenna. */
+			write_phy_ofdm(dev, 0x0D, 0x54);	/* Reg0d : 54 */
+			write_phy_ofdm(dev, 0x18, 0xb2);	/* Reg18 : b2 */
+		} else { /*  use main antenna */
+			/* Mac register, main antenna */
 			write_nic_byte(dev, ANTSEL, 0x03);
-			/* base band				*/
-			/*  Config CCK RX antenna.	*/
-			write_phy_cck(dev, 0x11, 0x9b); /* Reg11 : 9b	*/
-			write_phy_cck(dev, 0x01, 0xc7); /* Reg01 : c7	*/
+			/* base band */
+			/*  Config CCK RX antenna. */
+			write_phy_cck(dev, 0x11, 0x9b); /* Reg11 : 9b */
+			write_phy_cck(dev, 0x01, 0xc7); /* Reg01 : c7 */
 
 			/* Config OFDM RX antenna. */
 			write_phy_ofdm(dev, 0x0d, 0x5c);  /* Reg0d : 5c	*/
 			write_phy_ofdm(dev, 0x18, 0xb2);  /* Reg18 : b2	*/
 		}
-	}	else	{
+	} else {
 		/* Disable Antenna Diversity. */
-		if (DefaultAnt == 1)	{	/* aux Antenna */
+		if (DefaultAnt == 1) { /* aux Antenna */
 			/* Mac register, aux antenna */
 			write_nic_byte(dev, ANTSEL, 0x00);
 
 			/* Config CCK RX antenna. */
-			write_phy_cck(dev, 0x11, 0xbb); /* Reg11 : bb	*/
-			write_phy_cck(dev, 0x01, 0x47); /* Reg01 : 47	*/
+			write_phy_cck(dev, 0x11, 0xbb); /* Reg11 : bb */
+			write_phy_cck(dev, 0x01, 0x47); /* Reg01 : 47 */
 
 			/* Config OFDM RX antenna. */
-			write_phy_ofdm(dev, 0x0D, 0x54);	/* Reg0d : 54	*/
-			write_phy_ofdm(dev, 0x18, 0x32);	/* Reg18 : 32	*/
-		}	else	{	/* main Antenna */
+			write_phy_ofdm(dev, 0x0D, 0x54);	/* Reg0d : 54 */
+			write_phy_ofdm(dev, 0x18, 0x32);	/* Reg18 : 32 */
+		} else { /* main Antenna */
 			/* Mac register, main antenna */
 			write_nic_byte(dev, ANTSEL, 0x03);
 
@@ -502,25 +451,22 @@ SetAntennaConfig87SE(
 			write_phy_cck(dev, 0x11, 0x9b); /* Reg11 : 9b */
 			write_phy_cck(dev, 0x01, 0x47); /* Reg01 : 47 */
 
-			/* Config OFDM RX antenna.		*/
-			write_phy_ofdm(dev, 0x0D, 0x5c);	/* Reg0d : 5c	*/
-			write_phy_ofdm(dev, 0x18, 0x32);	/*Reg18 : 32	*/
+			/* Config OFDM RX antenna. */
+			write_phy_ofdm(dev, 0x0D, 0x5c); /* Reg0d : 5c */
+			write_phy_ofdm(dev, 0x18, 0x32); /*Reg18 : 32 */
 		}
 	}
 	priv->CurrAntennaIndex = DefaultAnt; /* Update default settings. */
 	return	bAntennaSwitched;
 }
-/* by amy 080312 */
 /*
----------------------------------------------------------------
-	*	Hardware Initialization.
-	*	the code is ported from Windows source code
-----------------------------------------------------------------*/
-
-void
-ZEBRA_Config_85BASIC_HardCode(
-	struct net_device *dev
-	)
+ *--------------------------------------------------------------
+ *		Hardware Initialization.
+ *		the code is ported from Windows source code
+ *--------------------------------------------------------------
+ */
+
+void ZEBRA_Config_85BASIC_HardCode(struct net_device *dev)
 {
 
 	struct r8180_priv *priv = (struct r8180_priv *)ieee80211_priv(dev);
@@ -532,163 +478,151 @@ ZEBRA_Config_85BASIC_HardCode(
 
 
 /*
-=============================================================================
-	87S_PCIE :: RADIOCFG.TXT
-=============================================================================
-*/
+ *===========================================================================
+ *	87S_PCIE :: RADIOCFG.TXT
+ *===========================================================================
+ */
 
 
 	/* Page1 : reg16-reg30 */
-	RF_WriteReg(dev, 0x00, 0x013f);			mdelay(1); /* switch to page1 */
-	u4bRF23 = RF_ReadReg(dev, 0x08);			mdelay(1);
-	u4bRF24 = RF_ReadReg(dev, 0x09);			mdelay(1);
+	RF_WriteReg(dev, 0x00, 0x013f);		mdelay(1); /* switch to page1 */
+	u4bRF23 = RF_ReadReg(dev, 0x08);	mdelay(1);
+	u4bRF24 = RF_ReadReg(dev, 0x09);	mdelay(1);
 
 	if (u4bRF23 == 0x818 && u4bRF24 == 0x70C) {
 		d_cut = 1;
 		printk(KERN_INFO "rtl8187se: card type changed from C- to D-cut\n");
 	}
 
-	/* Page0 : reg0-reg15	*/
-
-	RF_WriteReg(dev, 0x00, 0x009f);			mdelay(1);/* 1	*/
-
-	RF_WriteReg(dev, 0x01, 0x06e0);			mdelay(1);
-
-	RF_WriteReg(dev, 0x02, 0x004d);			mdelay(1);/* 2	*/
-
-	RF_WriteReg(dev, 0x03, 0x07f1);			mdelay(1);/* 3	*/
-
-	RF_WriteReg(dev, 0x04, 0x0975);			mdelay(1);
-	RF_WriteReg(dev, 0x05, 0x0c72);			mdelay(1);
-	RF_WriteReg(dev, 0x06, 0x0ae6);			mdelay(1);
-	RF_WriteReg(dev, 0x07, 0x00ca);			mdelay(1);
-	RF_WriteReg(dev, 0x08, 0x0e1c);			mdelay(1);
-	RF_WriteReg(dev, 0x09, 0x02f0);			mdelay(1);
-	RF_WriteReg(dev, 0x0a, 0x09d0);			mdelay(1);
-	RF_WriteReg(dev, 0x0b, 0x01ba);			mdelay(1);
-	RF_WriteReg(dev, 0x0c, 0x0640);			mdelay(1);
-	RF_WriteReg(dev, 0x0d, 0x08df);			mdelay(1);
-	RF_WriteReg(dev, 0x0e, 0x0020);			mdelay(1);
-	RF_WriteReg(dev, 0x0f, 0x0990);			mdelay(1);
-
+	/* Page0 : reg0-reg15 */
+
+	RF_WriteReg(dev, 0x00, 0x009f);		mdelay(1);/* 1	*/
+	RF_WriteReg(dev, 0x01, 0x06e0);		mdelay(1);
+	RF_WriteReg(dev, 0x02, 0x004d);		mdelay(1);/* 2	*/
+	RF_WriteReg(dev, 0x03, 0x07f1);		mdelay(1);/* 3	*/
+	RF_WriteReg(dev, 0x04, 0x0975);		mdelay(1);
+	RF_WriteReg(dev, 0x05, 0x0c72);		mdelay(1);
+	RF_WriteReg(dev, 0x06, 0x0ae6);		mdelay(1);
+	RF_WriteReg(dev, 0x07, 0x00ca);		mdelay(1);
+	RF_WriteReg(dev, 0x08, 0x0e1c);		mdelay(1);
+	RF_WriteReg(dev, 0x09, 0x02f0);		mdelay(1);
+	RF_WriteReg(dev, 0x0a, 0x09d0);		mdelay(1);
+	RF_WriteReg(dev, 0x0b, 0x01ba);		mdelay(1);
+	RF_WriteReg(dev, 0x0c, 0x0640);		mdelay(1);
+	RF_WriteReg(dev, 0x0d, 0x08df);		mdelay(1);
+	RF_WriteReg(dev, 0x0e, 0x0020);		mdelay(1);
+	RF_WriteReg(dev, 0x0f, 0x0990);		mdelay(1);
 
 	/*  Page1 : reg16-reg30 */
-	RF_WriteReg(dev, 0x00, 0x013f);			mdelay(1);
-
-	RF_WriteReg(dev, 0x03, 0x0806);			mdelay(1);
-
-	RF_WriteReg(dev, 0x04, 0x03a7);			mdelay(1);
-	RF_WriteReg(dev, 0x05, 0x059b);			mdelay(1);
-	RF_WriteReg(dev, 0x06, 0x0081);			mdelay(1);
-
-
-	RF_WriteReg(dev, 0x07, 0x01A0);			mdelay(1);
+	RF_WriteReg(dev, 0x00, 0x013f);		mdelay(1);
+	RF_WriteReg(dev, 0x03, 0x0806);		mdelay(1);
+	RF_WriteReg(dev, 0x04, 0x03a7);		mdelay(1);
+	RF_WriteReg(dev, 0x05, 0x059b);		mdelay(1);
+	RF_WriteReg(dev, 0x06, 0x0081);		mdelay(1);
+	RF_WriteReg(dev, 0x07, 0x01A0);		mdelay(1);
 /* Don't write RF23/RF24 to make a difference between 87S C cut and D cut. asked by SD3 stevenl. */
-	RF_WriteReg(dev, 0x0a, 0x0001);			mdelay(1);
-	RF_WriteReg(dev, 0x0b, 0x0418);			mdelay(1);
+	RF_WriteReg(dev, 0x0a, 0x0001);		mdelay(1);
+	RF_WriteReg(dev, 0x0b, 0x0418);		mdelay(1);
 
 	if (d_cut) {
-		RF_WriteReg(dev, 0x0c, 0x0fbe);			mdelay(1);
-		RF_WriteReg(dev, 0x0d, 0x0008);			mdelay(1);
-		RF_WriteReg(dev, 0x0e, 0x0807);			mdelay(1); /* RX LO buffer */
-	}	else	{
-		RF_WriteReg(dev, 0x0c, 0x0fbe);			mdelay(1);
-		RF_WriteReg(dev, 0x0d, 0x0008);			mdelay(1);
-		RF_WriteReg(dev, 0x0e, 0x0806);			mdelay(1); /* RX LO buffer */
+		RF_WriteReg(dev, 0x0c, 0x0fbe);		mdelay(1);
+		RF_WriteReg(dev, 0x0d, 0x0008);		mdelay(1);
+		RF_WriteReg(dev, 0x0e, 0x0807);		mdelay(1); /* RX LO buffer */
+	} else {
+		RF_WriteReg(dev, 0x0c, 0x0fbe);		mdelay(1);
+		RF_WriteReg(dev, 0x0d, 0x0008);		mdelay(1);
+		RF_WriteReg(dev, 0x0e, 0x0806);		mdelay(1); /* RX LO buffer */
 	}
 
-	RF_WriteReg(dev, 0x0f, 0x0acc);			mdelay(1);
-
-	RF_WriteReg(dev, 0x00, 0x01d7);			mdelay(1); /* 6 */
+	RF_WriteReg(dev, 0x0f, 0x0acc);		mdelay(1);
+	RF_WriteReg(dev, 0x00, 0x01d7);		mdelay(1); /* 6 */
+	RF_WriteReg(dev, 0x03, 0x0e00);		mdelay(1);
+	RF_WriteReg(dev, 0x04, 0x0e50);		mdelay(1);
 
-	RF_WriteReg(dev, 0x03, 0x0e00);			mdelay(1);
-	RF_WriteReg(dev, 0x04, 0x0e50);			mdelay(1);
-	for (i = 0; i <= 36; i++)	{
-		RF_WriteReg(dev, 0x01, i);                     mdelay(1);
+	for (i = 0; i <= 36; i++) {
+		RF_WriteReg(dev, 0x01, i);		mdelay(1);
 		RF_WriteReg(dev, 0x02, ZEBRA_RF_RX_GAIN_TABLE[i]); mdelay(1);
 	}
 
-	RF_WriteReg(dev, 0x05, 0x0203);			mdelay(1);	/* 203, 343	*/
-	RF_WriteReg(dev, 0x06, 0x0200);			mdelay(1);	/* 400		*/
+	RF_WriteReg(dev, 0x05, 0x0203);		mdelay(1); /* 203, 343 */
+	RF_WriteReg(dev, 0x06, 0x0200);		mdelay(1); /* 400 */
+	RF_WriteReg(dev, 0x00, 0x0137);		mdelay(1); /* switch to reg16-reg30, and HSSI disable 137 */
+	mdelay(10); /* Deay 10 ms. */		/* 0xfd */
 
-	RF_WriteReg(dev, 0x00, 0x0137);			mdelay(1);	/* switch to reg16-reg30, and HSSI disable 137	*/
-	mdelay(10);		/* Deay 10 ms.	*/	/* 0xfd */
+	RF_WriteReg(dev, 0x0d, 0x0008);		mdelay(1); /* Z4 synthesizer loop filter setting, 392 */
+	mdelay(10); /* Deay 10 ms. */		/* 0xfd */
 
-	RF_WriteReg(dev, 0x0d, 0x0008);			mdelay(1);	/* Z4 synthesizer loop filter setting, 392		*/
-	mdelay(10);		/* Deay 10 ms.	*/	/* 0xfd */
+	RF_WriteReg(dev, 0x00, 0x0037);		mdelay(1); /* switch to reg0-reg15, and HSSI disable */
+	mdelay(10); /* Deay 10 ms. */		/* 0xfd	*/
 
-	RF_WriteReg(dev, 0x00, 0x0037);			mdelay(1);	/* switch to reg0-reg15, and HSSI disable		*/
-	mdelay(10);		/* Deay 10 ms.	*/	/* 0xfd	*/
+	RF_WriteReg(dev, 0x04, 0x0160);		mdelay(1); /* CBC on, Tx Rx disable, High gain */
+	mdelay(10); /* Deay 10 ms. */		/* 0xfd	*/
 
-	RF_WriteReg(dev, 0x04, 0x0160);			mdelay(1);	/* CBC on, Tx Rx disable, High gain				*/
-	mdelay(10);		/* Deay 10 ms.	*/	/* 0xfd	*/
+	RF_WriteReg(dev, 0x07, 0x0080);		mdelay(1); /* Z4 setted channel 1 */
+	mdelay(10); /* Deay 10 ms. */		/* 0xfd	*/
 
-	RF_WriteReg(dev, 0x07, 0x0080);			mdelay(1);	/* Z4 setted channel 1							*/
-	mdelay(10);		/* Deay 10 ms.	*/	/* 0xfd	*/
+	RF_WriteReg(dev, 0x02, 0x088D);		mdelay(1); /* LC calibration */
+	mdelay(200); /* Deay 200 ms. */		/* 0xfd	*/
+	mdelay(10);  /* Deay 10 ms. */		/* 0xfd	*/
+	mdelay(10);  /* Deay 10 ms. */		/* 0xfd	*/
 
-	RF_WriteReg(dev, 0x02, 0x088D);			mdelay(1);	/* LC calibration								*/
-	mdelay(200);	/* Deay 200 ms.	*/	/* 0xfd	*/
-	mdelay(10);		/* Deay 10 ms.	*/	/* 0xfd	*/
-	mdelay(10);		/* Deay 10 ms.	*/	/* 0xfd	*/
+	RF_WriteReg(dev, 0x00, 0x0137);		mdelay(1); /* switch to reg16-reg30 137, and HSSI disable 137 */
+	mdelay(10); /* Deay 10 ms. */		/* 0xfd	*/
 
-	RF_WriteReg(dev, 0x00, 0x0137);			mdelay(1);	/* switch to reg16-reg30 137, and HSSI disable 137	*/
-	mdelay(10);		/* Deay 10 ms.	*/	/* 0xfd	*/
-
-	RF_WriteReg(dev, 0x07, 0x0000);			mdelay(1);
-	RF_WriteReg(dev, 0x07, 0x0180);			mdelay(1);
-	RF_WriteReg(dev, 0x07, 0x0220);			mdelay(1);
-	RF_WriteReg(dev, 0x07, 0x03E0);			mdelay(1);
+	RF_WriteReg(dev, 0x07, 0x0000);		mdelay(1);
+	RF_WriteReg(dev, 0x07, 0x0180);		mdelay(1);
+	RF_WriteReg(dev, 0x07, 0x0220);		mdelay(1);
+	RF_WriteReg(dev, 0x07, 0x03E0);		mdelay(1);
 
 	/* DAC calibration off 20070702	*/
-	RF_WriteReg(dev, 0x06, 0x00c1);			mdelay(1);
-	RF_WriteReg(dev, 0x0a, 0x0001);			mdelay(1);
-/* {by amy 080312 */
+	RF_WriteReg(dev, 0x06, 0x00c1);		mdelay(1);
+	RF_WriteReg(dev, 0x0a, 0x0001);		mdelay(1);
 	/* For crystal calibration, added by Roger, 2007.12.11. */
-	if (priv->bXtalCalibration)	{	/* reg 30.	*/
-	 /* enable crystal calibration.
-			RF Reg[30], (1)Xin:[12:9], Xout:[8:5],  addr[4:0].
-			(2)PA Pwr delay timer[15:14], default: 2.4us, set BIT15=0
-			(3)RF signal on/off when calibration[13], default: on, set BIT13=0.
-			So we should minus 4 BITs offset.		*/
-		RF_WriteReg(dev, 0x0f, (priv->XtalCal_Xin<<5) | (priv->XtalCal_Xout<<1) | BIT11 | BIT9);			mdelay(1);
+	if (priv->bXtalCalibration) { /* reg 30.	*/
+	 /*
+ 	  *  enable crystal calibration.
+	  *		RF Reg[30], (1)Xin:[12:9], Xout:[8:5],  addr[4:0].
+	  *		(2)PA Pwr delay timer[15:14], default: 2.4us, set BIT15=0
+	  *		(3)RF signal on/off when calibration[13], default: on, set BIT13=0.
+	  *		So we should minus 4 BITs offset. 
+	  */
+		RF_WriteReg(dev, 0x0f, (priv->XtalCal_Xin<<5) | (priv->XtalCal_Xout<<1) | BIT11 | BIT9); mdelay(1);
 		printk("ZEBRA_Config_85BASIC_HardCode(): (%02x)\n",
-				(priv->XtalCal_Xin<<5) | (priv->XtalCal_Xout<<1) | BIT11 | BIT9);
-	}	else	{
+		      (priv->XtalCal_Xin<<5) | (priv->XtalCal_Xout<<1) | BIT11 | BIT9);
+	} else {
 		/* using default value. Xin=6, Xout=6.	*/
-		RF_WriteReg(dev, 0x0f, 0x0acc);			mdelay(1);
+		RF_WriteReg(dev, 0x0f, 0x0acc);		mdelay(1);
 	}
-/* by amy 080312 */
 
-	RF_WriteReg(dev, 0x00, 0x00bf);			mdelay(1); /* switch to reg0-reg15, and HSSI enable	*/
-	RF_WriteReg(dev, 0x0d, 0x08df);			mdelay(1); /* Rx BB start calibration, 00c//+edward	*/
-	RF_WriteReg(dev, 0x02, 0x004d);			mdelay(1); /* temperature meter off					*/
-	RF_WriteReg(dev, 0x04, 0x0975);			mdelay(1); /* Rx mode								*/
+	RF_WriteReg(dev, 0x00, 0x00bf);		mdelay(1); /* switch to reg0-reg15, and HSSI enable */
+	RF_WriteReg(dev, 0x0d, 0x08df);		mdelay(1); /* Rx BB start calibration, 00c//+edward */
+	RF_WriteReg(dev, 0x02, 0x004d);		mdelay(1); /* temperature meter off */
+	RF_WriteReg(dev, 0x04, 0x0975);		mdelay(1); /* Rx mode */
 	mdelay(10);	/* Deay 10 ms.*/	/* 0xfe	*/
 	mdelay(10);	/* Deay 10 ms.*/	/* 0xfe	*/
 	mdelay(10);	/* Deay 10 ms.*/	/* 0xfe	*/
-	RF_WriteReg(dev, 0x00, 0x0197);			mdelay(1); /* Rx mode*/	/*+edward	*/
-	RF_WriteReg(dev, 0x05, 0x05ab);			mdelay(1); /* Rx mode*/	/*+edward	*/
-	RF_WriteReg(dev, 0x00, 0x009f);			mdelay(1); /* Rx mode*/	/*+edward	*/
-
-	RF_WriteReg(dev, 0x01, 0x0000);			mdelay(1); /* Rx mode*/	/*+edward	*/
-	RF_WriteReg(dev, 0x02, 0x0000);			mdelay(1); /* Rx mode*/	/*+edward	*/
-	/* power save parameters.	*/
+	RF_WriteReg(dev, 0x00, 0x0197);		mdelay(1); /* Rx mode*/	/*+edward */
+	RF_WriteReg(dev, 0x05, 0x05ab);		mdelay(1); /* Rx mode*/	/*+edward */
+	RF_WriteReg(dev, 0x00, 0x009f);		mdelay(1); /* Rx mode*/	/*+edward */
+	RF_WriteReg(dev, 0x01, 0x0000);		mdelay(1); /* Rx mode*/	/*+edward */
+	RF_WriteReg(dev, 0x02, 0x0000);		mdelay(1); /* Rx mode*/	/*+edward */
+	/* power save parameters. */
 	u1b24E = read_nic_byte(dev, 0x24E);
 	write_nic_byte(dev, 0x24E, (u1b24E & (~(BIT5|BIT6))));
 
 	/*=============================================================================
-
-	=============================================================================
-	CCKCONF.TXT
-	=============================================================================
-	*/
-	/*	[POWER SAVE] Power Saving Parameters by jong. 2007-11-27
-		CCK reg0x00[7]=1'b1 :power saving for TX (default)
-		CCK reg0x00[6]=1'b1: power saving for RX (default)
-		CCK reg0x06[4]=1'b1: turn off channel estimation related circuits if not doing channel estimation.
-		CCK reg0x06[3]=1'b1: turn off unused circuits before cca = 1
-		CCK reg0x06[2]=1'b1: turn off cck's circuit if macrst =0
-	*/
+	 *
+	 *===========================================================================
+	 * CCKCONF.TXT
+	 *===========================================================================
+	 *
+	 *	[POWER SAVE] Power Saving Parameters by jong. 2007-11-27
+	 *	CCK reg0x00[7]=1'b1 :power saving for TX (default)
+	 *	CCK reg0x00[6]=1'b1: power saving for RX (default)
+	 *	CCK reg0x06[4]=1'b1: turn off channel estimation related circuits if not doing channel estimation.
+	 *	CCK reg0x06[3]=1'b1: turn off unused circuits before cca = 1
+	 *	CCK reg0x06[2]=1'b1: turn off cck's circuit if macrst =0
+	 */
 
 	write_phy_cck(dev, 0x00, 0xc8);
 	write_phy_cck(dev, 0x06, 0x1c);
@@ -697,7 +631,7 @@ ZEBRA_Config_85BASIC_HardCode(
 	write_phy_cck(dev, 0x2f, 0x06);
 	write_phy_cck(dev, 0x01, 0x46);
 
-	/* power control	*/
+	/* power control */
 	write_nic_byte(dev, CCK_TXAGC, 0x10);
 	write_nic_byte(dev, OFDM_TXAGC, 0x1B);
 	write_nic_byte(dev, ANTSEL, 0x03);
@@ -705,14 +639,14 @@ ZEBRA_Config_85BASIC_HardCode(
 
 
 	/*
-	=============================================================================
-		AGC.txt
-	=============================================================================
-	*/
+	 *===========================================================================
+	 *	AGC.txt
+	 *===========================================================================
+	 */
 
 	write_phy_ofdm(dev, 0x00, 0x12);
 
-	for (i = 0; i < 128; i++)	{
+	for (i = 0; i < 128; i++) {
 
 		data = ZEBRA_AGC[i+1];
 		data = data << 8;
@@ -730,49 +664,43 @@ ZEBRA_Config_85BASIC_HardCode(
 	PlatformIOWrite4Byte(dev, PhyAddr, 0x00001080);	/* Annie, 2006-05-05 */
 
 	/*
-	=============================================================================
-
-	=============================================================================
-	OFDMCONF.TXT
-	=============================================================================
-	*/
-
-	for (i = 0; i < 60; i++)	{
+	 *===========================================================================
+	 *
+	 *===========================================================================
+	 * OFDMCONF.TXT
+	 *===========================================================================
+	 */
+
+	for (i = 0; i < 60; i++) {
 		u4bRegOffset = i;
 		u4bRegValue = OFDM_CONFIG[i];
 
 		WriteBBPortUchar(dev,
-						(0x00000080 |
-						(u4bRegOffset & 0x7f) |
-						((u4bRegValue & 0xff) << 8)));
+				(0x00000080 |
+				(u4bRegOffset & 0x7f) |
+				((u4bRegValue & 0xff) << 8)));
 	}
 
 	/*
-	=============================================================================
-	by amy for antenna
-	=============================================================================
-	*/
-/* {by amy 080312 */
+	 *===========================================================================
+	 * by amy for antenna
+	 *===========================================================================
+	 */
 	/* Config Sw/Hw  Combinational Antenna Diversity. Added by Roger, 2008.02.26.	*/
 	SetAntennaConfig87SE(dev, priv->bDefaultAntenna1, priv->bSwAntennaDiverity);
-/* by amy 080312} */
-/* by amy for antenna */
 }
 
 
-void
-UpdateInitialGain(
-	struct net_device *dev
-	)
+void UpdateInitialGain(struct net_device *dev)
 {
 	struct r8180_priv *priv = (struct r8180_priv *)ieee80211_priv(dev);
 
 	/* lzm add 080826 */
-	if (priv->eRFPowerState != eRfOn)	{
+	if (priv->eRFPowerState != eRfOn) {
 		/*	Don't access BB/RF under disable PLL situation.
-			RT_TRACE(COMP_DIG, DBG_LOUD, ("UpdateInitialGain - pHalData->eRFPowerState!=eRfOn\n"));
-			Back to the original state
-		*/
+		 *	RT_TRACE(COMP_DIG, DBG_LOUD, ("UpdateInitialGain - pHalData->eRFPowerState!=eRfOn\n"));
+		 *	Back to the original state
+		 */
 		priv->InitialGain = priv->InitialGainBackUp;
 		return;
 	}
@@ -826,7 +754,7 @@ UpdateInitialGain(
 		write_phy_ofdm(dev, 0x05, 0xfc);	mdelay(1);
 		break;
 
-	default:	/* MP */
+	default: /* MP */
 		write_phy_ofdm(dev, 0x17, 0x26);	mdelay(1);
 		write_phy_ofdm(dev, 0x24, 0x86);	mdelay(1);
 		write_phy_ofdm(dev, 0x05, 0xfa);	mdelay(1);
@@ -834,14 +762,11 @@ UpdateInitialGain(
 	}
 }
 /*
-	Description:
-		Tx Power tracking mechanism routine on 87SE.
-	Created by Roger, 2007.12.11.
-*/
-void
-InitTxPwrTracking87SE(
-	struct net_device *dev
-)
+ *	Description:
+ *		Tx Power tracking mechanism routine on 87SE.
+ *	Created by Roger, 2007.12.11.
+ */
+void InitTxPwrTracking87SE(struct net_device *dev)
 {
 	u32	u4bRfReg;
 
@@ -851,49 +776,41 @@ InitTxPwrTracking87SE(
 	RF_WriteReg(dev, 0x02, u4bRfReg|PWR_METER_EN);			mdelay(1);
 }
 
-void
-PhyConfig8185(
-	struct net_device *dev
-	)
+void PhyConfig8185(struct net_device *dev)
 {
 	struct r8180_priv *priv = (struct r8180_priv *)ieee80211_priv(dev);
 		write_nic_dword(dev, RCR, priv->ReceiveConfig);
 	   priv->RFProgType = read_nic_byte(dev, CONFIG4) & 0x03;
 	/*  RF config */
 	ZEBRA_Config_85BASIC_HardCode(dev);
-/* {by amy 080312 */
 	/* Set default initial gain state to 4, approved by SD3 DZ, by Bruce, 2007-06-06. */
-	if (priv->bDigMechanism)	{
+	if (priv->bDigMechanism) {
 		if (priv->InitialGain == 0)
 			priv->InitialGain = 4;
 	}
 
 	/*
-		Enable thermal meter indication to implement TxPower tracking on 87SE.
-		We initialize thermal meter here to avoid unsuccessful configuration.
-		Added by Roger, 2007.12.11.
-	*/
+	 *	Enable thermal meter indication to implement TxPower tracking on 87SE.
+	 *	We initialize thermal meter here to avoid unsuccessful configuration.
+	 *	Added by Roger, 2007.12.11.
+	 */
 	if (priv->bTxPowerTrack)
 		InitTxPwrTracking87SE(dev);
 
-/* by amy 080312} */
 	priv->InitialGainBackUp = priv->InitialGain;
 	UpdateInitialGain(dev);
 
 	return;
 }
 
-void
-HwConfigureRTL8185(
-		struct net_device *dev
-		)
+void HwConfigureRTL8185(struct net_device *dev)
 {
 	/* RTL8185_TODO: Determine Retrylimit, TxAGC, AutoRateFallback control. */
-	u8		bUNIVERSAL_CONTROL_RL = 0;
-	u8		bUNIVERSAL_CONTROL_AGC = 1;
-	u8		bUNIVERSAL_CONTROL_ANT = 1;
-	u8		bAUTO_RATE_FALLBACK_CTL = 1;
-	u8		val8;
+	u8 bUNIVERSAL_CONTROL_RL = 0;
+	u8 bUNIVERSAL_CONTROL_AGC = 1;
+	u8 bUNIVERSAL_CONTROL_ANT = 1;
+	u8 bAUTO_RATE_FALLBACK_CTL = 1;
+	u8 val8;
 	write_nic_word(dev, BRSR, 0x0fff);
 	/* Retry limit */
 	val8 = read_nic_byte(dev, CW_CONF);
@@ -907,24 +824,24 @@ HwConfigureRTL8185(
 
 	/* Tx AGC */
 	val8 = read_nic_byte(dev, TXAGC_CTL);
-	if (bUNIVERSAL_CONTROL_AGC)	{
+	if (bUNIVERSAL_CONTROL_AGC) {
 		write_nic_byte(dev, CCK_TXAGC, 128);
 		write_nic_byte(dev, OFDM_TXAGC, 128);
 		val8 = val8 & 0xfe;
-	}	else	{
+	} else {
 		val8 = val8 | 0x01 ;
 	}
 
 
 	write_nic_byte(dev, TXAGC_CTL, val8);
 
-	/* Tx Antenna including Feedback control	*/
+	/* Tx Antenna including Feedback control */
 	val8 = read_nic_byte(dev, TXAGC_CTL);
 
-	if (bUNIVERSAL_CONTROL_ANT)	{
+	if (bUNIVERSAL_CONTROL_ANT) {
 		write_nic_byte(dev, ANTSEL, 0x00);
 		val8 = val8 & 0xfd;
-	}	else	{
+	} else {
 		val8 = val8 & (val8|0x02); /* xiong-2006-11-15 */
 	}
 
@@ -933,7 +850,7 @@ HwConfigureRTL8185(
 	/* Auto Rate fallback control	*/
 	val8 = read_nic_byte(dev, RATE_FALLBACK);
 	val8 &= 0x7c;
-	if (bAUTO_RATE_FALLBACK_CTL)	{
+	if (bAUTO_RATE_FALLBACK_CTL) {
 		val8 |= RATE_FALLBACK_CTL_ENABLE | RATE_FALLBACK_CTL_AUTO_STEP1;
 
 		/* <RJ_TODO_8185B> We shall set up the ARFR according to user's setting. */
@@ -942,40 +859,34 @@ HwConfigureRTL8185(
 	write_nic_byte(dev, RATE_FALLBACK, val8);
 }
 
-static void
-MacConfig_85BASIC_HardCode(
-	struct net_device *dev)
+static void MacConfig_85BASIC_HardCode(struct net_device *dev)
 {
 	/*
-	============================================================================
-	MACREG.TXT
-	============================================================================
-	*/
-	int			nLinesRead = 0;
-
-	u32	u4bRegOffset, u4bRegValue, u4bPageIndex = 0;
-	int	i;
+	 *==========================================================================
+	 * MACREG.TXT
+	 *==========================================================================
+	 */
+	int nLinesRead = 0;
+	u32 u4bRegOffset, u4bRegValue, u4bPageIndex = 0;
+	int i;
 
 	nLinesRead = sizeof(MAC_REG_TABLE)/2;
 
-	for (i = 0; i < nLinesRead; i++)	{	/* nLinesRead=101 */
+	for (i = 0; i < nLinesRead; i++) { /* nLinesRead=101 */
 		u4bRegOffset = MAC_REG_TABLE[i][0];
 		u4bRegValue = MAC_REG_TABLE[i][1];
 
 				if (u4bRegOffset == 0x5e)
 					u4bPageIndex = u4bRegValue;
-
 				else
-						u4bRegOffset |= (u4bPageIndex << 8);
+					u4bRegOffset |= (u4bPageIndex << 8);
 
 		write_nic_byte(dev, u4bRegOffset, (u8)u4bRegValue);
 	}
 	/* ============================================================================ */
 }
 
-static void
-MacConfig_85BASIC(
-	struct net_device *dev)
+static void MacConfig_85BASIC(struct net_device *dev)
 {
 
 	u8			u1DA;
@@ -994,18 +905,18 @@ MacConfig_85BASIC(
 	PlatformIOWrite4Byte(dev, 0x1F4, 0x00000000);
 	PlatformIOWrite1Byte(dev, 0x1F8, 0x00);
 
-	/* Asked for by SD3 CM Lin, 2006.06.27, by rcnjko.	*/
-	/* power save parameter based on "87SE power save parameters 20071127.doc", as follow.	*/
+	/* Asked for by SD3 CM Lin, 2006.06.27, by rcnjko. */
+	/* power save parameter based on "87SE power save parameters 20071127.doc", as follow. */
 
 	/* Enable DA10 TX power saving */
 	u1DA = read_nic_byte(dev, PHYPR);
 	write_nic_byte(dev, PHYPR, (u1DA | BIT2));
 
-	/* POWER:	*/
+	/* POWER: */
 	write_nic_word(dev, 0x360, 0x1000);
 	write_nic_word(dev, 0x362, 0x1000);
 
-	/* AFE.		*/
+	/* AFE. */
 	write_nic_word(dev, 0x370, 0x0560);
 	write_nic_word(dev, 0x372, 0x0560);
 	write_nic_word(dev, 0x374, 0x0DA4);
@@ -1013,54 +924,48 @@ MacConfig_85BASIC(
 	write_nic_word(dev, 0x378, 0x0560);
 	write_nic_word(dev, 0x37A, 0x0560);
 	write_nic_word(dev, 0x37C, 0x00EC);
-	write_nic_word(dev, 0x37E, 0x00EC);	/*+edward	*/
+	write_nic_word(dev, 0x37E, 0x00EC); /* +edward */
 	write_nic_byte(dev, 0x24E, 0x01);
 }
 
-u8
-GetSupportedWirelessMode8185(
-	struct net_device *dev
-)
+u8 GetSupportedWirelessMode8185(struct net_device *dev)
 {
-	u8			btSupportedWirelessMode = 0;
+	u8 btSupportedWirelessMode = 0;
 
 	btSupportedWirelessMode = (WIRELESS_MODE_B | WIRELESS_MODE_G);
 	return btSupportedWirelessMode;
 }
 
-void
-ActUpdateChannelAccessSetting(
-	struct net_device *dev,
-	WIRELESS_MODE			WirelessMode,
-	PCHANNEL_ACCESS_SETTING	ChnlAccessSetting
-	)
+void ActUpdateChannelAccessSetting(struct net_device *dev,
+				   WIRELESS_MODE WirelessMode,
+				   PCHANNEL_ACCESS_SETTING ChnlAccessSetting)
 {
-	struct r8180_priv *priv = ieee80211_priv(dev);
-	struct ieee80211_device *ieee = priv->ieee80211;
+	struct		r8180_priv *priv = ieee80211_priv(dev);
+	struct		ieee80211_device *ieee = priv->ieee80211;
 	AC_CODING	eACI;
 	AC_PARAM	AcParam;
-	u8	bFollowLegacySetting = 0;
-	u8   u1bAIFS;
+	u8		bFollowLegacySetting = 0;
+	u8		u1bAIFS;
 
 	/*
-		<RJ_TODO_8185B>
-		TODO: We still don't know how to set up these registers, just follow WMAC to
-		verify 8185B FPAG.
-
-		<RJ_TODO_8185B>
-		Jong said CWmin/CWmax register are not functional in 8185B,
-		so we shall fill channel access realted register into AC parameter registers,
-		even in nQBss.
-	*/
+	 *	<RJ_TODO_8185B>
+	 *	TODO: We still don't know how to set up these registers, just follow WMAC to
+	 *	verify 8185B FPAG.
+	 *
+	 *	<RJ_TODO_8185B>
+	 *	Jong said CWmin/CWmax register are not functional in 8185B,
+	 *	so we shall fill channel access realted register into AC parameter registers,
+	 *	even in nQBss.
+	 */
 	ChnlAccessSetting->SIFS_Timer = 0x22; /* Suggested by Jong, 2005.12.08. */
-	ChnlAccessSetting->DIFS_Timer = 0x1C; /* 2006.06.02, by rcnjko.			*/
-	ChnlAccessSetting->SlotTimeTimer = 9; /* 2006.06.02, by rcnjko.			*/
-	ChnlAccessSetting->EIFS_Timer = 0x5B; /* Suggested by wcchu, it is the default value of EIFS register, 2005.12.08.	*/
-	ChnlAccessSetting->CWminIndex = 3; /* 2006.06.02, by rcnjko.			*/
-	ChnlAccessSetting->CWmaxIndex = 7; /* 2006.06.02, by rcnjko.			*/
+	ChnlAccessSetting->DIFS_Timer = 0x1C; /* 2006.06.02, by rcnjko. */
+	ChnlAccessSetting->SlotTimeTimer = 9; /* 2006.06.02, by rcnjko. */
+	ChnlAccessSetting->EIFS_Timer = 0x5B; /* Suggested by wcchu, it is the default value of EIFS register, 2005.12.08. */
+	ChnlAccessSetting->CWminIndex = 3; /* 2006.06.02, by rcnjko. */
+	ChnlAccessSetting->CWmaxIndex = 7; /* 2006.06.02, by rcnjko. */
 
 	write_nic_byte(dev, SIFS, ChnlAccessSetting->SIFS_Timer);
-	write_nic_byte(dev, SLOT, ChnlAccessSetting->SlotTimeTimer);	/* Rewrited from directly use PlatformEFIOWrite1Byte(), by Annie, 2006-03-29. */
+	write_nic_byte(dev, SLOT, ChnlAccessSetting->SlotTimeTimer); /* Rewrited from directly use PlatformEFIOWrite1Byte(), by Annie, 2006-03-29. */
 
 	u1bAIFS = aSifsTime + (2 * ChnlAccessSetting->SlotTimeTimer);
 
@@ -1074,17 +979,17 @@ ActUpdateChannelAccessSetting(
 	}
 
 	/* this setting is copied from rtl8187B.  xiong-2006-11-13 */
-	if (bFollowLegacySetting)	{
+	if (bFollowLegacySetting) {
 
 		/*
-			Follow 802.11 seeting to AC parameter, all AC shall use the same parameter.
-			2005.12.01, by rcnjko.
-		*/
+		 *	Follow 802.11 seeting to AC parameter, all AC shall use the same parameter.
+		 *	2005.12.01, by rcnjko.
+		 */
 		AcParam.longData = 0;
 		AcParam.f.AciAifsn.f.AIFSN = 2; /* Follow 802.11 DIFS.	*/
 		AcParam.f.AciAifsn.f.ACM = 0;
-		AcParam.f.Ecw.f.ECWmin = ChnlAccessSetting->CWminIndex; /*	Follow 802.11 CWmin.	*/
-		AcParam.f.Ecw.f.ECWmax = ChnlAccessSetting->CWmaxIndex; /*	Follow 802.11 CWmax.	*/
+		AcParam.f.Ecw.f.ECWmin = ChnlAccessSetting->CWminIndex; /* Follow 802.11 CWmin. */
+		AcParam.f.Ecw.f.ECWmax = ChnlAccessSetting->CWmaxIndex; /* Follow 802.11 CWmax. */
 		AcParam.f.TXOPLimit = 0;
 
 		/* lzm reserved 080826 */
@@ -1095,7 +1000,7 @@ ActUpdateChannelAccessSetting(
 		if (ieee->iw_mode == IW_MODE_ADHOC)
 			AcParam.f.TXOPLimit = 0x0020;
 
-		for (eACI = 0; eACI < AC_MAX; eACI++)	{
+		for (eACI = 0; eACI < AC_MAX; eACI++) {
 			AcParam.f.AciAifsn.f.ACI = (u8)eACI;
 			{
 				PAC_PARAM	pAcParam = (PAC_PARAM)(&AcParam);
@@ -1103,7 +1008,7 @@ ActUpdateChannelAccessSetting(
 				u8		u1bAIFS;
 				u32		u4bAcParam;
 
-				/*  Retrive paramters to udpate. */
+				/*  Retrieve paramters to update. */
 				eACI = pAcParam->f.AciAifsn.f.ACI;
 				u1bAIFS = pAcParam->f.AciAifsn.f.AIFSN * ChnlAccessSetting->SlotTimeTimer + aSifsTime;
 				u4bAcParam = ((((u32)(pAcParam->f.TXOPLimit)) << AC_PARAM_TXOP_LIMIT_OFFSET)	|
@@ -1111,7 +1016,7 @@ ActUpdateChannelAccessSetting(
 						(((u32)(pAcParam->f.Ecw.f.ECWmin)) << AC_PARAM_ECW_MIN_OFFSET)	|
 						(((u32)u1bAIFS) << AC_PARAM_AIFS_OFFSET));
 
-				switch (eACI)	{
+				switch (eACI) {
 				case AC1_BK:
 					/* write_nic_dword(dev, AC_BK_PARAM, u4bAcParam); */
 					break;
@@ -1133,47 +1038,46 @@ ActUpdateChannelAccessSetting(
 					break;
 				}
 
-				/* Cehck ACM bit.	*/
+				/* Cehck ACM bit. */
 				/* If it is set, immediately set ACM control bit to downgrading AC for passing WMM testplan. Annie, 2005-12-13.	*/
 				{
 					PACI_AIFSN	pAciAifsn = (PACI_AIFSN)(&pAcParam->f.AciAifsn);
 					AC_CODING	eACI = pAciAifsn->f.ACI;
 
-					/*modified Joseph				*/
-					/*for 8187B AsynIORead issue	*/
+					/*for 8187B AsynIORead issue */
 					u8	AcmCtrl = 0;
-					if (pAciAifsn->f.ACM)	{
+					if (pAciAifsn->f.ACM) {
 						/* ACM bit is 1. */
-						switch (eACI)	{
+						switch (eACI) {
 						case AC0_BE:
-							AcmCtrl |= (BEQ_ACM_EN|BEQ_ACM_CTL|ACM_HW_EN);	/* or 0x21 */
+							AcmCtrl |= (BEQ_ACM_EN|BEQ_ACM_CTL|ACM_HW_EN); /* or 0x21 */
 							break;
 
 						case AC2_VI:
-							AcmCtrl |= (VIQ_ACM_EN|VIQ_ACM_CTL|ACM_HW_EN);	/* or 0x42	*/
+							AcmCtrl |= (VIQ_ACM_EN|VIQ_ACM_CTL|ACM_HW_EN); /* or 0x42 */
 							break;
 
 						case AC3_VO:
-							AcmCtrl |= (VOQ_ACM_EN|VOQ_ACM_CTL|ACM_HW_EN);	/* or 0x84 */
+							AcmCtrl |= (VOQ_ACM_EN|VOQ_ACM_CTL|ACM_HW_EN); /* or 0x84 */
 							break;
 
 						default:
 							DMESGW("SetHwReg8185(): [HW_VAR_ACM_CTRL] ACM set failed: eACI is %d\n", eACI);
 							break;
 						}
-					}	else	{
+					} else {
 						/* ACM bit is 0. */
-						switch (eACI)	{
+						switch (eACI) {
 						case AC0_BE:
-							AcmCtrl &= ((~BEQ_ACM_EN) & (~BEQ_ACM_CTL) & (~ACM_HW_EN));	/* and 0xDE */
+							AcmCtrl &= ((~BEQ_ACM_EN) & (~BEQ_ACM_CTL) & (~ACM_HW_EN)); /* and 0xDE */
 							break;
 
 						case AC2_VI:
-							AcmCtrl &= ((~VIQ_ACM_EN) & (~VIQ_ACM_CTL) & (~ACM_HW_EN));	/* and 0xBD */
+							AcmCtrl &= ((~VIQ_ACM_EN) & (~VIQ_ACM_CTL) & (~ACM_HW_EN)); /* and 0xBD */
 							break;
 
 						case AC3_VO:
-							AcmCtrl &= ((~VOQ_ACM_EN) & (~VOQ_ACM_CTL) & (~ACM_HW_EN));	/* and 0x7B */
+							AcmCtrl &= ((~VOQ_ACM_EN) & (~VOQ_ACM_CTL) & (~ACM_HW_EN)); /* and 0x7B */
 							break;
 
 						default:
@@ -1187,53 +1091,51 @@ ActUpdateChannelAccessSetting(
 	}
 }
 
-void
-ActSetWirelessMode8185(
-	struct net_device *dev,
-	u8				btWirelessMode
-	)
+void ActSetWirelessMode8185(struct net_device *dev, u8 btWirelessMode)
 {
-	struct r8180_priv *priv = (struct r8180_priv *)ieee80211_priv(dev);
-	struct ieee80211_device *ieee = priv->ieee80211;
+	struct  r8180_priv *priv = (struct r8180_priv *)ieee80211_priv(dev);
+	struct  ieee80211_device *ieee = priv->ieee80211;
 	u8	btSupportedWirelessMode = GetSupportedWirelessMode8185(dev);
 
 	if ((btWirelessMode & btSupportedWirelessMode) == 0)	{
-		/* Don't switch to unsupported wireless mode, 2006.02.15, by rcnjko.	*/
+		/* Don't switch to unsupported wireless mode, 2006.02.15, by rcnjko. */
 		DMESGW("ActSetWirelessMode8185(): WirelessMode(%d) is not supported (%d)!\n",
 			btWirelessMode, btSupportedWirelessMode);
 		return;
 	}
 
-	/* 1. Assign wireless mode to swtich if necessary.	*/
-	if (btWirelessMode == WIRELESS_MODE_AUTO)	{
-		if ((btSupportedWirelessMode & WIRELESS_MODE_A))	{
+	/* 1. Assign wireless mode to switch if necessary. */
+	if (btWirelessMode == WIRELESS_MODE_AUTO) {
+		if ((btSupportedWirelessMode & WIRELESS_MODE_A)) {
 			btWirelessMode = WIRELESS_MODE_A;
-		}	else if (btSupportedWirelessMode & WIRELESS_MODE_G)	{
+		} else if (btSupportedWirelessMode & WIRELESS_MODE_G) {
 				btWirelessMode = WIRELESS_MODE_G;
 
-		}	else if ((btSupportedWirelessMode & WIRELESS_MODE_B))	{
-				btWirelessMode = WIRELESS_MODE_B;
-		}	else	{
-				DMESGW("ActSetWirelessMode8185(): No valid wireless mode supported, btSupportedWirelessMode(%x)!!!\n",
-						btSupportedWirelessMode);
+		} else if ((btSupportedWirelessMode & WIRELESS_MODE_B))	{
 				btWirelessMode = WIRELESS_MODE_B;
+		} else {
+			DMESGW("ActSetWirelessMode8185(): No valid wireless mode supported, btSupportedWirelessMode(%x)!!!\n",
+			       btSupportedWirelessMode);
+			btWirelessMode = WIRELESS_MODE_B;
 		}
 	}
 
-	/* 2. Swtich band: RF or BB specific actions,
+	/* 
+ 	 * 2. Swtich band: RF or BB specific actions,
 	 * for example, refresh tables in omc8255, or change initial gain if necessary.
 	 * Nothing to do for Zebra to switch band.
-	 * Update current wireless mode if we swtich to specified band successfully. */
+	 * Update current wireless mode if we switch to specified band successfully. 
+	 */
 
 	ieee->mode = (WIRELESS_MODE)btWirelessMode;
 
-	/* 3. Change related setting.	*/
-	if( ieee->mode == WIRELESS_MODE_A )	{
+	/* 3. Change related setting. */
+	if( ieee->mode == WIRELESS_MODE_A ) {
 		DMESG("WIRELESS_MODE_A\n");
-	}	else if( ieee->mode == WIRELESS_MODE_B )	{
-			DMESG("WIRELESS_MODE_B\n");
-	}	else if( ieee->mode == WIRELESS_MODE_G )	{
-			DMESG("WIRELESS_MODE_G\n");
+	} else if( ieee->mode == WIRELESS_MODE_B ) {
+		DMESG("WIRELESS_MODE_B\n");
+	} else if( ieee->mode == WIRELESS_MODE_G ) {
+		DMESG("WIRELESS_MODE_G\n");
 	}
 	ActUpdateChannelAccessSetting( dev, ieee->mode, &priv->ChannelAccessSetting);
 }
@@ -1245,22 +1147,16 @@ void rtl8185b_irq_enable(struct net_device *dev)
 	priv->irq_enabled = 1;
 	write_nic_dword(dev, IMR, priv->IntrMask);
 }
-/* by amy for power save */
-void
-DrvIFIndicateDisassociation(
-	struct net_device *dev,
-	u16			reason
-	)
+
+void DrvIFIndicateDisassociation(struct net_device *dev, u16 reason)
 {
 		/* nothing is needed after disassociation request. */
-	}
-void
-MgntDisconnectIBSS(
-	struct net_device *dev
-)
+}
+
+void MgntDisconnectIBSS(struct net_device *dev)
 {
 	struct r8180_priv *priv = (struct r8180_priv *)ieee80211_priv(dev);
-	u8			i;
+	u8 i;
 
 	DrvIFIndicateDisassociation(dev, unspec_reason);
 
@@ -1271,166 +1167,143 @@ MgntDisconnectIBSS(
 
 	priv->ieee80211->state = IEEE80211_NOLINK;
 	/*
-		Stop Beacon.
-
-		Vista add a Adhoc profile, HW radio off until OID_DOT11_RESET_REQUEST
-		Driver would set MSR=NO_LINK, then HW Radio ON, MgntQueue Stuck.
-		Because Bcn DMA isn't complete, mgnt queue would stuck until Bcn packet send.
-
-		Disable Beacon Queue Own bit, suggested by jong	*/
+	 *	Stop Beacon.
+	 *
+	 *	Vista add a Adhoc profile, HW radio off until OID_DOT11_RESET_REQUEST
+	 *	Driver would set MSR=NO_LINK, then HW Radio ON, MgntQueue Stuck.
+	 *	Because Bcn DMA isn't complete, mgnt queue would stuck until Bcn packet send.
+	 *
+	 *	Disable Beacon Queue Own bit, suggested by jong	
+	 */
 	ieee80211_stop_send_beacons(priv->ieee80211);
 
 	priv->ieee80211->link_change(dev);
 	notify_wx_assoc_event(priv->ieee80211);
 }
-void
-MlmeDisassociateRequest(
-	struct net_device *dev,
-	u8 *asSta,
-	u8	asRsn
-	)
+
+void MlmeDisassociateRequest(struct net_device *dev, u8 *asSta, u8 asRsn)
 {
 	struct r8180_priv *priv = (struct r8180_priv *)ieee80211_priv(dev);
 	u8 i;
 
 	SendDisassociation(priv->ieee80211, asSta, asRsn);
 
-	if (memcmp(priv->ieee80211->current_network.bssid, asSta, 6) == 0)	{
-		/*ShuChen TODO: change media status.			*/
-		/*ShuChen TODO: What to do when disassociate.	*/
+	if (memcmp(priv->ieee80211->current_network.bssid, asSta, 6) == 0) {
+		/* ShuChen TODO: change media status. */
+		/* ShuChen TODO: What to do when disassociate. */
 		DrvIFIndicateDisassociation(dev, unspec_reason);
 
-
-
 		for (i = 0; i < 6; i++)
 			priv->ieee80211->current_network.bssid[i] = 0x22;
 
 		ieee80211_disassociate(priv->ieee80211);
 	}
-
 }
 
-void
-MgntDisconnectAP(
-	struct net_device *dev,
-	u8			asRsn
-)
+void MgntDisconnectAP(struct net_device *dev, u8 asRsn)
 {
 	struct r8180_priv *priv = (struct r8180_priv *)ieee80211_priv(dev);
 
 	/*
-	Commented out by rcnjko, 2005.01.27:
-	I move SecClearAllKeys() to MgntActSet_802_11_DISASSOCIATE().
-
-		2004/09/15, kcwu, the key should be cleared, or the new handshaking will not success
-
-		In WPA WPA2 need to Clear all key ... because new key will set after new handshaking.
-		2004.10.11, by rcnjko. */
+	 * Commented out by rcnjko, 2005.01.27:
+	 * I move SecClearAllKeys() to MgntActSet_802_11_DISASSOCIATE().
+	 *
+	 *	2004/09/15, kcwu, the key should be cleared, or the new handshaking will not success
+	 *
+	 *	In WPA WPA2 need to Clear all key ... because new key will set after new handshaking.
+	 *	2004.10.11, by rcnjko. 
+	 */
 	MlmeDisassociateRequest(dev, priv->ieee80211->current_network.bssid, asRsn);
 
 	priv->ieee80211->state = IEEE80211_NOLINK;
 }
-bool
-MgntDisconnect(
-	struct net_device *dev,
-	u8			asRsn
-)
+
+bool MgntDisconnect(struct net_device *dev, u8 asRsn)
 {
 	struct r8180_priv *priv = (struct r8180_priv *)ieee80211_priv(dev);
 	/*
-		Schedule an workitem to wake up for ps mode, 070109, by rcnjko.
-	*/
+	 *	Schedule an workitem to wake up for ps mode, 070109, by rcnjko.
+	 */
 
 	if (IS_DOT11D_ENABLE(priv->ieee80211))
 		Dot11d_Reset(priv->ieee80211);
-	/* In adhoc mode, update beacon frame.	*/
+	/* In adhoc mode, update beacon frame. */
 	if (priv->ieee80211->state == IEEE80211_LINKED)	{
 		if (priv->ieee80211->iw_mode == IW_MODE_ADHOC)
 			MgntDisconnectIBSS(dev);
 
-		if (priv->ieee80211->iw_mode == IW_MODE_INFRA)	{
-			/*	We clear key here instead of MgntDisconnectAP() because that
-				MgntActSet_802_11_DISASSOCIATE() is an interface called by OS,
-				e.g. OID_802_11_DISASSOCIATE in Windows while as MgntDisconnectAP() is
-				used to handle disassociation related things to AP, e.g. send Disassoc
-				frame to AP.  2005.01.27, by rcnjko.	*/
+		if (priv->ieee80211->iw_mode == IW_MODE_INFRA) {
+			/*
+ 			 * 	We clear key here instead of MgntDisconnectAP() because that
+			 *	MgntActSet_802_11_DISASSOCIATE() is an interface called by OS,
+			 *	e.g. OID_802_11_DISASSOCIATE in Windows while as MgntDisconnectAP() is
+			 *	used to handle disassociation related things to AP, e.g. send Disassoc
+			 *	frame to AP.  2005.01.27, by rcnjko. 
+			 */
 			MgntDisconnectAP(dev, asRsn);
 		}
-		/* Inidicate Disconnect, 2005.02.23, by rcnjko.	*/
+		/* Indicate Disconnect, 2005.02.23, by rcnjko.	*/
 	}
 	return true;
 }
 /*
-	Description:
-		Chang RF Power State.
-		Note that, only MgntActSet_RF_State() is allowed to set HW_VAR_RF_STATE.
-
-	Assumption:
-		PASSIVE LEVEL.
-*/
-bool
-SetRFPowerState(
-	struct net_device *dev,
-	RT_RF_POWER_STATE	eRFPowerState
-	)
+ *	Description:
+ *		Chang RF Power State.
+ *		Note that, only MgntActSet_RF_State() is allowed to set HW_VAR_RF_STATE.
+ *
+ *	Assumption:
+ *		PASSIVE LEVEL.
+ */
+bool SetRFPowerState(struct net_device *dev, RT_RF_POWER_STATE eRFPowerState)
 {
-	struct r8180_priv *priv = (struct r8180_priv *)ieee80211_priv(dev);
-	bool			bResult = false;
+	struct	r8180_priv *priv = (struct r8180_priv *)ieee80211_priv(dev);
+	bool	bResult = false;
 
 	if (eRFPowerState == priv->eRFPowerState)
 		return bResult;
 
-	 bResult = SetZebraRFPowerState8185(dev, eRFPowerState);
+	bResult = SetZebraRFPowerState8185(dev, eRFPowerState);
 
 	return bResult;
 }
-void
-HalEnableRx8185Dummy(
-	struct net_device *dev
-	)
+
+void HalEnableRx8185Dummy(struct net_device *dev)
 {
 }
-void
-HalDisableRx8185Dummy(
-	struct net_device *dev
-	)
+
+void HalDisableRx8185Dummy(struct net_device *dev)
 {
 }
 
-bool
-MgntActSet_RF_State(
-	struct net_device *dev,
-	RT_RF_POWER_STATE	StateToSet,
-	u32	ChangeSource
-	)
+bool MgntActSet_RF_State(struct net_device *dev, RT_RF_POWER_STATE StateToSet, u32 ChangeSource)
 {
-	struct r8180_priv *priv = (struct r8180_priv *)ieee80211_priv(dev);
-	bool				bActionAllowed = false;
-	bool				bConnectBySSID = false;
-	RT_RF_POWER_STATE	rtState;
-	u16				RFWaitCounter = 0;
+	struct	r8180_priv *priv = (struct r8180_priv *)ieee80211_priv(dev);
+	bool	bActionAllowed = false;
+	bool	bConnectBySSID = false;
+	RT_RF_POWER_STATE rtState;
+	u16	RFWaitCounter = 0;
 	unsigned long flag;
 	/*
-		Prevent the race condition of RF state change. By Bruce, 2007-11-28.
-		Only one thread can change the RF state at one time, and others should wait to be executed.
-	*/
-	while (true)	{
+	 *	Prevent the race condition of RF state change. By Bruce, 2007-11-28.
+	 *	Only one thread can change the RF state at one time, and others should wait to be executed.
+	 */
+	while (true) {
 		spin_lock_irqsave(&priv->rf_ps_lock, flag);
-		if (priv->RFChangeInProgress)	{
+		if (priv->RFChangeInProgress) {
 			spin_unlock_irqrestore(&priv->rf_ps_lock, flag);
 			/*  Set RF after the previous action is done.	*/
-			while (priv->RFChangeInProgress)	{
+			while (priv->RFChangeInProgress) {
 				RFWaitCounter++;
 				udelay(1000); /* 1 ms	*/
 
-				/* Wait too long, return FALSE to avoid to be stuck here.	*/
-				if (RFWaitCounter > 1000)	{	/* 1sec	*/
+				/* Wait too long, return FALSE to avoid to be stuck here. */
+				if (RFWaitCounter > 1000) { /* 1sec */
 					printk("MgntActSet_RF_State(): Wait too long to set RF\n");
-					/* TODO: Reset RF state?	*/
+					/* TODO: Reset RF state? */
 					return false;
 				}
 			}
-		}	else	{
+		} else {
 			priv->RFChangeInProgress = true;
 			spin_unlock_irqrestore(&priv->rf_ps_lock, flag);
 			break;
@@ -1438,12 +1311,12 @@ MgntActSet_RF_State(
 	}
 	rtState = priv->eRFPowerState;
 
-	switch (StateToSet)	{
+	switch (StateToSet) {
 	case eRfOn:
 		/*
-			Turn On RF no matter the IPS setting because we need to update the RF state to Ndis under Vista, or
-			the Windows does not allow the driver to perform site survey any more. By Bruce, 2007-10-02.
-		*/
+		 *	Turn On RF no matter the IPS setting because we need to update the RF state to Ndis under Vista, or
+		 *	the Windows does not allow the driver to perform site survey any more. By Bruce, 2007-10-02.
+		 */
 		priv->RfOffReason &= (~ChangeSource);
 
 		if (!priv->RfOffReason)	{
@@ -1453,25 +1326,24 @@ MgntActSet_RF_State(
 			if (rtState == eRfOff && ChangeSource >= RF_CHANGE_BY_HW && !priv->bInHctTest)
 				bConnectBySSID = true;
 
-		}	else
-				;
+		} else
+			;
 		break;
 
 	case eRfOff:
-		 /* 070125, rcnjko: we always keep connected in AP mode.	*/
-
-			if (priv->RfOffReason > RF_CHANGE_BY_IPS)	{
-				/*
-					060808, Annie:
-					Disconnect to current BSS when radio off. Asked by QuanTa.
-
-					Calling MgntDisconnect() instead of MgntActSet_802_11_DISASSOCIATE(),
-					because we do NOT need to set ssid to dummy ones.
-				*/
-				MgntDisconnect(dev, disas_lv_ss);
-
-				/* Clear content of bssDesc[] and bssDesc4Query[] to avoid reporting old bss to UI.	*/
-			}
+		 /* 070125, rcnjko: we always keep connected in AP mode. */
+
+		if (priv->RfOffReason > RF_CHANGE_BY_IPS) {
+			/*
+			 *	060808, Annie:
+			 *	Disconnect to current BSS when radio off. Asked by QuanTa.
+			 *
+			 *	Calling MgntDisconnect() instead of MgntActSet_802_11_DISASSOCIATE(),
+			 *	because we do NOT need to set ssid to dummy ones.
+			 */
+			MgntDisconnect(dev, disas_lv_ss);
+			/* Clear content of bssDesc[] and bssDesc4Query[] to avoid reporting old bss to UI. */
+		}
 
 		priv->RfOffReason |= ChangeSource;
 		bActionAllowed = true;
@@ -1484,14 +1356,14 @@ MgntActSet_RF_State(
 		break;
 	}
 
-	if (bActionAllowed)	{
-				/* Config HW to the specified mode.	*/
+	if (bActionAllowed) {
+		/* Config HW to the specified mode. */
 		SetRFPowerState(dev, StateToSet);
 
 		/* Turn on RF.	*/
-		if (StateToSet == eRfOn)		{
+		if (StateToSet == eRfOn) {
 			HalEnableRx8185Dummy(dev);
-			if (bConnectBySSID)	{
+			if (bConnectBySSID) {
 				/* by amy not supported	*/
 			}
 		}
@@ -1507,69 +1379,61 @@ MgntActSet_RF_State(
 	spin_unlock_irqrestore(&priv->rf_ps_lock, flag);
 	return bActionAllowed;
 }
-void
-InactivePowerSave(
-	struct net_device *dev
-	)
+
+void InactivePowerSave(struct net_device *dev)
 {
 	struct r8180_priv *priv = (struct r8180_priv *)ieee80211_priv(dev);
 	/*
-		This flag "bSwRfProcessing", indicates the status of IPS procedure, should be set if the IPS workitem
-		is really scheduled.
-		The old code, sets this flag before scheduling the IPS workitem and however, at the same time the
-		previous IPS workitem did not end yet, fails to schedule the current workitem. Thus, bSwRfProcessing
-		blocks the IPS procedure of switching RF.
-	*/
+	 *	This flag "bSwRfProcessing", indicates the status of IPS procedure, should be set if the IPS workitem
+	 *	is really scheduled.
+	 *	The old code, sets this flag before scheduling the IPS workitem and however, at the same time the
+	 *	previous IPS workitem did not end yet, fails to schedule the current workitem. Thus, bSwRfProcessing
+	 *	blocks the IPS procedure of switching RF.
+	 */
 	priv->bSwRfProcessing = true;
 
 	MgntActSet_RF_State(dev, priv->eInactivePowerState, RF_CHANGE_BY_IPS);
 
 	/*
-		To solve CAM values miss in RF OFF, rewrite CAM values after RF ON. By Bruce, 2007-09-20.
-	*/
+	 *	To solve CAM values miss in RF OFF, rewrite CAM values after RF ON. By Bruce, 2007-09-20.
+	 */
 
 	priv->bSwRfProcessing = false;
 }
 
 /*
-	Description:
-		Enter the inactive power save mode. RF will be off
-*/
-void
-IPSEnter(
-	struct net_device *dev
-	)
+ *	Description:
+ *		Enter the inactive power save mode. RF will be off
+ */
+void IPSEnter(struct net_device *dev)
 {
 	struct r8180_priv *priv = (struct r8180_priv *)ieee80211_priv(dev);
 	RT_RF_POWER_STATE rtState;
-	if (priv->bInactivePs)	{
+	if (priv->bInactivePs) {
 		rtState = priv->eRFPowerState;
 
 		/*
-			Do not enter IPS in the following conditions:
-			(1) RF is already OFF or Sleep
-			(2) bSwRfProcessing (indicates the IPS is still under going)
-			(3) Connectted (only disconnected can trigger IPS)
-			(4) IBSS (send Beacon)
-			(5) AP mode (send Beacon)
-		*/
+		 *	Do not enter IPS in the following conditions:
+		 *	(1) RF is already OFF or Sleep
+		 *	(2) bSwRfProcessing (indicates the IPS is still under going)
+		 *	(3) Connected (only disconnected can trigger IPS)
+		 *	(4) IBSS (send Beacon)
+		 *	(5) AP mode (send Beacon)
+		 */
 		if (rtState == eRfOn && !priv->bSwRfProcessing
-			&& (priv->ieee80211->state != IEEE80211_LINKED))	{
+			&& (priv->ieee80211->state != IEEE80211_LINKED)) {
 			priv->eInactivePowerState = eRfOff;
 			InactivePowerSave(dev);
 		}
 	}
 }
-void
-IPSLeave(
-	struct net_device *dev
-	)
+void IPSLeave(struct net_device *dev)
 {
 	struct r8180_priv *priv = (struct r8180_priv *)ieee80211_priv(dev);
 	RT_RF_POWER_STATE rtState;
-	if (priv->bInactivePs)	{
+	if (priv->bInactivePs) {
 		rtState = priv->eRFPowerState;
-		if ((rtState == eRfOff || rtState == eRfSleep) && (!priv->bSwRfProcessing) && priv->RfOffReason <= RF_CHANGE_BY_IPS)	{
+		if ((rtState == eRfOff || rtState == eRfSleep) && (!priv->bSwRfProcessing) && priv->RfOffReason <= RF_CHANGE_BY_IPS) {
 			priv->eInactivePowerState = eRfOn;
 			InactivePowerSave(dev);
 		}
@@ -1582,8 +1446,8 @@ void rtl8185b_adapter_start(struct net_device *dev)
 	struct ieee80211_device *ieee = priv->ieee80211;
 
 	u8 SupportedWirelessMode;
-	u8			InitWirelessMode;
-	u8			bInvalidWirelessMode = 0;
+	u8 InitWirelessMode;
+	u8 bInvalidWirelessMode = 0;
 	u8 tmpu8;
 	u8 btCR9346;
 	u8 TmpU1b;
@@ -1598,89 +1462,89 @@ void rtl8185b_adapter_start(struct net_device *dev)
 	HwConfigureRTL8185(dev);
 	write_nic_dword(dev, MAC0, ((u32 *)dev->dev_addr)[0]);
 	write_nic_word(dev, MAC4, ((u32 *)dev->dev_addr)[1] & 0xffff);
-	write_nic_byte(dev, MSR, read_nic_byte(dev, MSR) & 0xf3);	/* default network type to 'No	Link'	*/
+	write_nic_byte(dev, MSR, read_nic_byte(dev, MSR) & 0xf3); /* default network type to 'No Link' */
 	write_nic_word(dev, BcnItv, 100);
 	write_nic_word(dev, AtimWnd, 2);
 	PlatformIOWrite2Byte(dev, FEMR, 0xFFFF);
 	write_nic_byte(dev, WPA_CONFIG, 0);
 	MacConfig_85BASIC(dev);
-	/*	Override the RFSW_CTRL (MAC offset 0x272-0x273), 2006.06.07, by rcnjko.	*/
-	/*	BT_DEMO_BOARD type	*/
+	/* Override the RFSW_CTRL (MAC offset 0x272-0x273), 2006.06.07, by rcnjko. */
+	/* BT_DEMO_BOARD type */
 	PlatformIOWrite2Byte(dev, RFSW_CTRL, 0x569a);
 
 	/*
-	-----------------------------------------------------------------------------
-		Set up PHY related.
-	-----------------------------------------------------------------------------
-	*/
-	/* Enable Config3.PARAM_En to revise AnaaParm.	*/
-	write_nic_byte(dev, CR9346, 0xc0);	/* enable config register write */
+	 *---------------------------------------------------------------------------
+	 *	Set up PHY related.
+	 *---------------------------------------------------------------------------
+	 */
+	/* Enable Config3.PARAM_En to revise AnaaParm. */
+	write_nic_byte(dev, CR9346, 0xc0); /* enable config register write */
 	tmpu8 = read_nic_byte(dev, CONFIG3);
 	write_nic_byte(dev, CONFIG3, (tmpu8 | CONFIG3_PARM_En));
-	/* Turn on Analog power.	*/
-	/* Asked for by William, otherwise, MAC 3-wire can't work, 2006.06.27, by rcnjko.	*/
+	/* Turn on Analog power. */
+	/* Asked for by William, otherwise, MAC 3-wire can't work, 2006.06.27, by rcnjko. */
 	write_nic_dword(dev, ANAPARAM2, ANAPARM2_ASIC_ON);
 	write_nic_dword(dev, ANAPARAM, ANAPARM_ASIC_ON);
 	write_nic_word(dev, ANAPARAM3, 0x0010);
 
 	write_nic_byte(dev, CONFIG3, tmpu8);
 	write_nic_byte(dev, CR9346, 0x00);
-	/* enable EEM0 and EEM1 in 9346CR			*/
+	/* enable EEM0 and EEM1 in 9346CR */
 	btCR9346 = read_nic_byte(dev, CR9346);
 	write_nic_byte(dev, CR9346, (btCR9346 | 0xC0));
 
-	/* B cut use LED1 to control HW RF on/off	*/
+	/* B cut use LED1 to control HW RF on/off */
 	TmpU1b = read_nic_byte(dev, CONFIG5);
 	TmpU1b = TmpU1b & ~BIT3;
 	write_nic_byte(dev, CONFIG5, TmpU1b);
 
-	/* disable EEM0 and EEM1 in 9346CR	*/
+	/* disable EEM0 and EEM1 in 9346CR */
 	btCR9346 &= ~(0xC0);
 	write_nic_byte(dev, CR9346, btCR9346);
 
-	/* Enable Led (suggested by Jong)	*/
-	/* B-cut RF Radio on/off  5e[3]=0	*/
+	/* Enable Led (suggested by Jong) */
+	/* B-cut RF Radio on/off  5e[3]=0 */
 	btPSR = read_nic_byte(dev, PSR);
 	write_nic_byte(dev, PSR, (btPSR | BIT3));
-	/* setup initial timing for RFE.	*/
+	/* setup initial timing for RFE. */
 	write_nic_word(dev, RFPinsOutput, 0x0480);
 	SetOutputEnableOfRfPins(dev);
 	write_nic_word(dev, RFPinsSelect, 0x2488);
 
-	/* PHY config.	*/
+	/* PHY config. */
 	PhyConfig8185(dev);
 
 	/*
-		We assume RegWirelessMode has already been initialized before,
-		however, we has to validate the wireless mode here and provide a
-		reasonable initialized value if necessary. 2005.01.13, by rcnjko.
-	*/
+	 *	We assume RegWirelessMode has already been initialized before,
+	 *	however, we has to validate the wireless mode here and provide a
+	 *	reasonable initialized value if necessary. 2005.01.13, by rcnjko.
+	 */
 	SupportedWirelessMode = GetSupportedWirelessMode8185(dev);
 	if ((ieee->mode != WIRELESS_MODE_B) &&
 		(ieee->mode != WIRELESS_MODE_G) &&
 		(ieee->mode != WIRELESS_MODE_A) &&
-		(ieee->mode != WIRELESS_MODE_AUTO))	{
-		/* It should be one of B, G, A, or AUTO.	*/
+		(ieee->mode != WIRELESS_MODE_AUTO)) {
+		/* It should be one of B, G, A, or AUTO. */
 		bInvalidWirelessMode = 1;
-	}	else	{
-	/* One of B, G, A, or AUTO.	*/
-		/* Check if the wireless mode is supported by RF.	*/
+	} else {
+	/* One of B, G, A, or AUTO. */
+		/* Check if the wireless mode is supported by RF. */
 		if	((ieee->mode != WIRELESS_MODE_AUTO) &&
-			(ieee->mode & SupportedWirelessMode) == 0)	{
+			(ieee->mode & SupportedWirelessMode) == 0) {
 			bInvalidWirelessMode = 1;
 		}
 	}
 
-	if (bInvalidWirelessMode || ieee->mode == WIRELESS_MODE_AUTO)	{
-		/* Auto or other invalid value.				*/
-		/* Assigne a wireless mode to initialize.	*/
-		if ((SupportedWirelessMode & WIRELESS_MODE_A))	{
+	if (bInvalidWirelessMode || ieee->mode == WIRELESS_MODE_AUTO) {
+		/* Auto or other invalid value. */
+		/* Assigne a wireless mode to initialize. */
+		if ((SupportedWirelessMode & WIRELESS_MODE_A)) {
 			InitWirelessMode = WIRELESS_MODE_A;
-		}	else if ((SupportedWirelessMode & WIRELESS_MODE_G))	{
+		} else if ((SupportedWirelessMode & WIRELESS_MODE_G)) {
 			InitWirelessMode = WIRELESS_MODE_G;
-		}	else if ((SupportedWirelessMode & WIRELESS_MODE_B))	{
+		} else if ((SupportedWirelessMode & WIRELESS_MODE_B)) {
 			InitWirelessMode = WIRELESS_MODE_B;
-		}	else	{
+		} else {
 			DMESGW("InitializeAdapter8185(): No valid wireless mode supported, SupportedWirelessMode(%x)!!!\n",
 				 SupportedWirelessMode);
 			InitWirelessMode = WIRELESS_MODE_B;
@@ -1690,24 +1554,21 @@ void rtl8185b_adapter_start(struct net_device *dev)
 		if (bInvalidWirelessMode)
 			ieee->mode = (WIRELESS_MODE)InitWirelessMode;
 
-	}	else	{
-	/* One of B, G, A.	*/
+	} else {
+	/* One of B, G, A. */
 		InitWirelessMode = ieee->mode;
 	}
-/* by amy for power save	*/
 	priv->eRFPowerState = eRfOff;
 	priv->RfOffReason = 0;
 	{
 		MgntActSet_RF_State(dev, eRfOn, 0);
 	}
 		/*
-			If inactive power mode is enabled, disable rf while in disconnected state.
-		*/
+		 *	If inactive power mode is enabled, disable rf while in disconnected state.
+		 */
 	if (priv->bInactivePs)
 		MgntActSet_RF_State(dev , eRfOff, RF_CHANGE_BY_IPS);
 
-/* by amy for power save	*/
-
 	ActSetWirelessMode8185(dev, (u8)(InitWirelessMode));
 
 	/* ----------------------------------------------------------------------------- */
@@ -1715,7 +1576,7 @@ void rtl8185b_adapter_start(struct net_device *dev)
 	rtl8185b_irq_enable(dev);
 
 	netif_start_queue(dev);
-	}
+}
 
 void rtl8185b_rx_enable(struct net_device *dev)
 {
@@ -1728,7 +1589,7 @@ void rtl8185b_rx_enable(struct net_device *dev)
 		DMESG("NIC in promisc mode");
 
 	if (priv->ieee80211->iw_mode == IW_MODE_MONITOR || \
-	   dev->flags & IFF_PROMISC)	{
+	   dev->flags & IFF_PROMISC) {
 		priv->ReceiveConfig = priv->ReceiveConfig & (~RCR_APM);
 		priv->ReceiveConfig = priv->ReceiveConfig | RCR_AAP;
 	}
diff --git a/drivers/staging/rtl8192e/Kconfig b/drivers/staging/rtl8192e/Kconfig
index f87e2110185..4602a47cdb4 100644
--- a/drivers/staging/rtl8192e/Kconfig
+++ b/drivers/staging/rtl8192e/Kconfig
@@ -14,6 +14,7 @@ if RTLLIB
 config RTLLIB_CRYPTO_CCMP
 	tristate "Support for rtllib CCMP crypto"
 	depends on RTLLIB
+	select CRYPTO_AES
 	default y
 	---help---
 	  CCMP crypto driver for rtllib.
@@ -23,6 +24,8 @@ config RTLLIB_CRYPTO_CCMP
 config RTLLIB_CRYPTO_TKIP
 	tristate "Support for rtllib TKIP crypto"
 	depends on RTLLIB
+	select CRYPTO_ARC4
+	select CRYPTO_MICHAEL_MIC
 	default y
 	---help---
 	  TKIP crypto driver for rtllib.
@@ -31,6 +34,7 @@ config RTLLIB_CRYPTO_TKIP
 
 config RTLLIB_CRYPTO_WEP
 	tristate "Support for rtllib WEP crypto"
+	select CRYPTO_ARC4
 	depends on RTLLIB
 	default y
 	---help---
diff --git a/drivers/staging/rtl8192e/rtl8192e/r8192E_cmdpkt.c b/drivers/staging/rtl8192e/rtl8192e/r8192E_cmdpkt.c
index 58d044ea552..ea91744f7cc 100644
--- a/drivers/staging/rtl8192e/rtl8192e/r8192E_cmdpkt.c
+++ b/drivers/staging/rtl8192e/rtl8192e/r8192E_cmdpkt.c
@@ -342,7 +342,6 @@ static	void cmpk_handle_tx_rate_history(struct net_device *dev, u8 *pmsg)
 u32 cmpk_message_handle_rx(struct net_device *dev,
 			   struct rtllib_rx_stats *pstats)
 {
-	struct r8192_priv *priv = rtllib_priv(dev);
 	int			total_length;
 	u8			cmd_length, exe_cnt = 0;
 	u8			element_id;
@@ -409,8 +408,6 @@ u32 cmpk_message_handle_rx(struct net_device *dev,
 			return 1;
 		}
 
-		priv->stats.rxcmdpkt[element_id]++;
-
 		total_length -= cmd_length;
 		pcmd_buff    += cmd_length;
 	}
diff --git a/drivers/staging/rtl8192e/rtl8192e/r8192E_firmware.c b/drivers/staging/rtl8192e/rtl8192e/r8192E_firmware.c
index 37719859bda..b526fa42867 100644
--- a/drivers/staging/rtl8192e/rtl8192e/r8192E_firmware.c
+++ b/drivers/staging/rtl8192e/rtl8192e/r8192E_firmware.c
@@ -216,7 +216,7 @@ static bool firmware_check_ready(struct net_device *dev,
 		break;
 	default:
 		rt_status = false;
-		RT_TRACE(COMP_FIRMWARE, "Unknown firware status");
+		RT_TRACE(COMP_FIRMWARE, "Unknown firmware status");
 		break;
 	}
 
diff --git a/drivers/staging/rtl8192e/rtl8192e/r8192E_phy.c b/drivers/staging/rtl8192e/rtl8192e/r8192E_phy.c
index 3e705efaaf2..9676c591c85 100644
--- a/drivers/staging/rtl8192e/rtl8192e/r8192E_phy.c
+++ b/drivers/staging/rtl8192e/rtl8192e/r8192E_phy.c
@@ -689,7 +689,7 @@ void rtl8192_phy_setTxPower(struct net_device *dev, u8 channel)
 	case RF_8258:
 		break;
 	default:
-		RT_TRACE(COMP_ERR, "unknown rf chip in funtion %s()\n",
+		RT_TRACE(COMP_ERR, "unknown rf chip in function %s()\n",
 			 __func__);
 		break;
 	}
diff --git a/drivers/staging/rtl8192e/rtl8192e/r819xE_phyreg.h b/drivers/staging/rtl8192e/rtl8192e/r819xE_phyreg.h
index d5de279f664..970298b07af 100644
--- a/drivers/staging/rtl8192e/rtl8192e/r819xE_phyreg.h
+++ b/drivers/staging/rtl8192e/rtl8192e/r819xE_phyreg.h
@@ -306,7 +306,7 @@
 #define bRFStart                  	0x0000f000
 #define bBBStart                  	0x000000f0
 #define bBBCCKStart               	0x0000000f
-/* Reg)x814 */
+/* Reg x814 */
 #define bPAEnd                    	0xf
 #define bTREnd                    	0x0f000000
 #define bRFEnd                    	0x000f0000
@@ -844,7 +844,7 @@
 #define bRTL8258_RxLPFBW          0xc00
 #define bRTL8258_RSSILPFBW        0xc0
 
-/* byte endable for sb_write */
+/* byte enable for sb_write */
 #define bByte0                    0x1
 #define bByte1                    0x2
 #define bByte2                    0x4
diff --git a/drivers/staging/rtl8192e/rtl8192e/rtl_core.c b/drivers/staging/rtl8192e/rtl8192e/rtl_core.c
index 71adb6b3344..4f602b227b5 100644
--- a/drivers/staging/rtl8192e/rtl8192e/rtl_core.c
+++ b/drivers/staging/rtl8192e/rtl8192e/rtl_core.c
@@ -1025,7 +1025,7 @@ static int rtl8192_sta_down(struct net_device *dev, bool shutdownrf)
 			break;
 		}
 		RT_TRACE(COMP_DBG, "===>%s():RF is in progress, need to wait "
-			 "until rf chang is done.\n", __func__);
+			 "until rf change is done.\n", __func__);
 		mdelay(1);
 		RFInProgressTimeOut++;
 		spin_lock_irqsave(&priv->rf_ps_lock, flags);
@@ -1211,7 +1211,7 @@ static void rtl8192_init_priv_variable(struct net_device *dev)
 	priv->AcmControl = 0;
 	priv->pFirmware = vzalloc(sizeof(struct rt_firmware));
 	if (!priv->pFirmware)
-		printk(KERN_ERR "rtl8193e: Unable to allocate space "
+		printk(KERN_ERR "rtl8192e: Unable to allocate space "
 		       "for firmware\n");
 
 	skb_queue_head_init(&priv->rx_queue);
@@ -2024,10 +2024,10 @@ short rtl8192_tx(struct net_device *dev, struct sk_buff *skb)
 	stype = WLAN_FC_GET_STYPE(fc);
 	pda_addr = header->addr1;
 
-	if (is_multicast_ether_addr(pda_addr))
-		multi_addr = true;
-	else if (is_broadcast_ether_addr(pda_addr))
+	if (is_broadcast_ether_addr(pda_addr))
 		broad_addr = true;
+	else if (is_multicast_ether_addr(pda_addr))
+		multi_addr = true;
 	else
 		uni_addr = true;
 
@@ -2358,8 +2358,7 @@ static void rtl8192_rx_normal(struct net_device *dev)
 				stats.RxBufShift);
 			skb_trim(skb, skb->len - 4/*sCrcLng*/);
 			rtllib_hdr = (struct rtllib_hdr_1addr *)skb->data;
-			if (!is_broadcast_ether_addr(rtllib_hdr->addr1) &&
-			!is_multicast_ether_addr(rtllib_hdr->addr1)) {
+			if (!is_multicast_ether_addr(rtllib_hdr->addr1)) {
 				/* unicast packet */
 				unicast_packet = true;
 			}
diff --git a/drivers/staging/rtl8192e/rtl8192e/rtl_core.h b/drivers/staging/rtl8192e/rtl8192e/rtl_core.h
index 2a2519cc284..320d5fc026b 100644
--- a/drivers/staging/rtl8192e/rtl8192e/rtl_core.h
+++ b/drivers/staging/rtl8192e/rtl8192e/rtl_core.h
@@ -353,7 +353,6 @@ struct rt_stats {
 	unsigned long rxrdu;
 	unsigned long rxok;
 	unsigned long rxframgment;
-	unsigned long rxcmdpkt[8];
 	unsigned long rxurberr;
 	unsigned long rxstaterr;
 	unsigned long rxdatacrcerr;
@@ -944,7 +943,7 @@ struct r8192_priv {
 	bool		bfsync_processing;
 	u32		rate_record;
 	u32		rateCountDiffRecord;
-	u32		ContiuneDiffCount;
+	u32		ContinueDiffCount;
 	bool		bswitch_fsync;
 	u8		framesync;
 	u32		framesyncC34;
diff --git a/drivers/staging/rtl8192e/rtl8192e/rtl_dm.c b/drivers/staging/rtl8192e/rtl8192e/rtl_dm.c
index f026b7171f6..481b1e4d491 100644
--- a/drivers/staging/rtl8192e/rtl8192e/rtl_dm.c
+++ b/drivers/staging/rtl8192e/rtl8192e/rtl_dm.c
@@ -493,7 +493,7 @@ static void dm_TXPowerTrackingCallback_TSSI(struct net_device *dev)
 
 				if (priv->bResetInProgress) {
 					RT_TRACE(COMP_POWER_TRACKING,
-						 "we are in slient reset progress, so return\n");
+						 "we are in silent reset progress, so return\n");
 					write_nic_byte(dev, Pw_Track_Flag, 0);
 					write_nic_byte(dev, FW_Busy_Flag, 0);
 					return;
@@ -2615,22 +2615,22 @@ void dm_fsync_timer_callback(unsigned long data)
 				      rate_count_diff;
 			if (DiffNum >=
 			    priv->rtllib->fsync_seconddiff_ratethreshold)
-				priv->ContiuneDiffCount++;
+				priv->ContinueDiffCount++;
 			else
-				priv->ContiuneDiffCount = 0;
+				priv->ContinueDiffCount = 0;
 
-			if (priv->ContiuneDiffCount >= 2) {
+			if (priv->ContinueDiffCount >= 2) {
 				bSwitchFromCountDiff = true;
-				priv->ContiuneDiffCount = 0;
+				priv->ContinueDiffCount = 0;
 			}
 		} else {
-			priv->ContiuneDiffCount = 0;
+			priv->ContinueDiffCount = 0;
 		}
 
 		if (rate_count_diff <=
 		    priv->rtllib->fsync_firstdiff_ratethreshold) {
 			bSwitchFromCountDiff = true;
-			priv->ContiuneDiffCount = 0;
+			priv->ContinueDiffCount = 0;
 		}
 		priv->rate_record = rate_count;
 		priv->rateCountDiffRecord = rate_count_diff;
@@ -2677,10 +2677,10 @@ void dm_fsync_timer_callback(unsigned long data)
 			write_nic_byte(dev, 0xC36, 0x5c);
 			write_nic_byte(dev, 0xC3e, 0x96);
 		}
-		priv->ContiuneDiffCount = 0;
+		priv->ContinueDiffCount = 0;
 		write_nic_dword(dev, rOFDM0_RxDetector2, 0x465c52cd);
 	}
-	RT_TRACE(COMP_HALDM, "ContiuneDiffCount %d\n", priv->ContiuneDiffCount);
+	RT_TRACE(COMP_HALDM, "ContinueDiffCount %d\n", priv->ContinueDiffCount);
 	RT_TRACE(COMP_HALDM, "rateRecord %d rateCount %d, rateCountdiff %d "
 		 "bSwitchFsync %d\n", priv->rate_record, rate_count,
 		 rate_count_diff, priv->bswitch_fsync);
@@ -2723,7 +2723,7 @@ static void dm_EndSWFsync(struct net_device *dev)
 		write_nic_byte(dev, 0xC3e, 0x96);
 	}
 
-	priv->ContiuneDiffCount = 0;
+	priv->ContinueDiffCount = 0;
 	write_nic_dword(dev, rOFDM0_RxDetector2, 0x465c52cd);
 }
 
@@ -2735,7 +2735,7 @@ static void dm_StartSWFsync(struct net_device *dev)
 
 	RT_TRACE(COMP_HALDM, "%s\n", __func__);
 	priv->rate_record = 0;
-	priv->ContiuneDiffCount = 0;
+	priv->ContinueDiffCount = 0;
 	priv->rateCountDiffRecord = 0;
 	priv->bswitch_fsync  = false;
 
diff --git a/drivers/staging/rtl8192e/rtl8192e/rtl_wx.c b/drivers/staging/rtl8192e/rtl8192e/rtl_wx.c
index 4e93669210a..778d7baf8e0 100644
--- a/drivers/staging/rtl8192e/rtl8192e/rtl_wx.c
+++ b/drivers/staging/rtl8192e/rtl8192e/rtl_wx.c
@@ -1322,9 +1322,9 @@ static struct iw_statistics *r8192_get_wireless_stats(struct net_device *dev)
 
 struct iw_handler_def  r8192_wx_handlers_def = {
 	.standard = r8192_wx_handlers,
-	.num_standard = sizeof(r8192_wx_handlers) / sizeof(iw_handler),
+	.num_standard = ARRAY_SIZE(r8192_wx_handlers),
 	.private = r8192_private_handler,
-	.num_private = sizeof(r8192_private_handler) / sizeof(iw_handler),
+	.num_private = ARRAY_SIZE(r8192_private_handler),
 	.num_private_args = sizeof(r8192_private_args) /
 			    sizeof(struct iw_priv_args),
 	.get_wireless_stats = r8192_get_wireless_stats,
diff --git a/drivers/staging/rtl8192e/rtl819x_TSProc.c b/drivers/staging/rtl8192e/rtl819x_TSProc.c
index 711a096be7a..658e875232a 100644
--- a/drivers/staging/rtl8192e/rtl819x_TSProc.c
+++ b/drivers/staging/rtl8192e/rtl819x_TSProc.c
@@ -310,7 +310,7 @@ bool GetTs(struct rtllib_device *ieee, struct ts_common_info **ppTS,
 	   u8 *Addr, u8 TID, enum tr_select TxRxSelect, bool bAddNewTs)
 {
 	u8	UP = 0;
-	if (is_broadcast_ether_addr(Addr) || is_multicast_ether_addr(Addr)) {
+	if (is_multicast_ether_addr(Addr)) {
 		RTLLIB_DEBUG(RTLLIB_DL_ERR, "ERR! get TS for Broadcast or "
 			     "Multicast\n");
 		return false;
diff --git a/drivers/staging/rtl8192e/rtllib.h b/drivers/staging/rtl8192e/rtllib.h
index e26aec86a5c..d7460ae3a76 100644
--- a/drivers/staging/rtl8192e/rtllib.h
+++ b/drivers/staging/rtl8192e/rtllib.h
@@ -169,7 +169,7 @@ struct cb_desc {
 
 	u8 nStuckCount;
 
-	/* Tx Firmware Relaged flags (10-11)*/
+	/* Tx Firmware Related flags (10-11)*/
 	u8 bCTSEnable:1;
 	u8 bRTSEnable:1;
 	u8 bUseShortGI:1;
@@ -1690,7 +1690,7 @@ enum rtllib_state {
 	/* the association procedure is sending AUTH request*/
 	RTLLIB_ASSOCIATING_AUTHENTICATING,
 
-	/* the association procedure has successfully authentcated
+	/* the association procedure has successfully authenticated
 	 * and is sending association request
 	 */
 	RTLLIB_ASSOCIATING_AUTHENTICATED,
@@ -2409,7 +2409,7 @@ struct rtllib_device {
 
 	/* used instead of hard_start_xmit (not softmac_hard_start_xmit)
 	 * if the IEEE_SOFTMAC_TX_QUEUE feature is used to TX data
-	 * frames. I the option IEEE_SOFTMAC_SINGLE_QUEUE is also set
+	 * frames. If the option IEEE_SOFTMAC_SINGLE_QUEUE is also set
 	 * then also management frames are sent via this callback.
 	 * This function can't sleep.
 	 */
@@ -2422,12 +2422,12 @@ struct rtllib_device {
 	 */
 	void (*data_hard_stop)(struct net_device *dev);
 
-	/* OK this is complementar to data_poll_hard_stop */
+	/* OK this is complementing to data_poll_hard_stop */
 	void (*data_hard_resume)(struct net_device *dev);
 
 	/* ask to the driver to retune the radio .
 	 * This function can sleep. the driver should ensure
-	 * the radio has been swithced before return.
+	 * the radio has been switched before return.
 	 */
 	void (*set_chan)(struct net_device *dev, short ch);
 
@@ -2438,7 +2438,7 @@ struct rtllib_device {
 	 * The syncro version is similar to the start_scan but
 	 * does not return until all channels has been scanned.
 	 * this is called in user context and should sleep,
-	 * it is called in a work_queue when swithcing to ad-hoc mode
+	 * it is called in a work_queue when switching to ad-hoc mode
 	 * or in behalf of iwlist scan when the card is associated
 	 * and root user ask for a scan.
 	 * the fucntion stop_scan should stop both the syncro and
@@ -2481,7 +2481,7 @@ struct rtllib_device {
 				     struct rtllib_network *network);
 
 
-	/* check whether Tx hw resouce available */
+	/* check whether Tx hw resource available */
 	short (*check_nic_enough_desc)(struct net_device *dev, int queue_index);
 	short (*get_nic_desc_num)(struct net_device *dev, int queue_index);
 	void (*SetBWModeHandler)(struct net_device *dev,
@@ -2543,10 +2543,10 @@ struct rtllib_device {
 /* Generate probe requests */
 #define IEEE_SOFTMAC_PROBERQ (1<<4)
 
-/* Generate respones to probe requests */
+/* Generate response to probe requests */
 #define IEEE_SOFTMAC_PROBERS (1<<5)
 
-/* The ieee802.11 stack will manages the netif queue
+/* The ieee802.11 stack will manage the netif queue
  * wake/stop for the driver, taking care of 802.11
  * fragmentation. See softmac.c for details. */
 #define IEEE_SOFTMAC_TX_QUEUE (1<<7)
diff --git a/drivers/staging/rtl8192e/rtllib_rx.c b/drivers/staging/rtl8192e/rtllib_rx.c
index 13979b5ea32..8b8a5c661a2 100644
--- a/drivers/staging/rtl8192e/rtllib_rx.c
+++ b/drivers/staging/rtl8192e/rtllib_rx.c
@@ -496,7 +496,7 @@ void rtllib_indicate_packets(struct rtllib_device *ieee, struct rtllib_rxb **prx
 				memcpy(skb_push(sub_skb, ETH_ALEN), prxb->dst, ETH_ALEN);
 			}
 
-			/* Indicat the packets to upper layer */
+			/* Indicate the packets to upper layer */
 			if (sub_skb) {
 				stats->rx_packets++;
 				stats->rx_bytes += sub_skb->len;
@@ -1000,7 +1000,7 @@ static int rtllib_rx_data_filter(struct rtllib_device *ieee, u16 fc,
 			return -1;
 
 		/* {broad,multi}cast packets to our BSS go through */
-		if (is_multicast_ether_addr(dst) || is_broadcast_ether_addr(dst)) {
+		if (is_multicast_ether_addr(dst)) {
 			if (memcmp(bssid, ieee->current_network.bssid, ETH_ALEN))
 				return -1;
 		}
@@ -1233,7 +1233,7 @@ static void rtllib_rx_indicate_pkt_legacy(struct rtllib_device *ieee,
 			if (is_multicast_ether_addr(dst))
 				ieee->stats.multicast++;
 
-			/* Indicat the packets to upper layer */
+			/* Indicate the packets to upper layer */
 			memset(sub_skb->cb, 0, sizeof(sub_skb->cb));
 			sub_skb->protocol = eth_type_trans(sub_skb, dev);
 			sub_skb->dev = dev;
@@ -1269,7 +1269,7 @@ static int rtllib_rx_InfraAdhoc(struct rtllib_device *ieee, struct sk_buff *skb,
 	sc = le16_to_cpu(hdr->seq_ctl);
 
 	/*Filter pkt not to me*/
-	multicast = is_multicast_ether_addr(hdr->addr1)|is_broadcast_ether_addr(hdr->addr1);
+	multicast = is_multicast_ether_addr(hdr->addr1);
 	unicast = !multicast;
 	if (unicast && (compare_ether_addr(dev->dev_addr, hdr->addr1) != 0)) {
 		if (ieee->bNetPromiscuousMode)
@@ -1350,7 +1350,7 @@ static int rtllib_rx_InfraAdhoc(struct rtllib_device *ieee, struct sk_buff *skb,
 	/* Get TS for Rx Reorder  */
 	hdr = (struct rtllib_hdr_4addr *) skb->data;
 	if (ieee->current_network.qos_data.active && IsQoSDataFrame(skb->data)
-		&& !is_multicast_ether_addr(hdr->addr1) && !is_broadcast_ether_addr(hdr->addr1)
+		&& !is_multicast_ether_addr(hdr->addr1)
 		&& (!bToOtherSTA)) {
 		TID = Frame_QoSTID(skb->data);
 		SeqNum = WLAN_GET_SEQ_SEQ(sc);
diff --git a/drivers/staging/rtl8192e/rtllib_softmac.c b/drivers/staging/rtl8192e/rtllib_softmac.c
index c5a15dba1bf..a21b4d91a59 100644
--- a/drivers/staging/rtl8192e/rtllib_softmac.c
+++ b/drivers/staging/rtl8192e/rtllib_softmac.c
@@ -31,7 +31,7 @@ short rtllib_is_shortslot(const struct rtllib_network *net)
 	return net->capability & WLAN_CAPABILITY_SHORT_SLOT_TIME;
 }
 
-/* returns the total length needed for pleacing the RATE MFIE
+/* returns the total length needed for placing the RATE MFIE
  * tag and the EXTENDED RATE MFIE tag if needed.
  * It encludes two bytes per tag for the tag itself and its len
  */
@@ -49,7 +49,7 @@ static unsigned int rtllib_MFIE_rate_len(struct rtllib_device *ieee)
 	return rate_len;
 }
 
-/* pleace the MFIE rate, tag to the memory (double) poined.
+/* place the MFIE rate, tag to the memory (double) pointed.
  * Then it updates the pointer so that
  * it points after the new MFIE tag added.
  */
@@ -557,7 +557,7 @@ void rtllib_softmac_scan_syncro(struct rtllib_device *ieee, u8 is_mesh)
 		 *    new network events, despite for updating the net list,
 		 *    but we are temporarly 'unlinked' as the driver shall
 		 *    not filter RX frames and the channel is changing.
-		 * So the only situation in witch are interested is to check
+		 * So the only situation in which are interested is to check
 		 * if the state become LINKED because of the #1 situation
 		 */
 
@@ -1681,7 +1681,7 @@ inline void rtllib_softmac_new_net(struct rtllib_device *ieee,
 
 		/* if the user set the AP check if match.
 		 * if the network does not broadcast essid we check the
-		 *	 user supplyed ANY essid
+		 *	 user supplied ANY essid
 		 * if the network does broadcast and the user does not set
 		 *	 essid it is OK
 		 * if the network does broadcast and the user did set essid
@@ -2444,16 +2444,16 @@ inline int rtllib_rx_frame_softmac(struct rtllib_device *ieee,
 
 /* following are for a simplier TX queue management.
  * Instead of using netif_[stop/wake]_queue the driver
- * will uses these two function (plus a reset one), that
- * will internally uses the kernel netif_* and takes
+ * will use these two functions (plus a reset one), that
+ * will internally use the kernel netif_* and takes
  * care of the ieee802.11 fragmentation.
  * So the driver receives a fragment per time and might
- * call the stop function when it want without take care
- * to have enought room to TX an entire packet.
- * This might be useful if each fragment need it's own
+ * call the stop function when it wants to not
+ * have enough room to TX an entire packet.
+ * This might be useful if each fragment needs it's own
  * descriptor, thus just keep a total free memory > than
- * the max fragmentation treshold is not enought.. If the
- * ieee802.11 stack passed a TXB struct then you needed
+ * the max fragmentation threshold is not enough.. If the
+ * ieee802.11 stack passed a TXB struct then you need
  * to keep N free descriptors where
  * N = MAX_PACKET_SIZE / MIN_FRAG_TRESHOLD
  * In this way you need just one and the 802.11 stack
@@ -2696,15 +2696,15 @@ static void rtllib_start_ibss_wq(void *data)
 	rtllib_softmac_check_all_nets(ieee);
 
 
-	/* if not then the state is not linked. Maybe the user swithced to
+	/* if not then the state is not linked. Maybe the user switched to
 	 * ad-hoc mode just after being in monitor mode, or just after
 	 * being very few time in managed mode (so the card have had no
 	 * time to scan all the chans..) or we have just run up the iface
 	 * after setting ad-hoc mode. So we have to give another try..
 	 * Here, in ibss mode, should be safe to do this without extra care
-	 * (in bss mode we had to make sure no-one tryed to associate when
+	 * (in bss mode we had to make sure no-one tried to associate when
 	 * we had just checked the ieee->state and we was going to start the
-	 * scan) beacause in ibss mode the rtllib_new_net function, when
+	 * scan) because in ibss mode the rtllib_new_net function, when
 	 * finds a good net, just set the ieee->state to RTLLIB_LINKED,
 	 * so, at worst, we waste a bit of time to initiate an unneeded syncro
 	 * scan, that will stop at the first round because it sees the state
@@ -2819,7 +2819,7 @@ void rtllib_start_bss(struct rtllib_device *ieee)
 
 	/* ensure no-one start an associating process (thus setting
 	 * the ieee->state to rtllib_ASSOCIATING) while we
-	 * have just cheked it and we are going to enable scan.
+	 * have just checked it and we are going to enable scan.
 	 * The rtllib_new_net function is always called with
 	 * lock held (from both rtllib_softmac_check_all_nets and
 	 * the rx path), so we cannot be in the middle of such function
@@ -2872,7 +2872,7 @@ static void rtllib_associate_retry_wq(void *data)
 
 	/* until we do not set the state to RTLLIB_NOLINK
 	* there are no possibility to have someone else trying
-	* to start an association procdure (we get here with
+	* to start an association procedure (we get here with
 	* ieee->state = RTLLIB_ASSOCIATING).
 	* When we set the state to RTLLIB_NOLINK it is possible
 	* that the RX path run an attempt to associate, but
@@ -3679,8 +3679,7 @@ void rtllib_MlmeDisassociateRequest(struct rtllib_device *rtllib, u8 *asSta,
 
 	RemovePeerTS(rtllib, asSta);
 
-
-	if (memcpy(rtllib->current_network.bssid, asSta, 6) == NULL) {
+	if (memcmp(rtllib->current_network.bssid, asSta, 6) == 0) {
 		rtllib->state = RTLLIB_NOLINK;
 
 		for (i = 0; i < 6; i++)
diff --git a/drivers/staging/rtl8192e/rtllib_softmac_wx.c b/drivers/staging/rtl8192e/rtllib_softmac_wx.c
index 1523bc7a210..1bb6b52e0f2 100644
--- a/drivers/staging/rtl8192e/rtllib_softmac_wx.c
+++ b/drivers/staging/rtl8192e/rtllib_softmac_wx.c
@@ -479,7 +479,7 @@ int rtllib_wx_set_essid(struct rtllib_device *ieee,
 
 
 	/* this is just to be sure that the GET wx callback
-	 * has consisten infos. not needed otherwise
+	 * has consistent infos. not needed otherwise
 	 */
 	spin_lock_irqsave(&ieee->lock, flags);
 
@@ -575,7 +575,7 @@ int rtllib_wx_set_power(struct rtllib_device *ieee,
 	if ((!ieee->sta_wake_up) ||
 	    (!ieee->enter_sleep_state) ||
 	    (!ieee->ps_is_queue_empty)) {
-		RTLLIB_DEBUG(RTLLIB_DL_ERR, "%s(): PS mode is tryied to be use "
+		RTLLIB_DEBUG(RTLLIB_DL_ERR, "%s(): PS mode is tried to be use "
 			     "but driver missed a callback\n\n", __func__);
 		return -1;
 	}
diff --git a/drivers/staging/rtl8192e/rtllib_tx.c b/drivers/staging/rtl8192e/rtllib_tx.c
index f451bfc27a8..42900ee4825 100644
--- a/drivers/staging/rtl8192e/rtllib_tx.c
+++ b/drivers/staging/rtl8192e/rtllib_tx.c
@@ -59,7 +59,7 @@
 802.11 Data Frame
 
 
-802.11 frame_contorl for data frames - 2 bytes
+802.11 frame_control for data frames - 2 bytes
      ,-----------------------------------------------------------------------------------------.
 bits | 0  |  1  |  2  |  3  |  4  |  5  |  6  |  7  |  8  |  9  |  a  |  b  |  c  |  d  |  e   |
      |----|-----|-----|-----|-----|-----|-----|-----|-----|-----|-----|-----|-----|-----|------|
@@ -296,8 +296,7 @@ static void rtllib_tx_query_agg_cap(struct rtllib_device *ieee,
 		return;
 	if (!IsQoSDataFrame(skb->data))
 		return;
-	if (is_multicast_ether_addr(hdr->addr1) ||
-	    is_broadcast_ether_addr(hdr->addr1))
+	if (is_multicast_ether_addr(hdr->addr1))
 		return;
 
 	if (tcb_desc->bdhcp || ieee->CntAfterLink < 2)
@@ -515,7 +514,7 @@ u16 rtllib_query_seqnum(struct rtllib_device *ieee, struct sk_buff *skb,
 {
 	u16 seqnum = 0;
 
-	if (is_multicast_ether_addr(dst) || is_broadcast_ether_addr(dst))
+	if (is_multicast_ether_addr(dst))
 		return 0;
 	if (IsQoSDataFrame(skb->data)) {
 		struct tx_ts_record *pTS = NULL;
@@ -576,7 +575,7 @@ int rtllib_xmit_inter(struct sk_buff *skb, struct net_device *dev)
 
 	spin_lock_irqsave(&ieee->lock, flags);
 
-	/* If there is no driver handler to take the TXB, dont' bother
+	/* If there is no driver handler to take the TXB, don't bother
 	 * creating it... */
 	if ((!ieee->hard_start_xmit && !(ieee->softmac_features &
 	   IEEE_SOFTMAC_TX_QUEUE)) ||
@@ -698,8 +697,7 @@ int rtllib_xmit_inter(struct sk_buff *skb, struct net_device *dev)
 			       ETH_ALEN);
 		}
 
-		bIsMulticast = is_broadcast_ether_addr(header.addr1) ||
-			       is_multicast_ether_addr(header.addr1);
+		bIsMulticast = is_multicast_ether_addr(header.addr1);
 
 		header.frame_ctl = cpu_to_le16(fc);
 
@@ -738,7 +736,7 @@ int rtllib_xmit_inter(struct sk_buff *skb, struct net_device *dev)
 		   (CFG_RTLLIB_COMPUTE_FCS | CFG_RTLLIB_RESERVE_FCS))
 			bytes_per_frag -= RTLLIB_FCS_LEN;
 
-		/* Each fragment may need to have room for encryptiong
+		/* Each fragment may need to have room for encrypting
 		 * pre/postfix */
 		if (encrypt) {
 			bytes_per_frag -= crypt->ops->extra_mpdu_prefix_len +
diff --git a/drivers/staging/rtl8192e/rtllib_wx.c b/drivers/staging/rtl8192e/rtllib_wx.c
index c27ff7edbaf..c7e8d4d8ec2 100644
--- a/drivers/staging/rtl8192e/rtllib_wx.c
+++ b/drivers/staging/rtl8192e/rtllib_wx.c
@@ -88,7 +88,7 @@ static inline char *rtl819x_translate_scan(struct rtllib_device *ieee,
 	}
 	/* Add the protocol name */
 	iwe.cmd = SIOCGIWNAME;
-	for (i = 0; i < (sizeof(rtllib_modes)/sizeof(rtllib_modes[0])); i++) {
+	for (i = 0; i < ARRAY_SIZE(rtllib_modes); i++) {
 		if (network->mode&(1<<i)) {
 			sprintf(pname, rtllib_modes[i].mode_string,
 				rtllib_modes[i].mode_size);
@@ -408,7 +408,7 @@ int rtllib_wx_set_encode(struct rtllib_device *ieee,
 				       (*crypt)->priv);
 		sec.flags |= (1 << key);
 		/* This ensures a key will be activated if no key is
-		 * explicitely set */
+		 * explicitly set */
 		if (key == sec.active_key)
 			sec.flags |= SEC_ACTIVE_KEY;
 		ieee->crypt_info.tx_keyidx = key;
diff --git a/drivers/staging/rtl8192u/ieee80211/ieee80211_module.c b/drivers/staging/rtl8192u/ieee80211/ieee80211_module.c
index e3d47bcf4ca..82d4bf6a86a 100644
--- a/drivers/staging/rtl8192u/ieee80211/ieee80211_module.c
+++ b/drivers/staging/rtl8192u/ieee80211/ieee80211_module.c
@@ -161,7 +161,7 @@ struct net_device *alloc_ieee80211(int sizeof_priv)
 	if (ieee->pHTInfo == NULL)
 	{
 		IEEE80211_DEBUG(IEEE80211_DL_ERR, "can't alloc memory for HTInfo\n");
-		return NULL;
+		goto failed;
 	}
 	HTUpdateDefaultSetting(ieee);
 	HTInitializeHTInfo(ieee); //may move to other place.
diff --git a/drivers/staging/rtl8192u/ieee80211/ieee80211_rx.c b/drivers/staging/rtl8192u/ieee80211/ieee80211_rx.c
index be2a28cf8ed..e3cf7a45b90 100644
--- a/drivers/staging/rtl8192u/ieee80211/ieee80211_rx.c
+++ b/drivers/staging/rtl8192u/ieee80211/ieee80211_rx.c
@@ -671,7 +671,7 @@ void RxReorderIndicatePacket( struct ieee80211_device *ieee,
 		index = 1;
 	} else {
 		/* Current packet is going to be inserted into pending list.*/
-		//IEEE80211_DEBUG(IEEE80211_DL_REORDER,"%s(): We RX no ordered packed, insert to orderd list\n",__FUNCTION__);
+		//IEEE80211_DEBUG(IEEE80211_DL_REORDER,"%s(): We RX no ordered packed, insert to ordered list\n",__FUNCTION__);
 		if(!list_empty(&ieee->RxReorder_Unused_List)) {
 			pReorderEntry = (PRX_REORDER_ENTRY)list_entry(ieee->RxReorder_Unused_List.next,RX_REORDER_ENTRY,List);
 			list_del_init(&pReorderEntry->List);
@@ -1285,7 +1285,7 @@ int ieee80211_rx(struct ieee80211_device *ieee, struct sk_buff *skb,
 */
 //added by amy for reorder
 	if(ieee->current_network.qos_data.active && IsQoSDataFrame(skb->data)
-		&& !is_multicast_ether_addr(hdr->addr1) && !is_broadcast_ether_addr(hdr->addr1))
+		&& !is_multicast_ether_addr(hdr->addr1))
 	{
 		TID = Frame_QoSTID(skb->data);
 		SeqNum = WLAN_GET_SEQ_SEQ(sc);
diff --git a/drivers/staging/rtl8192u/ieee80211/ieee80211_softmac.c b/drivers/staging/rtl8192u/ieee80211/ieee80211_softmac.c
index c2ab5fa1546..f6ff8cff313 100644
--- a/drivers/staging/rtl8192u/ieee80211/ieee80211_softmac.c
+++ b/drivers/staging/rtl8192u/ieee80211/ieee80211_softmac.c
@@ -2062,7 +2062,7 @@ ieee80211_rx_frame_softmac(struct ieee80211_device *ieee, struct sk_buff *skb,
 							}
 						}else{
 							ieee->softmac_stats.rx_auth_rs_err++;
-							IEEE80211_DEBUG_MGMT("Authentication respose status code 0x%x",errcode);
+							IEEE80211_DEBUG_MGMT("Authentication response status code 0x%x",errcode);
 							ieee80211_associate_abort(ieee);
 						}
 
diff --git a/drivers/staging/rtl8192u/ieee80211/ieee80211_tx.c b/drivers/staging/rtl8192u/ieee80211/ieee80211_tx.c
index 59c45a510ef..3f5ceeb88b6 100644
--- a/drivers/staging/rtl8192u/ieee80211/ieee80211_tx.c
+++ b/drivers/staging/rtl8192u/ieee80211/ieee80211_tx.c
@@ -314,7 +314,7 @@ void ieee80211_tx_query_agg_cap(struct ieee80211_device* ieee, struct sk_buff* s
 	if (!IsQoSDataFrame(skb->data))
 		return;
 
-	if (is_multicast_ether_addr(hdr->addr1) || is_broadcast_ether_addr(hdr->addr1))
+	if (is_multicast_ether_addr(hdr->addr1))
 		return;
 	//check packet and mode later
 #ifdef TO_DO_LIST
@@ -575,7 +575,7 @@ void ieee80211_txrate_selectmode(struct ieee80211_device* ieee, cb_desc* tcb_des
 
 void ieee80211_query_seqnum(struct ieee80211_device*ieee, struct sk_buff* skb, u8* dst)
 {
-	if (is_multicast_ether_addr(dst) || is_broadcast_ether_addr(dst))
+	if (is_multicast_ether_addr(dst))
 		return;
 	if (IsQoSDataFrame(skb->data)) //we deal qos data only
 	{
@@ -693,8 +693,7 @@ int ieee80211_xmit(struct sk_buff *skb, struct net_device *dev)
 
 		/* Determine fragmentation size based on destination (multicast
 		* and broadcast are not fragmented) */
-		if (is_multicast_ether_addr(header.addr1) ||
-		is_broadcast_ether_addr(header.addr1)) {
+		if (is_multicast_ether_addr(header.addr1)) {
 			frag_size = MAX_FRAG_THRESHOLD;
 			qos_ctl |= QOS_CTL_NOTCONTAIN_ACK;
 		}
diff --git a/drivers/staging/rtl8192u/ieee80211/rtl819x_TSProc.c b/drivers/staging/rtl8192u/ieee80211/rtl819x_TSProc.c
index 957ce4ef48b..06a9824bbff 100644
--- a/drivers/staging/rtl8192u/ieee80211/rtl819x_TSProc.c
+++ b/drivers/staging/rtl8192u/ieee80211/rtl819x_TSProc.c
@@ -350,7 +350,7 @@ bool GetTs(
 	// We do not build any TS for Broadcast or Multicast stream.
 	// So reject these kinds of search here.
 	//
-	if(is_broadcast_ether_addr(Addr) || is_multicast_ether_addr(Addr))
+	if (is_multicast_ether_addr(Addr))
 	{
 		IEEE80211_DEBUG(IEEE80211_DL_ERR, "get TS for Broadcast or Multicast\n");
 		return false;
diff --git a/drivers/staging/rtl8192u/r8180_93cx6.c b/drivers/staging/rtl8192u/r8180_93cx6.c
index 8878cfeb0fb..3c515b7bc54 100644
--- a/drivers/staging/rtl8192u/r8180_93cx6.c
+++ b/drivers/staging/rtl8192u/r8180_93cx6.c
@@ -14,7 +14,7 @@
 
    Parts of this driver are based on the Intel Pro Wireless 2100 GPL driver.
 
-   We want to tanks the Authors of those projects and the Ndiswrapper
+   We want to thank the Authors of those projects and the Ndiswrapper
    project Authors.
 */
 
diff --git a/drivers/staging/rtl8192u/r8180_93cx6.h b/drivers/staging/rtl8192u/r8180_93cx6.h
index fb3ac9766ea..5cea51e1142 100644
--- a/drivers/staging/rtl8192u/r8180_93cx6.h
+++ b/drivers/staging/rtl8192u/r8180_93cx6.h
@@ -7,7 +7,7 @@
 	Parts of this driver are based on the rtl8180 driver skeleton from Patric Schenke & Andres Salomon
 	Parts of this driver are based on the Intel Pro Wireless 2100 GPL driver
 
-	We want to tanks the Authors of such projects and the Ndiswrapper project Authors.
+	We want to thank the Authors of such projects and the Ndiswrapper project Authors.
 */
 
 /*This files contains card eeprom (93c46 or 93c56) programming routines*/
diff --git a/drivers/staging/rtl8192u/r8192U.h b/drivers/staging/rtl8192u/r8192U.h
index 9b81f26d40f..57e3383cc93 100644
--- a/drivers/staging/rtl8192u/r8192U.h
+++ b/drivers/staging/rtl8192u/r8192U.h
@@ -11,7 +11,7 @@
 
    Parts of this driver are based on the Intel Pro Wireless 2100 GPL driver
 
-   We want to tanks the Authors of those projects and the Ndiswrapper
+   We want to thank the Authors of those projects and the Ndiswrapper
    project Authors.
 */
 
@@ -98,7 +98,7 @@ do { if(rt_global_debug_component & component) \
 #define COMP_INIT				BIT2		// during driver initialization / halt / reset.
 
 
-#define COMP_RECV				BIT3		// Reveive part data path.
+#define COMP_RECV				BIT3		// Receive data path.
 #define COMP_SEND				BIT4		// Send part path.
 #define COMP_IO					BIT5		// I/O Related. Added by Annie, 2006-03-02.
 #define COMP_POWER				BIT6		// 802.11 Power Save mode or System/Device Power state related.
@@ -322,7 +322,7 @@ typedef struct _tx_fwinfo_819x_usb {
         u8		TxSubCarrier:2;         // This is used for legacy OFDM rate only.
         u8		STBC:2;
         u8		AllowAggregation:1;
-        u8		RtsHT:1;                //Interpre RtsRate field as high throughput data rate
+        u8		RtsHT:1;                //Interpret RtsRate field as high throughput data rate
         u8		RtsShort:1;             //Short PLCP for CCK, or short GI for 11n MCS
         u8		RtsBandwidth:1;         // This is used for HT MCS rate only.
         u8		RtsSubcarrier:2;        // This is used for legacy OFDM rate only.
@@ -610,7 +610,6 @@ typedef struct Stats
 //	unsigned long rxnopointer;
 	unsigned long rxok;
 	unsigned long rxframgment;
-	unsigned long rxcmdpkt[4];		//08/05/08 amy rx cmd element txfeedback/bcn report/cfg set/query
 	unsigned long rxurberr;
 	unsigned long rxstaterr;
 	unsigned long received_rate_histogram[4][32];	//0: Total, 1:OK, 2:CRC, 3:ICV, 2007 07 03 cosa
@@ -1117,7 +1116,7 @@ typedef struct r8192_priv
 	bool bfsync_processing;	// 500ms Fsync timer is active or not
 	u32 	rate_record;
 	u32 	rateCountDiffRecord;
-	u32	ContiuneDiffCount;
+	u32	ContinueDiffCount;
 	bool bswitch_fsync;
 
 	u8	framesync;
diff --git a/drivers/staging/rtl8192u/r8192U_core.c b/drivers/staging/rtl8192u/r8192U_core.c
index 9c00865f302..5981d665832 100644
--- a/drivers/staging/rtl8192u/r8192U_core.c
+++ b/drivers/staging/rtl8192u/r8192U_core.c
@@ -203,7 +203,7 @@ static void rtl819x_set_channel_map(u8 channel_plan, struct r8192_priv* priv)
 		{
 			Dot11d_Init(ieee);
 			ieee->bGlobalDomain = false;
-			//acturally 8225 & 8256 rf chip only support B,G,24N mode
+			//actually 8225 & 8256 rf chips only support B,G,24N mode
 			if ((priv->rf_chip == RF_8225) || (priv->rf_chip == RF_8256))
 			{
 				min_chan = 1;
@@ -1103,7 +1103,7 @@ inline u16 rtl8192_rate2rate(short rate)
 }
 
 
-/* The protype of rx_isr has changed since one verion of Linux Kernel */
+/* The prototype of rx_isr has changed since one version of Linux Kernel */
 static void rtl8192_rx_isr(struct urb *urb)
 {
 	struct sk_buff *skb = (struct sk_buff *) urb->context;
@@ -1476,7 +1476,7 @@ static void rtl8192_tx_isr(struct urb *tx_urb)
 	if(tcb_desc->queue_index != TXCMD_QUEUE) {
 		if(tx_urb->status == 0) {
 			dev->trans_start = jiffies;
-			// As act as station mode, destion shall be  unicast address.
+			// Act as station mode, destination shall be unicast address.
 			//priv->ieee80211->stats.tx_bytes+=(skb->len - priv->ieee80211->tx_headroom);
 			//priv->ieee80211->stats.tx_packets++;
 			priv->stats.txoktotal++;
@@ -1522,13 +1522,13 @@ static void rtl8192_tx_isr(struct urb *tx_urb)
 			else if ((skb_queue_len(&priv->ieee80211->skb_drv_aggQ[queue_index])!= 0)&&\
 				(!(priv->ieee80211->queue_stop))) {
 				// Tx Driver Aggregation process
-				/* The driver will aggregation the packets according to the following stets
+				/* The driver will aggregation the packets according to the following stats
 				 * 1. check whether there's tx irq available, for it's a completion return
 				 *    function, it should contain enough tx irq;
-				 * 2. check pakcet type;
+				 * 2. check packet type;
 				 * 3. initialize sendlist, check whether the to-be send packet no greater than 1
-				 * 4. aggregation the packets, and fill firmware info and tx desc to it, etc.
-				 * 5. check whehter the packet could be sent, otherwise just insert to wait head
+				 * 4. aggregates the packets, and fill firmware info and tx desc into it, etc.
+				 * 5. check whether the packet could be sent, otherwise just insert into wait head
 				 * */
 				skb = skb_dequeue(&priv->ieee80211->skb_drv_aggQ[queue_index]);
 				if(!check_nic_enough_desc(dev, queue_index)) {
@@ -2447,7 +2447,7 @@ static int rtl8192_qos_handle_probe_response(struct r8192_priv *priv,
 	return 0;
 }
 
-/* handle manage frame frame beacon and probe response */
+/* handle and manage frame from beacon and probe response */
 static int rtl8192_handle_beacon(struct net_device * dev,
 			      struct ieee80211_beacon * beacon,
 			      struct ieee80211_network * network)
@@ -2625,7 +2625,7 @@ bool GetHalfNmodeSupportByAPs819xUsb(struct net_device* dev)
 void rtl8192_refresh_supportrate(struct r8192_priv* priv)
 {
 	struct ieee80211_device* ieee = priv->ieee80211;
-	//we donot consider set support rate for ABG mode, only HT MCS rate is set here.
+	//we do not consider set support rate for ABG mode, only HT MCS rate is set here.
 	if (ieee->mode == WIRELESS_MODE_N_24G || ieee->mode == WIRELESS_MODE_N_5G)
 	{
 		memcpy(ieee->Regdot11HTOperationalRateSet, ieee->RegHTSuppRateSet, 16);
@@ -2780,10 +2780,10 @@ static void rtl8192_init_priv_variable(struct net_device* dev)
 	priv->TransmitConfig =
 	//	TCR_DurProcMode |	//for RTL8185B, duration setting by HW
 	//?	TCR_DISReqQsize |
-		(TCR_MXDMA_2048<<TCR_MXDMA_OFFSET)|  // Max DMA Burst Size per Tx DMA Burst, 7: reservied.
+		(TCR_MXDMA_2048<<TCR_MXDMA_OFFSET)|  // Max DMA Burst Size per Tx DMA Burst, 7: reserved.
 		(priv->ShortRetryLimit<<TCR_SRL_OFFSET)|	// Short retry limit
 		(priv->LongRetryLimit<<TCR_LRL_OFFSET) |	// Long retry limit
-		(false ? TCR_SAT: 0);	// FALSE: HW provies PLCP length and LENGEXT, TURE: SW proiveds them
+		(false ? TCR_SAT: 0);	// FALSE: HW provides PLCP length and LENGEXT, TRUE: SW provides them
 #ifdef TO_DO_LIST
 	if(Adapter->bInHctTest)
 		pHalData->ReceiveConfig	=	pHalData->CSMethod |
@@ -3437,7 +3437,7 @@ if(Adapter->ResetProgress == RESET_TYPE_NORESET)
 		{ // User disable RF via registry.
 			RT_TRACE((COMP_INIT|COMP_RF), DBG_LOUD, ("InitializeAdapter819xUsb(): Turn off RF for RegRfOff ----------\n"));
 			MgntActSet_RF_State(Adapter, eRfOff, RF_CHANGE_BY_SW);
-			// Those action will be discard in MgntActSet_RF_State because off the same state
+			// Those actions will be discard in MgntActSet_RF_State because of the same state
 			for(eRFPath = 0; eRFPath <pHalData->NumTotalRFPath; eRFPath++)
 				PHY_SetRFReg(Adapter, (RF90_RADIO_PATH_E)eRFPath, 0x4, 0xC00, 0x0);
 		}
@@ -3458,7 +3458,7 @@ if(Adapter->ResetProgress == RESET_TYPE_NORESET)
 		if(pHalData->eRFPowerState == eRfOff)
 		{
 			MgntActSet_RF_State(Adapter, eRfOff, pMgntInfo->RfOffReason);
-			// Those action will be discard in MgntActSet_RF_State because off the same state
+			// Those actions will be discard in MgntActSet_RF_State because of the same state
 			for(eRFPath = 0; eRFPath <pHalData->NumTotalRFPath; eRFPath++)
 				PHY_SetRFReg(Adapter, (RF90_RADIO_PATH_E)eRFPath, 0x4, 0xC00, 0x0);
 		}
@@ -3586,7 +3586,7 @@ TxCheckStuck(struct net_device *dev)
 	//unsigned long flags;
 
 	//
-	// Decide Stuch threshold according to current power save mode
+	// Decide such threshold according to current power save mode
 	//
 
 //     RT_TRACE(COMP_RESET, " ==> TxCheckStuck()\n");
@@ -3745,7 +3745,7 @@ rtl819x_ifcheck_resetornot(struct net_device *dev)
 
 		// Driver should not check RX stuck in IBSS mode because it is required to
 		// set Check BSSID in order to send beacon, however, if check BSSID is
-		// set, STA cannot hear any packet a all. Emily, 2008.04.12
+		// set, STA cannot hear any packet at all. Emily, 2008.04.12
 		RxResetType = RxCheckStuck(dev);
 	}
 	if(TxResetType==RESET_TYPE_NORMAL || RxResetType==RESET_TYPE_NORMAL)
@@ -3962,7 +3962,7 @@ RESET_START:
 		up(&priv->wx_sem);
 		RT_TRACE(COMP_RESET,"%s():<==========down process is finished\n",__FUNCTION__);
 	//rtl8192_irq_disable(dev);
-		RT_TRACE(COMP_RESET,"%s():===========>start to up the driver\n",__FUNCTION__);
+		RT_TRACE(COMP_RESET,"%s():===========>start up the driver\n",__FUNCTION__);
 		reset_status = _rtl8192_up(dev);
 
 		RT_TRACE(COMP_RESET,"%s():<===========up process is finished\n",__FUNCTION__);
@@ -4155,7 +4155,7 @@ extern	void	rtl819x_watchdog_wqcallback(struct work_struct *work)
 void watch_dog_timer_callback(unsigned long data)
 {
 	struct r8192_priv *priv = ieee80211_priv((struct net_device *) data);
-	//printk("===============>watch_dog  timer\n");
+	//printk("===============>watch_dog timer\n");
 	queue_delayed_work(priv->priv_wq,&priv->watch_dog_wq, 0);
 	mod_timer(&priv->watch_dog_timer, jiffies + MSECS(IEEE80211_WATCH_DOG_TIME));
 }
@@ -4170,7 +4170,7 @@ int _rtl8192_up(struct net_device *dev)
 	init_status = rtl8192_adapter_start(dev);
 	if(!init_status)
 	{
-		RT_TRACE(COMP_ERR,"ERR!!! %s(): initialization is failed!\n", __FUNCTION__);
+		RT_TRACE(COMP_ERR,"ERR!!! %s(): initialization failed!\n", __FUNCTION__);
 		priv->up=priv->ieee80211->ieee_up = 0;
 		return -EAGAIN;
 	}
@@ -4256,7 +4256,7 @@ int rtl8192_down(struct net_device *dev)
 		skb_queue_purge(&priv->ieee80211->skb_drv_aggQ [i]);
 	}
 
-	//as cancel_delayed_work will del work->timer, so if work is not definedas struct delayed_work, it will corrupt
+	//as cancel_delayed_work will del work->timer, so if work is not defined as struct delayed_work, it will corrupt
 //	flush_scheduled_work();
 	rtl8192_cancel_deferred_work(priv);
 	deinit_hal_dm(dev);
@@ -4516,7 +4516,7 @@ u8 HwRateToMRate90(bool bIsHT, u8 rate)
 
 /**
  * Function:     UpdateRxPktTimeStamp
- * Overview:     Recored down the TSF time stamp when receiving a packet
+ * Overview:     Record the TSF time stamp when receiving a packet
  *
  * Input:
  *       PADAPTER        Adapter
@@ -4556,10 +4556,10 @@ long rtl819x_translate_todbm(u8 signal_strength_index	)// 0-100 index.
 }
 
 
-/* 2008/01/22 MH We can not delcare RSSI/EVM total value of sliding window to
+/* 2008/01/22 MH We can not declare RSSI/EVM total value of sliding window to
     be a local static. Otherwise, it may increase when we return from S3/S4. The
-    value will be kept in memory or disk. We must delcare the value in adapter
-    and it will be reinitialized when return from S3/S4. */
+    value will be kept in memory or disk. Declare the value in the adaptor
+    and it will be reinitialized when returned from S3/S4. */
 void rtl8192_process_phyinfo(struct r8192_priv * priv,u8* buffer, struct ieee80211_rx_stats * pprevious_stats, struct ieee80211_rx_stats * pcurrent_stats)
 {
 	bool bcheck = false;
@@ -5091,8 +5091,8 @@ static void rtl8192_query_rxphystatus(
 			tmp_rxevm =	pofdm_buf->rxevm_X[i];
 			rx_evmX = (char)(tmp_rxevm);
 
-			// Do not use shift operation like "rx_evmX >>= 1" because the compilor of free build environment
-			// fill most significant bit to "zero" when doing shifting operation which may change a negative
+			// Do not use shift operation like "rx_evmX >>= 1" because the compiler of free build environment
+			// will set the most significant bit to "zero" when doing shifting operation which may change a negative
 			// value to positive one, then the dbm value (which is supposed to be negative)  is not correct anymore.
 			rx_evmX /= 2;	//dbm
 
@@ -5171,7 +5171,7 @@ void TranslateRxSignalStuff819xUsb(struct sk_buff *skb,
 	type = WLAN_FC_GET_TYPE(fc);
 	praddr = hdr->addr1;
 
-	/* Check if the received packet is acceptabe. */
+	/* Check if the received packet is acceptable. */
 	bpacket_match_bssid = ((IEEE80211_FTYPE_CTL != type) &&
 							(eqMacAddr(priv->ieee80211->current_network.bssid,  (fc & IEEE80211_FCTL_TODS)? hdr->addr1 : (fc & IEEE80211_FCTL_FROMDS )? hdr->addr2 : hdr->addr3))
 								 && (!pstats->bHwError) && (!pstats->bCRC)&& (!pstats->bICV));
@@ -5211,7 +5211,7 @@ void TranslateRxSignalStuff819xUsb(struct sk_buff *skb,
 
 /**
 * Function:	UpdateReceivedRateHistogramStatistics
-* Overview:	Recored down the received data rate
+* Overview:	Record the received data rate
 *
 * Input:
 * 	struct net_device *dev
@@ -5401,7 +5401,7 @@ void query_rxdesc_status(struct sk_buff *skb, struct ieee80211_rx_stats *stats,
 	}
 
 #ifdef USB_RX_AGGREGATION_SUPPORT
-	/* for the rx aggregated sub frame, the redundant space truelly contained in the packet */
+	/* for the rx aggregated sub frame, the redundant space truly contained in the packet */
 	if(bIsRxAggrSubframe) {
 		skb_pull(skb, 8);
 	}
@@ -5480,7 +5480,7 @@ void rtl8192_rx_nomal(struct sk_buff* skb)
 			PacketShiftBytes = GetRxPacketShiftBytes819xUsb(&stats, false);
 		}
 #endif
-		/* Process the MPDU recevied */
+		/* Process the MPDU received */
 		skb_trim(skb, skb->len - 4/*sCrcLng*/);
 
 		rx_pkt_len = skb->len;
@@ -5538,7 +5538,7 @@ void rtl8192_rx_nomal(struct sk_buff* skb)
 				if(PacketLength > agg_skb->len) {
 					break;
 				}
-				/* Process the MPDU recevied */
+				/* Process the MPDU received */
 				skb = dev_alloc_skb(PacketLength);
 				memcpy(skb_put(skb,PacketLength),agg_skb->data, PacketLength);
 				skb_trim(skb, skb->len - 4/*sCrcLng*/);
diff --git a/drivers/staging/rtl8192u/r8192U_dm.c b/drivers/staging/rtl8192u/r8192U_dm.c
index 2dde9fa5c21..cd8dc85e9c0 100644
--- a/drivers/staging/rtl8192u/r8192U_dm.c
+++ b/drivers/staging/rtl8192u/r8192U_dm.c
@@ -38,7 +38,7 @@ static u32 edca_setting_UL[HT_IOT_PEER_MAX] =
 /*------------------------Define global variable-----------------------------*/
 // Debug variable ?
 dig_t	dm_digtable;
-// Store current shoftware write register content for MAC PHY.
+// Store current software write register content for MAC PHY.
 u8		dm_shadow[16][256] = {{0}};
 // For Dynamic Rx Path Selection by Signal Strength
 DRxPathSel	DM_RxPathSelTable;
@@ -119,7 +119,7 @@ static	void	dm_pd_th(struct net_device *dev);
 static	void	dm_cs_ratio(struct net_device *dev);
 
 static	void dm_init_ctstoself(struct net_device *dev);
-// DM --> EDCA turboe mode control
+// DM --> EDCA turbo mode control
 static	void	dm_check_edca_turbo(struct net_device *dev);
 
 // DM --> HW RF control
@@ -348,7 +348,7 @@ extern void init_rate_adaptive(struct net_device * dev)
  *
  * Revised History:
  *	When		Who		Remark
- *	05/26/08	amy 	Create version 0 proting from windows code.
+ *	05/26/08	amy 	Create version 0 porting from windows code.
  *
  *---------------------------------------------------------------------------*/
 static void dm_check_rate_adaptive(struct net_device * dev)
@@ -543,7 +543,7 @@ static u32 OFDMSwingTable[OFDM_Table_Length] = {
 	0x5a400169,	// 3, +3db
 	0x50800142,	// 4, +2db
 	0x47c0011f,	// 5, +1db
-	0x40000100,	// 6, +0db ===> default, upper for higher temprature, lower for low temprature
+	0x40000100,	// 6, +0db ===> default, upper for higher temperature, lower for low temperature
 	0x390000e4,	// 7, -1db
 	0x32c000cb,	// 8, -2db
 	0x2d4000b5,	// 9, -3db
@@ -678,7 +678,7 @@ static void dm_TXPowerTrackingCallback_TSSI(struct net_device * dev)
 		{
 			write_nic_byte(dev, 0x1ba, 0);
 			viviflag = FALSE;
-			RT_TRACE(COMP_POWER_TRACKING, "we filted this data\n");
+			RT_TRACE(COMP_POWER_TRACKING, "we filtered the data\n");
 			for(k = 0;k < 5; k++)
 				tmp_report[k] = 0;
 			break;
@@ -864,14 +864,14 @@ static void dm_TXPowerTrackingCallback_ThermalMeter(struct net_device * dev)
 	RT_TRACE(COMP_POWER_TRACKING, "Readback ThermalMeterA = %d \n", tmpRegA);
 	if(tmpRegA < 3 || tmpRegA > 13)
 		return;
-	if(tmpRegA >= 12)	// if over 12, TP will be bad when high temprature
+	if(tmpRegA >= 12)	// if over 12, TP will be bad when high temperature
 		tmpRegA = 12;
 	RT_TRACE(COMP_POWER_TRACKING, "Valid ThermalMeterA = %d \n", tmpRegA);
 	priv->ThermalMeter[0] = ThermalMeterVal;	//We use fixed value by Bryant's suggestion
 	priv->ThermalMeter[1] = ThermalMeterVal;	//We use fixed value by Bryant's suggestion
 
-	//Get current RF-A temprature index
-	if(priv->ThermalMeter[0] >= (u8)tmpRegA)	//lower temprature
+	//Get current RF-A temperature index
+	if(priv->ThermalMeter[0] >= (u8)tmpRegA)	//lower temperature
 	{
 		tmpOFDMindex = tmpCCK20Mindex = 6+(priv->ThermalMeter[0]-(u8)tmpRegA);
 		tmpCCK40Mindex = tmpCCK20Mindex - 6;
@@ -885,7 +885,7 @@ static void dm_TXPowerTrackingCallback_ThermalMeter(struct net_device * dev)
 	else
 	{
 		tmpval = ((u8)tmpRegA - priv->ThermalMeter[0]);
-		if(tmpval >= 6)								// higher temprature
+		if(tmpval >= 6)								// higher temperature
 			tmpOFDMindex = tmpCCK20Mindex = 0;		// max to +6dB
 		else
 			tmpOFDMindex = tmpCCK20Mindex = 6 - tmpval;
@@ -1457,9 +1457,9 @@ static void dm_InitializeTXPowerTracking_ThermalMeter(struct net_device *dev)
 {
 	struct r8192_priv *priv = ieee80211_priv(dev);
 
-	// Tx Power tracking by Theremal Meter require Firmware R/W 3-wire. This mechanism
+	// Tx Power tracking by Thermal Meter requires Firmware R/W 3-wire. This mechanism
 	// can be enabled only when Firmware R/W 3-wire is enabled. Otherwise, frequent r/w
-	// 3-wire by driver cause RF goes into wrong state.
+	// 3-wire by driver causes RF to go into a wrong state.
 	if(priv->ieee80211->FwRWRF)
 		priv->btxpower_tracking = TRUE;
 	else
@@ -1520,7 +1520,7 @@ static void dm_CheckTXPowerTracking_ThermalMeter(struct net_device *dev)
 
 	if(!TM_Trigger)
 	{
-		//Attention!! You have to wirte all 12bits data to RF, or it may cause RF to crash
+		//Attention!! You have to write all 12bits of data to RF, or it may cause RF to crash
 		//actually write reg0x02 bit1=0, then bit1=1.
 		//DbgPrint("Trigger ThermalMeter, write RF reg0x2 = 0x4d to 0x4f\n");
 		rtl8192_phy_SetRFReg(dev, RF90_PATH_A, 0x02, bMask12Bits, 0x4d);
@@ -1744,7 +1744,7 @@ extern void dm_restore_dynamic_mechanism_state(struct net_device *dev)
 			write_nic_dword(dev, RATR0, ratr_value);
 			write_nic_byte(dev, UFWP, 1);
 	}
-	//Resore TX Power Tracking Index
+	//Restore TX Power Tracking Index
 	if(priv->btxpower_trackingInit && priv->btxpower_tracking){
 		dm_txpower_reset_recovery(dev);
 	}
@@ -2031,7 +2031,7 @@ static void dm_dig_init(struct net_device *dev)
 	dm_digtable.dbg_mode = DM_DBG_OFF;	//off=by real rssi value, on=by DM_DigTable.Rssi_val for new dig
 	dm_digtable.dig_algorithm_switch = 0;
 
-	/* 2007/10/04 MH Define init gain threshol. */
+	/* 2007/10/04 MH Define init gain threshold. */
 	dm_digtable.dig_state		= DM_STA_DIG_MAX;
 	dm_digtable.dig_highpwr_state	= DM_STA_DIG_MAX;
 	dm_digtable.initialgain_lowerbound_state = false;
@@ -2097,7 +2097,7 @@ static void dm_ctrl_initgain_byrssi_by_driverrssi(
 		return;
 
 	//DbgPrint("Dig by Sw Rssi \n");
-	if(dm_digtable.dig_algorithm_switch)	// if swithed algorithm, we have to disable FW Dig.
+	if(dm_digtable.dig_algorithm_switch)	// if switched algorithm, we have to disable FW Dig.
 		fw_dig = 0;
 	if(fw_dig <= 3)	// execute several times to make sure the FW Dig is disabled
 	{// FW DIG Off
@@ -2160,8 +2160,8 @@ static void dm_ctrl_initgain_byrssi_by_fwfalse_alarm(
 	/*DbgPrint("DIG Check\n\r RSSI=%d LOW=%d HIGH=%d STATE=%d",
 	pHalData->UndecoratedSmoothedPWDB, DM_DigTable.RssiLowThresh,
 	DM_DigTable.RssiHighThresh, DM_DigTable.Dig_State);*/
-	/* 1. When RSSI decrease, We have to judge if it is smaller than a treshold
-		  and then execute below step. */
+	/* 1. When RSSI decrease, We have to judge if it is smaller than a threshold
+		  and then execute the step below. */
 	if ((priv->undecorated_smoothed_pwdb <= dm_digtable.rssi_low_thresh))
 	{
 		/* 2008/02/05 MH When we execute silent reset, the DIG PHY parameters
@@ -2220,8 +2220,8 @@ static void dm_ctrl_initgain_byrssi_by_fwfalse_alarm(
 
 	}
 
-	/* 2. When RSSI increase, We have to judge if it is larger than a treshold
-		  and then execute below step.  */
+	/* 2. When RSSI increase, We have to judge if it is larger than a threshold
+		  and then execute the step below.  */
 	if ((priv->undecorated_smoothed_pwdb >= dm_digtable.rssi_high_thresh) )
 	{
 		u8 reset_flag = 0;
@@ -2329,7 +2329,7 @@ static void dm_ctrl_initgain_byrssi_highpwr(
 	}
 
 	/* 3. When RSSI >75% or <70%, it is a high power issue. We have to judge if
-		  it is larger than a treshold and then execute below step.  */
+		  it is larger than a threshold and then execute the step below.  */
 	// 2008/02/05 MH SD3-Jerry Modify PD_TH for high power issue.
 	if (priv->undecorated_smoothed_pwdb >= dm_digtable.rssi_high_power_highthresh)
 	{
@@ -2841,8 +2841,8 @@ static void dm_check_rfctrl_gpio(struct net_device * dev)
 {
 	//struct r8192_priv *priv = ieee80211_priv(dev);
 
-	// Walk around for DTM test, we will not enable HW - radio on/off because r/w
-	// page 1 register before Lextra bus is enabled cause system fails when resuming
+	// Work around for DTM test, we will not enable HW - radio on/off because r/w
+	// page 1 register before extra bus is enabled causing system failures when resuming
 	// from S4. 20080218, Emily
 
 	// Stop to execute workitem to prevent S3/S4 bug.
@@ -3377,30 +3377,30 @@ extern void dm_fsync_timer_callback(unsigned long data)
 		{
 
 			u32 DiffNum = priv->rateCountDiffRecord - rate_count_diff;
-			// Contiune count
+			// Continue count
 			if(DiffNum >= priv->ieee80211->fsync_seconddiff_ratethreshold)
-				priv->ContiuneDiffCount++;
+				priv->ContinueDiffCount++;
 			else
-				priv->ContiuneDiffCount = 0;
+				priv->ContinueDiffCount = 0;
 
-			// Contiune count over
-			if(priv->ContiuneDiffCount >=2)
+			// Continue count over
+			if(priv->ContinueDiffCount >=2)
 			{
 				bSwitchFromCountDiff = true;
-				priv->ContiuneDiffCount = 0;
+				priv->ContinueDiffCount = 0;
 			}
 		}
 		else
 		{
-			// Stop contiune count
-			priv->ContiuneDiffCount = 0;
+			// Stop the continued count
+			priv->ContinueDiffCount = 0;
 		}
 
 		//If Count diff <= FsyncRateCountThreshold
 		if(rate_count_diff <= priv->ieee80211->fsync_firstdiff_ratethreshold)
 		{
 			bSwitchFromCountDiff = true;
-			priv->ContiuneDiffCount = 0;
+			priv->ContinueDiffCount = 0;
 		}
 		priv->rate_record = rate_count;
 		priv->rateCountDiffRecord = rate_count_diff;
@@ -3468,14 +3468,14 @@ extern void dm_fsync_timer_callback(unsigned long data)
 		#endif
 			write_nic_byte(dev, 0xC3e, 0x96);
 		}
-		priv->ContiuneDiffCount = 0;
+		priv->ContinueDiffCount = 0;
 	#ifdef RTL8190P
 		write_nic_dword(dev, rOFDM0_RxDetector2, 0x164052cd);
 	#else
 		write_nic_dword(dev, rOFDM0_RxDetector2, 0x465c52cd);
 	#endif
 	}
-	RT_TRACE(COMP_HALDM, "ContiuneDiffCount %d\n", priv->ContiuneDiffCount);
+	RT_TRACE(COMP_HALDM, "ContinueDiffCount %d\n", priv->ContinueDiffCount);
 	RT_TRACE(COMP_HALDM, "rateRecord %d rateCount %d, rateCountdiff %d bSwitchFsync %d\n", priv->rate_record, rate_count, rate_count_diff , priv->bswitch_fsync);
 }
 
@@ -3507,7 +3507,7 @@ static void dm_EndSWFsync(struct net_device *dev)
 		write_nic_byte(dev, 0xC3e, 0x96);
 	}
 
-	priv->ContiuneDiffCount = 0;
+	priv->ContinueDiffCount = 0;
 #ifndef RTL8190P
 	write_nic_dword(dev, rOFDM0_RxDetector2, 0x465c52cd);
 #endif
@@ -3523,8 +3523,8 @@ static void dm_StartSWFsync(struct net_device *dev)
 	RT_TRACE(COMP_HALDM,"%s\n", __FUNCTION__);
 	// Initial rate record to zero, start to record.
 	priv->rate_record = 0;
-	// Initial contiune diff count to zero, start to record.
-	priv->ContiuneDiffCount = 0;
+	// Initialize continue diff count to zero, start to record.
+	priv->ContinueDiffCount = 0;
 	priv->rateCountDiffRecord = 0;
 	priv->bswitch_fsync  = false;
 
@@ -3875,7 +3875,7 @@ static void dm_send_rssi_tofw(struct net_device *dev)
 
 	// If we test chariot, we should stop the TX command ?
 	// Because 92E will always silent reset when we send tx command. We use register
-	// 0x1e0(byte) to botify driver.
+	// 0x1e0(byte) to notify driver.
 	write_nic_byte(dev, DRIVER_RSSI, (u8)priv->undecorated_smoothed_pwdb);
 	return;
 	tx_cmd.Op		= TXCMD_SET_RX_RSSI;
diff --git a/drivers/staging/rtl8192u/r8192U_hw.h b/drivers/staging/rtl8192u/r8192U_hw.h
index e89aaf70143..1bfe871dcfb 100644
--- a/drivers/staging/rtl8192u/r8192U_hw.h
+++ b/drivers/staging/rtl8192u/r8192U_hw.h
@@ -10,7 +10,7 @@
 	Parts of this driver are based on the Intel Pro Wireless
 	2100 GPL driver.
 
-	We want to tanks the Authors of those projects
+	We want to thank the Authors of those projects
 	and the Ndiswrapper project Authors.
 */
 
diff --git a/drivers/staging/rtl8192u/r8192U_wx.c b/drivers/staging/rtl8192u/r8192U_wx.c
index f6408f98ede..71f2d2349c3 100644
--- a/drivers/staging/rtl8192u/r8192U_wx.c
+++ b/drivers/staging/rtl8192u/r8192U_wx.c
@@ -13,7 +13,7 @@
 
    Parts of this driver are based on the Intel Pro Wireless 2100 GPL driver.
 
-   We want to tanks the Authors of those projects and the Ndiswrapper
+   We want to thank the Authors of those projects and the Ndiswrapper
    project Authors.
 */
 
@@ -256,7 +256,7 @@ static int r8192_wx_get_ap_status(struct net_device *dev,
 	//count the length of input ssid
 	for(name_len=0 ; ((char*)wrqu->data.pointer)[name_len]!='\0' ; name_len++);
 
-	//search for the correspoding info which is received
+	//search for the corresponding info which is received
 	list_for_each_entry(target, &ieee->network_list, list) {
 		if ( (target->ssid_len == name_len) &&
 		     (strncmp(target->ssid, (char*)wrqu->data.pointer, name_len)==0)){
@@ -419,7 +419,7 @@ static int rtl8180_wx_get_range(struct net_device *dev,
 	range->max_qual.updated = 7; /* Updated all three */
 
 	range->avg_qual.qual = 92; /* > 8% missed beacons is 'bad' */
-	/* TODO: Find real 'good' to 'bad' threshol value for RSSI */
+	/* TODO: Find real 'good' to 'bad' threshold value for RSSI */
 	range->avg_qual.level = 20 + -98;
 	range->avg_qual.noise = 0;
 	range->avg_qual.updated = 7; /* Updated all three */
@@ -1047,7 +1047,7 @@ static iw_handler r8192_wx_handlers[] =
 #else
 	 NULL,
 #endif
-	dummy,                     /* SIOCGIWAPLIST -- depricated */
+	dummy,                     /* SIOCGIWAPLIST -- deprecated */
 	r8192_wx_set_scan,        /* SIOCSIWSCAN */
 	r8192_wx_get_scan,        /* SIOCGIWSCAN */
 	r8192_wx_set_essid,       /* SIOCSIWESSID */
@@ -1211,9 +1211,9 @@ struct iw_statistics *r8192_get_wireless_stats(struct net_device *dev)
 
 struct iw_handler_def  r8192_wx_handlers_def={
 	.standard = r8192_wx_handlers,
-	.num_standard = sizeof(r8192_wx_handlers) / sizeof(iw_handler),
+	.num_standard = ARRAY_SIZE(r8192_wx_handlers),
 	.private = r8192_private_handler,
-	.num_private = sizeof(r8192_private_handler) / sizeof(iw_handler),
+	.num_private = ARRAY_SIZE(r8192_private_handler),
 	.num_private_args = sizeof(r8192_private_args) / sizeof(struct iw_priv_args),
 #if WIRELESS_EXT >= 17
 	.get_wireless_stats = r8192_get_wireless_stats,
diff --git a/drivers/staging/rtl8192u/r8192U_wx.h b/drivers/staging/rtl8192u/r8192U_wx.h
index f4cf2801136..9f6b1050542 100644
--- a/drivers/staging/rtl8192u/r8192U_wx.h
+++ b/drivers/staging/rtl8192u/r8192U_wx.h
@@ -7,7 +7,7 @@
 	Parts of this driver are based on the rtl8180 driver skeleton from Patric Schenke & Andres Salomon
 	Parts of this driver are based on the Intel Pro Wireless 2100 GPL driver
 
-	We want to tanks the Authors of such projects and the Ndiswrapper project Authors.
+	We want to thank the Authors of such projects and the Ndiswrapper project Authors.
 */
 
 /* this file (will) contains wireless extension handlers*/
diff --git a/drivers/staging/rtl8192u/r819xU_HTType.h b/drivers/staging/rtl8192u/r819xU_HTType.h
index 2ac421626e7..e07f8b17a0d 100644
--- a/drivers/staging/rtl8192u/r819xU_HTType.h
+++ b/drivers/staging/rtl8192u/r819xU_HTType.h
@@ -211,7 +211,7 @@ typedef struct _RT_HIGH_THROUGHPUT{
 	u8				bEnableHT;
 	u8				bCurrentHTSupport;
 
-	u8				bRegBW40MHz;				// Tx 40MHz channel capablity
+	u8				bRegBW40MHz;				// Tx 40MHz channel capability
 	u8				bCurBW40MHz;				// Tx 40MHz channel capability
 
 	u8				bRegShortGI40MHz;			// Tx Short GI for 40Mhz
diff --git a/drivers/staging/rtl8192u/r819xU_cmdpkt.c b/drivers/staging/rtl8192u/r819xU_cmdpkt.c
index 0cb28c776c4..a8a6dc2c365 100644
--- a/drivers/staging/rtl8192u/r819xU_cmdpkt.c
+++ b/drivers/staging/rtl8192u/r819xU_cmdpkt.c
@@ -157,7 +157,7 @@ SendTxCommandPacket(
 		seg_ptr = skb_put(skb, buffer_len);
 		/*
 		 * Transform from little endian to big endian
-		 * and pending  zero
+		 * and pending zero
 		 */
 		memcpy(seg_ptr,codevirtualaddress,buffer_len);
 		tcb_desc->txbuf_size= (u16)buffer_len;
@@ -697,7 +697,6 @@ cmpk_message_handle_rx(
 	struct ieee80211_rx_stats *pstats)
 {
 //	u32			debug_level = DBG_LOUD;
-	struct r8192_priv *priv = ieee80211_priv(dev);
 	int			total_length;
 	u8			cmd_length, exe_cnt = 0;
 	u8			element_id;
@@ -719,15 +718,15 @@ cmpk_message_handle_rx(
 	/* 2. Read virtual address from RFD. */
 	pcmd_buff = pstats->virtual_address;
 
-	/* 3. Read command pakcet element id and length. */
+	/* 3. Read command packet element id and length. */
 	element_id = pcmd_buff[0];
 	/*RT_TRACE(COMP_SEND, DebugLevel,
 			("\n\r[CMPK]-->element ID=%d Len=%d", element_id, total_length));*/
 
-	/* 4. Check every received command packet conent according to different
+	/* 4. Check every received command packet content according to different
 	      element type. Because FW may aggregate RX command packet to minimize
 	      transmit time between DRV and FW.*/
-	// Add a counter to prevent to locked in the loop too long
+	// Add a counter to prevent the lock in the loop from being held too long
 	while (total_length > 0 || exe_cnt++ >100)
 	{
 		/* 2007/01/17 MH We support aggregation of different cmd in the same packet. */
@@ -779,9 +778,6 @@ cmpk_message_handle_rx(
 		// 2007/01/22 MH Add to display tx statistic.
 		//cmpk_DisplayTxStatistic(pAdapter);
 
-		/* 2007/03/09 MH Collect sidderent cmd element pkt num. */
-		priv->stats.rxcmdpkt[element_id]++;
-
 		total_length -= cmd_length;
 		pcmd_buff    += cmd_length;
 	}	/* while (total_length > 0) */
diff --git a/drivers/staging/rtl8192u/r819xU_firmware.c b/drivers/staging/rtl8192u/r819xU_firmware.c
index 4bb5fffca5b..b12d1907979 100644
--- a/drivers/staging/rtl8192u/r819xU_firmware.c
+++ b/drivers/staging/rtl8192u/r819xU_firmware.c
@@ -275,11 +275,11 @@ bool init_firmware(struct net_device *dev)
 
 	/*
 	 * Download boot, main, and data image for System reset.
-	 * Download data image for firmware reseta
+	 * Download data image for firmware reset
 	 */
 	for(init_step = starting_state; init_step <= FW_INIT_STEP2_DATA; init_step++) {
 		/*
-		 * Open Image file, and map file to contineous memory if open file success.
+		 * Open image file, and map file to continuous memory if open file success.
 		 * or read image file from array. Default load from IMG file
 		 */
 		if(rst_opt == OPT_SYSTEM_RESET) {
diff --git a/drivers/staging/rtl8192u/r819xU_phy.c b/drivers/staging/rtl8192u/r819xU_phy.c
index c4586b0817d..dd1954daea2 100644
--- a/drivers/staging/rtl8192u/r819xU_phy.c
+++ b/drivers/staging/rtl8192u/r819xU_phy.c
@@ -40,7 +40,7 @@ static u32 RF_CHANNEL_TABLE_ZEBRA[] = {
  *	     and do register read/write
  *   input:  u32	dwBitMask  //taget bit pos in the addr to be modified
  *  output:  none
- *  return:  u32	return the shift bit bit position of the mask
+ *  return:  u32	return the shift bit position of the mask
  * ****************************************************************************/
 u32 rtl8192_CalculateBitShift(u32 dwBitMask)
 {
@@ -176,7 +176,7 @@ u32 rtl8192_phy_RFSerialRead(struct net_device* dev, RF90_RADIO_PATH_E eRFPath,
 	rtl8192_setBBreg(dev, pPhyReg->rfHSSIPara2,  bLSSIReadEdge, 0x1);
 
 
-	// TODO: we should not delay such a  long time. Ask help from SD3
+	// TODO: we should not delay such a long time. Ask for help from SD3
 	msleep(1);
 
 	ret = rtl8192_QueryBBReg(dev, pPhyReg->rfLSSIReadBack, bLSSIReadBackData);
@@ -252,7 +252,7 @@ void rtl8192_phy_RFSerialWrite(struct net_device* dev, RF90_RADIO_PATH_E eRFPath
 		NewOffset = Offset;
 	}
 
-	// Put write addr in [5:0]  and write data in [31:16]
+	// Put write addr in [5:0] and write data in [31:16]
 	DataAndAddr = (Data<<16) | (NewOffset&0x3f);
 
 	// Write Operation
@@ -525,7 +525,7 @@ void rtl8192_phy_configmac(struct net_device* dev)
 }
 
 /******************************************************************************
- *function:  This function do dirty work
+ *function:  This function does dirty work
  *   input:  dev
  *  output:  none
  *  return:  none
@@ -578,7 +578,7 @@ void rtl8192_phyConfigBB(struct net_device* dev, u8 ConfigType)
 void rtl8192_InitBBRFRegDef(struct net_device* dev)
 {
 	struct r8192_priv *priv = ieee80211_priv(dev);
-// RF Interface Sowrtware Control
+// RF Interface Software Control
 	priv->PHYRegDef[RF90_PATH_A].rfintfs = rFPGA0_XAB_RFInterfaceSW; // 16 LSBs if read 32-bit from 0x870
 	priv->PHYRegDef[RF90_PATH_B].rfintfs = rFPGA0_XAB_RFInterfaceSW; // 16 MSBs if read 32-bit from 0x870 (16-bit for 0x872)
 	priv->PHYRegDef[RF90_PATH_C].rfintfs = rFPGA0_XCD_RFInterfaceSW;// 16 LSBs if read 32-bit from 0x874
@@ -602,7 +602,7 @@ void rtl8192_InitBBRFRegDef(struct net_device* dev)
 	priv->PHYRegDef[RF90_PATH_C].rfintfe = rFPGA0_XC_RFInterfaceOE;// 16 MSBs if read 32-bit from 0x86A (16-bit for 0x86A)
 	priv->PHYRegDef[RF90_PATH_D].rfintfe = rFPGA0_XD_RFInterfaceOE;// 16 MSBs if read 32-bit from 0x86C (16-bit for 0x86E)
 
-	//Addr of LSSI. Wirte RF register by driver
+	//Addr of LSSI. Write RF register by driver
 	priv->PHYRegDef[RF90_PATH_A].rf3wireOffset = rFPGA0_XA_LSSIParameter; //LSSI Parameter
 	priv->PHYRegDef[RF90_PATH_B].rf3wireOffset = rFPGA0_XB_LSSIParameter;
 	priv->PHYRegDef[RF90_PATH_C].rf3wireOffset = rFPGA0_XC_LSSIParameter;
@@ -1384,7 +1384,7 @@ u8 rtl8192_phy_SwChnlStepByStep(struct net_device *dev, u8 channel, u8* stage, u
 }
 
 /******************************************************************************
- *function:  This function does acturally set channel work
+ *function:  This function does actually set channel work
  *   input:  struct net_device *dev
  *   	     u8 		channel
  *  output:  none
@@ -1425,7 +1425,7 @@ void rtl8192_SwChnl_WorkItem(struct net_device *dev)
 }
 
 /******************************************************************************
- *function:  This function scheduled actural workitem to set channel
+ *function:  This function scheduled actual work item to set channel
  *   input:  net_device dev
  *   	     u8		channel //channel to set
  *  output:  none
diff --git a/drivers/staging/rtl8192u/r819xU_phyreg.h b/drivers/staging/rtl8192u/r819xU_phyreg.h
index 06b0b539e1b..50f24dce8b1 100644
--- a/drivers/staging/rtl8192u/r819xU_phyreg.h
+++ b/drivers/staging/rtl8192u/r819xU_phyreg.h
@@ -443,7 +443,7 @@
 #define bCCKRxIG                  			0x7f00
 #define bCCKLNAPolarity           		0x800000
 #define bCCKRx1stGain             		0x7f0000
-#define bCCKRFExtend              		0x20000000 //CCK Rx Iinital gain polarity
+#define bCCKRFExtend              		0x20000000 //CCK Rx inital gain polarity
 #define bCCKRxAGCSatLevel        		0x1f000000
 #define bCCKRxAGCSatCount       		0xe0
 #define bCCKRxRFSettle            		0x1f       //AGCsamp_dly
diff --git a/drivers/staging/rtl8712/big_endian.h b/drivers/staging/rtl8712/big_endian.h
deleted file mode 100644
index b16f8ecf99c..00000000000
--- a/drivers/staging/rtl8712/big_endian.h
+++ /dev/null
@@ -1,94 +0,0 @@
-/******************************************************************************
- *
- * Copyright(c) 2007 - 2010 Realtek 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
- *
- * Modifications for inclusion into the Linux staging tree are
- * Copyright(c) 2010 Larry Finger. All rights reserved.
- *
- * Contact information:
- * WLAN FAE <wlanfae@realtek.com>
- * Larry Finger <Larry.Finger@lwfinger.net>
- *
- ******************************************************************************/
-#ifndef _LINUX_BYTEORDER_BIG_ENDIAN_H
-#define _LINUX_BYTEORDER_BIG_ENDIAN_H
-
-#ifndef __BIG_ENDIAN
-#define __BIG_ENDIAN 4321
-#endif
-#ifndef __BIG_ENDIAN_BITFIELD
-#define __BIG_ENDIAN_BITFIELD
-#endif
-
-#include "swab.h"
-
-#define __constant_htonl(x) ((__u32)(x))
-#define __constant_ntohl(x) ((__u32)(x))
-#define __constant_htons(x) ((__u16)(x))
-#define __constant_ntohs(x) ((__u16)(x))
-#define __constant_cpu_to_le64(x) ___constant_swab64((x))
-#define __constant_le64_to_cpu(x) ___constant_swab64((x))
-#define __constant_cpu_to_le32(x) ___constant_swab32((x))
-#define __constant_le32_to_cpu(x) ___constant_swab32((x))
-#define __constant_cpu_to_le16(x) ___constant_swab16((x))
-#define __constant_le16_to_cpu(x) ___constant_swab16((x))
-#define __constant_cpu_to_be64(x) ((__u64)(x))
-#define __constant_be64_to_cpu(x) ((__u64)(x))
-#define __constant_cpu_to_be32(x) ((__u32)(x))
-#define __constant_be32_to_cpu(x) ((__u32)(x))
-#define __constant_cpu_to_be16(x) ((__u16)(x))
-#define __constant_be16_to_cpu(x) ((__u16)(x))
-#define __cpu_to_le64(x) __swab64((x))
-#define __le64_to_cpu(x) __swab64((x))
-#define __cpu_to_le32(x) __swab32((x))
-#define __le32_to_cpu(x) __swab32((x))
-#define __cpu_to_le16(x) __swab16((x))
-#define __le16_to_cpu(x) __swab16((x))
-#define __cpu_to_be64(x) ((__u64)(x))
-#define __be64_to_cpu(x) ((__u64)(x))
-#define __cpu_to_be32(x) ((__u32)(x))
-#define __be32_to_cpu(x) ((__u32)(x))
-#define __cpu_to_be16(x) ((__u16)(x))
-#define __be16_to_cpu(x) ((__u16)(x))
-#define __cpu_to_le64p(x) __swab64p((x))
-#define __le64_to_cpup(x) __swab64p((x))
-#define __cpu_to_le32p(x) __swab32p((x))
-#define __le32_to_cpup(x) __swab32p((x))
-#define __cpu_to_le16p(x) __swab16p((x))
-#define __le16_to_cpup(x) __swab16p((x))
-#define __cpu_to_be64p(x) (*(__u64 *)(x))
-#define __be64_to_cpup(x) (*(__u64 *)(x))
-#define __cpu_to_be32p(x) (*(__u32 *)(x))
-#define __be32_to_cpup(x) (*(__u32 *)(x))
-#define __cpu_to_be16p(x) (*(__u16 *)(x))
-#define __be16_to_cpup(x) (*(__u16 *)(x))
-#define __cpu_to_le64s(x) __swab64s((x))
-#define __le64_to_cpus(x) __swab64s((x))
-#define __cpu_to_le32s(x) __swab32s((x))
-#define __le32_to_cpus(x) __swab32s((x))
-#define __cpu_to_le16s(x) __swab16s((x))
-#define __le16_to_cpus(x) __swab16s((x))
-#define __cpu_to_be64s(x) do {} while (0)
-#define __be64_to_cpus(x) do {} while (0)
-#define __cpu_to_be32s(x) do {} while (0)
-#define __be32_to_cpus(x) do {} while (0)
-#define __cpu_to_be16s(x) do {} while (0)
-#define __be16_to_cpus(x) do {} while (0)
-
-#include "generic.h"
-
-#endif /* _LINUX_BYTEORDER_BIG_ENDIAN_H */
-
diff --git a/drivers/staging/rtl8712/drv_types.h b/drivers/staging/rtl8712/drv_types.h
index e83665d0602..62b55663c63 100644
--- a/drivers/staging/rtl8712/drv_types.h
+++ b/drivers/staging/rtl8712/drv_types.h
@@ -146,7 +146,7 @@ struct dvobj_priv {
 /**
  * struct _adapter - the main adapter structure for this device.
  *
- * bup: True indicates that the interface is Up.
+ * bup: True indicates that the interface is up.
  */
 struct _adapter {
 	struct	dvobj_priv dvobjpriv;
diff --git a/drivers/staging/rtl8712/generic.h b/drivers/staging/rtl8712/generic.h
deleted file mode 100644
index 8868c9f4adf..00000000000
--- a/drivers/staging/rtl8712/generic.h
+++ /dev/null
@@ -1,178 +0,0 @@
-/******************************************************************************
- *
- * Copyright(c) 2007 - 2010 Realtek 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
- *
- * Modifications for inclusion into the Linux staging tree are
- * Copyright(c) 2010 Larry Finger. All rights reserved.
- *
- * Contact information:
- * WLAN FAE <wlanfae@realtek.com>
- * Larry Finger <Larry.Finger@lwfinger.net>
- *
- ******************************************************************************/
-#ifndef _LINUX_BYTEORDER_GENERIC_H
-#define _LINUX_BYTEORDER_GENERIC_H
-
-/*
- * linux/byteorder_generic.h
- * Generic Byte-reordering support
- *
- * Francois-Rene Rideau <fare@tunes.org> 19970707
- *    gathered all the good ideas from all asm-foo/byteorder.h into one file,
- *    cleaned them up.
- *    I hope it is compliant with non-GCC compilers.
- *    I decided to put __BYTEORDER_HAS_U64__ in byteorder.h,
- *    because I wasn't sure it would be ok to put it in types.h
- *    Upgraded it to 2.1.43
- * Francois-Rene Rideau <fare@tunes.org> 19971012
- *    Upgraded it to 2.1.57
- *    to please Linus T., replaced huge #ifdef's between little/big endian
- *    by nestedly #include'd files.
- * Francois-Rene Rideau <fare@tunes.org> 19971205
- *    Made it to 2.1.71; now a facelift:
- *    Put files under include/linux/byteorder/
- *    Split swab from generic support.
- *
- * TODO:
- *   = Regular kernel maintainers could also replace all these manual
- *    byteswap macros that remain, disseminated among drivers,
- *    after some grep or the sources...
- *   = Linus might want to rename all these macros and files to fit his taste,
- *    to fit his personal naming scheme.
- *   = it seems that a few drivers would also appreciate
- *    nybble swapping support...
- *   = every architecture could add their byteswap macro in asm/byteorder.h
- *    see how some architectures already do (i386, alpha, ppc, etc)
- *   = cpu_to_beXX and beXX_to_cpu might some day need to be well
- *    distinguished throughout the kernel. This is not the case currently,
- *    since little endian, big endian, and pdp endian machines needn't it.
- *    But this might be the case for, say, a port of Linux to 20/21 bit
- *    architectures (and F21 Linux addict around?).
- */
-
-/*
- * The following macros are to be defined by <asm/byteorder.h>:
- *
- * Conversion of long and short int between network and host format
- *	ntohl(__u32 x)
- *	ntohs(__u16 x)
- *	htonl(__u32 x)
- *	htons(__u16 x)
- * It seems that some programs (which? where? or perhaps a standard? POSIX?)
- * might like the above to be functions, not macros (why?).
- * if that's true, then detect them, and take measures.
- * Anyway, the measure is: define only ___ntohl as a macro instead,
- * and in a separate file, have
- * unsigned long inline ntohl(x){return ___ntohl(x);}
- *
- * The same for constant arguments
- *	__constant_ntohl(__u32 x)
- *	__constant_ntohs(__u16 x)
- *	__constant_htonl(__u32 x)
- *	__constant_htons(__u16 x)
- *
- * Conversion of XX-bit integers (16- 32- or 64-)
- * between native CPU format and little/big endian format
- * 64-bit stuff only defined for proper architectures
- *	cpu_to_[bl]eXX(__uXX x)
- *	[bl]eXX_to_cpu(__uXX x)
- *
- * The same, but takes a pointer to the value to convert
- *	cpu_to_[bl]eXXp(__uXX x)
- *	[bl]eXX_to_cpup(__uXX x)
- *
- * The same, but change in situ
- *	cpu_to_[bl]eXXs(__uXX x)
- *	[bl]eXX_to_cpus(__uXX x)
- *
- * See asm-foo/byteorder.h for examples of how to provide
- * architecture-optimized versions
- *
- */
-
-
-/*
- * inside the kernel, we can use nicknames;
- * outside of it, we must avoid POSIX namespace pollution...
- */
-#define cpu_to_le64 __cpu_to_le64
-#define le64_to_cpu __le64_to_cpu
-#define cpu_to_le32 __cpu_to_le32
-#define le32_to_cpu __le32_to_cpu
-#define cpu_to_le16 __cpu_to_le16
-#define le16_to_cpu __le16_to_cpu
-#define cpu_to_be64 __cpu_to_be64
-#define be64_to_cpu __be64_to_cpu
-#define cpu_to_be32 __cpu_to_be32
-#define be32_to_cpu __be32_to_cpu
-#define cpu_to_be16 __cpu_to_be16
-#define be16_to_cpu __be16_to_cpu
-#define cpu_to_le64p __cpu_to_le64p
-#define le64_to_cpup __le64_to_cpup
-#define cpu_to_le32p __cpu_to_le32p
-#define le32_to_cpup __le32_to_cpup
-#define cpu_to_le16p __cpu_to_le16p
-#define le16_to_cpup __le16_to_cpup
-#define cpu_to_be64p __cpu_to_be64p
-#define be64_to_cpup __be64_to_cpup
-#define cpu_to_be32p __cpu_to_be32p
-#define be32_to_cpup __be32_to_cpup
-#define cpu_to_be16p __cpu_to_be16p
-#define be16_to_cpup __be16_to_cpup
-#define cpu_to_le64s __cpu_to_le64s
-#define le64_to_cpus __le64_to_cpus
-#define cpu_to_le32s __cpu_to_le32s
-#define le32_to_cpus __le32_to_cpus
-#define cpu_to_le16s __cpu_to_le16s
-#define le16_to_cpus __le16_to_cpus
-#define cpu_to_be64s __cpu_to_be64s
-#define be64_to_cpus __be64_to_cpus
-#define cpu_to_be32s __cpu_to_be32s
-#define be32_to_cpus __be32_to_cpus
-#define cpu_to_be16s __cpu_to_be16s
-#define be16_to_cpus __be16_to_cpus
-
-
-/*
- * Handle ntohl and suches. These have various compatibility
- * issues - like we want to give the prototype even though we
- * also have a macro for them in case some strange program
- * wants to take the address of the thing or something..
- *
- * Note that these used to return a "long" in libc5, even though
- * long is often 64-bit these days.. Thus the casts.
- *
- * They have to be macros in order to do the constant folding
- * correctly - if the argument passed into a inline function
- * it is no longer constant according to gcc..
- */
-
-#undef ntohl
-#undef ntohs
-#undef htonl
-#undef htons
-
-/*
- * Do the prototypes. Somebody might want to take the
- * address or some such sick thing..
- */
-extern __u32			ntohl(__u32);
-extern __u32			htonl(__u32);
-extern unsigned short int	ntohs(unsigned short int);
-extern unsigned short int	htons(unsigned short int);
-
-#endif /* _LINUX_BYTEORDER_GENERIC_H */
-
diff --git a/drivers/staging/rtl8712/hal_init.c b/drivers/staging/rtl8712/hal_init.c
index cc893c0f5ad..cb9d4cfe8fe 100644
--- a/drivers/staging/rtl8712/hal_init.c
+++ b/drivers/staging/rtl8712/hal_init.c
@@ -36,7 +36,6 @@
 
 #include "osdep_service.h"
 #include "drv_types.h"
-#include "rtl871x_byteorder.h"
 #include "usb_osintf.h"
 
 #define FWBUFF_ALIGN_SZ 512
diff --git a/drivers/staging/rtl8712/ieee80211.h b/drivers/staging/rtl8712/ieee80211.h
index 3c0092b7de0..21515c31b37 100644
--- a/drivers/staging/rtl8712/ieee80211.h
+++ b/drivers/staging/rtl8712/ieee80211.h
@@ -705,7 +705,7 @@ enum ieee80211_state {
 	IEEE80211_ASSOCIATING_RETRY,
 	/* the association procedure is sending AUTH request*/
 	IEEE80211_ASSOCIATING_AUTHENTICATING,
-	/* the association procedure has successfully authentcated
+	/* the association procedure has successfully authenticated
 	 * and is sending association request
 	 */
 	IEEE80211_ASSOCIATING_AUTHENTICATED,
diff --git a/drivers/staging/rtl8712/if_ether.h b/drivers/staging/rtl8712/if_ether.h
deleted file mode 100644
index 2bbe527bcd5..00000000000
--- a/drivers/staging/rtl8712/if_ether.h
+++ /dev/null
@@ -1,141 +0,0 @@
-/******************************************************************************
- *
- * Copyright(c) 2007 - 2010 Realtek 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
- *
- * Modifications for inclusion into the Linux staging tree are
- * Copyright(c) 2010 Larry Finger. All rights reserved.
- *
- * Contact information:
- * WLAN FAE <wlanfae@realtek.com>
- * Larry Finger <Larry.Finger@lwfinger.net>
- *
- ******************************************************************************/
-/*
- * INET		An implementation of the TCP/IP protocol suite for the LINUX
- *		operating system.  INET is implemented using the  BSD Socket
- *		interface as the means of communication with the user level.
- *
- *		Global definitions for the Ethernet IEEE 802.3 interface.
- *
- * Version:	@(#)if_ether.h	1.0.1a	02/08/94
- *
- * Author:	Fred N. van Kempen, <waltje@uWalt.NL.Mugnet.ORG>
- *		Donald Becker, <becker@super.org>
- *		Alan Cox, <alan@redhat.com>
- *		Steve Whitehouse, <gw7rrm@eeshack3.swan.ac.uk>
- *
- *		This program is free software; you can redistribute it and/or
- *		modify it under the terms of the GNU General Public License
- *		as published by the Free Software Foundation; either version
- *		2 of the License, or (at your option) any later version.
- */
-
-#ifndef _LINUX_IF_ETHER_H
-#define _LINUX_IF_ETHER_H
-
-/*
- *	IEEE 802.3 Ethernet magic constants.  The frame sizes omit the preamble
- *	and FCS/CRC (frame check sequence).
- */
-
-#define ETH_ALEN	6		/* Octets in one ethernet addr	 */
-#define ETH_HLEN	14		/* Total octets in header.	 */
-#define ETH_ZLEN	60		/* Min. octets in frame sans FCS */
-#define ETH_DATA_LEN	1500		/* Max. octets in payload	 */
-#define ETH_FRAME_LEN	1514		/* Max. octets in frame sans FCS */
-
-/*
- *	These are the defined Ethernet Protocol ID's.
- */
-
-#define ETH_P_LOOP	0x0060		/* Ethernet Loopback packet	*/
-#define ETH_P_PUP	0x0200		/* Xerox PUP packet		*/
-#define ETH_P_PUPAT	0x0201		/* Xerox PUP Addr Trans packet	*/
-#define ETH_P_IP	0x0800		/* Internet Protocol packet	*/
-#define ETH_P_X25	0x0805		/* CCITT X.25			*/
-#define ETH_P_ARP	0x0806		/* Address Resolution packet	*/
-#define	ETH_P_BPQ	0x08FF		/* G8BPQ AX.25 Ethernet Packet
-					 * [ NOT AN OFFICIAL ID ] */
-#define ETH_P_IEEEPUP	0x0a00		/* Xerox IEEE802.3 PUP packet */
-#define ETH_P_IEEEPUPAT	0x0a01		/* Xerox IEEE802.3 PUP Addr
-					 * Trans packet */
-#define ETH_P_DEC       0x6000          /* DEC Assigned proto           */
-#define ETH_P_DNA_DL    0x6001          /* DEC DNA Dump/Load            */
-#define ETH_P_DNA_RC    0x6002          /* DEC DNA Remote Console       */
-#define ETH_P_DNA_RT    0x6003          /* DEC DNA Routing              */
-#define ETH_P_LAT       0x6004          /* DEC LAT                      */
-#define ETH_P_DIAG      0x6005          /* DEC Diagnostics              */
-#define ETH_P_CUST      0x6006          /* DEC Customer use             */
-#define ETH_P_SCA       0x6007          /* DEC Systems Comms Arch       */
-#define ETH_P_RARP      0x8035		/* Reverse Addr Res packet	*/
-#define ETH_P_ATALK	0x809B		/* Appletalk DDP		*/
-#define ETH_P_AARP	0x80F3		/* Appletalk AARP		*/
-#define ETH_P_8021Q	0x8100          /* 802.1Q VLAN Extended Header  */
-#define ETH_P_IPX	0x8137		/* IPX over DIX			*/
-#define ETH_P_IPV6	0x86DD		/* IPv6 over bluebook		*/
-#define ETH_P_PPP_DISC	0x8863		/* PPPoE discovery messages     */
-#define ETH_P_PPP_SES	0x8864		/* PPPoE session messages	*/
-#define ETH_P_ATMMPOA	0x884c		/* MultiProtocol Over ATM	*/
-#define ETH_P_ATMFATE	0x8884		/* Frame-based ATM Transport
-					 * over Ethernet
-					 */
-
-/*
- *	Non DIX types. Won't clash for 1500 types.
- */
-
-#define ETH_P_802_3	0x0001		/* Dummy type for 802.3 frames  */
-#define ETH_P_AX25	0x0002		/* Dummy protocol id for AX.25  */
-#define ETH_P_ALL	0x0003		/* Every packet (be careful!!!) */
-#define ETH_P_802_2	0x0004		/* 802.2 frames			*/
-#define ETH_P_SNAP	0x0005		/* Internal only		*/
-#define ETH_P_DDCMP     0x0006          /* DEC DDCMP: Internal only     */
-#define ETH_P_WAN_PPP   0x0007          /* Dummy type for WAN PPP frames*/
-#define ETH_P_PPP_MP    0x0008          /* Dummy type for PPP MP frames */
-#define ETH_P_LOCALTALK 0x0009		/* Localtalk pseudo type	*/
-#define ETH_P_PPPTALK	0x0010		/* Dummy type for Atalk over PPP*/
-#define ETH_P_TR_802_2	0x0011i		/* 802.2 frames			*/
-#define ETH_P_MOBITEX	0x0015		/* Mobitex (kaz@cafe.net)	*/
-#define ETH_P_CONTROL	0x0016		/* Card specific control frames */
-#define ETH_P_IRDA	0x0017		/* Linux-IrDA			*/
-#define ETH_P_ECONET	0x0018		/* Acorn Econet			*/
-
-/*
- *	This is an Ethernet frame header.
- */
-
-struct ethhdr {
-	unsigned char	h_dest[ETH_ALEN];	/* destination eth addr	*/
-	unsigned char	h_source[ETH_ALEN];	/* source ether addr	*/
-	unsigned short	h_proto;		/* packet type ID field	*/
-};
-
-struct _vlan {
-	unsigned short  h_vlan_TCI;	/* Encapsulates priority and VLAN ID*/
-	unsigned short  h_vlan_encapsulated_proto;
-};
-
-
-
-#define get_vlan_id(pvlan) ((ntohs((unsigned short)pvlan->h_vlan_TCI)) & 0xfff)
-#define get_vlan_priority(pvlan) ((ntohs((unsigned short)\
-				 pvlan->h_vlan_TCI)) >> 13)
-#define get_vlan_encap_proto(pvlan) (ntohs((unsigned short)\
-				    pvlan->h_vlan_encapsulated_proto))
-
-
-#endif	/* _LINUX_IF_ETHER_H */
-
diff --git a/drivers/staging/rtl8712/ip.h b/drivers/staging/rtl8712/ip.h
deleted file mode 100644
index f37b0f8d14d..00000000000
--- a/drivers/staging/rtl8712/ip.h
+++ /dev/null
@@ -1,137 +0,0 @@
-/*
- * INET		An implementation of the TCP/IP protocol suite for the LINUX
- *		operating system.  INET is implemented using the  BSD Socket
- *		interface as the means of communication with the user level.
- *
- *		Definitions for the IP protocol.
- *
- * Version:	@(#)ip.h	1.0.2	04/28/93
- *
- * Authors:	Fred N. van Kempen, <waltje@uWalt.NL.Mugnet.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.
- */
-#ifndef _LINUX_IP_H
-#define _LINUX_IP_H
-
-#include "rtl871x_byteorder.h"
-
-/* SOL_IP socket options */
-
-#define IPTOS_TOS_MASK		0x1E
-#define IPTOS_TOS(tos)		((tos)&IPTOS_TOS_MASK)
-#define	IPTOS_LOWDELAY		0x10
-#define	IPTOS_THROUGHPUT	0x08
-#define	IPTOS_RELIABILITY	0x04
-#define	IPTOS_MINCOST		0x02
-
-#define IPTOS_PREC_MASK		0xE0
-#define IPTOS_PREC(tos)		((tos)&IPTOS_PREC_MASK)
-#define IPTOS_PREC_NETCONTROL           0xe0
-#define IPTOS_PREC_INTERNETCONTROL      0xc0
-#define IPTOS_PREC_CRITIC_ECP           0xa0
-#define IPTOS_PREC_FLASHOVERRIDE        0x80
-#define IPTOS_PREC_FLASH                0x60
-#define IPTOS_PREC_IMMEDIATE            0x40
-#define IPTOS_PREC_PRIORITY             0x20
-#define IPTOS_PREC_ROUTINE              0x00
-
-/* IP options */
-#define IPOPT_COPY		0x80
-#define IPOPT_CLASS_MASK	0x60
-#define IPOPT_NUMBER_MASK	0x1f
-
-#define	IPOPT_COPIED(o)		((o)&IPOPT_COPY)
-#define	IPOPT_CLASS(o)		((o)&IPOPT_CLASS_MASK)
-#define	IPOPT_NUMBER(o)		((o)&IPOPT_NUMBER_MASK)
-
-#define	IPOPT_CONTROL		0x00
-#define	IPOPT_RESERVED1		0x20
-#define	IPOPT_MEASUREMENT	0x40
-#define	IPOPT_RESERVED2		0x60
-
-#define IPOPT_END	(0 | IPOPT_CONTROL)
-#define IPOPT_NOOP	(1 | IPOPT_CONTROL)
-#define IPOPT_SEC	(2 | IPOPT_CONTROL|IPOPT_COPY)
-#define IPOPT_LSRR	(3 | IPOPT_CONTROL|IPOPT_COPY)
-#define IPOPT_TIMESTAMP	(4 | IPOPT_MEASUREMENT)
-#define IPOPT_RR	(7 | IPOPT_CONTROL)
-#define IPOPT_SID	(8 | IPOPT_CONTROL | IPOPT_COPY)
-#define IPOPT_SSRR	(9 | IPOPT_CONTROL | IPOPT_COPY)
-#define IPOPT_RA	(20 | IPOPT_CONTROL | IPOPT_COPY)
-
-#define IPVERSION	4
-#define MAXTTL		255
-#define IPDEFTTL	64
-
-/* struct timestamp, struct route and MAX_ROUTES are removed.
- *
- * REASONS: it is clear that nobody used them because:
- * - MAX_ROUTES value was wrong.
- * - "struct route" was wrong.
- * - "struct timestamp" had fatally misaligned bitfields and was completely
- *   unusable.
- */
-
-#define IPOPT_OPTVAL 0
-#define IPOPT_OLEN   1
-#define IPOPT_OFFSET 2
-#define IPOPT_MINOFF 4
-#define MAX_IPOPTLEN 40
-#define IPOPT_NOP IPOPT_NOOP
-#define IPOPT_EOL IPOPT_END
-#define IPOPT_TS  IPOPT_TIMESTAMP
-
-#define	IPOPT_TS_TSONLY		0		/* timestamps only */
-#define	IPOPT_TS_TSANDADDR	1		/* timestamps and addresses */
-#define	IPOPT_TS_PRESPEC	3		/* specified modules only */
-
-struct ip_options {
-	__u32		faddr;			/* Saved first hop address */
-	unsigned char	optlen;
-	unsigned char srr;
-	unsigned char rr;
-	unsigned char ts;
-	unsigned char is_setbyuser:1,	/* Set by setsockopt?		      */
-		      is_data:1,	/* Options in __data, rather than skb */
-		      is_strictroute:1, /* Strict source route		      */
-		      srr_is_hit:1,	/* Packet destination addr was our one*/
-		      is_changed:1,	/* IP checksum more not valid	      */
-		      rr_needaddr:1,	/* Need to record addr of outgoing dev*/
-		      ts_needtime:1,	/* Need to record timestamp	      */
-		      ts_needaddr:1;	/* Need to record addr of outgoing dev*/
-	unsigned char router_alert;
-	unsigned char __pad1;
-	unsigned char __pad2;
-	unsigned char __data[0];
-};
-
-#define optlength(opt) (sizeof(struct ip_options) + opt->optlen)
-
-struct iphdr {
-#if defined(__LITTLE_ENDIAN_BITFIELD)
-	__u8	ihl:4,
-		version:4;
-#elif defined(__BIG_ENDIAN_BITFIELD)
-	__u8	version:4,
-		ihl:4;
-#else
-#error	"Please fix <asm/byteorder.h>"
-#endif
-	__u8	tos;
-	__u16	tot_len;
-	__u16	id;
-	__u16	frag_off;
-	__u8	ttl;
-	__u8	protocol;
-	__u16	check;
-	__u32	saddr;
-	__u32	daddr;
-	/*The options start here. */
-};
-
-#endif	/* _LINUX_IP_H */
-
diff --git a/drivers/staging/rtl8712/little_endian.h b/drivers/staging/rtl8712/little_endian.h
deleted file mode 100644
index cd57d6c2850..00000000000
--- a/drivers/staging/rtl8712/little_endian.h
+++ /dev/null
@@ -1,94 +0,0 @@
-/******************************************************************************
- *
- * Copyright(c) 2007 - 2010 Realtek 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
- *
- * Modifications for inclusion into the Linux staging tree are
- * Copyright(c) 2010 Larry Finger. All rights reserved.
- *
- * Contact information:
- * WLAN FAE <wlanfae@realtek.com>
- * Larry Finger <Larry.Finger@lwfinger.net>
- *
- ******************************************************************************/
-#ifndef _LINUX_BYTEORDER_LITTLE_ENDIAN_H
-#define _LINUX_BYTEORDER_LITTLE_ENDIAN_H
-
-#ifndef __LITTLE_ENDIAN
-#define __LITTLE_ENDIAN 1234
-#endif
-#ifndef __LITTLE_ENDIAN_BITFIELD
-#define __LITTLE_ENDIAN_BITFIELD
-#endif
-
-#include "swab.h"
-
-#define __constant_htonl(x) ___constant_swab32((x))
-#define __constant_ntohl(x) ___constant_swab32((x))
-#define __constant_htons(x) ___constant_swab16((x))
-#define __constant_ntohs(x) ___constant_swab16((x))
-#define __constant_cpu_to_le64(x) ((__u64)(x))
-#define __constant_le64_to_cpu(x) ((__u64)(x))
-#define __constant_cpu_to_le32(x) ((__u32)(x))
-#define __constant_le32_to_cpu(x) ((__u32)(x))
-#define __constant_cpu_to_le16(x) ((__u16)(x))
-#define __constant_le16_to_cpu(x) ((__u16)(x))
-#define __constant_cpu_to_be64(x) ___constant_swab64((x))
-#define __constant_be64_to_cpu(x) ___constant_swab64((x))
-#define __constant_cpu_to_be32(x) ___constant_swab32((x))
-#define __constant_be32_to_cpu(x) ___constant_swab32((x))
-#define __constant_cpu_to_be16(x) ___constant_swab16((x))
-#define __constant_be16_to_cpu(x) ___constant_swab16((x))
-#define __cpu_to_le64(x) ((__u64)(x))
-#define __le64_to_cpu(x) ((__u64)(x))
-#define __cpu_to_le32(x) ((__u32)(x))
-#define __le32_to_cpu(x) ((__u32)(x))
-#define __cpu_to_le16(x) ((__u16)(x))
-#define __le16_to_cpu(x) ((__u16)(x))
-#define __cpu_to_be64(x) __swab64((x))
-#define __be64_to_cpu(x) __swab64((x))
-#define __cpu_to_be32(x) __swab32((x))
-#define __be32_to_cpu(x) __swab32((x))
-#define __cpu_to_be16(x) __swab16((x))
-#define __be16_to_cpu(x) __swab16((x))
-#define __cpu_to_le64p(x) (*(__u64 *)(x))
-#define __le64_to_cpup(x) (*(__u64 *)(x))
-#define __cpu_to_le32p(x) (*(__u32 *)(x))
-#define __le32_to_cpup(x) (*(__u32 *)(x))
-#define __cpu_to_le16p(x) (*(__u16 *)(x))
-#define __le16_to_cpup(x) (*(__u16 *)(x))
-#define __cpu_to_be64p(x) __swab64p((x))
-#define __be64_to_cpup(x) __swab64p((x))
-#define __cpu_to_be32p(x) __swab32p((x))
-#define __be32_to_cpup(x) __swab32p((x))
-#define __cpu_to_be16p(x) __swab16p((x))
-#define __be16_to_cpup(x) __swab16p((x))
-#define __cpu_to_le64s(x) do {} while (0)
-#define __le64_to_cpus(x) do {} while (0)
-#define __cpu_to_le32s(x) do {} while (0)
-#define __le32_to_cpus(x) do {} while (0)
-#define __cpu_to_le16s(x) do {} while (0)
-#define __le16_to_cpus(x) do {} while (0)
-#define __cpu_to_be64s(x) __swab64s((x))
-#define __be64_to_cpus(x) __swab64s((x))
-#define __cpu_to_be32s(x) __swab32s((x))
-#define __be32_to_cpus(x) __swab32s((x))
-#define __cpu_to_be16s(x) __swab16s((x))
-#define __be16_to_cpus(x) __swab16s((x))
-
-#include "generic.h"
-
-#endif /* _LINUX_BYTEORDER_LITTLE_ENDIAN_H */
-
diff --git a/drivers/staging/rtl8712/os_intfs.c b/drivers/staging/rtl8712/os_intfs.c
index 7bbd53a410e..448f00dd68f 100644
--- a/drivers/staging/rtl8712/os_intfs.c
+++ b/drivers/staging/rtl8712/os_intfs.c
@@ -52,7 +52,7 @@ static int lbkmode = RTL8712_AIR_TRX;
 static int hci = RTL8712_USB;
 static int ampdu_enable = 1;/*for enable tx_ampdu*/
 
-/* The video_mode variable is for vedio mode.*/
+/* The video_mode variable is for video mode.*/
 /* It may be specify when inserting module with video_mode=1 parameter.*/
 static int video_mode = 1;   /* enable video mode*/
 
@@ -248,7 +248,7 @@ static u32 start_drv_threads(struct _adapter *padapter)
 
 void r8712_stop_drv_threads(struct _adapter *padapter)
 {
-	/*Below is to termindate r8712_cmd_thread & event_thread...*/
+	/*Below is to terminate r8712_cmd_thread & event_thread...*/
 	up(&padapter->cmdpriv.cmd_queue_sema);
 	if (padapter->cmdThread)
 		_down_sema(&padapter->cmdpriv.terminate_cmdthread_sema);
diff --git a/drivers/staging/rtl8712/osdep_service.h b/drivers/staging/rtl8712/osdep_service.h
index 9ba603310fd..f1ccc7ebbda 100644
--- a/drivers/staging/rtl8712/osdep_service.h
+++ b/drivers/staging/rtl8712/osdep_service.h
@@ -29,7 +29,6 @@
 #define _SUCCESS	1
 #define _FAIL		0
 
-#include <linux/version.h>
 #include <linux/spinlock.h>
 
 #include <linux/interrupt.h>
@@ -107,8 +106,6 @@ static inline void _set_workitem(_workitem *pwork)
 	schedule_work(pwork);
 }
 
-#include "rtl871x_byteorder.h"
-
 #ifndef BIT
 	#define BIT(x)	(1 << (x))
 #endif
diff --git a/drivers/staging/rtl8712/rtl8712_cmd.c b/drivers/staging/rtl8712/rtl8712_cmd.c
index 9f6ebc419b0..088647cdca9 100644
--- a/drivers/staging/rtl8712/rtl8712_cmd.c
+++ b/drivers/staging/rtl8712/rtl8712_cmd.c
@@ -50,7 +50,6 @@
 #include "drv_types.h"
 #include "recv_osdep.h"
 #include "mlme_osdep.h"
-#include "rtl871x_byteorder.h"
 #include "rtl871x_ioctl_set.h"
 
 static void check_hw_pbc(struct _adapter *padapter)
@@ -69,7 +68,7 @@ static void check_hw_pbc(struct _adapter *padapter)
 		 * After trigger PBC, the variable will be set to false */
 		DBG_8712("CheckPbcGPIO - PBC is pressed !!!!\n");
 		/* 0 is the default value and it means the application monitors
-		 * the HW PBC doesn't privde its pid to driver. */
+		 * the HW PBC doesn't provide its pid to driver. */
 		if (padapter->pid == 0)
 			return;
 		kill_pid(find_vpid(padapter->pid), SIGUSR1, 1);
@@ -382,7 +381,7 @@ _next:
 			*pcmdbuf = cpu_to_le32((cmdsz & 0x0000ffff) |
 					       (pcmd->cmdcode << 16) |
 					       (pcmdpriv->cmd_seq << 24));
-			pcmdbuf += 2 ; /* 8 bytes aligment */
+			pcmdbuf += 2 ; /* 8 bytes alignment */
 			memcpy((u8 *)pcmdbuf, pcmd->parmbuf, pcmd->cmdsz);
 			while (check_cmd_fifo(padapter, wr_sz) == _FAIL) {
 				if ((padapter->bDriverStopped == true) ||
@@ -471,7 +470,7 @@ void r8712_event_handle(struct _adapter *padapter, uint *peventbuf)
 	pevt_priv->event_seq++;	/* update evt_seq */
 	if (pevt_priv->event_seq > 127)
 		pevt_priv->event_seq = 0;
-	peventbuf = peventbuf + 2; /* move to event content, 8 bytes aligment */
+	peventbuf = peventbuf + 2; /* move to event content, 8 bytes alignment */
 	if (peventbuf) {
 		event_callback = wlanevents[evt_code].event_callback;
 		if (event_callback)
diff --git a/drivers/staging/rtl8712/rtl8712_cmd.h b/drivers/staging/rtl8712/rtl8712_cmd.h
index 766a6463266..039ab3e9717 100644
--- a/drivers/staging/rtl8712/rtl8712_cmd.h
+++ b/drivers/staging/rtl8712/rtl8712_cmd.h
@@ -121,7 +121,7 @@ enum rtl8712_h2c_cmd {
 	GEN_CMD_CODE(_GetCurDataRate) ,
 
 	GEN_CMD_CODE(_GetTxRetrycnt),  /* to record times that Tx retry to
-					* transmmit packet after association
+					* transmit packet after association
 					*/
 	GEN_CMD_CODE(_GetRxRetrycnt),  /* to record total number of the
 					* received frame with ReTry bit set in
diff --git a/drivers/staging/rtl8712/rtl8712_efuse.c b/drivers/staging/rtl8712/rtl8712_efuse.c
index b08e9a25c9c..377fca90580 100644
--- a/drivers/staging/rtl8712/rtl8712_efuse.c
+++ b/drivers/staging/rtl8712/rtl8712_efuse.c
@@ -417,7 +417,7 @@ u8 r8712_efuse_pg_packet_write(struct _adapter *padapter, const u8 offset,
 		} else { /* write header fail */
 			bResult = false;
 			if (0xFF == efuse_data)
-				return bResult; /* not thing damaged. */
+				return bResult; /* nothing damaged. */
 			/* call rescue procedure */
 			if (fix_header(padapter, efuse_data, efuse_addr) ==
 			    false)
diff --git a/drivers/staging/rtl8712/rtl8712_gp_bitdef.h b/drivers/staging/rtl8712/rtl8712_gp_bitdef.h
index 884a8212176..138ea453d9d 100644
--- a/drivers/staging/rtl8712/rtl8712_gp_bitdef.h
+++ b/drivers/staging/rtl8712/rtl8712_gp_bitdef.h
@@ -70,7 +70,7 @@
 #define		GPIOSEL_BT	2	/* BT_coex*/
 #define		GPIOSEL_WLANDBG	3	/* WLANDBG*/
 #define		GPIOSEL_GPIO_MASK	(~(BIT(0)|BIT(1)))
-/* HW Readio OFF switch (GPIO BIT) */
+/* HW Radio OFF switch (GPIO BIT) */
 #define		HAL_8192S_HW_GPIO_OFF_BIT	BIT(3)
 #define		HAL_8192S_HW_GPIO_OFF_MASK	0xF7
 #define		HAL_8192S_HW_GPIO_WPS_BIT	BIT(4)
diff --git a/drivers/staging/rtl8712/rtl8712_hal.h b/drivers/staging/rtl8712/rtl8712_hal.h
index d19865a5a50..4c51fa373b5 100644
--- a/drivers/staging/rtl8712/rtl8712_hal.h
+++ b/drivers/staging/rtl8712/rtl8712_hal.h
@@ -83,7 +83,7 @@ struct fw_priv {   /*8-bytes alignment required*/
 	unsigned char rfintfs;    /* 0:SWSI, 1:HWSI, 2:HWPI*/
 	unsigned char def_nettype;
 	unsigned char turboMode;
-	unsigned char lowPowerMode;/* 0: noral mode, 1: low power mode*/
+	unsigned char lowPowerMode;/* 0: normal mode, 1: low power mode*/
 	/*--- long word 2 ----*/
 	unsigned char lbk_mode; /*0x00: normal, 0x03: MACLBK, 0x01: PHYLBK*/
 	unsigned char mp_mode; /* 1: for MP use, 0: for normal driver */
@@ -123,7 +123,7 @@ struct fw_priv {   /*8-bytes alignment required*/
 	unsigned char rsvd053;
 };
 
-struct fw_hdr {/*8-byte alinment required*/
+struct fw_hdr {/*8-byte alignment required*/
 	unsigned short	signature;
 	unsigned short	version;	/*0x8000 ~ 0x8FFF for FPGA version,
 					 *0x0000 ~ 0x7FFF for ASIC version,*/
diff --git a/drivers/staging/rtl8712/rtl8712_led.c b/drivers/staging/rtl8712/rtl8712_led.c
index bac56e5caf1..c9eb4b74799 100644
--- a/drivers/staging/rtl8712/rtl8712_led.c
+++ b/drivers/staging/rtl8712/rtl8712_led.c
@@ -60,7 +60,7 @@ enum _LED_STATE_871x {
 			     * the # of times to blink is depend on time
 			     * for scanning. */
 	LED_NO_LINK_BLINK = 7, /* LED is blinking during no link state. */
-	LED_BLINK_StartToBlink = 8,/* Customzied for Sercomm Printer
+	LED_BLINK_StartToBlink = 8,/* Customized for Sercomm Printer
 				    * Server case */
 	LED_BLINK_WPS = 9,	/* LED is blinkg during WPS communication */
 	LED_TXRX_BLINK = 10,
@@ -826,7 +826,7 @@ static void BlinkTimerCallback(unsigned long data)
 {
 	struct LED_871x  *pLed = (struct LED_871x *)data;
 
-	/* This fixed the crash problem on Fedora 12 when trying to do thei
+	/* This fixed the crash problem on Fedora 12 when trying to do the
 	 * insmod;ifconfig up;rmmod commands. */
 	if ((pLed->padapter->bSurpriseRemoved == true) ||
 	    (pLed->padapter->bDriverStopped == true))
@@ -836,7 +836,7 @@ static void BlinkTimerCallback(unsigned long data)
 
 /*	Description:
  *		Callback function of LED BlinkWorkItem.
- *		We dispatch acture LED blink action according to LedStrategy.
+ *		We dispatch actual LED blink action according to LedStrategy.
  */
 static void BlinkWorkItemCallback(struct work_struct *work)
 {
diff --git a/drivers/staging/rtl8712/rtl8712_recv.c b/drivers/staging/rtl8712/rtl8712_recv.c
index fa6dc9c09b3..8e82ce2fee3 100644
--- a/drivers/staging/rtl8712/rtl8712_recv.c
+++ b/drivers/staging/rtl8712/rtl8712_recv.c
@@ -28,12 +28,13 @@
 
 #define _RTL8712_RECV_C_
 
+#include <linux/if_ether.h>
+#include <linux/ip.h>
+
 #include "osdep_service.h"
 #include "drv_types.h"
 #include "recv_osdep.h"
 #include "mlme_osdep.h"
-#include "ip.h"
-#include "if_ether.h"
 #include "ethernet.h"
 #include "usb_ops.h"
 #include "wifi.h"
@@ -459,7 +460,7 @@ void r8712_rxcmd_event_hdl(struct _adapter *padapter, void *prxcmdbuf)
 		cmd_seq = (u8)((le32_to_cpu(voffset) >> 24) & 0x7f);
 		eid = (u8)((le32_to_cpu(voffset) >> 16) & 0xff);
 		r8712_event_handle(padapter, (uint *)poffset);
-		poffset += (cmd_len + 8);/*8 bytes aligment*/
+		poffset += (cmd_len + 8);/*8 bytes alignment*/
 	} while (le32_to_cpu(voffset) & BIT(31));
 
 }
@@ -603,7 +604,7 @@ static int recv_indicatepkt_reorder(struct _adapter *padapter,
 		}
 	}
 	spin_lock_irqsave(&ppending_recvframe_queue->lock, irql);
-	/*s2. check if winstart_b(indicate_seq) needs to been updated*/
+	/*s2. check if winstart_b(indicate_seq) needs to be updated*/
 	if (!check_indicate_seq(preorder_ctrl, pattrib->seq_num))
 		goto _err_exit;
 	/*s3. Insert all packet into Reorder Queue to maintain its ordering.*/
diff --git a/drivers/staging/rtl8712/rtl8712_xmit.c b/drivers/staging/rtl8712/rtl8712_xmit.c
index 693331955d6..3d23514c022 100644
--- a/drivers/staging/rtl8712/rtl8712_xmit.c
+++ b/drivers/staging/rtl8712/rtl8712_xmit.c
@@ -30,7 +30,6 @@
 
 #include "osdep_service.h"
 #include "drv_types.h"
-#include "rtl871x_byteorder.h"
 #include "wifi.h"
 #include "osdep_intf.h"
 #include "usb_ops.h"
diff --git a/drivers/staging/rtl8712/rtl871x_byteorder.h b/drivers/staging/rtl8712/rtl871x_byteorder.h
deleted file mode 100644
index bd3703b98bc..00000000000
--- a/drivers/staging/rtl8712/rtl871x_byteorder.h
+++ /dev/null
@@ -1,32 +0,0 @@
-/******************************************************************************
- *
- * Copyright(c) 2007 - 2010 Realtek 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
- *
- *
- ******************************************************************************/
-#ifndef _RTL871X_BYTEORDER_H_
-#define _RTL871X_BYTEORDER_H_
-
-#if defined(__LITTLE_ENDIAN)
-#  include "little_endian.h"
-#elif defined(__BIG_ENDIAN)
-#  include "big_endian.h"
-#else
-#  error "Must be LITTLE/BIG Endian Host"
-#endif
-
-#endif /* _RTL871X_BYTEORDER_H_ */
-
diff --git a/drivers/staging/rtl8712/rtl871x_cmd.c b/drivers/staging/rtl8712/rtl871x_cmd.c
index d77388bdba7..659683e022b 100644
--- a/drivers/staging/rtl8712/rtl871x_cmd.c
+++ b/drivers/staging/rtl8712/rtl871x_cmd.c
@@ -50,7 +50,6 @@
 #include "drv_types.h"
 #include "recv_osdep.h"
 #include "mlme_osdep.h"
-#include "rtl871x_byteorder.h"
 
 /*
 Caller and the r8712_cmd_thread can protect cmd_q by spin_lock.
diff --git a/drivers/staging/rtl8712/rtl871x_cmd.h b/drivers/staging/rtl8712/rtl871x_cmd.h
index 757ebf77e9d..9d93189d870 100644
--- a/drivers/staging/rtl8712/rtl871x_cmd.h
+++ b/drivers/staging/rtl8712/rtl871x_cmd.h
@@ -720,7 +720,7 @@ struct DisconnectCtrlEx_param {
  * Result:
  * 0x00: success
  * 0x01: success, and check Response.
- * 0x02: cmd ignored due to duplicated sequcne number
+ * 0x02: cmd ignored due to duplicated sequence number
  * 0x03: cmd dropped due to invalid cmd code
  * 0x04: reserved.
  */
diff --git a/drivers/staging/rtl8712/rtl871x_io.h b/drivers/staging/rtl8712/rtl871x_io.h
index d3d8727c2ec..dc23395fec3 100644
--- a/drivers/staging/rtl8712/rtl871x_io.h
+++ b/drivers/staging/rtl8712/rtl871x_io.h
@@ -117,7 +117,7 @@ struct io_req {
 	u32	command;
 	u32	status;
 	u8	*pbuf;
-	void (*_async_io_callback)(struct _adapter *padater,
+	void (*_async_io_callback)(struct _adapter *padapter,
 				   struct io_req *pio_req, u8 *cnxt);
 	u8 *cnxt;
 };
diff --git a/drivers/staging/rtl8712/rtl871x_ioctl_linux.c b/drivers/staging/rtl8712/rtl871x_ioctl_linux.c
index ef35bc29a3f..35e781fca4a 100644
--- a/drivers/staging/rtl8712/rtl871x_ioctl_linux.c
+++ b/drivers/staging/rtl8712/rtl871x_ioctl_linux.c
@@ -43,7 +43,6 @@
 #include <linux/module.h>
 #include <linux/kernel.h>
 #include <linux/init.h>
-#include <linux/version.h>
 #include <linux/io.h>
 #include <linux/semaphore.h>
 #include <net/iw_handler.h>
@@ -242,7 +241,7 @@ static inline char *translate_scan(struct _adapter *padapter,
 	/* Add frequency/channel */
 	iwe.cmd = SIOCGIWFREQ;
 	{
-		/*  check legel index */
+		/*  check legal index */
 		u8 dsconfig = pnetwork->network.Configuration.DSConfig;
 		if (dsconfig >= 1 && dsconfig <= sizeof(
 		    ieee80211_wlan_frequencies) / sizeof(long))
@@ -810,11 +809,11 @@ static int r871x_wx_set_pmkid(struct net_device *dev,
 
 /*
 	There are the BSSID information in the bssid.sa_data array.
-	If cmd is IW_PMKSA_FLUSH, it means the wpa_suppplicant wants to clear
-	 all the PMKID information. If cmd is IW_PMKSA_ADD, it means the
-	  wpa_supplicant wants to add a PMKID/BSSID to driver.
+	If cmd is IW_PMKSA_FLUSH, it means the wpa_supplicant wants to clear
+	all the PMKID information. If cmd is IW_PMKSA_ADD, it means the
+	wpa_supplicant wants to add a PMKID/BSSID to driver.
 	If cmd is IW_PMKSA_REMOVE, it means the wpa_supplicant wants to
-	  remove a PMKID/BSSID from driver.
+	remove a PMKID/BSSID from driver.
 */
 	if (pPMK == NULL)
 		return -EINVAL;
@@ -924,7 +923,7 @@ static int r8711_wx_get_range(struct net_device *dev,
 	range->max_qual.noise = 100;
 	range->max_qual.updated = 7; /* Updated all three */
 	range->avg_qual.qual = 92; /* > 8% missed beacons is 'bad' */
-	/* TODO: Find real 'good' to 'bad' threshol value for RSSI */
+	/* TODO: Find real 'good' to 'bad' threshold value for RSSI */
 	range->avg_qual.level = 20 + -98;
 	range->avg_qual.noise = 0;
 	range->avg_qual.updated = 7; /* Updated all three */
@@ -1071,7 +1070,7 @@ FREE_EXT:
  * MAC# of a preferred Access Point.
  * Currently, the request comes via Wireless Extensions' SIOCSIWAP ioctl.
  *
- * For this operation to succeed, there is no need for the interface to be Up.
+ * For this operation to succeed, there is no need for the interface to be up.
  *
  */
 static int r8711_wx_set_wap(struct net_device *dev,
@@ -2389,10 +2388,10 @@ static struct iw_statistics *r871x_get_wireless_stats(struct net_device *dev)
 
 struct iw_handler_def r871x_handlers_def = {
 	.standard = r8711_handlers,
-	.num_standard = sizeof(r8711_handlers) / sizeof(iw_handler),
+	.num_standard = ARRAY_SIZE(r8711_handlers),
 	.private = r8711_private_handler,
 	.private_args = (struct iw_priv_args *)r8711_private_args,
-	.num_private = sizeof(r8711_private_handler) / sizeof(iw_handler),
+	.num_private = ARRAY_SIZE(r8711_private_handler),
 	.num_private_args = sizeof(r8711_private_args) /
 			    sizeof(struct iw_priv_args),
 	.get_wireless_stats = r871x_get_wireless_stats
diff --git a/drivers/staging/rtl8712/rtl871x_ioctl_set.c b/drivers/staging/rtl8712/rtl871x_ioctl_set.c
index fb29b423752..f352b32355a 100644
--- a/drivers/staging/rtl8712/rtl871x_ioctl_set.c
+++ b/drivers/staging/rtl8712/rtl871x_ioctl_set.c
@@ -264,7 +264,7 @@ void r8712_set_802_11_infrastructure_mode(struct _adapter *padapter,
 		    (*pold_state == Ndis802_11Infrastructure) ||
 		    (*pold_state == Ndis802_11IBSS)) {
 			/* will clr Linked_state before this function,
-			 * we must have chked whether issue dis-assoc_cmd or
+			 * we must have checked whether issue dis-assoc_cmd or
 			 * not */
 			r8712_ind_disconnect(padapter);
 		}
diff --git a/drivers/staging/rtl8712/rtl871x_mlme.c b/drivers/staging/rtl8712/rtl871x_mlme.c
index 4277d0304b7..dc7adc132d1 100644
--- a/drivers/staging/rtl8712/rtl871x_mlme.c
+++ b/drivers/staging/rtl8712/rtl871x_mlme.c
@@ -137,7 +137,7 @@ static void _free_network_nolock(struct mlme_priv *pmlmepriv,
 
 /*
 	return the wlan_network with the matching addr
-	Shall be calle under atomic context...
+	Shall be called under atomic context...
 	to avoid possible racing condition...
 */
 static struct wlan_network *_r8712_find_network(struct  __queue *scanned_queue,
@@ -255,7 +255,7 @@ void r8712_free_network_queue(struct _adapter *dev)
 /*
 	return the wlan_network with the matching addr
 
-	Shall be calle under atomic context...
+	Shall be called under atomic context...
 	to avoid possible racing condition...
 */
 static struct wlan_network *r8712_find_network(struct  __queue *scanned_queue,
@@ -1037,7 +1037,7 @@ void r8712_cpwm_event_callback(struct _adapter *adapter, u8 *pbuf)
  *	 and the WiFi client will drop the data with seq number 0.
  *	So, the 8712 firmware has to inform driver with receiving the
  *	 ADDBA-Req frame so that the driver can reset the
- *	sequence value of Rx reorder contorl.
+ *	sequence value of Rx reorder control.
  */
 void r8712_got_addbareq_event_callback(struct _adapter *adapter, u8 *pbuf)
 {
@@ -1775,7 +1775,7 @@ static void update_ht_cap(struct _adapter *padapter, u8 *pie, uint ie_len)
 		phtpriv->rx_ampdu_maxlen = max_ampdu_sz;
 	}
 	/* for A-MPDU Rx reordering buffer control for bmc_sta & sta_info
-	 * if A-MPDU Rx is enabled, reseting rx_ordering_ctrl
+	 * if A-MPDU Rx is enabled, resetting rx_ordering_ctrl
 	 * wstart_b(indicate_seq) to default value=0xffff
 	 * todo: check if AP can send A-MPDU packets
 	 */
diff --git a/drivers/staging/rtl8712/rtl871x_mlme.h b/drivers/staging/rtl8712/rtl871x_mlme.h
index 71ca01350b5..42bd0bf8a81 100644
--- a/drivers/staging/rtl8712/rtl871x_mlme.h
+++ b/drivers/staging/rtl8712/rtl871x_mlme.h
@@ -69,8 +69,8 @@ since mlme_priv is a shared resource between many threads,
 like ISR/Call-Back functions, the OID handlers, and even timer functions.
 Each _queue has its own locks, already.
 Other items are protected by mlme_priv.lock.
-To avoid possible dead lock, any thread trying to modifiying mlme_priv
-SHALL not lock up more than one locks at a time!
+To avoid possible dead lock, any thread trying to modify mlme_priv
+SHALL not lock up more than one lock at a time!
 */
 
 #define traffic_threshold	10
@@ -132,7 +132,7 @@ static inline sint get_fwstate(struct mlme_priv *pmlmepriv)
  * therefore set it to be the critical section...
  *
  * ### NOTE:#### (!!!!)
- * TAKE CARE THAT BEFORE CALLING THIS FUNC, LOCK pmlmepriv->lock
+ * TAKE CARE BEFORE CALLING THIS FUNC, LOCK pmlmepriv->lock
  */
 static inline void set_fwstate(struct mlme_priv *pmlmepriv, sint state)
 {
diff --git a/drivers/staging/rtl8712/rtl871x_mp_phy_regdef.h b/drivers/staging/rtl8712/rtl871x_mp_phy_regdef.h
index 23532a79385..8e2586231ff 100644
--- a/drivers/staging/rtl8712/rtl871x_mp_phy_regdef.h
+++ b/drivers/staging/rtl8712/rtl871x_mp_phy_regdef.h
@@ -184,7 +184,7 @@
 
 /*RxIQ DC offset, Rx digital filter, DC notch filter */
 #define	rOFDM0_XARxAFE			0xc10
-#define	rOFDM0_XARxIQImbalance		0xc14  /* RxIQ imblance matrix */
+#define	rOFDM0_XARxIQImbalance		0xc14  /* RxIQ imbalance matrix */
 #define	rOFDM0_XBRxAFE			0xc18
 #define	rOFDM0_XBRxIQImbalance		0xc1c
 #define	rOFDM0_XCRxAFE			0xc20
@@ -603,7 +603,7 @@
 #define	bCCKRxIG		0x7f00
 #define	bCCKLNAPolarity		0x800000
 #define	bCCKRx1stGain		0x7f0000
-#define	bCCKRFExtend		0x20000000 /* CCK Rx Iinital gain polarity */
+#define	bCCKRFExtend		0x20000000 /* CCK Rx inital gain polarity */
 #define	bCCKRxAGCSatLevel	0x1f000000
 #define	bCCKRxAGCSatCount       0xe0
 #define	bCCKRxRFSettle          0x1f       /* AGCsamp_dly */
diff --git a/drivers/staging/rtl8712/rtl871x_recv.c b/drivers/staging/rtl8712/rtl871x_recv.c
index 5b03b405883..c9d1743e5c5 100644
--- a/drivers/staging/rtl8712/rtl871x_recv.c
+++ b/drivers/staging/rtl8712/rtl871x_recv.c
@@ -28,15 +28,15 @@
 
 #define _RTL871X_RECV_C_
 
+#include <linux/ip.h>
 #include <linux/slab.h>
+#include <linux/if_ether.h>
 #include <linux/kmemleak.h>
 
 #include "osdep_service.h"
 #include "drv_types.h"
 #include "recv_osdep.h"
 #include "mlme_osdep.h"
-#include "ip.h"
-#include "if_ether.h"
 #include "ethernet.h"
 #include "usb_ops.h"
 #include "wifi.h"
diff --git a/drivers/staging/rtl8712/rtl871x_xmit.c b/drivers/staging/rtl8712/rtl871x_xmit.c
index aa57e7754f0..78f570b571a 100644
--- a/drivers/staging/rtl8712/rtl871x_xmit.c
+++ b/drivers/staging/rtl8712/rtl871x_xmit.c
@@ -30,7 +30,6 @@
 
 #include "osdep_service.h"
 #include "drv_types.h"
-#include "rtl871x_byteorder.h"
 #include "wifi.h"
 #include "osdep_intf.h"
 #include "usb_ops.h"
@@ -72,7 +71,7 @@ sint _r8712_init_xmit_priv(struct xmit_priv *pxmitpriv,
 	memset((unsigned char *)pxmitpriv, 0, sizeof(struct xmit_priv));
 	spin_lock_init(&pxmitpriv->lock);
 	/*
-	Please insert all the queue initializaiton using _init_queue below
+	Please insert all the queue initialization using _init_queue below
 	*/
 	pxmitpriv->adapter = padapter;
 	_init_queue(&pxmitpriv->be_pending);
diff --git a/drivers/staging/rtl8712/rtl871x_xmit.h b/drivers/staging/rtl8712/rtl871x_xmit.h
index 638b79b4c5a..ee906987735 100644
--- a/drivers/staging/rtl8712/rtl871x_xmit.h
+++ b/drivers/staging/rtl8712/rtl871x_xmit.h
@@ -119,7 +119,7 @@ struct pkt_attrib {
 
 	u8	priority;
 	u8	encrypt;	/* when 0 indicate no encrypt. when non-zero,
-				 * indicate the encrypt algorith*/
+				 * indicate the encrypt algorithm*/
 	u8	iv_len;
 	u8	icv_len;
 	unsigned char iv[8];
diff --git a/drivers/staging/rtl8712/swab.h b/drivers/staging/rtl8712/swab.h
deleted file mode 100644
index f12781829c1..00000000000
--- a/drivers/staging/rtl8712/swab.h
+++ /dev/null
@@ -1,131 +0,0 @@
-/******************************************************************************
- *
- * Copyright(c) 2007 - 2010 Realtek 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
- *
- * Modifications for inclusion into the Linux staging tree are
- * Copyright(c) 2010 Larry Finger. All rights reserved.
- *
- * Contact information:
- * WLAN FAE <wlanfae@realtek.com>
- * Larry Finger <Larry.Finger@lwfinger.net>
- *
- ******************************************************************************/
-#ifndef _LINUX_BYTEORDER_SWAB_H
-#define _LINUX_BYTEORDER_SWAB_H
-
-#ifndef __u16
- #define __u16 unsigned short
-#endif
-
-#ifndef __u32
- #define __u32 unsigned int
-#endif
-
-#ifndef __u8
- #define __u8 unsigned char
-#endif
-
-#ifndef __u64
- #define __u64 unsigned long long
-#endif
-
-
-static inline __u16  ___swab16(__u16 x)
-{
-	__u16 __x = x;
-	return (__u16)(
-		(((__u16)(__x) & (__u16)0x00ffU) << 8) |
-		(((__u16)(__x) & (__u16)0xff00U) >> 8));
-
-}
-
-static inline __u32  ___swab32(__u32 x)
-{
-	__u32 __x = (x);
-	return (__u32)(
-		(((__u32)(__x) & (__u32)0x000000ffUL) << 24) |
-		(((__u32)(__x) & (__u32)0x0000ff00UL) <<  8) |
-		(((__u32)(__x) & (__u32)0x00ff0000UL) >>  8) |
-		(((__u32)(__x) & (__u32)0xff000000UL) >> 24));
-}
-
-static inline __u64  ___swab64(__u64 x)
-{
-	__u64 __x = (x);
-
-	return (__u64)( \
-		(__u64)(((__u64)(__x) & (__u64)0x00000000000000ffULL) << 56) | \
-		(__u64)(((__u64)(__x) & (__u64)0x000000000000ff00ULL) << 40) | \
-		(__u64)(((__u64)(__x) & (__u64)0x0000000000ff0000ULL) << 24) | \
-		(__u64)(((__u64)(__x) & (__u64)0x00000000ff000000ULL) <<  8) | \
-		(__u64)(((__u64)(__x) & (__u64)0x000000ff00000000ULL) >>  8) | \
-		(__u64)(((__u64)(__x) & (__u64)0x0000ff0000000000ULL) >> 24) | \
-		(__u64)(((__u64)(__x) & (__u64)0x00ff000000000000ULL) >> 40) | \
-		(__u64)(((__u64)(__x) & (__u64)0xff00000000000000ULL) >> 56));
-}
-
-#ifndef __arch__swab16
-static inline __u16 __arch__swab16(__u16 x)
-{
-	return ___swab16(x);
-}
-
-#endif
-
-#ifndef __arch__swab32
-static inline __u32 __arch__swab32(__u32 x)
-{
-	__u32 __tmp = (x) ;
-	return ___swab32(__tmp);
-}
-#endif
-
-#ifndef __arch__swab64
-
-static inline __u64 __arch__swab64(__u64 x)
-{
-	__u64 __tmp = (x) ;
-	return ___swab64(__tmp);
-}
-
-
-#endif
-
-#define __swab16(x) __fswab16(x)
-#define __swab32(x) __fswab32(x)
-#define __swab64(x) __fswab64(x)
-
-static inline const __u16 __fswab16(__u16 x)
-{
-	return __arch__swab16(x);
-}
-static inline const __u32 __fswab32(__u32 x)
-{
-	return __arch__swab32(x);
-}
-
-#define swab16 __swab16
-#define swab32 __swab32
-#define swab64 __swab64
-#define swab16p __swab16p
-#define swab32p __swab32p
-#define swab64p __swab64p
-#define swab16s __swab16s
-#define swab32s __swab32s
-#define swab64s __swab64s
-
-#endif /* _LINUX_BYTEORDER_SWAB_H */
-
diff --git a/drivers/staging/rtl8712/usb_halinit.c b/drivers/staging/rtl8712/usb_halinit.c
index 46287c17a41..b4ae11a78b4 100644
--- a/drivers/staging/rtl8712/usb_halinit.c
+++ b/drivers/staging/rtl8712/usb_halinit.c
@@ -141,7 +141,7 @@ u8 r8712_usb_hal_bus_init(struct _adapter *padapter)
 		/* Enable AFE PLL Macro Block */
 		val8 = r8712_read8(padapter, AFE_PLL_CTRL);
 		r8712_write8(padapter, AFE_PLL_CTRL, (val8 | 0x11));
-		/* Attatch AFE PLL to MACTOP/BB/PCIe Digital */
+		/* Attach AFE PLL to MACTOP/BB/PCIe Digital */
 		val8 = r8712_read8(padapter, SYS_ISO_CTRL);
 		r8712_write8(padapter, SYS_ISO_CTRL, (val8 & 0xEE));
 		/* Switch to 40M clock */
@@ -234,7 +234,7 @@ u8 r8712_usb_hal_bus_init(struct _adapter *padapter)
 		udelay(500);
 		r8712_write8(padapter, AFE_PLL_CTRL, (val8 | 0x11));
 		udelay(500);
-		/* Attatch AFE PLL to MACTOP/BB/PCIe Digital */
+		/* Attach AFE PLL to MACTOP/BB/PCIe Digital */
 		val8 = r8712_read8(padapter, SYS_ISO_CTRL);
 		r8712_write8(padapter, SYS_ISO_CTRL, (val8 & 0xEE));
 		/* Switch to 40M clock */
diff --git a/drivers/staging/rtl8712/usb_intf.c b/drivers/staging/rtl8712/usb_intf.c
index e419b4fd82b..9bd18e2d051 100644
--- a/drivers/staging/rtl8712/usb_intf.c
+++ b/drivers/staging/rtl8712/usb_intf.c
@@ -621,30 +621,28 @@ static void r871xu_dev_remove(struct usb_interface *pusb_intf)
 	struct usb_device *udev = interface_to_usbdev(pusb_intf);
 
 	usb_set_intfdata(pusb_intf, NULL);
-	if (padapter) {
-		if (padapter->fw_found)
-			release_firmware(padapter->fw);
-		/* never exit with a firmware callback pending */
-		wait_for_completion(&padapter->rtl8712_fw_ready);
-		if (drvpriv.drv_registered == true)
-			padapter->bSurpriseRemoved = true;
-		if (pnetdev != NULL) {
-			/* will call netdev_close() */
-			unregister_netdev(pnetdev);
-		}
-		flush_scheduled_work();
-		udelay(1);
-		/*Stop driver mlme relation timer */
-		if (padapter->fw_found)
-			r8712_stop_drv_timers(padapter);
-		r871x_dev_unload(padapter);
-		r8712_free_drv_sw(padapter);
+	if (padapter->fw_found)
+		release_firmware(padapter->fw);
+	/* never exit with a firmware callback pending */
+	wait_for_completion(&padapter->rtl8712_fw_ready);
+	if (drvpriv.drv_registered == true)
+		padapter->bSurpriseRemoved = true;
+	if (pnetdev != NULL) {
+		/* will call netdev_close() */
+		unregister_netdev(pnetdev);
 	}
+	flush_scheduled_work();
+	udelay(1);
+	/*Stop driver mlme relation timer */
+	if (padapter->fw_found)
+		r8712_stop_drv_timers(padapter);
+	r871x_dev_unload(padapter);
+	r8712_free_drv_sw(padapter);
 	usb_set_intfdata(pusb_intf, NULL);
 	/* decrease the reference count of the usb device structure
 	 * when disconnect */
 	usb_put_dev(udev);
-	/* If we didn't unplug usb dongle and remove/insert modlue, driver
+	/* If we didn't unplug usb dongle and remove/insert module, driver
 	 * fails on sitesurvey for the first time when device is up.
 	 * Reset usb port for sitesurvey fail issue. */
 	if (udev->state != USB_STATE_NOTATTACHED)
diff --git a/drivers/staging/rtl8712/usb_ops.c b/drivers/staging/rtl8712/usb_ops.c
index 5a8b0ebd0b7..c03508d935f 100644
--- a/drivers/staging/rtl8712/usb_ops.c
+++ b/drivers/staging/rtl8712/usb_ops.c
@@ -33,7 +33,6 @@
 #include "osdep_intf.h"
 #include "usb_ops.h"
 #include "recv_osdep.h"
-#include "rtl871x_byteorder.h"
 
 static u8 usb_read8(struct intf_hdl *pintfhdl, u32 addr)
 {
diff --git a/drivers/staging/rtl8712/wifi.h b/drivers/staging/rtl8712/wifi.h
index 277398cff0a..793443e758a 100644
--- a/drivers/staging/rtl8712/wifi.h
+++ b/drivers/staging/rtl8712/wifi.h
@@ -26,7 +26,6 @@
 #ifndef _WIFI_H_
 #define _WIFI_H_
 
-#include "rtl871x_byteorder.h"
 #include <linux/compiler.h>
 
 #ifdef BIT
diff --git a/drivers/staging/rtl8712/xmit_linux.c b/drivers/staging/rtl8712/xmit_linux.c
index c9703627c8f..65542cb7168 100644
--- a/drivers/staging/rtl8712/xmit_linux.c
+++ b/drivers/staging/rtl8712/xmit_linux.c
@@ -29,14 +29,12 @@
 #define _XMIT_OSDEP_C_
 
 #include <linux/usb.h>
+#include <linux/ip.h>
+#include <linux/if_ether.h>
 
 #include "osdep_service.h"
 #include "drv_types.h"
 
-
-#include "if_ether.h"
-#include "ip.h"
-#include "rtl871x_byteorder.h"
 #include "wifi.h"
 #include "mlme_osdep.h"
 #include "xmit_osdep.h"
diff --git a/drivers/staging/rts5139/ms.c b/drivers/staging/rts5139/ms.c
index b0e9071c8e5..6eef33b03f5 100644
--- a/drivers/staging/rts5139/ms.c
+++ b/drivers/staging/rts5139/ms.c
@@ -2680,7 +2680,7 @@ static int mspro_set_rw_cmd(struct rts51x_chip *chip, u32 start_sec,
 	return STATUS_SUCCESS;
 }
 
-void mspro_stop_seq_mode(struct rts51x_chip *chip)
+static void mspro_stop_seq_mode(struct rts51x_chip *chip)
 {
 	struct ms_info *ms_card = &(chip->ms_card);
 	int retval;
@@ -3149,7 +3149,7 @@ Fail:
 		TRACE_RET(chip, STATUS_FAIL);
 
 	sec_cnt = chip->rsp_buf[0];
-	RTS51X_DEBUGP("%d pages need be trasferred, %d pages remained\n",
+	RTS51X_DEBUGP("%d pages need be transferred, %d pages remained\n",
 		       (int)page_cnt, (int)sec_cnt);
 	page_addr = start_page + (page_cnt - sec_cnt);
 
@@ -3864,7 +3864,7 @@ static int ms_rw_multi_sector(struct scsi_cmnd *srb, struct rts51x_chip *chip,
 	log_blk = (u16) (start_sector >> ms_card->block_shift);
 	start_page = (u8) (start_sector & ms_card->page_off);
 
-	for (seg_no = 0; seg_no < sizeof(ms_start_idx) / 2; seg_no++) {
+	for (seg_no = 0; seg_no < ARRAY_SIZE(ms_start_idx) - 1; seg_no++) {
 		if (log_blk < ms_start_idx[seg_no + 1])
 			break;
 	}
@@ -4020,7 +4020,8 @@ static int ms_rw_multi_sector(struct scsi_cmnd *srb, struct rts51x_chip *chip,
 
 		log_blk++;
 
-		for (seg_no = 0; seg_no < sizeof(ms_start_idx) / 2; seg_no++) {
+		for (seg_no = 0; seg_no < ARRAY_SIZE(ms_start_idx) - 1;
+				seg_no++) {
 			if (log_blk < ms_start_idx[seg_no + 1])
 				break;
 		}
@@ -4134,7 +4135,7 @@ void ms_cleanup_work(struct rts51x_chip *chip)
 	}
 }
 
-int ms_power_off_card3v3(struct rts51x_chip *chip)
+static int ms_power_off_card3v3(struct rts51x_chip *chip)
 {
 	int retval;
 
diff --git a/drivers/staging/rts5139/ms.h b/drivers/staging/rts5139/ms.h
index 3ce1dc90f19..0321d06e776 100644
--- a/drivers/staging/rts5139/ms.h
+++ b/drivers/staging/rts5139/ms.h
@@ -234,7 +234,6 @@
 void mspro_polling_format_status(struct rts51x_chip *chip);
 void mspro_format_sense(struct rts51x_chip *chip, unsigned int lun);
 
-void mspro_stop_seq_mode(struct rts51x_chip *chip);
 int reset_ms_card(struct rts51x_chip *chip);
 int ms_rw(struct scsi_cmnd *srb, struct rts51x_chip *chip, u32 start_sector,
 	  u16 sector_cnt);
@@ -242,7 +241,6 @@ int mspro_format(struct scsi_cmnd *srb, struct rts51x_chip *chip,
 		 int short_data_len, int quick_format);
 void ms_free_l2p_tbl(struct rts51x_chip *chip);
 void ms_cleanup_work(struct rts51x_chip *chip);
-int ms_power_off_card3v3(struct rts51x_chip *chip);
 int release_ms_card(struct rts51x_chip *chip);
 int ms_delay_write(struct rts51x_chip *chip);
 
diff --git a/drivers/staging/rts5139/ms_mg.c b/drivers/staging/rts5139/ms_mg.c
index 154b5230aa5..057d96c1a93 100644
--- a/drivers/staging/rts5139/ms_mg.c
+++ b/drivers/staging/rts5139/ms_mg.c
@@ -38,7 +38,7 @@
 
 #ifdef SUPPORT_MAGIC_GATE
 
-int mg_check_int_error(struct rts51x_chip *chip)
+static int mg_check_int_error(struct rts51x_chip *chip)
 {
 	u8 value;
 
@@ -444,7 +444,7 @@ int mg_rsp(struct scsi_cmnd *srb, struct rts51x_chip *chip)
   *
   * Since the extra 4 bytes data is just only a prefix to original data
   * that read from medium, so that the 4-byte data pushed into Ring buffer
-  * precedes data tramsinssion from medium to Ring buffer by DMA mechanisim
+  * precedes data transmission from medium to Ring buffer by DMA mechanism
   * in order to get maximum performance and minimum code size simultaneously.
   */
 int mg_get_ICV(struct scsi_cmnd *srb, struct rts51x_chip *chip)
diff --git a/drivers/staging/rts5139/rts51x.c b/drivers/staging/rts5139/rts51x.c
index 2b9f785954d..c3fe7dda1f4 100644
--- a/drivers/staging/rts5139/rts51x.c
+++ b/drivers/staging/rts5139/rts51x.c
@@ -56,12 +56,6 @@ MODULE_DESCRIPTION(RTS51X_DESC);
 MODULE_LICENSE("GPL");
 MODULE_VERSION(DRIVER_VERSION);
 
-#ifdef SCSI_SCAN_DELAY
-static unsigned int delay_use = 5;
-module_param(delay_use, uint, S_IRUGO | S_IWUSR);
-MODULE_PARM_DESC(delay_use, "seconds to delay before using a new device");
-#endif
-
 static int auto_delink_en;
 module_param(auto_delink_en, int, S_IRUGO | S_IWUSR);
 MODULE_PARM_DESC(auto_delink_en, "enable auto delink");
@@ -114,7 +108,7 @@ static inline void usb_autopm_disable(struct usb_interface *intf)
 	usb_autopm_get_interface(intf);
 }
 
-void rts51x_try_to_enter_ss(struct rts51x_chip *chip)
+static void rts51x_try_to_enter_ss(struct rts51x_chip *chip)
 {
 	RTS51X_DEBUGP("Ready to enter SS state\n");
 	usb_autopm_enable(chip->usb->pusb_intf);
@@ -207,7 +201,7 @@ int rts51x_reset_resume(struct usb_interface *iface)
 
 #else /* CONFIG_PM */
 
-void rts51x_try_to_enter_ss(struct rts51x_chip *chip)
+static void rts51x_try_to_enter_ss(struct rts51x_chip *chip)
 {
 }
 
@@ -364,11 +358,6 @@ static int rts51x_polling_thread(void *__chip)
 {
 	struct rts51x_chip *chip = (struct rts51x_chip *)__chip;
 
-#ifdef SCSI_SCAN_DELAY
-	/* Wait until SCSI scan finished */
-	wait_timeout((delay_use + 5) * HZ);
-#endif
-
 	for (;;) {
 		wait_timeout(POLLING_INTERVAL);
 
@@ -432,38 +421,6 @@ static int rts51x_polling_thread(void *__chip)
 	return 0;
 }
 
-#ifdef SCSI_SCAN_DELAY
-/* Thread to carry out delayed SCSI-device scanning */
-static int rts51x_scan_thread(void *__chip)
-{
-	struct rts51x_chip *chip = (struct rts51x_chip *)__chip;
-
-	printk(KERN_DEBUG
-	       "rts51x: device found at %d\n", chip->usb->pusb_dev->devnum);
-
-	set_freezable();
-	/* Wait for the timeout to expire or for a disconnect */
-	if (delay_use > 0) {
-		printk(KERN_DEBUG "rts51x: waiting for device "
-		       "to settle before scanning\n");
-		wait_event_freezable_timeout(chip->usb->delay_wait,
-					     test_bit(FLIDX_DONT_SCAN,
-						      &chip->usb->dflags),
-					     delay_use * HZ);
-	}
-
-	/* If the device is still connected, perform the scanning */
-	if (!test_bit(FLIDX_DONT_SCAN, &chip->usb->dflags)) {
-		scsi_scan_host(rts51x_to_host(chip));
-		printk(KERN_DEBUG "rts51x: device scan complete\n");
-
-		/* Should we unbind if no devices were detected? */
-	}
-
-	complete_and_exit(&chip->usb->scanning_done, 0);
-}
-#endif
-
 /* Associate our private data with the USB device */
 static int associate_dev(struct rts51x_chip *chip, struct usb_interface *intf)
 {
@@ -521,7 +478,6 @@ static void rts51x_init_options(struct rts51x_chip *chip)
 {
 	struct rts51x_option *option = &(chip->option);
 
-	option->led_blink_speed = 7;
 	option->mspro_formatter_enable = 1;
 
 	option->fpga_sd_sdr104_clk = CLK_100;
@@ -549,7 +505,6 @@ static void rts51x_init_options(struct rts51x_chip *chip)
 
 	option->ss_en = ss_en;
 	option->ss_delay = ss_delay;
-	option->needs_remote_wakeup = needs_remote_wakeup;
 
 	option->auto_delink_en = auto_delink_en;
 
@@ -561,10 +516,7 @@ static void rts51x_init_options(struct rts51x_chip *chip)
 	option->rts5129_D3318_off_enable = 0;
 	option->sd20_pad_drive = 0;
 	option->reset_or_rw_fail_set_pad_drive = 1;
-	option->rcc_fail_flag = 0;
-	option->rcc_bug_fix_en = 1;
 	option->debounce_num = 2;
-	option->polling_time = 100;
 	option->led_toggle_interval = 6;
 	option->xd_rwn_step = 0;
 	option->sd_send_status_en = 0;
@@ -737,15 +689,6 @@ static void quiesce_and_remove_host(struct rts51x_chip *chip)
 	if (rts51x->pusb_dev->state == USB_STATE_NOTATTACHED)
 		set_bit(FLIDX_DISCONNECTING, &rts51x->dflags);
 
-#ifdef SCSI_SCAN_DELAY
-	/* Prevent SCSI-scanning (if it hasn't started yet)
-	 * and wait for the SCSI-scanning thread to stop.
-	 */
-	set_bit(FLIDX_DONT_SCAN, &rts51x->dflags);
-	wake_up(&rts51x->delay_wait);
-	wait_for_completion(&rts51x->scanning_done);
-#endif
-
 	/* Removing the host will perform an orderly shutdown: caches
 	 * synchronized, disks spun down, etc.
 	 */
@@ -757,9 +700,6 @@ static void quiesce_and_remove_host(struct rts51x_chip *chip)
 	scsi_lock(host);
 	set_bit(FLIDX_DISCONNECTING, &rts51x->dflags);
 	scsi_unlock(host);
-#ifdef SCSI_SCAN_DELAY
-	wake_up(&rts51x->delay_wait);
-#endif
 }
 
 /* Second stage of disconnect processing: deallocate all resources */
@@ -818,10 +758,6 @@ static int rts51x_probe(struct usb_interface *intf,
 	init_completion(&rts51x->control_exit);
 	init_completion(&rts51x->polling_exit);
 	init_completion(&(rts51x->notify));
-#ifdef SCSI_SCAN_DELAY
-	init_waitqueue_head(&rts51x->delay_wait);
-	init_completion(&rts51x->scanning_done);
-#endif
 
 	chip->usb = rts51x;
 
@@ -855,22 +791,7 @@ static int rts51x_probe(struct usb_interface *intf,
 		printk(KERN_WARNING RTS51X_TIP "Unable to add the scsi host\n");
 		goto BadDevice;
 	}
-#ifdef SCSI_SCAN_DELAY
-	/* Start up the thread for delayed SCSI-device scanning */
-	th = kthread_create(rts51x_scan_thread, chip, RTS51X_SCAN_THREAD);
-	if (IS_ERR(th)) {
-		printk(KERN_WARNING RTS51X_TIP
-		       "Unable to start the device-scanning thread\n");
-		complete(&rts51x->scanning_done);
-		quiesce_and_remove_host(chip);
-		result = PTR_ERR(th);
-		goto BadDevice;
-	}
-
-	wake_up_process(th);
-#else
 	scsi_scan_host(rts51x_to_host(chip));
-#endif
 
 	/* Start up our polling thread */
 	th = kthread_run(rts51x_polling_thread, chip, RTS51X_POLLING_THREAD);
diff --git a/drivers/staging/rts5139/rts51x.h b/drivers/staging/rts5139/rts51x.h
index b2c58390bfc..ecc0109a5b1 100644
--- a/drivers/staging/rts5139/rts51x.h
+++ b/drivers/staging/rts5139/rts51x.h
@@ -47,11 +47,9 @@
 #define RTS51X_DESC		"Realtek RTS5139/29 USB card reader driver"
 #define RTS51X_NAME		"rts5139"
 #define RTS51X_CTL_THREAD	"rts5139-control"
-#define RTS51X_SCAN_THREAD	"rts5139-scan"
 #define RTS51X_POLLING_THREAD	"rts5139-polling"
 
 #define POLLING_IN_THREAD
-/* #define SCSI_SCAN_DELAY */
 #define SUPPORT_FILE_OP
 
 #define wait_timeout_x(task_state, msecs)	\
@@ -66,8 +64,6 @@ do {						\
 
 /* Size of the DMA-mapped I/O buffer */
 #define RTS51X_IOBUF_SIZE	1024
-/* Size of the autosense data buffer */
-#define RTS51X_SENSE_SIZE	18
 
 /* Dynamic bitflag definitions (dflags): used in set_bit() etc. */
 #define FLIDX_URB_ACTIVE	0	/* current_urb is in use    */
@@ -76,7 +72,6 @@ do {						\
 #define FLIDX_DISCONNECTING	3	/* disconnect in progress   */
 #define FLIDX_RESETTING		4	/* device reset in progress */
 #define FLIDX_TIMED_OUT		5	/* SCSI midlayer timed out  */
-#define FLIDX_DONT_SCAN		6	/* don't scan (disconnect)  */
 
 struct rts51x_chip;
 
@@ -116,10 +111,6 @@ struct rts51x_usb {
 	struct completion control_exit;	/* control thread exit     */
 	struct completion polling_exit;	/* polling thread exit     */
 	struct completion notify;	/* thread begin/end        */
-#ifdef SCSI_SCAN_DELAY
-	wait_queue_head_t delay_wait;	/* wait during scan, reset */
-	struct completion scanning_done;	/* wait for scan thread    */
-#endif
 };
 
 extern struct usb_driver rts51x_driver;
@@ -188,7 +179,6 @@ enum xfer_buf_dir { TO_XFER_BUF, FROM_XFER_BUF };
 
 /* General routines provided by the usb-storage standard core */
 #ifdef CONFIG_PM
-void rts51x_try_to_enter_ss(struct rts51x_chip *chip);
 void rts51x_try_to_exit_ss(struct rts51x_chip *chip);
 int rts51x_suspend(struct usb_interface *iface, pm_message_t message);
 int rts51x_resume(struct usb_interface *iface);
diff --git a/drivers/staging/rts5139/rts51x_card.c b/drivers/staging/rts5139/rts51x_card.c
index 424a84581b8..4192c3bac12 100644
--- a/drivers/staging/rts5139/rts51x_card.c
+++ b/drivers/staging/rts5139/rts51x_card.c
@@ -37,7 +37,6 @@
 #include "rts51x_chip.h"
 #include "rts51x_card.h"
 #include "rts51x_transport.h"
-#include "rts51x_sys.h"
 #include "xd.h"
 #include "sd.h"
 #include "ms.h"
@@ -94,7 +93,7 @@ void do_remaining_work(struct rts51x_chip *chip)
 		ms_cleanup_work(chip);
 }
 
-void do_reset_xd_card(struct rts51x_chip *chip)
+static void do_reset_xd_card(struct rts51x_chip *chip)
 {
 	int retval;
 
@@ -148,7 +147,7 @@ void do_reset_sd_card(struct rts51x_chip *chip)
 	}
 }
 
-void do_reset_ms_card(struct rts51x_chip *chip)
+static void do_reset_ms_card(struct rts51x_chip *chip)
 {
 	int retval;
 
@@ -175,7 +174,7 @@ void do_reset_ms_card(struct rts51x_chip *chip)
 	}
 }
 
-void card_cd_debounce(struct rts51x_chip *chip, u8 *need_reset,
+static void card_cd_debounce(struct rts51x_chip *chip, u8 *need_reset,
 		      u8 *need_release)
 {
 	int retval;
@@ -191,7 +190,6 @@ void card_cd_debounce(struct rts51x_chip *chip, u8 *need_reset,
 		goto Exit_Debounce;
 
 	if (chip->card_exist) {
-		rts51x_clear_start_time(chip);
 		retval = rts51x_read_register(chip, CARD_INT_PEND, &value);
 		if (retval != STATUS_SUCCESS) {
 			rts51x_ep0_write_register(chip, MC_FIFO_CTL, FIFO_FLUSH,
@@ -214,17 +212,11 @@ void card_cd_debounce(struct rts51x_chip *chip, u8 *need_reset,
 		}
 	} else {
 		if (chip->card_status & XD_CD) {
-			rts51x_clear_start_time(chip);
 			reset_map |= XD_CARD;
 		} else if (chip->card_status & SD_CD) {
-			rts51x_clear_start_time(chip);
 			reset_map |= SD_CARD;
 		} else if (chip->card_status & MS_CD) {
-			rts51x_clear_start_time(chip);
 			reset_map |= MS_CARD;
-		} else {
-			if (rts51x_check_start_time(chip))
-				rts51x_set_start_time(chip);
 		}
 	}
 
@@ -709,7 +701,7 @@ u8 get_lun_card(struct rts51x_chip *chip, unsigned int lun)
 	return 0;
 }
 
-int card_share_mode(struct rts51x_chip *chip, int card)
+static int card_share_mode(struct rts51x_chip *chip, int card)
 {
 	u8 value;
 
@@ -823,22 +815,6 @@ int enable_card_clock(struct rts51x_chip *chip, u8 card)
 	return STATUS_SUCCESS;
 }
 
-int disable_card_clock(struct rts51x_chip *chip, u8 card)
-{
-	u8 clk_en = 0;
-
-	if (card & XD_CARD)
-		clk_en |= XD_CLK_EN;
-	if (card & SD_CARD)
-		clk_en |= SD_CLK_EN;
-	if (card & MS_CARD)
-		clk_en |= MS_CLK_EN;
-
-	RTS51X_WRITE_REG(chip, CARD_CLK_EN, clk_en, 0);
-
-	return STATUS_SUCCESS;
-}
-
 int card_power_on(struct rts51x_chip *chip, u8 card)
 {
 	u8 mask, val1, val2;
@@ -851,16 +827,7 @@ int card_power_on(struct rts51x_chip *chip, u8 card)
 	if ((card == SD_CARD) || (card == XD_CARD)) {
 		RTS51X_WRITE_REG(chip, CARD_PWR_CTL, mask | LDO3318_PWR_MASK,
 				 val1 | LDO_SUSPEND);
-		/* RTS51X_WRITE_REG(chip, CARD_PWR_CTL,
-				LDO3318_PWR_MASK, LDO_SUSPEND); */
 	}
-	/* else if(card==XD_CARD)
-	{
-		RTS51X_WRITE_REG(chip, CARD_PWR_CTL,
-			mask|LDO3318_PWR_MASK, val1|LDO_SUSPEND);
-		//RTS51X_WRITE_REG(chip, CARD_PWR_CTL,
-		//	LDO3318_PWR_MASK, LDO_SUSPEND);
-	} */
 	else {
 #endif
 		RTS51X_WRITE_REG(chip, CARD_PWR_CTL, mask, val1);
@@ -879,17 +846,6 @@ int card_power_on(struct rts51x_chip *chip, u8 card)
 	return STATUS_SUCCESS;
 }
 
-int card_power_off(struct rts51x_chip *chip, u8 card)
-{
-	u8 mask, val;
-
-	mask = POWER_MASK;
-	val = POWER_OFF;
-	RTS51X_WRITE_REG(chip, CARD_PWR_CTL, mask, val);
-
-	return STATUS_SUCCESS;
-}
-
 int monitor_card_cd(struct rts51x_chip *chip, u8 card)
 {
 	int retval;
diff --git a/drivers/staging/rts5139/rts51x_card.h b/drivers/staging/rts5139/rts51x_card.h
index ac3c1e741ab..c5c03cce98b 100644
--- a/drivers/staging/rts5139/rts51x_card.h
+++ b/drivers/staging/rts5139/rts51x_card.h
@@ -204,13 +204,7 @@
 
 /* LDO_POWER_CFG */
 #define TUNE_SD18_MASK			0x1C
-#define TUNE_SD18_1V7			0x00
 #define TUNE_SD18_1V8			(0x01 << 2)
-#define TUNE_SD18_1V9			(0x02 << 2)
-#define TUNE_SD18_2V0			(0x03 << 2)
-#define TUNE_SD18_2V7			(0x04 << 2)
-#define TUNE_SD18_2V8			(0x05 << 2)
-#define TUNE_SD18_2V9			(0x06 << 2)
 #define TUNE_SD18_3V3			(0x07 << 2)
 
 /* XD_CP_WAITTIME */
@@ -744,9 +738,7 @@
 int monitor_card_cd(struct rts51x_chip *chip, u8 card);
 
 void do_remaining_work(struct rts51x_chip *chip);
-void do_reset_xd_card(struct rts51x_chip *chip);
 void do_reset_sd_card(struct rts51x_chip *chip);
-void do_reset_ms_card(struct rts51x_chip *chip);
 void rts51x_init_cards(struct rts51x_chip *chip);
 void rts51x_release_cards(struct rts51x_chip *chip);
 int switch_ssc_clock(struct rts51x_chip *chip, int clk);
@@ -754,15 +746,12 @@ int switch_normal_clock(struct rts51x_chip *chip, int clk);
 int card_rw(struct scsi_cmnd *srb, struct rts51x_chip *chip, u32 sec_addr,
 	    u16 sec_cnt);
 u8 get_lun_card(struct rts51x_chip *chip, unsigned int lun);
-int card_share_mode(struct rts51x_chip *chip, int card);
 int rts51x_select_card(struct rts51x_chip *chip, int card);
 void eject_card(struct rts51x_chip *chip, unsigned int lun);
 void trans_dma_enable(enum dma_data_direction dir, struct rts51x_chip *chip,
 		      u32 byte_cnt, u8 pack_size);
 int enable_card_clock(struct rts51x_chip *chip, u8 card);
-int disable_card_clock(struct rts51x_chip *chip, u8 card);
 int card_power_on(struct rts51x_chip *chip, u8 card);
-int card_power_off(struct rts51x_chip *chip, u8 card);
 int toggle_gpio(struct rts51x_chip *chip, u8 gpio);
 int turn_on_led(struct rts51x_chip *chip, u8 gpio);
 int turn_off_led(struct rts51x_chip *chip, u8 gpio);
diff --git a/drivers/staging/rts5139/rts51x_chip.c b/drivers/staging/rts5139/rts51x_chip.c
index b3e0bb22b0f..db88d7a194b 100644
--- a/drivers/staging/rts5139/rts51x_chip.c
+++ b/drivers/staging/rts5139/rts51x_chip.c
@@ -34,7 +34,6 @@
 #include "rts51x_chip.h"
 #include "rts51x_card.h"
 #include "rts51x_transport.h"
-#include "rts51x_sys.h"
 #include "xd.h"
 #include "ms.h"
 #include "sd.h"
@@ -79,20 +78,18 @@ int rts51x_reset_chip(struct rts51x_chip *chip)
 				      chip->option.sd20_pad_drive);
 		if (chip->rts5179)
 			rts51x_write_register(chip, CARD_PULL_CTL5, 0x03, 0x01);
-		if (!chip->option.ww_enable) {
-			if (CHECK_PKG(chip, LQFP48)) {
-				rts51x_write_register(chip, CARD_PULL_CTL3,
-						      0x80, 0x80);
-				rts51x_write_register(chip, CARD_PULL_CTL6,
-						      0xf0, 0xA0);
-			} else {
-				rts51x_write_register(chip, CARD_PULL_CTL1,
-						      0x30, 0x20);
-				rts51x_write_register(chip, CARD_PULL_CTL3,
-						      0x80, 0x80);
-				rts51x_write_register(chip, CARD_PULL_CTL6,
-						      0x0c, 0x08);
-			}
+		if (CHECK_PKG(chip, LQFP48)) {
+			rts51x_write_register(chip, CARD_PULL_CTL3,
+					      0x80, 0x80);
+			rts51x_write_register(chip, CARD_PULL_CTL6,
+					      0xf0, 0xA0);
+		} else {
+			rts51x_write_register(chip, CARD_PULL_CTL1,
+					      0x30, 0x20);
+			rts51x_write_register(chip, CARD_PULL_CTL3,
+					      0x80, 0x80);
+			rts51x_write_register(chip, CARD_PULL_CTL6,
+					      0x0c, 0x08);
 		}
 	}
 	if (chip->option.sd_ctl & SUPPORT_UHS50_MMC44) {
@@ -121,12 +118,6 @@ int rts51x_reset_chip(struct rts51x_chip *chip)
 
 	/* GPIO OE */
 	rts51x_add_cmd(chip, WRITE_REG_CMD, CARD_GPIO, GPIO_OE, GPIO_OE);
-#ifdef LED_AUTO_BLINK
-	/* LED autoblink */
-	rts51x_add_cmd(chip, WRITE_REG_CMD, CARD_AUTO_BLINK,
-		       BLINK_ENABLE | BLINK_SPEED_MASK,
-		       BLINK_ENABLE | chip->option.led_blink_speed);
-#endif
 	rts51x_add_cmd(chip, WRITE_REG_CMD, CARD_DMA1_CTL,
 		       EXTEND_DMA1_ASYNC_SIGNAL, EXTEND_DMA1_ASYNC_SIGNAL);
 
@@ -144,7 +135,6 @@ int rts51x_reset_chip(struct rts51x_chip *chip)
 		card_power_on(chip, SD_CARD | MS_CARD | XD_CARD);
 		wait_timeout(10);
 	}
-	rts51x_clear_start_time(chip);
 
 	return STATUS_SUCCESS;
 }
@@ -164,12 +154,6 @@ int rts51x_init_chip(struct rts51x_chip *chip)
 	chip->card_ejected = 0;
 
 	chip->lun2card[0] = XD_CARD | SD_CARD | MS_CARD;
-#if 0
-	chip->option.sdr50_tx_phase = 0x01;
-	chip->option.sdr50_rx_phase = 0x05;
-	chip->option.ddr50_tx_phase = 0x09;
-	chip->option.ddr50_rx_phase = 0x06; /* add for debug */
-#endif
 #ifdef CLOSE_SSC_POWER
 	rts51x_write_register(chip, FPDCTL, SSC_POWER_MASK, SSC_POWER_ON);
 	udelay(100);
@@ -178,9 +162,6 @@ int rts51x_init_chip(struct rts51x_chip *chip)
 	RTS51X_SET_STAT(chip, STAT_RUN);
 
 	RTS51X_READ_REG(chip, HW_VERSION, &val);
-	if ((val & 0x0f) >= 2)
-		chip->option.rcc_bug_fix_en = 0;
-	RTS51X_DEBUGP("rcc bug fix enable:%d\n", chip->option.rcc_bug_fix_en);
 	RTS51X_DEBUGP("HW_VERSION: 0x%x\n", val);
 	if (val & FPGA_VER) {
 		chip->asic_code = 0;
@@ -237,7 +218,6 @@ int rts51x_release_chip(struct rts51x_chip *chip)
 	return STATUS_SUCCESS;
 }
 
-#ifndef LED_AUTO_BLINK
 static inline void rts51x_blink_led(struct rts51x_chip *chip)
 {
 	/* Read/Write */
@@ -251,20 +231,6 @@ static inline void rts51x_blink_led(struct rts51x_chip *chip)
 		}
 	}
 }
-#endif
-
-int rts51x_check_start_time(struct rts51x_chip *chip)
-{
-	return 0;
-}
-
-void rts51x_set_start_time(struct rts51x_chip *chip)
-{
-}
-
-void rts51x_clear_start_time(struct rts51x_chip *chip)
-{
-}
 
 static void rts51x_auto_delink_cmd(struct rts51x_chip *chip)
 {
@@ -287,7 +253,6 @@ static void rts51x_auto_delink_polling_cycle(struct rts51x_chip *chip)
 			chip->option.delink_delay * 2) {
 		if (chip->auto_delink_counter ==
 		    chip->option.delink_delay) {
-			clear_first_install_mark(chip);
 			if (chip->card_exist) {
 				/* False card */
 				if (!chip->card_ejected) {
@@ -321,91 +286,13 @@ static void rts51x_auto_delink(struct rts51x_chip *chip)
 }
 #else
 /* some of called funcs are not implemented, so comment it out */
-#if 0
-/* using precise time as delink time */
-static void rts51x_auto_delink_precise_time(struct rts51x_chip *chip)
-{
-	int retvalue = 0;
-
-	retvalue = rts51x_get_card_status(chip, &chip->card_status);
-	/* get card CD status success and card CD not exist,
-	 * then check whether delink */
-	if ((retvalue == STATUS_SUCCESS)
-	    && (!(chip->card_status & (SD_CD | MS_CD | XD_CD)))) {
-		if (rts51x_count_delink_time(chip) >=
-		    chip->option.delink_delay) {
-			clear_first_install_mark(chip);
-			RTS51X_DEBUGP("No card inserted, do delink\n");
-			/* sangdy2010-05-17:disable because there is error
-			 * after SSC clock closed and card power
-			 * has been closed before */
-			/* rts51x_write_register(chip, CARD_PWR_CTL,
-					DV3318_AUTO_PWR_OFF, 0); */
-			rts51x_auto_delink_cmd(chip);
-	}
-	/* card CD exist and not ready, then do force delink */
-	if ((retvalue == STATUS_SUCCESS)
-	    && (chip->card_status & (SD_CD | MS_CD | XD_CD))) {
-		/* if card is not ejected or safely remove,
-		 * then do force delink */
-		if (!chip->card_ejected) {
-			/* sangdy2010-11-16:polling at least 2 cycles
-			 * then do force delink for card may force delink
-			 * if card is extracted and insert quickly
-			 * after ready. */
-			if (chip->auto_delink_counter > 1) {
-				if (rts51x_count_delink_time(chip) >
-				    chip->option.delink_delay * 2) {
-					RTS51X_DEBUGP("Try to do force"
-							"delink\n");
-					rts51x_auto_delink_force_cmd(chip);
-				}
-			}
-		}
-	}
-	chip->auto_delink_counter++;
-}
-#else
-static void rts51x_auto_delink_precise_time(struct rts51x_chip *chip)
-{
-}
-#endif
-
 static void rts51x_auto_delink(struct rts51x_chip *chip)
 {
-	rts51x_auto_delink_precise_time(chip);
 }
 #endif
 
 void rts51x_polling_func(struct rts51x_chip *chip)
 {
-#ifdef SUPPORT_SD_LOCK
-	struct sd_info *sd_card = &(chip->sd_card);
-
-	if (sd_card->sd_erase_status) {
-		if (chip->card_exist & SD_CARD) {
-			u8 val;
-			rts51x_read_register(chip, SD_BUS_STAT, &val);
-			if (val & SD_DAT0_STATUS) {
-				/* Erase completed */
-				sd_card->sd_erase_status = SD_NOT_ERASE;
-				sd_card->sd_lock_notify = 1;
-
-				/* SD card should be reinited,
-				 * so we release it here. */
-				sd_cleanup_work(chip);
-				release_sd_card(chip);
-				chip->card_ready &= ~SD_CARD;
-				chip->card_exist &= ~SD_CARD;
-				chip->rw_card[chip->card2lun[SD_CARD]] = NULL;
-				clear_bit(chip->card2lun[SD_CARD],
-					  &(chip->lun_mc));
-			}
-		} else {
-			sd_card->sd_erase_status = SD_NOT_ERASE;
-		}
-	}
-#endif
 
 	rts51x_init_cards(chip);
 
@@ -431,9 +318,7 @@ void rts51x_polling_func(struct rts51x_chip *chip)
 		if (!RTS51X_CHK_STAT(chip, STAT_IDLE)) {
 			RTS51X_DEBUGP("Idle state!\n");
 			RTS51X_SET_STAT(chip, STAT_IDLE);
-#ifndef LED_AUTO_BLINK
 			chip->led_toggle_counter = 0;
-#endif
 			/* Idle state, turn off LED
 			 * to reduce power consumption */
 			if (chip->option.led_always_on
@@ -467,9 +352,7 @@ void rts51x_polling_func(struct rts51x_chip *chip)
 
 	switch (RTS51X_GET_STAT(chip)) {
 	case STAT_RUN:
-#ifndef LED_AUTO_BLINK
 		rts51x_blink_led(chip);
-#endif
 		do_remaining_work(chip);
 		break;
 
@@ -484,7 +367,6 @@ void rts51x_polling_func(struct rts51x_chip *chip)
 		rts51x_auto_delink(chip);
 	} else {
 		chip->auto_delink_counter = 0;
-		rts51x_clear_start_time(chip);
 	}
 }
 
@@ -831,7 +713,7 @@ void rts51x_do_before_power_down(struct rts51x_chip *chip)
 	chip->cur_clk = 0;
 	chip->card_exist = 0;
 	chip->cur_card = 0;
-	if (chip->asic_code && !chip->option.ww_enable) {
+	if (chip->asic_code) {
 		if (CHECK_PKG(chip, LQFP48)) {
 			rts51x_write_register(chip, CARD_PULL_CTL3, 0x80, 0x00);
 			rts51x_write_register(chip, CARD_PULL_CTL6, 0xf0, 0x50);
@@ -863,16 +745,6 @@ void rts51x_prepare_run(struct rts51x_chip *chip)
 		rts51x_write_register(chip, CLK_DIV, CLK_CHANGE, 0x00);
 	}
 #endif
-#if 0
-	if (chip->option.ss_en && RTS51X_CHK_STAT(chip, STAT_SS)) {
-		rts51x_try_to_exit_ss(chip);
-		wait_timeout(100);
-		rts51x_init_chip(chip);
-		rts51x_init_cards(chip);
-	}
-
-	RTS51X_SET_STAT(chip, STAT_RUN);
-#endif
 }
 
 #ifdef _MSG_TRACE
@@ -1017,24 +889,6 @@ void rts51x_pp_status(struct rts51x_chip *chip, unsigned int lun, u8 *status,
 				status[0x0F] = 0x00;
 		}
 	}
-#ifdef SUPPORT_SD_LOCK
-	/* SD Lock/Unlock */
-	if (card == SD_CARD) {
-		status[0x17] = 0x80;
-		if (sd_card->sd_erase_status)
-			status[0x17] |= 0x01; /* Under erasing */
-		if (sd_card->sd_lock_status & SD_LOCKED) {
-			status[0x17] |= 0x02; /* Locked */
-			status[0x07] |= 0x40; /* Read protected */
-		}
-		if (sd_card->sd_lock_status & SD_PWD_EXIST)
-			status[0x17] |= 0x04; /* Contain PWD */
-	} else {
-		status[0x17] = 0x00;
-	}
-
-	RTS51X_DEBUGP("status[0x17] = 0x%x\n", status[0x17]);
-#endif
 
 	/* Function 0
 	 * Support Magic Gate, CPRM and PhyRegister R/W */
@@ -1044,12 +898,6 @@ void rts51x_pp_status(struct rts51x_chip *chip, unsigned int lun, u8 *status,
 	 * Support OC LUN status & WP LUN status */
 	status[0x1A] = 0x28;
 
-	/* Function 7 */
-#ifdef SUPPORT_SD_LOCK
-	/* Support SD Lock/Unlock */
-	status[0x1F] = 0x01;
-#endif
-
 	/* Function 2
 	 * Support OC LUN status & WP LUN status */
 	status[0x1A] = 0x28;
diff --git a/drivers/staging/rts5139/rts51x_chip.h b/drivers/staging/rts5139/rts51x_chip.h
index 13fc2a410d9..6d395b6533a 100644
--- a/drivers/staging/rts5139/rts51x_chip.h
+++ b/drivers/staging/rts5139/rts51x_chip.h
@@ -39,12 +39,7 @@
 #define SUPPORT_CPRM
 #define SUPPORT_MAGIC_GATE
 #define SUPPORT_MSXC
-/* #define LED_AUTO_BLINK */
-
-/* { wwang, 2010-07-26
- * Add support for SD lock/unlock */
-/* #define SUPPORT_SD_LOCK */
-/* } wwang, 2010-07-26 */
+#define USING_POLLING_CYCLE_DELINK
 
 #ifdef SUPPORT_MAGIC_GA
 /* Using NORMAL_WRITE instead of AUTO_WRITE to set ICVTE */
@@ -63,7 +58,6 @@
 #define SUPPORT_OCP
 
 #define MS_SPEEDUP
-/* #define XD_SPEEDUP */
 
 #define SD_XD_IO_FOLLOW_PWR
 
@@ -81,7 +75,6 @@
 
 #define MAX_ALLOWED_LUN_CNT	8
 #define CMD_BUF_LEN		1024
-#define RSP_BUF_LEN		1024
 #define POLLING_INTERVAL	50	/* 50ms */
 
 #define XD_FREE_TABLE_CNT	1200
@@ -128,8 +121,6 @@
 #endif
 
 #define STATUS_FAIL		1
-#define STATUS_READ_FAIL	2
-#define STATUS_WRITE_FAIL	3
 #define STATUS_TIMEDOUT		4
 #define STATUS_NOMEM		5
 #define STATUS_TRANS_SHORT	6
@@ -139,8 +130,6 @@
 
 #define IDLE_MAX_COUNT		10
 #define POLLING_WAIT_CNT	1
-#define DELINK_DELAY		100
-#define LED_TOGGLE_INTERVAL	6
 #define LED_GPIO		0
 
 /* package */
@@ -157,8 +146,6 @@
 #define TRANSPORT_GOOD		0
 /* Transport good, command failed */
 #define TRANSPORT_FAILED	1
-/* Command failed, no auto-sense */
-#define TRANSPORT_NO_SENSE	2
 /* Transport bad (i.e. device dead) */
 #define TRANSPORT_ERROR		3
 
@@ -195,7 +182,6 @@ struct trace_msg_t {
 #define	SENSE_TYPE_MEDIA_INVALID_CMD_FIELD		6
 #define	SENSE_TYPE_MEDIA_UNRECOVER_READ_ERR		7
 #define	SENSE_TYPE_MEDIA_WRITE_ERR			8
-#define SENSE_TYPE_FORMAT_IN_PROGRESS			9
 #define SENSE_TYPE_FORMAT_CMD_FAILED			10
 #ifdef SUPPORT_MAGIC_GATE
 /* COPY PROTECTION KEY EXCHANGE FAILURE - KEY NOT ESTABLISHED */
@@ -207,83 +193,27 @@ struct trace_msg_t {
 /* WRITE ERROR */
 #define SENSE_TYPE_MG_WRITE_ERR				0x0e
 #endif
-#ifdef SUPPORT_SD_LOCK
-/* FOR Locked SD card */
-#define SENSE_TYPE_MEDIA_READ_FORBIDDEN			0x10
-#endif
 
 /*---- sense key ----*/
-#define ILI                     0x20	/* ILI bit is on                    */
-
-#define NO_SENSE                0x00	/* not exist sense key              */
-#define RECOVER_ERR             0x01	/* Target/Logical unit is recoverd  */
-#define NOT_READY               0x02	/* Logical unit is not ready        */
-#define MEDIA_ERR               0x03	/* medium/data error                */
-#define HARDWARE_ERR            0x04	/* hardware error                   */
 #define ILGAL_REQ               0x05	/* CDB/parameter/identify msg error */
-#define UNIT_ATTENTION          0x06	/* unit attention condition occur   */
-#define DAT_PRTCT               0x07	/* read/write is desable            */
-#define BLNC_CHK                0x08	/* find blank/DOF in read           */
-					/* write to unblank area            */
-#define CPY_ABRT                0x0a	/* Copy/Compare/Copy&Verify illgal  */
-#define ABRT_CMD                0x0b	/* Target make the command in error */
-#define EQUAL                   0x0c	/* Search Data end with Equal       */
-#define VLM_OVRFLW              0x0d	/* Some data are left in buffer     */
-#define MISCMP                  0x0e	/* find inequality                  */
 
 /*-----------------------------------
     SENSE_DATA
 -----------------------------------*/
-/*---- valid ----*/
-#define SENSE_VALID             0x80	/* Sense data is valid as SCSI2     */
-#define SENSE_INVALID           0x00	/* Sense data is invalid as SCSI2   */
 
 /*---- error code ----*/
 #define CUR_ERR                 0x70	/* current error                    */
-#define DEF_ERR                 0x71	/* specific command error           */
 
 /*---- sense key Infomation ----*/
-#define SNSKEYINFO_LEN          3	/* length of sense key infomation   */
 
 #define SKSV                    0x80
 #define CDB_ILLEGAL             0x40
-#define DAT_ILLEGAL             0x00
-#define BPV                     0x08
-#define BIT_ILLEGAL0            0	/* bit0 is illegal                  */
-#define BIT_ILLEGAL1            1	/* bit1 is illegal                  */
-#define BIT_ILLEGAL2            2	/* bit2 is illegal                  */
-#define BIT_ILLEGAL3            3	/* bit3 is illegal                  */
-#define BIT_ILLEGAL4            4	/* bit4 is illegal                  */
-#define BIT_ILLEGAL5            5	/* bit5 is illegal                  */
-#define BIT_ILLEGAL6            6	/* bit6 is illegal                  */
-#define BIT_ILLEGAL7            7	/* bit7 is illegal                  */
 
 /*---- ASC ----*/
-#define ASC_NO_INFO             0x00
-#define ASC_MISCMP              0x1d
 #define ASC_INVLD_CDB           0x24
-#define ASC_INVLD_PARA          0x26
-#define ASC_LU_NOT_READY	0x04
-#define ASC_WRITE_ERR           0x0c
-#define ASC_READ_ERR            0x11
-#define ASC_LOAD_EJCT_ERR       0x53
-#define	ASC_MEDIA_NOT_PRESENT	0x3A
-#define	ASC_MEDIA_CHANGED	0x28
-#define	ASC_MEDIA_IN_PROCESS	0x04
-#define	ASC_WRITE_PROTECT	0x27
-#define ASC_LUN_NOT_SUPPORTED	0x25
 
 /*---- ASQC ----*/
-#define ASCQ_NO_INFO            0x00
-#define	ASCQ_MEDIA_IN_PROCESS	0x01
-#define ASCQ_MISCMP             0x00
 #define ASCQ_INVLD_CDB          0x00
-#define ASCQ_INVLD_PARA         0x02
-#define ASCQ_LU_NOT_READY	0x02
-#define ASCQ_WRITE_ERR          0x02
-#define ASCQ_READ_ERR           0x00
-#define ASCQ_LOAD_EJCT_ERR      0x00
-#define	ASCQ_WRITE_PROTECT	0x00
 
 struct sense_data_t {
 	unsigned char err_code;	/* error code */
@@ -296,13 +226,13 @@ struct sense_data_t {
 	unsigned char seg_no;	/* segment No.                      */
 	unsigned char sense_key;	/* byte5 : ILI                      */
 	/* bit3-0 : sense key              */
-	unsigned char info[4];	/* infomation                       */
+	unsigned char info[4];	/* information                       */
 	unsigned char ad_sense_len;	/* additional sense data length     */
-	unsigned char cmd_info[4];	/* command specific infomation      */
+	unsigned char cmd_info[4];	/* command specific information      */
 	unsigned char asc;	/* ASC                              */
 	unsigned char ascq;	/* ASCQ                             */
 	unsigned char rfu;	/* FRU                              */
-	unsigned char sns_key_info[3];	/* sense key specific infomation    */
+	unsigned char sns_key_info[3];	/* sense key specific information    */
 };
 
 /* sd_ctl bit map */
@@ -323,8 +253,6 @@ struct sense_data_t {
 #define SUPPORT_UHS50_MMC44		0x40
 
 struct rts51x_option {
-	u8 led_blink_speed;
-
 	int mspro_formatter_enable;
 
 	/* card clock expected by user for fpga platform */
@@ -368,8 +296,6 @@ struct rts51x_option {
 	int ss_en;
 	/* Interval to enter SS from IDLE state (second) */
 	int ss_delay;
-	int needs_remote_wakeup;
-	u8 ww_enable;	/* sangdy2010-08-03:add for remote wakeup */
 
 	/* Enable SSC clock */
 	int ssc_en;
@@ -392,10 +318,7 @@ struct rts51x_option {
 	/*if reset or rw fail,then set SD20 pad drive again */
 	u8 reset_or_rw_fail_set_pad_drive;
 
-	u8 rcc_fail_flag;	/* add to indicate whether rcc bug happen */
-	u8 rcc_bug_fix_en;	/* if set,then support fixing rcc bug */
 	u8 debounce_num;	/* debounce number */
-	int polling_time;	/* polling delay time */
 	u8 led_toggle_interval;	/* used to control led toggle speed */
 	int xd_rwn_step;
 	u8 sd_send_status_en;
@@ -405,7 +328,7 @@ struct rts51x_option {
 	u8 ddr50_rx_phase;
 	u8 sdr50_tx_phase;
 	u8 sdr50_rx_phase;
-	/* used to enable select sdr50 tx phase according to  proportion. */
+	/* used to enable select sdr50 tx phase according to proportion. */
 	u8 sdr50_phase_sel;
 	u8 ms_errreg_fix;
 	u8 reset_mmc_first;
@@ -614,11 +537,6 @@ struct sd_info {
 	u8 sd_reset_fail;	/* sangdy2010-07-01 */
 	u8 sd_send_status_en;
 
-#ifdef SUPPORT_SD_LOCK
-	u8 sd_lock_status;
-	u8 sd_erase_status;
-	u8 sd_lock_notify;
-#endif
 };
 
 #define MODE_512_SEQ		0x01
@@ -720,9 +638,8 @@ struct rts51x_chip {
 	struct scsi_cmnd *srb;
 	struct sense_data_t sense_buffer[MAX_ALLOWED_LUN_CNT];
 
-#ifndef LED_AUTO_BLINK
 	int led_toggle_counter;
-#endif
+
 	int ss_counter;
 	int idle_counter;
 	int auto_delink_counter;
diff --git a/drivers/staging/rts5139/rts51x_fop.c b/drivers/staging/rts5139/rts51x_fop.c
index 6eaebb6223c..ef893c8cdec 100644
--- a/drivers/staging/rts5139/rts51x_fop.c
+++ b/drivers/staging/rts5139/rts51x_fop.c
@@ -234,12 +234,7 @@ ssize_t rts51x_write(struct file *filp, const char __user *buf, size_t count,
 	return 0;
 }
 
-#if 0 /* LINUX_VERSION_CODE < KERNEL_VERSION(2, 6, 36) */
-int rts51x_ioctl(struct inode *inode, struct file *filp, unsigned int cmd,
-		 unsigned long arg)
-#else
 long rts51x_ioctl(struct file *filp, unsigned int cmd, unsigned long arg)
-#endif
 {
 	struct rts51x_chip *chip;
 	struct sd_direct_cmnd cmnd;
diff --git a/drivers/staging/rts5139/rts51x_fop.h b/drivers/staging/rts5139/rts51x_fop.h
index 94d75f08d25..eb45acf50d1 100644
--- a/drivers/staging/rts5139/rts51x_fop.h
+++ b/drivers/staging/rts5139/rts51x_fop.h
@@ -50,12 +50,7 @@ ssize_t rts51x_read(struct file *filp, char __user *buf, size_t count,
 		    loff_t *f_pos);
 ssize_t rts51x_write(struct file *filp, const char __user *buf, size_t count,
 		     loff_t *f_pos);
-#if 0 /* LINUX_VERSION_CODE < KERNEL_VERSION(2, 6, 36) */
-int rts51x_ioctl(struct inode *inode, struct file *filp, unsigned int cmd,
-		 unsigned long arg);
-#else
 long rts51x_ioctl(struct file *filp, unsigned int cmd, unsigned long arg);
-#endif
 
 #endif
 
diff --git a/drivers/staging/rts5139/rts51x_scsi.c b/drivers/staging/rts5139/rts51x_scsi.c
index 87c9cdc8bd2..e07a1f4f58c 100644
--- a/drivers/staging/rts5139/rts51x_scsi.c
+++ b/drivers/staging/rts5139/rts51x_scsi.c
@@ -40,7 +40,6 @@
 #include "rts51x_scsi.h"
 #include "rts51x_card.h"
 #include "rts51x_transport.h"
-#include "rts51x_sys.h"
 #include "sd_cprm.h"
 #include "ms_mg.h"
 #include "trace.h"
@@ -370,10 +369,6 @@ void set_sense_type(struct rts51x_chip *chip, unsigned int lun, int sense_type)
 			       ASC_INVLD_CDB, ASCQ_INVLD_CDB, CDB_ILLEGAL, 1);
 		break;
 
-	case SENSE_TYPE_FORMAT_IN_PROGRESS:
-		set_sense_data(chip, lun, CUR_ERR, 0x02, 0, 0x04, 0x04, 0, 0);
-		break;
-
 	case SENSE_TYPE_FORMAT_CMD_FAILED:
 		set_sense_data(chip, lun, CUR_ERR, 0x03, 0, 0x31, 0x01, 0, 0);
 		break;
@@ -396,12 +391,6 @@ void set_sense_type(struct rts51x_chip *chip, unsigned int lun, int sense_type)
 		break;
 #endif
 
-#ifdef SUPPORT_SD_LOCK
-	case SENSE_TYPE_MEDIA_READ_FORBIDDEN:
-		set_sense_data(chip, lun, CUR_ERR, 0x07, 0, 0x11, 0x13, 0, 0);
-		break;
-#endif
-
 	case SENSE_TYPE_NO_SENSE:
 	default:
 		set_sense_data(chip, lun, CUR_ERR, 0, 0, 0, 0, 0, 0);
@@ -448,20 +437,6 @@ static int test_unit_ready(struct scsi_cmnd *srb, struct rts51x_chip *chip)
 		set_sense_type(chip, lun, SENSE_TYPE_MEDIA_CHANGE);
 		return TRANSPORT_FAILED;
 	}
-#ifdef SUPPORT_SD_LOCK
-	if (get_lun_card(chip, SCSI_LUN(srb)) == SD_CARD) {
-		struct sd_info *sd_card = &(chip->sd_card);
-		if (sd_card->sd_lock_notify) {
-			sd_card->sd_lock_notify = 0;
-			set_sense_type(chip, lun, SENSE_TYPE_MEDIA_CHANGE);
-			return TRANSPORT_FAILED;
-		} else if (sd_card->sd_lock_status & SD_LOCKED) {
-			set_sense_type(chip, lun,
-				       SENSE_TYPE_MEDIA_READ_FORBIDDEN);
-			return TRANSPORT_FAILED;
-		}
-	}
-#endif
 
 	return TRANSPORT_GOOD;
 }
@@ -797,9 +772,6 @@ static int request_sense(struct scsi_cmnd *srb, struct rts51x_chip *chip)
 
 static int read_write(struct scsi_cmnd *srb, struct rts51x_chip *chip)
 {
-#ifdef SUPPORT_SD_LOCK
-	struct sd_info *sd_card = &(chip->sd_card);
-#endif
 	unsigned int lun = SCSI_LUN(srb);
 	int retval;
 	u32 start_sec;
@@ -819,25 +791,6 @@ static int read_write(struct scsi_cmnd *srb, struct rts51x_chip *chip)
 	rts51x_prepare_run(chip);
 	RTS51X_SET_STAT(chip, STAT_RUN);
 
-#ifdef SUPPORT_SD_LOCK
-	if (sd_card->sd_erase_status) {
-		/* Accessing to any card is forbidden
-		 * until the erase procedure of SD is completed */
-		RTS51X_DEBUGP("SD card being erased!\n");
-		set_sense_type(chip, lun, SENSE_TYPE_MEDIA_READ_FORBIDDEN);
-		TRACE_RET(chip, TRANSPORT_FAILED);
-	}
-
-	if (get_lun_card(chip, lun) == SD_CARD) {
-		if (sd_card->sd_lock_status & SD_LOCKED) {
-			RTS51X_DEBUGP("SD card locked!\n");
-			set_sense_type(chip, lun,
-				       SENSE_TYPE_MEDIA_READ_FORBIDDEN);
-			TRACE_RET(chip, TRANSPORT_FAILED);
-		}
-	}
-#endif
-
 	if ((srb->cmnd[0] == READ_10) || (srb->cmnd[0] == WRITE_10)) {
 		start_sec =
 		    ((u32) srb->cmnd[2] << 24) |
@@ -883,20 +836,12 @@ static int read_write(struct scsi_cmnd *srb, struct rts51x_chip *chip)
 
 	retval = card_rw(srb, chip, start_sec, sec_cnt);
 	if (retval != STATUS_SUCCESS) {
-#if 0
-		if (chip->need_release & chip->lun2card[lun]) {
-			set_sense_type(chip, lun, SENSE_TYPE_MEDIA_NOT_PRESENT);
-		} else {
-#endif
 		if (srb->sc_data_direction == DMA_FROM_DEVICE) {
 			set_sense_type(chip, lun,
 				       SENSE_TYPE_MEDIA_UNRECOVER_READ_ERR);
 		} else {
 			set_sense_type(chip, lun, SENSE_TYPE_MEDIA_WRITE_ERR);
 		}
-#if 0
-		}
-#endif
 		TRACE_RET(chip, TRANSPORT_FAILED);
 	}
 
@@ -1516,7 +1461,7 @@ static int ms_format_cmnd(struct scsi_cmnd *srb, struct rts51x_chip *chip)
 }
 
 #ifdef SUPPORT_PCGL_1P18
-int get_ms_information(struct scsi_cmnd *srb, struct rts51x_chip *chip)
+static int get_ms_information(struct scsi_cmnd *srb, struct rts51x_chip *chip)
 {
 	struct ms_info *ms_card = &(chip->ms_card);
 	unsigned int lun = SCSI_LUN(srb);
@@ -1677,7 +1622,7 @@ static int sd_extention_cmnd(struct scsi_cmnd *srb, struct rts51x_chip *chip)
 #endif
 
 #ifdef SUPPORT_MAGIC_GATE
-int mg_report_key(struct scsi_cmnd *srb, struct rts51x_chip *chip)
+static int mg_report_key(struct scsi_cmnd *srb, struct rts51x_chip *chip)
 {
 	struct ms_info *ms_card = &(chip->ms_card);
 	unsigned int lun = SCSI_LUN(srb);
@@ -1764,7 +1709,7 @@ int mg_report_key(struct scsi_cmnd *srb, struct rts51x_chip *chip)
 	return TRANSPORT_GOOD;
 }
 
-int mg_send_key(struct scsi_cmnd *srb, struct rts51x_chip *chip)
+static int mg_send_key(struct scsi_cmnd *srb, struct rts51x_chip *chip)
 {
 	struct ms_info *ms_card = &(chip->ms_card);
 	unsigned int lun = SCSI_LUN(srb);
@@ -1871,30 +1816,10 @@ int mg_send_key(struct scsi_cmnd *srb, struct rts51x_chip *chip)
 
 int rts51x_scsi_handler(struct scsi_cmnd *srb, struct rts51x_chip *chip)
 {
-#ifdef SUPPORT_SD_LOCK
-	struct sd_info *sd_card = &(chip->sd_card);
-#endif
 	struct ms_info *ms_card = &(chip->ms_card);
 	unsigned int lun = SCSI_LUN(srb);
 	int result = TRANSPORT_GOOD;
 
-#ifdef SUPPORT_SD_LOCK
-	if (sd_card->sd_erase_status) {
-		/* Block all SCSI command except for REQUEST_SENSE
-		 * and rs_ppstatus */
-		if (!
-		    ((srb->cmnd[0] == VENDOR_CMND)
-		     && (srb->cmnd[1] == SCSI_APP_CMD)
-		     && (srb->cmnd[2] == GET_DEV_STATUS))
-		    && (srb->cmnd[0] != REQUEST_SENSE)) {
-			/* Logical Unit Not Ready Format in Progress */
-			set_sense_data(chip, lun, CUR_ERR, 0x02, 0, 0x04, 0x04,
-				       0, 0);
-			TRACE_RET(chip, TRANSPORT_FAILED);
-		}
-	}
-#endif
-
 	if ((get_lun_card(chip, lun) == MS_CARD) &&
 	    (ms_card->format_status == FORMAT_IN_PROGRESS)) {
 		if ((srb->cmnd[0] != REQUEST_SENSE)
@@ -1994,11 +1919,6 @@ int rts51x_scsi_handler(struct scsi_cmnd *srb, struct rts51x_chip *chip)
  * Host functions
  ***********************************************************************/
 
-const char *host_info(struct Scsi_Host *host)
-{
-	return "SCSI emulation for RTS51xx USB driver-based card reader";
-}
-
 int slave_alloc(struct scsi_device *sdev)
 {
 	/*
@@ -2111,14 +2031,7 @@ int queuecommand_lck(struct scsi_cmnd *srb, void (*done) (struct scsi_cmnd *))
 	return 0;
 }
 
-#if 0 /* LINUX_VERSION_CODE < KERNEL_VERSION(2, 6, 37) */
-int queuecommand(struct scsi_cmnd *srb, void (*done) (struct scsi_cmnd *))
-{
-	return queuecommand_lck(srb, done);
-}
-#else
 DEF_SCSI_QCMD(queuecommand)
-#endif
 /***********************************************************************
  * Error handling functions
  ***********************************************************************/
diff --git a/drivers/staging/rts5139/rts51x_scsi.h b/drivers/staging/rts5139/rts51x_scsi.h
index 3a8ca069b27..060d2c2e77e 100644
--- a/drivers/staging/rts5139/rts51x_scsi.h
+++ b/drivers/staging/rts5139/rts51x_scsi.h
@@ -145,16 +145,11 @@ struct Scsi_Host;
 struct scsi_device;
 struct scsi_cmnd;
 
-const char *host_info(struct Scsi_Host *host);
 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);
-#if 0 /* LINUX_VERSION_CODE < KERNEL_VERSION(2, 6, 37) */
-int queuecommand(struct scsi_cmnd *srb, void (*done) (struct scsi_cmnd *));
-#else
 int queuecommand(struct Scsi_Host *, struct scsi_cmnd *);
-#endif
 int command_abort(struct scsi_cmnd *srb);
 int device_reset(struct scsi_cmnd *srb);
 int bus_reset(struct scsi_cmnd *srb);
diff --git a/drivers/staging/rts5139/rts51x_sys.h b/drivers/staging/rts5139/rts51x_sys.h
deleted file mode 100644
index b09cd34a6c0..00000000000
--- a/drivers/staging/rts5139/rts51x_sys.h
+++ /dev/null
@@ -1,54 +0,0 @@
-/* Driver for Realtek USB RTS51xx card reader
- * Header file
- *
- * Copyright(c) 2009 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:
- *   wwang (wei_wang@realsil.com.cn)
- *   No. 450, Shenhu Road, Suzhou Industry Park, Suzhou, China
- * Maintainer:
- *   Edwin Rong (edwin_rong@realsil.com.cn)
- *   No. 450, Shenhu Road, Suzhou Industry Park, Suzhou, China
- */
-
-#ifndef __RTS51X_SYS_H
-#define __RTS51X_SYS_H
-
-#include "rts51x.h"
-#include "rts51x_chip.h"
-#include "rts51x_card.h"
-
-#define USING_POLLING_CYCLE_DELINK
-
-extern int  rts51x_check_start_time(struct rts51x_chip *chip);
-extern void rts51x_set_start_time(struct rts51x_chip *chip);
-extern void rts51x_clear_start_time(struct rts51x_chip *chip);
-
-/* typedef dma_addr_t ULONG_PTR; */
-
-static inline void rts51x_reset_detected_cards(struct rts51x_chip *chip)
-{
-/*      rts51x_reset_cards(chip); */
-}
-
-static inline void clear_first_install_mark(struct rts51x_chip *chip)
-{
-}
-
-void rts51x_enter_ss(struct rts51x_chip *chip);
-void rts51x_exit_ss(struct rts51x_chip *chip);
-
-#endif /* __RTS51X_SYS_H */
diff --git a/drivers/staging/rts5139/rts51x_transport.c b/drivers/staging/rts5139/rts51x_transport.c
index da9c83b4942..89e4d805a34 100644
--- a/drivers/staging/rts5139/rts51x_transport.c
+++ b/drivers/staging/rts5139/rts51x_transport.c
@@ -120,7 +120,7 @@ unsigned int rts51x_access_sglist(unsigned char *buffer,
 	return cnt;
 }
 
-unsigned int rts51x_access_xfer_buf(unsigned char *buffer,
+static unsigned int rts51x_access_xfer_buf(unsigned char *buffer,
 				    unsigned int buflen, struct scsi_cmnd *srb,
 				    struct scatterlist **sgptr,
 				    unsigned int *offset, enum xfer_buf_dir dir)
@@ -252,6 +252,8 @@ static int rts51x_msg_common(struct rts51x_chip *chip, struct urb *urb,
 	return status;
 }
 
+static int rts51x_clear_halt(struct rts51x_chip *chip, unsigned int pipe);
+
 /*
  * Interpret the results of a URB transfer
  */
@@ -359,7 +361,7 @@ int rts51x_ctrl_transfer(struct rts51x_chip *chip, unsigned int pipe,
 				    rts51x->current_urb->actual_length);
 }
 
-int rts51x_clear_halt(struct rts51x_chip *chip, unsigned int pipe)
+static int rts51x_clear_halt(struct rts51x_chip *chip, unsigned int pipe)
 {
 	int result;
 	int endp = usb_pipeendpoint(pipe);
@@ -378,11 +380,6 @@ int rts51x_clear_halt(struct rts51x_chip *chip, unsigned int pipe)
 	return STATUS_SUCCESS;
 }
 
-int rts51x_reset_pipe(struct rts51x_chip *chip, char pipe)
-{
-	return rts51x_clear_halt(chip, pipe);
-}
-
 static void rts51x_sg_clean(struct usb_sg_request *io)
 {
 	if (io->urbs) {
@@ -391,226 +388,17 @@ static void rts51x_sg_clean(struct usb_sg_request *io)
 		kfree(io->urbs);
 		io->urbs = NULL;
 	}
-#if 0 /* LINUX_VERSION_CODE < KERNEL_VERSION(2, 6, 35) */
-	if (io->dev->dev.dma_mask != NULL)
-		usb_buffer_unmap_sg(io->dev, usb_pipein(io->pipe),
-				    io->sg, io->nents);
-#endif
 	io->dev = NULL;
 }
-#if 0
-static void rts51x_sg_complete(struct urb *urb)
-{
-	struct usb_sg_request *io = urb->context;
-	int status = urb->status;
-
-	spin_lock(&io->lock);
-
-	/* In 2.5 we require hcds' endpoint queues not to progress after fault
-	* reports, until the completion callback (this!) returns.  That lets
-	* device driver code (like this routine) unlink queued urbs first,
-	* if it needs to, since the HC won't work on them at all.  So it's
-	* not possible for page N+1 to overwrite page N, and so on.
-	*
-	* That's only for "hard" faults; "soft" faults (unlinks) sometimes
-	* complete before the HCD can get requests away from hardware,
-	* though never during cleanup after a hard fault.
-	*/
-	if (io->status
-		&& (io->status != -ECONNRESET
-		|| status != -ECONNRESET)
-		&& urb->actual_length) {
-			dev_err(io->dev->bus->controller,
-				"dev %s ep%d%s scatterlist error %d/%d\n",
-				io->dev->devpath,
-				usb_endpoint_num(&urb->ep->desc),
-				usb_urb_dir_in(urb) ? "in" : "out",
-				status, io->status);
-			/* BUG (); */
-	}
-
-	if (io->status == 0 && status && status != -ECONNRESET) {
-		int i, found, retval;
-
-		io->status = status;
-
-		/* the previous urbs, and this one, completed already.
-		* unlink pending urbs so they won't rx/tx bad data.
-		* careful: unlink can sometimes be synchronous...
-		*/
-		spin_unlock(&io->lock);
-		for (i = 0, found = 0; i < io->entries; i++) {
-			if (!io->urbs[i] || !io->urbs[i]->dev)
-				continue;
-			if (found) {
-				retval = usb_unlink_urb(io->urbs[i]);
-				if (retval != -EINPROGRESS &&
-					retval != -ENODEV &&
-					retval != -EBUSY)
-					dev_err(&io->dev->dev,
-						"%s, unlink --> %d\n",
-						__func__, retval);
-			} else if (urb == io->urbs[i])
-				found = 1;
-		}
-		spin_lock(&io->lock);
-	}
-	urb->dev = NULL;
-
-	/* on the last completion, signal usb_sg_wait() */
-	io->bytes += urb->actual_length;
-	io->count--;
-	if (!io->count)
-		complete(&io->complete);
-
-	spin_unlock(&io->lock);
-}
-
-/* This function is ported from usb_sg_init, which can transfer
- * sg list partially */
-int rts51x_sg_init_partial(struct usb_sg_request *io, struct usb_device *dev,
-	unsigned pipe, unsigned period, void *buf, struct scatterlist **sgptr,
-	unsigned int *offset, int nents, size_t length, gfp_t mem_flags)
-{
-	int i;
-	int urb_flags;
-	int dma;
-	struct scatterlist *sg = *sgptr, *first_sg;
-
-	first_sg = (struct scatterlist *)buf;
-	if (!sg)
-		sg = first_sg;
-
-	if (!io || !dev || !sg
-		|| usb_pipecontrol(pipe)
-		|| usb_pipeisoc(pipe)
-		|| (nents <= 0))
-		return -EINVAL;
-
-	spin_lock_init(&io->lock);
-	io->dev = dev;
-	io->pipe = pipe;
-	io->sg = first_sg;  /* used by unmap */
-	io->nents = nents;
-
-	RTS51X_DEBUGP("Before map, sg address: 0x%x\n", (unsigned int)sg);
-	RTS51X_DEBUGP("Before map, dev address: 0x%x\n", (unsigned int)dev);
-
-	/* not all host controllers use DMA (like the mainstream pci ones);
-	* they can use PIO (sl811) or be software over another transport.
-	*/
-	dma = (dev->dev.dma_mask != NULL);
-	if (dma) {
-		/* map the whole sg list, because here we only know the
-		 * total nents */
-		io->entries = usb_buffer_map_sg(dev, usb_pipein(pipe),
-		first_sg, nents);
-	} else {
-		io->entries = nents;
-	}
-
-	/* initialize all the urbs we'll use */
-	if (io->entries <= 0)
-		return io->entries;
-
-	io->urbs = kmalloc(io->entries * sizeof *io->urbs, mem_flags);
-	if (!io->urbs)
-		goto nomem;
-
-	urb_flags = URB_NO_INTERRUPT;
-	if (dma)
-		urb_flags |= URB_NO_TRANSFER_DMA_MAP;
-	if (usb_pipein(pipe))
-		urb_flags |= URB_SHORT_NOT_OK;
-
-	RTS51X_DEBUGP("io->entries = %d\n", io->entries);
-
-	for (i = 0; (sg != NULL) && (length > 0); i++) {
-		unsigned len;
-
-		RTS51X_DEBUGP("sg address: 0x%x\n", (unsigned int)sg);
-		RTS51X_DEBUGP("length = %d, *offset = %d\n", length, *offset);
-
-		io->urbs[i] = usb_alloc_urb(0, mem_flags);
-		if (!io->urbs[i]) {
-			io->entries = i;
-			goto nomem;
-		}
-
-		io->urbs[i]->dev = NULL;
-		io->urbs[i]->pipe = pipe;
-		io->urbs[i]->interval = period;
-		io->urbs[i]->transfer_flags = urb_flags;
-
-		io->urbs[i]->complete = rts51x_sg_complete;
-		io->urbs[i]->context = io;
-
-		if (dma) {
-			io->urbs[i]->transfer_dma =
-				sg_dma_address(sg) + *offset;
-			len = sg_dma_len(sg) - *offset;
-			io->urbs[i]->transfer_buffer = NULL;
-			RTS51X_DEBUGP(" -- sg entry dma length = %d\n",
-						sg_dma_len(sg));
-		} else {
-			/* hc may use _only_ transfer_buffer */
-			io->urbs[i]->transfer_buffer = sg_virt(sg) + *offset;
-			len = sg->length - *offset;
-			RTS51X_DEBUGP(" -- sg entry length = %d\n",
-						sg->length);
-		}
-
-		if (length >= len) {
-			*offset = 0;
-			io->urbs[i]->transfer_buffer_length = len;
-			length -= len;
-			sg = sg_next(sg);
-		} else {
-			*offset += length;
-			io->urbs[i]->transfer_buffer_length = length;
-			length = 0;
-		}
-		if (length == 0)
-			io->entries = i + 1;
-#if 0
-		if (length) {
-			len = min_t(unsigned, len, length);
-			length -= len;
-			if (length == 0) {
-				io->entries = i + 1;
-				*offset += len;
-			} else {
-				*offset = 0;
-			}
-		}
-#endif
-	}
-	RTS51X_DEBUGP("In %s, urb count: %d\n", __func__, i);
-	io->urbs[--i]->transfer_flags &= ~URB_NO_INTERRUPT;
-
-	RTS51X_DEBUGP("sg address stored in sgptr: 0x%x\n", (unsigned int)sg);
-	*sgptr = sg;
-
-	/* transaction state */
-	io->count = io->entries;
-	io->status = 0;
-	io->bytes = 0;
-	init_completion(&io->complete);
-	return 0;
 
-nomem:
-	rts51x_sg_clean(io);
-	return -ENOMEM;
-}
-#endif
-int rts51x_sg_init(struct usb_sg_request *io, struct usb_device *dev,
+static int rts51x_sg_init(struct usb_sg_request *io, struct usb_device *dev,
 		   unsigned pipe, unsigned period, struct scatterlist *sg,
 		   int nents, size_t length, gfp_t mem_flags)
 {
 	return usb_sg_init(io, dev, pipe, period, sg, nents, length, mem_flags);
 }
 
-int rts51x_sg_wait(struct usb_sg_request *io, int timeout)
+static int rts51x_sg_wait(struct usb_sg_request *io, int timeout)
 {
 	long timeleft;
 	int i;
@@ -630,7 +418,7 @@ int rts51x_sg_wait(struct usb_sg_request *io, int timeout)
 		 */
 		spin_unlock_irq(&io->lock);
 		switch (retval) {
-			/* maybe we retrying will recover */
+			/* maybe the retry will recover */
 		case -ENXIO:	/* hc didn't queue this one */
 		case -EAGAIN:
 		case -ENOMEM:
@@ -740,56 +528,9 @@ static int rts51x_bulk_transfer_sglist(struct rts51x_chip *chip,
 	return interpret_urb_result(chip, pipe, length, result,
 				    chip->usb->current_sg.bytes);
 }
-#if 0
-static int rts51x_bulk_transfer_sglist_partial(struct rts51x_chip *chip,
-		unsigned int pipe, void *buf, struct scatterlist **sgptr,
-		unsigned int *offset, int num_sg, unsigned int length,
-		unsigned int *act_len, int timeout)
-{
-	int result;
-
-	/* don't submit s-g requests during abort processing */
-	if (test_bit(FLIDX_ABORTING, &chip->usb->dflags))
-		TRACE_RET(chip, STATUS_ERROR);
 
-	/* initialize the scatter-gather request block */
-	RTS51X_DEBUGP("%s: xfer %u bytes, %d entries\n", __func__,
-			length, num_sg);
-	result = rts51x_sg_init_partial(&chip->usb->current_sg,
-			chip->usb->pusb_dev, pipe, 0, buf, sgptr, offset,
-			num_sg, length, GFP_NOIO);
-	if (result) {
-		RTS51X_DEBUGP("rts51x_sg_init_partial returned %d\n", result);
-		TRACE_RET(chip, STATUS_ERROR);
-	}
-
-	/* since the block has been initialized successfully, it's now
-	 * okay to cancel it */
-	set_bit(FLIDX_SG_ACTIVE, &chip->usb->dflags);
-
-	/* did an abort occur during the submission? */
-	if (test_bit(FLIDX_ABORTING, &chip->usb->dflags)) {
-
-		/* cancel the request, if it hasn't been cancelled already */
-		if (test_and_clear_bit(FLIDX_SG_ACTIVE, &chip->usb->dflags)) {
-			RTS51X_DEBUGP("-- cancelling sg request\n");
-			usb_sg_cancel(&chip->usb->current_sg);
-		}
-	}
-
-	/* wait for the completion of the transfer */
-	result = rts51x_sg_wait(&chip->usb->current_sg, timeout);
-
-	clear_bit(FLIDX_SG_ACTIVE, &chip->usb->dflags);
-
-	/* result = us->current_sg.status; */
-	if (act_len)
-		*act_len = chip->usb->current_sg.bytes;
-	return interpret_urb_result(chip, pipe, length, result,
-		chip->usb->current_sg.bytes);
-}
-#endif
-int rts51x_bulk_transfer_buf(struct rts51x_chip *chip, unsigned int pipe,
+static int rts51x_bulk_transfer_buf(struct rts51x_chip *chip,
+			     unsigned int pipe,
 			     void *buf, unsigned int length,
 			     unsigned int *act_len, int timeout)
 {
@@ -860,11 +601,6 @@ int rts51x_transfer_data_partial(struct rts51x_chip *chip, unsigned int pipe,
 		}
 
 		kfree(tmp_buf);
-#if 0
-		result = rts51x_bulk_transfer_sglist_partial(chip, pipe, buf,
-					(struct scatterlist **)ptr, offset,
-					use_sg, len, act_len, timeout);
-#endif
 	} else {
 		unsigned int step = 0;
 		if (offset)
diff --git a/drivers/staging/rts5139/rts51x_transport.h b/drivers/staging/rts5139/rts51x_transport.h
index 9dd556ea9c0..024f115540a 100644
--- a/drivers/staging/rts5139/rts51x_transport.h
+++ b/drivers/staging/rts5139/rts51x_transport.h
@@ -40,11 +40,6 @@ unsigned int rts51x_access_sglist(unsigned char *buffer,
 				  unsigned int buflen, void *sglist,
 				  void **sgptr, unsigned int *offset,
 				  enum xfer_buf_dir dir);
-unsigned int rts51x_access_xfer_buf(unsigned char *buffer, unsigned int buflen,
-				    struct scsi_cmnd *srb,
-				    struct scatterlist **sgptr,
-				    unsigned int *offset,
-				    enum xfer_buf_dir dir);
 void rts51x_set_xfer_buf(unsigned char *buffer, unsigned int buflen,
 			 struct scsi_cmnd *srb);
 void rts51x_get_xfer_buf(unsigned char *buffer, unsigned int buflen,
@@ -53,7 +48,6 @@ void rts51x_get_xfer_buf(unsigned char *buffer, unsigned int buflen,
 int rts51x_ctrl_transfer(struct rts51x_chip *chip, unsigned int pipe,
 			 u8 request, u8 requesttype, u16 value, u16 index,
 			 void *data, u16 size, int timeout);
-int rts51x_clear_halt(struct rts51x_chip *chip, unsigned int pipe);
 int rts51x_transfer_data(struct rts51x_chip *chip, unsigned int pipe,
 			 void *buf, unsigned int len, int use_sg,
 			 unsigned int *act_len, int timeout);
@@ -62,12 +56,6 @@ int rts51x_transfer_data_partial(struct rts51x_chip *chip, unsigned int pipe,
 				 unsigned int len, int use_sg,
 				 unsigned int *act_len, int timeout);
 
-/* whichPipe:
- * 0: bulk in pipe
- * 1: bulk out pipe
- * 2: intr  in pipe */
-int rts51x_reset_pipe(struct rts51x_chip *chip, char pipe);
-
 #ifndef POLLING_IN_THREAD
 int rts51x_start_epc_transfer(struct rts51x_chip *chip);
 void rts51x_cancel_epc_transfer(struct rts51x_chip *chip);
diff --git a/drivers/staging/rts5139/sd.c b/drivers/staging/rts5139/sd.c
index d5dd2f926d1..b739f26f78c 100644
--- a/drivers/staging/rts5139/sd.c
+++ b/drivers/staging/rts5139/sd.c
@@ -246,12 +246,7 @@ RTY_SEND_CMD:
 				if (buf[1] & 0x80)
 					TRACE_RET(chip, STATUS_FAIL);
 			}
-#ifdef SUPPORT_SD_LOCK
-			/* exclude bit25 CARD_IS_LOCKED */
-			if (buf[1] & 0x7D) {
-#else
 			if (buf[1] & 0x7F) {
-#endif
 				RTS51X_DEBUGP("buf[1]: 0x%02x\n", buf[1]);
 				TRACE_RET(chip, STATUS_FAIL);
 			}
@@ -709,37 +704,7 @@ int sd_select_card(struct rts51x_chip *chip, int select)
 	return STATUS_SUCCESS;
 }
 
-#ifdef SUPPORT_SD_LOCK
-int sd_update_lock_status(struct rts51x_chip *chip)
-{
-	struct sd_info *sd_card = &(chip->sd_card);
-	int retval;
-	u8 rsp[5];
-
-	retval =
-	    sd_send_cmd_get_rsp(chip, SEND_STATUS, sd_card->sd_addr,
-				SD_RSP_TYPE_R1, rsp, 5);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, STATUS_FAIL);
-
-	if (rsp[1] & 0x02)
-		sd_card->sd_lock_status |= SD_LOCKED;
-	else
-		sd_card->sd_lock_status &= ~SD_LOCKED;
-
-	RTS51X_DEBUGP("sd_card->sd_lock_status = 0x%x\n",
-		       sd_card->sd_lock_status);
-
-	if (rsp[1] & 0x01) {
-		/* LOCK_UNLOCK_FAILED */
-		TRACE_RET(chip, STATUS_FAIL);
-	}
-
-	return STATUS_SUCCESS;
-}
-#endif
-
-int sd_wait_currentstate_dataready(struct rts51x_chip *chip, u8 statechk,
+static int sd_wait_currentstate_dataready(struct rts51x_chip *chip, u8 statechk,
 				   u8 rdychk, u16 pollingcnt)
 {
 	struct sd_info *sd_card = &(chip->sd_card);
@@ -1197,15 +1162,6 @@ static int sd_switch_function(struct rts51x_chip *chip, u8 bus_width)
 	RTS51X_DEBUGP("SD_FUNC_GROUP_1: func_to_switch = 0x%02x",
 		       func_to_switch);
 
-#ifdef SUPPORT_SD_LOCK
-	if ((sd_card->sd_lock_status & SD_SDR_RST)
-	    && (DDR50_SUPPORT == func_to_switch)
-	    && (sd_card->func_group1_mask & SDR50_SUPPORT_MASK)) {
-		func_to_switch = SDR50_SUPPORT;
-		RTS51X_DEBUGP("Using SDR50 instead of DDR50 for SD Lock\n");
-	}
-#endif
-
 	if (func_to_switch) {
 		retval =
 		    sd_check_switch(chip, SD_FUNC_GROUP_1, func_to_switch,
@@ -1562,7 +1518,7 @@ static u8 sd_search_final_phase(struct rts51x_chip *chip, u32 phase_map,
 	}
 
 Search_Finish:
-	RTS51X_DEBUGP("Final choosen phase: %d\n", final_phase);
+	RTS51X_DEBUGP("Final chosen phase: %d\n", final_phase);
 	return final_phase;
 }
 
@@ -2024,10 +1980,6 @@ Switch_Fail:
 	k = 0;
 	hi_cap_flow = 0;
 	support_1v8 = 0;
-#ifdef SUPPORT_SD_LOCK
-	if (sd_card->sd_lock_status & SD_UNLOCK_POW_ON)
-		goto SD_UNLOCK_ENTRY;
-#endif
 
 	retval = sd_prepare_reset(chip);
 	if (retval != STATUS_SUCCESS)
@@ -2182,7 +2134,7 @@ RTY_CMD55:
 	sd_card->sd_addr += (u32) rsp[2] << 16;
 
 	/* Get CSD register for Calculating Timing,Capacity,
-	 * Check CSD to determaine as if this is the SD ROM card */
+	 * Check CSD to determine as if this is the SD ROM card */
 	retval = sd_check_csd(chip, 1);
 	if (retval != STATUS_SUCCESS)
 		TRACE_RET(chip, retval);
@@ -2190,20 +2142,6 @@ RTY_CMD55:
 	retval = sd_select_card(chip, 1);
 	if (retval != STATUS_SUCCESS)
 		TRACE_RET(chip, retval);
-#ifdef SUPPORT_SD_LOCK
-SD_UNLOCK_ENTRY:
-	/* Get SD lock status */
-	retval = sd_update_lock_status(chip);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, STATUS_FAIL);
-
-	if (sd_card->sd_lock_status & SD_LOCKED) {
-		sd_card->sd_lock_status |= (SD_LOCK_1BIT_MODE | SD_PWD_EXIST);
-		return STATUS_SUCCESS;
-	} else if (!(sd_card->sd_lock_status & SD_UNLOCK_POW_ON)) {
-		sd_card->sd_lock_status &= ~SD_PWD_EXIST;
-	}
-#endif
 
 	/* ACMD42 */
 	retval =
@@ -2294,10 +2232,6 @@ SD_UNLOCK_ENTRY:
 		if (retval != STATUS_SUCCESS)
 			TRACE_RET(chip, retval);
 	}
-#ifdef SUPPORT_SD_LOCK
-	/* clear 1 bit mode status */
-	sd_card->sd_lock_status &= ~SD_LOCK_1BIT_MODE;
-#endif
 
 	if (CHK_SD30_SPEED(sd_card)) {
 		rts51x_write_register(chip, SD30_DRIVE_SEL, SD30_DRIVE_MASK,
@@ -2380,19 +2314,6 @@ SD_UNLOCK_ENTRY:
 
 	chip->card_bus_width[chip->card2lun[SD_CARD]] = 4;
 
-#ifdef SUPPORT_SD_LOCK
-	if (sd_card->sd_lock_status & SD_UNLOCK_POW_ON) {
-		rts51x_init_cmd(chip);
-
-		rts51x_add_cmd(chip, WRITE_REG_CMD, SD_BLOCK_CNT_H, 0xFF, 0x02);
-		rts51x_add_cmd(chip, WRITE_REG_CMD, SD_BLOCK_CNT_L, 0xFF, 0x00);
-
-		retval = rts51x_send_cmd(chip, MODE_C, 100);
-		if (retval != STATUS_SUCCESS)
-			TRACE_RET(chip, retval);
-	}
-#endif
-
 	return STATUS_SUCCESS;
 }
 
@@ -2587,17 +2508,10 @@ static int mmc_switch_timing_bus(struct rts51x_chip *chip)
 		sd_card->capacity =
 		    ((u32) buf[5] << 24) | ((u32) buf[4] << 16) |
 		    ((u32) buf[3] << 8) | ((u32) buf[2]);
-#ifdef SUPPORT_SD_LOCK
-	if (!(sd_card->sd_lock_status & SD_SDR_RST) && CHECK_UHS50(chip))
-		card_type_mask = 0x07;
-	else
-		card_type_mask = 0x03;
-#else
 	if (CHECK_UHS50(chip))
 		card_type_mask = 0x07;
 	else
 		card_type_mask = 0x03;
-#endif
 
 	card_type = buf[1] & card_type_mask;
 	if (card_type) {
@@ -2626,15 +2540,9 @@ static int mmc_switch_timing_bus(struct rts51x_chip *chip)
 	if (mmc_test_switch_bus(chip, MMC_8BIT_BUS) == STATUS_SUCCESS) {
 		SET_MMC_8BIT(sd_card);
 		chip->card_bus_width[chip->card2lun[SD_CARD]] = 8;
-#ifdef SUPPORT_SD_LOCK
-		sd_card->sd_lock_status &= ~SD_LOCK_1BIT_MODE;
-#endif
 	} else if (mmc_test_switch_bus(chip, MMC_4BIT_BUS) == STATUS_SUCCESS) {
 		SET_MMC_4BIT(sd_card);
 		chip->card_bus_width[chip->card2lun[SD_CARD]] = 4;
-#ifdef SUPPORT_SD_LOCK
-		sd_card->sd_lock_status &= ~SD_LOCK_1BIT_MODE;
-#endif
 	} else {
 		CLR_MMC_8BIT(sd_card);
 		CLR_MMC_4BIT(sd_card);
@@ -2652,11 +2560,6 @@ static int reset_mmc(struct rts51x_chip *chip)
 	u8 change_to_ddr52 = 1;
 	u8 cmd[5];
 
-#ifdef SUPPORT_SD_LOCK
-	if (sd_card->sd_lock_status & SD_UNLOCK_POW_ON)
-		goto MMC_UNLOCK_ENTRY;
-#endif
-
 MMC_DDR_FAIL:
 
 	retval = sd_prepare_reset(chip);
@@ -2745,7 +2648,7 @@ RTY_MMC_RST:
 		TRACE_RET(chip, retval);
 
 	/* Get CSD register for Calculating Timing,Capacity
-	 * Check CSD to determaine as if this is the SD ROM card */
+	 * Check CSD to determine as if this is the SD ROM card */
 	retval = sd_check_csd(chip, 1);
 	if (retval != STATUS_SUCCESS)
 		TRACE_RET(chip, retval);
@@ -2763,13 +2666,6 @@ RTY_MMC_RST:
 				0);
 	if (retval != STATUS_SUCCESS)
 		TRACE_RET(chip, retval);
-#ifdef SUPPORT_SD_LOCK
-MMC_UNLOCK_ENTRY:
-	/* Get SD lock status */
-	retval = sd_update_lock_status(chip);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, STATUS_FAIL);
-#endif
 
 	RTS51X_WRITE_REG(chip, SD_CFG1, SD_CLK_DIVIDE_MASK, SD_CLK_DIVIDE_0);
 
@@ -2842,18 +2738,6 @@ MMC_UNLOCK_ENTRY:
 			}
 		}
 	}
-#ifdef SUPPORT_SD_LOCK
-	if (sd_card->sd_lock_status & SD_UNLOCK_POW_ON) {
-		rts51x_init_cmd(chip);
-
-		rts51x_add_cmd(chip, WRITE_REG_CMD, SD_BLOCK_CNT_H, 0xFF, 0x02);
-		rts51x_add_cmd(chip, WRITE_REG_CMD, SD_BLOCK_CNT_L, 0xFF, 0x00);
-
-		retval = rts51x_send_cmd(chip, MODE_C, 100);
-		if (retval != STATUS_SUCCESS)
-			TRACE_RET(chip, retval);
-	}
-#endif
 
 	retval = rts51x_get_card_status(chip, &(chip->card_status));
 	if (retval != STATUS_SUCCESS)
@@ -2879,11 +2763,6 @@ int reset_sd_card(struct rts51x_chip *chip)
 	sd_card->capacity = 0;
 	sd_card->sd_switch_fail = 0;
 
-#ifdef SUPPORT_SD_LOCK
-	sd_card->sd_lock_status = 0;
-	sd_card->sd_erase_status = 0;
-#endif
-
 	sd_clear_reset_fail(chip);
 	enable_card_clock(chip, SD_CARD);
 
@@ -3006,7 +2885,7 @@ static int wait_data_buf_ready(struct rts51x_chip *chip)
 	TRACE_RET(chip, STATUS_FAIL);
 }
 
-void sd_stop_seq_mode(struct rts51x_chip *chip)
+static void sd_stop_seq_mode(struct rts51x_chip *chip)
 {
 	struct sd_info *sd_card = &(chip->sd_card);
 	int retval;
@@ -3300,7 +3179,7 @@ void sd_cleanup_work(struct rts51x_chip *chip)
 	}
 }
 
-inline void sd_fill_power_off_card3v3(struct rts51x_chip *chip)
+static inline void sd_fill_power_off_card3v3(struct rts51x_chip *chip)
 {
 	rts51x_add_cmd(chip, WRITE_REG_CMD, CARD_CLK_EN, SD_CLK_EN, 0);
 
@@ -3322,7 +3201,7 @@ inline void sd_fill_power_off_card3v3(struct rts51x_chip *chip)
 	}
 }
 
-int sd_power_off_card3v3(struct rts51x_chip *chip)
+static int sd_power_off_card3v3(struct rts51x_chip *chip)
 {
 	int retval;
 
@@ -3346,17 +3225,12 @@ int release_sd_card(struct rts51x_chip *chip)
 	struct sd_info *sd_card = &(chip->sd_card);
 	int retval;
 
-	RTS51X_DEBUGP("elease_sd_card\n");
+	RTS51X_DEBUGP("release_sd_card\n");
 
 	chip->card_ready &= ~SD_CARD;
 	chip->card_fail &= ~SD_CARD;
 	chip->card_wp &= ~SD_CARD;
 
-#ifdef SUPPORT_SD_LOCK
-	sd_card->sd_lock_status = 0;
-	sd_card->sd_erase_status = 0;
-#endif
-
 	memset(sd_card->raw_csd, 0, 16);
 	memset(sd_card->raw_scr, 0, 8);
 
diff --git a/drivers/staging/rts5139/sd.h b/drivers/staging/rts5139/sd.h
index 0805edcaea8..de155d8e682 100644
--- a/drivers/staging/rts5139/sd.h
+++ b/drivers/staging/rts5139/sd.h
@@ -141,29 +141,6 @@
 #define	SWITCH_MODE_ERR	  0x06
 #define	SWITCH_PASS	  0x07
 
-#ifdef SUPPORT_SD_LOCK
-/* CMD42 Parameter */
-#define SD_ERASE		0x08
-#define SD_LOCK			0x04
-#define SD_UNLOCK		0x00
-#define SD_CLR_PWD		0x02
-#define SD_SET_PWD		0x01
-
-#define SD_PWD_LEN		0x10
-
-/* SD lock unlock Status */
-#define SD_LOCKED		0x80	/* Global lock status */
-#define SD_LOCK_1BIT_MODE	0x40 /**/
-#define SD_PWD_EXIST		0x20
-#define SD_UNLOCK_POW_ON	0x01 /**/
-#define SD_SDR_RST		0x02 /* Reset SD30 card with current DDR mode to SDR mode. */
-/* g_bySDEraseStatus */
-#define SD_NOT_ERASE		0x00
-#define SD_UNDER_ERASING	0x01
-#define SD_COMPLETE_ERASE	0x02
-/* SD_RW FAIL status */
-#define SD_RW_FORBIDDEN		0x0F	/* read/write is forbidden (SD card)  */
-#endif
 /* Function Group Definition */
 /* Function Group 1 */
 #define	HS_SUPPORT			0x01
@@ -282,17 +259,11 @@ struct timing_phase_path {
 int sd_select_card(struct rts51x_chip *chip, int select);
 int reset_sd_card(struct rts51x_chip *chip);
 int sd_switch_clock(struct rts51x_chip *chip);
-void sd_stop_seq_mode(struct rts51x_chip *chip);
 int sd_rw(struct scsi_cmnd *srb, struct rts51x_chip *chip, u32 start_sector,
 	  u16 sector_cnt);
 void sd_cleanup_work(struct rts51x_chip *chip);
-int sd_power_off_card3v3(struct rts51x_chip *chip);
 int release_sd_card(struct rts51x_chip *chip);
 
-#ifdef SUPPORT_SD_LOCK
-int sd_update_lock_status(struct rts51x_chip *chip);
-#endif
-
 #ifdef SUPPORT_CPRM
 extern int reset_sd(struct rts51x_chip *chip);
 extern int sd_check_data0_status(struct rts51x_chip *chip);
diff --git a/drivers/staging/rts5139/sd_cprm.c b/drivers/staging/rts5139/sd_cprm.c
index d5969d992d8..f8c60711f71 100644
--- a/drivers/staging/rts5139/sd_cprm.c
+++ b/drivers/staging/rts5139/sd_cprm.c
@@ -77,12 +77,7 @@ static inline int get_rsp_type(u8 rsp_code, u8 *rsp_type, int *rsp_len)
 	return STATUS_SUCCESS;
 }
 
-int soft_reset_sd_card(struct rts51x_chip *chip)
-{
-	return reset_sd(chip);
-}
-
-int ext_sd_send_cmd_get_rsp(struct rts51x_chip *chip, u8 cmd_idx,
+static int ext_sd_send_cmd_get_rsp(struct rts51x_chip *chip, u8 cmd_idx,
 			    u32 arg, u8 rsp_type, u8 *rsp, int rsp_len,
 			    int special_check)
 {
@@ -206,11 +201,7 @@ RTY_SEND_CMD:
 			if (buf[1] & 0x80)
 				TRACE_RET(chip, STATUS_FAIL);
 		}
-#ifdef SUPPORT_SD_LOCK
-		if (buf[1] & 0x7D) {
-#else
 		if (buf[1] & 0x7F) {
-#endif
 			TRACE_RET(chip, STATUS_FAIL);
 		}
 		if (buf[2] & 0xF8)
@@ -233,7 +224,7 @@ RTY_SEND_CMD:
 	return STATUS_SUCCESS;
 }
 
-int ext_sd_get_rsp(struct rts51x_chip *chip, int len, u8 *rsp, u8 rsp_type)
+static int ext_sd_get_rsp(struct rts51x_chip *chip, int len, u8 *rsp, u8 rsp_type)
 {
 	int retval, rsp_len;
 	u16 reg_addr;
@@ -305,26 +296,8 @@ int ext_sd_execute_no_data(struct rts51x_chip *chip, unsigned int lun,
 	retval = sd_switch_clock(chip);
 	if (retval != STATUS_SUCCESS)
 		TRACE_RET(chip, TRANSPORT_FAILED);
-#ifdef SUPPORT_SD_LOCK
-	if ((sd_card->sd_lock_status & SD_LOCK_1BIT_MODE) == 0) {
-		if (CHK_MMC_8BIT(sd_card)) {
-			retval =
-			    rts51x_write_register(chip, SD_CFG1, 0x03,
-						  SD_BUS_WIDTH_8);
-			if (retval != STATUS_SUCCESS)
-				TRACE_RET(chip, TRANSPORT_FAILED);
-		} else if (CHK_SD(sd_card) || CHK_MMC_4BIT(sd_card)) {
-			retval =
-			    rts51x_write_register(chip, SD_CFG1, 0x03,
-						  SD_BUS_WIDTH_4);
-			if (retval != STATUS_SUCCESS)
-				TRACE_RET(chip, TRANSPORT_FAILED);
-		}
-	}
-#else
 	/* Set H/W SD/MMC Bus Width */
 	rts51x_write_register(chip, SD_CFG1, 0x03, SD_BUS_WIDTH_4);
-#endif
 
 	if (standby) {
 		retval = sd_select_card(chip, 0);
@@ -350,12 +323,6 @@ int ext_sd_execute_no_data(struct rts51x_chip *chip, unsigned int lun,
 		if (retval != STATUS_SUCCESS)
 			TRACE_GOTO(chip, SD_Execute_Cmd_Failed);
 	}
-#ifdef SUPPORT_SD_LOCK
-	/* Get SD lock status */
-	retval = sd_update_lock_status(chip);
-	if (retval != STATUS_SUCCESS)
-		TRACE_GOTO(chip, SD_Execute_Cmd_Failed);
-#endif
 
 	return TRANSPORT_GOOD;
 
@@ -399,21 +366,7 @@ int ext_sd_execute_read_data(struct rts51x_chip *chip, unsigned int lun,
 	retval = sd_switch_clock(chip);
 	if (retval != STATUS_SUCCESS)
 		TRACE_RET(chip, TRANSPORT_FAILED);
-#ifdef SUPPORT_SD_LOCK
-	if ((sd_card->sd_lock_status & SD_LOCK_1BIT_MODE) == 0) {
-		if (CHK_MMC_8BIT(sd_card))
-			bus_width = SD_BUS_WIDTH_8;
-		else if (CHK_SD(sd_card) || CHK_MMC_4BIT(sd_card))
-			bus_width = SD_BUS_WIDTH_4;
-		else
-			bus_width = SD_BUS_WIDTH_1;
-	} else {
-		bus_width = SD_BUS_WIDTH_4;
-	}
-	RTS51X_DEBUGP("bus_width = %d\n", bus_width);
-#else
 	bus_width = SD_BUS_WIDTH_4;
-#endif
 
 	if (data_len < 512) {
 		retval = ext_sd_send_cmd_get_rsp(chip, SET_BLOCKLEN, data_len,
@@ -599,11 +552,6 @@ int ext_sd_execute_write_data(struct rts51x_chip *chip, unsigned int lun,
 	int cmd13_checkbit = 0, write_err = 0;
 	u8 rsp_type;
 	u32 i;
-#ifdef SUPPORT_SD_LOCK
-	int lock_cmd_fail = 0;
-	u8 sd_lock_state = 0;
-	u8 lock_cmd_type = 0;
-#endif
 
 	if (sd_card->pre_cmd_err) {
 		sd_card->pre_cmd_err = 0;
@@ -614,12 +562,6 @@ int ext_sd_execute_write_data(struct rts51x_chip *chip, unsigned int lun,
 	retval = sd_switch_clock(chip);
 	if (retval != STATUS_SUCCESS)
 		TRACE_RET(chip, STATUS_FAIL);
-#ifdef SUPPORT_SD_LOCK
-	if (cmd_idx == LOCK_UNLOCK) {
-		sd_lock_state = sd_card->sd_lock_status;
-		sd_lock_state &= SD_LOCKED;
-	}
-#endif
 
 	retval = get_rsp_type(rsp_code, &rsp_type, &rsp_len);
 	if (retval != STATUS_SUCCESS) {
@@ -631,25 +573,7 @@ int ext_sd_execute_write_data(struct rts51x_chip *chip, unsigned int lun,
 	retval = sd_switch_clock(chip);
 	if (retval != STATUS_SUCCESS)
 		TRACE_RET(chip, TRANSPORT_FAILED);
-#ifdef SUPPORT_SD_LOCK
-	if ((sd_card->sd_lock_status & SD_LOCK_1BIT_MODE) == 0) {
-		if (CHK_MMC_8BIT(sd_card)) {
-			retval =
-			    rts51x_write_register(chip, SD_CFG1, 0x03,
-						  SD_BUS_WIDTH_8);
-			if (retval != STATUS_SUCCESS)
-				TRACE_RET(chip, TRANSPORT_FAILED);
-		} else if (CHK_SD(sd_card) || CHK_MMC_4BIT(sd_card)) {
-			retval =
-			    rts51x_write_register(chip, SD_CFG1, 0x03,
-						  SD_BUS_WIDTH_4);
-			if (retval != STATUS_SUCCESS)
-				TRACE_RET(chip, TRANSPORT_FAILED);
-		}
-	}
-#else
 	rts51x_write_register(chip, SD_CFG1, 0x03, SD_BUS_WIDTH_4);
-#endif
 
 	if (data_len < 512) {
 		retval = ext_sd_send_cmd_get_rsp(chip, SET_BLOCKLEN, data_len,
@@ -692,10 +616,6 @@ int ext_sd_execute_write_data(struct rts51x_chip *chip, unsigned int lun,
 		else
 			memcpy(buf, data_buf, data_len);
 
-#ifdef SUPPORT_SD_LOCK
-		if (cmd_idx == LOCK_UNLOCK)
-			lock_cmd_type = buf[0] & 0x0F;
-#endif
 
 		if (data_len > 256) {
 			rts51x_init_cmd(chip);
@@ -802,29 +722,6 @@ int ext_sd_execute_write_data(struct rts51x_chip *chip, unsigned int lun,
 				      SD_STOP | SD_CLR_ERR);
 		TRACE_GOTO(chip, SD_Execute_Write_Cmd_Failed);
 	}
-#ifdef SUPPORT_SD_LOCK
-	if (cmd_idx == LOCK_UNLOCK) {
-		if (lock_cmd_type == SD_ERASE) {
-			sd_card->sd_erase_status = SD_UNDER_ERASING;
-			scsi_set_resid(srb, 0);
-			return TRANSPORT_GOOD;
-		}
-
-		rts51x_init_cmd(chip);
-		rts51x_add_cmd(chip, CHECK_REG_CMD, SD_BUS_STAT, SD_DAT0_STATUS,
-			       SD_DAT0_STATUS);
-		retval = rts51x_send_cmd(chip, MODE_CR, 250);
-		if (retval != STATUS_SUCCESS)
-			TRACE_GOTO(chip, SD_Execute_Write_Cmd_Failed);
-		rts51x_get_rsp(chip, 1, 200); /* Don't care return value */
-
-		retval = sd_update_lock_status(chip);
-		if (retval != STATUS_SUCCESS) {
-			RTS51X_DEBUGP("Lock command fail!\n");
-			lock_cmd_fail = 1;
-		}
-	}
-#endif /* SUPPORT_SD_LOCK */
 
 	if (standby) {
 		retval = sd_select_card(chip, 1);
@@ -865,51 +762,6 @@ int ext_sd_execute_write_data(struct rts51x_chip *chip, unsigned int lun,
 	}
 	if (retval != STATUS_SUCCESS)
 		TRACE_GOTO(chip, SD_Execute_Write_Cmd_Failed);
-#ifdef SUPPORT_SD_LOCK
-	if (cmd_idx == LOCK_UNLOCK) {
-		if (!lock_cmd_fail) {
-			RTS51X_DEBUGP("lock_cmd_type = 0x%x\n",
-				       lock_cmd_type);
-			if (lock_cmd_type & SD_CLR_PWD)
-				sd_card->sd_lock_status &= ~SD_PWD_EXIST;
-			if (lock_cmd_type & SD_SET_PWD)
-				sd_card->sd_lock_status |= SD_PWD_EXIST;
-		}
-
-		RTS51X_DEBUGP("sd_lock_state = 0x%x,"
-				"sd_card->sd_lock_status = 0x%x\n",
-				sd_lock_state, sd_card->sd_lock_status);
-		if (sd_lock_state ^ (sd_card->sd_lock_status & SD_LOCKED)) {
-			sd_card->sd_lock_notify = 1;
-			if (sd_lock_state) {
-				if (sd_card->sd_lock_status &
-						SD_LOCK_1BIT_MODE) {
-					sd_card->sd_lock_status |=
-					    (SD_UNLOCK_POW_ON | SD_SDR_RST);
-					if (CHK_SD(sd_card)) {
-						retval = reset_sd(chip);
-						if (retval != STATUS_SUCCESS) {
-							sd_card->sd_lock_status
-							&= ~(SD_UNLOCK_POW_ON |
-							      SD_SDR_RST);
-							TRACE_GOTO(chip,
-								   SD_Execute_Write_Cmd_Failed);
-						}
-					}
-
-					sd_card->sd_lock_status &=
-					    ~(SD_UNLOCK_POW_ON | SD_SDR_RST);
-				}
-			}
-		}
-	}
-
-	if (lock_cmd_fail) {
-		scsi_set_resid(srb, 0);
-		set_sense_type(chip, lun, SENSE_TYPE_NO_SENSE);
-		TRACE_RET(chip, TRANSPORT_FAILED);
-	}
-#endif /* SUPPORT_SD_LOCK */
 
 	return TRANSPORT_GOOD;
 
@@ -1173,30 +1025,18 @@ int sd_hw_rst(struct scsi_cmnd *srb, struct rts51x_chip *chip)
 	switch (srb->cmnd[1] & 0x0F) {
 	case 0:
 		/* SD Card Power Off -> ON and Initialization */
-#ifdef SUPPORT_SD_LOCK
-		if (0x64 == srb->cmnd[9]) {
-			/* Command Mode */
-			sd_card->sd_lock_status |= SD_SDR_RST;
-		}
-#endif /* SUPPORT_SD_LOCK */
 		retval = reset_sd_card(chip);
 		if (retval != STATUS_SUCCESS) {
-#ifdef SUPPORT_SD_LOCK
-			sd_card->sd_lock_status &= ~SD_SDR_RST;
-#endif
 			set_sense_type(chip, lun, SENSE_TYPE_MEDIA_NOT_PRESENT);
 			sd_card->pre_cmd_err = 1;
 			TRACE_RET(chip, TRANSPORT_FAILED);
 		}
-#ifdef SUPPORT_SD_LOCK
-		sd_card->sd_lock_status &= ~SD_SDR_RST;
-#endif
 		break;
 
 	case 1:
 		/* reset CMD(CMD0) and Initialization
 		 * (without SD Card Power Off -> ON) */
-		retval = soft_reset_sd_card(chip);
+		retval = reset_sd(chip);
 		if (retval != STATUS_SUCCESS) {
 			set_sense_type(chip, lun, SENSE_TYPE_MEDIA_NOT_PRESENT);
 			sd_card->pre_cmd_err = 1;
diff --git a/drivers/staging/rts5139/xd.c b/drivers/staging/rts5139/xd.c
index 5820605d180..58f8ba24cae 100644
--- a/drivers/staging/rts5139/xd.c
+++ b/drivers/staging/rts5139/xd.c
@@ -47,13 +47,6 @@ static inline void xd_set_err_code(struct rts51x_chip *chip, u8 err_code)
 	xd_card->err_code = err_code;
 }
 
-static inline int xd_check_err_code(struct rts51x_chip *chip, u8 err_code)
-{
-	struct xd_info *xd_card = &(chip->xd_card);
-
-	return (xd_card->err_code == err_code);
-}
-
 static int xd_set_init_para(struct rts51x_chip *chip)
 {
 	struct xd_info *xd_card = &(chip->xd_card);
@@ -862,6 +855,8 @@ static void xd_set_l2p_tbl(struct rts51x_chip *chip, int zone_no, u16 log_off,
 	zone->l2p_table[log_off] = phy_off;
 }
 
+static int xd_delay_write(struct rts51x_chip *chip);
+
 static u32 xd_get_l2p_tbl(struct rts51x_chip *chip, int zone_no, u16 log_off)
 {
 	struct xd_info *xd_card = &(chip->xd_card);
@@ -1182,91 +1177,6 @@ static int xd_copy_page(struct rts51x_chip *chip,
 	return STATUS_SUCCESS;
 }
 
-#ifdef XD_SPEEDUP
-static int xd_auto_copy_page(struct rts51x_chip *chip,
-			     u32 old_blk, u32 new_blk,
-			     u8 start_page, u8 end_page)
-{
-	struct xd_info *xd_card = &(chip->xd_card);
-	u32 old_page, new_page;
-	int retval;
-	u8 page_count;
-
-	RTS51X_DEBUGP("Auto copy page from block 0x%x to block 0x%x\n",
-		       old_blk, new_blk);
-
-	if (start_page > end_page)
-		TRACE_RET(chip, STATUS_FAIL);
-
-	page_count = end_page - start_page;
-
-	if ((old_blk == BLK_NOT_FOUND) || (new_blk == BLK_NOT_FOUND))
-		TRACE_RET(chip, STATUS_FAIL);
-
-	old_page = (old_blk << xd_card->block_shift) + start_page;
-	new_page = (new_blk << xd_card->block_shift) + start_page;
-
-	XD_CLR_BAD_NEWBLK(xd_card);
-
-	rts51x_init_cmd(chip);
-
-	rts51x_add_cmd(chip, WRITE_REG_CMD, XD_CP_WAITTIME, 0x03, WAIT_FF);
-	rts51x_add_cmd(chip, WRITE_REG_CMD, XD_CP_PAGELEN, 0xFF, page_count);
-
-	rts51x_add_cmd(chip, WRITE_REG_CMD, XD_CP_READADDR0, 0xFF, 0);
-	rts51x_add_cmd(chip, WRITE_REG_CMD, XD_CP_READADDR1, 0xFF,
-		       (u8) old_page);
-	rts51x_add_cmd(chip, WRITE_REG_CMD, XD_CP_READADDR2, 0xFF,
-		       (u8) (old_page >> 8));
-	rts51x_add_cmd(chip, WRITE_REG_CMD, XD_CP_READADDR3, 0xFF,
-		       (u8) (old_page >> 16));
-	rts51x_add_cmd(chip, WRITE_REG_CMD, XD_CP_READADDR4, 0xFF, 0);
-
-	rts51x_add_cmd(chip, WRITE_REG_CMD, XD_CP_WRITEADDR0, 0xFF, 0);
-	rts51x_add_cmd(chip, WRITE_REG_CMD, XD_CP_WRITEADDR1, 0xFF,
-		       (u8) new_page);
-	rts51x_add_cmd(chip, WRITE_REG_CMD, XD_CP_WRITEADDR2, 0xFF,
-		       (u8) (new_page >> 8));
-	rts51x_add_cmd(chip, WRITE_REG_CMD, XD_CP_WRITEADDR3, 0xFF,
-		       (u8) (new_page >> 16));
-	rts51x_add_cmd(chip, WRITE_REG_CMD, XD_CP_WRITEADDR4, 0xFF, 0);
-
-	rts51x_add_cmd(chip, WRITE_REG_CMD, CARD_DATA_SOURCE, 0x01,
-		       PINGPONG_BUFFER);
-
-	rts51x_add_cmd(chip, WRITE_REG_CMD, XD_CFG,
-		       XD_BA_TRANSFORM | XD_ADDR_MASK, 0 | xd_card->addr_cycle);
-
-	rts51x_add_cmd(chip, WRITE_REG_CMD, XD_CHK_DATA_STATUS,
-		       XD_AUTO_CHK_DATA_STATUS, 0);
-	rts51x_add_cmd(chip, WRITE_REG_CMD, XD_TRANSFER, 0xFF,
-		       XD_TRANSFER_START | XD_COPY_PAGES);
-	rts51x_add_cmd(chip, CHECK_REG_CMD, XD_TRANSFER, XD_TRANSFER_END,
-		       XD_TRANSFER_END);
-
-	retval = rts51x_send_cmd(chip, MODE_CR, 100);
-	if (retval != STATUS_SUCCESS) {
-		rts51x_clear_xd_error(chip);
-		TRACE_GOTO(chip, Copy_Fail);
-	}
-
-	retval = rts51x_get_rsp(chip, 1, 800);
-	if (retval != STATUS_SUCCESS) {
-		rts51x_clear_xd_error(chip);
-		TRACE_GOTO(chip, Copy_Fail);
-	}
-
-	return STATUS_SUCCESS;
-
-Copy_Fail:
-	retval = xd_copy_page(chip, old_blk, new_blk, start_page, end_page);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, retval);
-
-	return STATUS_SUCCESS;
-}
-#endif
-
 static int xd_reset_cmd(struct rts51x_chip *chip)
 {
 	int retval;
@@ -1686,15 +1596,9 @@ Fail:
 			XD_CLR_BAD_OLDBLK(xd_card);
 			TRACE_RET(chip, STATUS_FAIL);
 		}
-#ifdef XD_SPEEDUP
-		retval =
-		    xd_auto_copy_page(chip, phy_blk, new_blk, 0,
-				      xd_card->page_off + 1);
-#else
 		retval =
 		    xd_copy_page(chip, phy_blk, new_blk, 0,
 				 xd_card->page_off + 1);
-#endif
 		if (retval != STATUS_SUCCESS) {
 			if (!XD_CHK_BAD_NEWBLK(xd_card)) {
 				retval = xd_erase_block(chip, new_blk);
@@ -1741,13 +1645,8 @@ static int xd_finish_write(struct rts51x_chip *chip,
 			TRACE_RET(chip, STATUS_FAIL);
 		}
 	} else {
-#ifdef XD_SPEEDUP
-		retval = xd_auto_copy_page(chip, old_blk, new_blk,
-					   page_off, xd_card->page_off + 1);
-#else
 		retval = xd_copy_page(chip, old_blk, new_blk,
 				      page_off, xd_card->page_off + 1);
-#endif
 		if (retval != STATUS_SUCCESS) {
 			if (!XD_CHK_BAD_NEWBLK(xd_card)) {
 				retval = xd_erase_block(chip, new_blk);
@@ -1789,11 +1688,7 @@ static int xd_prepare_write(struct rts51x_chip *chip,
 				old_blk, new_blk, log_blk, (int)page_off);
 
 	if (page_off) {
-#ifdef XD_SPEEDUP
-		retval = xd_auto_copy_page(chip, old_blk, new_blk, 0, page_off);
-#else
 		retval = xd_copy_page(chip, old_blk, new_blk, 0, page_off);
-#endif
 		if (retval != STATUS_SUCCESS)
 			TRACE_RET(chip, retval);
 	}
@@ -1922,7 +1817,7 @@ Fail:
 	TRACE_RET(chip, STATUS_FAIL);
 }
 
-int xd_delay_write(struct rts51x_chip *chip)
+static int xd_delay_write(struct rts51x_chip *chip)
 {
 	struct xd_info *xd_card = &(chip->xd_card);
 	struct xd_delay_write_tag *delay_write = &(xd_card->delay_write);
@@ -1999,18 +1894,11 @@ int xd_rw(struct scsi_cmnd *srb, struct rts51x_chip *chip, u32 start_sector,
 		    (start_page > delay_write->pageoff)) {
 			delay_write->delay_write_flag = 0;
 			if (delay_write->old_phyblock != BLK_NOT_FOUND) {
-#ifdef XD_SPEEDUP
-				retval = xd_auto_copy_page(chip,
-					delay_write->old_phyblock,
-					delay_write->new_phyblock,
-					delay_write->pageoff, start_page);
-#else
 				retval = xd_copy_page(chip,
 						      delay_write->old_phyblock,
 						      delay_write->new_phyblock,
 						      delay_write->pageoff,
 						      start_page);
-#endif
 				if (retval != STATUS_SUCCESS) {
 					set_sense_type(chip, lun,
 						SENSE_TYPE_MEDIA_WRITE_ERR);
@@ -2198,7 +2086,7 @@ void xd_cleanup_work(struct rts51x_chip *chip)
 	}
 }
 
-int xd_power_off_card3v3(struct rts51x_chip *chip)
+static int xd_power_off_card3v3(struct rts51x_chip *chip)
 {
 	int retval;
 
@@ -2232,7 +2120,7 @@ int release_xd_card(struct rts51x_chip *chip)
 	struct xd_info *xd_card = &(chip->xd_card);
 	int retval;
 
-	RTS51X_DEBUGP("elease_xd_card\n");
+	RTS51X_DEBUGP("release_xd_card\n");
 
 	chip->card_ready &= ~XD_CARD;
 	chip->card_fail &= ~XD_CARD;
diff --git a/drivers/staging/rts5139/xd.h b/drivers/staging/rts5139/xd.h
index fa695903ba6..55e4205e23f 100644
--- a/drivers/staging/rts5139/xd.h
+++ b/drivers/staging/rts5139/xd.h
@@ -182,12 +182,10 @@
 #define	CIS1_9			(256 + 9)
 
 int reset_xd_card(struct rts51x_chip *chip);
-int xd_delay_write(struct rts51x_chip *chip);
 int xd_rw(struct scsi_cmnd *srb, struct rts51x_chip *chip, u32 start_sector,
 	  u16 sector_cnt);
 void xd_free_l2p_tbl(struct rts51x_chip *chip);
 void xd_cleanup_work(struct rts51x_chip *chip);
-int xd_power_off_card3v3(struct rts51x_chip *chip);
 int release_xd_card(struct rts51x_chip *chip);
 
 #endif /* __RTS51X_XD_H */
diff --git a/drivers/staging/rts_pstor/ms.c b/drivers/staging/rts_pstor/ms.c
index f9a4498984c..0bf6d95b3fa 100644
--- a/drivers/staging/rts_pstor/ms.c
+++ b/drivers/staging/rts_pstor/ms.c
@@ -4136,7 +4136,7 @@ int mg_set_ICV(struct scsi_cmnd *srb, struct rtsx_chip *chip)
 #else
 	retval = ms_transfer_data(chip, MS_TM_AUTO_WRITE, PRO_WRITE_LONG_DATA,
 				2, WAIT_INT, 0, 0, buf + 4, 1024);
-	if ((retval != STATUS_SUCCESS) || check_ms_err(chip) {
+	if ((retval != STATUS_SUCCESS) || check_ms_err(chip)) {
 		rtsx_clear_ms_error(chip);
 		if (ms_card->mg_auth == 0) {
 			if ((buf[5] & 0xC0) != 0) {
diff --git a/drivers/staging/rts_pstor/rtsx_transport.c b/drivers/staging/rts_pstor/rtsx_transport.c
index 9b2e5c99870..54a474235f2 100644
--- a/drivers/staging/rts_pstor/rtsx_transport.c
+++ b/drivers/staging/rts_pstor/rtsx_transport.c
@@ -130,7 +130,7 @@ unsigned int rtsx_stor_access_xfer_buf(unsigned char *buffer,
 /* Store the contents of buffer into srb's transfer buffer and set the
 * SCSI residue. */
 void rtsx_stor_set_xfer_buf(unsigned char *buffer,
-       unsigned int buflen, struct scsi_cmnd *srb)
+	unsigned int buflen, struct scsi_cmnd *srb)
 {
 	unsigned int index = 0, offset = 0;
 
@@ -141,7 +141,7 @@ void rtsx_stor_set_xfer_buf(unsigned char *buffer,
 }
 
 void rtsx_stor_get_xfer_buf(unsigned char *buffer,
-       unsigned int buflen, struct scsi_cmnd *srb)
+	unsigned int buflen, struct scsi_cmnd *srb)
 {
 	unsigned int index = 0, offset = 0;
 
diff --git a/drivers/staging/sep/sep_driver_config.h b/drivers/staging/sep/sep_driver_config.h
index fa7c0d09bfa..9d9fc7c94a6 100644
--- a/drivers/staging/sep/sep_driver_config.h
+++ b/drivers/staging/sep/sep_driver_config.h
@@ -68,11 +68,11 @@
 #define SEP_DRIVER_MIN_DATA_SIZE_PER_TABLE		16
 
 /* flag that signifies tah the lock is
-currently held by the proccess (struct file) */
+currently held by the process (struct file) */
 #define SEP_DRIVER_OWN_LOCK_FLAG                        1
 
 /* flag that signifies tah the lock is currently NOT
-held by the proccess (struct file) */
+held by the process (struct file) */
 #define SEP_DRIVER_DISOWN_LOCK_FLAG                     0
 
 /* indicates whether driver has mapped/unmapped shared area */
@@ -280,7 +280,7 @@ held by the proccess (struct file) */
 
 /*
  * Used to limit number of concurrent processes
- * allowed to allocte dynamic buffers in fastcall
+ * allowed to allocate dynamic buffers in fastcall
  * interface.
  */
 #define SEP_DOUBLEBUF_USERS_LIMIT		3
diff --git a/drivers/staging/sep/sep_main.c b/drivers/staging/sep/sep_main.c
index f1701bc6e31..df1d13e96fc 100644
--- a/drivers/staging/sep/sep_main.c
+++ b/drivers/staging/sep/sep_main.c
@@ -786,7 +786,7 @@ static unsigned int sep_poll(struct file *filp, poll_table *wait)
 		"[PID%d] poll: send_ct is %lx reply ct is %lx\n",
 			current->pid, sep->send_ct, sep->reply_ct);
 
-	/* Check if error occured during poll */
+	/* Check if error occurred during poll */
 	retval2 = sep_read_reg(sep, HW_HOST_SEP_HOST_GPR3_REG_ADDR);
 	if ((retval2 != 0x0) && (retval2 != 0x8)) {
 		dev_dbg(&sep->pdev->dev, "[PID%d] poll; poll error %x\n",
@@ -1160,7 +1160,7 @@ static int sep_lock_kernel_pages(struct sep_device *sep,
 
 	/* Put mapped kernel sg into kernel resource array */
 
-	/* Set output params acording to the in_out flag */
+	/* Set output params according to the in_out flag */
 	if (in_out_flag == SEP_DRIVER_IN_FLAG) {
 		*lli_array_ptr = lli_array;
 		dma_ctx->dma_res_arr[dma_ctx->nr_dcb_creat].in_num_pages =
@@ -1358,7 +1358,7 @@ static int sep_lock_user_pages(struct sep_device *sep,
 			lli_array[num_pages - 1].block_size);
 	}
 
-	/* Set output params acording to the in_out flag */
+	/* Set output params according to the in_out flag */
 	if (in_out_flag == SEP_DRIVER_IN_FLAG) {
 		*lli_array_ptr = lli_array;
 		dma_ctx->dma_res_arr[dma_ctx->nr_dcb_creat].in_num_pages =
@@ -2038,7 +2038,7 @@ static int sep_prepare_input_dma_table(struct sep_device *sep,
 
 		/*
 		 * If this is not the last table -
-		 * then allign it to the block size
+		 * then align it to the block size
 		 */
 		if (!last_table_flag)
 			table_data_size =
@@ -3033,7 +3033,7 @@ static int sep_free_dcb_handler(struct sep_device *sep,
  * @cmd: command
  * @arg: pointer to argument structure
  *
- * Implement the ioctl methods availble on the SEP device.
+ * Implement the ioctl methods available on the SEP device.
  */
 static long sep_ioctl(struct file *filp, unsigned int cmd, unsigned long arg)
 {
@@ -4460,7 +4460,7 @@ static int sep_pm_runtime_suspend(struct device *dev)
  * @sep_pm_runtime_resume:	resume- no communication with cpu & main memory
  * @sep_pm_runtime_suspend:	suspend- no communication with cpu & main memory
  * @sep_pci_suspend:		suspend - main memory is still ON
- * @sep_pci_resume:		resume - main meory is still ON
+ * @sep_pci_resume:		resume - main memory is still ON
  */
 static const struct dev_pm_ops sep_pm = {
 	.runtime_resume = sep_pm_runtime_resume,
diff --git a/drivers/staging/serial/68360serial.c b/drivers/staging/serial/68360serial.c
deleted file mode 100644
index daf0b1d0dc2..00000000000
--- a/drivers/staging/serial/68360serial.c
+++ /dev/null
@@ -1,2979 +0,0 @@
-/*
- *  UART driver for 68360 CPM SCC or SMC
- *  Copyright (c) 2000 D. Jeff Dionne <jeff@uclinux.org>,
- *  Copyright (c) 2000 Michael Leslie <mleslie@lineo.ca>
- *  Copyright (c) 1997 Dan Malek <dmalek@jlc.net>
- *
- * I used the serial.c driver as the framework for this driver.
- * Give credit to those guys.
- * The original code was written for the MBX860 board.  I tried to make
- * it generic, but there may be some assumptions in the structures that
- * have to be fixed later.
- * To save porting time, I did not bother to change any object names
- * that are not accessed outside of this file.
- * It still needs lots of work........When it was easy, I included code
- * to support the SCCs, but this has never been tested, nor is it complete.
- * Only the SCCs support modem control, so that is not complete either.
- *
- * This module exports the following rs232 io functions:
- *
- *	int rs_360_init(void);
- */
-
-#include <linux/module.h>
-#include <linux/errno.h>
-#include <linux/signal.h>
-#include <linux/sched.h>
-#include <linux/timer.h>
-#include <linux/interrupt.h>
-#include <linux/tty.h>
-#include <linux/tty_flip.h>
-#include <linux/serial.h>
-#include <linux/serialP.h> 
-#include <linux/major.h>
-#include <linux/string.h>
-#include <linux/fcntl.h>
-#include <linux/ptrace.h>
-#include <linux/mm.h>
-#include <linux/init.h>
-#include <linux/delay.h>
-#include <asm/irq.h>
-#include <asm/m68360.h>
-#include <asm/commproc.h>
-
- 
-#ifdef CONFIG_KGDB
-extern void breakpoint(void);
-extern void set_debug_traps(void);
-extern int  kgdb_output_string (const char* s, unsigned int count);
-#endif
-
-
-/* #ifdef CONFIG_SERIAL_CONSOLE */ /* This seems to be a post 2.0 thing - mles */
-#include <linux/console.h>
-#include <linux/jiffies.h>
-
-/* this defines the index into rs_table for the port to use
- */
-#ifndef CONFIG_SERIAL_CONSOLE_PORT
-#define CONFIG_SERIAL_CONSOLE_PORT	1 /* ie SMC2 - note USE_SMC2 must be defined */
-#endif
-/* #endif */
-
-#if 0
-/* SCC2 for console
- */
-#undef CONFIG_SERIAL_CONSOLE_PORT
-#define CONFIG_SERIAL_CONSOLE_PORT	2
-#endif
-
-
-#define TX_WAKEUP	ASYNC_SHARE_IRQ
-
-static char *serial_name = "CPM UART driver";
-static char *serial_version = "0.03";
-
-static struct tty_driver *serial_driver;
-int serial_console_setup(struct console *co, char *options);
-
-/*
- * Serial driver configuration section.  Here are the various options:
- */
-#define SERIAL_PARANOIA_CHECK
-#define CONFIG_SERIAL_NOPAUSE_IO
-#define SERIAL_DO_RESTART
-
-/* Set of debugging defines */
-
-#undef SERIAL_DEBUG_INTR
-#undef SERIAL_DEBUG_OPEN
-#undef SERIAL_DEBUG_FLOW
-#undef SERIAL_DEBUG_RS_WAIT_UNTIL_SENT
-
-#define _INLINE_ inline
-  
-#define DBG_CNT(s)
-
-/* We overload some of the items in the data structure to meet our
- * needs.  For example, the port address is the CPM parameter ram
- * offset for the SCC or SMC.  The maximum number of ports is 4 SCCs and
- * 2 SMCs.  The "hub6" field is used to indicate the channel number, with
- * a flag indicating SCC or SMC, and the number is used as an index into
- * the CPM parameter area for this device.
- * The "type" field is currently set to 0, for PORT_UNKNOWN.  It is
- * not currently used.  I should probably use it to indicate the port
- * type of SMC or SCC.
- * The SMCs do not support any modem control signals.
- */
-#define smc_scc_num	hub6
-#define NUM_IS_SCC	((int)0x00010000)
-#define PORT_NUM(P)	((P) & 0x0000ffff)
-
-
-#if defined (CONFIG_UCQUICC)
-
-volatile extern void *_periph_base;
-/* sipex transceiver
- *   mode bits for       are on pins
- *
- *    SCC2                d16..19
- *    SCC3                d20..23
- *    SCC4                d24..27
- */
-#define SIPEX_MODE(n,m) ((m & 0x0f)<<(16+4*(n-1)))
-
-static uint sipex_mode_bits = 0x00000000;
-
-#endif
-
-/* There is no `serial_state' defined back here in 2.0.
- * Try to get by with serial_struct
- */
-/* #define serial_state serial_struct */
-
-/* 2.4 -> 2.0 portability problem: async_icount in 2.4 has a few
- * extras: */
-
-#if 0
-struct async_icount_24 {
-	__u32   cts, dsr, rng, dcd, tx, rx;
-	__u32   frame, parity, overrun, brk;
-	__u32   buf_overrun;
-} icount;
-#endif
-
-#if 0
-
-struct serial_state {
-        int     magic;
-        int     baud_base;
-        unsigned long   port;
-        int     irq;
-        int     flags;
-        int     hub6;
-        int     type;
-        int     line;
-        int     revision;       /* Chip revision (950) */
-        int     xmit_fifo_size;
-        int     custom_divisor;
-        int     count;
-        u8      *iomem_base;
-        u16     iomem_reg_shift;
-        unsigned short  close_delay;
-        unsigned short  closing_wait; /* time to wait before closing */
-        struct async_icount_24     icount; 
-        int     io_type;
-        struct async_struct *info;
-};
-#endif
-
-#define SSTATE_MAGIC 0x5302
-
-
-
-/* SMC2 is sometimes used for low performance TDM interfaces.  Define
- * this as 1 if you want SMC2 as a serial port UART managed by this driver.
- * Define this as 0 if you wish to use SMC2 for something else.
- */
-#define USE_SMC2 1
-
-#if 0
-/* Define SCC to ttySx mapping. */
-#define SCC_NUM_BASE	(USE_SMC2 + 1)	/* SCC base tty "number" */
-
-/* Define which SCC is the first one to use for a serial port.  These
- * are 0-based numbers, i.e. this assumes the first SCC (SCC1) is used
- * for Ethernet, and the first available SCC for serial UART is SCC2.
- * NOTE:  IF YOU CHANGE THIS, you have to change the PROFF_xxx and
- * interrupt vectors in the table below to match.
- */
-#define SCC_IDX_BASE	1	/* table index */
-#endif
-
-
-/* Processors other than the 860 only get SMCs configured by default.
- * Either they don't have SCCs or they are allocated somewhere else.
- * Of course, there are now 860s without some SCCs, so we will need to
- * address that someday.
- * The Embedded Planet Multimedia I/O cards use TDM interfaces to the
- * stereo codec parts, and we use SMC2 to help support that.
- */
-static struct serial_state rs_table[] = {
-/*  type   line   PORT           IRQ       FLAGS  smc_scc_num (F.K.A. hub6) */
-	{  0,     0, PRSLOT_SMC1, CPMVEC_SMC1,   0,    0 }    /* SMC1 ttyS0 */
-#if USE_SMC2
-	,{ 0,     0, PRSLOT_SMC2, CPMVEC_SMC2,   0,    1 }     /* SMC2 ttyS1 */
-#endif
-
-#if defined(CONFIG_SERIAL_68360_SCC)
-	,{ 0,     0, PRSLOT_SCC2, CPMVEC_SCC2,   0, (NUM_IS_SCC | 1) }    /* SCC2 ttyS2 */
-	,{ 0,     0, PRSLOT_SCC3, CPMVEC_SCC3,   0, (NUM_IS_SCC | 2) }    /* SCC3 ttyS3 */
-	,{ 0,     0, PRSLOT_SCC4, CPMVEC_SCC4,   0, (NUM_IS_SCC | 3) }    /* SCC4 ttyS4 */
-#endif
-};
-
-#define NR_PORTS	(sizeof(rs_table)/sizeof(struct serial_state))
-
-/* The number of buffer descriptors and their sizes.
- */
-#define RX_NUM_FIFO	4
-#define RX_BUF_SIZE	32
-#define TX_NUM_FIFO	4
-#define TX_BUF_SIZE	32
-
-#define CONSOLE_NUM_FIFO 2
-#define CONSOLE_BUF_SIZE 4
-
-char *console_fifos[CONSOLE_NUM_FIFO * CONSOLE_BUF_SIZE];
-
-/* The async_struct in serial.h does not really give us what we
- * need, so define our own here.
- */
-typedef struct serial_info {
-	int			magic;
-	int			flags;
-
-	struct serial_state	*state;
- 	/* struct serial_struct	*state; */
- 	/* struct async_struct	*state; */
-	
-	struct tty_struct 	*tty;
-	int			read_status_mask;
-	int			ignore_status_mask;
-	int			timeout;
-	int			line;
-	int			x_char;	/* xon/xoff character */
-	int			close_delay;
-	unsigned short		closing_wait;
-	unsigned short		closing_wait2;
-	unsigned long		event;
-	unsigned long		last_active;
-	int			blocked_open; /* # of blocked opens */
-	struct work_struct	tqueue;
-	struct work_struct	tqueue_hangup;
- 	wait_queue_head_t	open_wait; 
- 	wait_queue_head_t	close_wait; 
-
-	
-/* CPM Buffer Descriptor pointers.
-	*/
-	QUICC_BD			*rx_bd_base;
-	QUICC_BD			*rx_cur;
-	QUICC_BD			*tx_bd_base;
-	QUICC_BD			*tx_cur;
-} ser_info_t;
-
-
-/* since kmalloc_init() does not get called until much after this initialization: */
-static ser_info_t  quicc_ser_info[NR_PORTS];
-static char rx_buf_pool[NR_PORTS * RX_NUM_FIFO * RX_BUF_SIZE];
-static char tx_buf_pool[NR_PORTS * TX_NUM_FIFO * TX_BUF_SIZE];
-
-static void change_speed(ser_info_t *info);
-static void rs_360_wait_until_sent(struct tty_struct *tty, int timeout);
-
-static inline int serial_paranoia_check(ser_info_t *info,
-					char *name, const char *routine)
-{
-#ifdef SERIAL_PARANOIA_CHECK
-	static const char *badmagic =
-		"Warning: bad magic number for serial struct (%s) in %s\n";
-	static const char *badinfo =
-		"Warning: null async_struct for (%s) in %s\n";
-
-	if (!info) {
-		printk(badinfo, name, routine);
-		return 1;
-	}
-	if (info->magic != SERIAL_MAGIC) {
-		printk(badmagic, name, routine);
-		return 1;
-	}
-#endif
-	return 0;
-}
-
-/*
- * This is used to figure out the divisor speeds and the timeouts,
- * indexed by the termio value.  The generic CPM functions are responsible
- * for setting and assigning baud rate generators for us.
- */
-static int baud_table[] = {
-	0, 50, 75, 110, 134, 150, 200, 300, 600, 1200, 1800, 2400, 4800,
-	9600, 19200, 38400, 57600, 115200, 230400, 460800, 0 };
-
-/* This sucks. There is a better way: */
-#if defined(CONFIG_CONSOLE_9600)
-  #define CONSOLE_BAUDRATE 9600
-#elif defined(CONFIG_CONSOLE_19200)
-  #define CONSOLE_BAUDRATE 19200
-#elif defined(CONFIG_CONSOLE_115200)
-  #define CONSOLE_BAUDRATE 115200
-#else
-  #warning "console baud rate undefined"
-  #define CONSOLE_BAUDRATE 9600
-#endif
-
-/*
- * ------------------------------------------------------------
- * rs_stop() and rs_start()
- *
- * This routines are called before setting or resetting tty->stopped.
- * They enable or disable transmitter interrupts, as necessary.
- * ------------------------------------------------------------
- */
-static void rs_360_stop(struct tty_struct *tty)
-{
-	ser_info_t *info = (ser_info_t *)tty->driver_data;
-	int	idx;
-	unsigned long flags;
- 	volatile struct scc_regs *sccp;
- 	volatile struct smc_regs *smcp;
-
-	if (serial_paranoia_check(info, tty->name, "rs_stop"))
-		return;
-	
-	local_irq_save(flags);
-	idx = PORT_NUM(info->state->smc_scc_num);
-	if (info->state->smc_scc_num & NUM_IS_SCC) {
-		sccp = &pquicc->scc_regs[idx];
-		sccp->scc_sccm &= ~UART_SCCM_TX;
-	} else {
-		/* smcp = &cpmp->cp_smc[idx]; */
-		smcp = &pquicc->smc_regs[idx];
-		smcp->smc_smcm &= ~SMCM_TX;
-	}
-	local_irq_restore(flags);
-}
-
-
-static void rs_360_start(struct tty_struct *tty)
-{
-	ser_info_t *info = (ser_info_t *)tty->driver_data;
-	int	idx;
-	unsigned long flags;
-	volatile struct scc_regs *sccp;
-	volatile struct smc_regs *smcp;
-
-	if (serial_paranoia_check(info, tty->name, "rs_stop"))
-		return;
-	
-	local_irq_save(flags);
-	idx = PORT_NUM(info->state->smc_scc_num);
-	if (info->state->smc_scc_num & NUM_IS_SCC) {
-		sccp = &pquicc->scc_regs[idx];
-		sccp->scc_sccm |= UART_SCCM_TX;
-	} else {
-		smcp = &pquicc->smc_regs[idx];
-		smcp->smc_smcm |= SMCM_TX;
-	}
-	local_irq_restore(flags);
-}
-
-/*
- * ----------------------------------------------------------------------
- *
- * Here starts the interrupt handling routines.  All of the following
- * subroutines are declared as inline and are folded into
- * rs_interrupt().  They were separated out for readability's sake.
- *
- * Note: rs_interrupt() is a "fast" interrupt, which means that it
- * runs with interrupts turned off.  People who may want to modify
- * rs_interrupt() should try to keep the interrupt handler as fast as
- * possible.  After you are done making modifications, it is not a bad
- * idea to do:
- * 
- * gcc -S -DKERNEL -Wall -Wstrict-prototypes -O6 -fomit-frame-pointer serial.c
- *
- * and look at the resulting assemble code in serial.s.
- *
- * 				- Ted Ts'o (tytso@mit.edu), 7-Mar-93
- * -----------------------------------------------------------------------
- */
-
-static _INLINE_ void receive_chars(ser_info_t *info)
-{
-	struct tty_struct *tty = info->port.tty;
-	unsigned char ch, flag, *cp;
-	/*int	ignored = 0;*/
-	int	i;
-	ushort	status;
-	 struct	async_icount *icount; 
-	/* struct	async_icount_24 *icount; */
-	volatile QUICC_BD	*bdp;
-
-	icount = &info->state->icount;
-
-	/* Just loop through the closed BDs and copy the characters into
-	 * the buffer.
-	 */
-	bdp = info->rx_cur;
-	for (;;) {
-		if (bdp->status & BD_SC_EMPTY)	/* If this one is empty */
-			break;			/*   we are all done */
-
-		/* The read status mask tell us what we should do with
-		 * incoming characters, especially if errors occur.
-		 * One special case is the use of BD_SC_EMPTY.  If
-		 * this is not set, we are supposed to be ignoring
-		 * inputs.  In this case, just mark the buffer empty and
-		 * continue.
-		 */
-		if (!(info->read_status_mask & BD_SC_EMPTY)) {
-			bdp->status |= BD_SC_EMPTY;
-			bdp->status &=
-				~(BD_SC_BR | BD_SC_FR | BD_SC_PR | BD_SC_OV);
-
-			if (bdp->status & BD_SC_WRAP)
-				bdp = info->rx_bd_base;
-			else
-				bdp++;
-			continue;
-		}
-
-		/* Get the number of characters and the buffer pointer.
-		*/
-		i = bdp->length;
-		/* cp = (unsigned char *)__va(bdp->buf); */
-		cp = (char *)bdp->buf;
-		status = bdp->status;
-
-		while (i-- > 0) {
-			ch = *cp++;
-			icount->rx++;
-
-#ifdef SERIAL_DEBUG_INTR
-			printk("DR%02x:%02x...", ch, status);
-#endif
-			flag = TTY_NORMAL;
-
-			if (status & (BD_SC_BR | BD_SC_FR |
-				       BD_SC_PR | BD_SC_OV)) {
-				/*
-				 * For statistics only
-				 */
-				if (status & BD_SC_BR)
-					icount->brk++;
-				else if (status & BD_SC_PR)
-					icount->parity++;
-				else if (status & BD_SC_FR)
-					icount->frame++;
-				if (status & BD_SC_OV)
-					icount->overrun++;
-
-				/*
-				 * Now check to see if character should be
-				 * ignored, and mask off conditions which
-				 * should be ignored.
-				if (status & info->ignore_status_mask) {
-					if (++ignored > 100)
-						break;
-					continue;
-				}
-				 */
-				status &= info->read_status_mask;
-		
-				if (status & (BD_SC_BR)) {
-#ifdef SERIAL_DEBUG_INTR
-					printk("handling break....");
-#endif
-					*tty->flip.flag_buf_ptr = TTY_BREAK;
-					if (info->flags & ASYNC_SAK)
-						do_SAK(tty);
-				} else if (status & BD_SC_PR)
-					flag = TTY_PARITY;
-				else if (status & BD_SC_FR)
-					flag = TTY_FRAME;
-			}
-			tty_insert_flip_char(tty, ch, flag);
-			if (status & BD_SC_OV)
-				/*
-				 * Overrun is special, since it's
-				 * reported immediately, and doesn't
-				 * affect the current character
-				 */
-				tty_insert_flip_char(tty, 0, TTY_OVERRUN);
-		}
-
-		/* This BD is ready to be used again.  Clear status.
-		 * Get next BD.
-		 */
-		bdp->status |= BD_SC_EMPTY;
-		bdp->status &= ~(BD_SC_BR | BD_SC_FR | BD_SC_PR | BD_SC_OV);
-
-		if (bdp->status & BD_SC_WRAP)
-			bdp = info->rx_bd_base;
-		else
-			bdp++;
-	}
-
-	info->rx_cur = (QUICC_BD *)bdp;
-
-	tty_schedule_flip(tty);
-}
-
-static _INLINE_ void receive_break(ser_info_t *info)
-{
-	struct tty_struct *tty = info->port.tty;
-
-	info->state->icount.brk++;
-	/* Check to see if there is room in the tty buffer for
-	 * the break.  If not, we exit now, losing the break.  FIXME
-	 */
-	tty_insert_flip_char(tty, 0, TTY_BREAK);
-	tty_schedule_flip(tty);
-}
-
-static _INLINE_ void transmit_chars(ser_info_t *info)
-{
-
-	if ((info->flags & TX_WAKEUP) ||
-	    (info->port.tty->flags & (1 << TTY_DO_WRITE_WAKEUP))) {
-		schedule_work(&info->tqueue);
-	}
-
-#ifdef SERIAL_DEBUG_INTR
-	printk("THRE...");
-#endif
-}
-
-#ifdef notdef
-	/* I need to do this for the SCCs, so it is left as a reminder.
-	*/
-static _INLINE_ void check_modem_status(struct async_struct *info)
-{
-	int	status;
-	/* struct	async_icount *icount; */
-	struct	async_icount_24 *icount;
-	
-	status = serial_in(info, UART_MSR);
-
-	if (status & UART_MSR_ANY_DELTA) {
-		icount = &info->state->icount;
-		/* update input line counters */
-		if (status & UART_MSR_TERI)
-			icount->rng++;
-		if (status & UART_MSR_DDSR)
-			icount->dsr++;
-		if (status & UART_MSR_DDCD) {
-			icount->dcd++;
-#ifdef CONFIG_HARD_PPS
-			if ((info->flags & ASYNC_HARDPPS_CD) &&
-			    (status & UART_MSR_DCD))
-				hardpps();
-#endif
-		}
-		if (status & UART_MSR_DCTS)
-			icount->cts++;
-		wake_up_interruptible(&info->delta_msr_wait);
-	}
-
-	if ((info->flags & ASYNC_CHECK_CD) && (status & UART_MSR_DDCD)) {
-#if (defined(SERIAL_DEBUG_OPEN) || defined(SERIAL_DEBUG_INTR))
-		printk("ttys%d CD now %s...", info->line,
-		       (status & UART_MSR_DCD) ? "on" : "off");
-#endif		
-		if (status & UART_MSR_DCD)
-			wake_up_interruptible(&info->open_wait);
-		else {
-#ifdef SERIAL_DEBUG_OPEN
-			printk("scheduling hangup...");
-#endif
-			queue_task(&info->tqueue_hangup,
-					   &tq_scheduler);
-		}
-	}
-	if (info->flags & ASYNC_CTS_FLOW) {
-		if (info->port.tty->hw_stopped) {
-			if (status & UART_MSR_CTS) {
-#if (defined(SERIAL_DEBUG_INTR) || defined(SERIAL_DEBUG_FLOW))
-				printk("CTS tx start...");
-#endif
-				info->port.tty->hw_stopped = 0;
-				info->IER |= UART_IER_THRI;
-				serial_out(info, UART_IER, info->IER);
-				rs_sched_event(info, RS_EVENT_WRITE_WAKEUP);
-				return;
-			}
-		} else {
-			if (!(status & UART_MSR_CTS)) {
-#if (defined(SERIAL_DEBUG_INTR) || defined(SERIAL_DEBUG_FLOW))
-				printk("CTS tx stop...");
-#endif
-				info->port.tty->hw_stopped = 1;
-				info->IER &= ~UART_IER_THRI;
-				serial_out(info, UART_IER, info->IER);
-			}
-		}
-	}
-}
-#endif
-
-/*
- * This is the serial driver's interrupt routine for a single port
- */
-/* static void rs_360_interrupt(void *dev_id) */ /* until and if we start servicing irqs here */
-static void rs_360_interrupt(int vec, void *dev_id)
-{
-	u_char	events;
-	int	idx;
-	ser_info_t *info;
-	volatile struct smc_regs *smcp;
-	volatile struct scc_regs *sccp;
-	
-	info = dev_id;
-
-	idx = PORT_NUM(info->state->smc_scc_num);
-	if (info->state->smc_scc_num & NUM_IS_SCC) {
-		sccp = &pquicc->scc_regs[idx];
-		events = sccp->scc_scce;
-		if (events & SCCM_RX)
-			receive_chars(info);
-		if (events & SCCM_TX)
-			transmit_chars(info);
-		sccp->scc_scce = events;
-	} else {
-		smcp = &pquicc->smc_regs[idx];
-		events = smcp->smc_smce;
-		if (events & SMCM_BRKE)
-			receive_break(info);
-		if (events & SMCM_RX)
-			receive_chars(info);
-		if (events & SMCM_TX)
-			transmit_chars(info);
-		smcp->smc_smce = events;
-	}
-	
-#ifdef SERIAL_DEBUG_INTR
-	printk("rs_interrupt_single(%d, %x)...",
-					info->state->smc_scc_num, events);
-#endif
-#ifdef modem_control
-	check_modem_status(info);
-#endif
-	info->last_active = jiffies;
-#ifdef SERIAL_DEBUG_INTR
-	printk("end.\n");
-#endif
-}
-
-
-/*
- * -------------------------------------------------------------------
- * Here ends the serial interrupt routines.
- * -------------------------------------------------------------------
- */
-
-
-static void do_softint(void *private_)
-{
-	ser_info_t	*info = (ser_info_t *) private_;
-	struct tty_struct	*tty;
-	
-	tty = info->port.tty;
-	if (!tty)
-		return;
-
-	if (test_and_clear_bit(RS_EVENT_WRITE_WAKEUP, &info->event))
-		tty_wakeup(tty);
-}
-
-
-/*
- * This routine is called from the scheduler tqueue when the interrupt
- * routine has signalled that a hangup has occurred.  The path of
- * hangup processing is:
- *
- * 	serial interrupt routine -> (scheduler tqueue) ->
- * 	do_serial_hangup() -> tty->hangup() -> rs_hangup()
- * 
- */
-static void do_serial_hangup(void *private_)
-{
-	struct async_struct	*info = (struct async_struct *) private_;
-	struct tty_struct	*tty;
-	
-	tty = info->port.tty;
-	if (!tty)
-		return;
-
-	tty_hangup(tty);
-}
-
-
-static int startup(ser_info_t *info)
-{
-	unsigned long flags;
-	int	retval=0;
-	int	idx;
-	/*struct serial_state *state = info->state;*/
-	volatile struct smc_regs *smcp;
-	volatile struct scc_regs *sccp;
-	volatile struct smc_uart_pram	*up;
-	volatile struct uart_pram	    *scup;
-
-
-	local_irq_save(flags);
-
-	if (info->flags & ASYNC_INITIALIZED) {
-		goto errout;
-	}
-
-#ifdef maybe
-	if (!state->port || !state->type) {
-		if (info->port.tty)
-			set_bit(TTY_IO_ERROR, &info->port.tty->flags);
-		goto errout;
-	}
-#endif
-
-#ifdef SERIAL_DEBUG_OPEN
-	printk("starting up ttys%d (irq %d)...", info->line, state->irq);
-#endif
-
-
-#ifdef modem_control
-	info->MCR = 0;
-	if (info->port.tty->termios->c_cflag & CBAUD)
-		info->MCR = UART_MCR_DTR | UART_MCR_RTS;
-#endif
-	
-	if (info->port.tty)
-		clear_bit(TTY_IO_ERROR, &info->port.tty->flags);
-
-	/*
-	 * and set the speed of the serial port
-	 */
-	change_speed(info);
-
-	idx = PORT_NUM(info->state->smc_scc_num);
-	if (info->state->smc_scc_num & NUM_IS_SCC) {
-		sccp = &pquicc->scc_regs[idx];
-		scup = &pquicc->pram[info->state->port].scc.pscc.u;
-
-		scup->mrblr = RX_BUF_SIZE;
-		scup->max_idl = RX_BUF_SIZE;
-
-		sccp->scc_sccm |= (UART_SCCM_TX | UART_SCCM_RX);
-		sccp->scc_gsmr.w.low |= (SCC_GSMRL_ENR | SCC_GSMRL_ENT);
-
-	} else {
-		smcp = &pquicc->smc_regs[idx];
-
-		/* Enable interrupts and I/O.
-		*/
-		smcp->smc_smcm |= (SMCM_RX | SMCM_TX);
-		smcp->smc_smcmr |= (SMCMR_REN | SMCMR_TEN);
-
-		/* We can tune the buffer length and idle characters
-		 * to take advantage of the entire incoming buffer size.
-		 * If mrblr is something other than 1, maxidl has to be
-		 * non-zero or we never get an interrupt.  The maxidl
-		 * is the number of character times we wait after reception
-		 * of the last character before we decide no more characters
-		 * are coming.
-		 */
-		/* up = (smc_uart_t *)&pquicc->cp_dparam[state->port]; */
-		/* holy unionized structures, Batman: */
-		up = &pquicc->pram[info->state->port].scc.pothers.idma_smc.psmc.u;
-
-		up->mrblr = RX_BUF_SIZE;
-		up->max_idl = RX_BUF_SIZE;
-
-		up->brkcr = 1;	/* number of break chars */
-	}
-
-	info->flags |= ASYNC_INITIALIZED;
-	local_irq_restore(flags);
-	return 0;
-	
-errout:
-	local_irq_restore(flags);
-	return retval;
-}
-
-/*
- * This routine will shutdown a serial port; interrupts are disabled, and
- * DTR is dropped if the hangup on close termio flag is on.
- */
-static void shutdown(ser_info_t *info)
-{
-	unsigned long	flags;
-	struct serial_state *state;
-	int		idx;
-	volatile struct smc_regs	*smcp;
-	volatile struct scc_regs	*sccp;
-
-	if (!(info->flags & ASYNC_INITIALIZED))
-		return;
-
-	state = info->state;
-
-#ifdef SERIAL_DEBUG_OPEN
-	printk("Shutting down serial port %d (irq %d)....", info->line,
-	       state->irq);
-#endif
-	
-	local_irq_save(flags);
-
-	idx = PORT_NUM(state->smc_scc_num);
-	if (state->smc_scc_num & NUM_IS_SCC) {
-		sccp = &pquicc->scc_regs[idx];
-		sccp->scc_gsmr.w.low &= ~(SCC_GSMRL_ENR | SCC_GSMRL_ENT);
-#ifdef CONFIG_SERIAL_CONSOLE
-		/* We can't disable the transmitter if this is the
-		 * system console.
-		 */
-		if ((state - rs_table) != CONFIG_SERIAL_CONSOLE_PORT)
-#endif
-		sccp->scc_sccm &= ~(UART_SCCM_TX | UART_SCCM_RX);
-	} else {
-		smcp = &pquicc->smc_regs[idx];
-
-		/* Disable interrupts and I/O.
-		 */
-		smcp->smc_smcm &= ~(SMCM_RX | SMCM_TX);
-#ifdef CONFIG_SERIAL_CONSOLE
-		/* We can't disable the transmitter if this is the
-		 * system console.
-		 */
-		if ((state - rs_table) != CONFIG_SERIAL_CONSOLE_PORT)
-#endif
-			smcp->smc_smcmr &= ~(SMCMR_REN | SMCMR_TEN);
-	}
-	
-	if (info->port.tty)
-		set_bit(TTY_IO_ERROR, &info->port.tty->flags);
-
-	info->flags &= ~ASYNC_INITIALIZED;
-	local_irq_restore(flags);
-}
-
-/*
- * This routine is called to set the UART divisor registers to match
- * the specified baud rate for a serial port.
- */
-static void change_speed(ser_info_t *info)
-{
-	int	baud_rate;
-	unsigned cflag, cval, scval, prev_mode;
-	int	i, bits, sbits, idx;
-	unsigned long	flags;
-	struct serial_state *state;
-	volatile struct smc_regs	*smcp;
-	volatile struct scc_regs	*sccp;
-
-	if (!info->port.tty || !info->port.tty->termios)
-		return;
-	cflag = info->port.tty->termios->c_cflag;
-
-	state = info->state;
-
-	/* Character length programmed into the mode register is the
-	 * sum of: 1 start bit, number of data bits, 0 or 1 parity bit,
-	 * 1 or 2 stop bits, minus 1.
-	 * The value 'bits' counts this for us.
-	 */
-	cval = 0;
-	scval = 0;
-
-	/* byte size and parity */
-	switch (cflag & CSIZE) {
-	      case CS5: bits = 5; break;
-	      case CS6: bits = 6; break;
-	      case CS7: bits = 7; break;
-	      case CS8: bits = 8; break;
-	      /* Never happens, but GCC is too dumb to figure it out */
-	      default:  bits = 8; break;
-	}
-	sbits = bits - 5;
-
-	if (cflag & CSTOPB) {
-		cval |= SMCMR_SL;	/* Two stops */
-		scval |= SCU_PMSR_SL;
-		bits++;
-	}
-	if (cflag & PARENB) {
-		cval |= SMCMR_PEN;
-		scval |= SCU_PMSR_PEN;
-		bits++;
-	}
-	if (!(cflag & PARODD)) {
-		cval |= SMCMR_PM_EVEN;
-		scval |= (SCU_PMSR_REVP | SCU_PMSR_TEVP);
-	}
-
-	/* Determine divisor based on baud rate */
-	i = cflag & CBAUD;
-	if (i >= (sizeof(baud_table)/sizeof(int)))
-		baud_rate = 9600;
-	else
-		baud_rate = baud_table[i];
-
-	info->timeout = (TX_BUF_SIZE*HZ*bits);
-	info->timeout += HZ/50;		/* Add .02 seconds of slop */
-
-#ifdef modem_control
-	/* CTS flow control flag and modem status interrupts */
-	info->IER &= ~UART_IER_MSI;
-	if (info->flags & ASYNC_HARDPPS_CD)
-		info->IER |= UART_IER_MSI;
-	if (cflag & CRTSCTS) {
-		info->flags |= ASYNC_CTS_FLOW;
-		info->IER |= UART_IER_MSI;
-	} else
-		info->flags &= ~ASYNC_CTS_FLOW;
-	if (cflag & CLOCAL)
-		info->flags &= ~ASYNC_CHECK_CD;
-	else {
-		info->flags |= ASYNC_CHECK_CD;
-		info->IER |= UART_IER_MSI;
-	}
-	serial_out(info, UART_IER, info->IER);
-#endif
-
-	/*
-	 * Set up parity check flag
-	 */
-	info->read_status_mask = (BD_SC_EMPTY | BD_SC_OV);
-	if (I_INPCK(info->port.tty))
-		info->read_status_mask |= BD_SC_FR | BD_SC_PR;
-	if (I_BRKINT(info->port.tty) || I_PARMRK(info->port.tty))
-		info->read_status_mask |= BD_SC_BR;
-	
-	/*
-	 * Characters to ignore
-	 */
-	info->ignore_status_mask = 0;
-	if (I_IGNPAR(info->port.tty))
-		info->ignore_status_mask |= BD_SC_PR | BD_SC_FR;
-	if (I_IGNBRK(info->port.tty)) {
-		info->ignore_status_mask |= BD_SC_BR;
-		/*
-		 * If we're ignore parity and break indicators, ignore 
-		 * overruns too.  (For real raw support).
-		 */
-		if (I_IGNPAR(info->port.tty))
-			info->ignore_status_mask |= BD_SC_OV;
-	}
-	/*
-	 * !!! ignore all characters if CREAD is not set
-	 */
-	if ((cflag & CREAD) == 0)
-	 info->read_status_mask &= ~BD_SC_EMPTY;
-	 local_irq_save(flags);
-
-	 /* Start bit has not been added (so don't, because we would just
-	  * subtract it later), and we need to add one for the number of
-	  * stops bits (there is always at least one).
-	  */
-	 bits++;
-	 idx = PORT_NUM(state->smc_scc_num);
-	 if (state->smc_scc_num & NUM_IS_SCC) {
-         sccp = &pquicc->scc_regs[idx];
-         sccp->scc_psmr = (sbits << 12) | scval;
-     } else {
-         smcp = &pquicc->smc_regs[idx];
-
-		/* Set the mode register.  We want to keep a copy of the
-		 * enables, because we want to put them back if they were
-		 * present.
-		 */
-		prev_mode = smcp->smc_smcmr;
-		smcp->smc_smcmr = smcr_mk_clen(bits) | cval |  SMCMR_SM_UART;
-		smcp->smc_smcmr |= (prev_mode & (SMCMR_REN | SMCMR_TEN));
-	}
-
-	m360_cpm_setbrg((state - rs_table), baud_rate);
-
-	local_irq_restore(flags);
-}
-
-static void rs_360_put_char(struct tty_struct *tty, unsigned char ch)
-{
-	ser_info_t *info = (ser_info_t *)tty->driver_data;
-	volatile QUICC_BD	*bdp;
-
-	if (serial_paranoia_check(info, tty->name, "rs_put_char"))
-		return 0;
-
-	if (!tty)
-		return 0;
-
-	bdp = info->tx_cur;
-	while (bdp->status & BD_SC_READY);
-
-	/* *((char *)__va(bdp->buf)) = ch; */
-	*((char *)bdp->buf) = ch;
-	bdp->length = 1;
-	bdp->status |= BD_SC_READY;
-
-	/* Get next BD.
-	*/
-	if (bdp->status & BD_SC_WRAP)
-		bdp = info->tx_bd_base;
-	else
-		bdp++;
-
-	info->tx_cur = (QUICC_BD *)bdp;
-	return 1;
-
-}
-
-static int rs_360_write(struct tty_struct * tty,
-		    const unsigned char *buf, int count)
-{
-	int	c, ret = 0;
-	ser_info_t *info = (ser_info_t *)tty->driver_data;
-	volatile QUICC_BD *bdp;
-
-#ifdef CONFIG_KGDB
-	/* Try to let stub handle output. Returns true if it did. */ 
-	if (kgdb_output_string(buf, count))
-		return ret;
-#endif
-
-	if (serial_paranoia_check(info, tty->name, "rs_write"))
-		return 0;
-
-	if (!tty) 
-		return 0;
-
-	bdp = info->tx_cur;
-
-	while (1) {
-		c = min(count, TX_BUF_SIZE);
-
-		if (c <= 0)
-			break;
-
-		if (bdp->status & BD_SC_READY) {
-			info->flags |= TX_WAKEUP;
-			break;
-		}
-
-		/* memcpy(__va(bdp->buf), buf, c); */
-		memcpy((void *)bdp->buf, buf, c);
-
-		bdp->length = c;
-		bdp->status |= BD_SC_READY;
-
-		buf += c;
-		count -= c;
-		ret += c;
-
-		/* Get next BD.
-		*/
-		if (bdp->status & BD_SC_WRAP)
-			bdp = info->tx_bd_base;
-		else
-			bdp++;
-		info->tx_cur = (QUICC_BD *)bdp;
-	}
-	return ret;
-}
-
-static int rs_360_write_room(struct tty_struct *tty)
-{
-	ser_info_t *info = (ser_info_t *)tty->driver_data;
-	int	ret;
-
-	if (serial_paranoia_check(info, tty->name, "rs_write_room"))
-		return 0;
-
-	if ((info->tx_cur->status & BD_SC_READY) == 0) {
-		info->flags &= ~TX_WAKEUP;
-		ret = TX_BUF_SIZE;
-	}
-	else {
-		info->flags |= TX_WAKEUP;
-		ret = 0;
-	}
-	return ret;
-}
-
-/* I could track this with transmit counters....maybe later.
-*/
-static int rs_360_chars_in_buffer(struct tty_struct *tty)
-{
-	ser_info_t *info = (ser_info_t *)tty->driver_data;
-				
-	if (serial_paranoia_check(info, tty->name, "rs_chars_in_buffer"))
-		return 0;
-	return 0;
-}
-
-static void rs_360_flush_buffer(struct tty_struct *tty)
-{
-	ser_info_t *info = (ser_info_t *)tty->driver_data;
-				
-	if (serial_paranoia_check(info, tty->name, "rs_flush_buffer"))
-		return;
-
-	/* There is nothing to "flush", whatever we gave the CPM
-	 * is on its way out.
-	 */
-	tty_wakeup(tty);
-	info->flags &= ~TX_WAKEUP;
-}
-
-/*
- * This function is used to send a high-priority XON/XOFF character to
- * the device
- */
-static void rs_360_send_xchar(struct tty_struct *tty, char ch)
-{
-	volatile QUICC_BD	*bdp;
-
-	ser_info_t *info = (ser_info_t *)tty->driver_data;
-
-	if (serial_paranoia_check(info, tty->name, "rs_send_char"))
-		return;
-
-	bdp = info->tx_cur;
-	while (bdp->status & BD_SC_READY);
-
-	/* *((char *)__va(bdp->buf)) = ch; */
-	*((char *)bdp->buf) = ch;
-	bdp->length = 1;
-	bdp->status |= BD_SC_READY;
-
-	/* Get next BD.
-	*/
-	if (bdp->status & BD_SC_WRAP)
-		bdp = info->tx_bd_base;
-	else
-		bdp++;
-
-	info->tx_cur = (QUICC_BD *)bdp;
-}
-
-/*
- * ------------------------------------------------------------
- * rs_throttle()
- * 
- * This routine is called by the upper-layer tty layer to signal that
- * incoming characters should be throttled.
- * ------------------------------------------------------------
- */
-static void rs_360_throttle(struct tty_struct * tty)
-{
-	ser_info_t *info = (ser_info_t *)tty->driver_data;
-#ifdef SERIAL_DEBUG_THROTTLE
-	char	buf[64];
-	
-	printk("throttle %s: %d....\n", _tty_name(tty, buf),
-	       tty->ldisc.chars_in_buffer(tty));
-#endif
-
-	if (serial_paranoia_check(info, tty->name, "rs_throttle"))
-		return;
-	
-	if (I_IXOFF(tty))
-		rs_360_send_xchar(tty, STOP_CHAR(tty));
-
-#ifdef modem_control
-	if (tty->termios->c_cflag & CRTSCTS)
-		info->MCR &= ~UART_MCR_RTS;
-
-	local_irq_disable();
-	serial_out(info, UART_MCR, info->MCR);
-	local_irq_enable();
-#endif
-}
-
-static void rs_360_unthrottle(struct tty_struct * tty)
-{
-	ser_info_t *info = (ser_info_t *)tty->driver_data;
-#ifdef SERIAL_DEBUG_THROTTLE
-	char	buf[64];
-	
-	printk("unthrottle %s: %d....\n", _tty_name(tty, buf),
-	       tty->ldisc.chars_in_buffer(tty));
-#endif
-
-	if (serial_paranoia_check(info, tty->name, "rs_unthrottle"))
-		return;
-	
-	if (I_IXOFF(tty)) {
-		if (info->x_char)
-			info->x_char = 0;
-		else
-			rs_360_send_xchar(tty, START_CHAR(tty));
-	}
-#ifdef modem_control
-	if (tty->termios->c_cflag & CRTSCTS)
-		info->MCR |= UART_MCR_RTS;
-	local_irq_disable();
-	serial_out(info, UART_MCR, info->MCR);
-	local_irq_enable();
-#endif
-}
-
-/*
- * ------------------------------------------------------------
- * rs_ioctl() and friends
- * ------------------------------------------------------------
- */
-
-#ifdef maybe
-/*
- * get_lsr_info - get line status register info
- *
- * Purpose: Let user call ioctl() to get info when the UART physically
- * 	    is emptied.  On bus types like RS485, the transmitter must
- * 	    release the bus after transmitting. This must be done when
- * 	    the transmit shift register is empty, not be done when the
- * 	    transmit holding register is empty.  This functionality
- * 	    allows an RS485 driver to be written in user space. 
- */
-static int get_lsr_info(struct async_struct * info, unsigned int *value)
-{
-	unsigned char status;
-	unsigned int result;
-
-	local_irq_disable();
-	status = serial_in(info, UART_LSR);
-	local_irq_enable();
-	result = ((status & UART_LSR_TEMT) ? TIOCSER_TEMT : 0);
-	return put_user(result,value);
-}
-#endif
-
-static int rs_360_tiocmget(struct tty_struct *tty)
-{
-	ser_info_t *info = (ser_info_t *)tty->driver_data;
-	unsigned int result = 0;
-#ifdef modem_control
-	unsigned char control, status;
-
-	if (serial_paranoia_check(info, tty->name, __func__))
-		return -ENODEV;
-
-	if (tty->flags & (1 << TTY_IO_ERROR))
-		return -EIO;
-
-	control = info->MCR;
-	local_irq_disable();
-	status = serial_in(info, UART_MSR);
-	local_irq_enable();
-	result =  ((control & UART_MCR_RTS) ? TIOCM_RTS : 0)
-		| ((control & UART_MCR_DTR) ? TIOCM_DTR : 0)
-#ifdef TIOCM_OUT1
-		| ((control & UART_MCR_OUT1) ? TIOCM_OUT1 : 0)
-		| ((control & UART_MCR_OUT2) ? TIOCM_OUT2 : 0)
-#endif
-		| ((status  & UART_MSR_DCD) ? TIOCM_CAR : 0)
-		| ((status  & UART_MSR_RI) ? TIOCM_RNG : 0)
-		| ((status  & UART_MSR_DSR) ? TIOCM_DSR : 0)
-		| ((status  & UART_MSR_CTS) ? TIOCM_CTS : 0);
-#endif
-	return result;
-}
-
-static int rs_360_tiocmset(struct tty_struct *tty,
-			   unsigned int set, unsigned int clear)
-{
-#ifdef modem_control
-	ser_info_t *info = (ser_info_t *)tty->driver_data;
- 	unsigned int arg;
-
-	if (serial_paranoia_check(info, tty->name, __func__))
-		return -ENODEV;
-
-	if (tty->flags & (1 << TTY_IO_ERROR))
-		return -EIO;
-	/* FIXME: locking on info->mcr */
- 	if (set & TIOCM_RTS)
- 		info->mcr |= UART_MCR_RTS;
- 	if (set & TIOCM_DTR)
- 		info->mcr |= UART_MCR_DTR;
-	if (clear & TIOCM_RTS)
-		info->MCR &= ~UART_MCR_RTS;
-	if (clear & TIOCM_DTR)
-		info->MCR &= ~UART_MCR_DTR;
-
-#ifdef TIOCM_OUT1
-	if (set & TIOCM_OUT1)
-		info->MCR |= UART_MCR_OUT1;
-	if (set & TIOCM_OUT2)
-		info->MCR |= UART_MCR_OUT2;
-	if (clear & TIOCM_OUT1)
-		info->MCR &= ~UART_MCR_OUT1;
-	if (clear & TIOCM_OUT2)
-		info->MCR &= ~UART_MCR_OUT2;
-#endif
-
-	local_irq_disable();
-	serial_out(info, UART_MCR, info->MCR);
-	local_irq_enable();
-#endif
-	return 0;
-}
-
-/* Sending a break is a two step process on the SMC/SCC.  It is accomplished
- * by sending a STOP TRANSMIT command followed by a RESTART TRANSMIT
- * command.  We take advantage of the begin/end functions to make this
- * happen.
- */
-static ushort	smc_chan_map[] = {
-	CPM_CR_CH_SMC1,
-	CPM_CR_CH_SMC2
-};
-
-static ushort	scc_chan_map[] = {
-	CPM_CR_CH_SCC1,
-	CPM_CR_CH_SCC2,
-	CPM_CR_CH_SCC3,
-	CPM_CR_CH_SCC4
-};
-
-static void begin_break(ser_info_t *info)
-{
-	volatile QUICC *cp;
-	ushort	chan;
-	int     idx;
-
-	cp = pquicc;
-
-	idx = PORT_NUM(info->state->smc_scc_num);
-	if (info->state->smc_scc_num & NUM_IS_SCC)
-		chan = scc_chan_map[idx];
-	else
-		chan = smc_chan_map[idx];
-
-	cp->cp_cr = mk_cr_cmd(chan, CPM_CR_STOP_TX) | CPM_CR_FLG;
-	while (cp->cp_cr & CPM_CR_FLG);
-}
-
-static void end_break(ser_info_t *info)
-{
-	volatile QUICC *cp;
-	ushort	chan;
-	int idx;
-
-	cp = pquicc;
-
-	idx = PORT_NUM(info->state->smc_scc_num);
-	if (info->state->smc_scc_num & NUM_IS_SCC)
-		chan = scc_chan_map[idx];
-	else
-		chan = smc_chan_map[idx];
-
-	cp->cp_cr = mk_cr_cmd(chan, CPM_CR_RESTART_TX) | CPM_CR_FLG;
-	while (cp->cp_cr & CPM_CR_FLG);
-}
-
-/*
- * This routine sends a break character out the serial port.
- */
-static void send_break(ser_info_t *info, unsigned int duration)
-{
-#ifdef SERIAL_DEBUG_SEND_BREAK
-	printk("rs_send_break(%d) jiff=%lu...", duration, jiffies);
-#endif
-	begin_break(info);
-	msleep_interruptible(duration);
-	end_break(info);
-#ifdef SERIAL_DEBUG_SEND_BREAK
-	printk("done jiffies=%lu\n", jiffies);
-#endif
-}
-
-
-/*
- * Get counter of input serial line interrupts (DCD,RI,DSR,CTS)
- * Return: write counters to the user passed counter struct
- * NB: both 1->0 and 0->1 transitions are counted except for
- *     RI where only 0->1 is counted.
- */
-static int rs_360_get_icount(struct tty_struct *tty,
-				struct serial_icounter_struct *icount)
-{
-	ser_info_t *info = (ser_info_t *)tty->driver_data;
-	struct async_icount cnow;
-
-	local_irq_disable();
-	cnow = info->state->icount;
-	local_irq_enable();
-
-	icount->cts = cnow.cts;
-	icount->dsr = cnow.dsr;
-	icount->rng = cnow.rng;
-	icount->dcd = cnow.dcd;
-
-	return 0;
-}
-
-static int rs_360_ioctl(struct tty_struct *tty,
-		    unsigned int cmd, unsigned long arg)
-{
-	int error;
-	ser_info_t *info = (ser_info_t *)tty->driver_data;
-	int retval;
-	struct async_icount cnow; 
-	/* struct async_icount_24 cnow;*/ 	/* kernel counter temps */
-	struct serial_icounter_struct *p_cuser;	/* user space */
-
-	if (serial_paranoia_check(info, tty->name, "rs_ioctl"))
-		return -ENODEV;
-
-	if (cmd != TIOCMIWAIT) {
-		if (tty->flags & (1 << TTY_IO_ERROR))
-		    return -EIO;
-	}
-	
-	switch (cmd) {
-		case TCSBRK:	/* SVID version: non-zero arg --> no break */
-			retval = tty_check_change(tty);
-			if (retval)
-				return retval;
-			tty_wait_until_sent(tty, 0);
-			if (signal_pending(current))
-				return -EINTR;
-			if (!arg) {
-				send_break(info, 250);	/* 1/4 second */
-				if (signal_pending(current))
-					return -EINTR;
-			}
-			return 0;
-		case TCSBRKP:	/* support for POSIX tcsendbreak() */
-			retval = tty_check_change(tty);
-			if (retval)
-				return retval;
-			tty_wait_until_sent(tty, 0);
-			if (signal_pending(current))
-				return -EINTR;
-			send_break(info, arg ? arg*100 : 250);
-			if (signal_pending(current))
-				return -EINTR;
-			return 0;
-		case TIOCSBRK:
-			retval = tty_check_change(tty);
-			if (retval)
-				return retval;
-			tty_wait_until_sent(tty, 0);
-			begin_break(info);
-			return 0;
-		case TIOCCBRK:
-			retval = tty_check_change(tty);
-			if (retval)
-				return retval;
-			end_break(info);
-			return 0;
-#ifdef maybe
-		case TIOCSERGETLSR: /* Get line status register */
-			return get_lsr_info(info, (unsigned int *) arg);
-#endif
-		/*
-		 * Wait for any of the 4 modem inputs (DCD,RI,DSR,CTS) to change
-		 * - mask passed in arg for lines of interest
- 		 *   (use |'ed TIOCM_RNG/DSR/CD/CTS for masking)
-		 * Caller should use TIOCGICOUNT to see which one it was
-		 */
-		 case TIOCMIWAIT:
-#ifdef modem_control
-			local_irq_disable();
-			/* note the counters on entry */
-			cprev = info->state->icount;
-			local_irq_enable();
-			while (1) {
-				interruptible_sleep_on(&info->delta_msr_wait);
-				/* see if a signal did it */
-				if (signal_pending(current))
-					return -ERESTARTSYS;
-				local_irq_disable();
-				cnow = info->state->icount; /* atomic copy */
-				local_irq_enable();
-				if (cnow.rng == cprev.rng && cnow.dsr == cprev.dsr && 
-				    cnow.dcd == cprev.dcd && cnow.cts == cprev.cts)
-					return -EIO; /* no change => error */
-				if ( ((arg & TIOCM_RNG) && (cnow.rng != cprev.rng)) ||
-				     ((arg & TIOCM_DSR) && (cnow.dsr != cprev.dsr)) ||
-				     ((arg & TIOCM_CD)  && (cnow.dcd != cprev.dcd)) ||
-				     ((arg & TIOCM_CTS) && (cnow.cts != cprev.cts)) ) {
-					return 0;
-				}
-				cprev = cnow;
-			}
-			/* NOTREACHED */
-#else
-			return 0;
-#endif
-
-
-		default:
-			return -ENOIOCTLCMD;
-		}
-	return 0;
-}
-
-/* FIX UP modem control here someday......
-*/
-static void rs_360_set_termios(struct tty_struct *tty, struct ktermios *old_termios)
-{
-	ser_info_t *info = (ser_info_t *)tty->driver_data;
-
-	change_speed(info);
-
-#ifdef modem_control
-	/* Handle transition to B0 status */
-	if ((old_termios->c_cflag & CBAUD) &&
-	    !(tty->termios->c_cflag & CBAUD)) {
-		info->MCR &= ~(UART_MCR_DTR|UART_MCR_RTS);
-		local_irq_disable();
-		serial_out(info, UART_MCR, info->MCR);
-		local_irq_enable();
-	}
-	
-	/* Handle transition away from B0 status */
-	if (!(old_termios->c_cflag & CBAUD) &&
-	    (tty->termios->c_cflag & CBAUD)) {
-		info->MCR |= UART_MCR_DTR;
-		if (!tty->hw_stopped ||
-		    !(tty->termios->c_cflag & CRTSCTS)) {
-			info->MCR |= UART_MCR_RTS;
-		}
-		local_irq_disable();
-		serial_out(info, UART_MCR, info->MCR);
-		local_irq_enable();
-	}
-	
-	/* Handle turning off CRTSCTS */
-	if ((old_termios->c_cflag & CRTSCTS) &&
-	    !(tty->termios->c_cflag & CRTSCTS)) {
-		tty->hw_stopped = 0;
-		rs_360_start(tty);
-	}
-#endif
-
-#if 0
-	/*
-	 * No need to wake up processes in open wait, since they
-	 * sample the CLOCAL flag once, and don't recheck it.
-	 * XXX  It's not clear whether the current behavior is correct
-	 * or not.  Hence, this may change.....
-	 */
-	if (!(old_termios->c_cflag & CLOCAL) &&
-	    (tty->termios->c_cflag & CLOCAL))
-		wake_up_interruptible(&info->open_wait);
-#endif
-}
-
-/*
- * ------------------------------------------------------------
- * rs_close()
- * 
- * This routine is called when the serial port gets closed.  First, we
- * wait for the last remaining data to be sent.  Then, we unlink its
- * async structure from the interrupt chain if necessary, and we free
- * that IRQ if nothing is left in the chain.
- * ------------------------------------------------------------
- */
-static void rs_360_close(struct tty_struct *tty, struct file * filp)
-{
-	ser_info_t *info = (ser_info_t *)tty->driver_data;
-	/* struct async_state *state; */
-	struct serial_state *state;
-	unsigned long	flags;
-	int		idx;
-	volatile struct smc_regs	*smcp;
-	volatile struct scc_regs	*sccp;
-
-	if (!info || serial_paranoia_check(info, tty->name, "rs_close"))
-		return;
-
-	state = info->state;
-	
-	local_irq_save(flags);
-	
-	if (tty_hung_up_p(filp)) {
-		DBG_CNT("before DEC-hung");
-		local_irq_restore(flags);
-		return;
-	}
-	
-#ifdef SERIAL_DEBUG_OPEN
-	printk("rs_close ttys%d, count = %d\n", info->line, state->count);
-#endif
-	if ((tty->count == 1) && (state->count != 1)) {
-		/*
-		 * Uh, oh.  tty->count is 1, which means that the tty
-		 * structure will be freed.  state->count should always
-		 * be one in these conditions.  If it's greater than
-		 * one, we've got real problems, since it means the
-		 * serial port won't be shutdown.
-		 */
-		printk("rs_close: bad serial port count; tty->count is 1, "
-		       "state->count is %d\n", state->count);
-		state->count = 1;
-	}
-	if (--state->count < 0) {
-		printk("rs_close: bad serial port count for ttys%d: %d\n",
-		       info->line, state->count);
-		state->count = 0;
-	}
-	if (state->count) {
-		DBG_CNT("before DEC-2");
-		local_irq_restore(flags);
-		return;
-	}
-	info->flags |= ASYNC_CLOSING;
-	/*
-	 * Now we wait for the transmit buffer to clear; and we notify 
-	 * the line discipline to only process XON/XOFF characters.
-	 */
-	tty->closing = 1;
-	if (info->closing_wait != ASYNC_CLOSING_WAIT_NONE)
-		tty_wait_until_sent(tty, info->closing_wait);
-	/*
-	 * At this point we stop accepting input.  To do this, we
-	 * disable the receive line status interrupts, and tell the
-	 * interrupt driver to stop checking the data ready bit in the
-	 * line status register.
-	 */
-	info->read_status_mask &= ~BD_SC_EMPTY;
-	if (info->flags & ASYNC_INITIALIZED) {
-
-		idx = PORT_NUM(info->state->smc_scc_num);
-		if (info->state->smc_scc_num & NUM_IS_SCC) {
-			sccp = &pquicc->scc_regs[idx];
-			sccp->scc_sccm &= ~UART_SCCM_RX;
-			sccp->scc_gsmr.w.low &= ~SCC_GSMRL_ENR;
-		} else {
-			smcp = &pquicc->smc_regs[idx];
-			smcp->smc_smcm &= ~SMCM_RX;
-			smcp->smc_smcmr &= ~SMCMR_REN;
-		}
-		/*
-		 * Before we drop DTR, make sure the UART transmitter
-		 * has completely drained; this is especially
-		 * important if there is a transmit FIFO!
-		 */
-		rs_360_wait_until_sent(tty, info->timeout);
-	}
-	shutdown(info);
-	rs_360_flush_buffer(tty);
-	tty_ldisc_flush(tty);		
-	tty->closing = 0;
-	info->event = 0;
-	info->port.tty = NULL;
-	if (info->blocked_open) {
-		if (info->close_delay) {
-			msleep_interruptible(jiffies_to_msecs(info->close_delay));
-		}
-		wake_up_interruptible(&info->open_wait);
-	}
-	info->flags &= ~(ASYNC_NORMAL_ACTIVE|ASYNC_CLOSING);
-	wake_up_interruptible(&info->close_wait);
-	local_irq_restore(flags);
-}
-
-/*
- * rs_wait_until_sent() --- wait until the transmitter is empty
- */
-static void rs_360_wait_until_sent(struct tty_struct *tty, int timeout)
-{
-	ser_info_t *info = (ser_info_t *)tty->driver_data;
-	unsigned long orig_jiffies, char_time;
-	/*int lsr;*/
-	volatile QUICC_BD *bdp;
-	
-	if (serial_paranoia_check(info, tty->name, "rs_wait_until_sent"))
-		return;
-
-#ifdef maybe
-	if (info->state->type == PORT_UNKNOWN)
-		return;
-#endif
-
-	orig_jiffies = jiffies;
-	/*
-	 * Set the check interval to be 1/5 of the estimated time to
-	 * send a single character, and make it at least 1.  The check
-	 * interval should also be less than the timeout.
-	 * 
-	 * Note: we have to use pretty tight timings here to satisfy
-	 * the NIST-PCTS.
-	 */
-	char_time = 1;
-	if (timeout)
-		char_time = min(char_time, (unsigned long)timeout);
-#ifdef SERIAL_DEBUG_RS_WAIT_UNTIL_SENT
-	printk("In rs_wait_until_sent(%d) check=%lu...", timeout, char_time);
-	printk("jiff=%lu...", jiffies);
-#endif
-
-	/* We go through the loop at least once because we can't tell
-	 * exactly when the last character exits the shifter.  There can
-	 * be at least two characters waiting to be sent after the buffers
-	 * are empty.
-	 */
-	do {
-#ifdef SERIAL_DEBUG_RS_WAIT_UNTIL_SENT
-		printk("lsr = %d (jiff=%lu)...", lsr, jiffies);
-#endif
-/*		current->counter = 0;	 make us low-priority */
-		msleep_interruptible(jiffies_to_msecs(char_time));
-		if (signal_pending(current))
-			break;
-		if (timeout && (time_after(jiffies, orig_jiffies + timeout)))
-			break;
-		/* The 'tx_cur' is really the next buffer to send.  We
-		 * have to back up to the previous BD and wait for it
-		 * to go.  This isn't perfect, because all this indicates
-		 * is the buffer is available.  There are still characters
-		 * in the CPM FIFO.
-		 */
-		bdp = info->tx_cur;
-		if (bdp == info->tx_bd_base)
-			bdp += (TX_NUM_FIFO-1);
-		else
-			bdp--;
-	} while (bdp->status & BD_SC_READY);
-	current->state = TASK_RUNNING;
-#ifdef SERIAL_DEBUG_RS_WAIT_UNTIL_SENT
-	printk("lsr = %d (jiff=%lu)...done\n", lsr, jiffies);
-#endif
-}
-
-/*
- * rs_hangup() --- called by tty_hangup() when a hangup is signaled.
- */
-static void rs_360_hangup(struct tty_struct *tty)
-{
-	ser_info_t *info = (ser_info_t *)tty->driver_data;
-	struct serial_state *state = info->state;
-	
-	if (serial_paranoia_check(info, tty->name, "rs_hangup"))
-		return;
-
-	state = info->state;
-	
-	rs_360_flush_buffer(tty);
-	shutdown(info);
-	info->event = 0;
-	state->count = 0;
-	info->flags &= ~ASYNC_NORMAL_ACTIVE;
-	info->port.tty = NULL;
-	wake_up_interruptible(&info->open_wait);
-}
-
-/*
- * ------------------------------------------------------------
- * rs_open() and friends
- * ------------------------------------------------------------
- */
-static int block_til_ready(struct tty_struct *tty, struct file * filp,
-			   ser_info_t *info)
-{
-#ifdef DO_THIS_LATER
-	DECLARE_WAITQUEUE(wait, current);
-#endif
-	struct serial_state *state = info->state;
-	int		retval;
-	int		do_clocal = 0;
-
-	/*
-	 * If the device is in the middle of being closed, then block
-	 * until it's done, and then try again.
-	 */
-	if (tty_hung_up_p(filp) ||
-	    (info->flags & ASYNC_CLOSING)) {
-		if (info->flags & ASYNC_CLOSING)
-			interruptible_sleep_on(&info->close_wait);
-#ifdef SERIAL_DO_RESTART
-		if (info->flags & ASYNC_HUP_NOTIFY)
-			return -EAGAIN;
-		else
-			return -ERESTARTSYS;
-#else
-		return -EAGAIN;
-#endif
-	}
-
-	/*
-	 * If non-blocking mode is set, or the port is not enabled,
-	 * then make the check up front and then exit.
-	 * If this is an SMC port, we don't have modem control to wait
-	 * for, so just get out here.
-	 */
-	if ((filp->f_flags & O_NONBLOCK) ||
-	    (tty->flags & (1 << TTY_IO_ERROR)) ||
-	    !(info->state->smc_scc_num & NUM_IS_SCC)) {
-		info->flags |= ASYNC_NORMAL_ACTIVE;
-		return 0;
-	}
-
-	if (tty->termios->c_cflag & CLOCAL)
-		do_clocal = 1;
-	
-	/*
-	 * Block waiting for the carrier detect and the line to become
-	 * free (i.e., not in use by the callout).  While we are in
-	 * this loop, state->count is dropped by one, so that
-	 * rs_close() knows when to free things.  We restore it upon
-	 * exit, either normal or abnormal.
-	 */
-	retval = 0;
-#ifdef DO_THIS_LATER
-	add_wait_queue(&info->open_wait, &wait);
-#ifdef SERIAL_DEBUG_OPEN
-	printk("block_til_ready before block: ttys%d, count = %d\n",
-	       state->line, state->count);
-#endif
-	local_irq_disable();
-	if (!tty_hung_up_p(filp)) 
-		state->count--;
-	local_irq_enable();
-	info->blocked_open++;
-	while (1) {
-		local_irq_disable();
-		if (tty->termios->c_cflag & CBAUD)
-			serial_out(info, UART_MCR,
-				   serial_inp(info, UART_MCR) |
-				   (UART_MCR_DTR | UART_MCR_RTS));
-		local_irq_enable();
-		set_current_state(TASK_INTERRUPTIBLE);
-		if (tty_hung_up_p(filp) ||
-		    !(info->flags & ASYNC_INITIALIZED)) {
-#ifdef SERIAL_DO_RESTART
-			if (info->flags & ASYNC_HUP_NOTIFY)
-				retval = -EAGAIN;
-			else
-				retval = -ERESTARTSYS;	
-#else
-			retval = -EAGAIN;
-#endif
-			break;
-		}
-		if (!(info->flags & ASYNC_CLOSING) &&
-		    (do_clocal || (serial_in(info, UART_MSR) &
-				   UART_MSR_DCD)))
-			break;
-		if (signal_pending(current)) {
-			retval = -ERESTARTSYS;
-			break;
-		}
-#ifdef SERIAL_DEBUG_OPEN
-		printk("block_til_ready blocking: ttys%d, count = %d\n",
-		       info->line, state->count);
-#endif
-		tty_unlock();
-		schedule();
-		tty_lock();
-	}
-	current->state = TASK_RUNNING;
-	remove_wait_queue(&info->open_wait, &wait);
-	if (!tty_hung_up_p(filp))
-		state->count++;
-	info->blocked_open--;
-#ifdef SERIAL_DEBUG_OPEN
-	printk("block_til_ready after blocking: ttys%d, count = %d\n",
-	       info->line, state->count);
-#endif
-#endif /* DO_THIS_LATER */
-	if (retval)
-		return retval;
-	info->flags |= ASYNC_NORMAL_ACTIVE;
-	return 0;
-}
-
-static int get_async_struct(int line, ser_info_t **ret_info)
-{
-	struct serial_state *sstate;
-
-	sstate = rs_table + line;
-	if (sstate->info) {
-		sstate->count++;
-		*ret_info = (ser_info_t *)sstate->info;
-		return 0;
-	}
-	else {
-		return -ENOMEM;
-	}
-}
-
-/*
- * This routine is called whenever a serial port is opened.  It
- * enables interrupts for a serial port, linking in its async structure into
- * the IRQ chain.   It also performs the serial-specific
- * initialization for the tty structure.
- */
-static int rs_360_open(struct tty_struct *tty, struct file * filp)
-{
-	ser_info_t	*info;
-	int 		retval, line;
-
-	line = tty->index;
-	if ((line < 0) || (line >= NR_PORTS))
-		return -ENODEV;
-	retval = get_async_struct(line, &info);
-	if (retval)
-		return retval;
-	if (serial_paranoia_check(info, tty->name, "rs_open"))
-		return -ENODEV;
-
-#ifdef SERIAL_DEBUG_OPEN
-	printk("rs_open %s, count = %d\n", tty->name, info->state->count);
-#endif
-	tty->driver_data = info;
-	info->port.tty = tty;
-
-	/*
-	 * Start up serial port
-	 */
-	retval = startup(info);
-	if (retval)
-		return retval;
-
-	retval = block_til_ready(tty, filp, info);
-	if (retval) {
-#ifdef SERIAL_DEBUG_OPEN
-		printk("rs_open returning after block_til_ready with %d\n",
-		       retval);
-#endif
-		return retval;
-	}
-
-#ifdef SERIAL_DEBUG_OPEN
-	printk("rs_open %s successful...", tty->name);
-#endif
-	return 0;
-}
-
-/*
- * /proc fs routines....
- */
-
-static inline int line_info(char *buf, struct serial_state *state)
-{
-#ifdef notdef
-	struct async_struct *info = state->info, scr_info;
-	char	stat_buf[30], control, status;
-#endif
-	int	ret;
-
-	ret = sprintf(buf, "%d: uart:%s port:%X irq:%d",
-		      state->line,
-		      (state->smc_scc_num & NUM_IS_SCC) ? "SCC" : "SMC",
-		      (unsigned int)(state->port), state->irq);
-
-	if (!state->port || (state->type == PORT_UNKNOWN)) {
-		ret += sprintf(buf+ret, "\n");
-		return ret;
-	}
-
-#ifdef notdef
-	/*
-	 * Figure out the current RS-232 lines
-	 */
-	if (!info) {
-		info = &scr_info;	/* This is just for serial_{in,out} */
-
-		info->magic = SERIAL_MAGIC;
-		info->port = state->port;
-		info->flags = state->flags;
-		info->quot = 0;
-		info->port.tty = NULL;
-	}
-	local_irq_disable();
-	status = serial_in(info, UART_MSR);
-	control = info ? info->MCR : serial_in(info, UART_MCR);
-	local_irq_enable();
-	
-	stat_buf[0] = 0;
-	stat_buf[1] = 0;
-	if (control & UART_MCR_RTS)
-		strcat(stat_buf, "|RTS");
-	if (status & UART_MSR_CTS)
-		strcat(stat_buf, "|CTS");
-	if (control & UART_MCR_DTR)
-		strcat(stat_buf, "|DTR");
-	if (status & UART_MSR_DSR)
-		strcat(stat_buf, "|DSR");
-	if (status & UART_MSR_DCD)
-		strcat(stat_buf, "|CD");
-	if (status & UART_MSR_RI)
-		strcat(stat_buf, "|RI");
-
-	if (info->quot) {
-		ret += sprintf(buf+ret, " baud:%d",
-			       state->baud_base / info->quot);
-	}
-
-	ret += sprintf(buf+ret, " tx:%d rx:%d",
-		      state->icount.tx, state->icount.rx);
-
-	if (state->icount.frame)
-		ret += sprintf(buf+ret, " fe:%d", state->icount.frame);
-	
-	if (state->icount.parity)
-		ret += sprintf(buf+ret, " pe:%d", state->icount.parity);
-	
-	if (state->icount.brk)
-		ret += sprintf(buf+ret, " brk:%d", state->icount.brk);	
-
-	if (state->icount.overrun)
-		ret += sprintf(buf+ret, " oe:%d", state->icount.overrun);
-
-	/*
-	 * Last thing is the RS-232 status lines
-	 */
-	ret += sprintf(buf+ret, " %s\n", stat_buf+1);
-#endif
-	return ret;
-}
-
-int rs_360_read_proc(char *page, char **start, off_t off, int count,
-		 int *eof, void *data)
-{
-	int i, len = 0;
-	off_t	begin = 0;
-
-	len += sprintf(page, "serinfo:1.0 driver:%s\n", serial_version);
-	for (i = 0; i < NR_PORTS && len < 4000; i++) {
-		len += line_info(page + len, &rs_table[i]);
-		if (len+begin > off+count)
-			goto done;
-		if (len+begin < off) {
-			begin += len;
-			len = 0;
-		}
-	}
-	*eof = 1;
-done:
-	if (off >= len+begin)
-		return 0;
-	*start = page + (begin-off);
-	return ((count < begin+len-off) ? count : begin+len-off);
-}
-
-/*
- * ---------------------------------------------------------------------
- * rs_init() and friends
- *
- * rs_init() is called at boot-time to initialize the serial driver.
- * ---------------------------------------------------------------------
- */
-
-/*
- * This routine prints out the appropriate serial driver version
- * number, and identifies which options were configured into this
- * driver.
- */
-static _INLINE_ void show_serial_version(void)
-{
- 	printk(KERN_INFO "%s version %s\n", serial_name, serial_version);
-}
-
-
-/*
- * The serial console driver used during boot.  Note that these names
- * clash with those found in "serial.c", so we currently can't support
- * the 16xxx uarts and these at the same time.  I will fix this to become
- * an indirect function call from tty_io.c (or something).
- */
-
-#ifdef CONFIG_SERIAL_CONSOLE
-
-/*
- * Print a string to the serial port trying not to disturb any possible
- * real use of the port...
- */
-static void my_console_write(int idx, const char *s,
-				unsigned count)
-{
-	struct		serial_state	*ser;
-	ser_info_t		*info;
-	unsigned		i;
-	QUICC_BD		*bdp, *bdbase;
-	volatile struct smc_uart_pram	*up;
-	volatile	u_char		*cp;
-
-	ser = rs_table + idx;
-
-
-	/* If the port has been initialized for general use, we have
-	 * to use the buffer descriptors allocated there.  Otherwise,
-	 * we simply use the single buffer allocated.
-	 */
-	if ((info = (ser_info_t *)ser->info) != NULL) {
-		bdp = info->tx_cur;
-		bdbase = info->tx_bd_base;
-	}
-	else {
-		/* Pointer to UART in parameter ram.
-		*/
-		/* up = (smc_uart_t *)&cpmp->cp_dparam[ser->port]; */
-		up = &pquicc->pram[ser->port].scc.pothers.idma_smc.psmc.u;
-
-		/* Get the address of the host memory buffer.
-		 */
-		bdp = bdbase = (QUICC_BD *)((uint)pquicc + (uint)up->tbase);
-	}
-
-	/*
-	 * We need to gracefully shut down the transmitter, disable
-	 * interrupts, then send our bytes out.
-	 */
-
-	/*
-	 * Now, do each character.  This is not as bad as it looks
-	 * since this is a holding FIFO and not a transmitting FIFO.
-	 * We could add the complexity of filling the entire transmit
-	 * buffer, but we would just wait longer between accesses......
-	 */
-	for (i = 0; i < count; i++, s++) {
-		/* Wait for transmitter fifo to empty.
-		 * Ready indicates output is ready, and xmt is doing
-		 * that, not that it is ready for us to send.
-		 */
-		while (bdp->status & BD_SC_READY);
-
-		/* Send the character out.
-		 */
-		cp = bdp->buf;
-		*cp = *s;
-		
-		bdp->length = 1;
-		bdp->status |= BD_SC_READY;
-
-		if (bdp->status & BD_SC_WRAP)
-			bdp = bdbase;
-		else
-			bdp++;
-
-		/* if a LF, also do CR... */
-		if (*s == 10) {
-			while (bdp->status & BD_SC_READY);
-			/* cp = __va(bdp->buf); */
-			cp = bdp->buf;
-			*cp = 13;
-			bdp->length = 1;
-			bdp->status |= BD_SC_READY;
-
-			if (bdp->status & BD_SC_WRAP) {
-				bdp = bdbase;
-			}
-			else {
-				bdp++;
-			}
-		}
-	}
-
-	/*
-	 * Finally, Wait for transmitter & holding register to empty
-	 *  and restore the IER
-	 */
-	while (bdp->status & BD_SC_READY);
-
-	if (info)
-		info->tx_cur = (QUICC_BD *)bdp;
-}
-
-static void serial_console_write(struct console *c, const char *s,
-				unsigned count)
-{
-#ifdef CONFIG_KGDB
-	/* Try to let stub handle output. Returns true if it did. */ 
-	if (kgdb_output_string(s, count))
-		return;
-#endif
-	my_console_write(c->index, s, count);
-}
-
-
-
-/*void console_print_68360(const char *p)
-{
-	const char *cp = p;
-	int i;
-
-	for (i=0;cp[i]!=0;i++);
-
-	serial_console_write (p, i);
-
-	//Comment this if you want to have a strict interrupt-driven output
-	//rs_fair_output();
-
-	return;
-}*/
-
-
-
-
-
-
-#ifdef CONFIG_XMON
-int
-xmon_360_write(const char *s, unsigned count)
-{
-	my_console_write(0, s, count);
-	return(count);
-}
-#endif
-
-#ifdef CONFIG_KGDB
-void
-putDebugChar(char ch)
-{
-	my_console_write(0, &ch, 1);
-}
-#endif
-
-/*
- * Receive character from the serial port.  This only works well
- * before the port is initialized for real use.
- */
-static int my_console_wait_key(int idx, int xmon, char *obuf)
-{
-	struct serial_state		*ser;
-	u_char			c, *cp;
-	ser_info_t		*info;
-	QUICC_BD		*bdp;
-	volatile struct smc_uart_pram	*up;
-	int				i;
-
-	ser = rs_table + idx;
-
-	/* Get the address of the host memory buffer.
-	 * If the port has been initialized for general use, we must
-	 * use information from the port structure.
-	 */
-	if ((info = (ser_info_t *)ser->info))
-		bdp = info->rx_cur;
-	else
-		/* bdp = (QUICC_BD *)&cpmp->cp_dpmem[up->smc_rbase]; */
-		bdp = (QUICC_BD *)((uint)pquicc + (uint)up->tbase);
-
-	/* Pointer to UART in parameter ram.
-	 */
-	/* up = (smc_uart_t *)&cpmp->cp_dparam[ser->port]; */
-	up = &pquicc->pram[info->state->port].scc.pothers.idma_smc.psmc.u;
-
-	/*
-	 * We need to gracefully shut down the receiver, disable
-	 * interrupts, then read the input.
-	 * XMON just wants a poll.  If no character, return -1, else
-	 * return the character.
-	 */
-	if (!xmon) {
-		while (bdp->status & BD_SC_EMPTY);
-	}
-	else {
-		if (bdp->status & BD_SC_EMPTY)
-			return -1;
-	}
-
-	cp = (char *)bdp->buf;
-
-	if (obuf) {
-		i = c = bdp->length;
-		while (i-- > 0)
-			*obuf++ = *cp++;
-	}
-	else {
-		c = *cp;
-	}
-	bdp->status |= BD_SC_EMPTY;
-
-	if (info) {
-		if (bdp->status & BD_SC_WRAP) {
-			bdp = info->rx_bd_base;
-		}
-		else {
-			bdp++;
-		}
-		info->rx_cur = (QUICC_BD *)bdp;
-	}
-
-	return((int)c);
-}
-
-static int serial_console_wait_key(struct console *co)
-{
-	return(my_console_wait_key(co->index, 0, NULL));
-}
-
-#ifdef CONFIG_XMON
-int
-xmon_360_read_poll(void)
-{
-	return(my_console_wait_key(0, 1, NULL));
-}
-
-int
-xmon_360_read_char(void)
-{
-	return(my_console_wait_key(0, 0, NULL));
-}
-#endif
-
-#ifdef CONFIG_KGDB
-static char kgdb_buf[RX_BUF_SIZE], *kgdp;
-static int kgdb_chars;
-
-unsigned char
-getDebugChar(void)
-{
-	if (kgdb_chars <= 0) {
-		kgdb_chars = my_console_wait_key(0, 0, kgdb_buf);
-		kgdp = kgdb_buf;
-	}
-	kgdb_chars--;
-
-	return(*kgdp++);
-}
-
-void kgdb_interruptible(int state)
-{
-}
-void kgdb_map_scc(void)
-{
-	struct		serial_state *ser;
-	uint		mem_addr;
-	volatile	QUICC_BD		*bdp;
-	volatile	smc_uart_t	*up;
-
-	cpmp = (cpm360_t *)&(((immap_t *)IMAP_ADDR)->im_cpm);
-
-	/* To avoid data cache CPM DMA coherency problems, allocate a
-	 * buffer in the CPM DPRAM.  This will work until the CPM and
-	 * serial ports are initialized.  At that time a memory buffer
-	 * will be allocated.
-	 * The port is already initialized from the boot procedure, all
-	 * we do here is give it a different buffer and make it a FIFO.
-	 */
-
-	ser = rs_table;
-
-	/* Right now, assume we are using SMCs.
-	*/
-	up = (smc_uart_t *)&cpmp->cp_dparam[ser->port];
-
-	/* Allocate space for an input FIFO, plus a few bytes for output.
-	 * Allocate bytes to maintain word alignment.
-	 */
-	mem_addr = (uint)(&cpmp->cp_dpmem[0x1000]);
-
-	/* Set the physical address of the host memory buffers in
-	 * the buffer descriptors.
-	 */
-	bdp = (QUICC_BD *)&cpmp->cp_dpmem[up->smc_rbase];
-	bdp->buf = mem_addr;
-
-	bdp = (QUICC_BD *)&cpmp->cp_dpmem[up->smc_tbase];
-	bdp->buf = mem_addr+RX_BUF_SIZE;
-
-	up->smc_mrblr = RX_BUF_SIZE;		/* receive buffer length */
-	up->smc_maxidl = RX_BUF_SIZE;
-}
-#endif
-
-static struct tty_struct *serial_console_device(struct console *c, int *index)
-{
-	*index = c->index;
-	return serial_driver;
-}
-
-
-struct console sercons = {
- 	.name		= "ttyS",
- 	.write		= serial_console_write,
- 	.device		= serial_console_device,
- 	.wait_key	= serial_console_wait_key,
- 	.setup		= serial_console_setup,
- 	.flags		= CON_PRINTBUFFER,
- 	.index		= CONFIG_SERIAL_CONSOLE_PORT, 
-};
-
-
-
-/*
- *	Register console.
- */
-long console_360_init(long kmem_start, long kmem_end)
-{
-	register_console(&sercons);
-	/*register_console (console_print_68360); - 2.0.38 only required a write
-      function pointer. */
-	return kmem_start;
-}
-
-#endif
-
-/* Index in baud rate table of the default console baud rate.
-*/
-static	int	baud_idx;
-
-static const struct tty_operations rs_360_ops = {
-	.owner = THIS_MODULE,
-	.open = rs_360_open,
-	.close = rs_360_close,
-	.write = rs_360_write,
-	.put_char = rs_360_put_char,
-	.write_room = rs_360_write_room,
-	.chars_in_buffer = rs_360_chars_in_buffer,
-	.flush_buffer = rs_360_flush_buffer,
-	.ioctl = rs_360_ioctl,
-	.throttle = rs_360_throttle,
-	.unthrottle = rs_360_unthrottle,
-	/* .send_xchar = rs_360_send_xchar, */
-	.set_termios = rs_360_set_termios,
-	.stop = rs_360_stop,
-	.start = rs_360_start,
-	.hangup = rs_360_hangup,
-	/* .wait_until_sent = rs_360_wait_until_sent, */
-	/* .read_proc = rs_360_read_proc, */
-	.tiocmget = rs_360_tiocmget,
-	.tiocmset = rs_360_tiocmset,
-	.get_icount = rs_360_get_icount,
-};
-
-static int __init rs_360_init(void)
-{
-	struct serial_state * state;
-	ser_info_t	*info;
-	void       *mem_addr;
-	uint 		dp_addr, iobits;
-	int		    i, j, idx;
-	ushort		chan;
-	QUICC_BD	*bdp;
-	volatile	QUICC		*cp;
-	volatile	struct smc_regs	*sp;
-	volatile	struct smc_uart_pram	*up;
-	volatile	struct scc_regs	*scp;
-	volatile	struct uart_pram	*sup;
-	/* volatile	immap_t		*immap; */
-	
-	serial_driver = alloc_tty_driver(NR_PORTS);
-	if (!serial_driver)
-		return -1;
-
-	show_serial_version();
-
-	serial_driver->name = "ttyS";
-	serial_driver->major = TTY_MAJOR;
-	serial_driver->minor_start = 64;
-	serial_driver->type = TTY_DRIVER_TYPE_SERIAL;
-	serial_driver->subtype = SERIAL_TYPE_NORMAL;
-	serial_driver->init_termios = tty_std_termios;
-	serial_driver->init_termios.c_cflag =
-		baud_idx | CS8 | CREAD | HUPCL | CLOCAL;
-	serial_driver->flags = TTY_DRIVER_REAL_RAW;
-	tty_set_operations(serial_driver, &rs_360_ops);
-	
-	if (tty_register_driver(serial_driver))
-		panic("Couldn't register serial driver\n");
-
-	cp = pquicc;	/* Get pointer to Communication Processor */
-	/* immap = (immap_t *)IMAP_ADDR; */	/* and to internal registers */
-
-
-	/* Configure SCC2, SCC3, and SCC4 instead of port A parallel I/O.
-	 */
-	/* The "standard" configuration through the 860.
-	*/
-/* 	immap->im_ioport.iop_papar |= 0x00fc; */
-/* 	immap->im_ioport.iop_padir &= ~0x00fc; */
-/* 	immap->im_ioport.iop_paodr &= ~0x00fc; */
-	cp->pio_papar |= 0x00fc;
-	cp->pio_padir &= ~0x00fc;
-	/* cp->pio_paodr &= ~0x00fc; */
-
-
-	/* Since we don't yet do modem control, connect the port C pins
-	 * as general purpose I/O.  This will assert CTS and CD for the
-	 * SCC ports.
-	 */
-	/* FIXME: see 360um p.7-365 and 860um p.34-12 
-	 * I can't make sense of these bits - mleslie*/
-/* 	immap->im_ioport.iop_pcdir |= 0x03c6; */
-/* 	immap->im_ioport.iop_pcpar &= ~0x03c6; */
-
-/* 	cp->pio_pcdir |= 0x03c6; */
-/* 	cp->pio_pcpar &= ~0x03c6; */
-
-
-
-	/* Connect SCC2 and SCC3 to NMSI.  Connect BRG3 to SCC2 and
-	 * BRG4 to SCC3.
-	 */
-	cp->si_sicr &= ~0x00ffff00;
-	cp->si_sicr |=  0x001b1200;
-
-#ifdef CONFIG_PP04
-	/* Frequentis PP04 forced to RS-232 until we know better.
-	 * Port C 12 and 13 low enables RS-232 on SCC3 and SCC4.
-	 */
-	immap->im_ioport.iop_pcdir |= 0x000c;
-	immap->im_ioport.iop_pcpar &= ~0x000c;
-	immap->im_ioport.iop_pcdat &= ~0x000c;
-
-	/* This enables the TX driver.
-	*/
-	cp->cp_pbpar &= ~0x6000;
-	cp->cp_pbdat &= ~0x6000;
-#endif
-
-	for (i = 0, state = rs_table; i < NR_PORTS; i++,state++) {
-		state->magic = SSTATE_MAGIC;
-		state->line = i;
-		state->type = PORT_UNKNOWN;
-		state->custom_divisor = 0;
-		state->close_delay = 5*HZ/10;
-		state->closing_wait = 30*HZ;
-		state->icount.cts = state->icount.dsr = 
-			state->icount.rng = state->icount.dcd = 0;
-		state->icount.rx = state->icount.tx = 0;
-		state->icount.frame = state->icount.parity = 0;
-		state->icount.overrun = state->icount.brk = 0;
-		printk(KERN_INFO "ttyS%d at irq 0x%02x is an %s\n",
-		       i, (unsigned int)(state->irq),
-		       (state->smc_scc_num & NUM_IS_SCC) ? "SCC" : "SMC");
-
-#ifdef CONFIG_SERIAL_CONSOLE
-		/* If we just printed the message on the console port, and
-		 * we are about to initialize it for general use, we have
-		 * to wait a couple of character times for the CR/NL to
-		 * make it out of the transmit buffer.
-		 */
-		if (i == CONFIG_SERIAL_CONSOLE_PORT)
-			mdelay(8);
-
-
-/* 		idx = PORT_NUM(info->state->smc_scc_num); */
-/* 		if (info->state->smc_scc_num & NUM_IS_SCC) */
-/* 			chan = scc_chan_map[idx]; */
-/* 		else */
-/* 			chan = smc_chan_map[idx]; */
-
-/* 		cp->cp_cr = mk_cr_cmd(chan, CPM_CR_STOP_TX) | CPM_CR_FLG; */
-/* 		while (cp->cp_cr & CPM_CR_FLG); */
-
-#endif
-		/* info = kmalloc(sizeof(ser_info_t), GFP_KERNEL); */
-		info = &quicc_ser_info[i];
-		if (info) {
-			memset (info, 0, sizeof(ser_info_t));
-			info->magic = SERIAL_MAGIC;
-			info->line = i;
-			info->flags = state->flags;
-			INIT_WORK(&info->tqueue, do_softint, info);
-			INIT_WORK(&info->tqueue_hangup, do_serial_hangup, info);
-			init_waitqueue_head(&info->open_wait);
-			init_waitqueue_head(&info->close_wait);
-			info->state = state;
-			state->info = (struct async_struct *)info;
-
-			/* We need to allocate a transmit and receive buffer
-			 * descriptors from dual port ram, and a character
-			 * buffer area from host mem.
-			 */
-			dp_addr = m360_cpm_dpalloc(sizeof(QUICC_BD) * RX_NUM_FIFO);
-
-			/* Allocate space for FIFOs in the host memory.
-			 *  (for now this is from a static array of buffers :(
-			 */
-			/* mem_addr = m360_cpm_hostalloc(RX_NUM_FIFO * RX_BUF_SIZE); */
-			/* mem_addr = kmalloc (RX_NUM_FIFO * RX_BUF_SIZE, GFP_BUFFER); */
-			mem_addr = &rx_buf_pool[i * RX_NUM_FIFO * RX_BUF_SIZE];
-
-			/* Set the physical address of the host memory
-			 * buffers in the buffer descriptors, and the
-			 * virtual address for us to work with.
-			 */
-			bdp = (QUICC_BD *)((uint)pquicc + dp_addr);
-			info->rx_cur = info->rx_bd_base = bdp;
-
-			/* initialize rx buffer descriptors */
-			for (j=0; j<(RX_NUM_FIFO-1); j++) {
-				bdp->buf = &rx_buf_pool[(i * RX_NUM_FIFO + j ) * RX_BUF_SIZE];
-				bdp->status = BD_SC_EMPTY | BD_SC_INTRPT;
-				mem_addr += RX_BUF_SIZE;
-				bdp++;
-			}
-			bdp->buf = &rx_buf_pool[(i * RX_NUM_FIFO + j ) * RX_BUF_SIZE];
-			bdp->status = BD_SC_WRAP | BD_SC_EMPTY | BD_SC_INTRPT;
-
-
-			idx = PORT_NUM(info->state->smc_scc_num);
-			if (info->state->smc_scc_num & NUM_IS_SCC) {
-
-#if defined (CONFIG_UCQUICC) && 1
-				/* set the transceiver mode to RS232 */
-				sipex_mode_bits &= ~(uint)SIPEX_MODE(idx,0x0f); /* clear current mode */
-				sipex_mode_bits |= (uint)SIPEX_MODE(idx,0x02);
-				*(uint *)_periph_base = sipex_mode_bits;
-				/* printk ("sipex bits = 0x%08x\n", sipex_mode_bits); */
-#endif
-			}
-
-			dp_addr = m360_cpm_dpalloc(sizeof(QUICC_BD) * TX_NUM_FIFO);
-
-			/* Allocate space for FIFOs in the host memory.
-			*/
-			/* mem_addr = m360_cpm_hostalloc(TX_NUM_FIFO * TX_BUF_SIZE); */
-			/* mem_addr = kmalloc (TX_NUM_FIFO * TX_BUF_SIZE, GFP_BUFFER); */
-			mem_addr = &tx_buf_pool[i * TX_NUM_FIFO * TX_BUF_SIZE];
-
-			/* Set the physical address of the host memory
-			 * buffers in the buffer descriptors, and the
-			 * virtual address for us to work with.
-			 */
-			/* bdp = (QUICC_BD *)&cp->cp_dpmem[dp_addr]; */
-			bdp = (QUICC_BD *)((uint)pquicc + dp_addr);
-			info->tx_cur = info->tx_bd_base = (QUICC_BD *)bdp;
-
-			/* initialize tx buffer descriptors */
-			for (j=0; j<(TX_NUM_FIFO-1); j++) {
-				bdp->buf = &tx_buf_pool[(i * TX_NUM_FIFO + j ) * TX_BUF_SIZE];
-				bdp->status = BD_SC_INTRPT;
-				mem_addr += TX_BUF_SIZE;
-				bdp++;
-			}
-			bdp->buf = &tx_buf_pool[(i * TX_NUM_FIFO + j ) * TX_BUF_SIZE];
-			bdp->status = (BD_SC_WRAP | BD_SC_INTRPT);
-
-			if (info->state->smc_scc_num & NUM_IS_SCC) {
-				scp = &pquicc->scc_regs[idx];
-				sup = &pquicc->pram[info->state->port].scc.pscc.u;
-				sup->rbase = dp_addr;
-				sup->tbase = dp_addr;
-
-				/* Set up the uart parameters in the
-				 * parameter ram.
-				 */
-				sup->rfcr = SMC_EB;
-				sup->tfcr = SMC_EB;
-
-				/* Set this to 1 for now, so we get single
-				 * character interrupts.  Using idle character
-				 * time requires some additional tuning.
-				 */
-				sup->mrblr = 1;
-				sup->max_idl = 0;
-				sup->brkcr = 1;
-				sup->parec = 0;
-				sup->frmer = 0;
-				sup->nosec = 0;
-				sup->brkec = 0;
-				sup->uaddr1 = 0;
-				sup->uaddr2 = 0;
-				sup->toseq = 0;
-				{
-					int i;
-					for (i=0;i<8;i++)
-						sup->cc[i] = 0x8000;
-				}
-				sup->rccm = 0xc0ff;
-
-				/* Send the CPM an initialize command.
-				*/
-				chan = scc_chan_map[idx];
-
-				/* execute the INIT RX & TX PARAMS command for this channel. */
-				cp->cp_cr = mk_cr_cmd(chan, CPM_CR_INIT_TRX) | CPM_CR_FLG;
-				while (cp->cp_cr & CPM_CR_FLG);
-
-				/* Set UART mode, 8 bit, no parity, one stop.
-				 * Enable receive and transmit.
-				 */
-				scp->scc_gsmr.w.high = 0;
-				scp->scc_gsmr.w.low = 
-					(SCC_GSMRL_MODE_UART | SCC_GSMRL_TDCR_16 | SCC_GSMRL_RDCR_16);
-
-				/* Disable all interrupts and clear all pending
-				 * events.
-				 */
-				scp->scc_sccm = 0;
-				scp->scc_scce = 0xffff;
-				scp->scc_dsr = 0x7e7e;
-				scp->scc_psmr = 0x3000;
-
-				/* If the port is the console, enable Rx and Tx.
-				*/
-#ifdef CONFIG_SERIAL_CONSOLE
-				if (i == CONFIG_SERIAL_CONSOLE_PORT)
-					scp->scc_gsmr.w.low |= (SCC_GSMRL_ENR | SCC_GSMRL_ENT);
-#endif
-			}
-			else {
-				/* Configure SMCs Tx/Rx instead of port B
-				 * parallel I/O.
-				 */
-				up = &pquicc->pram[info->state->port].scc.pothers.idma_smc.psmc.u;
-				up->rbase = dp_addr;
-
-				iobits = 0xc0 << (idx * 4);
-				cp->pip_pbpar |= iobits;
-				cp->pip_pbdir &= ~iobits;
-				cp->pip_pbodr &= ~iobits;
-
-
-				/* Connect the baud rate generator to the
-				 * SMC based upon index in rs_table.  Also
-				 * make sure it is connected to NMSI.
-				 */
-				cp->si_simode &= ~(0xffff << (idx * 16));
-				cp->si_simode |= (i << ((idx * 16) + 12));
-
-				up->tbase = dp_addr;
-
-				/* Set up the uart parameters in the
-				 * parameter ram.
-				 */
-				up->rfcr = SMC_EB;
-				up->tfcr = SMC_EB;
-
-				/* Set this to 1 for now, so we get single
-				 * character interrupts.  Using idle character
-				 * time requires some additional tuning.
-				 */
-				up->mrblr = 1;
-				up->max_idl = 0;
-				up->brkcr = 1;
-
-				/* Send the CPM an initialize command.
-				*/
-				chan = smc_chan_map[idx];
-
-				cp->cp_cr = mk_cr_cmd(chan,
-									  CPM_CR_INIT_TRX) | CPM_CR_FLG;
-#ifdef CONFIG_SERIAL_CONSOLE
-				if (i == CONFIG_SERIAL_CONSOLE_PORT)
-					printk("");
-#endif
-				while (cp->cp_cr & CPM_CR_FLG);
-
-				/* Set UART mode, 8 bit, no parity, one stop.
-				 * Enable receive and transmit.
-				 */
-				sp = &cp->smc_regs[idx];
-				sp->smc_smcmr = smcr_mk_clen(9) | SMCMR_SM_UART;
-
-				/* Disable all interrupts and clear all pending
-				 * events.
-				 */
-				sp->smc_smcm = 0;
-				sp->smc_smce = 0xff;
-
-				/* If the port is the console, enable Rx and Tx.
-				*/
-#ifdef CONFIG_SERIAL_CONSOLE
-				if (i == CONFIG_SERIAL_CONSOLE_PORT)
-					sp->smc_smcmr |= SMCMR_REN | SMCMR_TEN;
-#endif
-			}
-
-			/* Install interrupt handler.
-			*/
-			/* cpm_install_handler(IRQ_MACHSPEC | state->irq, rs_360_interrupt, info);  */
-			/*request_irq(IRQ_MACHSPEC | state->irq, rs_360_interrupt, */
-			request_irq(state->irq, rs_360_interrupt, 0, "ttyS",
-				    (void *)info);
-
-			/* Set up the baud rate generator.
-			*/
-			m360_cpm_setbrg(i, baud_table[baud_idx]);
-
-		}
-	}
-
-	return 0;
-}
-module_init(rs_360_init);
-
-/* This must always be called before the rs_360_init() function, otherwise
- * it blows away the port control information.
- */
-//static int __init serial_console_setup( struct console *co, char *options)
-int serial_console_setup( struct console *co, char *options)
-{
-	struct		serial_state	*ser;
-	uint		mem_addr, dp_addr, bidx, idx, iobits;
-	ushort		chan;
-	QUICC_BD	*bdp;
-	volatile	QUICC			*cp;
-	volatile	struct smc_regs	*sp;
-	volatile	struct scc_regs	*scp;
-	volatile	struct smc_uart_pram	*up;
-	volatile	struct uart_pram		*sup;
-
-/* mleslie TODO:
- * add something to the 68k bootloader to store a desired initial console baud rate */
-
-/* 	bd_t						*bd; */ /* a board info struct used by EPPC-bug */
-/* 	bd = (bd_t *)__res; */
-
- 	for (bidx = 0; bidx < (sizeof(baud_table) / sizeof(int)); bidx++)
-	 /* if (bd->bi_baudrate == baud_table[bidx]) */
- 		if (CONSOLE_BAUDRATE == baud_table[bidx])
-			break;
-
-	/* co->cflag = CREAD|CLOCAL|bidx|CS8; */
-	baud_idx = bidx;
-
-	ser = rs_table + CONFIG_SERIAL_CONSOLE_PORT;
-
-	cp = pquicc;	/* Get pointer to Communication Processor */
-
-	idx = PORT_NUM(ser->smc_scc_num);
-	if (ser->smc_scc_num & NUM_IS_SCC) {
-
-		/* TODO: need to set up SCC pin assignment etc. here */
-		
-	}
-	else {
-		iobits = 0xc0 << (idx * 4);
-		cp->pip_pbpar |= iobits;
-		cp->pip_pbdir &= ~iobits;
-		cp->pip_pbodr &= ~iobits;
-
-		/* Connect the baud rate generator to the
-		 * SMC based upon index in rs_table.  Also
-		 * make sure it is connected to NMSI.
-		 */
-		cp->si_simode &= ~(0xffff << (idx * 16));
-		cp->si_simode |= (idx << ((idx * 16) + 12));
-	}
-
-	/* When we get here, the CPM has been reset, so we need
-	 * to configure the port.
-	 * We need to allocate a transmit and receive buffer descriptor
-	 * from dual port ram, and a character buffer area from host mem.
-	 */
-
-	/* Allocate space for two buffer descriptors in the DP ram.
-	*/
-	dp_addr = m360_cpm_dpalloc(sizeof(QUICC_BD) * CONSOLE_NUM_FIFO);
-
-	/* Allocate space for two 2 byte FIFOs in the host memory.
-	 */
-	/* mem_addr = m360_cpm_hostalloc(8); */
-	mem_addr = (uint)console_fifos;
-
-
-	/* Set the physical address of the host memory buffers in
-	 * the buffer descriptors.
-	 */
-	/* bdp = (QUICC_BD *)&cp->cp_dpmem[dp_addr]; */
-	bdp = (QUICC_BD *)((uint)pquicc + dp_addr);
-	bdp->buf = (char *)mem_addr;
-	(bdp+1)->buf = (char *)(mem_addr+4);
-
-	/* For the receive, set empty and wrap.
-	 * For transmit, set wrap.
-	 */
-	bdp->status = BD_SC_EMPTY | BD_SC_WRAP;
-	(bdp+1)->status = BD_SC_WRAP;
-
-	/* Set up the uart parameters in the parameter ram.
-	 */
-	if (ser->smc_scc_num & NUM_IS_SCC) {
-		scp = &cp->scc_regs[idx];
-		/* sup = (scc_uart_t *)&cp->cp_dparam[ser->port]; */
-		sup = &pquicc->pram[ser->port].scc.pscc.u;
-
-		sup->rbase = dp_addr;
-		sup->tbase = dp_addr + sizeof(QUICC_BD);
-
-		/* Set up the uart parameters in the
-		 * parameter ram.
-		 */
-		sup->rfcr = SMC_EB;
-		sup->tfcr = SMC_EB;
-
-		/* Set this to 1 for now, so we get single
-		 * character interrupts.  Using idle character
-		 * time requires some additional tuning.
-		 */
-		sup->mrblr = 1;
-		sup->max_idl = 0;
-		sup->brkcr = 1;
-		sup->parec = 0;
-		sup->frmer = 0;
-		sup->nosec = 0;
-		sup->brkec = 0;
-		sup->uaddr1 = 0;
-		sup->uaddr2 = 0;
-		sup->toseq = 0;
-		{
-			int i;
-			for (i=0;i<8;i++)
-				sup->cc[i] = 0x8000;
-		}
-		sup->rccm = 0xc0ff;
-
-		/* Send the CPM an initialize command.
-		*/
-		chan = scc_chan_map[idx];
-
-		cp->cp_cr = mk_cr_cmd(chan, CPM_CR_INIT_TRX) | CPM_CR_FLG;
-		while (cp->cp_cr & CPM_CR_FLG);
-
-		/* Set UART mode, 8 bit, no parity, one stop.
-		 * Enable receive and transmit.
-		 */
-		scp->scc_gsmr.w.high = 0;
-		scp->scc_gsmr.w.low = 
-			(SCC_GSMRL_MODE_UART | SCC_GSMRL_TDCR_16 | SCC_GSMRL_RDCR_16);
-
-		/* Disable all interrupts and clear all pending
-		 * events.
-		 */
-		scp->scc_sccm = 0;
-		scp->scc_scce = 0xffff;
-		scp->scc_dsr = 0x7e7e;
-		scp->scc_psmr = 0x3000;
-
-		scp->scc_gsmr.w.low |= (SCC_GSMRL_ENR | SCC_GSMRL_ENT);
-
-	}
-	else {
-		/* up = (smc_uart_t *)&cp->cp_dparam[ser->port]; */
-		up = &pquicc->pram[ser->port].scc.pothers.idma_smc.psmc.u;
-
-		up->rbase = dp_addr;	/* Base of receive buffer desc. */
-		up->tbase = dp_addr+sizeof(QUICC_BD);	/* Base of xmt buffer desc. */
-		up->rfcr = SMC_EB;
-		up->tfcr = SMC_EB;
-
-		/* Set this to 1 for now, so we get single character interrupts.
-		*/
-		up->mrblr = 1;		/* receive buffer length */
-		up->max_idl = 0;		/* wait forever for next char */
-
-		/* Send the CPM an initialize command.
-		*/
-		chan = smc_chan_map[idx];
-		cp->cp_cr = mk_cr_cmd(chan, CPM_CR_INIT_TRX) | CPM_CR_FLG;
-		while (cp->cp_cr & CPM_CR_FLG);
-
-		/* Set UART mode, 8 bit, no parity, one stop.
-		 * Enable receive and transmit.
-		 */
-		sp = &cp->smc_regs[idx];
-		sp->smc_smcmr = smcr_mk_clen(9) |  SMCMR_SM_UART;
-
-		/* And finally, enable Rx and Tx.
-		*/
-		sp->smc_smcmr |= SMCMR_REN | SMCMR_TEN;
-	}
-
-	/* Set up the baud rate generator.
-	*/
-	/* m360_cpm_setbrg((ser - rs_table), bd->bi_baudrate); */
-	m360_cpm_setbrg((ser - rs_table), CONSOLE_BAUDRATE);
-
-	return 0;
-}
-
-/*
- * Local variables:
- *  c-indent-level: 4
- *  c-basic-offset: 4
- *  tab-width: 4
- * End:
- */
diff --git a/drivers/staging/serial/Kconfig b/drivers/staging/serial/Kconfig
deleted file mode 100644
index 9489688397e..00000000000
--- a/drivers/staging/serial/Kconfig
+++ /dev/null
@@ -1,16 +0,0 @@
-config SERIAL_68360_SMC
-	bool "68360 SMC uart support"
-	depends on M68360
-	help
-	  This driver supports the SMC serial ports of the Motorola 68360 CPU.
-
-config SERIAL_68360_SCC
-	bool "68360 SCC uart support"
-	depends on M68360
-	help
-	  This driver supports the SCC serial ports of the Motorola 68360 CPU.
-
-config SERIAL_68360
-	bool
-	depends on SERIAL_68360_SMC || SERIAL_68360_SCC
-	default y
diff --git a/drivers/staging/serial/Makefile b/drivers/staging/serial/Makefile
deleted file mode 100644
index 37a6a0b35fb..00000000000
--- a/drivers/staging/serial/Makefile
+++ /dev/null
@@ -1 +0,0 @@
-obj-$(CONFIG_SERIAL_68360) += 68360serial.o
diff --git a/drivers/staging/serial/TODO b/drivers/staging/serial/TODO
deleted file mode 100644
index a19cda81dab..00000000000
--- a/drivers/staging/serial/TODO
+++ /dev/null
@@ -1,6 +0,0 @@
-These are a few serial drivers that either do not build, or do not work if they
-do build, or if they seem to work, are for obsolete hardware, or are full of
-unfixable races and no one uses them anymore.
-
-If no one steps up to adopt any of these drivers, they will be removed
-in the 3.4 release.
diff --git a/drivers/staging/serqt_usb2/serqt_usb2.c b/drivers/staging/serqt_usb2/serqt_usb2.c
index ae1d815e2a5..43045db982d 100644
--- a/drivers/staging/serqt_usb2/serqt_usb2.c
+++ b/drivers/staging/serqt_usb2/serqt_usb2.c
@@ -41,13 +41,6 @@ static bool debug;
 #define QUATECH_HSU200B	0xC0B1	/* HSU200B */
 #define QUATECH_HSU200C	0xC0B2	/* HSU200C */
 #define QUATECH_HSU200D	0xC0B3	/* HSU200D */
-#define QUATECH_SSU100_2  0xC120	/* SSU100_2 */
-#define QUATECH_DSU100_2  0xC140	/* DSU100_2 */
-#define QUATECH_DSU400_2  0xC150	/* DSU400_2 */
-#define QUATECH_QSU100_2  0xC160	/* QSU100_2 */
-#define QUATECH_QSU400_2  0xC170	/* QSU400_2 */
-#define QUATECH_ESU400_2  0xC180	/* ESU400_2 */
-#define QUATECH_ESU100_2  0xC1A0	/* ESU100_2 */
 
 #define QT_SET_GET_DEVICE           0xc2
 #define QT_OPEN_CLOSE_CHANNEL       0xca
@@ -125,7 +118,7 @@ static bool debug;
 #define MODEM_CTRL          0x40
 #define RS232_MODE          0x00
 
-static const struct usb_device_id serqt_id_table[] = {
+static const struct usb_device_id id_table[] = {
 	{USB_DEVICE(USB_VENDOR_ID_QUATECH, QUATECH_SSU200)},
 	{USB_DEVICE(USB_VENDOR_ID_QUATECH, QUATECH_DSU100)},
 	{USB_DEVICE(USB_VENDOR_ID_QUATECH, QUATECH_DSU200)},
@@ -143,17 +136,9 @@ static const struct usb_device_id serqt_id_table[] = {
 	{USB_DEVICE(USB_VENDOR_ID_QUATECH, QUATECH_HSU200B)},
 	{USB_DEVICE(USB_VENDOR_ID_QUATECH, QUATECH_HSU200C)},
 	{USB_DEVICE(USB_VENDOR_ID_QUATECH, QUATECH_HSU200D)},
-	{USB_DEVICE(USB_VENDOR_ID_QUATECH, QUATECH_SSU100_2)},
-	{USB_DEVICE(USB_VENDOR_ID_QUATECH, QUATECH_DSU100_2)},
-	{USB_DEVICE(USB_VENDOR_ID_QUATECH, QUATECH_DSU400_2)},
-	{USB_DEVICE(USB_VENDOR_ID_QUATECH, QUATECH_QSU100_2)},
-	{USB_DEVICE(USB_VENDOR_ID_QUATECH, QUATECH_QSU400_2)},
-	{USB_DEVICE(USB_VENDOR_ID_QUATECH, QUATECH_ESU400_2)},
-	{USB_DEVICE(USB_VENDOR_ID_QUATECH, QUATECH_ESU100_2)},
 	{}			/* Terminating entry */
 };
-
-MODULE_DEVICE_TABLE(usb, serqt_id_table);
+MODULE_DEVICE_TABLE(usb, id_table);
 
 struct qt_get_device_data {
 	__u8 porta;
@@ -195,13 +180,6 @@ struct quatech_port {
 	char closePending;
 };
 
-static struct usb_driver serqt_usb_driver = {
-	.name = "quatech-usb-serial",
-	.probe = usb_serial_probe,
-	.disconnect = usb_serial_disconnect,
-	.id_table = serqt_id_table,
-};
-
 static int port_paranoia_check(struct usb_serial_port *port,
 			       const char *function)
 {
@@ -304,8 +282,6 @@ static void qt_write_bulk_callback(struct urb *urb)
 
 	quatech_port = urb->context;
 
-	dbg("%s - port %d\n", __func__, quatech_port->port_num);
-
 	tty = tty_port_tty_get(&quatech_port->port->port);
 
 	if (tty)
@@ -351,7 +327,6 @@ static void qt_read_bulk_callback(struct urb *urb)
 	/* index = MINOR(port->tty->device) - serial->minor; */
 	index = tty->index - serial->minor;
 
-	dbg("%s - port %d\n", __func__, port->number);
 	dbg("%s - port->RxHolding = %d\n", __func__, qt_port->RxHolding);
 
 	if (port_paranoia_check(port, __func__) != 0) {
@@ -726,8 +701,6 @@ static int qt_startup(struct usb_serial *serial)
 	int i;
 	int status;
 
-	dbg("enterting %s", __func__);
-
 	/* Now setup per port private data */
 	for (i = 0; i < serial->num_ports; i++) {
 		port = serial->port[i];
@@ -855,8 +828,6 @@ static void qt_release(struct usb_serial *serial)
 	struct quatech_port *qt_port;
 	int i;
 
-	dbg("enterting %s", __func__);
-
 	for (i = 0; i < serial->num_ports; i++) {
 		port = serial->port[i];
 		if (!port)
@@ -882,8 +853,6 @@ static int qt_open(struct tty_struct *tty,
 	if (port_paranoia_check(port, __func__))
 		return -ENODEV;
 
-	dbg("%s - port %d\n", __func__, port->number);
-
 	serial = port->serial;
 
 	if (serial_paranoia_check(serial, __func__))
@@ -1006,8 +975,6 @@ static int qt_chars_in_buffer(struct tty_struct *tty)
 
 	serial = get_usb_serial(port, __func__);
 
-	dbg("%s - port %d\n", __func__, port->number);
-
 	if (serial->num_bulk_out) {
 		if (port->write_urb->status == -EINPROGRESS)
 			chars = port->write_urb->transfer_buffer_length;
@@ -1054,8 +1021,6 @@ static void qt_close(struct usb_serial_port *port)
 	unsigned int index;
 	status = 0;
 
-	dbg("%s - port %d\n", __func__, port->number);
-
 	tty = tty_port_tty_get(&port->port);
 	index = tty->index - serial->minor;
 
@@ -1109,8 +1074,6 @@ static int qt_write(struct tty_struct *tty, struct usb_serial_port *port,
 	if (serial == NULL)
 		return -ENODEV;
 
-	dbg("%s - port %d\n", __func__, port->number);
-
 	if (count == 0) {
 		dbg("%s - write request of 0 bytes\n", __func__);
 		return 0;
@@ -1173,8 +1136,6 @@ static int qt_write_room(struct tty_struct *tty)
 
 	mutex_lock(&qt_port->lock);
 
-	dbg("%s - port %d\n", __func__, port->number);
-
 	if (serial->num_bulk_out) {
 		if (port->write_urb->status != -EINPROGRESS)
 			retval = port->bulk_out_size;
@@ -1241,8 +1202,6 @@ static void qt_set_termios(struct tty_struct *tty,
 	int baud, divisor, remainder;
 	int status;
 
-	dbg("%s", __func__);
-
 	index = tty->index - port->serial->minor;
 
 	switch (cflag) {
@@ -1365,8 +1324,6 @@ static void qt_break(struct tty_struct *tty, int break_state)
 
 	mutex_lock(&qt_port->lock);
 
-	dbg("%s - port %d\n", __func__, port->number);
-
 	result =
 	    usb_control_msg(serial->dev, usb_sndctrlpipe(serial->dev, 0),
 			    QT_BREAK_CONTROL, 0x40, onoff, index, NULL, 0, 300);
@@ -1385,8 +1342,6 @@ static inline int qt_real_tiocmget(struct tty_struct *tty,
 	int status;
 	unsigned int index;
 
-	dbg("%s - port %d, tty =0x%p\n", __func__, port->number, tty);
-
 	index = tty->index - serial->minor;
 	status =
 	    BoxGetRegister(port->serial, index, MODEM_CONTROL_REGISTER, &mcr);
@@ -1426,8 +1381,6 @@ static inline int qt_real_tiocmset(struct tty_struct *tty,
 	int status;
 	unsigned int index;
 
-	dbg("%s - port %d\n", __func__, port->number);
-
 	index = tty->index - serial->minor;
 	status =
 	    BoxGetRegister(port->serial, index, MODEM_CONTROL_REGISTER, &mcr);
@@ -1461,18 +1414,11 @@ static int qt_tiocmget(struct tty_struct *tty)
 	struct quatech_port *qt_port = qt_get_port_private(port);
 	int retval = -ENODEV;
 
-	dbg("In %s\n", __func__);
-
 	if (!serial)
 		return -ENODEV;
 
 	mutex_lock(&qt_port->lock);
-
-	dbg("%s - port %d\n", __func__, port->number);
-	dbg("%s - port->RxHolding = %d\n", __func__, qt_port->RxHolding);
-
 	retval = qt_real_tiocmget(tty, port, serial);
-
 	mutex_unlock(&qt_port->lock);
 	return retval;
 }
@@ -1486,18 +1432,11 @@ static int qt_tiocmset(struct tty_struct *tty,
 	struct quatech_port *qt_port = qt_get_port_private(port);
 	int retval = -ENODEV;
 
-	dbg("In %s\n", __func__);
-
 	if (!serial)
 		return -ENODEV;
 
 	mutex_lock(&qt_port->lock);
-
-	dbg("%s - port %d\n", __func__, port->number);
-	dbg("%s - qt_port->RxHolding = %d\n", __func__, qt_port->RxHolding);
-
 	retval = qt_real_tiocmset(tty, port, serial, set);
-
 	mutex_unlock(&qt_port->lock);
 	return retval;
 }
@@ -1508,8 +1447,6 @@ static void qt_throttle(struct tty_struct *tty)
 	struct usb_serial *serial = get_usb_serial(port, __func__);
 	struct quatech_port *qt_port;
 
-	dbg("%s - port %d\n", __func__, port->number);
-
 	if (!serial)
 		return;
 
@@ -1519,7 +1456,6 @@ static void qt_throttle(struct tty_struct *tty)
 
 	/* pass on to the driver specific version of this function */
 	qt_port->RxHolding = 1;
-	dbg("%s - port->RxHolding = 1\n", __func__);
 
 	mutex_unlock(&qt_port->lock);
 	return;
@@ -1539,8 +1475,6 @@ static void qt_unthrottle(struct tty_struct *tty)
 
 	mutex_lock(&qt_port->lock);
 
-	dbg("%s - port %d\n", __func__, port->number);
-
 	if (qt_port->RxHolding == 1) {
 		dbg("%s -qt_port->RxHolding == 1\n", __func__);
 
@@ -1559,8 +1493,9 @@ static void qt_unthrottle(struct tty_struct *tty)
 					  qt_read_bulk_callback, port);
 			result = usb_submit_urb(port->read_urb, GFP_ATOMIC);
 			if (result)
-				err("%s - failed restarting read urb, error %d",
-				    __func__, result);
+				dev_err(&port->dev,
+					"%s - failed restarting read urb, error %d\n",
+					__func__, result);
 		}
 	}
 	mutex_unlock(&qt_port->lock);
@@ -1589,7 +1524,7 @@ static struct usb_serial_driver quatech_device = {
 		   .name = "serqt",
 		   },
 	.description = DRIVER_DESC,
-	.id_table = serqt_id_table,
+	.id_table = id_table,
 	.num_ports = 8,
 	.open = qt_open,
 	.close = qt_close,
@@ -1612,7 +1547,7 @@ static struct usb_serial_driver * const serial_drivers[] = {
 	&quatech_device, NULL
 };
 
-module_usb_serial_driver(serqt_usb_driver, serial_drivers);
+module_usb_serial_driver(serial_drivers, id_table);
 
 MODULE_AUTHOR(DRIVER_AUTHOR);
 MODULE_DESCRIPTION(DRIVER_DESC);
diff --git a/drivers/staging/sm7xx/smtcfb.c b/drivers/staging/sm7xx/smtcfb.c
index 83c582ed12e..746c4cd5d30 100644
--- a/drivers/staging/sm7xx/smtcfb.c
+++ b/drivers/staging/sm7xx/smtcfb.c
@@ -2,31 +2,19 @@
  * Silicon Motion SM7XX frame buffer device
  *
  * Copyright (C) 2006 Silicon Motion Technology Corp.
- * Authors: Ge Wang, gewang@siliconmotion.com
- *	    Boyod boyod.yang@siliconmotion.com.cn
+ * Authors:  Ge Wang, gewang@siliconmotion.com
+ *	     Boyod boyod.yang@siliconmotion.com.cn
  *
  * Copyright (C) 2009 Lemote, Inc.
- * Author: Wu Zhangjin, wuzhangjin@gmail.com
+ * Author:   Wu Zhangjin, wuzhangjin@gmail.com
  *
  * Copyright (C) 2011 Igalia, S.L.
- * Author: Javier M. Mellid <jmunhoz@igalia.com>
+ * Author:   Javier M. Mellid <jmunhoz@igalia.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.
+ * 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.
  *
- * Version 0.10.26192.21.01
- *	- Add PowerPC/Big endian support
- *	- Verified on 2.6.19.2
- *	Boyod.yang <boyod.yang@siliconmotion.com.cn>
- *
- * Version 0.09.2621.00.01
- *	- Only support Linux Kernel's version 2.6.21
- *	Boyod.yang <boyod.yang@siliconmotion.com.cn>
- *
- * Version 0.09
- *	- Only support Linux Kernel's version 2.6.12
- *	Boyod.yang <boyod.yang@siliconmotion.com.cn>
  */
 
 #include <linux/io.h>
@@ -45,40 +33,19 @@
 
 #include "smtcfb.h"
 
-#ifdef DEBUG
-#define smdbg(format, arg...) printk(KERN_DEBUG format , ## arg)
-#else
-#define smdbg(format, arg...)
-#endif
-
 struct screen_info smtc_screen_info;
 
 /*
 * Private structure
 */
 struct smtcfb_info {
-	/*
-	 * The following is a pointer to be passed into the
-	 * functions below.  The modules outside the main
-	 * voyager.c driver have no knowledge as to what
-	 * is within this structure.
-	 */
 	struct fb_info fb;
-	struct display_switch *dispsw;
-	struct pci_dev *dev;
-	signed int currcon;
-
+	struct pci_dev *pdev;
 	struct {
 		u8 red, green, blue;
 	} palette[NR_RGB];
-
 	u_int palette_size;
-};
 
-struct par_info {
-	/*
-	 * Hardware
-	 */
 	u16 chipID;
 	unsigned char __iomem *m_pMMIO;
 	char __iomem *m_pLFB;
@@ -121,15 +88,6 @@ char __iomem *smtc_RegBaseAddress;	/* Memory Map IO starting address */
 char __iomem *smtc_VRAMBaseAddress;	/* video memory starting address */
 
 static u32 colreg[17];
-static struct par_info hw;	/* hardware information */
-
-u16 smtc_ChipIDs[] = {
-	0x710,
-	0x712,
-	0x720
-};
-
-#define numSMTCchipIDs ARRAY_SIZE(smtc_ChipIDs)
 
 static struct fb_var_screeninfo smtcfb_var = {
 	.xres           = 1024,
@@ -154,30 +112,29 @@ static struct fb_fix_screeninfo smtcfb_fix = {
 	.accel          = FB_ACCEL_SMI_LYNX,
 };
 
-static void sm712_set_timing(struct smtcfb_info *sfb,
-			     struct par_info *ppar_info)
+static void sm712_set_timing(struct smtcfb_info *sfb)
 {
 	int i = 0, j = 0;
 	u32 m_nScreenStride;
 
-	smdbg("\nppar_info->width = %d ppar_info->height = %d"
-			"sfb->fb.var.bits_per_pixel = %d ppar_info->hz = %d\n",
-			ppar_info->width, ppar_info->height,
-			sfb->fb.var.bits_per_pixel, ppar_info->hz);
+	dev_dbg(&sfb->pdev->dev,
+		"sfb->width=%d sfb->height=%d "
+		"sfb->fb.var.bits_per_pixel=%d sfb->hz=%d\n",
+		sfb->width, sfb->height, sfb->fb.var.bits_per_pixel, sfb->hz);
 
 	for (j = 0; j < numVGAModes; j++) {
-		if (VGAMode[j].mmSizeX == ppar_info->width &&
-		    VGAMode[j].mmSizeY == ppar_info->height &&
+		if (VGAMode[j].mmSizeX == sfb->width &&
+		    VGAMode[j].mmSizeY == sfb->height &&
 		    VGAMode[j].bpp == sfb->fb.var.bits_per_pixel &&
-		    VGAMode[j].hz == ppar_info->hz) {
+		    VGAMode[j].hz == sfb->hz) {
 
-			smdbg("\nVGAMode[j].mmSizeX  = %d VGAMode[j].mmSizeY ="
-					"%d VGAMode[j].bpp = %d"
-					"VGAMode[j].hz=%d\n",
-					VGAMode[j].mmSizeX, VGAMode[j].mmSizeY,
-					VGAMode[j].bpp, VGAMode[j].hz);
+			dev_dbg(&sfb->pdev->dev,
+				"VGAMode[j].mmSizeX=%d VGAMode[j].mmSizeY=%d "
+				"VGAMode[j].bpp=%d VGAMode[j].hz=%d\n",
+				VGAMode[j].mmSizeX, VGAMode[j].mmSizeY,
+				VGAMode[j].bpp, VGAMode[j].hz);
 
-			smdbg("VGAMode index=%d\n", j);
+			dev_dbg(&sfb->pdev->dev, "VGAMode index=%d\n", j);
 
 			smtc_mmiowb(0x0, 0x3c6);
 
@@ -238,37 +195,37 @@ static void sm712_set_timing(struct smtcfb_info *sfb,
 	smtc_mmiowb(0x67, 0x3c2);
 
 	/* set VPR registers */
-	writel(0x0, ppar_info->m_pVPR + 0x0C);
-	writel(0x0, ppar_info->m_pVPR + 0x40);
+	writel(0x0, sfb->m_pVPR + 0x0C);
+	writel(0x0, sfb->m_pVPR + 0x40);
 
 	/* set data width */
 	m_nScreenStride =
-		(ppar_info->width * sfb->fb.var.bits_per_pixel) / 64;
+		(sfb->width * sfb->fb.var.bits_per_pixel) / 64;
 	switch (sfb->fb.var.bits_per_pixel) {
 	case 8:
-		writel(0x0, ppar_info->m_pVPR + 0x0);
+		writel(0x0, sfb->m_pVPR + 0x0);
 		break;
 	case 16:
-		writel(0x00020000, ppar_info->m_pVPR + 0x0);
+		writel(0x00020000, sfb->m_pVPR + 0x0);
 		break;
 	case 24:
-		writel(0x00040000, ppar_info->m_pVPR + 0x0);
+		writel(0x00040000, sfb->m_pVPR + 0x0);
 		break;
 	case 32:
-		writel(0x00030000, ppar_info->m_pVPR + 0x0);
+		writel(0x00030000, sfb->m_pVPR + 0x0);
 		break;
 	}
 	writel((u32) (((m_nScreenStride + 2) << 16) | m_nScreenStride),
-	       ppar_info->m_pVPR + 0x10);
+	       sfb->m_pVPR + 0x10);
 
 }
 
 static void sm712_setpalette(int regno, unsigned red, unsigned green,
 			     unsigned blue, struct fb_info *info)
 {
-	struct par_info *cur_par = (struct par_info *)info->par;
+	struct smtcfb_info *sfb = info->par;
 
-	if (cur_par->BaseAddressInVRAM)
+	if (sfb->BaseAddressInVRAM)
 		/*
 		 * second display palette for dual head. Enable CRT RAM, 6-bit
 		 * RAM
@@ -283,14 +240,13 @@ static void sm712_setpalette(int regno, unsigned red, unsigned green,
 	smtc_mmiowb(blue >> 10, dac_val);
 }
 
-static void smtc_set_timing(struct smtcfb_info *sfb, struct par_info
-		*ppar_info)
+static void smtc_set_timing(struct smtcfb_info *sfb)
 {
-	switch (ppar_info->chipID) {
+	switch (sfb->chipID) {
 	case 0x710:
 	case 0x712:
 	case 0x720:
-		sm712_set_timing(sfb, ppar_info);
+		sm712_set_timing(sfb);
 		break;
 	}
 }
@@ -310,7 +266,7 @@ static inline unsigned int chan_to_field(unsigned int chan,
 	return chan << bf->offset;
 }
 
-static int cfb_blank(int blank_mode, struct fb_info *info)
+static int smtc_blank(int blank_mode, struct fb_info *info)
 {
 	/* clear DPMS setting */
 	switch (blank_mode) {
@@ -660,10 +616,10 @@ void smtcfb_setmode(struct smtcfb_info *sfb)
 		break;
 	}
 
-	hw.width = sfb->fb.var.xres;
-	hw.height = sfb->fb.var.yres;
-	hw.hz = 60;
-	smtc_set_timing(sfb, &hw);
+	sfb->width = sfb->fb.var.xres;
+	sfb->height = sfb->fb.var.yres;
+	sfb->hz = 60;
+	smtc_set_timing(sfb);
 }
 
 static int smtc_check_var(struct fb_var_screeninfo *var, struct fb_info *info)
@@ -697,7 +653,7 @@ static struct fb_ops smtcfb_ops = {
 	.fb_check_var = smtc_check_var,
 	.fb_set_par   = smtc_set_par,
 	.fb_setcolreg = smtc_setcolreg,
-	.fb_blank     = cfb_blank,
+	.fb_blank     = smtc_blank,
 	.fb_fillrect  = cfb_fillrect,
 	.fb_imageblit = cfb_imageblit,
 	.fb_copyarea  = cfb_copyarea,
@@ -710,8 +666,7 @@ static struct fb_ops smtcfb_ops = {
 /*
  * Alloc struct smtcfb_info and assign the default value
  */
-static struct smtcfb_info *smtc_alloc_fb_info(struct pci_dev *dev,
-							char *name)
+static struct smtcfb_info *smtc_alloc_fb_info(struct pci_dev *pdev, char *name)
 {
 	struct smtcfb_info *sfb;
 
@@ -720,8 +675,7 @@ static struct smtcfb_info *smtc_alloc_fb_info(struct pci_dev *dev,
 	if (!sfb)
 		return NULL;
 
-	sfb->currcon = -1;
-	sfb->dev = dev;
+	sfb->pdev = pdev;
 
 	/*** Init sfb->fb with default value ***/
 	sfb->fb.flags = FBINFO_FLAG_DEFAULT;
@@ -745,7 +699,9 @@ static struct smtcfb_info *smtc_alloc_fb_info(struct pci_dev *dev,
 	/* text mode acceleration */
 	sfb->fb.var.accel_flags = FB_ACCELF_TEXT;
 	sfb->fb.var.vmode = FB_VMODE_NONINTERLACED;
-	sfb->fb.par = &hw;
+
+	sfb->fb.par = sfb;
+
 	sfb->fb.pseudo_palette = colreg;
 
 	return sfb;
@@ -766,17 +722,17 @@ static void smtc_unmap_mmio(struct smtcfb_info *sfb)
  */
 
 static int smtc_map_smem(struct smtcfb_info *sfb,
-		struct pci_dev *dev, u_long smem_len)
+		struct pci_dev *pdev, u_long smem_len)
 {
 	if (sfb->fb.var.bits_per_pixel == 32) {
 #ifdef __BIG_ENDIAN
-		sfb->fb.fix.smem_start = pci_resource_start(dev, 0)
+		sfb->fb.fix.smem_start = pci_resource_start(pdev, 0)
 			+ 0x800000;
 #else
-		sfb->fb.fix.smem_start = pci_resource_start(dev, 0);
+		sfb->fb.fix.smem_start = pci_resource_start(pdev, 0);
 #endif
 	} else {
-		sfb->fb.fix.smem_start = pci_resource_start(dev, 0);
+		sfb->fb.fix.smem_start = pci_resource_start(pdev, 0);
 	}
 
 	sfb->fb.fix.smem_len = smem_len;
@@ -784,8 +740,8 @@ static int smtc_map_smem(struct smtcfb_info *sfb,
 	sfb->fb.screen_base = smtc_VRAMBaseAddress;
 
 	if (!sfb->fb.screen_base) {
-		printk(KERN_ERR "%s: unable to map screen memory\n",
-				sfb->fb.fix.id);
+		dev_err(&pdev->dev,
+			"%s: unable to map screen memory\n", sfb->fb.fix.id);
 		return -ENOMEM;
 	}
 
@@ -831,16 +787,14 @@ static int __init sm712vga_setup(char *options)
 {
 	int index;
 
-	if (!options || !*options) {
-		smdbg("\n No vga parameter\n");
+	if (!options || !*options)
 		return -EINVAL;
-	}
 
 	smtc_screen_info.lfb_width = 0;
 	smtc_screen_info.lfb_height = 0;
 	smtc_screen_info.lfb_depth = 0;
 
-	smdbg("\nsm712vga_setup = %s\n", options);
+	pr_debug("sm712vga_setup = %s\n", options);
 
 	for (index = 0;
 	     index < ARRAY_SIZE(vesa_mode);
@@ -858,10 +812,6 @@ static int __init sm712vga_setup(char *options)
 }
 __setup("vga=", sm712vga_setup);
 
-/* Jason (08/13/2009)
- * Original init function changed to probe method to be used by pci_drv
- * process used to detect chips replaced with kernel process in pci_drv
- */
 static int __devinit smtcfb_pci_probe(struct pci_dev *pdev,
 				   const struct pci_device_id *ent)
 {
@@ -871,23 +821,20 @@ static int __devinit smtcfb_pci_probe(struct pci_dev *pdev,
 	int err;
 	unsigned long pFramebufferPhysical;
 
-	printk(KERN_INFO
-		"Silicon Motion display driver " SMTC_LINUX_FB_VERSION "\n");
+	dev_info(&pdev->dev, "Silicon Motion display driver.");
 
 	err = pci_enable_device(pdev);	/* enable SMTC chip */
 	if (err)
 		return err;
 
-	hw.chipID = ent->device;
-	sprintf(name, "sm%Xfb", hw.chipID);
-
 	sfb = smtc_alloc_fb_info(pdev, name);
 
 	if (!sfb)
 		goto failed_free;
-	/* Jason (08/13/2009)
-	 * Store fb_info to be further used when suspending and resuming
-	 */
+
+	sfb->chipID = ent->device;
+	sprintf(name, "sm%Xfb", sfb->chipID);
+
 	pci_set_drvdata(pdev, sfb);
 
 	sm7xx_init_hw();
@@ -910,37 +857,37 @@ static int __devinit smtcfb_pci_probe(struct pci_dev *pdev,
 #endif
 	/* Map address and memory detection */
 	pFramebufferPhysical = pci_resource_start(pdev, 0);
-	pci_read_config_byte(pdev, PCI_REVISION_ID, &hw.chipRevID);
+	pci_read_config_byte(pdev, PCI_REVISION_ID, &sfb->chipRevID);
 
-	switch (hw.chipID) {
+	switch (sfb->chipID) {
 	case 0x710:
 	case 0x712:
 		sfb->fb.fix.mmio_start = pFramebufferPhysical + 0x00400000;
 		sfb->fb.fix.mmio_len = 0x00400000;
 		smem_size = SM712_VIDEOMEMORYSIZE;
 #ifdef __BIG_ENDIAN
-		hw.m_pLFB = (smtc_VRAMBaseAddress =
+		sfb->m_pLFB = (smtc_VRAMBaseAddress =
 		    ioremap(pFramebufferPhysical, 0x00c00000));
 #else
-		hw.m_pLFB = (smtc_VRAMBaseAddress =
+		sfb->m_pLFB = (smtc_VRAMBaseAddress =
 		    ioremap(pFramebufferPhysical, 0x00800000));
 #endif
-		hw.m_pMMIO = (smtc_RegBaseAddress =
+		sfb->m_pMMIO = (smtc_RegBaseAddress =
 		    smtc_VRAMBaseAddress + 0x00700000);
-		hw.m_pDPR = smtc_VRAMBaseAddress + 0x00408000;
-		hw.m_pVPR = hw.m_pLFB + 0x0040c000;
+		sfb->m_pDPR = smtc_VRAMBaseAddress + 0x00408000;
+		sfb->m_pVPR = sfb->m_pLFB + 0x0040c000;
 #ifdef __BIG_ENDIAN
 		if (sfb->fb.var.bits_per_pixel == 32) {
 			smtc_VRAMBaseAddress += 0x800000;
-			hw.m_pLFB += 0x800000;
-			printk(KERN_INFO
-				"\nsmtc_VRAMBaseAddress=%p hw.m_pLFB=%p\n",
-					smtc_VRAMBaseAddress, hw.m_pLFB);
+			sfb->m_pLFB += 0x800000;
+			dev_info(&pdev->dev,
+				 "smtc_VRAMBaseAddress=%p sfb->m_pLFB=%p",
+				  smtc_VRAMBaseAddress, sfb->m_pLFB);
 		}
 #endif
 		if (!smtc_RegBaseAddress) {
-			printk(KERN_ERR
-				"%s: unable to map memory mapped IO\n",
+			dev_err(&pdev->dev,
+				"%s: unable to map memory mapped IO!",
 				sfb->fb.fix.id);
 			err = -ENOMEM;
 			goto failed_fb;
@@ -962,20 +909,20 @@ static int __devinit smtcfb_pci_probe(struct pci_dev *pdev,
 		sfb->fb.fix.mmio_start = pFramebufferPhysical;
 		sfb->fb.fix.mmio_len = 0x00200000;
 		smem_size = SM722_VIDEOMEMORYSIZE;
-		hw.m_pDPR = ioremap(pFramebufferPhysical, 0x00a00000);
-		hw.m_pLFB = (smtc_VRAMBaseAddress =
-		    hw.m_pDPR + 0x00200000);
-		hw.m_pMMIO = (smtc_RegBaseAddress =
-		    hw.m_pDPR + 0x000c0000);
-		hw.m_pVPR = hw.m_pDPR + 0x800;
+		sfb->m_pDPR = ioremap(pFramebufferPhysical, 0x00a00000);
+		sfb->m_pLFB = (smtc_VRAMBaseAddress =
+		    sfb->m_pDPR + 0x00200000);
+		sfb->m_pMMIO = (smtc_RegBaseAddress =
+		    sfb->m_pDPR + 0x000c0000);
+		sfb->m_pVPR = sfb->m_pDPR + 0x800;
 
 		smtc_seqw(0x62, 0xff);
 		smtc_seqw(0x6a, 0x0d);
 		smtc_seqw(0x6b, 0x02);
 		break;
 	default:
-		printk(KERN_ERR
-		"No valid Silicon Motion display chip was detected!\n");
+		dev_err(&pdev->dev,
+			"No valid Silicon Motion display chip was detected!");
 
 		goto failed_fb;
 	}
@@ -992,22 +939,21 @@ static int __devinit smtcfb_pci_probe(struct pci_dev *pdev,
 
 	smtcfb_setmode(sfb);
 	/* Primary display starting from 0 position */
-	hw.BaseAddressInVRAM = 0;
-	sfb->fb.par = &hw;
+	sfb->BaseAddressInVRAM = 0;
 
 	err = register_framebuffer(&sfb->fb);
 	if (err < 0)
 		goto failed;
 
-	printk(KERN_INFO "Silicon Motion SM%X Rev%X primary display mode"
-			"%dx%d-%d Init Complete.\n", hw.chipID, hw.chipRevID,
-			sfb->fb.var.xres, sfb->fb.var.yres,
-			sfb->fb.var.bits_per_pixel);
+	dev_info(&pdev->dev,
+		 "Silicon Motion SM%X Rev%X primary display mode %dx%d-%d Init Complete.",
+		 sfb->chipID, sfb->chipRevID, sfb->fb.var.xres,
+		 sfb->fb.var.yres, sfb->fb.var.bits_per_pixel);
 
 	return 0;
 
 failed:
-	printk(KERN_ERR "Silicon Motion, Inc.  primary display init fail\n");
+	dev_err(&pdev->dev, "Silicon Motion, Inc. primary display init fail.");
 
 	smtc_unmap_smem(sfb);
 	smtc_unmap_mmio(sfb);
@@ -1021,7 +967,6 @@ failed_free:
 }
 
 
-/* Jason (08/11/2009) PCI_DRV wrapper essential structs */
 static DEFINE_PCI_DEVICE_TABLE(smtcfb_pci_table) = {
 	{ PCI_DEVICE(0x126f, 0x710), },
 	{ PCI_DEVICE(0x126f, 0x712), },
@@ -1030,9 +975,6 @@ static DEFINE_PCI_DEVICE_TABLE(smtcfb_pci_table) = {
 };
 
 
-/* Jason (08/14/2009)
- * do some clean up when the driver module is removed
- */
 static void __devexit smtcfb_pci_remove(struct pci_dev *pdev)
 {
 	struct smtcfb_info *sfb;
@@ -1078,7 +1020,7 @@ static int smtcfb_pci_resume(struct device *device)
 
 	/* reinit hardware */
 	sm7xx_init_hw();
-	switch (hw.chipID) {
+	switch (sfb->chipID) {
 	case 0x710:
 	case 0x712:
 		/* set MCLK = 14.31818 *  (0x16 / 0x2) */
diff --git a/drivers/staging/sm7xx/smtcfb.h b/drivers/staging/sm7xx/smtcfb.h
index ab95af2b9c0..43d86f87341 100644
--- a/drivers/staging/sm7xx/smtcfb.h
+++ b/drivers/staging/sm7xx/smtcfb.h
@@ -13,19 +13,11 @@
  *  more details.
  */
 
-#define SMTC_LINUX_FB_VERSION	"version 0.11.2619.21.01 July 27, 2008"
-
 #define NR_PALETTE        256
 #define NR_RGB            2
 
 #define FB_ACCEL_SMI_LYNX 88
 
-#ifdef __BIG_ENDIAN
-#define PC_VGA            0
-#else
-#define PC_VGA            1
-#endif
-
 #define SCREEN_X_RES      1024
 #define SCREEN_Y_RES      600
 #define SCREEN_BPP        16
diff --git a/drivers/staging/telephony/ixj.c b/drivers/staging/telephony/ixj.c
index f96027921f6..fd7757ad7fa 100644
--- a/drivers/staging/telephony/ixj.c
+++ b/drivers/staging/telephony/ixj.c
@@ -19,20 +19,20 @@
  *                  David W. Erhart, <derhart@quicknet.net>
  *                  John Sellers, <jsellers@quicknet.net>
  *                  Mike Preston, <mpreston@quicknet.net>
- *    
+ *
  * Fixes:           David Huggins-Daines, <dhd@cepstral.com>
  *                  Fabio Ferrari, <fabio.ferrari@digitro.com.br>
  *                  Artis Kugevics, <artis@mt.lv>
  *                  Daniele Bellucci, <bellucda@tiscali.it>
  *
- * More information about the hardware related to this driver can be found  
+ * More information about the hardware related to this driver can be found
  * at our website:    http://www.quicknet.net
  *
  * IN NO EVENT SHALL QUICKNET TECHNOLOGIES, INC. BE LIABLE TO ANY PARTY FOR
  * DIRECT, INDIRECT, SPECIAL, INCIDENTAL, OR CONSEQUENTIAL DAMAGES ARISING OUT
  * OF THE USE OF THIS SOFTWARE AND ITS DOCUMENTATION, EVEN IF QUICKNET
  * TECHNOLOGIES, INC. HAS BEEN ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
- *    
+ *
  * QUICKNET TECHNOLOGIES, INC. SPECIFICALLY DISCLAIMS ANY WARRANTIES,
  * INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY
  * AND FITNESS FOR A PARTICULAR PURPOSE.  THE SOFTWARE PROVIDED HEREUNDER IS
@@ -317,7 +317,7 @@ static IXJ *ixj[IXJMAX];
 /*
  *	Allocate a free IXJ device
  */
- 
+
 static IXJ *ixj_alloc()
 {
 	for(cnt=0; cnt<IXJMAX; cnt++)
@@ -366,7 +366,7 @@ static IXJ ixj[IXJMAX];
 /*
  *	Allocate a free IXJ device
  */
- 
+
 static IXJ *ixj_alloc(void)
 {
 	int cnt;
@@ -1084,7 +1084,7 @@ static void ixj_pstn_state(IXJ *j)
 							printk(KERN_INFO "IXJ /dev/phone%d Next Ring Cadence state at %u min %ld - %ld - max %ld\n", j->board,
 						j->cadence_f[4].on3, j->cadence_f[4].on3min, j->cadence_f[4].on3dot, j->cadence_f[4].on3max);
 							break;
-						case 6:	
+						case 6:
 							printk(KERN_INFO "IXJ /dev/phone%d Next Ring Cadence state at %u min %ld - %ld - max %ld\n", j->board,
 						j->cadence_f[4].off3, j->cadence_f[4].off3min, j->cadence_f[4].off3dot, j->cadence_f[4].off3max);
 							break;
@@ -1109,7 +1109,7 @@ static void ixj_pstn_state(IXJ *j)
 				}
 				j->pstn_ring_stop = j->pstn_ring_int = 0;
 				daa_set_mode(j, SOP_PU_SLEEP);
-			} 
+			}
 			outb_p(j->pld_scrw.byte, j->XILINXbase);
 			if (j->pstn_cid_intr && time_after(jiffies, j->pstn_cid_received + hertz)) {
 				ixj_daa_cid_read(j);
@@ -1133,7 +1133,7 @@ static void ixj_pstn_state(IXJ *j)
 							printk("IXJ DAA possible wink /dev/phone%d %ld\n", j->board, jiffies);
 						}
 						j->pstn_winkstart = jiffies;
-					} 
+					}
 				} else {
 					if (j->pstn_winkstart) {
 						if(ixjdebug & 0x0008) {
@@ -1524,7 +1524,7 @@ static inline void LED_SetState(int state, IXJ *j)
 /*********************************************************************
 *  GPIO Pins are configured as follows on the Quicknet Internet
 *  PhoneJACK Telephony Cards
-* 
+*
 * POTS Select        GPIO_6=0 GPIO_7=0
 * Mic/Speaker Select GPIO_6=0 GPIO_7=1
 * Handset Select     GPIO_6=1 GPIO_7=0
@@ -1932,7 +1932,7 @@ static int ixj_hookstate(IXJ *j)
 			if(fOffHook != j->p_hook) {
 				if(!j->checkwait) {
 					j->checkwait = jiffies;
-				} 
+				}
 				if(time_before(jiffies, j->checkwait + 2)) {
 					fOffHook ^= 1;
 				} else {
@@ -2342,8 +2342,8 @@ static int ixj_release(struct inode *inode, struct file *file_p)
 		j->ixj_signals[cnt] = SIGIO;
 
 	/* Set the excetion signal enable flags */
-	j->ex_sig.bits.dtmf_ready = j->ex_sig.bits.hookstate = j->ex_sig.bits.flash = j->ex_sig.bits.pstn_ring = 
-	j->ex_sig.bits.caller_id = j->ex_sig.bits.pstn_wink = j->ex_sig.bits.f0 = j->ex_sig.bits.f1 = j->ex_sig.bits.f2 = 
+	j->ex_sig.bits.dtmf_ready = j->ex_sig.bits.hookstate = j->ex_sig.bits.flash = j->ex_sig.bits.pstn_ring =
+	j->ex_sig.bits.caller_id = j->ex_sig.bits.pstn_wink = j->ex_sig.bits.f0 = j->ex_sig.bits.f1 = j->ex_sig.bits.f2 =
 	j->ex_sig.bits.f3 = j->ex_sig.bits.fc0 = j->ex_sig.bits.fc1 = j->ex_sig.bits.fc2 = j->ex_sig.bits.fc3 = 1;
 
 	file_p->private_data = NULL;
@@ -2506,7 +2506,7 @@ static int read_filters(IXJ *j)
 					j->cadence_f[cnt].on1, j->cadence_f[cnt].on1min, j->cadence_f[cnt].on1dot, j->cadence_f[cnt].on1max);
 						break;
 					case 2:
-						printk(KERN_INFO "IXJ /dev/phone%d Next Tone Cadence state at %ld - %ld\n", j->board, j->cadence_f[cnt].off1min, 
+						printk(KERN_INFO "IXJ /dev/phone%d Next Tone Cadence state at %ld - %ld\n", j->board, j->cadence_f[cnt].off1min,
 															j->cadence_f[cnt].off1max);
 						break;
 					case 3:
@@ -2521,12 +2521,12 @@ static int read_filters(IXJ *j)
 						printk(KERN_INFO "IXJ /dev/phone%d Next Tone Cadence state at %ld - %ld\n", j->board, j->cadence_f[cnt].on3min,
 															j->cadence_f[cnt].on3max);
 						break;
-					case 6:	
+					case 6:
 						printk(KERN_INFO "IXJ /dev/phone%d Next Tone Cadence state at %ld - %ld\n", j->board, j->cadence_f[cnt].off3min,
 															j->cadence_f[cnt].off3max);
 						break;
 				}
-			} 
+			}
 		}
 		if (j->cadence_f[cnt].state == 7) {
 			j->cadence_f[cnt].state = 0;
@@ -2656,37 +2656,37 @@ static void ulaw2alaw(unsigned char *buff, unsigned long len)
 {
 	static unsigned char table_ulaw2alaw[] =
 	{
-		0x2A, 0x2B, 0x28, 0x29, 0x2E, 0x2F, 0x2C, 0x2D, 
-		0x22, 0x23, 0x20, 0x21, 0x26, 0x27, 0x24, 0x25, 
-		0x3A, 0x3B, 0x38, 0x39, 0x3E, 0x3F, 0x3C, 0x3D, 
-		0x32, 0x33, 0x30, 0x31, 0x36, 0x37, 0x34, 0x35, 
-		0x0B, 0x08, 0x09, 0x0E, 0x0F, 0x0C, 0x0D, 0x02, 
-		0x03, 0x00, 0x01, 0x06, 0x07, 0x04, 0x05, 0x1A, 
-		0x1B, 0x18, 0x19, 0x1E, 0x1F, 0x1C, 0x1D, 0x12, 
-		0x13, 0x10, 0x11, 0x16, 0x17, 0x14, 0x15, 0x6B, 
-		0x68, 0x69, 0x6E, 0x6F, 0x6C, 0x6D, 0x62, 0x63, 
-		0x60, 0x61, 0x66, 0x67, 0x64, 0x65, 0x7B, 0x79, 
-		0x7E, 0x7F, 0x7C, 0x7D, 0x72, 0x73, 0x70, 0x71, 
-		0x76, 0x77, 0x74, 0x75, 0x4B, 0x49, 0x4F, 0x4D, 
-		0x42, 0x43, 0x40, 0x41, 0x46, 0x47, 0x44, 0x45, 
-		0x5A, 0x5B, 0x58, 0x59, 0x5E, 0x5F, 0x5C, 0x5D, 
-		0x52, 0x52, 0x53, 0x53, 0x50, 0x50, 0x51, 0x51, 
-		0x56, 0x56, 0x57, 0x57, 0x54, 0x54, 0x55, 0xD5, 
-		0xAA, 0xAB, 0xA8, 0xA9, 0xAE, 0xAF, 0xAC, 0xAD, 
-		0xA2, 0xA3, 0xA0, 0xA1, 0xA6, 0xA7, 0xA4, 0xA5, 
-		0xBA, 0xBB, 0xB8, 0xB9, 0xBE, 0xBF, 0xBC, 0xBD, 
-		0xB2, 0xB3, 0xB0, 0xB1, 0xB6, 0xB7, 0xB4, 0xB5, 
-		0x8B, 0x88, 0x89, 0x8E, 0x8F, 0x8C, 0x8D, 0x82, 
-		0x83, 0x80, 0x81, 0x86, 0x87, 0x84, 0x85, 0x9A, 
-		0x9B, 0x98, 0x99, 0x9E, 0x9F, 0x9C, 0x9D, 0x92, 
-		0x93, 0x90, 0x91, 0x96, 0x97, 0x94, 0x95, 0xEB, 
-		0xE8, 0xE9, 0xEE, 0xEF, 0xEC, 0xED, 0xE2, 0xE3, 
-		0xE0, 0xE1, 0xE6, 0xE7, 0xE4, 0xE5, 0xFB, 0xF9, 
-		0xFE, 0xFF, 0xFC, 0xFD, 0xF2, 0xF3, 0xF0, 0xF1, 
-		0xF6, 0xF7, 0xF4, 0xF5, 0xCB, 0xC9, 0xCF, 0xCD, 
-		0xC2, 0xC3, 0xC0, 0xC1, 0xC6, 0xC7, 0xC4, 0xC5, 
-		0xDA, 0xDB, 0xD8, 0xD9, 0xDE, 0xDF, 0xDC, 0xDD, 
-		0xD2, 0xD2, 0xD3, 0xD3, 0xD0, 0xD0, 0xD1, 0xD1, 
+		0x2A, 0x2B, 0x28, 0x29, 0x2E, 0x2F, 0x2C, 0x2D,
+		0x22, 0x23, 0x20, 0x21, 0x26, 0x27, 0x24, 0x25,
+		0x3A, 0x3B, 0x38, 0x39, 0x3E, 0x3F, 0x3C, 0x3D,
+		0x32, 0x33, 0x30, 0x31, 0x36, 0x37, 0x34, 0x35,
+		0x0B, 0x08, 0x09, 0x0E, 0x0F, 0x0C, 0x0D, 0x02,
+		0x03, 0x00, 0x01, 0x06, 0x07, 0x04, 0x05, 0x1A,
+		0x1B, 0x18, 0x19, 0x1E, 0x1F, 0x1C, 0x1D, 0x12,
+		0x13, 0x10, 0x11, 0x16, 0x17, 0x14, 0x15, 0x6B,
+		0x68, 0x69, 0x6E, 0x6F, 0x6C, 0x6D, 0x62, 0x63,
+		0x60, 0x61, 0x66, 0x67, 0x64, 0x65, 0x7B, 0x79,
+		0x7E, 0x7F, 0x7C, 0x7D, 0x72, 0x73, 0x70, 0x71,
+		0x76, 0x77, 0x74, 0x75, 0x4B, 0x49, 0x4F, 0x4D,
+		0x42, 0x43, 0x40, 0x41, 0x46, 0x47, 0x44, 0x45,
+		0x5A, 0x5B, 0x58, 0x59, 0x5E, 0x5F, 0x5C, 0x5D,
+		0x52, 0x52, 0x53, 0x53, 0x50, 0x50, 0x51, 0x51,
+		0x56, 0x56, 0x57, 0x57, 0x54, 0x54, 0x55, 0xD5,
+		0xAA, 0xAB, 0xA8, 0xA9, 0xAE, 0xAF, 0xAC, 0xAD,
+		0xA2, 0xA3, 0xA0, 0xA1, 0xA6, 0xA7, 0xA4, 0xA5,
+		0xBA, 0xBB, 0xB8, 0xB9, 0xBE, 0xBF, 0xBC, 0xBD,
+		0xB2, 0xB3, 0xB0, 0xB1, 0xB6, 0xB7, 0xB4, 0xB5,
+		0x8B, 0x88, 0x89, 0x8E, 0x8F, 0x8C, 0x8D, 0x82,
+		0x83, 0x80, 0x81, 0x86, 0x87, 0x84, 0x85, 0x9A,
+		0x9B, 0x98, 0x99, 0x9E, 0x9F, 0x9C, 0x9D, 0x92,
+		0x93, 0x90, 0x91, 0x96, 0x97, 0x94, 0x95, 0xEB,
+		0xE8, 0xE9, 0xEE, 0xEF, 0xEC, 0xED, 0xE2, 0xE3,
+		0xE0, 0xE1, 0xE6, 0xE7, 0xE4, 0xE5, 0xFB, 0xF9,
+		0xFE, 0xFF, 0xFC, 0xFD, 0xF2, 0xF3, 0xF0, 0xF1,
+		0xF6, 0xF7, 0xF4, 0xF5, 0xCB, 0xC9, 0xCF, 0xCD,
+		0xC2, 0xC3, 0xC0, 0xC1, 0xC6, 0xC7, 0xC4, 0xC5,
+		0xDA, 0xDB, 0xD8, 0xD9, 0xDE, 0xDF, 0xDC, 0xDD,
+		0xD2, 0xD2, 0xD3, 0xD3, 0xD0, 0xD0, 0xD1, 0xD1,
 		0xD6, 0xD6, 0xD7, 0xD7, 0xD4, 0xD4, 0xD5, 0xD5
 	};
 
@@ -2701,37 +2701,37 @@ static void alaw2ulaw(unsigned char *buff, unsigned long len)
 {
 	static unsigned char table_alaw2ulaw[] =
 	{
-		0x29, 0x2A, 0x27, 0x28, 0x2D, 0x2E, 0x2B, 0x2C, 
-		0x21, 0x22, 0x1F, 0x20, 0x25, 0x26, 0x23, 0x24, 
-		0x39, 0x3A, 0x37, 0x38, 0x3D, 0x3E, 0x3B, 0x3C, 
-		0x31, 0x32, 0x2F, 0x30, 0x35, 0x36, 0x33, 0x34, 
-		0x0A, 0x0B, 0x08, 0x09, 0x0E, 0x0F, 0x0C, 0x0D, 
-		0x02, 0x03, 0x00, 0x01, 0x06, 0x07, 0x04, 0x05, 
-		0x1A, 0x1B, 0x18, 0x19, 0x1E, 0x1F, 0x1C, 0x1D, 
-		0x12, 0x13, 0x10, 0x11, 0x16, 0x17, 0x14, 0x15, 
-		0x62, 0x63, 0x60, 0x61, 0x66, 0x67, 0x64, 0x65, 
-		0x5D, 0x5D, 0x5C, 0x5C, 0x5F, 0x5F, 0x5E, 0x5E, 
-		0x74, 0x76, 0x70, 0x72, 0x7C, 0x7E, 0x78, 0x7A, 
-		0x6A, 0x6B, 0x68, 0x69, 0x6E, 0x6F, 0x6C, 0x6D, 
-		0x48, 0x49, 0x46, 0x47, 0x4C, 0x4D, 0x4A, 0x4B, 
-		0x40, 0x41, 0x3F, 0x3F, 0x44, 0x45, 0x42, 0x43, 
-		0x56, 0x57, 0x54, 0x55, 0x5A, 0x5B, 0x58, 0x59, 
-		0x4F, 0x4F, 0x4E, 0x4E, 0x52, 0x53, 0x50, 0x51, 
-		0xA9, 0xAA, 0xA7, 0xA8, 0xAD, 0xAE, 0xAB, 0xAC, 
-		0xA1, 0xA2, 0x9F, 0xA0, 0xA5, 0xA6, 0xA3, 0xA4, 
-		0xB9, 0xBA, 0xB7, 0xB8, 0xBD, 0xBE, 0xBB, 0xBC, 
-		0xB1, 0xB2, 0xAF, 0xB0, 0xB5, 0xB6, 0xB3, 0xB4, 
-		0x8A, 0x8B, 0x88, 0x89, 0x8E, 0x8F, 0x8C, 0x8D, 
-		0x82, 0x83, 0x80, 0x81, 0x86, 0x87, 0x84, 0x85, 
-		0x9A, 0x9B, 0x98, 0x99, 0x9E, 0x9F, 0x9C, 0x9D, 
-		0x92, 0x93, 0x90, 0x91, 0x96, 0x97, 0x94, 0x95, 
-		0xE2, 0xE3, 0xE0, 0xE1, 0xE6, 0xE7, 0xE4, 0xE5, 
-		0xDD, 0xDD, 0xDC, 0xDC, 0xDF, 0xDF, 0xDE, 0xDE, 
-		0xF4, 0xF6, 0xF0, 0xF2, 0xFC, 0xFE, 0xF8, 0xFA, 
-		0xEA, 0xEB, 0xE8, 0xE9, 0xEE, 0xEF, 0xEC, 0xED, 
-		0xC8, 0xC9, 0xC6, 0xC7, 0xCC, 0xCD, 0xCA, 0xCB, 
-		0xC0, 0xC1, 0xBF, 0xBF, 0xC4, 0xC5, 0xC2, 0xC3, 
-		0xD6, 0xD7, 0xD4, 0xD5, 0xDA, 0xDB, 0xD8, 0xD9, 
+		0x29, 0x2A, 0x27, 0x28, 0x2D, 0x2E, 0x2B, 0x2C,
+		0x21, 0x22, 0x1F, 0x20, 0x25, 0x26, 0x23, 0x24,
+		0x39, 0x3A, 0x37, 0x38, 0x3D, 0x3E, 0x3B, 0x3C,
+		0x31, 0x32, 0x2F, 0x30, 0x35, 0x36, 0x33, 0x34,
+		0x0A, 0x0B, 0x08, 0x09, 0x0E, 0x0F, 0x0C, 0x0D,
+		0x02, 0x03, 0x00, 0x01, 0x06, 0x07, 0x04, 0x05,
+		0x1A, 0x1B, 0x18, 0x19, 0x1E, 0x1F, 0x1C, 0x1D,
+		0x12, 0x13, 0x10, 0x11, 0x16, 0x17, 0x14, 0x15,
+		0x62, 0x63, 0x60, 0x61, 0x66, 0x67, 0x64, 0x65,
+		0x5D, 0x5D, 0x5C, 0x5C, 0x5F, 0x5F, 0x5E, 0x5E,
+		0x74, 0x76, 0x70, 0x72, 0x7C, 0x7E, 0x78, 0x7A,
+		0x6A, 0x6B, 0x68, 0x69, 0x6E, 0x6F, 0x6C, 0x6D,
+		0x48, 0x49, 0x46, 0x47, 0x4C, 0x4D, 0x4A, 0x4B,
+		0x40, 0x41, 0x3F, 0x3F, 0x44, 0x45, 0x42, 0x43,
+		0x56, 0x57, 0x54, 0x55, 0x5A, 0x5B, 0x58, 0x59,
+		0x4F, 0x4F, 0x4E, 0x4E, 0x52, 0x53, 0x50, 0x51,
+		0xA9, 0xAA, 0xA7, 0xA8, 0xAD, 0xAE, 0xAB, 0xAC,
+		0xA1, 0xA2, 0x9F, 0xA0, 0xA5, 0xA6, 0xA3, 0xA4,
+		0xB9, 0xBA, 0xB7, 0xB8, 0xBD, 0xBE, 0xBB, 0xBC,
+		0xB1, 0xB2, 0xAF, 0xB0, 0xB5, 0xB6, 0xB3, 0xB4,
+		0x8A, 0x8B, 0x88, 0x89, 0x8E, 0x8F, 0x8C, 0x8D,
+		0x82, 0x83, 0x80, 0x81, 0x86, 0x87, 0x84, 0x85,
+		0x9A, 0x9B, 0x98, 0x99, 0x9E, 0x9F, 0x9C, 0x9D,
+		0x92, 0x93, 0x90, 0x91, 0x96, 0x97, 0x94, 0x95,
+		0xE2, 0xE3, 0xE0, 0xE1, 0xE6, 0xE7, 0xE4, 0xE5,
+		0xDD, 0xDD, 0xDC, 0xDC, 0xDF, 0xDF, 0xDE, 0xDE,
+		0xF4, 0xF6, 0xF0, 0xF2, 0xFC, 0xFE, 0xF8, 0xFA,
+		0xEA, 0xEB, 0xE8, 0xE9, 0xEE, 0xEF, 0xEC, 0xED,
+		0xC8, 0xC9, 0xC6, 0xC7, 0xCC, 0xCD, 0xCA, 0xCB,
+		0xC0, 0xC1, 0xBF, 0xBF, 0xC4, 0xC5, 0xC2, 0xC3,
+		0xD6, 0xD7, 0xD4, 0xD5, 0xDA, 0xDB, 0xD8, 0xD9,
 		0xCF, 0xCF, 0xCE, 0xCE, 0xD2, 0xD3, 0xD0, 0xD1
 	};
 
@@ -3090,7 +3090,7 @@ static int ixj_write_cid_string(IXJ *j, char *s, int checksum)
 
 static void ixj_pad_fsk(IXJ *j, int pad)
 {
-	int cnt; 
+	int cnt;
 
 	for (cnt = 0; cnt < pad; cnt++) {
 		if(j->fskdcnt < (j->fsksize - 1))
@@ -3474,7 +3474,7 @@ static void ixj_write_frame(IXJ *j)
 			ixj_post_cid(j);
 		}
 		/* This may seem rude, but if we just played one frame of FSK data for CallerID
-		   and there is real audio data in the buffer, we need to throw it away because 
+		   and there is real audio data in the buffer, we need to throw it away because
 		   we just used it's time slot */
 		if (j->write_buffer_rp > j->write_buffer_wp) {
 			j->write_buffer_rp += j->cid_play_frame_size * 2;
@@ -3486,7 +3486,7 @@ static void ixj_write_frame(IXJ *j)
 
 			wake_up_interruptible(&j->poll_q);	/* Wake any blocked selects */
 		}
-	} else if (j->write_buffer && j->write_buffers_empty < 1) { 
+	} else if (j->write_buffer && j->write_buffers_empty < 1) {
 		if (j->write_buffer_wp > j->write_buffer_rp) {
 			frame_count =
 			    (j->write_buffer_wp - j->write_buffer_rp) / (j->play_frame_size * 2);
@@ -4150,7 +4150,7 @@ static void ixj_aec_start(IXJ *j, int level)
 
 			ixj_WriteDSPCommand(0xCF97, j);	/* Set AGC Enable */
 			ixj_WriteDSPCommand(0x0000, j);	/* to off */
-			
+
 			break;
 
 		case AEC_MED:
@@ -4161,7 +4161,7 @@ static void ixj_aec_start(IXJ *j, int level)
 
 			ixj_WriteDSPCommand(0xCF97, j);	/* Set AGC Enable */
 			ixj_WriteDSPCommand(0x0000, j);	/* to off */
-			
+
 			break;
 
 		case AEC_HIGH:
@@ -4172,7 +4172,7 @@ static void ixj_aec_start(IXJ *j, int level)
 
 			ixj_WriteDSPCommand(0xCF97, j);	/* Set AGC Enable */
 			ixj_WriteDSPCommand(0x0000, j);	/* to off */
-			
+
 			break;
 
 		case AEC_AGC:
@@ -4197,28 +4197,28 @@ static void ixj_aec_start(IXJ *j, int level)
 			/* Now we can set the AGC initial parameters and turn it on */
 			ixj_WriteDSPCommand(0xCF90, j);	/* Set AGC Minimum gain */
 			ixj_WriteDSPCommand(0x0020, j);	/* to 0.125 (-18dB) */
-	
+
 			ixj_WriteDSPCommand(0xCF91, j);	/* Set AGC Maximum gain */
 			ixj_WriteDSPCommand(0x1000, j);	/* to 16 (24dB) */
-			
+
 			ixj_WriteDSPCommand(0xCF92, j);	/* Set AGC start gain */
 			ixj_WriteDSPCommand(0x0800, j);	/* to 8 (+18dB) */
-		
+
 			ixj_WriteDSPCommand(0xCF93, j);	/* Set AGC hold time */
 			ixj_WriteDSPCommand(0x1F40, j);	/* to 2 seconds (units are 250us) */
-			
+
 			ixj_WriteDSPCommand(0xCF94, j);	/* Set AGC Attack Time Constant */
 			ixj_WriteDSPCommand(0x0005, j);	/* to 8ms */
-			
+
 			ixj_WriteDSPCommand(0xCF95, j);	/* Set AGC Decay Time Constant */
 			ixj_WriteDSPCommand(0x000D, j);	/* to 4096ms */
-			
+
 			ixj_WriteDSPCommand(0xCF96, j);	/* Set AGC Attack Threshold */
 			ixj_WriteDSPCommand(0x1200, j);	/* to 25% */
-			
+
 			ixj_WriteDSPCommand(0xCF97, j);	/* Set AGC Enable */
 			ixj_WriteDSPCommand(0x0001, j);	/* to on */
-			
+
 			break;
 
 		case AEC_AUTO:
@@ -4495,7 +4495,7 @@ static int ixj_play_start(IXJ *j)
 		return -ENOMEM;
 	}
 /*	j->write_buffers_empty = 2; */
-	j->write_buffers_empty = 1; 
+	j->write_buffers_empty = 1;
 	j->write_buffer_size = j->play_frame_size * 2;
 	j->write_buffer_end = j->write_buffer + j->play_frame_size * 2;
 	j->write_buffer_rp = j->write_buffer_wp = j->write_buffer;
@@ -6465,9 +6465,9 @@ static long do_ixj_ioctl(struct file *file_p, unsigned int cmd, unsigned long ar
 		ixj_ringback(j);
 		break;
 	case PHONE_WINK:
-		if(j->cardtype == QTI_PHONEJACK) 
+		if(j->cardtype == QTI_PHONEJACK)
 			retval = -1;
-		else 
+		else
 			retval = ixj_wink(j);
 		break;
 	case PHONE_CPT_STOP:
@@ -6553,7 +6553,7 @@ static long do_ixj_ioctl(struct file *file_p, unsigned int cmd, unsigned long ar
 		ixj_write_vmwi(j, arg);
 		break;
 	case IXJCTL_CID:
-		if (copy_to_user(argp, &j->cid, sizeof(PHONE_CID))) 
+		if (copy_to_user(argp, &j->cid, sizeof(PHONE_CID)))
 			retval = -EFAULT;
 		j->ex.bits.caller_id = 0;
 		break;
@@ -6575,13 +6575,13 @@ static long do_ixj_ioctl(struct file *file_p, unsigned int cmd, unsigned long ar
 		break;
 	case PHONE_CAPABILITIES_LIST:
 		add_caps(j);
-		if (copy_to_user(argp, j->caplist, sizeof(struct phone_capability) * j->caps)) 
+		if (copy_to_user(argp, j->caplist, sizeof(struct phone_capability) * j->caps))
 			retval = -EFAULT;
 		break;
 	case PHONE_CAPABILITIES_CHECK:
 		{
 			struct phone_capability cap;
-			if (copy_from_user(&cap, argp, sizeof(cap))) 
+			if (copy_from_user(&cap, argp, sizeof(cap)))
 				retval = -EFAULT;
 			else {
 				add_caps(j);
@@ -6597,13 +6597,13 @@ static long do_ixj_ioctl(struct file *file_p, unsigned int cmd, unsigned long ar
 		j->ex.bits.pstn_ring = 0;
 		break;
 	case IXJCTL_SET_FILTER:
-		if (copy_from_user(&jf, argp, sizeof(jf))) 
+		if (copy_from_user(&jf, argp, sizeof(jf)))
 			retval = -EFAULT;
 		else
 			retval = ixj_init_filter(j, &jf);
 		break;
 	case IXJCTL_SET_FILTER_RAW:
-		if (copy_from_user(&jfr, argp, sizeof(jfr))) 
+		if (copy_from_user(&jfr, argp, sizeof(jfr)))
 			retval = -EFAULT;
 		else
 			retval = ixj_init_filter_raw(j, &jfr);
@@ -6638,9 +6638,9 @@ static long do_ixj_ioctl(struct file *file_p, unsigned int cmd, unsigned long ar
 				raise *= 2;
 			}
 			if(j->sigdef.signal)
-				j->ex_sig.bytes |= raise; 
+				j->ex_sig.bytes |= raise;
 			else
-				j->ex_sig.bytes &= (raise^0xffff); 
+				j->ex_sig.bytes &= (raise^0xffff);
 		}
 		break;
 	case IXJCTL_INTERCOM_STOP:
@@ -7040,9 +7040,9 @@ static int ixj_selfprobe(IXJ *j)
 
 	/* initialise the DTMF prescale to a sensible value */
 	if (j->cardtype == QTI_LINEJACK) {
-		set_dtmf_prescale(j, 0x10); 
+		set_dtmf_prescale(j, 0x10);
 	} else {
-		set_dtmf_prescale(j, 0x40); 
+		set_dtmf_prescale(j, 0x40);
 	}
 	set_play_volume(j, 0x100);
 	set_rec_volume(j, 0x100);
@@ -7095,15 +7095,15 @@ static int ixj_selfprobe(IXJ *j)
 		j->ixj_signals[cnt] = SIGIO;
 
 	/* Set the excetion signal enable flags */
-	j->ex_sig.bits.dtmf_ready = j->ex_sig.bits.hookstate = j->ex_sig.bits.flash = j->ex_sig.bits.pstn_ring = 
-	j->ex_sig.bits.caller_id = j->ex_sig.bits.pstn_wink = j->ex_sig.bits.f0 = j->ex_sig.bits.f1 = j->ex_sig.bits.f2 = 
+	j->ex_sig.bits.dtmf_ready = j->ex_sig.bits.hookstate = j->ex_sig.bits.flash = j->ex_sig.bits.pstn_ring =
+	j->ex_sig.bits.caller_id = j->ex_sig.bits.pstn_wink = j->ex_sig.bits.f0 = j->ex_sig.bits.f1 = j->ex_sig.bits.f2 =
 	j->ex_sig.bits.f3 = j->ex_sig.bits.fc0 = j->ex_sig.bits.fc1 = j->ex_sig.bits.fc2 = j->ex_sig.bits.fc3 = 1;
 #ifdef IXJ_DYN_ALLOC
 	j->fskdata = NULL;
 #endif
 	j->fskdcnt = 0;
 	j->cidcw_wait = 0;
- 
+
 	/* Register with the Telephony for Linux subsystem */
 	j->p.f_op = &ixj_fops;
 	j->p.open = ixj_open;
@@ -7118,7 +7118,7 @@ static int ixj_selfprobe(IXJ *j)
 /*
  *	Exported service for pcmcia card handling
  */
- 
+
 IXJ *ixj_pcmcia_probe(unsigned long dsp, unsigned long xilinx)
 {
 	IXJ *j = ixj_alloc();
@@ -7320,7 +7320,7 @@ static int ixj_get_status_proc(char *buf)
 			len += sprintf(buf + len, "\nRec volume 0x%x", get_rec_volume(j));
 			len += sprintf(buf + len, "\nPlay volume 0x%x", get_play_volume(j));
 			len += sprintf(buf + len, "\nDTMF prescale 0x%x", get_dtmf_prescale(j));
-			
+
 			len += sprintf(buf + len, "\nHook state %d", j->hookstate); /* j->r_hook);	*/
 
 			if (j->cardtype == QTI_LINEJACK) {
@@ -7417,7 +7417,7 @@ static int ixj_get_status_proc(char *buf)
                         len += sprintf(buf + len, "\nPControl Wait Fails %ld", j->pcontrolwaitfail);
                         len += sprintf(buf + len, "\nIs Control Ready Checks %ld", j->iscontrolready);
                         len += sprintf(buf + len, "\nIs Control Ready Check failures %ld", j->iscontrolreadyfail);
- 
+
 #endif
 			len += sprintf(buf + len, "\n");
 		}
@@ -7608,7 +7608,7 @@ static IXJ *new_ixj(unsigned long port)
 }
 
 static int __init ixj_probe_isapnp(int *cnt)
-{               
+{
 	int probe = 0;
 	int func = 0x110;
         struct pnp_dev *dev = NULL, *old_dev = NULL;
@@ -7686,7 +7686,7 @@ static int __init ixj_probe_isapnp(int *cnt)
 	}
 	return probe;
 }
-                        
+
 static int __init ixj_probe_isa(int *cnt)
 {
 	int i, probe;
@@ -7713,7 +7713,7 @@ static int __init ixj_probe_isa(int *cnt)
 
 static int __init ixj_probe_pci(int *cnt)
 {
-	struct pci_dev *pci = NULL;   
+	struct pci_dev *pci = NULL;
 	int i, probe = 0;
 	IXJ *j = NULL;
 
@@ -7745,7 +7745,7 @@ static int __init ixj_probe_pci(int *cnt)
 static int __init ixj_init(void)
 {
 	int cnt = 0;
-	int probe = 0;   
+	int probe = 0;
 
 	cnt = 0;
 
@@ -7887,7 +7887,7 @@ static void DAA_Coeff_US(IXJ *j)
 /*	j->m_DAAShadowRegs.COP_REGS.COP.RingerImpendance_1[2] = 0x2D; */
 /*	j->m_DAAShadowRegs.COP_REGS.COP.RingerImpendance_1[1] = 0x62; */
 /*	j->m_DAAShadowRegs.COP_REGS.COP.RingerImpendance_1[0] = 0x2D; */
-	/* Bytes for Ringing part 2 (06):13,42,A6,BA,D4,73,CA,D5 */ 
+	/* Bytes for Ringing part 2 (06):13,42,A6,BA,D4,73,CA,D5 */
 /*	j->m_DAAShadowRegs.COP_REGS.COP.RingerImpendance_2[7] = 0x2D; */
 /*	j->m_DAAShadowRegs.COP_REGS.COP.RingerImpendance_2[6] = 0x62; */
 /*	j->m_DAAShadowRegs.COP_REGS.COP.RingerImpendance_2[5] = 0xA6; */
diff --git a/drivers/staging/tidspbridge/core/io_sm.c b/drivers/staging/tidspbridge/core/io_sm.c
index 9b50b5bd4ed..c51f651dfd1 100644
--- a/drivers/staging/tidspbridge/core/io_sm.c
+++ b/drivers/staging/tidspbridge/core/io_sm.c
@@ -2212,7 +2212,7 @@ void dump_dl_modules(struct bridge_dev_context *bridge_context)
 
 		if (status) {
 			pr_debug(
-			"%s: Failed to read dll_module stuct for 0x%x.\n",
+			"%s: Failed to read dll_module struct for 0x%x.\n",
 			__func__, module_dsp_addr);
 			break;
 		}
diff --git a/drivers/staging/tidspbridge/core/ue_deh.c b/drivers/staging/tidspbridge/core/ue_deh.c
index 006ffd75289..3d28b2345fb 100644
--- a/drivers/staging/tidspbridge/core/ue_deh.c
+++ b/drivers/staging/tidspbridge/core/ue_deh.c
@@ -215,7 +215,7 @@ static inline const char *event_to_string(int event)
 	case DSP_MMUFAULT: return "DSP_MMUFAULT"; break;
 	case DSP_PWRERROR: return "DSP_PWRERROR"; break;
 	case DSP_WDTOVERFLOW: return "DSP_WDTOVERFLOW"; break;
-	default: return "unkown event"; break;
+	default: return "unknown event"; break;
 	}
 }
 
diff --git a/drivers/staging/usbip/stub_dev.c b/drivers/staging/usbip/stub_dev.c
index fa870e3f7f6..92ced35e6b7 100644
--- a/drivers/staging/usbip/stub_dev.c
+++ b/drivers/staging/usbip/stub_dev.c
@@ -113,8 +113,8 @@ static ssize_t store_sockfd(struct device *dev, struct device_attribute *attr,
 
 		spin_unlock(&sdev->ud.lock);
 
-		sdev->ud.tcp_rx = kthread_run(stub_rx_loop, &sdev->ud, "stub_rx");
-		sdev->ud.tcp_tx = kthread_run(stub_tx_loop, &sdev->ud, "stub_tx");
+		sdev->ud.tcp_rx = kthread_get_run(stub_rx_loop, &sdev->ud, "stub_rx");
+		sdev->ud.tcp_tx = kthread_get_run(stub_tx_loop, &sdev->ud, "stub_tx");
 
 		spin_lock(&sdev->ud.lock);
 		sdev->ud.status = SDEV_ST_USED;
@@ -187,10 +187,10 @@ static void stub_shutdown_connection(struct usbip_device *ud)
 	}
 
 	/* 1. stop threads */
-	if (ud->tcp_rx && !task_is_dead(ud->tcp_rx))
-		kthread_stop(ud->tcp_rx);
-	if (ud->tcp_tx && !task_is_dead(ud->tcp_tx))
-		kthread_stop(ud->tcp_tx);
+	if (ud->tcp_rx)
+		kthread_stop_put(ud->tcp_rx);
+	if (ud->tcp_tx)
+		kthread_stop_put(ud->tcp_tx);
 
 	/*
 	 * 2. close the socket
diff --git a/drivers/staging/usbip/usbip_common.h b/drivers/staging/usbip/usbip_common.h
index c7b888ca54f..5d89c0fd6f7 100644
--- a/drivers/staging/usbip/usbip_common.h
+++ b/drivers/staging/usbip/usbip_common.h
@@ -292,6 +292,23 @@ struct usbip_device {
 	} eh_ops;
 };
 
+#define kthread_get_run(threadfn, data, namefmt, ...)			   \
+({									   \
+	struct task_struct *__k						   \
+		= kthread_create(threadfn, data, namefmt, ## __VA_ARGS__); \
+	if (!IS_ERR(__k)) {						   \
+		get_task_struct(__k);					   \
+		wake_up_process(__k);					   \
+	}								   \
+	__k;								   \
+})
+
+#define kthread_stop_put(k)		\
+	do {				\
+		kthread_stop(k);	\
+		put_task_struct(k);	\
+	} while (0)
+
 /* usbip_common.c */
 void usbip_dump_urb(struct urb *purb);
 void usbip_dump_header(struct usbip_header *pdu);
diff --git a/drivers/staging/usbip/usbip_protocol.txt b/drivers/staging/usbip/usbip_protocol.txt
index 0f102081e86..16b6fe27284 100644
--- a/drivers/staging/usbip/usbip_protocol.txt
+++ b/drivers/staging/usbip/usbip_protocol.txt
@@ -27,7 +27,7 @@ Once the client knows the list of exported USB devices it may decide to use one
 of them. First the client opens a TCP/IP connection towards the server and
 sends an OP_REQ_IMPORT packet. The server replies with OP_REP_IMPORT. If the
 import was successful the TCP/IP connection remains open and will be used
-to trasfer the URB traffic between the client and the server. The client may
+to transfer the URB traffic between the client and the server. The client may
 send two types of packets: the USBIP_CMD_SUBMIT to submit an URB, and
 USBIP_CMD_UNLINK to unlink a previously submitted URB. The answers of the
 server may be USBIP_RET_SUBMIT and USBIP_RET_UNLINK respectively.
diff --git a/drivers/staging/usbip/userspace/libsrc/vhci_driver.c b/drivers/staging/usbip/userspace/libsrc/vhci_driver.c
index 269787751b2..0958ba53e94 100644
--- a/drivers/staging/usbip/userspace/libsrc/vhci_driver.c
+++ b/drivers/staging/usbip/userspace/libsrc/vhci_driver.c
@@ -59,7 +59,10 @@ static int parse_status(char *value)
 
 
 	/* skip a header line */
-	c = strchr(value, '\n') + 1;
+	c = strchr(value, '\n');
+	if (!c)
+		return -1;
+	c++;
 
 	while (*c != '\0') {
 		int port, status, speed, devid;
@@ -109,7 +112,10 @@ static int parse_status(char *value)
 
 
 		/* go to the next line */
-		c = strchr(c, '\n') + 1;
+		c = strchr(c, '\n');
+		if (!c)
+			break;
+		c++;
 	}
 
 	dbg("exit");
@@ -264,11 +270,17 @@ static int get_nports(void)
 	    attr_status->method, attr_status->value);
 
 	/* skip a header line */
-	c = strchr(attr_status->value, '\n') + 1;
+	c = strchr(attr_status->value, '\n');
+	if (!c)
+		return 0;
+	c++;
 
 	while (*c != '\0') {
 		/* go to the next line */
-		c = strchr(c, '\n') + 1;
+		c = strchr(c, '\n');
+		if (!c)
+			return nports;
+		c++;
 		nports += 1;
 	}
 
diff --git a/drivers/staging/usbip/vhci_hcd.c b/drivers/staging/usbip/vhci_hcd.c
index dca9bf11f0c..f708cbaee16 100644
--- a/drivers/staging/usbip/vhci_hcd.c
+++ b/drivers/staging/usbip/vhci_hcd.c
@@ -821,10 +821,10 @@ static void vhci_shutdown_connection(struct usbip_device *ud)
 	}
 
 	/* kill threads related to this sdev, if v.c. exists */
-	if (vdev->ud.tcp_rx && !task_is_dead(vdev->ud.tcp_rx))
-		kthread_stop(vdev->ud.tcp_rx);
-	if (vdev->ud.tcp_tx && !task_is_dead(vdev->ud.tcp_tx))
-		kthread_stop(vdev->ud.tcp_tx);
+	if (vdev->ud.tcp_rx)
+		kthread_stop_put(vdev->ud.tcp_rx);
+	if (vdev->ud.tcp_tx)
+		kthread_stop_put(vdev->ud.tcp_tx);
 
 	pr_info("stop threads\n");
 
diff --git a/drivers/staging/usbip/vhci_rx.c b/drivers/staging/usbip/vhci_rx.c
index f5fba7320c5..f0eaf04fa25 100644
--- a/drivers/staging/usbip/vhci_rx.c
+++ b/drivers/staging/usbip/vhci_rx.c
@@ -162,7 +162,7 @@ static void vhci_recv_ret_unlink(struct vhci_device *vdev,
 		 * already received the result of its submit result and gave
 		 * back the URB.
 		 */
-		pr_info("the urb (seqnum %d) was already given backed\n",
+		pr_info("the urb (seqnum %d) was already given back\n",
 			pdu->base.seqnum);
 	} else {
 		usbip_dbg_vhci_rx("now giveback urb %p\n", urb);
diff --git a/drivers/staging/usbip/vhci_sysfs.c b/drivers/staging/usbip/vhci_sysfs.c
index 0cd039bb5fd..7ce9c2f7e44 100644
--- a/drivers/staging/usbip/vhci_sysfs.c
+++ b/drivers/staging/usbip/vhci_sysfs.c
@@ -222,8 +222,8 @@ static ssize_t store_attach(struct device *dev, struct device_attribute *attr,
 	spin_unlock(&the_controller->lock);
 	/* end the lock */
 
-	vdev->ud.tcp_rx = kthread_run(vhci_rx_loop, &vdev->ud, "vhci_rx");
-	vdev->ud.tcp_tx = kthread_run(vhci_tx_loop, &vdev->ud, "vhci_tx");
+	vdev->ud.tcp_rx = kthread_get_run(vhci_rx_loop, &vdev->ud, "vhci_rx");
+	vdev->ud.tcp_tx = kthread_get_run(vhci_tx_loop, &vdev->ud, "vhci_tx");
 
 	rh_port_connect(rhport, speed);
 
diff --git a/drivers/staging/vme/Makefile b/drivers/staging/vme/Makefile
index b4ea3f8d0a5..accdb72e39e 100644
--- a/drivers/staging/vme/Makefile
+++ b/drivers/staging/vme/Makefile
@@ -1,8 +1 @@
-#
-# Makefile for the VME bridge device drivers.
-#
-obj-$(CONFIG_VME_BUS)		+= vme.o
-
-obj-y				+= bridges/
 obj-y				+= devices/
-obj-y				+= boards/
diff --git a/drivers/staging/vme/TODO b/drivers/staging/vme/TODO
deleted file mode 100644
index 79f00333e7e..00000000000
--- a/drivers/staging/vme/TODO
+++ /dev/null
@@ -1,5 +0,0 @@
-				TODO
-				====
-
-- Add one or more device drivers which use the VME framework.
-
diff --git a/drivers/staging/vme/devices/Kconfig b/drivers/staging/vme/devices/Kconfig
index 55ec30cb1fa..d0cab176619 100644
--- a/drivers/staging/vme/devices/Kconfig
+++ b/drivers/staging/vme/devices/Kconfig
@@ -2,6 +2,7 @@ comment "VME Device Drivers"
 
 config VME_USER
 	tristate "VME user space access driver"
+	depends on STAGING
 	help
 	  If you say Y here you want to be able to access a limited number of
 	  VME windows in a manner at least semi-compatible with the interface
@@ -9,7 +10,7 @@ config VME_USER
 
 config VME_PIO2
 	tristate "GE PIO2 VME"
-	depends on GPIOLIB
+	depends on STAGING && GPIOLIB
 	help
 	  Say Y here to include support for the GE PIO2. The PIO2 is a 6U VME
 	  slave card, implementing 32 solid-state relay switched IO lines, in
diff --git a/drivers/staging/vme/devices/vme_pio2_cntr.c b/drivers/staging/vme/devices/vme_pio2_cntr.c
index 08e0d59806c..6335471faa3 100644
--- a/drivers/staging/vme/devices/vme_pio2_cntr.c
+++ b/drivers/staging/vme/devices/vme_pio2_cntr.c
@@ -17,8 +17,8 @@
 #include <linux/device.h>
 #include <linux/types.h>
 #include <linux/gpio.h>
+#include <linux/vme.h>
 
-#include "../vme.h"
 #include "vme_pio2.h"
 
 static int pio2_cntr_irq_set(struct pio2_card *card, int id)
diff --git a/drivers/staging/vme/devices/vme_pio2_core.c b/drivers/staging/vme/devices/vme_pio2_core.c
index 573c80003f0..4bf8e05ac31 100644
--- a/drivers/staging/vme/devices/vme_pio2_core.c
+++ b/drivers/staging/vme/devices/vme_pio2_core.c
@@ -10,7 +10,6 @@
  * option) any later version.
  */
 
-#include <linux/version.h>
 #include <linux/module.h>
 #include <linux/moduleparam.h>
 #include <linux/types.h>
@@ -20,8 +19,8 @@
 #include <linux/ctype.h>
 #include <linux/gpio.h>
 #include <linux/slab.h>
+#include <linux/vme.h>
 
-#include "../vme.h"
 #include "vme_pio2.h"
 
 
diff --git a/drivers/staging/vme/devices/vme_pio2_gpio.c b/drivers/staging/vme/devices/vme_pio2_gpio.c
index 858484915f0..ad76a477825 100644
--- a/drivers/staging/vme/devices/vme_pio2_gpio.c
+++ b/drivers/staging/vme/devices/vme_pio2_gpio.c
@@ -10,7 +10,6 @@
  * option) any later version.
  */
 
-#include <linux/version.h>
 #include <linux/module.h>
 #include <linux/moduleparam.h>
 #include <linux/types.h>
@@ -21,8 +20,8 @@
 #include <linux/ctype.h>
 #include <linux/gpio.h>
 #include <linux/slab.h>
+#include <linux/vme.h>
 
-#include "../vme.h"
 #include "vme_pio2.h"
 
 static const char driver_name[] = "pio2_gpio";
@@ -79,7 +78,7 @@ static void pio2_gpio_set(struct gpio_chip *chip, unsigned int offset,
 	if ((card->bank[PIO2_CHANNEL_BANK[offset]].config == INPUT) |
 		(card->bank[PIO2_CHANNEL_BANK[offset]].config == NOFIT)) {
 
-		dev_err(&card->vdev->dev, "Channel not availabe as output\n");
+		dev_err(&card->vdev->dev, "Channel not available as output\n");
 		return;
 	}
 
diff --git a/drivers/staging/vme/devices/vme_user.c b/drivers/staging/vme/devices/vme_user.c
index 7dcd1622b5f..e24a6f95db1 100644
--- a/drivers/staging/vme/devices/vme_user.c
+++ b/drivers/staging/vme/devices/vme_user.c
@@ -27,7 +27,7 @@
 #include <linux/module.h>
 #include <linux/pagemap.h>
 #include <linux/pci.h>
-#include <linux/semaphore.h>
+#include <linux/mutex.h>
 #include <linux/slab.h>
 #include <linux/spinlock.h>
 #include <linux/syscalls.h>
@@ -36,8 +36,8 @@
 
 #include <linux/io.h>
 #include <linux/uaccess.h>
+#include <linux/vme.h>
 
-#include "../vme.h"
 #include "vme_user.h"
 
 static DEFINE_MUTEX(vme_user_mutex);
@@ -95,7 +95,7 @@ struct image_desc {
 	void *kern_buf;	/* Buffer address in kernel space */
 	dma_addr_t pci_buf;	/* Buffer address in PCI address space */
 	unsigned long long size_buf;	/* Buffer size */
-	struct semaphore sem;	/* Semaphore for locking image */
+	struct mutex mutex;	/* Mutex for locking image */
 	struct device *device;	/* Sysfs device */
 	struct vme_resource *resource;	/* VME resource */
 	int users;		/* Number of current users */
@@ -168,7 +168,7 @@ static int vme_user_open(struct inode *inode, struct file *file)
 	int err;
 	unsigned int minor = MINOR(inode->i_rdev);
 
-	down(&image[minor].sem);
+	mutex_lock(&image[minor].mutex);
 	/* Allow device to be opened if a resource is needed and allocated. */
 	if (minor < CONTROL_MINOR && image[minor].resource == NULL) {
 		printk(KERN_ERR "No resources allocated for device\n");
@@ -179,12 +179,12 @@ static int vme_user_open(struct inode *inode, struct file *file)
 	/* Increment user count */
 	image[minor].users++;
 
-	up(&image[minor].sem);
+	mutex_unlock(&image[minor].mutex);
 
 	return 0;
 
 err_res:
-	up(&image[minor].sem);
+	mutex_unlock(&image[minor].mutex);
 
 	return err;
 }
@@ -193,12 +193,12 @@ static int vme_user_release(struct inode *inode, struct file *file)
 {
 	unsigned int minor = MINOR(inode->i_rdev);
 
-	down(&image[minor].sem);
+	mutex_lock(&image[minor].mutex);
 
 	/* Decrement user count */
 	image[minor].users--;
 
-	up(&image[minor].sem);
+	mutex_unlock(&image[minor].mutex);
 
 	return 0;
 }
@@ -325,14 +325,14 @@ static ssize_t vme_user_read(struct file *file, char __user *buf, size_t count,
 	if (minor == CONTROL_MINOR)
 		return 0;
 
-	down(&image[minor].sem);
+	mutex_lock(&image[minor].mutex);
 
 	/* XXX Do we *really* want this helper - we can use vme_*_get ? */
 	image_size = vme_get_size(image[minor].resource);
 
 	/* Ensure we are starting at a valid location */
 	if ((*ppos < 0) || (*ppos > (image_size - 1))) {
-		up(&image[minor].sem);
+		mutex_unlock(&image[minor].mutex);
 		return 0;
 	}
 
@@ -353,8 +353,7 @@ static ssize_t vme_user_read(struct file *file, char __user *buf, size_t count,
 		retval = -EINVAL;
 	}
 
-	up(&image[minor].sem);
-
+	mutex_unlock(&image[minor].mutex);
 	if (retval > 0)
 		*ppos += retval;
 
@@ -372,13 +371,13 @@ static ssize_t vme_user_write(struct file *file, const char __user *buf,
 	if (minor == CONTROL_MINOR)
 		return 0;
 
-	down(&image[minor].sem);
+	mutex_lock(&image[minor].mutex);
 
 	image_size = vme_get_size(image[minor].resource);
 
 	/* Ensure we are starting at a valid location */
 	if ((*ppos < 0) || (*ppos > (image_size - 1))) {
-		up(&image[minor].sem);
+		mutex_unlock(&image[minor].mutex);
 		return 0;
 	}
 
@@ -398,8 +397,8 @@ static ssize_t vme_user_write(struct file *file, const char __user *buf,
 	default:
 		retval = -EINVAL;
 	}
-
-	up(&image[minor].sem);
+	
+	mutex_unlock(&image[minor].mutex);
 
 	if (retval > 0)
 		*ppos += retval;
@@ -416,7 +415,7 @@ static loff_t vme_user_llseek(struct file *file, loff_t off, int whence)
 	if (minor == CONTROL_MINOR)
 		return -EINVAL;
 
-	down(&image[minor].sem);
+	mutex_lock(&image[minor].mutex);
 	image_size = vme_get_size(image[minor].resource);
 
 	switch (whence) {
@@ -430,19 +429,19 @@ static loff_t vme_user_llseek(struct file *file, loff_t off, int whence)
 		absolute = image_size + off;
 		break;
 	default:
-		up(&image[minor].sem);
+		mutex_unlock(&image[minor].mutex);
 		return -EINVAL;
 		break;
 	}
 
 	if ((absolute < 0) || (absolute >= image_size)) {
-		up(&image[minor].sem);
+		mutex_unlock(&image[minor].mutex);
 		return -EINVAL;
 	}
 
 	file->f_pos = absolute;
 
-	up(&image[minor].sem);
+	mutex_unlock(&image[minor].mutex);
 
 	return absolute;
 }
@@ -696,7 +695,7 @@ static int __devinit vme_user_probe(struct vme_dev *vdev)
 	for (i = 0; i < VME_DEVS; i++) {
 		image[i].kern_buf = NULL;
 		image[i].pci_buf = 0;
-		sema_init(&image[i].sem, 1);
+		mutex_init(&image[i].mutex);
 		image[i].device = NULL;
 		image[i].resource = NULL;
 		image[i].users = 0;
@@ -858,8 +857,10 @@ static int __devexit vme_user_remove(struct vme_dev *dev)
 	int i;
 
 	/* Remove sysfs Entries */
-	for (i = 0; i < VME_DEVS; i++)
+	for (i = 0; i < VME_DEVS; i++) {
+		mutex_destroy(&image[i].mutex);
 		device_destroy(vme_user_sysfs_class, MKDEV(VME_MAJOR, i));
+	}
 	class_destroy(vme_user_sysfs_class);
 
 	for (i = MASTER_MINOR; i < (MASTER_MAX + 1); i++) {
diff --git a/drivers/staging/vme/vme.h b/drivers/staging/vme/vme.h
deleted file mode 100644
index c9d65bf14ce..00000000000
--- a/drivers/staging/vme/vme.h
+++ /dev/null
@@ -1,174 +0,0 @@
-#ifndef _VME_H_
-#define _VME_H_
-
-/* Resource Type */
-enum vme_resource_type {
-	VME_MASTER,
-	VME_SLAVE,
-	VME_DMA,
-	VME_LM
-};
-
-/* VME Address Spaces */
-#define VME_A16		0x1
-#define VME_A24		0x2
-#define	VME_A32		0x4
-#define VME_A64		0x8
-#define VME_CRCSR	0x10
-#define VME_USER1	0x20
-#define VME_USER2	0x40
-#define VME_USER3	0x80
-#define VME_USER4	0x100
-
-#define VME_A16_MAX	0x10000ULL
-#define VME_A24_MAX	0x1000000ULL
-#define VME_A32_MAX	0x100000000ULL
-#define VME_A64_MAX	0x10000000000000000ULL
-#define VME_CRCSR_MAX	0x1000000ULL
-
-
-/* VME Cycle Types */
-#define VME_SCT		0x1
-#define VME_BLT		0x2
-#define VME_MBLT	0x4
-#define VME_2eVME	0x8
-#define VME_2eSST	0x10
-#define VME_2eSSTB	0x20
-
-#define VME_2eSST160	0x100
-#define VME_2eSST267	0x200
-#define VME_2eSST320	0x400
-
-#define	VME_SUPER	0x1000
-#define	VME_USER	0x2000
-#define	VME_PROG	0x4000
-#define	VME_DATA	0x8000
-
-/* VME Data Widths */
-#define VME_D8		0x1
-#define VME_D16		0x2
-#define VME_D32		0x4
-#define VME_D64		0x8
-
-/* Arbitration Scheduling Modes */
-#define VME_R_ROBIN_MODE	0x1
-#define VME_PRIORITY_MODE	0x2
-
-#define VME_DMA_PATTERN			(1<<0)
-#define VME_DMA_PCI			(1<<1)
-#define VME_DMA_VME			(1<<2)
-
-#define VME_DMA_PATTERN_BYTE		(1<<0)
-#define VME_DMA_PATTERN_WORD		(1<<1)
-#define VME_DMA_PATTERN_INCREMENT	(1<<2)
-
-#define VME_DMA_VME_TO_MEM		(1<<0)
-#define VME_DMA_MEM_TO_VME		(1<<1)
-#define VME_DMA_VME_TO_VME		(1<<2)
-#define VME_DMA_MEM_TO_MEM		(1<<3)
-#define VME_DMA_PATTERN_TO_VME		(1<<4)
-#define VME_DMA_PATTERN_TO_MEM		(1<<5)
-
-struct vme_dma_attr {
-	u32 type;
-	void *private;
-};
-
-struct vme_resource {
-	enum vme_resource_type type;
-	struct list_head *entry;
-};
-
-extern struct bus_type vme_bus_type;
-
-/* VME_MAX_BRIDGES comes from the type of vme_bus_numbers */
-#define VME_MAX_BRIDGES		(sizeof(unsigned int)*8)
-#define VME_MAX_SLOTS		32
-
-#define VME_SLOT_CURRENT	-1
-#define VME_SLOT_ALL		-2
-
-/**
- * Structure representing a VME device
- * @num: The device number
- * @bridge: Pointer to the bridge device this device is on
- * @dev: Internal device structure
- * @drv_list: List of devices (per driver)
- * @bridge_list: List of devices (per bridge)
- */
-struct vme_dev {
-	int num;
-	struct vme_bridge *bridge;
-	struct device dev;
-	struct list_head drv_list;
-	struct list_head bridge_list;
-};
-
-struct vme_driver {
-	struct list_head node;
-	const char *name;
-	int (*match)(struct vme_dev *);
-	int (*probe)(struct vme_dev *);
-	int (*remove)(struct vme_dev *);
-	void (*shutdown)(void);
-	struct device_driver driver;
-	struct list_head devices;
-};
-
-void *vme_alloc_consistent(struct vme_resource *, size_t, dma_addr_t *);
-void vme_free_consistent(struct vme_resource *, size_t,  void *,
-	dma_addr_t);
-
-size_t vme_get_size(struct vme_resource *);
-
-struct vme_resource *vme_slave_request(struct vme_dev *, u32, u32);
-int vme_slave_set(struct vme_resource *, int, unsigned long long,
-	unsigned long long, dma_addr_t, u32, u32);
-int vme_slave_get(struct vme_resource *, int *, unsigned long long *,
-	unsigned long long *, dma_addr_t *, u32 *, u32 *);
-void vme_slave_free(struct vme_resource *);
-
-struct vme_resource *vme_master_request(struct vme_dev *, u32, u32, u32);
-int vme_master_set(struct vme_resource *, int, unsigned long long,
-	unsigned long long, u32, u32, u32);
-int vme_master_get(struct vme_resource *, int *, unsigned long long *,
-	unsigned long long *, u32 *, u32 *, u32 *);
-ssize_t vme_master_read(struct vme_resource *, void *, size_t, loff_t);
-ssize_t vme_master_write(struct vme_resource *, void *, size_t, loff_t);
-unsigned int vme_master_rmw(struct vme_resource *, unsigned int, unsigned int,
-	unsigned int, loff_t);
-void vme_master_free(struct vme_resource *);
-
-struct vme_resource *vme_dma_request(struct vme_dev *, u32);
-struct vme_dma_list *vme_new_dma_list(struct vme_resource *);
-struct vme_dma_attr *vme_dma_pattern_attribute(u32, u32);
-struct vme_dma_attr *vme_dma_pci_attribute(dma_addr_t);
-struct vme_dma_attr *vme_dma_vme_attribute(unsigned long long, u32, u32, u32);
-void vme_dma_free_attribute(struct vme_dma_attr *);
-int vme_dma_list_add(struct vme_dma_list *, struct vme_dma_attr *,
-	struct vme_dma_attr *, size_t);
-int vme_dma_list_exec(struct vme_dma_list *);
-int vme_dma_list_free(struct vme_dma_list *);
-int vme_dma_free(struct vme_resource *);
-
-int vme_irq_request(struct vme_dev *, int, int,
-	void (*callback)(int, int, void *), void *);
-void vme_irq_free(struct vme_dev *, int, int);
-int vme_irq_generate(struct vme_dev *, int, int);
-
-struct vme_resource *vme_lm_request(struct vme_dev *);
-int vme_lm_count(struct vme_resource *);
-int vme_lm_set(struct vme_resource *, unsigned long long, u32, u32);
-int vme_lm_get(struct vme_resource *, unsigned long long *, u32 *, u32 *);
-int vme_lm_attach(struct vme_resource *, int, void (*callback)(int));
-int vme_lm_detach(struct vme_resource *, int);
-void vme_lm_free(struct vme_resource *);
-
-int vme_slot_get(struct vme_dev *);
-
-int vme_register_driver(struct vme_driver *, unsigned int);
-void vme_unregister_driver(struct vme_driver *);
-
-
-#endif /* _VME_H_ */
-
diff --git a/drivers/staging/vme/vme_api.txt b/drivers/staging/vme/vme_api.txt
deleted file mode 100644
index 856efa35f6e..00000000000
--- a/drivers/staging/vme/vme_api.txt
+++ /dev/null
@@ -1,396 +0,0 @@
-			VME Device Driver API
-			=====================
-
-Driver registration
-===================
-
-As with other subsystems within the Linux kernel, VME device drivers register
-with the VME subsystem, typically called from the devices init routine.  This is
-achieved via a call to the following function:
-
-	int vme_register_driver (struct vme_driver *driver);
-
-If driver registration is successful this function returns zero, if an error
-occurred a negative error code will be returned.
-
-A pointer to a structure of type 'vme_driver' must be provided to the
-registration function. The structure is as follows:
-
-	struct vme_driver {
-		struct list_head node;
-		const char *name;
-		int (*match)(struct vme_dev *);
-		int (*probe)(struct vme_dev *);
-		int (*remove)(struct vme_dev *);
-		void (*shutdown)(void);
-		struct device_driver driver;
-		struct list_head devices;
-		unsigned int ndev;
-	};
-
-At the minimum, the '.name', '.match' and '.probe' elements of this structure
-should be correctly set. The '.name' element is a pointer to a string holding
-the device driver's name.
-
-The '.match' function allows controlling the number of devices that need to
-be registered. The match function should return 1 if a device should be
-probed and 0 otherwise. This example match function (from vme_user.c) limits
-the number of devices probed to one:
-
-	#define USER_BUS_MAX	1
-	...
-	static int vme_user_match(struct vme_dev *vdev)
-	{
-		if (vdev->id.num >= USER_BUS_MAX)
-			return 0;
-		return 1;
-	}
-
-The '.probe' element should contain a pointer to the probe routine. The
-probe routine is passed a 'struct vme_dev' pointer as an argument. The
-'struct vme_dev' structure looks like the following:
-
-	struct vme_dev {
-		int num;
-		struct vme_bridge *bridge;
-		struct device dev;
-		struct list_head drv_list;
-		struct list_head bridge_list;
-	};
-
-Here, the 'num' field refers to the sequential device ID for this specific
-driver. The bridge number (or bus number) can be accessed using
-dev->bridge->num.
-
-A function is also provided to unregister the driver from the VME core and is
-usually called from the device driver's exit routine:
-
-	void vme_unregister_driver (struct vme_driver *driver);
-
-
-Resource management
-===================
-
-Once a driver has registered with the VME core the provided match routine will
-be called the number of times specified during the registration. If a match
-succeeds, a non-zero value should be returned. A zero return value indicates
-failure. For all successful matches, the probe routine of the corresponding
-driver is called. The probe routine is passed a pointer to the devices
-device structure. This pointer should be saved, it will be required for
-requesting VME resources.
-
-The driver can request ownership of one or more master windows, slave windows
-and/or dma channels. Rather than allowing the device driver to request a
-specific window or DMA channel (which may be used by a different driver) this
-driver allows a resource to be assigned based on the required attributes of the
-driver in question:
-
-	struct vme_resource * vme_master_request(struct vme_dev *dev,
-		u32 aspace, u32 cycle, u32 width);
-
-	struct vme_resource * vme_slave_request(struct vme_dev *dev, u32 aspace,
-		u32 cycle);
-
-	struct vme_resource *vme_dma_request(struct vme_dev *dev, u32 route);
-
-For slave windows these attributes are split into the VME address spaces that
-need to be accessed in 'aspace' and VME bus cycle types required in 'cycle'.
-Master windows add a further set of attributes in 'width' specifying the
-required data transfer widths. These attributes are defined as bitmasks and as
-such any combination of the attributes can be requested for a single window,
-the core will assign a window that meets the requirements, returning a pointer
-of type vme_resource that should be used to identify the allocated resource
-when it is used. For DMA controllers, the request function requires the
-potential direction of any transfers to be provided in the route attributes.
-This is typically VME-to-MEM and/or MEM-to-VME, though some hardware can
-support VME-to-VME and MEM-to-MEM transfers as well as test pattern generation.
-If an unallocated window fitting the requirements can not be found a NULL
-pointer will be returned.
-
-Functions are also provided to free window allocations once they are no longer
-required. These functions should be passed the pointer to the resource provided
-during resource allocation:
-
-	void vme_master_free(struct vme_resource *res);
-
-	void vme_slave_free(struct vme_resource *res);
-
-	void vme_dma_free(struct vme_resource *res);
-
-
-Master windows
-==============
-
-Master windows provide access from the local processor[s] out onto the VME bus.
-The number of windows available and the available access modes is dependent on
-the underlying chipset. A window must be configured before it can be used.
-
-
-Master window configuration
----------------------------
-
-Once a master window has been assigned the following functions can be used to
-configure it and retrieve the current settings:
-
-	int vme_master_set (struct vme_resource *res, int enabled,
-		unsigned long long base, unsigned long long size, u32 aspace,
-		u32 cycle, u32 width);
-
-	int vme_master_get (struct vme_resource *res, int *enabled,
-		unsigned long long *base, unsigned long long *size, u32 *aspace,
-		u32 *cycle, u32 *width);
-
-The address spaces, transfer widths and cycle types are the same as described
-under resource management, however some of the options are mutually exclusive.
-For example, only one address space may be specified.
-
-These functions return 0 on success or an error code should the call fail.
-
-
-Master window access
---------------------
-
-The following functions can be used to read from and write to configured master
-windows. These functions return the number of bytes copied:
-
-	ssize_t vme_master_read(struct vme_resource *res, void *buf,
-		size_t count, loff_t offset);
-
-	ssize_t vme_master_write(struct vme_resource *res, void *buf,
-		size_t count, loff_t offset);
-
-In addition to simple reads and writes, a function is provided to do a
-read-modify-write transaction. This function returns the original value of the
-VME bus location :
-
-	unsigned int vme_master_rmw (struct vme_resource *res,
-		unsigned int mask, unsigned int compare, unsigned int swap,
-		loff_t offset);
-
-This functions by reading the offset, applying the mask. If the bits selected in
-the mask match with the values of the corresponding bits in the compare field,
-the value of swap is written the specified offset.
-
-
-Slave windows
-=============
-
-Slave windows provide devices on the VME bus access into mapped portions of the
-local memory. The number of windows available and the access modes that can be
-used is dependent on the underlying chipset. A window must be configured before
-it can be used.
-
-
-Slave window configuration
---------------------------
-
-Once a slave window has been assigned the following functions can be used to
-configure it and retrieve the current settings:
-
-	int vme_slave_set (struct vme_resource *res, int enabled,
-		unsigned long long base, unsigned long long size,
-		dma_addr_t mem, u32 aspace, u32 cycle);
-
-	int vme_slave_get (struct vme_resource *res, int *enabled,
-		unsigned long long *base, unsigned long long *size,
-		dma_addr_t *mem, u32 *aspace, u32 *cycle);
-
-The address spaces, transfer widths and cycle types are the same as described
-under resource management, however some of the options are mutually exclusive.
-For example, only one address space may be specified.
-
-These functions return 0 on success or an error code should the call fail.
-
-
-Slave window buffer allocation
-------------------------------
-
-Functions are provided to allow the user to allocate and free a contiguous
-buffers which will be accessible by the VME bridge. These functions do not have
-to be used, other methods can be used to allocate a buffer, though care must be
-taken to ensure that they are contiguous and accessible by the VME bridge:
-
-	void * vme_alloc_consistent(struct vme_resource *res, size_t size,
-		dma_addr_t *mem);
-
-	void vme_free_consistent(struct vme_resource *res, size_t size,
-		void *virt,	dma_addr_t mem);
-
-
-Slave window access
--------------------
-
-Slave windows map local memory onto the VME bus, the standard methods for
-accessing memory should be used.
-
-
-DMA channels
-============
-
-The VME DMA transfer provides the ability to run link-list DMA transfers. The
-API introduces the concept of DMA lists. Each DMA list is a link-list which can
-be passed to a DMA controller. Multiple lists can be created, extended,
-executed, reused and destroyed.
-
-
-List Management
----------------
-
-The following functions are provided to create and destroy DMA lists. Execution
-of a list will not automatically destroy the list, thus enabling a list to be
-reused for repetitive tasks:
-
-	struct vme_dma_list *vme_new_dma_list(struct vme_resource *res);
-
-	int vme_dma_list_free(struct vme_dma_list *list);
-
-
-List Population
----------------
-
-An item can be added to a list using the following function ( the source and
-destination attributes need to be created before calling this function, this is
-covered under "Transfer Attributes"):
-
-	int vme_dma_list_add(struct vme_dma_list *list,
-		struct vme_dma_attr *src, struct vme_dma_attr *dest,
-		size_t count);
-
-NOTE:	The detailed attributes of the transfers source and destination
-	are not checked until an entry is added to a DMA list, the request
-	for a DMA channel purely checks the directions in which the
-	controller is expected to transfer data. As a result it is
-	possible for this call to return an error, for example if the
-	source or destination is in an unsupported VME address space.
-
-Transfer Attributes
--------------------
-
-The attributes for the source and destination are handled separately from adding
-an item to a list. This is due to the diverse attributes required for each type
-of source and destination. There are functions to create attributes for PCI, VME
-and pattern sources and destinations (where appropriate):
-
-Pattern source:
-
-	struct vme_dma_attr *vme_dma_pattern_attribute(u32 pattern, u32 type);
-
-PCI source or destination:
-
-	struct vme_dma_attr *vme_dma_pci_attribute(dma_addr_t mem);
-
-VME source or destination:
-
-	struct vme_dma_attr *vme_dma_vme_attribute(unsigned long long base,
-		u32 aspace, u32 cycle, u32 width);
-
-The following function should be used to free an attribute:
-
-	void vme_dma_free_attribute(struct vme_dma_attr *attr);
-
-
-List Execution
---------------
-
-The following function queues a list for execution. The function will return
-once the list has been executed:
-
-	int vme_dma_list_exec(struct vme_dma_list *list);
-
-
-Interrupts
-==========
-
-The VME API provides functions to attach and detach callbacks to specific VME
-level and status ID combinations and for the generation of VME interrupts with
-specific VME level and status IDs.
-
-
-Attaching Interrupt Handlers
-----------------------------
-
-The following functions can be used to attach and free a specific VME level and
-status ID combination. Any given combination can only be assigned a single
-callback function. A void pointer parameter is provided, the value of which is
-passed to the callback function, the use of this pointer is user undefined:
-
-	int vme_irq_request(struct vme_dev *dev, int level, int statid,
-		void (*callback)(int, int, void *), void *priv);
-
-	void vme_irq_free(struct vme_dev *dev, int level, int statid);
-
-The callback parameters are as follows. Care must be taken in writing a callback
-function, callback functions run in interrupt context:
-
-	void callback(int level, int statid, void *priv);
-
-
-Interrupt Generation
---------------------
-
-The following function can be used to generate a VME interrupt at a given VME
-level and VME status ID:
-
-	int vme_irq_generate(struct vme_dev *dev, int level, int statid);
-
-
-Location monitors
-=================
-
-The VME API provides the following functionality to configure the location
-monitor.
-
-
-Location Monitor Management
----------------------------
-
-The following functions are provided to request the use of a block of location
-monitors and to free them after they are no longer required:
-
-	struct vme_resource * vme_lm_request(struct vme_dev *dev);
-
-	void vme_lm_free(struct vme_resource * res);
-
-Each block may provide a number of location monitors, monitoring adjacent
-locations. The following function can be used to determine how many locations
-are provided:
-
-	int vme_lm_count(struct vme_resource * res);
-
-
-Location Monitor Configuration
-------------------------------
-
-Once a bank of location monitors has been allocated, the following functions
-are provided to configure the location and mode of the location monitor:
-
-	int vme_lm_set(struct vme_resource *res, unsigned long long base,
-		u32 aspace, u32 cycle);
-
-	int vme_lm_get(struct vme_resource *res, unsigned long long *base,
-		u32 *aspace, u32 *cycle);
-
-
-Location Monitor Use
---------------------
-
-The following functions allow a callback to be attached and detached from each
-location monitor location. Each location monitor can monitor a number of
-adjacent locations:
-
-	int vme_lm_attach(struct vme_resource *res, int num,
-		void (*callback)(int));
-
-	int vme_lm_detach(struct vme_resource *res, int num);
-
-The callback function is declared as follows.
-
-	void callback(int num);
-
-
-Slot Detection
-==============
-
-This function returns the slot ID of the provided bridge.
-
-	int vme_slot_get(struct vme_dev *dev);
diff --git a/drivers/staging/vt6655/wpa.c b/drivers/staging/vt6655/wpa.c
index 61ac46fa505..0afb9fe0379 100644
--- a/drivers/staging/vt6655/wpa.c
+++ b/drivers/staging/vt6655/wpa.c
@@ -148,7 +148,7 @@ WPA_ParseRSN (
         {
             j = 0;
             DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"wPKCount: %d, sizeof(pBSSList->abyPKType): %zu\n", pRSN->wPKCount, sizeof(pBSSList->abyPKType));
-            for(i = 0; (i < pRSN->wPKCount) && (j < sizeof(pBSSList->abyPKType)/sizeof(unsigned char)); i++) {
+            for(i = 0; (i < pRSN->wPKCount) && (j < ARRAY_SIZE(pBSSList->abyPKType)); i++) {
                 if(pRSN->len >= 12+i*4+4) { //oui1(4)+ver(2)+GKS(4)+PKSCnt(2)+PKS(4*i)
                     if ( !memcmp(pRSN->PKSList[i].abyOUI, abyOUI00, 4))
                         pBSSList->abyPKType[j++] = WPA_NONE;
@@ -180,7 +180,7 @@ WPA_ParseRSN (
             j = 0;
             DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"wAuthCount: %d, sizeof(pBSSList->abyAuthType): %zu\n",
                           pIE_RSN_Auth->wAuthCount, sizeof(pBSSList->abyAuthType));
-            for(i = 0; (i < pIE_RSN_Auth->wAuthCount) && (j < sizeof(pBSSList->abyAuthType)/sizeof(unsigned char)); i++) {
+            for(i = 0; (i < pIE_RSN_Auth->wAuthCount) && (j < ARRAY_SIZE(pBSSList->abyAuthType)); i++) {
                 if(pRSN->len >= 14+4+(m+i)*4) { //oui1(4)+ver(2)+GKS(4)+PKSCnt(2)+PKS(4*m)+AKC(2)+AKS(4*i)
                     if ( !memcmp(pIE_RSN_Auth->AuthKSList[i].abyOUI, abyOUI01, 4))
                         pBSSList->abyAuthType[j++] = WPA_AUTH_IEEE802_1X;
diff --git a/drivers/staging/vt6656/dpc.c b/drivers/staging/vt6656/dpc.c
index c0edf97535d..e4bdf2a2b58 100644
--- a/drivers/staging/vt6656/dpc.c
+++ b/drivers/staging/vt6656/dpc.c
@@ -452,7 +452,7 @@ RXbBulkInProcessData (
        }
     }
 
-    if (!is_multicast_ether_addr(pMACHeader->abyAddr1) && !is_broadcast_ether_addr(pMACHeader->abyAddr1)) {
+    if (!is_multicast_ether_addr(pMACHeader->abyAddr1)) {
         if ( WCTLbIsDuplicate(&(pDevice->sDupRxCache), (PS802_11Header) pbyFrame) ) {
             pDevice->s802_11Counter.FrameDuplicateCount++;
             return FALSE;
diff --git a/drivers/staging/vt6656/ioctl.c b/drivers/staging/vt6656/ioctl.c
index d59456c29df..5b9a84f9518 100644
--- a/drivers/staging/vt6656/ioctl.c
+++ b/drivers/staging/vt6656/ioctl.c
@@ -90,18 +90,17 @@ int private_ioctl(PSDevice pDevice, struct ifreq *rq)
 		spin_lock_irq(&pDevice->lock);
 
 		if (memcmp(pMgmt->abyCurrBSSID, &abyNullAddr[0], 6) == 0)
-			BSSvClearBSSList((void *)pDevice, FALSE);
+			BSSvClearBSSList(pDevice, FALSE);
 		else
-			BSSvClearBSSList((void *)pDevice, pDevice->bLinkPass);
+			BSSvClearBSSList(pDevice, pDevice->bLinkPass);
 
 		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "WLAN_CMD_BSS_SCAN..begin\n");
 
 		if (pItemSSID->len != 0)
-			bScheduleCommand((void *)pDevice,
-					 WLAN_CMD_BSSID_SCAN,
+			bScheduleCommand(pDevice, WLAN_CMD_BSSID_SCAN,
 					 abyScanSSID);
 		else
-			bScheduleCommand((void *) pDevice, WLAN_CMD_BSSID_SCAN, NULL);
+			bScheduleCommand(pDevice, WLAN_CMD_BSSID_SCAN, NULL);
 
 		spin_unlock_irq(&pDevice->lock);
 		break;
@@ -150,6 +149,7 @@ int private_ioctl(PSDevice pDevice, struct ifreq *rq)
 			}
 		}
 		break;
+
 	case WLAN_CMD_BSS_JOIN:
 		if (copy_from_user(&sJoinCmd, pReq->data, sizeof(SCmdBSSJoin))) {
 			result = -EFAULT;
@@ -190,10 +190,9 @@ int private_ioctl(PSDevice pDevice, struct ifreq *rq)
 		netif_stop_queue(pDevice->dev);
 		spin_lock_irq(&pDevice->lock);
 		pMgmt->eCurrState = WMAC_STATE_IDLE;
-		bScheduleCommand((void *) pDevice,
-				 WLAN_CMD_BSSID_SCAN,
+		bScheduleCommand(pDevice, WLAN_CMD_BSSID_SCAN,
 				 pMgmt->abyDesireSSID);
-		bScheduleCommand((void *) pDevice, WLAN_CMD_SSID, NULL);
+		bScheduleCommand(pDevice, WLAN_CMD_SSID, NULL);
 		spin_unlock_irq(&pDevice->lock);
 		break;
 
@@ -299,7 +298,7 @@ int private_ioctl(PSDevice pDevice, struct ifreq *rq)
 			result = -EINVAL;
 			break;
 		}
-		pList = (PSBSSIDList)kmalloc(sizeof(SBSSIDList) + (sList.uItem * sizeof(SBSSIDItem)), (int)GFP_ATOMIC);
+		pList = kmalloc(sizeof(SBSSIDList) + (sList.uItem * sizeof(SBSSIDItem)), GFP_ATOMIC);
 		if (pList == NULL) {
 			result = -ENOMEM;
 			break;
@@ -313,7 +312,7 @@ int private_ioctl(PSDevice pDevice, struct ifreq *rq)
 				pList->sBSSIDList[ii].wBeaconInterval = pBSS->wBeaconInterval;
 				pList->sBSSIDList[ii].wCapInfo = pBSS->wCapInfo;
 				RFvRSSITodBm(pDevice, (BYTE)(pBSS->uRSSI), &ldBm);
-				pList->sBSSIDList[ii].uRSSI = (unsigned int) ldBm;
+				pList->sBSSIDList[ii].uRSSI = (unsigned int)ldBm;
 				/* pList->sBSSIDList[ii].uRSSI = pBSS->uRSSI; */
 				memcpy(pList->sBSSIDList[ii].abyBSSID, pBSS->abyBSSID, WLAN_BSSID_LEN);
 				pItemSSID = (PWLAN_IE_SSID)pBSS->abySSID;
@@ -356,6 +355,7 @@ int private_ioctl(PSDevice pDevice, struct ifreq *rq)
 			break;
 		}
 		break;
+
 	case WLAN_CMD_STOP_MAC:
 		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "WLAN_CMD_STOP_MAC\n");
 		/* Todo xxxxxx */
@@ -534,7 +534,7 @@ int private_ioctl(PSDevice pDevice, struct ifreq *rq)
 
 		netif_stop_queue(pDevice->dev);
 		spin_lock_irq(&pDevice->lock);
-		bScheduleCommand((void *)pDevice, WLAN_CMD_RUN_AP, NULL);
+		bScheduleCommand(pDevice, WLAN_CMD_RUN_AP, NULL);
 		spin_unlock_irq(&pDevice->lock);
 		break;
 
@@ -565,7 +565,7 @@ int private_ioctl(PSDevice pDevice, struct ifreq *rq)
 			result = -ENOMEM;
 			break;
 		}
-		pNodeList = kmalloc(sizeof(SNodeList) + (sNodeList.uItem * sizeof(SNodeItem)), (int)GFP_ATOMIC);
+		pNodeList = kmalloc(sizeof(SNodeList) + (sNodeList.uItem * sizeof(SNodeItem)), GFP_ATOMIC);
 		if (pNodeList == NULL) {
 			result = -ENOMEM;
 			break;
diff --git a/drivers/staging/vt6656/main_usb.c b/drivers/staging/vt6656/main_usb.c
index 763e028a5cc..ee5261a3688 100644
--- a/drivers/staging/vt6656/main_usb.c
+++ b/drivers/staging/vt6656/main_usb.c
@@ -1257,9 +1257,7 @@ static void __devexit vt6656_disconnect(struct usb_interface *intf)
 	}
 
 	device_release_WPADEV(device);
-
-	if (device->firmware)
-		release_firmware(device->firmware);
+	release_firmware(device->firmware);
 
 	usb_set_intfdata(intf, NULL);
 	usb_put_dev(interface_to_usbdev(intf));
diff --git a/drivers/staging/vt6656/wpa.c b/drivers/staging/vt6656/wpa.c
index 7dde3d6941a..b16d4ddc117 100644
--- a/drivers/staging/vt6656/wpa.c
+++ b/drivers/staging/vt6656/wpa.c
@@ -149,7 +149,7 @@ WPA_ParseRSN(
             j = 0;
             DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"wPKCount: %d, sizeof(pBSSList->abyPKType): %zu\n", pRSN->wPKCount, sizeof(pBSSList->abyPKType));
 	    for (i = 0; (i < pRSN->wPKCount) &&
-		   (j < sizeof(pBSSList->abyPKType)/sizeof(BYTE)); i++) {
+		   (j < ARRAY_SIZE(pBSSList->abyPKType)); i++) {
                 if(pRSN->len >= 12+i*4+4) { //oui1(4)+ver(2)+GKS(4)+PKSCnt(2)+PKS(4*i)
                     if ( !memcmp(pRSN->PKSList[i].abyOUI, abyOUI00, 4))
                         pBSSList->abyPKType[j++] = WPA_NONE;
@@ -182,7 +182,7 @@ WPA_ParseRSN(
             DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"wAuthCount: %d, sizeof(pBSSList->abyAuthType): %zu\n",
                           pIE_RSN_Auth->wAuthCount, sizeof(pBSSList->abyAuthType));
 	    for (i = 0; (i < pIE_RSN_Auth->wAuthCount) &&
-		   (j < sizeof(pBSSList->abyAuthType)/sizeof(BYTE)); i++) {
+		   (j < ARRAY_SIZE(pBSSList->abyAuthType)); i++) {
                 if(pRSN->len >= 14+4+(m+i)*4) { //oui1(4)+ver(2)+GKS(4)+PKSCnt(2)+PKS(4*m)+AKC(2)+AKS(4*i)
                     if ( !memcmp(pIE_RSN_Auth->AuthKSList[i].abyOUI, abyOUI01, 4))
                         pBSSList->abyAuthType[j++] = WPA_AUTH_IEEE802_1X;
diff --git a/drivers/staging/wlags49_h2/README.ubuntu b/drivers/staging/wlags49_h2/README.ubuntu
index edee8b9385b..5f1cfb8fd42 100644
--- a/drivers/staging/wlags49_h2/README.ubuntu
+++ b/drivers/staging/wlags49_h2/README.ubuntu
@@ -87,7 +87,7 @@ The linux driver files (wl_xxxx.c) are changed in the following ways:
 -- Recovery actions added
 
 The major problem was the order in which calls can be made. The original
-looks like a traditonal UNIX driver. To call an "ioctl" function you
+looks like a traditional UNIX driver. To call an "ioctl" function you
 have to "open" the device first to get a handle and after "close" no
 "ioctl" function can be called anymore. With the 2.6 driver this all
 changed; the former ioctl functions are now called before "open" and
diff --git a/drivers/staging/wlags49_h2/hcf.c b/drivers/staging/wlags49_h2/hcf.c
index 5957c3a439a..366e4a4b75c 100644
--- a/drivers/staging/wlags49_h2/hcf.c
+++ b/drivers/staging/wlags49_h2/hcf.c
@@ -2871,8 +2871,8 @@ or
 *   The Assert validates the HCF assumption about Hermes implementation upon which the range of
 *   Pseudo-RIDs is based.
 *   Then the control fields up to the start of the 802.3 frame are read from the NIC into the lookahead buffer.
-*   The status field is converted to native Endianess.
-*   The length is, after implicit Endianess conversion if needed, and adjustment for the 14 bytes of the
+*   The status field is converted to native Endianness.
+*   The length is, after implicit Endianness conversion if needed, and adjustment for the 14 bytes of the
 *   802.3 MAC header, stored in IFB_RxLen.
 *   In MAC Monitor mode, 802.11 control frames with a TOTAL length of 14 are received, so without this
 *   length adjustment, IFB_RxLen could not be used to distinguish these frames from "no frame".
@@ -2894,7 +2894,7 @@ or
 *     - the Hermes reported Tunnel encapsulation or
 *     - the Hermes reported 1042 Encapsulation and hcf_encap reports that the HCF would not have used
 *       1042 as the encapsulation mechanism
-*   Note that the first field of the RxFS in bufp has Native Endianess due to the conversion done by the
+*   Note that the first field of the RxFS in bufp has Native Endianness due to the conversion done by the
 *   BE_PAR in get_frag.
 *36: The Type field is the only word kept (after moving) of the just read 8 bytes, it is moved to the
 *   L-field.  The original L-field and 6 byte SNAP header are discarded, so IFB_RxLen and buf_addr must
@@ -3831,7 +3831,7 @@ get_fid( IFBP ifbp )
  *.DESCRIPTION
  * process the single byte (if applicable) read by the previous get_frag and copy len (or len-1) bytes from
  * NIC to bufp.
- * On a Big Endian platform, the parameter word_len controls the number of leading bytes whose endianess is
+ * On a Big Endian platform, the parameter word_len controls the number of leading bytes whose endianness is
  * converted (i.e. byte swapped)
  *
  *
@@ -3980,7 +3980,7 @@ get_frag( IFBP ifbp, wci_bufp bufp, int len BE_PAR( int word_len ) )
  *    appropriate means on H-I: always
  *    and on H-II if F/W supplier reflects a primary (i.e. only after an Hermes Reset or Init
  *    command).
- *    QUESTION ;? !!!!!! should, For each of the above RIDs the Endianess is converted to native Endianess.
+ *    QUESTION ;? !!!!!! should, For each of the above RIDs the Endianness is converted to native Endianness.
  *    Only the return code of the first hcf_get_info is used. All hcf_get_info calls are made, regardless of
  *    the success or failure of the 1st hcf_get_info. The assumptions are:
  *     - if any call fails, they all fail, so remembering the result of the 1st call is adequate
diff --git a/drivers/staging/wlags49_h2/hcf.h b/drivers/staging/wlags49_h2/hcf.h
index 95527b5cf86..68e23303b5e 100644
--- a/drivers/staging/wlags49_h2/hcf.h
+++ b/drivers/staging/wlags49_h2/hcf.h
@@ -90,7 +90,7 @@
 
 #define LOF(x) 			(sizeof(x)/sizeof(hcf_16)-1)
 
-/*	Endianess
+/*	Endianness
  *	Little Endian (a.k.a. Intel), least significant byte first
  *	Big Endian (a.k.a. Motorola), most significant byte first
  *
@@ -101,7 +101,7 @@
  */
 
 /* To increase portability, use unsigned char and unsigned char * when accessing parts of larger
- * types to convert their Endianess
+ * types to convert their Endianness
  */
 
 #define CNV_END_SHORT(w)  (hcf_16)( ((hcf_16)(w) & 0x00FF) << 8 | ((hcf_16)(w) & 0xFF00) >> 8 )
@@ -109,14 +109,14 @@
 
 #if HCF_BIG_ENDIAN
 //******************************************** B I G   E N D I A N *******************************************
-#define CNV_LITTLE_TO_SHORT(w)	CNV_END_SHORT(w)	//    endianess conversion needed
-#define CNV_BIG_TO_SHORT(w)		(w)				// no endianess conversion needed
+#define CNV_LITTLE_TO_SHORT(w)	CNV_END_SHORT(w)	//    endianness conversion needed
+#define CNV_BIG_TO_SHORT(w)		(w)				// no endianness conversion needed
 #define CNV_LITTLE_TO_LONG(dw)	CNV_END_LONG(dw)
 #define CNV_LONG_TO_LITTLE(dw)	CNV_END_LONG(dw)
 #else
 //****************************************** L I T T L E   E N D I A N ****************************************
-#define CNV_LITTLE_TO_SHORT(w) 	(w)				// no endianess conversion needed
-#define CNV_BIG_TO_SHORT(w)		CNV_END_SHORT(w)	//    endianess conversion needed
+#define CNV_LITTLE_TO_SHORT(w) 	(w)				// no endianness conversion needed
+#define CNV_BIG_TO_SHORT(w)		CNV_END_SHORT(w)	//    endianness conversion needed
 #define CNV_LITTLE_TO_LONG(dw)	(dw)
 #define CNV_LONG_TO_LITTLE(dw)	(dw)
 
diff --git a/drivers/staging/wlags49_h2/mmd.c b/drivers/staging/wlags49_h2/mmd.c
index c8f52107e6c..7204a373bc5 100644
--- a/drivers/staging/wlags49_h2/mmd.c
+++ b/drivers/staging/wlags49_h2/mmd.c
@@ -101,10 +101,10 @@
 *	supp	address of the supplier specification
 *
 *	Description: mmd_check_comp is a support routine to check the compatibility between an actor and a
-*	supplier.  mmd_check_comp is independent of the endianess of the actp and supp structures. This is
+*	supplier.  mmd_check_comp is independent of the endianness of the actp and supp structures. This is
 *	achieved by checking the "bottom" or "role" fields of these structures. Since these fields are restricted
 *	to a limited range, comparing the contents to a value with a known endian-ess gives a clue to their actual
-*	endianess.
+*	endianness.
 *
 *.DIAGRAM
 *1a: The role-field of the actor structure has a known non-zero, not "byte symmetric" value (namely
@@ -114,16 +114,16 @@
 *	for a supplier. A supplier has always exactly 1 variant,top,bottom record with (officially, but see the
 *	note below) each of these 3 values in the range 1 through 99, so one byte of the word value of variant,
 *	top and bottom words is 0x00 and the other byte is non-zero. Whether the lowest address byte or the
-*	highest address byte is non-zero depends on the Endianess of the LTV. If and only if the word value of
+*	highest address byte is non-zero depends on the Endianness of the LTV. If and only if the word value of
 *	bottom is less than 0x0100, the supplier is Native Endian.
 *	NOTE: the variant field of the supplier structure can not be used for the Endian Detection Algorithm,
 *	because a a zero-valued variant has been used as Controlled Deployment indication in the past.
 *	Note: An actor may have multiple sets of variant,top,bottom records, including dummy sets with variant,
-*	top and bottom fields with a zero-value. As a consequence the endianess of the actor can not be determined
+*	top and bottom fields with a zero-value. As a consequence the endianness of the actor can not be determined
 *	based on its variant,top,bottom values.
 *
 *	Note: the L and T field of the structures are always in Native Endian format, so you can not draw
-*	conclusions concerning the Endianess of the structure based on these two fields.
+*	conclusions concerning the Endianness of the structure based on these two fields.
 *
 *1b/2b
 *	The only purpose of the CFG_RANGE_SPEC_BYTE_STRCT is to give easy access to the non-zero byte of the word
@@ -149,7 +149,7 @@
 *
 *	This is implemented as:
 *	#if HCF_BIG_ENDIAN == 0	//platform is LE
-*		sup/act_endian becomes reverse of structure-endianess as determined in 1a/1b
+*		sup/act_endian becomes reverse of structure-endianness as determined in 1a/1b
 *	#endif
 *6:	Each of the actor variant-bottom-top records is checked against the (single) supplier variant-bottom-top
 *	range till either an acceptable match is found or all actor records are tried. As explained above, due to
diff --git a/drivers/staging/wlags49_h2/wl_cs.c b/drivers/staging/wlags49_h2/wl_cs.c
index 7084f414846..7c7c77f9c86 100644
--- a/drivers/staging/wlags49_h2/wl_cs.c
+++ b/drivers/staging/wlags49_h2/wl_cs.c
@@ -177,10 +177,9 @@ static void wl_adapter_detach(struct pcmcia_device *link)
 	if (dev) {
 		unregister_wlags_sysfs(dev);
 		unregister_netdev(dev);
+		wl_device_dealloc(dev);
 	}
 
-	wl_device_dealloc(dev);
-
 	DBG_LEAVE(DbgInfo);
 } /* wl_adapter_detach */
 /*============================================================================*/
diff --git a/drivers/staging/wlags49_h2/wl_netdev.c b/drivers/staging/wlags49_h2/wl_netdev.c
index 90820ff1ace..824b8523235 100644
--- a/drivers/staging/wlags49_h2/wl_netdev.c
+++ b/drivers/staging/wlags49_h2/wl_netdev.c
@@ -1063,7 +1063,7 @@ void wl_multicast( struct net_device *dev )
 #if DBG
     if( DBG_FLAGS( DbgInfo ) & DBG_PARAM_ON ) {
         DBG_PRINT("  flags: %s%s%s\n",
-            ( dev->flags & IFF_PROMISC ) ? "Promiscous " : "",
+            ( dev->flags & IFF_PROMISC ) ? "Promiscuous " : "",
             ( dev->flags & IFF_MULTICAST ) ? "Multicast " : "",
             ( dev->flags & IFF_ALLMULTI ) ? "All-Multicast" : "" );
 
@@ -1510,8 +1510,11 @@ void wl_wds_device_alloc( struct wl_private *lp )
     for( count = 0; count < NUM_WDS_PORTS; count++ ) {
         struct net_device *dev_wds = NULL;
 
-        dev_wds = kmalloc( sizeof( struct net_device ), GFP_KERNEL );
-        memset( dev_wds, 0, sizeof( struct net_device ));
+	dev_wds = kzalloc(sizeof(struct net_device), GFP_KERNEL);
+	if (!dev_wds) {
+		DBG_LEAVE(DbgInfo);
+		return;
+	}
 
         ether_setup( dev_wds );
 
diff --git a/drivers/staging/wlags49_h2/wl_pci.c b/drivers/staging/wlags49_h2/wl_pci.c
index 3df990c7306..0b31b01bd49 100644
--- a/drivers/staging/wlags49_h2/wl_pci.c
+++ b/drivers/staging/wlags49_h2/wl_pci.c
@@ -524,6 +524,7 @@ int wl_pci_setup( struct pci_dev *pdev )
     /* Make sure that space was allocated for our private adapter struct */
     if( dev->priv == NULL ) {
         DBG_ERROR( DbgInfo, "Private adapter struct was not allocated!!!\n" );
+	wl_device_dealloc(dev);
         DBG_LEAVE( DbgInfo );
         return -ENOMEM;
     }
@@ -532,6 +533,7 @@ int wl_pci_setup( struct pci_dev *pdev )
     /* Allocate DMA Descriptors */
     if( wl_pci_dma_alloc( pdev, dev->priv ) < 0 ) {
         DBG_ERROR( DbgInfo, "Could not allocate DMA descriptor memory!!!\n" );
+	wl_device_dealloc(dev);
         DBG_LEAVE( DbgInfo );
         return -ENOMEM;
     }
@@ -561,6 +563,8 @@ int wl_pci_setup( struct pci_dev *pdev )
     result = request_irq(dev->irq, wl_isr, SA_SHIRQ, dev->name, dev);
     if( result ) {
         DBG_WARNING( DbgInfo, "Could not register ISR!!!\n" );
+	wl_remove(dev);
+	wl_device_dealloc(dev);
         DBG_LEAVE( DbgInfo );
         return result;
 	}
diff --git a/drivers/staging/wlags49_h2/wl_profile.c b/drivers/staging/wlags49_h2/wl_profile.c
index b8c96cf18de..0e49272bc7a 100644
--- a/drivers/staging/wlags49_h2/wl_profile.c
+++ b/drivers/staging/wlags49_h2/wl_profile.c
@@ -401,7 +401,7 @@ void translate_option(char *buffer, struct wl_private *lp)
 		if ((value_convert >= PARM_MIN_BRSC) || (value_convert <= PARM_MAX_BRSC))
 			lp->brsc[0] = value_convert;
 		else
-			DBG_WARNING(DbgInfo, "%s invaid; will be ignored\n", PARM_NAME_BRSC_2GHZ);
+			DBG_WARNING(DbgInfo, "%s invalid; will be ignored\n", PARM_NAME_BRSC_2GHZ);
 	} else if (strcmp(key, PARM_NAME_BRSC_5GHZ) == 0) {
 		DBG_TRACE(DbgInfo, "%s, value: %s\n", PARM_NAME_BRSC_5GHZ, value);
 
@@ -409,7 +409,7 @@ void translate_option(char *buffer, struct wl_private *lp)
 		if ((value_convert >= PARM_MIN_BRSC) || (value_convert <= PARM_MAX_BRSC))
 			lp->brsc[1] = value_convert;
 		else
-			DBG_WARNING(DbgInfo, "%s invaid; will be ignored\n", PARM_NAME_BRSC_5GHZ);
+			DBG_WARNING(DbgInfo, "%s invalid; will be ignored\n", PARM_NAME_BRSC_5GHZ);
 	} else if ((strcmp(key, PARM_NAME_DESIRED_SSID) == 0) || (strcmp(key, PARM_NAME_OWN_SSID) == 0)) {
 		DBG_TRACE(DbgInfo, "SSID, value: %s\n", value);
 
@@ -556,7 +556,7 @@ void translate_option(char *buffer, struct wl_private *lp)
 		if ((value_convert >= PARM_MIN_SRSC) || (value_convert <= PARM_MAX_SRSC))
 			lp->srsc[0] = value_convert;
 		else
-			DBG_WARNING(DbgInfo, "%s invaid; will be ignored\n", PARM_NAME_SRSC_2GHZ);
+			DBG_WARNING(DbgInfo, "%s invalid; will be ignored\n", PARM_NAME_SRSC_2GHZ);
 	} else if (strcmp(key, PARM_NAME_SRSC_5GHZ) == 0) {
 		DBG_TRACE(DbgInfo, "%s, value: %s\n", PARM_NAME_SRSC_5GHZ, value);
 
@@ -564,7 +564,7 @@ void translate_option(char *buffer, struct wl_private *lp)
 		if ((value_convert >= PARM_MIN_SRSC) || (value_convert <= PARM_MAX_SRSC))
 			lp->srsc[1] = value_convert;
 		else
-			DBG_WARNING(DbgInfo, "%s invaid; will be ignored\n", PARM_NAME_SRSC_5GHZ);
+			DBG_WARNING(DbgInfo, "%s invalid; will be ignored\n", PARM_NAME_SRSC_5GHZ);
 	} else if (strcmp(key, PARM_NAME_SYSTEM_SCALE) == 0) {
 		DBG_TRACE(DbgInfo, "%s, value: %s\n", PARM_NAME_SYSTEM_SCALE, value);
 
diff --git a/drivers/staging/wlags49_h2/wl_util.c b/drivers/staging/wlags49_h2/wl_util.c
index f104e6f1e98..404ec7da034 100644
--- a/drivers/staging/wlags49_h2/wl_util.c
+++ b/drivers/staging/wlags49_h2/wl_util.c
@@ -98,8 +98,7 @@
  ******************************************************************************/
 
 /* A matrix which maps channels to frequencies */
-#define MAX_CHAN_FREQ_MAP_ENTRIES   50
-static const long chan_freq_list[][MAX_CHAN_FREQ_MAP_ENTRIES] =
+static const long chan_freq_list[][2] =
 {
     {1,2412},
     {2,2417},
@@ -846,7 +845,7 @@ int wl_is_a_valid_chan( int channel )
     }
 
     /* Iterate through the matrix and retrieve the frequency */
-    for( i = 0; i < MAX_CHAN_FREQ_MAP_ENTRIES; i++ ) {
+    for( i = 0; i < ARRAY_SIZE(chan_freq_list); i++ ) {
         if( chan_freq_list[i][0] == channel ) {
             return 1;
         }
@@ -884,7 +883,7 @@ int wl_is_a_valid_freq( long frequency )
 
 
     /* Iterate through the matrix and retrieve the channel */
-    for( i = 0; i < MAX_CHAN_FREQ_MAP_ENTRIES; i++ ) {
+    for( i = 0; i < ARRAY_SIZE(chan_freq_list); i++ ) {
         if( chan_freq_list[i][1] == frequency ) {
             return 1;
         }
@@ -927,7 +926,7 @@ long wl_get_freq_from_chan( int channel )
     }
 
     /* Iterate through the matrix and retrieve the frequency */
-    for( i = 0; i < MAX_CHAN_FREQ_MAP_ENTRIES; i++ ) {
+    for( i = 0; i < ARRAY_SIZE(chan_freq_list); i++ ) {
         if( chan_freq_list[i][0] == channel ) {
             return chan_freq_list[i][1];
         }
@@ -965,7 +964,7 @@ int wl_get_chan_from_freq( long frequency )
 
 
     /* Iterate through the matrix and retrieve the channel */
-    for( i = 0; i < MAX_CHAN_FREQ_MAP_ENTRIES; i++ ) {
+    for( i = 0; i < ARRAY_SIZE(chan_freq_list); i++ ) {
         if( chan_freq_list[i][1] == frequency ) {
             return chan_freq_list[i][0];
         }
diff --git a/drivers/staging/wlan-ng/cfg80211.c b/drivers/staging/wlan-ng/cfg80211.c
index 4cd3ba5d564..8bc562b8c4d 100644
--- a/drivers/staging/wlan-ng/cfg80211.c
+++ b/drivers/staging/wlan-ng/cfg80211.c
@@ -332,6 +332,7 @@ int prism2_scan(struct wiphy *wiphy, struct net_device *dev,
 	wlandevice_t *wlandev = dev->ml_priv;
 	struct p80211msg_dot11req_scan msg1;
 	struct p80211msg_dot11req_scan_results msg2;
+	struct cfg80211_bss *bss;
 	int result;
 	int err = 0;
 	int numbss = 0;
@@ -401,7 +402,7 @@ int prism2_scan(struct wiphy *wiphy, struct net_device *dev,
 		ie_buf[1] = msg2.ssid.data.len;
 		ie_len = ie_buf[1] + 2;
 		memcpy(&ie_buf[2], &(msg2.ssid.data.data), msg2.ssid.data.len);
-		cfg80211_inform_bss(wiphy,
+		bss = cfg80211_inform_bss(wiphy,
 			ieee80211_get_channel(wiphy, ieee80211_dsss_chan_to_freq(msg2.dschannel.data)),
 			(const u8 *) &(msg2.bssid.data.data),
 			msg2.timestamp.data, msg2.capinfo.data,
@@ -411,6 +412,13 @@ int prism2_scan(struct wiphy *wiphy, struct net_device *dev,
 			(msg2.signal.data - 65536) * 100, /* Conversion to signed type */
 			GFP_KERNEL
 		);
+
+		if (!bss) {
+			err = -ENOMEM;
+			goto exit;
+		}
+
+		cfg80211_put_bss(bss);
 	}
 
 	if (result)
diff --git a/drivers/staging/wlan-ng/prism2mgmt.c b/drivers/staging/wlan-ng/prism2mgmt.c
index c3bb05dd744..4efa9bc0fcf 100644
--- a/drivers/staging/wlan-ng/prism2mgmt.c
+++ b/drivers/staging/wlan-ng/prism2mgmt.c
@@ -380,8 +380,8 @@ int prism2mgmt_scan_results(wlandevice_t *wlandev, void *msgp)
 	}
 
 	count = (hw->scanresults->framelen - 3) / 32;
-	if (count > 32)
-		count = 32;
+	if (count > HFA384x_SCANRESULT_MAX)
+		count = HFA384x_SCANRESULT_MAX;
 
 	if (req->bssindex.data >= count) {
 		pr_debug("requested index (%d) out of range (%d)\n",
diff --git a/drivers/staging/xgifb/XGI_main.h b/drivers/staging/xgifb/XGI_main.h
index e828fd403c3..9c62aeb9ede 100644
--- a/drivers/staging/xgifb/XGI_main.h
+++ b/drivers/staging/xgifb/XGI_main.h
@@ -12,9 +12,6 @@
 
 #define XGIFAIL(x) do { printk(x "\n"); return -EINVAL; } while (0)
 
-#ifndef PCI_DEVICE_ID_XGI_41
-#define PCI_DEVICE_ID_XGI_41      0x041
-#endif
 #ifndef PCI_DEVICE_ID_XGI_42
 #define PCI_DEVICE_ID_XGI_42      0x042
 #endif
@@ -82,177 +79,79 @@ static int XGIfb_tvplug = -1;
 /* TW: For ioctl XGIFB_GET_INFO */
 /* XGIfb_info XGIfbinfo; */
 
-#define MD_XGI300 1
-#define MD_XGI315 2
+#define MD_XGI315 1
 
 /* mode table */
 static const struct _XGIbios_mode {
-	char name[15];
 	u8 mode_no;
 	u16 vesa_mode_no_1;  /* "XGI defined" VESA mode number */
 	u16 vesa_mode_no_2;  /* Real VESA mode numbers */
 	u16 xres;
 	u16 yres;
 	u16 bpp;
-	u16 rate_idx;
-	u16 cols;
-	u16 rows;
 	u8  chipset;
 } XGIbios_mode[] = {
-	{"320x240x16",   0x56, 0x0000, 0x0000,  320,  240, 16, 1,  40, 15,
-	 MD_XGI315},
-	{"320x480x8",    0x5A, 0x0000, 0x0000,  320,  480,  8, 1,  40, 30,
-	 MD_XGI315},  /* TW: FSTN */
-	{"320x480x16",   0x5B, 0x0000, 0x0000,  320,  480, 16, 1,  40, 30,
-	 MD_XGI315},  /* TW: FSTN */
-	{"640x480x8",    0x2E, 0x0101, 0x0101,  640,  480,  8, 1,  80, 30,
-	 MD_XGI300|MD_XGI315},
-	{"640x480x16",   0x44, 0x0111, 0x0111,  640,  480, 16, 1,  80, 30,
-	 MD_XGI300|MD_XGI315},
-	{"640x480x24",   0x62, 0x013a, 0x0112,  640,  480, 32, 1,  80, 30,
-	 MD_XGI300|MD_XGI315},  /* TW: That's for people who mix up color-
-					and fb depth */
-	{"640x480x32",   0x62, 0x013a, 0x0112,  640,  480, 32, 1,  80, 30,
-	 MD_XGI300|MD_XGI315},
-	{"720x480x8",    0x31, 0x0000, 0x0000,  720,  480,  8, 1,  90, 30,
-	 MD_XGI300|MD_XGI315},
-	{"720x480x16",   0x33, 0x0000, 0x0000,  720,  480, 16, 1,  90, 30,
-	 MD_XGI300|MD_XGI315},
-	{"720x480x24",   0x35, 0x0000, 0x0000,  720,  480, 32, 1,  90, 30,
-	 MD_XGI300|MD_XGI315},
-	{"720x480x32",   0x35, 0x0000, 0x0000,  720,  480, 32, 1,  90, 30,
-	 MD_XGI300|MD_XGI315},
-	{"720x576x8",    0x32, 0x0000, 0x0000,  720,  576,  8, 1,  90, 36,
-	 MD_XGI300|MD_XGI315},
-	{"720x576x16",   0x34, 0x0000, 0x0000,  720,  576, 16, 1,  90, 36,
-	 MD_XGI300|MD_XGI315},
-	{"720x576x24",   0x36, 0x0000, 0x0000,  720,  576, 32, 1,  90, 36,
-	 MD_XGI300|MD_XGI315},
-	{"720x576x32",   0x36, 0x0000, 0x0000,  720,  576, 32, 1,  90, 36,
-	 MD_XGI300|MD_XGI315},
-	{"800x480x8",    0x70, 0x0000, 0x0000,  800,  480,  8, 1, 100, 30,
-	 MD_XGI300|MD_XGI315},
-	{"800x480x16",   0x7a, 0x0000, 0x0000,  800,  480, 16, 1, 100, 30,
-	 MD_XGI300|MD_XGI315},
-	{"800x480x24",   0x76, 0x0000, 0x0000,  800,  480, 32, 1, 100, 30,
-	 MD_XGI300|MD_XGI315},
-	{"800x480x32",   0x76, 0x0000, 0x0000,  800,  480, 32, 1, 100, 30,
-	 MD_XGI300|MD_XGI315},
-	{"800x600x8",    0x30, 0x0103, 0x0103,  800,  600,  8, 1, 100, 37,
-	 MD_XGI300|MD_XGI315},
-#define DEFAULT_MODE              20 /* index for 800x600x16 */
-	{"800x600x16",   0x47, 0x0114, 0x0114,  800,  600, 16, 1, 100, 37,
-	 MD_XGI300|MD_XGI315},
-	{"800x600x24",   0x63, 0x013b, 0x0115,  800,  600, 32, 1, 100, 37,
-	 MD_XGI300|MD_XGI315},
-	{"800x600x32",   0x63, 0x013b, 0x0115,  800,  600, 32, 1, 100, 37,
-	 MD_XGI300|MD_XGI315},
-	{"1024x576x8",   0x71, 0x0000, 0x0000, 1024,  576,  8, 1, 128, 36,
-	 MD_XGI300|MD_XGI315},
-	{"1024x576x16",  0x74, 0x0000, 0x0000, 1024,  576, 16, 1, 128, 36,
-	 MD_XGI300|MD_XGI315},
-	{"1024x576x24",  0x77, 0x0000, 0x0000, 1024,  576, 32, 1, 128, 36,
-	 MD_XGI300|MD_XGI315},
-	{"1024x576x32",  0x77, 0x0000, 0x0000, 1024,  576, 32, 1, 128, 36,
-	 MD_XGI300|MD_XGI315},
-	{"1024x600x8",   0x20, 0x0000, 0x0000, 1024,  600,  8, 1, 128, 37,
-	 MD_XGI300          },  /* TW: 300 series only */
-	{"1024x600x16",  0x21, 0x0000, 0x0000, 1024,  600, 16, 1, 128, 37,
-	 MD_XGI300          },
-	{"1024x600x24",  0x22, 0x0000, 0x0000, 1024,  600, 32, 1, 128, 37,
-	 MD_XGI300          },
-	{"1024x600x32",  0x22, 0x0000, 0x0000, 1024,  600, 32, 1, 128, 37,
-	 MD_XGI300          },
-	{"1024x768x8",   0x38, 0x0105, 0x0105, 1024,  768,  8, 1, 128, 48,
-	 MD_XGI300|MD_XGI315},
-	{"1024x768x16",  0x4A, 0x0117, 0x0117, 1024,  768, 16, 1, 128, 48,
-	 MD_XGI300|MD_XGI315},
-	{"1024x768x24",  0x64, 0x013c, 0x0118, 1024,  768, 32, 1, 128, 48,
-	 MD_XGI300|MD_XGI315},
-	{"1024x768x32",  0x64, 0x013c, 0x0118, 1024,  768, 32, 1, 128, 48,
-	 MD_XGI300|MD_XGI315},
-	{"1152x768x8",   0x23, 0x0000, 0x0000, 1152,  768,  8, 1, 144, 48,
-	 MD_XGI300          },  /* TW: 300 series only */
-	{"1152x768x16",  0x24, 0x0000, 0x0000, 1152,  768, 16, 1, 144, 48,
-	 MD_XGI300          },
-	{"1152x768x24",  0x25, 0x0000, 0x0000, 1152,  768, 32, 1, 144, 48,
-	 MD_XGI300          },
-	{"1152x768x32",  0x25, 0x0000, 0x0000, 1152,  768, 32, 1, 144, 48,
-	 MD_XGI300          },
-	{"1280x720x8",   0x79, 0x0000, 0x0000, 1280,  720,  8, 1, 160, 45,
-	 MD_XGI300|MD_XGI315},
-	{"1280x720x16",  0x75, 0x0000, 0x0000, 1280,  720, 16, 1, 160, 45,
-	 MD_XGI300|MD_XGI315},
-	{"1280x720x24",  0x78, 0x0000, 0x0000, 1280,  720, 32, 1, 160, 45,
-	 MD_XGI300|MD_XGI315},
-	{"1280x720x32",  0x78, 0x0000, 0x0000, 1280,  720, 32, 1, 160, 45,
-	 MD_XGI300|MD_XGI315},
-	{"1280x768x8",   0x23, 0x0000, 0x0000, 1280,  768,  8, 1, 160, 48,
-	 MD_XGI315},  /* TW: 310/325 series only */
-	{"1280x768x16",  0x24, 0x0000, 0x0000, 1280,  768, 16, 1, 160, 48,
-	 MD_XGI315},
-	{"1280x768x24",  0x25, 0x0000, 0x0000, 1280,  768, 32, 1, 160, 48,
-	 MD_XGI315},
-	{"1280x768x32",  0x25, 0x0000, 0x0000, 1280,  768, 32, 1, 160, 48,
-	 MD_XGI315},
-	{"1280x960x8",   0x7C, 0x0000, 0x0000, 1280,  960,  8, 1, 160, 60,
-	 MD_XGI300|MD_XGI315},
-	{"1280x960x16",  0x7D, 0x0000, 0x0000, 1280,  960, 16, 1, 160, 60,
-	 MD_XGI300|MD_XGI315},
-	{"1280x960x24",  0x7E, 0x0000, 0x0000, 1280,  960, 32, 1, 160, 60,
-	 MD_XGI300|MD_XGI315},
-	{"1280x960x32",  0x7E, 0x0000, 0x0000, 1280,  960, 32, 1, 160, 60,
-	 MD_XGI300|MD_XGI315},
-	{"1280x1024x8",  0x3A, 0x0107, 0x0107, 1280, 1024,  8, 1, 160, 64,
-	 MD_XGI300|MD_XGI315},
-	{"1280x1024x16", 0x4D, 0x011a, 0x011a, 1280, 1024, 16, 1, 160, 64,
-	 MD_XGI300|MD_XGI315},
-	{"1280x1024x24", 0x65, 0x013d, 0x011b, 1280, 1024, 32, 1, 160, 64,
-	 MD_XGI300|MD_XGI315},
-	{"1280x1024x32", 0x65, 0x013d, 0x011b, 1280, 1024, 32, 1, 160, 64,
-	 MD_XGI300|MD_XGI315},
-	{"1400x1050x8",  0x26, 0x0000, 0x0000, 1400, 1050,  8, 1, 175, 65,
-	 MD_XGI315},  /* TW: 310/325 series only */
-	{"1400x1050x16", 0x27, 0x0000, 0x0000, 1400, 1050, 16, 1, 175, 65,
-	 MD_XGI315},
-	{"1400x1050x24", 0x28, 0x0000, 0x0000, 1400, 1050, 32, 1, 175, 65,
-	 MD_XGI315},
-	{"1400x1050x32", 0x28, 0x0000, 0x0000, 1400, 1050, 32, 1, 175, 65,
-	 MD_XGI315},
-	{"1600x1200x8",  0x3C, 0x0130, 0x011c, 1600, 1200,  8, 1, 200, 75,
-	 MD_XGI300|MD_XGI315},
-	{"1600x1200x16", 0x3D, 0x0131, 0x011e, 1600, 1200, 16, 1, 200, 75,
-	 MD_XGI300|MD_XGI315},
-	{"1600x1200x24", 0x66, 0x013e, 0x011f, 1600, 1200, 32, 1, 200, 75,
-	 MD_XGI300|MD_XGI315},
-	{"1600x1200x32", 0x66, 0x013e, 0x011f, 1600, 1200, 32, 1, 200, 75,
-	 MD_XGI300|MD_XGI315},
-	{"1920x1440x8",  0x68, 0x013f, 0x0000, 1920, 1440,  8, 1, 240, 75,
-	 MD_XGI300|MD_XGI315},
-	{"1920x1440x16", 0x69, 0x0140, 0x0000, 1920, 1440, 16, 1, 240, 75,
-	 MD_XGI300|MD_XGI315},
-	{"1920x1440x24", 0x6B, 0x0141, 0x0000, 1920, 1440, 32, 1, 240, 75,
-	 MD_XGI300|MD_XGI315},
-	{"1920x1440x32", 0x6B, 0x0141, 0x0000, 1920, 1440, 32, 1, 240, 75,
-	 MD_XGI300|MD_XGI315},
-	{"2048x1536x8",  0x6c, 0x0000, 0x0000, 2048, 1536,  8, 1, 256, 96,
-	 MD_XGI315},  /* TW: 310/325 series only */
-	{"2048x1536x16", 0x6d, 0x0000, 0x0000, 2048, 1536, 16, 1, 256, 96,
-	 MD_XGI315},
-	{"2048x1536x24", 0x6e, 0x0000, 0x0000, 2048, 1536, 32, 1, 256, 96,
-	 MD_XGI315},
-	{"2048x1536x32", 0x6e, 0x0000, 0x0000, 2048, 1536, 32, 1, 256, 96,
-	 MD_XGI315},
-	{"\0", 0x00, 0, 0, 0, 0, 0, 0, 0}
+	{ 0x56, 0x0000, 0x0000,  320,  240, 16, MD_XGI315 },
+	{ 0x5A, 0x0000, 0x0000,  320,  480,  8, MD_XGI315 },
+	{ 0x5B, 0x0000, 0x0000,  320,  480, 16, MD_XGI315 },
+	{ 0x2E, 0x0101, 0x0101,  640,  480,  8, MD_XGI315 },
+	{ 0x44, 0x0111, 0x0111,  640,  480, 16, MD_XGI315 },
+	{ 0x62, 0x013a, 0x0112,  640,  480, 32, MD_XGI315 },
+	{ 0x31, 0x0000, 0x0000,  720,  480,  8, MD_XGI315 },
+	{ 0x33, 0x0000, 0x0000,  720,  480, 16, MD_XGI315 },
+	{ 0x35, 0x0000, 0x0000,  720,  480, 32, MD_XGI315 },
+	{ 0x32, 0x0000, 0x0000,  720,  576,  8, MD_XGI315 },
+	{ 0x34, 0x0000, 0x0000,  720,  576, 16, MD_XGI315 },
+	{ 0x36, 0x0000, 0x0000,  720,  576, 32, MD_XGI315 },
+	{ 0x36, 0x0000, 0x0000,  720,  576, 32, MD_XGI315 },
+	{ 0x70, 0x0000, 0x0000,  800,  480,  8, MD_XGI315 },
+	{ 0x7a, 0x0000, 0x0000,  800,  480, 16, MD_XGI315 },
+	{ 0x76, 0x0000, 0x0000,  800,  480, 32, MD_XGI315 },
+	{ 0x30, 0x0103, 0x0103,  800,  600,  8, MD_XGI315 },
+#define DEFAULT_MODE              17 /* index for 800x600x16 */
+	{ 0x47, 0x0114, 0x0114,  800,  600, 16, MD_XGI315 },
+	{ 0x63, 0x013b, 0x0115,  800,  600, 32, MD_XGI315 },
+	{ 0x71, 0x0000, 0x0000, 1024,  576,  8, MD_XGI315 },
+	{ 0x74, 0x0000, 0x0000, 1024,  576, 16, MD_XGI315 },
+	{ 0x77, 0x0000, 0x0000, 1024,  576, 32, MD_XGI315 },
+	{ 0x77, 0x0000, 0x0000, 1024,  576, 32, MD_XGI315 },
+	{ 0x20, 0x0000, 0x0000, 1024,  600,  8, },
+	{ 0x21, 0x0000, 0x0000, 1024,  600, 16, },
+	{ 0x22, 0x0000, 0x0000, 1024,  600, 32, },
+	{ 0x38, 0x0105, 0x0105, 1024,  768,  8, MD_XGI315 },
+	{ 0x4A, 0x0117, 0x0117, 1024,  768, 16, MD_XGI315 },
+	{ 0x64, 0x013c, 0x0118, 1024,  768, 32, MD_XGI315 },
+	{ 0x64, 0x013c, 0x0118, 1024,  768, 32, MD_XGI315 },
+	{ 0x23, 0x0000, 0x0000, 1152,  768,  8, },
+	{ 0x24, 0x0000, 0x0000, 1152,  768, 16, },
+	{ 0x25, 0x0000, 0x0000, 1152,  768, 32, },
+	{ 0x79, 0x0000, 0x0000, 1280,  720,  8, MD_XGI315 },
+	{ 0x75, 0x0000, 0x0000, 1280,  720, 16, MD_XGI315 },
+	{ 0x78, 0x0000, 0x0000, 1280,  720, 32, MD_XGI315 },
+	{ 0x23, 0x0000, 0x0000, 1280,  768,  8, MD_XGI315 },
+	{ 0x24, 0x0000, 0x0000, 1280,  768, 16, MD_XGI315 },
+	{ 0x25, 0x0000, 0x0000, 1280,  768, 32, MD_XGI315 },
+	{ 0x7C, 0x0000, 0x0000, 1280,  960,  8, MD_XGI315 },
+	{ 0x7D, 0x0000, 0x0000, 1280,  960, 16, MD_XGI315 },
+	{ 0x7E, 0x0000, 0x0000, 1280,  960, 32, MD_XGI315 },
+	{ 0x3A, 0x0107, 0x0107, 1280, 1024,  8, MD_XGI315 },
+	{ 0x4D, 0x011a, 0x011a, 1280, 1024, 16, MD_XGI315 },
+	{ 0x65, 0x013d, 0x011b, 1280, 1024, 32, MD_XGI315 },
+	{ 0x26, 0x0000, 0x0000, 1400, 1050,  8, MD_XGI315 },
+	{ 0x27, 0x0000, 0x0000, 1400, 1050, 16, MD_XGI315 },
+	{ 0x28, 0x0000, 0x0000, 1400, 1050, 32, MD_XGI315 },
+	{ 0x3C, 0x0130, 0x011c, 1600, 1200,  8, MD_XGI315 },
+	{ 0x3D, 0x0131, 0x011e, 1600, 1200, 16, MD_XGI315 },
+	{ 0x66, 0x013e, 0x011f, 1600, 1200, 32, MD_XGI315 },
+	{ 0x68, 0x013f, 0x0000, 1920, 1440,  8, MD_XGI315 },
+	{ 0x69, 0x0140, 0x0000, 1920, 1440, 16, MD_XGI315 },
+	{ 0x6B, 0x0141, 0x0000, 1920, 1440, 32, MD_XGI315 },
+	{ 0x6c, 0x0000, 0x0000, 2048, 1536,  8, MD_XGI315 },
+	{ 0x6d, 0x0000, 0x0000, 2048, 1536, 16, MD_XGI315 },
+	{ 0x6e, 0x0000, 0x0000, 2048, 1536, 32, MD_XGI315 },
+	{ 0 },
 };
 
-/* TW: CR36 evaluation */
-static const unsigned short XGI300paneltype[] = {
-	 LCD_UNKNOWN,  LCD_800x600, LCD_1024x768, LCD_1280x1024,
-	LCD_1280x960,  LCD_640x480, LCD_1024x600, LCD_1152x768,
-	LCD_1024x768, LCD_1024x768, LCD_1024x768,
-	LCD_1024x768, LCD_1024x768, LCD_1024x768, LCD_1024x768};
-
 static const unsigned short XGI310paneltype[] = {
 	 LCD_UNKNOWN,   LCD_800x600, LCD_1024x768, LCD_1280x1024,
 	 LCD_640x480,  LCD_1024x600, LCD_1152x864, LCD_1280x960,
diff --git a/drivers/staging/xgifb/XGI_main_26.c b/drivers/staging/xgifb/XGI_main_26.c
index 21c037827de..85dbf32b1f6 100644
--- a/drivers/staging/xgifb/XGI_main_26.c
+++ b/drivers/staging/xgifb/XGI_main_26.c
@@ -156,25 +156,14 @@ static int XGIfb_mode_rate_to_dclock(struct vb_device_info *XGI_Pr,
 	unsigned short ModeNo = modeno;
 	unsigned short ModeIdIndex = 0, ClockIndex = 0;
 	unsigned short RefreshRateTableIndex = 0;
-
-	/* unsigned long  temp = 0; */
 	int Clock;
 	InitTo330Pointer(HwDeviceExtension->jChipType, XGI_Pr);
 
+	XGI_SearchModeID(ModeNo, &ModeIdIndex, XGI_Pr);
+
 	RefreshRateTableIndex = XGI_GetRatePtrCRT2(HwDeviceExtension, ModeNo,
 			ModeIdIndex, XGI_Pr);
 
-	/*
-	temp = XGI_SearchModeID(ModeNo , &ModeIdIndex,  XGI_Pr) ;
-	if (!temp) {
-		printk(KERN_ERR "Could not find mode %x\n", ModeNo);
-		return 65000;
-	}
-
-	RefreshRateTableIndex = XGI_Pr->EModeIDTable[ModeIdIndex].REFindex;
-	RefreshRateTableIndex += (rateindex - 1);
-
-	*/
 	ClockIndex = XGI_Pr->RefIndex[RefreshRateTableIndex].Ext_CRTVCLK;
 
 	Clock = XGI_Pr->VCLKData[ClockIndex].CLOCK * 1000;
@@ -190,7 +179,7 @@ static int XGIfb_mode_rate_to_ddata(struct vb_device_info *XGI_Pr,
 		u32 *vmode)
 {
 	unsigned short ModeNo = modeno;
-	unsigned short ModeIdIndex = 0, index = 0;
+	unsigned short ModeIdIndex, index = 0;
 	unsigned short RefreshRateTableIndex = 0;
 
 	unsigned short VRE, VBE, VRS, VBS, VDE, VT;
@@ -199,16 +188,10 @@ static int XGIfb_mode_rate_to_ddata(struct vb_device_info *XGI_Pr,
 	unsigned long cr_data3;
 	int A, B, C, D, E, F, temp, j;
 	InitTo330Pointer(HwDeviceExtension->jChipType, XGI_Pr);
+	if (!XGI_SearchModeID(ModeNo, &ModeIdIndex, XGI_Pr))
+		return 0;
 	RefreshRateTableIndex = XGI_GetRatePtrCRT2(HwDeviceExtension, ModeNo,
 			ModeIdIndex, XGI_Pr);
-	/*
-	temp = XGI_SearchModeID(ModeNo, &ModeIdIndex, XGI_Pr);
-	if (!temp)
-		return 0;
-
-	RefreshRateTableIndex = XGI_Pr->EModeIDTable[ModeIdIndex].REFindex;
-	RefreshRateTableIndex += (rateindex - 1);
-	*/
 	index = XGI_Pr->RefIndex[RefreshRateTableIndex].Ext_CRT1CRTC;
 
 	sr_data = XGI_Pr->XGINEWUB_CRT1Table[index].CR[5];
@@ -219,12 +202,6 @@ static int XGIfb_mode_rate_to_ddata(struct vb_device_info *XGI_Pr,
 	HT = (cr_data & 0xff) | ((unsigned short) (sr_data & 0x03) << 8);
 	A = HT + 5;
 
-	/*
-	cr_data = XGI_Pr->XGINEWUB_CRT1Table[index].CR[1];
-
-	Horizontal display enable end
-	HDE = (cr_data & 0xff) | ((unsigned short) (sr_data & 0x0C) << 6);
-	*/
 	HDE = (XGI_Pr->RefIndex[RefreshRateTableIndex].XRes >> 3) - 1;
 	E = HDE + 1;
 
@@ -358,7 +335,6 @@ static int XGIfb_mode_rate_to_ddata(struct vb_device_info *XGI_Pr,
 
 static void XGIRegInit(struct vb_device_info *XGI_Pr, unsigned long BaseAddr)
 {
-	XGI_Pr->RelIO = BaseAddr;
 	XGI_Pr->P3c4 = BaseAddr + 0x14;
 	XGI_Pr->P3d4 = BaseAddr + 0x24;
 	XGI_Pr->P3c0 = BaseAddr + 0x10;
@@ -414,19 +390,26 @@ static int XGIfb_GetXG21DefaultLVDSModeIdx(struct xgifb_video_info *xgifb_info)
 static void XGIfb_search_mode(struct xgifb_video_info *xgifb_info,
 			      const char *name)
 {
-	int i = 0, j = 0, l;
+	unsigned int xres;
+	unsigned int yres;
+	unsigned int bpp;
+	int i;
 
-	while (XGIbios_mode[i].mode_no != 0) {
-		l = min(strlen(name), strlen(XGIbios_mode[i].name));
-		if (!strncmp(name, XGIbios_mode[i].name, l)) {
+	if (sscanf(name, "%ux%ux%u", &xres, &yres, &bpp) != 3)
+		goto invalid_mode;
+
+	if (bpp == 24)
+		bpp = 32; /* That's for people who mix up color and fb depth. */
+
+	for (i = 0; XGIbios_mode[i].mode_no != 0; i++)
+		if (XGIbios_mode[i].xres == xres &&
+		    XGIbios_mode[i].yres == yres &&
+		    XGIbios_mode[i].bpp == bpp) {
 			xgifb_info->mode_idx = i;
-			j = 1;
-			break;
+			return;
 		}
-		i++;
-	}
-	if (!j)
-		pr_info("Invalid mode '%s'\n", name);
+invalid_mode:
+	pr_info("Invalid mode '%s'\n", name);
 }
 
 static void XGIfb_search_vesamode(struct xgifb_video_info *xgifb_info,
@@ -1088,7 +1071,7 @@ static int XGIfb_do_set_var(struct fb_var_screeninfo *var, int isactive,
 	unsigned int vtotal = var->upper_margin + var->yres + var->lower_margin
 			+ var->vsync_len;
 #if defined(__powerpc__)
-	u8 sr_data, cr_data;
+	u8 cr_data;
 #endif
 	unsigned int drate = 0, hrate = 0;
 	int found_mode = 0;
@@ -1162,8 +1145,7 @@ static int XGIfb_do_set_var(struct fb_var_screeninfo *var, int isactive,
 
 	if (XGIfb_search_refresh_rate(xgifb_info,
 				      xgifb_info->refresh_rate) == 0) {
-		xgifb_info->rate_idx =
-			XGIbios_mode[xgifb_info->mode_idx].rate_idx;
+		xgifb_info->rate_idx = 1;
 		xgifb_info->refresh_rate = 60;
 	}
 
@@ -1680,17 +1662,6 @@ static int XGIfb_get_dram_size(struct xgifb_video_info *xgifb_info)
 			ChannelNum = 1;
 		break;
 
-	case XG45:
-		if (tmp == 1)
-			ChannelNum = 2;
-		else if (tmp == 2)
-			ChannelNum = 3;
-		else if (tmp == 3)
-			ChannelNum = 4;
-		else
-			ChannelNum = 1;
-		break;
-
 	case XG40:
 	default:
 		if (tmp == 2)
@@ -1911,11 +1882,9 @@ static int __devinit xgifb_probe(struct pci_dev *pdev,
 	xgifb_info->mmio_base = pci_resource_start(pdev, 1);
 	xgifb_info->mmio_size = pci_resource_len(pdev, 1);
 	xgifb_info->vga_base = pci_resource_start(pdev, 2) + 0x30;
-	hw_info->pjIOAddress = (unsigned char *)xgifb_info->vga_base;
-	/* XGI_Pr.RelIO  = ioremap(pci_resource_start(pdev, 2), 128) + 0x30; */
-	pr_info("Relocate IO address: %lx [%08lx]\n",
-	       (unsigned long)pci_resource_start(pdev, 2),
-	       xgifb_info->dev_info.RelIO);
+	pr_info("Relocate IO address: %Lx [%08lx]\n",
+	       (u64) pci_resource_start(pdev, 2),
+	       xgifb_info->vga_base);
 
 	if (pci_enable_device(pdev)) {
 		ret = -EIO;
@@ -1927,7 +1896,7 @@ static int __devinit xgifb_probe(struct pci_dev *pdev,
 		xgifb_info->display2_force = true;
 	}
 
-	XGIRegInit(&xgifb_info->dev_info, (unsigned long)hw_info->pjIOAddress);
+	XGIRegInit(&xgifb_info->dev_info, xgifb_info->vga_base);
 
 	xgifb_reg_set(XGISR, IND_SIS_PASSWORD, SIS_PASSWORD);
 	reg1 = xgifb_reg_get(XGISR, IND_SIS_PASSWORD);
@@ -1950,9 +1919,6 @@ static int __devinit xgifb_probe(struct pci_dev *pdev,
 	case PCI_DEVICE_ID_XGI_40:
 		xgifb_info->chip = XG40;
 		break;
-	case PCI_DEVICE_ID_XGI_41:
-		xgifb_info->chip = XG41;
-		break;
 	case PCI_DEVICE_ID_XGI_42:
 		xgifb_info->chip = XG42;
 		break;
@@ -2006,13 +1972,13 @@ static int __devinit xgifb_probe(struct pci_dev *pdev,
 	xgifb_info->mmio_vbase = ioremap(xgifb_info->mmio_base,
 					    xgifb_info->mmio_size);
 
-	pr_info("Framebuffer at 0x%lx, mapped to 0x%p, size %dk\n",
-	       xgifb_info->video_base,
+	pr_info("Framebuffer at 0x%Lx, mapped to 0x%p, size %dk\n",
+	       (u64) xgifb_info->video_base,
 	       xgifb_info->video_vbase,
 	       xgifb_info->video_size / 1024);
 
-	pr_info("MMIO at 0x%lx, mapped to 0x%p, size %ldk\n",
-	       xgifb_info->mmio_base, xgifb_info->mmio_vbase,
+	pr_info("MMIO at 0x%Lx, mapped to 0x%p, size %ldk\n",
+	       (u64) xgifb_info->mmio_base, xgifb_info->mmio_vbase,
 	       xgifb_info->mmio_size / 1024);
 
 	pci_set_drvdata(pdev, xgifb_info);
@@ -2174,8 +2140,7 @@ static int __devinit xgifb_probe(struct pci_dev *pdev,
 		xgifb_info->refresh_rate = 60;
 	if (XGIfb_search_refresh_rate(xgifb_info,
 			xgifb_info->refresh_rate) == 0) {
-		xgifb_info->rate_idx =
-			XGIbios_mode[xgifb_info->mode_idx].rate_idx;
+		xgifb_info->rate_idx = 1;
 		xgifb_info->refresh_rate = 60;
 	}
 
diff --git a/drivers/staging/xgifb/XGIfb.h b/drivers/staging/xgifb/XGIfb.h
index 37bb730de04..9068c5ad76e 100644
--- a/drivers/staging/xgifb/XGIfb.h
+++ b/drivers/staging/xgifb/XGIfb.h
@@ -23,9 +23,7 @@ enum xgifb_display_type {
 
 enum XGI_CHIP_TYPE {
 	XG40 = 32,
-	XG41,
 	XG42,
-	XG45,
 	XG20 = 48,
 	XG21,
 	XG27,
@@ -66,9 +64,9 @@ struct xgifb_video_info {
 
 	int           chip_id;
 	unsigned int  video_size;
-	unsigned long video_base;
+	phys_addr_t   video_base;
 	void __iomem *video_vbase;
-	unsigned long mmio_base;
+	phys_addr_t   mmio_base;
 	unsigned long mmio_size;
 	void __iomem *mmio_vbase;
 	unsigned long vga_base;
diff --git a/drivers/staging/xgifb/vb_init.c b/drivers/staging/xgifb/vb_init.c
index 3650bbff768..c222d611431 100644
--- a/drivers/staging/xgifb/vb_init.c
+++ b/drivers/staging/xgifb/vb_init.c
@@ -353,7 +353,6 @@ static void XGINew_DDR1x_DefaultRegister(
 		XGINew_SetMemoryClock(HwDeviceExtension, pVBInfo);
 
 		switch (HwDeviceExtension->jChipType) {
-		case XG41:
 		case XG42:
 			/* CR82 */
 			xgifb_reg_set(P3d4,
@@ -556,8 +555,7 @@ static void XGINew_SetDRAMDefaultRegister340(
 		xgifb_reg_set(P3d4, (0x8A + j),
 				pVBInfo->CR40[1 + j][pVBInfo->ram_type]);
 
-	if ((HwDeviceExtension->jChipType == XG41) ||
-	    (HwDeviceExtension->jChipType == XG42))
+	if (HwDeviceExtension->jChipType == XG42)
 		xgifb_reg_set(P3d4, 0x8C, 0x87);
 
 	xgifb_reg_set(P3d4,
@@ -854,78 +852,6 @@ static void XGINew_CheckChannel(struct xgi_hw_device_info *HwDeviceExtension,
 		pVBInfo->ram_channel = 1; /* Single channel */
 		xgifb_reg_set(pVBInfo->P3c4, 0x14, 0x51); /* 32Mx16 bit*/
 		break;
-	case XG41:
-		if (XGINew_CheckFrequence(pVBInfo) == 1) {
-			pVBInfo->ram_bus = 32; /* 32 bits */
-			pVBInfo->ram_channel = 3; /* Quad Channel */
-			xgifb_reg_set(pVBInfo->P3c4, 0x13, 0xA1);
-			xgifb_reg_set(pVBInfo->P3c4, 0x14, 0x4C);
-
-			if (XGINew_ReadWriteRest(25, 23, pVBInfo) == 1)
-				return;
-
-			pVBInfo->ram_channel = 2; /* Dual channels */
-			xgifb_reg_set(pVBInfo->P3c4, 0x14, 0x48);
-
-			if (XGINew_ReadWriteRest(24, 23, pVBInfo) == 1)
-				return;
-
-			xgifb_reg_set(pVBInfo->P3c4, 0x14, 0x49);
-
-			if (XGINew_ReadWriteRest(24, 23, pVBInfo) == 1)
-				return;
-
-			pVBInfo->ram_channel = 3;
-			xgifb_reg_set(pVBInfo->P3c4, 0x13, 0x21);
-			xgifb_reg_set(pVBInfo->P3c4, 0x14, 0x3C);
-
-			if (XGINew_ReadWriteRest(24, 23, pVBInfo) == 1)
-				return;
-
-			xgifb_reg_set(pVBInfo->P3c4, 0x14, 0x38);
-
-			if (XGINew_ReadWriteRest(8, 4, pVBInfo) == 1)
-				return;
-			else
-				xgifb_reg_set(pVBInfo->P3c4, 0x14, 0x39);
-		} else { /* DDR */
-			pVBInfo->ram_bus = 64; /* 64 bits */
-			pVBInfo->ram_channel = 2; /* Dual channels */
-			xgifb_reg_set(pVBInfo->P3c4, 0x13, 0xA1);
-			xgifb_reg_set(pVBInfo->P3c4, 0x14, 0x5A);
-
-			if (XGINew_ReadWriteRest(25, 24, pVBInfo) == 1)
-				return;
-
-			pVBInfo->ram_channel = 1; /* Single channels */
-			xgifb_reg_set(pVBInfo->P3c4, 0x14, 0x52);
-
-			if (XGINew_ReadWriteRest(24, 23, pVBInfo) == 1)
-				return;
-
-			xgifb_reg_set(pVBInfo->P3c4, 0x14, 0x53);
-
-			if (XGINew_ReadWriteRest(24, 23, pVBInfo) == 1)
-				return;
-
-			pVBInfo->ram_channel = 2; /* Dual channels */
-			xgifb_reg_set(pVBInfo->P3c4, 0x13, 0x21);
-			xgifb_reg_set(pVBInfo->P3c4, 0x14, 0x4A);
-
-			if (XGINew_ReadWriteRest(24, 23, pVBInfo) == 1)
-				return;
-
-			pVBInfo->ram_channel = 1; /* Single channels */
-			xgifb_reg_set(pVBInfo->P3c4, 0x14, 0x42);
-
-			if (XGINew_ReadWriteRest(8, 4, pVBInfo) == 1)
-				return;
-			else
-				xgifb_reg_set(pVBInfo->P3c4, 0x14, 0x43);
-		}
-
-		break;
-
 	case XG42:
 		/*
 		 XG42 SR14 D[3] Reserve
@@ -1478,7 +1404,7 @@ unsigned char XGIInitNew(struct pci_dev *pdev)
 
 	pVBInfo->FBAddr = HwDeviceExtension->pjVideoMemoryAddress;
 
-	pVBInfo->BaseAddr = (unsigned long) HwDeviceExtension->pjIOAddress;
+	pVBInfo->BaseAddr = xgifb_info->vga_base;
 
 	/* Newdebugcode(0x99); */
 
diff --git a/drivers/staging/xgifb/vb_setmode.c b/drivers/staging/xgifb/vb_setmode.c
index 60d4adf9992..b2f4338b110 100644
--- a/drivers/staging/xgifb/vb_setmode.c
+++ b/drivers/staging/xgifb/vb_setmode.c
@@ -16,36 +16,6 @@
 
 #define  IndexMask 0xff
 
-static const unsigned short XGINew_MDA_DAC[] = {
-	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
-	0x15, 0x15, 0x15, 0x15, 0x15, 0x15, 0x15, 0x15,
-	0x15, 0x15, 0x15, 0x15, 0x15, 0x15, 0x15, 0x15,
-	0x3F, 0x3F, 0x3F, 0x3F, 0x3F, 0x3F, 0x3F, 0x3F,
-	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
-	0x15, 0x15, 0x15, 0x15, 0x15, 0x15, 0x15, 0x15,
-	0x15, 0x15, 0x15, 0x15, 0x15, 0x15, 0x15, 0x15,
-	0x3F, 0x3F, 0x3F, 0x3F, 0x3F, 0x3F, 0x3F, 0x3F};
-
-static const unsigned short XGINew_CGA_DAC[] = {
-	0x00, 0x10, 0x04, 0x14, 0x01, 0x11, 0x09, 0x15,
-	0x00, 0x10, 0x04, 0x14, 0x01, 0x11, 0x09, 0x15,
-	0x2A, 0x3A, 0x2E, 0x3E, 0x2B, 0x3B, 0x2F, 0x3F,
-	0x2A, 0x3A, 0x2E, 0x3E, 0x2B, 0x3B, 0x2F, 0x3F,
-	0x00, 0x10, 0x04, 0x14, 0x01, 0x11, 0x09, 0x15,
-	0x00, 0x10, 0x04, 0x14, 0x01, 0x11, 0x09, 0x15,
-	0x2A, 0x3A, 0x2E, 0x3E, 0x2B, 0x3B, 0x2F, 0x3F,
-	0x2A, 0x3A, 0x2E, 0x3E, 0x2B, 0x3B, 0x2F, 0x3F};
-
-static const unsigned short XGINew_EGA_DAC[] = {
-	0x00, 0x10, 0x04, 0x14, 0x01, 0x11, 0x05, 0x15,
-	0x20, 0x30, 0x24, 0x34, 0x21, 0x31, 0x25, 0x35,
-	0x08, 0x18, 0x0C, 0x1C, 0x09, 0x19, 0x0D, 0x1D,
-	0x28, 0x38, 0x2C, 0x3C, 0x29, 0x39, 0x2D, 0x3D,
-	0x02, 0x12, 0x06, 0x16, 0x03, 0x13, 0x07, 0x17,
-	0x22, 0x32, 0x26, 0x36, 0x23, 0x33, 0x27, 0x37,
-	0x0A, 0x1A, 0x0E, 0x1E, 0x0B, 0x1B, 0x0F, 0x1F,
-	0x2A, 0x3A, 0x2E, 0x3E, 0x2B, 0x3B, 0x2F, 0x3F};
-
 static const unsigned short XGINew_VGA_DAC[] = {
 	0x00, 0x10, 0x04, 0x14, 0x01, 0x11, 0x09, 0x15,
 	0x2A, 0x3A, 0x2E, 0x3E, 0x2B, 0x3B, 0x2F, 0x3F,
@@ -60,8 +30,7 @@ static const unsigned short XGINew_VGA_DAC[] = {
 
 void InitTo330Pointer(unsigned char ChipType, struct vb_device_info *pVBInfo)
 {
-	pVBInfo->SModeIDTable = (struct XGI_StStruct *) XGI330_SModeIDTable;
-	pVBInfo->StandTable = (struct SiS_StandTable_S *) XGI330_StandTable;
+	pVBInfo->StandTable = (struct SiS_StandTable_S *) &XGI330_StandTable;
 	pVBInfo->EModeIDTable = (struct XGI_ExtStruct *) XGI330_EModeIDTable;
 	pVBInfo->RefIndex = (struct XGI_Ext2Struct *) XGI330_RefIndex;
 	pVBInfo->XGINEWUB_CRT1Table
@@ -182,38 +151,17 @@ void InitTo330Pointer(unsigned char ChipType, struct vb_device_info *pVBInfo)
 
 }
 
-static unsigned char XGI_GetModePtr(unsigned short ModeNo,
-				    unsigned short ModeIdIndex,
-				    struct vb_device_info *pVBInfo)
-{
-	unsigned char index;
-
-	if (ModeNo <= 0x13)
-		index = pVBInfo->SModeIDTable[ModeIdIndex].St_StTableIndex;
-	else {
-		if (pVBInfo->ModeType <= 0x02)
-			index = 0x1B; /* 02 -> ModeEGA */
-		else
-			index = 0x0F;
-	}
-	return index; /* Get pVBInfo->StandTable index */
-}
-
 static void XGI_SetSeqRegs(unsigned short ModeNo,
-			   unsigned short StandTableIndex,
 			   unsigned short ModeIdIndex,
 			   struct vb_device_info *pVBInfo)
 {
 	unsigned char tempah, SRdata;
 	unsigned short i, modeflag;
 
-	if (ModeNo <= 0x13)
-		modeflag = pVBInfo->SModeIDTable[ModeIdIndex].St_ModeFlag;
-	else
-		modeflag = pVBInfo->EModeIDTable[ModeIdIndex].Ext_ModeFlag;
+	modeflag = pVBInfo->EModeIDTable[ModeIdIndex].Ext_ModeFlag;
 
 	xgifb_reg_set(pVBInfo->P3c4, 0x00, 0x03); /* Set SR0 */
-	tempah = pVBInfo->StandTable[StandTableIndex].SR[0];
+	tempah = pVBInfo->StandTable->SR[0];
 
 	i = XGI_SetCRT2ToLCDA;
 	if (pVBInfo->VBInfo & XGI_SetCRT2ToLCDA) {
@@ -230,13 +178,12 @@ static void XGI_SetSeqRegs(unsigned short ModeNo,
 
 	for (i = 02; i <= 04; i++) {
 		/* Get SR2,3,4 from file */
-		SRdata = pVBInfo->StandTable[StandTableIndex].SR[i - 1];
+		SRdata = pVBInfo->StandTable->SR[i - 1];
 		xgifb_reg_set(pVBInfo->P3c4, i, SRdata); /* Set SR2 3 4 */
 	}
 }
 
 static void XGI_SetCRTCRegs(struct xgi_hw_device_info *HwDeviceExtension,
-			    unsigned short StandTableIndex,
 			    struct vb_device_info *pVBInfo)
 {
 	unsigned char CRTCdata;
@@ -248,26 +195,22 @@ static void XGI_SetCRTCRegs(struct xgi_hw_device_info *HwDeviceExtension,
 
 	for (i = 0; i <= 0x18; i++) {
 		/* Get CRTC from file */
-		CRTCdata = pVBInfo->StandTable[StandTableIndex].CRTC[i];
+		CRTCdata = pVBInfo->StandTable->CRTC[i];
 		xgifb_reg_set(pVBInfo->P3d4, i, CRTCdata); /* Set CRTC(3d4) */
 	}
 }
 
 static void XGI_SetATTRegs(unsigned short ModeNo,
-			   unsigned short StandTableIndex,
 			   unsigned short ModeIdIndex,
 			   struct vb_device_info *pVBInfo)
 {
 	unsigned char ARdata;
 	unsigned short i, modeflag;
 
-	if (ModeNo <= 0x13)
-		modeflag = pVBInfo->SModeIDTable[ModeIdIndex].St_ModeFlag;
-	else
-		modeflag = pVBInfo->EModeIDTable[ModeIdIndex].Ext_ModeFlag;
+	modeflag = pVBInfo->EModeIDTable[ModeIdIndex].Ext_ModeFlag;
 
 	for (i = 0; i <= 0x13; i++) {
-		ARdata = pVBInfo->StandTable[StandTableIndex].ATTR[i];
+		ARdata = pVBInfo->StandTable->ATTR[i];
 		if (modeflag & Charx8Dot) { /* ifndef Dot9 */
 			if (i == 0x13) {
 				if (pVBInfo->VBInfo & XGI_SetCRT2ToLCDA) {
@@ -295,15 +238,14 @@ static void XGI_SetATTRegs(unsigned short ModeNo,
 	outb(0x20, pVBInfo->P3c0);
 }
 
-static void XGI_SetGRCRegs(unsigned short StandTableIndex,
-			   struct vb_device_info *pVBInfo)
+static void XGI_SetGRCRegs(struct vb_device_info *pVBInfo)
 {
 	unsigned char GRdata;
 	unsigned short i;
 
 	for (i = 0; i <= 0x08; i++) {
 		/* Get GR from file */
-		GRdata = pVBInfo->StandTable[StandTableIndex].GRC[i];
+		GRdata = pVBInfo->StandTable->GRC[i];
 		xgifb_reg_set(pVBInfo->P3ce, i, GRdata); /* Set GR(3ce) */
 	}
 
@@ -344,12 +286,7 @@ static unsigned char XGI_AjustCRT2Rate(unsigned short ModeNo,
 {
 	unsigned short tempax, tempbx, resinfo, modeflag, infoflag;
 
-	if (ModeNo <= 0x13)
-		/* si+St_ModeFlag */
-		modeflag = pVBInfo->SModeIDTable[ModeIdIndex].St_ModeFlag;
-	else
-		modeflag = pVBInfo->EModeIDTable[ModeIdIndex].Ext_ModeFlag;
-
+	modeflag = pVBInfo->EModeIDTable[ModeIdIndex].Ext_ModeFlag;
 	resinfo = pVBInfo->EModeIDTable[ModeIdIndex].Ext_RESINFO;
 	tempbx = pVBInfo->RefIndex[RefreshRateTableIndex + (*i)].ModeID;
 	tempax = 0;
@@ -584,11 +521,7 @@ static void XGI_SetCRT1Timing_V(unsigned short ModeIdIndex,
 	data &= 0x80;
 	data = data >> 2;
 
-	if (ModeNo <= 0x13)
-		i = pVBInfo->SModeIDTable[ModeIdIndex].St_ModeFlag;
-	else
-		i = pVBInfo->EModeIDTable[ModeIdIndex].Ext_ModeFlag;
-
+	i = pVBInfo->EModeIDTable[ModeIdIndex].Ext_ModeFlag;
 	i &= DoubleScanMode;
 	if (i)
 		data |= 0x80;
@@ -641,158 +574,97 @@ static void XGI_SetXG21CRTC(unsigned short ModeNo, unsigned short ModeIdIndex,
 		unsigned short RefreshRateTableIndex,
 		struct vb_device_info *pVBInfo)
 {
-	unsigned char StandTableIndex, index, Tempax, Tempbx, Tempcx, Tempdx;
+	unsigned char index, Tempax, Tempbx, Tempcx, Tempdx;
 	unsigned short Temp1, Temp2, Temp3;
 
-	if (ModeNo <= 0x13) {
-		StandTableIndex = XGI_GetModePtr(ModeNo, ModeIdIndex, pVBInfo);
-		/* CR04 HRS */
-		Tempax = pVBInfo->StandTable[StandTableIndex].CRTC[4];
-		/* SR2E [7:0]->HRS */
-		xgifb_reg_set(pVBInfo->P3c4, 0x2E, Tempax);
-		/* Tempbx: CR05 HRE */
-		Tempbx = pVBInfo->StandTable[StandTableIndex].CRTC[5];
-		Tempbx &= 0x1F; /* Tempbx: HRE[4:0] */
-		Tempcx = Tempax;
-		Tempcx &= 0xE0; /* Tempcx: HRS[7:5] */
-		Tempdx = Tempcx | Tempbx; /* Tempdx(HRE): HRS[7:5]HRE[4:0] */
-		if (Tempbx < (Tempax & 0x1F)) /* IF HRE < HRS */
-			Tempdx |= 0x20; /* Tempdx: HRE = HRE + 0x20 */
-		Tempdx <<= 2; /* Tempdx << 2 */
-		/* SR2F [7:2]->HRE */
-		xgifb_reg_set(pVBInfo->P3c4, 0x2F, Tempdx);
-		xgifb_reg_and_or(pVBInfo->P3c4, 0x30, 0xE3, 00);
-
-		/* Tempax: CR16 VRS */
-		Tempax = pVBInfo->StandTable[StandTableIndex].CRTC[16];
-		Tempbx = Tempax; /* Tempbx=Tempax */
-		Tempax &= 0x01; /* Tempax: VRS[0] */
-		xgifb_reg_or(pVBInfo->P3c4, 0x33, Tempax); /* SR33[0]->VRS */
-
-		/* Tempax: CR7 VRS */
-		Tempax = pVBInfo->StandTable[StandTableIndex].CRTC[7];
-		Tempdx = Tempbx >> 1; /* Tempdx: VRS[7:1] */
-		Tempcx = Tempax & 0x04; /* Tempcx: CR7[2] */
-		Tempcx <<= 5; /* Tempcx[7]: VRS[8] */
-		Tempdx |= Tempcx; /* Tempdx: VRS[8:1] */
-		/* SR34[7:0]: VRS[8:1] */
-		xgifb_reg_set(pVBInfo->P3c4, 0x34, Tempdx);
-
-		/* Temp1[8]: VRS[8] unsigned char -> unsigned short */
-		Temp1 = Tempcx << 1;
-		Temp1 |= Tempbx; /* Temp1[8:0]: VRS[8:0] */
-		Tempax &= 0x80; /* Tempax[7]: CR7[7] */
-		Temp2 = Tempax << 2; /* Temp2[9]: VRS[9] */
-		Temp1 |= Temp2; /* Temp1[9:0]: VRS[9:0] */
-
-		/* CR16 VRE */
-		Tempax = pVBInfo->StandTable[StandTableIndex].CRTC[17];
-		Tempax &= 0x0F; /* Tempax[3:0]: VRE[3:0] */
-		Temp2 = Temp1 & 0x3F0; /* Temp2[9:4]: VRS[9:4] */
-		Temp2 |= Tempax; /* Temp2[9:0]: VRE[9:0] */
-		Temp3 = Temp1 & 0x0F; /* Temp3[3:0]: VRS[3:0] */
-		if (Tempax < Temp3) /* VRE[3:0]<VRS[3:0] */
-			Temp2 |= 0x10; /* Temp2: VRE + 0x10 */
-		Temp2 &= 0xFF; /* Temp2[7:0]: VRE[7:0] */
-		Tempax = (unsigned char) Temp2; /* Tempax[7:0]: VRE[7:0] */
-		Tempax <<= 2; /* Tempax << 2: VRE[5:0] */
-		Temp1 &= 0x600; /* Temp1[10:9]: VRS[10:9] */
-		Temp1 >>= 9; /* [10:9]->[1:0] */
-		Tempbx = (unsigned char) Temp1; /* Tempbx[1:0]: VRS[10:9] */
-		Tempax |= Tempbx; /* VRE[5:0]VRS[10:9] */
-		Tempax &= 0x7F;
-		/* SR3F D[7:2]->VRE D[1:0]->VRS */
-		xgifb_reg_set(pVBInfo->P3c4, 0x3F, Tempax);
-	} else {
-		index = pVBInfo->RefIndex[RefreshRateTableIndex].Ext_CRT1CRTC;
-		/* Tempax: CR4 HRS */
-		Tempax = pVBInfo->XGINEWUB_CRT1Table[index].CR[3];
-		Tempcx = Tempax; /* Tempcx: HRS */
-		/* SR2E[7:0]->HRS */
-		xgifb_reg_set(pVBInfo->P3c4, 0x2E, Tempax);
-
-		Tempdx = pVBInfo->XGINEWUB_CRT1Table[index].CR[5]; /* SRB */
-		Tempdx &= 0xC0; /* Tempdx[7:6]: SRB[7:6] */
-		Temp1 = Tempdx; /* Temp1[7:6]: HRS[9:8] */
-		Temp1 <<= 2; /* Temp1[9:8]: HRS[9:8] */
-		Temp1 |= Tempax; /* Temp1[9:0]: HRS[9:0] */
-
-		Tempax = pVBInfo->XGINEWUB_CRT1Table[index].CR[4]; /* CR5 HRE */
-		Tempax &= 0x1F; /* Tempax[4:0]: HRE[4:0] */
-
-		Tempbx = pVBInfo->XGINEWUB_CRT1Table[index].CR[6]; /* SRC */
-		Tempbx &= 0x04; /* Tempbx[2]: HRE[5] */
-		Tempbx <<= 3; /* Tempbx[5]: HRE[5] */
-		Tempax |= Tempbx; /* Tempax[5:0]: HRE[5:0] */
-
-		Temp2 = Temp1 & 0x3C0; /* Temp2[9:6]: HRS[9:6] */
-		Temp2 |= Tempax; /* Temp2[9:0]: HRE[9:0] */
-
-		Tempcx &= 0x3F; /* Tempcx[5:0]: HRS[5:0] */
-		if (Tempax < Tempcx) /* HRE < HRS */
-			Temp2 |= 0x40; /* Temp2 + 0x40 */
-
-		Temp2 &= 0xFF;
-		Tempax = (unsigned char) Temp2; /* Tempax: HRE[7:0] */
-		Tempax <<= 2; /* Tempax[7:2]: HRE[5:0] */
-		Tempdx >>= 6; /* Tempdx[7:6]->[1:0] HRS[9:8] */
-		Tempax |= Tempdx; /* HRE[5:0]HRS[9:8] */
-		/* SR2F D[7:2]->HRE, D[1:0]->HRS */
-		xgifb_reg_set(pVBInfo->P3c4, 0x2F, Tempax);
-		xgifb_reg_and_or(pVBInfo->P3c4, 0x30, 0xE3, 00);
-
-		/* CR10 VRS */
-		Tempax = pVBInfo->XGINEWUB_CRT1Table[index].CR[10];
-		Tempbx = Tempax; /* Tempbx: VRS */
-		Tempax &= 0x01; /* Tempax[0]: VRS[0] */
-		xgifb_reg_or(pVBInfo->P3c4, 0x33, Tempax); /* SR33[0]->VRS[0] */
-		/* CR7[2][7] VRE */
-		Tempax = pVBInfo->XGINEWUB_CRT1Table[index].CR[9];
-		Tempcx = Tempbx >> 1; /* Tempcx[6:0]: VRS[7:1] */
-		Tempdx = Tempax & 0x04; /* Tempdx[2]: CR7[2] */
-		Tempdx <<= 5; /* Tempdx[7]: VRS[8] */
-		Tempcx |= Tempdx; /* Tempcx[7:0]: VRS[8:1] */
-		xgifb_reg_set(pVBInfo->P3c4, 0x34, Tempcx); /* SR34[8:1]->VRS */
-
-		Temp1 = Tempdx; /* Temp1[7]: Tempdx[7] */
-		Temp1 <<= 1; /* Temp1[8]: VRS[8] */
-		Temp1 |= Tempbx; /* Temp1[8:0]: VRS[8:0] */
-		Tempax &= 0x80;
-		Temp2 = Tempax << 2; /* Temp2[9]: VRS[9] */
-		Temp1 |= Temp2; /* Temp1[9:0]: VRS[9:0] */
-		/* Tempax: SRA */
-		Tempax = pVBInfo->XGINEWUB_CRT1Table[index].CR[14];
-		Tempax &= 0x08; /* Tempax[3]: VRS[3] */
-		Temp2 = Tempax;
-		Temp2 <<= 7; /* Temp2[10]: VRS[10] */
-		Temp1 |= Temp2; /* Temp1[10:0]: VRS[10:0] */
-
-		/* Tempax: CR11 VRE */
-		Tempax = pVBInfo->XGINEWUB_CRT1Table[index].CR[11];
-		Tempax &= 0x0F; /* Tempax[3:0]: VRE[3:0] */
-		/* Tempbx: SRA */
-		Tempbx = pVBInfo->XGINEWUB_CRT1Table[index].CR[14];
-		Tempbx &= 0x20; /* Tempbx[5]: VRE[5] */
-		Tempbx >>= 1; /* Tempbx[4]: VRE[4] */
-		Tempax |= Tempbx; /* Tempax[4:0]: VRE[4:0] */
-		Temp2 = Temp1 & 0x7E0; /* Temp2[10:5]: VRS[10:5] */
-		Temp2 |= Tempax; /* Temp2[10:5]: VRE[10:5] */
-
-		Temp3 = Temp1 & 0x1F; /* Temp3[4:0]: VRS[4:0] */
-		if (Tempax < Temp3) /* VRE < VRS */
-			Temp2 |= 0x20; /* VRE + 0x20 */
-
-		Temp2 &= 0xFF;
-		Tempax = (unsigned char) Temp2; /* Tempax: VRE[7:0] */
-		Tempax <<= 2; /* Tempax[7:0]; VRE[5:0]00 */
-		Temp1 &= 0x600; /* Temp1[10:9]: VRS[10:9] */
-		Temp1 >>= 9; /* Temp1[1:0]: VRS[10:9] */
-		Tempbx = (unsigned char) Temp1;
-		Tempax |= Tempbx; /* Tempax[7:0]: VRE[5:0]VRS[10:9] */
-		Tempax &= 0x7F;
-		/* SR3F D[7:2]->VRE D[1:0]->VRS */
-		xgifb_reg_set(pVBInfo->P3c4, 0x3F, Tempax);
-	}
+	index = pVBInfo->RefIndex[RefreshRateTableIndex].Ext_CRT1CRTC;
+	/* Tempax: CR4 HRS */
+	Tempax = pVBInfo->XGINEWUB_CRT1Table[index].CR[3];
+	Tempcx = Tempax; /* Tempcx: HRS */
+	/* SR2E[7:0]->HRS */
+	xgifb_reg_set(pVBInfo->P3c4, 0x2E, Tempax);
+
+	Tempdx = pVBInfo->XGINEWUB_CRT1Table[index].CR[5]; /* SRB */
+	Tempdx &= 0xC0; /* Tempdx[7:6]: SRB[7:6] */
+	Temp1 = Tempdx; /* Temp1[7:6]: HRS[9:8] */
+	Temp1 <<= 2; /* Temp1[9:8]: HRS[9:8] */
+	Temp1 |= Tempax; /* Temp1[9:0]: HRS[9:0] */
+
+	Tempax = pVBInfo->XGINEWUB_CRT1Table[index].CR[4]; /* CR5 HRE */
+	Tempax &= 0x1F; /* Tempax[4:0]: HRE[4:0] */
+
+	Tempbx = pVBInfo->XGINEWUB_CRT1Table[index].CR[6]; /* SRC */
+	Tempbx &= 0x04; /* Tempbx[2]: HRE[5] */
+	Tempbx <<= 3; /* Tempbx[5]: HRE[5] */
+	Tempax |= Tempbx; /* Tempax[5:0]: HRE[5:0] */
+
+	Temp2 = Temp1 & 0x3C0; /* Temp2[9:6]: HRS[9:6] */
+	Temp2 |= Tempax; /* Temp2[9:0]: HRE[9:0] */
+
+	Tempcx &= 0x3F; /* Tempcx[5:0]: HRS[5:0] */
+	if (Tempax < Tempcx) /* HRE < HRS */
+		Temp2 |= 0x40; /* Temp2 + 0x40 */
+
+	Temp2 &= 0xFF;
+	Tempax = (unsigned char) Temp2; /* Tempax: HRE[7:0] */
+	Tempax <<= 2; /* Tempax[7:2]: HRE[5:0] */
+	Tempdx >>= 6; /* Tempdx[7:6]->[1:0] HRS[9:8] */
+	Tempax |= Tempdx; /* HRE[5:0]HRS[9:8] */
+	/* SR2F D[7:2]->HRE, D[1:0]->HRS */
+	xgifb_reg_set(pVBInfo->P3c4, 0x2F, Tempax);
+	xgifb_reg_and_or(pVBInfo->P3c4, 0x30, 0xE3, 00);
+
+	/* CR10 VRS */
+	Tempax = pVBInfo->XGINEWUB_CRT1Table[index].CR[10];
+	Tempbx = Tempax; /* Tempbx: VRS */
+	Tempax &= 0x01; /* Tempax[0]: VRS[0] */
+	xgifb_reg_or(pVBInfo->P3c4, 0x33, Tempax); /* SR33[0]->VRS[0] */
+	/* CR7[2][7] VRE */
+	Tempax = pVBInfo->XGINEWUB_CRT1Table[index].CR[9];
+	Tempcx = Tempbx >> 1; /* Tempcx[6:0]: VRS[7:1] */
+	Tempdx = Tempax & 0x04; /* Tempdx[2]: CR7[2] */
+	Tempdx <<= 5; /* Tempdx[7]: VRS[8] */
+	Tempcx |= Tempdx; /* Tempcx[7:0]: VRS[8:1] */
+	xgifb_reg_set(pVBInfo->P3c4, 0x34, Tempcx); /* SR34[8:1]->VRS */
+
+	Temp1 = Tempdx; /* Temp1[7]: Tempdx[7] */
+	Temp1 <<= 1; /* Temp1[8]: VRS[8] */
+	Temp1 |= Tempbx; /* Temp1[8:0]: VRS[8:0] */
+	Tempax &= 0x80;
+	Temp2 = Tempax << 2; /* Temp2[9]: VRS[9] */
+	Temp1 |= Temp2; /* Temp1[9:0]: VRS[9:0] */
+	/* Tempax: SRA */
+	Tempax = pVBInfo->XGINEWUB_CRT1Table[index].CR[14];
+	Tempax &= 0x08; /* Tempax[3]: VRS[3] */
+	Temp2 = Tempax;
+	Temp2 <<= 7; /* Temp2[10]: VRS[10] */
+	Temp1 |= Temp2; /* Temp1[10:0]: VRS[10:0] */
+
+	/* Tempax: CR11 VRE */
+	Tempax = pVBInfo->XGINEWUB_CRT1Table[index].CR[11];
+	Tempax &= 0x0F; /* Tempax[3:0]: VRE[3:0] */
+	/* Tempbx: SRA */
+	Tempbx = pVBInfo->XGINEWUB_CRT1Table[index].CR[14];
+	Tempbx &= 0x20; /* Tempbx[5]: VRE[5] */
+	Tempbx >>= 1; /* Tempbx[4]: VRE[4] */
+	Tempax |= Tempbx; /* Tempax[4:0]: VRE[4:0] */
+	Temp2 = Temp1 & 0x7E0; /* Temp2[10:5]: VRS[10:5] */
+	Temp2 |= Tempax; /* Temp2[10:5]: VRE[10:5] */
+
+	Temp3 = Temp1 & 0x1F; /* Temp3[4:0]: VRS[4:0] */
+	if (Tempax < Temp3) /* VRE < VRS */
+		Temp2 |= 0x20; /* VRE + 0x20 */
+
+	Temp2 &= 0xFF;
+	Tempax = (unsigned char) Temp2; /* Tempax: VRE[7:0] */
+	Tempax <<= 2; /* Tempax[7:0]; VRE[5:0]00 */
+	Temp1 &= 0x600; /* Temp1[10:9]: VRS[10:9] */
+	Temp1 >>= 9; /* Temp1[1:0]: VRS[10:9] */
+	Tempbx = (unsigned char) Temp1;
+	Tempax |= Tempbx; /* Tempax[7:0]: VRE[5:0]VRS[10:9] */
+	Tempax &= 0x7F;
+	/* SR3F D[7:2]->VRE D[1:0]->VRS */
+	xgifb_reg_set(pVBInfo->P3c4, 0x3F, Tempax);
 }
 
 static void XGI_SetXG27CRTC(unsigned short ModeNo,
@@ -800,139 +672,88 @@ static void XGI_SetXG27CRTC(unsigned short ModeNo,
 			    unsigned short RefreshRateTableIndex,
 			    struct vb_device_info *pVBInfo)
 {
-	unsigned short StandTableIndex, index, Tempax, Tempbx, Tempcx, Tempdx;
-
-	if (ModeNo <= 0x13) {
-		StandTableIndex = XGI_GetModePtr(ModeNo, ModeIdIndex, pVBInfo);
-		/* CR04 HRS */
-		Tempax = pVBInfo->StandTable[StandTableIndex].CRTC[4];
-		/* SR2E [7:0]->HRS */
-		xgifb_reg_set(pVBInfo->P3c4, 0x2E, Tempax);
-		/* Tempbx: CR05 HRE */
-		Tempbx = pVBInfo->StandTable[StandTableIndex].CRTC[5];
-		Tempbx &= 0x1F; /* Tempbx: HRE[4:0] */
-		Tempcx = Tempax;
-		Tempcx &= 0xE0; /* Tempcx: HRS[7:5] */
-		Tempdx = Tempcx | Tempbx; /* Tempdx(HRE): HRS[7:5]HRE[4:0] */
-		if (Tempbx < (Tempax & 0x1F)) /* IF HRE < HRS */
-			Tempdx |= 0x20; /* Tempdx: HRE = HRE + 0x20 */
-		Tempdx <<= 2; /* Tempdx << 2 */
-		/* SR2F [7:2]->HRE */
-		xgifb_reg_set(pVBInfo->P3c4, 0x2F, Tempdx);
-		xgifb_reg_and_or(pVBInfo->P3c4, 0x30, 0xE3, 00);
-
-		/* Tempax: CR10 VRS */
-		Tempax = pVBInfo->StandTable[StandTableIndex].CRTC[16];
-		xgifb_reg_set(pVBInfo->P3c4, 0x34, Tempax); /* SR34[7:0]->VRS */
-		Tempcx = Tempax; /* Tempcx=Tempax=VRS[7:0] */
-		/* Tempax[7][2]: CR7[7][2] VRS[9][8] */
-		Tempax = pVBInfo->StandTable[StandTableIndex].CRTC[7];
-		Tempbx = Tempax; /* Tempbx=CR07 */
-		Tempax &= 0x04; /* Tempax[2]: CR07[2] VRS[8] */
-		Tempax >>= 2;
-		/* SR35 D[0]->VRS D[8] */
-		xgifb_reg_and_or(pVBInfo->P3c4, 0x35, ~0x01, Tempax);
-		Tempcx |= (Tempax << 8); /* Tempcx[8] |= VRS[8] */
-		Tempcx |= (Tempbx & 0x80) << 2; /* Tempcx[9] |= VRS[9] */
-
-		/* CR11 VRE */
-		Tempax = pVBInfo->StandTable[StandTableIndex].CRTC[17];
-		Tempax &= 0x0F; /* Tempax: VRE[3:0] */
-		Tempbx = Tempcx; /* Tempbx=Tempcx=VRS[9:0] */
-		Tempbx &= 0x3F0; /* Tempbx[9:4]: VRS[9:4] */
-		Tempbx |= Tempax; /* Tempbx[9:0]: VRE[9:0] */
-		if (Tempax <= (Tempcx & 0x0F)) /* VRE[3:0]<=VRS[3:0] */
-			Tempbx |= 0x10; /* Tempbx: VRE + 0x10 */
-		/* Tempax[7:0]: VRE[7:0] */
-		Tempax = (unsigned char) Tempbx & 0xFF;
-		Tempax <<= 2; /* Tempax << 2: VRE[5:0] */
-		Tempcx = (Tempcx & 0x600) >> 8; /* Tempcx VRS[10:9] */
-		/* SR3F D[7:2]->VRE D[5:0] */
-		xgifb_reg_and_or(pVBInfo->P3c4, 0x3F, ~0xFC, Tempax);
-		/* SR35 D[2:1]->VRS[10:9] */
-		xgifb_reg_and_or(pVBInfo->P3c4, 0x35, ~0x06, Tempcx);
-	} else {
-		index = pVBInfo->RefIndex[RefreshRateTableIndex].Ext_CRT1CRTC;
-		/* Tempax: CR4 HRS */
-		Tempax = pVBInfo->XGINEWUB_CRT1Table[index].CR[3];
-		Tempbx = Tempax; /* Tempbx: HRS[7:0] */
-		/* SR2E[7:0]->HRS */
-		xgifb_reg_set(pVBInfo->P3c4, 0x2E, Tempax);
-
-		/* SR0B */
-		Tempax = pVBInfo->XGINEWUB_CRT1Table[index].CR[5];
-		Tempax &= 0xC0; /* Tempax[7:6]: SR0B[7:6]: HRS[9:8]*/
-		Tempbx |= (Tempax << 2); /* Tempbx: HRS[9:0] */
-
-		Tempax = pVBInfo->XGINEWUB_CRT1Table[index].CR[4]; /* CR5 HRE */
-		Tempax &= 0x1F; /* Tempax[4:0]: HRE[4:0] */
-		Tempcx = Tempax; /* Tempcx: HRE[4:0] */
-
-		Tempax = pVBInfo->XGINEWUB_CRT1Table[index].CR[6]; /* SRC */
-		Tempax &= 0x04; /* Tempax[2]: HRE[5] */
-		Tempax <<= 3; /* Tempax[5]: HRE[5] */
-		Tempcx |= Tempax; /* Tempcx[5:0]: HRE[5:0] */
-
-		Tempbx = Tempbx & 0x3C0; /* Tempbx[9:6]: HRS[9:6] */
-		Tempbx |= Tempcx; /* Tempbx: HRS[9:6]HRE[5:0] */
-
-		/* Tempax: CR4 HRS */
-		Tempax = pVBInfo->XGINEWUB_CRT1Table[index].CR[3];
-		Tempax &= 0x3F; /* Tempax: HRS[5:0] */
-		if (Tempcx <= Tempax) /* HRE[5:0] < HRS[5:0] */
-			Tempbx += 0x40; /* Tempbx= Tempbx + 0x40 : HRE[9:0]*/
-
-		Tempax = pVBInfo->XGINEWUB_CRT1Table[index].CR[5]; /* SR0B */
-		Tempax &= 0xC0; /* Tempax[7:6]: SR0B[7:6]: HRS[9:8]*/
-		Tempax >>= 6; /* Tempax[1:0]: HRS[9:8]*/
-		Tempax |= ((Tempbx << 2) & 0xFF); /* Tempax[7:2]: HRE[5:0] */
-		/* SR2F [7:2][1:0]: HRE[5:0]HRS[9:8] */
-		xgifb_reg_set(pVBInfo->P3c4, 0x2F, Tempax);
-		xgifb_reg_and_or(pVBInfo->P3c4, 0x30, 0xE3, 00);
-
-		/* CR10 VRS */
-		Tempax = pVBInfo->XGINEWUB_CRT1Table[index].CR[10];
-		/* SR34[7:0]->VRS[7:0] */
-		xgifb_reg_set(pVBInfo->P3c4, 0x34, Tempax);
-
-		Tempcx = Tempax; /* Tempcx <= VRS[7:0] */
-		/* CR7[7][2] VRS[9][8] */
-		Tempax = pVBInfo->XGINEWUB_CRT1Table[index].CR[9];
-		Tempbx = Tempax; /* Tempbx <= CR07[7:0] */
-		Tempax = Tempax & 0x04; /* Tempax[2]: CR7[2]: VRS[8] */
-		Tempax >>= 2; /* Tempax[0]: VRS[8] */
-		/* SR35[0]: VRS[8] */
-		xgifb_reg_and_or(pVBInfo->P3c4, 0x35, ~0x01, Tempax);
-		Tempcx |= (Tempax << 8); /* Tempcx <= VRS[8:0] */
-		Tempcx |= ((Tempbx & 0x80) << 2); /* Tempcx <= VRS[9:0] */
-		/* Tempax: SR0A */
-		Tempax = pVBInfo->XGINEWUB_CRT1Table[index].CR[14];
-		Tempax &= 0x08; /* SR0A[3] VRS[10] */
-		Tempcx |= (Tempax << 7); /* Tempcx <= VRS[10:0] */
-
-		/* Tempax: CR11 VRE */
-		Tempax = pVBInfo->XGINEWUB_CRT1Table[index].CR[11];
-		Tempax &= 0x0F; /* Tempax[3:0]: VRE[3:0] */
-		/* Tempbx: SR0A */
-		Tempbx = pVBInfo->XGINEWUB_CRT1Table[index].CR[14];
-		Tempbx &= 0x20; /* Tempbx[5]: SR0A[5]: VRE[4] */
-		Tempbx >>= 1; /* Tempbx[4]: VRE[4] */
-		Tempax |= Tempbx; /* Tempax[4:0]: VRE[4:0] */
-		Tempbx = Tempcx; /* Tempbx: VRS[10:0] */
-		Tempbx &= 0x7E0; /* Tempbx[10:5]: VRS[10:5] */
-		Tempbx |= Tempax; /* Tempbx: VRS[10:5]VRE[4:0] */
-
-		if (Tempbx <= Tempcx) /* VRE <= VRS */
-			Tempbx |= 0x20; /* VRE + 0x20 */
-
-		/* Tempax: Tempax[7:0]; VRE[5:0]00 */
-		Tempax = (Tempbx << 2) & 0xFF;
-		/* SR3F[7:2]:VRE[5:0] */
-		xgifb_reg_and_or(pVBInfo->P3c4, 0x3F, ~0xFC, Tempax);
-		Tempax = Tempcx >> 8;
-		/* SR35[2:0]:VRS[10:8] */
-		xgifb_reg_and_or(pVBInfo->P3c4, 0x35, ~0x07, Tempax);
-	}
+	unsigned short index, Tempax, Tempbx, Tempcx;
+
+	index = pVBInfo->RefIndex[RefreshRateTableIndex].Ext_CRT1CRTC;
+	/* Tempax: CR4 HRS */
+	Tempax = pVBInfo->XGINEWUB_CRT1Table[index].CR[3];
+	Tempbx = Tempax; /* Tempbx: HRS[7:0] */
+	/* SR2E[7:0]->HRS */
+	xgifb_reg_set(pVBInfo->P3c4, 0x2E, Tempax);
+
+	/* SR0B */
+	Tempax = pVBInfo->XGINEWUB_CRT1Table[index].CR[5];
+	Tempax &= 0xC0; /* Tempax[7:6]: SR0B[7:6]: HRS[9:8]*/
+	Tempbx |= (Tempax << 2); /* Tempbx: HRS[9:0] */
+
+	Tempax = pVBInfo->XGINEWUB_CRT1Table[index].CR[4]; /* CR5 HRE */
+	Tempax &= 0x1F; /* Tempax[4:0]: HRE[4:0] */
+	Tempcx = Tempax; /* Tempcx: HRE[4:0] */
+
+	Tempax = pVBInfo->XGINEWUB_CRT1Table[index].CR[6]; /* SRC */
+	Tempax &= 0x04; /* Tempax[2]: HRE[5] */
+	Tempax <<= 3; /* Tempax[5]: HRE[5] */
+	Tempcx |= Tempax; /* Tempcx[5:0]: HRE[5:0] */
+
+	Tempbx = Tempbx & 0x3C0; /* Tempbx[9:6]: HRS[9:6] */
+	Tempbx |= Tempcx; /* Tempbx: HRS[9:6]HRE[5:0] */
+
+	/* Tempax: CR4 HRS */
+	Tempax = pVBInfo->XGINEWUB_CRT1Table[index].CR[3];
+	Tempax &= 0x3F; /* Tempax: HRS[5:0] */
+	if (Tempcx <= Tempax) /* HRE[5:0] < HRS[5:0] */
+		Tempbx += 0x40; /* Tempbx= Tempbx + 0x40 : HRE[9:0]*/
+
+	Tempax = pVBInfo->XGINEWUB_CRT1Table[index].CR[5]; /* SR0B */
+	Tempax &= 0xC0; /* Tempax[7:6]: SR0B[7:6]: HRS[9:8]*/
+	Tempax >>= 6; /* Tempax[1:0]: HRS[9:8]*/
+	Tempax |= ((Tempbx << 2) & 0xFF); /* Tempax[7:2]: HRE[5:0] */
+	/* SR2F [7:2][1:0]: HRE[5:0]HRS[9:8] */
+	xgifb_reg_set(pVBInfo->P3c4, 0x2F, Tempax);
+	xgifb_reg_and_or(pVBInfo->P3c4, 0x30, 0xE3, 00);
+
+	/* CR10 VRS */
+	Tempax = pVBInfo->XGINEWUB_CRT1Table[index].CR[10];
+	/* SR34[7:0]->VRS[7:0] */
+	xgifb_reg_set(pVBInfo->P3c4, 0x34, Tempax);
+
+	Tempcx = Tempax; /* Tempcx <= VRS[7:0] */
+	/* CR7[7][2] VRS[9][8] */
+	Tempax = pVBInfo->XGINEWUB_CRT1Table[index].CR[9];
+	Tempbx = Tempax; /* Tempbx <= CR07[7:0] */
+	Tempax = Tempax & 0x04; /* Tempax[2]: CR7[2]: VRS[8] */
+	Tempax >>= 2; /* Tempax[0]: VRS[8] */
+	/* SR35[0]: VRS[8] */
+	xgifb_reg_and_or(pVBInfo->P3c4, 0x35, ~0x01, Tempax);
+	Tempcx |= (Tempax << 8); /* Tempcx <= VRS[8:0] */
+	Tempcx |= ((Tempbx & 0x80) << 2); /* Tempcx <= VRS[9:0] */
+	/* Tempax: SR0A */
+	Tempax = pVBInfo->XGINEWUB_CRT1Table[index].CR[14];
+	Tempax &= 0x08; /* SR0A[3] VRS[10] */
+	Tempcx |= (Tempax << 7); /* Tempcx <= VRS[10:0] */
+
+	/* Tempax: CR11 VRE */
+	Tempax = pVBInfo->XGINEWUB_CRT1Table[index].CR[11];
+	Tempax &= 0x0F; /* Tempax[3:0]: VRE[3:0] */
+	/* Tempbx: SR0A */
+	Tempbx = pVBInfo->XGINEWUB_CRT1Table[index].CR[14];
+	Tempbx &= 0x20; /* Tempbx[5]: SR0A[5]: VRE[4] */
+	Tempbx >>= 1; /* Tempbx[4]: VRE[4] */
+	Tempax |= Tempbx; /* Tempax[4:0]: VRE[4:0] */
+	Tempbx = Tempcx; /* Tempbx: VRS[10:0] */
+	Tempbx &= 0x7E0; /* Tempbx[10:5]: VRS[10:5] */
+	Tempbx |= Tempax; /* Tempbx: VRS[10:5]VRE[4:0] */
+
+	if (Tempbx <= Tempcx) /* VRE <= VRS */
+		Tempbx |= 0x20; /* VRE + 0x20 */
+
+	/* Tempax: Tempax[7:0]; VRE[5:0]00 */
+	Tempax = (Tempbx << 2) & 0xFF;
+	/* SR3F[7:2]:VRE[5:0] */
+	xgifb_reg_and_or(pVBInfo->P3c4, 0x3F, ~0xFC, Tempax);
+	Tempax = Tempcx >> 8;
+	/* SR35[2:0]:VRS[10:8] */
+	xgifb_reg_and_or(pVBInfo->P3c4, 0x35, ~0x07, Tempax);
 }
 
 static void XGI_SetXG27FPBits(struct vb_device_info *pVBInfo)
@@ -954,7 +775,7 @@ static void xgifb_set_lcd(int chip_id,
 			  unsigned short RefreshRateTableIndex,
 			  unsigned short ModeNo)
 {
-	unsigned short Data, Temp, b3CC;
+	unsigned short Data, Temp;
 	unsigned short XGI_P3cc;
 
 	XGI_P3cc = pVBInfo->P3cc;
@@ -995,23 +816,13 @@ static void xgifb_set_lcd(int chip_id,
 	xgifb_reg_and(pVBInfo->P3c4, 0x30, ~0x20); /* Hsync polarity */
 	xgifb_reg_and(pVBInfo->P3c4, 0x35, ~0x80); /* Vsync polarity */
 
-	if (ModeNo <= 0x13) {
-		b3CC = (unsigned char) inb(XGI_P3cc);
-		if (b3CC & 0x40)
-			/* Hsync polarity */
-			xgifb_reg_or(pVBInfo->P3c4, 0x30, 0x20);
-		if (b3CC & 0x80)
-			/* Vsync polarity */
-			xgifb_reg_or(pVBInfo->P3c4, 0x35, 0x80);
-	} else {
-		Data = pVBInfo->RefIndex[RefreshRateTableIndex].Ext_InfoFlag;
-		if (Data & 0x4000)
-			/* Hsync polarity */
-			xgifb_reg_or(pVBInfo->P3c4, 0x30, 0x20);
-		if (Data & 0x8000)
-			/* Vsync polarity */
-			xgifb_reg_or(pVBInfo->P3c4, 0x35, 0x80);
-	}
+	Data = pVBInfo->RefIndex[RefreshRateTableIndex].Ext_InfoFlag;
+	if (Data & 0x4000)
+		/* Hsync polarity */
+		xgifb_reg_or(pVBInfo->P3c4, 0x30, 0x20);
+	if (Data & 0x8000)
+		/* Vsync polarity */
+		xgifb_reg_or(pVBInfo->P3c4, 0x35, 0x80);
 }
 
 /* --------------------------------------------------------------------- */
@@ -1024,30 +835,22 @@ static void XGI_UpdateXG21CRTC(unsigned short ModeNo,
 			       struct vb_device_info *pVBInfo,
 			       unsigned short RefreshRateTableIndex)
 {
-	int i, index = -1;
+	int index = -1;
 
 	xgifb_reg_and(pVBInfo->P3d4, 0x11, 0x7F); /* Unlock CR0~7 */
-	if (ModeNo <= 0x13) {
-		for (i = 0; i < 12; i++) {
-			if (ModeNo == pVBInfo->UpdateCRT1[i].ModeID)
-				index = i;
-		}
-	} else {
-		if (ModeNo == 0x2E &&
-		    (pVBInfo->RefIndex[RefreshRateTableIndex].Ext_CRT1CRTC ==
-							      RES640x480x60))
-			index = 12;
-		else if (ModeNo == 0x2E &&
-			 (pVBInfo->RefIndex[RefreshRateTableIndex].
+	if (ModeNo == 0x2E &&
+	    (pVBInfo->RefIndex[RefreshRateTableIndex].Ext_CRT1CRTC ==
+						      RES640x480x60))
+		index = 12;
+	else if (ModeNo == 0x2E && (pVBInfo->RefIndex[RefreshRateTableIndex].
 				Ext_CRT1CRTC == RES640x480x72))
-			index = 13;
-		else if (ModeNo == 0x2F)
-			index = 14;
-		else if (ModeNo == 0x50)
-			index = 15;
-		else if (ModeNo == 0x59)
-			index = 16;
-	}
+		index = 13;
+	else if (ModeNo == 0x2F)
+		index = 14;
+	else if (ModeNo == 0x50)
+		index = 15;
+	else if (ModeNo == 0x59)
+		index = 16;
 
 	if (index != -1) {
 		xgifb_reg_set(pVBInfo->P3d4, 0x02,
@@ -1061,20 +864,6 @@ static void XGI_UpdateXG21CRTC(unsigned short ModeNo,
 	}
 }
 
-static unsigned short XGI_GetResInfo(unsigned short ModeNo,
-		unsigned short ModeIdIndex, struct vb_device_info *pVBInfo)
-{
-	unsigned short resindex;
-
-	if (ModeNo <= 0x13)
-		/* si+St_ResInfo */
-		resindex = pVBInfo->SModeIDTable[ModeIdIndex].St_ResInfo;
-	else
-		/* si+Ext_ResInfo */
-		resindex = pVBInfo->EModeIDTable[ModeIdIndex].Ext_RESINFO;
-	return resindex;
-}
-
 static void XGI_SetCRT1DE(struct xgi_hw_device_info *HwDeviceExtension,
 		unsigned short ModeNo, unsigned short ModeIdIndex,
 		unsigned short RefreshRateTableIndex,
@@ -1084,33 +873,25 @@ static void XGI_SetCRT1DE(struct xgi_hw_device_info *HwDeviceExtension,
 
 	unsigned char data;
 
-	resindex = XGI_GetResInfo(ModeNo, ModeIdIndex, pVBInfo);
+	resindex = pVBInfo->EModeIDTable[ModeIdIndex].Ext_RESINFO;
 
-	if (ModeNo <= 0x13) {
-		modeflag = pVBInfo->SModeIDTable[ModeIdIndex].St_ModeFlag;
-		tempax = pVBInfo->StResInfo[resindex].HTotal;
-		tempbx = pVBInfo->StResInfo[resindex].VTotal;
-	} else {
-		modeflag = pVBInfo->EModeIDTable[ModeIdIndex].Ext_ModeFlag;
-		tempax = pVBInfo->ModeResInfo[resindex].HTotal;
-		tempbx = pVBInfo->ModeResInfo[resindex].VTotal;
-	}
+	modeflag = pVBInfo->EModeIDTable[ModeIdIndex].Ext_ModeFlag;
+	tempax = pVBInfo->ModeResInfo[resindex].HTotal;
+	tempbx = pVBInfo->ModeResInfo[resindex].VTotal;
 
 	if (modeflag & HalfDCLK)
 		tempax = tempax >> 1;
 
-	if (ModeNo > 0x13) {
-		if (modeflag & HalfDCLK)
-			tempax = tempax << 1;
+	if (modeflag & HalfDCLK)
+		tempax = tempax << 1;
 
-		temp = pVBInfo->RefIndex[RefreshRateTableIndex].Ext_InfoFlag;
+	temp = pVBInfo->RefIndex[RefreshRateTableIndex].Ext_InfoFlag;
 
-		if (temp & InterlaceMode)
-			tempbx = tempbx >> 1;
+	if (temp & InterlaceMode)
+		tempbx = tempbx >> 1;
 
-		if (modeflag & DoubleScanMode)
-			tempbx = tempbx << 1;
-	}
+	if (modeflag & DoubleScanMode)
+		tempbx = tempbx << 1;
 
 	tempcx = 8;
 
@@ -1258,18 +1039,10 @@ static unsigned short XGI_GetVCLK2Ptr(unsigned short ModeNo,
 	unsigned short CRT2Index, VCLKIndex;
 	unsigned short modeflag, resinfo;
 
-	if (ModeNo <= 0x13) {
-		/* si+St_ResInfo */
-		modeflag = pVBInfo->SModeIDTable[ModeIdIndex].St_ModeFlag;
-		resinfo = pVBInfo->SModeIDTable[ModeIdIndex].St_ResInfo;
-		CRT2Index = pVBInfo->SModeIDTable[ModeIdIndex].St_CRT2CRTC;
-	} else {
-		/* si+Ext_ResInfo */
-		modeflag = pVBInfo->EModeIDTable[ModeIdIndex].Ext_ModeFlag;
-		resinfo = pVBInfo->EModeIDTable[ModeIdIndex].Ext_RESINFO;
-		CRT2Index = pVBInfo->RefIndex[RefreshRateTableIndex].
-				Ext_CRT2CRTC;
-	}
+	/* si+Ext_ResInfo */
+	modeflag = pVBInfo->EModeIDTable[ModeIdIndex].Ext_ModeFlag;
+	resinfo = pVBInfo->EModeIDTable[ModeIdIndex].Ext_RESINFO;
+	CRT2Index = pVBInfo->RefIndex[RefreshRateTableIndex].Ext_CRT2CRTC;
 
 	if (pVBInfo->IF_DEF_LVDS == 0) {
 		CRT2Index = CRT2Index >> 6; /*  for LCD */
@@ -1318,23 +1091,13 @@ static unsigned short XGI_GetVCLK2Ptr(unsigned short ModeNo,
 				VCLKIndex += 25;
 			}
 		} else { /* for CRT2 */
-			/* Port 3cch */
-			VCLKIndex = (unsigned char) inb((pVBInfo->P3ca + 0x02));
-			VCLKIndex = ((VCLKIndex >> 2) & 0x03);
-			if (ModeNo > 0x13) {
-				/* di+Ext_CRTVCLK */
-				VCLKIndex = pVBInfo->RefIndex[
-							RefreshRateTableIndex].
+			/* di+Ext_CRTVCLK */
+			VCLKIndex = pVBInfo->RefIndex[RefreshRateTableIndex].
 								Ext_CRTVCLK;
-				VCLKIndex &= IndexMask;
-			}
+			VCLKIndex &= IndexMask;
 		}
 	} else { /* LVDS */
-		if (ModeNo <= 0x13)
-			VCLKIndex = CRT2Index;
-		else
-			VCLKIndex = CRT2Index;
-
+		VCLKIndex = CRT2Index;
 		VCLKIndex = VCLKIndex >> 6;
 		if ((pVBInfo->LCDResInfo == Panel_800x600) ||
 		    (pVBInfo->LCDResInfo == Panel_320x480))
@@ -1431,27 +1194,13 @@ static void XGI_SetCRT1FIFO(unsigned short ModeNo,
 	data &= 0xfe;
 	xgifb_reg_set(pVBInfo->P3c4, 0x3D, data); /* diable auto-threshold */
 
-	if (ModeNo > 0x13) {
-		xgifb_reg_set(pVBInfo->P3c4, 0x08, 0x34);
-		data = xgifb_reg_get(pVBInfo->P3c4, 0x09);
-		data &= 0xC0;
-		xgifb_reg_set(pVBInfo->P3c4, 0x09, data | 0x30);
-		data = xgifb_reg_get(pVBInfo->P3c4, 0x3D);
-		data |= 0x01;
-		xgifb_reg_set(pVBInfo->P3c4, 0x3D, data);
-	} else {
-		if (HwDeviceExtension->jChipType == XG27) {
-			xgifb_reg_set(pVBInfo->P3c4, 0x08, 0x0E);
-			data = xgifb_reg_get(pVBInfo->P3c4, 0x09);
-			data &= 0xC0;
-			xgifb_reg_set(pVBInfo->P3c4, 0x09, data | 0x20);
-		} else {
-			xgifb_reg_set(pVBInfo->P3c4, 0x08, 0xAE);
-			data = xgifb_reg_get(pVBInfo->P3c4, 0x09);
-			data &= 0xF0;
-			xgifb_reg_set(pVBInfo->P3c4, 0x09, data);
-		}
-	}
+	xgifb_reg_set(pVBInfo->P3c4, 0x08, 0x34);
+	data = xgifb_reg_get(pVBInfo->P3c4, 0x09);
+	data &= 0xC0;
+	xgifb_reg_set(pVBInfo->P3c4, 0x09, data | 0x30);
+	data = xgifb_reg_get(pVBInfo->P3c4, 0x3D);
+	data |= 0x01;
+	xgifb_reg_set(pVBInfo->P3c4, 0x3D, data);
 
 	if (HwDeviceExtension->jChipType == XG21)
 		XGI_SetXG21FPBits(pVBInfo); /* Fix SR9[7:6] can't read back */
@@ -1466,13 +1215,9 @@ static void XGI_SetVCLKState(struct xgi_hw_device_info *HwDeviceExtension,
 
 	unsigned char index;
 
-	if (ModeNo <= 0x13)
-		VCLK = 0;
-	else {
-		index = pVBInfo->RefIndex[RefreshRateTableIndex].Ext_CRTVCLK;
-		index &= IndexMask;
-		VCLK = pVBInfo->VCLKData[index].CLOCK;
-	}
+	index = pVBInfo->RefIndex[RefreshRateTableIndex].Ext_CRTVCLK;
+	index &= IndexMask;
+	VCLK = pVBInfo->VCLKData[index].CLOCK;
 
 	data = xgifb_reg_get(pVBInfo->P3c4, 0x32);
 	data &= 0xf3;
@@ -1508,44 +1253,26 @@ static void XGI_SetCRT1ModeRegs(struct xgi_hw_device_info *HwDeviceExtension,
 	unsigned short data, data2, data3, infoflag = 0, modeflag, resindex,
 			xres;
 
-	if (ModeNo > 0x13) {
-		modeflag = pVBInfo->EModeIDTable[ModeIdIndex].Ext_ModeFlag;
-		infoflag = pVBInfo->RefIndex[RefreshRateTableIndex].
-				Ext_InfoFlag;
-	} else
-		/* si+St_ModeFlag */
-		modeflag = pVBInfo->SModeIDTable[ModeIdIndex].St_ModeFlag;
+	modeflag = pVBInfo->EModeIDTable[ModeIdIndex].Ext_ModeFlag;
+	infoflag = pVBInfo->RefIndex[RefreshRateTableIndex].Ext_InfoFlag;
 
 	if (xgifb_reg_get(pVBInfo->P3d4, 0x31) & 0x01)
 		xgifb_reg_and_or(pVBInfo->P3c4, 0x1F, 0x3F, 0x00);
 
-	if (ModeNo > 0x13)
-		data = infoflag;
-	else
-		data = 0;
-
+	data = infoflag;
 	data2 = 0;
-
-	if (ModeNo > 0x13) {
-		if (pVBInfo->ModeType > 0x02) {
-			data2 |= 0x02;
-			data3 = pVBInfo->ModeType - ModeVGA;
-			data3 = data3 << 2;
-			data2 |= data3;
-		}
-	}
-
+	data2 |= 0x02;
+	data3 = pVBInfo->ModeType - ModeVGA;
+	data3 = data3 << 2;
+	data2 |= data3;
 	data &= InterlaceMode;
 
 	if (data)
 		data2 |= 0x20;
 
 	xgifb_reg_and_or(pVBInfo->P3c4, 0x06, ~0x3F, data2);
-	resindex = XGI_GetResInfo(ModeNo, ModeIdIndex, pVBInfo);
-	if (ModeNo <= 0x13)
-		xres = pVBInfo->StResInfo[resindex].HTotal;
-	else
-		xres = pVBInfo->ModeResInfo[resindex].HTotal; /* xres->ax */
+	resindex = pVBInfo->EModeIDTable[ModeIdIndex].Ext_RESINFO;
+	xres = pVBInfo->ModeResInfo[resindex].HTotal; /* xres->ax */
 
 	data = 0x0000;
 	if (infoflag & InterlaceMode) {
@@ -1568,18 +1295,10 @@ static void XGI_SetCRT1ModeRegs(struct xgi_hw_device_info *HwDeviceExtension,
 	if (modeflag & LineCompareOff)
 		data2 |= 0x08;
 
-	if (ModeNo > 0x13) {
-		if (pVBInfo->ModeType == ModeEGA)
-			data2 |= 0x40;
-	}
-
 	xgifb_reg_and_or(pVBInfo->P3c4, 0x0F, ~0x48, data2);
 	data = 0x60;
-	if (pVBInfo->ModeType != ModeText) {
-		data = data ^ 0x60;
-		if (pVBInfo->ModeType != ModeEGA)
-			data = data ^ 0xA0;
-	}
+	data = data ^ 0x60;
+	data = data ^ 0xA0;
 	xgifb_reg_and_or(pVBInfo->P3c4, 0x21, 0x1F, data);
 
 	XGI_SetVCLKState(HwDeviceExtension, ModeNo, RefreshRateTableIndex,
@@ -1644,38 +1363,13 @@ static void XGI_WriteDAC(unsigned short dl,
 static void XGI_LoadDAC(unsigned short ModeNo, unsigned short ModeIdIndex,
 		struct vb_device_info *pVBInfo)
 {
-	unsigned short data, data2, time, i, j, k, m, n, o, si, di, bx, dl, al,
-			ah, dh;
-	const unsigned short *table = NULL;
-
-	if (ModeNo <= 0x13)
-		data = pVBInfo->SModeIDTable[ModeIdIndex].St_ModeFlag;
-	else
-		data = pVBInfo->EModeIDTable[ModeIdIndex].Ext_ModeFlag;
-
-	data &= DACInfoFlag;
-	time = 64;
-
-	if (data == 0x00)
-		table = XGINew_MDA_DAC;
-	else if (data == 0x08)
-		table = XGINew_CGA_DAC;
-	else if (data == 0x10)
-		table = XGINew_EGA_DAC;
-	else if (data == 0x18) {
-		time = 256;
-		table = XGINew_VGA_DAC;
-	}
-
-	if (time == 256)
-		j = 16;
-	else
-		j = time;
+	unsigned short data, data2, i, k, m, n, o, si, di, bx, dl, al, ah, dh;
+	const unsigned short *table = XGINew_VGA_DAC;
 
 	outb(0xFF, pVBInfo->P3c6);
 	outb(0x00, pVBInfo->P3c8);
 
-	for (i = 0; i < j; i++) {
+	for (i = 0; i < 16; i++) {
 		data = table[i];
 
 		for (k = 0; k < 3; k++) {
@@ -1692,45 +1386,43 @@ static void XGI_LoadDAC(unsigned short ModeNo, unsigned short ModeIdIndex,
 		}
 	}
 
-	if (time == 256) {
-		for (i = 16; i < 32; i++) {
-			data = table[i];
-
-			for (k = 0; k < 3; k++)
-				outb(data, pVBInfo->P3c9);
-		}
+	for (i = 16; i < 32; i++) {
+		data = table[i];
 
-		si = 32;
+		for (k = 0; k < 3; k++)
+			outb(data, pVBInfo->P3c9);
+	}
 
-		for (m = 0; m < 9; m++) {
-			di = si;
-			bx = si + 0x04;
-			dl = 0;
+	si = 32;
 
-			for (n = 0; n < 3; n++) {
-				for (o = 0; o < 5; o++) {
-					dh = table[si];
-					ah = table[di];
-					al = table[bx];
-					si++;
-					XGI_WriteDAC(dl, ah, al, dh, pVBInfo);
-				}
+	for (m = 0; m < 9; m++) {
+		di = si;
+		bx = si + 0x04;
+		dl = 0;
 
-				si -= 2;
+		for (n = 0; n < 3; n++) {
+			for (o = 0; o < 5; o++) {
+				dh = table[si];
+				ah = table[di];
+				al = table[bx];
+				si++;
+				XGI_WriteDAC(dl, ah, al, dh, pVBInfo);
+			}
 
-				for (o = 0; o < 3; o++) {
-					dh = table[bx];
-					ah = table[di];
-					al = table[si];
-					si--;
-					XGI_WriteDAC(dl, ah, al, dh, pVBInfo);
-				}
+			si -= 2;
 
-				dl++;
+			for (o = 0; o < 3; o++) {
+				dh = table[bx];
+				ah = table[di];
+				al = table[si];
+				si--;
+				XGI_WriteDAC(dl, ah, al, dh, pVBInfo);
 			}
 
-			si += 5;
+			dl++;
 		}
+
+		si += 5;
 	}
 }
 
@@ -1740,34 +1432,20 @@ static void XGI_GetLVDSResInfo(unsigned short ModeNo,
 {
 	unsigned short resindex, xres, yres, modeflag;
 
-	if (ModeNo <= 0x13)
-		/* si+St_ResInfo */
-		modeflag = pVBInfo->SModeIDTable[ModeIdIndex].St_ResInfo;
-	else
-		/* si+Ext_ResInfo */
-		modeflag = pVBInfo->EModeIDTable[ModeIdIndex].Ext_RESINFO;
+	/* si+Ext_ResInfo */
+	modeflag = pVBInfo->EModeIDTable[ModeIdIndex].Ext_RESINFO;
 
-	if (ModeNo <= 0x13)
-		/* si+St_ResInfo */
-		resindex = pVBInfo->SModeIDTable[ModeIdIndex].St_ResInfo;
-	else
-		/* si+Ext_ResInfo */
-		resindex = pVBInfo->EModeIDTable[ModeIdIndex].Ext_RESINFO;
+	/* si+Ext_ResInfo */
+	resindex = pVBInfo->EModeIDTable[ModeIdIndex].Ext_RESINFO;
 
-	if (ModeNo <= 0x13) {
-		xres = pVBInfo->StResInfo[resindex].HTotal;
-		yres = pVBInfo->StResInfo[resindex].VTotal;
-	} else {
-		xres = pVBInfo->ModeResInfo[resindex].HTotal;
-		yres = pVBInfo->ModeResInfo[resindex].VTotal;
-	}
-	if (ModeNo > 0x13) {
-		if (modeflag & HalfDCLK)
-			xres = xres << 1;
+	xres = pVBInfo->ModeResInfo[resindex].HTotal;
+	yres = pVBInfo->ModeResInfo[resindex].VTotal;
 
-		if (modeflag & DoubleScanMode)
-			yres = yres << 1;
-	}
+	if (modeflag & HalfDCLK)
+		xres = xres << 1;
+
+	if (modeflag & DoubleScanMode)
+		yres = yres << 1;
 
 	if (xres == 720)
 		xres = 640;
@@ -1789,32 +1467,16 @@ static void *XGI_GetLcdPtr(unsigned short BX, unsigned short ModeNo,
 
 	tempbx = BX;
 
-	if (ModeNo <= 0x13) {
-		modeflag = pVBInfo->SModeIDTable[ModeIdIndex].St_ModeFlag;
-		tempal = pVBInfo->SModeIDTable[ModeIdIndex].St_CRT2CRTC;
-	} else {
-		modeflag = pVBInfo->EModeIDTable[ModeIdIndex].Ext_ModeFlag;
-		tempal = pVBInfo->RefIndex[RefreshRateTableIndex].Ext_CRT2CRTC;
-	}
+	modeflag = pVBInfo->EModeIDTable[ModeIdIndex].Ext_ModeFlag;
+	tempal = pVBInfo->RefIndex[RefreshRateTableIndex].Ext_CRT2CRTC;
 
 	tempal = tempal & 0x0f;
 
 	if (tempbx <= 1) { /* ExpLink */
-		if (ModeNo <= 0x13) {
-			/* find no Ext_CRT2CRTC2 */
-			tempal = pVBInfo->SModeIDTable[ModeIdIndex].St_CRT2CRTC;
-		} else {
-			tempal = pVBInfo->RefIndex[RefreshRateTableIndex].
-					Ext_CRT2CRTC;
-		}
+		tempal = pVBInfo->RefIndex[RefreshRateTableIndex].Ext_CRT2CRTC;
 
 		if (pVBInfo->VBInfo & XGI_SetCRT2ToLCDA) {
-			if (ModeNo <= 0x13)
-				tempal = pVBInfo->SModeIDTable[ModeIdIndex].
-						St_CRT2CRTC2;
-			else
-				tempal = pVBInfo->RefIndex[
-						RefreshRateTableIndex].
+			tempal = pVBInfo->RefIndex[RefreshRateTableIndex].
 							Ext_CRT2CRTC2;
 		}
 
@@ -1882,9 +1544,6 @@ static void *XGI_GetLcdPtr(unsigned short BX, unsigned short ModeNo,
 			tempbx = tempdi[i].MASK;
 			tempdx = pVBInfo->LCDInfo;
 
-			if (ModeNo <= 0x13) /* alan 09/10/2003 */
-				tempdx |= SetLCDStdMode;
-
 			if (modeflag & HalfDCLK)
 				tempdx |= SetLCDLowResolution;
 
@@ -2238,15 +1897,8 @@ static void *XGI_GetTVPtr(unsigned short BX, unsigned short ModeNo,
 	struct XGI330_TVDataTablStruct *tempdi = NULL;
 
 	tempbx = BX;
-
-	if (ModeNo <= 0x13) {
-		modeflag = pVBInfo->SModeIDTable[ModeIdIndex].St_ModeFlag;
-		tempal = pVBInfo->SModeIDTable[ModeIdIndex].St_CRT2CRTC;
-	} else {
-		modeflag = pVBInfo->EModeIDTable[ModeIdIndex].Ext_ModeFlag;
-		tempal = pVBInfo->RefIndex[RefreshRateTableIndex].Ext_CRT2CRTC;
-	}
-
+	modeflag = pVBInfo->EModeIDTable[ModeIdIndex].Ext_ModeFlag;
+	tempal = pVBInfo->RefIndex[RefreshRateTableIndex].Ext_CRT2CRTC;
 	tempal = tempal & 0x3f;
 	table = tempbx;
 
@@ -2413,11 +2065,7 @@ static void XGI_ModCRT1Regs(unsigned short ModeNo, unsigned short ModeIdIndex,
 	struct XGI_LVDSCRT1HDataStruct *LCDPtr = NULL;
 	struct XGI_LVDSCRT1VDataStruct *LCDPtr1 = NULL;
 
-	if (ModeNo <= 0x13)
-		index = pVBInfo->SModeIDTable[ModeIdIndex].St_CRT2CRTC;
-	else
-		index = pVBInfo->RefIndex[RefreshRateTableIndex].Ext_CRT2CRTC;
-
+	index = pVBInfo->RefIndex[RefreshRateTableIndex].Ext_CRT2CRTC;
 	index = index & IndexMask;
 
 	tempbx = 0;
@@ -2530,14 +2178,10 @@ static void XGI_SetLVDSRegs(unsigned short ModeNo, unsigned short ModeIdIndex,
 {
 	unsigned short tempbx, tempax, tempcx, tempdx, push1, push2, modeflag;
 	unsigned long temp, temp1, temp2, temp3, push3;
-	struct XGI330_LCDDataDesStruct *LCDPtr = NULL;
+	struct XGI_LCDDesStruct *LCDPtr = NULL;
 	struct XGI330_LCDDataDesStruct2 *LCDPtr1 = NULL;
 
-	if (ModeNo > 0x13)
-		modeflag = pVBInfo->EModeIDTable[ModeIdIndex].Ext_ModeFlag;
-	else
-		modeflag = pVBInfo->SModeIDTable[ModeIdIndex].St_ModeFlag;
-
+	modeflag = pVBInfo->EModeIDTable[ModeIdIndex].Ext_ModeFlag;
 	tempbx = 3;
 	if (pVBInfo->LCDInfo & EnableScalingLCD)
 		LCDPtr1 =
@@ -2550,7 +2194,7 @@ static void XGI_SetLVDSRegs(unsigned short ModeNo, unsigned short ModeIdIndex,
 					  pVBInfo);
 	else
 		LCDPtr =
-		    (struct XGI330_LCDDataDesStruct *)
+		    (struct XGI_LCDDesStruct *)
 				XGI_GetLcdPtr(
 					  tempbx,
 					  ModeNo,
@@ -2829,12 +2473,8 @@ static unsigned char XGI_GetVCLKPtr(unsigned short RefreshRateTableIndex,
 	unsigned short index, modeflag;
 	unsigned char tempal;
 
-	if (ModeNo <= 0x13)
-		/* si+St_ResInfo */
-		modeflag = pVBInfo->SModeIDTable[ModeIdIndex].St_ModeFlag;
-	else
-		/* si+Ext_ResInfo */
-		modeflag = pVBInfo->EModeIDTable[ModeIdIndex].Ext_ModeFlag;
+	/* si+Ext_ResInfo */
+	modeflag = pVBInfo->EModeIDTable[ModeIdIndex].Ext_ModeFlag;
 
 	if ((pVBInfo->SetFlag & ProgrammingCRT2) &&
 	    (!(pVBInfo->LCDInfo & EnableScalingLCD))) { /* {LCDA/LCDB} */
@@ -2895,9 +2535,6 @@ static unsigned char XGI_GetVCLKPtr(unsigned short RefreshRateTableIndex,
 	if ((pVBInfo->LCDInfo & EnableScalingLCD) && (modeflag & Charx8Dot))
 		tempal = tempal ^ tempal; /* ; set to VCLK25MHz always */
 
-	if (ModeNo <= 0x13)
-		return tempal;
-
 	tempal = pVBInfo->RefIndex[RefreshRateTableIndex].Ext_CRTVCLK;
 	return tempal;
 }
@@ -3079,11 +2716,7 @@ static void XGI_GetVBInfo(unsigned short ModeNo, unsigned short ModeIdIndex,
 {
 	unsigned short tempax, push, tempbx, temp, modeflag;
 
-	if (ModeNo <= 0x13)
-		modeflag = pVBInfo->SModeIDTable[ModeIdIndex].St_ModeFlag;
-	else
-		modeflag = pVBInfo->EModeIDTable[ModeIdIndex].Ext_ModeFlag;
-
+	modeflag = pVBInfo->EModeIDTable[ModeIdIndex].Ext_ModeFlag;
 	pVBInfo->SetFlag = 0;
 	pVBInfo->ModeType = modeflag & ModeTypeMask;
 	tempbx = 0;
@@ -3283,17 +2916,8 @@ static void XGI_GetTVInfo(unsigned short ModeNo, unsigned short ModeIdIndex,
 	resinfo = 0;
 
 	if (pVBInfo->VBInfo & SetCRT2ToTV) {
-		if (ModeNo <= 0x13) {
-			modeflag = pVBInfo->SModeIDTable[ModeIdIndex].
-					St_ModeFlag; /* si+St_ModeFlag */
-			resinfo = pVBInfo->SModeIDTable[ModeIdIndex].
-					St_ResInfo; /* si+St_ResInfo */
-		} else {
-			modeflag = pVBInfo->EModeIDTable[ModeIdIndex].
-					Ext_ModeFlag;
-			resinfo = pVBInfo->EModeIDTable[ModeIdIndex].
-					Ext_RESINFO; /* si+Ext_ResInfo */
-		}
+		modeflag = pVBInfo->EModeIDTable[ModeIdIndex].Ext_ModeFlag;
+		resinfo = pVBInfo->EModeIDTable[ModeIdIndex].Ext_RESINFO;
 
 		if (pVBInfo->VBInfo & SetCRT2ToTV) {
 			temp = xgifb_reg_get(pVBInfo->P3d4, 0x35);
@@ -3380,15 +3004,9 @@ static unsigned char XGI_GetLCDInfo(unsigned short ModeNo,
 	pVBInfo->LCDTypeInfo = 0;
 	pVBInfo->LCDInfo = 0;
 
-	if (ModeNo <= 0x13) {
-		/* si+St_ModeFlag // */
-		modeflag = pVBInfo->SModeIDTable[ModeIdIndex].St_ModeFlag;
-	} else {
-		modeflag = pVBInfo->EModeIDTable[ModeIdIndex].Ext_ModeFlag;
-		/* si+Ext_ResInfo // */
-		resinfo = pVBInfo->EModeIDTable[ModeIdIndex].Ext_RESINFO;
-	}
-
+	modeflag = pVBInfo->EModeIDTable[ModeIdIndex].Ext_ModeFlag;
+	/* si+Ext_ResInfo // */
+	resinfo = pVBInfo->EModeIDTable[ModeIdIndex].Ext_RESINFO;
 	temp = xgifb_reg_get(pVBInfo->P3d4, 0x36); /* Get LCD Res.Info */
 	tempbx = temp & 0x0F;
 
@@ -3442,8 +3060,8 @@ static unsigned char XGI_GetLCDInfo(unsigned short ModeNo,
 
 	if (pVBInfo->IF_DEF_LVDS == 0) {
 		if ((pVBInfo->LCDResInfo == Panel_1400x1050) && (pVBInfo->VBInfo
-				& SetCRT2ToLCD) && (ModeNo > 0x13) && (resinfo
-				== 9) && (!(tempbx & EnableScalingLCD)))
+				& SetCRT2ToLCD) && (resinfo == 9) &&
+				(!(tempbx & EnableScalingLCD)))
 			/* set to center in 1280x1024 LCDB for Panel_1400x1050 */
 			tempbx |= SetLCDtoNonExpanding;
 	}
@@ -3453,12 +3071,9 @@ static unsigned char XGI_GetLCDInfo(unsigned short ModeNo,
 			if (!(tempbx & SetLCDtoNonExpanding)) {
 				tempbx |= XGI_EnableLVDSDDA;
 			} else {
-				if (ModeNo > 0x13) {
-					if (pVBInfo->LCDResInfo
-							== Panel_1024x768) {
-						if (resinfo == 4) {/* 512x384 */
-							tempbx |= XGI_EnableLVDSDDA;
-						}
+				if (pVBInfo->LCDResInfo == Panel_1024x768) {
+					if (resinfo == 4) {/* 512x384 */
+						tempbx |= XGI_EnableLVDSDDA;
 					}
 				}
 			}
@@ -3474,56 +3089,17 @@ static unsigned char XGI_GetLCDInfo(unsigned short ModeNo,
 
 	pVBInfo->LCDInfo = tempbx;
 
-	if (pVBInfo->IF_DEF_LVDS == 0) {
-		if (tempax & (LockLCDBToA | StLCDBToA)) {
-			if (pVBInfo->VBInfo & SetInSlaveMode) {
-				if (!(tempax & LockLCDBToA)) {
-					if (ModeNo <= 0x13) {
-						pVBInfo->VBInfo &=
-							~(SetSimuScanMode |
-							  SetInSlaveMode |
-							  SetCRT2ToLCD);
-						pVBInfo->VBInfo |=
-							XGI_SetCRT2ToLCDA |
-							SetCRT2ToDualEdge;
-					}
-				}
-			}
-		}
-	}
-
 	return 1;
 }
 
 unsigned char XGI_SearchModeID(unsigned short ModeNo,
 		unsigned short *ModeIdIndex, struct vb_device_info *pVBInfo)
 {
-	if (ModeNo <= 5)
-		ModeNo |= 1;
-	if (ModeNo <= 0x13) {
-		for (*ModeIdIndex = 0;; (*ModeIdIndex)++) {
-			if (pVBInfo->SModeIDTable[*ModeIdIndex].St_ModeID ==
-			    ModeNo)
-				break;
-			if (pVBInfo->SModeIDTable[*ModeIdIndex].St_ModeID ==
-			    0xFF)
-				return 0;
-		}
-
-		if (ModeNo == 0x07)
-			(*ModeIdIndex)++; /* 400 lines */
-		if (ModeNo <= 3)
-			(*ModeIdIndex) += 2; /* 400 lines */
-		/* else 350 lines */
-	} else {
-		for (*ModeIdIndex = 0;; (*ModeIdIndex)++) {
-			if (pVBInfo->EModeIDTable[*ModeIdIndex].Ext_ModeID ==
-			    ModeNo)
-				break;
-			if (pVBInfo->EModeIDTable[*ModeIdIndex].Ext_ModeID ==
-			    0xFF)
-				return 0;
-		}
+	for (*ModeIdIndex = 0;; (*ModeIdIndex)++) {
+		if (pVBInfo->EModeIDTable[*ModeIdIndex].Ext_ModeID == ModeNo)
+			break;
+		if (pVBInfo->EModeIDTable[*ModeIdIndex].Ext_ModeID == 0xFF)
+			return 0;
 	}
 
 	return 1;
@@ -3789,22 +3365,17 @@ static void XGI_GetCRT2ResInfo(unsigned short ModeNo,
 {
 	unsigned short xres, yres, modeflag, resindex;
 
-	resindex = XGI_GetResInfo(ModeNo, ModeIdIndex, pVBInfo);
-	if (ModeNo <= 0x13) {
-		xres = pVBInfo->StResInfo[resindex].HTotal;
-		yres = pVBInfo->StResInfo[resindex].VTotal;
-	} else {
-		xres = pVBInfo->ModeResInfo[resindex].HTotal; /* xres->ax */
-		yres = pVBInfo->ModeResInfo[resindex].VTotal; /* yres->bx */
-		/* si+St_ModeFlag */
-		modeflag = pVBInfo->EModeIDTable[ModeIdIndex].Ext_ModeFlag;
+	resindex = pVBInfo->EModeIDTable[ModeIdIndex].Ext_RESINFO;
+	xres = pVBInfo->ModeResInfo[resindex].HTotal; /* xres->ax */
+	yres = pVBInfo->ModeResInfo[resindex].VTotal; /* yres->bx */
+	/* si+St_ModeFlag */
+	modeflag = pVBInfo->EModeIDTable[ModeIdIndex].Ext_ModeFlag;
 
-		if (modeflag & HalfDCLK)
-			xres *= 2;
+	if (modeflag & HalfDCLK)
+		xres *= 2;
 
-		if (modeflag & DoubleScanMode)
-			yres *= 2;
-	}
+	if (modeflag & DoubleScanMode)
+		yres *= 2;
 
 	if (pVBInfo->VBInfo & SetCRT2ToLCD) {
 		if (pVBInfo->IF_DEF_LVDS == 0) {
@@ -3868,37 +3439,23 @@ static void XGI_GetRAMDAC2DATA(unsigned short ModeNo,
 			       struct vb_device_info *pVBInfo)
 {
 	unsigned short tempax, tempbx, temp1, temp2, modeflag = 0, tempcx,
-			StandTableIndex, CRT1Index;
+			CRT1Index;
 
 	pVBInfo->RVBHCMAX = 1;
 	pVBInfo->RVBHCFACT = 1;
-
-	if (ModeNo <= 0x13) {
-		modeflag = pVBInfo->SModeIDTable[ModeIdIndex].St_ModeFlag;
-		StandTableIndex = XGI_GetModePtr(ModeNo, ModeIdIndex, pVBInfo);
-		tempax = pVBInfo->StandTable[StandTableIndex].CRTC[0];
-		tempbx = pVBInfo->StandTable[StandTableIndex].CRTC[6];
-		temp1 = pVBInfo->StandTable[StandTableIndex].CRTC[7];
-	} else {
-		modeflag = pVBInfo->EModeIDTable[ModeIdIndex].Ext_ModeFlag;
-		CRT1Index = pVBInfo->RefIndex[RefreshRateTableIndex].
-				Ext_CRT1CRTC;
-		CRT1Index &= IndexMask;
-		temp1 = (unsigned short) pVBInfo->
-			XGINEWUB_CRT1Table[CRT1Index].CR[0];
-		temp2 = (unsigned short) pVBInfo->
-			XGINEWUB_CRT1Table[CRT1Index].CR[5];
-		tempax = (temp1 & 0xFF) | ((temp2 & 0x03) << 8);
-		tempbx = (unsigned short) pVBInfo->
-			XGINEWUB_CRT1Table[CRT1Index].CR[8];
-		tempcx = (unsigned short) pVBInfo->
-			XGINEWUB_CRT1Table[CRT1Index].CR[14] << 8;
-		tempcx &= 0x0100;
-		tempcx = tempcx << 2;
-		tempbx |= tempcx;
-		temp1 = (unsigned short) pVBInfo->
-			XGINEWUB_CRT1Table[CRT1Index].CR[9];
-	}
+	modeflag = pVBInfo->EModeIDTable[ModeIdIndex].Ext_ModeFlag;
+	CRT1Index = pVBInfo->RefIndex[RefreshRateTableIndex].Ext_CRT1CRTC;
+	CRT1Index &= IndexMask;
+	temp1 = (unsigned short) pVBInfo->XGINEWUB_CRT1Table[CRT1Index].CR[0];
+	temp2 = (unsigned short) pVBInfo->XGINEWUB_CRT1Table[CRT1Index].CR[5];
+	tempax = (temp1 & 0xFF) | ((temp2 & 0x03) << 8);
+	tempbx = (unsigned short) pVBInfo->XGINEWUB_CRT1Table[CRT1Index].CR[8];
+	tempcx = (unsigned short)
+			pVBInfo->XGINEWUB_CRT1Table[CRT1Index].CR[14] << 8;
+	tempcx &= 0x0100;
+	tempcx = tempcx << 2;
+	tempbx |= tempcx;
+	temp1 = (unsigned short) pVBInfo->XGINEWUB_CRT1Table[CRT1Index].CR[9];
 
 	if (temp1 & 0x01)
 		tempbx |= 0x0100;
@@ -3928,16 +3485,9 @@ static void XGI_GetCRT2Data(unsigned short ModeNo, unsigned short ModeIdIndex,
 	struct SiS_LCDData *LCDPtr = NULL;
 	struct SiS_TVData *TVPtr = NULL;
 
-	if (ModeNo <= 0x13) {
-		/* si+St_ResInfo */
-		modeflag = pVBInfo->SModeIDTable[ModeIdIndex].St_ModeFlag;
-		resinfo = pVBInfo->SModeIDTable[ModeIdIndex].St_ResInfo;
-	} else {
-		/* si+Ext_ResInfo */
-		modeflag = pVBInfo->EModeIDTable[ModeIdIndex].Ext_ModeFlag;
-		resinfo = pVBInfo->EModeIDTable[ModeIdIndex].Ext_RESINFO;
-	}
-
+	/* si+Ext_ResInfo */
+	modeflag = pVBInfo->EModeIDTable[ModeIdIndex].Ext_ModeFlag;
+	resinfo = pVBInfo->EModeIDTable[ModeIdIndex].Ext_RESINFO;
 	pVBInfo->NewFlickerMode = 0;
 	pVBInfo->RVBHRS = 50;
 
@@ -4141,11 +3691,7 @@ static unsigned short XGI_GetColorDepth(unsigned short ModeNo,
 	short index;
 	unsigned short modeflag;
 
-	if (ModeNo <= 0x13)
-		modeflag = pVBInfo->SModeIDTable[ModeIdIndex].St_ModeFlag;
-	else
-		modeflag = pVBInfo->EModeIDTable[ModeIdIndex].Ext_ModeFlag;
-
+	modeflag = pVBInfo->EModeIDTable[ModeIdIndex].Ext_ModeFlag;
 	index = (modeflag & ModeTypeMask) - ModeEGA;
 
 	if (index < 0)
@@ -4164,11 +3710,7 @@ static unsigned short XGI_GetOffset(unsigned short ModeNo,
 			ColorDepth[] = { 0x01, 0x02, 0x04 };
 
 	modeinfo = pVBInfo->EModeIDTable[ModeIdIndex].Ext_ModeInfo;
-	if (ModeNo <= 0x14)
-		infoflag = 0;
-	else
-		infoflag = pVBInfo->
-				RefIndex[RefreshRateTableIndex].Ext_InfoFlag;
+	infoflag = pVBInfo->RefIndex[RefreshRateTableIndex].Ext_InfoFlag;
 
 	index = (modeinfo >> 8) & 0xFF;
 
@@ -4228,12 +3770,9 @@ static void XGI_PreSetGroup1(unsigned short ModeNo, unsigned short ModeIdIndex,
 {
 	unsigned short tempcx = 0, CRT1Index = 0, resinfo = 0;
 
-	if (ModeNo > 0x13) {
-		CRT1Index = pVBInfo->
-				RefIndex[RefreshRateTableIndex].Ext_CRT1CRTC;
-		CRT1Index &= IndexMask;
-		resinfo = pVBInfo->EModeIDTable[ModeIdIndex].Ext_RESINFO;
-	}
+	CRT1Index = pVBInfo->RefIndex[RefreshRateTableIndex].Ext_CRT1CRTC;
+	CRT1Index &= IndexMask;
+	resinfo = pVBInfo->EModeIDTable[ModeIdIndex].Ext_RESINFO;
 
 	XGI_SetCRT2Offset(ModeNo, ModeIdIndex, RefreshRateTableIndex,
 			HwDeviceExtension, pVBInfo);
@@ -4254,17 +3793,10 @@ static void XGI_SetGroup1(unsigned short ModeNo, unsigned short ModeIdIndex,
 	unsigned short temp = 0, tempax = 0, tempbx = 0, tempcx = 0,
 			pushbx = 0, CRT1Index = 0, modeflag, resinfo = 0;
 
-	if (ModeNo > 0x13) {
-		CRT1Index = pVBInfo->
-				RefIndex[RefreshRateTableIndex].Ext_CRT1CRTC;
-		CRT1Index &= IndexMask;
-		resinfo = pVBInfo->EModeIDTable[ModeIdIndex].Ext_RESINFO;
-	}
-
-	if (ModeNo <= 0x13)
-		modeflag = pVBInfo->SModeIDTable[ModeIdIndex].St_ModeFlag;
-	else
-		modeflag = pVBInfo->EModeIDTable[ModeIdIndex].Ext_ModeFlag;
+	CRT1Index = pVBInfo->RefIndex[RefreshRateTableIndex].Ext_CRT1CRTC;
+	CRT1Index &= IndexMask;
+	resinfo = pVBInfo->EModeIDTable[ModeIdIndex].Ext_RESINFO;
+	modeflag = pVBInfo->EModeIDTable[ModeIdIndex].Ext_ModeFlag;
 
 	/* bainy change table name */
 	if (modeflag & HalfDCLK) {
@@ -4422,18 +3954,11 @@ static void XGI_SetLockRegs(unsigned short ModeNo, unsigned short ModeIdIndex,
 	unsigned short push1, push2, tempax, tempbx = 0, tempcx, temp, resinfo,
 			modeflag, CRT1Index;
 
-	if (ModeNo <= 0x13) {
-		/* si+St_ResInfo */
-		modeflag = pVBInfo->SModeIDTable[ModeIdIndex].St_ModeFlag;
-		resinfo = pVBInfo->SModeIDTable[ModeIdIndex].St_ResInfo;
-	} else {
-		/* si+Ext_ResInfo */
-		modeflag = pVBInfo->EModeIDTable[ModeIdIndex].Ext_ModeFlag;
-		resinfo = pVBInfo->EModeIDTable[ModeIdIndex].Ext_RESINFO;
-		CRT1Index = pVBInfo->
-				RefIndex[RefreshRateTableIndex].Ext_CRT1CRTC;
-		CRT1Index &= IndexMask;
-	}
+	/* si+Ext_ResInfo */
+	modeflag = pVBInfo->EModeIDTable[ModeIdIndex].Ext_ModeFlag;
+	resinfo = pVBInfo->EModeIDTable[ModeIdIndex].Ext_RESINFO;
+	CRT1Index = pVBInfo->RefIndex[RefreshRateTableIndex].Ext_CRT1CRTC;
+	CRT1Index &= IndexMask;
 
 	if (!(pVBInfo->VBInfo & SetInSlaveMode))
 		return;
@@ -4500,8 +4025,7 @@ static void XGI_SetLockRegs(unsigned short ModeNo, unsigned short ModeIdIndex,
 			temp -= 6;
 			if (pVBInfo->TVInfo & TVSimuMode) {
 				temp -= 4;
-				if (ModeNo > 0x13)
-					temp -= 10;
+				temp -= 10;
 			}
 		}
 	} else {
@@ -4523,14 +4047,6 @@ static void XGI_SetLockRegs(unsigned short ModeNo, unsigned short ModeIdIndex,
 			if (pVBInfo->LCDResInfo != Panel_1280x960 &&
 			    pVBInfo->VGAHDE >= 800) {
 				temp -= 7;
-				if (pVBInfo->ModeType == ModeEGA &&
-				    pVBInfo->VGAVDE == 1024) {
-					temp += 15;
-					if (pVBInfo->LCDResInfo !=
-						Panel_1280x1024)
-						temp += 7;
-				}
-
 				if (pVBInfo->VGAHDE >= 1280 &&
 				    pVBInfo->LCDResInfo != Panel_1280x960 &&
 				    (pVBInfo->LCDInfo & LCDNonExpanding))
@@ -4546,48 +4062,7 @@ static void XGI_SetLockRegs(unsigned short ModeNo, unsigned short ModeIdIndex,
 
 	if (pVBInfo->VBInfo & SetCRT2ToTV) {
 		if (pVBInfo->TVInfo & TVSimuMode) {
-			if ((ModeNo == 0x06) || (ModeNo == 0x10) || (ModeNo
-					== 0x11) || (ModeNo == 0x13) || (ModeNo
-					== 0x0F)) {
-				xgifb_reg_set(pVBInfo->Part1Port, 0x07, 0x5b);
-				xgifb_reg_set(pVBInfo->Part1Port, 0x08, 0x03);
-			}
-
-			if ((ModeNo == 0x00) || (ModeNo == 0x01)) {
-				if (pVBInfo->TVInfo & SetNTSCTV) {
-					xgifb_reg_set(pVBInfo->Part1Port,
-							0x07, 0x2A);
-					xgifb_reg_set(pVBInfo->Part1Port,
-							0x08, 0x61);
-				} else {
-					xgifb_reg_set(pVBInfo->Part1Port,
-							0x07, 0x2A);
-					xgifb_reg_set(pVBInfo->Part1Port,
-							0x08, 0x41);
-					xgifb_reg_set(pVBInfo->Part1Port,
-							0x0C, 0xF0);
-				}
-			}
-
-			if ((ModeNo == 0x02) || (ModeNo == 0x03) || (ModeNo
-					== 0x07)) {
-				if (pVBInfo->TVInfo & SetNTSCTV) {
-					xgifb_reg_set(pVBInfo->Part1Port,
-							0x07, 0x54);
-					xgifb_reg_set(pVBInfo->Part1Port,
-							0x08, 0x00);
-				} else {
-					xgifb_reg_set(pVBInfo->Part1Port,
-							0x07, 0x55);
-					xgifb_reg_set(pVBInfo->Part1Port,
-							0x08, 0x00);
-					xgifb_reg_set(pVBInfo->Part1Port,
-							0x0C, 0xF0);
-				}
-			}
-
-			if ((ModeNo == 0x04) || (ModeNo == 0x05) || (ModeNo
-					== 0x0D) || (ModeNo == 0x50)) {
+			if (ModeNo == 0x50) {
 				if (pVBInfo->TVInfo & SetNTSCTV) {
 					xgifb_reg_set(pVBInfo->Part1Port,
 							0x07, 0x30);
@@ -4796,18 +4271,10 @@ static void XGI_SetGroup2(unsigned short ModeNo, unsigned short ModeIdIndex,
 
 	unsigned long longtemp, tempeax, tempebx, temp2, tempecx;
 
-	if (ModeNo <= 0x13) {
-		/* si+St_ResInfo */
-		modeflag = pVBInfo->SModeIDTable[ModeIdIndex].St_ModeFlag;
-		resinfo = pVBInfo->SModeIDTable[ModeIdIndex].St_ResInfo;
-		crt2crtc = pVBInfo->SModeIDTable[ModeIdIndex].St_CRT2CRTC;
-	} else {
-		/* si+Ext_ResInfo */
-		modeflag = pVBInfo->EModeIDTable[ModeIdIndex].Ext_ModeFlag;
-		resinfo = pVBInfo->EModeIDTable[ModeIdIndex].Ext_RESINFO;
-		crt2crtc = pVBInfo->RefIndex[RefreshRateTableIndex].
-				Ext_CRT2CRTC;
-	}
+	/* si+Ext_ResInfo */
+	modeflag = pVBInfo->EModeIDTable[ModeIdIndex].Ext_ModeFlag;
+	resinfo = pVBInfo->EModeIDTable[ModeIdIndex].Ext_RESINFO;
+	crt2crtc = pVBInfo->RefIndex[RefreshRateTableIndex].Ext_CRT2CRTC;
 
 	tempax = 0;
 
@@ -5245,18 +4712,11 @@ static void XGI_SetLCDRegs(unsigned short ModeNo, unsigned short ModeIdIndex,
 
 	struct XGI_LCDDesStruct *LCDBDesPtr = NULL;
 
-	if (ModeNo <= 0x13) {
-		/* si+St_ResInfo */
-		modeflag = pVBInfo->SModeIDTable[ModeIdIndex].St_ModeFlag;
-		resinfo = pVBInfo->SModeIDTable[ModeIdIndex].St_ResInfo;
-	} else {
-		/* si+Ext_ResInfo */
-		modeflag = pVBInfo->EModeIDTable[ModeIdIndex].Ext_ModeFlag;
-		resinfo = pVBInfo->EModeIDTable[ModeIdIndex].Ext_RESINFO;
-		CRT1Index = pVBInfo->RefIndex[RefreshRateTableIndex].
-			Ext_CRT1CRTC;
-		CRT1Index &= IndexMask;
-	}
+	/* si+Ext_ResInfo */
+	modeflag = pVBInfo->EModeIDTable[ModeIdIndex].Ext_ModeFlag;
+	resinfo = pVBInfo->EModeIDTable[ModeIdIndex].Ext_RESINFO;
+	CRT1Index = pVBInfo->RefIndex[RefreshRateTableIndex].Ext_CRT1CRTC;
+	CRT1Index &= IndexMask;
 
 	if (!(pVBInfo->VBInfo & SetCRT2ToLCD))
 		return;
@@ -5274,16 +4734,6 @@ static void XGI_SetLCDRegs(unsigned short ModeNo, unsigned short ModeIdIndex,
 	xgifb_reg_and_or(pVBInfo->Part2Port, 0x2B, 0x0F, temp);
 	temp = 0x01;
 
-	if (pVBInfo->LCDResInfo == Panel_1280x1024) {
-		if (pVBInfo->ModeType == ModeEGA) {
-			if (pVBInfo->VGAHDE >= 1024) {
-				temp = 0x02;
-				if (pVBInfo->LCDInfo & XGI_LCDVESATiming)
-					temp = 0x01;
-			}
-		}
-	}
-
 	xgifb_reg_set(pVBInfo->Part2Port, 0x0B, temp);
 	tempbx = pVBInfo->VDE; /* RTVACTEO=(VDE-1)&0xFF */
 	push1 = tempbx;
@@ -5542,12 +4992,8 @@ static void XGI_SetGroup3(unsigned short ModeNo, unsigned short ModeIdIndex,
 	unsigned char *tempdi;
 	unsigned short modeflag;
 
-	if (ModeNo <= 0x13)
-		/* si+St_ResInfo */
-		modeflag = pVBInfo->SModeIDTable[ModeIdIndex].St_ModeFlag;
-	else
-		/* si+Ext_ResInfo */
-		modeflag = pVBInfo->EModeIDTable[ModeIdIndex].Ext_ModeFlag;
+	/* si+Ext_ResInfo */
+	modeflag = pVBInfo->EModeIDTable[ModeIdIndex].Ext_ModeFlag;
 
 	xgifb_reg_set(pVBInfo->Part3Port, 0x00, 0x00);
 	if (pVBInfo->TVInfo & TVSetPAL) {
@@ -5605,13 +5051,8 @@ static void XGI_SetGroup4(unsigned short ModeNo, unsigned short ModeIdIndex,
 
 	unsigned long tempebx, tempeax, templong;
 
-	if (ModeNo <= 0x13)
-		/* si+St_ResInfo */
-		modeflag = pVBInfo->SModeIDTable[ModeIdIndex].St_ModeFlag;
-	else
-		/* si+Ext_ResInfo */
-		modeflag = pVBInfo->EModeIDTable[ModeIdIndex].Ext_ModeFlag;
-
+	/* si+Ext_ResInfo */
+	modeflag = pVBInfo->EModeIDTable[ModeIdIndex].Ext_ModeFlag;
 	temp = pVBInfo->RVBHCFACT;
 	xgifb_reg_set(pVBInfo->Part4Port, 0x13, temp);
 
@@ -5818,32 +5259,22 @@ static unsigned char XGI_XG21CheckLVDSMode(struct xgifb_video_info *xgifb_info,
 {
 	unsigned short xres, yres, colordepth, modeflag, resindex;
 
-	resindex = XGI_GetResInfo(ModeNo, ModeIdIndex, pVBInfo);
-	if (ModeNo <= 0x13) {
-		xres = pVBInfo->StResInfo[resindex].HTotal;
-		yres = pVBInfo->StResInfo[resindex].VTotal;
-		/* si+St_ResInfo */
-		modeflag = pVBInfo->SModeIDTable[ModeIdIndex].St_ModeFlag;
-	} else {
-		xres = pVBInfo->ModeResInfo[resindex].HTotal; /* xres->ax */
-		yres = pVBInfo->ModeResInfo[resindex].VTotal; /* yres->bx */
-		/* si+St_ModeFlag */
-		modeflag = pVBInfo->EModeIDTable[ModeIdIndex].Ext_ModeFlag;
-	}
+	resindex = pVBInfo->EModeIDTable[ModeIdIndex].Ext_RESINFO;
+	xres = pVBInfo->ModeResInfo[resindex].HTotal; /* xres->ax */
+	yres = pVBInfo->ModeResInfo[resindex].VTotal; /* yres->bx */
+	/* si+St_ModeFlag */
+	modeflag = pVBInfo->EModeIDTable[ModeIdIndex].Ext_ModeFlag;
 
 	if (!(modeflag & Charx8Dot)) {
 		xres /= 9;
 		xres *= 8;
 	}
 
-	if (ModeNo > 0x13) {
-		if ((ModeNo > 0x13) && (modeflag & HalfDCLK))
-			xres *= 2;
-
-		if ((ModeNo > 0x13) && (modeflag & DoubleScanMode))
-			yres *= 2;
+	if ((ModeNo > 0x13) && (modeflag & HalfDCLK))
+		xres *= 2;
 
-	}
+	if ((ModeNo > 0x13) && (modeflag & DoubleScanMode))
+		yres *= 2;
 
 	if (xres > xgifb_info->lvds_data.LVDSHDE)
 		return 0;
@@ -5851,16 +5282,11 @@ static unsigned char XGI_XG21CheckLVDSMode(struct xgifb_video_info *xgifb_info,
 	if (yres > xgifb_info->lvds_data.LVDSVDE)
 		return 0;
 
-	if (ModeNo > 0x13) {
-		if (xres != xgifb_info->lvds_data.LVDSHDE ||
-		    yres != xgifb_info->lvds_data.LVDSVDE) {
-			colordepth = XGI_GetColorDepth(ModeNo,
-						       ModeIdIndex,
-						       pVBInfo);
-			if (colordepth > 2)
-				return 0;
-
-		}
+	if (xres != xgifb_info->lvds_data.LVDSHDE ||
+	    yres != xgifb_info->lvds_data.LVDSVDE) {
+		colordepth = XGI_GetColorDepth(ModeNo, ModeIdIndex, pVBInfo);
+		if (colordepth > 2)
+			return 0;
 	}
 	return 1;
 }
@@ -5895,18 +5321,11 @@ static void xgifb_set_lvds(struct xgifb_video_info *xgifb_info,
 	else
 		XGI_SetXG21FPBits(pVBInfo);
 
-	resindex = XGI_GetResInfo(ModeNo, ModeIdIndex, pVBInfo);
-	if (ModeNo <= 0x13) {
-		xres = pVBInfo->StResInfo[resindex].HTotal;
-		yres = pVBInfo->StResInfo[resindex].VTotal;
-		/* si+St_ResInfo */
-		modeflag = pVBInfo->SModeIDTable[ModeIdIndex].St_ModeFlag;
-	} else {
-		xres = pVBInfo->ModeResInfo[resindex].HTotal; /* xres->ax */
-		yres = pVBInfo->ModeResInfo[resindex].VTotal; /* yres->bx */
-		/* si+St_ModeFlag */
-		modeflag = pVBInfo->EModeIDTable[ModeIdIndex].Ext_ModeFlag;
-	}
+	resindex = pVBInfo->EModeIDTable[ModeIdIndex].Ext_RESINFO;
+	xres = pVBInfo->ModeResInfo[resindex].HTotal; /* xres->ax */
+	yres = pVBInfo->ModeResInfo[resindex].VTotal; /* yres->bx */
+	/* si+St_ModeFlag */
+	modeflag = pVBInfo->EModeIDTable[ModeIdIndex].Ext_ModeFlag;
 
 	if (!(modeflag & Charx8Dot))
 		xres = xres * 8 / 9;
@@ -5914,8 +5333,6 @@ static void xgifb_set_lvds(struct xgifb_video_info *xgifb_info,
 	LVDSHT = xgifb_info->lvds_data.LVDSHT;
 
 	LVDSHBS = xres + (xgifb_info->lvds_data.LVDSHDE - xres) / 2;
-	if ((ModeNo <= 0x13) && (modeflag & HalfDCLK))
-		LVDSHBS -= xres / 4;
 
 	if (LVDSHBS > LVDSHT)
 		LVDSHBS -= LVDSHT;
@@ -5933,7 +5350,7 @@ static void xgifb_set_lvds(struct xgifb_video_info *xgifb_info,
 	LVDSVT = xgifb_info->lvds_data.LVDSVT;
 
 	LVDSVBS = yres + (xgifb_info->lvds_data.LVDSVDE - yres) / 2;
-	if ((ModeNo > 0x13) && (modeflag & DoubleScanMode))
+	if (modeflag & DoubleScanMode)
 		LVDSVBS += yres / 2;
 
 	if (LVDSVBS > LVDSVT)
@@ -6527,7 +5944,7 @@ static void XGI_SetAntiFlicker(unsigned short ModeNo,
 			       unsigned short ModeIdIndex,
 			       struct vb_device_info *pVBInfo)
 {
-	unsigned short tempbx, index;
+	unsigned short tempbx;
 
 	unsigned char tempah;
 
@@ -6536,13 +5953,6 @@ static void XGI_SetAntiFlicker(unsigned short ModeNo,
 
 	tempbx = XGI_GetTVPtrIndex(pVBInfo);
 	tempbx &= 0xFE;
-
-	if (ModeNo <= 0x13)
-		index = pVBInfo->SModeIDTable[ModeIdIndex].VB_StTVFlickerIndex;
-	else
-		index = pVBInfo->EModeIDTable[ModeIdIndex].VB_ExtTVFlickerIndex;
-
-	tempbx += index;
 	tempah = TVAntiFlickList[tempbx];
 	tempah = tempah << 4;
 
@@ -6553,19 +5963,12 @@ static void XGI_SetEdgeEnhance(unsigned short ModeNo,
 			       unsigned short ModeIdIndex,
 			       struct vb_device_info *pVBInfo)
 {
-	unsigned short tempbx, index;
+	unsigned short tempbx;
 
 	unsigned char tempah;
 
 	tempbx = XGI_GetTVPtrIndex(pVBInfo);
 	tempbx &= 0xFE;
-
-	if (ModeNo <= 0x13)
-		index = pVBInfo->SModeIDTable[ModeIdIndex].VB_StTVEdgeIndex;
-	else
-		index = pVBInfo->EModeIDTable[ModeIdIndex].VB_ExtTVEdgeIndex;
-
-	tempbx += index;
 	tempah = TVEdgeList[tempbx];
 	tempah = tempah << 5;
 
@@ -6631,13 +6034,7 @@ static void XGI_SetYFilter(unsigned short ModeNo, unsigned short ModeIdIndex,
 		return;
 	}
 
-	if (ModeNo <= 0x13)
-		tempal = pVBInfo->SModeIDTable[ModeIdIndex].
-				VB_StTVYFilterIndex;
-	else
-		tempal = pVBInfo->EModeIDTable[ModeIdIndex].
-				VB_ExtTVYFilterIndex;
-
+	tempal = pVBInfo->EModeIDTable[ModeIdIndex].VB_ExtTVYFilterIndex;
 	if (tempcl == 0)
 		index = tempal * 4;
 	else
@@ -6712,16 +6109,14 @@ static void XGI_SetCRT2ModeRegs(unsigned short ModeNo,
 		if (pVBInfo->VBInfo & (SetCRT2ToRAMDAC | SetCRT2ToTV
 				| SetCRT2ToLCD)) {
 			tempah = 0x40; /* BTDRAM */
-			if (ModeNo > 0x13) {
-				tempcl = pVBInfo->ModeType;
-				tempcl -= ModeVGA;
-				if (tempcl >= 0) {
-					/* BT Color */
-					tempah = (0x008 >> tempcl);
-					if (tempah == 0)
-						tempah = 1;
-					tempah |= 0x040;
-				}
+			tempcl = pVBInfo->ModeType;
+			tempcl -= ModeVGA;
+			if (tempcl >= 0) {
+				/* BT Color */
+				tempah = (0x008 >> tempcl);
+				if (tempah == 0)
+					tempah = 1;
+				tempah |= 0x040;
 			}
 			if (pVBInfo->VBInfo & SetInSlaveMode)
 				tempah ^= 0x50; /* BTDAC */
@@ -6797,10 +6192,8 @@ static void XGI_SetCRT2ModeRegs(unsigned short ModeNo,
 
 		if (pVBInfo->VBInfo & SetCRT2ToTV) {
 			tempah |= 0x020;
-			if (ModeNo > 0x13) {
-				if (pVBInfo->VBInfo & DriverMode)
-					tempah = tempah ^ 0x20;
-			}
+			if (pVBInfo->VBInfo & DriverMode)
+				tempah = tempah ^ 0x20;
 		}
 
 		xgifb_reg_and_or(pVBInfo->Part4Port, 0x0D, ~0x0BF, tempah);
@@ -6925,13 +6318,7 @@ unsigned short XGI_GetRatePtrCRT2(struct xgi_hw_device_info *pXGIHWDE,
 
 	unsigned short RefreshRateTableIndex, i, modeflag, index, temp;
 
-	if (ModeNo <= 0x13)
-		modeflag = pVBInfo->SModeIDTable[ModeIdIndex].St_ModeFlag;
-	else
-		modeflag = pVBInfo->EModeIDTable[ModeIdIndex].Ext_ModeFlag;
-
-	if (ModeNo < 0x14)
-		return 0xFFFF;
+	modeflag = pVBInfo->EModeIDTable[ModeIdIndex].Ext_ModeFlag;
 
 	index = xgifb_reg_get(pVBInfo->P3d4, 0x33);
 	index = index >> pVBInfo->SelectCRT2Rate;
@@ -7297,16 +6684,13 @@ static void XGI_SetCRT1Group(struct xgifb_video_info *xgifb_info,
 		unsigned short ModeNo, unsigned short ModeIdIndex,
 		struct vb_device_info *pVBInfo)
 {
-	unsigned short StandTableIndex, RefreshRateTableIndex, b3CC, temp;
+	unsigned short RefreshRateTableIndex, temp;
 
-	unsigned short XGINew_P3cc = pVBInfo->P3cc;
-
-	StandTableIndex = XGI_GetModePtr(ModeNo, ModeIdIndex, pVBInfo);
-	XGI_SetSeqRegs(ModeNo, StandTableIndex, ModeIdIndex, pVBInfo);
-	outb(pVBInfo->StandTable[StandTableIndex].MISC, pVBInfo->P3c2);
-	XGI_SetCRTCRegs(HwDeviceExtension, StandTableIndex, pVBInfo);
-	XGI_SetATTRegs(ModeNo, StandTableIndex, ModeIdIndex, pVBInfo);
-	XGI_SetGRCRegs(StandTableIndex, pVBInfo);
+	XGI_SetSeqRegs(ModeNo, ModeIdIndex, pVBInfo);
+	outb(pVBInfo->StandTable->MISC, pVBInfo->P3c2);
+	XGI_SetCRTCRegs(HwDeviceExtension, pVBInfo);
+	XGI_SetATTRegs(ModeNo, ModeIdIndex, pVBInfo);
+	XGI_SetGRCRegs(pVBInfo);
 	XGI_ClearExt1Regs(pVBInfo);
 
 	if (HwDeviceExtension->jChipType == XG27) {
@@ -7340,22 +6724,6 @@ static void XGI_SetCRT1Group(struct xgifb_video_info *xgifb_info,
 				RefreshRateTableIndex, pVBInfo);
 	}
 
-	if ((HwDeviceExtension->jChipType >= XG20) &&
-	    (HwDeviceExtension->jChipType < XG27)) { /* fix H/W DCLK/2 bug */
-		if ((ModeNo == 0x00) | (ModeNo == 0x01)) {
-			xgifb_reg_set(pVBInfo->P3c4, 0x2B, 0x4E);
-			xgifb_reg_set(pVBInfo->P3c4, 0x2C, 0xE9);
-			b3CC = (unsigned char) inb(XGINew_P3cc);
-			outb((b3CC |= 0x0C), XGINew_P3cc);
-		} else if ((ModeNo == 0x04) | (ModeNo == 0x05) | (ModeNo
-				== 0x0D)) {
-			xgifb_reg_set(pVBInfo->P3c4, 0x2B, 0x1B);
-			xgifb_reg_set(pVBInfo->P3c4, 0x2C, 0xE3);
-			b3CC = (unsigned char) inb(XGINew_P3cc);
-			outb((b3CC |= 0x0C), XGINew_P3cc);
-		}
-	}
-
 	if (HwDeviceExtension->jChipType >= XG21) {
 		temp = xgifb_reg_get(pVBInfo->P3d4, 0x38);
 		if (temp & 0xA0) {
@@ -7394,7 +6762,7 @@ unsigned char XGISetModeNew(struct xgifb_video_info *xgifb_info,
 	unsigned short ModeIdIndex;
 	struct vb_device_info VBINF;
 	struct vb_device_info *pVBInfo = &VBINF;
-	pVBInfo->BaseAddr = (unsigned long) HwDeviceExtension->pjIOAddress;
+	pVBInfo->BaseAddr = xgifb_info->vga_base;
 	pVBInfo->IF_DEF_LVDS = 0;
 	pVBInfo->IF_DEF_LCDA = 1;
 
@@ -7509,13 +6877,8 @@ unsigned char XGISetModeNew(struct xgifb_video_info *xgifb_info,
 						   pVBInfo))
 				return 0;
 
-		if (ModeNo <= 0x13) {
-			pVBInfo->ModeType = pVBInfo->SModeIDTable[ModeIdIndex].
-						St_ModeFlag & ModeTypeMask;
-		} else {
-			pVBInfo->ModeType = pVBInfo->EModeIDTable[ModeIdIndex].
+		pVBInfo->ModeType = pVBInfo->EModeIDTable[ModeIdIndex].
 						Ext_ModeFlag & ModeTypeMask;
-		}
 
 		pVBInfo->SetFlag = 0;
 		pVBInfo->VBInfo = DisableCRT2Display;
diff --git a/drivers/staging/xgifb/vb_struct.h b/drivers/staging/xgifb/vb_struct.h
index a5bd56af92b..38f47ffc69c 100644
--- a/drivers/staging/xgifb/vb_struct.h
+++ b/drivers/staging/xgifb/vb_struct.h
@@ -10,28 +10,11 @@ struct XGI_LVDSCRT1VDataStruct {
 	unsigned char Reg[7];
 };
 
-struct XGI_StStruct {
-	unsigned char St_ModeID;
-	unsigned short St_ModeFlag;
-	unsigned char St_StTableIndex;
-	unsigned char St_CRT2CRTC;
-	unsigned char St_CRT2CRTC2;
-	unsigned char St_ResInfo;
-	unsigned char VB_StTVFlickerIndex;
-	unsigned char VB_StTVEdgeIndex;
-	unsigned char VB_StTVYFilterIndex;
-};
-
 struct XGI_ExtStruct {
 	unsigned char Ext_ModeID;
 	unsigned short Ext_ModeFlag;
 	unsigned short Ext_ModeInfo;
-	unsigned short Ext_Point;
-	unsigned short Ext_VESAID;
-	unsigned char Ext_VESAMEMSize;
 	unsigned char Ext_RESINFO;
-	unsigned char VB_ExtTVFlickerIndex;
-	unsigned char VB_ExtTVEdgeIndex;
 	unsigned char VB_ExtTVYFilterIndex;
 	unsigned char REFindex;
 };
@@ -68,14 +51,6 @@ struct XGI_LCDDataTablStruct {
 	unsigned short DATAPTR;
 };
 
-struct XGI330_LCDDataDesStruct {
-	unsigned short LCDHDES;
-	unsigned short LCDHRS;
-	unsigned short LCDVDES;
-	unsigned short LCDVRS;
-};
-
-
 struct XGI330_LVDSDataStruct {
 	unsigned short VGAHT;
 	unsigned short VGAVT;
@@ -236,7 +211,6 @@ struct vb_device_info {
 
 	void __iomem *FBAddr;
 	unsigned long BaseAddr;
-	unsigned long RelIO;
 
 	unsigned char (*CR6B)[4];
 	unsigned char (*CR6E)[4];
@@ -314,7 +288,6 @@ struct vb_device_info {
 	struct XGI_TimingHStruct  *TimingH;
 	struct XGI_TimingVStruct  *TimingV;
 
-	struct XGI_StStruct          *SModeIDTable;
 	struct SiS_StandTable_S  *StandTable;
 	struct XGI_ExtStruct         *EModeIDTable;
 	struct XGI_Ext2Struct        *RefIndex;
diff --git a/drivers/staging/xgifb/vb_table.h b/drivers/staging/xgifb/vb_table.h
index e8d6f674b27..d22e599cb30 100644
--- a/drivers/staging/xgifb/vb_table.h
+++ b/drivers/staging/xgifb/vb_table.h
@@ -135,609 +135,92 @@ static unsigned char XGI330_SR33;
 static unsigned char XG40_CRCF = 0x13;
 static unsigned char XG40_DRAMTypeDefinition = 0xFF ;
 
-static struct XGI_StStruct XGI330_SModeIDTable[] = {
-	{0x01, 0x9208, 0x01, 0x00, 0x10, 0x00, 0x00, 0x01, 0x00},
-	{0x01, 0x1210, 0x14, 0x01, 0x00, 0x01, 0x00, 0x01, 0x00},
-	{0x01, 0x1010, 0x17, 0x02, 0x11, 0x00, 0x00, 0x01, 0x01},
-	{0x03, 0x8208, 0x03, 0x00, 0x14, 0x00, 0x00, 0x01, 0x02},
-	{0x03, 0x0210, 0x16, 0x01, 0x04, 0x01, 0x00, 0x01, 0x02},
-	{0x03, 0x0010, 0x18, 0x02, 0x15, 0x00, 0x00, 0x01, 0x03},
-	{0x05, 0x9209, 0x05, 0x00, 0x10, 0x00, 0x00, 0x00, 0x04},
-	{0x06, 0x8209, 0x06, 0x00, 0x14, 0x00, 0x00, 0x00, 0x05},
-	{0x07, 0x0000, 0x07, 0x03, 0x05, 0x03, 0x00, 0x01, 0x03},
-	{0x07, 0x0000, 0x19, 0x02, 0x15, 0x02, 0x00, 0x01, 0x03},
-	{0x0d, 0x920a, 0x0d, 0x00, 0x10, 0x00, 0x00, 0x00, 0x04},
-	{0x0e, 0x820a, 0x0e, 0x00, 0x14, 0x00, 0x00, 0x00, 0x05},
-	{0x0f, 0x0202, 0x11, 0x01, 0x04, 0x01, 0x00, 0x00, 0x05},
-	{0x10, 0x0212, 0x12, 0x01, 0x04, 0x01, 0x00, 0x00, 0x05},
-	{0x11, 0x0212, 0x1a, 0x04, 0x24, 0x04, 0x00, 0x00, 0x05},
-	{0x12, 0x0212, 0x1b, 0x04, 0x24, 0x04, 0x00, 0x00, 0x05},
-	{0x13, 0x021b, 0x1c, 0x00, 0x14, 0x00, 0x00, 0x00, 0x04},
-	{0x12, 0x0010, 0x18, 0x02, 0x24, 0x02, 0x00, 0x00, 0x05},/* St_CRT2CRTC2
-								    not sure */
-	{0x12, 0x0210, 0x18, 0x01, 0x24, 0x01, 0x00, 0x00, 0x05},/* St_CRT2CRTC2
-								    not sure */
-	{0xff, 0x0000, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00}
-};
-
-
 static struct XGI_ExtStruct XGI330_EModeIDTable[] = {
-	{0x6a, 0x2212, 0x0407, 0x3a81, 0x0102, 0x08,
-		0x07, 0x00, 0x00, 0x07, 0x0e},
-	{0x2e, 0x0a1b, 0x0306, 0x3a57, 0x0101, 0x08,
-		0x06, 0x00, 0x00, 0x05, 0x06},
-	{0x2f, 0x0a1b, 0x0305, 0x3a50, 0x0100, 0x08,
-		0x05, 0x00, 0x00, 0x05, 0x05},
-	{0x30, 0x2a1b, 0x0407, 0x3a81, 0x0103, 0x08,
-		0x07, 0x00, 0x00, 0x07, 0x0e},
-	{0x31, 0x0a1b, 0x030d, 0x3b85, 0x0000, 0x08,
-		0x0d, 0x00, 0x00, 0x06, 0x3d},
-	{0x32, 0x0a1b, 0x0a0e, 0x3b8c, 0x0000, 0x08,
-		0x0e, 0x00, 0x00, 0x06, 0x3e},
-	{0x33, 0x0a1d, 0x0a0d, 0x3b85, 0x0000, 0x08,
-		0x0d, 0x00, 0x00, 0x06, 0x3d},
-	{0x34, 0x2a1d, 0x0a0e, 0x3b8c, 0x0000, 0x08,
-		0x0e, 0x00, 0x00, 0x06, 0x3e},
-	{0x35, 0x0a1f, 0x0a0d, 0x3b85, 0x0000, 0x08,
-		0x0d, 0x00, 0x00, 0x06, 0x3d},
-	{0x36, 0x2a1f, 0x0a0e, 0x3b8c, 0x0000, 0x08,
-		0x0e, 0x00, 0x00, 0x06, 0x3e},
-	{0x37, 0x0212, 0x0508, 0x3aab, 0x0104, 0x08,
-		0x08, 0x00, 0x00, 0x00, 0x16},
-	{0x38, 0x0a1b, 0x0508, 0x3aab, 0x0105, 0x08,
-		0x08, 0x00, 0x00, 0x00, 0x16},
-	{0x3a, 0x0e3b, 0x0609, 0x3adc, 0x0107, 0x08,
-		0x09, 0x00, 0x00, 0x00, 0x1e},
-	{0x3c, 0x0e3b, 0x070a, 0x3af2, 0x0130, 0x08,
-		0x0a, 0x00, 0x00, 0x00, 0x22},	/* mode 1600x1200
+	{0x2e, 0x0a1b, 0x0306, 0x06, 0x05, 0x06},
+	{0x2f, 0x0a1b, 0x0305, 0x05, 0x05, 0x05},
+	{0x30, 0x2a1b, 0x0407, 0x07, 0x07, 0x0e},
+	{0x31, 0x0a1b, 0x030d, 0x0d, 0x06, 0x3d},
+	{0x32, 0x0a1b, 0x0a0e, 0x0e, 0x06, 0x3e},
+	{0x33, 0x0a1d, 0x0a0d, 0x0d, 0x06, 0x3d},
+	{0x34, 0x2a1d, 0x0a0e, 0x0e, 0x06, 0x3e},
+	{0x35, 0x0a1f, 0x0a0d, 0x0d, 0x06, 0x3d},
+	{0x36, 0x2a1f, 0x0a0e, 0x0e, 0x06, 0x3e},
+	{0x38, 0x0a1b, 0x0508, 0x08, 0x00, 0x16},
+	{0x3a, 0x0e3b, 0x0609, 0x09, 0x00, 0x1e},
+	{0x3c, 0x0e3b, 0x070a, 0x0a, 0x00, 0x22}, /* mode 1600x1200
 						   add CRT2MODE [2003/10/07] */
-	{0x3d, 0x0e7d, 0x070a, 0x3af2, 0x0131, 0x08,
-		0x0a, 0x00, 0x00, 0x00, 0x22},	/* mode 1600x1200
+	{0x3d, 0x0e7d, 0x070a, 0x0a, 0x00, 0x22}, /* mode 1600x1200
 						   add CRT2MODE */
-	{0x40, 0x9a1c, 0x0000, 0x3a34, 0x010d, 0x08,
-		0x00, 0x00, 0x00, 0x04, 0x00},
-	{0x41, 0x9a1d, 0x0000, 0x3a34, 0x010e, 0x08,
-		0x00, 0x00, 0x00, 0x04, 0x00},	/* ModeIdIndex = 0x10 */
-	{0x43, 0x0a1c, 0x0306, 0x3a57, 0x0110, 0x08,
-		0x06, 0x00, 0x00, 0x05, 0x06},
-	{0x44, 0x0a1d, 0x0306, 0x3a57, 0x0111, 0x08,
-		0x06, 0x00, 0x00, 0x05, 0x06},
-	{0x46, 0x2a1c, 0x0407, 0x3a81, 0x0113, 0x08,
-		0x07, 0x00, 0x00, 0x07, 0x0e},
-	{0x47, 0x2a1d, 0x0407, 0x3a81, 0x0114, 0x08,
-		0x07, 0x00, 0x00, 0x07, 0x0e},
-	{0x49, 0x0a3c, 0x0508, 0x3aab, 0x0116, 0x08,
-		0x08, 0x00, 0x00, 0x00, 0x16},
-	{0x4a, 0x0a3d, 0x0508, 0x3aab, 0x0117, 0x08,
-		0x08, 0x00, 0x00, 0x00, 0x16},
-	{0x4c, 0x0e7c, 0x0609, 0x3adc, 0x0119, 0x08,
-		0x09, 0x00, 0x00, 0x00, 0x1e},
-	{0x4d, 0x0e7d, 0x0609, 0x3adc, 0x011a, 0x08,
-		0x09, 0x00, 0x00, 0x00, 0x1e},
-	{0x50, 0x9a1b, 0x0001, 0x3a3b, 0x0132, 0x08,
-		0x01, 0x00, 0x00, 0x04, 0x02},
-	{0x51, 0xba1b, 0x0103, 0x3a42, 0x0133, 0x08,
-		0x03, 0x00, 0x00, 0x07, 0x03},
-	{0x52, 0x9a1b, 0x0204, 0x3a49, 0x0134, 0x08,
-		0x04, 0x00, 0x00, 0x00, 0x04},
-	{0x56, 0x9a1d, 0x0001, 0x3a3b, 0x0135, 0x08,
-		0x01, 0x00, 0x00, 0x04, 0x02},
-	{0x57, 0xba1d, 0x0103, 0x3a42, 0x0136, 0x08,
-		0x03, 0x00, 0x00, 0x07, 0x03},
-	{0x58, 0x9a1d, 0x0204, 0x3a49, 0x0137, 0x08,
-		0x04, 0x00, 0x00, 0x00, 0x04},
-	{0x59, 0x9a1b, 0x0000, 0x3a34, 0x0138, 0x08,
-		0x00, 0x00, 0x00, 0x04, 0x00},
-	{0x5A, 0x021b, 0x0014, 0x3b83, 0x0138, 0x08,
-		0x01, 0x00, 0x00, 0x04, 0x3f},	/* ModeIdIndex = 0x20 */
-	{0x5B, 0x0a1d, 0x0014, 0x3b83, 0x0135, 0x08,
-		0x01, 0x00, 0x00, 0x04, 0x3f},
-	{0x5d, 0x0a1d, 0x0305, 0x3a50, 0x0139, 0x08,
-		0x05, 0x00, 0x00, 0x07, 0x05},
-	{0x62, 0x0a3f, 0x0306, 0x3a57, 0x013a, 0x08,
-		0x06, 0x00, 0x00, 0x05, 0x06},
-	{0x63, 0x2a3f, 0x0407, 0x3a81, 0x013b, 0x08,
-		0x07, 0x00, 0x00, 0x07, 0x0e},
-	{0x64, 0x0a7f, 0x0508, 0x3aab, 0x013c, 0x08,
-		0x08, 0x00, 0x00, 0x00, 0x16},
-	{0x65, 0x0eff, 0x0609, 0x3adc, 0x013d, 0x08,
-		0x09, 0x00, 0x00, 0x00, 0x1e},
-	{0x66, 0x0eff, 0x070a, 0x3af2, 0x013e, 0x08,
-		0x0a, 0x00, 0x00, 0x00, 0x22},	/* mode 1600x1200
+	{0x40, 0x9a1c, 0x0000, 0x00, 0x04, 0x00},
+	{0x41, 0x9a1d, 0x0000, 0x00, 0x04, 0x00},
+	{0x43, 0x0a1c, 0x0306, 0x06, 0x05, 0x06},
+	{0x44, 0x0a1d, 0x0306, 0x06, 0x05, 0x06},
+	{0x46, 0x2a1c, 0x0407, 0x07, 0x07, 0x0e},
+	{0x47, 0x2a1d, 0x0407, 0x07, 0x07, 0x0e},
+	{0x49, 0x0a3c, 0x0508, 0x08, 0x00, 0x16},
+	{0x4a, 0x0a3d, 0x0508, 0x08, 0x00, 0x16},
+	{0x4c, 0x0e7c, 0x0609, 0x09, 0x00, 0x1e},
+	{0x4d, 0x0e7d, 0x0609, 0x09, 0x00, 0x1e},
+	{0x50, 0x9a1b, 0x0001, 0x01, 0x04, 0x02},
+	{0x51, 0xba1b, 0x0103, 0x03, 0x07, 0x03},
+	{0x52, 0x9a1b, 0x0204, 0x04, 0x00, 0x04},
+	{0x56, 0x9a1d, 0x0001, 0x01, 0x04, 0x02},
+	{0x57, 0xba1d, 0x0103, 0x03, 0x07, 0x03},
+	{0x58, 0x9a1d, 0x0204, 0x04, 0x00, 0x04},
+	{0x59, 0x9a1b, 0x0000, 0x00, 0x04, 0x00},
+	{0x5A, 0x021b, 0x0014, 0x01, 0x04, 0x3f},
+	{0x5B, 0x0a1d, 0x0014, 0x01, 0x04, 0x3f},
+	{0x5d, 0x0a1d, 0x0305, 0x05, 0x07, 0x05},
+	{0x62, 0x0a3f, 0x0306, 0x06, 0x05, 0x06},
+	{0x63, 0x2a3f, 0x0407, 0x07, 0x07, 0x0e},
+	{0x64, 0x0a7f, 0x0508, 0x08, 0x00, 0x16},
+	{0x65, 0x0eff, 0x0609, 0x09, 0x00, 0x1e},
+	{0x66, 0x0eff, 0x070a, 0x0a, 0x00, 0x22}, /* mode 1600x1200
 						   add CRT2MODE */
-	{0x68, 0x067b, 0x080b, 0x3b17, 0x013f, 0x08,
-		0x0b, 0x00, 0x00, 0x00, 0x29},
-	{0x69, 0x06fd, 0x080b, 0x3b17, 0x0140, 0x08,
-		0x0b, 0x00, 0x00, 0x00, 0x29},
-	{0x6b, 0x07ff, 0x080b, 0x3b17, 0x0141, 0x10,
-		0x0b, 0x00, 0x00, 0x00, 0x29},
-	{0x6c, 0x067b, 0x090c, 0x3b37, 0x0000, 0x08,
-		0x0c, 0x00, 0x00, 0x00, 0x2f},
-	{0x6d, 0x06fd, 0x090c, 0x3b37, 0x0000, 0x10,
-		0x0c, 0x00, 0x00, 0x00, 0x2f},
-	{0x6e, 0x07ff, 0x090c, 0x3b37, 0x0000, 0x10,
-		0x0c, 0x00, 0x00, 0x00, 0x2f},
-	{0x70, 0x2a1b, 0x0410, 0x3b52, 0x0000, 0x08,
-		0x10, 0x00, 0x00, 0x07, 0x34},
-	{0x71, 0x0a1b, 0x0511, 0x3b63, 0x0000, 0x08,
-		0x11, 0x00, 0x00, 0x00, 0x37},
-	{0x74, 0x0a1d, 0x0511, 0x3b63, 0x0000, 0x08,
-		0x11, 0x00, 0x00, 0x00, 0x37},	/* ModeIdIndex = 0x30 */
-	{0x75, 0x0a3d, 0x0612, 0x3b74, 0x0000, 0x08,
-		0x12, 0x00, 0x00, 0x00, 0x3a},
-	{0x76, 0x2a1f, 0x0410, 0x3b52, 0x0000, 0x08,
-		0x10, 0x00, 0x00, 0x07, 0x34},
-	{0x77, 0x0a1f, 0x0511, 0x3b63, 0x0000, 0x08,
-		0x11, 0x00, 0x00, 0x00, 0x37},
-	{0x78, 0x0a3f, 0x0612, 0x3b74, 0x0000, 0x08,
-		0x12, 0x00, 0x00, 0x00, 0x3a},
-	{0x79, 0x0a3b, 0x0612, 0x3b74, 0x0000, 0x08,
-		0x12, 0x00, 0x00, 0x00, 0x3a},
-	{0x7a, 0x2a1d, 0x0410, 0x3b52, 0x0000, 0x08,
-		0x10, 0x00, 0x00, 0x07, 0x34},
-	{0x7b, 0x0e3b, 0x060f, 0x3ad0, 0x0000, 0x08,
-		0x0f, 0x00, 0x00, 0x00, 0x1d},
-	{0x7c, 0x0e7d, 0x060f, 0x3ad0, 0x0000, 0x08,
-		0x0f, 0x00, 0x00, 0x00, 0x1d},
-	{0x7d, 0x0eff, 0x060f, 0x3ad0, 0x0000, 0x08,
-		0x0f, 0x00, 0x00, 0x00, 0x1d},
-	{0x20, 0x0e3b, 0x0D16, 0x49e0, 0x0000, 0x08,
-		0x16, 0x00, 0x00, 0x00, 0x43},
-	{0x21, 0x0e7d, 0x0D16, 0x49e0, 0x0000, 0x08,
-		0x16, 0x00, 0x00, 0x00, 0x43},
-	{0x22, 0x0eff, 0x0D16, 0x49e0, 0x0000, 0x08,
-		0x16, 0x00, 0x00, 0x00, 0x43},
-	{0x23, 0x0e3b, 0x0614, 0x49d5, 0x0000, 0x08,
-		0x14, 0x00, 0x00, 0x00, 0x41},
-	{0x24, 0x0e7d, 0x0614, 0x49d5, 0x0000, 0x08,
-		0x14, 0x00, 0x00, 0x00, 0x41},
-	{0x25, 0x0eff, 0x0614, 0x49d5, 0x0000, 0x08,
-		0x14, 0x00, 0x00, 0x00, 0x41},
-	{0x26, 0x063b, 0x0c15, 0x49dc, 0x0000, 0x08,
-		0x15, 0x00, 0x00, 0x00, 0x42},	/* ModeIdIndex = 0x40 */
-	{0x27, 0x067d, 0x0c15, 0x49dc, 0x0000, 0x08,
-		0x15, 0x00, 0x00, 0x00, 0x42},
-	{0x28, 0x06ff, 0x0c15, 0x49dc, 0x0000, 0x08,
-		0x15, 0x00, 0x00, 0x00, 0x42},
-	{0xff, 0x0000, 0x0000, 0x0000, 0x0000, 0x00,
-		0x00, 0x00, 0x00, 0x00, 0x00}
-};
-
-static struct SiS_StandTable_S XGI330_StandTable[] = {
-/* MD_0_200 */
-	{
-		0x28, 0x18, 0x08, 0x0800,
-		{0x09, 0x03, 0x00, 0x02},
-		0x63,
-		{0x2d, 0x27, 0x28, 0x90, 0x2b, 0xa0, 0xbf, 0x1f,
-		 0x00, 0xc7, 0x06, 0x07, 0x00, 0x00, 0x00, 0x00,
-		 0x9c, 0x8e, 0x8f, 0x14, 0x1f, 0x96, 0xb9, 0xa3,
-		 0xff},
-		{0x00, 0x01, 0x02, 0x03, 0x04, 0x05, 0x06, 0x07,
-		 0x10, 0x11, 0x12, 0x13, 0x14, 0x15, 0x16, 0x17,
-		 0x08, 0x00, 0x0f, 0x00},
-		{0x00, 0x00, 0x00, 0x00, 0x00, 0x10, 0x0e, 0x00,
-		 0xff}
-	},
-/* MD_1_200 */
-	{
-		0x28, 0x18, 0x08, 0x0800,
-		{0x09, 0x03, 0x00, 0x02},
-		0x63,
-		{0x2d, 0x27, 0x28, 0x90, 0x2b, 0xa0, 0xbf, 0x1f,
-		 0x00, 0xc7, 0x06, 0x07, 0x00, 0x00, 0x00, 0x00,
-		 0x9c, 0x8e, 0x8f, 0x14, 0x1f, 0x96, 0xb9, 0xa3,
-		 0xff},
-		{0x00, 0x01, 0x02, 0x03, 0x04, 0x05, 0x06, 0x07,
-		 0x10, 0x11, 0x12, 0x13, 0x14, 0x15, 0x16, 0x17,
-		 0x08, 0x00, 0x0f, 0x00},
-		{0x00, 0x00, 0x00, 0x00, 0x00, 0x10, 0x0e, 0x00,
-		 0xff}
-	},
-/* MD_2_200 */
-	{
-		0x50, 0x18, 0x08, 0x1000,
-		{0x01, 0x03, 0x00, 0x02},
-		 0x63,
-		{0x5f, 0x4f, 0x50, 0x82, 0x55, 0x81, 0xbf, 0x1f,
-		 0x00, 0xc7, 0x06, 0x07, 0x00, 0x00, 0x00, 0x00,
-		 0x9c, 0x8e, 0x8f, 0x28, 0x1f, 0x96, 0xb9, 0xa3,
-		 0xff},
-		{0x00, 0x01, 0x02, 0x03, 0x04, 0x05, 0x06, 0x07,
-		 0x10, 0x11, 0x12, 0x13, 0x14, 0x15, 0x16, 0x17,
-		 0x08, 0x00, 0x0f, 0x00},
-		{0x00, 0x00, 0x00, 0x00, 0x00, 0x10, 0x0e, 0x00,
-		 0xff}
-	},
-/* MD_3_200 */
-	{
-		0x50, 0x18, 0x08, 0x1000,
-		{0x01, 0x03, 0x00, 0x02},
-		 0x63,
-		{0x5f, 0x4f, 0x50, 0x82, 0x55, 0x81, 0xbf, 0x1f,
-		 0x00, 0xc7, 0x06, 0x07, 0x00, 0x00, 0x00, 0x00,
-		 0x9c, 0x8e, 0x8f, 0x28, 0x1f, 0x96, 0xb9, 0xa3,
-		 0xff},
-		{0x00, 0x01, 0x02, 0x03, 0x04, 0x05, 0x06, 0x07,
-		 0x10, 0x11, 0x12, 0x13, 0x14, 0x15, 0x16, 0x17,
-		 0x08, 0x00, 0x0f, 0x00},
-		{0x00, 0x00, 0x00, 0x00, 0x00, 0x10, 0x0e, 0x00,
-		 0xff}
-	},
-/* MD_4 */
-	{
-		0x28, 0x18, 0x08, 0x4000,
-		{0x09, 0x03, 0x00, 0x02},
-		 0x63,
-		{0x2d, 0x27, 0x28, 0x90, 0x2c, 0x80, 0xbf, 0x1f,
-		 0x00, 0xc1, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
-		 0x9c, 0x8e, 0x8f, 0x14, 0x00, 0x96, 0xb9, 0xa2,
-		 0xff},
-		{0x00, 0x13, 0x15, 0x17, 0x02, 0x04, 0x06, 0x07,
-		 0x10, 0x11, 0x12, 0x13, 0x14, 0x15, 0x16, 0x17,
-		 0x01, 0x00, 0x03, 0x00},
-		{0x00, 0x00, 0x00, 0x00, 0x00, 0x30, 0x0f, 0x00,
-		 0xff}
-	},
-/* MD_5 */
-	{
-		0x28, 0x18, 0x08, 0x4000,
-		{0x09, 0x03, 0x00, 0x02},
-		 0x63,
-		{0x2d, 0x27, 0x28, 0x90, 0x2c, 0x80, 0xbf, 0x1f,
-		 0x00, 0xc1, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
-		 0x9c, 0x8e, 0x8f, 0x14, 0x00, 0x96, 0xb9, 0xa2,
-		 0xff},
-		{0x00, 0x13, 0x15, 0x17, 0x02, 0x04, 0x06, 0x07,
-		 0x10, 0x11, 0x12, 0x13, 0x14, 0x15, 0x16, 0x17,
-		 0x01, 0x00, 0x03, 0x00},
-		{0x00, 0x00, 0x00, 0x00, 0x00, 0x30, 0x0f, 0x00,
-		 0xff}
-	},
-/* MD_6 */
-	{
-		0x50, 0x18, 0x08, 0x4000,
-		{0x01, 0x01, 0x00, 0x06},
-		 0x63,
-		{0x5f, 0x4f, 0x50, 0x82, 0x55, 0x81, 0xbf, 0x1f,
-		 0x00, 0xc1, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
-		 0x9c, 0x8e, 0x8f, 0x28, 0x00, 0x96, 0xb9, 0xc2,
-		 0xff},
-		{0x00, 0x17, 0x17, 0x17, 0x17, 0x17, 0x17, 0x17,
-		 0x17, 0x17, 0x17, 0x17, 0x17, 0x17, 0x17, 0x17,
-		 0x01, 0x00, 0x01, 0x00},
-		{0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x0d, 0x00,
-		 0xff}
-	},
-/* MD_7 */
-	{
-		0x50, 0x18, 0x0e, 0x1000,
-		{0x00, 0x03, 0x00, 0x03},
-		 0xa6,
-		{0x5f, 0x4f, 0x50, 0x82, 0x55, 0x81, 0xbf, 0x1f,
-		 0x00, 0x4d, 0x0b, 0x0c, 0x00, 0x00, 0x00, 0x00,
-		 0x83, 0x85, 0x5d, 0x28, 0x0d, 0x63, 0xba, 0xa3,
-		 0xff},
-		{0x00, 0x08, 0x08, 0x08, 0x08, 0x08, 0x08, 0x08,
-		 0x10, 0x18, 0x18, 0x18, 0x18, 0x18, 0x18, 0x18,
-		 0x0e, 0x00, 0x0f, 0x08},
-		{0x00, 0x00, 0x00, 0x00, 0x00, 0x10, 0x0a, 0x00,
-		 0xff}
-	},
-/* MDA_DAC */
-	{
-		0x00, 0x00, 0x00, 0x0000,
-		{0x00, 0x00, 0x00, 0x15},
-		 0x15,
-		{0x15, 0x15, 0x15, 0x15, 0x15, 0x15, 0x15, 0x15,
-		 0x15, 0x15, 0x15, 0x15, 0x15, 0x15, 0x3f, 0x3f,
-		 0x3f, 0x3f, 0x3f, 0x3f, 0x3f, 0x3f, 0x00, 0x00,
-		 0x00},
-		{0x00, 0x00, 0x00, 0x00, 0x00, 0x15, 0x15, 0x15,
-		 0x15, 0x15, 0x15, 0x15, 0x15, 0x15, 0x15, 0x15,
-		 0x15, 0x15, 0x15, 0x15},
-		{0x15, 0x3f, 0x3f, 0x3f, 0x3f, 0x3f, 0x3f, 0x3f,
-		 0x3f}
-	},
-/* CGA_DAC */
-	{
-		0x00, 0x10, 0x04, 0x0114,
-		{0x11, 0x09, 0x15, 0x00},
-		 0x10,
-		{0x04, 0x14, 0x01, 0x11, 0x09, 0x15, 0x2a, 0x3a,
-		 0x2e, 0x3e, 0x2b, 0x3b, 0x2f, 0x3f, 0x2a, 0x3a,
-		 0x2e, 0x3e, 0x2b, 0x3b, 0x2f, 0x3f, 0x00, 0x10,
-		 0x04},
-		{0x14, 0x01, 0x11, 0x09, 0x15, 0x00, 0x10, 0x04,
-		 0x14, 0x01, 0x11, 0x09, 0x15, 0x2a, 0x3a, 0x2e,
-		 0x3e, 0x2b, 0x3b, 0x2f},
-		{0x3f, 0x2a, 0x3a, 0x2e, 0x3e, 0x2b, 0x3b, 0x2f,
-		 0x3f}
-	},
-/* EGA_DAC */
-	{
-		0x00, 0x10, 0x04, 0x0114,
-		{0x11, 0x05, 0x15, 0x20},
-		 0x30,
-		{0x24, 0x34, 0x21, 0x31, 0x25, 0x35, 0x08, 0x18,
-		 0x0c, 0x1c, 0x09, 0x19, 0x0d, 0x1d, 0x28, 0x38,
-		 0x2c, 0x3c, 0x29, 0x39, 0x2d, 0x3d, 0x02, 0x12,
-		 0x06},
-		{0x16, 0x03, 0x13, 0x07, 0x17, 0x22, 0x32, 0x26,
-		 0x36, 0x23, 0x33, 0x27, 0x37, 0x0a, 0x1a, 0x0e,
-		 0x1e, 0x0b, 0x1b, 0x0f},
-		{0x1f, 0x2a, 0x3a, 0x2e, 0x3e, 0x2b, 0x3b, 0x2f,
-		 0x3f}
-	},
-/* VGA_DAC */
-	{
-		0x00, 0x10, 0x04, 0x0114,
-		{0x11, 0x09, 0x15, 0x2a},
-		 0x3a,
-		{0x2e, 0x3e, 0x2b, 0x3b, 0x2f, 0x3f, 0x00, 0x05,
-		 0x08, 0x0b, 0x0e, 0x11, 0x14, 0x18, 0x1c, 0x20,
-		 0x24, 0x28, 0x2d, 0x32, 0x38, 0x3f, 0x00, 0x10,
-		 0x1f},
-		{0x2f, 0x3f, 0x1f, 0x27, 0x2f, 0x37, 0x3f, 0x2d,
-		 0x31, 0x36, 0x3a, 0x3f, 0x00, 0x07, 0x0e, 0x15,
-		 0x1c, 0x0e, 0x11, 0x15},
-		{0x18, 0x1c, 0x14, 0x16, 0x18, 0x1a, 0x1c, 0x00,
-		 0x04}
-	},
-	{
-		0x08, 0x0c, 0x10, 0x0a08,
-		{0x0c, 0x0e, 0x10, 0x0b},
-		 0x0c,
-		{0x0d, 0x0f, 0x10, 0x10, 0x01, 0x08, 0x00, 0x00,
-		 0x00, 0x00, 0x01, 0x00, 0x02, 0x02, 0x01, 0x00,
-		 0x04, 0x04, 0x01, 0x00, 0x05, 0x02, 0x05, 0x00,
-		 0x06},
-		{0x01, 0x06, 0x05, 0x06, 0x00, 0x08, 0x01, 0x08,
-		 0x00, 0x07, 0x02, 0x07, 0x06, 0x07, 0x00, 0x00,
-		 0x00, 0x00, 0x00, 0x00},
-		{0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
-		 0x00}
-	},
-/* MD_D */
-	{
-		0x28, 0x18, 0x08, 0x2000,
-		{0x09, 0x0f, 0x00, 0x06},
-		 0x63,
-		{0x2d, 0x27, 0x28, 0x90, 0x2c, 0x80, 0xbf, 0x1f,
-		 0x00, 0xc0, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
-		 0x9c, 0x8e, 0x8f, 0x14, 0x00, 0x96, 0xb9, 0xe3,
-		 0xff},
-		{0x00, 0x01, 0x02, 0x03, 0x04, 0x05, 0x06, 0x07,
-		 0x10, 0x11, 0x12, 0x13, 0x14, 0x15, 0x16, 0x17,
-		 0x01, 0x00, 0x0f, 0x00},
-		{0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x05, 0x0f,
-		 0xff}
-	},
-/* MD_E */
-	{
-		0x50, 0x18, 0x08, 0x4000,
-		{0x01, 0x0f, 0x00, 0x06},
-		 0x63,
-		{0x5f, 0x4f, 0x50, 0x82, 0x55, 0x81, 0xbf, 0x1f,
-		 0x00, 0xc0, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
-		 0x9c, 0x8e, 0x8f, 0x28, 0x00, 0x96, 0xb9, 0xe3,
-		 0xff},
-		{0x00, 0x01, 0x02, 0x03, 0x04, 0x05, 0x06, 0x07,
-		 0x10, 0x11, 0x12, 0x13, 0x14, 0x15, 0x16, 0x17,
-		 0x01, 0x00, 0x0f, 0x00},
-		{0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x05, 0x0f,
-		 0xff}
-	},
+	{0x68, 0x067b, 0x080b, 0x0b, 0x00, 0x29},
+	{0x69, 0x06fd, 0x080b, 0x0b, 0x00, 0x29},
+	{0x6b, 0x07ff, 0x080b, 0x0b, 0x00, 0x29},
+	{0x6c, 0x067b, 0x090c, 0x0c, 0x00, 0x2f},
+	{0x6d, 0x06fd, 0x090c, 0x0c, 0x00, 0x2f},
+	{0x6e, 0x07ff, 0x090c, 0x0c, 0x00, 0x2f},
+	{0x70, 0x2a1b, 0x0410, 0x10, 0x07, 0x34},
+	{0x71, 0x0a1b, 0x0511, 0x11, 0x00, 0x37},
+	{0x74, 0x0a1d, 0x0511, 0x11, 0x00, 0x37},
+	{0x75, 0x0a3d, 0x0612, 0x12, 0x00, 0x3a},
+	{0x76, 0x2a1f, 0x0410, 0x10, 0x07, 0x34},
+	{0x77, 0x0a1f, 0x0511, 0x11, 0x00, 0x37},
+	{0x78, 0x0a3f, 0x0612, 0x12, 0x00, 0x3a},
+	{0x79, 0x0a3b, 0x0612, 0x12, 0x00, 0x3a},
+	{0x7a, 0x2a1d, 0x0410, 0x10, 0x07, 0x34},
+	{0x7b, 0x0e3b, 0x060f, 0x0f, 0x00, 0x1d},
+	{0x7c, 0x0e7d, 0x060f, 0x0f, 0x00, 0x1d},
+	{0x7d, 0x0eff, 0x060f, 0x0f, 0x00, 0x1d},
+	{0x20, 0x0e3b, 0x0D16, 0x16, 0x00, 0x43},
+	{0x21, 0x0e7d, 0x0D16, 0x16, 0x00, 0x43},
+	{0x22, 0x0eff, 0x0D16, 0x16, 0x00, 0x43},
+	{0x23, 0x0e3b, 0x0614, 0x14, 0x00, 0x41},
+	{0x24, 0x0e7d, 0x0614, 0x14, 0x00, 0x41},
+	{0x25, 0x0eff, 0x0614, 0x14, 0x00, 0x41},
+	{0x26, 0x063b, 0x0c15, 0x15, 0x00, 0x42},
+	{0x27, 0x067d, 0x0c15, 0x15, 0x00, 0x42},
+	{0x28, 0x06ff, 0x0c15, 0x15, 0x00, 0x42},
+	{0xff, 0x0000, 0x0000, 0x00, 0x00, 0x00}
+};
+
+static struct SiS_StandTable_S XGI330_StandTable = {
 /* ExtVGATable */
-	{
-		0x00, 0x00, 0x00, 0x0000,
-		{0x01, 0x0f, 0x00, 0x0e},
-		 0x23,
-		{0x5f, 0x4f, 0x50, 0x82, 0x54, 0x80, 0x0b, 0x3e,
-		 0x00, 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
-		 0xea, 0x8c, 0xdf, 0x28, 0x40, 0xe7, 0x04, 0xa3,
-		 0xff},
-		{0x00, 0x01, 0x02, 0x03, 0x04, 0x05, 0x06, 0x07,
-		 0x08, 0x09, 0x0a, 0x0b, 0x0c, 0x0d, 0x0e, 0x0f,
-		 0x01, 0x00, 0x00, 0x00},
-		{0x00, 0x00, 0x00, 0x00, 0x00, 0x40, 0x05, 0x0f,
-		 0xff}
-	},
-/* ROM_SAVEPTR */
-	{
-		0x9f, 0x3b, 0x00, 0x00c0,
-		{0x00, 0x00, 0x00, 0x00},
-		 0x00,
-		{0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0xbb, 0x3f,
-		 0x00, 0xc0, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
-		 0x00, 0x00, 0x1a, 0x00, 0xac, 0x3e, 0x00, 0xc0,
-		 0x00},
-		{0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
-		 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
-		 0x00, 0x00, 0x00, 0x00},
-		{0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
-		 0x00}
-	},
-/* MD_F */
-	{
-		0x50, 0x18, 0x0e, 0x8000,
-		{0x01, 0x0f, 0x00, 0x06},
-		 0xa2,
-		{0x5f, 0x4f, 0x50, 0x82, 0x55, 0x81, 0xbf, 0x1f,
-		 0x00, 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
-		 0x82, 0x84, 0x5d, 0x28, 0x0f, 0x63, 0xba, 0xe3,
-		 0xff},
-		{0x00, 0x08, 0x00, 0x00, 0x18, 0x18, 0x00, 0x00,
-		 0x00, 0x08, 0x00, 0x00, 0x00, 0x18, 0x00, 0x00,
-		 0x0b, 0x00, 0x05, 0x00},
-		{0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x05, 0x05,
-		 0xff}
-	},
-/* MD_10 */
-	{
-		0x50, 0x18, 0x0e, 0x8000,
-		{0x01, 0x0f, 0x00, 0x06},
-		 0xa3,
-		{0x5f, 0x4f, 0x50, 0x82, 0x55, 0x81, 0xbf, 0x1f,
-		 0x00, 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
-		 0x82, 0x84, 0x5d, 0x28, 0x0f, 0x63, 0xba, 0xe3,
-		 0xff},
-		{0x00, 0x01, 0x02, 0x03, 0x04, 0x05, 0x14, 0x07,
-		 0x38, 0x39, 0x3a, 0x3b, 0x3c, 0x3d, 0x3e, 0x3f,
-		 0x01, 0x00, 0x0f, 0x00},
-		{0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x05, 0x0f,
-		 0xff}
-	},
-/* MD_0_350 */
-	{
-		0x28, 0x18, 0x0e, 0x0800,
-		{0x09, 0x03, 0x00, 0x02},
-		 0xa3,
-		{0x2d, 0x27, 0x28, 0x90, 0x2b, 0xb1, 0xbf, 0x1f,
-		 0x00, 0x4d, 0x0b, 0x0c, 0x00, 0x00, 0x00, 0x00,
-		 0x83, 0x85, 0x5d, 0x14, 0x1f, 0x63, 0xba, 0xa3,
-		 0xff},
-		{0x00, 0x01, 0x02, 0x03, 0x04, 0x05, 0x14, 0x07,
-		 0x38, 0x39, 0x3a, 0x3b, 0x3c, 0x3d, 0x3e, 0x3f,
-		 0x08, 0x00, 0x0f, 0x00},
-		{0x00, 0x00, 0x00, 0x00, 0x00, 0x10, 0x0e, 0x00,
-		 0xff}
-	},
-/* MD_1_350 */
-	{
-		0x28, 0x18, 0x0e, 0x0800,
-		{0x09, 0x03, 0x00, 0x02},
-		 0xa3,
-		{0x2d, 0x27, 0x28, 0x90, 0x2b, 0xa0, 0xbf, 0x1f,
-		 0x00, 0x4d, 0x0b, 0x0c, 0x00, 0x00, 0x00, 0x00,
-		 0x83, 0x85, 0x5d, 0x14, 0x1f, 0x63, 0xba, 0xa3,
-		 0xff},
-		{0x00, 0x01, 0x02, 0x03, 0x04, 0x05, 0x14, 0x07,
-		 0x38, 0x39, 0x3a, 0x3b, 0x3c, 0x3d, 0x3e, 0x3f,
-		 0x08, 0x00, 0x0f, 0x00},
-		{0x00, 0x00, 0x00, 0x00, 0x00, 0x10, 0x0e, 0x00,
-		 0xff}
-	},
-/* MD_2_350 */
-	{
-		0x50, 0x18, 0x0e, 0x1000,
-		{0x01, 0x03, 0x00, 0x02},
-		 0xa3,
-		{0x5f, 0x4f, 0x50, 0x82, 0x55, 0x81, 0xbf, 0x1f,
-		 0x00, 0x4d, 0x0b, 0x0c, 0x00, 0x00, 0x00, 0x00,
-		 0x83, 0x85, 0x5d, 0x28, 0x1f, 0x63, 0xba, 0xa3,
-		 0xff},
-		{0x00, 0x01, 0x02, 0x03, 0x04, 0x05, 0x14, 0x07,
-		 0x38, 0x39, 0x3a, 0x3b, 0x3c, 0x3d, 0x3e, 0x3f,
-		 0x08, 0x00, 0x0f, 0x00},
-		{0x00, 0x00, 0x00, 0x00, 0x00, 0x10, 0x0e, 0x00,
-		 0xff}
-	},
-/* MD_3_350 */
-	{
-		0x50, 0x18, 0x0e, 0x1000,
-		{0x01, 0x03, 0x00, 0x02},
-		 0xa3,
-		{0x5f, 0x4f, 0x50, 0x82, 0x55, 0x81, 0xbf, 0x1f,
-		 0x00, 0x4d, 0x0b, 0x0c, 0x00, 0x00, 0x00, 0x00,
-		 0x83, 0x85, 0x5d, 0x28, 0x1f, 0x63, 0xba, 0xa3,
-		 0xff},
-		{0x00, 0x01, 0x02, 0x03, 0x04, 0x05, 0x14, 0x07,
-		 0x38, 0x39, 0x3a, 0x3b, 0x3c, 0x3d, 0x3e, 0x3f,
-		 0x08, 0x00, 0x0f, 0x00},
-		{0x00, 0x00, 0x00, 0x00, 0x00, 0x10, 0x0e, 0x00,
-		 0xff}
-	},
-/* MD_0_1_400 */
-	{
-		0x28, 0x18, 0x10, 0x0800,
-		{0x08, 0x03, 0x00, 0x02},
-		 0x67,
-		{0x2d, 0x27, 0x28, 0x90, 0x2b, 0xb1, 0xbf, 0x1f,
-		 0x00, 0x4f, 0x0d, 0x0e, 0x00, 0x00, 0x00, 0x00,
-		 0x9c, 0x8e, 0x8f, 0x14, 0x1f, 0x96, 0xb9, 0xa3,
-		 0xff},
-		{0x00, 0x01, 0x02, 0x03, 0x04, 0x05, 0x14, 0x07,
-		 0x38, 0x39, 0x3a, 0x3b, 0x3c, 0x3d, 0x3e, 0x3f,
-		 0x0c, 0x00, 0x0f, 0x08},
-		{0x00, 0x00, 0x00, 0x00, 0x00, 0x10, 0x0e, 0x00,
-		 0xff}
-	},
-/* MD_2_3_400 */
-	{
-		0x50, 0x18, 0x10, 0x1000,
-		{0x00, 0x03, 0x00, 0x02},
-		 0x67,
-		{0x5f, 0x4f, 0x50, 0x82, 0x55, 0x81, 0xbf, 0x1f,
-		 0x00, 0x4f, 0x0d, 0x0e, 0x00, 0x00, 0x00, 0x00,
-		 0x9c, 0x8e, 0x8f, 0x28, 0x1f, 0x96, 0xb9, 0xa3,
-		 0xff},
-		{0x00, 0x01, 0x02, 0x03, 0x04, 0x05, 0x14, 0x07,
-		 0x38, 0x39, 0x3a, 0x3b, 0x3c, 0x3d, 0x3e, 0x3f,
-		 0x0c, 0x00, 0x0f, 0x08},
-		{0x00, 0x00, 0x00, 0x00, 0x00, 0x10, 0x0e, 0x00,
-		 0xff}
-	},
-/* MD_7_400 */
-	{
-		0x50, 0x18, 0x10, 0x1000,
-		{0x00, 0x03, 0x00, 0x02},
-		 0x66,
-		{0x5f, 0x4f, 0x50, 0x82, 0x55, 0x81, 0xbf, 0x1f,
-		 0x00, 0x4f, 0x0d, 0x0e, 0x00, 0x00, 0x00, 0x00,
-		 0x9c, 0x8e, 0x8f, 0x28, 0x0f, 0x96, 0xb9, 0xa3,
-		 0xff},
-		{0x00, 0x08, 0x08, 0x08, 0x08, 0x08, 0x08, 0x08,
-		 0x10, 0x18, 0x18, 0x18, 0x18, 0x18, 0x18, 0x18,
-		 0x0e, 0x00, 0x0f, 0x08},
-		{0x00, 0x00, 0x00, 0x00, 0x00, 0x10, 0x0a, 0x00,
-		 0xff}
-	},
-/* MD_11 */
-	{
-		0x50, 0x1d, 0x10, 0xa000,
-		{0x01, 0x0f, 0x00, 0x06},
-		 0xe3,
-		{0x5f, 0x4f, 0x50, 0x82, 0x55, 0x81, 0x0b, 0x3e,
-		 0x00, 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
-		 0xe9, 0x8b, 0xdf, 0x28, 0x00, 0xe7, 0x04, 0xc3,
-		 0xff},
-		{0x00, 0x3f, 0x3f, 0x3f, 0x3f, 0x3f, 0x3f, 0x3f,
-		 0x3f, 0x3f, 0x3f, 0x3f, 0x3f, 0x3f, 0x3f, 0x3f,
-		 0x01, 0x00, 0x0f, 0x00},
-		{0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x05, 0x01,
-		 0xff}
-	},
-/* ExtEGATable */
-	{
-		0x50, 0x1d, 0x10, 0xa000,
-		{0x01, 0x0f, 0x00, 0x06},
-		 0xe3,
-		{0x5f, 0x4f, 0x50, 0x82, 0x55, 0x81, 0x0b, 0x3e,
-		 0x00, 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
-		 0xe9, 0x8b, 0xdf, 0x28, 0x00, 0xe7, 0x04, 0xe3,
-		 0xff},
-		{0x00, 0x01, 0x02, 0x03, 0x04, 0x05, 0x14, 0x07,
-		 0x38, 0x39, 0x3a, 0x3b, 0x3c, 0x3d, 0x3e, 0x3f,
-		 0x01, 0x00, 0x0f, 0x00},
-		{0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x05, 0x0f,
-		 0xff}
-	},
-/* MD_13 */
-	{
-		0x28, 0x18, 0x08, 0x2000,
-		{0x01, 0x0f, 0x00, 0x0e},
-		 0x63,
-		{0x5f, 0x4f, 0x50, 0x82, 0x55, 0x81, 0xbf, 0x1f,
-		 0x00, 0x41, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
-		 0x9c, 0x8e, 0x8f, 0x28, 0x40, 0x96, 0xb9, 0xa3,
-		 0xff},
-		{0x00, 0x01, 0x02, 0x03, 0x04, 0x05, 0x06, 0x07,
-		 0x08, 0x09, 0x0a, 0x0b, 0x0c, 0x0d, 0x0e, 0x0f,
-		 0x41, 0x00, 0x0f, 0x00},
-		{0x00, 0x00, 0x00, 0x00, 0x00, 0x40, 0x05, 0x0f,
-		 0xff}
-	}
+	0x00, 0x00, 0x00, 0x0000,
+	{0x01, 0x0f, 0x00, 0x0e},
+	 0x23,
+	{0x5f, 0x4f, 0x50, 0x82, 0x54, 0x80, 0x0b, 0x3e,
+	 0x00, 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	 0xea, 0x8c, 0xdf, 0x28, 0x40, 0xe7, 0x04, 0xa3,
+	 0xff},
+	{0x00, 0x01, 0x02, 0x03, 0x04, 0x05, 0x06, 0x07,
+	 0x08, 0x09, 0x0a, 0x0b, 0x0c, 0x0d, 0x0e, 0x0f,
+	 0x01, 0x00, 0x00, 0x00},
+	{0x00, 0x00, 0x00, 0x00, 0x00, 0x40, 0x05, 0x0f,
+	 0xff}
 };
 
 static struct XGI_TimingHStruct XGI_TimingH[1];
@@ -1143,7 +626,7 @@ static struct XGI330_LCDDataStruct  XGI_NoScalingDatax75[] = {
 	{1, 1, 1688, 806,  1688, 806}   /* ; 0A (1280x768x75Hz) */
 };
 
-static struct XGI330_LCDDataDesStruct  XGI_ExtLCDDes1024x768Data[] = {
+static struct XGI_LCDDesStruct XGI_ExtLCDDes1024x768Data[] = {
 	{9, 1057, 0,   771}, /* ; 00 (320x200,320x400,640x200,640x400) */
 	{9, 1057, 0,   771}, /* ; 01 (320x350,640x350) */
 	{9, 1057, 0,   771}, /* ; 02 (360x400,720x400) */
@@ -1153,7 +636,7 @@ static struct XGI330_LCDDataDesStruct  XGI_ExtLCDDes1024x768Data[] = {
 	{9, 1057, 805, 770}  /* ; 06 (1024x768x60Hz) */
 };
 
-static struct XGI330_LCDDataDesStruct  XGI_StLCDDes1024x768Data[] = {
+static struct XGI_LCDDesStruct XGI_StLCDDes1024x768Data[] = {
 	{9, 1057, 737, 703}, /* ; 00 (320x200,320x400,640x200,640x400) */
 	{9, 1057, 686, 651}, /* ; 01 (320x350,640x350) */
 	{9, 1057, 737, 703}, /* ; 02 (360x400,720x400) */
@@ -1163,7 +646,7 @@ static struct XGI330_LCDDataDesStruct  XGI_StLCDDes1024x768Data[] = {
 	{9, 1057, 805, 770}  /* ; 06 (1024x768x60Hz) */
 };
 
-static struct XGI330_LCDDataDesStruct  XGI_CetLCDDes1024x768Data[] = {
+static struct XGI_LCDDesStruct XGI_CetLCDDes1024x768Data[] = {
 	{1152, 856,  622, 587}, /* ; 00 (320x200,320x400,640x200,640x400) */
 	{1152, 856,  597, 562}, /* ; 01 (320x350,640x350) */
 	{1152, 856,  622, 587}, /* ; 02 (360x400,720x400) */
@@ -1173,7 +656,7 @@ static struct XGI330_LCDDataDesStruct  XGI_CetLCDDes1024x768Data[] = {
 	{0,    1048, 805, 770}  /* ; 06 (1024x768x60Hz) */
 };
 
-static struct XGI330_LCDDataDesStruct XGI_ExtLCDDLDes1280x1024Data[] = {
+static struct XGI_LCDDesStruct XGI_ExtLCDDLDes1280x1024Data[] = {
 	{18, 1346, 981,  940},  /* 00 (320x200,320x400,640x200,640x400) */
 	{18, 1346, 926,  865},  /* 01 (320x350,640x350) */
 	{18, 1346, 981,  940},  /* 02 (360x400,720x400) */
@@ -1184,7 +667,7 @@ static struct XGI330_LCDDataDesStruct XGI_ExtLCDDLDes1280x1024Data[] = {
 	{18, 1346, 1065, 1024}  /* 07 (1280x1024x60Hz) */
 };
 
-static struct XGI330_LCDDataDesStruct XGI_StLCDDLDes1280x1024Data[] = {
+static struct XGI_LCDDesStruct XGI_StLCDDLDes1280x1024Data[] = {
 	{18, 1346, 970,  907},  /* 00 (320x200,320x400,640x200,640x400) */
 	{18, 1346, 917,  854},  /* 01 (320x350,640x350) */
 	{18, 1346, 970,  907},  /* 02 (360x400,720x400) */
@@ -1195,7 +678,7 @@ static struct XGI330_LCDDataDesStruct XGI_StLCDDLDes1280x1024Data[] = {
 	{18, 1346, 1065, 1024}  /* 07 (1280x1024x60Hz) */
 };
 
-static struct XGI330_LCDDataDesStruct  XGI_CetLCDDLDes1280x1024Data[] = {
+static struct XGI_LCDDesStruct XGI_CetLCDDLDes1280x1024Data[] = {
 	{1368, 1008, 752,  711}, /* 00 (320x200,320x400,640x200,640x400) */
 	{1368, 1008, 729,  688}, /* 01 (320x350,640x350) */
 	{1368, 1008, 752,  711}, /* 02 (360x400,720x400) */
@@ -1206,7 +689,7 @@ static struct XGI330_LCDDataDesStruct  XGI_CetLCDDLDes1280x1024Data[] = {
 	{18,   1346, 1065, 1024} /* 07 (1280x1024x60Hz) */
 };
 
-static struct XGI330_LCDDataDesStruct  XGI_ExtLCDDes1280x1024Data[] = {
+static struct XGI_LCDDesStruct XGI_ExtLCDDes1280x1024Data[] = {
 	{9, 1337, 981,  940},  /* ; 00 (320x200,320x400,640x200,640x400) */
 	{9, 1337, 926,  884},  /* ; 01 (320x350,640x350) alan, 2003/09/30 */
 	{9, 1337, 981,  940},  /* ; 02 (360x400,720x400) */
@@ -1217,7 +700,7 @@ static struct XGI330_LCDDataDesStruct  XGI_ExtLCDDes1280x1024Data[] = {
 	{9, 1337, 1065, 1024}  /* ; 07 (1280x1024x60Hz) */
 };
 
-static struct XGI330_LCDDataDesStruct  XGI_StLCDDes1280x1024Data[] = {
+static struct XGI_LCDDesStruct XGI_StLCDDes1280x1024Data[] = {
 	{9, 1337, 970,  907},  /* ; 00 (320x200,320x400,640x200,640x400) */
 	{9, 1337, 917,  854},  /* ; 01 (320x350,640x350) */
 	{9, 1337, 970,  907},  /* ; 02 (360x400,720x400) */
@@ -1228,7 +711,7 @@ static struct XGI330_LCDDataDesStruct  XGI_StLCDDes1280x1024Data[] = {
 	{9, 1337, 1065, 1024}  /* ; 07 (1280x1024x60Hz) */
 };
 
-static struct XGI330_LCDDataDesStruct  XGI_CetLCDDes1280x1024Data[] = {
+static struct XGI_LCDDesStruct XGI_CetLCDDes1280x1024Data[] = {
 	{1368, 1008, 752,  711}, /* 00 (320x200,320x400,640x200,640x400) */
 	{1368, 1008, 729,  688}, /* 01 (320x350,640x350) */
 	{1368, 1008, 752,  711}, /* 02 (360x400,720x400) */
@@ -1239,7 +722,7 @@ static struct XGI330_LCDDataDesStruct  XGI_CetLCDDes1280x1024Data[] = {
 	{9,    1337, 1065, 1024} /* 07 (1280x1024x60Hz) */
 };
 
-static struct XGI330_LCDDataDesStruct xgifb_lcddldes_1400x1050[] = {
+static struct XGI_LCDDesStruct xgifb_lcddldes_1400x1050[] = {
 	{18,   1464, 0,    1051}, /* 00 (320x200,320x400,640x200,640x400) */
 	{18,   1464, 0,    1051}, /* 01 (320x350,640x350) */
 	{18,   1464, 0,    1051}, /* 02 (360x400,720x400) */
@@ -1251,7 +734,7 @@ static struct XGI330_LCDDataDesStruct xgifb_lcddldes_1400x1050[] = {
 	{18,   1464, 0,    1051}  /* 08 (1400x1050x60Hz) */
 };
 
-static struct XGI330_LCDDataDesStruct xgifb_lcddes_1400x1050[] = {
+static struct XGI_LCDDesStruct xgifb_lcddes_1400x1050[] = {
 	{9,    1455, 0,    1051}, /* 00 (320x200,320x400,640x200,640x400) */
 	{9,    1455, 0,    1051}, /* 01 (320x350,640x350) */
 	{9,    1455, 0,    1051}, /* 02 (360x400,720x400) */
@@ -1263,7 +746,7 @@ static struct XGI330_LCDDataDesStruct xgifb_lcddes_1400x1050[] = {
 	{9,    1455, 0,    1051}  /* 08 (1400x1050x60Hz) */
 };
 
-static struct XGI330_LCDDataDesStruct  XGI_CetLCDDes1400x1050Data[] = {
+static struct XGI_LCDDesStruct XGI_CetLCDDes1400x1050Data[] = {
 	{1308, 1068, 781,  766},  /* 00 (320x200,320x400,640x200,640x400) */
 	{1308, 1068, 781,  766},  /* 01 (320x350,640x350) */
 	{1308, 1068, 781,  766},  /* 02 (360x400,720x400) */
@@ -1275,7 +758,7 @@ static struct XGI330_LCDDataDesStruct  XGI_CetLCDDes1400x1050Data[] = {
 	{18,   1464, 0,    1051}  /* 08 (1400x1050x60Hz) */
 };
 
-static struct XGI330_LCDDataDesStruct  XGI_CetLCDDes1400x1050Data2[] = {
+static struct XGI_LCDDesStruct XGI_CetLCDDes1400x1050Data2[] = {
 	{0, 1448, 0, 1051}, /* 00 (320x200,320x400,640x200,640x400) */
 	{0, 1448, 0, 1051}, /* 01 (320x350,640x350) */
 	{0, 1448, 0, 1051}, /* 02 (360x400,720x400) */
@@ -1283,7 +766,7 @@ static struct XGI330_LCDDataDesStruct  XGI_CetLCDDes1400x1050Data2[] = {
 	{0, 1448, 0, 1051}  /* 04 (640x480x60Hz) */
 };
 
-static struct XGI330_LCDDataDesStruct  XGI_ExtLCDDLDes1600x1200Data[] = {
+static struct XGI_LCDDesStruct XGI_ExtLCDDLDes1600x1200Data[] = {
 	{18, 1682, 0, 1201}, /* 00 (320x200,320x400,640x200,640x400) */
 	{18, 1682, 0, 1201}, /* 01 (320x350,640x350) */
 	{18, 1682, 0, 1201}, /* 02 (360x400,720x400) */
@@ -1296,7 +779,7 @@ static struct XGI330_LCDDataDesStruct  XGI_ExtLCDDLDes1600x1200Data[] = {
 	{18, 1682, 0, 1201}  /* 09 (1600x1200x60Hz) */
 };
 
-static struct XGI330_LCDDataDesStruct  XGI_StLCDDLDes1600x1200Data[] = {
+static struct XGI_LCDDesStruct XGI_StLCDDLDes1600x1200Data[] = {
 	{18, 1682, 1150, 1101}, /* 00 (320x200,320x400,640x200,640x400) */
 	{18, 1682, 1083, 1034}, /* 01 (320x350,640x350) */
 	{18, 1682, 1150, 1101}, /* 02 (360x400,720x400) */
@@ -1309,7 +792,7 @@ static struct XGI330_LCDDataDesStruct  XGI_StLCDDLDes1600x1200Data[] = {
 	{18, 1682, 0,    1201} /* 09 (1600x1200x60Hz) */
 };
 
-static struct XGI330_LCDDataDesStruct  XGI_ExtLCDDes1600x1200Data[] = {
+static struct XGI_LCDDesStruct XGI_ExtLCDDes1600x1200Data[] = {
 	{9, 1673, 0, 1201}, /* 00 (320x200,320x400,640x200,640x400) */
 	{9, 1673, 0, 1201}, /* 01 (320x350,640x350) */
 	{9, 1673, 0, 1201}, /* 02 (360x400,720x400) */
@@ -1322,7 +805,7 @@ static struct XGI330_LCDDataDesStruct  XGI_ExtLCDDes1600x1200Data[] = {
 	{9, 1673, 0, 1201}  /* 09 (1600x1200x60Hz) */
 };
 
-static struct XGI330_LCDDataDesStruct  XGI_StLCDDes1600x1200Data[] = {
+static struct XGI_LCDDesStruct XGI_StLCDDes1600x1200Data[] = {
 	{9, 1673, 1150, 1101}, /* 00 (320x200,320x400,640x200,640x400) */
 	{9, 1673, 1083, 1034}, /* 01 (320x350,640x350) */
 	{9, 1673, 1150, 1101}, /* 02 (360x400,720x400) */
@@ -1352,7 +835,7 @@ static struct XGI330_LCDDataDesStruct2  XGI_NoScalingDesData[] = {
 };
 
 /* ;;1024x768x75Hz */
-static struct XGI330_LCDDataDesStruct xgifb_lcddes_1024x768x75[] = {
+static struct XGI_LCDDesStruct xgifb_lcddes_1024x768x75[] = {
 	{9, 1049, 0, 769}, /* ; 00 (320x200,320x400,640x200,640x400) */
 	{9, 1049, 0, 769}, /* ; 01 (320x350,640x350) */
 	{9, 1049, 0, 769}, /* ; 02 (360x400,720x400) */
@@ -1363,7 +846,7 @@ static struct XGI330_LCDDataDesStruct xgifb_lcddes_1024x768x75[] = {
 };
 
 /* ;;1024x768x75Hz */
-static struct XGI330_LCDDataDesStruct  XGI_CetLCDDes1024x768x75Data[] = {
+static struct XGI_LCDDesStruct XGI_CetLCDDes1024x768x75Data[] = {
 	{1152, 856,  622, 587}, /* ; 00 (320x200,320x400,640x200,640x400) */
 	{1152, 856,  597, 562}, /* ; 01 (320x350,640x350) */
 	{1192, 896,  622, 587}, /* ; 02 (360x400,720x400) */
@@ -1374,7 +857,7 @@ static struct XGI330_LCDDataDesStruct  XGI_CetLCDDes1024x768x75Data[] = {
 };
 
 /* ;;1280x1024x75Hz */
-static struct XGI330_LCDDataDesStruct xgifb_lcddldes_1280x1024x75[] = {
+static struct XGI_LCDDesStruct xgifb_lcddldes_1280x1024x75[] = {
 	{18, 1314, 0, 1025}, /* ; 00 (320x200,320x400,640x200,640x400) */
 	{18, 1314, 0, 1025}, /* ; 01 (320x350,640x350) */
 	{18, 1314, 0, 1025}, /* ; 02 (360x400,720x400) */
@@ -1386,7 +869,7 @@ static struct XGI330_LCDDataDesStruct xgifb_lcddldes_1280x1024x75[] = {
 };
 
 /* 1280x1024x75Hz */
-static struct XGI330_LCDDataDesStruct  XGI_CetLCDDLDes1280x1024x75Data[] = {
+static struct XGI_LCDDesStruct XGI_CetLCDDLDes1280x1024x75Data[] = {
 	{1368, 1008, 752, 711}, /* ; 00 (320x200,320x400,640x200,640x400) */
 	{1368, 1008, 729, 688}, /* ; 01 (320x350,640x350) */
 	{1408, 1048, 752, 711}, /* ; 02 (360x400,720x400) */
@@ -1398,7 +881,7 @@ static struct XGI330_LCDDataDesStruct  XGI_CetLCDDLDes1280x1024x75Data[] = {
 };
 
 /* ;;1280x1024x75Hz */
-static struct XGI330_LCDDataDesStruct xgifb_lcddes_1280x1024x75[] = {
+static struct XGI_LCDDesStruct xgifb_lcddes_1280x1024x75[] = {
 	{9, 1305, 0, 1025}, /* ; 00 (320x200,320x400,640x200,640x400) */
 	{9, 1305, 0, 1025}, /* ; 01 (320x350,640x350) */
 	{9, 1305, 0, 1025}, /* ; 02 (360x400,720x400) */
@@ -1410,7 +893,7 @@ static struct XGI330_LCDDataDesStruct xgifb_lcddes_1280x1024x75[] = {
 };
 
 /* 1280x1024x75Hz */
-static struct XGI330_LCDDataDesStruct  XGI_CetLCDDes1280x1024x75Data[] = {
+static struct XGI_LCDDesStruct XGI_CetLCDDes1280x1024x75Data[] = {
 	{1368, 1008, 752, 711}, /* ; 00 (320x200,320x400,640x200,640x400) */
 	{1368, 1008, 729, 688}, /* ; 01 (320x350,640x350) */
 	{1408, 1048, 752, 711}, /* ; 02 (360x400,720x400) */
diff --git a/drivers/staging/xgifb/vgatypes.h b/drivers/staging/xgifb/vgatypes.h
index a7208e31581..30cdd1af81f 100644
--- a/drivers/staging/xgifb/vgatypes.h
+++ b/drivers/staging/xgifb/vgatypes.h
@@ -66,8 +66,6 @@ struct xgi_hw_device_info {
 	unsigned long ulVideoMemorySize; /* size, in bytes, of the
 					    memory on the board */
 
-	unsigned char *pjIOAddress; /* base I/O address of VGA ports (0x3B0) */
-
 	unsigned char jChipType; /* Used to Identify Graphics Chip */
 				 /* defined in the data structure type  */
 				 /* "XGI_CHIP_TYPE" */
diff --git a/drivers/staging/zsmalloc/zsmalloc-main.c b/drivers/staging/zsmalloc/zsmalloc-main.c
index 917461c6601..44967377328 100644
--- a/drivers/staging/zsmalloc/zsmalloc-main.c
+++ b/drivers/staging/zsmalloc/zsmalloc-main.c
@@ -45,12 +45,12 @@ static DEFINE_PER_CPU(struct mapping_area, zs_map_area);
 
 static int is_first_page(struct page *page)
 {
-	return test_bit(PG_private, &page->flags);
+	return PagePrivate(page);
 }
 
 static int is_last_page(struct page *page)
 {
-	return test_bit(PG_private_2, &page->flags);
+	return PagePrivate2(page);
 }
 
 static void get_zspage_mapping(struct page *page, unsigned int *class_idx,
@@ -180,7 +180,7 @@ out:
  * link together 3 PAGE_SIZE sized pages to form a zspage
  * since then we can perfectly fit in 8 such objects.
  */
-static int get_zspage_order(int class_size)
+static int get_pages_per_zspage(int class_size)
 {
 	int i, max_usedpc = 0;
 	/* zspage order which gives maximum used size per KB */
@@ -368,7 +368,7 @@ static struct page *alloc_zspage(struct size_class *class, gfp_t flags)
 	 * identify the last page.
 	 */
 	error = -ENOMEM;
-	for (i = 0; i < class->zspage_order; i++) {
+	for (i = 0; i < class->pages_per_zspage; i++) {
 		struct page *page, *prev_page;
 
 		page = alloc_page(flags);
@@ -377,7 +377,7 @@ static struct page *alloc_zspage(struct size_class *class, gfp_t flags)
 
 		INIT_LIST_HEAD(&page->lru);
 		if (i == 0) {	/* first page */
-			set_bit(PG_private, &page->flags);
+			SetPagePrivate(page);
 			set_page_private(page, 0);
 			first_page = page;
 			first_page->inuse = 0;
@@ -388,9 +388,8 @@ static struct page *alloc_zspage(struct size_class *class, gfp_t flags)
 			page->first_page = first_page;
 		if (i >= 2)
 			list_add(&page->lru, &prev_page->lru);
-		if (i == class->zspage_order - 1)	/* last page */
-			set_bit(PG_private_2, &page->flags);
-
+		if (i == class->pages_per_zspage - 1)	/* last page */
+			SetPagePrivate2(page);
 		prev_page = page;
 	}
 
@@ -398,7 +397,7 @@ static struct page *alloc_zspage(struct size_class *class, gfp_t flags)
 
 	first_page->freelist = obj_location_to_handle(first_page, 0);
 	/* Maximum number of objects we can store in this zspage */
-	first_page->objects = class->zspage_order * PAGE_SIZE / class->size;
+	first_page->objects = class->pages_per_zspage * PAGE_SIZE / class->size;
 
 	error = 0; /* Success */
 
@@ -513,7 +512,7 @@ struct zs_pool *zs_create_pool(const char *name, gfp_t flags)
 		class->size = size;
 		class->index = i;
 		spin_lock_init(&class->lock);
-		class->zspage_order = get_zspage_order(size);
+		class->pages_per_zspage = get_pages_per_zspage(size);
 
 	}
 
@@ -567,13 +566,9 @@ EXPORT_SYMBOL_GPL(zs_destroy_pool);
  * zs_malloc - Allocate block of given size from pool.
  * @pool: pool to allocate from
  * @size: size of block to allocate
- * @page: page no. that holds the object
- * @offset: location of object within page
- *
- * On success, <page, offset> identifies block allocated
- * and 0 is returned. On failure, <page, offset> is set to
- * 0 and -ENOMEM is returned.
  *
+ * On success, handle to the allocated object is returned,
+ * otherwise NULL.
  * Allocation requests with size > ZS_MAX_ALLOC_SIZE will fail.
  */
 void *zs_malloc(struct zs_pool *pool, size_t size)
@@ -604,7 +599,7 @@ void *zs_malloc(struct zs_pool *pool, size_t size)
 
 		set_zspage_mapping(first_page, class->index, ZS_EMPTY);
 		spin_lock(&class->lock);
-		class->pages_allocated += class->zspage_order;
+		class->pages_allocated += class->pages_per_zspage;
 	}
 
 	obj = first_page->freelist;
@@ -659,7 +654,7 @@ void zs_free(struct zs_pool *pool, void *obj)
 	fullness = fix_fullness_group(pool, first_page);
 
 	if (fullness == ZS_EMPTY)
-		class->pages_allocated -= class->zspage_order;
+		class->pages_allocated -= class->pages_per_zspage;
 
 	spin_unlock(&class->lock);
 
@@ -668,6 +663,15 @@ void zs_free(struct zs_pool *pool, void *obj)
 }
 EXPORT_SYMBOL_GPL(zs_free);
 
+/**
+ * zs_map_object - get address of allocated object from handle.
+ * @pool: pool from which the object was allocated
+ * @handle: handle returned from zs_malloc
+ *
+ * Before using an object allocated from zs_malloc, it must be mapped using
+ * this function. When done with the object, it must be unmapped using
+ * zs_unmap_object
+*/
 void *zs_map_object(struct zs_pool *pool, void *handle)
 {
 	struct page *page;
diff --git a/drivers/staging/zsmalloc/zsmalloc_int.h b/drivers/staging/zsmalloc/zsmalloc_int.h
index 92eefc663af..6fd32a9e031 100644
--- a/drivers/staging/zsmalloc/zsmalloc_int.h
+++ b/drivers/staging/zsmalloc/zsmalloc_int.h
@@ -124,7 +124,7 @@ struct size_class {
 	unsigned int index;
 
 	/* Number of PAGE_SIZE sized pages to combine to form a 'zspage' */
-	int zspage_order;
+	int pages_per_zspage;
 
 	spinlock_t lock;
 
diff --git a/drivers/target/Kconfig b/drivers/target/Kconfig
index b28794b7212..18303686eb5 100644
--- a/drivers/target/Kconfig
+++ b/drivers/target/Kconfig
@@ -32,5 +32,6 @@ config TCM_PSCSI
 source "drivers/target/loopback/Kconfig"
 source "drivers/target/tcm_fc/Kconfig"
 source "drivers/target/iscsi/Kconfig"
+source "drivers/target/sbp/Kconfig"
 
 endif
diff --git a/drivers/target/Makefile b/drivers/target/Makefile
index 62e54053bcd..61648d84fbb 100644
--- a/drivers/target/Makefile
+++ b/drivers/target/Makefile
@@ -25,3 +25,4 @@ obj-$(CONFIG_TCM_PSCSI)		+= target_core_pscsi.o
 obj-$(CONFIG_LOOPBACK_TARGET)	+= loopback/
 obj-$(CONFIG_TCM_FC)		+= tcm_fc/
 obj-$(CONFIG_ISCSI_TARGET)	+= iscsi/
+obj-$(CONFIG_SBP_TARGET)	+= sbp/
diff --git a/drivers/target/iscsi/iscsi_target.c b/drivers/target/iscsi/iscsi_target.c
index 8b1d5e62ed4..d57d10cb2e4 100644
--- a/drivers/target/iscsi/iscsi_target.c
+++ b/drivers/target/iscsi/iscsi_target.c
@@ -27,8 +27,10 @@
 #include <asm/unaligned.h>
 #include <scsi/scsi_device.h>
 #include <scsi/iscsi_proto.h>
+#include <scsi/scsi_tcq.h>
 #include <target/target_core_base.h>
 #include <target/target_core_fabric.h>
+#include <target/target_core_configfs.h>
 
 #include "iscsi_target_core.h"
 #include "iscsi_target_parameters.h"
@@ -593,7 +595,7 @@ static void __exit iscsi_target_cleanup_module(void)
 	kfree(iscsit_global);
 }
 
-int iscsit_add_reject(
+static int iscsit_add_reject(
 	u8 reason,
 	int fail_conn,
 	unsigned char *buf,
@@ -622,7 +624,7 @@ int iscsit_add_reject(
 	}
 
 	spin_lock_bh(&conn->cmd_lock);
-	list_add_tail(&cmd->i_list, &conn->conn_cmd_list);
+	list_add_tail(&cmd->i_conn_node, &conn->conn_cmd_list);
 	spin_unlock_bh(&conn->cmd_lock);
 
 	cmd->i_state = ISTATE_SEND_REJECT;
@@ -669,7 +671,7 @@ int iscsit_add_reject_from_cmd(
 
 	if (add_to_conn) {
 		spin_lock_bh(&conn->cmd_lock);
-		list_add_tail(&cmd->i_list, &conn->conn_cmd_list);
+		list_add_tail(&cmd->i_conn_node, &conn->conn_cmd_list);
 		spin_unlock_bh(&conn->cmd_lock);
 	}
 
@@ -685,9 +687,7 @@ int iscsit_add_reject_from_cmd(
 
 /*
  * Map some portion of the allocated scatterlist to an iovec, suitable for
- * kernel sockets to copy data in/out. This handles both pages and slab-allocated
- * buffers, since we have been tricky and mapped t_mem_sg to the buffer in
- * either case (see iscsit_alloc_buffs)
+ * kernel sockets to copy data in/out.
  */
 static int iscsit_map_iovec(
 	struct iscsi_cmd *cmd,
@@ -700,10 +700,9 @@ static int iscsit_map_iovec(
 	unsigned int page_off;
 
 	/*
-	 * We have a private mapping of the allocated pages in t_mem_sg.
-	 * At this point, we also know each contains a page.
+	 * We know each entry in t_data_sg contains a page.
 	 */
-	sg = &cmd->t_mem_sg[data_offset / PAGE_SIZE];
+	sg = &cmd->se_cmd.t_data_sg[data_offset / PAGE_SIZE];
 	page_off = (data_offset % PAGE_SIZE);
 
 	cmd->first_data_sg = sg;
@@ -744,7 +743,7 @@ static void iscsit_ack_from_expstatsn(struct iscsi_conn *conn, u32 exp_statsn)
 	conn->exp_statsn = exp_statsn;
 
 	spin_lock_bh(&conn->cmd_lock);
-	list_for_each_entry(cmd, &conn->conn_cmd_list, i_list) {
+	list_for_each_entry(cmd, &conn->conn_cmd_list, i_conn_node) {
 		spin_lock(&cmd->istate_lock);
 		if ((cmd->i_state == ISTATE_SENT_STATUS) &&
 		    (cmd->stat_sn < exp_statsn)) {
@@ -761,8 +760,7 @@ static void iscsit_ack_from_expstatsn(struct iscsi_conn *conn, u32 exp_statsn)
 
 static int iscsit_allocate_iovecs(struct iscsi_cmd *cmd)
 {
-	u32 iov_count = (cmd->se_cmd.t_data_nents == 0) ? 1 :
-				cmd->se_cmd.t_data_nents;
+	u32 iov_count = max(1UL, DIV_ROUND_UP(cmd->se_cmd.data_length, PAGE_SIZE));
 
 	iov_count += ISCSI_IOV_DATA_BUFFER;
 
@@ -776,64 +774,6 @@ static int iscsit_allocate_iovecs(struct iscsi_cmd *cmd)
 	return 0;
 }
 
-static int iscsit_alloc_buffs(struct iscsi_cmd *cmd)
-{
-	struct scatterlist *sgl;
-	u32 length = cmd->se_cmd.data_length;
-	int nents = DIV_ROUND_UP(length, PAGE_SIZE);
-	int i = 0, j = 0, ret;
-	/*
-	 * If no SCSI payload is present, allocate the default iovecs used for
-	 * iSCSI PDU Header
-	 */
-	if (!length)
-		return iscsit_allocate_iovecs(cmd);
-
-	sgl = kzalloc(sizeof(*sgl) * nents, GFP_KERNEL);
-	if (!sgl)
-		return -ENOMEM;
-
-	sg_init_table(sgl, nents);
-
-	while (length) {
-		int buf_size = min_t(int, length, PAGE_SIZE);
-		struct page *page;
-
-		page = alloc_page(GFP_KERNEL | __GFP_ZERO);
-		if (!page)
-			goto page_alloc_failed;
-
-		sg_set_page(&sgl[i], page, buf_size, 0);
-
-		length -= buf_size;
-		i++;
-	}
-
-	cmd->t_mem_sg = sgl;
-	cmd->t_mem_sg_nents = nents;
-
-	/* BIDI ops not supported */
-
-	/* Tell the core about our preallocated memory */
-	transport_generic_map_mem_to_cmd(&cmd->se_cmd, sgl, nents, NULL, 0);
-	/*
-	 * Allocate iovecs for SCSI payload after transport_generic_map_mem_to_cmd
-	 * so that cmd->se_cmd.t_tasks_se_num has been set.
-	 */
-        ret = iscsit_allocate_iovecs(cmd);
-        if (ret < 0)
-		return -ENOMEM;
-
-	return 0;
-
-page_alloc_failed:
-	while (j < i)
-		__free_page(sg_page(&sgl[j++]));
-
-	kfree(sgl);
-	return -ENOMEM;
-}
-
 static int iscsit_handle_scsi_cmd(
 	struct iscsi_conn *conn,
 	unsigned char *buf)
@@ -842,6 +782,8 @@ static int iscsit_handle_scsi_cmd(
 	int	dump_immediate_data = 0, send_check_condition = 0, payload_length;
 	struct iscsi_cmd	*cmd = NULL;
 	struct iscsi_scsi_req *hdr;
+	int iscsi_task_attr;
+	int sam_task_attr;
 
 	spin_lock_bh(&conn->sess->session_stats_lock);
 	conn->sess->cmd_pdus++;
@@ -958,15 +900,30 @@ done:
 			 (hdr->flags & ISCSI_FLAG_CMD_READ) ? DMA_FROM_DEVICE :
 			  DMA_NONE;
 
-	cmd = iscsit_allocate_se_cmd(conn, hdr->data_length, data_direction,
-				(hdr->flags & ISCSI_FLAG_CMD_ATTR_MASK));
+	cmd = iscsit_allocate_cmd(conn, GFP_KERNEL);
 	if (!cmd)
 		return iscsit_add_reject(ISCSI_REASON_BOOKMARK_NO_RESOURCES, 1,
-					buf, conn);
+					 buf, conn);
 
-	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);
+	cmd->data_direction = data_direction;
+	iscsi_task_attr = hdr->flags & ISCSI_FLAG_CMD_ATTR_MASK;
+	/*
+	 * Figure out the SAM Task Attribute for the incoming SCSI CDB
+	 */
+	if ((iscsi_task_attr == ISCSI_ATTR_UNTAGGED) ||
+	    (iscsi_task_attr == ISCSI_ATTR_SIMPLE))
+		sam_task_attr = MSG_SIMPLE_TAG;
+	else if (iscsi_task_attr == ISCSI_ATTR_ORDERED)
+		sam_task_attr = MSG_ORDERED_TAG;
+	else if (iscsi_task_attr == ISCSI_ATTR_HEAD_OF_QUEUE)
+		sam_task_attr = MSG_HEAD_TAG;
+	else if (iscsi_task_attr == ISCSI_ATTR_ACA)
+		sam_task_attr = MSG_ACA_TAG;
+	else {
+		pr_debug("Unknown iSCSI Task Attribute: 0x%02x, using"
+			" MSG_SIMPLE_TAG\n", iscsi_task_attr);
+		sam_task_attr = MSG_SIMPLE_TAG;
+	}
 
 	cmd->iscsi_opcode	= ISCSI_OP_SCSI_CMD;
 	cmd->i_state		= ISTATE_NEW_CMD;
@@ -1003,6 +960,17 @@ done:
 	}
 
 	/*
+	 * Initialize struct se_cmd descriptor from target_core_mod infrastructure
+	 */
+	transport_init_se_cmd(&cmd->se_cmd, &lio_target_fabric_configfs->tf_ops,
+			conn->sess->se_sess, hdr->data_length, cmd->data_direction,
+			sam_task_attr, &cmd->sense_buffer[0]);
+
+	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);
+
+	/*
 	 * The CDB is going to an se_device_t.
 	 */
 	ret = transport_lookup_cmd_lun(&cmd->se_cmd,
@@ -1016,13 +984,8 @@ done:
 		send_check_condition = 1;
 		goto attach_cmd;
 	}
-	/*
-	 * The Initiator Node has access to the LUN (the addressing method
-	 * is handled inside of iscsit_get_lun_for_cmd()).  Now it's time to
-	 * allocate 1->N transport tasks (depending on sector count and
-	 * maximum request size the physical HBA(s) can handle.
-	 */
-	transport_ret = transport_generic_allocate_tasks(&cmd->se_cmd, hdr->cdb);
+
+	transport_ret = target_setup_cmd_from_cdb(&cmd->se_cmd, hdr->cdb);
 	if (transport_ret == -ENOMEM) {
 		return iscsit_add_reject_from_cmd(
 				ISCSI_REASON_BOOKMARK_NO_RESOURCES,
@@ -1035,9 +998,7 @@ done:
 		 */
 		send_check_condition = 1;
 	} else {
-		cmd->data_length = cmd->se_cmd.data_length;
-
-		if (iscsit_decide_list_to_build(cmd, payload_length) < 0)
+		if (iscsit_build_pdu_and_seq_lists(cmd, payload_length) < 0)
 			return iscsit_add_reject_from_cmd(
 				ISCSI_REASON_BOOKMARK_NO_RESOURCES,
 				1, 1, buf, cmd);
@@ -1045,18 +1006,15 @@ done:
 
 attach_cmd:
 	spin_lock_bh(&conn->cmd_lock);
-	list_add_tail(&cmd->i_list, &conn->conn_cmd_list);
+	list_add_tail(&cmd->i_conn_node, &conn->conn_cmd_list);
 	spin_unlock_bh(&conn->cmd_lock);
 	/*
 	 * Check if we need to delay processing because of ALUA
 	 * Active/NonOptimized primary access state..
 	 */
 	core_alua_check_nonop_delay(&cmd->se_cmd);
-	/*
-	 * Allocate and setup SGL used with transport_generic_map_mem_to_cmd().
-	 * also call iscsit_allocate_iovecs()
-	 */
-	ret = iscsit_alloc_buffs(cmd);
+
+	ret = iscsit_allocate_iovecs(cmd);
 	if (ret < 0)
 		return iscsit_add_reject_from_cmd(
 				ISCSI_REASON_BOOKMARK_NO_RESOURCES,
@@ -1303,10 +1261,10 @@ static int iscsit_handle_data_out(struct iscsi_conn *conn, unsigned char *buf)
 	se_cmd = &cmd->se_cmd;
 	iscsit_mod_dataout_timer(cmd);
 
-	if ((hdr->offset + payload_length) > cmd->data_length) {
+	if ((hdr->offset + payload_length) > cmd->se_cmd.data_length) {
 		pr_err("DataOut Offset: %u, Length %u greater than"
 			" iSCSI Command EDTL %u, protocol error.\n",
-			hdr->offset, payload_length, cmd->data_length);
+			hdr->offset, payload_length, cmd->se_cmd.data_length);
 		return iscsit_add_reject_from_cmd(ISCSI_REASON_BOOKMARK_INVALID,
 				1, 0, buf, cmd);
 	}
@@ -1442,7 +1400,7 @@ static int iscsit_handle_data_out(struct iscsi_conn *conn, unsigned char *buf)
 		return 0;
 	else if (ret == DATAOUT_SEND_R2T) {
 		iscsit_set_dataout_sequence_values(cmd);
-		iscsit_build_r2ts_for_cmd(cmd, conn, 0);
+		iscsit_build_r2ts_for_cmd(cmd, conn, false);
 	} else if (ret == DATAOUT_SEND_TO_TRANSPORT) {
 		/*
 		 * Handle extra special case for out of order
@@ -1617,7 +1575,7 @@ static int iscsit_handle_nop_out(
 		 * Initiator is expecting a NopIN ping reply,
 		 */
 		spin_lock_bh(&conn->cmd_lock);
-		list_add_tail(&cmd->i_list, &conn->conn_cmd_list);
+		list_add_tail(&cmd->i_conn_node, &conn->conn_cmd_list);
 		spin_unlock_bh(&conn->cmd_lock);
 
 		iscsit_ack_from_expstatsn(conn, hdr->exp_statsn);
@@ -1723,10 +1681,75 @@ static int iscsit_handle_task_mgt_cmd(
 	    (hdr->refcmdsn != ISCSI_RESERVED_TAG))
 		hdr->refcmdsn = ISCSI_RESERVED_TAG;
 
-	cmd = iscsit_allocate_se_cmd_for_tmr(conn, function);
+	cmd = iscsit_allocate_cmd(conn, GFP_KERNEL);
 	if (!cmd)
 		return iscsit_add_reject(ISCSI_REASON_BOOKMARK_NO_RESOURCES,
-					1, buf, conn);
+					 1, buf, conn);
+
+	cmd->data_direction = DMA_NONE;
+
+	cmd->tmr_req = kzalloc(sizeof(struct iscsi_tmr_req), GFP_KERNEL);
+	if (!cmd->tmr_req) {
+		pr_err("Unable to allocate memory for"
+			" Task Management command!\n");
+		return iscsit_add_reject_from_cmd(
+			ISCSI_REASON_BOOKMARK_NO_RESOURCES,
+			1, 1, buf, cmd);
+	}
+
+	/*
+	 * TASK_REASSIGN for ERL=2 / connection stays inside of
+	 * LIO-Target $FABRIC_MOD
+	 */
+	if (function != ISCSI_TM_FUNC_TASK_REASSIGN) {
+
+		u8 tcm_function;
+		int ret;
+
+		transport_init_se_cmd(&cmd->se_cmd,
+				      &lio_target_fabric_configfs->tf_ops,
+				      conn->sess->se_sess, 0, DMA_NONE,
+				      MSG_SIMPLE_TAG, &cmd->sense_buffer[0]);
+
+		switch (function) {
+		case ISCSI_TM_FUNC_ABORT_TASK:
+			tcm_function = TMR_ABORT_TASK;
+			break;
+		case ISCSI_TM_FUNC_ABORT_TASK_SET:
+			tcm_function = TMR_ABORT_TASK_SET;
+			break;
+		case ISCSI_TM_FUNC_CLEAR_ACA:
+			tcm_function = TMR_CLEAR_ACA;
+			break;
+		case ISCSI_TM_FUNC_CLEAR_TASK_SET:
+			tcm_function = TMR_CLEAR_TASK_SET;
+			break;
+		case ISCSI_TM_FUNC_LOGICAL_UNIT_RESET:
+			tcm_function = TMR_LUN_RESET;
+			break;
+		case ISCSI_TM_FUNC_TARGET_WARM_RESET:
+			tcm_function = TMR_TARGET_WARM_RESET;
+			break;
+		case ISCSI_TM_FUNC_TARGET_COLD_RESET:
+			tcm_function = TMR_TARGET_COLD_RESET;
+			break;
+		default:
+			pr_err("Unknown iSCSI TMR Function:"
+			       " 0x%02x\n", function);
+			return iscsit_add_reject_from_cmd(
+				ISCSI_REASON_BOOKMARK_NO_RESOURCES,
+				1, 1, buf, cmd);
+		}
+
+		ret = core_tmr_alloc_req(&cmd->se_cmd, cmd->tmr_req,
+					 tcm_function, GFP_KERNEL);
+		if (ret < 0)
+			return iscsit_add_reject_from_cmd(
+				ISCSI_REASON_BOOKMARK_NO_RESOURCES,
+				1, 1, buf, cmd);
+
+		cmd->tmr_req->se_tmr_req = cmd->se_cmd.se_tmr_req;
+	}
 
 	cmd->iscsi_opcode	= ISCSI_OP_SCSI_TMFUNC;
 	cmd->i_state		= ISTATE_SEND_TASKMGTRSP;
@@ -1804,7 +1827,7 @@ static int iscsit_handle_task_mgt_cmd(
 		se_tmr->call_transport = 1;
 attach:
 	spin_lock_bh(&conn->cmd_lock);
-	list_add_tail(&cmd->i_list, &conn->conn_cmd_list);
+	list_add_tail(&cmd->i_conn_node, &conn->conn_cmd_list);
 	spin_unlock_bh(&conn->cmd_lock);
 
 	if (!(hdr->opcode & ISCSI_OP_IMMEDIATE)) {
@@ -1980,7 +2003,7 @@ static int iscsit_handle_text_cmd(
 	cmd->data_direction	= DMA_NONE;
 
 	spin_lock_bh(&conn->cmd_lock);
-	list_add_tail(&cmd->i_list, &conn->conn_cmd_list);
+	list_add_tail(&cmd->i_conn_node, &conn->conn_cmd_list);
 	spin_unlock_bh(&conn->cmd_lock);
 
 	iscsit_ack_from_expstatsn(conn, hdr->exp_statsn);
@@ -2168,7 +2191,7 @@ static int iscsit_handle_logout_cmd(
 		logout_remove = 1;
 
 	spin_lock_bh(&conn->cmd_lock);
-	list_add_tail(&cmd->i_list, &conn->conn_cmd_list);
+	list_add_tail(&cmd->i_conn_node, &conn->conn_cmd_list);
 	spin_unlock_bh(&conn->cmd_lock);
 
 	if (reason_code != ISCSI_LOGOUT_REASON_RECOVERY)
@@ -2178,7 +2201,7 @@ static int iscsit_handle_logout_cmd(
 	 * Immediate commands are executed, well, immediately.
 	 * Non-Immediate Logout Commands are executed in CmdSN order.
 	 */
-	if (hdr->opcode & ISCSI_OP_IMMEDIATE) {
+	if (cmd->immediate_cmd) {
 		int ret = iscsit_execute_cmd(cmd, 0);
 
 		if (ret < 0)
@@ -2336,7 +2359,7 @@ static int iscsit_handle_immediate_data(
 
 	cmd->write_data_done += length;
 
-	if (cmd->write_data_done == cmd->data_length) {
+	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;
@@ -2381,7 +2404,7 @@ static void iscsit_build_conn_drop_async_message(struct iscsi_conn *conn)
 	cmd->i_state = ISTATE_SEND_ASYNCMSG;
 
 	spin_lock_bh(&conn_p->cmd_lock);
-	list_add_tail(&cmd->i_list, &conn_p->conn_cmd_list);
+	list_add_tail(&cmd->i_conn_node, &conn_p->conn_cmd_list);
 	spin_unlock_bh(&conn_p->cmd_lock);
 
 	iscsit_add_cmd_to_response_queue(cmd, conn_p, cmd->i_state);
@@ -2434,10 +2457,19 @@ static int iscsit_send_conn_drop_async_message(
 	return 0;
 }
 
+static void iscsit_tx_thread_wait_for_tcp(struct iscsi_conn *conn)
+{
+	if ((conn->sock->sk->sk_shutdown & SEND_SHUTDOWN) ||
+	    (conn->sock->sk->sk_shutdown & RCV_SHUTDOWN)) {
+		wait_for_completion_interruptible_timeout(
+					&conn->tx_half_close_comp,
+					ISCSI_TX_THREAD_TCP_TIMEOUT * HZ);
+	}
+}
+
 static int iscsit_send_data_in(
 	struct iscsi_cmd *cmd,
-	struct iscsi_conn *conn,
-	int *eodr)
+	struct iscsi_conn *conn)
 {
 	int iov_ret = 0, set_statsn = 0;
 	u32 iov_count = 0, tx_size = 0;
@@ -2445,6 +2477,8 @@ static int iscsit_send_data_in(
 	struct iscsi_datain_req *dr;
 	struct iscsi_data_rsp *hdr;
 	struct kvec *iov;
+	int eodr = 0;
+	int ret;
 
 	memset(&datain, 0, sizeof(struct iscsi_datain));
 	dr = iscsit_get_datain_values(cmd, &datain);
@@ -2457,11 +2491,11 @@ static int iscsit_send_data_in(
 	/*
 	 * Be paranoid and double check the logic for now.
 	 */
-	if ((datain.offset + datain.length) > cmd->data_length) {
+	if ((datain.offset + datain.length) > cmd->se_cmd.data_length) {
 		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->data_length);
+				cmd->se_cmd.data_length);
 		return -1;
 	}
 
@@ -2577,13 +2611,26 @@ static int iscsit_send_data_in(
 		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);
+	else
+		ret = iscsit_send_tx_data(cmd, conn, 0);
+
+	iscsit_unmap_iovec(cmd);
+
+	if (ret < 0) {
+		iscsit_tx_thread_wait_for_tcp(conn);
+		return ret;
+	}
+
 	if (dr->dr_complete) {
-		*eodr = (cmd->se_cmd.se_cmd_flags & SCF_TRANSPORT_TASK_SENSE) ?
+		eodr = (cmd->se_cmd.se_cmd_flags & SCF_TRANSPORT_TASK_SENSE) ?
 				2 : 1;
 		iscsit_free_datain_req(cmd, dr);
 	}
 
-	return 0;
+	return eodr;
 }
 
 static int iscsit_send_logout_response(
@@ -2715,6 +2762,7 @@ static int iscsit_send_unsolicited_nopin(
 {
 	int tx_size = ISCSI_HDR_LEN;
 	struct iscsi_nopin *hdr;
+	int ret;
 
 	hdr			= (struct iscsi_nopin *) cmd->pdu;
 	memset(hdr, 0, ISCSI_HDR_LEN);
@@ -2747,6 +2795,17 @@ static int iscsit_send_unsolicited_nopin(
 	pr_debug("Sending Unsolicited NOPIN TTT: 0x%08x StatSN:"
 		" 0x%08x CID: %hu\n", hdr->ttt, cmd->stat_sn, conn->cid);
 
+	ret = iscsit_send_tx_data(cmd, conn, 1);
+	if (ret < 0) {
+		iscsit_tx_thread_wait_for_tcp(conn);
+		return ret;
+	}
+
+	spin_lock_bh(&cmd->istate_lock);
+	cmd->i_state = want_response ?
+		ISTATE_SENT_NOPIN_WANT_RESPONSE : ISTATE_SENT_STATUS;
+	spin_unlock_bh(&cmd->istate_lock);
+
 	return 0;
 }
 
@@ -2837,13 +2896,14 @@ static int iscsit_send_nopin_response(
 	return 0;
 }
 
-int iscsit_send_r2t(
+static int iscsit_send_r2t(
 	struct iscsi_cmd *cmd,
 	struct iscsi_conn *conn)
 {
 	int tx_size = 0;
 	struct iscsi_r2t *r2t;
 	struct iscsi_r2t_rsp *hdr;
+	int ret;
 
 	r2t = iscsit_get_r2t_from_list(cmd);
 	if (!r2t)
@@ -2899,19 +2959,27 @@ int iscsit_send_r2t(
 	r2t->sent_r2t = 1;
 	spin_unlock_bh(&cmd->r2t_lock);
 
+	ret = iscsit_send_tx_data(cmd, conn, 1);
+	if (ret < 0) {
+		iscsit_tx_thread_wait_for_tcp(conn);
+		return ret;
+	}
+
+	spin_lock_bh(&cmd->dataout_timeout_lock);
+	iscsit_start_dataout_timer(cmd, conn);
+	spin_unlock_bh(&cmd->dataout_timeout_lock);
+
 	return 0;
 }
 
 /*
- *	type 0: Normal Operation.
- *	type 1: Called from Storage Transport.
- *	type 2: Called from iscsi_task_reassign_complete_write() for
- *	        connection recovery.
+ *	@recovery: If called from iscsi_task_reassign_complete_write() for
+ *		connection recovery.
  */
 int iscsit_build_r2ts_for_cmd(
 	struct iscsi_cmd *cmd,
 	struct iscsi_conn *conn,
-	int type)
+	bool recovery)
 {
 	int first_r2t = 1;
 	u32 offset = 0, xfer_len = 0;
@@ -2922,32 +2990,37 @@ int iscsit_build_r2ts_for_cmd(
 		return 0;
 	}
 
-	if (conn->sess->sess_ops->DataSequenceInOrder && (type != 2))
-		if (cmd->r2t_offset < cmd->write_data_done)
-			cmd->r2t_offset = cmd->write_data_done;
+	if (conn->sess->sess_ops->DataSequenceInOrder &&
+	    !recovery)
+		cmd->r2t_offset = max(cmd->r2t_offset, cmd->write_data_done);
 
 	while (cmd->outstanding_r2ts < conn->sess->sess_ops->MaxOutstandingR2T) {
 		if (conn->sess->sess_ops->DataSequenceInOrder) {
 			offset = cmd->r2t_offset;
 
-			if (first_r2t && (type == 2)) {
-				xfer_len = ((offset +
-					     (conn->sess->sess_ops->MaxBurstLength -
-					     cmd->next_burst_len) >
-					     cmd->data_length) ?
-					    (cmd->data_length - offset) :
-					    (conn->sess->sess_ops->MaxBurstLength -
-					     cmd->next_burst_len));
+			if (first_r2t && recovery) {
+				int new_data_end = offset +
+					conn->sess->sess_ops->MaxBurstLength -
+					cmd->next_burst_len;
+
+				if (new_data_end > cmd->se_cmd.data_length)
+					xfer_len = cmd->se_cmd.data_length - offset;
+				else
+					xfer_len =
+						conn->sess->sess_ops->MaxBurstLength -
+						cmd->next_burst_len;
 			} else {
-				xfer_len = ((offset +
-					     conn->sess->sess_ops->MaxBurstLength) >
-					     cmd->data_length) ?
-					     (cmd->data_length - offset) :
-					     conn->sess->sess_ops->MaxBurstLength;
+				int new_data_end = offset +
+					conn->sess->sess_ops->MaxBurstLength;
+
+				if (new_data_end > cmd->se_cmd.data_length)
+					xfer_len = cmd->se_cmd.data_length - offset;
+				else
+					xfer_len = conn->sess->sess_ops->MaxBurstLength;
 			}
 			cmd->r2t_offset += xfer_len;
 
-			if (cmd->r2t_offset == cmd->data_length)
+			if (cmd->r2t_offset == cmd->se_cmd.data_length)
 				cmd->cmd_flags |= ICF_SENT_LAST_R2T;
 		} else {
 			struct iscsi_seq *seq;
@@ -3179,6 +3252,8 @@ static bool iscsit_check_inaddr_any(struct iscsi_np *np)
 	return ret;
 }
 
+#define SENDTARGETS_BUF_LIMIT 32768U
+
 static int iscsit_build_sendtargets_response(struct iscsi_cmd *cmd)
 {
 	char *payload = NULL;
@@ -3187,12 +3262,10 @@ static int iscsit_build_sendtargets_response(struct iscsi_cmd *cmd)
 	struct iscsi_tiqn *tiqn;
 	struct iscsi_tpg_np *tpg_np;
 	int buffer_len, end_of_buf = 0, len = 0, payload_len = 0;
-	unsigned char buf[256];
+	unsigned char buf[ISCSI_IQN_LEN+12]; /* iqn + "TargetName=" + \0 */
 
-	buffer_len = (conn->conn_ops->MaxRecvDataSegmentLength > 32768) ?
-			32768 : conn->conn_ops->MaxRecvDataSegmentLength;
-
-	memset(buf, 0, 256);
+	buffer_len = max(conn->conn_ops->MaxRecvDataSegmentLength,
+			 SENDTARGETS_BUF_LIMIT);
 
 	payload = kzalloc(buffer_len, GFP_KERNEL);
 	if (!payload) {
@@ -3408,18 +3481,6 @@ static int iscsit_send_reject(
 	return 0;
 }
 
-static void iscsit_tx_thread_wait_for_tcp(struct iscsi_conn *conn)
-{
-	if ((conn->sock->sk->sk_shutdown & SEND_SHUTDOWN) ||
-	    (conn->sock->sk->sk_shutdown & RCV_SHUTDOWN)) {
-		wait_for_completion_interruptible_timeout(
-					&conn->tx_half_close_comp,
-					ISCSI_TX_THREAD_TCP_TIMEOUT * HZ);
-	}
-}
-
-#ifdef CONFIG_SMP
-
 void iscsit_thread_get_cpumask(struct iscsi_conn *conn)
 {
 	struct iscsi_thread_set *ts = conn->thread_set;
@@ -3433,10 +3494,6 @@ void iscsit_thread_get_cpumask(struct iscsi_conn *conn)
 	 * execute upon.
 	 */
 	ord = ts->thread_id % cpumask_weight(cpu_online_mask);
-#if 0
-	pr_debug(">>>>>>>>>>>>>>>>>>>> Generated ord: %d from"
-			" thread_id: %d\n", ord, ts->thread_id);
-#endif
 	for_each_online_cpu(cpu) {
 		if (ord-- == 0) {
 			cpumask_set_cpu(cpu, conn->conn_cpumask);
@@ -3476,34 +3533,196 @@ static inline void iscsit_thread_check_cpumask(
 	 */
 	memset(buf, 0, 128);
 	cpumask_scnprintf(buf, 128, conn->conn_cpumask);
-#if 0
-	pr_debug(">>>>>>>>>>>>>> Calling set_cpus_allowed_ptr():"
-			" %s for %s\n", buf, p->comm);
-#endif
 	set_cpus_allowed_ptr(p, conn->conn_cpumask);
 }
 
-#else
-
-void iscsit_thread_get_cpumask(struct iscsi_conn *conn)
+static int handle_immediate_queue(struct iscsi_conn *conn)
 {
-	return;
+	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);
+
+			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);
+			goto err;
+		}
+	}
+
+	return 0;
+
+err:
+	return -1;
 }
 
-#define iscsit_thread_check_cpumask(X, Y, Z) ({})
-#endif /* CONFIG_SMP */
+static int handle_response_queue(struct iscsi_conn *conn)
+{
+	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);
+
+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);
+				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;
+			}
+
+			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;
+		}
+		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);
+				goto err;
+			}
+			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))
+			break;
+	}
+
+	return 0;
+
+err:
+	return -1;
+restart:
+	return -EAGAIN;
+}
 
 int iscsi_target_tx_thread(void *arg)
 {
-	u8 state;
-	int eodr = 0;
 	int ret = 0;
-	int sent_status = 0;
-	int use_misc = 0;
-	int map_sg = 0;
-	struct iscsi_cmd *cmd = NULL;
 	struct iscsi_conn *conn;
-	struct iscsi_queue_req *qr = NULL;
 	struct iscsi_thread_set *ts = arg;
 	/*
 	 * Allow ourselves to be interrupted by SIGINT so that a
@@ -3516,7 +3735,7 @@ restart:
 	if (!conn)
 		goto out;
 
-	eodr = map_sg = ret = sent_status = use_misc = 0;
+	ret = 0;
 
 	while (!kthread_should_stop()) {
 		/*
@@ -3531,251 +3750,15 @@ restart:
 		     signal_pending(current))
 			goto transport_err;
 
-get_immediate:
-		qr = iscsit_get_cmd_from_immediate_queue(conn);
-		if (qr) {
-			atomic_set(&conn->check_immediate_queue, 0);
-			cmd = qr->cmd;
-			state = qr->state;
-			kmem_cache_free(lio_qr_cache, qr);
-
-			spin_lock_bh(&cmd->istate_lock);
-			switch (state) {
-			case ISTATE_SEND_R2T:
-				spin_unlock_bh(&cmd->istate_lock);
-				ret = iscsit_send_r2t(cmd, conn);
-				break;
-			case ISTATE_REMOVE:
-				spin_unlock_bh(&cmd->istate_lock);
-
-				if (cmd->data_direction == DMA_TO_DEVICE)
-					iscsit_stop_dataout_timer(cmd);
-
-				spin_lock_bh(&conn->cmd_lock);
-				list_del(&cmd->i_list);
-				spin_unlock_bh(&conn->cmd_lock);
-
-				iscsit_free_cmd(cmd);
-				goto get_immediate;
-			case ISTATE_SEND_NOPIN_WANT_RESPONSE:
-				spin_unlock_bh(&cmd->istate_lock);
-				iscsit_mod_nopin_response_timer(conn);
-				ret = iscsit_send_unsolicited_nopin(cmd,
-						conn, 1);
-				break;
-			case ISTATE_SEND_NOPIN_NO_RESPONSE:
-				spin_unlock_bh(&cmd->istate_lock);
-				ret = iscsit_send_unsolicited_nopin(cmd,
-						conn, 0);
-				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);
-				spin_unlock_bh(&cmd->istate_lock);
-				goto transport_err;
-			}
-			if (ret < 0) {
-				conn->tx_immediate_queue = 0;
-				goto transport_err;
-			}
-
-			if (iscsit_send_tx_data(cmd, conn, 1) < 0) {
-				conn->tx_immediate_queue = 0;
-				iscsit_tx_thread_wait_for_tcp(conn);
-				goto transport_err;
-			}
-
-			spin_lock_bh(&cmd->istate_lock);
-			switch (state) {
-			case ISTATE_SEND_R2T:
-				spin_unlock_bh(&cmd->istate_lock);
-				spin_lock_bh(&cmd->dataout_timeout_lock);
-				iscsit_start_dataout_timer(cmd, conn);
-				spin_unlock_bh(&cmd->dataout_timeout_lock);
-				break;
-			case ISTATE_SEND_NOPIN_WANT_RESPONSE:
-				cmd->i_state = ISTATE_SENT_NOPIN_WANT_RESPONSE;
-				spin_unlock_bh(&cmd->istate_lock);
-				break;
-			case ISTATE_SEND_NOPIN_NO_RESPONSE:
-				cmd->i_state = ISTATE_SENT_STATUS;
-				spin_unlock_bh(&cmd->istate_lock);
-				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);
-				spin_unlock_bh(&cmd->istate_lock);
-				goto transport_err;
-			}
-			goto get_immediate;
-		} else
-			conn->tx_immediate_queue = 0;
-
-get_response:
-		qr = iscsit_get_cmd_from_response_queue(conn);
-		if (qr) {
-			cmd = qr->cmd;
-			state = qr->state;
-			kmem_cache_free(lio_qr_cache, qr);
-
-			spin_lock_bh(&cmd->istate_lock);
-check_rsp_state:
-			switch (state) {
-			case ISTATE_SEND_DATAIN:
-				spin_unlock_bh(&cmd->istate_lock);
-				ret = iscsit_send_data_in(cmd, conn,
-							  &eodr);
-				map_sg = 1;
-				break;
-			case ISTATE_SEND_STATUS:
-			case ISTATE_SEND_STATUS_RECOVERY:
-				spin_unlock_bh(&cmd->istate_lock);
-				use_misc = 1;
-				ret = iscsit_send_status(cmd, conn);
-				break;
-			case ISTATE_SEND_LOGOUTRSP:
-				spin_unlock_bh(&cmd->istate_lock);
-				use_misc = 1;
-				ret = iscsit_send_logout_response(cmd, conn);
-				break;
-			case ISTATE_SEND_ASYNCMSG:
-				spin_unlock_bh(&cmd->istate_lock);
-				use_misc = 1;
-				ret = iscsit_send_conn_drop_async_message(
-						cmd, conn);
-				break;
-			case ISTATE_SEND_NOPIN:
-				spin_unlock_bh(&cmd->istate_lock);
-				use_misc = 1;
-				ret = iscsit_send_nopin_response(cmd, conn);
-				break;
-			case ISTATE_SEND_REJECT:
-				spin_unlock_bh(&cmd->istate_lock);
-				use_misc = 1;
-				ret = iscsit_send_reject(cmd, conn);
-				break;
-			case ISTATE_SEND_TASKMGTRSP:
-				spin_unlock_bh(&cmd->istate_lock);
-				use_misc = 1;
-				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:
-				spin_unlock_bh(&cmd->istate_lock);
-				use_misc = 1;
-				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);
-				spin_unlock_bh(&cmd->istate_lock);
-				goto transport_err;
-			}
-			if (ret < 0) {
-				conn->tx_response_queue = 0;
-				goto transport_err;
-			}
-
-			if (map_sg && !conn->conn_ops->IFMarker) {
-				if (iscsit_fe_sendpage_sg(cmd, conn) < 0) {
-					conn->tx_response_queue = 0;
-					iscsit_tx_thread_wait_for_tcp(conn);
-					iscsit_unmap_iovec(cmd);
-					goto transport_err;
-				}
-			} else {
-				if (iscsit_send_tx_data(cmd, conn, use_misc) < 0) {
-					conn->tx_response_queue = 0;
-					iscsit_tx_thread_wait_for_tcp(conn);
-					iscsit_unmap_iovec(cmd);
-					goto transport_err;
-				}
-			}
-			map_sg = 0;
-			iscsit_unmap_iovec(cmd);
-
-			spin_lock_bh(&cmd->istate_lock);
-			switch (state) {
-			case ISTATE_SEND_DATAIN:
-				if (!eodr)
-					goto check_rsp_state;
-
-				if (eodr == 1) {
-					cmd->i_state = ISTATE_SENT_LAST_DATAIN;
-					sent_status = 1;
-					eodr = use_misc = 0;
-				} else if (eodr == 2) {
-					cmd->i_state = state =
-							ISTATE_SEND_STATUS;
-					sent_status = 0;
-					eodr = use_misc = 0;
-					goto check_rsp_state;
-				}
-				break;
-			case ISTATE_SEND_STATUS:
-				use_misc = 0;
-				sent_status = 1;
-				break;
-			case ISTATE_SEND_ASYNCMSG:
-			case ISTATE_SEND_NOPIN:
-			case ISTATE_SEND_STATUS_RECOVERY:
-			case ISTATE_SEND_TEXTRSP:
-				use_misc = 0;
-				sent_status = 1;
-				break;
-			case ISTATE_SEND_REJECT:
-				use_misc = 0;
-				if (cmd->cmd_flags & ICF_REJECT_FAIL_CONN) {
-					cmd->cmd_flags &= ~ICF_REJECT_FAIL_CONN;
-					spin_unlock_bh(&cmd->istate_lock);
-					complete(&cmd->reject_comp);
-					goto transport_err;
-				}
-				complete(&cmd->reject_comp);
-				break;
-			case ISTATE_SEND_TASKMGTRSP:
-				use_misc = 0;
-				sent_status = 1;
-				break;
-			case ISTATE_SEND_LOGOUTRSP:
-				spin_unlock_bh(&cmd->istate_lock);
-				if (!iscsit_logout_post_handler(cmd, conn))
-					goto restart;
-				spin_lock_bh(&cmd->istate_lock);
-				use_misc = 0;
-				sent_status = 1;
-				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);
-				spin_unlock_bh(&cmd->istate_lock);
-				goto transport_err;
-			}
-
-			if (sent_status) {
-				cmd->i_state = ISTATE_SENT_STATUS;
-				sent_status = 0;
-			}
-			spin_unlock_bh(&cmd->istate_lock);
-
-			if (atomic_read(&conn->check_immediate_queue))
-				goto get_immediate;
+		ret = handle_immediate_queue(conn);
+		if (ret < 0)
+			goto transport_err;
 
-			goto get_response;
-		} else
-			conn->tx_response_queue = 0;
+		ret = handle_response_queue(conn);
+		if (ret == -EAGAIN)
+			goto restart;
+		else if (ret < 0)
+			goto transport_err;
 	}
 
 transport_err:
@@ -3952,9 +3935,9 @@ static void iscsit_release_commands_from_conn(struct iscsi_conn *conn)
 	 * has been reset -> returned sleeping pre-handler state.
 	 */
 	spin_lock_bh(&conn->cmd_lock);
-	list_for_each_entry_safe(cmd, cmd_tmp, &conn->conn_cmd_list, i_list) {
+	list_for_each_entry_safe(cmd, cmd_tmp, &conn->conn_cmd_list, i_conn_node) {
 
-		list_del(&cmd->i_list);
+		list_del(&cmd->i_conn_node);
 		spin_unlock_bh(&conn->cmd_lock);
 
 		iscsit_increment_maxcmdsn(cmd, sess);
@@ -3972,7 +3955,7 @@ static void iscsit_stop_timers_for_cmds(
 	struct iscsi_cmd *cmd;
 
 	spin_lock_bh(&conn->cmd_lock);
-	list_for_each_entry(cmd, &conn->conn_cmd_list, i_list) {
+	list_for_each_entry(cmd, &conn->conn_cmd_list, i_conn_node) {
 		if (cmd->data_direction == DMA_TO_DEVICE)
 			iscsit_stop_dataout_timer(cmd);
 	}
diff --git a/drivers/target/iscsi/iscsi_target.h b/drivers/target/iscsi/iscsi_target.h
index 5db2ddeed5e..12abb4c9e34 100644
--- a/drivers/target/iscsi/iscsi_target.h
+++ b/drivers/target/iscsi/iscsi_target.h
@@ -18,8 +18,7 @@ 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_send_r2t(struct iscsi_cmd *, struct iscsi_conn *);
-extern int iscsit_build_r2ts_for_cmd(struct iscsi_cmd *, struct iscsi_conn *, int);
+extern int iscsit_build_r2ts_for_cmd(struct iscsi_cmd *, struct iscsi_conn *, 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_configfs.c b/drivers/target/iscsi/iscsi_target_configfs.c
index 00c58cc82c8..69dc8e35c03 100644
--- a/drivers/target/iscsi/iscsi_target_configfs.c
+++ b/drivers/target/iscsi/iscsi_target_configfs.c
@@ -1538,7 +1538,7 @@ static int lio_write_pending(struct se_cmd *se_cmd)
 	struct iscsi_cmd *cmd = container_of(se_cmd, struct iscsi_cmd, se_cmd);
 
 	if (!cmd->immediate_data && !cmd->unsolicited_data)
-		return iscsit_build_r2ts_for_cmd(cmd, cmd->conn, 1);
+		return iscsit_build_r2ts_for_cmd(cmd, cmd->conn, false);
 
 	return 0;
 }
diff --git a/drivers/target/iscsi/iscsi_target_core.h b/drivers/target/iscsi/iscsi_target_core.h
index 2aaee7efa68..1c70144cdaf 100644
--- a/drivers/target/iscsi/iscsi_target_core.h
+++ b/drivers/target/iscsi/iscsi_target_core.h
@@ -296,12 +296,11 @@ struct iscsi_datain_req {
 	u32			runlength;
 	u32			data_length;
 	u32			data_offset;
-	u32			data_offset_end;
 	u32			data_sn;
 	u32			next_burst_len;
 	u32			read_data_done;
 	u32			seq_send_order;
-	struct list_head	dr_list;
+	struct list_head	cmd_datain_node;
 } ____cacheline_aligned;
 
 struct iscsi_ooo_cmdsn {
@@ -381,8 +380,6 @@ struct iscsi_cmd {
 	u32			buf_ptr_size;
 	/* Used to store DataDigest */
 	u32			data_crc;
-	/* Total size in bytes associated with command */
-	u32			data_length;
 	/* Counter for MaxOutstandingR2T */
 	u32			outstanding_r2ts;
 	/* Next R2T Offset when DataSequenceInOrder=Yes */
@@ -464,16 +461,13 @@ struct iscsi_cmd {
 	/* Session the command is part of,  used for connection recovery */
 	struct iscsi_session	*sess;
 	/* list_head for connection list */
-	struct list_head	i_list;
+	struct list_head	i_conn_node;
 	/* The TCM I/O descriptor that is accessed via container_of() */
 	struct se_cmd		se_cmd;
 	/* Sense buffer that will be mapped into outgoing status */
 #define ISCSI_SENSE_BUFFER_LEN          (TRANSPORT_SENSE_BUFFER + 2)
 	unsigned char		sense_buffer[ISCSI_SENSE_BUFFER_LEN];
 
-	struct scatterlist	*t_mem_sg;
-	u32			t_mem_sg_nents;
-
 	u32			padding;
 	u8			pad_bytes[4];
 
@@ -500,8 +494,6 @@ struct iscsi_conn {
 	u8			network_transport;
 	enum iscsi_timer_flags_table nopin_timer_flags;
 	enum iscsi_timer_flags_table nopin_response_timer_flags;
-	u8			tx_immediate_queue;
-	u8			tx_response_queue;
 	/* Used to know what thread encountered a transport failure */
 	u8			which_thread;
 	/* connection id assigned by the Initiator */
diff --git a/drivers/target/iscsi/iscsi_target_datain_values.c b/drivers/target/iscsi/iscsi_target_datain_values.c
index 8c049512951..848fee76894 100644
--- a/drivers/target/iscsi/iscsi_target_datain_values.c
+++ b/drivers/target/iscsi/iscsi_target_datain_values.c
@@ -37,7 +37,7 @@ struct iscsi_datain_req *iscsit_allocate_datain_req(void)
 				" struct iscsi_datain_req\n");
 		return NULL;
 	}
-	INIT_LIST_HEAD(&dr->dr_list);
+	INIT_LIST_HEAD(&dr->cmd_datain_node);
 
 	return dr;
 }
@@ -45,14 +45,14 @@ struct iscsi_datain_req *iscsit_allocate_datain_req(void)
 void iscsit_attach_datain_req(struct iscsi_cmd *cmd, struct iscsi_datain_req *dr)
 {
 	spin_lock(&cmd->datain_lock);
-	list_add_tail(&dr->dr_list, &cmd->datain_list);
+	list_add_tail(&dr->cmd_datain_node, &cmd->datain_list);
 	spin_unlock(&cmd->datain_lock);
 }
 
 void iscsit_free_datain_req(struct iscsi_cmd *cmd, struct iscsi_datain_req *dr)
 {
 	spin_lock(&cmd->datain_lock);
-	list_del(&dr->dr_list);
+	list_del(&dr->cmd_datain_node);
 	spin_unlock(&cmd->datain_lock);
 
 	kmem_cache_free(lio_dr_cache, dr);
@@ -63,8 +63,8 @@ void iscsit_free_all_datain_reqs(struct iscsi_cmd *cmd)
 	struct iscsi_datain_req *dr, *dr_tmp;
 
 	spin_lock(&cmd->datain_lock);
-	list_for_each_entry_safe(dr, dr_tmp, &cmd->datain_list, dr_list) {
-		list_del(&dr->dr_list);
+	list_for_each_entry_safe(dr, dr_tmp, &cmd->datain_list, cmd_datain_node) {
+		list_del(&dr->cmd_datain_node);
 		kmem_cache_free(lio_dr_cache, dr);
 	}
 	spin_unlock(&cmd->datain_lock);
@@ -72,17 +72,14 @@ void iscsit_free_all_datain_reqs(struct iscsi_cmd *cmd)
 
 struct iscsi_datain_req *iscsit_get_datain_req(struct iscsi_cmd *cmd)
 {
-	struct iscsi_datain_req *dr;
-
 	if (list_empty(&cmd->datain_list)) {
 		pr_err("cmd->datain_list is empty for ITT:"
 			" 0x%08x\n", cmd->init_task_tag);
 		return NULL;
 	}
-	list_for_each_entry(dr, &cmd->datain_list, dr_list)
-		break;
 
-	return dr;
+	return list_first_entry(&cmd->datain_list, struct iscsi_datain_req,
+				cmd_datain_node);
 }
 
 /*
@@ -113,7 +110,7 @@ static struct iscsi_datain_req *iscsit_set_datain_values_yes_and_yes(
 	read_data_done = (!dr->recovery) ?
 			cmd->read_data_done : dr->read_data_done;
 
-	read_data_left = (cmd->data_length - read_data_done);
+	read_data_left = (cmd->se_cmd.data_length - read_data_done);
 	if (!read_data_left) {
 		pr_err("ITT: 0x%08x read_data_left is zero!\n",
 				cmd->init_task_tag);
@@ -212,7 +209,7 @@ static struct iscsi_datain_req *iscsit_set_datain_values_no_and_yes(
 	seq_send_order = (!dr->recovery) ?
 			cmd->seq_send_order : dr->seq_send_order;
 
-	read_data_left = (cmd->data_length - read_data_done);
+	read_data_left = (cmd->se_cmd.data_length - read_data_done);
 	if (!read_data_left) {
 		pr_err("ITT: 0x%08x read_data_left is zero!\n",
 				cmd->init_task_tag);
@@ -231,8 +228,8 @@ static struct iscsi_datain_req *iscsit_set_datain_values_no_and_yes(
 	offset = (seq->offset + seq->next_burst_len);
 
 	if ((offset + conn->conn_ops->MaxRecvDataSegmentLength) >=
-	     cmd->data_length) {
-		datain->length = (cmd->data_length - offset);
+	     cmd->se_cmd.data_length) {
+		datain->length = (cmd->se_cmd.data_length - offset);
 		datain->offset = offset;
 
 		datain->flags |= ISCSI_FLAG_CMD_FINAL;
@@ -264,7 +261,7 @@ static struct iscsi_datain_req *iscsit_set_datain_values_no_and_yes(
 		}
 	}
 
-	if ((read_data_done + datain->length) == cmd->data_length)
+	if ((read_data_done + datain->length) == cmd->se_cmd.data_length)
 		datain->flags |= ISCSI_FLAG_DATA_STATUS;
 
 	datain->data_sn = (!dr->recovery) ? cmd->data_sn++ : dr->data_sn++;
@@ -333,7 +330,7 @@ static struct iscsi_datain_req *iscsit_set_datain_values_yes_and_no(
 	read_data_done = (!dr->recovery) ?
 			cmd->read_data_done : dr->read_data_done;
 
-	read_data_left = (cmd->data_length - read_data_done);
+	read_data_left = (cmd->se_cmd.data_length - read_data_done);
 	if (!read_data_left) {
 		pr_err("ITT: 0x%08x read_data_left is zero!\n",
 				cmd->init_task_tag);
@@ -344,7 +341,7 @@ static struct iscsi_datain_req *iscsit_set_datain_values_yes_and_no(
 	if (!pdu)
 		return dr;
 
-	if ((read_data_done + pdu->length) == cmd->data_length) {
+	if ((read_data_done + pdu->length) == cmd->se_cmd.data_length) {
 		pdu->flags |= (ISCSI_FLAG_CMD_FINAL | ISCSI_FLAG_DATA_STATUS);
 		if (conn->sess->sess_ops->ErrorRecoveryLevel > 0)
 			pdu->flags |= ISCSI_FLAG_DATA_ACK;
@@ -433,7 +430,7 @@ static struct iscsi_datain_req *iscsit_set_datain_values_no_and_no(
 	seq_send_order = (!dr->recovery) ?
 			cmd->seq_send_order : dr->seq_send_order;
 
-	read_data_left = (cmd->data_length - read_data_done);
+	read_data_left = (cmd->se_cmd.data_length - read_data_done);
 	if (!read_data_left) {
 		pr_err("ITT: 0x%08x read_data_left is zero!\n",
 				cmd->init_task_tag);
@@ -463,7 +460,7 @@ static struct iscsi_datain_req *iscsit_set_datain_values_no_and_no(
 	} else
 		seq->next_burst_len += pdu->length;
 
-	if ((read_data_done + pdu->length) == cmd->data_length)
+	if ((read_data_done + pdu->length) == cmd->se_cmd.data_length)
 		pdu->flags |= ISCSI_FLAG_DATA_STATUS;
 
 	pdu->data_sn = (!dr->recovery) ? cmd->data_sn++ : dr->data_sn++;
diff --git a/drivers/target/iscsi/iscsi_target_erl0.c b/drivers/target/iscsi/iscsi_target_erl0.c
index 1ab0560b092..1a02016ecda 100644
--- a/drivers/target/iscsi/iscsi_target_erl0.c
+++ b/drivers/target/iscsi/iscsi_target_erl0.c
@@ -48,9 +48,9 @@ void iscsit_set_dataout_sequence_values(
 	if (cmd->unsolicited_data) {
 		cmd->seq_start_offset = cmd->write_data_done;
 		cmd->seq_end_offset = (cmd->write_data_done +
-			(cmd->data_length >
+			(cmd->se_cmd.data_length >
 			 conn->sess->sess_ops->FirstBurstLength) ?
-			conn->sess->sess_ops->FirstBurstLength : cmd->data_length);
+			conn->sess->sess_ops->FirstBurstLength : cmd->se_cmd.data_length);
 		return;
 	}
 
@@ -59,15 +59,15 @@ void iscsit_set_dataout_sequence_values(
 
 	if (!cmd->seq_start_offset && !cmd->seq_end_offset) {
 		cmd->seq_start_offset = cmd->write_data_done;
-		cmd->seq_end_offset = (cmd->data_length >
+		cmd->seq_end_offset = (cmd->se_cmd.data_length >
 			conn->sess->sess_ops->MaxBurstLength) ?
 			(cmd->write_data_done +
-			conn->sess->sess_ops->MaxBurstLength) : cmd->data_length;
+			conn->sess->sess_ops->MaxBurstLength) : cmd->se_cmd.data_length;
 	} else {
 		cmd->seq_start_offset = cmd->seq_end_offset;
 		cmd->seq_end_offset = ((cmd->seq_end_offset +
 			conn->sess->sess_ops->MaxBurstLength) >=
-			cmd->data_length) ? cmd->data_length :
+			cmd->se_cmd.data_length) ? cmd->se_cmd.data_length :
 			(cmd->seq_end_offset +
 			 conn->sess->sess_ops->MaxBurstLength);
 	}
@@ -182,13 +182,13 @@ static int iscsit_dataout_check_unsolicited_sequence(
 		if (!conn->sess->sess_ops->DataPDUInOrder)
 			goto out;
 
-		if ((first_burst_len != cmd->data_length) &&
+		if ((first_burst_len != cmd->se_cmd.data_length) &&
 		    (first_burst_len != conn->sess->sess_ops->FirstBurstLength)) {
 			pr_err("Unsolicited non-immediate data"
 			" received %u does not equal FirstBurstLength: %u, and"
 			" does not equal ExpXferLen %u.\n", first_burst_len,
 				conn->sess->sess_ops->FirstBurstLength,
-				cmd->data_length);
+				cmd->se_cmd.data_length);
 			transport_send_check_condition_and_sense(&cmd->se_cmd,
 					TCM_INCORRECT_AMOUNT_OF_DATA, 0);
 			return DATAOUT_CANNOT_RECOVER;
@@ -201,10 +201,10 @@ static int iscsit_dataout_check_unsolicited_sequence(
 				conn->sess->sess_ops->FirstBurstLength);
 			return DATAOUT_CANNOT_RECOVER;
 		}
-		if (first_burst_len == cmd->data_length) {
+		if (first_burst_len == cmd->se_cmd.data_length) {
 			pr_err("Command ITT: 0x%08x reached"
 			" ExpXferLen: %u, but ISCSI_FLAG_CMD_FINAL is not set. protocol"
-			" error.\n", cmd->init_task_tag, cmd->data_length);
+			" error.\n", cmd->init_task_tag, cmd->se_cmd.data_length);
 			return DATAOUT_CANNOT_RECOVER;
 		}
 	}
@@ -294,7 +294,7 @@ static int iscsit_dataout_check_sequence(
 			if ((next_burst_len <
 			     conn->sess->sess_ops->MaxBurstLength) &&
 			   ((cmd->write_data_done + payload_length) <
-			     cmd->data_length)) {
+			     cmd->se_cmd.data_length)) {
 				pr_err("Command ITT: 0x%08x set ISCSI_FLAG_CMD_FINAL"
 				" before end of DataOUT sequence, protocol"
 				" error.\n", cmd->init_task_tag);
@@ -319,7 +319,7 @@ static int iscsit_dataout_check_sequence(
 				return DATAOUT_CANNOT_RECOVER;
 			}
 			if ((cmd->write_data_done + payload_length) ==
-					cmd->data_length) {
+					cmd->se_cmd.data_length) {
 				pr_err("Command ITT: 0x%08x reached"
 				" last DataOUT PDU in sequence but ISCSI_FLAG_"
 				"CMD_FINAL is not set, protocol error.\n",
@@ -640,9 +640,12 @@ static int iscsit_dataout_post_crc_passed(
 
 	cmd->write_data_done += payload_length;
 
-	return (cmd->write_data_done == cmd->data_length) ?
-		DATAOUT_SEND_TO_TRANSPORT : (send_r2t) ?
-		DATAOUT_SEND_R2T : DATAOUT_NORMAL;
+	if (cmd->write_data_done == cmd->se_cmd.data_length)
+		return DATAOUT_SEND_TO_TRANSPORT;
+	else if (send_r2t)
+		return DATAOUT_SEND_R2T;
+	else
+		return DATAOUT_NORMAL;
 }
 
 static int iscsit_dataout_post_crc_failed(
diff --git a/drivers/target/iscsi/iscsi_target_erl1.c b/drivers/target/iscsi/iscsi_target_erl1.c
index 006f605edb0..ecdd46deedd 100644
--- a/drivers/target/iscsi/iscsi_target_erl1.c
+++ b/drivers/target/iscsi/iscsi_target_erl1.c
@@ -279,11 +279,9 @@ int iscsit_create_recovery_datain_values_datasequenceinorder_no(
 		 * seq->first_datasn and seq->last_datasn have not been set.
 		 */
 		if (!seq->sent) {
-#if 0
 			pr_err("Ignoring non-sent sequence 0x%08x ->"
 				" 0x%08x\n\n", seq->first_datasn,
 				seq->last_datasn);
-#endif
 			continue;
 		}
 
@@ -294,11 +292,10 @@ int iscsit_create_recovery_datain_values_datasequenceinorder_no(
 		 */
 		if ((seq->first_datasn < begrun) &&
 				(seq->last_datasn < begrun)) {
-#if 0
 			pr_err("Pre BegRun sequence 0x%08x ->"
 				" 0x%08x\n", seq->first_datasn,
 				seq->last_datasn);
-#endif
+
 			read_data_done += cmd->seq_list[i].xfer_len;
 			seq->next_burst_len = seq->pdu_send_order = 0;
 			continue;
@@ -309,11 +306,10 @@ int iscsit_create_recovery_datain_values_datasequenceinorder_no(
 		 */
 		if ((seq->first_datasn <= begrun) &&
 				(seq->last_datasn >= begrun)) {
-#if 0
 			pr_err("Found sequence begrun: 0x%08x in"
 				" 0x%08x -> 0x%08x\n", begrun,
 				seq->first_datasn, seq->last_datasn);
-#endif
+
 			seq_send_order = seq->seq_send_order;
 			data_sn = seq->first_datasn;
 			seq->next_burst_len = seq->pdu_send_order = 0;
@@ -369,10 +365,9 @@ int iscsit_create_recovery_datain_values_datasequenceinorder_no(
 		 */
 		if ((seq->first_datasn > begrun) ||
 				(seq->last_datasn > begrun)) {
-#if 0
 			pr_err("Post BegRun sequence 0x%08x -> 0x%08x\n",
 					seq->first_datasn, seq->last_datasn);
-#endif
+
 			seq->next_burst_len = seq->pdu_send_order = 0;
 			continue;
 		}
@@ -526,7 +521,7 @@ int iscsit_handle_status_snack(
 		found_cmd = 0;
 
 		spin_lock_bh(&conn->cmd_lock);
-		list_for_each_entry(cmd, &conn->conn_cmd_list, i_list) {
+		list_for_each_entry(cmd, &conn->conn_cmd_list, i_conn_node) {
 			if (cmd->stat_sn == begrun) {
 				found_cmd = 1;
 				break;
@@ -987,7 +982,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, 0);
+				iscsit_build_r2ts_for_cmd(cmd, cmd->conn, false);
 			}
 			return 0;
 		}
@@ -1121,8 +1116,8 @@ static int iscsit_set_dataout_timeout_values(
 	if (cmd->unsolicited_data) {
 		*offset = 0;
 		*length = (conn->sess->sess_ops->FirstBurstLength >
-			   cmd->data_length) ?
-			   cmd->data_length :
+			   cmd->se_cmd.data_length) ?
+			   cmd->se_cmd.data_length :
 			   conn->sess->sess_ops->FirstBurstLength;
 		return 0;
 	}
@@ -1193,8 +1188,8 @@ static void iscsit_handle_dataout_timeout(unsigned long data)
 		if (conn->sess->sess_ops->DataPDUInOrder) {
 			pdu_offset = cmd->write_data_done;
 			if ((pdu_offset + (conn->sess->sess_ops->MaxBurstLength -
-			     cmd->next_burst_len)) > cmd->data_length)
-				pdu_length = (cmd->data_length -
+			     cmd->next_burst_len)) > cmd->se_cmd.data_length)
+				pdu_length = (cmd->se_cmd.data_length -
 					cmd->write_data_done);
 			else
 				pdu_length = (conn->sess->sess_ops->MaxBurstLength -
diff --git a/drivers/target/iscsi/iscsi_target_erl2.c b/drivers/target/iscsi/iscsi_target_erl2.c
index 1af1f21af21..65aac14fd83 100644
--- a/drivers/target/iscsi/iscsi_target_erl2.c
+++ b/drivers/target/iscsi/iscsi_target_erl2.c
@@ -138,9 +138,9 @@ void iscsit_free_connection_recovery_entires(struct iscsi_session *sess)
 
 		spin_lock(&cr->conn_recovery_cmd_lock);
 		list_for_each_entry_safe(cmd, cmd_tmp,
-				&cr->conn_recovery_cmd_list, i_list) {
+				&cr->conn_recovery_cmd_list, i_conn_node) {
 
-			list_del(&cmd->i_list);
+			list_del(&cmd->i_conn_node);
 			cmd->conn = NULL;
 			spin_unlock(&cr->conn_recovery_cmd_lock);
 			iscsit_free_cmd(cmd);
@@ -160,9 +160,9 @@ void iscsit_free_connection_recovery_entires(struct iscsi_session *sess)
 
 		spin_lock(&cr->conn_recovery_cmd_lock);
 		list_for_each_entry_safe(cmd, cmd_tmp,
-				&cr->conn_recovery_cmd_list, i_list) {
+				&cr->conn_recovery_cmd_list, i_conn_node) {
 
-			list_del(&cmd->i_list);
+			list_del(&cmd->i_conn_node);
 			cmd->conn = NULL;
 			spin_unlock(&cr->conn_recovery_cmd_lock);
 			iscsit_free_cmd(cmd);
@@ -220,7 +220,7 @@ int iscsit_remove_cmd_from_connection_recovery(
 	}
 	cr = cmd->cr;
 
-	list_del(&cmd->i_list);
+	list_del(&cmd->i_conn_node);
 	return --cr->cmd_count;
 }
 
@@ -234,7 +234,7 @@ void iscsit_discard_cr_cmds_by_expstatsn(
 
 	spin_lock(&cr->conn_recovery_cmd_lock);
 	list_for_each_entry_safe(cmd, cmd_tmp,
-			&cr->conn_recovery_cmd_list, i_list) {
+			&cr->conn_recovery_cmd_list, i_conn_node) {
 
 		if (((cmd->deferred_i_state != ISTATE_SENT_STATUS) &&
 		     (cmd->deferred_i_state != ISTATE_REMOVE)) ||
@@ -297,11 +297,11 @@ int iscsit_discard_unacknowledged_ooo_cmdsns_for_conn(struct iscsi_conn *conn)
 	mutex_unlock(&sess->cmdsn_mutex);
 
 	spin_lock_bh(&conn->cmd_lock);
-	list_for_each_entry_safe(cmd, cmd_tmp, &conn->conn_cmd_list, i_list) {
+	list_for_each_entry_safe(cmd, cmd_tmp, &conn->conn_cmd_list, i_conn_node) {
 		if (!(cmd->cmd_flags & ICF_OOO_CMDSN))
 			continue;
 
-		list_del(&cmd->i_list);
+		list_del(&cmd->i_conn_node);
 
 		spin_unlock_bh(&conn->cmd_lock);
 		iscsit_free_cmd(cmd);
@@ -339,14 +339,14 @@ int iscsit_prepare_cmds_for_realligance(struct iscsi_conn *conn)
 	/*
 	 * Only perform connection recovery on ISCSI_OP_SCSI_CMD or
 	 * ISCSI_OP_NOOP_OUT opcodes.  For all other opcodes call
-	 * list_del(&cmd->i_list); to release the command to the
+	 * list_del(&cmd->i_conn_node); to release the command to the
 	 * session pool and remove it from the connection's list.
 	 *
 	 * Also stop the DataOUT timer, which will be restarted after
 	 * sending the TMR response.
 	 */
 	spin_lock_bh(&conn->cmd_lock);
-	list_for_each_entry_safe(cmd, cmd_tmp, &conn->conn_cmd_list, i_list) {
+	list_for_each_entry_safe(cmd, cmd_tmp, &conn->conn_cmd_list, i_conn_node) {
 
 		if ((cmd->iscsi_opcode != ISCSI_OP_SCSI_CMD) &&
 		    (cmd->iscsi_opcode != ISCSI_OP_NOOP_OUT)) {
@@ -355,7 +355,7 @@ int iscsit_prepare_cmds_for_realligance(struct iscsi_conn *conn)
 				" CID: %hu\n", cmd->iscsi_opcode,
 				cmd->init_task_tag, cmd->cmd_sn, conn->cid);
 
-			list_del(&cmd->i_list);
+			list_del(&cmd->i_conn_node);
 			spin_unlock_bh(&conn->cmd_lock);
 			iscsit_free_cmd(cmd);
 			spin_lock_bh(&conn->cmd_lock);
@@ -375,7 +375,7 @@ int iscsit_prepare_cmds_for_realligance(struct iscsi_conn *conn)
 		 */
 		if (!(cmd->cmd_flags & ICF_OOO_CMDSN) && !cmd->immediate_cmd &&
 		     (cmd->cmd_sn >= conn->sess->exp_cmd_sn)) {
-			list_del(&cmd->i_list);
+			list_del(&cmd->i_conn_node);
 			spin_unlock_bh(&conn->cmd_lock);
 			iscsit_free_cmd(cmd);
 			spin_lock_bh(&conn->cmd_lock);
@@ -397,7 +397,7 @@ int iscsit_prepare_cmds_for_realligance(struct iscsi_conn *conn)
 
 		cmd->sess = conn->sess;
 
-		list_del(&cmd->i_list);
+		list_del(&cmd->i_conn_node);
 		spin_unlock_bh(&conn->cmd_lock);
 
 		iscsit_free_all_datain_reqs(cmd);
@@ -407,7 +407,7 @@ int iscsit_prepare_cmds_for_realligance(struct iscsi_conn *conn)
 		 * Add the struct iscsi_cmd to the connection recovery cmd list
 		 */
 		spin_lock(&cr->conn_recovery_cmd_lock);
-		list_add_tail(&cmd->i_list, &cr->conn_recovery_cmd_list);
+		list_add_tail(&cmd->i_conn_node, &cr->conn_recovery_cmd_list);
 		spin_unlock(&cr->conn_recovery_cmd_lock);
 
 		spin_lock_bh(&conn->cmd_lock);
diff --git a/drivers/target/iscsi/iscsi_target_parameters.c b/drivers/target/iscsi/iscsi_target_parameters.c
index eb05c9d751e..ed5241e7f12 100644
--- a/drivers/target/iscsi/iscsi_target_parameters.c
+++ b/drivers/target/iscsi/iscsi_target_parameters.c
@@ -803,14 +803,6 @@ static int iscsi_check_numerical_value(struct iscsi_param *param, char *value_pt
 
 	value = simple_strtoul(value_ptr, &tmpptr, 0);
 
-/* #warning FIXME: Fix this */
-#if 0
-	if (strspn(endptr, WHITE_SPACE) != strlen(endptr)) {
-		pr_err("Illegal value \"%s\" for \"%s\".\n",
-			value, param->name);
-		return -1;
-	}
-#endif
 	if (IS_TYPERANGE_0_TO_2(param)) {
 		if ((value < 0) || (value > 2)) {
 			pr_err("Illegal value for \"%s\", must be"
@@ -1045,13 +1037,6 @@ static char *iscsi_check_valuelist_for_support(
 			tmp2 = strchr(acceptor_values, ',');
 			if (tmp2)
 				*tmp2 = '\0';
-			if (!acceptor_values || !proposer_values) {
-				if (tmp1)
-					*tmp1 = ',';
-				if (tmp2)
-					*tmp2 = ',';
-				return NULL;
-			}
 			if (!strcmp(acceptor_values, proposer_values)) {
 				if (tmp2)
 					*tmp2 = ',';
@@ -1061,8 +1046,6 @@ static char *iscsi_check_valuelist_for_support(
 				*tmp2++ = ',';
 
 			acceptor_values = tmp2;
-			if (!acceptor_values)
-				break;
 		} while (acceptor_values);
 		if (tmp1)
 			*tmp1++ = ',';
diff --git a/drivers/target/iscsi/iscsi_target_seq_pdu_list.c b/drivers/target/iscsi/iscsi_target_seq_pdu_list.c
index fc694082bfc..85a306e067b 100644
--- a/drivers/target/iscsi/iscsi_target_seq_pdu_list.c
+++ b/drivers/target/iscsi/iscsi_target_seq_pdu_list.c
@@ -24,11 +24,13 @@
 
 #include "iscsi_target_core.h"
 #include "iscsi_target_util.h"
+#include "iscsi_target_tpg.h"
 #include "iscsi_target_seq_pdu_list.h"
 
 #define OFFLOAD_BUF_SIZE	32768
 
-void iscsit_dump_seq_list(struct iscsi_cmd *cmd)
+#ifdef DEBUG
+static void iscsit_dump_seq_list(struct iscsi_cmd *cmd)
 {
 	int i;
 	struct iscsi_seq *seq;
@@ -46,7 +48,7 @@ void iscsit_dump_seq_list(struct iscsi_cmd *cmd)
 	}
 }
 
-void iscsit_dump_pdu_list(struct iscsi_cmd *cmd)
+static void iscsit_dump_pdu_list(struct iscsi_cmd *cmd)
 {
 	int i;
 	struct iscsi_pdu *pdu;
@@ -61,6 +63,10 @@ void iscsit_dump_pdu_list(struct iscsi_cmd *cmd)
 			pdu->length, pdu->pdu_send_order, pdu->seq_no);
 	}
 }
+#else
+static void iscsit_dump_seq_list(struct iscsi_cmd *cmd) {}
+static void iscsit_dump_pdu_list(struct iscsi_cmd *cmd) {}
+#endif
 
 static void iscsit_ordered_seq_lists(
 	struct iscsi_cmd *cmd,
@@ -135,11 +141,11 @@ redo:
 			seq_count++;
 			continue;
 		}
-		array = kzalloc(seq_count * sizeof(u32), GFP_KERNEL);
+		array = kcalloc(seq_count, sizeof(u32), GFP_KERNEL);
 		if (!array) {
 			pr_err("Unable to allocate memory"
 				" for random array.\n");
-			return -1;
+			return -ENOMEM;
 		}
 		iscsit_create_random_array(array, seq_count);
 
@@ -155,11 +161,11 @@ redo:
 	}
 
 	if (seq_count) {
-		array = kzalloc(seq_count * sizeof(u32), GFP_KERNEL);
+		array = kcalloc(seq_count, sizeof(u32), GFP_KERNEL);
 		if (!array) {
 			pr_err("Unable to allocate memory for"
 				" random array.\n");
-			return -1;
+			return -ENOMEM;
 		}
 		iscsit_create_random_array(array, seq_count);
 
@@ -187,10 +193,10 @@ static int iscsit_randomize_seq_lists(
 	if (!seq_count)
 		return 0;
 
-	array = kzalloc(seq_count * sizeof(u32), GFP_KERNEL);
+	array = kcalloc(seq_count, sizeof(u32), GFP_KERNEL);
 	if (!array) {
 		pr_err("Unable to allocate memory for random array.\n");
-		return -1;
+		return -ENOMEM;
 	}
 	iscsit_create_random_array(array, seq_count);
 
@@ -221,11 +227,10 @@ static void iscsit_determine_counts_for_list(
 
 	if ((bl->type == PDULIST_UNSOLICITED) ||
 	    (bl->type == PDULIST_IMMEDIATE_AND_UNSOLICITED))
-		unsolicited_data_length = (cmd->data_length >
-			conn->sess->sess_ops->FirstBurstLength) ?
-			conn->sess->sess_ops->FirstBurstLength : cmd->data_length;
+		unsolicited_data_length = min(cmd->se_cmd.data_length,
+			conn->sess->sess_ops->FirstBurstLength);
 
-	while (offset < cmd->data_length) {
+	while (offset < cmd->se_cmd.data_length) {
 		*pdu_count += 1;
 
 		if (check_immediate) {
@@ -239,10 +244,10 @@ static void iscsit_determine_counts_for_list(
 		}
 		if (unsolicited_data_length > 0) {
 			if ((offset + conn->conn_ops->MaxRecvDataSegmentLength)
-					>= cmd->data_length) {
+					>= cmd->se_cmd.data_length) {
 				unsolicited_data_length -=
-					(cmd->data_length - offset);
-				offset += (cmd->data_length - offset);
+					(cmd->se_cmd.data_length - offset);
+				offset += (cmd->se_cmd.data_length - offset);
 				continue;
 			}
 			if ((offset + conn->conn_ops->MaxRecvDataSegmentLength)
@@ -263,8 +268,8 @@ static void iscsit_determine_counts_for_list(
 			continue;
 		}
 		if ((offset + conn->conn_ops->MaxRecvDataSegmentLength) >=
-		     cmd->data_length) {
-			offset += (cmd->data_length - offset);
+		     cmd->se_cmd.data_length) {
+			offset += (cmd->se_cmd.data_length - offset);
 			continue;
 		}
 		if ((burstlength + conn->conn_ops->MaxRecvDataSegmentLength) >=
@@ -283,10 +288,10 @@ static void iscsit_determine_counts_for_list(
 
 
 /*
- *	Builds PDU and/or Sequence list,  called while DataSequenceInOrder=No
- *	and DataPDUInOrder=No.
+ *	Builds PDU and/or Sequence list, called while DataSequenceInOrder=No
+ *	or DataPDUInOrder=No.
  */
-static int iscsit_build_pdu_and_seq_list(
+static int iscsit_do_build_pdu_and_seq_lists(
 	struct iscsi_cmd *cmd,
 	struct iscsi_build_list *bl)
 {
@@ -306,11 +311,10 @@ static int iscsit_build_pdu_and_seq_list(
 
 	if ((bl->type == PDULIST_UNSOLICITED) ||
 	    (bl->type == PDULIST_IMMEDIATE_AND_UNSOLICITED))
-		unsolicited_data_length = (cmd->data_length >
-			conn->sess->sess_ops->FirstBurstLength) ?
-			conn->sess->sess_ops->FirstBurstLength : cmd->data_length;
+		unsolicited_data_length = min(cmd->se_cmd.data_length,
+			conn->sess->sess_ops->FirstBurstLength);
 
-	while (offset < cmd->data_length) {
+	while (offset < cmd->se_cmd.data_length) {
 		pdu_count++;
 		if (!datapduinorder) {
 			pdu[i].offset = offset;
@@ -346,21 +350,21 @@ static int iscsit_build_pdu_and_seq_list(
 		if (unsolicited_data_length > 0) {
 			if ((offset +
 			     conn->conn_ops->MaxRecvDataSegmentLength) >=
-			     cmd->data_length) {
+			     cmd->se_cmd.data_length) {
 				if (!datapduinorder) {
 					pdu[i].type = PDUTYPE_UNSOLICITED;
 					pdu[i].length =
-						(cmd->data_length - offset);
+						(cmd->se_cmd.data_length - offset);
 				}
 				if (!datasequenceinorder) {
 					seq[seq_no].type = SEQTYPE_UNSOLICITED;
 					seq[seq_no].pdu_count = pdu_count;
 					seq[seq_no].xfer_len = (burstlength +
-						(cmd->data_length - offset));
+						(cmd->se_cmd.data_length - offset));
 				}
 				unsolicited_data_length -=
-						(cmd->data_length - offset);
-				offset += (cmd->data_length - offset);
+						(cmd->se_cmd.data_length - offset);
+				offset += (cmd->se_cmd.data_length - offset);
 				continue;
 			}
 			if ((offset +
@@ -402,18 +406,18 @@ static int iscsit_build_pdu_and_seq_list(
 			continue;
 		}
 		if ((offset + conn->conn_ops->MaxRecvDataSegmentLength) >=
-		     cmd->data_length) {
+		     cmd->se_cmd.data_length) {
 			if (!datapduinorder) {
 				pdu[i].type = PDUTYPE_NORMAL;
-				pdu[i].length = (cmd->data_length - offset);
+				pdu[i].length = (cmd->se_cmd.data_length - offset);
 			}
 			if (!datasequenceinorder) {
 				seq[seq_no].type = SEQTYPE_NORMAL;
 				seq[seq_no].pdu_count = pdu_count;
 				seq[seq_no].xfer_len = (burstlength +
-					(cmd->data_length - offset));
+					(cmd->se_cmd.data_length - offset));
 			}
-			offset += (cmd->data_length - offset);
+			offset += (cmd->se_cmd.data_length - offset);
 			continue;
 		}
 		if ((burstlength + conn->conn_ops->MaxRecvDataSegmentLength) >=
@@ -464,9 +468,8 @@ static int iscsit_build_pdu_and_seq_list(
 			} else
 				iscsit_ordered_seq_lists(cmd, bl->type);
 		}
-#if 0
+
 		iscsit_dump_seq_list(cmd);
-#endif
 	}
 	if (!datapduinorder) {
 		if (bl->data_direction & ISCSI_PDU_WRITE) {
@@ -484,50 +487,86 @@ static int iscsit_build_pdu_and_seq_list(
 			} else
 				iscsit_ordered_pdu_lists(cmd, bl->type);
 		}
-#if 0
+
 		iscsit_dump_pdu_list(cmd);
-#endif
 	}
 
 	return 0;
 }
 
-/*
- *	Only called while DataSequenceInOrder=No or DataPDUInOrder=No.
- */
-int iscsit_do_build_list(
+int iscsit_build_pdu_and_seq_lists(
 	struct iscsi_cmd *cmd,
-	struct iscsi_build_list *bl)
+	u32 immediate_data_length)
 {
+	struct iscsi_build_list bl;
 	u32 pdu_count = 0, seq_count = 1;
 	struct iscsi_conn *conn = cmd->conn;
 	struct iscsi_pdu *pdu = NULL;
 	struct iscsi_seq *seq = NULL;
 
-	iscsit_determine_counts_for_list(cmd, bl, &seq_count, &pdu_count);
+	struct iscsi_session *sess = conn->sess;
+	struct iscsi_node_attrib *na;
+
+	/*
+	 * Do nothing if no OOO shenanigans
+	 */
+	if (sess->sess_ops->DataSequenceInOrder &&
+	    sess->sess_ops->DataPDUInOrder)
+		return 0;
+
+	if (cmd->data_direction == DMA_NONE)
+		return 0;
+
+	na = iscsit_tpg_get_node_attrib(sess);
+	memset(&bl, 0, sizeof(struct iscsi_build_list));
+
+	if (cmd->data_direction == DMA_FROM_DEVICE) {
+		bl.data_direction = ISCSI_PDU_READ;
+		bl.type = PDULIST_NORMAL;
+		if (na->random_datain_pdu_offsets)
+			bl.randomize |= RANDOM_DATAIN_PDU_OFFSETS;
+		if (na->random_datain_seq_offsets)
+			bl.randomize |= RANDOM_DATAIN_SEQ_OFFSETS;
+	} else {
+		bl.data_direction = ISCSI_PDU_WRITE;
+		bl.immediate_data_length = immediate_data_length;
+		if (na->random_r2t_offsets)
+			bl.randomize |= RANDOM_R2T_OFFSETS;
+
+		if (!cmd->immediate_data && !cmd->unsolicited_data)
+			bl.type = PDULIST_NORMAL;
+		else if (cmd->immediate_data && !cmd->unsolicited_data)
+			bl.type = PDULIST_IMMEDIATE;
+		else if (!cmd->immediate_data && cmd->unsolicited_data)
+			bl.type = PDULIST_UNSOLICITED;
+		else if (cmd->immediate_data && cmd->unsolicited_data)
+			bl.type = PDULIST_IMMEDIATE_AND_UNSOLICITED;
+	}
+
+	iscsit_determine_counts_for_list(cmd, &bl, &seq_count, &pdu_count);
 
 	if (!conn->sess->sess_ops->DataSequenceInOrder) {
-		seq = kzalloc(seq_count * sizeof(struct iscsi_seq), GFP_ATOMIC);
+		seq = kcalloc(seq_count, sizeof(struct iscsi_seq), GFP_ATOMIC);
 		if (!seq) {
 			pr_err("Unable to allocate struct iscsi_seq list\n");
-			return -1;
+			return -ENOMEM;
 		}
 		cmd->seq_list = seq;
 		cmd->seq_count = seq_count;
 	}
 
 	if (!conn->sess->sess_ops->DataPDUInOrder) {
-		pdu = kzalloc(pdu_count * sizeof(struct iscsi_pdu), GFP_ATOMIC);
+		pdu = kcalloc(pdu_count, sizeof(struct iscsi_pdu), GFP_ATOMIC);
 		if (!pdu) {
 			pr_err("Unable to allocate struct iscsi_pdu list.\n");
 			kfree(seq);
-			return -1;
+			return -ENOMEM;
 		}
 		cmd->pdu_list = pdu;
 		cmd->pdu_count = pdu_count;
 	}
 
-	return iscsit_build_pdu_and_seq_list(cmd, bl);
+	return iscsit_do_build_pdu_and_seq_lists(cmd, &bl);
 }
 
 struct iscsi_pdu *iscsit_get_pdu_holder(
@@ -572,13 +611,12 @@ redo:
 		pdu = &cmd->pdu_list[cmd->pdu_start];
 
 		for (i = 0; pdu[i].seq_no != cmd->seq_no; i++) {
-#if 0
 			pr_debug("pdu[i].seq_no: %d, pdu[i].pdu"
 				"_send_order: %d, pdu[i].offset: %d,"
 				" pdu[i].length: %d\n", pdu[i].seq_no,
 				pdu[i].pdu_send_order, pdu[i].offset,
 				pdu[i].length);
-#endif
+
 			if (pdu[i].pdu_send_order == cmd->pdu_send_order) {
 				cmd->pdu_send_order++;
 				return &pdu[i];
@@ -601,11 +639,11 @@ redo:
 			pr_err("struct iscsi_seq is NULL!\n");
 			return NULL;
 		}
-#if 0
+
 		pr_debug("seq->pdu_start: %d, seq->pdu_count: %d,"
 			" seq->seq_no: %d\n", seq->pdu_start, seq->pdu_count,
 			seq->seq_no);
-#endif
+
 		pdu = &cmd->pdu_list[seq->pdu_start];
 
 		if (seq->pdu_send_order == seq->pdu_count) {
@@ -645,12 +683,11 @@ struct iscsi_seq *iscsit_get_seq_holder(
 	}
 
 	for (i = 0; i < cmd->seq_count; i++) {
-#if 0
 		pr_debug("seq_list[i].orig_offset: %d, seq_list[i]."
 			"xfer_len: %d, seq_list[i].seq_no %u\n",
 			cmd->seq_list[i].orig_offset, cmd->seq_list[i].xfer_len,
 			cmd->seq_list[i].seq_no);
-#endif
+
 		if ((cmd->seq_list[i].orig_offset +
 				cmd->seq_list[i].xfer_len) >=
 				(offset + length))
diff --git a/drivers/target/iscsi/iscsi_target_seq_pdu_list.h b/drivers/target/iscsi/iscsi_target_seq_pdu_list.h
index 0d52a10e306..d5b153751a8 100644
--- a/drivers/target/iscsi/iscsi_target_seq_pdu_list.h
+++ b/drivers/target/iscsi/iscsi_target_seq_pdu_list.h
@@ -78,7 +78,7 @@ struct iscsi_seq {
 	u32		xfer_len;
 } ____cacheline_aligned;
 
-extern int iscsit_do_build_list(struct iscsi_cmd *, struct iscsi_build_list *);
+extern int iscsit_build_pdu_and_seq_lists(struct iscsi_cmd *, u32);
 extern struct iscsi_pdu *iscsit_get_pdu_holder(struct iscsi_cmd *, u32, u32);
 extern struct iscsi_pdu *iscsit_get_pdu_holder_for_seq(struct iscsi_cmd *, struct iscsi_seq *);
 extern struct iscsi_seq *iscsit_get_seq_holder(struct iscsi_cmd *, u32, u32);
diff --git a/drivers/target/iscsi/iscsi_target_tmr.c b/drivers/target/iscsi/iscsi_target_tmr.c
index e01da9d2b37..f4e640b51fd 100644
--- a/drivers/target/iscsi/iscsi_target_tmr.c
+++ b/drivers/target/iscsi/iscsi_target_tmr.c
@@ -78,10 +78,7 @@ int iscsit_tmr_task_warm_reset(
 {
 	struct iscsi_session *sess = conn->sess;
 	struct iscsi_node_attrib *na = iscsit_tpg_get_node_attrib(sess);
-#if 0
-	struct iscsi_init_task_mgt_cmnd *hdr =
-		(struct iscsi_init_task_mgt_cmnd *) buf;
-#endif
+
 	if (!na->tmr_warm_reset) {
 		pr_err("TMR Opcode TARGET_WARM_RESET authorization"
 			" failed for Initiator Node: %s\n",
@@ -216,7 +213,7 @@ static int iscsit_task_reassign_complete_nop_out(
 	iscsit_task_reassign_remove_cmd(cmd, cr, conn->sess);
 
 	spin_lock_bh(&conn->cmd_lock);
-	list_add_tail(&cmd->i_list, &conn->conn_cmd_list);
+	list_add_tail(&cmd->i_conn_node, &conn->conn_cmd_list);
 	spin_unlock_bh(&conn->cmd_lock);
 
 	cmd->i_state = ISTATE_SEND_NOPIN;
@@ -272,9 +269,9 @@ static int iscsit_task_reassign_complete_write(
 		offset = cmd->next_burst_len = cmd->write_data_done;
 
 		if ((conn->sess->sess_ops->FirstBurstLength - offset) >=
-		     cmd->data_length) {
+		     cmd->se_cmd.data_length) {
 			no_build_r2ts = 1;
-			length = (cmd->data_length - offset);
+			length = (cmd->se_cmd.data_length - offset);
 		} else
 			length = (conn->sess->sess_ops->FirstBurstLength - offset);
 
@@ -292,7 +289,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, 2);
+	return iscsit_build_r2ts_for_cmd(cmd, conn, true);
 }
 
 static int iscsit_task_reassign_complete_read(
@@ -385,7 +382,7 @@ static int iscsit_task_reassign_complete_scsi_cmnd(
 	iscsit_task_reassign_remove_cmd(cmd, cr, conn->sess);
 
 	spin_lock_bh(&conn->cmd_lock);
-	list_add_tail(&cmd->i_list, &conn->conn_cmd_list);
+	list_add_tail(&cmd->i_conn_node, &conn->conn_cmd_list);
 	spin_unlock_bh(&conn->cmd_lock);
 
 	if (se_cmd->se_cmd_flags & SCF_SENT_CHECK_CONDITION) {
diff --git a/drivers/target/iscsi/iscsi_target_util.c b/drivers/target/iscsi/iscsi_target_util.c
index 4eba86d2bd8..b42cdeb153d 100644
--- a/drivers/target/iscsi/iscsi_target_util.c
+++ b/drivers/target/iscsi/iscsi_target_util.c
@@ -163,7 +163,7 @@ struct iscsi_cmd *iscsit_allocate_cmd(struct iscsi_conn *conn, gfp_t gfp_mask)
 	}
 
 	cmd->conn	= conn;
-	INIT_LIST_HEAD(&cmd->i_list);
+	INIT_LIST_HEAD(&cmd->i_conn_node);
 	INIT_LIST_HEAD(&cmd->datain_list);
 	INIT_LIST_HEAD(&cmd->cmd_r2t_list);
 	init_completion(&cmd->reject_comp);
@@ -176,174 +176,6 @@ struct iscsi_cmd *iscsit_allocate_cmd(struct iscsi_conn *conn, gfp_t gfp_mask)
 	return cmd;
 }
 
-/*
- * Called from iscsi_handle_scsi_cmd()
- */
-struct iscsi_cmd *iscsit_allocate_se_cmd(
-	struct iscsi_conn *conn,
-	u32 data_length,
-	int data_direction,
-	int iscsi_task_attr)
-{
-	struct iscsi_cmd *cmd;
-	struct se_cmd *se_cmd;
-	int sam_task_attr;
-
-	cmd = iscsit_allocate_cmd(conn, GFP_KERNEL);
-	if (!cmd)
-		return NULL;
-
-	cmd->data_direction = data_direction;
-	cmd->data_length = data_length;
-	/*
-	 * Figure out the SAM Task Attribute for the incoming SCSI CDB
-	 */
-	if ((iscsi_task_attr == ISCSI_ATTR_UNTAGGED) ||
-	    (iscsi_task_attr == ISCSI_ATTR_SIMPLE))
-		sam_task_attr = MSG_SIMPLE_TAG;
-	else if (iscsi_task_attr == ISCSI_ATTR_ORDERED)
-		sam_task_attr = MSG_ORDERED_TAG;
-	else if (iscsi_task_attr == ISCSI_ATTR_HEAD_OF_QUEUE)
-		sam_task_attr = MSG_HEAD_TAG;
-	else if (iscsi_task_attr == ISCSI_ATTR_ACA)
-		sam_task_attr = MSG_ACA_TAG;
-	else {
-		pr_debug("Unknown iSCSI Task Attribute: 0x%02x, using"
-			" MSG_SIMPLE_TAG\n", iscsi_task_attr);
-		sam_task_attr = MSG_SIMPLE_TAG;
-	}
-
-	se_cmd = &cmd->se_cmd;
-	/*
-	 * Initialize struct se_cmd descriptor from target_core_mod infrastructure
-	 */
-	transport_init_se_cmd(se_cmd, &lio_target_fabric_configfs->tf_ops,
-			conn->sess->se_sess, data_length, data_direction,
-			sam_task_attr, &cmd->sense_buffer[0]);
-	return cmd;
-}
-
-struct iscsi_cmd *iscsit_allocate_se_cmd_for_tmr(
-	struct iscsi_conn *conn,
-	u8 function)
-{
-	struct iscsi_cmd *cmd;
-	struct se_cmd *se_cmd;
-	int rc;
-	u8 tcm_function;
-
-	cmd = iscsit_allocate_cmd(conn, GFP_KERNEL);
-	if (!cmd)
-		return NULL;
-
-	cmd->data_direction = DMA_NONE;
-
-	cmd->tmr_req = kzalloc(sizeof(struct iscsi_tmr_req), GFP_KERNEL);
-	if (!cmd->tmr_req) {
-		pr_err("Unable to allocate memory for"
-			" Task Management command!\n");
-		goto out;
-	}
-	/*
-	 * TASK_REASSIGN for ERL=2 / connection stays inside of
-	 * LIO-Target $FABRIC_MOD
-	 */
-	if (function == ISCSI_TM_FUNC_TASK_REASSIGN)
-		return cmd;
-
-	se_cmd = &cmd->se_cmd;
-	/*
-	 * Initialize struct se_cmd descriptor from target_core_mod infrastructure
-	 */
-	transport_init_se_cmd(se_cmd, &lio_target_fabric_configfs->tf_ops,
-				conn->sess->se_sess, 0, DMA_NONE,
-				MSG_SIMPLE_TAG, &cmd->sense_buffer[0]);
-
-	switch (function) {
-	case ISCSI_TM_FUNC_ABORT_TASK:
-		tcm_function = TMR_ABORT_TASK;
-		break;
-	case ISCSI_TM_FUNC_ABORT_TASK_SET:
-		tcm_function = TMR_ABORT_TASK_SET;
-		break;
-	case ISCSI_TM_FUNC_CLEAR_ACA:
-		tcm_function = TMR_CLEAR_ACA;
-		break;
-	case ISCSI_TM_FUNC_CLEAR_TASK_SET:
-		tcm_function = TMR_CLEAR_TASK_SET;
-		break;
-	case ISCSI_TM_FUNC_LOGICAL_UNIT_RESET:
-		tcm_function = TMR_LUN_RESET;
-		break;
-	case ISCSI_TM_FUNC_TARGET_WARM_RESET:
-		tcm_function = TMR_TARGET_WARM_RESET;
-		break;
-	case ISCSI_TM_FUNC_TARGET_COLD_RESET:
-		tcm_function = TMR_TARGET_COLD_RESET;
-		break;
-	default:
-		pr_err("Unknown iSCSI TMR Function:"
-			" 0x%02x\n", function);
-		goto out;
-	}
-
-	rc = core_tmr_alloc_req(se_cmd, cmd->tmr_req, tcm_function, GFP_KERNEL);
-	if (rc < 0)
-		goto out;
-
-	cmd->tmr_req->se_tmr_req = se_cmd->se_tmr_req;
-
-	return cmd;
-out:
-	iscsit_release_cmd(cmd);
-	return NULL;
-}
-
-int iscsit_decide_list_to_build(
-	struct iscsi_cmd *cmd,
-	u32 immediate_data_length)
-{
-	struct iscsi_build_list bl;
-	struct iscsi_conn *conn = cmd->conn;
-	struct iscsi_session *sess = conn->sess;
-	struct iscsi_node_attrib *na;
-
-	if (sess->sess_ops->DataSequenceInOrder &&
-	    sess->sess_ops->DataPDUInOrder)
-		return 0;
-
-	if (cmd->data_direction == DMA_NONE)
-		return 0;
-
-	na = iscsit_tpg_get_node_attrib(sess);
-	memset(&bl, 0, sizeof(struct iscsi_build_list));
-
-	if (cmd->data_direction == DMA_FROM_DEVICE) {
-		bl.data_direction = ISCSI_PDU_READ;
-		bl.type = PDULIST_NORMAL;
-		if (na->random_datain_pdu_offsets)
-			bl.randomize |= RANDOM_DATAIN_PDU_OFFSETS;
-		if (na->random_datain_seq_offsets)
-			bl.randomize |= RANDOM_DATAIN_SEQ_OFFSETS;
-	} else {
-		bl.data_direction = ISCSI_PDU_WRITE;
-		bl.immediate_data_length = immediate_data_length;
-		if (na->random_r2t_offsets)
-			bl.randomize |= RANDOM_R2T_OFFSETS;
-
-		if (!cmd->immediate_data && !cmd->unsolicited_data)
-			bl.type = PDULIST_NORMAL;
-		else if (cmd->immediate_data && !cmd->unsolicited_data)
-			bl.type = PDULIST_IMMEDIATE;
-		else if (!cmd->immediate_data && cmd->unsolicited_data)
-			bl.type = PDULIST_UNSOLICITED;
-		else if (cmd->immediate_data && cmd->unsolicited_data)
-			bl.type = PDULIST_IMMEDIATE_AND_UNSOLICITED;
-	}
-
-	return iscsit_do_build_list(cmd, &bl);
-}
-
 struct iscsi_seq *iscsit_get_seq_holder_for_datain(
 	struct iscsi_cmd *cmd,
 	u32 seq_send_order)
@@ -502,14 +334,14 @@ int iscsit_check_unsolicited_dataout(struct iscsi_cmd *cmd, unsigned char *buf)
 	if (!(hdr->flags & ISCSI_FLAG_CMD_FINAL))
 		return 0;
 
-	if (((cmd->first_burst_len + payload_length) != cmd->data_length) &&
+	if (((cmd->first_burst_len + payload_length) != cmd->se_cmd.data_length) &&
 	    ((cmd->first_burst_len + payload_length) !=
 	      conn->sess->sess_ops->FirstBurstLength)) {
 		pr_err("Unsolicited non-immediate data received %u"
 			" does not equal FirstBurstLength: %u, and does"
 			" not equal ExpXferLen %u.\n",
 			(cmd->first_burst_len + payload_length),
-			conn->sess->sess_ops->FirstBurstLength, cmd->data_length);
+			conn->sess->sess_ops->FirstBurstLength, cmd->se_cmd.data_length);
 		transport_send_check_condition_and_sense(se_cmd,
 				TCM_INCORRECT_AMOUNT_OF_DATA, 0);
 		return -1;
@@ -524,7 +356,7 @@ struct iscsi_cmd *iscsit_find_cmd_from_itt(
 	struct iscsi_cmd *cmd;
 
 	spin_lock_bh(&conn->cmd_lock);
-	list_for_each_entry(cmd, &conn->conn_cmd_list, i_list) {
+	list_for_each_entry(cmd, &conn->conn_cmd_list, i_conn_node) {
 		if (cmd->init_task_tag == init_task_tag) {
 			spin_unlock_bh(&conn->cmd_lock);
 			return cmd;
@@ -545,7 +377,7 @@ struct iscsi_cmd *iscsit_find_cmd_from_itt_or_dump(
 	struct iscsi_cmd *cmd;
 
 	spin_lock_bh(&conn->cmd_lock);
-	list_for_each_entry(cmd, &conn->conn_cmd_list, i_list) {
+	list_for_each_entry(cmd, &conn->conn_cmd_list, i_conn_node) {
 		if (cmd->init_task_tag == init_task_tag) {
 			spin_unlock_bh(&conn->cmd_lock);
 			return cmd;
@@ -568,7 +400,7 @@ struct iscsi_cmd *iscsit_find_cmd_from_ttt(
 	struct iscsi_cmd *cmd = NULL;
 
 	spin_lock_bh(&conn->cmd_lock);
-	list_for_each_entry(cmd, &conn->conn_cmd_list, i_list) {
+	list_for_each_entry(cmd, &conn->conn_cmd_list, i_conn_node) {
 		if (cmd->targ_xfer_tag == targ_xfer_tag) {
 			spin_unlock_bh(&conn->cmd_lock);
 			return cmd;
@@ -596,7 +428,7 @@ int iscsit_find_cmd_for_recovery(
 	spin_lock(&sess->cr_i_lock);
 	list_for_each_entry(cr, &sess->cr_inactive_list, cr_list) {
 		spin_lock(&cr->conn_recovery_cmd_lock);
-		list_for_each_entry(cmd, &cr->conn_recovery_cmd_list, i_list) {
+		list_for_each_entry(cmd, &cr->conn_recovery_cmd_list, i_conn_node) {
 			if (cmd->init_task_tag == init_task_tag) {
 				spin_unlock(&cr->conn_recovery_cmd_lock);
 				spin_unlock(&sess->cr_i_lock);
@@ -616,7 +448,7 @@ int iscsit_find_cmd_for_recovery(
 	spin_lock(&sess->cr_a_lock);
 	list_for_each_entry(cr, &sess->cr_active_list, cr_list) {
 		spin_lock(&cr->conn_recovery_cmd_lock);
-		list_for_each_entry(cmd, &cr->conn_recovery_cmd_list, i_list) {
+		list_for_each_entry(cmd, &cr->conn_recovery_cmd_list, i_conn_node) {
 			if (cmd->init_task_tag == init_task_tag) {
 				spin_unlock(&cr->conn_recovery_cmd_lock);
 				spin_unlock(&sess->cr_a_lock);
@@ -813,7 +645,6 @@ void iscsit_free_queue_reqs_for_conn(struct iscsi_conn *conn)
 void iscsit_release_cmd(struct iscsi_cmd *cmd)
 {
 	struct iscsi_conn *conn = cmd->conn;
-	int i;
 
 	iscsit_free_r2ts_from_list(cmd);
 	iscsit_free_all_datain_reqs(cmd);
@@ -824,11 +655,6 @@ void iscsit_release_cmd(struct iscsi_cmd *cmd)
 	kfree(cmd->tmr_req);
 	kfree(cmd->iov_data);
 
-	for (i = 0; i < cmd->t_mem_sg_nents; i++)
-		__free_page(sg_page(&cmd->t_mem_sg[i]));
-
-	kfree(cmd->t_mem_sg);
-
 	if (conn) {
 		iscsit_remove_cmd_from_immediate_queue(cmd, conn);
 		iscsit_remove_cmd_from_response_queue(cmd, conn);
@@ -1038,7 +864,7 @@ static int iscsit_add_nopin(struct iscsi_conn *conn, int want_response)
 	spin_unlock_bh(&conn->sess->ttt_lock);
 
 	spin_lock_bh(&conn->cmd_lock);
-	list_add_tail(&cmd->i_list, &conn->conn_cmd_list);
+	list_add_tail(&cmd->i_conn_node, &conn->conn_cmd_list);
 	spin_unlock_bh(&conn->cmd_lock);
 
 	if (want_response)
diff --git a/drivers/target/iscsi/iscsi_target_util.h b/drivers/target/iscsi/iscsi_target_util.h
index 835bf7de028..e1c729b8a1c 100644
--- a/drivers/target/iscsi/iscsi_target_util.h
+++ b/drivers/target/iscsi/iscsi_target_util.h
@@ -9,9 +9,6 @@ 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_allocate_cmd(struct iscsi_conn *, gfp_t);
-extern struct iscsi_cmd *iscsit_allocate_se_cmd(struct iscsi_conn *, u32, int, int);
-extern struct iscsi_cmd *iscsit_allocate_se_cmd_for_tmr(struct iscsi_conn *, u8);
-extern int iscsit_decide_list_to_build(struct iscsi_cmd *, u32);
 extern struct iscsi_seq *iscsit_get_seq_holder_for_datain(struct iscsi_cmd *, u32);
 extern struct iscsi_seq *iscsit_get_seq_holder_for_r2t(struct iscsi_cmd *);
 extern struct iscsi_r2t *iscsit_get_holder_for_r2tsn(struct iscsi_cmd *, u32);
diff --git a/drivers/target/loopback/tcm_loop.c b/drivers/target/loopback/tcm_loop.c
index a9b4eeefe9f..38dfac2b0a1 100644
--- a/drivers/target/loopback/tcm_loop.c
+++ b/drivers/target/loopback/tcm_loop.c
@@ -213,7 +213,7 @@ static void tcm_loop_submission_work(struct work_struct *work)
 	 * associated read buffers, go ahead and do that here for type
 	 * SCF_SCSI_CONTROL_SG_IO_CDB.  Also note that this is currently
 	 * guaranteed to be a single SGL for SCF_SCSI_CONTROL_SG_IO_CDB
-	 * by target core in transport_generic_allocate_tasks() ->
+	 * by target core in target_setup_cmd_from_cdb() ->
 	 * transport_generic_cmd_sequencer().
 	 */
 	if (se_cmd->se_cmd_flags & SCF_SCSI_CONTROL_SG_IO_CDB &&
@@ -227,7 +227,7 @@ static void tcm_loop_submission_work(struct work_struct *work)
 		}
 	}
 
-	ret = transport_generic_allocate_tasks(se_cmd, sc->cmnd);
+	ret = target_setup_cmd_from_cdb(se_cmd, sc->cmnd);
 	if (ret == -ENOMEM) {
 		transport_send_check_condition_and_sense(se_cmd,
 				TCM_LOGICAL_UNIT_COMMUNICATION_FAILURE, 0);
diff --git a/drivers/target/sbp/Kconfig b/drivers/target/sbp/Kconfig
new file mode 100644
index 00000000000..132da544eaf
--- /dev/null
+++ b/drivers/target/sbp/Kconfig
@@ -0,0 +1,11 @@
+config SBP_TARGET
+	tristate "FireWire SBP-2 fabric module"
+	depends on FIREWIRE && EXPERIMENTAL
+	help
+	  Say Y or M here to enable SCSI target functionality over FireWire.
+	  This enables you to expose SCSI devices to other nodes on the FireWire
+	  bus, for example hard disks. Similar to FireWire Target Disk mode on
+	  many Apple computers.
+
+	  To compile this driver as a module, say M here: The module will be
+	  called sbp-target.
diff --git a/drivers/target/sbp/Makefile b/drivers/target/sbp/Makefile
new file mode 100644
index 00000000000..27747ad054c
--- /dev/null
+++ b/drivers/target/sbp/Makefile
@@ -0,0 +1 @@
+obj-$(CONFIG_SBP_TARGET) += sbp_target.o
diff --git a/drivers/target/sbp/sbp_target.c b/drivers/target/sbp/sbp_target.c
new file mode 100644
index 00000000000..37c609898f8
--- /dev/null
+++ b/drivers/target/sbp/sbp_target.c
@@ -0,0 +1,2621 @@
+/*
+ * SBP2 target driver (SCSI over IEEE1394 in target mode)
+ *
+ * Copyright (C) 2011  Chris Boot <bootc@bootc.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.
+ */
+
+#define KMSG_COMPONENT "sbp_target"
+#define pr_fmt(fmt) KMSG_COMPONENT ": " fmt
+
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/init.h>
+#include <linux/types.h>
+#include <linux/string.h>
+#include <linux/configfs.h>
+#include <linux/ctype.h>
+#include <linux/firewire.h>
+#include <linux/firewire-constants.h>
+#include <scsi/scsi.h>
+#include <scsi/scsi_tcq.h>
+#include <target/target_core_base.h>
+#include <target/target_core_backend.h>
+#include <target/target_core_fabric.h>
+#include <target/target_core_fabric_configfs.h>
+#include <target/target_core_configfs.h>
+#include <target/configfs_macros.h>
+#include <asm/unaligned.h>
+
+#include "sbp_target.h"
+
+/* Local pointer to allocated TCM configfs fabric module */
+static struct target_fabric_configfs *sbp_fabric_configfs;
+
+/* FireWire address region for management and command block address handlers */
+static const struct fw_address_region sbp_register_region = {
+	.start	= CSR_REGISTER_BASE + 0x10000,
+	.end	= 0x1000000000000ULL,
+};
+
+static const u32 sbp_unit_directory_template[] = {
+	0x1200609e, /* unit_specifier_id: NCITS/T10 */
+	0x13010483, /* unit_sw_version: 1155D Rev 4 */
+	0x3800609e, /* command_set_specifier_id: NCITS/T10 */
+	0x390104d8, /* command_set: SPC-2 */
+	0x3b000000, /* command_set_revision: 0 */
+	0x3c000001, /* firmware_revision: 1 */
+};
+
+#define SESSION_MAINTENANCE_INTERVAL HZ
+
+static atomic_t login_id = ATOMIC_INIT(0);
+
+static void session_maintenance_work(struct work_struct *);
+static int sbp_run_transaction(struct fw_card *, int, int, int, int,
+		unsigned long long, void *, size_t);
+
+static int read_peer_guid(u64 *guid, const struct sbp_management_request *req)
+{
+	int ret;
+	__be32 high, low;
+
+	ret = sbp_run_transaction(req->card, TCODE_READ_QUADLET_REQUEST,
+			req->node_addr, req->generation, req->speed,
+			(CSR_REGISTER_BASE | CSR_CONFIG_ROM) + 3 * 4,
+			&high, sizeof(high));
+	if (ret != RCODE_COMPLETE)
+		return ret;
+
+	ret = sbp_run_transaction(req->card, TCODE_READ_QUADLET_REQUEST,
+			req->node_addr, req->generation, req->speed,
+			(CSR_REGISTER_BASE | CSR_CONFIG_ROM) + 4 * 4,
+			&low, sizeof(low));
+	if (ret != RCODE_COMPLETE)
+		return ret;
+
+	*guid = (u64)be32_to_cpu(high) << 32 | be32_to_cpu(low);
+
+	return RCODE_COMPLETE;
+}
+
+static struct sbp_session *sbp_session_find_by_guid(
+	struct sbp_tpg *tpg, u64 guid)
+{
+	struct se_session *se_sess;
+	struct sbp_session *sess, *found = NULL;
+
+	spin_lock_bh(&tpg->se_tpg.session_lock);
+	list_for_each_entry(se_sess, &tpg->se_tpg.tpg_sess_list, sess_list) {
+		sess = se_sess->fabric_sess_ptr;
+		if (sess->guid == guid)
+			found = sess;
+	}
+	spin_unlock_bh(&tpg->se_tpg.session_lock);
+
+	return found;
+}
+
+static struct sbp_login_descriptor *sbp_login_find_by_lun(
+		struct sbp_session *session, struct se_lun *lun)
+{
+	struct sbp_login_descriptor *login, *found = NULL;
+
+	spin_lock_bh(&session->lock);
+	list_for_each_entry(login, &session->login_list, link) {
+		if (login->lun == lun)
+			found = login;
+	}
+	spin_unlock_bh(&session->lock);
+
+	return found;
+}
+
+static int sbp_login_count_all_by_lun(
+		struct sbp_tpg *tpg,
+		struct se_lun *lun,
+		int exclusive)
+{
+	struct se_session *se_sess;
+	struct sbp_session *sess;
+	struct sbp_login_descriptor *login;
+	int count = 0;
+
+	spin_lock_bh(&tpg->se_tpg.session_lock);
+	list_for_each_entry(se_sess, &tpg->se_tpg.tpg_sess_list, sess_list) {
+		sess = se_sess->fabric_sess_ptr;
+
+		spin_lock_bh(&sess->lock);
+		list_for_each_entry(login, &sess->login_list, link) {
+			if (login->lun != lun)
+				continue;
+
+			if (!exclusive || login->exclusive)
+				count++;
+		}
+		spin_unlock_bh(&sess->lock);
+	}
+	spin_unlock_bh(&tpg->se_tpg.session_lock);
+
+	return count;
+}
+
+static struct sbp_login_descriptor *sbp_login_find_by_id(
+	struct sbp_tpg *tpg, int login_id)
+{
+	struct se_session *se_sess;
+	struct sbp_session *sess;
+	struct sbp_login_descriptor *login, *found = NULL;
+
+	spin_lock_bh(&tpg->se_tpg.session_lock);
+	list_for_each_entry(se_sess, &tpg->se_tpg.tpg_sess_list, sess_list) {
+		sess = se_sess->fabric_sess_ptr;
+
+		spin_lock_bh(&sess->lock);
+		list_for_each_entry(login, &sess->login_list, link) {
+			if (login->login_id == login_id)
+				found = login;
+		}
+		spin_unlock_bh(&sess->lock);
+	}
+	spin_unlock_bh(&tpg->se_tpg.session_lock);
+
+	return found;
+}
+
+static struct se_lun *sbp_get_lun_from_tpg(struct sbp_tpg *tpg, int lun)
+{
+	struct se_portal_group *se_tpg = &tpg->se_tpg;
+	struct se_lun *se_lun;
+
+	if (lun >= TRANSPORT_MAX_LUNS_PER_TPG)
+		return ERR_PTR(-EINVAL);
+
+	spin_lock(&se_tpg->tpg_lun_lock);
+	se_lun = se_tpg->tpg_lun_list[lun];
+
+	if (se_lun->lun_status != TRANSPORT_LUN_STATUS_ACTIVE)
+		se_lun = ERR_PTR(-ENODEV);
+
+	spin_unlock(&se_tpg->tpg_lun_lock);
+
+	return se_lun;
+}
+
+static struct sbp_session *sbp_session_create(
+		struct sbp_tpg *tpg,
+		u64 guid)
+{
+	struct sbp_session *sess;
+	int ret;
+	char guid_str[17];
+	struct se_node_acl *se_nacl;
+
+	sess = kmalloc(sizeof(*sess), GFP_KERNEL);
+	if (!sess) {
+		pr_err("failed to allocate session descriptor\n");
+		return ERR_PTR(-ENOMEM);
+	}
+
+	sess->se_sess = transport_init_session();
+	if (IS_ERR(sess->se_sess)) {
+		pr_err("failed to init se_session\n");
+
+		ret = PTR_ERR(sess->se_sess);
+		kfree(sess);
+		return ERR_PTR(ret);
+	}
+
+	snprintf(guid_str, sizeof(guid_str), "%016llx", guid);
+
+	se_nacl = core_tpg_check_initiator_node_acl(&tpg->se_tpg, guid_str);
+	if (!se_nacl) {
+		pr_warn("Node ACL not found for %s\n", guid_str);
+
+		transport_free_session(sess->se_sess);
+		kfree(sess);
+
+		return ERR_PTR(-EPERM);
+	}
+
+	sess->se_sess->se_node_acl = se_nacl;
+
+	spin_lock_init(&sess->lock);
+	INIT_LIST_HEAD(&sess->login_list);
+	INIT_DELAYED_WORK(&sess->maint_work, session_maintenance_work);
+
+	sess->guid = guid;
+
+	transport_register_session(&tpg->se_tpg, se_nacl, sess->se_sess, sess);
+
+	return sess;
+}
+
+static void sbp_session_release(struct sbp_session *sess, bool cancel_work)
+{
+	spin_lock_bh(&sess->lock);
+	if (!list_empty(&sess->login_list)) {
+		spin_unlock_bh(&sess->lock);
+		return;
+	}
+	spin_unlock_bh(&sess->lock);
+
+	if (cancel_work)
+		cancel_delayed_work_sync(&sess->maint_work);
+
+	transport_deregister_session_configfs(sess->se_sess);
+	transport_deregister_session(sess->se_sess);
+
+	if (sess->card)
+		fw_card_put(sess->card);
+
+	kfree(sess);
+}
+
+static void sbp_target_agent_unregister(struct sbp_target_agent *);
+
+static void sbp_login_release(struct sbp_login_descriptor *login,
+	bool cancel_work)
+{
+	struct sbp_session *sess = login->sess;
+
+	/* FIXME: abort/wait on tasks */
+
+	sbp_target_agent_unregister(login->tgt_agt);
+
+	if (sess) {
+		spin_lock_bh(&sess->lock);
+		list_del(&login->link);
+		spin_unlock_bh(&sess->lock);
+
+		sbp_session_release(sess, cancel_work);
+	}
+
+	kfree(login);
+}
+
+static struct sbp_target_agent *sbp_target_agent_register(
+	struct sbp_login_descriptor *);
+
+static void sbp_management_request_login(
+	struct sbp_management_agent *agent, struct sbp_management_request *req,
+	int *status_data_size)
+{
+	struct sbp_tport *tport = agent->tport;
+	struct sbp_tpg *tpg = tport->tpg;
+	struct se_lun *se_lun;
+	int ret;
+	u64 guid;
+	struct sbp_session *sess;
+	struct sbp_login_descriptor *login;
+	struct sbp_login_response_block *response;
+	int login_response_len;
+
+	se_lun = sbp_get_lun_from_tpg(tpg,
+			LOGIN_ORB_LUN(be32_to_cpu(req->orb.misc)));
+	if (IS_ERR(se_lun)) {
+		pr_notice("login to unknown LUN: %d\n",
+			LOGIN_ORB_LUN(be32_to_cpu(req->orb.misc)));
+
+		req->status.status = cpu_to_be32(
+			STATUS_BLOCK_RESP(STATUS_RESP_REQUEST_COMPLETE) |
+			STATUS_BLOCK_SBP_STATUS(SBP_STATUS_LUN_NOTSUPP));
+		return;
+	}
+
+	ret = read_peer_guid(&guid, req);
+	if (ret != RCODE_COMPLETE) {
+		pr_warn("failed to read peer GUID: %d\n", ret);
+
+		req->status.status = cpu_to_be32(
+			STATUS_BLOCK_RESP(STATUS_RESP_TRANSPORT_FAILURE) |
+			STATUS_BLOCK_SBP_STATUS(SBP_STATUS_UNSPECIFIED_ERROR));
+		return;
+	}
+
+	pr_notice("mgt_agent LOGIN to LUN %d from %016llx\n",
+		se_lun->unpacked_lun, guid);
+
+	sess = sbp_session_find_by_guid(tpg, guid);
+	if (sess) {
+		login = sbp_login_find_by_lun(sess, se_lun);
+		if (login) {
+			pr_notice("initiator already logged-in\n");
+
+			/*
+			 * SBP-2 R4 says we should return access denied, but
+			 * that can confuse initiators. Instead we need to
+			 * treat this like a reconnect, but send the login
+			 * response block like a fresh login.
+			 *
+			 * This is required particularly in the case of Apple
+			 * devices booting off the FireWire target, where
+			 * the firmware has an active login to the target. When
+			 * the OS takes control of the session it issues its own
+			 * LOGIN rather than a RECONNECT. To avoid the machine
+			 * waiting until the reconnect_hold expires, we can skip
+			 * the ACCESS_DENIED errors to speed things up.
+			 */
+
+			goto already_logged_in;
+		}
+	}
+
+	/*
+	 * check exclusive bit in login request
+	 * reject with access_denied if any logins present
+	 */
+	if (LOGIN_ORB_EXCLUSIVE(be32_to_cpu(req->orb.misc)) &&
+			sbp_login_count_all_by_lun(tpg, se_lun, 0)) {
+		pr_warn("refusing exclusive login with other active logins\n");
+
+		req->status.status = cpu_to_be32(
+			STATUS_BLOCK_RESP(STATUS_RESP_REQUEST_COMPLETE) |
+			STATUS_BLOCK_SBP_STATUS(SBP_STATUS_ACCESS_DENIED));
+		return;
+	}
+
+	/*
+	 * check exclusive bit in any existing login descriptor
+	 * reject with access_denied if any exclusive logins present
+	 */
+	if (sbp_login_count_all_by_lun(tpg, se_lun, 1)) {
+		pr_warn("refusing login while another exclusive login present\n");
+
+		req->status.status = cpu_to_be32(
+			STATUS_BLOCK_RESP(STATUS_RESP_REQUEST_COMPLETE) |
+			STATUS_BLOCK_SBP_STATUS(SBP_STATUS_ACCESS_DENIED));
+		return;
+	}
+
+	/*
+	 * check we haven't exceeded the number of allowed logins
+	 * reject with resources_unavailable if we have
+	 */
+	if (sbp_login_count_all_by_lun(tpg, se_lun, 0) >=
+			tport->max_logins_per_lun) {
+		pr_warn("max number of logins reached\n");
+
+		req->status.status = cpu_to_be32(
+			STATUS_BLOCK_RESP(STATUS_RESP_REQUEST_COMPLETE) |
+			STATUS_BLOCK_SBP_STATUS(SBP_STATUS_RESOURCES_UNAVAIL));
+		return;
+	}
+
+	if (!sess) {
+		sess = sbp_session_create(tpg, guid);
+		if (IS_ERR(sess)) {
+			switch (PTR_ERR(sess)) {
+			case -EPERM:
+				ret = SBP_STATUS_ACCESS_DENIED;
+				break;
+			default:
+				ret = SBP_STATUS_RESOURCES_UNAVAIL;
+				break;
+			}
+
+			req->status.status = cpu_to_be32(
+				STATUS_BLOCK_RESP(
+					STATUS_RESP_REQUEST_COMPLETE) |
+				STATUS_BLOCK_SBP_STATUS(ret));
+			return;
+		}
+
+		sess->node_id = req->node_addr;
+		sess->card = fw_card_get(req->card);
+		sess->generation = req->generation;
+		sess->speed = req->speed;
+
+		schedule_delayed_work(&sess->maint_work,
+				SESSION_MAINTENANCE_INTERVAL);
+	}
+
+	/* only take the latest reconnect_hold into account */
+	sess->reconnect_hold = min(
+		1 << LOGIN_ORB_RECONNECT(be32_to_cpu(req->orb.misc)),
+		tport->max_reconnect_timeout) - 1;
+
+	login = kmalloc(sizeof(*login), GFP_KERNEL);
+	if (!login) {
+		pr_err("failed to allocate login descriptor\n");
+
+		sbp_session_release(sess, true);
+
+		req->status.status = cpu_to_be32(
+			STATUS_BLOCK_RESP(STATUS_RESP_REQUEST_COMPLETE) |
+			STATUS_BLOCK_SBP_STATUS(SBP_STATUS_RESOURCES_UNAVAIL));
+		return;
+	}
+
+	login->sess = sess;
+	login->lun = se_lun;
+	login->status_fifo_addr = sbp2_pointer_to_addr(&req->orb.status_fifo);
+	login->exclusive = LOGIN_ORB_EXCLUSIVE(be32_to_cpu(req->orb.misc));
+	login->login_id = atomic_inc_return(&login_id);
+
+	login->tgt_agt = sbp_target_agent_register(login);
+	if (IS_ERR(login->tgt_agt)) {
+		ret = PTR_ERR(login->tgt_agt);
+		pr_err("failed to map command block handler: %d\n", ret);
+
+		sbp_session_release(sess, true);
+		kfree(login);
+
+		req->status.status = cpu_to_be32(
+			STATUS_BLOCK_RESP(STATUS_RESP_REQUEST_COMPLETE) |
+			STATUS_BLOCK_SBP_STATUS(SBP_STATUS_RESOURCES_UNAVAIL));
+		return;
+	}
+
+	spin_lock_bh(&sess->lock);
+	list_add_tail(&login->link, &sess->login_list);
+	spin_unlock_bh(&sess->lock);
+
+already_logged_in:
+	response = kzalloc(sizeof(*response), GFP_KERNEL);
+	if (!response) {
+		pr_err("failed to allocate login response block\n");
+
+		sbp_login_release(login, true);
+
+		req->status.status = cpu_to_be32(
+			STATUS_BLOCK_RESP(STATUS_RESP_REQUEST_COMPLETE) |
+			STATUS_BLOCK_SBP_STATUS(SBP_STATUS_RESOURCES_UNAVAIL));
+		return;
+	}
+
+	login_response_len = clamp_val(
+			LOGIN_ORB_RESPONSE_LENGTH(be32_to_cpu(req->orb.length)),
+			12, sizeof(*response));
+	response->misc = cpu_to_be32(
+		((login_response_len & 0xffff) << 16) |
+		(login->login_id & 0xffff));
+	response->reconnect_hold = cpu_to_be32(sess->reconnect_hold & 0xffff);
+	addr_to_sbp2_pointer(login->tgt_agt->handler.offset,
+		&response->command_block_agent);
+
+	ret = sbp_run_transaction(sess->card, TCODE_WRITE_BLOCK_REQUEST,
+		sess->node_id, sess->generation, sess->speed,
+		sbp2_pointer_to_addr(&req->orb.ptr2), response,
+		login_response_len);
+	if (ret != RCODE_COMPLETE) {
+		pr_debug("failed to write login response block: %x\n", ret);
+
+		kfree(response);
+		sbp_login_release(login, true);
+
+		req->status.status = cpu_to_be32(
+			STATUS_BLOCK_RESP(STATUS_RESP_TRANSPORT_FAILURE) |
+			STATUS_BLOCK_SBP_STATUS(SBP_STATUS_UNSPECIFIED_ERROR));
+		return;
+	}
+
+	kfree(response);
+
+	req->status.status = cpu_to_be32(
+		STATUS_BLOCK_RESP(STATUS_RESP_REQUEST_COMPLETE) |
+		STATUS_BLOCK_SBP_STATUS(SBP_STATUS_OK));
+}
+
+static void sbp_management_request_query_logins(
+	struct sbp_management_agent *agent, struct sbp_management_request *req,
+	int *status_data_size)
+{
+	pr_notice("QUERY LOGINS not implemented\n");
+	/* FIXME: implement */
+
+	req->status.status = cpu_to_be32(
+		STATUS_BLOCK_RESP(STATUS_RESP_REQUEST_COMPLETE) |
+		STATUS_BLOCK_SBP_STATUS(SBP_STATUS_REQ_TYPE_NOTSUPP));
+}
+
+static void sbp_management_request_reconnect(
+	struct sbp_management_agent *agent, struct sbp_management_request *req,
+	int *status_data_size)
+{
+	struct sbp_tport *tport = agent->tport;
+	struct sbp_tpg *tpg = tport->tpg;
+	int ret;
+	u64 guid;
+	struct sbp_login_descriptor *login;
+
+	ret = read_peer_guid(&guid, req);
+	if (ret != RCODE_COMPLETE) {
+		pr_warn("failed to read peer GUID: %d\n", ret);
+
+		req->status.status = cpu_to_be32(
+			STATUS_BLOCK_RESP(STATUS_RESP_TRANSPORT_FAILURE) |
+			STATUS_BLOCK_SBP_STATUS(SBP_STATUS_UNSPECIFIED_ERROR));
+		return;
+	}
+
+	pr_notice("mgt_agent RECONNECT from %016llx\n", guid);
+
+	login = sbp_login_find_by_id(tpg,
+		RECONNECT_ORB_LOGIN_ID(be32_to_cpu(req->orb.misc)));
+
+	if (!login) {
+		pr_err("mgt_agent RECONNECT unknown login ID\n");
+
+		req->status.status = cpu_to_be32(
+			STATUS_BLOCK_RESP(STATUS_RESP_REQUEST_COMPLETE) |
+			STATUS_BLOCK_SBP_STATUS(SBP_STATUS_ACCESS_DENIED));
+		return;
+	}
+
+	if (login->sess->guid != guid) {
+		pr_err("mgt_agent RECONNECT login GUID doesn't match\n");
+
+		req->status.status = cpu_to_be32(
+			STATUS_BLOCK_RESP(STATUS_RESP_REQUEST_COMPLETE) |
+			STATUS_BLOCK_SBP_STATUS(SBP_STATUS_ACCESS_DENIED));
+		return;
+	}
+
+	spin_lock_bh(&login->sess->lock);
+	if (login->sess->card)
+		fw_card_put(login->sess->card);
+
+	/* update the node details */
+	login->sess->generation = req->generation;
+	login->sess->node_id = req->node_addr;
+	login->sess->card = fw_card_get(req->card);
+	login->sess->speed = req->speed;
+	spin_unlock_bh(&login->sess->lock);
+
+	req->status.status = cpu_to_be32(
+		STATUS_BLOCK_RESP(STATUS_RESP_REQUEST_COMPLETE) |
+		STATUS_BLOCK_SBP_STATUS(SBP_STATUS_OK));
+}
+
+static void sbp_management_request_logout(
+	struct sbp_management_agent *agent, struct sbp_management_request *req,
+	int *status_data_size)
+{
+	struct sbp_tport *tport = agent->tport;
+	struct sbp_tpg *tpg = tport->tpg;
+	int login_id;
+	struct sbp_login_descriptor *login;
+
+	login_id = LOGOUT_ORB_LOGIN_ID(be32_to_cpu(req->orb.misc));
+
+	login = sbp_login_find_by_id(tpg, login_id);
+	if (!login) {
+		pr_warn("cannot find login: %d\n", login_id);
+
+		req->status.status = cpu_to_be32(
+			STATUS_BLOCK_RESP(STATUS_RESP_REQUEST_COMPLETE) |
+			STATUS_BLOCK_SBP_STATUS(SBP_STATUS_LOGIN_ID_UNKNOWN));
+		return;
+	}
+
+	pr_info("mgt_agent LOGOUT from LUN %d session %d\n",
+		login->lun->unpacked_lun, login->login_id);
+
+	if (req->node_addr != login->sess->node_id) {
+		pr_warn("logout from different node ID\n");
+
+		req->status.status = cpu_to_be32(
+			STATUS_BLOCK_RESP(STATUS_RESP_REQUEST_COMPLETE) |
+			STATUS_BLOCK_SBP_STATUS(SBP_STATUS_ACCESS_DENIED));
+		return;
+	}
+
+	sbp_login_release(login, true);
+
+	req->status.status = cpu_to_be32(
+		STATUS_BLOCK_RESP(STATUS_RESP_REQUEST_COMPLETE) |
+		STATUS_BLOCK_SBP_STATUS(SBP_STATUS_OK));
+}
+
+static void session_check_for_reset(struct sbp_session *sess)
+{
+	bool card_valid = false;
+
+	spin_lock_bh(&sess->lock);
+
+	if (sess->card) {
+		spin_lock_irq(&sess->card->lock);
+		card_valid = (sess->card->local_node != NULL);
+		spin_unlock_irq(&sess->card->lock);
+
+		if (!card_valid) {
+			fw_card_put(sess->card);
+			sess->card = NULL;
+		}
+	}
+
+	if (!card_valid || (sess->generation != sess->card->generation)) {
+		pr_info("Waiting for reconnect from node: %016llx\n",
+				sess->guid);
+
+		sess->node_id = -1;
+		sess->reconnect_expires = get_jiffies_64() +
+			((sess->reconnect_hold + 1) * HZ);
+	}
+
+	spin_unlock_bh(&sess->lock);
+}
+
+static void session_reconnect_expired(struct sbp_session *sess)
+{
+	struct sbp_login_descriptor *login, *temp;
+	LIST_HEAD(login_list);
+
+	pr_info("Reconnect timer expired for node: %016llx\n", sess->guid);
+
+	spin_lock_bh(&sess->lock);
+	list_for_each_entry_safe(login, temp, &sess->login_list, link) {
+		login->sess = NULL;
+		list_del(&login->link);
+		list_add_tail(&login->link, &login_list);
+	}
+	spin_unlock_bh(&sess->lock);
+
+	list_for_each_entry_safe(login, temp, &login_list, link) {
+		list_del(&login->link);
+		sbp_login_release(login, false);
+	}
+
+	sbp_session_release(sess, false);
+}
+
+static void session_maintenance_work(struct work_struct *work)
+{
+	struct sbp_session *sess = container_of(work, struct sbp_session,
+			maint_work.work);
+
+	/* could be called while tearing down the session */
+	spin_lock_bh(&sess->lock);
+	if (list_empty(&sess->login_list)) {
+		spin_unlock_bh(&sess->lock);
+		return;
+	}
+	spin_unlock_bh(&sess->lock);
+
+	if (sess->node_id != -1) {
+		/* check for bus reset and make node_id invalid */
+		session_check_for_reset(sess);
+
+		schedule_delayed_work(&sess->maint_work,
+				SESSION_MAINTENANCE_INTERVAL);
+	} else if (!time_after64(get_jiffies_64(), sess->reconnect_expires)) {
+		/* still waiting for reconnect */
+		schedule_delayed_work(&sess->maint_work,
+				SESSION_MAINTENANCE_INTERVAL);
+	} else {
+		/* reconnect timeout has expired */
+		session_reconnect_expired(sess);
+	}
+}
+
+static int tgt_agent_rw_agent_state(struct fw_card *card, int tcode, void *data,
+		struct sbp_target_agent *agent)
+{
+	__be32 state;
+
+	switch (tcode) {
+	case TCODE_READ_QUADLET_REQUEST:
+		pr_debug("tgt_agent AGENT_STATE READ\n");
+
+		spin_lock_bh(&agent->lock);
+		state = cpu_to_be32(agent->state);
+		spin_unlock_bh(&agent->lock);
+		memcpy(data, &state, sizeof(state));
+
+		return RCODE_COMPLETE;
+
+	case TCODE_WRITE_QUADLET_REQUEST:
+		/* ignored */
+		return RCODE_COMPLETE;
+
+	default:
+		return RCODE_TYPE_ERROR;
+	}
+}
+
+static int tgt_agent_rw_agent_reset(struct fw_card *card, int tcode, void *data,
+		struct sbp_target_agent *agent)
+{
+	switch (tcode) {
+	case TCODE_WRITE_QUADLET_REQUEST:
+		pr_debug("tgt_agent AGENT_RESET\n");
+		spin_lock_bh(&agent->lock);
+		agent->state = AGENT_STATE_RESET;
+		spin_unlock_bh(&agent->lock);
+		return RCODE_COMPLETE;
+
+	default:
+		return RCODE_TYPE_ERROR;
+	}
+}
+
+static int tgt_agent_rw_orb_pointer(struct fw_card *card, int tcode, void *data,
+		struct sbp_target_agent *agent)
+{
+	struct sbp2_pointer *ptr = data;
+
+	switch (tcode) {
+	case TCODE_WRITE_BLOCK_REQUEST:
+		spin_lock_bh(&agent->lock);
+		if (agent->state != AGENT_STATE_SUSPENDED &&
+				agent->state != AGENT_STATE_RESET) {
+			spin_unlock_bh(&agent->lock);
+			pr_notice("Ignoring ORB_POINTER write while active.\n");
+			return RCODE_CONFLICT_ERROR;
+		}
+		agent->state = AGENT_STATE_ACTIVE;
+		spin_unlock_bh(&agent->lock);
+
+		agent->orb_pointer = sbp2_pointer_to_addr(ptr);
+		agent->doorbell = false;
+
+		pr_debug("tgt_agent ORB_POINTER write: 0x%llx\n",
+				agent->orb_pointer);
+
+		queue_work(system_unbound_wq, &agent->work);
+
+		return RCODE_COMPLETE;
+
+	case TCODE_READ_BLOCK_REQUEST:
+		pr_debug("tgt_agent ORB_POINTER READ\n");
+		spin_lock_bh(&agent->lock);
+		addr_to_sbp2_pointer(agent->orb_pointer, ptr);
+		spin_unlock_bh(&agent->lock);
+		return RCODE_COMPLETE;
+
+	default:
+		return RCODE_TYPE_ERROR;
+	}
+}
+
+static int tgt_agent_rw_doorbell(struct fw_card *card, int tcode, void *data,
+		struct sbp_target_agent *agent)
+{
+	switch (tcode) {
+	case TCODE_WRITE_QUADLET_REQUEST:
+		spin_lock_bh(&agent->lock);
+		if (agent->state != AGENT_STATE_SUSPENDED) {
+			spin_unlock_bh(&agent->lock);
+			pr_debug("Ignoring DOORBELL while active.\n");
+			return RCODE_CONFLICT_ERROR;
+		}
+		agent->state = AGENT_STATE_ACTIVE;
+		spin_unlock_bh(&agent->lock);
+
+		agent->doorbell = true;
+
+		pr_debug("tgt_agent DOORBELL\n");
+
+		queue_work(system_unbound_wq, &agent->work);
+
+		return RCODE_COMPLETE;
+
+	case TCODE_READ_QUADLET_REQUEST:
+		return RCODE_COMPLETE;
+
+	default:
+		return RCODE_TYPE_ERROR;
+	}
+}
+
+static int tgt_agent_rw_unsolicited_status_enable(struct fw_card *card,
+		int tcode, void *data, struct sbp_target_agent *agent)
+{
+	switch (tcode) {
+	case TCODE_WRITE_QUADLET_REQUEST:
+		pr_debug("tgt_agent UNSOLICITED_STATUS_ENABLE\n");
+		/* ignored as we don't send unsolicited status */
+		return RCODE_COMPLETE;
+
+	case TCODE_READ_QUADLET_REQUEST:
+		return RCODE_COMPLETE;
+
+	default:
+		return RCODE_TYPE_ERROR;
+	}
+}
+
+static void tgt_agent_rw(struct fw_card *card, struct fw_request *request,
+		int tcode, int destination, int source, int generation,
+		unsigned long long offset, void *data, size_t length,
+		void *callback_data)
+{
+	struct sbp_target_agent *agent = callback_data;
+	struct sbp_session *sess = agent->login->sess;
+	int sess_gen, sess_node, rcode;
+
+	spin_lock_bh(&sess->lock);
+	sess_gen = sess->generation;
+	sess_node = sess->node_id;
+	spin_unlock_bh(&sess->lock);
+
+	if (generation != sess_gen) {
+		pr_notice("ignoring request with wrong generation\n");
+		rcode = RCODE_TYPE_ERROR;
+		goto out;
+	}
+
+	if (source != sess_node) {
+		pr_notice("ignoring request from foreign node (%x != %x)\n",
+				source, sess_node);
+		rcode = RCODE_TYPE_ERROR;
+		goto out;
+	}
+
+	/* turn offset into the offset from the start of the block */
+	offset -= agent->handler.offset;
+
+	if (offset == 0x00 && length == 4) {
+		/* AGENT_STATE */
+		rcode = tgt_agent_rw_agent_state(card, tcode, data, agent);
+	} else if (offset == 0x04 && length == 4) {
+		/* AGENT_RESET */
+		rcode = tgt_agent_rw_agent_reset(card, tcode, data, agent);
+	} else if (offset == 0x08 && length == 8) {
+		/* ORB_POINTER */
+		rcode = tgt_agent_rw_orb_pointer(card, tcode, data, agent);
+	} else if (offset == 0x10 && length == 4) {
+		/* DOORBELL */
+		rcode = tgt_agent_rw_doorbell(card, tcode, data, agent);
+	} else if (offset == 0x14 && length == 4) {
+		/* UNSOLICITED_STATUS_ENABLE */
+		rcode = tgt_agent_rw_unsolicited_status_enable(card, tcode,
+				data, agent);
+	} else {
+		rcode = RCODE_ADDRESS_ERROR;
+	}
+
+out:
+	fw_send_response(card, request, rcode);
+}
+
+static void sbp_handle_command(struct sbp_target_request *);
+static int sbp_send_status(struct sbp_target_request *);
+static void sbp_free_request(struct sbp_target_request *);
+
+static void tgt_agent_process_work(struct work_struct *work)
+{
+	struct sbp_target_request *req =
+		container_of(work, struct sbp_target_request, work);
+
+	pr_debug("tgt_orb ptr:0x%llx next_ORB:0x%llx data_descriptor:0x%llx misc:0x%x\n",
+			req->orb_pointer,
+			sbp2_pointer_to_addr(&req->orb.next_orb),
+			sbp2_pointer_to_addr(&req->orb.data_descriptor),
+			be32_to_cpu(req->orb.misc));
+
+	if (req->orb_pointer >> 32)
+		pr_debug("ORB with high bits set\n");
+
+	switch (ORB_REQUEST_FORMAT(be32_to_cpu(req->orb.misc))) {
+		case 0:/* Format specified by this standard */
+			sbp_handle_command(req);
+			return;
+		case 1: /* Reserved for future standardization */
+		case 2: /* Vendor-dependent */
+			req->status.status |= cpu_to_be32(
+					STATUS_BLOCK_RESP(
+						STATUS_RESP_REQUEST_COMPLETE) |
+					STATUS_BLOCK_DEAD(0) |
+					STATUS_BLOCK_LEN(1) |
+					STATUS_BLOCK_SBP_STATUS(
+						SBP_STATUS_REQ_TYPE_NOTSUPP));
+			sbp_send_status(req);
+			sbp_free_request(req);
+			return;
+		case 3: /* Dummy ORB */
+			req->status.status |= cpu_to_be32(
+					STATUS_BLOCK_RESP(
+						STATUS_RESP_REQUEST_COMPLETE) |
+					STATUS_BLOCK_DEAD(0) |
+					STATUS_BLOCK_LEN(1) |
+					STATUS_BLOCK_SBP_STATUS(
+						SBP_STATUS_DUMMY_ORB_COMPLETE));
+			sbp_send_status(req);
+			sbp_free_request(req);
+			return;
+		default:
+			BUG();
+	}
+}
+
+/* used to double-check we haven't been issued an AGENT_RESET */
+static inline bool tgt_agent_check_active(struct sbp_target_agent *agent)
+{
+	bool active;
+
+	spin_lock_bh(&agent->lock);
+	active = (agent->state == AGENT_STATE_ACTIVE);
+	spin_unlock_bh(&agent->lock);
+
+	return active;
+}
+
+static void tgt_agent_fetch_work(struct work_struct *work)
+{
+	struct sbp_target_agent *agent =
+		container_of(work, struct sbp_target_agent, work);
+	struct sbp_session *sess = agent->login->sess;
+	struct sbp_target_request *req;
+	int ret;
+	bool doorbell = agent->doorbell;
+	u64 next_orb = agent->orb_pointer;
+
+	while (next_orb && tgt_agent_check_active(agent)) {
+		req = kzalloc(sizeof(*req), GFP_KERNEL);
+		if (!req) {
+			spin_lock_bh(&agent->lock);
+			agent->state = AGENT_STATE_DEAD;
+			spin_unlock_bh(&agent->lock);
+			return;
+		}
+
+		req->login = agent->login;
+		req->orb_pointer = next_orb;
+
+		req->status.status = cpu_to_be32(STATUS_BLOCK_ORB_OFFSET_HIGH(
+					req->orb_pointer >> 32));
+		req->status.orb_low = cpu_to_be32(
+				req->orb_pointer & 0xfffffffc);
+
+		/* read in the ORB */
+		ret = sbp_run_transaction(sess->card, TCODE_READ_BLOCK_REQUEST,
+				sess->node_id, sess->generation, sess->speed,
+				req->orb_pointer, &req->orb, sizeof(req->orb));
+		if (ret != RCODE_COMPLETE) {
+			pr_debug("tgt_orb fetch failed: %x\n", ret);
+			req->status.status |= cpu_to_be32(
+					STATUS_BLOCK_SRC(
+						STATUS_SRC_ORB_FINISHED) |
+					STATUS_BLOCK_RESP(
+						STATUS_RESP_TRANSPORT_FAILURE) |
+					STATUS_BLOCK_DEAD(1) |
+					STATUS_BLOCK_LEN(1) |
+					STATUS_BLOCK_SBP_STATUS(
+						SBP_STATUS_UNSPECIFIED_ERROR));
+			spin_lock_bh(&agent->lock);
+			agent->state = AGENT_STATE_DEAD;
+			spin_unlock_bh(&agent->lock);
+
+			sbp_send_status(req);
+			sbp_free_request(req);
+			return;
+		}
+
+		/* check the next_ORB field */
+		if (be32_to_cpu(req->orb.next_orb.high) & 0x80000000) {
+			next_orb = 0;
+			req->status.status |= cpu_to_be32(STATUS_BLOCK_SRC(
+						STATUS_SRC_ORB_FINISHED));
+		} else {
+			next_orb = sbp2_pointer_to_addr(&req->orb.next_orb);
+			req->status.status |= cpu_to_be32(STATUS_BLOCK_SRC(
+						STATUS_SRC_ORB_CONTINUING));
+		}
+
+		if (tgt_agent_check_active(agent) && !doorbell) {
+			INIT_WORK(&req->work, tgt_agent_process_work);
+			queue_work(system_unbound_wq, &req->work);
+		} else {
+			/* don't process this request, just check next_ORB */
+			sbp_free_request(req);
+		}
+
+		spin_lock_bh(&agent->lock);
+		doorbell = agent->doorbell = false;
+
+		/* check if we should carry on processing */
+		if (next_orb)
+			agent->orb_pointer = next_orb;
+		else
+			agent->state = AGENT_STATE_SUSPENDED;
+
+		spin_unlock_bh(&agent->lock);
+	};
+}
+
+static struct sbp_target_agent *sbp_target_agent_register(
+		struct sbp_login_descriptor *login)
+{
+	struct sbp_target_agent *agent;
+	int ret;
+
+	agent = kmalloc(sizeof(*agent), GFP_KERNEL);
+	if (!agent)
+		return ERR_PTR(-ENOMEM);
+
+	spin_lock_init(&agent->lock);
+
+	agent->handler.length = 0x20;
+	agent->handler.address_callback = tgt_agent_rw;
+	agent->handler.callback_data = agent;
+
+	agent->login = login;
+	agent->state = AGENT_STATE_RESET;
+	INIT_WORK(&agent->work, tgt_agent_fetch_work);
+	agent->orb_pointer = 0;
+	agent->doorbell = false;
+
+	ret = fw_core_add_address_handler(&agent->handler,
+			&sbp_register_region);
+	if (ret < 0) {
+		kfree(agent);
+		return ERR_PTR(ret);
+	}
+
+	return agent;
+}
+
+static void sbp_target_agent_unregister(struct sbp_target_agent *agent)
+{
+	fw_core_remove_address_handler(&agent->handler);
+	cancel_work_sync(&agent->work);
+	kfree(agent);
+}
+
+/*
+ * Simple wrapper around fw_run_transaction that retries the transaction several
+ * times in case of failure, with an exponential backoff.
+ */
+static int sbp_run_transaction(struct fw_card *card, int tcode, int destination_id,
+		int generation, int speed, unsigned long long offset,
+		void *payload, size_t length)
+{
+	int attempt, ret, delay;
+
+	for (attempt = 1; attempt <= 5; attempt++) {
+		ret = fw_run_transaction(card, tcode, destination_id,
+				generation, speed, offset, payload, length);
+
+		switch (ret) {
+		case RCODE_COMPLETE:
+		case RCODE_TYPE_ERROR:
+		case RCODE_ADDRESS_ERROR:
+		case RCODE_GENERATION:
+			return ret;
+
+		default:
+			delay = 5 * attempt * attempt;
+			usleep_range(delay, delay * 2);
+		}
+	}
+
+	return ret;
+}
+
+/*
+ * Wrapper around sbp_run_transaction that gets the card, destination,
+ * generation and speed out of the request's session.
+ */
+static int sbp_run_request_transaction(struct sbp_target_request *req,
+		int tcode, unsigned long long offset, void *payload,
+		size_t length)
+{
+	struct sbp_login_descriptor *login = req->login;
+	struct sbp_session *sess = login->sess;
+	struct fw_card *card;
+	int node_id, generation, speed, ret;
+
+	spin_lock_bh(&sess->lock);
+	card = fw_card_get(sess->card);
+	node_id = sess->node_id;
+	generation = sess->generation;
+	speed = sess->speed;
+	spin_unlock_bh(&sess->lock);
+
+	ret = sbp_run_transaction(card, tcode, node_id, generation, speed,
+			offset, payload, length);
+
+	fw_card_put(card);
+
+	return ret;
+}
+
+static int sbp_fetch_command(struct sbp_target_request *req)
+{
+	int ret, cmd_len, copy_len;
+
+	cmd_len = scsi_command_size(req->orb.command_block);
+
+	req->cmd_buf = kmalloc(cmd_len, GFP_KERNEL);
+	if (!req->cmd_buf)
+		return -ENOMEM;
+
+	memcpy(req->cmd_buf, req->orb.command_block,
+		min_t(int, cmd_len, sizeof(req->orb.command_block)));
+
+	if (cmd_len > sizeof(req->orb.command_block)) {
+		pr_debug("sbp_fetch_command: filling in long command\n");
+		copy_len = cmd_len - sizeof(req->orb.command_block);
+
+		ret = sbp_run_request_transaction(req,
+				TCODE_READ_BLOCK_REQUEST,
+				req->orb_pointer + sizeof(req->orb),
+				req->cmd_buf + sizeof(req->orb.command_block),
+				copy_len);
+		if (ret != RCODE_COMPLETE)
+			return -EIO;
+	}
+
+	return 0;
+}
+
+static int sbp_fetch_page_table(struct sbp_target_request *req)
+{
+	int pg_tbl_sz, ret;
+	struct sbp_page_table_entry *pg_tbl;
+
+	if (!CMDBLK_ORB_PG_TBL_PRESENT(be32_to_cpu(req->orb.misc)))
+		return 0;
+
+	pg_tbl_sz = CMDBLK_ORB_DATA_SIZE(be32_to_cpu(req->orb.misc)) *
+		sizeof(struct sbp_page_table_entry);
+
+	pg_tbl = kmalloc(pg_tbl_sz, GFP_KERNEL);
+	if (!pg_tbl)
+		return -ENOMEM;
+
+	ret = sbp_run_request_transaction(req, TCODE_READ_BLOCK_REQUEST,
+			sbp2_pointer_to_addr(&req->orb.data_descriptor),
+			pg_tbl, pg_tbl_sz);
+	if (ret != RCODE_COMPLETE) {
+		kfree(pg_tbl);
+		return -EIO;
+	}
+
+	req->pg_tbl = pg_tbl;
+	return 0;
+}
+
+static void sbp_calc_data_length_direction(struct sbp_target_request *req,
+	u32 *data_len, enum dma_data_direction *data_dir)
+{
+	int data_size, direction, idx;
+
+	data_size = CMDBLK_ORB_DATA_SIZE(be32_to_cpu(req->orb.misc));
+	direction = CMDBLK_ORB_DIRECTION(be32_to_cpu(req->orb.misc));
+
+	if (!data_size) {
+		*data_len = 0;
+		*data_dir = DMA_NONE;
+		return;
+	}
+
+	*data_dir = direction ? DMA_FROM_DEVICE : DMA_TO_DEVICE;
+
+	if (req->pg_tbl) {
+		*data_len = 0;
+		for (idx = 0; idx < data_size; idx++) {
+			*data_len += be16_to_cpu(
+					req->pg_tbl[idx].segment_length);
+		}
+	} else {
+		*data_len = data_size;
+	}
+}
+
+static void sbp_handle_command(struct sbp_target_request *req)
+{
+	struct sbp_login_descriptor *login = req->login;
+	struct sbp_session *sess = login->sess;
+	int ret, unpacked_lun;
+	u32 data_length;
+	enum dma_data_direction data_dir;
+
+	ret = sbp_fetch_command(req);
+	if (ret) {
+		pr_debug("sbp_handle_command: fetch command failed: %d\n", ret);
+		req->status.status |= cpu_to_be32(
+			STATUS_BLOCK_RESP(STATUS_RESP_TRANSPORT_FAILURE) |
+			STATUS_BLOCK_DEAD(0) |
+			STATUS_BLOCK_LEN(1) |
+			STATUS_BLOCK_SBP_STATUS(SBP_STATUS_UNSPECIFIED_ERROR));
+		sbp_send_status(req);
+		sbp_free_request(req);
+		return;
+	}
+
+	ret = sbp_fetch_page_table(req);
+	if (ret) {
+		pr_debug("sbp_handle_command: fetch page table failed: %d\n",
+			ret);
+		req->status.status |= cpu_to_be32(
+			STATUS_BLOCK_RESP(STATUS_RESP_TRANSPORT_FAILURE) |
+			STATUS_BLOCK_DEAD(0) |
+			STATUS_BLOCK_LEN(1) |
+			STATUS_BLOCK_SBP_STATUS(SBP_STATUS_UNSPECIFIED_ERROR));
+		sbp_send_status(req);
+		sbp_free_request(req);
+		return;
+	}
+
+	unpacked_lun = req->login->lun->unpacked_lun;
+	sbp_calc_data_length_direction(req, &data_length, &data_dir);
+
+	pr_debug("sbp_handle_command ORB:0x%llx unpacked_lun:%d data_len:%d data_dir:%d\n",
+			req->orb_pointer, unpacked_lun, data_length, data_dir);
+
+	target_submit_cmd(&req->se_cmd, sess->se_sess, req->cmd_buf,
+			req->sense_buf, unpacked_lun, data_length,
+			MSG_SIMPLE_TAG, data_dir, 0);
+}
+
+/*
+ * DMA_TO_DEVICE = read from initiator (SCSI WRITE)
+ * DMA_FROM_DEVICE = write to initiator (SCSI READ)
+ */
+static int sbp_rw_data(struct sbp_target_request *req)
+{
+	struct sbp_session *sess = req->login->sess;
+	int tcode, sg_miter_flags, max_payload, pg_size, speed, node_id,
+		generation, num_pte, length, tfr_length,
+		rcode = RCODE_COMPLETE;
+	struct sbp_page_table_entry *pte;
+	unsigned long long offset;
+	struct fw_card *card;
+	struct sg_mapping_iter iter;
+
+	if (req->se_cmd.data_direction == DMA_FROM_DEVICE) {
+		tcode = TCODE_WRITE_BLOCK_REQUEST;
+		sg_miter_flags = SG_MITER_FROM_SG;
+	} else {
+		tcode = TCODE_READ_BLOCK_REQUEST;
+		sg_miter_flags = SG_MITER_TO_SG;
+	}
+
+	max_payload = 4 << CMDBLK_ORB_MAX_PAYLOAD(be32_to_cpu(req->orb.misc));
+	speed = CMDBLK_ORB_SPEED(be32_to_cpu(req->orb.misc));
+
+	pg_size = CMDBLK_ORB_PG_SIZE(be32_to_cpu(req->orb.misc));
+	if (pg_size) {
+		pr_err("sbp_run_transaction: page size ignored\n");
+		pg_size = 0x100 << pg_size;
+	}
+
+	spin_lock_bh(&sess->lock);
+	card = fw_card_get(sess->card);
+	node_id = sess->node_id;
+	generation = sess->generation;
+	spin_unlock_bh(&sess->lock);
+
+	if (req->pg_tbl) {
+		pte = req->pg_tbl;
+		num_pte = CMDBLK_ORB_DATA_SIZE(be32_to_cpu(req->orb.misc));
+
+		offset = 0;
+		length = 0;
+	} else {
+		pte = NULL;
+		num_pte = 0;
+
+		offset = sbp2_pointer_to_addr(&req->orb.data_descriptor);
+		length = req->se_cmd.data_length;
+	}
+
+	sg_miter_start(&iter, req->se_cmd.t_data_sg, req->se_cmd.t_data_nents,
+		sg_miter_flags);
+
+	while (length || num_pte) {
+		if (!length) {
+			offset = (u64)be16_to_cpu(pte->segment_base_hi) << 32 |
+				be32_to_cpu(pte->segment_base_lo);
+			length = be16_to_cpu(pte->segment_length);
+
+			pte++;
+			num_pte--;
+		}
+
+		sg_miter_next(&iter);
+
+		tfr_length = min3(length, max_payload, (int)iter.length);
+
+		/* FIXME: take page_size into account */
+
+		rcode = sbp_run_transaction(card, tcode, node_id,
+				generation, speed,
+				offset, iter.addr, tfr_length);
+
+		if (rcode != RCODE_COMPLETE)
+			break;
+
+		length -= tfr_length;
+		offset += tfr_length;
+		iter.consumed = tfr_length;
+	}
+
+	sg_miter_stop(&iter);
+	fw_card_put(card);
+
+	if (rcode == RCODE_COMPLETE) {
+		WARN_ON(length != 0);
+		return 0;
+	} else {
+		return -EIO;
+	}
+}
+
+static int sbp_send_status(struct sbp_target_request *req)
+{
+	int ret, length;
+	struct sbp_login_descriptor *login = req->login;
+
+	length = (((be32_to_cpu(req->status.status) >> 24) & 0x07) + 1) * 4;
+
+	ret = sbp_run_request_transaction(req, TCODE_WRITE_BLOCK_REQUEST,
+			login->status_fifo_addr, &req->status, length);
+	if (ret != RCODE_COMPLETE) {
+		pr_debug("sbp_send_status: write failed: 0x%x\n", ret);
+		return -EIO;
+	}
+
+	pr_debug("sbp_send_status: status write complete for ORB: 0x%llx\n",
+			req->orb_pointer);
+
+	return 0;
+}
+
+static void sbp_sense_mangle(struct sbp_target_request *req)
+{
+	struct se_cmd *se_cmd = &req->se_cmd;
+	u8 *sense = req->sense_buf;
+	u8 *status = req->status.data;
+
+	WARN_ON(se_cmd->scsi_sense_length < 18);
+
+	switch (sense[0] & 0x7f) { 		/* sfmt */
+	case 0x70: /* current, fixed */
+		status[0] = 0 << 6;
+		break;
+	case 0x71: /* deferred, fixed */
+		status[0] = 1 << 6;
+		break;
+	case 0x72: /* current, descriptor */
+	case 0x73: /* deferred, descriptor */
+	default:
+		/*
+		 * TODO: SBP-3 specifies what we should do with descriptor
+		 * format sense data
+		 */
+		pr_err("sbp_send_sense: unknown sense format: 0x%x\n",
+			sense[0]);
+		req->status.status |= cpu_to_be32(
+			STATUS_BLOCK_RESP(STATUS_RESP_REQUEST_COMPLETE) |
+			STATUS_BLOCK_DEAD(0) |
+			STATUS_BLOCK_LEN(1) |
+			STATUS_BLOCK_SBP_STATUS(SBP_STATUS_REQUEST_ABORTED));
+		return;
+	}
+
+	status[0] |= se_cmd->scsi_status & 0x3f;/* status */
+	status[1] =
+		(sense[0] & 0x80) |		/* valid */
+		((sense[2] & 0xe0) >> 1) |	/* mark, eom, ili */
+		(sense[2] & 0x0f);		/* sense_key */
+	status[2] = se_cmd->scsi_asc;		/* sense_code */
+	status[3] = se_cmd->scsi_ascq;		/* sense_qualifier */
+
+	/* information */
+	status[4] = sense[3];
+	status[5] = sense[4];
+	status[6] = sense[5];
+	status[7] = sense[6];
+
+	/* CDB-dependent */
+	status[8] = sense[8];
+	status[9] = sense[9];
+	status[10] = sense[10];
+	status[11] = sense[11];
+
+	/* fru */
+	status[12] = sense[14];
+
+	/* sense_key-dependent */
+	status[13] = sense[15];
+	status[14] = sense[16];
+	status[15] = sense[17];
+
+	req->status.status |= cpu_to_be32(
+		STATUS_BLOCK_RESP(STATUS_RESP_REQUEST_COMPLETE) |
+		STATUS_BLOCK_DEAD(0) |
+		STATUS_BLOCK_LEN(5) |
+		STATUS_BLOCK_SBP_STATUS(SBP_STATUS_OK));
+}
+
+static int sbp_send_sense(struct sbp_target_request *req)
+{
+	struct se_cmd *se_cmd = &req->se_cmd;
+
+	if (se_cmd->scsi_sense_length) {
+		sbp_sense_mangle(req);
+	} else {
+		req->status.status |= cpu_to_be32(
+			STATUS_BLOCK_RESP(STATUS_RESP_REQUEST_COMPLETE) |
+			STATUS_BLOCK_DEAD(0) |
+			STATUS_BLOCK_LEN(1) |
+			STATUS_BLOCK_SBP_STATUS(SBP_STATUS_OK));
+	}
+
+	return sbp_send_status(req);
+}
+
+static void sbp_free_request(struct sbp_target_request *req)
+{
+	kfree(req->pg_tbl);
+	kfree(req->cmd_buf);
+	kfree(req);
+}
+
+static void sbp_mgt_agent_process(struct work_struct *work)
+{
+	struct sbp_management_agent *agent =
+		container_of(work, struct sbp_management_agent, work);
+	struct sbp_management_request *req = agent->request;
+	int ret;
+	int status_data_len = 0;
+
+	/* fetch the ORB from the initiator */
+	ret = sbp_run_transaction(req->card, TCODE_READ_BLOCK_REQUEST,
+		req->node_addr, req->generation, req->speed,
+		agent->orb_offset, &req->orb, sizeof(req->orb));
+	if (ret != RCODE_COMPLETE) {
+		pr_debug("mgt_orb fetch failed: %x\n", ret);
+		goto out;
+	}
+
+	pr_debug("mgt_orb ptr1:0x%llx ptr2:0x%llx misc:0x%x len:0x%x status_fifo:0x%llx\n",
+		sbp2_pointer_to_addr(&req->orb.ptr1),
+		sbp2_pointer_to_addr(&req->orb.ptr2),
+		be32_to_cpu(req->orb.misc), be32_to_cpu(req->orb.length),
+		sbp2_pointer_to_addr(&req->orb.status_fifo));
+
+	if (!ORB_NOTIFY(be32_to_cpu(req->orb.misc)) ||
+		ORB_REQUEST_FORMAT(be32_to_cpu(req->orb.misc)) != 0) {
+		pr_err("mgt_orb bad request\n");
+		goto out;
+	}
+
+	switch (MANAGEMENT_ORB_FUNCTION(be32_to_cpu(req->orb.misc))) {
+	case MANAGEMENT_ORB_FUNCTION_LOGIN:
+		sbp_management_request_login(agent, req, &status_data_len);
+		break;
+
+	case MANAGEMENT_ORB_FUNCTION_QUERY_LOGINS:
+		sbp_management_request_query_logins(agent, req,
+				&status_data_len);
+		break;
+
+	case MANAGEMENT_ORB_FUNCTION_RECONNECT:
+		sbp_management_request_reconnect(agent, req, &status_data_len);
+		break;
+
+	case MANAGEMENT_ORB_FUNCTION_SET_PASSWORD:
+		pr_notice("SET PASSWORD not implemented\n");
+
+		req->status.status = cpu_to_be32(
+			STATUS_BLOCK_RESP(STATUS_RESP_REQUEST_COMPLETE) |
+			STATUS_BLOCK_SBP_STATUS(SBP_STATUS_REQ_TYPE_NOTSUPP));
+
+		break;
+
+	case MANAGEMENT_ORB_FUNCTION_LOGOUT:
+		sbp_management_request_logout(agent, req, &status_data_len);
+		break;
+
+	case MANAGEMENT_ORB_FUNCTION_ABORT_TASK:
+		pr_notice("ABORT TASK not implemented\n");
+
+		req->status.status = cpu_to_be32(
+			STATUS_BLOCK_RESP(STATUS_RESP_REQUEST_COMPLETE) |
+			STATUS_BLOCK_SBP_STATUS(SBP_STATUS_REQ_TYPE_NOTSUPP));
+
+		break;
+
+	case MANAGEMENT_ORB_FUNCTION_ABORT_TASK_SET:
+		pr_notice("ABORT TASK SET not implemented\n");
+
+		req->status.status = cpu_to_be32(
+			STATUS_BLOCK_RESP(STATUS_RESP_REQUEST_COMPLETE) |
+			STATUS_BLOCK_SBP_STATUS(SBP_STATUS_REQ_TYPE_NOTSUPP));
+
+		break;
+
+	case MANAGEMENT_ORB_FUNCTION_LOGICAL_UNIT_RESET:
+		pr_notice("LOGICAL UNIT RESET not implemented\n");
+
+		req->status.status = cpu_to_be32(
+			STATUS_BLOCK_RESP(STATUS_RESP_REQUEST_COMPLETE) |
+			STATUS_BLOCK_SBP_STATUS(SBP_STATUS_REQ_TYPE_NOTSUPP));
+
+		break;
+
+	case MANAGEMENT_ORB_FUNCTION_TARGET_RESET:
+		pr_notice("TARGET RESET not implemented\n");
+
+		req->status.status = cpu_to_be32(
+			STATUS_BLOCK_RESP(STATUS_RESP_REQUEST_COMPLETE) |
+			STATUS_BLOCK_SBP_STATUS(SBP_STATUS_REQ_TYPE_NOTSUPP));
+
+		break;
+
+	default:
+		pr_notice("unknown management function 0x%x\n",
+			MANAGEMENT_ORB_FUNCTION(be32_to_cpu(req->orb.misc)));
+
+		req->status.status = cpu_to_be32(
+			STATUS_BLOCK_RESP(STATUS_RESP_REQUEST_COMPLETE) |
+			STATUS_BLOCK_SBP_STATUS(SBP_STATUS_REQ_TYPE_NOTSUPP));
+
+		break;
+	}
+
+	req->status.status |= cpu_to_be32(
+		STATUS_BLOCK_SRC(1) | /* Response to ORB, next_ORB absent */
+		STATUS_BLOCK_LEN(DIV_ROUND_UP(status_data_len, 4) + 1) |
+		STATUS_BLOCK_ORB_OFFSET_HIGH(agent->orb_offset >> 32));
+	req->status.orb_low = cpu_to_be32(agent->orb_offset);
+
+	/* write the status block back to the initiator */
+	ret = sbp_run_transaction(req->card, TCODE_WRITE_BLOCK_REQUEST,
+		req->node_addr, req->generation, req->speed,
+		sbp2_pointer_to_addr(&req->orb.status_fifo),
+		&req->status, 8 + status_data_len);
+	if (ret != RCODE_COMPLETE) {
+		pr_debug("mgt_orb status write failed: %x\n", ret);
+		goto out;
+	}
+
+out:
+	fw_card_put(req->card);
+	kfree(req);
+
+	spin_lock_bh(&agent->lock);
+	agent->state = MANAGEMENT_AGENT_STATE_IDLE;
+	spin_unlock_bh(&agent->lock);
+}
+
+static void sbp_mgt_agent_rw(struct fw_card *card,
+	struct fw_request *request, int tcode, int destination, int source,
+	int generation, unsigned long long offset, void *data, size_t length,
+	void *callback_data)
+{
+	struct sbp_management_agent *agent = callback_data;
+	struct sbp2_pointer *ptr = data;
+	int rcode = RCODE_ADDRESS_ERROR;
+
+	if (!agent->tport->enable)
+		goto out;
+
+	if ((offset != agent->handler.offset) || (length != 8))
+		goto out;
+
+	if (tcode == TCODE_WRITE_BLOCK_REQUEST) {
+		struct sbp_management_request *req;
+		int prev_state;
+
+		spin_lock_bh(&agent->lock);
+		prev_state = agent->state;
+		agent->state = MANAGEMENT_AGENT_STATE_BUSY;
+		spin_unlock_bh(&agent->lock);
+
+		if (prev_state == MANAGEMENT_AGENT_STATE_BUSY) {
+			pr_notice("ignoring management request while busy\n");
+			rcode = RCODE_CONFLICT_ERROR;
+			goto out;
+		}
+
+		req = kzalloc(sizeof(*req), GFP_ATOMIC);
+		if (!req) {
+			rcode = RCODE_CONFLICT_ERROR;
+			goto out;
+		}
+
+		req->card = fw_card_get(card);
+		req->generation = generation;
+		req->node_addr = source;
+		req->speed = fw_get_request_speed(request);
+
+		agent->orb_offset = sbp2_pointer_to_addr(ptr);
+		agent->request = req;
+
+		queue_work(system_unbound_wq, &agent->work);
+		rcode = RCODE_COMPLETE;
+	} else if (tcode == TCODE_READ_BLOCK_REQUEST) {
+		addr_to_sbp2_pointer(agent->orb_offset, ptr);
+		rcode = RCODE_COMPLETE;
+	} else {
+		rcode = RCODE_TYPE_ERROR;
+	}
+
+out:
+	fw_send_response(card, request, rcode);
+}
+
+static struct sbp_management_agent *sbp_management_agent_register(
+		struct sbp_tport *tport)
+{
+	int ret;
+	struct sbp_management_agent *agent;
+
+	agent = kmalloc(sizeof(*agent), GFP_KERNEL);
+	if (!agent)
+		return ERR_PTR(-ENOMEM);
+
+	spin_lock_init(&agent->lock);
+	agent->tport = tport;
+	agent->handler.length = 0x08;
+	agent->handler.address_callback = sbp_mgt_agent_rw;
+	agent->handler.callback_data = agent;
+	agent->state = MANAGEMENT_AGENT_STATE_IDLE;
+	INIT_WORK(&agent->work, sbp_mgt_agent_process);
+	agent->orb_offset = 0;
+	agent->request = NULL;
+
+	ret = fw_core_add_address_handler(&agent->handler,
+			&sbp_register_region);
+	if (ret < 0) {
+		kfree(agent);
+		return ERR_PTR(ret);
+	}
+
+	return agent;
+}
+
+static void sbp_management_agent_unregister(struct sbp_management_agent *agent)
+{
+	fw_core_remove_address_handler(&agent->handler);
+	cancel_work_sync(&agent->work);
+	kfree(agent);
+}
+
+static int sbp_check_true(struct se_portal_group *se_tpg)
+{
+	return 1;
+}
+
+static int sbp_check_false(struct se_portal_group *se_tpg)
+{
+	return 0;
+}
+
+static char *sbp_get_fabric_name(void)
+{
+	return "sbp";
+}
+
+static char *sbp_get_fabric_wwn(struct se_portal_group *se_tpg)
+{
+	struct sbp_tpg *tpg = container_of(se_tpg, struct sbp_tpg, se_tpg);
+	struct sbp_tport *tport = tpg->tport;
+
+	return &tport->tport_name[0];
+}
+
+static u16 sbp_get_tag(struct se_portal_group *se_tpg)
+{
+	struct sbp_tpg *tpg = container_of(se_tpg, struct sbp_tpg, se_tpg);
+	return tpg->tport_tpgt;
+}
+
+static u32 sbp_get_default_depth(struct se_portal_group *se_tpg)
+{
+	return 1;
+}
+
+static struct se_node_acl *sbp_alloc_fabric_acl(struct se_portal_group *se_tpg)
+{
+	struct sbp_nacl *nacl;
+
+	nacl = kzalloc(sizeof(struct sbp_nacl), GFP_KERNEL);
+	if (!nacl) {
+		pr_err("Unable to alocate struct sbp_nacl\n");
+		return NULL;
+	}
+
+	return &nacl->se_node_acl;
+}
+
+static void sbp_release_fabric_acl(
+	struct se_portal_group *se_tpg,
+	struct se_node_acl *se_nacl)
+{
+	struct sbp_nacl *nacl =
+		container_of(se_nacl, struct sbp_nacl, se_node_acl);
+	kfree(nacl);
+}
+
+static u32 sbp_tpg_get_inst_index(struct se_portal_group *se_tpg)
+{
+	return 1;
+}
+
+static void sbp_release_cmd(struct se_cmd *se_cmd)
+{
+	struct sbp_target_request *req = container_of(se_cmd,
+			struct sbp_target_request, se_cmd);
+
+	sbp_free_request(req);
+}
+
+static int sbp_shutdown_session(struct se_session *se_sess)
+{
+	return 0;
+}
+
+static void sbp_close_session(struct se_session *se_sess)
+{
+	return;
+}
+
+static u32 sbp_sess_get_index(struct se_session *se_sess)
+{
+	return 0;
+}
+
+static int sbp_write_pending(struct se_cmd *se_cmd)
+{
+	struct sbp_target_request *req = container_of(se_cmd,
+			struct sbp_target_request, se_cmd);
+	int ret;
+
+	ret = sbp_rw_data(req);
+	if (ret) {
+		req->status.status |= cpu_to_be32(
+			STATUS_BLOCK_RESP(
+				STATUS_RESP_TRANSPORT_FAILURE) |
+			STATUS_BLOCK_DEAD(0) |
+			STATUS_BLOCK_LEN(1) |
+			STATUS_BLOCK_SBP_STATUS(
+				SBP_STATUS_UNSPECIFIED_ERROR));
+		sbp_send_status(req);
+		return ret;
+	}
+
+	transport_generic_process_write(se_cmd);
+
+	return 0;
+}
+
+static int sbp_write_pending_status(struct se_cmd *se_cmd)
+{
+	return 0;
+}
+
+static void sbp_set_default_node_attrs(struct se_node_acl *nacl)
+{
+	return;
+}
+
+static u32 sbp_get_task_tag(struct se_cmd *se_cmd)
+{
+	struct sbp_target_request *req = container_of(se_cmd,
+			struct sbp_target_request, se_cmd);
+
+	/* only used for printk until we do TMRs */
+	return (u32)req->orb_pointer;
+}
+
+static int sbp_get_cmd_state(struct se_cmd *se_cmd)
+{
+	return 0;
+}
+
+static int sbp_queue_data_in(struct se_cmd *se_cmd)
+{
+	struct sbp_target_request *req = container_of(se_cmd,
+			struct sbp_target_request, se_cmd);
+	int ret;
+
+	ret = sbp_rw_data(req);
+	if (ret) {
+		req->status.status |= cpu_to_be32(
+			STATUS_BLOCK_RESP(STATUS_RESP_TRANSPORT_FAILURE) |
+			STATUS_BLOCK_DEAD(0) |
+			STATUS_BLOCK_LEN(1) |
+			STATUS_BLOCK_SBP_STATUS(SBP_STATUS_UNSPECIFIED_ERROR));
+		sbp_send_status(req);
+		return ret;
+	}
+
+	return sbp_send_sense(req);
+}
+
+/*
+ * Called after command (no data transfer) or after the write (to device)
+ * operation is completed
+ */
+static int sbp_queue_status(struct se_cmd *se_cmd)
+{
+	struct sbp_target_request *req = container_of(se_cmd,
+			struct sbp_target_request, se_cmd);
+
+	return sbp_send_sense(req);
+}
+
+static int sbp_queue_tm_rsp(struct se_cmd *se_cmd)
+{
+	return 0;
+}
+
+static u16 sbp_set_fabric_sense_len(struct se_cmd *se_cmd, u32 sense_length)
+{
+	return 0;
+}
+
+static u16 sbp_get_fabric_sense_len(void)
+{
+	return 0;
+}
+
+static int sbp_check_stop_free(struct se_cmd *se_cmd)
+{
+	struct sbp_target_request *req = container_of(se_cmd,
+			struct sbp_target_request, se_cmd);
+
+	transport_generic_free_cmd(&req->se_cmd, 0);
+	return 1;
+}
+
+/*
+ * Handlers for Serial Bus Protocol 2/3 (SBP-2 / SBP-3)
+ */
+static u8 sbp_get_fabric_proto_ident(struct se_portal_group *se_tpg)
+{
+	/*
+	 * Return a IEEE 1394 SCSI Protocol identifier for loopback operations
+	 * This is defined in section 7.5.1 Table 362 in spc4r17
+	 */
+	return SCSI_PROTOCOL_SBP;
+}
+
+static u32 sbp_get_pr_transport_id(
+	struct se_portal_group *se_tpg,
+	struct se_node_acl *se_nacl,
+	struct t10_pr_registration *pr_reg,
+	int *format_code,
+	unsigned char *buf)
+{
+	int ret;
+
+	/*
+	 * Set PROTOCOL IDENTIFIER to 3h for SBP
+	 */
+	buf[0] = SCSI_PROTOCOL_SBP;
+	/*
+	 * From spc4r17, 7.5.4.4 TransportID for initiator ports using SCSI
+	 * over IEEE 1394
+	 */
+	ret = hex2bin(&buf[8], se_nacl->initiatorname, 8);
+	if (ret < 0)
+		pr_debug("sbp transport_id: invalid hex string\n");
+
+	/*
+	 * The IEEE 1394 Transport ID is a hardcoded 24-byte length
+	 */
+	return 24;
+}
+
+static u32 sbp_get_pr_transport_id_len(
+	struct se_portal_group *se_tpg,
+	struct se_node_acl *se_nacl,
+	struct t10_pr_registration *pr_reg,
+	int *format_code)
+{
+	*format_code = 0;
+	/*
+	 * From spc4r17, 7.5.4.4 TransportID for initiator ports using SCSI
+	 * over IEEE 1394
+	 *
+	 * The SBP Transport ID is a hardcoded 24-byte length
+	 */
+	return 24;
+}
+
+/*
+ * Used for handling SCSI fabric dependent TransportIDs in SPC-3 and above
+ * Persistent Reservation SPEC_I_PT=1 and PROUT REGISTER_AND_MOVE operations.
+ */
+static char *sbp_parse_pr_out_transport_id(
+	struct se_portal_group *se_tpg,
+	const char *buf,
+	u32 *out_tid_len,
+	char **port_nexus_ptr)
+{
+	/*
+	 * Assume the FORMAT CODE 00b from spc4r17, 7.5.4.4 TransportID
+	 * for initiator ports using SCSI over SBP Serial SCSI Protocol
+	 *
+	 * The TransportID for a IEEE 1394 Initiator Port is of fixed size of
+	 * 24 bytes, and IEEE 1394 does not contain a I_T nexus identifier,
+	 * so we return the **port_nexus_ptr set to NULL.
+	 */
+	*port_nexus_ptr = NULL;
+	*out_tid_len = 24;
+
+	return (char *)&buf[8];
+}
+
+static int sbp_count_se_tpg_luns(struct se_portal_group *tpg)
+{
+	int i, count = 0;
+
+	spin_lock(&tpg->tpg_lun_lock);
+	for (i = 0; i < TRANSPORT_MAX_LUNS_PER_TPG; i++) {
+		struct se_lun *se_lun = tpg->tpg_lun_list[i];
+
+		if (se_lun->lun_status == TRANSPORT_LUN_STATUS_FREE)
+			continue;
+
+		count++;
+	}
+	spin_unlock(&tpg->tpg_lun_lock);
+
+	return count;
+}
+
+static int sbp_update_unit_directory(struct sbp_tport *tport)
+{
+	int num_luns, num_entries, idx = 0, mgt_agt_addr, ret, i;
+	u32 *data;
+
+	if (tport->unit_directory.data) {
+		fw_core_remove_descriptor(&tport->unit_directory);
+		kfree(tport->unit_directory.data);
+		tport->unit_directory.data = NULL;
+	}
+
+	if (!tport->enable || !tport->tpg)
+		return 0;
+
+	num_luns = sbp_count_se_tpg_luns(&tport->tpg->se_tpg);
+
+	/*
+	 * Number of entries in the final unit directory:
+	 *  - all of those in the template
+	 *  - management_agent
+	 *  - unit_characteristics
+	 *  - reconnect_timeout
+	 *  - unit unique ID
+	 *  - one for each LUN
+	 *
+	 *  MUST NOT include leaf or sub-directory entries
+	 */
+	num_entries = ARRAY_SIZE(sbp_unit_directory_template) + 4 + num_luns;
+
+	if (tport->directory_id != -1)
+		num_entries++;
+
+	/* allocate num_entries + 4 for the header and unique ID leaf */
+	data = kcalloc((num_entries + 4), sizeof(u32), GFP_KERNEL);
+	if (!data)
+		return -ENOMEM;
+
+	/* directory_length */
+	data[idx++] = num_entries << 16;
+
+	/* directory_id */
+	if (tport->directory_id != -1)
+		data[idx++] = (CSR_DIRECTORY_ID << 24) | tport->directory_id;
+
+	/* unit directory template */
+	memcpy(&data[idx], sbp_unit_directory_template,
+			sizeof(sbp_unit_directory_template));
+	idx += ARRAY_SIZE(sbp_unit_directory_template);
+
+	/* management_agent */
+	mgt_agt_addr = (tport->mgt_agt->handler.offset - CSR_REGISTER_BASE) / 4;
+	data[idx++] = 0x54000000 | (mgt_agt_addr & 0x00ffffff);
+
+	/* unit_characteristics */
+	data[idx++] = 0x3a000000 |
+		(((tport->mgt_orb_timeout * 2) << 8) & 0xff00) |
+		SBP_ORB_FETCH_SIZE;
+
+	/* reconnect_timeout */
+	data[idx++] = 0x3d000000 | (tport->max_reconnect_timeout & 0xffff);
+
+	/* unit unique ID (leaf is just after LUNs) */
+	data[idx++] = 0x8d000000 | (num_luns + 1);
+
+	spin_lock(&tport->tpg->se_tpg.tpg_lun_lock);
+	for (i = 0; i < TRANSPORT_MAX_LUNS_PER_TPG; i++) {
+		struct se_lun *se_lun = tport->tpg->se_tpg.tpg_lun_list[i];
+		struct se_device *dev;
+		int type;
+
+		if (se_lun->lun_status == TRANSPORT_LUN_STATUS_FREE)
+			continue;
+
+		spin_unlock(&tport->tpg->se_tpg.tpg_lun_lock);
+
+		dev = se_lun->lun_se_dev;
+		type = dev->transport->get_device_type(dev);
+
+		/* logical_unit_number */
+		data[idx++] = 0x14000000 |
+			((type << 16) & 0x1f0000) |
+			(se_lun->unpacked_lun & 0xffff);
+
+		spin_lock(&tport->tpg->se_tpg.tpg_lun_lock);
+	}
+	spin_unlock(&tport->tpg->se_tpg.tpg_lun_lock);
+
+	/* unit unique ID leaf */
+	data[idx++] = 2 << 16;
+	data[idx++] = tport->guid >> 32;
+	data[idx++] = tport->guid;
+
+	tport->unit_directory.length = idx;
+	tport->unit_directory.key = (CSR_DIRECTORY | CSR_UNIT) << 24;
+	tport->unit_directory.data = data;
+
+	ret = fw_core_add_descriptor(&tport->unit_directory);
+	if (ret < 0) {
+		kfree(tport->unit_directory.data);
+		tport->unit_directory.data = NULL;
+	}
+
+	return ret;
+}
+
+static ssize_t sbp_parse_wwn(const char *name, u64 *wwn, int strict)
+{
+	const char *cp;
+	char c, nibble;
+	int pos = 0, err;
+
+	*wwn = 0;
+	for (cp = name; cp < &name[SBP_NAMELEN - 1]; cp++) {
+		c = *cp;
+		if (c == '\n' && cp[1] == '\0')
+			continue;
+		if (c == '\0') {
+			err = 2;
+			if (pos != 16)
+				goto fail;
+			return cp - name;
+		}
+		err = 3;
+		if (isdigit(c))
+			nibble = c - '0';
+		else if (isxdigit(c) && (islower(c) || !strict))
+			nibble = tolower(c) - 'a' + 10;
+		else
+			goto fail;
+		*wwn = (*wwn << 4) | nibble;
+		pos++;
+	}
+	err = 4;
+fail:
+	printk(KERN_INFO "err %u len %zu pos %u\n",
+			err, cp - name, pos);
+	return -1;
+}
+
+static ssize_t sbp_format_wwn(char *buf, size_t len, u64 wwn)
+{
+	return snprintf(buf, len, "%016llx", wwn);
+}
+
+static struct se_node_acl *sbp_make_nodeacl(
+		struct se_portal_group *se_tpg,
+		struct config_group *group,
+		const char *name)
+{
+	struct se_node_acl *se_nacl, *se_nacl_new;
+	struct sbp_nacl *nacl;
+	u64 guid = 0;
+	u32 nexus_depth = 1;
+
+	if (sbp_parse_wwn(name, &guid, 1) < 0)
+		return ERR_PTR(-EINVAL);
+
+	se_nacl_new = sbp_alloc_fabric_acl(se_tpg);
+	if (!se_nacl_new)
+		return ERR_PTR(-ENOMEM);
+
+	/*
+	 * se_nacl_new may be released by core_tpg_add_initiator_node_acl()
+	 * when converting a NodeACL from demo mode -> explict
+	 */
+	se_nacl = core_tpg_add_initiator_node_acl(se_tpg, se_nacl_new,
+			name, nexus_depth);
+	if (IS_ERR(se_nacl)) {
+		sbp_release_fabric_acl(se_tpg, se_nacl_new);
+		return se_nacl;
+	}
+
+	nacl = container_of(se_nacl, struct sbp_nacl, se_node_acl);
+	nacl->guid = guid;
+	sbp_format_wwn(nacl->iport_name, SBP_NAMELEN, guid);
+
+	return se_nacl;
+}
+
+static void sbp_drop_nodeacl(struct se_node_acl *se_acl)
+{
+	struct sbp_nacl *nacl =
+		container_of(se_acl, struct sbp_nacl, se_node_acl);
+
+	core_tpg_del_initiator_node_acl(se_acl->se_tpg, se_acl, 1);
+	kfree(nacl);
+}
+
+static int sbp_post_link_lun(
+		struct se_portal_group *se_tpg,
+		struct se_lun *se_lun)
+{
+	struct sbp_tpg *tpg = container_of(se_tpg, struct sbp_tpg, se_tpg);
+
+	return sbp_update_unit_directory(tpg->tport);
+}
+
+static void sbp_pre_unlink_lun(
+		struct se_portal_group *se_tpg,
+		struct se_lun *se_lun)
+{
+	struct sbp_tpg *tpg = container_of(se_tpg, struct sbp_tpg, se_tpg);
+	struct sbp_tport *tport = tpg->tport;
+	int ret;
+
+	if (sbp_count_se_tpg_luns(&tpg->se_tpg) == 0)
+		tport->enable = 0;
+
+	ret = sbp_update_unit_directory(tport);
+	if (ret < 0)
+		pr_err("unlink LUN: failed to update unit directory\n");
+}
+
+static struct se_portal_group *sbp_make_tpg(
+		struct se_wwn *wwn,
+		struct config_group *group,
+		const char *name)
+{
+	struct sbp_tport *tport =
+		container_of(wwn, struct sbp_tport, tport_wwn);
+
+	struct sbp_tpg *tpg;
+	unsigned long tpgt;
+	int ret;
+
+	if (strstr(name, "tpgt_") != name)
+		return ERR_PTR(-EINVAL);
+	if (kstrtoul(name + 5, 10, &tpgt) || tpgt > UINT_MAX)
+		return ERR_PTR(-EINVAL);
+
+	if (tport->tpg) {
+		pr_err("Only one TPG per Unit is possible.\n");
+		return ERR_PTR(-EBUSY);
+	}
+
+	tpg = kzalloc(sizeof(*tpg), GFP_KERNEL);
+	if (!tpg) {
+		pr_err("Unable to allocate struct sbp_tpg\n");
+		return ERR_PTR(-ENOMEM);
+	}
+
+	tpg->tport = tport;
+	tpg->tport_tpgt = tpgt;
+	tport->tpg = tpg;
+
+	/* default attribute values */
+	tport->enable = 0;
+	tport->directory_id = -1;
+	tport->mgt_orb_timeout = 15;
+	tport->max_reconnect_timeout = 5;
+	tport->max_logins_per_lun = 1;
+
+	tport->mgt_agt = sbp_management_agent_register(tport);
+	if (IS_ERR(tport->mgt_agt)) {
+		ret = PTR_ERR(tport->mgt_agt);
+		kfree(tpg);
+		return ERR_PTR(ret);
+	}
+
+	ret = core_tpg_register(&sbp_fabric_configfs->tf_ops, wwn,
+			&tpg->se_tpg, (void *)tpg,
+			TRANSPORT_TPG_TYPE_NORMAL);
+	if (ret < 0) {
+		sbp_management_agent_unregister(tport->mgt_agt);
+		kfree(tpg);
+		return ERR_PTR(ret);
+	}
+
+	return &tpg->se_tpg;
+}
+
+static void sbp_drop_tpg(struct se_portal_group *se_tpg)
+{
+	struct sbp_tpg *tpg = container_of(se_tpg, struct sbp_tpg, se_tpg);
+	struct sbp_tport *tport = tpg->tport;
+
+	core_tpg_deregister(se_tpg);
+	sbp_management_agent_unregister(tport->mgt_agt);
+	tport->tpg = NULL;
+	kfree(tpg);
+}
+
+static struct se_wwn *sbp_make_tport(
+		struct target_fabric_configfs *tf,
+		struct config_group *group,
+		const char *name)
+{
+	struct sbp_tport *tport;
+	u64 guid = 0;
+
+	if (sbp_parse_wwn(name, &guid, 1) < 0)
+		return ERR_PTR(-EINVAL);
+
+	tport = kzalloc(sizeof(*tport), GFP_KERNEL);
+	if (!tport) {
+		pr_err("Unable to allocate struct sbp_tport\n");
+		return ERR_PTR(-ENOMEM);
+	}
+
+	tport->guid = guid;
+	sbp_format_wwn(tport->tport_name, SBP_NAMELEN, guid);
+
+	return &tport->tport_wwn;
+}
+
+static void sbp_drop_tport(struct se_wwn *wwn)
+{
+	struct sbp_tport *tport =
+		container_of(wwn, struct sbp_tport, tport_wwn);
+
+	kfree(tport);
+}
+
+static ssize_t sbp_wwn_show_attr_version(
+		struct target_fabric_configfs *tf,
+		char *page)
+{
+	return sprintf(page, "FireWire SBP fabric module %s\n", SBP_VERSION);
+}
+
+TF_WWN_ATTR_RO(sbp, version);
+
+static struct configfs_attribute *sbp_wwn_attrs[] = {
+	&sbp_wwn_version.attr,
+	NULL,
+};
+
+static ssize_t sbp_tpg_show_directory_id(
+		struct se_portal_group *se_tpg,
+		char *page)
+{
+	struct sbp_tpg *tpg = container_of(se_tpg, struct sbp_tpg, se_tpg);
+	struct sbp_tport *tport = tpg->tport;
+
+	if (tport->directory_id == -1)
+		return sprintf(page, "implicit\n");
+	else
+		return sprintf(page, "%06x\n", tport->directory_id);
+}
+
+static ssize_t sbp_tpg_store_directory_id(
+		struct se_portal_group *se_tpg,
+		const char *page,
+		size_t count)
+{
+	struct sbp_tpg *tpg = container_of(se_tpg, struct sbp_tpg, se_tpg);
+	struct sbp_tport *tport = tpg->tport;
+	unsigned long val;
+
+	if (tport->enable) {
+		pr_err("Cannot change the directory_id on an active target.\n");
+		return -EBUSY;
+	}
+
+	if (strstr(page, "implicit") == page) {
+		tport->directory_id = -1;
+	} else {
+		if (kstrtoul(page, 16, &val) < 0)
+			return -EINVAL;
+		if (val > 0xffffff)
+			return -EINVAL;
+
+		tport->directory_id = val;
+	}
+
+	return count;
+}
+
+static ssize_t sbp_tpg_show_enable(
+		struct se_portal_group *se_tpg,
+		char *page)
+{
+	struct sbp_tpg *tpg = container_of(se_tpg, struct sbp_tpg, se_tpg);
+	struct sbp_tport *tport = tpg->tport;
+	return sprintf(page, "%d\n", tport->enable);
+}
+
+static ssize_t sbp_tpg_store_enable(
+		struct se_portal_group *se_tpg,
+		const char *page,
+		size_t count)
+{
+	struct sbp_tpg *tpg = container_of(se_tpg, struct sbp_tpg, se_tpg);
+	struct sbp_tport *tport = tpg->tport;
+	unsigned long val;
+	int ret;
+
+	if (kstrtoul(page, 0, &val) < 0)
+		return -EINVAL;
+	if ((val != 0) && (val != 1))
+		return -EINVAL;
+
+	if (tport->enable == val)
+		return count;
+
+	if (val) {
+		if (sbp_count_se_tpg_luns(&tpg->se_tpg) == 0) {
+			pr_err("Cannot enable a target with no LUNs!\n");
+			return -EINVAL;
+		}
+	} else {
+		/* XXX: force-shutdown sessions instead? */
+		spin_lock_bh(&se_tpg->session_lock);
+		if (!list_empty(&se_tpg->tpg_sess_list)) {
+			spin_unlock_bh(&se_tpg->session_lock);
+			return -EBUSY;
+		}
+		spin_unlock_bh(&se_tpg->session_lock);
+	}
+
+	tport->enable = val;
+
+	ret = sbp_update_unit_directory(tport);
+	if (ret < 0) {
+		pr_err("Could not update Config ROM\n");
+		return ret;
+	}
+
+	return count;
+}
+
+TF_TPG_BASE_ATTR(sbp, directory_id, S_IRUGO | S_IWUSR);
+TF_TPG_BASE_ATTR(sbp, enable, S_IRUGO | S_IWUSR);
+
+static struct configfs_attribute *sbp_tpg_base_attrs[] = {
+	&sbp_tpg_directory_id.attr,
+	&sbp_tpg_enable.attr,
+	NULL,
+};
+
+static ssize_t sbp_tpg_attrib_show_mgt_orb_timeout(
+		struct se_portal_group *se_tpg,
+		char *page)
+{
+	struct sbp_tpg *tpg = container_of(se_tpg, struct sbp_tpg, se_tpg);
+	struct sbp_tport *tport = tpg->tport;
+	return sprintf(page, "%d\n", tport->mgt_orb_timeout);
+}
+
+static ssize_t sbp_tpg_attrib_store_mgt_orb_timeout(
+		struct se_portal_group *se_tpg,
+		const char *page,
+		size_t count)
+{
+	struct sbp_tpg *tpg = container_of(se_tpg, struct sbp_tpg, se_tpg);
+	struct sbp_tport *tport = tpg->tport;
+	unsigned long val;
+	int ret;
+
+	if (kstrtoul(page, 0, &val) < 0)
+		return -EINVAL;
+	if ((val < 1) || (val > 127))
+		return -EINVAL;
+
+	if (tport->mgt_orb_timeout == val)
+		return count;
+
+	tport->mgt_orb_timeout = val;
+
+	ret = sbp_update_unit_directory(tport);
+	if (ret < 0)
+		return ret;
+
+	return count;
+}
+
+static ssize_t sbp_tpg_attrib_show_max_reconnect_timeout(
+		struct se_portal_group *se_tpg,
+		char *page)
+{
+	struct sbp_tpg *tpg = container_of(se_tpg, struct sbp_tpg, se_tpg);
+	struct sbp_tport *tport = tpg->tport;
+	return sprintf(page, "%d\n", tport->max_reconnect_timeout);
+}
+
+static ssize_t sbp_tpg_attrib_store_max_reconnect_timeout(
+		struct se_portal_group *se_tpg,
+		const char *page,
+		size_t count)
+{
+	struct sbp_tpg *tpg = container_of(se_tpg, struct sbp_tpg, se_tpg);
+	struct sbp_tport *tport = tpg->tport;
+	unsigned long val;
+	int ret;
+
+	if (kstrtoul(page, 0, &val) < 0)
+		return -EINVAL;
+	if ((val < 1) || (val > 32767))
+		return -EINVAL;
+
+	if (tport->max_reconnect_timeout == val)
+		return count;
+
+	tport->max_reconnect_timeout = val;
+
+	ret = sbp_update_unit_directory(tport);
+	if (ret < 0)
+		return ret;
+
+	return count;
+}
+
+static ssize_t sbp_tpg_attrib_show_max_logins_per_lun(
+		struct se_portal_group *se_tpg,
+		char *page)
+{
+	struct sbp_tpg *tpg = container_of(se_tpg, struct sbp_tpg, se_tpg);
+	struct sbp_tport *tport = tpg->tport;
+	return sprintf(page, "%d\n", tport->max_logins_per_lun);
+}
+
+static ssize_t sbp_tpg_attrib_store_max_logins_per_lun(
+		struct se_portal_group *se_tpg,
+		const char *page,
+		size_t count)
+{
+	struct sbp_tpg *tpg = container_of(se_tpg, struct sbp_tpg, se_tpg);
+	struct sbp_tport *tport = tpg->tport;
+	unsigned long val;
+
+	if (kstrtoul(page, 0, &val) < 0)
+		return -EINVAL;
+	if ((val < 1) || (val > 127))
+		return -EINVAL;
+
+	/* XXX: also check against current count? */
+
+	tport->max_logins_per_lun = val;
+
+	return count;
+}
+
+TF_TPG_ATTRIB_ATTR(sbp, mgt_orb_timeout, S_IRUGO | S_IWUSR);
+TF_TPG_ATTRIB_ATTR(sbp, max_reconnect_timeout, S_IRUGO | S_IWUSR);
+TF_TPG_ATTRIB_ATTR(sbp, max_logins_per_lun, S_IRUGO | S_IWUSR);
+
+static struct configfs_attribute *sbp_tpg_attrib_attrs[] = {
+	&sbp_tpg_attrib_mgt_orb_timeout.attr,
+	&sbp_tpg_attrib_max_reconnect_timeout.attr,
+	&sbp_tpg_attrib_max_logins_per_lun.attr,
+	NULL,
+};
+
+static struct target_core_fabric_ops sbp_ops = {
+	.get_fabric_name		= sbp_get_fabric_name,
+	.get_fabric_proto_ident		= sbp_get_fabric_proto_ident,
+	.tpg_get_wwn			= sbp_get_fabric_wwn,
+	.tpg_get_tag			= sbp_get_tag,
+	.tpg_get_default_depth		= sbp_get_default_depth,
+	.tpg_get_pr_transport_id	= sbp_get_pr_transport_id,
+	.tpg_get_pr_transport_id_len	= sbp_get_pr_transport_id_len,
+	.tpg_parse_pr_out_transport_id	= sbp_parse_pr_out_transport_id,
+	.tpg_check_demo_mode		= sbp_check_true,
+	.tpg_check_demo_mode_cache	= sbp_check_true,
+	.tpg_check_demo_mode_write_protect = sbp_check_false,
+	.tpg_check_prod_mode_write_protect = sbp_check_false,
+	.tpg_alloc_fabric_acl		= sbp_alloc_fabric_acl,
+	.tpg_release_fabric_acl		= sbp_release_fabric_acl,
+	.tpg_get_inst_index		= sbp_tpg_get_inst_index,
+	.release_cmd			= sbp_release_cmd,
+	.shutdown_session		= sbp_shutdown_session,
+	.close_session			= sbp_close_session,
+	.sess_get_index			= sbp_sess_get_index,
+	.write_pending			= sbp_write_pending,
+	.write_pending_status		= sbp_write_pending_status,
+	.set_default_node_attributes	= sbp_set_default_node_attrs,
+	.get_task_tag			= sbp_get_task_tag,
+	.get_cmd_state			= sbp_get_cmd_state,
+	.queue_data_in			= sbp_queue_data_in,
+	.queue_status			= sbp_queue_status,
+	.queue_tm_rsp			= sbp_queue_tm_rsp,
+	.get_fabric_sense_len		= sbp_get_fabric_sense_len,
+	.set_fabric_sense_len		= sbp_set_fabric_sense_len,
+	.check_stop_free		= sbp_check_stop_free,
+
+	.fabric_make_wwn		= sbp_make_tport,
+	.fabric_drop_wwn		= sbp_drop_tport,
+	.fabric_make_tpg		= sbp_make_tpg,
+	.fabric_drop_tpg		= sbp_drop_tpg,
+	.fabric_post_link		= sbp_post_link_lun,
+	.fabric_pre_unlink		= sbp_pre_unlink_lun,
+	.fabric_make_np			= NULL,
+	.fabric_drop_np			= NULL,
+	.fabric_make_nodeacl		= sbp_make_nodeacl,
+	.fabric_drop_nodeacl		= sbp_drop_nodeacl,
+};
+
+static int sbp_register_configfs(void)
+{
+	struct target_fabric_configfs *fabric;
+	int ret;
+
+	fabric = target_fabric_configfs_init(THIS_MODULE, "sbp");
+	if (!fabric) {
+		pr_err("target_fabric_configfs_init() failed\n");
+		return -ENOMEM;
+	}
+
+	fabric->tf_ops = sbp_ops;
+
+	/*
+	 * Setup default attribute lists for various fabric->tf_cit_tmpl
+	 */
+	TF_CIT_TMPL(fabric)->tfc_wwn_cit.ct_attrs = sbp_wwn_attrs;
+	TF_CIT_TMPL(fabric)->tfc_tpg_base_cit.ct_attrs = sbp_tpg_base_attrs;
+	TF_CIT_TMPL(fabric)->tfc_tpg_attrib_cit.ct_attrs = sbp_tpg_attrib_attrs;
+	TF_CIT_TMPL(fabric)->tfc_tpg_param_cit.ct_attrs = NULL;
+	TF_CIT_TMPL(fabric)->tfc_tpg_np_base_cit.ct_attrs = NULL;
+	TF_CIT_TMPL(fabric)->tfc_tpg_nacl_base_cit.ct_attrs = NULL;
+	TF_CIT_TMPL(fabric)->tfc_tpg_nacl_attrib_cit.ct_attrs = NULL;
+	TF_CIT_TMPL(fabric)->tfc_tpg_nacl_auth_cit.ct_attrs = NULL;
+	TF_CIT_TMPL(fabric)->tfc_tpg_nacl_param_cit.ct_attrs = NULL;
+
+	ret = target_fabric_configfs_register(fabric);
+	if (ret < 0) {
+		pr_err("target_fabric_configfs_register() failed for SBP\n");
+		return ret;
+	}
+
+	sbp_fabric_configfs = fabric;
+
+	return 0;
+};
+
+static void sbp_deregister_configfs(void)
+{
+	if (!sbp_fabric_configfs)
+		return;
+
+	target_fabric_configfs_deregister(sbp_fabric_configfs);
+	sbp_fabric_configfs = NULL;
+};
+
+static int __init sbp_init(void)
+{
+	int ret;
+
+	ret = sbp_register_configfs();
+	if (ret < 0)
+		return ret;
+
+	return 0;
+};
+
+static void sbp_exit(void)
+{
+	sbp_deregister_configfs();
+};
+
+MODULE_DESCRIPTION("FireWire SBP fabric driver");
+MODULE_LICENSE("GPL");
+module_init(sbp_init);
+module_exit(sbp_exit);
diff --git a/drivers/target/sbp/sbp_target.h b/drivers/target/sbp/sbp_target.h
new file mode 100644
index 00000000000..6d0d74a2c54
--- /dev/null
+++ b/drivers/target/sbp/sbp_target.h
@@ -0,0 +1,251 @@
+#ifndef _SBP_BASE_H
+#define _SBP_BASE_H
+
+#include <linux/firewire.h>
+#include <linux/spinlock.h>
+#include <linux/types.h>
+#include <linux/workqueue.h>
+#include <target/target_core_base.h>
+
+#define SBP_VERSION  "v0.1"
+#define SBP_NAMELEN 32
+
+#define SBP_ORB_FETCH_SIZE	8
+
+#define MANAGEMENT_AGENT_STATE_IDLE	0
+#define MANAGEMENT_AGENT_STATE_BUSY	1
+
+#define ORB_NOTIFY(v)			(((v) >> 31) & 0x01)
+#define ORB_REQUEST_FORMAT(v)		(((v) >> 29) & 0x03)
+
+#define MANAGEMENT_ORB_FUNCTION(v)	(((v) >> 16) & 0x0f)
+
+#define MANAGEMENT_ORB_FUNCTION_LOGIN			0x0
+#define MANAGEMENT_ORB_FUNCTION_QUERY_LOGINS		0x1
+#define MANAGEMENT_ORB_FUNCTION_RECONNECT		0x3
+#define MANAGEMENT_ORB_FUNCTION_SET_PASSWORD		0x4
+#define MANAGEMENT_ORB_FUNCTION_LOGOUT			0x7
+#define MANAGEMENT_ORB_FUNCTION_ABORT_TASK		0xb
+#define MANAGEMENT_ORB_FUNCTION_ABORT_TASK_SET		0xc
+#define MANAGEMENT_ORB_FUNCTION_LOGICAL_UNIT_RESET	0xe
+#define MANAGEMENT_ORB_FUNCTION_TARGET_RESET		0xf
+
+#define LOGIN_ORB_EXCLUSIVE(v)		(((v) >> 28) &   0x01)
+#define LOGIN_ORB_RESERVED(v)		(((v) >> 24) &   0x0f)
+#define LOGIN_ORB_RECONNECT(v)		(((v) >> 20) &   0x0f)
+#define LOGIN_ORB_LUN(v)		(((v) >>  0) & 0xffff)
+#define LOGIN_ORB_PASSWORD_LENGTH(v)	(((v) >> 16) & 0xffff)
+#define LOGIN_ORB_RESPONSE_LENGTH(v)	(((v) >>  0) & 0xffff)
+
+#define RECONNECT_ORB_LOGIN_ID(v)	(((v) >>  0) & 0xffff)
+#define LOGOUT_ORB_LOGIN_ID(v)		(((v) >>  0) & 0xffff)
+
+#define CMDBLK_ORB_DIRECTION(v)		(((v) >> 27) &   0x01)
+#define CMDBLK_ORB_SPEED(v)		(((v) >> 24) &   0x07)
+#define CMDBLK_ORB_MAX_PAYLOAD(v)	(((v) >> 20) &   0x0f)
+#define CMDBLK_ORB_PG_TBL_PRESENT(v)	(((v) >> 19) &   0x01)
+#define CMDBLK_ORB_PG_SIZE(v)		(((v) >> 16) &   0x07)
+#define CMDBLK_ORB_DATA_SIZE(v)		(((v) >>  0) & 0xffff)
+
+#define STATUS_BLOCK_SRC(v)		(((v) &   0x03) << 30)
+#define STATUS_BLOCK_RESP(v)		(((v) &   0x03) << 28)
+#define STATUS_BLOCK_DEAD(v)		(((v) ? 1 : 0)  << 27)
+#define STATUS_BLOCK_LEN(v)		(((v) &   0x07) << 24)
+#define STATUS_BLOCK_SBP_STATUS(v)	(((v) &   0xff) << 16)
+#define STATUS_BLOCK_ORB_OFFSET_HIGH(v)	(((v) & 0xffff) <<  0)
+
+#define STATUS_SRC_ORB_CONTINUING	0
+#define STATUS_SRC_ORB_FINISHED		1
+#define STATUS_SRC_UNSOLICITED		2
+
+#define STATUS_RESP_REQUEST_COMPLETE	0
+#define STATUS_RESP_TRANSPORT_FAILURE	1
+#define STATUS_RESP_ILLEGAL_REQUEST	2
+#define STATUS_RESP_VENDOR_DEPENDENT	3
+
+#define SBP_STATUS_OK			0
+#define SBP_STATUS_REQ_TYPE_NOTSUPP	1
+#define SBP_STATUS_SPEED_NOTSUPP	2
+#define SBP_STATUS_PAGE_SIZE_NOTSUPP	3
+#define SBP_STATUS_ACCESS_DENIED	4
+#define SBP_STATUS_LUN_NOTSUPP		5
+#define SBP_STATUS_PAYLOAD_TOO_SMALL	6
+/* 7 is reserved */
+#define SBP_STATUS_RESOURCES_UNAVAIL	8
+#define SBP_STATUS_FUNCTION_REJECTED	9
+#define SBP_STATUS_LOGIN_ID_UNKNOWN	10
+#define SBP_STATUS_DUMMY_ORB_COMPLETE	11
+#define SBP_STATUS_REQUEST_ABORTED	12
+#define SBP_STATUS_UNSPECIFIED_ERROR	0xff
+
+#define AGENT_STATE_RESET	0
+#define AGENT_STATE_ACTIVE	1
+#define AGENT_STATE_SUSPENDED	2
+#define AGENT_STATE_DEAD	3
+
+struct sbp2_pointer {
+	__be32 high;
+	__be32 low;
+};
+
+struct sbp_command_block_orb {
+	struct sbp2_pointer next_orb;
+	struct sbp2_pointer data_descriptor;
+	__be32 misc;
+	u8 command_block[12];
+};
+
+struct sbp_page_table_entry {
+	__be16 segment_length;
+	__be16 segment_base_hi;
+	__be32 segment_base_lo;
+};
+
+struct sbp_management_orb {
+	struct sbp2_pointer ptr1;
+	struct sbp2_pointer ptr2;
+	__be32 misc;
+	__be32 length;
+	struct sbp2_pointer status_fifo;
+};
+
+struct sbp_status_block {
+	__be32 status;
+	__be32 orb_low;
+	u8 data[24];
+};
+
+struct sbp_login_response_block {
+	__be32 misc;
+	struct sbp2_pointer command_block_agent;
+	__be32 reconnect_hold;
+};
+
+struct sbp_login_descriptor {
+	struct sbp_session *sess;
+	struct list_head link;
+
+	struct se_lun *lun;
+
+	u64 status_fifo_addr;
+	int exclusive;
+	u16 login_id;
+
+	struct sbp_target_agent *tgt_agt;
+};
+
+struct sbp_session {
+	spinlock_t lock;
+	struct se_session *se_sess;
+	struct list_head login_list;
+	struct delayed_work maint_work;
+
+	u64 guid; /* login_owner_EUI_64 */
+	int node_id; /* login_owner_ID */
+
+	struct fw_card *card;
+	int generation;
+	int speed;
+
+	int reconnect_hold;
+	u64 reconnect_expires;
+};
+
+struct sbp_nacl {
+	/* Initiator EUI-64 */
+	u64 guid;
+	/* ASCII formatted GUID for SBP Initiator port */
+	char iport_name[SBP_NAMELEN];
+	/* Returned by sbp_make_nodeacl() */
+	struct se_node_acl se_node_acl;
+};
+
+struct sbp_tpg {
+	/* Target portal group tag for TCM */
+	u16 tport_tpgt;
+	/* Pointer back to sbp_tport */
+	struct sbp_tport *tport;
+	/* Returned by sbp_make_tpg() */
+	struct se_portal_group se_tpg;
+};
+
+struct sbp_tport {
+	/* Target Unit Identifier (EUI-64) */
+	u64 guid;
+	/* Target port name */
+	char tport_name[SBP_NAMELEN];
+	/* Returned by sbp_make_tport() */
+	struct se_wwn tport_wwn;
+
+	struct sbp_tpg *tpg;
+
+	/* FireWire unit directory */
+	struct fw_descriptor unit_directory;
+
+	/* SBP Management Agent */
+	struct sbp_management_agent *mgt_agt;
+
+	/* Parameters */
+	int enable;
+	s32 directory_id;
+	int mgt_orb_timeout;
+	int max_reconnect_timeout;
+	int max_logins_per_lun;
+};
+
+static inline u64 sbp2_pointer_to_addr(const struct sbp2_pointer *ptr)
+{
+	return (u64)(be32_to_cpu(ptr->high) & 0x0000ffff) << 32 |
+		(be32_to_cpu(ptr->low) & 0xfffffffc);
+}
+
+static inline void addr_to_sbp2_pointer(u64 addr, struct sbp2_pointer *ptr)
+{
+	ptr->high = cpu_to_be32(addr >> 32);
+	ptr->low = cpu_to_be32(addr);
+}
+
+struct sbp_target_agent {
+	spinlock_t lock;
+	struct fw_address_handler handler;
+	struct sbp_login_descriptor *login;
+	int state;
+	struct work_struct work;
+	u64 orb_pointer;
+	bool doorbell;
+};
+
+struct sbp_target_request {
+	struct sbp_login_descriptor *login;
+	u64 orb_pointer;
+	struct sbp_command_block_orb orb;
+	struct sbp_status_block status;
+	struct work_struct work;
+
+	struct se_cmd se_cmd;
+	struct sbp_page_table_entry *pg_tbl;
+	void *cmd_buf;
+
+	unsigned char sense_buf[TRANSPORT_SENSE_BUFFER];
+};
+
+struct sbp_management_agent {
+	spinlock_t lock;
+	struct sbp_tport *tport;
+	struct fw_address_handler handler;
+	int state;
+	struct work_struct work;
+	u64 orb_offset;
+	struct sbp_management_request *request;
+};
+
+struct sbp_management_request {
+	struct sbp_management_orb orb;
+	struct sbp_status_block status;
+	struct fw_card *card;
+	int generation;
+	int node_addr;
+	int speed;
+};
+
+#endif
diff --git a/drivers/target/target_core_alua.c b/drivers/target/target_core_alua.c
index c7746a3339d..e624b836469 100644
--- a/drivers/target/target_core_alua.c
+++ b/drivers/target/target_core_alua.c
@@ -59,26 +59,31 @@ struct t10_alua_lu_gp *default_lu_gp;
  *
  * See spc4r17 section 6.27
  */
-int target_emulate_report_target_port_groups(struct se_task *task)
+int target_emulate_report_target_port_groups(struct se_cmd *cmd)
 {
-	struct se_cmd *cmd = task->task_se_cmd;
 	struct se_subsystem_dev *su_dev = cmd->se_dev->se_sub_dev;
 	struct se_port *port;
 	struct t10_alua_tg_pt_gp *tg_pt_gp;
 	struct t10_alua_tg_pt_gp_member *tg_pt_gp_mem;
 	unsigned char *buf;
-	u32 rd_len = 0, off = 4; /* Skip over RESERVED area to first
-				    Target port group descriptor */
+	u32 rd_len = 0, off;
+	int ext_hdr = (cmd->t_task_cdb[1] & 0x20);
 	/*
-	 * Need at least 4 bytes of response data or else we can't
-	 * even fit the return data length.
+	 * Skip over RESERVED area to first Target port group descriptor
+	 * depending on the PARAMETER DATA FORMAT type..
 	 */
-	if (cmd->data_length < 4) {
-		pr_warn("REPORT TARGET PORT GROUPS allocation length %u"
-			" too small\n", cmd->data_length);
+	if (ext_hdr != 0)
+		off = 8;
+	else
+		off = 4;
+
+	if (cmd->data_length < off) {
+		pr_warn("REPORT TARGET PORT GROUPS allocation length %u too"
+			" small for %s header\n", cmd->data_length,
+			(ext_hdr) ? "extended" : "normal");
+		cmd->scsi_sense_reason = TCM_INVALID_CDB_FIELD;
 		return -EINVAL;
 	}
-
 	buf = transport_kmap_data_sg(cmd);
 
 	spin_lock(&su_dev->t10_alua.tg_pt_gps_lock);
@@ -159,15 +164,34 @@ int target_emulate_report_target_port_groups(struct se_task *task)
 	/*
 	 * Set the RETURN DATA LENGTH set in the header of the DataIN Payload
 	 */
-	buf[0] = ((rd_len >> 24) & 0xff);
-	buf[1] = ((rd_len >> 16) & 0xff);
-	buf[2] = ((rd_len >> 8) & 0xff);
-	buf[3] = (rd_len & 0xff);
+	put_unaligned_be32(rd_len, &buf[0]);
 
+	/*
+	 * Fill in the Extended header parameter data format if requested
+	 */
+	if (ext_hdr != 0) {
+		buf[4] = 0x10;
+		/*
+		 * Set the implict transition time (in seconds) for the application
+		 * client to use as a base for it's transition timeout value.
+		 *
+		 * Use the current tg_pt_gp_mem -> tg_pt_gp membership from the LUN
+		 * this CDB was received upon to determine this value individually
+		 * for ALUA target port group.
+		 */
+		port = cmd->se_lun->lun_sep;
+		tg_pt_gp_mem = port->sep_alua_tg_pt_gp_mem;
+		if (tg_pt_gp_mem) {
+			spin_lock(&tg_pt_gp_mem->tg_pt_gp_mem_lock);
+			tg_pt_gp = tg_pt_gp_mem->tg_pt_gp;
+			if (tg_pt_gp)
+				buf[5] = tg_pt_gp->tg_pt_gp_implict_trans_secs;
+			spin_unlock(&tg_pt_gp_mem->tg_pt_gp_mem_lock);
+		}
+	}
 	transport_kunmap_data_sg(cmd);
 
-	task->task_scsi_status = GOOD;
-	transport_complete_task(task, 1);
+	target_complete_cmd(cmd, GOOD);
 	return 0;
 }
 
@@ -176,9 +200,8 @@ int target_emulate_report_target_port_groups(struct se_task *task)
  *
  * See spc4r17 section 6.35
  */
-int target_emulate_set_target_port_groups(struct se_task *task)
+int target_emulate_set_target_port_groups(struct se_cmd *cmd)
 {
-	struct se_cmd *cmd = task->task_se_cmd;
 	struct se_device *dev = cmd->se_dev;
 	struct se_subsystem_dev *su_dev = dev->se_sub_dev;
 	struct se_port *port, *l_port = cmd->se_lun->lun_sep;
@@ -351,8 +374,7 @@ int target_emulate_set_target_port_groups(struct se_task *task)
 
 out:
 	transport_kunmap_data_sg(cmd);
-	task->task_scsi_status = GOOD;
-	transport_complete_task(task, 1);
+	target_complete_cmd(cmd, GOOD);
 	return 0;
 }
 
@@ -391,7 +413,7 @@ static inline int core_alua_state_standby(
 	case RECEIVE_DIAGNOSTIC:
 	case SEND_DIAGNOSTIC:
 	case MAINTENANCE_IN:
-		switch (cdb[1]) {
+		switch (cdb[1] & 0x1f) {
 		case MI_REPORT_TARGET_PGS:
 			return 0;
 		default:
@@ -433,7 +455,7 @@ static inline int core_alua_state_unavailable(
 	case INQUIRY:
 	case REPORT_LUNS:
 	case MAINTENANCE_IN:
-		switch (cdb[1]) {
+		switch (cdb[1] & 0x1f) {
 		case MI_REPORT_TARGET_PGS:
 			return 0;
 		default:
@@ -473,7 +495,7 @@ static inline int core_alua_state_transition(
 	case INQUIRY:
 	case REPORT_LUNS:
 	case MAINTENANCE_IN:
-		switch (cdb[1]) {
+		switch (cdb[1] & 0x1f) {
 		case MI_REPORT_TARGET_PGS:
 			return 0;
 		default:
@@ -1359,6 +1381,7 @@ struct t10_alua_tg_pt_gp *core_alua_allocate_tg_pt_gp(
 	 */
 	tg_pt_gp->tg_pt_gp_nonop_delay_msecs = ALUA_DEFAULT_NONOP_DELAY_MSECS;
 	tg_pt_gp->tg_pt_gp_trans_delay_msecs = ALUA_DEFAULT_TRANS_DELAY_MSECS;
+	tg_pt_gp->tg_pt_gp_implict_trans_secs = ALUA_DEFAULT_IMPLICT_TRANS_SECS;
 
 	if (def_group) {
 		spin_lock(&su_dev->t10_alua.tg_pt_gps_lock);
@@ -1855,6 +1878,37 @@ ssize_t core_alua_store_trans_delay_msecs(
 	return count;
 }
 
+ssize_t core_alua_show_implict_trans_secs(
+	struct t10_alua_tg_pt_gp *tg_pt_gp,
+	char *page)
+{
+	return sprintf(page, "%d\n", tg_pt_gp->tg_pt_gp_implict_trans_secs);
+}
+
+ssize_t core_alua_store_implict_trans_secs(
+	struct t10_alua_tg_pt_gp *tg_pt_gp,
+	const char *page,
+	size_t count)
+{
+	unsigned long tmp;
+	int ret;
+
+	ret = strict_strtoul(page, 0, &tmp);
+	if (ret < 0) {
+		pr_err("Unable to extract implict_trans_secs\n");
+		return -EINVAL;
+	}
+	if (tmp > ALUA_MAX_IMPLICT_TRANS_SECS) {
+		pr_err("Passed implict_trans_secs: %lu, exceeds"
+			" ALUA_MAX_IMPLICT_TRANS_SECS: %d\n", tmp,
+			ALUA_MAX_IMPLICT_TRANS_SECS);
+		return  -EINVAL;
+	}
+	tg_pt_gp->tg_pt_gp_implict_trans_secs = (int)tmp;
+
+	return count;
+}
+
 ssize_t core_alua_show_preferred_bit(
 	struct t10_alua_tg_pt_gp *tg_pt_gp,
 	char *page)
diff --git a/drivers/target/target_core_alua.h b/drivers/target/target_core_alua.h
index c5b4ecd3e74..f920c170d47 100644
--- a/drivers/target/target_core_alua.h
+++ b/drivers/target/target_core_alua.h
@@ -52,6 +52,12 @@
 #define ALUA_DEFAULT_TRANS_DELAY_MSECS			0
 #define ALUA_MAX_TRANS_DELAY_MSECS			30000 /* 30 seconds */
 /*
+ * Used for the recommended application client implict transition timeout
+ * in seconds, returned by the REPORT_TARGET_PORT_GROUPS w/ extended header.
+ */
+#define ALUA_DEFAULT_IMPLICT_TRANS_SECS			0
+#define ALUA_MAX_IMPLICT_TRANS_SECS			255
+/*
  * Used by core_alua_update_tpg_primary_metadata() and
  * core_alua_update_tpg_secondary_metadata()
  */
@@ -66,8 +72,8 @@ extern struct kmem_cache *t10_alua_lu_gp_mem_cache;
 extern struct kmem_cache *t10_alua_tg_pt_gp_cache;
 extern struct kmem_cache *t10_alua_tg_pt_gp_mem_cache;
 
-extern int target_emulate_report_target_port_groups(struct se_task *);
-extern int target_emulate_set_target_port_groups(struct se_task *);
+extern int target_emulate_report_target_port_groups(struct se_cmd *);
+extern int target_emulate_set_target_port_groups(struct se_cmd *);
 extern int core_alua_check_nonop_delay(struct se_cmd *);
 extern int core_alua_do_port_transition(struct t10_alua_tg_pt_gp *,
 				struct se_device *, struct se_port *,
@@ -107,6 +113,10 @@ extern ssize_t core_alua_show_trans_delay_msecs(struct t10_alua_tg_pt_gp *,
 					char *);
 extern ssize_t core_alua_store_trans_delay_msecs(struct t10_alua_tg_pt_gp *,
 					const char *, size_t);
+extern ssize_t core_alua_show_implict_trans_secs(struct t10_alua_tg_pt_gp *,
+					char *);
+extern ssize_t core_alua_store_implict_trans_secs(struct t10_alua_tg_pt_gp *,
+					const char *, size_t);
 extern ssize_t core_alua_show_preferred_bit(struct t10_alua_tg_pt_gp *,
 					char *);
 extern ssize_t core_alua_store_preferred_bit(struct t10_alua_tg_pt_gp *,
diff --git a/drivers/target/target_core_cdb.c b/drivers/target/target_core_cdb.c
index 30a67707036..9888693a18f 100644
--- a/drivers/target/target_core_cdb.c
+++ b/drivers/target/target_core_cdb.c
@@ -432,6 +432,7 @@ static int
 target_emulate_evpd_b0(struct se_cmd *cmd, unsigned char *buf)
 {
 	struct se_device *dev = cmd->se_dev;
+	u32 max_sectors;
 	int have_tp = 0;
 
 	/*
@@ -456,7 +457,9 @@ target_emulate_evpd_b0(struct se_cmd *cmd, unsigned char *buf)
 	/*
 	 * Set MAXIMUM TRANSFER LENGTH
 	 */
-	put_unaligned_be32(dev->se_sub_dev->se_dev_attrib.fabric_max_sectors, &buf[8]);
+	max_sectors = min(dev->se_sub_dev->se_dev_attrib.fabric_max_sectors,
+			  dev->se_sub_dev->se_dev_attrib.hw_max_sectors);
+	put_unaligned_be32(max_sectors, &buf[8]);
 
 	/*
 	 * Set OPTIMAL TRANSFER LENGTH
@@ -598,9 +601,8 @@ target_emulate_evpd_00(struct se_cmd *cmd, unsigned char *buf)
 	return 0;
 }
 
-int target_emulate_inquiry(struct se_task *task)
+int target_emulate_inquiry(struct se_cmd *cmd)
 {
-	struct se_cmd *cmd = task->task_se_cmd;
 	struct se_device *dev = cmd->se_dev;
 	struct se_portal_group *tpg = cmd->se_lun->lun_sep->sep_tpg;
 	unsigned char *buf, *map_buf;
@@ -664,16 +666,13 @@ out:
 	}
 	transport_kunmap_data_sg(cmd);
 
-	if (!ret) {
-		task->task_scsi_status = GOOD;
-		transport_complete_task(task, 1);
-	}
+	if (!ret)
+		target_complete_cmd(cmd, GOOD);
 	return ret;
 }
 
-int target_emulate_readcapacity(struct se_task *task)
+int target_emulate_readcapacity(struct se_cmd *cmd)
 {
-	struct se_cmd *cmd = task->task_se_cmd;
 	struct se_device *dev = cmd->se_dev;
 	unsigned char *buf;
 	unsigned long long blocks_long = dev->transport->get_blocks(dev);
@@ -697,14 +696,12 @@ int target_emulate_readcapacity(struct se_task *task)
 
 	transport_kunmap_data_sg(cmd);
 
-	task->task_scsi_status = GOOD;
-	transport_complete_task(task, 1);
+	target_complete_cmd(cmd, GOOD);
 	return 0;
 }
 
-int target_emulate_readcapacity_16(struct se_task *task)
+int target_emulate_readcapacity_16(struct se_cmd *cmd)
 {
-	struct se_cmd *cmd = task->task_se_cmd;
 	struct se_device *dev = cmd->se_dev;
 	unsigned char *buf;
 	unsigned long long blocks = dev->transport->get_blocks(dev);
@@ -732,8 +729,7 @@ int target_emulate_readcapacity_16(struct se_task *task)
 
 	transport_kunmap_data_sg(cmd);
 
-	task->task_scsi_status = GOOD;
-	transport_complete_task(task, 1);
+	target_complete_cmd(cmd, GOOD);
 	return 0;
 }
 
@@ -872,9 +868,8 @@ target_modesense_dpofua(unsigned char *buf, int type)
 	}
 }
 
-int target_emulate_modesense(struct se_task *task)
+int target_emulate_modesense(struct se_cmd *cmd)
 {
-	struct se_cmd *cmd = task->task_se_cmd;
 	struct se_device *dev = cmd->se_dev;
 	char *cdb = cmd->t_task_cdb;
 	unsigned char *rbuf;
@@ -947,14 +942,12 @@ int target_emulate_modesense(struct se_task *task)
 	memcpy(rbuf, buf, offset);
 	transport_kunmap_data_sg(cmd);
 
-	task->task_scsi_status = GOOD;
-	transport_complete_task(task, 1);
+	target_complete_cmd(cmd, GOOD);
 	return 0;
 }
 
-int target_emulate_request_sense(struct se_task *task)
+int target_emulate_request_sense(struct se_cmd *cmd)
 {
-	struct se_cmd *cmd = task->task_se_cmd;
 	unsigned char *cdb = cmd->t_task_cdb;
 	unsigned char *buf;
 	u8 ua_asc = 0, ua_ascq = 0;
@@ -1008,8 +1001,7 @@ int target_emulate_request_sense(struct se_task *task)
 
 end:
 	transport_kunmap_data_sg(cmd);
-	task->task_scsi_status = GOOD;
-	transport_complete_task(task, 1);
+	target_complete_cmd(cmd, GOOD);
 	return 0;
 }
 
@@ -1017,9 +1009,8 @@ end:
  * Used for TCM/IBLOCK and TCM/FILEIO for block/blk-lib.c level discard support.
  * Note this is not used for TCM/pSCSI passthrough
  */
-int target_emulate_unmap(struct se_task *task)
+int target_emulate_unmap(struct se_cmd *cmd)
 {
-	struct se_cmd *cmd = task->task_se_cmd;
 	struct se_device *dev = cmd->se_dev;
 	unsigned char *buf, *ptr = NULL;
 	unsigned char *cdb = &cmd->t_task_cdb[0];
@@ -1066,10 +1057,8 @@ int target_emulate_unmap(struct se_task *task)
 
 err:
 	transport_kunmap_data_sg(cmd);
-	if (!ret) {
-		task->task_scsi_status = GOOD;
-		transport_complete_task(task, 1);
-	}
+	if (!ret)
+		target_complete_cmd(cmd, GOOD);
 	return ret;
 }
 
@@ -1077,9 +1066,8 @@ err:
  * Used for TCM/IBLOCK and TCM/FILEIO for block/blk-lib.c level discard support.
  * Note this is not used for TCM/pSCSI passthrough
  */
-int target_emulate_write_same(struct se_task *task)
+int target_emulate_write_same(struct se_cmd *cmd)
 {
-	struct se_cmd *cmd = task->task_se_cmd;
 	struct se_device *dev = cmd->se_dev;
 	sector_t range;
 	sector_t lba = cmd->t_task_lba;
@@ -1118,79 +1106,25 @@ int target_emulate_write_same(struct se_task *task)
 		return ret;
 	}
 
-	task->task_scsi_status = GOOD;
-	transport_complete_task(task, 1);
+	target_complete_cmd(cmd, GOOD);
 	return 0;
 }
 
-int target_emulate_synchronize_cache(struct se_task *task)
+int target_emulate_synchronize_cache(struct se_cmd *cmd)
 {
-	struct se_device *dev = task->task_se_cmd->se_dev;
-	struct se_cmd *cmd = task->task_se_cmd;
-
-	if (!dev->transport->do_sync_cache) {
+	if (!cmd->se_dev->transport->do_sync_cache) {
 		pr_err("SYNCHRONIZE_CACHE emulation not supported"
-			" for: %s\n", dev->transport->name);
+			" for: %s\n", cmd->se_dev->transport->name);
 		cmd->scsi_sense_reason = TCM_UNSUPPORTED_SCSI_OPCODE;
 		return -ENOSYS;
 	}
 
-	dev->transport->do_sync_cache(task);
+	cmd->se_dev->transport->do_sync_cache(cmd);
 	return 0;
 }
 
-int target_emulate_noop(struct se_task *task)
+int target_emulate_noop(struct se_cmd *cmd)
 {
-	task->task_scsi_status = GOOD;
-	transport_complete_task(task, 1);
+	target_complete_cmd(cmd, GOOD);
 	return 0;
 }
-
-/*
- * Write a CDB into @cdb that is based on the one the intiator sent us,
- * but updated to only cover the sectors that the current task handles.
- */
-void target_get_task_cdb(struct se_task *task, unsigned char *cdb)
-{
-	struct se_cmd *cmd = task->task_se_cmd;
-	unsigned int cdb_len = scsi_command_size(cmd->t_task_cdb);
-
-	memcpy(cdb, cmd->t_task_cdb, cdb_len);
-	if (cmd->se_cmd_flags & SCF_SCSI_DATA_SG_IO_CDB) {
-		unsigned long long lba = task->task_lba;
-		u32 sectors = task->task_sectors;
-
-		switch (cdb_len) {
-		case 6:
-			/* 21-bit LBA and 8-bit sectors */
-			cdb[1] = (lba >> 16) & 0x1f;
-			cdb[2] = (lba >> 8) & 0xff;
-			cdb[3] = lba & 0xff;
-			cdb[4] = sectors & 0xff;
-			break;
-		case 10:
-			/* 32-bit LBA and 16-bit sectors */
-			put_unaligned_be32(lba, &cdb[2]);
-			put_unaligned_be16(sectors, &cdb[7]);
-			break;
-		case 12:
-			/* 32-bit LBA and 32-bit sectors */
-			put_unaligned_be32(lba, &cdb[2]);
-			put_unaligned_be32(sectors, &cdb[6]);
-			break;
-		case 16:
-			/* 64-bit LBA and 32-bit sectors */
-			put_unaligned_be64(lba, &cdb[2]);
-			put_unaligned_be32(sectors, &cdb[10]);
-			break;
-		case 32:
-			/* 64-bit LBA and 32-bit sectors, extended CDB */
-			put_unaligned_be64(lba, &cdb[12]);
-			put_unaligned_be32(sectors, &cdb[28]);
-			break;
-		default:
-			BUG();
-		}
-	}
-}
-EXPORT_SYMBOL(target_get_task_cdb);
diff --git a/drivers/target/target_core_configfs.c b/drivers/target/target_core_configfs.c
index cbb66537d23..801efa89204 100644
--- a/drivers/target/target_core_configfs.c
+++ b/drivers/target/target_core_configfs.c
@@ -683,9 +683,6 @@ SE_DEV_ATTR(block_size, S_IRUGO | S_IWUSR);
 DEF_DEV_ATTRIB_RO(hw_max_sectors);
 SE_DEV_ATTR_RO(hw_max_sectors);
 
-DEF_DEV_ATTRIB(max_sectors);
-SE_DEV_ATTR(max_sectors, S_IRUGO | S_IWUSR);
-
 DEF_DEV_ATTRIB(fabric_max_sectors);
 SE_DEV_ATTR(fabric_max_sectors, S_IRUGO | S_IWUSR);
 
@@ -727,7 +724,6 @@ static struct configfs_attribute *target_core_dev_attrib_attrs[] = {
 	&target_core_dev_attrib_hw_block_size.attr,
 	&target_core_dev_attrib_block_size.attr,
 	&target_core_dev_attrib_hw_max_sectors.attr,
-	&target_core_dev_attrib_max_sectors.attr,
 	&target_core_dev_attrib_fabric_max_sectors.attr,
 	&target_core_dev_attrib_optimal_sectors.attr,
 	&target_core_dev_attrib_hw_queue_depth.attr,
@@ -2451,6 +2447,26 @@ static ssize_t target_core_alua_tg_pt_gp_store_attr_trans_delay_msecs(
 SE_DEV_ALUA_TG_PT_ATTR(trans_delay_msecs, S_IRUGO | S_IWUSR);
 
 /*
+ * implict_trans_secs
+ */
+static ssize_t target_core_alua_tg_pt_gp_show_attr_implict_trans_secs(
+	struct t10_alua_tg_pt_gp *tg_pt_gp,
+	char *page)
+{
+	return core_alua_show_implict_trans_secs(tg_pt_gp, page);
+}
+
+static ssize_t target_core_alua_tg_pt_gp_store_attr_implict_trans_secs(
+	struct t10_alua_tg_pt_gp *tg_pt_gp,
+	const char *page,
+	size_t count)
+{
+	return core_alua_store_implict_trans_secs(tg_pt_gp, page, count);
+}
+
+SE_DEV_ALUA_TG_PT_ATTR(implict_trans_secs, S_IRUGO | S_IWUSR);
+
+/*
  * preferred
  */
 
@@ -2574,6 +2590,7 @@ static struct configfs_attribute *target_core_alua_tg_pt_gp_attrs[] = {
 	&target_core_alua_tg_pt_gp_alua_write_metadata.attr,
 	&target_core_alua_tg_pt_gp_nonop_delay_msecs.attr,
 	&target_core_alua_tg_pt_gp_trans_delay_msecs.attr,
+	&target_core_alua_tg_pt_gp_implict_trans_secs.attr,
 	&target_core_alua_tg_pt_gp_preferred.attr,
 	&target_core_alua_tg_pt_gp_tg_pt_gp_id.attr,
 	&target_core_alua_tg_pt_gp_members.attr,
diff --git a/drivers/target/target_core_device.c b/drivers/target/target_core_device.c
index aa626774638..5ad972856a8 100644
--- a/drivers/target/target_core_device.c
+++ b/drivers/target/target_core_device.c
@@ -643,9 +643,8 @@ void core_dev_unexport(
 	lun->lun_se_dev = NULL;
 }
 
-int target_report_luns(struct se_task *se_task)
+int target_report_luns(struct se_cmd *se_cmd)
 {
-	struct se_cmd *se_cmd = se_task->task_se_cmd;
 	struct se_dev_entry *deve;
 	struct se_session *se_sess = se_cmd->se_sess;
 	unsigned char *buf;
@@ -696,8 +695,7 @@ done:
 	buf[3] = (lun_count & 0xff);
 	transport_kunmap_data_sg(se_cmd);
 
-	se_task->task_scsi_status = GOOD;
-	transport_complete_task(se_task, 1);
+	target_complete_cmd(se_cmd, GOOD);
 	return 0;
 }
 
@@ -878,15 +876,12 @@ void se_dev_set_default_attribs(
 	dev->se_sub_dev->se_dev_attrib.hw_block_size = limits->logical_block_size;
 	dev->se_sub_dev->se_dev_attrib.block_size = limits->logical_block_size;
 	/*
-	 * max_sectors is based on subsystem plugin dependent requirements.
+	 * Align max_hw_sectors down to PAGE_SIZE I/O transfers
 	 */
-	dev->se_sub_dev->se_dev_attrib.hw_max_sectors = limits->max_hw_sectors;
-	/*
-	 * Align max_sectors down to PAGE_SIZE to follow transport_allocate_data_tasks()
-	 */
-	limits->max_sectors = se_dev_align_max_sectors(limits->max_sectors,
+	limits->max_hw_sectors = se_dev_align_max_sectors(limits->max_hw_sectors,
 						limits->logical_block_size);
-	dev->se_sub_dev->se_dev_attrib.max_sectors = limits->max_sectors;
+	dev->se_sub_dev->se_dev_attrib.hw_max_sectors = limits->max_hw_sectors;
+
 	/*
 	 * Set fabric_max_sectors, which is reported in block limits
 	 * VPD page (B0h).
@@ -1170,64 +1165,6 @@ int se_dev_set_queue_depth(struct se_device *dev, u32 queue_depth)
 	return 0;
 }
 
-int se_dev_set_max_sectors(struct se_device *dev, u32 max_sectors)
-{
-	int force = 0; /* Force setting for VDEVS */
-
-	if (atomic_read(&dev->dev_export_obj.obj_access_count)) {
-		pr_err("dev[%p]: Unable to change SE Device"
-			" max_sectors while dev_export_obj: %d count exists\n",
-			dev, atomic_read(&dev->dev_export_obj.obj_access_count));
-		return -EINVAL;
-	}
-	if (!max_sectors) {
-		pr_err("dev[%p]: Illegal ZERO value for"
-			" max_sectors\n", dev);
-		return -EINVAL;
-	}
-	if (max_sectors < DA_STATUS_MAX_SECTORS_MIN) {
-		pr_err("dev[%p]: Passed max_sectors: %u less than"
-			" DA_STATUS_MAX_SECTORS_MIN: %u\n", dev, max_sectors,
-				DA_STATUS_MAX_SECTORS_MIN);
-		return -EINVAL;
-	}
-	if (dev->transport->transport_type == TRANSPORT_PLUGIN_PHBA_PDEV) {
-		if (max_sectors > dev->se_sub_dev->se_dev_attrib.hw_max_sectors) {
-			pr_err("dev[%p]: Passed max_sectors: %u"
-				" greater than TCM/SE_Device max_sectors:"
-				" %u\n", dev, max_sectors,
-				dev->se_sub_dev->se_dev_attrib.hw_max_sectors);
-			 return -EINVAL;
-		}
-	} else {
-		if (!force && (max_sectors >
-				 dev->se_sub_dev->se_dev_attrib.hw_max_sectors)) {
-			pr_err("dev[%p]: Passed max_sectors: %u"
-				" greater than TCM/SE_Device max_sectors"
-				": %u, use force=1 to override.\n", dev,
-				max_sectors, dev->se_sub_dev->se_dev_attrib.hw_max_sectors);
-			return -EINVAL;
-		}
-		if (max_sectors > DA_STATUS_MAX_SECTORS_MAX) {
-			pr_err("dev[%p]: Passed max_sectors: %u"
-				" greater than DA_STATUS_MAX_SECTORS_MAX:"
-				" %u\n", dev, max_sectors,
-				DA_STATUS_MAX_SECTORS_MAX);
-			return -EINVAL;
-		}
-	}
-	/*
-	 * Align max_sectors down to PAGE_SIZE to follow transport_allocate_data_tasks()
-	 */
-	max_sectors = se_dev_align_max_sectors(max_sectors,
-				dev->se_sub_dev->se_dev_attrib.block_size);
-
-	dev->se_sub_dev->se_dev_attrib.max_sectors = max_sectors;
-	pr_debug("dev[%p]: SE Device max_sectors changed to %u\n",
-			dev, max_sectors);
-	return 0;
-}
-
 int se_dev_set_fabric_max_sectors(struct se_device *dev, u32 fabric_max_sectors)
 {
 	if (atomic_read(&dev->dev_export_obj.obj_access_count)) {
@@ -1341,7 +1278,6 @@ struct se_lun *core_dev_add_lun(
 	u32 lun)
 {
 	struct se_lun *lun_p;
-	u32 lun_access = 0;
 	int rc;
 
 	if (atomic_read(&dev->dev_access_obj.obj_access_count) != 0) {
@@ -1354,12 +1290,8 @@ struct se_lun *core_dev_add_lun(
 	if (IS_ERR(lun_p))
 		return lun_p;
 
-	if (dev->dev_flags & DF_READ_ONLY)
-		lun_access = TRANSPORT_LUNFLAGS_READ_ONLY;
-	else
-		lun_access = TRANSPORT_LUNFLAGS_READ_WRITE;
-
-	rc = core_tpg_post_addlun(tpg, lun_p, lun_access, dev);
+	rc = core_tpg_post_addlun(tpg, lun_p,
+				TRANSPORT_LUNFLAGS_READ_WRITE, dev);
 	if (rc < 0)
 		return ERR_PTR(rc);
 
diff --git a/drivers/target/target_core_file.c b/drivers/target/target_core_file.c
index 7ed58e2df79..686dba189f8 100644
--- a/drivers/target/target_core_file.c
+++ b/drivers/target/target_core_file.c
@@ -133,15 +133,10 @@ static struct se_device *fd_create_virtdevice(
 		ret = PTR_ERR(dev_p);
 		goto fail;
 	}
-#if 0
-	if (di->no_create_file)
-		flags = O_RDWR | O_LARGEFILE;
-	else
-		flags = O_RDWR | O_CREAT | O_LARGEFILE;
-#else
+
+	/* O_DIRECT too? */
 	flags = O_RDWR | O_CREAT | O_LARGEFILE;
-#endif
-/*	flags |= O_DIRECT; */
+
 	/*
 	 * If fd_buffered_io=1 has not been set explicitly (the default),
 	 * use O_SYNC to force FILEIO writes to disk.
@@ -169,6 +164,7 @@ static struct se_device *fd_create_virtdevice(
 	inode = file->f_mapping->host;
 	if (S_ISBLK(inode->i_mode)) {
 		struct request_queue *q;
+		unsigned long long dev_size;
 		/*
 		 * Setup the local scope queue_limits from struct request_queue->limits
 		 * to pass into transport_add_device_to_core_hba() as struct se_dev_limits.
@@ -183,13 +179,12 @@ static struct se_device *fd_create_virtdevice(
 		 * one (1) logical sector from underlying struct block_device
 		 */
 		fd_dev->fd_block_size = bdev_logical_block_size(inode->i_bdev);
-		fd_dev->fd_dev_size = (i_size_read(file->f_mapping->host) -
+		dev_size = (i_size_read(file->f_mapping->host) -
 				       fd_dev->fd_block_size);
 
 		pr_debug("FILEIO: Using size: %llu bytes from struct"
 			" block_device blocks: %llu logical_block_size: %d\n",
-			fd_dev->fd_dev_size,
-			div_u64(fd_dev->fd_dev_size, fd_dev->fd_block_size),
+			dev_size, div_u64(dev_size, fd_dev->fd_block_size),
 			fd_dev->fd_block_size);
 	} else {
 		if (!(fd_dev->fbd_flags & FBDF_HAS_SIZE)) {
@@ -249,53 +244,33 @@ static void fd_free_device(void *p)
 	kfree(fd_dev);
 }
 
-static inline struct fd_request *FILE_REQ(struct se_task *task)
+static int fd_do_readv(struct se_cmd *cmd, struct scatterlist *sgl,
+		u32 sgl_nents)
 {
-	return container_of(task, struct fd_request, fd_task);
-}
-
-
-static struct se_task *
-fd_alloc_task(unsigned char *cdb)
-{
-	struct fd_request *fd_req;
-
-	fd_req = kzalloc(sizeof(struct fd_request), GFP_KERNEL);
-	if (!fd_req) {
-		pr_err("Unable to allocate struct fd_request\n");
-		return NULL;
-	}
-
-	return &fd_req->fd_task;
-}
-
-static int fd_do_readv(struct se_task *task)
-{
-	struct fd_request *req = FILE_REQ(task);
-	struct se_device *se_dev = req->fd_task.task_se_cmd->se_dev;
+	struct se_device *se_dev = cmd->se_dev;
 	struct fd_dev *dev = se_dev->dev_ptr;
 	struct file *fd = dev->fd_file;
-	struct scatterlist *sg = task->task_sg;
+	struct scatterlist *sg;
 	struct iovec *iov;
 	mm_segment_t old_fs;
-	loff_t pos = (task->task_lba *
+	loff_t pos = (cmd->t_task_lba *
 		      se_dev->se_sub_dev->se_dev_attrib.block_size);
 	int ret = 0, i;
 
-	iov = kzalloc(sizeof(struct iovec) * task->task_sg_nents, GFP_KERNEL);
+	iov = kzalloc(sizeof(struct iovec) * sgl_nents, GFP_KERNEL);
 	if (!iov) {
 		pr_err("Unable to allocate fd_do_readv iov[]\n");
 		return -ENOMEM;
 	}
 
-	for_each_sg(task->task_sg, sg, task->task_sg_nents, i) {
+	for_each_sg(sgl, sg, sgl_nents, i) {
 		iov[i].iov_len = sg->length;
 		iov[i].iov_base = sg_virt(sg);
 	}
 
 	old_fs = get_fs();
 	set_fs(get_ds());
-	ret = vfs_readv(fd, &iov[0], task->task_sg_nents, &pos);
+	ret = vfs_readv(fd, &iov[0], sgl_nents, &pos);
 	set_fs(old_fs);
 
 	kfree(iov);
@@ -305,10 +280,10 @@ static int fd_do_readv(struct se_task *task)
 	 * block_device.
 	 */
 	if (S_ISBLK(fd->f_dentry->d_inode->i_mode)) {
-		if (ret < 0 || ret != task->task_size) {
+		if (ret < 0 || ret != cmd->data_length) {
 			pr_err("vfs_readv() returned %d,"
 				" expecting %d for S_ISBLK\n", ret,
-				(int)task->task_size);
+				(int)cmd->data_length);
 			return (ret < 0 ? ret : -EINVAL);
 		}
 	} else {
@@ -322,38 +297,38 @@ static int fd_do_readv(struct se_task *task)
 	return 1;
 }
 
-static int fd_do_writev(struct se_task *task)
+static int fd_do_writev(struct se_cmd *cmd, struct scatterlist *sgl,
+		u32 sgl_nents)
 {
-	struct fd_request *req = FILE_REQ(task);
-	struct se_device *se_dev = req->fd_task.task_se_cmd->se_dev;
+	struct se_device *se_dev = cmd->se_dev;
 	struct fd_dev *dev = se_dev->dev_ptr;
 	struct file *fd = dev->fd_file;
-	struct scatterlist *sg = task->task_sg;
+	struct scatterlist *sg;
 	struct iovec *iov;
 	mm_segment_t old_fs;
-	loff_t pos = (task->task_lba *
+	loff_t pos = (cmd->t_task_lba *
 		      se_dev->se_sub_dev->se_dev_attrib.block_size);
 	int ret, i = 0;
 
-	iov = kzalloc(sizeof(struct iovec) * task->task_sg_nents, GFP_KERNEL);
+	iov = kzalloc(sizeof(struct iovec) * sgl_nents, GFP_KERNEL);
 	if (!iov) {
 		pr_err("Unable to allocate fd_do_writev iov[]\n");
 		return -ENOMEM;
 	}
 
-	for_each_sg(task->task_sg, sg, task->task_sg_nents, i) {
+	for_each_sg(sgl, sg, sgl_nents, i) {
 		iov[i].iov_len = sg->length;
 		iov[i].iov_base = sg_virt(sg);
 	}
 
 	old_fs = get_fs();
 	set_fs(get_ds());
-	ret = vfs_writev(fd, &iov[0], task->task_sg_nents, &pos);
+	ret = vfs_writev(fd, &iov[0], sgl_nents, &pos);
 	set_fs(old_fs);
 
 	kfree(iov);
 
-	if (ret < 0 || ret != task->task_size) {
+	if (ret < 0 || ret != cmd->data_length) {
 		pr_err("vfs_writev() returned %d\n", ret);
 		return (ret < 0 ? ret : -EINVAL);
 	}
@@ -361,9 +336,8 @@ static int fd_do_writev(struct se_task *task)
 	return 1;
 }
 
-static void fd_emulate_sync_cache(struct se_task *task)
+static void fd_emulate_sync_cache(struct se_cmd *cmd)
 {
-	struct se_cmd *cmd = task->task_se_cmd;
 	struct se_device *dev = cmd->se_dev;
 	struct fd_dev *fd_dev = dev->dev_ptr;
 	int immed = (cmd->t_task_cdb[1] & 0x2);
@@ -375,7 +349,7 @@ static void fd_emulate_sync_cache(struct se_task *task)
 	 * for this SYNCHRONIZE_CACHE op
 	 */
 	if (immed)
-		transport_complete_sync_cache(cmd, 1);
+		target_complete_cmd(cmd, SAM_STAT_GOOD);
 
 	/*
 	 * Determine if we will be flushing the entire device.
@@ -395,33 +369,37 @@ static void fd_emulate_sync_cache(struct se_task *task)
 	if (ret != 0)
 		pr_err("FILEIO: vfs_fsync_range() failed: %d\n", ret);
 
-	if (!immed)
-		transport_complete_sync_cache(cmd, ret == 0);
+	if (immed)
+		return;
+
+	if (ret) {
+		cmd->scsi_sense_reason = TCM_LOGICAL_UNIT_COMMUNICATION_FAILURE;
+		target_complete_cmd(cmd, SAM_STAT_CHECK_CONDITION);
+	} else {
+		target_complete_cmd(cmd, SAM_STAT_GOOD);
+	}
 }
 
-/*
- * WRITE Force Unit Access (FUA) emulation on a per struct se_task
- * LBA range basis..
- */
-static void fd_emulate_write_fua(struct se_cmd *cmd, struct se_task *task)
+static void fd_emulate_write_fua(struct se_cmd *cmd)
 {
 	struct se_device *dev = cmd->se_dev;
 	struct fd_dev *fd_dev = dev->dev_ptr;
-	loff_t start = task->task_lba * dev->se_sub_dev->se_dev_attrib.block_size;
-	loff_t end = start + task->task_size;
+	loff_t start = cmd->t_task_lba *
+		dev->se_sub_dev->se_dev_attrib.block_size;
+	loff_t end = start + cmd->data_length;
 	int ret;
 
 	pr_debug("FILEIO: FUA WRITE LBA: %llu, bytes: %u\n",
-			task->task_lba, task->task_size);
+		cmd->t_task_lba, cmd->data_length);
 
 	ret = vfs_fsync_range(fd_dev->fd_file, start, end, 1);
 	if (ret != 0)
 		pr_err("FILEIO: vfs_fsync_range() failed: %d\n", ret);
 }
 
-static int fd_do_task(struct se_task *task)
+static int fd_execute_cmd(struct se_cmd *cmd, struct scatterlist *sgl,
+		u32 sgl_nents, enum dma_data_direction data_direction)
 {
-	struct se_cmd *cmd = task->task_se_cmd;
 	struct se_device *dev = cmd->se_dev;
 	int ret = 0;
 
@@ -429,10 +407,10 @@ static int fd_do_task(struct se_task *task)
 	 * Call vectorized fileio functions to map struct scatterlist
 	 * physical memory addresses to struct iovec virtual memory.
 	 */
-	if (task->task_data_direction == DMA_FROM_DEVICE) {
-		ret = fd_do_readv(task);
+	if (data_direction == DMA_FROM_DEVICE) {
+		ret = fd_do_readv(cmd, sgl, sgl_nents);
 	} else {
-		ret = fd_do_writev(task);
+		ret = fd_do_writev(cmd, sgl, sgl_nents);
 
 		if (ret > 0 &&
 		    dev->se_sub_dev->se_dev_attrib.emulate_write_cache > 0 &&
@@ -443,7 +421,7 @@ static int fd_do_task(struct se_task *task)
 			 * and return some sense data to let the initiator
 			 * know the FUA WRITE cache sync failed..?
 			 */
-			fd_emulate_write_fua(cmd, task);
+			fd_emulate_write_fua(cmd);
 		}
 
 	}
@@ -452,24 +430,11 @@ static int fd_do_task(struct se_task *task)
 		cmd->scsi_sense_reason = TCM_LOGICAL_UNIT_COMMUNICATION_FAILURE;
 		return ret;
 	}
-	if (ret) {
-		task->task_scsi_status = GOOD;
-		transport_complete_task(task, 1);
-	}
+	if (ret)
+		target_complete_cmd(cmd, SAM_STAT_GOOD);
 	return 0;
 }
 
-/*	fd_free_task(): (Part of se_subsystem_api_t template)
- *
- *
- */
-static void fd_free_task(struct se_task *task)
-{
-	struct fd_request *req = FILE_REQ(task);
-
-	kfree(req);
-}
-
 enum {
 	Opt_fd_dev_name, Opt_fd_dev_size, Opt_fd_buffered_io, Opt_err
 };
@@ -605,10 +570,20 @@ static u32 fd_get_device_type(struct se_device *dev)
 static sector_t fd_get_blocks(struct se_device *dev)
 {
 	struct fd_dev *fd_dev = dev->dev_ptr;
-	unsigned long long blocks_long = div_u64(fd_dev->fd_dev_size,
-			dev->se_sub_dev->se_dev_attrib.block_size);
+	struct file *f = fd_dev->fd_file;
+	struct inode *i = f->f_mapping->host;
+	unsigned long long dev_size;
+	/*
+	 * When using a file that references an underlying struct block_device,
+	 * ensure dev_size is always based on the current inode size in order
+	 * to handle underlying block_device resize operations.
+	 */
+	if (S_ISBLK(i->i_mode))
+		dev_size = (i_size_read(i) - fd_dev->fd_block_size);
+	else
+		dev_size = fd_dev->fd_dev_size;
 
-	return blocks_long;
+	return div_u64(dev_size, dev->se_sub_dev->se_dev_attrib.block_size);
 }
 
 static struct se_subsystem_api fileio_template = {
@@ -622,10 +597,8 @@ static struct se_subsystem_api fileio_template = {
 	.allocate_virtdevice	= fd_allocate_virtdevice,
 	.create_virtdevice	= fd_create_virtdevice,
 	.free_device		= fd_free_device,
-	.alloc_task		= fd_alloc_task,
-	.do_task		= fd_do_task,
+	.execute_cmd		= fd_execute_cmd,
 	.do_sync_cache		= fd_emulate_sync_cache,
-	.free_task		= fd_free_task,
 	.check_configfs_dev_params = fd_check_configfs_dev_params,
 	.set_configfs_dev_params = fd_set_configfs_dev_params,
 	.show_configfs_dev_params = fd_show_configfs_dev_params,
diff --git a/drivers/target/target_core_file.h b/drivers/target/target_core_file.h
index 59e6e73106c..fbd59ef7d8b 100644
--- a/drivers/target/target_core_file.h
+++ b/drivers/target/target_core_file.h
@@ -12,10 +12,6 @@
 #define RRF_EMULATE_CDB		0x01
 #define RRF_GOT_LBA		0x02
 
-struct fd_request {
-	struct se_task	fd_task;
-};
-
 #define FBDF_HAS_PATH		0x01
 #define FBDF_HAS_SIZE		0x02
 #define FDBD_USE_BUFFERED_IO	0x04
diff --git a/drivers/target/target_core_iblock.c b/drivers/target/target_core_iblock.c
index 2ec299e8a73..fd47950727b 100644
--- a/drivers/target/target_core_iblock.c
+++ b/drivers/target/target_core_iblock.c
@@ -189,26 +189,6 @@ static void iblock_free_device(void *p)
 	kfree(ib_dev);
 }
 
-static inline struct iblock_req *IBLOCK_REQ(struct se_task *task)
-{
-	return container_of(task, struct iblock_req, ib_task);
-}
-
-static struct se_task *
-iblock_alloc_task(unsigned char *cdb)
-{
-	struct iblock_req *ib_req;
-
-	ib_req = kzalloc(sizeof(struct iblock_req), GFP_KERNEL);
-	if (!ib_req) {
-		pr_err("Unable to allocate memory for struct iblock_req\n");
-		return NULL;
-	}
-
-	atomic_set(&ib_req->pending, 1);
-	return &ib_req->ib_task;
-}
-
 static unsigned long long iblock_emulate_read_cap_with_block_size(
 	struct se_device *dev,
 	struct block_device *bd,
@@ -295,8 +275,16 @@ static void iblock_end_io_flush(struct bio *bio, int err)
 	if (err)
 		pr_err("IBLOCK: cache flush failed: %d\n", err);
 
-	if (cmd)
-		transport_complete_sync_cache(cmd, err == 0);
+	if (cmd) {
+		if (err) {
+			cmd->scsi_sense_reason =
+				TCM_LOGICAL_UNIT_COMMUNICATION_FAILURE;
+			target_complete_cmd(cmd, SAM_STAT_CHECK_CONDITION);
+		} else {
+			target_complete_cmd(cmd, SAM_STAT_GOOD);
+		}
+	}
+
 	bio_put(bio);
 }
 
@@ -304,9 +292,8 @@ static void iblock_end_io_flush(struct bio *bio, int err)
  * Implement SYCHRONIZE CACHE.  Note that we can't handle lba ranges and must
  * always flush the whole cache.
  */
-static void iblock_emulate_sync_cache(struct se_task *task)
+static void iblock_emulate_sync_cache(struct se_cmd *cmd)
 {
-	struct se_cmd *cmd = task->task_se_cmd;
 	struct iblock_dev *ib_dev = cmd->se_dev->dev_ptr;
 	int immed = (cmd->t_task_cdb[1] & 0x2);
 	struct bio *bio;
@@ -316,7 +303,7 @@ static void iblock_emulate_sync_cache(struct se_task *task)
 	 * for this SYNCHRONIZE_CACHE op.
 	 */
 	if (immed)
-		transport_complete_sync_cache(cmd, 1);
+		target_complete_cmd(cmd, SAM_STAT_GOOD);
 
 	bio = bio_alloc(GFP_KERNEL, 0);
 	bio->bi_end_io = iblock_end_io_flush;
@@ -335,11 +322,6 @@ static int iblock_do_discard(struct se_device *dev, sector_t lba, u32 range)
 	return blkdev_issue_discard(bd, lba, range, GFP_KERNEL, barrier);
 }
 
-static void iblock_free_task(struct se_task *task)
-{
-	kfree(IBLOCK_REQ(task));
-}
-
 enum {
 	Opt_udev_path, Opt_force, Opt_err
 };
@@ -448,19 +430,35 @@ static ssize_t iblock_show_configfs_dev_params(
 	return bl;
 }
 
+static void iblock_complete_cmd(struct se_cmd *cmd)
+{
+	struct iblock_req *ibr = cmd->priv;
+	u8 status;
+
+	if (!atomic_dec_and_test(&ibr->pending))
+		return;
+
+	if (atomic_read(&ibr->ib_bio_err_cnt))
+		status = SAM_STAT_CHECK_CONDITION;
+	else
+		status = SAM_STAT_GOOD;
+
+	target_complete_cmd(cmd, status);
+	kfree(ibr);
+}
+
 static void iblock_bio_destructor(struct bio *bio)
 {
-	struct se_task *task = bio->bi_private;
-	struct iblock_dev *ib_dev = task->task_se_cmd->se_dev->dev_ptr;
+	struct se_cmd *cmd = bio->bi_private;
+	struct iblock_dev *ib_dev = cmd->se_dev->dev_ptr;
 
 	bio_free(bio, ib_dev->ibd_bio_set);
 }
 
 static struct bio *
-iblock_get_bio(struct se_task *task, sector_t lba, u32 sg_num)
+iblock_get_bio(struct se_cmd *cmd, sector_t lba, u32 sg_num)
 {
-	struct iblock_dev *ib_dev = task->task_se_cmd->se_dev->dev_ptr;
-	struct iblock_req *ib_req = IBLOCK_REQ(task);
+	struct iblock_dev *ib_dev = cmd->se_dev->dev_ptr;
 	struct bio *bio;
 
 	/*
@@ -476,19 +474,11 @@ iblock_get_bio(struct se_task *task, sector_t lba, u32 sg_num)
 		return NULL;
 	}
 
-	pr_debug("Allocated bio: %p task_sg_nents: %u using ibd_bio_set:"
-		" %p\n", bio, task->task_sg_nents, ib_dev->ibd_bio_set);
-	pr_debug("Allocated bio: %p task_size: %u\n", bio, task->task_size);
-
 	bio->bi_bdev = ib_dev->ibd_bd;
-	bio->bi_private = task;
+	bio->bi_private = cmd;
 	bio->bi_destructor = iblock_bio_destructor;
 	bio->bi_end_io = &iblock_bio_done;
 	bio->bi_sector = lba;
-	atomic_inc(&ib_req->pending);
-
-	pr_debug("Set bio->bi_sector: %llu\n", (unsigned long long)bio->bi_sector);
-	pr_debug("Set ib_req->pending: %d\n", atomic_read(&ib_req->pending));
 	return bio;
 }
 
@@ -503,20 +493,21 @@ static void iblock_submit_bios(struct bio_list *list, int rw)
 	blk_finish_plug(&plug);
 }
 
-static int iblock_do_task(struct se_task *task)
+static int iblock_execute_cmd(struct se_cmd *cmd, struct scatterlist *sgl,
+		u32 sgl_nents, enum dma_data_direction data_direction)
 {
-	struct se_cmd *cmd = task->task_se_cmd;
 	struct se_device *dev = cmd->se_dev;
-	struct iblock_req *ibr = IBLOCK_REQ(task);
+	struct iblock_req *ibr;
 	struct bio *bio;
 	struct bio_list list;
 	struct scatterlist *sg;
-	u32 i, sg_num = task->task_sg_nents;
+	u32 sg_num = sgl_nents;
 	sector_t block_lba;
 	unsigned bio_cnt;
 	int rw;
+	int i;
 
-	if (task->task_data_direction == DMA_TO_DEVICE) {
+	if (data_direction == DMA_TO_DEVICE) {
 		/*
 		 * Force data to disk if we pretend to not have a volatile
 		 * write cache, or the initiator set the Force Unit Access bit.
@@ -532,17 +523,17 @@ static int iblock_do_task(struct se_task *task)
 	}
 
 	/*
-	 * Do starting conversion up from non 512-byte blocksize with
-	 * struct se_task SCSI blocksize into Linux/Block 512 units for BIO.
+	 * Convert the blocksize advertised to the initiator to the 512 byte
+	 * units unconditionally used by the Linux block layer.
 	 */
 	if (dev->se_sub_dev->se_dev_attrib.block_size == 4096)
-		block_lba = (task->task_lba << 3);
+		block_lba = (cmd->t_task_lba << 3);
 	else if (dev->se_sub_dev->se_dev_attrib.block_size == 2048)
-		block_lba = (task->task_lba << 2);
+		block_lba = (cmd->t_task_lba << 2);
 	else if (dev->se_sub_dev->se_dev_attrib.block_size == 1024)
-		block_lba = (task->task_lba << 1);
+		block_lba = (cmd->t_task_lba << 1);
 	else if (dev->se_sub_dev->se_dev_attrib.block_size == 512)
-		block_lba = task->task_lba;
+		block_lba = cmd->t_task_lba;
 	else {
 		pr_err("Unsupported SCSI -> BLOCK LBA conversion:"
 				" %u\n", dev->se_sub_dev->se_dev_attrib.block_size);
@@ -550,17 +541,22 @@ static int iblock_do_task(struct se_task *task)
 		return -ENOSYS;
 	}
 
-	bio = iblock_get_bio(task, block_lba, sg_num);
-	if (!bio) {
-		cmd->scsi_sense_reason = TCM_LOGICAL_UNIT_COMMUNICATION_FAILURE;
-		return -ENOMEM;
-	}
+	ibr = kzalloc(sizeof(struct iblock_req), GFP_KERNEL);
+	if (!ibr)
+		goto fail;
+	cmd->priv = ibr;
+
+	bio = iblock_get_bio(cmd, block_lba, sgl_nents);
+	if (!bio)
+		goto fail_free_ibr;
 
 	bio_list_init(&list);
 	bio_list_add(&list, bio);
+
+	atomic_set(&ibr->pending, 2);
 	bio_cnt = 1;
 
-	for_each_sg(task->task_sg, sg, task->task_sg_nents, i) {
+	for_each_sg(sgl, sg, sgl_nents, i) {
 		/*
 		 * XXX: if the length the device accepts is shorter than the
 		 *	length of the S/G list entry this will cause and
@@ -573,9 +569,11 @@ static int iblock_do_task(struct se_task *task)
 				bio_cnt = 0;
 			}
 
-			bio = iblock_get_bio(task, block_lba, sg_num);
+			bio = iblock_get_bio(cmd, block_lba, sg_num);
 			if (!bio)
-				goto fail;
+				goto fail_put_bios;
+
+			atomic_inc(&ibr->pending);
 			bio_list_add(&list, bio);
 			bio_cnt++;
 		}
@@ -586,17 +584,16 @@ static int iblock_do_task(struct se_task *task)
 	}
 
 	iblock_submit_bios(&list, rw);
-
-	if (atomic_dec_and_test(&ibr->pending)) {
-		transport_complete_task(task,
-				!atomic_read(&ibr->ib_bio_err_cnt));
-	}
+	iblock_complete_cmd(cmd);
 	return 0;
 
-fail:
+fail_put_bios:
 	while ((bio = bio_list_pop(&list)))
 		bio_put(bio);
+fail_free_ibr:
+	kfree(ibr);
 	cmd->scsi_sense_reason = TCM_LOGICAL_UNIT_COMMUNICATION_FAILURE;
+fail:
 	return -ENOMEM;
 }
 
@@ -621,8 +618,8 @@ static sector_t iblock_get_blocks(struct se_device *dev)
 
 static void iblock_bio_done(struct bio *bio, int err)
 {
-	struct se_task *task = bio->bi_private;
-	struct iblock_req *ibr = IBLOCK_REQ(task);
+	struct se_cmd *cmd = bio->bi_private;
+	struct iblock_req *ibr = cmd->priv;
 
 	/*
 	 * Set -EIO if !BIO_UPTODATE and the passed is still err=0
@@ -642,14 +639,7 @@ static void iblock_bio_done(struct bio *bio, int err)
 
 	bio_put(bio);
 
-	if (!atomic_dec_and_test(&ibr->pending))
-		return;
-
-	pr_debug("done[%p] bio: %p task_lba: %llu bio_lba: %llu err=%d\n",
-		 task, bio, task->task_lba,
-		 (unsigned long long)bio->bi_sector, err);
-
-	transport_complete_task(task, !atomic_read(&ibr->ib_bio_err_cnt));
+	iblock_complete_cmd(cmd);
 }
 
 static struct se_subsystem_api iblock_template = {
@@ -663,11 +653,9 @@ static struct se_subsystem_api iblock_template = {
 	.allocate_virtdevice	= iblock_allocate_virtdevice,
 	.create_virtdevice	= iblock_create_virtdevice,
 	.free_device		= iblock_free_device,
-	.alloc_task		= iblock_alloc_task,
-	.do_task		= iblock_do_task,
+	.execute_cmd		= iblock_execute_cmd,
 	.do_discard		= iblock_do_discard,
 	.do_sync_cache		= iblock_emulate_sync_cache,
-	.free_task		= iblock_free_task,
 	.check_configfs_dev_params = iblock_check_configfs_dev_params,
 	.set_configfs_dev_params = iblock_set_configfs_dev_params,
 	.show_configfs_dev_params = iblock_show_configfs_dev_params,
diff --git a/drivers/target/target_core_iblock.h b/drivers/target/target_core_iblock.h
index e929370b6fd..66cf7b9e205 100644
--- a/drivers/target/target_core_iblock.h
+++ b/drivers/target/target_core_iblock.h
@@ -7,7 +7,6 @@
 #define IBLOCK_LBA_SHIFT	9
 
 struct iblock_req {
-	struct se_task ib_task;
 	atomic_t pending;
 	atomic_t ib_bio_err_cnt;
 } ____cacheline_aligned;
diff --git a/drivers/target/target_core_internal.h b/drivers/target/target_core_internal.h
index 21c05638f15..165e8242968 100644
--- a/drivers/target/target_core_internal.h
+++ b/drivers/target/target_core_internal.h
@@ -5,15 +5,15 @@
 extern struct t10_alua_lu_gp *default_lu_gp;
 
 /* target_core_cdb.c */
-int	target_emulate_inquiry(struct se_task *task);
-int	target_emulate_readcapacity(struct se_task *task);
-int	target_emulate_readcapacity_16(struct se_task *task);
-int	target_emulate_modesense(struct se_task *task);
-int	target_emulate_request_sense(struct se_task *task);
-int	target_emulate_unmap(struct se_task *task);
-int	target_emulate_write_same(struct se_task *task);
-int	target_emulate_synchronize_cache(struct se_task *task);
-int	target_emulate_noop(struct se_task *task);
+int	target_emulate_inquiry(struct se_cmd *cmd);
+int	target_emulate_readcapacity(struct se_cmd *cmd);
+int	target_emulate_readcapacity_16(struct se_cmd *cmd);
+int	target_emulate_modesense(struct se_cmd *cmd);
+int	target_emulate_request_sense(struct se_cmd *cmd);
+int	target_emulate_unmap(struct se_cmd *cmd);
+int	target_emulate_write_same(struct se_cmd *cmd);
+int	target_emulate_synchronize_cache(struct se_cmd *cmd);
+int	target_emulate_noop(struct se_cmd *cmd);
 
 /* target_core_device.c */
 struct se_dev_entry *core_get_se_deve_from_rtpi(struct se_node_acl *, u16);
@@ -28,7 +28,7 @@ int	core_dev_export(struct se_device *, struct se_portal_group *,
 		struct se_lun *);
 void	core_dev_unexport(struct se_device *, struct se_portal_group *,
 		struct se_lun *);
-int	target_report_luns(struct se_task *);
+int	target_report_luns(struct se_cmd *);
 void	se_release_device_for_hba(struct se_device *);
 void	se_release_vpd_for_dev(struct se_device *);
 int	se_free_virtual_device(struct se_device *, struct se_hba *);
@@ -104,8 +104,7 @@ void	release_se_kmem_caches(void);
 u32	scsi_get_new_index(scsi_index_t);
 void	transport_subsystem_check_init(void);
 void	transport_cmd_finish_abort(struct se_cmd *, int);
-void	__transport_remove_task_from_execute_queue(struct se_task *,
-		struct se_device *);
+void	__target_remove_from_execute_list(struct se_cmd *);
 unsigned char *transport_dump_cmd_direction(struct se_cmd *);
 void	transport_dump_dev_state(struct se_device *, char *, int *);
 void	transport_dump_dev_info(struct se_device *, struct se_lun *,
@@ -114,7 +113,7 @@ void	transport_dump_vpd_proto_id(struct t10_vpd *, unsigned char *, int);
 int	transport_dump_vpd_assoc(struct t10_vpd *, unsigned char *, int);
 int	transport_dump_vpd_ident_type(struct t10_vpd *, unsigned char *, int);
 int	transport_dump_vpd_ident(struct t10_vpd *, unsigned char *, int);
-bool	target_stop_task(struct se_task *task, unsigned long *flags);
+bool	target_stop_cmd(struct se_cmd *cmd, unsigned long *flags);
 int	transport_clear_lun_from_sessions(struct se_lun *);
 void	transport_send_task_abort(struct se_cmd *);
 
diff --git a/drivers/target/target_core_pr.c b/drivers/target/target_core_pr.c
index 86f0c3b5d50..85564998500 100644
--- a/drivers/target/target_core_pr.c
+++ b/drivers/target/target_core_pr.c
@@ -193,9 +193,8 @@ static int target_check_scsi2_reservation_conflict(struct se_cmd *cmd)
 	return 0;
 }
 
-int target_scsi2_reservation_release(struct se_task *task)
+int target_scsi2_reservation_release(struct se_cmd *cmd)
 {
-	struct se_cmd *cmd = task->task_se_cmd;
 	struct se_device *dev = cmd->se_dev;
 	struct se_session *sess = cmd->se_sess;
 	struct se_portal_group *tpg = sess->se_tpg;
@@ -220,6 +219,9 @@ int target_scsi2_reservation_release(struct se_task *task)
 	if (dev->dev_reserved_node_acl != sess->se_node_acl)
 		goto out_unlock;
 
+	if (dev->dev_res_bin_isid != sess->sess_bin_isid)
+		goto out_unlock;
+
 	dev->dev_reserved_node_acl = NULL;
 	dev->dev_flags &= ~DF_SPC2_RESERVATIONS;
 	if (dev->dev_flags & DF_SPC2_RESERVATIONS_WITH_ISID) {
@@ -234,16 +236,13 @@ int target_scsi2_reservation_release(struct se_task *task)
 out_unlock:
 	spin_unlock(&dev->dev_reservation_lock);
 out:
-	if (!ret) {
-		task->task_scsi_status = GOOD;
-		transport_complete_task(task, 1);
-	}
+	if (!ret)
+		target_complete_cmd(cmd, GOOD);
 	return ret;
 }
 
-int target_scsi2_reservation_reserve(struct se_task *task)
+int target_scsi2_reservation_reserve(struct se_cmd *cmd)
 {
-	struct se_cmd *cmd = task->task_se_cmd;
 	struct se_device *dev = cmd->se_dev;
 	struct se_session *sess = cmd->se_sess;
 	struct se_portal_group *tpg = sess->se_tpg;
@@ -304,10 +303,8 @@ int target_scsi2_reservation_reserve(struct se_task *task)
 out_unlock:
 	spin_unlock(&dev->dev_reservation_lock);
 out:
-	if (!ret) {
-		task->task_scsi_status = GOOD;
-		transport_complete_task(task, 1);
-	}
+	if (!ret)
+		target_complete_cmd(cmd, GOOD);
 	return ret;
 }
 
@@ -500,11 +497,10 @@ static int core_scsi3_pr_seq_non_holder(
 	 * statement.
 	 */
 	if (!ret && !other_cdb) {
-#if 0
 		pr_debug("Allowing explict CDB: 0x%02x for %s"
 			" reservation holder\n", cdb[0],
 			core_scsi3_pr_dump_type(pr_reg_type));
-#endif
+
 		return ret;
 	}
 	/*
@@ -532,14 +528,14 @@ static int core_scsi3_pr_seq_non_holder(
 			 * as we expect registered non-reservation holding
 			 * nexuses to issue CDBs.
 			 */
-#if 0
+
 			if (!registered_nexus) {
 				pr_debug("Allowing implict CDB: 0x%02x"
 					" for %s reservation on unregistered"
 					" nexus\n", cdb[0],
 					core_scsi3_pr_dump_type(pr_reg_type));
 			}
-#endif
+
 			return 0;
 		}
 	} else if ((reg_only) || (all_reg)) {
@@ -548,11 +544,11 @@ static int core_scsi3_pr_seq_non_holder(
 			 * For PR_*_REG_ONLY and PR_*_ALL_REG reservations,
 			 * allow commands from registered nexuses.
 			 */
-#if 0
+
 			pr_debug("Allowing implict CDB: 0x%02x for %s"
 				" reservation\n", cdb[0],
 				core_scsi3_pr_dump_type(pr_reg_type));
-#endif
+
 			return 0;
 		}
 	}
@@ -1666,12 +1662,12 @@ static int core_scsi3_decode_spec_i_port(
 			ret = -EINVAL;
 			goto out;
 		}
-#if 0
+
 		pr_debug("SPC-3 PR SPEC_I_PT: Got %s data_length: %u tpdl: %u"
 			" tid_len: %d for %s + %s\n",
 			dest_tpg->se_tpg_tfo->get_fabric_name(), cmd->data_length,
 			tpdl, tid_len, i_str, iport_ptr);
-#endif
+
 		if (tid_len > tpdl) {
 			pr_err("SPC-3 PR SPEC_I_PT: Illegal tid_len:"
 				" %u for Transport ID: %s\n", tid_len, ptr);
@@ -1714,12 +1710,12 @@ static int core_scsi3_decode_spec_i_port(
 			ret = -EINVAL;
 			goto out;
 		}
-#if 0
+
 		pr_debug("SPC-3 PR SPEC_I_PT: Located %s Node: %s"
 			" dest_se_deve mapped_lun: %u\n",
 			dest_tpg->se_tpg_tfo->get_fabric_name(),
 			dest_node_acl->initiatorname, dest_se_deve->mapped_lun);
-#endif
+
 		/*
 		 * Skip any TransportIDs that already have a registration for
 		 * this target port.
@@ -3473,10 +3469,10 @@ static int core_scsi3_emulate_pro_register_and_move(
 
 	buf = transport_kmap_data_sg(cmd);
 	proto_ident = (buf[24] & 0x0f);
-#if 0
+
 	pr_debug("SPC-3 PR REGISTER_AND_MOVE: Extracted Protocol Identifier:"
 			" 0x%02x\n", proto_ident);
-#endif
+
 	if (proto_ident != dest_tf_ops->get_fabric_proto_ident(dest_se_tpg)) {
 		pr_err("SPC-3 PR REGISTER_AND_MOVE: Received"
 			" proto_ident: 0x%02x does not match ident: 0x%02x"
@@ -3575,11 +3571,11 @@ after_iport_check:
 		ret = -EINVAL;
 		goto out;
 	}
-#if 0
+
 	pr_debug("SPC-3 PR REGISTER_AND_MOVE: Found %s dest_node_acl:"
 		" %s from TransportID\n", dest_tf_ops->get_fabric_name(),
 		dest_node_acl->initiatorname);
-#endif
+
 	/*
 	 * Locate the struct se_dev_entry pointer for the matching RELATIVE TARGET
 	 * PORT IDENTIFIER.
@@ -3603,12 +3599,12 @@ after_iport_check:
 		ret = -EINVAL;
 		goto out;
 	}
-#if 0
+
 	pr_debug("SPC-3 PR REGISTER_AND_MOVE: Located %s node %s LUN"
 		" ACL for dest_se_deve->mapped_lun: %u\n",
 		dest_tf_ops->get_fabric_name(), dest_node_acl->initiatorname,
 		dest_se_deve->mapped_lun);
-#endif
+
 	/*
 	 * A persistent reservation needs to already existing in order to
 	 * successfully complete the REGISTER_AND_MOVE service action..
@@ -3799,9 +3795,8 @@ static unsigned long long core_scsi3_extract_reservation_key(unsigned char *cdb)
 /*
  * See spc4r17 section 6.14 Table 170
  */
-int target_scsi3_emulate_pr_out(struct se_task *task)
+int target_scsi3_emulate_pr_out(struct se_cmd *cmd)
 {
-	struct se_cmd *cmd = task->task_se_cmd;
 	unsigned char *cdb = &cmd->t_task_cdb[0];
 	unsigned char *buf;
 	u64 res_key, sa_res_key;
@@ -3941,10 +3936,8 @@ int target_scsi3_emulate_pr_out(struct se_task *task)
 	}
 
 out:
-	if (!ret) {
-		task->task_scsi_status = GOOD;
-		transport_complete_task(task, 1);
-	}
+	if (!ret)
+		target_complete_cmd(cmd, GOOD);
 	return ret;
 }
 
@@ -4299,9 +4292,8 @@ static int core_scsi3_pri_read_full_status(struct se_cmd *cmd)
 	return 0;
 }
 
-int target_scsi3_emulate_pr_in(struct se_task *task)
+int target_scsi3_emulate_pr_in(struct se_cmd *cmd)
 {
-	struct se_cmd *cmd = task->task_se_cmd;
 	int ret;
 
 	/*
@@ -4342,10 +4334,8 @@ int target_scsi3_emulate_pr_in(struct se_task *task)
 		break;
 	}
 
-	if (!ret) {
-		task->task_scsi_status = GOOD;
-		transport_complete_task(task, 1);
-	}
+	if (!ret)
+		target_complete_cmd(cmd, GOOD);
 	return ret;
 }
 
diff --git a/drivers/target/target_core_pr.h b/drivers/target/target_core_pr.h
index 7a233feb7e9..af6c460d886 100644
--- a/drivers/target/target_core_pr.h
+++ b/drivers/target/target_core_pr.h
@@ -47,8 +47,8 @@ extern struct kmem_cache *t10_pr_reg_cache;
 
 extern int core_pr_dump_initiator_port(struct t10_pr_registration *,
 			char *, u32);
-extern int target_scsi2_reservation_release(struct se_task *task);
-extern int target_scsi2_reservation_reserve(struct se_task *task);
+extern int target_scsi2_reservation_release(struct se_cmd *);
+extern int target_scsi2_reservation_reserve(struct se_cmd *);
 extern int core_scsi3_alloc_aptpl_registration(
 			struct t10_reservation *, u64,
 			unsigned char *, unsigned char *, u32,
@@ -61,8 +61,8 @@ extern void core_scsi3_free_pr_reg_from_nacl(struct se_device *,
 extern void core_scsi3_free_all_registrations(struct se_device *);
 extern unsigned char *core_scsi3_pr_dump_type(int);
 
-extern int target_scsi3_emulate_pr_in(struct se_task *task);
-extern int target_scsi3_emulate_pr_out(struct se_task *task);
+extern int target_scsi3_emulate_pr_in(struct se_cmd *);
+extern int target_scsi3_emulate_pr_out(struct se_cmd *);
 extern int core_setup_reservations(struct se_device *, int);
 
 #endif /* TARGET_CORE_PR_H */
diff --git a/drivers/target/target_core_pscsi.c b/drivers/target/target_core_pscsi.c
index 94c905fcbce..4ce2cf642fc 100644
--- a/drivers/target/target_core_pscsi.c
+++ b/drivers/target/target_core_pscsi.c
@@ -663,22 +663,12 @@ static void pscsi_free_device(void *p)
 	kfree(pdv);
 }
 
-static inline struct pscsi_plugin_task *PSCSI_TASK(struct se_task *task)
+static int pscsi_transport_complete(struct se_cmd *cmd, struct scatterlist *sg)
 {
-	return container_of(task, struct pscsi_plugin_task, pscsi_task);
-}
-
-
-/*	pscsi_transport_complete():
- *
- *
- */
-static int pscsi_transport_complete(struct se_task *task)
-{
-	struct pscsi_dev_virt *pdv = task->task_se_cmd->se_dev->dev_ptr;
+	struct pscsi_dev_virt *pdv = cmd->se_dev->dev_ptr;
 	struct scsi_device *sd = pdv->pdv_sd;
 	int result;
-	struct pscsi_plugin_task *pt = PSCSI_TASK(task);
+	struct pscsi_plugin_task *pt = cmd->priv;
 	unsigned char *cdb = &pt->pscsi_cdb[0];
 
 	result = pt->pscsi_result;
@@ -688,12 +678,11 @@ static int pscsi_transport_complete(struct se_task *task)
 	 */
 	if (((cdb[0] == MODE_SENSE) || (cdb[0] == MODE_SENSE_10)) &&
 	     (status_byte(result) << 1) == SAM_STAT_GOOD) {
-		if (!task->task_se_cmd->se_deve)
+		if (!cmd->se_deve)
 			goto after_mode_sense;
 
-		if (task->task_se_cmd->se_deve->lun_flags &
-				TRANSPORT_LUNFLAGS_READ_ONLY) {
-			unsigned char *buf = transport_kmap_data_sg(task->task_se_cmd);
+		if (cmd->se_deve->lun_flags & TRANSPORT_LUNFLAGS_READ_ONLY) {
+			unsigned char *buf = transport_kmap_data_sg(cmd);
 
 			if (cdb[0] == MODE_SENSE_10) {
 				if (!(buf[3] & 0x80))
@@ -703,7 +692,7 @@ static int pscsi_transport_complete(struct se_task *task)
 					buf[2] |= 0x80;
 			}
 
-			transport_kunmap_data_sg(task->task_se_cmd);
+			transport_kunmap_data_sg(cmd);
 		}
 	}
 after_mode_sense:
@@ -722,7 +711,6 @@ after_mode_sense:
 	if (((cdb[0] == MODE_SELECT) || (cdb[0] == MODE_SELECT_10)) &&
 	      (status_byte(result) << 1) == SAM_STAT_GOOD) {
 		unsigned char *buf;
-		struct scatterlist *sg = task->task_sg;
 		u16 bdl;
 		u32 blocksize;
 
@@ -757,35 +745,6 @@ after_mode_select:
 	return 0;
 }
 
-static struct se_task *
-pscsi_alloc_task(unsigned char *cdb)
-{
-	struct pscsi_plugin_task *pt;
-
-	/*
-	 * Dynamically alloc cdb space, since it may be larger than
-	 * TCM_MAX_COMMAND_SIZE
-	 */
-	pt = kzalloc(sizeof(*pt) + scsi_command_size(cdb), GFP_KERNEL);
-	if (!pt) {
-		pr_err("Unable to allocate struct pscsi_plugin_task\n");
-		return NULL;
-	}
-
-	return &pt->pscsi_task;
-}
-
-static void pscsi_free_task(struct se_task *task)
-{
-	struct pscsi_plugin_task *pt = PSCSI_TASK(task);
-
-	/*
-	 * We do not release the bio(s) here associated with this task, as
-	 * this is handled by bio_put() and pscsi_bi_endio().
-	 */
-	kfree(pt);
-}
-
 enum {
 	Opt_scsi_host_id, Opt_scsi_channel_id, Opt_scsi_target_id,
 	Opt_scsi_lun_id, Opt_err
@@ -958,26 +917,25 @@ static inline struct bio *pscsi_get_bio(int sg_num)
 	return bio;
 }
 
-static int pscsi_map_sg(struct se_task *task, struct scatterlist *task_sg,
+static int pscsi_map_sg(struct se_cmd *cmd, struct scatterlist *sgl,
+		u32 sgl_nents, enum dma_data_direction data_direction,
 		struct bio **hbio)
 {
-	struct se_cmd *cmd = task->task_se_cmd;
-	struct pscsi_dev_virt *pdv = task->task_se_cmd->se_dev->dev_ptr;
-	u32 task_sg_num = task->task_sg_nents;
+	struct pscsi_dev_virt *pdv = cmd->se_dev->dev_ptr;
 	struct bio *bio = NULL, *tbio = NULL;
 	struct page *page;
 	struct scatterlist *sg;
-	u32 data_len = task->task_size, i, len, bytes, off;
-	int nr_pages = (task->task_size + task_sg[0].offset +
+	u32 data_len = cmd->data_length, i, len, bytes, off;
+	int nr_pages = (cmd->data_length + sgl[0].offset +
 			PAGE_SIZE - 1) >> PAGE_SHIFT;
 	int nr_vecs = 0, rc;
-	int rw = (task->task_data_direction == DMA_TO_DEVICE);
+	int rw = (data_direction == DMA_TO_DEVICE);
 
 	*hbio = NULL;
 
 	pr_debug("PSCSI: nr_pages: %d\n", nr_pages);
 
-	for_each_sg(task_sg, sg, task_sg_num, i) {
+	for_each_sg(sgl, sg, sgl_nents, i) {
 		page = sg_page(sg);
 		off = sg->offset;
 		len = sg->length;
@@ -1009,7 +967,7 @@ static int pscsi_map_sg(struct se_task *task, struct scatterlist *task_sg,
 				 * Set *hbio pointer to handle the case:
 				 * nr_pages > BIO_MAX_PAGES, where additional
 				 * bios need to be added to complete a given
-				 * struct se_task
+				 * command.
 				 */
 				if (!*hbio)
 					*hbio = tbio = bio;
@@ -1049,7 +1007,7 @@ static int pscsi_map_sg(struct se_task *task, struct scatterlist *task_sg,
 		}
 	}
 
-	return task->task_sg_nents;
+	return sgl_nents;
 fail:
 	while (*hbio) {
 		bio = *hbio;
@@ -1061,52 +1019,61 @@ fail:
 	return -ENOMEM;
 }
 
-static int pscsi_do_task(struct se_task *task)
+static int pscsi_execute_cmd(struct se_cmd *cmd, struct scatterlist *sgl,
+		u32 sgl_nents, enum dma_data_direction data_direction)
 {
-	struct se_cmd *cmd = task->task_se_cmd;
-	struct pscsi_dev_virt *pdv = task->task_se_cmd->se_dev->dev_ptr;
-	struct pscsi_plugin_task *pt = PSCSI_TASK(task);
+	struct pscsi_dev_virt *pdv = cmd->se_dev->dev_ptr;
+	struct pscsi_plugin_task *pt;
 	struct request *req;
 	struct bio *hbio;
 	int ret;
 
-	target_get_task_cdb(task, pt->pscsi_cdb);
+	/*
+	 * Dynamically alloc cdb space, since it may be larger than
+	 * TCM_MAX_COMMAND_SIZE
+	 */
+	pt = kzalloc(sizeof(*pt) + scsi_command_size(cmd->t_task_cdb), GFP_KERNEL);
+	if (!pt) {
+		cmd->scsi_sense_reason = TCM_LOGICAL_UNIT_COMMUNICATION_FAILURE;
+		return -ENOMEM;
+	}
+	cmd->priv = pt;
+
+	memcpy(pt->pscsi_cdb, cmd->t_task_cdb,
+		scsi_command_size(cmd->t_task_cdb));
 
-	if (task->task_se_cmd->se_cmd_flags & SCF_SCSI_NON_DATA_CDB) {
+	if (cmd->se_cmd_flags & SCF_SCSI_NON_DATA_CDB) {
 		req = blk_get_request(pdv->pdv_sd->request_queue,
-				(task->task_data_direction == DMA_TO_DEVICE),
+				(data_direction == DMA_TO_DEVICE),
 				GFP_KERNEL);
 		if (!req || IS_ERR(req)) {
 			pr_err("PSCSI: blk_get_request() failed: %ld\n",
 					req ? IS_ERR(req) : -ENOMEM);
 			cmd->scsi_sense_reason =
 				TCM_LOGICAL_UNIT_COMMUNICATION_FAILURE;
-			return -ENODEV;
+			goto fail;
 		}
 	} else {
-		BUG_ON(!task->task_size);
+		BUG_ON(!cmd->data_length);
 
-		/*
-		 * Setup the main struct request for the task->task_sg[] payload
-		 */
-		ret = pscsi_map_sg(task, task->task_sg, &hbio);
+		ret = pscsi_map_sg(cmd, sgl, sgl_nents, data_direction, &hbio);
 		if (ret < 0) {
 			cmd->scsi_sense_reason =
 				TCM_LOGICAL_UNIT_COMMUNICATION_FAILURE;
-			return ret;
+			goto fail;
 		}
 
 		req = blk_make_request(pdv->pdv_sd->request_queue, hbio,
 				       GFP_KERNEL);
 		if (IS_ERR(req)) {
 			pr_err("pSCSI: blk_make_request() failed\n");
-			goto fail;
+			goto fail_free_bio;
 		}
 	}
 
 	req->cmd_type = REQ_TYPE_BLOCK_PC;
 	req->end_io = pscsi_req_done;
-	req->end_io_data = task;
+	req->end_io_data = cmd;
 	req->cmd_len = scsi_command_size(pt->pscsi_cdb);
 	req->cmd = &pt->pscsi_cdb[0];
 	req->sense = &pt->pscsi_sense[0];
@@ -1118,12 +1085,12 @@ static int pscsi_do_task(struct se_task *task)
 	req->retries = PS_RETRY;
 
 	blk_execute_rq_nowait(pdv->pdv_sd->request_queue, NULL, req,
-			(task->task_se_cmd->sam_task_attr == MSG_HEAD_TAG),
+			(cmd->sam_task_attr == MSG_HEAD_TAG),
 			pscsi_req_done);
 
 	return 0;
 
-fail:
+fail_free_bio:
 	while (hbio) {
 		struct bio *bio = hbio;
 		hbio = hbio->bi_next;
@@ -1131,16 +1098,14 @@ fail:
 		bio_endio(bio, 0);	/* XXX: should be error */
 	}
 	cmd->scsi_sense_reason = TCM_LOGICAL_UNIT_COMMUNICATION_FAILURE;
+fail:
+	kfree(pt);
 	return -ENOMEM;
 }
 
-/*	pscsi_get_sense_buffer():
- *
- *
- */
-static unsigned char *pscsi_get_sense_buffer(struct se_task *task)
+static unsigned char *pscsi_get_sense_buffer(struct se_cmd *cmd)
 {
-	struct pscsi_plugin_task *pt = PSCSI_TASK(task);
+	struct pscsi_plugin_task *pt = cmd->priv;
 
 	return pt->pscsi_sense;
 }
@@ -1180,48 +1145,36 @@ static sector_t pscsi_get_blocks(struct se_device *dev)
 	return 0;
 }
 
-/*	pscsi_handle_SAM_STATUS_failures():
- *
- *
- */
-static inline void pscsi_process_SAM_status(
-	struct se_task *task,
-	struct pscsi_plugin_task *pt)
+static void pscsi_req_done(struct request *req, int uptodate)
 {
-	task->task_scsi_status = status_byte(pt->pscsi_result);
-	if (task->task_scsi_status) {
-		task->task_scsi_status <<= 1;
-		pr_debug("PSCSI Status Byte exception at task: %p CDB:"
-			" 0x%02x Result: 0x%08x\n", task, pt->pscsi_cdb[0],
+	struct se_cmd *cmd = req->end_io_data;
+	struct pscsi_plugin_task *pt = cmd->priv;
+
+	pt->pscsi_result = req->errors;
+	pt->pscsi_resid = req->resid_len;
+
+	cmd->scsi_status = status_byte(pt->pscsi_result) << 1;
+	if (cmd->scsi_status) {
+		pr_debug("PSCSI Status Byte exception at cmd: %p CDB:"
+			" 0x%02x Result: 0x%08x\n", cmd, pt->pscsi_cdb[0],
 			pt->pscsi_result);
 	}
 
 	switch (host_byte(pt->pscsi_result)) {
 	case DID_OK:
-		transport_complete_task(task, (!task->task_scsi_status));
+		target_complete_cmd(cmd, cmd->scsi_status);
 		break;
 	default:
-		pr_debug("PSCSI Host Byte exception at task: %p CDB:"
-			" 0x%02x Result: 0x%08x\n", task, pt->pscsi_cdb[0],
+		pr_debug("PSCSI Host Byte exception at cmd: %p CDB:"
+			" 0x%02x Result: 0x%08x\n", cmd, pt->pscsi_cdb[0],
 			pt->pscsi_result);
-		task->task_scsi_status = SAM_STAT_CHECK_CONDITION;
-		task->task_se_cmd->scsi_sense_reason =
-					TCM_UNSUPPORTED_SCSI_OPCODE;
-		transport_complete_task(task, 0);
+		cmd->scsi_sense_reason = TCM_UNSUPPORTED_SCSI_OPCODE;
+		target_complete_cmd(cmd, SAM_STAT_CHECK_CONDITION);
 		break;
 	}
-}
 
-static void pscsi_req_done(struct request *req, int uptodate)
-{
-	struct se_task *task = req->end_io_data;
-	struct pscsi_plugin_task *pt = PSCSI_TASK(task);
-
-	pt->pscsi_result = req->errors;
-	pt->pscsi_resid = req->resid_len;
-
-	pscsi_process_SAM_status(task, pt);
 	__blk_put_request(req->q, req);
+	kfree(pt);
 }
 
 static struct se_subsystem_api pscsi_template = {
@@ -1235,9 +1188,7 @@ static struct se_subsystem_api pscsi_template = {
 	.create_virtdevice	= pscsi_create_virtdevice,
 	.free_device		= pscsi_free_device,
 	.transport_complete	= pscsi_transport_complete,
-	.alloc_task		= pscsi_alloc_task,
-	.do_task		= pscsi_do_task,
-	.free_task		= pscsi_free_task,
+	.execute_cmd		= pscsi_execute_cmd,
 	.check_configfs_dev_params = pscsi_check_configfs_dev_params,
 	.set_configfs_dev_params = pscsi_set_configfs_dev_params,
 	.show_configfs_dev_params = pscsi_show_configfs_dev_params,
diff --git a/drivers/target/target_core_pscsi.h b/drivers/target/target_core_pscsi.h
index 43f1c419e8e..bc1e5e11eca 100644
--- a/drivers/target/target_core_pscsi.h
+++ b/drivers/target/target_core_pscsi.h
@@ -22,7 +22,6 @@
 #include <linux/kobject.h>
 
 struct pscsi_plugin_task {
-	struct se_task pscsi_task;
 	unsigned char pscsi_sense[SCSI_SENSE_BUFFERSIZE];
 	int	pscsi_direction;
 	int	pscsi_result;
diff --git a/drivers/target/target_core_rd.c b/drivers/target/target_core_rd.c
index 8b68f7b8263..d0ceb873c0e 100644
--- a/drivers/target/target_core_rd.c
+++ b/drivers/target/target_core_rd.c
@@ -64,9 +64,6 @@ static int rd_attach_hba(struct se_hba *hba, u32 host_id)
 	pr_debug("CORE_HBA[%d] - TCM Ramdisk HBA Driver %s on"
 		" Generic Target Core Stack %s\n", hba->hba_id,
 		RD_HBA_VERSION, TARGET_CORE_MOD_VERSION);
-	pr_debug("CORE_HBA[%d] - Attached Ramdisk HBA: %u to Generic"
-		" MaxSectors: %u\n", hba->hba_id,
-		rd_host->rd_host_id, RD_MAX_SECTORS);
 
 	return 0;
 }
@@ -199,10 +196,7 @@ static int rd_build_device_space(struct rd_dev *rd_dev)
 	return 0;
 }
 
-static void *rd_allocate_virtdevice(
-	struct se_hba *hba,
-	const char *name,
-	int rd_direct)
+static void *rd_allocate_virtdevice(struct se_hba *hba, const char *name)
 {
 	struct rd_dev *rd_dev;
 	struct rd_host *rd_host = hba->hba_ptr;
@@ -214,25 +208,12 @@ static void *rd_allocate_virtdevice(
 	}
 
 	rd_dev->rd_host = rd_host;
-	rd_dev->rd_direct = rd_direct;
 
 	return rd_dev;
 }
 
-static void *rd_MEMCPY_allocate_virtdevice(struct se_hba *hba, const char *name)
-{
-	return rd_allocate_virtdevice(hba, name, 0);
-}
-
-/*	rd_create_virtdevice():
- *
- *
- */
-static struct se_device *rd_create_virtdevice(
-	struct se_hba *hba,
-	struct se_subsystem_dev *se_dev,
-	void *p,
-	int rd_direct)
+static struct se_device *rd_create_virtdevice(struct se_hba *hba,
+		struct se_subsystem_dev *se_dev, void *p)
 {
 	struct se_device *dev;
 	struct se_dev_limits dev_limits;
@@ -247,13 +228,12 @@ static struct se_device *rd_create_virtdevice(
 	if (ret < 0)
 		goto fail;
 
-	snprintf(prod, 16, "RAMDISK-%s", (rd_dev->rd_direct) ? "DR" : "MCP");
-	snprintf(rev, 4, "%s", (rd_dev->rd_direct) ? RD_DR_VERSION :
-						RD_MCP_VERSION);
+	snprintf(prod, 16, "RAMDISK-MCP");
+	snprintf(rev, 4, "%s", RD_MCP_VERSION);
 
 	dev_limits.limits.logical_block_size = RD_BLOCKSIZE;
-	dev_limits.limits.max_hw_sectors = RD_MAX_SECTORS;
-	dev_limits.limits.max_sectors = RD_MAX_SECTORS;
+	dev_limits.limits.max_hw_sectors = UINT_MAX;
+	dev_limits.limits.max_sectors = UINT_MAX;
 	dev_limits.hw_queue_depth = RD_MAX_DEVICE_QUEUE_DEPTH;
 	dev_limits.queue_depth = RD_DEVICE_QUEUE_DEPTH;
 
@@ -264,12 +244,10 @@ static struct se_device *rd_create_virtdevice(
 		goto fail;
 
 	rd_dev->rd_dev_id = rd_host->rd_host_dev_id_count++;
-	rd_dev->rd_queue_depth = dev->queue_depth;
 
-	pr_debug("CORE_RD[%u] - Added TCM %s Ramdisk Device ID: %u of"
+	pr_debug("CORE_RD[%u] - Added TCM MEMCPY Ramdisk Device ID: %u of"
 		" %u pages in %u tables, %lu total bytes\n",
-		rd_host->rd_host_id, (!rd_dev->rd_direct) ? "MEMCPY" :
-		"DIRECT", rd_dev->rd_dev_id, rd_dev->rd_page_count,
+		rd_host->rd_host_id, rd_dev->rd_dev_id, rd_dev->rd_page_count,
 		rd_dev->sg_table_count,
 		(unsigned long)(rd_dev->rd_page_count * PAGE_SIZE));
 
@@ -280,18 +258,6 @@ fail:
 	return ERR_PTR(ret);
 }
 
-static struct se_device *rd_MEMCPY_create_virtdevice(
-	struct se_hba *hba,
-	struct se_subsystem_dev *se_dev,
-	void *p)
-{
-	return rd_create_virtdevice(hba, se_dev, p, 0);
-}
-
-/*	rd_free_device(): (Part of se_subsystem_api_t template)
- *
- *
- */
 static void rd_free_device(void *p)
 {
 	struct rd_dev *rd_dev = p;
@@ -300,29 +266,6 @@ static void rd_free_device(void *p)
 	kfree(rd_dev);
 }
 
-static inline struct rd_request *RD_REQ(struct se_task *task)
-{
-	return container_of(task, struct rd_request, rd_task);
-}
-
-static struct se_task *
-rd_alloc_task(unsigned char *cdb)
-{
-	struct rd_request *rd_req;
-
-	rd_req = kzalloc(sizeof(struct rd_request), GFP_KERNEL);
-	if (!rd_req) {
-		pr_err("Unable to allocate struct rd_request\n");
-		return NULL;
-	}
-
-	return &rd_req->rd_task;
-}
-
-/*	rd_get_sg_table():
- *
- *
- */
 static struct rd_dev_sg_table *rd_get_sg_table(struct rd_dev *rd_dev, u32 page)
 {
 	u32 i;
@@ -341,31 +284,41 @@ static struct rd_dev_sg_table *rd_get_sg_table(struct rd_dev *rd_dev, u32 page)
 	return NULL;
 }
 
-static int rd_MEMCPY(struct rd_request *req, u32 read_rd)
+static int rd_execute_cmd(struct se_cmd *cmd, struct scatterlist *sgl,
+		u32 sgl_nents, enum dma_data_direction data_direction)
 {
-	struct se_task *task = &req->rd_task;
-	struct rd_dev *dev = req->rd_task.task_se_cmd->se_dev->dev_ptr;
+	struct se_device *se_dev = cmd->se_dev;
+	struct rd_dev *dev = se_dev->dev_ptr;
 	struct rd_dev_sg_table *table;
 	struct scatterlist *rd_sg;
 	struct sg_mapping_iter m;
-	u32 rd_offset = req->rd_offset;
+	u32 rd_offset;
+	u32 rd_size;
+	u32 rd_page;
 	u32 src_len;
+	u64 tmp;
+
+	tmp = cmd->t_task_lba * se_dev->se_sub_dev->se_dev_attrib.block_size;
+	rd_offset = do_div(tmp, PAGE_SIZE);
+	rd_page = tmp;
+	rd_size = cmd->data_length;
 
-	table = rd_get_sg_table(dev, req->rd_page);
+	table = rd_get_sg_table(dev, rd_page);
 	if (!table)
 		return -EINVAL;
 
-	rd_sg = &table->sg_table[req->rd_page - table->page_start_offset];
+	rd_sg = &table->sg_table[rd_page - table->page_start_offset];
 
 	pr_debug("RD[%u]: %s LBA: %llu, Size: %u Page: %u, Offset: %u\n",
-			dev->rd_dev_id, read_rd ? "Read" : "Write",
-			task->task_lba, req->rd_size, req->rd_page,
-			rd_offset);
+			dev->rd_dev_id,
+			data_direction == DMA_FROM_DEVICE ? "Read" : "Write",
+			cmd->t_task_lba, rd_size, rd_page, rd_offset);
 
 	src_len = PAGE_SIZE - rd_offset;
-	sg_miter_start(&m, task->task_sg, task->task_sg_nents,
-			read_rd ? SG_MITER_TO_SG : SG_MITER_FROM_SG);
-	while (req->rd_size) {
+	sg_miter_start(&m, sgl, sgl_nents,
+			data_direction == DMA_FROM_DEVICE ?
+				SG_MITER_TO_SG : SG_MITER_FROM_SG);
+	while (rd_size) {
 		u32 len;
 		void *rd_addr;
 
@@ -375,13 +328,13 @@ static int rd_MEMCPY(struct rd_request *req, u32 read_rd)
 
 		rd_addr = sg_virt(rd_sg) + rd_offset;
 
-		if (read_rd)
+		if (data_direction == DMA_FROM_DEVICE)
 			memcpy(m.addr, rd_addr, len);
 		else
 			memcpy(rd_addr, m.addr, len);
 
-		req->rd_size -= len;
-		if (!req->rd_size)
+		rd_size -= len;
+		if (!rd_size)
 			continue;
 
 		src_len -= len;
@@ -391,15 +344,15 @@ static int rd_MEMCPY(struct rd_request *req, u32 read_rd)
 		}
 
 		/* rd page completed, next one please */
-		req->rd_page++;
+		rd_page++;
 		rd_offset = 0;
 		src_len = PAGE_SIZE;
-		if (req->rd_page <= table->page_end_offset) {
+		if (rd_page <= table->page_end_offset) {
 			rd_sg++;
 			continue;
 		}
 
-		table = rd_get_sg_table(dev, req->rd_page);
+		table = rd_get_sg_table(dev, rd_page);
 		if (!table) {
 			sg_miter_stop(&m);
 			return -EINVAL;
@@ -409,43 +362,11 @@ static int rd_MEMCPY(struct rd_request *req, u32 read_rd)
 		rd_sg = table->sg_table;
 	}
 	sg_miter_stop(&m);
-	return 0;
-}
 
-/*	rd_MEMCPY_do_task(): (Part of se_subsystem_api_t template)
- *
- *
- */
-static int rd_MEMCPY_do_task(struct se_task *task)
-{
-	struct se_device *dev = task->task_se_cmd->se_dev;
-	struct rd_request *req = RD_REQ(task);
-	u64 tmp;
-	int ret;
-
-	tmp = task->task_lba * dev->se_sub_dev->se_dev_attrib.block_size;
-	req->rd_offset = do_div(tmp, PAGE_SIZE);
-	req->rd_page = tmp;
-	req->rd_size = task->task_size;
-
-	ret = rd_MEMCPY(req, task->task_data_direction == DMA_FROM_DEVICE);
-	if (ret != 0)
-		return ret;
-
-	task->task_scsi_status = GOOD;
-	transport_complete_task(task, 1);
+	target_complete_cmd(cmd, SAM_STAT_GOOD);
 	return 0;
 }
 
-/*	rd_free_task(): (Part of se_subsystem_api_t template)
- *
- *
- */
-static void rd_free_task(struct se_task *task)
-{
-	kfree(RD_REQ(task));
-}
-
 enum {
 	Opt_rd_pages, Opt_err
 };
@@ -512,9 +433,8 @@ static ssize_t rd_show_configfs_dev_params(
 	char *b)
 {
 	struct rd_dev *rd_dev = se_dev->se_dev_su_ptr;
-	ssize_t bl = sprintf(b, "TCM RamDisk ID: %u  RamDisk Makeup: %s\n",
-			rd_dev->rd_dev_id, (rd_dev->rd_direct) ?
-			"rd_direct" : "rd_mcp");
+	ssize_t bl = sprintf(b, "TCM RamDisk ID: %u  RamDisk Makeup: rd_mcp\n",
+			rd_dev->rd_dev_id);
 	bl += sprintf(b + bl, "        PAGES/PAGE_SIZE: %u*%lu"
 			"  SG_table_count: %u\n", rd_dev->rd_page_count,
 			PAGE_SIZE, rd_dev->sg_table_count);
@@ -545,12 +465,10 @@ static struct se_subsystem_api rd_mcp_template = {
 	.transport_type		= TRANSPORT_PLUGIN_VHBA_VDEV,
 	.attach_hba		= rd_attach_hba,
 	.detach_hba		= rd_detach_hba,
-	.allocate_virtdevice	= rd_MEMCPY_allocate_virtdevice,
-	.create_virtdevice	= rd_MEMCPY_create_virtdevice,
+	.allocate_virtdevice	= rd_allocate_virtdevice,
+	.create_virtdevice	= rd_create_virtdevice,
 	.free_device		= rd_free_device,
-	.alloc_task		= rd_alloc_task,
-	.do_task		= rd_MEMCPY_do_task,
-	.free_task		= rd_free_task,
+	.execute_cmd		= rd_execute_cmd,
 	.check_configfs_dev_params = rd_check_configfs_dev_params,
 	.set_configfs_dev_params = rd_set_configfs_dev_params,
 	.show_configfs_dev_params = rd_show_configfs_dev_params,
diff --git a/drivers/target/target_core_rd.h b/drivers/target/target_core_rd.h
index 784e56a0410..21458125fe5 100644
--- a/drivers/target/target_core_rd.h
+++ b/drivers/target/target_core_rd.h
@@ -2,7 +2,6 @@
 #define TARGET_CORE_RD_H
 
 #define RD_HBA_VERSION		"v4.0"
-#define RD_DR_VERSION		"4.0"
 #define RD_MCP_VERSION		"4.0"
 
 /* Largest piece of memory kmalloc can allocate */
@@ -10,28 +9,11 @@
 #define RD_DEVICE_QUEUE_DEPTH	32
 #define RD_MAX_DEVICE_QUEUE_DEPTH 128
 #define RD_BLOCKSIZE		512
-#define RD_MAX_SECTORS		1024
 
 /* Used in target_core_init_configfs() for virtual LUN 0 access */
 int __init rd_module_init(void);
 void rd_module_exit(void);
 
-#define RRF_EMULATE_CDB		0x01
-#define RRF_GOT_LBA		0x02
-
-struct rd_request {
-	struct se_task	rd_task;
-
-	/* Offset from start of page */
-	u32		rd_offset;
-	/* Starting page in Ramdisk for request */
-	u32		rd_page;
-	/* Total number of pages needed for request */
-	u32		rd_page_count;
-	/* Scatterlist count */
-	u32		rd_size;
-} ____cacheline_aligned;
-
 struct rd_dev_sg_table {
 	u32		page_start_offset;
 	u32		page_end_offset;
@@ -42,7 +24,6 @@ struct rd_dev_sg_table {
 #define RDF_HAS_PAGE_COUNT	0x01
 
 struct rd_dev {
-	int		rd_direct;
 	u32		rd_flags;
 	/* Unique Ramdisk Device ID in Ramdisk HBA */
 	u32		rd_dev_id;
@@ -50,7 +31,6 @@ struct rd_dev {
 	u32		rd_page_count;
 	/* Number of SG tables in sg_table_array */
 	u32		sg_table_count;
-	u32		rd_queue_depth;
 	/* Array of rd_dev_sg_table_t containing scatterlists */
 	struct rd_dev_sg_table *sg_table_array;
 	/* Ramdisk HBA device is connected to */
diff --git a/drivers/target/target_core_tmr.c b/drivers/target/target_core_tmr.c
index f015839aef8..84caf1bed9a 100644
--- a/drivers/target/target_core_tmr.c
+++ b/drivers/target/target_core_tmr.c
@@ -244,7 +244,7 @@ static void core_tmr_drain_tmr_list(
 	}
 }
 
-static void core_tmr_drain_task_list(
+static void core_tmr_drain_state_list(
 	struct se_device *dev,
 	struct se_cmd *prout_cmd,
 	struct se_node_acl *tmr_nacl,
@@ -252,12 +252,13 @@ static void core_tmr_drain_task_list(
 	struct list_head *preempt_and_abort_list)
 {
 	LIST_HEAD(drain_task_list);
-	struct se_cmd *cmd;
-	struct se_task *task, *task_tmp;
+	struct se_cmd *cmd, *next;
 	unsigned long flags;
 	int fe_count;
+
 	/*
-	 * Complete outstanding struct se_task CDBs with TASK_ABORTED SAM status.
+	 * Complete outstanding commands with TASK_ABORTED SAM status.
+	 *
 	 * This is following sam4r17, section 5.6 Aborting commands, Table 38
 	 * for TMR LUN_RESET:
 	 *
@@ -278,56 +279,43 @@ static void core_tmr_drain_task_list(
 	 * in the Control Mode Page.
 	 */
 	spin_lock_irqsave(&dev->execute_task_lock, flags);
-	list_for_each_entry_safe(task, task_tmp, &dev->state_task_list,
-				t_state_list) {
-		if (!task->task_se_cmd) {
-			pr_err("task->task_se_cmd is NULL!\n");
-			continue;
-		}
-		cmd = task->task_se_cmd;
-
+	list_for_each_entry_safe(cmd, next, &dev->state_list, state_list) {
 		/*
 		 * For PREEMPT_AND_ABORT usage, only process commands
 		 * with a matching reservation key.
 		 */
 		if (target_check_cdb_and_preempt(preempt_and_abort_list, cmd))
 			continue;
+
 		/*
 		 * Not aborting PROUT PREEMPT_AND_ABORT CDB..
 		 */
 		if (prout_cmd == cmd)
 			continue;
 
-		list_move_tail(&task->t_state_list, &drain_task_list);
-		task->t_state_active = false;
-		/*
-		 * Remove from task execute list before processing drain_task_list
-		 */
-		if (!list_empty(&task->t_execute_list))
-			__transport_remove_task_from_execute_queue(task, dev);
+		list_move_tail(&cmd->state_list, &drain_task_list);
+		cmd->state_active = false;
+
+		if (!list_empty(&cmd->execute_list))
+			__target_remove_from_execute_list(cmd);
 	}
 	spin_unlock_irqrestore(&dev->execute_task_lock, flags);
 
 	while (!list_empty(&drain_task_list)) {
-		task = list_entry(drain_task_list.next, struct se_task, t_state_list);
-		list_del(&task->t_state_list);
-		cmd = task->task_se_cmd;
+		cmd = list_entry(drain_task_list.next, struct se_cmd, state_list);
+		list_del(&cmd->state_list);
 
-		pr_debug("LUN_RESET: %s cmd: %p task: %p"
+		pr_debug("LUN_RESET: %s cmd: %p"
 			" ITT/CmdSN: 0x%08x/0x%08x, i_state: %d, t_state: %d"
 			"cdb: 0x%02x\n",
-			(preempt_and_abort_list) ? "Preempt" : "", cmd, task,
+			(preempt_and_abort_list) ? "Preempt" : "", cmd,
 			cmd->se_tfo->get_task_tag(cmd), 0,
 			cmd->se_tfo->get_cmd_state(cmd), cmd->t_state,
 			cmd->t_task_cdb[0]);
 		pr_debug("LUN_RESET: ITT[0x%08x] - pr_res_key: 0x%016Lx"
-			" t_task_cdbs: %d t_task_cdbs_left: %d"
-			" t_task_cdbs_sent: %d -- CMD_T_ACTIVE: %d"
+			" -- CMD_T_ACTIVE: %d"
 			" CMD_T_STOP: %d CMD_T_SENT: %d\n",
 			cmd->se_tfo->get_task_tag(cmd), cmd->pr_res_key,
-			cmd->t_task_list_num,
-			atomic_read(&cmd->t_task_cdbs_left),
-			atomic_read(&cmd->t_task_cdbs_sent),
 			(cmd->transport_state & CMD_T_ACTIVE) != 0,
 			(cmd->transport_state & CMD_T_STOP) != 0,
 			(cmd->transport_state & CMD_T_SENT) != 0);
@@ -343,20 +331,13 @@ static void core_tmr_drain_task_list(
 			cancel_work_sync(&cmd->work);
 
 		spin_lock_irqsave(&cmd->t_state_lock, flags);
-		target_stop_task(task, &flags);
+		target_stop_cmd(cmd, &flags);
 
-		if (!atomic_dec_and_test(&cmd->t_task_cdbs_ex_left)) {
-			spin_unlock_irqrestore(&cmd->t_state_lock, flags);
-			pr_debug("LUN_RESET: Skipping task: %p, dev: %p for"
-				" t_task_cdbs_ex_left: %d\n", task, dev,
-				atomic_read(&cmd->t_task_cdbs_ex_left));
-			continue;
-		}
 		fe_count = atomic_read(&cmd->t_fe_count);
 
 		if (!(cmd->transport_state & CMD_T_ACTIVE)) {
 			pr_debug("LUN_RESET: got CMD_T_ACTIVE for"
-				" task: %p, t_fe_count: %d dev: %p\n", task,
+				" cdb: %p, t_fe_count: %d dev: %p\n", cmd,
 				fe_count, dev);
 			cmd->transport_state |= CMD_T_ABORTED;
 			spin_unlock_irqrestore(&cmd->t_state_lock, flags);
@@ -364,8 +345,8 @@ static void core_tmr_drain_task_list(
 			core_tmr_handle_tas_abort(tmr_nacl, cmd, tas, fe_count);
 			continue;
 		}
-		pr_debug("LUN_RESET: Got !CMD_T_ACTIVE for task: %p,"
-			" t_fe_count: %d dev: %p\n", task, fe_count, dev);
+		pr_debug("LUN_RESET: Got !CMD_T_ACTIVE for cdb: %p,"
+			" t_fe_count: %d dev: %p\n", cmd, fe_count, dev);
 		cmd->transport_state |= CMD_T_ABORTED;
 		spin_unlock_irqrestore(&cmd->t_state_lock, flags);
 
@@ -384,13 +365,11 @@ static void core_tmr_drain_cmd_list(
 	struct se_queue_obj *qobj = &dev->dev_queue_obj;
 	struct se_cmd *cmd, *tcmd;
 	unsigned long flags;
+
 	/*
-	 * Release all commands remaining in the struct se_device cmd queue.
+	 * Release all commands remaining in the per-device command queue.
 	 *
-	 * This follows the same logic as above for the struct se_device
-	 * struct se_task state list, where commands are returned with
-	 * TASK_ABORTED status, if there is an outstanding $FABRIC_MOD
-	 * reference, otherwise the struct se_cmd is released.
+	 * This follows the same logic as above for the state list.
 	 */
 	spin_lock_irqsave(&qobj->cmd_queue_lock, flags);
 	list_for_each_entry_safe(cmd, tcmd, &qobj->qobj_list, se_queue_node) {
@@ -466,7 +445,7 @@ int core_tmr_lun_reset(
 		dev->transport->name, tas);
 
 	core_tmr_drain_tmr_list(dev, tmr, preempt_and_abort_list);
-	core_tmr_drain_task_list(dev, prout_cmd, tmr_nacl, tas,
+	core_tmr_drain_state_list(dev, prout_cmd, tmr_nacl, tas,
 				preempt_and_abort_list);
 	core_tmr_drain_cmd_list(dev, prout_cmd, tmr_nacl, tas,
 				preempt_and_abort_list);
diff --git a/drivers/target/target_core_tpg.c b/drivers/target/target_core_tpg.c
index 70c3ffb981e..8bd58e28418 100644
--- a/drivers/target/target_core_tpg.c
+++ b/drivers/target/target_core_tpg.c
@@ -60,7 +60,6 @@ static void core_clear_initiator_node_from_tpg(
 	int i;
 	struct se_dev_entry *deve;
 	struct se_lun *lun;
-	struct se_lun_acl *acl, *acl_tmp;
 
 	spin_lock_irq(&nacl->device_list_lock);
 	for (i = 0; i < TRANSPORT_MAX_LUNS_PER_TPG; i++) {
@@ -81,28 +80,7 @@ static void core_clear_initiator_node_from_tpg(
 		core_update_device_list_for_node(lun, NULL, deve->mapped_lun,
 			TRANSPORT_LUNFLAGS_NO_ACCESS, nacl, tpg, 0);
 
-		spin_lock(&lun->lun_acl_lock);
-		list_for_each_entry_safe(acl, acl_tmp,
-					&lun->lun_acl_list, lacl_list) {
-			if (!strcmp(acl->initiatorname, nacl->initiatorname) &&
-			    (acl->mapped_lun == deve->mapped_lun))
-				break;
-		}
-
-		if (!acl) {
-			pr_err("Unable to locate struct se_lun_acl for %s,"
-				" mapped_lun: %u\n", nacl->initiatorname,
-				deve->mapped_lun);
-			spin_unlock(&lun->lun_acl_lock);
-			spin_lock_irq(&nacl->device_list_lock);
-			continue;
-		}
-
-		list_del(&acl->lacl_list);
-		spin_unlock(&lun->lun_acl_lock);
-
 		spin_lock_irq(&nacl->device_list_lock);
-		kfree(acl);
 	}
 	spin_unlock_irq(&nacl->device_list_lock);
 }
@@ -175,10 +153,7 @@ void core_tpg_add_node_to_devs(
 		 * demo_mode_write_protect is ON, or READ_ONLY;
 		 */
 		if (!tpg->se_tpg_tfo->tpg_check_demo_mode_write_protect(tpg)) {
-			if (dev->dev_flags & DF_READ_ONLY)
-				lun_access = TRANSPORT_LUNFLAGS_READ_ONLY;
-			else
-				lun_access = TRANSPORT_LUNFLAGS_READ_WRITE;
+			lun_access = TRANSPORT_LUNFLAGS_READ_WRITE;
 		} else {
 			/*
 			 * Allow only optical drives to issue R/W in default RO
diff --git a/drivers/target/target_core_transport.c b/drivers/target/target_core_transport.c
index 443704f84fd..b05fdc0c05d 100644
--- a/drivers/target/target_core_transport.c
+++ b/drivers/target/target_core_transport.c
@@ -72,7 +72,6 @@ static int __transport_execute_tasks(struct se_device *dev, struct se_cmd *);
 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 void transport_free_dev_tasks(struct se_cmd *cmd);
 static int transport_generic_get_mem(struct se_cmd *cmd);
 static void transport_put_cmd(struct se_cmd *cmd);
 static void transport_remove_cmd_from_queue(struct se_cmd *cmd);
@@ -331,9 +330,9 @@ void target_get_session(struct se_session *se_sess)
 }
 EXPORT_SYMBOL(target_get_session);
 
-int target_put_session(struct se_session *se_sess)
+void target_put_session(struct se_session *se_sess)
 {
-	return kref_put(&se_sess->sess_kref, target_release_session);
+	kref_put(&se_sess->sess_kref, target_release_session);
 }
 EXPORT_SYMBOL(target_put_session);
 
@@ -444,31 +443,23 @@ EXPORT_SYMBOL(transport_deregister_session);
 /*
  * Called with cmd->t_state_lock held.
  */
-static void transport_all_task_dev_remove_state(struct se_cmd *cmd)
+static void target_remove_from_state_list(struct se_cmd *cmd)
 {
 	struct se_device *dev = cmd->se_dev;
-	struct se_task *task;
 	unsigned long flags;
 
 	if (!dev)
 		return;
 
-	list_for_each_entry(task, &cmd->t_task_list, t_list) {
-		if (task->task_flags & TF_ACTIVE)
-			continue;
-
-		spin_lock_irqsave(&dev->execute_task_lock, flags);
-		if (task->t_state_active) {
-			pr_debug("Removed ITT: 0x%08x dev: %p task[%p]\n",
-				cmd->se_tfo->get_task_tag(cmd), dev, task);
+	if (cmd->transport_state & CMD_T_BUSY)
+		return;
 
-			list_del(&task->t_state_list);
-			atomic_dec(&cmd->t_task_cdbs_ex_left);
-			task->t_state_active = false;
-		}
-		spin_unlock_irqrestore(&dev->execute_task_lock, flags);
+	spin_lock_irqsave(&dev->execute_task_lock, flags);
+	if (cmd->state_active) {
+		list_del(&cmd->state_list);
+		cmd->state_active = false;
 	}
-
+	spin_unlock_irqrestore(&dev->execute_task_lock, flags);
 }
 
 /*	transport_cmd_check_stop():
@@ -497,7 +488,7 @@ static int transport_cmd_check_stop(
 
 		cmd->transport_state &= ~CMD_T_ACTIVE;
 		if (transport_off == 2)
-			transport_all_task_dev_remove_state(cmd);
+			target_remove_from_state_list(cmd);
 		spin_unlock_irqrestore(&cmd->t_state_lock, flags);
 
 		complete(&cmd->transport_lun_stop_comp);
@@ -513,7 +504,7 @@ static int transport_cmd_check_stop(
 			cmd->se_tfo->get_task_tag(cmd));
 
 		if (transport_off == 2)
-			transport_all_task_dev_remove_state(cmd);
+			target_remove_from_state_list(cmd);
 
 		/*
 		 * Clear struct se_cmd->se_lun before the transport_off == 2 handoff
@@ -529,7 +520,7 @@ static int transport_cmd_check_stop(
 	if (transport_off) {
 		cmd->transport_state &= ~CMD_T_ACTIVE;
 		if (transport_off == 2) {
-			transport_all_task_dev_remove_state(cmd);
+			target_remove_from_state_list(cmd);
 			/*
 			 * Clear struct se_cmd->se_lun before the transport_off == 2
 			 * handoff to fabric module.
@@ -577,7 +568,7 @@ static void transport_lun_remove_cmd(struct se_cmd *cmd)
 	spin_lock_irqsave(&cmd->t_state_lock, flags);
 	if (cmd->transport_state & CMD_T_DEV_ACTIVE) {
 		cmd->transport_state &= ~CMD_T_DEV_ACTIVE;
-		transport_all_task_dev_remove_state(cmd);
+		target_remove_from_state_list(cmd);
 	}
 	spin_unlock_irqrestore(&cmd->t_state_lock, flags);
 
@@ -669,29 +660,6 @@ static void transport_remove_cmd_from_queue(struct se_cmd *cmd)
 	spin_unlock_irqrestore(&qobj->cmd_queue_lock, flags);
 }
 
-/*
- * Completion function used by TCM subsystem plugins (such as FILEIO)
- * for queueing up response from struct se_subsystem_api->do_task()
- */
-void transport_complete_sync_cache(struct se_cmd *cmd, int good)
-{
-	struct se_task *task = list_entry(cmd->t_task_list.next,
-				struct se_task, t_list);
-
-	if (good) {
-		cmd->scsi_status = SAM_STAT_GOOD;
-		task->task_scsi_status = GOOD;
-	} else {
-		task->task_scsi_status = SAM_STAT_CHECK_CONDITION;
-		task->task_se_cmd->scsi_sense_reason =
-				TCM_LOGICAL_UNIT_COMMUNICATION_FAILURE;
-
-	}
-
-	transport_complete_task(task, good);
-}
-EXPORT_SYMBOL(transport_complete_sync_cache);
-
 static void target_complete_failure_work(struct work_struct *work)
 {
 	struct se_cmd *cmd = container_of(work, struct se_cmd, work);
@@ -699,40 +667,32 @@ static void target_complete_failure_work(struct work_struct *work)
 	transport_generic_request_failure(cmd);
 }
 
-/*	transport_complete_task():
- *
- *	Called from interrupt and non interrupt context depending
- *	on the transport plugin.
- */
-void transport_complete_task(struct se_task *task, int success)
+void target_complete_cmd(struct se_cmd *cmd, u8 scsi_status)
 {
-	struct se_cmd *cmd = task->task_se_cmd;
 	struct se_device *dev = cmd->se_dev;
+	int success = scsi_status == GOOD;
 	unsigned long flags;
 
+	cmd->scsi_status = scsi_status;
+
+
 	spin_lock_irqsave(&cmd->t_state_lock, flags);
-	task->task_flags &= ~TF_ACTIVE;
+	cmd->transport_state &= ~CMD_T_BUSY;
 
-	/*
-	 * See if any sense data exists, if so set the TASK_SENSE flag.
-	 * Also check for any other post completion work that needs to be
-	 * done by the plugins.
-	 */
 	if (dev && dev->transport->transport_complete) {
-		if (dev->transport->transport_complete(task) != 0) {
+		if (dev->transport->transport_complete(cmd,
+				cmd->t_data_sg) != 0) {
 			cmd->se_cmd_flags |= SCF_TRANSPORT_TASK_SENSE;
-			task->task_flags |= TF_HAS_SENSE;
 			success = 1;
 		}
 	}
 
 	/*
-	 * See if we are waiting for outstanding struct se_task
-	 * to complete for an exception condition
+	 * See if we are waiting to complete for an exception condition.
 	 */
-	if (task->task_flags & TF_REQUEST_STOP) {
+	if (cmd->transport_state & CMD_T_REQUEST_STOP) {
 		spin_unlock_irqrestore(&cmd->t_state_lock, flags);
-		complete(&task->task_stop_comp);
+		complete(&cmd->task_stop_comp);
 		return;
 	}
 
@@ -740,15 +700,6 @@ void transport_complete_task(struct se_task *task, int success)
 		cmd->transport_state |= CMD_T_FAILED;
 
 	/*
-	 * Decrement the outstanding t_task_cdbs_left count.  The last
-	 * struct se_task from struct se_cmd will complete itself into the
-	 * device queue depending upon int success.
-	 */
-	if (!atomic_dec_and_test(&cmd->t_task_cdbs_left)) {
-		spin_unlock_irqrestore(&cmd->t_state_lock, flags);
-		return;
-	}
-	/*
 	 * Check for case where an explict ABORT_TASK has been received
 	 * and transport_wait_for_tasks() will be waiting for completion..
 	 */
@@ -770,157 +721,77 @@ void transport_complete_task(struct se_task *task, int success)
 
 	queue_work(target_completion_wq, &cmd->work);
 }
-EXPORT_SYMBOL(transport_complete_task);
-
-/*
- * Called by transport_add_tasks_from_cmd() once a struct se_cmd's
- * struct se_task list are ready to be added to the active execution list
- * struct se_device
+EXPORT_SYMBOL(target_complete_cmd);
 
- * Called with se_dev_t->execute_task_lock called.
- */
-static inline int transport_add_task_check_sam_attr(
-	struct se_task *task,
-	struct se_task *task_prev,
-	struct se_device *dev)
+static void target_add_to_state_list(struct se_cmd *cmd)
 {
-	/*
-	 * No SAM Task attribute emulation enabled, add to tail of
-	 * execution queue
-	 */
-	if (dev->dev_task_attr_type != SAM_TASK_ATTR_EMULATED) {
-		list_add_tail(&task->t_execute_list, &dev->execute_task_list);
-		return 0;
-	}
-	/*
-	 * HEAD_OF_QUEUE attribute for received CDB, which means
-	 * the first task that is associated with a struct se_cmd goes to
-	 * head of the struct se_device->execute_task_list, and task_prev
-	 * after that for each subsequent task
-	 */
-	if (task->task_se_cmd->sam_task_attr == MSG_HEAD_TAG) {
-		list_add(&task->t_execute_list,
-				(task_prev != NULL) ?
-				&task_prev->t_execute_list :
-				&dev->execute_task_list);
-
-		pr_debug("Set HEAD_OF_QUEUE for task CDB: 0x%02x"
-				" in execution queue\n",
-				task->task_se_cmd->t_task_cdb[0]);
-		return 1;
+	struct se_device *dev = cmd->se_dev;
+	unsigned long flags;
+
+	spin_lock_irqsave(&dev->execute_task_lock, flags);
+	if (!cmd->state_active) {
+		list_add_tail(&cmd->state_list, &dev->state_list);
+		cmd->state_active = true;
 	}
-	/*
-	 * For ORDERED, SIMPLE or UNTAGGED attribute tasks once they have been
-	 * transitioned from Dermant -> Active state, and are added to the end
-	 * of the struct se_device->execute_task_list
-	 */
-	list_add_tail(&task->t_execute_list, &dev->execute_task_list);
-	return 0;
+	spin_unlock_irqrestore(&dev->execute_task_lock, flags);
 }
 
-/*	__transport_add_task_to_execute_queue():
- *
- *	Called with se_dev_t->execute_task_lock called.
- */
-static void __transport_add_task_to_execute_queue(
-	struct se_task *task,
-	struct se_task *task_prev,
-	struct se_device *dev)
+static void __target_add_to_execute_list(struct se_cmd *cmd)
 {
-	int head_of_queue;
-
-	head_of_queue = transport_add_task_check_sam_attr(task, task_prev, dev);
-	atomic_inc(&dev->execute_tasks);
+	struct se_device *dev = cmd->se_dev;
+	bool head_of_queue = false;
 
-	if (task->t_state_active)
+	if (!list_empty(&cmd->execute_list))
 		return;
-	/*
-	 * Determine if this task needs to go to HEAD_OF_QUEUE for the
-	 * state list as well.  Running with SAM Task Attribute emulation
-	 * will always return head_of_queue == 0 here
-	 */
-	if (head_of_queue)
-		list_add(&task->t_state_list, (task_prev) ?
-				&task_prev->t_state_list :
-				&dev->state_task_list);
-	else
-		list_add_tail(&task->t_state_list, &dev->state_task_list);
 
-	task->t_state_active = true;
+	if (dev->dev_task_attr_type == SAM_TASK_ATTR_EMULATED &&
+	    cmd->sam_task_attr == MSG_HEAD_TAG)
+		head_of_queue = true;
 
-	pr_debug("Added ITT: 0x%08x task[%p] to dev: %p\n",
-		task->task_se_cmd->se_tfo->get_task_tag(task->task_se_cmd),
-		task, dev);
-}
+	if (head_of_queue)
+		list_add(&cmd->execute_list, &dev->execute_list);
+	else
+		list_add_tail(&cmd->execute_list, &dev->execute_list);
 
-static void transport_add_tasks_to_state_queue(struct se_cmd *cmd)
-{
-	struct se_device *dev = cmd->se_dev;
-	struct se_task *task;
-	unsigned long flags;
+	atomic_inc(&dev->execute_tasks);
 
-	spin_lock_irqsave(&cmd->t_state_lock, flags);
-	list_for_each_entry(task, &cmd->t_task_list, t_list) {
-		spin_lock(&dev->execute_task_lock);
-		if (!task->t_state_active) {
-			list_add_tail(&task->t_state_list,
-				      &dev->state_task_list);
-			task->t_state_active = true;
-
-			pr_debug("Added ITT: 0x%08x task[%p] to dev: %p\n",
-				task->task_se_cmd->se_tfo->get_task_tag(
-				task->task_se_cmd), task, dev);
-		}
-		spin_unlock(&dev->execute_task_lock);
-	}
-	spin_unlock_irqrestore(&cmd->t_state_lock, flags);
-}
+	if (cmd->state_active)
+		return;
 
-static void __transport_add_tasks_from_cmd(struct se_cmd *cmd)
-{
-	struct se_device *dev = cmd->se_dev;
-	struct se_task *task, *task_prev = NULL;
+	if (head_of_queue)
+		list_add(&cmd->state_list, &dev->state_list);
+	else
+		list_add_tail(&cmd->state_list, &dev->state_list);
 
-	list_for_each_entry(task, &cmd->t_task_list, t_list) {
-		if (!list_empty(&task->t_execute_list))
-			continue;
-		/*
-		 * __transport_add_task_to_execute_queue() handles the
-		 * SAM Task Attribute emulation if enabled
-		 */
-		__transport_add_task_to_execute_queue(task, task_prev, dev);
-		task_prev = task;
-	}
+	cmd->state_active = true;
 }
 
-static void transport_add_tasks_from_cmd(struct se_cmd *cmd)
+static void target_add_to_execute_list(struct se_cmd *cmd)
 {
 	unsigned long flags;
 	struct se_device *dev = cmd->se_dev;
 
 	spin_lock_irqsave(&dev->execute_task_lock, flags);
-	__transport_add_tasks_from_cmd(cmd);
+	__target_add_to_execute_list(cmd);
 	spin_unlock_irqrestore(&dev->execute_task_lock, flags);
 }
 
-void __transport_remove_task_from_execute_queue(struct se_task *task,
-		struct se_device *dev)
+void __target_remove_from_execute_list(struct se_cmd *cmd)
 {
-	list_del_init(&task->t_execute_list);
-	atomic_dec(&dev->execute_tasks);
+	list_del_init(&cmd->execute_list);
+	atomic_dec(&cmd->se_dev->execute_tasks);
 }
 
-static void transport_remove_task_from_execute_queue(
-	struct se_task *task,
-	struct se_device *dev)
+static void target_remove_from_execute_list(struct se_cmd *cmd)
 {
+	struct se_device *dev = cmd->se_dev;
 	unsigned long flags;
 
-	if (WARN_ON(list_empty(&task->t_execute_list)))
+	if (WARN_ON(list_empty(&cmd->execute_list)))
 		return;
 
 	spin_lock_irqsave(&dev->execute_task_lock, flags);
-	__transport_remove_task_from_execute_queue(task, dev);
+	__target_remove_from_execute_list(cmd);
 	spin_unlock_irqrestore(&dev->execute_task_lock, flags);
 }
 
@@ -999,8 +870,9 @@ void transport_dump_dev_state(
 
 	*bl += sprintf(b + *bl, "  Execute/Max Queue Depth: %d/%d",
 		atomic_read(&dev->execute_tasks), dev->queue_depth);
-	*bl += sprintf(b + *bl, "  SectorSize: %u  MaxSectors: %u\n",
-		dev->se_sub_dev->se_dev_attrib.block_size, dev->se_sub_dev->se_dev_attrib.max_sectors);
+	*bl += sprintf(b + *bl, "  SectorSize: %u  HwMaxSectors: %u\n",
+		dev->se_sub_dev->se_dev_attrib.block_size,
+		dev->se_sub_dev->se_dev_attrib.hw_max_sectors);
 	*bl += sprintf(b + *bl, "        ");
 }
 
@@ -1344,9 +1216,9 @@ struct se_device *transport_add_device_to_core_hba(
 	INIT_LIST_HEAD(&dev->dev_list);
 	INIT_LIST_HEAD(&dev->dev_sep_list);
 	INIT_LIST_HEAD(&dev->dev_tmr_list);
-	INIT_LIST_HEAD(&dev->execute_task_list);
+	INIT_LIST_HEAD(&dev->execute_list);
 	INIT_LIST_HEAD(&dev->delayed_cmd_list);
-	INIT_LIST_HEAD(&dev->state_task_list);
+	INIT_LIST_HEAD(&dev->state_list);
 	INIT_LIST_HEAD(&dev->qf_cmd_list);
 	spin_lock_init(&dev->execute_task_lock);
 	spin_lock_init(&dev->delayed_cmd_lock);
@@ -1457,6 +1329,7 @@ static inline void transport_generic_prepare_cdb(
 	case VERIFY_16: /* SBC - VRProtect */
 	case WRITE_VERIFY: /* SBC - VRProtect */
 	case WRITE_VERIFY_12: /* SBC - VRProtect */
+	case MAINTENANCE_IN: /* SPC - Parameter Data Format for SA RTPG */
 		break;
 	default:
 		cdb[1] &= 0x1f; /* clear logical unit number */
@@ -1464,29 +1337,6 @@ static inline void transport_generic_prepare_cdb(
 	}
 }
 
-static struct se_task *
-transport_generic_get_task(struct se_cmd *cmd,
-		enum dma_data_direction data_direction)
-{
-	struct se_task *task;
-	struct se_device *dev = cmd->se_dev;
-
-	task = dev->transport->alloc_task(cmd->t_task_cdb);
-	if (!task) {
-		pr_err("Unable to allocate struct se_task\n");
-		return NULL;
-	}
-
-	INIT_LIST_HEAD(&task->t_list);
-	INIT_LIST_HEAD(&task->t_execute_list);
-	INIT_LIST_HEAD(&task->t_state_list);
-	init_completion(&task->task_stop_comp);
-	task->task_se_cmd = cmd;
-	task->task_data_direction = data_direction;
-
-	return task;
-}
-
 static int transport_generic_cmd_sequencer(struct se_cmd *, unsigned char *);
 
 /*
@@ -1507,11 +1357,13 @@ void transport_init_se_cmd(
 	INIT_LIST_HEAD(&cmd->se_qf_node);
 	INIT_LIST_HEAD(&cmd->se_queue_node);
 	INIT_LIST_HEAD(&cmd->se_cmd_list);
-	INIT_LIST_HEAD(&cmd->t_task_list);
+	INIT_LIST_HEAD(&cmd->execute_list);
+	INIT_LIST_HEAD(&cmd->state_list);
 	init_completion(&cmd->transport_lun_fe_stop_comp);
 	init_completion(&cmd->transport_lun_stop_comp);
 	init_completion(&cmd->t_transport_stop_comp);
 	init_completion(&cmd->cmd_wait_comp);
+	init_completion(&cmd->task_stop_comp);
 	spin_lock_init(&cmd->t_state_lock);
 	cmd->transport_state = CMD_T_DEV_ACTIVE;
 
@@ -1521,6 +1373,8 @@ void transport_init_se_cmd(
 	cmd->data_direction = data_direction;
 	cmd->sam_task_attr = task_attr;
 	cmd->sense_buffer = sense_buffer;
+
+	cmd->state_active = false;
 }
 EXPORT_SYMBOL(transport_init_se_cmd);
 
@@ -1550,11 +1404,11 @@ static int transport_check_alloc_task_attr(struct se_cmd *cmd)
 	return 0;
 }
 
-/*	transport_generic_allocate_tasks():
+/*	target_setup_cmd_from_cdb():
  *
  *	Called from fabric RX Thread.
  */
-int transport_generic_allocate_tasks(
+int target_setup_cmd_from_cdb(
 	struct se_cmd *cmd,
 	unsigned char *cdb)
 {
@@ -1620,7 +1474,7 @@ int transport_generic_allocate_tasks(
 	spin_unlock(&cmd->se_lun->lun_sep_lock);
 	return 0;
 }
-EXPORT_SYMBOL(transport_generic_allocate_tasks);
+EXPORT_SYMBOL(target_setup_cmd_from_cdb);
 
 /*
  * Used by fabric module frontends to queue tasks directly.
@@ -1701,6 +1555,8 @@ void target_submit_cmd(struct se_cmd *se_cmd, struct se_session *se_sess,
 	 */
 	transport_init_se_cmd(se_cmd, se_tpg->se_tpg_tfo, se_sess,
 				data_length, data_dir, task_attr, sense);
+	if (flags & TARGET_SCF_UNKNOWN_SIZE)
+		se_cmd->unknown_data_length = 1;
 	/*
 	 * Obtain struct se_cmd->cmd_kref reference and add new cmd to
 	 * se_sess->sess_cmd_list.  A second kref_get here is necessary
@@ -1726,11 +1582,18 @@ void target_submit_cmd(struct se_cmd *se_cmd, struct se_session *se_sess,
 	 * Sanitize CDBs via transport_generic_cmd_sequencer() and
 	 * allocate the necessary tasks to complete the received CDB+data
 	 */
-	rc = transport_generic_allocate_tasks(se_cmd, cdb);
+	rc = target_setup_cmd_from_cdb(se_cmd, cdb);
 	if (rc != 0) {
 		transport_generic_request_failure(se_cmd);
 		return;
 	}
+
+	/*
+	 * Check if we need to delay processing because of ALUA
+	 * Active/NonOptimized primary access state..
+	 */
+	core_alua_check_nonop_delay(se_cmd);
+
 	/*
 	 * Dispatch se_cmd descriptor to se_lun->lun_se_dev backend
 	 * for immediate execution of READs, otherwise wait for
@@ -1872,72 +1735,30 @@ int transport_generic_handle_tmr(
 EXPORT_SYMBOL(transport_generic_handle_tmr);
 
 /*
- * If the task is active, request it to be stopped and sleep until it
+ * If the cmd is active, request it to be stopped and sleep until it
  * has completed.
  */
-bool target_stop_task(struct se_task *task, unsigned long *flags)
+bool target_stop_cmd(struct se_cmd *cmd, unsigned long *flags)
 {
-	struct se_cmd *cmd = task->task_se_cmd;
 	bool was_active = false;
 
-	if (task->task_flags & TF_ACTIVE) {
-		task->task_flags |= TF_REQUEST_STOP;
+	if (cmd->transport_state & CMD_T_BUSY) {
+		cmd->transport_state |= CMD_T_REQUEST_STOP;
 		spin_unlock_irqrestore(&cmd->t_state_lock, *flags);
 
-		pr_debug("Task %p waiting to complete\n", task);
-		wait_for_completion(&task->task_stop_comp);
-		pr_debug("Task %p stopped successfully\n", task);
+		pr_debug("cmd %p waiting to complete\n", cmd);
+		wait_for_completion(&cmd->task_stop_comp);
+		pr_debug("cmd %p stopped successfully\n", cmd);
 
 		spin_lock_irqsave(&cmd->t_state_lock, *flags);
-		atomic_dec(&cmd->t_task_cdbs_left);
-		task->task_flags &= ~(TF_ACTIVE | TF_REQUEST_STOP);
+		cmd->transport_state &= ~CMD_T_REQUEST_STOP;
+		cmd->transport_state &= ~CMD_T_BUSY;
 		was_active = true;
 	}
 
 	return was_active;
 }
 
-static int transport_stop_tasks_for_cmd(struct se_cmd *cmd)
-{
-	struct se_task *task, *task_tmp;
-	unsigned long flags;
-	int ret = 0;
-
-	pr_debug("ITT[0x%08x] - Stopping tasks\n",
-		cmd->se_tfo->get_task_tag(cmd));
-
-	/*
-	 * No tasks remain in the execution queue
-	 */
-	spin_lock_irqsave(&cmd->t_state_lock, flags);
-	list_for_each_entry_safe(task, task_tmp,
-				&cmd->t_task_list, t_list) {
-		pr_debug("Processing task %p\n", task);
-		/*
-		 * If the struct se_task has not been sent and is not active,
-		 * remove the struct se_task from the execution queue.
-		 */
-		if (!(task->task_flags & (TF_ACTIVE | TF_SENT))) {
-			spin_unlock_irqrestore(&cmd->t_state_lock,
-					flags);
-			transport_remove_task_from_execute_queue(task,
-					cmd->se_dev);
-
-			pr_debug("Task %p removed from execute queue\n", task);
-			spin_lock_irqsave(&cmd->t_state_lock, flags);
-			continue;
-		}
-
-		if (!target_stop_task(task, &flags)) {
-			pr_debug("Task %p - did nothing\n", task);
-			ret++;
-		}
-	}
-	spin_unlock_irqrestore(&cmd->t_state_lock, flags);
-
-	return ret;
-}
-
 /*
  * Handle SAM-esque emulation for generic transport request failures.
  */
@@ -1951,13 +1772,7 @@ void transport_generic_request_failure(struct se_cmd *cmd)
 	pr_debug("-----[ i_state: %d t_state: %d scsi_sense_reason: %d\n",
 		cmd->se_tfo->get_cmd_state(cmd),
 		cmd->t_state, cmd->scsi_sense_reason);
-	pr_debug("-----[ t_tasks: %d t_task_cdbs_left: %d"
-		" t_task_cdbs_sent: %d t_task_cdbs_ex_left: %d --"
-		" CMD_T_ACTIVE: %d CMD_T_STOP: %d CMD_T_SENT: %d\n",
-		cmd->t_task_list_num,
-		atomic_read(&cmd->t_task_cdbs_left),
-		atomic_read(&cmd->t_task_cdbs_sent),
-		atomic_read(&cmd->t_task_cdbs_ex_left),
+	pr_debug("-----[ CMD_T_ACTIVE: %d CMD_T_STOP: %d CMD_T_SENT: %d\n",
 		(cmd->transport_state & CMD_T_ACTIVE) != 0,
 		(cmd->transport_state & CMD_T_STOP) != 0,
 		(cmd->transport_state & CMD_T_SENT) != 0);
@@ -2156,7 +1971,7 @@ static inline int transport_execute_task_attr(struct se_cmd *cmd)
  * Called from fabric module context in transport_generic_new_cmd() and
  * transport_generic_process_write()
  */
-static int transport_execute_tasks(struct se_cmd *cmd)
+static void transport_execute_tasks(struct se_cmd *cmd)
 {
 	int add_tasks;
 	struct se_device *se_dev = cmd->se_dev;
@@ -2170,71 +1985,52 @@ static int transport_execute_tasks(struct se_cmd *cmd)
 		 * attribute for the tasks of the received struct se_cmd CDB
 		 */
 		add_tasks = transport_execute_task_attr(cmd);
-		if (!add_tasks)
-			goto execute_tasks;
-		/*
-		 * __transport_execute_tasks() -> __transport_add_tasks_from_cmd()
-		 * adds associated se_tasks while holding dev->execute_task_lock
-		 * before I/O dispath to avoid a double spinlock access.
-		 */
-		__transport_execute_tasks(se_dev, cmd);
-		return 0;
+		if (add_tasks) {
+			__transport_execute_tasks(se_dev, cmd);
+			return;
+		}
 	}
-
-execute_tasks:
 	__transport_execute_tasks(se_dev, NULL);
-	return 0;
 }
 
-/*
- * Called to check struct se_device tcq depth window, and once open pull struct se_task
- * from struct se_device->execute_task_list and
- *
- * Called from transport_processing_thread()
- */
 static int __transport_execute_tasks(struct se_device *dev, struct se_cmd *new_cmd)
 {
 	int error;
 	struct se_cmd *cmd = NULL;
-	struct se_task *task = NULL;
 	unsigned long flags;
 
 check_depth:
 	spin_lock_irq(&dev->execute_task_lock);
 	if (new_cmd != NULL)
-		__transport_add_tasks_from_cmd(new_cmd);
+		__target_add_to_execute_list(new_cmd);
 
-	if (list_empty(&dev->execute_task_list)) {
+	if (list_empty(&dev->execute_list)) {
 		spin_unlock_irq(&dev->execute_task_lock);
 		return 0;
 	}
-	task = list_first_entry(&dev->execute_task_list,
-				struct se_task, t_execute_list);
-	__transport_remove_task_from_execute_queue(task, dev);
+	cmd = list_first_entry(&dev->execute_list, struct se_cmd, execute_list);
+	__target_remove_from_execute_list(cmd);
 	spin_unlock_irq(&dev->execute_task_lock);
 
-	cmd = task->task_se_cmd;
 	spin_lock_irqsave(&cmd->t_state_lock, flags);
-	task->task_flags |= (TF_ACTIVE | TF_SENT);
-	atomic_inc(&cmd->t_task_cdbs_sent);
-
-	if (atomic_read(&cmd->t_task_cdbs_sent) ==
-	    cmd->t_task_list_num)
-		cmd->transport_state |= CMD_T_SENT;
+	cmd->transport_state |= CMD_T_BUSY;
+	cmd->transport_state |= CMD_T_SENT;
 
 	spin_unlock_irqrestore(&cmd->t_state_lock, flags);
 
-	if (cmd->execute_task)
-		error = cmd->execute_task(task);
-	else
-		error = dev->transport->do_task(task);
+	if (cmd->execute_cmd)
+		error = cmd->execute_cmd(cmd);
+	else {
+		error = dev->transport->execute_cmd(cmd, cmd->t_data_sg,
+				cmd->t_data_nents, cmd->data_direction);
+	}
+
 	if (error != 0) {
 		spin_lock_irqsave(&cmd->t_state_lock, flags);
-		task->task_flags &= ~TF_ACTIVE;
+		cmd->transport_state &= ~CMD_T_BUSY;
 		cmd->transport_state &= ~CMD_T_SENT;
 		spin_unlock_irqrestore(&cmd->t_state_lock, flags);
 
-		transport_stop_tasks_for_cmd(cmd);
 		transport_generic_request_failure(cmd);
 	}
 
@@ -2392,12 +2188,12 @@ static inline u32 transport_get_size(
 		} else /* bytes */
 			return sectors;
 	}
-#if 0
+
 	pr_debug("Returning block_size: %u, sectors: %u == %u for"
-			" %s object\n", dev->se_sub_dev->se_dev_attrib.block_size, sectors,
-			dev->se_sub_dev->se_dev_attrib.block_size * sectors,
-			dev->transport->name);
-#endif
+		" %s object\n", dev->se_sub_dev->se_dev_attrib.block_size,
+		sectors, dev->se_sub_dev->se_dev_attrib.block_size * sectors,
+		dev->transport->name);
+
 	return dev->se_sub_dev->se_dev_attrib.block_size * sectors;
 }
 
@@ -2462,7 +2258,6 @@ static int transport_get_sense_data(struct se_cmd *cmd)
 {
 	unsigned char *buffer = cmd->sense_buffer, *sense_buffer = NULL;
 	struct se_device *dev = cmd->se_dev;
-	struct se_task *task = NULL, *task_tmp;
 	unsigned long flags;
 	u32 offset = 0;
 
@@ -2477,44 +2272,37 @@ static int transport_get_sense_data(struct se_cmd *cmd)
 		return 0;
 	}
 
-	list_for_each_entry_safe(task, task_tmp,
-				&cmd->t_task_list, t_list) {
-		if (!(task->task_flags & TF_HAS_SENSE))
-			continue;
-
-		if (!dev->transport->get_sense_buffer) {
-			pr_err("dev->transport->get_sense_buffer"
-					" is NULL\n");
-			continue;
-		}
-
-		sense_buffer = dev->transport->get_sense_buffer(task);
-		if (!sense_buffer) {
-			pr_err("ITT[0x%08x]_TASK[%p]: Unable to locate"
-				" sense buffer for task with sense\n",
-				cmd->se_tfo->get_task_tag(cmd), task);
-			continue;
-		}
-		spin_unlock_irqrestore(&cmd->t_state_lock, flags);
+	if (!(cmd->se_cmd_flags & SCF_TRANSPORT_TASK_SENSE))
+		goto out;
 
-		offset = cmd->se_tfo->set_fabric_sense_len(cmd,
-				TRANSPORT_SENSE_BUFFER);
+	if (!dev->transport->get_sense_buffer) {
+		pr_err("dev->transport->get_sense_buffer is NULL\n");
+		goto out;
+	}
 
-		memcpy(&buffer[offset], sense_buffer,
-				TRANSPORT_SENSE_BUFFER);
-		cmd->scsi_status = task->task_scsi_status;
-		/* Automatically padded */
-		cmd->scsi_sense_length =
-				(TRANSPORT_SENSE_BUFFER + offset);
-
-		pr_debug("HBA_[%u]_PLUG[%s]: Set SAM STATUS: 0x%02x"
-				" and sense\n",
-			dev->se_hba->hba_id, dev->transport->name,
-				cmd->scsi_status);
-		return 0;
+	sense_buffer = dev->transport->get_sense_buffer(cmd);
+	if (!sense_buffer) {
+		pr_err("ITT 0x%08x cmd %p: Unable to locate"
+			" sense buffer for task with sense\n",
+			cmd->se_tfo->get_task_tag(cmd), cmd);
+		goto out;
 	}
+
 	spin_unlock_irqrestore(&cmd->t_state_lock, flags);
 
+	offset = cmd->se_tfo->set_fabric_sense_len(cmd, TRANSPORT_SENSE_BUFFER);
+
+	memcpy(&buffer[offset], sense_buffer, TRANSPORT_SENSE_BUFFER);
+
+	/* Automatically padded */
+	cmd->scsi_sense_length = TRANSPORT_SENSE_BUFFER + offset;
+
+	pr_debug("HBA_[%u]_PLUG[%s]: Set SAM STATUS: 0x%02x and sense\n",
+		dev->se_hba->hba_id, dev->transport->name, cmd->scsi_status);
+	return 0;
+
+out:
+	spin_unlock_irqrestore(&cmd->t_state_lock, flags);
 	return -1;
 }
 
@@ -2581,7 +2369,7 @@ static int target_check_write_same_discard(unsigned char *flags, struct se_devic
  *	Generic Command Sequencer that should work for most DAS transport
  *	drivers.
  *
- *	Called from transport_generic_allocate_tasks() in the $FABRIC_MOD
+ *	Called from target_setup_cmd_from_cdb() in the $FABRIC_MOD
  *	RX Thread.
  *
  *	FIXME: Need to support other SCSI OPCODES where as well.
@@ -2615,11 +2403,10 @@ static int transport_generic_cmd_sequencer(
 		 * by the ALUA primary or secondary access state..
 		 */
 		if (ret > 0) {
-#if 0
 			pr_debug("[%s]: ALUA TG Port not available,"
 				" SenseKey: NOT_READY, ASC/ASCQ: 0x04/0x%02x\n",
 				cmd->se_tfo->get_fabric_name(), alua_ascq);
-#endif
+
 			transport_set_sense_codes(cmd, 0x04, alua_ascq);
 			cmd->se_cmd_flags |= SCF_SCSI_CDB_EXCEPTION;
 			cmd->scsi_sense_reason = TCM_CHECK_CONDITION_NOT_READY;
@@ -2695,6 +2482,7 @@ static int transport_generic_cmd_sequencer(
 		cmd->se_cmd_flags |= SCF_SCSI_DATA_SG_IO_CDB;
 		break;
 	case WRITE_10:
+	case WRITE_VERIFY:
 		sectors = transport_get_sectors_10(cdb, cmd, &sector_ret);
 		if (sector_ret)
 			goto out_unsupported_cdb;
@@ -2796,7 +2584,7 @@ static int transport_generic_cmd_sequencer(
 			if (target_check_write_same_discard(&cdb[10], dev) < 0)
 				goto out_unsupported_cdb;
 			if (!passthrough)
-				cmd->execute_task = target_emulate_write_same;
+				cmd->execute_cmd = target_emulate_write_same;
 			break;
 		default:
 			pr_err("VARIABLE_LENGTH_CMD service action"
@@ -2810,9 +2598,9 @@ static int transport_generic_cmd_sequencer(
 			/*
 			 * Check for emulated MI_REPORT_TARGET_PGS.
 			 */
-			if (cdb[1] == MI_REPORT_TARGET_PGS &&
+			if ((cdb[1] & 0x1f) == MI_REPORT_TARGET_PGS &&
 			    su_dev->t10_alua.alua_type == SPC3_ALUA_EMULATED) {
-				cmd->execute_task =
+				cmd->execute_cmd =
 					target_emulate_report_target_port_groups;
 			}
 			size = (cdb[6] << 24) | (cdb[7] << 16) |
@@ -2835,13 +2623,13 @@ static int transport_generic_cmd_sequencer(
 		size = cdb[4];
 		cmd->se_cmd_flags |= SCF_SCSI_CONTROL_SG_IO_CDB;
 		if (!passthrough)
-			cmd->execute_task = target_emulate_modesense;
+			cmd->execute_cmd = target_emulate_modesense;
 		break;
 	case MODE_SENSE_10:
 		size = (cdb[7] << 8) + cdb[8];
 		cmd->se_cmd_flags |= SCF_SCSI_CONTROL_SG_IO_CDB;
 		if (!passthrough)
-			cmd->execute_task = target_emulate_modesense;
+			cmd->execute_cmd = target_emulate_modesense;
 		break;
 	case GPCMD_READ_BUFFER_CAPACITY:
 	case GPCMD_SEND_OPC:
@@ -2863,13 +2651,13 @@ static int transport_generic_cmd_sequencer(
 		break;
 	case PERSISTENT_RESERVE_IN:
 		if (su_dev->t10_pr.res_type == SPC3_PERSISTENT_RESERVATIONS)
-			cmd->execute_task = target_scsi3_emulate_pr_in;
+			cmd->execute_cmd = target_scsi3_emulate_pr_in;
 		size = (cdb[7] << 8) + cdb[8];
 		cmd->se_cmd_flags |= SCF_SCSI_CONTROL_SG_IO_CDB;
 		break;
 	case PERSISTENT_RESERVE_OUT:
 		if (su_dev->t10_pr.res_type == SPC3_PERSISTENT_RESERVATIONS)
-			cmd->execute_task = target_scsi3_emulate_pr_out;
+			cmd->execute_cmd = target_scsi3_emulate_pr_out;
 		size = (cdb[7] << 8) + cdb[8];
 		cmd->se_cmd_flags |= SCF_SCSI_CONTROL_SG_IO_CDB;
 		break;
@@ -2890,7 +2678,7 @@ static int transport_generic_cmd_sequencer(
 			 */
 			if (cdb[1] == MO_SET_TARGET_PGS &&
 			    su_dev->t10_alua.alua_type == SPC3_ALUA_EMULATED) {
-				cmd->execute_task =
+				cmd->execute_cmd =
 					target_emulate_set_target_port_groups;
 			}
 
@@ -2912,7 +2700,7 @@ static int transport_generic_cmd_sequencer(
 			cmd->sam_task_attr = MSG_HEAD_TAG;
 		cmd->se_cmd_flags |= SCF_SCSI_CONTROL_SG_IO_CDB;
 		if (!passthrough)
-			cmd->execute_task = target_emulate_inquiry;
+			cmd->execute_cmd = target_emulate_inquiry;
 		break;
 	case READ_BUFFER:
 		size = (cdb[6] << 16) + (cdb[7] << 8) + cdb[8];
@@ -2922,7 +2710,7 @@ static int transport_generic_cmd_sequencer(
 		size = READ_CAP_LEN;
 		cmd->se_cmd_flags |= SCF_SCSI_CONTROL_SG_IO_CDB;
 		if (!passthrough)
-			cmd->execute_task = target_emulate_readcapacity;
+			cmd->execute_cmd = target_emulate_readcapacity;
 		break;
 	case READ_MEDIA_SERIAL_NUMBER:
 	case SECURITY_PROTOCOL_IN:
@@ -2934,7 +2722,7 @@ static int transport_generic_cmd_sequencer(
 		switch (cmd->t_task_cdb[1] & 0x1f) {
 		case SAI_READ_CAPACITY_16:
 			if (!passthrough)
-				cmd->execute_task =
+				cmd->execute_cmd =
 					target_emulate_readcapacity_16;
 			break;
 		default:
@@ -2977,7 +2765,7 @@ static int transport_generic_cmd_sequencer(
 		size = cdb[4];
 		cmd->se_cmd_flags |= SCF_SCSI_CONTROL_SG_IO_CDB;
 		if (!passthrough)
-			cmd->execute_task = target_emulate_request_sense;
+			cmd->execute_cmd = target_emulate_request_sense;
 		break;
 	case READ_ELEMENT_STATUS:
 		size = 65536 * cdb[7] + 256 * cdb[8] + cdb[9];
@@ -3006,7 +2794,7 @@ static int transport_generic_cmd_sequencer(
 		 * emulation disabled.
 		 */
 		if (su_dev->t10_pr.res_type != SPC_PASSTHROUGH)
-			cmd->execute_task = target_scsi2_reservation_reserve;
+			cmd->execute_cmd = target_scsi2_reservation_reserve;
 		cmd->se_cmd_flags |= SCF_SCSI_NON_DATA_CDB;
 		break;
 	case RELEASE:
@@ -3021,7 +2809,7 @@ static int transport_generic_cmd_sequencer(
 			size = cmd->data_length;
 
 		if (su_dev->t10_pr.res_type != SPC_PASSTHROUGH)
-			cmd->execute_task = target_scsi2_reservation_release;
+			cmd->execute_cmd = target_scsi2_reservation_release;
 		cmd->se_cmd_flags |= SCF_SCSI_NON_DATA_CDB;
 		break;
 	case SYNCHRONIZE_CACHE:
@@ -3053,13 +2841,13 @@ static int transport_generic_cmd_sequencer(
 			if (transport_cmd_get_valid_sectors(cmd) < 0)
 				goto out_invalid_cdb_field;
 		}
-		cmd->execute_task = target_emulate_synchronize_cache;
+		cmd->execute_cmd = target_emulate_synchronize_cache;
 		break;
 	case UNMAP:
 		size = get_unaligned_be16(&cdb[7]);
 		cmd->se_cmd_flags |= SCF_SCSI_CONTROL_SG_IO_CDB;
 		if (!passthrough)
-			cmd->execute_task = target_emulate_unmap;
+			cmd->execute_cmd = target_emulate_unmap;
 		break;
 	case WRITE_SAME_16:
 		sectors = transport_get_sectors_16(cdb, cmd, &sector_ret);
@@ -3079,7 +2867,7 @@ static int transport_generic_cmd_sequencer(
 		if (target_check_write_same_discard(&cdb[1], dev) < 0)
 			goto out_unsupported_cdb;
 		if (!passthrough)
-			cmd->execute_task = target_emulate_write_same;
+			cmd->execute_cmd = target_emulate_write_same;
 		break;
 	case WRITE_SAME:
 		sectors = transport_get_sectors_10(cdb, cmd, &sector_ret);
@@ -3102,7 +2890,7 @@ static int transport_generic_cmd_sequencer(
 		if (target_check_write_same_discard(&cdb[1], dev) < 0)
 			goto out_unsupported_cdb;
 		if (!passthrough)
-			cmd->execute_task = target_emulate_write_same;
+			cmd->execute_cmd = target_emulate_write_same;
 		break;
 	case ALLOW_MEDIUM_REMOVAL:
 	case ERASE:
@@ -3115,7 +2903,7 @@ static int transport_generic_cmd_sequencer(
 	case WRITE_FILEMARKS:
 		cmd->se_cmd_flags |= SCF_SCSI_NON_DATA_CDB;
 		if (!passthrough)
-			cmd->execute_task = target_emulate_noop;
+			cmd->execute_cmd = target_emulate_noop;
 		break;
 	case GPCMD_CLOSE_TRACK:
 	case INITIALIZE_ELEMENT_STATUS:
@@ -3125,7 +2913,7 @@ static int transport_generic_cmd_sequencer(
 		cmd->se_cmd_flags |= SCF_SCSI_NON_DATA_CDB;
 		break;
 	case REPORT_LUNS:
-		cmd->execute_task = target_report_luns;
+		cmd->execute_cmd = target_report_luns;
 		size = (cdb[6] << 24) | (cdb[7] << 16) | (cdb[8] << 8) | cdb[9];
 		/*
 		 * Do implict HEAD_OF_QUEUE processing for REPORT_LUNS
@@ -3135,6 +2923,42 @@ static int transport_generic_cmd_sequencer(
 			cmd->sam_task_attr = MSG_HEAD_TAG;
 		cmd->se_cmd_flags |= SCF_SCSI_CONTROL_SG_IO_CDB;
 		break;
+	case GET_EVENT_STATUS_NOTIFICATION:
+		size = (cdb[7] << 8) | cdb[8];
+		cmd->se_cmd_flags |= SCF_SCSI_CONTROL_SG_IO_CDB;
+		break;
+	case ATA_16:
+		/* Only support ATA passthrough to pSCSI backends.. */
+		if (!passthrough)
+			goto out_unsupported_cdb;
+
+		/* T_LENGTH */
+		switch (cdb[2] & 0x3) {
+		case 0x0:
+			sectors = 0;
+			break;
+		case 0x1:
+			sectors = (((cdb[1] & 0x1) ? cdb[3] : 0) << 8) | cdb[4];
+			break;
+		case 0x2:
+			sectors = (((cdb[1] & 0x1) ? cdb[5] : 0) << 8) | cdb[6];
+			break;
+		case 0x3:
+			pr_err("T_LENGTH=0x3 not supported for ATA_16\n");
+			goto out_invalid_cdb_field;
+		}
+
+		/* BYTE_BLOCK */
+		if (cdb[2] & 0x4) {
+			/* BLOCK T_TYPE: 512 or sector */
+			size = sectors * ((cdb[2] & 0x10) ?
+				dev->se_sub_dev->se_dev_attrib.block_size : 512);
+		} else {
+			/* BYTE */
+			size = sectors;
+		}
+		cmd->se_cmd_flags |= SCF_SCSI_CONTROL_SG_IO_CDB;
+		break;
 	default:
 		pr_warn("TARGET_CORE[%s]: Unsupported SCSI Opcode"
 			" 0x%02x, sending CHECK_CONDITION.\n",
@@ -3142,6 +2966,9 @@ static int transport_generic_cmd_sequencer(
 		goto out_unsupported_cdb;
 	}
 
+	if (cmd->unknown_data_length)
+		cmd->data_length = size;
+
 	if (size != cmd->data_length) {
 		pr_warn("TARGET_CORE[%s]: Expected Transfer Length:"
 			" %u does not match SCSI CDB Length: %u for SAM Opcode:"
@@ -3177,15 +3004,25 @@ static int transport_generic_cmd_sequencer(
 		cmd->data_length = size;
 	}
 
-	if (cmd->se_cmd_flags & SCF_SCSI_DATA_SG_IO_CDB &&
-	    sectors > dev->se_sub_dev->se_dev_attrib.fabric_max_sectors) {
-		printk_ratelimited(KERN_ERR "SCSI OP %02xh with too big sectors %u\n",
-				   cdb[0], sectors);
-		goto out_invalid_cdb_field;
+	if (cmd->se_cmd_flags & SCF_SCSI_DATA_SG_IO_CDB) {
+		if (sectors > su_dev->se_dev_attrib.fabric_max_sectors) {
+			printk_ratelimited(KERN_ERR "SCSI OP %02xh with too"
+				" big sectors %u exceeds fabric_max_sectors:"
+				" %u\n", cdb[0], sectors,
+				su_dev->se_dev_attrib.fabric_max_sectors);
+			goto out_invalid_cdb_field;
+		}
+		if (sectors > su_dev->se_dev_attrib.hw_max_sectors) {
+			printk_ratelimited(KERN_ERR "SCSI OP %02xh with too"
+				" big sectors %u exceeds backend hw_max_sectors:"
+				" %u\n", cdb[0], sectors,
+				su_dev->se_dev_attrib.hw_max_sectors);
+			goto out_invalid_cdb_field;
+		}
 	}
 
 	/* reject any command that we don't have a handler for */
-	if (!(passthrough || cmd->execute_task ||
+	if (!(passthrough || cmd->execute_cmd ||
 	     (cmd->se_cmd_flags & SCF_SCSI_DATA_SG_IO_CDB)))
 		goto out_unsupported_cdb;
 
@@ -3250,7 +3087,7 @@ static void transport_complete_task_attr(struct se_cmd *cmd)
 			cmd_p->t_task_cdb[0],
 			cmd_p->sam_task_attr, cmd_p->se_ordered_id);
 
-		transport_add_tasks_from_cmd(cmd_p);
+		target_add_to_execute_list(cmd_p);
 		new_active_tasks++;
 
 		spin_lock(&dev->delayed_cmd_lock);
@@ -3346,10 +3183,6 @@ static void target_complete_ok_work(struct work_struct *work)
 		if (transport_get_sense_data(cmd) < 0)
 			reason = TCM_NON_EXISTENT_LUN;
 
-		/*
-		 * Only set when an struct se_task->task_scsi_status returned
-		 * a non GOOD status.
-		 */
 		if (cmd->scsi_status) {
 			ret = transport_send_check_condition_and_sense(
 					cmd, reason, 1);
@@ -3424,33 +3257,6 @@ queue_full:
 	transport_handle_queue_full(cmd, cmd->se_dev);
 }
 
-static void transport_free_dev_tasks(struct se_cmd *cmd)
-{
-	struct se_task *task, *task_tmp;
-	unsigned long flags;
-	LIST_HEAD(dispose_list);
-
-	spin_lock_irqsave(&cmd->t_state_lock, flags);
-	list_for_each_entry_safe(task, task_tmp,
-				&cmd->t_task_list, t_list) {
-		if (!(task->task_flags & TF_ACTIVE))
-			list_move_tail(&task->t_list, &dispose_list);
-	}
-	spin_unlock_irqrestore(&cmd->t_state_lock, flags);
-
-	while (!list_empty(&dispose_list)) {
-		task = list_first_entry(&dispose_list, struct se_task, t_list);
-
-		if (task->task_sg != cmd->t_data_sg &&
-		    task->task_sg != cmd->t_bidi_data_sg)
-			kfree(task->task_sg);
-
-		list_del(&task->t_list);
-
-		cmd->se_dev->transport->free_task(task);
-	}
-}
-
 static inline void transport_free_sgl(struct scatterlist *sgl, int nents)
 {
 	struct scatterlist *sg;
@@ -3511,7 +3317,6 @@ static void transport_release_cmd(struct se_cmd *cmd)
 static void transport_put_cmd(struct se_cmd *cmd)
 {
 	unsigned long flags;
-	int free_tasks = 0;
 
 	spin_lock_irqsave(&cmd->t_state_lock, flags);
 	if (atomic_read(&cmd->t_fe_count)) {
@@ -3519,21 +3324,12 @@ static void transport_put_cmd(struct se_cmd *cmd)
 			goto out_busy;
 	}
 
-	if (atomic_read(&cmd->t_se_count)) {
-		if (!atomic_dec_and_test(&cmd->t_se_count))
-			goto out_busy;
-	}
-
 	if (cmd->transport_state & CMD_T_DEV_ACTIVE) {
 		cmd->transport_state &= ~CMD_T_DEV_ACTIVE;
-		transport_all_task_dev_remove_state(cmd);
-		free_tasks = 1;
+		target_remove_from_state_list(cmd);
 	}
 	spin_unlock_irqrestore(&cmd->t_state_lock, flags);
 
-	if (free_tasks != 0)
-		transport_free_dev_tasks(cmd);
-
 	transport_free_pages(cmd);
 	transport_release_cmd(cmd);
 	return;
@@ -3683,245 +3479,14 @@ out:
 	return -ENOMEM;
 }
 
-/* Reduce sectors if they are too long for the device */
-static inline sector_t transport_limit_task_sectors(
-	struct se_device *dev,
-	unsigned long long lba,
-	sector_t sectors)
-{
-	sectors = min_t(sector_t, sectors, dev->se_sub_dev->se_dev_attrib.max_sectors);
-
-	if (dev->transport->get_device_type(dev) == TYPE_DISK)
-		if ((lba + sectors) > transport_dev_end_lba(dev))
-			sectors = ((transport_dev_end_lba(dev) - lba) + 1);
-
-	return sectors;
-}
-
-
-/*
- * This function can be used by HW target mode drivers to create a linked
- * scatterlist from all contiguously allocated struct se_task->task_sg[].
- * This is intended to be called during the completion path by TCM Core
- * when struct target_core_fabric_ops->check_task_sg_chaining is enabled.
- */
-void transport_do_task_sg_chain(struct se_cmd *cmd)
-{
-	struct scatterlist *sg_first = NULL;
-	struct scatterlist *sg_prev = NULL;
-	int sg_prev_nents = 0;
-	struct scatterlist *sg;
-	struct se_task *task;
-	u32 chained_nents = 0;
-	int i;
-
-	BUG_ON(!cmd->se_tfo->task_sg_chaining);
-
-	/*
-	 * Walk the struct se_task list and setup scatterlist chains
-	 * for each contiguously allocated struct se_task->task_sg[].
-	 */
-	list_for_each_entry(task, &cmd->t_task_list, t_list) {
-		if (!task->task_sg)
-			continue;
-
-		if (!sg_first) {
-			sg_first = task->task_sg;
-			chained_nents = task->task_sg_nents;
-		} else {
-			sg_chain(sg_prev, sg_prev_nents, task->task_sg);
-			chained_nents += task->task_sg_nents;
-		}
-		/*
-		 * For the padded tasks, use the extra SGL vector allocated
-		 * in transport_allocate_data_tasks() for the sg_prev_nents
-		 * offset into sg_chain() above.
-		 *
-		 * We do not need the padding for the last task (or a single
-		 * task), but in that case we will never use the sg_prev_nents
-		 * value below which would be incorrect.
-		 */
-		sg_prev_nents = (task->task_sg_nents + 1);
-		sg_prev = task->task_sg;
-	}
-	/*
-	 * Setup the starting pointer and total t_tasks_sg_linked_no including
-	 * padding SGs for linking and to mark the end.
-	 */
-	cmd->t_tasks_sg_chained = sg_first;
-	cmd->t_tasks_sg_chained_no = chained_nents;
-
-	pr_debug("Setup cmd: %p cmd->t_tasks_sg_chained: %p and"
-		" t_tasks_sg_chained_no: %u\n", cmd, cmd->t_tasks_sg_chained,
-		cmd->t_tasks_sg_chained_no);
-
-	for_each_sg(cmd->t_tasks_sg_chained, sg,
-			cmd->t_tasks_sg_chained_no, i) {
-
-		pr_debug("SG[%d]: %p page: %p length: %d offset: %d\n",
-			i, sg, sg_page(sg), sg->length, sg->offset);
-		if (sg_is_chain(sg))
-			pr_debug("SG: %p sg_is_chain=1\n", sg);
-		if (sg_is_last(sg))
-			pr_debug("SG: %p sg_is_last=1\n", sg);
-	}
-}
-EXPORT_SYMBOL(transport_do_task_sg_chain);
-
-/*
- * Break up cmd into chunks transport can handle
- */
-static int
-transport_allocate_data_tasks(struct se_cmd *cmd,
-	enum dma_data_direction data_direction,
-	struct scatterlist *cmd_sg, unsigned int sgl_nents)
-{
-	struct se_device *dev = cmd->se_dev;
-	int task_count, i;
-	unsigned long long lba;
-	sector_t sectors, dev_max_sectors;
-	u32 sector_size;
-
-	if (transport_cmd_get_valid_sectors(cmd) < 0)
-		return -EINVAL;
-
-	dev_max_sectors = dev->se_sub_dev->se_dev_attrib.max_sectors;
-	sector_size = dev->se_sub_dev->se_dev_attrib.block_size;
-
-	WARN_ON(cmd->data_length % sector_size);
-
-	lba = cmd->t_task_lba;
-	sectors = DIV_ROUND_UP(cmd->data_length, sector_size);
-	task_count = DIV_ROUND_UP_SECTOR_T(sectors, dev_max_sectors);
-
-	/*
-	 * If we need just a single task reuse the SG list in the command
-	 * and avoid a lot of work.
-	 */
-	if (task_count == 1) {
-		struct se_task *task;
-		unsigned long flags;
-
-		task = transport_generic_get_task(cmd, data_direction);
-		if (!task)
-			return -ENOMEM;
-
-		task->task_sg = cmd_sg;
-		task->task_sg_nents = sgl_nents;
-
-		task->task_lba = lba;
-		task->task_sectors = sectors;
-		task->task_size = task->task_sectors * sector_size;
-
-		spin_lock_irqsave(&cmd->t_state_lock, flags);
-		list_add_tail(&task->t_list, &cmd->t_task_list);
-		spin_unlock_irqrestore(&cmd->t_state_lock, flags);
-
-		return task_count;
-	}
-
-	for (i = 0; i < task_count; i++) {
-		struct se_task *task;
-		unsigned int task_size, task_sg_nents_padded;
-		struct scatterlist *sg;
-		unsigned long flags;
-		int count;
-
-		task = transport_generic_get_task(cmd, data_direction);
-		if (!task)
-			return -ENOMEM;
-
-		task->task_lba = lba;
-		task->task_sectors = min(sectors, dev_max_sectors);
-		task->task_size = task->task_sectors * sector_size;
-
-		/*
-		 * This now assumes that passed sg_ents are in PAGE_SIZE chunks
-		 * in order to calculate the number per task SGL entries
-		 */
-		task->task_sg_nents = DIV_ROUND_UP(task->task_size, PAGE_SIZE);
-		/*
-		 * Check if the fabric module driver is requesting that all
-		 * struct se_task->task_sg[] be chained together..  If so,
-		 * then allocate an extra padding SG entry for linking and
-		 * marking the end of the chained SGL for every task except
-		 * the last one for (task_count > 1) operation, or skipping
-		 * the extra padding for the (task_count == 1) case.
-		 */
-		if (cmd->se_tfo->task_sg_chaining && (i < (task_count - 1))) {
-			task_sg_nents_padded = (task->task_sg_nents + 1);
-		} else
-			task_sg_nents_padded = task->task_sg_nents;
-
-		task->task_sg = kmalloc(sizeof(struct scatterlist) *
-					task_sg_nents_padded, GFP_KERNEL);
-		if (!task->task_sg) {
-			cmd->se_dev->transport->free_task(task);
-			return -ENOMEM;
-		}
-
-		sg_init_table(task->task_sg, task_sg_nents_padded);
-
-		task_size = task->task_size;
-
-		/* Build new sgl, only up to task_size */
-		for_each_sg(task->task_sg, sg, task->task_sg_nents, count) {
-			if (cmd_sg->length > task_size)
-				break;
-
-			*sg = *cmd_sg;
-			task_size -= cmd_sg->length;
-			cmd_sg = sg_next(cmd_sg);
-		}
-
-		lba += task->task_sectors;
-		sectors -= task->task_sectors;
-
-		spin_lock_irqsave(&cmd->t_state_lock, flags);
-		list_add_tail(&task->t_list, &cmd->t_task_list);
-		spin_unlock_irqrestore(&cmd->t_state_lock, flags);
-	}
-
-	return task_count;
-}
-
-static int
-transport_allocate_control_task(struct se_cmd *cmd)
-{
-	struct se_task *task;
-	unsigned long flags;
-
-	/* Workaround for handling zero-length control CDBs */
-	if ((cmd->se_cmd_flags & SCF_SCSI_CONTROL_SG_IO_CDB) &&
-	    !cmd->data_length)
-		return 0;
-
-	task = transport_generic_get_task(cmd, cmd->data_direction);
-	if (!task)
-		return -ENOMEM;
-
-	task->task_sg = cmd->t_data_sg;
-	task->task_size = cmd->data_length;
-	task->task_sg_nents = cmd->t_data_nents;
-
-	spin_lock_irqsave(&cmd->t_state_lock, flags);
-	list_add_tail(&task->t_list, &cmd->t_task_list);
-	spin_unlock_irqrestore(&cmd->t_state_lock, flags);
-
-	/* Success! Return number of tasks allocated */
-	return 1;
-}
-
 /*
- * Allocate any required ressources to execute the command, and either place
- * it on the execution queue if possible.  For writes we might not have the
- * payload yet, thus notify the fabric via a call to ->write_pending instead.
+ * Allocate any required resources to execute the command.  For writes we
+ * might not have the payload yet, so notify the fabric via a call to
+ * ->write_pending instead. Otherwise place it on the execution queue.
  */
 int transport_generic_new_cmd(struct se_cmd *cmd)
 {
 	struct se_device *dev = cmd->se_dev;
-	int task_cdbs, task_cdbs_bidi = 0;
-	int set_counts = 1;
 	int ret = 0;
 
 	/*
@@ -3936,35 +3501,9 @@ int transport_generic_new_cmd(struct se_cmd *cmd)
 			goto out_fail;
 	}
 
-	/*
-	 * For BIDI command set up the read tasks first.
-	 */
-	if (cmd->t_bidi_data_sg &&
-	    dev->transport->transport_type != TRANSPORT_PLUGIN_PHBA_PDEV) {
-		BUG_ON(!(cmd->se_cmd_flags & SCF_SCSI_DATA_SG_IO_CDB));
-
-		task_cdbs_bidi = transport_allocate_data_tasks(cmd,
-				DMA_FROM_DEVICE, cmd->t_bidi_data_sg,
-				cmd->t_bidi_data_nents);
-		if (task_cdbs_bidi <= 0)
-			goto out_fail;
-
-		atomic_inc(&cmd->t_fe_count);
-		atomic_inc(&cmd->t_se_count);
-		set_counts = 0;
-	}
-
-	if (cmd->se_cmd_flags & SCF_SCSI_DATA_SG_IO_CDB) {
-		task_cdbs = transport_allocate_data_tasks(cmd,
-					cmd->data_direction, cmd->t_data_sg,
-					cmd->t_data_nents);
-	} else {
-		task_cdbs = transport_allocate_control_task(cmd);
-	}
-
-	if (task_cdbs < 0)
-		goto out_fail;
-	else if (!task_cdbs && (cmd->se_cmd_flags & SCF_SCSI_DATA_SG_IO_CDB)) {
+	/* Workaround for handling zero-length control CDBs */
+	if ((cmd->se_cmd_flags & SCF_SCSI_CONTROL_SG_IO_CDB) &&
+	    !cmd->data_length) {
 		spin_lock_irq(&cmd->t_state_lock);
 		cmd->t_state = TRANSPORT_COMPLETE;
 		cmd->transport_state |= CMD_T_ACTIVE;
@@ -3982,29 +3521,31 @@ int transport_generic_new_cmd(struct se_cmd *cmd)
 		return 0;
 	}
 
-	if (set_counts) {
-		atomic_inc(&cmd->t_fe_count);
-		atomic_inc(&cmd->t_se_count);
+	if (cmd->se_cmd_flags & SCF_SCSI_DATA_SG_IO_CDB) {
+		struct se_dev_attrib *attr = &dev->se_sub_dev->se_dev_attrib;
+
+		if (transport_cmd_get_valid_sectors(cmd) < 0)
+			return -EINVAL;
+
+		BUG_ON(cmd->data_length % attr->block_size);
+		BUG_ON(DIV_ROUND_UP(cmd->data_length, attr->block_size) >
+			attr->hw_max_sectors);
 	}
 
-	cmd->t_task_list_num = (task_cdbs + task_cdbs_bidi);
-	atomic_set(&cmd->t_task_cdbs_left, cmd->t_task_list_num);
-	atomic_set(&cmd->t_task_cdbs_ex_left, cmd->t_task_list_num);
+	atomic_inc(&cmd->t_fe_count);
 
 	/*
-	 * For WRITEs, let the fabric know its buffer is ready..
-	 * This WRITE struct se_cmd (and all of its associated struct se_task's)
-	 * will be added to the struct se_device execution queue after its WRITE
-	 * data has arrived. (ie: It gets handled by the transport processing
-	 * thread a second time)
+	 * For WRITEs, let the fabric know its buffer is ready.
+	 *
+	 * The command will be added to the execution queue after its write
+	 * data has arrived.
 	 */
 	if (cmd->data_direction == DMA_TO_DEVICE) {
-		transport_add_tasks_to_state_queue(cmd);
+		target_add_to_state_list(cmd);
 		return transport_generic_write_pending(cmd);
 	}
 	/*
-	 * Everything else but a WRITE, add the struct se_cmd's struct se_task's
-	 * to the execution queue.
+	 * Everything else but a WRITE, add the command to the execution queue.
 	 */
 	transport_execute_tasks(cmd);
 	return 0;
@@ -4091,8 +3632,6 @@ void transport_generic_free_cmd(struct se_cmd *cmd, int wait_for_tasks)
 		if (cmd->se_lun)
 			transport_lun_remove_cmd(cmd);
 
-		transport_free_dev_tasks(cmd);
-
 		transport_put_cmd(cmd);
 	}
 }
@@ -4233,7 +3772,8 @@ EXPORT_SYMBOL(target_wait_for_sess_cmds);
 static int transport_lun_wait_for_tasks(struct se_cmd *cmd, struct se_lun *lun)
 {
 	unsigned long flags;
-	int ret;
+	int ret = 0;
+
 	/*
 	 * If the frontend has already requested this struct se_cmd to
 	 * be stopped, we can safely ignore this struct se_cmd.
@@ -4253,10 +3793,21 @@ static int transport_lun_wait_for_tasks(struct se_cmd *cmd, struct se_lun *lun)
 
 	wake_up_interruptible(&cmd->se_dev->dev_queue_obj.thread_wq);
 
-	ret = transport_stop_tasks_for_cmd(cmd);
+	// XXX: audit task_flags checks.
+	spin_lock_irqsave(&cmd->t_state_lock, flags);
+	if ((cmd->transport_state & CMD_T_BUSY) &&
+	    (cmd->transport_state & CMD_T_SENT)) {
+		if (!target_stop_cmd(cmd, &flags))
+			ret++;
+		spin_unlock_irqrestore(&cmd->t_state_lock, flags);
+	} else {
+		spin_unlock_irqrestore(&cmd->t_state_lock,
+				flags);
+		target_remove_from_execute_list(cmd);
+	}
 
-	pr_debug("ConfigFS: cmd: %p t_tasks: %d stop tasks ret:"
-			" %d\n", cmd, cmd->t_task_list_num, ret);
+	pr_debug("ConfigFS: cmd: %p stop tasks ret:"
+			" %d\n", cmd, ret);
 	if (!ret) {
 		pr_debug("ConfigFS: ITT[0x%08x] - stopping cmd....\n",
 				cmd->se_tfo->get_task_tag(cmd));
@@ -4328,10 +3879,9 @@ static void __transport_clear_lun_from_sessions(struct se_lun *lun)
 			goto check_cond;
 		}
 		cmd->transport_state &= ~CMD_T_DEV_ACTIVE;
-		transport_all_task_dev_remove_state(cmd);
+		target_remove_from_state_list(cmd);
 		spin_unlock_irqrestore(&cmd->t_state_lock, cmd_flags);
 
-		transport_free_dev_tasks(cmd);
 		/*
 		 * The Storage engine stopped this struct se_cmd before it was
 		 * send to the fabric frontend for delivery back to the
@@ -4444,7 +3994,7 @@ bool transport_wait_for_tasks(struct se_cmd *cmd)
 		wait_for_completion(&cmd->transport_lun_fe_stop_comp);
 		spin_lock_irqsave(&cmd->t_state_lock, flags);
 
-		transport_all_task_dev_remove_state(cmd);
+		target_remove_from_state_list(cmd);
 		/*
 		 * At this point, the frontend who was the originator of this
 		 * struct se_cmd, now owns the structure and can be released through
@@ -4710,12 +4260,12 @@ int transport_check_aborted_status(struct se_cmd *cmd, int send_status)
 		if (!send_status ||
 		     (cmd->se_cmd_flags & SCF_SENT_DELAYED_TAS))
 			return 1;
-#if 0
+
 		pr_debug("Sending delayed SAM_STAT_TASK_ABORTED"
 			" status for CDB: 0x%02x ITT: 0x%08x\n",
 			cmd->t_task_cdb[0],
 			cmd->se_tfo->get_task_tag(cmd));
-#endif
+
 		cmd->se_cmd_flags |= SCF_SENT_DELAYED_TAS;
 		cmd->se_tfo->queue_status(cmd);
 		ret = 1;
@@ -4748,11 +4298,11 @@ void transport_send_task_abort(struct se_cmd *cmd)
 		}
 	}
 	cmd->scsi_status = SAM_STAT_TASK_ABORTED;
-#if 0
+
 	pr_debug("Setting SAM_STAT_TASK_ABORTED status for CDB: 0x%02x,"
 		" ITT: 0x%08x\n", cmd->t_task_cdb[0],
 		cmd->se_tfo->get_task_tag(cmd));
-#endif
+
 	cmd->se_tfo->queue_status(cmd);
 }
 
@@ -4865,7 +4415,7 @@ get_cmd:
 	}
 
 out:
-	WARN_ON(!list_empty(&dev->state_task_list));
+	WARN_ON(!list_empty(&dev->state_list));
 	WARN_ON(!list_empty(&dev->dev_queue_obj.qobj_list));
 	dev->process_thread = NULL;
 	return 0;
diff --git a/drivers/target/tcm_fc/tfc_cmd.c b/drivers/target/tcm_fc/tfc_cmd.c
index a375f257aab..f03fb9730f5 100644
--- a/drivers/target/tcm_fc/tfc_cmd.c
+++ b/drivers/target/tcm_fc/tfc_cmd.c
@@ -215,20 +215,10 @@ int ft_write_pending(struct se_cmd *se_cmd)
 		 */
 		if ((ep->xid <= lport->lro_xid) &&
 		    (fh->fh_r_ctl == FC_RCTL_DD_DATA_DESC)) {
-			if (se_cmd->se_cmd_flags & SCF_SCSI_DATA_SG_IO_CDB) {
-				/*
-				 * cmd may have been broken up into multiple
-				 * tasks. Link their sgs together so we can
-				 * operate on them all at once.
-				 */
-				transport_do_task_sg_chain(se_cmd);
-				cmd->sg = se_cmd->t_tasks_sg_chained;
-				cmd->sg_cnt =
-					se_cmd->t_tasks_sg_chained_no;
-			}
-			if (cmd->sg && lport->tt.ddp_target(lport, ep->xid,
-							    cmd->sg,
-							    cmd->sg_cnt))
+			if ((se_cmd->se_cmd_flags & SCF_SCSI_DATA_SG_IO_CDB) &&
+			    lport->tt.ddp_target(lport, ep->xid,
+						 se_cmd->t_data_sg,
+						 se_cmd->t_data_nents))
 				cmd->was_ddp_setup = 1;
 		}
 	}
diff --git a/drivers/target/tcm_fc/tfc_conf.c b/drivers/target/tcm_fc/tfc_conf.c
index 2948dc94461..9501844fae2 100644
--- a/drivers/target/tcm_fc/tfc_conf.c
+++ b/drivers/target/tcm_fc/tfc_conf.c
@@ -576,9 +576,6 @@ int ft_register_configfs(void)
 	}
 	fabric->tf_ops = ft_fabric_ops;
 
-	/* Allowing support for task_sg_chaining */
-	fabric->tf_ops.task_sg_chaining = 1;
-
 	/*
 	 * Setup default attribute lists for various fabric->tf_cit_tmpl
 	 */
diff --git a/drivers/target/tcm_fc/tfc_io.c b/drivers/target/tcm_fc/tfc_io.c
index dc7c0db26e2..071a505f98f 100644
--- a/drivers/target/tcm_fc/tfc_io.c
+++ b/drivers/target/tcm_fc/tfc_io.c
@@ -228,7 +228,7 @@ void ft_recv_write_data(struct ft_cmd *cmd, struct fc_frame *fp)
 				"payload, Frame will be dropped if"
 				"'Sequence Initiative' bit in f_ctl is"
 				"not set\n", __func__, ep->xid, f_ctl,
-				cmd->sg, cmd->sg_cnt);
+				se_cmd->t_data_sg, se_cmd->t_data_nents);
 		/*
 		 * Invalidate HW DDP context if it was setup for respective
 		 * command. Invalidation of HW DDP context is requited in both
diff --git a/drivers/tty/amiserial.c b/drivers/tty/amiserial.c
index 6cc4358f68c..35819e31262 100644
--- a/drivers/tty/amiserial.c
+++ b/drivers/tty/amiserial.c
@@ -1033,7 +1033,7 @@ static int get_serial_info(struct tty_struct *tty, struct serial_state *state,
 	if (!retinfo)
 		return -EFAULT;
 	memset(&tmp, 0, sizeof(tmp));
-	tty_lock();
+	tty_lock(tty);
 	tmp.line = tty->index;
 	tmp.port = state->port;
 	tmp.flags = state->tport.flags;
@@ -1042,7 +1042,7 @@ static int get_serial_info(struct tty_struct *tty, struct serial_state *state,
 	tmp.close_delay = state->tport.close_delay;
 	tmp.closing_wait = state->tport.closing_wait;
 	tmp.custom_divisor = state->custom_divisor;
-	tty_unlock();
+	tty_unlock(tty);
 	if (copy_to_user(retinfo,&tmp,sizeof(*retinfo)))
 		return -EFAULT;
 	return 0;
@@ -1059,12 +1059,12 @@ static int set_serial_info(struct tty_struct *tty, struct serial_state *state,
 	if (copy_from_user(&new_serial,new_info,sizeof(new_serial)))
 		return -EFAULT;
 
-	tty_lock();
+	tty_lock(tty);
 	change_spd = ((new_serial.flags ^ port->flags) & ASYNC_SPD_MASK) ||
 		new_serial.custom_divisor != state->custom_divisor;
 	if (new_serial.irq || new_serial.port != state->port ||
 			new_serial.xmit_fifo_size != state->xmit_fifo_size) {
-		tty_unlock();
+		tty_unlock(tty);
 		return -EINVAL;
 	}
   
@@ -1074,7 +1074,7 @@ static int set_serial_info(struct tty_struct *tty, struct serial_state *state,
 		    (new_serial.xmit_fifo_size != state->xmit_fifo_size) ||
 		    ((new_serial.flags & ~ASYNC_USR_MASK) !=
 		     (port->flags & ~ASYNC_USR_MASK))) {
-			tty_unlock();
+			tty_unlock(tty);
 			return -EPERM;
 		}
 		port->flags = ((port->flags & ~ASYNC_USR_MASK) |
@@ -1084,7 +1084,7 @@ static int set_serial_info(struct tty_struct *tty, struct serial_state *state,
 	}
 
 	if (new_serial.baud_base < 9600) {
-		tty_unlock();
+		tty_unlock(tty);
 		return -EINVAL;
 	}
 
@@ -1116,7 +1116,7 @@ check_and_exit:
 		}
 	} else
 		retval = startup(tty, state);
-	tty_unlock();
+	tty_unlock(tty);
 	return retval;
 }
 
diff --git a/drivers/tty/bfin_jtag_comm.c b/drivers/tty/bfin_jtag_comm.c
index 946f799861f..61fc74fe174 100644
--- a/drivers/tty/bfin_jtag_comm.c
+++ b/drivers/tty/bfin_jtag_comm.c
@@ -62,9 +62,7 @@ static inline uint32_t bfin_write_emudat_chars(char a, char b, char c, char d)
 
 static struct tty_driver *bfin_jc_driver;
 static struct task_struct *bfin_jc_kthread;
-static struct tty_struct * volatile bfin_jc_tty;
-static unsigned long bfin_jc_count;
-static DEFINE_MUTEX(bfin_jc_tty_mutex);
+static struct tty_port port;
 static volatile struct circ_buf bfin_jc_write_buf;
 
 static int
@@ -73,18 +71,21 @@ bfin_jc_emudat_manager(void *arg)
 	uint32_t inbound_len = 0, outbound_len = 0;
 
 	while (!kthread_should_stop()) {
+		struct tty_struct *tty = tty_port_tty_get(&port);
 		/* no one left to give data to, so sleep */
-		if (bfin_jc_tty == NULL && circ_empty(&bfin_jc_write_buf)) {
+		if (tty == NULL && circ_empty(&bfin_jc_write_buf)) {
 			pr_debug("waiting for readers\n");
 			__set_current_state(TASK_UNINTERRUPTIBLE);
 			schedule();
 			__set_current_state(TASK_RUNNING);
+			continue;
 		}
 
 		/* no data available, so just chill */
 		if (!(bfin_read_DBGSTAT() & EMUDIF) && circ_empty(&bfin_jc_write_buf)) {
 			pr_debug("waiting for data (in_len = %i) (circ: %i %i)\n",
 				inbound_len, bfin_jc_write_buf.tail, bfin_jc_write_buf.head);
+			tty_kref_put(tty);
 			if (inbound_len)
 				schedule();
 			else
@@ -94,9 +95,6 @@ bfin_jc_emudat_manager(void *arg)
 
 		/* if incoming data is ready, eat it */
 		if (bfin_read_DBGSTAT() & EMUDIF) {
-			struct tty_struct *tty;
-			mutex_lock(&bfin_jc_tty_mutex);
-			tty = (struct tty_struct *)bfin_jc_tty;
 			if (tty != NULL) {
 				uint32_t emudat = bfin_read_emudat();
 				if (inbound_len == 0) {
@@ -110,7 +108,6 @@ bfin_jc_emudat_manager(void *arg)
 					tty_flip_buffer_push(tty);
 				}
 			}
-			mutex_unlock(&bfin_jc_tty_mutex);
 		}
 
 		/* if outgoing data is ready, post it */
@@ -120,7 +117,6 @@ bfin_jc_emudat_manager(void *arg)
 				bfin_write_emudat(outbound_len);
 				pr_debug("outgoing length: 0x%08x\n", outbound_len);
 			} else {
-				struct tty_struct *tty;
 				int tail = bfin_jc_write_buf.tail;
 				size_t ate = (4 <= outbound_len ? 4 : outbound_len);
 				uint32_t emudat =
@@ -132,14 +128,12 @@ bfin_jc_emudat_manager(void *arg)
 				);
 				bfin_jc_write_buf.tail += ate;
 				outbound_len -= ate;
-				mutex_lock(&bfin_jc_tty_mutex);
-				tty = (struct tty_struct *)bfin_jc_tty;
 				if (tty)
 					tty_wakeup(tty);
-				mutex_unlock(&bfin_jc_tty_mutex);
 				pr_debug("  outgoing data: 0x%08x (pushing %zu)\n", emudat, ate);
 			}
 		}
+		tty_kref_put(tty);
 	}
 
 	__set_current_state(TASK_RUNNING);
@@ -149,24 +143,28 @@ bfin_jc_emudat_manager(void *arg)
 static int
 bfin_jc_open(struct tty_struct *tty, struct file *filp)
 {
-	mutex_lock(&bfin_jc_tty_mutex);
-	pr_debug("open %lu\n", bfin_jc_count);
-	++bfin_jc_count;
-	bfin_jc_tty = tty;
+	unsigned long flags;
+
+	spin_lock_irqsave(&port.lock, flags);
+	port.count++;
+	spin_unlock_irqrestore(&port.lock, flags);
+	tty_port_tty_set(&port, tty);
 	wake_up_process(bfin_jc_kthread);
-	mutex_unlock(&bfin_jc_tty_mutex);
 	return 0;
 }
 
 static void
 bfin_jc_close(struct tty_struct *tty, struct file *filp)
 {
-	mutex_lock(&bfin_jc_tty_mutex);
-	pr_debug("close %lu\n", bfin_jc_count);
-	if (--bfin_jc_count == 0)
-		bfin_jc_tty = NULL;
+	unsigned long flags;
+	bool last;
+
+	spin_lock_irqsave(&port.lock, flags);
+	last = --port.count == 0;
+	spin_unlock_irqrestore(&port.lock, flags);
+	if (last)
+		tty_port_tty_set(&port, NULL);
 	wake_up_process(bfin_jc_kthread);
-	mutex_unlock(&bfin_jc_tty_mutex);
 }
 
 /* XXX: we dont handle the put_char() case where we must handle count = 1 */
@@ -242,6 +240,8 @@ static int __init bfin_jc_init(void)
 {
 	int ret;
 
+	tty_port_init(&port);
+
 	bfin_jc_kthread = kthread_create(bfin_jc_emudat_manager, NULL, DRV_NAME);
 	if (IS_ERR(bfin_jc_kthread))
 		return PTR_ERR(bfin_jc_kthread);
diff --git a/drivers/tty/cyclades.c b/drivers/tty/cyclades.c
index e61cabdd69d..6984e1a2686 100644
--- a/drivers/tty/cyclades.c
+++ b/drivers/tty/cyclades.c
@@ -1599,7 +1599,7 @@ static int cy_open(struct tty_struct *tty, struct file *filp)
 	 * If the port is the middle of closing, bail out now
 	 */
 	if (tty_hung_up_p(filp) || (info->port.flags & ASYNC_CLOSING)) {
-		wait_event_interruptible_tty(info->port.close_wait,
+		wait_event_interruptible_tty(tty, info->port.close_wait,
 				!(info->port.flags & ASYNC_CLOSING));
 		return (info->port.flags & ASYNC_HUP_NOTIFY) ? -EAGAIN: -ERESTARTSYS;
 	}
diff --git a/drivers/tty/hvc/hvc_console.c b/drivers/tty/hvc/hvc_console.c
index 8880adf5fc6..2d691eb7c40 100644
--- a/drivers/tty/hvc/hvc_console.c
+++ b/drivers/tty/hvc/hvc_console.c
@@ -107,7 +107,7 @@ static struct hvc_struct *hvc_get_by_index(int index)
 	list_for_each_entry(hp, &hvc_structs, next) {
 		spin_lock_irqsave(&hp->lock, flags);
 		if (hp->index == index) {
-			kref_get(&hp->kref);
+			tty_port_get(&hp->port);
 			spin_unlock_irqrestore(&hp->lock, flags);
 			spin_unlock(&hvc_structs_lock);
 			return hp;
@@ -229,9 +229,9 @@ static int __init hvc_console_init(void)
 console_initcall(hvc_console_init);
 
 /* callback when the kboject ref count reaches zero. */
-static void destroy_hvc_struct(struct kref *kref)
+static void hvc_port_destruct(struct tty_port *port)
 {
-	struct hvc_struct *hp = container_of(kref, struct hvc_struct, kref);
+	struct hvc_struct *hp = container_of(port, struct hvc_struct, port);
 	unsigned long flags;
 
 	spin_lock(&hvc_structs_lock);
@@ -264,7 +264,7 @@ int hvc_instantiate(uint32_t vtermno, int index, const struct hv_ops *ops)
 	/* make sure no no tty has been registered in this index */
 	hp = hvc_get_by_index(index);
 	if (hp) {
-		kref_put(&hp->kref, destroy_hvc_struct);
+		tty_port_put(&hp->port);
 		return -1;
 	}
 
@@ -313,20 +313,17 @@ static int hvc_open(struct tty_struct *tty, struct file * filp)
 	if (!(hp = hvc_get_by_index(tty->index)))
 		return -ENODEV;
 
-	spin_lock_irqsave(&hp->lock, flags);
+	spin_lock_irqsave(&hp->port.lock, flags);
 	/* Check and then increment for fast path open. */
-	if (hp->count++ > 0) {
-		tty_kref_get(tty);
-		spin_unlock_irqrestore(&hp->lock, flags);
+	if (hp->port.count++ > 0) {
+		spin_unlock_irqrestore(&hp->port.lock, flags);
 		hvc_kick();
 		return 0;
 	} /* else count == 0 */
+	spin_unlock_irqrestore(&hp->port.lock, flags);
 
 	tty->driver_data = hp;
-
-	hp->tty = tty_kref_get(tty);
-
-	spin_unlock_irqrestore(&hp->lock, flags);
+	tty_port_tty_set(&hp->port, tty);
 
 	if (hp->ops->notifier_add)
 		rc = hp->ops->notifier_add(hp, hp->data);
@@ -338,12 +335,9 @@ static int hvc_open(struct tty_struct *tty, struct file * filp)
 	 * tty fields and return the kref reference.
 	 */
 	if (rc) {
-		spin_lock_irqsave(&hp->lock, flags);
-		hp->tty = NULL;
-		spin_unlock_irqrestore(&hp->lock, flags);
-		tty_kref_put(tty);
+		tty_port_tty_set(&hp->port, NULL);
 		tty->driver_data = NULL;
-		kref_put(&hp->kref, destroy_hvc_struct);
+		tty_port_put(&hp->port);
 		printk(KERN_ERR "hvc_open: request_irq failed with rc %d.\n", rc);
 	}
 	/* Force wakeup of the polling thread */
@@ -370,12 +364,12 @@ static void hvc_close(struct tty_struct *tty, struct file * filp)
 
 	hp = tty->driver_data;
 
-	spin_lock_irqsave(&hp->lock, flags);
+	spin_lock_irqsave(&hp->port.lock, flags);
 
-	if (--hp->count == 0) {
+	if (--hp->port.count == 0) {
+		spin_unlock_irqrestore(&hp->port.lock, flags);
 		/* We are done with the tty pointer now. */
-		hp->tty = NULL;
-		spin_unlock_irqrestore(&hp->lock, flags);
+		tty_port_tty_set(&hp->port, NULL);
 
 		if (hp->ops->notifier_del)
 			hp->ops->notifier_del(hp, hp->data);
@@ -390,14 +384,13 @@ static void hvc_close(struct tty_struct *tty, struct file * filp)
 		 */
 		tty_wait_until_sent_from_close(tty, HVC_CLOSE_WAIT);
 	} else {
-		if (hp->count < 0)
+		if (hp->port.count < 0)
 			printk(KERN_ERR "hvc_close %X: oops, count is %d\n",
-				hp->vtermno, hp->count);
-		spin_unlock_irqrestore(&hp->lock, flags);
+				hp->vtermno, hp->port.count);
+		spin_unlock_irqrestore(&hp->port.lock, flags);
 	}
 
-	tty_kref_put(tty);
-	kref_put(&hp->kref, destroy_hvc_struct);
+	tty_port_put(&hp->port);
 }
 
 static void hvc_hangup(struct tty_struct *tty)
@@ -412,32 +405,31 @@ static void hvc_hangup(struct tty_struct *tty)
 	/* cancel pending tty resize work */
 	cancel_work_sync(&hp->tty_resize);
 
-	spin_lock_irqsave(&hp->lock, flags);
+	spin_lock_irqsave(&hp->port.lock, flags);
 
 	/*
 	 * The N_TTY line discipline has problems such that in a close vs
 	 * open->hangup case this can be called after the final close so prevent
 	 * that from happening for now.
 	 */
-	if (hp->count <= 0) {
-		spin_unlock_irqrestore(&hp->lock, flags);
+	if (hp->port.count <= 0) {
+		spin_unlock_irqrestore(&hp->port.lock, flags);
 		return;
 	}
 
-	temp_open_count = hp->count;
-	hp->count = 0;
-	hp->n_outbuf = 0;
-	hp->tty = NULL;
+	temp_open_count = hp->port.count;
+	hp->port.count = 0;
+	spin_unlock_irqrestore(&hp->port.lock, flags);
+	tty_port_tty_set(&hp->port, NULL);
 
-	spin_unlock_irqrestore(&hp->lock, flags);
+	hp->n_outbuf = 0;
 
 	if (hp->ops->notifier_hangup)
 		hp->ops->notifier_hangup(hp, hp->data);
 
 	while(temp_open_count) {
 		--temp_open_count;
-		tty_kref_put(tty);
-		kref_put(&hp->kref, destroy_hvc_struct);
+		tty_port_put(&hp->port);
 	}
 }
 
@@ -478,7 +470,8 @@ static int hvc_write(struct tty_struct *tty, const unsigned char *buf, int count
 	if (!hp)
 		return -EPIPE;
 
-	if (hp->count <= 0)
+	/* FIXME what's this (unprotected) check for? */
+	if (hp->port.count <= 0)
 		return -EIO;
 
 	spin_lock_irqsave(&hp->lock, flags);
@@ -526,13 +519,12 @@ static void hvc_set_winsz(struct work_struct *work)
 
 	hp = container_of(work, struct hvc_struct, tty_resize);
 
-	spin_lock_irqsave(&hp->lock, hvc_flags);
-	if (!hp->tty) {
-		spin_unlock_irqrestore(&hp->lock, hvc_flags);
+	tty = tty_port_tty_get(&hp->port);
+	if (!tty)
 		return;
-	}
-	ws  = hp->ws;
-	tty = tty_kref_get(hp->tty);
+
+	spin_lock_irqsave(&hp->lock, hvc_flags);
+	ws = hp->ws;
 	spin_unlock_irqrestore(&hp->lock, hvc_flags);
 
 	tty_do_resize(tty, &ws);
@@ -601,7 +593,7 @@ int hvc_poll(struct hvc_struct *hp)
 	}
 
 	/* No tty attached, just skip */
-	tty = tty_kref_get(hp->tty);
+	tty = tty_port_tty_get(&hp->port);
 	if (tty == NULL)
 		goto bail;
 
@@ -681,8 +673,7 @@ int hvc_poll(struct hvc_struct *hp)
 
 		tty_flip_buffer_push(tty);
 	}
-	if (tty)
-		tty_kref_put(tty);
+	tty_kref_put(tty);
 
 	return poll_mask;
 }
@@ -817,6 +808,10 @@ static const struct tty_operations hvc_ops = {
 #endif
 };
 
+static const struct tty_port_operations hvc_port_ops = {
+	.destruct = hvc_port_destruct,
+};
+
 struct hvc_struct *hvc_alloc(uint32_t vtermno, int data,
 			     const struct hv_ops *ops,
 			     int outbuf_size)
@@ -842,7 +837,8 @@ struct hvc_struct *hvc_alloc(uint32_t vtermno, int data,
 	hp->outbuf_size = outbuf_size;
 	hp->outbuf = &((char *)hp)[ALIGN(sizeof(*hp), sizeof(long))];
 
-	kref_init(&hp->kref);
+	tty_port_init(&hp->port);
+	hp->port.ops = &hvc_port_ops;
 
 	INIT_WORK(&hp->tty_resize, hvc_set_winsz);
 	spin_lock_init(&hp->lock);
@@ -875,9 +871,9 @@ int hvc_remove(struct hvc_struct *hp)
 	unsigned long flags;
 	struct tty_struct *tty;
 
-	spin_lock_irqsave(&hp->lock, flags);
-	tty = tty_kref_get(hp->tty);
+	tty = tty_port_tty_get(&hp->port);
 
+	spin_lock_irqsave(&hp->lock, flags);
 	if (hp->index < MAX_NR_HVC_CONSOLES)
 		vtermnos[hp->index] = -1;
 
@@ -891,7 +887,7 @@ int hvc_remove(struct hvc_struct *hp)
 	 * kref cause it to be removed, which will probably be the tty_vhangup
 	 * below.
 	 */
-	kref_put(&hp->kref, destroy_hvc_struct);
+	tty_port_put(&hp->port);
 
 	/*
 	 * This function call will auto chain call hvc_hangup.
diff --git a/drivers/tty/hvc/hvc_console.h b/drivers/tty/hvc/hvc_console.h
index c335a1492a5..674d23cb919 100644
--- a/drivers/tty/hvc/hvc_console.h
+++ b/drivers/tty/hvc/hvc_console.h
@@ -46,10 +46,9 @@
 #define HVC_ALLOC_TTY_ADAPTERS	8
 
 struct hvc_struct {
+	struct tty_port port;
 	spinlock_t lock;
 	int index;
-	struct tty_struct *tty;
-	int count;
 	int do_wakeup;
 	char *outbuf;
 	int outbuf_size;
@@ -61,7 +60,6 @@ struct hvc_struct {
 	struct winsize ws;
 	struct work_struct tty_resize;
 	struct list_head next;
-	struct kref kref; /* ref count & hvc_struct lifetime */
 };
 
 /* implemented by a low level driver */
diff --git a/drivers/tty/hvc/hvc_xen.c b/drivers/tty/hvc/hvc_xen.c
index 83d5c88e716..d3d91dae065 100644
--- a/drivers/tty/hvc/hvc_xen.c
+++ b/drivers/tty/hvc/hvc_xen.c
@@ -430,9 +430,9 @@ static int __devinit xencons_probe(struct xenbus_device *dev,
 	if (devid == 0)
 		return -ENODEV;
 
-	info = kzalloc(sizeof(struct xencons_info), GFP_KERNEL | __GFP_ZERO);
+	info = kzalloc(sizeof(struct xencons_info), GFP_KERNEL);
 	if (!info)
-		goto error_nomem;
+		return -ENOMEM;
 	dev_set_drvdata(&dev->dev, info);
 	info->xbdev = dev;
 	info->vtermno = xenbus_devid_to_vtermno(devid);
diff --git a/drivers/tty/hvc/hvcs.c b/drivers/tty/hvc/hvcs.c
index 3436436fe2d..d56788c8397 100644
--- a/drivers/tty/hvc/hvcs.c
+++ b/drivers/tty/hvc/hvcs.c
@@ -261,6 +261,7 @@ static DEFINE_SPINLOCK(hvcs_pi_lock);
 
 /* One vty-server per hvcs_struct */
 struct hvcs_struct {
+	struct tty_port port;
 	spinlock_t lock;
 
 	/*
@@ -269,9 +270,6 @@ struct hvcs_struct {
 	 */
 	unsigned int index;
 
-	struct tty_struct *tty;
-	int open_count;
-
 	/*
 	 * Used to tell the driver kernel_thread what operations need to take
 	 * place upon this hvcs_struct instance.
@@ -290,12 +288,11 @@ struct hvcs_struct {
 	int chars_in_buffer;
 
 	/*
-	 * Any variable below the kref is valid before a tty is connected and
+	 * Any variable below is valid before a tty is connected and
 	 * stays valid after the tty is disconnected.  These shouldn't be
 	 * whacked until the kobject refcount reaches zero though some entries
 	 * may be changed via sysfs initiatives.
 	 */
-	struct kref kref; /* ref count & hvcs_struct lifetime */
 	int connected; /* is the vty-server currently connected to a vty? */
 	uint32_t p_unit_address; /* partner unit address */
 	uint32_t p_partition_ID; /* partner partition ID */
@@ -304,9 +301,6 @@ struct hvcs_struct {
 	struct vio_dev *vdev;
 };
 
-/* Required to back map a kref to its containing object */
-#define from_kref(k) container_of(k, struct hvcs_struct, kref)
-
 static LIST_HEAD(hvcs_structs);
 static DEFINE_SPINLOCK(hvcs_structs_lock);
 static DEFINE_MUTEX(hvcs_init_mutex);
@@ -422,7 +416,7 @@ static ssize_t hvcs_vterm_state_store(struct device *dev, struct device_attribut
 
 	spin_lock_irqsave(&hvcsd->lock, flags);
 
-	if (hvcsd->open_count > 0) {
+	if (hvcsd->port.count > 0) {
 		spin_unlock_irqrestore(&hvcsd->lock, flags);
 		printk(KERN_INFO "HVCS: vterm state unchanged.  "
 				"The hvcs device node is still in use.\n");
@@ -564,7 +558,7 @@ static irqreturn_t hvcs_handle_interrupt(int irq, void *dev_instance)
 static void hvcs_try_write(struct hvcs_struct *hvcsd)
 {
 	uint32_t unit_address = hvcsd->vdev->unit_address;
-	struct tty_struct *tty = hvcsd->tty;
+	struct tty_struct *tty = hvcsd->port.tty;
 	int sent;
 
 	if (hvcsd->todo_mask & HVCS_TRY_WRITE) {
@@ -602,7 +596,7 @@ static int hvcs_io(struct hvcs_struct *hvcsd)
 	spin_lock_irqsave(&hvcsd->lock, flags);
 
 	unit_address = hvcsd->vdev->unit_address;
-	tty = hvcsd->tty;
+	tty = hvcsd->port.tty;
 
 	hvcs_try_write(hvcsd);
 
@@ -701,10 +695,9 @@ static void hvcs_return_index(int index)
 		hvcs_index_list[index] = -1;
 }
 
-/* callback when the kref ref count reaches zero */
-static void destroy_hvcs_struct(struct kref *kref)
+static void hvcs_destruct_port(struct tty_port *p)
 {
-	struct hvcs_struct *hvcsd = from_kref(kref);
+	struct hvcs_struct *hvcsd = container_of(p, struct hvcs_struct, port);
 	struct vio_dev *vdev;
 	unsigned long flags;
 
@@ -741,6 +734,10 @@ static void destroy_hvcs_struct(struct kref *kref)
 	kfree(hvcsd);
 }
 
+static const struct tty_port_operations hvcs_port_ops = {
+	.destruct = hvcs_destruct_port,
+};
+
 static int hvcs_get_index(void)
 {
 	int i;
@@ -789,10 +786,9 @@ static int __devinit hvcs_probe(
 	if (!hvcsd)
 		return -ENODEV;
 
-
+	tty_port_init(&hvcsd->port);
+	hvcsd->port.ops = &hvcs_port_ops;
 	spin_lock_init(&hvcsd->lock);
-	/* Automatically incs the refcount the first time */
-	kref_init(&hvcsd->kref);
 
 	hvcsd->vdev = dev;
 	dev_set_drvdata(&dev->dev, hvcsd);
@@ -852,7 +848,7 @@ static int __devexit hvcs_remove(struct vio_dev *dev)
 
 	spin_lock_irqsave(&hvcsd->lock, flags);
 
-	tty = hvcsd->tty;
+	tty = hvcsd->port.tty;
 
 	spin_unlock_irqrestore(&hvcsd->lock, flags);
 
@@ -860,7 +856,7 @@ static int __devexit hvcs_remove(struct vio_dev *dev)
 	 * Let the last holder of this object cause it to be removed, which
 	 * would probably be tty_hangup below.
 	 */
-	kref_put(&hvcsd->kref, destroy_hvcs_struct);
+	tty_port_put(&hvcsd->port);
 
 	/*
 	 * The hangup is a scheduled function which will auto chain call
@@ -1094,7 +1090,7 @@ static struct hvcs_struct *hvcs_get_by_index(int index)
 	list_for_each_entry(hvcsd, &hvcs_structs, next) {
 		spin_lock_irqsave(&hvcsd->lock, flags);
 		if (hvcsd->index == index) {
-			kref_get(&hvcsd->kref);
+			tty_port_get(&hvcsd->port);
 			spin_unlock_irqrestore(&hvcsd->lock, flags);
 			spin_unlock(&hvcs_structs_lock);
 			return hvcsd;
@@ -1138,8 +1134,8 @@ static int hvcs_open(struct tty_struct *tty, struct file *filp)
 		if ((retval = hvcs_partner_connect(hvcsd)))
 			goto error_release;
 
-	hvcsd->open_count = 1;
-	hvcsd->tty = tty;
+	hvcsd->port.count = 1;
+	hvcsd->port.tty = tty;
 	tty->driver_data = hvcsd;
 
 	memset(&hvcsd->buffer[0], 0x00, HVCS_BUFF_LEN);
@@ -1160,7 +1156,7 @@ static int hvcs_open(struct tty_struct *tty, struct file *filp)
 	 * and will grab the spinlock and free the connection if it fails.
 	 */
 	if (((rc = hvcs_enable_device(hvcsd, unit_address, irq, vdev)))) {
-		kref_put(&hvcsd->kref, destroy_hvcs_struct);
+		tty_port_put(&hvcsd->port);
 		printk(KERN_WARNING "HVCS: enable device failed.\n");
 		return rc;
 	}
@@ -1171,8 +1167,8 @@ fast_open:
 	hvcsd = tty->driver_data;
 
 	spin_lock_irqsave(&hvcsd->lock, flags);
-	kref_get(&hvcsd->kref);
-	hvcsd->open_count++;
+	tty_port_get(&hvcsd->port);
+	hvcsd->port.count++;
 	hvcsd->todo_mask |= HVCS_SCHED_READ;
 	spin_unlock_irqrestore(&hvcsd->lock, flags);
 
@@ -1186,7 +1182,7 @@ open_success:
 
 error_release:
 	spin_unlock_irqrestore(&hvcsd->lock, flags);
-	kref_put(&hvcsd->kref, destroy_hvcs_struct);
+	tty_port_put(&hvcsd->port);
 
 	printk(KERN_WARNING "HVCS: partner connect failed.\n");
 	return retval;
@@ -1216,7 +1212,7 @@ static void hvcs_close(struct tty_struct *tty, struct file *filp)
 	hvcsd = tty->driver_data;
 
 	spin_lock_irqsave(&hvcsd->lock, flags);
-	if (--hvcsd->open_count == 0) {
+	if (--hvcsd->port.count == 0) {
 
 		vio_disable_interrupts(hvcsd->vdev);
 
@@ -1225,7 +1221,7 @@ static void hvcs_close(struct tty_struct *tty, struct file *filp)
 		 * execute any operations on the TTY even though it is obligated
 		 * to deliver any pending I/O to the hypervisor.
 		 */
-		hvcsd->tty = NULL;
+		hvcsd->port.tty = NULL;
 
 		irq = hvcsd->vdev->irq;
 		spin_unlock_irqrestore(&hvcsd->lock, flags);
@@ -1240,16 +1236,16 @@ static void hvcs_close(struct tty_struct *tty, struct file *filp)
 		tty->driver_data = NULL;
 
 		free_irq(irq, hvcsd);
-		kref_put(&hvcsd->kref, destroy_hvcs_struct);
+		tty_port_put(&hvcsd->port);
 		return;
-	} else if (hvcsd->open_count < 0) {
+	} else if (hvcsd->port.count < 0) {
 		printk(KERN_ERR "HVCS: vty-server@%X open_count: %d"
 				" is missmanaged.\n",
-		hvcsd->vdev->unit_address, hvcsd->open_count);
+		hvcsd->vdev->unit_address, hvcsd->port.count);
 	}
 
 	spin_unlock_irqrestore(&hvcsd->lock, flags);
-	kref_put(&hvcsd->kref, destroy_hvcs_struct);
+	tty_port_put(&hvcsd->port);
 }
 
 static void hvcs_hangup(struct tty_struct * tty)
@@ -1261,7 +1257,7 @@ static void hvcs_hangup(struct tty_struct * tty)
 
 	spin_lock_irqsave(&hvcsd->lock, flags);
 	/* Preserve this so that we know how many kref refs to put */
-	temp_open_count = hvcsd->open_count;
+	temp_open_count = hvcsd->port.count;
 
 	/*
 	 * Don't kref put inside the spinlock because the destruction
@@ -1273,10 +1269,10 @@ static void hvcs_hangup(struct tty_struct * tty)
 	hvcsd->todo_mask = 0;
 
 	/* I don't think the tty needs the hvcs_struct pointer after a hangup */
-	hvcsd->tty->driver_data = NULL;
-	hvcsd->tty = NULL;
+	tty->driver_data = NULL;
+	hvcsd->port.tty = NULL;
 
-	hvcsd->open_count = 0;
+	hvcsd->port.count = 0;
 
 	/* This will drop any buffered data on the floor which is OK in a hangup
 	 * scenario. */
@@ -1301,7 +1297,7 @@ static void hvcs_hangup(struct tty_struct * tty)
 		 * NOTE:  If this hangup was signaled from user space then the
 		 * final put will never happen.
 		 */
-		kref_put(&hvcsd->kref, destroy_hvcs_struct);
+		tty_port_put(&hvcsd->port);
 	}
 }
 
@@ -1347,7 +1343,7 @@ static int hvcs_write(struct tty_struct *tty,
 	 * the middle of a write operation?  This is a crummy place to do this
 	 * but we want to keep it all in the spinlock.
 	 */
-	if (hvcsd->open_count <= 0) {
+	if (hvcsd->port.count <= 0) {
 		spin_unlock_irqrestore(&hvcsd->lock, flags);
 		return -ENODEV;
 	}
@@ -1421,7 +1417,7 @@ static int hvcs_write_room(struct tty_struct *tty)
 {
 	struct hvcs_struct *hvcsd = tty->driver_data;
 
-	if (!hvcsd || hvcsd->open_count <= 0)
+	if (!hvcsd || hvcsd->port.count <= 0)
 		return 0;
 
 	return HVCS_BUFF_LEN - hvcsd->chars_in_buffer;
diff --git a/drivers/tty/hvc/hvsi.c b/drivers/tty/hvc/hvsi.c
index a7488b74864..6f5bc49c441 100644
--- a/drivers/tty/hvc/hvsi.c
+++ b/drivers/tty/hvc/hvsi.c
@@ -69,14 +69,13 @@
 #define __ALIGNED__	__attribute__((__aligned__(sizeof(long))))
 
 struct hvsi_struct {
+	struct tty_port port;
 	struct delayed_work writer;
 	struct work_struct handshaker;
 	wait_queue_head_t emptyq; /* woken when outbuf is emptied */
 	wait_queue_head_t stateq; /* woken when HVSI state changes */
 	spinlock_t lock;
 	int index;
-	struct tty_struct *tty;
-	int count;
 	uint8_t throttle_buf[128];
 	uint8_t outbuf[N_OUTBUF]; /* to implement write_room and chars_in_buffer */
 	/* inbuf is for packet reassembly. leave a little room for leftovers. */
@@ -237,7 +236,7 @@ static int hvsi_read(struct hvsi_struct *hp, char *buf, int count)
 }
 
 static void hvsi_recv_control(struct hvsi_struct *hp, uint8_t *packet,
-	struct tty_struct **to_hangup, struct hvsi_struct **to_handshake)
+	struct tty_struct *tty, struct hvsi_struct **to_handshake)
 {
 	struct hvsi_control *header = (struct hvsi_control *)packet;
 
@@ -247,9 +246,8 @@ static void hvsi_recv_control(struct hvsi_struct *hp, uint8_t *packet,
 				/* CD went away; no more connection */
 				pr_debug("hvsi%i: CD dropped\n", hp->index);
 				hp->mctrl &= TIOCM_CD;
-				/* If userland hasn't done an open(2) yet, hp->tty is NULL. */
-				if (hp->tty && !(hp->tty->flags & CLOCAL))
-					*to_hangup = hp->tty;
+				if (tty && !C_CLOCAL(tty))
+					tty_hangup(tty);
 			}
 			break;
 		case VSV_CLOSE_PROTOCOL:
@@ -331,7 +329,8 @@ static void hvsi_recv_query(struct hvsi_struct *hp, uint8_t *packet)
 	}
 }
 
-static void hvsi_insert_chars(struct hvsi_struct *hp, const char *buf, int len)
+static void hvsi_insert_chars(struct hvsi_struct *hp, struct tty_struct *tty,
+		const char *buf, int len)
 {
 	int i;
 
@@ -347,7 +346,7 @@ static void hvsi_insert_chars(struct hvsi_struct *hp, const char *buf, int len)
 			continue;
 		}
 #endif /* CONFIG_MAGIC_SYSRQ */
-		tty_insert_flip_char(hp->tty, c, 0);
+		tty_insert_flip_char(tty, c, 0);
 	}
 }
 
@@ -360,7 +359,7 @@ static void hvsi_insert_chars(struct hvsi_struct *hp, const char *buf, int len)
  * revisited.
  */
 #define TTY_THRESHOLD_THROTTLE 128
-static struct tty_struct *hvsi_recv_data(struct hvsi_struct *hp,
+static bool hvsi_recv_data(struct hvsi_struct *hp, struct tty_struct *tty,
 		const uint8_t *packet)
 {
 	const struct hvsi_header *header = (const struct hvsi_header *)packet;
@@ -371,14 +370,14 @@ static struct tty_struct *hvsi_recv_data(struct hvsi_struct *hp,
 	pr_debug("queueing %i chars '%.*s'\n", datalen, datalen, data);
 
 	if (datalen == 0)
-		return NULL;
+		return false;
 
 	if (overflow > 0) {
 		pr_debug("%s: got >TTY_THRESHOLD_THROTTLE bytes\n", __func__);
 		datalen = TTY_THRESHOLD_THROTTLE;
 	}
 
-	hvsi_insert_chars(hp, data, datalen);
+	hvsi_insert_chars(hp, tty, data, datalen);
 
 	if (overflow > 0) {
 		/*
@@ -390,7 +389,7 @@ static struct tty_struct *hvsi_recv_data(struct hvsi_struct *hp,
 		hp->n_throttle = overflow;
 	}
 
-	return hp->tty;
+	return true;
 }
 
 /*
@@ -399,14 +398,13 @@ static struct tty_struct *hvsi_recv_data(struct hvsi_struct *hp,
  * machine during console handshaking (in which case tty = NULL and we ignore
  * incoming data).
  */
-static int hvsi_load_chunk(struct hvsi_struct *hp, struct tty_struct **flip,
-		struct tty_struct **hangup, struct hvsi_struct **handshake)
+static int hvsi_load_chunk(struct hvsi_struct *hp, struct tty_struct *tty,
+		struct hvsi_struct **handshake)
 {
 	uint8_t *packet = hp->inbuf;
 	int chunklen;
+	bool flip = false;
 
-	*flip = NULL;
-	*hangup = NULL;
 	*handshake = NULL;
 
 	chunklen = hvsi_read(hp, hp->inbuf_end, HVSI_MAX_READ);
@@ -440,12 +438,12 @@ static int hvsi_load_chunk(struct hvsi_struct *hp, struct tty_struct **flip,
 			case VS_DATA_PACKET_HEADER:
 				if (!is_open(hp))
 					break;
-				if (hp->tty == NULL)
+				if (tty == NULL)
 					break; /* no tty buffer to put data in */
-				*flip = hvsi_recv_data(hp, packet);
+				flip = hvsi_recv_data(hp, tty, packet);
 				break;
 			case VS_CONTROL_PACKET_HEADER:
-				hvsi_recv_control(hp, packet, hangup, handshake);
+				hvsi_recv_control(hp, packet, tty, handshake);
 				break;
 			case VS_QUERY_RESPONSE_PACKET_HEADER:
 				hvsi_recv_response(hp, packet);
@@ -462,28 +460,26 @@ static int hvsi_load_chunk(struct hvsi_struct *hp, struct tty_struct **flip,
 
 		packet += len_packet(packet);
 
-		if (*hangup || *handshake) {
-			pr_debug("%s: hangup or handshake\n", __func__);
-			/*
-			 * we need to send the hangup now before receiving any more data.
-			 * If we get "data, hangup, data", we can't deliver the second
-			 * data before the hangup.
-			 */
+		if (*handshake) {
+			pr_debug("%s: handshake\n", __func__);
 			break;
 		}
 	}
 
 	compact_inbuf(hp, packet);
 
+	if (flip)
+		tty_flip_buffer_push(tty);
+
 	return 1;
 }
 
-static void hvsi_send_overflow(struct hvsi_struct *hp)
+static void hvsi_send_overflow(struct hvsi_struct *hp, struct tty_struct *tty)
 {
 	pr_debug("%s: delivering %i bytes overflow\n", __func__,
 			hp->n_throttle);
 
-	hvsi_insert_chars(hp, hp->throttle_buf, hp->n_throttle);
+	hvsi_insert_chars(hp, tty, hp->throttle_buf, hp->n_throttle);
 	hp->n_throttle = 0;
 }
 
@@ -494,35 +490,20 @@ static void hvsi_send_overflow(struct hvsi_struct *hp)
 static irqreturn_t hvsi_interrupt(int irq, void *arg)
 {
 	struct hvsi_struct *hp = (struct hvsi_struct *)arg;
-	struct tty_struct *flip;
-	struct tty_struct *hangup;
 	struct hvsi_struct *handshake;
+	struct tty_struct *tty;
 	unsigned long flags;
 	int again = 1;
 
 	pr_debug("%s\n", __func__);
 
+	tty = tty_port_tty_get(&hp->port);
+
 	while (again) {
 		spin_lock_irqsave(&hp->lock, flags);
-		again = hvsi_load_chunk(hp, &flip, &hangup, &handshake);
+		again = hvsi_load_chunk(hp, tty, &handshake);
 		spin_unlock_irqrestore(&hp->lock, flags);
 
-		/*
-		 * we have to call tty_flip_buffer_push() and tty_hangup() outside our
-		 * spinlock. But we also have to keep going until we've read all the
-		 * available data.
-		 */
-
-		if (flip) {
-			/* there was data put in the tty flip buffer */
-			tty_flip_buffer_push(flip);
-			flip = NULL;
-		}
-
-		if (hangup) {
-			tty_hangup(hangup);
-		}
-
 		if (handshake) {
 			pr_debug("hvsi%i: attempting re-handshake\n", handshake->index);
 			schedule_work(&handshake->handshaker);
@@ -530,18 +511,15 @@ static irqreturn_t hvsi_interrupt(int irq, void *arg)
 	}
 
 	spin_lock_irqsave(&hp->lock, flags);
-	if (hp->tty && hp->n_throttle
-			&& (!test_bit(TTY_THROTTLED, &hp->tty->flags))) {
-		/* we weren't hung up and we weren't throttled, so we can deliver the
-		 * rest now */
-		flip = hp->tty;
-		hvsi_send_overflow(hp);
+	if (tty && hp->n_throttle && !test_bit(TTY_THROTTLED, &tty->flags)) {
+		/* we weren't hung up and we weren't throttled, so we can
+		 * deliver the rest now */
+		hvsi_send_overflow(hp, tty);
+		tty_flip_buffer_push(tty);
 	}
 	spin_unlock_irqrestore(&hp->lock, flags);
 
-	if (flip) {
-		tty_flip_buffer_push(flip);
-	}
+	tty_kref_put(tty);
 
 	return IRQ_HANDLED;
 }
@@ -749,9 +727,9 @@ static int hvsi_open(struct tty_struct *tty, struct file *filp)
 	if (hp->state == HVSI_FSP_DIED)
 		return -EIO;
 
+	tty_port_tty_set(&hp->port, tty);
 	spin_lock_irqsave(&hp->lock, flags);
-	hp->tty = tty;
-	hp->count++;
+	hp->port.count++;
 	atomic_set(&hp->seqno, 0);
 	h_vio_signal(hp->vtermno, VIO_IRQ_ENABLE);
 	spin_unlock_irqrestore(&hp->lock, flags);
@@ -808,8 +786,8 @@ static void hvsi_close(struct tty_struct *tty, struct file *filp)
 
 	spin_lock_irqsave(&hp->lock, flags);
 
-	if (--hp->count == 0) {
-		hp->tty = NULL;
+	if (--hp->port.count == 0) {
+		tty_port_tty_set(&hp->port, NULL);
 		hp->inbuf_end = hp->inbuf; /* discard remaining partial packets */
 
 		/* only close down connection if it is not the console */
@@ -841,9 +819,9 @@ static void hvsi_close(struct tty_struct *tty, struct file *filp)
 
 			spin_lock_irqsave(&hp->lock, flags);
 		}
-	} else if (hp->count < 0)
+	} else if (hp->port.count < 0)
 		printk(KERN_ERR "hvsi_close %lu: oops, count is %d\n",
-		       hp - hvsi_ports, hp->count);
+		       hp - hvsi_ports, hp->port.count);
 
 	spin_unlock_irqrestore(&hp->lock, flags);
 }
@@ -855,12 +833,11 @@ static void hvsi_hangup(struct tty_struct *tty)
 
 	pr_debug("%s\n", __func__);
 
-	spin_lock_irqsave(&hp->lock, flags);
+	tty_port_tty_set(&hp->port, NULL);
 
-	hp->count = 0;
+	spin_lock_irqsave(&hp->lock, flags);
+	hp->port.count = 0;
 	hp->n_outbuf = 0;
-	hp->tty = NULL;
-
 	spin_unlock_irqrestore(&hp->lock, flags);
 }
 
@@ -888,6 +865,7 @@ static void hvsi_write_worker(struct work_struct *work)
 {
 	struct hvsi_struct *hp =
 		container_of(work, struct hvsi_struct, writer.work);
+	struct tty_struct *tty;
 	unsigned long flags;
 #ifdef DEBUG
 	static long start_j = 0;
@@ -921,7 +899,11 @@ static void hvsi_write_worker(struct work_struct *work)
 		start_j = 0;
 #endif /* DEBUG */
 		wake_up_all(&hp->emptyq);
-		tty_wakeup(hp->tty);
+		tty = tty_port_tty_get(&hp->port);
+		if (tty) {
+			tty_wakeup(tty);
+			tty_kref_put(tty);
+		}
 	}
 
 out:
@@ -966,8 +948,8 @@ static int hvsi_write(struct tty_struct *tty,
 	 * and hvsi_write_worker will be scheduled. subsequent hvsi_write() calls
 	 * will see there is no room in outbuf and return.
 	 */
-	while ((count > 0) && (hvsi_write_room(hp->tty) > 0)) {
-		int chunksize = min(count, hvsi_write_room(hp->tty));
+	while ((count > 0) && (hvsi_write_room(tty) > 0)) {
+		int chunksize = min(count, hvsi_write_room(tty));
 
 		BUG_ON(hp->n_outbuf < 0);
 		memcpy(hp->outbuf + hp->n_outbuf, source, chunksize);
@@ -1014,19 +996,16 @@ static void hvsi_unthrottle(struct tty_struct *tty)
 {
 	struct hvsi_struct *hp = tty->driver_data;
 	unsigned long flags;
-	int shouldflip = 0;
 
 	pr_debug("%s\n", __func__);
 
 	spin_lock_irqsave(&hp->lock, flags);
 	if (hp->n_throttle) {
-		hvsi_send_overflow(hp);
-		shouldflip = 1;
+		hvsi_send_overflow(hp, tty);
+		tty_flip_buffer_push(tty);
 	}
 	spin_unlock_irqrestore(&hp->lock, flags);
 
-	if (shouldflip)
-		tty_flip_buffer_push(hp->tty);
 
 	h_vio_signal(hp->vtermno, VIO_IRQ_ENABLE);
 }
@@ -1228,6 +1207,7 @@ static int __init hvsi_console_init(void)
 		init_waitqueue_head(&hp->emptyq);
 		init_waitqueue_head(&hp->stateq);
 		spin_lock_init(&hp->lock);
+		tty_port_init(&hp->port);
 		hp->index = hvsi_count;
 		hp->inbuf_end = hp->inbuf;
 		hp->state = HVSI_CLOSED;
diff --git a/drivers/tty/hvc/hvsi_lib.c b/drivers/tty/hvc/hvsi_lib.c
index 6f4dd83d869..59c135dd5d2 100644
--- a/drivers/tty/hvc/hvsi_lib.c
+++ b/drivers/tty/hvc/hvsi_lib.c
@@ -377,7 +377,7 @@ int hvsilib_open(struct hvsi_priv *pv, struct hvc_struct *hp)
 	pr_devel("HVSI@%x: open !\n", pv->termno);
 
 	/* Keep track of the tty data structure */
-	pv->tty = tty_kref_get(hp->tty);
+	pv->tty = tty_port_tty_get(&hp->port);
 
 	hvsilib_establish(pv);
 
diff --git a/drivers/tty/ipwireless/tty.c b/drivers/tty/ipwireless/tty.c
index 4daf962f705..f8b5fa0093a 100644
--- a/drivers/tty/ipwireless/tty.c
+++ b/drivers/tty/ipwireless/tty.c
@@ -44,14 +44,13 @@
 #define TTYTYPE_RAS_RAW  (2)
 
 struct ipw_tty {
+	struct tty_port port;
 	int index;
 	struct ipw_hardware *hardware;
 	unsigned int channel_idx;
 	unsigned int secondary_channel_idx;
 	int tty_type;
 	struct ipw_network *network;
-	struct tty_struct *linux_tty;
-	int open_count;
 	unsigned int control_lines;
 	struct mutex ipw_tty_mutex;
 	int tx_bytes_queued;
@@ -73,23 +72,6 @@ static char *tty_type_name(int tty_type)
 	return channel_names[tty_type];
 }
 
-static void report_registering(struct ipw_tty *tty)
-{
-	char *iftype = tty_type_name(tty->tty_type);
-
-	printk(KERN_INFO IPWIRELESS_PCCARD_NAME
-	       ": registering %s device ttyIPWp%d\n", iftype, tty->index);
-}
-
-static void report_deregistering(struct ipw_tty *tty)
-{
-	char *iftype = tty_type_name(tty->tty_type);
-
-	printk(KERN_INFO IPWIRELESS_PCCARD_NAME
-	       ": deregistering %s device ttyIPWp%d\n", iftype,
-	       tty->index);
-}
-
 static struct ipw_tty *get_tty(int index)
 {
 	/*
@@ -117,12 +99,12 @@ static int ipw_open(struct tty_struct *linux_tty, struct file *filp)
 		mutex_unlock(&tty->ipw_tty_mutex);
 		return -ENODEV;
 	}
-	if (tty->open_count == 0)
+	if (tty->port.count == 0)
 		tty->tx_bytes_queued = 0;
 
-	tty->open_count++;
+	tty->port.count++;
 
-	tty->linux_tty = linux_tty;
+	tty->port.tty = linux_tty;
 	linux_tty->driver_data = tty;
 	linux_tty->low_latency = 1;
 
@@ -136,13 +118,13 @@ static int ipw_open(struct tty_struct *linux_tty, struct file *filp)
 
 static void do_ipw_close(struct ipw_tty *tty)
 {
-	tty->open_count--;
+	tty->port.count--;
 
-	if (tty->open_count == 0) {
-		struct tty_struct *linux_tty = tty->linux_tty;
+	if (tty->port.count == 0) {
+		struct tty_struct *linux_tty = tty->port.tty;
 
 		if (linux_tty != NULL) {
-			tty->linux_tty = NULL;
+			tty->port.tty = NULL;
 			linux_tty->driver_data = NULL;
 
 			if (tty->tty_type == TTYTYPE_MODEM)
@@ -159,7 +141,7 @@ static void ipw_hangup(struct tty_struct *linux_tty)
 		return;
 
 	mutex_lock(&tty->ipw_tty_mutex);
-	if (tty->open_count == 0) {
+	if (tty->port.count == 0) {
 		mutex_unlock(&tty->ipw_tty_mutex);
 		return;
 	}
@@ -182,13 +164,13 @@ void ipwireless_tty_received(struct ipw_tty *tty, unsigned char *data,
 	int work = 0;
 
 	mutex_lock(&tty->ipw_tty_mutex);
-	linux_tty = tty->linux_tty;
+	linux_tty = tty->port.tty;
 	if (linux_tty == NULL) {
 		mutex_unlock(&tty->ipw_tty_mutex);
 		return;
 	}
 
-	if (!tty->open_count) {
+	if (!tty->port.count) {
 		mutex_unlock(&tty->ipw_tty_mutex);
 		return;
 	}
@@ -230,7 +212,7 @@ static int ipw_write(struct tty_struct *linux_tty,
 		return -ENODEV;
 
 	mutex_lock(&tty->ipw_tty_mutex);
-	if (!tty->open_count) {
+	if (!tty->port.count) {
 		mutex_unlock(&tty->ipw_tty_mutex);
 		return -EINVAL;
 	}
@@ -270,7 +252,7 @@ static int ipw_write_room(struct tty_struct *linux_tty)
 	if (!tty)
 		return -ENODEV;
 
-	if (!tty->open_count)
+	if (!tty->port.count)
 		return -EINVAL;
 
 	room = IPWIRELESS_TX_QUEUE_SIZE - tty->tx_bytes_queued;
@@ -312,7 +294,7 @@ static int ipw_chars_in_buffer(struct tty_struct *linux_tty)
 	if (!tty)
 		return 0;
 
-	if (!tty->open_count)
+	if (!tty->port.count)
 		return 0;
 
 	return tty->tx_bytes_queued;
@@ -393,7 +375,7 @@ static int ipw_tiocmget(struct tty_struct *linux_tty)
 	if (!tty)
 		return -ENODEV;
 
-	if (!tty->open_count)
+	if (!tty->port.count)
 		return -EINVAL;
 
 	return get_control_lines(tty);
@@ -409,7 +391,7 @@ ipw_tiocmset(struct tty_struct *linux_tty,
 	if (!tty)
 		return -ENODEV;
 
-	if (!tty->open_count)
+	if (!tty->port.count)
 		return -EINVAL;
 
 	return set_control_lines(tty, set, clear);
@@ -423,7 +405,7 @@ static int ipw_ioctl(struct tty_struct *linux_tty,
 	if (!tty)
 		return -ENODEV;
 
-	if (!tty->open_count)
+	if (!tty->port.count)
 		return -EINVAL;
 
 	/* FIXME: Exactly how is the tty object locked here .. */
@@ -492,6 +474,7 @@ static int add_tty(int j,
 	ttys[j]->network = network;
 	ttys[j]->tty_type = tty_type;
 	mutex_init(&ttys[j]->ipw_tty_mutex);
+	tty_port_init(&ttys[j]->port);
 
 	tty_register_device(ipw_tty_driver, j, NULL);
 	ipwireless_associate_network_tty(network, channel_idx, ttys[j]);
@@ -500,8 +483,12 @@ static int add_tty(int j,
 		ipwireless_associate_network_tty(network,
 						 secondary_channel_idx,
 						 ttys[j]);
-	if (get_tty(j) == ttys[j])
-		report_registering(ttys[j]);
+	/* check if we provide raw device (if loopback is enabled) */
+	if (get_tty(j))
+		printk(KERN_INFO IPWIRELESS_PCCARD_NAME
+		       ": registering %s device ttyIPWp%d\n",
+		       tty_type_name(tty_type), j);
+
 	return 0;
 }
 
@@ -560,19 +547,21 @@ void ipwireless_tty_free(struct ipw_tty *tty)
 
 		if (ttyj) {
 			mutex_lock(&ttyj->ipw_tty_mutex);
-			if (get_tty(j) == ttyj)
-				report_deregistering(ttyj);
+			if (get_tty(j))
+				printk(KERN_INFO IPWIRELESS_PCCARD_NAME
+				       ": deregistering %s device ttyIPWp%d\n",
+				       tty_type_name(ttyj->tty_type), j);
 			ttyj->closing = 1;
-			if (ttyj->linux_tty != NULL) {
+			if (ttyj->port.tty != NULL) {
 				mutex_unlock(&ttyj->ipw_tty_mutex);
-				tty_hangup(ttyj->linux_tty);
-				/* Wait till the tty_hangup has completed */
-				flush_work_sync(&ttyj->linux_tty->hangup_work);
+				tty_vhangup(ttyj->port.tty);
 				/* FIXME: Exactly how is the tty object locked here
 				   against a parallel ioctl etc */
+				/* FIXME2: hangup does not mean all processes
+				 * are gone */
 				mutex_lock(&ttyj->ipw_tty_mutex);
 			}
-			while (ttyj->open_count)
+			while (ttyj->port.count)
 				do_ipw_close(ttyj);
 			ipwireless_disassociate_network_ttys(network,
 							     ttyj->channel_idx);
@@ -661,8 +650,8 @@ ipwireless_tty_notify_control_line_change(struct ipw_tty *tty,
 	 */
 	if ((old_control_lines & IPW_CONTROL_LINE_DCD)
 			&& !(tty->control_lines & IPW_CONTROL_LINE_DCD)
-			&& tty->linux_tty) {
-		tty_hangup(tty->linux_tty);
+			&& tty->port.tty) {
+		tty_hangup(tty->port.tty);
 	}
 }
 
diff --git a/drivers/tty/mxser.c b/drivers/tty/mxser.c
index c6f372dd562..90cc680c4f0 100644
--- a/drivers/tty/mxser.c
+++ b/drivers/tty/mxser.c
@@ -2326,7 +2326,7 @@ static const struct tty_operations mxser_ops = {
 	.get_icount = mxser_get_icount,
 };
 
-struct tty_port_operations mxser_port_ops = {
+static struct tty_port_operations mxser_port_ops = {
 	.carrier_raised = mxser_carrier_raised,
 	.dtr_rts = mxser_dtr_rts,
 	.activate = mxser_activate,
diff --git a/drivers/tty/n_r3964.c b/drivers/tty/n_r3964.c
index 5c6c31459a2..656ad93bbc9 100644
--- a/drivers/tty/n_r3964.c
+++ b/drivers/tty/n_r3964.c
@@ -1065,7 +1065,8 @@ static ssize_t r3964_read(struct tty_struct *tty, struct file *file,
 
 	TRACE_L("read()");
 
-	tty_lock();
+	/* FIXME: should use a private lock */
+	tty_lock(tty);
 
 	pClient = findClient(pInfo, task_pid(current));
 	if (pClient) {
@@ -1077,7 +1078,7 @@ static ssize_t r3964_read(struct tty_struct *tty, struct file *file,
 				goto unlock;
 			}
 			/* block until there is a message: */
-			wait_event_interruptible_tty(pInfo->read_wait,
+			wait_event_interruptible_tty(tty, pInfo->read_wait,
 					(pMsg = remove_msg(pInfo, pClient)));
 		}
 
@@ -1107,7 +1108,7 @@ static ssize_t r3964_read(struct tty_struct *tty, struct file *file,
 	}
 	ret = -EPERM;
 unlock:
-	tty_unlock();
+	tty_unlock(tty);
 	return ret;
 }
 
@@ -1156,7 +1157,7 @@ static ssize_t r3964_write(struct tty_struct *tty, struct file *file,
 	pHeader->locks = 0;
 	pHeader->owner = NULL;
 
-	tty_lock();
+	tty_lock(tty);
 
 	pClient = findClient(pInfo, task_pid(current));
 	if (pClient) {
@@ -1175,7 +1176,7 @@ static ssize_t r3964_write(struct tty_struct *tty, struct file *file,
 	add_tx_queue(pInfo, pHeader);
 	trigger_transmit(pInfo);
 
-	tty_unlock();
+	tty_unlock(tty);
 
 	return 0;
 }
diff --git a/drivers/tty/n_tty.c b/drivers/tty/n_tty.c
index 94b6eda87af..ee1c268f5f9 100644
--- a/drivers/tty/n_tty.c
+++ b/drivers/tty/n_tty.c
@@ -1630,6 +1630,7 @@ static int copy_from_read_buf(struct tty_struct *tty,
 	int retval;
 	size_t n;
 	unsigned long flags;
+	bool is_eof;
 
 	retval = 0;
 	spin_lock_irqsave(&tty->read_lock, flags);
@@ -1639,15 +1640,15 @@ static int copy_from_read_buf(struct tty_struct *tty,
 	if (n) {
 		retval = copy_to_user(*b, &tty->read_buf[tty->read_tail], n);
 		n -= retval;
+		is_eof = n == 1 &&
+			tty->read_buf[tty->read_tail] == EOF_CHAR(tty);
 		tty_audit_add_data(tty, &tty->read_buf[tty->read_tail], n);
 		spin_lock_irqsave(&tty->read_lock, flags);
 		tty->read_tail = (tty->read_tail + n) & (N_TTY_BUF_SIZE-1);
 		tty->read_cnt -= n;
 		/* Turn single EOF into zero-length read */
-		if (L_EXTPROC(tty) && tty->icanon && n == 1) {
-			if (!tty->read_cnt && (*b)[n-1] == EOF_CHAR(tty))
-				n--;
-		}
+		if (L_EXTPROC(tty) && tty->icanon && is_eof && !tty->read_cnt)
+			n = 0;
 		spin_unlock_irqrestore(&tty->read_lock, flags);
 		*b += n;
 		*nr -= n;
diff --git a/drivers/tty/pty.c b/drivers/tty/pty.c
index eeae7fafe9a..65c7c62c7aa 100644
--- a/drivers/tty/pty.c
+++ b/drivers/tty/pty.c
@@ -26,11 +26,13 @@
 #include <linux/bitops.h>
 #include <linux/devpts_fs.h>
 #include <linux/slab.h>
+#include <linux/mutex.h>
 
 
 #ifdef CONFIG_UNIX98_PTYS
 static struct tty_driver *ptm_driver;
 static struct tty_driver *pts_driver;
+static DEFINE_MUTEX(devpts_mutex);
 #endif
 
 static void pty_close(struct tty_struct *tty, struct file *filp)
@@ -45,6 +47,7 @@ static void pty_close(struct tty_struct *tty, struct file *filp)
 	wake_up_interruptible(&tty->read_wait);
 	wake_up_interruptible(&tty->write_wait);
 	tty->packet = 0;
+	/* Review - krefs on tty_link ?? */
 	if (!tty->link)
 		return;
 	tty->link->packet = 0;
@@ -54,12 +57,15 @@ static void pty_close(struct tty_struct *tty, struct file *filp)
 	if (tty->driver->subtype == PTY_TYPE_MASTER) {
 		set_bit(TTY_OTHER_CLOSED, &tty->flags);
 #ifdef CONFIG_UNIX98_PTYS
-		if (tty->driver == ptm_driver)
+		if (tty->driver == ptm_driver) {
+		        mutex_lock(&devpts_mutex);
 			devpts_pty_kill(tty->link);
+		        mutex_unlock(&devpts_mutex);
+		}
 #endif
-		tty_unlock();
+		tty_unlock(tty);
 		tty_vhangup(tty->link);
-		tty_lock();
+		tty_lock(tty);
 	}
 }
 
@@ -475,13 +481,17 @@ static struct tty_struct *ptm_unix98_lookup(struct tty_driver *driver,
  *	@idx: tty index
  *
  *	Look up a pty master device. Called under the tty_mutex for now.
- *	This provides our locking.
+ *	This provides our locking for the tty pointer.
  */
 
 static struct tty_struct *pts_unix98_lookup(struct tty_driver *driver,
 		struct inode *pts_inode, int idx)
 {
-	struct tty_struct *tty = devpts_get_tty(pts_inode, idx);
+	struct tty_struct *tty;
+
+	mutex_lock(&devpts_mutex);
+	tty = devpts_get_tty(pts_inode, idx);
+	mutex_unlock(&devpts_mutex);
 	/* Master must be open before slave */
 	if (!tty)
 		return ERR_PTR(-EIO);
@@ -613,24 +623,27 @@ static int ptmx_open(struct inode *inode, struct file *filp)
 		return retval;
 
 	/* find a device that is not in use. */
-	tty_lock();
+	mutex_lock(&devpts_mutex);
 	index = devpts_new_index(inode);
-	tty_unlock();
 	if (index < 0) {
 		retval = index;
 		goto err_file;
 	}
 
+	mutex_unlock(&devpts_mutex);
+
 	mutex_lock(&tty_mutex);
-	tty_lock();
 	tty = tty_init_dev(ptm_driver, index);
-	mutex_unlock(&tty_mutex);
 
 	if (IS_ERR(tty)) {
 		retval = PTR_ERR(tty);
 		goto out;
 	}
 
+	/* The tty returned here is locked so we can safely
+	   drop the mutex */
+	mutex_unlock(&tty_mutex);
+
 	set_bit(TTY_PTY_LOCK, &tty->flags); /* LOCK THE SLAVE */
 
 	tty_add_file(tty, filp);
@@ -643,16 +656,17 @@ static int ptmx_open(struct inode *inode, struct file *filp)
 	if (retval)
 		goto err_release;
 
-	tty_unlock();
+	tty_unlock(tty);
 	return 0;
 err_release:
-	tty_unlock();
+	tty_unlock(tty);
 	tty_release(inode, filp);
 	return retval;
 out:
+	mutex_unlock(&tty_mutex);
 	devpts_kill_index(inode, index);
-	tty_unlock();
 err_file:
+        mutex_unlock(&devpts_mutex);
 	tty_free_file(filp);
 	return retval;
 }
diff --git a/drivers/tty/serial/68328serial.c b/drivers/tty/serial/68328serial.c
index 5ce782529d6..3ed20e435e5 100644
--- a/drivers/tty/serial/68328serial.c
+++ b/drivers/tty/serial/68328serial.c
@@ -17,6 +17,7 @@
 #include <asm/dbg.h>
 #include <linux/module.h>
 #include <linux/errno.h>
+#include <linux/serial.h>
 #include <linux/signal.h>
 #include <linux/sched.h>
 #include <linux/timer.h>
@@ -56,8 +57,6 @@
 #endif /* CONFIG_M68VZ328 */
 #endif /* CONFIG_M68EZ328 */
 
-#include "68328serial.h"
-
 /* Turn off usage of real serial interrupt code, to "support" Copilot */
 #ifdef CONFIG_XCOPILOT_BUGS
 #undef USE_INTS
@@ -65,33 +64,82 @@
 #define USE_INTS
 #endif
 
-static struct m68k_serial m68k_soft[NR_PORTS];
+/*
+ * I believe this is the optimal setting that reduces the number of interrupts.
+ * At high speeds the output might become a little "bursted" (use USTCNT_TXHE
+ * if that bothers you), but in most cases it will not, since we try to
+ * transmit characters every time rs_interrupt is called. Thus, quite often
+ * you'll see that a receive interrupt occures before the transmit one.
+ *                                  -- Vladimir Gurevich
+ */
+#define USTCNT_TX_INTR_MASK (USTCNT_TXEE)
 
-static unsigned int uart_irqs[NR_PORTS] = UART_IRQ_DEFNS;
+/*
+ * 68328 and 68EZ328 UARTS are a little bit different. EZ328 has special
+ * "Old data interrupt" which occures whenever the data stay in the FIFO
+ * longer than 30 bits time. This allows us to use FIFO without compromising
+ * latency. '328 does not have this feature and without the real  328-based
+ * board I would assume that RXRE is the safest setting.
+ *
+ * For EZ328 I use RXHE (Half empty) interrupt to reduce the number of
+ * interrupts. RXFE (receive queue full) causes the system to lose data
+ * at least at 115200 baud
+ *
+ * If your board is busy doing other stuff, you might consider to use
+ * RXRE (data ready intrrupt) instead.
+ *
+ * The other option is to make these INTR masks run-time configurable, so
+ * that people can dynamically adapt them according to the current usage.
+ *                                  -- Vladimir Gurevich
+ */
 
-/* multiple ports are contiguous in memory */
-m68328_uart *uart_addr = (m68328_uart *)USTCNT_ADDR;
+/* (es) */
+#if defined(CONFIG_M68EZ328) || defined(CONFIG_M68VZ328)
+#define USTCNT_RX_INTR_MASK (USTCNT_RXHE | USTCNT_ODEN)
+#elif defined(CONFIG_M68328)
+#define USTCNT_RX_INTR_MASK (USTCNT_RXRE)
+#else
+#error Please, define the Rx interrupt events for your CPU
+#endif
+/* (/es) */
 
-struct tty_struct m68k_ttys;
-struct m68k_serial *m68k_consinfo = 0;
+/*
+ * This is our internal structure for each serial port's state.
+ */
+struct m68k_serial {
+	struct tty_port		tport;
+	char			is_cons;	/* Is this our console. */
+	int			magic;
+	int			baud_base;
+	int			port;
+	int			irq;
+	int			type;		/* UART type */
+	int			custom_divisor;
+	int			x_char;		/* xon/xoff character */
+	int			line;
+	unsigned char		*xmit_buf;
+	int			xmit_head;
+	int			xmit_tail;
+	int			xmit_cnt;
+};
 
-#define M68K_CLOCK (16667000) /* FIXME: 16MHz is likely wrong */
+#define SERIAL_MAGIC 0x5301
 
-struct tty_driver *serial_driver;
+/*
+ * Define the number of ports supported and their irqs.
+ */
+#define NR_PORTS 1
 
-/* number of characters left in xmit buffer before we ask for more */
-#define WAKEUP_CHARS 256
+static struct m68k_serial m68k_soft[NR_PORTS];
 
-/* Debugging... DEBUG_INTR is bad to use when one of the zs
- * lines is your console ;(
- */
-#undef SERIAL_DEBUG_INTR
-#undef SERIAL_DEBUG_OPEN
-#undef SERIAL_DEBUG_FLOW
+static unsigned int uart_irqs[NR_PORTS] = { UART_IRQ_NUM };
 
-#define RS_ISR_PASS_LIMIT 256
+/* multiple ports are contiguous in memory */
+m68328_uart *uart_addr = (m68328_uart *)USTCNT_ADDR;
+
+struct tty_driver *serial_driver;
 
-static void change_speed(struct m68k_serial *info);
+static void change_speed(struct m68k_serial *info, struct tty_struct *tty);
 
 /*
  *	Setup for console. Argument comes from the boot command line.
@@ -143,17 +191,6 @@ static int baud_table[] = {
 	0, 50, 75, 110, 134, 150, 200, 300, 600, 1200, 1800, 2400, 4800,
 	9600, 19200, 38400, 57600, 115200, 0 };
 
-/* Sets or clears DTR/RTS on the requested line */
-static inline void m68k_rtsdtr(struct m68k_serial *ss, int set)
-{
-	if (set) {
-		/* set the RTS/CTS line */
-	} else {
-		/* clear it */
-	}
-	return;
-}
-
 /* Utility routines */
 static inline int get_baud(struct m68k_serial *ss)
 {
@@ -189,7 +226,8 @@ static void rs_stop(struct tty_struct *tty)
 
 static int rs_put_char(char ch)
 {
-        int flags, loops = 0;
+	unsigned long flags;
+	int loops = 0;
 
         local_irq_save(flags);
 
@@ -224,28 +262,9 @@ static void rs_start(struct tty_struct *tty)
 	local_irq_restore(flags);
 }
 
-/* Drop into either the boot monitor or kadb upon receiving a break
- * from keyboard/console input.
- */
-static void batten_down_hatches(void)
-{
-	/* Drop into the debugger */
-}
-
-static void status_handle(struct m68k_serial *info, unsigned short status)
+static void receive_chars(struct m68k_serial *info, struct tty_struct *tty,
+		unsigned short rx)
 {
-	/* If this is console input and this is a
-	 * 'break asserted' status change interrupt
-	 * see if we can drop into the debugger
-	 */
-	if((status & URX_BREAK) && info->break_abort)
-		batten_down_hatches();
-	return;
-}
-
-static void receive_chars(struct m68k_serial *info, unsigned short rx)
-{
-	struct tty_struct *tty = info->tty;
 	m68328_uart *uart = &uart_addr[info->line];
 	unsigned char ch, flag;
 
@@ -259,7 +278,6 @@ static void receive_chars(struct m68k_serial *info, unsigned short rx)
 	
 		if(info->is_cons) {
 			if(URX_BREAK & rx) { /* whee, break received */
-				status_handle(info, rx);
 				return;
 #ifdef CONFIG_MAGIC_SYSRQ
 			} else if (ch == 0x10) { /* ^P */
@@ -280,16 +298,13 @@ static void receive_chars(struct m68k_serial *info, unsigned short rx)
 		
 		flag = TTY_NORMAL;
 
-		if(rx & URX_PARITY_ERROR) {
+		if (rx & URX_PARITY_ERROR)
 			flag = TTY_PARITY;
-			status_handle(info, rx);
-		} else if(rx & URX_OVRUN) {
+		else if (rx & URX_OVRUN)
 			flag = TTY_OVERRUN;
-			status_handle(info, rx);
-		} else if(rx & URX_FRAME_ERROR) {
+		else if (rx & URX_FRAME_ERROR)
 			flag = TTY_FRAME;
-			status_handle(info, rx);
-		}
+
 		tty_insert_flip_char(tty, ch, flag);
 #ifndef CONFIG_XCOPILOT_BUGS
 	} while((rx = uart->urx.w) & URX_DATA_READY);
@@ -301,7 +316,7 @@ clear_and_exit:
 	return;
 }
 
-static void transmit_chars(struct m68k_serial *info)
+static void transmit_chars(struct m68k_serial *info, struct tty_struct *tty)
 {
 	m68328_uart *uart = &uart_addr[info->line];
 
@@ -312,7 +327,7 @@ static void transmit_chars(struct m68k_serial *info)
 		goto clear_and_return;
 	}
 
-	if((info->xmit_cnt <= 0) || info->tty->stopped) {
+	if ((info->xmit_cnt <= 0) || !tty || tty->stopped) {
 		/* That's peculiar... TX ints off */
 		uart->ustcnt &= ~USTCNT_TX_INTR_MASK;
 		goto clear_and_return;
@@ -340,6 +355,7 @@ clear_and_return:
 irqreturn_t rs_interrupt(int irq, void *dev_id)
 {
 	struct m68k_serial *info = dev_id;
+	struct tty_struct *tty = tty_port_tty_get(&info->tport);
 	m68328_uart *uart;
 	unsigned short rx;
 	unsigned short tx;
@@ -350,20 +366,24 @@ irqreturn_t rs_interrupt(int irq, void *dev_id)
 #ifdef USE_INTS
 	tx = uart->utx.w;
 
-	if (rx & URX_DATA_READY) receive_chars(info, rx);
-	if (tx & UTX_TX_AVAIL)   transmit_chars(info);
+	if (rx & URX_DATA_READY)
+		receive_chars(info, tty, rx);
+	if (tx & UTX_TX_AVAIL)
+		transmit_chars(info, tty);
 #else
-	receive_chars(info, rx);		
+	receive_chars(info, tty, rx);
 #endif
+	tty_kref_put(tty);
+
 	return IRQ_HANDLED;
 }
 
-static int startup(struct m68k_serial * info)
+static int startup(struct m68k_serial *info, struct tty_struct *tty)
 {
 	m68328_uart *uart = &uart_addr[info->line];
 	unsigned long flags;
 	
-	if (info->flags & S_INITIALIZED)
+	if (info->tport.flags & ASYNC_INITIALIZED)
 		return 0;
 
 	if (!info->xmit_buf) {
@@ -380,7 +400,6 @@ static int startup(struct m68k_serial * info)
 	 */
 
 	uart->ustcnt = USTCNT_UEN;
-	info->xmit_fifo_size = 1;
 	uart->ustcnt = USTCNT_UEN | USTCNT_RXEN | USTCNT_TXEN;
 	(void)uart->urx.w;
 
@@ -394,17 +413,17 @@ static int startup(struct m68k_serial * info)
 	uart->ustcnt = USTCNT_UEN | USTCNT_RXEN | USTCNT_RX_INTR_MASK;
 #endif
 
-	if (info->tty)
-		clear_bit(TTY_IO_ERROR, &info->tty->flags);
+	if (tty)
+		clear_bit(TTY_IO_ERROR, &tty->flags);
 	info->xmit_cnt = info->xmit_head = info->xmit_tail = 0;
 
 	/*
 	 * and set the speed of the serial port
 	 */
 
-	change_speed(info);
+	change_speed(info, tty);
 
-	info->flags |= S_INITIALIZED;
+	info->tport.flags |= ASYNC_INITIALIZED;
 	local_irq_restore(flags);
 	return 0;
 }
@@ -413,13 +432,13 @@ static int startup(struct m68k_serial * info)
  * This routine will shutdown a serial port; interrupts are disabled, and
  * DTR is dropped if the hangup on close termio flag is on.
  */
-static void shutdown(struct m68k_serial * info)
+static void shutdown(struct m68k_serial *info, struct tty_struct *tty)
 {
 	m68328_uart *uart = &uart_addr[info->line];
 	unsigned long	flags;
 
 	uart->ustcnt = 0; /* All off! */
-	if (!(info->flags & S_INITIALIZED))
+	if (!(info->tport.flags & ASYNC_INITIALIZED))
 		return;
 
 	local_irq_save(flags);
@@ -429,10 +448,10 @@ static void shutdown(struct m68k_serial * info)
 		info->xmit_buf = 0;
 	}
 
-	if (info->tty)
-		set_bit(TTY_IO_ERROR, &info->tty->flags);
+	if (tty)
+		set_bit(TTY_IO_ERROR, &tty->flags);
 	
-	info->flags &= ~S_INITIALIZED;
+	info->tport.flags &= ~ASYNC_INITIALIZED;
 	local_irq_restore(flags);
 }
 
@@ -488,7 +507,7 @@ struct {
  * This routine is called to set the UART divisor registers to match
  * the specified baud rate for a serial port.
  */
-static void change_speed(struct m68k_serial *info)
+static void change_speed(struct m68k_serial *info, struct tty_struct *tty)
 {
 	m68328_uart *uart = &uart_addr[info->line];
 	unsigned short port;
@@ -496,9 +515,7 @@ static void change_speed(struct m68k_serial *info)
 	unsigned cflag;
 	int	i;
 
-	if (!info->tty || !info->tty->termios)
-		return;
-	cflag = info->tty->termios->c_cflag;
+	cflag = tty->termios->c_cflag;
 	if (!(port = info->port))
 		return;
 
@@ -510,7 +527,6 @@ static void change_speed(struct m68k_serial *info)
                 i = (i & ~CBAUDEX) + B38400;
         }
 
-	info->baud = baud_table[i];
 	uart->ubaud = PUT_FIELD(UBAUD_DIVIDE,    hw_baud_table[i].divisor) | 
 		PUT_FIELD(UBAUD_PRESCALER, hw_baud_table[i].prescale);
 
@@ -807,10 +823,10 @@ static int get_serial_info(struct m68k_serial * info,
 	tmp.line = info->line;
 	tmp.port = info->port;
 	tmp.irq = info->irq;
-	tmp.flags = info->flags;
+	tmp.flags = info->tport.flags;
 	tmp.baud_base = info->baud_base;
-	tmp.close_delay = info->close_delay;
-	tmp.closing_wait = info->closing_wait;
+	tmp.close_delay = info->tport.close_delay;
+	tmp.closing_wait = info->tport.closing_wait;
 	tmp.custom_divisor = info->custom_divisor;
 	if (copy_to_user(retinfo, &tmp, sizeof(*retinfo)))
 		return -EFAULT;
@@ -818,9 +834,10 @@ static int get_serial_info(struct m68k_serial * info,
 	return 0;
 }
 
-static int set_serial_info(struct m68k_serial * info,
+static int set_serial_info(struct m68k_serial *info, struct tty_struct *tty,
 			   struct serial_struct * new_info)
 {
+	struct tty_port *port = &info->tport;
 	struct serial_struct new_serial;
 	struct m68k_serial old_info;
 	int 			retval = 0;
@@ -834,17 +851,17 @@ static int set_serial_info(struct m68k_serial * info,
 	if (!capable(CAP_SYS_ADMIN)) {
 		if ((new_serial.baud_base != info->baud_base) ||
 		    (new_serial.type != info->type) ||
-		    (new_serial.close_delay != info->close_delay) ||
-		    ((new_serial.flags & ~S_USR_MASK) !=
-		     (info->flags & ~S_USR_MASK)))
+		    (new_serial.close_delay != port->close_delay) ||
+		    ((new_serial.flags & ~ASYNC_USR_MASK) !=
+		     (port->flags & ~ASYNC_USR_MASK)))
 			return -EPERM;
-		info->flags = ((info->flags & ~S_USR_MASK) |
-			       (new_serial.flags & S_USR_MASK));
+		port->flags = ((port->flags & ~ASYNC_USR_MASK) |
+			       (new_serial.flags & ASYNC_USR_MASK));
 		info->custom_divisor = new_serial.custom_divisor;
 		goto check_and_exit;
 	}
 
-	if (info->count > 1)
+	if (port->count > 1)
 		return -EBUSY;
 
 	/*
@@ -853,14 +870,14 @@ static int set_serial_info(struct m68k_serial * info,
 	 */
 
 	info->baud_base = new_serial.baud_base;
-	info->flags = ((info->flags & ~S_FLAGS) |
-			(new_serial.flags & S_FLAGS));
+	port->flags = ((port->flags & ~ASYNC_FLAGS) |
+			(new_serial.flags & ASYNC_FLAGS));
 	info->type = new_serial.type;
-	info->close_delay = new_serial.close_delay;
-	info->closing_wait = new_serial.closing_wait;
+	port->close_delay = new_serial.close_delay;
+	port->closing_wait = new_serial.closing_wait;
 
 check_and_exit:
-	retval = startup(info);
+	retval = startup(info, tty);
 	return retval;
 }
 
@@ -946,7 +963,7 @@ static int rs_ioctl(struct tty_struct *tty,
 			return get_serial_info(info,
 				       (struct serial_struct *) arg);
 		case TIOCSSERIAL:
-			return set_serial_info(info,
+			return set_serial_info(info, tty,
 					       (struct serial_struct *) arg);
 		case TIOCSERGETLSR: /* Get line status register */
 			return get_lsr_info(info, (unsigned int *) arg);
@@ -965,7 +982,7 @@ static void rs_set_termios(struct tty_struct *tty, struct ktermios *old_termios)
 {
 	struct m68k_serial *info = (struct m68k_serial *)tty->driver_data;
 
-	change_speed(info);
+	change_speed(info, tty);
 
 	if ((old_termios->c_cflag & CRTSCTS) &&
 	    !(tty->termios->c_cflag & CRTSCTS)) {
@@ -988,6 +1005,7 @@ static void rs_set_termios(struct tty_struct *tty, struct ktermios *old_termios)
 static void rs_close(struct tty_struct *tty, struct file * filp)
 {
 	struct m68k_serial * info = (struct m68k_serial *)tty->driver_data;
+	struct tty_port *port = &info->tport;
 	m68328_uart *uart = &uart_addr[info->line];
 	unsigned long flags;
 
@@ -1001,7 +1019,7 @@ static void rs_close(struct tty_struct *tty, struct file * filp)
 		return;
 	}
 	
-	if ((tty->count == 1) && (info->count != 1)) {
+	if ((tty->count == 1) && (port->count != 1)) {
 		/*
 		 * Uh, oh.  tty->count is 1, which means that the tty
 		 * structure will be freed.  Info->count should always
@@ -1010,26 +1028,26 @@ static void rs_close(struct tty_struct *tty, struct file * filp)
 		 * serial port won't be shutdown.
 		 */
 		printk("rs_close: bad serial port count; tty->count is 1, "
-		       "info->count is %d\n", info->count);
-		info->count = 1;
+		       "port->count is %d\n", port->count);
+		port->count = 1;
 	}
-	if (--info->count < 0) {
+	if (--port->count < 0) {
 		printk("rs_close: bad serial port count for ttyS%d: %d\n",
-		       info->line, info->count);
-		info->count = 0;
+		       info->line, port->count);
+		port->count = 0;
 	}
-	if (info->count) {
+	if (port->count) {
 		local_irq_restore(flags);
 		return;
 	}
-	info->flags |= S_CLOSING;
+	port->flags |= ASYNC_CLOSING;
 	/*
 	 * Now we wait for the transmit buffer to clear; and we notify 
 	 * the line discipline to only process XON/XOFF characters.
 	 */
 	tty->closing = 1;
-	if (info->closing_wait != S_CLOSING_WAIT_NONE)
-		tty_wait_until_sent(tty, info->closing_wait);
+	if (port->closing_wait != ASYNC_CLOSING_WAIT_NONE)
+		tty_wait_until_sent(tty, port->closing_wait);
 	/*
 	 * At this point we stop accepting input.  To do this, we
 	 * disable the receive line status interrupts, and tell the
@@ -1040,13 +1058,12 @@ static void rs_close(struct tty_struct *tty, struct file * filp)
 	uart->ustcnt &= ~USTCNT_RXEN;
 	uart->ustcnt &= ~(USTCNT_RXEN | USTCNT_RX_INTR_MASK);
 
-	shutdown(info);
+	shutdown(info, tty);
 	rs_flush_buffer(tty);
 		
 	tty_ldisc_flush(tty);
 	tty->closing = 0;
-	info->event = 0;
-	info->tty = NULL;
+	tty_port_tty_set(&info->tport, NULL);
 #warning "This is not and has never been valid so fix it"	
 #if 0
 	if (tty->ldisc.num != ldiscs[N_TTY].num) {
@@ -1058,14 +1075,13 @@ static void rs_close(struct tty_struct *tty, struct file * filp)
 			(tty->ldisc.open)(tty);
 	}
 #endif	
-	if (info->blocked_open) {
-		if (info->close_delay) {
-			msleep_interruptible(jiffies_to_msecs(info->close_delay));
-		}
-		wake_up_interruptible(&info->open_wait);
+	if (port->blocked_open) {
+		if (port->close_delay)
+			msleep_interruptible(jiffies_to_msecs(port->close_delay));
+		wake_up_interruptible(&port->open_wait);
 	}
-	info->flags &= ~(S_NORMAL_ACTIVE|S_CLOSING);
-	wake_up_interruptible(&info->close_wait);
+	port->flags &= ~(ASYNC_NORMAL_ACTIVE|ASYNC_CLOSING);
+	wake_up_interruptible(&port->close_wait);
 	local_irq_restore(flags);
 }
 
@@ -1080,107 +1096,14 @@ void rs_hangup(struct tty_struct *tty)
 		return;
 	
 	rs_flush_buffer(tty);
-	shutdown(info);
-	info->event = 0;
-	info->count = 0;
-	info->flags &= ~S_NORMAL_ACTIVE;
-	info->tty = NULL;
-	wake_up_interruptible(&info->open_wait);
+	shutdown(info, tty);
+	info->tport.count = 0;
+	info->tport.flags &= ~ASYNC_NORMAL_ACTIVE;
+	tty_port_tty_set(&info->tport, NULL);
+	wake_up_interruptible(&info->tport.open_wait);
 }
 
 /*
- * ------------------------------------------------------------
- * rs_open() and friends
- * ------------------------------------------------------------
- */
-static int block_til_ready(struct tty_struct *tty, struct file * filp,
-			   struct m68k_serial *info)
-{
-	DECLARE_WAITQUEUE(wait, current);
-	int		retval;
-	int		do_clocal = 0;
-
-	/*
-	 * If the device is in the middle of being closed, then block
-	 * until it's done, and then try again.
-	 */
-	if (info->flags & S_CLOSING) {
-		interruptible_sleep_on(&info->close_wait);
-#ifdef SERIAL_DO_RESTART
-		if (info->flags & S_HUP_NOTIFY)
-			return -EAGAIN;
-		else
-			return -ERESTARTSYS;
-#else
-		return -EAGAIN;
-#endif
-	}
-	
-	/*
-	 * If non-blocking mode is set, or the port is not enabled,
-	 * then make the check up front and then exit.
-	 */
-	if ((filp->f_flags & O_NONBLOCK) ||
-	    (tty->flags & (1 << TTY_IO_ERROR))) {
-		info->flags |= S_NORMAL_ACTIVE;
-		return 0;
-	}
-
-	if (tty->termios->c_cflag & CLOCAL)
-		do_clocal = 1;
-
-	/*
-	 * Block waiting for the carrier detect and the line to become
-	 * free (i.e., not in use by the callout).  While we are in
-	 * this loop, info->count is dropped by one, so that
-	 * rs_close() knows when to free things.  We restore it upon
-	 * exit, either normal or abnormal.
-	 */
-	retval = 0;
-	add_wait_queue(&info->open_wait, &wait);
-
-	info->count--;
-	info->blocked_open++;
-	while (1) {
-		local_irq_disable();
-		m68k_rtsdtr(info, 1);
-		local_irq_enable();
-		current->state = TASK_INTERRUPTIBLE;
-		if (tty_hung_up_p(filp) ||
-		    !(info->flags & S_INITIALIZED)) {
-#ifdef SERIAL_DO_RESTART
-			if (info->flags & S_HUP_NOTIFY)
-				retval = -EAGAIN;
-			else
-				retval = -ERESTARTSYS;	
-#else
-			retval = -EAGAIN;
-#endif
-			break;
-		}
-		if (!(info->flags & S_CLOSING) && do_clocal)
-			break;
-                if (signal_pending(current)) {
-			retval = -ERESTARTSYS;
-			break;
-		}
-		tty_unlock();
-		schedule();
-		tty_lock();
-	}
-	current->state = TASK_RUNNING;
-	remove_wait_queue(&info->open_wait, &wait);
-	if (!tty_hung_up_p(filp))
-		info->count++;
-	info->blocked_open--;
-
-	if (retval)
-		return retval;
-	info->flags |= S_NORMAL_ACTIVE;
-	return 0;
-}	
-
-/*
  * This routine is called whenever a serial port is opened.  It
  * enables interrupts for a serial port, linking in its S structure into
  * the IRQ chain.   It also performs the serial-specific
@@ -1196,18 +1119,18 @@ int rs_open(struct tty_struct *tty, struct file * filp)
 	if (serial_paranoia_check(info, tty->name, "rs_open"))
 		return -ENODEV;
 
-	info->count++;
+	info->tport.count++;
 	tty->driver_data = info;
-	info->tty = tty;
+	tty_port_tty_set(&info->tport, tty);
 
 	/*
 	 * Start up serial port
 	 */
-	retval = startup(info);
+	retval = startup(info, tty);
 	if (retval)
 		return retval;
 
-	return block_til_ready(tty, filp, info);
+	return tty_port_block_til_ready(&info->tport, tty, filp);
 }
 
 /* Finally, routines used to initialize the serial driver. */
@@ -1235,11 +1158,15 @@ static const struct tty_operations rs_ops = {
 	.set_ldisc = rs_set_ldisc,
 };
 
+static const struct tty_port_operations rs_port_ops = {
+};
+
 /* rs_init inits the driver */
 static int __init
 rs68328_init(void)
 {
-	int flags, i;
+	unsigned long flags;
+	int i;
 	struct m68k_serial *info;
 
 	serial_driver = alloc_tty_driver(NR_PORTS);
@@ -1273,19 +1200,13 @@ rs68328_init(void)
 	for(i=0;i<NR_PORTS;i++) {
 
 	    info = &m68k_soft[i];
+	    tty_port_init(&info->tport);
+	    info->tport.ops = &rs_port_ops;
 	    info->magic = SERIAL_MAGIC;
 	    info->port = (int) &uart_addr[i];
-	    info->tty = NULL;
 	    info->irq = uart_irqs[i];
 	    info->custom_divisor = 16;
-	    info->close_delay = 50;
-	    info->closing_wait = 3000;
 	    info->x_char = 0;
-	    info->event = 0;
-	    info->count = 0;
-	    info->blocked_open = 0;
-	    init_waitqueue_head(&info->open_wait);
-	    init_waitqueue_head(&info->close_wait);
 	    info->line = i;
 	    info->is_cons = 1; /* Means shortcuts work */
 	    
diff --git a/drivers/tty/serial/68328serial.h b/drivers/tty/serial/68328serial.h
deleted file mode 100644
index 3d2faabd766..00000000000
--- a/drivers/tty/serial/68328serial.h
+++ /dev/null
@@ -1,186 +0,0 @@
-/* 68328serial.h: Definitions for the mc68328 serial driver.
- *
- * Copyright (C) 1995       David S. Miller    <davem@caip.rutgers.edu>
- * Copyright (C) 1998       Kenneth Albanowski <kjahds@kjahds.com>
- * Copyright (C) 1998, 1999 D. Jeff Dionne     <jeff@uclinux.org>
- * Copyright (C) 1999       Vladimir Gurevich  <vgurevic@cisco.com>
- *
- * VZ Support/Fixes             Evan Stawnyczy <e@lineo.ca>
- */
-
-#ifndef _MC683XX_SERIAL_H
-#define _MC683XX_SERIAL_H
-
-
-struct serial_struct {
-	int	type;
-	int	line;
-	int	port;
-	int	irq;
-	int	flags;
-	int	xmit_fifo_size;
-	int	custom_divisor;
-	int	baud_base;
-	unsigned short	close_delay;
-	char	reserved_char[2];
-	int	hub6;  /* FIXME: We don't have AT&T Hub6 boards! */
-	unsigned short	closing_wait; /* time to wait before closing */
-	unsigned short	closing_wait2; /* no longer used... */
-	int	reserved[4];
-};
-
-/*
- * For the close wait times, 0 means wait forever for serial port to
- * flush its output.  65535 means don't wait at all.
- */
-#define S_CLOSING_WAIT_INF	0
-#define S_CLOSING_WAIT_NONE	65535
-
-/*
- * Definitions for S_struct (and serial_struct) flags field
- */
-#define S_HUP_NOTIFY 0x0001 /* Notify getty on hangups and closes 
-				   on the callout port */
-#define S_FOURPORT  0x0002	/* Set OU1, OUT2 per AST Fourport settings */
-#define S_SAK	0x0004	/* Secure Attention Key (Orange book) */
-#define S_SPLIT_TERMIOS 0x0008 /* Separate termios for dialin/callout */
-
-#define S_SPD_MASK	0x0030
-#define S_SPD_HI	0x0010	/* Use 56000 instead of 38400 bps */
-
-#define S_SPD_VHI	0x0020  /* Use 115200 instead of 38400 bps */
-#define S_SPD_CUST	0x0030  /* Use user-specified divisor */
-
-#define S_SKIP_TEST	0x0040 /* Skip UART test during autoconfiguration */
-#define S_AUTO_IRQ  0x0080 /* Do automatic IRQ during autoconfiguration */
-#define S_SESSION_LOCKOUT 0x0100 /* Lock out cua opens based on session */
-#define S_PGRP_LOCKOUT    0x0200 /* Lock out cua opens based on pgrp */
-#define S_CALLOUT_NOHUP   0x0400 /* Don't do hangups for cua device */
-
-#define S_FLAGS	0x0FFF	/* Possible legal S flags */
-#define S_USR_MASK 0x0430	/* Legal flags that non-privileged
-				 * users can set or reset */
-
-/* Internal flags used only by kernel/chr_drv/serial.c */
-#define S_INITIALIZED	0x80000000 /* Serial port was initialized */
-#define S_CALLOUT_ACTIVE	0x40000000 /* Call out device is active */
-#define S_NORMAL_ACTIVE	0x20000000 /* Normal device is active */
-#define S_BOOT_AUTOCONF	0x10000000 /* Autoconfigure port on bootup */
-#define S_CLOSING		0x08000000 /* Serial port is closing */
-#define S_CTS_FLOW		0x04000000 /* Do CTS flow control */
-#define S_CHECK_CD		0x02000000 /* i.e., CLOCAL */
-
-/* Software state per channel */
-
-#ifdef __KERNEL__
-
-/*
- * I believe this is the optimal setting that reduces the number of interrupts.
- * At high speeds the output might become a little "bursted" (use USTCNT_TXHE
- * if that bothers you), but in most cases it will not, since we try to 
- * transmit characters every time rs_interrupt is called. Thus, quite often
- * you'll see that a receive interrupt occures before the transmit one.
- *                                  -- Vladimir Gurevich
- */
-#define USTCNT_TX_INTR_MASK (USTCNT_TXEE)
-
-/*
- * 68328 and 68EZ328 UARTS are a little bit different. EZ328 has special
- * "Old data interrupt" which occures whenever the data stay in the FIFO
- * longer than 30 bits time. This allows us to use FIFO without compromising
- * latency. '328 does not have this feature and without the real  328-based
- * board I would assume that RXRE is the safest setting.
- *
- * For EZ328 I use RXHE (Half empty) interrupt to reduce the number of
- * interrupts. RXFE (receive queue full) causes the system to lose data
- * at least at 115200 baud
- *
- * If your board is busy doing other stuff, you might consider to use
- * RXRE (data ready intrrupt) instead.
- *
- * The other option is to make these INTR masks run-time configurable, so
- * that people can dynamically adapt them according to the current usage.
- *                                  -- Vladimir Gurevich
- */
-
-/* (es) */
-#if defined(CONFIG_M68EZ328) || defined(CONFIG_M68VZ328)
-#define USTCNT_RX_INTR_MASK (USTCNT_RXHE | USTCNT_ODEN)
-#elif defined(CONFIG_M68328)
-#define USTCNT_RX_INTR_MASK (USTCNT_RXRE)
-#else
-#error Please, define the Rx interrupt events for your CPU
-#endif
-/* (/es) */
-
-/*
- * This is our internal structure for each serial port's state.
- * 
- * Many fields are paralleled by the structure used by the serial_struct
- * structure.
- *
- * For definitions of the flags field, see tty.h
- */
-
-struct m68k_serial {
-	char soft_carrier;  /* Use soft carrier on this channel */
-	char break_abort;   /* Is serial console in, so process brk/abrt */
-	char is_cons;       /* Is this our console. */
-
-	/* We need to know the current clock divisor
-	 * to read the bps rate the chip has currently
-	 * loaded.
-	 */
-	unsigned char clk_divisor;  /* May be 1, 16, 32, or 64 */
-	int baud;
-	int			magic;
-	int			baud_base;
-	int			port;
-	int			irq;
-	int			flags; 		/* defined in tty.h */
-	int			type; 		/* UART type */
-	struct tty_struct 	*tty;
-	int			read_status_mask;
-	int			ignore_status_mask;
-	int			timeout;
-	int			xmit_fifo_size;
-	int			custom_divisor;
-	int			x_char;	/* xon/xoff character */
-	int			close_delay;
-	unsigned short		closing_wait;
-	unsigned short		closing_wait2;
-	unsigned long		event;
-	unsigned long		last_active;
-	int			line;
-	int			count;	    /* # of fd on device */
-	int			blocked_open; /* # of blocked opens */
-	unsigned char 		*xmit_buf;
-	int			xmit_head;
-	int			xmit_tail;
-	int			xmit_cnt;
-	wait_queue_head_t	open_wait;
-	wait_queue_head_t	close_wait;
-};
-
-
-#define SERIAL_MAGIC 0x5301
-
-/*
- * The size of the serial xmit buffer is 1 page, or 4096 bytes
- */
-#define SERIAL_XMIT_SIZE 4096
-
-/*
- * Events are used to schedule things to happen at timer-interrupt
- * time, instead of at rs interrupt time.
- */
-#define RS_EVENT_WRITE_WAKEUP	0
-
-/* 
- * Define the number of ports supported and their irqs.
- */
-#define NR_PORTS 1
-#define UART_IRQ_DEFNS {UART_IRQ_NUM}
-
-#endif /* __KERNEL__ */
-#endif /* !(_MC683XX_SERIAL_H) */
diff --git a/drivers/tty/serial/8250/8250.c b/drivers/tty/serial/8250/8250.c
index 5c27f7e6c9f..47d061b9ad4 100644
--- a/drivers/tty/serial/8250/8250.c
+++ b/drivers/tty/serial/8250/8250.c
@@ -284,7 +284,20 @@ static const struct serial8250_config uart_config[] = {
 	},
 };
 
-#if defined(CONFIG_MIPS_ALCHEMY)
+/* Uart divisor latch read */
+static int default_serial_dl_read(struct uart_8250_port *up)
+{
+	return serial_in(up, UART_DLL) | serial_in(up, UART_DLM) << 8;
+}
+
+/* Uart divisor latch write */
+static void default_serial_dl_write(struct uart_8250_port *up, int value)
+{
+	serial_out(up, UART_DLL, value & 0xff);
+	serial_out(up, UART_DLM, value >> 8 & 0xff);
+}
+
+#ifdef CONFIG_MIPS_ALCHEMY
 
 /* Au1x00 UART hardware has a weird register layout */
 static const u8 au_io_in_map[] = {
@@ -305,22 +318,32 @@ static const u8 au_io_out_map[] = {
 	[UART_MCR] = 6,
 };
 
-/* sane hardware needs no mapping */
-static inline int map_8250_in_reg(struct uart_port *p, int offset)
+static unsigned int au_serial_in(struct uart_port *p, int offset)
+{
+	offset = au_io_in_map[offset] << p->regshift;
+	return __raw_readl(p->membase + offset);
+}
+
+static void au_serial_out(struct uart_port *p, int offset, int value)
+{
+	offset = au_io_out_map[offset] << p->regshift;
+	__raw_writel(value, p->membase + offset);
+}
+
+/* Au1x00 haven't got a standard divisor latch */
+static int au_serial_dl_read(struct uart_8250_port *up)
 {
-	if (p->iotype != UPIO_AU)
-		return offset;
-	return au_io_in_map[offset];
+	return __raw_readl(up->port.membase + 0x28);
 }
 
-static inline int map_8250_out_reg(struct uart_port *p, int offset)
+static void au_serial_dl_write(struct uart_8250_port *up, int value)
 {
-	if (p->iotype != UPIO_AU)
-		return offset;
-	return au_io_out_map[offset];
+	__raw_writel(value, up->port.membase + 0x28);
 }
 
-#elif defined(CONFIG_SERIAL_8250_RM9K)
+#endif
+
+#ifdef CONFIG_SERIAL_8250_RM9K
 
 static const u8
 	regmap_in[8] = {
@@ -344,87 +367,79 @@ static const u8
 		[UART_SCR]	= 0x2c
 	};
 
-static inline int map_8250_in_reg(struct uart_port *p, int offset)
+static unsigned int rm9k_serial_in(struct uart_port *p, int offset)
 {
-	if (p->iotype != UPIO_RM9000)
-		return offset;
-	return regmap_in[offset];
+	offset = regmap_in[offset] << p->regshift;
+	return readl(p->membase + offset);
 }
 
-static inline int map_8250_out_reg(struct uart_port *p, int offset)
+static void rm9k_serial_out(struct uart_port *p, int offset, int value)
 {
-	if (p->iotype != UPIO_RM9000)
-		return offset;
-	return regmap_out[offset];
+	offset = regmap_out[offset] << p->regshift;
+	writel(value, p->membase + offset);
 }
 
-#else
+static int rm9k_serial_dl_read(struct uart_8250_port *up)
+{
+	return ((__raw_readl(up->port.membase + 0x10) << 8) |
+		(__raw_readl(up->port.membase + 0x08) & 0xff)) & 0xffff;
+}
 
-/* sane hardware needs no mapping */
-#define map_8250_in_reg(up, offset) (offset)
-#define map_8250_out_reg(up, offset) (offset)
+static void rm9k_serial_dl_write(struct uart_8250_port *up, int value)
+{
+	__raw_writel(value, up->port.membase + 0x08);
+	__raw_writel(value >> 8, up->port.membase + 0x10);
+}
 
 #endif
 
 static unsigned int hub6_serial_in(struct uart_port *p, int offset)
 {
-	offset = map_8250_in_reg(p, offset) << p->regshift;
+	offset = offset << p->regshift;
 	outb(p->hub6 - 1 + offset, p->iobase);
 	return inb(p->iobase + 1);
 }
 
 static void hub6_serial_out(struct uart_port *p, int offset, int value)
 {
-	offset = map_8250_out_reg(p, offset) << p->regshift;
+	offset = offset << p->regshift;
 	outb(p->hub6 - 1 + offset, p->iobase);
 	outb(value, p->iobase + 1);
 }
 
 static unsigned int mem_serial_in(struct uart_port *p, int offset)
 {
-	offset = map_8250_in_reg(p, offset) << p->regshift;
+	offset = offset << p->regshift;
 	return readb(p->membase + offset);
 }
 
 static void mem_serial_out(struct uart_port *p, int offset, int value)
 {
-	offset = map_8250_out_reg(p, offset) << p->regshift;
+	offset = offset << p->regshift;
 	writeb(value, p->membase + offset);
 }
 
 static void mem32_serial_out(struct uart_port *p, int offset, int value)
 {
-	offset = map_8250_out_reg(p, offset) << p->regshift;
+	offset = offset << p->regshift;
 	writel(value, p->membase + offset);
 }
 
 static unsigned int mem32_serial_in(struct uart_port *p, int offset)
 {
-	offset = map_8250_in_reg(p, offset) << p->regshift;
+	offset = offset << p->regshift;
 	return readl(p->membase + offset);
 }
 
-static unsigned int au_serial_in(struct uart_port *p, int offset)
-{
-	offset = map_8250_in_reg(p, offset) << p->regshift;
-	return __raw_readl(p->membase + offset);
-}
-
-static void au_serial_out(struct uart_port *p, int offset, int value)
-{
-	offset = map_8250_out_reg(p, offset) << p->regshift;
-	__raw_writel(value, p->membase + offset);
-}
-
 static unsigned int io_serial_in(struct uart_port *p, int offset)
 {
-	offset = map_8250_in_reg(p, offset) << p->regshift;
+	offset = offset << p->regshift;
 	return inb(p->iobase + offset);
 }
 
 static void io_serial_out(struct uart_port *p, int offset, int value)
 {
-	offset = map_8250_out_reg(p, offset) << p->regshift;
+	offset = offset << p->regshift;
 	outb(value, p->iobase + offset);
 }
 
@@ -434,6 +449,10 @@ static void set_io_from_upio(struct uart_port *p)
 {
 	struct uart_8250_port *up =
 		container_of(p, struct uart_8250_port, port);
+
+	up->dl_read = default_serial_dl_read;
+	up->dl_write = default_serial_dl_write;
+
 	switch (p->iotype) {
 	case UPIO_HUB6:
 		p->serial_in = hub6_serial_in;
@@ -445,16 +464,28 @@ static void set_io_from_upio(struct uart_port *p)
 		p->serial_out = mem_serial_out;
 		break;
 
-	case UPIO_RM9000:
 	case UPIO_MEM32:
 		p->serial_in = mem32_serial_in;
 		p->serial_out = mem32_serial_out;
 		break;
 
+#ifdef CONFIG_SERIAL_8250_RM9K
+	case UPIO_RM9000:
+		p->serial_in = rm9k_serial_in;
+		p->serial_out = rm9k_serial_out;
+		up->dl_read = rm9k_serial_dl_read;
+		up->dl_write = rm9k_serial_dl_write;
+		break;
+#endif
+
+#ifdef CONFIG_MIPS_ALCHEMY
 	case UPIO_AU:
 		p->serial_in = au_serial_in;
 		p->serial_out = au_serial_out;
+		up->dl_read = au_serial_dl_read;
+		up->dl_write = au_serial_dl_write;
 		break;
+#endif
 
 	default:
 		p->serial_in = io_serial_in;
@@ -481,59 +512,6 @@ serial_port_out_sync(struct uart_port *p, int offset, int value)
 	}
 }
 
-/* Uart divisor latch read */
-static inline int _serial_dl_read(struct uart_8250_port *up)
-{
-	return serial_in(up, UART_DLL) | serial_in(up, UART_DLM) << 8;
-}
-
-/* Uart divisor latch write */
-static inline void _serial_dl_write(struct uart_8250_port *up, int value)
-{
-	serial_out(up, UART_DLL, value & 0xff);
-	serial_out(up, UART_DLM, value >> 8 & 0xff);
-}
-
-#if defined(CONFIG_MIPS_ALCHEMY)
-/* Au1x00 haven't got a standard divisor latch */
-static int serial_dl_read(struct uart_8250_port *up)
-{
-	if (up->port.iotype == UPIO_AU)
-		return __raw_readl(up->port.membase + 0x28);
-	else
-		return _serial_dl_read(up);
-}
-
-static void serial_dl_write(struct uart_8250_port *up, int value)
-{
-	if (up->port.iotype == UPIO_AU)
-		__raw_writel(value, up->port.membase + 0x28);
-	else
-		_serial_dl_write(up, value);
-}
-#elif defined(CONFIG_SERIAL_8250_RM9K)
-static int serial_dl_read(struct uart_8250_port *up)
-{
-	return	(up->port.iotype == UPIO_RM9000) ?
-		(((__raw_readl(up->port.membase + 0x10) << 8) |
-		(__raw_readl(up->port.membase + 0x08) & 0xff)) & 0xffff) :
-		_serial_dl_read(up);
-}
-
-static void serial_dl_write(struct uart_8250_port *up, int value)
-{
-	if (up->port.iotype == UPIO_RM9000) {
-		__raw_writel(value, up->port.membase + 0x08);
-		__raw_writel(value >> 8, up->port.membase + 0x10);
-	} else {
-		_serial_dl_write(up, value);
-	}
-}
-#else
-#define serial_dl_read(up) _serial_dl_read(up)
-#define serial_dl_write(up, value) _serial_dl_write(up, value)
-#endif
-
 /*
  * For the 16C950
  */
@@ -568,6 +546,16 @@ static void serial8250_clear_fifos(struct uart_8250_port *p)
 	}
 }
 
+void serial8250_clear_and_reinit_fifos(struct uart_8250_port *p)
+{
+	unsigned char fcr;
+
+	serial8250_clear_fifos(p);
+	fcr = uart_config[p->port.type].fcr;
+	serial_out(p, UART_FCR, fcr);
+}
+EXPORT_SYMBOL_GPL(serial8250_clear_and_reinit_fifos);
+
 /*
  * IER sleep support.  UARTs which have EFRs need the "extended
  * capability" bit enabled.  Note that on XR16C850s, we need to
@@ -1332,27 +1320,6 @@ static void serial8250_enable_ms(struct uart_port *port)
 }
 
 /*
- * Clear the Tegra rx fifo after a break
- *
- * FIXME: This needs to become a port specific callback once we have a
- * framework for this
- */
-static void clear_rx_fifo(struct uart_8250_port *up)
-{
-	unsigned int status, tmout = 10000;
-	do {
-		status = serial_in(up, UART_LSR);
-		if (status & (UART_LSR_FIFOE | UART_LSR_BRK_ERROR_BITS))
-			status = serial_in(up, UART_RX);
-		else
-			break;
-		if (--tmout == 0)
-			break;
-		udelay(1);
-	} while (1);
-}
-
-/*
  * serial8250_rx_chars: processes according to the passed in LSR
  * value, and returns the remaining LSR bits not handled
  * by this Rx routine.
@@ -1386,20 +1353,10 @@ serial8250_rx_chars(struct uart_8250_port *up, unsigned char lsr)
 		up->lsr_saved_flags = 0;
 
 		if (unlikely(lsr & UART_LSR_BRK_ERROR_BITS)) {
-			/*
-			 * For statistics only
-			 */
 			if (lsr & UART_LSR_BI) {
 				lsr &= ~(UART_LSR_FE | UART_LSR_PE);
 				port->icount.brk++;
 				/*
-				 * If tegra port then clear the rx fifo to
-				 * accept another break/character.
-				 */
-				if (port->type == PORT_TEGRA)
-					clear_rx_fifo(up);
-
-				/*
 				 * We do the SysRQ and SAK checking
 				 * here because otherwise the break
 				 * may get masked by ignore_status_mask
@@ -2280,10 +2237,11 @@ serial8250_do_set_termios(struct uart_port *port, struct ktermios *termios,
 		quot++;
 
 	if (up->capabilities & UART_CAP_FIFO && port->fifosize > 1) {
-		if (baud < 2400)
-			fcr = UART_FCR_ENABLE_FIFO | UART_FCR_TRIGGER_1;
-		else
-			fcr = uart_config[port->type].fcr;
+		fcr = uart_config[port->type].fcr;
+		if (baud < 2400) {
+			fcr &= ~UART_FCR_TRIGGER_MASK;
+			fcr |= UART_FCR_TRIGGER_1;
+		}
 	}
 
 	/*
@@ -3037,6 +2995,7 @@ static int __devinit serial8250_probe(struct platform_device *dev)
 		port.serial_in		= p->serial_in;
 		port.serial_out		= p->serial_out;
 		port.handle_irq		= p->handle_irq;
+		port.handle_break	= p->handle_break;
 		port.set_termios	= p->set_termios;
 		port.pm			= p->pm;
 		port.dev		= &dev->dev;
@@ -3153,7 +3112,7 @@ static struct uart_8250_port *serial8250_find_match_or_unused(struct uart_port *
 }
 
 /**
- *	serial8250_register_port - register a serial port
+ *	serial8250_register_8250_port - register a serial port
  *	@port: serial port template
  *
  *	Configure the serial port specified by the request. If the
@@ -3165,50 +3124,56 @@ static struct uart_8250_port *serial8250_find_match_or_unused(struct uart_port *
  *
  *	On success the port is ready to use and the line number is returned.
  */
-int serial8250_register_port(struct uart_port *port)
+int serial8250_register_8250_port(struct uart_8250_port *up)
 {
 	struct uart_8250_port *uart;
 	int ret = -ENOSPC;
 
-	if (port->uartclk == 0)
+	if (up->port.uartclk == 0)
 		return -EINVAL;
 
 	mutex_lock(&serial_mutex);
 
-	uart = serial8250_find_match_or_unused(port);
+	uart = serial8250_find_match_or_unused(&up->port);
 	if (uart) {
 		uart_remove_one_port(&serial8250_reg, &uart->port);
 
-		uart->port.iobase       = port->iobase;
-		uart->port.membase      = port->membase;
-		uart->port.irq          = port->irq;
-		uart->port.irqflags     = port->irqflags;
-		uart->port.uartclk      = port->uartclk;
-		uart->port.fifosize     = port->fifosize;
-		uart->port.regshift     = port->regshift;
-		uart->port.iotype       = port->iotype;
-		uart->port.flags        = port->flags | UPF_BOOT_AUTOCONF;
-		uart->port.mapbase      = port->mapbase;
-		uart->port.private_data = port->private_data;
-		if (port->dev)
-			uart->port.dev = port->dev;
-
-		if (port->flags & UPF_FIXED_TYPE)
-			serial8250_init_fixed_type_port(uart, port->type);
+		uart->port.iobase       = up->port.iobase;
+		uart->port.membase      = up->port.membase;
+		uart->port.irq          = up->port.irq;
+		uart->port.irqflags     = up->port.irqflags;
+		uart->port.uartclk      = up->port.uartclk;
+		uart->port.fifosize     = up->port.fifosize;
+		uart->port.regshift     = up->port.regshift;
+		uart->port.iotype       = up->port.iotype;
+		uart->port.flags        = up->port.flags | UPF_BOOT_AUTOCONF;
+		uart->port.mapbase      = up->port.mapbase;
+		uart->port.private_data = up->port.private_data;
+		if (up->port.dev)
+			uart->port.dev = up->port.dev;
+
+		if (up->port.flags & UPF_FIXED_TYPE)
+			serial8250_init_fixed_type_port(uart, up->port.type);
 
 		set_io_from_upio(&uart->port);
 		/* Possibly override default I/O functions.  */
-		if (port->serial_in)
-			uart->port.serial_in = port->serial_in;
-		if (port->serial_out)
-			uart->port.serial_out = port->serial_out;
-		if (port->handle_irq)
-			uart->port.handle_irq = port->handle_irq;
+		if (up->port.serial_in)
+			uart->port.serial_in = up->port.serial_in;
+		if (up->port.serial_out)
+			uart->port.serial_out = up->port.serial_out;
+		if (up->port.handle_irq)
+			uart->port.handle_irq = up->port.handle_irq;
 		/*  Possibly override set_termios call */
-		if (port->set_termios)
-			uart->port.set_termios = port->set_termios;
-		if (port->pm)
-			uart->port.pm = port->pm;
+		if (up->port.set_termios)
+			uart->port.set_termios = up->port.set_termios;
+		if (up->port.pm)
+			uart->port.pm = up->port.pm;
+		if (up->port.handle_break)
+			uart->port.handle_break = up->port.handle_break;
+		if (up->dl_read)
+			uart->dl_read = up->dl_read;
+		if (up->dl_write)
+			uart->dl_write = up->dl_write;
 
 		if (serial8250_isa_config != NULL)
 			serial8250_isa_config(0, &uart->port,
@@ -3222,6 +3187,29 @@ int serial8250_register_port(struct uart_port *port)
 
 	return ret;
 }
+EXPORT_SYMBOL(serial8250_register_8250_port);
+
+/**
+ *	serial8250_register_port - register a serial port
+ *	@port: serial port template
+ *
+ *	Configure the serial port specified by the request. If the
+ *	port exists and is in use, it is hung up and unregistered
+ *	first.
+ *
+ *	The port is then probed and if necessary the IRQ is autodetected
+ *	If this fails an error is returned.
+ *
+ *	On success the port is ready to use and the line number is returned.
+ */
+int serial8250_register_port(struct uart_port *port)
+{
+	struct uart_8250_port up;
+
+	memset(&up, 0, sizeof(up));
+	memcpy(&up.port, port, sizeof(*port));
+	return serial8250_register_8250_port(&up);
+}
 EXPORT_SYMBOL(serial8250_register_port);
 
 /**
diff --git a/drivers/tty/serial/8250/8250.h b/drivers/tty/serial/8250/8250.h
index 2868a1da254..f9719d167c8 100644
--- a/drivers/tty/serial/8250/8250.h
+++ b/drivers/tty/serial/8250/8250.h
@@ -37,6 +37,10 @@ struct uart_8250_port {
 	unsigned char		lsr_saved_flags;
 #define MSR_SAVE_FLAGS UART_MSR_ANY_DELTA
 	unsigned char		msr_saved_flags;
+
+	/* 8250 specific callbacks */
+	int			(*dl_read)(struct uart_8250_port *);
+	void			(*dl_write)(struct uart_8250_port *, int);
 };
 
 struct old_serial_port {
@@ -96,6 +100,18 @@ static inline void serial_out(struct uart_8250_port *up, int offset, int value)
 	up->port.serial_out(&up->port, offset, value);
 }
 
+void serial8250_clear_and_reinit_fifos(struct uart_8250_port *p);
+
+static inline int serial_dl_read(struct uart_8250_port *up)
+{
+	return up->dl_read(up);
+}
+
+static inline void serial_dl_write(struct uart_8250_port *up, int value)
+{
+	up->dl_write(up, value);
+}
+
 #if defined(__alpha__) && !defined(CONFIG_PCI)
 /*
  * Digital did something really horribly wrong with the OUT1 and OUT2
diff --git a/drivers/tty/serial/8250/8250_em.c b/drivers/tty/serial/8250/8250_em.c
new file mode 100644
index 00000000000..3a0363e7f3a
--- /dev/null
+++ b/drivers/tty/serial/8250/8250_em.c
@@ -0,0 +1,186 @@
+/*
+ * Renesas Emma Mobile 8250 driver
+ *
+ *  Copyright (C) 2012 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/device.h>
+#include <linux/init.h>
+#include <linux/io.h>
+#include <linux/module.h>
+#include <linux/serial_8250.h>
+#include <linux/serial_core.h>
+#include <linux/serial_reg.h>
+#include <linux/platform_device.h>
+#include <linux/clk.h>
+#include <linux/slab.h>
+
+#include "8250.h"
+
+#define UART_DLL_EM 9
+#define UART_DLM_EM 10
+
+struct serial8250_em_priv {
+	struct clk *sclk;
+	int line;
+};
+
+static void serial8250_em_serial_out(struct uart_port *p, int offset, int value)
+{
+	switch (offset) {
+	case UART_TX: /* TX @ 0x00 */
+		writeb(value, p->membase);
+		break;
+	case UART_FCR: /* FCR @ 0x0c (+1) */
+	case UART_LCR: /* LCR @ 0x10 (+1) */
+	case UART_MCR: /* MCR @ 0x14 (+1) */
+	case UART_SCR: /* SCR @ 0x20 (+1) */
+		writel(value, p->membase + ((offset + 1) << 2));
+		break;
+	case UART_IER: /* IER @ 0x04 */
+		value &= 0x0f; /* only 4 valid bits - not Xscale */
+		/* fall-through */
+	case UART_DLL_EM: /* DLL @ 0x24 (+9) */
+	case UART_DLM_EM: /* DLM @ 0x28 (+9) */
+		writel(value, p->membase + (offset << 2));
+	}
+}
+
+static unsigned int serial8250_em_serial_in(struct uart_port *p, int offset)
+{
+	switch (offset) {
+	case UART_RX: /* RX @ 0x00 */
+		return readb(p->membase);
+	case UART_MCR: /* MCR @ 0x14 (+1) */
+	case UART_LSR: /* LSR @ 0x18 (+1) */
+	case UART_MSR: /* MSR @ 0x1c (+1) */
+	case UART_SCR: /* SCR @ 0x20 (+1) */
+		return readl(p->membase + ((offset + 1) << 2));
+	case UART_IER: /* IER @ 0x04 */
+	case UART_IIR: /* IIR @ 0x08 */
+	case UART_DLL_EM: /* DLL @ 0x24 (+9) */
+	case UART_DLM_EM: /* DLM @ 0x28 (+9) */
+		return readl(p->membase + (offset << 2));
+	}
+	return 0;
+}
+
+static int serial8250_em_serial_dl_read(struct uart_8250_port *up)
+{
+	return serial_in(up, UART_DLL_EM) | serial_in(up, UART_DLM_EM) << 8;
+}
+
+static void serial8250_em_serial_dl_write(struct uart_8250_port *up, int value)
+{
+	serial_out(up, UART_DLL_EM, value & 0xff);
+	serial_out(up, UART_DLM_EM, value >> 8 & 0xff);
+}
+
+static int __devinit serial8250_em_probe(struct platform_device *pdev)
+{
+	struct resource *regs = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	struct resource *irq = platform_get_resource(pdev, IORESOURCE_IRQ, 0);
+	struct serial8250_em_priv *priv;
+	struct uart_8250_port up;
+	int ret = -EINVAL;
+
+	if (!regs || !irq) {
+		dev_err(&pdev->dev, "missing registers or irq\n");
+		goto err0;
+	}
+
+	priv = kzalloc(sizeof(*priv), GFP_KERNEL);
+	if (!priv) {
+		dev_err(&pdev->dev, "unable to allocate private data\n");
+		ret = -ENOMEM;
+		goto err0;
+	}
+
+	priv->sclk = clk_get(&pdev->dev, "sclk");
+	if (IS_ERR(priv->sclk)) {
+		dev_err(&pdev->dev, "unable to get clock\n");
+		ret = PTR_ERR(priv->sclk);
+		goto err1;
+	}
+
+	memset(&up, 0, sizeof(up));
+	up.port.mapbase = regs->start;
+	up.port.irq = irq->start;
+	up.port.type = PORT_UNKNOWN;
+	up.port.flags = UPF_BOOT_AUTOCONF | UPF_FIXED_PORT | UPF_IOREMAP;
+	up.port.dev = &pdev->dev;
+	up.port.private_data = priv;
+
+	clk_enable(priv->sclk);
+	up.port.uartclk = clk_get_rate(priv->sclk);
+
+	up.port.iotype = UPIO_MEM32;
+	up.port.serial_in = serial8250_em_serial_in;
+	up.port.serial_out = serial8250_em_serial_out;
+	up.dl_read = serial8250_em_serial_dl_read;
+	up.dl_write = serial8250_em_serial_dl_write;
+
+	ret = serial8250_register_8250_port(&up);
+	if (ret < 0) {
+		dev_err(&pdev->dev, "unable to register 8250 port\n");
+		goto err2;
+	}
+
+	priv->line = ret;
+	platform_set_drvdata(pdev, priv);
+	return 0;
+
+ err2:
+	clk_disable(priv->sclk);
+	clk_put(priv->sclk);
+ err1:
+	kfree(priv);
+ err0:
+	return ret;
+}
+
+static int __devexit serial8250_em_remove(struct platform_device *pdev)
+{
+	struct serial8250_em_priv *priv = platform_get_drvdata(pdev);
+
+	serial8250_unregister_port(priv->line);
+	clk_disable(priv->sclk);
+	clk_put(priv->sclk);
+	kfree(priv);
+	return 0;
+}
+
+static const struct of_device_id serial8250_em_dt_ids[] __devinitconst = {
+	{ .compatible = "renesas,em-uart", },
+	{},
+};
+MODULE_DEVICE_TABLE(of, serial8250_em_dt_ids);
+
+static struct platform_driver serial8250_em_platform_driver = {
+	.driver = {
+		.name		= "serial8250-em",
+		.of_match_table = serial8250_em_dt_ids,
+		.owner		= THIS_MODULE,
+	},
+	.probe			= serial8250_em_probe,
+	.remove			= __devexit_p(serial8250_em_remove),
+};
+
+module_platform_driver(serial8250_em_platform_driver);
+
+MODULE_AUTHOR("Magnus Damm");
+MODULE_DESCRIPTION("Renesas Emma Mobile 8250 Driver");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/tty/serial/8250/8250_mca.c b/drivers/tty/serial/8250/8250_mca.c
deleted file mode 100644
index d20abf04541..00000000000
--- a/drivers/tty/serial/8250/8250_mca.c
+++ /dev/null
@@ -1,61 +0,0 @@
-/*
- *  Copyright (C) 2005 Russell King.
- *  Data taken from include/asm-i386/serial.h
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License version 2 as
- * published by the Free Software Foundation.
- */
-#include <linux/module.h>
-#include <linux/init.h>
-#include <linux/mca.h>
-#include <linux/serial_8250.h>
-
-/*
- * FIXME: Should we be doing AUTO_IRQ here?
- */
-#ifdef CONFIG_SERIAL_8250_DETECT_IRQ
-#define MCA_FLAGS	UPF_BOOT_AUTOCONF | UPF_SKIP_TEST | UPF_AUTO_IRQ
-#else
-#define MCA_FLAGS	UPF_BOOT_AUTOCONF | UPF_SKIP_TEST
-#endif
-
-#define PORT(_base,_irq)			\
-	{					\
-		.iobase		= _base,	\
-		.irq		= _irq,		\
-		.uartclk	= 1843200,	\
-		.iotype		= UPIO_PORT,	\
-		.flags		= MCA_FLAGS,	\
-	}
-
-static struct plat_serial8250_port mca_data[] = {
-	PORT(0x3220, 3),
-	PORT(0x3228, 3),
-	PORT(0x4220, 3),
-	PORT(0x4228, 3),
-	PORT(0x5220, 3),
-	PORT(0x5228, 3),
-	{ },
-};
-
-static struct platform_device mca_device = {
-	.name			= "serial8250",
-	.id			= PLAT8250_DEV_MCA,
-	.dev			= {
-		.platform_data	= mca_data,
-	},
-};
-
-static int __init mca_init(void)
-{
-	if (!MCA_bus)
-		return -ENODEV;
-	return platform_device_register(&mca_device);
-}
-
-module_init(mca_init);
-
-MODULE_AUTHOR("Russell King");
-MODULE_DESCRIPTION("8250 serial probe module for MCA ports");
-MODULE_LICENSE("GPL");
diff --git a/drivers/tty/serial/8250/8250_pci.c b/drivers/tty/serial/8250/8250_pci.c
index 858dca865d6..28e7c7cce89 100644
--- a/drivers/tty/serial/8250/8250_pci.c
+++ b/drivers/tty/serial/8250/8250_pci.c
@@ -17,6 +17,7 @@
 #include <linux/slab.h>
 #include <linux/delay.h>
 #include <linux/tty.h>
+#include <linux/serial_reg.h>
 #include <linux/serial_core.h>
 #include <linux/8250_pci.h>
 #include <linux/bitops.h>
@@ -1092,11 +1093,49 @@ static int skip_tx_en_setup(struct serial_private *priv,
 	return pci_default_setup(priv, board, port, idx);
 }
 
+static void kt_handle_break(struct uart_port *p)
+{
+	struct uart_8250_port *up =
+		container_of(p, struct uart_8250_port, port);
+	/*
+	 * On receipt of a BI, serial device in Intel ME (Intel
+	 * management engine) needs to have its fifos cleared for sane
+	 * SOL (Serial Over Lan) output.
+	 */
+	serial8250_clear_and_reinit_fifos(up);
+}
+
+static unsigned int kt_serial_in(struct uart_port *p, int offset)
+{
+	struct uart_8250_port *up =
+		container_of(p, struct uart_8250_port, port);
+	unsigned int val;
+
+	/*
+	 * When the Intel ME (management engine) gets reset its serial
+	 * port registers could return 0 momentarily.  Functions like
+	 * serial8250_console_write, read and save the IER, perform
+	 * some operation and then restore it.  In order to avoid
+	 * setting IER register inadvertently to 0, if the value read
+	 * is 0, double check with ier value in uart_8250_port and use
+	 * that instead.  up->ier should be the same value as what is
+	 * currently configured.
+	 */
+	val = inb(p->iobase + offset);
+	if (offset == UART_IER) {
+		if (val == 0)
+			val = up->ier;
+	}
+	return val;
+}
+
 static int kt_serial_setup(struct serial_private *priv,
 			   const struct pciserial_board *board,
 			   struct uart_port *port, int idx)
 {
 	port->flags |= UPF_BUG_THRE;
+	port->serial_in = kt_serial_in;
+	port->handle_break = kt_handle_break;
 	return skip_tx_en_setup(priv, board, port, idx);
 }
 
@@ -1609,54 +1648,72 @@ static struct pci_serial_quirk pci_serial_quirks[] __refdata = {
 	{
 		.vendor         = PCI_VENDOR_ID_INTEL,
 		.device         = 0x8811,
+		.subvendor	= PCI_ANY_ID,
+		.subdevice	= PCI_ANY_ID,
 		.init		= pci_eg20t_init,
 		.setup		= pci_default_setup,
 	},
 	{
 		.vendor         = PCI_VENDOR_ID_INTEL,
 		.device         = 0x8812,
+		.subvendor	= PCI_ANY_ID,
+		.subdevice	= PCI_ANY_ID,
 		.init		= pci_eg20t_init,
 		.setup		= pci_default_setup,
 	},
 	{
 		.vendor         = PCI_VENDOR_ID_INTEL,
 		.device         = 0x8813,
+		.subvendor	= PCI_ANY_ID,
+		.subdevice	= PCI_ANY_ID,
 		.init		= pci_eg20t_init,
 		.setup		= pci_default_setup,
 	},
 	{
 		.vendor         = PCI_VENDOR_ID_INTEL,
 		.device         = 0x8814,
+		.subvendor	= PCI_ANY_ID,
+		.subdevice	= PCI_ANY_ID,
 		.init		= pci_eg20t_init,
 		.setup		= pci_default_setup,
 	},
 	{
 		.vendor         = 0x10DB,
 		.device         = 0x8027,
+		.subvendor	= PCI_ANY_ID,
+		.subdevice	= PCI_ANY_ID,
 		.init		= pci_eg20t_init,
 		.setup		= pci_default_setup,
 	},
 	{
 		.vendor         = 0x10DB,
 		.device         = 0x8028,
+		.subvendor	= PCI_ANY_ID,
+		.subdevice	= PCI_ANY_ID,
 		.init		= pci_eg20t_init,
 		.setup		= pci_default_setup,
 	},
 	{
 		.vendor         = 0x10DB,
 		.device         = 0x8029,
+		.subvendor	= PCI_ANY_ID,
+		.subdevice	= PCI_ANY_ID,
 		.init		= pci_eg20t_init,
 		.setup		= pci_default_setup,
 	},
 	{
 		.vendor         = 0x10DB,
 		.device         = 0x800C,
+		.subvendor	= PCI_ANY_ID,
+		.subdevice	= PCI_ANY_ID,
 		.init		= pci_eg20t_init,
 		.setup		= pci_default_setup,
 	},
 	{
 		.vendor         = 0x10DB,
 		.device         = 0x800D,
+		.subvendor	= PCI_ANY_ID,
+		.subdevice	= PCI_ANY_ID,
 		.init		= pci_eg20t_init,
 		.setup		= pci_default_setup,
 	},
@@ -2775,6 +2832,12 @@ void pciserial_suspend_ports(struct serial_private *priv)
 	for (i = 0; i < priv->nr; i++)
 		if (priv->line[i] >= 0)
 			serial8250_suspend_port(priv->line[i]);
+
+	/*
+	 * Ensure that every init quirk is properly torn down
+	 */
+	if (priv->quirk->exit)
+		priv->quirk->exit(priv->dev);
 }
 EXPORT_SYMBOL_GPL(pciserial_suspend_ports);
 
diff --git a/drivers/tty/serial/8250/Kconfig b/drivers/tty/serial/8250/Kconfig
index 591f8018e7d..a27dd0569bd 100644
--- a/drivers/tty/serial/8250/Kconfig
+++ b/drivers/tty/serial/8250/Kconfig
@@ -241,15 +241,6 @@ config SERIAL_8250_RSA
 	help
 	  ::: To be written :::
 
-config SERIAL_8250_MCA
-	tristate "Support 8250-type ports on MCA buses"
-	depends on SERIAL_8250 != n && MCA
-	help
-	  Say Y here if you have a MCA serial ports.
-
-	  To compile this driver as a module, choose M here: the module
-	  will be called 8250_mca.
-
 config SERIAL_8250_ACORN
 	tristate "Acorn expansion card serial port support"
 	depends on ARCH_ACORN && SERIAL_8250
@@ -278,3 +269,11 @@ config SERIAL_8250_DW
 	help
 	  Selecting this option will enable handling of the extra features
 	  present in the Synopsys DesignWare APB UART.
+
+config SERIAL_8250_EM
+	tristate "Support for Emma Mobile intergrated serial port"
+	depends on SERIAL_8250 && ARM && HAVE_CLK
+	help
+	  Selecting this option will add support for the integrated serial
+	  port hardware found on the Emma Mobile line of processors.
+	  If unsure, say N.
diff --git a/drivers/tty/serial/8250/Makefile b/drivers/tty/serial/8250/Makefile
index 867bba73890..d7533c7d2c1 100644
--- a/drivers/tty/serial/8250/Makefile
+++ b/drivers/tty/serial/8250/Makefile
@@ -15,6 +15,6 @@ obj-$(CONFIG_SERIAL_8250_ACCENT)	+= 8250_accent.o
 obj-$(CONFIG_SERIAL_8250_BOCA)		+= 8250_boca.o
 obj-$(CONFIG_SERIAL_8250_EXAR_ST16C554)	+= 8250_exar_st16c554.o
 obj-$(CONFIG_SERIAL_8250_HUB6)		+= 8250_hub6.o
-obj-$(CONFIG_SERIAL_8250_MCA)		+= 8250_mca.o
 obj-$(CONFIG_SERIAL_8250_FSL)		+= 8250_fsl.o
 obj-$(CONFIG_SERIAL_8250_DW)		+= 8250_dw.o
+obj-$(CONFIG_SERIAL_8250_EM)		+= 8250_em.o
diff --git a/drivers/tty/serial/amba-pl011.c b/drivers/tty/serial/amba-pl011.c
index 3d569cd68f5..4ad721fb840 100644
--- a/drivers/tty/serial/amba-pl011.c
+++ b/drivers/tty/serial/amba-pl011.c
@@ -52,6 +52,7 @@
 #include <linux/scatterlist.h>
 #include <linux/delay.h>
 #include <linux/types.h>
+#include <linux/pinctrl/consumer.h>
 
 #include <asm/io.h>
 #include <asm/sizes.h>
@@ -67,30 +68,6 @@
 #define UART_DR_ERROR		(UART011_DR_OE|UART011_DR_BE|UART011_DR_PE|UART011_DR_FE)
 #define UART_DUMMY_DR_RX	(1 << 16)
 
-
-#define UART_WA_SAVE_NR 14
-
-static void pl011_lockup_wa(unsigned long data);
-static const u32 uart_wa_reg[UART_WA_SAVE_NR] = {
-	ST_UART011_DMAWM,
-	ST_UART011_TIMEOUT,
-	ST_UART011_LCRH_RX,
-	UART011_IBRD,
-	UART011_FBRD,
-	ST_UART011_LCRH_TX,
-	UART011_IFLS,
-	ST_UART011_XFCR,
-	ST_UART011_XON1,
-	ST_UART011_XON2,
-	ST_UART011_XOFF1,
-	ST_UART011_XOFF2,
-	UART011_CR,
-	UART011_IMSC
-};
-
-static u32 uart_wa_regdata[UART_WA_SAVE_NR];
-static DECLARE_TASKLET(pl011_lockup_tlet, pl011_lockup_wa, 0);
-
 /* There is by now at least one vendor with differing details, so handle it */
 struct vendor_data {
 	unsigned int		ifls;
@@ -100,6 +77,7 @@ struct vendor_data {
 	bool			oversampling;
 	bool			interrupt_may_hang;   /* vendor-specific */
 	bool			dma_threshold;
+	bool			cts_event_workaround;
 };
 
 static struct vendor_data vendor_arm = {
@@ -109,6 +87,7 @@ static struct vendor_data vendor_arm = {
 	.lcrh_rx		= UART011_LCRH,
 	.oversampling		= false,
 	.dma_threshold		= false,
+	.cts_event_workaround	= false,
 };
 
 static struct vendor_data vendor_st = {
@@ -119,6 +98,7 @@ static struct vendor_data vendor_st = {
 	.oversampling		= true,
 	.interrupt_may_hang	= true,
 	.dma_threshold		= true,
+	.cts_event_workaround	= true,
 };
 
 static struct uart_amba_port *amba_ports[UART_NR];
@@ -1054,69 +1034,6 @@ static inline bool pl011_dma_rx_running(struct uart_amba_port *uap)
 #define pl011_dma_flush_buffer	NULL
 #endif
 
-
-/*
- * pl011_lockup_wa
- * This workaround aims to break the deadlock situation
- * when after long transfer over uart in hardware flow
- * control, uart interrupt registers cannot be cleared.
- * Hence uart transfer gets blocked.
- *
- * It is seen that during such deadlock condition ICR
- * don't get cleared even on multiple write. This leads
- * pass_counter to decrease and finally reach zero. This
- * can be taken as trigger point to run this UART_BT_WA.
- *
- */
-static void pl011_lockup_wa(unsigned long data)
-{
-	struct uart_amba_port *uap = amba_ports[0];
-	void __iomem *base = uap->port.membase;
-	struct circ_buf *xmit = &uap->port.state->xmit;
-	struct tty_struct *tty = uap->port.state->port.tty;
-	int buf_empty_retries = 200;
-	int loop;
-
-	/* Stop HCI layer from submitting data for tx */
-	tty->hw_stopped = 1;
-	while (!uart_circ_empty(xmit)) {
-		if (buf_empty_retries-- == 0)
-			break;
-		udelay(100);
-	}
-
-	/* Backup registers */
-	for (loop = 0; loop < UART_WA_SAVE_NR; loop++)
-		uart_wa_regdata[loop] = readl(base + uart_wa_reg[loop]);
-
-	/* Disable UART so that FIFO data is flushed out */
-	writew(0x00, uap->port.membase + UART011_CR);
-
-	/* Soft reset UART module */
-	if (uap->port.dev->platform_data) {
-		struct amba_pl011_data *plat;
-
-		plat = uap->port.dev->platform_data;
-		if (plat->reset)
-			plat->reset();
-	}
-
-	/* Restore registers */
-	for (loop = 0; loop < UART_WA_SAVE_NR; loop++)
-		writew(uart_wa_regdata[loop] ,
-				uap->port.membase + uart_wa_reg[loop]);
-
-	/* Initialise the old status of the modem signals */
-	uap->old_status = readw(uap->port.membase + UART01x_FR) &
-		UART01x_FR_MODEM_ANY;
-
-	if (readl(base + UART011_MIS) & 0x2)
-		printk(KERN_EMERG "UART_BT_WA: ***FAILED***\n");
-
-	/* Start Tx/Rx */
-	tty->hw_stopped = 0;
-}
-
 static void pl011_stop_tx(struct uart_port *port)
 {
 	struct uart_amba_port *uap = (struct uart_amba_port *)port;
@@ -1245,12 +1162,26 @@ static irqreturn_t pl011_int(int irq, void *dev_id)
 	unsigned long flags;
 	unsigned int status, pass_counter = AMBA_ISR_PASS_LIMIT;
 	int handled = 0;
+	unsigned int dummy_read;
 
 	spin_lock_irqsave(&uap->port.lock, flags);
 
 	status = readw(uap->port.membase + UART011_MIS);
 	if (status) {
 		do {
+			if (uap->vendor->cts_event_workaround) {
+				/* workaround to make sure that all bits are unlocked.. */
+				writew(0x00, uap->port.membase + UART011_ICR);
+
+				/*
+				 * WA: introduce 26ns(1 uart clk) delay before W1C;
+				 * single apb access will incur 2 pclk(133.12Mhz) delay,
+				 * so add 2 dummy reads
+				 */
+				dummy_read = readw(uap->port.membase + UART011_ICR);
+				dummy_read = readw(uap->port.membase + UART011_ICR);
+			}
+
 			writew(status & ~(UART011_TXIS|UART011_RTIS|
 					  UART011_RXIS),
 			       uap->port.membase + UART011_ICR);
@@ -1267,11 +1198,8 @@ static irqreturn_t pl011_int(int irq, void *dev_id)
 			if (status & UART011_TXIS)
 				pl011_tx_chars(uap);
 
-			if (pass_counter-- == 0) {
-				if (uap->interrupt_may_hang)
-					tasklet_schedule(&pl011_lockup_tlet);
+			if (pass_counter-- == 0)
 				break;
-			}
 
 			status = readw(uap->port.membase + UART011_MIS);
 		} while (status != 0);
@@ -1916,6 +1844,7 @@ static int pl011_probe(struct amba_device *dev, const struct amba_id *id)
 {
 	struct uart_amba_port *uap;
 	struct vendor_data *vendor = id->data;
+	struct pinctrl *pinctrl;
 	void __iomem *base;
 	int i, ret;
 
@@ -1940,6 +1869,12 @@ static int pl011_probe(struct amba_device *dev, const struct amba_id *id)
 		goto free;
 	}
 
+	pinctrl = devm_pinctrl_get_select_default(&dev->dev);
+	if (IS_ERR(pinctrl)) {
+		ret = PTR_ERR(pinctrl);
+		goto unmap;
+	}
+
 	uap->clk = clk_get(&dev->dev, NULL);
 	if (IS_ERR(uap->clk)) {
 		ret = PTR_ERR(uap->clk);
diff --git a/drivers/tty/serial/bfin_uart.c b/drivers/tty/serial/bfin_uart.c
index 5832fdef11e..bd97db23985 100644
--- a/drivers/tty/serial/bfin_uart.c
+++ b/drivers/tty/serial/bfin_uart.c
@@ -1,7 +1,7 @@
 /*
  * Blackfin On-Chip Serial Driver
  *
- * Copyright 2006-2010 Analog Devices Inc.
+ * Copyright 2006-2011 Analog Devices Inc.
  *
  * Enter bugs at http://blackfin.uclinux.org/
  *
@@ -35,10 +35,6 @@
 #include <asm/portmux.h>
 #include <asm/cacheflush.h>
 #include <asm/dma.h>
-
-#define port_membase(uart)     (((struct bfin_serial_port *)(uart))->port.membase)
-#define get_lsr_cache(uart)    (((struct bfin_serial_port *)(uart))->lsr)
-#define put_lsr_cache(uart, v) (((struct bfin_serial_port *)(uart))->lsr = (v))
 #include <asm/bfin_serial.h>
 
 #ifdef CONFIG_SERIAL_BFIN_MODULE
@@ -166,7 +162,7 @@ static void bfin_serial_stop_tx(struct uart_port *port)
 	uart->tx_count = 0;
 	uart->tx_done = 1;
 #else
-#ifdef CONFIG_BF54x
+#if defined(CONFIG_BF54x) || defined(CONFIG_BF60x)
 	/* Clear TFI bit */
 	UART_PUT_LSR(uart, TFI);
 #endif
@@ -337,7 +333,7 @@ static void bfin_serial_tx_chars(struct bfin_serial_port *uart)
 	struct circ_buf *xmit = &uart->port.state->xmit;
 
 	if (uart_circ_empty(xmit) || uart_tx_stopped(&uart->port)) {
-#ifdef CONFIG_BF54x
+#if defined(CONFIG_BF54x) || defined(CONFIG_BF60x)
 		/* Clear TFI bit */
 		UART_PUT_LSR(uart, TFI);
 #endif
@@ -536,7 +532,7 @@ static irqreturn_t bfin_serial_dma_tx_int(int irq, void *dev_id)
 		 */
 		UART_CLEAR_IER(uart, ETBEI);
 		uart->port.icount.tx += uart->tx_count;
-		if (!uart_circ_empty(xmit)) {
+		if (!(xmit->tail == 0 && xmit->head == 0)) {
 			xmit->tail = (xmit->tail + uart->tx_count) & (UART_XMIT_SIZE - 1);
 
 			if (uart_circ_chars_pending(xmit) < WAKEUP_CHARS)
@@ -553,7 +549,7 @@ static irqreturn_t bfin_serial_dma_tx_int(int irq, void *dev_id)
 static irqreturn_t bfin_serial_dma_rx_int(int irq, void *dev_id)
 {
 	struct bfin_serial_port *uart = dev_id;
-	unsigned short irqstat;
+	unsigned int irqstat;
 	int x_pos, pos;
 
 	spin_lock(&uart->rx_lock);
@@ -586,7 +582,7 @@ static irqreturn_t bfin_serial_dma_rx_int(int irq, void *dev_id)
 static unsigned int bfin_serial_tx_empty(struct uart_port *port)
 {
 	struct bfin_serial_port *uart = (struct bfin_serial_port *)port;
-	unsigned short lsr;
+	unsigned int lsr;
 
 	lsr = UART_GET_LSR(uart);
 	if (lsr & TEMT)
@@ -598,7 +594,7 @@ static unsigned int bfin_serial_tx_empty(struct uart_port *port)
 static void bfin_serial_break_ctl(struct uart_port *port, int break_state)
 {
 	struct bfin_serial_port *uart = (struct bfin_serial_port *)port;
-	u16 lcr = UART_GET_LCR(uart);
+	u32 lcr = UART_GET_LCR(uart);
 	if (break_state)
 		lcr |= SB;
 	else
@@ -745,7 +741,7 @@ static int bfin_serial_startup(struct uart_port *port)
 		}
 
 		/* CTS RTS PINs are negative assertive. */
-		UART_PUT_MCR(uart, ACTS);
+		UART_PUT_MCR(uart, UART_GET_MCR(uart) | ACTS);
 		UART_SET_IER(uart, EDSSI);
 	}
 #endif
@@ -803,7 +799,7 @@ bfin_serial_set_termios(struct uart_port *port, struct ktermios *termios,
 	struct bfin_serial_port *uart = (struct bfin_serial_port *)port;
 	unsigned long flags;
 	unsigned int baud, quot;
-	unsigned short val, ier, lcr = 0;
+	unsigned int ier, lcr = 0;
 
 	switch (termios->c_cflag & CSIZE) {
 	case CS8:
@@ -875,26 +871,23 @@ bfin_serial_set_termios(struct uart_port *port, struct ktermios *termios,
 
 	/* Disable UART */
 	ier = UART_GET_IER(uart);
+	UART_PUT_GCTL(uart, UART_GET_GCTL(uart) & ~UCEN);
 	UART_DISABLE_INTS(uart);
 
-	/* Set DLAB in LCR to Access DLL and DLH */
+	/* Set DLAB in LCR to Access CLK */
 	UART_SET_DLAB(uart);
 
-	UART_PUT_DLL(uart, quot & 0xFF);
-	UART_PUT_DLH(uart, (quot >> 8) & 0xFF);
+	UART_PUT_CLK(uart, quot);
 	SSYNC();
 
 	/* Clear DLAB in LCR to Access THR RBR IER */
 	UART_CLEAR_DLAB(uart);
 
-	UART_PUT_LCR(uart, lcr);
+	UART_PUT_LCR(uart, (UART_GET_LCR(uart) & ~LCR_MASK) | lcr);
 
 	/* Enable UART */
 	UART_ENABLE_INTS(uart, ier);
-
-	val = UART_GET_GCTL(uart);
-	val |= UCEN;
-	UART_PUT_GCTL(uart, val);
+	UART_PUT_GCTL(uart, UART_GET_GCTL(uart) | UCEN);
 
 	/* Port speed changed, update the per-port timeout. */
 	uart_update_timeout(port, termios->c_cflag, baud);
@@ -954,17 +947,17 @@ bfin_serial_verify_port(struct uart_port *port, struct serial_struct *ser)
 static void bfin_serial_set_ldisc(struct uart_port *port, int ld)
 {
 	struct bfin_serial_port *uart = (struct bfin_serial_port *)port;
-	unsigned short val;
+	unsigned int val;
 
 	switch (ld) {
 	case N_IRDA:
 		val = UART_GET_GCTL(uart);
-		val |= (IREN | RPOLC);
+		val |= (UMOD_IRDA | RPOLC);
 		UART_PUT_GCTL(uart, val);
 		break;
 	default:
 		val = UART_GET_GCTL(uart);
-		val &= ~(IREN | RPOLC);
+		val &= ~(UMOD_MASK | RPOLC);
 		UART_PUT_GCTL(uart, val);
 	}
 }
@@ -972,13 +965,13 @@ static void bfin_serial_set_ldisc(struct uart_port *port, int ld)
 static void bfin_serial_reset_irda(struct uart_port *port)
 {
 	struct bfin_serial_port *uart = (struct bfin_serial_port *)port;
-	unsigned short val;
+	unsigned int val;
 
 	val = UART_GET_GCTL(uart);
-	val &= ~(IREN | RPOLC);
+	val &= ~(UMOD_MASK | RPOLC);
 	UART_PUT_GCTL(uart, val);
 	SSYNC();
-	val |= (IREN | RPOLC);
+	val |= (UMOD_IRDA | RPOLC);
 	UART_PUT_GCTL(uart, val);
 	SSYNC();
 }
@@ -1070,12 +1063,12 @@ static void __init
 bfin_serial_console_get_options(struct bfin_serial_port *uart, int *baud,
 			   int *parity, int *bits)
 {
-	unsigned short status;
+	unsigned int status;
 
 	status = UART_GET_IER(uart) & (ERBFI | ETBEI);
 	if (status == (ERBFI | ETBEI)) {
 		/* ok, the port was enabled */
-		u16 lcr, dlh, dll;
+		u32 lcr, clk;
 
 		lcr = UART_GET_LCR(uart);
 
@@ -1086,30 +1079,17 @@ bfin_serial_console_get_options(struct bfin_serial_port *uart, int *baud,
 			else
 				*parity = 'o';
 		}
-		switch (lcr & 0x03) {
-		case 0:
-			*bits = 5;
-			break;
-		case 1:
-			*bits = 6;
-			break;
-		case 2:
-			*bits = 7;
-			break;
-		case 3:
-			*bits = 8;
-			break;
-		}
-		/* Set DLAB in LCR to Access DLL and DLH */
+		*bits = ((lcr & WLS_MASK) >> WLS_OFFSET) + 5;
+
+		/* Set DLAB in LCR to Access CLK */
 		UART_SET_DLAB(uart);
 
-		dll = UART_GET_DLL(uart);
-		dlh = UART_GET_DLH(uart);
+		clk = UART_GET_CLK(uart);
 
 		/* Clear DLAB in LCR to Access THR RBR IER */
 		UART_CLEAR_DLAB(uart);
 
-		*baud = get_sclk() / (16*(dll | dlh << 8));
+		*baud = get_sclk() / (16*clk);
 	}
 	pr_debug("%s:baud = %d, parity = %c, bits= %d\n", __func__, *baud, *parity, *bits);
 }
diff --git a/drivers/tty/serial/clps711x.c b/drivers/tty/serial/clps711x.c
index 836fe273123..d0f719fafc8 100644
--- a/drivers/tty/serial/clps711x.c
+++ b/drivers/tty/serial/clps711x.c
@@ -40,7 +40,6 @@
 
 #include <mach/hardware.h>
 #include <asm/irq.h>
-#include <asm/hardware/clps7111.h>
 
 #define UART_NR		2
 
diff --git a/drivers/tty/serial/crisv10.c b/drivers/tty/serial/crisv10.c
index 5b07c0c3a10..7264d4d2671 100644
--- a/drivers/tty/serial/crisv10.c
+++ b/drivers/tty/serial/crisv10.c
@@ -952,19 +952,6 @@ static const struct control_pins e100_modem_pins[NR_PORTS] =
 /* Input */
 #define E100_DSR_GET(info) ((*e100_modem_pins[(info)->line].dsr_port) & e100_modem_pins[(info)->line].dsr_mask)
 
-
-/*
- * tmp_buf is used as a temporary buffer by serial_write.  We need to
- * lock it in case the memcpy_fromfs blocks while swapping in a page,
- * and some other program tries to do a serial write at the same time.
- * Since the lock will only come under contention when the system is
- * swapping and available memory is low, it makes sense to share one
- * buffer across all the serial ports, since it significantly saves
- * memory if large numbers of serial ports are open.
- */
-static unsigned char *tmp_buf;
-static DEFINE_MUTEX(tmp_buf_mutex);
-
 /* Calculate the chartime depending on baudrate, numbor of bits etc. */
 static void update_char_time(struct e100_serial * info)
 {
@@ -3150,7 +3137,7 @@ static int rs_raw_write(struct tty_struct *tty,
 
 	/* first some sanity checks */
 
-	if (!tty || !info->xmit.buf || !tmp_buf)
+	if (!tty || !info->xmit.buf)
 		return 0;
 
 #ifdef SERIAL_DEBUG_DATA
@@ -3989,7 +3976,7 @@ block_til_ready(struct tty_struct *tty, struct file * filp,
 	 */
 	if (tty_hung_up_p(filp) ||
 	    (info->flags & ASYNC_CLOSING)) {
-		wait_event_interruptible_tty(info->close_wait,
+		wait_event_interruptible_tty(tty, info->close_wait,
 			!(info->flags & ASYNC_CLOSING));
 #ifdef SERIAL_DO_RESTART
 		if (info->flags & ASYNC_HUP_NOTIFY)
@@ -4065,9 +4052,9 @@ block_til_ready(struct tty_struct *tty, struct file * filp,
 		printk("block_til_ready blocking: ttyS%d, count = %d\n",
 		       info->line, info->count);
 #endif
-		tty_unlock();
+		tty_unlock(tty);
 		schedule();
-		tty_lock();
+		tty_lock(tty);
 	}
 	set_current_state(TASK_RUNNING);
 	remove_wait_queue(&info->open_wait, &wait);
@@ -4106,7 +4093,6 @@ rs_open(struct tty_struct *tty, struct file * filp)
 {
 	struct e100_serial	*info;
 	int 			retval;
-	unsigned long           page;
 	int                     allocated_resources = 0;
 
 	info = rs_table + tty->index;
@@ -4124,23 +4110,12 @@ rs_open(struct tty_struct *tty, struct file * filp)
 
 	tty->low_latency = !!(info->flags & ASYNC_LOW_LATENCY);
 
-	if (!tmp_buf) {
-		page = get_zeroed_page(GFP_KERNEL);
-		if (!page) {
-			return -ENOMEM;
-		}
-		if (tmp_buf)
-			free_page(page);
-		else
-			tmp_buf = (unsigned char *) page;
-	}
-
 	/*
 	 * If the port is in the middle of closing, bail out now
 	 */
 	if (tty_hung_up_p(filp) ||
 	    (info->flags & ASYNC_CLOSING)) {
-		wait_event_interruptible_tty(info->close_wait,
+		wait_event_interruptible_tty(tty, info->close_wait,
 			!(info->flags & ASYNC_CLOSING));
 #ifdef SERIAL_DO_RESTART
 		return ((info->flags & ASYNC_HUP_NOTIFY) ?
@@ -4487,6 +4462,7 @@ static int __init rs_init(void)
 				info->enabled = 0;
 			}
 		}
+		tty_port_init(&info->port);
 		info->uses_dma_in = 0;
 		info->uses_dma_out = 0;
 		info->line = i;
diff --git a/drivers/tty/serial/imx.c b/drivers/tty/serial/imx.c
index e7feceeebc2..4ef747307ec 100644
--- a/drivers/tty/serial/imx.c
+++ b/drivers/tty/serial/imx.c
@@ -47,6 +47,7 @@
 #include <linux/slab.h>
 #include <linux/of.h>
 #include <linux/of_device.h>
+#include <linux/pinctrl/consumer.h>
 
 #include <asm/io.h>
 #include <asm/irq.h>
@@ -204,7 +205,8 @@ struct imx_port {
 	unsigned int		irda_inv_rx:1;
 	unsigned int		irda_inv_tx:1;
 	unsigned short		trcv_delay; /* transceiver delay */
-	struct clk		*clk;
+	struct clk		*clk_ipg;
+	struct clk		*clk_per;
 	struct imx_uart_data	*devdata;
 };
 
@@ -672,7 +674,7 @@ static int imx_setup_ufcr(struct imx_port *sport, unsigned int mode)
 	 * RFDIV is set such way to satisfy requested uartclk value
 	 */
 	val = TXTL << 10 | RXTL;
-	ufcr_rfdiv = (clk_get_rate(sport->clk) + sport->port.uartclk / 2)
+	ufcr_rfdiv = (clk_get_rate(sport->clk_per) + sport->port.uartclk / 2)
 			/ sport->port.uartclk;
 
 	if(!ufcr_rfdiv)
@@ -1285,7 +1287,7 @@ imx_console_get_options(struct imx_port *sport, int *baud,
 		else
 			ucfr_rfdiv = 6 - ucfr_rfdiv;
 
-		uartclk = clk_get_rate(sport->clk);
+		uartclk = clk_get_rate(sport->clk_per);
 		uartclk /= ucfr_rfdiv;
 
 		{	/*
@@ -1464,6 +1466,7 @@ static int serial_imx_probe(struct platform_device *pdev)
 	void __iomem *base;
 	int ret = 0;
 	struct resource *res;
+	struct pinctrl *pinctrl;
 
 	sport = kzalloc(sizeof(*sport), GFP_KERNEL);
 	if (!sport)
@@ -1503,14 +1506,28 @@ static int serial_imx_probe(struct platform_device *pdev)
 	sport->timer.function = imx_timeout;
 	sport->timer.data     = (unsigned long)sport;
 
-	sport->clk = clk_get(&pdev->dev, "uart");
-	if (IS_ERR(sport->clk)) {
-		ret = PTR_ERR(sport->clk);
+	pinctrl = devm_pinctrl_get_select_default(&pdev->dev);
+	if (IS_ERR(pinctrl)) {
+		ret = PTR_ERR(pinctrl);
 		goto unmap;
 	}
-	clk_prepare_enable(sport->clk);
 
-	sport->port.uartclk = clk_get_rate(sport->clk);
+	sport->clk_ipg = devm_clk_get(&pdev->dev, "ipg");
+	if (IS_ERR(sport->clk_ipg)) {
+		ret = PTR_ERR(sport->clk_ipg);
+		goto unmap;
+	}
+
+	sport->clk_per = devm_clk_get(&pdev->dev, "per");
+	if (IS_ERR(sport->clk_per)) {
+		ret = PTR_ERR(sport->clk_per);
+		goto unmap;
+	}
+
+	clk_prepare_enable(sport->clk_per);
+	clk_prepare_enable(sport->clk_ipg);
+
+	sport->port.uartclk = clk_get_rate(sport->clk_per);
 
 	imx_ports[sport->port.line] = sport;
 
@@ -1531,8 +1548,8 @@ deinit:
 	if (pdata && pdata->exit)
 		pdata->exit(pdev);
 clkput:
-	clk_disable_unprepare(sport->clk);
-	clk_put(sport->clk);
+	clk_disable_unprepare(sport->clk_per);
+	clk_disable_unprepare(sport->clk_ipg);
 unmap:
 	iounmap(sport->port.membase);
 free:
@@ -1550,11 +1567,10 @@ static int serial_imx_remove(struct platform_device *pdev)
 
 	platform_set_drvdata(pdev, NULL);
 
-	if (sport) {
-		uart_remove_one_port(&imx_reg, &sport->port);
-		clk_disable_unprepare(sport->clk);
-		clk_put(sport->clk);
-	}
+	uart_remove_one_port(&imx_reg, &sport->port);
+
+	clk_disable_unprepare(sport->clk_per);
+	clk_disable_unprepare(sport->clk_ipg);
 
 	if (pdata && pdata->exit)
 		pdata->exit(pdev);
diff --git a/drivers/tty/serial/lantiq.c b/drivers/tty/serial/lantiq.c
index 96c1cacc736..02da071fe1e 100644
--- a/drivers/tty/serial/lantiq.c
+++ b/drivers/tty/serial/lantiq.c
@@ -31,16 +31,19 @@
 #include <linux/tty_flip.h>
 #include <linux/serial_core.h>
 #include <linux/serial.h>
-#include <linux/platform_device.h>
+#include <linux/of_platform.h>
+#include <linux/of_address.h>
+#include <linux/of_irq.h>
 #include <linux/io.h>
 #include <linux/clk.h>
+#include <linux/gpio.h>
 
 #include <lantiq_soc.h>
 
 #define PORT_LTQ_ASC		111
 #define MAXPORTS		2
 #define UART_DUMMY_UER_RX	1
-#define DRVNAME			"ltq_asc"
+#define DRVNAME			"lantiq,asc"
 #ifdef __BIG_ENDIAN
 #define LTQ_ASC_TBUF		(0x0020 + 3)
 #define LTQ_ASC_RBUF		(0x0024 + 3)
@@ -114,6 +117,9 @@ static DEFINE_SPINLOCK(ltq_asc_lock);
 
 struct ltq_uart_port {
 	struct uart_port	port;
+	/* clock used to derive divider */
+	struct clk		*fpiclk;
+	/* clock gating of the ASC core */
 	struct clk		*clk;
 	unsigned int		tx_irq;
 	unsigned int		rx_irq;
@@ -316,7 +322,9 @@ lqasc_startup(struct uart_port *port)
 	struct ltq_uart_port *ltq_port = to_ltq_uart_port(port);
 	int retval;
 
-	port->uartclk = clk_get_rate(ltq_port->clk);
+	if (ltq_port->clk)
+		clk_enable(ltq_port->clk);
+	port->uartclk = clk_get_rate(ltq_port->fpiclk);
 
 	ltq_w32_mask(ASCCLC_DISS | ASCCLC_RMCMASK, (1 << ASCCLC_RMCOFFSET),
 		port->membase + LTQ_ASC_CLC);
@@ -382,6 +390,8 @@ lqasc_shutdown(struct uart_port *port)
 		port->membase + LTQ_ASC_RXFCON);
 	ltq_w32_mask(ASCTXFCON_TXFEN, ASCTXFCON_TXFFLU,
 		port->membase + LTQ_ASC_TXFCON);
+	if (ltq_port->clk)
+		clk_disable(ltq_port->clk);
 }
 
 static void
@@ -630,7 +640,7 @@ lqasc_console_setup(struct console *co, char *options)
 
 	port = &ltq_port->port;
 
-	port->uartclk = clk_get_rate(ltq_port->clk);
+	port->uartclk = clk_get_rate(ltq_port->fpiclk);
 
 	if (options)
 		uart_parse_options(options, &baud, &parity, &bits, &flow);
@@ -668,37 +678,32 @@ static struct uart_driver lqasc_reg = {
 static int __init
 lqasc_probe(struct platform_device *pdev)
 {
+	struct device_node *node = pdev->dev.of_node;
 	struct ltq_uart_port *ltq_port;
 	struct uart_port *port;
-	struct resource *mmres, *irqres;
-	int tx_irq, rx_irq, err_irq;
-	struct clk *clk;
+	struct resource *mmres, irqres[3];
+	int line = 0;
 	int ret;
 
 	mmres = platform_get_resource(pdev, IORESOURCE_MEM, 0);
-	irqres = platform_get_resource(pdev, IORESOURCE_IRQ, 0);
-	if (!mmres || !irqres)
+	ret = of_irq_to_resource_table(node, irqres, 3);
+	if (!mmres || (ret != 3)) {
+		dev_err(&pdev->dev,
+			"failed to get memory/irq for serial port\n");
 		return -ENODEV;
+	}
 
-	if (pdev->id >= MAXPORTS)
-		return -EBUSY;
+	/* check if this is the console port */
+	if (mmres->start != CPHYSADDR(LTQ_EARLY_ASC))
+		line = 1;
 
-	if (lqasc_port[pdev->id] != NULL)
+	if (lqasc_port[line]) {
+		dev_err(&pdev->dev, "port %d already allocated\n", line);
 		return -EBUSY;
-
-	clk = clk_get(&pdev->dev, "fpi");
-	if (IS_ERR(clk)) {
-		pr_err("failed to get fpi clk\n");
-		return -ENOENT;
 	}
 
-	tx_irq = platform_get_irq_byname(pdev, "tx");
-	rx_irq = platform_get_irq_byname(pdev, "rx");
-	err_irq = platform_get_irq_byname(pdev, "err");
-	if ((tx_irq < 0) | (rx_irq < 0) | (err_irq < 0))
-		return -ENODEV;
-
-	ltq_port = kzalloc(sizeof(struct ltq_uart_port), GFP_KERNEL);
+	ltq_port = devm_kzalloc(&pdev->dev, sizeof(struct ltq_uart_port),
+			GFP_KERNEL);
 	if (!ltq_port)
 		return -ENOMEM;
 
@@ -709,19 +714,26 @@ lqasc_probe(struct platform_device *pdev)
 	port->ops	= &lqasc_pops;
 	port->fifosize	= 16;
 	port->type	= PORT_LTQ_ASC,
-	port->line	= pdev->id;
+	port->line	= line;
 	port->dev	= &pdev->dev;
-
-	port->irq	= tx_irq; /* unused, just to be backward-compatibe */
+	/* unused, just to be backward-compatible */
+	port->irq	= irqres[0].start;
 	port->mapbase	= mmres->start;
 
-	ltq_port->clk	= clk;
+	ltq_port->fpiclk = clk_get_fpi();
+	if (IS_ERR(ltq_port->fpiclk)) {
+		pr_err("failed to get fpi clk\n");
+		return -ENOENT;
+	}
 
-	ltq_port->tx_irq = tx_irq;
-	ltq_port->rx_irq = rx_irq;
-	ltq_port->err_irq = err_irq;
+	/* not all asc ports have clock gates, lets ignore the return code */
+	ltq_port->clk = clk_get(&pdev->dev, NULL);
 
-	lqasc_port[pdev->id] = ltq_port;
+	ltq_port->tx_irq = irqres[0].start;
+	ltq_port->rx_irq = irqres[1].start;
+	ltq_port->err_irq = irqres[2].start;
+
+	lqasc_port[line] = ltq_port;
 	platform_set_drvdata(pdev, ltq_port);
 
 	ret = uart_add_one_port(&lqasc_reg, port);
@@ -729,10 +741,17 @@ lqasc_probe(struct platform_device *pdev)
 	return ret;
 }
 
+static const struct of_device_id ltq_asc_match[] = {
+	{ .compatible = DRVNAME },
+	{},
+};
+MODULE_DEVICE_TABLE(of, ltq_asc_match);
+
 static struct platform_driver lqasc_driver = {
 	.driver		= {
 		.name	= DRVNAME,
 		.owner	= THIS_MODULE,
+		.of_match_table = ltq_asc_match,
 	},
 };
 
diff --git a/drivers/tty/serial/mxs-auart.c b/drivers/tty/serial/mxs-auart.c
index 55fd362b987..ec56d8397aa 100644
--- a/drivers/tty/serial/mxs-auart.c
+++ b/drivers/tty/serial/mxs-auart.c
@@ -32,6 +32,7 @@
 #include <linux/clk.h>
 #include <linux/delay.h>
 #include <linux/io.h>
+#include <linux/pinctrl/consumer.h>
 
 #include <asm/cacheflush.h>
 
@@ -369,6 +370,8 @@ static void mxs_auart_settermios(struct uart_port *u,
 
 	writel(ctrl, u->membase + AUART_LINECTRL);
 	writel(ctrl2, u->membase + AUART_CTRL2);
+
+	uart_update_timeout(u, termios->c_cflag, baud);
 }
 
 static irqreturn_t mxs_auart_irq_handle(int irq, void *context)
@@ -678,6 +681,7 @@ static int __devinit mxs_auart_probe(struct platform_device *pdev)
 	u32 version;
 	int ret = 0;
 	struct resource *r;
+	struct pinctrl *pinctrl;
 
 	s = kzalloc(sizeof(struct mxs_auart_port), GFP_KERNEL);
 	if (!s) {
@@ -685,6 +689,12 @@ static int __devinit mxs_auart_probe(struct platform_device *pdev)
 		goto out;
 	}
 
+	pinctrl = devm_pinctrl_get_select_default(&pdev->dev);
+	if (IS_ERR(pinctrl)) {
+		ret = PTR_ERR(pinctrl);
+		goto out_free;
+	}
+
 	s->clk = clk_get(&pdev->dev, NULL);
 	if (IS_ERR(s->clk)) {
 		ret = PTR_ERR(s->clk);
diff --git a/drivers/tty/serial/of_serial.c b/drivers/tty/serial/of_serial.c
index e8c9cee07d0..5410c063726 100644
--- a/drivers/tty/serial/of_serial.c
+++ b/drivers/tty/serial/of_serial.c
@@ -12,10 +12,13 @@
 #include <linux/init.h>
 #include <linux/module.h>
 #include <linux/slab.h>
+#include <linux/delay.h>
 #include <linux/serial_core.h>
 #include <linux/serial_8250.h>
+#include <linux/serial_reg.h>
 #include <linux/of_address.h>
 #include <linux/of_irq.h>
+#include <linux/of_serial.h>
 #include <linux/of_platform.h>
 #include <linux/nwpserial.h>
 
@@ -24,6 +27,26 @@ struct of_serial_info {
 	int line;
 };
 
+#ifdef CONFIG_ARCH_TEGRA
+void tegra_serial_handle_break(struct uart_port *p)
+{
+	unsigned int status, tmout = 10000;
+
+	do {
+		status = p->serial_in(p, UART_LSR);
+		if (status & (UART_LSR_FIFOE | UART_LSR_BRK_ERROR_BITS))
+			status = p->serial_in(p, UART_RX);
+		else
+			break;
+		if (--tmout == 0)
+			break;
+		udelay(1);
+	} while (1);
+}
+/* FIXME remove this export when tegra finishes conversion to open firmware */
+EXPORT_SYMBOL_GPL(tegra_serial_handle_break);
+#endif
+
 /*
  * Fill a struct uart_port for a given device node
  */
@@ -84,6 +107,9 @@ static int __devinit of_platform_serial_setup(struct platform_device *ofdev,
 		| UPF_FIXED_PORT | UPF_FIXED_TYPE;
 	port->dev = &ofdev->dev;
 
+	if (type == PORT_TEGRA)
+		port->handle_break = tegra_serial_handle_break;
+
 	return 0;
 }
 
diff --git a/drivers/tty/serial/omap-serial.c b/drivers/tty/serial/omap-serial.c
index d00b38eb268..d3cda0cb2df 100644
--- a/drivers/tty/serial/omap-serial.c
+++ b/drivers/tty/serial/omap-serial.c
@@ -44,6 +44,13 @@
 #include <plat/dmtimer.h>
 #include <plat/omap-serial.h>
 
+#define UART_BUILD_REVISION(x, y)	(((x) << 8) | (y))
+
+#define OMAP_UART_REV_42 0x0402
+#define OMAP_UART_REV_46 0x0406
+#define OMAP_UART_REV_52 0x0502
+#define OMAP_UART_REV_63 0x0603
+
 #define DEFAULT_CLK_SPEED 48000000 /* 48Mhz*/
 
 /* SCR register bitmasks */
@@ -53,6 +60,17 @@
 #define OMAP_UART_FCR_RX_FIFO_TRIG_SHIFT		6
 #define OMAP_UART_FCR_RX_FIFO_TRIG_MASK			(0x3 << 6)
 
+/* MVR register bitmasks */
+#define OMAP_UART_MVR_SCHEME_SHIFT	30
+
+#define OMAP_UART_LEGACY_MVR_MAJ_MASK	0xf0
+#define OMAP_UART_LEGACY_MVR_MAJ_SHIFT	4
+#define OMAP_UART_LEGACY_MVR_MIN_MASK	0x0f
+
+#define OMAP_UART_MVR_MAJ_MASK		0x700
+#define OMAP_UART_MVR_MAJ_SHIFT		8
+#define OMAP_UART_MVR_MIN_MASK		0x3f
+
 static struct uart_omap_port *ui[OMAP_MAX_HSUART_PORTS];
 
 /* Forward declaration of functions */
@@ -1346,6 +1364,59 @@ static void uart_tx_dma_callback(int lch, u16 ch_status, void *data)
 	return;
 }
 
+static void omap_serial_fill_features_erratas(struct uart_omap_port *up)
+{
+	u32 mvr, scheme;
+	u16 revision, major, minor;
+
+	mvr = serial_in(up, UART_OMAP_MVER);
+
+	/* Check revision register scheme */
+	scheme = mvr >> OMAP_UART_MVR_SCHEME_SHIFT;
+
+	switch (scheme) {
+	case 0: /* Legacy Scheme: OMAP2/3 */
+		/* MINOR_REV[0:4], MAJOR_REV[4:7] */
+		major = (mvr & OMAP_UART_LEGACY_MVR_MAJ_MASK) >>
+					OMAP_UART_LEGACY_MVR_MAJ_SHIFT;
+		minor = (mvr & OMAP_UART_LEGACY_MVR_MIN_MASK);
+		break;
+	case 1:
+		/* New Scheme: OMAP4+ */
+		/* MINOR_REV[0:5], MAJOR_REV[8:10] */
+		major = (mvr & OMAP_UART_MVR_MAJ_MASK) >>
+					OMAP_UART_MVR_MAJ_SHIFT;
+		minor = (mvr & OMAP_UART_MVR_MIN_MASK);
+		break;
+	default:
+		dev_warn(&up->pdev->dev,
+			"Unknown %s revision, defaulting to highest\n",
+			up->name);
+		/* highest possible revision */
+		major = 0xff;
+		minor = 0xff;
+	}
+
+	/* normalize revision for the driver */
+	revision = UART_BUILD_REVISION(major, minor);
+
+	switch (revision) {
+	case OMAP_UART_REV_46:
+		up->errata |= (UART_ERRATA_i202_MDR1_ACCESS |
+				UART_ERRATA_i291_DMA_FORCEIDLE);
+		break;
+	case OMAP_UART_REV_52:
+		up->errata |= (UART_ERRATA_i202_MDR1_ACCESS |
+				UART_ERRATA_i291_DMA_FORCEIDLE);
+		break;
+	case OMAP_UART_REV_63:
+		up->errata |= UART_ERRATA_i202_MDR1_ACCESS;
+		break;
+	default:
+		break;
+	}
+}
+
 static struct omap_uart_port_info *of_get_uart_port_info(struct device *dev)
 {
 	struct omap_uart_port_info *omap_up_info;
@@ -1439,7 +1510,6 @@ static int serial_omap_probe(struct platform_device *pdev)
 						"%d\n", DEFAULT_CLK_SPEED);
 	}
 	up->uart_dma.uart_base = mem->start;
-	up->errata = omap_up_info->errata;
 
 	if (omap_up_info->dma_enabled) {
 		up->uart_dma.uart_dma_tx = dma_tx->start;
@@ -1469,6 +1539,8 @@ static int serial_omap_probe(struct platform_device *pdev)
 	pm_runtime_enable(&pdev->dev);
 	pm_runtime_get_sync(&pdev->dev);
 
+	omap_serial_fill_features_erratas(up);
+
 	ui[up->port.line] = up;
 	serial_omap_add_console_port(up);
 
diff --git a/drivers/tty/serial/pch_uart.c b/drivers/tty/serial/pch_uart.c
index c2816f49480..4fdec6a6b75 100644
--- a/drivers/tty/serial/pch_uart.c
+++ b/drivers/tty/serial/pch_uart.c
@@ -39,6 +39,7 @@ enum {
 	PCH_UART_HANDLED_RX_ERR_INT_SHIFT,
 	PCH_UART_HANDLED_RX_TRG_INT_SHIFT,
 	PCH_UART_HANDLED_MS_INT_SHIFT,
+	PCH_UART_HANDLED_LS_INT_SHIFT,
 };
 
 enum {
@@ -63,6 +64,8 @@ enum {
 					PCH_UART_HANDLED_RX_TRG_INT_SHIFT)<<1))
 #define PCH_UART_HANDLED_MS_INT	(1<<((PCH_UART_HANDLED_MS_INT_SHIFT)<<1))
 
+#define PCH_UART_HANDLED_LS_INT	(1<<((PCH_UART_HANDLED_LS_INT_SHIFT)<<1))
+
 #define PCH_UART_RBR		0x00
 #define PCH_UART_THR		0x00
 
@@ -229,7 +232,6 @@ struct eg20t_port {
 	int start_tx;
 	int start_rx;
 	int tx_empty;
-	int int_dis_flag;
 	int trigger;
 	int trigger_level;
 	struct pch_uart_buffer rxbuf;
@@ -237,7 +239,6 @@ struct eg20t_port {
 	unsigned int fcr;
 	unsigned int mcr;
 	unsigned int use_dma;
-	unsigned int use_dma_flag;
 	struct dma_async_tx_descriptor	*desc_tx;
 	struct dma_async_tx_descriptor	*desc_rx;
 	struct pch_dma_slave		param_tx;
@@ -560,14 +561,10 @@ static int pch_uart_hal_read(struct eg20t_port *priv, unsigned char *buf,
 	return i;
 }
 
-static unsigned int pch_uart_hal_get_iid(struct eg20t_port *priv)
+static unsigned char pch_uart_hal_get_iid(struct eg20t_port *priv)
 {
-	unsigned int iir;
-	int ret;
-
-	iir = ioread8(priv->membase + UART_IIR);
-	ret = (iir & (PCH_UART_IIR_IID | PCH_UART_IIR_TOI | PCH_UART_IIR_IP));
-	return ret;
+	return ioread8(priv->membase + UART_IIR) &\
+		      (PCH_UART_IIR_IID | PCH_UART_IIR_TOI | PCH_UART_IIR_IP);
 }
 
 static u8 pch_uart_hal_get_line_status(struct eg20t_port *priv)
@@ -666,10 +663,13 @@ static void pch_free_dma(struct uart_port *port)
 		dma_release_channel(priv->chan_rx);
 		priv->chan_rx = NULL;
 	}
-	if (sg_dma_address(&priv->sg_rx))
-		dma_free_coherent(port->dev, port->fifosize,
-				  sg_virt(&priv->sg_rx),
-				  sg_dma_address(&priv->sg_rx));
+
+	if (priv->rx_buf_dma) {
+		dma_free_coherent(port->dev, port->fifosize, priv->rx_buf_virt,
+				  priv->rx_buf_dma);
+		priv->rx_buf_virt = NULL;
+		priv->rx_buf_dma = 0;
+	}
 
 	return;
 }
@@ -1053,12 +1053,17 @@ static irqreturn_t pch_uart_interrupt(int irq, void *dev_id)
 	unsigned int handled;
 	u8 lsr;
 	int ret = 0;
-	unsigned int iid;
+	unsigned char iid;
 	unsigned long flags;
+	int next = 1;
+	u8 msr;
 
 	spin_lock_irqsave(&priv->port.lock, flags);
 	handled = 0;
-	while ((iid = pch_uart_hal_get_iid(priv)) > 1) {
+	while (next) {
+		iid = pch_uart_hal_get_iid(priv);
+		if (iid & PCH_UART_IIR_IP) /* No Interrupt */
+			break;
 		switch (iid) {
 		case PCH_UART_IID_RLS:	/* Receiver Line Status */
 			lsr = pch_uart_hal_get_line_status(priv);
@@ -1066,6 +1071,8 @@ static irqreturn_t pch_uart_interrupt(int irq, void *dev_id)
 						UART_LSR_PE | UART_LSR_OE)) {
 				pch_uart_err_ir(priv, lsr);
 				ret = PCH_UART_HANDLED_RX_ERR_INT;
+			} else {
+				ret = PCH_UART_HANDLED_LS_INT;
 			}
 			break;
 		case PCH_UART_IID_RDR:	/* Received Data Ready */
@@ -1092,20 +1099,22 @@ static irqreturn_t pch_uart_interrupt(int irq, void *dev_id)
 				ret = handle_tx(priv);
 			break;
 		case PCH_UART_IID_MS:	/* Modem Status */
-			ret = PCH_UART_HANDLED_MS_INT;
+			msr = pch_uart_hal_get_modem(priv);
+			next = 0; /* MS ir prioirty is the lowest. So, MS ir
+				     means final interrupt */
+			if ((msr & UART_MSR_ANY_DELTA) == 0)
+				break;
+			ret |= PCH_UART_HANDLED_MS_INT;
 			break;
 		default:	/* Never junp to this label */
-			dev_err(priv->port.dev, "%s:iid=%d (%lu)\n", __func__,
+			dev_err(priv->port.dev, "%s:iid=%02x (%lu)\n", __func__,
 				iid, jiffies);
 			ret = -1;
+			next = 0;
 			break;
 		}
 		handled |= (unsigned int)ret;
 	}
-	if (handled == 0 && iid <= 1) {
-		if (priv->int_dis_flag)
-			priv->int_dis_flag = 0;
-	}
 
 	spin_unlock_irqrestore(&priv->port.lock, flags);
 	return IRQ_RETVAL(handled);
@@ -1200,7 +1209,6 @@ static void pch_uart_stop_rx(struct uart_port *port)
 	priv = container_of(port, struct eg20t_port, port);
 	priv->start_rx = 0;
 	pch_uart_hal_disable_interrupt(priv, PCH_UART_HAL_RX_INT);
-	priv->int_dis_flag = 1;
 }
 
 /* Enable the modem status interrupts. */
@@ -1447,7 +1455,6 @@ static int pch_uart_verify_port(struct uart_port *port,
 			__func__);
 		return -EOPNOTSUPP;
 #endif
-		priv->use_dma_flag = 1;
 		dev_info(priv->port.dev, "PCH UART : Use DMA Mode\n");
 		if (!priv->use_dma)
 			pch_request_dma(port);
diff --git a/drivers/tty/serial/sb1250-duart.c b/drivers/tty/serial/sb1250-duart.c
index 0be8a2f00d0..f76b1688c5c 100644
--- a/drivers/tty/serial/sb1250-duart.c
+++ b/drivers/tty/serial/sb1250-duart.c
@@ -31,6 +31,7 @@
 #include <linux/interrupt.h>
 #include <linux/ioport.h>
 #include <linux/kernel.h>
+#include <linux/module.h>
 #include <linux/major.h>
 #include <linux/serial.h>
 #include <linux/serial_core.h>
diff --git a/drivers/tty/serial/serial_core.c b/drivers/tty/serial/serial_core.c
index 9c4c05b2825..246b823c1b2 100644
--- a/drivers/tty/serial/serial_core.c
+++ b/drivers/tty/serial/serial_core.c
@@ -2282,6 +2282,7 @@ void uart_unregister_driver(struct uart_driver *drv)
 	tty_unregister_driver(p);
 	put_tty_driver(p);
 	kfree(drv->state);
+	drv->state = NULL;
 	drv->tty_driver = NULL;
 }
 
diff --git a/drivers/tty/serial/sh-sci.c b/drivers/tty/serial/sh-sci.c
index 3158e17b665..4604153b795 100644
--- a/drivers/tty/serial/sh-sci.c
+++ b/drivers/tty/serial/sh-sci.c
@@ -1052,9 +1052,17 @@ static int sci_request_irq(struct sci_port *port)
 		if (SCIx_IRQ_IS_MUXED(port)) {
 			i = SCIx_MUX_IRQ;
 			irq = up->irq;
-		} else
+		} else {
 			irq = port->cfg->irqs[i];
 
+			/*
+			 * Certain port types won't support all of the
+			 * available interrupt sources.
+			 */
+			if (unlikely(!irq))
+				continue;
+		}
+
 		desc = sci_irq_desc + i;
 		port->irqstr[j] = kasprintf(GFP_KERNEL, "%s:%s",
 					    dev_name(up->dev), desc->desc);
@@ -1094,6 +1102,15 @@ static void sci_free_irq(struct sci_port *port)
 	 * IRQ first.
 	 */
 	for (i = 0; i < SCIx_NR_IRQS; i++) {
+		unsigned int irq = port->cfg->irqs[i];
+
+		/*
+		 * Certain port types won't support all of the available
+		 * interrupt sources.
+		 */
+		if (unlikely(!irq))
+			continue;
+
 		free_irq(port->cfg->irqs[i], port);
 		kfree(port->irqstr[i]);
 
@@ -1564,10 +1581,32 @@ static void sci_enable_ms(struct uart_port *port)
 
 static void sci_break_ctl(struct uart_port *port, int break_state)
 {
-	/*
-	 * Not supported by hardware. Most parts couple break and rx
-	 * interrupts together, with break detection always enabled.
-	 */
+	struct sci_port *s = to_sci_port(port);
+	struct plat_sci_reg *reg = sci_regmap[s->cfg->regtype] + SCSPTR;
+	unsigned short scscr, scsptr;
+
+	/* check wheter the port has SCSPTR */
+	if (!reg->size) {
+		/*
+		 * Not supported by hardware. Most parts couple break and rx
+		 * interrupts together, with break detection always enabled.
+		 */
+		return;
+	}
+
+	scsptr = serial_port_in(port, SCSPTR);
+	scscr = serial_port_in(port, SCSCR);
+
+	if (break_state == -1) {
+		scsptr = (scsptr | SCSPTR_SPB2IO) & ~SCSPTR_SPB2DT;
+		scscr &= ~SCSCR_TE;
+	} else {
+		scsptr = (scsptr | SCSPTR_SPB2DT) & ~SCSPTR_SPB2IO;
+		scscr |= SCSCR_TE;
+	}
+
+	serial_port_out(port, SCSPTR, scsptr);
+	serial_port_out(port, SCSCR, scscr);
 }
 
 #ifdef CONFIG_SERIAL_SH_SCI_DMA
diff --git a/drivers/tty/serial/zs.c b/drivers/tty/serial/zs.c
index 4001eee6c08..92c00b24d0d 100644
--- a/drivers/tty/serial/zs.c
+++ b/drivers/tty/serial/zs.c
@@ -57,6 +57,7 @@
 #include <linux/ioport.h>
 #include <linux/irqflags.h>
 #include <linux/kernel.h>
+#include <linux/module.h>
 #include <linux/major.h>
 #include <linux/serial.h>
 #include <linux/serial_core.h>
diff --git a/drivers/tty/synclink.c b/drivers/tty/synclink.c
index 593d40ad0a6..5ed0daae656 100644
--- a/drivers/tty/synclink.c
+++ b/drivers/tty/synclink.c
@@ -3338,9 +3338,9 @@ static int block_til_ready(struct tty_struct *tty, struct file * filp,
 			printk("%s(%d):block_til_ready blocking on %s count=%d\n",
 				 __FILE__,__LINE__, tty->driver->name, port->count );
 				 
-		tty_unlock();
+		tty_unlock(tty);
 		schedule();
-		tty_lock();
+		tty_lock(tty);
 	}
 	
 	set_current_state(TASK_RUNNING);
diff --git a/drivers/tty/synclink_gt.c b/drivers/tty/synclink_gt.c
index aa1debf97cc..45b43f11ca3 100644
--- a/drivers/tty/synclink_gt.c
+++ b/drivers/tty/synclink_gt.c
@@ -3336,9 +3336,9 @@ static int block_til_ready(struct tty_struct *tty, struct file *filp,
 		}
 
 		DBGINFO(("%s block_til_ready wait\n", tty->driver->name));
-		tty_unlock();
+		tty_unlock(tty);
 		schedule();
-		tty_lock();
+		tty_lock(tty);
 	}
 
 	set_current_state(TASK_RUNNING);
diff --git a/drivers/tty/synclinkmp.c b/drivers/tty/synclinkmp.c
index a3dddc12d2f..4a1e4f07765 100644
--- a/drivers/tty/synclinkmp.c
+++ b/drivers/tty/synclinkmp.c
@@ -3357,9 +3357,9 @@ static int block_til_ready(struct tty_struct *tty, struct file *filp,
 			printk("%s(%d):%s block_til_ready() count=%d\n",
 				 __FILE__,__LINE__, tty->driver->name, port->count );
 
-		tty_unlock();
+		tty_unlock(tty);
 		schedule();
-		tty_lock();
+		tty_lock(tty);
 	}
 
 	set_current_state(TASK_RUNNING);
diff --git a/drivers/tty/tty_buffer.c b/drivers/tty/tty_buffer.c
index 6c9b7cd6778..91e326ffe7d 100644
--- a/drivers/tty/tty_buffer.c
+++ b/drivers/tty/tty_buffer.c
@@ -185,25 +185,19 @@ static struct tty_buffer *tty_buffer_find(struct tty_struct *tty, size_t size)
 	/* Should possibly check if this fails for the largest buffer we
 	   have queued and recycle that ? */
 }
-
 /**
- *	tty_buffer_request_room		-	grow tty buffer if needed
+ *	__tty_buffer_request_room		-	grow tty buffer if needed
  *	@tty: tty structure
  *	@size: size desired
  *
  *	Make at least size bytes of linear space available for the tty
  *	buffer. If we fail return the size we managed to find.
- *
- *	Locking: Takes tty->buf.lock
+ *      Locking: Caller must hold tty->buf.lock
  */
-int tty_buffer_request_room(struct tty_struct *tty, size_t size)
+static int __tty_buffer_request_room(struct tty_struct *tty, size_t size)
 {
 	struct tty_buffer *b, *n;
 	int left;
-	unsigned long flags;
-
-	spin_lock_irqsave(&tty->buf.lock, flags);
-
 	/* OPTIMISATION: We could keep a per tty "zero" sized buffer to
 	   remove this conditional if its worth it. This would be invisible
 	   to the callers */
@@ -225,9 +219,30 @@ int tty_buffer_request_room(struct tty_struct *tty, size_t size)
 			size = left;
 	}
 
-	spin_unlock_irqrestore(&tty->buf.lock, flags);
 	return size;
 }
+
+
+/**
+ *	tty_buffer_request_room		-	grow tty buffer if needed
+ *	@tty: tty structure
+ *	@size: size desired
+ *
+ *	Make at least size bytes of linear space available for the tty
+ *	buffer. If we fail return the size we managed to find.
+ *
+ *	Locking: Takes tty->buf.lock
+ */
+int tty_buffer_request_room(struct tty_struct *tty, size_t size)
+{
+	unsigned long flags;
+	int length;
+
+	spin_lock_irqsave(&tty->buf.lock, flags);
+	length = __tty_buffer_request_room(tty, size);
+	spin_unlock_irqrestore(&tty->buf.lock, flags);
+	return length;
+}
 EXPORT_SYMBOL_GPL(tty_buffer_request_room);
 
 /**
@@ -249,14 +264,22 @@ int tty_insert_flip_string_fixed_flag(struct tty_struct *tty,
 	int copied = 0;
 	do {
 		int goal = min_t(size_t, size - copied, TTY_BUFFER_PAGE);
-		int space = tty_buffer_request_room(tty, goal);
-		struct tty_buffer *tb = tty->buf.tail;
+		int space;
+		unsigned long flags;
+		struct tty_buffer *tb;
+
+		spin_lock_irqsave(&tty->buf.lock, flags);
+		space = __tty_buffer_request_room(tty, goal);
+		tb = tty->buf.tail;
 		/* If there is no space then tb may be NULL */
-		if (unlikely(space == 0))
+		if (unlikely(space == 0)) {
+			spin_unlock_irqrestore(&tty->buf.lock, flags);
 			break;
+		}
 		memcpy(tb->char_buf_ptr + tb->used, chars, space);
 		memset(tb->flag_buf_ptr + tb->used, flag, space);
 		tb->used += space;
+		spin_unlock_irqrestore(&tty->buf.lock, flags);
 		copied += space;
 		chars += space;
 		/* There is a small chance that we need to split the data over
@@ -286,14 +309,22 @@ int tty_insert_flip_string_flags(struct tty_struct *tty,
 	int copied = 0;
 	do {
 		int goal = min_t(size_t, size - copied, TTY_BUFFER_PAGE);
-		int space = tty_buffer_request_room(tty, goal);
-		struct tty_buffer *tb = tty->buf.tail;
+		int space;
+		unsigned long __flags;
+		struct tty_buffer *tb;
+
+		spin_lock_irqsave(&tty->buf.lock, __flags);
+		space = __tty_buffer_request_room(tty, goal);
+		tb = tty->buf.tail;
 		/* If there is no space then tb may be NULL */
-		if (unlikely(space == 0))
+		if (unlikely(space == 0)) {
+			spin_unlock_irqrestore(&tty->buf.lock, __flags);
 			break;
+		}
 		memcpy(tb->char_buf_ptr + tb->used, chars, space);
 		memcpy(tb->flag_buf_ptr + tb->used, flags, space);
 		tb->used += space;
+		spin_unlock_irqrestore(&tty->buf.lock, __flags);
 		copied += space;
 		chars += space;
 		flags += space;
@@ -344,13 +375,20 @@ EXPORT_SYMBOL(tty_schedule_flip);
 int tty_prepare_flip_string(struct tty_struct *tty, unsigned char **chars,
 								size_t size)
 {
-	int space = tty_buffer_request_room(tty, size);
+	int space;
+	unsigned long flags;
+	struct tty_buffer *tb;
+
+	spin_lock_irqsave(&tty->buf.lock, flags);
+	space = __tty_buffer_request_room(tty, size);
+
+	tb = tty->buf.tail;
 	if (likely(space)) {
-		struct tty_buffer *tb = tty->buf.tail;
 		*chars = tb->char_buf_ptr + tb->used;
 		memset(tb->flag_buf_ptr + tb->used, TTY_NORMAL, space);
 		tb->used += space;
 	}
+	spin_unlock_irqrestore(&tty->buf.lock, flags);
 	return space;
 }
 EXPORT_SYMBOL_GPL(tty_prepare_flip_string);
@@ -374,13 +412,20 @@ EXPORT_SYMBOL_GPL(tty_prepare_flip_string);
 int tty_prepare_flip_string_flags(struct tty_struct *tty,
 			unsigned char **chars, char **flags, size_t size)
 {
-	int space = tty_buffer_request_room(tty, size);
+	int space;
+	unsigned long __flags;
+	struct tty_buffer *tb;
+
+	spin_lock_irqsave(&tty->buf.lock, __flags);
+	space = __tty_buffer_request_room(tty, size);
+
+	tb = tty->buf.tail;
 	if (likely(space)) {
-		struct tty_buffer *tb = tty->buf.tail;
 		*chars = tb->char_buf_ptr + tb->used;
 		*flags = tb->flag_buf_ptr + tb->used;
 		tb->used += space;
 	}
+	spin_unlock_irqrestore(&tty->buf.lock, __flags);
 	return space;
 }
 EXPORT_SYMBOL_GPL(tty_prepare_flip_string_flags);
diff --git a/drivers/tty/tty_io.c b/drivers/tty/tty_io.c
index d939bd705c7..9e930c009bf 100644
--- a/drivers/tty/tty_io.c
+++ b/drivers/tty/tty_io.c
@@ -185,6 +185,7 @@ void free_tty_struct(struct tty_struct *tty)
 		put_device(tty->dev);
 	kfree(tty->write_buf);
 	tty_buffer_free_all(tty);
+	tty->magic = 0xDEADDEAD;
 	kfree(tty);
 }
 
@@ -573,7 +574,7 @@ void __tty_hangup(struct tty_struct *tty)
 	}
 	spin_unlock(&redirect_lock);
 
-	tty_lock();
+	tty_lock(tty);
 
 	/* some functions below drop BTM, so we need this bit */
 	set_bit(TTY_HUPPING, &tty->flags);
@@ -666,7 +667,7 @@ void __tty_hangup(struct tty_struct *tty)
 	clear_bit(TTY_HUPPING, &tty->flags);
 	tty_ldisc_enable(tty);
 
-	tty_unlock();
+	tty_unlock(tty);
 
 	if (f)
 		fput(f);
@@ -855,10 +856,11 @@ void disassociate_ctty(int on_exit)
  */
 void no_tty(void)
 {
+	/* FIXME: Review locking here. The tty_lock never covered any race
+	   between a new association and proc_clear_tty but possible we need
+	   to protect against this anyway */
 	struct task_struct *tsk = current;
-	tty_lock();
 	disassociate_ctty(0);
-	tty_unlock();
 	proc_clear_tty(tsk);
 }
 
@@ -1102,12 +1104,12 @@ void tty_write_message(struct tty_struct *tty, char *msg)
 {
 	if (tty) {
 		mutex_lock(&tty->atomic_write_lock);
-		tty_lock();
+		tty_lock(tty);
 		if (tty->ops->write && !test_bit(TTY_CLOSING, &tty->flags)) {
-			tty_unlock();
+			tty_unlock(tty);
 			tty->ops->write(tty, msg, strlen(msg));
 		} else
-			tty_unlock();
+			tty_unlock(tty);
 		tty_write_unlock(tty);
 	}
 	return;
@@ -1402,6 +1404,7 @@ struct tty_struct *tty_init_dev(struct tty_driver *driver, int idx)
 	}
 	initialize_tty_struct(tty, driver, idx);
 
+	tty_lock(tty);
 	retval = tty_driver_install_tty(driver, tty);
 	if (retval < 0)
 		goto err_deinit_tty;
@@ -1414,9 +1417,11 @@ struct tty_struct *tty_init_dev(struct tty_driver *driver, int idx)
 	retval = tty_ldisc_setup(tty, tty->link);
 	if (retval)
 		goto err_release_tty;
+	/* Return the tty locked so that it cannot vanish under the caller */
 	return tty;
 
 err_deinit_tty:
+	tty_unlock(tty);
 	deinitialize_tty_struct(tty);
 	free_tty_struct(tty);
 err_module_put:
@@ -1425,6 +1430,7 @@ err_module_put:
 
 	/* call the tty release_tty routine to clean out this slot */
 err_release_tty:
+	tty_unlock(tty);
 	printk_ratelimited(KERN_INFO "tty_init_dev: ldisc open failed, "
 				 "clearing slot %d\n", idx);
 	release_tty(tty, idx);
@@ -1627,7 +1633,7 @@ int tty_release(struct inode *inode, struct file *filp)
 	if (tty_paranoia_check(tty, inode, __func__))
 		return 0;
 
-	tty_lock();
+	tty_lock(tty);
 	check_tty_count(tty, __func__);
 
 	__tty_fasync(-1, filp, 0);
@@ -1636,10 +1642,11 @@ int tty_release(struct inode *inode, struct file *filp)
 	pty_master = (tty->driver->type == TTY_DRIVER_TYPE_PTY &&
 		      tty->driver->subtype == PTY_TYPE_MASTER);
 	devpts = (tty->driver->flags & TTY_DRIVER_DEVPTS_MEM) != 0;
+	/* Review: parallel close */
 	o_tty = tty->link;
 
 	if (tty_release_checks(tty, o_tty, idx)) {
-		tty_unlock();
+		tty_unlock(tty);
 		return 0;
 	}
 
@@ -1651,7 +1658,7 @@ int tty_release(struct inode *inode, struct file *filp)
 	if (tty->ops->close)
 		tty->ops->close(tty, filp);
 
-	tty_unlock();
+	tty_unlock(tty);
 	/*
 	 * Sanity check: if tty->count is going to zero, there shouldn't be
 	 * any waiters on tty->read_wait or tty->write_wait.  We test the
@@ -1674,7 +1681,7 @@ int tty_release(struct inode *inode, struct file *filp)
 		   opens on /dev/tty */
 
 		mutex_lock(&tty_mutex);
-		tty_lock();
+		tty_lock_pair(tty, o_tty);
 		tty_closing = tty->count <= 1;
 		o_tty_closing = o_tty &&
 			(o_tty->count <= (pty_master ? 1 : 0));
@@ -1705,7 +1712,7 @@ int tty_release(struct inode *inode, struct file *filp)
 
 		printk(KERN_WARNING "%s: %s: read/write wait queue active!\n",
 				__func__, tty_name(tty, buf));
-		tty_unlock();
+		tty_unlock_pair(tty, o_tty);
 		mutex_unlock(&tty_mutex);
 		schedule();
 	}
@@ -1768,7 +1775,7 @@ int tty_release(struct inode *inode, struct file *filp)
 
 	/* check whether both sides are closing ... */
 	if (!tty_closing || (o_tty && !o_tty_closing)) {
-		tty_unlock();
+		tty_unlock_pair(tty, o_tty);
 		return 0;
 	}
 
@@ -1781,14 +1788,16 @@ int tty_release(struct inode *inode, struct file *filp)
 	tty_ldisc_release(tty, o_tty);
 	/*
 	 * The release_tty function takes care of the details of clearing
-	 * the slots and preserving the termios structure.
+	 * the slots and preserving the termios structure. The tty_unlock_pair
+	 * should be safe as we keep a kref while the tty is locked (so the
+	 * unlock never unlocks a freed tty).
 	 */
 	release_tty(tty, idx);
+	tty_unlock_pair(tty, o_tty);
 
 	/* Make this pty number available for reallocation */
 	if (devpts)
 		devpts_kill_index(inode, idx);
-	tty_unlock();
 	return 0;
 }
 
@@ -1800,6 +1809,9 @@ int tty_release(struct inode *inode, struct file *filp)
  *
  *	We cannot return driver and index like for the other nodes because
  *	devpts will not work then. It expects inodes to be from devpts FS.
+ *
+ *	We need to move to returning a refcounted object from all the lookup
+ *	paths including this one.
  */
 static struct tty_struct *tty_open_current_tty(dev_t device, struct file *filp)
 {
@@ -1816,6 +1828,7 @@ static struct tty_struct *tty_open_current_tty(dev_t device, struct file *filp)
 	/* noctty = 1; */
 	tty_kref_put(tty);
 	/* FIXME: we put a reference and return a TTY! */
+	/* This is only safe because the caller holds tty_mutex */
 	return tty;
 }
 
@@ -1888,6 +1901,9 @@ static struct tty_driver *tty_lookup_driver(dev_t device, struct file *filp,
  *	Locking: tty_mutex protects tty, tty_lookup_driver and tty_init_dev.
  *		 tty->count should protect the rest.
  *		 ->siglock protects ->signal/->sighand
+ *
+ *	Note: the tty_unlock/lock cases without a ref are only safe due to
+ *	tty_mutex
  */
 
 static int tty_open(struct inode *inode, struct file *filp)
@@ -1911,8 +1927,7 @@ retry_open:
 	retval = 0;
 
 	mutex_lock(&tty_mutex);
-	tty_lock();
-
+	/* This is protected by the tty_mutex */
 	tty = tty_open_current_tty(device, filp);
 	if (IS_ERR(tty)) {
 		retval = PTR_ERR(tty);
@@ -1933,17 +1948,19 @@ retry_open:
 	}
 
 	if (tty) {
+		tty_lock(tty);
 		retval = tty_reopen(tty);
-		if (retval)
+		if (retval < 0) {
+			tty_unlock(tty);
 			tty = ERR_PTR(retval);
-	} else
+		}
+	} else	/* Returns with the tty_lock held for now */
 		tty = tty_init_dev(driver, index);
 
 	mutex_unlock(&tty_mutex);
 	if (driver)
 		tty_driver_kref_put(driver);
 	if (IS_ERR(tty)) {
-		tty_unlock();
 		retval = PTR_ERR(tty);
 		goto err_file;
 	}
@@ -1972,7 +1989,7 @@ retry_open:
 		printk(KERN_DEBUG "%s: error %d in opening %s...\n", __func__,
 				retval, tty->name);
 #endif
-		tty_unlock(); /* need to call tty_release without BTM */
+		tty_unlock(tty); /* need to call tty_release without BTM */
 		tty_release(inode, filp);
 		if (retval != -ERESTARTSYS)
 			return retval;
@@ -1984,17 +2001,15 @@ retry_open:
 		/*
 		 * Need to reset f_op in case a hangup happened.
 		 */
-		tty_lock();
 		if (filp->f_op == &hung_up_tty_fops)
 			filp->f_op = &tty_fops;
-		tty_unlock();
 		goto retry_open;
 	}
-	tty_unlock();
+	tty_unlock(tty);
 
 
 	mutex_lock(&tty_mutex);
-	tty_lock();
+	tty_lock(tty);
 	spin_lock_irq(&current->sighand->siglock);
 	if (!noctty &&
 	    current->signal->leader &&
@@ -2002,11 +2017,10 @@ retry_open:
 	    tty->session == NULL)
 		__proc_set_tty(current, tty);
 	spin_unlock_irq(&current->sighand->siglock);
-	tty_unlock();
+	tty_unlock(tty);
 	mutex_unlock(&tty_mutex);
 	return 0;
 err_unlock:
-	tty_unlock();
 	mutex_unlock(&tty_mutex);
 	/* after locks to avoid deadlock */
 	if (!IS_ERR_OR_NULL(driver))
@@ -2089,10 +2103,13 @@ out:
 
 static int tty_fasync(int fd, struct file *filp, int on)
 {
+	struct tty_struct *tty = file_tty(filp);
 	int retval;
-	tty_lock();
+
+	tty_lock(tty);
 	retval = __tty_fasync(fd, filp, on);
-	tty_unlock();
+	tty_unlock(tty);
+
 	return retval;
 }
 
@@ -2929,6 +2946,7 @@ void initialize_tty_struct(struct tty_struct *tty,
 	tty->pgrp = NULL;
 	tty->overrun_time = jiffies;
 	tty_buffer_init(tty);
+	mutex_init(&tty->legacy_mutex);
 	mutex_init(&tty->termios_mutex);
 	mutex_init(&tty->ldisc_mutex);
 	init_waitqueue_head(&tty->write_wait);
diff --git a/drivers/tty/tty_ldisc.c b/drivers/tty/tty_ldisc.c
index 24b95db75d8..ba8be396a62 100644
--- a/drivers/tty/tty_ldisc.c
+++ b/drivers/tty/tty_ldisc.c
@@ -28,7 +28,6 @@
 
 static DEFINE_SPINLOCK(tty_ldisc_lock);
 static DECLARE_WAIT_QUEUE_HEAD(tty_ldisc_wait);
-static DECLARE_WAIT_QUEUE_HEAD(tty_ldisc_idle);
 /* Line disc dispatch table */
 static struct tty_ldisc_ops *tty_ldiscs[NR_LDISCS];
 
@@ -65,7 +64,7 @@ static void put_ldisc(struct tty_ldisc *ld)
 		return;
 	}
 	local_irq_restore(flags);
-	wake_up(&tty_ldisc_idle);
+	wake_up(&ld->wq_idle);
 }
 
 /**
@@ -200,6 +199,8 @@ static struct tty_ldisc *tty_ldisc_get(int disc)
 
 	ld->ops = ldops;
 	atomic_set(&ld->users, 1);
+	init_waitqueue_head(&ld->wq_idle);
+
 	return ld;
 }
 
@@ -538,7 +539,7 @@ static void tty_ldisc_flush_works(struct tty_struct *tty)
 static int tty_ldisc_wait_idle(struct tty_struct *tty, long timeout)
 {
 	long ret;
-	ret = wait_event_timeout(tty_ldisc_idle,
+	ret = wait_event_timeout(tty->ldisc->wq_idle,
 			atomic_read(&tty->ldisc->users) == 1, timeout);
 	return ret > 0 ? 0 : -EBUSY;
 }
@@ -567,7 +568,7 @@ int tty_set_ldisc(struct tty_struct *tty, int ldisc)
 	if (IS_ERR(new_ldisc))
 		return PTR_ERR(new_ldisc);
 
-	tty_lock();
+	tty_lock(tty);
 	/*
 	 *	We need to look at the tty locking here for pty/tty pairs
 	 *	when both sides try to change in parallel.
@@ -581,12 +582,12 @@ int tty_set_ldisc(struct tty_struct *tty, int ldisc)
 	 */
 
 	if (tty->ldisc->ops->num == ldisc) {
-		tty_unlock();
+		tty_unlock(tty);
 		tty_ldisc_put(new_ldisc);
 		return 0;
 	}
 
-	tty_unlock();
+	tty_unlock(tty);
 	/*
 	 *	Problem: What do we do if this blocks ?
 	 *	We could deadlock here
@@ -594,7 +595,7 @@ int tty_set_ldisc(struct tty_struct *tty, int ldisc)
 
 	tty_wait_until_sent(tty, 0);
 
-	tty_lock();
+	tty_lock(tty);
 	mutex_lock(&tty->ldisc_mutex);
 
 	/*
@@ -604,10 +605,10 @@ int tty_set_ldisc(struct tty_struct *tty, int ldisc)
 
 	while (test_bit(TTY_LDISC_CHANGING, &tty->flags)) {
 		mutex_unlock(&tty->ldisc_mutex);
-		tty_unlock();
+		tty_unlock(tty);
 		wait_event(tty_ldisc_wait,
 			test_bit(TTY_LDISC_CHANGING, &tty->flags) == 0);
-		tty_lock();
+		tty_lock(tty);
 		mutex_lock(&tty->ldisc_mutex);
 	}
 
@@ -622,7 +623,7 @@ int tty_set_ldisc(struct tty_struct *tty, int ldisc)
 
 	o_ldisc = tty->ldisc;
 
-	tty_unlock();
+	tty_unlock(tty);
 	/*
 	 *	Make sure we don't change while someone holds a
 	 *	reference to the line discipline. The TTY_LDISC bit
@@ -649,7 +650,7 @@ int tty_set_ldisc(struct tty_struct *tty, int ldisc)
 
 	retval = tty_ldisc_wait_idle(tty, 5 * HZ);
 
-	tty_lock();
+	tty_lock(tty);
 	mutex_lock(&tty->ldisc_mutex);
 
 	/* handle wait idle failure locked */
@@ -664,7 +665,7 @@ int tty_set_ldisc(struct tty_struct *tty, int ldisc)
 		clear_bit(TTY_LDISC_CHANGING, &tty->flags);
 		mutex_unlock(&tty->ldisc_mutex);
 		tty_ldisc_put(new_ldisc);
-		tty_unlock();
+		tty_unlock(tty);
 		return -EIO;
 	}
 
@@ -707,7 +708,7 @@ enable:
 	if (o_work)
 		schedule_work(&o_tty->buf.work);
 	mutex_unlock(&tty->ldisc_mutex);
-	tty_unlock();
+	tty_unlock(tty);
 	return retval;
 }
 
@@ -815,11 +816,11 @@ void tty_ldisc_hangup(struct tty_struct *tty)
 	 * need to wait for another function taking the BTM
 	 */
 	clear_bit(TTY_LDISC, &tty->flags);
-	tty_unlock();
+	tty_unlock(tty);
 	cancel_work_sync(&tty->buf.work);
 	mutex_unlock(&tty->ldisc_mutex);
 retry:
-	tty_lock();
+	tty_lock(tty);
 	mutex_lock(&tty->ldisc_mutex);
 
 	/* At this point we have a closed ldisc and we want to
@@ -830,7 +831,7 @@ retry:
 		if (atomic_read(&tty->ldisc->users) != 1) {
 			char cur_n[TASK_COMM_LEN], tty_n[64];
 			long timeout = 3 * HZ;
-			tty_unlock();
+			tty_unlock(tty);
 
 			while (tty_ldisc_wait_idle(tty, timeout) == -EBUSY) {
 				timeout = MAX_SCHEDULE_TIMEOUT;
@@ -893,6 +894,23 @@ int tty_ldisc_setup(struct tty_struct *tty, struct tty_struct *o_tty)
 	tty_ldisc_enable(tty);
 	return 0;
 }
+
+static void tty_ldisc_kill(struct tty_struct *tty)
+{
+	mutex_lock(&tty->ldisc_mutex);
+	/*
+	 * Now kill off the ldisc
+	 */
+	tty_ldisc_close(tty, tty->ldisc);
+	tty_ldisc_put(tty->ldisc);
+	/* Force an oops if we mess this up */
+	tty->ldisc = NULL;
+
+	/* Ensure the next open requests the N_TTY ldisc */
+	tty_set_termios_ldisc(tty, N_TTY);
+	mutex_unlock(&tty->ldisc_mutex);
+}
+
 /**
  *	tty_ldisc_release		-	release line discipline
  *	@tty: tty being shut down
@@ -911,27 +929,19 @@ void tty_ldisc_release(struct tty_struct *tty, struct tty_struct *o_tty)
 	 * race with the set_ldisc code path.
 	 */
 
-	tty_unlock();
+	tty_unlock_pair(tty, o_tty);
 	tty_ldisc_halt(tty);
 	tty_ldisc_flush_works(tty);
-	tty_lock();
-
-	mutex_lock(&tty->ldisc_mutex);
-	/*
-	 * Now kill off the ldisc
-	 */
-	tty_ldisc_close(tty, tty->ldisc);
-	tty_ldisc_put(tty->ldisc);
-	/* Force an oops if we mess this up */
-	tty->ldisc = NULL;
+	if (o_tty) {
+		tty_ldisc_halt(o_tty);
+		tty_ldisc_flush_works(o_tty);
+	}
+	tty_lock_pair(tty, o_tty);
 
-	/* Ensure the next open requests the N_TTY ldisc */
-	tty_set_termios_ldisc(tty, N_TTY);
-	mutex_unlock(&tty->ldisc_mutex);
 
-	/* This will need doing differently if we need to lock */
+	tty_ldisc_kill(tty);
 	if (o_tty)
-		tty_ldisc_release(o_tty, NULL);
+		tty_ldisc_kill(o_tty);
 
 	/* And the memory resources remaining (buffers, termios) will be
 	   disposed of when the kref hits zero */
diff --git a/drivers/tty/tty_mutex.c b/drivers/tty/tty_mutex.c
index 9ff986c32a2..69adc80c98c 100644
--- a/drivers/tty/tty_mutex.c
+++ b/drivers/tty/tty_mutex.c
@@ -4,29 +4,59 @@
 #include <linux/semaphore.h>
 #include <linux/sched.h>
 
-/*
- * The 'big tty mutex'
- *
- * This mutex is taken and released by tty_lock() and tty_unlock(),
- * replacing the older big kernel lock.
- * It can no longer be taken recursively, and does not get
- * released implicitly while sleeping.
- *
- * Don't use in new code.
- */
-static DEFINE_MUTEX(big_tty_mutex);
+/* Legacy tty mutex glue */
 
 /*
  * Getting the big tty mutex.
  */
-void __lockfunc tty_lock(void)
+
+void __lockfunc tty_lock(struct tty_struct *tty)
 {
-	mutex_lock(&big_tty_mutex);
+	if (tty->magic != TTY_MAGIC) {
+		printk(KERN_ERR "L Bad %p\n", tty);
+		WARN_ON(1);
+		return;
+	}
+	tty_kref_get(tty);
+	mutex_lock(&tty->legacy_mutex);
 }
 EXPORT_SYMBOL(tty_lock);
 
-void __lockfunc tty_unlock(void)
+void __lockfunc tty_unlock(struct tty_struct *tty)
 {
-	mutex_unlock(&big_tty_mutex);
+	if (tty->magic != TTY_MAGIC) {
+		printk(KERN_ERR "U Bad %p\n", tty);
+		WARN_ON(1);
+		return;
+	}
+	mutex_unlock(&tty->legacy_mutex);
+	tty_kref_put(tty);
 }
 EXPORT_SYMBOL(tty_unlock);
+
+/*
+ * Getting the big tty mutex for a pair of ttys with lock ordering
+ * On a non pty/tty pair tty2 can be NULL which is just fine.
+ */
+void __lockfunc tty_lock_pair(struct tty_struct *tty,
+					struct tty_struct *tty2)
+{
+	if (tty < tty2) {
+		tty_lock(tty);
+		tty_lock(tty2);
+	} else {
+		if (tty2 && tty2 != tty)
+			tty_lock(tty2);
+		tty_lock(tty);
+	}
+}
+EXPORT_SYMBOL(tty_lock_pair);
+
+void __lockfunc tty_unlock_pair(struct tty_struct *tty,
+						struct tty_struct *tty2)
+{
+	tty_unlock(tty);
+	if (tty2 && tty2 != tty)
+		tty_unlock(tty2);
+}
+EXPORT_SYMBOL(tty_unlock_pair);
diff --git a/drivers/tty/tty_port.c b/drivers/tty/tty_port.c
index bf6e238146a..d9cca95a545 100644
--- a/drivers/tty/tty_port.c
+++ b/drivers/tty/tty_port.c
@@ -230,7 +230,7 @@ int tty_port_block_til_ready(struct tty_port *port,
 
 	/* block if port is in the process of being closed */
 	if (tty_hung_up_p(filp) || port->flags & ASYNC_CLOSING) {
-		wait_event_interruptible_tty(port->close_wait,
+		wait_event_interruptible_tty(tty, port->close_wait,
 				!(port->flags & ASYNC_CLOSING));
 		if (port->flags & ASYNC_HUP_NOTIFY)
 			return -EAGAIN;
@@ -296,9 +296,9 @@ int tty_port_block_til_ready(struct tty_port *port,
 			retval = -ERESTARTSYS;
 			break;
 		}
-		tty_unlock();
+		tty_unlock(tty);
 		schedule();
-		tty_lock();
+		tty_lock(tty);
 	}
 	finish_wait(&port->open_wait, &wait);
 
diff --git a/drivers/tty/vt/consolemap.c b/drivers/tty/vt/consolemap.c
index 8308fc7cdc2..2aaa0c22840 100644
--- a/drivers/tty/vt/consolemap.c
+++ b/drivers/tty/vt/consolemap.c
@@ -19,6 +19,7 @@
 #include <linux/init.h>
 #include <linux/tty.h>
 #include <asm/uaccess.h>
+#include <linux/console.h>
 #include <linux/consolemap.h>
 #include <linux/vt_kern.h>
 
@@ -312,6 +313,7 @@ int con_set_trans_old(unsigned char __user * arg)
 	if (!access_ok(VERIFY_READ, arg, E_TABSZ))
 		return -EFAULT;
 
+	console_lock();
 	for (i=0; i<E_TABSZ ; i++) {
 		unsigned char uc;
 		__get_user(uc, arg+i);
@@ -319,6 +321,7 @@ int con_set_trans_old(unsigned char __user * arg)
 	}
 
 	update_user_maps();
+	console_unlock();
 	return 0;
 }
 
@@ -330,11 +333,13 @@ int con_get_trans_old(unsigned char __user * arg)
 	if (!access_ok(VERIFY_WRITE, arg, E_TABSZ))
 		return -EFAULT;
 
+	console_lock();
 	for (i=0; i<E_TABSZ ; i++)
-	  {
-	    ch = conv_uni_to_pc(vc_cons[fg_console].d, p[i]);
-	    __put_user((ch & ~0xff) ? 0 : ch, arg+i);
-	  }
+	{
+		ch = conv_uni_to_pc(vc_cons[fg_console].d, p[i]);
+		__put_user((ch & ~0xff) ? 0 : ch, arg+i);
+	}
+	console_unlock();
 	return 0;
 }
 
@@ -346,6 +351,7 @@ int con_set_trans_new(ushort __user * arg)
 	if (!access_ok(VERIFY_READ, arg, E_TABSZ*sizeof(unsigned short)))
 		return -EFAULT;
 
+	console_lock();
 	for (i=0; i<E_TABSZ ; i++) {
 		unsigned short us;
 		__get_user(us, arg+i);
@@ -353,6 +359,7 @@ int con_set_trans_new(ushort __user * arg)
 	}
 
 	update_user_maps();
+	console_unlock();
 	return 0;
 }
 
@@ -364,8 +371,10 @@ int con_get_trans_new(ushort __user * arg)
 	if (!access_ok(VERIFY_WRITE, arg, E_TABSZ*sizeof(unsigned short)))
 		return -EFAULT;
 
+	console_lock();
 	for (i=0; i<E_TABSZ ; i++)
 	  __put_user(p[i], arg+i);
+	console_unlock();
 	
 	return 0;
 }
@@ -407,6 +416,7 @@ static void con_release_unimap(struct uni_pagedir *p)
 	}
 }
 
+/* Caller must hold the console lock */
 void con_free_unimap(struct vc_data *vc)
 {
 	struct uni_pagedir *p;
@@ -487,17 +497,21 @@ con_insert_unipair(struct uni_pagedir *p, u_short unicode, u_short fontpos)
 	return 0;
 }
 
-/* ui is a leftover from using a hashtable, but might be used again */
-int con_clear_unimap(struct vc_data *vc, struct unimapinit *ui)
+/* ui is a leftover from using a hashtable, but might be used again
+   Caller must hold the lock */
+static int con_do_clear_unimap(struct vc_data *vc, struct unimapinit *ui)
 {
 	struct uni_pagedir *p, *q;
-  
+
 	p = (struct uni_pagedir *)*vc->vc_uni_pagedir_loc;
-	if (p && p->readonly) return -EIO;
+	if (p && p->readonly)
+		return -EIO;
+
 	if (!p || --p->refcount) {
 		q = kzalloc(sizeof(*p), GFP_KERNEL);
 		if (!q) {
-			if (p) p->refcount++;
+			if (p)
+				p->refcount++;
 			return -ENOMEM;
 		}
 		q->refcount=1;
@@ -511,23 +525,43 @@ int con_clear_unimap(struct vc_data *vc, struct unimapinit *ui)
 	return 0;
 }
 
+int con_clear_unimap(struct vc_data *vc, struct unimapinit *ui)
+{
+	int ret;
+	console_lock();
+	ret = con_do_clear_unimap(vc, ui);
+	console_unlock();
+	return ret;
+}
+	
 int con_set_unimap(struct vc_data *vc, ushort ct, struct unipair __user *list)
 {
 	int err = 0, err1, i;
 	struct uni_pagedir *p, *q;
 
+	console_lock();
+
 	/* Save original vc_unipagdir_loc in case we allocate a new one */
 	p = (struct uni_pagedir *)*vc->vc_uni_pagedir_loc;
-	if (p->readonly) return -EIO;
+	if (p->readonly) {
+		console_unlock();
+		return -EIO;
+	}
 	
-	if (!ct) return 0;
+	if (!ct) {
+		console_unlock();
+		return 0;
+	}
 	
 	if (p->refcount > 1) {
 		int j, k;
 		u16 **p1, *p2, l;
 		
-		err1 = con_clear_unimap(vc, NULL);
-		if (err1) return err1;
+		err1 = con_do_clear_unimap(vc, NULL);
+		if (err1) {
+			console_unlock();
+			return err1;
+		}
 		
 		/*
 		 * Since refcount was > 1, con_clear_unimap() allocated a
@@ -558,7 +592,8 @@ int con_set_unimap(struct vc_data *vc, ushort ct, struct unipair __user *list)
 						*vc->vc_uni_pagedir_loc = (unsigned long)p;
 						con_release_unimap(q);
 						kfree(q);
-						return err1;
+						console_unlock();
+						return err1; 
 					}
 				}
 			} else {
@@ -592,21 +627,30 @@ int con_set_unimap(struct vc_data *vc, ushort ct, struct unipair __user *list)
 	/*
 	 * Merge with fontmaps of any other virtual consoles.
 	 */
-	if (con_unify_unimap(vc, p))
+	if (con_unify_unimap(vc, p)) {
+		console_unlock();
 		return err;
+	}
 
 	for (i = 0; i <= 3; i++)
 		set_inverse_transl(vc, p, i); /* Update inverse translations */
 	set_inverse_trans_unicode(vc, p);
-  
+
+	console_unlock();
 	return err;
 }
 
-/* Loads the unimap for the hardware font, as defined in uni_hash.tbl.
-   The representation used was the most compact I could come up
-   with.  This routine is executed at sys_setup time, and when the
-   PIO_FONTRESET ioctl is called. */
-
+/**
+ *	con_set_default_unimap	-	set default unicode map
+ *	@vc: the console we are updating
+ *
+ *	Loads the unimap for the hardware font, as defined in uni_hash.tbl.
+ *	The representation used was the most compact I could come up
+ *	with.  This routine is executed at video setup, and when the
+ *	PIO_FONTRESET ioctl is called. 
+ *
+ *	The caller must hold the console lock
+ */
 int con_set_default_unimap(struct vc_data *vc)
 {
 	int i, j, err = 0, err1;
@@ -617,6 +661,7 @@ int con_set_default_unimap(struct vc_data *vc)
 		p = (struct uni_pagedir *)*vc->vc_uni_pagedir_loc;
 		if (p == dflt)
 			return 0;
+
 		dflt->refcount++;
 		*vc->vc_uni_pagedir_loc = (unsigned long)dflt;
 		if (p && !--p->refcount) {
@@ -628,8 +673,9 @@ int con_set_default_unimap(struct vc_data *vc)
 	
 	/* The default font is always 256 characters */
 
-	err = con_clear_unimap(vc, NULL);
-	if (err) return err;
+	err = con_do_clear_unimap(vc, NULL);
+	if (err)
+		return err;
     
 	p = (struct uni_pagedir *)*vc->vc_uni_pagedir_loc;
 	q = dfont_unitable;
@@ -654,6 +700,13 @@ int con_set_default_unimap(struct vc_data *vc)
 }
 EXPORT_SYMBOL(con_set_default_unimap);
 
+/**
+ *	con_copy_unimap		-	copy unimap between two vts
+ *	@dst_vc: target
+ *	@src_vt: source
+ *
+ *	The caller must hold the console lock when invoking this method
+ */
 int con_copy_unimap(struct vc_data *dst_vc, struct vc_data *src_vc)
 {
 	struct uni_pagedir *q;
@@ -668,13 +721,23 @@ int con_copy_unimap(struct vc_data *dst_vc, struct vc_data *src_vc)
 	*dst_vc->vc_uni_pagedir_loc = (long)q;
 	return 0;
 }
+EXPORT_SYMBOL(con_copy_unimap);
 
+/**
+ *	con_get_unimap		-	get the unicode map
+ *	@vc: the console to read from
+ *
+ *	Read the console unicode data for this console. Called from the ioctl
+ *	handlers.
+ */
 int con_get_unimap(struct vc_data *vc, ushort ct, ushort __user *uct, struct unipair __user *list)
 {
 	int i, j, k, ect;
 	u16 **p1, *p2;
 	struct uni_pagedir *p;
 
+	console_lock();
+
 	ect = 0;
 	if (*vc->vc_uni_pagedir_loc) {
 		p = (struct uni_pagedir *)*vc->vc_uni_pagedir_loc;
@@ -694,22 +757,19 @@ int con_get_unimap(struct vc_data *vc, ushort ct, ushort __user *uct, struct uni
 				}
 	}
 	__put_user(ect, uct);
+	console_unlock();
 	return ((ect <= ct) ? 0 : -ENOMEM);
 }
 
-void con_protect_unimap(struct vc_data *vc, int rdonly)
-{
-	struct uni_pagedir *p = (struct uni_pagedir *)*vc->vc_uni_pagedir_loc;
-	
-	if (p)
-		p->readonly = rdonly;
-}
-
 /*
  * Always use USER_MAP. These functions are used by the keyboard,
  * which shouldn't be affected by G0/G1 switching, etc.
  * If the user map still contains default values, i.e. the
  * direct-to-font mapping, then assume user is using Latin1.
+ *
+ * FIXME: at some point we need to decide if we want to lock the table
+ * update element itself via the keyboard_event_lock for consistency with the
+ * keyboard driver as well as the consoles
  */
 /* may be called during an interrupt */
 u32 conv_8bit_to_uni(unsigned char c)
@@ -777,4 +837,3 @@ console_map_init(void)
 			con_set_default_unimap(vc_cons[i].d);
 }
 
-EXPORT_SYMBOL(con_copy_unimap);
diff --git a/drivers/tty/vt/keyboard.c b/drivers/tty/vt/keyboard.c
index 29ca20dbd33..48cc6f25cfd 100644
--- a/drivers/tty/vt/keyboard.c
+++ b/drivers/tty/vt/keyboard.c
@@ -53,17 +53,13 @@ extern void ctrl_alt_del(void);
 
 #define KBD_DEFMODE ((1 << VC_REPEAT) | (1 << VC_META))
 
-/*
- * Some laptops take the 789uiojklm,. keys as number pad when NumLock is on.
- * This seems a good reason to start with NumLock off. On HIL keyboards
- * of PARISC machines however there is no NumLock key and everyone expects the
- * keypad to be used for numbers.
- */
-
-#if defined(CONFIG_PARISC) && (defined(CONFIG_KEYBOARD_HIL) || defined(CONFIG_KEYBOARD_HIL_OLD))
-#define KBD_DEFLEDS (1 << VC_NUMLOCK)
+#if defined(CONFIG_X86) || defined(CONFIG_PARISC)
+#include <asm/kbdleds.h>
 #else
-#define KBD_DEFLEDS 0
+static inline int kbd_defleds(void)
+{
+	return 0;
+}
 #endif
 
 #define KBD_DEFLOCK 0
@@ -1524,8 +1520,8 @@ int __init kbd_init(void)
 	int error;
 
 	for (i = 0; i < MAX_NR_CONSOLES; i++) {
-		kbd_table[i].ledflagstate = KBD_DEFLEDS;
-		kbd_table[i].default_ledflagstate = KBD_DEFLEDS;
+		kbd_table[i].ledflagstate = kbd_defleds();
+		kbd_table[i].default_ledflagstate = kbd_defleds();
 		kbd_table[i].ledmode = LED_SHOW_FLAGS;
 		kbd_table[i].lockstate = KBD_DEFLOCK;
 		kbd_table[i].slockstate = 0;
@@ -2044,7 +2040,7 @@ int vt_do_kdskled(int console, int cmd, unsigned long arg, int perm)
 		kbd->default_ledflagstate = ((arg >> 4) & 7);
 		set_leds();
                 spin_unlock_irqrestore(&kbd_event_lock, flags);
-		break;
+		return 0;
 
 	/* the ioctls below only set the lights, not the functions */
 	/* for those, see KDGKBLED and KDSKBLED above */
diff --git a/drivers/tty/vt/vt.c b/drivers/tty/vt/vt.c
index 2156188db4a..84cbf298c09 100644
--- a/drivers/tty/vt/vt.c
+++ b/drivers/tty/vt/vt.c
@@ -3892,36 +3892,6 @@ static void set_palette(struct vc_data *vc)
 		vc->vc_sw->con_set_palette(vc, color_table);
 }
 
-static int set_get_cmap(unsigned char __user *arg, int set)
-{
-    int i, j, k;
-
-    WARN_CONSOLE_UNLOCKED();
-
-    for (i = 0; i < 16; i++)
-	if (set) {
-	    get_user(default_red[i], arg++);
-	    get_user(default_grn[i], arg++);
-	    get_user(default_blu[i], arg++);
-	} else {
-	    put_user(default_red[i], arg++);
-	    put_user(default_grn[i], arg++);
-	    put_user(default_blu[i], arg++);
-	}
-    if (set) {
-	for (i = 0; i < MAX_NR_CONSOLES; i++)
-	    if (vc_cons_allocated(i)) {
-		for (j = k = 0; j < 16; j++) {
-		    vc_cons[i].d->vc_palette[k++] = default_red[j];
-		    vc_cons[i].d->vc_palette[k++] = default_grn[j];
-		    vc_cons[i].d->vc_palette[k++] = default_blu[j];
-		}
-		set_palette(vc_cons[i].d);
-	    }
-    }
-    return 0;
-}
-
 /*
  * Load palette into the DAC registers. arg points to a colour
  * map, 3 bytes per colour, 16 colours, range from 0 to 255.
@@ -3929,24 +3899,50 @@ static int set_get_cmap(unsigned char __user *arg, int set)
 
 int con_set_cmap(unsigned char __user *arg)
 {
-	int rc;
+	int i, j, k;
+	unsigned char colormap[3*16];
+
+	if (copy_from_user(colormap, arg, sizeof(colormap)))
+		return -EFAULT;
 
 	console_lock();
-	rc = set_get_cmap (arg,1);
+	for (i = k = 0; i < 16; i++) {
+		default_red[i] = colormap[k++];
+		default_grn[i] = colormap[k++];
+		default_blu[i] = colormap[k++];
+	}
+	for (i = 0; i < MAX_NR_CONSOLES; i++) {
+		if (!vc_cons_allocated(i))
+			continue;
+		for (j = k = 0; j < 16; j++) {
+			vc_cons[i].d->vc_palette[k++] = default_red[j];
+			vc_cons[i].d->vc_palette[k++] = default_grn[j];
+			vc_cons[i].d->vc_palette[k++] = default_blu[j];
+		}
+		set_palette(vc_cons[i].d);
+	}
 	console_unlock();
 
-	return rc;
+	return 0;
 }
 
 int con_get_cmap(unsigned char __user *arg)
 {
-	int rc;
+	int i, k;
+	unsigned char colormap[3*16];
 
 	console_lock();
-	rc = set_get_cmap (arg,0);
+	for (i = k = 0; i < 16; i++) {
+		colormap[k++] = default_red[i];
+		colormap[k++] = default_grn[i];
+		colormap[k++] = default_blu[i];
+	}
 	console_unlock();
 
-	return rc;
+	if (copy_to_user(arg, colormap, sizeof(colormap)))
+		return -EFAULT;
+
+	return 0;
 }
 
 void reset_palette(struct vc_data *vc)
diff --git a/drivers/tty/vt/vt_ioctl.c b/drivers/tty/vt/vt_ioctl.c
index ede2ef18d2f..64618547be1 100644
--- a/drivers/tty/vt/vt_ioctl.c
+++ b/drivers/tty/vt/vt_ioctl.c
@@ -910,7 +910,9 @@ int vt_ioctl(struct tty_struct *tty,
 		ret = con_font_op(vc_cons[fg_console].d, &op);
 		if (ret)
 			break;
+		console_lock();
 		con_set_default_unimap(vc_cons[fg_console].d);
+		console_unlock();
 		break;
 		}
 #endif
@@ -934,33 +936,23 @@ int vt_ioctl(struct tty_struct *tty,
 	case PIO_SCRNMAP:
 		if (!perm)
 			ret = -EPERM;
-		else {
-			tty_lock();
+		else
 			ret = con_set_trans_old(up);
-			tty_unlock();
-		}
 		break;
 
 	case GIO_SCRNMAP:
-		tty_lock();
 		ret = con_get_trans_old(up);
-		tty_unlock();
 		break;
 
 	case PIO_UNISCRNMAP:
 		if (!perm)
 			ret = -EPERM;
-		else {
-			tty_lock();
+		else
 			ret = con_set_trans_new(up);
-			tty_unlock();
-		}
 		break;
 
 	case GIO_UNISCRNMAP:
-		tty_lock();
 		ret = con_get_trans_new(up);
-		tty_unlock();
 		break;
 
 	case PIO_UNIMAPCLR:
@@ -970,19 +962,14 @@ int vt_ioctl(struct tty_struct *tty,
 		ret = copy_from_user(&ui, up, sizeof(struct unimapinit));
 		if (ret)
 			ret = -EFAULT;
-		else {
-			tty_lock();
+		else
 			con_clear_unimap(vc, &ui);
-			tty_unlock();
-		}
 		break;
 	      }
 
 	case PIO_UNIMAP:
 	case GIO_UNIMAP:
-		tty_lock();
 		ret = do_unimap_ioctl(cmd, up, perm, vc);
-		tty_unlock();
 		break;
 
 	case VT_LOCKSWITCH:
@@ -1196,9 +1183,7 @@ long vt_compat_ioctl(struct tty_struct *tty,
 
 	case PIO_UNIMAP:
 	case GIO_UNIMAP:
-		tty_lock();
 		ret = compat_unimap_ioctl(cmd, up, perm, vc);
-		tty_unlock();
 		break;
 
 	/*
diff --git a/drivers/uio/uio_pdrv_genirq.c b/drivers/uio/uio_pdrv_genirq.c
index b98371d93a9..42202cd8315 100644
--- a/drivers/uio/uio_pdrv_genirq.c
+++ b/drivers/uio/uio_pdrv_genirq.c
@@ -146,6 +146,14 @@ static int uio_pdrv_genirq_probe(struct platform_device *pdev)
 	priv->flags = 0; /* interrupt is enabled to begin with */
 	priv->pdev = pdev;
 
+	if (!uioinfo->irq) {
+		ret = platform_get_irq(pdev, 0);
+		if (ret < 0) {
+			dev_err(&pdev->dev, "failed to get IRQ\n");
+			goto bad0;
+		}
+		uioinfo->irq = ret;
+	}
 	uiomem = &uioinfo->mem[0];
 
 	for (i = 0; i < pdev->num_resources; ++i) {
diff --git a/drivers/usb/Kconfig b/drivers/usb/Kconfig
index 76316a33061..a7773a3e02b 100644
--- a/drivers/usb/Kconfig
+++ b/drivers/usb/Kconfig
@@ -133,6 +133,8 @@ source "drivers/usb/host/Kconfig"
 
 source "drivers/usb/musb/Kconfig"
 
+source "drivers/usb/chipidea/Kconfig"
+
 source "drivers/usb/renesas_usbhs/Kconfig"
 
 source "drivers/usb/class/Kconfig"
@@ -177,6 +179,8 @@ source "drivers/usb/serial/Kconfig"
 
 source "drivers/usb/misc/Kconfig"
 
+source "drivers/usb/phy/Kconfig"
+
 source "drivers/usb/atm/Kconfig"
 
 source "drivers/usb/gadget/Kconfig"
diff --git a/drivers/usb/Makefile b/drivers/usb/Makefile
index 53a7bc07dd8..c691eea5153 100644
--- a/drivers/usb/Makefile
+++ b/drivers/usb/Makefile
@@ -46,12 +46,14 @@ obj-$(CONFIG_USB_MICROTEK)	+= image/
 obj-$(CONFIG_USB_SERIAL)	+= serial/
 
 obj-$(CONFIG_USB)		+= misc/
+obj-$(CONFIG_USB)		+= phy/
 obj-$(CONFIG_EARLY_PRINTK_DBGP)	+= early/
 
 obj-$(CONFIG_USB_ATM)		+= atm/
 obj-$(CONFIG_USB_SPEEDTOUCH)	+= atm/
 
 obj-$(CONFIG_USB_MUSB_HDRC)	+= musb/
+obj-$(CONFIG_USB_CHIPIDEA)	+= chipidea/
 obj-$(CONFIG_USB_RENESAS_USBHS)	+= renesas_usbhs/
 obj-$(CONFIG_USB_GADGET)	+= gadget/
 
diff --git a/drivers/usb/atm/cxacru.c b/drivers/usb/atm/cxacru.c
index 98b89fe1986..b7eb86ad6bf 100644
--- a/drivers/usb/atm/cxacru.c
+++ b/drivers/usb/atm/cxacru.c
@@ -674,7 +674,7 @@ static int cxacru_cm(struct cxacru_data *instance, enum cxacru_cm_request cm,
 	}
 
 	ret = offd;
-	dbg("cm %#x", cm);
+	usb_dbg(instance->usbatm, "cm %#x\n", cm);
 fail:
 	mutex_unlock(&instance->cm_serialize);
 err:
@@ -733,7 +733,7 @@ static int cxacru_card_status(struct cxacru_data *instance)
 {
 	int ret = cxacru_cm(instance, CM_REQUEST_CARD_GET_STATUS, NULL, 0, NULL, 0);
 	if (ret < 0) {		/* firmware not loaded */
-		dbg("cxacru_adsl_start: CARD_GET_STATUS returned %d", ret);
+		usb_dbg(instance->usbatm, "cxacru_adsl_start: CARD_GET_STATUS returned %d\n", ret);
 		return ret;
 	}
 	return 0;
@@ -758,7 +758,7 @@ static int cxacru_atm_start(struct usbatm_data *usbatm_instance,
 	int ret;
 	int start_polling = 1;
 
-	dbg("cxacru_atm_start");
+	dev_dbg(&intf->dev, "%s\n", __func__);
 
 	/* Read MAC address */
 	ret = cxacru_cm(instance, CM_REQUEST_CARD_GET_MAC_ADDRESS, NULL, 0,
@@ -962,13 +962,13 @@ static int cxacru_fw(struct usb_device *usb_dev, enum cxacru_fw_request fw,
 			ret = usb_bulk_msg(usb_dev, usb_sndbulkpipe(usb_dev, CXACRU_EP_CMD),
 					   buf, offb, NULL, CMD_TIMEOUT);
 			if (ret < 0) {
-				dbg("sending fw %#x failed", fw);
+				dev_dbg(&usb_dev->dev, "sending fw %#x failed\n", fw);
 				goto cleanup;
 			}
 			offb = 0;
 		}
 	} while (offd < size);
-	dbg("sent fw %#x", fw);
+	dev_dbg(&usb_dev->dev, "sent fw %#x\n", fw);
 
 	ret = 0;
 
@@ -988,7 +988,7 @@ static void cxacru_upload_firmware(struct cxacru_data *instance,
 			       usb_dev->descriptor.idProduct };
 	__le32 val;
 
-	dbg("cxacru_upload_firmware");
+	usb_dbg(usbatm, "%s\n", __func__);
 
 	/* FirmwarePllFClkValue */
 	val = cpu_to_le32(instance->modem_type->pll_f_clk);
@@ -1074,7 +1074,7 @@ static int cxacru_find_firmware(struct cxacru_data *instance,
 	char buf[16];
 
 	sprintf(buf, "cxacru-%s.bin", phase);
-	dbg("cxacru_find_firmware: looking for %s", buf);
+	usb_dbg(usbatm, "cxacru_find_firmware: looking for %s\n", buf);
 
 	if (request_firmware(fw_p, buf, dev)) {
 		usb_dbg(usbatm, "no stage %s firmware found\n", phase);
@@ -1115,9 +1115,9 @@ static int cxacru_heavy_init(struct usbatm_data *usbatm_instance,
 
 	ret = cxacru_card_status(instance);
 	if (ret)
-		dbg("modem initialisation failed");
+		usb_dbg(usbatm_instance, "modem initialisation failed\n");
 	else
-		dbg("done setting up the modem");
+		usb_dbg(usbatm_instance, "done setting up the modem\n");
 
 	return ret;
 }
@@ -1133,7 +1133,7 @@ static int cxacru_bind(struct usbatm_data *usbatm_instance,
 	/* instance init */
 	instance = kzalloc(sizeof(*instance), GFP_KERNEL);
 	if (!instance) {
-		dbg("cxacru_bind: no memory for instance data");
+		usb_dbg(usbatm_instance, "cxacru_bind: no memory for instance data\n");
 		return -ENOMEM;
 	}
 
@@ -1149,31 +1149,31 @@ static int cxacru_bind(struct usbatm_data *usbatm_instance,
 
 	instance->rcv_buf = (u8 *) __get_free_page(GFP_KERNEL);
 	if (!instance->rcv_buf) {
-		dbg("cxacru_bind: no memory for rcv_buf");
+		usb_dbg(usbatm_instance, "cxacru_bind: no memory for rcv_buf\n");
 		ret = -ENOMEM;
 		goto fail;
 	}
 	instance->snd_buf = (u8 *) __get_free_page(GFP_KERNEL);
 	if (!instance->snd_buf) {
-		dbg("cxacru_bind: no memory for snd_buf");
+		usb_dbg(usbatm_instance, "cxacru_bind: no memory for snd_buf\n");
 		ret = -ENOMEM;
 		goto fail;
 	}
 	instance->rcv_urb = usb_alloc_urb(0, GFP_KERNEL);
 	if (!instance->rcv_urb) {
-		dbg("cxacru_bind: no memory for rcv_urb");
+		usb_dbg(usbatm_instance, "cxacru_bind: no memory for rcv_urb\n");
 		ret = -ENOMEM;
 		goto fail;
 	}
 	instance->snd_urb = usb_alloc_urb(0, GFP_KERNEL);
 	if (!instance->snd_urb) {
-		dbg("cxacru_bind: no memory for snd_urb");
+		usb_dbg(usbatm_instance, "cxacru_bind: no memory for snd_urb\n");
 		ret = -ENOMEM;
 		goto fail;
 	}
 
 	if (!cmd_ep) {
-		dbg("cxacru_bind: no command endpoint");
+		usb_dbg(usbatm_instance, "cxacru_bind: no command endpoint\n");
 		ret = -ENODEV;
 		goto fail;
 	}
@@ -1227,10 +1227,10 @@ static void cxacru_unbind(struct usbatm_data *usbatm_instance,
 	struct cxacru_data *instance = usbatm_instance->driver_data;
 	int is_polling = 1;
 
-	dbg("cxacru_unbind entered");
+	usb_dbg(usbatm_instance, "cxacru_unbind entered\n");
 
 	if (!instance) {
-		dbg("cxacru_unbind: NULL instance!");
+		usb_dbg(usbatm_instance, "cxacru_unbind: NULL instance!\n");
 		return;
 	}
 
diff --git a/drivers/usb/atm/speedtch.c b/drivers/usb/atm/speedtch.c
index 98dd9e49b68..975e9c6691d 100644
--- a/drivers/usb/atm/speedtch.c
+++ b/drivers/usb/atm/speedtch.c
@@ -170,7 +170,7 @@ static void speedtch_set_swbuff(struct speedtch_instance_data *instance, int sta
 			 "%sabling SW buffering: usb_control_msg returned %d\n",
 			 state ? "En" : "Dis", ret);
 	else
-		dbg("speedtch_set_swbuff: %sbled SW buffering", state ? "En" : "Dis");
+		usb_dbg(usbatm, "speedtch_set_swbuff: %sbled SW buffering\n", state ? "En" : "Dis");
 }
 
 static void speedtch_test_sequence(struct speedtch_instance_data *instance)
diff --git a/drivers/usb/atm/ueagle-atm.c b/drivers/usb/atm/ueagle-atm.c
index 01ea5d7421d..d7e422dc0ef 100644
--- a/drivers/usb/atm/ueagle-atm.c
+++ b/drivers/usb/atm/ueagle-atm.c
@@ -1357,10 +1357,8 @@ static int uea_stat_e1(struct uea_softc *sc)
 		/* release the dsp firmware as it is not needed until
 		 * the next failure
 		 */
-		if (sc->dsp_firm) {
-			release_firmware(sc->dsp_firm);
-			sc->dsp_firm = NULL;
-		}
+		release_firmware(sc->dsp_firm);
+		sc->dsp_firm = NULL;
 	}
 
 	/* always update it as atm layer could not be init when we switch to
@@ -1496,10 +1494,8 @@ static int uea_stat_e4(struct uea_softc *sc)
 		/* release the dsp firmware as it is not needed until
 		 * the next failure
 		 */
-		if (sc->dsp_firm) {
-			release_firmware(sc->dsp_firm);
-			sc->dsp_firm = NULL;
-		}
+		release_firmware(sc->dsp_firm);
+		sc->dsp_firm = NULL;
 	}
 
 	/* always update it as atm layer could not be init when we switch to
@@ -2240,8 +2236,7 @@ static void uea_stop(struct uea_softc *sc)
 	/* flush the work item, when no one can schedule it */
 	flush_work_sync(&sc->task);
 
-	if (sc->dsp_firm)
-		release_firmware(sc->dsp_firm);
+	release_firmware(sc->dsp_firm);
 	uea_leaves(INS_TO_USBDEV(sc));
 }
 
diff --git a/drivers/usb/atm/usbatm.c b/drivers/usb/atm/usbatm.c
index d3448ca110c..ee62b3576f9 100644
--- a/drivers/usb/atm/usbatm.c
+++ b/drivers/usb/atm/usbatm.c
@@ -86,7 +86,7 @@
 #ifdef VERBOSE_DEBUG
 static int usbatm_print_packet(const unsigned char *data, int len);
 #define PACKETDEBUG(arg...)	usbatm_print_packet(arg)
-#define vdbg(arg...)		dbg(arg)
+#define vdbg(arg...)		dev_dbg(arg)
 #else
 #define PACKETDEBUG(arg...)
 #define vdbg(arg...)
@@ -714,7 +714,7 @@ static void usbatm_destroy_instance(struct kref *kref)
 {
 	struct usbatm_data *instance = container_of(kref, struct usbatm_data, refcount);
 
-	dbg("%s", __func__);
+	usb_dbg(instance, "%s\n", __func__);
 
 	tasklet_kill(&instance->rx_channel.tasklet);
 	tasklet_kill(&instance->tx_channel.tasklet);
@@ -724,14 +724,14 @@ static void usbatm_destroy_instance(struct kref *kref)
 
 static void usbatm_get_instance(struct usbatm_data *instance)
 {
-	dbg("%s", __func__);
+	usb_dbg(instance, "%s\n", __func__);
 
 	kref_get(&instance->refcount);
 }
 
 static void usbatm_put_instance(struct usbatm_data *instance)
 {
-	dbg("%s", __func__);
+	usb_dbg(instance, "%s\n", __func__);
 
 	kref_put(&instance->refcount, usbatm_destroy_instance);
 }
@@ -745,11 +745,10 @@ static void usbatm_atm_dev_close(struct atm_dev *atm_dev)
 {
 	struct usbatm_data *instance = atm_dev->dev_data;
 
-	dbg("%s", __func__);
-
 	if (!instance)
 		return;
 
+	usb_dbg(instance, "%s\n", __func__);
 	atm_dev->dev_data = NULL; /* catch bugs */
 	usbatm_put_instance(instance);	/* taken in usbatm_atm_init */
 }
@@ -759,10 +758,8 @@ static int usbatm_atm_proc_read(struct atm_dev *atm_dev, loff_t * pos, char *pag
 	struct usbatm_data *instance = atm_dev->dev_data;
 	int left = *pos;
 
-	if (!instance) {
-		dbg("%s: NULL instance!", __func__);
+	if (!instance)
 		return -ENODEV;
-	}
 
 	if (!left--)
 		return sprintf(page, "%s\n", instance->description);
@@ -804,10 +801,8 @@ static int usbatm_atm_open(struct atm_vcc *vcc)
 	int vci = vcc->vci;
 	short vpi = vcc->vpi;
 
-	if (!instance) {
-		dbg("%s: NULL data!", __func__);
+	if (!instance)
 		return -ENODEV;
-	}
 
 	atm_dbg(instance, "%s: vpi %hd, vci %d\n", __func__, vpi, vci);
 
@@ -884,10 +879,8 @@ static void usbatm_atm_close(struct atm_vcc *vcc)
 	struct usbatm_data *instance = vcc->dev->dev_data;
 	struct usbatm_vcc_data *vcc_data = vcc->dev_data;
 
-	if (!instance || !vcc_data) {
-		dbg("%s: NULL data!", __func__);
+	if (!instance || !vcc_data)
 		return;
-	}
 
 	atm_dbg(instance, "%s entered\n", __func__);
 
@@ -929,10 +922,8 @@ static int usbatm_atm_ioctl(struct atm_dev *atm_dev, unsigned int cmd,
 {
 	struct usbatm_data *instance = atm_dev->dev_data;
 
-	if (!instance || instance->disconnected) {
-		dbg("%s: %s!", __func__, instance ? "disconnected" : "NULL instance");
+	if (!instance || instance->disconnected)
 		return -ENODEV;
-	}
 
 	switch (cmd) {
 	case ATM_QUERYLOOP:
@@ -1336,8 +1327,6 @@ EXPORT_SYMBOL_GPL(usbatm_usb_disconnect);
 
 static int __init usbatm_usb_init(void)
 {
-	dbg("%s: driver version %s", __func__, DRIVER_VERSION);
-
 	if (sizeof(struct usbatm_control) > FIELD_SIZEOF(struct sk_buff, cb)) {
 		printk(KERN_ERR "%s unusable with this kernel!\n", usbatm_driver_name);
 		return -EIO;
@@ -1357,7 +1346,6 @@ module_init(usbatm_usb_init);
 
 static void __exit usbatm_usb_exit(void)
 {
-	dbg("%s", __func__);
 }
 module_exit(usbatm_usb_exit);
 
diff --git a/drivers/usb/atm/xusbatm.c b/drivers/usb/atm/xusbatm.c
index 48ee0c5ff28..14ec9f0c592 100644
--- a/drivers/usb/atm/xusbatm.c
+++ b/drivers/usb/atm/xusbatm.c
@@ -187,8 +187,6 @@ static int __init xusbatm_init(void)
 {
 	int i;
 
-	dbg("xusbatm_init");
-
 	if (!num_vendor ||
 	    num_vendor != num_product ||
 	    num_vendor != num_rx_endpoint ||
@@ -221,8 +219,6 @@ module_init(xusbatm_init);
 
 static void __exit xusbatm_exit(void)
 {
-	dbg("xusbatm_exit entered");
-
 	usb_deregister(&xusbatm_usb_driver);
 }
 module_exit(xusbatm_exit);
diff --git a/drivers/usb/chipidea/Kconfig b/drivers/usb/chipidea/Kconfig
new file mode 100644
index 00000000000..fd36dc8b889
--- /dev/null
+++ b/drivers/usb/chipidea/Kconfig
@@ -0,0 +1,32 @@
+config USB_CHIPIDEA
+	tristate "ChipIdea Highspeed Dual Role Controller"
+	depends on USB
+	help
+          Say Y here if your system has a dual role high speed USB
+          controller based on ChipIdea silicon IP. Currently, only the
+	  peripheral mode is supported.
+
+	  When compiled dynamically, the module will be called ci-hdrc.ko.
+
+if USB_CHIPIDEA
+
+config USB_CHIPIDEA_UDC
+	bool "ChipIdea device controller"
+	depends on USB_GADGET
+	select USB_GADGET_DUALSPEED
+	help
+	  Say Y here to enable device controller functionality of the
+	  ChipIdea driver.
+
+config USB_CHIPIDEA_HOST
+	bool "ChipIdea host controller"
+	help
+	  Say Y here to enable host controller functionality of the
+	  ChipIdea driver.
+
+config USB_CHIPIDEA_DEBUG
+	bool "ChipIdea driver debug"
+	help
+	  Say Y here to enable debugging output of the ChipIdea driver.
+
+endif
diff --git a/drivers/usb/chipidea/Makefile b/drivers/usb/chipidea/Makefile
new file mode 100644
index 00000000000..cc349376972
--- /dev/null
+++ b/drivers/usb/chipidea/Makefile
@@ -0,0 +1,14 @@
+obj-$(CONFIG_USB_CHIPIDEA)		+= ci_hdrc.o
+
+ci_hdrc-y				:= core.o
+ci_hdrc-$(CONFIG_USB_CHIPIDEA_UDC)	+= udc.o
+ci_hdrc-$(CONFIG_USB_CHIPIDEA_HOST)	+= host.o
+ci_hdrc-$(CONFIG_USB_CHIPIDEA_DEBUG)	+= debug.o
+
+ifneq ($(CONFIG_PCI),)
+	obj-$(CONFIG_USB_CHIPIDEA)	+= ci13xxx_pci.o
+endif
+
+ifneq ($(CONFIG_ARCH_MSM),)
+	obj-$(CONFIG_USB_CHIPIDEA)	+= ci13xxx_msm.o
+endif
diff --git a/drivers/usb/chipidea/bits.h b/drivers/usb/chipidea/bits.h
new file mode 100644
index 00000000000..050de8562a0
--- /dev/null
+++ b/drivers/usb/chipidea/bits.h
@@ -0,0 +1,90 @@
+/*
+ * bits.h - register bits of the ChipIdea USB IP core
+ *
+ * Copyright (C) 2008 Chipidea - MIPS Technologies, Inc. All rights reserved.
+ *
+ * Author: David Lopo
+ *
+ * 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 __DRIVERS_USB_CHIPIDEA_BITS_H
+#define __DRIVERS_USB_CHIPIDEA_BITS_H
+
+#include <linux/usb/ehci_def.h>
+
+/* HCCPARAMS */
+#define HCCPARAMS_LEN         BIT(17)
+
+/* DCCPARAMS */
+#define DCCPARAMS_DEN         (0x1F << 0)
+#define DCCPARAMS_DC          BIT(7)
+#define DCCPARAMS_HC          BIT(8)
+
+/* TESTMODE */
+#define TESTMODE_FORCE        BIT(0)
+
+/* USBCMD */
+#define USBCMD_RS             BIT(0)
+#define USBCMD_RST            BIT(1)
+#define USBCMD_SUTW           BIT(13)
+#define USBCMD_ATDTW          BIT(14)
+
+/* USBSTS & USBINTR */
+#define USBi_UI               BIT(0)
+#define USBi_UEI              BIT(1)
+#define USBi_PCI              BIT(2)
+#define USBi_URI              BIT(6)
+#define USBi_SLI              BIT(8)
+
+/* DEVICEADDR */
+#define DEVICEADDR_USBADRA    BIT(24)
+#define DEVICEADDR_USBADR     (0x7FUL << 25)
+
+/* PORTSC */
+#define PORTSC_FPR            BIT(6)
+#define PORTSC_SUSP           BIT(7)
+#define PORTSC_HSP            BIT(9)
+#define PORTSC_PTC            (0x0FUL << 16)
+
+/* DEVLC */
+#define DEVLC_PSPD            (0x03UL << 25)
+#define    DEVLC_PSPD_HS      (0x02UL << 25)
+
+/* OTGSC */
+#define OTGSC_IDPU	      BIT(5)
+#define OTGSC_ID	      BIT(8)
+#define OTGSC_AVV	      BIT(9)
+#define OTGSC_ASV	      BIT(10)
+#define OTGSC_BSV	      BIT(11)
+#define OTGSC_BSE	      BIT(12)
+#define OTGSC_IDIS	      BIT(16)
+#define OTGSC_AVVIS	      BIT(17)
+#define OTGSC_ASVIS	      BIT(18)
+#define OTGSC_BSVIS	      BIT(19)
+#define OTGSC_BSEIS	      BIT(20)
+#define OTGSC_IDIE	      BIT(24)
+#define OTGSC_AVVIE	      BIT(25)
+#define OTGSC_ASVIE	      BIT(26)
+#define OTGSC_BSVIE	      BIT(27)
+#define OTGSC_BSEIE	      BIT(28)
+
+/* USBMODE */
+#define USBMODE_CM            (0x03UL <<  0)
+#define USBMODE_CM_DC         (0x02UL <<  0)
+#define USBMODE_SLOM          BIT(3)
+#define USBMODE_CI_SDIS       BIT(4)
+
+/* ENDPTCTRL */
+#define ENDPTCTRL_RXS         BIT(0)
+#define ENDPTCTRL_RXT         (0x03UL <<  2)
+#define ENDPTCTRL_RXR         BIT(6)         /* reserved for port 0 */
+#define ENDPTCTRL_RXE         BIT(7)
+#define ENDPTCTRL_TXS         BIT(16)
+#define ENDPTCTRL_TXT         (0x03UL << 18)
+#define ENDPTCTRL_TXR         BIT(22)        /* reserved for port 0 */
+#define ENDPTCTRL_TXE         BIT(23)
+
+#endif /* __DRIVERS_USB_CHIPIDEA_BITS_H */
diff --git a/drivers/usb/chipidea/ci.h b/drivers/usb/chipidea/ci.h
new file mode 100644
index 00000000000..50911f8490d
--- /dev/null
+++ b/drivers/usb/chipidea/ci.h
@@ -0,0 +1,313 @@
+/*
+ * ci.h - common structures, functions, and macros of the ChipIdea driver
+ *
+ * Copyright (C) 2008 Chipidea - MIPS Technologies, Inc. All rights reserved.
+ *
+ * Author: David Lopo
+ *
+ * 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 __DRIVERS_USB_CHIPIDEA_CI_H
+#define __DRIVERS_USB_CHIPIDEA_CI_H
+
+#include <linux/list.h>
+#include <linux/irqreturn.h>
+#include <linux/usb.h>
+#include <linux/usb/gadget.h>
+
+/******************************************************************************
+ * DEFINE
+ *****************************************************************************/
+#define CI13XXX_PAGE_SIZE  4096ul /* page size for TD's */
+#define ENDPT_MAX          32
+
+/******************************************************************************
+ * STRUCTURES
+ *****************************************************************************/
+/**
+ * struct ci13xxx_ep - endpoint representation
+ * @ep: endpoint structure for gadget drivers
+ * @dir: endpoint direction (TX/RX)
+ * @num: endpoint number
+ * @type: endpoint type
+ * @name: string description of the endpoint
+ * @qh: queue head for this endpoint
+ * @wedge: is the endpoint wedged
+ * @udc: pointer to the controller
+ * @lock: pointer to controller's spinlock
+ * @td_pool: pointer to controller's TD pool
+ */
+struct ci13xxx_ep {
+	struct usb_ep				ep;
+	u8					dir;
+	u8					num;
+	u8					type;
+	char					name[16];
+	struct {
+		struct list_head	queue;
+		struct ci13xxx_qh	*ptr;
+		dma_addr_t		dma;
+	}					qh;
+	int					wedge;
+
+	/* global resources */
+	struct ci13xxx				*udc;
+	spinlock_t				*lock;
+	struct dma_pool				*td_pool;
+};
+
+enum ci_role {
+	CI_ROLE_HOST = 0,
+	CI_ROLE_GADGET,
+	CI_ROLE_END,
+};
+
+/**
+ * struct ci_role_driver - host/gadget role driver
+ * start: start this role
+ * stop: stop this role
+ * irq: irq handler for this role
+ * name: role name string (host/gadget)
+ */
+struct ci_role_driver {
+	int		(*start)(struct ci13xxx *);
+	void		(*stop)(struct ci13xxx *);
+	irqreturn_t	(*irq)(struct ci13xxx *);
+	const char	*name;
+};
+
+/**
+ * struct hw_bank - hardware register mapping representation
+ * @lpm: set if the device is LPM capable
+ * @phys: physical address of the controller's registers
+ * @abs: absolute address of the beginning of register window
+ * @cap: capability registers
+ * @op: operational registers
+ * @size: size of the register window
+ * @regmap: register lookup table
+ */
+struct hw_bank {
+	unsigned	lpm;
+	resource_size_t	phys;
+	void __iomem	*abs;
+	void __iomem	*cap;
+	void __iomem	*op;
+	size_t		size;
+	void __iomem	**regmap;
+};
+
+/**
+ * struct ci13xxx - chipidea device representation
+ * @dev: pointer to parent device
+ * @lock: access synchronization
+ * @hw_bank: hardware register mapping
+ * @irq: IRQ number
+ * @roles: array of supported roles for this controller
+ * @role: current role
+ * @is_otg: if the device is otg-capable
+ * @work: work for role changing
+ * @wq: workqueue thread
+ * @qh_pool: allocation pool for queue heads
+ * @td_pool: allocation pool for transfer descriptors
+ * @gadget: device side representation for peripheral controller
+ * @driver: gadget driver
+ * @hw_ep_max: total number of endpoints supported by hardware
+ * @ci13xxx_ep: array of endpoints
+ * @ep0_dir: ep0 direction
+ * @ep0out: pointer to ep0 OUT endpoint
+ * @ep0in: pointer to ep0 IN endpoint
+ * @status: ep0 status request
+ * @setaddr: if we should set the address on status completion
+ * @address: usb address received from the host
+ * @remote_wakeup: host-enabled remote wakeup
+ * @suspended: suspended by host
+ * @test_mode: the selected test mode
+ * @udc_driver: platform specific information supplied by parent device
+ * @vbus_active: is VBUS active
+ * @transceiver: pointer to USB PHY, if any
+ * @hcd: pointer to usb_hcd for ehci host driver
+ */
+struct ci13xxx {
+	struct device			*dev;
+	spinlock_t			lock;
+	struct hw_bank			hw_bank;
+	int				irq;
+	struct ci_role_driver		*roles[CI_ROLE_END];
+	enum ci_role			role;
+	bool				is_otg;
+	struct work_struct		work;
+	struct workqueue_struct		*wq;
+
+	struct dma_pool			*qh_pool;
+	struct dma_pool			*td_pool;
+
+	struct usb_gadget		gadget;
+	struct usb_gadget_driver	*driver;
+	unsigned			hw_ep_max;
+	struct ci13xxx_ep		ci13xxx_ep[ENDPT_MAX];
+	u32				ep0_dir;
+	struct ci13xxx_ep		*ep0out, *ep0in;
+
+	struct usb_request		*status;
+	bool				setaddr;
+	u8				address;
+	u8				remote_wakeup;
+	u8				suspended;
+	u8				test_mode;
+
+	struct ci13xxx_udc_driver	*udc_driver;
+	int				vbus_active;
+	struct usb_phy			*transceiver;
+	struct usb_hcd			*hcd;
+};
+
+static inline struct ci_role_driver *ci_role(struct ci13xxx *ci)
+{
+	BUG_ON(ci->role >= CI_ROLE_END || !ci->roles[ci->role]);
+	return ci->roles[ci->role];
+}
+
+static inline int ci_role_start(struct ci13xxx *ci, enum ci_role role)
+{
+	int ret;
+
+	if (role >= CI_ROLE_END)
+		return -EINVAL;
+
+	if (!ci->roles[role])
+		return -ENXIO;
+
+	ret = ci->roles[role]->start(ci);
+	if (!ret)
+		ci->role = role;
+	return ret;
+}
+
+static inline void ci_role_stop(struct ci13xxx *ci)
+{
+	enum ci_role role = ci->role;
+
+	if (role == CI_ROLE_END)
+		return;
+
+	ci->role = CI_ROLE_END;
+
+	ci->roles[role]->stop(ci);
+}
+
+/******************************************************************************
+ * REGISTERS
+ *****************************************************************************/
+/* register size */
+#define REG_BITS   (32)
+
+/* register indices */
+enum ci13xxx_regs {
+	CAP_CAPLENGTH,
+	CAP_HCCPARAMS,
+	CAP_DCCPARAMS,
+	CAP_TESTMODE,
+	CAP_LAST = CAP_TESTMODE,
+	OP_USBCMD,
+	OP_USBSTS,
+	OP_USBINTR,
+	OP_DEVICEADDR,
+	OP_ENDPTLISTADDR,
+	OP_PORTSC,
+	OP_DEVLC,
+	OP_OTGSC,
+	OP_USBMODE,
+	OP_ENDPTSETUPSTAT,
+	OP_ENDPTPRIME,
+	OP_ENDPTFLUSH,
+	OP_ENDPTSTAT,
+	OP_ENDPTCOMPLETE,
+	OP_ENDPTCTRL,
+	/* endptctrl1..15 follow */
+	OP_LAST = OP_ENDPTCTRL + ENDPT_MAX / 2,
+};
+
+/**
+ * ffs_nr: find first (least significant) bit set
+ * @x: the word to search
+ *
+ * This function returns bit number (instead of position)
+ */
+static inline int ffs_nr(u32 x)
+{
+	int n = ffs(x);
+
+	return n ? n-1 : 32;
+}
+
+/**
+ * hw_read: reads from a hw register
+ * @reg:  register index
+ * @mask: bitfield mask
+ *
+ * This function returns register contents
+ */
+static inline u32 hw_read(struct ci13xxx *udc, enum ci13xxx_regs reg, u32 mask)
+{
+	return ioread32(udc->hw_bank.regmap[reg]) & mask;
+}
+
+/**
+ * hw_write: writes to a hw register
+ * @reg:  register index
+ * @mask: bitfield mask
+ * @data: new value
+ */
+static inline void hw_write(struct ci13xxx *udc, enum ci13xxx_regs reg,
+			    u32 mask, u32 data)
+{
+	if (~mask)
+		data = (ioread32(udc->hw_bank.regmap[reg]) & ~mask)
+			| (data & mask);
+
+	iowrite32(data, udc->hw_bank.regmap[reg]);
+}
+
+/**
+ * hw_test_and_clear: tests & clears a hw register
+ * @reg:  register index
+ * @mask: bitfield mask
+ *
+ * This function returns register contents
+ */
+static inline u32 hw_test_and_clear(struct ci13xxx *udc, enum ci13xxx_regs reg,
+				    u32 mask)
+{
+	u32 val = ioread32(udc->hw_bank.regmap[reg]) & mask;
+
+	iowrite32(val, udc->hw_bank.regmap[reg]);
+	return val;
+}
+
+/**
+ * hw_test_and_write: tests & writes a hw register
+ * @reg:  register index
+ * @mask: bitfield mask
+ * @data: new value
+ *
+ * This function returns register contents
+ */
+static inline u32 hw_test_and_write(struct ci13xxx *udc, enum ci13xxx_regs reg,
+				    u32 mask, u32 data)
+{
+	u32 val = hw_read(udc, reg, ~0);
+
+	hw_write(udc, reg, mask, data);
+	return (val & mask) >> ffs_nr(mask);
+}
+
+int hw_device_reset(struct ci13xxx *ci, u32 mode);
+
+int hw_port_test_set(struct ci13xxx *ci, u8 mode);
+
+u8 hw_port_test_get(struct ci13xxx *ci);
+
+#endif	/* __DRIVERS_USB_CHIPIDEA_CI_H */
diff --git a/drivers/usb/gadget/ci13xxx_msm.c b/drivers/usb/chipidea/ci13xxx_msm.c
index d07e44c05e9..958069ef95e 100644
--- a/drivers/usb/gadget/ci13xxx_msm.c
+++ b/drivers/usb/chipidea/ci13xxx_msm.c
@@ -10,15 +10,12 @@
 #include <linux/pm_runtime.h>
 #include <linux/usb/msm_hsusb_hw.h>
 #include <linux/usb/ulpi.h>
+#include <linux/usb/gadget.h>
+#include <linux/usb/chipidea.h>
 
-#include "ci13xxx_udc.c"
+#include "ci.h"
 
-#define MSM_USB_BASE	(udc->regs)
-
-static irqreturn_t msm_udc_irq(int irq, void *data)
-{
-	return udc_irq();
-}
+#define MSM_USB_BASE	(udc->hw_bank.abs)
 
 static void ci13xxx_msm_notify_event(struct ci13xxx *udc, unsigned event)
 {
@@ -60,53 +57,40 @@ static struct ci13xxx_udc_driver ci13xxx_msm_udc_driver = {
 
 static int ci13xxx_msm_probe(struct platform_device *pdev)
 {
-	struct resource *res;
-	void __iomem *regs;
-	int irq;
+	struct platform_device *plat_ci;
 	int ret;
 
 	dev_dbg(&pdev->dev, "ci13xxx_msm_probe\n");
 
-	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
-	if (!res) {
-		dev_err(&pdev->dev, "failed to get platform resource mem\n");
-		return -ENXIO;
-	}
-
-	regs = ioremap(res->start, resource_size(res));
-	if (!regs) {
-		dev_err(&pdev->dev, "ioremap failed\n");
+	plat_ci = platform_device_alloc("ci_hdrc", -1);
+	if (!plat_ci) {
+		dev_err(&pdev->dev, "can't allocate ci_hdrc platform device\n");
 		return -ENOMEM;
 	}
 
-	ret = udc_probe(&ci13xxx_msm_udc_driver, &pdev->dev, regs);
-	if (ret < 0) {
-		dev_err(&pdev->dev, "udc_probe failed\n");
-		goto iounmap;
+	ret = platform_device_add_resources(plat_ci, pdev->resource,
+					    pdev->num_resources);
+	if (ret) {
+		dev_err(&pdev->dev, "can't add resources to platform device\n");
+		goto put_platform;
 	}
 
-	irq = platform_get_irq(pdev, 0);
-	if (irq < 0) {
-		dev_err(&pdev->dev, "IRQ not found\n");
-		ret = -ENXIO;
-		goto udc_remove;
-	}
+	ret = platform_device_add_data(plat_ci, &ci13xxx_msm_udc_driver,
+				       sizeof(ci13xxx_msm_udc_driver));
+	if (ret)
+		goto put_platform;
 
-	ret = request_irq(irq, msm_udc_irq, IRQF_SHARED, pdev->name, pdev);
-	if (ret < 0) {
-		dev_err(&pdev->dev, "request_irq failed\n");
-		goto udc_remove;
-	}
+	ret = platform_device_add(plat_ci);
+	if (ret)
+		goto put_platform;
 
 	pm_runtime_no_callbacks(&pdev->dev);
 	pm_runtime_enable(&pdev->dev);
 
 	return 0;
 
-udc_remove:
-	udc_remove();
-iounmap:
-	iounmap(regs);
+put_platform:
+	platform_device_put(plat_ci);
 
 	return ret;
 }
diff --git a/drivers/usb/gadget/ci13xxx_pci.c b/drivers/usb/chipidea/ci13xxx_pci.c
index 883ab5e832d..e3dab27f5c7 100644
--- a/drivers/usb/gadget/ci13xxx_pci.c
+++ b/drivers/usb/chipidea/ci13xxx_pci.c
@@ -10,10 +10,12 @@
  * published by the Free Software Foundation.
  */
 
+#include <linux/platform_device.h>
 #include <linux/module.h>
 #include <linux/pci.h>
-
-#include "ci13xxx_udc.c"
+#include <linux/interrupt.h>
+#include <linux/usb/gadget.h>
+#include <linux/usb/chipidea.h>
 
 /* driver name */
 #define UDC_DRIVER_NAME   "ci13xxx_pci"
@@ -21,25 +23,20 @@
 /******************************************************************************
  * PCI block
  *****************************************************************************/
-/**
- * ci13xxx_pci_irq: interrut handler
- * @irq:  irq number
- * @pdev: USB Device Controller interrupt source
- *
- * This function returns IRQ_HANDLED if the IRQ has been handled
- * This is an ISR don't trace, use attribute interface instead
- */
-static irqreturn_t ci13xxx_pci_irq(int irq, void *pdev)
-{
-	if (irq == 0) {
-		dev_err(&((struct pci_dev *)pdev)->dev, "Invalid IRQ0 usage!");
-		return IRQ_HANDLED;
-	}
-	return udc_irq();
-}
+struct ci13xxx_udc_driver pci_driver = {
+	.name		= UDC_DRIVER_NAME,
+	.capoffset	= DEF_CAPOFFSET,
+};
 
-static struct ci13xxx_udc_driver ci13xxx_pci_udc_driver = {
+struct ci13xxx_udc_driver langwell_pci_driver = {
 	.name		= UDC_DRIVER_NAME,
+	.capoffset	= 0,
+};
+
+struct ci13xxx_udc_driver penwell_pci_driver = {
+	.name		= UDC_DRIVER_NAME,
+	.capoffset	= 0,
+	.power_budget	= 200,
 };
 
 /**
@@ -54,11 +51,15 @@ static struct ci13xxx_udc_driver ci13xxx_pci_udc_driver = {
 static int __devinit ci13xxx_pci_probe(struct pci_dev *pdev,
 				       const struct pci_device_id *id)
 {
-	void __iomem *regs = NULL;
-	int retval = 0;
-
-	if (id == NULL)
-		return -EINVAL;
+	struct ci13xxx_udc_driver *driver = (void *)id->driver_data;
+	struct platform_device *plat_ci;
+	struct resource res[3];
+	int retval = 0, nres = 2;
+
+	if (!driver) {
+		dev_err(&pdev->dev, "device doesn't provide driver data\n");
+		return -ENODEV;
+	}
 
 	retval = pci_enable_device(pdev);
 	if (retval)
@@ -70,41 +71,50 @@ static int __devinit ci13xxx_pci_probe(struct pci_dev *pdev,
 		goto disable_device;
 	}
 
-	retval = pci_request_regions(pdev, UDC_DRIVER_NAME);
-	if (retval)
-		goto disable_device;
+	pci_set_power_state(pdev, PCI_D0);
+	pci_set_master(pdev);
+	pci_try_set_mwi(pdev);
 
-	/* BAR 0 holds all the registers */
-	regs = pci_iomap(pdev, 0, 0);
-	if (!regs) {
-		dev_err(&pdev->dev, "Error mapping memory!");
-		retval = -EFAULT;
-		goto release_regions;
+	plat_ci = platform_device_alloc("ci_hdrc", -1);
+	if (!plat_ci) {
+		dev_err(&pdev->dev, "can't allocate ci_hdrc platform device\n");
+		retval = -ENOMEM;
+		goto disable_device;
 	}
-	pci_set_drvdata(pdev, (__force void *)regs);
 
-	pci_set_master(pdev);
-	pci_try_set_mwi(pdev);
+	memset(res, 0, sizeof(res));
+	res[0].start	= pci_resource_start(pdev, 0);
+	res[0].end	= pci_resource_end(pdev, 0);
+	res[0].flags	= IORESOURCE_MEM;
+	res[1].start	= pdev->irq;
+	res[1].flags	= IORESOURCE_IRQ;
+
+	retval = platform_device_add_resources(plat_ci, res, nres);
+	if (retval) {
+		dev_err(&pdev->dev, "can't add resources to platform device\n");
+		goto put_platform;
+	}
 
-	retval = udc_probe(&ci13xxx_pci_udc_driver, &pdev->dev, regs);
+	retval = platform_device_add_data(plat_ci, driver, sizeof(*driver));
 	if (retval)
-		goto iounmap;
+		goto put_platform;
+
+	dma_set_coherent_mask(&plat_ci->dev, pdev->dev.coherent_dma_mask);
+	plat_ci->dev.dma_mask = pdev->dev.dma_mask;
+	plat_ci->dev.dma_parms = pdev->dev.dma_parms;
+	plat_ci->dev.parent = &pdev->dev;
 
-	/* our device does not have MSI capability */
+	pci_set_drvdata(pdev, plat_ci);
 
-	retval = request_irq(pdev->irq, ci13xxx_pci_irq, IRQF_SHARED,
-			     UDC_DRIVER_NAME, pdev);
+	retval = platform_device_add(plat_ci);
 	if (retval)
-		goto gadget_remove;
+		goto put_platform;
 
 	return 0;
 
- gadget_remove:
-	udc_remove();
- iounmap:
-	pci_iounmap(pdev, regs);
- release_regions:
-	pci_release_regions(pdev);
+ put_platform:
+	pci_set_drvdata(pdev, NULL);
+	platform_device_put(plat_ci);
  disable_device:
 	pci_disable_device(pdev);
  done:
@@ -121,10 +131,10 @@ static int __devinit ci13xxx_pci_probe(struct pci_dev *pdev,
  */
 static void __devexit ci13xxx_pci_remove(struct pci_dev *pdev)
 {
-	free_irq(pdev->irq, pdev);
-	udc_remove();
-	pci_iounmap(pdev, (__force void __iomem *)pci_get_drvdata(pdev));
-	pci_release_regions(pdev);
+	struct platform_device *plat_ci = pci_get_drvdata(pdev);
+
+	platform_device_unregister(plat_ci);
+	pci_set_drvdata(pdev, NULL);
 	pci_disable_device(pdev);
 }
 
@@ -135,8 +145,22 @@ static void __devexit ci13xxx_pci_remove(struct pci_dev *pdev)
  * Check "pci.h" for details
  */
 static DEFINE_PCI_DEVICE_TABLE(ci13xxx_pci_id_table) = {
-	{ PCI_DEVICE(0x153F, 0x1004) },
-	{ PCI_DEVICE(0x153F, 0x1006) },
+	{
+		PCI_DEVICE(0x153F, 0x1004),
+		.driver_data = (kernel_ulong_t)&pci_driver,
+	},
+	{
+		PCI_DEVICE(0x153F, 0x1006),
+		.driver_data = (kernel_ulong_t)&pci_driver,
+	},
+	{
+		PCI_DEVICE(PCI_VENDOR_ID_INTEL, 0x0811),
+		.driver_data = (kernel_ulong_t)&langwell_pci_driver,
+	},
+	{
+		PCI_DEVICE(PCI_VENDOR_ID_INTEL, 0x0829),
+		.driver_data = (kernel_ulong_t)&penwell_pci_driver,
+	},
 	{ 0, 0, 0, 0, 0, 0, 0 /* end: all zeroes */ }
 };
 MODULE_DEVICE_TABLE(pci, ci13xxx_pci_id_table);
@@ -148,27 +172,7 @@ static struct pci_driver ci13xxx_pci_driver = {
 	.remove       =	__devexit_p(ci13xxx_pci_remove),
 };
 
-/**
- * ci13xxx_pci_init: module init
- *
- * Driver load
- */
-static int __init ci13xxx_pci_init(void)
-{
-	return pci_register_driver(&ci13xxx_pci_driver);
-}
-module_init(ci13xxx_pci_init);
-
-/**
- * ci13xxx_pci_exit: module exit
- *
- * Driver unload
- */
-static void __exit ci13xxx_pci_exit(void)
-{
-	pci_unregister_driver(&ci13xxx_pci_driver);
-}
-module_exit(ci13xxx_pci_exit);
+module_pci_driver(ci13xxx_pci_driver);
 
 MODULE_AUTHOR("MIPS - David Lopo <dlopo@chipidea.mips.com>");
 MODULE_DESCRIPTION("MIPS CI13XXX USB Peripheral Controller");
diff --git a/drivers/usb/chipidea/core.c b/drivers/usb/chipidea/core.c
new file mode 100644
index 00000000000..15e03b308f8
--- /dev/null
+++ b/drivers/usb/chipidea/core.c
@@ -0,0 +1,474 @@
+/*
+ * core.c - ChipIdea USB IP core family device controller
+ *
+ * Copyright (C) 2008 Chipidea - MIPS Technologies, Inc. All rights reserved.
+ *
+ * Author: David Lopo
+ *
+ * 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.
+ */
+
+/*
+ * Description: ChipIdea USB IP core family device controller
+ *
+ * This driver is composed of several blocks:
+ * - HW:     hardware interface
+ * - DBG:    debug facilities (optional)
+ * - UTIL:   utilities
+ * - ISR:    interrupts handling
+ * - ENDPT:  endpoint operations (Gadget API)
+ * - GADGET: gadget operations (Gadget API)
+ * - BUS:    bus glue code, bus abstraction layer
+ *
+ * Compile Options
+ * - CONFIG_USB_GADGET_DEBUG_FILES: enable debug facilities
+ * - STALL_IN:  non-empty bulk-in pipes cannot be halted
+ *              if defined mass storage compliance succeeds but with warnings
+ *              => case 4: Hi >  Dn
+ *              => case 5: Hi >  Di
+ *              => case 8: Hi <> Do
+ *              if undefined usbtest 13 fails
+ * - TRACE:     enable function tracing (depends on DEBUG)
+ *
+ * Main Features
+ * - Chapter 9 & Mass Storage Compliance with Gadget File Storage
+ * - Chapter 9 Compliance with Gadget Zero (STALL_IN undefined)
+ * - Normal & LPM support
+ *
+ * USBTEST Report
+ * - OK: 0-12, 13 (STALL_IN defined) & 14
+ * - Not Supported: 15 & 16 (ISO)
+ *
+ * TODO List
+ * - OTG
+ * - Isochronous & Interrupt Traffic
+ * - Handle requests which spawns into several TDs
+ * - GET_STATUS(device) - always reports 0
+ * - Gadget API (majority of optional features)
+ * - Suspend & Remote Wakeup
+ */
+#include <linux/delay.h>
+#include <linux/device.h>
+#include <linux/dmapool.h>
+#include <linux/dma-mapping.h>
+#include <linux/init.h>
+#include <linux/platform_device.h>
+#include <linux/module.h>
+#include <linux/interrupt.h>
+#include <linux/io.h>
+#include <linux/irq.h>
+#include <linux/kernel.h>
+#include <linux/slab.h>
+#include <linux/pm_runtime.h>
+#include <linux/usb/ch9.h>
+#include <linux/usb/gadget.h>
+#include <linux/usb/otg.h>
+#include <linux/usb/chipidea.h>
+
+#include "ci.h"
+#include "udc.h"
+#include "bits.h"
+#include "host.h"
+#include "debug.h"
+
+/* Controller register map */
+static uintptr_t ci_regs_nolpm[] = {
+	[CAP_CAPLENGTH]		= 0x000UL,
+	[CAP_HCCPARAMS]		= 0x008UL,
+	[CAP_DCCPARAMS]		= 0x024UL,
+	[CAP_TESTMODE]		= 0x038UL,
+	[OP_USBCMD]		= 0x000UL,
+	[OP_USBSTS]		= 0x004UL,
+	[OP_USBINTR]		= 0x008UL,
+	[OP_DEVICEADDR]		= 0x014UL,
+	[OP_ENDPTLISTADDR]	= 0x018UL,
+	[OP_PORTSC]		= 0x044UL,
+	[OP_DEVLC]		= 0x084UL,
+	[OP_OTGSC]		= 0x064UL,
+	[OP_USBMODE]		= 0x068UL,
+	[OP_ENDPTSETUPSTAT]	= 0x06CUL,
+	[OP_ENDPTPRIME]		= 0x070UL,
+	[OP_ENDPTFLUSH]		= 0x074UL,
+	[OP_ENDPTSTAT]		= 0x078UL,
+	[OP_ENDPTCOMPLETE]	= 0x07CUL,
+	[OP_ENDPTCTRL]		= 0x080UL,
+};
+
+static uintptr_t ci_regs_lpm[] = {
+	[CAP_CAPLENGTH]		= 0x000UL,
+	[CAP_HCCPARAMS]		= 0x008UL,
+	[CAP_DCCPARAMS]		= 0x024UL,
+	[CAP_TESTMODE]		= 0x0FCUL,
+	[OP_USBCMD]		= 0x000UL,
+	[OP_USBSTS]		= 0x004UL,
+	[OP_USBINTR]		= 0x008UL,
+	[OP_DEVICEADDR]		= 0x014UL,
+	[OP_ENDPTLISTADDR]	= 0x018UL,
+	[OP_PORTSC]		= 0x044UL,
+	[OP_DEVLC]		= 0x084UL,
+	[OP_OTGSC]		= 0x0C4UL,
+	[OP_USBMODE]		= 0x0C8UL,
+	[OP_ENDPTSETUPSTAT]	= 0x0D8UL,
+	[OP_ENDPTPRIME]		= 0x0DCUL,
+	[OP_ENDPTFLUSH]		= 0x0E0UL,
+	[OP_ENDPTSTAT]		= 0x0E4UL,
+	[OP_ENDPTCOMPLETE]	= 0x0E8UL,
+	[OP_ENDPTCTRL]		= 0x0ECUL,
+};
+
+static int hw_alloc_regmap(struct ci13xxx *ci, bool is_lpm)
+{
+	int i;
+
+	kfree(ci->hw_bank.regmap);
+
+	ci->hw_bank.regmap = kzalloc((OP_LAST + 1) * sizeof(void *),
+				     GFP_KERNEL);
+	if (!ci->hw_bank.regmap)
+		return -ENOMEM;
+
+	for (i = 0; i < OP_ENDPTCTRL; i++)
+		ci->hw_bank.regmap[i] =
+			(i <= CAP_LAST ? ci->hw_bank.cap : ci->hw_bank.op) +
+			(is_lpm ? ci_regs_lpm[i] : ci_regs_nolpm[i]);
+
+	for (; i <= OP_LAST; i++)
+		ci->hw_bank.regmap[i] = ci->hw_bank.op +
+			4 * (i - OP_ENDPTCTRL) +
+			(is_lpm
+			 ? ci_regs_lpm[OP_ENDPTCTRL]
+			 : ci_regs_nolpm[OP_ENDPTCTRL]);
+
+	return 0;
+}
+
+/**
+ * hw_port_test_set: writes port test mode (execute without interruption)
+ * @mode: new value
+ *
+ * This function returns an error code
+ */
+int hw_port_test_set(struct ci13xxx *ci, u8 mode)
+{
+	const u8 TEST_MODE_MAX = 7;
+
+	if (mode > TEST_MODE_MAX)
+		return -EINVAL;
+
+	hw_write(ci, OP_PORTSC, PORTSC_PTC, mode << ffs_nr(PORTSC_PTC));
+	return 0;
+}
+
+/**
+ * hw_port_test_get: reads port test mode value
+ *
+ * This function returns port test mode value
+ */
+u8 hw_port_test_get(struct ci13xxx *ci)
+{
+	return hw_read(ci, OP_PORTSC, PORTSC_PTC) >> ffs_nr(PORTSC_PTC);
+}
+
+static int hw_device_init(struct ci13xxx *ci, void __iomem *base)
+{
+	u32 reg;
+
+	/* bank is a module variable */
+	ci->hw_bank.abs = base;
+
+	ci->hw_bank.cap = ci->hw_bank.abs;
+	ci->hw_bank.cap += ci->udc_driver->capoffset;
+	ci->hw_bank.op = ci->hw_bank.cap + ioread8(ci->hw_bank.cap);
+
+	hw_alloc_regmap(ci, false);
+	reg = hw_read(ci, CAP_HCCPARAMS, HCCPARAMS_LEN) >>
+		ffs_nr(HCCPARAMS_LEN);
+	ci->hw_bank.lpm  = reg;
+	hw_alloc_regmap(ci, !!reg);
+	ci->hw_bank.size = ci->hw_bank.op - ci->hw_bank.abs;
+	ci->hw_bank.size += OP_LAST;
+	ci->hw_bank.size /= sizeof(u32);
+
+	reg = hw_read(ci, CAP_DCCPARAMS, DCCPARAMS_DEN) >>
+		ffs_nr(DCCPARAMS_DEN);
+	ci->hw_ep_max = reg * 2;   /* cache hw ENDPT_MAX */
+
+	if (ci->hw_ep_max > ENDPT_MAX)
+		return -ENODEV;
+
+	dev_dbg(ci->dev, "ChipIdea HDRC found, lpm: %d; cap: %p op: %p\n",
+		ci->hw_bank.lpm, ci->hw_bank.cap, ci->hw_bank.op);
+
+	/* setup lock mode ? */
+
+	/* ENDPTSETUPSTAT is '0' by default */
+
+	/* HCSPARAMS.bf.ppc SHOULD BE zero for device */
+
+	return 0;
+}
+
+/**
+ * hw_device_reset: resets chip (execute without interruption)
+ * @ci: the controller
+  *
+ * This function returns an error code
+ */
+int hw_device_reset(struct ci13xxx *ci, u32 mode)
+{
+	/* should flush & stop before reset */
+	hw_write(ci, OP_ENDPTFLUSH, ~0, ~0);
+	hw_write(ci, OP_USBCMD, USBCMD_RS, 0);
+
+	hw_write(ci, OP_USBCMD, USBCMD_RST, USBCMD_RST);
+	while (hw_read(ci, OP_USBCMD, USBCMD_RST))
+		udelay(10);		/* not RTOS friendly */
+
+
+	if (ci->udc_driver->notify_event)
+		ci->udc_driver->notify_event(ci,
+			CI13XXX_CONTROLLER_RESET_EVENT);
+
+	if (ci->udc_driver->flags & CI13XXX_DISABLE_STREAMING)
+		hw_write(ci, OP_USBMODE, USBMODE_CI_SDIS, USBMODE_CI_SDIS);
+
+	/* USBMODE should be configured step by step */
+	hw_write(ci, OP_USBMODE, USBMODE_CM, USBMODE_CM_IDLE);
+	hw_write(ci, OP_USBMODE, USBMODE_CM, mode);
+	/* HW >= 2.3 */
+	hw_write(ci, OP_USBMODE, USBMODE_SLOM, USBMODE_SLOM);
+
+	if (hw_read(ci, OP_USBMODE, USBMODE_CM) != mode) {
+		pr_err("cannot enter in %s mode", ci_role(ci)->name);
+		pr_err("lpm = %i", ci->hw_bank.lpm);
+		return -ENODEV;
+	}
+
+	return 0;
+}
+
+/**
+ * ci_otg_role - pick role based on ID pin state
+ * @ci: the controller
+ */
+static enum ci_role ci_otg_role(struct ci13xxx *ci)
+{
+	u32 sts = hw_read(ci, OP_OTGSC, ~0);
+	enum ci_role role = sts & OTGSC_ID
+		? CI_ROLE_GADGET
+		: CI_ROLE_HOST;
+
+	return role;
+}
+
+/**
+ * ci_role_work - perform role changing based on ID pin
+ * @work: work struct
+ */
+static void ci_role_work(struct work_struct *work)
+{
+	struct ci13xxx *ci = container_of(work, struct ci13xxx, work);
+	enum ci_role role = ci_otg_role(ci);
+
+	hw_write(ci, OP_OTGSC, OTGSC_IDIS, OTGSC_IDIS);
+
+	if (role != ci->role) {
+		dev_dbg(ci->dev, "switching from %s to %s\n",
+			ci_role(ci)->name, ci->roles[role]->name);
+
+		ci_role_stop(ci);
+		ci_role_start(ci, role);
+	}
+}
+
+static ssize_t show_role(struct device *dev, struct device_attribute *attr,
+			 char *buf)
+{
+	struct ci13xxx *ci = dev_get_drvdata(dev);
+
+	return sprintf(buf, "%s\n", ci_role(ci)->name);
+}
+
+static ssize_t store_role(struct device *dev, struct device_attribute *attr,
+			  const char *buf, size_t count)
+{
+	struct ci13xxx *ci = dev_get_drvdata(dev);
+	enum ci_role role;
+	int ret;
+
+	for (role = CI_ROLE_HOST; role < CI_ROLE_END; role++)
+		if (ci->roles[role] && !strcmp(buf, ci->roles[role]->name))
+			break;
+
+	if (role == CI_ROLE_END || role == ci->role)
+		return -EINVAL;
+
+	ci_role_stop(ci);
+	ret = ci_role_start(ci, role);
+	if (ret)
+		return ret;
+
+	return count;
+}
+
+static DEVICE_ATTR(role, S_IRUSR | S_IWUSR, show_role, store_role);
+
+static irqreturn_t ci_irq(int irq, void *data)
+{
+	struct ci13xxx *ci = data;
+	irqreturn_t ret = IRQ_NONE;
+
+	if (ci->is_otg) {
+		u32 sts = hw_read(ci, OP_OTGSC, ~0);
+
+		if (sts & OTGSC_IDIS) {
+			queue_work(ci->wq, &ci->work);
+			ret = IRQ_HANDLED;
+		}
+	}
+
+	return ci->role == CI_ROLE_END ? ret : ci_role(ci)->irq(ci);
+}
+
+static int __devinit ci_hdrc_probe(struct platform_device *pdev)
+{
+	struct device	*dev = &pdev->dev;
+	struct ci13xxx	*ci;
+	struct resource	*res;
+	void __iomem	*base;
+	int		ret;
+
+	if (!dev->platform_data) {
+		dev_err(dev, "platform data missing\n");
+		return -ENODEV;
+	}
+
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	if (!res) {
+		dev_err(dev, "missing resource\n");
+		return -ENODEV;
+	}
+
+	base = devm_request_and_ioremap(dev, res);
+	if (!res) {
+		dev_err(dev, "can't request and ioremap resource\n");
+		return -ENOMEM;
+	}
+
+	ci = devm_kzalloc(dev, sizeof(*ci), GFP_KERNEL);
+	if (!ci) {
+		dev_err(dev, "can't allocate device\n");
+		return -ENOMEM;
+	}
+
+	ci->dev = dev;
+	ci->udc_driver = dev->platform_data;
+
+	ret = hw_device_init(ci, base);
+	if (ret < 0) {
+		dev_err(dev, "can't initialize hardware\n");
+		return -ENODEV;
+	}
+
+	ci->hw_bank.phys = res->start;
+
+	ci->irq = platform_get_irq(pdev, 0);
+	if (ci->irq < 0) {
+		dev_err(dev, "missing IRQ\n");
+		return -ENODEV;
+	}
+
+	INIT_WORK(&ci->work, ci_role_work);
+	ci->wq = create_singlethread_workqueue("ci_otg");
+	if (!ci->wq) {
+		dev_err(dev, "can't create workqueue\n");
+		return -ENODEV;
+	}
+
+	/* initialize role(s) before the interrupt is requested */
+	ret = ci_hdrc_host_init(ci);
+	if (ret)
+		dev_info(dev, "doesn't support host\n");
+
+	ret = ci_hdrc_gadget_init(ci);
+	if (ret)
+		dev_info(dev, "doesn't support gadget\n");
+
+	if (!ci->roles[CI_ROLE_HOST] && !ci->roles[CI_ROLE_GADGET]) {
+		dev_err(dev, "no supported roles\n");
+		ret = -ENODEV;
+		goto rm_wq;
+	}
+
+	if (ci->roles[CI_ROLE_HOST] && ci->roles[CI_ROLE_GADGET]) {
+		ci->is_otg = true;
+		ci->role = ci_otg_role(ci);
+	} else {
+		ci->role = ci->roles[CI_ROLE_HOST]
+			? CI_ROLE_HOST
+			: CI_ROLE_GADGET;
+	}
+
+	ret = ci_role_start(ci, ci->role);
+	if (ret) {
+		dev_err(dev, "can't start %s role\n", ci_role(ci)->name);
+		ret = -ENODEV;
+		goto rm_wq;
+	}
+
+	platform_set_drvdata(pdev, ci);
+	ret = request_irq(ci->irq, ci_irq, IRQF_SHARED, ci->udc_driver->name,
+			  ci);
+	if (ret)
+		goto stop;
+
+	ret = device_create_file(dev, &dev_attr_role);
+	if (ret)
+		goto rm_attr;
+
+	if (ci->is_otg)
+		hw_write(ci, OP_OTGSC, OTGSC_IDIE, OTGSC_IDIE);
+
+	return ret;
+
+rm_attr:
+	device_remove_file(dev, &dev_attr_role);
+stop:
+	ci_role_stop(ci);
+rm_wq:
+	flush_workqueue(ci->wq);
+	destroy_workqueue(ci->wq);
+
+	return ret;
+}
+
+static int __devexit ci_hdrc_remove(struct platform_device *pdev)
+{
+	struct ci13xxx *ci = platform_get_drvdata(pdev);
+
+	flush_workqueue(ci->wq);
+	destroy_workqueue(ci->wq);
+	device_remove_file(ci->dev, &dev_attr_role);
+	free_irq(ci->irq, ci);
+	ci_role_stop(ci);
+
+	return 0;
+}
+
+static struct platform_driver ci_hdrc_driver = {
+	.probe	= ci_hdrc_probe,
+	.remove	= __devexit_p(ci_hdrc_remove),
+	.driver	= {
+		.name	= "ci_hdrc",
+	},
+};
+
+module_platform_driver(ci_hdrc_driver);
+
+MODULE_ALIAS("platform:ci_hdrc");
+MODULE_ALIAS("platform:ci13xxx");
+MODULE_LICENSE("GPL v2");
+MODULE_AUTHOR("David Lopo <dlopo@chipidea.mips.com>");
+MODULE_DESCRIPTION("ChipIdea HDRC Driver");
diff --git a/drivers/usb/chipidea/debug.c b/drivers/usb/chipidea/debug.c
new file mode 100644
index 00000000000..c4b3e15532d
--- /dev/null
+++ b/drivers/usb/chipidea/debug.c
@@ -0,0 +1,804 @@
+#include <linux/delay.h>
+#include <linux/device.h>
+#include <linux/dmapool.h>
+#include <linux/dma-mapping.h>
+#include <linux/init.h>
+#include <linux/platform_device.h>
+#include <linux/module.h>
+#include <linux/interrupt.h>
+#include <linux/io.h>
+#include <linux/irq.h>
+#include <linux/kernel.h>
+#include <linux/slab.h>
+#include <linux/pm_runtime.h>
+#include <linux/usb/ch9.h>
+#include <linux/usb/gadget.h>
+#include <linux/usb/otg.h>
+#include <linux/usb/chipidea.h>
+
+#include "ci.h"
+#include "udc.h"
+#include "bits.h"
+#include "debug.h"
+
+/* Interrupt statistics */
+#define ISR_MASK   0x1F
+static struct isr_statistics {
+	u32 test;
+	u32 ui;
+	u32 uei;
+	u32 pci;
+	u32 uri;
+	u32 sli;
+	u32 none;
+	struct {
+		u32 cnt;
+		u32 buf[ISR_MASK+1];
+		u32 idx;
+	} hndl;
+} isr_statistics;
+
+void dbg_interrupt(u32 intmask)
+{
+	if (!intmask) {
+		isr_statistics.none++;
+		return;
+	}
+
+	isr_statistics.hndl.buf[isr_statistics.hndl.idx++] = intmask;
+	isr_statistics.hndl.idx &= ISR_MASK;
+	isr_statistics.hndl.cnt++;
+
+	if (USBi_URI & intmask)
+		isr_statistics.uri++;
+	if (USBi_PCI & intmask)
+		isr_statistics.pci++;
+	if (USBi_UEI & intmask)
+		isr_statistics.uei++;
+	if (USBi_UI  & intmask)
+		isr_statistics.ui++;
+	if (USBi_SLI & intmask)
+		isr_statistics.sli++;
+}
+
+/**
+ * hw_register_read: reads all device registers (execute without interruption)
+ * @buf:  destination buffer
+ * @size: buffer size
+ *
+ * This function returns number of registers read
+ */
+static size_t hw_register_read(struct ci13xxx *udc, u32 *buf, size_t size)
+{
+	unsigned i;
+
+	if (size > udc->hw_bank.size)
+		size = udc->hw_bank.size;
+
+	for (i = 0; i < size; i++)
+		buf[i] = hw_read(udc, i * sizeof(u32), ~0);
+
+	return size;
+}
+
+/**
+ * hw_register_write: writes to register
+ * @addr: register address
+ * @data: register value
+ *
+ * This function returns an error code
+ */
+static int hw_register_write(struct ci13xxx *udc, u16 addr, u32 data)
+{
+	/* align */
+	addr /= sizeof(u32);
+
+	if (addr >= udc->hw_bank.size)
+		return -EINVAL;
+
+	/* align */
+	addr *= sizeof(u32);
+
+	hw_write(udc, addr, ~0, data);
+	return 0;
+}
+
+/**
+ * hw_intr_clear: disables interrupt & clears interrupt status (execute without
+ *                interruption)
+ * @n: interrupt bit
+ *
+ * This function returns an error code
+ */
+static int hw_intr_clear(struct ci13xxx *udc, int n)
+{
+	if (n >= REG_BITS)
+		return -EINVAL;
+
+	hw_write(udc, OP_USBINTR, BIT(n), 0);
+	hw_write(udc, OP_USBSTS,  BIT(n), BIT(n));
+	return 0;
+}
+
+/**
+ * hw_intr_force: enables interrupt & forces interrupt status (execute without
+ *                interruption)
+ * @n: interrupt bit
+ *
+ * This function returns an error code
+ */
+static int hw_intr_force(struct ci13xxx *udc, int n)
+{
+	if (n >= REG_BITS)
+		return -EINVAL;
+
+	hw_write(udc, CAP_TESTMODE, TESTMODE_FORCE, TESTMODE_FORCE);
+	hw_write(udc, OP_USBINTR,  BIT(n), BIT(n));
+	hw_write(udc, OP_USBSTS,   BIT(n), BIT(n));
+	hw_write(udc, CAP_TESTMODE, TESTMODE_FORCE, 0);
+	return 0;
+}
+
+/**
+ * show_device: prints information about device capabilities and status
+ *
+ * Check "device.h" for details
+ */
+static ssize_t show_device(struct device *dev, struct device_attribute *attr,
+			   char *buf)
+{
+	struct ci13xxx *udc = container_of(dev, struct ci13xxx, gadget.dev);
+	struct usb_gadget *gadget = &udc->gadget;
+	int n = 0;
+
+	if (attr == NULL || buf == NULL) {
+		dev_err(udc->dev, "[%s] EINVAL\n", __func__);
+		return 0;
+	}
+
+	n += scnprintf(buf + n, PAGE_SIZE - n, "speed             = %d\n",
+		       gadget->speed);
+	n += scnprintf(buf + n, PAGE_SIZE - n, "max_speed         = %d\n",
+		       gadget->max_speed);
+	/* TODO: Scheduled for removal in 3.8. */
+	n += scnprintf(buf + n, PAGE_SIZE - n, "is_dualspeed      = %d\n",
+		       gadget_is_dualspeed(gadget));
+	n += scnprintf(buf + n, PAGE_SIZE - n, "is_otg            = %d\n",
+		       gadget->is_otg);
+	n += scnprintf(buf + n, PAGE_SIZE - n, "is_a_peripheral   = %d\n",
+		       gadget->is_a_peripheral);
+	n += scnprintf(buf + n, PAGE_SIZE - n, "b_hnp_enable      = %d\n",
+		       gadget->b_hnp_enable);
+	n += scnprintf(buf + n, PAGE_SIZE - n, "a_hnp_support     = %d\n",
+		       gadget->a_hnp_support);
+	n += scnprintf(buf + n, PAGE_SIZE - n, "a_alt_hnp_support = %d\n",
+		       gadget->a_alt_hnp_support);
+	n += scnprintf(buf + n, PAGE_SIZE - n, "name              = %s\n",
+		       (gadget->name ? gadget->name : ""));
+
+	return n;
+}
+static DEVICE_ATTR(device, S_IRUSR, show_device, NULL);
+
+/**
+ * show_driver: prints information about attached gadget (if any)
+ *
+ * Check "device.h" for details
+ */
+static ssize_t show_driver(struct device *dev, struct device_attribute *attr,
+			   char *buf)
+{
+	struct ci13xxx *udc = container_of(dev, struct ci13xxx, gadget.dev);
+	struct usb_gadget_driver *driver = udc->driver;
+	int n = 0;
+
+	if (attr == NULL || buf == NULL) {
+		dev_err(dev, "[%s] EINVAL\n", __func__);
+		return 0;
+	}
+
+	if (driver == NULL)
+		return scnprintf(buf, PAGE_SIZE,
+				 "There is no gadget attached!\n");
+
+	n += scnprintf(buf + n, PAGE_SIZE - n, "function  = %s\n",
+		       (driver->function ? driver->function : ""));
+	n += scnprintf(buf + n, PAGE_SIZE - n, "max speed = %d\n",
+		       driver->max_speed);
+
+	return n;
+}
+static DEVICE_ATTR(driver, S_IRUSR, show_driver, NULL);
+
+/* Maximum event message length */
+#define DBG_DATA_MSG   64UL
+
+/* Maximum event messages */
+#define DBG_DATA_MAX   128UL
+
+/* Event buffer descriptor */
+static struct {
+	char     (buf[DBG_DATA_MAX])[DBG_DATA_MSG];   /* buffer */
+	unsigned idx;   /* index */
+	unsigned tty;   /* print to console? */
+	rwlock_t lck;   /* lock */
+} dbg_data = {
+	.idx = 0,
+	.tty = 0,
+	.lck = __RW_LOCK_UNLOCKED(lck)
+};
+
+/**
+ * dbg_dec: decrements debug event index
+ * @idx: buffer index
+ */
+static void dbg_dec(unsigned *idx)
+{
+	*idx = (*idx - 1) & (DBG_DATA_MAX-1);
+}
+
+/**
+ * dbg_inc: increments debug event index
+ * @idx: buffer index
+ */
+static void dbg_inc(unsigned *idx)
+{
+	*idx = (*idx + 1) & (DBG_DATA_MAX-1);
+}
+
+/**
+ * dbg_print:  prints the common part of the event
+ * @addr:   endpoint address
+ * @name:   event name
+ * @status: status
+ * @extra:  extra information
+ */
+static void dbg_print(u8 addr, const char *name, int status, const char *extra)
+{
+	struct timeval tval;
+	unsigned int stamp;
+	unsigned long flags;
+
+	write_lock_irqsave(&dbg_data.lck, flags);
+
+	do_gettimeofday(&tval);
+	stamp = tval.tv_sec & 0xFFFF;	/* 2^32 = 4294967296. Limit to 4096s */
+	stamp = stamp * 1000000 + tval.tv_usec;
+
+	scnprintf(dbg_data.buf[dbg_data.idx], DBG_DATA_MSG,
+		  "%04X\t? %02X %-7.7s %4i ?\t%s\n",
+		  stamp, addr, name, status, extra);
+
+	dbg_inc(&dbg_data.idx);
+
+	write_unlock_irqrestore(&dbg_data.lck, flags);
+
+	if (dbg_data.tty != 0)
+		pr_notice("%04X\t? %02X %-7.7s %4i ?\t%s\n",
+			  stamp, addr, name, status, extra);
+}
+
+/**
+ * dbg_done: prints a DONE event
+ * @addr:   endpoint address
+ * @td:     transfer descriptor
+ * @status: status
+ */
+void dbg_done(u8 addr, const u32 token, int status)
+{
+	char msg[DBG_DATA_MSG];
+
+	scnprintf(msg, sizeof(msg), "%d %02X",
+		  (int)(token & TD_TOTAL_BYTES) >> ffs_nr(TD_TOTAL_BYTES),
+		  (int)(token & TD_STATUS)      >> ffs_nr(TD_STATUS));
+	dbg_print(addr, "DONE", status, msg);
+}
+
+/**
+ * dbg_event: prints a generic event
+ * @addr:   endpoint address
+ * @name:   event name
+ * @status: status
+ */
+void dbg_event(u8 addr, const char *name, int status)
+{
+	if (name != NULL)
+		dbg_print(addr, name, status, "");
+}
+
+/*
+ * dbg_queue: prints a QUEUE event
+ * @addr:   endpoint address
+ * @req:    USB request
+ * @status: status
+ */
+void dbg_queue(u8 addr, const struct usb_request *req, int status)
+{
+	char msg[DBG_DATA_MSG];
+
+	if (req != NULL) {
+		scnprintf(msg, sizeof(msg),
+			  "%d %d", !req->no_interrupt, req->length);
+		dbg_print(addr, "QUEUE", status, msg);
+	}
+}
+
+/**
+ * dbg_setup: prints a SETUP event
+ * @addr: endpoint address
+ * @req:  setup request
+ */
+void dbg_setup(u8 addr, const struct usb_ctrlrequest *req)
+{
+	char msg[DBG_DATA_MSG];
+
+	if (req != NULL) {
+		scnprintf(msg, sizeof(msg),
+			  "%02X %02X %04X %04X %d", req->bRequestType,
+			  req->bRequest, le16_to_cpu(req->wValue),
+			  le16_to_cpu(req->wIndex), le16_to_cpu(req->wLength));
+		dbg_print(addr, "SETUP", 0, msg);
+	}
+}
+
+/**
+ * show_events: displays the event buffer
+ *
+ * Check "device.h" for details
+ */
+static ssize_t show_events(struct device *dev, struct device_attribute *attr,
+			   char *buf)
+{
+	unsigned long flags;
+	unsigned i, j, n = 0;
+
+	if (attr == NULL || buf == NULL) {
+		dev_err(dev->parent, "[%s] EINVAL\n", __func__);
+		return 0;
+	}
+
+	read_lock_irqsave(&dbg_data.lck, flags);
+
+	i = dbg_data.idx;
+	for (dbg_dec(&i); i != dbg_data.idx; dbg_dec(&i)) {
+		n += strlen(dbg_data.buf[i]);
+		if (n >= PAGE_SIZE) {
+			n -= strlen(dbg_data.buf[i]);
+			break;
+		}
+	}
+	for (j = 0, dbg_inc(&i); j < n; dbg_inc(&i))
+		j += scnprintf(buf + j, PAGE_SIZE - j,
+			       "%s", dbg_data.buf[i]);
+
+	read_unlock_irqrestore(&dbg_data.lck, flags);
+
+	return n;
+}
+
+/**
+ * store_events: configure if events are going to be also printed to console
+ *
+ * Check "device.h" for details
+ */
+static ssize_t store_events(struct device *dev, struct device_attribute *attr,
+			    const char *buf, size_t count)
+{
+	unsigned tty;
+
+	if (attr == NULL || buf == NULL) {
+		dev_err(dev, "[%s] EINVAL\n", __func__);
+		goto done;
+	}
+
+	if (sscanf(buf, "%u", &tty) != 1 || tty > 1) {
+		dev_err(dev, "<1|0>: enable|disable console log\n");
+		goto done;
+	}
+
+	dbg_data.tty = tty;
+	dev_info(dev, "tty = %u", dbg_data.tty);
+
+ done:
+	return count;
+}
+static DEVICE_ATTR(events, S_IRUSR | S_IWUSR, show_events, store_events);
+
+/**
+ * show_inters: interrupt status, enable status and historic
+ *
+ * Check "device.h" for details
+ */
+static ssize_t show_inters(struct device *dev, struct device_attribute *attr,
+			   char *buf)
+{
+	struct ci13xxx *udc = container_of(dev, struct ci13xxx, gadget.dev);
+	unsigned long flags;
+	u32 intr;
+	unsigned i, j, n = 0;
+
+	if (attr == NULL || buf == NULL) {
+		dev_err(udc->dev, "[%s] EINVAL\n", __func__);
+		return 0;
+	}
+
+	spin_lock_irqsave(&udc->lock, flags);
+
+	/*n += scnprintf(buf + n, PAGE_SIZE - n,
+		       "status = %08x\n", hw_read_intr_status(udc));
+	n += scnprintf(buf + n, PAGE_SIZE - n,
+	"enable = %08x\n", hw_read_intr_enable(udc));*/
+
+	n += scnprintf(buf + n, PAGE_SIZE - n, "*test = %d\n",
+		       isr_statistics.test);
+	n += scnprintf(buf + n, PAGE_SIZE - n, "? ui  = %d\n",
+		       isr_statistics.ui);
+	n += scnprintf(buf + n, PAGE_SIZE - n, "? uei = %d\n",
+		       isr_statistics.uei);
+	n += scnprintf(buf + n, PAGE_SIZE - n, "? pci = %d\n",
+		       isr_statistics.pci);
+	n += scnprintf(buf + n, PAGE_SIZE - n, "? uri = %d\n",
+		       isr_statistics.uri);
+	n += scnprintf(buf + n, PAGE_SIZE - n, "? sli = %d\n",
+		       isr_statistics.sli);
+	n += scnprintf(buf + n, PAGE_SIZE - n, "*none = %d\n",
+		       isr_statistics.none);
+	n += scnprintf(buf + n, PAGE_SIZE - n, "*hndl = %d\n",
+		       isr_statistics.hndl.cnt);
+
+	for (i = isr_statistics.hndl.idx, j = 0; j <= ISR_MASK; j++, i++) {
+		i   &= ISR_MASK;
+		intr = isr_statistics.hndl.buf[i];
+
+		if (USBi_UI  & intr)
+			n += scnprintf(buf + n, PAGE_SIZE - n, "ui  ");
+		intr &= ~USBi_UI;
+		if (USBi_UEI & intr)
+			n += scnprintf(buf + n, PAGE_SIZE - n, "uei ");
+		intr &= ~USBi_UEI;
+		if (USBi_PCI & intr)
+			n += scnprintf(buf + n, PAGE_SIZE - n, "pci ");
+		intr &= ~USBi_PCI;
+		if (USBi_URI & intr)
+			n += scnprintf(buf + n, PAGE_SIZE - n, "uri ");
+		intr &= ~USBi_URI;
+		if (USBi_SLI & intr)
+			n += scnprintf(buf + n, PAGE_SIZE - n, "sli ");
+		intr &= ~USBi_SLI;
+		if (intr)
+			n += scnprintf(buf + n, PAGE_SIZE - n, "??? ");
+		if (isr_statistics.hndl.buf[i])
+			n += scnprintf(buf + n, PAGE_SIZE - n, "\n");
+	}
+
+	spin_unlock_irqrestore(&udc->lock, flags);
+
+	return n;
+}
+
+/**
+ * store_inters: enable & force or disable an individual interrutps
+ *                   (to be used for test purposes only)
+ *
+ * Check "device.h" for details
+ */
+static ssize_t store_inters(struct device *dev, struct device_attribute *attr,
+			    const char *buf, size_t count)
+{
+	struct ci13xxx *udc = container_of(dev, struct ci13xxx, gadget.dev);
+	unsigned long flags;
+	unsigned en, bit;
+
+	if (attr == NULL || buf == NULL) {
+		dev_err(udc->dev, "EINVAL\n");
+		goto done;
+	}
+
+	if (sscanf(buf, "%u %u", &en, &bit) != 2 || en > 1) {
+		dev_err(udc->dev, "<1|0> <bit>: enable|disable interrupt\n");
+		goto done;
+	}
+
+	spin_lock_irqsave(&udc->lock, flags);
+	if (en) {
+		if (hw_intr_force(udc, bit))
+			dev_err(dev, "invalid bit number\n");
+		else
+			isr_statistics.test++;
+	} else {
+		if (hw_intr_clear(udc, bit))
+			dev_err(dev, "invalid bit number\n");
+	}
+	spin_unlock_irqrestore(&udc->lock, flags);
+
+ done:
+	return count;
+}
+static DEVICE_ATTR(inters, S_IRUSR | S_IWUSR, show_inters, store_inters);
+
+/**
+ * show_port_test: reads port test mode
+ *
+ * Check "device.h" for details
+ */
+static ssize_t show_port_test(struct device *dev,
+			      struct device_attribute *attr, char *buf)
+{
+	struct ci13xxx *udc = container_of(dev, struct ci13xxx, gadget.dev);
+	unsigned long flags;
+	unsigned mode;
+
+	if (attr == NULL || buf == NULL) {
+		dev_err(udc->dev, "EINVAL\n");
+		return 0;
+	}
+
+	spin_lock_irqsave(&udc->lock, flags);
+	mode = hw_port_test_get(udc);
+	spin_unlock_irqrestore(&udc->lock, flags);
+
+	return scnprintf(buf, PAGE_SIZE, "mode = %u\n", mode);
+}
+
+/**
+ * store_port_test: writes port test mode
+ *
+ * Check "device.h" for details
+ */
+static ssize_t store_port_test(struct device *dev,
+			       struct device_attribute *attr,
+			       const char *buf, size_t count)
+{
+	struct ci13xxx *udc = container_of(dev, struct ci13xxx, gadget.dev);
+	unsigned long flags;
+	unsigned mode;
+
+	if (attr == NULL || buf == NULL) {
+		dev_err(udc->dev, "[%s] EINVAL\n", __func__);
+		goto done;
+	}
+
+	if (sscanf(buf, "%u", &mode) != 1) {
+		dev_err(udc->dev, "<mode>: set port test mode");
+		goto done;
+	}
+
+	spin_lock_irqsave(&udc->lock, flags);
+	if (hw_port_test_set(udc, mode))
+		dev_err(udc->dev, "invalid mode\n");
+	spin_unlock_irqrestore(&udc->lock, flags);
+
+ done:
+	return count;
+}
+static DEVICE_ATTR(port_test, S_IRUSR | S_IWUSR,
+		   show_port_test, store_port_test);
+
+/**
+ * show_qheads: DMA contents of all queue heads
+ *
+ * Check "device.h" for details
+ */
+static ssize_t show_qheads(struct device *dev, struct device_attribute *attr,
+			   char *buf)
+{
+	struct ci13xxx *udc = container_of(dev, struct ci13xxx, gadget.dev);
+	unsigned long flags;
+	unsigned i, j, n = 0;
+
+	if (attr == NULL || buf == NULL) {
+		dev_err(udc->dev, "[%s] EINVAL\n", __func__);
+		return 0;
+	}
+
+	spin_lock_irqsave(&udc->lock, flags);
+	for (i = 0; i < udc->hw_ep_max/2; i++) {
+		struct ci13xxx_ep *mEpRx = &udc->ci13xxx_ep[i];
+		struct ci13xxx_ep *mEpTx =
+			&udc->ci13xxx_ep[i + udc->hw_ep_max/2];
+		n += scnprintf(buf + n, PAGE_SIZE - n,
+			       "EP=%02i: RX=%08X TX=%08X\n",
+			       i, (u32)mEpRx->qh.dma, (u32)mEpTx->qh.dma);
+		for (j = 0; j < (sizeof(struct ci13xxx_qh)/sizeof(u32)); j++) {
+			n += scnprintf(buf + n, PAGE_SIZE - n,
+				       " %04X:    %08X    %08X\n", j,
+				       *((u32 *)mEpRx->qh.ptr + j),
+				       *((u32 *)mEpTx->qh.ptr + j));
+		}
+	}
+	spin_unlock_irqrestore(&udc->lock, flags);
+
+	return n;
+}
+static DEVICE_ATTR(qheads, S_IRUSR, show_qheads, NULL);
+
+/**
+ * show_registers: dumps all registers
+ *
+ * Check "device.h" for details
+ */
+#define DUMP_ENTRIES	512
+static ssize_t show_registers(struct device *dev,
+			      struct device_attribute *attr, char *buf)
+{
+	struct ci13xxx *udc = container_of(dev, struct ci13xxx, gadget.dev);
+	unsigned long flags;
+	u32 *dump;
+	unsigned i, k, n = 0;
+
+	if (attr == NULL || buf == NULL) {
+		dev_err(udc->dev, "[%s] EINVAL\n", __func__);
+		return 0;
+	}
+
+	dump = kmalloc(sizeof(u32) * DUMP_ENTRIES, GFP_KERNEL);
+	if (!dump) {
+		dev_err(udc->dev, "%s: out of memory\n", __func__);
+		return 0;
+	}
+
+	spin_lock_irqsave(&udc->lock, flags);
+	k = hw_register_read(udc, dump, DUMP_ENTRIES);
+	spin_unlock_irqrestore(&udc->lock, flags);
+
+	for (i = 0; i < k; i++) {
+		n += scnprintf(buf + n, PAGE_SIZE - n,
+			       "reg[0x%04X] = 0x%08X\n",
+			       i * (unsigned)sizeof(u32), dump[i]);
+	}
+	kfree(dump);
+
+	return n;
+}
+
+/**
+ * store_registers: writes value to register address
+ *
+ * Check "device.h" for details
+ */
+static ssize_t store_registers(struct device *dev,
+			       struct device_attribute *attr,
+			       const char *buf, size_t count)
+{
+	struct ci13xxx *udc = container_of(dev, struct ci13xxx, gadget.dev);
+	unsigned long addr, data, flags;
+
+	if (attr == NULL || buf == NULL) {
+		dev_err(udc->dev, "[%s] EINVAL\n", __func__);
+		goto done;
+	}
+
+	if (sscanf(buf, "%li %li", &addr, &data) != 2) {
+		dev_err(udc->dev,
+			"<addr> <data>: write data to register address\n");
+		goto done;
+	}
+
+	spin_lock_irqsave(&udc->lock, flags);
+	if (hw_register_write(udc, addr, data))
+		dev_err(udc->dev, "invalid address range\n");
+	spin_unlock_irqrestore(&udc->lock, flags);
+
+ done:
+	return count;
+}
+static DEVICE_ATTR(registers, S_IRUSR | S_IWUSR,
+		   show_registers, store_registers);
+
+/**
+ * show_requests: DMA contents of all requests currently queued (all endpts)
+ *
+ * Check "device.h" for details
+ */
+static ssize_t show_requests(struct device *dev, struct device_attribute *attr,
+			     char *buf)
+{
+	struct ci13xxx *udc = container_of(dev, struct ci13xxx, gadget.dev);
+	unsigned long flags;
+	struct list_head   *ptr = NULL;
+	struct ci13xxx_req *req = NULL;
+	unsigned i, j, n = 0, qSize = sizeof(struct ci13xxx_td)/sizeof(u32);
+
+	if (attr == NULL || buf == NULL) {
+		dev_err(udc->dev, "[%s] EINVAL\n", __func__);
+		return 0;
+	}
+
+	spin_lock_irqsave(&udc->lock, flags);
+	for (i = 0; i < udc->hw_ep_max; i++)
+		list_for_each(ptr, &udc->ci13xxx_ep[i].qh.queue)
+		{
+			req = list_entry(ptr, struct ci13xxx_req, queue);
+
+			n += scnprintf(buf + n, PAGE_SIZE - n,
+					"EP=%02i: TD=%08X %s\n",
+					i % udc->hw_ep_max/2, (u32)req->dma,
+					((i < udc->hw_ep_max/2) ? "RX" : "TX"));
+
+			for (j = 0; j < qSize; j++)
+				n += scnprintf(buf + n, PAGE_SIZE - n,
+						" %04X:    %08X\n", j,
+						*((u32 *)req->ptr + j));
+		}
+	spin_unlock_irqrestore(&udc->lock, flags);
+
+	return n;
+}
+static DEVICE_ATTR(requests, S_IRUSR, show_requests, NULL);
+
+/**
+ * dbg_create_files: initializes the attribute interface
+ * @dev: device
+ *
+ * This function returns an error code
+ */
+int dbg_create_files(struct device *dev)
+{
+	int retval = 0;
+
+	if (dev == NULL)
+		return -EINVAL;
+	retval = device_create_file(dev, &dev_attr_device);
+	if (retval)
+		goto done;
+	retval = device_create_file(dev, &dev_attr_driver);
+	if (retval)
+		goto rm_device;
+	retval = device_create_file(dev, &dev_attr_events);
+	if (retval)
+		goto rm_driver;
+	retval = device_create_file(dev, &dev_attr_inters);
+	if (retval)
+		goto rm_events;
+	retval = device_create_file(dev, &dev_attr_port_test);
+	if (retval)
+		goto rm_inters;
+	retval = device_create_file(dev, &dev_attr_qheads);
+	if (retval)
+		goto rm_port_test;
+	retval = device_create_file(dev, &dev_attr_registers);
+	if (retval)
+		goto rm_qheads;
+	retval = device_create_file(dev, &dev_attr_requests);
+	if (retval)
+		goto rm_registers;
+	return 0;
+
+ rm_registers:
+	device_remove_file(dev, &dev_attr_registers);
+ rm_qheads:
+	device_remove_file(dev, &dev_attr_qheads);
+ rm_port_test:
+	device_remove_file(dev, &dev_attr_port_test);
+ rm_inters:
+	device_remove_file(dev, &dev_attr_inters);
+ rm_events:
+	device_remove_file(dev, &dev_attr_events);
+ rm_driver:
+	device_remove_file(dev, &dev_attr_driver);
+ rm_device:
+	device_remove_file(dev, &dev_attr_device);
+ done:
+	return retval;
+}
+
+/**
+ * dbg_remove_files: destroys the attribute interface
+ * @dev: device
+ *
+ * This function returns an error code
+ */
+int dbg_remove_files(struct device *dev)
+{
+	if (dev == NULL)
+		return -EINVAL;
+	device_remove_file(dev, &dev_attr_requests);
+	device_remove_file(dev, &dev_attr_registers);
+	device_remove_file(dev, &dev_attr_qheads);
+	device_remove_file(dev, &dev_attr_port_test);
+	device_remove_file(dev, &dev_attr_inters);
+	device_remove_file(dev, &dev_attr_events);
+	device_remove_file(dev, &dev_attr_driver);
+	device_remove_file(dev, &dev_attr_device);
+	return 0;
+}
diff --git a/drivers/usb/chipidea/debug.h b/drivers/usb/chipidea/debug.h
new file mode 100644
index 00000000000..80d96865775
--- /dev/null
+++ b/drivers/usb/chipidea/debug.h
@@ -0,0 +1,56 @@
+/*
+ * debug.h - ChipIdea USB driver debug interfaces
+ *
+ * Copyright (C) 2008 Chipidea - MIPS Technologies, Inc. All rights reserved.
+ *
+ * Author: David Lopo
+ *
+ * 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 __DRIVERS_USB_CHIPIDEA_DEBUG_H
+#define __DRIVERS_USB_CHIPIDEA_DEBUG_H
+
+#ifdef CONFIG_USB_CHIPIDEA_DEBUG
+void dbg_interrupt(u32 intmask);
+void dbg_done(u8 addr, const u32 token, int status);
+void dbg_event(u8 addr, const char *name, int status);
+void dbg_queue(u8 addr, const struct usb_request *req, int status);
+void dbg_setup(u8 addr, const struct usb_ctrlrequest *req);
+int dbg_create_files(struct device *dev);
+int dbg_remove_files(struct device *dev);
+#else
+static inline void dbg_interrupt(u32 intmask)
+{
+}
+
+static inline void dbg_done(u8 addr, const u32 token, int status)
+{
+}
+
+static inline void dbg_event(u8 addr, const char *name, int status)
+{
+}
+
+static inline void dbg_queue(u8 addr, const struct usb_request *req, int status)
+{
+}
+
+static inline void dbg_setup(u8 addr, const struct usb_ctrlrequest *req)
+{
+}
+
+static inline int dbg_create_files(struct device *dev)
+{
+	return 0;
+}
+
+static inline int dbg_remove_files(struct device *dev)
+{
+	return 0;
+}
+#endif
+
+#endif /* __DRIVERS_USB_CHIPIDEA_DEBUG_H */
diff --git a/drivers/usb/chipidea/host.c b/drivers/usb/chipidea/host.c
new file mode 100644
index 00000000000..9eacd21c0cd
--- /dev/null
+++ b/drivers/usb/chipidea/host.c
@@ -0,0 +1,160 @@
+/*
+ * host.c - ChipIdea USB host controller driver
+ *
+ * Copyright (c) 2012 Intel Corporation
+ *
+ * Author: Alexander Shishkin
+ *
+ * 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., 675 Mass Ave, Cambridge, MA 02139, USA.
+ */
+
+#include <linux/kernel.h>
+#include <linux/usb.h>
+#include <linux/usb/hcd.h>
+#include <linux/usb/chipidea.h>
+
+#define CHIPIDEA_EHCI
+#include "../host/ehci-hcd.c"
+
+#include "ci.h"
+#include "bits.h"
+#include "host.h"
+
+static int ci_ehci_setup(struct usb_hcd *hcd)
+{
+	struct ehci_hcd *ehci = hcd_to_ehci(hcd);
+	int ret;
+
+	hcd->has_tt = 1;
+
+	ret = ehci_setup(hcd);
+	if (ret)
+		return ret;
+
+	ehci_port_power(ehci, 0);
+
+	return ret;
+}
+
+static const struct hc_driver ci_ehci_hc_driver = {
+	.description	= "ehci_hcd",
+	.product_desc	= "ChipIdea HDRC EHCI",
+	.hcd_priv_size	= sizeof(struct ehci_hcd),
+
+	/*
+	 * generic hardware linkage
+	 */
+	.irq	= ehci_irq,
+	.flags	= HCD_MEMORY | HCD_USB2,
+
+	/*
+	 * basic lifecycle operations
+	 */
+	.reset		= ci_ehci_setup,
+	.start		= ehci_run,
+	.stop		= ehci_stop,
+	.shutdown	= ehci_shutdown,
+
+	/*
+	 * managing i/o requests and associated device resources
+	 */
+	.urb_enqueue		= ehci_urb_enqueue,
+	.urb_dequeue		= ehci_urb_dequeue,
+	.endpoint_disable	= ehci_endpoint_disable,
+	.endpoint_reset		= ehci_endpoint_reset,
+
+	/*
+	 * scheduling support
+	 */
+	.get_frame_number = ehci_get_frame,
+
+	/*
+	 * root hub support
+	 */
+	.hub_status_data	= ehci_hub_status_data,
+	.hub_control		= ehci_hub_control,
+	.bus_suspend		= ehci_bus_suspend,
+	.bus_resume		= ehci_bus_resume,
+	.relinquish_port	= ehci_relinquish_port,
+	.port_handed_over	= ehci_port_handed_over,
+
+	.clear_tt_buffer_complete = ehci_clear_tt_buffer_complete,
+};
+
+static irqreturn_t host_irq(struct ci13xxx *ci)
+{
+	return usb_hcd_irq(ci->irq, ci->hcd);
+}
+
+static int host_start(struct ci13xxx *ci)
+{
+	struct usb_hcd *hcd;
+	struct ehci_hcd *ehci;
+	int ret;
+
+	if (usb_disabled())
+		return -ENODEV;
+
+	hcd = usb_create_hcd(&ci_ehci_hc_driver, ci->dev, dev_name(ci->dev));
+	if (!hcd)
+		return -ENOMEM;
+
+	dev_set_drvdata(ci->dev, ci);
+	hcd->rsrc_start = ci->hw_bank.phys;
+	hcd->rsrc_len = ci->hw_bank.size;
+	hcd->regs = ci->hw_bank.abs;
+	hcd->has_tt = 1;
+
+	hcd->power_budget = ci->udc_driver->power_budget;
+
+	ehci = hcd_to_ehci(hcd);
+	ehci->caps = ci->hw_bank.cap;
+	ehci->has_hostpc = ci->hw_bank.lpm;
+
+	ret = usb_add_hcd(hcd, 0, 0);
+	if (ret)
+		usb_put_hcd(hcd);
+	else
+		ci->hcd = hcd;
+
+	return ret;
+}
+
+static void host_stop(struct ci13xxx *ci)
+{
+	struct usb_hcd *hcd = ci->hcd;
+
+	usb_remove_hcd(hcd);
+	usb_put_hcd(hcd);
+}
+
+int ci_hdrc_host_init(struct ci13xxx *ci)
+{
+	struct ci_role_driver *rdrv;
+
+	if (!hw_read(ci, CAP_DCCPARAMS, DCCPARAMS_HC))
+		return -ENXIO;
+
+	rdrv = devm_kzalloc(ci->dev, sizeof(struct ci_role_driver), GFP_KERNEL);
+	if (!rdrv)
+		return -ENOMEM;
+
+	rdrv->start	= host_start;
+	rdrv->stop	= host_stop;
+	rdrv->irq	= host_irq;
+	rdrv->name	= "host";
+	ci->roles[CI_ROLE_HOST] = rdrv;
+
+	return 0;
+}
diff --git a/drivers/usb/chipidea/host.h b/drivers/usb/chipidea/host.h
new file mode 100644
index 00000000000..761fb1fd6d9
--- /dev/null
+++ b/drivers/usb/chipidea/host.h
@@ -0,0 +1,17 @@
+#ifndef __DRIVERS_USB_CHIPIDEA_HOST_H
+#define __DRIVERS_USB_CHIPIDEA_HOST_H
+
+#ifdef CONFIG_USB_CHIPIDEA_HOST
+
+int ci_hdrc_host_init(struct ci13xxx *ci);
+
+#else
+
+static inline int ci_hdrc_host_init(struct ci13xxx *ci)
+{
+	return -ENXIO;
+}
+
+#endif
+
+#endif /* __DRIVERS_USB_CHIPIDEA_HOST_H */
diff --git a/drivers/usb/chipidea/udc.c b/drivers/usb/chipidea/udc.c
new file mode 100644
index 00000000000..51f96942dc5
--- /dev/null
+++ b/drivers/usb/chipidea/udc.c
@@ -0,0 +1,1809 @@
+/*
+ * udc.c - ChipIdea UDC driver
+ *
+ * Copyright (C) 2008 Chipidea - MIPS Technologies, Inc. All rights reserved.
+ *
+ * Author: David Lopo
+ *
+ * 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/dmapool.h>
+#include <linux/dma-mapping.h>
+#include <linux/init.h>
+#include <linux/platform_device.h>
+#include <linux/module.h>
+#include <linux/interrupt.h>
+#include <linux/io.h>
+#include <linux/irq.h>
+#include <linux/kernel.h>
+#include <linux/slab.h>
+#include <linux/pm_runtime.h>
+#include <linux/usb/ch9.h>
+#include <linux/usb/gadget.h>
+#include <linux/usb/otg.h>
+#include <linux/usb/chipidea.h>
+
+#include "ci.h"
+#include "udc.h"
+#include "bits.h"
+#include "debug.h"
+
+/* control endpoint description */
+static const struct usb_endpoint_descriptor
+ctrl_endpt_out_desc = {
+	.bLength         = USB_DT_ENDPOINT_SIZE,
+	.bDescriptorType = USB_DT_ENDPOINT,
+
+	.bEndpointAddress = USB_DIR_OUT,
+	.bmAttributes    = USB_ENDPOINT_XFER_CONTROL,
+	.wMaxPacketSize  = cpu_to_le16(CTRL_PAYLOAD_MAX),
+};
+
+static const struct usb_endpoint_descriptor
+ctrl_endpt_in_desc = {
+	.bLength         = USB_DT_ENDPOINT_SIZE,
+	.bDescriptorType = USB_DT_ENDPOINT,
+
+	.bEndpointAddress = USB_DIR_IN,
+	.bmAttributes    = USB_ENDPOINT_XFER_CONTROL,
+	.wMaxPacketSize  = cpu_to_le16(CTRL_PAYLOAD_MAX),
+};
+
+/**
+ * hw_ep_bit: calculates the bit number
+ * @num: endpoint number
+ * @dir: endpoint direction
+ *
+ * This function returns bit number
+ */
+static inline int hw_ep_bit(int num, int dir)
+{
+	return num + (dir ? 16 : 0);
+}
+
+static inline int ep_to_bit(struct ci13xxx *udc, int n)
+{
+	int fill = 16 - udc->hw_ep_max / 2;
+
+	if (n >= udc->hw_ep_max / 2)
+		n += fill;
+
+	return n;
+}
+
+/**
+ * hw_device_state: enables/disables interrupts & starts/stops device (execute
+ *                  without interruption)
+ * @dma: 0 => disable, !0 => enable and set dma engine
+ *
+ * This function returns an error code
+ */
+static int hw_device_state(struct ci13xxx *udc, u32 dma)
+{
+	if (dma) {
+		hw_write(udc, OP_ENDPTLISTADDR, ~0, dma);
+		/* interrupt, error, port change, reset, sleep/suspend */
+		hw_write(udc, OP_USBINTR, ~0,
+			     USBi_UI|USBi_UEI|USBi_PCI|USBi_URI|USBi_SLI);
+		hw_write(udc, OP_USBCMD, USBCMD_RS, USBCMD_RS);
+	} else {
+		hw_write(udc, OP_USBCMD, USBCMD_RS, 0);
+		hw_write(udc, OP_USBINTR, ~0, 0);
+	}
+	return 0;
+}
+
+/**
+ * hw_ep_flush: flush endpoint fifo (execute without interruption)
+ * @num: endpoint number
+ * @dir: endpoint direction
+ *
+ * This function returns an error code
+ */
+static int hw_ep_flush(struct ci13xxx *udc, int num, int dir)
+{
+	int n = hw_ep_bit(num, dir);
+
+	do {
+		/* flush any pending transfer */
+		hw_write(udc, OP_ENDPTFLUSH, BIT(n), BIT(n));
+		while (hw_read(udc, OP_ENDPTFLUSH, BIT(n)))
+			cpu_relax();
+	} while (hw_read(udc, OP_ENDPTSTAT, BIT(n)));
+
+	return 0;
+}
+
+/**
+ * hw_ep_disable: disables endpoint (execute without interruption)
+ * @num: endpoint number
+ * @dir: endpoint direction
+ *
+ * This function returns an error code
+ */
+static int hw_ep_disable(struct ci13xxx *udc, int num, int dir)
+{
+	hw_ep_flush(udc, num, dir);
+	hw_write(udc, OP_ENDPTCTRL + num,
+		 dir ? ENDPTCTRL_TXE : ENDPTCTRL_RXE, 0);
+	return 0;
+}
+
+/**
+ * hw_ep_enable: enables endpoint (execute without interruption)
+ * @num:  endpoint number
+ * @dir:  endpoint direction
+ * @type: endpoint type
+ *
+ * This function returns an error code
+ */
+static int hw_ep_enable(struct ci13xxx *udc, int num, int dir, int type)
+{
+	u32 mask, data;
+
+	if (dir) {
+		mask  = ENDPTCTRL_TXT;  /* type    */
+		data  = type << ffs_nr(mask);
+
+		mask |= ENDPTCTRL_TXS;  /* unstall */
+		mask |= ENDPTCTRL_TXR;  /* reset data toggle */
+		data |= ENDPTCTRL_TXR;
+		mask |= ENDPTCTRL_TXE;  /* enable  */
+		data |= ENDPTCTRL_TXE;
+	} else {
+		mask  = ENDPTCTRL_RXT;  /* type    */
+		data  = type << ffs_nr(mask);
+
+		mask |= ENDPTCTRL_RXS;  /* unstall */
+		mask |= ENDPTCTRL_RXR;  /* reset data toggle */
+		data |= ENDPTCTRL_RXR;
+		mask |= ENDPTCTRL_RXE;  /* enable  */
+		data |= ENDPTCTRL_RXE;
+	}
+	hw_write(udc, OP_ENDPTCTRL + num, mask, data);
+	return 0;
+}
+
+/**
+ * hw_ep_get_halt: return endpoint halt status
+ * @num: endpoint number
+ * @dir: endpoint direction
+ *
+ * This function returns 1 if endpoint halted
+ */
+static int hw_ep_get_halt(struct ci13xxx *udc, int num, int dir)
+{
+	u32 mask = dir ? ENDPTCTRL_TXS : ENDPTCTRL_RXS;
+
+	return hw_read(udc, OP_ENDPTCTRL + num, mask) ? 1 : 0;
+}
+
+/**
+ * hw_test_and_clear_setup_status: test & clear setup status (execute without
+ *                                 interruption)
+ * @n: endpoint number
+ *
+ * This function returns setup status
+ */
+static int hw_test_and_clear_setup_status(struct ci13xxx *udc, int n)
+{
+	n = ep_to_bit(udc, n);
+	return hw_test_and_clear(udc, OP_ENDPTSETUPSTAT, BIT(n));
+}
+
+/**
+ * hw_ep_prime: primes endpoint (execute without interruption)
+ * @num:     endpoint number
+ * @dir:     endpoint direction
+ * @is_ctrl: true if control endpoint
+ *
+ * This function returns an error code
+ */
+static int hw_ep_prime(struct ci13xxx *udc, int num, int dir, int is_ctrl)
+{
+	int n = hw_ep_bit(num, dir);
+
+	if (is_ctrl && dir == RX && hw_read(udc, OP_ENDPTSETUPSTAT, BIT(num)))
+		return -EAGAIN;
+
+	hw_write(udc, OP_ENDPTPRIME, BIT(n), BIT(n));
+
+	while (hw_read(udc, OP_ENDPTPRIME, BIT(n)))
+		cpu_relax();
+	if (is_ctrl && dir == RX && hw_read(udc, OP_ENDPTSETUPSTAT, BIT(num)))
+		return -EAGAIN;
+
+	/* status shoult be tested according with manual but it doesn't work */
+	return 0;
+}
+
+/**
+ * hw_ep_set_halt: configures ep halt & resets data toggle after clear (execute
+ *                 without interruption)
+ * @num:   endpoint number
+ * @dir:   endpoint direction
+ * @value: true => stall, false => unstall
+ *
+ * This function returns an error code
+ */
+static int hw_ep_set_halt(struct ci13xxx *udc, int num, int dir, int value)
+{
+	if (value != 0 && value != 1)
+		return -EINVAL;
+
+	do {
+		enum ci13xxx_regs reg = OP_ENDPTCTRL + num;
+		u32 mask_xs = dir ? ENDPTCTRL_TXS : ENDPTCTRL_RXS;
+		u32 mask_xr = dir ? ENDPTCTRL_TXR : ENDPTCTRL_RXR;
+
+		/* data toggle - reserved for EP0 but it's in ESS */
+		hw_write(udc, reg, mask_xs|mask_xr,
+			  value ? mask_xs : mask_xr);
+	} while (value != hw_ep_get_halt(udc, num, dir));
+
+	return 0;
+}
+
+/**
+ * hw_is_port_high_speed: test if port is high speed
+ *
+ * This function returns true if high speed port
+ */
+static int hw_port_is_high_speed(struct ci13xxx *udc)
+{
+	return udc->hw_bank.lpm ? hw_read(udc, OP_DEVLC, DEVLC_PSPD) :
+		hw_read(udc, OP_PORTSC, PORTSC_HSP);
+}
+
+/**
+ * hw_read_intr_enable: returns interrupt enable register
+ *
+ * This function returns register data
+ */
+static u32 hw_read_intr_enable(struct ci13xxx *udc)
+{
+	return hw_read(udc, OP_USBINTR, ~0);
+}
+
+/**
+ * hw_read_intr_status: returns interrupt status register
+ *
+ * This function returns register data
+ */
+static u32 hw_read_intr_status(struct ci13xxx *udc)
+{
+	return hw_read(udc, OP_USBSTS, ~0);
+}
+
+/**
+ * hw_test_and_clear_complete: test & clear complete status (execute without
+ *                             interruption)
+ * @n: endpoint number
+ *
+ * This function returns complete status
+ */
+static int hw_test_and_clear_complete(struct ci13xxx *udc, int n)
+{
+	n = ep_to_bit(udc, n);
+	return hw_test_and_clear(udc, OP_ENDPTCOMPLETE, BIT(n));
+}
+
+/**
+ * hw_test_and_clear_intr_active: test & clear active interrupts (execute
+ *                                without interruption)
+ *
+ * This function returns active interrutps
+ */
+static u32 hw_test_and_clear_intr_active(struct ci13xxx *udc)
+{
+	u32 reg = hw_read_intr_status(udc) & hw_read_intr_enable(udc);
+
+	hw_write(udc, OP_USBSTS, ~0, reg);
+	return reg;
+}
+
+/**
+ * hw_test_and_clear_setup_guard: test & clear setup guard (execute without
+ *                                interruption)
+ *
+ * This function returns guard value
+ */
+static int hw_test_and_clear_setup_guard(struct ci13xxx *udc)
+{
+	return hw_test_and_write(udc, OP_USBCMD, USBCMD_SUTW, 0);
+}
+
+/**
+ * hw_test_and_set_setup_guard: test & set setup guard (execute without
+ *                              interruption)
+ *
+ * This function returns guard value
+ */
+static int hw_test_and_set_setup_guard(struct ci13xxx *udc)
+{
+	return hw_test_and_write(udc, OP_USBCMD, USBCMD_SUTW, USBCMD_SUTW);
+}
+
+/**
+ * hw_usb_set_address: configures USB address (execute without interruption)
+ * @value: new USB address
+ *
+ * This function explicitly sets the address, without the "USBADRA" (advance)
+ * feature, which is not supported by older versions of the controller.
+ */
+static void hw_usb_set_address(struct ci13xxx *udc, u8 value)
+{
+	hw_write(udc, OP_DEVICEADDR, DEVICEADDR_USBADR,
+		 value << ffs_nr(DEVICEADDR_USBADR));
+}
+
+/**
+ * hw_usb_reset: restart device after a bus reset (execute without
+ *               interruption)
+ *
+ * This function returns an error code
+ */
+static int hw_usb_reset(struct ci13xxx *udc)
+{
+	hw_usb_set_address(udc, 0);
+
+	/* ESS flushes only at end?!? */
+	hw_write(udc, OP_ENDPTFLUSH,    ~0, ~0);
+
+	/* clear setup token semaphores */
+	hw_write(udc, OP_ENDPTSETUPSTAT, 0,  0);
+
+	/* clear complete status */
+	hw_write(udc, OP_ENDPTCOMPLETE,  0,  0);
+
+	/* wait until all bits cleared */
+	while (hw_read(udc, OP_ENDPTPRIME, ~0))
+		udelay(10);             /* not RTOS friendly */
+
+	/* reset all endpoints ? */
+
+	/* reset internal status and wait for further instructions
+	   no need to verify the port reset status (ESS does it) */
+
+	return 0;
+}
+
+/******************************************************************************
+ * UTIL block
+ *****************************************************************************/
+/**
+ * _usb_addr: calculates endpoint address from direction & number
+ * @ep:  endpoint
+ */
+static inline u8 _usb_addr(struct ci13xxx_ep *ep)
+{
+	return ((ep->dir == TX) ? USB_ENDPOINT_DIR_MASK : 0) | ep->num;
+}
+
+/**
+ * _hardware_queue: configures a request at hardware level
+ * @gadget: gadget
+ * @mEp:    endpoint
+ *
+ * This function returns an error code
+ */
+static int _hardware_enqueue(struct ci13xxx_ep *mEp, struct ci13xxx_req *mReq)
+{
+	struct ci13xxx *udc = mEp->udc;
+	unsigned i;
+	int ret = 0;
+	unsigned length = mReq->req.length;
+
+	/* don't queue twice */
+	if (mReq->req.status == -EALREADY)
+		return -EALREADY;
+
+	mReq->req.status = -EALREADY;
+
+	if (mReq->req.zero && length && (length % mEp->ep.maxpacket == 0)) {
+		mReq->zptr = dma_pool_alloc(mEp->td_pool, GFP_ATOMIC,
+					   &mReq->zdma);
+		if (mReq->zptr == NULL)
+			return -ENOMEM;
+
+		memset(mReq->zptr, 0, sizeof(*mReq->zptr));
+		mReq->zptr->next    = TD_TERMINATE;
+		mReq->zptr->token   = TD_STATUS_ACTIVE;
+		if (!mReq->req.no_interrupt)
+			mReq->zptr->token   |= TD_IOC;
+	}
+	ret = usb_gadget_map_request(&udc->gadget, &mReq->req, mEp->dir);
+	if (ret)
+		return ret;
+
+	/*
+	 * TD configuration
+	 * TODO - handle requests which spawns into several TDs
+	 */
+	memset(mReq->ptr, 0, sizeof(*mReq->ptr));
+	mReq->ptr->token    = length << ffs_nr(TD_TOTAL_BYTES);
+	mReq->ptr->token   &= TD_TOTAL_BYTES;
+	mReq->ptr->token   |= TD_STATUS_ACTIVE;
+	if (mReq->zptr) {
+		mReq->ptr->next    = mReq->zdma;
+	} else {
+		mReq->ptr->next    = TD_TERMINATE;
+		if (!mReq->req.no_interrupt)
+			mReq->ptr->token  |= TD_IOC;
+	}
+	mReq->ptr->page[0]  = mReq->req.dma;
+	for (i = 1; i < 5; i++)
+		mReq->ptr->page[i] =
+			(mReq->req.dma + i * CI13XXX_PAGE_SIZE) & ~TD_RESERVED_MASK;
+
+	if (!list_empty(&mEp->qh.queue)) {
+		struct ci13xxx_req *mReqPrev;
+		int n = hw_ep_bit(mEp->num, mEp->dir);
+		int tmp_stat;
+
+		mReqPrev = list_entry(mEp->qh.queue.prev,
+				struct ci13xxx_req, queue);
+		if (mReqPrev->zptr)
+			mReqPrev->zptr->next = mReq->dma & TD_ADDR_MASK;
+		else
+			mReqPrev->ptr->next = mReq->dma & TD_ADDR_MASK;
+		wmb();
+		if (hw_read(udc, OP_ENDPTPRIME, BIT(n)))
+			goto done;
+		do {
+			hw_write(udc, OP_USBCMD, USBCMD_ATDTW, USBCMD_ATDTW);
+			tmp_stat = hw_read(udc, OP_ENDPTSTAT, BIT(n));
+		} while (!hw_read(udc, OP_USBCMD, USBCMD_ATDTW));
+		hw_write(udc, OP_USBCMD, USBCMD_ATDTW, 0);
+		if (tmp_stat)
+			goto done;
+	}
+
+	/*  QH configuration */
+	mEp->qh.ptr->td.next   = mReq->dma;    /* TERMINATE = 0 */
+	mEp->qh.ptr->td.token &= ~TD_STATUS;   /* clear status */
+	mEp->qh.ptr->cap |=  QH_ZLT;
+
+	wmb();   /* synchronize before ep prime */
+
+	ret = hw_ep_prime(udc, mEp->num, mEp->dir,
+			   mEp->type == USB_ENDPOINT_XFER_CONTROL);
+done:
+	return ret;
+}
+
+/**
+ * _hardware_dequeue: handles a request at hardware level
+ * @gadget: gadget
+ * @mEp:    endpoint
+ *
+ * This function returns an error code
+ */
+static int _hardware_dequeue(struct ci13xxx_ep *mEp, struct ci13xxx_req *mReq)
+{
+	if (mReq->req.status != -EALREADY)
+		return -EINVAL;
+
+	if ((TD_STATUS_ACTIVE & mReq->ptr->token) != 0)
+		return -EBUSY;
+
+	if (mReq->zptr) {
+		if ((TD_STATUS_ACTIVE & mReq->zptr->token) != 0)
+			return -EBUSY;
+		dma_pool_free(mEp->td_pool, mReq->zptr, mReq->zdma);
+		mReq->zptr = NULL;
+	}
+
+	mReq->req.status = 0;
+
+	usb_gadget_unmap_request(&mEp->udc->gadget, &mReq->req, mEp->dir);
+
+	mReq->req.status = mReq->ptr->token & TD_STATUS;
+	if ((TD_STATUS_HALTED & mReq->req.status) != 0)
+		mReq->req.status = -1;
+	else if ((TD_STATUS_DT_ERR & mReq->req.status) != 0)
+		mReq->req.status = -1;
+	else if ((TD_STATUS_TR_ERR & mReq->req.status) != 0)
+		mReq->req.status = -1;
+
+	mReq->req.actual   = mReq->ptr->token & TD_TOTAL_BYTES;
+	mReq->req.actual >>= ffs_nr(TD_TOTAL_BYTES);
+	mReq->req.actual   = mReq->req.length - mReq->req.actual;
+	mReq->req.actual   = mReq->req.status ? 0 : mReq->req.actual;
+
+	return mReq->req.actual;
+}
+
+/**
+ * _ep_nuke: dequeues all endpoint requests
+ * @mEp: endpoint
+ *
+ * This function returns an error code
+ * Caller must hold lock
+ */
+static int _ep_nuke(struct ci13xxx_ep *mEp)
+__releases(mEp->lock)
+__acquires(mEp->lock)
+{
+	if (mEp == NULL)
+		return -EINVAL;
+
+	hw_ep_flush(mEp->udc, mEp->num, mEp->dir);
+
+	while (!list_empty(&mEp->qh.queue)) {
+
+		/* pop oldest request */
+		struct ci13xxx_req *mReq = \
+			list_entry(mEp->qh.queue.next,
+				   struct ci13xxx_req, queue);
+		list_del_init(&mReq->queue);
+		mReq->req.status = -ESHUTDOWN;
+
+		if (mReq->req.complete != NULL) {
+			spin_unlock(mEp->lock);
+			mReq->req.complete(&mEp->ep, &mReq->req);
+			spin_lock(mEp->lock);
+		}
+	}
+	return 0;
+}
+
+/**
+ * _gadget_stop_activity: stops all USB activity, flushes & disables all endpts
+ * @gadget: gadget
+ *
+ * This function returns an error code
+ */
+static int _gadget_stop_activity(struct usb_gadget *gadget)
+{
+	struct usb_ep *ep;
+	struct ci13xxx    *udc = container_of(gadget, struct ci13xxx, gadget);
+	unsigned long flags;
+
+	spin_lock_irqsave(&udc->lock, flags);
+	udc->gadget.speed = USB_SPEED_UNKNOWN;
+	udc->remote_wakeup = 0;
+	udc->suspended = 0;
+	spin_unlock_irqrestore(&udc->lock, flags);
+
+	/* flush all endpoints */
+	gadget_for_each_ep(ep, gadget) {
+		usb_ep_fifo_flush(ep);
+	}
+	usb_ep_fifo_flush(&udc->ep0out->ep);
+	usb_ep_fifo_flush(&udc->ep0in->ep);
+
+	if (udc->driver)
+		udc->driver->disconnect(gadget);
+
+	/* make sure to disable all endpoints */
+	gadget_for_each_ep(ep, gadget) {
+		usb_ep_disable(ep);
+	}
+
+	if (udc->status != NULL) {
+		usb_ep_free_request(&udc->ep0in->ep, udc->status);
+		udc->status = NULL;
+	}
+
+	return 0;
+}
+
+/******************************************************************************
+ * ISR block
+ *****************************************************************************/
+/**
+ * isr_reset_handler: USB reset interrupt handler
+ * @udc: UDC device
+ *
+ * This function resets USB engine after a bus reset occurred
+ */
+static void isr_reset_handler(struct ci13xxx *udc)
+__releases(udc->lock)
+__acquires(udc->lock)
+{
+	int retval;
+
+	dbg_event(0xFF, "BUS RST", 0);
+
+	spin_unlock(&udc->lock);
+	retval = _gadget_stop_activity(&udc->gadget);
+	if (retval)
+		goto done;
+
+	retval = hw_usb_reset(udc);
+	if (retval)
+		goto done;
+
+	udc->status = usb_ep_alloc_request(&udc->ep0in->ep, GFP_ATOMIC);
+	if (udc->status == NULL)
+		retval = -ENOMEM;
+
+done:
+	spin_lock(&udc->lock);
+
+	if (retval)
+		dev_err(udc->dev, "error: %i\n", retval);
+}
+
+/**
+ * isr_get_status_complete: get_status request complete function
+ * @ep:  endpoint
+ * @req: request handled
+ *
+ * Caller must release lock
+ */
+static void isr_get_status_complete(struct usb_ep *ep, struct usb_request *req)
+{
+	if (ep == NULL || req == NULL)
+		return;
+
+	kfree(req->buf);
+	usb_ep_free_request(ep, req);
+}
+
+/**
+ * isr_get_status_response: get_status request response
+ * @udc: udc struct
+ * @setup: setup request packet
+ *
+ * This function returns an error code
+ */
+static int isr_get_status_response(struct ci13xxx *udc,
+				   struct usb_ctrlrequest *setup)
+__releases(mEp->lock)
+__acquires(mEp->lock)
+{
+	struct ci13xxx_ep *mEp = udc->ep0in;
+	struct usb_request *req = NULL;
+	gfp_t gfp_flags = GFP_ATOMIC;
+	int dir, num, retval;
+
+	if (mEp == NULL || setup == NULL)
+		return -EINVAL;
+
+	spin_unlock(mEp->lock);
+	req = usb_ep_alloc_request(&mEp->ep, gfp_flags);
+	spin_lock(mEp->lock);
+	if (req == NULL)
+		return -ENOMEM;
+
+	req->complete = isr_get_status_complete;
+	req->length   = 2;
+	req->buf      = kzalloc(req->length, gfp_flags);
+	if (req->buf == NULL) {
+		retval = -ENOMEM;
+		goto err_free_req;
+	}
+
+	if ((setup->bRequestType & USB_RECIP_MASK) == USB_RECIP_DEVICE) {
+		/* Assume that device is bus powered for now. */
+		*(u16 *)req->buf = udc->remote_wakeup << 1;
+		retval = 0;
+	} else if ((setup->bRequestType & USB_RECIP_MASK) \
+		   == USB_RECIP_ENDPOINT) {
+		dir = (le16_to_cpu(setup->wIndex) & USB_ENDPOINT_DIR_MASK) ?
+			TX : RX;
+		num =  le16_to_cpu(setup->wIndex) & USB_ENDPOINT_NUMBER_MASK;
+		*(u16 *)req->buf = hw_ep_get_halt(udc, num, dir);
+	}
+	/* else do nothing; reserved for future use */
+
+	spin_unlock(mEp->lock);
+	retval = usb_ep_queue(&mEp->ep, req, gfp_flags);
+	spin_lock(mEp->lock);
+	if (retval)
+		goto err_free_buf;
+
+	return 0;
+
+ err_free_buf:
+	kfree(req->buf);
+ err_free_req:
+	spin_unlock(mEp->lock);
+	usb_ep_free_request(&mEp->ep, req);
+	spin_lock(mEp->lock);
+	return retval;
+}
+
+/**
+ * isr_setup_status_complete: setup_status request complete function
+ * @ep:  endpoint
+ * @req: request handled
+ *
+ * Caller must release lock. Put the port in test mode if test mode
+ * feature is selected.
+ */
+static void
+isr_setup_status_complete(struct usb_ep *ep, struct usb_request *req)
+{
+	struct ci13xxx *udc = req->context;
+	unsigned long flags;
+
+	if (udc->setaddr) {
+		hw_usb_set_address(udc, udc->address);
+		udc->setaddr = false;
+	}
+
+	spin_lock_irqsave(&udc->lock, flags);
+	if (udc->test_mode)
+		hw_port_test_set(udc, udc->test_mode);
+	spin_unlock_irqrestore(&udc->lock, flags);
+}
+
+/**
+ * isr_setup_status_phase: queues the status phase of a setup transation
+ * @udc: udc struct
+ *
+ * This function returns an error code
+ */
+static int isr_setup_status_phase(struct ci13xxx *udc)
+__releases(mEp->lock)
+__acquires(mEp->lock)
+{
+	int retval;
+	struct ci13xxx_ep *mEp;
+
+	mEp = (udc->ep0_dir == TX) ? udc->ep0out : udc->ep0in;
+	udc->status->context = udc;
+	udc->status->complete = isr_setup_status_complete;
+
+	spin_unlock(mEp->lock);
+	retval = usb_ep_queue(&mEp->ep, udc->status, GFP_ATOMIC);
+	spin_lock(mEp->lock);
+
+	return retval;
+}
+
+/**
+ * isr_tr_complete_low: transaction complete low level handler
+ * @mEp: endpoint
+ *
+ * This function returns an error code
+ * Caller must hold lock
+ */
+static int isr_tr_complete_low(struct ci13xxx_ep *mEp)
+__releases(mEp->lock)
+__acquires(mEp->lock)
+{
+	struct ci13xxx_req *mReq, *mReqTemp;
+	struct ci13xxx_ep *mEpTemp = mEp;
+	int uninitialized_var(retval);
+
+	if (list_empty(&mEp->qh.queue))
+		return -EINVAL;
+
+	list_for_each_entry_safe(mReq, mReqTemp, &mEp->qh.queue,
+			queue) {
+		retval = _hardware_dequeue(mEp, mReq);
+		if (retval < 0)
+			break;
+		list_del_init(&mReq->queue);
+		dbg_done(_usb_addr(mEp), mReq->ptr->token, retval);
+		if (mReq->req.complete != NULL) {
+			spin_unlock(mEp->lock);
+			if ((mEp->type == USB_ENDPOINT_XFER_CONTROL) &&
+					mReq->req.length)
+				mEpTemp = mEp->udc->ep0in;
+			mReq->req.complete(&mEpTemp->ep, &mReq->req);
+			spin_lock(mEp->lock);
+		}
+	}
+
+	if (retval == -EBUSY)
+		retval = 0;
+	if (retval < 0)
+		dbg_event(_usb_addr(mEp), "DONE", retval);
+
+	return retval;
+}
+
+/**
+ * isr_tr_complete_handler: transaction complete interrupt handler
+ * @udc: UDC descriptor
+ *
+ * This function handles traffic events
+ */
+static void isr_tr_complete_handler(struct ci13xxx *udc)
+__releases(udc->lock)
+__acquires(udc->lock)
+{
+	unsigned i;
+	u8 tmode = 0;
+
+	for (i = 0; i < udc->hw_ep_max; i++) {
+		struct ci13xxx_ep *mEp  = &udc->ci13xxx_ep[i];
+		int type, num, dir, err = -EINVAL;
+		struct usb_ctrlrequest req;
+
+		if (mEp->ep.desc == NULL)
+			continue;   /* not configured */
+
+		if (hw_test_and_clear_complete(udc, i)) {
+			err = isr_tr_complete_low(mEp);
+			if (mEp->type == USB_ENDPOINT_XFER_CONTROL) {
+				if (err > 0)   /* needs status phase */
+					err = isr_setup_status_phase(udc);
+				if (err < 0) {
+					dbg_event(_usb_addr(mEp),
+						  "ERROR", err);
+					spin_unlock(&udc->lock);
+					if (usb_ep_set_halt(&mEp->ep))
+						dev_err(udc->dev,
+							"error: ep_set_halt\n");
+					spin_lock(&udc->lock);
+				}
+			}
+		}
+
+		if (mEp->type != USB_ENDPOINT_XFER_CONTROL ||
+		    !hw_test_and_clear_setup_status(udc, i))
+			continue;
+
+		if (i != 0) {
+			dev_warn(udc->dev, "ctrl traffic at endpoint %d\n", i);
+			continue;
+		}
+
+		/*
+		 * Flush data and handshake transactions of previous
+		 * setup packet.
+		 */
+		_ep_nuke(udc->ep0out);
+		_ep_nuke(udc->ep0in);
+
+		/* read_setup_packet */
+		do {
+			hw_test_and_set_setup_guard(udc);
+			memcpy(&req, &mEp->qh.ptr->setup, sizeof(req));
+		} while (!hw_test_and_clear_setup_guard(udc));
+
+		type = req.bRequestType;
+
+		udc->ep0_dir = (type & USB_DIR_IN) ? TX : RX;
+
+		dbg_setup(_usb_addr(mEp), &req);
+
+		switch (req.bRequest) {
+		case USB_REQ_CLEAR_FEATURE:
+			if (type == (USB_DIR_OUT|USB_RECIP_ENDPOINT) &&
+					le16_to_cpu(req.wValue) ==
+					USB_ENDPOINT_HALT) {
+				if (req.wLength != 0)
+					break;
+				num  = le16_to_cpu(req.wIndex);
+				dir = num & USB_ENDPOINT_DIR_MASK;
+				num &= USB_ENDPOINT_NUMBER_MASK;
+				if (dir) /* TX */
+					num += udc->hw_ep_max/2;
+				if (!udc->ci13xxx_ep[num].wedge) {
+					spin_unlock(&udc->lock);
+					err = usb_ep_clear_halt(
+						&udc->ci13xxx_ep[num].ep);
+					spin_lock(&udc->lock);
+					if (err)
+						break;
+				}
+				err = isr_setup_status_phase(udc);
+			} else if (type == (USB_DIR_OUT|USB_RECIP_DEVICE) &&
+					le16_to_cpu(req.wValue) ==
+					USB_DEVICE_REMOTE_WAKEUP) {
+				if (req.wLength != 0)
+					break;
+				udc->remote_wakeup = 0;
+				err = isr_setup_status_phase(udc);
+			} else {
+				goto delegate;
+			}
+			break;
+		case USB_REQ_GET_STATUS:
+			if (type != (USB_DIR_IN|USB_RECIP_DEVICE)   &&
+			    type != (USB_DIR_IN|USB_RECIP_ENDPOINT) &&
+			    type != (USB_DIR_IN|USB_RECIP_INTERFACE))
+				goto delegate;
+			if (le16_to_cpu(req.wLength) != 2 ||
+			    le16_to_cpu(req.wValue)  != 0)
+				break;
+			err = isr_get_status_response(udc, &req);
+			break;
+		case USB_REQ_SET_ADDRESS:
+			if (type != (USB_DIR_OUT|USB_RECIP_DEVICE))
+				goto delegate;
+			if (le16_to_cpu(req.wLength) != 0 ||
+			    le16_to_cpu(req.wIndex)  != 0)
+				break;
+			udc->address = (u8)le16_to_cpu(req.wValue);
+			udc->setaddr = true;
+			err = isr_setup_status_phase(udc);
+			break;
+		case USB_REQ_SET_FEATURE:
+			if (type == (USB_DIR_OUT|USB_RECIP_ENDPOINT) &&
+					le16_to_cpu(req.wValue) ==
+					USB_ENDPOINT_HALT) {
+				if (req.wLength != 0)
+					break;
+				num  = le16_to_cpu(req.wIndex);
+				dir = num & USB_ENDPOINT_DIR_MASK;
+				num &= USB_ENDPOINT_NUMBER_MASK;
+				if (dir) /* TX */
+					num += udc->hw_ep_max/2;
+
+				spin_unlock(&udc->lock);
+				err = usb_ep_set_halt(&udc->ci13xxx_ep[num].ep);
+				spin_lock(&udc->lock);
+				if (!err)
+					isr_setup_status_phase(udc);
+			} else if (type == (USB_DIR_OUT|USB_RECIP_DEVICE)) {
+				if (req.wLength != 0)
+					break;
+				switch (le16_to_cpu(req.wValue)) {
+				case USB_DEVICE_REMOTE_WAKEUP:
+					udc->remote_wakeup = 1;
+					err = isr_setup_status_phase(udc);
+					break;
+				case USB_DEVICE_TEST_MODE:
+					tmode = le16_to_cpu(req.wIndex) >> 8;
+					switch (tmode) {
+					case TEST_J:
+					case TEST_K:
+					case TEST_SE0_NAK:
+					case TEST_PACKET:
+					case TEST_FORCE_EN:
+						udc->test_mode = tmode;
+						err = isr_setup_status_phase(
+								udc);
+						break;
+					default:
+						break;
+					}
+				default:
+					goto delegate;
+				}
+			} else {
+				goto delegate;
+			}
+			break;
+		default:
+delegate:
+			if (req.wLength == 0)   /* no data phase */
+				udc->ep0_dir = TX;
+
+			spin_unlock(&udc->lock);
+			err = udc->driver->setup(&udc->gadget, &req);
+			spin_lock(&udc->lock);
+			break;
+		}
+
+		if (err < 0) {
+			dbg_event(_usb_addr(mEp), "ERROR", err);
+
+			spin_unlock(&udc->lock);
+			if (usb_ep_set_halt(&mEp->ep))
+				dev_err(udc->dev, "error: ep_set_halt\n");
+			spin_lock(&udc->lock);
+		}
+	}
+}
+
+/******************************************************************************
+ * ENDPT block
+ *****************************************************************************/
+/**
+ * ep_enable: configure endpoint, making it usable
+ *
+ * Check usb_ep_enable() at "usb_gadget.h" for details
+ */
+static int ep_enable(struct usb_ep *ep,
+		     const struct usb_endpoint_descriptor *desc)
+{
+	struct ci13xxx_ep *mEp = container_of(ep, struct ci13xxx_ep, ep);
+	int retval = 0;
+	unsigned long flags;
+
+	if (ep == NULL || desc == NULL)
+		return -EINVAL;
+
+	spin_lock_irqsave(mEp->lock, flags);
+
+	/* only internal SW should enable ctrl endpts */
+
+	mEp->ep.desc = desc;
+
+	if (!list_empty(&mEp->qh.queue))
+		dev_warn(mEp->udc->dev, "enabling a non-empty endpoint!\n");
+
+	mEp->dir  = usb_endpoint_dir_in(desc) ? TX : RX;
+	mEp->num  = usb_endpoint_num(desc);
+	mEp->type = usb_endpoint_type(desc);
+
+	mEp->ep.maxpacket = usb_endpoint_maxp(desc);
+
+	dbg_event(_usb_addr(mEp), "ENABLE", 0);
+
+	mEp->qh.ptr->cap = 0;
+
+	if (mEp->type == USB_ENDPOINT_XFER_CONTROL)
+		mEp->qh.ptr->cap |=  QH_IOS;
+	else if (mEp->type == USB_ENDPOINT_XFER_ISOC)
+		mEp->qh.ptr->cap &= ~QH_MULT;
+	else
+		mEp->qh.ptr->cap &= ~QH_ZLT;
+
+	mEp->qh.ptr->cap |=
+		(mEp->ep.maxpacket << ffs_nr(QH_MAX_PKT)) & QH_MAX_PKT;
+	mEp->qh.ptr->td.next |= TD_TERMINATE;   /* needed? */
+
+	/*
+	 * Enable endpoints in the HW other than ep0 as ep0
+	 * is always enabled
+	 */
+	if (mEp->num)
+		retval |= hw_ep_enable(mEp->udc, mEp->num, mEp->dir, mEp->type);
+
+	spin_unlock_irqrestore(mEp->lock, flags);
+	return retval;
+}
+
+/**
+ * ep_disable: endpoint is no longer usable
+ *
+ * Check usb_ep_disable() at "usb_gadget.h" for details
+ */
+static int ep_disable(struct usb_ep *ep)
+{
+	struct ci13xxx_ep *mEp = container_of(ep, struct ci13xxx_ep, ep);
+	int direction, retval = 0;
+	unsigned long flags;
+
+	if (ep == NULL)
+		return -EINVAL;
+	else if (mEp->ep.desc == NULL)
+		return -EBUSY;
+
+	spin_lock_irqsave(mEp->lock, flags);
+
+	/* only internal SW should disable ctrl endpts */
+
+	direction = mEp->dir;
+	do {
+		dbg_event(_usb_addr(mEp), "DISABLE", 0);
+
+		retval |= _ep_nuke(mEp);
+		retval |= hw_ep_disable(mEp->udc, mEp->num, mEp->dir);
+
+		if (mEp->type == USB_ENDPOINT_XFER_CONTROL)
+			mEp->dir = (mEp->dir == TX) ? RX : TX;
+
+	} while (mEp->dir != direction);
+
+	mEp->ep.desc = NULL;
+
+	spin_unlock_irqrestore(mEp->lock, flags);
+	return retval;
+}
+
+/**
+ * ep_alloc_request: allocate a request object to use with this endpoint
+ *
+ * Check usb_ep_alloc_request() at "usb_gadget.h" for details
+ */
+static struct usb_request *ep_alloc_request(struct usb_ep *ep, gfp_t gfp_flags)
+{
+	struct ci13xxx_ep  *mEp  = container_of(ep, struct ci13xxx_ep, ep);
+	struct ci13xxx_req *mReq = NULL;
+
+	if (ep == NULL)
+		return NULL;
+
+	mReq = kzalloc(sizeof(struct ci13xxx_req), gfp_flags);
+	if (mReq != NULL) {
+		INIT_LIST_HEAD(&mReq->queue);
+
+		mReq->ptr = dma_pool_alloc(mEp->td_pool, gfp_flags,
+					   &mReq->dma);
+		if (mReq->ptr == NULL) {
+			kfree(mReq);
+			mReq = NULL;
+		}
+	}
+
+	dbg_event(_usb_addr(mEp), "ALLOC", mReq == NULL);
+
+	return (mReq == NULL) ? NULL : &mReq->req;
+}
+
+/**
+ * ep_free_request: frees a request object
+ *
+ * Check usb_ep_free_request() at "usb_gadget.h" for details
+ */
+static void ep_free_request(struct usb_ep *ep, struct usb_request *req)
+{
+	struct ci13xxx_ep  *mEp  = container_of(ep,  struct ci13xxx_ep, ep);
+	struct ci13xxx_req *mReq = container_of(req, struct ci13xxx_req, req);
+	unsigned long flags;
+
+	if (ep == NULL || req == NULL) {
+		return;
+	} else if (!list_empty(&mReq->queue)) {
+		dev_err(mEp->udc->dev, "freeing queued request\n");
+		return;
+	}
+
+	spin_lock_irqsave(mEp->lock, flags);
+
+	if (mReq->ptr)
+		dma_pool_free(mEp->td_pool, mReq->ptr, mReq->dma);
+	kfree(mReq);
+
+	dbg_event(_usb_addr(mEp), "FREE", 0);
+
+	spin_unlock_irqrestore(mEp->lock, flags);
+}
+
+/**
+ * ep_queue: queues (submits) an I/O request to an endpoint
+ *
+ * Check usb_ep_queue()* at usb_gadget.h" for details
+ */
+static int ep_queue(struct usb_ep *ep, struct usb_request *req,
+		    gfp_t __maybe_unused gfp_flags)
+{
+	struct ci13xxx_ep  *mEp  = container_of(ep,  struct ci13xxx_ep, ep);
+	struct ci13xxx_req *mReq = container_of(req, struct ci13xxx_req, req);
+	struct ci13xxx *udc = mEp->udc;
+	int retval = 0;
+	unsigned long flags;
+
+	if (ep == NULL || req == NULL || mEp->ep.desc == NULL)
+		return -EINVAL;
+
+	spin_lock_irqsave(mEp->lock, flags);
+
+	if (mEp->type == USB_ENDPOINT_XFER_CONTROL) {
+		if (req->length)
+			mEp = (udc->ep0_dir == RX) ?
+			       udc->ep0out : udc->ep0in;
+		if (!list_empty(&mEp->qh.queue)) {
+			_ep_nuke(mEp);
+			retval = -EOVERFLOW;
+			dev_warn(mEp->udc->dev, "endpoint ctrl %X nuked\n",
+				 _usb_addr(mEp));
+		}
+	}
+
+	/* first nuke then test link, e.g. previous status has not sent */
+	if (!list_empty(&mReq->queue)) {
+		retval = -EBUSY;
+		dev_err(mEp->udc->dev, "request already in queue\n");
+		goto done;
+	}
+
+	if (req->length > 4 * CI13XXX_PAGE_SIZE) {
+		req->length = 4 * CI13XXX_PAGE_SIZE;
+		retval = -EMSGSIZE;
+		dev_warn(mEp->udc->dev, "request length truncated\n");
+	}
+
+	dbg_queue(_usb_addr(mEp), req, retval);
+
+	/* push request */
+	mReq->req.status = -EINPROGRESS;
+	mReq->req.actual = 0;
+
+	retval = _hardware_enqueue(mEp, mReq);
+
+	if (retval == -EALREADY) {
+		dbg_event(_usb_addr(mEp), "QUEUE", retval);
+		retval = 0;
+	}
+	if (!retval)
+		list_add_tail(&mReq->queue, &mEp->qh.queue);
+
+ done:
+	spin_unlock_irqrestore(mEp->lock, flags);
+	return retval;
+}
+
+/**
+ * ep_dequeue: dequeues (cancels, unlinks) an I/O request from an endpoint
+ *
+ * Check usb_ep_dequeue() at "usb_gadget.h" for details
+ */
+static int ep_dequeue(struct usb_ep *ep, struct usb_request *req)
+{
+	struct ci13xxx_ep  *mEp  = container_of(ep,  struct ci13xxx_ep, ep);
+	struct ci13xxx_req *mReq = container_of(req, struct ci13xxx_req, req);
+	unsigned long flags;
+
+	if (ep == NULL || req == NULL || mReq->req.status != -EALREADY ||
+		mEp->ep.desc == NULL || list_empty(&mReq->queue) ||
+		list_empty(&mEp->qh.queue))
+		return -EINVAL;
+
+	spin_lock_irqsave(mEp->lock, flags);
+
+	dbg_event(_usb_addr(mEp), "DEQUEUE", 0);
+
+	hw_ep_flush(mEp->udc, mEp->num, mEp->dir);
+
+	/* pop request */
+	list_del_init(&mReq->queue);
+
+	usb_gadget_unmap_request(&mEp->udc->gadget, req, mEp->dir);
+
+	req->status = -ECONNRESET;
+
+	if (mReq->req.complete != NULL) {
+		spin_unlock(mEp->lock);
+		mReq->req.complete(&mEp->ep, &mReq->req);
+		spin_lock(mEp->lock);
+	}
+
+	spin_unlock_irqrestore(mEp->lock, flags);
+	return 0;
+}
+
+/**
+ * ep_set_halt: sets the endpoint halt feature
+ *
+ * Check usb_ep_set_halt() at "usb_gadget.h" for details
+ */
+static int ep_set_halt(struct usb_ep *ep, int value)
+{
+	struct ci13xxx_ep *mEp = container_of(ep, struct ci13xxx_ep, ep);
+	int direction, retval = 0;
+	unsigned long flags;
+
+	if (ep == NULL || mEp->ep.desc == NULL)
+		return -EINVAL;
+
+	spin_lock_irqsave(mEp->lock, flags);
+
+#ifndef STALL_IN
+	/* g_file_storage MS compliant but g_zero fails chapter 9 compliance */
+	if (value && mEp->type == USB_ENDPOINT_XFER_BULK && mEp->dir == TX &&
+	    !list_empty(&mEp->qh.queue)) {
+		spin_unlock_irqrestore(mEp->lock, flags);
+		return -EAGAIN;
+	}
+#endif
+
+	direction = mEp->dir;
+	do {
+		dbg_event(_usb_addr(mEp), "HALT", value);
+		retval |= hw_ep_set_halt(mEp->udc, mEp->num, mEp->dir, value);
+
+		if (!value)
+			mEp->wedge = 0;
+
+		if (mEp->type == USB_ENDPOINT_XFER_CONTROL)
+			mEp->dir = (mEp->dir == TX) ? RX : TX;
+
+	} while (mEp->dir != direction);
+
+	spin_unlock_irqrestore(mEp->lock, flags);
+	return retval;
+}
+
+/**
+ * ep_set_wedge: sets the halt feature and ignores clear requests
+ *
+ * Check usb_ep_set_wedge() at "usb_gadget.h" for details
+ */
+static int ep_set_wedge(struct usb_ep *ep)
+{
+	struct ci13xxx_ep *mEp = container_of(ep, struct ci13xxx_ep, ep);
+	unsigned long flags;
+
+	if (ep == NULL || mEp->ep.desc == NULL)
+		return -EINVAL;
+
+	spin_lock_irqsave(mEp->lock, flags);
+
+	dbg_event(_usb_addr(mEp), "WEDGE", 0);
+	mEp->wedge = 1;
+
+	spin_unlock_irqrestore(mEp->lock, flags);
+
+	return usb_ep_set_halt(ep);
+}
+
+/**
+ * ep_fifo_flush: flushes contents of a fifo
+ *
+ * Check usb_ep_fifo_flush() at "usb_gadget.h" for details
+ */
+static void ep_fifo_flush(struct usb_ep *ep)
+{
+	struct ci13xxx_ep *mEp = container_of(ep, struct ci13xxx_ep, ep);
+	unsigned long flags;
+
+	if (ep == NULL) {
+		dev_err(mEp->udc->dev, "%02X: -EINVAL\n", _usb_addr(mEp));
+		return;
+	}
+
+	spin_lock_irqsave(mEp->lock, flags);
+
+	dbg_event(_usb_addr(mEp), "FFLUSH", 0);
+	hw_ep_flush(mEp->udc, mEp->num, mEp->dir);
+
+	spin_unlock_irqrestore(mEp->lock, flags);
+}
+
+/**
+ * Endpoint-specific part of the API to the USB controller hardware
+ * Check "usb_gadget.h" for details
+ */
+static const struct usb_ep_ops usb_ep_ops = {
+	.enable	       = ep_enable,
+	.disable       = ep_disable,
+	.alloc_request = ep_alloc_request,
+	.free_request  = ep_free_request,
+	.queue	       = ep_queue,
+	.dequeue       = ep_dequeue,
+	.set_halt      = ep_set_halt,
+	.set_wedge     = ep_set_wedge,
+	.fifo_flush    = ep_fifo_flush,
+};
+
+/******************************************************************************
+ * GADGET block
+ *****************************************************************************/
+static int ci13xxx_vbus_session(struct usb_gadget *_gadget, int is_active)
+{
+	struct ci13xxx *udc = container_of(_gadget, struct ci13xxx, gadget);
+	unsigned long flags;
+	int gadget_ready = 0;
+
+	if (!(udc->udc_driver->flags & CI13XXX_PULLUP_ON_VBUS))
+		return -EOPNOTSUPP;
+
+	spin_lock_irqsave(&udc->lock, flags);
+	udc->vbus_active = is_active;
+	if (udc->driver)
+		gadget_ready = 1;
+	spin_unlock_irqrestore(&udc->lock, flags);
+
+	if (gadget_ready) {
+		if (is_active) {
+			pm_runtime_get_sync(&_gadget->dev);
+			hw_device_reset(udc, USBMODE_CM_DC);
+			hw_device_state(udc, udc->ep0out->qh.dma);
+		} else {
+			hw_device_state(udc, 0);
+			if (udc->udc_driver->notify_event)
+				udc->udc_driver->notify_event(udc,
+				CI13XXX_CONTROLLER_STOPPED_EVENT);
+			_gadget_stop_activity(&udc->gadget);
+			pm_runtime_put_sync(&_gadget->dev);
+		}
+	}
+
+	return 0;
+}
+
+static int ci13xxx_wakeup(struct usb_gadget *_gadget)
+{
+	struct ci13xxx *udc = container_of(_gadget, struct ci13xxx, gadget);
+	unsigned long flags;
+	int ret = 0;
+
+	spin_lock_irqsave(&udc->lock, flags);
+	if (!udc->remote_wakeup) {
+		ret = -EOPNOTSUPP;
+		goto out;
+	}
+	if (!hw_read(udc, OP_PORTSC, PORTSC_SUSP)) {
+		ret = -EINVAL;
+		goto out;
+	}
+	hw_write(udc, OP_PORTSC, PORTSC_FPR, PORTSC_FPR);
+out:
+	spin_unlock_irqrestore(&udc->lock, flags);
+	return ret;
+}
+
+static int ci13xxx_vbus_draw(struct usb_gadget *_gadget, unsigned mA)
+{
+	struct ci13xxx *udc = container_of(_gadget, struct ci13xxx, gadget);
+
+	if (udc->transceiver)
+		return usb_phy_set_power(udc->transceiver, mA);
+	return -ENOTSUPP;
+}
+
+static int ci13xxx_start(struct usb_gadget *gadget,
+			 struct usb_gadget_driver *driver);
+static int ci13xxx_stop(struct usb_gadget *gadget,
+			struct usb_gadget_driver *driver);
+/**
+ * Device operations part of the API to the USB controller hardware,
+ * which don't involve endpoints (or i/o)
+ * Check  "usb_gadget.h" for details
+ */
+static const struct usb_gadget_ops usb_gadget_ops = {
+	.vbus_session	= ci13xxx_vbus_session,
+	.wakeup		= ci13xxx_wakeup,
+	.vbus_draw	= ci13xxx_vbus_draw,
+	.udc_start	= ci13xxx_start,
+	.udc_stop	= ci13xxx_stop,
+};
+
+static int init_eps(struct ci13xxx *udc)
+{
+	int retval = 0, i, j;
+
+	for (i = 0; i < udc->hw_ep_max/2; i++)
+		for (j = RX; j <= TX; j++) {
+			int k = i + j * udc->hw_ep_max/2;
+			struct ci13xxx_ep *mEp = &udc->ci13xxx_ep[k];
+
+			scnprintf(mEp->name, sizeof(mEp->name), "ep%i%s", i,
+					(j == TX)  ? "in" : "out");
+
+			mEp->udc          = udc;
+			mEp->lock         = &udc->lock;
+			mEp->td_pool      = udc->td_pool;
+
+			mEp->ep.name      = mEp->name;
+			mEp->ep.ops       = &usb_ep_ops;
+			mEp->ep.maxpacket = CTRL_PAYLOAD_MAX;
+
+			INIT_LIST_HEAD(&mEp->qh.queue);
+			mEp->qh.ptr = dma_pool_alloc(udc->qh_pool, GFP_KERNEL,
+						     &mEp->qh.dma);
+			if (mEp->qh.ptr == NULL)
+				retval = -ENOMEM;
+			else
+				memset(mEp->qh.ptr, 0, sizeof(*mEp->qh.ptr));
+
+			/*
+			 * set up shorthands for ep0 out and in endpoints,
+			 * don't add to gadget's ep_list
+			 */
+			if (i == 0) {
+				if (j == RX)
+					udc->ep0out = mEp;
+				else
+					udc->ep0in = mEp;
+
+				continue;
+			}
+
+			list_add_tail(&mEp->ep.ep_list, &udc->gadget.ep_list);
+		}
+
+	return retval;
+}
+
+/**
+ * ci13xxx_start: register a gadget driver
+ * @gadget: our gadget
+ * @driver: the driver being registered
+ *
+ * Interrupts are enabled here.
+ */
+static int ci13xxx_start(struct usb_gadget *gadget,
+			 struct usb_gadget_driver *driver)
+{
+	struct ci13xxx *udc = container_of(gadget, struct ci13xxx, gadget);
+	unsigned long flags;
+	int retval = -ENOMEM;
+
+	if (driver->disconnect == NULL)
+		return -EINVAL;
+
+
+	udc->ep0out->ep.desc = &ctrl_endpt_out_desc;
+	retval = usb_ep_enable(&udc->ep0out->ep);
+	if (retval)
+		return retval;
+
+	udc->ep0in->ep.desc = &ctrl_endpt_in_desc;
+	retval = usb_ep_enable(&udc->ep0in->ep);
+	if (retval)
+		return retval;
+	spin_lock_irqsave(&udc->lock, flags);
+
+	udc->driver = driver;
+	pm_runtime_get_sync(&udc->gadget.dev);
+	if (udc->udc_driver->flags & CI13XXX_PULLUP_ON_VBUS) {
+		if (udc->vbus_active) {
+			if (udc->udc_driver->flags & CI13XXX_REGS_SHARED)
+				hw_device_reset(udc, USBMODE_CM_DC);
+		} else {
+			pm_runtime_put_sync(&udc->gadget.dev);
+			goto done;
+		}
+	}
+
+	retval = hw_device_state(udc, udc->ep0out->qh.dma);
+	if (retval)
+		pm_runtime_put_sync(&udc->gadget.dev);
+
+ done:
+	spin_unlock_irqrestore(&udc->lock, flags);
+	return retval;
+}
+
+/**
+ * ci13xxx_stop: unregister a gadget driver
+ */
+static int ci13xxx_stop(struct usb_gadget *gadget,
+			struct usb_gadget_driver *driver)
+{
+	struct ci13xxx *udc = container_of(gadget, struct ci13xxx, gadget);
+	unsigned long flags;
+
+	spin_lock_irqsave(&udc->lock, flags);
+
+	if (!(udc->udc_driver->flags & CI13XXX_PULLUP_ON_VBUS) ||
+			udc->vbus_active) {
+		hw_device_state(udc, 0);
+		if (udc->udc_driver->notify_event)
+			udc->udc_driver->notify_event(udc,
+			CI13XXX_CONTROLLER_STOPPED_EVENT);
+		udc->driver = NULL;
+		spin_unlock_irqrestore(&udc->lock, flags);
+		_gadget_stop_activity(&udc->gadget);
+		spin_lock_irqsave(&udc->lock, flags);
+		pm_runtime_put(&udc->gadget.dev);
+	}
+
+	spin_unlock_irqrestore(&udc->lock, flags);
+
+	return 0;
+}
+
+/******************************************************************************
+ * BUS block
+ *****************************************************************************/
+/**
+ * udc_irq: udc interrupt handler
+ *
+ * This function returns IRQ_HANDLED if the IRQ has been handled
+ * It locks access to registers
+ */
+static irqreturn_t udc_irq(struct ci13xxx *udc)
+{
+	irqreturn_t retval;
+	u32 intr;
+
+	if (udc == NULL)
+		return IRQ_HANDLED;
+
+	spin_lock(&udc->lock);
+
+	if (udc->udc_driver->flags & CI13XXX_REGS_SHARED) {
+		if (hw_read(udc, OP_USBMODE, USBMODE_CM) !=
+				USBMODE_CM_DC) {
+			spin_unlock(&udc->lock);
+			return IRQ_NONE;
+		}
+	}
+	intr = hw_test_and_clear_intr_active(udc);
+	dbg_interrupt(intr);
+
+	if (intr) {
+		/* order defines priority - do NOT change it */
+		if (USBi_URI & intr)
+			isr_reset_handler(udc);
+
+		if (USBi_PCI & intr) {
+			udc->gadget.speed = hw_port_is_high_speed(udc) ?
+				USB_SPEED_HIGH : USB_SPEED_FULL;
+			if (udc->suspended && udc->driver->resume) {
+				spin_unlock(&udc->lock);
+				udc->driver->resume(&udc->gadget);
+				spin_lock(&udc->lock);
+				udc->suspended = 0;
+			}
+		}
+
+		if (USBi_UI  & intr)
+			isr_tr_complete_handler(udc);
+
+		if (USBi_SLI & intr) {
+			if (udc->gadget.speed != USB_SPEED_UNKNOWN &&
+			    udc->driver->suspend) {
+				udc->suspended = 1;
+				spin_unlock(&udc->lock);
+				udc->driver->suspend(&udc->gadget);
+				spin_lock(&udc->lock);
+			}
+		}
+		retval = IRQ_HANDLED;
+	} else {
+		retval = IRQ_NONE;
+	}
+	spin_unlock(&udc->lock);
+
+	return retval;
+}
+
+/**
+ * udc_release: driver release function
+ * @dev: device
+ *
+ * Currently does nothing
+ */
+static void udc_release(struct device *dev)
+{
+}
+
+/**
+ * udc_start: initialize gadget role
+ * @udc: chipidea controller
+ */
+static int udc_start(struct ci13xxx *udc)
+{
+	struct device *dev = udc->dev;
+	int retval = 0;
+
+	if (!udc)
+		return -EINVAL;
+
+	spin_lock_init(&udc->lock);
+
+	udc->gadget.ops          = &usb_gadget_ops;
+	udc->gadget.speed        = USB_SPEED_UNKNOWN;
+	udc->gadget.max_speed    = USB_SPEED_HIGH;
+	udc->gadget.is_otg       = 0;
+	udc->gadget.name         = udc->udc_driver->name;
+
+	INIT_LIST_HEAD(&udc->gadget.ep_list);
+
+	dev_set_name(&udc->gadget.dev, "gadget");
+	udc->gadget.dev.dma_mask = dev->dma_mask;
+	udc->gadget.dev.coherent_dma_mask = dev->coherent_dma_mask;
+	udc->gadget.dev.parent   = dev;
+	udc->gadget.dev.release  = udc_release;
+
+	/* alloc resources */
+	udc->qh_pool = dma_pool_create("ci13xxx_qh", dev,
+				       sizeof(struct ci13xxx_qh),
+				       64, CI13XXX_PAGE_SIZE);
+	if (udc->qh_pool == NULL)
+		return -ENOMEM;
+
+	udc->td_pool = dma_pool_create("ci13xxx_td", dev,
+				       sizeof(struct ci13xxx_td),
+				       64, CI13XXX_PAGE_SIZE);
+	if (udc->td_pool == NULL) {
+		retval = -ENOMEM;
+		goto free_qh_pool;
+	}
+
+	retval = init_eps(udc);
+	if (retval)
+		goto free_pools;
+
+	udc->gadget.ep0 = &udc->ep0in->ep;
+
+	udc->transceiver = usb_get_transceiver();
+
+	if (udc->udc_driver->flags & CI13XXX_REQUIRE_TRANSCEIVER) {
+		if (udc->transceiver == NULL) {
+			retval = -ENODEV;
+			goto free_pools;
+		}
+	}
+
+	if (!(udc->udc_driver->flags & CI13XXX_REGS_SHARED)) {
+		retval = hw_device_reset(udc, USBMODE_CM_DC);
+		if (retval)
+			goto put_transceiver;
+	}
+
+	retval = device_register(&udc->gadget.dev);
+	if (retval) {
+		put_device(&udc->gadget.dev);
+		goto put_transceiver;
+	}
+
+	retval = dbg_create_files(&udc->gadget.dev);
+	if (retval)
+		goto unreg_device;
+
+	if (udc->transceiver) {
+		retval = otg_set_peripheral(udc->transceiver->otg,
+						&udc->gadget);
+		if (retval)
+			goto remove_dbg;
+	}
+
+	retval = usb_add_gadget_udc(dev, &udc->gadget);
+	if (retval)
+		goto remove_trans;
+
+	pm_runtime_no_callbacks(&udc->gadget.dev);
+	pm_runtime_enable(&udc->gadget.dev);
+
+	return retval;
+
+remove_trans:
+	if (udc->transceiver) {
+		otg_set_peripheral(udc->transceiver->otg, &udc->gadget);
+		usb_put_transceiver(udc->transceiver);
+	}
+
+	dev_err(dev, "error = %i\n", retval);
+remove_dbg:
+	dbg_remove_files(&udc->gadget.dev);
+unreg_device:
+	device_unregister(&udc->gadget.dev);
+put_transceiver:
+	if (udc->transceiver)
+		usb_put_transceiver(udc->transceiver);
+free_pools:
+	dma_pool_destroy(udc->td_pool);
+free_qh_pool:
+	dma_pool_destroy(udc->qh_pool);
+	return retval;
+}
+
+/**
+ * udc_remove: parent remove must call this to remove UDC
+ *
+ * No interrupts active, the IRQ has been released
+ */
+static void udc_stop(struct ci13xxx *udc)
+{
+	int i;
+
+	if (udc == NULL)
+		return;
+
+	usb_del_gadget_udc(&udc->gadget);
+
+	for (i = 0; i < udc->hw_ep_max; i++) {
+		struct ci13xxx_ep *mEp = &udc->ci13xxx_ep[i];
+
+		dma_pool_free(udc->qh_pool, mEp->qh.ptr, mEp->qh.dma);
+	}
+
+	dma_pool_destroy(udc->td_pool);
+	dma_pool_destroy(udc->qh_pool);
+
+	if (udc->transceiver) {
+		otg_set_peripheral(udc->transceiver->otg, NULL);
+		usb_put_transceiver(udc->transceiver);
+	}
+	dbg_remove_files(&udc->gadget.dev);
+	device_unregister(&udc->gadget.dev);
+	/* my kobject is dynamic, I swear! */
+	memset(&udc->gadget, 0, sizeof(udc->gadget));
+}
+
+/**
+ * ci_hdrc_gadget_init - initialize device related bits
+ * ci: the controller
+ *
+ * This function enables the gadget role, if the device is "device capable".
+ */
+int ci_hdrc_gadget_init(struct ci13xxx *ci)
+{
+	struct ci_role_driver *rdrv;
+
+	if (!hw_read(ci, CAP_DCCPARAMS, DCCPARAMS_DC))
+		return -ENXIO;
+
+	rdrv = devm_kzalloc(ci->dev, sizeof(struct ci_role_driver), GFP_KERNEL);
+	if (!rdrv)
+		return -ENOMEM;
+
+	rdrv->start	= udc_start;
+	rdrv->stop	= udc_stop;
+	rdrv->irq	= udc_irq;
+	rdrv->name	= "gadget";
+	ci->roles[CI_ROLE_GADGET] = rdrv;
+
+	return 0;
+}
diff --git a/drivers/usb/chipidea/udc.h b/drivers/usb/chipidea/udc.h
new file mode 100644
index 00000000000..4ff2384d7ca
--- /dev/null
+++ b/drivers/usb/chipidea/udc.h
@@ -0,0 +1,93 @@
+/*
+ * udc.h - ChipIdea UDC structures
+ *
+ * Copyright (C) 2008 Chipidea - MIPS Technologies, Inc. All rights reserved.
+ *
+ * Author: David Lopo
+ *
+ * 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 __DRIVERS_USB_CHIPIDEA_UDC_H
+#define __DRIVERS_USB_CHIPIDEA_UDC_H
+
+#include <linux/list.h>
+
+#define CTRL_PAYLOAD_MAX   64
+#define RX        0  /* similar to USB_DIR_OUT but can be used as an index */
+#define TX        1  /* similar to USB_DIR_IN  but can be used as an index */
+
+/* DMA layout of transfer descriptors */
+struct ci13xxx_td {
+	/* 0 */
+	u32 next;
+#define TD_TERMINATE          BIT(0)
+#define TD_ADDR_MASK          (0xFFFFFFEUL << 5)
+	/* 1 */
+	u32 token;
+#define TD_STATUS             (0x00FFUL <<  0)
+#define TD_STATUS_TR_ERR      BIT(3)
+#define TD_STATUS_DT_ERR      BIT(5)
+#define TD_STATUS_HALTED      BIT(6)
+#define TD_STATUS_ACTIVE      BIT(7)
+#define TD_MULTO              (0x0003UL << 10)
+#define TD_IOC                BIT(15)
+#define TD_TOTAL_BYTES        (0x7FFFUL << 16)
+	/* 2 */
+	u32 page[5];
+#define TD_CURR_OFFSET        (0x0FFFUL <<  0)
+#define TD_FRAME_NUM          (0x07FFUL <<  0)
+#define TD_RESERVED_MASK      (0x0FFFUL <<  0)
+} __attribute__ ((packed));
+
+/* DMA layout of queue heads */
+struct ci13xxx_qh {
+	/* 0 */
+	u32 cap;
+#define QH_IOS                BIT(15)
+#define QH_MAX_PKT            (0x07FFUL << 16)
+#define QH_ZLT                BIT(29)
+#define QH_MULT               (0x0003UL << 30)
+	/* 1 */
+	u32 curr;
+	/* 2 - 8 */
+	struct ci13xxx_td        td;
+	/* 9 */
+	u32 RESERVED;
+	struct usb_ctrlrequest   setup;
+} __attribute__ ((packed));
+
+/**
+ * struct ci13xxx_req - usb request representation
+ * @req: request structure for gadget drivers
+ * @queue: link to QH list
+ * @ptr: transfer descriptor for this request
+ * @dma: dma address for the transfer descriptor
+ * @zptr: transfer descriptor for the zero packet
+ * @zdma: dma address of the zero packet's transfer descriptor
+ */
+struct ci13xxx_req {
+	struct usb_request	req;
+	struct list_head	queue;
+	struct ci13xxx_td	*ptr;
+	dma_addr_t		dma;
+	struct ci13xxx_td	*zptr;
+	dma_addr_t		zdma;
+};
+
+#ifdef CONFIG_USB_CHIPIDEA_UDC
+
+int ci_hdrc_gadget_init(struct ci13xxx *ci);
+
+#else
+
+static inline int ci_hdrc_gadget_init(struct ci13xxx *ci)
+{
+	return -ENXIO;
+}
+
+#endif
+
+#endif /* __DRIVERS_USB_CHIPIDEA_UDC_H */
diff --git a/drivers/usb/class/cdc-acm.c b/drivers/usb/class/cdc-acm.c
index b32ccb46101..f2a120eea9d 100644
--- a/drivers/usb/class/cdc-acm.c
+++ b/drivers/usb/class/cdc-acm.c
@@ -1664,6 +1664,7 @@ static struct usb_driver acm_driver = {
 #ifdef CONFIG_PM
 	.supports_autosuspend = 1,
 #endif
+	.disable_hub_initiated_lpm = 1,
 };
 
 /*
diff --git a/drivers/usb/class/cdc-wdm.c b/drivers/usb/class/cdc-wdm.c
index 0bb2b3248da..ea8b304f0e8 100644
--- a/drivers/usb/class/cdc-wdm.c
+++ b/drivers/usb/class/cdc-wdm.c
@@ -309,9 +309,6 @@ static void free_urbs(struct wdm_device *desc)
 
 static void cleanup(struct wdm_device *desc)
 {
-	spin_lock(&wdm_device_list_lock);
-	list_del(&desc->device_list);
-	spin_unlock(&wdm_device_list_lock);
 	kfree(desc->sbuf);
 	kfree(desc->inbuf);
 	kfree(desc->orq);
@@ -369,6 +366,7 @@ static ssize_t wdm_write
 	r = usb_autopm_get_interface(desc->intf);
 	if (r < 0) {
 		kfree(buf);
+		rv = usb_translate_errors(r);
 		goto outnp;
 	}
 
@@ -384,6 +382,7 @@ static ssize_t wdm_write
 
 	if (r < 0) {
 		kfree(buf);
+		rv = r;
 		goto out;
 	}
 
@@ -415,6 +414,7 @@ static ssize_t wdm_write
 		desc->outbuf = NULL;
 		clear_bit(WDM_IN_USE, &desc->flags);
 		dev_err(&desc->intf->dev, "Tx URB error: %d\n", rv);
+		rv = usb_translate_errors(rv);
 	} else {
 		dev_dbg(&desc->intf->dev, "Tx URB has been submitted index=%d",
 			req->wIndex);
@@ -530,11 +530,13 @@ static int wdm_flush(struct file *file, fl_owner_t id)
 	struct wdm_device *desc = file->private_data;
 
 	wait_event(desc->wait, !test_bit(WDM_IN_USE, &desc->flags));
-	if (desc->werr < 0)
+
+	/* cannot dereference desc->intf if WDM_DISCONNECTING */
+	if (desc->werr < 0 && !test_bit(WDM_DISCONNECTING, &desc->flags))
 		dev_err(&desc->intf->dev, "Error in flush path: %d\n",
 			desc->werr);
 
-	return desc->werr;
+	return usb_translate_errors(desc->werr);
 }
 
 static unsigned int wdm_poll(struct file *file, struct poll_table_struct *wait)
@@ -545,7 +547,7 @@ static unsigned int wdm_poll(struct file *file, struct poll_table_struct *wait)
 
 	spin_lock_irqsave(&desc->iuspin, flags);
 	if (test_bit(WDM_DISCONNECTING, &desc->flags)) {
-		mask = POLLERR;
+		mask = POLLHUP | POLLERR;
 		spin_unlock_irqrestore(&desc->iuspin, flags);
 		goto desc_out;
 	}
@@ -596,6 +598,7 @@ static int wdm_open(struct inode *inode, struct file *file)
 			desc->count--;
 			dev_err(&desc->intf->dev,
 				"Error submitting int urb - %d\n", rv);
+			rv = usb_translate_errors(rv);
 		}
 	} else {
 		rv = 0;
@@ -621,10 +624,15 @@ static int wdm_release(struct inode *inode, struct file *file)
 	mutex_unlock(&desc->wlock);
 
 	if (!desc->count) {
-		dev_dbg(&desc->intf->dev, "wdm_release: cleanup");
-		kill_urbs(desc);
-		if (!test_bit(WDM_DISCONNECTING, &desc->flags))
+		if (!test_bit(WDM_DISCONNECTING, &desc->flags)) {
+			dev_dbg(&desc->intf->dev, "wdm_release: cleanup");
+			kill_urbs(desc);
 			desc->manage_power(desc->intf, 0);
+		} else {
+			/* must avoid dev_printk here as desc->intf is invalid */
+			pr_debug(KBUILD_MODNAME " %s: device gone - cleaning up\n", __func__);
+			cleanup(desc);
+		}
 	}
 	mutex_unlock(&wdm_mutex);
 	return 0;
@@ -771,6 +779,9 @@ static int wdm_create(struct usb_interface *intf, struct usb_endpoint_descriptor
 out:
 	return rv;
 err:
+	spin_lock(&wdm_device_list_lock);
+	list_del(&desc->device_list);
+	spin_unlock(&wdm_device_list_lock);
 	cleanup(desc);
 	return rv;
 }
@@ -896,8 +907,16 @@ static void wdm_disconnect(struct usb_interface *intf)
 	cancel_work_sync(&desc->rxwork);
 	mutex_unlock(&desc->wlock);
 	mutex_unlock(&desc->rlock);
+
+	/* the desc->intf pointer used as list key is now invalid */
+	spin_lock(&wdm_device_list_lock);
+	list_del(&desc->device_list);
+	spin_unlock(&wdm_device_list_lock);
+
 	if (!desc->count)
 		cleanup(desc);
+	else
+		dev_dbg(&intf->dev, "%s: %d open files - postponing cleanup\n", __func__, desc->count);
 	mutex_unlock(&wdm_mutex);
 }
 
@@ -1015,6 +1034,7 @@ static struct usb_driver wdm_driver = {
 	.post_reset =	wdm_post_reset,
 	.id_table =	wdm_ids,
 	.supports_autosuspend = 1,
+	.disable_hub_initiated_lpm = 1,
 };
 
 module_usb_driver(wdm_driver);
diff --git a/drivers/usb/class/usblp.c b/drivers/usb/class/usblp.c
index a68c1a63dc6..d4c47d5d762 100644
--- a/drivers/usb/class/usblp.c
+++ b/drivers/usb/class/usblp.c
@@ -172,27 +172,31 @@ struct usblp {
 #ifdef DEBUG
 static void usblp_dump(struct usblp *usblp)
 {
+	struct device *dev = &usblp->intf->dev;
 	int p;
 
-	dbg("usblp=0x%p", usblp);
-	dbg("dev=0x%p", usblp->dev);
-	dbg("present=%d", usblp->present);
-	dbg("readbuf=0x%p", usblp->readbuf);
-	dbg("readcount=%d", usblp->readcount);
-	dbg("ifnum=%d", usblp->ifnum);
-    for (p = USBLP_FIRST_PROTOCOL; p <= USBLP_LAST_PROTOCOL; p++) {
-	dbg("protocol[%d].alt_setting=%d", p, usblp->protocol[p].alt_setting);
-	dbg("protocol[%d].epwrite=%p", p, usblp->protocol[p].epwrite);
-	dbg("protocol[%d].epread=%p", p, usblp->protocol[p].epread);
-    }
-	dbg("current_protocol=%d", usblp->current_protocol);
-	dbg("minor=%d", usblp->minor);
-	dbg("wstatus=%d", usblp->wstatus);
-	dbg("rstatus=%d", usblp->rstatus);
-	dbg("quirks=%d", usblp->quirks);
-	dbg("used=%d", usblp->used);
-	dbg("bidir=%d", usblp->bidir);
-	dbg("device_id_string=\"%s\"",
+	dev_dbg(dev, "usblp=0x%p\n", usblp);
+	dev_dbg(dev, "dev=0x%p\n", usblp->dev);
+	dev_dbg(dev, "present=%d\n", usblp->present);
+	dev_dbg(dev, "readbuf=0x%p\n", usblp->readbuf);
+	dev_dbg(dev, "readcount=%d\n", usblp->readcount);
+	dev_dbg(dev, "ifnum=%d\n", usblp->ifnum);
+	for (p = USBLP_FIRST_PROTOCOL; p <= USBLP_LAST_PROTOCOL; p++) {
+		dev_dbg(dev, "protocol[%d].alt_setting=%d\n", p,
+			usblp->protocol[p].alt_setting);
+		dev_dbg(dev, "protocol[%d].epwrite=%p\n", p,
+			usblp->protocol[p].epwrite);
+		dev_dbg(dev, "protocol[%d].epread=%p\n", p,
+			usblp->protocol[p].epread);
+	}
+	dev_dbg(dev, "current_protocol=%d\n", usblp->current_protocol);
+	dev_dbg(dev, "minor=%d\n", usblp->minor);
+	dev_dbg(dev, "wstatus=%d\n", usblp->wstatus);
+	dev_dbg(dev, "rstatus=%d\n", usblp->rstatus);
+	dev_dbg(dev, "quirks=%d\n", usblp->quirks);
+	dev_dbg(dev, "used=%d\n", usblp->used);
+	dev_dbg(dev, "bidir=%d\n", usblp->bidir);
+	dev_dbg(dev, "device_id_string=\"%s\"\n",
 		usblp->device_id_string ?
 			usblp->device_id_string + 2 :
 			(unsigned char *)"(null)");
@@ -262,7 +266,8 @@ static int usblp_ctrl_msg(struct usblp *usblp, int request, int type, int dir, i
 	retval = usb_control_msg(usblp->dev,
 		dir ? usb_rcvctrlpipe(usblp->dev, 0) : usb_sndctrlpipe(usblp->dev, 0),
 		request, type | dir | recip, value, index, buf, len, USBLP_CTL_TIMEOUT);
-	dbg("usblp_control_msg: rq: 0x%02x dir: %d recip: %d value: %d idx: %d len: %#x result: %d",
+	dev_dbg(&usblp->intf->dev,
+		"usblp_control_msg: rq: 0x%02x dir: %d recip: %d value: %d idx: %d len: %#x result: %d\n",
 		request, !!dir, recip, value, index, len, retval);
 	return retval < 0 ? retval : 0;
 }
@@ -500,8 +505,9 @@ static long usblp_ioctl(struct file *file, unsigned int cmd, unsigned long arg)
 		goto done;
 	}
 
-	dbg("usblp_ioctl: cmd=0x%x (%c nr=%d len=%d dir=%d)", cmd, _IOC_TYPE(cmd),
-		_IOC_NR(cmd), _IOC_SIZE(cmd), _IOC_DIR(cmd));
+	dev_dbg(&usblp->intf->dev,
+		"usblp_ioctl: cmd=0x%x (%c nr=%d len=%d dir=%d)\n", cmd,
+		_IOC_TYPE(cmd), _IOC_NR(cmd), _IOC_SIZE(cmd), _IOC_DIR(cmd));
 
 	if (_IOC_TYPE(cmd) == 'P')	/* new-style ioctl number */
 
@@ -594,7 +600,8 @@ static long usblp_ioctl(struct file *file, unsigned int cmd, unsigned long arg)
 				goto done;
 			}
 
-			dbg("usblp%d requested/got HP channel %ld/%d",
+			dev_dbg(&usblp->intf->dev,
+				"usblp%d requested/got HP channel %ld/%d\n",
 				usblp->minor, arg, newChannel);
 			break;
 
@@ -614,7 +621,8 @@ static long usblp_ioctl(struct file *file, unsigned int cmd, unsigned long arg)
 				goto done;
 			}
 
-			dbg("usblp%d is bus=%d, device=%d",
+			dev_dbg(&usblp->intf->dev,
+				"usblp%d is bus=%d, device=%d\n",
 				usblp->minor, twoints[0], twoints[1]);
 			break;
 
@@ -634,7 +642,8 @@ static long usblp_ioctl(struct file *file, unsigned int cmd, unsigned long arg)
 				goto done;
 			}
 
-			dbg("usblp%d is VID=0x%4.4X, PID=0x%4.4X",
+			dev_dbg(&usblp->intf->dev,
+				"usblp%d is VID=0x%4.4X, PID=0x%4.4X\n",
 				usblp->minor, twoints[0], twoints[1]);
 			break;
 
@@ -987,7 +996,7 @@ static int usblp_submit_read(struct usblp *usblp)
 	usblp->rcomplete = 0;
 	spin_unlock_irqrestore(&usblp->lock, flags);
 	if ((rc = usb_submit_urb(urb, GFP_KERNEL)) < 0) {
-		dbg("error submitting urb (%d)", rc);
+		dev_dbg(&usblp->intf->dev, "error submitting urb (%d)\n", rc);
 		spin_lock_irqsave(&usblp->lock, flags);
 		usblp->rstatus = rc;
 		usblp->rcomplete = 1;
@@ -1129,7 +1138,8 @@ static int usblp_probe(struct usb_interface *intf,
 	/* Analyze and pick initial alternate settings and endpoints. */
 	protocol = usblp_select_alts(usblp);
 	if (protocol < 0) {
-		dbg("incompatible printer-class device 0x%4.4X/0x%4.4X",
+		dev_dbg(&intf->dev,
+			"incompatible printer-class device 0x%4.4X/0x%4.4X\n",
 			le16_to_cpu(dev->descriptor.idVendor),
 			le16_to_cpu(dev->descriptor.idProduct));
 		retval = -ENODEV;
@@ -1158,14 +1168,14 @@ static int usblp_probe(struct usb_interface *intf,
 
 	retval = usb_register_dev(intf, &usblp_class);
 	if (retval) {
-		printk(KERN_ERR "usblp: Not able to get a minor"
-		    " (base %u, slice default): %d\n",
-		    USBLP_MINOR_BASE, retval);
+		dev_err(&intf->dev,
+			"usblp: Not able to get a minor (base %u, slice default): %d\n",
+			USBLP_MINOR_BASE, retval);
 		goto abort_intfdata;
 	}
 	usblp->minor = intf->minor;
-	printk(KERN_INFO "usblp%d: USB %sdirectional printer dev %d "
-		"if %d alt %d proto %d vid 0x%4.4X pid 0x%4.4X\n",
+	dev_info(&intf->dev,
+		"usblp%d: USB %sdirectional printer dev %d if %d alt %d proto %d vid 0x%4.4X pid 0x%4.4X\n",
 		usblp->minor, usblp->bidir ? "Bi" : "Uni", dev->devnum,
 		usblp->ifnum,
 		usblp->protocol[usblp->current_protocol].alt_setting,
@@ -1302,7 +1312,8 @@ static int usblp_set_protocol(struct usblp *usblp, int protocol)
 
 	usblp->bidir = (usblp->protocol[protocol].epread != NULL);
 	usblp->current_protocol = protocol;
-	dbg("usblp%d set protocol %d", usblp->minor, protocol);
+	dev_dbg(&usblp->intf->dev, "usblp%d set protocol %d\n",
+		usblp->minor, protocol);
 	return 0;
 }
 
@@ -1315,7 +1326,8 @@ static int usblp_cache_device_id_string(struct usblp *usblp)
 
 	err = usblp_get_id(usblp, 0, usblp->device_id_string, USBLP_DEVICE_ID_SIZE - 1);
 	if (err < 0) {
-		dbg("usblp%d: error = %d reading IEEE-1284 Device ID string",
+		dev_dbg(&usblp->intf->dev,
+			"usblp%d: error = %d reading IEEE-1284 Device ID string\n",
 			usblp->minor, err);
 		usblp->device_id_string[0] = usblp->device_id_string[1] = '\0';
 		return -EIO;
@@ -1331,7 +1343,7 @@ static int usblp_cache_device_id_string(struct usblp *usblp)
 		length = USBLP_DEVICE_ID_SIZE - 1;
 	usblp->device_id_string[length] = '\0';
 
-	dbg("usblp%d Device ID string [len=%d]=\"%s\"",
+	dev_dbg(&usblp->intf->dev, "usblp%d Device ID string [len=%d]=\"%s\"\n",
 		usblp->minor, length, &usblp->device_id_string[2]);
 
 	return length;
diff --git a/drivers/usb/core/Kconfig b/drivers/usb/core/Kconfig
index 18d02e32a3d..9981984b365 100644
--- a/drivers/usb/core/Kconfig
+++ b/drivers/usb/core/Kconfig
@@ -27,58 +27,6 @@ config USB_ANNOUNCE_NEW_DEVICES
 comment "Miscellaneous USB options"
 	depends on USB
 
-config USB_DEVICEFS
-	bool "USB device filesystem (DEPRECATED)"
-	depends on USB
-	---help---
-	  If you say Y here (and to "/proc file system support" in the "File
-	  systems" section, above), you will get a file /proc/bus/usb/devices
-	  which lists the devices currently connected to your USB bus or
-	  busses, and for every connected device a file named
-	  "/proc/bus/usb/xxx/yyy", where xxx is the bus number and yyy the
-	  device number; the latter files can be used by user space programs
-	  to talk directly to the device. These files are "virtual", meaning
-	  they are generated on the fly and not stored on the hard drive.
-
-	  You may need to mount the usbfs file system to see the files, use
-	  mount -t usbfs none /proc/bus/usb
-
-	  For the format of the various /proc/bus/usb/ files, please read
-	  <file:Documentation/usb/proc_usb_info.txt>.
-
-	  Modern Linux systems do not use this.
-
-	  Usbfs entries are files and not character devices; usbfs can't
-	  handle Access Control Lists (ACL) which are the default way to
-	  grant access to USB devices for untrusted users of a desktop
-	  system.
-
-	  The usbfs functionality is replaced by real device-nodes managed by
-	  udev.  These nodes lived in /dev/bus/usb and are used by libusb.
-
-config USB_DEVICE_CLASS
-	bool "USB device class-devices (DEPRECATED)"
-	depends on USB
-	default y
-	---help---
-	  Userspace access to USB devices is granted by device-nodes exported
-	  directly from the usbdev in sysfs. Old versions of the driver
-	  core and udev needed additional class devices to export device nodes.
-
-	  These additional devices are difficult to handle in userspace, if
-	  information about USB interfaces must be available. One device
-	  contains the device node, the other device contains the interface
-	  data. Both devices are at the same level in sysfs (siblings) and one
-	  can't access the other. The device node created directly by the
-	  usb device is the parent device of the interface and therefore
-	  easily accessible from the interface event.
-
-	  This option provides backward compatibility for libusb device
-	  nodes (lsusb) when usbfs is not used, and the following udev rule
-	  doesn't exist:
-	    SUBSYSTEM=="usb", ACTION=="add", ENV{DEVTYPE}=="usb_device", \
-	    NAME="bus/usb/$env{BUSNUM}/$env{DEVNUM}", MODE="0644"
-
 config USB_DYNAMIC_MINORS
 	bool "Dynamic USB minor allocation"
 	depends on USB
@@ -125,7 +73,6 @@ config USB_OTG_WHITELIST
 	bool "Rely on OTG Targeted Peripherals List"
 	depends on USB_OTG || EXPERT
 	default y if USB_OTG
-	default n if EXPERT
 	help
 	  If you say Y here, the "otg_whitelist.h" file will be used as a
 	  product whitelist, so USB peripherals not listed there will be
diff --git a/drivers/usb/core/Makefile b/drivers/usb/core/Makefile
index 507a4e1b636..26059b93dbf 100644
--- a/drivers/usb/core/Makefile
+++ b/drivers/usb/core/Makefile
@@ -9,6 +9,6 @@ usbcore-y += config.o file.o buffer.o sysfs.o endpoint.o
 usbcore-y += devio.o notify.o generic.o quirks.o devices.o
 
 usbcore-$(CONFIG_PCI)		+= hcd-pci.o
-usbcore-$(CONFIG_USB_DEVICEFS)	+= inode.o
+usbcore-$(CONFIG_ACPI)		+= usb-acpi.o
 
 obj-$(CONFIG_USB)		+= usbcore.o
diff --git a/drivers/usb/core/devio.c b/drivers/usb/core/devio.c
index 8df4b76465a..e0f107948eb 100644
--- a/drivers/usb/core/devio.c
+++ b/drivers/usb/core/devio.c
@@ -333,17 +333,14 @@ static struct async *async_getcompleted(struct dev_state *ps)
 static struct async *async_getpending(struct dev_state *ps,
 					     void __user *userurb)
 {
-	unsigned long flags;
 	struct async *as;
 
-	spin_lock_irqsave(&ps->lock, flags);
 	list_for_each_entry(as, &ps->async_pending, asynclist)
 		if (as->userurb == userurb) {
 			list_del_init(&as->asynclist);
-			spin_unlock_irqrestore(&ps->lock, flags);
 			return as;
 		}
-	spin_unlock_irqrestore(&ps->lock, flags);
+
 	return NULL;
 }
 
@@ -398,6 +395,7 @@ static void cancel_bulk_urbs(struct dev_state *ps, unsigned bulk_addr)
 __releases(ps->lock)
 __acquires(ps->lock)
 {
+	struct urb *urb;
 	struct async *as;
 
 	/* Mark all the pending URBs that match bulk_addr, up to but not
@@ -420,8 +418,11 @@ __acquires(ps->lock)
 	list_for_each_entry(as, &ps->async_pending, asynclist) {
 		if (as->bulk_status == AS_UNLINK) {
 			as->bulk_status = 0;		/* Only once */
+			urb = as->urb;
+			usb_get_urb(urb);
 			spin_unlock(&ps->lock);		/* Allow completions */
-			usb_unlink_urb(as->urb);
+			usb_unlink_urb(urb);
+			usb_put_urb(urb);
 			spin_lock(&ps->lock);
 			goto rescan;
 		}
@@ -472,6 +473,7 @@ static void async_completed(struct urb *urb)
 
 static void destroy_async(struct dev_state *ps, struct list_head *list)
 {
+	struct urb *urb;
 	struct async *as;
 	unsigned long flags;
 
@@ -479,10 +481,13 @@ static void destroy_async(struct dev_state *ps, struct list_head *list)
 	while (!list_empty(list)) {
 		as = list_entry(list->next, struct async, asynclist);
 		list_del_init(&as->asynclist);
+		urb = as->urb;
+		usb_get_urb(urb);
 
 		/* drop the spinlock so the completion handler can run */
 		spin_unlock_irqrestore(&ps->lock, flags);
-		usb_kill_urb(as->urb);
+		usb_kill_urb(urb);
+		usb_put_urb(urb);
 		spin_lock_irqsave(&ps->lock, flags);
 	}
 	spin_unlock_irqrestore(&ps->lock, flags);
@@ -727,17 +732,6 @@ static int usbdev_open(struct inode *inode, struct file *file)
 	if (imajor(inode) == USB_DEVICE_MAJOR)
 		dev = usbdev_lookup_by_devt(inode->i_rdev);
 
-#ifdef CONFIG_USB_DEVICEFS
-	/* procfs file */
-	if (!dev) {
-		dev = inode->i_private;
-		if (dev && dev->usbfs_dentry &&
-					dev->usbfs_dentry->d_inode == inode)
-			usb_get_dev(dev);
-		else
-			dev = NULL;
-	}
-#endif
 	mutex_unlock(&usbfs_mutex);
 
 	if (!dev)
@@ -1410,12 +1404,24 @@ static int proc_submiturb(struct dev_state *ps, void __user *arg)
 
 static int proc_unlinkurb(struct dev_state *ps, void __user *arg)
 {
+	struct urb *urb;
 	struct async *as;
+	unsigned long flags;
 
+	spin_lock_irqsave(&ps->lock, flags);
 	as = async_getpending(ps, arg);
-	if (!as)
+	if (!as) {
+		spin_unlock_irqrestore(&ps->lock, flags);
 		return -EINVAL;
-	usb_kill_urb(as->urb);
+	}
+
+	urb = as->urb;
+	usb_get_urb(urb);
+	spin_unlock_irqrestore(&ps->lock, flags);
+
+	usb_kill_urb(urb);
+	usb_put_urb(urb);
+
 	return 0;
 }
 
@@ -2062,44 +2068,13 @@ static void usbdev_remove(struct usb_device *udev)
 	}
 }
 
-#ifdef CONFIG_USB_DEVICE_CLASS
-static struct class *usb_classdev_class;
-
-static int usb_classdev_add(struct usb_device *dev)
-{
-	struct device *cldev;
-
-	cldev = device_create(usb_classdev_class, &dev->dev, dev->dev.devt,
-			      NULL, "usbdev%d.%d", dev->bus->busnum,
-			      dev->devnum);
-	if (IS_ERR(cldev))
-		return PTR_ERR(cldev);
-	dev->usb_classdev = cldev;
-	return 0;
-}
-
-static void usb_classdev_remove(struct usb_device *dev)
-{
-	if (dev->usb_classdev)
-		device_unregister(dev->usb_classdev);
-}
-
-#else
-#define usb_classdev_add(dev)		0
-#define usb_classdev_remove(dev)	do {} while (0)
-
-#endif
-
 static int usbdev_notify(struct notifier_block *self,
 			       unsigned long action, void *dev)
 {
 	switch (action) {
 	case USB_DEVICE_ADD:
-		if (usb_classdev_add(dev))
-			return NOTIFY_BAD;
 		break;
 	case USB_DEVICE_REMOVE:
-		usb_classdev_remove(dev);
 		usbdev_remove(dev);
 		break;
 	}
@@ -2129,21 +2104,6 @@ int __init usb_devio_init(void)
 		       USB_DEVICE_MAJOR);
 		goto error_cdev;
 	}
-#ifdef CONFIG_USB_DEVICE_CLASS
-	usb_classdev_class = class_create(THIS_MODULE, "usb_device");
-	if (IS_ERR(usb_classdev_class)) {
-		printk(KERN_ERR "Unable to register usb_device class\n");
-		retval = PTR_ERR(usb_classdev_class);
-		cdev_del(&usb_device_cdev);
-		usb_classdev_class = NULL;
-		goto out;
-	}
-	/* devices of this class shadow the major:minor of their parent
-	 * device, so clear ->dev_kobj to prevent adding duplicate entries
-	 * to /sys/dev
-	 */
-	usb_classdev_class->dev_kobj = NULL;
-#endif
 	usb_register_notify(&usbdev_nb);
 out:
 	return retval;
@@ -2156,9 +2116,6 @@ error_cdev:
 void usb_devio_cleanup(void)
 {
 	usb_unregister_notify(&usbdev_nb);
-#ifdef CONFIG_USB_DEVICE_CLASS
-	class_destroy(usb_classdev_class);
-#endif
 	cdev_del(&usb_device_cdev);
 	unregister_chrdev_region(USB_DEVICE_DEV, USB_DEVICE_MAX);
 }
diff --git a/drivers/usb/core/driver.c b/drivers/usb/core/driver.c
index 9a56635dc19..f536aebc958 100644
--- a/drivers/usb/core/driver.c
+++ b/drivers/usb/core/driver.c
@@ -79,6 +79,30 @@ ssize_t usb_store_new_id(struct usb_dynids *dynids,
 }
 EXPORT_SYMBOL_GPL(usb_store_new_id);
 
+ssize_t usb_show_dynids(struct usb_dynids *dynids, char *buf)
+{
+	struct usb_dynid *dynid;
+	size_t count = 0;
+
+	list_for_each_entry(dynid, &dynids->list, node)
+		if (dynid->id.bInterfaceClass != 0)
+			count += scnprintf(&buf[count], PAGE_SIZE - count, "%04x %04x %02x\n",
+					   dynid->id.idVendor, dynid->id.idProduct,
+					   dynid->id.bInterfaceClass);
+		else
+			count += scnprintf(&buf[count], PAGE_SIZE - count, "%04x %04x\n",
+					   dynid->id.idVendor, dynid->id.idProduct);
+	return count;
+}
+EXPORT_SYMBOL_GPL(usb_show_dynids);
+
+static ssize_t show_dynids(struct device_driver *driver, char *buf)
+{
+	struct usb_driver *usb_drv = to_usb_driver(driver);
+
+	return usb_show_dynids(&usb_drv->dynids, buf);
+}
+
 static ssize_t store_new_id(struct device_driver *driver,
 			    const char *buf, size_t count)
 {
@@ -86,7 +110,7 @@ static ssize_t store_new_id(struct device_driver *driver,
 
 	return usb_store_new_id(&usb_drv->dynids, driver, buf, count);
 }
-static DRIVER_ATTR(new_id, S_IWUSR, NULL, store_new_id);
+static DRIVER_ATTR(new_id, S_IRUGO | S_IWUSR, show_dynids, store_new_id);
 
 /**
  * store_remove_id - remove a USB device ID from this driver
@@ -127,7 +151,7 @@ store_remove_id(struct device_driver *driver, const char *buf, size_t count)
 		return retval;
 	return count;
 }
-static DRIVER_ATTR(remove_id, S_IWUSR, NULL, store_remove_id);
+static DRIVER_ATTR(remove_id, S_IRUGO | S_IWUSR, show_dynids, store_remove_id);
 
 static int usb_create_newid_files(struct usb_driver *usb_drv)
 {
@@ -264,6 +288,7 @@ static int usb_probe_interface(struct device *dev)
 	struct usb_device *udev = interface_to_usbdev(intf);
 	const struct usb_device_id *id;
 	int error = -ENODEV;
+	int lpm_disable_error;
 
 	dev_dbg(dev, "%s\n", __func__);
 
@@ -300,6 +325,25 @@ static int usb_probe_interface(struct device *dev)
 	if (driver->supports_autosuspend)
 		pm_runtime_enable(dev);
 
+	/* If the new driver doesn't allow hub-initiated LPM, and we can't
+	 * disable hub-initiated LPM, then fail the probe.
+	 *
+	 * Otherwise, leaving LPM enabled should be harmless, because the
+	 * endpoint intervals should remain the same, and the U1/U2 timeouts
+	 * should remain the same.
+	 *
+	 * If we need to install alt setting 0 before probe, or another alt
+	 * setting during probe, that should also be fine.  usb_set_interface()
+	 * will attempt to disable LPM, and fail if it can't disable it.
+	 */
+	lpm_disable_error = usb_unlocked_disable_lpm(udev);
+	if (lpm_disable_error && driver->disable_hub_initiated_lpm) {
+		dev_err(&intf->dev, "%s Failed to disable LPM for driver %s\n.",
+				__func__, driver->name);
+		error = lpm_disable_error;
+		goto err;
+	}
+
 	/* Carry out a deferred switch to altsetting 0 */
 	if (intf->needs_altsetting0) {
 		error = usb_set_interface(udev, intf->altsetting[0].
@@ -314,6 +358,11 @@ static int usb_probe_interface(struct device *dev)
 		goto err;
 
 	intf->condition = USB_INTERFACE_BOUND;
+
+	/* If the LPM disable succeeded, balance the ref counts. */
+	if (!lpm_disable_error)
+		usb_unlocked_enable_lpm(udev);
+
 	usb_autosuspend_device(udev);
 	return error;
 
@@ -337,7 +386,7 @@ static int usb_unbind_interface(struct device *dev)
 	struct usb_driver *driver = to_usb_driver(dev->driver);
 	struct usb_interface *intf = to_usb_interface(dev);
 	struct usb_device *udev;
-	int error, r;
+	int error, r, lpm_disable_error;
 
 	intf->condition = USB_INTERFACE_UNBINDING;
 
@@ -345,6 +394,13 @@ static int usb_unbind_interface(struct device *dev)
 	udev = interface_to_usbdev(intf);
 	error = usb_autoresume_device(udev);
 
+	/* Hub-initiated LPM policy may change, so attempt to disable LPM until
+	 * the driver is unbound.  If LPM isn't disabled, that's fine because it
+	 * wouldn't be enabled unless all the bound interfaces supported
+	 * hub-initiated LPM.
+	 */
+	lpm_disable_error = usb_unlocked_disable_lpm(udev);
+
 	/* Terminate all URBs for this interface unless the driver
 	 * supports "soft" unbinding.
 	 */
@@ -378,6 +434,10 @@ static int usb_unbind_interface(struct device *dev)
 	intf->condition = USB_INTERFACE_UNBOUND;
 	intf->needs_remote_wakeup = 0;
 
+	/* Attempt to re-enable USB3 LPM, if the disable succeeded. */
+	if (!lpm_disable_error)
+		usb_unlocked_enable_lpm(udev);
+
 	/* Unbound interfaces are always runtime-PM-disabled and -suspended */
 	if (driver->supports_autosuspend)
 		pm_runtime_disable(dev);
@@ -418,17 +478,29 @@ int usb_driver_claim_interface(struct usb_driver *driver,
 				struct usb_interface *iface, void *priv)
 {
 	struct device *dev = &iface->dev;
+	struct usb_device *udev;
 	int retval = 0;
+	int lpm_disable_error;
 
 	if (dev->driver)
 		return -EBUSY;
 
+	udev = interface_to_usbdev(iface);
+
 	dev->driver = &driver->drvwrap.driver;
 	usb_set_intfdata(iface, priv);
 	iface->needs_binding = 0;
 
 	iface->condition = USB_INTERFACE_BOUND;
 
+	/* Disable LPM until this driver is bound. */
+	lpm_disable_error = usb_unlocked_disable_lpm(udev);
+	if (lpm_disable_error && driver->disable_hub_initiated_lpm) {
+		dev_err(&iface->dev, "%s Failed to disable LPM for driver %s\n.",
+				__func__, driver->name);
+		return -ENOMEM;
+	}
+
 	/* Claimed interfaces are initially inactive (suspended) and
 	 * runtime-PM-enabled, but only if the driver has autosuspend
 	 * support.  Otherwise they are marked active, to prevent the
@@ -447,6 +519,10 @@ int usb_driver_claim_interface(struct usb_driver *driver,
 	if (device_is_registered(dev))
 		retval = device_bind_driver(dev);
 
+	/* Attempt to re-enable USB3 LPM, if the disable was successful. */
+	if (!lpm_disable_error)
+		usb_unlocked_enable_lpm(udev);
+
 	return retval;
 }
 EXPORT_SYMBOL_GPL(usb_driver_claim_interface);
@@ -726,16 +802,6 @@ static int usb_uevent(struct device *dev, struct kobj_uevent_env *env)
 		return -ENODEV;
 	}
 
-#ifdef	CONFIG_USB_DEVICEFS
-	/* If this is available, userspace programs can directly read
-	 * all the device descriptors we don't tell them about.  Or
-	 * act as usermode drivers.
-	 */
-	if (add_uevent_var(env, "DEVICE=/proc/bus/usb/%03d/%03d",
-			   usb_dev->bus->busnum, usb_dev->devnum))
-		return -ENOMEM;
-#endif
-
 	/* per-device configurations are common */
 	if (add_uevent_var(env, "PRODUCT=%x/%x/%x",
 			   le16_to_cpu(usb_dev->descriptor.idVendor),
@@ -788,15 +854,13 @@ int usb_register_device_driver(struct usb_device_driver *new_udriver,
 
 	retval = driver_register(&new_udriver->drvwrap.driver);
 
-	if (!retval) {
+	if (!retval)
 		pr_info("%s: registered new device driver %s\n",
 			usbcore_name, new_udriver->name);
-		usbfs_update_special();
-	} else {
+	else
 		printk(KERN_ERR "%s: error %d registering device "
 			"	driver %s\n",
 			usbcore_name, retval, new_udriver->name);
-	}
 
 	return retval;
 }
@@ -815,7 +879,6 @@ void usb_deregister_device_driver(struct usb_device_driver *udriver)
 			usbcore_name, udriver->name);
 
 	driver_unregister(&udriver->drvwrap.driver);
-	usbfs_update_special();
 }
 EXPORT_SYMBOL_GPL(usb_deregister_device_driver);
 
@@ -856,8 +919,6 @@ int usb_register_driver(struct usb_driver *new_driver, struct module *owner,
 	if (retval)
 		goto out;
 
-	usbfs_update_special();
-
 	retval = usb_create_newid_files(new_driver);
 	if (retval)
 		goto out_newid;
@@ -897,8 +958,6 @@ void usb_deregister(struct usb_driver *driver)
 	usb_remove_newid_files(driver);
 	driver_unregister(&driver->drvwrap.driver);
 	usb_free_dynids(driver);
-
-	usbfs_update_special();
 }
 EXPORT_SYMBOL_GPL(usb_deregister);
 
diff --git a/drivers/usb/core/file.c b/drivers/usb/core/file.c
index d95760de9e8..e673b26e598 100644
--- a/drivers/usb/core/file.c
+++ b/drivers/usb/core/file.c
@@ -183,7 +183,7 @@ int usb_register_dev(struct usb_interface *intf,
 	if (retval)
 		return retval;
 
-	dev_dbg(&intf->dev, "looking for a minor, starting at %d", minor_base);
+	dev_dbg(&intf->dev, "looking for a minor, starting at %d\n", minor_base);
 
 	down_write(&minor_rwsem);
 	for (minor = minor_base; minor < MAX_USB_MINORS; ++minor) {
@@ -239,7 +239,7 @@ void usb_deregister_dev(struct usb_interface *intf,
 	if (intf->minor == -1)
 		return;
 
-	dbg ("removing %d minor", intf->minor);
+	dev_dbg(&intf->dev, "removing %d minor\n", intf->minor);
 
 	down_write(&minor_rwsem);
 	usb_minors[intf->minor] = NULL;
diff --git a/drivers/usb/core/hcd.c b/drivers/usb/core/hcd.c
index 140d3e11f21..190b1ec7bdc 100644
--- a/drivers/usb/core/hcd.c
+++ b/drivers/usb/core/hcd.c
@@ -138,7 +138,7 @@ static const u8 usb3_rh_dev_descriptor[18] = {
 	0x03,       /*  __u8  bDeviceProtocol; USB 3.0 hub */
 	0x09,       /*  __u8  bMaxPacketSize0; 2^9 = 512 Bytes */
 
-	0x6b, 0x1d, /*  __le16 idVendor; Linux Foundation */
+	0x6b, 0x1d, /*  __le16 idVendor; Linux Foundation 0x1d6b */
 	0x03, 0x00, /*  __le16 idProduct; device 0x0003 */
 	KERNEL_VER, KERNEL_REL, /*  __le16 bcdDevice */
 
@@ -159,7 +159,7 @@ static const u8 usb2_rh_dev_descriptor [18] = {
 	0x00,       /*  __u8  bDeviceProtocol; [ usb 2.0 no TT ] */
 	0x40,       /*  __u8  bMaxPacketSize0; 64 Bytes */
 
-	0x6b, 0x1d, /*  __le16 idVendor; Linux Foundation */
+	0x6b, 0x1d, /*  __le16 idVendor; Linux Foundation 0x1d6b */
 	0x02, 0x00, /*  __le16 idProduct; device 0x0002 */
 	KERNEL_VER, KERNEL_REL, /*  __le16 bcdDevice */
 
@@ -182,7 +182,7 @@ static const u8 usb11_rh_dev_descriptor [18] = {
 	0x00,       /*  __u8  bDeviceProtocol; [ low/full speeds only ] */
 	0x40,       /*  __u8  bMaxPacketSize0; 64 Bytes */
 
-	0x6b, 0x1d, /*  __le16 idVendor; Linux Foundation */
+	0x6b, 0x1d, /*  __le16 idVendor; Linux Foundation 0x1d6b */
 	0x01, 0x00, /*  __le16 idProduct; device 0x0001 */
 	KERNEL_VER, KERNEL_REL, /*  __le16 bcdDevice */
 
@@ -997,6 +997,15 @@ static int register_root_hub(struct usb_hcd *hcd)
 				dev_name(&usb_dev->dev), retval);
 		return (retval < 0) ? retval : -EMSGSIZE;
 	}
+	if (usb_dev->speed == USB_SPEED_SUPER) {
+		retval = usb_get_bos_descriptor(usb_dev);
+		if (retval < 0) {
+			mutex_unlock(&usb_bus_list_lock);
+			dev_dbg(parent_dev, "can't read %s bos descriptor %d\n",
+					dev_name(&usb_dev->dev), retval);
+			return retval;
+		}
+	}
 
 	retval = usb_new_device (usb_dev);
 	if (retval) {
diff --git a/drivers/usb/core/hub.c b/drivers/usb/core/hub.c
index ec6c97dadbe..04fb834c3fa 100644
--- a/drivers/usb/core/hub.c
+++ b/drivers/usb/core/hub.c
@@ -177,6 +177,228 @@ static struct usb_hub *hdev_to_hub(struct usb_device *hdev)
 	return usb_get_intfdata(hdev->actconfig->interface[0]);
 }
 
+static int usb_device_supports_lpm(struct usb_device *udev)
+{
+	/* USB 2.1 (and greater) devices indicate LPM support through
+	 * their USB 2.0 Extended Capabilities BOS descriptor.
+	 */
+	if (udev->speed == USB_SPEED_HIGH) {
+		if (udev->bos->ext_cap &&
+			(USB_LPM_SUPPORT &
+			 le32_to_cpu(udev->bos->ext_cap->bmAttributes)))
+			return 1;
+		return 0;
+	}
+
+	/* All USB 3.0 must support LPM, but we need their max exit latency
+	 * information from the SuperSpeed Extended Capabilities BOS descriptor.
+	 */
+	if (!udev->bos->ss_cap) {
+		dev_warn(&udev->dev, "No LPM exit latency info found.  "
+				"Power management will be impacted.\n");
+		return 0;
+	}
+	if (udev->parent->lpm_capable)
+		return 1;
+
+	dev_warn(&udev->dev, "Parent hub missing LPM exit latency info.  "
+			"Power management will be impacted.\n");
+	return 0;
+}
+
+/*
+ * Set the Maximum Exit Latency (MEL) for the host to initiate a transition from
+ * either U1 or U2.
+ */
+static void usb_set_lpm_mel(struct usb_device *udev,
+		struct usb3_lpm_parameters *udev_lpm_params,
+		unsigned int udev_exit_latency,
+		struct usb_hub *hub,
+		struct usb3_lpm_parameters *hub_lpm_params,
+		unsigned int hub_exit_latency)
+{
+	unsigned int total_mel;
+	unsigned int device_mel;
+	unsigned int hub_mel;
+
+	/*
+	 * Calculate the time it takes to transition all links from the roothub
+	 * to the parent hub into U0.  The parent hub must then decode the
+	 * packet (hub header decode latency) to figure out which port it was
+	 * bound for.
+	 *
+	 * The Hub Header decode latency is expressed in 0.1us intervals (0x1
+	 * means 0.1us).  Multiply that by 100 to get nanoseconds.
+	 */
+	total_mel = hub_lpm_params->mel +
+		(hub->descriptor->u.ss.bHubHdrDecLat * 100);
+
+	/*
+	 * How long will it take to transition the downstream hub's port into
+	 * U0?  The greater of either the hub exit latency or the device exit
+	 * latency.
+	 *
+	 * The BOS U1/U2 exit latencies are expressed in 1us intervals.
+	 * Multiply that by 1000 to get nanoseconds.
+	 */
+	device_mel = udev_exit_latency * 1000;
+	hub_mel = hub_exit_latency * 1000;
+	if (device_mel > hub_mel)
+		total_mel += device_mel;
+	else
+		total_mel += hub_mel;
+
+	udev_lpm_params->mel = total_mel;
+}
+
+/*
+ * Set the maximum Device to Host Exit Latency (PEL) for the device to initiate
+ * a transition from either U1 or U2.
+ */
+static void usb_set_lpm_pel(struct usb_device *udev,
+		struct usb3_lpm_parameters *udev_lpm_params,
+		unsigned int udev_exit_latency,
+		struct usb_hub *hub,
+		struct usb3_lpm_parameters *hub_lpm_params,
+		unsigned int hub_exit_latency,
+		unsigned int port_to_port_exit_latency)
+{
+	unsigned int first_link_pel;
+	unsigned int hub_pel;
+
+	/*
+	 * First, the device sends an LFPS to transition the link between the
+	 * device and the parent hub into U0.  The exit latency is the bigger of
+	 * the device exit latency or the hub exit latency.
+	 */
+	if (udev_exit_latency > hub_exit_latency)
+		first_link_pel = udev_exit_latency * 1000;
+	else
+		first_link_pel = hub_exit_latency * 1000;
+
+	/*
+	 * When the hub starts to receive the LFPS, there is a slight delay for
+	 * it to figure out that one of the ports is sending an LFPS.  Then it
+	 * will forward the LFPS to its upstream link.  The exit latency is the
+	 * delay, plus the PEL that we calculated for this hub.
+	 */
+	hub_pel = port_to_port_exit_latency * 1000 + hub_lpm_params->pel;
+
+	/*
+	 * According to figure C-7 in the USB 3.0 spec, the PEL for this device
+	 * is the greater of the two exit latencies.
+	 */
+	if (first_link_pel > hub_pel)
+		udev_lpm_params->pel = first_link_pel;
+	else
+		udev_lpm_params->pel = hub_pel;
+}
+
+/*
+ * Set the System Exit Latency (SEL) to indicate the total worst-case time from
+ * when a device initiates a transition to U0, until when it will receive the
+ * first packet from the host controller.
+ *
+ * Section C.1.5.1 describes the four components to this:
+ *  - t1: device PEL
+ *  - t2: time for the ERDY to make it from the device to the host.
+ *  - t3: a host-specific delay to process the ERDY.
+ *  - t4: time for the packet to make it from the host to the device.
+ *
+ * t3 is specific to both the xHCI host and the platform the host is integrated
+ * into.  The Intel HW folks have said it's negligible, FIXME if a different
+ * vendor says otherwise.
+ */
+static void usb_set_lpm_sel(struct usb_device *udev,
+		struct usb3_lpm_parameters *udev_lpm_params)
+{
+	struct usb_device *parent;
+	unsigned int num_hubs;
+	unsigned int total_sel;
+
+	/* t1 = device PEL */
+	total_sel = udev_lpm_params->pel;
+	/* How many external hubs are in between the device & the root port. */
+	for (parent = udev->parent, num_hubs = 0; parent->parent;
+			parent = parent->parent)
+		num_hubs++;
+	/* t2 = 2.1us + 250ns * (num_hubs - 1) */
+	if (num_hubs > 0)
+		total_sel += 2100 + 250 * (num_hubs - 1);
+
+	/* t4 = 250ns * num_hubs */
+	total_sel += 250 * num_hubs;
+
+	udev_lpm_params->sel = total_sel;
+}
+
+static void usb_set_lpm_parameters(struct usb_device *udev)
+{
+	struct usb_hub *hub;
+	unsigned int port_to_port_delay;
+	unsigned int udev_u1_del;
+	unsigned int udev_u2_del;
+	unsigned int hub_u1_del;
+	unsigned int hub_u2_del;
+
+	if (!udev->lpm_capable || udev->speed != USB_SPEED_SUPER)
+		return;
+
+	hub = hdev_to_hub(udev->parent);
+	/* It doesn't take time to transition the roothub into U0, since it
+	 * doesn't have an upstream link.
+	 */
+	if (!hub)
+		return;
+
+	udev_u1_del = udev->bos->ss_cap->bU1devExitLat;
+	udev_u2_del = udev->bos->ss_cap->bU2DevExitLat;
+	hub_u1_del = udev->parent->bos->ss_cap->bU1devExitLat;
+	hub_u2_del = udev->parent->bos->ss_cap->bU2DevExitLat;
+
+	usb_set_lpm_mel(udev, &udev->u1_params, udev_u1_del,
+			hub, &udev->parent->u1_params, hub_u1_del);
+
+	usb_set_lpm_mel(udev, &udev->u2_params, udev_u2_del,
+			hub, &udev->parent->u2_params, hub_u2_del);
+
+	/*
+	 * Appendix C, section C.2.2.2, says that there is a slight delay from
+	 * when the parent hub notices the downstream port is trying to
+	 * transition to U0 to when the hub initiates a U0 transition on its
+	 * upstream port.  The section says the delays are tPort2PortU1EL and
+	 * tPort2PortU2EL, but it doesn't define what they are.
+	 *
+	 * The hub chapter, sections 10.4.2.4 and 10.4.2.5 seem to be talking
+	 * about the same delays.  Use the maximum delay calculations from those
+	 * sections.  For U1, it's tHubPort2PortExitLat, which is 1us max.  For
+	 * U2, it's tHubPort2PortExitLat + U2DevExitLat - U1DevExitLat.  I
+	 * assume the device exit latencies they are talking about are the hub
+	 * exit latencies.
+	 *
+	 * What do we do if the U2 exit latency is less than the U1 exit
+	 * latency?  It's possible, although not likely...
+	 */
+	port_to_port_delay = 1;
+
+	usb_set_lpm_pel(udev, &udev->u1_params, udev_u1_del,
+			hub, &udev->parent->u1_params, hub_u1_del,
+			port_to_port_delay);
+
+	if (hub_u2_del > hub_u1_del)
+		port_to_port_delay = 1 + hub_u2_del - hub_u1_del;
+	else
+		port_to_port_delay = 1 + hub_u1_del;
+
+	usb_set_lpm_pel(udev, &udev->u2_params, udev_u2_del,
+			hub, &udev->parent->u2_params, hub_u2_del,
+			port_to_port_delay);
+
+	/* Now that we've got PEL, calculate SEL. */
+	usb_set_lpm_sel(udev, &udev->u1_params);
+	usb_set_lpm_sel(udev, &udev->u2_params);
+}
+
 /* USB 2.0 spec Section 11.24.4.5 */
 static int get_hub_descriptor(struct usb_device *hdev, void *data)
 {
@@ -2480,6 +2702,12 @@ int usb_port_suspend(struct usb_device *udev, pm_message_t msg)
 	if (udev->usb2_hw_lpm_enabled == 1)
 		usb_set_usb2_hardware_lpm(udev, 0);
 
+	if (usb_unlocked_disable_lpm(udev)) {
+		dev_err(&udev->dev, "%s Failed to disable LPM before suspend\n.",
+				__func__);
+		return -ENOMEM;
+	}
+
 	/* see 7.1.7.6 */
 	if (hub_is_superspeed(hub->hdev))
 		status = set_port_feature(hub->hdev,
@@ -2499,6 +2727,13 @@ int usb_port_suspend(struct usb_device *udev, pm_message_t msg)
 				NULL, 0,
 				USB_CTRL_SET_TIMEOUT);
 
+		/* Try to enable USB2 hardware LPM again */
+		if (udev->usb2_hw_lpm_capable == 1)
+			usb_set_usb2_hardware_lpm(udev, 1);
+
+		/* Try to enable USB3 LPM again */
+		usb_unlocked_enable_lpm(udev);
+
 		/* System sleep transitions should never fail */
 		if (!PMSG_IS_AUTO(msg))
 			status = 0;
@@ -2696,6 +2931,9 @@ int usb_port_resume(struct usb_device *udev, pm_message_t msg)
 		/* Try to enable USB2 hardware LPM */
 		if (udev->usb2_hw_lpm_capable == 1)
 			usb_set_usb2_hardware_lpm(udev, 1);
+
+		/* Try to enable USB3 LPM */
+		usb_unlocked_enable_lpm(udev);
 	}
 
 	return status;
@@ -2824,11 +3062,429 @@ void usb_root_hub_lost_power(struct usb_device *rhdev)
 }
 EXPORT_SYMBOL_GPL(usb_root_hub_lost_power);
 
+static const char * const usb3_lpm_names[]  = {
+	"U0",
+	"U1",
+	"U2",
+	"U3",
+};
+
+/*
+ * Send a Set SEL control transfer to the device, prior to enabling
+ * device-initiated U1 or U2.  This lets the device know the exit latencies from
+ * the time the device initiates a U1 or U2 exit, to the time it will receive a
+ * packet from the host.
+ *
+ * This function will fail if the SEL or PEL values for udev are greater than
+ * the maximum allowed values for the link state to be enabled.
+ */
+static int usb_req_set_sel(struct usb_device *udev, enum usb3_link_state state)
+{
+	struct usb_set_sel_req *sel_values;
+	unsigned long long u1_sel;
+	unsigned long long u1_pel;
+	unsigned long long u2_sel;
+	unsigned long long u2_pel;
+	int ret;
+
+	/* Convert SEL and PEL stored in ns to us */
+	u1_sel = DIV_ROUND_UP(udev->u1_params.sel, 1000);
+	u1_pel = DIV_ROUND_UP(udev->u1_params.pel, 1000);
+	u2_sel = DIV_ROUND_UP(udev->u2_params.sel, 1000);
+	u2_pel = DIV_ROUND_UP(udev->u2_params.pel, 1000);
+
+	/*
+	 * Make sure that the calculated SEL and PEL values for the link
+	 * state we're enabling aren't bigger than the max SEL/PEL
+	 * value that will fit in the SET SEL control transfer.
+	 * Otherwise the device would get an incorrect idea of the exit
+	 * latency for the link state, and could start a device-initiated
+	 * U1/U2 when the exit latencies are too high.
+	 */
+	if ((state == USB3_LPM_U1 &&
+				(u1_sel > USB3_LPM_MAX_U1_SEL_PEL ||
+				 u1_pel > USB3_LPM_MAX_U1_SEL_PEL)) ||
+			(state == USB3_LPM_U2 &&
+			 (u2_sel > USB3_LPM_MAX_U2_SEL_PEL ||
+			  u2_pel > USB3_LPM_MAX_U2_SEL_PEL))) {
+		dev_dbg(&udev->dev, "Device-initiated %s disabled due "
+				"to long SEL %llu ms or PEL %llu ms\n",
+				usb3_lpm_names[state], u1_sel, u1_pel);
+		return -EINVAL;
+	}
+
+	/*
+	 * If we're enabling device-initiated LPM for one link state,
+	 * but the other link state has a too high SEL or PEL value,
+	 * just set those values to the max in the Set SEL request.
+	 */
+	if (u1_sel > USB3_LPM_MAX_U1_SEL_PEL)
+		u1_sel = USB3_LPM_MAX_U1_SEL_PEL;
+
+	if (u1_pel > USB3_LPM_MAX_U1_SEL_PEL)
+		u1_pel = USB3_LPM_MAX_U1_SEL_PEL;
+
+	if (u2_sel > USB3_LPM_MAX_U2_SEL_PEL)
+		u2_sel = USB3_LPM_MAX_U2_SEL_PEL;
+
+	if (u2_pel > USB3_LPM_MAX_U2_SEL_PEL)
+		u2_pel = USB3_LPM_MAX_U2_SEL_PEL;
+
+	/*
+	 * usb_enable_lpm() can be called as part of a failed device reset,
+	 * which may be initiated by an error path of a mass storage driver.
+	 * Therefore, use GFP_NOIO.
+	 */
+	sel_values = kmalloc(sizeof *(sel_values), GFP_NOIO);
+	if (!sel_values)
+		return -ENOMEM;
+
+	sel_values->u1_sel = u1_sel;
+	sel_values->u1_pel = u1_pel;
+	sel_values->u2_sel = cpu_to_le16(u2_sel);
+	sel_values->u2_pel = cpu_to_le16(u2_pel);
+
+	ret = usb_control_msg(udev, usb_sndctrlpipe(udev, 0),
+			USB_REQ_SET_SEL,
+			USB_RECIP_DEVICE,
+			0, 0,
+			sel_values, sizeof *(sel_values),
+			USB_CTRL_SET_TIMEOUT);
+	kfree(sel_values);
+	return ret;
+}
+
+/*
+ * Enable or disable device-initiated U1 or U2 transitions.
+ */
+static int usb_set_device_initiated_lpm(struct usb_device *udev,
+		enum usb3_link_state state, bool enable)
+{
+	int ret;
+	int feature;
+
+	switch (state) {
+	case USB3_LPM_U1:
+		feature = USB_DEVICE_U1_ENABLE;
+		break;
+	case USB3_LPM_U2:
+		feature = USB_DEVICE_U2_ENABLE;
+		break;
+	default:
+		dev_warn(&udev->dev, "%s: Can't %s non-U1 or U2 state.\n",
+				__func__, enable ? "enable" : "disable");
+		return -EINVAL;
+	}
+
+	if (udev->state != USB_STATE_CONFIGURED) {
+		dev_dbg(&udev->dev, "%s: Can't %s %s state "
+				"for unconfigured device.\n",
+				__func__, enable ? "enable" : "disable",
+				usb3_lpm_names[state]);
+		return 0;
+	}
+
+	if (enable) {
+		/*
+		 * First, let the device know about the exit latencies
+		 * associated with the link state we're about to enable.
+		 */
+		ret = usb_req_set_sel(udev, state);
+		if (ret < 0) {
+			dev_warn(&udev->dev, "Set SEL for device-initiated "
+					"%s failed.\n", usb3_lpm_names[state]);
+			return -EBUSY;
+		}
+		/*
+		 * Now send the control transfer to enable device-initiated LPM
+		 * for either U1 or U2.
+		 */
+		ret = usb_control_msg(udev, usb_sndctrlpipe(udev, 0),
+				USB_REQ_SET_FEATURE,
+				USB_RECIP_DEVICE,
+				feature,
+				0, NULL, 0,
+				USB_CTRL_SET_TIMEOUT);
+	} else {
+		ret = usb_control_msg(udev, usb_sndctrlpipe(udev, 0),
+				USB_REQ_CLEAR_FEATURE,
+				USB_RECIP_DEVICE,
+				feature,
+				0, NULL, 0,
+				USB_CTRL_SET_TIMEOUT);
+	}
+	if (ret < 0) {
+		dev_warn(&udev->dev, "%s of device-initiated %s failed.\n",
+				enable ? "Enable" : "Disable",
+				usb3_lpm_names[state]);
+		return -EBUSY;
+	}
+	return 0;
+}
+
+static int usb_set_lpm_timeout(struct usb_device *udev,
+		enum usb3_link_state state, int timeout)
+{
+	int ret;
+	int feature;
+
+	switch (state) {
+	case USB3_LPM_U1:
+		feature = USB_PORT_FEAT_U1_TIMEOUT;
+		break;
+	case USB3_LPM_U2:
+		feature = USB_PORT_FEAT_U2_TIMEOUT;
+		break;
+	default:
+		dev_warn(&udev->dev, "%s: Can't set timeout for non-U1 or U2 state.\n",
+				__func__);
+		return -EINVAL;
+	}
+
+	if (state == USB3_LPM_U1 && timeout > USB3_LPM_U1_MAX_TIMEOUT &&
+			timeout != USB3_LPM_DEVICE_INITIATED) {
+		dev_warn(&udev->dev, "Failed to set %s timeout to 0x%x, "
+				"which is a reserved value.\n",
+				usb3_lpm_names[state], timeout);
+		return -EINVAL;
+	}
+
+	ret = set_port_feature(udev->parent,
+			USB_PORT_LPM_TIMEOUT(timeout) | udev->portnum,
+			feature);
+	if (ret < 0) {
+		dev_warn(&udev->dev, "Failed to set %s timeout to 0x%x,"
+				"error code %i\n", usb3_lpm_names[state],
+				timeout, ret);
+		return -EBUSY;
+	}
+	if (state == USB3_LPM_U1)
+		udev->u1_params.timeout = timeout;
+	else
+		udev->u2_params.timeout = timeout;
+	return 0;
+}
+
+/*
+ * Enable the hub-initiated U1/U2 idle timeouts, and enable device-initiated
+ * U1/U2 entry.
+ *
+ * We will attempt to enable U1 or U2, but there are no guarantees that the
+ * control transfers to set the hub timeout or enable device-initiated U1/U2
+ * will be successful.
+ *
+ * If we cannot set the parent hub U1/U2 timeout, we attempt to let the xHCI
+ * driver know about it.  If that call fails, it should be harmless, and just
+ * take up more slightly more bus bandwidth for unnecessary U1/U2 exit latency.
+ */
+static void usb_enable_link_state(struct usb_hcd *hcd, struct usb_device *udev,
+		enum usb3_link_state state)
+{
+	int timeout;
+
+	/* We allow the host controller to set the U1/U2 timeout internally
+	 * first, so that it can change its schedule to account for the
+	 * additional latency to send data to a device in a lower power
+	 * link state.
+	 */
+	timeout = hcd->driver->enable_usb3_lpm_timeout(hcd, udev, state);
+
+	/* xHCI host controller doesn't want to enable this LPM state. */
+	if (timeout == 0)
+		return;
+
+	if (timeout < 0) {
+		dev_warn(&udev->dev, "Could not enable %s link state, "
+				"xHCI error %i.\n", usb3_lpm_names[state],
+				timeout);
+		return;
+	}
+
+	if (usb_set_lpm_timeout(udev, state, timeout))
+		/* If we can't set the parent hub U1/U2 timeout,
+		 * device-initiated LPM won't be allowed either, so let the xHCI
+		 * host know that this link state won't be enabled.
+		 */
+		hcd->driver->disable_usb3_lpm_timeout(hcd, udev, state);
+
+	/* Only a configured device will accept the Set Feature U1/U2_ENABLE */
+	else if (udev->actconfig)
+		usb_set_device_initiated_lpm(udev, state, true);
+
+}
+
+/*
+ * Disable the hub-initiated U1/U2 idle timeouts, and disable device-initiated
+ * U1/U2 entry.
+ *
+ * If this function returns -EBUSY, the parent hub will still allow U1/U2 entry.
+ * If zero is returned, the parent will not allow the link to go into U1/U2.
+ *
+ * If zero is returned, device-initiated U1/U2 entry may still be enabled, but
+ * it won't have an effect on the bus link state because the parent hub will
+ * still disallow device-initiated U1/U2 entry.
+ *
+ * If zero is returned, the xHCI host controller may still think U1/U2 entry is
+ * possible.  The result will be slightly more bus bandwidth will be taken up
+ * (to account for U1/U2 exit latency), but it should be harmless.
+ */
+static int usb_disable_link_state(struct usb_hcd *hcd, struct usb_device *udev,
+		enum usb3_link_state state)
+{
+	int feature;
+
+	switch (state) {
+	case USB3_LPM_U1:
+		feature = USB_PORT_FEAT_U1_TIMEOUT;
+		break;
+	case USB3_LPM_U2:
+		feature = USB_PORT_FEAT_U2_TIMEOUT;
+		break;
+	default:
+		dev_warn(&udev->dev, "%s: Can't disable non-U1 or U2 state.\n",
+				__func__);
+		return -EINVAL;
+	}
+
+	if (usb_set_lpm_timeout(udev, state, 0))
+		return -EBUSY;
+
+	usb_set_device_initiated_lpm(udev, state, false);
+
+	if (hcd->driver->disable_usb3_lpm_timeout(hcd, udev, state))
+		dev_warn(&udev->dev, "Could not disable xHCI %s timeout, "
+				"bus schedule bandwidth may be impacted.\n",
+				usb3_lpm_names[state]);
+	return 0;
+}
+
+/*
+ * Disable hub-initiated and device-initiated U1 and U2 entry.
+ * Caller must own the bandwidth_mutex.
+ *
+ * This will call usb_enable_lpm() on failure, which will decrement
+ * lpm_disable_count, and will re-enable LPM if lpm_disable_count reaches zero.
+ */
+int usb_disable_lpm(struct usb_device *udev)
+{
+	struct usb_hcd *hcd;
+
+	if (!udev || !udev->parent ||
+			udev->speed != USB_SPEED_SUPER ||
+			!udev->lpm_capable)
+		return 0;
+
+	hcd = bus_to_hcd(udev->bus);
+	if (!hcd || !hcd->driver->disable_usb3_lpm_timeout)
+		return 0;
+
+	udev->lpm_disable_count++;
+	if ((udev->u1_params.timeout == 0 && udev->u1_params.timeout == 0))
+		return 0;
+
+	/* If LPM is enabled, attempt to disable it. */
+	if (usb_disable_link_state(hcd, udev, USB3_LPM_U1))
+		goto enable_lpm;
+	if (usb_disable_link_state(hcd, udev, USB3_LPM_U2))
+		goto enable_lpm;
+
+	return 0;
+
+enable_lpm:
+	usb_enable_lpm(udev);
+	return -EBUSY;
+}
+EXPORT_SYMBOL_GPL(usb_disable_lpm);
+
+/* Grab the bandwidth_mutex before calling usb_disable_lpm() */
+int usb_unlocked_disable_lpm(struct usb_device *udev)
+{
+	struct usb_hcd *hcd = bus_to_hcd(udev->bus);
+	int ret;
+
+	if (!hcd)
+		return -EINVAL;
+
+	mutex_lock(hcd->bandwidth_mutex);
+	ret = usb_disable_lpm(udev);
+	mutex_unlock(hcd->bandwidth_mutex);
+
+	return ret;
+}
+EXPORT_SYMBOL_GPL(usb_unlocked_disable_lpm);
+
+/*
+ * Attempt to enable device-initiated and hub-initiated U1 and U2 entry.  The
+ * xHCI host policy may prevent U1 or U2 from being enabled.
+ *
+ * Other callers may have disabled link PM, so U1 and U2 entry will be disabled
+ * until the lpm_disable_count drops to zero.  Caller must own the
+ * bandwidth_mutex.
+ */
+void usb_enable_lpm(struct usb_device *udev)
+{
+	struct usb_hcd *hcd;
+
+	if (!udev || !udev->parent ||
+			udev->speed != USB_SPEED_SUPER ||
+			!udev->lpm_capable)
+		return;
+
+	udev->lpm_disable_count--;
+	hcd = bus_to_hcd(udev->bus);
+	/* Double check that we can both enable and disable LPM.
+	 * Device must be configured to accept set feature U1/U2 timeout.
+	 */
+	if (!hcd || !hcd->driver->enable_usb3_lpm_timeout ||
+			!hcd->driver->disable_usb3_lpm_timeout)
+		return;
+
+	if (udev->lpm_disable_count > 0)
+		return;
+
+	usb_enable_link_state(hcd, udev, USB3_LPM_U1);
+	usb_enable_link_state(hcd, udev, USB3_LPM_U2);
+}
+EXPORT_SYMBOL_GPL(usb_enable_lpm);
+
+/* Grab the bandwidth_mutex before calling usb_enable_lpm() */
+void usb_unlocked_enable_lpm(struct usb_device *udev)
+{
+	struct usb_hcd *hcd = bus_to_hcd(udev->bus);
+
+	if (!hcd)
+		return;
+
+	mutex_lock(hcd->bandwidth_mutex);
+	usb_enable_lpm(udev);
+	mutex_unlock(hcd->bandwidth_mutex);
+}
+EXPORT_SYMBOL_GPL(usb_unlocked_enable_lpm);
+
+
 #else	/* CONFIG_PM */
 
 #define hub_suspend		NULL
 #define hub_resume		NULL
 #define hub_reset_resume	NULL
+
+int usb_disable_lpm(struct usb_device *udev)
+{
+	return 0;
+}
+EXPORT_SYMBOL_GPL(usb_disable_lpm);
+
+void usb_enable_lpm(struct usb_device *udev) { }
+EXPORT_SYMBOL_GPL(usb_enable_lpm);
+
+int usb_unlocked_disable_lpm(struct usb_device *udev)
+{
+	return 0;
+}
+EXPORT_SYMBOL_GPL(usb_unlocked_disable_lpm);
+
+void usb_unlocked_enable_lpm(struct usb_device *udev) { }
+EXPORT_SYMBOL_GPL(usb_unlocked_enable_lpm);
 #endif
 
 
@@ -3208,9 +3864,8 @@ hub_port_init (struct usb_hub *hub, struct usb_device *udev, int port1,
 	if (udev->wusb == 0 && le16_to_cpu(udev->descriptor.bcdUSB) >= 0x0201) {
 		retval = usb_get_bos_descriptor(udev);
 		if (!retval) {
-			if (udev->bos->ext_cap && (USB_LPM_SUPPORT &
-				le32_to_cpu(udev->bos->ext_cap->bmAttributes)))
-					udev->lpm_capable = 1;
+			udev->lpm_capable = usb_device_supports_lpm(udev);
+			usb_set_lpm_parameters(udev);
 		}
 	}
 
@@ -4042,11 +4697,22 @@ static int usb_reset_and_verify_device(struct usb_device *udev)
 		goto done;
 
 	mutex_lock(hcd->bandwidth_mutex);
+	/* Disable LPM while we reset the device and reinstall the alt settings.
+	 * Device-initiated LPM settings, and system exit latency settings are
+	 * cleared when the device is reset, so we have to set them up again.
+	 */
+	ret = usb_disable_lpm(udev);
+	if (ret) {
+		dev_err(&udev->dev, "%s Failed to disable LPM\n.", __func__);
+		mutex_unlock(hcd->bandwidth_mutex);
+		goto done;
+	}
 	ret = usb_hcd_alloc_bandwidth(udev, udev->actconfig, NULL, NULL);
 	if (ret < 0) {
 		dev_warn(&udev->dev,
 				"Busted HC?  Not enough HCD resources for "
 				"old configuration.\n");
+		usb_enable_lpm(udev);
 		mutex_unlock(hcd->bandwidth_mutex);
 		goto re_enumerate;
 	}
@@ -4058,6 +4724,7 @@ static int usb_reset_and_verify_device(struct usb_device *udev)
 		dev_err(&udev->dev,
 			"can't restore configuration #%d (error=%d)\n",
 			udev->actconfig->desc.bConfigurationValue, ret);
+		usb_enable_lpm(udev);
 		mutex_unlock(hcd->bandwidth_mutex);
 		goto re_enumerate;
   	}
@@ -4096,10 +4763,13 @@ static int usb_reset_and_verify_device(struct usb_device *udev)
 				desc->bInterfaceNumber,
 				desc->bAlternateSetting,
 				ret);
+			usb_unlocked_enable_lpm(udev);
 			goto re_enumerate;
 		}
 	}
 
+	/* Now that the alt settings are re-installed, enable LPM. */
+	usb_unlocked_enable_lpm(udev);
 done:
 	return 0;
  
diff --git a/drivers/usb/core/inode.c b/drivers/usb/core/inode.c
deleted file mode 100644
index d2b9af59cba..00000000000
--- a/drivers/usb/core/inode.c
+++ /dev/null
@@ -1,748 +0,0 @@
-/*****************************************************************************/
-
-/*
- *	inode.c  --  Inode/Dentry functions for the USB device file system.
- *
- *	Copyright (C) 2000 Thomas Sailer (sailer@ife.ee.ethz.ch)
- *	Copyright (C) 2001,2002,2004 Greg Kroah-Hartman (greg@kroah.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.
- *
- *  History:
- *   0.1  04.01.2000  Created
- *   0.2  10.12.2001  converted to use the vfs layer better
- */
-
-/*****************************************************************************/
-
-#include <linux/module.h>
-#include <linux/fs.h>
-#include <linux/mount.h>
-#include <linux/pagemap.h>
-#include <linux/init.h>
-#include <linux/proc_fs.h>
-#include <linux/usb.h>
-#include <linux/namei.h>
-#include <linux/usbdevice_fs.h>
-#include <linux/parser.h>
-#include <linux/notifier.h>
-#include <linux/seq_file.h>
-#include <linux/usb/hcd.h>
-#include <asm/byteorder.h>
-#include "usb.h"
-
-#define USBFS_DEFAULT_DEVMODE (S_IWUSR | S_IRUGO)
-#define USBFS_DEFAULT_BUSMODE (S_IXUGO | S_IRUGO)
-#define USBFS_DEFAULT_LISTMODE S_IRUGO
-
-static const struct file_operations default_file_operations;
-static struct vfsmount *usbfs_mount;
-static int usbfs_mount_count;	/* = 0 */
-
-static struct dentry *devices_usbfs_dentry;
-static int num_buses;	/* = 0 */
-
-static uid_t devuid;	/* = 0 */
-static uid_t busuid;	/* = 0 */
-static uid_t listuid;	/* = 0 */
-static gid_t devgid;	/* = 0 */
-static gid_t busgid;	/* = 0 */
-static gid_t listgid;	/* = 0 */
-static umode_t devmode = USBFS_DEFAULT_DEVMODE;
-static umode_t busmode = USBFS_DEFAULT_BUSMODE;
-static umode_t listmode = USBFS_DEFAULT_LISTMODE;
-
-static int usbfs_show_options(struct seq_file *seq, struct dentry *root)
-{
-	if (devuid != 0)
-		seq_printf(seq, ",devuid=%u", devuid);
-	if (devgid != 0)
-		seq_printf(seq, ",devgid=%u", devgid);
-	if (devmode != USBFS_DEFAULT_DEVMODE)
-		seq_printf(seq, ",devmode=%o", devmode);
-	if (busuid != 0)
-		seq_printf(seq, ",busuid=%u", busuid);
-	if (busgid != 0)
-		seq_printf(seq, ",busgid=%u", busgid);
-	if (busmode != USBFS_DEFAULT_BUSMODE)
-		seq_printf(seq, ",busmode=%o", busmode);
-	if (listuid != 0)
-		seq_printf(seq, ",listuid=%u", listuid);
-	if (listgid != 0)
-		seq_printf(seq, ",listgid=%u", listgid);
-	if (listmode != USBFS_DEFAULT_LISTMODE)
-		seq_printf(seq, ",listmode=%o", listmode);
-
-	return 0;
-}
-
-enum {
-	Opt_devuid, Opt_devgid, Opt_devmode,
-	Opt_busuid, Opt_busgid, Opt_busmode,
-	Opt_listuid, Opt_listgid, Opt_listmode,
-	Opt_err,
-};
-
-static const match_table_t tokens = {
-	{Opt_devuid, "devuid=%u"},
-	{Opt_devgid, "devgid=%u"},
-	{Opt_devmode, "devmode=%o"},
-	{Opt_busuid, "busuid=%u"},
-	{Opt_busgid, "busgid=%u"},
-	{Opt_busmode, "busmode=%o"},
-	{Opt_listuid, "listuid=%u"},
-	{Opt_listgid, "listgid=%u"},
-	{Opt_listmode, "listmode=%o"},
-	{Opt_err, NULL}
-};
-
-static int parse_options(struct super_block *s, char *data)
-{
-	char *p;
-	int option;
-
-	/* (re)set to defaults. */
-	devuid = 0;
-	busuid = 0;
-	listuid = 0;
-	devgid = 0;
-	busgid = 0;
-	listgid = 0;
-	devmode = USBFS_DEFAULT_DEVMODE;
-	busmode = USBFS_DEFAULT_BUSMODE;
-	listmode = USBFS_DEFAULT_LISTMODE;
-
-	while ((p = strsep(&data, ",")) != NULL) {
-		substring_t args[MAX_OPT_ARGS];
-		int token;
-		if (!*p)
-			continue;
-
-		token = match_token(p, tokens, args);
-		switch (token) {
-		case Opt_devuid:
-			if (match_int(&args[0], &option))
-			       return -EINVAL;
-			devuid = option;
-			break;
-		case Opt_devgid:
-			if (match_int(&args[0], &option))
-			       return -EINVAL;
-			devgid = option;
-			break;
-		case Opt_devmode:
-			if (match_octal(&args[0], &option))
-				return -EINVAL;
-			devmode = option & S_IRWXUGO;
-			break;
-		case Opt_busuid:
-			if (match_int(&args[0], &option))
-			       return -EINVAL;
-			busuid = option;
-			break;
-		case Opt_busgid:
-			if (match_int(&args[0], &option))
-			       return -EINVAL;
-			busgid = option;
-			break;
-		case Opt_busmode:
-			if (match_octal(&args[0], &option))
-				return -EINVAL;
-			busmode = option & S_IRWXUGO;
-			break;
-		case Opt_listuid:
-			if (match_int(&args[0], &option))
-			       return -EINVAL;
-			listuid = option;
-			break;
-		case Opt_listgid:
-			if (match_int(&args[0], &option))
-			       return -EINVAL;
-			listgid = option;
-			break;
-		case Opt_listmode:
-			if (match_octal(&args[0], &option))
-				return -EINVAL;
-			listmode = option & S_IRWXUGO;
-			break;
-		default:
-			printk(KERN_ERR "usbfs: unrecognised mount option "
-			       "\"%s\" or missing value\n", p);
-			return -EINVAL;
-		}
-	}
-
-	return 0;
-}
-
-static void update_special(struct dentry *special)
-{
-	special->d_inode->i_uid = listuid;
-	special->d_inode->i_gid = listgid;
-	special->d_inode->i_mode = S_IFREG | listmode;
-}
-
-static void update_dev(struct dentry *dev)
-{
-	dev->d_inode->i_uid = devuid;
-	dev->d_inode->i_gid = devgid;
-	dev->d_inode->i_mode = S_IFREG | devmode;
-}
-
-static void update_bus(struct dentry *bus)
-{
-	struct dentry *dev = NULL;
-
-	bus->d_inode->i_uid = busuid;
-	bus->d_inode->i_gid = busgid;
-	bus->d_inode->i_mode = S_IFDIR | busmode;
-
-	mutex_lock(&bus->d_inode->i_mutex);
-
-	list_for_each_entry(dev, &bus->d_subdirs, d_u.d_child)
-		if (dev->d_inode)
-			update_dev(dev);
-
-	mutex_unlock(&bus->d_inode->i_mutex);
-}
-
-static void update_sb(struct super_block *sb)
-{
-	struct dentry *root = sb->s_root;
-	struct dentry *bus = NULL;
-
-	if (!root)
-		return;
-
-	mutex_lock_nested(&root->d_inode->i_mutex, I_MUTEX_PARENT);
-
-	list_for_each_entry(bus, &root->d_subdirs, d_u.d_child) {
-		if (bus->d_inode) {
-			switch (S_IFMT & bus->d_inode->i_mode) {
-			case S_IFDIR:
-				update_bus(bus);
-				break;
-			case S_IFREG:
-				update_special(bus);
-				break;
-			default:
-				printk(KERN_WARNING "usbfs: Unknown node %s "
-				       "mode %x found on remount!\n",
-				       bus->d_name.name, bus->d_inode->i_mode);
-				break;
-			}
-		}
-	}
-
-	mutex_unlock(&root->d_inode->i_mutex);
-}
-
-static int remount(struct super_block *sb, int *flags, char *data)
-{
-	/* If this is not a real mount,
-	 * i.e. it's a simple_pin_fs from create_special_files,
-	 * then ignore it.
-	 */
-	if (*flags & MS_KERNMOUNT)
-		return 0;
-
-	if (parse_options(sb, data)) {
-		printk(KERN_WARNING "usbfs: mount parameter error.\n");
-		return -EINVAL;
-	}
-
-	if (usbfs_mount)
-		update_sb(usbfs_mount->mnt_sb);
-
-	return 0;
-}
-
-static struct inode *usbfs_get_inode (struct super_block *sb, umode_t mode, dev_t dev)
-{
-	struct inode *inode = new_inode(sb);
-
-	if (inode) {
-		inode->i_ino = get_next_ino();
-		inode_init_owner(inode, NULL, mode);
-		inode->i_atime = inode->i_mtime = inode->i_ctime = CURRENT_TIME;
-		switch (mode & S_IFMT) {
-		default:
-			init_special_inode(inode, mode, dev);
-			break;
-		case S_IFREG:
-			inode->i_fop = &default_file_operations;
-			break;
-		case S_IFDIR:
-			inode->i_op = &simple_dir_inode_operations;
-			inode->i_fop = &simple_dir_operations;
-
-			/* directory inodes start off with i_nlink == 2 (for "." entry) */
-			inc_nlink(inode);
-			break;
-		}
-	}
-	return inode; 
-}
-
-/* SMP-safe */
-static int usbfs_mknod (struct inode *dir, struct dentry *dentry, umode_t mode,
-			dev_t dev)
-{
-	struct inode *inode = usbfs_get_inode(dir->i_sb, mode, dev);
-	int error = -EPERM;
-
-	if (dentry->d_inode)
-		return -EEXIST;
-
-	if (inode) {
-		d_instantiate(dentry, inode);
-		dget(dentry);
-		error = 0;
-	}
-	return error;
-}
-
-static int usbfs_mkdir (struct inode *dir, struct dentry *dentry, umode_t mode)
-{
-	int res;
-
-	mode = (mode & (S_IRWXUGO | S_ISVTX)) | S_IFDIR;
-	res = usbfs_mknod (dir, dentry, mode, 0);
-	if (!res)
-		inc_nlink(dir);
-	return res;
-}
-
-static int usbfs_create (struct inode *dir, struct dentry *dentry, umode_t mode)
-{
-	mode = (mode & S_IALLUGO) | S_IFREG;
-	return usbfs_mknod (dir, dentry, mode, 0);
-}
-
-static inline int usbfs_positive (struct dentry *dentry)
-{
-	return dentry->d_inode && !d_unhashed(dentry);
-}
-
-static int usbfs_empty (struct dentry *dentry)
-{
-	struct list_head *list;
-
-	spin_lock(&dentry->d_lock);
-	list_for_each(list, &dentry->d_subdirs) {
-		struct dentry *de = list_entry(list, struct dentry, d_u.d_child);
-
-		spin_lock_nested(&de->d_lock, DENTRY_D_LOCK_NESTED);
-		if (usbfs_positive(de)) {
-			spin_unlock(&de->d_lock);
-			spin_unlock(&dentry->d_lock);
-			return 0;
-		}
-		spin_unlock(&de->d_lock);
-	}
-	spin_unlock(&dentry->d_lock);
-	return 1;
-}
-
-static int usbfs_unlink (struct inode *dir, struct dentry *dentry)
-{
-	struct inode *inode = dentry->d_inode;
-	mutex_lock(&inode->i_mutex);
-	drop_nlink(dentry->d_inode);
-	dput(dentry);
-	mutex_unlock(&inode->i_mutex);
-	d_delete(dentry);
-	return 0;
-}
-
-static int usbfs_rmdir(struct inode *dir, struct dentry *dentry)
-{
-	int error = -ENOTEMPTY;
-	struct inode * inode = dentry->d_inode;
-
-	mutex_lock(&inode->i_mutex);
-	dentry_unhash(dentry);
-	if (usbfs_empty(dentry)) {
-		dont_mount(dentry);
-		drop_nlink(dentry->d_inode);
-		drop_nlink(dentry->d_inode);
-		dput(dentry);
-		inode->i_flags |= S_DEAD;
-		drop_nlink(dir);
-		error = 0;
-	}
-	mutex_unlock(&inode->i_mutex);
-	if (!error)
-		d_delete(dentry);
-	return error;
-}
-
-
-/* default file operations */
-static ssize_t default_read_file (struct file *file, char __user *buf,
-				  size_t count, loff_t *ppos)
-{
-	return 0;
-}
-
-static ssize_t default_write_file (struct file *file, const char __user *buf,
-				   size_t count, loff_t *ppos)
-{
-	return count;
-}
-
-static loff_t default_file_lseek (struct file *file, loff_t offset, int orig)
-{
-	loff_t retval = -EINVAL;
-
-	mutex_lock(&file->f_path.dentry->d_inode->i_mutex);
-	switch(orig) {
-	case 0:
-		if (offset > 0) {
-			file->f_pos = offset;
-			retval = file->f_pos;
-		} 
-		break;
-	case 1:
-		if ((offset + file->f_pos) > 0) {
-			file->f_pos += offset;
-			retval = file->f_pos;
-		} 
-		break;
-	default:
-		break;
-	}
-	mutex_unlock(&file->f_path.dentry->d_inode->i_mutex);
-	return retval;
-}
-
-static const struct file_operations default_file_operations = {
-	.read =		default_read_file,
-	.write =	default_write_file,
-	.open =		simple_open,
-	.llseek =	default_file_lseek,
-};
-
-static const struct super_operations usbfs_ops = {
-	.statfs =	simple_statfs,
-	.drop_inode =	generic_delete_inode,
-	.remount_fs =	remount,
-	.show_options = usbfs_show_options,
-};
-
-static int usbfs_fill_super(struct super_block *sb, void *data, int silent)
-{
-	struct inode *inode;
-
-	sb->s_blocksize = PAGE_CACHE_SIZE;
-	sb->s_blocksize_bits = PAGE_CACHE_SHIFT;
-	sb->s_magic = USBDEVICE_SUPER_MAGIC;
-	sb->s_op = &usbfs_ops;
-	sb->s_time_gran = 1;
-	inode = usbfs_get_inode(sb, S_IFDIR | 0755, 0);
-	sb->s_root = d_make_root(inode);
-	if (!sb->s_root) {
-		dbg("%s: could not get root dentry!",__func__);
-		return -ENOMEM;
-	}
-	return 0;
-}
-
-/*
- * fs_create_by_name - create a file, given a name
- * @name:	name of file
- * @mode:	type of file
- * @parent:	dentry of directory to create it in
- * @dentry:	resulting dentry of file
- *
- * This function handles both regular files and directories.
- */
-static int fs_create_by_name (const char *name, umode_t mode,
-			      struct dentry *parent, struct dentry **dentry)
-{
-	int error = 0;
-
-	/* If the parent is not specified, we create it in the root.
-	 * We need the root dentry to do this, which is in the super 
-	 * block. A pointer to that is in the struct vfsmount that we
-	 * have around.
-	 */
-	if (!parent ) {
-		if (usbfs_mount)
-			parent = usbfs_mount->mnt_root;
-	}
-
-	if (!parent) {
-		dbg("Ah! can not find a parent!");
-		return -EFAULT;
-	}
-
-	*dentry = NULL;
-	mutex_lock(&parent->d_inode->i_mutex);
-	*dentry = lookup_one_len(name, parent, strlen(name));
-	if (!IS_ERR(*dentry)) {
-		if (S_ISDIR(mode))
-			error = usbfs_mkdir (parent->d_inode, *dentry, mode);
-		else 
-			error = usbfs_create (parent->d_inode, *dentry, mode);
-	} else
-		error = PTR_ERR(*dentry);
-	mutex_unlock(&parent->d_inode->i_mutex);
-
-	return error;
-}
-
-static struct dentry *fs_create_file (const char *name, umode_t mode,
-				      struct dentry *parent, void *data,
-				      const struct file_operations *fops,
-				      uid_t uid, gid_t gid)
-{
-	struct dentry *dentry;
-	int error;
-
-	dbg("creating file '%s'",name);
-
-	error = fs_create_by_name (name, mode, parent, &dentry);
-	if (error) {
-		dentry = NULL;
-	} else {
-		if (dentry->d_inode) {
-			if (data)
-				dentry->d_inode->i_private = data;
-			if (fops)
-				dentry->d_inode->i_fop = fops;
-			dentry->d_inode->i_uid = uid;
-			dentry->d_inode->i_gid = gid;
-		}
-	}
-
-	return dentry;
-}
-
-static void fs_remove_file (struct dentry *dentry)
-{
-	struct dentry *parent = dentry->d_parent;
-	
-	if (!parent || !parent->d_inode)
-		return;
-
-	mutex_lock_nested(&parent->d_inode->i_mutex, I_MUTEX_PARENT);
-	if (usbfs_positive(dentry)) {
-		if (dentry->d_inode) {
-			if (S_ISDIR(dentry->d_inode->i_mode))
-				usbfs_rmdir(parent->d_inode, dentry);
-			else
-				usbfs_unlink(parent->d_inode, dentry);
-		dput(dentry);
-		}
-	}
-	mutex_unlock(&parent->d_inode->i_mutex);
-}
-
-/* --------------------------------------------------------------------- */
-
-static struct dentry *usb_mount(struct file_system_type *fs_type,
-	int flags, const char *dev_name, void *data)
-{
-	return mount_single(fs_type, flags, data, usbfs_fill_super);
-}
-
-static struct file_system_type usb_fs_type = {
-	.owner =	THIS_MODULE,
-	.name =		"usbfs",
-	.mount =	usb_mount,
-	.kill_sb =	kill_litter_super,
-};
-
-/* --------------------------------------------------------------------- */
-
-static int create_special_files (void)
-{
-	struct dentry *parent;
-	int retval;
-
-	/* create the devices special file */
-	retval = simple_pin_fs(&usb_fs_type, &usbfs_mount, &usbfs_mount_count);
-	if (retval) {
-		printk(KERN_ERR "Unable to get usbfs mount\n");
-		goto exit;
-	}
-
-	parent = usbfs_mount->mnt_root;
-	devices_usbfs_dentry = fs_create_file ("devices",
-					       listmode | S_IFREG, parent,
-					       NULL, &usbfs_devices_fops,
-					       listuid, listgid);
-	if (devices_usbfs_dentry == NULL) {
-		printk(KERN_ERR "Unable to create devices usbfs file\n");
-		retval = -ENODEV;
-		goto error_clean_mounts;
-	}
-
-	goto exit;
-	
-error_clean_mounts:
-	simple_release_fs(&usbfs_mount, &usbfs_mount_count);
-exit:
-	return retval;
-}
-
-static void remove_special_files (void)
-{
-	if (devices_usbfs_dentry)
-		fs_remove_file (devices_usbfs_dentry);
-	devices_usbfs_dentry = NULL;
-	simple_release_fs(&usbfs_mount, &usbfs_mount_count);
-}
-
-void usbfs_update_special (void)
-{
-	struct inode *inode;
-
-	if (devices_usbfs_dentry) {
-		inode = devices_usbfs_dentry->d_inode;
-		if (inode)
-			inode->i_atime = inode->i_mtime = inode->i_ctime = CURRENT_TIME;
-	}
-}
-
-static void usbfs_add_bus(struct usb_bus *bus)
-{
-	struct dentry *parent;
-	char name[8];
-	int retval;
-
-	/* create the special files if this is the first bus added */
-	if (num_buses == 0) {
-		retval = create_special_files();
-		if (retval)
-			return;
-	}
-	++num_buses;
-
-	sprintf (name, "%03d", bus->busnum);
-
-	parent = usbfs_mount->mnt_root;
-	bus->usbfs_dentry = fs_create_file (name, busmode | S_IFDIR, parent,
-					    bus, NULL, busuid, busgid);
-	if (bus->usbfs_dentry == NULL) {
-		printk(KERN_ERR "Error creating usbfs bus entry\n");
-		return;
-	}
-}
-
-static void usbfs_remove_bus(struct usb_bus *bus)
-{
-	if (bus->usbfs_dentry) {
-		fs_remove_file (bus->usbfs_dentry);
-		bus->usbfs_dentry = NULL;
-	}
-
-	--num_buses;
-	if (num_buses <= 0) {
-		remove_special_files();
-		num_buses = 0;
-	}
-}
-
-static void usbfs_add_device(struct usb_device *dev)
-{
-	char name[8];
-	int i;
-	int i_size;
-
-	sprintf (name, "%03d", dev->devnum);
-	dev->usbfs_dentry = fs_create_file (name, devmode | S_IFREG,
-					    dev->bus->usbfs_dentry, dev,
-					    &usbdev_file_operations,
-					    devuid, devgid);
-	if (dev->usbfs_dentry == NULL) {
-		printk(KERN_ERR "Error creating usbfs device entry\n");
-		return;
-	}
-
-	/* Set the size of the device's file to be
-	 * equal to the size of the device descriptors. */
-	i_size = sizeof (struct usb_device_descriptor);
-	for (i = 0; i < dev->descriptor.bNumConfigurations; ++i) {
-		struct usb_config_descriptor *config =
-			(struct usb_config_descriptor *)dev->rawdescriptors[i];
-		i_size += le16_to_cpu(config->wTotalLength);
-	}
-	if (dev->usbfs_dentry->d_inode)
-		dev->usbfs_dentry->d_inode->i_size = i_size;
-}
-
-static void usbfs_remove_device(struct usb_device *dev)
-{
-	if (dev->usbfs_dentry) {
-		fs_remove_file (dev->usbfs_dentry);
-		dev->usbfs_dentry = NULL;
-	}
-}
-
-static int usbfs_notify(struct notifier_block *self, unsigned long action, void *dev)
-{
-	switch (action) {
-	case USB_DEVICE_ADD:
-		usbfs_add_device(dev);
-		break;
-	case USB_DEVICE_REMOVE:
-		usbfs_remove_device(dev);
-		break;
-	case USB_BUS_ADD:
-		usbfs_add_bus(dev);
-		break;
-	case USB_BUS_REMOVE:
-		usbfs_remove_bus(dev);
-	}
-
-	usbfs_update_special();
-	usbfs_conn_disc_event();
-	return NOTIFY_OK;
-}
-
-static struct notifier_block usbfs_nb = {
-	.notifier_call = 	usbfs_notify,
-};
-
-/* --------------------------------------------------------------------- */
-
-static struct proc_dir_entry *usbdir = NULL;
-
-int __init usbfs_init(void)
-{
-	int retval;
-
-	retval = register_filesystem(&usb_fs_type);
-	if (retval)
-		return retval;
-
-	usb_register_notify(&usbfs_nb);
-
-	/* create mount point for usbfs */
-	usbdir = proc_mkdir("bus/usb", NULL);
-
-	return 0;
-}
-
-void usbfs_cleanup(void)
-{
-	usb_unregister_notify(&usbfs_nb);
-	unregister_filesystem(&usb_fs_type);
-	if (usbdir)
-		remove_proc_entry("bus/usb", NULL);
-}
-
diff --git a/drivers/usb/core/message.c b/drivers/usb/core/message.c
index ca717da3be9..b548cf1dbc6 100644
--- a/drivers/usb/core/message.c
+++ b/drivers/usb/core/message.c
@@ -1308,10 +1308,19 @@ int usb_set_interface(struct usb_device *dev, int interface, int alternate)
 	 * Remove the current alt setting and add the new alt setting.
 	 */
 	mutex_lock(hcd->bandwidth_mutex);
+	/* Disable LPM, and re-enable it once the new alt setting is installed,
+	 * so that the xHCI driver can recalculate the U1/U2 timeouts.
+	 */
+	if (usb_disable_lpm(dev)) {
+		dev_err(&iface->dev, "%s Failed to disable LPM\n.", __func__);
+		mutex_unlock(hcd->bandwidth_mutex);
+		return -ENOMEM;
+	}
 	ret = usb_hcd_alloc_bandwidth(dev, NULL, iface->cur_altsetting, alt);
 	if (ret < 0) {
 		dev_info(&dev->dev, "Not enough bandwidth for altsetting %d\n",
 				alternate);
+		usb_enable_lpm(dev);
 		mutex_unlock(hcd->bandwidth_mutex);
 		return ret;
 	}
@@ -1334,6 +1343,7 @@ int usb_set_interface(struct usb_device *dev, int interface, int alternate)
 	} else if (ret < 0) {
 		/* Re-instate the old alt setting */
 		usb_hcd_alloc_bandwidth(dev, NULL, alt, iface->cur_altsetting);
+		usb_enable_lpm(dev);
 		mutex_unlock(hcd->bandwidth_mutex);
 		return ret;
 	}
@@ -1354,6 +1364,9 @@ int usb_set_interface(struct usb_device *dev, int interface, int alternate)
 
 	iface->cur_altsetting = alt;
 
+	/* Now that the interface is installed, re-enable LPM. */
+	usb_unlocked_enable_lpm(dev);
+
 	/* If the interface only has one altsetting and the device didn't
 	 * accept the request, we attempt to carry out the equivalent action
 	 * by manually clearing the HALT feature for each endpoint in the
@@ -1437,6 +1450,14 @@ int usb_reset_configuration(struct usb_device *dev)
 	config = dev->actconfig;
 	retval = 0;
 	mutex_lock(hcd->bandwidth_mutex);
+	/* Disable LPM, and re-enable it once the configuration is reset, so
+	 * that the xHCI driver can recalculate the U1/U2 timeouts.
+	 */
+	if (usb_disable_lpm(dev)) {
+		dev_err(&dev->dev, "%s Failed to disable LPM\n.", __func__);
+		mutex_unlock(hcd->bandwidth_mutex);
+		return -ENOMEM;
+	}
 	/* Make sure we have enough bandwidth for each alternate setting 0 */
 	for (i = 0; i < config->desc.bNumInterfaces; i++) {
 		struct usb_interface *intf = config->interface[i];
@@ -1465,6 +1486,7 @@ reset_old_alts:
 				usb_hcd_alloc_bandwidth(dev, NULL,
 						alt, intf->cur_altsetting);
 		}
+		usb_enable_lpm(dev);
 		mutex_unlock(hcd->bandwidth_mutex);
 		return retval;
 	}
@@ -1502,6 +1524,8 @@ reset_old_alts:
 			create_intf_ep_devs(intf);
 		}
 	}
+	/* Now that the interfaces are installed, re-enable LPM. */
+	usb_unlocked_enable_lpm(dev);
 	return 0;
 }
 EXPORT_SYMBOL_GPL(usb_reset_configuration);
@@ -1763,8 +1787,18 @@ free_interfaces:
 	 * this call fails, the device state is unchanged.
 	 */
 	mutex_lock(hcd->bandwidth_mutex);
+	/* Disable LPM, and re-enable it once the new configuration is
+	 * installed, so that the xHCI driver can recalculate the U1/U2
+	 * timeouts.
+	 */
+	if (usb_disable_lpm(dev)) {
+		dev_err(&dev->dev, "%s Failed to disable LPM\n.", __func__);
+		mutex_unlock(hcd->bandwidth_mutex);
+		return -ENOMEM;
+	}
 	ret = usb_hcd_alloc_bandwidth(dev, cp, NULL, NULL);
 	if (ret < 0) {
+		usb_enable_lpm(dev);
 		mutex_unlock(hcd->bandwidth_mutex);
 		usb_autosuspend_device(dev);
 		goto free_interfaces;
@@ -1784,6 +1818,7 @@ free_interfaces:
 	if (!cp) {
 		usb_set_device_state(dev, USB_STATE_ADDRESS);
 		usb_hcd_alloc_bandwidth(dev, NULL, NULL, NULL);
+		usb_enable_lpm(dev);
 		mutex_unlock(hcd->bandwidth_mutex);
 		usb_autosuspend_device(dev);
 		goto free_interfaces;
@@ -1838,6 +1873,9 @@ free_interfaces:
 			!(dev->quirks & USB_QUIRK_CONFIG_INTF_STRINGS))
 		cp->string = usb_cache_string(dev, cp->desc.iConfiguration);
 
+	/* Now that the interfaces are installed, re-enable LPM. */
+	usb_unlocked_enable_lpm(dev);
+
 	/* Now that all the interfaces are set up, register them
 	 * to trigger binding of drivers to interfaces.  probe()
 	 * routines may install different altsettings and may
diff --git a/drivers/usb/core/quirks.c b/drivers/usb/core/quirks.c
index 4c65eb6a867..32d3adc315f 100644
--- a/drivers/usb/core/quirks.c
+++ b/drivers/usb/core/quirks.c
@@ -123,6 +123,9 @@ static const struct usb_device_id usb_quirk_list[] = {
 	/* Guillemot Webcam Hercules Dualpix Exchange*/
 	{ USB_DEVICE(0x06f8, 0x3005), .driver_info = USB_QUIRK_RESET_RESUME },
 
+	/* Midiman M-Audio Keystation 88es */
+	{ USB_DEVICE(0x0763, 0x0192), .driver_info = USB_QUIRK_RESET_RESUME },
+
 	/* M-Systems Flash Disk Pioneers */
 	{ USB_DEVICE(0x08ec, 0x1000), .driver_info = USB_QUIRK_RESET_RESUME },
 
diff --git a/drivers/usb/core/sysfs.c b/drivers/usb/core/sysfs.c
index 566d9f94f73..9a56e3adf47 100644
--- a/drivers/usb/core/sysfs.c
+++ b/drivers/usb/core/sysfs.c
@@ -73,7 +73,7 @@ set_bConfigurationValue(struct device *dev, struct device_attribute *attr,
 	return (value < 0) ? value : count;
 }
 
-static DEVICE_ATTR(bConfigurationValue, S_IRUGO | S_IWUSR,
+static DEVICE_ATTR_IGNORE_LOCKDEP(bConfigurationValue, S_IRUGO | S_IWUSR,
 		show_bConfigurationValue, set_bConfigurationValue);
 
 /* String fields */
@@ -595,7 +595,7 @@ static ssize_t usb_dev_authorized_store(struct device *dev,
 	return result < 0? result : size;
 }
 
-static DEVICE_ATTR(authorized, 0644,
+static DEVICE_ATTR_IGNORE_LOCKDEP(authorized, 0644,
 	    usb_dev_authorized_show, usb_dev_authorized_store);
 
 /* "Safely remove a device" */
@@ -618,7 +618,7 @@ static ssize_t usb_remove_store(struct device *dev,
 	usb_unlock_device(udev);
 	return rc;
 }
-static DEVICE_ATTR(remove, 0200, NULL, usb_remove_store);
+static DEVICE_ATTR_IGNORE_LOCKDEP(remove, 0200, NULL, usb_remove_store);
 
 
 static struct attribute *dev_attrs[] = {
diff --git a/drivers/usb/core/urb.c b/drivers/usb/core/urb.c
index cd9b3a2cd8a..9d912bfdcff 100644
--- a/drivers/usb/core/urb.c
+++ b/drivers/usb/core/urb.c
@@ -681,6 +681,27 @@ void usb_unpoison_urb(struct urb *urb)
 EXPORT_SYMBOL_GPL(usb_unpoison_urb);
 
 /**
+ * usb_block_urb - reliably prevent further use of an URB
+ * @urb: pointer to URB to be blocked, may be NULL
+ *
+ * After the routine has run, attempts to resubmit the URB will fail
+ * with error -EPERM.  Thus even if the URB's completion handler always
+ * tries to resubmit, it will not succeed and the URB will become idle.
+ *
+ * The URB must not be deallocated while this routine is running.  In
+ * particular, when a driver calls this routine, it must insure that the
+ * completion handler cannot deallocate the URB.
+ */
+void usb_block_urb(struct urb *urb)
+{
+	if (!urb)
+		return;
+
+	atomic_inc(&urb->reject);
+}
+EXPORT_SYMBOL_GPL(usb_block_urb);
+
+/**
  * usb_kill_anchored_urbs - cancel transfer requests en masse
  * @anchor: anchor the requests are bound to
  *
diff --git a/drivers/usb/core/usb-acpi.c b/drivers/usb/core/usb-acpi.c
new file mode 100644
index 00000000000..8947b203d5a
--- /dev/null
+++ b/drivers/usb/core/usb-acpi.c
@@ -0,0 +1,117 @@
+/*
+ * USB-ACPI glue code
+ *
+ * Copyright 2012 Red Hat <mjg@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, version 2.
+ *
+ */
+#include <linux/module.h>
+#include <linux/usb.h>
+#include <linux/device.h>
+#include <linux/errno.h>
+#include <linux/kernel.h>
+#include <linux/acpi.h>
+#include <linux/pci.h>
+#include <acpi/acpi_bus.h>
+
+#include "usb.h"
+
+static int usb_acpi_check_upc(struct usb_device *udev, acpi_handle handle)
+{
+	acpi_status status;
+	struct acpi_buffer buffer = { ACPI_ALLOCATE_BUFFER, NULL };
+	union acpi_object *upc;
+	int ret = 0;
+
+	status = acpi_evaluate_object(handle, "_UPC", NULL, &buffer);
+
+	if (ACPI_FAILURE(status))
+		return -ENODEV;
+
+	upc = buffer.pointer;
+
+	if (!upc || (upc->type != ACPI_TYPE_PACKAGE)
+		|| upc->package.count != 4) {
+		ret = -EINVAL;
+		goto out;
+	}
+
+	if (upc->package.elements[0].integer.value)
+		udev->removable = USB_DEVICE_REMOVABLE;
+	else
+		udev->removable = USB_DEVICE_FIXED;
+
+out:
+	kfree(upc);
+	return ret;
+}
+
+static int usb_acpi_check_pld(struct usb_device *udev, acpi_handle handle)
+{
+	acpi_status status;
+	struct acpi_pld pld;
+
+	status = acpi_get_physical_device_location(handle, &pld);
+
+	if (ACPI_FAILURE(status))
+		return -ENODEV;
+
+	if (pld.user_visible)
+		udev->removable = USB_DEVICE_REMOVABLE;
+	else
+		udev->removable = USB_DEVICE_FIXED;
+
+	return 0;
+}
+
+static int usb_acpi_find_device(struct device *dev, acpi_handle *handle)
+{
+	struct usb_device *udev;
+	struct device *parent;
+	acpi_handle *parent_handle;
+
+	if (!is_usb_device(dev))
+		return -ENODEV;
+
+	udev = to_usb_device(dev);
+	parent = dev->parent;
+	parent_handle = DEVICE_ACPI_HANDLE(parent);
+
+	if (!parent_handle)
+		return -ENODEV;
+
+	*handle = acpi_get_child(parent_handle, udev->portnum);
+
+	if (!*handle)
+		return -ENODEV;
+
+	/*
+	 * PLD will tell us whether a port is removable to the user or
+	 * not. If we don't get an answer from PLD (it's not present
+	 * or it's malformed) then try to infer it from UPC. If a
+	 * device isn't connectable then it's probably not removable.
+	 */
+	if (usb_acpi_check_pld(udev, *handle) != 0)
+		usb_acpi_check_upc(udev, *handle);
+
+	return 0;
+}
+
+static struct acpi_bus_type usb_acpi_bus = {
+	.bus = &usb_bus_type,
+	.find_bridge = NULL,
+	.find_device = usb_acpi_find_device,
+};
+
+int usb_acpi_register(void)
+{
+	return register_acpi_bus_type(&usb_acpi_bus);
+}
+
+void usb_acpi_unregister(void)
+{
+	unregister_acpi_bus_type(&usb_acpi_bus);
+}
diff --git a/drivers/usb/core/usb.c b/drivers/usb/core/usb.c
index c74ba7bbc74..25d0c61c3f8 100644
--- a/drivers/usb/core/usb.c
+++ b/drivers/usb/core/usb.c
@@ -1015,6 +1015,7 @@ static int __init usb_init(void)
 	if (retval)
 		goto out;
 
+	usb_acpi_register();
 	retval = bus_register(&usb_bus_type);
 	if (retval)
 		goto bus_register_failed;
@@ -1030,9 +1031,6 @@ static int __init usb_init(void)
 	retval = usb_devio_init();
 	if (retval)
 		goto usb_devio_init_failed;
-	retval = usbfs_init();
-	if (retval)
-		goto fs_init_failed;
 	retval = usb_hub_init();
 	if (retval)
 		goto hub_init_failed;
@@ -1042,8 +1040,6 @@ static int __init usb_init(void)
 
 	usb_hub_cleanup();
 hub_init_failed:
-	usbfs_cleanup();
-fs_init_failed:
 	usb_devio_cleanup();
 usb_devio_init_failed:
 	usb_deregister(&usbfs_driver);
@@ -1054,6 +1050,7 @@ major_init_failed:
 bus_notifier_failed:
 	bus_unregister(&usb_bus_type);
 bus_register_failed:
+	usb_acpi_unregister();
 	usb_debugfs_cleanup();
 out:
 	return retval;
@@ -1070,12 +1067,12 @@ static void __exit usb_exit(void)
 
 	usb_deregister_device_driver(&usb_generic_driver);
 	usb_major_cleanup();
-	usbfs_cleanup();
 	usb_deregister(&usbfs_driver);
 	usb_devio_cleanup();
 	usb_hub_cleanup();
 	bus_unregister_notifier(&usb_bus_type, &usb_bus_nb);
 	bus_unregister(&usb_bus_type);
+	usb_acpi_unregister();
 	usb_debugfs_cleanup();
 }
 
diff --git a/drivers/usb/core/usb.h b/drivers/usb/core/usb.h
index 71648dcbe43..5c5c538ea73 100644
--- a/drivers/usb/core/usb.h
+++ b/drivers/usb/core/usb.h
@@ -156,3 +156,10 @@ extern void usb_notify_remove_device(struct usb_device *udev);
 extern void usb_notify_add_bus(struct usb_bus *ubus);
 extern void usb_notify_remove_bus(struct usb_bus *ubus);
 
+#ifdef CONFIG_ACPI
+extern int usb_acpi_register(void);
+extern void usb_acpi_unregister(void);
+#else
+static inline int usb_acpi_register(void) { return 0; };
+static inline void usb_acpi_unregister(void) { };
+#endif
diff --git a/drivers/usb/dwc3/Kconfig b/drivers/usb/dwc3/Kconfig
index d8f741f9e56..d13c60f4213 100644
--- a/drivers/usb/dwc3/Kconfig
+++ b/drivers/usb/dwc3/Kconfig
@@ -4,7 +4,7 @@ config USB_DWC3
 	select USB_OTG_UTILS
 	select USB_GADGET_DUALSPEED
 	select USB_GADGET_SUPERSPEED
-	select USB_XHCI_PLATFORM
+	select USB_XHCI_PLATFORM if USB_SUPPORT && USB_XHCI_HCD
 	help
 	  Say Y or M here if your system has a Dual Role SuperSpeed
 	  USB controller based on the DesignWare USB3 IP Core.
diff --git a/drivers/usb/dwc3/core.c b/drivers/usb/dwc3/core.c
index 99b58d84553..1040bdb8dc8 100644
--- a/drivers/usb/dwc3/core.c
+++ b/drivers/usb/dwc3/core.c
@@ -410,7 +410,6 @@ static int __devinit dwc3_probe(struct platform_device *pdev)
 	struct device		*dev = &pdev->dev;
 
 	int			ret = -ENOMEM;
-	int			irq;
 
 	void __iomem		*regs;
 	void			*mem;
@@ -425,15 +424,28 @@ static int __devinit dwc3_probe(struct platform_device *pdev)
 	dwc = PTR_ALIGN(mem, DWC3_ALIGN_MASK + 1);
 	dwc->mem = mem;
 
-	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	res = platform_get_resource(pdev, IORESOURCE_IRQ, 0);
 	if (!res) {
-		dev_err(dev, "missing resource\n");
+		dev_err(dev, "missing IRQ\n");
 		return -ENODEV;
 	}
+	dwc->xhci_resources[1] = *res;
 
-	dwc->res = res;
-
-	res = devm_request_mem_region(dev, res->start, resource_size(res),
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	if (!res) {
+		dev_err(dev, "missing memory resource\n");
+		return -ENODEV;
+	}
+	dwc->xhci_resources[0] = *res;
+	dwc->xhci_resources[0].end = dwc->xhci_resources[0].start +
+					DWC3_XHCI_REGS_END;
+
+	 /*
+	  * Request memory region but exclude xHCI regs,
+	  * since it will be requested by the xhci-plat driver.
+	  */
+	res = devm_request_mem_region(dev, res->start + DWC3_GLOBALS_REGS_START,
+			resource_size(res) - DWC3_GLOBALS_REGS_START,
 			dev_name(dev));
 	if (!res) {
 		dev_err(dev, "can't request mem region\n");
@@ -446,19 +458,12 @@ static int __devinit dwc3_probe(struct platform_device *pdev)
 		return -ENOMEM;
 	}
 
-	irq = platform_get_irq(pdev, 0);
-	if (irq < 0) {
-		dev_err(dev, "missing IRQ\n");
-		return -ENODEV;
-	}
-
 	spin_lock_init(&dwc->lock);
 	platform_set_drvdata(pdev, dwc);
 
 	dwc->regs	= regs;
 	dwc->regs_size	= resource_size(res);
 	dwc->dev	= dev;
-	dwc->irq	= irq;
 
 	if (!strncmp("super", maximum_speed, 5))
 		dwc->maximum_speed = DWC3_DCFG_SUPERSPEED;
diff --git a/drivers/usb/dwc3/core.h b/drivers/usb/dwc3/core.h
index 6c7945b4cad..f69c877add0 100644
--- a/drivers/usb/dwc3/core.h
+++ b/drivers/usb/dwc3/core.h
@@ -51,7 +51,9 @@
 #include <linux/usb/gadget.h>
 
 /* Global constants */
+#define DWC3_EP0_BOUNCE_SIZE	512
 #define DWC3_ENDPOINTS_NUM	32
+#define DWC3_XHCI_RESOURCES_NUM	2
 
 #define DWC3_EVENT_BUFFERS_SIZE	PAGE_SIZE
 #define DWC3_EVENT_TYPE_MASK	0xfe
@@ -75,6 +77,16 @@
 #define DWC3_GSNPSID_MASK	0xffff0000
 #define DWC3_GSNPSREV_MASK	0xffff
 
+/* DWC3 registers memory space boundries */
+#define DWC3_XHCI_REGS_START		0x0
+#define DWC3_XHCI_REGS_END		0x7fff
+#define DWC3_GLOBALS_REGS_START		0xc100
+#define DWC3_GLOBALS_REGS_END		0xc6ff
+#define DWC3_DEVICE_REGS_START		0xc700
+#define DWC3_DEVICE_REGS_END		0xcbff
+#define DWC3_OTG_REGS_START		0xcc00
+#define DWC3_OTG_REGS_END		0xccff
+
 /* Global Registers */
 #define DWC3_GSBUSCFG0		0xc100
 #define DWC3_GSBUSCFG1		0xc104
@@ -183,6 +195,7 @@
 #define DWC3_GHWPARAMS1_EN_PWROPT_CLK	1
 
 /* Device Configuration Register */
+#define DWC3_DCFG_LPM_CAP	(1 << 22)
 #define DWC3_DCFG_DEVADDR(addr)	((addr) << 3)
 #define DWC3_DCFG_DEVADDR_MASK	DWC3_DCFG_DEVADDR(0x7f)
 
@@ -272,12 +285,14 @@
 #define DWC3_DGCMD_SET_ENDPOINT_NRDY	0x0c
 #define DWC3_DGCMD_RUN_SOC_BUS_LOOPBACK	0x10
 
+#define DWC3_DGCMD_STATUS(n)		(((n) >> 15) & 1)
+#define DWC3_DGCMD_CMDACT		(1 << 10)
+
 /* Device Endpoint Command Register */
 #define DWC3_DEPCMD_PARAM_SHIFT		16
 #define DWC3_DEPCMD_PARAM(x)		((x) << DWC3_DEPCMD_PARAM_SHIFT)
 #define DWC3_DEPCMD_GET_RSC_IDX(x)     (((x) >> DWC3_DEPCMD_PARAM_SHIFT) & 0x7f)
-#define DWC3_DEPCMD_STATUS_MASK		(0x0f << 12)
-#define DWC3_DEPCMD_STATUS(x)		(((x) & DWC3_DEPCMD_STATUS_MASK) >> 12)
+#define DWC3_DEPCMD_STATUS(x)		(((x) >> 15) & 1)
 #define DWC3_DEPCMD_HIPRI_FORCERM	(1 << 11)
 #define DWC3_DEPCMD_CMDACT		(1 << 10)
 #define DWC3_DEPCMD_CMDIOC		(1 << 8)
@@ -361,7 +376,6 @@ struct dwc3_ep {
 	dma_addr_t		trb_pool_dma;
 	u32			free_slot;
 	u32			busy_slot;
-	const struct usb_endpoint_descriptor *desc;
 	const struct usb_ss_ep_comp_descriptor *comp_desc;
 	struct dwc3		*dwc;
 
@@ -561,6 +575,11 @@ struct dwc3_request {
  * @setup_packet_pending: true when there's a Setup Packet in FIFO. Workaround
  * @needs_fifo_resize: not all users might want fifo resizing, flag it
  * @resize_fifos: tells us it's ok to reconfigure our TxFIFO sizes.
+ * @isoch_delay: wValue from Set Isochronous Delay request;
+ * @u2sel: parameter from Set SEL request.
+ * @u2pel: parameter from Set SEL request.
+ * @u1sel: parameter from Set SEL request.
+ * @u1pel: parameter from Set SEL request.
  * @ep0_next_event: hold the next expected event
  * @ep0state: state of endpoint zero
  * @link_state: link state
@@ -583,7 +602,7 @@ struct dwc3 {
 	struct device		*dev;
 
 	struct platform_device	*xhci;
-	struct resource		*res;
+	struct resource		xhci_resources[DWC3_XHCI_RESOURCES_NUM];
 
 	struct dwc3_event_buffer **ev_buffs;
 	struct dwc3_ep		*eps[DWC3_ENDPOINTS_NUM];
@@ -594,8 +613,6 @@ struct dwc3 {
 	void __iomem		*regs;
 	size_t			regs_size;
 
-	int			irq;
-
 	u32			num_event_buffers;
 	u32			u1u2;
 	u32			maximum_speed;
@@ -609,6 +626,10 @@ struct dwc3 {
 #define DWC3_REVISION_185A	0x5533185a
 #define DWC3_REVISION_188A	0x5533188a
 #define DWC3_REVISION_190A	0x5533190a
+#define DWC3_REVISION_200A	0x5533200a
+#define DWC3_REVISION_202A	0x5533202a
+#define DWC3_REVISION_210A	0x5533210a
+#define DWC3_REVISION_220A	0x5533220a
 
 	unsigned		is_selfpowered:1;
 	unsigned		three_stage_setup:1;
@@ -625,7 +646,14 @@ struct dwc3 {
 	enum dwc3_link_state	link_state;
 	enum dwc3_device_state	dev_state;
 
+	u16			isoch_delay;
+	u16			u2sel;
+	u16			u2pel;
+	u8			u1sel;
+	u8			u1pel;
+
 	u8			speed;
+
 	void			*mem;
 
 	struct dwc3_hwparams	hwparams;
diff --git a/drivers/usb/dwc3/dwc3-omap.c b/drivers/usb/dwc3/dwc3-omap.c
index d7d9c0ec951..479dc047da3 100644
--- a/drivers/usb/dwc3/dwc3-omap.c
+++ b/drivers/usb/dwc3/dwc3-omap.c
@@ -49,7 +49,6 @@
 #include <linux/of.h>
 
 #include "core.h"
-#include "io.h"
 
 /*
  * All these registers belong to OMAP's Wrapper around the
@@ -143,6 +142,17 @@ struct dwc3_omap {
 	u32			dma_status:1;
 };
 
+static inline u32 dwc3_omap_readl(void __iomem *base, u32 offset)
+{
+	return readl(base + offset);
+}
+
+static inline void dwc3_omap_writel(void __iomem *base, u32 offset, u32 value)
+{
+	writel(value, base + offset);
+}
+
+
 static irqreturn_t dwc3_omap_interrupt(int irq, void *_omap)
 {
 	struct dwc3_omap	*omap = _omap;
@@ -150,7 +160,7 @@ static irqreturn_t dwc3_omap_interrupt(int irq, void *_omap)
 
 	spin_lock(&omap->lock);
 
-	reg = dwc3_readl(omap->base, USBOTGSS_IRQSTATUS_1);
+	reg = dwc3_omap_readl(omap->base, USBOTGSS_IRQSTATUS_1);
 
 	if (reg & USBOTGSS_IRQ1_DMADISABLECLR) {
 		dev_dbg(omap->dev, "DMA Disable was Cleared\n");
@@ -184,10 +194,10 @@ static irqreturn_t dwc3_omap_interrupt(int irq, void *_omap)
 	if (reg & USBOTGSS_IRQ1_IDPULLUP_FALL)
 		dev_dbg(omap->dev, "IDPULLUP Fall\n");
 
-	dwc3_writel(omap->base, USBOTGSS_IRQSTATUS_1, reg);
+	dwc3_omap_writel(omap->base, USBOTGSS_IRQSTATUS_1, reg);
 
-	reg = dwc3_readl(omap->base, USBOTGSS_IRQSTATUS_0);
-	dwc3_writel(omap->base, USBOTGSS_IRQSTATUS_0, reg);
+	reg = dwc3_omap_readl(omap->base, USBOTGSS_IRQSTATUS_0);
+	dwc3_omap_writel(omap->base, USBOTGSS_IRQSTATUS_0, reg);
 
 	spin_unlock(&omap->lock);
 
@@ -270,7 +280,7 @@ static int __devinit dwc3_omap_probe(struct platform_device *pdev)
 	omap->base	= base;
 	omap->dwc3	= dwc3;
 
-	reg = dwc3_readl(omap->base, USBOTGSS_UTMI_OTG_STATUS);
+	reg = dwc3_omap_readl(omap->base, USBOTGSS_UTMI_OTG_STATUS);
 
 	utmi_mode = of_get_property(node, "utmi-mode", &size);
 	if (utmi_mode && size == sizeof(*utmi_mode)) {
@@ -293,10 +303,10 @@ static int __devinit dwc3_omap_probe(struct platform_device *pdev)
 		}
 	}
 
-	dwc3_writel(omap->base, USBOTGSS_UTMI_OTG_STATUS, reg);
+	dwc3_omap_writel(omap->base, USBOTGSS_UTMI_OTG_STATUS, reg);
 
 	/* check the DMA Status */
-	reg = dwc3_readl(omap->base, USBOTGSS_SYSCONFIG);
+	reg = dwc3_omap_readl(omap->base, USBOTGSS_SYSCONFIG);
 	omap->dma_status = !!(reg & USBOTGSS_SYSCONFIG_DMADISABLE);
 
 	/* Set No-Idle and No-Standby */
@@ -306,7 +316,7 @@ static int __devinit dwc3_omap_probe(struct platform_device *pdev)
 	reg |= (USBOTGSS_SYSCONFIG_STANDBYMODE(USBOTGSS_STANDBYMODE_NO_STANDBY)
 		| USBOTGSS_SYSCONFIG_IDLEMODE(USBOTGSS_IDLEMODE_NO_IDLE));
 
-	dwc3_writel(omap->base, USBOTGSS_SYSCONFIG, reg);
+	dwc3_omap_writel(omap->base, USBOTGSS_SYSCONFIG, reg);
 
 	ret = devm_request_irq(dev, omap->irq, dwc3_omap_interrupt, 0,
 			"dwc3-omap", omap);
@@ -318,7 +328,7 @@ static int __devinit dwc3_omap_probe(struct platform_device *pdev)
 
 	/* enable all IRQs */
 	reg = USBOTGSS_IRQO_COREIRQ_ST;
-	dwc3_writel(omap->base, USBOTGSS_IRQENABLE_SET_0, reg);
+	dwc3_omap_writel(omap->base, USBOTGSS_IRQENABLE_SET_0, reg);
 
 	reg = (USBOTGSS_IRQ1_OEVT |
 			USBOTGSS_IRQ1_DRVVBUS_RISE |
@@ -330,7 +340,7 @@ static int __devinit dwc3_omap_probe(struct platform_device *pdev)
 			USBOTGSS_IRQ1_DISCHRGVBUS_FALL |
 			USBOTGSS_IRQ1_IDPULLUP_FALL);
 
-	dwc3_writel(omap->base, USBOTGSS_IRQENABLE_SET_1, reg);
+	dwc3_omap_writel(omap->base, USBOTGSS_IRQENABLE_SET_1, reg);
 
 	ret = platform_device_add_resources(dwc3, pdev->resource,
 			pdev->num_resources);
diff --git a/drivers/usb/dwc3/ep0.c b/drivers/usb/dwc3/ep0.c
index 3584a169886..9e8a3dce69f 100644
--- a/drivers/usb/dwc3/ep0.c
+++ b/drivers/usb/dwc3/ep0.c
@@ -179,7 +179,7 @@ int dwc3_gadget_ep0_queue(struct usb_ep *ep, struct usb_request *request,
 	int				ret;
 
 	spin_lock_irqsave(&dwc->lock, flags);
-	if (!dep->desc) {
+	if (!dep->endpoint.desc) {
 		dev_dbg(dwc->dev, "trying to queue request %p to disabled %s\n",
 				request, dep->name);
 		ret = -ESHUTDOWN;
@@ -261,6 +261,7 @@ static int dwc3_ep0_handle_status(struct dwc3 *dwc,
 {
 	struct dwc3_ep		*dep;
 	u32			recip;
+	u32			reg;
 	u16			usb_status = 0;
 	__le16			*response_pkt;
 
@@ -268,10 +269,18 @@ static int dwc3_ep0_handle_status(struct dwc3 *dwc,
 	switch (recip) {
 	case USB_RECIP_DEVICE:
 		/*
-		 * We are self-powered. U1/U2/LTM will be set later
-		 * once we handle this states. RemoteWakeup is 0 on SS
+		 * LTM will be set once we know how to set this in HW.
 		 */
 		usb_status |= dwc->is_selfpowered << USB_DEVICE_SELF_POWERED;
+
+		if (dwc->speed == DWC3_DSTS_SUPERSPEED) {
+			reg = dwc3_readl(dwc->regs, DWC3_DCTL);
+			if (reg & DWC3_DCTL_INITU1ENA)
+				usb_status |= 1 << USB_DEV_STAT_U1_ENABLED;
+			if (reg & DWC3_DCTL_INITU2ENA)
+				usb_status |= 1 << USB_DEV_STAT_U2_ENABLED;
+		}
+
 		break;
 
 	case USB_RECIP_INTERFACE:
@@ -312,6 +321,7 @@ static int dwc3_ep0_handle_feature(struct dwc3 *dwc,
 	u32			recip;
 	u32			wValue;
 	u32			wIndex;
+	u32			reg;
 	int			ret;
 
 	wValue = le16_to_cpu(ctrl->wValue);
@@ -320,29 +330,43 @@ static int dwc3_ep0_handle_feature(struct dwc3 *dwc,
 	switch (recip) {
 	case USB_RECIP_DEVICE:
 
+		switch (wValue) {
+		case USB_DEVICE_REMOTE_WAKEUP:
+			break;
 		/*
 		 * 9.4.1 says only only for SS, in AddressState only for
 		 * default control pipe
 		 */
-		switch (wValue) {
 		case USB_DEVICE_U1_ENABLE:
-		case USB_DEVICE_U2_ENABLE:
-		case USB_DEVICE_LTM_ENABLE:
 			if (dwc->dev_state != DWC3_CONFIGURED_STATE)
 				return -EINVAL;
 			if (dwc->speed != DWC3_DSTS_SUPERSPEED)
 				return -EINVAL;
-		}
 
-		/* XXX add U[12] & LTM */
-		switch (wValue) {
-		case USB_DEVICE_REMOTE_WAKEUP:
-			break;
-		case USB_DEVICE_U1_ENABLE:
+			reg = dwc3_readl(dwc->regs, DWC3_DCTL);
+			if (set)
+				reg |= DWC3_DCTL_INITU1ENA;
+			else
+				reg &= ~DWC3_DCTL_INITU1ENA;
+			dwc3_writel(dwc->regs, DWC3_DCTL, reg);
 			break;
+
 		case USB_DEVICE_U2_ENABLE:
+			if (dwc->dev_state != DWC3_CONFIGURED_STATE)
+				return -EINVAL;
+			if (dwc->speed != DWC3_DSTS_SUPERSPEED)
+				return -EINVAL;
+
+			reg = dwc3_readl(dwc->regs, DWC3_DCTL);
+			if (set)
+				reg |= DWC3_DCTL_INITU2ENA;
+			else
+				reg &= ~DWC3_DCTL_INITU2ENA;
+			dwc3_writel(dwc->regs, DWC3_DCTL, reg);
 			break;
+
 		case USB_DEVICE_LTM_ENABLE:
+			return -EINVAL;
 			break;
 
 		case USB_DEVICE_TEST_MODE:
@@ -469,6 +493,107 @@ static int dwc3_ep0_set_config(struct dwc3 *dwc, struct usb_ctrlrequest *ctrl)
 	return ret;
 }
 
+static void dwc3_ep0_set_sel_cmpl(struct usb_ep *ep, struct usb_request *req)
+{
+	struct dwc3_ep	*dep = to_dwc3_ep(ep);
+	struct dwc3	*dwc = dep->dwc;
+
+	u32		param = 0;
+	u32		reg;
+
+	struct timing {
+		u8	u1sel;
+		u8	u1pel;
+		u16	u2sel;
+		u16	u2pel;
+	} __packed timing;
+
+	int		ret;
+
+	memcpy(&timing, req->buf, sizeof(timing));
+
+	dwc->u1sel = timing.u1sel;
+	dwc->u1pel = timing.u1pel;
+	dwc->u2sel = timing.u2sel;
+	dwc->u2pel = timing.u2pel;
+
+	reg = dwc3_readl(dwc->regs, DWC3_DCTL);
+	if (reg & DWC3_DCTL_INITU2ENA)
+		param = dwc->u2pel;
+	if (reg & DWC3_DCTL_INITU1ENA)
+		param = dwc->u1pel;
+
+	/*
+	 * According to Synopsys Databook, if parameter is
+	 * greater than 125, a value of zero should be
+	 * programmed in the register.
+	 */
+	if (param > 125)
+		param = 0;
+
+	/* now that we have the time, issue DGCMD Set Sel */
+	ret = dwc3_send_gadget_generic_command(dwc,
+			DWC3_DGCMD_SET_PERIODIC_PAR, param);
+	WARN_ON(ret < 0);
+}
+
+static int dwc3_ep0_set_sel(struct dwc3 *dwc, struct usb_ctrlrequest *ctrl)
+{
+	struct dwc3_ep	*dep;
+	u16		wLength;
+	u16		wValue;
+
+	if (dwc->dev_state == DWC3_DEFAULT_STATE)
+		return -EINVAL;
+
+	wValue = le16_to_cpu(ctrl->wValue);
+	wLength = le16_to_cpu(ctrl->wLength);
+
+	if (wLength != 6) {
+		dev_err(dwc->dev, "Set SEL should be 6 bytes, got %d\n",
+				wLength);
+		return -EINVAL;
+	}
+
+	/*
+	 * To handle Set SEL we need to receive 6 bytes from Host. So let's
+	 * queue a usb_request for 6 bytes.
+	 *
+	 * Remember, though, this controller can't handle non-wMaxPacketSize
+	 * aligned transfers on the OUT direction, so we queue a request for
+	 * wMaxPacketSize instead.
+	 */
+	dep = dwc->eps[0];
+	dwc->ep0_usb_req.dep = dep;
+	dwc->ep0_usb_req.request.length = dep->endpoint.maxpacket;
+	dwc->ep0_usb_req.request.buf = dwc->setup_buf;
+	dwc->ep0_usb_req.request.complete = dwc3_ep0_set_sel_cmpl;
+
+	return __dwc3_gadget_ep0_queue(dep, &dwc->ep0_usb_req);
+}
+
+static int dwc3_ep0_set_isoch_delay(struct dwc3 *dwc, struct usb_ctrlrequest *ctrl)
+{
+	u16		wLength;
+	u16		wValue;
+	u16		wIndex;
+
+	wValue = le16_to_cpu(ctrl->wValue);
+	wLength = le16_to_cpu(ctrl->wLength);
+	wIndex = le16_to_cpu(ctrl->wIndex);
+
+	if (wIndex || wLength)
+		return -EINVAL;
+
+	/*
+	 * REVISIT It's unclear from Databook what to do with this
+	 * value. For now, just cache it.
+	 */
+	dwc->isoch_delay = wValue;
+
+	return 0;
+}
+
 static int dwc3_ep0_std_request(struct dwc3 *dwc, struct usb_ctrlrequest *ctrl)
 {
 	int ret;
@@ -494,6 +619,14 @@ static int dwc3_ep0_std_request(struct dwc3 *dwc, struct usb_ctrlrequest *ctrl)
 		dev_vdbg(dwc->dev, "USB_REQ_SET_CONFIGURATION\n");
 		ret = dwc3_ep0_set_config(dwc, ctrl);
 		break;
+	case USB_REQ_SET_SEL:
+		dev_vdbg(dwc->dev, "USB_REQ_SET_SEL\n");
+		ret = dwc3_ep0_set_sel(dwc, ctrl);
+		break;
+	case USB_REQ_SET_ISOCH_DELAY:
+		dev_vdbg(dwc->dev, "USB_REQ_SET_ISOCH_DELAY\n");
+		ret = dwc3_ep0_set_isoch_delay(dwc, ctrl);
+		break;
 	default:
 		dev_vdbg(dwc->dev, "Forwarding to gadget driver\n");
 		ret = dwc3_ep0_delegate_req(dwc, ctrl);
diff --git a/drivers/usb/dwc3/gadget.c b/drivers/usb/dwc3/gadget.c
index 5255fe975ea..3df1a1973b0 100644
--- a/drivers/usb/dwc3/gadget.c
+++ b/drivers/usb/dwc3/gadget.c
@@ -178,8 +178,8 @@ int dwc3_gadget_resize_tx_fifos(struct dwc3 *dwc)
 		if (!(dep->flags & DWC3_EP_ENABLED))
 			continue;
 
-		if (usb_endpoint_xfer_bulk(dep->desc)
-				|| usb_endpoint_xfer_isoc(dep->desc))
+		if (usb_endpoint_xfer_bulk(dep->endpoint.desc)
+				|| usb_endpoint_xfer_isoc(dep->endpoint.desc))
 			mult = 3;
 
 		/*
@@ -229,7 +229,7 @@ void dwc3_gadget_giveback(struct dwc3_ep *dep, struct dwc3_request *req,
 		 * completed (not the LINK TRB).
 		 */
 		if (((dep->busy_slot & DWC3_TRB_MASK) == DWC3_TRB_NUM - 1) &&
-				usb_endpoint_xfer_isoc(dep->desc))
+				usb_endpoint_xfer_isoc(dep->endpoint.desc))
 			dep->busy_slot++;
 	}
 	list_del(&req->list);
@@ -276,6 +276,33 @@ static const char *dwc3_gadget_ep_cmd_string(u8 cmd)
 	}
 }
 
+int dwc3_send_gadget_generic_command(struct dwc3 *dwc, int cmd, u32 param)
+{
+	u32		timeout = 500;
+	u32		reg;
+
+	dwc3_writel(dwc->regs, DWC3_DGCMDPAR, param);
+	dwc3_writel(dwc->regs, DWC3_DGCMD, cmd | DWC3_DGCMD_CMDACT);
+
+	do {
+		reg = dwc3_readl(dwc->regs, DWC3_DGCMD);
+		if (!(reg & DWC3_DGCMD_CMDACT)) {
+			dev_vdbg(dwc->dev, "Command Complete --> %d\n",
+					DWC3_DGCMD_STATUS(reg));
+			return 0;
+		}
+
+		/*
+		 * We can't sleep here, because it's also called from
+		 * interrupt context.
+		 */
+		timeout--;
+		if (!timeout)
+			return -ETIMEDOUT;
+		udelay(1);
+	} while (1);
+}
+
 int dwc3_send_gadget_ep_cmd(struct dwc3 *dwc, unsigned ep,
 		unsigned cmd, struct dwc3_gadget_ep_cmd_params *params)
 {
@@ -470,7 +497,7 @@ static int __dwc3_gadget_ep_enable(struct dwc3_ep *dep,
 		if (ret)
 			return ret;
 
-		dep->desc = desc;
+		dep->endpoint.desc = desc;
 		dep->comp_desc = comp_desc;
 		dep->type = usb_endpoint_type(desc);
 		dep->flags |= DWC3_EP_ENABLED;
@@ -533,7 +560,6 @@ static int __dwc3_gadget_ep_disable(struct dwc3_ep *dep)
 	dwc3_writel(dwc->regs, DWC3_DALEPENA, reg);
 
 	dep->stream_capable = false;
-	dep->desc = NULL;
 	dep->endpoint.desc = NULL;
 	dep->comp_desc = NULL;
 	dep->type = 0;
@@ -694,7 +720,7 @@ static void dwc3_prepare_one_trb(struct dwc3_ep *dep,
 
 	/* Skip the LINK-TRB on ISOC */
 	if (((cur_slot & DWC3_TRB_MASK) == DWC3_TRB_NUM - 1) &&
-			usb_endpoint_xfer_isoc(dep->desc))
+			usb_endpoint_xfer_isoc(dep->endpoint.desc))
 		return;
 
 	if (!req->trb) {
@@ -707,7 +733,7 @@ static void dwc3_prepare_one_trb(struct dwc3_ep *dep,
 	trb->bpl = lower_32_bits(dma);
 	trb->bph = upper_32_bits(dma);
 
-	switch (usb_endpoint_type(dep->desc)) {
+	switch (usb_endpoint_type(dep->endpoint.desc)) {
 	case USB_ENDPOINT_XFER_CONTROL:
 		trb->ctrl = DWC3_TRBCTL_CONTROL_SETUP;
 		break;
@@ -732,7 +758,7 @@ static void dwc3_prepare_one_trb(struct dwc3_ep *dep,
 		BUG();
 	}
 
-	if (usb_endpoint_xfer_isoc(dep->desc)) {
+	if (usb_endpoint_xfer_isoc(dep->endpoint.desc)) {
 		trb->ctrl |= DWC3_TRB_CTRL_ISP_IMI;
 		trb->ctrl |= DWC3_TRB_CTRL_CSP;
 	} else {
@@ -743,7 +769,7 @@ static void dwc3_prepare_one_trb(struct dwc3_ep *dep,
 			trb->ctrl |= DWC3_TRB_CTRL_LST;
 	}
 
-	if (usb_endpoint_xfer_bulk(dep->desc) && dep->stream_capable)
+	if (usb_endpoint_xfer_bulk(dep->endpoint.desc) && dep->stream_capable)
 		trb->ctrl |= DWC3_TRB_CTRL_SID_SOFN(req->request.stream_id);
 
 	trb->ctrl |= DWC3_TRB_CTRL_HWO;
@@ -771,7 +797,7 @@ static void dwc3_prepare_trbs(struct dwc3_ep *dep, bool starting)
 	trbs_left = (dep->busy_slot - dep->free_slot) & DWC3_TRB_MASK;
 
 	/* Can't wrap around on a non-isoc EP since there's no link TRB */
-	if (!usb_endpoint_xfer_isoc(dep->desc)) {
+	if (!usb_endpoint_xfer_isoc(dep->endpoint.desc)) {
 		max = DWC3_TRB_NUM - (dep->free_slot & DWC3_TRB_MASK);
 		if (trbs_left > max)
 			trbs_left = max;
@@ -797,7 +823,7 @@ static void dwc3_prepare_trbs(struct dwc3_ep *dep, bool starting)
 		 * processed from the first TRB until the last one. Since we
 		 * don't wrap around we have to start at the beginning.
 		 */
-		if (usb_endpoint_xfer_isoc(dep->desc)) {
+		if (usb_endpoint_xfer_isoc(dep->endpoint.desc)) {
 			dep->busy_slot = 1;
 			dep->free_slot = 1;
 		} else {
@@ -807,7 +833,7 @@ static void dwc3_prepare_trbs(struct dwc3_ep *dep, bool starting)
 	}
 
 	/* The last TRB is a link TRB, not used for xfer */
-	if ((trbs_left <= 1) && usb_endpoint_xfer_isoc(dep->desc))
+	if ((trbs_left <= 1) && usb_endpoint_xfer_isoc(dep->endpoint.desc))
 		return;
 
 	list_for_each_entry_safe(req, n, &dep->request_list, list) {
@@ -930,10 +956,12 @@ static int __dwc3_gadget_kick_transfer(struct dwc3_ep *dep, u16 cmd_param,
 	}
 
 	dep->flags |= DWC3_EP_BUSY;
-	dep->res_trans_idx = dwc3_gadget_ep_get_transfer_index(dwc,
-			dep->number);
 
-	WARN_ON_ONCE(!dep->res_trans_idx);
+	if (start_new) {
+		dep->res_trans_idx = dwc3_gadget_ep_get_transfer_index(dwc,
+				dep->number);
+		WARN_ON_ONCE(!dep->res_trans_idx);
+	}
 
 	return 0;
 }
@@ -967,28 +995,37 @@ static int __dwc3_gadget_ep_queue(struct dwc3_ep *dep, struct dwc3_request *req)
 
 	list_add_tail(&req->list, &dep->request_list);
 
+	if (usb_endpoint_xfer_isoc(dep->endpoint.desc) && (dep->flags & DWC3_EP_BUSY))
+		dep->flags |= DWC3_EP_PENDING_REQUEST;
+
 	/*
-	 * There is one special case: XferNotReady with
-	 * empty list of requests. We need to kick the
-	 * transfer here in that situation, otherwise
-	 * we will be NAKing forever.
+	 * There are two special cases:
+	 *
+	 * 1. XferNotReady with empty list of requests. We need to kick the
+	 *    transfer here in that situation, otherwise we will be NAKing
+	 *    forever. If we get XferNotReady before gadget driver has a
+	 *    chance to queue a request, we will ACK the IRQ but won't be
+	 *    able to receive the data until the next request is queued.
+	 *    The following code is handling exactly that.
 	 *
-	 * If we get XferNotReady before gadget driver
-	 * has a chance to queue a request, we will ACK
-	 * the IRQ but won't be able to receive the data
-	 * until the next request is queued. The following
-	 * code is handling exactly that.
+	 * 2. XferInProgress on Isoc EP with an active transfer. We need to
+	 *    kick the transfer here after queuing a request, otherwise the
+	 *    core may not see the modified TRB(s).
 	 */
 	if (dep->flags & DWC3_EP_PENDING_REQUEST) {
-		int ret;
-		int start_trans;
+		int	ret;
+		int	start_trans = 1;
+		u8	trans_idx = dep->res_trans_idx;
 
-		start_trans = 1;
-		if (usb_endpoint_xfer_isoc(dep->desc) &&
-				(dep->flags & DWC3_EP_BUSY))
+		if (usb_endpoint_xfer_isoc(dep->endpoint.desc) &&
+				(dep->flags & DWC3_EP_BUSY)) {
 			start_trans = 0;
+			WARN_ON_ONCE(!trans_idx);
+		} else {
+			trans_idx = 0;
+		}
 
-		ret = __dwc3_gadget_kick_transfer(dep, 0, start_trans);
+		ret = __dwc3_gadget_kick_transfer(dep, trans_idx, start_trans);
 		if (ret && ret != -EBUSY) {
 			struct dwc3	*dwc = dep->dwc;
 
@@ -1011,7 +1048,7 @@ static int dwc3_gadget_ep_queue(struct usb_ep *ep, struct usb_request *request,
 
 	int				ret;
 
-	if (!dep->desc) {
+	if (!dep->endpoint.desc) {
 		dev_dbg(dwc->dev, "trying to queue request %p to disabled %s\n",
 				request, ep->name);
 		return -ESHUTDOWN;
@@ -1125,7 +1162,7 @@ static int dwc3_gadget_ep_set_halt(struct usb_ep *ep, int value)
 
 	spin_lock_irqsave(&dwc->lock, flags);
 
-	if (usb_endpoint_xfer_isoc(dep->desc)) {
+	if (usb_endpoint_xfer_isoc(dep->endpoint.desc)) {
 		dev_err(dwc->dev, "%s is of Isochronous type\n", dep->name);
 		ret = -EINVAL;
 		goto out;
@@ -1356,7 +1393,24 @@ static int dwc3_gadget_start(struct usb_gadget *g,
 
 	reg = dwc3_readl(dwc->regs, DWC3_DCFG);
 	reg &= ~(DWC3_DCFG_SPEED_MASK);
-	reg |= dwc->maximum_speed;
+
+	/**
+	 * WORKAROUND: DWC3 revision < 2.20a have an issue
+	 * which would cause metastability state on Run/Stop
+	 * bit if we try to force the IP to USB2-only mode.
+	 *
+	 * Because of that, we cannot configure the IP to any
+	 * speed other than the SuperSpeed
+	 *
+	 * Refers to:
+	 *
+	 * STAR#9000525659: Clock Domain Crossing on DCTL in
+	 * USB 2.0 Mode
+	 */
+	if (dwc->revision < DWC3_REVISION_220A)
+		reg |= DWC3_DCFG_SUPERSPEED;
+	else
+		reg |= dwc->maximum_speed;
 	dwc3_writel(dwc->regs, DWC3_DCFG, reg);
 
 	dwc->start_config_issued = false;
@@ -1681,7 +1735,7 @@ static void dwc3_endpoint_interrupt(struct dwc3 *dwc,
 	case DWC3_DEPEVT_XFERCOMPLETE:
 		dep->res_trans_idx = 0;
 
-		if (usb_endpoint_xfer_isoc(dep->desc)) {
+		if (usb_endpoint_xfer_isoc(dep->endpoint.desc)) {
 			dev_dbg(dwc->dev, "%s is an Isochronous endpoint\n",
 					dep->name);
 			return;
@@ -1690,7 +1744,7 @@ static void dwc3_endpoint_interrupt(struct dwc3 *dwc,
 		dwc3_endpoint_transfer_complete(dwc, dep, event, 1);
 		break;
 	case DWC3_DEPEVT_XFERINPROGRESS:
-		if (!usb_endpoint_xfer_isoc(dep->desc)) {
+		if (!usb_endpoint_xfer_isoc(dep->endpoint.desc)) {
 			dev_dbg(dwc->dev, "%s is not an Isochronous endpoint\n",
 					dep->name);
 			return;
@@ -1699,7 +1753,7 @@ static void dwc3_endpoint_interrupt(struct dwc3 *dwc,
 		dwc3_endpoint_transfer_complete(dwc, dep, event, 0);
 		break;
 	case DWC3_DEPEVT_XFERNOTREADY:
-		if (usb_endpoint_xfer_isoc(dep->desc)) {
+		if (usb_endpoint_xfer_isoc(dep->endpoint.desc)) {
 			dwc3_gadget_start_isoc(dwc, dep, event);
 		} else {
 			int ret;
@@ -1720,7 +1774,7 @@ static void dwc3_endpoint_interrupt(struct dwc3 *dwc,
 
 		break;
 	case DWC3_DEPEVT_STREAMEVT:
-		if (!usb_endpoint_xfer_bulk(dep->desc)) {
+		if (!usb_endpoint_xfer_bulk(dep->endpoint.desc)) {
 			dev_err(dwc->dev, "Stream event for non-Bulk %s\n",
 					dep->name);
 			return;
@@ -1916,6 +1970,7 @@ static void dwc3_gadget_reset_interrupt(struct dwc3 *dwc)
 
 	reg = dwc3_readl(dwc->regs, DWC3_DCTL);
 	reg &= ~DWC3_DCTL_TSTCTRL_MASK;
+	reg &= ~(DWC3_DCTL_INITU1ENA | DWC3_DCTL_INITU2ENA);
 	dwc3_writel(dwc->regs, DWC3_DCTL, reg);
 	dwc->test_mode = false;
 
@@ -2263,8 +2318,7 @@ int __devinit dwc3_gadget_init(struct dwc3 *dwc)
 		goto err1;
 	}
 
-	dwc->setup_buf = kzalloc(sizeof(*dwc->setup_buf) * 2,
-			GFP_KERNEL);
+	dwc->setup_buf = kzalloc(DWC3_EP0_BOUNCE_SIZE, GFP_KERNEL);
 	if (!dwc->setup_buf) {
 		dev_err(dwc->dev, "failed to allocate setup buffer\n");
 		ret = -ENOMEM;
@@ -2272,7 +2326,8 @@ int __devinit dwc3_gadget_init(struct dwc3 *dwc)
 	}
 
 	dwc->ep0_bounce = dma_alloc_coherent(dwc->dev,
-			512, &dwc->ep0_bounce_addr, GFP_KERNEL);
+			DWC3_EP0_BOUNCE_SIZE, &dwc->ep0_bounce_addr,
+			GFP_KERNEL);
 	if (!dwc->ep0_bounce) {
 		dev_err(dwc->dev, "failed to allocate ep0 bounce buffer\n");
 		ret = -ENOMEM;
@@ -2313,6 +2368,14 @@ int __devinit dwc3_gadget_init(struct dwc3 *dwc)
 		goto err5;
 	}
 
+	reg = dwc3_readl(dwc->regs, DWC3_DCFG);
+	reg |= DWC3_DCFG_LPM_CAP;
+	dwc3_writel(dwc->regs, DWC3_DCFG, reg);
+
+	reg = dwc3_readl(dwc->regs, DWC3_DCTL);
+	reg |= DWC3_DCTL_ACCEPTU1ENA | DWC3_DCTL_ACCEPTU2ENA;
+	dwc3_writel(dwc->regs, DWC3_DCTL, reg);
+
 	/* Enable all but Start and End of Frame IRQs */
 	reg = (DWC3_DEVTEN_VNDRDEVTSTRCVEDEN |
 			DWC3_DEVTEN_EVNTOVERFLOWEN |
@@ -2351,8 +2414,8 @@ err5:
 	dwc3_gadget_free_endpoints(dwc);
 
 err4:
-	dma_free_coherent(dwc->dev, 512, dwc->ep0_bounce,
-			dwc->ep0_bounce_addr);
+	dma_free_coherent(dwc->dev, DWC3_EP0_BOUNCE_SIZE,
+			dwc->ep0_bounce, dwc->ep0_bounce_addr);
 
 err3:
 	kfree(dwc->setup_buf);
@@ -2381,8 +2444,8 @@ void dwc3_gadget_exit(struct dwc3 *dwc)
 
 	dwc3_gadget_free_endpoints(dwc);
 
-	dma_free_coherent(dwc->dev, 512, dwc->ep0_bounce,
-			dwc->ep0_bounce_addr);
+	dma_free_coherent(dwc->dev, DWC3_EP0_BOUNCE_SIZE,
+			dwc->ep0_bounce, dwc->ep0_bounce_addr);
 
 	kfree(dwc->setup_buf);
 
diff --git a/drivers/usb/dwc3/gadget.h b/drivers/usb/dwc3/gadget.h
index a8600084348..95ef6a2f776 100644
--- a/drivers/usb/dwc3/gadget.h
+++ b/drivers/usb/dwc3/gadget.h
@@ -111,6 +111,7 @@ int dwc3_gadget_ep0_queue(struct usb_ep *ep, struct usb_request *request,
 int __dwc3_gadget_ep_set_halt(struct dwc3_ep *dep, int value);
 int dwc3_send_gadget_ep_cmd(struct dwc3 *dwc, unsigned ep,
 		unsigned cmd, struct dwc3_gadget_ep_cmd_params *params);
+int dwc3_send_gadget_generic_command(struct dwc3 *dwc, int cmd, u32 param);
 
 /**
  * dwc3_gadget_ep_get_transfer_index - Gets transfer index from HW
diff --git a/drivers/usb/dwc3/host.c b/drivers/usb/dwc3/host.c
index b108d18fd40..56a62342884 100644
--- a/drivers/usb/dwc3/host.c
+++ b/drivers/usb/dwc3/host.c
@@ -39,15 +39,6 @@
 
 #include "core.h"
 
-static struct resource generic_resources[] = {
-	{
-		.flags = IORESOURCE_IRQ,
-	},
-	{
-		.flags = IORESOURCE_MEM,
-	},
-};
-
 int dwc3_host_init(struct dwc3 *dwc)
 {
 	struct platform_device	*xhci;
@@ -68,14 +59,8 @@ int dwc3_host_init(struct dwc3 *dwc)
 
 	dwc->xhci = xhci;
 
-	/* setup resources */
-	generic_resources[0].start = dwc->irq;
-
-	generic_resources[1].start = dwc->res->start;
-	generic_resources[1].end = dwc->res->start + 0x7fff;
-
-	ret = platform_device_add_resources(xhci, generic_resources,
-			ARRAY_SIZE(generic_resources));
+	ret = platform_device_add_resources(xhci, dwc->xhci_resources,
+						DWC3_XHCI_RESOURCES_NUM);
 	if (ret) {
 		dev_err(dwc->dev, "couldn't add resources to xHCI device\n");
 		goto err1;
diff --git a/drivers/usb/dwc3/io.h b/drivers/usb/dwc3/io.h
index 071d561f3e6..a50f76b9d19 100644
--- a/drivers/usb/dwc3/io.h
+++ b/drivers/usb/dwc3/io.h
@@ -41,14 +41,26 @@
 
 #include <linux/io.h>
 
+#include "core.h"
+
 static inline u32 dwc3_readl(void __iomem *base, u32 offset)
 {
-	return readl(base + offset);
+	/*
+	 * We requested the mem region starting from the Globals address
+	 * space, see dwc3_probe in core.c.
+	 * However, the offsets are given starting from xHCI address space.
+	 */
+	return readl(base + (offset - DWC3_GLOBALS_REGS_START));
 }
 
 static inline void dwc3_writel(void __iomem *base, u32 offset, u32 value)
 {
-	writel(value, base + offset);
+	/*
+	 * We requested the mem region starting from the Globals address
+	 * space, see dwc3_probe in core.c.
+	 * However, the offsets are given starting from xHCI address space.
+	 */
+	writel(value, base + (offset - DWC3_GLOBALS_REGS_START));
 }
 
 #endif /* __DRIVERS_USB_DWC3_IO_H */
diff --git a/drivers/usb/gadget/Kconfig b/drivers/usb/gadget/Kconfig
index 2633f759511..bddc8fd9a7b 100644
--- a/drivers/usb/gadget/Kconfig
+++ b/drivers/usb/gadget/Kconfig
@@ -123,13 +123,7 @@ config USB_GADGET_STORAGE_NUM_BUFFERS
 #   - discrete ones (including all PCI-only controllers)
 #   - debug/dummy gadget+hcd is last.
 #
-choice
-	prompt "USB Peripheral Controller"
-	help
-	   A USB device uses a controller to talk to its host.
-	   Systems should have only one such upstream link.
-	   Many controller drivers are platform-specific; these
-	   often need board-specific hooks.
+menu "USB Peripheral Controller"
 
 #
 # Integrated controllers
@@ -147,6 +141,17 @@ config USB_AT91
 	   dynamically linked module called "at91_udc" and force all
 	   gadget drivers to also be dynamically linked.
 
+config USB_LPC32XX
+	tristate "LPC32XX USB Peripheral Controller"
+	depends on ARCH_LPC32XX
+	select USB_ISP1301
+	help
+	   This option selects the USB device controller in the LPC32xx SoC.
+
+	   Say "y" to link the driver statically, or "m" to build a
+	   dynamically linked module called "lpc32xx_udc" and force all
+	   gadget drivers to also be dynamically linked.
+
 config USB_ATMEL_USBA
 	tristate "Atmel USBA"
 	select USB_GADGET_DUALSPEED
@@ -161,7 +166,7 @@ config USB_FSL_USB2
 	select USB_GADGET_DUALSPEED
 	select USB_FSL_MPH_DR_OF if OF
 	help
-	   Some of Freescale PowerPC processors have a High Speed
+	   Some of Freescale PowerPC and i.MX processors have a High Speed
 	   Dual-Role(DR) USB controller, which supports device mode.
 
 	   The number of programmable endpoints is different through
@@ -373,18 +378,6 @@ config USB_FSL_QE
 	   Set CONFIG_USB_GADGET to "m" to build this driver as a
 	   dynamically linked module called "fsl_qe_udc".
 
-config USB_CI13XXX_PCI
-	tristate "MIPS USB CI13xxx PCI UDC"
-	depends on PCI
-	select USB_GADGET_DUALSPEED
-	help
-	  MIPS USB IP core family device controller
-	  Currently it only supports IP part number CI13412
-
-	  Say "y" to link the driver statically, or "m" to build a
-	  dynamically linked module called "ci13xxx_udc" and force all
-	  gadget drivers to also be dynamically linked.
-
 config USB_NET2272
 	tristate "PLX NET2272"
 	select USB_GADGET_DUALSPEED
@@ -438,22 +431,6 @@ config USB_GOKU
 	   dynamically linked module called "goku_udc" and to force all
 	   gadget drivers to also be dynamically linked.
 
-config USB_LANGWELL
-	tristate "Intel Langwell USB Device Controller"
-	depends on PCI
-	depends on !PHYS_ADDR_T_64BIT
-	select USB_GADGET_DUALSPEED
-	help
-	   Intel Langwell USB Device Controller is a High-Speed USB
-	   On-The-Go device controller.
-
-	   The number of programmable endpoints is different through
-	   controller revision.
-
-	   Say "y" to link the driver statically, or "m" to build a
-	   dynamically linked module called "langwell_udc" and force all
-	   gadget drivers to also be dynamically linked.
-
 config USB_EG20T
 	tristate "Intel EG20T PCH/LAPIS Semiconductor IOH(ML7213/ML7831) UDC"
 	depends on PCI
@@ -477,23 +454,6 @@ config USB_EG20T
 	  ML7213/ML7831 is companion chip for Intel Atom E6xx series.
 	  ML7213/ML7831 is completely compatible for Intel EG20T PCH.
 
-config USB_CI13XXX_MSM
-	tristate "MIPS USB CI13xxx for MSM"
-	depends on ARCH_MSM
-	select USB_GADGET_DUALSPEED
-	select USB_MSM_OTG
-	help
-	  MSM SoC has chipidea USB controller.  This driver uses
-	  ci13xxx_udc core.
-	  This driver depends on OTG driver for PHY initialization,
-	  clock management, powering up VBUS, and power management.
-	  This driver is not supported on boards like trout which
-	  has an external PHY.
-
-	  Say "y" to link the driver statically, or "m" to build a
-	  dynamically linked module called "ci13xxx_msm" and force all
-	  gadget drivers to also be dynamically linked.
-
 #
 # LAST -- dummy/emulated controller
 #
@@ -525,7 +485,7 @@ config USB_DUMMY_HCD
 # NOTE:  Please keep dummy_hcd LAST so that "real hardware" appears
 # first and will be selected by default.
 
-endchoice
+endmenu
 
 # Selected by UDC drivers that support high-speed operation.
 config USB_GADGET_DUALSPEED
@@ -798,6 +758,16 @@ config USB_MASS_STORAGE
 	  Say "y" to link the driver statically, or "m" to build
 	  a dynamically linked module called "g_mass_storage".
 
+config USB_GADGET_TARGET
+	tristate "USB Gadget Target Fabric Module"
+	depends on TARGET_CORE
+	help
+	  This fabric is an USB gadget. Two USB protocols are supported that is
+	  BBB or BOT (Bulk Only Transport) and UAS (USB Attached SCSI). BOT is
+	  advertised on alternative interface 0 (primary) and UAS is on
+	  alternative interface 1. Both protocols can work on USB2.0 and USB3.0.
+	  UAS utilizes the USB 3.0 feature called streams support.
+
 config USB_G_SERIAL
 	tristate "Serial Gadget (with CDC ACM and CDC OBEX support)"
 	help
diff --git a/drivers/usb/gadget/Makefile b/drivers/usb/gadget/Makefile
index b7f6eefc392..1811513f1c2 100644
--- a/drivers/usb/gadget/Makefile
+++ b/drivers/usb/gadget/Makefile
@@ -22,14 +22,12 @@ fsl_usb2_udc-$(CONFIG_ARCH_MXC)	+= fsl_mxc_udc.o
 obj-$(CONFIG_USB_M66592)	+= m66592-udc.o
 obj-$(CONFIG_USB_R8A66597)	+= r8a66597-udc.o
 obj-$(CONFIG_USB_FSL_QE)	+= fsl_qe_udc.o
-obj-$(CONFIG_USB_CI13XXX_PCI)	+= ci13xxx_pci.o
 obj-$(CONFIG_USB_S3C_HSOTG)	+= s3c-hsotg.o
 obj-$(CONFIG_USB_S3C_HSUDC)	+= s3c-hsudc.o
-obj-$(CONFIG_USB_LANGWELL)	+= langwell_udc.o
+obj-$(CONFIG_USB_LPC32XX)	+= lpc32xx_udc.o
 obj-$(CONFIG_USB_EG20T)		+= pch_udc.o
 obj-$(CONFIG_USB_MV_UDC)	+= mv_udc.o
 mv_udc-y			:= mv_udc_core.o
-obj-$(CONFIG_USB_CI13XXX_MSM)	+= ci13xxx_msm.o
 obj-$(CONFIG_USB_FUSB300)	+= fusb300_udc.o
 
 #
@@ -52,6 +50,7 @@ g_nokia-y			:= nokia.o
 g_webcam-y			:= webcam.o
 g_ncm-y				:= ncm.o
 g_acm_ms-y			:= acm_ms.o
+g_tcm_usb_gadget-y		:= tcm_usb_gadget.o
 
 obj-$(CONFIG_USB_ZERO)		+= g_zero.o
 obj-$(CONFIG_USB_AUDIO)		+= g_audio.o
@@ -71,3 +70,4 @@ obj-$(CONFIG_USB_G_NOKIA)	+= g_nokia.o
 obj-$(CONFIG_USB_G_WEBCAM)	+= g_webcam.o
 obj-$(CONFIG_USB_G_NCM)		+= g_ncm.o
 obj-$(CONFIG_USB_G_ACM_MS)	+= g_acm_ms.o
+obj-$(CONFIG_USB_GADGET_TARGET)	+= tcm_usb_gadget.o
diff --git a/drivers/usb/gadget/amd5536udc.c b/drivers/usb/gadget/amd5536udc.c
index 77779271f48..187d21181cd 100644
--- a/drivers/usb/gadget/amd5536udc.c
+++ b/drivers/usb/gadget/amd5536udc.c
@@ -333,7 +333,7 @@ udc_ep_enable(struct usb_ep *usbep, const struct usb_endpoint_descriptor *desc)
 		return -ESHUTDOWN;
 
 	spin_lock_irqsave(&dev->lock, iflags);
-	ep->desc = desc;
+	ep->ep.desc = desc;
 
 	ep->halted = 0;
 
@@ -442,7 +442,6 @@ static void ep_init(struct udc_regs __iomem *regs, struct udc_ep *ep)
 	u32		tmp;
 
 	VDBG(ep->dev, "ep-%d reset\n", ep->num);
-	ep->desc = NULL;
 	ep->ep.desc = NULL;
 	ep->ep.ops = &udc_ep_ops;
 	INIT_LIST_HEAD(&ep->queue);
@@ -489,7 +488,7 @@ static int udc_ep_disable(struct usb_ep *usbep)
 		return -EINVAL;
 
 	ep = container_of(usbep, struct udc_ep, ep);
-	if (usbep->name == ep0_string || !ep->desc)
+	if (usbep->name == ep0_string || !ep->ep.desc)
 		return -EINVAL;
 
 	DBG(ep->dev, "Disable ep-%d\n", ep->num);
@@ -1066,7 +1065,7 @@ udc_queue(struct usb_ep *usbep, struct usb_request *usbreq, gfp_t gfp)
 		return -EINVAL;
 
 	ep = container_of(usbep, struct udc_ep, ep);
-	if (!ep->desc && (ep->num != 0 && ep->num != UDC_EP0OUT_IX))
+	if (!ep->ep.desc && (ep->num != 0 && ep->num != UDC_EP0OUT_IX))
 		return -EINVAL;
 
 	VDBG(ep->dev, "udc_queue(): ep%d-in=%d\n", ep->num, ep->in);
@@ -1257,7 +1256,7 @@ static int udc_dequeue(struct usb_ep *usbep, struct usb_request *usbreq)
 	unsigned long		iflags;
 
 	ep = container_of(usbep, struct udc_ep, ep);
-	if (!usbep || !usbreq || (!ep->desc && (ep->num != 0
+	if (!usbep || !usbreq || (!ep->ep.desc && (ep->num != 0
 				&& ep->num != UDC_EP0OUT_IX)))
 		return -EINVAL;
 
@@ -1317,7 +1316,7 @@ udc_set_halt(struct usb_ep *usbep, int halt)
 	pr_debug("set_halt %s: halt=%d\n", usbep->name, halt);
 
 	ep = container_of(usbep, struct udc_ep, ep);
-	if (!ep->desc && (ep->num != 0 && ep->num != UDC_EP0OUT_IX))
+	if (!ep->ep.desc && (ep->num != 0 && ep->num != UDC_EP0OUT_IX))
 		return -EINVAL;
 	if (!ep->dev->driver || ep->dev->gadget.speed == USB_SPEED_UNKNOWN)
 		return -ESHUTDOWN;
@@ -1539,7 +1538,7 @@ static void udc_setup_endpoints(struct udc *dev)
 		 * disabling ep interrupts when ENUM interrupt occurs but ep is
 		 * not enabled by gadget driver
 		 */
-		if (!ep->desc)
+		if (!ep->ep.desc)
 			ep_init(dev->regs, ep);
 
 		if (use_dma) {
@@ -3402,19 +3401,7 @@ static struct pci_driver udc_pci_driver = {
 	.remove =	udc_pci_remove,
 };
 
-/* Inits driver */
-static int __init init(void)
-{
-	return pci_register_driver(&udc_pci_driver);
-}
-module_init(init);
-
-/* Cleans driver */
-static void __exit cleanup(void)
-{
-	pci_unregister_driver(&udc_pci_driver);
-}
-module_exit(cleanup);
+module_pci_driver(udc_pci_driver);
 
 MODULE_DESCRIPTION(UDC_MOD_DESCRIPTION);
 MODULE_AUTHOR("Thomas Dahlmann");
diff --git a/drivers/usb/gadget/amd5536udc.h b/drivers/usb/gadget/amd5536udc.h
index f87e29c6532..14af87d65ca 100644
--- a/drivers/usb/gadget/amd5536udc.h
+++ b/drivers/usb/gadget/amd5536udc.h
@@ -512,7 +512,6 @@ struct udc_ep {
 
 	/* queue for requests */
 	struct list_head		queue;
-	const struct usb_endpoint_descriptor	*desc;
 	unsigned			halted;
 	unsigned			cancel_transfer;
 	unsigned			num : 5,
diff --git a/drivers/usb/gadget/at91_udc.c b/drivers/usb/gadget/at91_udc.c
index 9d7bcd91007..1a4430f315c 100644
--- a/drivers/usb/gadget/at91_udc.c
+++ b/drivers/usb/gadget/at91_udc.c
@@ -212,7 +212,7 @@ static int proc_udc_show(struct seq_file *s, void *unused)
 	if (udc->enabled && udc->vbus) {
 		proc_ep_show(s, &udc->ep[0]);
 		list_for_each_entry (ep, &udc->gadget.ep_list, ep.ep_list) {
-			if (ep->desc)
+			if (ep->ep.desc)
 				proc_ep_show(s, ep);
 		}
 	}
@@ -475,7 +475,7 @@ static int at91_ep_enable(struct usb_ep *_ep,
 	unsigned long	flags;
 
 	if (!_ep || !ep
-			|| !desc || ep->desc
+			|| !desc || ep->ep.desc
 			|| _ep->name == ep0name
 			|| desc->bDescriptorType != USB_DT_ENDPOINT
 			|| (maxpacket = usb_endpoint_maxp(desc)) == 0
@@ -530,7 +530,7 @@ ok:
 	tmp |= AT91_UDP_EPEDS;
 	__raw_writel(tmp, ep->creg);
 
-	ep->desc = desc;
+	ep->ep.desc = desc;
 	ep->ep.maxpacket = maxpacket;
 
 	/*
@@ -558,7 +558,6 @@ static int at91_ep_disable (struct usb_ep * _ep)
 	nuke(ep, -ESHUTDOWN);
 
 	/* restore the endpoint's pristine config */
-	ep->desc = NULL;
 	ep->ep.desc = NULL;
 	ep->ep.maxpacket = ep->maxpacket;
 
@@ -618,7 +617,7 @@ static int at91_ep_queue(struct usb_ep *_ep,
 		return -EINVAL;
 	}
 
-	if (!_ep || (!ep->desc && ep->ep.name != ep0name)) {
+	if (!_ep || (!ep->ep.desc && ep->ep.name != ep0name)) {
 		DBG("invalid ep\n");
 		return -EINVAL;
 	}
@@ -833,7 +832,7 @@ static void udc_reinit(struct at91_udc *udc)
 
 		if (i != 0)
 			list_add_tail(&ep->ep.ep_list, &udc->gadget.ep_list);
-		ep->desc = NULL;
+		ep->ep.desc = NULL;
 		ep->stopped = 0;
 		ep->fifo_bank = 0;
 		ep->ep.maxpacket = ep->maxpacket;
@@ -978,18 +977,18 @@ static int at91_set_selfpowered(struct usb_gadget *gadget, int is_on)
 	return 0;
 }
 
-static int at91_start(struct usb_gadget_driver *driver,
-		int (*bind)(struct usb_gadget *));
-static int at91_stop(struct usb_gadget_driver *driver);
-
+static int at91_start(struct usb_gadget *gadget,
+		struct usb_gadget_driver *driver);
+static int at91_stop(struct usb_gadget *gadget,
+		struct usb_gadget_driver *driver);
 static const struct usb_gadget_ops at91_udc_ops = {
 	.get_frame		= at91_get_frame,
 	.wakeup			= at91_wakeup,
 	.set_selfpowered	= at91_set_selfpowered,
 	.vbus_session		= at91_vbus_session,
 	.pullup			= at91_pullup,
-	.start			= at91_start,
-	.stop			= at91_stop,
+	.udc_start		= at91_start,
+	.udc_stop		= at91_stop,
 
 	/*
 	 * VBUS-powered devices may also also want to support bigger
@@ -1172,7 +1171,7 @@ static void handle_setup(struct at91_udc *udc, struct at91_ep *ep, u32 csr)
 			| USB_REQ_GET_STATUS:
 		tmp = w_index & USB_ENDPOINT_NUMBER_MASK;
 		ep = &udc->ep[tmp];
-		if (tmp >= NUM_ENDPOINTS || (tmp && !ep->desc))
+		if (tmp >= NUM_ENDPOINTS || (tmp && !ep->ep.desc))
 			goto stall;
 
 		if (tmp) {
@@ -1197,7 +1196,7 @@ static void handle_setup(struct at91_udc *udc, struct at91_ep *ep, u32 csr)
 		ep = &udc->ep[tmp];
 		if (w_value != USB_ENDPOINT_HALT || tmp >= NUM_ENDPOINTS)
 			goto stall;
-		if (!ep->desc || ep->is_iso)
+		if (!ep->ep.desc || ep->is_iso)
 			goto stall;
 		if ((w_index & USB_DIR_IN)) {
 			if (!ep->is_in)
@@ -1218,7 +1217,7 @@ static void handle_setup(struct at91_udc *udc, struct at91_ep *ep, u32 csr)
 			goto stall;
 		if (tmp == 0)
 			goto succeed;
-		if (!ep->desc || ep->is_iso)
+		if (!ep->ep.desc || ep->is_iso)
 			goto stall;
 		if ((w_index & USB_DIR_IN)) {
 			if (!ep->is_in)
@@ -1627,66 +1626,34 @@ static void at91_vbus_timer(unsigned long data)
 		schedule_work(&udc->vbus_timer_work);
 }
 
-static int at91_start(struct usb_gadget_driver *driver,
-		int (*bind)(struct usb_gadget *))
+static int at91_start(struct usb_gadget *gadget,
+		struct usb_gadget_driver *driver)
 {
-	struct at91_udc	*udc = &controller;
-	int		retval;
-	unsigned long	flags;
-
-	if (!driver
-			|| driver->max_speed < USB_SPEED_FULL
-			|| !bind
-			|| !driver->setup) {
-		DBG("bad parameter.\n");
-		return -EINVAL;
-	}
-
-	if (udc->driver) {
-		DBG("UDC already has a gadget driver\n");
-		return -EBUSY;
-	}
+	struct at91_udc	*udc;
 
+	udc = container_of(gadget, struct at91_udc, gadget);
 	udc->driver = driver;
 	udc->gadget.dev.driver = &driver->driver;
 	dev_set_drvdata(&udc->gadget.dev, &driver->driver);
 	udc->enabled = 1;
 	udc->selfpowered = 1;
 
-	retval = bind(&udc->gadget);
-	if (retval) {
-		DBG("bind() returned %d\n", retval);
-		udc->driver = NULL;
-		udc->gadget.dev.driver = NULL;
-		dev_set_drvdata(&udc->gadget.dev, NULL);
-		udc->enabled = 0;
-		udc->selfpowered = 0;
-		return retval;
-	}
-
-	spin_lock_irqsave(&udc->lock, flags);
-	pullup(udc, 1);
-	spin_unlock_irqrestore(&udc->lock, flags);
-
 	DBG("bound to %s\n", driver->driver.name);
 	return 0;
 }
 
-static int at91_stop(struct usb_gadget_driver *driver)
+static int at91_stop(struct usb_gadget *gadget,
+		struct usb_gadget_driver *driver)
 {
-	struct at91_udc *udc = &controller;
+	struct at91_udc *udc;
 	unsigned long	flags;
 
-	if (!driver || driver != udc->driver || !driver->unbind)
-		return -EINVAL;
-
+	udc = container_of(gadget, struct at91_udc, gadget);
 	spin_lock_irqsave(&udc->lock, flags);
 	udc->enabled = 0;
 	at91_udp_write(udc, AT91_UDP_IDR, ~0);
-	pullup(udc, 0);
 	spin_unlock_irqrestore(&udc->lock, flags);
 
-	driver->unbind(&udc->gadget);
 	udc->gadget.dev.driver = NULL;
 	dev_set_drvdata(&udc->gadget.dev, NULL);
 	udc->driver = NULL;
diff --git a/drivers/usb/gadget/at91_udc.h b/drivers/usb/gadget/at91_udc.h
index 3c0315b86ac..e647d1c2ada 100644
--- a/drivers/usb/gadget/at91_udc.h
+++ b/drivers/usb/gadget/at91_udc.h
@@ -105,9 +105,6 @@ struct at91_ep {
 	unsigned			is_in:1;
 	unsigned			is_iso:1;
 	unsigned			fifo_bank:1;
-
-	const struct usb_endpoint_descriptor
-					*desc;
 };
 
 /*
diff --git a/drivers/usb/gadget/atmel_usba_udc.c b/drivers/usb/gadget/atmel_usba_udc.c
index 9f98508966d..e23bf7984aa 100644
--- a/drivers/usb/gadget/atmel_usba_udc.c
+++ b/drivers/usb/gadget/atmel_usba_udc.c
@@ -599,13 +599,13 @@ usba_ep_enable(struct usb_ep *_ep, const struct usb_endpoint_descriptor *desc)
 
 	spin_lock_irqsave(&ep->udc->lock, flags);
 
-	if (ep->desc) {
+	if (ep->ep.desc) {
 		spin_unlock_irqrestore(&ep->udc->lock, flags);
 		DBG(DBG_ERR, "ep%d already enabled\n", ep->index);
 		return -EBUSY;
 	}
 
-	ep->desc = desc;
+	ep->ep.desc = desc;
 	ep->ep.maxpacket = maxpacket;
 
 	usba_ep_writel(ep, CFG, ept_cfg);
@@ -647,7 +647,7 @@ static int usba_ep_disable(struct usb_ep *_ep)
 
 	spin_lock_irqsave(&udc->lock, flags);
 
-	if (!ep->desc) {
+	if (!ep->ep.desc) {
 		spin_unlock_irqrestore(&udc->lock, flags);
 		/* REVISIT because this driver disables endpoints in
 		 * reset_all_endpoints() before calling disconnect(),
@@ -658,7 +658,6 @@ static int usba_ep_disable(struct usb_ep *_ep)
 					ep->ep.name);
 		return -EINVAL;
 	}
-	ep->desc = NULL;
 	ep->ep.desc = NULL;
 
 	list_splice_init(&ep->queue, &req_list);
@@ -752,7 +751,7 @@ static int queue_dma(struct usba_udc *udc, struct usba_ep *ep,
 	 */
 	ret = -ESHUTDOWN;
 	spin_lock_irqsave(&udc->lock, flags);
-	if (ep->desc) {
+	if (ep->ep.desc) {
 		if (list_empty(&ep->queue))
 			submit_request(ep, req);
 
@@ -776,7 +775,8 @@ usba_ep_queue(struct usb_ep *_ep, struct usb_request *_req, gfp_t gfp_flags)
 	DBG(DBG_GADGET | DBG_QUEUE | DBG_REQ, "%s: queue req %p, len %u\n",
 			ep->ep.name, req, _req->length);
 
-	if (!udc->driver || udc->gadget.speed == USB_SPEED_UNKNOWN || !ep->desc)
+	if (!udc->driver || udc->gadget.speed == USB_SPEED_UNKNOWN ||
+	    !ep->ep.desc)
 		return -ESHUTDOWN;
 
 	req->submitted = 0;
@@ -792,7 +792,7 @@ usba_ep_queue(struct usb_ep *_ep, struct usb_request *_req, gfp_t gfp_flags)
 	/* May have received a reset since last time we checked */
 	ret = -ESHUTDOWN;
 	spin_lock_irqsave(&udc->lock, flags);
-	if (ep->desc) {
+	if (ep->ep.desc) {
 		list_add_tail(&req->queue, &ep->queue);
 
 		if ((!ep_is_control(ep) && ep->is_in) ||
@@ -905,7 +905,7 @@ static int usba_ep_set_halt(struct usb_ep *_ep, int value)
 	DBG(DBG_GADGET, "endpoint %s: %s HALT\n", ep->ep.name,
 			value ? "set" : "clear");
 
-	if (!ep->desc) {
+	if (!ep->ep.desc) {
 		DBG(DBG_ERR, "Attempted to halt uninitialized ep %s\n",
 				ep->ep.name);
 		return -ENODEV;
@@ -1008,16 +1008,16 @@ usba_udc_set_selfpowered(struct usb_gadget *gadget, int is_selfpowered)
 	return 0;
 }
 
-static int atmel_usba_start(struct usb_gadget_driver *driver,
-		int (*bind)(struct usb_gadget *));
-static int atmel_usba_stop(struct usb_gadget_driver *driver);
-
+static int atmel_usba_start(struct usb_gadget *gadget,
+		struct usb_gadget_driver *driver);
+static int atmel_usba_stop(struct usb_gadget *gadget,
+		struct usb_gadget_driver *driver);
 static const struct usb_gadget_ops usba_udc_ops = {
 	.get_frame		= usba_udc_get_frame,
 	.wakeup			= usba_udc_wakeup,
 	.set_selfpowered	= usba_udc_set_selfpowered,
-	.start			= atmel_usba_start,
-	.stop			= atmel_usba_stop,
+	.udc_start		= atmel_usba_start,
+	.udc_stop		= atmel_usba_stop,
 };
 
 static struct usb_endpoint_descriptor usba_ep0_desc = {
@@ -1071,7 +1071,7 @@ static void reset_all_endpoints(struct usba_udc *udc)
 	 * FIXME remove this code ... and retest thoroughly.
 	 */
 	list_for_each_entry(ep, &udc->gadget.ep_list, ep.ep_list) {
-		if (ep->desc) {
+		if (ep->ep.desc) {
 			spin_unlock(&udc->lock);
 			usba_ep_disable(&ep->ep);
 			spin_lock(&udc->lock);
@@ -1089,9 +1089,9 @@ static struct usba_ep *get_ep_by_addr(struct usba_udc *udc, u16 wIndex)
 	list_for_each_entry (ep, &udc->gadget.ep_list, ep.ep_list) {
 		u8 bEndpointAddress;
 
-		if (!ep->desc)
+		if (!ep->ep.desc)
 			continue;
-		bEndpointAddress = ep->desc->bEndpointAddress;
+		bEndpointAddress = ep->ep.desc->bEndpointAddress;
 		if ((wIndex ^ bEndpointAddress) & USB_DIR_IN)
 			continue;
 		if ((bEndpointAddress & USB_ENDPOINT_NUMBER_MASK)
@@ -1727,7 +1727,7 @@ static irqreturn_t usba_udc_irq(int irq, void *devid)
 		    usb_speed_string(udc->gadget.speed));
 
 		ep0 = &usba_ep[0];
-		ep0->desc = &usba_ep0_desc;
+		ep0->ep.desc = &usba_ep0_desc;
 		ep0->state = WAIT_FOR_SETUP;
 		usba_ep_writel(ep0, CFG,
 				(USBA_BF(EPT_SIZE, EP0_EPT_SIZE)
@@ -1795,21 +1795,13 @@ out:
 	return IRQ_HANDLED;
 }
 
-static int atmel_usba_start(struct usb_gadget_driver *driver,
-		int (*bind)(struct usb_gadget *))
+static int atmel_usba_start(struct usb_gadget *gadget,
+		struct usb_gadget_driver *driver)
 {
-	struct usba_udc *udc = &the_udc;
+	struct usba_udc *udc = container_of(gadget, struct usba_udc, gadget);
 	unsigned long flags;
-	int ret;
-
-	if (!udc->pdev)
-		return -ENODEV;
 
 	spin_lock_irqsave(&udc->lock, flags);
-	if (udc->driver) {
-		spin_unlock_irqrestore(&udc->lock, flags);
-		return -EBUSY;
-	}
 
 	udc->devstatus = 1 << USB_DEVICE_SELF_POWERED;
 	udc->driver = driver;
@@ -1819,13 +1811,6 @@ static int atmel_usba_start(struct usb_gadget_driver *driver,
 	clk_enable(udc->pclk);
 	clk_enable(udc->hclk);
 
-	ret = bind(&udc->gadget);
-	if (ret) {
-		DBG(DBG_ERR, "Could not bind to driver %s: error %d\n",
-			driver->driver.name, ret);
-		goto err_driver_bind;
-	}
-
 	DBG(DBG_GADGET, "registered driver `%s'\n", driver->driver.name);
 
 	udc->vbus_prev = 0;
@@ -1842,23 +1827,14 @@ static int atmel_usba_start(struct usb_gadget_driver *driver,
 	spin_unlock_irqrestore(&udc->lock, flags);
 
 	return 0;
-
-err_driver_bind:
-	udc->driver = NULL;
-	udc->gadget.dev.driver = NULL;
-	return ret;
 }
 
-static int atmel_usba_stop(struct usb_gadget_driver *driver)
+static int atmel_usba_stop(struct usb_gadget *gadget,
+		struct usb_gadget_driver *driver)
 {
-	struct usba_udc *udc = &the_udc;
+	struct usba_udc *udc = container_of(gadget, struct usba_udc, gadget);
 	unsigned long flags;
 
-	if (!udc->pdev)
-		return -ENODEV;
-	if (driver != udc->driver || !driver->unbind)
-		return -EINVAL;
-
 	if (gpio_is_valid(udc->vbus_pin))
 		disable_irq(gpio_to_irq(udc->vbus_pin));
 
@@ -1871,10 +1847,6 @@ static int atmel_usba_stop(struct usb_gadget_driver *driver)
 	toggle_bias(0);
 	usba_writel(udc, CTRL, USBA_DISABLE_MASK);
 
-	if (udc->driver->disconnect)
-		udc->driver->disconnect(&udc->gadget);
-
-	driver->unbind(&udc->gadget);
 	udc->gadget.dev.driver = NULL;
 	udc->driver = NULL;
 
diff --git a/drivers/usb/gadget/atmel_usba_udc.h b/drivers/usb/gadget/atmel_usba_udc.h
index 88a2e07a11a..9791259cbda 100644
--- a/drivers/usb/gadget/atmel_usba_udc.h
+++ b/drivers/usb/gadget/atmel_usba_udc.h
@@ -280,7 +280,6 @@ struct usba_ep {
 	struct usba_udc				*udc;
 
 	struct list_head			queue;
-	const struct usb_endpoint_descriptor	*desc;
 
 	u16					fifo_size;
 	u8					nr_banks;
diff --git a/drivers/usb/gadget/ci13xxx_udc.c b/drivers/usb/gadget/ci13xxx_udc.c
deleted file mode 100644
index 243ef1adf96..00000000000
--- a/drivers/usb/gadget/ci13xxx_udc.c
+++ /dev/null
@@ -1,2996 +0,0 @@
-/*
- * ci13xxx_udc.c - MIPS USB IP core family device controller
- *
- * Copyright (C) 2008 Chipidea - MIPS Technologies, Inc. All rights reserved.
- *
- * Author: David Lopo
- *
- * 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.
- */
-
-/*
- * Description: MIPS USB IP core family device controller
- *              Currently it only supports IP part number CI13412
- *
- * This driver is composed of several blocks:
- * - HW:     hardware interface
- * - DBG:    debug facilities (optional)
- * - UTIL:   utilities
- * - ISR:    interrupts handling
- * - ENDPT:  endpoint operations (Gadget API)
- * - GADGET: gadget operations (Gadget API)
- * - BUS:    bus glue code, bus abstraction layer
- *
- * Compile Options
- * - CONFIG_USB_GADGET_DEBUG_FILES: enable debug facilities
- * - STALL_IN:  non-empty bulk-in pipes cannot be halted
- *              if defined mass storage compliance succeeds but with warnings
- *              => case 4: Hi >  Dn
- *              => case 5: Hi >  Di
- *              => case 8: Hi <> Do
- *              if undefined usbtest 13 fails
- * - TRACE:     enable function tracing (depends on DEBUG)
- *
- * Main Features
- * - Chapter 9 & Mass Storage Compliance with Gadget File Storage
- * - Chapter 9 Compliance with Gadget Zero (STALL_IN undefined)
- * - Normal & LPM support
- *
- * USBTEST Report
- * - OK: 0-12, 13 (STALL_IN defined) & 14
- * - Not Supported: 15 & 16 (ISO)
- *
- * TODO List
- * - OTG
- * - Isochronous & Interrupt Traffic
- * - Handle requests which spawns into several TDs
- * - GET_STATUS(device) - always reports 0
- * - Gadget API (majority of optional features)
- * - Suspend & Remote Wakeup
- */
-#include <linux/delay.h>
-#include <linux/device.h>
-#include <linux/dmapool.h>
-#include <linux/dma-mapping.h>
-#include <linux/init.h>
-#include <linux/interrupt.h>
-#include <linux/io.h>
-#include <linux/irq.h>
-#include <linux/kernel.h>
-#include <linux/slab.h>
-#include <linux/pm_runtime.h>
-#include <linux/usb/ch9.h>
-#include <linux/usb/gadget.h>
-#include <linux/usb/otg.h>
-
-#include "ci13xxx_udc.h"
-
-
-/******************************************************************************
- * DEFINE
- *****************************************************************************/
-
-#define DMA_ADDR_INVALID	(~(dma_addr_t)0)
-
-/* ctrl register bank access */
-static DEFINE_SPINLOCK(udc_lock);
-
-/* control endpoint description */
-static const struct usb_endpoint_descriptor
-ctrl_endpt_out_desc = {
-	.bLength         = USB_DT_ENDPOINT_SIZE,
-	.bDescriptorType = USB_DT_ENDPOINT,
-
-	.bEndpointAddress = USB_DIR_OUT,
-	.bmAttributes    = USB_ENDPOINT_XFER_CONTROL,
-	.wMaxPacketSize  = cpu_to_le16(CTRL_PAYLOAD_MAX),
-};
-
-static const struct usb_endpoint_descriptor
-ctrl_endpt_in_desc = {
-	.bLength         = USB_DT_ENDPOINT_SIZE,
-	.bDescriptorType = USB_DT_ENDPOINT,
-
-	.bEndpointAddress = USB_DIR_IN,
-	.bmAttributes    = USB_ENDPOINT_XFER_CONTROL,
-	.wMaxPacketSize  = cpu_to_le16(CTRL_PAYLOAD_MAX),
-};
-
-/* UDC descriptor */
-static struct ci13xxx *_udc;
-
-/* Interrupt statistics */
-#define ISR_MASK   0x1F
-static struct {
-	u32 test;
-	u32 ui;
-	u32 uei;
-	u32 pci;
-	u32 uri;
-	u32 sli;
-	u32 none;
-	struct {
-		u32 cnt;
-		u32 buf[ISR_MASK+1];
-		u32 idx;
-	} hndl;
-} isr_statistics;
-
-/**
- * ffs_nr: find first (least significant) bit set
- * @x: the word to search
- *
- * This function returns bit number (instead of position)
- */
-static int ffs_nr(u32 x)
-{
-	int n = ffs(x);
-
-	return n ? n-1 : 32;
-}
-
-/******************************************************************************
- * HW block
- *****************************************************************************/
-/* register bank descriptor */
-static struct {
-	unsigned      lpm;    /* is LPM? */
-	void __iomem *abs;    /* bus map offset */
-	void __iomem *cap;    /* bus map offset + CAP offset + CAP data */
-	size_t        size;   /* bank size */
-} hw_bank;
-
-/* MSM specific */
-#define ABS_AHBBURST        (0x0090UL)
-#define ABS_AHBMODE         (0x0098UL)
-/* UDC register map */
-#define ABS_CAPLENGTH       (0x100UL)
-#define ABS_HCCPARAMS       (0x108UL)
-#define ABS_DCCPARAMS       (0x124UL)
-#define ABS_TESTMODE        (hw_bank.lpm ? 0x0FCUL : 0x138UL)
-/* offset to CAPLENTGH (addr + data) */
-#define CAP_USBCMD          (0x000UL)
-#define CAP_USBSTS          (0x004UL)
-#define CAP_USBINTR         (0x008UL)
-#define CAP_DEVICEADDR      (0x014UL)
-#define CAP_ENDPTLISTADDR   (0x018UL)
-#define CAP_PORTSC          (0x044UL)
-#define CAP_DEVLC           (0x084UL)
-#define CAP_USBMODE         (hw_bank.lpm ? 0x0C8UL : 0x068UL)
-#define CAP_ENDPTSETUPSTAT  (hw_bank.lpm ? 0x0D8UL : 0x06CUL)
-#define CAP_ENDPTPRIME      (hw_bank.lpm ? 0x0DCUL : 0x070UL)
-#define CAP_ENDPTFLUSH      (hw_bank.lpm ? 0x0E0UL : 0x074UL)
-#define CAP_ENDPTSTAT       (hw_bank.lpm ? 0x0E4UL : 0x078UL)
-#define CAP_ENDPTCOMPLETE   (hw_bank.lpm ? 0x0E8UL : 0x07CUL)
-#define CAP_ENDPTCTRL       (hw_bank.lpm ? 0x0ECUL : 0x080UL)
-#define CAP_LAST            (hw_bank.lpm ? 0x12CUL : 0x0C0UL)
-
-/* maximum number of enpoints: valid only after hw_device_reset() */
-static unsigned hw_ep_max;
-
-/**
- * hw_ep_bit: calculates the bit number
- * @num: endpoint number
- * @dir: endpoint direction
- *
- * This function returns bit number
- */
-static inline int hw_ep_bit(int num, int dir)
-{
-	return num + (dir ? 16 : 0);
-}
-
-static int ep_to_bit(int n)
-{
-	int fill = 16 - hw_ep_max / 2;
-
-	if (n >= hw_ep_max / 2)
-		n += fill;
-
-	return n;
-}
-
-/**
- * hw_aread: reads from register bitfield
- * @addr: address relative to bus map
- * @mask: bitfield mask
- *
- * This function returns register bitfield data
- */
-static u32 hw_aread(u32 addr, u32 mask)
-{
-	return ioread32(addr + hw_bank.abs) & mask;
-}
-
-/**
- * hw_awrite: writes to register bitfield
- * @addr: address relative to bus map
- * @mask: bitfield mask
- * @data: new data
- */
-static void hw_awrite(u32 addr, u32 mask, u32 data)
-{
-	iowrite32(hw_aread(addr, ~mask) | (data & mask),
-		  addr + hw_bank.abs);
-}
-
-/**
- * hw_cread: reads from register bitfield
- * @addr: address relative to CAP offset plus content
- * @mask: bitfield mask
- *
- * This function returns register bitfield data
- */
-static u32 hw_cread(u32 addr, u32 mask)
-{
-	return ioread32(addr + hw_bank.cap) & mask;
-}
-
-/**
- * hw_cwrite: writes to register bitfield
- * @addr: address relative to CAP offset plus content
- * @mask: bitfield mask
- * @data: new data
- */
-static void hw_cwrite(u32 addr, u32 mask, u32 data)
-{
-	iowrite32(hw_cread(addr, ~mask) | (data & mask),
-		  addr + hw_bank.cap);
-}
-
-/**
- * hw_ctest_and_clear: tests & clears register bitfield
- * @addr: address relative to CAP offset plus content
- * @mask: bitfield mask
- *
- * This function returns register bitfield data
- */
-static u32 hw_ctest_and_clear(u32 addr, u32 mask)
-{
-	u32 reg = hw_cread(addr, mask);
-
-	iowrite32(reg, addr + hw_bank.cap);
-	return reg;
-}
-
-/**
- * hw_ctest_and_write: tests & writes register bitfield
- * @addr: address relative to CAP offset plus content
- * @mask: bitfield mask
- * @data: new data
- *
- * This function returns register bitfield data
- */
-static u32 hw_ctest_and_write(u32 addr, u32 mask, u32 data)
-{
-	u32 reg = hw_cread(addr, ~0);
-
-	iowrite32((reg & ~mask) | (data & mask), addr + hw_bank.cap);
-	return (reg & mask) >> ffs_nr(mask);
-}
-
-static int hw_device_init(void __iomem *base)
-{
-	u32 reg;
-
-	/* bank is a module variable */
-	hw_bank.abs = base;
-
-	hw_bank.cap = hw_bank.abs;
-	hw_bank.cap += ABS_CAPLENGTH;
-	hw_bank.cap += ioread8(hw_bank.cap);
-
-	reg = hw_aread(ABS_HCCPARAMS, HCCPARAMS_LEN) >> ffs_nr(HCCPARAMS_LEN);
-	hw_bank.lpm  = reg;
-	hw_bank.size = hw_bank.cap - hw_bank.abs;
-	hw_bank.size += CAP_LAST;
-	hw_bank.size /= sizeof(u32);
-
-	reg = hw_aread(ABS_DCCPARAMS, DCCPARAMS_DEN) >> ffs_nr(DCCPARAMS_DEN);
-	hw_ep_max = reg * 2;   /* cache hw ENDPT_MAX */
-
-	if (hw_ep_max == 0 || hw_ep_max > ENDPT_MAX)
-		return -ENODEV;
-
-	/* setup lock mode ? */
-
-	/* ENDPTSETUPSTAT is '0' by default */
-
-	/* HCSPARAMS.bf.ppc SHOULD BE zero for device */
-
-	return 0;
-}
-/**
- * hw_device_reset: resets chip (execute without interruption)
- * @base: register base address
- *
- * This function returns an error code
- */
-static int hw_device_reset(struct ci13xxx *udc)
-{
-	/* should flush & stop before reset */
-	hw_cwrite(CAP_ENDPTFLUSH, ~0, ~0);
-	hw_cwrite(CAP_USBCMD, USBCMD_RS, 0);
-
-	hw_cwrite(CAP_USBCMD, USBCMD_RST, USBCMD_RST);
-	while (hw_cread(CAP_USBCMD, USBCMD_RST))
-		udelay(10);             /* not RTOS friendly */
-
-
-	if (udc->udc_driver->notify_event)
-		udc->udc_driver->notify_event(udc,
-			CI13XXX_CONTROLLER_RESET_EVENT);
-
-	if (udc->udc_driver->flags & CI13XXX_DISABLE_STREAMING)
-		hw_cwrite(CAP_USBMODE, USBMODE_SDIS, USBMODE_SDIS);
-
-	/* USBMODE should be configured step by step */
-	hw_cwrite(CAP_USBMODE, USBMODE_CM, USBMODE_CM_IDLE);
-	hw_cwrite(CAP_USBMODE, USBMODE_CM, USBMODE_CM_DEVICE);
-	hw_cwrite(CAP_USBMODE, USBMODE_SLOM, USBMODE_SLOM);  /* HW >= 2.3 */
-
-	if (hw_cread(CAP_USBMODE, USBMODE_CM) != USBMODE_CM_DEVICE) {
-		pr_err("cannot enter in device mode");
-		pr_err("lpm = %i", hw_bank.lpm);
-		return -ENODEV;
-	}
-
-	return 0;
-}
-
-/**
- * hw_device_state: enables/disables interrupts & starts/stops device (execute
- *                  without interruption)
- * @dma: 0 => disable, !0 => enable and set dma engine
- *
- * This function returns an error code
- */
-static int hw_device_state(u32 dma)
-{
-	if (dma) {
-		hw_cwrite(CAP_ENDPTLISTADDR, ~0, dma);
-		/* interrupt, error, port change, reset, sleep/suspend */
-		hw_cwrite(CAP_USBINTR, ~0,
-			     USBi_UI|USBi_UEI|USBi_PCI|USBi_URI|USBi_SLI);
-		hw_cwrite(CAP_USBCMD, USBCMD_RS, USBCMD_RS);
-	} else {
-		hw_cwrite(CAP_USBCMD, USBCMD_RS, 0);
-		hw_cwrite(CAP_USBINTR, ~0, 0);
-	}
-	return 0;
-}
-
-/**
- * hw_ep_flush: flush endpoint fifo (execute without interruption)
- * @num: endpoint number
- * @dir: endpoint direction
- *
- * This function returns an error code
- */
-static int hw_ep_flush(int num, int dir)
-{
-	int n = hw_ep_bit(num, dir);
-
-	do {
-		/* flush any pending transfer */
-		hw_cwrite(CAP_ENDPTFLUSH, BIT(n), BIT(n));
-		while (hw_cread(CAP_ENDPTFLUSH, BIT(n)))
-			cpu_relax();
-	} while (hw_cread(CAP_ENDPTSTAT, BIT(n)));
-
-	return 0;
-}
-
-/**
- * hw_ep_disable: disables endpoint (execute without interruption)
- * @num: endpoint number
- * @dir: endpoint direction
- *
- * This function returns an error code
- */
-static int hw_ep_disable(int num, int dir)
-{
-	hw_ep_flush(num, dir);
-	hw_cwrite(CAP_ENDPTCTRL + num * sizeof(u32),
-		  dir ? ENDPTCTRL_TXE : ENDPTCTRL_RXE, 0);
-	return 0;
-}
-
-/**
- * hw_ep_enable: enables endpoint (execute without interruption)
- * @num:  endpoint number
- * @dir:  endpoint direction
- * @type: endpoint type
- *
- * This function returns an error code
- */
-static int hw_ep_enable(int num, int dir, int type)
-{
-	u32 mask, data;
-
-	if (dir) {
-		mask  = ENDPTCTRL_TXT;  /* type    */
-		data  = type << ffs_nr(mask);
-
-		mask |= ENDPTCTRL_TXS;  /* unstall */
-		mask |= ENDPTCTRL_TXR;  /* reset data toggle */
-		data |= ENDPTCTRL_TXR;
-		mask |= ENDPTCTRL_TXE;  /* enable  */
-		data |= ENDPTCTRL_TXE;
-	} else {
-		mask  = ENDPTCTRL_RXT;  /* type    */
-		data  = type << ffs_nr(mask);
-
-		mask |= ENDPTCTRL_RXS;  /* unstall */
-		mask |= ENDPTCTRL_RXR;  /* reset data toggle */
-		data |= ENDPTCTRL_RXR;
-		mask |= ENDPTCTRL_RXE;  /* enable  */
-		data |= ENDPTCTRL_RXE;
-	}
-	hw_cwrite(CAP_ENDPTCTRL + num * sizeof(u32), mask, data);
-	return 0;
-}
-
-/**
- * hw_ep_get_halt: return endpoint halt status
- * @num: endpoint number
- * @dir: endpoint direction
- *
- * This function returns 1 if endpoint halted
- */
-static int hw_ep_get_halt(int num, int dir)
-{
-	u32 mask = dir ? ENDPTCTRL_TXS : ENDPTCTRL_RXS;
-
-	return hw_cread(CAP_ENDPTCTRL + num * sizeof(u32), mask) ? 1 : 0;
-}
-
-/**
- * hw_test_and_clear_setup_status: test & clear setup status (execute without
- *                                 interruption)
- * @n: endpoint number
- *
- * This function returns setup status
- */
-static int hw_test_and_clear_setup_status(int n)
-{
-	n = ep_to_bit(n);
-	return hw_ctest_and_clear(CAP_ENDPTSETUPSTAT, BIT(n));
-}
-
-/**
- * hw_ep_prime: primes endpoint (execute without interruption)
- * @num:     endpoint number
- * @dir:     endpoint direction
- * @is_ctrl: true if control endpoint
- *
- * This function returns an error code
- */
-static int hw_ep_prime(int num, int dir, int is_ctrl)
-{
-	int n = hw_ep_bit(num, dir);
-
-	if (is_ctrl && dir == RX && hw_cread(CAP_ENDPTSETUPSTAT, BIT(num)))
-		return -EAGAIN;
-
-	hw_cwrite(CAP_ENDPTPRIME, BIT(n), BIT(n));
-
-	while (hw_cread(CAP_ENDPTPRIME, BIT(n)))
-		cpu_relax();
-	if (is_ctrl && dir == RX  && hw_cread(CAP_ENDPTSETUPSTAT, BIT(num)))
-		return -EAGAIN;
-
-	/* status shoult be tested according with manual but it doesn't work */
-	return 0;
-}
-
-/**
- * hw_ep_set_halt: configures ep halt & resets data toggle after clear (execute
- *                 without interruption)
- * @num:   endpoint number
- * @dir:   endpoint direction
- * @value: true => stall, false => unstall
- *
- * This function returns an error code
- */
-static int hw_ep_set_halt(int num, int dir, int value)
-{
-	if (value != 0 && value != 1)
-		return -EINVAL;
-
-	do {
-		u32 addr = CAP_ENDPTCTRL + num * sizeof(u32);
-		u32 mask_xs = dir ? ENDPTCTRL_TXS : ENDPTCTRL_RXS;
-		u32 mask_xr = dir ? ENDPTCTRL_TXR : ENDPTCTRL_RXR;
-
-		/* data toggle - reserved for EP0 but it's in ESS */
-		hw_cwrite(addr, mask_xs|mask_xr, value ? mask_xs : mask_xr);
-
-	} while (value != hw_ep_get_halt(num, dir));
-
-	return 0;
-}
-
-/**
- * hw_intr_clear: disables interrupt & clears interrupt status (execute without
- *                interruption)
- * @n: interrupt bit
- *
- * This function returns an error code
- */
-static int hw_intr_clear(int n)
-{
-	if (n >= REG_BITS)
-		return -EINVAL;
-
-	hw_cwrite(CAP_USBINTR, BIT(n), 0);
-	hw_cwrite(CAP_USBSTS,  BIT(n), BIT(n));
-	return 0;
-}
-
-/**
- * hw_intr_force: enables interrupt & forces interrupt status (execute without
- *                interruption)
- * @n: interrupt bit
- *
- * This function returns an error code
- */
-static int hw_intr_force(int n)
-{
-	if (n >= REG_BITS)
-		return -EINVAL;
-
-	hw_awrite(ABS_TESTMODE, TESTMODE_FORCE, TESTMODE_FORCE);
-	hw_cwrite(CAP_USBINTR,  BIT(n), BIT(n));
-	hw_cwrite(CAP_USBSTS,   BIT(n), BIT(n));
-	hw_awrite(ABS_TESTMODE, TESTMODE_FORCE, 0);
-	return 0;
-}
-
-/**
- * hw_is_port_high_speed: test if port is high speed
- *
- * This function returns true if high speed port
- */
-static int hw_port_is_high_speed(void)
-{
-	return hw_bank.lpm ? hw_cread(CAP_DEVLC, DEVLC_PSPD) :
-		hw_cread(CAP_PORTSC, PORTSC_HSP);
-}
-
-/**
- * hw_port_test_get: reads port test mode value
- *
- * This function returns port test mode value
- */
-static u8 hw_port_test_get(void)
-{
-	return hw_cread(CAP_PORTSC, PORTSC_PTC) >> ffs_nr(PORTSC_PTC);
-}
-
-/**
- * hw_port_test_set: writes port test mode (execute without interruption)
- * @mode: new value
- *
- * This function returns an error code
- */
-static int hw_port_test_set(u8 mode)
-{
-	const u8 TEST_MODE_MAX = 7;
-
-	if (mode > TEST_MODE_MAX)
-		return -EINVAL;
-
-	hw_cwrite(CAP_PORTSC, PORTSC_PTC, mode << ffs_nr(PORTSC_PTC));
-	return 0;
-}
-
-/**
- * hw_read_intr_enable: returns interrupt enable register
- *
- * This function returns register data
- */
-static u32 hw_read_intr_enable(void)
-{
-	return hw_cread(CAP_USBINTR, ~0);
-}
-
-/**
- * hw_read_intr_status: returns interrupt status register
- *
- * This function returns register data
- */
-static u32 hw_read_intr_status(void)
-{
-	return hw_cread(CAP_USBSTS, ~0);
-}
-
-/**
- * hw_register_read: reads all device registers (execute without interruption)
- * @buf:  destination buffer
- * @size: buffer size
- *
- * This function returns number of registers read
- */
-static size_t hw_register_read(u32 *buf, size_t size)
-{
-	unsigned i;
-
-	if (size > hw_bank.size)
-		size = hw_bank.size;
-
-	for (i = 0; i < size; i++)
-		buf[i] = hw_aread(i * sizeof(u32), ~0);
-
-	return size;
-}
-
-/**
- * hw_register_write: writes to register
- * @addr: register address
- * @data: register value
- *
- * This function returns an error code
- */
-static int hw_register_write(u16 addr, u32 data)
-{
-	/* align */
-	addr /= sizeof(u32);
-
-	if (addr >= hw_bank.size)
-		return -EINVAL;
-
-	/* align */
-	addr *= sizeof(u32);
-
-	hw_awrite(addr, ~0, data);
-	return 0;
-}
-
-/**
- * hw_test_and_clear_complete: test & clear complete status (execute without
- *                             interruption)
- * @n: endpoint number
- *
- * This function returns complete status
- */
-static int hw_test_and_clear_complete(int n)
-{
-	n = ep_to_bit(n);
-	return hw_ctest_and_clear(CAP_ENDPTCOMPLETE, BIT(n));
-}
-
-/**
- * hw_test_and_clear_intr_active: test & clear active interrupts (execute
- *                                without interruption)
- *
- * This function returns active interrutps
- */
-static u32 hw_test_and_clear_intr_active(void)
-{
-	u32 reg = hw_read_intr_status() & hw_read_intr_enable();
-
-	hw_cwrite(CAP_USBSTS, ~0, reg);
-	return reg;
-}
-
-/**
- * hw_test_and_clear_setup_guard: test & clear setup guard (execute without
- *                                interruption)
- *
- * This function returns guard value
- */
-static int hw_test_and_clear_setup_guard(void)
-{
-	return hw_ctest_and_write(CAP_USBCMD, USBCMD_SUTW, 0);
-}
-
-/**
- * hw_test_and_set_setup_guard: test & set setup guard (execute without
- *                              interruption)
- *
- * This function returns guard value
- */
-static int hw_test_and_set_setup_guard(void)
-{
-	return hw_ctest_and_write(CAP_USBCMD, USBCMD_SUTW, USBCMD_SUTW);
-}
-
-/**
- * hw_usb_set_address: configures USB address (execute without interruption)
- * @value: new USB address
- *
- * This function returns an error code
- */
-static int hw_usb_set_address(u8 value)
-{
-	/* advance */
-	hw_cwrite(CAP_DEVICEADDR, DEVICEADDR_USBADR | DEVICEADDR_USBADRA,
-		  value << ffs_nr(DEVICEADDR_USBADR) | DEVICEADDR_USBADRA);
-	return 0;
-}
-
-/**
- * hw_usb_reset: restart device after a bus reset (execute without
- *               interruption)
- *
- * This function returns an error code
- */
-static int hw_usb_reset(void)
-{
-	hw_usb_set_address(0);
-
-	/* ESS flushes only at end?!? */
-	hw_cwrite(CAP_ENDPTFLUSH,    ~0, ~0);   /* flush all EPs */
-
-	/* clear setup token semaphores */
-	hw_cwrite(CAP_ENDPTSETUPSTAT, 0,  0);   /* writes its content */
-
-	/* clear complete status */
-	hw_cwrite(CAP_ENDPTCOMPLETE,  0,  0);   /* writes its content */
-
-	/* wait until all bits cleared */
-	while (hw_cread(CAP_ENDPTPRIME, ~0))
-		udelay(10);             /* not RTOS friendly */
-
-	/* reset all endpoints ? */
-
-	/* reset internal status and wait for further instructions
-	   no need to verify the port reset status (ESS does it) */
-
-	return 0;
-}
-
-/******************************************************************************
- * DBG block
- *****************************************************************************/
-/**
- * show_device: prints information about device capabilities and status
- *
- * Check "device.h" for details
- */
-static ssize_t show_device(struct device *dev, struct device_attribute *attr,
-			   char *buf)
-{
-	struct ci13xxx *udc = container_of(dev, struct ci13xxx, gadget.dev);
-	struct usb_gadget *gadget = &udc->gadget;
-	int n = 0;
-
-	dbg_trace("[%s] %p\n", __func__, buf);
-	if (attr == NULL || buf == NULL) {
-		dev_err(dev, "[%s] EINVAL\n", __func__);
-		return 0;
-	}
-
-	n += scnprintf(buf + n, PAGE_SIZE - n, "speed             = %d\n",
-		       gadget->speed);
-	n += scnprintf(buf + n, PAGE_SIZE - n, "max_speed         = %d\n",
-		       gadget->max_speed);
-	/* TODO: Scheduled for removal in 3.8. */
-	n += scnprintf(buf + n, PAGE_SIZE - n, "is_dualspeed      = %d\n",
-		       gadget_is_dualspeed(gadget));
-	n += scnprintf(buf + n, PAGE_SIZE - n, "is_otg            = %d\n",
-		       gadget->is_otg);
-	n += scnprintf(buf + n, PAGE_SIZE - n, "is_a_peripheral   = %d\n",
-		       gadget->is_a_peripheral);
-	n += scnprintf(buf + n, PAGE_SIZE - n, "b_hnp_enable      = %d\n",
-		       gadget->b_hnp_enable);
-	n += scnprintf(buf + n, PAGE_SIZE - n, "a_hnp_support     = %d\n",
-		       gadget->a_hnp_support);
-	n += scnprintf(buf + n, PAGE_SIZE - n, "a_alt_hnp_support = %d\n",
-		       gadget->a_alt_hnp_support);
-	n += scnprintf(buf + n, PAGE_SIZE - n, "name              = %s\n",
-		       (gadget->name ? gadget->name : ""));
-
-	return n;
-}
-static DEVICE_ATTR(device, S_IRUSR, show_device, NULL);
-
-/**
- * show_driver: prints information about attached gadget (if any)
- *
- * Check "device.h" for details
- */
-static ssize_t show_driver(struct device *dev, struct device_attribute *attr,
-			   char *buf)
-{
-	struct ci13xxx *udc = container_of(dev, struct ci13xxx, gadget.dev);
-	struct usb_gadget_driver *driver = udc->driver;
-	int n = 0;
-
-	dbg_trace("[%s] %p\n", __func__, buf);
-	if (attr == NULL || buf == NULL) {
-		dev_err(dev, "[%s] EINVAL\n", __func__);
-		return 0;
-	}
-
-	if (driver == NULL)
-		return scnprintf(buf, PAGE_SIZE,
-				 "There is no gadget attached!\n");
-
-	n += scnprintf(buf + n, PAGE_SIZE - n, "function  = %s\n",
-		       (driver->function ? driver->function : ""));
-	n += scnprintf(buf + n, PAGE_SIZE - n, "max speed = %d\n",
-		       driver->max_speed);
-
-	return n;
-}
-static DEVICE_ATTR(driver, S_IRUSR, show_driver, NULL);
-
-/* Maximum event message length */
-#define DBG_DATA_MSG   64UL
-
-/* Maximum event messages */
-#define DBG_DATA_MAX   128UL
-
-/* Event buffer descriptor */
-static struct {
-	char     (buf[DBG_DATA_MAX])[DBG_DATA_MSG];   /* buffer */
-	unsigned idx;   /* index */
-	unsigned tty;   /* print to console? */
-	rwlock_t lck;   /* lock */
-} dbg_data = {
-	.idx = 0,
-	.tty = 0,
-	.lck = __RW_LOCK_UNLOCKED(lck)
-};
-
-/**
- * dbg_dec: decrements debug event index
- * @idx: buffer index
- */
-static void dbg_dec(unsigned *idx)
-{
-	*idx = (*idx - 1) & (DBG_DATA_MAX-1);
-}
-
-/**
- * dbg_inc: increments debug event index
- * @idx: buffer index
- */
-static void dbg_inc(unsigned *idx)
-{
-	*idx = (*idx + 1) & (DBG_DATA_MAX-1);
-}
-
-/**
- * dbg_print:  prints the common part of the event
- * @addr:   endpoint address
- * @name:   event name
- * @status: status
- * @extra:  extra information
- */
-static void dbg_print(u8 addr, const char *name, int status, const char *extra)
-{
-	struct timeval tval;
-	unsigned int stamp;
-	unsigned long flags;
-
-	write_lock_irqsave(&dbg_data.lck, flags);
-
-	do_gettimeofday(&tval);
-	stamp = tval.tv_sec & 0xFFFF;	/* 2^32 = 4294967296. Limit to 4096s */
-	stamp = stamp * 1000000 + tval.tv_usec;
-
-	scnprintf(dbg_data.buf[dbg_data.idx], DBG_DATA_MSG,
-		  "%04X\t? %02X %-7.7s %4i ?\t%s\n",
-		  stamp, addr, name, status, extra);
-
-	dbg_inc(&dbg_data.idx);
-
-	write_unlock_irqrestore(&dbg_data.lck, flags);
-
-	if (dbg_data.tty != 0)
-		pr_notice("%04X\t? %02X %-7.7s %4i ?\t%s\n",
-			  stamp, addr, name, status, extra);
-}
-
-/**
- * dbg_done: prints a DONE event
- * @addr:   endpoint address
- * @td:     transfer descriptor
- * @status: status
- */
-static void dbg_done(u8 addr, const u32 token, int status)
-{
-	char msg[DBG_DATA_MSG];
-
-	scnprintf(msg, sizeof(msg), "%d %02X",
-		  (int)(token & TD_TOTAL_BYTES) >> ffs_nr(TD_TOTAL_BYTES),
-		  (int)(token & TD_STATUS)      >> ffs_nr(TD_STATUS));
-	dbg_print(addr, "DONE", status, msg);
-}
-
-/**
- * dbg_event: prints a generic event
- * @addr:   endpoint address
- * @name:   event name
- * @status: status
- */
-static void dbg_event(u8 addr, const char *name, int status)
-{
-	if (name != NULL)
-		dbg_print(addr, name, status, "");
-}
-
-/*
- * dbg_queue: prints a QUEUE event
- * @addr:   endpoint address
- * @req:    USB request
- * @status: status
- */
-static void dbg_queue(u8 addr, const struct usb_request *req, int status)
-{
-	char msg[DBG_DATA_MSG];
-
-	if (req != NULL) {
-		scnprintf(msg, sizeof(msg),
-			  "%d %d", !req->no_interrupt, req->length);
-		dbg_print(addr, "QUEUE", status, msg);
-	}
-}
-
-/**
- * dbg_setup: prints a SETUP event
- * @addr: endpoint address
- * @req:  setup request
- */
-static void dbg_setup(u8 addr, const struct usb_ctrlrequest *req)
-{
-	char msg[DBG_DATA_MSG];
-
-	if (req != NULL) {
-		scnprintf(msg, sizeof(msg),
-			  "%02X %02X %04X %04X %d", req->bRequestType,
-			  req->bRequest, le16_to_cpu(req->wValue),
-			  le16_to_cpu(req->wIndex), le16_to_cpu(req->wLength));
-		dbg_print(addr, "SETUP", 0, msg);
-	}
-}
-
-/**
- * show_events: displays the event buffer
- *
- * Check "device.h" for details
- */
-static ssize_t show_events(struct device *dev, struct device_attribute *attr,
-			   char *buf)
-{
-	unsigned long flags;
-	unsigned i, j, n = 0;
-
-	dbg_trace("[%s] %p\n", __func__, buf);
-	if (attr == NULL || buf == NULL) {
-		dev_err(dev, "[%s] EINVAL\n", __func__);
-		return 0;
-	}
-
-	read_lock_irqsave(&dbg_data.lck, flags);
-
-	i = dbg_data.idx;
-	for (dbg_dec(&i); i != dbg_data.idx; dbg_dec(&i)) {
-		n += strlen(dbg_data.buf[i]);
-		if (n >= PAGE_SIZE) {
-			n -= strlen(dbg_data.buf[i]);
-			break;
-		}
-	}
-	for (j = 0, dbg_inc(&i); j < n; dbg_inc(&i))
-		j += scnprintf(buf + j, PAGE_SIZE - j,
-			       "%s", dbg_data.buf[i]);
-
-	read_unlock_irqrestore(&dbg_data.lck, flags);
-
-	return n;
-}
-
-/**
- * store_events: configure if events are going to be also printed to console
- *
- * Check "device.h" for details
- */
-static ssize_t store_events(struct device *dev, struct device_attribute *attr,
-			    const char *buf, size_t count)
-{
-	unsigned tty;
-
-	dbg_trace("[%s] %p, %d\n", __func__, buf, count);
-	if (attr == NULL || buf == NULL) {
-		dev_err(dev, "[%s] EINVAL\n", __func__);
-		goto done;
-	}
-
-	if (sscanf(buf, "%u", &tty) != 1 || tty > 1) {
-		dev_err(dev, "<1|0>: enable|disable console log\n");
-		goto done;
-	}
-
-	dbg_data.tty = tty;
-	dev_info(dev, "tty = %u", dbg_data.tty);
-
- done:
-	return count;
-}
-static DEVICE_ATTR(events, S_IRUSR | S_IWUSR, show_events, store_events);
-
-/**
- * show_inters: interrupt status, enable status and historic
- *
- * Check "device.h" for details
- */
-static ssize_t show_inters(struct device *dev, struct device_attribute *attr,
-			   char *buf)
-{
-	struct ci13xxx *udc = container_of(dev, struct ci13xxx, gadget.dev);
-	unsigned long flags;
-	u32 intr;
-	unsigned i, j, n = 0;
-
-	dbg_trace("[%s] %p\n", __func__, buf);
-	if (attr == NULL || buf == NULL) {
-		dev_err(dev, "[%s] EINVAL\n", __func__);
-		return 0;
-	}
-
-	spin_lock_irqsave(udc->lock, flags);
-
-	n += scnprintf(buf + n, PAGE_SIZE - n,
-		       "status = %08x\n", hw_read_intr_status());
-	n += scnprintf(buf + n, PAGE_SIZE - n,
-		       "enable = %08x\n", hw_read_intr_enable());
-
-	n += scnprintf(buf + n, PAGE_SIZE - n, "*test = %d\n",
-		       isr_statistics.test);
-	n += scnprintf(buf + n, PAGE_SIZE - n, "? ui  = %d\n",
-		       isr_statistics.ui);
-	n += scnprintf(buf + n, PAGE_SIZE - n, "? uei = %d\n",
-		       isr_statistics.uei);
-	n += scnprintf(buf + n, PAGE_SIZE - n, "? pci = %d\n",
-		       isr_statistics.pci);
-	n += scnprintf(buf + n, PAGE_SIZE - n, "? uri = %d\n",
-		       isr_statistics.uri);
-	n += scnprintf(buf + n, PAGE_SIZE - n, "? sli = %d\n",
-		       isr_statistics.sli);
-	n += scnprintf(buf + n, PAGE_SIZE - n, "*none = %d\n",
-		       isr_statistics.none);
-	n += scnprintf(buf + n, PAGE_SIZE - n, "*hndl = %d\n",
-		       isr_statistics.hndl.cnt);
-
-	for (i = isr_statistics.hndl.idx, j = 0; j <= ISR_MASK; j++, i++) {
-		i   &= ISR_MASK;
-		intr = isr_statistics.hndl.buf[i];
-
-		if (USBi_UI  & intr)
-			n += scnprintf(buf + n, PAGE_SIZE - n, "ui  ");
-		intr &= ~USBi_UI;
-		if (USBi_UEI & intr)
-			n += scnprintf(buf + n, PAGE_SIZE - n, "uei ");
-		intr &= ~USBi_UEI;
-		if (USBi_PCI & intr)
-			n += scnprintf(buf + n, PAGE_SIZE - n, "pci ");
-		intr &= ~USBi_PCI;
-		if (USBi_URI & intr)
-			n += scnprintf(buf + n, PAGE_SIZE - n, "uri ");
-		intr &= ~USBi_URI;
-		if (USBi_SLI & intr)
-			n += scnprintf(buf + n, PAGE_SIZE - n, "sli ");
-		intr &= ~USBi_SLI;
-		if (intr)
-			n += scnprintf(buf + n, PAGE_SIZE - n, "??? ");
-		if (isr_statistics.hndl.buf[i])
-			n += scnprintf(buf + n, PAGE_SIZE - n, "\n");
-	}
-
-	spin_unlock_irqrestore(udc->lock, flags);
-
-	return n;
-}
-
-/**
- * store_inters: enable & force or disable an individual interrutps
- *                   (to be used for test purposes only)
- *
- * Check "device.h" for details
- */
-static ssize_t store_inters(struct device *dev, struct device_attribute *attr,
-			    const char *buf, size_t count)
-{
-	struct ci13xxx *udc = container_of(dev, struct ci13xxx, gadget.dev);
-	unsigned long flags;
-	unsigned en, bit;
-
-	dbg_trace("[%s] %p, %d\n", __func__, buf, count);
-	if (attr == NULL || buf == NULL) {
-		dev_err(dev, "[%s] EINVAL\n", __func__);
-		goto done;
-	}
-
-	if (sscanf(buf, "%u %u", &en, &bit) != 2 || en > 1) {
-		dev_err(dev, "<1|0> <bit>: enable|disable interrupt");
-		goto done;
-	}
-
-	spin_lock_irqsave(udc->lock, flags);
-	if (en) {
-		if (hw_intr_force(bit))
-			dev_err(dev, "invalid bit number\n");
-		else
-			isr_statistics.test++;
-	} else {
-		if (hw_intr_clear(bit))
-			dev_err(dev, "invalid bit number\n");
-	}
-	spin_unlock_irqrestore(udc->lock, flags);
-
- done:
-	return count;
-}
-static DEVICE_ATTR(inters, S_IRUSR | S_IWUSR, show_inters, store_inters);
-
-/**
- * show_port_test: reads port test mode
- *
- * Check "device.h" for details
- */
-static ssize_t show_port_test(struct device *dev,
-			      struct device_attribute *attr, char *buf)
-{
-	struct ci13xxx *udc = container_of(dev, struct ci13xxx, gadget.dev);
-	unsigned long flags;
-	unsigned mode;
-
-	dbg_trace("[%s] %p\n", __func__, buf);
-	if (attr == NULL || buf == NULL) {
-		dev_err(dev, "[%s] EINVAL\n", __func__);
-		return 0;
-	}
-
-	spin_lock_irqsave(udc->lock, flags);
-	mode = hw_port_test_get();
-	spin_unlock_irqrestore(udc->lock, flags);
-
-	return scnprintf(buf, PAGE_SIZE, "mode = %u\n", mode);
-}
-
-/**
- * store_port_test: writes port test mode
- *
- * Check "device.h" for details
- */
-static ssize_t store_port_test(struct device *dev,
-			       struct device_attribute *attr,
-			       const char *buf, size_t count)
-{
-	struct ci13xxx *udc = container_of(dev, struct ci13xxx, gadget.dev);
-	unsigned long flags;
-	unsigned mode;
-
-	dbg_trace("[%s] %p, %d\n", __func__, buf, count);
-	if (attr == NULL || buf == NULL) {
-		dev_err(dev, "[%s] EINVAL\n", __func__);
-		goto done;
-	}
-
-	if (sscanf(buf, "%u", &mode) != 1) {
-		dev_err(dev, "<mode>: set port test mode");
-		goto done;
-	}
-
-	spin_lock_irqsave(udc->lock, flags);
-	if (hw_port_test_set(mode))
-		dev_err(dev, "invalid mode\n");
-	spin_unlock_irqrestore(udc->lock, flags);
-
- done:
-	return count;
-}
-static DEVICE_ATTR(port_test, S_IRUSR | S_IWUSR,
-		   show_port_test, store_port_test);
-
-/**
- * show_qheads: DMA contents of all queue heads
- *
- * Check "device.h" for details
- */
-static ssize_t show_qheads(struct device *dev, struct device_attribute *attr,
-			   char *buf)
-{
-	struct ci13xxx *udc = container_of(dev, struct ci13xxx, gadget.dev);
-	unsigned long flags;
-	unsigned i, j, n = 0;
-
-	dbg_trace("[%s] %p\n", __func__, buf);
-	if (attr == NULL || buf == NULL) {
-		dev_err(dev, "[%s] EINVAL\n", __func__);
-		return 0;
-	}
-
-	spin_lock_irqsave(udc->lock, flags);
-	for (i = 0; i < hw_ep_max/2; i++) {
-		struct ci13xxx_ep *mEpRx = &udc->ci13xxx_ep[i];
-		struct ci13xxx_ep *mEpTx = &udc->ci13xxx_ep[i + hw_ep_max/2];
-		n += scnprintf(buf + n, PAGE_SIZE - n,
-			       "EP=%02i: RX=%08X TX=%08X\n",
-			       i, (u32)mEpRx->qh.dma, (u32)mEpTx->qh.dma);
-		for (j = 0; j < (sizeof(struct ci13xxx_qh)/sizeof(u32)); j++) {
-			n += scnprintf(buf + n, PAGE_SIZE - n,
-				       " %04X:    %08X    %08X\n", j,
-				       *((u32 *)mEpRx->qh.ptr + j),
-				       *((u32 *)mEpTx->qh.ptr + j));
-		}
-	}
-	spin_unlock_irqrestore(udc->lock, flags);
-
-	return n;
-}
-static DEVICE_ATTR(qheads, S_IRUSR, show_qheads, NULL);
-
-/**
- * show_registers: dumps all registers
- *
- * Check "device.h" for details
- */
-#define DUMP_ENTRIES	512
-static ssize_t show_registers(struct device *dev,
-			      struct device_attribute *attr, char *buf)
-{
-	struct ci13xxx *udc = container_of(dev, struct ci13xxx, gadget.dev);
-	unsigned long flags;
-	u32 *dump;
-	unsigned i, k, n = 0;
-
-	dbg_trace("[%s] %p\n", __func__, buf);
-	if (attr == NULL || buf == NULL) {
-		dev_err(dev, "[%s] EINVAL\n", __func__);
-		return 0;
-	}
-
-	dump = kmalloc(sizeof(u32) * DUMP_ENTRIES, GFP_KERNEL);
-	if (!dump) {
-		dev_err(dev, "%s: out of memory\n", __func__);
-		return 0;
-	}
-
-	spin_lock_irqsave(udc->lock, flags);
-	k = hw_register_read(dump, DUMP_ENTRIES);
-	spin_unlock_irqrestore(udc->lock, flags);
-
-	for (i = 0; i < k; i++) {
-		n += scnprintf(buf + n, PAGE_SIZE - n,
-			       "reg[0x%04X] = 0x%08X\n",
-			       i * (unsigned)sizeof(u32), dump[i]);
-	}
-	kfree(dump);
-
-	return n;
-}
-
-/**
- * store_registers: writes value to register address
- *
- * Check "device.h" for details
- */
-static ssize_t store_registers(struct device *dev,
-			       struct device_attribute *attr,
-			       const char *buf, size_t count)
-{
-	struct ci13xxx *udc = container_of(dev, struct ci13xxx, gadget.dev);
-	unsigned long addr, data, flags;
-
-	dbg_trace("[%s] %p, %d\n", __func__, buf, count);
-	if (attr == NULL || buf == NULL) {
-		dev_err(dev, "[%s] EINVAL\n", __func__);
-		goto done;
-	}
-
-	if (sscanf(buf, "%li %li", &addr, &data) != 2) {
-		dev_err(dev, "<addr> <data>: write data to register address");
-		goto done;
-	}
-
-	spin_lock_irqsave(udc->lock, flags);
-	if (hw_register_write(addr, data))
-		dev_err(dev, "invalid address range\n");
-	spin_unlock_irqrestore(udc->lock, flags);
-
- done:
-	return count;
-}
-static DEVICE_ATTR(registers, S_IRUSR | S_IWUSR,
-		   show_registers, store_registers);
-
-/**
- * show_requests: DMA contents of all requests currently queued (all endpts)
- *
- * Check "device.h" for details
- */
-static ssize_t show_requests(struct device *dev, struct device_attribute *attr,
-			     char *buf)
-{
-	struct ci13xxx *udc = container_of(dev, struct ci13xxx, gadget.dev);
-	unsigned long flags;
-	struct list_head   *ptr = NULL;
-	struct ci13xxx_req *req = NULL;
-	unsigned i, j, n = 0, qSize = sizeof(struct ci13xxx_td)/sizeof(u32);
-
-	dbg_trace("[%s] %p\n", __func__, buf);
-	if (attr == NULL || buf == NULL) {
-		dev_err(dev, "[%s] EINVAL\n", __func__);
-		return 0;
-	}
-
-	spin_lock_irqsave(udc->lock, flags);
-	for (i = 0; i < hw_ep_max; i++)
-		list_for_each(ptr, &udc->ci13xxx_ep[i].qh.queue)
-		{
-			req = list_entry(ptr, struct ci13xxx_req, queue);
-
-			n += scnprintf(buf + n, PAGE_SIZE - n,
-					"EP=%02i: TD=%08X %s\n",
-					i % hw_ep_max/2, (u32)req->dma,
-					((i < hw_ep_max/2) ? "RX" : "TX"));
-
-			for (j = 0; j < qSize; j++)
-				n += scnprintf(buf + n, PAGE_SIZE - n,
-						" %04X:    %08X\n", j,
-						*((u32 *)req->ptr + j));
-		}
-	spin_unlock_irqrestore(udc->lock, flags);
-
-	return n;
-}
-static DEVICE_ATTR(requests, S_IRUSR, show_requests, NULL);
-
-/**
- * dbg_create_files: initializes the attribute interface
- * @dev: device
- *
- * This function returns an error code
- */
-__maybe_unused static int dbg_create_files(struct device *dev)
-{
-	int retval = 0;
-
-	if (dev == NULL)
-		return -EINVAL;
-	retval = device_create_file(dev, &dev_attr_device);
-	if (retval)
-		goto done;
-	retval = device_create_file(dev, &dev_attr_driver);
-	if (retval)
-		goto rm_device;
-	retval = device_create_file(dev, &dev_attr_events);
-	if (retval)
-		goto rm_driver;
-	retval = device_create_file(dev, &dev_attr_inters);
-	if (retval)
-		goto rm_events;
-	retval = device_create_file(dev, &dev_attr_port_test);
-	if (retval)
-		goto rm_inters;
-	retval = device_create_file(dev, &dev_attr_qheads);
-	if (retval)
-		goto rm_port_test;
-	retval = device_create_file(dev, &dev_attr_registers);
-	if (retval)
-		goto rm_qheads;
-	retval = device_create_file(dev, &dev_attr_requests);
-	if (retval)
-		goto rm_registers;
-	return 0;
-
- rm_registers:
-	device_remove_file(dev, &dev_attr_registers);
- rm_qheads:
-	device_remove_file(dev, &dev_attr_qheads);
- rm_port_test:
-	device_remove_file(dev, &dev_attr_port_test);
- rm_inters:
-	device_remove_file(dev, &dev_attr_inters);
- rm_events:
-	device_remove_file(dev, &dev_attr_events);
- rm_driver:
-	device_remove_file(dev, &dev_attr_driver);
- rm_device:
-	device_remove_file(dev, &dev_attr_device);
- done:
-	return retval;
-}
-
-/**
- * dbg_remove_files: destroys the attribute interface
- * @dev: device
- *
- * This function returns an error code
- */
-__maybe_unused static int dbg_remove_files(struct device *dev)
-{
-	if (dev == NULL)
-		return -EINVAL;
-	device_remove_file(dev, &dev_attr_requests);
-	device_remove_file(dev, &dev_attr_registers);
-	device_remove_file(dev, &dev_attr_qheads);
-	device_remove_file(dev, &dev_attr_port_test);
-	device_remove_file(dev, &dev_attr_inters);
-	device_remove_file(dev, &dev_attr_events);
-	device_remove_file(dev, &dev_attr_driver);
-	device_remove_file(dev, &dev_attr_device);
-	return 0;
-}
-
-/******************************************************************************
- * UTIL block
- *****************************************************************************/
-/**
- * _usb_addr: calculates endpoint address from direction & number
- * @ep:  endpoint
- */
-static inline u8 _usb_addr(struct ci13xxx_ep *ep)
-{
-	return ((ep->dir == TX) ? USB_ENDPOINT_DIR_MASK : 0) | ep->num;
-}
-
-/**
- * _hardware_queue: configures a request at hardware level
- * @gadget: gadget
- * @mEp:    endpoint
- *
- * This function returns an error code
- */
-static int _hardware_enqueue(struct ci13xxx_ep *mEp, struct ci13xxx_req *mReq)
-{
-	unsigned i;
-	int ret = 0;
-	unsigned length = mReq->req.length;
-
-	trace("%p, %p", mEp, mReq);
-
-	/* don't queue twice */
-	if (mReq->req.status == -EALREADY)
-		return -EALREADY;
-
-	mReq->req.status = -EALREADY;
-	if (length && mReq->req.dma == DMA_ADDR_INVALID) {
-		mReq->req.dma = \
-			dma_map_single(mEp->device, mReq->req.buf,
-				       length, mEp->dir ? DMA_TO_DEVICE :
-				       DMA_FROM_DEVICE);
-		if (mReq->req.dma == 0)
-			return -ENOMEM;
-
-		mReq->map = 1;
-	}
-
-	if (mReq->req.zero && length && (length % mEp->ep.maxpacket == 0)) {
-		mReq->zptr = dma_pool_alloc(mEp->td_pool, GFP_ATOMIC,
-					   &mReq->zdma);
-		if (mReq->zptr == NULL) {
-			if (mReq->map) {
-				dma_unmap_single(mEp->device, mReq->req.dma,
-					length, mEp->dir ? DMA_TO_DEVICE :
-					DMA_FROM_DEVICE);
-				mReq->req.dma = DMA_ADDR_INVALID;
-				mReq->map     = 0;
-			}
-			return -ENOMEM;
-		}
-		memset(mReq->zptr, 0, sizeof(*mReq->zptr));
-		mReq->zptr->next    = TD_TERMINATE;
-		mReq->zptr->token   = TD_STATUS_ACTIVE;
-		if (!mReq->req.no_interrupt)
-			mReq->zptr->token   |= TD_IOC;
-	}
-	/*
-	 * TD configuration
-	 * TODO - handle requests which spawns into several TDs
-	 */
-	memset(mReq->ptr, 0, sizeof(*mReq->ptr));
-	mReq->ptr->token    = length << ffs_nr(TD_TOTAL_BYTES);
-	mReq->ptr->token   &= TD_TOTAL_BYTES;
-	mReq->ptr->token   |= TD_STATUS_ACTIVE;
-	if (mReq->zptr) {
-		mReq->ptr->next    = mReq->zdma;
-	} else {
-		mReq->ptr->next    = TD_TERMINATE;
-		if (!mReq->req.no_interrupt)
-			mReq->ptr->token  |= TD_IOC;
-	}
-	mReq->ptr->page[0]  = mReq->req.dma;
-	for (i = 1; i < 5; i++)
-		mReq->ptr->page[i] =
-			(mReq->req.dma + i * CI13XXX_PAGE_SIZE) & ~TD_RESERVED_MASK;
-
-	if (!list_empty(&mEp->qh.queue)) {
-		struct ci13xxx_req *mReqPrev;
-		int n = hw_ep_bit(mEp->num, mEp->dir);
-		int tmp_stat;
-
-		mReqPrev = list_entry(mEp->qh.queue.prev,
-				struct ci13xxx_req, queue);
-		if (mReqPrev->zptr)
-			mReqPrev->zptr->next = mReq->dma & TD_ADDR_MASK;
-		else
-			mReqPrev->ptr->next = mReq->dma & TD_ADDR_MASK;
-		wmb();
-		if (hw_cread(CAP_ENDPTPRIME, BIT(n)))
-			goto done;
-		do {
-			hw_cwrite(CAP_USBCMD, USBCMD_ATDTW, USBCMD_ATDTW);
-			tmp_stat = hw_cread(CAP_ENDPTSTAT, BIT(n));
-		} while (!hw_cread(CAP_USBCMD, USBCMD_ATDTW));
-		hw_cwrite(CAP_USBCMD, USBCMD_ATDTW, 0);
-		if (tmp_stat)
-			goto done;
-	}
-
-	/*  QH configuration */
-	mEp->qh.ptr->td.next   = mReq->dma;    /* TERMINATE = 0 */
-	mEp->qh.ptr->td.token &= ~TD_STATUS;   /* clear status */
-	mEp->qh.ptr->cap |=  QH_ZLT;
-
-	wmb();   /* synchronize before ep prime */
-
-	ret = hw_ep_prime(mEp->num, mEp->dir,
-			   mEp->type == USB_ENDPOINT_XFER_CONTROL);
-done:
-	return ret;
-}
-
-/**
- * _hardware_dequeue: handles a request at hardware level
- * @gadget: gadget
- * @mEp:    endpoint
- *
- * This function returns an error code
- */
-static int _hardware_dequeue(struct ci13xxx_ep *mEp, struct ci13xxx_req *mReq)
-{
-	trace("%p, %p", mEp, mReq);
-
-	if (mReq->req.status != -EALREADY)
-		return -EINVAL;
-
-	if ((TD_STATUS_ACTIVE & mReq->ptr->token) != 0)
-		return -EBUSY;
-
-	if (mReq->zptr) {
-		if ((TD_STATUS_ACTIVE & mReq->zptr->token) != 0)
-			return -EBUSY;
-		dma_pool_free(mEp->td_pool, mReq->zptr, mReq->zdma);
-		mReq->zptr = NULL;
-	}
-
-	mReq->req.status = 0;
-
-	if (mReq->map) {
-		dma_unmap_single(mEp->device, mReq->req.dma, mReq->req.length,
-				 mEp->dir ? DMA_TO_DEVICE : DMA_FROM_DEVICE);
-		mReq->req.dma = DMA_ADDR_INVALID;
-		mReq->map     = 0;
-	}
-
-	mReq->req.status = mReq->ptr->token & TD_STATUS;
-	if ((TD_STATUS_HALTED & mReq->req.status) != 0)
-		mReq->req.status = -1;
-	else if ((TD_STATUS_DT_ERR & mReq->req.status) != 0)
-		mReq->req.status = -1;
-	else if ((TD_STATUS_TR_ERR & mReq->req.status) != 0)
-		mReq->req.status = -1;
-
-	mReq->req.actual   = mReq->ptr->token & TD_TOTAL_BYTES;
-	mReq->req.actual >>= ffs_nr(TD_TOTAL_BYTES);
-	mReq->req.actual   = mReq->req.length - mReq->req.actual;
-	mReq->req.actual   = mReq->req.status ? 0 : mReq->req.actual;
-
-	return mReq->req.actual;
-}
-
-/**
- * _ep_nuke: dequeues all endpoint requests
- * @mEp: endpoint
- *
- * This function returns an error code
- * Caller must hold lock
- */
-static int _ep_nuke(struct ci13xxx_ep *mEp)
-__releases(mEp->lock)
-__acquires(mEp->lock)
-{
-	trace("%p", mEp);
-
-	if (mEp == NULL)
-		return -EINVAL;
-
-	hw_ep_flush(mEp->num, mEp->dir);
-
-	while (!list_empty(&mEp->qh.queue)) {
-
-		/* pop oldest request */
-		struct ci13xxx_req *mReq = \
-			list_entry(mEp->qh.queue.next,
-				   struct ci13xxx_req, queue);
-		list_del_init(&mReq->queue);
-		mReq->req.status = -ESHUTDOWN;
-
-		if (mReq->req.complete != NULL) {
-			spin_unlock(mEp->lock);
-			mReq->req.complete(&mEp->ep, &mReq->req);
-			spin_lock(mEp->lock);
-		}
-	}
-	return 0;
-}
-
-/**
- * _gadget_stop_activity: stops all USB activity, flushes & disables all endpts
- * @gadget: gadget
- *
- * This function returns an error code
- */
-static int _gadget_stop_activity(struct usb_gadget *gadget)
-{
-	struct usb_ep *ep;
-	struct ci13xxx    *udc = container_of(gadget, struct ci13xxx, gadget);
-	unsigned long flags;
-
-	trace("%p", gadget);
-
-	if (gadget == NULL)
-		return -EINVAL;
-
-	spin_lock_irqsave(udc->lock, flags);
-	udc->gadget.speed = USB_SPEED_UNKNOWN;
-	udc->remote_wakeup = 0;
-	udc->suspended = 0;
-	spin_unlock_irqrestore(udc->lock, flags);
-
-	/* flush all endpoints */
-	gadget_for_each_ep(ep, gadget) {
-		usb_ep_fifo_flush(ep);
-	}
-	usb_ep_fifo_flush(&udc->ep0out.ep);
-	usb_ep_fifo_flush(&udc->ep0in.ep);
-
-	udc->driver->disconnect(gadget);
-
-	/* make sure to disable all endpoints */
-	gadget_for_each_ep(ep, gadget) {
-		usb_ep_disable(ep);
-	}
-
-	if (udc->status != NULL) {
-		usb_ep_free_request(&udc->ep0in.ep, udc->status);
-		udc->status = NULL;
-	}
-
-	return 0;
-}
-
-/******************************************************************************
- * ISR block
- *****************************************************************************/
-/**
- * isr_reset_handler: USB reset interrupt handler
- * @udc: UDC device
- *
- * This function resets USB engine after a bus reset occurred
- */
-static void isr_reset_handler(struct ci13xxx *udc)
-__releases(udc->lock)
-__acquires(udc->lock)
-{
-	int retval;
-
-	trace("%p", udc);
-
-	if (udc == NULL) {
-		err("EINVAL");
-		return;
-	}
-
-	dbg_event(0xFF, "BUS RST", 0);
-
-	spin_unlock(udc->lock);
-	retval = _gadget_stop_activity(&udc->gadget);
-	if (retval)
-		goto done;
-
-	retval = hw_usb_reset();
-	if (retval)
-		goto done;
-
-	udc->status = usb_ep_alloc_request(&udc->ep0in.ep, GFP_ATOMIC);
-	if (udc->status == NULL)
-		retval = -ENOMEM;
-
-	spin_lock(udc->lock);
-
- done:
-	if (retval)
-		err("error: %i", retval);
-}
-
-/**
- * isr_get_status_complete: get_status request complete function
- * @ep:  endpoint
- * @req: request handled
- *
- * Caller must release lock
- */
-static void isr_get_status_complete(struct usb_ep *ep, struct usb_request *req)
-{
-	trace("%p, %p", ep, req);
-
-	if (ep == NULL || req == NULL) {
-		err("EINVAL");
-		return;
-	}
-
-	kfree(req->buf);
-	usb_ep_free_request(ep, req);
-}
-
-/**
- * isr_get_status_response: get_status request response
- * @udc: udc struct
- * @setup: setup request packet
- *
- * This function returns an error code
- */
-static int isr_get_status_response(struct ci13xxx *udc,
-				   struct usb_ctrlrequest *setup)
-__releases(mEp->lock)
-__acquires(mEp->lock)
-{
-	struct ci13xxx_ep *mEp = &udc->ep0in;
-	struct usb_request *req = NULL;
-	gfp_t gfp_flags = GFP_ATOMIC;
-	int dir, num, retval;
-
-	trace("%p, %p", mEp, setup);
-
-	if (mEp == NULL || setup == NULL)
-		return -EINVAL;
-
-	spin_unlock(mEp->lock);
-	req = usb_ep_alloc_request(&mEp->ep, gfp_flags);
-	spin_lock(mEp->lock);
-	if (req == NULL)
-		return -ENOMEM;
-
-	req->complete = isr_get_status_complete;
-	req->length   = 2;
-	req->buf      = kzalloc(req->length, gfp_flags);
-	if (req->buf == NULL) {
-		retval = -ENOMEM;
-		goto err_free_req;
-	}
-
-	if ((setup->bRequestType & USB_RECIP_MASK) == USB_RECIP_DEVICE) {
-		/* Assume that device is bus powered for now. */
-		*((u16 *)req->buf) = _udc->remote_wakeup << 1;
-		retval = 0;
-	} else if ((setup->bRequestType & USB_RECIP_MASK) \
-		   == USB_RECIP_ENDPOINT) {
-		dir = (le16_to_cpu(setup->wIndex) & USB_ENDPOINT_DIR_MASK) ?
-			TX : RX;
-		num =  le16_to_cpu(setup->wIndex) & USB_ENDPOINT_NUMBER_MASK;
-		*((u16 *)req->buf) = hw_ep_get_halt(num, dir);
-	}
-	/* else do nothing; reserved for future use */
-
-	spin_unlock(mEp->lock);
-	retval = usb_ep_queue(&mEp->ep, req, gfp_flags);
-	spin_lock(mEp->lock);
-	if (retval)
-		goto err_free_buf;
-
-	return 0;
-
- err_free_buf:
-	kfree(req->buf);
- err_free_req:
-	spin_unlock(mEp->lock);
-	usb_ep_free_request(&mEp->ep, req);
-	spin_lock(mEp->lock);
-	return retval;
-}
-
-/**
- * isr_setup_status_complete: setup_status request complete function
- * @ep:  endpoint
- * @req: request handled
- *
- * Caller must release lock. Put the port in test mode if test mode
- * feature is selected.
- */
-static void
-isr_setup_status_complete(struct usb_ep *ep, struct usb_request *req)
-{
-	struct ci13xxx *udc = req->context;
-	unsigned long flags;
-
-	trace("%p, %p", ep, req);
-
-	spin_lock_irqsave(udc->lock, flags);
-	if (udc->test_mode)
-		hw_port_test_set(udc->test_mode);
-	spin_unlock_irqrestore(udc->lock, flags);
-}
-
-/**
- * isr_setup_status_phase: queues the status phase of a setup transation
- * @udc: udc struct
- *
- * This function returns an error code
- */
-static int isr_setup_status_phase(struct ci13xxx *udc)
-__releases(mEp->lock)
-__acquires(mEp->lock)
-{
-	int retval;
-	struct ci13xxx_ep *mEp;
-
-	trace("%p", udc);
-
-	mEp = (udc->ep0_dir == TX) ? &udc->ep0out : &udc->ep0in;
-	udc->status->context = udc;
-	udc->status->complete = isr_setup_status_complete;
-
-	spin_unlock(mEp->lock);
-	retval = usb_ep_queue(&mEp->ep, udc->status, GFP_ATOMIC);
-	spin_lock(mEp->lock);
-
-	return retval;
-}
-
-/**
- * isr_tr_complete_low: transaction complete low level handler
- * @mEp: endpoint
- *
- * This function returns an error code
- * Caller must hold lock
- */
-static int isr_tr_complete_low(struct ci13xxx_ep *mEp)
-__releases(mEp->lock)
-__acquires(mEp->lock)
-{
-	struct ci13xxx_req *mReq, *mReqTemp;
-	struct ci13xxx_ep *mEpTemp = mEp;
-	int uninitialized_var(retval);
-
-	trace("%p", mEp);
-
-	if (list_empty(&mEp->qh.queue))
-		return -EINVAL;
-
-	list_for_each_entry_safe(mReq, mReqTemp, &mEp->qh.queue,
-			queue) {
-		retval = _hardware_dequeue(mEp, mReq);
-		if (retval < 0)
-			break;
-		list_del_init(&mReq->queue);
-		dbg_done(_usb_addr(mEp), mReq->ptr->token, retval);
-		if (mReq->req.complete != NULL) {
-			spin_unlock(mEp->lock);
-			if ((mEp->type == USB_ENDPOINT_XFER_CONTROL) &&
-					mReq->req.length)
-				mEpTemp = &_udc->ep0in;
-			mReq->req.complete(&mEpTemp->ep, &mReq->req);
-			spin_lock(mEp->lock);
-		}
-	}
-
-	if (retval == -EBUSY)
-		retval = 0;
-	if (retval < 0)
-		dbg_event(_usb_addr(mEp), "DONE", retval);
-
-	return retval;
-}
-
-/**
- * isr_tr_complete_handler: transaction complete interrupt handler
- * @udc: UDC descriptor
- *
- * This function handles traffic events
- */
-static void isr_tr_complete_handler(struct ci13xxx *udc)
-__releases(udc->lock)
-__acquires(udc->lock)
-{
-	unsigned i;
-	u8 tmode = 0;
-
-	trace("%p", udc);
-
-	if (udc == NULL) {
-		err("EINVAL");
-		return;
-	}
-
-	for (i = 0; i < hw_ep_max; i++) {
-		struct ci13xxx_ep *mEp  = &udc->ci13xxx_ep[i];
-		int type, num, dir, err = -EINVAL;
-		struct usb_ctrlrequest req;
-
-		if (mEp->desc == NULL)
-			continue;   /* not configured */
-
-		if (hw_test_and_clear_complete(i)) {
-			err = isr_tr_complete_low(mEp);
-			if (mEp->type == USB_ENDPOINT_XFER_CONTROL) {
-				if (err > 0)   /* needs status phase */
-					err = isr_setup_status_phase(udc);
-				if (err < 0) {
-					dbg_event(_usb_addr(mEp),
-						  "ERROR", err);
-					spin_unlock(udc->lock);
-					if (usb_ep_set_halt(&mEp->ep))
-						err("error: ep_set_halt");
-					spin_lock(udc->lock);
-				}
-			}
-		}
-
-		if (mEp->type != USB_ENDPOINT_XFER_CONTROL ||
-		    !hw_test_and_clear_setup_status(i))
-			continue;
-
-		if (i != 0) {
-			warn("ctrl traffic received at endpoint");
-			continue;
-		}
-
-		/*
-		 * Flush data and handshake transactions of previous
-		 * setup packet.
-		 */
-		_ep_nuke(&udc->ep0out);
-		_ep_nuke(&udc->ep0in);
-
-		/* read_setup_packet */
-		do {
-			hw_test_and_set_setup_guard();
-			memcpy(&req, &mEp->qh.ptr->setup, sizeof(req));
-		} while (!hw_test_and_clear_setup_guard());
-
-		type = req.bRequestType;
-
-		udc->ep0_dir = (type & USB_DIR_IN) ? TX : RX;
-
-		dbg_setup(_usb_addr(mEp), &req);
-
-		switch (req.bRequest) {
-		case USB_REQ_CLEAR_FEATURE:
-			if (type == (USB_DIR_OUT|USB_RECIP_ENDPOINT) &&
-					le16_to_cpu(req.wValue) ==
-					USB_ENDPOINT_HALT) {
-				if (req.wLength != 0)
-					break;
-				num  = le16_to_cpu(req.wIndex);
-				dir = num & USB_ENDPOINT_DIR_MASK;
-				num &= USB_ENDPOINT_NUMBER_MASK;
-				if (dir) /* TX */
-					num += hw_ep_max/2;
-				if (!udc->ci13xxx_ep[num].wedge) {
-					spin_unlock(udc->lock);
-					err = usb_ep_clear_halt(
-						&udc->ci13xxx_ep[num].ep);
-					spin_lock(udc->lock);
-					if (err)
-						break;
-				}
-				err = isr_setup_status_phase(udc);
-			} else if (type == (USB_DIR_OUT|USB_RECIP_DEVICE) &&
-					le16_to_cpu(req.wValue) ==
-					USB_DEVICE_REMOTE_WAKEUP) {
-				if (req.wLength != 0)
-					break;
-				udc->remote_wakeup = 0;
-				err = isr_setup_status_phase(udc);
-			} else {
-				goto delegate;
-			}
-			break;
-		case USB_REQ_GET_STATUS:
-			if (type != (USB_DIR_IN|USB_RECIP_DEVICE)   &&
-			    type != (USB_DIR_IN|USB_RECIP_ENDPOINT) &&
-			    type != (USB_DIR_IN|USB_RECIP_INTERFACE))
-				goto delegate;
-			if (le16_to_cpu(req.wLength) != 2 ||
-			    le16_to_cpu(req.wValue)  != 0)
-				break;
-			err = isr_get_status_response(udc, &req);
-			break;
-		case USB_REQ_SET_ADDRESS:
-			if (type != (USB_DIR_OUT|USB_RECIP_DEVICE))
-				goto delegate;
-			if (le16_to_cpu(req.wLength) != 0 ||
-			    le16_to_cpu(req.wIndex)  != 0)
-				break;
-			err = hw_usb_set_address((u8)le16_to_cpu(req.wValue));
-			if (err)
-				break;
-			err = isr_setup_status_phase(udc);
-			break;
-		case USB_REQ_SET_FEATURE:
-			if (type == (USB_DIR_OUT|USB_RECIP_ENDPOINT) &&
-					le16_to_cpu(req.wValue) ==
-					USB_ENDPOINT_HALT) {
-				if (req.wLength != 0)
-					break;
-				num  = le16_to_cpu(req.wIndex);
-				dir = num & USB_ENDPOINT_DIR_MASK;
-				num &= USB_ENDPOINT_NUMBER_MASK;
-				if (dir) /* TX */
-					num += hw_ep_max/2;
-
-				spin_unlock(udc->lock);
-				err = usb_ep_set_halt(&udc->ci13xxx_ep[num].ep);
-				spin_lock(udc->lock);
-				if (!err)
-					isr_setup_status_phase(udc);
-			} else if (type == (USB_DIR_OUT|USB_RECIP_DEVICE)) {
-				if (req.wLength != 0)
-					break;
-				switch (le16_to_cpu(req.wValue)) {
-				case USB_DEVICE_REMOTE_WAKEUP:
-					udc->remote_wakeup = 1;
-					err = isr_setup_status_phase(udc);
-					break;
-				case USB_DEVICE_TEST_MODE:
-					tmode = le16_to_cpu(req.wIndex) >> 8;
-					switch (tmode) {
-					case TEST_J:
-					case TEST_K:
-					case TEST_SE0_NAK:
-					case TEST_PACKET:
-					case TEST_FORCE_EN:
-						udc->test_mode = tmode;
-						err = isr_setup_status_phase(
-								udc);
-						break;
-					default:
-						break;
-					}
-				default:
-					goto delegate;
-				}
-			} else {
-				goto delegate;
-			}
-			break;
-		default:
-delegate:
-			if (req.wLength == 0)   /* no data phase */
-				udc->ep0_dir = TX;
-
-			spin_unlock(udc->lock);
-			err = udc->driver->setup(&udc->gadget, &req);
-			spin_lock(udc->lock);
-			break;
-		}
-
-		if (err < 0) {
-			dbg_event(_usb_addr(mEp), "ERROR", err);
-
-			spin_unlock(udc->lock);
-			if (usb_ep_set_halt(&mEp->ep))
-				err("error: ep_set_halt");
-			spin_lock(udc->lock);
-		}
-	}
-}
-
-/******************************************************************************
- * ENDPT block
- *****************************************************************************/
-/**
- * ep_enable: configure endpoint, making it usable
- *
- * Check usb_ep_enable() at "usb_gadget.h" for details
- */
-static int ep_enable(struct usb_ep *ep,
-		     const struct usb_endpoint_descriptor *desc)
-{
-	struct ci13xxx_ep *mEp = container_of(ep, struct ci13xxx_ep, ep);
-	int retval = 0;
-	unsigned long flags;
-
-	trace("%p, %p", ep, desc);
-
-	if (ep == NULL || desc == NULL)
-		return -EINVAL;
-
-	spin_lock_irqsave(mEp->lock, flags);
-
-	/* only internal SW should enable ctrl endpts */
-
-	mEp->desc = desc;
-
-	if (!list_empty(&mEp->qh.queue))
-		warn("enabling a non-empty endpoint!");
-
-	mEp->dir  = usb_endpoint_dir_in(desc) ? TX : RX;
-	mEp->num  = usb_endpoint_num(desc);
-	mEp->type = usb_endpoint_type(desc);
-
-	mEp->ep.maxpacket = usb_endpoint_maxp(desc);
-
-	dbg_event(_usb_addr(mEp), "ENABLE", 0);
-
-	mEp->qh.ptr->cap = 0;
-
-	if (mEp->type == USB_ENDPOINT_XFER_CONTROL)
-		mEp->qh.ptr->cap |=  QH_IOS;
-	else if (mEp->type == USB_ENDPOINT_XFER_ISOC)
-		mEp->qh.ptr->cap &= ~QH_MULT;
-	else
-		mEp->qh.ptr->cap &= ~QH_ZLT;
-
-	mEp->qh.ptr->cap |=
-		(mEp->ep.maxpacket << ffs_nr(QH_MAX_PKT)) & QH_MAX_PKT;
-	mEp->qh.ptr->td.next |= TD_TERMINATE;   /* needed? */
-
-	/*
-	 * Enable endpoints in the HW other than ep0 as ep0
-	 * is always enabled
-	 */
-	if (mEp->num)
-		retval |= hw_ep_enable(mEp->num, mEp->dir, mEp->type);
-
-	spin_unlock_irqrestore(mEp->lock, flags);
-	return retval;
-}
-
-/**
- * ep_disable: endpoint is no longer usable
- *
- * Check usb_ep_disable() at "usb_gadget.h" for details
- */
-static int ep_disable(struct usb_ep *ep)
-{
-	struct ci13xxx_ep *mEp = container_of(ep, struct ci13xxx_ep, ep);
-	int direction, retval = 0;
-	unsigned long flags;
-
-	trace("%p", ep);
-
-	if (ep == NULL)
-		return -EINVAL;
-	else if (mEp->desc == NULL)
-		return -EBUSY;
-
-	spin_lock_irqsave(mEp->lock, flags);
-
-	/* only internal SW should disable ctrl endpts */
-
-	direction = mEp->dir;
-	do {
-		dbg_event(_usb_addr(mEp), "DISABLE", 0);
-
-		retval |= _ep_nuke(mEp);
-		retval |= hw_ep_disable(mEp->num, mEp->dir);
-
-		if (mEp->type == USB_ENDPOINT_XFER_CONTROL)
-			mEp->dir = (mEp->dir == TX) ? RX : TX;
-
-	} while (mEp->dir != direction);
-
-	mEp->desc = NULL;
-	mEp->ep.desc = NULL;
-
-	spin_unlock_irqrestore(mEp->lock, flags);
-	return retval;
-}
-
-/**
- * ep_alloc_request: allocate a request object to use with this endpoint
- *
- * Check usb_ep_alloc_request() at "usb_gadget.h" for details
- */
-static struct usb_request *ep_alloc_request(struct usb_ep *ep, gfp_t gfp_flags)
-{
-	struct ci13xxx_ep  *mEp  = container_of(ep, struct ci13xxx_ep, ep);
-	struct ci13xxx_req *mReq = NULL;
-
-	trace("%p, %i", ep, gfp_flags);
-
-	if (ep == NULL) {
-		err("EINVAL");
-		return NULL;
-	}
-
-	mReq = kzalloc(sizeof(struct ci13xxx_req), gfp_flags);
-	if (mReq != NULL) {
-		INIT_LIST_HEAD(&mReq->queue);
-		mReq->req.dma = DMA_ADDR_INVALID;
-
-		mReq->ptr = dma_pool_alloc(mEp->td_pool, gfp_flags,
-					   &mReq->dma);
-		if (mReq->ptr == NULL) {
-			kfree(mReq);
-			mReq = NULL;
-		}
-	}
-
-	dbg_event(_usb_addr(mEp), "ALLOC", mReq == NULL);
-
-	return (mReq == NULL) ? NULL : &mReq->req;
-}
-
-/**
- * ep_free_request: frees a request object
- *
- * Check usb_ep_free_request() at "usb_gadget.h" for details
- */
-static void ep_free_request(struct usb_ep *ep, struct usb_request *req)
-{
-	struct ci13xxx_ep  *mEp  = container_of(ep,  struct ci13xxx_ep, ep);
-	struct ci13xxx_req *mReq = container_of(req, struct ci13xxx_req, req);
-	unsigned long flags;
-
-	trace("%p, %p", ep, req);
-
-	if (ep == NULL || req == NULL) {
-		err("EINVAL");
-		return;
-	} else if (!list_empty(&mReq->queue)) {
-		err("EBUSY");
-		return;
-	}
-
-	spin_lock_irqsave(mEp->lock, flags);
-
-	if (mReq->ptr)
-		dma_pool_free(mEp->td_pool, mReq->ptr, mReq->dma);
-	kfree(mReq);
-
-	dbg_event(_usb_addr(mEp), "FREE", 0);
-
-	spin_unlock_irqrestore(mEp->lock, flags);
-}
-
-/**
- * ep_queue: queues (submits) an I/O request to an endpoint
- *
- * Check usb_ep_queue()* at usb_gadget.h" for details
- */
-static int ep_queue(struct usb_ep *ep, struct usb_request *req,
-		    gfp_t __maybe_unused gfp_flags)
-{
-	struct ci13xxx_ep  *mEp  = container_of(ep,  struct ci13xxx_ep, ep);
-	struct ci13xxx_req *mReq = container_of(req, struct ci13xxx_req, req);
-	int retval = 0;
-	unsigned long flags;
-
-	trace("%p, %p, %X", ep, req, gfp_flags);
-
-	if (ep == NULL || req == NULL || mEp->desc == NULL)
-		return -EINVAL;
-
-	spin_lock_irqsave(mEp->lock, flags);
-
-	if (mEp->type == USB_ENDPOINT_XFER_CONTROL) {
-		if (req->length)
-			mEp = (_udc->ep0_dir == RX) ?
-				&_udc->ep0out : &_udc->ep0in;
-		if (!list_empty(&mEp->qh.queue)) {
-			_ep_nuke(mEp);
-			retval = -EOVERFLOW;
-			warn("endpoint ctrl %X nuked", _usb_addr(mEp));
-		}
-	}
-
-	/* first nuke then test link, e.g. previous status has not sent */
-	if (!list_empty(&mReq->queue)) {
-		retval = -EBUSY;
-		err("request already in queue");
-		goto done;
-	}
-
-	if (req->length > (4 * CI13XXX_PAGE_SIZE)) {
-		req->length = (4 * CI13XXX_PAGE_SIZE);
-		retval = -EMSGSIZE;
-		warn("request length truncated");
-	}
-
-	dbg_queue(_usb_addr(mEp), req, retval);
-
-	/* push request */
-	mReq->req.status = -EINPROGRESS;
-	mReq->req.actual = 0;
-
-	retval = _hardware_enqueue(mEp, mReq);
-
-	if (retval == -EALREADY) {
-		dbg_event(_usb_addr(mEp), "QUEUE", retval);
-		retval = 0;
-	}
-	if (!retval)
-		list_add_tail(&mReq->queue, &mEp->qh.queue);
-
- done:
-	spin_unlock_irqrestore(mEp->lock, flags);
-	return retval;
-}
-
-/**
- * ep_dequeue: dequeues (cancels, unlinks) an I/O request from an endpoint
- *
- * Check usb_ep_dequeue() at "usb_gadget.h" for details
- */
-static int ep_dequeue(struct usb_ep *ep, struct usb_request *req)
-{
-	struct ci13xxx_ep  *mEp  = container_of(ep,  struct ci13xxx_ep, ep);
-	struct ci13xxx_req *mReq = container_of(req, struct ci13xxx_req, req);
-	unsigned long flags;
-
-	trace("%p, %p", ep, req);
-
-	if (ep == NULL || req == NULL || mReq->req.status != -EALREADY ||
-		mEp->desc == NULL || list_empty(&mReq->queue) ||
-		list_empty(&mEp->qh.queue))
-		return -EINVAL;
-
-	spin_lock_irqsave(mEp->lock, flags);
-
-	dbg_event(_usb_addr(mEp), "DEQUEUE", 0);
-
-	hw_ep_flush(mEp->num, mEp->dir);
-
-	/* pop request */
-	list_del_init(&mReq->queue);
-	if (mReq->map) {
-		dma_unmap_single(mEp->device, mReq->req.dma, mReq->req.length,
-				 mEp->dir ? DMA_TO_DEVICE : DMA_FROM_DEVICE);
-		mReq->req.dma = DMA_ADDR_INVALID;
-		mReq->map     = 0;
-	}
-	req->status = -ECONNRESET;
-
-	if (mReq->req.complete != NULL) {
-		spin_unlock(mEp->lock);
-		mReq->req.complete(&mEp->ep, &mReq->req);
-		spin_lock(mEp->lock);
-	}
-
-	spin_unlock_irqrestore(mEp->lock, flags);
-	return 0;
-}
-
-/**
- * ep_set_halt: sets the endpoint halt feature
- *
- * Check usb_ep_set_halt() at "usb_gadget.h" for details
- */
-static int ep_set_halt(struct usb_ep *ep, int value)
-{
-	struct ci13xxx_ep *mEp = container_of(ep, struct ci13xxx_ep, ep);
-	int direction, retval = 0;
-	unsigned long flags;
-
-	trace("%p, %i", ep, value);
-
-	if (ep == NULL || mEp->desc == NULL)
-		return -EINVAL;
-
-	spin_lock_irqsave(mEp->lock, flags);
-
-#ifndef STALL_IN
-	/* g_file_storage MS compliant but g_zero fails chapter 9 compliance */
-	if (value && mEp->type == USB_ENDPOINT_XFER_BULK && mEp->dir == TX &&
-	    !list_empty(&mEp->qh.queue)) {
-		spin_unlock_irqrestore(mEp->lock, flags);
-		return -EAGAIN;
-	}
-#endif
-
-	direction = mEp->dir;
-	do {
-		dbg_event(_usb_addr(mEp), "HALT", value);
-		retval |= hw_ep_set_halt(mEp->num, mEp->dir, value);
-
-		if (!value)
-			mEp->wedge = 0;
-
-		if (mEp->type == USB_ENDPOINT_XFER_CONTROL)
-			mEp->dir = (mEp->dir == TX) ? RX : TX;
-
-	} while (mEp->dir != direction);
-
-	spin_unlock_irqrestore(mEp->lock, flags);
-	return retval;
-}
-
-/**
- * ep_set_wedge: sets the halt feature and ignores clear requests
- *
- * Check usb_ep_set_wedge() at "usb_gadget.h" for details
- */
-static int ep_set_wedge(struct usb_ep *ep)
-{
-	struct ci13xxx_ep *mEp = container_of(ep, struct ci13xxx_ep, ep);
-	unsigned long flags;
-
-	trace("%p", ep);
-
-	if (ep == NULL || mEp->desc == NULL)
-		return -EINVAL;
-
-	spin_lock_irqsave(mEp->lock, flags);
-
-	dbg_event(_usb_addr(mEp), "WEDGE", 0);
-	mEp->wedge = 1;
-
-	spin_unlock_irqrestore(mEp->lock, flags);
-
-	return usb_ep_set_halt(ep);
-}
-
-/**
- * ep_fifo_flush: flushes contents of a fifo
- *
- * Check usb_ep_fifo_flush() at "usb_gadget.h" for details
- */
-static void ep_fifo_flush(struct usb_ep *ep)
-{
-	struct ci13xxx_ep *mEp = container_of(ep, struct ci13xxx_ep, ep);
-	unsigned long flags;
-
-	trace("%p", ep);
-
-	if (ep == NULL) {
-		err("%02X: -EINVAL", _usb_addr(mEp));
-		return;
-	}
-
-	spin_lock_irqsave(mEp->lock, flags);
-
-	dbg_event(_usb_addr(mEp), "FFLUSH", 0);
-	hw_ep_flush(mEp->num, mEp->dir);
-
-	spin_unlock_irqrestore(mEp->lock, flags);
-}
-
-/**
- * Endpoint-specific part of the API to the USB controller hardware
- * Check "usb_gadget.h" for details
- */
-static const struct usb_ep_ops usb_ep_ops = {
-	.enable	       = ep_enable,
-	.disable       = ep_disable,
-	.alloc_request = ep_alloc_request,
-	.free_request  = ep_free_request,
-	.queue	       = ep_queue,
-	.dequeue       = ep_dequeue,
-	.set_halt      = ep_set_halt,
-	.set_wedge     = ep_set_wedge,
-	.fifo_flush    = ep_fifo_flush,
-};
-
-/******************************************************************************
- * GADGET block
- *****************************************************************************/
-static int ci13xxx_vbus_session(struct usb_gadget *_gadget, int is_active)
-{
-	struct ci13xxx *udc = container_of(_gadget, struct ci13xxx, gadget);
-	unsigned long flags;
-	int gadget_ready = 0;
-
-	if (!(udc->udc_driver->flags & CI13XXX_PULLUP_ON_VBUS))
-		return -EOPNOTSUPP;
-
-	spin_lock_irqsave(udc->lock, flags);
-	udc->vbus_active = is_active;
-	if (udc->driver)
-		gadget_ready = 1;
-	spin_unlock_irqrestore(udc->lock, flags);
-
-	if (gadget_ready) {
-		if (is_active) {
-			pm_runtime_get_sync(&_gadget->dev);
-			hw_device_reset(udc);
-			hw_device_state(udc->ep0out.qh.dma);
-		} else {
-			hw_device_state(0);
-			if (udc->udc_driver->notify_event)
-				udc->udc_driver->notify_event(udc,
-				CI13XXX_CONTROLLER_STOPPED_EVENT);
-			_gadget_stop_activity(&udc->gadget);
-			pm_runtime_put_sync(&_gadget->dev);
-		}
-	}
-
-	return 0;
-}
-
-static int ci13xxx_wakeup(struct usb_gadget *_gadget)
-{
-	struct ci13xxx *udc = container_of(_gadget, struct ci13xxx, gadget);
-	unsigned long flags;
-	int ret = 0;
-
-	trace();
-
-	spin_lock_irqsave(udc->lock, flags);
-	if (!udc->remote_wakeup) {
-		ret = -EOPNOTSUPP;
-		trace("remote wakeup feature is not enabled\n");
-		goto out;
-	}
-	if (!hw_cread(CAP_PORTSC, PORTSC_SUSP)) {
-		ret = -EINVAL;
-		trace("port is not suspended\n");
-		goto out;
-	}
-	hw_cwrite(CAP_PORTSC, PORTSC_FPR, PORTSC_FPR);
-out:
-	spin_unlock_irqrestore(udc->lock, flags);
-	return ret;
-}
-
-static int ci13xxx_vbus_draw(struct usb_gadget *_gadget, unsigned mA)
-{
-	struct ci13xxx *udc = container_of(_gadget, struct ci13xxx, gadget);
-
-	if (udc->transceiver)
-		return usb_phy_set_power(udc->transceiver, mA);
-	return -ENOTSUPP;
-}
-
-static int ci13xxx_start(struct usb_gadget_driver *driver,
-		int (*bind)(struct usb_gadget *));
-static int ci13xxx_stop(struct usb_gadget_driver *driver);
-/**
- * Device operations part of the API to the USB controller hardware,
- * which don't involve endpoints (or i/o)
- * Check  "usb_gadget.h" for details
- */
-static const struct usb_gadget_ops usb_gadget_ops = {
-	.vbus_session	= ci13xxx_vbus_session,
-	.wakeup		= ci13xxx_wakeup,
-	.vbus_draw	= ci13xxx_vbus_draw,
-	.start		= ci13xxx_start,
-	.stop		= ci13xxx_stop,
-};
-
-/**
- * ci13xxx_start: register a gadget driver
- * @driver: the driver being registered
- * @bind: the driver's bind callback
- *
- * Check ci13xxx_start() at <linux/usb/gadget.h> for details.
- * Interrupts are enabled here.
- */
-static int ci13xxx_start(struct usb_gadget_driver *driver,
-		int (*bind)(struct usb_gadget *))
-{
-	struct ci13xxx *udc = _udc;
-	unsigned long flags;
-	int i, j;
-	int retval = -ENOMEM;
-
-	trace("%p", driver);
-
-	if (driver             == NULL ||
-	    bind               == NULL ||
-	    driver->setup      == NULL ||
-	    driver->disconnect == NULL)
-		return -EINVAL;
-	else if (udc         == NULL)
-		return -ENODEV;
-	else if (udc->driver != NULL)
-		return -EBUSY;
-
-	/* alloc resources */
-	udc->qh_pool = dma_pool_create("ci13xxx_qh", &udc->gadget.dev,
-				       sizeof(struct ci13xxx_qh),
-				       64, CI13XXX_PAGE_SIZE);
-	if (udc->qh_pool == NULL)
-		return -ENOMEM;
-
-	udc->td_pool = dma_pool_create("ci13xxx_td", &udc->gadget.dev,
-				       sizeof(struct ci13xxx_td),
-				       64, CI13XXX_PAGE_SIZE);
-	if (udc->td_pool == NULL) {
-		dma_pool_destroy(udc->qh_pool);
-		udc->qh_pool = NULL;
-		return -ENOMEM;
-	}
-
-	spin_lock_irqsave(udc->lock, flags);
-
-	info("hw_ep_max = %d", hw_ep_max);
-
-	udc->gadget.dev.driver = NULL;
-
-	retval = 0;
-	for (i = 0; i < hw_ep_max/2; i++) {
-		for (j = RX; j <= TX; j++) {
-			int k = i + j * hw_ep_max/2;
-			struct ci13xxx_ep *mEp = &udc->ci13xxx_ep[k];
-
-			scnprintf(mEp->name, sizeof(mEp->name), "ep%i%s", i,
-					(j == TX)  ? "in" : "out");
-
-			mEp->lock         = udc->lock;
-			mEp->device       = &udc->gadget.dev;
-			mEp->td_pool      = udc->td_pool;
-
-			mEp->ep.name      = mEp->name;
-			mEp->ep.ops       = &usb_ep_ops;
-			mEp->ep.maxpacket = CTRL_PAYLOAD_MAX;
-
-			INIT_LIST_HEAD(&mEp->qh.queue);
-			spin_unlock_irqrestore(udc->lock, flags);
-			mEp->qh.ptr = dma_pool_alloc(udc->qh_pool, GFP_KERNEL,
-					&mEp->qh.dma);
-			spin_lock_irqsave(udc->lock, flags);
-			if (mEp->qh.ptr == NULL)
-				retval = -ENOMEM;
-			else
-				memset(mEp->qh.ptr, 0, sizeof(*mEp->qh.ptr));
-
-			/* skip ep0 out and in endpoints */
-			if (i == 0)
-				continue;
-
-			list_add_tail(&mEp->ep.ep_list, &udc->gadget.ep_list);
-		}
-	}
-	if (retval)
-		goto done;
-	spin_unlock_irqrestore(udc->lock, flags);
-	udc->ep0out.ep.desc = &ctrl_endpt_out_desc;
-	retval = usb_ep_enable(&udc->ep0out.ep);
-	if (retval)
-		return retval;
-
-	udc->ep0in.ep.desc = &ctrl_endpt_in_desc;
-	retval = usb_ep_enable(&udc->ep0in.ep);
-	if (retval)
-		return retval;
-	spin_lock_irqsave(udc->lock, flags);
-
-	udc->gadget.ep0 = &udc->ep0in.ep;
-	/* bind gadget */
-	driver->driver.bus     = NULL;
-	udc->gadget.dev.driver = &driver->driver;
-
-	spin_unlock_irqrestore(udc->lock, flags);
-	retval = bind(&udc->gadget);                /* MAY SLEEP */
-	spin_lock_irqsave(udc->lock, flags);
-
-	if (retval) {
-		udc->gadget.dev.driver = NULL;
-		goto done;
-	}
-
-	udc->driver = driver;
-	pm_runtime_get_sync(&udc->gadget.dev);
-	if (udc->udc_driver->flags & CI13XXX_PULLUP_ON_VBUS) {
-		if (udc->vbus_active) {
-			if (udc->udc_driver->flags & CI13XXX_REGS_SHARED)
-				hw_device_reset(udc);
-		} else {
-			pm_runtime_put_sync(&udc->gadget.dev);
-			goto done;
-		}
-	}
-
-	retval = hw_device_state(udc->ep0out.qh.dma);
-	if (retval)
-		pm_runtime_put_sync(&udc->gadget.dev);
-
- done:
-	spin_unlock_irqrestore(udc->lock, flags);
-	return retval;
-}
-
-/**
- * ci13xxx_stop: unregister a gadget driver
- *
- * Check usb_gadget_unregister_driver() at "usb_gadget.h" for details
- */
-static int ci13xxx_stop(struct usb_gadget_driver *driver)
-{
-	struct ci13xxx *udc = _udc;
-	unsigned long i, flags;
-
-	trace("%p", driver);
-
-	if (driver             == NULL ||
-	    driver->unbind     == NULL ||
-	    driver->setup      == NULL ||
-	    driver->disconnect == NULL ||
-	    driver             != udc->driver)
-		return -EINVAL;
-
-	spin_lock_irqsave(udc->lock, flags);
-
-	if (!(udc->udc_driver->flags & CI13XXX_PULLUP_ON_VBUS) ||
-			udc->vbus_active) {
-		hw_device_state(0);
-		if (udc->udc_driver->notify_event)
-			udc->udc_driver->notify_event(udc,
-			CI13XXX_CONTROLLER_STOPPED_EVENT);
-		spin_unlock_irqrestore(udc->lock, flags);
-		_gadget_stop_activity(&udc->gadget);
-		spin_lock_irqsave(udc->lock, flags);
-		pm_runtime_put(&udc->gadget.dev);
-	}
-
-	/* unbind gadget */
-	spin_unlock_irqrestore(udc->lock, flags);
-	driver->unbind(&udc->gadget);               /* MAY SLEEP */
-	spin_lock_irqsave(udc->lock, flags);
-
-	udc->gadget.dev.driver = NULL;
-
-	/* free resources */
-	for (i = 0; i < hw_ep_max; i++) {
-		struct ci13xxx_ep *mEp = &udc->ci13xxx_ep[i];
-
-		if (!list_empty(&mEp->ep.ep_list))
-			list_del_init(&mEp->ep.ep_list);
-
-		if (mEp->qh.ptr != NULL)
-			dma_pool_free(udc->qh_pool, mEp->qh.ptr, mEp->qh.dma);
-	}
-
-	udc->gadget.ep0 = NULL;
-	udc->driver = NULL;
-
-	spin_unlock_irqrestore(udc->lock, flags);
-
-	if (udc->td_pool != NULL) {
-		dma_pool_destroy(udc->td_pool);
-		udc->td_pool = NULL;
-	}
-	if (udc->qh_pool != NULL) {
-		dma_pool_destroy(udc->qh_pool);
-		udc->qh_pool = NULL;
-	}
-
-	return 0;
-}
-
-/******************************************************************************
- * BUS block
- *****************************************************************************/
-/**
- * udc_irq: global interrupt handler
- *
- * This function returns IRQ_HANDLED if the IRQ has been handled
- * It locks access to registers
- */
-static irqreturn_t udc_irq(void)
-{
-	struct ci13xxx *udc = _udc;
-	irqreturn_t retval;
-	u32 intr;
-
-	trace();
-
-	if (udc == NULL) {
-		err("ENODEV");
-		return IRQ_HANDLED;
-	}
-
-	spin_lock(udc->lock);
-
-	if (udc->udc_driver->flags & CI13XXX_REGS_SHARED) {
-		if (hw_cread(CAP_USBMODE, USBMODE_CM) !=
-				USBMODE_CM_DEVICE) {
-			spin_unlock(udc->lock);
-			return IRQ_NONE;
-		}
-	}
-	intr = hw_test_and_clear_intr_active();
-	if (intr) {
-		isr_statistics.hndl.buf[isr_statistics.hndl.idx++] = intr;
-		isr_statistics.hndl.idx &= ISR_MASK;
-		isr_statistics.hndl.cnt++;
-
-		/* order defines priority - do NOT change it */
-		if (USBi_URI & intr) {
-			isr_statistics.uri++;
-			isr_reset_handler(udc);
-		}
-		if (USBi_PCI & intr) {
-			isr_statistics.pci++;
-			udc->gadget.speed = hw_port_is_high_speed() ?
-				USB_SPEED_HIGH : USB_SPEED_FULL;
-			if (udc->suspended && udc->driver->resume) {
-				spin_unlock(udc->lock);
-				udc->driver->resume(&udc->gadget);
-				spin_lock(udc->lock);
-				udc->suspended = 0;
-			}
-		}
-		if (USBi_UEI & intr)
-			isr_statistics.uei++;
-		if (USBi_UI  & intr) {
-			isr_statistics.ui++;
-			isr_tr_complete_handler(udc);
-		}
-		if (USBi_SLI & intr) {
-			if (udc->gadget.speed != USB_SPEED_UNKNOWN &&
-			    udc->driver->suspend) {
-				udc->suspended = 1;
-				spin_unlock(udc->lock);
-				udc->driver->suspend(&udc->gadget);
-				spin_lock(udc->lock);
-			}
-			isr_statistics.sli++;
-		}
-		retval = IRQ_HANDLED;
-	} else {
-		isr_statistics.none++;
-		retval = IRQ_NONE;
-	}
-	spin_unlock(udc->lock);
-
-	return retval;
-}
-
-/**
- * udc_release: driver release function
- * @dev: device
- *
- * Currently does nothing
- */
-static void udc_release(struct device *dev)
-{
-	trace("%p", dev);
-
-	if (dev == NULL)
-		err("EINVAL");
-}
-
-/**
- * udc_probe: parent probe must call this to initialize UDC
- * @dev:  parent device
- * @regs: registers base address
- * @name: driver name
- *
- * This function returns an error code
- * No interrupts active, the IRQ has not been requested yet
- * Kernel assumes 32-bit DMA operations by default, no need to dma_set_mask
- */
-static int udc_probe(struct ci13xxx_udc_driver *driver, struct device *dev,
-		void __iomem *regs)
-{
-	struct ci13xxx *udc;
-	int retval = 0;
-
-	trace("%p, %p, %p", dev, regs, driver->name);
-
-	if (dev == NULL || regs == NULL || driver == NULL ||
-			driver->name == NULL)
-		return -EINVAL;
-
-	udc = kzalloc(sizeof(struct ci13xxx), GFP_KERNEL);
-	if (udc == NULL)
-		return -ENOMEM;
-
-	udc->lock = &udc_lock;
-	udc->regs = regs;
-	udc->udc_driver = driver;
-
-	udc->gadget.ops          = &usb_gadget_ops;
-	udc->gadget.speed        = USB_SPEED_UNKNOWN;
-	udc->gadget.max_speed    = USB_SPEED_HIGH;
-	udc->gadget.is_otg       = 0;
-	udc->gadget.name         = driver->name;
-
-	INIT_LIST_HEAD(&udc->gadget.ep_list);
-	udc->gadget.ep0 = NULL;
-
-	dev_set_name(&udc->gadget.dev, "gadget");
-	udc->gadget.dev.dma_mask = dev->dma_mask;
-	udc->gadget.dev.coherent_dma_mask = dev->coherent_dma_mask;
-	udc->gadget.dev.parent   = dev;
-	udc->gadget.dev.release  = udc_release;
-
-	retval = hw_device_init(regs);
-	if (retval < 0)
-		goto free_udc;
-
-	udc->transceiver = usb_get_transceiver();
-
-	if (udc->udc_driver->flags & CI13XXX_REQUIRE_TRANSCEIVER) {
-		if (udc->transceiver == NULL) {
-			retval = -ENODEV;
-			goto free_udc;
-		}
-	}
-
-	if (!(udc->udc_driver->flags & CI13XXX_REGS_SHARED)) {
-		retval = hw_device_reset(udc);
-		if (retval)
-			goto put_transceiver;
-	}
-
-	retval = device_register(&udc->gadget.dev);
-	if (retval) {
-		put_device(&udc->gadget.dev);
-		goto put_transceiver;
-	}
-
-#ifdef CONFIG_USB_GADGET_DEBUG_FILES
-	retval = dbg_create_files(&udc->gadget.dev);
-#endif
-	if (retval)
-		goto unreg_device;
-
-	if (udc->transceiver) {
-		retval = otg_set_peripheral(udc->transceiver->otg,
-						&udc->gadget);
-		if (retval)
-			goto remove_dbg;
-	}
-
-	retval = usb_add_gadget_udc(dev, &udc->gadget);
-	if (retval)
-		goto remove_trans;
-
-	pm_runtime_no_callbacks(&udc->gadget.dev);
-	pm_runtime_enable(&udc->gadget.dev);
-
-	_udc = udc;
-	return retval;
-
-remove_trans:
-	if (udc->transceiver) {
-		otg_set_peripheral(udc->transceiver->otg, &udc->gadget);
-		usb_put_transceiver(udc->transceiver);
-	}
-
-	err("error = %i", retval);
-remove_dbg:
-#ifdef CONFIG_USB_GADGET_DEBUG_FILES
-	dbg_remove_files(&udc->gadget.dev);
-#endif
-unreg_device:
-	device_unregister(&udc->gadget.dev);
-put_transceiver:
-	if (udc->transceiver)
-		usb_put_transceiver(udc->transceiver);
-free_udc:
-	kfree(udc);
-	_udc = NULL;
-	return retval;
-}
-
-/**
- * udc_remove: parent remove must call this to remove UDC
- *
- * No interrupts active, the IRQ has been released
- */
-static void udc_remove(void)
-{
-	struct ci13xxx *udc = _udc;
-
-	if (udc == NULL) {
-		err("EINVAL");
-		return;
-	}
-	usb_del_gadget_udc(&udc->gadget);
-
-	if (udc->transceiver) {
-		otg_set_peripheral(udc->transceiver->otg, &udc->gadget);
-		usb_put_transceiver(udc->transceiver);
-	}
-#ifdef CONFIG_USB_GADGET_DEBUG_FILES
-	dbg_remove_files(&udc->gadget.dev);
-#endif
-	device_unregister(&udc->gadget.dev);
-
-	kfree(udc);
-	_udc = NULL;
-}
diff --git a/drivers/usb/gadget/ci13xxx_udc.h b/drivers/usb/gadget/ci13xxx_udc.h
deleted file mode 100644
index 0d31af56c98..00000000000
--- a/drivers/usb/gadget/ci13xxx_udc.h
+++ /dev/null
@@ -1,227 +0,0 @@
-/*
- * ci13xxx_udc.h - structures, registers, and macros MIPS USB IP core
- *
- * Copyright (C) 2008 Chipidea - MIPS Technologies, Inc. All rights reserved.
- *
- * Author: David Lopo
- *
- * 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.
- *
- * Description: MIPS USB IP core family device controller
- *              Structures, registers and logging macros
- */
-
-#ifndef _CI13XXX_h_
-#define _CI13XXX_h_
-
-/******************************************************************************
- * DEFINE
- *****************************************************************************/
-#define CI13XXX_PAGE_SIZE  4096ul /* page size for TD's */
-#define ENDPT_MAX          (32)
-#define CTRL_PAYLOAD_MAX   (64)
-#define RX        (0)  /* similar to USB_DIR_OUT but can be used as an index */
-#define TX        (1)  /* similar to USB_DIR_IN  but can be used as an index */
-
-/******************************************************************************
- * STRUCTURES
- *****************************************************************************/
-/* DMA layout of transfer descriptors */
-struct ci13xxx_td {
-	/* 0 */
-	u32 next;
-#define TD_TERMINATE          BIT(0)
-#define TD_ADDR_MASK          (0xFFFFFFEUL << 5)
-	/* 1 */
-	u32 token;
-#define TD_STATUS             (0x00FFUL <<  0)
-#define TD_STATUS_TR_ERR      BIT(3)
-#define TD_STATUS_DT_ERR      BIT(5)
-#define TD_STATUS_HALTED      BIT(6)
-#define TD_STATUS_ACTIVE      BIT(7)
-#define TD_MULTO              (0x0003UL << 10)
-#define TD_IOC                BIT(15)
-#define TD_TOTAL_BYTES        (0x7FFFUL << 16)
-	/* 2 */
-	u32 page[5];
-#define TD_CURR_OFFSET        (0x0FFFUL <<  0)
-#define TD_FRAME_NUM          (0x07FFUL <<  0)
-#define TD_RESERVED_MASK      (0x0FFFUL <<  0)
-} __attribute__ ((packed));
-
-/* DMA layout of queue heads */
-struct ci13xxx_qh {
-	/* 0 */
-	u32 cap;
-#define QH_IOS                BIT(15)
-#define QH_MAX_PKT            (0x07FFUL << 16)
-#define QH_ZLT                BIT(29)
-#define QH_MULT               (0x0003UL << 30)
-	/* 1 */
-	u32 curr;
-	/* 2 - 8 */
-	struct ci13xxx_td        td;
-	/* 9 */
-	u32 RESERVED;
-	struct usb_ctrlrequest   setup;
-} __attribute__ ((packed));
-
-/* Extension of usb_request */
-struct ci13xxx_req {
-	struct usb_request   req;
-	unsigned             map;
-	struct list_head     queue;
-	struct ci13xxx_td   *ptr;
-	dma_addr_t           dma;
-	struct ci13xxx_td   *zptr;
-	dma_addr_t           zdma;
-};
-
-/* Extension of usb_ep */
-struct ci13xxx_ep {
-	struct usb_ep                          ep;
-	const struct usb_endpoint_descriptor  *desc;
-	u8                                     dir;
-	u8                                     num;
-	u8                                     type;
-	char                                   name[16];
-	struct {
-		struct list_head   queue;
-		struct ci13xxx_qh *ptr;
-		dma_addr_t         dma;
-	}                                      qh;
-	int                                    wedge;
-
-	/* global resources */
-	spinlock_t                            *lock;
-	struct device                         *device;
-	struct dma_pool                       *td_pool;
-};
-
-struct ci13xxx;
-struct ci13xxx_udc_driver {
-	const char	*name;
-	unsigned long	 flags;
-#define CI13XXX_REGS_SHARED		BIT(0)
-#define CI13XXX_REQUIRE_TRANSCEIVER	BIT(1)
-#define CI13XXX_PULLUP_ON_VBUS		BIT(2)
-#define CI13XXX_DISABLE_STREAMING	BIT(3)
-
-#define CI13XXX_CONTROLLER_RESET_EVENT		0
-#define CI13XXX_CONTROLLER_STOPPED_EVENT	1
-	void	(*notify_event) (struct ci13xxx *udc, unsigned event);
-};
-
-/* CI13XXX UDC descriptor & global resources */
-struct ci13xxx {
-	spinlock_t		  *lock;      /* ctrl register bank access */
-	void __iomem              *regs;      /* registers address space */
-
-	struct dma_pool           *qh_pool;   /* DMA pool for queue heads */
-	struct dma_pool           *td_pool;   /* DMA pool for transfer descs */
-	struct usb_request        *status;    /* ep0 status request */
-
-	struct usb_gadget          gadget;     /* USB slave device */
-	struct ci13xxx_ep          ci13xxx_ep[ENDPT_MAX]; /* extended endpts */
-	u32                        ep0_dir;    /* ep0 direction */
-#define ep0out ci13xxx_ep[0]
-#define ep0in  ci13xxx_ep[hw_ep_max / 2]
-	u8                         remote_wakeup; /* Is remote wakeup feature
-							enabled by the host? */
-	u8                         suspended;  /* suspended by the host */
-	u8                         test_mode;  /* the selected test mode */
-
-	struct usb_gadget_driver  *driver;     /* 3rd party gadget driver */
-	struct ci13xxx_udc_driver *udc_driver; /* device controller driver */
-	int                        vbus_active; /* is VBUS active */
-	struct usb_phy            *transceiver; /* Transceiver struct */
-};
-
-/******************************************************************************
- * REGISTERS
- *****************************************************************************/
-/* register size */
-#define REG_BITS   (32)
-
-/* HCCPARAMS */
-#define HCCPARAMS_LEN         BIT(17)
-
-/* DCCPARAMS */
-#define DCCPARAMS_DEN         (0x1F << 0)
-#define DCCPARAMS_DC          BIT(7)
-
-/* TESTMODE */
-#define TESTMODE_FORCE        BIT(0)
-
-/* USBCMD */
-#define USBCMD_RS             BIT(0)
-#define USBCMD_RST            BIT(1)
-#define USBCMD_SUTW           BIT(13)
-#define USBCMD_ATDTW          BIT(14)
-
-/* USBSTS & USBINTR */
-#define USBi_UI               BIT(0)
-#define USBi_UEI              BIT(1)
-#define USBi_PCI              BIT(2)
-#define USBi_URI              BIT(6)
-#define USBi_SLI              BIT(8)
-
-/* DEVICEADDR */
-#define DEVICEADDR_USBADRA    BIT(24)
-#define DEVICEADDR_USBADR     (0x7FUL << 25)
-
-/* PORTSC */
-#define PORTSC_FPR            BIT(6)
-#define PORTSC_SUSP           BIT(7)
-#define PORTSC_HSP            BIT(9)
-#define PORTSC_PTC            (0x0FUL << 16)
-
-/* DEVLC */
-#define DEVLC_PSPD            (0x03UL << 25)
-#define    DEVLC_PSPD_HS      (0x02UL << 25)
-
-/* USBMODE */
-#define USBMODE_CM            (0x03UL <<  0)
-#define    USBMODE_CM_IDLE    (0x00UL <<  0)
-#define    USBMODE_CM_DEVICE  (0x02UL <<  0)
-#define    USBMODE_CM_HOST    (0x03UL <<  0)
-#define USBMODE_SLOM          BIT(3)
-#define USBMODE_SDIS          BIT(4)
-
-/* ENDPTCTRL */
-#define ENDPTCTRL_RXS         BIT(0)
-#define ENDPTCTRL_RXT         (0x03UL <<  2)
-#define ENDPTCTRL_RXR         BIT(6)         /* reserved for port 0 */
-#define ENDPTCTRL_RXE         BIT(7)
-#define ENDPTCTRL_TXS         BIT(16)
-#define ENDPTCTRL_TXT         (0x03UL << 18)
-#define ENDPTCTRL_TXR         BIT(22)        /* reserved for port 0 */
-#define ENDPTCTRL_TXE         BIT(23)
-
-/******************************************************************************
- * LOGGING
- *****************************************************************************/
-#define ci13xxx_printk(level, format, args...) \
-do { \
-	if (_udc == NULL) \
-		printk(level "[%s] " format "\n", __func__, ## args); \
-	else \
-		dev_printk(level, _udc->gadget.dev.parent, \
-			   "[%s] " format "\n", __func__, ## args); \
-} while (0)
-
-#define err(format, args...)    ci13xxx_printk(KERN_ERR, format, ## args)
-#define warn(format, args...)   ci13xxx_printk(KERN_WARNING, format, ## args)
-#define info(format, args...)   ci13xxx_printk(KERN_INFO, format, ## args)
-
-#ifdef TRACE
-#define trace(format, args...)      ci13xxx_printk(KERN_DEBUG, format, ## args)
-#define dbg_trace(format, args...)  dev_dbg(dev, format, ##args)
-#else
-#define trace(format, args...)      do {} while (0)
-#define dbg_trace(format, args...)  do {} while (0)
-#endif
-
-#endif	/* _CI13XXX_h_ */
diff --git a/drivers/usb/gadget/composite.c b/drivers/usb/gadget/composite.c
index baaebf2830f..390749bbb0c 100644
--- a/drivers/usb/gadget/composite.c
+++ b/drivers/usb/gadget/composite.c
@@ -40,27 +40,27 @@ static int (*composite_gadget_bind)(struct usb_composite_dev *cdev);
  */
 
 static ushort idVendor;
-module_param(idVendor, ushort, 0);
+module_param(idVendor, ushort, 0644);
 MODULE_PARM_DESC(idVendor, "USB Vendor ID");
 
 static ushort idProduct;
-module_param(idProduct, ushort, 0);
+module_param(idProduct, ushort, 0644);
 MODULE_PARM_DESC(idProduct, "USB Product ID");
 
 static ushort bcdDevice;
-module_param(bcdDevice, ushort, 0);
+module_param(bcdDevice, ushort, 0644);
 MODULE_PARM_DESC(bcdDevice, "USB Device version (BCD)");
 
 static char *iManufacturer;
-module_param(iManufacturer, charp, 0);
+module_param(iManufacturer, charp, 0644);
 MODULE_PARM_DESC(iManufacturer, "USB Manufacturer string");
 
 static char *iProduct;
-module_param(iProduct, charp, 0);
+module_param(iProduct, charp, 0644);
 MODULE_PARM_DESC(iProduct, "USB Product string");
 
 static char *iSerialNumber;
-module_param(iSerialNumber, charp, 0);
+module_param(iSerialNumber, charp, 0644);
 MODULE_PARM_DESC(iSerialNumber, "SerialNumber string");
 
 static char composite_manufacturer[50];
@@ -734,9 +734,23 @@ int usb_add_config(struct usb_composite_dev *cdev,
 
 	INIT_LIST_HEAD(&config->functions);
 	config->next_interface_id = 0;
+	memset(config->interface, 0, sizeof(config->interface));
 
 	status = bind(config);
 	if (status < 0) {
+		while (!list_empty(&config->functions)) {
+			struct usb_function		*f;
+
+			f = list_first_entry(&config->functions,
+					struct usb_function, list);
+			list_del(&f->list);
+			if (f->unbind) {
+				DBG(cdev, "unbind function '%s'/%p\n",
+					f->name, f);
+				f->unbind(config, f);
+				/* may free memory for "f" */
+			}
+		}
 		list_del(&config->list);
 		config->cdev = NULL;
 	} else {
@@ -774,6 +788,53 @@ done:
 	return status;
 }
 
+static void remove_config(struct usb_composite_dev *cdev,
+			      struct usb_configuration *config)
+{
+	while (!list_empty(&config->functions)) {
+		struct usb_function		*f;
+
+		f = list_first_entry(&config->functions,
+				struct usb_function, list);
+		list_del(&f->list);
+		if (f->unbind) {
+			DBG(cdev, "unbind function '%s'/%p\n", f->name, f);
+			f->unbind(config, f);
+			/* may free memory for "f" */
+		}
+	}
+	list_del(&config->list);
+	if (config->unbind) {
+		DBG(cdev, "unbind config '%s'/%p\n", config->label, config);
+		config->unbind(config);
+			/* may free memory for "c" */
+	}
+}
+
+/**
+ * usb_remove_config() - remove a configuration from a device.
+ * @cdev: wraps the USB gadget
+ * @config: the configuration
+ *
+ * Drivers must call usb_gadget_disconnect before calling this function
+ * to disconnect the device from the host and make sure the host will not
+ * try to enumerate the device while we are changing the config list.
+ */
+void usb_remove_config(struct usb_composite_dev *cdev,
+		      struct usb_configuration *config)
+{
+	unsigned long flags;
+
+	spin_lock_irqsave(&cdev->lock, flags);
+
+	if (cdev->config == config)
+		reset_config(cdev);
+
+	spin_unlock_irqrestore(&cdev->lock, flags);
+
+	remove_config(cdev, config);
+}
+
 /*-------------------------------------------------------------------------*/
 
 /* We support strings in multiple languages ... string descriptor zero
@@ -785,7 +846,7 @@ done:
 static void collect_langs(struct usb_gadget_strings **sp, __le16 *buf)
 {
 	const struct usb_gadget_strings	*s;
-	u16				language;
+	__le16				language;
 	__le16				*tmp;
 
 	while (*sp) {
@@ -877,7 +938,7 @@ static int get_string(struct usb_composite_dev *cdev,
 	else if (cdev->product_override == id)
 		str = iProduct ?: composite->iProduct;
 	else if (cdev->serial_override == id)
-		str = iSerialNumber;
+		str = iSerialNumber ?: composite->iSerialNumber;
 	else
 		str = NULL;
 	if (str) {
@@ -1328,28 +1389,9 @@ composite_unbind(struct usb_gadget *gadget)
 
 	while (!list_empty(&cdev->configs)) {
 		struct usb_configuration	*c;
-
 		c = list_first_entry(&cdev->configs,
 				struct usb_configuration, list);
-		while (!list_empty(&c->functions)) {
-			struct usb_function		*f;
-
-			f = list_first_entry(&c->functions,
-					struct usb_function, list);
-			list_del(&f->list);
-			if (f->unbind) {
-				DBG(cdev, "unbind function '%s'/%p\n",
-						f->name, f);
-				f->unbind(c, f);
-				/* may free memory for "f" */
-			}
-		}
-		list_del(&c->list);
-		if (c->unbind) {
-			DBG(cdev, "unbind config '%s'/%p\n", c->label, c);
-			c->unbind(c);
-			/* may free memory for "c" */
-		}
+		remove_config(cdev, c);
 	}
 	if (composite->unbind)
 		composite->unbind(cdev);
@@ -1431,10 +1473,16 @@ static int composite_bind(struct usb_gadget *gadget)
 	/* standardized runtime overrides for device ID data */
 	if (idVendor)
 		cdev->desc.idVendor = cpu_to_le16(idVendor);
+	else
+		idVendor = le16_to_cpu(cdev->desc.idVendor);
 	if (idProduct)
 		cdev->desc.idProduct = cpu_to_le16(idProduct);
+	else
+		idProduct = le16_to_cpu(cdev->desc.idProduct);
 	if (bcdDevice)
 		cdev->desc.bcdDevice = cpu_to_le16(bcdDevice);
+	else
+		bcdDevice = le16_to_cpu(cdev->desc.bcdDevice);
 
 	/* string overrides */
 	if (iManufacturer || !cdev->desc.iManufacturer) {
@@ -1455,7 +1503,8 @@ static int composite_bind(struct usb_gadget *gadget)
 		cdev->product_override =
 			override_id(cdev, &cdev->desc.iProduct);
 
-	if (iSerialNumber)
+	if (iSerialNumber ||
+	    (!cdev->desc.iSerialNumber && composite->iSerialNumber))
 		cdev->serial_override =
 			override_id(cdev, &cdev->desc.iSerialNumber);
 
diff --git a/drivers/usb/gadget/dummy_hcd.c b/drivers/usb/gadget/dummy_hcd.c
index 170cbe89d9f..b799106027a 100644
--- a/drivers/usb/gadget/dummy_hcd.c
+++ b/drivers/usb/gadget/dummy_hcd.c
@@ -595,14 +595,12 @@ static struct usb_request *dummy_alloc_request(struct usb_ep *_ep,
 
 static void dummy_free_request(struct usb_ep *_ep, struct usb_request *_req)
 {
-	struct dummy_ep		*ep;
 	struct dummy_request	*req;
 
-	if (!_ep || !_req)
-		return;
-	ep = usb_ep_to_dummy_ep(_ep);
-	if (!ep->desc && _ep->name != ep0name)
+	if (!_ep || !_req) {
+		WARN_ON(1);
 		return;
+	}
 
 	req = usb_request_to_dummy_request(_req);
 	WARN_ON(!list_empty(&req->queue));
diff --git a/drivers/usb/gadget/f_fs.c b/drivers/usb/gadget/f_fs.c
index f52cb1ae45d..dcd1c7fbb01 100644
--- a/drivers/usb/gadget/f_fs.c
+++ b/drivers/usb/gadget/f_fs.c
@@ -1031,6 +1031,12 @@ struct ffs_sb_fill_data {
 	struct ffs_file_perms perms;
 	umode_t root_mode;
 	const char *dev_name;
+	union {
+		/* set by ffs_fs_mount(), read by ffs_sb_fill() */
+		void *private_data;
+		/* set by ffs_sb_fill(), read by ffs_fs_mount */
+		struct ffs_data *ffs_data;
+	};
 };
 
 static int ffs_sb_fill(struct super_block *sb, void *_data, int silent)
@@ -1047,8 +1053,14 @@ static int ffs_sb_fill(struct super_block *sb, void *_data, int silent)
 		goto Enomem;
 
 	ffs->sb              = sb;
-	ffs->dev_name        = data->dev_name;
+	ffs->dev_name        = kstrdup(data->dev_name, GFP_KERNEL);
+	if (unlikely(!ffs->dev_name))
+		goto Enomem;
 	ffs->file_perms      = data->perms;
+	ffs->private_data    = data->private_data;
+
+	/* used by the caller of this function */
+	data->ffs_data       = ffs;
 
 	sb->s_fs_info        = ffs;
 	sb->s_blocksize      = PAGE_CACHE_SIZE;
@@ -1167,20 +1179,29 @@ ffs_fs_mount(struct file_system_type *t, int flags,
 		},
 		.root_mode = S_IFDIR | 0500,
 	};
+	struct dentry *rv;
 	int ret;
+	void *ffs_dev;
 
 	ENTER();
 
-	ret = functionfs_check_dev_callback(dev_name);
-	if (unlikely(ret < 0))
-		return ERR_PTR(ret);
-
 	ret = ffs_fs_parse_opts(&data, opts);
 	if (unlikely(ret < 0))
 		return ERR_PTR(ret);
 
+	ffs_dev = functionfs_acquire_dev_callback(dev_name);
+	if (IS_ERR(ffs_dev))
+		return ffs_dev;
+
 	data.dev_name = dev_name;
-	return mount_single(t, flags, &data, ffs_sb_fill);
+	data.private_data = ffs_dev;
+	rv = mount_nodev(t, flags, &data, ffs_sb_fill);
+
+	/* data.ffs_data is set by ffs_sb_fill */
+	if (IS_ERR(rv))
+		functionfs_release_dev_callback(data.ffs_data);
+
+	return rv;
 }
 
 static void
@@ -1189,8 +1210,10 @@ ffs_fs_kill_sb(struct super_block *sb)
 	ENTER();
 
 	kill_litter_super(sb);
-	if (sb->s_fs_info)
+	if (sb->s_fs_info) {
+		functionfs_release_dev_callback(sb->s_fs_info);
 		ffs_data_put(sb->s_fs_info);
+	}
 }
 
 static struct file_system_type ffs_fs_type = {
@@ -1256,6 +1279,7 @@ static void ffs_data_put(struct ffs_data *ffs)
 		ffs_data_clear(ffs);
 		BUG_ON(waitqueue_active(&ffs->ev.waitq) ||
 		       waitqueue_active(&ffs->ep0req_completion.wait));
+		kfree(ffs->dev_name);
 		kfree(ffs);
 	}
 }
@@ -1473,8 +1497,22 @@ static int functionfs_bind_config(struct usb_composite_dev *cdev,
 
 static void ffs_func_free(struct ffs_function *func)
 {
+	struct ffs_ep *ep         = func->eps;
+	unsigned count            = func->ffs->eps_count;
+	unsigned long flags;
+
 	ENTER();
 
+	/* cleanup after autoconfig */
+	spin_lock_irqsave(&func->ffs->eps_lock, flags);
+	do {
+		if (ep->ep && ep->req)
+			usb_ep_free_request(ep->ep, ep->req);
+		ep->req = NULL;
+		++ep;
+	} while (--count);
+	spin_unlock_irqrestore(&func->ffs->eps_lock, flags);
+
 	ffs_data_put(func->ffs);
 
 	kfree(func->eps);
diff --git a/drivers/usb/gadget/f_hid.c b/drivers/usb/gadget/f_hid.c
index b2113420b80..3b3932c5536 100644
--- a/drivers/usb/gadget/f_hid.c
+++ b/drivers/usb/gadget/f_hid.c
@@ -374,7 +374,7 @@ static int hidg_setup(struct usb_function *f,
 			break;
 
 		default:
-			VDBG(cdev, "Unknown decriptor request 0x%x\n",
+			VDBG(cdev, "Unknown descriptor request 0x%x\n",
 				 value >> 8);
 			goto stall;
 			break;
diff --git a/drivers/usb/gadget/f_loopback.c b/drivers/usb/gadget/f_loopback.c
index 2c0cd824c66..7275706caeb 100644
--- a/drivers/usb/gadget/f_loopback.c
+++ b/drivers/usb/gadget/f_loopback.c
@@ -286,7 +286,7 @@ static void disable_loopback(struct f_loopback *loop)
 	struct usb_composite_dev	*cdev;
 
 	cdev = loop->function.config->cdev;
-	disable_endpoints(cdev, loop->in_ep, loop->out_ep);
+	disable_endpoints(cdev, loop->in_ep, loop->out_ep, NULL, NULL);
 	VDBG(cdev, "%s disabled\n", loop->function.name);
 }
 
@@ -329,7 +329,7 @@ fail0:
 	 * than 'buflen' bytes each.
 	 */
 	for (i = 0; i < qlen && result == 0; i++) {
-		req = alloc_ep_req(ep);
+		req = alloc_ep_req(ep, 0);
 		if (req) {
 			req->complete = loopback_complete;
 			result = usb_ep_queue(ep, req, GFP_ATOMIC);
diff --git a/drivers/usb/gadget/f_mass_storage.c b/drivers/usb/gadget/f_mass_storage.c
index cb8c162cae5..f67b453740b 100644
--- a/drivers/usb/gadget/f_mass_storage.c
+++ b/drivers/usb/gadget/f_mass_storage.c
@@ -3110,13 +3110,6 @@ static int fsg_bind_config(struct usb_composite_dev *cdev,
 	return rc;
 }
 
-static inline int __deprecated __maybe_unused
-fsg_add(struct usb_composite_dev *cdev, struct usb_configuration *c,
-	struct fsg_common *common)
-{
-	return fsg_bind_config(cdev, c, common);
-}
-
 
 /************************* Module parameters *************************/
 
diff --git a/drivers/usb/gadget/f_rndis.c b/drivers/usb/gadget/f_rndis.c
index 52343654f5d..b1681e45aca 100644
--- a/drivers/usb/gadget/f_rndis.c
+++ b/drivers/usb/gadget/f_rndis.c
@@ -71,6 +71,8 @@ struct f_rndis {
 	struct gether			port;
 	u8				ctrl_id, data_id;
 	u8				ethaddr[ETH_ALEN];
+	u32				vendorID;
+	const char			*manufacturer;
 	int				config;
 
 	struct usb_ep			*notify;
@@ -637,7 +639,7 @@ static void rndis_open(struct gether *geth)
 
 	DBG(cdev, "%s\n", __func__);
 
-	rndis_set_param_medium(rndis->config, NDIS_MEDIUM_802_3,
+	rndis_set_param_medium(rndis->config, RNDIS_MEDIUM_802_3,
 				bitrate(cdev->gadget) / 100);
 	rndis_signal_connect(rndis->config);
 }
@@ -648,7 +650,7 @@ static void rndis_close(struct gether *geth)
 
 	DBG(geth->func.config->cdev, "%s\n", __func__);
 
-	rndis_set_param_medium(rndis->config, NDIS_MEDIUM_802_3, 0);
+	rndis_set_param_medium(rndis->config, RNDIS_MEDIUM_802_3, 0);
 	rndis_signal_disconnect(rndis->config);
 }
 
@@ -765,15 +767,13 @@ rndis_bind(struct usb_configuration *c, struct usb_function *f)
 		goto fail;
 	rndis->config = status;
 
-	rndis_set_param_medium(rndis->config, NDIS_MEDIUM_802_3, 0);
+	rndis_set_param_medium(rndis->config, RNDIS_MEDIUM_802_3, 0);
 	rndis_set_host_mac(rndis->config, rndis->ethaddr);
 
-#if 0
-// FIXME
-	if (rndis_set_param_vendor(rndis->config, vendorID,
-				manufacturer))
-		goto fail0;
-#endif
+	if (rndis->manufacturer && rndis->vendorID &&
+			rndis_set_param_vendor(rndis->config, rndis->vendorID,
+					       rndis->manufacturer))
+		goto fail;
 
 	/* NOTE:  all that is done without knowing or caring about
 	 * the network link ... which is unavailable to this code
@@ -820,6 +820,7 @@ rndis_unbind(struct usb_configuration *c, struct usb_function *f)
 
 	rndis_deregister(rndis->config);
 	rndis_exit();
+	rndis_string_defs[0].id = 0;
 
 	if (gadget_is_superspeed(c->cdev->gadget))
 		usb_free_descriptors(f->ss_descriptors);
@@ -840,20 +841,9 @@ static inline bool can_support_rndis(struct usb_configuration *c)
 	return true;
 }
 
-/**
- * rndis_bind_config - add RNDIS network link to a configuration
- * @c: the configuration to support the network link
- * @ethaddr: a buffer in which the ethernet address of the host side
- *	side of the link was recorded
- * Context: single threaded during gadget setup
- *
- * Returns zero on success, else negative errno.
- *
- * Caller must have called @gether_setup().  Caller is also responsible
- * for calling @gether_cleanup() before module unload.
- */
 int
-rndis_bind_config(struct usb_configuration *c, u8 ethaddr[ETH_ALEN])
+rndis_bind_config_vendor(struct usb_configuration *c, u8 ethaddr[ETH_ALEN],
+				u32 vendorID, const char *manufacturer)
 {
 	struct f_rndis	*rndis;
 	int		status;
@@ -898,6 +888,8 @@ rndis_bind_config(struct usb_configuration *c, u8 ethaddr[ETH_ALEN])
 		goto fail;
 
 	memcpy(rndis->ethaddr, ethaddr, ETH_ALEN);
+	rndis->vendorID = vendorID;
+	rndis->manufacturer = manufacturer;
 
 	/* RNDIS activates when the host changes this filter */
 	rndis->port.cdc_filter = 0;
diff --git a/drivers/usb/gadget/f_sourcesink.c b/drivers/usb/gadget/f_sourcesink.c
index 7aa7ac82c02..5c1b68b63c9 100644
--- a/drivers/usb/gadget/f_sourcesink.c
+++ b/drivers/usb/gadget/f_sourcesink.c
@@ -51,6 +51,9 @@ struct f_sourcesink {
 
 	struct usb_ep		*in_ep;
 	struct usb_ep		*out_ep;
+	struct usb_ep		*iso_in_ep;
+	struct usb_ep		*iso_out_ep;
+	int			cur_alt;
 };
 
 static inline struct f_sourcesink *func_to_ss(struct usb_function *f)
@@ -59,18 +62,45 @@ static inline struct f_sourcesink *func_to_ss(struct usb_function *f)
 }
 
 static unsigned pattern;
-module_param(pattern, uint, 0);
-MODULE_PARM_DESC(pattern, "0 = all zeroes, 1 = mod63 ");
+module_param(pattern, uint, S_IRUGO|S_IWUSR);
+MODULE_PARM_DESC(pattern, "0 = all zeroes, 1 = mod63, 2 = none");
+
+static unsigned isoc_interval = 4;
+module_param(isoc_interval, uint, S_IRUGO|S_IWUSR);
+MODULE_PARM_DESC(isoc_interval, "1 - 16");
+
+static unsigned isoc_maxpacket = 1024;
+module_param(isoc_maxpacket, uint, S_IRUGO|S_IWUSR);
+MODULE_PARM_DESC(isoc_maxpacket, "0 - 1023 (fs), 0 - 1024 (hs/ss)");
+
+static unsigned isoc_mult;
+module_param(isoc_mult, uint, S_IRUGO|S_IWUSR);
+MODULE_PARM_DESC(isoc_mult, "0 - 2 (hs/ss only)");
+
+static unsigned isoc_maxburst;
+module_param(isoc_maxburst, uint, S_IRUGO|S_IWUSR);
+MODULE_PARM_DESC(isoc_maxburst, "0 - 15 (ss only)");
 
 /*-------------------------------------------------------------------------*/
 
-static struct usb_interface_descriptor source_sink_intf = {
-	.bLength =		sizeof source_sink_intf,
+static struct usb_interface_descriptor source_sink_intf_alt0 = {
+	.bLength =		USB_DT_INTERFACE_SIZE,
 	.bDescriptorType =	USB_DT_INTERFACE,
 
+	.bAlternateSetting =	0,
 	.bNumEndpoints =	2,
 	.bInterfaceClass =	USB_CLASS_VENDOR_SPEC,
-	/* .iInterface = DYNAMIC */
+	/* .iInterface		= DYNAMIC */
+};
+
+static struct usb_interface_descriptor source_sink_intf_alt1 = {
+	.bLength =		USB_DT_INTERFACE_SIZE,
+	.bDescriptorType =	USB_DT_INTERFACE,
+
+	.bAlternateSetting =	1,
+	.bNumEndpoints =	4,
+	.bInterfaceClass =	USB_CLASS_VENDOR_SPEC,
+	/* .iInterface		= DYNAMIC */
 };
 
 /* full speed support: */
@@ -91,10 +121,36 @@ static struct usb_endpoint_descriptor fs_sink_desc = {
 	.bmAttributes =		USB_ENDPOINT_XFER_BULK,
 };
 
+static struct usb_endpoint_descriptor fs_iso_source_desc = {
+	.bLength =		USB_DT_ENDPOINT_SIZE,
+	.bDescriptorType =	USB_DT_ENDPOINT,
+
+	.bEndpointAddress =	USB_DIR_IN,
+	.bmAttributes =		USB_ENDPOINT_XFER_ISOC,
+	.wMaxPacketSize =	cpu_to_le16(1023),
+	.bInterval =		4,
+};
+
+static struct usb_endpoint_descriptor fs_iso_sink_desc = {
+	.bLength =		USB_DT_ENDPOINT_SIZE,
+	.bDescriptorType =	USB_DT_ENDPOINT,
+
+	.bEndpointAddress =	USB_DIR_OUT,
+	.bmAttributes =		USB_ENDPOINT_XFER_ISOC,
+	.wMaxPacketSize =	cpu_to_le16(1023),
+	.bInterval =		4,
+};
+
 static struct usb_descriptor_header *fs_source_sink_descs[] = {
-	(struct usb_descriptor_header *) &source_sink_intf,
+	(struct usb_descriptor_header *) &source_sink_intf_alt0,
 	(struct usb_descriptor_header *) &fs_sink_desc,
 	(struct usb_descriptor_header *) &fs_source_desc,
+	(struct usb_descriptor_header *) &source_sink_intf_alt1,
+#define FS_ALT_IFC_1_OFFSET	3
+	(struct usb_descriptor_header *) &fs_sink_desc,
+	(struct usb_descriptor_header *) &fs_source_desc,
+	(struct usb_descriptor_header *) &fs_iso_sink_desc,
+	(struct usb_descriptor_header *) &fs_iso_source_desc,
 	NULL,
 };
 
@@ -116,10 +172,34 @@ static struct usb_endpoint_descriptor hs_sink_desc = {
 	.wMaxPacketSize =	cpu_to_le16(512),
 };
 
+static struct usb_endpoint_descriptor hs_iso_source_desc = {
+	.bLength =		USB_DT_ENDPOINT_SIZE,
+	.bDescriptorType =	USB_DT_ENDPOINT,
+
+	.bmAttributes =		USB_ENDPOINT_XFER_ISOC,
+	.wMaxPacketSize =	cpu_to_le16(1024),
+	.bInterval =		4,
+};
+
+static struct usb_endpoint_descriptor hs_iso_sink_desc = {
+	.bLength =		USB_DT_ENDPOINT_SIZE,
+	.bDescriptorType =	USB_DT_ENDPOINT,
+
+	.bmAttributes =		USB_ENDPOINT_XFER_ISOC,
+	.wMaxPacketSize =	cpu_to_le16(1024),
+	.bInterval =		4,
+};
+
 static struct usb_descriptor_header *hs_source_sink_descs[] = {
-	(struct usb_descriptor_header *) &source_sink_intf,
+	(struct usb_descriptor_header *) &source_sink_intf_alt0,
 	(struct usb_descriptor_header *) &hs_source_desc,
 	(struct usb_descriptor_header *) &hs_sink_desc,
+	(struct usb_descriptor_header *) &source_sink_intf_alt1,
+#define HS_ALT_IFC_1_OFFSET	3
+	(struct usb_descriptor_header *) &hs_source_desc,
+	(struct usb_descriptor_header *) &hs_sink_desc,
+	(struct usb_descriptor_header *) &hs_iso_source_desc,
+	(struct usb_descriptor_header *) &hs_iso_sink_desc,
 	NULL,
 };
 
@@ -136,6 +216,7 @@ static struct usb_endpoint_descriptor ss_source_desc = {
 struct usb_ss_ep_comp_descriptor ss_source_comp_desc = {
 	.bLength =		USB_DT_SS_EP_COMP_SIZE,
 	.bDescriptorType =	USB_DT_SS_ENDPOINT_COMP,
+
 	.bMaxBurst =		0,
 	.bmAttributes =		0,
 	.wBytesPerInterval =	0,
@@ -152,17 +233,64 @@ static struct usb_endpoint_descriptor ss_sink_desc = {
 struct usb_ss_ep_comp_descriptor ss_sink_comp_desc = {
 	.bLength =		USB_DT_SS_EP_COMP_SIZE,
 	.bDescriptorType =	USB_DT_SS_ENDPOINT_COMP,
+
 	.bMaxBurst =		0,
 	.bmAttributes =		0,
 	.wBytesPerInterval =	0,
 };
 
+static struct usb_endpoint_descriptor ss_iso_source_desc = {
+	.bLength =		USB_DT_ENDPOINT_SIZE,
+	.bDescriptorType =	USB_DT_ENDPOINT,
+
+	.bmAttributes =		USB_ENDPOINT_XFER_ISOC,
+	.wMaxPacketSize =	cpu_to_le16(1024),
+	.bInterval =		4,
+};
+
+struct usb_ss_ep_comp_descriptor ss_iso_source_comp_desc = {
+	.bLength =		USB_DT_SS_EP_COMP_SIZE,
+	.bDescriptorType =	USB_DT_SS_ENDPOINT_COMP,
+
+	.bMaxBurst =		0,
+	.bmAttributes =		0,
+	.wBytesPerInterval =	cpu_to_le16(1024),
+};
+
+static struct usb_endpoint_descriptor ss_iso_sink_desc = {
+	.bLength =		USB_DT_ENDPOINT_SIZE,
+	.bDescriptorType =	USB_DT_ENDPOINT,
+
+	.bmAttributes =		USB_ENDPOINT_XFER_ISOC,
+	.wMaxPacketSize =	cpu_to_le16(1024),
+	.bInterval =		4,
+};
+
+struct usb_ss_ep_comp_descriptor ss_iso_sink_comp_desc = {
+	.bLength =		USB_DT_SS_EP_COMP_SIZE,
+	.bDescriptorType =	USB_DT_SS_ENDPOINT_COMP,
+
+	.bMaxBurst =		0,
+	.bmAttributes =		0,
+	.wBytesPerInterval =	cpu_to_le16(1024),
+};
+
 static struct usb_descriptor_header *ss_source_sink_descs[] = {
-	(struct usb_descriptor_header *) &source_sink_intf,
+	(struct usb_descriptor_header *) &source_sink_intf_alt0,
 	(struct usb_descriptor_header *) &ss_source_desc,
 	(struct usb_descriptor_header *) &ss_source_comp_desc,
 	(struct usb_descriptor_header *) &ss_sink_desc,
 	(struct usb_descriptor_header *) &ss_sink_comp_desc,
+	(struct usb_descriptor_header *) &source_sink_intf_alt1,
+#define SS_ALT_IFC_1_OFFSET	5
+	(struct usb_descriptor_header *) &ss_source_desc,
+	(struct usb_descriptor_header *) &ss_source_comp_desc,
+	(struct usb_descriptor_header *) &ss_sink_desc,
+	(struct usb_descriptor_header *) &ss_sink_comp_desc,
+	(struct usb_descriptor_header *) &ss_iso_source_desc,
+	(struct usb_descriptor_header *) &ss_iso_source_comp_desc,
+	(struct usb_descriptor_header *) &ss_iso_sink_desc,
+	(struct usb_descriptor_header *) &ss_iso_sink_comp_desc,
 	NULL,
 };
 
@@ -196,9 +324,10 @@ sourcesink_bind(struct usb_configuration *c, struct usb_function *f)
 	id = usb_interface_id(c, f);
 	if (id < 0)
 		return id;
-	source_sink_intf.bInterfaceNumber = id;
+	source_sink_intf_alt0.bInterfaceNumber = id;
+	source_sink_intf_alt1.bInterfaceNumber = id;
 
-	/* allocate endpoints */
+	/* allocate bulk endpoints */
 	ss->in_ep = usb_ep_autoconfig(cdev->gadget, &fs_source_desc);
 	if (!ss->in_ep) {
 autoconf_fail:
@@ -213,12 +342,74 @@ autoconf_fail:
 		goto autoconf_fail;
 	ss->out_ep->driver_data = cdev;	/* claim */
 
+	/* sanity check the isoc module parameters */
+	if (isoc_interval < 1)
+		isoc_interval = 1;
+	if (isoc_interval > 16)
+		isoc_interval = 16;
+	if (isoc_mult > 2)
+		isoc_mult = 2;
+	if (isoc_maxburst > 15)
+		isoc_maxburst = 15;
+
+	/* fill in the FS isoc descriptors from the module parameters */
+	fs_iso_source_desc.wMaxPacketSize = isoc_maxpacket > 1023 ?
+						1023 : isoc_maxpacket;
+	fs_iso_source_desc.bInterval = isoc_interval;
+	fs_iso_sink_desc.wMaxPacketSize = isoc_maxpacket > 1023 ?
+						1023 : isoc_maxpacket;
+	fs_iso_sink_desc.bInterval = isoc_interval;
+
+	/* allocate iso endpoints */
+	ss->iso_in_ep = usb_ep_autoconfig(cdev->gadget, &fs_iso_source_desc);
+	if (!ss->iso_in_ep)
+		goto no_iso;
+	ss->iso_in_ep->driver_data = cdev;	/* claim */
+
+	ss->iso_out_ep = usb_ep_autoconfig(cdev->gadget, &fs_iso_sink_desc);
+	if (ss->iso_out_ep) {
+		ss->iso_out_ep->driver_data = cdev;	/* claim */
+	} else {
+		ss->iso_in_ep->driver_data = NULL;
+		ss->iso_in_ep = NULL;
+no_iso:
+		/*
+		 * We still want to work even if the UDC doesn't have isoc
+		 * endpoints, so null out the alt interface that contains
+		 * them and continue.
+		 */
+		fs_source_sink_descs[FS_ALT_IFC_1_OFFSET] = NULL;
+		hs_source_sink_descs[HS_ALT_IFC_1_OFFSET] = NULL;
+		ss_source_sink_descs[SS_ALT_IFC_1_OFFSET] = NULL;
+	}
+
+	if (isoc_maxpacket > 1024)
+		isoc_maxpacket = 1024;
+
 	/* support high speed hardware */
 	if (gadget_is_dualspeed(c->cdev->gadget)) {
 		hs_source_desc.bEndpointAddress =
 				fs_source_desc.bEndpointAddress;
 		hs_sink_desc.bEndpointAddress =
 				fs_sink_desc.bEndpointAddress;
+
+		/*
+		 * Fill in the HS isoc descriptors from the module parameters.
+		 * We assume that the user knows what they are doing and won't
+		 * give parameters that their UDC doesn't support.
+		 */
+		hs_iso_source_desc.wMaxPacketSize = isoc_maxpacket;
+		hs_iso_source_desc.wMaxPacketSize |= isoc_mult << 11;
+		hs_iso_source_desc.bInterval = isoc_interval;
+		hs_iso_source_desc.bEndpointAddress =
+				fs_iso_source_desc.bEndpointAddress;
+
+		hs_iso_sink_desc.wMaxPacketSize = isoc_maxpacket;
+		hs_iso_sink_desc.wMaxPacketSize |= isoc_mult << 11;
+		hs_iso_sink_desc.bInterval = isoc_interval;
+		hs_iso_sink_desc.bEndpointAddress =
+				fs_iso_sink_desc.bEndpointAddress;
+
 		f->hs_descriptors = hs_source_sink_descs;
 	}
 
@@ -228,13 +419,39 @@ autoconf_fail:
 				fs_source_desc.bEndpointAddress;
 		ss_sink_desc.bEndpointAddress =
 				fs_sink_desc.bEndpointAddress;
+
+		/*
+		 * Fill in the SS isoc descriptors from the module parameters.
+		 * We assume that the user knows what they are doing and won't
+		 * give parameters that their UDC doesn't support.
+		 */
+		ss_iso_source_desc.wMaxPacketSize = isoc_maxpacket;
+		ss_iso_source_desc.bInterval = isoc_interval;
+		ss_iso_source_comp_desc.bmAttributes = isoc_mult;
+		ss_iso_source_comp_desc.bMaxBurst = isoc_maxburst;
+		ss_iso_source_comp_desc.wBytesPerInterval =
+			isoc_maxpacket * (isoc_mult + 1) * (isoc_maxburst + 1);
+		ss_iso_source_desc.bEndpointAddress =
+				fs_iso_source_desc.bEndpointAddress;
+
+		ss_iso_sink_desc.wMaxPacketSize = isoc_maxpacket;
+		ss_iso_sink_desc.bInterval = isoc_interval;
+		ss_iso_sink_comp_desc.bmAttributes = isoc_mult;
+		ss_iso_sink_comp_desc.bMaxBurst = isoc_maxburst;
+		ss_iso_sink_comp_desc.wBytesPerInterval =
+			isoc_maxpacket * (isoc_mult + 1) * (isoc_maxburst + 1);
+		ss_iso_sink_desc.bEndpointAddress =
+				fs_iso_sink_desc.bEndpointAddress;
+
 		f->ss_descriptors = ss_source_sink_descs;
 	}
 
-	DBG(cdev, "%s speed %s: IN/%s, OUT/%s\n",
+	DBG(cdev, "%s speed %s: IN/%s, OUT/%s, ISO-IN/%s, ISO-OUT/%s\n",
 	    (gadget_is_superspeed(c->cdev->gadget) ? "super" :
 	     (gadget_is_dualspeed(c->cdev->gadget) ? "dual" : "full")),
-			f->name, ss->in_ep->name, ss->out_ep->name);
+			f->name, ss->in_ep->name, ss->out_ep->name,
+			ss->iso_in_ep ? ss->iso_in_ep->name : "<none>",
+			ss->iso_out_ep ? ss->iso_out_ep->name : "<none>");
 	return 0;
 }
 
@@ -251,6 +468,9 @@ static int check_read_data(struct f_sourcesink *ss, struct usb_request *req)
 	u8			*buf = req->buf;
 	struct usb_composite_dev *cdev = ss->function.config->cdev;
 
+	if (pattern == 2)
+		return 0;
+
 	for (i = 0; i < req->actual; i++, buf++) {
 		switch (pattern) {
 
@@ -265,7 +485,7 @@ static int check_read_data(struct f_sourcesink *ss, struct usb_request *req)
 		 * each usb transfer request should be.  Resync is done
 		 * with set_interface or set_config.  (We *WANT* it to
 		 * get quickly out of sync if controllers or their drivers
-		 * stutter for any reason, including buffer duplcation...)
+		 * stutter for any reason, including buffer duplication...)
 		 */
 		case 1:
 			if (*buf == (u8)(i % 63))
@@ -292,21 +512,30 @@ static void reinit_write_data(struct usb_ep *ep, struct usb_request *req)
 		for  (i = 0; i < req->length; i++)
 			*buf++ = (u8) (i % 63);
 		break;
+	case 2:
+		break;
 	}
 }
 
 static void source_sink_complete(struct usb_ep *ep, struct usb_request *req)
 {
-	struct f_sourcesink	*ss = ep->driver_data;
-	struct usb_composite_dev *cdev = ss->function.config->cdev;
-	int			status = req->status;
+	struct usb_composite_dev	*cdev;
+	struct f_sourcesink		*ss = ep->driver_data;
+	int				status = req->status;
+
+	/* driver_data will be null if ep has been disabled */
+	if (!ss)
+		return;
+
+	cdev = ss->function.config->cdev;
 
 	switch (status) {
 
 	case 0:				/* normal completion? */
 		if (ep == ss->out_ep) {
 			check_read_data(ss, req);
-			memset(req->buf, 0x55, req->length);
+			if (pattern != 2)
+				memset(req->buf, 0x55, req->length);
 		} else
 			reinit_write_data(ep, req);
 		break;
@@ -344,32 +573,57 @@ static void source_sink_complete(struct usb_ep *ep, struct usb_request *req)
 	}
 }
 
-static int source_sink_start_ep(struct f_sourcesink *ss, bool is_in)
+static int source_sink_start_ep(struct f_sourcesink *ss, bool is_in,
+		bool is_iso, int speed)
 {
 	struct usb_ep		*ep;
 	struct usb_request	*req;
-	int			status;
+	int			i, size, status;
+
+	for (i = 0; i < 8; i++) {
+		if (is_iso) {
+			switch (speed) {
+			case USB_SPEED_SUPER:
+				size = isoc_maxpacket * (isoc_mult + 1) *
+						(isoc_maxburst + 1);
+				break;
+			case USB_SPEED_HIGH:
+				size = isoc_maxpacket * (isoc_mult + 1);
+				break;
+			default:
+				size = isoc_maxpacket > 1023 ?
+						1023 : isoc_maxpacket;
+				break;
+			}
+			ep = is_in ? ss->iso_in_ep : ss->iso_out_ep;
+			req = alloc_ep_req(ep, size);
+		} else {
+			ep = is_in ? ss->in_ep : ss->out_ep;
+			req = alloc_ep_req(ep, 0);
+		}
 
-	ep = is_in ? ss->in_ep : ss->out_ep;
-	req = alloc_ep_req(ep);
-	if (!req)
-		return -ENOMEM;
+		if (!req)
+			return -ENOMEM;
 
-	req->complete = source_sink_complete;
-	if (is_in)
-		reinit_write_data(ep, req);
-	else
-		memset(req->buf, 0x55, req->length);
+		req->complete = source_sink_complete;
+		if (is_in)
+			reinit_write_data(ep, req);
+		else if (pattern != 2)
+			memset(req->buf, 0x55, req->length);
 
-	status = usb_ep_queue(ep, req, GFP_ATOMIC);
-	if (status) {
-		struct usb_composite_dev	*cdev;
+		status = usb_ep_queue(ep, req, GFP_ATOMIC);
+		if (status) {
+			struct usb_composite_dev	*cdev;
 
-		cdev = ss->function.config->cdev;
-		ERROR(cdev, "start %s %s --> %d\n",
-				is_in ? "IN" : "OUT",
-				ep->name, status);
-		free_ep_req(ep, req);
+			cdev = ss->function.config->cdev;
+			ERROR(cdev, "start %s%s %s --> %d\n",
+			      is_iso ? "ISO-" : "", is_in ? "IN" : "OUT",
+			      ep->name, status);
+			free_ep_req(ep, req);
+		}
+
+		if (!is_iso)
+			break;
 	}
 
 	return status;
@@ -380,17 +634,20 @@ static void disable_source_sink(struct f_sourcesink *ss)
 	struct usb_composite_dev	*cdev;
 
 	cdev = ss->function.config->cdev;
-	disable_endpoints(cdev, ss->in_ep, ss->out_ep);
+	disable_endpoints(cdev, ss->in_ep, ss->out_ep, ss->iso_in_ep,
+			ss->iso_out_ep);
 	VDBG(cdev, "%s disabled\n", ss->function.name);
 }
 
 static int
-enable_source_sink(struct usb_composite_dev *cdev, struct f_sourcesink *ss)
+enable_source_sink(struct usb_composite_dev *cdev, struct f_sourcesink *ss,
+		int alt)
 {
 	int					result = 0;
+	int					speed = cdev->gadget->speed;
 	struct usb_ep				*ep;
 
-	/* one endpoint writes (sources) zeroes IN (to the host) */
+	/* one bulk endpoint writes (sources) zeroes IN (to the host) */
 	ep = ss->in_ep;
 	result = config_ep_by_speed(cdev->gadget, &(ss->function), ep);
 	if (result)
@@ -400,7 +657,7 @@ enable_source_sink(struct usb_composite_dev *cdev, struct f_sourcesink *ss)
 		return result;
 	ep->driver_data = ss;
 
-	result = source_sink_start_ep(ss, true);
+	result = source_sink_start_ep(ss, true, false, speed);
 	if (result < 0) {
 fail:
 		ep = ss->in_ep;
@@ -409,7 +666,7 @@ fail:
 		return result;
 	}
 
-	/* one endpoint reads (sinks) anything OUT (from the host) */
+	/* one bulk endpoint reads (sinks) anything OUT (from the host) */
 	ep = ss->out_ep;
 	result = config_ep_by_speed(cdev->gadget, &(ss->function), ep);
 	if (result)
@@ -419,27 +676,82 @@ fail:
 		goto fail;
 	ep->driver_data = ss;
 
-	result = source_sink_start_ep(ss, false);
+	result = source_sink_start_ep(ss, false, false, speed);
 	if (result < 0) {
+fail2:
+		ep = ss->out_ep;
 		usb_ep_disable(ep);
 		ep->driver_data = NULL;
 		goto fail;
 	}
 
-	DBG(cdev, "%s enabled\n", ss->function.name);
+	if (alt == 0)
+		goto out;
+
+	/* one iso endpoint writes (sources) zeroes IN (to the host) */
+	ep = ss->iso_in_ep;
+	if (ep) {
+		result = config_ep_by_speed(cdev->gadget, &(ss->function), ep);
+		if (result)
+			goto fail2;
+		result = usb_ep_enable(ep);
+		if (result < 0)
+			goto fail2;
+		ep->driver_data = ss;
+
+		result = source_sink_start_ep(ss, true, true, speed);
+		if (result < 0) {
+fail3:
+			ep = ss->iso_in_ep;
+			if (ep) {
+				usb_ep_disable(ep);
+				ep->driver_data = NULL;
+			}
+			goto fail2;
+		}
+	}
+
+	/* one iso endpoint reads (sinks) anything OUT (from the host) */
+	ep = ss->iso_out_ep;
+	if (ep) {
+		result = config_ep_by_speed(cdev->gadget, &(ss->function), ep);
+		if (result)
+			goto fail3;
+		result = usb_ep_enable(ep);
+		if (result < 0)
+			goto fail3;
+		ep->driver_data = ss;
+
+		result = source_sink_start_ep(ss, false, true, speed);
+		if (result < 0) {
+			usb_ep_disable(ep);
+			ep->driver_data = NULL;
+			goto fail3;
+		}
+	}
+out:
+	ss->cur_alt = alt;
+
+	DBG(cdev, "%s enabled, alt intf %d\n", ss->function.name, alt);
 	return result;
 }
 
 static int sourcesink_set_alt(struct usb_function *f,
 		unsigned intf, unsigned alt)
 {
-	struct f_sourcesink	*ss = func_to_ss(f);
-	struct usb_composite_dev *cdev = f->config->cdev;
+	struct f_sourcesink		*ss = func_to_ss(f);
+	struct usb_composite_dev	*cdev = f->config->cdev;
 
-	/* we know alt is zero */
 	if (ss->in_ep->driver_data)
 		disable_source_sink(ss);
-	return enable_source_sink(cdev, ss);
+	return enable_source_sink(cdev, ss, alt);
+}
+
+static int sourcesink_get_alt(struct usb_function *f, unsigned intf)
+{
+	struct f_sourcesink		*ss = func_to_ss(f);
+
+	return ss->cur_alt;
 }
 
 static void sourcesink_disable(struct usb_function *f)
@@ -465,6 +777,7 @@ static int __init sourcesink_bind_config(struct usb_configuration *c)
 	ss->function.bind = sourcesink_bind;
 	ss->function.unbind = sourcesink_unbind;
 	ss->function.set_alt = sourcesink_set_alt;
+	ss->function.get_alt = sourcesink_get_alt;
 	ss->function.disable = sourcesink_disable;
 
 	status = usb_add_function(c, &ss->function);
@@ -536,7 +849,7 @@ unknown:
 		req->length = value;
 		value = usb_ep_queue(c->cdev->gadget->ep0, req, GFP_ATOMIC);
 		if (value < 0)
-			ERROR(c->cdev, "source/sinkc response, err %d\n",
+			ERROR(c->cdev, "source/sink response, err %d\n",
 					value);
 	}
 
@@ -545,12 +858,12 @@ unknown:
 }
 
 static struct usb_configuration sourcesink_driver = {
-	.label		= "source/sink",
-	.strings	= sourcesink_strings,
-	.setup		= sourcesink_setup,
-	.bConfigurationValue = 3,
-	.bmAttributes	= USB_CONFIG_ATT_SELFPOWER,
-	/* .iConfiguration = DYNAMIC */
+	.label			= "source/sink",
+	.strings		= sourcesink_strings,
+	.setup			= sourcesink_setup,
+	.bConfigurationValue	= 3,
+	.bmAttributes		= USB_CONFIG_ATT_SELFPOWER,
+	/* .iConfiguration	= DYNAMIC */
 };
 
 /**
@@ -567,7 +880,8 @@ int __init sourcesink_add(struct usb_composite_dev *cdev, bool autoresume)
 		return id;
 	strings_sourcesink[0].id = id;
 
-	source_sink_intf.iInterface = id;
+	source_sink_intf_alt0.iInterface = id;
+	source_sink_intf_alt1.iInterface = id;
 	sourcesink_driver.iConfiguration = id;
 
 	/* support autoresume for remote wakeup testing */
diff --git a/drivers/usb/gadget/fsl_qe_udc.c b/drivers/usb/gadget/fsl_qe_udc.c
index 877a2c46672..51881f3bd07 100644
--- a/drivers/usb/gadget/fsl_qe_udc.c
+++ b/drivers/usb/gadget/fsl_qe_udc.c
@@ -71,9 +71,6 @@ static struct usb_endpoint_descriptor qe_ep0_desc = {
 	.wMaxPacketSize =	USB_MAX_CTRL_PAYLOAD,
 };
 
-/* it is initialized in probe()  */
-static struct qe_udc *udc_controller;
-
 /********************************************************************
  *      Internal Used Function Start
 ********************************************************************/
@@ -188,8 +185,8 @@ static int qe_ep0_stall(struct qe_udc *udc)
 {
 	qe_eptx_stall_change(&udc->eps[0], 1);
 	qe_eprx_stall_change(&udc->eps[0], 1);
-	udc_controller->ep0_state = WAIT_FOR_SETUP;
-	udc_controller->ep0_dir = 0;
+	udc->ep0_state = WAIT_FOR_SETUP;
+	udc->ep0_dir = 0;
 	return 0;
 }
 
@@ -450,13 +447,13 @@ static int qe_ep_rxbd_update(struct qe_ep *ep)
 
 	ep->rxbuf_d = virt_to_phys((void *)ep->rxbuffer);
 	if (ep->rxbuf_d == DMA_ADDR_INVALID) {
-		ep->rxbuf_d = dma_map_single(udc_controller->gadget.dev.parent,
+		ep->rxbuf_d = dma_map_single(ep->udc->gadget.dev.parent,
 					ep->rxbuffer,
 					size,
 					DMA_FROM_DEVICE);
 		ep->rxbufmap = 1;
 	} else {
-		dma_sync_single_for_device(udc_controller->gadget.dev.parent,
+		dma_sync_single_for_device(ep->udc->gadget.dev.parent,
 					ep->rxbuf_d, size,
 					DMA_FROM_DEVICE);
 		ep->rxbufmap = 0;
@@ -489,10 +486,10 @@ static int qe_ep_register_init(struct qe_udc *udc, unsigned char pipe_num)
 	epparam = udc->ep_param[pipe_num];
 
 	usep = 0;
-	logepnum = (ep->desc->bEndpointAddress & USB_ENDPOINT_NUMBER_MASK);
+	logepnum = (ep->ep.desc->bEndpointAddress & USB_ENDPOINT_NUMBER_MASK);
 	usep |= (logepnum << USB_EPNUM_SHIFT);
 
-	switch (ep->desc->bmAttributes & 0x03) {
+	switch (ep->ep.desc->bmAttributes & 0x03) {
 	case USB_ENDPOINT_XFER_BULK:
 		usep |= USB_TRANS_BULK;
 		break;
@@ -644,7 +641,7 @@ static int qe_ep_init(struct qe_udc *udc,
 	/* initialize ep structure */
 	ep->ep.maxpacket = max;
 	ep->tm = (u8)(desc->bmAttributes & USB_ENDPOINT_XFERTYPE_MASK);
-	ep->desc = desc;
+	ep->ep.desc = desc;
 	ep->stopped = 0;
 	ep->init = 1;
 
@@ -698,14 +695,14 @@ en_done:
 	return -ENODEV;
 }
 
-static inline void qe_usb_enable(void)
+static inline void qe_usb_enable(struct qe_udc *udc)
 {
-	setbits8(&udc_controller->usb_regs->usb_usmod, USB_MODE_EN);
+	setbits8(&udc->usb_regs->usb_usmod, USB_MODE_EN);
 }
 
-static inline void qe_usb_disable(void)
+static inline void qe_usb_disable(struct qe_udc *udc)
 {
-	clrbits8(&udc_controller->usb_regs->usb_usmod, USB_MODE_EN);
+	clrbits8(&udc->usb_regs->usb_usmod, USB_MODE_EN);
 }
 
 /*----------------------------------------------------------------------------*
@@ -1599,7 +1596,7 @@ static int qe_ep_enable(struct usb_ep *_ep,
 	ep = container_of(_ep, struct qe_ep, ep);
 
 	/* catch various bogus parameters */
-	if (!_ep || !desc || ep->desc || _ep->name == ep_name[0] ||
+	if (!_ep || !desc || ep->ep.desc || _ep->name == ep_name[0] ||
 			(desc->bDescriptorType != USB_DT_ENDPOINT))
 		return -EINVAL;
 
@@ -1629,7 +1626,7 @@ static int qe_ep_disable(struct usb_ep *_ep)
 	ep = container_of(_ep, struct qe_ep, ep);
 	udc = ep->udc;
 
-	if (!_ep || !ep->desc) {
+	if (!_ep || !ep->ep.desc) {
 		dev_dbg(udc->dev, "%s not enabled\n", _ep ? ep->ep.name : NULL);
 		return -EINVAL;
 	}
@@ -1637,7 +1634,6 @@ static int qe_ep_disable(struct usb_ep *_ep)
 	spin_lock_irqsave(&udc->lock, flags);
 	/* Nuke all pending requests (does flush) */
 	nuke(ep, -ESHUTDOWN);
-	ep->desc = NULL;
 	ep->ep.desc = NULL;
 	ep->stopped = 1;
 	ep->tx_req = NULL;
@@ -1656,13 +1652,13 @@ static int qe_ep_disable(struct usb_ep *_ep)
 	if (ep->dir != USB_DIR_IN) {
 		kfree(ep->rxframe);
 		if (ep->rxbufmap) {
-			dma_unmap_single(udc_controller->gadget.dev.parent,
+			dma_unmap_single(udc->gadget.dev.parent,
 					ep->rxbuf_d, size,
 					DMA_FROM_DEVICE);
 			ep->rxbuf_d = DMA_ADDR_INVALID;
 		} else {
 			dma_sync_single_for_cpu(
-					udc_controller->gadget.dev.parent,
+					udc->gadget.dev.parent,
 					ep->rxbuf_d, size,
 					DMA_FROM_DEVICE);
 		}
@@ -1715,7 +1711,7 @@ static int __qe_ep_queue(struct usb_ep *_ep, struct usb_request *_req)
 		dev_dbg(udc->dev, "bad params\n");
 		return -EINVAL;
 	}
-	if (!_ep || (!ep->desc && ep_index(ep))) {
+	if (!_ep || (!ep->ep.desc && ep_index(ep))) {
 		dev_dbg(udc->dev, "bad ep\n");
 		return -EINVAL;
 	}
@@ -1826,7 +1822,7 @@ static int qe_ep_set_halt(struct usb_ep *_ep, int value)
 	struct qe_udc *udc;
 
 	ep = container_of(_ep, struct qe_ep, ep);
-	if (!_ep || !ep->desc) {
+	if (!_ep || !ep->ep.desc) {
 		status = -EINVAL;
 		goto out;
 	}
@@ -1880,9 +1876,10 @@ static struct usb_ep_ops qe_ep_ops = {
 /* Get the current frame number */
 static int qe_get_frame(struct usb_gadget *gadget)
 {
+	struct qe_udc *udc = container_of(gadget, struct qe_udc, gadget);
 	u16 tmp;
 
-	tmp = in_be16(&udc_controller->usb_param->frame_n);
+	tmp = in_be16(&udc->usb_param->frame_n);
 	if (tmp & 0x8000)
 		tmp = tmp & 0x07ff;
 	else
@@ -1891,57 +1888,16 @@ static int qe_get_frame(struct usb_gadget *gadget)
 	return (int)tmp;
 }
 
-/* Tries to wake up the host connected to this gadget
- *
- * Return : 0-success
- * Negative-this feature not enabled by host or not supported by device hw
- */
-static int qe_wakeup(struct usb_gadget *gadget)
-{
-	return -ENOTSUPP;
-}
-
-/* Notify controller that VBUS is powered, Called by whatever
-   detects VBUS sessions */
-static int qe_vbus_session(struct usb_gadget *gadget, int is_active)
-{
-	return -ENOTSUPP;
-}
-
-/* constrain controller's VBUS power usage
- * This call is used by gadget drivers during SET_CONFIGURATION calls,
- * reporting how much power the device may consume.  For example, this
- * could affect how quickly batteries are recharged.
- *
- * Returns zero on success, else negative errno.
- */
-static int qe_vbus_draw(struct usb_gadget *gadget, unsigned mA)
-{
-	return -ENOTSUPP;
-}
-
-/* Change Data+ pullup status
- * this func is used by usb_gadget_connect/disconnect
- */
-static int qe_pullup(struct usb_gadget *gadget, int is_on)
-{
-	return -ENOTSUPP;
-}
-
-static int fsl_qe_start(struct usb_gadget_driver *driver,
-		int (*bind)(struct usb_gadget *));
-static int fsl_qe_stop(struct usb_gadget_driver *driver);
+static int fsl_qe_start(struct usb_gadget *gadget,
+		struct usb_gadget_driver *driver);
+static int fsl_qe_stop(struct usb_gadget *gadget,
+		struct usb_gadget_driver *driver);
 
 /* defined in usb_gadget.h */
 static struct usb_gadget_ops qe_gadget_ops = {
 	.get_frame = qe_get_frame,
-	.wakeup = qe_wakeup,
-/*	.set_selfpowered = qe_set_selfpowered,*/ /* always selfpowered */
-	.vbus_session = qe_vbus_session,
-	.vbus_draw = qe_vbus_draw,
-	.pullup = qe_pullup,
-	.start = fsl_qe_start,
-	.stop = fsl_qe_stop,
+	.udc_start = fsl_qe_start,
+	.udc_stop = fsl_qe_stop,
 };
 
 /*-------------------------------------------------------------------------
@@ -2015,7 +1971,7 @@ static void ch9getstatus(struct qe_udc *udc, u8 request_type, u16 value,
 		u16 usep;
 
 		/* stall if endpoint doesn't exist */
-		if (!target_ep->desc)
+		if (!target_ep->ep.desc)
 			goto stall;
 
 		usep = in_be16(&udc->usb_regs->usb_usep[pipe]);
@@ -2190,7 +2146,7 @@ static int reset_irq(struct qe_udc *udc)
 	if (udc->usb_state == USB_STATE_DEFAULT)
 		return 0;
 
-	qe_usb_disable();
+	qe_usb_disable(udc);
 	out_8(&udc->usb_regs->usb_usadr, 0);
 
 	for (i = 0; i < USB_MAX_ENDPOINTS; i++) {
@@ -2202,7 +2158,7 @@ static int reset_irq(struct qe_udc *udc)
 	udc->usb_state = USB_STATE_DEFAULT;
 	udc->ep0_state = WAIT_FOR_SETUP;
 	udc->ep0_dir = USB_DIR_OUT;
-	qe_usb_enable();
+	qe_usb_enable(udc);
 	return 0;
 }
 
@@ -2327,92 +2283,65 @@ static irqreturn_t qe_udc_irq(int irq, void *_udc)
 /*-------------------------------------------------------------------------
 	Gadget driver probe and unregister.
  --------------------------------------------------------------------------*/
-static int fsl_qe_start(struct usb_gadget_driver *driver,
-		int (*bind)(struct usb_gadget *))
+static int fsl_qe_start(struct usb_gadget *gadget,
+		struct usb_gadget_driver *driver)
 {
-	int retval;
-	unsigned long flags = 0;
-
-	/* standard operations */
-	if (!udc_controller)
-		return -ENODEV;
-
-	if (!driver || driver->max_speed < USB_SPEED_FULL
-			|| !bind || !driver->disconnect || !driver->setup)
-		return -EINVAL;
-
-	if (udc_controller->driver)
-		return -EBUSY;
+	struct qe_udc *udc;
+	unsigned long flags;
 
+	udc = container_of(gadget, struct qe_udc, gadget);
 	/* lock is needed but whether should use this lock or another */
-	spin_lock_irqsave(&udc_controller->lock, flags);
+	spin_lock_irqsave(&udc->lock, flags);
 
 	driver->driver.bus = NULL;
 	/* hook up the driver */
-	udc_controller->driver = driver;
-	udc_controller->gadget.dev.driver = &driver->driver;
-	udc_controller->gadget.speed = driver->max_speed;
-	spin_unlock_irqrestore(&udc_controller->lock, flags);
-
-	retval = bind(&udc_controller->gadget);
-	if (retval) {
-		dev_err(udc_controller->dev, "bind to %s --> %d",
-				driver->driver.name, retval);
-		udc_controller->gadget.dev.driver = NULL;
-		udc_controller->driver = NULL;
-		return retval;
-	}
+	udc->driver = driver;
+	udc->gadget.dev.driver = &driver->driver;
+	udc->gadget.speed = driver->max_speed;
 
 	/* Enable IRQ reg and Set usbcmd reg EN bit */
-	qe_usb_enable();
-
-	out_be16(&udc_controller->usb_regs->usb_usber, 0xffff);
-	out_be16(&udc_controller->usb_regs->usb_usbmr, USB_E_DEFAULT_DEVICE);
-	udc_controller->usb_state = USB_STATE_ATTACHED;
-	udc_controller->ep0_state = WAIT_FOR_SETUP;
-	udc_controller->ep0_dir = USB_DIR_OUT;
-	dev_info(udc_controller->dev, "%s bind to driver %s \n",
-		udc_controller->gadget.name, driver->driver.name);
+	qe_usb_enable(udc);
+
+	out_be16(&udc->usb_regs->usb_usber, 0xffff);
+	out_be16(&udc->usb_regs->usb_usbmr, USB_E_DEFAULT_DEVICE);
+	udc->usb_state = USB_STATE_ATTACHED;
+	udc->ep0_state = WAIT_FOR_SETUP;
+	udc->ep0_dir = USB_DIR_OUT;
+	spin_unlock_irqrestore(&udc->lock, flags);
+
+	dev_info(udc->dev, "%s bind to driver %s\n", udc->gadget.name,
+			driver->driver.name);
 	return 0;
 }
 
-static int fsl_qe_stop(struct usb_gadget_driver *driver)
+static int fsl_qe_stop(struct usb_gadget *gadget,
+		struct usb_gadget_driver *driver)
 {
+	struct qe_udc *udc;
 	struct qe_ep *loop_ep;
 	unsigned long flags;
 
-	if (!udc_controller)
-		return -ENODEV;
-
-	if (!driver || driver != udc_controller->driver)
-		return -EINVAL;
-
+	udc = container_of(gadget, struct qe_udc, gadget);
 	/* stop usb controller, disable intr */
-	qe_usb_disable();
+	qe_usb_disable(udc);
 
 	/* in fact, no needed */
-	udc_controller->usb_state = USB_STATE_ATTACHED;
-	udc_controller->ep0_state = WAIT_FOR_SETUP;
-	udc_controller->ep0_dir = 0;
+	udc->usb_state = USB_STATE_ATTACHED;
+	udc->ep0_state = WAIT_FOR_SETUP;
+	udc->ep0_dir = 0;
 
 	/* stand operation */
-	spin_lock_irqsave(&udc_controller->lock, flags);
-	udc_controller->gadget.speed = USB_SPEED_UNKNOWN;
-	nuke(&udc_controller->eps[0], -ESHUTDOWN);
-	list_for_each_entry(loop_ep, &udc_controller->gadget.ep_list,
-				ep.ep_list)
+	spin_lock_irqsave(&udc->lock, flags);
+	udc->gadget.speed = USB_SPEED_UNKNOWN;
+	nuke(&udc->eps[0], -ESHUTDOWN);
+	list_for_each_entry(loop_ep, &udc->gadget.ep_list, ep.ep_list)
 		nuke(loop_ep, -ESHUTDOWN);
-	spin_unlock_irqrestore(&udc_controller->lock, flags);
-
-	/* report disconnect; the controller is already quiesced */
-	driver->disconnect(&udc_controller->gadget);
+	spin_unlock_irqrestore(&udc->lock, flags);
 
-	/* unbind gadget and unhook driver. */
-	driver->unbind(&udc_controller->gadget);
-	udc_controller->gadget.dev.driver = NULL;
-	udc_controller->driver = NULL;
+	udc->gadget.dev.driver = NULL;
+	udc->driver = NULL;
 
-	dev_info(udc_controller->dev, "unregistered gadget driver '%s'\r\n",
+	dev_info(udc->dev, "unregistered gadget driver '%s'\r\n",
 			driver->driver.name);
 	return 0;
 }
@@ -2502,7 +2431,7 @@ static int __devinit qe_ep_config(struct qe_udc *udc, unsigned char pipe_num)
 	ep->ep.ops = &qe_ep_ops;
 	ep->stopped = 1;
 	ep->ep.maxpacket = (unsigned short) ~0;
-	ep->desc = NULL;
+	ep->ep.desc = NULL;
 	ep->dir = 0xff;
 	ep->epnum = (u8)pipe_num;
 	ep->sent = 0;
@@ -2531,21 +2460,22 @@ static int __devinit qe_ep_config(struct qe_udc *udc, unsigned char pipe_num)
  *----------------------------------------------------------------------*/
 static void qe_udc_release(struct device *dev)
 {
-	int i = 0;
+	struct qe_udc *udc = container_of(dev, struct qe_udc, gadget.dev);
+	int i;
 
-	complete(udc_controller->done);
-	cpm_muram_free(cpm_muram_offset(udc_controller->ep_param[0]));
+	complete(udc->done);
+	cpm_muram_free(cpm_muram_offset(udc->ep_param[0]));
 	for (i = 0; i < USB_MAX_ENDPOINTS; i++)
-		udc_controller->ep_param[i] = NULL;
+		udc->ep_param[i] = NULL;
 
-	kfree(udc_controller);
-	udc_controller = NULL;
+	kfree(udc);
 }
 
 /* Driver probe functions */
 static const struct of_device_id qe_udc_match[];
 static int __devinit qe_udc_probe(struct platform_device *ofdev)
 {
+	struct qe_udc *udc;
 	const struct of_device_id *match;
 	struct device_node *np = ofdev->dev.of_node;
 	struct qe_ep *ep;
@@ -2562,44 +2492,44 @@ static int __devinit qe_udc_probe(struct platform_device *ofdev)
 		return -ENODEV;
 
 	/* Initialize the udc structure including QH member and other member */
-	udc_controller = qe_udc_config(ofdev);
-	if (!udc_controller) {
+	udc = qe_udc_config(ofdev);
+	if (!udc) {
 		dev_err(&ofdev->dev, "failed to initialize\n");
 		return -ENOMEM;
 	}
 
-	udc_controller->soc_type = (unsigned long)match->data;
-	udc_controller->usb_regs = of_iomap(np, 0);
-	if (!udc_controller->usb_regs) {
+	udc->soc_type = (unsigned long)match->data;
+	udc->usb_regs = of_iomap(np, 0);
+	if (!udc->usb_regs) {
 		ret = -ENOMEM;
 		goto err1;
 	}
 
 	/* initialize usb hw reg except for regs for EP,
 	 * leave usbintr reg untouched*/
-	qe_udc_reg_init(udc_controller);
+	qe_udc_reg_init(udc);
 
 	/* here comes the stand operations for probe
 	 * set the qe_udc->gadget.xxx */
-	udc_controller->gadget.ops = &qe_gadget_ops;
+	udc->gadget.ops = &qe_gadget_ops;
 
 	/* gadget.ep0 is a pointer */
-	udc_controller->gadget.ep0 = &udc_controller->eps[0].ep;
+	udc->gadget.ep0 = &udc->eps[0].ep;
 
-	INIT_LIST_HEAD(&udc_controller->gadget.ep_list);
+	INIT_LIST_HEAD(&udc->gadget.ep_list);
 
 	/* modify in register gadget process */
-	udc_controller->gadget.speed = USB_SPEED_UNKNOWN;
+	udc->gadget.speed = USB_SPEED_UNKNOWN;
 
 	/* name: Identifies the controller hardware type. */
-	udc_controller->gadget.name = driver_name;
+	udc->gadget.name = driver_name;
 
-	device_initialize(&udc_controller->gadget.dev);
+	device_initialize(&udc->gadget.dev);
 
-	dev_set_name(&udc_controller->gadget.dev, "gadget");
+	dev_set_name(&udc->gadget.dev, "gadget");
 
-	udc_controller->gadget.dev.release = qe_udc_release;
-	udc_controller->gadget.dev.parent = &ofdev->dev;
+	udc->gadget.dev.release = qe_udc_release;
+	udc->gadget.dev.parent = &ofdev->dev;
 
 	/* initialize qe_ep struct */
 	for (i = 0; i < USB_MAX_ENDPOINTS ; i++) {
@@ -2608,104 +2538,104 @@ static int __devinit qe_udc_probe(struct platform_device *ofdev)
 
 		/* setup the qe_ep struct and link ep.ep.list
 		 * into gadget.ep_list */
-		qe_ep_config(udc_controller, (unsigned char)i);
+		qe_ep_config(udc, (unsigned char)i);
 	}
 
 	/* ep0 initialization in here */
-	ret = qe_ep_init(udc_controller, 0, &qe_ep0_desc);
+	ret = qe_ep_init(udc, 0, &qe_ep0_desc);
 	if (ret)
 		goto err2;
 
 	/* create a buf for ZLP send, need to remain zeroed */
-	udc_controller->nullbuf = kzalloc(256, GFP_KERNEL);
-	if (udc_controller->nullbuf == NULL) {
-		dev_err(udc_controller->dev, "cannot alloc nullbuf\n");
+	udc->nullbuf = kzalloc(256, GFP_KERNEL);
+	if (udc->nullbuf == NULL) {
+		dev_err(udc->dev, "cannot alloc nullbuf\n");
 		ret = -ENOMEM;
 		goto err3;
 	}
 
 	/* buffer for data of get_status request */
-	udc_controller->statusbuf = kzalloc(2, GFP_KERNEL);
-	if (udc_controller->statusbuf == NULL) {
+	udc->statusbuf = kzalloc(2, GFP_KERNEL);
+	if (udc->statusbuf == NULL) {
 		ret = -ENOMEM;
 		goto err4;
 	}
 
-	udc_controller->nullp = virt_to_phys((void *)udc_controller->nullbuf);
-	if (udc_controller->nullp == DMA_ADDR_INVALID) {
-		udc_controller->nullp = dma_map_single(
-					udc_controller->gadget.dev.parent,
-					udc_controller->nullbuf,
+	udc->nullp = virt_to_phys((void *)udc->nullbuf);
+	if (udc->nullp == DMA_ADDR_INVALID) {
+		udc->nullp = dma_map_single(
+					udc->gadget.dev.parent,
+					udc->nullbuf,
 					256,
 					DMA_TO_DEVICE);
-		udc_controller->nullmap = 1;
+		udc->nullmap = 1;
 	} else {
-		dma_sync_single_for_device(udc_controller->gadget.dev.parent,
-					udc_controller->nullp, 256,
+		dma_sync_single_for_device(udc->gadget.dev.parent,
+					udc->nullp, 256,
 					DMA_TO_DEVICE);
 	}
 
-	tasklet_init(&udc_controller->rx_tasklet, ep_rx_tasklet,
-			(unsigned long)udc_controller);
+	tasklet_init(&udc->rx_tasklet, ep_rx_tasklet,
+			(unsigned long)udc);
 	/* request irq and disable DR  */
-	udc_controller->usb_irq = irq_of_parse_and_map(np, 0);
-	if (!udc_controller->usb_irq) {
+	udc->usb_irq = irq_of_parse_and_map(np, 0);
+	if (!udc->usb_irq) {
 		ret = -EINVAL;
 		goto err_noirq;
 	}
 
-	ret = request_irq(udc_controller->usb_irq, qe_udc_irq, 0,
-				driver_name, udc_controller);
+	ret = request_irq(udc->usb_irq, qe_udc_irq, 0,
+				driver_name, udc);
 	if (ret) {
-		dev_err(udc_controller->dev, "cannot request irq %d err %d \n",
-			udc_controller->usb_irq, ret);
+		dev_err(udc->dev, "cannot request irq %d err %d\n",
+				udc->usb_irq, ret);
 		goto err5;
 	}
 
-	ret = device_add(&udc_controller->gadget.dev);
+	ret = device_add(&udc->gadget.dev);
 	if (ret)
 		goto err6;
 
-	ret = usb_add_gadget_udc(&ofdev->dev, &udc_controller->gadget);
+	ret = usb_add_gadget_udc(&ofdev->dev, &udc->gadget);
 	if (ret)
 		goto err7;
 
-	dev_info(udc_controller->dev,
+	dev_set_drvdata(&ofdev->dev, udc);
+	dev_info(udc->dev,
 			"%s USB controller initialized as device\n",
-			(udc_controller->soc_type == PORT_QE) ? "QE" : "CPM");
+			(udc->soc_type == PORT_QE) ? "QE" : "CPM");
 	return 0;
 
 err7:
-	device_unregister(&udc_controller->gadget.dev);
+	device_unregister(&udc->gadget.dev);
 err6:
-	free_irq(udc_controller->usb_irq, udc_controller);
+	free_irq(udc->usb_irq, udc);
 err5:
-	irq_dispose_mapping(udc_controller->usb_irq);
+	irq_dispose_mapping(udc->usb_irq);
 err_noirq:
-	if (udc_controller->nullmap) {
-		dma_unmap_single(udc_controller->gadget.dev.parent,
-			udc_controller->nullp, 256,
+	if (udc->nullmap) {
+		dma_unmap_single(udc->gadget.dev.parent,
+			udc->nullp, 256,
 				DMA_TO_DEVICE);
-			udc_controller->nullp = DMA_ADDR_INVALID;
+			udc->nullp = DMA_ADDR_INVALID;
 	} else {
-		dma_sync_single_for_cpu(udc_controller->gadget.dev.parent,
-			udc_controller->nullp, 256,
+		dma_sync_single_for_cpu(udc->gadget.dev.parent,
+			udc->nullp, 256,
 				DMA_TO_DEVICE);
 	}
-	kfree(udc_controller->statusbuf);
+	kfree(udc->statusbuf);
 err4:
-	kfree(udc_controller->nullbuf);
+	kfree(udc->nullbuf);
 err3:
-	ep = &udc_controller->eps[0];
+	ep = &udc->eps[0];
 	cpm_muram_free(cpm_muram_offset(ep->rxbase));
 	kfree(ep->rxframe);
 	kfree(ep->rxbuffer);
 	kfree(ep->txframe);
 err2:
-	iounmap(udc_controller->usb_regs);
+	iounmap(udc->usb_regs);
 err1:
-	kfree(udc_controller);
-	udc_controller = NULL;
+	kfree(udc);
 	return ret;
 }
 
@@ -2723,44 +2653,41 @@ static int qe_udc_resume(struct platform_device *dev)
 
 static int __devexit qe_udc_remove(struct platform_device *ofdev)
 {
+	struct qe_udc *udc = dev_get_drvdata(&ofdev->dev);
 	struct qe_ep *ep;
 	unsigned int size;
-
 	DECLARE_COMPLETION(done);
 
-	if (!udc_controller)
-		return -ENODEV;
-
-	usb_del_gadget_udc(&udc_controller->gadget);
+	usb_del_gadget_udc(&udc->gadget);
 
-	udc_controller->done = &done;
-	tasklet_disable(&udc_controller->rx_tasklet);
+	udc->done = &done;
+	tasklet_disable(&udc->rx_tasklet);
 
-	if (udc_controller->nullmap) {
-		dma_unmap_single(udc_controller->gadget.dev.parent,
-			udc_controller->nullp, 256,
+	if (udc->nullmap) {
+		dma_unmap_single(udc->gadget.dev.parent,
+			udc->nullp, 256,
 				DMA_TO_DEVICE);
-			udc_controller->nullp = DMA_ADDR_INVALID;
+			udc->nullp = DMA_ADDR_INVALID;
 	} else {
-		dma_sync_single_for_cpu(udc_controller->gadget.dev.parent,
-			udc_controller->nullp, 256,
+		dma_sync_single_for_cpu(udc->gadget.dev.parent,
+			udc->nullp, 256,
 				DMA_TO_DEVICE);
 	}
-	kfree(udc_controller->statusbuf);
-	kfree(udc_controller->nullbuf);
+	kfree(udc->statusbuf);
+	kfree(udc->nullbuf);
 
-	ep = &udc_controller->eps[0];
+	ep = &udc->eps[0];
 	cpm_muram_free(cpm_muram_offset(ep->rxbase));
 	size = (ep->ep.maxpacket + USB_CRC_SIZE + 2) * (USB_BDRING_LEN + 1);
 
 	kfree(ep->rxframe);
 	if (ep->rxbufmap) {
-		dma_unmap_single(udc_controller->gadget.dev.parent,
+		dma_unmap_single(udc->gadget.dev.parent,
 				ep->rxbuf_d, size,
 				DMA_FROM_DEVICE);
 		ep->rxbuf_d = DMA_ADDR_INVALID;
 	} else {
-		dma_sync_single_for_cpu(udc_controller->gadget.dev.parent,
+		dma_sync_single_for_cpu(udc->gadget.dev.parent,
 				ep->rxbuf_d, size,
 				DMA_FROM_DEVICE);
 	}
@@ -2768,14 +2695,14 @@ static int __devexit qe_udc_remove(struct platform_device *ofdev)
 	kfree(ep->rxbuffer);
 	kfree(ep->txframe);
 
-	free_irq(udc_controller->usb_irq, udc_controller);
-	irq_dispose_mapping(udc_controller->usb_irq);
+	free_irq(udc->usb_irq, udc);
+	irq_dispose_mapping(udc->usb_irq);
 
-	tasklet_kill(&udc_controller->rx_tasklet);
+	tasklet_kill(&udc->rx_tasklet);
 
-	iounmap(udc_controller->usb_regs);
+	iounmap(udc->usb_regs);
 
-	device_unregister(&udc_controller->gadget.dev);
+	device_unregister(&udc->gadget.dev);
 	/* wait for release() of gadget.dev to free udc */
 	wait_for_completion(&done);
 
diff --git a/drivers/usb/gadget/fsl_qe_udc.h b/drivers/usb/gadget/fsl_qe_udc.h
index 1da5fb03d21..4c07ca9cebf 100644
--- a/drivers/usb/gadget/fsl_qe_udc.h
+++ b/drivers/usb/gadget/fsl_qe_udc.h
@@ -266,7 +266,6 @@ struct qe_ep {
 	struct usb_ep ep;
 	struct list_head queue;
 	struct qe_udc *udc;
-	const struct usb_endpoint_descriptor *desc;
 	struct usb_gadget *gadget;
 
 	u8 state;
diff --git a/drivers/usb/gadget/fsl_udc_core.c b/drivers/usb/gadget/fsl_udc_core.c
index 55abfb6bd61..28316858208 100644
--- a/drivers/usb/gadget/fsl_udc_core.c
+++ b/drivers/usb/gadget/fsl_udc_core.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (C) 2004-2007,2011 Freescale Semiconductor, Inc.
+ * Copyright (C) 2004-2007,2011-2012 Freescale Semiconductor, Inc.
  * All rights reserved.
  *
  * Author: Li Yang <leoli@freescale.com>
@@ -58,9 +58,8 @@ static const char driver_name[] = "fsl-usb2-udc";
 static const char driver_desc[] = DRIVER_DESC;
 
 static struct usb_dr_device *dr_regs;
-#ifndef CONFIG_ARCH_MXC
+
 static struct usb_sys_interface *usb_sys_regs;
-#endif
 
 /* it is initialized in probe()  */
 static struct fsl_udc *udc_controller = NULL;
@@ -244,10 +243,9 @@ static int dr_controller_setup(struct fsl_udc *udc)
 {
 	unsigned int tmp, portctrl, ep_num;
 	unsigned int max_no_of_ep;
-#ifndef CONFIG_ARCH_MXC
 	unsigned int ctrl;
-#endif
 	unsigned long timeout;
+
 #define FSL_UDC_RESET_TIMEOUT 1000
 
 	/* Config PHY interface */
@@ -255,12 +253,32 @@ static int dr_controller_setup(struct fsl_udc *udc)
 	portctrl &= ~(PORTSCX_PHY_TYPE_SEL | PORTSCX_PORT_WIDTH);
 	switch (udc->phy_mode) {
 	case FSL_USB2_PHY_ULPI:
+		if (udc->pdata->have_sysif_regs) {
+			if (udc->pdata->controller_ver) {
+				/* controller version 1.6 or above */
+				ctrl = __raw_readl(&usb_sys_regs->control);
+				ctrl &= ~USB_CTRL_UTMI_PHY_EN;
+				ctrl |= USB_CTRL_USB_EN;
+				__raw_writel(ctrl, &usb_sys_regs->control);
+			}
+		}
 		portctrl |= PORTSCX_PTS_ULPI;
 		break;
 	case FSL_USB2_PHY_UTMI_WIDE:
 		portctrl |= PORTSCX_PTW_16BIT;
 		/* fall through */
 	case FSL_USB2_PHY_UTMI:
+		if (udc->pdata->have_sysif_regs) {
+			if (udc->pdata->controller_ver) {
+				/* controller version 1.6 or above */
+				ctrl = __raw_readl(&usb_sys_regs->control);
+				ctrl |= (USB_CTRL_UTMI_PHY_EN |
+					USB_CTRL_USB_EN);
+				__raw_writel(ctrl, &usb_sys_regs->control);
+				mdelay(FSL_UTMI_PHY_DLY); /* Delay for UTMI
+					PHY CLK to become stable - 10ms*/
+			}
+		}
 		portctrl |= PORTSCX_PTS_UTMI;
 		break;
 	case FSL_USB2_PHY_SERIAL:
@@ -549,7 +567,7 @@ static int fsl_ep_enable(struct usb_ep *_ep,
 	ep = container_of(_ep, struct fsl_ep, ep);
 
 	/* catch various bogus parameters */
-	if (!_ep || !desc || ep->desc
+	if (!_ep || !desc || ep->ep.desc
 			|| (desc->bDescriptorType != USB_DT_ENDPOINT))
 		return -EINVAL;
 
@@ -590,7 +608,7 @@ static int fsl_ep_enable(struct usb_ep *_ep,
 
 	spin_lock_irqsave(&udc->lock, flags);
 	ep->ep.maxpacket = max;
-	ep->desc = desc;
+	ep->ep.desc = desc;
 	ep->stopped = 0;
 
 	/* Controller related setup */
@@ -614,7 +632,7 @@ static int fsl_ep_enable(struct usb_ep *_ep,
 	retval = 0;
 
 	VDBG("enabled %s (ep%d%s) maxpacket %d",ep->ep.name,
-			ep->desc->bEndpointAddress & 0x0f,
+			ep->ep.desc->bEndpointAddress & 0x0f,
 			(desc->bEndpointAddress & USB_DIR_IN)
 				? "in" : "out", max);
 en_done:
@@ -634,7 +652,7 @@ static int fsl_ep_disable(struct usb_ep *_ep)
 	int ep_num;
 
 	ep = container_of(_ep, struct fsl_ep, ep);
-	if (!_ep || !ep->desc) {
+	if (!_ep || !ep->ep.desc) {
 		VDBG("%s not enabled", _ep ? ep->ep.name : NULL);
 		return -EINVAL;
 	}
@@ -657,7 +675,6 @@ static int fsl_ep_disable(struct usb_ep *_ep)
 	/* nuke all pending requests (does flush) */
 	nuke(ep, -ESHUTDOWN);
 
-	ep->desc = NULL;
 	ep->ep.desc = NULL;
 	ep->stopped = 1;
 	spin_unlock_irqrestore(&udc->lock, flags);
@@ -736,6 +753,8 @@ static void fsl_queue_td(struct fsl_ep *ep, struct fsl_req *req)
 		lastreq = list_entry(ep->queue.prev, struct fsl_req, queue);
 		lastreq->tail->next_td_ptr =
 			cpu_to_hc32(req->head->td_dma & DTD_ADDR_MASK);
+		/* Ensure dTD's next dtd pointer to be updated */
+		wmb();
 		/* Read prime bit, if 1 goto done */
 		if (fsl_readl(&dr_regs->endpointprime) & bitmask)
 			return;
@@ -874,11 +893,11 @@ fsl_ep_queue(struct usb_ep *_ep, struct usb_request *_req, gfp_t gfp_flags)
 		VDBG("%s, bad params", __func__);
 		return -EINVAL;
 	}
-	if (unlikely(!_ep || !ep->desc)) {
+	if (unlikely(!_ep || !ep->ep.desc)) {
 		VDBG("%s, bad ep", __func__);
 		return -EINVAL;
 	}
-	if (usb_endpoint_xfer_isoc(ep->desc)) {
+	if (usb_endpoint_xfer_isoc(ep->ep.desc)) {
 		if (req->req.length > ep->ep.maxpacket)
 			return -EMSGSIZE;
 	}
@@ -1017,12 +1036,12 @@ static int fsl_ep_set_halt(struct usb_ep *_ep, int value)
 
 	ep = container_of(_ep, struct fsl_ep, ep);
 	udc = ep->udc;
-	if (!_ep || !ep->desc) {
+	if (!_ep || !ep->ep.desc) {
 		status = -EINVAL;
 		goto out;
 	}
 
-	if (usb_endpoint_xfer_isoc(ep->desc)) {
+	if (usb_endpoint_xfer_isoc(ep->ep.desc)) {
 		status = -EOPNOTSUPP;
 		goto out;
 	}
@@ -1061,7 +1080,7 @@ static int fsl_ep_fifo_status(struct usb_ep *_ep)
 	struct ep_queue_head *qh;
 
 	ep = container_of(_ep, struct fsl_ep, ep);
-	if (!_ep || (!ep->desc && ep_index(ep) != 0))
+	if (!_ep || (!ep->ep.desc && ep_index(ep) != 0))
 		return -ENODEV;
 
 	udc = (struct fsl_udc *)ep->udc;
@@ -1094,7 +1113,7 @@ static void fsl_ep_fifo_flush(struct usb_ep *_ep)
 		return;
 	} else {
 		ep = container_of(_ep, struct fsl_ep, ep);
-		if (!ep->desc)
+		if (!ep->ep.desc)
 			return;
 	}
 	ep_num = ep_index(ep);
@@ -1349,7 +1368,7 @@ static void ch9getstatus(struct fsl_udc *udc, u8 request_type, u16 value,
 		target_ep = get_ep_by_pipe(udc, get_pipe_by_windex(index));
 
 		/* stall if endpoint doesn't exist */
-		if (!target_ep->desc)
+		if (!target_ep->ep.desc)
 			goto stall;
 		tmp = dr_ep_get_stall(ep_index(target_ep), ep_is_in(target_ep))
 				<< USB_ENDPOINT_HALT;
@@ -2259,7 +2278,7 @@ static int fsl_proc_read(char *page, char **start, off_t off, int count,
 	}
 	/* other gadget->eplist ep */
 	list_for_each_entry(ep, &udc->gadget.ep_list, ep.ep_list) {
-		if (ep->desc) {
+		if (ep->ep.desc) {
 			t = scnprintf(next, size,
 					"\nFor %s Maxpkt is 0x%x "
 					"index is 0x%x\n",
diff --git a/drivers/usb/gadget/fsl_usb2_udc.h b/drivers/usb/gadget/fsl_usb2_udc.h
index e651469fd39..5cd7b7e7ddb 100644
--- a/drivers/usb/gadget/fsl_usb2_udc.h
+++ b/drivers/usb/gadget/fsl_usb2_udc.h
@@ -1,4 +1,12 @@
 /*
+ * Copyright (C) 2004,2012 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.
+ *
  * Freescale USB device/endpoint management registers
  */
 #ifndef __FSL_USB2_UDC_H
@@ -348,6 +356,9 @@ struct usb_sys_interface {
 /* control Register Bit Masks */
 #define  USB_CTRL_IOENB                       0x00000004
 #define  USB_CTRL_ULPI_INT0EN                 0x00000001
+#define USB_CTRL_UTMI_PHY_EN		      0x00000200
+#define USB_CTRL_USB_EN			      0x00000004
+#define USB_CTRL_ULPI_PHY_CLK_SEL	      0x00000400
 
 /* Endpoint Queue Head data struct
  * Rem: all the variables of qh are LittleEndian Mode
@@ -450,7 +461,6 @@ struct fsl_ep {
 	struct list_head queue;
 	struct fsl_udc *udc;
 	struct ep_queue_head *qh;
-	const struct usb_endpoint_descriptor *desc;
 	struct usb_gadget *gadget;
 
 	char name[14];
diff --git a/drivers/usb/gadget/fusb300_udc.c b/drivers/usb/gadget/fusb300_udc.c
index 5831cb4a0b3..cdd94540e1c 100644
--- a/drivers/usb/gadget/fusb300_udc.c
+++ b/drivers/usb/gadget/fusb300_udc.c
@@ -203,7 +203,7 @@ static int config_ep(struct fusb300_ep *ep,
 	struct fusb300 *fusb300 = ep->fusb300;
 	struct fusb300_ep_info info;
 
-	ep->desc = desc;
+	ep->ep.desc = desc;
 
 	info.interval = 0;
 	info.addrofs = 0;
@@ -443,7 +443,7 @@ static int fusb300_queue(struct usb_ep *_ep, struct usb_request *_req,
 	req->req.actual = 0;
 	req->req.status = -EINPROGRESS;
 
-	if (ep->desc == NULL) /* ep0 */
+	if (ep->ep.desc == NULL) /* ep0 */
 		ep0_queue(ep, req);
 	else if (request && !ep->stall)
 		enable_fifo_int(ep);
diff --git a/drivers/usb/gadget/fusb300_udc.h b/drivers/usb/gadget/fusb300_udc.h
index 92745bd0306..542cd83cc80 100644
--- a/drivers/usb/gadget/fusb300_udc.h
+++ b/drivers/usb/gadget/fusb300_udc.h
@@ -650,7 +650,6 @@ struct fusb300_ep {
 
 	unsigned char		epnum;
 	unsigned char		type;
-	const struct usb_endpoint_descriptor	*desc;
 };
 
 struct fusb300 {
diff --git a/drivers/usb/gadget/g_ffs.c b/drivers/usb/gadget/g_ffs.c
index a85eaf40b94..d3ace9002a6 100644
--- a/drivers/usb/gadget/g_ffs.c
+++ b/drivers/usb/gadget/g_ffs.c
@@ -67,6 +67,15 @@ MODULE_LICENSE("GPL");
 #define GFS_VENDOR_ID	0x1d6b	/* Linux Foundation */
 #define GFS_PRODUCT_ID	0x0105	/* FunctionFS Gadget */
 
+#define GFS_MAX_DEVS	10
+
+struct gfs_ffs_obj {
+	const char *name;
+	bool mounted;
+	bool desc_ready;
+	struct ffs_data *ffs_data;
+};
+
 static struct usb_device_descriptor gfs_dev_desc = {
 	.bLength		= sizeof gfs_dev_desc,
 	.bDescriptorType	= USB_DT_DEVICE,
@@ -78,12 +87,17 @@ static struct usb_device_descriptor gfs_dev_desc = {
 	.idProduct		= cpu_to_le16(GFS_PRODUCT_ID),
 };
 
+static char *func_names[GFS_MAX_DEVS];
+static unsigned int func_num;
+
 module_param_named(bDeviceClass,    gfs_dev_desc.bDeviceClass,    byte,   0644);
 MODULE_PARM_DESC(bDeviceClass, "USB Device class");
 module_param_named(bDeviceSubClass, gfs_dev_desc.bDeviceSubClass, byte,   0644);
 MODULE_PARM_DESC(bDeviceSubClass, "USB Device subclass");
 module_param_named(bDeviceProtocol, gfs_dev_desc.bDeviceProtocol, byte,   0644);
 MODULE_PARM_DESC(bDeviceProtocol, "USB Device protocol");
+module_param_array_named(functions, func_names, charp, &func_num, 0);
+MODULE_PARM_DESC(functions, "USB Functions list");
 
 static const struct usb_descriptor_header *gfs_otg_desc[] = {
 	(const struct usb_descriptor_header *)
@@ -158,13 +172,34 @@ static struct usb_composite_driver gfs_driver = {
 	.iProduct	= DRIVER_DESC,
 };
 
-static struct ffs_data *gfs_ffs_data;
-static unsigned long gfs_registered;
+static DEFINE_MUTEX(gfs_lock);
+static unsigned int missing_funcs;
+static bool gfs_ether_setup;
+static bool gfs_registered;
+static bool gfs_single_func;
+static struct gfs_ffs_obj *ffs_tab;
 
 static int __init gfs_init(void)
 {
+	int i;
+
 	ENTER();
 
+	if (!func_num) {
+		gfs_single_func = true;
+		func_num = 1;
+	}
+
+	ffs_tab = kcalloc(func_num, sizeof *ffs_tab, GFP_KERNEL);
+	if (!ffs_tab)
+		return -ENOMEM;
+
+	if (!gfs_single_func)
+		for (i = 0; i < func_num; i++)
+			ffs_tab[i].name = func_names[i];
+
+	missing_funcs = func_num;
+
 	return functionfs_init();
 }
 module_init(gfs_init);
@@ -172,63 +207,165 @@ module_init(gfs_init);
 static void __exit gfs_exit(void)
 {
 	ENTER();
+	mutex_lock(&gfs_lock);
 
-	if (test_and_clear_bit(0, &gfs_registered))
+	if (gfs_registered)
 		usb_composite_unregister(&gfs_driver);
+	gfs_registered = false;
 
 	functionfs_cleanup();
+
+	mutex_unlock(&gfs_lock);
+	kfree(ffs_tab);
 }
 module_exit(gfs_exit);
 
+static struct gfs_ffs_obj *gfs_find_dev(const char *dev_name)
+{
+	int i;
+
+	ENTER();
+
+	if (gfs_single_func)
+		return &ffs_tab[0];
+
+	for (i = 0; i < func_num; i++)
+		if (strcmp(ffs_tab[i].name, dev_name) == 0)
+			return &ffs_tab[i];
+
+	return NULL;
+}
+
 static int functionfs_ready_callback(struct ffs_data *ffs)
 {
+	struct gfs_ffs_obj *ffs_obj;
 	int ret;
 
 	ENTER();
+	mutex_lock(&gfs_lock);
+
+	ffs_obj = ffs->private_data;
+	if (!ffs_obj) {
+		ret = -EINVAL;
+		goto done;
+	}
+
+	if (WARN_ON(ffs_obj->desc_ready)) {
+		ret = -EBUSY;
+		goto done;
+	}
+	ffs_obj->desc_ready = true;
+	ffs_obj->ffs_data = ffs;
 
-	if (WARN_ON(test_and_set_bit(0, &gfs_registered)))
-		return -EBUSY;
+	if (--missing_funcs) {
+		ret = 0;
+		goto done;
+	}
+
+	if (gfs_registered) {
+		ret = -EBUSY;
+		goto done;
+	}
+	gfs_registered = true;
 
-	gfs_ffs_data = ffs;
 	ret = usb_composite_probe(&gfs_driver, gfs_bind);
 	if (unlikely(ret < 0))
-		clear_bit(0, &gfs_registered);
+		gfs_registered = false;
+
+done:
+	mutex_unlock(&gfs_lock);
 	return ret;
 }
 
 static void functionfs_closed_callback(struct ffs_data *ffs)
 {
+	struct gfs_ffs_obj *ffs_obj;
+
 	ENTER();
+	mutex_lock(&gfs_lock);
 
-	if (test_and_clear_bit(0, &gfs_registered))
+	ffs_obj = ffs->private_data;
+	if (!ffs_obj)
+		goto done;
+
+	ffs_obj->desc_ready = false;
+	missing_funcs++;
+
+	if (gfs_registered)
 		usb_composite_unregister(&gfs_driver);
+	gfs_registered = false;
+
+done:
+	mutex_unlock(&gfs_lock);
 }
 
-static int functionfs_check_dev_callback(const char *dev_name)
+static void *functionfs_acquire_dev_callback(const char *dev_name)
 {
-	return 0;
+	struct gfs_ffs_obj *ffs_dev;
+
+	ENTER();
+	mutex_lock(&gfs_lock);
+
+	ffs_dev = gfs_find_dev(dev_name);
+	if (!ffs_dev) {
+		ffs_dev = ERR_PTR(-ENODEV);
+		goto done;
+	}
+
+	if (ffs_dev->mounted) {
+		ffs_dev = ERR_PTR(-EBUSY);
+		goto done;
+	}
+	ffs_dev->mounted = true;
+
+done:
+	mutex_unlock(&gfs_lock);
+	return ffs_dev;
 }
 
+static void functionfs_release_dev_callback(struct ffs_data *ffs_data)
+{
+	struct gfs_ffs_obj *ffs_dev;
+
+	ENTER();
+	mutex_lock(&gfs_lock);
+
+	ffs_dev = ffs_data->private_data;
+	if (ffs_dev)
+		ffs_dev->mounted = false;
+
+	mutex_unlock(&gfs_lock);
+}
+
+/*
+ * It is assumed that gfs_bind is called from a context where gfs_lock is held
+ */
 static int gfs_bind(struct usb_composite_dev *cdev)
 {
 	int ret, i;
 
 	ENTER();
 
-	if (WARN_ON(!gfs_ffs_data))
+	if (missing_funcs)
 		return -ENODEV;
 
 	ret = gether_setup(cdev->gadget, gfs_hostaddr);
 	if (unlikely(ret < 0))
 		goto error_quick;
+	gfs_ether_setup = true;
 
 	ret = usb_string_ids_tab(cdev, gfs_strings);
 	if (unlikely(ret < 0))
 		goto error;
 
-	ret = functionfs_bind(gfs_ffs_data, cdev);
-	if (unlikely(ret < 0))
-		goto error;
+	for (i = func_num; --i; ) {
+		ret = functionfs_bind(ffs_tab[i].ffs_data, cdev);
+		if (unlikely(ret < 0)) {
+			while (++i < func_num)
+				functionfs_unbind(ffs_tab[i].ffs_data);
+			goto error;
+		}
+	}
 
 	for (i = 0; i < ARRAY_SIZE(gfs_configurations); ++i) {
 		struct gfs_configuration *c = gfs_configurations + i;
@@ -246,16 +383,22 @@ static int gfs_bind(struct usb_composite_dev *cdev)
 	return 0;
 
 error_unbind:
-	functionfs_unbind(gfs_ffs_data);
+	for (i = 0; i < func_num; i++)
+		functionfs_unbind(ffs_tab[i].ffs_data);
 error:
 	gether_cleanup();
 error_quick:
-	gfs_ffs_data = NULL;
+	gfs_ether_setup = false;
 	return ret;
 }
 
+/*
+ * It is assumed that gfs_unbind is called from a context where gfs_lock is held
+ */
 static int gfs_unbind(struct usb_composite_dev *cdev)
 {
+	int i;
+
 	ENTER();
 
 	/*
@@ -266,22 +409,29 @@ static int gfs_unbind(struct usb_composite_dev *cdev)
 	 * from composite on orror recovery, but what you're gonna
 	 * do...?
 	 */
-	if (gfs_ffs_data) {
+	if (gfs_ether_setup)
 		gether_cleanup();
-		functionfs_unbind(gfs_ffs_data);
-		gfs_ffs_data = NULL;
-	}
+	gfs_ether_setup = false;
+
+	for (i = func_num; --i; )
+		if (ffs_tab[i].ffs_data)
+			functionfs_unbind(ffs_tab[i].ffs_data);
 
 	return 0;
 }
 
+/*
+ * It is assumed that gfs_do_config is called from a context where
+ * gfs_lock is held
+ */
 static int gfs_do_config(struct usb_configuration *c)
 {
 	struct gfs_configuration *gc =
 		container_of(c, struct gfs_configuration, c);
+	int i;
 	int ret;
 
-	if (WARN_ON(!gfs_ffs_data))
+	if (missing_funcs)
 		return -ENODEV;
 
 	if (gadget_is_otg(c->cdev->gadget)) {
@@ -295,9 +445,11 @@ static int gfs_do_config(struct usb_configuration *c)
 			return ret;
 	}
 
-	ret = functionfs_bind_config(c->cdev, c, gfs_ffs_data);
-	if (unlikely(ret < 0))
-		return ret;
+	for (i = 0; i < func_num; i++) {
+		ret = functionfs_bind_config(c->cdev, c, ffs_tab[i].ffs_data);
+		if (unlikely(ret < 0))
+			return ret;
+	}
 
 	/*
 	 * After previous do_configs there may be some invalid
diff --git a/drivers/usb/gadget/g_zero.h b/drivers/usb/gadget/g_zero.h
index e84b3c47ed3..71ca193358b 100644
--- a/drivers/usb/gadget/g_zero.h
+++ b/drivers/usb/gadget/g_zero.h
@@ -13,10 +13,11 @@ extern unsigned buflen;
 extern const struct usb_descriptor_header *otg_desc[];
 
 /* common utilities */
-struct usb_request *alloc_ep_req(struct usb_ep *ep);
+struct usb_request *alloc_ep_req(struct usb_ep *ep, int len);
 void free_ep_req(struct usb_ep *ep, struct usb_request *req);
 void disable_endpoints(struct usb_composite_dev *cdev,
-		struct usb_ep *in, struct usb_ep *out);
+		struct usb_ep *in, struct usb_ep *out,
+		struct usb_ep *iso_in, struct usb_ep *iso_out);
 
 /* configuration-specific linkup */
 int sourcesink_add(struct usb_composite_dev *cdev, bool autoresume);
diff --git a/drivers/usb/gadget/gadget_chips.h b/drivers/usb/gadget/gadget_chips.h
index a8855d0b7f3..b8b3a341121 100644
--- a/drivers/usb/gadget/gadget_chips.h
+++ b/drivers/usb/gadget/gadget_chips.h
@@ -37,6 +37,7 @@
 #define gadget_is_goku(g)		(!strcmp("goku_udc", (g)->name))
 #define gadget_is_imx(g)		(!strcmp("imx_udc", (g)->name))
 #define gadget_is_langwell(g)		(!strcmp("langwell_udc", (g)->name))
+#define gadget_is_lpc32xx(g)		(!strcmp("lpc32xx_udc", (g)->name))
 #define gadget_is_m66592(g)		(!strcmp("m66592_udc", (g)->name))
 #define gadget_is_musbhdrc(g)		(!strcmp("musb-hdrc", (g)->name))
 #define gadget_is_net2272(g)		(!strcmp("net2272", (g)->name))
@@ -118,6 +119,8 @@ static inline int usb_gadget_controller_number(struct usb_gadget *gadget)
 		return 0x31;
 	else if (gadget_is_dwc3(gadget))
 		return 0x32;
+	else if (gadget_is_lpc32xx(gadget))
+		return 0x33;
 
 	return -ENOENT;
 }
diff --git a/drivers/usb/gadget/goku_udc.c b/drivers/usb/gadget/goku_udc.c
index e151d6b87de..b241e6c6a7f 100644
--- a/drivers/usb/gadget/goku_udc.c
+++ b/drivers/usb/gadget/goku_udc.c
@@ -102,7 +102,7 @@ goku_ep_enable(struct usb_ep *_ep, const struct usb_endpoint_descriptor *desc)
 	unsigned long	flags;
 
 	ep = container_of(_ep, struct goku_ep, ep);
-	if (!_ep || !desc || ep->desc
+	if (!_ep || !desc || ep->ep.desc
 			|| desc->bDescriptorType != USB_DT_ENDPOINT)
 		return -EINVAL;
 	dev = ep->dev;
@@ -176,7 +176,7 @@ goku_ep_enable(struct usb_ep *_ep, const struct usb_endpoint_descriptor *desc)
 	command(ep->dev->regs, COMMAND_RESET, ep->num);
 	ep->ep.maxpacket = max;
 	ep->stopped = 0;
-	ep->desc = desc;
+	ep->ep.desc = desc;
 	spin_unlock_irqrestore(&ep->dev->lock, flags);
 
 	DBG(dev, "enable %s %s %s maxpacket %u\n", ep->ep.name,
@@ -233,7 +233,6 @@ static void ep_reset(struct goku_udc_regs __iomem *regs, struct goku_ep *ep)
 	}
 
 	ep->ep.maxpacket = MAX_FIFO_SIZE;
-	ep->desc = NULL;
 	ep->ep.desc = NULL;
 	ep->stopped = 1;
 	ep->irqs = 0;
@@ -247,7 +246,7 @@ static int goku_ep_disable(struct usb_ep *_ep)
 	unsigned long	flags;
 
 	ep = container_of(_ep, struct goku_ep, ep);
-	if (!_ep || !ep->desc)
+	if (!_ep || !ep->ep.desc)
 		return -ENODEV;
 	dev = ep->dev;
 	if (dev->ep0state == EP0_SUSPEND)
@@ -722,7 +721,7 @@ goku_queue(struct usb_ep *_ep, struct usb_request *_req, gfp_t gfp_flags)
 			|| !_req->buf || !list_empty(&req->queue)))
 		return -EINVAL;
 	ep = container_of(_ep, struct goku_ep, ep);
-	if (unlikely(!_ep || (!ep->desc && ep->num != 0)))
+	if (unlikely(!_ep || (!ep->ep.desc && ep->num != 0)))
 		return -EINVAL;
 	dev = ep->dev;
 	if (unlikely(!dev->driver || dev->gadget.speed == USB_SPEED_UNKNOWN))
@@ -815,7 +814,7 @@ static int goku_dequeue(struct usb_ep *_ep, struct usb_request *_req)
 	unsigned long		flags;
 
 	ep = container_of(_ep, struct goku_ep, ep);
-	if (!_ep || !_req || (!ep->desc && ep->num != 0))
+	if (!_ep || !_req || (!ep->ep.desc && ep->num != 0))
 		return -EINVAL;
 	dev = ep->dev;
 	if (!dev->driver)
@@ -896,7 +895,7 @@ static int goku_set_halt(struct usb_ep *_ep, int value)
 			return -EINVAL;
 
 	/* don't change EPxSTATUS_EP_INVALID to READY */
-	} else if (!ep->desc) {
+	} else if (!ep->ep.desc) {
 		DBG(ep->dev, "%s %s inactive?\n", __func__, ep->ep.name);
 		return -EINVAL;
 	}
@@ -955,7 +954,7 @@ static void goku_fifo_flush(struct usb_ep *_ep)
 	VDBG(ep->dev, "%s %s\n", __func__, ep->ep.name);
 
 	/* don't change EPxSTATUS_EP_INVALID to READY */
-	if (!ep->desc && ep->num != 0) {
+	if (!ep->ep.desc && ep->num != 0) {
 		DBG(ep->dev, "%s %s inactive?\n", __func__, ep->ep.name);
 		return;
 	}
@@ -1152,7 +1151,7 @@ udc_proc_read(char *buffer, char **start, off_t off, int count,
 		struct goku_ep		*ep = &dev->ep [i];
 		struct goku_request	*req;
 
-		if (i && !ep->desc)
+		if (i && !ep->ep.desc)
 			continue;
 
 		tmp = readl(ep->reg_status);
@@ -1473,7 +1472,8 @@ static void ep0_setup(struct goku_udc *dev)
 			case USB_RECIP_ENDPOINT:
 				tmp = le16_to_cpu(ctrl.wIndex) & 0x0f;
 				/* active endpoint */
-				if (tmp > 3 || (!dev->ep[tmp].desc && tmp != 0))
+				if (tmp > 3 ||
+				    (!dev->ep[tmp].ep.desc && tmp != 0))
 					goto stall;
 				if (ctrl.wIndex & cpu_to_le16(
 						USB_DIR_IN)) {
@@ -1895,14 +1895,4 @@ static struct pci_driver goku_pci_driver = {
 	/* FIXME add power management support */
 };
 
-static int __init init (void)
-{
-	return pci_register_driver (&goku_pci_driver);
-}
-module_init (init);
-
-static void __exit cleanup (void)
-{
-	pci_unregister_driver (&goku_pci_driver);
-}
-module_exit (cleanup);
+module_pci_driver(goku_pci_driver);
diff --git a/drivers/usb/gadget/goku_udc.h b/drivers/usb/gadget/goku_udc.h
index e7e0c69d3b1..85cdce0d190 100644
--- a/drivers/usb/gadget/goku_udc.h
+++ b/drivers/usb/gadget/goku_udc.h
@@ -216,7 +216,6 @@ struct goku_ep {
 
 	/* analogous to a host-side qh */
 	struct list_head			queue;
-	const struct usb_endpoint_descriptor	*desc;
 
 	u32 __iomem				*reg_fifo;
 	u32 __iomem				*reg_mode;
diff --git a/drivers/usb/gadget/imx_udc.c b/drivers/usb/gadget/imx_udc.c
index 8d1c75abd73..54034f84f99 100644
--- a/drivers/usb/gadget/imx_udc.c
+++ b/drivers/usb/gadget/imx_udc.c
@@ -1237,14 +1237,15 @@ irq_handler_t intr_handler(int i)
  *******************************************************************************
  */
 
-static int imx_udc_start(struct usb_gadget_driver *driver,
-		int (*bind)(struct usb_gadget *));
-static int imx_udc_stop(struct usb_gadget_driver *driver);
+static int imx_udc_start(struct usb_gadget *gadget,
+		struct usb_gadget_driver *driver);
+static int imx_udc_stop(struct usb_gadget *gadget,
+		struct usb_gadget_driver *driver);
 static const struct usb_gadget_ops imx_udc_ops = {
-	.get_frame	 = imx_udc_get_frame,
-	.wakeup		 = imx_udc_wakeup,
-	.start		= imx_udc_start,
-	.stop		= imx_udc_stop,
+	.get_frame	= imx_udc_get_frame,
+	.wakeup		= imx_udc_wakeup,
+	.udc_start	= imx_udc_start,
+	.udc_stop	= imx_udc_stop,
 };
 
 static struct imx_udc_struct controller = {
@@ -1329,23 +1330,13 @@ static struct imx_udc_struct controller = {
  * USB gadget driver functions
  *******************************************************************************
  */
-static int imx_udc_start(struct usb_gadget_driver *driver,
-		int (*bind)(struct usb_gadget *))
+static int imx_udc_start(struct usb_gadget *gadget,
+		struct usb_gadget_driver *driver)
 {
-	struct imx_udc_struct *imx_usb = &controller;
+	struct imx_udc_struct *imx_usb;
 	int retval;
 
-	if (!driver
-		|| driver->max_speed < USB_SPEED_FULL
-		|| !bind
-		|| !driver->disconnect
-		|| !driver->setup)
-			return -EINVAL;
-	if (!imx_usb)
-		return -ENODEV;
-	if (imx_usb->driver)
-		return -EBUSY;
-
+	imx_usb = container_of(gadget, struct imx_udc_struct, gadget);
 	/* first hook up the driver ... */
 	imx_usb->driver = driver;
 	imx_usb->gadget.dev.driver = &driver->driver;
@@ -1353,14 +1344,6 @@ static int imx_udc_start(struct usb_gadget_driver *driver,
 	retval = device_add(&imx_usb->gadget.dev);
 	if (retval)
 		goto fail;
-	retval = bind(&imx_usb->gadget);
-	if (retval) {
-		D_ERR(imx_usb->dev, "<%s> bind to driver %s --> error %d\n",
-			__func__, driver->driver.name, retval);
-		device_del(&imx_usb->gadget.dev);
-
-		goto fail;
-	}
 
 	D_INI(imx_usb->dev, "<%s> registered gadget driver '%s'\n",
 		__func__, driver->driver.name);
@@ -1374,20 +1357,16 @@ fail:
 	return retval;
 }
 
-static int imx_udc_stop(struct usb_gadget_driver *driver)
+static int imx_udc_stop(struct usb_gadget *gadget,
+		struct usb_gadget_driver *driver)
 {
-	struct imx_udc_struct *imx_usb = &controller;
-
-	if (!imx_usb)
-		return -ENODEV;
-	if (!driver || driver != imx_usb->driver || !driver->unbind)
-		return -EINVAL;
+	struct imx_udc_struct *imx_usb = container_of(gadget,
+			struct imx_udc_struct, gadget);
 
 	udc_stop_activity(imx_usb, driver);
 	imx_udc_disable(imx_usb);
 	del_timer(&imx_usb->timer);
 
-	driver->unbind(&imx_usb->gadget);
 	imx_usb->gadget.dev.driver = NULL;
 	imx_usb->driver = NULL;
 
diff --git a/drivers/usb/gadget/langwell_udc.c b/drivers/usb/gadget/langwell_udc.c
deleted file mode 100644
index f9cedd52cf2..00000000000
--- a/drivers/usb/gadget/langwell_udc.c
+++ /dev/null
@@ -1,3434 +0,0 @@
-/*
- * Intel Langwell USB Device Controller driver
- * Copyright (C) 2008-2009, 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.
- */
-
-
-/* #undef	DEBUG */
-/* #undef	VERBOSE_DEBUG */
-
-#include <linux/module.h>
-#include <linux/pci.h>
-#include <linux/dma-mapping.h>
-#include <linux/kernel.h>
-#include <linux/delay.h>
-#include <linux/ioport.h>
-#include <linux/sched.h>
-#include <linux/slab.h>
-#include <linux/errno.h>
-#include <linux/init.h>
-#include <linux/timer.h>
-#include <linux/list.h>
-#include <linux/interrupt.h>
-#include <linux/moduleparam.h>
-#include <linux/device.h>
-#include <linux/usb/ch9.h>
-#include <linux/usb/gadget.h>
-#include <linux/usb/otg.h>
-#include <linux/pm.h>
-#include <linux/io.h>
-#include <linux/irq.h>
-#include <asm/unaligned.h>
-
-#include "langwell_udc.h"
-
-
-#define	DRIVER_DESC		"Intel Langwell USB Device Controller driver"
-#define	DRIVER_VERSION		"16 May 2009"
-
-static const char driver_name[] = "langwell_udc";
-static const char driver_desc[] = DRIVER_DESC;
-
-
-/* for endpoint 0 operations */
-static const struct usb_endpoint_descriptor
-langwell_ep0_desc = {
-	.bLength =		USB_DT_ENDPOINT_SIZE,
-	.bDescriptorType =	USB_DT_ENDPOINT,
-	.bEndpointAddress =	0,
-	.bmAttributes =		USB_ENDPOINT_XFER_CONTROL,
-	.wMaxPacketSize =	EP0_MAX_PKT_SIZE,
-};
-
-
-/*-------------------------------------------------------------------------*/
-/* debugging */
-
-#ifdef	VERBOSE_DEBUG
-static inline void print_all_registers(struct langwell_udc *dev)
-{
-	int	i;
-
-	/* Capability Registers */
-	dev_dbg(&dev->pdev->dev,
-		"Capability Registers (offset: 0x%04x, length: 0x%08x)\n",
-		CAP_REG_OFFSET, (u32)sizeof(struct langwell_cap_regs));
-	dev_dbg(&dev->pdev->dev, "caplength=0x%02x\n",
-			readb(&dev->cap_regs->caplength));
-	dev_dbg(&dev->pdev->dev, "hciversion=0x%04x\n",
-			readw(&dev->cap_regs->hciversion));
-	dev_dbg(&dev->pdev->dev, "hcsparams=0x%08x\n",
-			readl(&dev->cap_regs->hcsparams));
-	dev_dbg(&dev->pdev->dev, "hccparams=0x%08x\n",
-			readl(&dev->cap_regs->hccparams));
-	dev_dbg(&dev->pdev->dev, "dciversion=0x%04x\n",
-			readw(&dev->cap_regs->dciversion));
-	dev_dbg(&dev->pdev->dev, "dccparams=0x%08x\n",
-			readl(&dev->cap_regs->dccparams));
-
-	/* Operational Registers */
-	dev_dbg(&dev->pdev->dev,
-		"Operational Registers (offset: 0x%04x, length: 0x%08x)\n",
-		OP_REG_OFFSET, (u32)sizeof(struct langwell_op_regs));
-	dev_dbg(&dev->pdev->dev, "extsts=0x%08x\n",
-			readl(&dev->op_regs->extsts));
-	dev_dbg(&dev->pdev->dev, "extintr=0x%08x\n",
-			readl(&dev->op_regs->extintr));
-	dev_dbg(&dev->pdev->dev, "usbcmd=0x%08x\n",
-			readl(&dev->op_regs->usbcmd));
-	dev_dbg(&dev->pdev->dev, "usbsts=0x%08x\n",
-			readl(&dev->op_regs->usbsts));
-	dev_dbg(&dev->pdev->dev, "usbintr=0x%08x\n",
-			readl(&dev->op_regs->usbintr));
-	dev_dbg(&dev->pdev->dev, "frindex=0x%08x\n",
-			readl(&dev->op_regs->frindex));
-	dev_dbg(&dev->pdev->dev, "ctrldssegment=0x%08x\n",
-			readl(&dev->op_regs->ctrldssegment));
-	dev_dbg(&dev->pdev->dev, "deviceaddr=0x%08x\n",
-			readl(&dev->op_regs->deviceaddr));
-	dev_dbg(&dev->pdev->dev, "endpointlistaddr=0x%08x\n",
-			readl(&dev->op_regs->endpointlistaddr));
-	dev_dbg(&dev->pdev->dev, "ttctrl=0x%08x\n",
-			readl(&dev->op_regs->ttctrl));
-	dev_dbg(&dev->pdev->dev, "burstsize=0x%08x\n",
-			readl(&dev->op_regs->burstsize));
-	dev_dbg(&dev->pdev->dev, "txfilltuning=0x%08x\n",
-			readl(&dev->op_regs->txfilltuning));
-	dev_dbg(&dev->pdev->dev, "txttfilltuning=0x%08x\n",
-			readl(&dev->op_regs->txttfilltuning));
-	dev_dbg(&dev->pdev->dev, "ic_usb=0x%08x\n",
-			readl(&dev->op_regs->ic_usb));
-	dev_dbg(&dev->pdev->dev, "ulpi_viewport=0x%08x\n",
-			readl(&dev->op_regs->ulpi_viewport));
-	dev_dbg(&dev->pdev->dev, "configflag=0x%08x\n",
-			readl(&dev->op_regs->configflag));
-	dev_dbg(&dev->pdev->dev, "portsc1=0x%08x\n",
-			readl(&dev->op_regs->portsc1));
-	dev_dbg(&dev->pdev->dev, "devlc=0x%08x\n",
-			readl(&dev->op_regs->devlc));
-	dev_dbg(&dev->pdev->dev, "otgsc=0x%08x\n",
-			readl(&dev->op_regs->otgsc));
-	dev_dbg(&dev->pdev->dev, "usbmode=0x%08x\n",
-			readl(&dev->op_regs->usbmode));
-	dev_dbg(&dev->pdev->dev, "endptnak=0x%08x\n",
-			readl(&dev->op_regs->endptnak));
-	dev_dbg(&dev->pdev->dev, "endptnaken=0x%08x\n",
-			readl(&dev->op_regs->endptnaken));
-	dev_dbg(&dev->pdev->dev, "endptsetupstat=0x%08x\n",
-			readl(&dev->op_regs->endptsetupstat));
-	dev_dbg(&dev->pdev->dev, "endptprime=0x%08x\n",
-			readl(&dev->op_regs->endptprime));
-	dev_dbg(&dev->pdev->dev, "endptflush=0x%08x\n",
-			readl(&dev->op_regs->endptflush));
-	dev_dbg(&dev->pdev->dev, "endptstat=0x%08x\n",
-			readl(&dev->op_regs->endptstat));
-	dev_dbg(&dev->pdev->dev, "endptcomplete=0x%08x\n",
-			readl(&dev->op_regs->endptcomplete));
-
-	for (i = 0; i < dev->ep_max / 2; i++) {
-		dev_dbg(&dev->pdev->dev, "endptctrl[%d]=0x%08x\n",
-				i, readl(&dev->op_regs->endptctrl[i]));
-	}
-}
-#else
-
-#define	print_all_registers(dev)	do { } while (0)
-
-#endif /* VERBOSE_DEBUG */
-
-
-/*-------------------------------------------------------------------------*/
-
-#define	is_in(ep)	(((ep)->ep_num == 0) ? ((ep)->dev->ep0_dir ==	\
-			USB_DIR_IN) : (usb_endpoint_dir_in((ep)->desc)))
-
-#define	DIR_STRING(ep)	(is_in(ep) ? "in" : "out")
-
-
-static char *type_string(const struct usb_endpoint_descriptor *desc)
-{
-	switch (usb_endpoint_type(desc)) {
-	case USB_ENDPOINT_XFER_BULK:
-		return "bulk";
-	case USB_ENDPOINT_XFER_ISOC:
-		return "iso";
-	case USB_ENDPOINT_XFER_INT:
-		return "int";
-	};
-
-	return "control";
-}
-
-
-/* configure endpoint control registers */
-static void ep_reset(struct langwell_ep *ep, unsigned char ep_num,
-		unsigned char is_in, unsigned char ep_type)
-{
-	struct langwell_udc	*dev;
-	u32			endptctrl;
-
-	dev = ep->dev;
-	dev_vdbg(&dev->pdev->dev, "---> %s()\n", __func__);
-
-	endptctrl = readl(&dev->op_regs->endptctrl[ep_num]);
-	if (is_in) {	/* TX */
-		if (ep_num)
-			endptctrl |= EPCTRL_TXR;
-		endptctrl |= EPCTRL_TXE;
-		endptctrl |= ep_type << EPCTRL_TXT_SHIFT;
-	} else {	/* RX */
-		if (ep_num)
-			endptctrl |= EPCTRL_RXR;
-		endptctrl |= EPCTRL_RXE;
-		endptctrl |= ep_type << EPCTRL_RXT_SHIFT;
-	}
-
-	writel(endptctrl, &dev->op_regs->endptctrl[ep_num]);
-
-	dev_vdbg(&dev->pdev->dev, "<--- %s()\n", __func__);
-}
-
-
-/* reset ep0 dQH and endptctrl */
-static void ep0_reset(struct langwell_udc *dev)
-{
-	struct langwell_ep	*ep;
-	int			i;
-
-	dev_vdbg(&dev->pdev->dev, "---> %s()\n", __func__);
-
-	/* ep0 in and out */
-	for (i = 0; i < 2; i++) {
-		ep = &dev->ep[i];
-		ep->dev = dev;
-
-		/* ep0 dQH */
-		ep->dqh = &dev->ep_dqh[i];
-
-		/* configure ep0 endpoint capabilities in dQH */
-		ep->dqh->dqh_ios = 1;
-		ep->dqh->dqh_mpl = EP0_MAX_PKT_SIZE;
-
-		/* enable ep0-in HW zero length termination select */
-		if (is_in(ep))
-			ep->dqh->dqh_zlt = 0;
-		ep->dqh->dqh_mult = 0;
-
-		ep->dqh->dtd_next = DTD_TERM;
-
-		/* configure ep0 control registers */
-		ep_reset(&dev->ep[0], 0, i, USB_ENDPOINT_XFER_CONTROL);
-	}
-
-	dev_vdbg(&dev->pdev->dev, "<--- %s()\n", __func__);
-}
-
-
-/*-------------------------------------------------------------------------*/
-
-/* endpoints operations */
-
-/* configure endpoint, making it usable */
-static int langwell_ep_enable(struct usb_ep *_ep,
-		const struct usb_endpoint_descriptor *desc)
-{
-	struct langwell_udc	*dev;
-	struct langwell_ep	*ep;
-	u16			max = 0;
-	unsigned long		flags;
-	int			i, retval = 0;
-	unsigned char		zlt, ios = 0, mult = 0;
-
-	ep = container_of(_ep, struct langwell_ep, ep);
-	dev = ep->dev;
-	dev_vdbg(&dev->pdev->dev, "---> %s()\n", __func__);
-
-	if (!_ep || !desc || ep->desc
-			|| desc->bDescriptorType != USB_DT_ENDPOINT)
-		return -EINVAL;
-
-	if (!dev->driver || dev->gadget.speed == USB_SPEED_UNKNOWN)
-		return -ESHUTDOWN;
-
-	max = usb_endpoint_maxp(desc);
-
-	/*
-	 * disable HW zero length termination select
-	 * driver handles zero length packet through req->req.zero
-	 */
-	zlt = 1;
-
-	/*
-	 * sanity check type, direction, address, and then
-	 * initialize the endpoint capabilities fields in dQH
-	 */
-	switch (usb_endpoint_type(desc)) {
-	case USB_ENDPOINT_XFER_CONTROL:
-		ios = 1;
-		break;
-	case USB_ENDPOINT_XFER_BULK:
-		if ((dev->gadget.speed == USB_SPEED_HIGH
-					&& max != 512)
-				|| (dev->gadget.speed == USB_SPEED_FULL
-					&& max > 64)) {
-			goto done;
-		}
-		break;
-	case USB_ENDPOINT_XFER_INT:
-		if (strstr(ep->ep.name, "-iso")) /* bulk is ok */
-			goto done;
-
-		switch (dev->gadget.speed) {
-		case USB_SPEED_HIGH:
-			if (max <= 1024)
-				break;
-		case USB_SPEED_FULL:
-			if (max <= 64)
-				break;
-		default:
-			if (max <= 8)
-				break;
-			goto done;
-		}
-		break;
-	case USB_ENDPOINT_XFER_ISOC:
-		if (strstr(ep->ep.name, "-bulk")
-				|| strstr(ep->ep.name, "-int"))
-			goto done;
-
-		switch (dev->gadget.speed) {
-		case USB_SPEED_HIGH:
-			if (max <= 1024)
-				break;
-		case USB_SPEED_FULL:
-			if (max <= 1023)
-				break;
-		default:
-			goto done;
-		}
-		/*
-		 * FIXME:
-		 * calculate transactions needed for high bandwidth iso
-		 */
-		mult = (unsigned char)(1 + ((max >> 11) & 0x03));
-		max = max & 0x8ff;	/* bit 0~10 */
-		/* 3 transactions at most */
-		if (mult > 3)
-			goto done;
-		break;
-	default:
-		goto done;
-	}
-
-	spin_lock_irqsave(&dev->lock, flags);
-
-	ep->ep.maxpacket = max;
-	ep->desc = desc;
-	ep->stopped = 0;
-	ep->ep_num = usb_endpoint_num(desc);
-
-	/* ep_type */
-	ep->ep_type = usb_endpoint_type(desc);
-
-	/* configure endpoint control registers */
-	ep_reset(ep, ep->ep_num, is_in(ep), ep->ep_type);
-
-	/* configure endpoint capabilities in dQH */
-	i = ep->ep_num * 2 + is_in(ep);
-	ep->dqh = &dev->ep_dqh[i];
-	ep->dqh->dqh_ios = ios;
-	ep->dqh->dqh_mpl = cpu_to_le16(max);
-	ep->dqh->dqh_zlt = zlt;
-	ep->dqh->dqh_mult = mult;
-	ep->dqh->dtd_next = DTD_TERM;
-
-	dev_dbg(&dev->pdev->dev, "enabled %s (ep%d%s-%s), max %04x\n",
-			_ep->name,
-			ep->ep_num,
-			DIR_STRING(ep),
-			type_string(desc),
-			max);
-
-	spin_unlock_irqrestore(&dev->lock, flags);
-done:
-	dev_vdbg(&dev->pdev->dev, "<--- %s()\n", __func__);
-	return retval;
-}
-
-
-/*-------------------------------------------------------------------------*/
-
-/* retire a request */
-static void done(struct langwell_ep *ep, struct langwell_request *req,
-		int status)
-{
-	struct langwell_udc	*dev = ep->dev;
-	unsigned		stopped = ep->stopped;
-	struct langwell_dtd	*curr_dtd, *next_dtd;
-	int			i;
-
-	dev_vdbg(&dev->pdev->dev, "---> %s()\n", __func__);
-
-	/* remove the req from ep->queue */
-	list_del_init(&req->queue);
-
-	if (req->req.status == -EINPROGRESS)
-		req->req.status = status;
-	else
-		status = req->req.status;
-
-	/* free dTD for the request */
-	next_dtd = req->head;
-	for (i = 0; i < req->dtd_count; i++) {
-		curr_dtd = next_dtd;
-		if (i != req->dtd_count - 1)
-			next_dtd = curr_dtd->next_dtd_virt;
-		dma_pool_free(dev->dtd_pool, curr_dtd, curr_dtd->dtd_dma);
-	}
-
-	usb_gadget_unmap_request(&dev->gadget, &req->req, is_in(ep));
-
-	if (status != -ESHUTDOWN)
-		dev_dbg(&dev->pdev->dev,
-				"complete %s, req %p, stat %d, len %u/%u\n",
-				ep->ep.name, &req->req, status,
-				req->req.actual, req->req.length);
-
-	/* don't modify queue heads during completion callback */
-	ep->stopped = 1;
-
-	spin_unlock(&dev->lock);
-	/* complete routine from gadget driver */
-	if (req->req.complete)
-		req->req.complete(&ep->ep, &req->req);
-
-	spin_lock(&dev->lock);
-	ep->stopped = stopped;
-
-	dev_vdbg(&dev->pdev->dev, "<--- %s()\n", __func__);
-}
-
-
-static void langwell_ep_fifo_flush(struct usb_ep *_ep);
-
-/* delete all endpoint requests, called with spinlock held */
-static void nuke(struct langwell_ep *ep, int status)
-{
-	/* called with spinlock held */
-	ep->stopped = 1;
-
-	/* endpoint fifo flush */
-	if (&ep->ep && ep->desc)
-		langwell_ep_fifo_flush(&ep->ep);
-
-	while (!list_empty(&ep->queue)) {
-		struct langwell_request	*req = NULL;
-		req = list_entry(ep->queue.next, struct langwell_request,
-				queue);
-		done(ep, req, status);
-	}
-}
-
-
-/*-------------------------------------------------------------------------*/
-
-/* endpoint is no longer usable */
-static int langwell_ep_disable(struct usb_ep *_ep)
-{
-	struct langwell_ep	*ep;
-	unsigned long		flags;
-	struct langwell_udc	*dev;
-	int			ep_num;
-	u32			endptctrl;
-
-	ep = container_of(_ep, struct langwell_ep, ep);
-	dev = ep->dev;
-	dev_vdbg(&dev->pdev->dev, "---> %s()\n", __func__);
-
-	if (!_ep || !ep->desc)
-		return -EINVAL;
-
-	spin_lock_irqsave(&dev->lock, flags);
-
-	/* disable endpoint control register */
-	ep_num = ep->ep_num;
-	endptctrl = readl(&dev->op_regs->endptctrl[ep_num]);
-	if (is_in(ep))
-		endptctrl &= ~EPCTRL_TXE;
-	else
-		endptctrl &= ~EPCTRL_RXE;
-	writel(endptctrl, &dev->op_regs->endptctrl[ep_num]);
-
-	/* nuke all pending requests (does flush) */
-	nuke(ep, -ESHUTDOWN);
-
-	ep->desc = NULL;
-	ep->ep.desc = NULL;
-	ep->stopped = 1;
-
-	spin_unlock_irqrestore(&dev->lock, flags);
-
-	dev_dbg(&dev->pdev->dev, "disabled %s\n", _ep->name);
-	dev_vdbg(&dev->pdev->dev, "<--- %s()\n", __func__);
-
-	return 0;
-}
-
-
-/* allocate a request object to use with this endpoint */
-static struct usb_request *langwell_alloc_request(struct usb_ep *_ep,
-		gfp_t gfp_flags)
-{
-	struct langwell_ep	*ep;
-	struct langwell_udc	*dev;
-	struct langwell_request	*req = NULL;
-
-	if (!_ep)
-		return NULL;
-
-	ep = container_of(_ep, struct langwell_ep, ep);
-	dev = ep->dev;
-	dev_vdbg(&dev->pdev->dev, "---> %s()\n", __func__);
-
-	req = kzalloc(sizeof(*req), gfp_flags);
-	if (!req)
-		return NULL;
-
-	req->req.dma = DMA_ADDR_INVALID;
-	INIT_LIST_HEAD(&req->queue);
-
-	dev_vdbg(&dev->pdev->dev, "alloc request for %s\n", _ep->name);
-	dev_vdbg(&dev->pdev->dev, "<--- %s()\n", __func__);
-	return &req->req;
-}
-
-
-/* free a request object */
-static void langwell_free_request(struct usb_ep *_ep,
-		struct usb_request *_req)
-{
-	struct langwell_ep	*ep;
-	struct langwell_udc	*dev;
-	struct langwell_request	*req = NULL;
-
-	ep = container_of(_ep, struct langwell_ep, ep);
-	dev = ep->dev;
-	dev_vdbg(&dev->pdev->dev, "---> %s()\n", __func__);
-
-	if (!_ep || !_req)
-		return;
-
-	req = container_of(_req, struct langwell_request, req);
-	WARN_ON(!list_empty(&req->queue));
-
-	if (_req)
-		kfree(req);
-
-	dev_vdbg(&dev->pdev->dev, "free request for %s\n", _ep->name);
-	dev_vdbg(&dev->pdev->dev, "<--- %s()\n", __func__);
-}
-
-
-/*-------------------------------------------------------------------------*/
-
-/* queue dTD and PRIME endpoint */
-static int queue_dtd(struct langwell_ep *ep, struct langwell_request *req)
-{
-	u32			bit_mask, usbcmd, endptstat, dtd_dma;
-	u8			dtd_status;
-	int			i;
-	struct langwell_dqh	*dqh;
-	struct langwell_udc	*dev;
-
-	dev = ep->dev;
-	dev_vdbg(&dev->pdev->dev, "---> %s()\n", __func__);
-
-	i = ep->ep_num * 2 + is_in(ep);
-	dqh = &dev->ep_dqh[i];
-
-	if (ep->ep_num)
-		dev_vdbg(&dev->pdev->dev, "%s\n", ep->name);
-	else
-		/* ep0 */
-		dev_vdbg(&dev->pdev->dev, "%s-%s\n", ep->name, DIR_STRING(ep));
-
-	dev_vdbg(&dev->pdev->dev, "ep_dqh[%d] addr: 0x%p\n",
-			i, &(dev->ep_dqh[i]));
-
-	bit_mask = is_in(ep) ?
-		(1 << (ep->ep_num + 16)) : (1 << (ep->ep_num));
-
-	dev_vdbg(&dev->pdev->dev, "bit_mask = 0x%08x\n", bit_mask);
-
-	/* check if the pipe is empty */
-	if (!(list_empty(&ep->queue))) {
-		/* add dTD to the end of linked list */
-		struct langwell_request	*lastreq;
-		lastreq = list_entry(ep->queue.prev,
-				struct langwell_request, queue);
-
-		lastreq->tail->dtd_next =
-			cpu_to_le32(req->head->dtd_dma & DTD_NEXT_MASK);
-
-		/* read prime bit, if 1 goto out */
-		if (readl(&dev->op_regs->endptprime) & bit_mask)
-			goto out;
-
-		do {
-			/* set ATDTW bit in USBCMD */
-			usbcmd = readl(&dev->op_regs->usbcmd);
-			writel(usbcmd | CMD_ATDTW, &dev->op_regs->usbcmd);
-
-			/* read correct status bit */
-			endptstat = readl(&dev->op_regs->endptstat) & bit_mask;
-
-		} while (!(readl(&dev->op_regs->usbcmd) & CMD_ATDTW));
-
-		/* write ATDTW bit to 0 */
-		usbcmd = readl(&dev->op_regs->usbcmd);
-		writel(usbcmd & ~CMD_ATDTW, &dev->op_regs->usbcmd);
-
-		if (endptstat)
-			goto out;
-	}
-
-	/* write dQH next pointer and terminate bit to 0 */
-	dtd_dma = req->head->dtd_dma & DTD_NEXT_MASK;
-	dqh->dtd_next = cpu_to_le32(dtd_dma);
-
-	/* clear active and halt bit */
-	dtd_status = (u8) ~(DTD_STS_ACTIVE | DTD_STS_HALTED);
-	dqh->dtd_status &= dtd_status;
-	dev_vdbg(&dev->pdev->dev, "dqh->dtd_status = 0x%x\n", dqh->dtd_status);
-
-	/* ensure that updates to the dQH will occur before priming */
-	wmb();
-
-	/* write 1 to endptprime register to PRIME endpoint */
-	bit_mask = is_in(ep) ? (1 << (ep->ep_num + 16)) : (1 << ep->ep_num);
-	dev_vdbg(&dev->pdev->dev, "endprime bit_mask = 0x%08x\n", bit_mask);
-	writel(bit_mask, &dev->op_regs->endptprime);
-out:
-	dev_vdbg(&dev->pdev->dev, "<--- %s()\n", __func__);
-	return 0;
-}
-
-
-/* fill in the dTD structure to build a transfer descriptor */
-static struct langwell_dtd *build_dtd(struct langwell_request *req,
-		unsigned *length, dma_addr_t *dma, int *is_last)
-{
-	u32			 buf_ptr;
-	struct langwell_dtd	*dtd;
-	struct langwell_udc	*dev;
-	int			i;
-
-	dev = req->ep->dev;
-	dev_vdbg(&dev->pdev->dev, "---> %s()\n", __func__);
-
-	/* the maximum transfer length, up to 16k bytes */
-	*length = min(req->req.length - req->req.actual,
-			(unsigned)DTD_MAX_TRANSFER_LENGTH);
-
-	/* create dTD dma_pool resource */
-	dtd = dma_pool_alloc(dev->dtd_pool, GFP_KERNEL, dma);
-	if (dtd == NULL)
-		return dtd;
-	dtd->dtd_dma = *dma;
-
-	/* initialize buffer page pointers */
-	buf_ptr = (u32)(req->req.dma + req->req.actual);
-	for (i = 0; i < 5; i++)
-		dtd->dtd_buf[i] = cpu_to_le32(buf_ptr + i * PAGE_SIZE);
-
-	req->req.actual += *length;
-
-	/* fill in total bytes with transfer size */
-	dtd->dtd_total = cpu_to_le16(*length);
-	dev_vdbg(&dev->pdev->dev, "dtd->dtd_total = %d\n", dtd->dtd_total);
-
-	/* set is_last flag if req->req.zero is set or not */
-	if (req->req.zero) {
-		if (*length == 0 || (*length % req->ep->ep.maxpacket) != 0)
-			*is_last = 1;
-		else
-			*is_last = 0;
-	} else if (req->req.length == req->req.actual) {
-		*is_last = 1;
-	} else
-		*is_last = 0;
-
-	if (*is_last == 0)
-		dev_vdbg(&dev->pdev->dev, "multi-dtd request!\n");
-
-	/* set interrupt on complete bit for the last dTD */
-	if (*is_last && !req->req.no_interrupt)
-		dtd->dtd_ioc = 1;
-
-	/* set multiplier override 0 for non-ISO and non-TX endpoint */
-	dtd->dtd_multo = 0;
-
-	/* set the active bit of status field to 1 */
-	dtd->dtd_status = DTD_STS_ACTIVE;
-	dev_vdbg(&dev->pdev->dev, "dtd->dtd_status = 0x%02x\n",
-			dtd->dtd_status);
-
-	dev_vdbg(&dev->pdev->dev, "length = %d, dma addr= 0x%08x\n",
-			*length, (int)*dma);
-	dev_vdbg(&dev->pdev->dev, "<--- %s()\n", __func__);
-	return dtd;
-}
-
-
-/* generate dTD linked list for a request */
-static int req_to_dtd(struct langwell_request *req)
-{
-	unsigned		count;
-	int			is_last, is_first = 1;
-	struct langwell_dtd	*dtd, *last_dtd = NULL;
-	struct langwell_udc	*dev;
-	dma_addr_t		dma;
-
-	dev = req->ep->dev;
-	dev_vdbg(&dev->pdev->dev, "---> %s()\n", __func__);
-	do {
-		dtd = build_dtd(req, &count, &dma, &is_last);
-		if (dtd == NULL)
-			return -ENOMEM;
-
-		if (is_first) {
-			is_first = 0;
-			req->head = dtd;
-		} else {
-			last_dtd->dtd_next = cpu_to_le32(dma);
-			last_dtd->next_dtd_virt = dtd;
-		}
-		last_dtd = dtd;
-		req->dtd_count++;
-	} while (!is_last);
-
-	/* set terminate bit to 1 for the last dTD */
-	dtd->dtd_next = DTD_TERM;
-
-	req->tail = dtd;
-
-	dev_vdbg(&dev->pdev->dev, "<--- %s()\n", __func__);
-	return 0;
-}
-
-/*-------------------------------------------------------------------------*/
-
-/* queue (submits) an I/O requests to an endpoint */
-static int langwell_ep_queue(struct usb_ep *_ep, struct usb_request *_req,
-		gfp_t gfp_flags)
-{
-	struct langwell_request	*req;
-	struct langwell_ep	*ep;
-	struct langwell_udc	*dev;
-	unsigned long		flags;
-	int			is_iso = 0;
-	int			ret;
-
-	/* always require a cpu-view buffer */
-	req = container_of(_req, struct langwell_request, req);
-	ep = container_of(_ep, struct langwell_ep, ep);
-
-	if (!_req || !_req->complete || !_req->buf
-			|| !list_empty(&req->queue)) {
-		return -EINVAL;
-	}
-
-	if (unlikely(!_ep || !ep->desc))
-		return -EINVAL;
-
-	dev = ep->dev;
-	req->ep = ep;
-	dev_vdbg(&dev->pdev->dev, "---> %s()\n", __func__);
-
-	if (usb_endpoint_xfer_isoc(ep->desc)) {
-		if (req->req.length > ep->ep.maxpacket)
-			return -EMSGSIZE;
-		is_iso = 1;
-	}
-
-	if (unlikely(!dev->driver || dev->gadget.speed == USB_SPEED_UNKNOWN))
-		return -ESHUTDOWN;
-
-	/* set up dma mapping */
-	ret = usb_gadget_map_request(&dev->gadget, &req->req, is_in(ep));
-	if (ret)
-		return ret;
-
-	dev_dbg(&dev->pdev->dev,
-			"%s queue req %p, len %u, buf %p, dma 0x%08x\n",
-			_ep->name,
-			_req, _req->length, _req->buf, (int)_req->dma);
-
-	_req->status = -EINPROGRESS;
-	_req->actual = 0;
-	req->dtd_count = 0;
-
-	spin_lock_irqsave(&dev->lock, flags);
-
-	/* build and put dTDs to endpoint queue */
-	if (!req_to_dtd(req)) {
-		queue_dtd(ep, req);
-	} else {
-		spin_unlock_irqrestore(&dev->lock, flags);
-		return -ENOMEM;
-	}
-
-	/* update ep0 state */
-	if (ep->ep_num == 0)
-		dev->ep0_state = DATA_STATE_XMIT;
-
-	if (likely(req != NULL)) {
-		list_add_tail(&req->queue, &ep->queue);
-		dev_vdbg(&dev->pdev->dev, "list_add_tail()\n");
-	}
-
-	spin_unlock_irqrestore(&dev->lock, flags);
-
-	dev_vdbg(&dev->pdev->dev, "<--- %s()\n", __func__);
-	return 0;
-}
-
-
-/* dequeue (cancels, unlinks) an I/O request from an endpoint */
-static int langwell_ep_dequeue(struct usb_ep *_ep, struct usb_request *_req)
-{
-	struct langwell_ep	*ep;
-	struct langwell_udc	*dev;
-	struct langwell_request	*req;
-	unsigned long		flags;
-	int			stopped, ep_num, retval = 0;
-	u32			endptctrl;
-
-	ep = container_of(_ep, struct langwell_ep, ep);
-	dev = ep->dev;
-	dev_vdbg(&dev->pdev->dev, "---> %s()\n", __func__);
-
-	if (!_ep || !ep->desc || !_req)
-		return -EINVAL;
-
-	if (!dev->driver)
-		return -ESHUTDOWN;
-
-	spin_lock_irqsave(&dev->lock, flags);
-	stopped = ep->stopped;
-
-	/* quiesce dma while we patch the queue */
-	ep->stopped = 1;
-	ep_num = ep->ep_num;
-
-	/* disable endpoint control register */
-	endptctrl = readl(&dev->op_regs->endptctrl[ep_num]);
-	if (is_in(ep))
-		endptctrl &= ~EPCTRL_TXE;
-	else
-		endptctrl &= ~EPCTRL_RXE;
-	writel(endptctrl, &dev->op_regs->endptctrl[ep_num]);
-
-	/* make sure it's still queued on this endpoint */
-	list_for_each_entry(req, &ep->queue, queue) {
-		if (&req->req == _req)
-			break;
-	}
-
-	if (&req->req != _req) {
-		retval = -EINVAL;
-		goto done;
-	}
-
-	/* queue head may be partially complete. */
-	if (ep->queue.next == &req->queue) {
-		dev_dbg(&dev->pdev->dev, "unlink (%s) dma\n", _ep->name);
-		_req->status = -ECONNRESET;
-		langwell_ep_fifo_flush(&ep->ep);
-
-		/* not the last request in endpoint queue */
-		if (likely(ep->queue.next == &req->queue)) {
-			struct langwell_dqh	*dqh;
-			struct langwell_request	*next_req;
-
-			dqh = ep->dqh;
-			next_req = list_entry(req->queue.next,
-					struct langwell_request, queue);
-
-			/* point the dQH to the first dTD of next request */
-			writel((u32) next_req->head, &dqh->dqh_current);
-		}
-	} else {
-		struct langwell_request	*prev_req;
-
-		prev_req = list_entry(req->queue.prev,
-				struct langwell_request, queue);
-		writel(readl(&req->tail->dtd_next),
-				&prev_req->tail->dtd_next);
-	}
-
-	done(ep, req, -ECONNRESET);
-
-done:
-	/* enable endpoint again */
-	endptctrl = readl(&dev->op_regs->endptctrl[ep_num]);
-	if (is_in(ep))
-		endptctrl |= EPCTRL_TXE;
-	else
-		endptctrl |= EPCTRL_RXE;
-	writel(endptctrl, &dev->op_regs->endptctrl[ep_num]);
-
-	ep->stopped = stopped;
-	spin_unlock_irqrestore(&dev->lock, flags);
-
-	dev_vdbg(&dev->pdev->dev, "<--- %s()\n", __func__);
-	return retval;
-}
-
-
-/*-------------------------------------------------------------------------*/
-
-/* endpoint set/clear halt */
-static void ep_set_halt(struct langwell_ep *ep, int value)
-{
-	u32			endptctrl = 0;
-	int			ep_num;
-	struct langwell_udc	*dev = ep->dev;
-	dev_vdbg(&dev->pdev->dev, "---> %s()\n", __func__);
-
-	ep_num = ep->ep_num;
-	endptctrl = readl(&dev->op_regs->endptctrl[ep_num]);
-
-	/* value: 1 - set halt, 0 - clear halt */
-	if (value) {
-		/* set the stall bit */
-		if (is_in(ep))
-			endptctrl |= EPCTRL_TXS;
-		else
-			endptctrl |= EPCTRL_RXS;
-	} else {
-		/* clear the stall bit and reset data toggle */
-		if (is_in(ep)) {
-			endptctrl &= ~EPCTRL_TXS;
-			endptctrl |= EPCTRL_TXR;
-		} else {
-			endptctrl &= ~EPCTRL_RXS;
-			endptctrl |= EPCTRL_RXR;
-		}
-	}
-
-	writel(endptctrl, &dev->op_regs->endptctrl[ep_num]);
-
-	dev_vdbg(&dev->pdev->dev, "<--- %s()\n", __func__);
-}
-
-
-/* set the endpoint halt feature */
-static int langwell_ep_set_halt(struct usb_ep *_ep, int value)
-{
-	struct langwell_ep	*ep;
-	struct langwell_udc	*dev;
-	unsigned long		flags;
-	int			retval = 0;
-
-	ep = container_of(_ep, struct langwell_ep, ep);
-	dev = ep->dev;
-
-	dev_vdbg(&dev->pdev->dev, "---> %s()\n", __func__);
-
-	if (!_ep || !ep->desc)
-		return -EINVAL;
-
-	if (!dev->driver || dev->gadget.speed == USB_SPEED_UNKNOWN)
-		return -ESHUTDOWN;
-
-	if (usb_endpoint_xfer_isoc(ep->desc))
-		return  -EOPNOTSUPP;
-
-	spin_lock_irqsave(&dev->lock, flags);
-
-	/*
-	 * attempt to halt IN ep will fail if any transfer requests
-	 * are still queue
-	 */
-	if (!list_empty(&ep->queue) && is_in(ep) && value) {
-		/* IN endpoint FIFO holds bytes */
-		dev_dbg(&dev->pdev->dev, "%s FIFO holds bytes\n", _ep->name);
-		retval = -EAGAIN;
-		goto done;
-	}
-
-	/* endpoint set/clear halt */
-	if (ep->ep_num) {
-		ep_set_halt(ep, value);
-	} else { /* endpoint 0 */
-		dev->ep0_state = WAIT_FOR_SETUP;
-		dev->ep0_dir = USB_DIR_OUT;
-	}
-done:
-	spin_unlock_irqrestore(&dev->lock, flags);
-	dev_dbg(&dev->pdev->dev, "%s %s halt\n",
-			_ep->name, value ? "set" : "clear");
-	dev_vdbg(&dev->pdev->dev, "<--- %s()\n", __func__);
-	return retval;
-}
-
-
-/* set the halt feature and ignores clear requests */
-static int langwell_ep_set_wedge(struct usb_ep *_ep)
-{
-	struct langwell_ep	*ep;
-	struct langwell_udc	*dev;
-
-	ep = container_of(_ep, struct langwell_ep, ep);
-	dev = ep->dev;
-
-	dev_vdbg(&dev->pdev->dev, "---> %s()\n", __func__);
-
-	if (!_ep || !ep->desc)
-		return -EINVAL;
-
-	dev_vdbg(&dev->pdev->dev, "<--- %s()\n", __func__);
-	return usb_ep_set_halt(_ep);
-}
-
-
-/* flush contents of a fifo */
-static void langwell_ep_fifo_flush(struct usb_ep *_ep)
-{
-	struct langwell_ep	*ep;
-	struct langwell_udc	*dev;
-	u32			flush_bit;
-	unsigned long		timeout;
-
-	ep = container_of(_ep, struct langwell_ep, ep);
-	dev = ep->dev;
-
-	dev_vdbg(&dev->pdev->dev, "---> %s()\n", __func__);
-
-	if (!_ep || !ep->desc) {
-		dev_vdbg(&dev->pdev->dev, "ep or ep->desc is NULL\n");
-		dev_vdbg(&dev->pdev->dev, "<--- %s()\n", __func__);
-		return;
-	}
-
-	dev_vdbg(&dev->pdev->dev, "%s-%s fifo flush\n",
-			_ep->name, DIR_STRING(ep));
-
-	/* flush endpoint buffer */
-	if (ep->ep_num == 0)
-		flush_bit = (1 << 16) | 1;
-	else if (is_in(ep))
-		flush_bit = 1 << (ep->ep_num + 16);	/* TX */
-	else
-		flush_bit = 1 << ep->ep_num;		/* RX */
-
-	/* wait until flush complete */
-	timeout = jiffies + FLUSH_TIMEOUT;
-	do {
-		writel(flush_bit, &dev->op_regs->endptflush);
-		while (readl(&dev->op_regs->endptflush)) {
-			if (time_after(jiffies, timeout)) {
-				dev_err(&dev->pdev->dev, "ep flush timeout\n");
-				goto done;
-			}
-			cpu_relax();
-		}
-	} while (readl(&dev->op_regs->endptstat) & flush_bit);
-done:
-	dev_vdbg(&dev->pdev->dev, "<--- %s()\n", __func__);
-}
-
-
-/* endpoints operations structure */
-static const struct usb_ep_ops langwell_ep_ops = {
-
-	/* configure endpoint, making it usable */
-	.enable		= langwell_ep_enable,
-
-	/* endpoint is no longer usable */
-	.disable	= langwell_ep_disable,
-
-	/* allocate a request object to use with this endpoint */
-	.alloc_request	= langwell_alloc_request,
-
-	/* free a request object */
-	.free_request	= langwell_free_request,
-
-	/* queue (submits) an I/O requests to an endpoint */
-	.queue		= langwell_ep_queue,
-
-	/* dequeue (cancels, unlinks) an I/O request from an endpoint */
-	.dequeue	= langwell_ep_dequeue,
-
-	/* set the endpoint halt feature */
-	.set_halt	= langwell_ep_set_halt,
-
-	/* set the halt feature and ignores clear requests */
-	.set_wedge	= langwell_ep_set_wedge,
-
-	/* flush contents of a fifo */
-	.fifo_flush	= langwell_ep_fifo_flush,
-};
-
-
-/*-------------------------------------------------------------------------*/
-
-/* device controller usb_gadget_ops structure */
-
-/* returns the current frame number */
-static int langwell_get_frame(struct usb_gadget *_gadget)
-{
-	struct langwell_udc	*dev;
-	u16			retval;
-
-	if (!_gadget)
-		return -ENODEV;
-
-	dev = container_of(_gadget, struct langwell_udc, gadget);
-	dev_vdbg(&dev->pdev->dev, "---> %s()\n", __func__);
-
-	retval = readl(&dev->op_regs->frindex) & FRINDEX_MASK;
-
-	dev_vdbg(&dev->pdev->dev, "<--- %s()\n", __func__);
-	return retval;
-}
-
-
-/* enter or exit PHY low power state */
-static void langwell_phy_low_power(struct langwell_udc *dev, bool flag)
-{
-	u32		devlc;
-	u8		devlc_byte2;
-	dev_dbg(&dev->pdev->dev, "---> %s()\n", __func__);
-
-	devlc = readl(&dev->op_regs->devlc);
-	dev_vdbg(&dev->pdev->dev, "devlc = 0x%08x\n", devlc);
-
-	if (flag)
-		devlc |= LPM_PHCD;
-	else
-		devlc &= ~LPM_PHCD;
-
-	/* FIXME: workaround for Langwell A1/A2/A3 sighting */
-	devlc_byte2 = (devlc >> 16) & 0xff;
-	writeb(devlc_byte2, (u8 *)&dev->op_regs->devlc + 2);
-
-	devlc = readl(&dev->op_regs->devlc);
-	dev_vdbg(&dev->pdev->dev,
-			"%s PHY low power suspend, devlc = 0x%08x\n",
-			flag ? "enter" : "exit", devlc);
-}
-
-
-/* tries to wake up the host connected to this gadget */
-static int langwell_wakeup(struct usb_gadget *_gadget)
-{
-	struct langwell_udc	*dev;
-	u32			portsc1;
-	unsigned long		flags;
-
-	if (!_gadget)
-		return 0;
-
-	dev = container_of(_gadget, struct langwell_udc, gadget);
-	dev_vdbg(&dev->pdev->dev, "---> %s()\n", __func__);
-
-	/* remote wakeup feature not enabled by host */
-	if (!dev->remote_wakeup) {
-		dev_info(&dev->pdev->dev, "remote wakeup is disabled\n");
-		return -ENOTSUPP;
-	}
-
-	spin_lock_irqsave(&dev->lock, flags);
-
-	portsc1 = readl(&dev->op_regs->portsc1);
-	if (!(portsc1 & PORTS_SUSP)) {
-		spin_unlock_irqrestore(&dev->lock, flags);
-		return 0;
-	}
-
-	/* LPM L1 to L0 or legacy remote wakeup */
-	if (dev->lpm && dev->lpm_state == LPM_L1)
-		dev_info(&dev->pdev->dev, "LPM L1 to L0 remote wakeup\n");
-	else
-		dev_info(&dev->pdev->dev, "device remote wakeup\n");
-
-	/* exit PHY low power suspend */
-	if (dev->pdev->device != 0x0829)
-		langwell_phy_low_power(dev, 0);
-
-	/* force port resume */
-	portsc1 |= PORTS_FPR;
-	writel(portsc1, &dev->op_regs->portsc1);
-
-	spin_unlock_irqrestore(&dev->lock, flags);
-
-	dev_vdbg(&dev->pdev->dev, "<--- %s()\n", __func__);
-	return 0;
-}
-
-
-/* notify controller that VBUS is powered or not */
-static int langwell_vbus_session(struct usb_gadget *_gadget, int is_active)
-{
-	struct langwell_udc	*dev;
-	unsigned long		flags;
-	u32			usbcmd;
-
-	if (!_gadget)
-		return -ENODEV;
-
-	dev = container_of(_gadget, struct langwell_udc, gadget);
-	dev_vdbg(&dev->pdev->dev, "---> %s()\n", __func__);
-
-	spin_lock_irqsave(&dev->lock, flags);
-	dev_vdbg(&dev->pdev->dev, "VBUS status: %s\n",
-			is_active ? "on" : "off");
-
-	dev->vbus_active = (is_active != 0);
-	if (dev->driver && dev->softconnected && dev->vbus_active) {
-		usbcmd = readl(&dev->op_regs->usbcmd);
-		usbcmd |= CMD_RUNSTOP;
-		writel(usbcmd, &dev->op_regs->usbcmd);
-	} else {
-		usbcmd = readl(&dev->op_regs->usbcmd);
-		usbcmd &= ~CMD_RUNSTOP;
-		writel(usbcmd, &dev->op_regs->usbcmd);
-	}
-
-	spin_unlock_irqrestore(&dev->lock, flags);
-
-	dev_vdbg(&dev->pdev->dev, "<--- %s()\n", __func__);
-	return 0;
-}
-
-
-/* constrain controller's VBUS power usage */
-static int langwell_vbus_draw(struct usb_gadget *_gadget, unsigned mA)
-{
-	struct langwell_udc	*dev;
-
-	if (!_gadget)
-		return -ENODEV;
-
-	dev = container_of(_gadget, struct langwell_udc, gadget);
-	dev_vdbg(&dev->pdev->dev, "---> %s()\n", __func__);
-
-	if (dev->transceiver) {
-		dev_vdbg(&dev->pdev->dev, "usb_phy_set_power\n");
-		dev_vdbg(&dev->pdev->dev, "<--- %s()\n", __func__);
-		return usb_phy_set_power(dev->transceiver, mA);
-	}
-
-	dev_vdbg(&dev->pdev->dev, "<--- %s()\n", __func__);
-	return -ENOTSUPP;
-}
-
-
-/* D+ pullup, software-controlled connect/disconnect to USB host */
-static int langwell_pullup(struct usb_gadget *_gadget, int is_on)
-{
-	struct langwell_udc	*dev;
-	u32			usbcmd;
-	unsigned long		flags;
-
-	if (!_gadget)
-		return -ENODEV;
-
-	dev = container_of(_gadget, struct langwell_udc, gadget);
-
-	dev_vdbg(&dev->pdev->dev, "---> %s()\n", __func__);
-
-	spin_lock_irqsave(&dev->lock, flags);
-	dev->softconnected = (is_on != 0);
-
-	if (dev->driver && dev->softconnected && dev->vbus_active) {
-		usbcmd = readl(&dev->op_regs->usbcmd);
-		usbcmd |= CMD_RUNSTOP;
-		writel(usbcmd, &dev->op_regs->usbcmd);
-	} else {
-		usbcmd = readl(&dev->op_regs->usbcmd);
-		usbcmd &= ~CMD_RUNSTOP;
-		writel(usbcmd, &dev->op_regs->usbcmd);
-	}
-	spin_unlock_irqrestore(&dev->lock, flags);
-
-	dev_vdbg(&dev->pdev->dev, "<--- %s()\n", __func__);
-	return 0;
-}
-
-static int langwell_start(struct usb_gadget *g,
-		struct usb_gadget_driver *driver);
-
-static int langwell_stop(struct usb_gadget *g,
-		struct usb_gadget_driver *driver);
-
-/* device controller usb_gadget_ops structure */
-static const struct usb_gadget_ops langwell_ops = {
-
-	/* returns the current frame number */
-	.get_frame	= langwell_get_frame,
-
-	/* tries to wake up the host connected to this gadget */
-	.wakeup		= langwell_wakeup,
-
-	/* set the device selfpowered feature, always selfpowered */
-	/* .set_selfpowered = langwell_set_selfpowered, */
-
-	/* notify controller that VBUS is powered or not */
-	.vbus_session	= langwell_vbus_session,
-
-	/* constrain controller's VBUS power usage */
-	.vbus_draw	= langwell_vbus_draw,
-
-	/* D+ pullup, software-controlled connect/disconnect to USB host */
-	.pullup		= langwell_pullup,
-
-	.udc_start	= langwell_start,
-	.udc_stop	= langwell_stop,
-};
-
-
-/*-------------------------------------------------------------------------*/
-
-/* device controller operations */
-
-/* reset device controller */
-static int langwell_udc_reset(struct langwell_udc *dev)
-{
-	u32		usbcmd, usbmode, devlc, endpointlistaddr;
-	u8		devlc_byte0, devlc_byte2;
-	unsigned long	timeout;
-
-	if (!dev)
-		return -EINVAL;
-
-	dev_dbg(&dev->pdev->dev, "---> %s()\n", __func__);
-
-	/* set controller to stop state */
-	usbcmd = readl(&dev->op_regs->usbcmd);
-	usbcmd &= ~CMD_RUNSTOP;
-	writel(usbcmd, &dev->op_regs->usbcmd);
-
-	/* reset device controller */
-	usbcmd = readl(&dev->op_regs->usbcmd);
-	usbcmd |= CMD_RST;
-	writel(usbcmd, &dev->op_regs->usbcmd);
-
-	/* wait for reset to complete */
-	timeout = jiffies + RESET_TIMEOUT;
-	while (readl(&dev->op_regs->usbcmd) & CMD_RST) {
-		if (time_after(jiffies, timeout)) {
-			dev_err(&dev->pdev->dev, "device reset timeout\n");
-			return -ETIMEDOUT;
-		}
-		cpu_relax();
-	}
-
-	/* set controller to device mode */
-	usbmode = readl(&dev->op_regs->usbmode);
-	usbmode |= MODE_DEVICE;
-
-	/* turn setup lockout off, require setup tripwire in usbcmd */
-	usbmode |= MODE_SLOM;
-
-	writel(usbmode, &dev->op_regs->usbmode);
-	usbmode = readl(&dev->op_regs->usbmode);
-	dev_vdbg(&dev->pdev->dev, "usbmode=0x%08x\n", usbmode);
-
-	/* Write-Clear setup status */
-	writel(0, &dev->op_regs->usbsts);
-
-	/* if support USB LPM, ACK all LPM token */
-	if (dev->lpm) {
-		devlc = readl(&dev->op_regs->devlc);
-		dev_vdbg(&dev->pdev->dev, "devlc = 0x%08x\n", devlc);
-		/* FIXME: workaround for Langwell A1/A2/A3 sighting */
-		devlc &= ~LPM_STL;	/* don't STALL LPM token */
-		devlc &= ~LPM_NYT_ACK;	/* ACK LPM token */
-		devlc_byte0 = devlc & 0xff;
-		devlc_byte2 = (devlc >> 16) & 0xff;
-		writeb(devlc_byte0, (u8 *)&dev->op_regs->devlc);
-		writeb(devlc_byte2, (u8 *)&dev->op_regs->devlc + 2);
-		devlc = readl(&dev->op_regs->devlc);
-		dev_vdbg(&dev->pdev->dev,
-				"ACK LPM token, devlc = 0x%08x\n", devlc);
-	}
-
-	/* fill endpointlistaddr register */
-	endpointlistaddr = dev->ep_dqh_dma;
-	endpointlistaddr &= ENDPOINTLISTADDR_MASK;
-	writel(endpointlistaddr, &dev->op_regs->endpointlistaddr);
-
-	dev_vdbg(&dev->pdev->dev,
-		"dQH base (vir: %p, phy: 0x%08x), endpointlistaddr=0x%08x\n",
-		dev->ep_dqh, endpointlistaddr,
-		readl(&dev->op_regs->endpointlistaddr));
-	dev_dbg(&dev->pdev->dev, "<--- %s()\n", __func__);
-	return 0;
-}
-
-
-/* reinitialize device controller endpoints */
-static int eps_reinit(struct langwell_udc *dev)
-{
-	struct langwell_ep	*ep;
-	char			name[14];
-	int			i;
-
-	dev_vdbg(&dev->pdev->dev, "---> %s()\n", __func__);
-
-	/* initialize ep0 */
-	ep = &dev->ep[0];
-	ep->dev = dev;
-	strncpy(ep->name, "ep0", sizeof(ep->name));
-	ep->ep.name = ep->name;
-	ep->ep.ops = &langwell_ep_ops;
-	ep->stopped = 0;
-	ep->ep.maxpacket = EP0_MAX_PKT_SIZE;
-	ep->ep_num = 0;
-	ep->desc = &langwell_ep0_desc;
-	INIT_LIST_HEAD(&ep->queue);
-
-	ep->ep_type = USB_ENDPOINT_XFER_CONTROL;
-
-	/* initialize other endpoints */
-	for (i = 2; i < dev->ep_max; i++) {
-		ep = &dev->ep[i];
-		if (i % 2)
-			snprintf(name, sizeof(name), "ep%din", i / 2);
-		else
-			snprintf(name, sizeof(name), "ep%dout", i / 2);
-		ep->dev = dev;
-		strncpy(ep->name, name, sizeof(ep->name));
-		ep->ep.name = ep->name;
-
-		ep->ep.ops = &langwell_ep_ops;
-		ep->stopped = 0;
-		ep->ep.maxpacket = (unsigned short) ~0;
-		ep->ep_num = i / 2;
-
-		INIT_LIST_HEAD(&ep->queue);
-		list_add_tail(&ep->ep.ep_list, &dev->gadget.ep_list);
-	}
-
-	dev_vdbg(&dev->pdev->dev, "<--- %s()\n", __func__);
-	return 0;
-}
-
-
-/* enable interrupt and set controller to run state */
-static void langwell_udc_start(struct langwell_udc *dev)
-{
-	u32	usbintr, usbcmd;
-	dev_dbg(&dev->pdev->dev, "---> %s()\n", __func__);
-
-	/* enable interrupts */
-	usbintr = INTR_ULPIE	/* ULPI */
-		| INTR_SLE	/* suspend */
-		/* | INTR_SRE	SOF received */
-		| INTR_URE	/* USB reset */
-		| INTR_AAE	/* async advance */
-		| INTR_SEE	/* system error */
-		| INTR_FRE	/* frame list rollover */
-		| INTR_PCE	/* port change detect */
-		| INTR_UEE	/* USB error interrupt */
-		| INTR_UE;	/* USB interrupt */
-	writel(usbintr, &dev->op_regs->usbintr);
-
-	/* clear stopped bit */
-	dev->stopped = 0;
-
-	/* set controller to run */
-	usbcmd = readl(&dev->op_regs->usbcmd);
-	usbcmd |= CMD_RUNSTOP;
-	writel(usbcmd, &dev->op_regs->usbcmd);
-
-	dev_dbg(&dev->pdev->dev, "<--- %s()\n", __func__);
-}
-
-
-/* disable interrupt and set controller to stop state */
-static void langwell_udc_stop(struct langwell_udc *dev)
-{
-	u32	usbcmd;
-
-	dev_dbg(&dev->pdev->dev, "---> %s()\n", __func__);
-
-	/* disable all interrupts */
-	writel(0, &dev->op_regs->usbintr);
-
-	/* set stopped bit */
-	dev->stopped = 1;
-
-	/* set controller to stop state */
-	usbcmd = readl(&dev->op_regs->usbcmd);
-	usbcmd &= ~CMD_RUNSTOP;
-	writel(usbcmd, &dev->op_regs->usbcmd);
-
-	dev_dbg(&dev->pdev->dev, "<--- %s()\n", __func__);
-}
-
-
-/* stop all USB activities */
-static void stop_activity(struct langwell_udc *dev)
-{
-	struct langwell_ep	*ep;
-	dev_dbg(&dev->pdev->dev, "---> %s()\n", __func__);
-
-	nuke(&dev->ep[0], -ESHUTDOWN);
-
-	list_for_each_entry(ep, &dev->gadget.ep_list, ep.ep_list) {
-		nuke(ep, -ESHUTDOWN);
-	}
-
-	/* report disconnect; the driver is already quiesced */
-	if (dev->driver) {
-		spin_unlock(&dev->lock);
-		dev->driver->disconnect(&dev->gadget);
-		spin_lock(&dev->lock);
-	}
-
-	dev_dbg(&dev->pdev->dev, "<--- %s()\n", __func__);
-}
-
-
-/*-------------------------------------------------------------------------*/
-
-/* device "function" sysfs attribute file */
-static ssize_t show_function(struct device *_dev,
-		struct device_attribute *attr, char *buf)
-{
-	struct langwell_udc	*dev = dev_get_drvdata(_dev);
-
-	if (!dev->driver || !dev->driver->function
-			|| strlen(dev->driver->function) > PAGE_SIZE)
-		return 0;
-
-	return scnprintf(buf, PAGE_SIZE, "%s\n", dev->driver->function);
-}
-static DEVICE_ATTR(function, S_IRUGO, show_function, NULL);
-
-
-static inline enum usb_device_speed lpm_device_speed(u32 reg)
-{
-	switch (LPM_PSPD(reg)) {
-	case LPM_SPEED_HIGH:
-		return USB_SPEED_HIGH;
-	case LPM_SPEED_FULL:
-		return USB_SPEED_FULL;
-	case LPM_SPEED_LOW:
-		return USB_SPEED_LOW;
-	default:
-		return USB_SPEED_UNKNOWN;
-	}
-}
-
-/* device "langwell_udc" sysfs attribute file */
-static ssize_t show_langwell_udc(struct device *_dev,
-		struct device_attribute *attr, char *buf)
-{
-	struct langwell_udc	*dev = dev_get_drvdata(_dev);
-	struct langwell_request *req;
-	struct langwell_ep	*ep = NULL;
-	char			*next;
-	unsigned		size;
-	unsigned		t;
-	unsigned		i;
-	unsigned long		flags;
-	u32			tmp_reg;
-
-	next = buf;
-	size = PAGE_SIZE;
-	spin_lock_irqsave(&dev->lock, flags);
-
-	/* driver basic information */
-	t = scnprintf(next, size,
-			DRIVER_DESC "\n"
-			"%s version: %s\n"
-			"Gadget driver: %s\n\n",
-			driver_name, DRIVER_VERSION,
-			dev->driver ? dev->driver->driver.name : "(none)");
-	size -= t;
-	next += t;
-
-	/* device registers */
-	tmp_reg = readl(&dev->op_regs->usbcmd);
-	t = scnprintf(next, size,
-			"USBCMD reg:\n"
-			"SetupTW: %d\n"
-			"Run/Stop: %s\n\n",
-			(tmp_reg & CMD_SUTW) ? 1 : 0,
-			(tmp_reg & CMD_RUNSTOP) ? "Run" : "Stop");
-	size -= t;
-	next += t;
-
-	tmp_reg = readl(&dev->op_regs->usbsts);
-	t = scnprintf(next, size,
-			"USB Status Reg:\n"
-			"Device Suspend: %d\n"
-			"Reset Received: %d\n"
-			"System Error: %s\n"
-			"USB Error Interrupt: %s\n\n",
-			(tmp_reg & STS_SLI) ? 1 : 0,
-			(tmp_reg & STS_URI) ? 1 : 0,
-			(tmp_reg & STS_SEI) ? "Error" : "No error",
-			(tmp_reg & STS_UEI) ? "Error detected" : "No error");
-	size -= t;
-	next += t;
-
-	tmp_reg = readl(&dev->op_regs->usbintr);
-	t = scnprintf(next, size,
-			"USB Intrrupt Enable Reg:\n"
-			"Sleep Enable: %d\n"
-			"SOF Received Enable: %d\n"
-			"Reset Enable: %d\n"
-			"System Error Enable: %d\n"
-			"Port Change Dectected Enable: %d\n"
-			"USB Error Intr Enable: %d\n"
-			"USB Intr Enable: %d\n\n",
-			(tmp_reg & INTR_SLE) ? 1 : 0,
-			(tmp_reg & INTR_SRE) ? 1 : 0,
-			(tmp_reg & INTR_URE) ? 1 : 0,
-			(tmp_reg & INTR_SEE) ? 1 : 0,
-			(tmp_reg & INTR_PCE) ? 1 : 0,
-			(tmp_reg & INTR_UEE) ? 1 : 0,
-			(tmp_reg & INTR_UE) ? 1 : 0);
-	size -= t;
-	next += t;
-
-	tmp_reg = readl(&dev->op_regs->frindex);
-	t = scnprintf(next, size,
-			"USB Frame Index Reg:\n"
-			"Frame Number is 0x%08x\n\n",
-			(tmp_reg & FRINDEX_MASK));
-	size -= t;
-	next += t;
-
-	tmp_reg = readl(&dev->op_regs->deviceaddr);
-	t = scnprintf(next, size,
-			"USB Device Address Reg:\n"
-			"Device Addr is 0x%x\n\n",
-			USBADR(tmp_reg));
-	size -= t;
-	next += t;
-
-	tmp_reg = readl(&dev->op_regs->endpointlistaddr);
-	t = scnprintf(next, size,
-			"USB Endpoint List Address Reg:\n"
-			"Endpoint List Pointer is 0x%x\n\n",
-			EPBASE(tmp_reg));
-	size -= t;
-	next += t;
-
-	tmp_reg = readl(&dev->op_regs->portsc1);
-	t = scnprintf(next, size,
-		"USB Port Status & Control Reg:\n"
-		"Port Reset: %s\n"
-		"Port Suspend Mode: %s\n"
-		"Over-current Change: %s\n"
-		"Port Enable/Disable Change: %s\n"
-		"Port Enabled/Disabled: %s\n"
-		"Current Connect Status: %s\n"
-		"LPM Suspend Status: %s\n\n",
-		(tmp_reg & PORTS_PR) ? "Reset" : "Not Reset",
-		(tmp_reg & PORTS_SUSP) ? "Suspend " : "Not Suspend",
-		(tmp_reg & PORTS_OCC) ? "Detected" : "No",
-		(tmp_reg & PORTS_PEC) ? "Changed" : "Not Changed",
-		(tmp_reg & PORTS_PE) ? "Enable" : "Not Correct",
-		(tmp_reg & PORTS_CCS) ?  "Attached" : "Not Attached",
-		(tmp_reg & PORTS_SLP) ? "LPM L1" : "LPM L0");
-	size -= t;
-	next += t;
-
-	tmp_reg = readl(&dev->op_regs->devlc);
-	t = scnprintf(next, size,
-		"Device LPM Control Reg:\n"
-		"Parallel Transceiver : %d\n"
-		"Serial Transceiver : %d\n"
-		"Port Speed: %s\n"
-		"Port Force Full Speed Connenct: %s\n"
-		"PHY Low Power Suspend Clock: %s\n"
-		"BmAttributes: %d\n\n",
-		LPM_PTS(tmp_reg),
-		(tmp_reg & LPM_STS) ? 1 : 0,
-		usb_speed_string(lpm_device_speed(tmp_reg)),
-		(tmp_reg & LPM_PFSC) ? "Force Full Speed" : "Not Force",
-		(tmp_reg & LPM_PHCD) ? "Disabled" : "Enabled",
-		LPM_BA(tmp_reg));
-	size -= t;
-	next += t;
-
-	tmp_reg = readl(&dev->op_regs->usbmode);
-	t = scnprintf(next, size,
-			"USB Mode Reg:\n"
-			"Controller Mode is : %s\n\n", ({
-				char *s;
-				switch (MODE_CM(tmp_reg)) {
-				case MODE_IDLE:
-					s = "Idle"; break;
-				case MODE_DEVICE:
-					s = "Device Controller"; break;
-				case MODE_HOST:
-					s = "Host Controller"; break;
-				default:
-					s = "None"; break;
-				}
-				s;
-			}));
-	size -= t;
-	next += t;
-
-	tmp_reg = readl(&dev->op_regs->endptsetupstat);
-	t = scnprintf(next, size,
-			"Endpoint Setup Status Reg:\n"
-			"SETUP on ep 0x%04x\n\n",
-			tmp_reg & SETUPSTAT_MASK);
-	size -= t;
-	next += t;
-
-	for (i = 0; i < dev->ep_max / 2; i++) {
-		tmp_reg = readl(&dev->op_regs->endptctrl[i]);
-		t = scnprintf(next, size, "EP Ctrl Reg [%d]: 0x%08x\n",
-				i, tmp_reg);
-		size -= t;
-		next += t;
-	}
-	tmp_reg = readl(&dev->op_regs->endptprime);
-	t = scnprintf(next, size, "EP Prime Reg: 0x%08x\n\n", tmp_reg);
-	size -= t;
-	next += t;
-
-	/* langwell_udc, langwell_ep, langwell_request structure information */
-	ep = &dev->ep[0];
-	t = scnprintf(next, size, "%s MaxPacketSize: 0x%x, ep_num: %d\n",
-			ep->ep.name, ep->ep.maxpacket, ep->ep_num);
-	size -= t;
-	next += t;
-
-	if (list_empty(&ep->queue)) {
-		t = scnprintf(next, size, "its req queue is empty\n\n");
-		size -= t;
-		next += t;
-	} else {
-		list_for_each_entry(req, &ep->queue, queue) {
-			t = scnprintf(next, size,
-				"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, &dev->gadget.ep_list, ep.ep_list) {
-		if (ep->desc) {
-			t = scnprintf(next, size,
-					"\n%s MaxPacketSize: 0x%x, "
-					"ep_num: %d\n",
-					ep->ep.name, ep->ep.maxpacket,
-					ep->ep_num);
-			size -= t;
-			next += t;
-
-			if (list_empty(&ep->queue)) {
-				t = scnprintf(next, size,
-						"its req queue is empty\n\n");
-				size -= t;
-				next += t;
-			} else {
-				list_for_each_entry(req, &ep->queue, queue) {
-					t = scnprintf(next, size,
-						"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;
-				}
-			}
-		}
-	}
-
-	spin_unlock_irqrestore(&dev->lock, flags);
-	return PAGE_SIZE - size;
-}
-static DEVICE_ATTR(langwell_udc, S_IRUGO, show_langwell_udc, NULL);
-
-
-/* device "remote_wakeup" sysfs attribute file */
-static ssize_t store_remote_wakeup(struct device *_dev,
-		struct device_attribute *attr, const char *buf, size_t count)
-{
-	struct langwell_udc	*dev = dev_get_drvdata(_dev);
-	unsigned long		flags;
-	ssize_t			rc = count;
-
-	if (count > 2)
-		return -EINVAL;
-
-	if (count > 0 && buf[count-1] == '\n')
-		((char *) buf)[count-1] = 0;
-
-	if (buf[0] != '1')
-		return -EINVAL;
-
-	/* force remote wakeup enabled in case gadget driver doesn't support */
-	spin_lock_irqsave(&dev->lock, flags);
-	dev->remote_wakeup = 1;
-	dev->dev_status |= (1 << USB_DEVICE_REMOTE_WAKEUP);
-	spin_unlock_irqrestore(&dev->lock, flags);
-
-	langwell_wakeup(&dev->gadget);
-
-	return rc;
-}
-static DEVICE_ATTR(remote_wakeup, S_IWUSR, NULL, store_remote_wakeup);
-
-
-/*-------------------------------------------------------------------------*/
-
-/*
- * when a driver is successfully registered, it will receive
- * control requests including set_configuration(), which enables
- * non-control requests.  then usb traffic follows until a
- * disconnect is reported.  then a host may connect again, or
- * the driver might get unbound.
- */
-
-static int langwell_start(struct usb_gadget *g,
-		struct usb_gadget_driver *driver)
-{
-	struct langwell_udc	*dev = gadget_to_langwell(g);
-	unsigned long		flags;
-	int			retval;
-
-	dev_dbg(&dev->pdev->dev, "---> %s()\n", __func__);
-
-	spin_lock_irqsave(&dev->lock, flags);
-
-	/* hook up the driver ... */
-	driver->driver.bus = NULL;
-	dev->driver = driver;
-	dev->gadget.dev.driver = &driver->driver;
-
-	spin_unlock_irqrestore(&dev->lock, flags);
-
-	retval = device_create_file(&dev->pdev->dev, &dev_attr_function);
-	if (retval)
-		goto err;
-
-	dev->usb_state = USB_STATE_ATTACHED;
-	dev->ep0_state = WAIT_FOR_SETUP;
-	dev->ep0_dir = USB_DIR_OUT;
-
-	/* enable interrupt and set controller to run state */
-	if (dev->got_irq)
-		langwell_udc_start(dev);
-
-	dev_vdbg(&dev->pdev->dev,
-			"After langwell_udc_start(), print all registers:\n");
-	print_all_registers(dev);
-
-	dev_info(&dev->pdev->dev, "register driver: %s\n",
-			driver->driver.name);
-	dev_dbg(&dev->pdev->dev, "<--- %s()\n", __func__);
-
-	return 0;
-
-err:
-	dev->gadget.dev.driver = NULL;
-	dev->driver = NULL;
-
-	dev_dbg(&dev->pdev->dev, "<--- %s()\n", __func__);
-
-	return retval;
-}
-
-/* unregister gadget driver */
-static int langwell_stop(struct usb_gadget *g,
-		struct usb_gadget_driver *driver)
-{
-	struct langwell_udc	*dev = gadget_to_langwell(g);
-	unsigned long		flags;
-
-	dev_dbg(&dev->pdev->dev, "---> %s()\n", __func__);
-
-	/* exit PHY low power suspend */
-	if (dev->pdev->device != 0x0829)
-		langwell_phy_low_power(dev, 0);
-
-	/* unbind OTG transceiver */
-	if (dev->transceiver)
-		(void)otg_set_peripheral(dev->transceiver->otg, 0);
-
-	/* disable interrupt and set controller to stop state */
-	langwell_udc_stop(dev);
-
-	dev->usb_state = USB_STATE_ATTACHED;
-	dev->ep0_state = WAIT_FOR_SETUP;
-	dev->ep0_dir = USB_DIR_OUT;
-
-	spin_lock_irqsave(&dev->lock, flags);
-
-	/* stop all usb activities */
-	dev->gadget.speed = USB_SPEED_UNKNOWN;
-	dev->gadget.dev.driver = NULL;
-	dev->driver = NULL;
-	stop_activity(dev);
-	spin_unlock_irqrestore(&dev->lock, flags);
-
-	device_remove_file(&dev->pdev->dev, &dev_attr_function);
-
-	dev_info(&dev->pdev->dev, "unregistered driver '%s'\n",
-			driver->driver.name);
-	dev_dbg(&dev->pdev->dev, "<--- %s()\n", __func__);
-
-	return 0;
-}
-
-/*-------------------------------------------------------------------------*/
-
-/*
- * setup tripwire is used as a semaphore to ensure that the setup data
- * payload is extracted from a dQH without being corrupted
- */
-static void setup_tripwire(struct langwell_udc *dev)
-{
-	u32			usbcmd,
-				endptsetupstat;
-	unsigned long		timeout;
-	struct langwell_dqh	*dqh;
-
-	dev_vdbg(&dev->pdev->dev, "---> %s()\n", __func__);
-
-	/* ep0 OUT dQH */
-	dqh = &dev->ep_dqh[EP_DIR_OUT];
-
-	/* Write-Clear endptsetupstat */
-	endptsetupstat = readl(&dev->op_regs->endptsetupstat);
-	writel(endptsetupstat, &dev->op_regs->endptsetupstat);
-
-	/* wait until endptsetupstat is cleared */
-	timeout = jiffies + SETUPSTAT_TIMEOUT;
-	while (readl(&dev->op_regs->endptsetupstat)) {
-		if (time_after(jiffies, timeout)) {
-			dev_err(&dev->pdev->dev, "setup_tripwire timeout\n");
-			break;
-		}
-		cpu_relax();
-	}
-
-	/* while a hazard exists when setup packet arrives */
-	do {
-		/* set setup tripwire bit */
-		usbcmd = readl(&dev->op_regs->usbcmd);
-		writel(usbcmd | CMD_SUTW, &dev->op_regs->usbcmd);
-
-		/* copy the setup packet to local buffer */
-		memcpy(&dev->local_setup_buff, &dqh->dqh_setup, 8);
-	} while (!(readl(&dev->op_regs->usbcmd) & CMD_SUTW));
-
-	/* Write-Clear setup tripwire bit */
-	usbcmd = readl(&dev->op_regs->usbcmd);
-	writel(usbcmd & ~CMD_SUTW, &dev->op_regs->usbcmd);
-
-	dev_vdbg(&dev->pdev->dev, "<--- %s()\n", __func__);
-}
-
-
-/* protocol ep0 stall, will automatically be cleared on new transaction */
-static void ep0_stall(struct langwell_udc *dev)
-{
-	u32	endptctrl;
-
-	dev_vdbg(&dev->pdev->dev, "---> %s()\n", __func__);
-
-	/* set TX and RX to stall */
-	endptctrl = readl(&dev->op_regs->endptctrl[0]);
-	endptctrl |= EPCTRL_TXS | EPCTRL_RXS;
-	writel(endptctrl, &dev->op_regs->endptctrl[0]);
-
-	/* update ep0 state */
-	dev->ep0_state = WAIT_FOR_SETUP;
-	dev->ep0_dir = USB_DIR_OUT;
-
-	dev_vdbg(&dev->pdev->dev, "<--- %s()\n", __func__);
-}
-
-
-/* PRIME a status phase for ep0 */
-static int prime_status_phase(struct langwell_udc *dev, int dir)
-{
-	struct langwell_request	*req;
-	struct langwell_ep	*ep;
-	int			status = 0;
-
-	dev_vdbg(&dev->pdev->dev, "---> %s()\n", __func__);
-
-	if (dir == EP_DIR_IN)
-		dev->ep0_dir = USB_DIR_IN;
-	else
-		dev->ep0_dir = USB_DIR_OUT;
-
-	ep = &dev->ep[0];
-	dev->ep0_state = WAIT_FOR_OUT_STATUS;
-
-	req = dev->status_req;
-
-	req->ep = ep;
-	req->req.length = 0;
-	req->req.status = -EINPROGRESS;
-	req->req.actual = 0;
-	req->req.complete = NULL;
-	req->dtd_count = 0;
-
-	if (!req_to_dtd(req))
-		status = queue_dtd(ep, req);
-	else
-		return -ENOMEM;
-
-	if (status)
-		dev_err(&dev->pdev->dev, "can't queue ep0 status request\n");
-
-	list_add_tail(&req->queue, &ep->queue);
-
-	dev_vdbg(&dev->pdev->dev, "<--- %s()\n", __func__);
-	return status;
-}
-
-
-/* SET_ADDRESS request routine */
-static void set_address(struct langwell_udc *dev, u16 value,
-		u16 index, u16 length)
-{
-	dev_vdbg(&dev->pdev->dev, "---> %s()\n", __func__);
-
-	/* save the new address to device struct */
-	dev->dev_addr = (u8) value;
-	dev_vdbg(&dev->pdev->dev, "dev->dev_addr = %d\n", dev->dev_addr);
-
-	/* update usb state */
-	dev->usb_state = USB_STATE_ADDRESS;
-
-	/* STATUS phase */
-	if (prime_status_phase(dev, EP_DIR_IN))
-		ep0_stall(dev);
-
-	dev_vdbg(&dev->pdev->dev, "<--- %s()\n", __func__);
-}
-
-
-/* return endpoint by windex */
-static struct langwell_ep *get_ep_by_windex(struct langwell_udc *dev,
-		u16 wIndex)
-{
-	struct langwell_ep		*ep;
-	dev_vdbg(&dev->pdev->dev, "---> %s()\n", __func__);
-
-	if ((wIndex & USB_ENDPOINT_NUMBER_MASK) == 0)
-		return &dev->ep[0];
-
-	list_for_each_entry(ep, &dev->gadget.ep_list, ep.ep_list) {
-		u8	bEndpointAddress;
-		if (!ep->desc)
-			continue;
-
-		bEndpointAddress = ep->desc->bEndpointAddress;
-		if ((wIndex ^ bEndpointAddress) & USB_DIR_IN)
-			continue;
-
-		if ((wIndex & USB_ENDPOINT_NUMBER_MASK)
-			== (bEndpointAddress & USB_ENDPOINT_NUMBER_MASK))
-			return ep;
-	}
-
-	dev_vdbg(&dev->pdev->dev, "<--- %s()\n", __func__);
-	return NULL;
-}
-
-
-/* return whether endpoint is stalled, 0: not stalled; 1: stalled */
-static int ep_is_stall(struct langwell_ep *ep)
-{
-	struct langwell_udc	*dev = ep->dev;
-	u32			endptctrl;
-	int			retval;
-
-	dev_vdbg(&dev->pdev->dev, "---> %s()\n", __func__);
-
-	endptctrl = readl(&dev->op_regs->endptctrl[ep->ep_num]);
-	if (is_in(ep))
-		retval = endptctrl & EPCTRL_TXS ? 1 : 0;
-	else
-		retval = endptctrl & EPCTRL_RXS ? 1 : 0;
-
-	dev_vdbg(&dev->pdev->dev, "<--- %s()\n", __func__);
-	return retval;
-}
-
-
-/* GET_STATUS request routine */
-static void get_status(struct langwell_udc *dev, u8 request_type, u16 value,
-		u16 index, u16 length)
-{
-	struct langwell_request	*req;
-	struct langwell_ep	*ep;
-	u16	status_data = 0;	/* 16 bits cpu view status data */
-	int	status = 0;
-
-	dev_vdbg(&dev->pdev->dev, "---> %s()\n", __func__);
-
-	ep = &dev->ep[0];
-
-	if ((request_type & USB_RECIP_MASK) == USB_RECIP_DEVICE) {
-		/* get device status */
-		status_data = dev->dev_status;
-	} else if ((request_type & USB_RECIP_MASK) == USB_RECIP_INTERFACE) {
-		/* get interface status */
-		status_data = 0;
-	} else if ((request_type & USB_RECIP_MASK) == USB_RECIP_ENDPOINT) {
-		/* get endpoint status */
-		struct langwell_ep	*epn;
-		epn = get_ep_by_windex(dev, index);
-		/* stall if endpoint doesn't exist */
-		if (!epn)
-			goto stall;
-
-		status_data = ep_is_stall(epn) << USB_ENDPOINT_HALT;
-	}
-
-	dev_dbg(&dev->pdev->dev, "get status data: 0x%04x\n", status_data);
-
-	dev->ep0_dir = USB_DIR_IN;
-
-	/* borrow the per device status_req */
-	req = dev->status_req;
-
-	/* fill in the reqest structure */
-	*((u16 *) req->req.buf) = cpu_to_le16(status_data);
-	req->ep = ep;
-	req->req.length = 2;
-	req->req.status = -EINPROGRESS;
-	req->req.actual = 0;
-	req->req.complete = NULL;
-	req->dtd_count = 0;
-
-	/* prime the data phase */
-	if (!req_to_dtd(req))
-		status = queue_dtd(ep, req);
-	else			/* no mem */
-		goto stall;
-
-	if (status) {
-		dev_err(&dev->pdev->dev,
-				"response error on GET_STATUS request\n");
-		goto stall;
-	}
-
-	list_add_tail(&req->queue, &ep->queue);
-	dev->ep0_state = DATA_STATE_XMIT;
-
-	dev_vdbg(&dev->pdev->dev, "<--- %s()\n", __func__);
-	return;
-stall:
-	ep0_stall(dev);
-	dev_vdbg(&dev->pdev->dev, "<--- %s()\n", __func__);
-}
-
-
-/* setup packet interrupt handler */
-static void handle_setup_packet(struct langwell_udc *dev,
-		struct usb_ctrlrequest *setup)
-{
-	u16	wValue = le16_to_cpu(setup->wValue);
-	u16	wIndex = le16_to_cpu(setup->wIndex);
-	u16	wLength = le16_to_cpu(setup->wLength);
-	u32	portsc1;
-
-	dev_vdbg(&dev->pdev->dev, "---> %s()\n", __func__);
-
-	/* ep0 fifo flush */
-	nuke(&dev->ep[0], -ESHUTDOWN);
-
-	dev_dbg(&dev->pdev->dev, "SETUP %02x.%02x v%04x i%04x l%04x\n",
-			setup->bRequestType, setup->bRequest,
-			wValue, wIndex, wLength);
-
-	/* RNDIS gadget delegate */
-	if ((setup->bRequestType == 0x21) && (setup->bRequest == 0x00)) {
-		/* USB_CDC_SEND_ENCAPSULATED_COMMAND */
-		goto delegate;
-	}
-
-	/* USB_CDC_GET_ENCAPSULATED_RESPONSE */
-	if ((setup->bRequestType == 0xa1) && (setup->bRequest == 0x01)) {
-		/* USB_CDC_GET_ENCAPSULATED_RESPONSE */
-		goto delegate;
-	}
-
-	/* We process some stardard setup requests here */
-	switch (setup->bRequest) {
-	case USB_REQ_GET_STATUS:
-		dev_dbg(&dev->pdev->dev, "SETUP: USB_REQ_GET_STATUS\n");
-		/* get status, DATA and STATUS phase */
-		if ((setup->bRequestType & (USB_DIR_IN | USB_TYPE_MASK))
-					!= (USB_DIR_IN | USB_TYPE_STANDARD))
-			break;
-		get_status(dev, setup->bRequestType, wValue, wIndex, wLength);
-		goto end;
-
-	case USB_REQ_SET_ADDRESS:
-		dev_dbg(&dev->pdev->dev, "SETUP: USB_REQ_SET_ADDRESS\n");
-		/* STATUS phase */
-		if (setup->bRequestType != (USB_DIR_OUT | USB_TYPE_STANDARD
-						| USB_RECIP_DEVICE))
-			break;
-		set_address(dev, wValue, wIndex, wLength);
-		goto end;
-
-	case USB_REQ_CLEAR_FEATURE:
-	case USB_REQ_SET_FEATURE:
-		/* STATUS phase */
-	{
-		int rc = -EOPNOTSUPP;
-		if (setup->bRequest == USB_REQ_SET_FEATURE)
-			dev_dbg(&dev->pdev->dev,
-					"SETUP: USB_REQ_SET_FEATURE\n");
-		else if (setup->bRequest == USB_REQ_CLEAR_FEATURE)
-			dev_dbg(&dev->pdev->dev,
-					"SETUP: USB_REQ_CLEAR_FEATURE\n");
-
-		if ((setup->bRequestType & (USB_RECIP_MASK | USB_TYPE_MASK))
-				== (USB_RECIP_ENDPOINT | USB_TYPE_STANDARD)) {
-			struct langwell_ep	*epn;
-			epn = get_ep_by_windex(dev, wIndex);
-			/* stall if endpoint doesn't exist */
-			if (!epn) {
-				ep0_stall(dev);
-				goto end;
-			}
-
-			if (wValue != 0 || wLength != 0
-					|| epn->ep_num > dev->ep_max)
-				break;
-
-			spin_unlock(&dev->lock);
-			rc = langwell_ep_set_halt(&epn->ep,
-				(setup->bRequest == USB_REQ_SET_FEATURE)
-				? 1 : 0);
-			spin_lock(&dev->lock);
-
-		} else if ((setup->bRequestType & (USB_RECIP_MASK
-				| USB_TYPE_MASK)) == (USB_RECIP_DEVICE
-				| USB_TYPE_STANDARD)) {
-			rc = 0;
-			switch (wValue) {
-			case USB_DEVICE_REMOTE_WAKEUP:
-				if (setup->bRequest == USB_REQ_SET_FEATURE) {
-					dev->remote_wakeup = 1;
-					dev->dev_status |= (1 << wValue);
-				} else {
-					dev->remote_wakeup = 0;
-					dev->dev_status &= ~(1 << wValue);
-				}
-				break;
-			case USB_DEVICE_TEST_MODE:
-				dev_dbg(&dev->pdev->dev, "SETUP: TEST MODE\n");
-				if ((wIndex & 0xff) ||
-					(dev->gadget.speed != USB_SPEED_HIGH))
-					ep0_stall(dev);
-
-				switch (wIndex >> 8) {
-				case TEST_J:
-				case TEST_K:
-				case TEST_SE0_NAK:
-				case TEST_PACKET:
-				case TEST_FORCE_EN:
-					if (prime_status_phase(dev, EP_DIR_IN))
-						ep0_stall(dev);
-					portsc1 = readl(&dev->op_regs->portsc1);
-					portsc1 |= (wIndex & 0xf00) << 8;
-					writel(portsc1, &dev->op_regs->portsc1);
-					goto end;
-				default:
-					rc = -EOPNOTSUPP;
-				}
-				break;
-			default:
-				rc = -EOPNOTSUPP;
-				break;
-			}
-
-			if (!gadget_is_otg(&dev->gadget))
-				break;
-			else if (setup->bRequest == USB_DEVICE_B_HNP_ENABLE)
-				dev->gadget.b_hnp_enable = 1;
-			else if (setup->bRequest == USB_DEVICE_A_HNP_SUPPORT)
-				dev->gadget.a_hnp_support = 1;
-			else if (setup->bRequest ==
-					USB_DEVICE_A_ALT_HNP_SUPPORT)
-				dev->gadget.a_alt_hnp_support = 1;
-			else
-				break;
-		} else
-			break;
-
-		if (rc == 0) {
-			if (prime_status_phase(dev, EP_DIR_IN))
-				ep0_stall(dev);
-		}
-		goto end;
-	}
-
-	case USB_REQ_GET_DESCRIPTOR:
-		dev_dbg(&dev->pdev->dev,
-				"SETUP: USB_REQ_GET_DESCRIPTOR\n");
-		goto delegate;
-
-	case USB_REQ_SET_DESCRIPTOR:
-		dev_dbg(&dev->pdev->dev,
-				"SETUP: USB_REQ_SET_DESCRIPTOR unsupported\n");
-		goto delegate;
-
-	case USB_REQ_GET_CONFIGURATION:
-		dev_dbg(&dev->pdev->dev,
-				"SETUP: USB_REQ_GET_CONFIGURATION\n");
-		goto delegate;
-
-	case USB_REQ_SET_CONFIGURATION:
-		dev_dbg(&dev->pdev->dev,
-				"SETUP: USB_REQ_SET_CONFIGURATION\n");
-		goto delegate;
-
-	case USB_REQ_GET_INTERFACE:
-		dev_dbg(&dev->pdev->dev,
-				"SETUP: USB_REQ_GET_INTERFACE\n");
-		goto delegate;
-
-	case USB_REQ_SET_INTERFACE:
-		dev_dbg(&dev->pdev->dev,
-				"SETUP: USB_REQ_SET_INTERFACE\n");
-		goto delegate;
-
-	case USB_REQ_SYNCH_FRAME:
-		dev_dbg(&dev->pdev->dev,
-				"SETUP: USB_REQ_SYNCH_FRAME unsupported\n");
-		goto delegate;
-
-	default:
-		/* delegate USB standard requests to the gadget driver */
-		goto delegate;
-delegate:
-		/* USB requests handled by gadget */
-		if (wLength) {
-			/* DATA phase from gadget, STATUS phase from udc */
-			dev->ep0_dir = (setup->bRequestType & USB_DIR_IN)
-					?  USB_DIR_IN : USB_DIR_OUT;
-			dev_vdbg(&dev->pdev->dev,
-					"dev->ep0_dir = 0x%x, wLength = %d\n",
-					dev->ep0_dir, wLength);
-			spin_unlock(&dev->lock);
-			if (dev->driver->setup(&dev->gadget,
-					&dev->local_setup_buff) < 0)
-				ep0_stall(dev);
-			spin_lock(&dev->lock);
-			dev->ep0_state = (setup->bRequestType & USB_DIR_IN)
-					?  DATA_STATE_XMIT : DATA_STATE_RECV;
-		} else {
-			/* no DATA phase, IN STATUS phase from gadget */
-			dev->ep0_dir = USB_DIR_IN;
-			dev_vdbg(&dev->pdev->dev,
-					"dev->ep0_dir = 0x%x, wLength = %d\n",
-					dev->ep0_dir, wLength);
-			spin_unlock(&dev->lock);
-			if (dev->driver->setup(&dev->gadget,
-					&dev->local_setup_buff) < 0)
-				ep0_stall(dev);
-			spin_lock(&dev->lock);
-			dev->ep0_state = WAIT_FOR_OUT_STATUS;
-		}
-		break;
-	}
-end:
-	dev_vdbg(&dev->pdev->dev, "<--- %s()\n", __func__);
-}
-
-
-/* transfer completion, process endpoint request and free the completed dTDs
- * for this request
- */
-static int process_ep_req(struct langwell_udc *dev, int index,
-		struct langwell_request *curr_req)
-{
-	struct langwell_dtd	*curr_dtd;
-	struct langwell_dqh	*curr_dqh;
-	int			td_complete, actual, remaining_length;
-	int			i, dir;
-	u8			dtd_status = 0;
-	int			retval = 0;
-
-	curr_dqh = &dev->ep_dqh[index];
-	dir = index % 2;
-
-	curr_dtd = curr_req->head;
-	td_complete = 0;
-	actual = curr_req->req.length;
-
-	dev_vdbg(&dev->pdev->dev, "---> %s()\n", __func__);
-
-	for (i = 0; i < curr_req->dtd_count; i++) {
-
-		/* command execution states by dTD */
-		dtd_status = curr_dtd->dtd_status;
-
-		barrier();
-		remaining_length = le16_to_cpu(curr_dtd->dtd_total);
-		actual -= remaining_length;
-
-		if (!dtd_status) {
-			/* transfers completed successfully */
-			if (!remaining_length) {
-				td_complete++;
-				dev_vdbg(&dev->pdev->dev,
-					"dTD transmitted successfully\n");
-			} else {
-				if (dir) {
-					dev_vdbg(&dev->pdev->dev,
-						"TX dTD remains data\n");
-					retval = -EPROTO;
-					break;
-
-				} else {
-					td_complete++;
-					break;
-				}
-			}
-		} else {
-			/* transfers completed with errors */
-			if (dtd_status & DTD_STS_ACTIVE) {
-				dev_dbg(&dev->pdev->dev,
-					"dTD status ACTIVE dQH[%d]\n", index);
-				retval = 1;
-				return retval;
-			} else if (dtd_status & DTD_STS_HALTED) {
-				dev_err(&dev->pdev->dev,
-					"dTD error %08x dQH[%d]\n",
-					dtd_status, index);
-				/* clear the errors and halt condition */
-				curr_dqh->dtd_status = 0;
-				retval = -EPIPE;
-				break;
-			} else if (dtd_status & DTD_STS_DBE) {
-				dev_dbg(&dev->pdev->dev,
-					"data buffer (overflow) error\n");
-				retval = -EPROTO;
-				break;
-			} else if (dtd_status & DTD_STS_TRE) {
-				dev_dbg(&dev->pdev->dev,
-					"transaction(ISO) error\n");
-				retval = -EILSEQ;
-				break;
-			} else
-				dev_err(&dev->pdev->dev,
-					"unknown error (0x%x)!\n",
-					dtd_status);
-		}
-
-		if (i != curr_req->dtd_count - 1)
-			curr_dtd = (struct langwell_dtd *)
-				curr_dtd->next_dtd_virt;
-	}
-
-	if (retval)
-		return retval;
-
-	curr_req->req.actual = actual;
-
-	dev_vdbg(&dev->pdev->dev, "<--- %s()\n", __func__);
-	return 0;
-}
-
-
-/* complete DATA or STATUS phase of ep0 prime status phase if needed */
-static void ep0_req_complete(struct langwell_udc *dev,
-		struct langwell_ep *ep0, struct langwell_request *req)
-{
-	u32	new_addr;
-	dev_vdbg(&dev->pdev->dev, "---> %s()\n", __func__);
-
-	if (dev->usb_state == USB_STATE_ADDRESS) {
-		/* set the new address */
-		new_addr = (u32)dev->dev_addr;
-		writel(new_addr << USBADR_SHIFT, &dev->op_regs->deviceaddr);
-
-		new_addr = USBADR(readl(&dev->op_regs->deviceaddr));
-		dev_vdbg(&dev->pdev->dev, "new_addr = %d\n", new_addr);
-	}
-
-	done(ep0, req, 0);
-
-	switch (dev->ep0_state) {
-	case DATA_STATE_XMIT:
-		/* receive status phase */
-		if (prime_status_phase(dev, EP_DIR_OUT))
-			ep0_stall(dev);
-		break;
-	case DATA_STATE_RECV:
-		/* send status phase */
-		if (prime_status_phase(dev, EP_DIR_IN))
-			ep0_stall(dev);
-		break;
-	case WAIT_FOR_OUT_STATUS:
-		dev->ep0_state = WAIT_FOR_SETUP;
-		break;
-	case WAIT_FOR_SETUP:
-		dev_err(&dev->pdev->dev, "unexpect ep0 packets\n");
-		break;
-	default:
-		ep0_stall(dev);
-		break;
-	}
-
-	dev_vdbg(&dev->pdev->dev, "<--- %s()\n", __func__);
-}
-
-
-/* USB transfer completion interrupt */
-static void handle_trans_complete(struct langwell_udc *dev)
-{
-	u32			complete_bits;
-	int			i, ep_num, dir, bit_mask, status;
-	struct langwell_ep	*epn;
-	struct langwell_request	*curr_req, *temp_req;
-
-	dev_vdbg(&dev->pdev->dev, "---> %s()\n", __func__);
-
-	complete_bits = readl(&dev->op_regs->endptcomplete);
-	dev_vdbg(&dev->pdev->dev, "endptcomplete register: 0x%08x\n",
-			complete_bits);
-
-	/* Write-Clear the bits in endptcomplete register */
-	writel(complete_bits, &dev->op_regs->endptcomplete);
-
-	if (!complete_bits) {
-		dev_dbg(&dev->pdev->dev, "complete_bits = 0\n");
-		goto done;
-	}
-
-	for (i = 0; i < dev->ep_max; i++) {
-		ep_num = i / 2;
-		dir = i % 2;
-
-		bit_mask = 1 << (ep_num + 16 * dir);
-
-		if (!(complete_bits & bit_mask))
-			continue;
-
-		/* ep0 */
-		if (i == 1)
-			epn = &dev->ep[0];
-		else
-			epn = &dev->ep[i];
-
-		if (epn->name == NULL) {
-			dev_warn(&dev->pdev->dev, "invalid endpoint\n");
-			continue;
-		}
-
-		if (i < 2)
-			/* ep0 in and out */
-			dev_dbg(&dev->pdev->dev, "%s-%s transfer completed\n",
-					epn->name,
-					is_in(epn) ? "in" : "out");
-		else
-			dev_dbg(&dev->pdev->dev, "%s transfer completed\n",
-					epn->name);
-
-		/* process the req queue until an uncomplete request */
-		list_for_each_entry_safe(curr_req, temp_req,
-				&epn->queue, queue) {
-			status = process_ep_req(dev, i, curr_req);
-			dev_vdbg(&dev->pdev->dev, "%s req status: %d\n",
-					epn->name, status);
-
-			if (status)
-				break;
-
-			/* write back status to req */
-			curr_req->req.status = status;
-
-			/* ep0 request completion */
-			if (ep_num == 0) {
-				ep0_req_complete(dev, epn, curr_req);
-				break;
-			} else {
-				done(epn, curr_req, status);
-			}
-		}
-	}
-done:
-	dev_vdbg(&dev->pdev->dev, "<--- %s()\n", __func__);
-}
-
-/* port change detect interrupt handler */
-static void handle_port_change(struct langwell_udc *dev)
-{
-	u32	portsc1, devlc;
-
-	dev_vdbg(&dev->pdev->dev, "---> %s()\n", __func__);
-
-	if (dev->bus_reset)
-		dev->bus_reset = 0;
-
-	portsc1 = readl(&dev->op_regs->portsc1);
-	devlc = readl(&dev->op_regs->devlc);
-	dev_vdbg(&dev->pdev->dev, "portsc1 = 0x%08x, devlc = 0x%08x\n",
-			portsc1, devlc);
-
-	/* bus reset is finished */
-	if (!(portsc1 & PORTS_PR)) {
-		/* get the speed */
-		dev->gadget.speed = lpm_device_speed(devlc);
-		dev_vdbg(&dev->pdev->dev, "dev->gadget.speed = %d\n",
-			dev->gadget.speed);
-	}
-
-	/* LPM L0 to L1 */
-	if (dev->lpm && dev->lpm_state == LPM_L0)
-		if (portsc1 & PORTS_SUSP && portsc1 & PORTS_SLP) {
-			dev_info(&dev->pdev->dev, "LPM L0 to L1\n");
-			dev->lpm_state = LPM_L1;
-		}
-
-	/* LPM L1 to L0, force resume or remote wakeup finished */
-	if (dev->lpm && dev->lpm_state == LPM_L1)
-		if (!(portsc1 & PORTS_SUSP)) {
-			dev_info(&dev->pdev->dev, "LPM L1 to L0\n");
-			dev->lpm_state = LPM_L0;
-		}
-
-	/* update USB state */
-	if (!dev->resume_state)
-		dev->usb_state = USB_STATE_DEFAULT;
-
-	dev_vdbg(&dev->pdev->dev, "<--- %s()\n", __func__);
-}
-
-
-/* USB reset interrupt handler */
-static void handle_usb_reset(struct langwell_udc *dev)
-{
-	u32		deviceaddr,
-			endptsetupstat,
-			endptcomplete;
-	unsigned long	timeout;
-
-	dev_vdbg(&dev->pdev->dev, "---> %s()\n", __func__);
-
-	/* Write-Clear the device address */
-	deviceaddr = readl(&dev->op_regs->deviceaddr);
-	writel(deviceaddr & ~USBADR_MASK, &dev->op_regs->deviceaddr);
-
-	dev->dev_addr = 0;
-
-	/* clear usb state */
-	dev->resume_state = 0;
-
-	/* LPM L1 to L0, reset */
-	if (dev->lpm)
-		dev->lpm_state = LPM_L0;
-
-	dev->ep0_dir = USB_DIR_OUT;
-	dev->ep0_state = WAIT_FOR_SETUP;
-
-	/* remote wakeup reset to 0 when the device is reset */
-	dev->remote_wakeup = 0;
-	dev->dev_status = 1 << USB_DEVICE_SELF_POWERED;
-	dev->gadget.b_hnp_enable = 0;
-	dev->gadget.a_hnp_support = 0;
-	dev->gadget.a_alt_hnp_support = 0;
-
-	/* Write-Clear all the setup token semaphores */
-	endptsetupstat = readl(&dev->op_regs->endptsetupstat);
-	writel(endptsetupstat, &dev->op_regs->endptsetupstat);
-
-	/* Write-Clear all the endpoint complete status bits */
-	endptcomplete = readl(&dev->op_regs->endptcomplete);
-	writel(endptcomplete, &dev->op_regs->endptcomplete);
-
-	/* wait until all endptprime bits cleared */
-	timeout = jiffies + PRIME_TIMEOUT;
-	while (readl(&dev->op_regs->endptprime)) {
-		if (time_after(jiffies, timeout)) {
-			dev_err(&dev->pdev->dev, "USB reset timeout\n");
-			break;
-		}
-		cpu_relax();
-	}
-
-	/* write 1s to endptflush register to clear any primed buffers */
-	writel((u32) ~0, &dev->op_regs->endptflush);
-
-	if (readl(&dev->op_regs->portsc1) & PORTS_PR) {
-		dev_vdbg(&dev->pdev->dev, "USB bus reset\n");
-		/* bus is reseting */
-		dev->bus_reset = 1;
-
-		/* reset all the queues, stop all USB activities */
-		stop_activity(dev);
-		dev->usb_state = USB_STATE_DEFAULT;
-	} else {
-		dev_vdbg(&dev->pdev->dev, "device controller reset\n");
-		/* controller reset */
-		langwell_udc_reset(dev);
-
-		/* reset all the queues, stop all USB activities */
-		stop_activity(dev);
-
-		/* reset ep0 dQH and endptctrl */
-		ep0_reset(dev);
-
-		/* enable interrupt and set controller to run state */
-		langwell_udc_start(dev);
-
-		dev->usb_state = USB_STATE_ATTACHED;
-	}
-
-	dev_vdbg(&dev->pdev->dev, "<--- %s()\n", __func__);
-}
-
-
-/* USB bus suspend/resume interrupt */
-static void handle_bus_suspend(struct langwell_udc *dev)
-{
-	dev_dbg(&dev->pdev->dev, "---> %s()\n", __func__);
-
-	dev->resume_state = dev->usb_state;
-	dev->usb_state = USB_STATE_SUSPENDED;
-
-	/* report suspend to the driver */
-	if (dev->driver) {
-		if (dev->driver->suspend) {
-			spin_unlock(&dev->lock);
-			dev->driver->suspend(&dev->gadget);
-			spin_lock(&dev->lock);
-			dev_dbg(&dev->pdev->dev, "suspend %s\n",
-					dev->driver->driver.name);
-		}
-	}
-
-	/* enter PHY low power suspend */
-	if (dev->pdev->device != 0x0829)
-		langwell_phy_low_power(dev, 0);
-
-	dev_dbg(&dev->pdev->dev, "<--- %s()\n", __func__);
-}
-
-
-static void handle_bus_resume(struct langwell_udc *dev)
-{
-	dev_dbg(&dev->pdev->dev, "---> %s()\n", __func__);
-
-	dev->usb_state = dev->resume_state;
-	dev->resume_state = 0;
-
-	/* exit PHY low power suspend */
-	if (dev->pdev->device != 0x0829)
-		langwell_phy_low_power(dev, 0);
-
-	/* report resume to the driver */
-	if (dev->driver) {
-		if (dev->driver->resume) {
-			spin_unlock(&dev->lock);
-			dev->driver->resume(&dev->gadget);
-			spin_lock(&dev->lock);
-			dev_dbg(&dev->pdev->dev, "resume %s\n",
-					dev->driver->driver.name);
-		}
-	}
-
-	dev_dbg(&dev->pdev->dev, "<--- %s()\n", __func__);
-}
-
-
-/* USB device controller interrupt handler */
-static irqreturn_t langwell_irq(int irq, void *_dev)
-{
-	struct langwell_udc	*dev = _dev;
-	u32			usbsts,
-				usbintr,
-				irq_sts,
-				portsc1;
-
-	dev_vdbg(&dev->pdev->dev, "---> %s()\n", __func__);
-
-	if (dev->stopped) {
-		dev_vdbg(&dev->pdev->dev, "handle IRQ_NONE\n");
-		dev_vdbg(&dev->pdev->dev, "<--- %s()\n", __func__);
-		return IRQ_NONE;
-	}
-
-	spin_lock(&dev->lock);
-
-	/* USB status */
-	usbsts = readl(&dev->op_regs->usbsts);
-
-	/* USB interrupt enable */
-	usbintr = readl(&dev->op_regs->usbintr);
-
-	irq_sts = usbsts & usbintr;
-	dev_vdbg(&dev->pdev->dev,
-			"usbsts = 0x%08x, usbintr = 0x%08x, irq_sts = 0x%08x\n",
-			usbsts, usbintr, irq_sts);
-
-	if (!irq_sts) {
-		dev_vdbg(&dev->pdev->dev, "handle IRQ_NONE\n");
-		dev_vdbg(&dev->pdev->dev, "<--- %s()\n", __func__);
-		spin_unlock(&dev->lock);
-		return IRQ_NONE;
-	}
-
-	/* Write-Clear interrupt status bits */
-	writel(irq_sts, &dev->op_regs->usbsts);
-
-	/* resume from suspend */
-	portsc1 = readl(&dev->op_regs->portsc1);
-	if (dev->usb_state == USB_STATE_SUSPENDED)
-		if (!(portsc1 & PORTS_SUSP))
-			handle_bus_resume(dev);
-
-	/* USB interrupt */
-	if (irq_sts & STS_UI) {
-		dev_vdbg(&dev->pdev->dev, "USB interrupt\n");
-
-		/* setup packet received from ep0 */
-		if (readl(&dev->op_regs->endptsetupstat)
-				& EP0SETUPSTAT_MASK) {
-			dev_vdbg(&dev->pdev->dev,
-				"USB SETUP packet received interrupt\n");
-			/* setup tripwire semaphone */
-			setup_tripwire(dev);
-			handle_setup_packet(dev, &dev->local_setup_buff);
-		}
-
-		/* USB transfer completion */
-		if (readl(&dev->op_regs->endptcomplete)) {
-			dev_vdbg(&dev->pdev->dev,
-				"USB transfer completion interrupt\n");
-			handle_trans_complete(dev);
-		}
-	}
-
-	/* SOF received interrupt (for ISO transfer) */
-	if (irq_sts & STS_SRI) {
-		/* FIXME */
-		/* dev_vdbg(&dev->pdev->dev, "SOF received interrupt\n"); */
-	}
-
-	/* port change detect interrupt */
-	if (irq_sts & STS_PCI) {
-		dev_vdbg(&dev->pdev->dev, "port change detect interrupt\n");
-		handle_port_change(dev);
-	}
-
-	/* suspend interrupt */
-	if (irq_sts & STS_SLI) {
-		dev_vdbg(&dev->pdev->dev, "suspend interrupt\n");
-		handle_bus_suspend(dev);
-	}
-
-	/* USB reset interrupt */
-	if (irq_sts & STS_URI) {
-		dev_vdbg(&dev->pdev->dev, "USB reset interrupt\n");
-		handle_usb_reset(dev);
-	}
-
-	/* USB error or system error interrupt */
-	if (irq_sts & (STS_UEI | STS_SEI)) {
-		/* FIXME */
-		dev_warn(&dev->pdev->dev, "error IRQ, irq_sts: %x\n", irq_sts);
-	}
-
-	spin_unlock(&dev->lock);
-
-	dev_vdbg(&dev->pdev->dev, "<--- %s()\n", __func__);
-	return IRQ_HANDLED;
-}
-
-
-/*-------------------------------------------------------------------------*/
-
-/* release device structure */
-static void gadget_release(struct device *_dev)
-{
-	struct langwell_udc	*dev = dev_get_drvdata(_dev);
-
-	dev_dbg(&dev->pdev->dev, "---> %s()\n", __func__);
-
-	complete(dev->done);
-
-	dev_dbg(&dev->pdev->dev, "<--- %s()\n", __func__);
-	kfree(dev);
-}
-
-
-/* enable SRAM caching if SRAM detected */
-static void sram_init(struct langwell_udc *dev)
-{
-	struct pci_dev		*pdev = dev->pdev;
-
-	dev_dbg(&dev->pdev->dev, "---> %s()\n", __func__);
-
-	dev->sram_addr = pci_resource_start(pdev, 1);
-	dev->sram_size = pci_resource_len(pdev, 1);
-	dev_info(&dev->pdev->dev, "Found private SRAM at %x size:%x\n",
-			dev->sram_addr, dev->sram_size);
-	dev->got_sram = 1;
-
-	if (pci_request_region(pdev, 1, kobject_name(&pdev->dev.kobj))) {
-		dev_warn(&dev->pdev->dev, "SRAM request failed\n");
-		dev->got_sram = 0;
-	} else if (!dma_declare_coherent_memory(&pdev->dev, dev->sram_addr,
-			dev->sram_addr, dev->sram_size, DMA_MEMORY_MAP)) {
-		dev_warn(&dev->pdev->dev, "SRAM DMA declare failed\n");
-		pci_release_region(pdev, 1);
-		dev->got_sram = 0;
-	}
-
-	dev_dbg(&dev->pdev->dev, "<--- %s()\n", __func__);
-}
-
-
-/* release SRAM caching */
-static void sram_deinit(struct langwell_udc *dev)
-{
-	struct pci_dev *pdev = dev->pdev;
-
-	dev_dbg(&dev->pdev->dev, "---> %s()\n", __func__);
-
-	dma_release_declared_memory(&pdev->dev);
-	pci_release_region(pdev, 1);
-
-	dev->got_sram = 0;
-
-	dev_info(&dev->pdev->dev, "release SRAM caching\n");
-	dev_dbg(&dev->pdev->dev, "<--- %s()\n", __func__);
-}
-
-
-/* tear down the binding between this driver and the pci device */
-static void langwell_udc_remove(struct pci_dev *pdev)
-{
-	struct langwell_udc	*dev = pci_get_drvdata(pdev);
-
-	DECLARE_COMPLETION(done);
-
-	BUG_ON(dev->driver);
-	dev_dbg(&dev->pdev->dev, "---> %s()\n", __func__);
-
-	dev->done = &done;
-
-	/* free dTD dma_pool and dQH */
-	if (dev->dtd_pool)
-		dma_pool_destroy(dev->dtd_pool);
-
-	if (dev->ep_dqh)
-		dma_free_coherent(&pdev->dev, dev->ep_dqh_size,
-			dev->ep_dqh, dev->ep_dqh_dma);
-
-	/* release SRAM caching */
-	if (dev->has_sram && dev->got_sram)
-		sram_deinit(dev);
-
-	if (dev->status_req) {
-		kfree(dev->status_req->req.buf);
-		kfree(dev->status_req);
-	}
-
-	kfree(dev->ep);
-
-	/* disable IRQ handler */
-	if (dev->got_irq)
-		free_irq(pdev->irq, dev);
-
-	if (dev->cap_regs)
-		iounmap(dev->cap_regs);
-
-	if (dev->region)
-		release_mem_region(pci_resource_start(pdev, 0),
-				pci_resource_len(pdev, 0));
-
-	if (dev->enabled)
-		pci_disable_device(pdev);
-
-	dev->cap_regs = NULL;
-
-	dev_info(&dev->pdev->dev, "unbind\n");
-	dev_dbg(&dev->pdev->dev, "<--- %s()\n", __func__);
-
-	device_unregister(&dev->gadget.dev);
-	device_remove_file(&pdev->dev, &dev_attr_langwell_udc);
-	device_remove_file(&pdev->dev, &dev_attr_remote_wakeup);
-
-	pci_set_drvdata(pdev, NULL);
-
-	/* free dev, wait for the release() finished */
-	wait_for_completion(&done);
-}
-
-
-/*
- * wrap this driver around the specified device, but
- * don't respond over USB until a gadget driver binds to us.
- */
-static int langwell_udc_probe(struct pci_dev *pdev,
-		const struct pci_device_id *id)
-{
-	struct langwell_udc	*dev;
-	unsigned long		resource, len;
-	void			__iomem *base = NULL;
-	size_t			size;
-	int			retval;
-
-	/* alloc, and start init */
-	dev = kzalloc(sizeof *dev, GFP_KERNEL);
-	if (dev == NULL) {
-		retval = -ENOMEM;
-		goto error;
-	}
-
-	/* initialize device spinlock */
-	spin_lock_init(&dev->lock);
-
-	dev->pdev = pdev;
-	dev_dbg(&dev->pdev->dev, "---> %s()\n", __func__);
-
-	pci_set_drvdata(pdev, dev);
-
-	/* now all the pci goodies ... */
-	if (pci_enable_device(pdev) < 0) {
-		retval = -ENODEV;
-		goto error;
-	}
-	dev->enabled = 1;
-
-	/* control register: BAR 0 */
-	resource = pci_resource_start(pdev, 0);
-	len = pci_resource_len(pdev, 0);
-	if (!request_mem_region(resource, len, driver_name)) {
-		dev_err(&dev->pdev->dev, "controller already in use\n");
-		retval = -EBUSY;
-		goto error;
-	}
-	dev->region = 1;
-
-	base = ioremap_nocache(resource, len);
-	if (base == NULL) {
-		dev_err(&dev->pdev->dev, "can't map memory\n");
-		retval = -EFAULT;
-		goto error;
-	}
-
-	dev->cap_regs = (struct langwell_cap_regs __iomem *) base;
-	dev_vdbg(&dev->pdev->dev, "dev->cap_regs: %p\n", dev->cap_regs);
-	dev->op_regs = (struct langwell_op_regs __iomem *)
-		(base + OP_REG_OFFSET);
-	dev_vdbg(&dev->pdev->dev, "dev->op_regs: %p\n", dev->op_regs);
-
-	/* irq setup after old hardware is cleaned up */
-	if (!pdev->irq) {
-		dev_err(&dev->pdev->dev, "No IRQ. Check PCI setup!\n");
-		retval = -ENODEV;
-		goto error;
-	}
-
-	dev->has_sram = 1;
-	dev->got_sram = 0;
-	dev_vdbg(&dev->pdev->dev, "dev->has_sram: %d\n", dev->has_sram);
-
-	/* enable SRAM caching if detected */
-	if (dev->has_sram && !dev->got_sram)
-		sram_init(dev);
-
-	dev_info(&dev->pdev->dev,
-			"irq %d, io mem: 0x%08lx, len: 0x%08lx, pci mem 0x%p\n",
-			pdev->irq, resource, len, base);
-	/* enables bus-mastering for device dev */
-	pci_set_master(pdev);
-
-	if (request_irq(pdev->irq, langwell_irq, IRQF_SHARED,
-				driver_name, dev) != 0) {
-		dev_err(&dev->pdev->dev,
-				"request interrupt %d failed\n", pdev->irq);
-		retval = -EBUSY;
-		goto error;
-	}
-	dev->got_irq = 1;
-
-	/* set stopped bit */
-	dev->stopped = 1;
-
-	/* capabilities and endpoint number */
-	dev->lpm = (readl(&dev->cap_regs->hccparams) & HCC_LEN) ? 1 : 0;
-	dev->dciversion = readw(&dev->cap_regs->dciversion);
-	dev->devcap = (readl(&dev->cap_regs->dccparams) & DEVCAP) ? 1 : 0;
-	dev_vdbg(&dev->pdev->dev, "dev->lpm: %d\n", dev->lpm);
-	dev_vdbg(&dev->pdev->dev, "dev->dciversion: 0x%04x\n",
-			dev->dciversion);
-	dev_vdbg(&dev->pdev->dev, "dccparams: 0x%08x\n",
-			readl(&dev->cap_regs->dccparams));
-	dev_vdbg(&dev->pdev->dev, "dev->devcap: %d\n", dev->devcap);
-	if (!dev->devcap) {
-		dev_err(&dev->pdev->dev, "can't support device mode\n");
-		retval = -ENODEV;
-		goto error;
-	}
-
-	/* a pair of endpoints (out/in) for each address */
-	dev->ep_max = DEN(readl(&dev->cap_regs->dccparams)) * 2;
-	dev_vdbg(&dev->pdev->dev, "dev->ep_max: %d\n", dev->ep_max);
-
-	/* allocate endpoints memory */
-	dev->ep = kzalloc(sizeof(struct langwell_ep) * dev->ep_max,
-			GFP_KERNEL);
-	if (!dev->ep) {
-		dev_err(&dev->pdev->dev, "allocate endpoints memory failed\n");
-		retval = -ENOMEM;
-		goto error;
-	}
-
-	/* allocate device dQH memory */
-	size = dev->ep_max * sizeof(struct langwell_dqh);
-	dev_vdbg(&dev->pdev->dev, "orig size = %zd\n", size);
-	if (size < DQH_ALIGNMENT)
-		size = DQH_ALIGNMENT;
-	else if ((size % DQH_ALIGNMENT) != 0) {
-		size += DQH_ALIGNMENT + 1;
-		size &= ~(DQH_ALIGNMENT - 1);
-	}
-	dev->ep_dqh = dma_alloc_coherent(&pdev->dev, size,
-					&dev->ep_dqh_dma, GFP_KERNEL);
-	if (!dev->ep_dqh) {
-		dev_err(&dev->pdev->dev, "allocate dQH memory failed\n");
-		retval = -ENOMEM;
-		goto error;
-	}
-	dev->ep_dqh_size = size;
-	dev_vdbg(&dev->pdev->dev, "ep_dqh_size = %zd\n", dev->ep_dqh_size);
-
-	/* initialize ep0 status request structure */
-	dev->status_req = kzalloc(sizeof(struct langwell_request), GFP_KERNEL);
-	if (!dev->status_req) {
-		dev_err(&dev->pdev->dev,
-				"allocate status_req memory failed\n");
-		retval = -ENOMEM;
-		goto error;
-	}
-	INIT_LIST_HEAD(&dev->status_req->queue);
-
-	/* allocate a small amount of memory to get valid address */
-	dev->status_req->req.buf = kmalloc(8, GFP_KERNEL);
-	dev->status_req->req.dma = virt_to_phys(dev->status_req->req.buf);
-
-	dev->resume_state = USB_STATE_NOTATTACHED;
-	dev->usb_state = USB_STATE_POWERED;
-	dev->ep0_dir = USB_DIR_OUT;
-
-	/* remote wakeup reset to 0 when the device is reset */
-	dev->remote_wakeup = 0;
-	dev->dev_status = 1 << USB_DEVICE_SELF_POWERED;
-
-	/* reset device controller */
-	langwell_udc_reset(dev);
-
-	/* initialize gadget structure */
-	dev->gadget.ops = &langwell_ops;	/* usb_gadget_ops */
-	dev->gadget.ep0 = &dev->ep[0].ep;	/* gadget ep0 */
-	INIT_LIST_HEAD(&dev->gadget.ep_list);	/* ep_list */
-	dev->gadget.speed = USB_SPEED_UNKNOWN;	/* speed */
-	dev->gadget.max_speed = USB_SPEED_HIGH;	/* support dual speed */
-
-	/* the "gadget" abstracts/virtualizes the controller */
-	dev_set_name(&dev->gadget.dev, "gadget");
-	dev->gadget.dev.parent = &pdev->dev;
-	dev->gadget.dev.dma_mask = pdev->dev.dma_mask;
-	dev->gadget.dev.release = gadget_release;
-	dev->gadget.name = driver_name;		/* gadget name */
-
-	/* controller endpoints reinit */
-	eps_reinit(dev);
-
-	/* reset ep0 dQH and endptctrl */
-	ep0_reset(dev);
-
-	/* create dTD dma_pool resource */
-	dev->dtd_pool = dma_pool_create("langwell_dtd",
-			&dev->pdev->dev,
-			sizeof(struct langwell_dtd),
-			DTD_ALIGNMENT,
-			DMA_BOUNDARY);
-
-	if (!dev->dtd_pool) {
-		retval = -ENOMEM;
-		goto error;
-	}
-
-	/* done */
-	dev_info(&dev->pdev->dev, "%s\n", driver_desc);
-	dev_info(&dev->pdev->dev, "irq %d, pci mem %p\n", pdev->irq, base);
-	dev_info(&dev->pdev->dev, "Driver version: " DRIVER_VERSION "\n");
-	dev_info(&dev->pdev->dev, "Support (max) %d endpoints\n", dev->ep_max);
-	dev_info(&dev->pdev->dev, "Device interface version: 0x%04x\n",
-			dev->dciversion);
-	dev_info(&dev->pdev->dev, "Controller mode: %s\n",
-			dev->devcap ? "Device" : "Host");
-	dev_info(&dev->pdev->dev, "Support USB LPM: %s\n",
-			dev->lpm ? "Yes" : "No");
-
-	dev_vdbg(&dev->pdev->dev,
-			"After langwell_udc_probe(), print all registers:\n");
-	print_all_registers(dev);
-
-	retval = device_register(&dev->gadget.dev);
-	if (retval)
-		goto error;
-
-	retval = usb_add_gadget_udc(&pdev->dev, &dev->gadget);
-	if (retval)
-		goto error;
-
-	retval = device_create_file(&pdev->dev, &dev_attr_langwell_udc);
-	if (retval)
-		goto error;
-
-	retval = device_create_file(&pdev->dev, &dev_attr_remote_wakeup);
-	if (retval)
-		goto error_attr1;
-
-	dev_vdbg(&dev->pdev->dev, "<--- %s()\n", __func__);
-	return 0;
-
-error_attr1:
-	device_remove_file(&pdev->dev, &dev_attr_langwell_udc);
-error:
-	if (dev) {
-		dev_dbg(&dev->pdev->dev, "<--- %s()\n", __func__);
-		langwell_udc_remove(pdev);
-	}
-
-	return retval;
-}
-
-
-/* device controller suspend */
-static int langwell_udc_suspend(struct pci_dev *pdev, pm_message_t state)
-{
-	struct langwell_udc	*dev = pci_get_drvdata(pdev);
-
-	dev_dbg(&dev->pdev->dev, "---> %s()\n", __func__);
-
-	usb_del_gadget_udc(&dev->gadget);
-	/* disable interrupt and set controller to stop state */
-	langwell_udc_stop(dev);
-
-	/* disable IRQ handler */
-	if (dev->got_irq)
-		free_irq(pdev->irq, dev);
-	dev->got_irq = 0;
-
-	/* save PCI state */
-	pci_save_state(pdev);
-
-	spin_lock_irq(&dev->lock);
-	/* stop all usb activities */
-	stop_activity(dev);
-	spin_unlock_irq(&dev->lock);
-
-	/* free dTD dma_pool and dQH */
-	if (dev->dtd_pool)
-		dma_pool_destroy(dev->dtd_pool);
-
-	if (dev->ep_dqh)
-		dma_free_coherent(&pdev->dev, dev->ep_dqh_size,
-			dev->ep_dqh, dev->ep_dqh_dma);
-
-	/* release SRAM caching */
-	if (dev->has_sram && dev->got_sram)
-		sram_deinit(dev);
-
-	/* set device power state */
-	pci_set_power_state(pdev, PCI_D3hot);
-
-	/* enter PHY low power suspend */
-	if (dev->pdev->device != 0x0829)
-		langwell_phy_low_power(dev, 1);
-
-	dev_dbg(&dev->pdev->dev, "<--- %s()\n", __func__);
-	return 0;
-}
-
-
-/* device controller resume */
-static int langwell_udc_resume(struct pci_dev *pdev)
-{
-	struct langwell_udc	*dev = pci_get_drvdata(pdev);
-	size_t			size;
-
-	dev_dbg(&dev->pdev->dev, "---> %s()\n", __func__);
-
-	/* exit PHY low power suspend */
-	if (dev->pdev->device != 0x0829)
-		langwell_phy_low_power(dev, 0);
-
-	/* set device D0 power state */
-	pci_set_power_state(pdev, PCI_D0);
-
-	/* enable SRAM caching if detected */
-	if (dev->has_sram && !dev->got_sram)
-		sram_init(dev);
-
-	/* allocate device dQH memory */
-	size = dev->ep_max * sizeof(struct langwell_dqh);
-	dev_vdbg(&dev->pdev->dev, "orig size = %zd\n", size);
-	if (size < DQH_ALIGNMENT)
-		size = DQH_ALIGNMENT;
-	else if ((size % DQH_ALIGNMENT) != 0) {
-		size += DQH_ALIGNMENT + 1;
-		size &= ~(DQH_ALIGNMENT - 1);
-	}
-	dev->ep_dqh = dma_alloc_coherent(&pdev->dev, size,
-					&dev->ep_dqh_dma, GFP_KERNEL);
-	if (!dev->ep_dqh) {
-		dev_err(&dev->pdev->dev, "allocate dQH memory failed\n");
-		return -ENOMEM;
-	}
-	dev->ep_dqh_size = size;
-	dev_vdbg(&dev->pdev->dev, "ep_dqh_size = %zd\n", dev->ep_dqh_size);
-
-	/* create dTD dma_pool resource */
-	dev->dtd_pool = dma_pool_create("langwell_dtd",
-			&dev->pdev->dev,
-			sizeof(struct langwell_dtd),
-			DTD_ALIGNMENT,
-			DMA_BOUNDARY);
-
-	if (!dev->dtd_pool)
-		return -ENOMEM;
-
-	/* restore PCI state */
-	pci_restore_state(pdev);
-
-	/* enable IRQ handler */
-	if (request_irq(pdev->irq, langwell_irq, IRQF_SHARED,
-				driver_name, dev) != 0) {
-		dev_err(&dev->pdev->dev, "request interrupt %d failed\n",
-				pdev->irq);
-		return -EBUSY;
-	}
-	dev->got_irq = 1;
-
-	/* reset and start controller to run state */
-	if (dev->stopped) {
-		/* reset device controller */
-		langwell_udc_reset(dev);
-
-		/* reset ep0 dQH and endptctrl */
-		ep0_reset(dev);
-
-		/* start device if gadget is loaded */
-		if (dev->driver)
-			langwell_udc_start(dev);
-	}
-
-	/* reset USB status */
-	dev->usb_state = USB_STATE_ATTACHED;
-	dev->ep0_state = WAIT_FOR_SETUP;
-	dev->ep0_dir = USB_DIR_OUT;
-
-	dev_dbg(&dev->pdev->dev, "<--- %s()\n", __func__);
-	return 0;
-}
-
-
-/* pci driver shutdown */
-static void langwell_udc_shutdown(struct pci_dev *pdev)
-{
-	struct langwell_udc	*dev = pci_get_drvdata(pdev);
-	u32			usbmode;
-
-	dev_dbg(&dev->pdev->dev, "---> %s()\n", __func__);
-
-	/* reset controller mode to IDLE */
-	usbmode = readl(&dev->op_regs->usbmode);
-	dev_dbg(&dev->pdev->dev, "usbmode = 0x%08x\n", usbmode);
-	usbmode &= (~3 | MODE_IDLE);
-	writel(usbmode, &dev->op_regs->usbmode);
-
-	dev_dbg(&dev->pdev->dev, "<--- %s()\n", __func__);
-}
-
-/*-------------------------------------------------------------------------*/
-
-static const struct pci_device_id pci_ids[] = { {
-	.class =	((PCI_CLASS_SERIAL_USB << 8) | 0xfe),
-	.class_mask =	~0,
-	.vendor =	0x8086,
-	.device =	0x0811,
-	.subvendor =	PCI_ANY_ID,
-	.subdevice =	PCI_ANY_ID,
-}, { /* end: all zeroes */ }
-};
-
-MODULE_DEVICE_TABLE(pci, pci_ids);
-
-
-static struct pci_driver langwell_pci_driver = {
-	.name =		(char *) driver_name,
-	.id_table =	pci_ids,
-
-	.probe =	langwell_udc_probe,
-	.remove =	langwell_udc_remove,
-
-	/* device controller suspend/resume */
-	.suspend =	langwell_udc_suspend,
-	.resume =	langwell_udc_resume,
-
-	.shutdown =	langwell_udc_shutdown,
-};
-
-
-static int __init init(void)
-{
-	return pci_register_driver(&langwell_pci_driver);
-}
-module_init(init);
-
-
-static void __exit cleanup(void)
-{
-	pci_unregister_driver(&langwell_pci_driver);
-}
-module_exit(cleanup);
-
-
-MODULE_DESCRIPTION(DRIVER_DESC);
-MODULE_AUTHOR("Xiaochen Shen <xiaochen.shen@intel.com>");
-MODULE_VERSION(DRIVER_VERSION);
-MODULE_LICENSE("GPL");
-
diff --git a/drivers/usb/gadget/langwell_udc.h b/drivers/usb/gadget/langwell_udc.h
deleted file mode 100644
index 8c8087abb48..00000000000
--- a/drivers/usb/gadget/langwell_udc.h
+++ /dev/null
@@ -1,224 +0,0 @@
-/*
- * Intel Langwell USB Device Controller driver
- * Copyright (C) 2008-2009, 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.
- */
-
-#include <linux/usb/langwell_udc.h>
-
-/*-------------------------------------------------------------------------*/
-
-/* driver data structures and utilities */
-
-/*
- * dTD: Device Endpoint Transfer Descriptor
- * describe to the device controller the location and quantity of
- * data to be send/received for given transfer
- */
-struct langwell_dtd {
-	u32	dtd_next;
-/* bits 31:5, next transfer element pointer */
-#define	DTD_NEXT(d)	(((d)>>5)&0x7ffffff)
-#define	DTD_NEXT_MASK	(0x7ffffff << 5)
-/* terminate */
-#define	DTD_TERM	BIT(0)
-	/* bits 7:0, execution back states */
-	u32	dtd_status:8;
-#define	DTD_STATUS(d)	(((d)>>0)&0xff)
-#define	DTD_STS_ACTIVE	BIT(7)	/* active */
-#define	DTD_STS_HALTED	BIT(6)	/* halted */
-#define	DTD_STS_DBE	BIT(5)	/* data buffer error */
-#define	DTD_STS_TRE	BIT(3)	/* transaction error  */
-	/* bits 9:8 */
-	u32	dtd_res0:2;
-	/* bits 11:10, multipier override */
-	u32	dtd_multo:2;
-#define	DTD_MULTO	(BIT(11) | BIT(10))
-	/* bits 14:12 */
-	u32	dtd_res1:3;
-	/* bit 15, interrupt on complete */
-	u32	dtd_ioc:1;
-#define	DTD_IOC		BIT(15)
-	/* bits 30:16, total bytes */
-	u32	dtd_total:15;
-#define	DTD_TOTAL(d)	(((d)>>16)&0x7fff)
-#define	DTD_MAX_TRANSFER_LENGTH	0x4000
-	/* bit 31 */
-	u32	dtd_res2:1;
-	/* dTD buffer pointer page 0 to 4 */
-	u32	dtd_buf[5];
-#define	DTD_OFFSET_MASK	0xfff
-/* bits 31:12, buffer pointer */
-#define	DTD_BUFFER(d)	(((d)>>12)&0x3ff)
-/* bits 11:0, current offset */
-#define	DTD_C_OFFSET(d)	(((d)>>0)&0xfff)
-/* bits 10:0, frame number */
-#define	DTD_FRAME(d)	(((d)>>0)&0x7ff)
-
-	/* driver-private parts */
-
-	/* dtd dma address */
-	dma_addr_t		dtd_dma;
-	/* next dtd virtual address */
-	struct langwell_dtd	*next_dtd_virt;
-};
-
-
-/*
- * dQH: Device Endpoint Queue Head
- * describe where all transfers are managed
- * 48-byte data structure, aligned on 64-byte boundary
- *
- * These are associated with dTD structure
- */
-struct langwell_dqh {
-	/* endpoint capabilities and characteristics */
-	u32	dqh_res0:15;	/* bits 14:0 */
-	u32	dqh_ios:1;	/* bit 15, interrupt on setup */
-#define	DQH_IOS		BIT(15)
-	u32	dqh_mpl:11;	/* bits 26:16, maximum packet length */
-#define	DQH_MPL		(0x7ff << 16)
-	u32	dqh_res1:2;	/* bits 28:27 */
-	u32	dqh_zlt:1;	/* bit 29, zero length termination */
-#define	DQH_ZLT		BIT(29)
-	u32	dqh_mult:2;	/* bits 31:30 */
-#define	DQH_MULT	(BIT(30) | BIT(31))
-
-	/* current dTD pointer */
-	u32	dqh_current;	/* locate the transfer in progress */
-#define DQH_C_DTD(e)	\
-	(((e)>>5)&0x7ffffff)	/* bits 31:5, current dTD pointer */
-
-	/* transfer overlay, hardware parts of a struct langwell_dtd */
-	u32	dtd_next;
-	u32	dtd_status:8;	/* bits 7:0, execution back states */
-	u32	dtd_res0:2;	/* bits 9:8 */
-	u32	dtd_multo:2;	/* bits 11:10, multipier override */
-	u32	dtd_res1:3;	/* bits 14:12 */
-	u32	dtd_ioc:1;	/* bit 15, interrupt on complete */
-	u32	dtd_total:15;	/* bits 30:16, total bytes */
-	u32	dtd_res2:1;	/* bit 31 */
-	u32	dtd_buf[5];	/* dTD buffer pointer page 0 to 4 */
-
-	u32	dqh_res2;
-	struct usb_ctrlrequest	dqh_setup;	/* setup packet buffer */
-} __attribute__ ((aligned(64)));
-
-
-/* endpoint data structure */
-struct langwell_ep {
-	struct usb_ep		ep;
-	dma_addr_t		dma;
-	struct langwell_udc	*dev;
-	unsigned long		irqs;
-	struct list_head	queue;
-	struct langwell_dqh	*dqh;
-	const struct usb_endpoint_descriptor	*desc;
-	char			name[14];
-	unsigned		stopped:1,
-				ep_type:2,
-				ep_num:8;
-};
-
-
-/* request data structure */
-struct langwell_request {
-	struct usb_request	req;
-	struct langwell_dtd	*dtd, *head, *tail;
-	struct langwell_ep	*ep;
-	dma_addr_t		dtd_dma;
-	struct list_head	queue;
-	unsigned		dtd_count;
-	unsigned		mapped:1;
-};
-
-
-/* ep0 transfer state */
-enum ep0_state {
-	WAIT_FOR_SETUP,
-	DATA_STATE_XMIT,
-	DATA_STATE_NEED_ZLP,
-	WAIT_FOR_OUT_STATUS,
-	DATA_STATE_RECV,
-};
-
-
-/* device suspend state */
-enum lpm_state {
-	LPM_L0,	/* on */
-	LPM_L1,	/* LPM L1 sleep */
-	LPM_L2,	/* suspend */
-	LPM_L3,	/* off */
-};
-
-
-/* device data structure */
-struct langwell_udc {
-	/* each pci device provides one gadget, several endpoints */
-	struct usb_gadget	gadget;
-	spinlock_t		lock;	/* device lock */
-	struct langwell_ep	*ep;
-	struct usb_gadget_driver	*driver;
-	struct usb_phy		*transceiver;
-	u8			dev_addr;
-	u32			usb_state;
-	u32			resume_state;
-	u32			bus_reset;
-	enum lpm_state		lpm_state;
-	enum ep0_state		ep0_state;
-	u32			ep0_dir;
-	u16			dciversion;
-	unsigned		ep_max;
-	unsigned		devcap:1,
-				enabled:1,
-				region:1,
-				got_irq:1,
-				powered:1,
-				remote_wakeup:1,
-				rate:1,
-				is_reset:1,
-				softconnected:1,
-				vbus_active:1,
-				suspended:1,
-				stopped:1,
-				lpm:1,		/* LPM capability */
-				has_sram:1,	/* SRAM caching */
-				got_sram:1;
-
-	/* pci state used to access those endpoints */
-	struct pci_dev		*pdev;
-
-	/* Langwell otg transceiver */
-	struct langwell_otg	*lotg;
-
-	/* control registers */
-	struct langwell_cap_regs	__iomem	*cap_regs;
-	struct langwell_op_regs		__iomem	*op_regs;
-
-	struct usb_ctrlrequest	local_setup_buff;
-	struct langwell_dqh	*ep_dqh;
-	size_t			ep_dqh_size;
-	dma_addr_t		ep_dqh_dma;
-
-	/* ep0 status request */
-	struct langwell_request	*status_req;
-
-	/* dma pool */
-	struct dma_pool		*dtd_pool;
-
-	/* make sure release() is done */
-	struct completion	*done;
-
-	/* for private SRAM caching */
-	unsigned int		sram_addr;
-	unsigned int		sram_size;
-
-	/* device status data for get_status request */
-	u16			dev_status;
-};
-
-#define gadget_to_langwell(g)	container_of((g), struct langwell_udc, gadget)
-
diff --git a/drivers/usb/gadget/lpc32xx_udc.c b/drivers/usb/gadget/lpc32xx_udc.c
new file mode 100644
index 00000000000..262acfd53e3
--- /dev/null
+++ b/drivers/usb/gadget/lpc32xx_udc.c
@@ -0,0 +1,3538 @@
+/*
+ * USB Gadget driver for LPC32xx
+ *
+ * Authors:
+ *    Kevin Wells <kevin.wells@nxp.com>
+ *    Mike James
+ *    Roland Stigge <stigge@antcom.de>
+ *
+ * Copyright (C) 2006 Philips Semiconductors
+ * Copyright (C) 2009 NXP Semiconductors
+ * Copyright (C) 2012 Roland Stigge
+ *
+ * Note: This driver is based on original work done by Mike James for
+ *       the LPC3180.
+ *
+ * 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/platform_device.h>
+#include <linux/delay.h>
+#include <linux/ioport.h>
+#include <linux/slab.h>
+#include <linux/errno.h>
+#include <linux/init.h>
+#include <linux/list.h>
+#include <linux/interrupt.h>
+#include <linux/proc_fs.h>
+#include <linux/clk.h>
+#include <linux/usb/ch9.h>
+#include <linux/usb/gadget.h>
+#include <linux/i2c.h>
+#include <linux/kthread.h>
+#include <linux/freezer.h>
+#include <linux/dma-mapping.h>
+#include <linux/dmapool.h>
+#include <linux/workqueue.h>
+#include <linux/of.h>
+#include <linux/usb/isp1301.h>
+
+#include <asm/byteorder.h>
+#include <mach/hardware.h>
+#include <linux/io.h>
+#include <asm/irq.h>
+#include <asm/system.h>
+
+#include <mach/platform.h>
+#include <mach/irqs.h>
+#include <mach/board.h>
+#ifdef CONFIG_USB_GADGET_DEBUG_FILES
+#include <linux/seq_file.h>
+#endif
+
+/*
+ * USB device configuration structure
+ */
+typedef void (*usc_chg_event)(int);
+struct lpc32xx_usbd_cfg {
+	int vbus_drv_pol;   /* 0=active low drive for VBUS via ISP1301 */
+	usc_chg_event conn_chgb; /* Connection change event (optional) */
+	usc_chg_event susp_chgb; /* Suspend/resume event (optional) */
+	usc_chg_event rmwk_chgb; /* Enable/disable remote wakeup */
+};
+
+/*
+ * controller driver data structures
+ */
+
+/* 16 endpoints (not to be confused with 32 hardware endpoints) */
+#define	NUM_ENDPOINTS	16
+
+/*
+ * IRQ indices make reading the code a little easier
+ */
+#define IRQ_USB_LP	0
+#define IRQ_USB_HP	1
+#define IRQ_USB_DEVDMA	2
+#define IRQ_USB_ATX	3
+
+#define EP_OUT 0 /* RX (from host) */
+#define EP_IN 1 /* TX (to host) */
+
+/* Returns the interrupt mask for the selected hardware endpoint */
+#define EP_MASK_SEL(ep, dir) (1 << (((ep) * 2) + dir))
+
+#define EP_INT_TYPE 0
+#define EP_ISO_TYPE 1
+#define EP_BLK_TYPE 2
+#define EP_CTL_TYPE 3
+
+/* EP0 states */
+#define WAIT_FOR_SETUP 0 /* Wait for setup packet */
+#define DATA_IN        1 /* Expect dev->host transfer */
+#define DATA_OUT       2 /* Expect host->dev transfer */
+
+/* DD (DMA Descriptor) structure, requires word alignment, this is already
+ * defined in the LPC32XX USB device header file, but this version is slightly
+ * modified to tag some work data with each DMA descriptor. */
+struct lpc32xx_usbd_dd_gad {
+	u32 dd_next_phy;
+	u32 dd_setup;
+	u32 dd_buffer_addr;
+	u32 dd_status;
+	u32 dd_iso_ps_mem_addr;
+	u32 this_dma;
+	u32 iso_status[6]; /* 5 spare */
+	u32 dd_next_v;
+};
+
+/*
+ * Logical endpoint structure
+ */
+struct lpc32xx_ep {
+	struct usb_ep		ep;
+	struct list_head	queue;
+	struct lpc32xx_udc	*udc;
+
+	u32			hwep_num_base; /* Physical hardware EP */
+	u32			hwep_num; /* Maps to hardware endpoint */
+	u32			maxpacket;
+	u32			lep;
+
+	bool			is_in;
+	bool			req_pending;
+	u32			eptype;
+
+	u32                     totalints;
+
+	bool			wedge;
+
+	const struct usb_endpoint_descriptor *desc;
+};
+
+/*
+ * Common UDC structure
+ */
+struct lpc32xx_udc {
+	struct usb_gadget	gadget;
+	struct usb_gadget_driver *driver;
+	struct platform_device	*pdev;
+	struct device		*dev;
+	struct dentry		*pde;
+	spinlock_t		lock;
+	struct i2c_client	*isp1301_i2c_client;
+
+	/* Board and device specific */
+	struct lpc32xx_usbd_cfg	*board;
+	u32			io_p_start;
+	u32			io_p_size;
+	void __iomem		*udp_baseaddr;
+	int			udp_irq[4];
+	struct clk		*usb_pll_clk;
+	struct clk		*usb_slv_clk;
+
+	/* DMA support */
+	u32			*udca_v_base;
+	u32			udca_p_base;
+	struct dma_pool		*dd_cache;
+
+	/* Common EP and control data */
+	u32			enabled_devints;
+	u32			enabled_hwepints;
+	u32			dev_status;
+	u32			realized_eps;
+
+	/* VBUS detection, pullup, and power flags */
+	u8			vbus;
+	u8			last_vbus;
+	int			pullup;
+	int			poweron;
+
+	/* Work queues related to I2C support */
+	struct work_struct	pullup_job;
+	struct work_struct	vbus_job;
+	struct work_struct	power_job;
+
+	/* USB device peripheral - various */
+	struct lpc32xx_ep	ep[NUM_ENDPOINTS];
+	bool			enabled;
+	bool			clocked;
+	bool			suspended;
+	bool			selfpowered;
+	int                     ep0state;
+	atomic_t                enabled_ep_cnt;
+	wait_queue_head_t       ep_disable_wait_queue;
+};
+
+/*
+ * Endpoint request
+ */
+struct lpc32xx_request {
+	struct usb_request	req;
+	struct list_head	queue;
+	struct lpc32xx_usbd_dd_gad *dd_desc_ptr;
+	bool			mapped;
+	bool			send_zlp;
+};
+
+static inline struct lpc32xx_udc *to_udc(struct usb_gadget *g)
+{
+	return container_of(g, struct lpc32xx_udc, gadget);
+}
+
+#define ep_dbg(epp, fmt, arg...) \
+	dev_dbg(epp->udc->dev, "%s: " fmt, __func__, ## arg)
+#define ep_err(epp, fmt, arg...) \
+	dev_err(epp->udc->dev, "%s: " fmt, __func__, ## arg)
+#define ep_info(epp, fmt, arg...) \
+	dev_info(epp->udc->dev, "%s: " fmt, __func__, ## arg)
+#define ep_warn(epp, fmt, arg...) \
+	dev_warn(epp->udc->dev, "%s:" fmt, __func__, ## arg)
+
+#define UDCA_BUFF_SIZE (128)
+
+/* TODO: When the clock framework is introduced in LPC32xx, IO_ADDRESS will
+ * be replaced with an inremap()ed pointer, see USB_OTG_CLK_CTRL()
+ * */
+#define USB_CTRL		IO_ADDRESS(LPC32XX_CLK_PM_BASE + 0x64)
+#define USB_CLOCK_MASK		(AHB_M_CLOCK_ON | OTG_CLOCK_ON | \
+				 DEV_CLOCK_ON | I2C_CLOCK_ON)
+
+/* USB_CTRL bit defines */
+#define USB_SLAVE_HCLK_EN	(1 << 24)
+#define USB_HOST_NEED_CLK_EN	(1 << 21)
+#define USB_DEV_NEED_CLK_EN	(1 << 22)
+
+#define USB_OTG_CLK_CTRL(udc)	((udc)->udp_baseaddr + 0xFF4)
+#define USB_OTG_CLK_STAT(udc)	((udc)->udp_baseaddr + 0xFF8)
+
+/* USB_OTG_CLK_CTRL bit defines */
+#define AHB_M_CLOCK_ON		(1 << 4)
+#define OTG_CLOCK_ON		(1 << 3)
+#define I2C_CLOCK_ON		(1 << 2)
+#define DEV_CLOCK_ON		(1 << 1)
+#define HOST_CLOCK_ON		(1 << 0)
+
+#define USB_OTG_STAT_CONTROL(udc) (udc->udp_baseaddr + 0x110)
+
+/* USB_OTG_STAT_CONTROL bit defines */
+#define TRANSPARENT_I2C_EN	(1 << 7)
+#define HOST_EN			(1 << 0)
+
+/**********************************************************************
+ * USB device controller register offsets
+ **********************************************************************/
+
+#define USBD_DEVINTST(x)	((x) + 0x200)
+#define USBD_DEVINTEN(x)	((x) + 0x204)
+#define USBD_DEVINTCLR(x)	((x) + 0x208)
+#define USBD_DEVINTSET(x)	((x) + 0x20C)
+#define USBD_CMDCODE(x)		((x) + 0x210)
+#define USBD_CMDDATA(x)		((x) + 0x214)
+#define USBD_RXDATA(x)		((x) + 0x218)
+#define USBD_TXDATA(x)		((x) + 0x21C)
+#define USBD_RXPLEN(x)		((x) + 0x220)
+#define USBD_TXPLEN(x)		((x) + 0x224)
+#define USBD_CTRL(x)		((x) + 0x228)
+#define USBD_DEVINTPRI(x)	((x) + 0x22C)
+#define USBD_EPINTST(x)		((x) + 0x230)
+#define USBD_EPINTEN(x)		((x) + 0x234)
+#define USBD_EPINTCLR(x)	((x) + 0x238)
+#define USBD_EPINTSET(x)	((x) + 0x23C)
+#define USBD_EPINTPRI(x)	((x) + 0x240)
+#define USBD_REEP(x)		((x) + 0x244)
+#define USBD_EPIND(x)		((x) + 0x248)
+#define USBD_EPMAXPSIZE(x)	((x) + 0x24C)
+/* DMA support registers only below */
+/* Set, clear, or get enabled state of the DMA request status. If
+ * enabled, an IN or OUT token will start a DMA transfer for the EP */
+#define USBD_DMARST(x)		((x) + 0x250)
+#define USBD_DMARCLR(x)		((x) + 0x254)
+#define USBD_DMARSET(x)		((x) + 0x258)
+/* DMA UDCA head pointer */
+#define USBD_UDCAH(x)		((x) + 0x280)
+/* EP DMA status, enable, and disable. This is used to specifically
+ * enabled or disable DMA for a specific EP */
+#define USBD_EPDMAST(x)		((x) + 0x284)
+#define USBD_EPDMAEN(x)		((x) + 0x288)
+#define USBD_EPDMADIS(x)	((x) + 0x28C)
+/* DMA master interrupts enable and pending interrupts */
+#define USBD_DMAINTST(x)	((x) + 0x290)
+#define USBD_DMAINTEN(x)	((x) + 0x294)
+/* DMA end of transfer interrupt enable, disable, status */
+#define USBD_EOTINTST(x)	((x) + 0x2A0)
+#define USBD_EOTINTCLR(x)	((x) + 0x2A4)
+#define USBD_EOTINTSET(x)	((x) + 0x2A8)
+/* New DD request interrupt enable, disable, status */
+#define USBD_NDDRTINTST(x)	((x) + 0x2AC)
+#define USBD_NDDRTINTCLR(x)	((x) + 0x2B0)
+#define USBD_NDDRTINTSET(x)	((x) + 0x2B4)
+/* DMA error interrupt enable, disable, status */
+#define USBD_SYSERRTINTST(x)	((x) + 0x2B8)
+#define USBD_SYSERRTINTCLR(x)	((x) + 0x2BC)
+#define USBD_SYSERRTINTSET(x)	((x) + 0x2C0)
+
+/**********************************************************************
+ * USBD_DEVINTST/USBD_DEVINTEN/USBD_DEVINTCLR/USBD_DEVINTSET/
+ * USBD_DEVINTPRI register definitions
+ **********************************************************************/
+#define USBD_ERR_INT		(1 << 9)
+#define USBD_EP_RLZED		(1 << 8)
+#define USBD_TXENDPKT		(1 << 7)
+#define USBD_RXENDPKT		(1 << 6)
+#define USBD_CDFULL		(1 << 5)
+#define USBD_CCEMPTY		(1 << 4)
+#define USBD_DEV_STAT		(1 << 3)
+#define USBD_EP_SLOW		(1 << 2)
+#define USBD_EP_FAST		(1 << 1)
+#define USBD_FRAME		(1 << 0)
+
+/**********************************************************************
+ * USBD_EPINTST/USBD_EPINTEN/USBD_EPINTCLR/USBD_EPINTSET/
+ * USBD_EPINTPRI register definitions
+ **********************************************************************/
+/* End point selection macro (RX) */
+#define USBD_RX_EP_SEL(e)	(1 << ((e) << 1))
+
+/* End point selection macro (TX) */
+#define USBD_TX_EP_SEL(e)	(1 << (((e) << 1) + 1))
+
+/**********************************************************************
+ * USBD_REEP/USBD_DMARST/USBD_DMARCLR/USBD_DMARSET/USBD_EPDMAST/
+ * USBD_EPDMAEN/USBD_EPDMADIS/
+ * USBD_NDDRTINTST/USBD_NDDRTINTCLR/USBD_NDDRTINTSET/
+ * USBD_EOTINTST/USBD_EOTINTCLR/USBD_EOTINTSET/
+ * USBD_SYSERRTINTST/USBD_SYSERRTINTCLR/USBD_SYSERRTINTSET
+ * register definitions
+ **********************************************************************/
+/* Endpoint selection macro */
+#define USBD_EP_SEL(e)		(1 << (e))
+
+/**********************************************************************
+ * SBD_DMAINTST/USBD_DMAINTEN
+ **********************************************************************/
+#define USBD_SYS_ERR_INT	(1 << 2)
+#define USBD_NEW_DD_INT		(1 << 1)
+#define USBD_EOT_INT		(1 << 0)
+
+/**********************************************************************
+ * USBD_RXPLEN register definitions
+ **********************************************************************/
+#define USBD_PKT_RDY		(1 << 11)
+#define USBD_DV			(1 << 10)
+#define USBD_PK_LEN_MASK	0x3FF
+
+/**********************************************************************
+ * USBD_CTRL register definitions
+ **********************************************************************/
+#define USBD_LOG_ENDPOINT(e)	((e) << 2)
+#define USBD_WR_EN		(1 << 1)
+#define USBD_RD_EN		(1 << 0)
+
+/**********************************************************************
+ * USBD_CMDCODE register definitions
+ **********************************************************************/
+#define USBD_CMD_CODE(c)	((c) << 16)
+#define USBD_CMD_PHASE(p)	((p) << 8)
+
+/**********************************************************************
+ * USBD_DMARST/USBD_DMARCLR/USBD_DMARSET register definitions
+ **********************************************************************/
+#define USBD_DMAEP(e)		(1 << (e))
+
+/* DD (DMA Descriptor) structure, requires word alignment */
+struct lpc32xx_usbd_dd {
+	u32 *dd_next;
+	u32 dd_setup;
+	u32 dd_buffer_addr;
+	u32 dd_status;
+	u32 dd_iso_ps_mem_addr;
+};
+
+/* dd_setup bit defines */
+#define DD_SETUP_ATLE_DMA_MODE	0x01
+#define DD_SETUP_NEXT_DD_VALID	0x04
+#define DD_SETUP_ISO_EP		0x10
+#define DD_SETUP_PACKETLEN(n)	(((n) & 0x7FF) << 5)
+#define DD_SETUP_DMALENBYTES(n)	(((n) & 0xFFFF) << 16)
+
+/* dd_status bit defines */
+#define DD_STATUS_DD_RETIRED	0x01
+#define DD_STATUS_STS_MASK	0x1E
+#define DD_STATUS_STS_NS	0x00 /* Not serviced */
+#define DD_STATUS_STS_BS	0x02 /* Being serviced */
+#define DD_STATUS_STS_NC	0x04 /* Normal completion */
+#define DD_STATUS_STS_DUR	0x06 /* Data underrun (short packet) */
+#define DD_STATUS_STS_DOR	0x08 /* Data overrun */
+#define DD_STATUS_STS_SE	0x12 /* System error */
+#define DD_STATUS_PKT_VAL	0x20 /* Packet valid */
+#define DD_STATUS_LSB_EX	0x40 /* LS byte extracted (ATLE) */
+#define DD_STATUS_MSB_EX	0x80 /* MS byte extracted (ATLE) */
+#define DD_STATUS_MLEN(n)	(((n) >> 8) & 0x3F)
+#define DD_STATUS_CURDMACNT(n)	(((n) >> 16) & 0xFFFF)
+
+/*
+ *
+ * Protocol engine bits below
+ *
+ */
+/* Device Interrupt Bit Definitions */
+#define FRAME_INT		0x00000001
+#define EP_FAST_INT		0x00000002
+#define EP_SLOW_INT		0x00000004
+#define DEV_STAT_INT		0x00000008
+#define CCEMTY_INT		0x00000010
+#define CDFULL_INT		0x00000020
+#define RxENDPKT_INT		0x00000040
+#define TxENDPKT_INT		0x00000080
+#define EP_RLZED_INT		0x00000100
+#define ERR_INT			0x00000200
+
+/* Rx & Tx Packet Length Definitions */
+#define PKT_LNGTH_MASK		0x000003FF
+#define PKT_DV			0x00000400
+#define PKT_RDY			0x00000800
+
+/* USB Control Definitions */
+#define CTRL_RD_EN		0x00000001
+#define CTRL_WR_EN		0x00000002
+
+/* Command Codes */
+#define CMD_SET_ADDR		0x00D00500
+#define CMD_CFG_DEV		0x00D80500
+#define CMD_SET_MODE		0x00F30500
+#define CMD_RD_FRAME		0x00F50500
+#define DAT_RD_FRAME		0x00F50200
+#define CMD_RD_TEST		0x00FD0500
+#define DAT_RD_TEST		0x00FD0200
+#define CMD_SET_DEV_STAT	0x00FE0500
+#define CMD_GET_DEV_STAT	0x00FE0500
+#define DAT_GET_DEV_STAT	0x00FE0200
+#define CMD_GET_ERR_CODE	0x00FF0500
+#define DAT_GET_ERR_CODE	0x00FF0200
+#define CMD_RD_ERR_STAT		0x00FB0500
+#define DAT_RD_ERR_STAT		0x00FB0200
+#define DAT_WR_BYTE(x)		(0x00000100 | ((x) << 16))
+#define CMD_SEL_EP(x)		(0x00000500 | ((x) << 16))
+#define DAT_SEL_EP(x)		(0x00000200 | ((x) << 16))
+#define CMD_SEL_EP_CLRI(x)	(0x00400500 | ((x) << 16))
+#define DAT_SEL_EP_CLRI(x)	(0x00400200 | ((x) << 16))
+#define CMD_SET_EP_STAT(x)	(0x00400500 | ((x) << 16))
+#define CMD_CLR_BUF		0x00F20500
+#define DAT_CLR_BUF		0x00F20200
+#define CMD_VALID_BUF		0x00FA0500
+
+/* Device Address Register Definitions */
+#define DEV_ADDR_MASK		0x7F
+#define DEV_EN			0x80
+
+/* Device Configure Register Definitions */
+#define CONF_DVICE		0x01
+
+/* Device Mode Register Definitions */
+#define AP_CLK			0x01
+#define INAK_CI			0x02
+#define INAK_CO			0x04
+#define INAK_II			0x08
+#define INAK_IO			0x10
+#define INAK_BI			0x20
+#define INAK_BO			0x40
+
+/* Device Status Register Definitions */
+#define DEV_CON			0x01
+#define DEV_CON_CH		0x02
+#define DEV_SUS			0x04
+#define DEV_SUS_CH		0x08
+#define DEV_RST			0x10
+
+/* Error Code Register Definitions */
+#define ERR_EC_MASK		0x0F
+#define ERR_EA			0x10
+
+/* Error Status Register Definitions */
+#define ERR_PID			0x01
+#define ERR_UEPKT		0x02
+#define ERR_DCRC		0x04
+#define ERR_TIMOUT		0x08
+#define ERR_EOP			0x10
+#define ERR_B_OVRN		0x20
+#define ERR_BTSTF		0x40
+#define ERR_TGL			0x80
+
+/* Endpoint Select Register Definitions */
+#define EP_SEL_F		0x01
+#define EP_SEL_ST		0x02
+#define EP_SEL_STP		0x04
+#define EP_SEL_PO		0x08
+#define EP_SEL_EPN		0x10
+#define EP_SEL_B_1_FULL		0x20
+#define EP_SEL_B_2_FULL		0x40
+
+/* Endpoint Status Register Definitions */
+#define EP_STAT_ST		0x01
+#define EP_STAT_DA		0x20
+#define EP_STAT_RF_MO		0x40
+#define EP_STAT_CND_ST		0x80
+
+/* Clear Buffer Register Definitions */
+#define CLR_BUF_PO		0x01
+
+/* DMA Interrupt Bit Definitions */
+#define EOT_INT			0x01
+#define NDD_REQ_INT		0x02
+#define SYS_ERR_INT		0x04
+
+#define	DRIVER_VERSION	"1.03"
+static const char driver_name[] = "lpc32xx_udc";
+
+/*
+ *
+ * proc interface support
+ *
+ */
+#ifdef CONFIG_USB_GADGET_DEBUG_FILES
+static char *epnames[] = {"INT", "ISO", "BULK", "CTRL"};
+static const char debug_filename[] = "driver/udc";
+
+static void proc_ep_show(struct seq_file *s, struct lpc32xx_ep *ep)
+{
+	struct lpc32xx_request *req;
+
+	seq_printf(s, "\n");
+	seq_printf(s, "%12s, maxpacket %4d %3s",
+			ep->ep.name, ep->ep.maxpacket,
+			ep->is_in ? "in" : "out");
+	seq_printf(s, " type %4s", epnames[ep->eptype]);
+	seq_printf(s, " ints: %12d", ep->totalints);
+
+	if (list_empty(&ep->queue))
+		seq_printf(s, "\t(queue empty)\n");
+	else {
+		list_for_each_entry(req, &ep->queue, queue) {
+			u32 length = req->req.actual;
+
+			seq_printf(s, "\treq %p len %d/%d buf %p\n",
+				   &req->req, length,
+				   req->req.length, req->req.buf);
+		}
+	}
+}
+
+static int proc_udc_show(struct seq_file *s, void *unused)
+{
+	struct lpc32xx_udc *udc = s->private;
+	struct lpc32xx_ep *ep;
+	unsigned long flags;
+
+	seq_printf(s, "%s: version %s\n", driver_name, DRIVER_VERSION);
+
+	spin_lock_irqsave(&udc->lock, flags);
+
+	seq_printf(s, "vbus %s, pullup %s, %s powered%s, gadget %s\n\n",
+		   udc->vbus ? "present" : "off",
+		   udc->enabled ? (udc->vbus ? "active" : "enabled") :
+		   "disabled",
+		   udc->selfpowered ? "self" : "VBUS",
+		   udc->suspended ? ", suspended" : "",
+		   udc->driver ? udc->driver->driver.name : "(none)");
+
+	if (udc->enabled && udc->vbus) {
+		proc_ep_show(s, &udc->ep[0]);
+		list_for_each_entry(ep, &udc->gadget.ep_list, ep.ep_list) {
+			if (ep->desc)
+				proc_ep_show(s, ep);
+		}
+	}
+
+	spin_unlock_irqrestore(&udc->lock, flags);
+
+	return 0;
+}
+
+static int proc_udc_open(struct inode *inode, struct file *file)
+{
+	return single_open(file, proc_udc_show, PDE(inode)->data);
+}
+
+static const struct file_operations proc_ops = {
+	.owner		= THIS_MODULE,
+	.open		= proc_udc_open,
+	.read		= seq_read,
+	.llseek		= seq_lseek,
+	.release	= single_release,
+};
+
+static void create_debug_file(struct lpc32xx_udc *udc)
+{
+	udc->pde = debugfs_create_file(debug_filename, 0, NULL, udc, &proc_ops);
+}
+
+static void remove_debug_file(struct lpc32xx_udc *udc)
+{
+	if (udc->pde)
+		debugfs_remove(udc->pde);
+}
+
+#else
+static inline void create_debug_file(struct lpc32xx_udc *udc) {}
+static inline void remove_debug_file(struct lpc32xx_udc *udc) {}
+#endif
+
+/* Primary initialization sequence for the ISP1301 transceiver */
+static void isp1301_udc_configure(struct lpc32xx_udc *udc)
+{
+	/* LPC32XX only supports DAT_SE0 USB mode */
+	/* This sequence is important */
+
+	/* Disable transparent UART mode first */
+	i2c_smbus_write_byte_data(udc->isp1301_i2c_client,
+		(ISP1301_I2C_MODE_CONTROL_1 | ISP1301_I2C_REG_CLEAR_ADDR),
+		MC1_UART_EN);
+
+	/* Set full speed and SE0 mode */
+	i2c_smbus_write_byte_data(udc->isp1301_i2c_client,
+		(ISP1301_I2C_MODE_CONTROL_1 | ISP1301_I2C_REG_CLEAR_ADDR), ~0);
+	i2c_smbus_write_byte_data(udc->isp1301_i2c_client,
+		ISP1301_I2C_MODE_CONTROL_1, (MC1_SPEED_REG | MC1_DAT_SE0));
+
+	/*
+	 * The PSW_OE enable bit state is reversed in the ISP1301 User's Guide
+	 */
+	i2c_smbus_write_byte_data(udc->isp1301_i2c_client,
+		(ISP1301_I2C_MODE_CONTROL_2 | ISP1301_I2C_REG_CLEAR_ADDR), ~0);
+	i2c_smbus_write_byte_data(udc->isp1301_i2c_client,
+		ISP1301_I2C_MODE_CONTROL_2, (MC2_BI_DI | MC2_SPD_SUSP_CTRL));
+
+	/* Driver VBUS_DRV high or low depending on board setup */
+	if (udc->board->vbus_drv_pol != 0)
+		i2c_smbus_write_byte_data(udc->isp1301_i2c_client,
+			ISP1301_I2C_OTG_CONTROL_1, OTG1_VBUS_DRV);
+	else
+		i2c_smbus_write_byte_data(udc->isp1301_i2c_client,
+			ISP1301_I2C_OTG_CONTROL_1 | ISP1301_I2C_REG_CLEAR_ADDR,
+			OTG1_VBUS_DRV);
+
+	/* Bi-directional mode with suspend control
+	 * Enable both pulldowns for now - the pullup will be enable when VBUS
+	 * is detected */
+	i2c_smbus_write_byte_data(udc->isp1301_i2c_client,
+		(ISP1301_I2C_OTG_CONTROL_1 | ISP1301_I2C_REG_CLEAR_ADDR), ~0);
+	i2c_smbus_write_byte_data(udc->isp1301_i2c_client,
+		ISP1301_I2C_OTG_CONTROL_1,
+		(0 | OTG1_DM_PULLDOWN | OTG1_DP_PULLDOWN));
+
+	/* Discharge VBUS (just in case) */
+	i2c_smbus_write_byte_data(udc->isp1301_i2c_client,
+		ISP1301_I2C_OTG_CONTROL_1, OTG1_VBUS_DISCHRG);
+	msleep(1);
+	i2c_smbus_write_byte_data(udc->isp1301_i2c_client,
+		(ISP1301_I2C_OTG_CONTROL_1 | ISP1301_I2C_REG_CLEAR_ADDR),
+		OTG1_VBUS_DISCHRG);
+
+	/* Clear and enable VBUS high edge interrupt */
+	i2c_smbus_write_byte_data(udc->isp1301_i2c_client,
+		ISP1301_I2C_INTERRUPT_LATCH | ISP1301_I2C_REG_CLEAR_ADDR, ~0);
+	i2c_smbus_write_byte_data(udc->isp1301_i2c_client,
+		ISP1301_I2C_INTERRUPT_FALLING | ISP1301_I2C_REG_CLEAR_ADDR, ~0);
+	i2c_smbus_write_byte_data(udc->isp1301_i2c_client,
+		ISP1301_I2C_INTERRUPT_FALLING, INT_VBUS_VLD);
+	i2c_smbus_write_byte_data(udc->isp1301_i2c_client,
+		ISP1301_I2C_INTERRUPT_RISING | ISP1301_I2C_REG_CLEAR_ADDR, ~0);
+	i2c_smbus_write_byte_data(udc->isp1301_i2c_client,
+		ISP1301_I2C_INTERRUPT_RISING, INT_VBUS_VLD);
+
+	/* Enable usb_need_clk clock after transceiver is initialized */
+	writel((readl(USB_CTRL) | (1 << 22)), USB_CTRL);
+
+	dev_info(udc->dev, "ISP1301 Vendor ID  : 0x%04x\n",
+		 i2c_smbus_read_word_data(udc->isp1301_i2c_client, 0x00));
+	dev_info(udc->dev, "ISP1301 Product ID : 0x%04x\n",
+		 i2c_smbus_read_word_data(udc->isp1301_i2c_client, 0x02));
+	dev_info(udc->dev, "ISP1301 Version ID : 0x%04x\n",
+		 i2c_smbus_read_word_data(udc->isp1301_i2c_client, 0x14));
+}
+
+/* Enables or disables the USB device pullup via the ISP1301 transceiver */
+static void isp1301_pullup_set(struct lpc32xx_udc *udc)
+{
+	if (udc->pullup)
+		/* Enable pullup for bus signalling */
+		i2c_smbus_write_byte_data(udc->isp1301_i2c_client,
+			ISP1301_I2C_OTG_CONTROL_1, OTG1_DP_PULLUP);
+	else
+		/* Enable pullup for bus signalling */
+		i2c_smbus_write_byte_data(udc->isp1301_i2c_client,
+			ISP1301_I2C_OTG_CONTROL_1 | ISP1301_I2C_REG_CLEAR_ADDR,
+			OTG1_DP_PULLUP);
+}
+
+static void pullup_work(struct work_struct *work)
+{
+	struct lpc32xx_udc *udc =
+		container_of(work, struct lpc32xx_udc, pullup_job);
+
+	isp1301_pullup_set(udc);
+}
+
+static void isp1301_pullup_enable(struct lpc32xx_udc *udc, int en_pullup,
+				  int block)
+{
+	if (en_pullup == udc->pullup)
+		return;
+
+	udc->pullup = en_pullup;
+	if (block)
+		isp1301_pullup_set(udc);
+	else
+		/* defer slow i2c pull up setting */
+		schedule_work(&udc->pullup_job);
+}
+
+#ifdef CONFIG_PM
+/* Powers up or down the ISP1301 transceiver */
+static void isp1301_set_powerstate(struct lpc32xx_udc *udc, int enable)
+{
+	if (enable != 0)
+		/* Power up ISP1301 - this ISP1301 will automatically wakeup
+		   when VBUS is detected */
+		i2c_smbus_write_byte_data(udc->isp1301_i2c_client,
+			ISP1301_I2C_MODE_CONTROL_2 | ISP1301_I2C_REG_CLEAR_ADDR,
+			MC2_GLOBAL_PWR_DN);
+	else
+		/* Power down ISP1301 */
+		i2c_smbus_write_byte_data(udc->isp1301_i2c_client,
+			ISP1301_I2C_MODE_CONTROL_2, MC2_GLOBAL_PWR_DN);
+}
+
+static void power_work(struct work_struct *work)
+{
+	struct lpc32xx_udc *udc =
+		container_of(work, struct lpc32xx_udc, power_job);
+
+	isp1301_set_powerstate(udc, udc->poweron);
+}
+#endif
+
+/*
+ *
+ * USB protocol engine command/data read/write helper functions
+ *
+ */
+/* Issues a single command to the USB device state machine */
+static void udc_protocol_cmd_w(struct lpc32xx_udc *udc, u32 cmd)
+{
+	u32 pass = 0;
+	int to;
+
+	/* EP may lock on CLRI if this read isn't done */
+	u32 tmp = readl(USBD_DEVINTST(udc->udp_baseaddr));
+	(void) tmp;
+
+	while (pass == 0) {
+		writel(USBD_CCEMPTY, USBD_DEVINTCLR(udc->udp_baseaddr));
+
+		/* Write command code */
+		writel(cmd, USBD_CMDCODE(udc->udp_baseaddr));
+		to = 10000;
+		while (((readl(USBD_DEVINTST(udc->udp_baseaddr)) &
+			 USBD_CCEMPTY) == 0) && (to > 0)) {
+			to--;
+		}
+
+		if (to > 0)
+			pass = 1;
+
+		cpu_relax();
+	}
+}
+
+/* Issues 2 commands (or command and data) to the USB device state machine */
+static inline void udc_protocol_cmd_data_w(struct lpc32xx_udc *udc, u32 cmd,
+					   u32 data)
+{
+	udc_protocol_cmd_w(udc, cmd);
+	udc_protocol_cmd_w(udc, data);
+}
+
+/* Issues a single command to the USB device state machine and reads
+ * response data */
+static u32 udc_protocol_cmd_r(struct lpc32xx_udc *udc, u32 cmd)
+{
+	u32 tmp;
+	int to = 1000;
+
+	/* Write a command and read data from the protocol engine */
+	writel((USBD_CDFULL | USBD_CCEMPTY),
+		     USBD_DEVINTCLR(udc->udp_baseaddr));
+
+	/* Write command code */
+	udc_protocol_cmd_w(udc, cmd);
+
+	tmp = readl(USBD_DEVINTST(udc->udp_baseaddr));
+	while ((!(readl(USBD_DEVINTST(udc->udp_baseaddr)) & USBD_CDFULL))
+	       && (to > 0))
+		to--;
+	if (!to)
+		dev_dbg(udc->dev,
+			"Protocol engine didn't receive response (CDFULL)\n");
+
+	return readl(USBD_CMDDATA(udc->udp_baseaddr));
+}
+
+/*
+ *
+ * USB device interrupt mask support functions
+ *
+ */
+/* Enable one or more USB device interrupts */
+static inline void uda_enable_devint(struct lpc32xx_udc *udc, u32 devmask)
+{
+	udc->enabled_devints |= devmask;
+	writel(udc->enabled_devints, USBD_DEVINTEN(udc->udp_baseaddr));
+}
+
+/* Disable one or more USB device interrupts */
+static inline void uda_disable_devint(struct lpc32xx_udc *udc, u32 mask)
+{
+	udc->enabled_devints &= ~mask;
+	writel(udc->enabled_devints, USBD_DEVINTEN(udc->udp_baseaddr));
+}
+
+/* Clear one or more USB device interrupts */
+static inline void uda_clear_devint(struct lpc32xx_udc *udc, u32 mask)
+{
+	writel(mask, USBD_DEVINTCLR(udc->udp_baseaddr));
+}
+
+/*
+ *
+ * Endpoint interrupt disable/enable functions
+ *
+ */
+/* Enable one or more USB endpoint interrupts */
+static void uda_enable_hwepint(struct lpc32xx_udc *udc, u32 hwep)
+{
+	udc->enabled_hwepints |= (1 << hwep);
+	writel(udc->enabled_hwepints, USBD_EPINTEN(udc->udp_baseaddr));
+}
+
+/* Disable one or more USB endpoint interrupts */
+static void uda_disable_hwepint(struct lpc32xx_udc *udc, u32 hwep)
+{
+	udc->enabled_hwepints &= ~(1 << hwep);
+	writel(udc->enabled_hwepints, USBD_EPINTEN(udc->udp_baseaddr));
+}
+
+/* Clear one or more USB endpoint interrupts */
+static inline void uda_clear_hwepint(struct lpc32xx_udc *udc, u32 hwep)
+{
+	writel((1 << hwep), USBD_EPINTCLR(udc->udp_baseaddr));
+}
+
+/* Enable DMA for the HW channel */
+static inline void udc_ep_dma_enable(struct lpc32xx_udc *udc, u32 hwep)
+{
+	writel((1 << hwep), USBD_EPDMAEN(udc->udp_baseaddr));
+}
+
+/* Disable DMA for the HW channel */
+static inline void udc_ep_dma_disable(struct lpc32xx_udc *udc, u32 hwep)
+{
+	writel((1 << hwep), USBD_EPDMADIS(udc->udp_baseaddr));
+}
+
+/*
+ *
+ * Endpoint realize/unrealize functions
+ *
+ */
+/* Before an endpoint can be used, it needs to be realized
+ * in the USB protocol engine - this realizes the endpoint.
+ * The interrupt (FIFO or DMA) is not enabled with this function */
+static void udc_realize_hwep(struct lpc32xx_udc *udc, u32 hwep,
+			     u32 maxpacket)
+{
+	int to = 1000;
+
+	writel(USBD_EP_RLZED, USBD_DEVINTCLR(udc->udp_baseaddr));
+	writel(hwep, USBD_EPIND(udc->udp_baseaddr));
+	udc->realized_eps |= (1 << hwep);
+	writel(udc->realized_eps, USBD_REEP(udc->udp_baseaddr));
+	writel(maxpacket, USBD_EPMAXPSIZE(udc->udp_baseaddr));
+
+	/* Wait until endpoint is realized in hardware */
+	while ((!(readl(USBD_DEVINTST(udc->udp_baseaddr)) &
+		  USBD_EP_RLZED)) && (to > 0))
+		to--;
+	if (!to)
+		dev_dbg(udc->dev, "EP not correctly realized in hardware\n");
+
+	writel(USBD_EP_RLZED, USBD_DEVINTCLR(udc->udp_baseaddr));
+}
+
+/* Unrealize an EP */
+static void udc_unrealize_hwep(struct lpc32xx_udc *udc, u32 hwep)
+{
+	udc->realized_eps &= ~(1 << hwep);
+	writel(udc->realized_eps, USBD_REEP(udc->udp_baseaddr));
+}
+
+/*
+ *
+ * Endpoint support functions
+ *
+ */
+/* Select and clear endpoint interrupt */
+static u32 udc_selep_clrint(struct lpc32xx_udc *udc, u32 hwep)
+{
+	udc_protocol_cmd_w(udc, CMD_SEL_EP_CLRI(hwep));
+	return udc_protocol_cmd_r(udc, DAT_SEL_EP_CLRI(hwep));
+}
+
+/* Disables the endpoint in the USB protocol engine */
+static void udc_disable_hwep(struct lpc32xx_udc *udc, u32 hwep)
+{
+	udc_protocol_cmd_data_w(udc, CMD_SET_EP_STAT(hwep),
+				DAT_WR_BYTE(EP_STAT_DA));
+}
+
+/* Stalls the endpoint - endpoint will return STALL */
+static void udc_stall_hwep(struct lpc32xx_udc *udc, u32 hwep)
+{
+	udc_protocol_cmd_data_w(udc, CMD_SET_EP_STAT(hwep),
+				DAT_WR_BYTE(EP_STAT_ST));
+}
+
+/* Clear stall or reset endpoint */
+static void udc_clrstall_hwep(struct lpc32xx_udc *udc, u32 hwep)
+{
+	udc_protocol_cmd_data_w(udc, CMD_SET_EP_STAT(hwep),
+				DAT_WR_BYTE(0));
+}
+
+/* Select an endpoint for endpoint status, clear, validate */
+static void udc_select_hwep(struct lpc32xx_udc *udc, u32 hwep)
+{
+	udc_protocol_cmd_w(udc, CMD_SEL_EP(hwep));
+}
+
+/*
+ *
+ * Endpoint buffer management functions
+ *
+ */
+/* Clear the current endpoint's buffer */
+static void udc_clr_buffer_hwep(struct lpc32xx_udc *udc, u32 hwep)
+{
+	udc_select_hwep(udc, hwep);
+	udc_protocol_cmd_w(udc, CMD_CLR_BUF);
+}
+
+/* Validate the current endpoint's buffer */
+static void udc_val_buffer_hwep(struct lpc32xx_udc *udc, u32 hwep)
+{
+	udc_select_hwep(udc, hwep);
+	udc_protocol_cmd_w(udc, CMD_VALID_BUF);
+}
+
+static inline u32 udc_clearep_getsts(struct lpc32xx_udc *udc, u32 hwep)
+{
+	/* Clear EP interrupt */
+	uda_clear_hwepint(udc, hwep);
+	return udc_selep_clrint(udc, hwep);
+}
+
+/*
+ *
+ * USB EP DMA support
+ *
+ */
+/* Allocate a DMA Descriptor */
+static struct lpc32xx_usbd_dd_gad *udc_dd_alloc(struct lpc32xx_udc *udc)
+{
+	dma_addr_t			dma;
+	struct lpc32xx_usbd_dd_gad	*dd;
+
+	dd = (struct lpc32xx_usbd_dd_gad *) dma_pool_alloc(
+			udc->dd_cache, (GFP_KERNEL | GFP_DMA), &dma);
+	if (dd)
+		dd->this_dma = dma;
+
+	return dd;
+}
+
+/* Free a DMA Descriptor */
+static void udc_dd_free(struct lpc32xx_udc *udc, struct lpc32xx_usbd_dd_gad *dd)
+{
+	dma_pool_free(udc->dd_cache, dd, dd->this_dma);
+}
+
+/*
+ *
+ * USB setup and shutdown functions
+ *
+ */
+/* Enables or disables most of the USB system clocks when low power mode is
+ * needed. Clocks are typically started on a connection event, and disabled
+ * when a cable is disconnected */
+#define OTGOFF_CLK_MASK (AHB_M_CLOCK_ON | I2C_CLOCK_ON)
+static void udc_clk_set(struct lpc32xx_udc *udc, int enable)
+{
+	int to = 1000;
+
+	if (enable != 0) {
+		if (udc->clocked)
+			return;
+
+		udc->clocked = 1;
+
+		/* 48MHz PLL up */
+		clk_enable(udc->usb_pll_clk);
+
+		/* Enable the USB device clock */
+		writel(readl(USB_CTRL) | USB_DEV_NEED_CLK_EN,
+			     USB_CTRL);
+
+		/* Set to enable all needed USB OTG clocks */
+		writel(USB_CLOCK_MASK, USB_OTG_CLK_CTRL(udc));
+
+		while (((readl(USB_OTG_CLK_STAT(udc)) & USB_CLOCK_MASK) !=
+			USB_CLOCK_MASK) && (to > 0))
+			to--;
+		if (!to)
+			dev_dbg(udc->dev, "Cannot enable USB OTG clocking\n");
+	} else {
+		if (!udc->clocked)
+			return;
+
+		udc->clocked = 0;
+
+		/* Never disable the USB_HCLK during normal operation */
+
+		/* 48MHz PLL dpwn */
+		clk_disable(udc->usb_pll_clk);
+
+		/* Enable the USB device clock */
+		writel(readl(USB_CTRL) & ~USB_DEV_NEED_CLK_EN,
+			     USB_CTRL);
+
+		/* Set to enable all needed USB OTG clocks */
+		writel(OTGOFF_CLK_MASK, USB_OTG_CLK_CTRL(udc));
+
+		while (((readl(USB_OTG_CLK_STAT(udc)) &
+			 OTGOFF_CLK_MASK) !=
+			OTGOFF_CLK_MASK) && (to > 0))
+			to--;
+		if (!to)
+			dev_dbg(udc->dev, "Cannot disable USB OTG clocking\n");
+	}
+}
+
+/* Set/reset USB device address */
+static void udc_set_address(struct lpc32xx_udc *udc, u32 addr)
+{
+	/* Address will be latched at the end of the status phase, or
+	   latched immediately if function is called twice */
+	udc_protocol_cmd_data_w(udc, CMD_SET_ADDR,
+				DAT_WR_BYTE(DEV_EN | addr));
+}
+
+/* Setup up a IN request for DMA transfer - this consists of determining the
+ * list of DMA addresses for the transfer, allocating DMA Descriptors,
+ * installing the DD into the UDCA, and then enabling the DMA for that EP */
+static int udc_ep_in_req_dma(struct lpc32xx_udc *udc, struct lpc32xx_ep *ep)
+{
+	struct lpc32xx_request *req;
+	u32 hwep = ep->hwep_num;
+
+	ep->req_pending = 1;
+
+	/* There will always be a request waiting here */
+	req = list_entry(ep->queue.next, struct lpc32xx_request, queue);
+
+	/* Place the DD Descriptor into the UDCA */
+	udc->udca_v_base[hwep] = req->dd_desc_ptr->this_dma;
+
+	/* Enable DMA and interrupt for the HW EP */
+	udc_ep_dma_enable(udc, hwep);
+
+	/* Clear ZLP if last packet is not of MAXP size */
+	if (req->req.length % ep->ep.maxpacket)
+		req->send_zlp = 0;
+
+	return 0;
+}
+
+/* Setup up a OUT request for DMA transfer - this consists of determining the
+ * list of DMA addresses for the transfer, allocating DMA Descriptors,
+ * installing the DD into the UDCA, and then enabling the DMA for that EP */
+static int udc_ep_out_req_dma(struct lpc32xx_udc *udc, struct lpc32xx_ep *ep)
+{
+	struct lpc32xx_request *req;
+	u32 hwep = ep->hwep_num;
+
+	ep->req_pending = 1;
+
+	/* There will always be a request waiting here */
+	req = list_entry(ep->queue.next, struct lpc32xx_request, queue);
+
+	/* Place the DD Descriptor into the UDCA */
+	udc->udca_v_base[hwep] = req->dd_desc_ptr->this_dma;
+
+	/* Enable DMA and interrupt for the HW EP */
+	udc_ep_dma_enable(udc, hwep);
+	return 0;
+}
+
+static void udc_disable(struct lpc32xx_udc *udc)
+{
+	u32 i;
+
+	/* Disable device */
+	udc_protocol_cmd_data_w(udc, CMD_CFG_DEV, DAT_WR_BYTE(0));
+	udc_protocol_cmd_data_w(udc, CMD_SET_DEV_STAT, DAT_WR_BYTE(0));
+
+	/* Disable all device interrupts (including EP0) */
+	uda_disable_devint(udc, 0x3FF);
+
+	/* Disable and reset all endpoint interrupts */
+	for (i = 0; i < 32; i++) {
+		uda_disable_hwepint(udc, i);
+		uda_clear_hwepint(udc, i);
+		udc_disable_hwep(udc, i);
+		udc_unrealize_hwep(udc, i);
+		udc->udca_v_base[i] = 0;
+
+		/* Disable and clear all interrupts and DMA */
+		udc_ep_dma_disable(udc, i);
+		writel((1 << i), USBD_EOTINTCLR(udc->udp_baseaddr));
+		writel((1 << i), USBD_NDDRTINTCLR(udc->udp_baseaddr));
+		writel((1 << i), USBD_SYSERRTINTCLR(udc->udp_baseaddr));
+		writel((1 << i), USBD_DMARCLR(udc->udp_baseaddr));
+	}
+
+	/* Disable DMA interrupts */
+	writel(0, USBD_DMAINTEN(udc->udp_baseaddr));
+
+	writel(0, USBD_UDCAH(udc->udp_baseaddr));
+}
+
+static void udc_enable(struct lpc32xx_udc *udc)
+{
+	u32 i;
+	struct lpc32xx_ep *ep = &udc->ep[0];
+
+	/* Start with known state */
+	udc_disable(udc);
+
+	/* Enable device */
+	udc_protocol_cmd_data_w(udc, CMD_SET_DEV_STAT, DAT_WR_BYTE(DEV_CON));
+
+	/* EP interrupts on high priority, FRAME interrupt on low priority */
+	writel(USBD_EP_FAST, USBD_DEVINTPRI(udc->udp_baseaddr));
+	writel(0xFFFF, USBD_EPINTPRI(udc->udp_baseaddr));
+
+	/* Clear any pending device interrupts */
+	writel(0x3FF, USBD_DEVINTCLR(udc->udp_baseaddr));
+
+	/* Setup UDCA - not yet used (DMA) */
+	writel(udc->udca_p_base, USBD_UDCAH(udc->udp_baseaddr));
+
+	/* Only enable EP0 in and out for now, EP0 only works in FIFO mode */
+	for (i = 0; i <= 1; i++) {
+		udc_realize_hwep(udc, i, ep->ep.maxpacket);
+		uda_enable_hwepint(udc, i);
+		udc_select_hwep(udc, i);
+		udc_clrstall_hwep(udc, i);
+		udc_clr_buffer_hwep(udc, i);
+	}
+
+	/* Device interrupt setup */
+	uda_clear_devint(udc, (USBD_ERR_INT | USBD_DEV_STAT | USBD_EP_SLOW |
+			       USBD_EP_FAST));
+	uda_enable_devint(udc, (USBD_ERR_INT | USBD_DEV_STAT | USBD_EP_SLOW |
+				USBD_EP_FAST));
+
+	/* Set device address to 0 - called twice to force a latch in the USB
+	   engine without the need of a setup packet status closure */
+	udc_set_address(udc, 0);
+	udc_set_address(udc, 0);
+
+	/* Enable master DMA interrupts */
+	writel((USBD_SYS_ERR_INT | USBD_EOT_INT),
+		     USBD_DMAINTEN(udc->udp_baseaddr));
+
+	udc->dev_status = 0;
+}
+
+/*
+ *
+ * USB device board specific events handled via callbacks
+ *
+ */
+/* Connection change event - notify board function of change */
+static void uda_power_event(struct lpc32xx_udc *udc, u32 conn)
+{
+	/* Just notify of a connection change event (optional) */
+	if (udc->board->conn_chgb != NULL)
+		udc->board->conn_chgb(conn);
+}
+
+/* Suspend/resume event - notify board function of change */
+static void uda_resm_susp_event(struct lpc32xx_udc *udc, u32 conn)
+{
+	/* Just notify of a Suspend/resume change event (optional) */
+	if (udc->board->susp_chgb != NULL)
+		udc->board->susp_chgb(conn);
+
+	if (conn)
+		udc->suspended = 0;
+	else
+		udc->suspended = 1;
+}
+
+/* Remote wakeup enable/disable - notify board function of change */
+static void uda_remwkp_cgh(struct lpc32xx_udc *udc)
+{
+	if (udc->board->rmwk_chgb != NULL)
+		udc->board->rmwk_chgb(udc->dev_status &
+				      (1 << USB_DEVICE_REMOTE_WAKEUP));
+}
+
+/* Reads data from FIFO, adjusts for alignment and data size */
+static void udc_pop_fifo(struct lpc32xx_udc *udc, u8 *data, u32 bytes)
+{
+	int n, i, bl;
+	u16 *p16;
+	u32 *p32, tmp, cbytes;
+
+	/* Use optimal data transfer method based on source address and size */
+	switch (((u32) data) & 0x3) {
+	case 0: /* 32-bit aligned */
+		p32 = (u32 *) data;
+		cbytes = (bytes & ~0x3);
+
+		/* Copy 32-bit aligned data first */
+		for (n = 0; n < cbytes; n += 4)
+			*p32++ = readl(USBD_RXDATA(udc->udp_baseaddr));
+
+		/* Handle any remaining bytes */
+		bl = bytes - cbytes;
+		if (bl) {
+			tmp = readl(USBD_RXDATA(udc->udp_baseaddr));
+			for (n = 0; n < bl; n++)
+				data[cbytes + n] = ((tmp >> (n * 8)) & 0xFF);
+
+		}
+		break;
+
+	case 1: /* 8-bit aligned */
+	case 3:
+		/* Each byte has to be handled independently */
+		for (n = 0; n < bytes; n += 4) {
+			tmp = readl(USBD_RXDATA(udc->udp_baseaddr));
+
+			bl = bytes - n;
+			if (bl > 3)
+				bl = 3;
+
+			for (i = 0; i < bl; i++)
+				data[n + i] = (u8) ((tmp >> (n * 8)) & 0xFF);
+		}
+		break;
+
+	case 2: /* 16-bit aligned */
+		p16 = (u16 *) data;
+		cbytes = (bytes & ~0x3);
+
+		/* Copy 32-bit sized objects first with 16-bit alignment */
+		for (n = 0; n < cbytes; n += 4) {
+			tmp = readl(USBD_RXDATA(udc->udp_baseaddr));
+			*p16++ = (u16)(tmp & 0xFFFF);
+			*p16++ = (u16)((tmp >> 16) & 0xFFFF);
+		}
+
+		/* Handle any remaining bytes */
+		bl = bytes - cbytes;
+		if (bl) {
+			tmp = readl(USBD_RXDATA(udc->udp_baseaddr));
+			for (n = 0; n < bl; n++)
+				data[cbytes + n] = ((tmp >> (n * 8)) & 0xFF);
+		}
+		break;
+	}
+}
+
+/* Read data from the FIFO for an endpoint. This function is for endpoints (such
+ * as EP0) that don't use DMA. This function should only be called if a packet
+ * is known to be ready to read for the endpoint. Note that the endpoint must
+ * be selected in the protocol engine prior to this call. */
+static u32 udc_read_hwep(struct lpc32xx_udc *udc, u32 hwep, u32 *data,
+			 u32 bytes)
+{
+	u32 tmpv;
+	int to = 1000;
+	u32 tmp, hwrep = ((hwep & 0x1E) << 1) | CTRL_RD_EN;
+
+	/* Setup read of endpoint */
+	writel(hwrep, USBD_CTRL(udc->udp_baseaddr));
+
+	/* Wait until packet is ready */
+	while ((((tmpv = readl(USBD_RXPLEN(udc->udp_baseaddr))) &
+		 PKT_RDY) == 0)	&& (to > 0))
+		to--;
+	if (!to)
+		dev_dbg(udc->dev, "No packet ready on FIFO EP read\n");
+
+	/* Mask out count */
+	tmp = tmpv & PKT_LNGTH_MASK;
+	if (bytes < tmp)
+		tmp = bytes;
+
+	if ((tmp > 0) && (data != NULL))
+		udc_pop_fifo(udc, (u8 *) data, tmp);
+
+	writel(((hwep & 0x1E) << 1), USBD_CTRL(udc->udp_baseaddr));
+
+	/* Clear the buffer */
+	udc_clr_buffer_hwep(udc, hwep);
+
+	return tmp;
+}
+
+/* Stuffs data into the FIFO, adjusts for alignment and data size */
+static void udc_stuff_fifo(struct lpc32xx_udc *udc, u8 *data, u32 bytes)
+{
+	int n, i, bl;
+	u16 *p16;
+	u32 *p32, tmp, cbytes;
+
+	/* Use optimal data transfer method based on source address and size */
+	switch (((u32) data) & 0x3) {
+	case 0: /* 32-bit aligned */
+		p32 = (u32 *) data;
+		cbytes = (bytes & ~0x3);
+
+		/* Copy 32-bit aligned data first */
+		for (n = 0; n < cbytes; n += 4)
+			writel(*p32++, USBD_TXDATA(udc->udp_baseaddr));
+
+		/* Handle any remaining bytes */
+		bl = bytes - cbytes;
+		if (bl) {
+			tmp = 0;
+			for (n = 0; n < bl; n++)
+				tmp |= data[cbytes + n] << (n * 8);
+
+			writel(tmp, USBD_TXDATA(udc->udp_baseaddr));
+		}
+		break;
+
+	case 1: /* 8-bit aligned */
+	case 3:
+		/* Each byte has to be handled independently */
+		for (n = 0; n < bytes; n += 4) {
+			bl = bytes - n;
+			if (bl > 4)
+				bl = 4;
+
+			tmp = 0;
+			for (i = 0; i < bl; i++)
+				tmp |= data[n + i] << (i * 8);
+
+			writel(tmp, USBD_TXDATA(udc->udp_baseaddr));
+		}
+		break;
+
+	case 2: /* 16-bit aligned */
+		p16 = (u16 *) data;
+		cbytes = (bytes & ~0x3);
+
+		/* Copy 32-bit aligned data first */
+		for (n = 0; n < cbytes; n += 4) {
+			tmp = *p16++ & 0xFFFF;
+			tmp |= (*p16++ & 0xFFFF) << 16;
+			writel(tmp, USBD_TXDATA(udc->udp_baseaddr));
+		}
+
+		/* Handle any remaining bytes */
+		bl = bytes - cbytes;
+		if (bl) {
+			tmp = 0;
+			for (n = 0; n < bl; n++)
+				tmp |= data[cbytes + n] << (n * 8);
+
+			writel(tmp, USBD_TXDATA(udc->udp_baseaddr));
+		}
+		break;
+	}
+}
+
+/* Write data to the FIFO for an endpoint. This function is for endpoints (such
+ * as EP0) that don't use DMA. Note that the endpoint must be selected in the
+ * protocol engine prior to this call. */
+static void udc_write_hwep(struct lpc32xx_udc *udc, u32 hwep, u32 *data,
+			   u32 bytes)
+{
+	u32 hwwep = ((hwep & 0x1E) << 1) | CTRL_WR_EN;
+
+	if ((bytes > 0) && (data == NULL))
+		return;
+
+	/* Setup write of endpoint */
+	writel(hwwep, USBD_CTRL(udc->udp_baseaddr));
+
+	writel(bytes, USBD_TXPLEN(udc->udp_baseaddr));
+
+	/* Need at least 1 byte to trigger TX */
+	if (bytes == 0)
+		writel(0, USBD_TXDATA(udc->udp_baseaddr));
+	else
+		udc_stuff_fifo(udc, (u8 *) data, bytes);
+
+	writel(((hwep & 0x1E) << 1), USBD_CTRL(udc->udp_baseaddr));
+
+	udc_val_buffer_hwep(udc, hwep);
+}
+
+/* USB device reset - resets USB to a default state with just EP0
+   enabled */
+static void uda_usb_reset(struct lpc32xx_udc *udc)
+{
+	u32 i = 0;
+	/* Re-init device controller and EP0 */
+	udc_enable(udc);
+	udc->gadget.speed = USB_SPEED_FULL;
+
+	for (i = 1; i < NUM_ENDPOINTS; i++) {
+		struct lpc32xx_ep *ep = &udc->ep[i];
+		ep->req_pending = 0;
+	}
+}
+
+/* Send a ZLP on EP0 */
+static void udc_ep0_send_zlp(struct lpc32xx_udc *udc)
+{
+	udc_write_hwep(udc, EP_IN, NULL, 0);
+}
+
+/* Get current frame number */
+static u16 udc_get_current_frame(struct lpc32xx_udc *udc)
+{
+	u16 flo, fhi;
+
+	udc_protocol_cmd_w(udc, CMD_RD_FRAME);
+	flo = (u16) udc_protocol_cmd_r(udc, DAT_RD_FRAME);
+	fhi = (u16) udc_protocol_cmd_r(udc, DAT_RD_FRAME);
+
+	return (fhi << 8) | flo;
+}
+
+/* Set the device as configured - enables all endpoints */
+static inline void udc_set_device_configured(struct lpc32xx_udc *udc)
+{
+	udc_protocol_cmd_data_w(udc, CMD_CFG_DEV, DAT_WR_BYTE(CONF_DVICE));
+}
+
+/* Set the device as unconfigured - disables all endpoints */
+static inline void udc_set_device_unconfigured(struct lpc32xx_udc *udc)
+{
+	udc_protocol_cmd_data_w(udc, CMD_CFG_DEV, DAT_WR_BYTE(0));
+}
+
+/* reinit == restore initial software state */
+static void udc_reinit(struct lpc32xx_udc *udc)
+{
+	u32 i;
+
+	INIT_LIST_HEAD(&udc->gadget.ep_list);
+	INIT_LIST_HEAD(&udc->gadget.ep0->ep_list);
+
+	for (i = 0; i < NUM_ENDPOINTS; i++) {
+		struct lpc32xx_ep *ep = &udc->ep[i];
+
+		if (i != 0)
+			list_add_tail(&ep->ep.ep_list, &udc->gadget.ep_list);
+		ep->desc = NULL;
+		ep->ep.maxpacket = ep->maxpacket;
+		INIT_LIST_HEAD(&ep->queue);
+		ep->req_pending = 0;
+	}
+
+	udc->ep0state = WAIT_FOR_SETUP;
+}
+
+/* Must be called with lock */
+static void done(struct lpc32xx_ep *ep, struct lpc32xx_request *req, int status)
+{
+	struct lpc32xx_udc *udc = ep->udc;
+
+	list_del_init(&req->queue);
+	if (req->req.status == -EINPROGRESS)
+		req->req.status = status;
+	else
+		status = req->req.status;
+
+	if (ep->lep) {
+		enum dma_data_direction direction;
+
+		if (ep->is_in)
+			direction = DMA_TO_DEVICE;
+		else
+			direction = DMA_FROM_DEVICE;
+
+		if (req->mapped) {
+			dma_unmap_single(ep->udc->gadget.dev.parent,
+					req->req.dma, req->req.length,
+					direction);
+			req->req.dma = 0;
+			req->mapped = 0;
+		} else
+			dma_sync_single_for_cpu(ep->udc->gadget.dev.parent,
+						req->req.dma, req->req.length,
+						direction);
+
+		/* Free DDs */
+		udc_dd_free(udc, req->dd_desc_ptr);
+	}
+
+	if (status && status != -ESHUTDOWN)
+		ep_dbg(ep, "%s done %p, status %d\n", ep->ep.name, req, status);
+
+	ep->req_pending = 0;
+	spin_unlock(&udc->lock);
+	req->req.complete(&ep->ep, &req->req);
+	spin_lock(&udc->lock);
+}
+
+/* Must be called with lock */
+static void nuke(struct lpc32xx_ep *ep, int status)
+{
+	struct lpc32xx_request *req;
+
+	while (!list_empty(&ep->queue)) {
+		req = list_entry(ep->queue.next, struct lpc32xx_request, queue);
+		done(ep, req, status);
+	}
+
+	if (ep->desc && status == -ESHUTDOWN) {
+		uda_disable_hwepint(ep->udc, ep->hwep_num);
+		udc_disable_hwep(ep->udc, ep->hwep_num);
+	}
+}
+
+/* IN endpoint 0 transfer */
+static int udc_ep0_in_req(struct lpc32xx_udc *udc)
+{
+	struct lpc32xx_request *req;
+	struct lpc32xx_ep *ep0 = &udc->ep[0];
+	u32 tsend, ts = 0;
+
+	if (list_empty(&ep0->queue))
+		/* Nothing to send */
+		return 0;
+	else
+		req = list_entry(ep0->queue.next, struct lpc32xx_request,
+				 queue);
+
+	tsend = ts = req->req.length - req->req.actual;
+	if (ts == 0) {
+		/* Send a ZLP */
+		udc_ep0_send_zlp(udc);
+		done(ep0, req, 0);
+		return 1;
+	} else if (ts > ep0->ep.maxpacket)
+		ts = ep0->ep.maxpacket; /* Just send what we can */
+
+	/* Write data to the EP0 FIFO and start transfer */
+	udc_write_hwep(udc, EP_IN, (req->req.buf + req->req.actual), ts);
+
+	/* Increment data pointer */
+	req->req.actual += ts;
+
+	if (tsend >= ep0->ep.maxpacket)
+		return 0; /* Stay in data transfer state */
+
+	/* Transfer request is complete */
+	udc->ep0state = WAIT_FOR_SETUP;
+	done(ep0, req, 0);
+	return 1;
+}
+
+/* OUT endpoint 0 transfer */
+static int udc_ep0_out_req(struct lpc32xx_udc *udc)
+{
+	struct lpc32xx_request *req;
+	struct lpc32xx_ep *ep0 = &udc->ep[0];
+	u32 tr, bufferspace;
+
+	if (list_empty(&ep0->queue))
+		return 0;
+	else
+		req = list_entry(ep0->queue.next, struct lpc32xx_request,
+				 queue);
+
+	if (req) {
+		if (req->req.length == 0) {
+			/* Just dequeue request */
+			done(ep0, req, 0);
+			udc->ep0state = WAIT_FOR_SETUP;
+			return 1;
+		}
+
+		/* Get data from FIFO */
+		bufferspace = req->req.length - req->req.actual;
+		if (bufferspace > ep0->ep.maxpacket)
+			bufferspace = ep0->ep.maxpacket;
+
+		/* Copy data to buffer */
+		prefetchw(req->req.buf + req->req.actual);
+		tr = udc_read_hwep(udc, EP_OUT, req->req.buf + req->req.actual,
+				   bufferspace);
+		req->req.actual += bufferspace;
+
+		if (tr < ep0->ep.maxpacket) {
+			/* This is the last packet */
+			done(ep0, req, 0);
+			udc->ep0state = WAIT_FOR_SETUP;
+			return 1;
+		}
+	}
+
+	return 0;
+}
+
+/* Must be called with lock */
+static void stop_activity(struct lpc32xx_udc *udc)
+{
+	struct usb_gadget_driver *driver = udc->driver;
+	int i;
+
+	if (udc->gadget.speed == USB_SPEED_UNKNOWN)
+		driver = NULL;
+
+	udc->gadget.speed = USB_SPEED_UNKNOWN;
+	udc->suspended = 0;
+
+	for (i = 0; i < NUM_ENDPOINTS; i++) {
+		struct lpc32xx_ep *ep = &udc->ep[i];
+		nuke(ep, -ESHUTDOWN);
+	}
+	if (driver) {
+		spin_unlock(&udc->lock);
+		driver->disconnect(&udc->gadget);
+		spin_lock(&udc->lock);
+	}
+
+	isp1301_pullup_enable(udc, 0, 0);
+	udc_disable(udc);
+	udc_reinit(udc);
+}
+
+/*
+ * Activate or kill host pullup
+ * Can be called with or without lock
+ */
+static void pullup(struct lpc32xx_udc *udc, int is_on)
+{
+	if (!udc->clocked)
+		return;
+
+	if (!udc->enabled || !udc->vbus)
+		is_on = 0;
+
+	if (is_on != udc->pullup)
+		isp1301_pullup_enable(udc, is_on, 0);
+}
+
+/* Must be called without lock */
+static int lpc32xx_ep_disable(struct usb_ep *_ep)
+{
+	struct lpc32xx_ep *ep = container_of(_ep, struct lpc32xx_ep, ep);
+	struct lpc32xx_udc *udc = ep->udc;
+	unsigned long	flags;
+
+	if ((ep->hwep_num_base == 0) || (ep->hwep_num == 0))
+		return -EINVAL;
+	spin_lock_irqsave(&udc->lock, flags);
+
+	nuke(ep, -ESHUTDOWN);
+
+	/* restore the endpoint's pristine config */
+	ep->desc = NULL;
+
+	/* Clear all DMA statuses for this EP */
+	udc_ep_dma_disable(udc, ep->hwep_num);
+	writel(1 << ep->hwep_num, USBD_EOTINTCLR(udc->udp_baseaddr));
+	writel(1 << ep->hwep_num, USBD_NDDRTINTCLR(udc->udp_baseaddr));
+	writel(1 << ep->hwep_num, USBD_SYSERRTINTCLR(udc->udp_baseaddr));
+	writel(1 << ep->hwep_num, USBD_DMARCLR(udc->udp_baseaddr));
+
+	/* Remove the DD pointer in the UDCA */
+	udc->udca_v_base[ep->hwep_num] = 0;
+
+	/* Disable and reset endpoint and interrupt */
+	uda_clear_hwepint(udc, ep->hwep_num);
+	udc_unrealize_hwep(udc, ep->hwep_num);
+
+	ep->hwep_num = 0;
+
+	spin_unlock_irqrestore(&udc->lock, flags);
+
+	atomic_dec(&udc->enabled_ep_cnt);
+	wake_up(&udc->ep_disable_wait_queue);
+
+	return 0;
+}
+
+/* Must be called without lock */
+static int lpc32xx_ep_enable(struct usb_ep *_ep,
+			     const struct usb_endpoint_descriptor *desc)
+{
+	struct lpc32xx_ep *ep = container_of(_ep, struct lpc32xx_ep, ep);
+	struct lpc32xx_udc *udc = ep->udc;
+	u16 maxpacket;
+	u32 tmp;
+	unsigned long flags;
+
+	/* Verify EP data */
+	if ((!_ep) || (!ep) || (!desc) || (ep->desc) ||
+	    (desc->bDescriptorType != USB_DT_ENDPOINT)) {
+		dev_dbg(udc->dev, "bad ep or descriptor\n");
+		return -EINVAL;
+	}
+	maxpacket = usb_endpoint_maxp(desc);
+	if ((maxpacket == 0) || (maxpacket > ep->maxpacket)) {
+		dev_dbg(udc->dev, "bad ep descriptor's packet size\n");
+		return -EINVAL;
+	}
+
+	/* Don't touch EP0 */
+	if (ep->hwep_num_base == 0) {
+		dev_dbg(udc->dev, "Can't re-enable EP0!!!\n");
+		return -EINVAL;
+	}
+
+	/* Is driver ready? */
+	if ((!udc->driver) || (udc->gadget.speed == USB_SPEED_UNKNOWN)) {
+		dev_dbg(udc->dev, "bogus device state\n");
+		return -ESHUTDOWN;
+	}
+
+	tmp = desc->bmAttributes & USB_ENDPOINT_XFERTYPE_MASK;
+	switch (tmp) {
+	case USB_ENDPOINT_XFER_CONTROL:
+		return -EINVAL;
+
+	case USB_ENDPOINT_XFER_INT:
+		if (maxpacket > ep->maxpacket) {
+			dev_dbg(udc->dev,
+				"Bad INT endpoint maxpacket %d\n", maxpacket);
+			return -EINVAL;
+		}
+		break;
+
+	case USB_ENDPOINT_XFER_BULK:
+		switch (maxpacket) {
+		case 8:
+		case 16:
+		case 32:
+		case 64:
+			break;
+
+		default:
+			dev_dbg(udc->dev,
+				"Bad BULK endpoint maxpacket %d\n", maxpacket);
+			return -EINVAL;
+		}
+		break;
+
+	case USB_ENDPOINT_XFER_ISOC:
+		break;
+	}
+	spin_lock_irqsave(&udc->lock, flags);
+
+	/* Initialize endpoint to match the selected descriptor */
+	ep->is_in = (desc->bEndpointAddress & USB_DIR_IN) != 0;
+	ep->desc = desc;
+	ep->ep.maxpacket = maxpacket;
+
+	/* Map hardware endpoint from base and direction */
+	if (ep->is_in)
+		/* IN endpoints are offset 1 from the OUT endpoint */
+		ep->hwep_num = ep->hwep_num_base + EP_IN;
+	else
+		ep->hwep_num = ep->hwep_num_base;
+
+	ep_dbg(ep, "EP enabled: %s, HW:%d, MP:%d IN:%d\n", ep->ep.name,
+	       ep->hwep_num, maxpacket, (ep->is_in == 1));
+
+	/* Realize the endpoint, interrupt is enabled later when
+	 * buffers are queued, IN EPs will NAK until buffers are ready */
+	udc_realize_hwep(udc, ep->hwep_num, ep->ep.maxpacket);
+	udc_clr_buffer_hwep(udc, ep->hwep_num);
+	uda_disable_hwepint(udc, ep->hwep_num);
+	udc_clrstall_hwep(udc, ep->hwep_num);
+
+	/* Clear all DMA statuses for this EP */
+	udc_ep_dma_disable(udc, ep->hwep_num);
+	writel(1 << ep->hwep_num, USBD_EOTINTCLR(udc->udp_baseaddr));
+	writel(1 << ep->hwep_num, USBD_NDDRTINTCLR(udc->udp_baseaddr));
+	writel(1 << ep->hwep_num, USBD_SYSERRTINTCLR(udc->udp_baseaddr));
+	writel(1 << ep->hwep_num, USBD_DMARCLR(udc->udp_baseaddr));
+
+	spin_unlock_irqrestore(&udc->lock, flags);
+
+	atomic_inc(&udc->enabled_ep_cnt);
+	return 0;
+}
+
+/*
+ * Allocate a USB request list
+ * Can be called with or without lock
+ */
+static struct usb_request *lpc32xx_ep_alloc_request(struct usb_ep *_ep,
+						    gfp_t gfp_flags)
+{
+	struct lpc32xx_request *req;
+
+	req = kzalloc(sizeof(struct lpc32xx_request), gfp_flags);
+	if (!req)
+		return NULL;
+
+	INIT_LIST_HEAD(&req->queue);
+	return &req->req;
+}
+
+/*
+ * De-allocate a USB request list
+ * Can be called with or without lock
+ */
+static void lpc32xx_ep_free_request(struct usb_ep *_ep,
+				    struct usb_request *_req)
+{
+	struct lpc32xx_request *req;
+
+	req = container_of(_req, struct lpc32xx_request, req);
+	BUG_ON(!list_empty(&req->queue));
+	kfree(req);
+}
+
+/* Must be called without lock */
+static int lpc32xx_ep_queue(struct usb_ep *_ep,
+			    struct usb_request *_req, gfp_t gfp_flags)
+{
+	struct lpc32xx_request *req;
+	struct lpc32xx_ep *ep;
+	struct lpc32xx_udc *udc;
+	unsigned long flags;
+	int status = 0;
+
+	req = container_of(_req, struct lpc32xx_request, req);
+	ep = container_of(_ep, struct lpc32xx_ep, ep);
+
+	if (!_req || !_req->complete || !_req->buf ||
+	    !list_empty(&req->queue))
+		return -EINVAL;
+
+	udc = ep->udc;
+
+	if (!_ep || (!ep->desc && ep->hwep_num_base != 0)) {
+		dev_dbg(udc->dev, "invalid ep\n");
+		return -EINVAL;
+	}
+
+
+	if ((!udc) || (!udc->driver) ||
+	    (udc->gadget.speed == USB_SPEED_UNKNOWN)) {
+		dev_dbg(udc->dev, "invalid device\n");
+		return -EINVAL;
+	}
+
+	if (ep->lep) {
+		enum dma_data_direction direction;
+		struct lpc32xx_usbd_dd_gad *dd;
+
+		/* Map DMA pointer */
+		if (ep->is_in)
+			direction = DMA_TO_DEVICE;
+		else
+			direction = DMA_FROM_DEVICE;
+
+		if (req->req.dma == 0) {
+			req->req.dma = dma_map_single(
+				ep->udc->gadget.dev.parent,
+				req->req.buf, req->req.length, direction);
+			req->mapped = 1;
+		} else {
+			dma_sync_single_for_device(
+				ep->udc->gadget.dev.parent, req->req.dma,
+				req->req.length, direction);
+			req->mapped = 0;
+		}
+
+		/* For the request, build a list of DDs */
+		dd = udc_dd_alloc(udc);
+		if (!dd) {
+			/* Error allocating DD */
+			return -ENOMEM;
+		}
+		req->dd_desc_ptr = dd;
+
+		/* Setup the DMA descriptor */
+		dd->dd_next_phy = dd->dd_next_v = 0;
+		dd->dd_buffer_addr = req->req.dma;
+		dd->dd_status = 0;
+
+		/* Special handling for ISO EPs */
+		if (ep->eptype == EP_ISO_TYPE) {
+			dd->dd_setup = DD_SETUP_ISO_EP |
+				DD_SETUP_PACKETLEN(0) |
+				DD_SETUP_DMALENBYTES(1);
+			dd->dd_iso_ps_mem_addr = dd->this_dma + 24;
+			if (ep->is_in)
+				dd->iso_status[0] = req->req.length;
+			else
+				dd->iso_status[0] = 0;
+		} else
+			dd->dd_setup = DD_SETUP_PACKETLEN(ep->ep.maxpacket) |
+				DD_SETUP_DMALENBYTES(req->req.length);
+	}
+
+	ep_dbg(ep, "%s queue req %p len %d buf %p (in=%d) z=%d\n", _ep->name,
+	       _req, _req->length, _req->buf, ep->is_in, _req->zero);
+
+	spin_lock_irqsave(&udc->lock, flags);
+
+	_req->status = -EINPROGRESS;
+	_req->actual = 0;
+	req->send_zlp = _req->zero;
+
+	/* Kickstart empty queues */
+	if (list_empty(&ep->queue)) {
+		list_add_tail(&req->queue, &ep->queue);
+
+		if (ep->hwep_num_base == 0) {
+			/* Handle expected data direction */
+			if (ep->is_in) {
+				/* IN packet to host */
+				udc->ep0state = DATA_IN;
+				status = udc_ep0_in_req(udc);
+			} else {
+				/* OUT packet from host */
+				udc->ep0state = DATA_OUT;
+				status = udc_ep0_out_req(udc);
+			}
+		} else if (ep->is_in) {
+			/* IN packet to host and kick off transfer */
+			if (!ep->req_pending)
+				udc_ep_in_req_dma(udc, ep);
+		} else
+			/* OUT packet from host and kick off list */
+			if (!ep->req_pending)
+				udc_ep_out_req_dma(udc, ep);
+	} else
+		list_add_tail(&req->queue, &ep->queue);
+
+	spin_unlock_irqrestore(&udc->lock, flags);
+
+	return (status < 0) ? status : 0;
+}
+
+/* Must be called without lock */
+static int lpc32xx_ep_dequeue(struct usb_ep *_ep, struct usb_request *_req)
+{
+	struct lpc32xx_ep *ep;
+	struct lpc32xx_request *req;
+	unsigned long flags;
+
+	ep = container_of(_ep, struct lpc32xx_ep, ep);
+	if (!_ep || ep->hwep_num_base == 0)
+		return -EINVAL;
+
+	spin_lock_irqsave(&ep->udc->lock, flags);
+
+	/* make sure it's actually queued on this endpoint */
+	list_for_each_entry(req, &ep->queue, queue) {
+		if (&req->req == _req)
+			break;
+	}
+	if (&req->req != _req) {
+		spin_unlock_irqrestore(&ep->udc->lock, flags);
+		return -EINVAL;
+	}
+
+	done(ep, req, -ECONNRESET);
+
+	spin_unlock_irqrestore(&ep->udc->lock, flags);
+
+	return 0;
+}
+
+/* Must be called without lock */
+static int lpc32xx_ep_set_halt(struct usb_ep *_ep, int value)
+{
+	struct lpc32xx_ep *ep = container_of(_ep, struct lpc32xx_ep, ep);
+	struct lpc32xx_udc *udc = ep->udc;
+	unsigned long flags;
+
+	if ((!ep) || (ep->desc == NULL) || (ep->hwep_num <= 1))
+		return -EINVAL;
+
+	/* Don't halt an IN EP */
+	if (ep->is_in)
+		return -EAGAIN;
+
+	spin_lock_irqsave(&udc->lock, flags);
+
+	if (value == 1) {
+		/* stall */
+		udc_protocol_cmd_data_w(udc, CMD_SET_EP_STAT(ep->hwep_num),
+					DAT_WR_BYTE(EP_STAT_ST));
+	} else {
+		/* End stall */
+		ep->wedge = 0;
+		udc_protocol_cmd_data_w(udc, CMD_SET_EP_STAT(ep->hwep_num),
+					DAT_WR_BYTE(0));
+	}
+
+	spin_unlock_irqrestore(&udc->lock, flags);
+
+	return 0;
+}
+
+/* set the halt feature and ignores clear requests */
+static int lpc32xx_ep_set_wedge(struct usb_ep *_ep)
+{
+	struct lpc32xx_ep *ep = container_of(_ep, struct lpc32xx_ep, ep);
+
+	if (!_ep || !ep->udc)
+		return -EINVAL;
+
+	ep->wedge = 1;
+
+	return usb_ep_set_halt(_ep);
+}
+
+static const struct usb_ep_ops lpc32xx_ep_ops = {
+	.enable		= lpc32xx_ep_enable,
+	.disable	= lpc32xx_ep_disable,
+	.alloc_request	= lpc32xx_ep_alloc_request,
+	.free_request	= lpc32xx_ep_free_request,
+	.queue		= lpc32xx_ep_queue,
+	.dequeue	= lpc32xx_ep_dequeue,
+	.set_halt	= lpc32xx_ep_set_halt,
+	.set_wedge	= lpc32xx_ep_set_wedge,
+};
+
+/* Send a ZLP on a non-0 IN EP */
+void udc_send_in_zlp(struct lpc32xx_udc *udc, struct lpc32xx_ep *ep)
+{
+	/* Clear EP status */
+	udc_clearep_getsts(udc, ep->hwep_num);
+
+	/* Send ZLP via FIFO mechanism */
+	udc_write_hwep(udc, ep->hwep_num, NULL, 0);
+}
+
+/*
+ * Handle EP completion for ZLP
+ * This function will only be called when a delayed ZLP needs to be sent out
+ * after a DMA transfer has filled both buffers.
+ */
+void udc_handle_eps(struct lpc32xx_udc *udc, struct lpc32xx_ep *ep)
+{
+	u32 epstatus;
+	struct lpc32xx_request *req;
+
+	if (ep->hwep_num <= 0)
+		return;
+
+	uda_clear_hwepint(udc, ep->hwep_num);
+
+	/* If this interrupt isn't enabled, return now */
+	if (!(udc->enabled_hwepints & (1 << ep->hwep_num)))
+		return;
+
+	/* Get endpoint status */
+	epstatus = udc_clearep_getsts(udc, ep->hwep_num);
+
+	/*
+	 * This should never happen, but protect against writing to the
+	 * buffer when full.
+	 */
+	if (epstatus & EP_SEL_F)
+		return;
+
+	if (ep->is_in) {
+		udc_send_in_zlp(udc, ep);
+		uda_disable_hwepint(udc, ep->hwep_num);
+	} else
+		return;
+
+	/* If there isn't a request waiting, something went wrong */
+	req = list_entry(ep->queue.next, struct lpc32xx_request, queue);
+	if (req) {
+		done(ep, req, 0);
+
+		/* Start another request if ready */
+		if (!list_empty(&ep->queue)) {
+			if (ep->is_in)
+				udc_ep_in_req_dma(udc, ep);
+			else
+				udc_ep_out_req_dma(udc, ep);
+		} else
+			ep->req_pending = 0;
+	}
+}
+
+
+/* DMA end of transfer completion */
+static void udc_handle_dma_ep(struct lpc32xx_udc *udc, struct lpc32xx_ep *ep)
+{
+	u32 status, epstatus;
+	struct lpc32xx_request *req;
+	struct lpc32xx_usbd_dd_gad *dd;
+
+#ifdef CONFIG_USB_GADGET_DEBUG_FILES
+	ep->totalints++;
+#endif
+
+	req = list_entry(ep->queue.next, struct lpc32xx_request, queue);
+	if (!req) {
+		ep_err(ep, "DMA interrupt on no req!\n");
+		return;
+	}
+	dd = req->dd_desc_ptr;
+
+	/* DMA descriptor should always be retired for this call */
+	if (!(dd->dd_status & DD_STATUS_DD_RETIRED))
+		ep_warn(ep, "DMA descriptor did not retire\n");
+
+	/* Disable DMA */
+	udc_ep_dma_disable(udc, ep->hwep_num);
+	writel((1 << ep->hwep_num), USBD_EOTINTCLR(udc->udp_baseaddr));
+	writel((1 << ep->hwep_num), USBD_NDDRTINTCLR(udc->udp_baseaddr));
+
+	/* System error? */
+	if (readl(USBD_SYSERRTINTST(udc->udp_baseaddr)) &
+	    (1 << ep->hwep_num)) {
+		writel((1 << ep->hwep_num),
+			     USBD_SYSERRTINTCLR(udc->udp_baseaddr));
+		ep_err(ep, "AHB critical error!\n");
+		ep->req_pending = 0;
+
+		/* The error could have occurred on a packet of a multipacket
+		 * transfer, so recovering the transfer is not possible. Close
+		 * the request with an error */
+		done(ep, req, -ECONNABORTED);
+		return;
+	}
+
+	/* Handle the current DD's status */
+	status = dd->dd_status;
+	switch (status & DD_STATUS_STS_MASK) {
+	case DD_STATUS_STS_NS:
+		/* DD not serviced? This shouldn't happen! */
+		ep->req_pending = 0;
+		ep_err(ep, "DMA critical EP error: DD not serviced (0x%x)!\n",
+		       status);
+
+		done(ep, req, -ECONNABORTED);
+		return;
+
+	case DD_STATUS_STS_BS:
+		/* Interrupt only fires on EOT - This shouldn't happen! */
+		ep->req_pending = 0;
+		ep_err(ep, "DMA critical EP error: EOT prior to service completion (0x%x)!\n",
+		       status);
+		done(ep, req, -ECONNABORTED);
+		return;
+
+	case DD_STATUS_STS_NC:
+	case DD_STATUS_STS_DUR:
+		/* Really just a short packet, not an underrun */
+		/* This is a good status and what we expect */
+		break;
+
+	default:
+		/* Data overrun, system error, or unknown */
+		ep->req_pending = 0;
+		ep_err(ep, "DMA critical EP error: System error (0x%x)!\n",
+		       status);
+		done(ep, req, -ECONNABORTED);
+		return;
+	}
+
+	/* ISO endpoints are handled differently */
+	if (ep->eptype == EP_ISO_TYPE) {
+		if (ep->is_in)
+			req->req.actual = req->req.length;
+		else
+			req->req.actual = dd->iso_status[0] & 0xFFFF;
+	} else
+		req->req.actual += DD_STATUS_CURDMACNT(status);
+
+	/* Send a ZLP if necessary. This will be done for non-int
+	 * packets which have a size that is a divisor of MAXP */
+	if (req->send_zlp) {
+		/*
+		 * If at least 1 buffer is available, send the ZLP now.
+		 * Otherwise, the ZLP send needs to be deferred until a
+		 * buffer is available.
+		 */
+		if (udc_clearep_getsts(udc, ep->hwep_num) & EP_SEL_F) {
+			udc_clearep_getsts(udc, ep->hwep_num);
+			uda_enable_hwepint(udc, ep->hwep_num);
+			epstatus = udc_clearep_getsts(udc, ep->hwep_num);
+
+			/* Let the EP interrupt handle the ZLP */
+			return;
+		} else
+			udc_send_in_zlp(udc, ep);
+	}
+
+	/* Transfer request is complete */
+	done(ep, req, 0);
+
+	/* Start another request if ready */
+	udc_clearep_getsts(udc, ep->hwep_num);
+	if (!list_empty((&ep->queue))) {
+		if (ep->is_in)
+			udc_ep_in_req_dma(udc, ep);
+		else
+			udc_ep_out_req_dma(udc, ep);
+	} else
+		ep->req_pending = 0;
+
+}
+
+/*
+ *
+ * Endpoint 0 functions
+ *
+ */
+static void udc_handle_dev(struct lpc32xx_udc *udc)
+{
+	u32 tmp;
+
+	udc_protocol_cmd_w(udc, CMD_GET_DEV_STAT);
+	tmp = udc_protocol_cmd_r(udc, DAT_GET_DEV_STAT);
+
+	if (tmp & DEV_RST)
+		uda_usb_reset(udc);
+	else if (tmp & DEV_CON_CH)
+		uda_power_event(udc, (tmp & DEV_CON));
+	else if (tmp & DEV_SUS_CH) {
+		if (tmp & DEV_SUS) {
+			if (udc->vbus == 0)
+				stop_activity(udc);
+			else if ((udc->gadget.speed != USB_SPEED_UNKNOWN) &&
+				 udc->driver) {
+				/* Power down transceiver */
+				udc->poweron = 0;
+				schedule_work(&udc->pullup_job);
+				uda_resm_susp_event(udc, 1);
+			}
+		} else if ((udc->gadget.speed != USB_SPEED_UNKNOWN) &&
+			   udc->driver && udc->vbus) {
+			uda_resm_susp_event(udc, 0);
+			/* Power up transceiver */
+			udc->poweron = 1;
+			schedule_work(&udc->pullup_job);
+		}
+	}
+}
+
+static int udc_get_status(struct lpc32xx_udc *udc, u16 reqtype, u16 wIndex)
+{
+	struct lpc32xx_ep *ep;
+	u32 ep0buff = 0, tmp;
+
+	switch (reqtype & USB_RECIP_MASK) {
+	case USB_RECIP_INTERFACE:
+		break; /* Not supported */
+
+	case USB_RECIP_DEVICE:
+		ep0buff = (udc->selfpowered << USB_DEVICE_SELF_POWERED);
+		if (udc->dev_status & (1 << USB_DEVICE_REMOTE_WAKEUP))
+			ep0buff |= (1 << USB_DEVICE_REMOTE_WAKEUP);
+		break;
+
+	case USB_RECIP_ENDPOINT:
+		tmp = wIndex & USB_ENDPOINT_NUMBER_MASK;
+		ep = &udc->ep[tmp];
+		if ((tmp == 0) || (tmp >= NUM_ENDPOINTS) || (tmp && !ep->desc))
+			return -EOPNOTSUPP;
+
+		if (wIndex & USB_DIR_IN) {
+			if (!ep->is_in)
+				return -EOPNOTSUPP; /* Something's wrong */
+		} else if (ep->is_in)
+			return -EOPNOTSUPP; /* Not an IN endpoint */
+
+		/* Get status of the endpoint */
+		udc_protocol_cmd_w(udc, CMD_SEL_EP(ep->hwep_num));
+		tmp = udc_protocol_cmd_r(udc, DAT_SEL_EP(ep->hwep_num));
+
+		if (tmp & EP_SEL_ST)
+			ep0buff = (1 << USB_ENDPOINT_HALT);
+		else
+			ep0buff = 0;
+		break;
+
+	default:
+		break;
+	}
+
+	/* Return data */
+	udc_write_hwep(udc, EP_IN, &ep0buff, 2);
+
+	return 0;
+}
+
+static void udc_handle_ep0_setup(struct lpc32xx_udc *udc)
+{
+	struct lpc32xx_ep *ep, *ep0 = &udc->ep[0];
+	struct usb_ctrlrequest ctrlpkt;
+	int i, bytes;
+	u16 wIndex, wValue, wLength, reqtype, req, tmp;
+
+	/* Nuke previous transfers */
+	nuke(ep0, -EPROTO);
+
+	/* Get setup packet */
+	bytes = udc_read_hwep(udc, EP_OUT, (u32 *) &ctrlpkt, 8);
+	if (bytes != 8) {
+		ep_warn(ep0, "Incorrectly sized setup packet (s/b 8, is %d)!\n",
+			bytes);
+		return;
+	}
+
+	/* Native endianness */
+	wIndex = le16_to_cpu(ctrlpkt.wIndex);
+	wValue = le16_to_cpu(ctrlpkt.wValue);
+	wLength = le16_to_cpu(ctrlpkt.wLength);
+	reqtype = le16_to_cpu(ctrlpkt.bRequestType);
+
+	/* Set direction of EP0 */
+	if (likely(reqtype & USB_DIR_IN))
+		ep0->is_in = 1;
+	else
+		ep0->is_in = 0;
+
+	/* Handle SETUP packet */
+	req = le16_to_cpu(ctrlpkt.bRequest);
+	switch (req) {
+	case USB_REQ_CLEAR_FEATURE:
+	case USB_REQ_SET_FEATURE:
+		switch (reqtype) {
+		case (USB_TYPE_STANDARD | USB_RECIP_DEVICE):
+			if (wValue != USB_DEVICE_REMOTE_WAKEUP)
+				goto stall; /* Nothing else handled */
+
+			/* Tell board about event */
+			if (req == USB_REQ_CLEAR_FEATURE)
+				udc->dev_status &=
+					~(1 << USB_DEVICE_REMOTE_WAKEUP);
+			else
+				udc->dev_status |=
+					(1 << USB_DEVICE_REMOTE_WAKEUP);
+			uda_remwkp_cgh(udc);
+			goto zlp_send;
+
+		case (USB_TYPE_STANDARD | USB_RECIP_ENDPOINT):
+			tmp = wIndex & USB_ENDPOINT_NUMBER_MASK;
+			if ((wValue != USB_ENDPOINT_HALT) ||
+			    (tmp >= NUM_ENDPOINTS))
+				break;
+
+			/* Find hardware endpoint from logical endpoint */
+			ep = &udc->ep[tmp];
+			tmp = ep->hwep_num;
+			if (tmp == 0)
+				break;
+
+			if (req == USB_REQ_SET_FEATURE)
+				udc_stall_hwep(udc, tmp);
+			else if (!ep->wedge)
+				udc_clrstall_hwep(udc, tmp);
+
+			goto zlp_send;
+
+		default:
+			break;
+		}
+
+
+	case USB_REQ_SET_ADDRESS:
+		if (reqtype == (USB_TYPE_STANDARD | USB_RECIP_DEVICE)) {
+			udc_set_address(udc, wValue);
+			goto zlp_send;
+		}
+		break;
+
+	case USB_REQ_GET_STATUS:
+		udc_get_status(udc, reqtype, wIndex);
+		return;
+
+	default:
+		break; /* Let GadgetFS handle the descriptor instead */
+	}
+
+	if (likely(udc->driver)) {
+		/* device-2-host (IN) or no data setup command, process
+		 * immediately */
+		spin_unlock(&udc->lock);
+		i = udc->driver->setup(&udc->gadget, &ctrlpkt);
+
+		spin_lock(&udc->lock);
+		if (req == USB_REQ_SET_CONFIGURATION) {
+			/* Configuration is set after endpoints are realized */
+			if (wValue) {
+				/* Set configuration */
+				udc_set_device_configured(udc);
+
+				udc_protocol_cmd_data_w(udc, CMD_SET_MODE,
+							DAT_WR_BYTE(AP_CLK |
+							INAK_BI | INAK_II));
+			} else {
+				/* Clear configuration */
+				udc_set_device_unconfigured(udc);
+
+				/* Disable NAK interrupts */
+				udc_protocol_cmd_data_w(udc, CMD_SET_MODE,
+							DAT_WR_BYTE(AP_CLK));
+			}
+		}
+
+		if (i < 0) {
+			/* setup processing failed, force stall */
+			dev_err(udc->dev,
+				"req %02x.%02x protocol STALL; stat %d\n",
+				reqtype, req, i);
+			udc->ep0state = WAIT_FOR_SETUP;
+			goto stall;
+		}
+	}
+
+	if (!ep0->is_in)
+		udc_ep0_send_zlp(udc); /* ZLP IN packet on data phase */
+
+	return;
+
+stall:
+	udc_stall_hwep(udc, EP_IN);
+	return;
+
+zlp_send:
+	udc_ep0_send_zlp(udc);
+	return;
+}
+
+/* IN endpoint 0 transfer */
+static void udc_handle_ep0_in(struct lpc32xx_udc *udc)
+{
+	struct lpc32xx_ep *ep0 = &udc->ep[0];
+	u32 epstatus;
+
+	/* Clear EP interrupt */
+	epstatus = udc_clearep_getsts(udc, EP_IN);
+
+#ifdef CONFIG_USB_GADGET_DEBUG_FILES
+	ep0->totalints++;
+#endif
+
+	/* Stalled? Clear stall and reset buffers */
+	if (epstatus & EP_SEL_ST) {
+		udc_clrstall_hwep(udc, EP_IN);
+		nuke(ep0, -ECONNABORTED);
+		udc->ep0state = WAIT_FOR_SETUP;
+		return;
+	}
+
+	/* Is a buffer available? */
+	if (!(epstatus & EP_SEL_F)) {
+		/* Handle based on current state */
+		if (udc->ep0state == DATA_IN)
+			udc_ep0_in_req(udc);
+		else {
+			/* Unknown state for EP0 oe end of DATA IN phase */
+			nuke(ep0, -ECONNABORTED);
+			udc->ep0state = WAIT_FOR_SETUP;
+		}
+	}
+}
+
+/* OUT endpoint 0 transfer */
+static void udc_handle_ep0_out(struct lpc32xx_udc *udc)
+{
+	struct lpc32xx_ep *ep0 = &udc->ep[0];
+	u32 epstatus;
+
+	/* Clear EP interrupt */
+	epstatus = udc_clearep_getsts(udc, EP_OUT);
+
+
+#ifdef CONFIG_USB_GADGET_DEBUG_FILES
+	ep0->totalints++;
+#endif
+
+	/* Stalled? */
+	if (epstatus & EP_SEL_ST) {
+		udc_clrstall_hwep(udc, EP_OUT);
+		nuke(ep0, -ECONNABORTED);
+		udc->ep0state = WAIT_FOR_SETUP;
+		return;
+	}
+
+	/* A NAK may occur if a packet couldn't be received yet */
+	if (epstatus & EP_SEL_EPN)
+		return;
+	/* Setup packet incoming? */
+	if (epstatus & EP_SEL_STP) {
+		nuke(ep0, 0);
+		udc->ep0state = WAIT_FOR_SETUP;
+	}
+
+	/* Data available? */
+	if (epstatus & EP_SEL_F)
+		/* Handle based on current state */
+		switch (udc->ep0state) {
+		case WAIT_FOR_SETUP:
+			udc_handle_ep0_setup(udc);
+			break;
+
+		case DATA_OUT:
+			udc_ep0_out_req(udc);
+			break;
+
+		default:
+			/* Unknown state for EP0 */
+			nuke(ep0, -ECONNABORTED);
+			udc->ep0state = WAIT_FOR_SETUP;
+		}
+}
+
+/* Must be called without lock */
+static int lpc32xx_get_frame(struct usb_gadget *gadget)
+{
+	int frame;
+	unsigned long flags;
+	struct lpc32xx_udc *udc = to_udc(gadget);
+
+	if (!udc->clocked)
+		return -EINVAL;
+
+	spin_lock_irqsave(&udc->lock, flags);
+
+	frame = (int) udc_get_current_frame(udc);
+
+	spin_unlock_irqrestore(&udc->lock, flags);
+
+	return frame;
+}
+
+static int lpc32xx_wakeup(struct usb_gadget *gadget)
+{
+	return -ENOTSUPP;
+}
+
+static int lpc32xx_set_selfpowered(struct usb_gadget *gadget, int is_on)
+{
+	struct lpc32xx_udc *udc = to_udc(gadget);
+
+	/* Always self-powered */
+	udc->selfpowered = (is_on != 0);
+
+	return 0;
+}
+
+/*
+ * vbus is here!  turn everything on that's ready
+ * Must be called without lock
+ */
+static int lpc32xx_vbus_session(struct usb_gadget *gadget, int is_active)
+{
+	unsigned long flags;
+	struct lpc32xx_udc *udc = to_udc(gadget);
+
+	spin_lock_irqsave(&udc->lock, flags);
+
+	/* Doesn't need lock */
+	if (udc->driver) {
+		udc_clk_set(udc, 1);
+		udc_enable(udc);
+		pullup(udc, is_active);
+	} else {
+		stop_activity(udc);
+		pullup(udc, 0);
+
+		spin_unlock_irqrestore(&udc->lock, flags);
+		/*
+		 *  Wait for all the endpoints to disable,
+		 *  before disabling clocks. Don't wait if
+		 *  endpoints are not enabled.
+		 */
+		if (atomic_read(&udc->enabled_ep_cnt))
+			wait_event_interruptible(udc->ep_disable_wait_queue,
+				 (atomic_read(&udc->enabled_ep_cnt) == 0));
+
+		spin_lock_irqsave(&udc->lock, flags);
+
+		udc_clk_set(udc, 0);
+	}
+
+	spin_unlock_irqrestore(&udc->lock, flags);
+
+	return 0;
+}
+
+/* Can be called with or without lock */
+static int lpc32xx_pullup(struct usb_gadget *gadget, int is_on)
+{
+	struct lpc32xx_udc *udc = to_udc(gadget);
+
+	/* Doesn't need lock */
+	pullup(udc, is_on);
+
+	return 0;
+}
+
+static int lpc32xx_start(struct usb_gadget_driver *driver,
+			 int (*bind)(struct usb_gadget *));
+static int lpc32xx_stop(struct usb_gadget_driver *driver);
+
+static const struct usb_gadget_ops lpc32xx_udc_ops = {
+	.get_frame		= lpc32xx_get_frame,
+	.wakeup			= lpc32xx_wakeup,
+	.set_selfpowered	= lpc32xx_set_selfpowered,
+	.vbus_session		= lpc32xx_vbus_session,
+	.pullup			= lpc32xx_pullup,
+	.start			= lpc32xx_start,
+	.stop			= lpc32xx_stop,
+};
+
+static void nop_release(struct device *dev)
+{
+	/* nothing to free */
+}
+
+static struct lpc32xx_udc controller = {
+	.gadget = {
+		.ops	= &lpc32xx_udc_ops,
+		.ep0	= &controller.ep[0].ep,
+		.name	= driver_name,
+		.dev	= {
+			.init_name = "gadget",
+			.release = nop_release,
+		}
+	},
+	.ep[0] = {
+		.ep = {
+			.name	= "ep0",
+			.ops	= &lpc32xx_ep_ops,
+		},
+		.udc		= &controller,
+		.maxpacket	= 64,
+		.hwep_num_base	= 0,
+		.hwep_num	= 0, /* Can be 0 or 1, has special handling */
+		.lep		= 0,
+		.eptype		= EP_CTL_TYPE,
+	},
+	.ep[1] = {
+		.ep = {
+			.name	= "ep1-int",
+			.ops	= &lpc32xx_ep_ops,
+		},
+		.udc		= &controller,
+		.maxpacket	= 64,
+		.hwep_num_base	= 2,
+		.hwep_num	= 0, /* 2 or 3, will be set later */
+		.lep		= 1,
+		.eptype		= EP_INT_TYPE,
+	},
+	.ep[2] = {
+		.ep = {
+			.name	= "ep2-bulk",
+			.ops	= &lpc32xx_ep_ops,
+		},
+		.udc		= &controller,
+		.maxpacket	= 64,
+		.hwep_num_base	= 4,
+		.hwep_num	= 0, /* 4 or 5, will be set later */
+		.lep		= 2,
+		.eptype		= EP_BLK_TYPE,
+	},
+	.ep[3] = {
+		.ep = {
+			.name	= "ep3-iso",
+			.ops	= &lpc32xx_ep_ops,
+		},
+		.udc		= &controller,
+		.maxpacket	= 1023,
+		.hwep_num_base	= 6,
+		.hwep_num	= 0, /* 6 or 7, will be set later */
+		.lep		= 3,
+		.eptype		= EP_ISO_TYPE,
+	},
+	.ep[4] = {
+		.ep = {
+			.name	= "ep4-int",
+			.ops	= &lpc32xx_ep_ops,
+		},
+		.udc		= &controller,
+		.maxpacket	= 64,
+		.hwep_num_base	= 8,
+		.hwep_num	= 0, /* 8 or 9, will be set later */
+		.lep		= 4,
+		.eptype		= EP_INT_TYPE,
+	},
+	.ep[5] = {
+		.ep = {
+			.name	= "ep5-bulk",
+			.ops	= &lpc32xx_ep_ops,
+		},
+		.udc		= &controller,
+		.maxpacket	= 64,
+		.hwep_num_base	= 10,
+		.hwep_num	= 0, /* 10 or 11, will be set later */
+		.lep		= 5,
+		.eptype		= EP_BLK_TYPE,
+	},
+	.ep[6] = {
+		.ep = {
+			.name	= "ep6-iso",
+			.ops	= &lpc32xx_ep_ops,
+		},
+		.udc		= &controller,
+		.maxpacket	= 1023,
+		.hwep_num_base	= 12,
+		.hwep_num	= 0, /* 12 or 13, will be set later */
+		.lep		= 6,
+		.eptype		= EP_ISO_TYPE,
+	},
+	.ep[7] = {
+		.ep = {
+			.name	= "ep7-int",
+			.ops	= &lpc32xx_ep_ops,
+		},
+		.udc		= &controller,
+		.maxpacket	= 64,
+		.hwep_num_base	= 14,
+		.hwep_num	= 0,
+		.lep		= 7,
+		.eptype		= EP_INT_TYPE,
+	},
+	.ep[8] = {
+		.ep = {
+			.name	= "ep8-bulk",
+			.ops	= &lpc32xx_ep_ops,
+		},
+		.udc		= &controller,
+		.maxpacket	= 64,
+		.hwep_num_base	= 16,
+		.hwep_num	= 0,
+		.lep		= 8,
+		.eptype		= EP_BLK_TYPE,
+	},
+	.ep[9] = {
+		.ep = {
+			.name	= "ep9-iso",
+			.ops	= &lpc32xx_ep_ops,
+		},
+		.udc		= &controller,
+		.maxpacket	= 1023,
+		.hwep_num_base	= 18,
+		.hwep_num	= 0,
+		.lep		= 9,
+		.eptype		= EP_ISO_TYPE,
+	},
+	.ep[10] = {
+		.ep = {
+			.name	= "ep10-int",
+			.ops	= &lpc32xx_ep_ops,
+		},
+		.udc		= &controller,
+		.maxpacket	= 64,
+		.hwep_num_base	= 20,
+		.hwep_num	= 0,
+		.lep		= 10,
+		.eptype		= EP_INT_TYPE,
+	},
+	.ep[11] = {
+		.ep = {
+			.name	= "ep11-bulk",
+			.ops	= &lpc32xx_ep_ops,
+		},
+		.udc		= &controller,
+		.maxpacket	= 64,
+		.hwep_num_base	= 22,
+		.hwep_num	= 0,
+		.lep		= 11,
+		.eptype		= EP_BLK_TYPE,
+	},
+	.ep[12] = {
+		.ep = {
+			.name	= "ep12-iso",
+			.ops	= &lpc32xx_ep_ops,
+		},
+		.udc		= &controller,
+		.maxpacket	= 1023,
+		.hwep_num_base	= 24,
+		.hwep_num	= 0,
+		.lep		= 12,
+		.eptype		= EP_ISO_TYPE,
+	},
+	.ep[13] = {
+		.ep = {
+			.name	= "ep13-int",
+			.ops	= &lpc32xx_ep_ops,
+		},
+		.udc		= &controller,
+		.maxpacket	= 64,
+		.hwep_num_base	= 26,
+		.hwep_num	= 0,
+		.lep		= 13,
+		.eptype		= EP_INT_TYPE,
+	},
+	.ep[14] = {
+		.ep = {
+			.name	= "ep14-bulk",
+			.ops	= &lpc32xx_ep_ops,
+		},
+		.udc		= &controller,
+		.maxpacket	= 64,
+		.hwep_num_base	= 28,
+		.hwep_num	= 0,
+		.lep		= 14,
+		.eptype		= EP_BLK_TYPE,
+	},
+	.ep[15] = {
+		.ep = {
+			.name	= "ep15-bulk",
+			.ops	= &lpc32xx_ep_ops,
+		},
+		.udc		= &controller,
+		.maxpacket	= 1023,
+		.hwep_num_base	= 30,
+		.hwep_num	= 0,
+		.lep		= 15,
+		.eptype		= EP_BLK_TYPE,
+	},
+};
+
+/* ISO and status interrupts */
+static irqreturn_t lpc32xx_usb_lp_irq(int irq, void *_udc)
+{
+	u32 tmp, devstat;
+	struct lpc32xx_udc *udc = _udc;
+
+	spin_lock(&udc->lock);
+
+	/* Read the device status register */
+	devstat = readl(USBD_DEVINTST(udc->udp_baseaddr));
+
+	devstat &= ~USBD_EP_FAST;
+	writel(devstat, USBD_DEVINTCLR(udc->udp_baseaddr));
+	devstat = devstat & udc->enabled_devints;
+
+	/* Device specific handling needed? */
+	if (devstat & USBD_DEV_STAT)
+		udc_handle_dev(udc);
+
+	/* Start of frame? (devstat & FRAME_INT):
+	 * The frame interrupt isn't really needed for ISO support,
+	 * as the driver will queue the necessary packets */
+
+	/* Error? */
+	if (devstat & ERR_INT) {
+		/* All types of errors, from cable removal during transfer to
+		 * misc protocol and bit errors. These are mostly for just info,
+		 * as the USB hardware will work around these. If these errors
+		 * happen alot, something is wrong. */
+		udc_protocol_cmd_w(udc, CMD_RD_ERR_STAT);
+		tmp = udc_protocol_cmd_r(udc, DAT_RD_ERR_STAT);
+		dev_dbg(udc->dev, "Device error (0x%x)!\n", tmp);
+	}
+
+	spin_unlock(&udc->lock);
+
+	return IRQ_HANDLED;
+}
+
+/* EP interrupts */
+static irqreturn_t lpc32xx_usb_hp_irq(int irq, void *_udc)
+{
+	u32 tmp;
+	struct lpc32xx_udc *udc = _udc;
+
+	spin_lock(&udc->lock);
+
+	/* Read the device status register */
+	writel(USBD_EP_FAST, USBD_DEVINTCLR(udc->udp_baseaddr));
+
+	/* Endpoints */
+	tmp = readl(USBD_EPINTST(udc->udp_baseaddr));
+
+	/* Special handling for EP0 */
+	if (tmp & (EP_MASK_SEL(0, EP_OUT) | EP_MASK_SEL(0, EP_IN))) {
+		/* Handle EP0 IN */
+		if (tmp & (EP_MASK_SEL(0, EP_IN)))
+			udc_handle_ep0_in(udc);
+
+		/* Handle EP0 OUT */
+		if (tmp & (EP_MASK_SEL(0, EP_OUT)))
+			udc_handle_ep0_out(udc);
+	}
+
+	/* All other EPs */
+	if (tmp & ~(EP_MASK_SEL(0, EP_OUT) | EP_MASK_SEL(0, EP_IN))) {
+		int i;
+
+		/* Handle other EP interrupts */
+		for (i = 1; i < NUM_ENDPOINTS; i++) {
+			if (tmp & (1 << udc->ep[i].hwep_num))
+				udc_handle_eps(udc, &udc->ep[i]);
+		}
+	}
+
+	spin_unlock(&udc->lock);
+
+	return IRQ_HANDLED;
+}
+
+static irqreturn_t lpc32xx_usb_devdma_irq(int irq, void *_udc)
+{
+	struct lpc32xx_udc *udc = _udc;
+
+	int i;
+	u32 tmp;
+
+	spin_lock(&udc->lock);
+
+	/* Handle EP DMA EOT interrupts */
+	tmp = readl(USBD_EOTINTST(udc->udp_baseaddr)) |
+		(readl(USBD_EPDMAST(udc->udp_baseaddr)) &
+		 readl(USBD_NDDRTINTST(udc->udp_baseaddr))) |
+		readl(USBD_SYSERRTINTST(udc->udp_baseaddr));
+	for (i = 1; i < NUM_ENDPOINTS; i++) {
+		if (tmp & (1 << udc->ep[i].hwep_num))
+			udc_handle_dma_ep(udc, &udc->ep[i]);
+	}
+
+	spin_unlock(&udc->lock);
+
+	return IRQ_HANDLED;
+}
+
+/*
+ *
+ * VBUS detection, pullup handler, and Gadget cable state notification
+ *
+ */
+static void vbus_work(struct work_struct *work)
+{
+	u8 value;
+	struct lpc32xx_udc *udc = container_of(work, struct lpc32xx_udc,
+					       vbus_job);
+
+	if (udc->enabled != 0) {
+		/* Discharge VBUS real quick */
+		i2c_smbus_write_byte_data(udc->isp1301_i2c_client,
+			ISP1301_I2C_OTG_CONTROL_1, OTG1_VBUS_DISCHRG);
+
+		/* Give VBUS some time (100mS) to discharge */
+		msleep(100);
+
+		/* Disable VBUS discharge resistor */
+		i2c_smbus_write_byte_data(udc->isp1301_i2c_client,
+			ISP1301_I2C_OTG_CONTROL_1 | ISP1301_I2C_REG_CLEAR_ADDR,
+			OTG1_VBUS_DISCHRG);
+
+		/* Clear interrupt */
+		i2c_smbus_write_byte_data(udc->isp1301_i2c_client,
+			ISP1301_I2C_INTERRUPT_LATCH |
+			ISP1301_I2C_REG_CLEAR_ADDR, ~0);
+
+		/* Get the VBUS status from the transceiver */
+		value = i2c_smbus_read_byte_data(udc->isp1301_i2c_client,
+						 ISP1301_I2C_OTG_CONTROL_2);
+
+		/* VBUS on or off? */
+		if (value & OTG_B_SESS_VLD)
+			udc->vbus = 1;
+		else
+			udc->vbus = 0;
+
+		/* VBUS changed? */
+		if (udc->last_vbus != udc->vbus) {
+			udc->last_vbus = udc->vbus;
+			lpc32xx_vbus_session(&udc->gadget, udc->vbus);
+		}
+	}
+
+	/* Re-enable after completion */
+	enable_irq(udc->udp_irq[IRQ_USB_ATX]);
+}
+
+static irqreturn_t lpc32xx_usb_vbus_irq(int irq, void *_udc)
+{
+	struct lpc32xx_udc *udc = _udc;
+
+	/* Defer handling of VBUS IRQ to work queue */
+	disable_irq_nosync(udc->udp_irq[IRQ_USB_ATX]);
+	schedule_work(&udc->vbus_job);
+
+	return IRQ_HANDLED;
+}
+
+static int lpc32xx_start(struct usb_gadget_driver *driver,
+			 int (*bind)(struct usb_gadget *))
+{
+	struct lpc32xx_udc *udc = &controller;
+	int retval, i;
+
+	if (!driver || driver->max_speed < USB_SPEED_FULL ||
+	    !bind || !driver->setup) {
+		dev_err(udc->dev, "bad parameter.\n");
+		return -EINVAL;
+	}
+
+	if (udc->driver) {
+		dev_err(udc->dev, "UDC already has a gadget driver\n");
+		return -EBUSY;
+	}
+
+	udc->driver = driver;
+	udc->gadget.dev.driver = &driver->driver;
+	udc->enabled = 1;
+	udc->selfpowered = 1;
+	udc->vbus = 0;
+
+	retval = bind(&udc->gadget);
+	if (retval) {
+		dev_err(udc->dev, "bind() returned %d\n", retval);
+		udc->enabled = 0;
+		udc->selfpowered = 0;
+		udc->driver = NULL;
+		udc->gadget.dev.driver = NULL;
+		return retval;
+	}
+
+	dev_dbg(udc->dev, "bound to %s\n", driver->driver.name);
+
+	/* Force VBUS process once to check for cable insertion */
+	udc->last_vbus = udc->vbus = 0;
+	schedule_work(&udc->vbus_job);
+
+	/* Do not re-enable ATX IRQ (3) */
+	for (i = IRQ_USB_LP; i < IRQ_USB_ATX; i++)
+		enable_irq(udc->udp_irq[i]);
+
+	return 0;
+}
+
+static int lpc32xx_stop(struct usb_gadget_driver *driver)
+{
+	int i;
+	struct lpc32xx_udc *udc = &controller;
+
+	if (!driver || driver != udc->driver || !driver->unbind)
+		return -EINVAL;
+
+	/* Disable USB pullup */
+	isp1301_pullup_enable(udc, 0, 1);
+
+	for (i = IRQ_USB_LP; i <= IRQ_USB_ATX; i++)
+		disable_irq(udc->udp_irq[i]);
+
+	if (udc->clocked) {
+
+		spin_lock(&udc->lock);
+		stop_activity(udc);
+		spin_unlock(&udc->lock);
+
+		/*
+		 *  Wait for all the endpoints to disable,
+		 *  before disabling clocks. Don't wait if
+		 *  endpoints are not enabled.
+		 */
+		if (atomic_read(&udc->enabled_ep_cnt))
+			wait_event_interruptible(udc->ep_disable_wait_queue,
+				(atomic_read(&udc->enabled_ep_cnt) == 0));
+
+		spin_lock(&udc->lock);
+		udc_clk_set(udc, 0);
+		spin_unlock(&udc->lock);
+	}
+
+	udc->enabled = 0;
+	pullup(udc, 0);
+
+	driver->unbind(&udc->gadget);
+	udc->gadget.dev.driver = NULL;
+	udc->driver = NULL;
+
+	dev_dbg(udc->dev, "unbound from %s\n", driver->driver.name);
+	return 0;
+}
+
+static void lpc32xx_udc_shutdown(struct platform_device *dev)
+{
+	/* Force disconnect on reboot */
+	struct lpc32xx_udc *udc = &controller;
+
+	pullup(udc, 0);
+}
+
+/*
+ * Callbacks to be overridden by options passed via OF (TODO)
+ */
+
+static void lpc32xx_usbd_conn_chg(int conn)
+{
+	/* Do nothing, it might be nice to enable an LED
+	 * based on conn state being !0 */
+}
+
+static void lpc32xx_usbd_susp_chg(int susp)
+{
+	/* Device suspend if susp != 0 */
+}
+
+static void lpc32xx_rmwkup_chg(int remote_wakup_enable)
+{
+	/* Enable or disable USB remote wakeup */
+}
+
+struct lpc32xx_usbd_cfg lpc32xx_usbddata = {
+	.vbus_drv_pol = 0,
+	.conn_chgb = &lpc32xx_usbd_conn_chg,
+	.susp_chgb = &lpc32xx_usbd_susp_chg,
+	.rmwk_chgb = &lpc32xx_rmwkup_chg,
+};
+
+
+static u64 lpc32xx_usbd_dmamask = ~(u32) 0x7F;
+
+static int __init lpc32xx_udc_probe(struct platform_device *pdev)
+{
+	struct device *dev = &pdev->dev;
+	struct lpc32xx_udc *udc = &controller;
+	int retval, i;
+	struct resource *res;
+	dma_addr_t dma_handle;
+	struct device_node *isp1301_node;
+
+	/* init software state */
+	udc->gadget.dev.parent = dev;
+	udc->pdev = pdev;
+	udc->dev = &pdev->dev;
+	udc->enabled = 0;
+
+	if (pdev->dev.of_node) {
+		isp1301_node = of_parse_phandle(pdev->dev.of_node,
+						"transceiver", 0);
+	} else {
+		isp1301_node = NULL;
+	}
+
+	udc->isp1301_i2c_client = isp1301_get_client(isp1301_node);
+	if (!udc->isp1301_i2c_client)
+		return -EPROBE_DEFER;
+
+	dev_info(udc->dev, "ISP1301 I2C device at address 0x%x\n",
+		 udc->isp1301_i2c_client->addr);
+
+	pdev->dev.dma_mask = &lpc32xx_usbd_dmamask;
+	pdev->dev.coherent_dma_mask = DMA_BIT_MASK(32);
+
+	udc->board = &lpc32xx_usbddata;
+
+	/*
+	 * Resources are mapped as follows:
+	 *  IORESOURCE_MEM, base address and size of USB space
+	 *  IORESOURCE_IRQ, USB device low priority interrupt number
+	 *  IORESOURCE_IRQ, USB device high priority interrupt number
+	 *  IORESOURCE_IRQ, USB device interrupt number
+	 *  IORESOURCE_IRQ, USB transceiver interrupt number
+	 */
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	if (!res)
+		return -ENXIO;
+
+	spin_lock_init(&udc->lock);
+
+	/* Get IRQs */
+	for (i = 0; i < 4; i++) {
+		udc->udp_irq[i] = platform_get_irq(pdev, i);
+		if (udc->udp_irq[i] < 0) {
+			dev_err(udc->dev,
+				"irq resource %d not available!\n", i);
+			return udc->udp_irq[i];
+		}
+	}
+
+	udc->io_p_start = res->start;
+	udc->io_p_size = resource_size(res);
+	if (!request_mem_region(udc->io_p_start, udc->io_p_size, driver_name)) {
+		dev_err(udc->dev, "someone's using UDC memory\n");
+		return -EBUSY;
+	}
+
+	udc->udp_baseaddr = ioremap(udc->io_p_start, udc->io_p_size);
+	if (!udc->udp_baseaddr) {
+		retval = -ENOMEM;
+		dev_err(udc->dev, "IO map failure\n");
+		goto io_map_fail;
+	}
+
+	/* Enable AHB slave USB clock, needed for further USB clock control */
+	writel(USB_SLAVE_HCLK_EN | (1 << 19), USB_CTRL);
+
+	/* Get required clocks */
+	udc->usb_pll_clk = clk_get(&pdev->dev, "ck_pll5");
+	if (IS_ERR(udc->usb_pll_clk)) {
+		dev_err(udc->dev, "failed to acquire USB PLL\n");
+		retval = PTR_ERR(udc->usb_pll_clk);
+		goto pll_get_fail;
+	}
+	udc->usb_slv_clk = clk_get(&pdev->dev, "ck_usbd");
+	if (IS_ERR(udc->usb_slv_clk)) {
+		dev_err(udc->dev, "failed to acquire USB device clock\n");
+		retval = PTR_ERR(udc->usb_slv_clk);
+		goto usb_clk_get_fail;
+	}
+
+	/* Setup PLL clock to 48MHz */
+	retval = clk_enable(udc->usb_pll_clk);
+	if (retval < 0) {
+		dev_err(udc->dev, "failed to start USB PLL\n");
+		goto pll_enable_fail;
+	}
+
+	retval = clk_set_rate(udc->usb_pll_clk, 48000);
+	if (retval < 0) {
+		dev_err(udc->dev, "failed to set USB clock rate\n");
+		goto pll_set_fail;
+	}
+
+	writel(readl(USB_CTRL) | USB_DEV_NEED_CLK_EN, USB_CTRL);
+
+	/* Enable USB device clock */
+	retval = clk_enable(udc->usb_slv_clk);
+	if (retval < 0) {
+		dev_err(udc->dev, "failed to start USB device clock\n");
+		goto usb_clk_enable_fail;
+	}
+
+	/* Set to enable all needed USB OTG clocks */
+	writel(USB_CLOCK_MASK, USB_OTG_CLK_CTRL(udc));
+
+	i = 1000;
+	while (((readl(USB_OTG_CLK_STAT(udc)) & USB_CLOCK_MASK) !=
+		USB_CLOCK_MASK) && (i > 0))
+		i--;
+	if (!i)
+		dev_dbg(udc->dev, "USB OTG clocks not correctly enabled\n");
+
+	/* Setup deferred workqueue data */
+	udc->poweron = udc->pullup = 0;
+	INIT_WORK(&udc->pullup_job, pullup_work);
+	INIT_WORK(&udc->vbus_job, vbus_work);
+#ifdef CONFIG_PM
+	INIT_WORK(&udc->power_job, power_work);
+#endif
+
+	/* All clocks are now on */
+	udc->clocked = 1;
+
+	isp1301_udc_configure(udc);
+	/* Allocate memory for the UDCA */
+	udc->udca_v_base = dma_alloc_coherent(&pdev->dev, UDCA_BUFF_SIZE,
+					      &dma_handle,
+					      (GFP_KERNEL | GFP_DMA));
+	if (!udc->udca_v_base) {
+		dev_err(udc->dev, "error getting UDCA region\n");
+		retval = -ENOMEM;
+		goto i2c_fail;
+	}
+	udc->udca_p_base = dma_handle;
+	dev_dbg(udc->dev, "DMA buffer(0x%x bytes), P:0x%08x, V:0x%p\n",
+		UDCA_BUFF_SIZE, udc->udca_p_base, udc->udca_v_base);
+
+	/* Setup the DD DMA memory pool */
+	udc->dd_cache = dma_pool_create("udc_dd", udc->dev,
+					sizeof(struct lpc32xx_usbd_dd_gad),
+					sizeof(u32), 0);
+	if (!udc->dd_cache) {
+		dev_err(udc->dev, "error getting DD DMA region\n");
+		retval = -ENOMEM;
+		goto dma_alloc_fail;
+	}
+
+	/* Clear USB peripheral and initialize gadget endpoints */
+	udc_disable(udc);
+	udc_reinit(udc);
+
+	retval = device_register(&udc->gadget.dev);
+	if (retval < 0) {
+		dev_err(udc->dev, "Device registration failure\n");
+		goto dev_register_fail;
+	}
+
+	/* Request IRQs - low and high priority USB device IRQs are routed to
+	 * the same handler, while the DMA interrupt is routed elsewhere */
+	retval = request_irq(udc->udp_irq[IRQ_USB_LP], lpc32xx_usb_lp_irq,
+			     0, "udc_lp", udc);
+	if (retval < 0) {
+		dev_err(udc->dev, "LP request irq %d failed\n",
+			udc->udp_irq[IRQ_USB_LP]);
+		goto irq_lp_fail;
+	}
+	retval = request_irq(udc->udp_irq[IRQ_USB_HP], lpc32xx_usb_hp_irq,
+			     0, "udc_hp", udc);
+	if (retval < 0) {
+		dev_err(udc->dev, "HP request irq %d failed\n",
+			udc->udp_irq[IRQ_USB_HP]);
+		goto irq_hp_fail;
+	}
+
+	retval = request_irq(udc->udp_irq[IRQ_USB_DEVDMA],
+			     lpc32xx_usb_devdma_irq, 0, "udc_dma", udc);
+	if (retval < 0) {
+		dev_err(udc->dev, "DEV request irq %d failed\n",
+			udc->udp_irq[IRQ_USB_DEVDMA]);
+		goto irq_dev_fail;
+	}
+
+	/* The transceiver interrupt is used for VBUS detection and will
+	   kick off the VBUS handler function */
+	retval = request_irq(udc->udp_irq[IRQ_USB_ATX], lpc32xx_usb_vbus_irq,
+			     0, "udc_otg", udc);
+	if (retval < 0) {
+		dev_err(udc->dev, "VBUS request irq %d failed\n",
+			udc->udp_irq[IRQ_USB_ATX]);
+		goto irq_xcvr_fail;
+	}
+
+	/* Initialize wait queue */
+	init_waitqueue_head(&udc->ep_disable_wait_queue);
+	atomic_set(&udc->enabled_ep_cnt, 0);
+
+	/* Keep all IRQs disabled until GadgetFS starts up */
+	for (i = IRQ_USB_LP; i <= IRQ_USB_ATX; i++)
+		disable_irq(udc->udp_irq[i]);
+
+	retval = usb_add_gadget_udc(dev, &udc->gadget);
+	if (retval < 0)
+		goto add_gadget_fail;
+
+	dev_set_drvdata(dev, udc);
+	device_init_wakeup(dev, 1);
+	create_debug_file(udc);
+
+	/* Disable clocks for now */
+	udc_clk_set(udc, 0);
+
+	dev_info(udc->dev, "%s version %s\n", driver_name, DRIVER_VERSION);
+	return 0;
+
+add_gadget_fail:
+	free_irq(udc->udp_irq[IRQ_USB_ATX], udc);
+irq_xcvr_fail:
+	free_irq(udc->udp_irq[IRQ_USB_DEVDMA], udc);
+irq_dev_fail:
+	free_irq(udc->udp_irq[IRQ_USB_HP], udc);
+irq_hp_fail:
+	free_irq(udc->udp_irq[IRQ_USB_LP], udc);
+irq_lp_fail:
+	device_unregister(&udc->gadget.dev);
+dev_register_fail:
+	dma_pool_destroy(udc->dd_cache);
+dma_alloc_fail:
+	dma_free_coherent(&pdev->dev, UDCA_BUFF_SIZE,
+			  udc->udca_v_base, udc->udca_p_base);
+i2c_fail:
+	clk_disable(udc->usb_slv_clk);
+usb_clk_enable_fail:
+pll_set_fail:
+	clk_disable(udc->usb_pll_clk);
+pll_enable_fail:
+	clk_put(udc->usb_slv_clk);
+usb_clk_get_fail:
+	clk_put(udc->usb_pll_clk);
+pll_get_fail:
+	iounmap(udc->udp_baseaddr);
+io_map_fail:
+	release_mem_region(udc->io_p_start, udc->io_p_size);
+	dev_err(udc->dev, "%s probe failed, %d\n", driver_name, retval);
+
+	return retval;
+}
+
+static int __devexit lpc32xx_udc_remove(struct platform_device *pdev)
+{
+	struct lpc32xx_udc *udc = platform_get_drvdata(pdev);
+
+	usb_del_gadget_udc(&udc->gadget);
+	if (udc->driver)
+		return -EBUSY;
+
+	udc_clk_set(udc, 1);
+	udc_disable(udc);
+	pullup(udc, 0);
+
+	free_irq(udc->udp_irq[IRQ_USB_ATX], udc);
+
+	device_init_wakeup(&pdev->dev, 0);
+	remove_debug_file(udc);
+
+	dma_pool_destroy(udc->dd_cache);
+	dma_free_coherent(&pdev->dev, UDCA_BUFF_SIZE,
+			  udc->udca_v_base, udc->udca_p_base);
+	free_irq(udc->udp_irq[IRQ_USB_DEVDMA], udc);
+	free_irq(udc->udp_irq[IRQ_USB_HP], udc);
+	free_irq(udc->udp_irq[IRQ_USB_LP], udc);
+
+	device_unregister(&udc->gadget.dev);
+
+	clk_disable(udc->usb_slv_clk);
+	clk_put(udc->usb_slv_clk);
+	clk_disable(udc->usb_pll_clk);
+	clk_put(udc->usb_pll_clk);
+	iounmap(udc->udp_baseaddr);
+	release_mem_region(udc->io_p_start, udc->io_p_size);
+
+	return 0;
+}
+
+#ifdef CONFIG_PM
+static int lpc32xx_udc_suspend(struct platform_device *pdev, pm_message_t mesg)
+{
+	int to = 1000;
+	struct lpc32xx_udc *udc = platform_get_drvdata(pdev);
+
+	if (udc->clocked) {
+		/* Power down ISP */
+		udc->poweron = 0;
+		isp1301_set_powerstate(udc, 0);
+
+		/* Disable clocking */
+		udc_clk_set(udc, 0);
+
+		/* Keep clock flag on, so we know to re-enable clocks
+		   on resume */
+		udc->clocked = 1;
+
+		/* Kill OTG and I2C clocks */
+		writel(0, USB_OTG_CLK_CTRL(udc));
+		while (((readl(USB_OTG_CLK_STAT(udc)) & OTGOFF_CLK_MASK) !=
+			OTGOFF_CLK_MASK) && (to > 0))
+			to--;
+		if (!to)
+			dev_dbg(udc->dev,
+				"USB OTG clocks not correctly enabled\n");
+
+		/* Kill global USB clock */
+		clk_disable(udc->usb_slv_clk);
+	}
+
+	return 0;
+}
+
+static int lpc32xx_udc_resume(struct platform_device *pdev)
+{
+	struct lpc32xx_udc *udc = platform_get_drvdata(pdev);
+
+	if (udc->clocked) {
+		/* Enable global USB clock */
+		clk_enable(udc->usb_slv_clk);
+
+		/* Enable clocking */
+		udc_clk_set(udc, 1);
+
+		/* ISP back to normal power mode */
+		udc->poweron = 1;
+		isp1301_set_powerstate(udc, 1);
+	}
+
+	return 0;
+}
+#else
+#define	lpc32xx_udc_suspend	NULL
+#define	lpc32xx_udc_resume	NULL
+#endif
+
+#ifdef CONFIG_OF
+static struct of_device_id lpc32xx_udc_of_match[] = {
+	{ .compatible = "nxp,lpc3220-udc", },
+	{ },
+};
+MODULE_DEVICE_TABLE(of, lpc32xx_udc_of_match);
+#endif
+
+static struct platform_driver lpc32xx_udc_driver = {
+	.remove		= __devexit_p(lpc32xx_udc_remove),
+	.shutdown	= lpc32xx_udc_shutdown,
+	.suspend	= lpc32xx_udc_suspend,
+	.resume		= lpc32xx_udc_resume,
+	.driver		= {
+		.name	= (char *) driver_name,
+		.owner	= THIS_MODULE,
+		.of_match_table = of_match_ptr(lpc32xx_udc_of_match),
+	},
+};
+
+static int __init udc_init_module(void)
+{
+	return platform_driver_probe(&lpc32xx_udc_driver, lpc32xx_udc_probe);
+}
+module_init(udc_init_module);
+
+static void __exit udc_exit_module(void)
+{
+	platform_driver_unregister(&lpc32xx_udc_driver);
+}
+module_exit(udc_exit_module);
+
+MODULE_DESCRIPTION("LPC32XX udc driver");
+MODULE_AUTHOR("Kevin Wells <kevin.wells@nxp.com>");
+MODULE_AUTHOR("Roland Stigge <stigge@antcom.de>");
+MODULE_LICENSE("GPL");
+MODULE_ALIAS("platform:lpc32xx_udc");
diff --git a/drivers/usb/gadget/m66592-udc.c b/drivers/usb/gadget/m66592-udc.c
index 3608b3bd573..8981fbb5748 100644
--- a/drivers/usb/gadget/m66592-udc.c
+++ b/drivers/usb/gadget/m66592-udc.c
@@ -390,7 +390,7 @@ static int alloc_pipe_config(struct m66592_ep *ep,
 	int *counter;
 	int ret;
 
-	ep->desc = desc;
+	ep->ep.desc = desc;
 
 	BUG_ON(ep->pipenum);
 
@@ -558,7 +558,7 @@ static void start_packet_read(struct m66592_ep *ep, struct m66592_request *req)
 
 static void start_packet(struct m66592_ep *ep, struct m66592_request *req)
 {
-	if (ep->desc->bEndpointAddress & USB_DIR_IN)
+	if (ep->ep.desc->bEndpointAddress & USB_DIR_IN)
 		start_packet_write(ep, req);
 	else
 		start_packet_read(ep, req);
@@ -734,7 +734,7 @@ __acquires(m66592->lock)
 
 	if (restart) {
 		req = list_entry(ep->queue.next, struct m66592_request, queue);
-		if (ep->desc)
+		if (ep->ep.desc)
 			start_packet(ep, req);
 	}
 }
@@ -917,7 +917,7 @@ static void irq_pipe_ready(struct m66592 *m66592, u16 status, u16 enb)
 				ep = m66592->pipenum2ep[pipenum];
 				req = list_entry(ep->queue.next,
 						 struct m66592_request, queue);
-				if (ep->desc->bEndpointAddress & USB_DIR_IN)
+				if (ep->ep.desc->bEndpointAddress & USB_DIR_IN)
 					irq_packet_write(ep, req);
 				else
 					irq_packet_read(ep, req);
@@ -1377,7 +1377,7 @@ static int m66592_queue(struct usb_ep *_ep, struct usb_request *_req,
 	req->req.actual = 0;
 	req->req.status = -EINPROGRESS;
 
-	if (ep->desc == NULL)	/* control */
+	if (ep->ep.desc == NULL)	/* control */
 		start_ep0(ep, req);
 	else {
 		if (request && !ep->busy)
diff --git a/drivers/usb/gadget/m66592-udc.h b/drivers/usb/gadget/m66592-udc.h
index 9d9f7e39f03..88c85b4116a 100644
--- a/drivers/usb/gadget/m66592-udc.h
+++ b/drivers/usb/gadget/m66592-udc.h
@@ -456,7 +456,7 @@ struct m66592_ep {
 	unsigned		use_dma:1;
 	u16			pipenum;
 	u16			type;
-	const struct usb_endpoint_descriptor	*desc;
+
 	/* register address */
 	unsigned long		fifoaddr;
 	unsigned long		fifosel;
diff --git a/drivers/usb/gadget/mv_udc.h b/drivers/usb/gadget/mv_udc.h
index e2be9519abb..9073436d8b2 100644
--- a/drivers/usb/gadget/mv_udc.h
+++ b/drivers/usb/gadget/mv_udc.h
@@ -232,7 +232,6 @@ struct mv_ep {
 	struct mv_udc		*udc;
 	struct list_head	queue;
 	struct mv_dqh		*dqh;
-	const struct usb_endpoint_descriptor	*desc;
 	u32			direction;
 	char			name[14];
 	unsigned		stopped:1,
diff --git a/drivers/usb/gadget/mv_udc_core.c b/drivers/usb/gadget/mv_udc_core.c
index a73cf406e2a..dbcd1329495 100644
--- a/drivers/usb/gadget/mv_udc_core.c
+++ b/drivers/usb/gadget/mv_udc_core.c
@@ -464,7 +464,7 @@ static int mv_ep_enable(struct usb_ep *_ep,
 	ep = container_of(_ep, struct mv_ep, ep);
 	udc = ep->udc;
 
-	if (!_ep || !desc || ep->desc
+	if (!_ep || !desc || ep->ep.desc
 			|| desc->bDescriptorType != USB_DT_ENDPOINT)
 		return -EINVAL;
 
@@ -528,7 +528,7 @@ static int mv_ep_enable(struct usb_ep *_ep,
 	dqh->size_ioc_int_sts = 0;
 
 	ep->ep.maxpacket = max;
-	ep->desc = desc;
+	ep->ep.desc = desc;
 	ep->stopped = 0;
 
 	/* Enable the endpoint for Rx or Tx and set the endpoint type */
@@ -580,7 +580,7 @@ static int  mv_ep_disable(struct usb_ep *_ep)
 	unsigned long flags;
 
 	ep = container_of(_ep, struct mv_ep, ep);
-	if ((_ep == NULL) || !ep->desc)
+	if ((_ep == NULL) || !ep->ep.desc)
 		return -EINVAL;
 
 	udc = ep->udc;
@@ -606,7 +606,6 @@ static int  mv_ep_disable(struct usb_ep *_ep)
 	/* nuke all pending requests (does flush) */
 	nuke(ep, -ESHUTDOWN);
 
-	ep->desc = NULL;
 	ep->ep.desc = NULL;
 	ep->stopped = 1;
 
@@ -651,7 +650,7 @@ static void mv_ep_fifo_flush(struct usb_ep *_ep)
 		return;
 
 	ep = container_of(_ep, struct mv_ep, ep);
-	if (!ep->desc)
+	if (!ep->ep.desc)
 		return;
 
 	udc = ep->udc;
@@ -715,11 +714,11 @@ mv_ep_queue(struct usb_ep *_ep, struct usb_request *_req, gfp_t gfp_flags)
 		dev_err(&udc->dev->dev, "%s, bad params", __func__);
 		return -EINVAL;
 	}
-	if (unlikely(!_ep || !ep->desc)) {
+	if (unlikely(!_ep || !ep->ep.desc)) {
 		dev_err(&udc->dev->dev, "%s, bad ep", __func__);
 		return -EINVAL;
 	}
-	if (ep->desc->bmAttributes == USB_ENDPOINT_XFER_ISOC) {
+	if (ep->ep.desc->bmAttributes == USB_ENDPOINT_XFER_ISOC) {
 		if (req->req.length > ep->ep.maxpacket)
 			return -EMSGSIZE;
 	}
@@ -925,12 +924,12 @@ static int mv_ep_set_halt_wedge(struct usb_ep *_ep, int halt, int wedge)
 
 	ep = container_of(_ep, struct mv_ep, ep);
 	udc = ep->udc;
-	if (!_ep || !ep->desc) {
+	if (!_ep || !ep->ep.desc) {
 		status = -EINVAL;
 		goto out;
 	}
 
-	if (ep->desc->bmAttributes == USB_ENDPOINT_XFER_ISOC) {
+	if (ep->ep.desc->bmAttributes == USB_ENDPOINT_XFER_ISOC) {
 		status = -EOPNOTSUPP;
 		goto out;
 	}
@@ -1279,7 +1278,7 @@ static int eps_init(struct mv_udc *udc)
 	ep->stopped = 0;
 	ep->ep.maxpacket = EP0_MAX_PKT_SIZE;
 	ep->ep_num = 0;
-	ep->desc = &mv_ep0_desc;
+	ep->ep.desc = &mv_ep0_desc;
 	INIT_LIST_HEAD(&ep->queue);
 
 	ep->ep_type = USB_ENDPOINT_XFER_CONTROL;
diff --git a/drivers/usb/gadget/ndis.h b/drivers/usb/gadget/ndis.h
index b0e52fc277b..a19f72dec0c 100644
--- a/drivers/usb/gadget/ndis.h
+++ b/drivers/usb/gadget/ndis.h
@@ -15,11 +15,6 @@
 #ifndef _LINUX_NDIS_H
 #define _LINUX_NDIS_H
 
-
-#define NDIS_STATUS_MULTICAST_FULL	  0xC0010009
-#define NDIS_STATUS_MULTICAST_EXISTS      0xC001000A
-#define NDIS_STATUS_MULTICAST_NOT_FOUND   0xC001000B
-
 enum NDIS_DEVICE_POWER_STATE {
 	NdisDeviceStateUnspecified = 0,
 	NdisDeviceStateD0,
@@ -35,11 +30,6 @@ struct NDIS_PM_WAKE_UP_CAPABILITIES {
 	enum NDIS_DEVICE_POWER_STATE  MinLinkChangeWakeUp;
 };
 
-/* NDIS_PNP_CAPABILITIES.Flags constants */
-#define NDIS_DEVICE_WAKE_UP_ENABLE                0x00000001
-#define NDIS_DEVICE_WAKE_ON_PATTERN_MATCH_ENABLE  0x00000002
-#define NDIS_DEVICE_WAKE_ON_MAGIC_PACKET_ENABLE   0x00000004
-
 struct NDIS_PNP_CAPABILITIES {
 	__le32					Flags;
 	struct NDIS_PM_WAKE_UP_CAPABILITIES	WakeUpCapabilities;
@@ -54,158 +44,4 @@ struct NDIS_PM_PACKET_PATTERN {
 	__le32	PatternFlags;
 };
 
-
-/* Required Object IDs (OIDs) */
-#define OID_GEN_SUPPORTED_LIST            0x00010101
-#define OID_GEN_HARDWARE_STATUS           0x00010102
-#define OID_GEN_MEDIA_SUPPORTED           0x00010103
-#define OID_GEN_MEDIA_IN_USE              0x00010104
-#define OID_GEN_MAXIMUM_LOOKAHEAD         0x00010105
-#define OID_GEN_MAXIMUM_FRAME_SIZE        0x00010106
-#define OID_GEN_LINK_SPEED                0x00010107
-#define OID_GEN_TRANSMIT_BUFFER_SPACE     0x00010108
-#define OID_GEN_RECEIVE_BUFFER_SPACE      0x00010109
-#define OID_GEN_TRANSMIT_BLOCK_SIZE       0x0001010A
-#define OID_GEN_RECEIVE_BLOCK_SIZE        0x0001010B
-#define OID_GEN_VENDOR_ID                 0x0001010C
-#define OID_GEN_VENDOR_DESCRIPTION        0x0001010D
-#define OID_GEN_CURRENT_PACKET_FILTER     0x0001010E
-#define OID_GEN_CURRENT_LOOKAHEAD         0x0001010F
-#define OID_GEN_DRIVER_VERSION            0x00010110
-#define OID_GEN_MAXIMUM_TOTAL_SIZE        0x00010111
-#define OID_GEN_PROTOCOL_OPTIONS          0x00010112
-#define OID_GEN_MAC_OPTIONS               0x00010113
-#define OID_GEN_MEDIA_CONNECT_STATUS      0x00010114
-#define OID_GEN_MAXIMUM_SEND_PACKETS      0x00010115
-#define OID_GEN_VENDOR_DRIVER_VERSION     0x00010116
-#define OID_GEN_SUPPORTED_GUIDS           0x00010117
-#define OID_GEN_NETWORK_LAYER_ADDRESSES   0x00010118
-#define OID_GEN_TRANSPORT_HEADER_OFFSET   0x00010119
-#define OID_GEN_MACHINE_NAME              0x0001021A
-#define OID_GEN_RNDIS_CONFIG_PARAMETER    0x0001021B
-#define OID_GEN_VLAN_ID                   0x0001021C
-
-/* Optional OIDs */
-#define OID_GEN_MEDIA_CAPABILITIES        0x00010201
-#define OID_GEN_PHYSICAL_MEDIUM           0x00010202
-
-/* Required statistics OIDs */
-#define OID_GEN_XMIT_OK                   0x00020101
-#define OID_GEN_RCV_OK                    0x00020102
-#define OID_GEN_XMIT_ERROR                0x00020103
-#define OID_GEN_RCV_ERROR                 0x00020104
-#define OID_GEN_RCV_NO_BUFFER             0x00020105
-
-/* Optional statistics OIDs */
-#define OID_GEN_DIRECTED_BYTES_XMIT       0x00020201
-#define OID_GEN_DIRECTED_FRAMES_XMIT      0x00020202
-#define OID_GEN_MULTICAST_BYTES_XMIT      0x00020203
-#define OID_GEN_MULTICAST_FRAMES_XMIT     0x00020204
-#define OID_GEN_BROADCAST_BYTES_XMIT      0x00020205
-#define OID_GEN_BROADCAST_FRAMES_XMIT     0x00020206
-#define OID_GEN_DIRECTED_BYTES_RCV        0x00020207
-#define OID_GEN_DIRECTED_FRAMES_RCV       0x00020208
-#define OID_GEN_MULTICAST_BYTES_RCV       0x00020209
-#define OID_GEN_MULTICAST_FRAMES_RCV      0x0002020A
-#define OID_GEN_BROADCAST_BYTES_RCV       0x0002020B
-#define OID_GEN_BROADCAST_FRAMES_RCV      0x0002020C
-#define OID_GEN_RCV_CRC_ERROR             0x0002020D
-#define OID_GEN_TRANSMIT_QUEUE_LENGTH     0x0002020E
-#define OID_GEN_GET_TIME_CAPS             0x0002020F
-#define OID_GEN_GET_NETCARD_TIME          0x00020210
-#define OID_GEN_NETCARD_LOAD              0x00020211
-#define OID_GEN_DEVICE_PROFILE            0x00020212
-#define OID_GEN_INIT_TIME_MS              0x00020213
-#define OID_GEN_RESET_COUNTS              0x00020214
-#define OID_GEN_MEDIA_SENSE_COUNTS        0x00020215
-#define OID_GEN_FRIENDLY_NAME             0x00020216
-#define OID_GEN_MINIPORT_INFO             0x00020217
-#define OID_GEN_RESET_VERIFY_PARAMETERS   0x00020218
-
-/* IEEE 802.3 (Ethernet) OIDs */
-#define NDIS_802_3_MAC_OPTION_PRIORITY    0x00000001
-
-#define OID_802_3_PERMANENT_ADDRESS       0x01010101
-#define OID_802_3_CURRENT_ADDRESS         0x01010102
-#define OID_802_3_MULTICAST_LIST          0x01010103
-#define OID_802_3_MAXIMUM_LIST_SIZE       0x01010104
-#define OID_802_3_MAC_OPTIONS             0x01010105
-#define OID_802_3_RCV_ERROR_ALIGNMENT     0x01020101
-#define OID_802_3_XMIT_ONE_COLLISION      0x01020102
-#define OID_802_3_XMIT_MORE_COLLISIONS    0x01020103
-#define OID_802_3_XMIT_DEFERRED           0x01020201
-#define OID_802_3_XMIT_MAX_COLLISIONS     0x01020202
-#define OID_802_3_RCV_OVERRUN             0x01020203
-#define OID_802_3_XMIT_UNDERRUN           0x01020204
-#define OID_802_3_XMIT_HEARTBEAT_FAILURE  0x01020205
-#define OID_802_3_XMIT_TIMES_CRS_LOST     0x01020206
-#define OID_802_3_XMIT_LATE_COLLISIONS    0x01020207
-
-/* OID_GEN_MINIPORT_INFO constants */
-#define NDIS_MINIPORT_BUS_MASTER                      0x00000001
-#define NDIS_MINIPORT_WDM_DRIVER                      0x00000002
-#define NDIS_MINIPORT_SG_LIST                         0x00000004
-#define NDIS_MINIPORT_SUPPORTS_MEDIA_QUERY            0x00000008
-#define NDIS_MINIPORT_INDICATES_PACKETS               0x00000010
-#define NDIS_MINIPORT_IGNORE_PACKET_QUEUE             0x00000020
-#define NDIS_MINIPORT_IGNORE_REQUEST_QUEUE            0x00000040
-#define NDIS_MINIPORT_IGNORE_TOKEN_RING_ERRORS        0x00000080
-#define NDIS_MINIPORT_INTERMEDIATE_DRIVER             0x00000100
-#define NDIS_MINIPORT_IS_NDIS_5                       0x00000200
-#define NDIS_MINIPORT_IS_CO                           0x00000400
-#define NDIS_MINIPORT_DESERIALIZE                     0x00000800
-#define NDIS_MINIPORT_REQUIRES_MEDIA_POLLING          0x00001000
-#define NDIS_MINIPORT_SUPPORTS_MEDIA_SENSE            0x00002000
-#define NDIS_MINIPORT_NETBOOT_CARD                    0x00004000
-#define NDIS_MINIPORT_PM_SUPPORTED                    0x00008000
-#define NDIS_MINIPORT_SUPPORTS_MAC_ADDRESS_OVERWRITE  0x00010000
-#define NDIS_MINIPORT_USES_SAFE_BUFFER_APIS           0x00020000
-#define NDIS_MINIPORT_HIDDEN                          0x00040000
-#define NDIS_MINIPORT_SWENUM                          0x00080000
-#define NDIS_MINIPORT_SURPRISE_REMOVE_OK              0x00100000
-#define NDIS_MINIPORT_NO_HALT_ON_SUSPEND              0x00200000
-#define NDIS_MINIPORT_HARDWARE_DEVICE                 0x00400000
-#define NDIS_MINIPORT_SUPPORTS_CANCEL_SEND_PACKETS    0x00800000
-#define NDIS_MINIPORT_64BITS_DMA                      0x01000000
-
-#define NDIS_MEDIUM_802_3		0x00000000
-#define NDIS_MEDIUM_802_5		0x00000001
-#define NDIS_MEDIUM_FDDI		0x00000002
-#define NDIS_MEDIUM_WAN			0x00000003
-#define NDIS_MEDIUM_LOCAL_TALK		0x00000004
-#define NDIS_MEDIUM_DIX			0x00000005
-#define NDIS_MEDIUM_ARCENT_RAW		0x00000006
-#define NDIS_MEDIUM_ARCENT_878_2	0x00000007
-#define NDIS_MEDIUM_ATM			0x00000008
-#define NDIS_MEDIUM_WIRELESS_LAN	0x00000009
-#define NDIS_MEDIUM_IRDA		0x0000000A
-#define NDIS_MEDIUM_BPC			0x0000000B
-#define NDIS_MEDIUM_CO_WAN		0x0000000C
-#define NDIS_MEDIUM_1394		0x0000000D
-
-#define NDIS_PACKET_TYPE_DIRECTED	0x00000001
-#define NDIS_PACKET_TYPE_MULTICAST	0x00000002
-#define NDIS_PACKET_TYPE_ALL_MULTICAST	0x00000004
-#define NDIS_PACKET_TYPE_BROADCAST	0x00000008
-#define NDIS_PACKET_TYPE_SOURCE_ROUTING	0x00000010
-#define NDIS_PACKET_TYPE_PROMISCUOUS	0x00000020
-#define NDIS_PACKET_TYPE_SMT		0x00000040
-#define NDIS_PACKET_TYPE_ALL_LOCAL	0x00000080
-#define NDIS_PACKET_TYPE_GROUP		0x00000100
-#define NDIS_PACKET_TYPE_ALL_FUNCTIONAL	0x00000200
-#define NDIS_PACKET_TYPE_FUNCTIONAL	0x00000400
-#define NDIS_PACKET_TYPE_MAC_FRAME	0x00000800
-
-#define NDIS_MEDIA_STATE_CONNECTED	0x00000000
-#define NDIS_MEDIA_STATE_DISCONNECTED	0x00000001
-
-#define NDIS_MAC_OPTION_COPY_LOOKAHEAD_DATA     0x00000001
-#define NDIS_MAC_OPTION_RECEIVE_SERIALIZED      0x00000002
-#define NDIS_MAC_OPTION_TRANSFERS_NOT_PEND      0x00000004
-#define NDIS_MAC_OPTION_NO_LOOPBACK             0x00000008
-#define NDIS_MAC_OPTION_FULL_DUPLEX             0x00000010
-#define NDIS_MAC_OPTION_EOTX_INDICATION         0x00000020
-#define NDIS_MAC_OPTION_8021P_PRIORITY          0x00000040
-#define NDIS_MAC_OPTION_RESERVED                0x80000000
-
 #endif /* _LINUX_NDIS_H */
diff --git a/drivers/usb/gadget/omap_udc.c b/drivers/usb/gadget/omap_udc.c
index 3b4b6dd0f95..7ba32469c5b 100644
--- a/drivers/usb/gadget/omap_udc.c
+++ b/drivers/usb/gadget/omap_udc.c
@@ -153,7 +153,7 @@ static int omap_ep_enable(struct usb_ep *_ep,
 	u16		maxp;
 
 	/* catch various bogus parameters */
-	if (!_ep || !desc || ep->desc
+	if (!_ep || !desc || ep->ep.desc
 			|| desc->bDescriptorType != USB_DT_ENDPOINT
 			|| ep->bEndpointAddress != desc->bEndpointAddress
 			|| ep->maxpacket < usb_endpoint_maxp(desc)) {
@@ -200,7 +200,7 @@ static int omap_ep_enable(struct usb_ep *_ep,
 
 	spin_lock_irqsave(&udc->lock, flags);
 
-	ep->desc = desc;
+	ep->ep.desc = desc;
 	ep->irqs = 0;
 	ep->stopped = 0;
 	ep->ep.maxpacket = maxp;
@@ -242,14 +242,13 @@ static int omap_ep_disable(struct usb_ep *_ep)
 	struct omap_ep	*ep = container_of(_ep, struct omap_ep, ep);
 	unsigned long	flags;
 
-	if (!_ep || !ep->desc) {
+	if (!_ep || !ep->ep.desc) {
 		DBG("%s, %s not enabled\n", __func__,
 			_ep ? ep->ep.name : NULL);
 		return -EINVAL;
 	}
 
 	spin_lock_irqsave(&ep->udc->lock, flags);
-	ep->desc = NULL;
 	ep->ep.desc = NULL;
 	nuke (ep, -ESHUTDOWN);
 	ep->ep.maxpacket = ep->maxpacket;
@@ -917,7 +916,7 @@ omap_ep_queue(struct usb_ep *_ep, struct usb_request *_req, gfp_t gfp_flags)
 		DBG("%s, bad params\n", __func__);
 		return -EINVAL;
 	}
-	if (!_ep || (!ep->desc && ep->bEndpointAddress)) {
+	if (!_ep || (!ep->ep.desc && ep->bEndpointAddress)) {
 		DBG("%s, bad ep\n", __func__);
 		return -EINVAL;
 	}
@@ -1121,7 +1120,7 @@ static int omap_ep_set_halt(struct usb_ep *_ep, int value)
 			status = 0;
 
 	/* otherwise, all active non-ISO endpoints can halt */
-	} else if (ep->bmAttributes != USB_ENDPOINT_XFER_ISOC && ep->desc) {
+	} else if (ep->bmAttributes != USB_ENDPOINT_XFER_ISOC && ep->ep.desc) {
 
 		/* IN endpoints must already be idle */
 		if ((ep->bEndpointAddress & USB_DIR_IN)
@@ -1625,7 +1624,7 @@ static void ep0_irq(struct omap_udc *udc, u16 irq_src)
 				if (w_index & USB_DIR_IN)
 					ep += 16;
 				if (ep->bmAttributes == USB_ENDPOINT_XFER_ISOC
-						|| !ep->desc)
+						|| !ep->ep.desc)
 					goto do_stall;
 				use_ep(ep, 0);
 				omap_writew(udc->clr_halt, UDC_CTRL);
@@ -1653,7 +1652,7 @@ static void ep0_irq(struct omap_udc *udc, u16 irq_src)
 			if (w_index & USB_DIR_IN)
 				ep += 16;
 			if (ep->bmAttributes == USB_ENDPOINT_XFER_ISOC
-					|| ep == ep0 || !ep->desc)
+					|| ep == ep0 || !ep->ep.desc)
 				goto do_stall;
 			if (use_dma && ep->has_dma) {
 				/* this has rude side-effects (aborts) and
@@ -1688,7 +1687,7 @@ ep0out_status_stage:
 			ep = &udc->ep[w_index & 0xf];
 			if (w_index & USB_DIR_IN)
 				ep += 16;
-			if (!ep->desc)
+			if (!ep->ep.desc)
 				goto do_stall;
 
 			/* iso never stalls */
@@ -2509,7 +2508,7 @@ static int proc_udc_show(struct seq_file *s, void *_)
 		if (tmp & UDC_ADD) {
 			list_for_each_entry (ep, &udc->gadget.ep_list,
 					ep.ep_list) {
-				if (ep->desc)
+				if (ep->ep.desc)
 					proc_ep_show(s, ep);
 			}
 		}
diff --git a/drivers/usb/gadget/omap_udc.h b/drivers/usb/gadget/omap_udc.h
index 59d3b2213cb..cfadeb5fc5d 100644
--- a/drivers/usb/gadget/omap_udc.h
+++ b/drivers/usb/gadget/omap_udc.h
@@ -140,7 +140,6 @@ struct omap_ep {
 	struct list_head		queue;
 	unsigned long			irqs;
 	struct list_head		iso;
-	const struct usb_endpoint_descriptor	*desc;
 	char				name[14];
 	u16				maxpacket;
 	u8				bEndpointAddress;
diff --git a/drivers/usb/gadget/pch_udc.c b/drivers/usb/gadget/pch_udc.c
index 65307064a6f..1cfcc9ecbfb 100644
--- a/drivers/usb/gadget/pch_udc.c
+++ b/drivers/usb/gadget/pch_udc.c
@@ -295,7 +295,6 @@ struct pch_udc_ep {
 	struct pch_udc_data_dma_desc	*td_data;
 	struct pch_udc_dev		*dev;
 	unsigned long			offset_addr;
-	const struct usb_endpoint_descriptor	*desc;
 	struct list_head		queue;
 	unsigned			num:5,
 					in:1,
@@ -1705,7 +1704,7 @@ static int pch_udc_pcd_ep_enable(struct usb_ep *usbep,
 	if (!dev->driver || (dev->gadget.speed == USB_SPEED_UNKNOWN))
 		return -ESHUTDOWN;
 	spin_lock_irqsave(&dev->lock, iflags);
-	ep->desc = desc;
+	ep->ep.desc = desc;
 	ep->halted = 0;
 	pch_udc_ep_enable(ep, &ep->dev->cfg_data, desc);
 	ep->ep.maxpacket = usb_endpoint_maxp(desc);
@@ -1734,7 +1733,7 @@ static int pch_udc_pcd_ep_disable(struct usb_ep *usbep)
 
 	ep = container_of(usbep, struct pch_udc_ep, ep);
 	dev = ep->dev;
-	if ((usbep->name == ep0_string) || !ep->desc)
+	if ((usbep->name == ep0_string) || !ep->ep.desc)
 		return -EINVAL;
 
 	spin_lock_irqsave(&ep->dev->lock, iflags);
@@ -1742,7 +1741,6 @@ static int pch_udc_pcd_ep_disable(struct usb_ep *usbep)
 	ep->halted = 1;
 	pch_udc_ep_disable(ep);
 	pch_udc_disable_ep_interrupts(ep->dev, PCH_UDC_EPINT(ep->in, ep->num));
-	ep->desc = NULL;
 	ep->ep.desc = NULL;
 	INIT_LIST_HEAD(&ep->queue);
 	spin_unlock_irqrestore(&ep->dev->lock, iflags);
@@ -1849,7 +1847,7 @@ static int pch_udc_pcd_queue(struct usb_ep *usbep, struct usb_request *usbreq,
 		return -EINVAL;
 	ep = container_of(usbep, struct pch_udc_ep, ep);
 	dev = ep->dev;
-	if (!ep->desc && ep->num)
+	if (!ep->ep.desc && ep->num)
 		return -EINVAL;
 	req = container_of(usbreq, struct pch_udc_request, req);
 	if (!list_empty(&req->queue))
@@ -1949,7 +1947,7 @@ static int pch_udc_pcd_dequeue(struct usb_ep *usbep,
 
 	ep = container_of(usbep, struct pch_udc_ep, ep);
 	dev = ep->dev;
-	if (!usbep || !usbreq || (!ep->desc && ep->num))
+	if (!usbep || !usbreq || (!ep->ep.desc && ep->num))
 		return ret;
 	req = container_of(usbreq, struct pch_udc_request, req);
 	spin_lock_irqsave(&ep->dev->lock, flags);
@@ -1988,7 +1986,7 @@ static int pch_udc_pcd_set_halt(struct usb_ep *usbep, int halt)
 		return -EINVAL;
 	ep = container_of(usbep, struct pch_udc_ep, ep);
 	dev = ep->dev;
-	if (!ep->desc && !ep->num)
+	if (!ep->ep.desc && !ep->num)
 		return -EINVAL;
 	if (!ep->dev->driver || (ep->dev->gadget.speed == USB_SPEED_UNKNOWN))
 		return -ESHUTDOWN;
@@ -2033,7 +2031,7 @@ static int pch_udc_pcd_set_wedge(struct usb_ep *usbep)
 		return -EINVAL;
 	ep = container_of(usbep, struct pch_udc_ep, ep);
 	dev = ep->dev;
-	if (!ep->desc && !ep->num)
+	if (!ep->ep.desc && !ep->num)
 		return -EINVAL;
 	if (!ep->dev->driver || (ep->dev->gadget.speed == USB_SPEED_UNKNOWN))
 		return -ESHUTDOWN;
@@ -2065,7 +2063,7 @@ static void pch_udc_pcd_fifo_flush(struct usb_ep *usbep)
 		return;
 
 	ep = container_of(usbep, struct pch_udc_ep, ep);
-	if (ep->desc || !ep->num)
+	if (ep->ep.desc || !ep->num)
 		pch_udc_ep_fifo_flush(ep, ep->in);
 }
 
@@ -3282,7 +3280,6 @@ static DEFINE_PCI_DEVICE_TABLE(pch_udc_pcidev_id) = {
 
 MODULE_DEVICE_TABLE(pci, pch_udc_pcidev_id);
 
-
 static struct pci_driver pch_udc_driver = {
 	.name =	KBUILD_MODNAME,
 	.id_table =	pch_udc_pcidev_id,
@@ -3293,17 +3290,7 @@ static struct pci_driver pch_udc_driver = {
 	.shutdown =	pch_udc_shutdown,
 };
 
-static int __init pch_udc_pci_init(void)
-{
-	return pci_register_driver(&pch_udc_driver);
-}
-module_init(pch_udc_pci_init);
-
-static void __exit pch_udc_pci_exit(void)
-{
-	pci_unregister_driver(&pch_udc_driver);
-}
-module_exit(pch_udc_pci_exit);
+module_pci_driver(pch_udc_driver);
 
 MODULE_DESCRIPTION("Intel EG20T USB Device Controller");
 MODULE_AUTHOR("LAPIS Semiconductor, <tomoya-linux@dsn.lapis-semi.com>");
diff --git a/drivers/usb/gadget/printer.c b/drivers/usb/gadget/printer.c
index 4e4dc1f5f38..f1f9290a2f4 100644
--- a/drivers/usb/gadget/printer.c
+++ b/drivers/usb/gadget/printer.c
@@ -51,6 +51,7 @@
  * the runtime footprint, and giving us at least some parts of what
  * a "gcc --combine ... part1.c part2.c part3.c ... " build would.
  */
+#include "composite.c"
 #include "usbstring.c"
 #include "config.c"
 #include "epautoconf.c"
@@ -75,8 +76,6 @@ struct printer_dev {
 	/* lock buffer lists during read/write calls */
 	struct mutex		lock_printer_io;
 	struct usb_gadget	*gadget;
-	struct usb_request	*req;		/* for control responses */
-	u8			config;
 	s8			interface;
 	struct usb_ep		*in_ep, *out_ep;
 
@@ -100,6 +99,7 @@ struct printer_dev {
 	struct device		*pdev;
 	u8			printer_cdev_open;
 	wait_queue_head_t	wait;
+	struct usb_function	function;
 };
 
 static struct printer_dev usb_printer_gadget;
@@ -120,26 +120,6 @@ static struct printer_dev usb_printer_gadget;
  * parameters are in UTF-8 (superset of ASCII's 7 bit characters).
  */
 
-static ushort idVendor;
-module_param(idVendor, ushort, S_IRUGO);
-MODULE_PARM_DESC(idVendor, "USB Vendor ID");
-
-static ushort idProduct;
-module_param(idProduct, ushort, S_IRUGO);
-MODULE_PARM_DESC(idProduct, "USB Product ID");
-
-static ushort bcdDevice;
-module_param(bcdDevice, ushort, S_IRUGO);
-MODULE_PARM_DESC(bcdDevice, "USB Device version (BCD)");
-
-static char *iManufacturer;
-module_param(iManufacturer, charp, S_IRUGO);
-MODULE_PARM_DESC(iManufacturer, "USB Manufacturer string");
-
-static char *iProduct;
-module_param(iProduct, charp, S_IRUGO);
-MODULE_PARM_DESC(iProduct, "USB Product string");
-
 static char *iSerialNum;
 module_param(iSerialNum, charp, S_IRUGO);
 MODULE_PARM_DESC(iSerialNum, "1");
@@ -154,47 +134,8 @@ module_param(qlen, uint, S_IRUGO|S_IWUSR);
 
 #define QLEN	qlen
 
-#ifdef CONFIG_USB_GADGET_DUALSPEED
-#define DEVSPEED	USB_SPEED_HIGH
-#else   /* full speed (low speed doesn't do bulk) */
-#define DEVSPEED        USB_SPEED_FULL
-#endif
-
 /*-------------------------------------------------------------------------*/
 
-#define xprintk(d, level, fmt, args...) \
-	printk(level "%s: " fmt, DRIVER_DESC, ## args)
-
-#ifdef DEBUG
-#define DBG(dev, fmt, args...) \
-	xprintk(dev, KERN_DEBUG, fmt, ## args)
-#else
-#define DBG(dev, fmt, args...) \
-	do { } while (0)
-#endif /* DEBUG */
-
-#ifdef VERBOSE
-#define VDBG(dev, fmt, args...) \
-	xprintk(dev, KERN_DEBUG, fmt, ## args)
-#else
-#define VDBG(dev, fmt, args...) \
-	do { } while (0)
-#endif /* VERBOSE */
-
-#define ERROR(dev, fmt, args...) \
-	xprintk(dev, KERN_ERR, fmt, ## args)
-#define WARNING(dev, fmt, args...) \
-	xprintk(dev, KERN_WARNING, fmt, ## args)
-#define INFO(dev, fmt, args...) \
-	xprintk(dev, KERN_INFO, fmt, ## args)
-
-/*-------------------------------------------------------------------------*/
-
-/* USB DRIVER HOOKUP (to the hardware driver, below us), mostly
- * ep0 implementation:  descriptors, config management, setup().
- * also optional class-specific notification interrupt transfer.
- */
-
 /*
  * DESCRIPTORS ... most are static, but strings and (full) configuration
  * descriptors are built on demand.
@@ -227,24 +168,6 @@ static struct usb_device_descriptor device_desc = {
 	.bNumConfigurations =	1
 };
 
-static struct usb_otg_descriptor otg_desc = {
-	.bLength =		sizeof otg_desc,
-	.bDescriptorType =	USB_DT_OTG,
-	.bmAttributes =		USB_OTG_SRP
-};
-
-static struct usb_config_descriptor config_desc = {
-	.bLength =		sizeof config_desc,
-	.bDescriptorType =	USB_DT_CONFIG,
-
-	/* compute wTotalLength on the fly */
-	.bNumInterfaces =	1,
-	.bConfigurationValue =	DEV_CONFIG_VALUE,
-	.iConfiguration =	0,
-	.bmAttributes =		USB_CONFIG_ATT_ONE | USB_CONFIG_ATT_SELFPOWER,
-	.bMaxPower =		CONFIG_USB_GADGET_VBUS_DRAW / 2,
-};
-
 static struct usb_interface_descriptor intf_desc = {
 	.bLength =		sizeof intf_desc,
 	.bDescriptorType =	USB_DT_INTERFACE,
@@ -270,16 +193,13 @@ static struct usb_endpoint_descriptor fs_ep_out_desc = {
 	.bmAttributes =		USB_ENDPOINT_XFER_BULK
 };
 
-static const struct usb_descriptor_header *fs_printer_function [11] = {
-	(struct usb_descriptor_header *) &otg_desc,
+static struct usb_descriptor_header *fs_printer_function[] = {
 	(struct usb_descriptor_header *) &intf_desc,
 	(struct usb_descriptor_header *) &fs_ep_in_desc,
 	(struct usb_descriptor_header *) &fs_ep_out_desc,
 	NULL
 };
 
-#ifdef	CONFIG_USB_GADGET_DUALSPEED
-
 /*
  * usb 2.0 devices need to expose both high speed and full speed
  * descriptors, unless they only run at full speed.
@@ -307,23 +227,26 @@ static struct usb_qualifier_descriptor dev_qualifier = {
 	.bNumConfigurations =	1
 };
 
-static const struct usb_descriptor_header *hs_printer_function [11] = {
-	(struct usb_descriptor_header *) &otg_desc,
+static struct usb_descriptor_header *hs_printer_function[] = {
 	(struct usb_descriptor_header *) &intf_desc,
 	(struct usb_descriptor_header *) &hs_ep_in_desc,
 	(struct usb_descriptor_header *) &hs_ep_out_desc,
 	NULL
 };
 
-/* maxpacket and other transfer characteristics vary by speed. */
-#define ep_desc(g, hs, fs) (((g)->speed == USB_SPEED_HIGH)?(hs):(fs))
-
-#else
+static struct usb_otg_descriptor otg_descriptor = {
+	.bLength =              sizeof otg_descriptor,
+	.bDescriptorType =      USB_DT_OTG,
+	.bmAttributes =         USB_OTG_SRP,
+};
 
-/* if there's no high speed support, maxpacket doesn't change. */
-#define ep_desc(g, hs, fs) (((void)(g)), (fs))
+static const struct usb_descriptor_header *otg_desc[] = {
+	(struct usb_descriptor_header *) &otg_descriptor,
+	NULL,
+};
 
-#endif	/* !CONFIG_USB_GADGET_DUALSPEED */
+/* maxpacket and other transfer characteristics vary by speed. */
+#define ep_desc(g, hs, fs) (((g)->speed == USB_SPEED_HIGH)?(hs):(fs))
 
 /*-------------------------------------------------------------------------*/
 
@@ -343,11 +266,16 @@ static struct usb_string		strings [] = {
 	{  }		/* end of list */
 };
 
-static struct usb_gadget_strings	stringtab = {
+static struct usb_gadget_strings	stringtab_dev = {
 	.language	= 0x0409,	/* en-us */
 	.strings	= strings,
 };
 
+static struct usb_gadget_strings *dev_strings[] = {
+	&stringtab_dev,
+	NULL,
+};
+
 /*-------------------------------------------------------------------------*/
 
 static struct usb_request *
@@ -937,82 +865,8 @@ static void printer_reset_interface(struct printer_dev *dev)
 	dev->interface = -1;
 }
 
-/* change our operational config.  must agree with the code
- * that returns config descriptors, and altsetting code.
- */
-static int
-printer_set_config(struct printer_dev *dev, unsigned number)
-{
-	int			result = 0;
-	struct usb_gadget	*gadget = dev->gadget;
-
-	switch (number) {
-	case DEV_CONFIG_VALUE:
-		result = 0;
-		break;
-	default:
-		result = -EINVAL;
-		/* FALL THROUGH */
-	case 0:
-		break;
-	}
-
-	if (result) {
-		usb_gadget_vbus_draw(dev->gadget,
-				dev->gadget->is_otg ? 8 : 100);
-	} else {
-		unsigned power;
-
-		power = 2 * config_desc.bMaxPower;
-		usb_gadget_vbus_draw(dev->gadget, power);
-
-		dev->config = number;
-		INFO(dev, "%s config #%d: %d mA, %s\n",
-		     usb_speed_string(gadget->speed),
-		     number, power, driver_desc);
-	}
-	return result;
-}
-
-static int
-config_buf(enum usb_device_speed speed, u8 *buf, u8 type, unsigned index,
-		int is_otg)
-{
-	int					len;
-	const struct usb_descriptor_header	**function;
-#ifdef CONFIG_USB_GADGET_DUALSPEED
-	int					hs = (speed == USB_SPEED_HIGH);
-
-	if (type == USB_DT_OTHER_SPEED_CONFIG)
-		hs = !hs;
-
-	if (hs) {
-		function = hs_printer_function;
-	} else {
-		function = fs_printer_function;
-	}
-#else
-	function = fs_printer_function;
-#endif
-
-	if (index >= device_desc.bNumConfigurations)
-		return -EINVAL;
-
-	/* for now, don't advertise srp-only devices */
-	if (!is_otg)
-		function++;
-
-	len = usb_gadget_config_buf(&config_desc, buf, USB_DESC_BUFSIZE,
-			function);
-	if (len < 0)
-		return len;
-	((struct usb_config_descriptor *) buf)->bDescriptorType = type;
-	return len;
-}
-
 /* Change our operational Interface. */
-static int
-set_interface(struct printer_dev *dev, unsigned number)
+static int set_interface(struct printer_dev *dev, unsigned number)
 {
 	int			result = 0;
 
@@ -1043,14 +897,6 @@ set_interface(struct printer_dev *dev, unsigned number)
 	return result;
 }
 
-static void printer_setup_complete(struct usb_ep *ep, struct usb_request *req)
-{
-	if (req->status || req->actual != req->length)
-		DBG((struct printer_dev *) ep->driver_data,
-				"setup complete --> %d, %d/%d\n",
-				req->status, req->actual, req->length);
-}
-
 static void printer_soft_reset(struct printer_dev *dev)
 {
 	struct usb_request	*req;
@@ -1107,11 +953,12 @@ static void printer_soft_reset(struct printer_dev *dev)
  * The setup() callback implements all the ep0 functionality that's not
  * handled lower down.
  */
-static int
-printer_setup(struct usb_gadget *gadget, const struct usb_ctrlrequest *ctrl)
+static int printer_func_setup(struct usb_function *f,
+		const struct usb_ctrlrequest *ctrl)
 {
-	struct printer_dev	*dev = get_gadget_data(gadget);
-	struct usb_request	*req = dev->req;
+	struct printer_dev *dev = container_of(f, struct printer_dev, function);
+	struct usb_composite_dev *cdev = f->config->cdev;
+	struct usb_request	*req = cdev->req;
 	int			value = -EOPNOTSUPP;
 	u16			wIndex = le16_to_cpu(ctrl->wIndex);
 	u16			wValue = le16_to_cpu(ctrl->wValue);
@@ -1120,102 +967,7 @@ printer_setup(struct usb_gadget *gadget, const struct usb_ctrlrequest *ctrl)
 	DBG(dev, "ctrl req%02x.%02x v%04x i%04x l%d\n",
 		ctrl->bRequestType, ctrl->bRequest, wValue, wIndex, wLength);
 
-	req->complete = printer_setup_complete;
-
 	switch (ctrl->bRequestType&USB_TYPE_MASK) {
-
-	case USB_TYPE_STANDARD:
-		switch (ctrl->bRequest) {
-
-		case USB_REQ_GET_DESCRIPTOR:
-			if (ctrl->bRequestType != USB_DIR_IN)
-				break;
-			switch (wValue >> 8) {
-
-			case USB_DT_DEVICE:
-				device_desc.bMaxPacketSize0 =
-					gadget->ep0->maxpacket;
-				value = min(wLength, (u16) sizeof device_desc);
-				memcpy(req->buf, &device_desc, value);
-				break;
-#ifdef CONFIG_USB_GADGET_DUALSPEED
-			case USB_DT_DEVICE_QUALIFIER:
-				if (!gadget_is_dualspeed(gadget))
-					break;
-				/*
-				 * assumes ep0 uses the same value for both
-				 * speeds
-				 */
-				dev_qualifier.bMaxPacketSize0 =
-					gadget->ep0->maxpacket;
-				value = min(wLength,
-						(u16) sizeof dev_qualifier);
-				memcpy(req->buf, &dev_qualifier, value);
-				break;
-
-			case USB_DT_OTHER_SPEED_CONFIG:
-				if (!gadget_is_dualspeed(gadget))
-					break;
-				/* FALLTHROUGH */
-#endif /* CONFIG_USB_GADGET_DUALSPEED */
-			case USB_DT_CONFIG:
-				value = config_buf(gadget->speed, req->buf,
-						wValue >> 8,
-						wValue & 0xff,
-						gadget->is_otg);
-				if (value >= 0)
-					value = min(wLength, (u16) value);
-				break;
-
-			case USB_DT_STRING:
-				value = usb_gadget_get_string(&stringtab,
-						wValue & 0xff, req->buf);
-				if (value >= 0)
-					value = min(wLength, (u16) value);
-				break;
-			}
-			break;
-
-		case USB_REQ_SET_CONFIGURATION:
-			if (ctrl->bRequestType != 0)
-				break;
-			if (gadget->a_hnp_support)
-				DBG(dev, "HNP available\n");
-			else if (gadget->a_alt_hnp_support)
-				DBG(dev, "HNP needs a different root port\n");
-			value = printer_set_config(dev, wValue);
-			if (!value)
-				value = set_interface(dev, PRINTER_INTERFACE);
-			break;
-		case USB_REQ_GET_CONFIGURATION:
-			if (ctrl->bRequestType != USB_DIR_IN)
-				break;
-			*(u8 *)req->buf = dev->config;
-			value = min(wLength, (u16) 1);
-			break;
-
-		case USB_REQ_SET_INTERFACE:
-			if (ctrl->bRequestType != USB_RECIP_INTERFACE ||
-					!dev->config)
-				break;
-
-			value = set_interface(dev, PRINTER_INTERFACE);
-			break;
-		case USB_REQ_GET_INTERFACE:
-			if (ctrl->bRequestType !=
-					(USB_DIR_IN|USB_RECIP_INTERFACE)
-					|| !dev->config)
-				break;
-
-			*(u8 *)req->buf = dev->interface;
-			value = min(wLength, (u16) 1);
-			break;
-
-		default:
-			goto unknown;
-		}
-		break;
-
 	case USB_TYPE_CLASS:
 		switch (ctrl->bRequest) {
 		case 0: /* Get the IEEE-1284 PNP String */
@@ -1261,44 +1013,50 @@ unknown:
 			wValue, wIndex, wLength);
 		break;
 	}
-
-	/* respond with data transfer before status phase? */
-	if (value >= 0) {
-		req->length = value;
-		req->zero = value < wLength;
-		value = usb_ep_queue(gadget->ep0, req, GFP_ATOMIC);
-		if (value < 0) {
-			DBG(dev, "ep_queue --> %d\n", value);
-			req->status = 0;
-			printer_setup_complete(gadget->ep0, req);
-		}
-	}
-
 	/* host either stalls (value < 0) or reports success */
 	return value;
 }
 
-static void
-printer_disconnect(struct usb_gadget *gadget)
+static int __init printer_func_bind(struct usb_configuration *c,
+		struct usb_function *f)
 {
-	struct printer_dev	*dev = get_gadget_data(gadget);
+	return 0;
+}
+
+static void printer_func_unbind(struct usb_configuration *c,
+		struct usb_function *f)
+{
+}
+
+static int printer_func_set_alt(struct usb_function *f,
+		unsigned intf, unsigned alt)
+{
+	struct printer_dev *dev = container_of(f, struct printer_dev, function);
+	int ret = -ENOTSUPP;
+
+	if (!alt)
+		ret = set_interface(dev, PRINTER_INTERFACE);
+	return ret;
+}
+
+static void printer_func_disable(struct usb_function *f)
+{
+	struct printer_dev *dev = container_of(f, struct printer_dev, function);
 	unsigned long		flags;
 
 	DBG(dev, "%s\n", __func__);
 
 	spin_lock_irqsave(&dev->lock, flags);
-
 	printer_reset_interface(dev);
-
 	spin_unlock_irqrestore(&dev->lock, flags);
 }
 
-static void
-printer_unbind(struct usb_gadget *gadget)
+static void printer_cfg_unbind(struct usb_configuration *c)
 {
-	struct printer_dev	*dev = get_gadget_data(gadget);
+	struct printer_dev	*dev;
 	struct usb_request	*req;
 
+	dev = &usb_printer_gadget;
 
 	DBG(dev, "%s\n", __func__);
 
@@ -1336,18 +1094,18 @@ printer_unbind(struct usb_gadget *gadget)
 		list_del(&req->list);
 		printer_req_free(dev->out_ep, req);
 	}
-
-	if (dev->req) {
-		printer_req_free(gadget->ep0, dev->req);
-		dev->req = NULL;
-	}
-
-	set_gadget_data(gadget, NULL);
 }
 
-static int __init
-printer_bind(struct usb_gadget *gadget)
+static struct usb_configuration printer_cfg_driver = {
+	.label			= "printer",
+	.unbind			= printer_cfg_unbind,
+	.bConfigurationValue	= 1,
+	.bmAttributes		= USB_CONFIG_ATT_ONE | USB_CONFIG_ATT_SELFPOWER,
+};
+
+static int __init printer_bind_config(struct usb_configuration *c)
 {
+	struct usb_gadget	*gadget = c->cdev->gadget;
 	struct printer_dev	*dev;
 	struct usb_ep		*in_ep, *out_ep;
 	int			status = -ENOMEM;
@@ -1358,6 +1116,14 @@ printer_bind(struct usb_gadget *gadget)
 
 	dev = &usb_printer_gadget;
 
+	dev->function.name = shortname;
+	dev->function.descriptors = fs_printer_function;
+	dev->function.hs_descriptors = hs_printer_function;
+	dev->function.bind = printer_func_bind;
+	dev->function.setup = printer_func_setup;
+	dev->function.unbind = printer_func_unbind;
+	dev->function.set_alt = printer_func_set_alt;
+	dev->function.disable = printer_func_disable;
 
 	/* Setup the sysfs files for the printer gadget. */
 	dev->pdev = device_create(usb_gadget_class, NULL, g_printer_devno,
@@ -1393,29 +1159,6 @@ printer_bind(struct usb_gadget *gadget)
 		init_utsname()->sysname, init_utsname()->release,
 		gadget->name);
 
-	device_desc.idVendor =
-		cpu_to_le16(PRINTER_VENDOR_NUM);
-	device_desc.idProduct =
-		cpu_to_le16(PRINTER_PRODUCT_NUM);
-
-	/* support optional vendor/distro customization */
-	if (idVendor) {
-		if (!idProduct) {
-			dev_err(&gadget->dev, "idVendor needs idProduct!\n");
-			return -ENODEV;
-		}
-		device_desc.idVendor = cpu_to_le16(idVendor);
-		device_desc.idProduct = cpu_to_le16(idProduct);
-		if (bcdDevice)
-			device_desc.bcdDevice = cpu_to_le16(bcdDevice);
-	}
-
-	if (iManufacturer)
-		strlcpy(manufacturer, iManufacturer, sizeof manufacturer);
-
-	if (iProduct)
-		strlcpy(product_desc, iProduct, sizeof product_desc);
-
 	if (iSerialNum)
 		strlcpy(serial_num, iSerialNum, sizeof serial_num);
 
@@ -1442,17 +1185,16 @@ autoconf_fail:
 		goto autoconf_fail;
 	out_ep->driver_data = out_ep;	/* claim */
 
-#ifdef	CONFIG_USB_GADGET_DUALSPEED
 	/* assumes that all endpoints are dual-speed */
 	hs_ep_in_desc.bEndpointAddress = fs_ep_in_desc.bEndpointAddress;
 	hs_ep_out_desc.bEndpointAddress = fs_ep_out_desc.bEndpointAddress;
-#endif	/* DUALSPEED */
 
 	usb_gadget_set_selfpowered(gadget);
 
 	if (gadget->is_otg) {
-		otg_desc.bmAttributes |= USB_OTG_HNP,
-		config_desc.bmAttributes |= USB_CONFIG_ATT_WAKEUP;
+		otg_descriptor.bmAttributes |= USB_OTG_HNP;
+		printer_cfg_driver.descriptors = otg_desc;
+		printer_cfg_driver.bmAttributes |= USB_CONFIG_ATT_WAKEUP;
 	}
 
 	spin_lock_init(&dev->lock);
@@ -1466,7 +1208,6 @@ autoconf_fail:
 	init_waitqueue_head(&dev->tx_wait);
 	init_waitqueue_head(&dev->tx_flush_wait);
 
-	dev->config = 0;
 	dev->interface = -1;
 	dev->printer_cdev_open = 0;
 	dev->printer_status = PRINTER_NOT_ERROR;
@@ -1477,14 +1218,6 @@ autoconf_fail:
 	dev->in_ep = in_ep;
 	dev->out_ep = out_ep;
 
-	/* preallocate control message data and buffer */
-	dev->req = printer_req_alloc(gadget->ep0, USB_DESC_BUFSIZE,
-			GFP_KERNEL);
-	if (!dev->req) {
-		status = -ENOMEM;
-		goto fail;
-	}
-
 	for (i = 0; i < QLEN; i++) {
 		req = printer_req_alloc(dev->in_ep, USB_BUFSIZE, GFP_KERNEL);
 		if (!req) {
@@ -1513,45 +1246,37 @@ autoconf_fail:
 		list_add(&req->list, &dev->rx_reqs);
 	}
 
-	dev->req->complete = printer_setup_complete;
-
 	/* finish hookup to lower layer ... */
 	dev->gadget = gadget;
-	set_gadget_data(gadget, dev);
-	gadget->ep0->driver_data = dev;
 
 	INFO(dev, "%s, version: " DRIVER_VERSION "\n", driver_desc);
 	INFO(dev, "using %s, OUT %s IN %s\n", gadget->name, out_ep->name,
 			in_ep->name);
-
 	return 0;
 
 fail:
-	printer_unbind(gadget);
+	printer_cfg_unbind(c);
 	return status;
 }
 
-/*-------------------------------------------------------------------------*/
+static int printer_unbind(struct usb_composite_dev *cdev)
+{
+	return 0;
+}
 
-static struct usb_gadget_driver printer_driver = {
-	.max_speed	= DEVSPEED,
+static int __init printer_bind(struct usb_composite_dev *cdev)
+{
+	return usb_add_config(cdev, &printer_cfg_driver, printer_bind_config);
+}
 
-	.function	= (char *) driver_desc,
+static struct usb_composite_driver printer_driver = {
+	.name           = shortname,
+	.dev            = &device_desc,
+	.strings        = dev_strings,
+	.max_speed      = USB_SPEED_HIGH,
 	.unbind		= printer_unbind,
-
-	.setup		= printer_setup,
-	.disconnect	= printer_disconnect,
-
-	.driver		= {
-		.name		= (char *) shortname,
-		.owner		= THIS_MODULE,
-	},
 };
 
-MODULE_DESCRIPTION(DRIVER_DESC);
-MODULE_AUTHOR("Craig Nadler");
-MODULE_LICENSE("GPL");
-
 static int __init
 init(void)
 {
@@ -1560,23 +1285,23 @@ init(void)
 	usb_gadget_class = class_create(THIS_MODULE, "usb_printer_gadget");
 	if (IS_ERR(usb_gadget_class)) {
 		status = PTR_ERR(usb_gadget_class);
-		ERROR(dev, "unable to create usb_gadget class %d\n", status);
+		pr_err("unable to create usb_gadget class %d\n", status);
 		return status;
 	}
 
 	status = alloc_chrdev_region(&g_printer_devno, 0, 1,
 			"USB printer gadget");
 	if (status) {
-		ERROR(dev, "alloc_chrdev_region %d\n", status);
+		pr_err("alloc_chrdev_region %d\n", status);
 		class_destroy(usb_gadget_class);
 		return status;
 	}
 
-	status = usb_gadget_probe_driver(&printer_driver, printer_bind);
+	status = usb_composite_probe(&printer_driver, printer_bind);
 	if (status) {
 		class_destroy(usb_gadget_class);
 		unregister_chrdev_region(g_printer_devno, 1);
-		DBG(dev, "usb_gadget_probe_driver %x\n", status);
+		pr_err("usb_gadget_probe_driver %x\n", status);
 	}
 
 	return status;
@@ -1586,15 +1311,14 @@ module_init(init);
 static void __exit
 cleanup(void)
 {
-	int status;
-
 	mutex_lock(&usb_printer_gadget.lock_printer_io);
-	status = usb_gadget_unregister_driver(&printer_driver);
-	if (status)
-		ERROR(dev, "usb_gadget_unregister_driver %x\n", status);
-
+	usb_composite_unregister(&printer_driver);
 	unregister_chrdev_region(g_printer_devno, 1);
 	class_destroy(usb_gadget_class);
 	mutex_unlock(&usb_printer_gadget.lock_printer_io);
 }
 module_exit(cleanup);
+
+MODULE_DESCRIPTION(DRIVER_DESC);
+MODULE_AUTHOR("Craig Nadler");
+MODULE_LICENSE("GPL");
diff --git a/drivers/usb/gadget/pxa25x_udc.c b/drivers/usb/gadget/pxa25x_udc.c
index 41ed69c96d8..d7c8cb3bf75 100644
--- a/drivers/usb/gadget/pxa25x_udc.c
+++ b/drivers/usb/gadget/pxa25x_udc.c
@@ -218,7 +218,7 @@ static int pxa25x_ep_enable (struct usb_ep *_ep,
 	struct pxa25x_udc       *dev;
 
 	ep = container_of (_ep, struct pxa25x_ep, ep);
-	if (!_ep || !desc || ep->desc || _ep->name == ep0name
+	if (!_ep || !desc || ep->ep.desc || _ep->name == ep0name
 			|| desc->bDescriptorType != USB_DT_ENDPOINT
 			|| ep->bEndpointAddress != desc->bEndpointAddress
 			|| ep->fifo_size < usb_endpoint_maxp (desc)) {
@@ -249,7 +249,7 @@ static int pxa25x_ep_enable (struct usb_ep *_ep,
 		return -ESHUTDOWN;
 	}
 
-	ep->desc = desc;
+	ep->ep.desc = desc;
 	ep->stopped = 0;
 	ep->pio_irqs = 0;
 	ep->ep.maxpacket = usb_endpoint_maxp (desc);
@@ -269,7 +269,7 @@ static int pxa25x_ep_disable (struct usb_ep *_ep)
 	unsigned long		flags;
 
 	ep = container_of (_ep, struct pxa25x_ep, ep);
-	if (!_ep || !ep->desc) {
+	if (!_ep || !ep->ep.desc) {
 		DMSG("%s, %s not enabled\n", __func__,
 			_ep ? ep->ep.name : NULL);
 		return -EINVAL;
@@ -281,7 +281,6 @@ static int pxa25x_ep_disable (struct usb_ep *_ep)
 	/* flush fifo (mostly for IN buffers) */
 	pxa25x_ep_fifo_flush (_ep);
 
-	ep->desc = NULL;
 	ep->ep.desc = NULL;
 	ep->stopped = 1;
 
@@ -390,7 +389,7 @@ write_fifo (struct pxa25x_ep *ep, struct pxa25x_request *req)
 {
 	unsigned		max;
 
-	max = usb_endpoint_maxp(ep->desc);
+	max = usb_endpoint_maxp(ep->ep.desc);
 	do {
 		unsigned	count;
 		int		is_last, is_short;
@@ -644,7 +643,7 @@ pxa25x_ep_queue(struct usb_ep *_ep, struct usb_request *_req, gfp_t gfp_flags)
 	}
 
 	ep = container_of(_ep, struct pxa25x_ep, ep);
-	if (unlikely (!_ep || (!ep->desc && ep->ep.name != ep0name))) {
+	if (unlikely(!_ep || (!ep->ep.desc && ep->ep.name != ep0name))) {
 		DMSG("%s, bad ep\n", __func__);
 		return -EINVAL;
 	}
@@ -660,7 +659,7 @@ pxa25x_ep_queue(struct usb_ep *_ep, struct usb_request *_req, gfp_t gfp_flags)
 	 * we can report per-packet status.  that also helps with dma.
 	 */
 	if (unlikely (ep->bmAttributes == USB_ENDPOINT_XFER_ISOC
-		        && req->req.length > usb_endpoint_maxp (ep->desc)))
+			&& req->req.length > usb_endpoint_maxp(ep->ep.desc)))
 		return -EMSGSIZE;
 
 	DBG(DBG_NOISY, "%s queue req %p, len %d buf %p\n",
@@ -673,7 +672,7 @@ pxa25x_ep_queue(struct usb_ep *_ep, struct usb_request *_req, gfp_t gfp_flags)
 
 	/* kickstart this i/o queue? */
 	if (list_empty(&ep->queue) && !ep->stopped) {
-		if (ep->desc == NULL/* ep0 */) {
+		if (ep->ep.desc == NULL/* ep0 */) {
 			unsigned	length = _req->length;
 
 			switch (dev->ep0state) {
@@ -722,7 +721,7 @@ pxa25x_ep_queue(struct usb_ep *_ep, struct usb_request *_req, gfp_t gfp_flags)
 			req = NULL;
 		}
 
-		if (likely (req && ep->desc))
+		if (likely(req && ep->ep.desc))
 			pio_irq_enable(ep->bEndpointAddress);
 	}
 
@@ -749,7 +748,7 @@ static void nuke(struct pxa25x_ep *ep, int status)
 				queue);
 		done(ep, req, status);
 	}
-	if (ep->desc)
+	if (ep->ep.desc)
 		pio_irq_disable (ep->bEndpointAddress);
 }
 
@@ -792,7 +791,7 @@ static int pxa25x_ep_set_halt(struct usb_ep *_ep, int value)
 
 	ep = container_of(_ep, struct pxa25x_ep, ep);
 	if (unlikely (!_ep
-			|| (!ep->desc && ep->ep.name != ep0name))
+			|| (!ep->ep.desc && ep->ep.name != ep0name))
 			|| ep->bmAttributes == USB_ENDPOINT_XFER_ISOC) {
 		DMSG("%s, bad ep\n", __func__);
 		return -EINVAL;
@@ -820,7 +819,7 @@ static int pxa25x_ep_set_halt(struct usb_ep *_ep, int value)
 	*ep->reg_udccs = UDCCS_BI_FST|UDCCS_BI_FTF;
 
 	/* ep0 needs special care */
-	if (!ep->desc) {
+	if (!ep->ep.desc) {
 		start_watchdog(ep->dev);
 		ep->dev->req_pending = 0;
 		ep->dev->ep0state = EP0_STALL;
@@ -1087,7 +1086,7 @@ udc_seq_show(struct seq_file *m, void *_d)
 		if (i != 0) {
 			const struct usb_endpoint_descriptor	*desc;
 
-			desc = ep->desc;
+			desc = ep->ep.desc;
 			if (!desc)
 				continue;
 			tmp = *dev->ep [i].reg_udccs;
@@ -1191,7 +1190,6 @@ static void udc_reinit(struct pxa25x_udc *dev)
 		if (i != 0)
 			list_add_tail (&ep->ep.ep_list, &dev->gadget.ep_list);
 
-		ep->desc = NULL;
 		ep->ep.desc = NULL;
 		ep->stopped = 0;
 		INIT_LIST_HEAD (&ep->queue);
diff --git a/drivers/usb/gadget/pxa25x_udc.h b/drivers/usb/gadget/pxa25x_udc.h
index 893e917f048..861f4df6ea2 100644
--- a/drivers/usb/gadget/pxa25x_udc.h
+++ b/drivers/usb/gadget/pxa25x_udc.h
@@ -41,7 +41,6 @@ struct pxa25x_ep {
 	struct usb_ep				ep;
 	struct pxa25x_udc			*dev;
 
-	const struct usb_endpoint_descriptor	*desc;
 	struct list_head			queue;
 	unsigned long				pio_irqs;
 
diff --git a/drivers/usb/gadget/r8a66597-udc.c b/drivers/usb/gadget/r8a66597-udc.c
index c4401e7dd3a..f3ac2a20c27 100644
--- a/drivers/usb/gadget/r8a66597-udc.c
+++ b/drivers/usb/gadget/r8a66597-udc.c
@@ -459,7 +459,7 @@ static int alloc_pipe_config(struct r8a66597_ep *ep,
 	unsigned char *counter;
 	int ret;
 
-	ep->desc = desc;
+	ep->ep.desc = desc;
 
 	if (ep->pipenum)	/* already allocated pipe  */
 		return 0;
@@ -648,7 +648,7 @@ static int sudmac_alloc_channel(struct r8a66597 *r8a66597,
 	/* set SUDMAC parameters */
 	dma = &r8a66597->dma;
 	dma->used = 1;
-	if (ep->desc->bEndpointAddress & USB_DIR_IN) {
+	if (ep->ep.desc->bEndpointAddress & USB_DIR_IN) {
 		dma->dir = 1;
 	} else {
 		dma->dir = 0;
@@ -770,7 +770,7 @@ static void start_packet_read(struct r8a66597_ep *ep,
 
 static void start_packet(struct r8a66597_ep *ep, struct r8a66597_request *req)
 {
-	if (ep->desc->bEndpointAddress & USB_DIR_IN)
+	if (ep->ep.desc->bEndpointAddress & USB_DIR_IN)
 		start_packet_write(ep, req);
 	else
 		start_packet_read(ep, req);
@@ -930,7 +930,7 @@ __acquires(r8a66597->lock)
 
 	if (restart) {
 		req = get_request_from_ep(ep);
-		if (ep->desc)
+		if (ep->ep.desc)
 			start_packet(ep, req);
 	}
 }
@@ -1116,7 +1116,7 @@ static void irq_pipe_ready(struct r8a66597 *r8a66597, u16 status, u16 enb)
 				r8a66597_write(r8a66597, ~check, BRDYSTS);
 				ep = r8a66597->pipenum2ep[pipenum];
 				req = get_request_from_ep(ep);
-				if (ep->desc->bEndpointAddress & USB_DIR_IN)
+				if (ep->ep.desc->bEndpointAddress & USB_DIR_IN)
 					irq_packet_write(ep, req);
 				else
 					irq_packet_read(ep, req);
@@ -1170,7 +1170,7 @@ __acquires(r8a66597->lock)
 
 	switch (ctrl->bRequestType & USB_RECIP_MASK) {
 	case USB_RECIP_DEVICE:
-		status = 1 << USB_DEVICE_SELF_POWERED;
+		status = r8a66597->device_status;
 		break;
 	case USB_RECIP_INTERFACE:
 		status = 0;
@@ -1627,7 +1627,7 @@ static int r8a66597_queue(struct usb_ep *_ep, struct usb_request *_req,
 	req->req.actual = 0;
 	req->req.status = -EINPROGRESS;
 
-	if (ep->desc == NULL)	/* control */
+	if (ep->ep.desc == NULL)	/* control */
 		start_ep0(ep, req);
 	else {
 		if (request && !ep->busy)
@@ -1692,7 +1692,7 @@ static int r8a66597_set_wedge(struct usb_ep *_ep)
 
 	ep = container_of(_ep, struct r8a66597_ep, ep);
 
-	if (!ep || !ep->desc)
+	if (!ep || !ep->ep.desc)
 		return -EINVAL;
 
 	spin_lock_irqsave(&ep->r8a66597->lock, flags);
@@ -1800,11 +1800,24 @@ static int r8a66597_pullup(struct usb_gadget *gadget, int is_on)
 	return 0;
 }
 
+static int r8a66597_set_selfpowered(struct usb_gadget *gadget, int is_self)
+{
+	struct r8a66597 *r8a66597 = gadget_to_r8a66597(gadget);
+
+	if (is_self)
+		r8a66597->device_status |= 1 << USB_DEVICE_SELF_POWERED;
+	else
+		r8a66597->device_status &= ~(1 << USB_DEVICE_SELF_POWERED);
+
+	return 0;
+}
+
 static struct usb_gadget_ops r8a66597_gadget_ops = {
 	.get_frame		= r8a66597_get_frame,
 	.udc_start		= r8a66597_start,
 	.udc_stop		= r8a66597_stop,
 	.pullup			= r8a66597_pullup,
+	.set_selfpowered	= r8a66597_set_selfpowered,
 };
 
 static int __exit r8a66597_remove(struct platform_device *pdev)
diff --git a/drivers/usb/gadget/r8a66597-udc.h b/drivers/usb/gadget/r8a66597-udc.h
index 8e3de61cd4b..99908c76ccd 100644
--- a/drivers/usb/gadget/r8a66597-udc.h
+++ b/drivers/usb/gadget/r8a66597-udc.h
@@ -72,7 +72,7 @@ struct r8a66597_ep {
 	unsigned		use_dma:1;
 	u16			pipenum;
 	u16			type;
-	const struct usb_endpoint_descriptor	*desc;
+
 	/* register address */
 	unsigned char		fifoaddr;
 	unsigned char		fifosel;
@@ -111,6 +111,7 @@ struct r8a66597 {
 	u16			old_vbus;
 	u16			scount;
 	u16			old_dvsq;
+	u16			device_status;	/* for GET_STATUS */
 
 	/* pipe config */
 	unsigned char bulk;
diff --git a/drivers/usb/gadget/rndis.c b/drivers/usb/gadget/rndis.c
index 73a934a170d..b35babed6fc 100644
--- a/drivers/usb/gadget/rndis.c
+++ b/drivers/usb/gadget/rndis.c
@@ -73,65 +73,65 @@ static rndis_resp_t *rndis_add_response(int configNr, u32 length);
 static const u32 oid_supported_list[] =
 {
 	/* the general stuff */
-	OID_GEN_SUPPORTED_LIST,
-	OID_GEN_HARDWARE_STATUS,
-	OID_GEN_MEDIA_SUPPORTED,
-	OID_GEN_MEDIA_IN_USE,
-	OID_GEN_MAXIMUM_FRAME_SIZE,
-	OID_GEN_LINK_SPEED,
-	OID_GEN_TRANSMIT_BLOCK_SIZE,
-	OID_GEN_RECEIVE_BLOCK_SIZE,
-	OID_GEN_VENDOR_ID,
-	OID_GEN_VENDOR_DESCRIPTION,
-	OID_GEN_VENDOR_DRIVER_VERSION,
-	OID_GEN_CURRENT_PACKET_FILTER,
-	OID_GEN_MAXIMUM_TOTAL_SIZE,
-	OID_GEN_MEDIA_CONNECT_STATUS,
-	OID_GEN_PHYSICAL_MEDIUM,
+	RNDIS_OID_GEN_SUPPORTED_LIST,
+	RNDIS_OID_GEN_HARDWARE_STATUS,
+	RNDIS_OID_GEN_MEDIA_SUPPORTED,
+	RNDIS_OID_GEN_MEDIA_IN_USE,
+	RNDIS_OID_GEN_MAXIMUM_FRAME_SIZE,
+	RNDIS_OID_GEN_LINK_SPEED,
+	RNDIS_OID_GEN_TRANSMIT_BLOCK_SIZE,
+	RNDIS_OID_GEN_RECEIVE_BLOCK_SIZE,
+	RNDIS_OID_GEN_VENDOR_ID,
+	RNDIS_OID_GEN_VENDOR_DESCRIPTION,
+	RNDIS_OID_GEN_VENDOR_DRIVER_VERSION,
+	RNDIS_OID_GEN_CURRENT_PACKET_FILTER,
+	RNDIS_OID_GEN_MAXIMUM_TOTAL_SIZE,
+	RNDIS_OID_GEN_MEDIA_CONNECT_STATUS,
+	RNDIS_OID_GEN_PHYSICAL_MEDIUM,
 
 	/* the statistical stuff */
-	OID_GEN_XMIT_OK,
-	OID_GEN_RCV_OK,
-	OID_GEN_XMIT_ERROR,
-	OID_GEN_RCV_ERROR,
-	OID_GEN_RCV_NO_BUFFER,
+	RNDIS_OID_GEN_XMIT_OK,
+	RNDIS_OID_GEN_RCV_OK,
+	RNDIS_OID_GEN_XMIT_ERROR,
+	RNDIS_OID_GEN_RCV_ERROR,
+	RNDIS_OID_GEN_RCV_NO_BUFFER,
 #ifdef	RNDIS_OPTIONAL_STATS
-	OID_GEN_DIRECTED_BYTES_XMIT,
-	OID_GEN_DIRECTED_FRAMES_XMIT,
-	OID_GEN_MULTICAST_BYTES_XMIT,
-	OID_GEN_MULTICAST_FRAMES_XMIT,
-	OID_GEN_BROADCAST_BYTES_XMIT,
-	OID_GEN_BROADCAST_FRAMES_XMIT,
-	OID_GEN_DIRECTED_BYTES_RCV,
-	OID_GEN_DIRECTED_FRAMES_RCV,
-	OID_GEN_MULTICAST_BYTES_RCV,
-	OID_GEN_MULTICAST_FRAMES_RCV,
-	OID_GEN_BROADCAST_BYTES_RCV,
-	OID_GEN_BROADCAST_FRAMES_RCV,
-	OID_GEN_RCV_CRC_ERROR,
-	OID_GEN_TRANSMIT_QUEUE_LENGTH,
+	RNDIS_OID_GEN_DIRECTED_BYTES_XMIT,
+	RNDIS_OID_GEN_DIRECTED_FRAMES_XMIT,
+	RNDIS_OID_GEN_MULTICAST_BYTES_XMIT,
+	RNDIS_OID_GEN_MULTICAST_FRAMES_XMIT,
+	RNDIS_OID_GEN_BROADCAST_BYTES_XMIT,
+	RNDIS_OID_GEN_BROADCAST_FRAMES_XMIT,
+	RNDIS_OID_GEN_DIRECTED_BYTES_RCV,
+	RNDIS_OID_GEN_DIRECTED_FRAMES_RCV,
+	RNDIS_OID_GEN_MULTICAST_BYTES_RCV,
+	RNDIS_OID_GEN_MULTICAST_FRAMES_RCV,
+	RNDIS_OID_GEN_BROADCAST_BYTES_RCV,
+	RNDIS_OID_GEN_BROADCAST_FRAMES_RCV,
+	RNDIS_OID_GEN_RCV_CRC_ERROR,
+	RNDIS_OID_GEN_TRANSMIT_QUEUE_LENGTH,
 #endif	/* RNDIS_OPTIONAL_STATS */
 
 	/* mandatory 802.3 */
 	/* the general stuff */
-	OID_802_3_PERMANENT_ADDRESS,
-	OID_802_3_CURRENT_ADDRESS,
-	OID_802_3_MULTICAST_LIST,
-	OID_802_3_MAC_OPTIONS,
-	OID_802_3_MAXIMUM_LIST_SIZE,
+	RNDIS_OID_802_3_PERMANENT_ADDRESS,
+	RNDIS_OID_802_3_CURRENT_ADDRESS,
+	RNDIS_OID_802_3_MULTICAST_LIST,
+	RNDIS_OID_802_3_MAC_OPTIONS,
+	RNDIS_OID_802_3_MAXIMUM_LIST_SIZE,
 
 	/* the statistical stuff */
-	OID_802_3_RCV_ERROR_ALIGNMENT,
-	OID_802_3_XMIT_ONE_COLLISION,
-	OID_802_3_XMIT_MORE_COLLISIONS,
+	RNDIS_OID_802_3_RCV_ERROR_ALIGNMENT,
+	RNDIS_OID_802_3_XMIT_ONE_COLLISION,
+	RNDIS_OID_802_3_XMIT_MORE_COLLISIONS,
 #ifdef	RNDIS_OPTIONAL_STATS
-	OID_802_3_XMIT_DEFERRED,
-	OID_802_3_XMIT_MAX_COLLISIONS,
-	OID_802_3_RCV_OVERRUN,
-	OID_802_3_XMIT_UNDERRUN,
-	OID_802_3_XMIT_HEARTBEAT_FAILURE,
-	OID_802_3_XMIT_TIMES_CRS_LOST,
-	OID_802_3_XMIT_LATE_COLLISIONS,
+	RNDIS_OID_802_3_XMIT_DEFERRED,
+	RNDIS_OID_802_3_XMIT_MAX_COLLISIONS,
+	RNDIS_OID_802_3_RCV_OVERRUN,
+	RNDIS_OID_802_3_XMIT_UNDERRUN,
+	RNDIS_OID_802_3_XMIT_HEARTBEAT_FAILURE,
+	RNDIS_OID_802_3_XMIT_TIMES_CRS_LOST,
+	RNDIS_OID_802_3_XMIT_LATE_COLLISIONS,
 #endif	/* RNDIS_OPTIONAL_STATS */
 
 #ifdef	RNDIS_PM
@@ -200,8 +200,8 @@ static int gen_ndis_query_resp(int configNr, u32 OID, u8 *buf,
 	/* general oids (table 4-1) */
 
 	/* mandatory */
-	case OID_GEN_SUPPORTED_LIST:
-		pr_debug("%s: OID_GEN_SUPPORTED_LIST\n", __func__);
+	case RNDIS_OID_GEN_SUPPORTED_LIST:
+		pr_debug("%s: RNDIS_OID_GEN_SUPPORTED_LIST\n", __func__);
 		length = sizeof(oid_supported_list);
 		count  = length / sizeof(u32);
 		for (i = 0; i < count; i++)
@@ -210,8 +210,8 @@ static int gen_ndis_query_resp(int configNr, u32 OID, u8 *buf,
 		break;
 
 	/* mandatory */
-	case OID_GEN_HARDWARE_STATUS:
-		pr_debug("%s: OID_GEN_HARDWARE_STATUS\n", __func__);
+	case RNDIS_OID_GEN_HARDWARE_STATUS:
+		pr_debug("%s: RNDIS_OID_GEN_HARDWARE_STATUS\n", __func__);
 		/* Bogus question!
 		 * Hardware must be ready to receive high level protocols.
 		 * BTW:
@@ -223,23 +223,23 @@ static int gen_ndis_query_resp(int configNr, u32 OID, u8 *buf,
 		break;
 
 	/* mandatory */
-	case OID_GEN_MEDIA_SUPPORTED:
-		pr_debug("%s: OID_GEN_MEDIA_SUPPORTED\n", __func__);
+	case RNDIS_OID_GEN_MEDIA_SUPPORTED:
+		pr_debug("%s: RNDIS_OID_GEN_MEDIA_SUPPORTED\n", __func__);
 		*outbuf = cpu_to_le32(rndis_per_dev_params[configNr].medium);
 		retval = 0;
 		break;
 
 	/* mandatory */
-	case OID_GEN_MEDIA_IN_USE:
-		pr_debug("%s: OID_GEN_MEDIA_IN_USE\n", __func__);
+	case RNDIS_OID_GEN_MEDIA_IN_USE:
+		pr_debug("%s: RNDIS_OID_GEN_MEDIA_IN_USE\n", __func__);
 		/* one medium, one transport... (maybe you do it better) */
 		*outbuf = cpu_to_le32(rndis_per_dev_params[configNr].medium);
 		retval = 0;
 		break;
 
 	/* mandatory */
-	case OID_GEN_MAXIMUM_FRAME_SIZE:
-		pr_debug("%s: OID_GEN_MAXIMUM_FRAME_SIZE\n", __func__);
+	case RNDIS_OID_GEN_MAXIMUM_FRAME_SIZE:
+		pr_debug("%s: RNDIS_OID_GEN_MAXIMUM_FRAME_SIZE\n", __func__);
 		if (rndis_per_dev_params[configNr].dev) {
 			*outbuf = cpu_to_le32(
 				rndis_per_dev_params[configNr].dev->mtu);
@@ -248,11 +248,11 @@ static int gen_ndis_query_resp(int configNr, u32 OID, u8 *buf,
 		break;
 
 	/* mandatory */
-	case OID_GEN_LINK_SPEED:
+	case RNDIS_OID_GEN_LINK_SPEED:
 		if (rndis_debug > 1)
-			pr_debug("%s: OID_GEN_LINK_SPEED\n", __func__);
+			pr_debug("%s: RNDIS_OID_GEN_LINK_SPEED\n", __func__);
 		if (rndis_per_dev_params[configNr].media_state
-				== NDIS_MEDIA_STATE_DISCONNECTED)
+				== RNDIS_MEDIA_STATE_DISCONNECTED)
 			*outbuf = cpu_to_le32(0);
 		else
 			*outbuf = cpu_to_le32(
@@ -261,8 +261,8 @@ static int gen_ndis_query_resp(int configNr, u32 OID, u8 *buf,
 		break;
 
 	/* mandatory */
-	case OID_GEN_TRANSMIT_BLOCK_SIZE:
-		pr_debug("%s: OID_GEN_TRANSMIT_BLOCK_SIZE\n", __func__);
+	case RNDIS_OID_GEN_TRANSMIT_BLOCK_SIZE:
+		pr_debug("%s: RNDIS_OID_GEN_TRANSMIT_BLOCK_SIZE\n", __func__);
 		if (rndis_per_dev_params[configNr].dev) {
 			*outbuf = cpu_to_le32(
 				rndis_per_dev_params[configNr].dev->mtu);
@@ -271,8 +271,8 @@ static int gen_ndis_query_resp(int configNr, u32 OID, u8 *buf,
 		break;
 
 	/* mandatory */
-	case OID_GEN_RECEIVE_BLOCK_SIZE:
-		pr_debug("%s: OID_GEN_RECEIVE_BLOCK_SIZE\n", __func__);
+	case RNDIS_OID_GEN_RECEIVE_BLOCK_SIZE:
+		pr_debug("%s: RNDIS_OID_GEN_RECEIVE_BLOCK_SIZE\n", __func__);
 		if (rndis_per_dev_params[configNr].dev) {
 			*outbuf = cpu_to_le32(
 				rndis_per_dev_params[configNr].dev->mtu);
@@ -281,16 +281,16 @@ static int gen_ndis_query_resp(int configNr, u32 OID, u8 *buf,
 		break;
 
 	/* mandatory */
-	case OID_GEN_VENDOR_ID:
-		pr_debug("%s: OID_GEN_VENDOR_ID\n", __func__);
+	case RNDIS_OID_GEN_VENDOR_ID:
+		pr_debug("%s: RNDIS_OID_GEN_VENDOR_ID\n", __func__);
 		*outbuf = cpu_to_le32(
 			rndis_per_dev_params[configNr].vendorID);
 		retval = 0;
 		break;
 
 	/* mandatory */
-	case OID_GEN_VENDOR_DESCRIPTION:
-		pr_debug("%s: OID_GEN_VENDOR_DESCRIPTION\n", __func__);
+	case RNDIS_OID_GEN_VENDOR_DESCRIPTION:
+		pr_debug("%s: RNDIS_OID_GEN_VENDOR_DESCRIPTION\n", __func__);
 		if (rndis_per_dev_params[configNr].vendorDescr) {
 			length = strlen(rndis_per_dev_params[configNr].
 					vendorDescr);
@@ -303,38 +303,38 @@ static int gen_ndis_query_resp(int configNr, u32 OID, u8 *buf,
 		retval = 0;
 		break;
 
-	case OID_GEN_VENDOR_DRIVER_VERSION:
-		pr_debug("%s: OID_GEN_VENDOR_DRIVER_VERSION\n", __func__);
+	case RNDIS_OID_GEN_VENDOR_DRIVER_VERSION:
+		pr_debug("%s: RNDIS_OID_GEN_VENDOR_DRIVER_VERSION\n", __func__);
 		/* Created as LE */
 		*outbuf = rndis_driver_version;
 		retval = 0;
 		break;
 
 	/* mandatory */
-	case OID_GEN_CURRENT_PACKET_FILTER:
-		pr_debug("%s: OID_GEN_CURRENT_PACKET_FILTER\n", __func__);
+	case RNDIS_OID_GEN_CURRENT_PACKET_FILTER:
+		pr_debug("%s: RNDIS_OID_GEN_CURRENT_PACKET_FILTER\n", __func__);
 		*outbuf = cpu_to_le32(*rndis_per_dev_params[configNr].filter);
 		retval = 0;
 		break;
 
 	/* mandatory */
-	case OID_GEN_MAXIMUM_TOTAL_SIZE:
-		pr_debug("%s: OID_GEN_MAXIMUM_TOTAL_SIZE\n", __func__);
+	case RNDIS_OID_GEN_MAXIMUM_TOTAL_SIZE:
+		pr_debug("%s: RNDIS_OID_GEN_MAXIMUM_TOTAL_SIZE\n", __func__);
 		*outbuf = cpu_to_le32(RNDIS_MAX_TOTAL_SIZE);
 		retval = 0;
 		break;
 
 	/* mandatory */
-	case OID_GEN_MEDIA_CONNECT_STATUS:
+	case RNDIS_OID_GEN_MEDIA_CONNECT_STATUS:
 		if (rndis_debug > 1)
-			pr_debug("%s: OID_GEN_MEDIA_CONNECT_STATUS\n", __func__);
+			pr_debug("%s: RNDIS_OID_GEN_MEDIA_CONNECT_STATUS\n", __func__);
 		*outbuf = cpu_to_le32(rndis_per_dev_params[configNr]
 						.media_state);
 		retval = 0;
 		break;
 
-	case OID_GEN_PHYSICAL_MEDIUM:
-		pr_debug("%s: OID_GEN_PHYSICAL_MEDIUM\n", __func__);
+	case RNDIS_OID_GEN_PHYSICAL_MEDIUM:
+		pr_debug("%s: RNDIS_OID_GEN_PHYSICAL_MEDIUM\n", __func__);
 		*outbuf = cpu_to_le32(0);
 		retval = 0;
 		break;
@@ -343,20 +343,20 @@ static int gen_ndis_query_resp(int configNr, u32 OID, u8 *buf,
 	 * of MS-Windows expect OIDs that aren't specified there.  Other
 	 * versions emit undefined RNDIS messages. DOCUMENT ALL THESE!
 	 */
-	case OID_GEN_MAC_OPTIONS:		/* from WinME */
-		pr_debug("%s: OID_GEN_MAC_OPTIONS\n", __func__);
+	case RNDIS_OID_GEN_MAC_OPTIONS:		/* from WinME */
+		pr_debug("%s: RNDIS_OID_GEN_MAC_OPTIONS\n", __func__);
 		*outbuf = cpu_to_le32(
-			  NDIS_MAC_OPTION_RECEIVE_SERIALIZED
-			| NDIS_MAC_OPTION_FULL_DUPLEX);
+			  RNDIS_MAC_OPTION_RECEIVE_SERIALIZED
+			| RNDIS_MAC_OPTION_FULL_DUPLEX);
 		retval = 0;
 		break;
 
 	/* statistics OIDs (table 4-2) */
 
 	/* mandatory */
-	case OID_GEN_XMIT_OK:
+	case RNDIS_OID_GEN_XMIT_OK:
 		if (rndis_debug > 1)
-			pr_debug("%s: OID_GEN_XMIT_OK\n", __func__);
+			pr_debug("%s: RNDIS_OID_GEN_XMIT_OK\n", __func__);
 		if (stats) {
 			*outbuf = cpu_to_le32(stats->tx_packets
 				- stats->tx_errors - stats->tx_dropped);
@@ -365,9 +365,9 @@ static int gen_ndis_query_resp(int configNr, u32 OID, u8 *buf,
 		break;
 
 	/* mandatory */
-	case OID_GEN_RCV_OK:
+	case RNDIS_OID_GEN_RCV_OK:
 		if (rndis_debug > 1)
-			pr_debug("%s: OID_GEN_RCV_OK\n", __func__);
+			pr_debug("%s: RNDIS_OID_GEN_RCV_OK\n", __func__);
 		if (stats) {
 			*outbuf = cpu_to_le32(stats->rx_packets
 				- stats->rx_errors - stats->rx_dropped);
@@ -376,9 +376,9 @@ static int gen_ndis_query_resp(int configNr, u32 OID, u8 *buf,
 		break;
 
 	/* mandatory */
-	case OID_GEN_XMIT_ERROR:
+	case RNDIS_OID_GEN_XMIT_ERROR:
 		if (rndis_debug > 1)
-			pr_debug("%s: OID_GEN_XMIT_ERROR\n", __func__);
+			pr_debug("%s: RNDIS_OID_GEN_XMIT_ERROR\n", __func__);
 		if (stats) {
 			*outbuf = cpu_to_le32(stats->tx_errors);
 			retval = 0;
@@ -386,9 +386,9 @@ static int gen_ndis_query_resp(int configNr, u32 OID, u8 *buf,
 		break;
 
 	/* mandatory */
-	case OID_GEN_RCV_ERROR:
+	case RNDIS_OID_GEN_RCV_ERROR:
 		if (rndis_debug > 1)
-			pr_debug("%s: OID_GEN_RCV_ERROR\n", __func__);
+			pr_debug("%s: RNDIS_OID_GEN_RCV_ERROR\n", __func__);
 		if (stats) {
 			*outbuf = cpu_to_le32(stats->rx_errors);
 			retval = 0;
@@ -396,8 +396,8 @@ static int gen_ndis_query_resp(int configNr, u32 OID, u8 *buf,
 		break;
 
 	/* mandatory */
-	case OID_GEN_RCV_NO_BUFFER:
-		pr_debug("%s: OID_GEN_RCV_NO_BUFFER\n", __func__);
+	case RNDIS_OID_GEN_RCV_NO_BUFFER:
+		pr_debug("%s: RNDIS_OID_GEN_RCV_NO_BUFFER\n", __func__);
 		if (stats) {
 			*outbuf = cpu_to_le32(stats->rx_dropped);
 			retval = 0;
@@ -407,8 +407,8 @@ static int gen_ndis_query_resp(int configNr, u32 OID, u8 *buf,
 	/* ieee802.3 OIDs (table 4-3) */
 
 	/* mandatory */
-	case OID_802_3_PERMANENT_ADDRESS:
-		pr_debug("%s: OID_802_3_PERMANENT_ADDRESS\n", __func__);
+	case RNDIS_OID_802_3_PERMANENT_ADDRESS:
+		pr_debug("%s: RNDIS_OID_802_3_PERMANENT_ADDRESS\n", __func__);
 		if (rndis_per_dev_params[configNr].dev) {
 			length = ETH_ALEN;
 			memcpy(outbuf,
@@ -419,8 +419,8 @@ static int gen_ndis_query_resp(int configNr, u32 OID, u8 *buf,
 		break;
 
 	/* mandatory */
-	case OID_802_3_CURRENT_ADDRESS:
-		pr_debug("%s: OID_802_3_CURRENT_ADDRESS\n", __func__);
+	case RNDIS_OID_802_3_CURRENT_ADDRESS:
+		pr_debug("%s: RNDIS_OID_802_3_CURRENT_ADDRESS\n", __func__);
 		if (rndis_per_dev_params[configNr].dev) {
 			length = ETH_ALEN;
 			memcpy(outbuf,
@@ -431,23 +431,23 @@ static int gen_ndis_query_resp(int configNr, u32 OID, u8 *buf,
 		break;
 
 	/* mandatory */
-	case OID_802_3_MULTICAST_LIST:
-		pr_debug("%s: OID_802_3_MULTICAST_LIST\n", __func__);
+	case RNDIS_OID_802_3_MULTICAST_LIST:
+		pr_debug("%s: RNDIS_OID_802_3_MULTICAST_LIST\n", __func__);
 		/* Multicast base address only */
 		*outbuf = cpu_to_le32(0xE0000000);
 		retval = 0;
 		break;
 
 	/* mandatory */
-	case OID_802_3_MAXIMUM_LIST_SIZE:
-		pr_debug("%s: OID_802_3_MAXIMUM_LIST_SIZE\n", __func__);
+	case RNDIS_OID_802_3_MAXIMUM_LIST_SIZE:
+		pr_debug("%s: RNDIS_OID_802_3_MAXIMUM_LIST_SIZE\n", __func__);
 		/* Multicast base address only */
 		*outbuf = cpu_to_le32(1);
 		retval = 0;
 		break;
 
-	case OID_802_3_MAC_OPTIONS:
-		pr_debug("%s: OID_802_3_MAC_OPTIONS\n", __func__);
+	case RNDIS_OID_802_3_MAC_OPTIONS:
+		pr_debug("%s: RNDIS_OID_802_3_MAC_OPTIONS\n", __func__);
 		*outbuf = cpu_to_le32(0);
 		retval = 0;
 		break;
@@ -455,8 +455,8 @@ static int gen_ndis_query_resp(int configNr, u32 OID, u8 *buf,
 	/* ieee802.3 statistics OIDs (table 4-4) */
 
 	/* mandatory */
-	case OID_802_3_RCV_ERROR_ALIGNMENT:
-		pr_debug("%s: OID_802_3_RCV_ERROR_ALIGNMENT\n", __func__);
+	case RNDIS_OID_802_3_RCV_ERROR_ALIGNMENT:
+		pr_debug("%s: RNDIS_OID_802_3_RCV_ERROR_ALIGNMENT\n", __func__);
 		if (stats) {
 			*outbuf = cpu_to_le32(stats->rx_frame_errors);
 			retval = 0;
@@ -464,15 +464,15 @@ static int gen_ndis_query_resp(int configNr, u32 OID, u8 *buf,
 		break;
 
 	/* mandatory */
-	case OID_802_3_XMIT_ONE_COLLISION:
-		pr_debug("%s: OID_802_3_XMIT_ONE_COLLISION\n", __func__);
+	case RNDIS_OID_802_3_XMIT_ONE_COLLISION:
+		pr_debug("%s: RNDIS_OID_802_3_XMIT_ONE_COLLISION\n", __func__);
 		*outbuf = cpu_to_le32(0);
 		retval = 0;
 		break;
 
 	/* mandatory */
-	case OID_802_3_XMIT_MORE_COLLISIONS:
-		pr_debug("%s: OID_802_3_XMIT_MORE_COLLISIONS\n", __func__);
+	case RNDIS_OID_802_3_XMIT_MORE_COLLISIONS:
+		pr_debug("%s: RNDIS_OID_802_3_XMIT_MORE_COLLISIONS\n", __func__);
 		*outbuf = cpu_to_le32(0);
 		retval = 0;
 		break;
@@ -516,7 +516,7 @@ static int gen_ndis_set_resp(u8 configNr, u32 OID, u8 *buf, u32 buf_len,
 
 	params = &rndis_per_dev_params[configNr];
 	switch (OID) {
-	case OID_GEN_CURRENT_PACKET_FILTER:
+	case RNDIS_OID_GEN_CURRENT_PACKET_FILTER:
 
 		/* these NDIS_PACKET_TYPE_* bitflags are shared with
 		 * cdc_filter; it's not RNDIS-specific
@@ -525,7 +525,7 @@ static int gen_ndis_set_resp(u8 configNr, u32 OID, u8 *buf, u32 buf_len,
 		 *	MULTICAST, ALL_MULTICAST, BROADCAST
 		 */
 		*params->filter = (u16)get_unaligned_le32(buf);
-		pr_debug("%s: OID_GEN_CURRENT_PACKET_FILTER %08x\n",
+		pr_debug("%s: RNDIS_OID_GEN_CURRENT_PACKET_FILTER %08x\n",
 			__func__, *params->filter);
 
 		/* this call has a significant side effect:  it's
@@ -545,9 +545,9 @@ static int gen_ndis_set_resp(u8 configNr, u32 OID, u8 *buf, u32 buf_len,
 		}
 		break;
 
-	case OID_802_3_MULTICAST_LIST:
+	case RNDIS_OID_802_3_MULTICAST_LIST:
 		/* I think we can ignore this */
-		pr_debug("%s: OID_802_3_MULTICAST_LIST\n", __func__);
+		pr_debug("%s: RNDIS_OID_802_3_MULTICAST_LIST\n", __func__);
 		retval = 0;
 		break;
 
@@ -577,7 +577,7 @@ static int rndis_init_response(int configNr, rndis_init_msg_type *buf)
 		return -ENOMEM;
 	resp = (rndis_init_cmplt_type *)r->buf;
 
-	resp->MessageType = cpu_to_le32(REMOTE_NDIS_INITIALIZE_CMPLT);
+	resp->MessageType = cpu_to_le32(RNDIS_MSG_INIT_C);
 	resp->MessageLength = cpu_to_le32(52);
 	resp->RequestID = buf->RequestID; /* Still LE in msg buffer */
 	resp->Status = cpu_to_le32(RNDIS_STATUS_SUCCESS);
@@ -621,7 +621,7 @@ static int rndis_query_response(int configNr, rndis_query_msg_type *buf)
 		return -ENOMEM;
 	resp = (rndis_query_cmplt_type *)r->buf;
 
-	resp->MessageType = cpu_to_le32(REMOTE_NDIS_QUERY_CMPLT);
+	resp->MessageType = cpu_to_le32(RNDIS_MSG_QUERY_C);
 	resp->RequestID = buf->RequestID; /* Still LE in msg buffer */
 
 	if (gen_ndis_query_resp(configNr, le32_to_cpu(buf->OID),
@@ -668,7 +668,7 @@ static int rndis_set_response(int configNr, rndis_set_msg_type *buf)
 	pr_debug("\n");
 #endif
 
-	resp->MessageType = cpu_to_le32(REMOTE_NDIS_SET_CMPLT);
+	resp->MessageType = cpu_to_le32(RNDIS_MSG_SET_C);
 	resp->MessageLength = cpu_to_le32(16);
 	resp->RequestID = buf->RequestID; /* Still LE in msg buffer */
 	if (gen_ndis_set_resp(configNr, le32_to_cpu(buf->OID),
@@ -692,7 +692,7 @@ static int rndis_reset_response(int configNr, rndis_reset_msg_type *buf)
 		return -ENOMEM;
 	resp = (rndis_reset_cmplt_type *)r->buf;
 
-	resp->MessageType = cpu_to_le32(REMOTE_NDIS_RESET_CMPLT);
+	resp->MessageType = cpu_to_le32(RNDIS_MSG_RESET_C);
 	resp->MessageLength = cpu_to_le32(16);
 	resp->Status = cpu_to_le32(RNDIS_STATUS_SUCCESS);
 	/* resent information */
@@ -716,8 +716,7 @@ static int rndis_keepalive_response(int configNr,
 		return -ENOMEM;
 	resp = (rndis_keepalive_cmplt_type *)r->buf;
 
-	resp->MessageType = cpu_to_le32(
-			REMOTE_NDIS_KEEPALIVE_CMPLT);
+	resp->MessageType = cpu_to_le32(RNDIS_MSG_KEEPALIVE_C);
 	resp->MessageLength = cpu_to_le32(16);
 	resp->RequestID = buf->RequestID; /* Still LE in msg buffer */
 	resp->Status = cpu_to_le32(RNDIS_STATUS_SUCCESS);
@@ -745,7 +744,7 @@ static int rndis_indicate_status_msg(int configNr, u32 status)
 		return -ENOMEM;
 	resp = (rndis_indicate_status_msg_type *)r->buf;
 
-	resp->MessageType = cpu_to_le32(REMOTE_NDIS_INDICATE_STATUS_MSG);
+	resp->MessageType = cpu_to_le32(RNDIS_MSG_INDICATE);
 	resp->MessageLength = cpu_to_le32(20);
 	resp->Status = cpu_to_le32(status);
 	resp->StatusBufferLength = cpu_to_le32(0);
@@ -758,7 +757,7 @@ static int rndis_indicate_status_msg(int configNr, u32 status)
 int rndis_signal_connect(int configNr)
 {
 	rndis_per_dev_params[configNr].media_state
-			= NDIS_MEDIA_STATE_CONNECTED;
+			= RNDIS_MEDIA_STATE_CONNECTED;
 	return rndis_indicate_status_msg(configNr,
 					  RNDIS_STATUS_MEDIA_CONNECT);
 }
@@ -766,7 +765,7 @@ int rndis_signal_connect(int configNr)
 int rndis_signal_disconnect(int configNr)
 {
 	rndis_per_dev_params[configNr].media_state
-			= NDIS_MEDIA_STATE_DISCONNECTED;
+			= RNDIS_MEDIA_STATE_DISCONNECTED;
 	return rndis_indicate_status_msg(configNr,
 					  RNDIS_STATUS_MEDIA_DISCONNECT);
 }
@@ -817,15 +816,15 @@ int rndis_msg_parser(u8 configNr, u8 *buf)
 
 	/* For USB: responses may take up to 10 seconds */
 	switch (MsgType) {
-	case REMOTE_NDIS_INITIALIZE_MSG:
-		pr_debug("%s: REMOTE_NDIS_INITIALIZE_MSG\n",
+	case RNDIS_MSG_INIT:
+		pr_debug("%s: RNDIS_MSG_INIT\n",
 			__func__);
 		params->state = RNDIS_INITIALIZED;
 		return rndis_init_response(configNr,
 					(rndis_init_msg_type *)buf);
 
-	case REMOTE_NDIS_HALT_MSG:
-		pr_debug("%s: REMOTE_NDIS_HALT_MSG\n",
+	case RNDIS_MSG_HALT:
+		pr_debug("%s: RNDIS_MSG_HALT\n",
 			__func__);
 		params->state = RNDIS_UNINITIALIZED;
 		if (params->dev) {
@@ -834,24 +833,24 @@ int rndis_msg_parser(u8 configNr, u8 *buf)
 		}
 		return 0;
 
-	case REMOTE_NDIS_QUERY_MSG:
+	case RNDIS_MSG_QUERY:
 		return rndis_query_response(configNr,
 					(rndis_query_msg_type *)buf);
 
-	case REMOTE_NDIS_SET_MSG:
+	case RNDIS_MSG_SET:
 		return rndis_set_response(configNr,
 					(rndis_set_msg_type *)buf);
 
-	case REMOTE_NDIS_RESET_MSG:
-		pr_debug("%s: REMOTE_NDIS_RESET_MSG\n",
+	case RNDIS_MSG_RESET:
+		pr_debug("%s: RNDIS_MSG_RESET\n",
 			__func__);
 		return rndis_reset_response(configNr,
 					(rndis_reset_msg_type *)buf);
 
-	case REMOTE_NDIS_KEEPALIVE_MSG:
+	case RNDIS_MSG_KEEPALIVE:
 		/* For USB: host does this every 5 seconds */
 		if (rndis_debug > 1)
-			pr_debug("%s: REMOTE_NDIS_KEEPALIVE_MSG\n",
+			pr_debug("%s: RNDIS_MSG_KEEPALIVE\n",
 				__func__);
 		return rndis_keepalive_response(configNr,
 						 (rndis_keepalive_msg_type *)
@@ -963,7 +962,7 @@ void rndis_add_hdr(struct sk_buff *skb)
 		return;
 	header = (void *)skb_push(skb, sizeof(*header));
 	memset(header, 0, sizeof *header);
-	header->MessageType = cpu_to_le32(REMOTE_NDIS_PACKET_MSG);
+	header->MessageType = cpu_to_le32(RNDIS_MSG_PACKET);
 	header->MessageLength = cpu_to_le32(skb->len);
 	header->DataOffset = cpu_to_le32(36);
 	header->DataLength = cpu_to_le32(skb->len - sizeof(*header));
@@ -1031,7 +1030,7 @@ int rndis_rm_hdr(struct gether *port,
 	__le32 *tmp = (void *)skb->data;
 
 	/* MessageType, MessageLength */
-	if (cpu_to_le32(REMOTE_NDIS_PACKET_MSG)
+	if (cpu_to_le32(RNDIS_MSG_PACKET)
 			!= get_unaligned(tmp++)) {
 		dev_kfree_skb_any(skb);
 		return -EINVAL;
@@ -1173,7 +1172,7 @@ int rndis_init(void)
 		rndis_per_dev_params[i].used = 0;
 		rndis_per_dev_params[i].state = RNDIS_UNINITIALIZED;
 		rndis_per_dev_params[i].media_state
-				= NDIS_MEDIA_STATE_DISCONNECTED;
+				= RNDIS_MEDIA_STATE_DISCONNECTED;
 		INIT_LIST_HEAD(&(rndis_per_dev_params[i].resp_queue));
 	}
 
diff --git a/drivers/usb/gadget/rndis.h b/drivers/usb/gadget/rndis.h
index 907c3300811..0647f2f34e8 100644
--- a/drivers/usb/gadget/rndis.h
+++ b/drivers/usb/gadget/rndis.h
@@ -15,58 +15,12 @@
 #ifndef _LINUX_RNDIS_H
 #define _LINUX_RNDIS_H
 
+#include <linux/rndis.h>
 #include "ndis.h"
 
 #define RNDIS_MAXIMUM_FRAME_SIZE	1518
 #define RNDIS_MAX_TOTAL_SIZE		1558
 
-/* Remote NDIS Versions */
-#define RNDIS_MAJOR_VERSION		1
-#define RNDIS_MINOR_VERSION		0
-
-/* Status Values */
-#define RNDIS_STATUS_SUCCESS		0x00000000U	/* Success           */
-#define RNDIS_STATUS_FAILURE		0xC0000001U	/* Unspecified error */
-#define RNDIS_STATUS_INVALID_DATA	0xC0010015U	/* Invalid data      */
-#define RNDIS_STATUS_NOT_SUPPORTED	0xC00000BBU	/* Unsupported request */
-#define RNDIS_STATUS_MEDIA_CONNECT	0x4001000BU	/* Device connected  */
-#define RNDIS_STATUS_MEDIA_DISCONNECT	0x4001000CU	/* Device disconnected */
-/* For all not specified status messages:
- * RNDIS_STATUS_Xxx -> NDIS_STATUS_Xxx
- */
-
-/* Message Set for Connectionless (802.3) Devices */
-#define REMOTE_NDIS_PACKET_MSG		0x00000001U
-#define REMOTE_NDIS_INITIALIZE_MSG	0x00000002U	/* Initialize device */
-#define REMOTE_NDIS_HALT_MSG		0x00000003U
-#define REMOTE_NDIS_QUERY_MSG		0x00000004U
-#define REMOTE_NDIS_SET_MSG		0x00000005U
-#define REMOTE_NDIS_RESET_MSG		0x00000006U
-#define REMOTE_NDIS_INDICATE_STATUS_MSG	0x00000007U
-#define REMOTE_NDIS_KEEPALIVE_MSG	0x00000008U
-
-/* Message completion */
-#define REMOTE_NDIS_INITIALIZE_CMPLT	0x80000002U
-#define REMOTE_NDIS_QUERY_CMPLT		0x80000004U
-#define REMOTE_NDIS_SET_CMPLT		0x80000005U
-#define REMOTE_NDIS_RESET_CMPLT		0x80000006U
-#define REMOTE_NDIS_KEEPALIVE_CMPLT	0x80000008U
-
-/* Device Flags */
-#define RNDIS_DF_CONNECTIONLESS		0x00000001U
-#define RNDIS_DF_CONNECTION_ORIENTED	0x00000002U
-
-#define RNDIS_MEDIUM_802_3		0x00000000U
-
-/* from drivers/net/sk98lin/h/skgepnmi.h */
-#define OID_PNP_CAPABILITIES			0xFD010100
-#define OID_PNP_SET_POWER			0xFD010101
-#define OID_PNP_QUERY_POWER			0xFD010102
-#define OID_PNP_ADD_WAKE_UP_PATTERN		0xFD010103
-#define OID_PNP_REMOVE_WAKE_UP_PATTERN		0xFD010104
-#define OID_PNP_ENABLE_WAKE_UP			0xFD010106
-
-
 typedef struct rndis_init_msg_type
 {
 	__le32	MessageType;
diff --git a/drivers/usb/gadget/s3c-hsotg.c b/drivers/usb/gadget/s3c-hsotg.c
index 105b206cd84..f4abb0ed987 100644
--- a/drivers/usb/gadget/s3c-hsotg.c
+++ b/drivers/usb/gadget/s3c-hsotg.c
@@ -1,4 +1,5 @@
-/* linux/drivers/usb/gadget/s3c-hsotg.c
+/**
+ * linux/drivers/usb/gadget/s3c-hsotg.c
  *
  * Copyright (c) 2011 Samsung Electronics Co., Ltd.
  *		http://www.samsung.com
@@ -13,7 +14,7 @@
  * This program is free software; you can redistribute it and/or modify
  * it under the terms of the GNU General Public License version 2 as
  * published by the Free Software Foundation.
-*/
+ */
 
 #include <linux/kernel.h>
 #include <linux/module.h>
@@ -27,21 +28,25 @@
 #include <linux/io.h>
 #include <linux/slab.h>
 #include <linux/clk.h>
+#include <linux/regulator/consumer.h>
 
 #include <linux/usb/ch9.h>
 #include <linux/usb/gadget.h>
+#include <linux/platform_data/s3c-hsotg.h>
 
 #include <mach/map.h>
 
-#include <plat/regs-usb-hsotg-phy.h>
-#include <plat/regs-usb-hsotg.h>
-#include <mach/regs-sys.h>
-#include <plat/udc-hs.h>
-#include <plat/cpu.h>
+#include "s3c-hsotg.h"
 
 #define DMA_ADDR_INVALID (~((dma_addr_t)0))
 
-/* EP0_MPS_LIMIT
+static const char * const s3c_hsotg_supply_names[] = {
+	"vusb_d",		/* digital USB supply, 1.2V */
+	"vusb_a",		/* analog USB supply, 1.1V */
+};
+
+/*
+ * EP0_MPS_LIMIT
  *
  * Unfortunately there seems to be a limit of the amount of data that can
  * be transferred by IN transactions on EP0. This is either 127 bytes or 3
@@ -125,8 +130,6 @@ struct s3c_hsotg_ep {
 	char			name[10];
 };
 
-#define S3C_HSOTG_EPS	(8+1)	/* limit to 9 for the moment */
-
 /**
  * struct s3c_hsotg - driver state.
  * @dev: The parent device supplied to the probe function
@@ -135,7 +138,9 @@ struct s3c_hsotg_ep {
  * @regs: The memory area mapped for accessing registers.
  * @regs_res: The resource that was allocated when claiming register space.
  * @irq: The IRQ number we are using
+ * @supplies: Definition of USB power supplies
  * @dedicated_fifos: Set if the hardware has dedicated IN-EP fifos.
+ * @num_of_eps: Number of available EPs (excluding EP0)
  * @debug_root: root directrory for debugfs.
  * @debug_file: main status file for debugfs.
  * @debug_fifo: FIFO status file for debugfs.
@@ -143,6 +148,8 @@ struct s3c_hsotg_ep {
  * @ep0_buff: Buffer for EP0 reply data, if needed.
  * @ctrl_buff: Buffer for EP0 control requests.
  * @ctrl_req: Request for EP0 control packets.
+ * @setup: NAK management for EP0 SETUP
+ * @last_rst: Time of last reset
  * @eps: The endpoints being supplied to the gadget framework
  */
 struct s3c_hsotg {
@@ -155,7 +162,10 @@ struct s3c_hsotg {
 	int			irq;
 	struct clk		*clk;
 
+	struct regulator_bulk_data supplies[ARRAY_SIZE(s3c_hsotg_supply_names)];
+
 	unsigned int		dedicated_fifos:1;
+	unsigned char           num_of_eps;
 
 	struct dentry		*debug_root;
 	struct dentry		*debug_file;
@@ -167,7 +177,9 @@ struct s3c_hsotg {
 	u8			ctrl_buff[8];
 
 	struct usb_gadget	gadget;
-	struct s3c_hsotg_ep	eps[];
+	unsigned int		setup;
+	unsigned long           last_rst;
+	struct s3c_hsotg_ep	*eps;
 };
 
 /**
@@ -244,14 +256,14 @@ static inline bool using_dma(struct s3c_hsotg *hsotg)
  */
 static void s3c_hsotg_en_gsint(struct s3c_hsotg *hsotg, u32 ints)
 {
-	u32 gsintmsk = readl(hsotg->regs + S3C_GINTMSK);
+	u32 gsintmsk = readl(hsotg->regs + GINTMSK);
 	u32 new_gsintmsk;
 
 	new_gsintmsk = gsintmsk | ints;
 
 	if (new_gsintmsk != gsintmsk) {
 		dev_dbg(hsotg->dev, "gsintmsk now 0x%08x\n", new_gsintmsk);
-		writel(new_gsintmsk, hsotg->regs + S3C_GINTMSK);
+		writel(new_gsintmsk, hsotg->regs + GINTMSK);
 	}
 }
 
@@ -262,13 +274,13 @@ static void s3c_hsotg_en_gsint(struct s3c_hsotg *hsotg, u32 ints)
  */
 static void s3c_hsotg_disable_gsint(struct s3c_hsotg *hsotg, u32 ints)
 {
-	u32 gsintmsk = readl(hsotg->regs + S3C_GINTMSK);
+	u32 gsintmsk = readl(hsotg->regs + GINTMSK);
 	u32 new_gsintmsk;
 
 	new_gsintmsk = gsintmsk & ~ints;
 
 	if (new_gsintmsk != gsintmsk)
-		writel(new_gsintmsk, hsotg->regs + S3C_GINTMSK);
+		writel(new_gsintmsk, hsotg->regs + GINTMSK);
 }
 
 /**
@@ -293,12 +305,12 @@ static void s3c_hsotg_ctrl_epint(struct s3c_hsotg *hsotg,
 		bit <<= 16;
 
 	local_irq_save(flags);
-	daint = readl(hsotg->regs + S3C_DAINTMSK);
+	daint = readl(hsotg->regs + DAINTMSK);
 	if (en)
 		daint |= bit;
 	else
 		daint &= ~bit;
-	writel(daint, hsotg->regs + S3C_DAINTMSK);
+	writel(daint, hsotg->regs + DAINTMSK);
 	local_irq_restore(flags);
 }
 
@@ -314,52 +326,51 @@ static void s3c_hsotg_init_fifo(struct s3c_hsotg *hsotg)
 	int timeout;
 	u32 val;
 
-	/* the ryu 2.6.24 release ahs
-	   writel(0x1C0, hsotg->regs + S3C_GRXFSIZ);
-	   writel(S3C_GNPTXFSIZ_NPTxFStAddr(0x200) |
-		S3C_GNPTXFSIZ_NPTxFDep(0x1C0),
-		hsotg->regs + S3C_GNPTXFSIZ);
-	*/
-
 	/* set FIFO sizes to 2048/1024 */
 
-	writel(2048, hsotg->regs + S3C_GRXFSIZ);
-	writel(S3C_GNPTXFSIZ_NPTxFStAddr(2048) |
-	       S3C_GNPTXFSIZ_NPTxFDep(1024),
-	       hsotg->regs + S3C_GNPTXFSIZ);
+	writel(2048, hsotg->regs + GRXFSIZ);
+	writel(GNPTXFSIZ_NPTxFStAddr(2048) |
+	       GNPTXFSIZ_NPTxFDep(1024),
+	       hsotg->regs + GNPTXFSIZ);
 
-	/* arange all the rest of the TX FIFOs, as some versions of this
+	/*
+	 * arange all the rest of the TX FIFOs, as some versions of this
 	 * block have overlapping default addresses. This also ensures
 	 * that if the settings have been changed, then they are set to
-	 * known values. */
+	 * known values.
+	 */
 
 	/* start at the end of the GNPTXFSIZ, rounded up */
 	addr = 2048 + 1024;
 	size = 768;
 
-	/* currently we allocate TX FIFOs for all possible endpoints,
-	 * and assume that they are all the same size. */
+	/*
+	 * currently we allocate TX FIFOs for all possible endpoints,
+	 * and assume that they are all the same size.
+	 */
 
 	for (ep = 1; ep <= 15; ep++) {
 		val = addr;
-		val |= size << S3C_DPTXFSIZn_DPTxFSize_SHIFT;
+		val |= size << DPTXFSIZn_DPTxFSize_SHIFT;
 		addr += size;
 
-		writel(val, hsotg->regs + S3C_DPTXFSIZn(ep));
+		writel(val, hsotg->regs + DPTXFSIZn(ep));
 	}
 
-	/* according to p428 of the design guide, we need to ensure that
-	 * all fifos are flushed before continuing */
+	/*
+	 * according to p428 of the design guide, we need to ensure that
+	 * all fifos are flushed before continuing
+	 */
 
-	writel(S3C_GRSTCTL_TxFNum(0x10) | S3C_GRSTCTL_TxFFlsh |
-	       S3C_GRSTCTL_RxFFlsh, hsotg->regs + S3C_GRSTCTL);
+	writel(GRSTCTL_TxFNum(0x10) | GRSTCTL_TxFFlsh |
+	       GRSTCTL_RxFFlsh, hsotg->regs + GRSTCTL);
 
 	/* wait until the fifos are both flushed */
 	timeout = 100;
 	while (1) {
-		val = readl(hsotg->regs + S3C_GRSTCTL);
+		val = readl(hsotg->regs + GRSTCTL);
 
-		if ((val & (S3C_GRSTCTL_TxFFlsh | S3C_GRSTCTL_RxFFlsh)) == 0)
+		if ((val & (GRSTCTL_TxFFlsh | GRSTCTL_RxFFlsh)) == 0)
 			break;
 
 		if (--timeout == 0) {
@@ -415,7 +426,7 @@ static inline int is_ep_periodic(struct s3c_hsotg_ep *hs_ep)
  *
  * This is the reverse of s3c_hsotg_map_dma(), called for the completion
  * of a request to ensure the buffer is ready for access by the caller.
-*/
+ */
 static void s3c_hsotg_unmap_dma(struct s3c_hsotg *hsotg,
 				struct s3c_hsotg_ep *hs_ep,
 				struct s3c_hsotg_req *hs_req)
@@ -456,13 +467,13 @@ static void s3c_hsotg_unmap_dma(struct s3c_hsotg *hsotg,
  * otherwise -ENOSPC is returned if the FIFO space was used up.
  *
  * This routine is only needed for PIO
-*/
+ */
 static int s3c_hsotg_write_fifo(struct s3c_hsotg *hsotg,
 				struct s3c_hsotg_ep *hs_ep,
 				struct s3c_hsotg_req *hs_req)
 {
 	bool periodic = is_ep_periodic(hs_ep);
-	u32 gnptxsts = readl(hsotg->regs + S3C_GNPTXSTS);
+	u32 gnptxsts = readl(hsotg->regs + GNPTXSTS);
 	int buf_pos = hs_req->req.actual;
 	int to_write = hs_ep->size_loaded;
 	void *data;
@@ -476,20 +487,23 @@ static int s3c_hsotg_write_fifo(struct s3c_hsotg *hsotg,
 		return 0;
 
 	if (periodic && !hsotg->dedicated_fifos) {
-		u32 epsize = readl(hsotg->regs + S3C_DIEPTSIZ(hs_ep->index));
+		u32 epsize = readl(hsotg->regs + DIEPTSIZ(hs_ep->index));
 		int size_left;
 		int size_done;
 
-		/* work out how much data was loaded so we can calculate
-		 * how much data is left in the fifo. */
+		/*
+		 * work out how much data was loaded so we can calculate
+		 * how much data is left in the fifo.
+		 */
 
-		size_left = S3C_DxEPTSIZ_XferSize_GET(epsize);
+		size_left = DxEPTSIZ_XferSize_GET(epsize);
 
-		/* if shared fifo, we cannot write anything until the
+		/*
+		 * if shared fifo, we cannot write anything until the
 		 * previous data has been completely sent.
 		 */
 		if (hs_ep->fifo_load != 0) {
-			s3c_hsotg_en_gsint(hsotg, S3C_GINTSTS_PTxFEmp);
+			s3c_hsotg_en_gsint(hsotg, GINTSTS_PTxFEmp);
 			return -ENOSPC;
 		}
 
@@ -510,47 +524,50 @@ static int s3c_hsotg_write_fifo(struct s3c_hsotg *hsotg,
 			__func__, can_write);
 
 		if (can_write <= 0) {
-			s3c_hsotg_en_gsint(hsotg, S3C_GINTSTS_PTxFEmp);
+			s3c_hsotg_en_gsint(hsotg, GINTSTS_PTxFEmp);
 			return -ENOSPC;
 		}
 	} else if (hsotg->dedicated_fifos && hs_ep->index != 0) {
-		can_write = readl(hsotg->regs + S3C_DTXFSTS(hs_ep->index));
+		can_write = readl(hsotg->regs + DTXFSTS(hs_ep->index));
 
 		can_write &= 0xffff;
 		can_write *= 4;
 	} else {
-		if (S3C_GNPTXSTS_NPTxQSpcAvail_GET(gnptxsts) == 0) {
+		if (GNPTXSTS_NPTxQSpcAvail_GET(gnptxsts) == 0) {
 			dev_dbg(hsotg->dev,
 				"%s: no queue slots available (0x%08x)\n",
 				__func__, gnptxsts);
 
-			s3c_hsotg_en_gsint(hsotg, S3C_GINTSTS_NPTxFEmp);
+			s3c_hsotg_en_gsint(hsotg, GINTSTS_NPTxFEmp);
 			return -ENOSPC;
 		}
 
-		can_write = S3C_GNPTXSTS_NPTxFSpcAvail_GET(gnptxsts);
+		can_write = GNPTXSTS_NPTxFSpcAvail_GET(gnptxsts);
 		can_write *= 4;	/* fifo size is in 32bit quantities. */
 	}
 
 	dev_dbg(hsotg->dev, "%s: GNPTXSTS=%08x, can=%d, to=%d, mps %d\n",
 		 __func__, gnptxsts, can_write, to_write, hs_ep->ep.maxpacket);
 
-	/* limit to 512 bytes of data, it seems at least on the non-periodic
+	/*
+	 * limit to 512 bytes of data, it seems at least on the non-periodic
 	 * FIFO, requests of >512 cause the endpoint to get stuck with a
 	 * fragment of the end of the transfer in it.
 	 */
 	if (can_write > 512)
 		can_write = 512;
 
-	/* limit the write to one max-packet size worth of data, but allow
+	/*
+	 * limit the write to one max-packet size worth of data, but allow
 	 * the transfer to return that it did not run out of fifo space
-	 * doing it. */
+	 * doing it.
+	 */
 	if (to_write > hs_ep->ep.maxpacket) {
 		to_write = hs_ep->ep.maxpacket;
 
 		s3c_hsotg_en_gsint(hsotg,
-				   periodic ? S3C_GINTSTS_PTxFEmp :
-				   S3C_GINTSTS_NPTxFEmp);
+				   periodic ? GINTSTS_PTxFEmp :
+				   GINTSTS_NPTxFEmp);
 	}
 
 	/* see if we can write data */
@@ -559,8 +576,8 @@ static int s3c_hsotg_write_fifo(struct s3c_hsotg *hsotg,
 		to_write = can_write;
 		pkt_round = to_write % hs_ep->ep.maxpacket;
 
-		/* Not sure, but we probably shouldn't be writing partial
-		 * packets into the FIFO, so round the write down to an
+		/*
+		 * Round the write down to an
 		 * exact number of packets.
 		 *
 		 * Note, we do not currently check to see if we can ever
@@ -570,12 +587,14 @@ static int s3c_hsotg_write_fifo(struct s3c_hsotg *hsotg,
 		if (pkt_round)
 			to_write -= pkt_round;
 
-		/* enable correct FIFO interrupt to alert us when there
-		 * is more room left. */
+		/*
+		 * enable correct FIFO interrupt to alert us when there
+		 * is more room left.
+		 */
 
 		s3c_hsotg_en_gsint(hsotg,
-				   periodic ? S3C_GINTSTS_PTxFEmp :
-				   S3C_GINTSTS_NPTxFEmp);
+				   periodic ? GINTSTS_PTxFEmp :
+				   GINTSTS_NPTxFEmp);
 	}
 
 	dev_dbg(hsotg->dev, "write %d/%d, can_write %d, done %d\n",
@@ -593,7 +612,7 @@ static int s3c_hsotg_write_fifo(struct s3c_hsotg *hsotg,
 	to_write = DIV_ROUND_UP(to_write, 4);
 	data = hs_req->req.buf + buf_pos;
 
-	writesl(hsotg->regs + S3C_EPFIFO(hs_ep->index), data, to_write);
+	writesl(hsotg->regs + EPFIFO(hs_ep->index), data, to_write);
 
 	return (to_write >= can_write) ? -ENOSPC : 0;
 }
@@ -612,12 +631,12 @@ static unsigned get_ep_limit(struct s3c_hsotg_ep *hs_ep)
 	unsigned maxpkt;
 
 	if (index != 0) {
-		maxsize = S3C_DxEPTSIZ_XferSize_LIMIT + 1;
-		maxpkt = S3C_DxEPTSIZ_PktCnt_LIMIT + 1;
+		maxsize = DxEPTSIZ_XferSize_LIMIT + 1;
+		maxpkt = DxEPTSIZ_PktCnt_LIMIT + 1;
 	} else {
 		maxsize = 64+64;
 		if (hs_ep->dir_in)
-			maxpkt = S3C_DIEPTSIZ0_PktCnt_LIMIT + 1;
+			maxpkt = DIEPTSIZ0_PktCnt_LIMIT + 1;
 		else
 			maxpkt = 2;
 	}
@@ -626,8 +645,10 @@ static unsigned get_ep_limit(struct s3c_hsotg_ep *hs_ep)
 	maxpkt--;
 	maxsize--;
 
-	/* constrain by packet count if maxpkts*pktsize is greater
-	 * than the length register size. */
+	/*
+	 * constrain by packet count if maxpkts*pktsize is greater
+	 * than the length register size.
+	 */
 
 	if ((maxpkt * hs_ep->ep.maxpacket) < maxsize)
 		maxsize = maxpkt * hs_ep->ep.maxpacket;
@@ -674,8 +695,8 @@ static void s3c_hsotg_start_req(struct s3c_hsotg *hsotg,
 		}
 	}
 
-	epctrl_reg = dir_in ? S3C_DIEPCTL(index) : S3C_DOEPCTL(index);
-	epsize_reg = dir_in ? S3C_DIEPTSIZ(index) : S3C_DOEPTSIZ(index);
+	epctrl_reg = dir_in ? DIEPCTL(index) : DOEPCTL(index);
+	epsize_reg = dir_in ? DIEPTSIZ(index) : DOEPTSIZ(index);
 
 	dev_dbg(hsotg->dev, "%s: DxEPCTL=0x%08x, ep %d, dir %s\n",
 		__func__, readl(hsotg->regs + epctrl_reg), index,
@@ -684,13 +705,14 @@ static void s3c_hsotg_start_req(struct s3c_hsotg *hsotg,
 	/* If endpoint is stalled, we will restart request later */
 	ctrl = readl(hsotg->regs + epctrl_reg);
 
-	if (ctrl & S3C_DxEPCTL_Stall) {
+	if (ctrl & DxEPCTL_Stall) {
 		dev_warn(hsotg->dev, "%s: ep%d is stalled\n", __func__, index);
 		return;
 	}
 
 	length = ureq->length - ureq->actual;
-
+	dev_dbg(hsotg->dev, "ureq->length:%d ureq->actual:%d\n",
+		ureq->length, ureq->actual);
 	if (0)
 		dev_dbg(hsotg->dev,
 			"REQ buf %p len %d dma 0x%08x noi=%d zp=%d snok=%d\n",
@@ -717,20 +739,22 @@ static void s3c_hsotg_start_req(struct s3c_hsotg *hsotg,
 		packets = 1;	/* send one packet if length is zero. */
 
 	if (dir_in && index != 0)
-		epsize = S3C_DxEPTSIZ_MC(1);
+		epsize = DxEPTSIZ_MC(1);
 	else
 		epsize = 0;
 
 	if (index != 0 && ureq->zero) {
-		/* test for the packets being exactly right for the
-		 * transfer */
+		/*
+		 * test for the packets being exactly right for the
+		 * transfer
+		 */
 
 		if (length == (packets * hs_ep->ep.maxpacket))
 			packets++;
 	}
 
-	epsize |= S3C_DxEPTSIZ_PktCnt(packets);
-	epsize |= S3C_DxEPTSIZ_XferSize(length);
+	epsize |= DxEPTSIZ_PktCnt(packets);
+	epsize |= DxEPTSIZ_XferSize(length);
 
 	dev_dbg(hsotg->dev, "%s: %d@%d/%d, 0x%08x => 0x%08x\n",
 		__func__, packets, length, ureq->length, epsize, epsize_reg);
@@ -744,26 +768,38 @@ static void s3c_hsotg_start_req(struct s3c_hsotg *hsotg,
 	if (using_dma(hsotg) && !continuing) {
 		unsigned int dma_reg;
 
-		/* write DMA address to control register, buffer already
-		 * synced by s3c_hsotg_ep_queue().  */
+		/*
+		 * write DMA address to control register, buffer already
+		 * synced by s3c_hsotg_ep_queue().
+		 */
 
-		dma_reg = dir_in ? S3C_DIEPDMA(index) : S3C_DOEPDMA(index);
+		dma_reg = dir_in ? DIEPDMA(index) : DOEPDMA(index);
 		writel(ureq->dma, hsotg->regs + dma_reg);
 
 		dev_dbg(hsotg->dev, "%s: 0x%08x => 0x%08x\n",
 			__func__, ureq->dma, dma_reg);
 	}
 
-	ctrl |= S3C_DxEPCTL_EPEna;	/* ensure ep enabled */
-	ctrl |= S3C_DxEPCTL_USBActEp;
-	ctrl |= S3C_DxEPCTL_CNAK;	/* clear NAK set by core */
+	ctrl |= DxEPCTL_EPEna;	/* ensure ep enabled */
+	ctrl |= DxEPCTL_USBActEp;
+
+	dev_dbg(hsotg->dev, "setup req:%d\n", hsotg->setup);
+
+	/* For Setup request do not clear NAK */
+	if (hsotg->setup && index == 0)
+		hsotg->setup = 0;
+	else
+		ctrl |= DxEPCTL_CNAK;	/* clear NAK set by core */
+
 
 	dev_dbg(hsotg->dev, "%s: DxEPCTL=0x%08x\n", __func__, ctrl);
 	writel(ctrl, hsotg->regs + epctrl_reg);
 
-	/* set these, it seems that DMA support increments past the end
+	/*
+	 * set these, it seems that DMA support increments past the end
 	 * of the packet buffer so we need to calculate the length from
-	 * this information. */
+	 * this information.
+	 */
 	hs_ep->size_loaded = length;
 	hs_ep->last_load = ureq->actual;
 
@@ -774,17 +810,21 @@ static void s3c_hsotg_start_req(struct s3c_hsotg *hsotg,
 		s3c_hsotg_write_fifo(hsotg, hs_ep, hs_req);
 	}
 
-	/* clear the INTknTXFEmpMsk when we start request, more as a aide
-	 * to debugging to see what is going on. */
+	/*
+	 * clear the INTknTXFEmpMsk when we start request, more as a aide
+	 * to debugging to see what is going on.
+	 */
 	if (dir_in)
-		writel(S3C_DIEPMSK_INTknTXFEmpMsk,
-		       hsotg->regs + S3C_DIEPINT(index));
+		writel(DIEPMSK_INTknTXFEmpMsk,
+		       hsotg->regs + DIEPINT(index));
 
-	/* Note, trying to clear the NAK here causes problems with transmit
-	 * on the S3C6400 ending up with the TXFIFO becoming full. */
+	/*
+	 * Note, trying to clear the NAK here causes problems with transmit
+	 * on the S3C6400 ending up with the TXFIFO becoming full.
+	 */
 
 	/* check ep is enabled */
-	if (!(readl(hsotg->regs + epctrl_reg) & S3C_DxEPCTL_EPEna))
+	if (!(readl(hsotg->regs + epctrl_reg) & DxEPCTL_EPEna))
 		dev_warn(hsotg->dev,
 			 "ep%d: failed to become enabled (DxEPCTL=0x%08x)?\n",
 			 index, readl(hsotg->regs + epctrl_reg));
@@ -804,7 +844,7 @@ static void s3c_hsotg_start_req(struct s3c_hsotg *hsotg,
  * then ensure the buffer has been synced to memory. If our buffer has no
  * DMA memory, then we map the memory and mark our request to allow us to
  * cleanup on completion.
-*/
+ */
 static int s3c_hsotg_map_dma(struct s3c_hsotg *hsotg,
 			     struct s3c_hsotg_ep *hs_ep,
 			     struct usb_request *req)
@@ -922,7 +962,7 @@ static void s3c_hsotg_complete_oursetup(struct usb_ep *ep,
  *
  * Convert the given wIndex into a pointer to an driver endpoint
  * structure, or return NULL if it is not a valid endpoint.
-*/
+ */
 static struct s3c_hsotg_ep *ep_from_windex(struct s3c_hsotg *hsotg,
 					   u32 windex)
 {
@@ -933,7 +973,7 @@ static struct s3c_hsotg_ep *ep_from_windex(struct s3c_hsotg *hsotg,
 	if (windex >= 0x100)
 		return NULL;
 
-	if (idx > S3C_HSOTG_EPS)
+	if (idx > hsotg->num_of_eps)
 		return NULL;
 
 	if (idx && ep->dir_in != dir)
@@ -1151,24 +1191,28 @@ static void s3c_hsotg_process_control(struct s3c_hsotg *hsotg,
 		 ctrl->bRequest, ctrl->bRequestType,
 		 ctrl->wValue, ctrl->wLength);
 
-	/* record the direction of the request, for later use when enquing
-	 * packets onto EP0. */
+	/*
+	 * record the direction of the request, for later use when enquing
+	 * packets onto EP0.
+	 */
 
 	ep0->dir_in = (ctrl->bRequestType & USB_DIR_IN) ? 1 : 0;
 	dev_dbg(hsotg->dev, "ctrl: dir_in=%d\n", ep0->dir_in);
 
-	/* if we've no data with this request, then the last part of the
-	 * transaction is going to implicitly be IN. */
+	/*
+	 * if we've no data with this request, then the last part of the
+	 * transaction is going to implicitly be IN.
+	 */
 	if (ctrl->wLength == 0)
 		ep0->dir_in = 1;
 
 	if ((ctrl->bRequestType & USB_TYPE_MASK) == USB_TYPE_STANDARD) {
 		switch (ctrl->bRequest) {
 		case USB_REQ_SET_ADDRESS:
-			dcfg = readl(hsotg->regs + S3C_DCFG);
-			dcfg &= ~S3C_DCFG_DevAddr_MASK;
-			dcfg |= ctrl->wValue << S3C_DCFG_DevAddr_SHIFT;
-			writel(dcfg, hsotg->regs + S3C_DCFG);
+			dcfg = readl(hsotg->regs + DCFG);
+			dcfg &= ~DCFG_DevAddr_MASK;
+			dcfg |= ctrl->wValue << DCFG_DevAddr_SHIFT;
+			writel(dcfg, hsotg->regs + DCFG);
 
 			dev_info(hsotg->dev, "new address %d\n", ctrl->wValue);
 
@@ -1194,7 +1238,8 @@ static void s3c_hsotg_process_control(struct s3c_hsotg *hsotg,
 			dev_dbg(hsotg->dev, "driver->setup() ret %d\n", ret);
 	}
 
-	/* the request is either unhandlable, or is not formatted correctly
+	/*
+	 * the request is either unhandlable, or is not formatted correctly
 	 * so respond with a STALL for the status stage to indicate failure.
 	 */
 
@@ -1203,22 +1248,26 @@ static void s3c_hsotg_process_control(struct s3c_hsotg *hsotg,
 		u32 ctrl;
 
 		dev_dbg(hsotg->dev, "ep0 stall (dir=%d)\n", ep0->dir_in);
-		reg = (ep0->dir_in) ? S3C_DIEPCTL0 : S3C_DOEPCTL0;
+		reg = (ep0->dir_in) ? DIEPCTL0 : DOEPCTL0;
 
-		/* S3C_DxEPCTL_Stall will be cleared by EP once it has
-		 * taken effect, so no need to clear later. */
+		/*
+		 * DxEPCTL_Stall will be cleared by EP once it has
+		 * taken effect, so no need to clear later.
+		 */
 
 		ctrl = readl(hsotg->regs + reg);
-		ctrl |= S3C_DxEPCTL_Stall;
-		ctrl |= S3C_DxEPCTL_CNAK;
+		ctrl |= DxEPCTL_Stall;
+		ctrl |= DxEPCTL_CNAK;
 		writel(ctrl, hsotg->regs + reg);
 
 		dev_dbg(hsotg->dev,
 			"written DxEPCTL=0x%08x to %08x (DxEPCTL=0x%08x)\n",
 			ctrl, reg, readl(hsotg->regs + reg));
 
-		/* don't believe we need to anything more to get the EP
-		 * to reply with a STALL packet */
+		/*
+		 * don't believe we need to anything more to get the EP
+		 * to reply with a STALL packet
+		 */
 	}
 }
 
@@ -1279,8 +1328,10 @@ static void s3c_hsotg_enqueue_setup(struct s3c_hsotg *hsotg)
 	ret = s3c_hsotg_ep_queue(&hsotg->eps[0].ep, req, GFP_ATOMIC);
 	if (ret < 0) {
 		dev_err(hsotg->dev, "%s: failed queue (%d)\n", __func__, ret);
-		/* Don't think there's much we can do other than watch the
-		 * driver fail. */
+		/*
+		 * Don't think there's much we can do other than watch the
+		 * driver fail.
+		 */
 	}
 }
 
@@ -1296,7 +1347,7 @@ static void s3c_hsotg_enqueue_setup(struct s3c_hsotg *hsotg)
  * on the endpoint.
  *
  * Note, expects the ep to already be locked as appropriate.
-*/
+ */
 static void s3c_hsotg_complete_request(struct s3c_hsotg *hsotg,
 				       struct s3c_hsotg_ep *hs_ep,
 				       struct s3c_hsotg_req *hs_req,
@@ -1312,8 +1363,10 @@ static void s3c_hsotg_complete_request(struct s3c_hsotg *hsotg,
 	dev_dbg(hsotg->dev, "complete: ep %p %s, req %p, %d => %p\n",
 		hs_ep, hs_ep->ep.name, hs_req, result, hs_req->req.complete);
 
-	/* only replace the status if we've not already set an error
-	 * from a previous transaction */
+	/*
+	 * only replace the status if we've not already set an error
+	 * from a previous transaction
+	 */
 
 	if (hs_req->req.status == -EINPROGRESS)
 		hs_req->req.status = result;
@@ -1324,8 +1377,10 @@ static void s3c_hsotg_complete_request(struct s3c_hsotg *hsotg,
 	if (using_dma(hsotg))
 		s3c_hsotg_unmap_dma(hsotg, hs_ep, hs_req);
 
-	/* call the complete request with the locks off, just in case the
-	 * request tries to queue more work for this endpoint. */
+	/*
+	 * call the complete request with the locks off, just in case the
+	 * request tries to queue more work for this endpoint.
+	 */
 
 	if (hs_req->req.complete) {
 		spin_unlock(&hs_ep->lock);
@@ -1333,9 +1388,11 @@ static void s3c_hsotg_complete_request(struct s3c_hsotg *hsotg,
 		spin_lock(&hs_ep->lock);
 	}
 
-	/* Look to see if there is anything else to do. Note, the completion
+	/*
+	 * Look to see if there is anything else to do. Note, the completion
 	 * of the previous request may have caused a new request to be started
-	 * so be careful when doing this. */
+	 * so be careful when doing this.
+	 */
 
 	if (!hs_ep->req && result >= 0) {
 		restart = !list_empty(&hs_ep->queue);
@@ -1355,7 +1412,7 @@ static void s3c_hsotg_complete_request(struct s3c_hsotg *hsotg,
  *
  * See s3c_hsotg_complete_request(), but called with the endpoint's
  * lock held.
-*/
+ */
 static void s3c_hsotg_complete_request_lock(struct s3c_hsotg *hsotg,
 					    struct s3c_hsotg_ep *hs_ep,
 					    struct s3c_hsotg_req *hs_req,
@@ -1382,13 +1439,13 @@ static void s3c_hsotg_rx_data(struct s3c_hsotg *hsotg, int ep_idx, int size)
 {
 	struct s3c_hsotg_ep *hs_ep = &hsotg->eps[ep_idx];
 	struct s3c_hsotg_req *hs_req = hs_ep->req;
-	void __iomem *fifo = hsotg->regs + S3C_EPFIFO(ep_idx);
+	void __iomem *fifo = hsotg->regs + EPFIFO(ep_idx);
 	int to_read;
 	int max_req;
 	int read_ptr;
 
 	if (!hs_req) {
-		u32 epctl = readl(hsotg->regs + S3C_DOEPCTL(ep_idx));
+		u32 epctl = readl(hsotg->regs + DOEPCTL(ep_idx));
 		int ptr;
 
 		dev_warn(hsotg->dev,
@@ -1412,7 +1469,8 @@ static void s3c_hsotg_rx_data(struct s3c_hsotg *hsotg, int ep_idx, int size)
 		__func__, to_read, max_req, read_ptr, hs_req->req.length);
 
 	if (to_read > max_req) {
-		/* more data appeared than we where willing
+		/*
+		 * more data appeared than we where willing
 		 * to deal with in this request.
 		 */
 
@@ -1424,8 +1482,10 @@ static void s3c_hsotg_rx_data(struct s3c_hsotg *hsotg, int ep_idx, int size)
 	hs_req->req.actual += to_read;
 	to_read = DIV_ROUND_UP(to_read, 4);
 
-	/* note, we might over-write the buffer end by 3 bytes depending on
-	 * alignment of the data. */
+	/*
+	 * note, we might over-write the buffer end by 3 bytes depending on
+	 * alignment of the data.
+	 */
 	readsl(fifo, hs_req->req.buf + read_ptr, to_read);
 
 	spin_unlock(&hs_ep->lock);
@@ -1465,14 +1525,14 @@ static void s3c_hsotg_send_zlp(struct s3c_hsotg *hsotg,
 	dev_dbg(hsotg->dev, "sending zero-length packet\n");
 
 	/* issue a zero-sized packet to terminate this */
-	writel(S3C_DxEPTSIZ_MC(1) | S3C_DxEPTSIZ_PktCnt(1) |
-	       S3C_DxEPTSIZ_XferSize(0), hsotg->regs + S3C_DIEPTSIZ(0));
+	writel(DxEPTSIZ_MC(1) | DxEPTSIZ_PktCnt(1) |
+	       DxEPTSIZ_XferSize(0), hsotg->regs + DIEPTSIZ(0));
 
-	ctrl = readl(hsotg->regs + S3C_DIEPCTL0);
-	ctrl |= S3C_DxEPCTL_CNAK;  /* clear NAK set by core */
-	ctrl |= S3C_DxEPCTL_EPEna; /* ensure ep enabled */
-	ctrl |= S3C_DxEPCTL_USBActEp;
-	writel(ctrl, hsotg->regs + S3C_DIEPCTL0);
+	ctrl = readl(hsotg->regs + DIEPCTL0);
+	ctrl |= DxEPCTL_CNAK;  /* clear NAK set by core */
+	ctrl |= DxEPCTL_EPEna; /* ensure ep enabled */
+	ctrl |= DxEPCTL_USBActEp;
+	writel(ctrl, hsotg->regs + DIEPCTL0);
 }
 
 /**
@@ -1484,15 +1544,15 @@ static void s3c_hsotg_send_zlp(struct s3c_hsotg *hsotg,
  * The RXFIFO has delivered an OutDone event, which means that the data
  * transfer for an OUT endpoint has been completed, either by a short
  * packet or by the finish of a transfer.
-*/
+ */
 static void s3c_hsotg_handle_outdone(struct s3c_hsotg *hsotg,
 				     int epnum, bool was_setup)
 {
-	u32 epsize = readl(hsotg->regs + S3C_DOEPTSIZ(epnum));
+	u32 epsize = readl(hsotg->regs + DOEPTSIZ(epnum));
 	struct s3c_hsotg_ep *hs_ep = &hsotg->eps[epnum];
 	struct s3c_hsotg_req *hs_req = hs_ep->req;
 	struct usb_request *req = &hs_req->req;
-	unsigned size_left = S3C_DxEPTSIZ_XferSize_GET(epsize);
+	unsigned size_left = DxEPTSIZ_XferSize_GET(epsize);
 	int result = 0;
 
 	if (!hs_req) {
@@ -1503,7 +1563,8 @@ static void s3c_hsotg_handle_outdone(struct s3c_hsotg *hsotg,
 	if (using_dma(hsotg)) {
 		unsigned size_done;
 
-		/* Calculate the size of the transfer by checking how much
+		/*
+		 * Calculate the size of the transfer by checking how much
 		 * is left in the endpoint size register and then working it
 		 * out from the amount we loaded for the transfer.
 		 *
@@ -1521,17 +1582,29 @@ static void s3c_hsotg_handle_outdone(struct s3c_hsotg *hsotg,
 	if (req->actual < req->length && size_left == 0) {
 		s3c_hsotg_start_req(hsotg, hs_ep, hs_req, true);
 		return;
+	} else if (epnum == 0) {
+		/*
+		 * After was_setup = 1 =>
+		 * set CNAK for non Setup requests
+		 */
+		hsotg->setup = was_setup ? 0 : 1;
 	}
 
 	if (req->actual < req->length && req->short_not_ok) {
 		dev_dbg(hsotg->dev, "%s: got %d/%d (short not ok) => error\n",
 			__func__, req->actual, req->length);
 
-		/* todo - what should we return here? there's no one else
-		 * even bothering to check the status. */
+		/*
+		 * todo - what should we return here? there's no one else
+		 * even bothering to check the status.
+		 */
 	}
 
 	if (epnum == 0) {
+		/*
+		 * Condition req->complete != s3c_hsotg_complete_setup says:
+		 * send ZLP when we have an asynchronous request from gadget
+		 */
 		if (!was_setup && req->complete != s3c_hsotg_complete_setup)
 			s3c_hsotg_send_zlp(hsotg, hs_req);
 	}
@@ -1544,14 +1617,14 @@ static void s3c_hsotg_handle_outdone(struct s3c_hsotg *hsotg,
  * @hsotg: The device instance
  *
  * Return the current frame number
-*/
+ */
 static u32 s3c_hsotg_read_frameno(struct s3c_hsotg *hsotg)
 {
 	u32 dsts;
 
-	dsts = readl(hsotg->regs + S3C_DSTS);
-	dsts &= S3C_DSTS_SOFFN_MASK;
-	dsts >>= S3C_DSTS_SOFFN_SHIFT;
+	dsts = readl(hsotg->regs + DSTS);
+	dsts &= DSTS_SOFFN_MASK;
+	dsts >>= DSTS_SOFFN_SHIFT;
 
 	return dsts;
 }
@@ -1574,29 +1647,29 @@ static u32 s3c_hsotg_read_frameno(struct s3c_hsotg *hsotg)
  */
 static void s3c_hsotg_handle_rx(struct s3c_hsotg *hsotg)
 {
-	u32 grxstsr = readl(hsotg->regs + S3C_GRXSTSP);
+	u32 grxstsr = readl(hsotg->regs + GRXSTSP);
 	u32 epnum, status, size;
 
 	WARN_ON(using_dma(hsotg));
 
-	epnum = grxstsr & S3C_GRXSTS_EPNum_MASK;
-	status = grxstsr & S3C_GRXSTS_PktSts_MASK;
+	epnum = grxstsr & GRXSTS_EPNum_MASK;
+	status = grxstsr & GRXSTS_PktSts_MASK;
 
-	size = grxstsr & S3C_GRXSTS_ByteCnt_MASK;
-	size >>= S3C_GRXSTS_ByteCnt_SHIFT;
+	size = grxstsr & GRXSTS_ByteCnt_MASK;
+	size >>= GRXSTS_ByteCnt_SHIFT;
 
 	if (1)
 		dev_dbg(hsotg->dev, "%s: GRXSTSP=0x%08x (%d@%d)\n",
 			__func__, grxstsr, size, epnum);
 
-#define __status(x) ((x) >> S3C_GRXSTS_PktSts_SHIFT)
+#define __status(x) ((x) >> GRXSTS_PktSts_SHIFT)
 
-	switch (status >> S3C_GRXSTS_PktSts_SHIFT) {
-	case __status(S3C_GRXSTS_PktSts_GlobalOutNAK):
+	switch (status >> GRXSTS_PktSts_SHIFT) {
+	case __status(GRXSTS_PktSts_GlobalOutNAK):
 		dev_dbg(hsotg->dev, "GlobalOutNAK\n");
 		break;
 
-	case __status(S3C_GRXSTS_PktSts_OutDone):
+	case __status(GRXSTS_PktSts_OutDone):
 		dev_dbg(hsotg->dev, "OutDone (Frame=0x%08x)\n",
 			s3c_hsotg_read_frameno(hsotg));
 
@@ -1604,24 +1677,24 @@ static void s3c_hsotg_handle_rx(struct s3c_hsotg *hsotg)
 			s3c_hsotg_handle_outdone(hsotg, epnum, false);
 		break;
 
-	case __status(S3C_GRXSTS_PktSts_SetupDone):
+	case __status(GRXSTS_PktSts_SetupDone):
 		dev_dbg(hsotg->dev,
 			"SetupDone (Frame=0x%08x, DOPEPCTL=0x%08x)\n",
 			s3c_hsotg_read_frameno(hsotg),
-			readl(hsotg->regs + S3C_DOEPCTL(0)));
+			readl(hsotg->regs + DOEPCTL(0)));
 
 		s3c_hsotg_handle_outdone(hsotg, epnum, true);
 		break;
 
-	case __status(S3C_GRXSTS_PktSts_OutRX):
+	case __status(GRXSTS_PktSts_OutRX):
 		s3c_hsotg_rx_data(hsotg, epnum, size);
 		break;
 
-	case __status(S3C_GRXSTS_PktSts_SetupRX):
+	case __status(GRXSTS_PktSts_SetupRX):
 		dev_dbg(hsotg->dev,
 			"SetupRX (Frame=0x%08x, DOPEPCTL=0x%08x)\n",
 			s3c_hsotg_read_frameno(hsotg),
-			readl(hsotg->regs + S3C_DOEPCTL(0)));
+			readl(hsotg->regs + DOEPCTL(0)));
 
 		s3c_hsotg_rx_data(hsotg, epnum, size);
 		break;
@@ -1638,18 +1711,18 @@ static void s3c_hsotg_handle_rx(struct s3c_hsotg *hsotg)
 /**
  * s3c_hsotg_ep0_mps - turn max packet size into register setting
  * @mps: The maximum packet size in bytes.
-*/
+ */
 static u32 s3c_hsotg_ep0_mps(unsigned int mps)
 {
 	switch (mps) {
 	case 64:
-		return S3C_D0EPCTL_MPS_64;
+		return D0EPCTL_MPS_64;
 	case 32:
-		return S3C_D0EPCTL_MPS_32;
+		return D0EPCTL_MPS_32;
 	case 16:
-		return S3C_D0EPCTL_MPS_16;
+		return D0EPCTL_MPS_16;
 	case 8:
-		return S3C_D0EPCTL_MPS_8;
+		return D0EPCTL_MPS_8;
 	}
 
 	/* bad max packet size, warn and return invalid result */
@@ -1680,7 +1753,7 @@ static void s3c_hsotg_set_ep_maxpacket(struct s3c_hsotg *hsotg,
 		if (mpsval > 3)
 			goto bad_mps;
 	} else {
-		if (mps >= S3C_DxEPCTL_MPS_LIMIT+1)
+		if (mps >= DxEPCTL_MPS_LIMIT+1)
 			goto bad_mps;
 
 		mpsval = mps;
@@ -1688,19 +1761,21 @@ static void s3c_hsotg_set_ep_maxpacket(struct s3c_hsotg *hsotg,
 
 	hs_ep->ep.maxpacket = mps;
 
-	/* update both the in and out endpoint controldir_ registers, even
-	 * if one of the directions may not be in use. */
+	/*
+	 * update both the in and out endpoint controldir_ registers, even
+	 * if one of the directions may not be in use.
+	 */
 
-	reg = readl(regs + S3C_DIEPCTL(ep));
-	reg &= ~S3C_DxEPCTL_MPS_MASK;
+	reg = readl(regs + DIEPCTL(ep));
+	reg &= ~DxEPCTL_MPS_MASK;
 	reg |= mpsval;
-	writel(reg, regs + S3C_DIEPCTL(ep));
+	writel(reg, regs + DIEPCTL(ep));
 
 	if (ep) {
-		reg = readl(regs + S3C_DOEPCTL(ep));
-		reg &= ~S3C_DxEPCTL_MPS_MASK;
+		reg = readl(regs + DOEPCTL(ep));
+		reg &= ~DxEPCTL_MPS_MASK;
 		reg |= mpsval;
-		writel(reg, regs + S3C_DOEPCTL(ep));
+		writel(reg, regs + DOEPCTL(ep));
 	}
 
 	return;
@@ -1719,16 +1794,16 @@ static void s3c_hsotg_txfifo_flush(struct s3c_hsotg *hsotg, unsigned int idx)
 	int timeout;
 	int val;
 
-	writel(S3C_GRSTCTL_TxFNum(idx) | S3C_GRSTCTL_TxFFlsh,
-		hsotg->regs + S3C_GRSTCTL);
+	writel(GRSTCTL_TxFNum(idx) | GRSTCTL_TxFFlsh,
+		hsotg->regs + GRSTCTL);
 
 	/* wait until the fifo is flushed */
 	timeout = 100;
 
 	while (1) {
-		val = readl(hsotg->regs + S3C_GRSTCTL);
+		val = readl(hsotg->regs + GRSTCTL);
 
-		if ((val & (S3C_GRSTCTL_TxFFlsh)) == 0)
+		if ((val & (GRSTCTL_TxFFlsh)) == 0)
 			break;
 
 		if (--timeout == 0) {
@@ -1778,7 +1853,7 @@ static void s3c_hsotg_complete_in(struct s3c_hsotg *hsotg,
 				  struct s3c_hsotg_ep *hs_ep)
 {
 	struct s3c_hsotg_req *hs_req = hs_ep->req;
-	u32 epsize = readl(hsotg->regs + S3C_DIEPTSIZ(hs_ep->index));
+	u32 epsize = readl(hsotg->regs + DIEPTSIZ(hs_ep->index));
 	int size_left, size_done;
 
 	if (!hs_req) {
@@ -1786,7 +1861,15 @@ static void s3c_hsotg_complete_in(struct s3c_hsotg *hsotg,
 		return;
 	}
 
-	/* Calculate the size of the transfer by checking how much is left
+	/* Finish ZLP handling for IN EP0 transactions */
+	if (hsotg->eps[0].sent_zlp) {
+		dev_dbg(hsotg->dev, "zlp packet received\n");
+		s3c_hsotg_complete_request_lock(hsotg, hs_ep, hs_req, 0);
+		return;
+	}
+
+	/*
+	 * Calculate the size of the transfer by checking how much is left
 	 * in the endpoint size register and then working it out from
 	 * the amount we loaded for the transfer.
 	 *
@@ -1795,7 +1878,7 @@ static void s3c_hsotg_complete_in(struct s3c_hsotg *hsotg,
 	 * aligned).
 	 */
 
-	size_left = S3C_DxEPTSIZ_XferSize_GET(epsize);
+	size_left = DxEPTSIZ_XferSize_GET(epsize);
 
 	size_done = hs_ep->size_loaded - size_left;
 	size_done += hs_ep->last_load;
@@ -1805,9 +1888,28 @@ static void s3c_hsotg_complete_in(struct s3c_hsotg *hsotg,
 			__func__, hs_req->req.actual, size_done);
 
 	hs_req->req.actual = size_done;
+	dev_dbg(hsotg->dev, "req->length:%d req->actual:%d req->zero:%d\n",
+		hs_req->req.length, hs_req->req.actual, hs_req->req.zero);
+
+	/*
+	 * Check if dealing with Maximum Packet Size(MPS) IN transfer at EP0
+	 * When sent data is a multiple MPS size (e.g. 64B ,128B ,192B
+	 * ,256B ... ), after last MPS sized packet send IN ZLP packet to
+	 * inform the host that no more data is available.
+	 * The state of req.zero member is checked to be sure that the value to
+	 * send is smaller than wValue expected from host.
+	 * Check req.length to NOT send another ZLP when the current one is
+	 * under completion (the one for which this completion has been called).
+	 */
+	if (hs_req->req.length && hs_ep->index == 0 && hs_req->req.zero &&
+	    hs_req->req.length == hs_req->req.actual &&
+	    !(hs_req->req.length % hs_ep->ep.maxpacket)) {
 
-	/* if we did all of the transfer, and there is more data left
-	 * around, then try restarting the rest of the request */
+		dev_dbg(hsotg->dev, "ep0 zlp IN packet sent\n");
+		s3c_hsotg_send_zlp(hsotg, hs_req);
+
+		return;
+	}
 
 	if (!size_left && hs_req->req.actual < hs_req->req.length) {
 		dev_dbg(hsotg->dev, "%s trying more for req...\n", __func__);
@@ -1823,14 +1925,14 @@ static void s3c_hsotg_complete_in(struct s3c_hsotg *hsotg,
  * @dir_in: Set if this is an IN endpoint
  *
  * Process and clear any interrupt pending for an individual endpoint
-*/
+ */
 static void s3c_hsotg_epint(struct s3c_hsotg *hsotg, unsigned int idx,
 			    int dir_in)
 {
 	struct s3c_hsotg_ep *hs_ep = &hsotg->eps[idx];
-	u32 epint_reg = dir_in ? S3C_DIEPINT(idx) : S3C_DOEPINT(idx);
-	u32 epctl_reg = dir_in ? S3C_DIEPCTL(idx) : S3C_DOEPCTL(idx);
-	u32 epsiz_reg = dir_in ? S3C_DIEPTSIZ(idx) : S3C_DOEPTSIZ(idx);
+	u32 epint_reg = dir_in ? DIEPINT(idx) : DOEPINT(idx);
+	u32 epctl_reg = dir_in ? DIEPCTL(idx) : DOEPCTL(idx);
+	u32 epsiz_reg = dir_in ? DIEPTSIZ(idx) : DOEPTSIZ(idx);
 	u32 ints;
 
 	ints = readl(hsotg->regs + epint_reg);
@@ -1841,28 +1943,32 @@ static void s3c_hsotg_epint(struct s3c_hsotg *hsotg, unsigned int idx,
 	dev_dbg(hsotg->dev, "%s: ep%d(%s) DxEPINT=0x%08x\n",
 		__func__, idx, dir_in ? "in" : "out", ints);
 
-	if (ints & S3C_DxEPINT_XferCompl) {
+	if (ints & DxEPINT_XferCompl) {
 		dev_dbg(hsotg->dev,
 			"%s: XferCompl: DxEPCTL=0x%08x, DxEPTSIZ=%08x\n",
 			__func__, readl(hsotg->regs + epctl_reg),
 			readl(hsotg->regs + epsiz_reg));
 
-		/* we get OutDone from the FIFO, so we only need to look
-		 * at completing IN requests here */
+		/*
+		 * we get OutDone from the FIFO, so we only need to look
+		 * at completing IN requests here
+		 */
 		if (dir_in) {
 			s3c_hsotg_complete_in(hsotg, hs_ep);
 
 			if (idx == 0 && !hs_ep->req)
 				s3c_hsotg_enqueue_setup(hsotg);
 		} else if (using_dma(hsotg)) {
-			/* We're using DMA, we need to fire an OutDone here
-			 * as we ignore the RXFIFO. */
+			/*
+			 * We're using DMA, we need to fire an OutDone here
+			 * as we ignore the RXFIFO.
+			 */
 
 			s3c_hsotg_handle_outdone(hsotg, idx, false);
 		}
 	}
 
-	if (ints & S3C_DxEPINT_EPDisbld) {
+	if (ints & DxEPINT_EPDisbld) {
 		dev_dbg(hsotg->dev, "%s: EPDisbld\n", __func__);
 
 		if (dir_in) {
@@ -1870,27 +1976,29 @@ static void s3c_hsotg_epint(struct s3c_hsotg *hsotg, unsigned int idx,
 
 			s3c_hsotg_txfifo_flush(hsotg, idx);
 
-			if ((epctl & S3C_DxEPCTL_Stall) &&
-				(epctl & S3C_DxEPCTL_EPType_Bulk)) {
-				int dctl = readl(hsotg->regs + S3C_DCTL);
+			if ((epctl & DxEPCTL_Stall) &&
+				(epctl & DxEPCTL_EPType_Bulk)) {
+				int dctl = readl(hsotg->regs + DCTL);
 
-				dctl |= S3C_DCTL_CGNPInNAK;
-				writel(dctl, hsotg->regs + S3C_DCTL);
+				dctl |= DCTL_CGNPInNAK;
+				writel(dctl, hsotg->regs + DCTL);
 			}
 		}
 	}
 
-	if (ints & S3C_DxEPINT_AHBErr)
+	if (ints & DxEPINT_AHBErr)
 		dev_dbg(hsotg->dev, "%s: AHBErr\n", __func__);
 
-	if (ints & S3C_DxEPINT_Setup) {  /* Setup or Timeout */
+	if (ints & DxEPINT_Setup) {  /* Setup or Timeout */
 		dev_dbg(hsotg->dev, "%s: Setup/Timeout\n",  __func__);
 
 		if (using_dma(hsotg) && idx == 0) {
-			/* this is the notification we've received a
+			/*
+			 * this is the notification we've received a
 			 * setup packet. In non-DMA mode we'd get this
 			 * from the RXFIFO, instead we need to process
-			 * the setup here. */
+			 * the setup here.
+			 */
 
 			if (dir_in)
 				WARN_ON_ONCE(1);
@@ -1899,26 +2007,25 @@ static void s3c_hsotg_epint(struct s3c_hsotg *hsotg, unsigned int idx,
 		}
 	}
 
-	if (ints & S3C_DxEPINT_Back2BackSetup)
+	if (ints & DxEPINT_Back2BackSetup)
 		dev_dbg(hsotg->dev, "%s: B2BSetup/INEPNakEff\n", __func__);
 
 	if (dir_in) {
-		/* not sure if this is important, but we'll clear it anyway
-		 */
-		if (ints & S3C_DIEPMSK_INTknTXFEmpMsk) {
+		/* not sure if this is important, but we'll clear it anyway */
+		if (ints & DIEPMSK_INTknTXFEmpMsk) {
 			dev_dbg(hsotg->dev, "%s: ep%d: INTknTXFEmpMsk\n",
 				__func__, idx);
 		}
 
 		/* this probably means something bad is happening */
-		if (ints & S3C_DIEPMSK_INTknEPMisMsk) {
+		if (ints & DIEPMSK_INTknEPMisMsk) {
 			dev_warn(hsotg->dev, "%s: ep%d: INTknEP\n",
 				 __func__, idx);
 		}
 
 		/* FIFO has space or is empty (see GAHBCFG) */
 		if (hsotg->dedicated_fifos &&
-		    ints & S3C_DIEPMSK_TxFIFOEmpty) {
+		    ints & DIEPMSK_TxFIFOEmpty) {
 			dev_dbg(hsotg->dev, "%s: ep%d: TxFIFOEmpty\n",
 				__func__, idx);
 			if (!using_dma(hsotg))
@@ -1933,40 +2040,45 @@ static void s3c_hsotg_epint(struct s3c_hsotg *hsotg, unsigned int idx,
  *
  * Handle updating the device settings after the enumeration phase has
  * been completed.
-*/
+ */
 static void s3c_hsotg_irq_enumdone(struct s3c_hsotg *hsotg)
 {
-	u32 dsts = readl(hsotg->regs + S3C_DSTS);
+	u32 dsts = readl(hsotg->regs + DSTS);
 	int ep0_mps = 0, ep_mps;
 
-	/* This should signal the finish of the enumeration phase
+	/*
+	 * This should signal the finish of the enumeration phase
 	 * of the USB handshaking, so we should now know what rate
-	 * we connected at. */
+	 * we connected at.
+	 */
 
 	dev_dbg(hsotg->dev, "EnumDone (DSTS=0x%08x)\n", dsts);
 
-	/* note, since we're limited by the size of transfer on EP0, and
+	/*
+	 * note, since we're limited by the size of transfer on EP0, and
 	 * it seems IN transfers must be a even number of packets we do
-	 * not advertise a 64byte MPS on EP0. */
+	 * not advertise a 64byte MPS on EP0.
+	 */
 
 	/* catch both EnumSpd_FS and EnumSpd_FS48 */
-	switch (dsts & S3C_DSTS_EnumSpd_MASK) {
-	case S3C_DSTS_EnumSpd_FS:
-	case S3C_DSTS_EnumSpd_FS48:
+	switch (dsts & DSTS_EnumSpd_MASK) {
+	case DSTS_EnumSpd_FS:
+	case DSTS_EnumSpd_FS48:
 		hsotg->gadget.speed = USB_SPEED_FULL;
 		ep0_mps = EP0_MPS_LIMIT;
 		ep_mps = 64;
 		break;
 
-	case S3C_DSTS_EnumSpd_HS:
+	case DSTS_EnumSpd_HS:
 		hsotg->gadget.speed = USB_SPEED_HIGH;
 		ep0_mps = EP0_MPS_LIMIT;
 		ep_mps = 512;
 		break;
 
-	case S3C_DSTS_EnumSpd_LS:
+	case DSTS_EnumSpd_LS:
 		hsotg->gadget.speed = USB_SPEED_LOW;
-		/* note, we don't actually support LS in this driver at the
+		/*
+		 * note, we don't actually support LS in this driver at the
 		 * moment, and the documentation seems to imply that it isn't
 		 * supported by the PHYs on some of the devices.
 		 */
@@ -1975,13 +2087,15 @@ static void s3c_hsotg_irq_enumdone(struct s3c_hsotg *hsotg)
 	dev_info(hsotg->dev, "new device is %s\n",
 		 usb_speed_string(hsotg->gadget.speed));
 
-	/* we should now know the maximum packet size for an
-	 * endpoint, so set the endpoints to a default value. */
+	/*
+	 * we should now know the maximum packet size for an
+	 * endpoint, so set the endpoints to a default value.
+	 */
 
 	if (ep0_mps) {
 		int i;
 		s3c_hsotg_set_ep_maxpacket(hsotg, 0, ep0_mps);
-		for (i = 1; i < S3C_HSOTG_EPS; i++)
+		for (i = 1; i < hsotg->num_of_eps; i++)
 			s3c_hsotg_set_ep_maxpacket(hsotg, i, ep_mps);
 	}
 
@@ -1990,8 +2104,8 @@ static void s3c_hsotg_irq_enumdone(struct s3c_hsotg *hsotg)
 	s3c_hsotg_enqueue_setup(hsotg);
 
 	dev_dbg(hsotg->dev, "EP0: DIEPCTL0=0x%08x, DOEPCTL0=0x%08x\n",
-		readl(hsotg->regs + S3C_DIEPCTL0),
-		readl(hsotg->regs + S3C_DOEPCTL0));
+		readl(hsotg->regs + DIEPCTL0),
+		readl(hsotg->regs + DOEPCTL0));
 }
 
 /**
@@ -2014,8 +2128,10 @@ static void kill_all_requests(struct s3c_hsotg *hsotg,
 	spin_lock_irqsave(&ep->lock, flags);
 
 	list_for_each_entry_safe(req, treq, &ep->queue, queue) {
-		/* currently, we can't do much about an already
-		 * running request on an in endpoint */
+		/*
+		 * currently, we can't do much about an already
+		 * running request on an in endpoint
+		 */
 
 		if (ep->req == req && ep->dir_in && !force)
 			continue;
@@ -2033,18 +2149,18 @@ static void kill_all_requests(struct s3c_hsotg *hsotg,
 		(_hs)->driver->_entry(&(_hs)->gadget);
 
 /**
- * s3c_hsotg_disconnect_irq - disconnect irq service
+ * s3c_hsotg_disconnect - disconnect service
  * @hsotg: The device state.
  *
- * A disconnect IRQ has been received, meaning that the host has
- * lost contact with the bus. Remove all current transactions
- * and signal the gadget driver that this has happened.
-*/
-static void s3c_hsotg_disconnect_irq(struct s3c_hsotg *hsotg)
+ * The device has been disconnected. Remove all current
+ * transactions and signal the gadget driver that this
+ * has happened.
+ */
+static void s3c_hsotg_disconnect(struct s3c_hsotg *hsotg)
 {
 	unsigned ep;
 
-	for (ep = 0; ep < S3C_HSOTG_EPS; ep++)
+	for (ep = 0; ep < hsotg->num_of_eps; ep++)
 		kill_all_requests(hsotg, &hsotg->eps[ep], -ESHUTDOWN, true);
 
 	call_gadget(hsotg, disconnect);
@@ -2062,7 +2178,7 @@ static void s3c_hsotg_irq_fifoempty(struct s3c_hsotg *hsotg, bool periodic)
 
 	/* look through for any more data to transmit */
 
-	for (epno = 0; epno < S3C_HSOTG_EPS; epno++) {
+	for (epno = 0; epno < hsotg->num_of_eps; epno++) {
 		ep = &hsotg->eps[epno];
 
 		if (!ep->dir_in)
@@ -2078,12 +2194,187 @@ static void s3c_hsotg_irq_fifoempty(struct s3c_hsotg *hsotg, bool periodic)
 	}
 }
 
-static struct s3c_hsotg *our_hsotg;
-
 /* IRQ flags which will trigger a retry around the IRQ loop */
-#define IRQ_RETRY_MASK (S3C_GINTSTS_NPTxFEmp | \
-			S3C_GINTSTS_PTxFEmp |  \
-			S3C_GINTSTS_RxFLvl)
+#define IRQ_RETRY_MASK (GINTSTS_NPTxFEmp | \
+			GINTSTS_PTxFEmp |  \
+			GINTSTS_RxFLvl)
+
+/**
+ * s3c_hsotg_corereset - issue softreset to the core
+ * @hsotg: The device state
+ *
+ * Issue a soft reset to the core, and await the core finishing it.
+ */
+static int s3c_hsotg_corereset(struct s3c_hsotg *hsotg)
+{
+	int timeout;
+	u32 grstctl;
+
+	dev_dbg(hsotg->dev, "resetting core\n");
+
+	/* issue soft reset */
+	writel(GRSTCTL_CSftRst, hsotg->regs + GRSTCTL);
+
+	timeout = 1000;
+	do {
+		grstctl = readl(hsotg->regs + GRSTCTL);
+	} while ((grstctl & GRSTCTL_CSftRst) && timeout-- > 0);
+
+	if (grstctl & GRSTCTL_CSftRst) {
+		dev_err(hsotg->dev, "Failed to get CSftRst asserted\n");
+		return -EINVAL;
+	}
+
+	timeout = 1000;
+
+	while (1) {
+		u32 grstctl = readl(hsotg->regs + GRSTCTL);
+
+		if (timeout-- < 0) {
+			dev_info(hsotg->dev,
+				 "%s: reset failed, GRSTCTL=%08x\n",
+				 __func__, grstctl);
+			return -ETIMEDOUT;
+		}
+
+		if (!(grstctl & GRSTCTL_AHBIdle))
+			continue;
+
+		break;		/* reset done */
+	}
+
+	dev_dbg(hsotg->dev, "reset successful\n");
+	return 0;
+}
+
+/**
+ * s3c_hsotg_core_init - issue softreset to the core
+ * @hsotg: The device state
+ *
+ * Issue a soft reset to the core, and await the core finishing it.
+ */
+static void s3c_hsotg_core_init(struct s3c_hsotg *hsotg)
+{
+	s3c_hsotg_corereset(hsotg);
+
+	/*
+	 * we must now enable ep0 ready for host detection and then
+	 * set configuration.
+	 */
+
+	/* set the PLL on, remove the HNP/SRP and set the PHY */
+	writel(GUSBCFG_PHYIf16 | GUSBCFG_TOutCal(7) |
+	       (0x5 << 10), hsotg->regs + GUSBCFG);
+
+	s3c_hsotg_init_fifo(hsotg);
+
+	__orr32(hsotg->regs + DCTL, DCTL_SftDiscon);
+
+	writel(1 << 18 | DCFG_DevSpd_HS,  hsotg->regs + DCFG);
+
+	/* Clear any pending OTG interrupts */
+	writel(0xffffffff, hsotg->regs + GOTGINT);
+
+	/* Clear any pending interrupts */
+	writel(0xffffffff, hsotg->regs + GINTSTS);
+
+	writel(GINTSTS_ErlySusp | GINTSTS_SessReqInt |
+	       GINTSTS_GOUTNakEff | GINTSTS_GINNakEff |
+	       GINTSTS_ConIDStsChng | GINTSTS_USBRst |
+	       GINTSTS_EnumDone | GINTSTS_OTGInt |
+	       GINTSTS_USBSusp | GINTSTS_WkUpInt,
+	       hsotg->regs + GINTMSK);
+
+	if (using_dma(hsotg))
+		writel(GAHBCFG_GlblIntrEn | GAHBCFG_DMAEn |
+		       GAHBCFG_HBstLen_Incr4,
+		       hsotg->regs + GAHBCFG);
+	else
+		writel(GAHBCFG_GlblIntrEn, hsotg->regs + GAHBCFG);
+
+	/*
+	 * Enabling INTknTXFEmpMsk here seems to be a big mistake, we end
+	 * up being flooded with interrupts if the host is polling the
+	 * endpoint to try and read data.
+	 */
+
+	writel(((hsotg->dedicated_fifos) ? DIEPMSK_TxFIFOEmpty : 0) |
+	       DIEPMSK_EPDisbldMsk | DIEPMSK_XferComplMsk |
+	       DIEPMSK_TimeOUTMsk | DIEPMSK_AHBErrMsk |
+	       DIEPMSK_INTknEPMisMsk,
+	       hsotg->regs + DIEPMSK);
+
+	/*
+	 * don't need XferCompl, we get that from RXFIFO in slave mode. In
+	 * DMA mode we may need this.
+	 */
+	writel((using_dma(hsotg) ? (DIEPMSK_XferComplMsk |
+				    DIEPMSK_TimeOUTMsk) : 0) |
+	       DOEPMSK_EPDisbldMsk | DOEPMSK_AHBErrMsk |
+	       DOEPMSK_SetupMsk,
+	       hsotg->regs + DOEPMSK);
+
+	writel(0, hsotg->regs + DAINTMSK);
+
+	dev_dbg(hsotg->dev, "EP0: DIEPCTL0=0x%08x, DOEPCTL0=0x%08x\n",
+		readl(hsotg->regs + DIEPCTL0),
+		readl(hsotg->regs + DOEPCTL0));
+
+	/* enable in and out endpoint interrupts */
+	s3c_hsotg_en_gsint(hsotg, GINTSTS_OEPInt | GINTSTS_IEPInt);
+
+	/*
+	 * Enable the RXFIFO when in slave mode, as this is how we collect
+	 * the data. In DMA mode, we get events from the FIFO but also
+	 * things we cannot process, so do not use it.
+	 */
+	if (!using_dma(hsotg))
+		s3c_hsotg_en_gsint(hsotg, GINTSTS_RxFLvl);
+
+	/* Enable interrupts for EP0 in and out */
+	s3c_hsotg_ctrl_epint(hsotg, 0, 0, 1);
+	s3c_hsotg_ctrl_epint(hsotg, 0, 1, 1);
+
+	__orr32(hsotg->regs + DCTL, DCTL_PWROnPrgDone);
+	udelay(10);  /* see openiboot */
+	__bic32(hsotg->regs + DCTL, DCTL_PWROnPrgDone);
+
+	dev_dbg(hsotg->dev, "DCTL=0x%08x\n", readl(hsotg->regs + DCTL));
+
+	/*
+	 * DxEPCTL_USBActEp says RO in manual, but seems to be set by
+	 * writing to the EPCTL register..
+	 */
+
+	/* set to read 1 8byte packet */
+	writel(DxEPTSIZ_MC(1) | DxEPTSIZ_PktCnt(1) |
+	       DxEPTSIZ_XferSize(8), hsotg->regs + DOEPTSIZ0);
+
+	writel(s3c_hsotg_ep0_mps(hsotg->eps[0].ep.maxpacket) |
+	       DxEPCTL_CNAK | DxEPCTL_EPEna |
+	       DxEPCTL_USBActEp,
+	       hsotg->regs + DOEPCTL0);
+
+	/* enable, but don't activate EP0in */
+	writel(s3c_hsotg_ep0_mps(hsotg->eps[0].ep.maxpacket) |
+	       DxEPCTL_USBActEp, hsotg->regs + DIEPCTL0);
+
+	s3c_hsotg_enqueue_setup(hsotg);
+
+	dev_dbg(hsotg->dev, "EP0: DIEPCTL0=0x%08x, DOEPCTL0=0x%08x\n",
+		readl(hsotg->regs + DIEPCTL0),
+		readl(hsotg->regs + DOEPCTL0));
+
+	/* clear global NAKs */
+	writel(DCTL_CGOUTNak | DCTL_CGNPInNAK,
+	       hsotg->regs + DCTL);
+
+	/* must be at-least 3ms to allow bus to see disconnect */
+	mdelay(3);
+
+	/* remove the soft-disconnect and let's go */
+	__bic32(hsotg->regs + DCTL, DCTL_SftDiscon);
+}
 
 /**
  * s3c_hsotg_irq - handle device interrupt
@@ -2098,52 +2389,45 @@ static irqreturn_t s3c_hsotg_irq(int irq, void *pw)
 	u32 gintmsk;
 
 irq_retry:
-	gintsts = readl(hsotg->regs + S3C_GINTSTS);
-	gintmsk = readl(hsotg->regs + S3C_GINTMSK);
+	gintsts = readl(hsotg->regs + GINTSTS);
+	gintmsk = readl(hsotg->regs + GINTMSK);
 
 	dev_dbg(hsotg->dev, "%s: %08x %08x (%08x) retry %d\n",
 		__func__, gintsts, gintsts & gintmsk, gintmsk, retry_count);
 
 	gintsts &= gintmsk;
 
-	if (gintsts & S3C_GINTSTS_OTGInt) {
-		u32 otgint = readl(hsotg->regs + S3C_GOTGINT);
+	if (gintsts & GINTSTS_OTGInt) {
+		u32 otgint = readl(hsotg->regs + GOTGINT);
 
 		dev_info(hsotg->dev, "OTGInt: %08x\n", otgint);
 
-		writel(otgint, hsotg->regs + S3C_GOTGINT);
-	}
-
-	if (gintsts & S3C_GINTSTS_DisconnInt) {
-		dev_dbg(hsotg->dev, "%s: DisconnInt\n", __func__);
-		writel(S3C_GINTSTS_DisconnInt, hsotg->regs + S3C_GINTSTS);
-
-		s3c_hsotg_disconnect_irq(hsotg);
+		writel(otgint, hsotg->regs + GOTGINT);
 	}
 
-	if (gintsts & S3C_GINTSTS_SessReqInt) {
+	if (gintsts & GINTSTS_SessReqInt) {
 		dev_dbg(hsotg->dev, "%s: SessReqInt\n", __func__);
-		writel(S3C_GINTSTS_SessReqInt, hsotg->regs + S3C_GINTSTS);
+		writel(GINTSTS_SessReqInt, hsotg->regs + GINTSTS);
 	}
 
-	if (gintsts & S3C_GINTSTS_EnumDone) {
-		writel(S3C_GINTSTS_EnumDone, hsotg->regs + S3C_GINTSTS);
+	if (gintsts & GINTSTS_EnumDone) {
+		writel(GINTSTS_EnumDone, hsotg->regs + GINTSTS);
 
 		s3c_hsotg_irq_enumdone(hsotg);
 	}
 
-	if (gintsts & S3C_GINTSTS_ConIDStsChng) {
+	if (gintsts & GINTSTS_ConIDStsChng) {
 		dev_dbg(hsotg->dev, "ConIDStsChg (DSTS=0x%08x, GOTCTL=%08x)\n",
-			readl(hsotg->regs + S3C_DSTS),
-			readl(hsotg->regs + S3C_GOTGCTL));
+			readl(hsotg->regs + DSTS),
+			readl(hsotg->regs + GOTGCTL));
 
-		writel(S3C_GINTSTS_ConIDStsChng, hsotg->regs + S3C_GINTSTS);
+		writel(GINTSTS_ConIDStsChng, hsotg->regs + GINTSTS);
 	}
 
-	if (gintsts & (S3C_GINTSTS_OEPInt | S3C_GINTSTS_IEPInt)) {
-		u32 daint = readl(hsotg->regs + S3C_DAINT);
-		u32 daint_out = daint >> S3C_DAINT_OutEP_SHIFT;
-		u32 daint_in = daint & ~(daint_out << S3C_DAINT_OutEP_SHIFT);
+	if (gintsts & (GINTSTS_OEPInt | GINTSTS_IEPInt)) {
+		u32 daint = readl(hsotg->regs + DAINT);
+		u32 daint_out = daint >> DAINT_OutEP_SHIFT;
+		u32 daint_in = daint & ~(daint_out << DAINT_OutEP_SHIFT);
 		int ep;
 
 		dev_dbg(hsotg->dev, "%s: daint=%08x\n", __func__, daint);
@@ -2159,102 +2443,116 @@ irq_retry:
 		}
 	}
 
-	if (gintsts & S3C_GINTSTS_USBRst) {
+	if (gintsts & GINTSTS_USBRst) {
+
+		u32 usb_status = readl(hsotg->regs + GOTGCTL);
+
 		dev_info(hsotg->dev, "%s: USBRst\n", __func__);
 		dev_dbg(hsotg->dev, "GNPTXSTS=%08x\n",
-			readl(hsotg->regs + S3C_GNPTXSTS));
+			readl(hsotg->regs + GNPTXSTS));
 
-		writel(S3C_GINTSTS_USBRst, hsotg->regs + S3C_GINTSTS);
+		writel(GINTSTS_USBRst, hsotg->regs + GINTSTS);
 
-		kill_all_requests(hsotg, &hsotg->eps[0], -ECONNRESET, true);
+		if (usb_status & GOTGCTL_BSESVLD) {
+			if (time_after(jiffies, hsotg->last_rst +
+				       msecs_to_jiffies(200))) {
 
-		/* it seems after a reset we can end up with a situation
-		 * where the TXFIFO still has data in it... the docs
-		 * suggest resetting all the fifos, so use the init_fifo
-		 * code to relayout and flush the fifos.
-		 */
+				kill_all_requests(hsotg, &hsotg->eps[0],
+							  -ECONNRESET, true);
 
-		s3c_hsotg_init_fifo(hsotg);
-
-		s3c_hsotg_enqueue_setup(hsotg);
+				s3c_hsotg_core_init(hsotg);
+				hsotg->last_rst = jiffies;
+			}
+		}
 	}
 
 	/* check both FIFOs */
 
-	if (gintsts & S3C_GINTSTS_NPTxFEmp) {
+	if (gintsts & GINTSTS_NPTxFEmp) {
 		dev_dbg(hsotg->dev, "NPTxFEmp\n");
 
-		/* Disable the interrupt to stop it happening again
+		/*
+		 * Disable the interrupt to stop it happening again
 		 * unless one of these endpoint routines decides that
-		 * it needs re-enabling */
+		 * it needs re-enabling
+		 */
 
-		s3c_hsotg_disable_gsint(hsotg, S3C_GINTSTS_NPTxFEmp);
+		s3c_hsotg_disable_gsint(hsotg, GINTSTS_NPTxFEmp);
 		s3c_hsotg_irq_fifoempty(hsotg, false);
 	}
 
-	if (gintsts & S3C_GINTSTS_PTxFEmp) {
+	if (gintsts & GINTSTS_PTxFEmp) {
 		dev_dbg(hsotg->dev, "PTxFEmp\n");
 
-		/* See note in S3C_GINTSTS_NPTxFEmp */
+		/* See note in GINTSTS_NPTxFEmp */
 
-		s3c_hsotg_disable_gsint(hsotg, S3C_GINTSTS_PTxFEmp);
+		s3c_hsotg_disable_gsint(hsotg, GINTSTS_PTxFEmp);
 		s3c_hsotg_irq_fifoempty(hsotg, true);
 	}
 
-	if (gintsts & S3C_GINTSTS_RxFLvl) {
-		/* note, since GINTSTS_RxFLvl doubles as FIFO-not-empty,
+	if (gintsts & GINTSTS_RxFLvl) {
+		/*
+		 * note, since GINTSTS_RxFLvl doubles as FIFO-not-empty,
 		 * we need to retry s3c_hsotg_handle_rx if this is still
-		 * set. */
+		 * set.
+		 */
 
 		s3c_hsotg_handle_rx(hsotg);
 	}
 
-	if (gintsts & S3C_GINTSTS_ModeMis) {
+	if (gintsts & GINTSTS_ModeMis) {
 		dev_warn(hsotg->dev, "warning, mode mismatch triggered\n");
-		writel(S3C_GINTSTS_ModeMis, hsotg->regs + S3C_GINTSTS);
+		writel(GINTSTS_ModeMis, hsotg->regs + GINTSTS);
 	}
 
-	if (gintsts & S3C_GINTSTS_USBSusp) {
-		dev_info(hsotg->dev, "S3C_GINTSTS_USBSusp\n");
-		writel(S3C_GINTSTS_USBSusp, hsotg->regs + S3C_GINTSTS);
+	if (gintsts & GINTSTS_USBSusp) {
+		dev_info(hsotg->dev, "GINTSTS_USBSusp\n");
+		writel(GINTSTS_USBSusp, hsotg->regs + GINTSTS);
 
 		call_gadget(hsotg, suspend);
+		s3c_hsotg_disconnect(hsotg);
 	}
 
-	if (gintsts & S3C_GINTSTS_WkUpInt) {
-		dev_info(hsotg->dev, "S3C_GINTSTS_WkUpIn\n");
-		writel(S3C_GINTSTS_WkUpInt, hsotg->regs + S3C_GINTSTS);
+	if (gintsts & GINTSTS_WkUpInt) {
+		dev_info(hsotg->dev, "GINTSTS_WkUpIn\n");
+		writel(GINTSTS_WkUpInt, hsotg->regs + GINTSTS);
 
 		call_gadget(hsotg, resume);
 	}
 
-	if (gintsts & S3C_GINTSTS_ErlySusp) {
-		dev_dbg(hsotg->dev, "S3C_GINTSTS_ErlySusp\n");
-		writel(S3C_GINTSTS_ErlySusp, hsotg->regs + S3C_GINTSTS);
+	if (gintsts & GINTSTS_ErlySusp) {
+		dev_dbg(hsotg->dev, "GINTSTS_ErlySusp\n");
+		writel(GINTSTS_ErlySusp, hsotg->regs + GINTSTS);
+
+		s3c_hsotg_disconnect(hsotg);
 	}
 
-	/* these next two seem to crop-up occasionally causing the core
+	/*
+	 * these next two seem to crop-up occasionally causing the core
 	 * to shutdown the USB transfer, so try clearing them and logging
-	 * the occurrence. */
+	 * the occurrence.
+	 */
 
-	if (gintsts & S3C_GINTSTS_GOUTNakEff) {
+	if (gintsts & GINTSTS_GOUTNakEff) {
 		dev_info(hsotg->dev, "GOUTNakEff triggered\n");
 
-		writel(S3C_DCTL_CGOUTNak, hsotg->regs + S3C_DCTL);
+		writel(DCTL_CGOUTNak, hsotg->regs + DCTL);
 
 		s3c_hsotg_dump(hsotg);
 	}
 
-	if (gintsts & S3C_GINTSTS_GINNakEff) {
+	if (gintsts & GINTSTS_GINNakEff) {
 		dev_info(hsotg->dev, "GINNakEff triggered\n");
 
-		writel(S3C_DCTL_CGNPInNAK, hsotg->regs + S3C_DCTL);
+		writel(DCTL_CGNPInNAK, hsotg->regs + DCTL);
 
 		s3c_hsotg_dump(hsotg);
 	}
 
-	/* if we've had fifo events, we should try and go around the
-	 * loop again to see if there's any point in returning yet. */
+	/*
+	 * if we've had fifo events, we should try and go around the
+	 * loop again to see if there's any point in returning yet.
+	 */
 
 	if (gintsts & IRQ_RETRY_MASK && --retry_count > 0)
 			goto irq_retry;
@@ -2268,7 +2566,7 @@ irq_retry:
  * @desc: The USB endpoint descriptor to configure with.
  *
  * This is called from the USB gadget code's usb_ep_enable().
-*/
+ */
 static int s3c_hsotg_ep_enable(struct usb_ep *ep,
 			       const struct usb_endpoint_descriptor *desc)
 {
@@ -2300,7 +2598,7 @@ static int s3c_hsotg_ep_enable(struct usb_ep *ep,
 
 	/* note, we handle this here instead of s3c_hsotg_set_ep_maxpacket */
 
-	epctrl_reg = dir_in ? S3C_DIEPCTL(index) : S3C_DOEPCTL(index);
+	epctrl_reg = dir_in ? DIEPCTL(index) : DOEPCTL(index);
 	epctrl = readl(hsotg->regs + epctrl_reg);
 
 	dev_dbg(hsotg->dev, "%s: read DxEPCTL=0x%08x from 0x%08x\n",
@@ -2308,20 +2606,23 @@ static int s3c_hsotg_ep_enable(struct usb_ep *ep,
 
 	spin_lock_irqsave(&hs_ep->lock, flags);
 
-	epctrl &= ~(S3C_DxEPCTL_EPType_MASK | S3C_DxEPCTL_MPS_MASK);
-	epctrl |= S3C_DxEPCTL_MPS(mps);
+	epctrl &= ~(DxEPCTL_EPType_MASK | DxEPCTL_MPS_MASK);
+	epctrl |= DxEPCTL_MPS(mps);
 
-	/* mark the endpoint as active, otherwise the core may ignore
-	 * transactions entirely for this endpoint */
-	epctrl |= S3C_DxEPCTL_USBActEp;
+	/*
+	 * mark the endpoint as active, otherwise the core may ignore
+	 * transactions entirely for this endpoint
+	 */
+	epctrl |= DxEPCTL_USBActEp;
 
-	/* set the NAK status on the endpoint, otherwise we might try and
+	/*
+	 * set the NAK status on the endpoint, otherwise we might try and
 	 * do something with data that we've yet got a request to process
 	 * since the RXFIFO will take data for an endpoint even if the
 	 * size register hasn't been set.
 	 */
 
-	epctrl |= S3C_DxEPCTL_SNAK;
+	epctrl |= DxEPCTL_SNAK;
 
 	/* update the endpoint state */
 	hs_ep->ep.maxpacket = mps;
@@ -2336,37 +2637,40 @@ static int s3c_hsotg_ep_enable(struct usb_ep *ep,
 		goto out;
 
 	case USB_ENDPOINT_XFER_BULK:
-		epctrl |= S3C_DxEPCTL_EPType_Bulk;
+		epctrl |= DxEPCTL_EPType_Bulk;
 		break;
 
 	case USB_ENDPOINT_XFER_INT:
 		if (dir_in) {
-			/* Allocate our TxFNum by simply using the index
+			/*
+			 * Allocate our TxFNum by simply using the index
 			 * of the endpoint for the moment. We could do
 			 * something better if the host indicates how
-			 * many FIFOs we are expecting to use. */
+			 * many FIFOs we are expecting to use.
+			 */
 
 			hs_ep->periodic = 1;
-			epctrl |= S3C_DxEPCTL_TxFNum(index);
+			epctrl |= DxEPCTL_TxFNum(index);
 		}
 
-		epctrl |= S3C_DxEPCTL_EPType_Intterupt;
+		epctrl |= DxEPCTL_EPType_Intterupt;
 		break;
 
 	case USB_ENDPOINT_XFER_CONTROL:
-		epctrl |= S3C_DxEPCTL_EPType_Control;
+		epctrl |= DxEPCTL_EPType_Control;
 		break;
 	}
 
-	/* if the hardware has dedicated fifos, we must give each IN EP
+	/*
+	 * if the hardware has dedicated fifos, we must give each IN EP
 	 * a unique tx-fifo even if it is non-periodic.
 	 */
 	if (dir_in && hsotg->dedicated_fifos)
-		epctrl |= S3C_DxEPCTL_TxFNum(index);
+		epctrl |= DxEPCTL_TxFNum(index);
 
 	/* for non control endpoints, set PID to D0 */
 	if (index)
-		epctrl |= S3C_DxEPCTL_SetD0PID;
+		epctrl |= DxEPCTL_SetD0PID;
 
 	dev_dbg(hsotg->dev, "%s: write DxEPCTL=0x%08x\n",
 		__func__, epctrl);
@@ -2383,6 +2687,10 @@ out:
 	return ret;
 }
 
+/**
+ * s3c_hsotg_ep_disable - disable given endpoint
+ * @ep: The endpoint to disable.
+ */
 static int s3c_hsotg_ep_disable(struct usb_ep *ep)
 {
 	struct s3c_hsotg_ep *hs_ep = our_ep(ep);
@@ -2400,7 +2708,7 @@ static int s3c_hsotg_ep_disable(struct usb_ep *ep)
 		return -EINVAL;
 	}
 
-	epctrl_reg = dir_in ? S3C_DIEPCTL(index) : S3C_DOEPCTL(index);
+	epctrl_reg = dir_in ? DIEPCTL(index) : DOEPCTL(index);
 
 	/* terminate all requests with shutdown */
 	kill_all_requests(hsotg, hs_ep, -ESHUTDOWN, false);
@@ -2408,9 +2716,9 @@ static int s3c_hsotg_ep_disable(struct usb_ep *ep)
 	spin_lock_irqsave(&hs_ep->lock, flags);
 
 	ctrl = readl(hsotg->regs + epctrl_reg);
-	ctrl &= ~S3C_DxEPCTL_EPEna;
-	ctrl &= ~S3C_DxEPCTL_USBActEp;
-	ctrl |= S3C_DxEPCTL_SNAK;
+	ctrl &= ~DxEPCTL_EPEna;
+	ctrl &= ~DxEPCTL_USBActEp;
+	ctrl |= DxEPCTL_SNAK;
 
 	dev_dbg(hsotg->dev, "%s: DxEPCTL=0x%08x\n", __func__, ctrl);
 	writel(ctrl, hsotg->regs + epctrl_reg);
@@ -2426,7 +2734,7 @@ static int s3c_hsotg_ep_disable(struct usb_ep *ep)
  * on_list - check request is on the given endpoint
  * @ep: The endpoint to check.
  * @test: The request to test if it is on the endpoint.
-*/
+ */
 static bool on_list(struct s3c_hsotg_ep *ep, struct s3c_hsotg_req *test)
 {
 	struct s3c_hsotg_req *req, *treq;
@@ -2439,6 +2747,11 @@ static bool on_list(struct s3c_hsotg_ep *ep, struct s3c_hsotg_req *test)
 	return false;
 }
 
+/**
+ * s3c_hsotg_ep_dequeue - dequeue given endpoint
+ * @ep: The endpoint to dequeue.
+ * @req: The request to be removed from a queue.
+ */
 static int s3c_hsotg_ep_dequeue(struct usb_ep *ep, struct usb_request *req)
 {
 	struct s3c_hsotg_req *hs_req = our_req(req);
@@ -2461,6 +2774,11 @@ static int s3c_hsotg_ep_dequeue(struct usb_ep *ep, struct usb_request *req)
 	return 0;
 }
 
+/**
+ * s3c_hsotg_ep_sethalt - set halt on a given endpoint
+ * @ep: The endpoint to set halt.
+ * @value: Set or unset the halt.
+ */
 static int s3c_hsotg_ep_sethalt(struct usb_ep *ep, int value)
 {
 	struct s3c_hsotg_ep *hs_ep = our_ep(ep);
@@ -2477,34 +2795,34 @@ static int s3c_hsotg_ep_sethalt(struct usb_ep *ep, int value)
 
 	/* write both IN and OUT control registers */
 
-	epreg = S3C_DIEPCTL(index);
+	epreg = DIEPCTL(index);
 	epctl = readl(hs->regs + epreg);
 
 	if (value) {
-		epctl |= S3C_DxEPCTL_Stall + S3C_DxEPCTL_SNAK;
-		if (epctl & S3C_DxEPCTL_EPEna)
-			epctl |= S3C_DxEPCTL_EPDis;
+		epctl |= DxEPCTL_Stall + DxEPCTL_SNAK;
+		if (epctl & DxEPCTL_EPEna)
+			epctl |= DxEPCTL_EPDis;
 	} else {
-		epctl &= ~S3C_DxEPCTL_Stall;
-		xfertype = epctl & S3C_DxEPCTL_EPType_MASK;
-		if (xfertype == S3C_DxEPCTL_EPType_Bulk ||
-			xfertype == S3C_DxEPCTL_EPType_Intterupt)
-				epctl |= S3C_DxEPCTL_SetD0PID;
+		epctl &= ~DxEPCTL_Stall;
+		xfertype = epctl & DxEPCTL_EPType_MASK;
+		if (xfertype == DxEPCTL_EPType_Bulk ||
+			xfertype == DxEPCTL_EPType_Intterupt)
+				epctl |= DxEPCTL_SetD0PID;
 	}
 
 	writel(epctl, hs->regs + epreg);
 
-	epreg = S3C_DOEPCTL(index);
+	epreg = DOEPCTL(index);
 	epctl = readl(hs->regs + epreg);
 
 	if (value)
-		epctl |= S3C_DxEPCTL_Stall;
+		epctl |= DxEPCTL_Stall;
 	else {
-		epctl &= ~S3C_DxEPCTL_Stall;
-		xfertype = epctl & S3C_DxEPCTL_EPType_MASK;
-		if (xfertype == S3C_DxEPCTL_EPType_Bulk ||
-			xfertype == S3C_DxEPCTL_EPType_Intterupt)
-				epctl |= S3C_DxEPCTL_SetD0PID;
+		epctl &= ~DxEPCTL_Stall;
+		xfertype = epctl & DxEPCTL_EPType_MASK;
+		if (xfertype == DxEPCTL_EPType_Bulk ||
+			xfertype == DxEPCTL_EPType_Intterupt)
+				epctl |= DxEPCTL_SetD0PID;
 	}
 
 	writel(epctl, hs->regs + epreg);
@@ -2526,57 +2844,91 @@ static struct usb_ep_ops s3c_hsotg_ep_ops = {
 };
 
 /**
- * s3c_hsotg_corereset - issue softreset to the core
- * @hsotg: The device state
+ * s3c_hsotg_phy_enable - enable platform phy dev
+ * @hsotg: The driver state
  *
- * Issue a soft reset to the core, and await the core finishing it.
-*/
-static int s3c_hsotg_corereset(struct s3c_hsotg *hsotg)
+ * A wrapper for platform code responsible for controlling
+ * low-level USB code
+ */
+static void s3c_hsotg_phy_enable(struct s3c_hsotg *hsotg)
 {
-	int timeout;
-	u32 grstctl;
+	struct platform_device *pdev = to_platform_device(hsotg->dev);
 
-	dev_dbg(hsotg->dev, "resetting core\n");
+	dev_dbg(hsotg->dev, "pdev 0x%p\n", pdev);
+	if (hsotg->plat->phy_init)
+		hsotg->plat->phy_init(pdev, hsotg->plat->phy_type);
+}
 
-	/* issue soft reset */
-	writel(S3C_GRSTCTL_CSftRst, hsotg->regs + S3C_GRSTCTL);
+/**
+ * s3c_hsotg_phy_disable - disable platform phy dev
+ * @hsotg: The driver state
+ *
+ * A wrapper for platform code responsible for controlling
+ * low-level USB code
+ */
+static void s3c_hsotg_phy_disable(struct s3c_hsotg *hsotg)
+{
+	struct platform_device *pdev = to_platform_device(hsotg->dev);
 
-	timeout = 1000;
-	do {
-		grstctl = readl(hsotg->regs + S3C_GRSTCTL);
-	} while ((grstctl & S3C_GRSTCTL_CSftRst) && timeout-- > 0);
+	if (hsotg->plat->phy_exit)
+		hsotg->plat->phy_exit(pdev, hsotg->plat->phy_type);
+}
 
-	if (grstctl & S3C_GRSTCTL_CSftRst) {
-		dev_err(hsotg->dev, "Failed to get CSftRst asserted\n");
-		return -EINVAL;
-	}
+/**
+ * s3c_hsotg_init - initalize the usb core
+ * @hsotg: The driver state
+ */
+static void s3c_hsotg_init(struct s3c_hsotg *hsotg)
+{
+	/* unmask subset of endpoint interrupts */
 
-	timeout = 1000;
+	writel(DIEPMSK_TimeOUTMsk | DIEPMSK_AHBErrMsk |
+	       DIEPMSK_EPDisbldMsk | DIEPMSK_XferComplMsk,
+	       hsotg->regs + DIEPMSK);
 
-	while (1) {
-		u32 grstctl = readl(hsotg->regs + S3C_GRSTCTL);
+	writel(DOEPMSK_SetupMsk | DOEPMSK_AHBErrMsk |
+	       DOEPMSK_EPDisbldMsk | DOEPMSK_XferComplMsk,
+	       hsotg->regs + DOEPMSK);
 
-		if (timeout-- < 0) {
-			dev_info(hsotg->dev,
-				 "%s: reset failed, GRSTCTL=%08x\n",
-				 __func__, grstctl);
-			return -ETIMEDOUT;
-		}
+	writel(0, hsotg->regs + DAINTMSK);
 
-		if (!(grstctl & S3C_GRSTCTL_AHBIdle))
-			continue;
+	/* Be in disconnected state until gadget is registered */
+	__orr32(hsotg->regs + DCTL, DCTL_SftDiscon);
 
-		break;		/* reset done */
+	if (0) {
+		/* post global nak until we're ready */
+		writel(DCTL_SGNPInNAK | DCTL_SGOUTNak,
+		       hsotg->regs + DCTL);
 	}
 
-	dev_dbg(hsotg->dev, "reset successful\n");
-	return 0;
+	/* setup fifos */
+
+	dev_dbg(hsotg->dev, "GRXFSIZ=0x%08x, GNPTXFSIZ=0x%08x\n",
+		readl(hsotg->regs + GRXFSIZ),
+		readl(hsotg->regs + GNPTXFSIZ));
+
+	s3c_hsotg_init_fifo(hsotg);
+
+	/* set the PLL on, remove the HNP/SRP and set the PHY */
+	writel(GUSBCFG_PHYIf16 | GUSBCFG_TOutCal(7) | (0x5 << 10),
+	       hsotg->regs + GUSBCFG);
+
+	writel(using_dma(hsotg) ? GAHBCFG_DMAEn : 0x0,
+	       hsotg->regs + GAHBCFG);
 }
 
-static int s3c_hsotg_start(struct usb_gadget_driver *driver,
-		int (*bind)(struct usb_gadget *))
+/**
+ * s3c_hsotg_udc_start - prepare the udc for work
+ * @gadget: The usb gadget state
+ * @driver: The usb gadget driver
+ *
+ * Perform initialization to prepare udc device and driver
+ * to work.
+ */
+static int s3c_hsotg_udc_start(struct usb_gadget *gadget,
+			   struct usb_gadget_driver *driver)
 {
-	struct s3c_hsotg *hsotg = our_hsotg;
+	struct s3c_hsotg *hsotg = to_hsotg(gadget);
 	int ret;
 
 	if (!hsotg) {
@@ -2592,7 +2944,7 @@ static int s3c_hsotg_start(struct usb_gadget_driver *driver,
 	if (driver->max_speed < USB_SPEED_FULL)
 		dev_err(hsotg->dev, "%s: bad speed\n", __func__);
 
-	if (!bind || !driver->setup) {
+	if (!driver->setup) {
 		dev_err(hsotg->dev, "%s: missing entry points\n", __func__);
 		return -EINVAL;
 	}
@@ -2605,135 +2957,17 @@ static int s3c_hsotg_start(struct usb_gadget_driver *driver,
 	hsotg->gadget.dev.dma_mask = hsotg->dev->dma_mask;
 	hsotg->gadget.speed = USB_SPEED_UNKNOWN;
 
-	ret = device_add(&hsotg->gadget.dev);
+	ret = regulator_bulk_enable(ARRAY_SIZE(hsotg->supplies),
+				    hsotg->supplies);
 	if (ret) {
-		dev_err(hsotg->dev, "failed to register gadget device\n");
+		dev_err(hsotg->dev, "failed to enable supplies: %d\n", ret);
 		goto err;
 	}
 
-	ret = bind(&hsotg->gadget);
-	if (ret) {
-		dev_err(hsotg->dev, "failed bind %s\n", driver->driver.name);
-
-		hsotg->gadget.dev.driver = NULL;
-		hsotg->driver = NULL;
-		goto err;
-	}
-
-	/* we must now enable ep0 ready for host detection and then
-	 * set configuration. */
-
-	s3c_hsotg_corereset(hsotg);
-
-	/* set the PLL on, remove the HNP/SRP and set the PHY */
-	writel(S3C_GUSBCFG_PHYIf16 | S3C_GUSBCFG_TOutCal(7) |
-	       (0x5 << 10), hsotg->regs + S3C_GUSBCFG);
-
-	/* looks like soft-reset changes state of FIFOs */
-	s3c_hsotg_init_fifo(hsotg);
-
-	__orr32(hsotg->regs + S3C_DCTL, S3C_DCTL_SftDiscon);
-
-	writel(1 << 18 | S3C_DCFG_DevSpd_HS,  hsotg->regs + S3C_DCFG);
-
-	/* Clear any pending OTG interrupts */
-	writel(0xffffffff, hsotg->regs + S3C_GOTGINT);
-
-	/* Clear any pending interrupts */
-	writel(0xffffffff, hsotg->regs + S3C_GINTSTS);
-
-	writel(S3C_GINTSTS_DisconnInt | S3C_GINTSTS_SessReqInt |
-	       S3C_GINTSTS_ConIDStsChng | S3C_GINTSTS_USBRst |
-	       S3C_GINTSTS_EnumDone | S3C_GINTSTS_OTGInt |
-	       S3C_GINTSTS_USBSusp | S3C_GINTSTS_WkUpInt |
-	       S3C_GINTSTS_GOUTNakEff | S3C_GINTSTS_GINNakEff |
-	       S3C_GINTSTS_ErlySusp,
-	       hsotg->regs + S3C_GINTMSK);
-
-	if (using_dma(hsotg))
-		writel(S3C_GAHBCFG_GlblIntrEn | S3C_GAHBCFG_DMAEn |
-		       S3C_GAHBCFG_HBstLen_Incr4,
-		       hsotg->regs + S3C_GAHBCFG);
-	else
-		writel(S3C_GAHBCFG_GlblIntrEn, hsotg->regs + S3C_GAHBCFG);
-
-	/* Enabling INTknTXFEmpMsk here seems to be a big mistake, we end
-	 * up being flooded with interrupts if the host is polling the
-	 * endpoint to try and read data. */
-
-	writel(S3C_DIEPMSK_TimeOUTMsk | S3C_DIEPMSK_AHBErrMsk |
-	       S3C_DIEPMSK_INTknEPMisMsk |
-	       S3C_DIEPMSK_EPDisbldMsk | S3C_DIEPMSK_XferComplMsk |
-	       ((hsotg->dedicated_fifos) ? S3C_DIEPMSK_TxFIFOEmpty : 0),
-	       hsotg->regs + S3C_DIEPMSK);
-
-	/* don't need XferCompl, we get that from RXFIFO in slave mode. In
-	 * DMA mode we may need this. */
-	writel(S3C_DOEPMSK_SetupMsk | S3C_DOEPMSK_AHBErrMsk |
-	       S3C_DOEPMSK_EPDisbldMsk |
-	       (using_dma(hsotg) ? (S3C_DIEPMSK_XferComplMsk |
-				   S3C_DIEPMSK_TimeOUTMsk) : 0),
-	       hsotg->regs + S3C_DOEPMSK);
-
-	writel(0, hsotg->regs + S3C_DAINTMSK);
-
-	dev_dbg(hsotg->dev, "EP0: DIEPCTL0=0x%08x, DOEPCTL0=0x%08x\n",
-		readl(hsotg->regs + S3C_DIEPCTL0),
-		readl(hsotg->regs + S3C_DOEPCTL0));
-
-	/* enable in and out endpoint interrupts */
-	s3c_hsotg_en_gsint(hsotg, S3C_GINTSTS_OEPInt | S3C_GINTSTS_IEPInt);
-
-	/* Enable the RXFIFO when in slave mode, as this is how we collect
-	 * the data. In DMA mode, we get events from the FIFO but also
-	 * things we cannot process, so do not use it. */
-	if (!using_dma(hsotg))
-		s3c_hsotg_en_gsint(hsotg, S3C_GINTSTS_RxFLvl);
-
-	/* Enable interrupts for EP0 in and out */
-	s3c_hsotg_ctrl_epint(hsotg, 0, 0, 1);
-	s3c_hsotg_ctrl_epint(hsotg, 0, 1, 1);
-
-	__orr32(hsotg->regs + S3C_DCTL, S3C_DCTL_PWROnPrgDone);
-	udelay(10);  /* see openiboot */
-	__bic32(hsotg->regs + S3C_DCTL, S3C_DCTL_PWROnPrgDone);
-
-	dev_dbg(hsotg->dev, "DCTL=0x%08x\n", readl(hsotg->regs + S3C_DCTL));
-
-	/* S3C_DxEPCTL_USBActEp says RO in manual, but seems to be set by
-	   writing to the EPCTL register.. */
-
-	/* set to read 1 8byte packet */
-	writel(S3C_DxEPTSIZ_MC(1) | S3C_DxEPTSIZ_PktCnt(1) |
-	       S3C_DxEPTSIZ_XferSize(8), hsotg->regs + DOEPTSIZ0);
-
-	writel(s3c_hsotg_ep0_mps(hsotg->eps[0].ep.maxpacket) |
-	       S3C_DxEPCTL_CNAK | S3C_DxEPCTL_EPEna |
-	       S3C_DxEPCTL_USBActEp,
-	       hsotg->regs + S3C_DOEPCTL0);
-
-	/* enable, but don't activate EP0in */
-	writel(s3c_hsotg_ep0_mps(hsotg->eps[0].ep.maxpacket) |
-	       S3C_DxEPCTL_USBActEp, hsotg->regs + S3C_DIEPCTL0);
-
-	s3c_hsotg_enqueue_setup(hsotg);
-
-	dev_dbg(hsotg->dev, "EP0: DIEPCTL0=0x%08x, DOEPCTL0=0x%08x\n",
-		readl(hsotg->regs + S3C_DIEPCTL0),
-		readl(hsotg->regs + S3C_DOEPCTL0));
-
-	/* clear global NAKs */
-	writel(S3C_DCTL_CGOUTNak | S3C_DCTL_CGNPInNAK,
-	       hsotg->regs + S3C_DCTL);
-
-	/* must be at-least 3ms to allow bus to see disconnect */
-	msleep(3);
-
-	/* remove the soft-disconnect and let's go */
-	__bic32(hsotg->regs + S3C_DCTL, S3C_DCTL_SftDiscon);
-
-	/* report to the user, and return */
+	s3c_hsotg_phy_enable(hsotg);
 
+	s3c_hsotg_core_init(hsotg);
+	hsotg->last_rst = jiffies;
 	dev_info(hsotg->dev, "bound driver %s\n", driver->driver.name);
 	return 0;
 
@@ -2743,9 +2977,17 @@ err:
 	return ret;
 }
 
-static int s3c_hsotg_stop(struct usb_gadget_driver *driver)
+/**
+ * s3c_hsotg_udc_stop - stop the udc
+ * @gadget: The usb gadget state
+ * @driver: The usb gadget driver
+ *
+ * Stop udc hw block and stay tunned for future transmissions
+ */
+static int s3c_hsotg_udc_stop(struct usb_gadget *gadget,
+			  struct usb_gadget_driver *driver)
 {
-	struct s3c_hsotg *hsotg = our_hsotg;
+	struct s3c_hsotg *hsotg = to_hsotg(gadget);
 	int ep;
 
 	if (!hsotg)
@@ -2755,16 +2997,15 @@ static int s3c_hsotg_stop(struct usb_gadget_driver *driver)
 		return -EINVAL;
 
 	/* all endpoints should be shutdown */
-	for (ep = 0; ep < S3C_HSOTG_EPS; ep++)
+	for (ep = 0; ep < hsotg->num_of_eps; ep++)
 		s3c_hsotg_ep_disable(&hsotg->eps[ep].ep);
 
-	call_gadget(hsotg, disconnect);
+	s3c_hsotg_phy_disable(hsotg);
+	regulator_bulk_disable(ARRAY_SIZE(hsotg->supplies), hsotg->supplies);
 
-	driver->unbind(&hsotg->gadget);
 	hsotg->driver = NULL;
 	hsotg->gadget.speed = USB_SPEED_UNKNOWN;
-
-	device_del(&hsotg->gadget.dev);
+	hsotg->gadget.dev.driver = NULL;
 
 	dev_info(hsotg->dev, "unregistered gadget driver '%s'\n",
 		 driver->driver.name);
@@ -2772,6 +3013,12 @@ static int s3c_hsotg_stop(struct usb_gadget_driver *driver)
 	return 0;
 }
 
+/**
+ * s3c_hsotg_gadget_getframe - read the frame number
+ * @gadget: The usb gadget state
+ *
+ * Read the {micro} frame number
+ */
 static int s3c_hsotg_gadget_getframe(struct usb_gadget *gadget)
 {
 	return s3c_hsotg_read_frameno(to_hsotg(gadget));
@@ -2779,8 +3026,8 @@ static int s3c_hsotg_gadget_getframe(struct usb_gadget *gadget)
 
 static struct usb_gadget_ops s3c_hsotg_gadget_ops = {
 	.get_frame	= s3c_hsotg_gadget_getframe,
-	.start		= s3c_hsotg_start,
-	.stop		= s3c_hsotg_stop,
+	.udc_start		= s3c_hsotg_udc_start,
+	.udc_stop		= s3c_hsotg_udc_stop,
 };
 
 /**
@@ -2827,111 +3074,42 @@ static void __devinit s3c_hsotg_initep(struct s3c_hsotg *hsotg,
 	hs_ep->ep.maxpacket = epnum ? 512 : EP0_MPS_LIMIT;
 	hs_ep->ep.ops = &s3c_hsotg_ep_ops;
 
-	/* Read the FIFO size for the Periodic TX FIFO, even if we're
+	/*
+	 * Read the FIFO size for the Periodic TX FIFO, even if we're
 	 * an OUT endpoint, we may as well do this if in future the
 	 * code is changed to make each endpoint's direction changeable.
 	 */
 
-	ptxfifo = readl(hsotg->regs + S3C_DPTXFSIZn(epnum));
-	hs_ep->fifo_size = S3C_DPTXFSIZn_DPTxFSize_GET(ptxfifo) * 4;
+	ptxfifo = readl(hsotg->regs + DPTXFSIZn(epnum));
+	hs_ep->fifo_size = DPTXFSIZn_DPTxFSize_GET(ptxfifo) * 4;
 
-	/* if we're using dma, we need to set the next-endpoint pointer
+	/*
+	 * if we're using dma, we need to set the next-endpoint pointer
 	 * to be something valid.
 	 */
 
 	if (using_dma(hsotg)) {
-		u32 next = S3C_DxEPCTL_NextEp((epnum + 1) % 15);
-		writel(next, hsotg->regs + S3C_DIEPCTL(epnum));
-		writel(next, hsotg->regs + S3C_DOEPCTL(epnum));
+		u32 next = DxEPCTL_NextEp((epnum + 1) % 15);
+		writel(next, hsotg->regs + DIEPCTL(epnum));
+		writel(next, hsotg->regs + DOEPCTL(epnum));
 	}
 }
 
 /**
- * s3c_hsotg_otgreset - reset the OtG phy block
- * @hsotg: The host state.
+ * s3c_hsotg_hw_cfg - read HW configuration registers
+ * @param: The device state
  *
- * Power up the phy, set the basic configuration and start the PHY.
+ * Read the USB core HW configuration registers
  */
-static void s3c_hsotg_otgreset(struct s3c_hsotg *hsotg)
-{
-	struct clk *xusbxti;
-	u32 pwr, osc;
-
-	pwr = readl(S3C_PHYPWR);
-	pwr &= ~0x19;
-	writel(pwr, S3C_PHYPWR);
-	mdelay(1);
-
-	osc = hsotg->plat->is_osc ? S3C_PHYCLK_EXT_OSC : 0;
-
-	xusbxti = clk_get(hsotg->dev, "xusbxti");
-	if (xusbxti && !IS_ERR(xusbxti)) {
-		switch (clk_get_rate(xusbxti)) {
-		case 12*MHZ:
-			osc |= S3C_PHYCLK_CLKSEL_12M;
-			break;
-		case 24*MHZ:
-			osc |= S3C_PHYCLK_CLKSEL_24M;
-			break;
-		default:
-		case 48*MHZ:
-			/* default reference clock */
-			break;
-		}
-		clk_put(xusbxti);
-	}
-
-	writel(osc | 0x10, S3C_PHYCLK);
-
-	/* issue a full set of resets to the otg and core */
-
-	writel(S3C_RSTCON_PHY, S3C_RSTCON);
-	udelay(20);	/* at-least 10uS */
-	writel(0, S3C_RSTCON);
-}
-
-
-static void s3c_hsotg_init(struct s3c_hsotg *hsotg)
+static void s3c_hsotg_hw_cfg(struct s3c_hsotg *hsotg)
 {
-	u32 cfg4;
-
-	/* unmask subset of endpoint interrupts */
-
-	writel(S3C_DIEPMSK_TimeOUTMsk | S3C_DIEPMSK_AHBErrMsk |
-	       S3C_DIEPMSK_EPDisbldMsk | S3C_DIEPMSK_XferComplMsk,
-	       hsotg->regs + S3C_DIEPMSK);
-
-	writel(S3C_DOEPMSK_SetupMsk | S3C_DOEPMSK_AHBErrMsk |
-	       S3C_DOEPMSK_EPDisbldMsk | S3C_DOEPMSK_XferComplMsk,
-	       hsotg->regs + S3C_DOEPMSK);
-
-	writel(0, hsotg->regs + S3C_DAINTMSK);
-
-	/* Be in disconnected state until gadget is registered */
-	__orr32(hsotg->regs + S3C_DCTL, S3C_DCTL_SftDiscon);
-
-	if (0) {
-		/* post global nak until we're ready */
-		writel(S3C_DCTL_SGNPInNAK | S3C_DCTL_SGOUTNak,
-		       hsotg->regs + S3C_DCTL);
-	}
-
-	/* setup fifos */
-
-	dev_dbg(hsotg->dev, "GRXFSIZ=0x%08x, GNPTXFSIZ=0x%08x\n",
-		readl(hsotg->regs + S3C_GRXFSIZ),
-		readl(hsotg->regs + S3C_GNPTXFSIZ));
-
-	s3c_hsotg_init_fifo(hsotg);
-
-	/* set the PLL on, remove the HNP/SRP and set the PHY */
-	writel(S3C_GUSBCFG_PHYIf16 | S3C_GUSBCFG_TOutCal(7) | (0x5 << 10),
-	       hsotg->regs + S3C_GUSBCFG);
+	u32 cfg2, cfg4;
+	/* check hardware configuration */
 
-	writel(using_dma(hsotg) ? S3C_GAHBCFG_DMAEn : 0x0,
-	       hsotg->regs + S3C_GAHBCFG);
+	cfg2 = readl(hsotg->regs + 0x48);
+	hsotg->num_of_eps = (cfg2 >> 10) & 0xF;
 
-	/* check hardware configuration */
+	dev_info(hsotg->dev, "EPs:%d\n", hsotg->num_of_eps);
 
 	cfg4 = readl(hsotg->regs + 0x50);
 	hsotg->dedicated_fifos = (cfg4 >> 25) & 1;
@@ -2940,6 +3118,10 @@ static void s3c_hsotg_init(struct s3c_hsotg *hsotg)
 		 hsotg->dedicated_fifos ? "dedicated" : "shared");
 }
 
+/**
+ * s3c_hsotg_dump - dump state of the udc
+ * @param: The device state
+ */
 static void s3c_hsotg_dump(struct s3c_hsotg *hsotg)
 {
 #ifdef DEBUG
@@ -2949,46 +3131,45 @@ static void s3c_hsotg_dump(struct s3c_hsotg *hsotg)
 	int idx;
 
 	dev_info(dev, "DCFG=0x%08x, DCTL=0x%08x, DIEPMSK=%08x\n",
-		 readl(regs + S3C_DCFG), readl(regs + S3C_DCTL),
-		 readl(regs + S3C_DIEPMSK));
+		 readl(regs + DCFG), readl(regs + DCTL),
+		 readl(regs + DIEPMSK));
 
 	dev_info(dev, "GAHBCFG=0x%08x, 0x44=0x%08x\n",
-		 readl(regs + S3C_GAHBCFG), readl(regs + 0x44));
+		 readl(regs + GAHBCFG), readl(regs + 0x44));
 
 	dev_info(dev, "GRXFSIZ=0x%08x, GNPTXFSIZ=0x%08x\n",
-		 readl(regs + S3C_GRXFSIZ), readl(regs + S3C_GNPTXFSIZ));
+		 readl(regs + GRXFSIZ), readl(regs + GNPTXFSIZ));
 
 	/* show periodic fifo settings */
 
 	for (idx = 1; idx <= 15; idx++) {
-		val = readl(regs + S3C_DPTXFSIZn(idx));
+		val = readl(regs + DPTXFSIZn(idx));
 		dev_info(dev, "DPTx[%d] FSize=%d, StAddr=0x%08x\n", idx,
-			 val >> S3C_DPTXFSIZn_DPTxFSize_SHIFT,
-			 val & S3C_DPTXFSIZn_DPTxFStAddr_MASK);
+			 val >> DPTXFSIZn_DPTxFSize_SHIFT,
+			 val & DPTXFSIZn_DPTxFStAddr_MASK);
 	}
 
 	for (idx = 0; idx < 15; idx++) {
 		dev_info(dev,
 			 "ep%d-in: EPCTL=0x%08x, SIZ=0x%08x, DMA=0x%08x\n", idx,
-			 readl(regs + S3C_DIEPCTL(idx)),
-			 readl(regs + S3C_DIEPTSIZ(idx)),
-			 readl(regs + S3C_DIEPDMA(idx)));
+			 readl(regs + DIEPCTL(idx)),
+			 readl(regs + DIEPTSIZ(idx)),
+			 readl(regs + DIEPDMA(idx)));
 
-		val = readl(regs + S3C_DOEPCTL(idx));
+		val = readl(regs + DOEPCTL(idx));
 		dev_info(dev,
 			 "ep%d-out: EPCTL=0x%08x, SIZ=0x%08x, DMA=0x%08x\n",
-			 idx, readl(regs + S3C_DOEPCTL(idx)),
-			 readl(regs + S3C_DOEPTSIZ(idx)),
-			 readl(regs + S3C_DOEPDMA(idx)));
+			 idx, readl(regs + DOEPCTL(idx)),
+			 readl(regs + DOEPTSIZ(idx)),
+			 readl(regs + DOEPDMA(idx)));
 
 	}
 
 	dev_info(dev, "DVBUSDIS=0x%08x, DVBUSPULSE=%08x\n",
-		 readl(regs + S3C_DVBUSDIS), readl(regs + S3C_DVBUSPULSE));
+		 readl(regs + DVBUSDIS), readl(regs + DVBUSPULSE));
 #endif
 }
 
-
 /**
  * state_show - debugfs: show overall driver and device state.
  * @seq: The seq file to write to.
@@ -3005,38 +3186,38 @@ static int state_show(struct seq_file *seq, void *v)
 	int idx;
 
 	seq_printf(seq, "DCFG=0x%08x, DCTL=0x%08x, DSTS=0x%08x\n",
-		 readl(regs + S3C_DCFG),
-		 readl(regs + S3C_DCTL),
-		 readl(regs + S3C_DSTS));
+		 readl(regs + DCFG),
+		 readl(regs + DCTL),
+		 readl(regs + DSTS));
 
 	seq_printf(seq, "DIEPMSK=0x%08x, DOEPMASK=0x%08x\n",
-		   readl(regs + S3C_DIEPMSK), readl(regs + S3C_DOEPMSK));
+		   readl(regs + DIEPMSK), readl(regs + DOEPMSK));
 
 	seq_printf(seq, "GINTMSK=0x%08x, GINTSTS=0x%08x\n",
-		   readl(regs + S3C_GINTMSK),
-		   readl(regs + S3C_GINTSTS));
+		   readl(regs + GINTMSK),
+		   readl(regs + GINTSTS));
 
 	seq_printf(seq, "DAINTMSK=0x%08x, DAINT=0x%08x\n",
-		   readl(regs + S3C_DAINTMSK),
-		   readl(regs + S3C_DAINT));
+		   readl(regs + DAINTMSK),
+		   readl(regs + DAINT));
 
 	seq_printf(seq, "GNPTXSTS=0x%08x, GRXSTSR=%08x\n",
-		   readl(regs + S3C_GNPTXSTS),
-		   readl(regs + S3C_GRXSTSR));
+		   readl(regs + GNPTXSTS),
+		   readl(regs + GRXSTSR));
 
 	seq_printf(seq, "\nEndpoint status:\n");
 
 	for (idx = 0; idx < 15; idx++) {
 		u32 in, out;
 
-		in = readl(regs + S3C_DIEPCTL(idx));
-		out = readl(regs + S3C_DOEPCTL(idx));
+		in = readl(regs + DIEPCTL(idx));
+		out = readl(regs + DOEPCTL(idx));
 
 		seq_printf(seq, "ep%d: DIEPCTL=0x%08x, DOEPCTL=0x%08x",
 			   idx, in, out);
 
-		in = readl(regs + S3C_DIEPTSIZ(idx));
-		out = readl(regs + S3C_DOEPTSIZ(idx));
+		in = readl(regs + DIEPTSIZ(idx));
+		out = readl(regs + DOEPTSIZ(idx));
 
 		seq_printf(seq, ", DIEPTSIZ=0x%08x, DOEPTSIZ=0x%08x",
 			   in, out);
@@ -3067,7 +3248,7 @@ static const struct file_operations state_fops = {
  *
  * Show the FIFO information for the overall fifo and all the
  * periodic transmission FIFOs.
-*/
+ */
 static int fifo_show(struct seq_file *seq, void *v)
 {
 	struct s3c_hsotg *hsotg = seq->private;
@@ -3076,21 +3257,21 @@ static int fifo_show(struct seq_file *seq, void *v)
 	int idx;
 
 	seq_printf(seq, "Non-periodic FIFOs:\n");
-	seq_printf(seq, "RXFIFO: Size %d\n", readl(regs + S3C_GRXFSIZ));
+	seq_printf(seq, "RXFIFO: Size %d\n", readl(regs + GRXFSIZ));
 
-	val = readl(regs + S3C_GNPTXFSIZ);
+	val = readl(regs + GNPTXFSIZ);
 	seq_printf(seq, "NPTXFIFO: Size %d, Start 0x%08x\n",
-		   val >> S3C_GNPTXFSIZ_NPTxFDep_SHIFT,
-		   val & S3C_GNPTXFSIZ_NPTxFStAddr_MASK);
+		   val >> GNPTXFSIZ_NPTxFDep_SHIFT,
+		   val & GNPTXFSIZ_NPTxFStAddr_MASK);
 
 	seq_printf(seq, "\nPeriodic TXFIFOs:\n");
 
 	for (idx = 1; idx <= 15; idx++) {
-		val = readl(regs + S3C_DPTXFSIZn(idx));
+		val = readl(regs + DPTXFSIZn(idx));
 
 		seq_printf(seq, "\tDPTXFIFO%2d: Size %d, Start 0x%08x\n", idx,
-			   val >> S3C_DPTXFSIZn_DPTxFSize_SHIFT,
-			   val & S3C_DPTXFSIZn_DPTxFStAddr_MASK);
+			   val >> DPTXFSIZn_DPTxFSize_SHIFT,
+			   val & DPTXFSIZn_DPTxFStAddr_MASK);
 	}
 
 	return 0;
@@ -3122,7 +3303,7 @@ static const char *decode_direction(int is_in)
  *
  * This debugfs entry shows the state of the given endpoint (one is
  * registered for each available).
-*/
+ */
 static int ep_show(struct seq_file *seq, void *v)
 {
 	struct s3c_hsotg_ep *ep = seq->private;
@@ -3139,20 +3320,20 @@ static int ep_show(struct seq_file *seq, void *v)
 	/* first show the register state */
 
 	seq_printf(seq, "\tDIEPCTL=0x%08x, DOEPCTL=0x%08x\n",
-		   readl(regs + S3C_DIEPCTL(index)),
-		   readl(regs + S3C_DOEPCTL(index)));
+		   readl(regs + DIEPCTL(index)),
+		   readl(regs + DOEPCTL(index)));
 
 	seq_printf(seq, "\tDIEPDMA=0x%08x, DOEPDMA=0x%08x\n",
-		   readl(regs + S3C_DIEPDMA(index)),
-		   readl(regs + S3C_DOEPDMA(index)));
+		   readl(regs + DIEPDMA(index)),
+		   readl(regs + DOEPDMA(index)));
 
 	seq_printf(seq, "\tDIEPINT=0x%08x, DOEPINT=0x%08x\n",
-		   readl(regs + S3C_DIEPINT(index)),
-		   readl(regs + S3C_DOEPINT(index)));
+		   readl(regs + DIEPINT(index)),
+		   readl(regs + DOEPINT(index)));
 
 	seq_printf(seq, "\tDIEPTSIZ=0x%08x, DOEPTSIZ=0x%08x\n",
-		   readl(regs + S3C_DIEPTSIZ(index)),
-		   readl(regs + S3C_DOEPTSIZ(index)));
+		   readl(regs + DIEPTSIZ(index)),
+		   readl(regs + DOEPTSIZ(index)));
 
 	seq_printf(seq, "\n");
 	seq_printf(seq, "mps %d\n", ep->ep.maxpacket);
@@ -3202,7 +3383,7 @@ static const struct file_operations ep_fops = {
  * about the state of the system. The directory name is created
  * with the same name as the device itself, in case we end up
  * with multiple blocks in future systems.
-*/
+ */
 static void __devinit s3c_hsotg_create_debug(struct s3c_hsotg *hsotg)
 {
 	struct dentry *root;
@@ -3231,7 +3412,7 @@ static void __devinit s3c_hsotg_create_debug(struct s3c_hsotg *hsotg)
 
 	/* create one file for each endpoint */
 
-	for (epidx = 0; epidx < S3C_HSOTG_EPS; epidx++) {
+	for (epidx = 0; epidx < hsotg->num_of_eps; epidx++) {
 		struct s3c_hsotg_ep *ep = &hsotg->eps[epidx];
 
 		ep->debugfs = debugfs_create_file(ep->name, 0444,
@@ -3248,12 +3429,12 @@ static void __devinit s3c_hsotg_create_debug(struct s3c_hsotg *hsotg)
  * @hsotg: The driver state
  *
  * Cleanup (remove) the debugfs files for use on module exit.
-*/
+ */
 static void __devexit s3c_hsotg_delete_debug(struct s3c_hsotg *hsotg)
 {
 	unsigned epidx;
 
-	for (epidx = 0; epidx < S3C_HSOTG_EPS; epidx++) {
+	for (epidx = 0; epidx < hsotg->num_of_eps; epidx++) {
 		struct s3c_hsotg_ep *ep = &hsotg->eps[epidx];
 		debugfs_remove(ep->debugfs);
 	}
@@ -3264,48 +3445,39 @@ static void __devexit s3c_hsotg_delete_debug(struct s3c_hsotg *hsotg)
 }
 
 /**
- * s3c_hsotg_gate - set the hardware gate for the block
- * @pdev: The device we bound to
- * @on: On or off.
- *
- * Set the hardware gate setting into the block. If we end up on
- * something other than an S3C64XX, then we might need to change this
- * to using a platform data callback, or some other mechanism.
+ * s3c_hsotg_release - release callback for hsotg device
+ * @dev: Device to for which release is called
  */
-static void s3c_hsotg_gate(struct platform_device *pdev, bool on)
+static void s3c_hsotg_release(struct device *dev)
 {
-	unsigned long flags;
-	u32 others;
-
-	local_irq_save(flags);
-
-	others = __raw_readl(S3C64XX_OTHERS);
-	if (on)
-		others |= S3C64XX_OTHERS_USBMASK;
-	else
-		others &= ~S3C64XX_OTHERS_USBMASK;
-	__raw_writel(others, S3C64XX_OTHERS);
+	struct s3c_hsotg *hsotg = dev_get_drvdata(dev);
 
-	local_irq_restore(flags);
+	kfree(hsotg);
 }
 
-static struct s3c_hsotg_plat s3c_hsotg_default_pdata;
+/**
+ * s3c_hsotg_probe - probe function for hsotg driver
+ * @pdev: The platform information for the driver
+ */
 
 static int __devinit s3c_hsotg_probe(struct platform_device *pdev)
 {
 	struct s3c_hsotg_plat *plat = pdev->dev.platform_data;
 	struct device *dev = &pdev->dev;
+	struct s3c_hsotg_ep *eps;
 	struct s3c_hsotg *hsotg;
 	struct resource *res;
 	int epnum;
 	int ret;
+	int i;
 
-	if (!plat)
-		plat = &s3c_hsotg_default_pdata;
+	plat = pdev->dev.platform_data;
+	if (!plat) {
+		dev_err(&pdev->dev, "no platform data defined\n");
+		return -EINVAL;
+	}
 
-	hsotg = kzalloc(sizeof(struct s3c_hsotg) +
-			sizeof(struct s3c_hsotg_ep) * S3C_HSOTG_EPS,
-			GFP_KERNEL);
+	hsotg = kzalloc(sizeof(struct s3c_hsotg), GFP_KERNEL);
 	if (!hsotg) {
 		dev_err(dev, "cannot get memory\n");
 		return -ENOMEM;
@@ -3371,6 +3543,54 @@ static int __devinit s3c_hsotg_probe(struct platform_device *pdev)
 
 	hsotg->gadget.dev.parent = dev;
 	hsotg->gadget.dev.dma_mask = dev->dma_mask;
+	hsotg->gadget.dev.release = s3c_hsotg_release;
+
+	/* reset the system */
+
+	clk_prepare_enable(hsotg->clk);
+
+	/* regulators */
+
+	for (i = 0; i < ARRAY_SIZE(hsotg->supplies); i++)
+		hsotg->supplies[i].supply = s3c_hsotg_supply_names[i];
+
+	ret = regulator_bulk_get(dev, ARRAY_SIZE(hsotg->supplies),
+				 hsotg->supplies);
+	if (ret) {
+		dev_err(dev, "failed to request supplies: %d\n", ret);
+		goto err_irq;
+	}
+
+	ret = regulator_bulk_enable(ARRAY_SIZE(hsotg->supplies),
+				    hsotg->supplies);
+
+	if (ret) {
+		dev_err(hsotg->dev, "failed to enable supplies: %d\n", ret);
+		goto err_supplies;
+	}
+
+	/* usb phy enable */
+	s3c_hsotg_phy_enable(hsotg);
+
+	s3c_hsotg_corereset(hsotg);
+	s3c_hsotg_init(hsotg);
+	s3c_hsotg_hw_cfg(hsotg);
+
+	/* hsotg->num_of_eps holds number of EPs other than ep0 */
+
+	if (hsotg->num_of_eps == 0) {
+		dev_err(dev, "wrong number of EPs (zero)\n");
+		goto err_supplies;
+	}
+
+	eps = kcalloc(hsotg->num_of_eps + 1, sizeof(struct s3c_hsotg_ep),
+		      GFP_KERNEL);
+	if (!eps) {
+		dev_err(dev, "cannot get memory\n");
+		goto err_supplies;
+	}
+
+	hsotg->eps = eps;
 
 	/* setup endpoint information */
 
@@ -3383,39 +3603,47 @@ static int __devinit s3c_hsotg_probe(struct platform_device *pdev)
 						     GFP_KERNEL);
 	if (!hsotg->ctrl_req) {
 		dev_err(dev, "failed to allocate ctrl req\n");
-		goto err_regs;
+		goto err_ep_mem;
 	}
 
-	/* reset the system */
+	/* initialise the endpoints now the core has been initialised */
+	for (epnum = 0; epnum < hsotg->num_of_eps; epnum++)
+		s3c_hsotg_initep(hsotg, &hsotg->eps[epnum], epnum);
 
-	clk_enable(hsotg->clk);
+	/* disable power and clock */
 
-	s3c_hsotg_gate(pdev, true);
+	ret = regulator_bulk_disable(ARRAY_SIZE(hsotg->supplies),
+				    hsotg->supplies);
+	if (ret) {
+		dev_err(hsotg->dev, "failed to disable supplies: %d\n", ret);
+		goto err_ep_mem;
+	}
 
-	s3c_hsotg_otgreset(hsotg);
-	s3c_hsotg_corereset(hsotg);
-	s3c_hsotg_init(hsotg);
+	s3c_hsotg_phy_disable(hsotg);
 
-	/* initialise the endpoints now the core has been initialised */
-	for (epnum = 0; epnum < S3C_HSOTG_EPS; epnum++)
-		s3c_hsotg_initep(hsotg, &hsotg->eps[epnum], epnum);
+	ret = device_add(&hsotg->gadget.dev);
+	if (ret) {
+		put_device(&hsotg->gadget.dev);
+		goto err_ep_mem;
+	}
 
 	ret = usb_add_gadget_udc(&pdev->dev, &hsotg->gadget);
 	if (ret)
-		goto err_add_udc;
+		goto err_ep_mem;
 
 	s3c_hsotg_create_debug(hsotg);
 
 	s3c_hsotg_dump(hsotg);
 
-	our_hsotg = hsotg;
 	return 0;
 
-err_add_udc:
-	s3c_hsotg_gate(pdev, false);
-	clk_disable(hsotg->clk);
-	clk_put(hsotg->clk);
-
+err_ep_mem:
+	kfree(eps);
+err_supplies:
+	s3c_hsotg_phy_disable(hsotg);
+	regulator_bulk_free(ARRAY_SIZE(hsotg->supplies), hsotg->supplies);
+err_irq:
+	free_irq(hsotg->irq, hsotg);
 err_regs:
 	iounmap(hsotg->regs);
 
@@ -3423,12 +3651,17 @@ err_regs_res:
 	release_resource(hsotg->regs_res);
 	kfree(hsotg->regs_res);
 err_clk:
+	clk_disable_unprepare(hsotg->clk);
 	clk_put(hsotg->clk);
 err_mem:
 	kfree(hsotg);
 	return ret;
 }
 
+/**
+ * s3c_hsotg_remove - remove function for hsotg driver
+ * @pdev: The platform information for the driver
+ */
 static int __devexit s3c_hsotg_remove(struct platform_device *pdev)
 {
 	struct s3c_hsotg *hsotg = platform_get_drvdata(pdev);
@@ -3437,7 +3670,10 @@ static int __devexit s3c_hsotg_remove(struct platform_device *pdev)
 
 	s3c_hsotg_delete_debug(hsotg);
 
-	usb_gadget_unregister_driver(hsotg->driver);
+	if (hsotg->driver) {
+		/* should have been done already by driver model core */
+		usb_gadget_unregister_driver(hsotg->driver);
+	}
 
 	free_irq(hsotg->irq, hsotg);
 	iounmap(hsotg->regs);
@@ -3445,12 +3681,13 @@ static int __devexit s3c_hsotg_remove(struct platform_device *pdev)
 	release_resource(hsotg->regs_res);
 	kfree(hsotg->regs_res);
 
-	s3c_hsotg_gate(pdev, false);
+	s3c_hsotg_phy_disable(hsotg);
+	regulator_bulk_free(ARRAY_SIZE(hsotg->supplies), hsotg->supplies);
 
-	clk_disable(hsotg->clk);
+	clk_disable_unprepare(hsotg->clk);
 	clk_put(hsotg->clk);
 
-	kfree(hsotg);
+	device_unregister(&hsotg->gadget.dev);
 	return 0;
 }
 
diff --git a/drivers/usb/gadget/s3c-hsotg.h b/drivers/usb/gadget/s3c-hsotg.h
new file mode 100644
index 00000000000..d650b129583
--- /dev/null
+++ b/drivers/usb/gadget/s3c-hsotg.h
@@ -0,0 +1,377 @@
+/* drivers/usb/gadget/s3c-hsotg.h
+ *
+ * Copyright 2008 Openmoko, Inc.
+ * Copyright 2008 Simtec Electronics
+ *      http://armlinux.simtec.co.uk/
+ *      Ben Dooks <ben@simtec.co.uk>
+ *
+ * USB2.0 Highspeed/OtG Synopsis DWC2 device block registers
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+*/
+
+#ifndef __REGS_USB_HSOTG_H
+#define __REGS_USB_HSOTG_H __FILE__
+
+#define HSOTG_REG(x) (x)
+
+#define GOTGCTL				HSOTG_REG(0x000)
+#define GOTGCTL_BSESVLD			(1 << 19)
+#define GOTGCTL_ASESVLD			(1 << 18)
+#define GOTGCTL_DBNC_SHORT			(1 << 17)
+#define GOTGCTL_CONID_B			(1 << 16)
+#define GOTGCTL_DEVHNPEN			(1 << 11)
+#define GOTGCTL_HSSETHNPEN			(1 << 10)
+#define GOTGCTL_HNPREQ				(1 << 9)
+#define GOTGCTL_HSTNEGSCS			(1 << 8)
+#define GOTGCTL_SESREQ				(1 << 1)
+#define GOTGCTL_SESREQSCS			(1 << 0)
+
+#define GOTGINT				HSOTG_REG(0x004)
+#define GOTGINT_DbnceDone			(1 << 19)
+#define GOTGINT_ADevTOUTChg			(1 << 18)
+#define GOTGINT_HstNegDet			(1 << 17)
+#define GOTGINT_HstnegSucStsChng		(1 << 9)
+#define GOTGINT_SesReqSucStsChng		(1 << 8)
+#define GOTGINT_SesEndDet			(1 << 2)
+
+#define GAHBCFG				HSOTG_REG(0x008)
+#define GAHBCFG_PTxFEmpLvl			(1 << 8)
+#define GAHBCFG_NPTxFEmpLvl			(1 << 7)
+#define GAHBCFG_DMAEn				(1 << 5)
+#define GAHBCFG_HBstLen_MASK			(0xf << 1)
+#define GAHBCFG_HBstLen_SHIFT			(1)
+#define GAHBCFG_HBstLen_Single			(0x0 << 1)
+#define GAHBCFG_HBstLen_Incr			(0x1 << 1)
+#define GAHBCFG_HBstLen_Incr4			(0x3 << 1)
+#define GAHBCFG_HBstLen_Incr8			(0x5 << 1)
+#define GAHBCFG_HBstLen_Incr16			(0x7 << 1)
+#define GAHBCFG_GlblIntrEn			(1 << 0)
+
+#define GUSBCFG				HSOTG_REG(0x00C)
+#define GUSBCFG_PHYLPClkSel			(1 << 15)
+#define GUSBCFG_HNPCap				(1 << 9)
+#define GUSBCFG_SRPCap				(1 << 8)
+#define GUSBCFG_PHYIf16			(1 << 3)
+#define GUSBCFG_TOutCal_MASK			(0x7 << 0)
+#define GUSBCFG_TOutCal_SHIFT			(0)
+#define GUSBCFG_TOutCal_LIMIT			(0x7)
+#define GUSBCFG_TOutCal(_x)			((_x) << 0)
+
+#define GRSTCTL				HSOTG_REG(0x010)
+
+#define GRSTCTL_AHBIdle			(1 << 31)
+#define GRSTCTL_DMAReq				(1 << 30)
+#define GRSTCTL_TxFNum_MASK			(0x1f << 6)
+#define GRSTCTL_TxFNum_SHIFT			(6)
+#define GRSTCTL_TxFNum_LIMIT			(0x1f)
+#define GRSTCTL_TxFNum(_x)			((_x) << 6)
+#define GRSTCTL_TxFFlsh			(1 << 5)
+#define GRSTCTL_RxFFlsh			(1 << 4)
+#define GRSTCTL_INTknQFlsh			(1 << 3)
+#define GRSTCTL_FrmCntrRst			(1 << 2)
+#define GRSTCTL_HSftRst			(1 << 1)
+#define GRSTCTL_CSftRst			(1 << 0)
+
+#define GINTSTS				HSOTG_REG(0x014)
+#define GINTMSK				HSOTG_REG(0x018)
+
+#define GINTSTS_WkUpInt			(1 << 31)
+#define GINTSTS_SessReqInt			(1 << 30)
+#define GINTSTS_DisconnInt			(1 << 29)
+#define GINTSTS_ConIDStsChng			(1 << 28)
+#define GINTSTS_PTxFEmp			(1 << 26)
+#define GINTSTS_HChInt				(1 << 25)
+#define GINTSTS_PrtInt				(1 << 24)
+#define GINTSTS_FetSusp			(1 << 22)
+#define GINTSTS_incompIP			(1 << 21)
+#define GINTSTS_IncomplSOIN			(1 << 20)
+#define GINTSTS_OEPInt				(1 << 19)
+#define GINTSTS_IEPInt				(1 << 18)
+#define GINTSTS_EPMis				(1 << 17)
+#define GINTSTS_EOPF				(1 << 15)
+#define GINTSTS_ISOutDrop			(1 << 14)
+#define GINTSTS_EnumDone			(1 << 13)
+#define GINTSTS_USBRst				(1 << 12)
+#define GINTSTS_USBSusp			(1 << 11)
+#define GINTSTS_ErlySusp			(1 << 10)
+#define GINTSTS_GOUTNakEff			(1 << 7)
+#define GINTSTS_GINNakEff			(1 << 6)
+#define GINTSTS_NPTxFEmp			(1 << 5)
+#define GINTSTS_RxFLvl				(1 << 4)
+#define GINTSTS_SOF				(1 << 3)
+#define GINTSTS_OTGInt				(1 << 2)
+#define GINTSTS_ModeMis			(1 << 1)
+#define GINTSTS_CurMod_Host			(1 << 0)
+
+#define GRXSTSR				HSOTG_REG(0x01C)
+#define GRXSTSP				HSOTG_REG(0x020)
+
+#define GRXSTS_FN_MASK				(0x7f << 25)
+#define GRXSTS_FN_SHIFT			(25)
+
+#define GRXSTS_PktSts_MASK			(0xf << 17)
+#define GRXSTS_PktSts_SHIFT			(17)
+#define GRXSTS_PktSts_GlobalOutNAK		(0x1 << 17)
+#define GRXSTS_PktSts_OutRX			(0x2 << 17)
+#define GRXSTS_PktSts_OutDone			(0x3 << 17)
+#define GRXSTS_PktSts_SetupDone		(0x4 << 17)
+#define GRXSTS_PktSts_SetupRX			(0x6 << 17)
+
+#define GRXSTS_DPID_MASK			(0x3 << 15)
+#define GRXSTS_DPID_SHIFT			(15)
+#define GRXSTS_ByteCnt_MASK			(0x7ff << 4)
+#define GRXSTS_ByteCnt_SHIFT			(4)
+#define GRXSTS_EPNum_MASK			(0xf << 0)
+#define GRXSTS_EPNum_SHIFT			(0)
+
+#define GRXFSIZ				HSOTG_REG(0x024)
+
+#define GNPTXFSIZ				HSOTG_REG(0x028)
+
+#define GNPTXFSIZ_NPTxFDep_MASK		(0xffff << 16)
+#define GNPTXFSIZ_NPTxFDep_SHIFT		(16)
+#define GNPTXFSIZ_NPTxFDep_LIMIT		(0xffff)
+#define GNPTXFSIZ_NPTxFDep(_x)			((_x) << 16)
+#define GNPTXFSIZ_NPTxFStAddr_MASK		(0xffff << 0)
+#define GNPTXFSIZ_NPTxFStAddr_SHIFT		(0)
+#define GNPTXFSIZ_NPTxFStAddr_LIMIT		(0xffff)
+#define GNPTXFSIZ_NPTxFStAddr(_x)		((_x) << 0)
+
+#define GNPTXSTS				HSOTG_REG(0x02C)
+
+#define GNPTXSTS_NPtxQTop_MASK			(0x7f << 24)
+#define GNPTXSTS_NPtxQTop_SHIFT		(24)
+
+#define GNPTXSTS_NPTxQSpcAvail_MASK		(0xff << 16)
+#define GNPTXSTS_NPTxQSpcAvail_SHIFT		(16)
+#define GNPTXSTS_NPTxQSpcAvail_GET(_v)		(((_v) >> 16) & 0xff)
+
+#define GNPTXSTS_NPTxFSpcAvail_MASK		(0xffff << 0)
+#define GNPTXSTS_NPTxFSpcAvail_SHIFT		(0)
+#define GNPTXSTS_NPTxFSpcAvail_GET(_v)		(((_v) >> 0) & 0xffff)
+
+
+#define HPTXFSIZ				HSOTG_REG(0x100)
+
+#define DPTXFSIZn(_a)		HSOTG_REG(0x104 + (((_a) - 1) * 4))
+
+#define DPTXFSIZn_DPTxFSize_MASK		(0xffff << 16)
+#define DPTXFSIZn_DPTxFSize_SHIFT		(16)
+#define DPTXFSIZn_DPTxFSize_GET(_v)		(((_v) >> 16) & 0xffff)
+#define DPTXFSIZn_DPTxFSize_LIMIT		(0xffff)
+#define DPTXFSIZn_DPTxFSize(_x)		((_x) << 16)
+
+#define DPTXFSIZn_DPTxFStAddr_MASK		(0xffff << 0)
+#define DPTXFSIZn_DPTxFStAddr_SHIFT		(0)
+
+/* Device mode registers */
+#define DCFG					HSOTG_REG(0x800)
+
+#define DCFG_EPMisCnt_MASK			(0x1f << 18)
+#define DCFG_EPMisCnt_SHIFT			(18)
+#define DCFG_EPMisCnt_LIMIT			(0x1f)
+#define DCFG_EPMisCnt(_x)			((_x) << 18)
+
+#define DCFG_PerFrInt_MASK			(0x3 << 11)
+#define DCFG_PerFrInt_SHIFT			(11)
+#define DCFG_PerFrInt_LIMIT			(0x3)
+#define DCFG_PerFrInt(_x)			((_x) << 11)
+
+#define DCFG_DevAddr_MASK			(0x7f << 4)
+#define DCFG_DevAddr_SHIFT			(4)
+#define DCFG_DevAddr_LIMIT			(0x7f)
+#define DCFG_DevAddr(_x)			((_x) << 4)
+
+#define DCFG_NZStsOUTHShk			(1 << 2)
+
+#define DCFG_DevSpd_MASK			(0x3 << 0)
+#define DCFG_DevSpd_SHIFT			(0)
+#define DCFG_DevSpd_HS				(0x0 << 0)
+#define DCFG_DevSpd_FS				(0x1 << 0)
+#define DCFG_DevSpd_LS				(0x2 << 0)
+#define DCFG_DevSpd_FS48			(0x3 << 0)
+
+#define DCTL					HSOTG_REG(0x804)
+
+#define DCTL_PWROnPrgDone			(1 << 11)
+#define DCTL_CGOUTNak				(1 << 10)
+#define DCTL_SGOUTNak				(1 << 9)
+#define DCTL_CGNPInNAK				(1 << 8)
+#define DCTL_SGNPInNAK				(1 << 7)
+#define DCTL_TstCtl_MASK			(0x7 << 4)
+#define DCTL_TstCtl_SHIFT			(4)
+#define DCTL_GOUTNakSts			(1 << 3)
+#define DCTL_GNPINNakSts			(1 << 2)
+#define DCTL_SftDiscon				(1 << 1)
+#define DCTL_RmtWkUpSig			(1 << 0)
+
+#define DSTS					HSOTG_REG(0x808)
+
+#define DSTS_SOFFN_MASK			(0x3fff << 8)
+#define DSTS_SOFFN_SHIFT			(8)
+#define DSTS_SOFFN_LIMIT			(0x3fff)
+#define DSTS_SOFFN(_x)				((_x) << 8)
+#define DSTS_ErraticErr			(1 << 3)
+#define DSTS_EnumSpd_MASK			(0x3 << 1)
+#define DSTS_EnumSpd_SHIFT			(1)
+#define DSTS_EnumSpd_HS			(0x0 << 1)
+#define DSTS_EnumSpd_FS			(0x1 << 1)
+#define DSTS_EnumSpd_LS			(0x2 << 1)
+#define DSTS_EnumSpd_FS48			(0x3 << 1)
+
+#define DSTS_SuspSts				(1 << 0)
+
+#define DIEPMSK				HSOTG_REG(0x810)
+
+#define DIEPMSK_TxFIFOEmpty			(1 << 7)
+#define DIEPMSK_INEPNakEffMsk			(1 << 6)
+#define DIEPMSK_INTknEPMisMsk			(1 << 5)
+#define DIEPMSK_INTknTXFEmpMsk			(1 << 4)
+#define DIEPMSK_TimeOUTMsk			(1 << 3)
+#define DIEPMSK_AHBErrMsk			(1 << 2)
+#define DIEPMSK_EPDisbldMsk			(1 << 1)
+#define DIEPMSK_XferComplMsk			(1 << 0)
+
+#define DOEPMSK				HSOTG_REG(0x814)
+
+#define DOEPMSK_Back2BackSetup			(1 << 6)
+#define DOEPMSK_OUTTknEPdisMsk			(1 << 4)
+#define DOEPMSK_SetupMsk			(1 << 3)
+#define DOEPMSK_AHBErrMsk			(1 << 2)
+#define DOEPMSK_EPDisbldMsk			(1 << 1)
+#define DOEPMSK_XferComplMsk			(1 << 0)
+
+#define DAINT					HSOTG_REG(0x818)
+#define DAINTMSK				HSOTG_REG(0x81C)
+
+#define DAINT_OutEP_SHIFT			(16)
+#define DAINT_OutEP(x)				(1 << ((x) + 16))
+#define DAINT_InEP(x)				(1 << (x))
+
+#define DTKNQR1				HSOTG_REG(0x820)
+#define DTKNQR2				HSOTG_REG(0x824)
+#define DTKNQR3				HSOTG_REG(0x830)
+#define DTKNQR4				HSOTG_REG(0x834)
+
+#define DVBUSDIS				HSOTG_REG(0x828)
+#define DVBUSPULSE				HSOTG_REG(0x82C)
+
+#define DIEPCTL0				HSOTG_REG(0x900)
+#define DOEPCTL0				HSOTG_REG(0xB00)
+#define DIEPCTL(_a)			HSOTG_REG(0x900 + ((_a) * 0x20))
+#define DOEPCTL(_a)			HSOTG_REG(0xB00 + ((_a) * 0x20))
+
+/* EP0 specialness:
+ * bits[29..28] - reserved (no SetD0PID, SetD1PID)
+ * bits[25..22] - should always be zero, this isn't a periodic endpoint
+ * bits[10..0] - MPS setting differenct for EP0
+ */
+#define D0EPCTL_MPS_MASK			(0x3 << 0)
+#define D0EPCTL_MPS_SHIFT			(0)
+#define D0EPCTL_MPS_64				(0x0 << 0)
+#define D0EPCTL_MPS_32				(0x1 << 0)
+#define D0EPCTL_MPS_16				(0x2 << 0)
+#define D0EPCTL_MPS_8				(0x3 << 0)
+
+#define DxEPCTL_EPEna				(1 << 31)
+#define DxEPCTL_EPDis				(1 << 30)
+#define DxEPCTL_SetD1PID			(1 << 29)
+#define DxEPCTL_SetOddFr			(1 << 29)
+#define DxEPCTL_SetD0PID			(1 << 28)
+#define DxEPCTL_SetEvenFr			(1 << 28)
+#define DxEPCTL_SNAK				(1 << 27)
+#define DxEPCTL_CNAK				(1 << 26)
+#define DxEPCTL_TxFNum_MASK			(0xf << 22)
+#define DxEPCTL_TxFNum_SHIFT			(22)
+#define DxEPCTL_TxFNum_LIMIT			(0xf)
+#define DxEPCTL_TxFNum(_x)			((_x) << 22)
+
+#define DxEPCTL_Stall				(1 << 21)
+#define DxEPCTL_Snp				(1 << 20)
+#define DxEPCTL_EPType_MASK			(0x3 << 18)
+#define DxEPCTL_EPType_SHIFT			(18)
+#define DxEPCTL_EPType_Control			(0x0 << 18)
+#define DxEPCTL_EPType_Iso			(0x1 << 18)
+#define DxEPCTL_EPType_Bulk			(0x2 << 18)
+#define DxEPCTL_EPType_Intterupt		(0x3 << 18)
+
+#define DxEPCTL_NAKsts				(1 << 17)
+#define DxEPCTL_DPID				(1 << 16)
+#define DxEPCTL_EOFrNum			(1 << 16)
+#define DxEPCTL_USBActEp			(1 << 15)
+#define DxEPCTL_NextEp_MASK			(0xf << 11)
+#define DxEPCTL_NextEp_SHIFT			(11)
+#define DxEPCTL_NextEp_LIMIT			(0xf)
+#define DxEPCTL_NextEp(_x)			((_x) << 11)
+
+#define DxEPCTL_MPS_MASK			(0x7ff << 0)
+#define DxEPCTL_MPS_SHIFT			(0)
+#define DxEPCTL_MPS_LIMIT			(0x7ff)
+#define DxEPCTL_MPS(_x)			((_x) << 0)
+
+#define DIEPINT(_a)			HSOTG_REG(0x908 + ((_a) * 0x20))
+#define DOEPINT(_a)			HSOTG_REG(0xB08 + ((_a) * 0x20))
+
+#define DxEPINT_INEPNakEff			(1 << 6)
+#define DxEPINT_Back2BackSetup			(1 << 6)
+#define DxEPINT_INTknEPMis			(1 << 5)
+#define DxEPINT_INTknTXFEmp			(1 << 4)
+#define DxEPINT_OUTTknEPdis			(1 << 4)
+#define DxEPINT_Timeout			(1 << 3)
+#define DxEPINT_Setup				(1 << 3)
+#define DxEPINT_AHBErr				(1 << 2)
+#define DxEPINT_EPDisbld			(1 << 1)
+#define DxEPINT_XferCompl			(1 << 0)
+
+#define DIEPTSIZ0				HSOTG_REG(0x910)
+
+#define DIEPTSIZ0_PktCnt_MASK			(0x3 << 19)
+#define DIEPTSIZ0_PktCnt_SHIFT			(19)
+#define DIEPTSIZ0_PktCnt_LIMIT			(0x3)
+#define DIEPTSIZ0_PktCnt(_x)			((_x) << 19)
+
+#define DIEPTSIZ0_XferSize_MASK		(0x7f << 0)
+#define DIEPTSIZ0_XferSize_SHIFT		(0)
+#define DIEPTSIZ0_XferSize_LIMIT		(0x7f)
+#define DIEPTSIZ0_XferSize(_x)			((_x) << 0)
+
+#define DOEPTSIZ0				HSOTG_REG(0xB10)
+#define DOEPTSIZ0_SUPCnt_MASK			(0x3 << 29)
+#define DOEPTSIZ0_SUPCnt_SHIFT			(29)
+#define DOEPTSIZ0_SUPCnt_LIMIT			(0x3)
+#define DOEPTSIZ0_SUPCnt(_x)			((_x) << 29)
+
+#define DOEPTSIZ0_PktCnt			(1 << 19)
+#define DOEPTSIZ0_XferSize_MASK		(0x7f << 0)
+#define DOEPTSIZ0_XferSize_SHIFT		(0)
+
+#define DIEPTSIZ(_a)			HSOTG_REG(0x910 + ((_a) * 0x20))
+#define DOEPTSIZ(_a)			HSOTG_REG(0xB10 + ((_a) * 0x20))
+
+#define DxEPTSIZ_MC_MASK			(0x3 << 29)
+#define DxEPTSIZ_MC_SHIFT			(29)
+#define DxEPTSIZ_MC_LIMIT			(0x3)
+#define DxEPTSIZ_MC(_x)			((_x) << 29)
+
+#define DxEPTSIZ_PktCnt_MASK			(0x3ff << 19)
+#define DxEPTSIZ_PktCnt_SHIFT			(19)
+#define DxEPTSIZ_PktCnt_GET(_v)		(((_v) >> 19) & 0x3ff)
+#define DxEPTSIZ_PktCnt_LIMIT			(0x3ff)
+#define DxEPTSIZ_PktCnt(_x)			((_x) << 19)
+
+#define DxEPTSIZ_XferSize_MASK			(0x7ffff << 0)
+#define DxEPTSIZ_XferSize_SHIFT		(0)
+#define DxEPTSIZ_XferSize_GET(_v)		(((_v) >> 0) & 0x7ffff)
+#define DxEPTSIZ_XferSize_LIMIT		(0x7ffff)
+#define DxEPTSIZ_XferSize(_x)			((_x) << 0)
+
+#define DIEPDMA(_a)			HSOTG_REG(0x914 + ((_a) * 0x20))
+#define DOEPDMA(_a)			HSOTG_REG(0xB14 + ((_a) * 0x20))
+#define DTXFSTS(_a)			HSOTG_REG(0x918 + ((_a) * 0x20))
+
+#define EPFIFO(_a)			HSOTG_REG(0x1000 + ((_a) * 0x1000))
+
+#endif /* __REGS_USB_HSOTG_H */
diff --git a/drivers/usb/gadget/s3c-hsudc.c b/drivers/usb/gadget/s3c-hsudc.c
index cef9b82ff91..36c6836eeb0 100644
--- a/drivers/usb/gadget/s3c-hsudc.c
+++ b/drivers/usb/gadget/s3c-hsudc.c
@@ -110,7 +110,6 @@ struct s3c_hsudc_ep {
 	struct usb_ep ep;
 	char name[20];
 	struct s3c_hsudc *dev;
-	const struct usb_endpoint_descriptor *desc;
 	struct list_head queue;
 	u8 stopped;
 	u8 wedge;
@@ -761,7 +760,7 @@ static int s3c_hsudc_ep_enable(struct usb_ep *_ep,
 	u32 ecr = 0;
 
 	hsep = our_ep(_ep);
-	if (!_ep || !desc || hsep->desc || _ep->name == ep0name
+	if (!_ep || !desc || hsep->ep.desc || _ep->name == ep0name
 		|| desc->bDescriptorType != USB_DT_ENDPOINT
 		|| hsep->bEndpointAddress != desc->bEndpointAddress
 		|| ep_maxpacket(hsep) < usb_endpoint_maxp(desc))
@@ -783,7 +782,7 @@ static int s3c_hsudc_ep_enable(struct usb_ep *_ep,
 	writel(ecr, hsudc->regs + S3C_ECR);
 
 	hsep->stopped = hsep->wedge = 0;
-	hsep->desc = desc;
+	hsep->ep.desc = desc;
 	hsep->ep.maxpacket = usb_endpoint_maxp(desc);
 
 	s3c_hsudc_set_halt(_ep, 0);
@@ -806,7 +805,7 @@ static int s3c_hsudc_ep_disable(struct usb_ep *_ep)
 	struct s3c_hsudc *hsudc = hsep->dev;
 	unsigned long flags;
 
-	if (!_ep || !hsep->desc)
+	if (!_ep || !hsep->ep.desc)
 		return -EINVAL;
 
 	spin_lock_irqsave(&hsudc->lock, flags);
@@ -816,7 +815,6 @@ static int s3c_hsudc_ep_disable(struct usb_ep *_ep)
 
 	s3c_hsudc_nuke_ep(hsep, -ESHUTDOWN);
 
-	hsep->desc = 0;
 	hsep->ep.desc = NULL;
 	hsep->stopped = 1;
 
@@ -1006,7 +1004,6 @@ static void s3c_hsudc_initep(struct s3c_hsudc *hsudc,
 	hsep->ep.maxpacket = epnum ? 512 : 64;
 	hsep->ep.ops = &s3c_hsudc_ep_ops;
 	hsep->fifo = hsudc->regs + S3C_BR(epnum);
-	hsep->desc = 0;
 	hsep->ep.desc = NULL;
 	hsep->stopped = 0;
 	hsep->wedge = 0;
diff --git a/drivers/usb/gadget/s3c2410_udc.c b/drivers/usb/gadget/s3c2410_udc.c
index 195524cde6c..3de71d37d75 100644
--- a/drivers/usb/gadget/s3c2410_udc.c
+++ b/drivers/usb/gadget/s3c2410_udc.c
@@ -1062,7 +1062,7 @@ static int s3c2410_udc_ep_enable(struct usb_ep *_ep,
 
 	ep = to_s3c2410_ep(_ep);
 
-	if (!_ep || !desc || ep->desc
+	if (!_ep || !desc || ep->ep.desc
 			|| _ep->name == ep0name
 			|| desc->bDescriptorType != USB_DT_ENDPOINT)
 		return -EINVAL;
@@ -1075,7 +1075,7 @@ static int s3c2410_udc_ep_enable(struct usb_ep *_ep,
 
 	local_irq_save (flags);
 	_ep->maxpacket = max & 0x7ff;
-	ep->desc = desc;
+	ep->ep.desc = desc;
 	ep->halted = 0;
 	ep->bEndpointAddress = desc->bEndpointAddress;
 
@@ -1136,7 +1136,7 @@ static int s3c2410_udc_ep_disable(struct usb_ep *_ep)
 	unsigned long flags;
 	u32 int_en_reg;
 
-	if (!_ep || !ep->desc) {
+	if (!_ep || !ep->ep.desc) {
 		dprintk(DEBUG_NORMAL, "%s not enabled\n",
 			_ep ? ep->ep.name : NULL);
 		return -EINVAL;
@@ -1146,7 +1146,6 @@ static int s3c2410_udc_ep_disable(struct usb_ep *_ep)
 
 	dprintk(DEBUG_NORMAL, "ep_disable: %s\n", _ep->name);
 
-	ep->desc = NULL;
 	ep->ep.desc = NULL;
 	ep->halted = 1;
 
@@ -1195,7 +1194,7 @@ s3c2410_udc_free_request(struct usb_ep *_ep, struct usb_request *_req)
 
 	dprintk(DEBUG_VERBOSE, "%s(%p,%p)\n", __func__, _ep, _req);
 
-	if (!ep || !_req || (!ep->desc && _ep->name != ep0name))
+	if (!ep || !_req || (!ep->ep.desc && _ep->name != ep0name))
 		return;
 
 	WARN_ON (!list_empty (&req->queue));
@@ -1215,7 +1214,7 @@ static int s3c2410_udc_queue(struct usb_ep *_ep, struct usb_request *_req,
 	int			fifo_count = 0;
 	unsigned long		flags;
 
-	if (unlikely (!_ep || (!ep->desc && ep->ep.name != ep0name))) {
+	if (unlikely(!_ep || (!ep->ep.desc && ep->ep.name != ep0name))) {
 		dprintk(DEBUG_NORMAL, "%s: invalid args\n", __func__);
 		return -EINVAL;
 	}
@@ -1363,7 +1362,7 @@ static int s3c2410_udc_set_halt(struct usb_ep *_ep, int value)
 	unsigned long		flags;
 	u32			idx;
 
-	if (unlikely (!_ep || (!ep->desc && ep->ep.name != ep0name))) {
+	if (unlikely(!_ep || (!ep->ep.desc && ep->ep.name != ep0name))) {
 		dprintk(DEBUG_NORMAL, "%s: inval 2\n", __func__);
 		return -EINVAL;
 	}
@@ -1629,7 +1628,6 @@ static void s3c2410_udc_reinit(struct s3c2410_udc *dev)
 			list_add_tail (&ep->ep.ep_list, &dev->gadget.ep_list);
 
 		ep->dev = dev;
-		ep->desc = NULL;
 		ep->ep.desc = NULL;
 		ep->halted = 0;
 		INIT_LIST_HEAD (&ep->queue);
diff --git a/drivers/usb/gadget/s3c2410_udc.h b/drivers/usb/gadget/s3c2410_udc.h
index 1653bae08b8..3e80fd5c820 100644
--- a/drivers/usb/gadget/s3c2410_udc.h
+++ b/drivers/usb/gadget/s3c2410_udc.h
@@ -19,7 +19,6 @@ struct s3c2410_ep {
 	unsigned long			last_io;	/* jiffies timestamp */
 	struct usb_gadget		*gadget;
 	struct s3c2410_udc		*dev;
-	const struct usb_endpoint_descriptor *desc;
 	struct usb_ep			ep;
 	u8				num;
 
diff --git a/drivers/usb/gadget/tcm_usb_gadget.c b/drivers/usb/gadget/tcm_usb_gadget.c
new file mode 100644
index 00000000000..c46439c8dd7
--- /dev/null
+++ b/drivers/usb/gadget/tcm_usb_gadget.c
@@ -0,0 +1,2480 @@
+/* Target based USB-Gadget
+ *
+ * UAS protocol handling, target callbacks, configfs handling,
+ * BBB (USB Mass Storage Class Bulk-Only (BBB) and Transport protocol handling.
+ *
+ * Author: Sebastian Andrzej Siewior <bigeasy at linutronix dot de>
+ * License: GPLv2 as published by FSF.
+ */
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/types.h>
+#include <linux/string.h>
+#include <linux/configfs.h>
+#include <linux/ctype.h>
+#include <linux/usb/ch9.h>
+#include <linux/usb/composite.h>
+#include <linux/usb/gadget.h>
+#include <linux/usb/storage.h>
+#include <scsi/scsi.h>
+#include <scsi/scsi_tcq.h>
+#include <target/target_core_base.h>
+#include <target/target_core_fabric.h>
+#include <target/target_core_fabric_configfs.h>
+#include <target/target_core_configfs.h>
+#include <target/configfs_macros.h>
+#include <asm/unaligned.h>
+
+#include "usbstring.c"
+#include "epautoconf.c"
+#include "config.c"
+#include "composite.c"
+
+#include "tcm_usb_gadget.h"
+
+static struct target_fabric_configfs *usbg_fabric_configfs;
+
+static inline struct f_uas *to_f_uas(struct usb_function *f)
+{
+	return container_of(f, struct f_uas, function);
+}
+
+static void usbg_cmd_release(struct kref *);
+
+static inline void usbg_cleanup_cmd(struct usbg_cmd *cmd)
+{
+	kref_put(&cmd->ref, usbg_cmd_release);
+}
+
+/* Start bot.c code */
+
+static int bot_enqueue_cmd_cbw(struct f_uas *fu)
+{
+	int ret;
+
+	if (fu->flags & USBG_BOT_CMD_PEND)
+		return 0;
+
+	ret = usb_ep_queue(fu->ep_out, fu->cmd.req, GFP_ATOMIC);
+	if (!ret)
+		fu->flags |= USBG_BOT_CMD_PEND;
+	return ret;
+}
+
+static void bot_status_complete(struct usb_ep *ep, struct usb_request *req)
+{
+	struct usbg_cmd *cmd = req->context;
+	struct f_uas *fu = cmd->fu;
+
+	usbg_cleanup_cmd(cmd);
+	if (req->status < 0) {
+		pr_err("ERR %s(%d)\n", __func__, __LINE__);
+		return;
+	}
+
+	/* CSW completed, wait for next CBW */
+	bot_enqueue_cmd_cbw(fu);
+}
+
+static void bot_enqueue_sense_code(struct f_uas *fu, struct usbg_cmd *cmd)
+{
+	struct bulk_cs_wrap *csw = &fu->bot_status.csw;
+	int ret;
+	u8 *sense;
+	unsigned int csw_stat;
+
+	csw_stat = cmd->csw_code;
+
+	/*
+	 * We can't send SENSE as a response. So we take ASC & ASCQ from our
+	 * sense buffer and queue it and hope the host sends a REQUEST_SENSE
+	 * command where it learns why we failed.
+	 */
+	sense = cmd->sense_iu.sense;
+
+	csw->Tag = cmd->bot_tag;
+	csw->Status = csw_stat;
+	fu->bot_status.req->context = cmd;
+	ret = usb_ep_queue(fu->ep_in, fu->bot_status.req, GFP_ATOMIC);
+	if (ret)
+		pr_err("%s(%d) ERR: %d\n", __func__, __LINE__, ret);
+}
+
+static void bot_err_compl(struct usb_ep *ep, struct usb_request *req)
+{
+	struct usbg_cmd *cmd = req->context;
+	struct f_uas *fu = cmd->fu;
+
+	if (req->status < 0)
+		pr_err("ERR %s(%d)\n", __func__, __LINE__);
+
+	if (cmd->data_len) {
+		if (cmd->data_len > ep->maxpacket) {
+			req->length = ep->maxpacket;
+			cmd->data_len -= ep->maxpacket;
+		} else {
+			req->length = cmd->data_len;
+			cmd->data_len = 0;
+		}
+
+		usb_ep_queue(ep, req, GFP_ATOMIC);
+		return ;
+	}
+	bot_enqueue_sense_code(fu, cmd);
+}
+
+static void bot_send_bad_status(struct usbg_cmd *cmd)
+{
+	struct f_uas *fu = cmd->fu;
+	struct bulk_cs_wrap *csw = &fu->bot_status.csw;
+	struct usb_request *req;
+	struct usb_ep *ep;
+
+	csw->Residue = cpu_to_le32(cmd->data_len);
+
+	if (cmd->data_len) {
+		if (cmd->is_read) {
+			ep = fu->ep_in;
+			req = fu->bot_req_in;
+		} else {
+			ep = fu->ep_out;
+			req = fu->bot_req_out;
+		}
+
+		if (cmd->data_len > fu->ep_in->maxpacket) {
+			req->length = ep->maxpacket;
+			cmd->data_len -= ep->maxpacket;
+		} else {
+			req->length = cmd->data_len;
+			cmd->data_len = 0;
+		}
+		req->complete = bot_err_compl;
+		req->context = cmd;
+		req->buf = fu->cmd.buf;
+		usb_ep_queue(ep, req, GFP_KERNEL);
+	} else {
+		bot_enqueue_sense_code(fu, cmd);
+	}
+}
+
+static int bot_send_status(struct usbg_cmd *cmd, bool moved_data)
+{
+	struct f_uas *fu = cmd->fu;
+	struct bulk_cs_wrap *csw = &fu->bot_status.csw;
+	int ret;
+
+	if (cmd->se_cmd.scsi_status == SAM_STAT_GOOD) {
+		if (!moved_data && cmd->data_len) {
+			/*
+			 * the host wants to move data, we don't. Fill / empty
+			 * the pipe and then send the csw with reside set.
+			 */
+			cmd->csw_code = US_BULK_STAT_OK;
+			bot_send_bad_status(cmd);
+			return 0;
+		}
+
+		csw->Tag = cmd->bot_tag;
+		csw->Residue = cpu_to_le32(0);
+		csw->Status = US_BULK_STAT_OK;
+		fu->bot_status.req->context = cmd;
+
+		ret = usb_ep_queue(fu->ep_in, fu->bot_status.req, GFP_KERNEL);
+		if (ret)
+			pr_err("%s(%d) ERR: %d\n", __func__, __LINE__, ret);
+	} else {
+		cmd->csw_code = US_BULK_STAT_FAIL;
+		bot_send_bad_status(cmd);
+	}
+	return 0;
+}
+
+/*
+ * Called after command (no data transfer) or after the write (to device)
+ * operation is completed
+ */
+static int bot_send_status_response(struct usbg_cmd *cmd)
+{
+	bool moved_data = false;
+
+	if (!cmd->is_read)
+		moved_data = true;
+	return bot_send_status(cmd, moved_data);
+}
+
+/* Read request completed, now we have to send the CSW */
+static void bot_read_compl(struct usb_ep *ep, struct usb_request *req)
+{
+	struct usbg_cmd *cmd = req->context;
+
+	if (req->status < 0)
+		pr_err("ERR %s(%d)\n", __func__, __LINE__);
+
+	bot_send_status(cmd, true);
+}
+
+static int bot_send_read_response(struct usbg_cmd *cmd)
+{
+	struct f_uas *fu = cmd->fu;
+	struct se_cmd *se_cmd = &cmd->se_cmd;
+	struct usb_gadget *gadget = fuas_to_gadget(fu);
+	int ret;
+
+	if (!cmd->data_len) {
+		cmd->csw_code = US_BULK_STAT_PHASE;
+		bot_send_bad_status(cmd);
+		return 0;
+	}
+
+	if (!gadget->sg_supported) {
+		cmd->data_buf = kmalloc(se_cmd->data_length, GFP_ATOMIC);
+		if (!cmd->data_buf)
+			return -ENOMEM;
+
+		sg_copy_to_buffer(se_cmd->t_data_sg,
+				se_cmd->t_data_nents,
+				cmd->data_buf,
+				se_cmd->data_length);
+
+		fu->bot_req_in->buf = cmd->data_buf;
+	} else {
+		fu->bot_req_in->buf = NULL;
+		fu->bot_req_in->num_sgs = se_cmd->t_data_nents;
+		fu->bot_req_in->sg = se_cmd->t_data_sg;
+	}
+
+	fu->bot_req_in->complete = bot_read_compl;
+	fu->bot_req_in->length = se_cmd->data_length;
+	fu->bot_req_in->context = cmd;
+	ret = usb_ep_queue(fu->ep_in, fu->bot_req_in, GFP_ATOMIC);
+	if (ret)
+		pr_err("%s(%d)\n", __func__, __LINE__);
+	return 0;
+}
+
+static void usbg_data_write_cmpl(struct usb_ep *, struct usb_request *);
+static int usbg_prepare_w_request(struct usbg_cmd *, struct usb_request *);
+
+static int bot_send_write_request(struct usbg_cmd *cmd)
+{
+	struct f_uas *fu = cmd->fu;
+	struct se_cmd *se_cmd = &cmd->se_cmd;
+	struct usb_gadget *gadget = fuas_to_gadget(fu);
+	int ret;
+
+	init_completion(&cmd->write_complete);
+	cmd->fu = fu;
+
+	if (!cmd->data_len) {
+		cmd->csw_code = US_BULK_STAT_PHASE;
+		return -EINVAL;
+	}
+
+	if (!gadget->sg_supported) {
+		cmd->data_buf = kmalloc(se_cmd->data_length, GFP_KERNEL);
+		if (!cmd->data_buf)
+			return -ENOMEM;
+
+		fu->bot_req_out->buf = cmd->data_buf;
+	} else {
+		fu->bot_req_out->buf = NULL;
+		fu->bot_req_out->num_sgs = se_cmd->t_data_nents;
+		fu->bot_req_out->sg = se_cmd->t_data_sg;
+	}
+
+	fu->bot_req_out->complete = usbg_data_write_cmpl;
+	fu->bot_req_out->length = se_cmd->data_length;
+	fu->bot_req_out->context = cmd;
+
+	ret = usbg_prepare_w_request(cmd, fu->bot_req_out);
+	if (ret)
+		goto cleanup;
+	ret = usb_ep_queue(fu->ep_out, fu->bot_req_out, GFP_KERNEL);
+	if (ret)
+		pr_err("%s(%d)\n", __func__, __LINE__);
+
+	wait_for_completion(&cmd->write_complete);
+	transport_generic_process_write(se_cmd);
+cleanup:
+	return ret;
+}
+
+static int bot_submit_command(struct f_uas *, void *, unsigned int);
+
+static void bot_cmd_complete(struct usb_ep *ep, struct usb_request *req)
+{
+	struct f_uas *fu = req->context;
+	int ret;
+
+	fu->flags &= ~USBG_BOT_CMD_PEND;
+
+	if (req->status < 0)
+		return;
+
+	ret = bot_submit_command(fu, req->buf, req->actual);
+	if (ret)
+		pr_err("%s(%d): %d\n", __func__, __LINE__, ret);
+}
+
+static int bot_prepare_reqs(struct f_uas *fu)
+{
+	int ret;
+
+	fu->bot_req_in = usb_ep_alloc_request(fu->ep_in, GFP_KERNEL);
+	if (!fu->bot_req_in)
+		goto err;
+
+	fu->bot_req_out = usb_ep_alloc_request(fu->ep_out, GFP_KERNEL);
+	if (!fu->bot_req_out)
+		goto err_out;
+
+	fu->cmd.req = usb_ep_alloc_request(fu->ep_out, GFP_KERNEL);
+	if (!fu->cmd.req)
+		goto err_cmd;
+
+	fu->bot_status.req = usb_ep_alloc_request(fu->ep_in, GFP_KERNEL);
+	if (!fu->bot_status.req)
+		goto err_sts;
+
+	fu->bot_status.req->buf = &fu->bot_status.csw;
+	fu->bot_status.req->length = US_BULK_CS_WRAP_LEN;
+	fu->bot_status.req->complete = bot_status_complete;
+	fu->bot_status.csw.Signature = cpu_to_le32(US_BULK_CS_SIGN);
+
+	fu->cmd.buf = kmalloc(fu->ep_out->maxpacket, GFP_KERNEL);
+	if (!fu->cmd.buf)
+		goto err_buf;
+
+	fu->cmd.req->complete = bot_cmd_complete;
+	fu->cmd.req->buf = fu->cmd.buf;
+	fu->cmd.req->length = fu->ep_out->maxpacket;
+	fu->cmd.req->context = fu;
+
+	ret = bot_enqueue_cmd_cbw(fu);
+	if (ret)
+		goto err_queue;
+	return 0;
+err_queue:
+	kfree(fu->cmd.buf);
+	fu->cmd.buf = NULL;
+err_buf:
+	usb_ep_free_request(fu->ep_in, fu->bot_status.req);
+err_sts:
+	usb_ep_free_request(fu->ep_out, fu->cmd.req);
+	fu->cmd.req = NULL;
+err_cmd:
+	usb_ep_free_request(fu->ep_out, fu->bot_req_out);
+	fu->bot_req_out = NULL;
+err_out:
+	usb_ep_free_request(fu->ep_in, fu->bot_req_in);
+	fu->bot_req_in = NULL;
+err:
+	pr_err("BOT: endpoint setup failed\n");
+	return -ENOMEM;
+}
+
+void bot_cleanup_old_alt(struct f_uas *fu)
+{
+	if (!(fu->flags & USBG_ENABLED))
+		return;
+
+	usb_ep_disable(fu->ep_in);
+	usb_ep_disable(fu->ep_out);
+
+	if (!fu->bot_req_in)
+		return;
+
+	usb_ep_free_request(fu->ep_in, fu->bot_req_in);
+	usb_ep_free_request(fu->ep_out, fu->bot_req_out);
+	usb_ep_free_request(fu->ep_out, fu->cmd.req);
+	usb_ep_free_request(fu->ep_out, fu->bot_status.req);
+
+	kfree(fu->cmd.buf);
+
+	fu->bot_req_in = NULL;
+	fu->bot_req_out = NULL;
+	fu->cmd.req = NULL;
+	fu->bot_status.req = NULL;
+	fu->cmd.buf = NULL;
+}
+
+static void bot_set_alt(struct f_uas *fu)
+{
+	struct usb_function *f = &fu->function;
+	struct usb_gadget *gadget = f->config->cdev->gadget;
+	int ret;
+
+	fu->flags = USBG_IS_BOT;
+
+	config_ep_by_speed(gadget, f, fu->ep_in);
+	ret = usb_ep_enable(fu->ep_in);
+	if (ret)
+		goto err_b_in;
+
+	config_ep_by_speed(gadget, f, fu->ep_out);
+	ret = usb_ep_enable(fu->ep_out);
+	if (ret)
+		goto err_b_out;
+
+	ret = bot_prepare_reqs(fu);
+	if (ret)
+		goto err_wq;
+	fu->flags |= USBG_ENABLED;
+	pr_info("Using the BOT protocol\n");
+	return;
+err_wq:
+	usb_ep_disable(fu->ep_out);
+err_b_out:
+	usb_ep_disable(fu->ep_in);
+err_b_in:
+	fu->flags = USBG_IS_BOT;
+}
+
+static int usbg_bot_setup(struct usb_function *f,
+		const struct usb_ctrlrequest *ctrl)
+{
+	struct f_uas *fu = to_f_uas(f);
+	struct usb_composite_dev *cdev = f->config->cdev;
+	u16 w_value = le16_to_cpu(ctrl->wValue);
+	u16 w_length = le16_to_cpu(ctrl->wLength);
+	int luns;
+	u8 *ret_lun;
+
+	switch (ctrl->bRequest) {
+	case US_BULK_GET_MAX_LUN:
+		if (ctrl->bRequestType != (USB_DIR_IN | USB_TYPE_CLASS |
+					USB_RECIP_INTERFACE))
+			return -ENOTSUPP;
+
+		if (w_length < 1)
+			return -EINVAL;
+		if (w_value != 0)
+			return -EINVAL;
+		luns = atomic_read(&fu->tpg->tpg_port_count);
+		if (!luns) {
+			pr_err("No LUNs configured?\n");
+			return -EINVAL;
+		}
+		/*
+		 * If 4 LUNs are present we return 3 i.e. LUN 0..3 can be
+		 * accessed. The upper limit is 0xf
+		 */
+		luns--;
+		if (luns > 0xf) {
+			pr_info_once("Limiting the number of luns to 16\n");
+			luns = 0xf;
+		}
+		ret_lun = cdev->req->buf;
+		*ret_lun = luns;
+		cdev->req->length = 1;
+		return usb_ep_queue(cdev->gadget->ep0, cdev->req, GFP_ATOMIC);
+		break;
+
+	case US_BULK_RESET_REQUEST:
+		/* XXX maybe we should remove previous requests for IN + OUT */
+		bot_enqueue_cmd_cbw(fu);
+		return 0;
+		break;
+	};
+	return -ENOTSUPP;
+}
+
+/* Start uas.c code */
+
+static void uasp_cleanup_one_stream(struct f_uas *fu, struct uas_stream *stream)
+{
+	/* We have either all three allocated or none */
+	if (!stream->req_in)
+		return;
+
+	usb_ep_free_request(fu->ep_in, stream->req_in);
+	usb_ep_free_request(fu->ep_out, stream->req_out);
+	usb_ep_free_request(fu->ep_status, stream->req_status);
+
+	stream->req_in = NULL;
+	stream->req_out = NULL;
+	stream->req_status = NULL;
+}
+
+static void uasp_free_cmdreq(struct f_uas *fu)
+{
+	usb_ep_free_request(fu->ep_cmd, fu->cmd.req);
+	kfree(fu->cmd.buf);
+	fu->cmd.req = NULL;
+	fu->cmd.buf = NULL;
+}
+
+static void uasp_cleanup_old_alt(struct f_uas *fu)
+{
+	int i;
+
+	if (!(fu->flags & USBG_ENABLED))
+		return;
+
+	usb_ep_disable(fu->ep_in);
+	usb_ep_disable(fu->ep_out);
+	usb_ep_disable(fu->ep_status);
+	usb_ep_disable(fu->ep_cmd);
+
+	for (i = 0; i < UASP_SS_EP_COMP_NUM_STREAMS; i++)
+		uasp_cleanup_one_stream(fu, &fu->stream[i]);
+	uasp_free_cmdreq(fu);
+}
+
+static void uasp_status_data_cmpl(struct usb_ep *ep, struct usb_request *req);
+
+static int uasp_prepare_r_request(struct usbg_cmd *cmd)
+{
+	struct se_cmd *se_cmd = &cmd->se_cmd;
+	struct f_uas *fu = cmd->fu;
+	struct usb_gadget *gadget = fuas_to_gadget(fu);
+	struct uas_stream *stream = cmd->stream;
+
+	if (!gadget->sg_supported) {
+		cmd->data_buf = kmalloc(se_cmd->data_length, GFP_ATOMIC);
+		if (!cmd->data_buf)
+			return -ENOMEM;
+
+		sg_copy_to_buffer(se_cmd->t_data_sg,
+				se_cmd->t_data_nents,
+				cmd->data_buf,
+				se_cmd->data_length);
+
+		stream->req_in->buf = cmd->data_buf;
+	} else {
+		stream->req_in->buf = NULL;
+		stream->req_in->num_sgs = se_cmd->t_data_nents;
+		stream->req_in->sg = se_cmd->t_data_sg;
+	}
+
+	stream->req_in->complete = uasp_status_data_cmpl;
+	stream->req_in->length = se_cmd->data_length;
+	stream->req_in->context = cmd;
+
+	cmd->state = UASP_SEND_STATUS;
+	return 0;
+}
+
+static void uasp_prepare_status(struct usbg_cmd *cmd)
+{
+	struct se_cmd *se_cmd = &cmd->se_cmd;
+	struct sense_iu *iu = &cmd->sense_iu;
+	struct uas_stream *stream = cmd->stream;
+
+	cmd->state = UASP_QUEUE_COMMAND;
+	iu->iu_id = IU_ID_STATUS;
+	iu->tag = cpu_to_be16(cmd->tag);
+
+	/*
+	 * iu->status_qual = cpu_to_be16(STATUS QUALIFIER SAM-4. Where R U?);
+	 */
+	iu->len = cpu_to_be16(se_cmd->scsi_sense_length);
+	iu->status = se_cmd->scsi_status;
+	stream->req_status->context = cmd;
+	stream->req_status->length = se_cmd->scsi_sense_length + 16;
+	stream->req_status->buf = iu;
+	stream->req_status->complete = uasp_status_data_cmpl;
+}
+
+static void uasp_status_data_cmpl(struct usb_ep *ep, struct usb_request *req)
+{
+	struct usbg_cmd *cmd = req->context;
+	struct uas_stream *stream = cmd->stream;
+	struct f_uas *fu = cmd->fu;
+	int ret;
+
+	if (req->status < 0)
+		goto cleanup;
+
+	switch (cmd->state) {
+	case UASP_SEND_DATA:
+		ret = uasp_prepare_r_request(cmd);
+		if (ret)
+			goto cleanup;
+		ret = usb_ep_queue(fu->ep_in, stream->req_in, GFP_ATOMIC);
+		if (ret)
+			pr_err("%s(%d) => %d\n", __func__, __LINE__, ret);
+		break;
+
+	case UASP_RECEIVE_DATA:
+		ret = usbg_prepare_w_request(cmd, stream->req_out);
+		if (ret)
+			goto cleanup;
+		ret = usb_ep_queue(fu->ep_out, stream->req_out, GFP_ATOMIC);
+		if (ret)
+			pr_err("%s(%d) => %d\n", __func__, __LINE__, ret);
+		break;
+
+	case UASP_SEND_STATUS:
+		uasp_prepare_status(cmd);
+		ret = usb_ep_queue(fu->ep_status, stream->req_status,
+				GFP_ATOMIC);
+		if (ret)
+			pr_err("%s(%d) => %d\n", __func__, __LINE__, ret);
+		break;
+
+	case UASP_QUEUE_COMMAND:
+		usbg_cleanup_cmd(cmd);
+		usb_ep_queue(fu->ep_cmd, fu->cmd.req, GFP_ATOMIC);
+		break;
+
+	default:
+		BUG();
+	};
+	return;
+
+cleanup:
+	usbg_cleanup_cmd(cmd);
+}
+
+static int uasp_send_status_response(struct usbg_cmd *cmd)
+{
+	struct f_uas *fu = cmd->fu;
+	struct uas_stream *stream = cmd->stream;
+	struct sense_iu *iu = &cmd->sense_iu;
+
+	iu->tag = cpu_to_be16(cmd->tag);
+	stream->req_status->complete = uasp_status_data_cmpl;
+	stream->req_status->context = cmd;
+	cmd->fu = fu;
+	uasp_prepare_status(cmd);
+	return usb_ep_queue(fu->ep_status, stream->req_status, GFP_ATOMIC);
+}
+
+static int uasp_send_read_response(struct usbg_cmd *cmd)
+{
+	struct f_uas *fu = cmd->fu;
+	struct uas_stream *stream = cmd->stream;
+	struct sense_iu *iu = &cmd->sense_iu;
+	int ret;
+
+	cmd->fu = fu;
+
+	iu->tag = cpu_to_be16(cmd->tag);
+	if (fu->flags & USBG_USE_STREAMS) {
+
+		ret = uasp_prepare_r_request(cmd);
+		if (ret)
+			goto out;
+		ret = usb_ep_queue(fu->ep_in, stream->req_in, GFP_ATOMIC);
+		if (ret) {
+			pr_err("%s(%d) => %d\n", __func__, __LINE__, ret);
+			kfree(cmd->data_buf);
+			cmd->data_buf = NULL;
+		}
+
+	} else {
+
+		iu->iu_id = IU_ID_READ_READY;
+		iu->tag = cpu_to_be16(cmd->tag);
+
+		stream->req_status->complete = uasp_status_data_cmpl;
+		stream->req_status->context = cmd;
+
+		cmd->state = UASP_SEND_DATA;
+		stream->req_status->buf = iu;
+		stream->req_status->length = sizeof(struct iu);
+
+		ret = usb_ep_queue(fu->ep_status, stream->req_status,
+				GFP_ATOMIC);
+		if (ret)
+			pr_err("%s(%d) => %d\n", __func__, __LINE__, ret);
+	}
+out:
+	return ret;
+}
+
+static int uasp_send_write_request(struct usbg_cmd *cmd)
+{
+	struct f_uas *fu = cmd->fu;
+	struct se_cmd *se_cmd = &cmd->se_cmd;
+	struct uas_stream *stream = cmd->stream;
+	struct sense_iu *iu = &cmd->sense_iu;
+	int ret;
+
+	init_completion(&cmd->write_complete);
+	cmd->fu = fu;
+
+	iu->tag = cpu_to_be16(cmd->tag);
+
+	if (fu->flags & USBG_USE_STREAMS) {
+
+		ret = usbg_prepare_w_request(cmd, stream->req_out);
+		if (ret)
+			goto cleanup;
+		ret = usb_ep_queue(fu->ep_out, stream->req_out, GFP_ATOMIC);
+		if (ret)
+			pr_err("%s(%d)\n", __func__, __LINE__);
+
+	} else {
+
+		iu->iu_id = IU_ID_WRITE_READY;
+		iu->tag = cpu_to_be16(cmd->tag);
+
+		stream->req_status->complete = uasp_status_data_cmpl;
+		stream->req_status->context = cmd;
+
+		cmd->state = UASP_RECEIVE_DATA;
+		stream->req_status->buf = iu;
+		stream->req_status->length = sizeof(struct iu);
+
+		ret = usb_ep_queue(fu->ep_status, stream->req_status,
+				GFP_ATOMIC);
+		if (ret)
+			pr_err("%s(%d)\n", __func__, __LINE__);
+	}
+
+	wait_for_completion(&cmd->write_complete);
+	transport_generic_process_write(se_cmd);
+cleanup:
+	return ret;
+}
+
+static int usbg_submit_command(struct f_uas *, void *, unsigned int);
+
+static void uasp_cmd_complete(struct usb_ep *ep, struct usb_request *req)
+{
+	struct f_uas *fu = req->context;
+	int ret;
+
+	if (req->status < 0)
+		return;
+
+	ret = usbg_submit_command(fu, req->buf, req->actual);
+	/*
+	 * Once we tune for performance enqueue the command req here again so
+	 * we can receive a second command while we processing this one. Pay
+	 * attention to properly sync STAUS endpoint with DATA IN + OUT so you
+	 * don't break HS.
+	 */
+	if (!ret)
+		return;
+	usb_ep_queue(fu->ep_cmd, fu->cmd.req, GFP_ATOMIC);
+}
+
+static int uasp_alloc_stream_res(struct f_uas *fu, struct uas_stream *stream)
+{
+	stream->req_in = usb_ep_alloc_request(fu->ep_in, GFP_KERNEL);
+	if (!stream->req_in)
+		goto out;
+
+	stream->req_out = usb_ep_alloc_request(fu->ep_out, GFP_KERNEL);
+	if (!stream->req_out)
+		goto err_out;
+
+	stream->req_status = usb_ep_alloc_request(fu->ep_status, GFP_KERNEL);
+	if (!stream->req_status)
+		goto err_sts;
+
+	return 0;
+err_sts:
+	usb_ep_free_request(fu->ep_status, stream->req_status);
+	stream->req_status = NULL;
+err_out:
+	usb_ep_free_request(fu->ep_out, stream->req_out);
+	stream->req_out = NULL;
+out:
+	return -ENOMEM;
+}
+
+static int uasp_alloc_cmd(struct f_uas *fu)
+{
+	fu->cmd.req = usb_ep_alloc_request(fu->ep_cmd, GFP_KERNEL);
+	if (!fu->cmd.req)
+		goto err;
+
+	fu->cmd.buf = kmalloc(fu->ep_cmd->maxpacket, GFP_KERNEL);
+	if (!fu->cmd.buf)
+		goto err_buf;
+
+	fu->cmd.req->complete = uasp_cmd_complete;
+	fu->cmd.req->buf = fu->cmd.buf;
+	fu->cmd.req->length = fu->ep_cmd->maxpacket;
+	fu->cmd.req->context = fu;
+	return 0;
+
+err_buf:
+	usb_ep_free_request(fu->ep_cmd, fu->cmd.req);
+err:
+	return -ENOMEM;
+}
+
+static void uasp_setup_stream_res(struct f_uas *fu, int max_streams)
+{
+	int i;
+
+	for (i = 0; i < max_streams; i++) {
+		struct uas_stream *s = &fu->stream[i];
+
+		s->req_in->stream_id = i + 1;
+		s->req_out->stream_id = i + 1;
+		s->req_status->stream_id = i + 1;
+	}
+}
+
+static int uasp_prepare_reqs(struct f_uas *fu)
+{
+	int ret;
+	int i;
+	int max_streams;
+
+	if (fu->flags & USBG_USE_STREAMS)
+		max_streams = UASP_SS_EP_COMP_NUM_STREAMS;
+	else
+		max_streams = 1;
+
+	for (i = 0; i < max_streams; i++) {
+		ret = uasp_alloc_stream_res(fu, &fu->stream[i]);
+		if (ret)
+			goto err_cleanup;
+	}
+
+	ret = uasp_alloc_cmd(fu);
+	if (ret)
+		goto err_free_stream;
+	uasp_setup_stream_res(fu, max_streams);
+
+	ret = usb_ep_queue(fu->ep_cmd, fu->cmd.req, GFP_ATOMIC);
+	if (ret)
+		goto err_free_stream;
+
+	return 0;
+
+err_free_stream:
+	uasp_free_cmdreq(fu);
+
+err_cleanup:
+	if (i) {
+		do {
+			uasp_cleanup_one_stream(fu, &fu->stream[i - 1]);
+			i--;
+		} while (i);
+	}
+	pr_err("UASP: endpoint setup failed\n");
+	return ret;
+}
+
+static void uasp_set_alt(struct f_uas *fu)
+{
+	struct usb_function *f = &fu->function;
+	struct usb_gadget *gadget = f->config->cdev->gadget;
+	int ret;
+
+	fu->flags = USBG_IS_UAS;
+
+	if (gadget->speed == USB_SPEED_SUPER)
+		fu->flags |= USBG_USE_STREAMS;
+
+	config_ep_by_speed(gadget, f, fu->ep_in);
+	ret = usb_ep_enable(fu->ep_in);
+	if (ret)
+		goto err_b_in;
+
+	config_ep_by_speed(gadget, f, fu->ep_out);
+	ret = usb_ep_enable(fu->ep_out);
+	if (ret)
+		goto err_b_out;
+
+	config_ep_by_speed(gadget, f, fu->ep_cmd);
+	ret = usb_ep_enable(fu->ep_cmd);
+	if (ret)
+		goto err_cmd;
+	config_ep_by_speed(gadget, f, fu->ep_status);
+	ret = usb_ep_enable(fu->ep_status);
+	if (ret)
+		goto err_status;
+
+	ret = uasp_prepare_reqs(fu);
+	if (ret)
+		goto err_wq;
+	fu->flags |= USBG_ENABLED;
+
+	pr_info("Using the UAS protocol\n");
+	return;
+err_wq:
+	usb_ep_disable(fu->ep_status);
+err_status:
+	usb_ep_disable(fu->ep_cmd);
+err_cmd:
+	usb_ep_disable(fu->ep_out);
+err_b_out:
+	usb_ep_disable(fu->ep_in);
+err_b_in:
+	fu->flags = 0;
+}
+
+static int get_cmd_dir(const unsigned char *cdb)
+{
+	int ret;
+
+	switch (cdb[0]) {
+	case READ_6:
+	case READ_10:
+	case READ_12:
+	case READ_16:
+	case INQUIRY:
+	case MODE_SENSE:
+	case MODE_SENSE_10:
+	case SERVICE_ACTION_IN:
+	case MAINTENANCE_IN:
+	case PERSISTENT_RESERVE_IN:
+	case SECURITY_PROTOCOL_IN:
+	case ACCESS_CONTROL_IN:
+	case REPORT_LUNS:
+	case READ_BLOCK_LIMITS:
+	case READ_POSITION:
+	case READ_CAPACITY:
+	case READ_TOC:
+	case READ_FORMAT_CAPACITIES:
+	case REQUEST_SENSE:
+		ret = DMA_FROM_DEVICE;
+		break;
+
+	case WRITE_6:
+	case WRITE_10:
+	case WRITE_12:
+	case WRITE_16:
+	case MODE_SELECT:
+	case MODE_SELECT_10:
+	case WRITE_VERIFY:
+	case WRITE_VERIFY_12:
+	case PERSISTENT_RESERVE_OUT:
+	case MAINTENANCE_OUT:
+	case SECURITY_PROTOCOL_OUT:
+	case ACCESS_CONTROL_OUT:
+		ret = DMA_TO_DEVICE;
+		break;
+	case ALLOW_MEDIUM_REMOVAL:
+	case TEST_UNIT_READY:
+	case SYNCHRONIZE_CACHE:
+	case START_STOP:
+	case ERASE:
+	case REZERO_UNIT:
+	case SEEK_10:
+	case SPACE:
+	case VERIFY:
+	case WRITE_FILEMARKS:
+		ret = DMA_NONE;
+		break;
+	default:
+		pr_warn("target: Unknown data direction for SCSI Opcode "
+				"0x%02x\n", cdb[0]);
+		ret = -EINVAL;
+	}
+	return ret;
+}
+
+static void usbg_data_write_cmpl(struct usb_ep *ep, struct usb_request *req)
+{
+	struct usbg_cmd *cmd = req->context;
+	struct se_cmd *se_cmd = &cmd->se_cmd;
+
+	if (req->status < 0) {
+		pr_err("%s() state %d transfer failed\n", __func__, cmd->state);
+		goto cleanup;
+	}
+
+	if (req->num_sgs == 0) {
+		sg_copy_from_buffer(se_cmd->t_data_sg,
+				se_cmd->t_data_nents,
+				cmd->data_buf,
+				se_cmd->data_length);
+	}
+
+	complete(&cmd->write_complete);
+	return;
+
+cleanup:
+	usbg_cleanup_cmd(cmd);
+}
+
+static int usbg_prepare_w_request(struct usbg_cmd *cmd, struct usb_request *req)
+{
+	struct se_cmd *se_cmd = &cmd->se_cmd;
+	struct f_uas *fu = cmd->fu;
+	struct usb_gadget *gadget = fuas_to_gadget(fu);
+
+	if (!gadget->sg_supported) {
+		cmd->data_buf = kmalloc(se_cmd->data_length, GFP_ATOMIC);
+		if (!cmd->data_buf)
+			return -ENOMEM;
+
+		req->buf = cmd->data_buf;
+	} else {
+		req->buf = NULL;
+		req->num_sgs = se_cmd->t_data_nents;
+		req->sg = se_cmd->t_data_sg;
+	}
+
+	req->complete = usbg_data_write_cmpl;
+	req->length = se_cmd->data_length;
+	req->context = cmd;
+	return 0;
+}
+
+static int usbg_send_status_response(struct se_cmd *se_cmd)
+{
+	struct usbg_cmd *cmd = container_of(se_cmd, struct usbg_cmd,
+			se_cmd);
+	struct f_uas *fu = cmd->fu;
+
+	if (fu->flags & USBG_IS_BOT)
+		return bot_send_status_response(cmd);
+	else
+		return uasp_send_status_response(cmd);
+}
+
+static int usbg_send_write_request(struct se_cmd *se_cmd)
+{
+	struct usbg_cmd *cmd = container_of(se_cmd, struct usbg_cmd,
+			se_cmd);
+	struct f_uas *fu = cmd->fu;
+
+	if (fu->flags & USBG_IS_BOT)
+		return bot_send_write_request(cmd);
+	else
+		return uasp_send_write_request(cmd);
+}
+
+static int usbg_send_read_response(struct se_cmd *se_cmd)
+{
+	struct usbg_cmd *cmd = container_of(se_cmd, struct usbg_cmd,
+			se_cmd);
+	struct f_uas *fu = cmd->fu;
+
+	if (fu->flags & USBG_IS_BOT)
+		return bot_send_read_response(cmd);
+	else
+		return uasp_send_read_response(cmd);
+}
+
+static void usbg_cmd_work(struct work_struct *work)
+{
+	struct usbg_cmd *cmd = container_of(work, struct usbg_cmd, work);
+	struct se_cmd *se_cmd;
+	struct tcm_usbg_nexus *tv_nexus;
+	struct usbg_tpg *tpg;
+	int dir;
+
+	se_cmd = &cmd->se_cmd;
+	tpg = cmd->fu->tpg;
+	tv_nexus = tpg->tpg_nexus;
+	dir = get_cmd_dir(cmd->cmd_buf);
+	if (dir < 0) {
+		transport_init_se_cmd(se_cmd,
+				tv_nexus->tvn_se_sess->se_tpg->se_tpg_tfo,
+				tv_nexus->tvn_se_sess, cmd->data_len, DMA_NONE,
+				cmd->prio_attr, cmd->sense_iu.sense);
+
+		transport_send_check_condition_and_sense(se_cmd,
+				TCM_UNSUPPORTED_SCSI_OPCODE, 1);
+		usbg_cleanup_cmd(cmd);
+		return;
+	}
+
+	target_submit_cmd(se_cmd, tv_nexus->tvn_se_sess,
+			cmd->cmd_buf, cmd->sense_iu.sense, cmd->unpacked_lun,
+			0, cmd->prio_attr, dir, TARGET_SCF_UNKNOWN_SIZE);
+}
+
+static int usbg_submit_command(struct f_uas *fu,
+		void *cmdbuf, unsigned int len)
+{
+	struct command_iu *cmd_iu = cmdbuf;
+	struct usbg_cmd *cmd;
+	struct usbg_tpg *tpg;
+	struct se_cmd *se_cmd;
+	struct tcm_usbg_nexus *tv_nexus;
+	u32 cmd_len;
+	int ret;
+
+	if (cmd_iu->iu_id != IU_ID_COMMAND) {
+		pr_err("Unsupported type %d\n", cmd_iu->iu_id);
+		return -EINVAL;
+	}
+
+	cmd = kzalloc(sizeof *cmd, GFP_ATOMIC);
+	if (!cmd)
+		return -ENOMEM;
+
+	cmd->fu = fu;
+
+	/* XXX until I figure out why I can't free in on complete */
+	kref_init(&cmd->ref);
+	kref_get(&cmd->ref);
+
+	tpg = fu->tpg;
+	cmd_len = (cmd_iu->len & ~0x3) + 16;
+	if (cmd_len > USBG_MAX_CMD)
+		goto err;
+
+	memcpy(cmd->cmd_buf, cmd_iu->cdb, cmd_len);
+
+	cmd->tag = be16_to_cpup(&cmd_iu->tag);
+	if (fu->flags & USBG_USE_STREAMS) {
+		if (cmd->tag > UASP_SS_EP_COMP_NUM_STREAMS)
+			goto err;
+		if (!cmd->tag)
+			cmd->stream = &fu->stream[0];
+		else
+			cmd->stream = &fu->stream[cmd->tag - 1];
+	} else {
+		cmd->stream = &fu->stream[0];
+	}
+
+	tv_nexus = tpg->tpg_nexus;
+	if (!tv_nexus) {
+		pr_err("Missing nexus, ignoring command\n");
+		goto err;
+	}
+
+	switch (cmd_iu->prio_attr & 0x7) {
+	case UAS_HEAD_TAG:
+		cmd->prio_attr = MSG_HEAD_TAG;
+		break;
+	case UAS_ORDERED_TAG:
+		cmd->prio_attr = MSG_ORDERED_TAG;
+		break;
+	case UAS_ACA:
+		cmd->prio_attr = MSG_ACA_TAG;
+		break;
+	default:
+		pr_debug_once("Unsupported prio_attr: %02x.\n",
+				cmd_iu->prio_attr);
+	case UAS_SIMPLE_TAG:
+		cmd->prio_attr = MSG_SIMPLE_TAG;
+		break;
+	}
+
+	se_cmd = &cmd->se_cmd;
+	cmd->unpacked_lun = scsilun_to_int(&cmd_iu->lun);
+
+	INIT_WORK(&cmd->work, usbg_cmd_work);
+	ret = queue_work(tpg->workqueue, &cmd->work);
+	if (ret < 0)
+		goto err;
+
+	return 0;
+err:
+	kfree(cmd);
+	return -EINVAL;
+}
+
+static void bot_cmd_work(struct work_struct *work)
+{
+	struct usbg_cmd *cmd = container_of(work, struct usbg_cmd, work);
+	struct se_cmd *se_cmd;
+	struct tcm_usbg_nexus *tv_nexus;
+	struct usbg_tpg *tpg;
+	int dir;
+
+	se_cmd = &cmd->se_cmd;
+	tpg = cmd->fu->tpg;
+	tv_nexus = tpg->tpg_nexus;
+	dir = get_cmd_dir(cmd->cmd_buf);
+	if (dir < 0) {
+		transport_init_se_cmd(se_cmd,
+				tv_nexus->tvn_se_sess->se_tpg->se_tpg_tfo,
+				tv_nexus->tvn_se_sess, cmd->data_len, DMA_NONE,
+				cmd->prio_attr, cmd->sense_iu.sense);
+
+		transport_send_check_condition_and_sense(se_cmd,
+				TCM_UNSUPPORTED_SCSI_OPCODE, 1);
+		usbg_cleanup_cmd(cmd);
+		return;
+	}
+
+	target_submit_cmd(se_cmd, tv_nexus->tvn_se_sess,
+			cmd->cmd_buf, cmd->sense_iu.sense, cmd->unpacked_lun,
+			cmd->data_len, cmd->prio_attr, dir, 0);
+}
+
+static int bot_submit_command(struct f_uas *fu,
+		void *cmdbuf, unsigned int len)
+{
+	struct bulk_cb_wrap *cbw = cmdbuf;
+	struct usbg_cmd *cmd;
+	struct usbg_tpg *tpg;
+	struct se_cmd *se_cmd;
+	struct tcm_usbg_nexus *tv_nexus;
+	u32 cmd_len;
+	int ret;
+
+	if (cbw->Signature != cpu_to_le32(US_BULK_CB_SIGN)) {
+		pr_err("Wrong signature on CBW\n");
+		return -EINVAL;
+	}
+	if (len != 31) {
+		pr_err("Wrong length for CBW\n");
+		return -EINVAL;
+	}
+
+	cmd_len = cbw->Length;
+	if (cmd_len < 1 || cmd_len > 16)
+		return -EINVAL;
+
+	cmd = kzalloc(sizeof *cmd, GFP_ATOMIC);
+	if (!cmd)
+		return -ENOMEM;
+
+	cmd->fu = fu;
+
+	/* XXX until I figure out why I can't free in on complete */
+	kref_init(&cmd->ref);
+	kref_get(&cmd->ref);
+
+	tpg = fu->tpg;
+
+	memcpy(cmd->cmd_buf, cbw->CDB, cmd_len);
+
+	cmd->bot_tag = cbw->Tag;
+
+	tv_nexus = tpg->tpg_nexus;
+	if (!tv_nexus) {
+		pr_err("Missing nexus, ignoring command\n");
+		goto err;
+	}
+
+	cmd->prio_attr = MSG_SIMPLE_TAG;
+	se_cmd = &cmd->se_cmd;
+	cmd->unpacked_lun = cbw->Lun;
+	cmd->is_read = cbw->Flags & US_BULK_FLAG_IN ? 1 : 0;
+	cmd->data_len = le32_to_cpu(cbw->DataTransferLength);
+
+	INIT_WORK(&cmd->work, bot_cmd_work);
+	ret = queue_work(tpg->workqueue, &cmd->work);
+	if (ret < 0)
+		goto err;
+
+	return 0;
+err:
+	kfree(cmd);
+	return -EINVAL;
+}
+
+/* Start fabric.c code */
+
+static int usbg_check_true(struct se_portal_group *se_tpg)
+{
+	return 1;
+}
+
+static int usbg_check_false(struct se_portal_group *se_tpg)
+{
+	return 0;
+}
+
+static char *usbg_get_fabric_name(void)
+{
+	return "usb_gadget";
+}
+
+static u8 usbg_get_fabric_proto_ident(struct se_portal_group *se_tpg)
+{
+	struct usbg_tpg *tpg = container_of(se_tpg,
+				struct usbg_tpg, se_tpg);
+	struct usbg_tport *tport = tpg->tport;
+	u8 proto_id;
+
+	switch (tport->tport_proto_id) {
+	case SCSI_PROTOCOL_SAS:
+	default:
+		proto_id = sas_get_fabric_proto_ident(se_tpg);
+		break;
+	}
+
+	return proto_id;
+}
+
+static char *usbg_get_fabric_wwn(struct se_portal_group *se_tpg)
+{
+	struct usbg_tpg *tpg = container_of(se_tpg,
+				struct usbg_tpg, se_tpg);
+	struct usbg_tport *tport = tpg->tport;
+
+	return &tport->tport_name[0];
+}
+
+static u16 usbg_get_tag(struct se_portal_group *se_tpg)
+{
+	struct usbg_tpg *tpg = container_of(se_tpg,
+				struct usbg_tpg, se_tpg);
+	return tpg->tport_tpgt;
+}
+
+static u32 usbg_get_default_depth(struct se_portal_group *se_tpg)
+{
+	return 1;
+}
+
+static u32 usbg_get_pr_transport_id(
+	struct se_portal_group *se_tpg,
+	struct se_node_acl *se_nacl,
+	struct t10_pr_registration *pr_reg,
+	int *format_code,
+	unsigned char *buf)
+{
+	struct usbg_tpg *tpg = container_of(se_tpg,
+				struct usbg_tpg, se_tpg);
+	struct usbg_tport *tport = tpg->tport;
+	int ret = 0;
+
+	switch (tport->tport_proto_id) {
+	case SCSI_PROTOCOL_SAS:
+	default:
+		ret = sas_get_pr_transport_id(se_tpg, se_nacl, pr_reg,
+					format_code, buf);
+		break;
+	}
+
+	return ret;
+}
+
+static u32 usbg_get_pr_transport_id_len(
+	struct se_portal_group *se_tpg,
+	struct se_node_acl *se_nacl,
+	struct t10_pr_registration *pr_reg,
+	int *format_code)
+{
+	struct usbg_tpg *tpg = container_of(se_tpg,
+				struct usbg_tpg, se_tpg);
+	struct usbg_tport *tport = tpg->tport;
+	int ret = 0;
+
+	switch (tport->tport_proto_id) {
+	case SCSI_PROTOCOL_SAS:
+	default:
+		ret = sas_get_pr_transport_id_len(se_tpg, se_nacl, pr_reg,
+					format_code);
+		break;
+	}
+
+	return ret;
+}
+
+static char *usbg_parse_pr_out_transport_id(
+	struct se_portal_group *se_tpg,
+	const char *buf,
+	u32 *out_tid_len,
+	char **port_nexus_ptr)
+{
+	struct usbg_tpg *tpg = container_of(se_tpg,
+				struct usbg_tpg, se_tpg);
+	struct usbg_tport *tport = tpg->tport;
+	char *tid = NULL;
+
+	switch (tport->tport_proto_id) {
+	case SCSI_PROTOCOL_SAS:
+	default:
+		tid = sas_parse_pr_out_transport_id(se_tpg, buf, out_tid_len,
+					port_nexus_ptr);
+	}
+
+	return tid;
+}
+
+static struct se_node_acl *usbg_alloc_fabric_acl(struct se_portal_group *se_tpg)
+{
+	struct usbg_nacl *nacl;
+
+	nacl = kzalloc(sizeof(struct usbg_nacl), GFP_KERNEL);
+	if (!nacl) {
+		printk(KERN_ERR "Unable to alocate struct usbg_nacl\n");
+		return NULL;
+	}
+
+	return &nacl->se_node_acl;
+}
+
+static void usbg_release_fabric_acl(
+	struct se_portal_group *se_tpg,
+	struct se_node_acl *se_nacl)
+{
+	struct usbg_nacl *nacl = container_of(se_nacl,
+			struct usbg_nacl, se_node_acl);
+	kfree(nacl);
+}
+
+static u32 usbg_tpg_get_inst_index(struct se_portal_group *se_tpg)
+{
+	return 1;
+}
+
+static int usbg_new_cmd(struct se_cmd *se_cmd)
+{
+	struct usbg_cmd *cmd = container_of(se_cmd, struct usbg_cmd,
+			se_cmd);
+	int ret;
+
+	ret = target_setup_cmd_from_cdb(se_cmd, cmd->cmd_buf);
+	if (ret)
+		return ret;
+
+	return transport_generic_map_mem_to_cmd(se_cmd, NULL, 0, NULL, 0);
+}
+
+static void usbg_cmd_release(struct kref *ref)
+{
+	struct usbg_cmd *cmd = container_of(ref, struct usbg_cmd,
+			ref);
+
+	transport_generic_free_cmd(&cmd->se_cmd, 0);
+}
+
+static void usbg_release_cmd(struct se_cmd *se_cmd)
+{
+	struct usbg_cmd *cmd = container_of(se_cmd, struct usbg_cmd,
+			se_cmd);
+	kfree(cmd->data_buf);
+	kfree(cmd);
+	return;
+}
+
+static int usbg_shutdown_session(struct se_session *se_sess)
+{
+	return 0;
+}
+
+static void usbg_close_session(struct se_session *se_sess)
+{
+	return;
+}
+
+static u32 usbg_sess_get_index(struct se_session *se_sess)
+{
+	return 0;
+}
+
+/*
+ * XXX Error recovery: return != 0 if we expect writes. Dunno when that could be
+ */
+static int usbg_write_pending_status(struct se_cmd *se_cmd)
+{
+	return 0;
+}
+
+static void usbg_set_default_node_attrs(struct se_node_acl *nacl)
+{
+	return;
+}
+
+static u32 usbg_get_task_tag(struct se_cmd *se_cmd)
+{
+	struct usbg_cmd *cmd = container_of(se_cmd, struct usbg_cmd,
+			se_cmd);
+	struct f_uas *fu = cmd->fu;
+
+	if (fu->flags & USBG_IS_BOT)
+		return le32_to_cpu(cmd->bot_tag);
+	else
+		return cmd->tag;
+}
+
+static int usbg_get_cmd_state(struct se_cmd *se_cmd)
+{
+	return 0;
+}
+
+static int usbg_queue_tm_rsp(struct se_cmd *se_cmd)
+{
+	return 0;
+}
+
+static u16 usbg_set_fabric_sense_len(struct se_cmd *se_cmd, u32 sense_length)
+{
+	return 0;
+}
+
+static u16 usbg_get_fabric_sense_len(void)
+{
+	return 0;
+}
+
+static const char *usbg_check_wwn(const char *name)
+{
+	const char *n;
+	unsigned int len;
+
+	n = strstr(name, "naa.");
+	if (!n)
+		return NULL;
+	n += 4;
+	len = strlen(n);
+	if (len == 0 || len > USBG_NAMELEN - 1)
+		return NULL;
+	return n;
+}
+
+static struct se_node_acl *usbg_make_nodeacl(
+	struct se_portal_group *se_tpg,
+	struct config_group *group,
+	const char *name)
+{
+	struct se_node_acl *se_nacl, *se_nacl_new;
+	struct usbg_nacl *nacl;
+	u64 wwpn = 0;
+	u32 nexus_depth;
+	const char *wnn_name;
+
+	wnn_name = usbg_check_wwn(name);
+	if (!wnn_name)
+		return ERR_PTR(-EINVAL);
+	se_nacl_new = usbg_alloc_fabric_acl(se_tpg);
+	if (!(se_nacl_new))
+		return ERR_PTR(-ENOMEM);
+
+	nexus_depth = 1;
+	/*
+	 * se_nacl_new may be released by core_tpg_add_initiator_node_acl()
+	 * when converting a NodeACL from demo mode -> explict
+	 */
+	se_nacl = core_tpg_add_initiator_node_acl(se_tpg, se_nacl_new,
+				name, nexus_depth);
+	if (IS_ERR(se_nacl)) {
+		usbg_release_fabric_acl(se_tpg, se_nacl_new);
+		return se_nacl;
+	}
+	/*
+	 * Locate our struct usbg_nacl and set the FC Nport WWPN
+	 */
+	nacl = container_of(se_nacl, struct usbg_nacl, se_node_acl);
+	nacl->iport_wwpn = wwpn;
+	snprintf(nacl->iport_name, sizeof(nacl->iport_name), "%s", name);
+	return se_nacl;
+}
+
+static void usbg_drop_nodeacl(struct se_node_acl *se_acl)
+{
+	struct usbg_nacl *nacl = container_of(se_acl,
+				struct usbg_nacl, se_node_acl);
+	core_tpg_del_initiator_node_acl(se_acl->se_tpg, se_acl, 1);
+	kfree(nacl);
+}
+
+struct usbg_tpg *the_only_tpg_I_currently_have;
+
+static struct se_portal_group *usbg_make_tpg(
+	struct se_wwn *wwn,
+	struct config_group *group,
+	const char *name)
+{
+	struct usbg_tport *tport = container_of(wwn, struct usbg_tport,
+			tport_wwn);
+	struct usbg_tpg *tpg;
+	unsigned long tpgt;
+	int ret;
+
+	if (strstr(name, "tpgt_") != name)
+		return ERR_PTR(-EINVAL);
+	if (kstrtoul(name + 5, 0, &tpgt) || tpgt > UINT_MAX)
+		return ERR_PTR(-EINVAL);
+	if (the_only_tpg_I_currently_have) {
+		pr_err("Until the gadget framework can't handle multiple\n");
+		pr_err("gadgets, you can't do this here.\n");
+		return ERR_PTR(-EBUSY);
+	}
+
+	tpg = kzalloc(sizeof(struct usbg_tpg), GFP_KERNEL);
+	if (!tpg) {
+		printk(KERN_ERR "Unable to allocate struct usbg_tpg");
+		return ERR_PTR(-ENOMEM);
+	}
+	mutex_init(&tpg->tpg_mutex);
+	atomic_set(&tpg->tpg_port_count, 0);
+	tpg->workqueue = alloc_workqueue("tcm_usb_gadget", 0, 1);
+	if (!tpg->workqueue) {
+		kfree(tpg);
+		return NULL;
+	}
+
+	tpg->tport = tport;
+	tpg->tport_tpgt = tpgt;
+
+	ret = core_tpg_register(&usbg_fabric_configfs->tf_ops, wwn,
+				&tpg->se_tpg, tpg,
+				TRANSPORT_TPG_TYPE_NORMAL);
+	if (ret < 0) {
+		destroy_workqueue(tpg->workqueue);
+		kfree(tpg);
+		return NULL;
+	}
+	the_only_tpg_I_currently_have = tpg;
+	return &tpg->se_tpg;
+}
+
+static void usbg_drop_tpg(struct se_portal_group *se_tpg)
+{
+	struct usbg_tpg *tpg = container_of(se_tpg,
+				struct usbg_tpg, se_tpg);
+
+	core_tpg_deregister(se_tpg);
+	destroy_workqueue(tpg->workqueue);
+	kfree(tpg);
+	the_only_tpg_I_currently_have = NULL;
+}
+
+static struct se_wwn *usbg_make_tport(
+	struct target_fabric_configfs *tf,
+	struct config_group *group,
+	const char *name)
+{
+	struct usbg_tport *tport;
+	const char *wnn_name;
+	u64 wwpn = 0;
+
+	wnn_name = usbg_check_wwn(name);
+	if (!wnn_name)
+		return ERR_PTR(-EINVAL);
+
+	tport = kzalloc(sizeof(struct usbg_tport), GFP_KERNEL);
+	if (!(tport)) {
+		printk(KERN_ERR "Unable to allocate struct usbg_tport");
+		return ERR_PTR(-ENOMEM);
+	}
+	tport->tport_wwpn = wwpn;
+	snprintf(tport->tport_name, sizeof(tport->tport_name), wnn_name);
+	return &tport->tport_wwn;
+}
+
+static void usbg_drop_tport(struct se_wwn *wwn)
+{
+	struct usbg_tport *tport = container_of(wwn,
+				struct usbg_tport, tport_wwn);
+	kfree(tport);
+}
+
+/*
+ * If somebody feels like dropping the version property, go ahead.
+ */
+static ssize_t usbg_wwn_show_attr_version(
+	struct target_fabric_configfs *tf,
+	char *page)
+{
+	return sprintf(page, "usb-gadget fabric module\n");
+}
+TF_WWN_ATTR_RO(usbg, version);
+
+static struct configfs_attribute *usbg_wwn_attrs[] = {
+	&usbg_wwn_version.attr,
+	NULL,
+};
+
+static ssize_t tcm_usbg_tpg_show_enable(
+		struct se_portal_group *se_tpg,
+		char *page)
+{
+	struct usbg_tpg  *tpg = container_of(se_tpg, struct usbg_tpg, se_tpg);
+
+	return snprintf(page, PAGE_SIZE, "%u\n", tpg->gadget_connect);
+}
+
+static int usbg_attach(struct usbg_tpg *);
+static void usbg_detach(struct usbg_tpg *);
+
+static ssize_t tcm_usbg_tpg_store_enable(
+		struct se_portal_group *se_tpg,
+		const char *page,
+		size_t count)
+{
+	struct usbg_tpg  *tpg = container_of(se_tpg, struct usbg_tpg, se_tpg);
+	unsigned long op;
+	ssize_t ret;
+
+	ret = kstrtoul(page, 0, &op);
+	if (ret < 0)
+		return -EINVAL;
+	if (op > 1)
+		return -EINVAL;
+
+	if (op && tpg->gadget_connect)
+		goto out;
+	if (!op && !tpg->gadget_connect)
+		goto out;
+
+	if (op) {
+		ret = usbg_attach(tpg);
+		if (ret)
+			goto out;
+	} else {
+		usbg_detach(tpg);
+	}
+	tpg->gadget_connect = op;
+out:
+	return count;
+}
+TF_TPG_BASE_ATTR(tcm_usbg, enable, S_IRUGO | S_IWUSR);
+
+static ssize_t tcm_usbg_tpg_show_nexus(
+		struct se_portal_group *se_tpg,
+		char *page)
+{
+	struct usbg_tpg *tpg = container_of(se_tpg, struct usbg_tpg, se_tpg);
+	struct tcm_usbg_nexus *tv_nexus;
+	ssize_t ret;
+
+	mutex_lock(&tpg->tpg_mutex);
+	tv_nexus = tpg->tpg_nexus;
+	if (!tv_nexus) {
+		ret = -ENODEV;
+		goto out;
+	}
+	ret = snprintf(page, PAGE_SIZE, "%s\n",
+			tv_nexus->tvn_se_sess->se_node_acl->initiatorname);
+out:
+	mutex_unlock(&tpg->tpg_mutex);
+	return ret;
+}
+
+static int tcm_usbg_make_nexus(struct usbg_tpg *tpg, char *name)
+{
+	struct se_portal_group *se_tpg;
+	struct tcm_usbg_nexus *tv_nexus;
+	int ret;
+
+	mutex_lock(&tpg->tpg_mutex);
+	if (tpg->tpg_nexus) {
+		ret = -EEXIST;
+		pr_debug("tpg->tpg_nexus already exists\n");
+		goto err_unlock;
+	}
+	se_tpg = &tpg->se_tpg;
+
+	ret = -ENOMEM;
+	tv_nexus = kzalloc(sizeof(*tv_nexus), GFP_KERNEL);
+	if (!tv_nexus) {
+		pr_err("Unable to allocate struct tcm_vhost_nexus\n");
+		goto err_unlock;
+	}
+	tv_nexus->tvn_se_sess = transport_init_session();
+	if (IS_ERR(tv_nexus->tvn_se_sess))
+		goto err_free;
+
+	/*
+	 * Since we are running in 'demo mode' this call with generate a
+	 * struct se_node_acl for the tcm_vhost struct se_portal_group with
+	 * the SCSI Initiator port name of the passed configfs group 'name'.
+	 */
+	tv_nexus->tvn_se_sess->se_node_acl = core_tpg_check_initiator_node_acl(
+			se_tpg, name);
+	if (!tv_nexus->tvn_se_sess->se_node_acl) {
+		pr_debug("core_tpg_check_initiator_node_acl() failed"
+				" for %s\n", name);
+		goto err_session;
+	}
+	/*
+	 * Now register the TCM vHost virtual I_T Nexus as active with the
+	 * call to __transport_register_session()
+	 */
+	__transport_register_session(se_tpg, tv_nexus->tvn_se_sess->se_node_acl,
+			tv_nexus->tvn_se_sess, tv_nexus);
+	tpg->tpg_nexus = tv_nexus;
+	mutex_unlock(&tpg->tpg_mutex);
+	return 0;
+
+err_session:
+	transport_free_session(tv_nexus->tvn_se_sess);
+err_free:
+	kfree(tv_nexus);
+err_unlock:
+	mutex_unlock(&tpg->tpg_mutex);
+	return ret;
+}
+
+static int tcm_usbg_drop_nexus(struct usbg_tpg *tpg)
+{
+	struct se_session *se_sess;
+	struct tcm_usbg_nexus *tv_nexus;
+	int ret = -ENODEV;
+
+	mutex_lock(&tpg->tpg_mutex);
+	tv_nexus = tpg->tpg_nexus;
+	if (!tv_nexus)
+		goto out;
+
+	se_sess = tv_nexus->tvn_se_sess;
+	if (!se_sess)
+		goto out;
+
+	if (atomic_read(&tpg->tpg_port_count)) {
+		ret = -EPERM;
+		pr_err("Unable to remove Host I_T Nexus with"
+				" active TPG port count: %d\n",
+				atomic_read(&tpg->tpg_port_count));
+		goto out;
+	}
+
+	pr_debug("Removing I_T Nexus to Initiator Port: %s\n",
+			tv_nexus->tvn_se_sess->se_node_acl->initiatorname);
+	/*
+	 * Release the SCSI I_T Nexus to the emulated vHost Target Port
+	 */
+	transport_deregister_session(tv_nexus->tvn_se_sess);
+	tpg->tpg_nexus = NULL;
+
+	kfree(tv_nexus);
+out:
+	mutex_unlock(&tpg->tpg_mutex);
+	return 0;
+}
+
+static ssize_t tcm_usbg_tpg_store_nexus(
+		struct se_portal_group *se_tpg,
+		const char *page,
+		size_t count)
+{
+	struct usbg_tpg *tpg = container_of(se_tpg, struct usbg_tpg, se_tpg);
+	unsigned char i_port[USBG_NAMELEN], *ptr;
+	int ret;
+
+	if (!strncmp(page, "NULL", 4)) {
+		ret = tcm_usbg_drop_nexus(tpg);
+		return (!ret) ? count : ret;
+	}
+	if (strlen(page) > USBG_NAMELEN) {
+		pr_err("Emulated NAA Sas Address: %s, exceeds"
+				" max: %d\n", page, USBG_NAMELEN);
+		return -EINVAL;
+	}
+	snprintf(i_port, USBG_NAMELEN, "%s", page);
+
+	ptr = strstr(i_port, "naa.");
+	if (!ptr) {
+		pr_err("Missing 'naa.' prefix\n");
+		return -EINVAL;
+	}
+
+	if (i_port[strlen(i_port) - 1] == '\n')
+		i_port[strlen(i_port) - 1] = '\0';
+
+	ret = tcm_usbg_make_nexus(tpg, &i_port[4]);
+	if (ret < 0)
+		return ret;
+	return count;
+}
+TF_TPG_BASE_ATTR(tcm_usbg, nexus, S_IRUGO | S_IWUSR);
+
+static struct configfs_attribute *usbg_base_attrs[] = {
+	&tcm_usbg_tpg_enable.attr,
+	&tcm_usbg_tpg_nexus.attr,
+	NULL,
+};
+
+static int usbg_port_link(struct se_portal_group *se_tpg, struct se_lun *lun)
+{
+	struct usbg_tpg *tpg = container_of(se_tpg, struct usbg_tpg, se_tpg);
+
+	atomic_inc(&tpg->tpg_port_count);
+	smp_mb__after_atomic_inc();
+	return 0;
+}
+
+static void usbg_port_unlink(struct se_portal_group *se_tpg,
+		struct se_lun *se_lun)
+{
+	struct usbg_tpg *tpg = container_of(se_tpg, struct usbg_tpg, se_tpg);
+
+	atomic_dec(&tpg->tpg_port_count);
+	smp_mb__after_atomic_dec();
+}
+
+static int usbg_check_stop_free(struct se_cmd *se_cmd)
+{
+	struct usbg_cmd *cmd = container_of(se_cmd, struct usbg_cmd,
+			se_cmd);
+
+	kref_put(&cmd->ref, usbg_cmd_release);
+	return 1;
+}
+
+static struct target_core_fabric_ops usbg_ops = {
+	.get_fabric_name		= usbg_get_fabric_name,
+	.get_fabric_proto_ident		= usbg_get_fabric_proto_ident,
+	.tpg_get_wwn			= usbg_get_fabric_wwn,
+	.tpg_get_tag			= usbg_get_tag,
+	.tpg_get_default_depth		= usbg_get_default_depth,
+	.tpg_get_pr_transport_id	= usbg_get_pr_transport_id,
+	.tpg_get_pr_transport_id_len	= usbg_get_pr_transport_id_len,
+	.tpg_parse_pr_out_transport_id	= usbg_parse_pr_out_transport_id,
+	.tpg_check_demo_mode		= usbg_check_true,
+	.tpg_check_demo_mode_cache	= usbg_check_false,
+	.tpg_check_demo_mode_write_protect = usbg_check_false,
+	.tpg_check_prod_mode_write_protect = usbg_check_false,
+	.tpg_alloc_fabric_acl		= usbg_alloc_fabric_acl,
+	.tpg_release_fabric_acl		= usbg_release_fabric_acl,
+	.tpg_get_inst_index		= usbg_tpg_get_inst_index,
+	.new_cmd_map			= usbg_new_cmd,
+	.release_cmd			= usbg_release_cmd,
+	.shutdown_session		= usbg_shutdown_session,
+	.close_session			= usbg_close_session,
+	.sess_get_index			= usbg_sess_get_index,
+	.sess_get_initiator_sid		= NULL,
+	.write_pending			= usbg_send_write_request,
+	.write_pending_status		= usbg_write_pending_status,
+	.set_default_node_attributes	= usbg_set_default_node_attrs,
+	.get_task_tag			= usbg_get_task_tag,
+	.get_cmd_state			= usbg_get_cmd_state,
+	.queue_data_in			= usbg_send_read_response,
+	.queue_status			= usbg_send_status_response,
+	.queue_tm_rsp			= usbg_queue_tm_rsp,
+	.get_fabric_sense_len		= usbg_get_fabric_sense_len,
+	.set_fabric_sense_len		= usbg_set_fabric_sense_len,
+	.check_stop_free		= usbg_check_stop_free,
+
+	.fabric_make_wwn		= usbg_make_tport,
+	.fabric_drop_wwn		= usbg_drop_tport,
+	.fabric_make_tpg		= usbg_make_tpg,
+	.fabric_drop_tpg		= usbg_drop_tpg,
+	.fabric_post_link		= usbg_port_link,
+	.fabric_pre_unlink		= usbg_port_unlink,
+	.fabric_make_np			= NULL,
+	.fabric_drop_np			= NULL,
+	.fabric_make_nodeacl		= usbg_make_nodeacl,
+	.fabric_drop_nodeacl		= usbg_drop_nodeacl,
+};
+
+static int usbg_register_configfs(void)
+{
+	struct target_fabric_configfs *fabric;
+	int ret;
+
+	fabric = target_fabric_configfs_init(THIS_MODULE, "usb_gadget");
+	if (IS_ERR(fabric)) {
+		printk(KERN_ERR "target_fabric_configfs_init() failed\n");
+		return PTR_ERR(fabric);
+	}
+
+	fabric->tf_ops = usbg_ops;
+	TF_CIT_TMPL(fabric)->tfc_wwn_cit.ct_attrs = usbg_wwn_attrs;
+	TF_CIT_TMPL(fabric)->tfc_tpg_base_cit.ct_attrs = usbg_base_attrs;
+	TF_CIT_TMPL(fabric)->tfc_tpg_attrib_cit.ct_attrs = NULL;
+	TF_CIT_TMPL(fabric)->tfc_tpg_param_cit.ct_attrs = NULL;
+	TF_CIT_TMPL(fabric)->tfc_tpg_np_base_cit.ct_attrs = NULL;
+	TF_CIT_TMPL(fabric)->tfc_tpg_nacl_base_cit.ct_attrs = NULL;
+	TF_CIT_TMPL(fabric)->tfc_tpg_nacl_attrib_cit.ct_attrs = NULL;
+	TF_CIT_TMPL(fabric)->tfc_tpg_nacl_auth_cit.ct_attrs = NULL;
+	TF_CIT_TMPL(fabric)->tfc_tpg_nacl_param_cit.ct_attrs = NULL;
+	ret = target_fabric_configfs_register(fabric);
+	if (ret < 0) {
+		printk(KERN_ERR "target_fabric_configfs_register() failed"
+				" for usb-gadget\n");
+		return ret;
+	}
+	usbg_fabric_configfs = fabric;
+	return 0;
+};
+
+static void usbg_deregister_configfs(void)
+{
+	if (!(usbg_fabric_configfs))
+		return;
+
+	target_fabric_configfs_deregister(usbg_fabric_configfs);
+	usbg_fabric_configfs = NULL;
+};
+
+/* Start gadget.c code */
+
+static struct usb_interface_descriptor bot_intf_desc = {
+	.bLength =              sizeof(bot_intf_desc),
+	.bDescriptorType =      USB_DT_INTERFACE,
+	.bAlternateSetting =	0,
+	.bNumEndpoints =        2,
+	.bAlternateSetting =	USB_G_ALT_INT_BBB,
+	.bInterfaceClass =      USB_CLASS_MASS_STORAGE,
+	.bInterfaceSubClass =   USB_SC_SCSI,
+	.bInterfaceProtocol =   USB_PR_BULK,
+	.iInterface =           USB_G_STR_INT_UAS,
+};
+
+static struct usb_interface_descriptor uasp_intf_desc = {
+	.bLength =		sizeof(uasp_intf_desc),
+	.bDescriptorType =	USB_DT_INTERFACE,
+	.bNumEndpoints =	4,
+	.bAlternateSetting =	USB_G_ALT_INT_UAS,
+	.bInterfaceClass =	USB_CLASS_MASS_STORAGE,
+	.bInterfaceSubClass =	USB_SC_SCSI,
+	.bInterfaceProtocol =	USB_PR_UAS,
+	.iInterface =		USB_G_STR_INT_BBB,
+};
+
+static struct usb_endpoint_descriptor uasp_bi_desc = {
+	.bLength =		USB_DT_ENDPOINT_SIZE,
+	.bDescriptorType =	USB_DT_ENDPOINT,
+	.bEndpointAddress =	USB_DIR_IN,
+	.bmAttributes =		USB_ENDPOINT_XFER_BULK,
+	.wMaxPacketSize =	cpu_to_le16(512),
+};
+
+static struct usb_endpoint_descriptor uasp_fs_bi_desc = {
+	.bLength =		USB_DT_ENDPOINT_SIZE,
+	.bDescriptorType =	USB_DT_ENDPOINT,
+	.bEndpointAddress =	USB_DIR_IN,
+	.bmAttributes =		USB_ENDPOINT_XFER_BULK,
+};
+
+static struct usb_pipe_usage_descriptor uasp_bi_pipe_desc = {
+	.bLength =		sizeof(uasp_bi_pipe_desc),
+	.bDescriptorType =	USB_DT_PIPE_USAGE,
+	.bPipeID =		DATA_IN_PIPE_ID,
+};
+
+static struct usb_endpoint_descriptor uasp_ss_bi_desc = {
+	.bLength =		USB_DT_ENDPOINT_SIZE,
+	.bDescriptorType =	USB_DT_ENDPOINT,
+	.bEndpointAddress =	USB_DIR_IN,
+	.bmAttributes =		USB_ENDPOINT_XFER_BULK,
+	.wMaxPacketSize =	cpu_to_le16(1024),
+};
+
+static struct usb_ss_ep_comp_descriptor uasp_bi_ep_comp_desc = {
+	.bLength =		sizeof(uasp_bi_ep_comp_desc),
+	.bDescriptorType =	USB_DT_SS_ENDPOINT_COMP,
+	.bMaxBurst =		0,
+	.bmAttributes =		UASP_SS_EP_COMP_LOG_STREAMS,
+	.wBytesPerInterval =	0,
+};
+
+static struct usb_ss_ep_comp_descriptor bot_bi_ep_comp_desc = {
+	.bLength =		sizeof(bot_bi_ep_comp_desc),
+	.bDescriptorType =	USB_DT_SS_ENDPOINT_COMP,
+	.bMaxBurst =		0,
+};
+
+static struct usb_endpoint_descriptor uasp_bo_desc = {
+	.bLength =		USB_DT_ENDPOINT_SIZE,
+	.bDescriptorType =	USB_DT_ENDPOINT,
+	.bEndpointAddress =	USB_DIR_OUT,
+	.bmAttributes =		USB_ENDPOINT_XFER_BULK,
+	.wMaxPacketSize =	cpu_to_le16(512),
+};
+
+static struct usb_endpoint_descriptor uasp_fs_bo_desc = {
+	.bLength =		USB_DT_ENDPOINT_SIZE,
+	.bDescriptorType =	USB_DT_ENDPOINT,
+	.bEndpointAddress =	USB_DIR_OUT,
+	.bmAttributes =		USB_ENDPOINT_XFER_BULK,
+};
+
+static struct usb_pipe_usage_descriptor uasp_bo_pipe_desc = {
+	.bLength =		sizeof(uasp_bo_pipe_desc),
+	.bDescriptorType =	USB_DT_PIPE_USAGE,
+	.bPipeID =		DATA_OUT_PIPE_ID,
+};
+
+static struct usb_endpoint_descriptor uasp_ss_bo_desc = {
+	.bLength =		USB_DT_ENDPOINT_SIZE,
+	.bDescriptorType =	USB_DT_ENDPOINT,
+	.bEndpointAddress =	USB_DIR_OUT,
+	.bmAttributes =		USB_ENDPOINT_XFER_BULK,
+	.wMaxPacketSize =	cpu_to_le16(0x400),
+};
+
+static struct usb_ss_ep_comp_descriptor uasp_bo_ep_comp_desc = {
+	.bLength =		sizeof(uasp_bo_ep_comp_desc),
+	.bDescriptorType =	USB_DT_SS_ENDPOINT_COMP,
+	.bmAttributes =		UASP_SS_EP_COMP_LOG_STREAMS,
+};
+
+static struct usb_ss_ep_comp_descriptor bot_bo_ep_comp_desc = {
+	.bLength =		sizeof(bot_bo_ep_comp_desc),
+	.bDescriptorType =	USB_DT_SS_ENDPOINT_COMP,
+};
+
+static struct usb_endpoint_descriptor uasp_status_desc = {
+	.bLength =		USB_DT_ENDPOINT_SIZE,
+	.bDescriptorType =	USB_DT_ENDPOINT,
+	.bEndpointAddress =	USB_DIR_IN,
+	.bmAttributes =		USB_ENDPOINT_XFER_BULK,
+	.wMaxPacketSize =	cpu_to_le16(512),
+};
+
+static struct usb_endpoint_descriptor uasp_fs_status_desc = {
+	.bLength =		USB_DT_ENDPOINT_SIZE,
+	.bDescriptorType =	USB_DT_ENDPOINT,
+	.bEndpointAddress =	USB_DIR_IN,
+	.bmAttributes =		USB_ENDPOINT_XFER_BULK,
+};
+
+static struct usb_pipe_usage_descriptor uasp_status_pipe_desc = {
+	.bLength =		sizeof(uasp_status_pipe_desc),
+	.bDescriptorType =	USB_DT_PIPE_USAGE,
+	.bPipeID =		STATUS_PIPE_ID,
+};
+
+static struct usb_endpoint_descriptor uasp_ss_status_desc = {
+	.bLength =		USB_DT_ENDPOINT_SIZE,
+	.bDescriptorType =	USB_DT_ENDPOINT,
+	.bEndpointAddress =	USB_DIR_IN,
+	.bmAttributes =		USB_ENDPOINT_XFER_BULK,
+	.wMaxPacketSize =	cpu_to_le16(1024),
+};
+
+static struct usb_ss_ep_comp_descriptor uasp_status_in_ep_comp_desc = {
+	.bLength =		sizeof(uasp_status_in_ep_comp_desc),
+	.bDescriptorType =	USB_DT_SS_ENDPOINT_COMP,
+	.bmAttributes =		UASP_SS_EP_COMP_LOG_STREAMS,
+};
+
+static struct usb_endpoint_descriptor uasp_cmd_desc = {
+	.bLength =		USB_DT_ENDPOINT_SIZE,
+	.bDescriptorType =	USB_DT_ENDPOINT,
+	.bEndpointAddress =	USB_DIR_OUT,
+	.bmAttributes =		USB_ENDPOINT_XFER_BULK,
+	.wMaxPacketSize =	cpu_to_le16(512),
+};
+
+static struct usb_endpoint_descriptor uasp_fs_cmd_desc = {
+	.bLength =		USB_DT_ENDPOINT_SIZE,
+	.bDescriptorType =	USB_DT_ENDPOINT,
+	.bEndpointAddress =	USB_DIR_OUT,
+	.bmAttributes =		USB_ENDPOINT_XFER_BULK,
+};
+
+static struct usb_pipe_usage_descriptor uasp_cmd_pipe_desc = {
+	.bLength =		sizeof(uasp_cmd_pipe_desc),
+	.bDescriptorType =	USB_DT_PIPE_USAGE,
+	.bPipeID =		CMD_PIPE_ID,
+};
+
+static struct usb_endpoint_descriptor uasp_ss_cmd_desc = {
+	.bLength =		USB_DT_ENDPOINT_SIZE,
+	.bDescriptorType =	USB_DT_ENDPOINT,
+	.bEndpointAddress =	USB_DIR_OUT,
+	.bmAttributes =		USB_ENDPOINT_XFER_BULK,
+	.wMaxPacketSize =	cpu_to_le16(1024),
+};
+
+static struct usb_ss_ep_comp_descriptor uasp_cmd_comp_desc = {
+	.bLength =		sizeof(uasp_cmd_comp_desc),
+	.bDescriptorType =	USB_DT_SS_ENDPOINT_COMP,
+};
+
+static struct usb_descriptor_header *uasp_fs_function_desc[] = {
+	(struct usb_descriptor_header *) &bot_intf_desc,
+	(struct usb_descriptor_header *) &uasp_fs_bi_desc,
+	(struct usb_descriptor_header *) &uasp_fs_bo_desc,
+
+	(struct usb_descriptor_header *) &uasp_intf_desc,
+	(struct usb_descriptor_header *) &uasp_fs_bi_desc,
+	(struct usb_descriptor_header *) &uasp_bi_pipe_desc,
+	(struct usb_descriptor_header *) &uasp_fs_bo_desc,
+	(struct usb_descriptor_header *) &uasp_bo_pipe_desc,
+	(struct usb_descriptor_header *) &uasp_fs_status_desc,
+	(struct usb_descriptor_header *) &uasp_status_pipe_desc,
+	(struct usb_descriptor_header *) &uasp_fs_cmd_desc,
+	(struct usb_descriptor_header *) &uasp_cmd_pipe_desc,
+};
+
+static struct usb_descriptor_header *uasp_hs_function_desc[] = {
+	(struct usb_descriptor_header *) &bot_intf_desc,
+	(struct usb_descriptor_header *) &uasp_bi_desc,
+	(struct usb_descriptor_header *) &uasp_bo_desc,
+
+	(struct usb_descriptor_header *) &uasp_intf_desc,
+	(struct usb_descriptor_header *) &uasp_bi_desc,
+	(struct usb_descriptor_header *) &uasp_bi_pipe_desc,
+	(struct usb_descriptor_header *) &uasp_bo_desc,
+	(struct usb_descriptor_header *) &uasp_bo_pipe_desc,
+	(struct usb_descriptor_header *) &uasp_status_desc,
+	(struct usb_descriptor_header *) &uasp_status_pipe_desc,
+	(struct usb_descriptor_header *) &uasp_cmd_desc,
+	(struct usb_descriptor_header *) &uasp_cmd_pipe_desc,
+	NULL,
+};
+
+static struct usb_descriptor_header *uasp_ss_function_desc[] = {
+	(struct usb_descriptor_header *) &bot_intf_desc,
+	(struct usb_descriptor_header *) &uasp_ss_bi_desc,
+	(struct usb_descriptor_header *) &bot_bi_ep_comp_desc,
+	(struct usb_descriptor_header *) &uasp_ss_bo_desc,
+	(struct usb_descriptor_header *) &bot_bo_ep_comp_desc,
+
+	(struct usb_descriptor_header *) &uasp_intf_desc,
+	(struct usb_descriptor_header *) &uasp_ss_bi_desc,
+	(struct usb_descriptor_header *) &uasp_bi_ep_comp_desc,
+	(struct usb_descriptor_header *) &uasp_bi_pipe_desc,
+	(struct usb_descriptor_header *) &uasp_ss_bo_desc,
+	(struct usb_descriptor_header *) &uasp_bo_ep_comp_desc,
+	(struct usb_descriptor_header *) &uasp_bo_pipe_desc,
+	(struct usb_descriptor_header *) &uasp_ss_status_desc,
+	(struct usb_descriptor_header *) &uasp_status_in_ep_comp_desc,
+	(struct usb_descriptor_header *) &uasp_status_pipe_desc,
+	(struct usb_descriptor_header *) &uasp_ss_cmd_desc,
+	(struct usb_descriptor_header *) &uasp_cmd_comp_desc,
+	(struct usb_descriptor_header *) &uasp_cmd_pipe_desc,
+	NULL,
+};
+
+#define UAS_VENDOR_ID	0x0525	/* NetChip */
+#define UAS_PRODUCT_ID	0xa4a5	/* Linux-USB File-backed Storage Gadget */
+
+static struct usb_device_descriptor usbg_device_desc = {
+	.bLength =		sizeof(usbg_device_desc),
+	.bDescriptorType =	USB_DT_DEVICE,
+	.bcdUSB =		cpu_to_le16(0x0200),
+	.bDeviceClass =		USB_CLASS_PER_INTERFACE,
+	.idVendor =		cpu_to_le16(UAS_VENDOR_ID),
+	.idProduct =		cpu_to_le16(UAS_PRODUCT_ID),
+	.iManufacturer =	USB_G_STR_MANUFACTOR,
+	.iProduct =		USB_G_STR_PRODUCT,
+	.iSerialNumber =	USB_G_STR_SERIAL,
+
+	.bNumConfigurations =   1,
+};
+
+static struct usb_string	usbg_us_strings[] = {
+	{ USB_G_STR_MANUFACTOR,	"Target Manufactor"},
+	{ USB_G_STR_PRODUCT,	"Target Product"},
+	{ USB_G_STR_SERIAL,	"000000000001"},
+	{ USB_G_STR_CONFIG,	"default config"},
+	{ USB_G_STR_INT_UAS,	"USB Attached SCSI"},
+	{ USB_G_STR_INT_BBB,	"Bulk Only Transport"},
+	{ },
+};
+
+static struct usb_gadget_strings usbg_stringtab = {
+	.language = 0x0409,
+	.strings = usbg_us_strings,
+};
+
+static struct usb_gadget_strings *usbg_strings[] = {
+	&usbg_stringtab,
+	NULL,
+};
+
+static int guas_unbind(struct usb_composite_dev *cdev)
+{
+	return 0;
+}
+
+static struct usb_configuration usbg_config_driver = {
+	.label                  = "Linux Target",
+	.bConfigurationValue    = 1,
+	.iConfiguration		= USB_G_STR_CONFIG,
+	.bmAttributes           = USB_CONFIG_ATT_SELFPOWER,
+};
+
+static void give_back_ep(struct usb_ep **pep)
+{
+	struct usb_ep *ep = *pep;
+	if (!ep)
+		return;
+	ep->driver_data = NULL;
+}
+
+static int usbg_bind(struct usb_configuration *c, struct usb_function *f)
+{
+	struct f_uas		*fu = to_f_uas(f);
+	struct usb_gadget	*gadget = c->cdev->gadget;
+	struct usb_ep		*ep;
+	int			iface;
+
+	iface = usb_interface_id(c, f);
+	if (iface < 0)
+		return iface;
+
+	bot_intf_desc.bInterfaceNumber = iface;
+	uasp_intf_desc.bInterfaceNumber = iface;
+	fu->iface = iface;
+	ep = usb_ep_autoconfig_ss(gadget, &uasp_ss_bi_desc,
+			&uasp_bi_ep_comp_desc);
+	if (!ep)
+		goto ep_fail;
+
+	ep->driver_data = fu;
+	fu->ep_in = ep;
+
+	ep = usb_ep_autoconfig_ss(gadget, &uasp_ss_bo_desc,
+			&uasp_bo_ep_comp_desc);
+	if (!ep)
+		goto ep_fail;
+	ep->driver_data = fu;
+	fu->ep_out = ep;
+
+	ep = usb_ep_autoconfig_ss(gadget, &uasp_ss_status_desc,
+			&uasp_status_in_ep_comp_desc);
+	if (!ep)
+		goto ep_fail;
+	ep->driver_data = fu;
+	fu->ep_status = ep;
+
+	ep = usb_ep_autoconfig_ss(gadget, &uasp_ss_cmd_desc,
+			&uasp_cmd_comp_desc);
+	if (!ep)
+		goto ep_fail;
+	ep->driver_data = fu;
+	fu->ep_cmd = ep;
+
+	/* Assume endpoint addresses are the same for both speeds */
+	uasp_bi_desc.bEndpointAddress =	uasp_ss_bi_desc.bEndpointAddress;
+	uasp_bo_desc.bEndpointAddress = uasp_ss_bo_desc.bEndpointAddress;
+	uasp_status_desc.bEndpointAddress =
+		uasp_ss_status_desc.bEndpointAddress;
+	uasp_cmd_desc.bEndpointAddress = uasp_ss_cmd_desc.bEndpointAddress;
+
+	uasp_fs_bi_desc.bEndpointAddress = uasp_ss_bi_desc.bEndpointAddress;
+	uasp_fs_bo_desc.bEndpointAddress = uasp_ss_bo_desc.bEndpointAddress;
+	uasp_fs_status_desc.bEndpointAddress =
+		uasp_ss_status_desc.bEndpointAddress;
+	uasp_fs_cmd_desc.bEndpointAddress = uasp_ss_cmd_desc.bEndpointAddress;
+
+	return 0;
+ep_fail:
+	pr_err("Can't claim all required eps\n");
+
+	give_back_ep(&fu->ep_in);
+	give_back_ep(&fu->ep_out);
+	give_back_ep(&fu->ep_status);
+	give_back_ep(&fu->ep_cmd);
+	return -ENOTSUPP;
+}
+
+static void usbg_unbind(struct usb_configuration *c, struct usb_function *f)
+{
+	struct f_uas *fu = to_f_uas(f);
+
+	kfree(fu);
+}
+
+struct guas_setup_wq {
+	struct work_struct work;
+	struct f_uas *fu;
+	unsigned int alt;
+};
+
+static void usbg_delayed_set_alt(struct work_struct *wq)
+{
+	struct guas_setup_wq *work = container_of(wq, struct guas_setup_wq,
+			work);
+	struct f_uas *fu = work->fu;
+	int alt = work->alt;
+
+	kfree(work);
+
+	if (fu->flags & USBG_IS_BOT)
+		bot_cleanup_old_alt(fu);
+	if (fu->flags & USBG_IS_UAS)
+		uasp_cleanup_old_alt(fu);
+
+	if (alt == USB_G_ALT_INT_BBB)
+		bot_set_alt(fu);
+	else if (alt == USB_G_ALT_INT_UAS)
+		uasp_set_alt(fu);
+	usb_composite_setup_continue(fu->function.config->cdev);
+}
+
+static int usbg_set_alt(struct usb_function *f, unsigned intf, unsigned alt)
+{
+	struct f_uas *fu = to_f_uas(f);
+
+	if ((alt == USB_G_ALT_INT_BBB) || (alt == USB_G_ALT_INT_UAS)) {
+		struct guas_setup_wq *work;
+
+		work = kmalloc(sizeof(*work), GFP_ATOMIC);
+		if (!work)
+			return -ENOMEM;
+		INIT_WORK(&work->work, usbg_delayed_set_alt);
+		work->fu = fu;
+		work->alt = alt;
+		schedule_work(&work->work);
+		return USB_GADGET_DELAYED_STATUS;
+	}
+	return -EOPNOTSUPP;
+}
+
+static void usbg_disable(struct usb_function *f)
+{
+	struct f_uas *fu = to_f_uas(f);
+
+	if (fu->flags & USBG_IS_UAS)
+		uasp_cleanup_old_alt(fu);
+	else if (fu->flags & USBG_IS_BOT)
+		bot_cleanup_old_alt(fu);
+	fu->flags = 0;
+}
+
+static int usbg_setup(struct usb_function *f,
+		const struct usb_ctrlrequest *ctrl)
+{
+	struct f_uas *fu = to_f_uas(f);
+
+	if (!(fu->flags & USBG_IS_BOT))
+		return -EOPNOTSUPP;
+
+	return usbg_bot_setup(f, ctrl);
+}
+
+static int usbg_cfg_bind(struct usb_configuration *c)
+{
+	struct f_uas *fu;
+	int ret;
+
+	fu = kzalloc(sizeof(*fu), GFP_KERNEL);
+	if (!fu)
+		return -ENOMEM;
+	fu->function.name = "Target Function";
+	fu->function.descriptors = uasp_fs_function_desc;
+	fu->function.hs_descriptors = uasp_hs_function_desc;
+	fu->function.ss_descriptors = uasp_ss_function_desc;
+	fu->function.bind = usbg_bind;
+	fu->function.unbind = usbg_unbind;
+	fu->function.set_alt = usbg_set_alt;
+	fu->function.setup = usbg_setup;
+	fu->function.disable = usbg_disable;
+	fu->tpg = the_only_tpg_I_currently_have;
+
+	ret = usb_add_function(c, &fu->function);
+	if (ret)
+		goto err;
+
+	return 0;
+err:
+	kfree(fu);
+	return ret;
+}
+
+static int usb_target_bind(struct usb_composite_dev *cdev)
+{
+	int ret;
+
+	ret = usb_add_config(cdev, &usbg_config_driver,
+			usbg_cfg_bind);
+	return 0;
+}
+
+static struct usb_composite_driver usbg_driver = {
+	.name           = "g_target",
+	.dev            = &usbg_device_desc,
+	.strings        = usbg_strings,
+	.max_speed      = USB_SPEED_SUPER,
+	.unbind         = guas_unbind,
+};
+
+static int usbg_attach(struct usbg_tpg *tpg)
+{
+	return usb_composite_probe(&usbg_driver, usb_target_bind);
+}
+
+static void usbg_detach(struct usbg_tpg *tpg)
+{
+	usb_composite_unregister(&usbg_driver);
+}
+
+static int __init usb_target_gadget_init(void)
+{
+	int ret;
+
+	ret = usbg_register_configfs();
+	return ret;
+}
+module_init(usb_target_gadget_init);
+
+static void __exit usb_target_gadget_exit(void)
+{
+	usbg_deregister_configfs();
+}
+module_exit(usb_target_gadget_exit);
+
+MODULE_AUTHOR("Sebastian Andrzej Siewior <bigeasy@linutronix.de>");
+MODULE_DESCRIPTION("usb-gadget fabric");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/usb/gadget/tcm_usb_gadget.h b/drivers/usb/gadget/tcm_usb_gadget.h
new file mode 100644
index 00000000000..bb18999a9a8
--- /dev/null
+++ b/drivers/usb/gadget/tcm_usb_gadget.h
@@ -0,0 +1,146 @@
+#ifndef __TARGET_USB_GADGET_H__
+#define __TARGET_USB_GADGET_H__
+
+#include <linux/kref.h>
+/* #include <linux/usb/uas.h> */
+#include <linux/usb/composite.h>
+#include <linux/usb/uas.h>
+#include <linux/usb/storage.h>
+#include <scsi/scsi.h>
+#include <target/target_core_base.h>
+#include <target/target_core_fabric.h>
+
+#define USBG_NAMELEN 32
+
+#define fuas_to_gadget(f)	(f->function.config->cdev->gadget)
+#define UASP_SS_EP_COMP_LOG_STREAMS 4
+#define UASP_SS_EP_COMP_NUM_STREAMS (1 << UASP_SS_EP_COMP_LOG_STREAMS)
+
+#define USB_G_STR_MANUFACTOR    1
+#define USB_G_STR_PRODUCT       2
+#define USB_G_STR_SERIAL        3
+#define USB_G_STR_CONFIG        4
+#define USB_G_STR_INT_UAS       5
+#define USB_G_STR_INT_BBB       6
+
+#define USB_G_ALT_INT_BBB       0
+#define USB_G_ALT_INT_UAS       1
+
+struct usbg_nacl {
+	/* Binary World Wide unique Port Name for SAS Initiator port */
+	u64 iport_wwpn;
+	/* ASCII formatted WWPN for Sas Initiator port */
+	char iport_name[USBG_NAMELEN];
+	/* Returned by usbg_make_nodeacl() */
+	struct se_node_acl se_node_acl;
+};
+
+struct tcm_usbg_nexus {
+	struct se_session *tvn_se_sess;
+};
+
+struct usbg_tpg {
+	struct mutex tpg_mutex;
+	/* SAS port target portal group tag for TCM */
+	u16 tport_tpgt;
+	/* Pointer back to usbg_tport */
+	struct usbg_tport *tport;
+	struct workqueue_struct *workqueue;
+	/* Returned by usbg_make_tpg() */
+	struct se_portal_group se_tpg;
+	u32 gadget_connect;
+	struct tcm_usbg_nexus *tpg_nexus;
+	atomic_t tpg_port_count;
+};
+
+struct usbg_tport {
+	/* SCSI protocol the tport is providing */
+	u8 tport_proto_id;
+	/* Binary World Wide unique Port Name for SAS Target port */
+	u64 tport_wwpn;
+	/* ASCII formatted WWPN for SAS Target port */
+	char tport_name[USBG_NAMELEN];
+	/* Returned by usbg_make_tport() */
+	struct se_wwn tport_wwn;
+};
+
+enum uas_state {
+	UASP_SEND_DATA,
+	UASP_RECEIVE_DATA,
+	UASP_SEND_STATUS,
+	UASP_QUEUE_COMMAND,
+};
+
+#define USBG_MAX_CMD    64
+struct usbg_cmd {
+	/* common */
+	u8 cmd_buf[USBG_MAX_CMD];
+	u32 data_len;
+	struct work_struct work;
+	int unpacked_lun;
+	struct se_cmd se_cmd;
+	void *data_buf; /* used if no sg support available */
+	struct f_uas *fu;
+	struct completion write_complete;
+	struct kref ref;
+
+	/* UAS only */
+	u16 tag;
+	u16 prio_attr;
+	struct sense_iu sense_iu;
+	enum uas_state state;
+	struct uas_stream *stream;
+
+	/* BOT only */
+	__le32 bot_tag;
+	unsigned int csw_code;
+	unsigned is_read:1;
+
+};
+
+struct uas_stream {
+	struct usb_request	*req_in;
+	struct usb_request	*req_out;
+	struct usb_request	*req_status;
+};
+
+struct usbg_cdb {
+	struct usb_request	*req;
+	void			*buf;
+};
+
+struct bot_status {
+	struct usb_request	*req;
+	struct bulk_cs_wrap	csw;
+};
+
+struct f_uas {
+	struct usbg_tpg		*tpg;
+	struct usb_function	function;
+	u16			iface;
+
+	u32			flags;
+#define USBG_ENABLED		(1 << 0)
+#define USBG_IS_UAS		(1 << 1)
+#define USBG_USE_STREAMS	(1 << 2)
+#define USBG_IS_BOT		(1 << 3)
+#define USBG_BOT_CMD_PEND	(1 << 4)
+
+	struct usbg_cdb		cmd;
+	struct usb_ep		*ep_in;
+	struct usb_ep		*ep_out;
+
+	/* UAS */
+	struct usb_ep		*ep_status;
+	struct usb_ep		*ep_cmd;
+	struct uas_stream	stream[UASP_SS_EP_COMP_NUM_STREAMS];
+
+	/* BOT */
+	struct bot_status	bot_status;
+	struct usb_request	*bot_req_in;
+	struct usb_request	*bot_req_out;
+};
+
+extern struct usbg_tpg *the_only_tpg_I_currently_have;
+
+#endif
diff --git a/drivers/usb/gadget/u_ether.c b/drivers/usb/gadget/u_ether.c
index 29c854bbca4..47cf48b51c9 100644
--- a/drivers/usb/gadget/u_ether.c
+++ b/drivers/usb/gadget/u_ether.c
@@ -744,10 +744,11 @@ static struct device_type gadget_type = {
 };
 
 /**
- * gether_setup - initialize one ethernet-over-usb link
+ * gether_setup_name - initialize one ethernet-over-usb link
  * @g: gadget to associated with these links
  * @ethaddr: NULL, or a buffer in which the ethernet address of the
  *	host side of the link is recorded
+ * @netname: name for network device (for example, "usb")
  * Context: may sleep
  *
  * This sets up the single network link that may be exported by a
@@ -756,7 +757,8 @@ static struct device_type gadget_type = {
  *
  * Returns negative errno, or zero on success
  */
-int gether_setup(struct usb_gadget *g, u8 ethaddr[ETH_ALEN])
+int gether_setup_name(struct usb_gadget *g, u8 ethaddr[ETH_ALEN],
+		const char *netname)
 {
 	struct eth_dev		*dev;
 	struct net_device	*net;
@@ -780,7 +782,7 @@ int gether_setup(struct usb_gadget *g, u8 ethaddr[ETH_ALEN])
 
 	/* network device setup */
 	dev->net = net;
-	strcpy(net->name, "usb%d");
+	snprintf(net->name, sizeof(net->name), "%s%%d", netname);
 
 	if (get_ether_addr(dev_addr, net->dev_addr))
 		dev_warn(&g->dev,
diff --git a/drivers/usb/gadget/u_ether.h b/drivers/usb/gadget/u_ether.h
index 8012357e98a..6f4a1623d85 100644
--- a/drivers/usb/gadget/u_ether.h
+++ b/drivers/usb/gadget/u_ether.h
@@ -69,9 +69,28 @@ struct gether {
 			|USB_CDC_PACKET_TYPE_PROMISCUOUS \
 			|USB_CDC_PACKET_TYPE_DIRECTED)
 
+/* variant of gether_setup that allows customizing network device name */
+int gether_setup_name(struct usb_gadget *g, u8 ethaddr[ETH_ALEN],
+		const char *netname);
 
 /* netdev setup/teardown as directed by the gadget driver */
-int gether_setup(struct usb_gadget *g, u8 ethaddr[ETH_ALEN]);
+/* gether_setup - initialize one ethernet-over-usb link
+ * @g: gadget to associated with these links
+ * @ethaddr: NULL, or a buffer in which the ethernet address of the
+ *	host side of the link is recorded
+ * Context: may sleep
+ *
+ * This sets up the single network link that may be exported by a
+ * gadget driver using this framework.  The link layer addresses are
+ * set up using module parameters.
+ *
+ * Returns negative errno, or zero on success
+ */
+static inline int gether_setup(struct usb_gadget *g, u8 ethaddr[ETH_ALEN])
+{
+	return gether_setup_name(g, ethaddr, "usb");
+}
+
 void gether_cleanup(void);
 
 /* connect/disconnect is handled by individual functions */
@@ -99,16 +118,37 @@ int eem_bind_config(struct usb_configuration *c);
 
 #ifdef USB_ETH_RNDIS
 
-int rndis_bind_config(struct usb_configuration *c, u8 ethaddr[ETH_ALEN]);
+int rndis_bind_config_vendor(struct usb_configuration *c, u8 ethaddr[ETH_ALEN],
+				u32 vendorID, const char *manufacturer);
 
 #else
 
 static inline int
-rndis_bind_config(struct usb_configuration *c, u8 ethaddr[ETH_ALEN])
+rndis_bind_config_vendor(struct usb_configuration *c, u8 ethaddr[ETH_ALEN],
+				u32 vendorID, const char *manufacturer)
 {
 	return 0;
 }
 
 #endif
 
+/**
+ * rndis_bind_config - add RNDIS network link to a configuration
+ * @c: the configuration to support the network link
+ * @ethaddr: a buffer in which the ethernet address of the host side
+ *	side of the link was recorded
+ * Context: single threaded during gadget setup
+ *
+ * Returns zero on success, else negative errno.
+ *
+ * Caller must have called @gether_setup().  Caller is also responsible
+ * for calling @gether_cleanup() before module unload.
+ */
+static inline int rndis_bind_config(struct usb_configuration *c,
+				    u8 ethaddr[ETH_ALEN])
+{
+	return rndis_bind_config_vendor(c, ethaddr, 0, NULL);
+}
+
+
 #endif /* __U_ETHER_H */
diff --git a/drivers/usb/gadget/u_serial.c b/drivers/usb/gadget/u_serial.c
index 6c23938d271..5b3f5fffea9 100644
--- a/drivers/usb/gadget/u_serial.c
+++ b/drivers/usb/gadget/u_serial.c
@@ -94,17 +94,14 @@ struct gs_buf {
  * (and thus for each /dev/ node).
  */
 struct gs_port {
+	struct tty_port		port;
 	spinlock_t		port_lock;	/* guard port_* access */
 
 	struct gserial		*port_usb;
-	struct tty_struct	*port_tty;
 
-	unsigned		open_count;
 	bool			openclose;	/* open/close in progress */
 	u8			port_num;
 
-	wait_queue_head_t	close_wait;	/* wait for last close */
-
 	struct list_head	read_pool;
 	int read_started;
 	int read_allocated;
@@ -412,8 +409,8 @@ __acquires(&port->port_lock)
 			break;
 	}
 
-	if (do_tty_wake && port->port_tty)
-		tty_wakeup(port->port_tty);
+	if (do_tty_wake && port->port.tty)
+		tty_wakeup(port->port.tty);
 	return status;
 }
 
@@ -435,7 +432,7 @@ __acquires(&port->port_lock)
 		struct tty_struct	*tty;
 
 		/* no more rx if closed */
-		tty = port->port_tty;
+		tty = port->port.tty;
 		if (!tty)
 			break;
 
@@ -488,7 +485,7 @@ static void gs_rx_push(unsigned long _port)
 
 	/* hand any queued data to the tty */
 	spin_lock_irq(&port->port_lock);
-	tty = port->port_tty;
+	tty = port->port.tty;
 	while (!list_empty(queue)) {
 		struct usb_request	*req;
 
@@ -699,7 +696,7 @@ static int gs_start_io(struct gs_port *port)
 
 	/* unblock any pending writes into our circular buffer */
 	if (started) {
-		tty_wakeup(port->port_tty);
+		tty_wakeup(port->port.tty);
 	} else {
 		gs_free_requests(ep, head, &port->read_allocated);
 		gs_free_requests(port->port_usb->in, &port->write_pool,
@@ -734,9 +731,9 @@ static int gs_open(struct tty_struct *tty, struct file *file)
 			spin_lock_irq(&port->port_lock);
 
 			/* already open?  Great. */
-			if (port->open_count) {
+			if (port->port.count) {
 				status = 0;
-				port->open_count++;
+				port->port.count++;
 
 			/* currently opening/closing? wait ... */
 			} else if (port->openclose) {
@@ -793,9 +790,9 @@ static int gs_open(struct tty_struct *tty, struct file *file)
 	/* REVISIT maybe wait for "carrier detect" */
 
 	tty->driver_data = port;
-	port->port_tty = tty;
+	port->port.tty = tty;
 
-	port->open_count = 1;
+	port->port.count = 1;
 	port->openclose = false;
 
 	/* if connected, start the I/O stream */
@@ -837,11 +834,11 @@ static void gs_close(struct tty_struct *tty, struct file *file)
 
 	spin_lock_irq(&port->port_lock);
 
-	if (port->open_count != 1) {
-		if (port->open_count == 0)
+	if (port->port.count != 1) {
+		if (port->port.count == 0)
 			WARN_ON(1);
 		else
-			--port->open_count;
+			--port->port.count;
 		goto exit;
 	}
 
@@ -851,7 +848,7 @@ static void gs_close(struct tty_struct *tty, struct file *file)
 	 * and sleep if necessary
 	 */
 	port->openclose = true;
-	port->open_count = 0;
+	port->port.count = 0;
 
 	gser = port->port_usb;
 	if (gser && gser->disconnect)
@@ -879,14 +876,14 @@ static void gs_close(struct tty_struct *tty, struct file *file)
 		gs_buf_clear(&port->port_write_buf);
 
 	tty->driver_data = NULL;
-	port->port_tty = NULL;
+	port->port.tty = NULL;
 
 	port->openclose = false;
 
 	pr_debug("gs_close: ttyGS%d (%p,%p) done!\n",
 			port->port_num, tty, file);
 
-	wake_up_interruptible(&port->close_wait);
+	wake_up_interruptible(&port->port.close_wait);
 exit:
 	spin_unlock_irq(&port->port_lock);
 }
@@ -917,7 +914,7 @@ static int gs_put_char(struct tty_struct *tty, unsigned char ch)
 	unsigned long	flags;
 	int		status;
 
-	pr_vdebug("gs_put_char: (%d,%p) char=0x%x, called from %p\n",
+	pr_vdebug("gs_put_char: (%d,%p) char=0x%x, called from %pf\n",
 		port->port_num, tty, ch, __builtin_return_address(0));
 
 	spin_lock_irqsave(&port->port_lock, flags);
@@ -1025,7 +1022,7 @@ static const struct tty_operations gs_tty_ops = {
 
 static struct tty_driver *gs_tty_driver;
 
-static int __init
+static int
 gs_port_alloc(unsigned port_num, struct usb_cdc_line_coding *coding)
 {
 	struct gs_port	*port;
@@ -1034,8 +1031,8 @@ gs_port_alloc(unsigned port_num, struct usb_cdc_line_coding *coding)
 	if (port == NULL)
 		return -ENOMEM;
 
+	tty_port_init(&port->port);
 	spin_lock_init(&port->port_lock);
-	init_waitqueue_head(&port->close_wait);
 	init_waitqueue_head(&port->drain_wait);
 
 	tasklet_init(&port->push, gs_rx_push, (unsigned long) port);
@@ -1071,7 +1068,7 @@ gs_port_alloc(unsigned port_num, struct usb_cdc_line_coding *coding)
  *
  * Returns negative errno or zero.
  */
-int __init gserial_setup(struct usb_gadget *g, unsigned count)
+int gserial_setup(struct usb_gadget *g, unsigned count)
 {
 	unsigned			i;
 	struct usb_cdc_line_coding	coding;
@@ -1155,7 +1152,7 @@ static int gs_closed(struct gs_port *port)
 	int cond;
 
 	spin_lock_irq(&port->port_lock);
-	cond = (port->open_count == 0) && !port->openclose;
+	cond = (port->port.count == 0) && !port->openclose;
 	spin_unlock_irq(&port->port_lock);
 	return cond;
 }
@@ -1194,7 +1191,7 @@ void gserial_cleanup(void)
 		tasklet_kill(&port->push);
 
 		/* wait for old opens to finish */
-		wait_event(port->close_wait, gs_closed(port));
+		wait_event(port->port.close_wait, gs_closed(port));
 
 		WARN_ON(port->port_usb != NULL);
 
@@ -1268,7 +1265,7 @@ int gserial_connect(struct gserial *gser, u8 port_num)
 	/* if it's already open, start I/O ... and notify the serial
 	 * protocol about open/close status (connect/disconnect).
 	 */
-	if (port->open_count) {
+	if (port->port.count) {
 		pr_debug("gserial_connect: start ttyGS%d\n", port->port_num);
 		gs_start_io(port);
 		if (gser->connect)
@@ -1315,10 +1312,10 @@ void gserial_disconnect(struct gserial *gser)
 
 	port->port_usb = NULL;
 	gser->ioport = NULL;
-	if (port->open_count > 0 || port->openclose) {
+	if (port->port.count > 0 || port->openclose) {
 		wake_up_interruptible(&port->drain_wait);
-		if (port->port_tty)
-			tty_hangup(port->port_tty);
+		if (port->port.tty)
+			tty_hangup(port->port.tty);
 	}
 	spin_unlock_irqrestore(&port->port_lock, flags);
 
@@ -1331,7 +1328,7 @@ void gserial_disconnect(struct gserial *gser)
 
 	/* finally, free any unused/unusable I/O buffers */
 	spin_lock_irqsave(&port->port_lock, flags);
-	if (port->open_count == 0 && !port->openclose)
+	if (port->port.count == 0 && !port->openclose)
 		gs_buf_free(&port->port_write_buf);
 	gs_free_requests(gser->out, &port->read_pool, NULL);
 	gs_free_requests(gser->out, &port->read_queue, NULL);
diff --git a/drivers/usb/gadget/uvc_queue.c b/drivers/usb/gadget/uvc_queue.c
index 0cdf89d32a1..104ae9c8125 100644
--- a/drivers/usb/gadget/uvc_queue.c
+++ b/drivers/usb/gadget/uvc_queue.c
@@ -543,7 +543,7 @@ done:
 	return ret;
 }
 
-/* called with queue->irqlock held.. */
+/* called with &queue_irqlock held.. */
 static struct uvc_buffer *
 uvc_queue_next_buffer(struct uvc_video_queue *queue, struct uvc_buffer *buf)
 {
diff --git a/drivers/usb/gadget/uvc_v4l2.c b/drivers/usb/gadget/uvc_v4l2.c
index 54d7ca559cb..2ca9386d655 100644
--- a/drivers/usb/gadget/uvc_v4l2.c
+++ b/drivers/usb/gadget/uvc_v4l2.c
@@ -296,7 +296,7 @@ uvc_v4l2_do_ioctl(struct file *file, unsigned int cmd, void *arg)
 		if (sub->type < UVC_EVENT_FIRST || sub->type > UVC_EVENT_LAST)
 			return -EINVAL;
 
-		return v4l2_event_subscribe(&handle->vfh, arg, 2);
+		return v4l2_event_subscribe(&handle->vfh, arg, 2, NULL);
 	}
 
 	case VIDIOC_UNSUBSCRIBE_EVENT:
diff --git a/drivers/usb/gadget/zero.c b/drivers/usb/gadget/zero.c
index 31d34832907..12ad516ada7 100644
--- a/drivers/usb/gadget/zero.c
+++ b/drivers/usb/gadget/zero.c
@@ -72,7 +72,7 @@
 
 static const char longname[] = "Gadget Zero";
 
-unsigned buflen = 4096;
+unsigned buflen = 4096;		/* only used for bulk endpoints */
 module_param(buflen, uint, 0);
 
 /*
@@ -170,14 +170,17 @@ static struct usb_gadget_strings *dev_strings[] = {
 
 /*-------------------------------------------------------------------------*/
 
-struct usb_request *alloc_ep_req(struct usb_ep *ep)
+struct usb_request *alloc_ep_req(struct usb_ep *ep, int len)
 {
 	struct usb_request	*req;
 
 	req = usb_ep_alloc_request(ep, GFP_ATOMIC);
 	if (req) {
-		req->length = buflen;
-		req->buf = kmalloc(buflen, GFP_ATOMIC);
+		if (len)
+			req->length = len;
+		else
+			req->length = buflen;
+		req->buf = kmalloc(req->length, GFP_ATOMIC);
 		if (!req->buf) {
 			usb_ep_free_request(ep, req);
 			req = NULL;
@@ -206,10 +209,15 @@ static void disable_ep(struct usb_composite_dev *cdev, struct usb_ep *ep)
 }
 
 void disable_endpoints(struct usb_composite_dev *cdev,
-		struct usb_ep *in, struct usb_ep *out)
+		struct usb_ep *in, struct usb_ep *out,
+		struct usb_ep *iso_in, struct usb_ep *iso_out)
 {
 	disable_ep(cdev, in);
 	disable_ep(cdev, out);
+	if (iso_in)
+		disable_ep(cdev, iso_in);
+	if (iso_out)
+		disable_ep(cdev, iso_out);
 }
 
 /*-------------------------------------------------------------------------*/
@@ -311,7 +319,6 @@ static int __init zero_bind(struct usb_composite_dev *cdev)
 		device_desc.bcdDevice = cpu_to_le16(0x9999);
 	}
 
-
 	INFO(cdev, "%s, version: " DRIVER_VERSION "\n", longname);
 
 	snprintf(manufacturer, sizeof manufacturer, "%s %s with %s",
diff --git a/drivers/usb/host/Kconfig b/drivers/usb/host/Kconfig
index f788eb86707..83e58df29fe 100644
--- a/drivers/usb/host/Kconfig
+++ b/drivers/usb/host/Kconfig
@@ -65,7 +65,7 @@ config USB_EHCI_HCD
 
 config USB_EHCI_ROOT_HUB_TT
 	bool "Root Hub Transaction Translators"
-	depends on USB_EHCI_HCD
+	depends on USB_EHCI_HCD || USB_CHIPIDEA_HOST
 	---help---
 	  Some EHCI chips have vendor-specific extensions to integrate
 	  transaction translators, so that no OHCI or UHCI companion
@@ -77,7 +77,7 @@ config USB_EHCI_ROOT_HUB_TT
 
 config USB_EHCI_TT_NEWSCHED
 	bool "Improved Transaction Translator scheduling"
-	depends on USB_EHCI_HCD
+	depends on USB_EHCI_HCD || USB_CHIPIDEA_HOST
 	default y
 	---help---
 	  This changes the periodic scheduling code to fill more of the low
@@ -110,13 +110,14 @@ config USB_EHCI_BIG_ENDIAN_MMIO
 	depends on USB_EHCI_HCD && (PPC_CELLEB || PPC_PS3 || 440EPX || \
 				    ARCH_IXP4XX || XPS_USB_HCD_XILINX || \
 				    PPC_MPC512x || CPU_CAVIUM_OCTEON || \
-				    PMC_MSP || SPARC_LEON)
+				    PMC_MSP || SPARC_LEON || MIPS_SEAD3)
 	default y
 
 config USB_EHCI_BIG_ENDIAN_DESC
 	bool
 	depends on USB_EHCI_HCD && (440EPX || ARCH_IXP4XX || XPS_USB_HCD_XILINX || \
-				    PPC_MPC512x || PMC_MSP || SPARC_LEON)
+				    PPC_MPC512x || PMC_MSP || SPARC_LEON || \
+				    MIPS_SEAD3)
 	default y
 
 config XPS_USB_HCD_XILINX
@@ -152,7 +153,7 @@ config USB_EHCI_HCD_OMAP
 	bool "EHCI support for OMAP3 and later chips"
 	depends on USB_EHCI_HCD && ARCH_OMAP
 	default y
-	--- help ---
+	---help---
 	  Enables support for the on-chip EHCI controller on
 	  OMAP3 and later chips.
 
@@ -291,6 +292,7 @@ config USB_OHCI_HCD
 	depends on USB && USB_ARCH_HAS_OHCI
 	select ISP1301_OMAP if MACH_OMAP_H2 || MACH_OMAP_H3
 	select USB_OTG_UTILS if ARCH_OMAP
+	select USB_ISP1301 if ARCH_LPC32XX || ARCH_PNX4008
 	---help---
 	  The Open Host Controller Interface (OHCI) is a standard for accessing
 	  USB 1.1 host controller hardware.  It does more in hardware than Intel's
@@ -373,10 +375,15 @@ config USB_OHCI_HCD_PCI
 	  If unsure, say Y.
 
 config USB_OHCI_HCD_SSB
-	bool "OHCI support for Broadcom SSB OHCI core"
+	bool "OHCI support for Broadcom SSB OHCI core (DEPRECATED)"
 	depends on USB_OHCI_HCD && (SSB = y || SSB = USB_OHCI_HCD) && EXPERIMENTAL
+	select USB_HCD_SSB
+	select USB_OHCI_HCD_PLATFORM
 	default n
 	---help---
+	  This option is deprecated now and the driver was removed, use
+	  USB_HCD_SSB and USB_OHCI_HCD_PLATFORM instead.
+
 	  Support for the Sonics Silicon Backplane (SSB) attached
 	  Broadcom USB OHCI core.
 
@@ -638,3 +645,27 @@ config USB_OCTEON_OHCI
 config USB_OCTEON2_COMMON
 	bool
 	default y if USB_OCTEON_EHCI || USB_OCTEON_OHCI
+
+config USB_HCD_BCMA
+	tristate "BCMA usb host driver"
+	depends on BCMA && EXPERIMENTAL
+	select USB_OHCI_HCD_PLATFORM if USB_OHCI_HCD
+	select USB_EHCI_HCD_PLATFORM if USB_EHCI_HCD
+	help
+	  Enbale support for the EHCI and OCHI host controller on an bcma bus.
+	  It converts the bcma driver into two platform device drivers
+	  for ehci and ohci.
+
+	  If unsure, say N.
+
+config USB_HCD_SSB
+	tristate "SSB usb host driver"
+	depends on SSB && EXPERIMENTAL
+	select USB_OHCI_HCD_PLATFORM if USB_OHCI_HCD
+	select USB_EHCI_HCD_PLATFORM if USB_EHCI_HCD
+	help
+	  Enbale support for the EHCI and OCHI host controller on an bcma bus.
+	  It converts the bcma driver into two platform device drivers
+	  for ehci and ohci.
+
+	  If unsure, say N.
diff --git a/drivers/usb/host/Makefile b/drivers/usb/host/Makefile
index 0982bcc140b..9e0a89ced15 100644
--- a/drivers/usb/host/Makefile
+++ b/drivers/usb/host/Makefile
@@ -41,3 +41,5 @@ obj-$(CONFIG_USB_IMX21_HCD)	+= imx21-hcd.o
 obj-$(CONFIG_USB_FSL_MPH_DR_OF)	+= fsl-mph-dr-of.o
 obj-$(CONFIG_USB_OCTEON2_COMMON) += octeon2-common.o
 obj-$(CONFIG_MIPS_ALCHEMY)	+= alchemy-common.o
+obj-$(CONFIG_USB_HCD_BCMA)	+= bcma-hcd.o
+obj-$(CONFIG_USB_HCD_SSB)	+= ssb-hcd.o
diff --git a/drivers/usb/host/bcma-hcd.c b/drivers/usb/host/bcma-hcd.c
new file mode 100644
index 00000000000..443da21d73c
--- /dev/null
+++ b/drivers/usb/host/bcma-hcd.c
@@ -0,0 +1,335 @@
+/*
+ * Broadcom specific Advanced Microcontroller Bus
+ * Broadcom USB-core driver (BCMA bus glue)
+ *
+ * Copyright 2011-2012 Hauke Mehrtens <hauke@hauke-m.de>
+ *
+ * Based on ssb-ohci driver
+ * Copyright 2007 Michael Buesch <m@bues.ch>
+ *
+ * Derived from the OHCI-PCI driver
+ * Copyright 1999 Roman Weissgaerber
+ * Copyright 2000-2002 David Brownell
+ * Copyright 1999 Linus Torvalds
+ * Copyright 1999 Gregory P. Smith
+ *
+ * Derived from the USBcore related parts of Broadcom-SB
+ * Copyright 2005-2011 Broadcom Corporation
+ *
+ * Licensed under the GNU/GPL. See COPYING for details.
+ */
+#include <linux/bcma/bcma.h>
+#include <linux/delay.h>
+#include <linux/platform_device.h>
+#include <linux/module.h>
+#include <linux/slab.h>
+#include <linux/usb/ehci_pdriver.h>
+#include <linux/usb/ohci_pdriver.h>
+
+MODULE_AUTHOR("Hauke Mehrtens");
+MODULE_DESCRIPTION("Common USB driver for BCMA Bus");
+MODULE_LICENSE("GPL");
+
+struct bcma_hcd_device {
+	struct platform_device *ehci_dev;
+	struct platform_device *ohci_dev;
+};
+
+/* Wait for bitmask in a register to get set or cleared.
+ * timeout is in units of ten-microseconds.
+ */
+static int bcma_wait_bits(struct bcma_device *dev, u16 reg, u32 bitmask,
+			  int timeout)
+{
+	int i;
+	u32 val;
+
+	for (i = 0; i < timeout; i++) {
+		val = bcma_read32(dev, reg);
+		if ((val & bitmask) == bitmask)
+			return 0;
+		udelay(10);
+	}
+
+	return -ETIMEDOUT;
+}
+
+static void __devinit bcma_hcd_4716wa(struct bcma_device *dev)
+{
+#ifdef CONFIG_BCMA_DRIVER_MIPS
+	/* Work around for 4716 failures. */
+	if (dev->bus->chipinfo.id == 0x4716) {
+		u32 tmp;
+
+		tmp = bcma_cpu_clock(&dev->bus->drv_mips);
+		if (tmp >= 480000000)
+			tmp = 0x1846b; /* set CDR to 0x11(fast) */
+		else if (tmp == 453000000)
+			tmp = 0x1046b; /* set CDR to 0x10(slow) */
+		else
+			tmp = 0;
+
+		/* Change Shim mdio control reg to fix host not acking at
+		 * high frequencies
+		 */
+		if (tmp) {
+			bcma_write32(dev, 0x524, 0x1); /* write sel to enable */
+			udelay(500);
+
+			bcma_write32(dev, 0x524, tmp);
+			udelay(500);
+			bcma_write32(dev, 0x524, 0x4ab);
+			udelay(500);
+			bcma_read32(dev, 0x528);
+			bcma_write32(dev, 0x528, 0x80000000);
+		}
+	}
+#endif /* CONFIG_BCMA_DRIVER_MIPS */
+}
+
+/* based on arch/mips/brcm-boards/bcm947xx/pcibios.c */
+static void __devinit bcma_hcd_init_chip(struct bcma_device *dev)
+{
+	u32 tmp;
+
+	/*
+	 * USB 2.0 special considerations:
+	 *
+	 * 1. Since the core supports both OHCI and EHCI functions, it must
+	 *    only be reset once.
+	 *
+	 * 2. In addition to the standard SI reset sequence, the Host Control
+	 *    Register must be programmed to bring the USB core and various
+	 *    phy components out of reset.
+	 */
+	if (!bcma_core_is_enabled(dev)) {
+		bcma_core_enable(dev, 0);
+		mdelay(10);
+		if (dev->id.rev >= 5) {
+			/* Enable Misc PLL */
+			tmp = bcma_read32(dev, 0x1e0);
+			tmp |= 0x100;
+			bcma_write32(dev, 0x1e0, tmp);
+			if (bcma_wait_bits(dev, 0x1e0, 1 << 24, 100))
+				printk(KERN_EMERG "Failed to enable misc PPL!\n");
+
+			/* Take out of resets */
+			bcma_write32(dev, 0x200, 0x4ff);
+			udelay(25);
+			bcma_write32(dev, 0x200, 0x6ff);
+			udelay(25);
+
+			/* Make sure digital and AFE are locked in USB PHY */
+			bcma_write32(dev, 0x524, 0x6b);
+			udelay(50);
+			tmp = bcma_read32(dev, 0x524);
+			udelay(50);
+			bcma_write32(dev, 0x524, 0xab);
+			udelay(50);
+			tmp = bcma_read32(dev, 0x524);
+			udelay(50);
+			bcma_write32(dev, 0x524, 0x2b);
+			udelay(50);
+			tmp = bcma_read32(dev, 0x524);
+			udelay(50);
+			bcma_write32(dev, 0x524, 0x10ab);
+			udelay(50);
+			tmp = bcma_read32(dev, 0x524);
+
+			if (bcma_wait_bits(dev, 0x528, 0xc000, 10000)) {
+				tmp = bcma_read32(dev, 0x528);
+				printk(KERN_EMERG
+				       "USB20H mdio_rddata 0x%08x\n", tmp);
+			}
+			bcma_write32(dev, 0x528, 0x80000000);
+			tmp = bcma_read32(dev, 0x314);
+			udelay(265);
+			bcma_write32(dev, 0x200, 0x7ff);
+			udelay(10);
+
+			/* Take USB and HSIC out of non-driving modes */
+			bcma_write32(dev, 0x510, 0);
+		} else {
+			bcma_write32(dev, 0x200, 0x7ff);
+
+			udelay(1);
+		}
+
+		bcma_hcd_4716wa(dev);
+	}
+}
+
+static const struct usb_ehci_pdata ehci_pdata = {
+};
+
+static const struct usb_ohci_pdata ohci_pdata = {
+};
+
+static struct platform_device * __devinit
+bcma_hcd_create_pdev(struct bcma_device *dev, bool ohci, u32 addr)
+{
+	struct platform_device *hci_dev;
+	struct resource hci_res[2];
+	int ret = -ENOMEM;
+
+	memset(hci_res, 0, sizeof(hci_res));
+
+	hci_res[0].start = addr;
+	hci_res[0].end = hci_res[0].start + 0x1000 - 1;
+	hci_res[0].flags = IORESOURCE_MEM;
+
+	hci_res[1].start = dev->irq;
+	hci_res[1].flags = IORESOURCE_IRQ;
+
+	hci_dev = platform_device_alloc(ohci ? "ohci-platform" :
+					"ehci-platform" , 0);
+	if (!hci_dev)
+		return NULL;
+
+	hci_dev->dev.parent = &dev->dev;
+	hci_dev->dev.dma_mask = &hci_dev->dev.coherent_dma_mask;
+
+	ret = platform_device_add_resources(hci_dev, hci_res,
+					    ARRAY_SIZE(hci_res));
+	if (ret)
+		goto err_alloc;
+	if (ohci)
+		ret = platform_device_add_data(hci_dev, &ohci_pdata,
+					       sizeof(ohci_pdata));
+	else
+		ret = platform_device_add_data(hci_dev, &ehci_pdata,
+					       sizeof(ehci_pdata));
+	if (ret)
+		goto err_alloc;
+	ret = platform_device_add(hci_dev);
+	if (ret)
+		goto err_alloc;
+
+	return hci_dev;
+
+err_alloc:
+	platform_device_put(hci_dev);
+	return ERR_PTR(ret);
+}
+
+static int __devinit bcma_hcd_probe(struct bcma_device *dev)
+{
+	int err;
+	u16 chipid_top;
+	u32 ohci_addr;
+	struct bcma_hcd_device *usb_dev;
+	struct bcma_chipinfo *chipinfo;
+
+	chipinfo = &dev->bus->chipinfo;
+	/* USBcores are only connected on embedded devices. */
+	chipid_top = (chipinfo->id & 0xFF00);
+	if (chipid_top != 0x4700 && chipid_top != 0x5300)
+		return -ENODEV;
+
+	/* TODO: Probably need checks here; is the core connected? */
+
+	if (dma_set_mask(dev->dma_dev, DMA_BIT_MASK(32)) ||
+	    dma_set_coherent_mask(dev->dma_dev, DMA_BIT_MASK(32)))
+		return -EOPNOTSUPP;
+
+	usb_dev = kzalloc(sizeof(struct bcma_hcd_device), GFP_KERNEL);
+	if (!usb_dev)
+		return -ENOMEM;
+
+	bcma_hcd_init_chip(dev);
+
+	/* In AI chips EHCI is addrspace 0, OHCI is 1 */
+	ohci_addr = dev->addr1;
+	if ((chipinfo->id == 0x5357 || chipinfo->id == 0x4749)
+	    && chipinfo->rev == 0)
+		ohci_addr = 0x18009000;
+
+	usb_dev->ohci_dev = bcma_hcd_create_pdev(dev, true, ohci_addr);
+	if (IS_ERR(usb_dev->ohci_dev)) {
+		err = PTR_ERR(usb_dev->ohci_dev);
+		goto err_free_usb_dev;
+	}
+
+	usb_dev->ehci_dev = bcma_hcd_create_pdev(dev, false, dev->addr);
+	if (IS_ERR(usb_dev->ehci_dev)) {
+		err = PTR_ERR(usb_dev->ehci_dev);
+		goto err_unregister_ohci_dev;
+	}
+
+	bcma_set_drvdata(dev, usb_dev);
+	return 0;
+
+err_unregister_ohci_dev:
+	platform_device_unregister(usb_dev->ohci_dev);
+err_free_usb_dev:
+	kfree(usb_dev);
+	return err;
+}
+
+static void __devexit bcma_hcd_remove(struct bcma_device *dev)
+{
+	struct bcma_hcd_device *usb_dev = bcma_get_drvdata(dev);
+	struct platform_device *ohci_dev = usb_dev->ohci_dev;
+	struct platform_device *ehci_dev = usb_dev->ehci_dev;
+
+	if (ohci_dev)
+		platform_device_unregister(ohci_dev);
+	if (ehci_dev)
+		platform_device_unregister(ehci_dev);
+
+	bcma_core_disable(dev, 0);
+}
+
+static void bcma_hcd_shutdown(struct bcma_device *dev)
+{
+	bcma_core_disable(dev, 0);
+}
+
+#ifdef CONFIG_PM
+
+static int bcma_hcd_suspend(struct bcma_device *dev)
+{
+	bcma_core_disable(dev, 0);
+
+	return 0;
+}
+
+static int bcma_hcd_resume(struct bcma_device *dev)
+{
+	bcma_core_enable(dev, 0);
+
+	return 0;
+}
+
+#else /* !CONFIG_PM */
+#define bcma_hcd_suspend	NULL
+#define bcma_hcd_resume	NULL
+#endif /* CONFIG_PM */
+
+static const struct bcma_device_id bcma_hcd_table[] __devinitconst = {
+	BCMA_CORE(BCMA_MANUF_BCM, BCMA_CORE_USB20_HOST, BCMA_ANY_REV, BCMA_ANY_CLASS),
+	BCMA_CORETABLE_END
+};
+MODULE_DEVICE_TABLE(bcma, bcma_hcd_table);
+
+static struct bcma_driver bcma_hcd_driver = {
+	.name		= KBUILD_MODNAME,
+	.id_table	= bcma_hcd_table,
+	.probe		= bcma_hcd_probe,
+	.remove		= __devexit_p(bcma_hcd_remove),
+	.shutdown	= bcma_hcd_shutdown,
+	.suspend	= bcma_hcd_suspend,
+	.resume		= bcma_hcd_resume,
+};
+
+static int __init bcma_hcd_init(void)
+{
+	return bcma_driver_register(&bcma_hcd_driver);
+}
+module_init(bcma_hcd_init);
+
+static void __exit bcma_hcd_exit(void)
+{
+	bcma_driver_unregister(&bcma_hcd_driver);
+}
+module_exit(bcma_hcd_exit);
diff --git a/drivers/usb/host/ehci-dbg.c b/drivers/usb/host/ehci-dbg.c
index 680e1a31fb8..7561966fbdc 100644
--- a/drivers/usb/host/ehci-dbg.c
+++ b/drivers/usb/host/ehci-dbg.c
@@ -1025,10 +1025,8 @@ static ssize_t debug_lpm_write(struct file *file, const char __user *user_buf,
 		if (strict_strtoul(buf + 5, 16, &hird))
 			return -EINVAL;
 		printk(KERN_INFO "setting hird %s %lu\n", buf + 6, hird);
-		temp = ehci_readl(ehci, &ehci->regs->command);
-		temp &= ~CMD_HIRD;
-		temp |= hird << 24;
-		ehci_writel(ehci, temp, &ehci->regs->command);
+		ehci->command = (ehci->command & ~CMD_HIRD) | (hird << 24);
+		ehci_writel(ehci, ehci->command, &ehci->regs->command);
 	} else if (strncmp(buf, "disable", 7) == 0) {
 		if (strict_strtoul(buf + 8, 10, &port))
 			return -EINVAL;
diff --git a/drivers/usb/host/ehci-fsl.c b/drivers/usb/host/ehci-fsl.c
index d0a84bd3f3e..43362577b54 100644
--- a/drivers/usb/host/ehci-fsl.c
+++ b/drivers/usb/host/ehci-fsl.c
@@ -1,6 +1,6 @@
 /*
  * Copyright 2005-2009 MontaVista Software, Inc.
- * Copyright 2008      Freescale Semiconductor, Inc.
+ * Copyright 2008,2012      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 as published by the
@@ -150,8 +150,7 @@ static int usb_hcd_fsl_probe(const struct hc_driver *driver,
 			retval = otg_set_host(ehci->transceiver->otg,
 					      &ehci_to_hcd(ehci)->self);
 			if (retval) {
-				if (ehci->transceiver)
-					put_device(ehci->transceiver->dev);
+				usb_put_transceiver(ehci->transceiver);
 				goto err4;
 			}
 		} else {
@@ -195,7 +194,7 @@ static void usb_hcd_fsl_remove(struct usb_hcd *hcd,
 
 	if (ehci->transceiver) {
 		otg_set_host(ehci->transceiver->otg, NULL);
-		put_device(ehci->transceiver->dev);
+		usb_put_transceiver(ehci->transceiver);
 	}
 
 	usb_remove_hcd(hcd);
@@ -211,22 +210,32 @@ static void usb_hcd_fsl_remove(struct usb_hcd *hcd,
 	usb_put_hcd(hcd);
 }
 
-static void ehci_fsl_setup_phy(struct ehci_hcd *ehci,
+static void ehci_fsl_setup_phy(struct usb_hcd *hcd,
 			       enum fsl_usb2_phy_modes phy_mode,
 			       unsigned int port_offset)
 {
-	u32 portsc;
-	struct usb_hcd *hcd = ehci_to_hcd(ehci);
+	u32 portsc, temp;
+	struct ehci_hcd *ehci = hcd_to_ehci(hcd);
 	void __iomem *non_ehci = hcd->regs;
-	struct fsl_usb2_platform_data *pdata;
+	struct device *dev = hcd->self.controller;
+	struct fsl_usb2_platform_data *pdata = dev->platform_data;
 
-	pdata = hcd->self.controller->platform_data;
+	if (pdata->controller_ver < 0) {
+		dev_warn(hcd->self.controller, "Could not get controller version\n");
+		return;
+	}
 
 	portsc = ehci_readl(ehci, &ehci->regs->port_status[port_offset]);
 	portsc &= ~(PORT_PTS_MSK | PORT_PTS_PTW);
 
 	switch (phy_mode) {
 	case FSL_USB2_PHY_ULPI:
+		if (pdata->controller_ver) {
+			/* controller version 1.6 or above */
+			temp = in_be32(non_ehci + FSL_SOC_USB_CTRL);
+			out_be32(non_ehci + FSL_SOC_USB_CTRL, temp |
+				USB_CTRL_USB_EN | ULPI_PHY_CLK_SEL);
+		}
 		portsc |= PORT_PTS_ULPI;
 		break;
 	case FSL_USB2_PHY_SERIAL:
@@ -236,6 +245,14 @@ static void ehci_fsl_setup_phy(struct ehci_hcd *ehci,
 		portsc |= PORT_PTS_PTW;
 		/* fall through */
 	case FSL_USB2_PHY_UTMI:
+		if (pdata->controller_ver) {
+			/* controller version 1.6 or above */
+			temp = in_be32(non_ehci + FSL_SOC_USB_CTRL);
+			out_be32(non_ehci + FSL_SOC_USB_CTRL, temp |
+				UTMI_PHY_EN | USB_CTRL_USB_EN);
+			mdelay(FSL_UTMI_PHY_DLY);  /* Delay for UTMI PHY CLK to
+						become stable - 10ms*/
+		}
 		/* enable UTMI PHY */
 		if (pdata->have_sysif_regs)
 			setbits32(non_ehci + FSL_SOC_USB_CTRL,
@@ -276,7 +293,7 @@ static void ehci_fsl_usb_setup(struct ehci_hcd *ehci)
 
 	if ((pdata->operating_mode == FSL_USB2_DR_HOST) ||
 			(pdata->operating_mode == FSL_USB2_DR_OTG))
-		ehci_fsl_setup_phy(ehci, pdata->phy_mode, 0);
+		ehci_fsl_setup_phy(hcd, pdata->phy_mode, 0);
 
 	if (pdata->operating_mode == FSL_USB2_MPH_HOST) {
 		unsigned int chip, rev, svr;
@@ -290,9 +307,9 @@ static void ehci_fsl_usb_setup(struct ehci_hcd *ehci)
 			ehci->has_fsl_port_bug = 1;
 
 		if (pdata->port_enables & FSL_USB2_PORT0_ENABLED)
-			ehci_fsl_setup_phy(ehci, pdata->phy_mode, 0);
+			ehci_fsl_setup_phy(hcd, pdata->phy_mode, 0);
 		if (pdata->port_enables & FSL_USB2_PORT1_ENABLED)
-			ehci_fsl_setup_phy(ehci, pdata->phy_mode, 1);
+			ehci_fsl_setup_phy(hcd, pdata->phy_mode, 1);
 	}
 
 	if (pdata->have_sysif_regs) {
diff --git a/drivers/usb/host/ehci-fsl.h b/drivers/usb/host/ehci-fsl.h
index 863fb0c080d..88403684d10 100644
--- a/drivers/usb/host/ehci-fsl.h
+++ b/drivers/usb/host/ehci-fsl.h
@@ -1,4 +1,4 @@
-/* Copyright (C) 2005-2010 Freescale Semiconductor, Inc.
+/* Copyright (C) 2005-2010,2012 Freescale Semiconductor, Inc.
  * Copyright (c) 2005 MontaVista Software
  *
  * This program is free software; you can redistribute  it and/or modify it
@@ -50,4 +50,15 @@
 #define CTRL_UTMI_PHY_EN	(1<<9)
 #define CTRL_PHY_CLK_VALID	(1 << 17)
 #define SNOOP_SIZE_2GB		0x1e
+
+/* control Register Bit Masks */
+#define ULPI_INT_EN             (1<<0)
+#define WU_INT_EN               (1<<1)
+#define USB_CTRL_USB_EN         (1<<2)
+#define LINE_STATE_FILTER__EN   (1<<3)
+#define KEEP_OTG_ON             (1<<4)
+#define OTG_PORT                (1<<5)
+#define PLL_RESET               (1<<8)
+#define UTMI_PHY_EN             (1<<9)
+#define ULPI_PHY_CLK_SEL        (1<<10)
 #endif				/* _EHCI_FSL_H */
diff --git a/drivers/usb/host/ehci-hcd.c b/drivers/usb/host/ehci-hcd.c
index 4a3bc5b7a06..b100f5f9f4b 100644
--- a/drivers/usb/host/ehci-hcd.c
+++ b/drivers/usb/host/ehci-hcd.c
@@ -226,8 +226,13 @@ static int ehci_halt (struct ehci_hcd *ehci)
 	if ((temp & STS_HALT) != 0)
 		return 0;
 
+	/*
+	 * This routine gets called during probe before ehci->command
+	 * has been initialized, so we can't rely on its value.
+	 */
+	ehci->command &= ~CMD_RUN;
 	temp = ehci_readl(ehci, &ehci->regs->command);
-	temp &= ~CMD_RUN;
+	temp &= ~(CMD_RUN | CMD_IAAD);
 	ehci_writel(ehci, temp, &ehci->regs->command);
 	return handshake (ehci, &ehci->regs->status,
 			  STS_HALT, STS_HALT, 16 * 125);
@@ -363,16 +368,14 @@ static void ehci_quiesce (struct ehci_hcd *ehci)
 #endif
 
 	/* wait for any schedule enables/disables to take effect */
-	temp = ehci_readl(ehci, &ehci->regs->command) << 10;
-	temp &= STS_ASS | STS_PSS;
+	temp = (ehci->command << 10) & (STS_ASS | STS_PSS);
 	if (handshake_on_error_set_halt(ehci, &ehci->regs->status,
 					STS_ASS | STS_PSS, temp, 16 * 125))
 		return;
 
 	/* then disable anything that's still active */
-	temp = ehci_readl(ehci, &ehci->regs->command);
-	temp &= ~(CMD_ASE | CMD_IAAD | CMD_PSE);
-	ehci_writel(ehci, temp, &ehci->regs->command);
+	ehci->command &= ~(CMD_ASE | CMD_PSE);
+	ehci_writel(ehci, ehci->command, &ehci->regs->command);
 
 	/* hardware can take 16 microframes to turn off ... */
 	handshake_on_error_set_halt(ehci, &ehci->regs->status,
@@ -417,9 +420,6 @@ static void ehci_iaa_watchdog(unsigned long param)
 		 * CMD_IAAD when it sets STS_IAA.)
 		 */
 		cmd = ehci_readl(ehci, &ehci->regs->command);
-		if (cmd & CMD_IAAD)
-			ehci_writel(ehci, cmd & ~CMD_IAAD,
-					&ehci->regs->command);
 
 		/* If IAA is set here it either legitimately triggered
 		 * before we cleared IAAD above (but _way_ late, so we'll
@@ -894,11 +894,8 @@ static irqreturn_t ehci_irq (struct usb_hcd *hcd)
 	/* complete the unlinking of some qh [4.15.2.3] */
 	if (status & STS_IAA) {
 		/* guard against (alleged) silicon errata */
-		if (cmd & CMD_IAAD) {
-			ehci_writel(ehci, cmd & ~CMD_IAAD,
-					&ehci->regs->command);
+		if (cmd & CMD_IAAD)
 			ehci_dbg(ehci, "IAA with IAAD still set?\n");
-		}
 		if (ehci->reclaim) {
 			COUNT(ehci->stats.reclaim);
 			end_unlink_async(ehci);
@@ -1248,6 +1245,13 @@ static int ehci_get_frame (struct usb_hcd *hcd)
 }
 
 /*-------------------------------------------------------------------------*/
+/*
+ * The EHCI in ChipIdea HDRC cannot be a separate module or device,
+ * because its registers (and irq) are shared between host/gadget/otg
+ * functions  and in order to facilitate role switching we cannot
+ * give the ehci driver exclusive access to those.
+ */
+#ifndef CHIPIDEA_EHCI
 
 MODULE_DESCRIPTION(DRIVER_DESC);
 MODULE_AUTHOR (DRIVER_AUTHOR);
@@ -1378,6 +1382,11 @@ MODULE_LICENSE ("GPL");
 #define PLATFORM_DRIVER		ehci_ls1x_driver
 #endif
 
+#ifdef CONFIG_MIPS_SEAD3
+#include "ehci-sead3.c"
+#define	PLATFORM_DRIVER		ehci_hcd_sead3_driver
+#endif
+
 #ifdef CONFIG_USB_EHCI_HCD_PLATFORM
 #include "ehci-platform.c"
 #define PLATFORM_DRIVER		ehci_platform_driver
@@ -1501,3 +1510,4 @@ static void __exit ehci_hcd_cleanup(void)
 }
 module_exit(ehci_hcd_cleanup);
 
+#endif /* CHIPIDEA_EHCI */
diff --git a/drivers/usb/host/ehci-hub.c b/drivers/usb/host/ehci-hub.c
index 38fe0762315..fc9e7cc6ac9 100644
--- a/drivers/usb/host/ehci-hub.c
+++ b/drivers/usb/host/ehci-hub.c
@@ -233,7 +233,6 @@ static int ehci_bus_suspend (struct usb_hcd *hcd)
 	/* stop schedules, clean any completed work */
 	if (ehci->rh_state == EHCI_RH_RUNNING)
 		ehci_quiesce (ehci);
-	ehci->command = ehci_readl(ehci, &ehci->regs->command);
 	ehci_work(ehci);
 
 	/* Unlike other USB host controller types, EHCI doesn't have
@@ -374,6 +373,7 @@ static int ehci_bus_resume (struct usb_hcd *hcd)
 	ehci_writel(ehci, (u32) ehci->async->qh_dma, &ehci->regs->async_next);
 
 	/* restore CMD_RUN, framelist size, and irq threshold */
+	ehci->command |= CMD_RUN;
 	ehci_writel(ehci, ehci->command, &ehci->regs->command);
 	ehci->rh_state = EHCI_RH_RUNNING;
 
@@ -531,7 +531,8 @@ static int check_reset_complete (
 		if (ehci->has_amcc_usb23)
 			set_ohci_hcfs(ehci, 1);
 	} else {
-		ehci_dbg (ehci, "port %d high speed\n", index + 1);
+		ehci_dbg(ehci, "port %d reset complete, port enabled\n",
+			index + 1);
 		/* ensure 440EPx ohci controller state is suspended */
 		if (ehci->has_amcc_usb23)
 			set_ohci_hcfs(ehci, 0);
@@ -699,6 +700,7 @@ static int ehci_hub_control (
 			goto error;
 		wIndex--;
 		temp = ehci_readl(ehci, status_reg);
+		temp &= ~PORT_RWC_BITS;
 
 		/*
 		 * Even if OWNER is set, so the port is owned by the
@@ -712,8 +714,7 @@ static int ehci_hub_control (
 			ehci_writel(ehci, temp & ~PORT_PE, status_reg);
 			break;
 		case USB_PORT_FEAT_C_ENABLE:
-			ehci_writel(ehci, (temp & ~PORT_RWC_BITS) | PORT_PEC,
-					status_reg);
+			ehci_writel(ehci, temp | PORT_PEC, status_reg);
 			break;
 		case USB_PORT_FEAT_SUSPEND:
 			if (temp & PORT_RESET)
@@ -742,7 +743,7 @@ static int ehci_hub_control (
 				spin_lock_irqsave(&ehci->lock, flags);
 			}
 			/* resume signaling for 20 msec */
-			temp &= ~(PORT_RWC_BITS | PORT_WAKE_BITS);
+			temp &= ~PORT_WAKE_BITS;
 			ehci_writel(ehci, temp | PORT_RESUME, status_reg);
 			ehci->reset_done[wIndex] = jiffies
 					+ msecs_to_jiffies(20);
@@ -752,9 +753,8 @@ static int ehci_hub_control (
 			break;
 		case USB_PORT_FEAT_POWER:
 			if (HCS_PPC (ehci->hcs_params))
-				ehci_writel(ehci,
-					  temp & ~(PORT_RWC_BITS | PORT_POWER),
-					  status_reg);
+				ehci_writel(ehci, temp & ~PORT_POWER,
+						status_reg);
 			break;
 		case USB_PORT_FEAT_C_CONNECTION:
 			if (ehci->has_lpm) {
@@ -762,12 +762,10 @@ static int ehci_hub_control (
 				temp &= ~PORT_LPM;
 				temp &= ~PORT_DEV_ADDR;
 			}
-			ehci_writel(ehci, (temp & ~PORT_RWC_BITS) | PORT_CSC,
-					status_reg);
+			ehci_writel(ehci, temp | PORT_CSC, status_reg);
 			break;
 		case USB_PORT_FEAT_C_OVER_CURRENT:
-			ehci_writel(ehci, (temp & ~PORT_RWC_BITS) | PORT_OCC,
-					status_reg);
+			ehci_writel(ehci, temp | PORT_OCC, status_reg);
 			break;
 		case USB_PORT_FEAT_C_RESET:
 			/* GetPortStatus clears reset */
diff --git a/drivers/usb/host/ehci-mxc.c b/drivers/usb/host/ehci-mxc.c
index a797d51ecbe..c778ffe4e4e 100644
--- a/drivers/usb/host/ehci-mxc.c
+++ b/drivers/usb/host/ehci-mxc.c
@@ -32,7 +32,7 @@
 #define ULPI_VIEWPORT_OFFSET	0x170
 
 struct ehci_mxc_priv {
-	struct clk *usbclk, *ahbclk, *phy1clk;
+	struct clk *usbclk, *ahbclk, *phyclk;
 	struct usb_hcd *hcd;
 };
 
@@ -166,31 +166,26 @@ static int ehci_mxc_drv_probe(struct platform_device *pdev)
 	}
 
 	/* enable clocks */
-	priv->usbclk = clk_get(dev, "usb");
+	priv->usbclk = clk_get(dev, "ipg");
 	if (IS_ERR(priv->usbclk)) {
 		ret = PTR_ERR(priv->usbclk);
 		goto err_clk;
 	}
-	clk_enable(priv->usbclk);
+	clk_prepare_enable(priv->usbclk);
 
-	if (!cpu_is_mx35() && !cpu_is_mx25()) {
-		priv->ahbclk = clk_get(dev, "usb_ahb");
-		if (IS_ERR(priv->ahbclk)) {
-			ret = PTR_ERR(priv->ahbclk);
-			goto err_clk_ahb;
-		}
-		clk_enable(priv->ahbclk);
+	priv->ahbclk = clk_get(dev, "ahb");
+	if (IS_ERR(priv->ahbclk)) {
+		ret = PTR_ERR(priv->ahbclk);
+		goto err_clk_ahb;
 	}
+	clk_prepare_enable(priv->ahbclk);
 
 	/* "dr" device has its own clock on i.MX51 */
-	if (cpu_is_mx51() && (pdev->id == 0)) {
-		priv->phy1clk = clk_get(dev, "usb_phy1");
-		if (IS_ERR(priv->phy1clk)) {
-			ret = PTR_ERR(priv->phy1clk);
-			goto err_clk_phy;
-		}
-		clk_enable(priv->phy1clk);
-	}
+	priv->phyclk = clk_get(dev, "phy");
+	if (IS_ERR(priv->phyclk))
+		priv->phyclk = NULL;
+	if (priv->phyclk)
+		clk_prepare_enable(priv->phyclk);
 
 
 	/* call platform specific init function */
@@ -265,17 +260,15 @@ err_add:
 	if (pdata && pdata->exit)
 		pdata->exit(pdev);
 err_init:
-	if (priv->phy1clk) {
-		clk_disable(priv->phy1clk);
-		clk_put(priv->phy1clk);
-	}
-err_clk_phy:
-	if (priv->ahbclk) {
-		clk_disable(priv->ahbclk);
-		clk_put(priv->ahbclk);
+	if (priv->phyclk) {
+		clk_disable_unprepare(priv->phyclk);
+		clk_put(priv->phyclk);
 	}
+
+	clk_disable_unprepare(priv->ahbclk);
+	clk_put(priv->ahbclk);
 err_clk_ahb:
-	clk_disable(priv->usbclk);
+	clk_disable_unprepare(priv->usbclk);
 	clk_put(priv->usbclk);
 err_clk:
 	iounmap(hcd->regs);
@@ -307,15 +300,14 @@ static int __exit ehci_mxc_drv_remove(struct platform_device *pdev)
 	usb_put_hcd(hcd);
 	platform_set_drvdata(pdev, NULL);
 
-	clk_disable(priv->usbclk);
+	clk_disable_unprepare(priv->usbclk);
 	clk_put(priv->usbclk);
-	if (priv->ahbclk) {
-		clk_disable(priv->ahbclk);
-		clk_put(priv->ahbclk);
-	}
-	if (priv->phy1clk) {
-		clk_disable(priv->phy1clk);
-		clk_put(priv->phy1clk);
+	clk_disable_unprepare(priv->ahbclk);
+	clk_put(priv->ahbclk);
+
+	if (priv->phyclk) {
+		clk_disable_unprepare(priv->phyclk);
+		clk_put(priv->phyclk);
 	}
 
 	kfree(priv);
diff --git a/drivers/usb/host/ehci-omap.c b/drivers/usb/host/ehci-omap.c
index 5c78f9e7146..a44294d1349 100644
--- a/drivers/usb/host/ehci-omap.c
+++ b/drivers/usb/host/ehci-omap.c
@@ -242,15 +242,6 @@ static int ehci_hcd_omap_probe(struct platform_device *pdev)
 
 	ehci_reset(omap_ehci);
 
-	ret = usb_add_hcd(hcd, irq, IRQF_SHARED);
-	if (ret) {
-		dev_err(dev, "failed to add hcd with err %d\n", ret);
-		goto err_add_hcd;
-	}
-
-	/* root ports should always stay powered */
-	ehci_port_power(omap_ehci, 1);
-
 	if (pdata->phy_reset) {
 		/* Hold the PHY in RESET for enough time till
 		 * PHY is settled and ready
@@ -258,12 +249,21 @@ static int ehci_hcd_omap_probe(struct platform_device *pdev)
 		udelay(10);
 
 		if (gpio_is_valid(pdata->reset_gpio_port[0]))
-			gpio_set_value(pdata->reset_gpio_port[0], 1);
+			gpio_set_value_cansleep(pdata->reset_gpio_port[0], 1);
 
 		if (gpio_is_valid(pdata->reset_gpio_port[1]))
-			gpio_set_value(pdata->reset_gpio_port[1], 1);
+			gpio_set_value_cansleep(pdata->reset_gpio_port[1], 1);
 	}
 
+	ret = usb_add_hcd(hcd, irq, IRQF_SHARED);
+	if (ret) {
+		dev_err(dev, "failed to add hcd with err %d\n", ret);
+		goto err_add_hcd;
+	}
+
+	/* root ports should always stay powered */
+	ehci_port_power(omap_ehci, 1);
+
 	return 0;
 
 err_add_hcd:
diff --git a/drivers/usb/host/ehci-orion.c b/drivers/usb/host/ehci-orion.c
index 6c6a5a3b4ea..82de1073aa5 100644
--- a/drivers/usb/host/ehci-orion.c
+++ b/drivers/usb/host/ehci-orion.c
@@ -12,6 +12,7 @@
 #include <linux/module.h>
 #include <linux/platform_device.h>
 #include <linux/mbus.h>
+#include <linux/clk.h>
 #include <plat/ehci-orion.h>
 
 #define rdl(off)	__raw_readl(hcd->regs + (off))
@@ -198,6 +199,7 @@ static int __devinit ehci_orion_drv_probe(struct platform_device *pdev)
 	struct resource *res;
 	struct usb_hcd *hcd;
 	struct ehci_hcd *ehci;
+	struct clk *clk;
 	void __iomem *regs;
 	int irq, err;
 
@@ -238,6 +240,14 @@ static int __devinit ehci_orion_drv_probe(struct platform_device *pdev)
 		goto err2;
 	}
 
+	/* Not all platforms can gate the clock, so it is not
+	   an error if the clock does not exists. */
+	clk = clk_get(&pdev->dev, NULL);
+	if (!IS_ERR(clk)) {
+		clk_prepare_enable(clk);
+		clk_put(clk);
+	}
+
 	hcd = usb_create_hcd(&ehci_orion_hc_driver,
 			&pdev->dev, dev_name(&pdev->dev));
 	if (!hcd) {
@@ -301,12 +311,18 @@ err1:
 static int __exit ehci_orion_drv_remove(struct platform_device *pdev)
 {
 	struct usb_hcd *hcd = platform_get_drvdata(pdev);
+	struct clk *clk;
 
 	usb_remove_hcd(hcd);
 	iounmap(hcd->regs);
 	release_mem_region(hcd->rsrc_start, hcd->rsrc_len);
 	usb_put_hcd(hcd);
 
+	clk = clk_get(&pdev->dev, NULL);
+	if (!IS_ERR(clk)) {
+		clk_disable_unprepare(clk);
+		clk_put(clk);
+	}
 	return 0;
 }
 
diff --git a/drivers/usb/host/ehci-pci.c b/drivers/usb/host/ehci-pci.c
index fe8dc069164..bc94d7bf072 100644
--- a/drivers/usb/host/ehci-pci.c
+++ b/drivers/usb/host/ehci-pci.c
@@ -368,7 +368,9 @@ static bool usb_is_intel_switchable_ehci(struct pci_dev *pdev)
 {
 	return pdev->class == PCI_CLASS_SERIAL_USB_EHCI &&
 		pdev->vendor == PCI_VENDOR_ID_INTEL &&
-		pdev->device == 0x1E26;
+		(pdev->device == 0x1E26 ||
+		 pdev->device == 0x8C2D ||
+		 pdev->device == 0x8C26);
 }
 
 static void ehci_enable_xhci_companion(void)
diff --git a/drivers/usb/host/ehci-platform.c b/drivers/usb/host/ehci-platform.c
index d238b4e24bb..dfe881a34ae 100644
--- a/drivers/usb/host/ehci-platform.c
+++ b/drivers/usb/host/ehci-platform.c
@@ -75,8 +75,6 @@ static const struct hc_driver ehci_platform_hc_driver = {
 	.relinquish_port	= ehci_relinquish_port,
 	.port_handed_over	= ehci_port_handed_over,
 
-	.update_device		= ehci_update_device,
-
 	.clear_tt_buffer_complete = ehci_clear_tt_buffer_complete,
 };
 
@@ -94,12 +92,12 @@ static int __devinit ehci_platform_probe(struct platform_device *dev)
 
 	irq = platform_get_irq(dev, 0);
 	if (irq < 0) {
-		pr_err("no irq provieded");
+		pr_err("no irq provided");
 		return irq;
 	}
 	res_mem = platform_get_resource(dev, IORESOURCE_MEM, 0);
 	if (!res_mem) {
-		pr_err("no memory recourse provieded");
+		pr_err("no memory recourse provided");
 		return -ENXIO;
 	}
 
diff --git a/drivers/usb/host/ehci-q.c b/drivers/usb/host/ehci-q.c
index 36ca5077cdf..4378bf72bba 100644
--- a/drivers/usb/host/ehci-q.c
+++ b/drivers/usb/host/ehci-q.c
@@ -943,7 +943,8 @@ qh_make (
 		}
 		break;
 	default:
-		dbg ("bogus dev %p speed %d", urb->dev, urb->dev->speed);
+		ehci_dbg(ehci, "bogus dev %p speed %d\n", urb->dev,
+			urb->dev->speed);
 done:
 		qh_put (qh);
 		return NULL;
@@ -981,14 +982,12 @@ static void qh_link_async (struct ehci_hcd *ehci, struct ehci_qh *qh)
 	head = ehci->async;
 	timer_action_done (ehci, TIMER_ASYNC_OFF);
 	if (!head->qh_next.qh) {
-		u32	cmd = ehci_readl(ehci, &ehci->regs->command);
-
-		if (!(cmd & CMD_ASE)) {
+		if (!(ehci->command & CMD_ASE)) {
 			/* in case a clear of CMD_ASE didn't take yet */
 			(void)handshake(ehci, &ehci->regs->status,
 					STS_ASS, 0, 150);
-			cmd |= CMD_ASE;
-			ehci_writel(ehci, cmd, &ehci->regs->command);
+			ehci->command |= CMD_ASE;
+			ehci_writel(ehci, ehci->command, &ehci->regs->command);
 			/* posted write need not be known to HC yet ... */
 		}
 	}
@@ -1204,7 +1203,6 @@ static void end_unlink_async (struct ehci_hcd *ehci)
 
 static void start_unlink_async (struct ehci_hcd *ehci, struct ehci_qh *qh)
 {
-	int		cmd = ehci_readl(ehci, &ehci->regs->command);
 	struct ehci_qh	*prev;
 
 #ifdef DEBUG
@@ -1222,8 +1220,8 @@ static void start_unlink_async (struct ehci_hcd *ehci, struct ehci_qh *qh)
 		if (ehci->rh_state != EHCI_RH_HALTED
 				&& !ehci->reclaim) {
 			/* ... and CMD_IAAD clear */
-			ehci_writel(ehci, cmd & ~CMD_ASE,
-				    &ehci->regs->command);
+			ehci->command &= ~CMD_ASE;
+			ehci_writel(ehci, ehci->command, &ehci->regs->command);
 			wmb ();
 			// handshake later, if we need to
 			timer_action_done (ehci, TIMER_ASYNC_OFF);
@@ -1253,8 +1251,7 @@ static void start_unlink_async (struct ehci_hcd *ehci, struct ehci_qh *qh)
 		return;
 	}
 
-	cmd |= CMD_IAAD;
-	ehci_writel(ehci, cmd, &ehci->regs->command);
+	ehci_writel(ehci, ehci->command | CMD_IAAD, &ehci->regs->command);
 	(void)ehci_readl(ehci, &ehci->regs->command);
 	iaa_watchdog_start(ehci);
 }
diff --git a/drivers/usb/host/ehci-s5p.c b/drivers/usb/host/ehci-s5p.c
index f098e2a291a..c474cec064e 100644
--- a/drivers/usb/host/ehci-s5p.c
+++ b/drivers/usb/host/ehci-s5p.c
@@ -232,6 +232,8 @@ static int s5p_ehci_suspend(struct device *dev)
 	if (pdata && pdata->phy_exit)
 		pdata->phy_exit(pdev, S5P_USB_PHY_HOST);
 
+	clk_disable(s5p_ehci->clk);
+
 	return rc;
 }
 
@@ -243,6 +245,8 @@ static int s5p_ehci_resume(struct device *dev)
 	struct platform_device *pdev = to_platform_device(dev);
 	struct s5p_ehci_platdata *pdata = pdev->dev.platform_data;
 
+	clk_enable(s5p_ehci->clk);
+
 	if (pdata && pdata->phy_init)
 		pdata->phy_init(pdev, S5P_USB_PHY_HOST);
 
diff --git a/drivers/usb/host/ehci-sched.c b/drivers/usb/host/ehci-sched.c
index a60679cbbf8..33182c6d1ff 100644
--- a/drivers/usb/host/ehci-sched.c
+++ b/drivers/usb/host/ehci-sched.c
@@ -481,7 +481,6 @@ static int tt_no_collision (
 
 static int enable_periodic (struct ehci_hcd *ehci)
 {
-	u32	cmd;
 	int	status;
 
 	if (ehci->periodic_sched++)
@@ -497,8 +496,8 @@ static int enable_periodic (struct ehci_hcd *ehci)
 		return status;
 	}
 
-	cmd = ehci_readl(ehci, &ehci->regs->command) | CMD_PSE;
-	ehci_writel(ehci, cmd, &ehci->regs->command);
+	ehci->command |= CMD_PSE;
+	ehci_writel(ehci, ehci->command, &ehci->regs->command);
 	/* posted write ... PSS happens later */
 
 	/* make sure ehci_work scans these */
@@ -511,7 +510,6 @@ static int enable_periodic (struct ehci_hcd *ehci)
 
 static int disable_periodic (struct ehci_hcd *ehci)
 {
-	u32	cmd;
 	int	status;
 
 	if (--ehci->periodic_sched)
@@ -537,8 +535,8 @@ static int disable_periodic (struct ehci_hcd *ehci)
 		return status;
 	}
 
-	cmd = ehci_readl(ehci, &ehci->regs->command) & ~CMD_PSE;
-	ehci_writel(ehci, cmd, &ehci->regs->command);
+	ehci->command &= ~CMD_PSE;
+	ehci_writel(ehci, ehci->command, &ehci->regs->command);
 	/* posted write ... */
 
 	free_cached_lists(ehci);
@@ -1333,34 +1331,36 @@ sitd_slot_ok (
 	if (mask & ~0xffff)
 		return 0;
 
+	/* check bandwidth */
+	uframe %= period_uframes;
+	frame = uframe >> 3;
+
+#ifdef CONFIG_USB_EHCI_TT_NEWSCHED
+	/* The tt's fullspeed bus bandwidth must be available.
+	 * tt_available scheduling guarantees 10+% for control/bulk.
+	 */
+	uf = uframe & 7;
+	if (!tt_available(ehci, period_uframes >> 3,
+			stream->udev, frame, uf, stream->tt_usecs))
+		return 0;
+#else
+	/* tt must be idle for start(s), any gap, and csplit.
+	 * assume scheduling slop leaves 10+% for control/bulk.
+	 */
+	if (!tt_no_collision(ehci, period_uframes >> 3,
+			stream->udev, frame, mask))
+		return 0;
+#endif
+
 	/* this multi-pass logic is simple, but performance may
 	 * suffer when the schedule data isn't cached.
 	 */
-
-	/* check bandwidth */
-	uframe %= period_uframes;
 	do {
 		u32		max_used;
 
 		frame = uframe >> 3;
 		uf = uframe & 7;
 
-#ifdef CONFIG_USB_EHCI_TT_NEWSCHED
-		/* The tt's fullspeed bus bandwidth must be available.
-		 * tt_available scheduling guarantees 10+% for control/bulk.
-		 */
-		if (!tt_available (ehci, period_uframes << 3,
-				stream->udev, frame, uf, stream->tt_usecs))
-			return 0;
-#else
-		/* tt must be idle for start(s), any gap, and csplit.
-		 * assume scheduling slop leaves 10+% for control/bulk.
-		 */
-		if (!tt_no_collision (ehci, period_uframes << 3,
-				stream->udev, frame, mask))
-			return 0;
-#endif
-
 		/* check starts (OUT uses more than one) */
 		max_used = ehci->uframe_periodic_max - stream->usecs;
 		for (tmp = stream->raw_mask & 0xff; tmp; tmp >>= 1, uf++) {
@@ -2358,7 +2358,8 @@ restart:
 				 * in the previous frame for completions.
 				 */
 				if (q.fstn->hw_prev != EHCI_LIST_END(ehci)) {
-					dbg ("ignoring completions from FSTNs");
+					ehci_dbg(ehci,
+						"ignoring completions from FSTNs\n");
 				}
 				type = Q_NEXT_TYPE(ehci, q.fstn->hw_next);
 				q = q.fstn->fstn_next;
@@ -2441,7 +2442,7 @@ restart:
 				q = *q_p;
 				break;
 			default:
-				dbg ("corrupt type %d frame %d shadow %p",
+				ehci_dbg(ehci, "corrupt type %d frame %d shadow %p\n",
 					type, frame, q.ptr);
 				// BUG ();
 				q.ptr = NULL;
diff --git a/drivers/usb/host/ehci-sead3.c b/drivers/usb/host/ehci-sead3.c
new file mode 100644
index 00000000000..cc199e87a7a
--- /dev/null
+++ b/drivers/usb/host/ehci-sead3.c
@@ -0,0 +1,266 @@
+/*
+ * MIPS CI13320A EHCI Host Controller driver
+ * Based on "ehci-au1xxx.c" by K.Boge <karsten.boge@amd.com>
+ *
+ * Copyright (C) 2012 MIPS Technologies, 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 <linux/platform_device.h>
+
+static int ehci_sead3_setup(struct usb_hcd *hcd)
+{
+	int ret;
+	struct ehci_hcd *ehci = hcd_to_ehci(hcd);
+
+	ehci->caps = hcd->regs + 0x100;
+
+#ifdef __BIG_ENDIAN
+	ehci->big_endian_mmio = 1;
+	ehci->big_endian_desc = 1;
+#endif
+
+	ret = ehci_setup(hcd);
+	if (ret)
+		return ret;
+
+	ehci->need_io_watchdog = 0;
+
+	/* Set burst length to 16 words. */
+	ehci_writel(ehci, 0x1010, &ehci->regs->reserved[1]);
+
+	return ret;
+}
+
+const struct hc_driver ehci_sead3_hc_driver = {
+	.description		= hcd_name,
+	.product_desc		= "SEAD-3 EHCI",
+	.hcd_priv_size		= sizeof(struct ehci_hcd),
+
+	/*
+	 * generic hardware linkage
+	 */
+	.irq			= ehci_irq,
+	.flags			= HCD_MEMORY | HCD_USB2,
+
+	/*
+	 * basic lifecycle operations
+	 *
+	 */
+	.reset			= ehci_sead3_setup,
+	.start			= ehci_run,
+	.stop			= ehci_stop,
+	.shutdown		= ehci_shutdown,
+
+	/*
+	 * managing i/o requests and associated device resources
+	 */
+	.urb_enqueue		= ehci_urb_enqueue,
+	.urb_dequeue		= ehci_urb_dequeue,
+	.endpoint_disable	= ehci_endpoint_disable,
+	.endpoint_reset		= ehci_endpoint_reset,
+
+	/*
+	 * scheduling support
+	 */
+	.get_frame_number	= ehci_get_frame,
+
+	/*
+	 * root hub support
+	 */
+	.hub_status_data	= ehci_hub_status_data,
+	.hub_control		= ehci_hub_control,
+	.bus_suspend		= ehci_bus_suspend,
+	.bus_resume		= ehci_bus_resume,
+	.relinquish_port	= ehci_relinquish_port,
+	.port_handed_over	= ehci_port_handed_over,
+
+	.clear_tt_buffer_complete	= ehci_clear_tt_buffer_complete,
+};
+
+static int ehci_hcd_sead3_drv_probe(struct platform_device *pdev)
+{
+	struct usb_hcd *hcd;
+	struct resource *res;
+	int ret;
+
+	if (usb_disabled())
+		return -ENODEV;
+
+	if (pdev->resource[1].flags != IORESOURCE_IRQ) {
+		pr_debug("resource[1] is not IORESOURCE_IRQ");
+		return -ENOMEM;
+	}
+	hcd = usb_create_hcd(&ehci_sead3_hc_driver, &pdev->dev, "SEAD-3");
+	if (!hcd)
+		return -ENOMEM;
+
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	hcd->rsrc_start = res->start;
+	hcd->rsrc_len = resource_size(res);
+
+	if (!request_mem_region(hcd->rsrc_start, hcd->rsrc_len, hcd_name)) {
+		pr_debug("request_mem_region failed");
+		ret = -EBUSY;
+		goto err1;
+	}
+
+	hcd->regs = ioremap(hcd->rsrc_start, hcd->rsrc_len);
+	if (!hcd->regs) {
+		pr_debug("ioremap failed");
+		ret = -ENOMEM;
+		goto err2;
+	}
+
+	/* Root hub has integrated TT. */
+	hcd->has_tt = 1;
+
+	ret = usb_add_hcd(hcd, pdev->resource[1].start,
+			  IRQF_SHARED);
+	if (ret == 0) {
+		platform_set_drvdata(pdev, hcd);
+		return ret;
+	}
+
+	iounmap(hcd->regs);
+err2:
+	release_mem_region(hcd->rsrc_start, hcd->rsrc_len);
+err1:
+	usb_put_hcd(hcd);
+	return ret;
+}
+
+static int ehci_hcd_sead3_drv_remove(struct platform_device *pdev)
+{
+	struct usb_hcd *hcd = platform_get_drvdata(pdev);
+
+	usb_remove_hcd(hcd);
+	iounmap(hcd->regs);
+	release_mem_region(hcd->rsrc_start, hcd->rsrc_len);
+	usb_put_hcd(hcd);
+	platform_set_drvdata(pdev, NULL);
+
+	return 0;
+}
+
+#ifdef CONFIG_PM
+static int ehci_hcd_sead3_drv_suspend(struct device *dev)
+{
+	struct usb_hcd *hcd = dev_get_drvdata(dev);
+	struct ehci_hcd *ehci = hcd_to_ehci(hcd);
+	unsigned long flags;
+	int rc = 0;
+
+	if (time_before(jiffies, ehci->next_statechange))
+		msleep(20);
+
+	/* Root hub was already suspended. Disable irq emission and
+	 * mark HW unaccessible.  The PM and USB cores make sure that
+	 * the root hub is either suspended or stopped.
+	 */
+	ehci_prepare_ports_for_controller_suspend(ehci, device_may_wakeup(dev));
+	spin_lock_irqsave(&ehci->lock, flags);
+	ehci_writel(ehci, 0, &ehci->regs->intr_enable);
+	(void)ehci_readl(ehci, &ehci->regs->intr_enable);
+
+	clear_bit(HCD_FLAG_HW_ACCESSIBLE, &hcd->flags);
+	spin_unlock_irqrestore(&ehci->lock, flags);
+
+	/* could save FLADJ in case of Vaux power loss
+	 * ... we'd only use it to handle clock skew
+	 */
+
+	return rc;
+}
+
+static int ehci_hcd_sead3_drv_resume(struct device *dev)
+{
+	struct usb_hcd *hcd = dev_get_drvdata(dev);
+	struct ehci_hcd *ehci = hcd_to_ehci(hcd);
+
+	/* maybe restore FLADJ. */
+
+	if (time_before(jiffies, ehci->next_statechange))
+		msleep(100);
+
+	/* Mark hardware accessible again as we are out of D3 state by now */
+	set_bit(HCD_FLAG_HW_ACCESSIBLE, &hcd->flags);
+
+	/* If CF is still set, we maintained PCI Vaux power.
+	 * Just undo the effect of ehci_pci_suspend().
+	 */
+	if (ehci_readl(ehci, &ehci->regs->configured_flag) == FLAG_CF) {
+		int	mask = INTR_MASK;
+
+		ehci_prepare_ports_for_controller_resume(ehci);
+		if (!hcd->self.root_hub->do_remote_wakeup)
+			mask &= ~STS_PCD;
+		ehci_writel(ehci, mask, &ehci->regs->intr_enable);
+		ehci_readl(ehci, &ehci->regs->intr_enable);
+		return 0;
+	}
+
+	ehci_dbg(ehci, "lost power, restarting\n");
+	usb_root_hub_lost_power(hcd->self.root_hub);
+
+	/* Else reset, to cope with power loss or flush-to-storage
+	 * style "resume" having let BIOS kick in during reboot.
+	 */
+	(void) ehci_halt(ehci);
+	(void) ehci_reset(ehci);
+
+	/* emptying the schedule aborts any urbs */
+	spin_lock_irq(&ehci->lock);
+	if (ehci->reclaim)
+		end_unlink_async(ehci);
+	ehci_work(ehci);
+	spin_unlock_irq(&ehci->lock);
+
+	ehci_writel(ehci, ehci->command, &ehci->regs->command);
+	ehci_writel(ehci, FLAG_CF, &ehci->regs->configured_flag);
+	ehci_readl(ehci, &ehci->regs->command);	/* unblock posted writes */
+
+	/* here we "know" root ports should always stay powered */
+	ehci_port_power(ehci, 1);
+
+	ehci->rh_state = EHCI_RH_SUSPENDED;
+
+	return 0;
+}
+
+static const struct dev_pm_ops sead3_ehci_pmops = {
+	.suspend	= ehci_hcd_sead3_drv_suspend,
+	.resume		= ehci_hcd_sead3_drv_resume,
+};
+
+#define SEAD3_EHCI_PMOPS (&sead3_ehci_pmops)
+
+#else
+#define SEAD3_EHCI_PMOPS NULL
+#endif
+
+static struct platform_driver ehci_hcd_sead3_driver = {
+	.probe		= ehci_hcd_sead3_drv_probe,
+	.remove		= ehci_hcd_sead3_drv_remove,
+	.shutdown	= usb_hcd_platform_shutdown,
+	.driver = {
+		.name	= "sead3-ehci",
+		.owner	= THIS_MODULE,
+		.pm	= SEAD3_EHCI_PMOPS,
+	}
+};
+
+MODULE_ALIAS("platform:sead3-ehci");
diff --git a/drivers/usb/host/ehci-sh.c b/drivers/usb/host/ehci-sh.c
index 9d9cf47d80d..ca819cdd0c5 100644
--- a/drivers/usb/host/ehci-sh.c
+++ b/drivers/usb/host/ehci-sh.c
@@ -11,6 +11,7 @@
  */
 #include <linux/platform_device.h>
 #include <linux/clk.h>
+#include <linux/platform_data/ehci-sh.h>
 
 struct ehci_sh_priv {
 	struct clk *iclk, *fclk;
@@ -100,6 +101,7 @@ static int ehci_hcd_sh_probe(struct platform_device *pdev)
 	const struct hc_driver *driver = &ehci_sh_hc_driver;
 	struct resource *res;
 	struct ehci_sh_priv *priv;
+	struct ehci_sh_platdata *pdata;
 	struct usb_hcd *hcd;
 	int irq, ret;
 
@@ -124,6 +126,9 @@ static int ehci_hcd_sh_probe(struct platform_device *pdev)
 		goto fail_create_hcd;
 	}
 
+	if (pdev->dev.platform_data != NULL)
+		pdata = pdev->dev.platform_data;
+
 	/* initialize hcd */
 	hcd = usb_create_hcd(&ehci_sh_hc_driver, &pdev->dev,
 			     dev_name(&pdev->dev));
@@ -168,6 +173,9 @@ static int ehci_hcd_sh_probe(struct platform_device *pdev)
 	clk_enable(priv->fclk);
 	clk_enable(priv->iclk);
 
+	if (pdata && pdata->phy_init)
+		pdata->phy_init();
+
 	ret = usb_add_hcd(hcd, irq, IRQF_SHARED);
 	if (ret != 0) {
 		dev_err(&pdev->dev, "Failed to add hcd");
diff --git a/drivers/usb/host/ehci-spear.c b/drivers/usb/host/ehci-spear.c
index 6e928559169..37ba8c8d2fd 100644
--- a/drivers/usb/host/ehci-spear.c
+++ b/drivers/usb/host/ehci-spear.c
@@ -13,6 +13,7 @@
 
 #include <linux/clk.h>
 #include <linux/jiffies.h>
+#include <linux/of.h>
 #include <linux/platform_device.h>
 #include <linux/pm.h>
 
@@ -25,12 +26,12 @@ struct spear_ehci {
 
 static void spear_start_ehci(struct spear_ehci *ehci)
 {
-	clk_enable(ehci->clk);
+	clk_prepare_enable(ehci->clk);
 }
 
 static void spear_stop_ehci(struct spear_ehci *ehci)
 {
-	clk_disable(ehci->clk);
+	clk_disable_unprepare(ehci->clk);
 }
 
 static int ehci_spear_setup(struct usb_hcd *hcd)
@@ -168,6 +169,8 @@ static int ehci_spear_drv_resume(struct device *dev)
 static SIMPLE_DEV_PM_OPS(ehci_spear_pm_ops, ehci_spear_drv_suspend,
 		ehci_spear_drv_resume);
 
+static u64 spear_ehci_dma_mask = DMA_BIT_MASK(32);
+
 static int spear_ehci_hcd_drv_probe(struct platform_device *pdev)
 {
 	struct usb_hcd *hcd ;
@@ -175,12 +178,9 @@ static int spear_ehci_hcd_drv_probe(struct platform_device *pdev)
 	struct resource *res;
 	struct clk *usbh_clk;
 	const struct hc_driver *driver = &ehci_spear_hc_driver;
-	int *pdata = pdev->dev.platform_data;
 	int irq, retval;
 	char clk_name[20] = "usbh_clk";
-
-	if (pdata == NULL)
-		return -EFAULT;
+	static int instance = -1;
 
 	if (usb_disabled())
 		return -ENODEV;
@@ -191,8 +191,22 @@ static int spear_ehci_hcd_drv_probe(struct platform_device *pdev)
 		goto fail_irq_get;
 	}
 
-	if (*pdata >= 0)
-		sprintf(clk_name, "usbh.%01d_clk", *pdata);
+	/*
+	 * Right now device-tree probed devices don't get dma_mask set.
+	 * Since shared usb code relies on it, set it here for now.
+	 * Once we have dma capability bindings this can go away.
+	 */
+	if (!pdev->dev.dma_mask)
+		pdev->dev.dma_mask = &spear_ehci_dma_mask;
+
+	/*
+	 * Increment the device instance, when probing via device-tree
+	 */
+	if (pdev->id < 0)
+		instance++;
+	else
+		instance = pdev->id;
+	sprintf(clk_name, "usbh.%01d_clk", instance);
 
 	usbh_clk = clk_get(NULL, clk_name);
 	if (IS_ERR(usbh_clk)) {
@@ -277,6 +291,11 @@ static int spear_ehci_hcd_drv_remove(struct platform_device *pdev)
 	return 0;
 }
 
+static struct of_device_id spear_ehci_id_table[] __devinitdata = {
+	{ .compatible = "st,spear600-ehci", },
+	{ },
+};
+
 static struct platform_driver spear_ehci_hcd_driver = {
 	.probe		= spear_ehci_hcd_drv_probe,
 	.remove		= spear_ehci_hcd_drv_remove,
@@ -285,6 +304,7 @@ static struct platform_driver spear_ehci_hcd_driver = {
 		.name = "spear-ehci",
 		.bus = &platform_bus_type,
 		.pm = &ehci_spear_pm_ops,
+		.of_match_table = of_match_ptr(spear_ehci_id_table),
 	}
 };
 
diff --git a/drivers/usb/host/ehci-tegra.c b/drivers/usb/host/ehci-tegra.c
index f214a80cdee..68548236ec4 100644
--- a/drivers/usb/host/ehci-tegra.c
+++ b/drivers/usb/host/ehci-tegra.c
@@ -147,18 +147,7 @@ static int tegra_ehci_hub_control(
 
 	spin_lock_irqsave(&ehci->lock, flags);
 
-	/*
-	 * In ehci_hub_control() for USB_PORT_FEAT_ENABLE clears the other bits
-	 * that are write on clear, by writing back the register read value, so
-	 * USB_PORT_FEAT_ENABLE is handled by masking the set on clear bits
-	 */
-	if (typeReq == ClearPortFeature && wValue == USB_PORT_FEAT_ENABLE) {
-		temp = ehci_readl(ehci, status_reg) & ~PORT_RWC_BITS;
-		ehci_writel(ehci, temp & ~PORT_PE, status_reg);
-		goto done;
-	}
-
-	else if (typeReq == GetPortStatus) {
+	if (typeReq == GetPortStatus) {
 		temp = ehci_readl(ehci, status_reg);
 		if (tegra->port_resuming && !(temp & PORT_SUSPEND)) {
 			/* Resume completed, re-enable disconnect detection */
@@ -174,7 +163,7 @@ static int tegra_ehci_hub_control(
 			goto done;
 		}
 
-		temp &= ~PORT_WKCONN_E;
+		temp &= ~(PORT_RWC_BITS | PORT_WKCONN_E);
 		temp |= PORT_WKDISC_E | PORT_WKOC_E;
 		ehci_writel(ehci, temp | PORT_SUSPEND, status_reg);
 
@@ -319,26 +308,23 @@ static int tegra_ehci_setup(struct usb_hcd *hcd)
 	return retval;
 }
 
-struct temp_buffer {
+struct dma_aligned_buffer {
 	void *kmalloc_ptr;
 	void *old_xfer_buffer;
 	u8 data[0];
 };
 
-static void free_temp_buffer(struct urb *urb)
+static void free_dma_aligned_buffer(struct urb *urb)
 {
-	enum dma_data_direction dir;
-	struct temp_buffer *temp;
+	struct dma_aligned_buffer *temp;
 
 	if (!(urb->transfer_flags & URB_ALIGNED_TEMP_BUFFER))
 		return;
 
-	dir = usb_urb_dir_in(urb) ? DMA_FROM_DEVICE : DMA_TO_DEVICE;
-
-	temp = container_of(urb->transfer_buffer, struct temp_buffer,
-			    data);
+	temp = container_of(urb->transfer_buffer,
+		struct dma_aligned_buffer, data);
 
-	if (dir == DMA_FROM_DEVICE)
+	if (usb_urb_dir_in(urb))
 		memcpy(temp->old_xfer_buffer, temp->data,
 		       urb->transfer_buffer_length);
 	urb->transfer_buffer = temp->old_xfer_buffer;
@@ -347,10 +333,9 @@ static void free_temp_buffer(struct urb *urb)
 	urb->transfer_flags &= ~URB_ALIGNED_TEMP_BUFFER;
 }
 
-static int alloc_temp_buffer(struct urb *urb, gfp_t mem_flags)
+static int alloc_dma_aligned_buffer(struct urb *urb, gfp_t mem_flags)
 {
-	enum dma_data_direction dir;
-	struct temp_buffer *temp, *kmalloc_ptr;
+	struct dma_aligned_buffer *temp, *kmalloc_ptr;
 	size_t kmalloc_size;
 
 	if (urb->num_sgs || urb->sg ||
@@ -358,22 +343,19 @@ static int alloc_temp_buffer(struct urb *urb, gfp_t mem_flags)
 	    !((uintptr_t)urb->transfer_buffer & (TEGRA_USB_DMA_ALIGN - 1)))
 		return 0;
 
-	dir = usb_urb_dir_in(urb) ? DMA_FROM_DEVICE : DMA_TO_DEVICE;
-
 	/* Allocate a buffer with enough padding for alignment */
 	kmalloc_size = urb->transfer_buffer_length +
-		sizeof(struct temp_buffer) + TEGRA_USB_DMA_ALIGN - 1;
+		sizeof(struct dma_aligned_buffer) + TEGRA_USB_DMA_ALIGN - 1;
 
 	kmalloc_ptr = kmalloc(kmalloc_size, mem_flags);
 	if (!kmalloc_ptr)
 		return -ENOMEM;
 
-	/* Position our struct temp_buffer such that data is aligned */
+	/* Position our struct dma_aligned_buffer such that data is aligned */
 	temp = PTR_ALIGN(kmalloc_ptr + 1, TEGRA_USB_DMA_ALIGN) - 1;
-
 	temp->kmalloc_ptr = kmalloc_ptr;
 	temp->old_xfer_buffer = urb->transfer_buffer;
-	if (dir == DMA_TO_DEVICE)
+	if (usb_urb_dir_out(urb))
 		memcpy(temp->data, urb->transfer_buffer,
 		       urb->transfer_buffer_length);
 	urb->transfer_buffer = temp->data;
@@ -388,13 +370,13 @@ static int tegra_ehci_map_urb_for_dma(struct usb_hcd *hcd, struct urb *urb,
 {
 	int ret;
 
-	ret = alloc_temp_buffer(urb, mem_flags);
+	ret = alloc_dma_aligned_buffer(urb, mem_flags);
 	if (ret)
 		return ret;
 
 	ret = usb_hcd_map_urb_for_dma(hcd, urb, mem_flags);
 	if (ret)
-		free_temp_buffer(urb);
+		free_dma_aligned_buffer(urb);
 
 	return ret;
 }
@@ -402,49 +384,51 @@ static int tegra_ehci_map_urb_for_dma(struct usb_hcd *hcd, struct urb *urb,
 static void tegra_ehci_unmap_urb_for_dma(struct usb_hcd *hcd, struct urb *urb)
 {
 	usb_hcd_unmap_urb_for_dma(hcd, urb);
-	free_temp_buffer(urb);
+	free_dma_aligned_buffer(urb);
 }
 
 static const struct hc_driver tegra_ehci_hc_driver = {
 	.description		= hcd_name,
 	.product_desc		= "Tegra EHCI Host Controller",
 	.hcd_priv_size		= sizeof(struct ehci_hcd),
-
 	.flags			= HCD_USB2 | HCD_MEMORY,
 
-	.reset			= tegra_ehci_setup,
+	/* standard ehci functions */
 	.irq			= ehci_irq,
-
 	.start			= ehci_run,
 	.stop			= ehci_stop,
-	.shutdown		= tegra_ehci_shutdown,
 	.urb_enqueue		= ehci_urb_enqueue,
 	.urb_dequeue		= ehci_urb_dequeue,
-	.map_urb_for_dma	= tegra_ehci_map_urb_for_dma,
-	.unmap_urb_for_dma	= tegra_ehci_unmap_urb_for_dma,
 	.endpoint_disable	= ehci_endpoint_disable,
 	.endpoint_reset		= ehci_endpoint_reset,
 	.get_frame_number	= ehci_get_frame,
 	.hub_status_data	= ehci_hub_status_data,
-	.hub_control		= tegra_ehci_hub_control,
 	.clear_tt_buffer_complete = ehci_clear_tt_buffer_complete,
+	.relinquish_port	= ehci_relinquish_port,
+	.port_handed_over	= ehci_port_handed_over,
+
+	/* modified ehci functions for tegra */
+	.reset			= tegra_ehci_setup,
+	.shutdown		= tegra_ehci_shutdown,
+	.map_urb_for_dma	= tegra_ehci_map_urb_for_dma,
+	.unmap_urb_for_dma	= tegra_ehci_unmap_urb_for_dma,
+	.hub_control		= tegra_ehci_hub_control,
 #ifdef CONFIG_PM
 	.bus_suspend		= ehci_bus_suspend,
 	.bus_resume		= ehci_bus_resume,
 #endif
-	.relinquish_port	= ehci_relinquish_port,
-	.port_handed_over	= ehci_port_handed_over,
 };
 
-static int setup_vbus_gpio(struct platform_device *pdev)
+static int setup_vbus_gpio(struct platform_device *pdev,
+			   struct tegra_ehci_platform_data *pdata)
 {
 	int err = 0;
 	int gpio;
 
-	if (!pdev->dev.of_node)
-		return 0;
-
-	gpio = of_get_named_gpio(pdev->dev.of_node, "nvidia,vbus-gpio", 0);
+	gpio = pdata->vbus_gpio;
+	if (!gpio_is_valid(gpio))
+		gpio = of_get_named_gpio(pdev->dev.of_node,
+					 "nvidia,vbus-gpio", 0);
 	if (!gpio_is_valid(gpio))
 		return 0;
 
@@ -664,7 +648,7 @@ static int tegra_ehci_probe(struct platform_device *pdev)
 	if (!pdev->dev.dma_mask)
 		pdev->dev.dma_mask = &tegra_ehci_dma_mask;
 
-	setup_vbus_gpio(pdev);
+	setup_vbus_gpio(pdev, pdata);
 
 	tegra = kzalloc(sizeof(struct tegra_ehci_hcd), GFP_KERNEL);
 	if (!tegra)
@@ -738,8 +722,9 @@ static int tegra_ehci_probe(struct platform_device *pdev)
 		}
 	}
 
-	tegra->phy = tegra_usb_phy_open(instance, hcd->regs, pdata->phy_config,
-						TEGRA_USB_PHY_MODE_HOST);
+	tegra->phy = tegra_usb_phy_open(&pdev->dev, instance, hcd->regs,
+					pdata->phy_config,
+					TEGRA_USB_PHY_MODE_HOST);
 	if (IS_ERR(tegra->phy)) {
 		dev_err(&pdev->dev, "Failed to open USB phy\n");
 		err = -ENXIO;
diff --git a/drivers/usb/host/fhci-tds.c b/drivers/usb/host/fhci-tds.c
index 0ea577bfca2..c5ed8819929 100644
--- a/drivers/usb/host/fhci-tds.c
+++ b/drivers/usb/host/fhci-tds.c
@@ -155,7 +155,7 @@ u32 fhci_create_ep(struct fhci_usb *usb, enum fhci_mem_alloc data_mem,
 	struct endpoint *ep;
 	struct usb_td __iomem *td;
 	unsigned long ep_offset;
-	char *err_for = "enpoint PRAM";
+	char *err_for = "endpoint PRAM";
 	int ep_mem_size;
 	u32 i;
 
diff --git a/drivers/usb/host/fsl-mph-dr-of.c b/drivers/usb/host/fsl-mph-dr-of.c
index ab333ac6071..22ff6b3a676 100644
--- a/drivers/usb/host/fsl-mph-dr-of.c
+++ b/drivers/usb/host/fsl-mph-dr-of.c
@@ -119,6 +119,39 @@ error:
 
 static const struct of_device_id fsl_usb2_mph_dr_of_match[];
 
+static int usb_get_ver_info(struct device_node *np)
+{
+	int ver = -1;
+
+	/*
+	 * returns 1 for usb controller version 1.6
+	 * returns 2 for usb controller version 2.2
+	 * returns 0 otherwise
+	 */
+	if (of_device_is_compatible(np, "fsl-usb2-dr")) {
+		if (of_device_is_compatible(np, "fsl-usb2-dr-v1.6"))
+			ver = FSL_USB_VER_1_6;
+		else if (of_device_is_compatible(np, "fsl-usb2-dr-v2.2"))
+			ver = FSL_USB_VER_2_2;
+		else /* for previous controller versions */
+			ver = FSL_USB_VER_OLD;
+
+		if (ver > -1)
+			return ver;
+	}
+
+	if (of_device_is_compatible(np, "fsl-usb2-mph")) {
+		if (of_device_is_compatible(np, "fsl-usb2-mph-v1.6"))
+			ver = FSL_USB_VER_1_6;
+		else if (of_device_is_compatible(np, "fsl-usb2-mph-v2.2"))
+			ver = FSL_USB_VER_2_2;
+		else /* for previous controller versions */
+			ver = FSL_USB_VER_OLD;
+	}
+
+	return ver;
+}
+
 static int __devinit fsl_usb2_mph_dr_of_probe(struct platform_device *ofdev)
 {
 	struct device_node *np = ofdev->dev.of_node;
@@ -166,6 +199,14 @@ static int __devinit fsl_usb2_mph_dr_of_probe(struct platform_device *ofdev)
 
 	prop = of_get_property(np, "phy_type", NULL);
 	pdata->phy_mode = determine_usb_phy(prop);
+	pdata->controller_ver = usb_get_ver_info(np);
+
+	if (pdata->have_sysif_regs) {
+		if (pdata->controller_ver < 0) {
+			dev_warn(&ofdev->dev, "Could not get controller version\n");
+			return -ENODEV;
+		}
+	}
 
 	for (i = 0; i < ARRAY_SIZE(dev_data->drivers); i++) {
 		if (!dev_data->drivers[i])
diff --git a/drivers/usb/host/isp1760-hcd.c b/drivers/usb/host/isp1760-hcd.c
index fc72d44bf78..a35bbddf896 100644
--- a/drivers/usb/host/isp1760-hcd.c
+++ b/drivers/usb/host/isp1760-hcd.c
@@ -1562,11 +1562,14 @@ static int isp1760_urb_enqueue(struct usb_hcd *hcd, struct urb *urb,
 
 	if (!test_bit(HCD_FLAG_HW_ACCESSIBLE, &hcd->flags)) {
 		retval = -ESHUTDOWN;
+		qtd_list_free(&new_qtds);
 		goto out;
 	}
 	retval = usb_hcd_link_urb_to_ep(hcd, urb);
-	if (retval)
+	if (retval) {
+		qtd_list_free(&new_qtds);
 		goto out;
+	}
 
 	qh = urb->ep->hcpriv;
 	if (qh) {
@@ -1584,6 +1587,7 @@ static int isp1760_urb_enqueue(struct usb_hcd *hcd, struct urb *urb,
 		if (!qh) {
 			retval = -ENOMEM;
 			usb_hcd_unlink_urb_from_ep(hcd, urb);
+			qtd_list_free(&new_qtds);
 			goto out;
 		}
 		list_add_tail(&qh->qh_list, ep_queue);
@@ -1683,6 +1687,7 @@ static int isp1760_urb_dequeue(struct usb_hcd *hcd, struct urb *urb,
 	list_for_each_entry(qtd, &qh->qtd_list, qtd_list)
 		if (qtd->urb == urb) {
 			dequeue_urb_from_qtd(hcd, qh, qtd);
+			list_move(&qtd->qtd_list, &qh->qtd_list);
 			break;
 		}
 
@@ -2176,7 +2181,7 @@ static const struct hc_driver isp1760_hc_driver = {
 
 int __init init_kmem_once(void)
 {
-	urb_listitem_cachep = kmem_cache_create("isp1760 urb_listitem",
+	urb_listitem_cachep = kmem_cache_create("isp1760_urb_listitem",
 			sizeof(struct urb_listitem), 0, SLAB_TEMPORARY |
 			SLAB_MEM_SPREAD, NULL);
 
diff --git a/drivers/usb/host/isp1760-if.c b/drivers/usb/host/isp1760-if.c
index 4592dc17a9f..fff114fd546 100644
--- a/drivers/usb/host/isp1760-if.c
+++ b/drivers/usb/host/isp1760-if.c
@@ -398,6 +398,9 @@ static int __devinit isp1760_plat_probe(struct platform_device *pdev)
 	hcd = isp1760_register(mem_res->start, mem_size, irq_res->start,
 			       irqflags, -ENOENT,
 			       &pdev->dev, dev_name(&pdev->dev), devflags);
+
+	dev_set_drvdata(&pdev->dev, hcd);
+
 	if (IS_ERR(hcd)) {
 		pr_warning("isp1760: Failed to register the HCD device\n");
 		ret = -ENODEV;
@@ -417,11 +420,16 @@ static int __devexit isp1760_plat_remove(struct platform_device *pdev)
 {
 	struct resource *mem_res;
 	resource_size_t mem_size;
+	struct usb_hcd *hcd = dev_get_drvdata(&pdev->dev);
+
+	usb_remove_hcd(hcd);
 
 	mem_res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
 	mem_size = resource_size(mem_res);
 	release_mem_region(mem_res->start, mem_size);
 
+	usb_put_hcd(hcd);
+
 	return 0;
 }
 
diff --git a/drivers/usb/host/ohci-at91.c b/drivers/usb/host/ohci-at91.c
index 13ebeca8e73..a665b3eaa74 100644
--- a/drivers/usb/host/ohci-at91.c
+++ b/drivers/usb/host/ohci-at91.c
@@ -129,7 +129,7 @@ static int __devinit usb_hcd_at91_probe(const struct hc_driver *driver,
 	if (!hcd)
 		return -ENOMEM;
 	hcd->rsrc_start = pdev->resource[0].start;
-	hcd->rsrc_len = pdev->resource[0].end - pdev->resource[0].start + 1;
+	hcd->rsrc_len = resource_size(&pdev->resource[0]);
 
 	if (!request_mem_region(hcd->rsrc_start, hcd->rsrc_len, hcd_name)) {
 		pr_debug("request_mem_region failed\n");
@@ -223,7 +223,7 @@ static void __devexit usb_hcd_at91_remove(struct usb_hcd *hcd,
 /*-------------------------------------------------------------------------*/
 
 static int __devinit
-ohci_at91_start (struct usb_hcd *hcd)
+ohci_at91_reset (struct usb_hcd *hcd)
 {
 	struct at91_usbh_data	*board = hcd->self.controller->platform_data;
 	struct ohci_hcd		*ohci = hcd_to_ohci (hcd);
@@ -233,9 +233,18 @@ ohci_at91_start (struct usb_hcd *hcd)
 		return ret;
 
 	ohci->num_ports = board->ports;
+	return 0;
+}
+
+static int __devinit
+ohci_at91_start (struct usb_hcd *hcd)
+{
+	struct ohci_hcd		*ohci = hcd_to_ohci (hcd);
+	int			ret;
 
 	if ((ret = ohci_run(ohci)) < 0) {
-		err("can't start %s", hcd->self.bus_name);
+		dev_err(hcd->self.controller, "can't start %s\n",
+			hcd->self.bus_name);
 		ohci_stop(hcd);
 		return ret;
 	}
@@ -418,6 +427,7 @@ static const struct hc_driver ohci_at91_hc_driver = {
 	/*
 	 * basic lifecycle operations
 	 */
+	.reset =		ohci_at91_reset,
 	.start =		ohci_at91_start,
 	.stop =			ohci_stop,
 	.shutdown =		ohci_shutdown,
diff --git a/drivers/usb/host/ohci-au1xxx.c b/drivers/usb/host/ohci-au1xxx.c
index 4ea63b2cac4..c611699b4aa 100644
--- a/drivers/usb/host/ohci-au1xxx.c
+++ b/drivers/usb/host/ohci-au1xxx.c
@@ -37,7 +37,8 @@ static int __devinit ohci_au1xxx_start(struct usb_hcd *hcd)
 		return ret;
 
 	if ((ret = ohci_run(ohci)) < 0) {
-		err ("can't start %s", hcd->self.bus_name);
+		dev_err(hcd->self.controller, "can't start %s",
+			hcd->self.bus_name);
 		ohci_stop(hcd);
 		return ret;
 	}
diff --git a/drivers/usb/host/ohci-cns3xxx.c b/drivers/usb/host/ohci-cns3xxx.c
index 5a00a1e1c6c..2c9f233047b 100644
--- a/drivers/usb/host/ohci-cns3xxx.c
+++ b/drivers/usb/host/ohci-cns3xxx.c
@@ -41,7 +41,8 @@ cns3xxx_ohci_start(struct usb_hcd *hcd)
 
 	ret = ohci_run(ohci);
 	if (ret < 0) {
-		err("can't start %s", hcd->self.bus_name);
+		dev_err(hcd->self.controller, "can't start %s\n",
+			hcd->self.bus_name);
 		ohci_stop(hcd);
 		return ret;
 	}
diff --git a/drivers/usb/host/ohci-da8xx.c b/drivers/usb/host/ohci-da8xx.c
index 843509778a3..269b1e0f769 100644
--- a/drivers/usb/host/ohci-da8xx.c
+++ b/drivers/usb/host/ohci-da8xx.c
@@ -454,3 +454,5 @@ static struct platform_driver ohci_hcd_da8xx_driver = {
 		.name	= "ohci",
 	},
 };
+
+MODULE_ALIAS("platform:ohci");
diff --git a/drivers/usb/host/ohci-dbg.c b/drivers/usb/host/ohci-dbg.c
index e4bcb62b930..31b81f9eacd 100644
--- a/drivers/usb/host/ohci-dbg.c
+++ b/drivers/usb/host/ohci-dbg.c
@@ -29,14 +29,14 @@ urb_print(struct urb * urb, char * str, int small, int status)
 	unsigned int pipe= urb->pipe;
 
 	if (!urb->dev || !urb->dev->bus) {
-		dbg("%s URB: no dev", str);
+		printk(KERN_DEBUG "%s URB: no dev\n", str);
 		return;
 	}
 
 #ifndef	OHCI_VERBOSE_DEBUG
 	if (status != 0)
 #endif
-	dbg("%s %p dev=%d ep=%d%s-%s flags=%x len=%d/%d stat=%d",
+	printk(KERN_DEBUG "%s %p dev=%d ep=%d%s-%s flags=%x len=%d/%d stat=%d\n",
 		    str,
 		    urb,
 		    usb_pipedevice (pipe),
diff --git a/drivers/usb/host/ohci-ep93xx.c b/drivers/usb/host/ohci-ep93xx.c
index 3d63574d2c7..dbfbd1dfd2e 100644
--- a/drivers/usb/host/ohci-ep93xx.c
+++ b/drivers/usb/host/ohci-ep93xx.c
@@ -47,7 +47,7 @@ static int usb_hcd_ep93xx_probe(const struct hc_driver *driver,
 	struct usb_hcd *hcd;
 
 	if (pdev->resource[1].flags != IORESOURCE_IRQ) {
-		dbg("resource[1] is not IORESOURCE_IRQ");
+		dev_dbg(&pdev->dev, "resource[1] is not IORESOURCE_IRQ\n");
 		return -ENOMEM;
 	}
 
@@ -65,14 +65,14 @@ static int usb_hcd_ep93xx_probe(const struct hc_driver *driver,
 
 	hcd->regs = ioremap(hcd->rsrc_start, hcd->rsrc_len);
 	if (hcd->regs == NULL) {
-		dbg("ioremap failed");
+		dev_dbg(&pdev->dev, "ioremap failed\n");
 		retval = -ENOMEM;
 		goto err2;
 	}
 
 	usb_host_clock = clk_get(&pdev->dev, NULL);
 	if (IS_ERR(usb_host_clock)) {
-		dbg("clk_get failed");
+		dev_dbg(&pdev->dev, "clk_get failed\n");
 		retval = PTR_ERR(usb_host_clock);
 		goto err3;
 	}
@@ -116,7 +116,8 @@ static int __devinit ohci_ep93xx_start(struct usb_hcd *hcd)
 		return ret;
 
 	if ((ret = ohci_run(ohci)) < 0) {
-		err("can't start %s", hcd->self.bus_name);
+		dev_err(hcd->self.controller, "can't start %s\n",
+			hcd->self.bus_name);
 		ohci_stop(hcd);
 		return ret;
 	}
diff --git a/drivers/usb/host/ohci-exynos.c b/drivers/usb/host/ohci-exynos.c
index 37bb20ebb6f..2909621ea19 100644
--- a/drivers/usb/host/ohci-exynos.c
+++ b/drivers/usb/host/ohci-exynos.c
@@ -35,7 +35,8 @@ static int ohci_exynos_start(struct usb_hcd *hcd)
 
 	ret = ohci_run(ohci);
 	if (ret < 0) {
-		err("can't start %s", hcd->self.bus_name);
+		dev_err(hcd->self.controller, "can't start %s\n",
+			hcd->self.bus_name);
 		ohci_stop(hcd);
 		return ret;
 	}
diff --git a/drivers/usb/host/ohci-hcd.c b/drivers/usb/host/ohci-hcd.c
index 235171f2946..e0adf5c0cf5 100644
--- a/drivers/usb/host/ohci-hcd.c
+++ b/drivers/usb/host/ohci-hcd.c
@@ -1080,11 +1080,6 @@ MODULE_LICENSE ("GPL");
 #define PS3_SYSTEM_BUS_DRIVER	ps3_ohci_driver
 #endif
 
-#ifdef CONFIG_USB_OHCI_HCD_SSB
-#include "ohci-ssb.c"
-#define SSB_OHCI_DRIVER		ssb_ohci_driver
-#endif
-
 #ifdef CONFIG_MFD_SM501
 #include "ohci-sm501.c"
 #define SM501_OHCI_DRIVER	ohci_hcd_sm501_driver
@@ -1128,8 +1123,7 @@ MODULE_LICENSE ("GPL");
 	!defined(SA1111_DRIVER) &&	\
 	!defined(PS3_SYSTEM_BUS_DRIVER) && \
 	!defined(SM501_OHCI_DRIVER) && \
-	!defined(TMIO_OHCI_DRIVER) && \
-	!defined(SSB_OHCI_DRIVER)
+	!defined(TMIO_OHCI_DRIVER)
 #error "missing bus glue for ohci-hcd"
 #endif
 
@@ -1195,12 +1189,6 @@ static int __init ohci_hcd_mod_init(void)
 		goto error_pci;
 #endif
 
-#ifdef SSB_OHCI_DRIVER
-	retval = ssb_driver_register(&SSB_OHCI_DRIVER);
-	if (retval)
-		goto error_ssb;
-#endif
-
 #ifdef SM501_OHCI_DRIVER
 	retval = platform_driver_register(&SM501_OHCI_DRIVER);
 	if (retval < 0)
@@ -1224,10 +1212,6 @@ static int __init ohci_hcd_mod_init(void)
 	platform_driver_unregister(&SM501_OHCI_DRIVER);
  error_sm501:
 #endif
-#ifdef SSB_OHCI_DRIVER
-	ssb_driver_unregister(&SSB_OHCI_DRIVER);
- error_ssb:
-#endif
 #ifdef PCI_DRIVER
 	pci_unregister_driver(&PCI_DRIVER);
  error_pci:
@@ -1275,9 +1259,6 @@ static void __exit ohci_hcd_mod_exit(void)
 #ifdef SM501_OHCI_DRIVER
 	platform_driver_unregister(&SM501_OHCI_DRIVER);
 #endif
-#ifdef SSB_OHCI_DRIVER
-	ssb_driver_unregister(&SSB_OHCI_DRIVER);
-#endif
 #ifdef PCI_DRIVER
 	pci_unregister_driver(&PCI_DRIVER);
 #endif
diff --git a/drivers/usb/host/ohci-nxp.c b/drivers/usb/host/ohci-nxp.c
index 6618de1d881..1e364ec962f 100644
--- a/drivers/usb/host/ohci-nxp.c
+++ b/drivers/usb/host/ohci-nxp.c
@@ -22,6 +22,8 @@
 #include <linux/clk.h>
 #include <linux/platform_device.h>
 #include <linux/i2c.h>
+#include <linux/of.h>
+#include <linux/usb/isp1301.h>
 
 #include <mach/hardware.h>
 #include <asm/mach-types.h>
@@ -29,7 +31,6 @@
 
 #include <mach/platform.h>
 #include <mach/irqs.h>
-#include <asm/gpio.h>
 
 #define USB_CONFIG_BASE		0x31020000
 #define PWRMAN_BASE		0x40004000
@@ -38,7 +39,9 @@
 
 /* USB_CTRL bit defines */
 #define USB_SLAVE_HCLK_EN	(1 << 24)
+#define USB_DEV_NEED_CLK_EN	(1 << 22)
 #define USB_HOST_NEED_CLK_EN	(1 << 21)
+#define PAD_CONTROL_LAST_DRIVEN	(1 << 19)
 
 #define USB_OTG_CLK_CTRL	IO_ADDRESS(USB_CONFIG_BASE + 0xFF4)
 #define USB_OTG_CLK_STAT	IO_ADDRESS(USB_CONFIG_BASE + 0xFF8)
@@ -56,54 +59,6 @@
 #define TRANSPARENT_I2C_EN	(1 << 7)
 #define HOST_EN			(1 << 0)
 
-/* ISP1301 USB transceiver I2C registers */
-#define	ISP1301_MODE_CONTROL_1		0x04	/* u8 read, set, +1 clear */
-
-#define	MC1_SPEED_REG		(1 << 0)
-#define	MC1_SUSPEND_REG		(1 << 1)
-#define	MC1_DAT_SE0		(1 << 2)
-#define	MC1_TRANSPARENT		(1 << 3)
-#define	MC1_BDIS_ACON_EN	(1 << 4)
-#define	MC1_OE_INT_EN		(1 << 5)
-#define	MC1_UART_EN		(1 << 6)
-#define	MC1_MASK		0x7f
-
-#define	ISP1301_MODE_CONTROL_2		0x12	/* u8 read, set, +1 clear */
-
-#define	MC2_GLOBAL_PWR_DN	(1 << 0)
-#define	MC2_SPD_SUSP_CTRL	(1 << 1)
-#define	MC2_BI_DI		(1 << 2)
-#define	MC2_TRANSP_BDIR0	(1 << 3)
-#define	MC2_TRANSP_BDIR1	(1 << 4)
-#define	MC2_AUDIO_EN		(1 << 5)
-#define	MC2_PSW_EN		(1 << 6)
-#define	MC2_EN2V7		(1 << 7)
-
-#define	ISP1301_OTG_CONTROL_1		0x06	/* u8 read, set, +1 clear */
-#	define	OTG1_DP_PULLUP		(1 << 0)
-#	define	OTG1_DM_PULLUP		(1 << 1)
-#	define	OTG1_DP_PULLDOWN	(1 << 2)
-#	define	OTG1_DM_PULLDOWN	(1 << 3)
-#	define	OTG1_ID_PULLDOWN	(1 << 4)
-#	define	OTG1_VBUS_DRV		(1 << 5)
-#	define	OTG1_VBUS_DISCHRG	(1 << 6)
-#	define	OTG1_VBUS_CHRG		(1 << 7)
-#define	ISP1301_OTG_STATUS		0x10	/* u8 readonly */
-#	define	OTG_B_SESS_END		(1 << 6)
-#	define	OTG_B_SESS_VLD		(1 << 7)
-
-#define ISP1301_I2C_ADDR 0x2C
-
-#define ISP1301_I2C_MODE_CONTROL_1 0x4
-#define ISP1301_I2C_MODE_CONTROL_2 0x12
-#define ISP1301_I2C_OTG_CONTROL_1 0x6
-#define ISP1301_I2C_OTG_CONTROL_2 0x10
-#define ISP1301_I2C_INTERRUPT_SOURCE 0x8
-#define ISP1301_I2C_INTERRUPT_LATCH 0xA
-#define ISP1301_I2C_INTERRUPT_FALLING 0xC
-#define ISP1301_I2C_INTERRUPT_RISING 0xE
-#define ISP1301_I2C_REG_CLEAR_ADDR 1
-
 /* On LPC32xx, those are undefined */
 #ifndef start_int_set_falling_edge
 #define start_int_set_falling_edge(irq)
@@ -113,42 +68,12 @@
 #define start_int_umask(irq)
 #endif
 
-static struct i2c_driver isp1301_driver;
 static struct i2c_client *isp1301_i2c_client;
 
 extern int usb_disabled(void);
-extern int ocpi_enable(void);
 
 static struct clk *usb_clk;
 
-static const unsigned short normal_i2c[] =
-    { ISP1301_I2C_ADDR, ISP1301_I2C_ADDR + 1, I2C_CLIENT_END };
-
-static int isp1301_probe(struct i2c_client *client,
-			 const struct i2c_device_id *id)
-{
-	return 0;
-}
-
-static int isp1301_remove(struct i2c_client *client)
-{
-	return 0;
-}
-
-static const struct i2c_device_id isp1301_id[] = {
-	{ "isp1301_nxp", 0 },
-	{ }
-};
-
-static struct i2c_driver isp1301_driver = {
-	.driver = {
-		.name = "isp1301_nxp",
-	},
-	.probe = isp1301_probe,
-	.remove = isp1301_remove,
-	.id_table = isp1301_id,
-};
-
 static void isp1301_configure_pnx4008(void)
 {
 	/* PNX4008 only supports DAT_SE0 USB mode */
@@ -220,7 +145,7 @@ static void isp1301_configure_lpc32xx(void)
 		ISP1301_I2C_INTERRUPT_RISING | ISP1301_I2C_REG_CLEAR_ADDR, ~0);
 
 	/* Enable usb_need_clk clock after transceiver is initialized */
-	__raw_writel((__raw_readl(USB_CTRL) | (1 << 22)), USB_CTRL);
+	__raw_writel(__raw_readl(USB_CTRL) | USB_HOST_NEED_CLK_EN, USB_CTRL);
 
 	printk(KERN_INFO "ISP1301 Vendor ID  : 0x%04x\n",
 	      i2c_smbus_read_word_data(isp1301_i2c_client, 0x00));
@@ -372,65 +297,55 @@ static int __devinit usb_hcd_nxp_probe(struct platform_device *pdev)
 	struct usb_hcd *hcd = 0;
 	struct ohci_hcd *ohci;
 	const struct hc_driver *driver = &ohci_nxp_hc_driver;
-	struct i2c_adapter *i2c_adap;
-	struct i2c_board_info i2c_info;
-
+	struct resource *res;
 	int ret = 0, irq;
+	struct device_node *isp1301_node;
 
-	dev_dbg(&pdev->dev, "%s: " DRIVER_DESC " (nxp)\n", hcd_name);
-	if (usb_disabled()) {
-		err("USB is disabled");
-		ret = -ENODEV;
-		goto out;
+	if (pdev->dev.of_node) {
+		isp1301_node = of_parse_phandle(pdev->dev.of_node,
+						"transceiver", 0);
+	} else {
+		isp1301_node = NULL;
 	}
 
-	if (pdev->num_resources != 2
-	    || pdev->resource[0].flags != IORESOURCE_MEM
-	    || pdev->resource[1].flags != IORESOURCE_IRQ) {
-		err("Invalid resource configuration");
-		ret = -ENODEV;
+	isp1301_i2c_client = isp1301_get_client(isp1301_node);
+	if (!isp1301_i2c_client) {
+		ret = -EPROBE_DEFER;
 		goto out;
 	}
 
-	/* Enable AHB slave USB clock, needed for further USB clock control */
-	__raw_writel(USB_SLAVE_HCLK_EN | (1 << 19), USB_CTRL);
+	pdev->dev.coherent_dma_mask = DMA_BIT_MASK(32);
+	pdev->dev.dma_mask = &pdev->dev.coherent_dma_mask;
 
-	ret = i2c_add_driver(&isp1301_driver);
-	if (ret < 0) {
-		err("failed to add ISP1301 driver");
-		goto out;
-	}
-	i2c_adap = i2c_get_adapter(2);
-	memset(&i2c_info, 0, sizeof(struct i2c_board_info));
-	strlcpy(i2c_info.type, "isp1301_nxp", I2C_NAME_SIZE);
-	isp1301_i2c_client = i2c_new_probed_device(i2c_adap, &i2c_info,
-						   normal_i2c, NULL);
-	i2c_put_adapter(i2c_adap);
-	if (!isp1301_i2c_client) {
-		err("failed to connect I2C to ISP1301 USB Transceiver");
+	dev_dbg(&pdev->dev, "%s: " DRIVER_DESC " (nxp)\n", hcd_name);
+	if (usb_disabled()) {
+		dev_err(&pdev->dev, "USB is disabled\n");
 		ret = -ENODEV;
-		goto out_i2c_driver;
+		goto out;
 	}
 
+	/* Enable AHB slave USB clock, needed for further USB clock control */
+	__raw_writel(USB_SLAVE_HCLK_EN | PAD_CONTROL_LAST_DRIVEN, USB_CTRL);
+
 	isp1301_configure();
 
 	/* Enable USB PLL */
 	usb_clk = clk_get(&pdev->dev, "ck_pll5");
 	if (IS_ERR(usb_clk)) {
-		err("failed to acquire USB PLL");
+		dev_err(&pdev->dev, "failed to acquire USB PLL\n");
 		ret = PTR_ERR(usb_clk);
 		goto out1;
 	}
 
 	ret = clk_enable(usb_clk);
 	if (ret < 0) {
-		err("failed to start USB PLL");
+		dev_err(&pdev->dev, "failed to start USB PLL\n");
 		goto out2;
 	}
 
 	ret = clk_set_rate(usb_clk, 48000);
 	if (ret < 0) {
-		err("failed to set USB clock rate");
+		dev_err(&pdev->dev, "failed to set USB clock rate\n");
 		goto out3;
 	}
 
@@ -442,9 +357,9 @@ static int __devinit usb_hcd_nxp_probe(struct platform_device *pdev)
 	while ((__raw_readl(USB_OTG_CLK_STAT) & USB_CLOCK_MASK) !=
 	       USB_CLOCK_MASK) ;
 
-	hcd = usb_create_hcd (driver, &pdev->dev, dev_name(&pdev->dev));
+	hcd = usb_create_hcd(driver, &pdev->dev, dev_name(&pdev->dev));
 	if (!hcd) {
-		err("Failed to allocate HC buffer");
+		dev_err(&pdev->dev, "Failed to allocate HC buffer\n");
 		ret = -ENOMEM;
 		goto out3;
 	}
@@ -452,14 +367,21 @@ static int __devinit usb_hcd_nxp_probe(struct platform_device *pdev)
 	/* Set all USB bits in the Start Enable register */
 	nxp_set_usb_bits();
 
-	hcd->rsrc_start = pdev->resource[0].start;
-	hcd->rsrc_len = pdev->resource[0].end - pdev->resource[0].start + 1;
-	if (!request_mem_region(hcd->rsrc_start, hcd->rsrc_len, hcd_name)) {
-		dev_dbg(&pdev->dev, "request_mem_region failed\n");
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	if (!res) {
+		dev_err(&pdev->dev, "Failed to get MEM resource\n");
+		ret =  -ENOMEM;
+		goto out4;
+	}
+
+	hcd->regs = devm_request_and_ioremap(&pdev->dev, res);
+	if (!hcd->regs) {
+		dev_err(&pdev->dev, "Failed to devm_request_and_ioremap\n");
 		ret =  -ENOMEM;
 		goto out4;
 	}
-	hcd->regs = (void __iomem *)pdev->resource[0].start;
+	hcd->rsrc_start = res->start;
+	hcd->rsrc_len = resource_size(res);
 
 	irq = platform_get_irq(pdev, 0);
 	if (irq < 0) {
@@ -486,10 +408,7 @@ out3:
 out2:
 	clk_put(usb_clk);
 out1:
-	i2c_unregister_device(isp1301_i2c_client);
 	isp1301_i2c_client = NULL;
-out_i2c_driver:
-	i2c_del_driver(&isp1301_driver);
 out:
 	return ret;
 }
@@ -507,7 +426,6 @@ static int usb_hcd_nxp_remove(struct platform_device *pdev)
 	clk_put(usb_clk);
 	i2c_unregister_device(isp1301_i2c_client);
 	isp1301_i2c_client = NULL;
-	i2c_del_driver(&isp1301_driver);
 
 	platform_set_drvdata(pdev, NULL);
 
@@ -517,10 +435,19 @@ static int usb_hcd_nxp_remove(struct platform_device *pdev)
 /* work with hotplug and coldplug */
 MODULE_ALIAS("platform:usb-ohci");
 
+#ifdef CONFIG_OF
+static const struct of_device_id usb_hcd_nxp_match[] = {
+	{ .compatible = "nxp,ohci-nxp" },
+	{},
+};
+MODULE_DEVICE_TABLE(of, usb_hcd_nxp_match);
+#endif
+
 static struct platform_driver usb_hcd_nxp_driver = {
 	.driver = {
 		.name = "usb-ohci",
 		.owner	= THIS_MODULE,
+		.of_match_table = of_match_ptr(usb_hcd_nxp_match),
 	},
 	.probe = usb_hcd_nxp_probe,
 	.remove = usb_hcd_nxp_remove,
diff --git a/drivers/usb/host/ohci-omap.c b/drivers/usb/host/ohci-omap.c
index 96451e41ee8..9ce35d0d9d5 100644
--- a/drivers/usb/host/ohci-omap.c
+++ b/drivers/usb/host/ohci-omap.c
@@ -205,8 +205,9 @@ static int ohci_omap_init(struct usb_hcd *hcd)
 	need_transceiver = need_transceiver
 			|| machine_is_omap_h2() || machine_is_omap_h3();
 
-	if (cpu_is_omap16xx())
-		ocpi_enable();
+	/* XXX OMAP16xx only */
+	if (config->ocpi_enable)
+		config->ocpi_enable();
 
 #ifdef	CONFIG_USB_OTG
 	if (need_transceiver) {
@@ -217,8 +218,7 @@ static int ohci_omap_init(struct usb_hcd *hcd)
 			dev_dbg(hcd->self.controller, "init %s transceiver, status %d\n",
 					ohci->transceiver->label, status);
 			if (status) {
-				if (ohci->transceiver)
-					put_device(ohci->transceiver->dev);
+				usb_put_transceiver(ohci->transceiver);
 				return status;
 			}
 		} else {
@@ -405,7 +405,7 @@ usb_hcd_omap_remove (struct usb_hcd *hcd, struct platform_device *pdev)
 	usb_remove_hcd(hcd);
 	if (ohci->transceiver) {
 		(void) otg_set_host(ohci->transceiver->otg, 0);
-		put_device(ohci->transceiver->dev);
+		usb_put_transceiver(ohci->transceiver);
 	}
 	if (machine_is_omap_osk())
 		gpio_free(9);
diff --git a/drivers/usb/host/ohci-platform.c b/drivers/usb/host/ohci-platform.c
index ec5c6791c8b..670c7059c9a 100644
--- a/drivers/usb/host/ohci-platform.c
+++ b/drivers/usb/host/ohci-platform.c
@@ -93,13 +93,13 @@ static int __devinit ohci_platform_probe(struct platform_device *dev)
 
 	irq = platform_get_irq(dev, 0);
 	if (irq < 0) {
-		pr_err("no irq provieded");
+		pr_err("no irq provided");
 		return irq;
 	}
 
 	res_mem = platform_get_resource(dev, IORESOURCE_MEM, 0);
 	if (!res_mem) {
-		pr_err("no memory recourse provieded");
+		pr_err("no memory recourse provided");
 		return -ENXIO;
 	}
 
diff --git a/drivers/usb/host/ohci-pnx8550.c b/drivers/usb/host/ohci-pnx8550.c
index f13d08f94d6..148d27d6a67 100644
--- a/drivers/usb/host/ohci-pnx8550.c
+++ b/drivers/usb/host/ohci-pnx8550.c
@@ -157,7 +157,8 @@ ohci_pnx8550_start (struct usb_hcd *hcd)
 		return ret;
 
 	if ((ret = ohci_run (ohci)) < 0) {
-		err ("can't start %s", hcd->self.bus_name);
+		dev_err(hcd->self.controller, "can't start %s",
+			hcd->self.bus_name);
 		ohci_stop (hcd);
 		return ret;
 	}
diff --git a/drivers/usb/host/ohci-ppc-of.c b/drivers/usb/host/ohci-ppc-of.c
index d24cc89de16..e27d5ae2b9e 100644
--- a/drivers/usb/host/ohci-ppc-of.c
+++ b/drivers/usb/host/ohci-ppc-of.c
@@ -29,7 +29,8 @@ ohci_ppc_of_start(struct usb_hcd *hcd)
 		return ret;
 
 	if ((ret = ohci_run(ohci)) < 0) {
-		err("can't start %s", ohci_to_hcd(ohci)->self.bus_name);
+		dev_err(hcd->self.controller, "can't start %s\n",
+			hcd->self.bus_name);
 		ohci_stop(hcd);
 		return ret;
 	}
@@ -236,7 +237,7 @@ MODULE_DEVICE_TABLE(of, ohci_hcd_ppc_of_match);
 
 #if	!defined(CONFIG_USB_OHCI_HCD_PPC_OF_BE) && \
 	!defined(CONFIG_USB_OHCI_HCD_PPC_OF_LE)
-#error "No endianess selected for ppc-of-ohci"
+#error "No endianness selected for ppc-of-ohci"
 #endif
 
 
diff --git a/drivers/usb/host/ohci-ppc-soc.c b/drivers/usb/host/ohci-ppc-soc.c
index 1514b706747..185c39ed81b 100644
--- a/drivers/usb/host/ohci-ppc-soc.c
+++ b/drivers/usb/host/ohci-ppc-soc.c
@@ -130,7 +130,8 @@ ohci_ppc_soc_start(struct usb_hcd *hcd)
 		return ret;
 
 	if ((ret = ohci_run(ohci)) < 0) {
-		err("can't start %s", ohci_to_hcd(ohci)->self.bus_name);
+		dev_err(hcd->self.controller, "can't start %s\n",
+			hcd->self.bus_name);
 		ohci_stop(hcd);
 		return ret;
 	}
diff --git a/drivers/usb/host/ohci-ps3.c b/drivers/usb/host/ohci-ps3.c
index 6fd4fa1f19b..2ee1d8d713d 100644
--- a/drivers/usb/host/ohci-ps3.c
+++ b/drivers/usb/host/ohci-ps3.c
@@ -45,7 +45,8 @@ static int __devinit ps3_ohci_hc_start(struct usb_hcd *hcd)
 	result = ohci_run(ohci);
 
 	if (result < 0) {
-		err("can't start %s", hcd->self.bus_name);
+		dev_err(hcd->self.controller, "can't start %s\n",
+			hcd->self.bus_name);
 		ohci_stop(hcd);
 	}
 
diff --git a/drivers/usb/host/ohci-pxa27x.c b/drivers/usb/host/ohci-pxa27x.c
index c31b2815be1..e1a3cc6d28d 100644
--- a/drivers/usb/host/ohci-pxa27x.c
+++ b/drivers/usb/host/ohci-pxa27x.c
@@ -419,7 +419,8 @@ ohci_pxa27x_start (struct usb_hcd *hcd)
 		return ret;
 
 	if ((ret = ohci_run (ohci)) < 0) {
-		err ("can't start %s", hcd->self.bus_name);
+		dev_err(hcd->self.controller, "can't start %s",
+			hcd->self.bus_name);
 		ohci_stop (hcd);
 		return ret;
 	}
diff --git a/drivers/usb/host/ohci-s3c2410.c b/drivers/usb/host/ohci-s3c2410.c
index 56dcf069246..664c869eb09 100644
--- a/drivers/usb/host/ohci-s3c2410.c
+++ b/drivers/usb/host/ohci-s3c2410.c
@@ -420,7 +420,8 @@ ohci_s3c2410_start(struct usb_hcd *hcd)
 
 	ret = ohci_run(ohci);
 	if (ret < 0) {
-		err("can't start %s", hcd->self.bus_name);
+		dev_err(hcd->self.controller, "can't start %s\n",
+			hcd->self.bus_name);
 		ohci_stop(hcd);
 		return ret;
 	}
diff --git a/drivers/usb/host/ohci-sa1111.c b/drivers/usb/host/ohci-sa1111.c
index e1004fb37bd..b6cc9252092 100644
--- a/drivers/usb/host/ohci-sa1111.c
+++ b/drivers/usb/host/ohci-sa1111.c
@@ -46,7 +46,7 @@ static void dump_hci_status(struct usb_hcd *hcd, const char *label)
 {
 	unsigned long status = sa1111_readl(hcd->regs + USB_STATUS);
 
-	dbg("%s USB_STATUS = { %s%s%s%s%s}", label,
+	printk(KERN_DEBUG "%s USB_STATUS = { %s%s%s%s%s}\n", label,
 	     ((status & USB_STATUS_IRQHCIRMTWKUP) ? "IRQHCIRMTWKUP " : ""),
 	     ((status & USB_STATUS_IRQHCIBUFFACC) ? "IRQHCIBUFFACC " : ""),
 	     ((status & USB_STATUS_NIRQHCIM) ? "" : "IRQHCIM "),
@@ -193,7 +193,7 @@ static int ohci_hcd_sa1111_probe(struct sa1111_dev *dev)
 	hcd->rsrc_len = resource_size(&dev->res);
 
 	if (!request_mem_region(hcd->rsrc_start, hcd->rsrc_len, hcd_name)) {
-		dbg("request_mem_region failed");
+		dev_dbg(&dev->dev, "request_mem_region failed\n");
 		ret = -EBUSY;
 		goto err1;
 	}
diff --git a/drivers/usb/host/ohci-sh.c b/drivers/usb/host/ohci-sh.c
index 84686d90805..76a20c27836 100644
--- a/drivers/usb/host/ohci-sh.c
+++ b/drivers/usb/host/ohci-sh.c
@@ -88,20 +88,20 @@ static int ohci_hcd_sh_probe(struct platform_device *pdev)
 
 	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
 	if (!res) {
-		err("platform_get_resource error.");
+		dev_err(&pdev->dev, "platform_get_resource error.\n");
 		return -ENODEV;
 	}
 
 	irq = platform_get_irq(pdev, 0);
 	if (irq < 0) {
-		err("platform_get_irq error.");
+		dev_err(&pdev->dev, "platform_get_irq error.\n");
 		return -ENODEV;
 	}
 
 	/* initialize hcd */
 	hcd = usb_create_hcd(&ohci_sh_hc_driver, &pdev->dev, (char *)hcd_name);
 	if (!hcd) {
-		err("Failed to create hcd");
+		dev_err(&pdev->dev, "Failed to create hcd\n");
 		return -ENOMEM;
 	}
 
@@ -110,7 +110,7 @@ static int ohci_hcd_sh_probe(struct platform_device *pdev)
 	hcd->rsrc_len = resource_size(res);
 	ret = usb_add_hcd(hcd, irq, IRQF_SHARED);
 	if (ret != 0) {
-		err("Failed to add hcd");
+		dev_err(&pdev->dev, "Failed to add hcd\n");
 		usb_put_hcd(hcd);
 		return ret;
 	}
diff --git a/drivers/usb/host/ohci-spear.c b/drivers/usb/host/ohci-spear.c
index 95c16489e88..fc7305ee3c9 100644
--- a/drivers/usb/host/ohci-spear.c
+++ b/drivers/usb/host/ohci-spear.c
@@ -14,6 +14,7 @@
 #include <linux/signal.h>
 #include <linux/platform_device.h>
 #include <linux/clk.h>
+#include <linux/of.h>
 
 struct spear_ohci {
 	struct ohci_hcd ohci;
@@ -24,12 +25,12 @@ struct spear_ohci {
 
 static void spear_start_ohci(struct spear_ohci *ohci)
 {
-	clk_enable(ohci->clk);
+	clk_prepare_enable(ohci->clk);
 }
 
 static void spear_stop_ohci(struct spear_ohci *ohci)
 {
-	clk_disable(ohci->clk);
+	clk_disable_unprepare(ohci->clk);
 }
 
 static int __devinit ohci_spear_start(struct usb_hcd *hcd)
@@ -90,6 +91,8 @@ static const struct hc_driver ohci_spear_hc_driver = {
 	.start_port_reset	= ohci_start_port_reset,
 };
 
+static u64 spear_ohci_dma_mask = DMA_BIT_MASK(32);
+
 static int spear_ohci_hcd_drv_probe(struct platform_device *pdev)
 {
 	const struct hc_driver *driver = &ohci_spear_hc_driver;
@@ -98,11 +101,8 @@ static int spear_ohci_hcd_drv_probe(struct platform_device *pdev)
 	struct spear_ohci *ohci_p;
 	struct resource *res;
 	int retval, irq;
-	int *pdata = pdev->dev.platform_data;
 	char clk_name[20] = "usbh_clk";
-
-	if (pdata == NULL)
-		return -EFAULT;
+	static int instance = -1;
 
 	irq = platform_get_irq(pdev, 0);
 	if (irq < 0) {
@@ -110,8 +110,22 @@ static int spear_ohci_hcd_drv_probe(struct platform_device *pdev)
 		goto fail_irq_get;
 	}
 
-	if (*pdata >= 0)
-		sprintf(clk_name, "usbh.%01d_clk", *pdata);
+	/*
+	 * Right now device-tree probed devices don't get dma_mask set.
+	 * Since shared usb code relies on it, set it here for now.
+	 * Once we have dma capability bindings this can go away.
+	 */
+	if (!pdev->dev.dma_mask)
+		pdev->dev.dma_mask = &spear_ohci_dma_mask;
+
+	/*
+	 * Increment the device instance, when probing via device-tree
+	 */
+	if (pdev->id < 0)
+		instance++;
+	else
+		instance = pdev->id;
+	sprintf(clk_name, "usbh.%01d_clk", instance);
 
 	usbh_clk = clk_get(NULL, clk_name);
 	if (IS_ERR(usbh_clk)) {
@@ -222,6 +236,11 @@ static int spear_ohci_hcd_drv_resume(struct platform_device *dev)
 }
 #endif
 
+static struct of_device_id spear_ohci_id_table[] __devinitdata = {
+	{ .compatible = "st,spear600-ohci", },
+	{ },
+};
+
 /* Driver definition to register with the platform bus */
 static struct platform_driver spear_ohci_hcd_driver = {
 	.probe =	spear_ohci_hcd_drv_probe,
@@ -233,6 +252,7 @@ static struct platform_driver spear_ohci_hcd_driver = {
 	.driver = {
 		.owner = THIS_MODULE,
 		.name = "spear-ohci",
+		.of_match_table = of_match_ptr(spear_ohci_id_table),
 	},
 };
 
diff --git a/drivers/usb/host/ohci-ssb.c b/drivers/usb/host/ohci-ssb.c
deleted file mode 100644
index 5ba18595d6f..00000000000
--- a/drivers/usb/host/ohci-ssb.c
+++ /dev/null
@@ -1,260 +0,0 @@
-/*
- * Sonics Silicon Backplane
- * Broadcom USB-core OHCI driver
- *
- * Copyright 2007 Michael Buesch <m@bues.ch>
- *
- * Derived from the OHCI-PCI driver
- * Copyright 1999 Roman Weissgaerber
- * Copyright 2000-2002 David Brownell
- * Copyright 1999 Linus Torvalds
- * Copyright 1999 Gregory P. Smith
- *
- * Derived from the USBcore related parts of Broadcom-SB
- * Copyright 2005 Broadcom Corporation
- *
- * Licensed under the GNU/GPL. See COPYING for details.
- */
-#include <linux/ssb/ssb.h>
-
-
-#define SSB_OHCI_TMSLOW_HOSTMODE	(1 << 29)
-
-struct ssb_ohci_device {
-	struct ohci_hcd ohci; /* _must_ be at the beginning. */
-
-	u32 enable_flags;
-};
-
-static inline
-struct ssb_ohci_device *hcd_to_ssb_ohci(struct usb_hcd *hcd)
-{
-	return (struct ssb_ohci_device *)(hcd->hcd_priv);
-}
-
-
-static int ssb_ohci_reset(struct usb_hcd *hcd)
-{
-	struct ssb_ohci_device *ohcidev = hcd_to_ssb_ohci(hcd);
-	struct ohci_hcd *ohci = &ohcidev->ohci;
-	int err;
-
-	ohci_hcd_init(ohci);
-	err = ohci_init(ohci);
-
-	return err;
-}
-
-static int ssb_ohci_start(struct usb_hcd *hcd)
-{
-	struct ssb_ohci_device *ohcidev = hcd_to_ssb_ohci(hcd);
-	struct ohci_hcd *ohci = &ohcidev->ohci;
-	int err;
-
-	err = ohci_run(ohci);
-	if (err < 0) {
-		ohci_err(ohci, "can't start\n");
-		ohci_stop(hcd);
-	}
-
-	return err;
-}
-
-static const struct hc_driver ssb_ohci_hc_driver = {
-	.description		= "ssb-usb-ohci",
-	.product_desc		= "SSB OHCI Controller",
-	.hcd_priv_size		= sizeof(struct ssb_ohci_device),
-
-	.irq			= ohci_irq,
-	.flags			= HCD_MEMORY | HCD_USB11,
-
-	.reset			= ssb_ohci_reset,
-	.start			= ssb_ohci_start,
-	.stop			= ohci_stop,
-	.shutdown		= ohci_shutdown,
-
-	.urb_enqueue		= ohci_urb_enqueue,
-	.urb_dequeue		= ohci_urb_dequeue,
-	.endpoint_disable	= ohci_endpoint_disable,
-
-	.get_frame_number	= ohci_get_frame,
-
-	.hub_status_data	= ohci_hub_status_data,
-	.hub_control		= ohci_hub_control,
-#ifdef	CONFIG_PM
-	.bus_suspend		= ohci_bus_suspend,
-	.bus_resume		= ohci_bus_resume,
-#endif
-
-	.start_port_reset	= ohci_start_port_reset,
-};
-
-static void ssb_ohci_detach(struct ssb_device *dev)
-{
-	struct usb_hcd *hcd = ssb_get_drvdata(dev);
-
-	if (hcd->driver->shutdown)
-		hcd->driver->shutdown(hcd);
-	usb_remove_hcd(hcd);
-	iounmap(hcd->regs);
-	release_mem_region(hcd->rsrc_start, hcd->rsrc_len);
-	usb_put_hcd(hcd);
-	ssb_device_disable(dev, 0);
-}
-
-static int ssb_ohci_attach(struct ssb_device *dev)
-{
-	struct ssb_ohci_device *ohcidev;
-	struct usb_hcd *hcd;
-	int err = -ENOMEM;
-	u32 tmp, flags = 0;
-
-	if (dma_set_mask(dev->dma_dev, DMA_BIT_MASK(32)) ||
-	    dma_set_coherent_mask(dev->dma_dev, DMA_BIT_MASK(32)))
-		return -EOPNOTSUPP;
-
-	if (dev->id.coreid == SSB_DEV_USB11_HOSTDEV) {
-		/* Put the device into host-mode. */
-		flags |= SSB_OHCI_TMSLOW_HOSTMODE;
-		ssb_device_enable(dev, flags);
-	} else if (dev->id.coreid == SSB_DEV_USB20_HOST) {
-		/*
-		 * USB 2.0 special considerations:
-		 *
-		 * In addition to the standard SSB reset sequence, the Host
-		 * Control Register must be programmed to bring the USB core
-		 * and various phy components out of reset.
-		 */
-		ssb_device_enable(dev, 0);
-		ssb_write32(dev, 0x200, 0x7ff);
-
-		/* Change Flush control reg */
-		tmp = ssb_read32(dev, 0x400);
-		tmp &= ~8;
-		ssb_write32(dev, 0x400, tmp);
-		tmp = ssb_read32(dev, 0x400);
-
-		/* Change Shim control reg */
-		tmp = ssb_read32(dev, 0x304);
-		tmp &= ~0x100;
-		ssb_write32(dev, 0x304, tmp);
-		tmp = ssb_read32(dev, 0x304);
-
-		udelay(1);
-
-		/* Work around for 5354 failures */
-		if (dev->id.revision == 2 && dev->bus->chip_id == 0x5354) {
-			/* Change syn01 reg */
-			tmp = 0x00fe00fe;
-			ssb_write32(dev, 0x894, tmp);
-
-			/* Change syn03 reg */
-			tmp = ssb_read32(dev, 0x89c);
-			tmp |= 0x1;
-			ssb_write32(dev, 0x89c, tmp);
-		}
-	} else
-		ssb_device_enable(dev, 0);
-
-	hcd = usb_create_hcd(&ssb_ohci_hc_driver, dev->dev,
-			dev_name(dev->dev));
-	if (!hcd)
-		goto err_dev_disable;
-	ohcidev = hcd_to_ssb_ohci(hcd);
-	ohcidev->enable_flags = flags;
-
-	tmp = ssb_read32(dev, SSB_ADMATCH0);
-	hcd->rsrc_start = ssb_admatch_base(tmp);
-	hcd->rsrc_len = ssb_admatch_size(tmp);
-	hcd->regs = ioremap_nocache(hcd->rsrc_start, hcd->rsrc_len);
-	if (!hcd->regs)
-		goto err_put_hcd;
-	err = usb_add_hcd(hcd, dev->irq, IRQF_SHARED);
-	if (err)
-		goto err_iounmap;
-
-	ssb_set_drvdata(dev, hcd);
-
-	return err;
-
-err_iounmap:
-	iounmap(hcd->regs);
-err_put_hcd:
-	usb_put_hcd(hcd);
-err_dev_disable:
-	ssb_device_disable(dev, flags);
-	return err;
-}
-
-static int ssb_ohci_probe(struct ssb_device *dev,
-		const struct ssb_device_id *id)
-{
-	int err;
-	u16 chipid_top;
-
-	/* USBcores are only connected on embedded devices. */
-	chipid_top = (dev->bus->chip_id & 0xFF00);
-	if (chipid_top != 0x4700 && chipid_top != 0x5300)
-		return -ENODEV;
-
-	/* TODO: Probably need checks here; is the core connected? */
-
-	if (usb_disabled())
-		return -ENODEV;
-
-	/* We currently always attach SSB_DEV_USB11_HOSTDEV
-	 * as HOST OHCI. If we want to attach it as Client device,
-	 * we must branch here and call into the (yet to
-	 * be written) Client mode driver. Same for remove(). */
-
-	err = ssb_ohci_attach(dev);
-
-	return err;
-}
-
-static void ssb_ohci_remove(struct ssb_device *dev)
-{
-	ssb_ohci_detach(dev);
-}
-
-#ifdef CONFIG_PM
-
-static int ssb_ohci_suspend(struct ssb_device *dev, pm_message_t state)
-{
-	ssb_device_disable(dev, 0);
-
-	return 0;
-}
-
-static int ssb_ohci_resume(struct ssb_device *dev)
-{
-	struct usb_hcd *hcd = ssb_get_drvdata(dev);
-	struct ssb_ohci_device *ohcidev = hcd_to_ssb_ohci(hcd);
-
-	ssb_device_enable(dev, ohcidev->enable_flags);
-
-	ohci_finish_controller_resume(hcd);
-	return 0;
-}
-
-#else /* !CONFIG_PM */
-#define ssb_ohci_suspend	NULL
-#define ssb_ohci_resume	NULL
-#endif /* CONFIG_PM */
-
-static const struct ssb_device_id ssb_ohci_table[] = {
-	SSB_DEVICE(SSB_VENDOR_BROADCOM, SSB_DEV_USB11_HOSTDEV, SSB_ANY_REV),
-	SSB_DEVICE(SSB_VENDOR_BROADCOM, SSB_DEV_USB11_HOST, SSB_ANY_REV),
-	SSB_DEVICE(SSB_VENDOR_BROADCOM, SSB_DEV_USB20_HOST, SSB_ANY_REV),
-	SSB_DEVTABLE_END
-};
-MODULE_DEVICE_TABLE(ssb, ssb_ohci_table);
-
-static struct ssb_driver ssb_ohci_driver = {
-	.name		= KBUILD_MODNAME,
-	.id_table	= ssb_ohci_table,
-	.probe		= ssb_ohci_probe,
-	.remove		= ssb_ohci_remove,
-	.suspend	= ssb_ohci_suspend,
-	.resume		= ssb_ohci_resume,
-};
diff --git a/drivers/usb/host/ohci-tmio.c b/drivers/usb/host/ohci-tmio.c
index 120bfe6ede3..60c2b0722f2 100644
--- a/drivers/usb/host/ohci-tmio.c
+++ b/drivers/usb/host/ohci-tmio.c
@@ -140,7 +140,8 @@ static int ohci_tmio_start(struct usb_hcd *hcd)
 		return ret;
 
 	if ((ret = ohci_run(ohci)) < 0) {
-		err("can't start %s", hcd->self.bus_name);
+		dev_err(hcd->self.controller, "can't start %s\n",
+			hcd->self.bus_name);
 		ohci_stop(hcd);
 		return ret;
 	}
diff --git a/drivers/usb/host/ohci-xls.c b/drivers/usb/host/ohci-xls.c
index a2247867af8..41e378f17c6 100644
--- a/drivers/usb/host/ohci-xls.c
+++ b/drivers/usb/host/ohci-xls.c
@@ -88,7 +88,8 @@ static int __devinit ohci_xls_start(struct usb_hcd *hcd)
 	ohci = hcd_to_ohci(hcd);
 	ret = ohci_run(ohci);
 	if (ret < 0) {
-		err("can't start %s", hcd->self.bus_name);
+		dev_err(hcd->self.controller, "can't start %s\n",
+			hcd->self.bus_name);
 		ohci_stop(hcd);
 		return ret;
 	}
diff --git a/drivers/usb/host/oxu210hp-hcd.c b/drivers/usb/host/oxu210hp-hcd.c
index 3b38030b02a..4f0f0339532 100644
--- a/drivers/usb/host/oxu210hp-hcd.c
+++ b/drivers/usb/host/oxu210hp-hcd.c
@@ -1399,8 +1399,8 @@ static struct ehci_qh *qh_make(struct oxu_hcd *oxu,
 				 * But interval 1 scheduling is simpler, and
 				 * includes high bandwidth.
 				 */
-				dbg("intr period %d uframes, NYET!",
-						urb->interval);
+				oxu_dbg(oxu, "intr period %d uframes, NYET!\n",
+					urb->interval);
 				goto done;
 			}
 		} else {
@@ -1471,7 +1471,7 @@ static struct ehci_qh *qh_make(struct oxu_hcd *oxu,
 		}
 		break;
 	default:
-		dbg("bogus dev %p speed %d", urb->dev, urb->dev->speed);
+		oxu_dbg(oxu, "bogus dev %p speed %d\n", urb->dev, urb->dev->speed);
 done:
 		qh_put(qh);
 		return NULL;
@@ -2307,7 +2307,7 @@ restart:
 				qh_put(temp.qh);
 				break;
 			default:
-				dbg("corrupt type %d frame %d shadow %p",
+				oxu_dbg(oxu, "corrupt type %d frame %d shadow %p\n",
 					type, frame, q.ptr);
 				q.ptr = NULL;
 			}
@@ -2991,8 +2991,9 @@ static int oxu_urb_dequeue(struct usb_hcd *hcd, struct urb *urb, int status)
 				/* shouldn't happen often, but ...
 				 * FIXME kill those tds' urbs
 				 */
-				err("can't reschedule qh %p, err %d",
-					qh, status);
+				dev_err(hcd->self.controller,
+					"can't reschedule qh %p, err %d\n", qh,
+					status);
 			}
 			return status;
 		}
diff --git a/drivers/usb/host/pci-quirks.c b/drivers/usb/host/pci-quirks.c
index 32dada8c8b4..df0828cb2aa 100644
--- a/drivers/usb/host/pci-quirks.c
+++ b/drivers/usb/host/pci-quirks.c
@@ -9,6 +9,7 @@
  */
 
 #include <linux/types.h>
+#include <linux/kconfig.h>
 #include <linux/kernel.h>
 #include <linux/pci.h>
 #include <linux/init.h>
@@ -712,12 +713,28 @@ static int handshake(void __iomem *ptr, u32 mask, u32 done,
 	return -ETIMEDOUT;
 }
 
-bool usb_is_intel_switchable_xhci(struct pci_dev *pdev)
+#define PCI_DEVICE_ID_INTEL_LYNX_POINT_XHCI	0x8C31
+
+bool usb_is_intel_ppt_switchable_xhci(struct pci_dev *pdev)
 {
 	return pdev->class == PCI_CLASS_SERIAL_USB_XHCI &&
 		pdev->vendor == PCI_VENDOR_ID_INTEL &&
 		pdev->device == PCI_DEVICE_ID_INTEL_PANTHERPOINT_XHCI;
 }
+
+/* The Intel Lynx Point chipset also has switchable ports. */
+bool usb_is_intel_lpt_switchable_xhci(struct pci_dev *pdev)
+{
+	return pdev->class == PCI_CLASS_SERIAL_USB_XHCI &&
+		pdev->vendor == PCI_VENDOR_ID_INTEL &&
+		pdev->device == PCI_DEVICE_ID_INTEL_LYNX_POINT_XHCI;
+}
+
+bool usb_is_intel_switchable_xhci(struct pci_dev *pdev)
+{
+	return usb_is_intel_ppt_switchable_xhci(pdev) ||
+		usb_is_intel_lpt_switchable_xhci(pdev);
+}
 EXPORT_SYMBOL_GPL(usb_is_intel_switchable_xhci);
 
 /*
@@ -742,6 +759,19 @@ void usb_enable_xhci_ports(struct pci_dev *xhci_pdev)
 {
 	u32		ports_available;
 
+	/* Don't switchover the ports if the user hasn't compiled the xHCI
+	 * driver.  Otherwise they will see "dead" USB ports that don't power
+	 * the devices.
+	 */
+	if (!IS_ENABLED(CONFIG_USB_XHCI_HCD)) {
+		dev_warn(&xhci_pdev->dev,
+				"CONFIG_USB_XHCI_HCD is turned off, "
+				"defaulting to EHCI.\n");
+		dev_warn(&xhci_pdev->dev,
+				"USB 3.0 devices will work at USB 2.0 speeds.\n");
+		return;
+	}
+
 	ports_available = 0xffffffff;
 	/* Write USB3_PSSEN, the USB 3.0 Port SuperSpeed Enable
 	 * Register, to turn on SuperSpeed terminations for all
diff --git a/drivers/usb/host/r8a66597-hcd.c b/drivers/usb/host/r8a66597-hcd.c
index 2bf1320dc9c..c868be65e76 100644
--- a/drivers/usb/host/r8a66597-hcd.c
+++ b/drivers/usb/host/r8a66597-hcd.c
@@ -401,7 +401,7 @@ static u8 alloc_usb_address(struct r8a66597 *r8a66597, struct urb *urb)
 		if (r8a66597->address_map & (1 << addr))
 			continue;
 
-		dbg("alloc_address: r8a66597_addr=%d", addr);
+		dev_dbg(&urb->dev->dev, "alloc_address: r8a66597_addr=%d\n", addr);
 		r8a66597->address_map |= 1 << addr;
 
 		if (make_r8a66597_device(r8a66597, urb, addr) < 0)
@@ -426,7 +426,7 @@ static void free_usb_address(struct r8a66597 *r8a66597,
 	if (!dev)
 		return;
 
-	dbg("free_addr: addr=%d", dev->address);
+	dev_dbg(&dev->udev->dev, "free_addr: addr=%d\n", dev->address);
 
 	dev->state = USB_STATE_DEFAULT;
 	r8a66597->address_map &= ~(1 << dev->address);
@@ -819,7 +819,7 @@ static void enable_r8a66597_pipe(struct r8a66597 *r8a66597, struct urb *urb,
 	struct r8a66597_device *dev = get_urb_to_r8a66597_dev(r8a66597, urb);
 	struct r8a66597_pipe *pipe = hep->hcpriv;
 
-	dbg("enable_pipe:");
+	dev_dbg(&dev->udev->dev, "enable_pipe:\n");
 
 	pipe->info = *info;
 	set_pipe_reg_addr(pipe, R8A66597_PIPE_NO_DMA);
@@ -898,7 +898,7 @@ static void disable_r8a66597_pipe_all(struct r8a66597 *r8a66597,
 		force_dequeue(r8a66597, pipenum, dev->address);
 	}
 
-	dbg("disable_pipe");
+	dev_dbg(&dev->udev->dev, "disable_pipe\n");
 
 	r8a66597->dma_map &= ~(dev->dma_map);
 	dev->dma_map = 0;
@@ -2264,7 +2264,7 @@ static int r8a66597_bus_suspend(struct usb_hcd *hcd)
 	struct r8a66597 *r8a66597 = hcd_to_r8a66597(hcd);
 	int port;
 
-	dbg("%s", __func__);
+	dev_dbg(&r8a66597->device0.udev->dev, "%s\n", __func__);
 
 	for (port = 0; port < r8a66597->max_root_hub; port++) {
 		struct r8a66597_root_hub *rh = &r8a66597->root_hub[port];
@@ -2273,7 +2273,7 @@ static int r8a66597_bus_suspend(struct usb_hcd *hcd)
 		if (!(rh->port & USB_PORT_STAT_ENABLE))
 			continue;
 
-		dbg("suspend port = %d", port);
+		dev_dbg(&rh->dev->udev->dev, "suspend port = %d\n", port);
 		r8a66597_bclr(r8a66597, UACT, dvstctr_reg);	/* suspend */
 		rh->port |= USB_PORT_STAT_SUSPEND;
 
@@ -2295,7 +2295,7 @@ static int r8a66597_bus_resume(struct usb_hcd *hcd)
 	struct r8a66597 *r8a66597 = hcd_to_r8a66597(hcd);
 	int port;
 
-	dbg("%s", __func__);
+	dev_dbg(&r8a66597->device0.udev->dev, "%s\n", __func__);
 
 	for (port = 0; port < r8a66597->max_root_hub; port++) {
 		struct r8a66597_root_hub *rh = &r8a66597->root_hub[port];
@@ -2304,7 +2304,7 @@ static int r8a66597_bus_resume(struct usb_hcd *hcd)
 		if (!(rh->port & USB_PORT_STAT_SUSPEND))
 			continue;
 
-		dbg("resume port = %d", port);
+		dev_dbg(&rh->dev->udev->dev, "resume port = %d\n", port);
 		rh->port &= ~USB_PORT_STAT_SUSPEND;
 		rh->port |= USB_PORT_STAT_C_SUSPEND << 16;
 		r8a66597_mdfy(r8a66597, RESUME, RESUME | UACT, dvstctr_reg);
@@ -2360,7 +2360,7 @@ static int r8a66597_suspend(struct device *dev)
 	struct r8a66597		*r8a66597 = dev_get_drvdata(dev);
 	int port;
 
-	dbg("%s", __func__);
+	dev_dbg(dev, "%s\n", __func__);
 
 	disable_controller(r8a66597);
 
@@ -2378,7 +2378,7 @@ static int r8a66597_resume(struct device *dev)
 	struct r8a66597		*r8a66597 = dev_get_drvdata(dev);
 	struct usb_hcd		*hcd = r8a66597_to_hcd(r8a66597);
 
-	dbg("%s", __func__);
+	dev_dbg(dev, "%s\n", __func__);
 
 	enable_controller(r8a66597);
 	usb_root_hub_lost_power(hcd->self.root_hub);
diff --git a/drivers/usb/host/ssb-hcd.c b/drivers/usb/host/ssb-hcd.c
new file mode 100644
index 00000000000..c2a29faba07
--- /dev/null
+++ b/drivers/usb/host/ssb-hcd.c
@@ -0,0 +1,280 @@
+/*
+ * Sonics Silicon Backplane
+ * Broadcom USB-core driver  (SSB bus glue)
+ *
+ * Copyright 2011-2012 Hauke Mehrtens <hauke@hauke-m.de>
+ *
+ * Based on ssb-ohci driver
+ * Copyright 2007 Michael Buesch <m@bues.ch>
+ *
+ * Derived from the OHCI-PCI driver
+ * Copyright 1999 Roman Weissgaerber
+ * Copyright 2000-2002 David Brownell
+ * Copyright 1999 Linus Torvalds
+ * Copyright 1999 Gregory P. Smith
+ *
+ * Derived from the USBcore related parts of Broadcom-SB
+ * Copyright 2005-2011 Broadcom Corporation
+ *
+ * Licensed under the GNU/GPL. See COPYING for details.
+ */
+#include <linux/ssb/ssb.h>
+#include <linux/delay.h>
+#include <linux/platform_device.h>
+#include <linux/module.h>
+#include <linux/slab.h>
+#include <linux/usb/ehci_pdriver.h>
+#include <linux/usb/ohci_pdriver.h>
+
+MODULE_AUTHOR("Hauke Mehrtens");
+MODULE_DESCRIPTION("Common USB driver for SSB Bus");
+MODULE_LICENSE("GPL");
+
+#define SSB_HCD_TMSLOW_HOSTMODE	(1 << 29)
+
+struct ssb_hcd_device {
+	struct platform_device *ehci_dev;
+	struct platform_device *ohci_dev;
+
+	u32 enable_flags;
+};
+
+static void __devinit ssb_hcd_5354wa(struct ssb_device *dev)
+{
+#ifdef CONFIG_SSB_DRIVER_MIPS
+	/* Work around for 5354 failures */
+	if (dev->id.revision == 2 && dev->bus->chip_id == 0x5354) {
+		/* Change syn01 reg */
+		ssb_write32(dev, 0x894, 0x00fe00fe);
+
+		/* Change syn03 reg */
+		ssb_write32(dev, 0x89c, ssb_read32(dev, 0x89c) | 0x1);
+	}
+#endif
+}
+
+static void __devinit ssb_hcd_usb20wa(struct ssb_device *dev)
+{
+	if (dev->id.coreid == SSB_DEV_USB20_HOST) {
+		/*
+		 * USB 2.0 special considerations:
+		 *
+		 * In addition to the standard SSB reset sequence, the Host
+		 * Control Register must be programmed to bring the USB core
+		 * and various phy components out of reset.
+		 */
+		ssb_write32(dev, 0x200, 0x7ff);
+
+		/* Change Flush control reg */
+		ssb_write32(dev, 0x400, ssb_read32(dev, 0x400) & ~8);
+		ssb_read32(dev, 0x400);
+
+		/* Change Shim control reg */
+		ssb_write32(dev, 0x304, ssb_read32(dev, 0x304) & ~0x100);
+		ssb_read32(dev, 0x304);
+
+		udelay(1);
+
+		ssb_hcd_5354wa(dev);
+	}
+}
+
+/* based on arch/mips/brcm-boards/bcm947xx/pcibios.c */
+static u32 __devinit ssb_hcd_init_chip(struct ssb_device *dev)
+{
+	u32 flags = 0;
+
+	if (dev->id.coreid == SSB_DEV_USB11_HOSTDEV)
+		/* Put the device into host-mode. */
+		flags |= SSB_HCD_TMSLOW_HOSTMODE;
+
+	ssb_device_enable(dev, flags);
+
+	ssb_hcd_usb20wa(dev);
+
+	return flags;
+}
+
+static const struct usb_ehci_pdata ehci_pdata = {
+};
+
+static const struct usb_ohci_pdata ohci_pdata = {
+};
+
+static struct platform_device * __devinit
+ssb_hcd_create_pdev(struct ssb_device *dev, bool ohci, u32 addr, u32 len)
+{
+	struct platform_device *hci_dev;
+	struct resource hci_res[2];
+	int ret = -ENOMEM;
+
+	memset(hci_res, 0, sizeof(hci_res));
+
+	hci_res[0].start = addr;
+	hci_res[0].end = hci_res[0].start + len - 1;
+	hci_res[0].flags = IORESOURCE_MEM;
+
+	hci_res[1].start = dev->irq;
+	hci_res[1].flags = IORESOURCE_IRQ;
+
+	hci_dev = platform_device_alloc(ohci ? "ohci-platform" :
+					"ehci-platform" , 0);
+	if (!hci_dev)
+		return NULL;
+
+	hci_dev->dev.parent = dev->dev;
+	hci_dev->dev.dma_mask = &hci_dev->dev.coherent_dma_mask;
+
+	ret = platform_device_add_resources(hci_dev, hci_res,
+					    ARRAY_SIZE(hci_res));
+	if (ret)
+		goto err_alloc;
+	if (ohci)
+		ret = platform_device_add_data(hci_dev, &ohci_pdata,
+					       sizeof(ohci_pdata));
+	else
+		ret = platform_device_add_data(hci_dev, &ehci_pdata,
+					       sizeof(ehci_pdata));
+	if (ret)
+		goto err_alloc;
+	ret = platform_device_add(hci_dev);
+	if (ret)
+		goto err_alloc;
+
+	return hci_dev;
+
+err_alloc:
+	platform_device_put(hci_dev);
+	return ERR_PTR(ret);
+}
+
+static int __devinit ssb_hcd_probe(struct ssb_device *dev,
+				   const struct ssb_device_id *id)
+{
+	int err, tmp;
+	int start, len;
+	u16 chipid_top;
+	u16 coreid = dev->id.coreid;
+	struct ssb_hcd_device *usb_dev;
+
+	/* USBcores are only connected on embedded devices. */
+	chipid_top = (dev->bus->chip_id & 0xFF00);
+	if (chipid_top != 0x4700 && chipid_top != 0x5300)
+		return -ENODEV;
+
+	/* TODO: Probably need checks here; is the core connected? */
+
+	if (dma_set_mask(dev->dma_dev, DMA_BIT_MASK(32)) ||
+	    dma_set_coherent_mask(dev->dma_dev, DMA_BIT_MASK(32)))
+		return -EOPNOTSUPP;
+
+	usb_dev = kzalloc(sizeof(struct ssb_hcd_device), GFP_KERNEL);
+	if (!usb_dev)
+		return -ENOMEM;
+
+	/* We currently always attach SSB_DEV_USB11_HOSTDEV
+	 * as HOST OHCI. If we want to attach it as Client device,
+	 * we must branch here and call into the (yet to
+	 * be written) Client mode driver. Same for remove(). */
+	usb_dev->enable_flags = ssb_hcd_init_chip(dev);
+
+	tmp = ssb_read32(dev, SSB_ADMATCH0);
+
+	start = ssb_admatch_base(tmp);
+	len = (coreid == SSB_DEV_USB20_HOST) ? 0x800 : ssb_admatch_size(tmp);
+	usb_dev->ohci_dev = ssb_hcd_create_pdev(dev, true, start, len);
+	if (IS_ERR(usb_dev->ohci_dev)) {
+		err = PTR_ERR(usb_dev->ohci_dev);
+		goto err_free_usb_dev;
+	}
+
+	if (coreid == SSB_DEV_USB20_HOST) {
+		start = ssb_admatch_base(tmp) + 0x800; /* ehci core offset */
+		usb_dev->ehci_dev = ssb_hcd_create_pdev(dev, false, start, len);
+		if (IS_ERR(usb_dev->ehci_dev)) {
+			err = PTR_ERR(usb_dev->ehci_dev);
+			goto err_unregister_ohci_dev;
+		}
+	}
+
+	ssb_set_drvdata(dev, usb_dev);
+	return 0;
+
+err_unregister_ohci_dev:
+	platform_device_unregister(usb_dev->ohci_dev);
+err_free_usb_dev:
+	kfree(usb_dev);
+	return err;
+}
+
+static void __devexit ssb_hcd_remove(struct ssb_device *dev)
+{
+	struct ssb_hcd_device *usb_dev = ssb_get_drvdata(dev);
+	struct platform_device *ohci_dev = usb_dev->ohci_dev;
+	struct platform_device *ehci_dev = usb_dev->ehci_dev;
+
+	if (ohci_dev)
+		platform_device_unregister(ohci_dev);
+	if (ehci_dev)
+		platform_device_unregister(ehci_dev);
+
+	ssb_device_disable(dev, 0);
+}
+
+static void __devexit ssb_hcd_shutdown(struct ssb_device *dev)
+{
+	ssb_device_disable(dev, 0);
+}
+
+#ifdef CONFIG_PM
+
+static int ssb_hcd_suspend(struct ssb_device *dev, pm_message_t state)
+{
+	ssb_device_disable(dev, 0);
+
+	return 0;
+}
+
+static int ssb_hcd_resume(struct ssb_device *dev)
+{
+	struct ssb_hcd_device *usb_dev = ssb_get_drvdata(dev);
+
+	ssb_device_enable(dev, usb_dev->enable_flags);
+
+	return 0;
+}
+
+#else /* !CONFIG_PM */
+#define ssb_hcd_suspend	NULL
+#define ssb_hcd_resume	NULL
+#endif /* CONFIG_PM */
+
+static const struct ssb_device_id ssb_hcd_table[] __devinitconst = {
+	SSB_DEVICE(SSB_VENDOR_BROADCOM, SSB_DEV_USB11_HOSTDEV, SSB_ANY_REV),
+	SSB_DEVICE(SSB_VENDOR_BROADCOM, SSB_DEV_USB11_HOST, SSB_ANY_REV),
+	SSB_DEVICE(SSB_VENDOR_BROADCOM, SSB_DEV_USB20_HOST, SSB_ANY_REV),
+	SSB_DEVTABLE_END
+};
+MODULE_DEVICE_TABLE(ssb, ssb_hcd_table);
+
+static struct ssb_driver ssb_hcd_driver = {
+	.name		= KBUILD_MODNAME,
+	.id_table	= ssb_hcd_table,
+	.probe		= ssb_hcd_probe,
+	.remove		= __devexit_p(ssb_hcd_remove),
+	.shutdown	= ssb_hcd_shutdown,
+	.suspend	= ssb_hcd_suspend,
+	.resume		= ssb_hcd_resume,
+};
+
+static int __init ssb_hcd_init(void)
+{
+	return ssb_driver_register(&ssb_hcd_driver);
+}
+module_init(ssb_hcd_init);
+
+static void __exit ssb_hcd_exit(void)
+{
+	ssb_driver_unregister(&ssb_hcd_driver);
+}
+module_exit(ssb_hcd_exit);
diff --git a/drivers/usb/host/xhci-hub.c b/drivers/usb/host/xhci-hub.c
index 673ad120c43..2732ef660c5 100644
--- a/drivers/usb/host/xhci-hub.c
+++ b/drivers/usb/host/xhci-hub.c
@@ -475,6 +475,7 @@ int xhci_hub_control(struct usb_hcd *hcd, u16 typeReq, u16 wValue,
 	struct xhci_bus_state *bus_state;
 	u16 link_state = 0;
 	u16 wake_mask = 0;
+	u16 timeout = 0;
 
 	max_ports = xhci_get_ports(hcd, &port_array);
 	bus_state = &xhci->bus_state[hcd_index(hcd)];
@@ -558,6 +559,7 @@ int xhci_hub_control(struct usb_hcd *hcd, u16 typeReq, u16 wValue,
 				xhci_dbg(xhci, "Resume USB2 port %d\n",
 					wIndex + 1);
 				bus_state->resume_done[wIndex] = 0;
+				clear_bit(wIndex, &bus_state->resuming_ports);
 				xhci_set_link_state(xhci, port_array, wIndex,
 							XDEV_U0);
 				xhci_dbg(xhci, "set port %d resume\n",
@@ -622,6 +624,8 @@ int xhci_hub_control(struct usb_hcd *hcd, u16 typeReq, u16 wValue,
 			link_state = (wIndex & 0xff00) >> 3;
 		if (wValue == USB_PORT_FEAT_REMOTE_WAKE_MASK)
 			wake_mask = wIndex & 0xff00;
+		/* The MSB of wIndex is the U1/U2 timeout */
+		timeout = (wIndex & 0xff00) >> 8;
 		wIndex &= 0xff;
 		if (!wIndex || wIndex > max_ports)
 			goto error;
@@ -746,6 +750,22 @@ int xhci_hub_control(struct usb_hcd *hcd, u16 typeReq, u16 wValue,
 
 			temp = xhci_readl(xhci, port_array[wIndex]);
 			break;
+		case USB_PORT_FEAT_U1_TIMEOUT:
+			if (hcd->speed != HCD_USB3)
+				goto error;
+			temp = xhci_readl(xhci, port_array[wIndex] + 1);
+			temp &= ~PORT_U1_TIMEOUT_MASK;
+			temp |= PORT_U1_TIMEOUT(timeout);
+			xhci_writel(xhci, temp, port_array[wIndex] + 1);
+			break;
+		case USB_PORT_FEAT_U2_TIMEOUT:
+			if (hcd->speed != HCD_USB3)
+				goto error;
+			temp = xhci_readl(xhci, port_array[wIndex] + 1);
+			temp &= ~PORT_U2_TIMEOUT_MASK;
+			temp |= PORT_U2_TIMEOUT(timeout);
+			xhci_writel(xhci, temp, port_array[wIndex] + 1);
+			break;
 		default:
 			goto error;
 		}
@@ -845,7 +865,12 @@ int xhci_hub_status_data(struct usb_hcd *hcd, char *buf)
 	/* Initial status is no changes */
 	retval = (max_ports + 8) / 8;
 	memset(buf, 0, retval);
-	status = 0;
+
+	/*
+	 * Inform the usbcore about resume-in-progress by returning
+	 * a non-zero value even if there are no status changes.
+	 */
+	status = bus_state->resuming_ports;
 
 	mask = PORT_CSC | PORT_PEC | PORT_OCC | PORT_PLC | PORT_WRC;
 
@@ -885,15 +910,11 @@ int xhci_bus_suspend(struct usb_hcd *hcd)
 	spin_lock_irqsave(&xhci->lock, flags);
 
 	if (hcd->self.root_hub->do_remote_wakeup) {
-		port_index = max_ports;
-		while (port_index--) {
-			if (bus_state->resume_done[port_index] != 0) {
-				spin_unlock_irqrestore(&xhci->lock, flags);
-				xhci_dbg(xhci, "suspend failed because "
-						"port %d is resuming\n",
-						port_index + 1);
-				return -EBUSY;
-			}
+		if (bus_state->resuming_ports) {
+			spin_unlock_irqrestore(&xhci->lock, flags);
+			xhci_dbg(xhci, "suspend failed because "
+						"a port is resuming\n");
+			return -EBUSY;
 		}
 	}
 
diff --git a/drivers/usb/host/xhci-mem.c b/drivers/usb/host/xhci-mem.c
index 68eaa908ac8..ec4338eec82 100644
--- a/drivers/usb/host/xhci-mem.c
+++ b/drivers/usb/host/xhci-mem.c
@@ -1791,6 +1791,14 @@ void xhci_mem_cleanup(struct xhci_hcd *xhci)
 {
 	struct pci_dev	*pdev = to_pci_dev(xhci_to_hcd(xhci)->self.controller);
 	struct dev_info	*dev_info, *next;
+	struct list_head *tt_list_head;
+	struct list_head *tt;
+	struct list_head *endpoints;
+	struct list_head *ep, *q;
+	struct xhci_tt_bw_info *tt_info;
+	struct xhci_interval_bw_table *bwt;
+	struct xhci_virt_ep *virt_ep;
+
 	unsigned long	flags;
 	int size;
 	int i;
@@ -1807,6 +1815,9 @@ void xhci_mem_cleanup(struct xhci_hcd *xhci)
 	xhci->event_ring = NULL;
 	xhci_dbg(xhci, "Freed event ring\n");
 
+	if (xhci->lpm_command)
+		xhci_free_command(xhci, xhci->lpm_command);
+	xhci->cmd_ring_reserved_trbs = 0;
 	if (xhci->cmd_ring)
 		xhci_ring_free(xhci, xhci->cmd_ring);
 	xhci->cmd_ring = NULL;
@@ -1849,8 +1860,26 @@ void xhci_mem_cleanup(struct xhci_hcd *xhci)
 	}
 	spin_unlock_irqrestore(&xhci->lock, flags);
 
+	bwt = &xhci->rh_bw->bw_table;
+	for (i = 0; i < XHCI_MAX_INTERVAL; i++) {
+		endpoints = &bwt->interval_bw[i].endpoints;
+		list_for_each_safe(ep, q, endpoints) {
+			virt_ep = list_entry(ep, struct xhci_virt_ep, bw_endpoint_list);
+			list_del(&virt_ep->bw_endpoint_list);
+			kfree(virt_ep);
+		}
+	}
+
+	tt_list_head = &xhci->rh_bw->tts;
+	list_for_each_safe(tt, q, tt_list_head) {
+		tt_info = list_entry(tt, struct xhci_tt_bw_info, tt_list);
+		list_del(tt);
+		kfree(tt_info);
+	}
+
 	xhci->num_usb2_ports = 0;
 	xhci->num_usb3_ports = 0;
+	xhci->num_active_eps = 0;
 	kfree(xhci->usb2_ports);
 	kfree(xhci->usb3_ports);
 	kfree(xhci->port_array);
@@ -2350,6 +2379,16 @@ int xhci_mem_init(struct xhci_hcd *xhci, gfp_t flags)
 	xhci_write_64(xhci, val_64, &xhci->op_regs->cmd_ring);
 	xhci_dbg_cmd_ptrs(xhci);
 
+	xhci->lpm_command = xhci_alloc_command(xhci, true, true, flags);
+	if (!xhci->lpm_command)
+		goto fail;
+
+	/* Reserve one command ring TRB for disabling LPM.
+	 * Since the USB core grabs the shared usb_bus bandwidth mutex before
+	 * disabling LPM, we only need to reserve one TRB for all devices.
+	 */
+	xhci->cmd_ring_reserved_trbs++;
+
 	val = xhci_readl(xhci, &xhci->cap_regs->db_off);
 	val &= DBOFF_MASK;
 	xhci_dbg(xhci, "// Doorbell array is located at offset 0x%x"
diff --git a/drivers/usb/host/xhci-pci.c b/drivers/usb/host/xhci-pci.c
index 7a856a767e7..18b231b0c5d 100644
--- a/drivers/usb/host/xhci-pci.c
+++ b/drivers/usb/host/xhci-pci.c
@@ -72,6 +72,7 @@ static void xhci_pci_quirks(struct device *dev, struct xhci_hcd *xhci)
 		xhci_dbg(xhci, "QUIRK: Fresco Logic revision %u "
 				"has broken MSI implementation\n",
 				pdev->revision);
+		xhci->quirks |= XHCI_TRUST_TX_LENGTH;
 	}
 
 	if (pdev->vendor == PCI_VENDOR_ID_NEC)
@@ -83,6 +84,10 @@ static void xhci_pci_quirks(struct device *dev, struct xhci_hcd *xhci)
 	/* AMD PLL quirk */
 	if (pdev->vendor == PCI_VENDOR_ID_AMD && usb_amd_find_chipset_info())
 		xhci->quirks |= XHCI_AMD_PLL_FIX;
+	if (pdev->vendor == PCI_VENDOR_ID_INTEL) {
+		xhci->quirks |= XHCI_LPM_SUPPORT;
+		xhci->quirks |= XHCI_INTEL_HOST;
+	}
 	if (pdev->vendor == PCI_VENDOR_ID_INTEL &&
 			pdev->device == PCI_DEVICE_ID_INTEL_PANTHERPOINT_XHCI) {
 		xhci->quirks |= XHCI_SPURIOUS_SUCCESS;
@@ -169,6 +174,13 @@ static int xhci_pci_probe(struct pci_dev *dev, const struct pci_device_id *id)
 	if (retval)
 		goto put_usb3_hcd;
 	/* Roothub already marked as USB 3.0 speed */
+
+	/* We know the LPM timeout algorithms for this host, let the USB core
+	 * enable and disable LPM for devices under the USB 3.0 roothub.
+	 */
+	if (xhci->quirks & XHCI_LPM_SUPPORT)
+		hcd_to_bus(xhci->shared_hcd)->root_hub->lpm_capable = 1;
+
 	return 0;
 
 put_usb3_hcd:
@@ -292,6 +304,8 @@ static const struct hc_driver xhci_pci_hc_driver = {
 	 */
 	.update_device =        xhci_update_device,
 	.set_usb2_hw_lpm =	xhci_set_usb2_hardware_lpm,
+	.enable_usb3_lpm_timeout =	xhci_enable_usb3_lpm_timeout,
+	.disable_usb3_lpm_timeout =	xhci_disable_usb3_lpm_timeout,
 };
 
 /*-------------------------------------------------------------------------*/
diff --git a/drivers/usb/host/xhci-ring.c b/drivers/usb/host/xhci-ring.c
index 3d9422f16a2..23b4aefd103 100644
--- a/drivers/usb/host/xhci-ring.c
+++ b/drivers/usb/host/xhci-ring.c
@@ -1377,6 +1377,7 @@ static void handle_port_status(struct xhci_hcd *xhci,
 			xhci_dbg(xhci, "resume HS port %d\n", port_id);
 			bus_state->resume_done[faked_port_index] = jiffies +
 				msecs_to_jiffies(20);
+			set_bit(faked_port_index, &bus_state->resuming_ports);
 			mod_timer(&hcd->rh_timer,
 				  bus_state->resume_done[faked_port_index]);
 			/* Do the rest in GetPortStatus */
@@ -1786,8 +1787,12 @@ static int process_isoc_td(struct xhci_hcd *xhci, struct xhci_td *td,
 	/* handle completion code */
 	switch (trb_comp_code) {
 	case COMP_SUCCESS:
-		frame->status = 0;
-		break;
+		if (TRB_LEN(le32_to_cpu(event->transfer_len)) == 0) {
+			frame->status = 0;
+			break;
+		}
+		if ((xhci->quirks & XHCI_TRUST_TX_LENGTH))
+			trb_comp_code = COMP_SHORT_TX;
 	case COMP_SHORT_TX:
 		frame->status = td->urb->transfer_flags & URB_SHORT_NOT_OK ?
 				-EREMOTEIO : 0;
@@ -1803,6 +1808,7 @@ static int process_isoc_td(struct xhci_hcd *xhci, struct xhci_td *td,
 		break;
 	case COMP_DEV_ERR:
 	case COMP_STALL:
+	case COMP_TX_ERR:
 		frame->status = -EPROTO;
 		skip_td = true;
 		break;
@@ -1883,13 +1889,16 @@ static int process_bulk_intr_td(struct xhci_hcd *xhci, struct xhci_td *td,
 	switch (trb_comp_code) {
 	case COMP_SUCCESS:
 		/* Double check that the HW transferred everything. */
-		if (event_trb != td->last_trb) {
+		if (event_trb != td->last_trb ||
+				TRB_LEN(le32_to_cpu(event->transfer_len)) != 0) {
 			xhci_warn(xhci, "WARN Successful completion "
 					"on short TX\n");
 			if (td->urb->transfer_flags & URB_SHORT_NOT_OK)
 				*status = -EREMOTEIO;
 			else
 				*status = 0;
+			if ((xhci->quirks & XHCI_TRUST_TX_LENGTH))
+				trb_comp_code = COMP_SHORT_TX;
 		} else {
 			*status = 0;
 		}
@@ -2048,6 +2057,13 @@ static int handle_tx_event(struct xhci_hcd *xhci,
 	 * transfer type
 	 */
 	case COMP_SUCCESS:
+		if (TRB_LEN(le32_to_cpu(event->transfer_len)) == 0)
+			break;
+		if (xhci->quirks & XHCI_TRUST_TX_LENGTH)
+			trb_comp_code = COMP_SHORT_TX;
+		else
+			xhci_warn(xhci, "WARN Successful completion on short TX: "
+					"needs XHCI_TRUST_TX_LENGTH quirk?\n");
 	case COMP_SHORT_TX:
 		break;
 	case COMP_STOP:
@@ -2270,7 +2286,7 @@ cleanup:
 					(status != 0 &&
 					 !usb_endpoint_xfer_isoc(&urb->ep->desc)))
 				xhci_dbg(xhci, "Giveback URB %p, len = %d, "
-						"expected = %x, status = %d\n",
+						"expected = %d, status = %d\n",
 						urb, urb->actual_length,
 						urb->transfer_buffer_length,
 						status);
@@ -3593,12 +3609,12 @@ int xhci_queue_configure_endpoint(struct xhci_hcd *xhci, dma_addr_t in_ctx_ptr,
 
 /* Queue an evaluate context command TRB */
 int xhci_queue_evaluate_context(struct xhci_hcd *xhci, dma_addr_t in_ctx_ptr,
-		u32 slot_id)
+		u32 slot_id, bool command_must_succeed)
 {
 	return queue_command(xhci, lower_32_bits(in_ctx_ptr),
 			upper_32_bits(in_ctx_ptr), 0,
 			TRB_TYPE(TRB_EVAL_CONTEXT) | SLOT_ID_FOR_TRB(slot_id),
-			false);
+			command_must_succeed);
 }
 
 /*
diff --git a/drivers/usb/host/xhci.c b/drivers/usb/host/xhci.c
index 36641a7f237..afdc73ee84a 100644
--- a/drivers/usb/host/xhci.c
+++ b/drivers/usb/host/xhci.c
@@ -152,7 +152,7 @@ int xhci_reset(struct xhci_hcd *xhci)
 {
 	u32 command;
 	u32 state;
-	int ret;
+	int ret, i;
 
 	state = xhci_readl(xhci, &xhci->op_regs->status);
 	if ((state & STS_HALT) == 0) {
@@ -175,7 +175,15 @@ int xhci_reset(struct xhci_hcd *xhci)
 	 * xHCI cannot write to any doorbells or operational registers other
 	 * than status until the "Controller Not Ready" flag is cleared.
 	 */
-	return handshake(xhci, &xhci->op_regs->status, STS_CNR, 0, 250 * 1000);
+	ret = handshake(xhci, &xhci->op_regs->status, STS_CNR, 0, 250 * 1000);
+
+	for (i = 0; i < 2; ++i) {
+		xhci->bus_state[i].port_c_suspend = 0;
+		xhci->bus_state[i].suspended_ports = 0;
+		xhci->bus_state[i].resuming_ports = 0;
+	}
+
+	return ret;
 }
 
 #ifdef CONFIG_PCI
@@ -2438,7 +2446,7 @@ static int xhci_configure_endpoint(struct xhci_hcd *xhci,
 				udev->slot_id, must_succeed);
 	else
 		ret = xhci_queue_evaluate_context(xhci, in_ctx->dma,
-				udev->slot_id);
+				udev->slot_id, must_succeed);
 	if (ret < 0) {
 		if (command)
 			list_del(&command->cmd_list);
@@ -3863,6 +3871,474 @@ int xhci_update_device(struct usb_hcd *hcd, struct usb_device *udev)
 
 #endif /* CONFIG_USB_SUSPEND */
 
+/*---------------------- USB 3.0 Link PM functions ------------------------*/
+
+#ifdef CONFIG_PM
+/* Service interval in nanoseconds = 2^(bInterval - 1) * 125us * 1000ns / 1us */
+static unsigned long long xhci_service_interval_to_ns(
+		struct usb_endpoint_descriptor *desc)
+{
+	return (1 << (desc->bInterval - 1)) * 125 * 1000;
+}
+
+static u16 xhci_get_timeout_no_hub_lpm(struct usb_device *udev,
+		enum usb3_link_state state)
+{
+	unsigned long long sel;
+	unsigned long long pel;
+	unsigned int max_sel_pel;
+	char *state_name;
+
+	switch (state) {
+	case USB3_LPM_U1:
+		/* Convert SEL and PEL stored in nanoseconds to microseconds */
+		sel = DIV_ROUND_UP(udev->u1_params.sel, 1000);
+		pel = DIV_ROUND_UP(udev->u1_params.pel, 1000);
+		max_sel_pel = USB3_LPM_MAX_U1_SEL_PEL;
+		state_name = "U1";
+		break;
+	case USB3_LPM_U2:
+		sel = DIV_ROUND_UP(udev->u2_params.sel, 1000);
+		pel = DIV_ROUND_UP(udev->u2_params.pel, 1000);
+		max_sel_pel = USB3_LPM_MAX_U2_SEL_PEL;
+		state_name = "U2";
+		break;
+	default:
+		dev_warn(&udev->dev, "%s: Can't get timeout for non-U1 or U2 state.\n",
+				__func__);
+		return -EINVAL;
+	}
+
+	if (sel <= max_sel_pel && pel <= max_sel_pel)
+		return USB3_LPM_DEVICE_INITIATED;
+
+	if (sel > max_sel_pel)
+		dev_dbg(&udev->dev, "Device-initiated %s disabled "
+				"due to long SEL %llu ms\n",
+				state_name, sel);
+	else
+		dev_dbg(&udev->dev, "Device-initiated %s disabled "
+				"due to long PEL %llu\n ms",
+				state_name, pel);
+	return USB3_LPM_DISABLED;
+}
+
+/* Returns the hub-encoded U1 timeout value.
+ * The U1 timeout should be the maximum of the following values:
+ *  - For control endpoints, U1 system exit latency (SEL) * 3
+ *  - For bulk endpoints, U1 SEL * 5
+ *  - For interrupt endpoints:
+ *    - Notification EPs, U1 SEL * 3
+ *    - Periodic EPs, max(105% of bInterval, U1 SEL * 2)
+ *  - For isochronous endpoints, max(105% of bInterval, U1 SEL * 2)
+ */
+static u16 xhci_calculate_intel_u1_timeout(struct usb_device *udev,
+		struct usb_endpoint_descriptor *desc)
+{
+	unsigned long long timeout_ns;
+	int ep_type;
+	int intr_type;
+
+	ep_type = usb_endpoint_type(desc);
+	switch (ep_type) {
+	case USB_ENDPOINT_XFER_CONTROL:
+		timeout_ns = udev->u1_params.sel * 3;
+		break;
+	case USB_ENDPOINT_XFER_BULK:
+		timeout_ns = udev->u1_params.sel * 5;
+		break;
+	case USB_ENDPOINT_XFER_INT:
+		intr_type = usb_endpoint_interrupt_type(desc);
+		if (intr_type == USB_ENDPOINT_INTR_NOTIFICATION) {
+			timeout_ns = udev->u1_params.sel * 3;
+			break;
+		}
+		/* Otherwise the calculation is the same as isoc eps */
+	case USB_ENDPOINT_XFER_ISOC:
+		timeout_ns = xhci_service_interval_to_ns(desc);
+		timeout_ns = DIV_ROUND_UP_ULL(timeout_ns * 105, 100);
+		if (timeout_ns < udev->u1_params.sel * 2)
+			timeout_ns = udev->u1_params.sel * 2;
+		break;
+	default:
+		return 0;
+	}
+
+	/* The U1 timeout is encoded in 1us intervals. */
+	timeout_ns = DIV_ROUND_UP_ULL(timeout_ns, 1000);
+	/* Don't return a timeout of zero, because that's USB3_LPM_DISABLED. */
+	if (timeout_ns == USB3_LPM_DISABLED)
+		timeout_ns++;
+
+	/* If the necessary timeout value is bigger than what we can set in the
+	 * USB 3.0 hub, we have to disable hub-initiated U1.
+	 */
+	if (timeout_ns <= USB3_LPM_U1_MAX_TIMEOUT)
+		return timeout_ns;
+	dev_dbg(&udev->dev, "Hub-initiated U1 disabled "
+			"due to long timeout %llu ms\n", timeout_ns);
+	return xhci_get_timeout_no_hub_lpm(udev, USB3_LPM_U1);
+}
+
+/* Returns the hub-encoded U2 timeout value.
+ * The U2 timeout should be the maximum of:
+ *  - 10 ms (to avoid the bandwidth impact on the scheduler)
+ *  - largest bInterval of any active periodic endpoint (to avoid going
+ *    into lower power link states between intervals).
+ *  - the U2 Exit Latency of the device
+ */
+static u16 xhci_calculate_intel_u2_timeout(struct usb_device *udev,
+		struct usb_endpoint_descriptor *desc)
+{
+	unsigned long long timeout_ns;
+	unsigned long long u2_del_ns;
+
+	timeout_ns = 10 * 1000 * 1000;
+
+	if ((usb_endpoint_xfer_int(desc) || usb_endpoint_xfer_isoc(desc)) &&
+			(xhci_service_interval_to_ns(desc) > timeout_ns))
+		timeout_ns = xhci_service_interval_to_ns(desc);
+
+	u2_del_ns = udev->bos->ss_cap->bU2DevExitLat * 1000;
+	if (u2_del_ns > timeout_ns)
+		timeout_ns = u2_del_ns;
+
+	/* The U2 timeout is encoded in 256us intervals */
+	timeout_ns = DIV_ROUND_UP_ULL(timeout_ns, 256 * 1000);
+	/* If the necessary timeout value is bigger than what we can set in the
+	 * USB 3.0 hub, we have to disable hub-initiated U2.
+	 */
+	if (timeout_ns <= USB3_LPM_U2_MAX_TIMEOUT)
+		return timeout_ns;
+	dev_dbg(&udev->dev, "Hub-initiated U2 disabled "
+			"due to long timeout %llu ms\n", timeout_ns);
+	return xhci_get_timeout_no_hub_lpm(udev, USB3_LPM_U2);
+}
+
+static u16 xhci_call_host_update_timeout_for_endpoint(struct xhci_hcd *xhci,
+		struct usb_device *udev,
+		struct usb_endpoint_descriptor *desc,
+		enum usb3_link_state state,
+		u16 *timeout)
+{
+	if (state == USB3_LPM_U1) {
+		if (xhci->quirks & XHCI_INTEL_HOST)
+			return xhci_calculate_intel_u1_timeout(udev, desc);
+	} else {
+		if (xhci->quirks & XHCI_INTEL_HOST)
+			return xhci_calculate_intel_u2_timeout(udev, desc);
+	}
+
+	return USB3_LPM_DISABLED;
+}
+
+static int xhci_update_timeout_for_endpoint(struct xhci_hcd *xhci,
+		struct usb_device *udev,
+		struct usb_endpoint_descriptor *desc,
+		enum usb3_link_state state,
+		u16 *timeout)
+{
+	u16 alt_timeout;
+
+	alt_timeout = xhci_call_host_update_timeout_for_endpoint(xhci, udev,
+		desc, state, timeout);
+
+	/* If we found we can't enable hub-initiated LPM, or
+	 * the U1 or U2 exit latency was too high to allow
+	 * device-initiated LPM as well, just stop searching.
+	 */
+	if (alt_timeout == USB3_LPM_DISABLED ||
+			alt_timeout == USB3_LPM_DEVICE_INITIATED) {
+		*timeout = alt_timeout;
+		return -E2BIG;
+	}
+	if (alt_timeout > *timeout)
+		*timeout = alt_timeout;
+	return 0;
+}
+
+static int xhci_update_timeout_for_interface(struct xhci_hcd *xhci,
+		struct usb_device *udev,
+		struct usb_host_interface *alt,
+		enum usb3_link_state state,
+		u16 *timeout)
+{
+	int j;
+
+	for (j = 0; j < alt->desc.bNumEndpoints; j++) {
+		if (xhci_update_timeout_for_endpoint(xhci, udev,
+					&alt->endpoint[j].desc, state, timeout))
+			return -E2BIG;
+		continue;
+	}
+	return 0;
+}
+
+static int xhci_check_intel_tier_policy(struct usb_device *udev,
+		enum usb3_link_state state)
+{
+	struct usb_device *parent;
+	unsigned int num_hubs;
+
+	if (state == USB3_LPM_U2)
+		return 0;
+
+	/* Don't enable U1 if the device is on a 2nd tier hub or lower. */
+	for (parent = udev->parent, num_hubs = 0; parent->parent;
+			parent = parent->parent)
+		num_hubs++;
+
+	if (num_hubs < 2)
+		return 0;
+
+	dev_dbg(&udev->dev, "Disabling U1 link state for device"
+			" below second-tier hub.\n");
+	dev_dbg(&udev->dev, "Plug device into first-tier hub "
+			"to decrease power consumption.\n");
+	return -E2BIG;
+}
+
+static int xhci_check_tier_policy(struct xhci_hcd *xhci,
+		struct usb_device *udev,
+		enum usb3_link_state state)
+{
+	if (xhci->quirks & XHCI_INTEL_HOST)
+		return xhci_check_intel_tier_policy(udev, state);
+	return -EINVAL;
+}
+
+/* Returns the U1 or U2 timeout that should be enabled.
+ * If the tier check or timeout setting functions return with a non-zero exit
+ * code, that means the timeout value has been finalized and we shouldn't look
+ * at any more endpoints.
+ */
+static u16 xhci_calculate_lpm_timeout(struct usb_hcd *hcd,
+			struct usb_device *udev, enum usb3_link_state state)
+{
+	struct xhci_hcd *xhci = hcd_to_xhci(hcd);
+	struct usb_host_config *config;
+	char *state_name;
+	int i;
+	u16 timeout = USB3_LPM_DISABLED;
+
+	if (state == USB3_LPM_U1)
+		state_name = "U1";
+	else if (state == USB3_LPM_U2)
+		state_name = "U2";
+	else {
+		dev_warn(&udev->dev, "Can't enable unknown link state %i\n",
+				state);
+		return timeout;
+	}
+
+	if (xhci_check_tier_policy(xhci, udev, state) < 0)
+		return timeout;
+
+	/* Gather some information about the currently installed configuration
+	 * and alternate interface settings.
+	 */
+	if (xhci_update_timeout_for_endpoint(xhci, udev, &udev->ep0.desc,
+			state, &timeout))
+		return timeout;
+
+	config = udev->actconfig;
+	if (!config)
+		return timeout;
+
+	for (i = 0; i < USB_MAXINTERFACES; i++) {
+		struct usb_driver *driver;
+		struct usb_interface *intf = config->interface[i];
+
+		if (!intf)
+			continue;
+
+		/* Check if any currently bound drivers want hub-initiated LPM
+		 * disabled.
+		 */
+		if (intf->dev.driver) {
+			driver = to_usb_driver(intf->dev.driver);
+			if (driver && driver->disable_hub_initiated_lpm) {
+				dev_dbg(&udev->dev, "Hub-initiated %s disabled "
+						"at request of driver %s\n",
+						state_name, driver->name);
+				return xhci_get_timeout_no_hub_lpm(udev, state);
+			}
+		}
+
+		/* Not sure how this could happen... */
+		if (!intf->cur_altsetting)
+			continue;
+
+		if (xhci_update_timeout_for_interface(xhci, udev,
+					intf->cur_altsetting,
+					state, &timeout))
+			return timeout;
+	}
+	return timeout;
+}
+
+/*
+ * Issue an Evaluate Context command to change the Maximum Exit Latency in the
+ * slot context.  If that succeeds, store the new MEL in the xhci_virt_device.
+ */
+static int xhci_change_max_exit_latency(struct xhci_hcd *xhci,
+			struct usb_device *udev, u16 max_exit_latency)
+{
+	struct xhci_virt_device *virt_dev;
+	struct xhci_command *command;
+	struct xhci_input_control_ctx *ctrl_ctx;
+	struct xhci_slot_ctx *slot_ctx;
+	unsigned long flags;
+	int ret;
+
+	spin_lock_irqsave(&xhci->lock, flags);
+	if (max_exit_latency == xhci->devs[udev->slot_id]->current_mel) {
+		spin_unlock_irqrestore(&xhci->lock, flags);
+		return 0;
+	}
+
+	/* Attempt to issue an Evaluate Context command to change the MEL. */
+	virt_dev = xhci->devs[udev->slot_id];
+	command = xhci->lpm_command;
+	xhci_slot_copy(xhci, command->in_ctx, virt_dev->out_ctx);
+	spin_unlock_irqrestore(&xhci->lock, flags);
+
+	ctrl_ctx = xhci_get_input_control_ctx(xhci, command->in_ctx);
+	ctrl_ctx->add_flags |= cpu_to_le32(SLOT_FLAG);
+	slot_ctx = xhci_get_slot_ctx(xhci, command->in_ctx);
+	slot_ctx->dev_info2 &= cpu_to_le32(~((u32) MAX_EXIT));
+	slot_ctx->dev_info2 |= cpu_to_le32(max_exit_latency);
+
+	xhci_dbg(xhci, "Set up evaluate context for LPM MEL change.\n");
+	xhci_dbg(xhci, "Slot %u Input Context:\n", udev->slot_id);
+	xhci_dbg_ctx(xhci, command->in_ctx, 0);
+
+	/* Issue and wait for the evaluate context command. */
+	ret = xhci_configure_endpoint(xhci, udev, command,
+			true, true);
+	xhci_dbg(xhci, "Slot %u Output Context:\n", udev->slot_id);
+	xhci_dbg_ctx(xhci, virt_dev->out_ctx, 0);
+
+	if (!ret) {
+		spin_lock_irqsave(&xhci->lock, flags);
+		virt_dev->current_mel = max_exit_latency;
+		spin_unlock_irqrestore(&xhci->lock, flags);
+	}
+	return ret;
+}
+
+static int calculate_max_exit_latency(struct usb_device *udev,
+		enum usb3_link_state state_changed,
+		u16 hub_encoded_timeout)
+{
+	unsigned long long u1_mel_us = 0;
+	unsigned long long u2_mel_us = 0;
+	unsigned long long mel_us = 0;
+	bool disabling_u1;
+	bool disabling_u2;
+	bool enabling_u1;
+	bool enabling_u2;
+
+	disabling_u1 = (state_changed == USB3_LPM_U1 &&
+			hub_encoded_timeout == USB3_LPM_DISABLED);
+	disabling_u2 = (state_changed == USB3_LPM_U2 &&
+			hub_encoded_timeout == USB3_LPM_DISABLED);
+
+	enabling_u1 = (state_changed == USB3_LPM_U1 &&
+			hub_encoded_timeout != USB3_LPM_DISABLED);
+	enabling_u2 = (state_changed == USB3_LPM_U2 &&
+			hub_encoded_timeout != USB3_LPM_DISABLED);
+
+	/* If U1 was already enabled and we're not disabling it,
+	 * or we're going to enable U1, account for the U1 max exit latency.
+	 */
+	if ((udev->u1_params.timeout != USB3_LPM_DISABLED && !disabling_u1) ||
+			enabling_u1)
+		u1_mel_us = DIV_ROUND_UP(udev->u1_params.mel, 1000);
+	if ((udev->u2_params.timeout != USB3_LPM_DISABLED && !disabling_u2) ||
+			enabling_u2)
+		u2_mel_us = DIV_ROUND_UP(udev->u2_params.mel, 1000);
+
+	if (u1_mel_us > u2_mel_us)
+		mel_us = u1_mel_us;
+	else
+		mel_us = u2_mel_us;
+	/* xHCI host controller max exit latency field is only 16 bits wide. */
+	if (mel_us > MAX_EXIT) {
+		dev_warn(&udev->dev, "Link PM max exit latency of %lluus "
+				"is too big.\n", mel_us);
+		return -E2BIG;
+	}
+	return mel_us;
+}
+
+/* Returns the USB3 hub-encoded value for the U1/U2 timeout. */
+int xhci_enable_usb3_lpm_timeout(struct usb_hcd *hcd,
+			struct usb_device *udev, enum usb3_link_state state)
+{
+	struct xhci_hcd	*xhci;
+	u16 hub_encoded_timeout;
+	int mel;
+	int ret;
+
+	xhci = hcd_to_xhci(hcd);
+	/* The LPM timeout values are pretty host-controller specific, so don't
+	 * enable hub-initiated timeouts unless the vendor has provided
+	 * information about their timeout algorithm.
+	 */
+	if (!xhci || !(xhci->quirks & XHCI_LPM_SUPPORT) ||
+			!xhci->devs[udev->slot_id])
+		return USB3_LPM_DISABLED;
+
+	hub_encoded_timeout = xhci_calculate_lpm_timeout(hcd, udev, state);
+	mel = calculate_max_exit_latency(udev, state, hub_encoded_timeout);
+	if (mel < 0) {
+		/* Max Exit Latency is too big, disable LPM. */
+		hub_encoded_timeout = USB3_LPM_DISABLED;
+		mel = 0;
+	}
+
+	ret = xhci_change_max_exit_latency(xhci, udev, mel);
+	if (ret)
+		return ret;
+	return hub_encoded_timeout;
+}
+
+int xhci_disable_usb3_lpm_timeout(struct usb_hcd *hcd,
+			struct usb_device *udev, enum usb3_link_state state)
+{
+	struct xhci_hcd	*xhci;
+	u16 mel;
+	int ret;
+
+	xhci = hcd_to_xhci(hcd);
+	if (!xhci || !(xhci->quirks & XHCI_LPM_SUPPORT) ||
+			!xhci->devs[udev->slot_id])
+		return 0;
+
+	mel = calculate_max_exit_latency(udev, state, USB3_LPM_DISABLED);
+	ret = xhci_change_max_exit_latency(xhci, udev, mel);
+	if (ret)
+		return ret;
+	return 0;
+}
+#else /* CONFIG_PM */
+
+int xhci_enable_usb3_lpm_timeout(struct usb_hcd *hcd,
+			struct usb_device *udev, enum usb3_link_state state)
+{
+	return USB3_LPM_DISABLED;
+}
+
+int xhci_disable_usb3_lpm_timeout(struct usb_hcd *hcd,
+			struct usb_device *udev, enum usb3_link_state state)
+{
+	return 0;
+}
+#endif	/* CONFIG_PM */
+
+/*-------------------------------------------------------------------------*/
+
 /* Once a hub descriptor is fetched for a device, we need to update the xHC's
  * internal data structures for the device.
  */
@@ -4090,7 +4566,6 @@ static int __init xhci_hcd_init(void)
 	BUILD_BUG_ON(sizeof(struct xhci_intr_reg) != 8*32/8);
 	/* xhci_run_regs has eight fields and embeds 128 xhci_intr_regs */
 	BUILD_BUG_ON(sizeof(struct xhci_run_regs) != (8+8*128)*32/8);
-	BUILD_BUG_ON(sizeof(struct xhci_doorbell_array) != 256*32/8);
 	return 0;
 unreg_pci:
 	xhci_unregister_pci();
diff --git a/drivers/usb/host/xhci.h b/drivers/usb/host/xhci.h
index 3d69c4b2b54..de3d6e3e57b 100644
--- a/drivers/usb/host/xhci.h
+++ b/drivers/usb/host/xhci.h
@@ -362,8 +362,10 @@ struct xhci_op_regs {
  * Timeout can be up to 127us.  0xFF means an infinite timeout.
  */
 #define PORT_U1_TIMEOUT(p)	((p) & 0xff)
+#define PORT_U1_TIMEOUT_MASK	0xff
 /* Inactivity timer value for transitions into U2 */
 #define PORT_U2_TIMEOUT(p)	(((p) & 0xff) << 8)
+#define PORT_U2_TIMEOUT_MASK	(0xff << 8)
 /* Bits 24:31 for port testing */
 
 /* USB2 Protocol PORTSPMSC */
@@ -914,6 +916,8 @@ struct xhci_virt_device {
 	u8				real_port;
 	struct xhci_interval_bw_table	*bw_table;
 	struct xhci_tt_bw_info		*tt_info;
+	/* The current max exit latency for the enabled USB3 link states. */
+	u16				current_mel;
 };
 
 /*
@@ -1362,6 +1366,8 @@ struct xhci_bus_state {
 	u32			suspended_ports;
 	u32			port_remote_wakeup;
 	unsigned long		resume_done[USB_MAXCHILDREN];
+	/* which ports have started to resume */
+	unsigned long		resuming_ports;
 };
 
 static inline unsigned int hcd_index(struct usb_hcd *hcd)
@@ -1422,6 +1428,8 @@ struct xhci_hcd {
 	/* slot enabling and address device helpers */
 	struct completion	addr_dev;
 	int slot_id;
+	/* For USB 3.0 LPM enable/disable. */
+	struct xhci_command		*lpm_command;
 	/* Internal mirror of the HW's dcbaa */
 	struct xhci_virt_device	*devs[MAX_HC_SLOTS];
 	/* For keeping track of bandwidth domains per roothub. */
@@ -1479,6 +1487,9 @@ struct xhci_hcd {
 #define XHCI_RESET_ON_RESUME	(1 << 7)
 #define	XHCI_SW_BW_CHECKING	(1 << 8)
 #define XHCI_AMD_0x96_HOST	(1 << 9)
+#define XHCI_TRUST_TX_LENGTH	(1 << 10)
+#define XHCI_LPM_SUPPORT	(1 << 11)
+#define XHCI_INTEL_HOST		(1 << 12)
 	unsigned int		num_active_eps;
 	unsigned int		limit_active_eps;
 	/* There are two roothubs to keep track of bus suspend info for */
@@ -1752,7 +1763,7 @@ int xhci_queue_isoc_tx_prepare(struct xhci_hcd *xhci, gfp_t mem_flags,
 int xhci_queue_configure_endpoint(struct xhci_hcd *xhci, dma_addr_t in_ctx_ptr,
 		u32 slot_id, bool command_must_succeed);
 int xhci_queue_evaluate_context(struct xhci_hcd *xhci, dma_addr_t in_ctx_ptr,
-		u32 slot_id);
+		u32 slot_id, bool command_must_succeed);
 int xhci_queue_reset_ep(struct xhci_hcd *xhci, int slot_id,
 		unsigned int ep_index);
 int xhci_queue_reset_device(struct xhci_hcd *xhci, u32 slot_id);
@@ -1776,6 +1787,10 @@ void xhci_ring_ep_doorbell(struct xhci_hcd *xhci, unsigned int slot_id,
 /* xHCI roothub code */
 void xhci_set_link_state(struct xhci_hcd *xhci, __le32 __iomem **port_array,
 				int port_id, u32 link_state);
+int xhci_enable_usb3_lpm_timeout(struct usb_hcd *hcd,
+			struct usb_device *udev, enum usb3_link_state state);
+int xhci_disable_usb3_lpm_timeout(struct usb_hcd *hcd,
+			struct usb_device *udev, enum usb3_link_state state);
 void xhci_test_and_clear_bit(struct xhci_hcd *xhci, __le32 __iomem **port_array,
 				int port_id, u32 port_bit);
 int xhci_hub_control(struct usb_hcd *hcd, u16 typeReq, u16 wValue, u16 wIndex,
diff --git a/drivers/usb/image/mdc800.c b/drivers/usb/image/mdc800.c
index 575b56c79e9..7121b50098d 100644
--- a/drivers/usb/image/mdc800.c
+++ b/drivers/usb/image/mdc800.c
@@ -284,18 +284,16 @@ static void mdc800_usb_irq (struct urb *urb)
 	int data_received=0, wake_up;
 	unsigned char* b=urb->transfer_buffer;
 	struct mdc800_data* mdc800=urb->context;
+	struct device *dev = &mdc800->dev->dev;
 	int status = urb->status;
 
 	if (status >= 0) {
-
-		//dbg ("%i %i %i %i %i %i %i %i \n",b[0],b[1],b[2],b[3],b[4],b[5],b[6],b[7]);
-
 		if (mdc800_isBusy (b))
 		{
 			if (!mdc800->camera_busy)
 			{
 				mdc800->camera_busy=1;
-				dbg ("gets busy");
+				dev_dbg(dev, "gets busy\n");
 			}
 		}
 		else
@@ -303,13 +301,13 @@ static void mdc800_usb_irq (struct urb *urb)
 			if (mdc800->camera_busy && mdc800_isReady (b))
 			{
 				mdc800->camera_busy=0;
-				dbg ("gets ready");
+				dev_dbg(dev, "gets ready\n");
 			}
 		}
 		if (!(mdc800_isBusy (b) || mdc800_isReady (b)))
 		{
 			/* Store Data in camera_answer field */
-			dbg ("%i %i %i %i %i %i %i %i ",b[0],b[1],b[2],b[3],b[4],b[5],b[6],b[7]);
+			dev_dbg(dev, "%i %i %i %i %i %i %i %i \n",b[0],b[1],b[2],b[3],b[4],b[5],b[6],b[7]);
 
 			memcpy (mdc800->camera_response,b,8);
 			data_received=1;
@@ -441,7 +439,7 @@ static int mdc800_usb_probe (struct usb_interface *intf,
 	int irq_interval=0;
 	int retval;
 
-	dbg ("(mdc800_usb_probe) called.");
+	dev_dbg(&intf->dev, "(%s) called.\n", __func__);
 
 
 	if (mdc800->dev != NULL)
@@ -554,7 +552,7 @@ static void mdc800_usb_disconnect (struct usb_interface *intf)
 {
 	struct mdc800_data* mdc800 = usb_get_intfdata(intf);
 
-	dbg ("(mdc800_usb_disconnect) called");
+	dev_dbg(&intf->dev, "(%s) called\n", __func__);
 
 	if (mdc800) {
 		if (mdc800->state == NOT_CONNECTED)
@@ -656,7 +654,7 @@ static int mdc800_device_open (struct inode* inode, struct file *file)
 	}
 
 	mdc800->open=1;
-	dbg ("Mustek MDC800 device opened.");
+	dev_dbg(&mdc800->dev->dev, "Mustek MDC800 device opened.\n");
 
 error_out:
 	mutex_unlock(&mdc800->io_lock);
@@ -670,7 +668,6 @@ error_out:
 static int mdc800_device_release (struct inode* inode, struct file *file)
 {
 	int retval=0;
-	dbg ("Mustek MDC800 device closed.");
 
 	mutex_lock(&mdc800->io_lock);
 	if (mdc800->open && (mdc800->state != NOT_CONNECTED))
@@ -927,7 +924,7 @@ static ssize_t mdc800_device_write (struct file *file, const char __user *buf, s
 						{
 							mdc800->pic_len=(int) 65536*(unsigned char) mdc800->camera_response[0]+256*(unsigned char) mdc800->camera_response[1]+(unsigned char) mdc800->camera_response[2];
 
-							dbg ("cached imagesize = %i",mdc800->pic_len);
+							dev_dbg(&mdc800->dev->dev, "cached imagesize = %i\n", mdc800->pic_len);
 						}
 
 					}
diff --git a/drivers/usb/misc/appledisplay.c b/drivers/usb/misc/appledisplay.c
index ac0d75a9005..0fc6e5fc745 100644
--- a/drivers/usb/misc/appledisplay.c
+++ b/drivers/usb/misc/appledisplay.c
@@ -88,6 +88,7 @@ static struct workqueue_struct *wq;
 static void appledisplay_complete(struct urb *urb)
 {
 	struct appledisplay *pdata = urb->context;
+	struct device *dev = &pdata->udev->dev;
 	unsigned long flags;
 	int status = urb->status;
 	int retval;
@@ -97,18 +98,18 @@ static void appledisplay_complete(struct urb *urb)
 		/* success */
 		break;
 	case -EOVERFLOW:
-		printk(KERN_ERR "appletouch: OVERFLOW with data "
-			"length %d, actual length is %d\n",
+		dev_err(dev,
+			"OVERFLOW with data length %d, actual length is %d\n",
 			ACD_URB_BUFFER_LEN, pdata->urb->actual_length);
 	case -ECONNRESET:
 	case -ENOENT:
 	case -ESHUTDOWN:
 		/* This urb is terminated, clean up */
-		dbg("%s - urb shuttingdown with status: %d",
+		dev_dbg(dev, "%s - urb shuttingdown with status: %d\n",
 			__func__, status);
 		return;
 	default:
-		dbg("%s - nonzero urb status received: %d",
+		dev_dbg(dev, "%s - nonzero urb status received: %d/n",
 			__func__, status);
 		goto exit;
 	}
@@ -132,8 +133,7 @@ static void appledisplay_complete(struct urb *urb)
 exit:
 	retval = usb_submit_urb(pdata->urb, GFP_ATOMIC);
 	if (retval) {
-		dev_err(&pdata->udev->dev,
-			"%s - usb_submit_urb failed with result %d\n",
+		dev_err(dev, "%s - usb_submit_urb failed with result %d\n",
 			__func__, retval);
 	}
 }
diff --git a/drivers/usb/misc/emi26.c b/drivers/usb/misc/emi26.c
index da97dcec1f3..d65984dee75 100644
--- a/drivers/usb/misc/emi26.c
+++ b/drivers/usb/misc/emi26.c
@@ -78,18 +78,14 @@ static int emi26_load_firmware (struct usb_device *dev)
 	const struct firmware *bitstream_fw = NULL;
 	const struct firmware *firmware_fw = NULL;
 	const struct ihex_binrec *rec;
-	int err;
+	int err = -ENOMEM;
 	int i;
 	__u32 addr;	/* Address to write */
 	__u8 *buf;
 
 	buf = kmalloc(FW_LOAD_SIZE, GFP_KERNEL);
-	if (!buf) {
-		dev_err(&dev->dev, "%s - error loading firmware: error = %d\n",
-			__func__, -ENOMEM);
-		err = -ENOMEM;
+	if (!buf)
 		goto wraperr;
-	}
 
 	err = request_ihex_firmware(&loader_fw, "emi26/loader.fw", &dev->dev);
 	if (err)
@@ -111,11 +107,8 @@ static int emi26_load_firmware (struct usb_device *dev)
 
 	/* Assert reset (stop the CPU in the EMI) */
 	err = emi26_set_reset(dev,1);
-	if (err < 0) {
-		dev_err(&dev->dev,"%s - error loading firmware: error = %d\n",
-			__func__, err);
+	if (err < 0)
 		goto wraperr;
-	}
 
 	rec = (const struct ihex_binrec *)loader_fw->data;
 	/* 1. We need to put the loader for the FPGA into the EZ-USB */
@@ -123,19 +116,15 @@ static int emi26_load_firmware (struct usb_device *dev)
 		err = emi26_writememory(dev, be32_to_cpu(rec->addr),
 					rec->data, be16_to_cpu(rec->len),
 					ANCHOR_LOAD_INTERNAL);
-		if (err < 0) {
-			err("%s - error loading firmware: error = %d", __func__, err);
+		if (err < 0)
 			goto wraperr;
-		}
 		rec = ihex_next_binrec(rec);
 	}
 
 	/* De-assert reset (let the CPU run) */
 	err = emi26_set_reset(dev,0);
-	if (err < 0) {
-		err("%s - error loading firmware: error = %d", __func__, err);
+	if (err < 0)
 		goto wraperr;
-	}
 	msleep(250);	/* let device settle */
 
 	/* 2. We upload the FPGA firmware into the EMI
@@ -153,18 +142,14 @@ static int emi26_load_firmware (struct usb_device *dev)
 			rec = ihex_next_binrec(rec);
 		}
 		err = emi26_writememory(dev, addr, buf, i, ANCHOR_LOAD_FPGA);
-		if (err < 0) {
-			err("%s - error loading firmware: error = %d", __func__, err);
+		if (err < 0)
 			goto wraperr;
-		}
 	} while (rec);
 
 	/* Assert reset (stop the CPU in the EMI) */
 	err = emi26_set_reset(dev,1);
-	if (err < 0) {
-		err("%s - error loading firmware: error = %d", __func__, err);
+	if (err < 0)
 		goto wraperr;
-	}
 
 	/* 3. We need to put the loader for the firmware into the EZ-USB (again...) */
 	for (rec = (const struct ihex_binrec *)loader_fw->data;
@@ -172,19 +157,15 @@ static int emi26_load_firmware (struct usb_device *dev)
 		err = emi26_writememory(dev, be32_to_cpu(rec->addr),
 					rec->data, be16_to_cpu(rec->len),
 					ANCHOR_LOAD_INTERNAL);
-		if (err < 0) {
-			err("%s - error loading firmware: error = %d", __func__, err);
+		if (err < 0)
 			goto wraperr;
-		}
 	}
 	msleep(250);	/* let device settle */
 
 	/* De-assert reset (let the CPU run) */
 	err = emi26_set_reset(dev,0);
-	if (err < 0) {
-		err("%s - error loading firmware: error = %d", __func__, err);
+	if (err < 0)
 		goto wraperr;
-	}
 
 	/* 4. We put the part of the firmware that lies in the external RAM into the EZ-USB */
 
@@ -194,19 +175,15 @@ static int emi26_load_firmware (struct usb_device *dev)
 			err = emi26_writememory(dev, be32_to_cpu(rec->addr),
 						rec->data, be16_to_cpu(rec->len),
 						ANCHOR_LOAD_EXTERNAL);
-			if (err < 0) {
-				err("%s - error loading firmware: error = %d", __func__, err);
+			if (err < 0)
 				goto wraperr;
-			}
 		}
 	}
-	
+
 	/* Assert reset (stop the CPU in the EMI) */
 	err = emi26_set_reset(dev,1);
-	if (err < 0) {
-		err("%s - error loading firmware: error = %d", __func__, err);
+	if (err < 0)
 		goto wraperr;
-	}
 
 	for (rec = (const struct ihex_binrec *)firmware_fw->data;
 	     rec; rec = ihex_next_binrec(rec)) {
@@ -214,19 +191,15 @@ static int emi26_load_firmware (struct usb_device *dev)
 			err = emi26_writememory(dev, be32_to_cpu(rec->addr),
 						rec->data, be16_to_cpu(rec->len),
 						ANCHOR_LOAD_INTERNAL);
-			if (err < 0) {
-				err("%s - error loading firmware: error = %d", __func__, err);
+			if (err < 0)
 				goto wraperr;
-			}
 		}
 	}
 
 	/* De-assert reset (let the CPU run) */
 	err = emi26_set_reset(dev,0);
-	if (err < 0) {
-		err("%s - error loading firmware: error = %d", __func__, err);
+	if (err < 0)
 		goto wraperr;
-	}
 	msleep(250);	/* let device settle */
 
 	/* return 1 to fail the driver inialization
@@ -234,6 +207,10 @@ static int emi26_load_firmware (struct usb_device *dev)
 	err = 1;
 
 wraperr:
+	if (err < 0)
+		dev_err(&dev->dev,"%s - error loading firmware: error = %d\n",
+			__func__, err);
+
 	release_firmware(loader_fw);
 	release_firmware(bitstream_fw);
 	release_firmware(firmware_fw);
diff --git a/drivers/usb/misc/emi62.c b/drivers/usb/misc/emi62.c
index 4e0f167a6c4..ff08015b230 100644
--- a/drivers/usb/misc/emi62.c
+++ b/drivers/usb/misc/emi62.c
@@ -56,7 +56,7 @@ static int emi62_writememory(struct usb_device *dev, int address,
 	unsigned char *buffer =  kmemdup(data, length, GFP_KERNEL);
 
 	if (!buffer) {
-		err("emi62: kmalloc(%d) failed.", length);
+		dev_err(&dev->dev, "kmalloc(%d) failed.\n", length);
 		return -ENOMEM;
 	}
 	/* Note: usb_control_msg returns negative value on error or length of the
@@ -73,9 +73,8 @@ static int emi62_set_reset (struct usb_device *dev, unsigned char reset_bit)
 	dev_info(&dev->dev, "%s - %d\n", __func__, reset_bit);
 	
 	response = emi62_writememory (dev, CPUCS_REG, &reset_bit, 1, 0xa0);
-	if (response < 0) {
-		err("emi62: set_reset (%d) failed", reset_bit);
-	}
+	if (response < 0)
+		dev_err(&dev->dev, "set_reset (%d) failed\n", reset_bit);
 	return response;
 }
 
@@ -87,18 +86,15 @@ static int emi62_load_firmware (struct usb_device *dev)
 	const struct firmware *bitstream_fw = NULL;
 	const struct firmware *firmware_fw = NULL;
 	const struct ihex_binrec *rec;
-	int err;
+	int err = -ENOMEM;
 	int i;
 	__u32 addr;	/* Address to write */
 	__u8 *buf;
 
 	dev_dbg(&dev->dev, "load_firmware\n");
 	buf = kmalloc(FW_LOAD_SIZE, GFP_KERNEL);
-	if (!buf) {
-		err( "%s - error loading firmware: error = %d", __func__, -ENOMEM);
-		err = -ENOMEM;
+	if (!buf)
 		goto wraperr;
-	}
 
 	err = request_ihex_firmware(&loader_fw, "emi62/loader.fw", &dev->dev);
 	if (err)
@@ -112,16 +108,13 @@ static int emi62_load_firmware (struct usb_device *dev)
 	err = request_ihex_firmware(&firmware_fw, FIRMWARE_FW, &dev->dev);
 	if (err) {
 	nofw:
-		err( "%s - request_firmware() failed", __func__);
 		goto wraperr;
 	}
 
 	/* Assert reset (stop the CPU in the EMI) */
 	err = emi62_set_reset(dev,1);
-	if (err < 0) {
-		err("%s - error loading firmware: error = %d", __func__, err);
+	if (err < 0)
 		goto wraperr;
-	}
 
 	rec = (const struct ihex_binrec *)loader_fw->data;
 
@@ -130,19 +123,15 @@ static int emi62_load_firmware (struct usb_device *dev)
 		err = emi62_writememory(dev, be32_to_cpu(rec->addr),
 					rec->data, be16_to_cpu(rec->len),
 					ANCHOR_LOAD_INTERNAL);
-		if (err < 0) {
-			err("%s - error loading firmware: error = %d", __func__, err);
+		if (err < 0)
 			goto wraperr;
-		}
 		rec = ihex_next_binrec(rec);
 	}
 
 	/* De-assert reset (let the CPU run) */
 	err = emi62_set_reset(dev,0);
-	if (err < 0) {
-		err("%s - error loading firmware: error = %d", __func__, err);
+	if (err < 0)
 		goto wraperr;
-	}
 	msleep(250);	/* let device settle */
 
 	/* 2. We upload the FPGA firmware into the EMI
@@ -160,18 +149,14 @@ static int emi62_load_firmware (struct usb_device *dev)
 			rec = ihex_next_binrec(rec);
 		}
 		err = emi62_writememory(dev, addr, buf, i, ANCHOR_LOAD_FPGA);
-		if (err < 0) {
-			err("%s - error loading firmware: error = %d", __func__, err);
+		if (err < 0)
 			goto wraperr;
-		}
 	} while (rec);
 
 	/* Assert reset (stop the CPU in the EMI) */
 	err = emi62_set_reset(dev,1);
-	if (err < 0) {
-		err("%s - error loading firmware: error = %d", __func__, err);
+	if (err < 0)
 		goto wraperr;
-	}
 
 	/* 3. We need to put the loader for the firmware into the EZ-USB (again...) */
 	for (rec = (const struct ihex_binrec *)loader_fw->data;
@@ -179,18 +164,14 @@ static int emi62_load_firmware (struct usb_device *dev)
 		err = emi62_writememory(dev, be32_to_cpu(rec->addr),
 					rec->data, be16_to_cpu(rec->len),
 					ANCHOR_LOAD_INTERNAL);
-		if (err < 0) {
-			err("%s - error loading firmware: error = %d", __func__, err);
+		if (err < 0)
 			goto wraperr;
-		}
 	}
 
 	/* De-assert reset (let the CPU run) */
 	err = emi62_set_reset(dev,0);
-	if (err < 0) {
-		err("%s - error loading firmware: error = %d", __func__, err);
+	if (err < 0)
 		goto wraperr;
-	}
 	msleep(250);	/* let device settle */
 
 	/* 4. We put the part of the firmware that lies in the external RAM into the EZ-USB */
@@ -201,19 +182,15 @@ static int emi62_load_firmware (struct usb_device *dev)
 			err = emi62_writememory(dev, be32_to_cpu(rec->addr),
 						rec->data, be16_to_cpu(rec->len),
 						ANCHOR_LOAD_EXTERNAL);
-			if (err < 0) {
-				err("%s - error loading firmware: error = %d", __func__, err);
+			if (err < 0)
 				goto wraperr;
-			}
 		}
 	}
 
 	/* Assert reset (stop the CPU in the EMI) */
 	err = emi62_set_reset(dev,1);
-	if (err < 0) {
-		err("%s - error loading firmware: error = %d", __func__, err);
+	if (err < 0)
 		goto wraperr;
-	}
 
 	for (rec = (const struct ihex_binrec *)firmware_fw->data;
 	     rec; rec = ihex_next_binrec(rec)) {
@@ -221,19 +198,15 @@ static int emi62_load_firmware (struct usb_device *dev)
 			err = emi62_writememory(dev, be32_to_cpu(rec->addr),
 						rec->data, be16_to_cpu(rec->len),
 						ANCHOR_LOAD_EXTERNAL);
-			if (err < 0) {
-				err("%s - error loading firmware: error = %d", __func__, err);
+			if (err < 0)
 				goto wraperr;
-			}
 		}
 	}
 
 	/* De-assert reset (let the CPU run) */
 	err = emi62_set_reset(dev,0);
-	if (err < 0) {
-		err("%s - error loading firmware: error = %d", __func__, err);
+	if (err < 0)
 		goto wraperr;
-	}
 	msleep(250);	/* let device settle */
 
 	release_firmware(loader_fw);
@@ -247,6 +220,9 @@ static int emi62_load_firmware (struct usb_device *dev)
 	return 1;
 
 wraperr:
+	if (err < 0)
+		dev_err(&dev->dev,"%s - error loading firmware: error = %d\n",
+			__func__, err);
 	release_firmware(loader_fw);
 	release_firmware(bitstream_fw);
 	release_firmware(firmware_fw);
diff --git a/drivers/usb/misc/idmouse.c b/drivers/usb/misc/idmouse.c
index 0dee2469850..ce978384fda 100644
--- a/drivers/usb/misc/idmouse.c
+++ b/drivers/usb/misc/idmouse.c
@@ -200,7 +200,8 @@ reset:
 			return -EAGAIN;
 
 	/* should be IMGSIZE == 65040 */
-	dbg("read %d bytes fingerprint data", bytes_read);
+	dev_dbg(&dev->interface->dev, "read %d bytes fingerprint data\n",
+		bytes_read);
 	return result;
 }
 
@@ -366,14 +367,14 @@ static int idmouse_probe(struct usb_interface *interface,
 			kmalloc(IMGSIZE + dev->bulk_in_size, GFP_KERNEL);
 
 		if (!dev->bulk_in_buffer) {
-			err("Unable to allocate input buffer.");
+			dev_err(&interface->dev, "Unable to allocate input buffer.\n");
 			idmouse_delete(dev);
 			return -ENOMEM;
 		}
 	}
 
 	if (!(dev->bulk_in_endpointAddr)) {
-		err("Unable to find bulk-in endpoint.");
+		dev_err(&interface->dev, "Unable to find bulk-in endpoint.\n");
 		idmouse_delete(dev);
 		return -ENODEV;
 	}
@@ -385,7 +386,7 @@ static int idmouse_probe(struct usb_interface *interface,
 	result = usb_register_dev(interface, &idmouse_class);
 	if (result) {
 		/* something prevented us from registering this device */
-		err("Unble to allocate minor number.");
+		dev_err(&interface->dev, "Unble to allocate minor number.\n");
 		usb_set_intfdata(interface, NULL);
 		idmouse_delete(dev);
 		return result;
diff --git a/drivers/usb/misc/iowarrior.c b/drivers/usb/misc/iowarrior.c
index 4fd0dc835ae..db46143c67a 100644
--- a/drivers/usb/misc/iowarrior.c
+++ b/drivers/usb/misc/iowarrior.c
@@ -610,8 +610,8 @@ static int iowarrior_open(struct inode *inode, struct file *file)
 	interface = usb_find_interface(&iowarrior_driver, subminor);
 	if (!interface) {
 		mutex_unlock(&iowarrior_mutex);
-		err("%s - error, can't find device for minor %d", __func__,
-		    subminor);
+		printk(KERN_ERR "%s - error, can't find device for minor %d\n",
+		       __func__, subminor);
 		return -ENODEV;
 	}
 
diff --git a/drivers/usb/misc/ldusb.c b/drivers/usb/misc/ldusb.c
index 5db4ab52061..ac762299eaa 100644
--- a/drivers/usb/misc/ldusb.c
+++ b/drivers/usb/misc/ldusb.c
@@ -334,8 +334,8 @@ static int ld_usb_open(struct inode *inode, struct file *file)
 	interface = usb_find_interface(&ld_usb_driver, subminor);
 
 	if (!interface) {
-		err("%s - error, can't find device for minor %d\n",
-		     __func__, subminor);
+		printk(KERN_ERR "%s - error, can't find device for minor %d\n",
+		       __func__, subminor);
 		return -ENODEV;
 	}
 
@@ -485,7 +485,7 @@ static ssize_t ld_usb_read(struct file *file, char __user *buffer, size_t count,
 	/* verify that the device wasn't unplugged */
 	if (dev->intf == NULL) {
 		retval = -ENODEV;
-		err("No device or device unplugged %d\n", retval);
+		printk(KERN_ERR "ldusb: No device or device unplugged %d\n", retval);
 		goto unlock_exit;
 	}
 
@@ -565,7 +565,7 @@ static ssize_t ld_usb_write(struct file *file, const char __user *buffer,
 	/* verify that the device wasn't unplugged */
 	if (dev->intf == NULL) {
 		retval = -ENODEV;
-		err("No device or device unplugged %d\n", retval);
+		printk(KERN_ERR "ldusb: No device or device unplugged %d\n", retval);
 		goto unlock_exit;
 	}
 
@@ -603,7 +603,9 @@ static ssize_t ld_usb_write(struct file *file, const char __user *buffer,
 					 bytes_to_write,
 					 USB_CTRL_SET_TIMEOUT * HZ);
 		if (retval < 0)
-			err("Couldn't submit HID_REQ_SET_REPORT %d\n", retval);
+			dev_err(&dev->intf->dev,
+				"Couldn't submit HID_REQ_SET_REPORT %d\n",
+				retval);
 		goto unlock_exit;
 	}
 
@@ -624,7 +626,8 @@ static ssize_t ld_usb_write(struct file *file, const char __user *buffer,
 	retval = usb_submit_urb(dev->interrupt_out_urb, GFP_KERNEL);
 	if (retval) {
 		dev->interrupt_out_busy = 0;
-		err("Couldn't submit interrupt_out_urb %d\n", retval);
+		dev_err(&dev->intf->dev,
+			"Couldn't submit interrupt_out_urb %d\n", retval);
 		goto unlock_exit;
 	}
 	retval = bytes_to_write;
diff --git a/drivers/usb/misc/legousbtower.c b/drivers/usb/misc/legousbtower.c
index 57522204276..a2702cbfe80 100644
--- a/drivers/usb/misc/legousbtower.c
+++ b/drivers/usb/misc/legousbtower.c
@@ -354,8 +354,8 @@ static int tower_open (struct inode *inode, struct file *file)
 	interface = usb_find_interface (&tower_driver, subminor);
 
 	if (!interface) {
-		err ("%s - error, can't find device for minor %d",
-		     __func__, subminor);
+		printk(KERN_ERR "%s - error, can't find device for minor %d\n",
+		       __func__, subminor);
 		retval = -ENODEV;
 		goto exit;
 	}
@@ -397,7 +397,8 @@ static int tower_open (struct inode *inode, struct file *file)
 				  sizeof(reset_reply),
 				  1000);
 	if (result < 0) {
-		err("LEGO USB Tower reset control request failed");
+		dev_err(&dev->udev->dev,
+			"LEGO USB Tower reset control request failed\n");
 		retval = result;
 		goto unlock_exit;
 	}
@@ -420,7 +421,8 @@ static int tower_open (struct inode *inode, struct file *file)
 
 	retval = usb_submit_urb (dev->interrupt_in_urb, GFP_KERNEL);
 	if (retval) {
-		err("Couldn't submit interrupt_in_urb %d", retval);
+		dev_err(&dev->udev->dev,
+			"Couldn't submit interrupt_in_urb %d\n", retval);
 		dev->interrupt_in_running = 0;
 		dev->open_count = 0;
 		goto unlock_exit;
@@ -608,7 +610,7 @@ static ssize_t tower_read (struct file *file, char __user *buffer, size_t count,
 	/* verify that the device wasn't unplugged */
 	if (dev->udev == NULL) {
 		retval = -ENODEV;
-		err("No device or device unplugged %d", retval);
+		printk(KERN_ERR "legousbtower: No device or device unplugged %d\n", retval);
 		goto unlock_exit;
 	}
 
@@ -697,7 +699,7 @@ static ssize_t tower_write (struct file *file, const char __user *buffer, size_t
 	/* verify that the device wasn't unplugged */
 	if (dev->udev == NULL) {
 		retval = -ENODEV;
-		err("No device or device unplugged %d", retval);
+		printk(KERN_ERR "legousbtower: No device or device unplugged %d\n", retval);
 		goto unlock_exit;
 	}
 
@@ -744,7 +746,8 @@ static ssize_t tower_write (struct file *file, const char __user *buffer, size_t
 	retval = usb_submit_urb (dev->interrupt_out_urb, GFP_KERNEL);
 	if (retval) {
 		dev->interrupt_out_busy = 0;
-		err("Couldn't submit interrupt_out_urb %d", retval);
+		dev_err(&dev->udev->dev,
+			"Couldn't submit interrupt_out_urb %d\n", retval);
 		goto unlock_exit;
 	}
 	retval = bytes_to_write;
@@ -803,9 +806,10 @@ resubmit:
 	/* resubmit if we're still running */
 	if (dev->interrupt_in_running && dev->udev) {
 		retval = usb_submit_urb (dev->interrupt_in_urb, GFP_ATOMIC);
-		if (retval) {
-			err("%s: usb_submit_urb failed (%d)", __func__, retval);
-		}
+		if (retval)
+			dev_err(&dev->udev->dev,
+				"%s: usb_submit_urb failed (%d)\n",
+				__func__, retval);
 	}
 
 exit:
@@ -852,6 +856,7 @@ static void tower_interrupt_out_callback (struct urb *urb)
  */
 static int tower_probe (struct usb_interface *interface, const struct usb_device_id *id)
 {
+	struct device *idev = &interface->dev;
 	struct usb_device *udev = interface_to_usbdev(interface);
 	struct lego_usb_tower *dev = NULL;
 	struct usb_host_interface *iface_desc;
@@ -871,7 +876,7 @@ static int tower_probe (struct usb_interface *interface, const struct usb_device
 	dev = kmalloc (sizeof(struct lego_usb_tower), GFP_KERNEL);
 
 	if (dev == NULL) {
-		err ("Out of memory");
+		dev_err(idev, "Out of memory\n");
 		goto exit;
 	}
 
@@ -915,37 +920,37 @@ static int tower_probe (struct usb_interface *interface, const struct usb_device
 		}
 	}
 	if(dev->interrupt_in_endpoint == NULL) {
-		err("interrupt in endpoint not found");
+		dev_err(idev, "interrupt in endpoint not found\n");
 		goto error;
 	}
 	if (dev->interrupt_out_endpoint == NULL) {
-		err("interrupt out endpoint not found");
+		dev_err(idev, "interrupt out endpoint not found\n");
 		goto error;
 	}
 
 	dev->read_buffer = kmalloc (read_buffer_size, GFP_KERNEL);
 	if (!dev->read_buffer) {
-		err("Couldn't allocate read_buffer");
+		dev_err(idev, "Couldn't allocate read_buffer\n");
 		goto error;
 	}
 	dev->interrupt_in_buffer = kmalloc (usb_endpoint_maxp(dev->interrupt_in_endpoint), GFP_KERNEL);
 	if (!dev->interrupt_in_buffer) {
-		err("Couldn't allocate interrupt_in_buffer");
+		dev_err(idev, "Couldn't allocate interrupt_in_buffer\n");
 		goto error;
 	}
 	dev->interrupt_in_urb = usb_alloc_urb(0, GFP_KERNEL);
 	if (!dev->interrupt_in_urb) {
-		err("Couldn't allocate interrupt_in_urb");
+		dev_err(idev, "Couldn't allocate interrupt_in_urb\n");
 		goto error;
 	}
 	dev->interrupt_out_buffer = kmalloc (write_buffer_size, GFP_KERNEL);
 	if (!dev->interrupt_out_buffer) {
-		err("Couldn't allocate interrupt_out_buffer");
+		dev_err(idev, "Couldn't allocate interrupt_out_buffer\n");
 		goto error;
 	}
 	dev->interrupt_out_urb = usb_alloc_urb(0, GFP_KERNEL);
 	if (!dev->interrupt_out_urb) {
-		err("Couldn't allocate interrupt_out_urb");
+		dev_err(idev, "Couldn't allocate interrupt_out_urb\n");
 		goto error;
 	}
 	dev->interrupt_in_interval = interrupt_in_interval ? interrupt_in_interval : dev->interrupt_in_endpoint->bInterval;
@@ -958,7 +963,7 @@ static int tower_probe (struct usb_interface *interface, const struct usb_device
 
 	if (retval) {
 		/* something prevented us from registering this driver */
-		err ("Not able to get a minor for this device.");
+		dev_err(idev, "Not able to get a minor for this device.\n");
 		usb_set_intfdata (interface, NULL);
 		goto error;
 	}
@@ -980,7 +985,7 @@ static int tower_probe (struct usb_interface *interface, const struct usb_device
 				  sizeof(get_version_reply),
 				  1000);
 	if (result < 0) {
-		err("LEGO USB Tower get version control request failed");
+		dev_err(idev, "LEGO USB Tower get version control request failed\n");
 		retval = result;
 		goto error;
 	}
diff --git a/drivers/usb/misc/rio500.c b/drivers/usb/misc/rio500.c
index 487a8ce0775..1084124c4a4 100644
--- a/drivers/usb/misc/rio500.c
+++ b/drivers/usb/misc/rio500.c
@@ -153,10 +153,10 @@ static long ioctl_rio(struct file *file, unsigned int cmd, unsigned long arg)
 
 		requesttype = rio_cmd.requesttype | USB_DIR_IN |
 		    USB_TYPE_VENDOR | USB_RECIP_DEVICE;
-		dbg
-		    ("sending command:reqtype=%0x req=%0x value=%0x index=%0x len=%0x",
-		     requesttype, rio_cmd.request, rio_cmd.value,
-		     rio_cmd.index, rio_cmd.length);
+		dev_dbg(&rio->rio_dev->dev,
+			"sending command:reqtype=%0x req=%0x value=%0x index=%0x len=%0x\n",
+			requesttype, rio_cmd.request, rio_cmd.value,
+			rio_cmd.index, rio_cmd.length);
 		/* Send rio control message */
 		retries = 3;
 		while (retries) {
@@ -171,11 +171,14 @@ static long ioctl_rio(struct file *file, unsigned int cmd, unsigned long arg)
 			if (result == -ETIMEDOUT)
 				retries--;
 			else if (result < 0) {
-				err("Error executing ioctrl. code = %d", result);
+				dev_err(&rio->rio_dev->dev,
+					"Error executing ioctrl. code = %d\n",
+					result);
 				retries = 0;
 			} else {
-				dbg("Executed ioctl. Result = %d (data=%02x)",
-				     result, buffer[0]);
+				dev_dbg(&rio->rio_dev->dev,
+					"Executed ioctl. Result = %d (data=%02x)\n",
+					result, buffer[0]);
 				if (copy_to_user(rio_cmd.buffer, buffer,
 						 rio_cmd.length)) {
 					free_page((unsigned long) buffer);
@@ -221,9 +224,10 @@ static long ioctl_rio(struct file *file, unsigned int cmd, unsigned long arg)
 
 		requesttype = rio_cmd.requesttype | USB_DIR_OUT |
 		    USB_TYPE_VENDOR | USB_RECIP_DEVICE;
-		dbg("sending command: reqtype=%0x req=%0x value=%0x index=%0x len=%0x",
-		     requesttype, rio_cmd.request, rio_cmd.value,
-		     rio_cmd.index, rio_cmd.length);
+		dev_dbg(&rio->rio_dev->dev,
+			"sending command: reqtype=%0x req=%0x value=%0x index=%0x len=%0x\n",
+			requesttype, rio_cmd.request, rio_cmd.value,
+			rio_cmd.index, rio_cmd.length);
 		/* Send rio control message */
 		retries = 3;
 		while (retries) {
@@ -238,10 +242,13 @@ static long ioctl_rio(struct file *file, unsigned int cmd, unsigned long arg)
 			if (result == -ETIMEDOUT)
 				retries--;
 			else if (result < 0) {
-				err("Error executing ioctrl. code = %d", result);
+				dev_err(&rio->rio_dev->dev,
+					"Error executing ioctrl. code = %d\n",
+					result);
 				retries = 0;
 			} else {
-				dbg("Executed ioctl. Result = %d", result);
+				dev_dbg(&rio->rio_dev->dev,
+					"Executed ioctl. Result = %d\n", result);
 				retries = 0;
 
 			}
@@ -313,8 +320,9 @@ write_rio(struct file *file, const char __user *buffer,
 					 usb_sndbulkpipe(rio->rio_dev, 2),
 					 obuf, thistime, &partial, 5000);
 
-			dbg("write stats: result:%d thistime:%lu partial:%u",
-			     result, thistime, partial);
+			dev_dbg(&rio->rio_dev->dev,
+				"write stats: result:%d thistime:%lu partial:%u\n",
+				result, thistime, partial);
 
 			if (result == -ETIMEDOUT) {	/* NAK - so hold for a while */
 				if (!maxretry--) {
@@ -332,7 +340,8 @@ write_rio(struct file *file, const char __user *buffer,
 				break;
 		};
 		if (result) {
-			err("Write Whoops - %x", result);
+			dev_err(&rio->rio_dev->dev, "Write Whoops - %x\n",
+				result);
 			errn = -EIO;
 			goto error;
 		}
@@ -393,15 +402,17 @@ read_rio(struct file *file, char __user *buffer, size_t count, loff_t * ppos)
 				      ibuf, this_read, &partial,
 				      8000);
 
-		dbg("read stats: result:%d this_read:%u partial:%u",
-		       result, this_read, partial);
+		dev_dbg(&rio->rio_dev->dev,
+			"read stats: result:%d this_read:%u partial:%u\n",
+			result, this_read, partial);
 
 		if (partial) {
 			count = this_read = partial;
 		} else if (result == -ETIMEDOUT || result == 15) {	/* FIXME: 15 ??? */
 			if (!maxretry--) {
 				mutex_unlock(&(rio->lock));
-				err("read_rio: maxretry timeout");
+				dev_err(&rio->rio_dev->dev,
+					"read_rio: maxretry timeout\n");
 				return -ETIME;
 			}
 			prepare_to_wait(&rio->wait_q, &wait, TASK_INTERRUPTIBLE);
@@ -410,8 +421,9 @@ read_rio(struct file *file, char __user *buffer, size_t count, loff_t * ppos)
 			continue;
 		} else if (result != -EREMOTEIO) {
 			mutex_unlock(&(rio->lock));
-			err("Read Whoops - result:%u partial:%u this_read:%u",
-			     result, partial, this_read);
+			dev_err(&rio->rio_dev->dev,
+				"Read Whoops - result:%u partial:%u this_read:%u\n",
+				result, partial, this_read);
 			return -EIO;
 		} else {
 			mutex_unlock(&(rio->lock));
@@ -459,26 +471,29 @@ static int probe_rio(struct usb_interface *intf,
 
 	retval = usb_register_dev(intf, &usb_rio_class);
 	if (retval) {
-		err("Not able to get a minor for this device.");
+		dev_err(&dev->dev,
+			"Not able to get a minor for this device.\n");
 		return -ENOMEM;
 	}
 
 	rio->rio_dev = dev;
 
 	if (!(rio->obuf = kmalloc(OBUF_SIZE, GFP_KERNEL))) {
-		err("probe_rio: Not enough memory for the output buffer");
+		dev_err(&dev->dev,
+			"probe_rio: Not enough memory for the output buffer\n");
 		usb_deregister_dev(intf, &usb_rio_class);
 		return -ENOMEM;
 	}
-	dbg("probe_rio: obuf address:%p", rio->obuf);
+	dev_dbg(&intf->dev, "obuf address:%p\n", rio->obuf);
 
 	if (!(rio->ibuf = kmalloc(IBUF_SIZE, GFP_KERNEL))) {
-		err("probe_rio: Not enough memory for the input buffer");
+		dev_err(&dev->dev,
+			"probe_rio: Not enough memory for the input buffer\n");
 		usb_deregister_dev(intf, &usb_rio_class);
 		kfree(rio->obuf);
 		return -ENOMEM;
 	}
-	dbg("probe_rio: ibuf address:%p", rio->ibuf);
+	dev_dbg(&intf->dev, "ibuf address:%p\n", rio->ibuf);
 
 	mutex_init(&(rio->lock));
 
diff --git a/drivers/usb/misc/usblcd.c b/drivers/usb/misc/usblcd.c
index e2b4bd31c2b..89927bcff97 100644
--- a/drivers/usb/misc/usblcd.c
+++ b/drivers/usb/misc/usblcd.c
@@ -87,8 +87,8 @@ static int lcd_open(struct inode *inode, struct file *file)
 	interface = usb_find_interface(&lcd_driver, subminor);
 	if (!interface) {
 		mutex_unlock(&lcd_mutex);
-		err("USBLCD: %s - error, can't find device for minor %d",
-		     __func__, subminor);
+		printk(KERN_ERR "USBLCD: %s - error, can't find device for minor %d\n",
+		       __func__, subminor);
 		return -ENODEV;
 	}
 
@@ -209,8 +209,8 @@ static void lcd_write_bulk_callback(struct urb *urb)
 	    !(status == -ENOENT ||
 	      status == -ECONNRESET ||
 	      status == -ESHUTDOWN)) {
-		dbg("USBLCD: %s - nonzero write bulk status received: %d",
-		    __func__, status);
+		dev_dbg(&dev->interface->dev,
+			"nonzero write bulk status received: %d\n", status);
 	}
 
 	/* free up our allocated buffer */
@@ -268,8 +268,9 @@ static ssize_t lcd_write(struct file *file, const char __user * user_buffer,
 	/* send the data out the bulk port */
 	retval = usb_submit_urb(urb, GFP_KERNEL);
 	if (retval) {
-		err("USBLCD: %s - failed submitting write urb, error %d",
-		    __func__, retval);
+		dev_err(&dev->udev->dev,
+			"%s - failed submitting write urb, error %d\n",
+			__func__, retval);
 		goto error_unanchor;
 	}
 
@@ -322,7 +323,7 @@ static int lcd_probe(struct usb_interface *interface,
 	/* allocate memory for our device state and initialize it */
 	dev = kzalloc(sizeof(*dev), GFP_KERNEL);
 	if (dev == NULL) {
-		err("Out of memory");
+		dev_err(&interface->dev, "Out of memory\n");
 		goto error;
 	}
 	kref_init(&dev->kref);
@@ -352,7 +353,8 @@ static int lcd_probe(struct usb_interface *interface,
 			dev->bulk_in_endpointAddr = endpoint->bEndpointAddress;
 			dev->bulk_in_buffer = kmalloc(buffer_size, GFP_KERNEL);
 			if (!dev->bulk_in_buffer) {
-				err("Could not allocate bulk_in_buffer");
+				dev_err(&interface->dev,
+					"Could not allocate bulk_in_buffer\n");
 				goto error;
 			}
 		}
@@ -364,7 +366,8 @@ static int lcd_probe(struct usb_interface *interface,
 		}
 	}
 	if (!(dev->bulk_in_endpointAddr && dev->bulk_out_endpointAddr)) {
-		err("Could not find both bulk-in and bulk-out endpoints");
+		dev_err(&interface->dev,
+			"Could not find both bulk-in and bulk-out endpoints\n");
 		goto error;
 	}
 
@@ -375,7 +378,8 @@ static int lcd_probe(struct usb_interface *interface,
 	retval = usb_register_dev(interface, &lcd_class);
 	if (retval) {
 		/* something prevented us from registering this driver */
-		err("Not able to get a minor for this device.");
+		dev_err(&interface->dev,
+			"Not able to get a minor for this device.\n");
 		usb_set_intfdata(interface, NULL);
 		goto error;
 	}
diff --git a/drivers/usb/misc/usbtest.c b/drivers/usb/misc/usbtest.c
index 9dcb68f04f0..055b84adeda 100644
--- a/drivers/usb/misc/usbtest.c
+++ b/drivers/usb/misc/usbtest.c
@@ -1028,7 +1028,10 @@ test_ctrl_queue(struct usbtest_dev *dev, struct usbtest_param *param)
 		case 13:	/* short read, resembling case 10 */
 			req.wValue = cpu_to_le16((USB_DT_CONFIG << 8) | 0);
 			/* last data packet "should" be DATA1, not DATA0 */
-			len = 1024 - udev->descriptor.bMaxPacketSize0;
+			if (udev->speed == USB_SPEED_SUPER)
+				len = 1024 - 512;
+			else
+				len = 1024 - udev->descriptor.bMaxPacketSize0;
 			expected = -EREMOTEIO;
 			break;
 		case 14:	/* short read; try to fill the last packet */
@@ -1387,11 +1390,15 @@ static int test_halt(struct usbtest_dev *tdev, int ep, struct urb *urb)
 
 static int halt_simple(struct usbtest_dev *dev)
 {
-	int		ep;
-	int		retval = 0;
-	struct urb	*urb;
+	int			ep;
+	int			retval = 0;
+	struct urb		*urb;
+	struct usb_device	*udev = testdev_to_usbdev(dev);
 
-	urb = simple_alloc_urb(testdev_to_usbdev(dev), 0, 512);
+	if (udev->speed == USB_SPEED_SUPER)
+		urb = simple_alloc_urb(udev, 0, 1024);
+	else
+		urb = simple_alloc_urb(udev, 0, 512);
 	if (urb == NULL)
 		return -ENOMEM;
 
diff --git a/drivers/usb/misc/uss720.c b/drivers/usb/misc/uss720.c
index 8b1d94a7691..29cad9e0a7a 100644
--- a/drivers/usb/misc/uss720.c
+++ b/drivers/usb/misc/uss720.c
@@ -85,9 +85,9 @@ static void destroy_priv(struct kref *kref)
 {
 	struct parport_uss720_private *priv = container_of(kref, struct parport_uss720_private, ref_count);
 
+	dev_dbg(&priv->usbdev->dev, "destroying priv datastructure\n");
 	usb_put_dev(priv->usbdev);
 	kfree(priv);
-	dbg("destroying priv datastructure");
 }
 
 static void destroy_async(struct kref *kref)
@@ -118,14 +118,17 @@ static void async_complete(struct urb *urb)
 	priv = rq->priv;
 	pp = priv->pp;
 	if (status) {
-		err("async_complete: urb error %d", status);
+		dev_err(&urb->dev->dev, "async_complete: urb error %d\n",
+			status);
 	} else if (rq->dr.bRequest == 3) {
 		memcpy(priv->reg, rq->reg, sizeof(priv->reg));
 #if 0
-		dbg("async_complete regs %02x %02x %02x %02x %02x %02x %02x",
-		    (unsigned int)priv->reg[0], (unsigned int)priv->reg[1], (unsigned int)priv->reg[2],
-		    (unsigned int)priv->reg[3], (unsigned int)priv->reg[4], (unsigned int)priv->reg[5],
-		    (unsigned int)priv->reg[6]);
+		dev_dbg(&priv->usbdev->dev,
+			"async_complete regs %02x %02x %02x %02x %02x %02x %02x\n",
+			(unsigned int)priv->reg[0], (unsigned int)priv->reg[1],
+			(unsigned int)priv->reg[2], (unsigned int)priv->reg[3],
+			(unsigned int)priv->reg[4], (unsigned int)priv->reg[5],
+			(unsigned int)priv->reg[6]);
 #endif
 		/* if nAck interrupts are enabled and we have an interrupt, call the interrupt procedure */
 		if (rq->reg[2] & rq->reg[1] & 0x10 && pp)
@@ -151,7 +154,7 @@ static struct uss720_async_request *submit_async_request(struct parport_uss720_p
 		return NULL;
 	rq = kmalloc(sizeof(struct uss720_async_request), mem_flags);
 	if (!rq) {
-		err("submit_async_request out of memory");
+		dev_err(&usbdev->dev, "submit_async_request out of memory\n");
 		return NULL;
 	}
 	kref_init(&rq->ref_count);
@@ -162,7 +165,7 @@ static struct uss720_async_request *submit_async_request(struct parport_uss720_p
 	rq->urb = usb_alloc_urb(0, mem_flags);
 	if (!rq->urb) {
 		kref_put(&rq->ref_count, destroy_async);
-		err("submit_async_request out of memory");
+		dev_err(&usbdev->dev, "submit_async_request out of memory\n");
 		return NULL;
 	}
 	rq->dr.bRequestType = requesttype;
@@ -182,7 +185,7 @@ static struct uss720_async_request *submit_async_request(struct parport_uss720_p
 	if (!ret)
 		return rq;
 	destroy_async(&rq->ref_count);
-	err("submit_async_request submit_urb failed with %d", ret);
+	dev_err(&usbdev->dev, "submit_async_request submit_urb failed with %d\n", ret);
 	return NULL;
 }
 
@@ -217,7 +220,8 @@ static int get_1284_register(struct parport *pp, unsigned char reg, unsigned cha
 	priv = pp->private_data;
 	rq = submit_async_request(priv, 3, 0xc0, ((unsigned int)reg) << 8, 0, mem_flags);
 	if (!rq) {
-		err("get_1284_register(%u) failed", (unsigned int)reg);
+		dev_err(&priv->usbdev->dev, "get_1284_register(%u) failed",
+			(unsigned int)reg);
 		return -EIO;
 	}
 	if (!val) {
@@ -248,7 +252,8 @@ static int set_1284_register(struct parport *pp, unsigned char reg, unsigned cha
 	priv = pp->private_data;
 	rq = submit_async_request(priv, 4, 0x40, (((unsigned int)reg) << 8) | val, 0, mem_flags);
 	if (!rq) {
-		err("set_1284_register(%u,%u) failed", (unsigned int)reg, (unsigned int)val);
+		dev_err(&priv->usbdev->dev, "set_1284_register(%u,%u) failed",
+			(unsigned int)reg, (unsigned int)val);
 		return -EIO;
 	}
 	kref_put(&rq->ref_count, destroy_async);
@@ -690,9 +695,9 @@ static int uss720_probe(struct usb_interface *intf,
 	unsigned char reg;
 	int i;
 
-	dbg("probe: vendor id 0x%x, device id 0x%x\n",
-	    le16_to_cpu(usbdev->descriptor.idVendor),
-	    le16_to_cpu(usbdev->descriptor.idProduct));
+	dev_dbg(&intf->dev, "probe: vendor id 0x%x, device id 0x%x\n",
+		le16_to_cpu(usbdev->descriptor.idVendor),
+		le16_to_cpu(usbdev->descriptor.idProduct));
 
 	/* our known interfaces have 3 alternate settings */
 	if (intf->num_altsetting != 3) {
@@ -700,7 +705,7 @@ static int uss720_probe(struct usb_interface *intf,
 		return -ENODEV;
 	}
 	i = usb_set_interface(usbdev, intf->altsetting->desc.bInterfaceNumber, 2);
-	dbg("set inteface result %d", i);
+	dev_dbg(&intf->dev, "set inteface result %d\n", i);
 
 	interface = intf->cur_altsetting;
 
@@ -731,11 +736,13 @@ static int uss720_probe(struct usb_interface *intf,
 	set_1284_register(pp, 2, 0x0c, GFP_KERNEL);
 	/* debugging */
 	get_1284_register(pp, 0, &reg, GFP_KERNEL);
-	dbg("reg: %02x %02x %02x %02x %02x %02x %02x",
-	    priv->reg[0], priv->reg[1], priv->reg[2], priv->reg[3], priv->reg[4], priv->reg[5], priv->reg[6]);
+	dev_dbg(&intf->dev, "reg: %02x %02x %02x %02x %02x %02x %02x\n",
+		priv->reg[0], priv->reg[1], priv->reg[2], priv->reg[3],
+		priv->reg[4], priv->reg[5], priv->reg[6]);
 
 	endpoint = &interface->endpoint[2];
-	dbg("epaddr %d interval %d", endpoint->desc.bEndpointAddress, endpoint->desc.bInterval);
+	dev_dbg(&intf->dev, "epaddr %d interval %d\n",
+		endpoint->desc.bEndpointAddress, endpoint->desc.bInterval);
 	parport_announce_port(pp);
 
 	usb_set_intfdata(intf, pp);
@@ -753,20 +760,20 @@ static void uss720_disconnect(struct usb_interface *intf)
 	struct parport_uss720_private *priv;
 	struct usb_device *usbdev;
 
-	dbg("disconnect");
+	dev_dbg(&intf->dev, "disconnect\n");
 	usb_set_intfdata(intf, NULL);
 	if (pp) {
 		priv = pp->private_data;
 		usbdev = priv->usbdev;
 		priv->usbdev = NULL;
 		priv->pp = NULL;
-		dbg("parport_remove_port");
+		dev_dbg(&intf->dev, "parport_remove_port\n");
 		parport_remove_port(pp);
 		parport_put_port(pp);
 		kill_all_async_requests_priv(priv);
 		kref_put(&priv->ref_count, destroy_priv);
 	}
-	dbg("disconnect done");
+	dev_dbg(&intf->dev, "disconnect done\n");
 }
 
 /* table of cables that work through this driver */
diff --git a/drivers/usb/misc/yurex.c b/drivers/usb/misc/yurex.c
index 70201462e19..42ad2e6d86c 100644
--- a/drivers/usb/misc/yurex.c
+++ b/drivers/usb/misc/yurex.c
@@ -83,7 +83,8 @@ static void yurex_control_callback(struct urb *urb)
 	int status = urb->status;
 
 	if (status) {
-		err("%s - control failed: %d\n", __func__, status);
+		dev_err(&urb->dev->dev, "%s - control failed: %d\n",
+			__func__, status);
 		wake_up_interruptible(&dev->waitq);
 		return;
 	}
@@ -94,7 +95,7 @@ static void yurex_delete(struct kref *kref)
 {
 	struct usb_yurex *dev = to_yurex_dev(kref);
 
-	dbg("yurex_delete");
+	dev_dbg(&dev->interface->dev, "%s\n", __func__);
 
 	usb_put_dev(dev->udev);
 	if (dev->cntl_urb) {
@@ -137,8 +138,9 @@ static void yurex_interrupt(struct urb *urb)
 	case 0: /*success*/
 		break;
 	case -EOVERFLOW:
-		err("%s - overflow with length %d, actual length is %d",
-		    __func__, YUREX_BUF_SIZE, dev->urb->actual_length);
+		dev_err(&dev->interface->dev,
+			"%s - overflow with length %d, actual length is %d\n",
+			__func__, YUREX_BUF_SIZE, dev->urb->actual_length);
 	case -ECONNRESET:
 	case -ENOENT:
 	case -ESHUTDOWN:
@@ -146,7 +148,8 @@ static void yurex_interrupt(struct urb *urb)
 		/* The device is terminated, clean up */
 		return;
 	default:
-		err("%s - unknown status received: %d", __func__, status);
+		dev_err(&dev->interface->dev,
+			"%s - unknown status received: %d\n", __func__, status);
 		goto exit;
 	}
 
@@ -162,16 +165,19 @@ static void yurex_interrupt(struct urb *urb)
 				if (i != 5)
 					dev->bbu <<= 8;
 			}
-			dbg("%s count: %lld", __func__, dev->bbu);
+			dev_dbg(&dev->interface->dev, "%s count: %lld\n",
+				__func__, dev->bbu);
 			spin_unlock_irqrestore(&dev->lock, flags);
 
 			kill_fasync(&dev->async_queue, SIGIO, POLL_IN);
 		}
 		else
-			dbg("data format error - no EOF");
+			dev_dbg(&dev->interface->dev,
+				"data format error - no EOF\n");
 		break;
 	case CMD_ACK:
-		dbg("%s ack: %c", __func__, buf[1]);
+		dev_dbg(&dev->interface->dev, "%s ack: %c\n",
+			__func__, buf[1]);
 		wake_up_interruptible(&dev->waitq);
 		break;
 	}
@@ -179,7 +185,7 @@ static void yurex_interrupt(struct urb *urb)
 exit:
 	retval = usb_submit_urb(dev->urb, GFP_ATOMIC);
 	if (retval) {
-		err("%s - usb_submit_urb failed: %d",
+		dev_err(&dev->interface->dev, "%s - usb_submit_urb failed: %d\n",
 			__func__, retval);
 	}
 }
@@ -196,7 +202,7 @@ static int yurex_probe(struct usb_interface *interface, const struct usb_device_
 	/* allocate memory for our device state and initialize it */
 	dev = kzalloc(sizeof(*dev), GFP_KERNEL);
 	if (!dev) {
-		err("Out of memory");
+		dev_err(&interface->dev, "Out of memory\n");
 		goto error;
 	}
 	kref_init(&dev->kref);
@@ -219,7 +225,7 @@ static int yurex_probe(struct usb_interface *interface, const struct usb_device_
 	}
 	if (!dev->int_in_endpointAddr) {
 		retval = -ENODEV;
-		err("Could not find endpoints");
+		dev_err(&interface->dev, "Could not find endpoints\n");
 		goto error;
 	}
 
@@ -227,14 +233,14 @@ static int yurex_probe(struct usb_interface *interface, const struct usb_device_
 	/* allocate control URB */
 	dev->cntl_urb = usb_alloc_urb(0, GFP_KERNEL);
 	if (!dev->cntl_urb) {
-		err("Could not allocate control URB");
+		dev_err(&interface->dev, "Could not allocate control URB\n");
 		goto error;
 	}
 
 	/* allocate buffer for control req */
 	dev->cntl_req = kmalloc(YUREX_BUF_SIZE, GFP_KERNEL);
 	if (!dev->cntl_req) {
-		err("Could not allocate cntl_req");
+		dev_err(&interface->dev, "Could not allocate cntl_req\n");
 		goto error;
 	}
 
@@ -243,7 +249,7 @@ static int yurex_probe(struct usb_interface *interface, const struct usb_device_
 					      GFP_KERNEL,
 					      &dev->cntl_urb->transfer_dma);
 	if (!dev->cntl_buffer) {
-		err("Could not allocate cntl_buffer");
+		dev_err(&interface->dev, "Could not allocate cntl_buffer\n");
 		goto error;
 	}
 
@@ -265,7 +271,7 @@ static int yurex_probe(struct usb_interface *interface, const struct usb_device_
 	/* allocate interrupt URB */
 	dev->urb = usb_alloc_urb(0, GFP_KERNEL);
 	if (!dev->urb) {
-		err("Could not allocate URB");
+		dev_err(&interface->dev, "Could not allocate URB\n");
 		goto error;
 	}
 
@@ -273,7 +279,7 @@ static int yurex_probe(struct usb_interface *interface, const struct usb_device_
 	dev->int_buffer = usb_alloc_coherent(dev->udev, YUREX_BUF_SIZE,
 					GFP_KERNEL, &dev->urb->transfer_dma);
 	if (!dev->int_buffer) {
-		err("Could not allocate int_buffer");
+		dev_err(&interface->dev, "Could not allocate int_buffer\n");
 		goto error;
 	}
 
@@ -285,7 +291,7 @@ static int yurex_probe(struct usb_interface *interface, const struct usb_device_
 	dev->urb->transfer_flags |= URB_NO_TRANSFER_DMA_MAP;
 	if (usb_submit_urb(dev->urb, GFP_KERNEL)) {
 		retval = -EIO;
-		err("Could not submitting URB");
+		dev_err(&interface->dev, "Could not submitting URB\n");
 		goto error;
 	}
 
@@ -295,7 +301,8 @@ static int yurex_probe(struct usb_interface *interface, const struct usb_device_
 	/* we can register the device now, as it is ready */
 	retval = usb_register_dev(interface, &yurex_class);
 	if (retval) {
-		err("Not able to get a minor for this device.");
+		dev_err(&interface->dev,
+			"Not able to get a minor for this device.\n");
 		usb_set_intfdata(interface, NULL);
 		goto error;
 	}
@@ -368,8 +375,8 @@ static int yurex_open(struct inode *inode, struct file *file)
 
 	interface = usb_find_interface(&yurex_driver, subminor);
 	if (!interface) {
-		err("%s - error, can't find device for minor %d",
-		    __func__, subminor);
+		printk(KERN_ERR "%s - error, can't find device for minor %d",
+		       __func__, subminor);
 		retval = -ENODEV;
 		goto exit;
 	}
@@ -505,7 +512,8 @@ static ssize_t yurex_write(struct file *file, const char *user_buffer, size_t co
 
 	/* send the data as the control msg */
 	prepare_to_wait(&dev->waitq, &wait, TASK_INTERRUPTIBLE);
-	dbg("%s - submit %c", __func__, dev->cntl_buffer[0]);
+	dev_dbg(&dev->interface->dev, "%s - submit %c\n", __func__,
+		dev->cntl_buffer[0]);
 	retval = usb_submit_urb(dev->cntl_urb, GFP_KERNEL);
 	if (retval >= 0)
 		timeout = schedule_timeout(YUREX_WRITE_TIMEOUT);
@@ -514,7 +522,9 @@ static ssize_t yurex_write(struct file *file, const char *user_buffer, size_t co
 	mutex_unlock(&dev->io_mutex);
 
 	if (retval < 0) {
-		err("%s - failed to send bulk msg, error %d", __func__, retval);
+		dev_err(&dev->interface->dev,
+			"%s - failed to send bulk msg, error %d\n",
+			__func__, retval);
 		goto error;
 	}
 	if (set && timeout)
diff --git a/drivers/usb/musb/Kconfig b/drivers/usb/musb/Kconfig
index f70cab3beee..ef0c3f9f094 100644
--- a/drivers/usb/musb/Kconfig
+++ b/drivers/usb/musb/Kconfig
@@ -8,6 +8,7 @@ config USB_MUSB_HDRC
 	tristate 'Inventra Highspeed Dual Role Controller (TI, ADI, ...)'
 	depends on USB && USB_GADGET
 	select NOP_USB_XCEIV if (ARCH_DAVINCI || MACH_OMAP3EVM || BLACKFIN)
+	select NOP_USB_XCEIV if (SOC_OMAPTI81XX || SOC_OMAPAM33XX)
 	select TWL4030_USB if MACH_OMAP_3430SDP
 	select TWL6030_USB if MACH_OMAP_4430SDP || MACH_OMAP4_PANDA
 	select USB_OTG_UTILS
@@ -54,6 +55,10 @@ config USB_MUSB_AM35X
 	tristate "AM35x"
 	depends on ARCH_OMAP
 
+config USB_MUSB_DSPS
+	tristate "TI DSPS platforms"
+	depends on SOC_OMAPTI81XX || SOC_OMAPAM33XX
+
 config USB_MUSB_BLACKFIN
 	tristate "Blackfin"
 	depends on (BF54x && !BF544) || (BF52x && ! BF522 && !BF523)
@@ -70,7 +75,8 @@ choice
 	default USB_INVENTRA_DMA if USB_MUSB_OMAP2PLUS || USB_MUSB_BLACKFIN
 	default USB_TI_CPPI_DMA if USB_MUSB_DAVINCI
 	default USB_TUSB_OMAP_DMA if USB_MUSB_TUSB6010
-	default MUSB_PIO_ONLY if USB_MUSB_TUSB6010 || USB_MUSB_DA8XX || USB_MUSB_AM35X
+	default MUSB_PIO_ONLY if USB_MUSB_TUSB6010 || USB_MUSB_DA8XX || USB_MUSB_AM35X \
+				|| USB_MUSB_DSPS
 	help
 	  Unfortunately, only one option can be enabled here. Ideally one
 	  should be able to build all these drivers into one kernel to
diff --git a/drivers/usb/musb/Makefile b/drivers/usb/musb/Makefile
index 88bfb9dee4b..3b858715b5e 100644
--- a/drivers/usb/musb/Makefile
+++ b/drivers/usb/musb/Makefile
@@ -13,6 +13,7 @@ musb_hdrc-$(CONFIG_DEBUG_FS)			+= musb_debugfs.o
 # Hardware Glue Layer
 obj-$(CONFIG_USB_MUSB_OMAP2PLUS)		+= omap2430.o
 obj-$(CONFIG_USB_MUSB_AM35X)			+= am35x.o
+obj-$(CONFIG_USB_MUSB_DSPS)			+= musb_dsps.o
 obj-$(CONFIG_USB_MUSB_TUSB6010)			+= tusb6010.o
 obj-$(CONFIG_USB_MUSB_DAVINCI)			+= davinci.o
 obj-$(CONFIG_USB_MUSB_DA8XX)			+= da8xx.o
diff --git a/drivers/usb/musb/cppi_dma.c b/drivers/usb/musb/cppi_dma.c
index 66bc376005d..8637c1f69fc 100644
--- a/drivers/usb/musb/cppi_dma.c
+++ b/drivers/usb/musb/cppi_dma.c
@@ -6,6 +6,7 @@
  * The TUSB6020, using VLYNQ, has CPPI that looks much like DaVinci.
  */
 
+#include <linux/module.h>
 #include <linux/platform_device.h>
 #include <linux/slab.h>
 #include <linux/usb.h>
diff --git a/drivers/usb/musb/musb_core.c b/drivers/usb/musb/musb_core.c
index 66aaccf0449..db3dff854b7 100644
--- a/drivers/usb/musb/musb_core.c
+++ b/drivers/usb/musb/musb_core.c
@@ -1034,7 +1034,9 @@ static void musb_shutdown(struct platform_device *pdev)
 	|| defined(CONFIG_USB_MUSB_OMAP2PLUS)		\
 	|| defined(CONFIG_USB_MUSB_OMAP2PLUS_MODULE)	\
 	|| defined(CONFIG_USB_MUSB_AM35X)		\
-	|| defined(CONFIG_USB_MUSB_AM35X_MODULE)
+	|| defined(CONFIG_USB_MUSB_AM35X_MODULE)	\
+	|| defined(CONFIG_USB_MUSB_DSPS)		\
+	|| defined(CONFIG_USB_MUSB_DSPS_MODULE)
 static ushort __devinitdata fifo_mode = 4;
 #elif defined(CONFIG_USB_MUSB_UX500)			\
 	|| defined(CONFIG_USB_MUSB_UX500_MODULE)
diff --git a/drivers/usb/musb/musb_dsps.c b/drivers/usb/musb/musb_dsps.c
new file mode 100644
index 00000000000..23db42db761
--- /dev/null
+++ b/drivers/usb/musb/musb_dsps.c
@@ -0,0 +1,711 @@
+/*
+ * Texas Instruments DSPS platforms "glue layer"
+ *
+ * Copyright (C) 2012, by Texas Instruments
+ *
+ * Based on the am35x "glue layer" code.
+ *
+ * This file is part of the Inventra Controller Driver for Linux.
+ *
+ * The Inventra Controller Driver for Linux 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.
+ *
+ * The Inventra Controller Driver for Linux 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 The Inventra Controller Driver for Linux ; if not,
+ * write to the Free Software Foundation, Inc., 59 Temple Place,
+ * Suite 330, Boston, MA  02111-1307  USA
+ *
+ * musb_dsps.c will be a common file for all the TI DSPS platforms
+ * such as dm64x, dm36x, dm35x, da8x, am35x and ti81x.
+ * For now only ti81x is using this and in future davinci.c, am35x.c
+ * da8xx.c would be merged to this file after testing.
+ */
+
+#include <linux/init.h>
+#include <linux/io.h>
+#include <linux/platform_device.h>
+#include <linux/dma-mapping.h>
+#include <linux/pm_runtime.h>
+#include <linux/module.h>
+
+#include <linux/of.h>
+#include <linux/of_device.h>
+#include <linux/of_address.h>
+
+#include <plat/usb.h>
+
+#include "musb_core.h"
+
+/**
+ * avoid using musb_readx()/musb_writex() as glue layer should not be
+ * dependent on musb core layer symbols.
+ */
+static inline u8 dsps_readb(const void __iomem *addr, unsigned offset)
+	{ return __raw_readb(addr + offset); }
+
+static inline u32 dsps_readl(const void __iomem *addr, unsigned offset)
+	{ return __raw_readl(addr + offset); }
+
+static inline void dsps_writeb(void __iomem *addr, unsigned offset, u8 data)
+	{ __raw_writeb(data, addr + offset); }
+
+static inline void dsps_writel(void __iomem *addr, unsigned offset, u32 data)
+	{ __raw_writel(data, addr + offset); }
+
+/**
+ * DSPS musb wrapper register offset.
+ * FIXME: This should be expanded to have all the wrapper registers from TI DSPS
+ * musb ips.
+ */
+struct dsps_musb_wrapper {
+	u16	revision;
+	u16	control;
+	u16	status;
+	u16	eoi;
+	u16	epintr_set;
+	u16	epintr_clear;
+	u16	epintr_status;
+	u16	coreintr_set;
+	u16	coreintr_clear;
+	u16	coreintr_status;
+	u16	phy_utmi;
+	u16	mode;
+
+	/* bit positions for control */
+	unsigned	reset:5;
+
+	/* bit positions for interrupt */
+	unsigned	usb_shift:5;
+	u32		usb_mask;
+	u32		usb_bitmap;
+	unsigned	drvvbus:5;
+
+	unsigned	txep_shift:5;
+	u32		txep_mask;
+	u32		txep_bitmap;
+
+	unsigned	rxep_shift:5;
+	u32		rxep_mask;
+	u32		rxep_bitmap;
+
+	/* bit positions for phy_utmi */
+	unsigned	otg_disable:5;
+
+	/* bit positions for mode */
+	unsigned	iddig:5;
+	/* miscellaneous stuff */
+	u32		musb_core_offset;
+	u8		poll_seconds;
+};
+
+/**
+ * DSPS glue structure.
+ */
+struct dsps_glue {
+	struct device *dev;
+	struct platform_device *musb;	/* child musb pdev */
+	const struct dsps_musb_wrapper *wrp; /* wrapper register offsets */
+	struct timer_list timer;	/* otg_workaround timer */
+};
+
+/**
+ * dsps_musb_enable - enable interrupts
+ */
+static void dsps_musb_enable(struct musb *musb)
+{
+	struct device *dev = musb->controller;
+	struct platform_device *pdev = to_platform_device(dev->parent);
+	struct dsps_glue *glue = platform_get_drvdata(pdev);
+	const struct dsps_musb_wrapper *wrp = glue->wrp;
+	void __iomem *reg_base = musb->ctrl_base;
+	u32 epmask, coremask;
+
+	/* Workaround: setup IRQs through both register sets. */
+	epmask = ((musb->epmask & wrp->txep_mask) << wrp->txep_shift) |
+	       ((musb->epmask & wrp->rxep_mask) << wrp->rxep_shift);
+	coremask = (wrp->usb_bitmap & ~MUSB_INTR_SOF);
+
+	dsps_writel(reg_base, wrp->epintr_set, epmask);
+	dsps_writel(reg_base, wrp->coreintr_set, coremask);
+	/* Force the DRVVBUS IRQ so we can start polling for ID change. */
+	if (is_otg_enabled(musb))
+		dsps_writel(reg_base, wrp->coreintr_set,
+			    (1 << wrp->drvvbus) << wrp->usb_shift);
+}
+
+/**
+ * dsps_musb_disable - disable HDRC and flush interrupts
+ */
+static void dsps_musb_disable(struct musb *musb)
+{
+	struct device *dev = musb->controller;
+	struct platform_device *pdev = to_platform_device(dev->parent);
+	struct dsps_glue *glue = platform_get_drvdata(pdev);
+	const struct dsps_musb_wrapper *wrp = glue->wrp;
+	void __iomem *reg_base = musb->ctrl_base;
+
+	dsps_writel(reg_base, wrp->coreintr_clear, wrp->usb_bitmap);
+	dsps_writel(reg_base, wrp->epintr_clear,
+			 wrp->txep_bitmap | wrp->rxep_bitmap);
+	dsps_writeb(musb->mregs, MUSB_DEVCTL, 0);
+	dsps_writel(reg_base, wrp->eoi, 0);
+}
+
+static void otg_timer(unsigned long _musb)
+{
+	struct musb *musb = (void *)_musb;
+	void __iomem *mregs = musb->mregs;
+	struct device *dev = musb->controller;
+	struct platform_device *pdev = to_platform_device(dev->parent);
+	struct dsps_glue *glue = platform_get_drvdata(pdev);
+	const struct dsps_musb_wrapper *wrp = glue->wrp;
+	u8 devctl;
+	unsigned long flags;
+
+	/*
+	 * We poll because DSPS IP's won't expose several OTG-critical
+	 * status change events (from the transceiver) otherwise.
+	 */
+	devctl = dsps_readb(mregs, MUSB_DEVCTL);
+	dev_dbg(musb->controller, "Poll devctl %02x (%s)\n", devctl,
+				otg_state_string(musb->xceiv->state));
+
+	spin_lock_irqsave(&musb->lock, flags);
+	switch (musb->xceiv->state) {
+	case OTG_STATE_A_WAIT_BCON:
+		devctl &= ~MUSB_DEVCTL_SESSION;
+		dsps_writeb(musb->mregs, MUSB_DEVCTL, devctl);
+
+		devctl = dsps_readb(musb->mregs, MUSB_DEVCTL);
+		if (devctl & MUSB_DEVCTL_BDEVICE) {
+			musb->xceiv->state = OTG_STATE_B_IDLE;
+			MUSB_DEV_MODE(musb);
+		} else {
+			musb->xceiv->state = OTG_STATE_A_IDLE;
+			MUSB_HST_MODE(musb);
+		}
+		break;
+	case OTG_STATE_A_WAIT_VFALL:
+		musb->xceiv->state = OTG_STATE_A_WAIT_VRISE;
+		dsps_writel(musb->ctrl_base, wrp->coreintr_set,
+			    MUSB_INTR_VBUSERROR << wrp->usb_shift);
+		break;
+	case OTG_STATE_B_IDLE:
+		if (!is_peripheral_enabled(musb))
+			break;
+
+		devctl = dsps_readb(mregs, MUSB_DEVCTL);
+		if (devctl & MUSB_DEVCTL_BDEVICE)
+			mod_timer(&glue->timer,
+					jiffies + wrp->poll_seconds * HZ);
+		else
+			musb->xceiv->state = OTG_STATE_A_IDLE;
+		break;
+	default:
+		break;
+	}
+	spin_unlock_irqrestore(&musb->lock, flags);
+}
+
+static void dsps_musb_try_idle(struct musb *musb, unsigned long timeout)
+{
+	struct device *dev = musb->controller;
+	struct platform_device *pdev = to_platform_device(dev->parent);
+	struct dsps_glue *glue = platform_get_drvdata(pdev);
+	static unsigned long last_timer;
+
+	if (!is_otg_enabled(musb))
+		return;
+
+	if (timeout == 0)
+		timeout = jiffies + msecs_to_jiffies(3);
+
+	/* Never idle if active, or when VBUS timeout is not set as host */
+	if (musb->is_active || (musb->a_wait_bcon == 0 &&
+				musb->xceiv->state == OTG_STATE_A_WAIT_BCON)) {
+		dev_dbg(musb->controller, "%s active, deleting timer\n",
+				otg_state_string(musb->xceiv->state));
+		del_timer(&glue->timer);
+		last_timer = jiffies;
+		return;
+	}
+
+	if (time_after(last_timer, timeout) && timer_pending(&glue->timer)) {
+		dev_dbg(musb->controller,
+			"Longer idle timer already pending, ignoring...\n");
+		return;
+	}
+	last_timer = timeout;
+
+	dev_dbg(musb->controller, "%s inactive, starting idle timer for %u ms\n",
+		otg_state_string(musb->xceiv->state),
+			jiffies_to_msecs(timeout - jiffies));
+	mod_timer(&glue->timer, timeout);
+}
+
+static irqreturn_t dsps_interrupt(int irq, void *hci)
+{
+	struct musb  *musb = hci;
+	void __iomem *reg_base = musb->ctrl_base;
+	struct device *dev = musb->controller;
+	struct platform_device *pdev = to_platform_device(dev->parent);
+	struct dsps_glue *glue = platform_get_drvdata(pdev);
+	const struct dsps_musb_wrapper *wrp = glue->wrp;
+	unsigned long flags;
+	irqreturn_t ret = IRQ_NONE;
+	u32 epintr, usbintr;
+
+	spin_lock_irqsave(&musb->lock, flags);
+
+	/* Get endpoint interrupts */
+	epintr = dsps_readl(reg_base, wrp->epintr_status);
+	musb->int_rx = (epintr & wrp->rxep_bitmap) >> wrp->rxep_shift;
+	musb->int_tx = (epintr & wrp->txep_bitmap) >> wrp->txep_shift;
+
+	if (epintr)
+		dsps_writel(reg_base, wrp->epintr_status, epintr);
+
+	/* Get usb core interrupts */
+	usbintr = dsps_readl(reg_base, wrp->coreintr_status);
+	if (!usbintr && !epintr)
+		goto eoi;
+
+	musb->int_usb =	(usbintr & wrp->usb_bitmap) >> wrp->usb_shift;
+	if (usbintr)
+		dsps_writel(reg_base, wrp->coreintr_status, usbintr);
+
+	dev_dbg(musb->controller, "usbintr (%x) epintr(%x)\n",
+			usbintr, epintr);
+	/*
+	 * DRVVBUS IRQs are the only proxy we have (a very poor one!) for
+	 * DSPS IP's missing ID change IRQ.  We need an ID change IRQ to
+	 * switch appropriately between halves of the OTG state machine.
+	 * Managing DEVCTL.SESSION per Mentor docs requires that we know its
+	 * value but DEVCTL.BDEVICE is invalid without DEVCTL.SESSION set.
+	 * Also, DRVVBUS pulses for SRP (but not at 5V) ...
+	 */
+	if ((usbintr & MUSB_INTR_BABBLE) && is_host_enabled(musb))
+		pr_info("CAUTION: musb: Babble Interrupt Occured\n");
+
+	if (usbintr & ((1 << wrp->drvvbus) << wrp->usb_shift)) {
+		int drvvbus = dsps_readl(reg_base, wrp->status);
+		void __iomem *mregs = musb->mregs;
+		u8 devctl = dsps_readb(mregs, MUSB_DEVCTL);
+		int err;
+
+		err = is_host_enabled(musb) && (musb->int_usb &
+						MUSB_INTR_VBUSERROR);
+		if (err) {
+			/*
+			 * The Mentor core doesn't debounce VBUS as needed
+			 * to cope with device connect current spikes. This
+			 * means it's not uncommon for bus-powered devices
+			 * to get VBUS errors during enumeration.
+			 *
+			 * This is a workaround, but newer RTL from Mentor
+			 * seems to allow a better one: "re"-starting sessions
+			 * without waiting for VBUS to stop registering in
+			 * devctl.
+			 */
+			musb->int_usb &= ~MUSB_INTR_VBUSERROR;
+			musb->xceiv->state = OTG_STATE_A_WAIT_VFALL;
+			mod_timer(&glue->timer,
+					jiffies + wrp->poll_seconds * HZ);
+			WARNING("VBUS error workaround (delay coming)\n");
+		} else if (is_host_enabled(musb) && drvvbus) {
+			musb->is_active = 1;
+			MUSB_HST_MODE(musb);
+			musb->xceiv->otg->default_a = 1;
+			musb->xceiv->state = OTG_STATE_A_WAIT_VRISE;
+			del_timer(&glue->timer);
+		} else {
+			musb->is_active = 0;
+			MUSB_DEV_MODE(musb);
+			musb->xceiv->otg->default_a = 0;
+			musb->xceiv->state = OTG_STATE_B_IDLE;
+		}
+
+		/* NOTE: this must complete power-on within 100 ms. */
+		dev_dbg(musb->controller, "VBUS %s (%s)%s, devctl %02x\n",
+				drvvbus ? "on" : "off",
+				otg_state_string(musb->xceiv->state),
+				err ? " ERROR" : "",
+				devctl);
+		ret = IRQ_HANDLED;
+	}
+
+	if (musb->int_tx || musb->int_rx || musb->int_usb)
+		ret |= musb_interrupt(musb);
+
+ eoi:
+	/* EOI needs to be written for the IRQ to be re-asserted. */
+	if (ret == IRQ_HANDLED || epintr || usbintr)
+		dsps_writel(reg_base, wrp->eoi, 1);
+
+	/* Poll for ID change */
+	if (is_otg_enabled(musb) && musb->xceiv->state == OTG_STATE_B_IDLE)
+		mod_timer(&glue->timer, jiffies + wrp->poll_seconds * HZ);
+
+	spin_unlock_irqrestore(&musb->lock, flags);
+
+	return ret;
+}
+
+static int dsps_musb_init(struct musb *musb)
+{
+	struct device *dev = musb->controller;
+	struct musb_hdrc_platform_data *plat = dev->platform_data;
+	struct platform_device *pdev = to_platform_device(dev->parent);
+	struct dsps_glue *glue = platform_get_drvdata(pdev);
+	const struct dsps_musb_wrapper *wrp = glue->wrp;
+	struct omap_musb_board_data *data = plat->board_data;
+	void __iomem *reg_base = musb->ctrl_base;
+	u32 rev, val;
+	int status;
+
+	/* mentor core register starts at offset of 0x400 from musb base */
+	musb->mregs += wrp->musb_core_offset;
+
+	/* NOP driver needs change if supporting dual instance */
+	usb_nop_xceiv_register();
+	musb->xceiv = usb_get_transceiver();
+	if (!musb->xceiv)
+		return -ENODEV;
+
+	/* Returns zero if e.g. not clocked */
+	rev = dsps_readl(reg_base, wrp->revision);
+	if (!rev) {
+		status = -ENODEV;
+		goto err0;
+	}
+
+	if (is_host_enabled(musb))
+		setup_timer(&glue->timer, otg_timer, (unsigned long) musb);
+
+	/* Reset the musb */
+	dsps_writel(reg_base, wrp->control, (1 << wrp->reset));
+
+	/* Start the on-chip PHY and its PLL. */
+	if (data->set_phy_power)
+		data->set_phy_power(1);
+
+	musb->isr = dsps_interrupt;
+
+	/* reset the otgdisable bit, needed for host mode to work */
+	val = dsps_readl(reg_base, wrp->phy_utmi);
+	val &= ~(1 << wrp->otg_disable);
+	dsps_writel(musb->ctrl_base, wrp->phy_utmi, val);
+
+	/* clear level interrupt */
+	dsps_writel(reg_base, wrp->eoi, 0);
+
+	return 0;
+err0:
+	usb_put_transceiver(musb->xceiv);
+	usb_nop_xceiv_unregister();
+	return status;
+}
+
+static int dsps_musb_exit(struct musb *musb)
+{
+	struct device *dev = musb->controller;
+	struct musb_hdrc_platform_data *plat = dev->platform_data;
+	struct omap_musb_board_data *data = plat->board_data;
+	struct platform_device *pdev = to_platform_device(dev->parent);
+	struct dsps_glue *glue = platform_get_drvdata(pdev);
+
+	if (is_host_enabled(musb))
+		del_timer_sync(&glue->timer);
+
+	/* Shutdown the on-chip PHY and its PLL. */
+	if (data->set_phy_power)
+		data->set_phy_power(0);
+
+	/* NOP driver needs change if supporting dual instance */
+	usb_put_transceiver(musb->xceiv);
+	usb_nop_xceiv_unregister();
+
+	return 0;
+}
+
+static struct musb_platform_ops dsps_ops = {
+	.init		= dsps_musb_init,
+	.exit		= dsps_musb_exit,
+
+	.enable		= dsps_musb_enable,
+	.disable	= dsps_musb_disable,
+
+	.try_idle	= dsps_musb_try_idle,
+};
+
+static u64 musb_dmamask = DMA_BIT_MASK(32);
+
+static int __devinit dsps_create_musb_pdev(struct dsps_glue *glue, u8 id)
+{
+	struct device *dev = glue->dev;
+	struct platform_device *pdev = to_platform_device(dev);
+	struct musb_hdrc_platform_data  *pdata = dev->platform_data;
+	struct platform_device	*musb;
+	struct resource *res;
+	struct resource	resources[2];
+	char res_name[10];
+	int ret;
+
+	/* get memory resource */
+	sprintf(res_name, "musb%d", id);
+	res = platform_get_resource_byname(pdev, IORESOURCE_MEM, res_name);
+	if (!res) {
+		dev_err(dev, "%s get mem resource failed\n", res_name);
+		ret = -ENODEV;
+		goto err0;
+	}
+	res->parent = NULL;
+	resources[0] = *res;
+
+	/* get irq resource */
+	sprintf(res_name, "musb%d-irq", id);
+	res = platform_get_resource_byname(pdev, IORESOURCE_IRQ, res_name);
+	if (!res) {
+		dev_err(dev, "%s get irq resource failed\n", res_name);
+		ret = -ENODEV;
+		goto err0;
+	}
+	strcpy((u8 *)res->name, "mc");
+	res->parent = NULL;
+	resources[1] = *res;
+
+	/* allocate the child platform device */
+	musb = platform_device_alloc("musb-hdrc", -1);
+	if (!musb) {
+		dev_err(dev, "failed to allocate musb device\n");
+		ret = -ENOMEM;
+		goto err0;
+	}
+
+	musb->dev.parent		= dev;
+	musb->dev.dma_mask		= &musb_dmamask;
+	musb->dev.coherent_dma_mask	= musb_dmamask;
+
+	glue->musb			= musb;
+
+	pdata->platform_ops		= &dsps_ops;
+
+	ret = platform_device_add_resources(musb, resources, 2);
+	if (ret) {
+		dev_err(dev, "failed to add resources\n");
+		goto err1;
+	}
+
+	ret = platform_device_add_data(musb, pdata, sizeof(*pdata));
+	if (ret) {
+		dev_err(dev, "failed to add platform_data\n");
+		goto err1;
+	}
+
+	ret = platform_device_add(musb);
+	if (ret) {
+		dev_err(dev, "failed to register musb device\n");
+		goto err1;
+	}
+
+	return 0;
+
+err1:
+	platform_device_put(musb);
+err0:
+	return ret;
+}
+
+static void __devexit dsps_delete_musb_pdev(struct dsps_glue *glue)
+{
+	platform_device_del(glue->musb);
+	platform_device_put(glue->musb);
+}
+
+static int __devinit dsps_probe(struct platform_device *pdev)
+{
+	const struct platform_device_id *id = platform_get_device_id(pdev);
+	const struct dsps_musb_wrapper *wrp =
+				(struct dsps_musb_wrapper *)id->driver_data;
+	struct dsps_glue *glue;
+	struct resource *iomem;
+	int ret;
+
+	/* allocate glue */
+	glue = kzalloc(sizeof(*glue), GFP_KERNEL);
+	if (!glue) {
+		dev_err(&pdev->dev, "unable to allocate glue memory\n");
+		ret = -ENOMEM;
+		goto err0;
+	}
+
+	/* get memory resource */
+	iomem = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	if (!iomem) {
+		dev_err(&pdev->dev, "failed to get usbss mem resourse\n");
+		ret = -ENODEV;
+		goto err1;
+	}
+
+	glue->dev = &pdev->dev;
+
+	glue->wrp = kmemdup(wrp, sizeof(*wrp), GFP_KERNEL);
+	if (!glue->wrp) {
+		dev_err(&pdev->dev, "failed to duplicate wrapper struct memory\n");
+		ret = -ENOMEM;
+		goto err1;
+	}
+	platform_set_drvdata(pdev, glue);
+
+	/* create the child platform device for first instances of musb */
+	ret = dsps_create_musb_pdev(glue, 0);
+	if (ret != 0) {
+		dev_err(&pdev->dev, "failed to create child pdev\n");
+		goto err2;
+	}
+
+	/* enable the usbss clocks */
+	pm_runtime_enable(&pdev->dev);
+
+	ret = pm_runtime_get_sync(&pdev->dev);
+	if (ret < 0) {
+		dev_err(&pdev->dev, "pm_runtime_get_sync FAILED");
+		goto err3;
+	}
+
+	return 0;
+
+err3:
+	pm_runtime_disable(&pdev->dev);
+err2:
+	kfree(glue->wrp);
+err1:
+	kfree(glue);
+err0:
+	return ret;
+}
+static int __devexit dsps_remove(struct platform_device *pdev)
+{
+	struct dsps_glue *glue = platform_get_drvdata(pdev);
+
+	/* delete the child platform device */
+	dsps_delete_musb_pdev(glue);
+
+	/* disable usbss clocks */
+	pm_runtime_put(&pdev->dev);
+	pm_runtime_disable(&pdev->dev);
+	kfree(glue->wrp);
+	kfree(glue);
+	return 0;
+}
+
+#ifdef CONFIG_PM_SLEEP
+static int dsps_suspend(struct device *dev)
+{
+	struct musb_hdrc_platform_data *plat = dev->platform_data;
+	struct omap_musb_board_data *data = plat->board_data;
+
+	/* Shutdown the on-chip PHY and its PLL. */
+	if (data->set_phy_power)
+		data->set_phy_power(0);
+
+	return 0;
+}
+
+static int dsps_resume(struct device *dev)
+{
+	struct musb_hdrc_platform_data *plat = dev->platform_data;
+	struct omap_musb_board_data *data = plat->board_data;
+
+	/* Start the on-chip PHY and its PLL. */
+	if (data->set_phy_power)
+		data->set_phy_power(1);
+
+	return 0;
+}
+#endif
+
+static SIMPLE_DEV_PM_OPS(dsps_pm_ops, dsps_suspend, dsps_resume);
+
+static const struct dsps_musb_wrapper ti81xx_driver_data __devinitconst = {
+	.revision		= 0x00,
+	.control		= 0x14,
+	.status			= 0x18,
+	.eoi			= 0x24,
+	.epintr_set		= 0x38,
+	.epintr_clear		= 0x40,
+	.epintr_status		= 0x30,
+	.coreintr_set		= 0x3c,
+	.coreintr_clear		= 0x44,
+	.coreintr_status	= 0x34,
+	.phy_utmi		= 0xe0,
+	.mode			= 0xe8,
+	.reset			= 0,
+	.otg_disable		= 21,
+	.iddig			= 8,
+	.usb_shift		= 0,
+	.usb_mask		= 0x1ff,
+	.usb_bitmap		= (0x1ff << 0),
+	.drvvbus		= 8,
+	.txep_shift		= 0,
+	.txep_mask		= 0xffff,
+	.txep_bitmap		= (0xffff << 0),
+	.rxep_shift		= 16,
+	.rxep_mask		= 0xfffe,
+	.rxep_bitmap		= (0xfffe << 16),
+	.musb_core_offset	= 0x400,
+	.poll_seconds		= 2,
+};
+
+static const struct platform_device_id musb_dsps_id_table[] __devinitconst = {
+	{
+		.name	= "musb-ti81xx",
+		.driver_data	= (kernel_ulong_t) &ti81xx_driver_data,
+	},
+	{  },	/* Terminating Entry */
+};
+MODULE_DEVICE_TABLE(platform, musb_dsps_id_table);
+
+static const struct of_device_id musb_dsps_of_match[] __devinitconst = {
+	{ .compatible = "musb-ti81xx", },
+	{ .compatible = "ti,ti81xx-musb", },
+	{ .compatible = "ti,am335x-musb", },
+	{  },
+};
+MODULE_DEVICE_TABLE(of, musb_dsps_of_match);
+
+static struct platform_driver dsps_usbss_driver = {
+	.probe		= dsps_probe,
+	.remove         = __devexit_p(dsps_remove),
+	.driver         = {
+		.name   = "musb-dsps",
+		.pm	= &dsps_pm_ops,
+		.of_match_table	= musb_dsps_of_match,
+	},
+	.id_table	= musb_dsps_id_table,
+};
+
+MODULE_DESCRIPTION("TI DSPS MUSB Glue Layer");
+MODULE_AUTHOR("Ravi B <ravibabu@ti.com>");
+MODULE_AUTHOR("Ajay Kumar Gupta <ajay.gupta@ti.com>");
+MODULE_LICENSE("GPL v2");
+
+static int __init dsps_init(void)
+{
+	return platform_driver_register(&dsps_usbss_driver);
+}
+subsys_initcall(dsps_init);
+
+static void __exit dsps_exit(void)
+{
+	platform_driver_unregister(&dsps_usbss_driver);
+}
+module_exit(dsps_exit);
diff --git a/drivers/usb/musb/musb_io.h b/drivers/usb/musb/musb_io.h
index 1d5eda26fbd..f7c1c8e2dc3 100644
--- a/drivers/usb/musb/musb_io.h
+++ b/drivers/usb/musb/musb_io.h
@@ -40,7 +40,7 @@
 #if !defined(CONFIG_ARM) && !defined(CONFIG_SUPERH) \
 	&& !defined(CONFIG_AVR32) && !defined(CONFIG_PPC32) \
 	&& !defined(CONFIG_PPC64) && !defined(CONFIG_BLACKFIN) \
-	&& !defined(CONFIG_MIPS)
+	&& !defined(CONFIG_MIPS) && !defined(CONFIG_M68K)
 static inline void readsl(const void __iomem *addr, void *buf, int len)
 	{ insl((unsigned long)addr, buf, len); }
 static inline void readsw(const void __iomem *addr, void *buf, int len)
diff --git a/drivers/usb/otg/gpio_vbus.c b/drivers/usb/otg/gpio_vbus.c
index a0a2178974f..bde6298a969 100644
--- a/drivers/usb/otg/gpio_vbus.c
+++ b/drivers/usb/otg/gpio_vbus.c
@@ -37,7 +37,9 @@ struct gpio_vbus_data {
 	struct regulator       *vbus_draw;
 	int			vbus_draw_enabled;
 	unsigned		mA;
-	struct work_struct	work;
+	struct delayed_work	work;
+	int			vbus;
+	int			irq;
 };
 
 
@@ -51,8 +53,7 @@ struct gpio_vbus_data {
  * edges might be workable.
  */
 #define VBUS_IRQ_FLAGS \
-	( IRQF_SAMPLE_RANDOM | IRQF_SHARED \
-	| IRQF_TRIGGER_RISING | IRQF_TRIGGER_FALLING )
+	(IRQF_SHARED | IRQF_TRIGGER_RISING | IRQF_TRIGGER_FALLING)
 
 
 /* interface to regulator framework */
@@ -94,20 +95,26 @@ static int is_vbus_powered(struct gpio_vbus_mach_info *pdata)
 static void gpio_vbus_work(struct work_struct *work)
 {
 	struct gpio_vbus_data *gpio_vbus =
-		container_of(work, struct gpio_vbus_data, work);
+		container_of(work, struct gpio_vbus_data, work.work);
 	struct gpio_vbus_mach_info *pdata = gpio_vbus->dev->platform_data;
-	int gpio, status;
+	int gpio, status, vbus;
 
 	if (!gpio_vbus->phy.otg->gadget)
 		return;
 
+	vbus = is_vbus_powered(pdata);
+	if ((vbus ^ gpio_vbus->vbus) == 0)
+		return;
+	gpio_vbus->vbus = vbus;
+
 	/* Peripheral controllers which manage the pullup themselves won't have
 	 * gpio_pullup configured here.  If it's configured here, we'll do what
 	 * isp1301_omap::b_peripheral() does and enable the pullup here... although
 	 * that may complicate usb_gadget_{,dis}connect() support.
 	 */
 	gpio = pdata->gpio_pullup;
-	if (is_vbus_powered(pdata)) {
+
+	if (vbus) {
 		status = USB_EVENT_VBUS;
 		gpio_vbus->phy.state = OTG_STATE_B_PERIPHERAL;
 		gpio_vbus->phy.last_event = status;
@@ -152,7 +159,7 @@ static irqreturn_t gpio_vbus_irq(int irq, void *data)
 		otg->gadget ? otg->gadget->name : "none");
 
 	if (otg->gadget)
-		schedule_work(&gpio_vbus->work);
+		schedule_delayed_work(&gpio_vbus->work, msecs_to_jiffies(100));
 
 	return IRQ_HANDLED;
 }
@@ -166,12 +173,11 @@ static int gpio_vbus_set_peripheral(struct usb_otg *otg,
 	struct gpio_vbus_data *gpio_vbus;
 	struct gpio_vbus_mach_info *pdata;
 	struct platform_device *pdev;
-	int gpio, irq;
+	int gpio;
 
 	gpio_vbus = container_of(otg->phy, struct gpio_vbus_data, phy);
 	pdev = to_platform_device(gpio_vbus->dev);
 	pdata = gpio_vbus->dev->platform_data;
-	irq = gpio_to_irq(pdata->gpio_vbus);
 	gpio = pdata->gpio_pullup;
 
 	if (!gadget) {
@@ -195,7 +201,8 @@ static int gpio_vbus_set_peripheral(struct usb_otg *otg,
 	dev_dbg(&pdev->dev, "registered gadget '%s'\n", gadget->name);
 
 	/* initialize connection state */
-	gpio_vbus_irq(irq, pdev);
+	gpio_vbus->vbus = 0; /* start with disconnected */
+	gpio_vbus_irq(gpio_vbus->irq, pdev);
 	return 0;
 }
 
@@ -235,6 +242,7 @@ static int __init gpio_vbus_probe(struct platform_device *pdev)
 	struct gpio_vbus_data *gpio_vbus;
 	struct resource *res;
 	int err, gpio, irq;
+	unsigned long irqflags;
 
 	if (!pdata || !gpio_is_valid(pdata->gpio_vbus))
 		return -EINVAL;
@@ -271,10 +279,13 @@ static int __init gpio_vbus_probe(struct platform_device *pdev)
 	res = platform_get_resource(pdev, IORESOURCE_IRQ, 0);
 	if (res) {
 		irq = res->start;
-		res->flags &= IRQF_TRIGGER_MASK;
-		res->flags |= IRQF_SAMPLE_RANDOM | IRQF_SHARED;
-	} else
+		irqflags = (res->flags & IRQF_TRIGGER_MASK) | IRQF_SHARED;
+	} else {
 		irq = gpio_to_irq(gpio);
+		irqflags = VBUS_IRQ_FLAGS;
+	}
+
+	gpio_vbus->irq = irq;
 
 	/* if data line pullup is in use, initialize it to "not pulling up" */
 	gpio = pdata->gpio_pullup;
@@ -290,8 +301,7 @@ static int __init gpio_vbus_probe(struct platform_device *pdev)
 		gpio_direction_output(gpio, pdata->gpio_pullup_inverted);
 	}
 
-	err = request_irq(irq, gpio_vbus_irq, VBUS_IRQ_FLAGS,
-		"vbus_detect", pdev);
+	err = request_irq(irq, gpio_vbus_irq, irqflags, "vbus_detect", pdev);
 	if (err) {
 		dev_err(&pdev->dev, "can't request irq %i, err: %d\n",
 			irq, err);
@@ -300,7 +310,7 @@ static int __init gpio_vbus_probe(struct platform_device *pdev)
 
 	ATOMIC_INIT_NOTIFIER_HEAD(&gpio_vbus->phy.notifier);
 
-	INIT_WORK(&gpio_vbus->work, gpio_vbus_work);
+	INIT_DELAYED_WORK(&gpio_vbus->work, gpio_vbus_work);
 
 	gpio_vbus->vbus_draw = regulator_get(&pdev->dev, "vbus_draw");
 	if (IS_ERR(gpio_vbus->vbus_draw)) {
@@ -317,9 +327,12 @@ static int __init gpio_vbus_probe(struct platform_device *pdev)
 		goto err_otg;
 	}
 
+	device_init_wakeup(&pdev->dev, pdata->wakeup);
+
 	return 0;
 err_otg:
-	free_irq(irq, &pdev->dev);
+	regulator_put(gpio_vbus->vbus_draw);
+	free_irq(irq, pdev);
 err_irq:
 	if (gpio_is_valid(pdata->gpio_pullup))
 		gpio_free(pdata->gpio_pullup);
@@ -337,11 +350,13 @@ static int __exit gpio_vbus_remove(struct platform_device *pdev)
 	struct gpio_vbus_mach_info *pdata = pdev->dev.platform_data;
 	int gpio = pdata->gpio_vbus;
 
+	device_init_wakeup(&pdev->dev, 0);
+	cancel_delayed_work_sync(&gpio_vbus->work);
 	regulator_put(gpio_vbus->vbus_draw);
 
 	usb_set_transceiver(NULL);
 
-	free_irq(gpio_to_irq(gpio), &pdev->dev);
+	free_irq(gpio_vbus->irq, pdev);
 	if (gpio_is_valid(pdata->gpio_pullup))
 		gpio_free(pdata->gpio_pullup);
 	gpio_free(gpio);
@@ -352,6 +367,33 @@ static int __exit gpio_vbus_remove(struct platform_device *pdev)
 	return 0;
 }
 
+#ifdef CONFIG_PM
+static int gpio_vbus_pm_suspend(struct device *dev)
+{
+	struct gpio_vbus_data *gpio_vbus = dev_get_drvdata(dev);
+
+	if (device_may_wakeup(dev))
+		enable_irq_wake(gpio_vbus->irq);
+
+	return 0;
+}
+
+static int gpio_vbus_pm_resume(struct device *dev)
+{
+	struct gpio_vbus_data *gpio_vbus = dev_get_drvdata(dev);
+
+	if (device_may_wakeup(dev))
+		disable_irq_wake(gpio_vbus->irq);
+
+	return 0;
+}
+
+static const struct dev_pm_ops gpio_vbus_dev_pm_ops = {
+	.suspend	= gpio_vbus_pm_suspend,
+	.resume		= gpio_vbus_pm_resume,
+};
+#endif
+
 /* NOTE:  the gpio-vbus device may *NOT* be hotplugged */
 
 MODULE_ALIAS("platform:gpio-vbus");
@@ -360,6 +402,9 @@ static struct platform_driver gpio_vbus_driver = {
 	.driver = {
 		.name  = "gpio-vbus",
 		.owner = THIS_MODULE,
+#ifdef CONFIG_PM
+		.pm = &gpio_vbus_dev_pm_ops,
+#endif
 	},
 	.remove  = __exit_p(gpio_vbus_remove),
 };
diff --git a/drivers/usb/otg/twl6030-usb.c b/drivers/usb/otg/twl6030-usb.c
index e3fa387ca81..d2a9a8e691b 100644
--- a/drivers/usb/otg/twl6030-usb.c
+++ b/drivers/usb/otg/twl6030-usb.c
@@ -455,7 +455,7 @@ static int __devinit twl6030_usb_probe(struct platform_device *pdev)
 
 	twl->irq_enabled = true;
 	status = request_threaded_irq(twl->irq1, NULL, twl6030_usbotg_irq,
-			IRQF_TRIGGER_FALLING | IRQF_TRIGGER_RISING,
+			IRQF_TRIGGER_FALLING | IRQF_TRIGGER_RISING | IRQF_ONESHOT,
 			"twl6030_usb", twl);
 	if (status < 0) {
 		dev_err(&pdev->dev, "can't get IRQ %d, err %d\n",
@@ -467,7 +467,7 @@ static int __devinit twl6030_usb_probe(struct platform_device *pdev)
 	}
 
 	status = request_threaded_irq(twl->irq2, NULL, twl6030_usb_irq,
-			IRQF_TRIGGER_FALLING | IRQF_TRIGGER_RISING,
+			IRQF_TRIGGER_FALLING | IRQF_TRIGGER_RISING | IRQF_ONESHOT,
 			"twl6030_usb", twl);
 	if (status < 0) {
 		dev_err(&pdev->dev, "can't get IRQ %d, err %d\n",
diff --git a/drivers/usb/phy/Kconfig b/drivers/usb/phy/Kconfig
new file mode 100644
index 00000000000..3cfabcba744
--- /dev/null
+++ b/drivers/usb/phy/Kconfig
@@ -0,0 +1,17 @@
+#
+# Physical Layer USB driver configuration
+#
+comment "USB Physical Layer drivers"
+	depends on USB
+
+config USB_ISP1301
+	tristate "NXP ISP1301 USB transceiver support"
+	depends on USB
+	depends on I2C
+	help
+	  Say Y here to add support for the NXP ISP1301 USB transceiver driver.
+	  This chip is typically used as USB transceiver for USB host, gadget
+	  and OTG drivers (to be selected separately).
+
+	  To compile this driver as a module, choose M here: the
+	  module will be called isp1301.
diff --git a/drivers/usb/phy/Makefile b/drivers/usb/phy/Makefile
new file mode 100644
index 00000000000..eca095b1a89
--- /dev/null
+++ b/drivers/usb/phy/Makefile
@@ -0,0 +1,7 @@
+#
+# Makefile for physical layer USB drivers
+#
+
+ccflags-$(CONFIG_USB_DEBUG) := -DDEBUG
+
+obj-$(CONFIG_USB_ISP1301)		+= isp1301.o
diff --git a/drivers/usb/phy/isp1301.c b/drivers/usb/phy/isp1301.c
new file mode 100644
index 00000000000..b19f4932a03
--- /dev/null
+++ b/drivers/usb/phy/isp1301.c
@@ -0,0 +1,77 @@
+/*
+ * NXP ISP1301 USB transceiver driver
+ *
+ * Copyright (C) 2012 Roland Stigge
+ *
+ * Author: Roland Stigge <stigge@antcom.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.
+ */
+
+#include <linux/module.h>
+#include <linux/i2c.h>
+
+#define DRV_NAME		"isp1301"
+
+#define ISP1301_I2C_ADDR	0x2C
+
+static const unsigned short normal_i2c[] = {
+	ISP1301_I2C_ADDR, ISP1301_I2C_ADDR + 1, I2C_CLIENT_END
+};
+
+static const struct i2c_device_id isp1301_id[] = {
+	{ "isp1301", 0 },
+	{ }
+};
+
+static struct i2c_client *isp1301_i2c_client;
+
+static int isp1301_probe(struct i2c_client *client,
+			 const struct i2c_device_id *i2c_id)
+{
+	isp1301_i2c_client = client;
+	return 0;
+}
+
+static int isp1301_remove(struct i2c_client *client)
+{
+	return 0;
+}
+
+static struct i2c_driver isp1301_driver = {
+	.driver = {
+		.name = DRV_NAME,
+	},
+	.probe = isp1301_probe,
+	.remove = isp1301_remove,
+	.id_table = isp1301_id,
+};
+
+module_i2c_driver(isp1301_driver);
+
+static int match(struct device *dev, void *data)
+{
+	struct device_node *node = (struct device_node *)data;
+	return (dev->of_node == node) &&
+		(dev->driver == &isp1301_driver.driver);
+}
+
+struct i2c_client *isp1301_get_client(struct device_node *node)
+{
+	if (node) { /* reference of ISP1301 I2C node via DT */
+		struct device *dev = bus_find_device(&i2c_bus_type, NULL,
+						     node, match);
+		if (!dev)
+			return NULL;
+		return to_i2c_client(dev);
+	} else { /* non-DT: only one ISP1301 chip supported */
+		return isp1301_i2c_client;
+	}
+}
+EXPORT_SYMBOL_GPL(isp1301_get_client);
+
+MODULE_AUTHOR("Roland Stigge <stigge@antcom.de>");
+MODULE_DESCRIPTION("NXP ISP1301 USB transceiver driver");
+MODULE_LICENSE("GPL");
diff --git a/drivers/usb/renesas_usbhs/mod_gadget.c b/drivers/usb/renesas_usbhs/mod_gadget.c
index 00bd2a5e036..28478ce26c3 100644
--- a/drivers/usb/renesas_usbhs/mod_gadget.c
+++ b/drivers/usb/renesas_usbhs/mod_gadget.c
@@ -55,6 +55,7 @@ struct usbhsg_gpriv {
 #define USBHSG_STATUS_STARTED		(1 << 0)
 #define USBHSG_STATUS_REGISTERD		(1 << 1)
 #define USBHSG_STATUS_WEDGE		(1 << 2)
+#define USBHSG_STATUS_SELF_POWERED	(1 << 3)
 };
 
 struct usbhsg_recip_handle {
@@ -333,7 +334,10 @@ static int usbhsg_recip_handler_std_get_device(struct usbhs_priv *priv,
 					       struct usb_ctrlrequest *ctrl)
 {
 	struct usbhsg_gpriv *gpriv = usbhsg_uep_to_gpriv(uep);
-	unsigned short status = 1 << USB_DEVICE_SELF_POWERED;
+	unsigned short status = 0;
+
+	if (usbhsg_status_has(gpriv, USBHSG_STATUS_SELF_POWERED))
+		status = 1 << USB_DEVICE_SELF_POWERED;
 
 	__usbhsg_recip_send_status(gpriv, status);
 
@@ -879,8 +883,21 @@ static int usbhsg_get_frame(struct usb_gadget *gadget)
 	return usbhs_frame_get_num(priv);
 }
 
+static int usbhsg_set_selfpowered(struct usb_gadget *gadget, int is_self)
+{
+	struct usbhsg_gpriv *gpriv = usbhsg_gadget_to_gpriv(gadget);
+
+	if (is_self)
+		usbhsg_status_set(gpriv, USBHSG_STATUS_SELF_POWERED);
+	else
+		usbhsg_status_clr(gpriv, USBHSG_STATUS_SELF_POWERED);
+
+	return 0;
+}
+
 static struct usb_gadget_ops usbhsg_gadget_ops = {
 	.get_frame		= usbhsg_get_frame,
+	.set_selfpowered	= usbhsg_set_selfpowered,
 	.udc_start		= usbhsg_gadget_start,
 	.udc_stop		= usbhsg_gadget_stop,
 };
diff --git a/drivers/usb/serial/Kconfig b/drivers/usb/serial/Kconfig
index 7141d659906..325d2910f9f 100644
--- a/drivers/usb/serial/Kconfig
+++ b/drivers/usb/serial/Kconfig
@@ -669,6 +669,15 @@ config USB_SERIAL_SSU100
 	  To compile this driver as a module, choose M here: the
 	  module will be called ssu100.
 
+config USB_SERIAL_QT2
+	tristate "USB Quatech Serial Driver for USB 2 devices"
+	help
+	  Say Y here if you want to use the Quatech USB 2
+	  serial adapters.
+
+	  To compile this driver as a module, choose M here: the
+	  module will be called quatech-serial.
+
 config USB_SERIAL_DEBUG
 	tristate "USB Debugging Device"
 	help
diff --git a/drivers/usb/serial/Makefile b/drivers/usb/serial/Makefile
index 07f198ee048..1dc483a8bfc 100644
--- a/drivers/usb/serial/Makefile
+++ b/drivers/usb/serial/Makefile
@@ -49,6 +49,7 @@ obj-$(CONFIG_USB_SERIAL_OTI6858)		+= oti6858.o
 obj-$(CONFIG_USB_SERIAL_PL2303)			+= pl2303.o
 obj-$(CONFIG_USB_SERIAL_QCAUX)			+= qcaux.o
 obj-$(CONFIG_USB_SERIAL_QUALCOMM)		+= qcserial.o
+obj-$(CONFIG_USB_SERIAL_QT2)			+= quatech2.o
 obj-$(CONFIG_USB_SERIAL_SAFE)			+= safe_serial.o
 obj-$(CONFIG_USB_SERIAL_SIEMENS_MPI)		+= siemens_mpi.o
 obj-$(CONFIG_USB_SERIAL_SIERRAWIRELESS)		+= sierra.o
diff --git a/drivers/usb/serial/aircable.c b/drivers/usb/serial/aircable.c
index eec4fb9a35c..d634e663563 100644
--- a/drivers/usb/serial/aircable.c
+++ b/drivers/usb/serial/aircable.c
@@ -111,13 +111,14 @@ static int aircable_probe(struct usb_serial *serial,
 	for (i = 0; i < iface_desc->desc.bNumEndpoints; i++) {
 		endpoint = &iface_desc->endpoint[i].desc;
 		if (usb_endpoint_is_bulk_out(endpoint)) {
-			dbg("found bulk out on endpoint %d", i);
+			dev_dbg(&serial->dev->dev,
+				"found bulk out on endpoint %d\n", i);
 			++num_bulk_out;
 		}
 	}
 
 	if (num_bulk_out == 0) {
-		dbg("Invalid interface, discarding");
+		dev_dbg(&serial->dev->dev, "Invalid interface, discarding\n");
 		return -ENODEV;
 	}
 
@@ -133,7 +134,7 @@ static int aircable_process_packet(struct tty_struct *tty,
 		packet += HCI_HEADER_LENGTH;
 	}
 	if (len <= 0) {
-		dbg("%s - malformed packet", __func__);
+		dev_dbg(&port->dev, "%s - malformed packet\n", __func__);
 		return 0;
 	}
 
@@ -170,13 +171,6 @@ static void aircable_process_read_urb(struct urb *urb)
 	tty_kref_put(tty);
 }
 
-static struct usb_driver aircable_driver = {
-	.name =		"aircable",
-	.probe =	usb_serial_probe,
-	.disconnect =	usb_serial_disconnect,
-	.id_table =	id_table,
-};
-
 static struct usb_serial_driver aircable_device = {
 	.driver = {
 		.owner =	THIS_MODULE,
@@ -196,7 +190,7 @@ static struct usb_serial_driver * const serial_drivers[] = {
 	&aircable_device, NULL
 };
 
-module_usb_serial_driver(aircable_driver, serial_drivers);
+module_usb_serial_driver(serial_drivers, id_table);
 
 MODULE_AUTHOR(DRIVER_AUTHOR);
 MODULE_DESCRIPTION(DRIVER_DESC);
diff --git a/drivers/usb/serial/ark3116.c b/drivers/usb/serial/ark3116.c
index f99f47100dd..f8ce97d8b0a 100644
--- a/drivers/usb/serial/ark3116.c
+++ b/drivers/usb/serial/ark3116.c
@@ -265,7 +265,7 @@ static void ark3116_set_termios(struct tty_struct *tty,
 	hcr = (cflag & CRTSCTS) ? 0x03 : 0x00;
 
 	/* calc baudrate */
-	dbg("%s - setting bps to %d", __func__, bps);
+	dev_dbg(&port->dev, "%s - setting bps to %d\n", __func__, bps);
 	eval = 0;
 	switch (bps) {
 	case 0:
@@ -292,8 +292,8 @@ static void ark3116_set_termios(struct tty_struct *tty,
 	/* keep old LCR_SBC bit */
 	lcr |= (priv->lcr & UART_LCR_SBC);
 
-	dbg("%s - setting hcr:0x%02x,lcr:0x%02x,quot:%d",
-	    __func__, hcr, lcr, quot);
+	dev_dbg(&port->dev, "%s - setting hcr:0x%02x,lcr:0x%02x,quot:%d\n",
+		__func__, hcr, lcr, quot);
 
 	/* handshake control */
 	if (priv->hcr != hcr) {
@@ -375,8 +375,9 @@ static int ark3116_open(struct tty_struct *tty, struct usb_serial_port *port)
 
 	result = usb_serial_generic_open(tty, port);
 	if (result) {
-		dbg("%s - usb_serial_generic_open failed: %d",
-		    __func__, result);
+		dev_dbg(&port->dev,
+			"%s - usb_serial_generic_open failed: %d\n",
+			__func__, result);
 		goto err_out;
 	}
 
@@ -622,24 +623,26 @@ static void ark3116_read_int_callback(struct urb *urb)
 	case -ENOENT:
 	case -ESHUTDOWN:
 		/* this urb is terminated, clean up */
-		dbg("%s - urb shutting down with status: %d",
-		    __func__, status);
+		dev_dbg(&port->dev, "%s - urb shutting down with status: %d\n",
+			__func__, status);
 		return;
 	default:
-		dbg("%s - nonzero urb status received: %d",
-		    __func__, status);
+		dev_dbg(&port->dev, "%s - nonzero urb status received: %d\n",
+			__func__, status);
 		break;
 	case 0: /* success */
 		/* discovered this by trail and error... */
 		if ((urb->actual_length == 4) && (data[0] == 0xe8)) {
 			const __u8 id = data[1]&UART_IIR_ID;
-			dbg("%s: iir=%02x", __func__, data[1]);
+			dev_dbg(&port->dev, "%s: iir=%02x\n", __func__, data[1]);
 			if (id == UART_IIR_MSI) {
-				dbg("%s: msr=%02x", __func__, data[3]);
+				dev_dbg(&port->dev, "%s: msr=%02x\n",
+					__func__, data[3]);
 				ark3116_update_msr(port, data[3]);
 				break;
 			} else if (id == UART_IIR_RLSI) {
-				dbg("%s: lsr=%02x", __func__, data[2]);
+				dev_dbg(&port->dev, "%s: lsr=%02x\n",
+					__func__, data[2]);
 				ark3116_update_lsr(port, data[2]);
 				break;
 			}
@@ -714,13 +717,6 @@ static void ark3116_process_read_urb(struct urb *urb)
 	tty_kref_put(tty);
 }
 
-static struct usb_driver ark3116_driver = {
-	.name =		"ark3116",
-	.probe =	usb_serial_probe,
-	.disconnect =	usb_serial_disconnect,
-	.id_table =	id_table,
-};
-
 static struct usb_serial_driver ark3116_device = {
 	.driver = {
 		.owner =	THIS_MODULE,
@@ -747,7 +743,7 @@ static struct usb_serial_driver * const serial_drivers[] = {
 	&ark3116_device, NULL
 };
 
-module_usb_serial_driver(ark3116_driver, serial_drivers);
+module_usb_serial_driver(serial_drivers, id_table);
 
 MODULE_LICENSE("GPL");
 
diff --git a/drivers/usb/serial/belkin_sa.c b/drivers/usb/serial/belkin_sa.c
index a52e0d2cec3..6b736563295 100644
--- a/drivers/usb/serial/belkin_sa.c
+++ b/drivers/usb/serial/belkin_sa.c
@@ -2,17 +2,17 @@
  * Belkin USB Serial Adapter Driver
  *
  *  Copyright (C) 2000		William Greathouse (wgreathouse@smva.com)
- *  Copyright (C) 2000-2001 	Greg Kroah-Hartman (greg@kroah.com)
+ *  Copyright (C) 2000-2001	Greg Kroah-Hartman (greg@kroah.com)
  *  Copyright (C) 2010		Johan Hovold (jhovold@gmail.com)
  *
  *  This program is largely derived from work by the linux-usb group
  *  and associated source files.  Please see the usb/serial files for
  *  individual credits and copyrights.
  *
- * 	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 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.
  *
  * See Documentation/usb/usb-serial.txt for more information on using this
  * driver
@@ -62,7 +62,7 @@ static int  belkin_sa_tiocmset(struct tty_struct *tty,
 					unsigned int set, unsigned int clear);
 
 
-static const struct usb_device_id id_table_combined[] = {
+static const struct usb_device_id id_table[] = {
 	{ USB_DEVICE(BELKIN_SA_VID, BELKIN_SA_PID) },
 	{ USB_DEVICE(BELKIN_OLD_VID, BELKIN_OLD_PID) },
 	{ USB_DEVICE(PERACOM_VID, PERACOM_PID) },
@@ -71,14 +71,7 @@ static const struct usb_device_id id_table_combined[] = {
 	{ USB_DEVICE(BELKIN_DOCKSTATION_VID, BELKIN_DOCKSTATION_PID) },
 	{ }	/* Terminating entry */
 };
-MODULE_DEVICE_TABLE(usb, id_table_combined);
-
-static struct usb_driver belkin_driver = {
-	.name =		"belkin",
-	.probe =	usb_serial_probe,
-	.disconnect =	usb_serial_disconnect,
-	.id_table =	id_table_combined,
-};
+MODULE_DEVICE_TABLE(usb, id_table);
 
 /* All of the device info needed for the serial converters */
 static struct usb_serial_driver belkin_device = {
@@ -87,7 +80,7 @@ static struct usb_serial_driver belkin_device = {
 		.name =		"belkin",
 	},
 	.description =		"Belkin / Peracom / GoHubs USB Serial Adapter",
-	.id_table =		id_table_combined,
+	.id_table =		id_table,
 	.num_ports =		1,
 	.open =			belkin_sa_open,
 	.close =		belkin_sa_close,
@@ -159,8 +152,6 @@ static void belkin_sa_release(struct usb_serial *serial)
 {
 	int i;
 
-	dbg("%s", __func__);
-
 	for (i = 0; i < serial->num_ports; ++i)
 		kfree(usb_get_serial_port_data(serial->port[i]));
 }
@@ -170,8 +161,6 @@ static int belkin_sa_open(struct tty_struct *tty,
 {
 	int retval;
 
-	dbg("%s port %d", __func__, port->number);
-
 	retval = usb_submit_urb(port->interrupt_in_urb, GFP_KERNEL);
 	if (retval) {
 		dev_err(&port->dev, "usb_submit_urb(read int) failed\n");
@@ -187,8 +176,6 @@ static int belkin_sa_open(struct tty_struct *tty,
 
 static void belkin_sa_close(struct usb_serial_port *port)
 {
-	dbg("%s port %d", __func__, port->number);
-
 	usb_serial_generic_close(port);
 	usb_kill_urb(port->interrupt_in_urb);
 }
@@ -210,12 +197,12 @@ static void belkin_sa_read_int_callback(struct urb *urb)
 	case -ENOENT:
 	case -ESHUTDOWN:
 		/* this urb is terminated, clean up */
-		dbg("%s - urb shutting down with status: %d",
-		    __func__, status);
+		dev_dbg(&port->dev, "%s - urb shutting down with status: %d\n",
+			__func__, status);
 		return;
 	default:
-		dbg("%s - nonzero urb status received: %d",
-		    __func__, status);
+		dev_dbg(&port->dev, "%s - nonzero urb status received: %d\n",
+			__func__, status);
 		goto exit;
 	}
 
@@ -403,7 +390,9 @@ static void belkin_sa_set_termios(struct tty_struct *tty,
 		case CS8:
 			urb_value = BELKIN_SA_DATA_BITS(8);
 			break;
-		default: dbg("CSIZE was not CS5-CS8, using default of 8");
+		default:
+			dev_dbg(&port->dev,
+				"CSIZE was not CS5-CS8, using default of 8\n");
 			urb_value = BELKIN_SA_DATA_BITS(8);
 			break;
 		}
@@ -463,8 +452,6 @@ static int belkin_sa_tiocmget(struct tty_struct *tty)
 	unsigned long control_state;
 	unsigned long flags;
 
-	dbg("%s", __func__);
-
 	spin_lock_irqsave(&priv->lock, flags);
 	control_state = priv->control_state;
 	spin_unlock_irqrestore(&priv->lock, flags);
@@ -484,8 +471,6 @@ static int belkin_sa_tiocmset(struct tty_struct *tty,
 	int rts = 0;
 	int dtr = 0;
 
-	dbg("%s", __func__);
-
 	spin_lock_irqsave(&priv->lock, flags);
 	control_state = priv->control_state;
 
@@ -524,7 +509,7 @@ exit:
 	return retval;
 }
 
-module_usb_serial_driver(belkin_driver, serial_drivers);
+module_usb_serial_driver(serial_drivers, id_table);
 
 MODULE_AUTHOR(DRIVER_AUTHOR);
 MODULE_DESCRIPTION(DRIVER_DESC);
diff --git a/drivers/usb/serial/bus.c b/drivers/usb/serial/bus.c
index ed8adb052ca..f398d1e3447 100644
--- a/drivers/usb/serial/bus.c
+++ b/drivers/usb/serial/bus.c
@@ -124,8 +124,15 @@ static ssize_t store_new_id(struct device_driver *driver,
 	return retval;
 }
 
+static ssize_t show_dynids(struct device_driver *driver, char *buf)
+{
+	struct usb_serial_driver *usb_drv = to_usb_serial_driver(driver);
+
+	return usb_show_dynids(&usb_drv->dynids, buf);
+}
+
 static struct driver_attribute drv_attrs[] = {
-	__ATTR(new_id, S_IWUSR, NULL, store_new_id),
+	__ATTR(new_id, S_IRUGO | S_IWUSR, show_dynids, store_new_id),
 	__ATTR_NULL,
 };
 
diff --git a/drivers/usb/serial/ch341.c b/drivers/usb/serial/ch341.c
index aaab32db31d..cabd1b15ddc 100644
--- a/drivers/usb/serial/ch341.c
+++ b/drivers/usb/serial/ch341.c
@@ -125,8 +125,6 @@ static int ch341_set_baudrate(struct usb_device *dev,
 	unsigned long factor;
 	short divisor;
 
-	dbg("ch341_set_baudrate(%d)", priv->baud_rate);
-
 	if (!priv->baud_rate)
 		return -EINVAL;
 	factor = (CH341_BAUDBASE_FACTOR / priv->baud_rate);
@@ -153,7 +151,6 @@ static int ch341_set_baudrate(struct usb_device *dev,
 
 static int ch341_set_handshake(struct usb_device *dev, u8 control)
 {
-	dbg("ch341_set_handshake(0x%02x)", control);
 	return ch341_control_out(dev, 0xa4, ~control, 0);
 }
 
@@ -164,8 +161,6 @@ static int ch341_get_status(struct usb_device *dev, struct ch341_private *priv)
 	const unsigned size = 8;
 	unsigned long flags;
 
-	dbg("ch341_get_status()");
-
 	buffer = kmalloc(size, GFP_KERNEL);
 	if (!buffer)
 		return -ENOMEM;
@@ -196,8 +191,6 @@ static int ch341_configure(struct usb_device *dev, struct ch341_private *priv)
 	int r;
 	const unsigned size = 8;
 
-	dbg("ch341_configure()");
-
 	buffer = kmalloc(size, GFP_KERNEL);
 	if (!buffer)
 		return -ENOMEM;
@@ -254,8 +247,6 @@ static int ch341_attach(struct usb_serial *serial)
 	struct ch341_private *priv;
 	int r;
 
-	dbg("ch341_attach()");
-
 	/* private data */
 	priv = kzalloc(sizeof(struct ch341_private), GFP_KERNEL);
 	if (!priv)
@@ -290,7 +281,6 @@ static void ch341_dtr_rts(struct usb_serial_port *port, int on)
 	struct ch341_private *priv = usb_get_serial_port_data(port);
 	unsigned long flags;
 
-	dbg("%s - port %d", __func__, port->number);
 	/* drop DTR and RTS */
 	spin_lock_irqsave(&priv->lock, flags);
 	if (on)
@@ -304,8 +294,6 @@ static void ch341_dtr_rts(struct usb_serial_port *port, int on)
 
 static void ch341_close(struct usb_serial_port *port)
 {
-	dbg("%s - port %d", __func__, port->number);
-
 	usb_serial_generic_close(port);
 	usb_kill_urb(port->interrupt_in_urb);
 }
@@ -318,8 +306,6 @@ static int ch341_open(struct tty_struct *tty, struct usb_serial_port *port)
 	struct ch341_private *priv = usb_get_serial_port_data(serial->port[0]);
 	int r;
 
-	dbg("ch341_open()");
-
 	priv->baud_rate = DEFAULT_BAUD_RATE;
 
 	r = ch341_configure(serial->dev, priv);
@@ -358,8 +344,6 @@ static void ch341_set_termios(struct tty_struct *tty,
 	unsigned baud_rate;
 	unsigned long flags;
 
-	dbg("ch341_set_termios()");
-
 	baud_rate = tty_get_baud_rate(tty);
 
 	priv->baud_rate = baud_rate;
@@ -393,8 +377,6 @@ static void ch341_break_ctl(struct tty_struct *tty, int break_state)
 	uint16_t reg_contents;
 	uint8_t *break_reg;
 
-	dbg("%s()", __func__);
-
 	break_reg = kmalloc(2, GFP_KERNEL);
 	if (!break_reg) {
 		dev_err(&port->dev, "%s - kmalloc failed\n", __func__);
@@ -461,8 +443,6 @@ static void ch341_read_int_callback(struct urb *urb)
 	unsigned int actual_length = urb->actual_length;
 	int status;
 
-	dbg("%s (%d)", __func__, port->number);
-
 	switch (urb->status) {
 	case 0:
 		/* success */
@@ -580,8 +560,6 @@ static int ch341_tiocmget(struct tty_struct *tty)
 	u8 status;
 	unsigned int result;
 
-	dbg("%s (%d)", __func__, port->number);
-
 	spin_lock_irqsave(&priv->lock, flags);
 	mcr = priv->line_control;
 	status = priv->line_status;
@@ -599,35 +577,18 @@ static int ch341_tiocmget(struct tty_struct *tty)
 	return result;
 }
 
-
-static int ch341_reset_resume(struct usb_interface *intf)
+static int ch341_reset_resume(struct usb_serial *serial)
 {
-	struct usb_device *dev = interface_to_usbdev(intf);
-	struct usb_serial *serial = NULL;
 	struct ch341_private *priv;
 
-	serial = usb_get_intfdata(intf);
 	priv = usb_get_serial_port_data(serial->port[0]);
 
-	/*reconfigure ch341 serial port after bus-reset*/
-	ch341_configure(dev, priv);
-
-	usb_serial_resume(intf);
+	/* reconfigure ch341 serial port after bus-reset */
+	ch341_configure(serial->dev, priv);
 
 	return 0;
 }
 
-static struct usb_driver ch341_driver = {
-	.name		= "ch341",
-	.probe		= usb_serial_probe,
-	.disconnect	= usb_serial_disconnect,
-	.suspend	= usb_serial_suspend,
-	.resume		= usb_serial_resume,
-	.reset_resume	= ch341_reset_resume,
-	.id_table	= id_table,
-	.supports_autosuspend =	1,
-};
-
 static struct usb_serial_driver ch341_device = {
 	.driver = {
 		.owner	= THIS_MODULE,
@@ -646,13 +607,14 @@ static struct usb_serial_driver ch341_device = {
 	.tiocmset          = ch341_tiocmset,
 	.read_int_callback = ch341_read_int_callback,
 	.attach            = ch341_attach,
+	.reset_resume      = ch341_reset_resume,
 };
 
 static struct usb_serial_driver * const serial_drivers[] = {
 	&ch341_device, NULL
 };
 
-module_usb_serial_driver(ch341_driver, serial_drivers);
+module_usb_serial_driver(serial_drivers, id_table);
 
 MODULE_LICENSE("GPL");
 
diff --git a/drivers/usb/serial/console.c b/drivers/usb/serial/console.c
index 1ee6b2ab0f8..b9cca6dcde0 100644
--- a/drivers/usb/serial/console.c
+++ b/drivers/usb/serial/console.c
@@ -113,7 +113,8 @@ static int usb_console_setup(struct console *co, char *options)
 	serial = usb_serial_get_by_index(co->index);
 	if (serial == NULL) {
 		/* no device is connected yet, sorry :( */
-		err("No USB device connected to ttyUSB%i", co->index);
+		printk(KERN_ERR "No USB device connected to ttyUSB%i\n",
+		       co->index);
 		return -ENODEV;
 	}
 
@@ -137,7 +138,7 @@ static int usb_console_setup(struct console *co, char *options)
 			tty = kzalloc(sizeof(*tty), GFP_KERNEL);
 			if (!tty) {
 				retval = -ENOMEM;
-				err("no more memory");
+				dev_err(&port->dev, "no more memory\n");
 				goto reset_open_count;
 			}
 			kref_init(&tty->kref);
@@ -146,7 +147,7 @@ static int usb_console_setup(struct console *co, char *options)
 			tty->index = co->index;
 			if (tty_init_termios(tty)) {
 				retval = -ENOMEM;
-				err("no more memory");
+				dev_err(&port->dev, "no more memory\n");
 				goto free_tty;
 			}
 		}
@@ -159,7 +160,7 @@ static int usb_console_setup(struct console *co, char *options)
 			retval = usb_serial_generic_open(NULL, port);
 
 		if (retval) {
-			err("could not open USB console port");
+			dev_err(&port->dev, "could not open USB console port\n");
 			goto fail;
 		}
 
diff --git a/drivers/usb/serial/cp210x.c b/drivers/usb/serial/cp210x.c
index ec30f95ef39..1b1926200ba 100644
--- a/drivers/usb/serial/cp210x.c
+++ b/drivers/usb/serial/cp210x.c
@@ -156,13 +156,6 @@ struct cp210x_port_private {
 	__u8			bInterfaceNumber;
 };
 
-static struct usb_driver cp210x_driver = {
-	.name		= "cp210x",
-	.probe		= usb_serial_probe,
-	.disconnect	= usb_serial_disconnect,
-	.id_table	= id_table,
-};
-
 static struct usb_serial_driver cp210x_device = {
 	.driver = {
 		.owner =	THIS_MODULE,
@@ -188,8 +181,10 @@ static struct usb_serial_driver * const serial_drivers[] = {
 };
 
 /* Config request types */
-#define REQTYPE_HOST_TO_DEVICE	0x41
-#define REQTYPE_DEVICE_TO_HOST	0xc1
+#define REQTYPE_HOST_TO_INTERFACE	0x41
+#define REQTYPE_INTERFACE_TO_HOST	0xc1
+#define REQTYPE_HOST_TO_DEVICE	0x40
+#define REQTYPE_DEVICE_TO_HOST	0xc0
 
 /* Config request codes */
 #define CP210X_IFC_ENABLE	0x00
@@ -286,7 +281,7 @@ static int cp210x_get_config(struct usb_serial_port *port, u8 request,
 
 	/* Issue the request, attempting to read 'size' bytes */
 	result = usb_control_msg(serial->dev, usb_rcvctrlpipe(serial->dev, 0),
-				request, REQTYPE_DEVICE_TO_HOST, 0x0000,
+				request, REQTYPE_INTERFACE_TO_HOST, 0x0000,
 				port_priv->bInterfaceNumber, buf, size,
 				USB_CTRL_GET_TIMEOUT);
 
@@ -340,13 +335,13 @@ static int cp210x_set_config(struct usb_serial_port *port, u8 request,
 	if (size > 2) {
 		result = usb_control_msg(serial->dev,
 				usb_sndctrlpipe(serial->dev, 0),
-				request, REQTYPE_HOST_TO_DEVICE, 0x0000,
+				request, REQTYPE_HOST_TO_INTERFACE, 0x0000,
 				port_priv->bInterfaceNumber, buf, size,
 				USB_CTRL_SET_TIMEOUT);
 	} else {
 		result = usb_control_msg(serial->dev,
 				usb_sndctrlpipe(serial->dev, 0),
-				request, REQTYPE_HOST_TO_DEVICE, data[0],
+				request, REQTYPE_HOST_TO_INTERFACE, data[0],
 				port_priv->bInterfaceNumber, NULL, 0,
 				USB_CTRL_SET_TIMEOUT);
 	}
@@ -422,8 +417,6 @@ static int cp210x_open(struct tty_struct *tty, struct usb_serial_port *port)
 {
 	int result;
 
-	dbg("%s - port %d", __func__, port->number);
-
 	result = cp210x_set_config_single(port, CP210X_IFC_ENABLE,
 								UART_ENABLE);
 	if (result) {
@@ -443,8 +436,6 @@ static int cp210x_open(struct tty_struct *tty, struct usb_serial_port *port)
 
 static void cp210x_close(struct usb_serial_port *port)
 {
-	dbg("%s - port %d", __func__, port->number);
-
 	usb_serial_generic_close(port);
 
 	mutex_lock(&port->serial->disc_mutex);
@@ -488,8 +479,6 @@ static void cp210x_get_termios_port(struct usb_serial_port *port,
 	unsigned int baud;
 	unsigned int bits;
 
-	dbg("%s - port %d", __func__, port->number);
-
 	cp210x_get_config(port, CP210X_GET_BAUDRATE, &baud, 4);
 
 	dbg("%s - baud rate = %d", __func__, baud);
@@ -787,8 +776,6 @@ static int cp210x_tiocmset_port(struct usb_serial_port *port,
 {
 	unsigned int control = 0;
 
-	dbg("%s - port %d", __func__, port->number);
-
 	if (set & TIOCM_RTS) {
 		control |= CONTROL_RTS;
 		control |= CONTROL_WRITE_RTS;
@@ -825,8 +812,6 @@ static int cp210x_tiocmget (struct tty_struct *tty)
 	unsigned int control;
 	int result;
 
-	dbg("%s - port %d", __func__, port->number);
-
 	cp210x_get_config(port, CP210X_GET_MDMSTS, &control, 1);
 
 	result = ((control & CONTROL_DTR) ? TIOCM_DTR : 0)
@@ -846,7 +831,6 @@ static void cp210x_break_ctl (struct tty_struct *tty, int break_state)
 	struct usb_serial_port *port = tty->driver_data;
 	unsigned int state;
 
-	dbg("%s - port %d", __func__, port->number);
 	if (break_state == 0)
 		state = BREAK_OFF;
 	else
@@ -891,7 +875,7 @@ static void cp210x_release(struct usb_serial *serial)
 	}
 }
 
-module_usb_serial_driver(cp210x_driver, serial_drivers);
+module_usb_serial_driver(serial_drivers, id_table);
 
 MODULE_DESCRIPTION(DRIVER_DESC);
 MODULE_VERSION(DRIVER_VERSION);
diff --git a/drivers/usb/serial/cyberjack.c b/drivers/usb/serial/cyberjack.c
index d39b9418f2f..3aa0b530f68 100644
--- a/drivers/usb/serial/cyberjack.c
+++ b/drivers/usb/serial/cyberjack.c
@@ -77,13 +77,6 @@ static const struct usb_device_id id_table[] = {
 
 MODULE_DEVICE_TABLE(usb, id_table);
 
-static struct usb_driver cyberjack_driver = {
-	.name =		"cyberjack",
-	.probe =	usb_serial_probe,
-	.disconnect =	usb_serial_disconnect,
-	.id_table =	id_table,
-};
-
 static struct usb_serial_driver cyberjack_device = {
 	.driver = {
 		.owner =	THIS_MODULE,
@@ -122,8 +115,6 @@ static int cyberjack_startup(struct usb_serial *serial)
 	struct cyberjack_private *priv;
 	int i;
 
-	dbg("%s", __func__);
-
 	/* allocate the private data structure */
 	priv = kmalloc(sizeof(struct cyberjack_private), GFP_KERNEL);
 	if (!priv)
@@ -155,8 +146,6 @@ static void cyberjack_disconnect(struct usb_serial *serial)
 {
 	int i;
 
-	dbg("%s", __func__);
-
 	for (i = 0; i < serial->num_ports; ++i)
 		usb_kill_urb(serial->port[i]->interrupt_in_urb);
 }
@@ -165,8 +154,6 @@ static void cyberjack_release(struct usb_serial *serial)
 {
 	int i;
 
-	dbg("%s", __func__);
-
 	for (i = 0; i < serial->num_ports; ++i) {
 		/* My special items, the standard routines free my urbs */
 		kfree(usb_get_serial_port_data(serial->port[i]));
@@ -180,8 +167,6 @@ static int  cyberjack_open(struct tty_struct *tty,
 	unsigned long flags;
 	int result = 0;
 
-	dbg("%s - port %d", __func__, port->number);
-
 	dbg("%s - usb_clear_halt", __func__);
 	usb_clear_halt(port->serial->dev, port->write_urb->pipe);
 
@@ -197,8 +182,6 @@ static int  cyberjack_open(struct tty_struct *tty,
 
 static void cyberjack_close(struct usb_serial_port *port)
 {
-	dbg("%s - port %d", __func__, port->number);
-
 	if (port->serial->dev) {
 		/* shutdown any bulk reads that might be going on */
 		usb_kill_urb(port->write_urb);
@@ -214,8 +197,6 @@ static int cyberjack_write(struct tty_struct *tty,
 	int result;
 	int wrexpected;
 
-	dbg("%s - port %d", __func__, port->number);
-
 	if (count == 0) {
 		dbg("%s - write request of 0 bytes", __func__);
 		return 0;
@@ -307,8 +288,6 @@ static void cyberjack_read_int_callback(struct urb *urb)
 	int status = urb->status;
 	int result;
 
-	dbg("%s - port %d", __func__, port->number);
-
 	/* the urb might have been killed. */
 	if (status)
 		return;
@@ -367,8 +346,6 @@ static void cyberjack_read_bulk_callback(struct urb *urb)
 	int result;
 	int status = urb->status;
 
-	dbg("%s - port %d", __func__, port->number);
-
 	usb_serial_debug_data(debug, &port->dev, __func__,
 						urb->actual_length, data);
 	if (status) {
@@ -417,8 +394,6 @@ static void cyberjack_write_bulk_callback(struct urb *urb)
 	struct cyberjack_private *priv = usb_get_serial_port_data(port);
 	int status = urb->status;
 
-	dbg("%s - port %d", __func__, port->number);
-
 	set_bit(0, &port->write_urbs_free);
 	if (status) {
 		dbg("%s - nonzero write bulk status received: %d",
@@ -475,7 +450,7 @@ exit:
 	usb_serial_port_softint(port);
 }
 
-module_usb_serial_driver(cyberjack_driver, serial_drivers);
+module_usb_serial_driver(serial_drivers, id_table);
 
 MODULE_AUTHOR(DRIVER_AUTHOR);
 MODULE_DESCRIPTION(DRIVER_DESC);
diff --git a/drivers/usb/serial/cypress_m8.c b/drivers/usb/serial/cypress_m8.c
index afc886c75d2..b78c34eb5d3 100644
--- a/drivers/usb/serial/cypress_m8.c
+++ b/drivers/usb/serial/cypress_m8.c
@@ -89,13 +89,6 @@ static const struct usb_device_id id_table_combined[] = {
 
 MODULE_DEVICE_TABLE(usb, id_table_combined);
 
-static struct usb_driver cypress_driver = {
-	.name =		"cypress",
-	.probe =	usb_serial_probe,
-	.disconnect =	usb_serial_disconnect,
-	.id_table =	id_table_combined,
-};
-
 enum packet_format {
 	packet_format_1,  /* b0:status, b1:payload count */
 	packet_format_2   /* b0[7:3]:status, b0[2:0]:payload count */
@@ -305,8 +298,6 @@ static int cypress_serial_control(struct tty_struct *tty,
 	const unsigned int feature_len = 5;
 	unsigned long flags;
 
-	dbg("%s", __func__);
-
 	priv = usb_get_serial_port_data(port);
 
 	if (!priv->comm_is_ok)
@@ -451,8 +442,6 @@ static int generic_startup(struct usb_serial *serial)
 	struct cypress_private *priv;
 	struct usb_serial_port *port = serial->port[0];
 
-	dbg("%s - port %d", __func__, port->number);
-
 	priv = kzalloc(sizeof(struct cypress_private), GFP_KERNEL);
 	if (!priv)
 		return -ENOMEM;
@@ -505,8 +494,6 @@ static int cypress_earthmate_startup(struct usb_serial *serial)
 	struct cypress_private *priv;
 	struct usb_serial_port *port = serial->port[0];
 
-	dbg("%s", __func__);
-
 	if (generic_startup(serial)) {
 		dbg("%s - Failed setting up port %d", __func__,
 				port->number);
@@ -537,8 +524,6 @@ static int cypress_hidcom_startup(struct usb_serial *serial)
 {
 	struct cypress_private *priv;
 
-	dbg("%s", __func__);
-
 	if (generic_startup(serial)) {
 		dbg("%s - Failed setting up port %d", __func__,
 				serial->port[0]->number);
@@ -556,8 +541,6 @@ static int cypress_ca42v2_startup(struct usb_serial *serial)
 {
 	struct cypress_private *priv;
 
-	dbg("%s", __func__);
-
 	if (generic_startup(serial)) {
 		dbg("%s - Failed setting up port %d", __func__,
 				serial->port[0]->number);
@@ -575,10 +558,7 @@ static void cypress_release(struct usb_serial *serial)
 {
 	struct cypress_private *priv;
 
-	dbg("%s - port %d", __func__, serial->port[0]->number);
-
 	/* all open ports are closed at this point */
-
 	priv = usb_get_serial_port_data(serial->port[0]);
 
 	if (priv) {
@@ -595,8 +575,6 @@ static int cypress_open(struct tty_struct *tty, struct usb_serial_port *port)
 	unsigned long flags;
 	int result = 0;
 
-	dbg("%s - port %d", __func__, port->number);
-
 	if (!priv->comm_is_ok)
 		return -EIO;
 
@@ -661,8 +639,6 @@ static void cypress_close(struct usb_serial_port *port)
 	struct cypress_private *priv = usb_get_serial_port_data(port);
 	unsigned long flags;
 
-	dbg("%s - port %d", __func__, port->number);
-
 	/* writing is potentially harmful, lock must be taken */
 	mutex_lock(&port->serial->disc_mutex);
 	if (port->serial->disconnected) {
@@ -720,7 +696,6 @@ static void cypress_send(struct usb_serial_port *port)
 	if (!priv->comm_is_ok)
 		return;
 
-	dbg("%s - port %d", __func__, port->number);
 	dbg("%s - interrupt out size is %d", __func__,
 						port->interrupt_out_size);
 
@@ -828,8 +803,6 @@ static int cypress_write_room(struct tty_struct *tty)
 	int room = 0;
 	unsigned long flags;
 
-	dbg("%s - port %d", __func__, port->number);
-
 	spin_lock_irqsave(&priv->lock, flags);
 	room = kfifo_avail(&priv->write_fifo);
 	spin_unlock_irqrestore(&priv->lock, flags);
@@ -847,8 +820,6 @@ static int cypress_tiocmget(struct tty_struct *tty)
 	unsigned int result = 0;
 	unsigned long flags;
 
-	dbg("%s - port %d", __func__, port->number);
-
 	spin_lock_irqsave(&priv->lock, flags);
 	control = priv->line_control;
 	status = priv->current_status;
@@ -874,8 +845,6 @@ static int cypress_tiocmset(struct tty_struct *tty,
 	struct cypress_private *priv = usb_get_serial_port_data(port);
 	unsigned long flags;
 
-	dbg("%s - port %d", __func__, port->number);
-
 	spin_lock_irqsave(&priv->lock, flags);
 	if (set & TIOCM_RTS)
 		priv->line_control |= CONTROL_RTS;
@@ -948,8 +917,6 @@ static void cypress_set_termios(struct tty_struct *tty,
 	__u8 oldlines;
 	int linechange = 0;
 
-	dbg("%s - port %d", __func__, port->number);
-
 	spin_lock_irqsave(&priv->lock, flags);
 	/* We can't clean this one up as we don't know the device type
 	   early enough */
@@ -1096,8 +1063,6 @@ static int cypress_chars_in_buffer(struct tty_struct *tty)
 	int chars = 0;
 	unsigned long flags;
 
-	dbg("%s - port %d", __func__, port->number);
-
 	spin_lock_irqsave(&priv->lock, flags);
 	chars = kfifo_len(&priv->write_fifo);
 	spin_unlock_irqrestore(&priv->lock, flags);
@@ -1112,8 +1077,6 @@ static void cypress_throttle(struct tty_struct *tty)
 	struct usb_serial_port *port = tty->driver_data;
 	struct cypress_private *priv = usb_get_serial_port_data(port);
 
-	dbg("%s - port %d", __func__, port->number);
-
 	spin_lock_irq(&priv->lock);
 	priv->rx_flags = THROTTLED;
 	spin_unlock_irq(&priv->lock);
@@ -1126,8 +1089,6 @@ static void cypress_unthrottle(struct tty_struct *tty)
 	struct cypress_private *priv = usb_get_serial_port_data(port);
 	int actually_throttled, result;
 
-	dbg("%s - port %d", __func__, port->number);
-
 	spin_lock_irq(&priv->lock);
 	actually_throttled = priv->rx_flags & ACTUALLY_THROTTLED;
 	priv->rx_flags = 0;
@@ -1161,8 +1122,6 @@ static void cypress_read_int_callback(struct urb *urb)
 	int i = 0;
 	int status = urb->status;
 
-	dbg("%s - port %d", __func__, port->number);
-
 	switch (status) {
 	case 0: /* success */
 		break;
@@ -1303,8 +1262,6 @@ static void cypress_write_int_callback(struct urb *urb)
 	int result;
 	int status = urb->status;
 
-	dbg("%s - port %d", __func__, port->number);
-
 	switch (status) {
 	case 0:
 		/* success */
@@ -1346,7 +1303,7 @@ static void cypress_write_int_callback(struct urb *urb)
 	cypress_send(port);
 }
 
-module_usb_serial_driver(cypress_driver, serial_drivers);
+module_usb_serial_driver(serial_drivers, id_table_combined);
 
 MODULE_AUTHOR(DRIVER_AUTHOR);
 MODULE_DESCRIPTION(DRIVER_DESC);
diff --git a/drivers/usb/serial/digi_acceleport.c b/drivers/usb/serial/digi_acceleport.c
index 999f91bf70d..b5cd838093e 100644
--- a/drivers/usb/serial/digi_acceleport.c
+++ b/drivers/usb/serial/digi_acceleport.c
@@ -271,14 +271,6 @@ static const struct usb_device_id id_table_4[] = {
 
 MODULE_DEVICE_TABLE(usb, id_table_combined);
 
-static struct usb_driver digi_driver = {
-	.name =		"digi_acceleport",
-	.probe =	usb_serial_probe,
-	.disconnect =	usb_serial_disconnect,
-	.id_table =	id_table_combined,
-};
-
-
 /* device info needed for the Digi serial converter */
 
 static struct usb_serial_driver digi_acceleport_2_device = {
@@ -657,9 +649,6 @@ static void digi_rx_throttle(struct tty_struct *tty)
 	struct usb_serial_port *port = tty->driver_data;
 	struct digi_port *priv = usb_get_serial_port_data(port);
 
-
-	dbg("digi_rx_throttle: TOP: port=%d", priv->dp_port_num);
-
 	/* stop receiving characters by not resubmitting the read urb */
 	spin_lock_irqsave(&priv->dp_port_lock, flags);
 	priv->dp_throttled = 1;
@@ -675,8 +664,6 @@ static void digi_rx_unthrottle(struct tty_struct *tty)
 	struct usb_serial_port *port = tty->driver_data;
 	struct digi_port *priv = usb_get_serial_port_data(port);
 
-	dbg("digi_rx_unthrottle: TOP: port=%d", priv->dp_port_num);
-
 	spin_lock_irqsave(&priv->dp_port_lock, flags);
 
 	/* restart read chain */
@@ -904,8 +891,6 @@ static int digi_tiocmget(struct tty_struct *tty)
 	unsigned int val;
 	unsigned long flags;
 
-	dbg("%s: TOP: port=%d", __func__, priv->dp_port_num);
-
 	spin_lock_irqsave(&priv->dp_port_lock, flags);
 	val = priv->dp_modem_signals;
 	spin_unlock_irqrestore(&priv->dp_port_lock, flags);
@@ -921,8 +906,6 @@ static int digi_tiocmset(struct tty_struct *tty,
 	unsigned int val;
 	unsigned long flags;
 
-	dbg("%s: TOP: port=%d", __func__, priv->dp_port_num);
-
 	spin_lock_irqsave(&priv->dp_port_lock, flags);
 	val = (priv->dp_modem_signals & ~clear) | set;
 	spin_unlock_irqrestore(&priv->dp_port_lock, flags);
@@ -1013,8 +996,6 @@ static void digi_write_bulk_callback(struct urb *urb)
 	int ret = 0;
 	int status = urb->status;
 
-	dbg("digi_write_bulk_callback: TOP, status=%d", status);
-
 	/* port and serial sanity check */
 	if (port == NULL || (priv = usb_get_serial_port_data(port)) == NULL) {
 		pr_err("%s: port or port->private is NULL, status=%d\n",
@@ -1121,8 +1102,6 @@ static int digi_open(struct tty_struct *tty, struct usb_serial_port *port)
 	struct digi_port *priv = usb_get_serial_port_data(port);
 	struct ktermios not_termios;
 
-	dbg("digi_open: TOP: port=%d", priv->dp_port_num);
-
 	/* be sure the device is started up */
 	if (digi_startup_device(port->serial) != 0)
 		return -ENXIO;
@@ -1160,8 +1139,6 @@ static void digi_close(struct usb_serial_port *port)
 	unsigned char buf[32];
 	struct digi_port *priv = usb_get_serial_port_data(port);
 
-	dbg("digi_close: TOP: port=%d", priv->dp_port_num);
-
 	mutex_lock(&port->serial->disc_mutex);
 	/* if disconnected, just clear flags */
 	if (port->serial->disconnected)
@@ -1220,7 +1197,6 @@ exit:
 	wake_up_interruptible(&priv->dp_close_wait);
 	spin_unlock_irq(&priv->dp_port_lock);
 	mutex_unlock(&port->serial->disc_mutex);
-	dbg("digi_close: done");
 }
 
 
@@ -1269,8 +1245,6 @@ static int digi_startup(struct usb_serial *serial)
 	struct digi_port *priv;
 	struct digi_serial *serial_priv;
 
-	dbg("digi_startup: TOP");
-
 	/* allocate the private data structures for all ports */
 	/* number of regular ports + 1 for the out-of-band port */
 	for (i = 0; i < serial->type->num_ports + 1; i++) {
@@ -1325,7 +1299,6 @@ static int digi_startup(struct usb_serial *serial)
 static void digi_disconnect(struct usb_serial *serial)
 {
 	int i;
-	dbg("digi_disconnect: TOP, in_interrupt()=%ld", in_interrupt());
 
 	/* stop reads and writes on all ports */
 	for (i = 0; i < serial->type->num_ports + 1; i++) {
@@ -1338,7 +1311,6 @@ static void digi_disconnect(struct usb_serial *serial)
 static void digi_release(struct usb_serial *serial)
 {
 	int i;
-	dbg("digi_release: TOP, in_interrupt()=%ld", in_interrupt());
 
 	/* free the private data structures for all ports */
 	/* number of regular ports + 1 for the out-of-band port */
@@ -1356,8 +1328,6 @@ static void digi_read_bulk_callback(struct urb *urb)
 	int ret;
 	int status = urb->status;
 
-	dbg("digi_read_bulk_callback: TOP");
-
 	/* port sanity check, do not resubmit if port is not valid */
 	if (port == NULL)
 		return;
@@ -1507,9 +1477,6 @@ static int digi_read_oob_callback(struct urb *urb)
 	int i;
 	unsigned int rts;
 
-	dbg("digi_read_oob_callback: port=%d, len=%d",
-			priv->dp_port_num, urb->actual_length);
-
 	/* handle each oob command */
 	for (i = 0; i < urb->actual_length - 3;) {
 		opcode = ((unsigned char *)urb->transfer_buffer)[i++];
@@ -1580,7 +1547,7 @@ static int digi_read_oob_callback(struct urb *urb)
 
 }
 
-module_usb_serial_driver(digi_driver, serial_drivers);
+module_usb_serial_driver(serial_drivers, id_table_combined);
 
 MODULE_AUTHOR(DRIVER_AUTHOR);
 MODULE_DESCRIPTION(DRIVER_DESC);
diff --git a/drivers/usb/serial/empeg.c b/drivers/usb/serial/empeg.c
index 5b99fc09e32..cdf61dd0731 100644
--- a/drivers/usb/serial/empeg.c
+++ b/drivers/usb/serial/empeg.c
@@ -51,13 +51,6 @@ static const struct usb_device_id id_table[] = {
 
 MODULE_DEVICE_TABLE(usb, id_table);
 
-static struct usb_driver empeg_driver = {
-	.name =		"empeg",
-	.probe =	usb_serial_probe,
-	.disconnect =	usb_serial_disconnect,
-	.id_table =	id_table,
-};
-
 static struct usb_serial_driver empeg_device = {
 	.driver = {
 		.owner =	THIS_MODULE,
@@ -80,14 +73,12 @@ static int empeg_startup(struct usb_serial *serial)
 {
 	int r;
 
-	dbg("%s", __func__);
-
 	if (serial->dev->actconfig->desc.bConfigurationValue != 1) {
 		dev_err(&serial->dev->dev, "active config #%d != 1 ??\n",
 			serial->dev->actconfig->desc.bConfigurationValue);
 		return -ENODEV;
 	}
-	dbg("%s - reset config", __func__);
+
 	r = usb_reset_configuration(serial->dev);
 
 	/* continue on with initialization */
@@ -138,7 +129,7 @@ static void empeg_init_termios(struct tty_struct *tty)
 	tty_encode_baud_rate(tty, 115200, 115200);
 }
 
-module_usb_serial_driver(empeg_driver, serial_drivers);
+module_usb_serial_driver(serial_drivers, id_table);
 
 MODULE_AUTHOR(DRIVER_AUTHOR);
 MODULE_DESCRIPTION(DRIVER_DESC);
diff --git a/drivers/usb/serial/ezusb.c b/drivers/usb/serial/ezusb.c
index 3cfc762f505..800e8eb6000 100644
--- a/drivers/usb/serial/ezusb.c
+++ b/drivers/usb/serial/ezusb.c
@@ -26,7 +26,6 @@ int ezusb_writememory(struct usb_serial *serial, int address,
 	int result;
 	unsigned char *transfer_buffer;
 
-	/* dbg("ezusb_writememory %x, %d", address, length); */
 	if (!serial->dev) {
 		printk(KERN_ERR "ezusb: %s - no physical device present, "
 		       "failing.\n", __func__);
@@ -50,7 +49,6 @@ int ezusb_set_reset(struct usb_serial *serial, unsigned char reset_bit)
 {
 	int response;
 
-	/* dbg("%s - %d", __func__, reset_bit); */
 	response = ezusb_writememory(serial, CPUCS_REG, &reset_bit, 1, 0xa0);
 	if (response < 0)
 		dev_err(&serial->dev->dev, "%s- %d failed\n",
diff --git a/drivers/usb/serial/f81232.c b/drivers/usb/serial/f81232.c
index 88c0b196392..499b15fd82f 100644
--- a/drivers/usb/serial/f81232.c
+++ b/drivers/usb/serial/f81232.c
@@ -68,8 +68,6 @@ static void f81232_read_int_callback(struct urb *urb)
 	int status = urb->status;
 	int retval;
 
-	dbg("%s (%d)", __func__, port->number);
-
 	switch (status) {
 	case 0:
 		/* success */
@@ -78,12 +76,12 @@ static void f81232_read_int_callback(struct urb *urb)
 	case -ENOENT:
 	case -ESHUTDOWN:
 		/* this urb is terminated, clean up */
-		dbg("%s - urb shutting down with status: %d", __func__,
-		    status);
+		dev_dbg(&port->dev, "%s - urb shutting down with status: %d\n",
+			__func__, status);
 		return;
 	default:
-		dbg("%s - nonzero urb status received: %d", __func__,
-		    status);
+		dev_dbg(&port->dev, "%s - nonzero urb status received: %d\n",
+			__func__, status);
 		goto exit;
 	}
 
@@ -133,7 +131,7 @@ static void f81232_process_read_urb(struct urb *urb)
 		tty_flag = TTY_PARITY;
 	else if (line_status & UART_FRAME_ERROR)
 		tty_flag = TTY_FRAME;
-	dbg("%s - tty_flag = %d", __func__, tty_flag);
+	dev_dbg(&port->dev, "%s - tty_flag = %d\n", __func__, tty_flag);
 
 	/* overrun is special, not associated with a char */
 	if (line_status & UART_OVERRUN_ERROR)
@@ -203,7 +201,6 @@ static int f81232_open(struct tty_struct *tty, struct usb_serial_port *port)
 	if (tty)
 		f81232_set_termios(tty, port, &tmp_termios);
 
-	dbg("%s - submitting interrupt urb", __func__);
 	result = usb_submit_urb(port->interrupt_in_urb, GFP_KERNEL);
 	if (result) {
 		dev_err(&port->dev, "%s - failed submitting interrupt urb,"
@@ -293,7 +290,9 @@ static int f81232_ioctl(struct tty_struct *tty,
 {
 	struct serial_struct ser;
 	struct usb_serial_port *port = tty->driver_data;
-	dbg("%s (%d) cmd = 0x%04x", __func__, port->number, cmd);
+
+	dev_dbg(&port->dev, "%s (%d) cmd = 0x%04x\n", __func__,
+		port->number, cmd);
 
 	switch (cmd) {
 	case TIOCGSERIAL:
@@ -309,10 +308,12 @@ static int f81232_ioctl(struct tty_struct *tty,
 		return 0;
 
 	case TIOCMIWAIT:
-		dbg("%s (%d) TIOCMIWAIT", __func__,  port->number);
+		dev_dbg(&port->dev, "%s (%d) TIOCMIWAIT\n", __func__,
+			port->number);
 		return wait_modem_info(port, arg);
 	default:
-		dbg("%s not supported = 0x%04x", __func__, cmd);
+		dev_dbg(&port->dev, "%s not supported = 0x%04x\n",
+			__func__, cmd);
 		break;
 	}
 	return -ENOIOCTLCMD;
@@ -353,24 +354,12 @@ static void f81232_release(struct usb_serial *serial)
 	}
 }
 
-static struct usb_driver f81232_driver = {
-	.name =		"f81232",
-	.probe =	usb_serial_probe,
-	.disconnect =	usb_serial_disconnect,
-	.id_table =	id_table,
-	.suspend =      usb_serial_suspend,
-	.resume =       usb_serial_resume,
-	.no_dynamic_id = 	1,
-	.supports_autosuspend =	1,
-};
-
 static struct usb_serial_driver f81232_device = {
 	.driver = {
 		.owner =	THIS_MODULE,
 		.name =		"f81232",
 	},
 	.id_table =		id_table,
-	.usb_driver = 		&f81232_driver,
 	.num_ports =		1,
 	.bulk_in_size =		256,
 	.bulk_out_size =	256,
@@ -394,7 +383,7 @@ static struct usb_serial_driver * const serial_drivers[] = {
 	NULL,
 };
 
-module_usb_serial_driver(f81232_driver, serial_drivers);
+module_usb_serial_driver(serial_drivers, id_table);
 
 MODULE_DESCRIPTION("Fintek F81232 USB to serial adaptor driver");
 MODULE_AUTHOR("Greg Kroah-Hartman <gregkh@linuxfoundation.org");
diff --git a/drivers/usb/serial/ftdi_sio.c b/drivers/usb/serial/ftdi_sio.c
index 02e7f2d32d5..8c084ea34e2 100644
--- a/drivers/usb/serial/ftdi_sio.c
+++ b/drivers/usb/serial/ftdi_sio.c
@@ -809,6 +809,7 @@ static struct usb_device_id id_table_combined [] = {
 		.driver_info = (kernel_ulong_t)&ftdi_jtag_quirk },
 	{ USB_DEVICE(LARSENBRUSGAARD_VID, LB_ALTITRACK_PID) },
 	{ USB_DEVICE(GN_OTOMETRICS_VID, AURICAL_USB_PID) },
+	{ USB_DEVICE(PI_VID, PI_E861_PID) },
 	{ USB_DEVICE(BAYER_VID, BAYER_CONTOUR_CABLE_PID) },
 	{ USB_DEVICE(FTDI_VID, MARVELL_OPENRD_PID),
 		.driver_info = (kernel_ulong_t)&ftdi_jtag_quirk },
@@ -861,13 +862,6 @@ static struct usb_device_id id_table_combined [] = {
 
 MODULE_DEVICE_TABLE(usb, id_table_combined);
 
-static struct usb_driver ftdi_driver = {
-	.name =		"ftdi_sio",
-	.probe =	usb_serial_probe,
-	.disconnect =	usb_serial_disconnect,
-	.id_table =	id_table_combined,
-};
-
 static const char *ftdi_chip_name[] = {
 	[SIO] = "SIO",	/* the serial part of FT8U100AX */
 	[FT8U232AM] = "FT8U232AM",
@@ -1285,8 +1279,6 @@ static int read_latency_timer(struct usb_serial_port *port)
 	unsigned char *buf;
 	int rv;
 
-	dbg("%s", __func__);
-
 	buf = kmalloc(1, GFP_KERNEL);
 	if (!buf)
 		return -ENOMEM;
@@ -1593,8 +1585,6 @@ static int create_sysfs_attrs(struct usb_serial_port *port)
 	struct ftdi_private *priv = usb_get_serial_port_data(port);
 	int retval = 0;
 
-	dbg("%s", __func__);
-
 	/* XXX I've no idea if the original SIO supports the event_char
 	 * sysfs parameter, so I'm playing it safe.  */
 	if (priv->chip_type != SIO) {
@@ -1619,8 +1609,6 @@ static void remove_sysfs_attrs(struct usb_serial_port *port)
 {
 	struct ftdi_private *priv = usb_get_serial_port_data(port);
 
-	dbg("%s", __func__);
-
 	/* XXX see create_sysfs_attrs */
 	if (priv->chip_type != SIO) {
 		device_remove_file(&port->dev, &dev_attr_event_char);
@@ -1667,8 +1655,6 @@ static int ftdi_sio_port_probe(struct usb_serial_port *port)
 	struct ftdi_sio_quirk *quirk = usb_get_serial_data(port->serial);
 
 
-	dbg("%s", __func__);
-
 	priv = kzalloc(sizeof(struct ftdi_private), GFP_KERNEL);
 	if (!priv) {
 		dev_err(&port->dev, "%s- kmalloc(%Zd) failed.\n", __func__,
@@ -1704,8 +1690,6 @@ static int ftdi_sio_port_probe(struct usb_serial_port *port)
 /* Called from usbserial:serial_probe */
 static void ftdi_USB_UIRT_setup(struct ftdi_private *priv)
 {
-	dbg("%s", __func__);
-
 	priv->flags |= ASYNC_SPD_CUST;
 	priv->custom_divisor = 77;
 	priv->force_baud = 38400;
@@ -1716,8 +1700,6 @@ static void ftdi_USB_UIRT_setup(struct ftdi_private *priv)
 
 static void ftdi_HE_TIRA1_setup(struct ftdi_private *priv)
 {
-	dbg("%s", __func__);
-
 	priv->flags |= ASYNC_SPD_CUST;
 	priv->custom_divisor = 240;
 	priv->force_baud = 38400;
@@ -1767,8 +1749,6 @@ static int ftdi_jtag_probe(struct usb_serial *serial)
 	struct usb_device *udev = serial->dev;
 	struct usb_interface *interface = serial->interface;
 
-	dbg("%s", __func__);
-
 	if (interface == udev->actconfig->interface[0]) {
 		dev_info(&udev->dev,
 			 "Ignoring serial port reserved for JTAG\n");
@@ -1782,8 +1762,6 @@ static int ftdi_8u2232c_probe(struct usb_serial *serial)
 {
 	struct usb_device *udev = serial->dev;
 
-	dbg("%s", __func__);
-
 	if ((udev->manufacturer && !strcmp(udev->manufacturer, "CALAO Systems")) ||
 	    (udev->product && !strcmp(udev->product, "BeagleBone/XDS100")))
 		return ftdi_jtag_probe(serial);
@@ -1800,8 +1778,6 @@ static int ftdi_stmclite_probe(struct usb_serial *serial)
 	struct usb_device *udev = serial->dev;
 	struct usb_interface *interface = serial->interface;
 
-	dbg("%s", __func__);
-
 	if (interface == udev->actconfig->interface[2])
 		return 0;
 
@@ -1839,8 +1815,6 @@ static int ftdi_sio_port_remove(struct usb_serial_port *port)
 {
 	struct ftdi_private *priv = usb_get_serial_port_data(port);
 
-	dbg("%s", __func__);
-
 	priv->dev_gone = true;
 	wake_up_interruptible_all(&priv->delta_msr_wait);
 
@@ -1858,8 +1832,6 @@ static int ftdi_open(struct tty_struct *tty, struct usb_serial_port *port)
 	struct ftdi_private *priv = usb_get_serial_port_data(port);
 	int result;
 
-	dbg("%s", __func__);
-
 	/* No error checking for this (will get errors later anyway) */
 	/* See ftdi_sio.h for description of what is reset */
 	usb_control_msg(dev, usb_sndctrlpipe(dev, 0),
@@ -1918,8 +1890,6 @@ static void ftdi_close(struct usb_serial_port *port)
 {
 	struct ftdi_private *priv = usb_get_serial_port_data(port);
 
-	dbg("%s", __func__);
-
 	usb_serial_generic_close(port);
 	kref_put(&priv->kref, ftdi_sio_priv_release);
 }
@@ -1976,8 +1946,6 @@ static int ftdi_process_packet(struct tty_struct *tty,
 	char flag;
 	char *ch;
 
-	dbg("%s - port %d", __func__, port->number);
-
 	if (len < 2) {
 		dbg("malformed packet");
 		return 0;
@@ -2121,8 +2089,6 @@ static void ftdi_set_termios(struct tty_struct *tty,
 	unsigned char vstop;
 	unsigned char vstart;
 
-	dbg("%s", __func__);
-
 	/* Force baud rate if this device requires it, unless it is set to
 	   B0. */
 	if (priv->force_baud && ((termios->c_cflag & CBAUD) != B0)) {
@@ -2295,8 +2261,6 @@ static int ftdi_tiocmget(struct tty_struct *tty)
 	int len;
 	int ret;
 
-	dbg("%s TIOCMGET", __func__);
-
 	buf = kmalloc(2, GFP_KERNEL);
 	if (!buf)
 		return -ENOMEM;
@@ -2346,7 +2310,7 @@ static int ftdi_tiocmset(struct tty_struct *tty,
 			unsigned int set, unsigned int clear)
 {
 	struct usb_serial_port *port = tty->driver_data;
-	dbg("%s TIOCMSET", __func__);
+
 	return update_mctrl(port, set, clear);
 }
 
@@ -2435,7 +2399,6 @@ static int __init ftdi_init(void)
 {
 	int retval;
 
-	dbg("%s", __func__);
 	if (vendor > 0 && product > 0) {
 		/* Add user specified VID/PID to reserved element of table. */
 		int i;
@@ -2445,7 +2408,7 @@ static int __init ftdi_init(void)
 		id_table_combined[i].idVendor = vendor;
 		id_table_combined[i].idProduct = product;
 	}
-	retval = usb_serial_register_drivers(&ftdi_driver, serial_drivers);
+	retval = usb_serial_register_drivers(serial_drivers, KBUILD_MODNAME, id_table_combined);
 	if (retval == 0)
 		printk(KERN_INFO KBUILD_MODNAME ": " DRIVER_VERSION ":"
 			       DRIVER_DESC "\n");
@@ -2454,9 +2417,7 @@ static int __init ftdi_init(void)
 
 static void __exit ftdi_exit(void)
 {
-	dbg("%s", __func__);
-
-	usb_serial_deregister_drivers(&ftdi_driver, serial_drivers);
+	usb_serial_deregister_drivers(serial_drivers);
 }
 
 
diff --git a/drivers/usb/serial/ftdi_sio_ids.h b/drivers/usb/serial/ftdi_sio_ids.h
index 0838baf892f..f3c7c78ede3 100644
--- a/drivers/usb/serial/ftdi_sio_ids.h
+++ b/drivers/usb/serial/ftdi_sio_ids.h
@@ -785,6 +785,14 @@
 #define RTSYSTEMS_SERIAL_VX7_PID	0x9e52	/* Serial converter for VX-7 Radios using FT232RL */
 #define RTSYSTEMS_CT29B_PID		0x9e54	/* CT29B Radio Cable */
 
+
+/*
+ * Physik Instrumente
+ * http://www.physikinstrumente.com/en/products/
+ */
+#define PI_VID              0x1a72  /* Vendor ID */
+#define PI_E861_PID         0x1008  /* E-861 piezo controller USB connection */
+
 /*
  * Bayer Ascensia Contour blood glucose meter USB-converter cable.
  * http://winglucofacts.com/cables/
diff --git a/drivers/usb/serial/funsoft.c b/drivers/usb/serial/funsoft.c
index 4577b360792..235707961ca 100644
--- a/drivers/usb/serial/funsoft.c
+++ b/drivers/usb/serial/funsoft.c
@@ -24,13 +24,6 @@ static const struct usb_device_id id_table[] = {
 };
 MODULE_DEVICE_TABLE(usb, id_table);
 
-static struct usb_driver funsoft_driver = {
-	.name =		"funsoft",
-	.probe =	usb_serial_probe,
-	.disconnect =	usb_serial_disconnect,
-	.id_table =	id_table,
-};
-
 static struct usb_serial_driver funsoft_device = {
 	.driver = {
 		.owner =	THIS_MODULE,
@@ -44,7 +37,7 @@ static struct usb_serial_driver * const serial_drivers[] = {
 	&funsoft_device, NULL
 };
 
-module_usb_serial_driver(funsoft_driver, serial_drivers);
+module_usb_serial_driver(serial_drivers, id_table);
 
 MODULE_LICENSE("GPL");
 
diff --git a/drivers/usb/serial/garmin_gps.c b/drivers/usb/serial/garmin_gps.c
index e8eb6347bf3..346c15a5106 100644
--- a/drivers/usb/serial/garmin_gps.c
+++ b/drivers/usb/serial/garmin_gps.c
@@ -216,16 +216,8 @@ static const struct usb_device_id id_table[] = {
 	{ USB_DEVICE(GARMIN_VENDOR_ID, 3) },
 	{ }					/* Terminating entry */
 };
-
 MODULE_DEVICE_TABLE(usb, id_table);
 
-static struct usb_driver garmin_driver = {
-	.name =		"garmin_gps",
-	.probe =	usb_serial_probe,
-	.disconnect =	usb_serial_disconnect,
-	.id_table =	id_table,
-};
-
 
 static inline int getLayerId(const __u8 *usbPacket)
 {
@@ -345,8 +337,6 @@ static void pkt_clear(struct garmin_data *garmin_data_p)
 	unsigned long flags;
 	struct garmin_packet *result = NULL;
 
-	dbg("%s", __func__);
-
 	spin_lock_irqsave(&garmin_data_p->lock, flags);
 	while (!list_empty(&garmin_data_p->pktlist)) {
 		result = (struct garmin_packet *)garmin_data_p->pktlist.next;
@@ -939,8 +929,6 @@ static int garmin_open(struct tty_struct *tty, struct usb_serial_port *port)
 	int status = 0;
 	struct garmin_data *garmin_data_p = usb_get_serial_port_data(port);
 
-	dbg("%s - port %d", __func__, port->number);
-
 	spin_lock_irqsave(&garmin_data_p->lock, flags);
 	garmin_data_p->mode  = initial_mode;
 	garmin_data_p->count = 0;
@@ -996,8 +984,6 @@ static void garmin_write_bulk_callback(struct urb *urb)
 		struct garmin_data *garmin_data_p =
 					usb_get_serial_port_data(port);
 
-		dbg("%s - port %d", __func__, port->number);
-
 		if (GARMIN_LAYERID_APPL == getLayerId(urb->transfer_buffer)) {
 
 			if (garmin_data_p->mode == MODE_GARMIN_SERIAL) {
@@ -1027,9 +1013,6 @@ static int garmin_write_bulk(struct usb_serial_port *port,
 	unsigned char *buffer;
 	int status;
 
-	dbg("%s - port %d, state %d", __func__, port->number,
-		garmin_data_p->state);
-
 	spin_lock_irqsave(&garmin_data_p->lock, flags);
 	garmin_data_p->flags &= ~FLAGS_DROP_DATA;
 	spin_unlock_irqrestore(&garmin_data_p->lock, flags);
@@ -1224,8 +1207,6 @@ static void garmin_read_bulk_callback(struct urb *urb)
 	int status = urb->status;
 	int retval;
 
-	dbg("%s - port %d", __func__, port->number);
-
 	if (!serial) {
 		dbg("%s - bad serial pointer, exiting", __func__);
 		return;
@@ -1384,7 +1365,6 @@ static void garmin_throttle(struct tty_struct *tty)
 	struct usb_serial_port *port = tty->driver_data;
 	struct garmin_data *garmin_data_p = usb_get_serial_port_data(port);
 
-	dbg("%s - port %d", __func__, port->number);
 	/* set flag, data received will be put into a queue
 	   for later processing */
 	spin_lock_irq(&garmin_data_p->lock);
@@ -1399,7 +1379,6 @@ static void garmin_unthrottle(struct tty_struct *tty)
 	struct garmin_data *garmin_data_p = usb_get_serial_port_data(port);
 	int status;
 
-	dbg("%s - port %d", __func__, port->number);
 	spin_lock_irq(&garmin_data_p->lock);
 	garmin_data_p->flags &= ~FLAGS_THROTTLED;
 	spin_unlock_irq(&garmin_data_p->lock);
@@ -1441,8 +1420,6 @@ static int garmin_attach(struct usb_serial *serial)
 	struct usb_serial_port *port = serial->port[0];
 	struct garmin_data *garmin_data_p = NULL;
 
-	dbg("%s", __func__);
-
 	garmin_data_p = kzalloc(sizeof(struct garmin_data), GFP_KERNEL);
 	if (garmin_data_p == NULL) {
 		dev_err(&port->dev, "%s - Out of memory\n", __func__);
@@ -1471,8 +1448,6 @@ static void garmin_disconnect(struct usb_serial *serial)
 	struct usb_serial_port *port = serial->port[0];
 	struct garmin_data *garmin_data_p = usb_get_serial_port_data(port);
 
-	dbg("%s", __func__);
-
 	usb_kill_urb(port->interrupt_in_urb);
 	del_timer_sync(&garmin_data_p->timer);
 }
@@ -1483,8 +1458,6 @@ static void garmin_release(struct usb_serial *serial)
 	struct usb_serial_port *port = serial->port[0];
 	struct garmin_data *garmin_data_p = usb_get_serial_port_data(port);
 
-	dbg("%s", __func__);
-
 	kfree(garmin_data_p);
 }
 
@@ -1516,7 +1489,7 @@ static struct usb_serial_driver * const serial_drivers[] = {
 	&garmin_device, NULL
 };
 
-module_usb_serial_driver(garmin_driver, serial_drivers);
+module_usb_serial_driver(serial_drivers, id_table);
 
 MODULE_AUTHOR(DRIVER_AUTHOR);
 MODULE_DESCRIPTION(DRIVER_DESC);
diff --git a/drivers/usb/serial/generic.c b/drivers/usb/serial/generic.c
index 664deb63807..105a6d898ca 100644
--- a/drivers/usb/serial/generic.c
+++ b/drivers/usb/serial/generic.c
@@ -28,9 +28,6 @@ static int debug;
 
 #ifdef CONFIG_USB_SERIAL_GENERIC
 
-static int generic_probe(struct usb_interface *interface,
-			 const struct usb_device_id *id);
-
 static __u16 vendor  = 0x05f9;
 static __u16 product = 0xffff;
 
@@ -49,13 +46,6 @@ static const struct usb_device_id generic_serial_ids[] = {
 	{}
 };
 
-static struct usb_driver generic_driver = {
-	.name =		"usbserial_generic",
-	.probe =	generic_probe,
-	.disconnect =	usb_serial_disconnect,
-	.id_table =	generic_serial_ids,
-};
-
 /* All of the device info needed for the Generic Serial Converter */
 struct usb_serial_driver usb_serial_generic_device = {
 	.driver = {
@@ -75,16 +65,6 @@ static struct usb_serial_driver * const serial_drivers[] = {
 	&usb_serial_generic_device, NULL
 };
 
-static int generic_probe(struct usb_interface *interface,
-			       const struct usb_device_id *id)
-{
-	const struct usb_device_id *id_pattern;
-
-	id_pattern = usb_match_id(interface, generic_device_ids);
-	if (id_pattern != NULL)
-		return usb_serial_probe(interface, id);
-	return -ENODEV;
-}
 #endif
 
 int usb_serial_generic_register(int _debug)
@@ -99,7 +79,7 @@ int usb_serial_generic_register(int _debug)
 		USB_DEVICE_ID_MATCH_VENDOR | USB_DEVICE_ID_MATCH_PRODUCT;
 
 	/* register our generic driver with ourselves */
-	retval = usb_serial_register_drivers(&generic_driver, serial_drivers);
+	retval = usb_serial_register_drivers(serial_drivers, "usbserial_generic", generic_serial_ids);
 #endif
 	return retval;
 }
@@ -108,7 +88,7 @@ void usb_serial_generic_deregister(void)
 {
 #ifdef CONFIG_USB_SERIAL_GENERIC
 	/* remove our generic driver */
-	usb_serial_deregister_drivers(&generic_driver, serial_drivers);
+	usb_serial_deregister_drivers(serial_drivers);
 #endif
 }
 
@@ -117,8 +97,6 @@ int usb_serial_generic_open(struct tty_struct *tty, struct usb_serial_port *port
 	int result = 0;
 	unsigned long flags;
 
-	dbg("%s - port %d", __func__, port->number);
-
 	/* clear the throttle flags */
 	spin_lock_irqsave(&port->lock, flags);
 	port->throttled = 0;
@@ -139,12 +117,9 @@ static void generic_cleanup(struct usb_serial_port *port)
 	unsigned long flags;
 	int i;
 
-	dbg("%s - port %d", __func__, port->number);
-
 	if (serial->dev) {
 		/* shutdown any bulk transfers that might be going on */
 		if (port->bulk_out_size) {
-			usb_kill_urb(port->write_urb);
 			for (i = 0; i < ARRAY_SIZE(port->write_urbs); ++i)
 				usb_kill_urb(port->write_urbs[i]);
 
@@ -161,7 +136,6 @@ static void generic_cleanup(struct usb_serial_port *port)
 
 void usb_serial_generic_close(struct usb_serial_port *port)
 {
-	dbg("%s - port %d", __func__, port->number);
 	generic_cleanup(port);
 }
 EXPORT_SYMBOL_GPL(usb_serial_generic_close);
@@ -249,8 +223,6 @@ int usb_serial_generic_write(struct tty_struct *tty,
 {
 	int result;
 
-	dbg("%s - port %d", __func__, port->number);
-
 	/* only do something if we have a bulk out endpoint */
 	if (!port->bulk_out_size)
 		return -ENODEV;
@@ -273,8 +245,6 @@ int usb_serial_generic_write_room(struct tty_struct *tty)
 	unsigned long flags;
 	int room;
 
-	dbg("%s - port %d", __func__, port->number);
-
 	if (!port->bulk_out_size)
 		return 0;
 
@@ -282,7 +252,7 @@ int usb_serial_generic_write_room(struct tty_struct *tty)
 	room = kfifo_avail(&port->write_fifo);
 	spin_unlock_irqrestore(&port->lock, flags);
 
-	dbg("%s - returns %d", __func__, room);
+	dev_dbg(&port->dev, "%s - returns %d\n", __func__, room);
 	return room;
 }
 
@@ -292,8 +262,6 @@ int usb_serial_generic_chars_in_buffer(struct tty_struct *tty)
 	unsigned long flags;
 	int chars;
 
-	dbg("%s - port %d", __func__, port->number);
-
 	if (!port->bulk_out_size)
 		return 0;
 
@@ -301,7 +269,7 @@ int usb_serial_generic_chars_in_buffer(struct tty_struct *tty)
 	chars = kfifo_len(&port->write_fifo) + port->tx_bytes;
 	spin_unlock_irqrestore(&port->lock, flags);
 
-	dbg("%s - returns %d", __func__, chars);
+	dev_dbg(&port->dev, "%s - returns %d\n", __func__, chars);
 	return chars;
 }
 
@@ -313,7 +281,8 @@ static int usb_serial_generic_submit_read_urb(struct usb_serial_port *port,
 	if (!test_and_clear_bit(index, &port->read_urbs_free))
 		return 0;
 
-	dbg("%s - port %d, urb %d\n", __func__, port->number, index);
+	dev_dbg(&port->dev, "%s - port %d, urb %d\n", __func__,
+		port->number, index);
 
 	res = usb_submit_urb(port->read_urbs[index], mem_flags);
 	if (res) {
@@ -335,8 +304,6 @@ int usb_serial_generic_submit_read_urbs(struct usb_serial_port *port,
 	int res;
 	int i;
 
-	dbg("%s - port %d", __func__, port->number);
-
 	for (i = 0; i < ARRAY_SIZE(port->read_urbs); ++i) {
 		res = usb_serial_generic_submit_read_urb(port, i, mem_flags);
 		if (res)
@@ -395,10 +362,12 @@ void usb_serial_generic_read_bulk_callback(struct urb *urb)
 	}
 	set_bit(i, &port->read_urbs_free);
 
-	dbg("%s - port %d, urb %d, len %d\n", __func__, port->number, i,
-							urb->actual_length);
+	dev_dbg(&port->dev, "%s - port %d, urb %d, len %d\n",
+		__func__, port->number, i, urb->actual_length);
+
 	if (urb->status) {
-		dbg("%s - non-zero urb status: %d\n", __func__, urb->status);
+		dev_dbg(&port->dev, "%s - non-zero urb status: %d\n",
+			__func__, urb->status);
 		return;
 	}
 
@@ -424,8 +393,6 @@ void usb_serial_generic_write_bulk_callback(struct urb *urb)
 	int status = urb->status;
 	int i;
 
-	dbg("%s - port %d", __func__, port->number);
-
 	for (i = 0; i < ARRAY_SIZE(port->write_urbs); ++i)
 		if (port->write_urbs[i] == urb)
 			break;
@@ -436,7 +403,8 @@ void usb_serial_generic_write_bulk_callback(struct urb *urb)
 	spin_unlock_irqrestore(&port->lock, flags);
 
 	if (status) {
-		dbg("%s - non-zero urb status: %d", __func__, status);
+		dev_dbg(&port->dev, "%s - non-zero urb status: %d\n",
+			__func__, status);
 
 		spin_lock_irqsave(&port->lock, flags);
 		kfifo_reset_out(&port->write_fifo);
@@ -454,8 +422,6 @@ void usb_serial_generic_throttle(struct tty_struct *tty)
 	struct usb_serial_port *port = tty->driver_data;
 	unsigned long flags;
 
-	dbg("%s - port %d", __func__, port->number);
-
 	/* Set the throttle request flag. It will be picked up
 	 * by usb_serial_generic_read_bulk_callback(). */
 	spin_lock_irqsave(&port->lock, flags);
@@ -469,8 +435,6 @@ void usb_serial_generic_unthrottle(struct tty_struct *tty)
 	struct usb_serial_port *port = tty->driver_data;
 	int was_throttled;
 
-	dbg("%s - port %d", __func__, port->number);
-
 	/* Clear the throttle flags */
 	spin_lock_irq(&port->lock);
 	was_throttled = port->throttled;
@@ -525,7 +489,8 @@ void usb_serial_handle_dcd_change(struct usb_serial_port *usb_port,
 {
 	struct tty_port *port = &usb_port->port;
 
-	dbg("%s - port %d, status %d", __func__, usb_port->number, status);
+	dev_dbg(&usb_port->dev, "%s - port %d, status %d\n", __func__,
+		usb_port->number, status);
 
 	if (status)
 		wake_up_interruptible(&port->open_wait);
@@ -566,8 +531,6 @@ void usb_serial_generic_disconnect(struct usb_serial *serial)
 {
 	int i;
 
-	dbg("%s", __func__);
-
 	/* stop reads and writes on all ports */
 	for (i = 0; i < serial->num_ports; ++i)
 		generic_cleanup(serial->port[i]);
@@ -576,5 +539,4 @@ EXPORT_SYMBOL_GPL(usb_serial_generic_disconnect);
 
 void usb_serial_generic_release(struct usb_serial *serial)
 {
-	dbg("%s", __func__);
 }
diff --git a/drivers/usb/serial/hp4x.c b/drivers/usb/serial/hp4x.c
index 2563e788c9b..0bbaf21a9d1 100644
--- a/drivers/usb/serial/hp4x.c
+++ b/drivers/usb/serial/hp4x.c
@@ -36,13 +36,6 @@ static const struct usb_device_id id_table[] = {
 
 MODULE_DEVICE_TABLE(usb, id_table);
 
-static struct usb_driver hp49gp_driver = {
-	.name =		"hp4X",
-	.probe =	usb_serial_probe,
-	.disconnect =	usb_serial_disconnect,
-	.id_table =	id_table,
-};
-
 static struct usb_serial_driver hp49gp_device = {
 	.driver = {
 		.owner =	THIS_MODULE,
@@ -56,7 +49,7 @@ static struct usb_serial_driver * const serial_drivers[] = {
 	&hp49gp_device, NULL
 };
 
-module_usb_serial_driver(hp49gp_driver, serial_drivers);
+module_usb_serial_driver(serial_drivers, id_table);
 
 MODULE_DESCRIPTION(DRIVER_DESC);
 MODULE_VERSION(DRIVER_VERSION);
diff --git a/drivers/usb/serial/io_edgeport.c b/drivers/usb/serial/io_edgeport.c
index 323e8723571..e1f5ccd1e8f 100644
--- a/drivers/usb/serial/io_edgeport.c
+++ b/drivers/usb/serial/io_edgeport.c
@@ -3181,7 +3181,7 @@ static void edge_release(struct usb_serial *serial)
 	kfree(edge_serial);
 }
 
-module_usb_serial_driver(io_driver, serial_drivers);
+module_usb_serial_driver(serial_drivers, id_table_combined);
 
 MODULE_AUTHOR(DRIVER_AUTHOR);
 MODULE_DESCRIPTION(DRIVER_DESC);
diff --git a/drivers/usb/serial/io_tables.h b/drivers/usb/serial/io_tables.h
index d0e7c9affb6..350afddb55b 100644
--- a/drivers/usb/serial/io_tables.h
+++ b/drivers/usb/serial/io_tables.h
@@ -95,13 +95,6 @@ static const struct usb_device_id id_table_combined[] = {
 
 MODULE_DEVICE_TABLE(usb, id_table_combined);
 
-static struct usb_driver io_driver = {
-	.name =		"io_edgeport",
-	.probe =	usb_serial_probe,
-	.disconnect =	usb_serial_disconnect,
-	.id_table =	id_table_combined,
-};
-
 static struct usb_serial_driver edgeport_2port_device = {
 	.driver = {
 		.owner		= THIS_MODULE,
diff --git a/drivers/usb/serial/io_ti.c b/drivers/usb/serial/io_ti.c
index 40a95a7fe38..3936904c641 100644
--- a/drivers/usb/serial/io_ti.c
+++ b/drivers/usb/serial/io_ti.c
@@ -197,14 +197,6 @@ static const struct usb_device_id id_table_combined[] = {
 
 MODULE_DEVICE_TABLE(usb, id_table_combined);
 
-static struct usb_driver io_driver = {
-	.name =		"io_ti",
-	.probe =	usb_serial_probe,
-	.disconnect =	usb_serial_disconnect,
-	.id_table =	id_table_combined,
-};
-
-
 static unsigned char OperationalMajorVersion;
 static unsigned char OperationalMinorVersion;
 static unsigned short OperationalBuildNumber;
@@ -547,6 +539,7 @@ static void chase_port(struct edgeport_port *port, unsigned long timeout,
 {
 	int baud_rate;
 	struct tty_struct *tty = tty_port_tty_get(&port->port->port);
+	struct usb_serial *serial = port->port->serial;
 	wait_queue_t wait;
 	unsigned long flags;
 
@@ -561,7 +554,7 @@ static void chase_port(struct edgeport_port *port, unsigned long timeout,
 		set_current_state(TASK_INTERRUPTIBLE);
 		if (kfifo_len(&port->write_fifo) == 0
 		|| timeout == 0 || signal_pending(current)
-		|| !usb_get_intfdata(port->port->serial->interface))
+		|| serial->disconnected)
 			/* disconnect */
 			break;
 		spin_unlock_irqrestore(&port->ep_lock, flags);
@@ -578,7 +571,7 @@ static void chase_port(struct edgeport_port *port, unsigned long timeout,
 	/* wait for data to drain from the device */
 	timeout += jiffies;
 	while ((long)(jiffies - timeout) < 0 && !signal_pending(current)
-	&& usb_get_intfdata(port->port->serial->interface)) {
+						&& !serial->disconnected) {
 		/* not disconnected */
 		if (!tx_active(port))
 			break;
@@ -586,7 +579,7 @@ static void chase_port(struct edgeport_port *port, unsigned long timeout,
 	}
 
 	/* disconnected */
-	if (!usb_get_intfdata(port->port->serial->interface))
+	if (serial->disconnected)
 		return;
 
 	/* wait one more character time, based on baud rate */
@@ -2003,8 +1996,8 @@ static void edge_close(struct usb_serial_port *port)
 {
 	struct edgeport_serial *edge_serial;
 	struct edgeport_port *edge_port;
+	struct usb_serial *serial = port->serial;
 	int port_number;
-	int status;
 
 	dbg("%s - port %d", __func__, port->number);
 
@@ -2028,12 +2021,18 @@ static void edge_close(struct usb_serial_port *port)
 	 * send a close port command to it */
 	dbg("%s - send umpc_close_port", __func__);
 	port_number = port->number - port->serial->minor;
-	status = send_cmd(port->serial->dev,
+
+	mutex_lock(&serial->disc_mutex);
+	if (!serial->disconnected) {
+		send_cmd(serial->dev,
 				     UMPC_CLOSE_PORT,
 				     (__u8)(UMPM_UART1_PORT + port_number),
 				     0,
 				     NULL,
 				     0);
+	}
+	mutex_unlock(&serial->disc_mutex);
+
 	mutex_lock(&edge_serial->es_lock);
 	--edge_port->edge_serial->num_ports_open;
 	if (edge_port->edge_serial->num_ports_open <= 0) {
@@ -2783,7 +2782,7 @@ static struct usb_serial_driver * const serial_drivers[] = {
 	&edgeport_1port_device, &edgeport_2port_device, NULL
 };
 
-module_usb_serial_driver(io_driver, serial_drivers);
+module_usb_serial_driver(serial_drivers, id_table_combined);
 
 MODULE_AUTHOR(DRIVER_AUTHOR);
 MODULE_DESCRIPTION(DRIVER_DESC);
diff --git a/drivers/usb/serial/ipaq.c b/drivers/usb/serial/ipaq.c
index 10c02b8b566..c85a7eb87d4 100644
--- a/drivers/usb/serial/ipaq.c
+++ b/drivers/usb/serial/ipaq.c
@@ -33,7 +33,6 @@
 #define DRIVER_AUTHOR "Ganesh Varadarajan <ganesh@veritas.com>"
 #define DRIVER_DESC "USB PocketPC PDA driver"
 
-static __u16 product, vendor;
 static bool debug;
 static int connect_retries = KP_RETRIES;
 static int initial_wait;
@@ -45,8 +44,6 @@ static int  ipaq_calc_num_ports(struct usb_serial *serial);
 static int  ipaq_startup(struct usb_serial *serial);
 
 static struct usb_device_id ipaq_id_table [] = {
-	/* The first entry is a placeholder for the insmod-specified device */
-	{ USB_DEVICE(0x049F, 0x0003) },
 	{ USB_DEVICE(0x0104, 0x00BE) }, /* Socket USB Sync */
 	{ USB_DEVICE(0x03F0, 0x1016) }, /* HP USB Sync */
 	{ USB_DEVICE(0x03F0, 0x1116) }, /* HP USB Sync 1611 */
@@ -505,13 +502,6 @@ static struct usb_device_id ipaq_id_table [] = {
 
 MODULE_DEVICE_TABLE(usb, ipaq_id_table);
 
-static struct usb_driver ipaq_driver = {
-	.name =		"ipaq",
-	.probe =	usb_serial_probe,
-	.disconnect =	usb_serial_disconnect,
-	.id_table =	ipaq_id_table,
-};
-
 
 /* All of the device info needed for the Compaq iPAQ */
 static struct usb_serial_driver ipaq_device = {
@@ -539,8 +529,6 @@ static int ipaq_open(struct tty_struct *tty,
 	int			result = 0;
 	int			retries = connect_retries;
 
-	dbg("%s - port %d", __func__, port->number);
-
 	msleep(1000*initial_wait);
 
 	/*
@@ -577,7 +565,7 @@ static int ipaq_calc_num_ports(struct usb_serial *serial)
 	 */
 	int ipaq_num_ports = 1;
 
-	dbg("%s - numberofendpoints: %d", __FUNCTION__,
+	dev_dbg(&serial->dev->dev, "%s - numberofendpoints: %d\n", __func__,
 		(int)serial->interface->cur_altsetting->desc.bNumEndpoints);
 
 	/*
@@ -596,8 +584,6 @@ static int ipaq_calc_num_ports(struct usb_serial *serial)
 
 static int ipaq_startup(struct usb_serial *serial)
 {
-	dbg("%s", __func__);
-
 	/* Some of the devices in ipaq_id_table[] are composite, and we
 	 * shouldn't bind to all the interfaces.  This test will rule out
 	 * some obviously invalid possibilities.
@@ -617,36 +603,14 @@ static int ipaq_startup(struct usb_serial *serial)
 		return -ENODEV;
 	}
 
-	dbg("%s - iPAQ module configured for %d ports",
-		__FUNCTION__, serial->num_ports);
+	dev_dbg(&serial->dev->dev,
+		"%s - iPAQ module configured for %d ports\n", __func__,
+		serial->num_ports);
 
 	return usb_reset_configuration(serial->dev);
 }
 
-static int __init ipaq_init(void)
-{
-	int retval;
-
-	if (vendor) {
-		ipaq_id_table[0].idVendor = vendor;
-		ipaq_id_table[0].idProduct = product;
-	}
-
-	retval = usb_serial_register_drivers(&ipaq_driver, serial_drivers);
-	if (retval == 0)
-		printk(KERN_INFO KBUILD_MODNAME ": " DRIVER_VERSION ":"
-			       DRIVER_DESC "\n");
-	return retval;
-}
-
-static void __exit ipaq_exit(void)
-{
-	usb_serial_deregister_drivers(&ipaq_driver, serial_drivers);
-}
-
-
-module_init(ipaq_init);
-module_exit(ipaq_exit);
+module_usb_serial_driver(serial_drivers, ipaq_id_table);
 
 MODULE_AUTHOR(DRIVER_AUTHOR);
 MODULE_DESCRIPTION(DRIVER_DESC);
@@ -655,12 +619,6 @@ MODULE_LICENSE("GPL");
 module_param(debug, bool, S_IRUGO | S_IWUSR);
 MODULE_PARM_DESC(debug, "Debug enabled or not");
 
-module_param(vendor, ushort, 0);
-MODULE_PARM_DESC(vendor, "User specified USB idVendor");
-
-module_param(product, ushort, 0);
-MODULE_PARM_DESC(product, "User specified USB idProduct");
-
 module_param(connect_retries, int, S_IRUGO|S_IWUSR);
 MODULE_PARM_DESC(connect_retries,
 		"Maximum number of connect retries (one second each)");
diff --git a/drivers/usb/serial/ipw.c b/drivers/usb/serial/ipw.c
index 76a06406e26..5811d34b6c6 100644
--- a/drivers/usb/serial/ipw.c
+++ b/drivers/usb/serial/ipw.c
@@ -132,19 +132,11 @@ enum {
 
 #define IPW_WANTS_TO_SEND	0x30
 
-static const struct usb_device_id usb_ipw_ids[] = {
+static const struct usb_device_id id_table[] = {
 	{ USB_DEVICE(IPW_VID, IPW_PID) },
 	{ },
 };
-
-MODULE_DEVICE_TABLE(usb, usb_ipw_ids);
-
-static struct usb_driver usb_ipw_driver = {
-	.name =		"ipwtty",
-	.probe =	usb_serial_probe,
-	.disconnect =	usb_serial_disconnect,
-	.id_table =	usb_ipw_ids,
-};
+MODULE_DEVICE_TABLE(usb, id_table);
 
 static bool debug;
 
@@ -155,8 +147,6 @@ static int ipw_open(struct tty_struct *tty, struct usb_serial_port *port)
 	u8 *buf_flow_init;
 	int result;
 
-	dbg("%s", __func__);
-
 	buf_flow_init = kmemdup(buf_flow_static, 16, GFP_KERNEL);
 	if (!buf_flow_init)
 		return -ENOMEM;
@@ -317,7 +307,7 @@ static struct usb_serial_driver ipw_device = {
 		.name =		"ipw",
 	},
 	.description =		"IPWireless converter",
-	.id_table =		usb_ipw_ids,
+	.id_table =		id_table,
 	.num_ports =		1,
 	.disconnect =		usb_wwan_disconnect,
 	.open =			ipw_open,
@@ -333,7 +323,7 @@ static struct usb_serial_driver * const serial_drivers[] = {
 	&ipw_device, NULL
 };
 
-module_usb_serial_driver(usb_ipw_driver, serial_drivers);
+module_usb_serial_driver(serial_drivers, id_table);
 
 /* Module information */
 MODULE_AUTHOR(DRIVER_AUTHOR);
diff --git a/drivers/usb/serial/ir-usb.c b/drivers/usb/serial/ir-usb.c
index 84965cd65c7..fc09414c960 100644
--- a/drivers/usb/serial/ir-usb.c
+++ b/drivers/usb/serial/ir-usb.c
@@ -77,13 +77,6 @@ static const struct usb_device_id ir_id_table[] = {
 
 MODULE_DEVICE_TABLE(usb, ir_id_table);
 
-static struct usb_driver ir_driver = {
-	.name		= "ir-usb",
-	.probe		= usb_serial_probe,
-	.disconnect	= usb_serial_disconnect,
-	.id_table	= ir_id_table,
-};
-
 static struct usb_serial_driver ir_device = {
 	.driver	= {
 		.owner	= THIS_MODULE,
@@ -103,18 +96,21 @@ static struct usb_serial_driver * const serial_drivers[] = {
 	&ir_device, NULL
 };
 
-static inline void irda_usb_dump_class_desc(struct usb_irda_cs_descriptor *desc)
+static inline void irda_usb_dump_class_desc(struct usb_serial *serial,
+					    struct usb_irda_cs_descriptor *desc)
 {
-	dbg("bLength=%x", desc->bLength);
-	dbg("bDescriptorType=%x", desc->bDescriptorType);
-	dbg("bcdSpecRevision=%x", __le16_to_cpu(desc->bcdSpecRevision));
-	dbg("bmDataSize=%x", desc->bmDataSize);
-	dbg("bmWindowSize=%x", desc->bmWindowSize);
-	dbg("bmMinTurnaroundTime=%d", desc->bmMinTurnaroundTime);
-	dbg("wBaudRate=%x", __le16_to_cpu(desc->wBaudRate));
-	dbg("bmAdditionalBOFs=%x", desc->bmAdditionalBOFs);
-	dbg("bIrdaRateSniff=%x", desc->bIrdaRateSniff);
-	dbg("bMaxUnicastList=%x", desc->bMaxUnicastList);
+	struct device *dev = &serial->dev->dev;
+
+	dev_dbg(dev, "bLength=%x\n", desc->bLength);
+	dev_dbg(dev, "bDescriptorType=%x\n", desc->bDescriptorType);
+	dev_dbg(dev, "bcdSpecRevision=%x\n", __le16_to_cpu(desc->bcdSpecRevision));
+	dev_dbg(dev, "bmDataSize=%x\n", desc->bmDataSize);
+	dev_dbg(dev, "bmWindowSize=%x\n", desc->bmWindowSize);
+	dev_dbg(dev, "bmMinTurnaroundTime=%d\n", desc->bmMinTurnaroundTime);
+	dev_dbg(dev, "wBaudRate=%x\n", __le16_to_cpu(desc->wBaudRate));
+	dev_dbg(dev, "bmAdditionalBOFs=%x\n", desc->bmAdditionalBOFs);
+	dev_dbg(dev, "bIrdaRateSniff=%x\n", desc->bIrdaRateSniff);
+	dev_dbg(dev, "bMaxUnicastList=%x\n", desc->bMaxUnicastList);
 }
 
 /*------------------------------------------------------------------*/
@@ -130,8 +126,9 @@ static inline void irda_usb_dump_class_desc(struct usb_irda_cs_descriptor *desc)
  * Based on the same function in drivers/net/irda/irda-usb.c
  */
 static struct usb_irda_cs_descriptor *
-irda_usb_find_class_desc(struct usb_device *dev, unsigned int ifnum)
+irda_usb_find_class_desc(struct usb_serial *serial, unsigned int ifnum)
 {
+	struct usb_device *dev = serial->dev;
 	struct usb_irda_cs_descriptor *desc;
 	int ret;
 
@@ -144,20 +141,20 @@ irda_usb_find_class_desc(struct usb_device *dev, unsigned int ifnum)
 			USB_DIR_IN | USB_TYPE_CLASS | USB_RECIP_INTERFACE,
 			0, ifnum, desc, sizeof(*desc), 1000);
 
-	dbg("%s -  ret=%d", __func__, ret);
+	dev_dbg(&serial->dev->dev, "%s -  ret=%d\n", __func__, ret);
 	if (ret < sizeof(*desc)) {
-		dbg("%s - class descriptor read %s (%d)",
-				__func__,
-				(ret < 0) ? "failed" : "too short",
-				ret);
+		dev_dbg(&serial->dev->dev,
+			"%s - class descriptor read %s (%d)\n", __func__,
+			(ret < 0) ? "failed" : "too short", ret);
 		goto error;
 	}
 	if (desc->bDescriptorType != USB_DT_CS_IRDA) {
-		dbg("%s - bad class descriptor type", __func__);
+		dev_dbg(&serial->dev->dev, "%s - bad class descriptor type\n",
+			__func__);
 		goto error;
 	}
 
-	irda_usb_dump_class_desc(desc);
+	irda_usb_dump_class_desc(serial, desc);
 	return desc;
 
 error:
@@ -207,14 +204,15 @@ static int ir_startup(struct usb_serial *serial)
 {
 	struct usb_irda_cs_descriptor *irda_desc;
 
-	irda_desc = irda_usb_find_class_desc(serial->dev, 0);
+	irda_desc = irda_usb_find_class_desc(serial, 0);
 	if (!irda_desc) {
 		dev_err(&serial->dev->dev,
 			"IRDA class descriptor not found, device not bound\n");
 		return -ENODEV;
 	}
 
-	dbg("%s - Baud rates supported:%s%s%s%s%s%s%s%s%s",
+	dev_dbg(&serial->dev->dev,
+		"%s - Baud rates supported:%s%s%s%s%s%s%s%s%s\n",
 		__func__,
 		(irda_desc->wBaudRate & USB_IRDA_BR_2400) ? " 2400" : "",
 		(irda_desc->wBaudRate & USB_IRDA_BR_9600) ? " 9600" : "",
@@ -264,8 +262,6 @@ static int ir_open(struct tty_struct *tty, struct usb_serial_port *port)
 {
 	int i;
 
-	dbg("%s - port %d", __func__, port->number);
-
 	for (i = 0; i < ARRAY_SIZE(port->write_urbs); ++i)
 		port->write_urbs[i]->transfer_flags = URB_ZERO_PACKET;
 
@@ -322,15 +318,11 @@ static void ir_process_read_urb(struct urb *urb)
 
 static void ir_set_termios_callback(struct urb *urb)
 {
-	struct usb_serial_port *port = urb->context;
-	int status = urb->status;
-
-	dbg("%s - port %d", __func__, port->number);
-
 	kfree(urb->transfer_buffer);
 
-	if (status)
-		dbg("%s - non-zero urb status: %d", __func__, status);
+	if (urb->status)
+		dev_dbg(&urb->dev->dev, "%s - non-zero urb status: %d\n",
+			__func__, urb->status);
 }
 
 static void ir_set_termios(struct tty_struct *tty,
@@ -342,8 +334,6 @@ static void ir_set_termios(struct tty_struct *tty,
 	speed_t baud;
 	int ir_baud;
 
-	dbg("%s - port %d", __func__, port->number);
-
 	baud = tty_get_baud_rate(tty);
 
 	/*
@@ -447,7 +437,7 @@ static int __init ir_init(void)
 		ir_device.bulk_out_size = buffer_size;
 	}
 
-	retval = usb_serial_register_drivers(&ir_driver, serial_drivers);
+	retval = usb_serial_register_drivers(serial_drivers, KBUILD_MODNAME, ir_id_table);
 	if (retval == 0)
 		printk(KERN_INFO KBUILD_MODNAME ": " DRIVER_VERSION ":"
 			       DRIVER_DESC "\n");
@@ -456,7 +446,7 @@ static int __init ir_init(void)
 
 static void __exit ir_exit(void)
 {
-	usb_serial_deregister_drivers(&ir_driver, serial_drivers);
+	usb_serial_deregister_drivers(serial_drivers);
 }
 
 
diff --git a/drivers/usb/serial/iuu_phoenix.c b/drivers/usb/serial/iuu_phoenix.c
index f2192d527db..22b1eb5040b 100644
--- a/drivers/usb/serial/iuu_phoenix.c
+++ b/drivers/usb/serial/iuu_phoenix.c
@@ -51,13 +51,6 @@ static const struct usb_device_id id_table[] = {
 };
 MODULE_DEVICE_TABLE(usb, id_table);
 
-static struct usb_driver iuu_driver = {
-	.name = "iuu_phoenix",
-	.probe = usb_serial_probe,
-	.disconnect = usb_serial_disconnect,
-	.id_table = id_table,
-};
-
 /* turbo parameter */
 static int boost = 100;
 static int clockmode = 1;
@@ -135,8 +128,6 @@ static void iuu_release(struct usb_serial *serial)
 	if (!port)
 		return;
 
-	dbg("%s", __func__);
-
 	if (priv) {
 		iuu_free_buf(priv);
 		dbg("%s - I will free all", __func__);
@@ -198,8 +189,6 @@ static void iuu_rxcmd(struct urb *urb)
 	int result;
 	int status = urb->status;
 
-	dbg("%s - enter", __func__);
-
 	if (status) {
 		dbg("%s - status = %d", __func__, status);
 		/* error stop all */
@@ -221,7 +210,6 @@ static int iuu_reset(struct usb_serial_port *port, u8 wt)
 	struct iuu_private *priv = usb_get_serial_port_data(port);
 	int result;
 	char *buf_ptr = port->write_urb->transfer_buffer;
-	dbg("%s - enter", __func__);
 
 	/* Prepare the reset sequence */
 
@@ -255,8 +243,6 @@ static void iuu_update_status_callback(struct urb *urb)
 	u8 *st;
 	int status = urb->status;
 
-	dbg("%s - enter", __func__);
-
 	if (status) {
 		dbg("%s - status = %d", __func__, status);
 		/* error stop all */
@@ -299,8 +285,6 @@ static int iuu_status(struct usb_serial_port *port)
 {
 	int result;
 
-	dbg("%s - enter", __func__);
-
 	memset(port->write_urb->transfer_buffer, IUU_GET_STATE_REGISTER, 1);
 	usb_fill_bulk_urb(port->write_urb, port->serial->dev,
 			  usb_sndbulkpipe(port->serial->dev,
@@ -318,8 +302,6 @@ static int bulk_immediate(struct usb_serial_port *port, u8 *buf, u8 count)
 	struct usb_serial *serial = port->serial;
 	int actual = 0;
 
-	dbg("%s - enter", __func__);
-
 	/* send the data out the bulk port */
 
 	status =
@@ -341,10 +323,7 @@ static int read_immediate(struct usb_serial_port *port, u8 *buf, u8 count)
 	struct usb_serial *serial = port->serial;
 	int actual = 0;
 
-	dbg("%s - enter", __func__);
-
 	/* send the data out the bulk port */
-
 	status =
 	    usb_bulk_msg(serial->dev,
 			 usb_rcvbulkpipe(serial->dev,
@@ -367,8 +346,6 @@ static int iuu_led(struct usb_serial_port *port, unsigned int R,
 	if (!buf)
 		return -ENOMEM;
 
-	dbg("%s - enter", __func__);
-
 	buf[0] = IUU_SET_LED;
 	buf[1] = R & 0xFF;
 	buf[2] = (R >> 8) & 0xFF;
@@ -460,8 +437,6 @@ static int iuu_clk(struct usb_serial_port *port, int dwFrq)
 	unsigned int P2 = 0;
 	int frq = (int)dwFrq;
 
-	dbg("%s - enter", __func__);
-
 	if (frq == 0) {
 		priv->buf[Count++] = IUU_UART_WRITE_I2C;
 		priv->buf[Count++] = FrqGenAdr << 1;
@@ -590,8 +565,6 @@ static int iuu_uart_flush(struct usb_serial_port *port)
 	u8 rxcmd = IUU_UART_RX;
 	struct iuu_private *priv = usb_get_serial_port_data(port);
 
-	dbg("%s - enter", __func__);
-
 	if (iuu_led(port, 0xF000, 0, 0, 0xFF) < 0)
 		return -EIO;
 
@@ -630,8 +603,6 @@ static void read_buf_callback(struct urb *urb)
 	struct tty_struct *tty;
 	int status = urb->status;
 
-	dbg("%s - status = %d", __func__, status);
-
 	if (status) {
 		if (status == -EPROTO) {
 			/* reschedule needed */
@@ -659,7 +630,6 @@ static int iuu_bulk_write(struct usb_serial_port *port)
 	int i;
 	int buf_len;
 	char *buf_ptr = port->write_urb->transfer_buffer;
-	dbg("%s - enter", __func__);
 
 	spin_lock_irqsave(&priv->lock, flags);
 	*buf_ptr++ = IUU_UART_ESC;
@@ -691,7 +661,6 @@ static int iuu_bulk_write(struct usb_serial_port *port)
 static int iuu_read_buf(struct usb_serial_port *port, int len)
 {
 	int result;
-	dbg("%s - enter", __func__);
 
 	usb_fill_bulk_urb(port->read_urb, port->serial->dev,
 			  usb_rcvbulkpipe(port->serial->dev,
@@ -713,8 +682,6 @@ static void iuu_uart_read_callback(struct urb *urb)
 	unsigned char *data = urb->transfer_buffer;
 	priv->poll++;
 
-	dbg("%s - enter", __func__);
-
 	if (status) {
 		dbg("%s - status = %d", __func__, status);
 		/* error stop all */
@@ -771,7 +738,6 @@ static int iuu_uart_write(struct tty_struct *tty, struct usb_serial_port *port,
 {
 	struct iuu_private *priv = usb_get_serial_port_data(port);
 	unsigned long flags;
-	dbg("%s - enter", __func__);
 
 	if (count > 256)
 		return -ENOMEM;
@@ -792,8 +758,6 @@ static void read_rxcmd_callback(struct urb *urb)
 	int result;
 	int status = urb->status;
 
-	dbg("%s - status = %d", __func__, status);
-
 	if (status) {
 		/* error stop all */
 		return;
@@ -1015,8 +979,6 @@ static void iuu_close(struct usb_serial_port *port)
 	if (!serial)
 		return;
 
-	dbg("%s - port %d", __func__, port->number);
-
 	iuu_uart_off(port);
 	if (serial->dev) {
 		/* free writebuf */
@@ -1031,7 +993,6 @@ static void iuu_close(struct usb_serial_port *port)
 
 static void iuu_init_termios(struct tty_struct *tty)
 {
-	dbg("%s - enter", __func__);
 	*(tty->termios) = tty_std_termios;
 	tty->termios->c_cflag = CLOCAL | CREAD | CS8 | B9600
 				| TIOCM_CTS | CSTOPB | PARENB;
@@ -1188,8 +1149,6 @@ static int iuu_vcc_set(struct usb_serial_port *port, unsigned int vcc)
 	if (!buf)
 		return -ENOMEM;
 
-	dbg("%s - enter", __func__);
-
 	buf[0] = IUU_SET_VCC;
 	buf[1] = vcc & 0xFF;
 	buf[2] = (vcc >> 8) & 0xFF;
@@ -1250,15 +1209,11 @@ static DEVICE_ATTR(vcc_mode, S_IRUSR | S_IWUSR, show_vcc_mode,
 
 static int iuu_create_sysfs_attrs(struct usb_serial_port *port)
 {
-	dbg("%s", __func__);
-
 	return device_create_file(&port->dev, &dev_attr_vcc_mode);
 }
 
 static int iuu_remove_sysfs_attrs(struct usb_serial_port *port)
 {
-	dbg("%s", __func__);
-
 	device_remove_file(&port->dev, &dev_attr_vcc_mode);
 	return 0;
 }
@@ -1294,7 +1249,7 @@ static struct usb_serial_driver * const serial_drivers[] = {
 	&iuu_device, NULL
 };
 
-module_usb_serial_driver(iuu_driver, serial_drivers);
+module_usb_serial_driver(serial_drivers, id_table);
 
 MODULE_AUTHOR("Alain Degreffe eczema@ecze.com");
 
diff --git a/drivers/usb/serial/keyspan.c b/drivers/usb/serial/keyspan.c
index a39ddd1b0dc..a1b99243dac 100644
--- a/drivers/usb/serial/keyspan.c
+++ b/drivers/usb/serial/keyspan.c
@@ -130,15 +130,13 @@ struct keyspan_port_private {
 #include "keyspan_usa67msg.h"
 
 
-module_usb_serial_driver(keyspan_driver, serial_drivers);
+module_usb_serial_driver(serial_drivers, keyspan_ids_combined);
 
 static void keyspan_break_ctl(struct tty_struct *tty, int break_state)
 {
 	struct usb_serial_port *port = tty->driver_data;
 	struct keyspan_port_private 	*p_priv;
 
-	dbg("%s", __func__);
-
 	p_priv = usb_get_serial_port_data(port);
 
 	if (break_state == -1)
@@ -158,8 +156,6 @@ static void keyspan_set_termios(struct tty_struct *tty,
 	const struct keyspan_device_details	*d_details;
 	unsigned int 			cflag;
 
-	dbg("%s", __func__);
-
 	p_priv = usb_get_serial_port_data(port);
 	d_details = p_priv->device_details;
 	cflag = tty->termios->c_cflag;
@@ -306,8 +302,6 @@ static void	usa26_indat_callback(struct urb *urb)
 	unsigned char 		*data = urb->transfer_buffer;
 	int status = urb->status;
 
-	dbg("%s", __func__);
-
 	endpoint = usb_pipeendpoint(urb->pipe);
 
 	if (status) {
@@ -369,8 +363,6 @@ static void	usa2x_outdat_callback(struct urb *urb)
 
 static void	usa26_inack_callback(struct urb *urb)
 {
-	dbg("%s", __func__);
-
 }
 
 static void	usa26_outcont_callback(struct urb *urb)
@@ -452,7 +444,6 @@ exit: ;
 
 static void	usa26_glocont_callback(struct urb *urb)
 {
-	dbg("%s", __func__);
 }
 
 
@@ -465,8 +456,6 @@ static void usa28_indat_callback(struct urb *urb)
 	struct keyspan_port_private             *p_priv;
 	int status = urb->status;
 
-	dbg("%s", __func__);
-
 	port =  urb->context;
 	p_priv = usb_get_serial_port_data(port);
 	data = urb->transfer_buffer;
@@ -505,7 +494,6 @@ static void usa28_indat_callback(struct urb *urb)
 
 static void	usa28_inack_callback(struct urb *urb)
 {
-	dbg("%s", __func__);
 }
 
 static void	usa28_outcont_callback(struct urb *urb)
@@ -585,7 +573,6 @@ exit: ;
 
 static void	usa28_glocont_callback(struct urb *urb)
 {
-	dbg("%s", __func__);
 }
 
 
@@ -596,8 +583,6 @@ static void	usa49_glocont_callback(struct urb *urb)
 	struct keyspan_port_private *p_priv;
 	int i;
 
-	dbg("%s", __func__);
-
 	serial =  urb->context;
 	for (i = 0; i < serial->num_ports; ++i) {
 		port = serial->port[i];
@@ -625,8 +610,6 @@ static void	usa49_instat_callback(struct urb *urb)
 	int old_dcd_state;
 	int status = urb->status;
 
-	dbg("%s", __func__);
-
 	serial =  urb->context;
 
 	if (status) {
@@ -679,7 +662,6 @@ exit:	;
 
 static void	usa49_inack_callback(struct urb *urb)
 {
-	dbg("%s", __func__);
 }
 
 static void	usa49_indat_callback(struct urb *urb)
@@ -691,8 +673,6 @@ static void	usa49_indat_callback(struct urb *urb)
 	unsigned char 		*data = urb->transfer_buffer;
 	int status = urb->status;
 
-	dbg("%s", __func__);
-
 	endpoint = usb_pipeendpoint(urb->pipe);
 
 	if (status) {
@@ -742,8 +722,6 @@ static void usa49wg_indat_callback(struct urb *urb)
 	unsigned char 		*data = urb->transfer_buffer;
 	int status = urb->status;
 
-	dbg("%s", __func__);
-
 	serial = urb->context;
 
 	if (status) {
@@ -806,7 +784,6 @@ static void usa49wg_indat_callback(struct urb *urb)
 /* not used, usa-49 doesn't have per-port control endpoints */
 static void usa49_outcont_callback(struct urb *urb)
 {
-	dbg("%s", __func__);
 }
 
 static void usa90_indat_callback(struct urb *urb)
@@ -819,8 +796,6 @@ static void usa90_indat_callback(struct urb *urb)
 	unsigned char 		*data = urb->transfer_buffer;
 	int status = urb->status;
 
-	dbg("%s", __func__);
-
 	endpoint = usb_pipeendpoint(urb->pipe);
 
 	if (status) {
@@ -957,8 +932,6 @@ static void	usa67_instat_callback(struct urb *urb)
 	int old_dcd_state;
 	int status = urb->status;
 
-	dbg("%s", __func__);
-
 	serial = urb->context;
 
 	if (status) {
@@ -1010,8 +983,6 @@ static void usa67_glocont_callback(struct urb *urb)
 	struct keyspan_port_private *p_priv;
 	int i;
 
-	dbg("%s", __func__);
-
 	serial = urb->context;
 	for (i = 0; i < serial->num_ports; ++i) {
 		port = serial->port[i];
@@ -1035,7 +1006,6 @@ static int keyspan_write_room(struct tty_struct *tty)
 	int				data_len;
 	struct urb			*this_urb;
 
-	dbg("%s", __func__);
 	p_priv = usb_get_serial_port_data(port);
 	d_details = p_priv->device_details;
 
@@ -1078,8 +1048,6 @@ static int keyspan_open(struct tty_struct *tty, struct usb_serial_port *port)
 	p_priv = usb_get_serial_port_data(port);
 	d_details = p_priv->device_details;
 
-	dbg("%s - port%d.", __func__, port->number);
-
 	/* Set some sane defaults */
 	p_priv->rts_state = 1;
 	p_priv->dtr_state = 1;
@@ -1165,7 +1133,6 @@ static void keyspan_close(struct usb_serial_port *port)
 	struct keyspan_serial_private 	*s_priv;
 	struct keyspan_port_private 	*p_priv;
 
-	dbg("%s", __func__);
 	s_priv = usb_get_serial_data(serial);
 	p_priv = usb_get_serial_port_data(port);
 
@@ -1438,8 +1405,6 @@ static void keyspan_setup_urbs(struct usb_serial *serial)
 	struct callbacks		*cback;
 	int				endp;
 
-	dbg("%s", __func__);
-
 	s_priv = usb_get_serial_data(serial);
 	d_details = s_priv->device_details;
 
@@ -1853,8 +1818,6 @@ static int keyspan_usa28_send_setup(struct usb_serial *serial,
 	struct urb				*this_urb;
 	int 					device_port, err;
 
-	dbg("%s", __func__);
-
 	s_priv = usb_get_serial_data(serial);
 	p_priv = usb_get_serial_port_data(port);
 	d_details = s_priv->device_details;
@@ -1980,8 +1943,6 @@ static int keyspan_usa49_send_setup(struct usb_serial *serial,
 	struct urb				*this_urb;
 	int 					err, device_port;
 
-	dbg("%s", __func__);
-
 	s_priv = usb_get_serial_data(serial);
 	p_priv = usb_get_serial_port_data(port);
 	d_details = s_priv->device_details;
@@ -2168,8 +2129,6 @@ static int keyspan_usa90_send_setup(struct usb_serial *serial,
 	int 					err;
 	u8						prescaler;
 
-	dbg("%s", __func__);
-
 	s_priv = usb_get_serial_data(serial);
 	p_priv = usb_get_serial_port_data(port);
 	d_details = s_priv->device_details;
@@ -2300,8 +2259,6 @@ static int keyspan_usa67_send_setup(struct usb_serial *serial,
 	struct urb				*this_urb;
 	int 					err, device_port;
 
-	dbg("%s", __func__);
-
 	s_priv = usb_get_serial_data(serial);
 	p_priv = usb_get_serial_port_data(port);
 	d_details = s_priv->device_details;
@@ -2442,8 +2399,6 @@ static void keyspan_send_setup(struct usb_serial_port *port, int reset_port)
 	struct keyspan_serial_private *s_priv;
 	const struct keyspan_device_details *d_details;
 
-	dbg("%s", __func__);
-
 	s_priv = usb_get_serial_data(serial);
 	d_details = s_priv->device_details;
 
@@ -2477,8 +2432,6 @@ static int keyspan_startup(struct usb_serial *serial)
 	struct keyspan_port_private	*p_priv;
 	const struct keyspan_device_details	*d_details;
 
-	dbg("%s", __func__);
-
 	for (i = 0; (d_details = keyspan_devices[i]) != NULL; ++i)
 		if (d_details->product_id ==
 				le16_to_cpu(serial->dev->descriptor.idProduct))
@@ -2538,8 +2491,6 @@ static void keyspan_disconnect(struct usb_serial *serial)
 	struct keyspan_serial_private 	*s_priv;
 	struct keyspan_port_private	*p_priv;
 
-	dbg("%s", __func__);
-
 	s_priv = usb_get_serial_data(serial);
 
 	/* Stop reading/writing urbs */
@@ -2579,8 +2530,6 @@ static void keyspan_release(struct usb_serial *serial)
 	struct usb_serial_port		*port;
 	struct keyspan_serial_private 	*s_priv;
 
-	dbg("%s", __func__);
-
 	s_priv = usb_get_serial_data(serial);
 
 	/*  dbg("Freeing serial->private."); */
diff --git a/drivers/usb/serial/keyspan.h b/drivers/usb/serial/keyspan.h
index 622853c9e38..fe1c5d91692 100644
--- a/drivers/usb/serial/keyspan.h
+++ b/drivers/usb/serial/keyspan.h
@@ -487,13 +487,6 @@ static const struct usb_device_id keyspan_ids_combined[] = {
 
 MODULE_DEVICE_TABLE(usb, keyspan_ids_combined);
 
-static struct usb_driver keyspan_driver = {
-	.name =		"keyspan",                
-	.probe =	usb_serial_probe,
-	.disconnect =	usb_serial_disconnect,
-	.id_table =	keyspan_ids_combined,
-};
-
 /* usb_device_id table for the pre-firmware download keyspan devices */
 static const struct usb_device_id keyspan_pre_ids[] = {
 	{ USB_DEVICE(KEYSPAN_VENDOR_ID, keyspan_usa18x_pre_product_id) },
diff --git a/drivers/usb/serial/keyspan_pda.c b/drivers/usb/serial/keyspan_pda.c
index 693bcdfcb3d..a4ac3cfeffc 100644
--- a/drivers/usb/serial/keyspan_pda.c
+++ b/drivers/usb/serial/keyspan_pda.c
@@ -86,13 +86,6 @@ static const struct usb_device_id id_table_combined[] = {
 
 MODULE_DEVICE_TABLE(usb, id_table_combined);
 
-static struct usb_driver keyspan_pda_driver = {
-	.name =		"keyspan_pda",
-	.probe =	usb_serial_probe,
-	.disconnect =	usb_serial_disconnect,
-	.id_table =	id_table_combined,
-};
-
 static const struct usb_device_id id_table_std[] = {
 	{ USB_DEVICE(KEYSPAN_VENDOR_ID, KEYSPAN_PDA_ID) },
 	{ }						/* Terminating entry */
@@ -131,7 +124,6 @@ static void keyspan_pda_request_unthrottle(struct work_struct *work)
 	struct usb_serial *serial = priv->serial;
 	int result;
 
-	dbg(" request_unthrottle");
 	/* ask the device to tell us when the tx buffer becomes
 	   sufficiently empty */
 	result = usb_control_msg(serial->dev,
@@ -226,7 +218,7 @@ static void keyspan_pda_rx_throttle(struct tty_struct *tty)
 	   send an XOFF, although it might make sense to foist that off
 	   upon the device too. */
 	struct usb_serial_port *port = tty->driver_data;
-	dbg("keyspan_pda_rx_throttle port %d", port->number);
+
 	usb_kill_urb(port->interrupt_in_urb);
 }
 
@@ -235,7 +227,7 @@ static void keyspan_pda_rx_unthrottle(struct tty_struct *tty)
 {
 	struct usb_serial_port *port = tty->driver_data;
 	/* just restart the receive interrupt URB */
-	dbg("keyspan_pda_rx_unthrottle port %d", port->number);
+
 	if (usb_submit_urb(port->interrupt_in_urb, GFP_KERNEL))
 		dbg(" usb_submit_urb(read urb) failed");
 }
@@ -466,7 +458,6 @@ static int keyspan_pda_write(struct tty_struct *tty,
 	   select() or poll() too) until we receive that unthrottle interrupt.
 	   Block if we can't write anything at all, otherwise write as much as
 	   we can. */
-	dbg("keyspan_pda_write(%d)", count);
 	if (count == 0) {
 		dbg(" write request of 0 bytes");
 		return 0;
@@ -766,8 +757,6 @@ static int keyspan_pda_startup(struct usb_serial *serial)
 
 static void keyspan_pda_release(struct usb_serial *serial)
 {
-	dbg("%s", __func__);
-
 	kfree(usb_get_serial_port_data(serial->port[0]));
 }
 
@@ -834,7 +823,7 @@ static struct usb_serial_driver * const serial_drivers[] = {
 	NULL
 };
 
-module_usb_serial_driver(keyspan_pda_driver, serial_drivers);
+module_usb_serial_driver(serial_drivers, id_table_combined);
 
 MODULE_AUTHOR(DRIVER_AUTHOR);
 MODULE_DESCRIPTION(DRIVER_DESC);
diff --git a/drivers/usb/serial/kl5kusb105.c b/drivers/usb/serial/kl5kusb105.c
index 10f05407e53..5bed59cd577 100644
--- a/drivers/usb/serial/kl5kusb105.c
+++ b/drivers/usb/serial/kl5kusb105.c
@@ -86,13 +86,6 @@ static const struct usb_device_id id_table[] = {
 
 MODULE_DEVICE_TABLE(usb, id_table);
 
-static struct usb_driver kl5kusb105d_driver = {
-	.name =		"kl5kusb105d",
-	.probe =	usb_serial_probe,
-	.disconnect =	usb_serial_disconnect,
-	.id_table =	id_table,
-};
-
 static struct usb_serial_driver kl5kusb105d_device = {
 	.driver = {
 		.owner =	THIS_MODULE,
@@ -282,8 +275,6 @@ static void klsi_105_release(struct usb_serial *serial)
 {
 	int i;
 
-	dbg("%s", __func__);
-
 	for (i = 0; i < serial->num_ports; ++i)
 		kfree(usb_get_serial_port_data(serial->port[i]));
 }
@@ -298,8 +289,6 @@ static int  klsi_105_open(struct tty_struct *tty, struct usb_serial_port *port)
 	struct klsi_105_port_settings *cfg;
 	unsigned long flags;
 
-	dbg("%s port %d", __func__, port->number);
-
 	/* Do a defined restart:
 	 * Set up sane default baud rate and send the 'READ_ON'
 	 * vendor command.
@@ -376,8 +365,6 @@ static void klsi_105_close(struct usb_serial_port *port)
 {
 	int rc;
 
-	dbg("%s port %d", __func__, port->number);
-
 	mutex_lock(&port->serial->disc_mutex);
 	if (!port->serial->disconnected) {
 		/* send READ_OFF */
@@ -646,7 +633,6 @@ static int klsi_105_tiocmget(struct tty_struct *tty)
 	unsigned long flags;
 	int rc;
 	unsigned long line_state;
-	dbg("%s - request, just guessing", __func__);
 
 	rc = klsi_105_get_line_state(port, &line_state);
 	if (rc < 0) {
@@ -668,8 +654,6 @@ static int klsi_105_tiocmset(struct tty_struct *tty,
 {
 	int retval = -EINVAL;
 
-	dbg("%s", __func__);
-
 /* if this ever gets implemented, it should be done something like this:
 	struct usb_serial *serial = port->serial;
 	struct klsi_105_private *priv = usb_get_serial_port_data(port);
@@ -692,7 +676,7 @@ static int klsi_105_tiocmset(struct tty_struct *tty,
 	return retval;
 }
 
-module_usb_serial_driver(kl5kusb105d_driver, serial_drivers);
+module_usb_serial_driver(serial_drivers, id_table);
 
 MODULE_AUTHOR(DRIVER_AUTHOR);
 MODULE_DESCRIPTION(DRIVER_DESC);
diff --git a/drivers/usb/serial/kobil_sct.c b/drivers/usb/serial/kobil_sct.c
index 4a9a75eb9b9..fafeabb64c5 100644
--- a/drivers/usb/serial/kobil_sct.c
+++ b/drivers/usb/serial/kobil_sct.c
@@ -81,18 +81,8 @@ static const struct usb_device_id id_table[] = {
 	{ USB_DEVICE(KOBIL_VENDOR_ID, KOBIL_KAAN_SIM_PRODUCT_ID) },
 	{ }			/* Terminating entry */
 };
-
-
 MODULE_DEVICE_TABLE(usb, id_table);
 
-static struct usb_driver kobil_driver = {
-	.name =		"kobil",
-	.probe =	usb_serial_probe,
-	.disconnect =	usb_serial_disconnect,
-	.id_table =	id_table,
-};
-
-
 static struct usb_serial_driver kobil_device = {
 	.driver = {
 		.owner =	THIS_MODULE,
@@ -193,7 +183,6 @@ static int kobil_startup(struct usb_serial *serial)
 static void kobil_release(struct usb_serial *serial)
 {
 	int i;
-	dbg("%s - port %d", __func__, serial->port[0]->number);
 
 	for (i = 0; i < serial->num_ports; ++i)
 		kfree(usb_get_serial_port_data(serial->port[i]));
@@ -217,7 +206,6 @@ static int kobil_open(struct tty_struct *tty, struct usb_serial_port *port)
 	int transfer_buffer_length = 8;
 	int write_urb_transfer_buffer_length = 8;
 
-	dbg("%s - port %d", __func__, port->number);
 	priv = usb_get_serial_port_data(port);
 
 	/* allocate memory for transfer buffer */
@@ -327,8 +315,6 @@ static int kobil_open(struct tty_struct *tty, struct usb_serial_port *port)
 
 static void kobil_close(struct usb_serial_port *port)
 {
-	dbg("%s - port %d", __func__, port->number);
-
 	/* FIXME: Add rts/dtr methods */
 	if (port->write_urb) {
 		usb_poison_urb(port->write_urb);
@@ -349,8 +335,6 @@ static void kobil_read_int_callback(struct urb *urb)
 	int status = urb->status;
 /*	char *dbg_data; */
 
-	dbg("%s - port %d", __func__, port->number);
-
 	if (status) {
 		dbg("%s - port %d Read int status not zero: %d",
 		    __func__, port->number, status);
@@ -474,7 +458,6 @@ static int kobil_write(struct tty_struct *tty, struct usb_serial_port *port,
 
 static int kobil_write_room(struct tty_struct *tty)
 {
-	/* dbg("%s - port %d", __func__, port->number); */
 	/* FIXME */
 	return 8;
 }
@@ -683,7 +666,7 @@ static int kobil_ioctl(struct tty_struct *tty,
 	}
 }
 
-module_usb_serial_driver(kobil_driver, serial_drivers);
+module_usb_serial_driver(serial_drivers, id_table);
 
 MODULE_AUTHOR(DRIVER_AUTHOR);
 MODULE_DESCRIPTION(DRIVER_DESC);
diff --git a/drivers/usb/serial/mct_u232.c b/drivers/usb/serial/mct_u232.c
index 6edd26130e2..d0ec1aa5271 100644
--- a/drivers/usb/serial/mct_u232.c
+++ b/drivers/usb/serial/mct_u232.c
@@ -73,22 +73,14 @@ static void mct_u232_unthrottle(struct tty_struct *tty);
 /*
  * All of the device info needed for the MCT USB-RS232 converter.
  */
-static const struct usb_device_id id_table_combined[] = {
+static const struct usb_device_id id_table[] = {
 	{ USB_DEVICE(MCT_U232_VID, MCT_U232_PID) },
 	{ USB_DEVICE(MCT_U232_VID, MCT_U232_SITECOM_PID) },
 	{ USB_DEVICE(MCT_U232_VID, MCT_U232_DU_H3SP_PID) },
 	{ USB_DEVICE(MCT_U232_BELKIN_F5U109_VID, MCT_U232_BELKIN_F5U109_PID) },
 	{ }		/* Terminating entry */
 };
-
-MODULE_DEVICE_TABLE(usb, id_table_combined);
-
-static struct usb_driver mct_u232_driver = {
-	.name =		"mct_u232",
-	.probe =	usb_serial_probe,
-	.disconnect =	usb_serial_disconnect,
-	.id_table =	id_table_combined,
-};
+MODULE_DEVICE_TABLE(usb, id_table);
 
 static struct usb_serial_driver mct_u232_device = {
 	.driver = {
@@ -96,7 +88,7 @@ static struct usb_serial_driver mct_u232_device = {
 		.name =		"mct_u232",
 	},
 	.description =	     "MCT U232",
-	.id_table =	     id_table_combined,
+	.id_table =	     id_table,
 	.num_ports =	     1,
 	.open =		     mct_u232_open,
 	.close =	     mct_u232_close,
@@ -427,8 +419,6 @@ static void mct_u232_release(struct usb_serial *serial)
 	struct mct_u232_private *priv;
 	int i;
 
-	dbg("%s", __func__);
-
 	for (i = 0; i < serial->num_ports; ++i) {
 		/* My special items, the standard routines free my urbs */
 		priv = usb_get_serial_port_data(serial->port[i]);
@@ -446,8 +436,6 @@ static int  mct_u232_open(struct tty_struct *tty, struct usb_serial_port *port)
 	unsigned char last_lcr;
 	unsigned char last_msr;
 
-	dbg("%s port %d", __func__, port->number);
-
 	/* Compensate for a hardware bug: although the Sitecom U232-P25
 	 * device reports a maximum output packet size of 32 bytes,
 	 * it seems to be able to accept only 16 bytes (and that's what
@@ -528,8 +516,6 @@ static void mct_u232_dtr_rts(struct usb_serial_port *port, int on)
 
 static void mct_u232_close(struct usb_serial_port *port)
 {
-	dbg("%s port %d", __func__, port->number);
-
 	if (port->serial->dev) {
 		/* shutdown our urbs */
 		usb_kill_urb(port->write_urb);
@@ -572,7 +558,6 @@ static void mct_u232_read_int_callback(struct urb *urb)
 		return;
 	}
 
-	dbg("%s - port %d", __func__, port->number);
 	usb_serial_debug_data(debug, &port->dev, __func__,
 					urb->actual_length, data);
 
@@ -733,8 +718,6 @@ static void mct_u232_break_ctl(struct tty_struct *tty, int break_state)
 	unsigned char lcr;
 	unsigned long flags;
 
-	dbg("%sstate=%d", __func__, break_state);
-
 	spin_lock_irqsave(&priv->lock, flags);
 	lcr = priv->last_lcr;
 
@@ -753,8 +736,6 @@ static int mct_u232_tiocmget(struct tty_struct *tty)
 	unsigned int control_state;
 	unsigned long flags;
 
-	dbg("%s", __func__);
-
 	spin_lock_irqsave(&priv->lock, flags);
 	control_state = priv->control_state;
 	spin_unlock_irqrestore(&priv->lock, flags);
@@ -771,8 +752,6 @@ static int mct_u232_tiocmset(struct tty_struct *tty,
 	unsigned int control_state;
 	unsigned long flags;
 
-	dbg("%s", __func__);
-
 	spin_lock_irqsave(&priv->lock, flags);
 	control_state = priv->control_state;
 
@@ -796,8 +775,6 @@ static void mct_u232_throttle(struct tty_struct *tty)
 	struct mct_u232_private *priv = usb_get_serial_port_data(port);
 	unsigned int control_state;
 
-	dbg("%s - port %d", __func__, port->number);
-
 	spin_lock_irq(&priv->lock);
 	priv->rx_flags |= THROTTLED;
 	if (C_CRTSCTS(tty)) {
@@ -816,8 +793,6 @@ static void mct_u232_unthrottle(struct tty_struct *tty)
 	struct mct_u232_private *priv = usb_get_serial_port_data(port);
 	unsigned int control_state;
 
-	dbg("%s - port %d", __func__, port->number);
-
 	spin_lock_irq(&priv->lock);
 	if ((priv->rx_flags & THROTTLED) && C_CRTSCTS(tty)) {
 		priv->rx_flags &= ~THROTTLED;
@@ -906,7 +881,7 @@ static int  mct_u232_get_icount(struct tty_struct *tty,
 	return 0;
 }
 
-module_usb_serial_driver(mct_u232_driver, serial_drivers);
+module_usb_serial_driver(serial_drivers, id_table);
 
 MODULE_AUTHOR(DRIVER_AUTHOR);
 MODULE_DESCRIPTION(DRIVER_DESC);
diff --git a/drivers/usb/serial/metro-usb.c b/drivers/usb/serial/metro-usb.c
index 08d16e8c002..81423f7361d 100644
--- a/drivers/usb/serial/metro-usb.c
+++ b/drivers/usb/serial/metro-usb.c
@@ -17,7 +17,6 @@
 #include <linux/tty_flip.h>
 #include <linux/moduleparam.h>
 #include <linux/spinlock.h>
-#include <linux/errno.h>
 #include <linux/uaccess.h>
 #include <linux/usb/serial.h>
 
@@ -56,6 +55,47 @@ MODULE_DEVICE_TABLE(usb, id_table);
 /* Input parameter constants. */
 static bool debug;
 
+/* UNI-Directional mode commands for device configure */
+#define UNI_CMD_OPEN	0x80
+#define UNI_CMD_CLOSE	0xFF
+
+inline int metrousb_is_unidirectional_mode(struct usb_serial_port *port)
+{
+	__u16 product_id = le16_to_cpu(
+		port->serial->dev->descriptor.idProduct);
+
+	return product_id == FOCUS_PRODUCT_ID_UNI;
+}
+
+static int metrousb_send_unidirectional_cmd(u8 cmd, struct usb_serial_port *port)
+{
+	int ret;
+	int actual_len;
+	u8 *buffer_cmd = NULL;
+
+	if (!metrousb_is_unidirectional_mode(port))
+		return 0;
+
+	buffer_cmd = kzalloc(sizeof(cmd), GFP_KERNEL);
+	if (!buffer_cmd)
+		return -ENOMEM;
+
+	*buffer_cmd = cmd;
+
+	ret = usb_interrupt_msg(port->serial->dev,
+		usb_sndintpipe(port->serial->dev, port->interrupt_out_endpointAddress),
+		buffer_cmd, sizeof(cmd),
+		&actual_len, USB_CTRL_SET_TIMEOUT);
+
+	kfree(buffer_cmd);
+
+	if (ret < 0)
+		return ret;
+	else if (actual_len != sizeof(cmd))
+		return -EIO;
+	return 0;
+}
+
 static void metrousb_read_int_callback(struct urb *urb)
 {
 	struct usb_serial_port *port = urb->context;
@@ -78,12 +118,12 @@ static void metrousb_read_int_callback(struct urb *urb)
 		/* urb has been terminated. */
 		dev_dbg(&port->dev,
 			"%s - urb shutting down, error code=%d\n",
-			__func__, result);
+			__func__, urb->status);
 		return;
 	default:
 		dev_dbg(&port->dev,
 			"%s - non-zero urb received, error code=%d\n",
-			__func__, result);
+			__func__, urb->status);
 		goto exit;
 	}
 
@@ -91,7 +131,7 @@ static void metrousb_read_int_callback(struct urb *urb)
 	/* Set the data read from the usb port into the serial port buffer. */
 	tty = tty_port_tty_get(&port->port);
 	if (!tty) {
-		dev_dbg(&port->dev, "%s - bad tty pointer - exiting\n",
+		dev_err(&port->dev, "%s - bad tty pointer - exiting\n",
 			__func__);
 		return;
 	}
@@ -121,7 +161,7 @@ static void metrousb_read_int_callback(struct urb *urb)
 		result = usb_submit_urb(port->interrupt_in_urb, GFP_ATOMIC);
 
 		if (result)
-			dev_dbg(&port->dev,
+			dev_err(&port->dev,
 				"%s - failed submitting interrupt in urb, error code=%d\n",
 				__func__, result);
 	}
@@ -131,11 +171,19 @@ exit:
 	/* Try to resubmit the urb. */
 	result = usb_submit_urb(urb, GFP_ATOMIC);
 	if (result)
-		dev_dbg(&port->dev,
+		dev_err(&port->dev,
 			"%s - failed submitting interrupt in urb, error code=%d\n",
 			__func__, result);
 }
 
+static void metrousb_write_int_callback(struct urb *urb)
+{
+	struct usb_serial_port *port = urb->context;
+
+	dev_warn(&port->dev, "%s not implemented yet.\n",
+		__func__);
+}
+
 static void metrousb_cleanup(struct usb_serial_port *port)
 {
 	dev_dbg(&port->dev, "%s\n", __func__);
@@ -146,6 +194,9 @@ static void metrousb_cleanup(struct usb_serial_port *port)
 			usb_unlink_urb(port->interrupt_in_urb);
 			usb_kill_urb(port->interrupt_in_urb);
 		}
+
+		/* Send deactivate cmd to device */
+		metrousb_send_unidirectional_cmd(UNI_CMD_CLOSE, port);
 	}
 }
 
@@ -160,7 +211,7 @@ static int metrousb_open(struct tty_struct *tty, struct usb_serial_port *port)
 
 	/* Make sure the urb is initialized. */
 	if (!port->interrupt_in_urb) {
-		dev_dbg(&port->dev, "%s - interrupt urb not initialized\n",
+		dev_err(&port->dev, "%s - interrupt urb not initialized\n",
 			__func__);
 		return -ENODEV;
 	}
@@ -191,12 +242,21 @@ static int metrousb_open(struct tty_struct *tty, struct usb_serial_port *port)
 	result = usb_submit_urb(port->interrupt_in_urb, GFP_KERNEL);
 
 	if (result) {
-		dev_dbg(&port->dev,
+		dev_err(&port->dev,
 			"%s - failed submitting interrupt in urb, error code=%d\n",
 			__func__, result);
 		goto exit;
 	}
 
+	/* Send activate cmd to device */
+	result = metrousb_send_unidirectional_cmd(UNI_CMD_OPEN, port);
+	if (result) {
+		dev_err(&port->dev,
+			"%s - failed to configure device for port number=%d, error code=%d\n",
+			__func__, port->number, result);
+		goto exit;
+	}
+
 	dev_dbg(&port->dev, "%s - port open\n", __func__);
 exit:
 	return result;
@@ -221,7 +281,7 @@ static int metrousb_set_modem_ctrl(struct usb_serial *serial, unsigned int contr
 				METROUSB_SET_REQUEST_TYPE, METROUSB_SET_MODEM_CTRL_REQUEST,
 				control_state, 0, NULL, 0, WDR_TIMEOUT);
 	if (retval < 0)
-		dev_dbg(&serial->dev->dev,
+		dev_err(&serial->dev->dev,
 			"%s - set modem ctrl=0x%x failed, error code=%d\n",
 			__func__, mcr, retval);
 
@@ -354,29 +414,23 @@ static void metrousb_unthrottle(struct tty_struct *tty)
 	port->interrupt_in_urb->dev = port->serial->dev;
 	result = usb_submit_urb(port->interrupt_in_urb, GFP_ATOMIC);
 	if (result)
-		dev_dbg(tty->dev,
+		dev_err(tty->dev,
 			"failed submitting interrupt in urb error code=%d\n",
 			result);
 }
 
-static struct usb_driver metrousb_driver = {
-	.name =		"metro-usb",
-	.probe =	usb_serial_probe,
-	.disconnect =	usb_serial_disconnect,
-	.id_table =	id_table
-};
-
 static struct usb_serial_driver metrousb_device = {
 	.driver = {
 		.owner =	THIS_MODULE,
 		.name =		"metro-usb",
 	},
-	.description		= "Metrologic USB to serial converter.",
+	.description		= "Metrologic USB to Serial",
 	.id_table		= id_table,
 	.num_ports		= 1,
 	.open			= metrousb_open,
 	.close			= metrousb_cleanup,
 	.read_int_callback	= metrousb_read_int_callback,
+	.write_int_callback	= metrousb_write_int_callback,
 	.attach			= metrousb_startup,
 	.release		= metrousb_shutdown,
 	.throttle		= metrousb_throttle,
@@ -390,7 +444,7 @@ static struct usb_serial_driver * const serial_drivers[] = {
 	NULL,
 };
 
-module_usb_serial_driver(metrousb_driver, serial_drivers);
+module_usb_serial_driver(serial_drivers, id_table);
 
 MODULE_LICENSE("GPL");
 MODULE_AUTHOR("Philip Nicastro");
diff --git a/drivers/usb/serial/mos7720.c b/drivers/usb/serial/mos7720.c
index bdce8203412..a07dd3c8cfe 100644
--- a/drivers/usb/serial/mos7720.c
+++ b/drivers/usb/serial/mos7720.c
@@ -79,12 +79,12 @@ static struct usb_serial_driver moschip7720_2port_driver;
 #define MOSCHIP_DEVICE_ID_7720		0x7720
 #define MOSCHIP_DEVICE_ID_7715		0x7715
 
-static const struct usb_device_id moschip_port_id_table[] = {
+static const struct usb_device_id id_table[] = {
 	{ USB_DEVICE(USB_VENDOR_ID_MOSCHIP, MOSCHIP_DEVICE_ID_7720) },
 	{ USB_DEVICE(USB_VENDOR_ID_MOSCHIP, MOSCHIP_DEVICE_ID_7715) },
 	{ } /* terminating entry */
 };
-MODULE_DEVICE_TABLE(usb, moschip_port_id_table);
+MODULE_DEVICE_TABLE(usb, id_table);
 
 #ifdef CONFIG_USB_SERIAL_MOS7715_PARPORT
 
@@ -257,7 +257,6 @@ static void destroy_mos_parport(struct kref *kref)
 	struct mos7715_parport *mos_parport =
 		container_of(kref, struct mos7715_parport, ref_count);
 
-	dbg("%s called", __func__);
 	kfree(mos_parport);
 }
 
@@ -266,7 +265,7 @@ static void destroy_urbtracker(struct kref *kref)
 	struct urbtracker *urbtrack =
 		container_of(kref, struct urbtracker, ref_count);
 	struct mos7715_parport *mos_parport = urbtrack->mos_parport;
-	dbg("%s called", __func__);
+
 	usb_free_urb(urbtrack->urb);
 	kfree(urbtrack);
 	kref_put(&mos_parport->ref_count, destroy_mos_parport);
@@ -285,8 +284,6 @@ static void send_deferred_urbs(unsigned long _mos_parport)
 	struct urbtracker *urbtrack;
 	struct list_head *cursor, *next;
 
-	dbg("%s called", __func__);
-
 	/* if release function ran, game over */
 	if (unlikely(mos_parport->serial == NULL))
 		return;
@@ -335,7 +332,7 @@ static void async_complete(struct urb *urb)
 {
 	struct urbtracker *urbtrack = urb->context;
 	int status = urb->status;
-	dbg("%s called", __func__);
+
 	if (unlikely(status))
 		dbg("%s - nonzero urb status received: %d", __func__, status);
 
@@ -355,7 +352,6 @@ static int write_parport_reg_nonblock(struct mos7715_parport *mos_parport,
 	struct usb_ctrlrequest setup;
 	struct usb_serial *serial = mos_parport->serial;
 	struct usb_device *usbdev = serial->dev;
-	dbg("%s called", __func__);
 
 	/* create and initialize the control urb and containing urbtracker */
 	urbtrack = kmalloc(sizeof(struct urbtracker), GFP_ATOMIC);
@@ -476,7 +472,7 @@ static inline void parport_epilogue(struct parport *pp)
 static void parport_mos7715_write_data(struct parport *pp, unsigned char d)
 {
 	struct mos7715_parport *mos_parport = pp->private_data;
-	dbg("%s called: %2.2x", __func__, d);
+
 	if (parport_prologue(pp) < 0)
 		return;
 	mos7715_change_mode(mos_parport, SPP);
@@ -488,7 +484,7 @@ static unsigned char parport_mos7715_read_data(struct parport *pp)
 {
 	struct mos7715_parport *mos_parport = pp->private_data;
 	unsigned char d;
-	dbg("%s called", __func__);
+
 	if (parport_prologue(pp) < 0)
 		return 0;
 	read_mos_reg(mos_parport->serial, dummy, DPR, &d);
@@ -500,7 +496,7 @@ static void parport_mos7715_write_control(struct parport *pp, unsigned char d)
 {
 	struct mos7715_parport *mos_parport = pp->private_data;
 	__u8 data;
-	dbg("%s called: %2.2x", __func__, d);
+
 	if (parport_prologue(pp) < 0)
 		return;
 	data = ((__u8)d & 0x0f) | (mos_parport->shadowDCR & 0xf0);
@@ -513,7 +509,7 @@ static unsigned char parport_mos7715_read_control(struct parport *pp)
 {
 	struct mos7715_parport *mos_parport = pp->private_data;
 	__u8 dcr;
-	dbg("%s called", __func__);
+
 	spin_lock(&release_lock);
 	mos_parport = pp->private_data;
 	if (unlikely(mos_parport == NULL)) {
@@ -531,7 +527,7 @@ static unsigned char parport_mos7715_frob_control(struct parport *pp,
 {
 	struct mos7715_parport *mos_parport = pp->private_data;
 	__u8 dcr;
-	dbg("%s called", __func__);
+
 	mask &= 0x0f;
 	val &= 0x0f;
 	if (parport_prologue(pp) < 0)
@@ -547,7 +543,7 @@ static unsigned char parport_mos7715_read_status(struct parport *pp)
 {
 	unsigned char status;
 	struct mos7715_parport *mos_parport = pp->private_data;
-	dbg("%s called", __func__);
+
 	spin_lock(&release_lock);
 	mos_parport = pp->private_data;
 	if (unlikely(mos_parport == NULL)) {	/* release called */
@@ -561,17 +557,16 @@ static unsigned char parport_mos7715_read_status(struct parport *pp)
 
 static void parport_mos7715_enable_irq(struct parport *pp)
 {
-	dbg("%s called", __func__);
 }
+
 static void parport_mos7715_disable_irq(struct parport *pp)
 {
-	dbg("%s called", __func__);
 }
 
 static void parport_mos7715_data_forward(struct parport *pp)
 {
 	struct mos7715_parport *mos_parport = pp->private_data;
-	dbg("%s called", __func__);
+
 	if (parport_prologue(pp) < 0)
 		return;
 	mos7715_change_mode(mos_parport, PS2);
@@ -583,7 +578,7 @@ static void parport_mos7715_data_forward(struct parport *pp)
 static void parport_mos7715_data_reverse(struct parport *pp)
 {
 	struct mos7715_parport *mos_parport = pp->private_data;
-	dbg("%s called", __func__);
+
 	if (parport_prologue(pp) < 0)
 		return;
 	mos7715_change_mode(mos_parport, PS2);
@@ -595,7 +590,6 @@ static void parport_mos7715_data_reverse(struct parport *pp)
 static void parport_mos7715_init_state(struct pardevice *dev,
 				       struct parport_state *s)
 {
-	dbg("%s called", __func__);
 	s->u.pc.ctr = DCR_INIT_VAL;
 	s->u.pc.ecr = ECR_INIT_VAL;
 }
@@ -605,7 +599,7 @@ static void parport_mos7715_save_state(struct parport *pp,
 				       struct parport_state *s)
 {
 	struct mos7715_parport *mos_parport;
-	dbg("%s called", __func__);
+
 	spin_lock(&release_lock);
 	mos_parport = pp->private_data;
 	if (unlikely(mos_parport == NULL)) {	/* release called */
@@ -622,7 +616,7 @@ static void parport_mos7715_restore_state(struct parport *pp,
 					  struct parport_state *s)
 {
 	struct mos7715_parport *mos_parport;
-	dbg("%s called", __func__);
+
 	spin_lock(&release_lock);
 	mos_parport = pp->private_data;
 	if (unlikely(mos_parport == NULL)) {	/* release called */
@@ -641,7 +635,7 @@ static size_t parport_mos7715_write_compat(struct parport *pp,
 	int retval;
 	struct mos7715_parport *mos_parport = pp->private_data;
 	int actual_len;
-	dbg("%s called: %u chars", __func__, (unsigned int)len);
+
 	if (parport_prologue(pp) < 0)
 		return 0;
 	mos7715_change_mode(mos_parport, PPF);
@@ -2164,20 +2158,13 @@ static void mos7720_release(struct usb_serial *serial)
 		kfree(usb_get_serial_port_data(serial->port[i]));
 }
 
-static struct usb_driver usb_driver = {
-	.name =		"moschip7720",
-	.probe =	usb_serial_probe,
-	.disconnect =	usb_serial_disconnect,
-	.id_table =	moschip_port_id_table,
-};
-
 static struct usb_serial_driver moschip7720_2port_driver = {
 	.driver = {
 		.owner =	THIS_MODULE,
 		.name =		"moschip7720",
 	},
 	.description		= "Moschip 2 port adapter",
-	.id_table		= moschip_port_id_table,
+	.id_table		= id_table,
 	.calc_num_ports		= mos77xx_calc_num_ports,
 	.open			= mos7720_open,
 	.close			= mos7720_close,
@@ -2203,7 +2190,7 @@ static struct usb_serial_driver * const serial_drivers[] = {
 	&moschip7720_2port_driver, NULL
 };
 
-module_usb_serial_driver(usb_driver, serial_drivers);
+module_usb_serial_driver(serial_drivers, id_table);
 
 MODULE_AUTHOR(DRIVER_AUTHOR);
 MODULE_DESCRIPTION(DRIVER_DESC);
diff --git a/drivers/usb/serial/mos7840.c b/drivers/usb/serial/mos7840.c
index c526550694a..29160f8b510 100644
--- a/drivers/usb/serial/mos7840.c
+++ b/drivers/usb/serial/mos7840.c
@@ -114,10 +114,10 @@
 #define USB_VENDOR_ID_MOSCHIP           0x9710
 #define MOSCHIP_DEVICE_ID_7840          0x7840
 #define MOSCHIP_DEVICE_ID_7820          0x7820
+#define MOSCHIP_DEVICE_ID_7810          0x7810
 /* The native component can have its vendor/device id's overridden
  * in vendor-specific implementations.  Such devices can be handled
- * by making a change here, in moschip_port_id_table, and in
- * moschip_id_table_combined
+ * by making a change here, in id_table.
  */
 #define USB_VENDOR_ID_BANDB              0x0856
 #define BANDB_DEVICE_ID_USO9ML2_2        0xAC22
@@ -184,31 +184,16 @@
 #define NUM_URBS                        16	/* URB Count */
 #define URB_TRANSFER_BUFFER_SIZE        32	/* URB Size  */
 
+/* LED on/off milliseconds*/
+#define LED_ON_MS	500
+#define LED_OFF_MS	500
 
-static const struct usb_device_id moschip_port_id_table[] = {
-	{USB_DEVICE(USB_VENDOR_ID_MOSCHIP, MOSCHIP_DEVICE_ID_7840)},
-	{USB_DEVICE(USB_VENDOR_ID_MOSCHIP, MOSCHIP_DEVICE_ID_7820)},
-	{USB_DEVICE(USB_VENDOR_ID_BANDB, BANDB_DEVICE_ID_USO9ML2_2)},
-	{USB_DEVICE(USB_VENDOR_ID_BANDB, BANDB_DEVICE_ID_USO9ML2_2P)},
-	{USB_DEVICE(USB_VENDOR_ID_BANDB, BANDB_DEVICE_ID_USO9ML2_4)},
-	{USB_DEVICE(USB_VENDOR_ID_BANDB, BANDB_DEVICE_ID_USO9ML2_4P)},
-	{USB_DEVICE(USB_VENDOR_ID_BANDB, BANDB_DEVICE_ID_US9ML2_2)},
-	{USB_DEVICE(USB_VENDOR_ID_BANDB, BANDB_DEVICE_ID_US9ML2_4)},
-	{USB_DEVICE(USB_VENDOR_ID_BANDB, BANDB_DEVICE_ID_USPTL4_2)},
-	{USB_DEVICE(USB_VENDOR_ID_BANDB, BANDB_DEVICE_ID_USPTL4_4)},
-	{USB_DEVICE(USB_VENDOR_ID_BANDB, BANDB_DEVICE_ID_USOPTL4_2)},
-	{USB_DEVICE(USB_VENDOR_ID_BANDB, BANDB_DEVICE_ID_USOPTL4_2P)},
-	{USB_DEVICE(USB_VENDOR_ID_BANDB, BANDB_DEVICE_ID_USOPTL4_4)},
-	{USB_DEVICE(USB_VENDOR_ID_BANDB, BANDB_DEVICE_ID_USOPTL4_4P)},
-	{USB_DEVICE(USB_VENDOR_ID_BANDB, BANDB_DEVICE_ID_USOPTL2_4)},
-	{USB_DEVICE(USB_VENDOR_ID_ATENINTL, ATENINTL_DEVICE_ID_UC2324)},
-	{USB_DEVICE(USB_VENDOR_ID_ATENINTL, ATENINTL_DEVICE_ID_UC2322)},
-	{}			/* terminating entry */
-};
+static int device_type;
 
-static const struct usb_device_id moschip_id_table_combined[] __devinitconst = {
+static const struct usb_device_id id_table[] __devinitconst = {
 	{USB_DEVICE(USB_VENDOR_ID_MOSCHIP, MOSCHIP_DEVICE_ID_7840)},
 	{USB_DEVICE(USB_VENDOR_ID_MOSCHIP, MOSCHIP_DEVICE_ID_7820)},
+	{USB_DEVICE(USB_VENDOR_ID_MOSCHIP, MOSCHIP_DEVICE_ID_7810)},
 	{USB_DEVICE(USB_VENDOR_ID_BANDB, BANDB_DEVICE_ID_USO9ML2_2)},
 	{USB_DEVICE(USB_VENDOR_ID_BANDB, BANDB_DEVICE_ID_USO9ML2_2P)},
 	{USB_DEVICE(USB_VENDOR_ID_BANDB, BANDB_DEVICE_ID_USO9ML2_4)},
@@ -226,8 +211,7 @@ static const struct usb_device_id moschip_id_table_combined[] __devinitconst = {
 	{USB_DEVICE(USB_VENDOR_ID_ATENINTL, ATENINTL_DEVICE_ID_UC2322)},
 	{}			/* terminating entry */
 };
-
-MODULE_DEVICE_TABLE(usb, moschip_id_table_combined);
+MODULE_DEVICE_TABLE(usb, id_table);
 
 /* This structure holds all of the local port information */
 
@@ -261,8 +245,13 @@ struct moschip_port {
 	struct urb *write_urb_pool[NUM_URBS];
 	char busy[NUM_URBS];
 	bool read_urb_busy;
-};
 
+	/* For device(s) with LED indicator */
+	bool has_led;
+	bool led_flag;
+	struct timer_list led_timer1;	/* Timer for LED on */
+	struct timer_list led_timer2;	/* Timer for LED off */
+};
 
 static bool debug;
 
@@ -572,6 +561,69 @@ static int mos7840_get_reg(struct moschip_port *mcs, __u16 Wval, __u16 reg,
 	return ret;
 }
 
+static void mos7840_set_led_callback(struct urb *urb)
+{
+	switch (urb->status) {
+	case 0:
+		/* Success */
+		break;
+	case -ECONNRESET:
+	case -ENOENT:
+	case -ESHUTDOWN:
+		/* This urb is terminated, clean up */
+		dbg("%s - urb shutting down with status: %d", __func__,
+			urb->status);
+		break;
+	default:
+		dbg("%s - nonzero urb status received: %d", __func__,
+			urb->status);
+	}
+}
+
+static void mos7840_set_led_async(struct moschip_port *mcs, __u16 wval,
+				__u16 reg)
+{
+	struct usb_device *dev = mcs->port->serial->dev;
+	struct usb_ctrlrequest *dr = mcs->dr;
+
+	dr->bRequestType = MCS_WR_RTYPE;
+	dr->bRequest = MCS_WRREQ;
+	dr->wValue = cpu_to_le16(wval);
+	dr->wIndex = cpu_to_le16(reg);
+	dr->wLength = cpu_to_le16(0);
+
+	usb_fill_control_urb(mcs->control_urb, dev, usb_sndctrlpipe(dev, 0),
+		(unsigned char *)dr, NULL, 0, mos7840_set_led_callback, NULL);
+
+	usb_submit_urb(mcs->control_urb, GFP_ATOMIC);
+}
+
+static void mos7840_set_led_sync(struct usb_serial_port *port, __u16 reg,
+				__u16 val)
+{
+	struct usb_device *dev = port->serial->dev;
+
+	usb_control_msg(dev, usb_sndctrlpipe(dev, 0), MCS_WRREQ, MCS_WR_RTYPE,
+			val, reg, NULL, 0, MOS_WDR_TIMEOUT);
+}
+
+static void mos7840_led_off(unsigned long arg)
+{
+	struct moschip_port *mcs = (struct moschip_port *) arg;
+
+	/* Turn off LED */
+	mos7840_set_led_async(mcs, 0x0300, MODEM_CONTROL_REGISTER);
+	mod_timer(&mcs->led_timer2,
+				jiffies + msecs_to_jiffies(LED_OFF_MS));
+}
+
+static void mos7840_led_flag_off(unsigned long arg)
+{
+	struct moschip_port *mcs = (struct moschip_port *) arg;
+
+	mcs->led_flag = false;
+}
+
 /*****************************************************************************
  * mos7840_interrupt_callback
  *	this is the callback function for when we have received data on the
@@ -591,8 +643,6 @@ static void mos7840_interrupt_callback(struct urb *urb)
 	__u16 wval, wreg = 0;
 	int status = urb->status;
 
-	dbg("%s", " : Entering");
-
 	switch (status) {
 	case 0:
 		/* success */
@@ -766,12 +816,8 @@ static void mos7840_bulk_in_callback(struct urb *urb)
 		return;
 	}
 
-	dbg("%s", "Entering... ");
-
 	data = urb->transfer_buffer;
 
-	dbg("%s", "Entering ...........");
-
 	if (urb->actual_length) {
 		tty = tty_port_tty_get(&mos7840_port->port->port);
 		if (tty) {
@@ -792,6 +838,14 @@ static void mos7840_bulk_in_callback(struct urb *urb)
 		return;
 	}
 
+	/* Turn on LED */
+	if (mos7840_port->has_led && !mos7840_port->led_flag) {
+		mos7840_port->led_flag = true;
+		mos7840_set_led_async(mos7840_port, 0x0301,
+					MODEM_CONTROL_REGISTER);
+		mod_timer(&mos7840_port->led_timer1,
+				jiffies + msecs_to_jiffies(LED_ON_MS));
+	}
 
 	mos7840_port->read_urb_busy = true;
 	retval = usb_submit_urb(mos7840_port->read_urb, GFP_ATOMIC);
@@ -835,8 +889,6 @@ static void mos7840_bulk_out_data_callback(struct urb *urb)
 		return;
 	}
 
-	dbg("%s", "Entering .........");
-
 	tty = tty_port_tty_get(&mos7840_port->port->port);
 	if (tty && mos7840_port->open)
 		tty_wakeup(tty);
@@ -878,8 +930,6 @@ static int mos7840_open(struct tty_struct *tty, struct usb_serial_port *port)
 	struct moschip_port *mos7840_port;
 	struct moschip_port *port0;
 
-	dbg ("%s enter", __func__);
-
 	if (mos7840_port_paranoia_check(port, __func__)) {
 		dbg("%s", "Port Paranoia failed");
 		return -ENODEV;
@@ -1151,10 +1201,7 @@ static int mos7840_open(struct tty_struct *tty, struct usb_serial_port *port)
 	dbg("usb_serial serial:%p       mos7840_port:%p\n      usb_serial_port port:%p",
 				serial, mos7840_port, port);
 
-	dbg ("%s leave", __func__);
-
 	return 0;
-
 }
 
 /*****************************************************************************
@@ -1175,18 +1222,14 @@ static int mos7840_chars_in_buffer(struct tty_struct *tty)
 	unsigned long flags;
 	struct moschip_port *mos7840_port;
 
-	dbg("%s", " mos7840_chars_in_buffer:entering ...........");
-
 	if (mos7840_port_paranoia_check(port, __func__)) {
 		dbg("%s", "Invalid port");
 		return 0;
 	}
 
 	mos7840_port = mos7840_get_port_private(port);
-	if (mos7840_port == NULL) {
-		dbg("%s", "mos7840_break:leaving ...........");
+	if (mos7840_port == NULL)
 		return 0;
-	}
 
 	spin_lock_irqsave(&mos7840_port->pool_lock, flags);
 	for (i = 0; i < NUM_URBS; ++i)
@@ -1211,8 +1254,6 @@ static void mos7840_close(struct usb_serial_port *port)
 	int j;
 	__u16 Data;
 
-	dbg("%s", "mos7840_close:entering...");
-
 	if (mos7840_port_paranoia_check(port, __func__)) {
 		dbg("%s", "Port Paranoia failed");
 		return;
@@ -1287,8 +1328,6 @@ static void mos7840_close(struct usb_serial_port *port)
 	mos7840_set_uart_reg(port, INTERRUPT_ENABLE_REGISTER, Data);
 
 	mos7840_port->open = 0;
-
-	dbg("%s", "Leaving ............");
 }
 
 /************************************************************************
@@ -1343,9 +1382,6 @@ static void mos7840_break(struct tty_struct *tty, int break_state)
 	struct usb_serial *serial;
 	struct moschip_port *mos7840_port;
 
-	dbg("%s", "Entering ...........");
-	dbg("mos7840_break: Start");
-
 	if (mos7840_port_paranoia_check(port, __func__)) {
 		dbg("%s", "Port Paranoia failed");
 		return;
@@ -1395,8 +1431,6 @@ static int mos7840_write_room(struct tty_struct *tty)
 	unsigned long flags;
 	struct moschip_port *mos7840_port;
 
-	dbg("%s", " mos7840_write_room:entering ...........");
-
 	if (mos7840_port_paranoia_check(port, __func__)) {
 		dbg("%s", "Invalid port");
 		dbg("%s", " mos7840_write_room:leaving ...........");
@@ -1445,9 +1479,6 @@ static int mos7840_write(struct tty_struct *tty, struct usb_serial_port *port,
 	/* __u16 Data; */
 	const unsigned char *current_position = data;
 	unsigned char *data1;
-	dbg("%s", "entering ...........");
-	/* dbg("mos7840_write: mos7840_port->shadowLCR is %x",
-					mos7840_port->shadowLCR); */
 
 #ifdef NOTMOS7840
 	Data = 0x00;
@@ -1554,6 +1585,14 @@ static int mos7840_write(struct tty_struct *tty, struct usb_serial_port *port,
 	data1 = urb->transfer_buffer;
 	dbg("bulkout endpoint is %d", port->bulk_out_endpointAddress);
 
+	/* Turn on LED */
+	if (mos7840_port->has_led && !mos7840_port->led_flag) {
+		mos7840_port->led_flag = true;
+		mos7840_set_led_sync(port, MODEM_CONTROL_REGISTER, 0x0301);
+		mod_timer(&mos7840_port->led_timer1,
+				jiffies + msecs_to_jiffies(LED_ON_MS));
+	}
+
 	/* send it down the pipe */
 	status = usb_submit_urb(urb, GFP_ATOMIC);
 
@@ -1602,8 +1641,6 @@ static void mos7840_throttle(struct tty_struct *tty)
 		return;
 	}
 
-	dbg("%s", "Entering ..........");
-
 	/* if we are implementing XON/XOFF, send the stop character */
 	if (I_IXOFF(tty)) {
 		unsigned char stop_char = STOP_CHAR(tty);
@@ -1646,8 +1683,6 @@ static void mos7840_unthrottle(struct tty_struct *tty)
 		return;
 	}
 
-	dbg("%s", "Entering ..........");
-
 	/* if we are implementing XON/XOFF, send the start character */
 	if (I_IXOFF(tty)) {
 		unsigned char start_char = START_CHAR(tty);
@@ -1676,8 +1711,6 @@ static int mos7840_tiocmget(struct tty_struct *tty)
 	int status;
 	mos7840_port = mos7840_get_port_private(port);
 
-	dbg("%s - port %d", __func__, port->number);
-
 	if (mos7840_port == NULL)
 		return -ENODEV;
 
@@ -1704,8 +1737,6 @@ static int mos7840_tiocmset(struct tty_struct *tty,
 	unsigned int mcr;
 	int status;
 
-	dbg("%s - port %d", __func__, port->number);
-
 	mos7840_port = mos7840_get_port_private(port);
 
 	if (mos7840_port == NULL)
@@ -1746,7 +1777,6 @@ static int mos7840_tiocmset(struct tty_struct *tty,
 static int mos7840_calc_baud_rate_divisor(int baudRate, int *divisor,
 					  __u16 *clk_sel_val)
 {
-
 	dbg("%s - %d", __func__, baudRate);
 
 	if (baudRate <= 115200) {
@@ -1839,8 +1869,6 @@ static int mos7840_send_cmd_write_baud_rate(struct moschip_port *mos7840_port,
 		return -1;
 	}
 
-	dbg("%s", "Entering ..........");
-
 	number = mos7840_port->port->number - mos7840_port->port->serial->minor;
 
 	dbg("%s - port = %d, baud = %d", __func__,
@@ -1966,8 +1994,6 @@ static void mos7840_change_port_settings(struct tty_struct *tty,
 		return;
 	}
 
-	dbg("%s", "Entering ..........");
-
 	lData = LCR_BITS_8;
 	lStop = LCR_STOP_1;
 	lParity = LCR_PAR_NONE;
@@ -2108,7 +2134,7 @@ static void mos7840_set_termios(struct tty_struct *tty,
 	unsigned int cflag;
 	struct usb_serial *serial;
 	struct moschip_port *mos7840_port;
-	dbg("mos7840_set_termios: START");
+
 	if (mos7840_port_paranoia_check(port, __func__)) {
 		dbg("%s", "Invalid port");
 		return;
@@ -2327,28 +2353,74 @@ static int mos7840_ioctl(struct tty_struct *tty,
 	return -ENOIOCTLCMD;
 }
 
+static int mos7810_check(struct usb_serial *serial)
+{
+	int i, pass_count = 0;
+	__u16 data = 0, mcr_data = 0;
+	__u16 test_pattern = 0x55AA;
+
+	/* Store MCR setting */
+	usb_control_msg(serial->dev, usb_rcvctrlpipe(serial->dev, 0),
+		MCS_RDREQ, MCS_RD_RTYPE, 0x0300, MODEM_CONTROL_REGISTER,
+		&mcr_data, VENDOR_READ_LENGTH, MOS_WDR_TIMEOUT);
+
+	for (i = 0; i < 16; i++) {
+		/* Send the 1-bit test pattern out to MCS7810 test pin */
+		usb_control_msg(serial->dev, usb_sndctrlpipe(serial->dev, 0),
+			MCS_WRREQ, MCS_WR_RTYPE,
+			(0x0300 | (((test_pattern >> i) & 0x0001) << 1)),
+			MODEM_CONTROL_REGISTER, NULL, 0, MOS_WDR_TIMEOUT);
+
+		/* Read the test pattern back */
+		usb_control_msg(serial->dev, usb_rcvctrlpipe(serial->dev, 0),
+			MCS_RDREQ, MCS_RD_RTYPE, 0, GPIO_REGISTER, &data,
+			VENDOR_READ_LENGTH, MOS_WDR_TIMEOUT);
+
+		/* If this is a MCS7810 device, both test patterns must match */
+		if (((test_pattern >> i) ^ (~data >> 1)) & 0x0001)
+			break;
+
+		pass_count++;
+	}
+
+	/* Restore MCR setting */
+	usb_control_msg(serial->dev, usb_sndctrlpipe(serial->dev, 0), MCS_WRREQ,
+		MCS_WR_RTYPE, 0x0300 | mcr_data, MODEM_CONTROL_REGISTER, NULL,
+		0, MOS_WDR_TIMEOUT);
+
+	if (pass_count == 16)
+		return 1;
+
+	return 0;
+}
+
 static int mos7840_calc_num_ports(struct usb_serial *serial)
 {
-	__u16 Data = 0x00;
-	int ret = 0;
+	__u16 data = 0x00;
 	int mos7840_num_ports;
 
-	ret = usb_control_msg(serial->dev, usb_rcvctrlpipe(serial->dev, 0),
-		MCS_RDREQ, MCS_RD_RTYPE, 0, GPIO_REGISTER, &Data,
+	usb_control_msg(serial->dev, usb_rcvctrlpipe(serial->dev, 0),
+		MCS_RDREQ, MCS_RD_RTYPE, 0, GPIO_REGISTER, &data,
 		VENDOR_READ_LENGTH, MOS_WDR_TIMEOUT);
 
-	if ((Data & 0x01) == 0) {
-		mos7840_num_ports = 2;
-		serial->num_bulk_in = 2;
-		serial->num_bulk_out = 2;
-		serial->num_ports = 2;
+	if (serial->dev->descriptor.idProduct == MOSCHIP_DEVICE_ID_7810 ||
+		serial->dev->descriptor.idProduct == MOSCHIP_DEVICE_ID_7820) {
+		device_type = serial->dev->descriptor.idProduct;
 	} else {
-		mos7840_num_ports = 4;
-		serial->num_bulk_in = 4;
-		serial->num_bulk_out = 4;
-		serial->num_ports = 4;
+		/* For a MCS7840 device GPIO0 must be set to 1 */
+		if ((data & 0x01) == 1)
+			device_type = MOSCHIP_DEVICE_ID_7840;
+		else if (mos7810_check(serial))
+			device_type = MOSCHIP_DEVICE_ID_7810;
+		else
+			device_type = MOSCHIP_DEVICE_ID_7820;
 	}
 
+	mos7840_num_ports = (device_type >> 4) & 0x000F;
+	serial->num_bulk_in = mos7840_num_ports;
+	serial->num_bulk_out = mos7840_num_ports;
+	serial->num_ports = mos7840_num_ports;
+
 	return mos7840_num_ports;
 }
 
@@ -2361,9 +2433,7 @@ static int mos7840_startup(struct usb_serial *serial)
 	struct moschip_port *mos7840_port;
 	struct usb_device *dev;
 	int i, status;
-
 	__u16 Data;
-	dbg("%s", "mos7840_startup :Entering..........");
 
 	if (!serial) {
 		dbg("%s", "Invalid Handler");
@@ -2372,9 +2442,6 @@ static int mos7840_startup(struct usb_serial *serial)
 
 	dev = serial->dev;
 
-	dbg("%s", "Entering...");
-	dbg ("mos7840_startup: serial = %p", serial);
-
 	/* we set up the pointers to the endpoints in the mos7840_open *
 	 * function, as the structures aren't created yet.             */
 
@@ -2563,6 +2630,34 @@ static int mos7840_startup(struct usb_serial *serial)
 			status = -ENOMEM;
 			goto error;
 		}
+
+		mos7840_port->has_led = false;
+
+		/* Initialize LED timers */
+		if (device_type == MOSCHIP_DEVICE_ID_7810) {
+			mos7840_port->has_led = true;
+
+			init_timer(&mos7840_port->led_timer1);
+			mos7840_port->led_timer1.function = mos7840_led_off;
+			mos7840_port->led_timer1.expires =
+					jiffies + msecs_to_jiffies(LED_ON_MS);
+			mos7840_port->led_timer1.data =
+						(unsigned long)mos7840_port;
+
+			init_timer(&mos7840_port->led_timer2);
+			mos7840_port->led_timer2.function =
+						mos7840_led_flag_off;
+			mos7840_port->led_timer2.expires =
+					jiffies + msecs_to_jiffies(LED_OFF_MS);
+			mos7840_port->led_timer2.data =
+						(unsigned long)mos7840_port;
+
+			mos7840_port->led_flag = false;
+
+			/* Turn off LED */
+			mos7840_set_led_sync(serial->port[i],
+						MODEM_CONTROL_REGISTER, 0x0300);
+		}
 	}
 	dbg ("mos7840_startup: all ports configured...........");
 
@@ -2602,7 +2697,6 @@ static void mos7840_disconnect(struct usb_serial *serial)
 	int i;
 	unsigned long flags;
 	struct moschip_port *mos7840_port;
-	dbg("%s", " disconnect :entering..........");
 
 	if (!serial) {
 		dbg("%s", "Invalid Handler");
@@ -2624,9 +2718,6 @@ static void mos7840_disconnect(struct usb_serial *serial)
 			usb_kill_urb(mos7840_port->control_urb);
 		}
 	}
-
-	dbg("%s", "Thank u :: ");
-
 }
 
 /****************************************************************************
@@ -2638,7 +2729,6 @@ static void mos7840_release(struct usb_serial *serial)
 {
 	int i;
 	struct moschip_port *mos7840_port;
-	dbg("%s", " release :entering..........");
 
 	if (!serial) {
 		dbg("%s", "Invalid Handler");
@@ -2654,30 +2744,28 @@ static void mos7840_release(struct usb_serial *serial)
 		mos7840_port = mos7840_get_port_private(serial->port[i]);
 		dbg("mos7840_port %d = %p", i, mos7840_port);
 		if (mos7840_port) {
+			if (mos7840_port->has_led) {
+				/* Turn off LED */
+				mos7840_set_led_sync(mos7840_port->port,
+						MODEM_CONTROL_REGISTER, 0x0300);
+
+				del_timer_sync(&mos7840_port->led_timer1);
+				del_timer_sync(&mos7840_port->led_timer2);
+			}
 			kfree(mos7840_port->ctrl_buf);
 			kfree(mos7840_port->dr);
 			kfree(mos7840_port);
 		}
 	}
-
-	dbg("%s", "Thank u :: ");
-
 }
 
-static struct usb_driver io_driver = {
-	.name = "mos7840",
-	.probe = usb_serial_probe,
-	.disconnect = usb_serial_disconnect,
-	.id_table = moschip_id_table_combined,
-};
-
 static struct usb_serial_driver moschip7840_4port_device = {
 	.driver = {
 		   .owner = THIS_MODULE,
 		   .name = "mos7840",
 		   },
 	.description = DRIVER_DESC,
-	.id_table = moschip_port_id_table,
+	.id_table = id_table,
 	.num_ports = 4,
 	.open = mos7840_open,
 	.close = mos7840_close,
@@ -2707,7 +2795,7 @@ static struct usb_serial_driver * const serial_drivers[] = {
 	&moschip7840_4port_device, NULL
 };
 
-module_usb_serial_driver(io_driver, serial_drivers);
+module_usb_serial_driver(serial_drivers, id_table);
 
 MODULE_DESCRIPTION(DRIVER_DESC);
 MODULE_LICENSE("GPL");
diff --git a/drivers/usb/serial/moto_modem.c b/drivers/usb/serial/moto_modem.c
index 3ab6214b4bb..c5ff6c7795a 100644
--- a/drivers/usb/serial/moto_modem.c
+++ b/drivers/usb/serial/moto_modem.c
@@ -31,13 +31,6 @@ static const struct usb_device_id id_table[] = {
 };
 MODULE_DEVICE_TABLE(usb, id_table);
 
-static struct usb_driver moto_driver = {
-	.name =		"moto-modem",
-	.probe =	usb_serial_probe,
-	.disconnect =	usb_serial_disconnect,
-	.id_table =	id_table,
-};
-
 static struct usb_serial_driver moto_device = {
 	.driver = {
 		.owner =	THIS_MODULE,
@@ -51,5 +44,5 @@ static struct usb_serial_driver * const serial_drivers[] = {
 	&moto_device, NULL
 };
 
-module_usb_serial_driver(moto_driver, serial_drivers);
+module_usb_serial_driver(serial_drivers, id_table);
 MODULE_LICENSE("GPL");
diff --git a/drivers/usb/serial/navman.c b/drivers/usb/serial/navman.c
index 29ab6eb5b53..d95452cc076 100644
--- a/drivers/usb/serial/navman.c
+++ b/drivers/usb/serial/navman.c
@@ -30,13 +30,6 @@ static const struct usb_device_id id_table[] = {
 };
 MODULE_DEVICE_TABLE(usb, id_table);
 
-static struct usb_driver navman_driver = {
-	.name =		"navman",
-	.probe =	usb_serial_probe,
-	.disconnect =	usb_serial_disconnect,
-	.id_table =	id_table,
-};
-
 static void navman_read_int_callback(struct urb *urb)
 {
 	struct usb_serial_port *port = urb->context;
@@ -53,12 +46,12 @@ static void navman_read_int_callback(struct urb *urb)
 	case -ENOENT:
 	case -ESHUTDOWN:
 		/* this urb is terminated, clean up */
-		dbg("%s - urb shutting down with status: %d",
-		    __func__, status);
+		dev_dbg(&port->dev, "%s - urb shutting down with status: %d\n",
+			__func__, status);
 		return;
 	default:
-		dbg("%s - nonzero urb status received: %d",
-		    __func__, status);
+		dev_dbg(&port->dev, "%s - nonzero urb status received: %d\n",
+			__func__, status);
 		goto exit;
 	}
 
@@ -84,10 +77,9 @@ static int navman_open(struct tty_struct *tty, struct usb_serial_port *port)
 {
 	int result = 0;
 
-	dbg("%s - port %d", __func__, port->number);
-
 	if (port->interrupt_in_urb) {
-		dbg("%s - adding interrupt input for treo", __func__);
+		dev_dbg(&port->dev, "%s - adding interrupt input for treo\n",
+			__func__);
 		result = usb_submit_urb(port->interrupt_in_urb, GFP_KERNEL);
 		if (result)
 			dev_err(&port->dev,
@@ -99,16 +91,12 @@ static int navman_open(struct tty_struct *tty, struct usb_serial_port *port)
 
 static void navman_close(struct usb_serial_port *port)
 {
-	dbg("%s - port %d", __func__, port->number);
-
 	usb_kill_urb(port->interrupt_in_urb);
 }
 
 static int navman_write(struct tty_struct *tty, struct usb_serial_port *port,
 			const unsigned char *buf, int count)
 {
-	dbg("%s - port %d", __func__, port->number);
-
 	/*
 	 * This device can't write any data, only read from the device
 	 */
@@ -132,7 +120,7 @@ static struct usb_serial_driver * const serial_drivers[] = {
 	&navman_device, NULL
 };
 
-module_usb_serial_driver(navman_driver, serial_drivers);
+module_usb_serial_driver(serial_drivers, id_table);
 
 MODULE_LICENSE("GPL");
 
diff --git a/drivers/usb/serial/omninet.c b/drivers/usb/serial/omninet.c
index 88dc785bb29..6f3d7051c7f 100644
--- a/drivers/usb/serial/omninet.c
+++ b/drivers/usb/serial/omninet.c
@@ -54,17 +54,8 @@ static const struct usb_device_id id_table[] = {
 	{ USB_DEVICE(ZYXEL_VENDOR_ID, BT_IGNITIONPRO_ID) },
 	{ }						/* Terminating entry */
 };
-
 MODULE_DEVICE_TABLE(usb, id_table);
 
-static struct usb_driver omninet_driver = {
-	.name =		"omninet",
-	.probe =	usb_serial_probe,
-	.disconnect =	usb_serial_disconnect,
-	.id_table =	id_table,
-};
-
-
 static struct usb_serial_driver zyxel_omninet_device = {
 	.driver = {
 		.owner =	THIS_MODULE,
@@ -144,8 +135,6 @@ static int omninet_open(struct tty_struct *tty, struct usb_serial_port *port)
 	struct usb_serial_port	*wport;
 	int			result = 0;
 
-	dbg("%s - port %d", __func__, port->number);
-
 	wport = serial->port[1];
 	tty_port_tty_set(&wport->port, tty);
 
@@ -160,7 +149,6 @@ static int omninet_open(struct tty_struct *tty, struct usb_serial_port *port)
 
 static void omninet_close(struct usb_serial_port *port)
 {
-	dbg("%s - port %d", __func__, port->number);
 	usb_kill_urb(port->read_urb);
 }
 
@@ -178,8 +166,6 @@ static void omninet_read_bulk_callback(struct urb *urb)
 	int result;
 	int i;
 
-	dbg("%s - port %d", __func__, port->number);
-
 	if (status) {
 		dbg("%s - nonzero read bulk status received: %d",
 		    __func__, status);
@@ -225,8 +211,6 @@ static int omninet_write(struct tty_struct *tty, struct usb_serial_port *port,
 
 	int			result;
 
-	dbg("%s - port %d", __func__, port->number);
-
 	if (count == 0) {
 		dbg("%s - write request of 0 bytes", __func__);
 		return 0;
@@ -289,8 +273,6 @@ static void omninet_write_bulk_callback(struct urb *urb)
 	struct usb_serial_port 	*port   =  urb->context;
 	int status = urb->status;
 
-	dbg("%s - port %0x", __func__, port->number);
-
 	set_bit(0, &port->write_urbs_free);
 	if (status) {
 		dbg("%s - nonzero write bulk status received: %d",
@@ -306,8 +288,6 @@ static void omninet_disconnect(struct usb_serial *serial)
 {
 	struct usb_serial_port *wport = serial->port[1];
 
-	dbg("%s", __func__);
-
 	usb_kill_urb(wport->write_urb);
 }
 
@@ -316,12 +296,10 @@ static void omninet_release(struct usb_serial *serial)
 {
 	struct usb_serial_port *port = serial->port[0];
 
-	dbg("%s", __func__);
-
 	kfree(usb_get_serial_port_data(port));
 }
 
-module_usb_serial_driver(omninet_driver, serial_drivers);
+module_usb_serial_driver(serial_drivers, id_table);
 
 MODULE_AUTHOR(DRIVER_AUTHOR);
 MODULE_DESCRIPTION(DRIVER_DESC);
diff --git a/drivers/usb/serial/opticon.c b/drivers/usb/serial/opticon.c
index 82cc9d202b8..02cb1b7f655 100644
--- a/drivers/usb/serial/opticon.c
+++ b/drivers/usb/serial/opticon.c
@@ -70,8 +70,6 @@ static void opticon_read_bulk_callback(struct urb *urb)
 	int data_length;
 	unsigned long flags;
 
-	dbg("%s - port %d", __func__, port->number);
-
 	switch (status) {
 	case 0:
 		/* success */
@@ -179,8 +177,6 @@ static int opticon_open(struct tty_struct *tty, struct usb_serial_port *port)
 	unsigned long flags;
 	int result = 0;
 
-	dbg("%s - port %d", __func__, port->number);
-
 	spin_lock_irqsave(&priv->lock, flags);
 	priv->throttled = false;
 	priv->actually_throttled = false;
@@ -216,8 +212,6 @@ static void opticon_close(struct usb_serial_port *port)
 {
 	struct opticon_private *priv = usb_get_serial_data(port->serial);
 
-	dbg("%s - port %d", __func__, port->number);
-
 	/* shutdown our urbs */
 	usb_kill_urb(priv->bulk_read_urb);
 }
@@ -256,8 +250,6 @@ static int opticon_write(struct tty_struct *tty, struct usb_serial_port *port,
 	int status;
 	struct usb_ctrlrequest *dr;
 
-	dbg("%s - port %d", __func__, port->number);
-
 	spin_lock_irqsave(&priv->lock, flags);
 	if (priv->outstanding_urbs > URB_UPPER_LIMIT) {
 		spin_unlock_irqrestore(&priv->lock, flags);
@@ -338,8 +330,6 @@ static int opticon_write_room(struct tty_struct *tty)
 	struct opticon_private *priv = usb_get_serial_data(port->serial);
 	unsigned long flags;
 
-	dbg("%s - port %d", __func__, port->number);
-
 	/*
 	 * We really can take almost anything the user throws at us
 	 * but let's pick a nice big number to tell the tty
@@ -362,7 +352,6 @@ static void opticon_throttle(struct tty_struct *tty)
 	struct opticon_private *priv = usb_get_serial_data(port->serial);
 	unsigned long flags;
 
-	dbg("%s - port %d", __func__, port->number);
 	spin_lock_irqsave(&priv->lock, flags);
 	priv->throttled = true;
 	spin_unlock_irqrestore(&priv->lock, flags);
@@ -376,8 +365,6 @@ static void opticon_unthrottle(struct tty_struct *tty)
 	unsigned long flags;
 	int result, was_throttled;
 
-	dbg("%s - port %d", __func__, port->number);
-
 	spin_lock_irqsave(&priv->lock, flags);
 	priv->throttled = false;
 	was_throttled = priv->actually_throttled;
@@ -400,10 +387,6 @@ static int opticon_tiocmget(struct tty_struct *tty)
 	unsigned long flags;
 	int result = 0;
 
-	dbg("%s - port %d", __func__, port->number);
-	if (!usb_get_intfdata(port->serial->interface))
-		return -ENODEV;
-
 	spin_lock_irqsave(&priv->lock, flags);
 	if (priv->rts)
 		result |= TIOCM_RTS;
@@ -419,13 +402,13 @@ static int opticon_tiocmset(struct tty_struct *tty,
 			   unsigned int set, unsigned int clear)
 {
 	struct usb_serial_port *port = tty->driver_data;
+	struct usb_serial *serial = port->serial;
 	struct opticon_private *priv = usb_get_serial_data(port->serial);
 	unsigned long flags;
 	bool rts;
 	bool changed = false;
+	int ret;
 
-	if (!usb_get_intfdata(port->serial->interface))
-		return -ENODEV;
 	/* We only support RTS so we only handle that */
 	spin_lock_irqsave(&priv->lock, flags);
 
@@ -441,7 +424,14 @@ static int opticon_tiocmset(struct tty_struct *tty,
 		return 0;
 
 	/* Send the new RTS state to the connected device */
-	return send_control_msg(port, CONTROL_RTS, !rts);
+	mutex_lock(&serial->disc_mutex);
+	if (!serial->disconnected)
+		ret = send_control_msg(port, CONTROL_RTS, !rts);
+	else
+		ret = -ENODEV;
+	mutex_unlock(&serial->disc_mutex);
+
+	return ret;
 }
 
 static int get_serial_info(struct opticon_private *priv,
@@ -555,8 +545,6 @@ static void opticon_disconnect(struct usb_serial *serial)
 {
 	struct opticon_private *priv = usb_get_serial_data(serial);
 
-	dbg("%s", __func__);
-
 	usb_kill_urb(priv->bulk_read_urb);
 	usb_free_urb(priv->bulk_read_urb);
 }
@@ -565,24 +553,20 @@ static void opticon_release(struct usb_serial *serial)
 {
 	struct opticon_private *priv = usb_get_serial_data(serial);
 
-	dbg("%s", __func__);
-
 	kfree(priv->bulk_in_buffer);
 	kfree(priv);
 }
 
-static int opticon_suspend(struct usb_interface *intf, pm_message_t message)
+static int opticon_suspend(struct usb_serial *serial, pm_message_t message)
 {
-	struct usb_serial *serial = usb_get_intfdata(intf);
 	struct opticon_private *priv = usb_get_serial_data(serial);
 
 	usb_kill_urb(priv->bulk_read_urb);
 	return 0;
 }
 
-static int opticon_resume(struct usb_interface *intf)
+static int opticon_resume(struct usb_serial *serial)
 {
-	struct usb_serial *serial = usb_get_intfdata(intf);
 	struct opticon_private *priv = usb_get_serial_data(serial);
 	struct usb_serial_port *port = serial->port[0];
 	int result;
@@ -597,15 +581,6 @@ static int opticon_resume(struct usb_interface *intf)
 	return result;
 }
 
-static struct usb_driver opticon_driver = {
-	.name =		"opticon",
-	.probe =	usb_serial_probe,
-	.disconnect =	usb_serial_disconnect,
-	.suspend =	opticon_suspend,
-	.resume =	opticon_resume,
-	.id_table =	id_table,
-};
-
 static struct usb_serial_driver opticon_device = {
 	.driver = {
 		.owner =	THIS_MODULE,
@@ -625,13 +600,15 @@ static struct usb_serial_driver opticon_device = {
 	.ioctl =		opticon_ioctl,
 	.tiocmget =		opticon_tiocmget,
 	.tiocmset =		opticon_tiocmset,
+	.suspend = 		opticon_suspend,
+	.resume =		opticon_resume,
 };
 
 static struct usb_serial_driver * const serial_drivers[] = {
 	&opticon_device, NULL
 };
 
-module_usb_serial_driver(opticon_driver, serial_drivers);
+module_usb_serial_driver(serial_drivers, id_table);
 
 MODULE_DESCRIPTION(DRIVER_DESC);
 MODULE_LICENSE("GPL");
diff --git a/drivers/usb/serial/option.c b/drivers/usb/serial/option.c
index f4465ccddc3..1aae9028cd0 100644
--- a/drivers/usb/serial/option.c
+++ b/drivers/usb/serial/option.c
@@ -1220,18 +1220,6 @@ static const struct usb_device_id option_ids[] = {
 };
 MODULE_DEVICE_TABLE(usb, option_ids);
 
-static struct usb_driver option_driver = {
-	.name       = "option",
-	.probe      = usb_serial_probe,
-	.disconnect = usb_serial_disconnect,
-#ifdef CONFIG_PM
-	.suspend    = usb_serial_suspend,
-	.resume     = usb_serial_resume,
-	.supports_autosuspend =	1,
-#endif
-	.id_table   = option_ids,
-};
-
 /* The card has three separate interfaces, which the serial driver
  * recognizes separately, thus num_port=1.
  */
@@ -1300,7 +1288,7 @@ struct option_port_private {
 	unsigned long tx_start_time[N_OUT_URB];
 };
 
-module_usb_serial_driver(option_driver, serial_drivers);
+module_usb_serial_driver(serial_drivers, option_ids);
 
 static bool is_blacklisted(const u8 ifnum, enum option_blacklist_reason reason,
 			   const struct option_blacklist_info *blacklist)
@@ -1375,7 +1363,6 @@ static void option_instat_callback(struct urb *urb)
 	struct usb_serial_port *port =  urb->context;
 	struct option_port_private *portdata = usb_get_serial_port_data(port);
 
-	dbg("%s", __func__);
 	dbg("%s: urb %p port %p has data %p", __func__, urb, port, portdata);
 
 	if (status == 0) {
@@ -1413,7 +1400,7 @@ static void option_instat_callback(struct urb *urb)
 				req_pkt->bRequestType, req_pkt->bRequest);
 		}
 	} else
-		err("%s: error %d", __func__, status);
+		dev_err(&port->dev, "%s: error %d\n", __func__, status);
 
 	/* Resubmit urb so we continue receiving IRQ data */
 	if (status != -ESHUTDOWN && status != -ENOENT) {
@@ -1437,7 +1424,6 @@ static int option_send_setup(struct usb_serial_port *port)
 	struct option_port_private *portdata;
 	int ifNum = serial->interface->cur_altsetting->desc.bInterfaceNumber;
 	int val = 0;
-	dbg("%s", __func__);
 
 	if (is_blacklisted(ifNum, OPTION_BLACKLIST_SENDSETUP,
 			(struct option_blacklist_info *) intfdata->private)) {
diff --git a/drivers/usb/serial/oti6858.c b/drivers/usb/serial/oti6858.c
index 5fdc33c6a3c..5976b65ab6e 100644
--- a/drivers/usb/serial/oti6858.c
+++ b/drivers/usb/serial/oti6858.c
@@ -66,13 +66,6 @@ static const struct usb_device_id id_table[] = {
 
 MODULE_DEVICE_TABLE(usb, id_table);
 
-static struct usb_driver oti6858_driver = {
-	.name =		"oti6858",
-	.probe =	usb_serial_probe,
-	.disconnect =	usb_serial_disconnect,
-	.id_table =	id_table,
-};
-
 static bool debug;
 
 /* requests */
@@ -211,8 +204,6 @@ static void setup_line(struct work_struct *work)
 	unsigned long flags;
 	int result;
 
-	dbg("%s(port = %d)", __func__, port->number);
-
 	new_setup = kmalloc(OTI6858_CTRL_PKT_SIZE, GFP_KERNEL);
 	if (new_setup == NULL) {
 		dev_err(&port->dev, "%s(): out of memory!\n", __func__);
@@ -282,8 +273,6 @@ static void send_data(struct work_struct *work)
 	unsigned long flags;
 	u8 *allow;
 
-	dbg("%s(port = %d)", __func__, port->number);
-
 	spin_lock_irqsave(&priv->lock, flags);
 	if (priv->flags.write_urb_in_use) {
 		spin_unlock_irqrestore(&priv->lock, flags);
@@ -379,8 +368,6 @@ static int oti6858_startup(struct usb_serial *serial)
 static int oti6858_write(struct tty_struct *tty, struct usb_serial_port *port,
 			const unsigned char *buf, int count)
 {
-	dbg("%s(port = %d, count = %d)", __func__, port->number, count);
-
 	if (!count)
 		return count;
 
@@ -395,8 +382,6 @@ static int oti6858_write_room(struct tty_struct *tty)
 	int room = 0;
 	unsigned long flags;
 
-	dbg("%s(port = %d)", __func__, port->number);
-
 	spin_lock_irqsave(&port->lock, flags);
 	room = kfifo_avail(&port->write_fifo);
 	spin_unlock_irqrestore(&port->lock, flags);
@@ -410,8 +395,6 @@ static int oti6858_chars_in_buffer(struct tty_struct *tty)
 	int chars = 0;
 	unsigned long flags;
 
-	dbg("%s(port = %d)", __func__, port->number);
-
 	spin_lock_irqsave(&port->lock, flags);
 	chars = kfifo_len(&port->write_fifo);
 	spin_unlock_irqrestore(&port->lock, flags);
@@ -437,8 +420,6 @@ static void oti6858_set_termios(struct tty_struct *tty,
 	__le16 divisor;
 	int br;
 
-	dbg("%s(port = %d)", __func__, port->number);
-
 	if (!tty) {
 		dbg("%s(): no tty structures", __func__);
 		return;
@@ -545,8 +526,6 @@ static int oti6858_open(struct tty_struct *tty, struct usb_serial_port *port)
 	unsigned long flags;
 	int result;
 
-	dbg("%s(port = %d)", __func__, port->number);
-
 	usb_clear_halt(serial->dev, port->write_urb->pipe);
 	usb_clear_halt(serial->dev, port->read_urb->pipe);
 
@@ -602,8 +581,6 @@ static void oti6858_close(struct usb_serial_port *port)
 	struct oti6858_private *priv = usb_get_serial_port_data(port);
 	unsigned long flags;
 
-	dbg("%s(port = %d)", __func__, port->number);
-
 	spin_lock_irqsave(&port->lock, flags);
 	/* clear out any remaining data in the buffer */
 	kfifo_reset_out(&port->write_fifo);
@@ -633,9 +610,6 @@ static int oti6858_tiocmset(struct tty_struct *tty,
 	dbg("%s(port = %d, set = 0x%08x, clear = 0x%08x)",
 				__func__, port->number, set, clear);
 
-	if (!usb_get_intfdata(port->serial->interface))
-		return -ENODEV;
-
 	/* FIXME: check if this is correct (active high/low) */
 	spin_lock_irqsave(&priv->lock, flags);
 	control = priv->pending_setup.control;
@@ -663,11 +637,6 @@ static int oti6858_tiocmget(struct tty_struct *tty)
 	unsigned pin_state;
 	unsigned result = 0;
 
-	dbg("%s(port = %d)", __func__, port->number);
-
-	if (!usb_get_intfdata(port->serial->interface))
-		return -ENODEV;
-
 	spin_lock_irqsave(&priv->lock, flags);
 	pin_state = priv->status.pin_state & PIN_MASK;
 	spin_unlock_irqrestore(&priv->lock, flags);
@@ -750,8 +719,6 @@ static void oti6858_release(struct usb_serial *serial)
 {
 	int i;
 
-	dbg("%s()", __func__);
-
 	for (i = 0; i < serial->num_ports; ++i)
 		kfree(usb_get_serial_port_data(serial->port[i]));
 }
@@ -763,9 +730,6 @@ static void oti6858_read_int_callback(struct urb *urb)
 	int transient = 0, can_recv = 0, resubmit = 1;
 	int status = urb->status;
 
-	dbg("%s(port = %d, status = %d)",
-				__func__, port->number, status);
-
 	switch (status) {
 	case 0:
 		/* success */
@@ -882,9 +846,6 @@ static void oti6858_read_bulk_callback(struct urb *urb)
 	int status = urb->status;
 	int result;
 
-	dbg("%s(port = %d, status = %d)",
-				__func__, port->number, status);
-
 	spin_lock_irqsave(&priv->lock, flags);
 	priv->flags.read_urb_in_use = 0;
 	spin_unlock_irqrestore(&priv->lock, flags);
@@ -916,9 +877,6 @@ static void oti6858_write_bulk_callback(struct urb *urb)
 	int status = urb->status;
 	int result;
 
-	dbg("%s(port = %d, status = %d)",
-				__func__, port->number, status);
-
 	switch (status) {
 	case 0:
 		/* success */
@@ -958,7 +916,7 @@ static void oti6858_write_bulk_callback(struct urb *urb)
 	}
 }
 
-module_usb_serial_driver(oti6858_driver, serial_drivers);
+module_usb_serial_driver(serial_drivers, id_table);
 
 MODULE_DESCRIPTION(OTI6858_DESCRIPTION);
 MODULE_AUTHOR(OTI6858_AUTHOR);
diff --git a/drivers/usb/serial/pl2303.c b/drivers/usb/serial/pl2303.c
index a1a9062954c..13b8dd6481f 100644
--- a/drivers/usb/serial/pl2303.c
+++ b/drivers/usb/serial/pl2303.c
@@ -38,8 +38,6 @@
 
 static bool debug;
 
-#define PL2303_CLOSING_WAIT	(30*HZ)
-
 static const struct usb_device_id id_table[] = {
 	{ USB_DEVICE(PL2303_VENDOR_ID, PL2303_PRODUCT_ID) },
 	{ USB_DEVICE(PL2303_VENDOR_ID, PL2303_PRODUCT_ID_RSAQ2) },
@@ -97,16 +95,6 @@ static const struct usb_device_id id_table[] = {
 
 MODULE_DEVICE_TABLE(usb, id_table);
 
-static struct usb_driver pl2303_driver = {
-	.name =		"pl2303",
-	.probe =	usb_serial_probe,
-	.disconnect =	usb_serial_disconnect,
-	.id_table =	id_table,
-	.suspend =      usb_serial_suspend,
-	.resume =       usb_serial_resume,
-	.supports_autosuspend =	1,
-};
-
 #define SET_LINE_REQUEST_TYPE		0x21
 #define SET_LINE_REQUEST		0x20
 
@@ -161,8 +149,9 @@ static int pl2303_vendor_read(__u16 value, __u16 index,
 	int res = usb_control_msg(serial->dev, usb_rcvctrlpipe(serial->dev, 0),
 			VENDOR_READ_REQUEST, VENDOR_READ_REQUEST_TYPE,
 			value, index, buf, 1, 100);
-	dbg("0x%x:0x%x:0x%x:0x%x  %d - %x", VENDOR_READ_REQUEST_TYPE,
-			VENDOR_READ_REQUEST, value, index, res, buf[0]);
+	dev_dbg(&serial->dev->dev, "0x%x:0x%x:0x%x:0x%x  %d - %x\n",
+		VENDOR_READ_REQUEST_TYPE, VENDOR_READ_REQUEST, value, index,
+		res, buf[0]);
 	return res;
 }
 
@@ -172,8 +161,9 @@ static int pl2303_vendor_write(__u16 value, __u16 index,
 	int res = usb_control_msg(serial->dev, usb_sndctrlpipe(serial->dev, 0),
 			VENDOR_WRITE_REQUEST, VENDOR_WRITE_REQUEST_TYPE,
 			value, index, NULL, 0, 100);
-	dbg("0x%x:0x%x:0x%x:0x%x  %d", VENDOR_WRITE_REQUEST_TYPE,
-			VENDOR_WRITE_REQUEST, value, index, res);
+	dev_dbg(&serial->dev->dev, "0x%x:0x%x:0x%x:0x%x  %d\n",
+		VENDOR_WRITE_REQUEST_TYPE, VENDOR_WRITE_REQUEST, value, index,
+		res);
 	return res;
 }
 
@@ -196,7 +186,7 @@ static int pl2303_startup(struct usb_serial *serial)
 		type = type_1;
 	else if (serial->dev->descriptor.bDeviceClass == 0xFF)
 		type = type_1;
-	dbg("device type: %d", type);
+	dev_dbg(&serial->interface->dev, "device type: %d\n", type);
 
 	for (i = 0; i < serial->num_ports; ++i) {
 		priv = kzalloc(sizeof(struct pl2303_private), GFP_KERNEL);
@@ -243,7 +233,8 @@ static int set_control_lines(struct usb_device *dev, u8 value)
 	retval = usb_control_msg(dev, usb_sndctrlpipe(dev, 0),
 				 SET_CONTROL_REQUEST, SET_CONTROL_REQUEST_TYPE,
 				 value, 0, NULL, 0, 100);
-	dbg("%s - value = %d, retval = %d", __func__, value, retval);
+	dev_dbg(&dev->dev, "%s - value = %d, retval = %d\n", __func__,
+		value, retval);
 	return retval;
 }
 
@@ -265,8 +256,6 @@ static void pl2303_set_termios(struct tty_struct *tty,
 	int baud_floor, baud_ceil;
 	int k;
 
-	dbg("%s -  port %d", __func__, port->number);
-
 	/* The PL2303 is reported to lose bytes if you change
 	   serial settings even to the same values as before. Thus
 	   we actually need to filter in this specific case */
@@ -287,7 +276,7 @@ static void pl2303_set_termios(struct tty_struct *tty,
 	i = usb_control_msg(serial->dev, usb_rcvctrlpipe(serial->dev, 0),
 			    GET_LINE_REQUEST, GET_LINE_REQUEST_TYPE,
 			    0, 0, buf, 7, 100);
-	dbg("0xa1:0x21:0:0  %d - %x %x %x %x %x %x %x", i,
+	dev_dbg(&port->dev, "0xa1:0x21:0:0  %d - %x %x %x %x %x %x %x\n", i,
 	    buf[0], buf[1], buf[2], buf[3], buf[4], buf[5], buf[6]);
 
 	if (cflag & CSIZE) {
@@ -306,7 +295,7 @@ static void pl2303_set_termios(struct tty_struct *tty,
 			buf[6] = 8;
 			break;
 		}
-		dbg("%s - data bits = %d", __func__, buf[6]);
+		dev_dbg(&port->dev, "data bits = %d\n", buf[6]);
 	}
 
 	/* For reference buf[0]:buf[3] baud rate value */
@@ -315,7 +304,7 @@ static void pl2303_set_termios(struct tty_struct *tty,
 	 *          9600 baud (at least my PL2303X always does)
 	 */
 	baud = tty_get_baud_rate(tty);
-	dbg("%s - baud requested = %d", __func__, baud);
+	dev_dbg(&port->dev, "baud requested = %d\n", baud);
 	if (baud) {
 		/* Set baudrate to nearest supported value */
 		for (k=0; k<ARRAY_SIZE(baud_sup); k++) {
@@ -341,7 +330,7 @@ static void pl2303_set_termios(struct tty_struct *tty,
 			else if (baud > 6000000)
 				baud = 6000000;
 		}
-		dbg("%s - baud set = %d", __func__, baud);
+		dev_dbg(&port->dev, "baud set = %d\n", baud);
 		if (baud <= 115200) {
 			buf[0] = baud & 0xff;
 			buf[1] = (baud >> 8) & 0xff;
@@ -372,14 +361,14 @@ static void pl2303_set_termios(struct tty_struct *tty,
 		 */
 		if ((cflag & CSIZE) == CS5) {
 			buf[4] = 1;
-			dbg("%s - stop bits = 1.5", __func__);
+			dev_dbg(&port->dev, "stop bits = 1.5\n");
 		} else {
 			buf[4] = 2;
-			dbg("%s - stop bits = 2", __func__);
+			dev_dbg(&port->dev, "stop bits = 2\n");
 		}
 	} else {
 		buf[4] = 0;
-		dbg("%s - stop bits = 1", __func__);
+		dev_dbg(&port->dev, "stop bits = 1\n");
 	}
 
 	if (cflag & PARENB) {
@@ -391,29 +380,29 @@ static void pl2303_set_termios(struct tty_struct *tty,
 		if (cflag & PARODD) {
 			if (cflag & CMSPAR) {
 				buf[5] = 3;
-				dbg("%s - parity = mark", __func__);
+				dev_dbg(&port->dev, "parity = mark\n");
 			} else {
 				buf[5] = 1;
-				dbg("%s - parity = odd", __func__);
+				dev_dbg(&port->dev, "parity = odd\n");
 			}
 		} else {
 			if (cflag & CMSPAR) {
 				buf[5] = 4;
-				dbg("%s - parity = space", __func__);
+				dev_dbg(&port->dev, "parity = space\n");
 			} else {
 				buf[5] = 2;
-				dbg("%s - parity = even", __func__);
+				dev_dbg(&port->dev, "parity = even\n");
 			}
 		}
 	} else {
 		buf[5] = 0;
-		dbg("%s - parity = none", __func__);
+		dev_dbg(&port->dev, "parity = none\n");
 	}
 
 	i = usb_control_msg(serial->dev, usb_sndctrlpipe(serial->dev, 0),
 			    SET_LINE_REQUEST, SET_LINE_REQUEST_TYPE,
 			    0, 0, buf, 7, 100);
-	dbg("0x21:0x20:0:0  %d", i);
+	dev_dbg(&port->dev, "0x21:0x20:0:0  %d\n", i);
 
 	/* change control lines if we are switching to or from B0 */
 	spin_lock_irqsave(&priv->lock, flags);
@@ -435,7 +424,7 @@ static void pl2303_set_termios(struct tty_struct *tty,
 	i = usb_control_msg(serial->dev, usb_rcvctrlpipe(serial->dev, 0),
 			    GET_LINE_REQUEST, GET_LINE_REQUEST_TYPE,
 			    0, 0, buf, 7, 100);
-	dbg("0xa1:0x21:0:0  %d - %x %x %x %x %x %x %x", i,
+	dev_dbg(&port->dev, "0xa1:0x21:0:0  %d - %x %x %x %x %x %x %x\n", i,
 	     buf[0], buf[1], buf[2], buf[3], buf[4], buf[5], buf[6]);
 
 	if (cflag & CRTSCTS) {
@@ -473,8 +462,6 @@ static void pl2303_dtr_rts(struct usb_serial_port *port, int on)
 
 static void pl2303_close(struct usb_serial_port *port)
 {
-	dbg("%s - port %d", __func__, port->number);
-
 	usb_serial_generic_close(port);
 	usb_kill_urb(port->interrupt_in_urb);
 }
@@ -486,8 +473,6 @@ static int pl2303_open(struct tty_struct *tty, struct usb_serial_port *port)
 	struct pl2303_private *priv = usb_get_serial_port_data(port);
 	int result;
 
-	dbg("%s -  port %d", __func__, port->number);
-
 	if (priv->type != HX) {
 		usb_clear_halt(serial->dev, port->write_urb->pipe);
 		usb_clear_halt(serial->dev, port->read_urb->pipe);
@@ -501,7 +486,6 @@ static int pl2303_open(struct tty_struct *tty, struct usb_serial_port *port)
 	if (tty)
 		pl2303_set_termios(tty, port, &tmp_termios);
 
-	dbg("%s - submitting interrupt urb", __func__);
 	result = usb_submit_urb(port->interrupt_in_urb, GFP_KERNEL);
 	if (result) {
 		dev_err(&port->dev, "%s - failed submitting interrupt urb,"
@@ -523,12 +507,11 @@ static int pl2303_tiocmset(struct tty_struct *tty,
 			   unsigned int set, unsigned int clear)
 {
 	struct usb_serial_port *port = tty->driver_data;
+	struct usb_serial *serial = port->serial;
 	struct pl2303_private *priv = usb_get_serial_port_data(port);
 	unsigned long flags;
 	u8 control;
-
-	if (!usb_get_intfdata(port->serial->interface))
-		return -ENODEV;
+	int ret;
 
 	spin_lock_irqsave(&priv->lock, flags);
 	if (set & TIOCM_RTS)
@@ -542,7 +525,14 @@ static int pl2303_tiocmset(struct tty_struct *tty,
 	control = priv->line_control;
 	spin_unlock_irqrestore(&priv->lock, flags);
 
-	return set_control_lines(port->serial->dev, control);
+	mutex_lock(&serial->disc_mutex);
+	if (!serial->disconnected)
+		ret = set_control_lines(serial->dev, control);
+	else
+		ret = -ENODEV;
+	mutex_unlock(&serial->disc_mutex);
+
+	return ret;
 }
 
 static int pl2303_tiocmget(struct tty_struct *tty)
@@ -554,11 +544,6 @@ static int pl2303_tiocmget(struct tty_struct *tty)
 	unsigned int status;
 	unsigned int result;
 
-	dbg("%s (%d)", __func__, port->number);
-
-	if (!usb_get_intfdata(port->serial->interface))
-		return -ENODEV;
-
 	spin_lock_irqsave(&priv->lock, flags);
 	mcr = priv->line_control;
 	status = priv->line_status;
@@ -571,7 +556,7 @@ static int pl2303_tiocmget(struct tty_struct *tty)
 		  | ((status & UART_RING)	? TIOCM_RI  : 0)
 		  | ((status & UART_DCD)	? TIOCM_CD  : 0);
 
-	dbg("%s - result = %x", __func__, result);
+	dev_dbg(&port->dev, "%s - result = %x\n", __func__, result);
 
 	return result;
 }
@@ -625,7 +610,8 @@ static int pl2303_ioctl(struct tty_struct *tty,
 {
 	struct serial_struct ser;
 	struct usb_serial_port *port = tty->driver_data;
-	dbg("%s (%d) cmd = 0x%04x", __func__, port->number, cmd);
+
+	dev_dbg(&port->dev, "%s cmd = 0x%04x\n", __func__, cmd);
 
 	switch (cmd) {
 	case TIOCGSERIAL:
@@ -641,10 +627,10 @@ static int pl2303_ioctl(struct tty_struct *tty,
 		return 0;
 
 	case TIOCMIWAIT:
-		dbg("%s (%d) TIOCMIWAIT", __func__,  port->number);
+		dev_dbg(&port->dev, "%s TIOCMIWAIT\n", __func__);
 		return wait_modem_info(port, arg);
 	default:
-		dbg("%s not supported = 0x%04x", __func__, cmd);
+		dev_dbg(&port->dev, "%s not supported = 0x%04x\n", __func__, cmd);
 		break;
 	}
 	return -ENOIOCTLCMD;
@@ -657,20 +643,18 @@ static void pl2303_break_ctl(struct tty_struct *tty, int break_state)
 	u16 state;
 	int result;
 
-	dbg("%s - port %d", __func__, port->number);
-
 	if (break_state == 0)
 		state = BREAK_OFF;
 	else
 		state = BREAK_ON;
-	dbg("%s - turning break %s", __func__,
+	dev_dbg(&port->dev, "%s - turning break %s\n", __func__,
 			state == BREAK_OFF ? "off" : "on");
 
 	result = usb_control_msg(serial->dev, usb_sndctrlpipe(serial->dev, 0),
 				 BREAK_REQUEST, BREAK_REQUEST_TYPE, state,
 				 0, NULL, 0, 100);
 	if (result)
-		dbg("%s - error sending break = %d", __func__, result);
+		dev_err(&port->dev, "error sending break = %d\n", result);
 }
 
 static void pl2303_release(struct usb_serial *serial)
@@ -678,8 +662,6 @@ static void pl2303_release(struct usb_serial *serial)
 	int i;
 	struct pl2303_private *priv;
 
-	dbg("%s", __func__);
-
 	for (i = 0; i < serial->num_ports; ++i) {
 		priv = usb_get_serial_port_data(serial->port[i]);
 		kfree(priv);
@@ -742,8 +724,6 @@ static void pl2303_read_int_callback(struct urb *urb)
 	int status = urb->status;
 	int retval;
 
-	dbg("%s (%d)", __func__, port->number);
-
 	switch (status) {
 	case 0:
 		/* success */
@@ -752,12 +732,12 @@ static void pl2303_read_int_callback(struct urb *urb)
 	case -ENOENT:
 	case -ESHUTDOWN:
 		/* this urb is terminated, clean up */
-		dbg("%s - urb shutting down with status: %d", __func__,
-		    status);
+		dev_dbg(&port->dev, "%s - urb shutting down with status: %d\n",
+			__func__, status);
 		return;
 	default:
-		dbg("%s - nonzero urb status received: %d", __func__,
-		    status);
+		dev_dbg(&port->dev, "%s - nonzero urb status received: %d\n",
+			__func__, status);
 		goto exit;
 	}
 
@@ -769,7 +749,7 @@ static void pl2303_read_int_callback(struct urb *urb)
 exit:
 	retval = usb_submit_urb(urb, GFP_ATOMIC);
 	if (retval)
-		dev_err(&urb->dev->dev,
+		dev_err(&port->dev,
 			"%s - usb_submit_urb failed with result %d\n",
 			__func__, retval);
 }
@@ -807,7 +787,7 @@ static void pl2303_process_read_urb(struct urb *urb)
 		tty_flag = TTY_PARITY;
 	else if (line_status & UART_FRAME_ERROR)
 		tty_flag = TTY_FRAME;
-	dbg("%s - tty_flag = %d", __func__, tty_flag);
+	dev_dbg(&port->dev, "%s - tty_flag = %d\n", __func__, tty_flag);
 
 	/* overrun is special, not associated with a char */
 	if (line_status & UART_OVERRUN_ERROR)
@@ -855,7 +835,7 @@ static struct usb_serial_driver * const serial_drivers[] = {
 	&pl2303_device, NULL
 };
 
-module_usb_serial_driver(pl2303_driver, serial_drivers);
+module_usb_serial_driver(serial_drivers, id_table);
 
 MODULE_DESCRIPTION(DRIVER_DESC);
 MODULE_LICENSE("GPL");
diff --git a/drivers/usb/serial/qcaux.c b/drivers/usb/serial/qcaux.c
index 966245680f5..a4edc7ee9c8 100644
--- a/drivers/usb/serial/qcaux.c
+++ b/drivers/usb/serial/qcaux.c
@@ -77,13 +77,6 @@ static struct usb_device_id id_table[] = {
 };
 MODULE_DEVICE_TABLE(usb, id_table);
 
-static struct usb_driver qcaux_driver = {
-	.name =		"qcaux",
-	.probe =	usb_serial_probe,
-	.disconnect =	usb_serial_disconnect,
-	.id_table =	id_table,
-};
-
 static struct usb_serial_driver qcaux_device = {
 	.driver = {
 		.owner =	THIS_MODULE,
@@ -97,5 +90,5 @@ static struct usb_serial_driver * const serial_drivers[] = {
 	&qcaux_device, NULL
 };
 
-module_usb_serial_driver(qcaux_driver, serial_drivers);
+module_usb_serial_driver(serial_drivers, id_table);
 MODULE_LICENSE("GPL");
diff --git a/drivers/usb/serial/qcserial.c b/drivers/usb/serial/qcserial.c
index 0206b10c9e6..0d5fe59ebb9 100644
--- a/drivers/usb/serial/qcserial.c
+++ b/drivers/usb/serial/qcserial.c
@@ -112,32 +112,22 @@ static const struct usb_device_id id_table[] = {
 };
 MODULE_DEVICE_TABLE(usb, id_table);
 
-static struct usb_driver qcdriver = {
-	.name			= "qcserial",
-	.probe			= usb_serial_probe,
-	.disconnect		= usb_serial_disconnect,
-	.id_table		= id_table,
-	.suspend		= usb_serial_suspend,
-	.resume			= usb_serial_resume,
-	.supports_autosuspend	= true,
-};
-
 static int qcprobe(struct usb_serial *serial, const struct usb_device_id *id)
 {
 	struct usb_wwan_intf_private *data;
 	struct usb_host_interface *intf = serial->interface->cur_altsetting;
+	struct device *dev = &serial->dev->dev;
 	int retval = -ENODEV;
 	__u8 nintf;
 	__u8 ifnum;
 	bool is_gobi1k = id->driver_info ? true : false;
 
-	dbg("%s", __func__);
-	dbg("Is Gobi 1000 = %d", is_gobi1k);
+	dev_dbg(dev, "Is Gobi 1000 = %d\n", is_gobi1k);
 
 	nintf = serial->dev->actconfig->desc.bNumInterfaces;
-	dbg("Num Interfaces = %d", nintf);
+	dev_dbg(dev, "Num Interfaces = %d\n", nintf);
 	ifnum = intf->desc.bInterfaceNumber;
-	dbg("This Interface = %d", ifnum);
+	dev_dbg(dev, "This Interface = %d\n", ifnum);
 
 	data = kzalloc(sizeof(struct usb_wwan_intf_private),
 					 GFP_KERNEL);
@@ -158,7 +148,7 @@ static int qcprobe(struct usb_serial *serial, const struct usb_device_id *id)
 		if (intf->desc.bNumEndpoints == 2 &&
 		    usb_endpoint_is_bulk_in(&intf->endpoint[0].desc) &&
 		    usb_endpoint_is_bulk_out(&intf->endpoint[1].desc)) {
-			dbg("QDL port found");
+			dev_dbg(dev, "QDL port found\n");
 
 			if (serial->interface->num_altsetting == 1) {
 				retval = 0; /* Success */
@@ -167,7 +157,7 @@ static int qcprobe(struct usb_serial *serial, const struct usb_device_id *id)
 
 			retval = usb_set_interface(serial->dev, ifnum, 1);
 			if (retval < 0) {
-				dev_err(&serial->dev->dev,
+				dev_err(dev,
 					"Could not set interface, error %d\n",
 					retval);
 				retval = -ENODEV;
@@ -196,20 +186,20 @@ static int qcprobe(struct usb_serial *serial, const struct usb_device_id *id)
 		 */
 
 		if (ifnum == 1 && !is_gobi1k) {
-			dbg("Gobi 2K+ DM/DIAG interface found");
+			dev_dbg(dev, "Gobi 2K+ DM/DIAG interface found\n");
 			retval = usb_set_interface(serial->dev, ifnum, 0);
 			if (retval < 0) {
-				dev_err(&serial->dev->dev,
+				dev_err(dev,
 					"Could not set interface, error %d\n",
 					retval);
 				retval = -ENODEV;
 				kfree(data);
 			}
 		} else if (ifnum == 2) {
-			dbg("Modem port found");
+			dev_dbg(dev, "Modem port found\n");
 			retval = usb_set_interface(serial->dev, ifnum, 0);
 			if (retval < 0) {
-				dev_err(&serial->dev->dev,
+				dev_err(dev,
 					"Could not set interface, error %d\n",
 					retval);
 				retval = -ENODEV;
@@ -221,10 +211,10 @@ static int qcprobe(struct usb_serial *serial, const struct usb_device_id *id)
 			 * # echo "\$GPS_START" > /dev/ttyUSBx
 			 * # echo "\$GPS_STOP"  > /dev/ttyUSBx
 			 */
-			dbg("Gobi 2K+ NMEA GPS interface found");
+			dev_dbg(dev, "Gobi 2K+ NMEA GPS interface found\n");
 			retval = usb_set_interface(serial->dev, ifnum, 0);
 			if (retval < 0) {
-				dev_err(&serial->dev->dev,
+				dev_err(dev,
 					"Could not set interface, error %d\n",
 					retval);
 				retval = -ENODEV;
@@ -234,8 +224,7 @@ static int qcprobe(struct usb_serial *serial, const struct usb_device_id *id)
 		break;
 
 	default:
-		dev_err(&serial->dev->dev,
-			"unknown number of interfaces: %d\n", nintf);
+		dev_err(dev, "unknown number of interfaces: %d\n", nintf);
 		kfree(data);
 		retval = -ENODEV;
 	}
@@ -250,8 +239,6 @@ static void qc_release(struct usb_serial *serial)
 {
 	struct usb_wwan_intf_private *priv = usb_get_serial_data(serial);
 
-	dbg("%s", __func__);
-
 	/* Call usb_wwan release & free the private data allocated in qcprobe */
 	usb_wwan_release(serial);
 	usb_set_serial_data(serial, NULL);
@@ -285,7 +272,7 @@ static struct usb_serial_driver * const serial_drivers[] = {
 	&qcdevice, NULL
 };
 
-module_usb_serial_driver(qcdriver, serial_drivers);
+module_usb_serial_driver(serial_drivers, id_table);
 
 MODULE_AUTHOR(DRIVER_AUTHOR);
 MODULE_DESCRIPTION(DRIVER_DESC);
diff --git a/drivers/usb/serial/quatech2.c b/drivers/usb/serial/quatech2.c
new file mode 100644
index 00000000000..8dd88ebe986
--- /dev/null
+++ b/drivers/usb/serial/quatech2.c
@@ -0,0 +1,1155 @@
+/*
+ * usb-serial driver for Quatech USB 2 devices
+ *
+ * Copyright (C) 2012 Bill Pemberton (wfp5p@virginia.edu)
+ *
+ * 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.
+ *
+ *
+ *  These devices all have only 1 bulk in and 1 bulk out that is shared
+ *  for all serial ports.
+ *
+ */
+
+#include <asm/unaligned.h>
+#include <linux/errno.h>
+#include <linux/init.h>
+#include <linux/slab.h>
+#include <linux/tty.h>
+#include <linux/tty_driver.h>
+#include <linux/tty_flip.h>
+#include <linux/module.h>
+#include <linux/serial.h>
+#include <linux/usb.h>
+#include <linux/usb/serial.h>
+#include <linux/serial_reg.h>
+#include <linux/uaccess.h>
+
+static bool debug;
+
+/* default urb timeout for usb operations */
+#define QT2_USB_TIMEOUT USB_CTRL_SET_TIMEOUT
+
+#define QT_OPEN_CLOSE_CHANNEL       0xca
+#define QT_SET_GET_DEVICE           0xc2
+#define QT_SET_GET_REGISTER         0xc0
+#define QT_GET_SET_PREBUF_TRIG_LVL  0xcc
+#define QT_SET_ATF                  0xcd
+#define QT_TRANSFER_IN              0xc0
+#define QT_HW_FLOW_CONTROL_MASK     0xc5
+#define QT_SW_FLOW_CONTROL_MASK     0xc6
+#define QT2_BREAK_CONTROL	    0xc8
+#define QT2_GET_SET_UART            0xc1
+#define QT2_FLUSH_DEVICE	    0xc4
+#define QT2_GET_SET_QMCR            0xe1
+#define QT2_QMCR_RS232              0x40
+#define QT2_QMCR_RS422              0x10
+
+#define  SERIAL_CRTSCTS ((UART_MCR_RTS << 8) | UART_MSR_CTS)
+
+#define  SERIAL_EVEN_PARITY         (UART_LCR_PARITY | UART_LCR_EPAR)
+
+/* status bytes for the device */
+#define QT2_CONTROL_BYTE    0x1b
+#define QT2_LINE_STATUS     0x00  /* following 1 byte is line status */
+#define QT2_MODEM_STATUS    0x01  /* following 1 byte is modem status */
+#define QT2_XMIT_HOLD       0x02  /* following 2 bytes are ?? */
+#define QT2_CHANGE_PORT     0x03  /* following 1 byte is port to change to */
+#define QT2_REC_FLUSH       0x04  /* no following info */
+#define QT2_XMIT_FLUSH      0x05  /* no following info */
+#define QT2_CONTROL_ESCAPE  0xff  /* pass through previous 2 control bytes */
+
+#define  MAX_BAUD_RATE              921600
+#define  DEFAULT_BAUD_RATE          9600
+
+#define QT2_WRITE_BUFFER_SIZE   512  /* size of write buffer */
+#define QT2_WRITE_CONTROL_SIZE  5    /* control bytes used for a write */
+
+/* Version Information */
+#define DRIVER_VERSION "v0.1"
+#define DRIVER_DESC "Quatech 2nd gen USB to Serial Driver"
+
+#define	USB_VENDOR_ID_QUATECH	0x061d
+#define QUATECH_SSU2_100	0xC120	/* RS232 single port */
+#define QUATECH_DSU2_100	0xC140	/* RS232 dual port */
+#define QUATECH_DSU2_400	0xC150	/* RS232/422/485 dual port */
+#define QUATECH_QSU2_100	0xC160	/* RS232 four port */
+#define QUATECH_QSU2_400	0xC170	/* RS232/422/485 four port */
+#define QUATECH_ESU2_100	0xC1A0	/* RS232 eight port */
+#define QUATECH_ESU2_400	0xC180	/* RS232/422/485 eight port */
+
+struct qt2_device_detail {
+	int product_id;
+	int num_ports;
+};
+
+#define QT_DETAILS(prod, ports)	\
+	.product_id = (prod),   \
+	.num_ports = (ports)
+
+static const struct qt2_device_detail qt2_device_details[] = {
+	{QT_DETAILS(QUATECH_SSU2_100, 1)},
+	{QT_DETAILS(QUATECH_DSU2_400, 2)},
+	{QT_DETAILS(QUATECH_DSU2_100, 2)},
+	{QT_DETAILS(QUATECH_QSU2_400, 4)},
+	{QT_DETAILS(QUATECH_QSU2_100, 4)},
+	{QT_DETAILS(QUATECH_ESU2_400, 8)},
+	{QT_DETAILS(QUATECH_ESU2_100, 8)},
+	{QT_DETAILS(0, 0)}	/* Terminating entry */
+};
+
+static const struct usb_device_id id_table[] = {
+	{USB_DEVICE(USB_VENDOR_ID_QUATECH, QUATECH_SSU2_100)},
+	{USB_DEVICE(USB_VENDOR_ID_QUATECH, QUATECH_DSU2_100)},
+	{USB_DEVICE(USB_VENDOR_ID_QUATECH, QUATECH_DSU2_400)},
+	{USB_DEVICE(USB_VENDOR_ID_QUATECH, QUATECH_QSU2_100)},
+	{USB_DEVICE(USB_VENDOR_ID_QUATECH, QUATECH_QSU2_400)},
+	{USB_DEVICE(USB_VENDOR_ID_QUATECH, QUATECH_ESU2_100)},
+	{USB_DEVICE(USB_VENDOR_ID_QUATECH, QUATECH_ESU2_400)},
+	{}			/* Terminating entry */
+};
+MODULE_DEVICE_TABLE(usb, id_table);
+
+struct qt2_serial_private {
+	unsigned char current_port;  /* current port for incoming data */
+
+	struct urb	*read_urb;   /* shared among all ports */
+	char		read_buffer[512];
+};
+
+struct qt2_port_private {
+	bool is_open;
+	u8   device_port;
+
+	spinlock_t urb_lock;
+	bool       urb_in_use;
+	struct urb *write_urb;
+	char       write_buffer[QT2_WRITE_BUFFER_SIZE];
+
+	spinlock_t  lock;
+	u8          shadowLSR;
+	u8          shadowMSR;
+
+	wait_queue_head_t   delta_msr_wait; /* Used for TIOCMIWAIT */
+	struct async_icount icount;
+
+	struct usb_serial_port *port;
+};
+
+static void qt2_update_lsr(struct usb_serial_port *port, unsigned char *ch);
+static void qt2_update_msr(struct usb_serial_port *port, unsigned char *ch);
+static void qt2_write_bulk_callback(struct urb *urb);
+static void qt2_read_bulk_callback(struct urb *urb);
+
+static void qt2_release(struct usb_serial *serial)
+{
+	int i;
+
+	kfree(usb_get_serial_data(serial));
+
+	for (i = 0; i < serial->num_ports; i++)
+		kfree(usb_get_serial_port_data(serial->port[i]));
+}
+
+static inline int calc_baud_divisor(int baudrate)
+{
+	int divisor, rem;
+
+	divisor = MAX_BAUD_RATE / baudrate;
+	rem = MAX_BAUD_RATE % baudrate;
+	/* Round to nearest divisor */
+	if (((rem * 2) >= baudrate) && (baudrate != 110))
+		divisor++;
+
+	return divisor;
+}
+
+static inline int qt2_set_port_config(struct usb_device *dev,
+				      unsigned char port_number,
+				      u16 baudrate, u16 lcr)
+{
+	int divisor = calc_baud_divisor(baudrate);
+	u16 index = ((u16) (lcr << 8) | (u16) (port_number));
+
+	return usb_control_msg(dev, usb_sndctrlpipe(dev, 0),
+			       QT2_GET_SET_UART, 0x40,
+			       divisor, index, NULL, 0, QT2_USB_TIMEOUT);
+}
+
+static inline int qt2_control_msg(struct usb_device *dev,
+				  u8 request, u16 data, u16 index)
+{
+	return usb_control_msg(dev, usb_sndctrlpipe(dev, 0),
+			       request, 0x40, data, index,
+			       NULL, 0, QT2_USB_TIMEOUT);
+}
+
+static inline int qt2_setdevice(struct usb_device *dev, u8 *data)
+{
+	u16 x = ((u16) (data[1] << 8) | (u16) (data[0]));
+
+	return qt2_control_msg(dev, QT_SET_GET_DEVICE, x, 0);
+}
+
+
+static inline int qt2_getdevice(struct usb_device *dev, u8 *data)
+{
+	return usb_control_msg(dev, usb_rcvctrlpipe(dev, 0),
+			       QT_SET_GET_DEVICE, 0xc0, 0, 0,
+			       data, 3, QT2_USB_TIMEOUT);
+}
+
+static inline int qt2_getregister(struct usb_device *dev,
+				  u8 uart,
+				  u8 reg,
+				  u8 *data)
+{
+	return usb_control_msg(dev, usb_rcvctrlpipe(dev, 0),
+			       QT_SET_GET_REGISTER, 0xc0, reg,
+			       uart, data, sizeof(*data), QT2_USB_TIMEOUT);
+
+}
+
+static inline int qt2_setregister(struct usb_device *dev,
+				  u8 uart, u8 reg, u16 data)
+{
+	u16 value = (data << 8) | reg;
+
+	return usb_control_msg(dev, usb_sndctrlpipe(dev, 0),
+			       QT_SET_GET_REGISTER, 0x40, value, uart,
+			       NULL, 0, QT2_USB_TIMEOUT);
+}
+
+static inline int update_mctrl(struct qt2_port_private *port_priv,
+			       unsigned int set, unsigned int clear)
+{
+	struct usb_serial_port *port = port_priv->port;
+	struct usb_device *dev = port->serial->dev;
+	unsigned urb_value;
+	int status;
+
+	if (((set | clear) & (TIOCM_DTR | TIOCM_RTS)) == 0) {
+		dev_dbg(&port->dev,
+			"update_mctrl - DTR|RTS not being set|cleared\n");
+		return 0;	/* no change */
+	}
+
+	clear &= ~set;	/* 'set' takes precedence over 'clear' */
+	urb_value = 0;
+	if (set & TIOCM_DTR)
+		urb_value |= UART_MCR_DTR;
+	if (set & TIOCM_RTS)
+		urb_value |= UART_MCR_RTS;
+
+	status = qt2_setregister(dev, port_priv->device_port, UART_MCR,
+				 urb_value);
+	if (status < 0)
+		dev_err(&port->dev,
+			"update_mctrl - Error from MODEM_CTRL urb: %i\n",
+			status);
+	return status;
+}
+
+static int qt2_calc_num_ports(struct usb_serial *serial)
+{
+	struct qt2_device_detail d;
+	int i;
+
+	for (i = 0; d = qt2_device_details[i], d.product_id != 0; i++) {
+		if (d.product_id == le16_to_cpu(serial->dev->descriptor.idProduct))
+			return d.num_ports;
+	}
+
+	/* we didn't recognize the device */
+	dev_err(&serial->dev->dev,
+		 "don't know the number of ports, assuming 1\n");
+
+	return 1;
+}
+
+static void qt2_set_termios(struct tty_struct *tty,
+			    struct usb_serial_port *port,
+			    struct ktermios *old_termios)
+{
+	struct usb_device *dev = port->serial->dev;
+	struct qt2_port_private *port_priv;
+	struct ktermios *termios = tty->termios;
+	u16 baud;
+	unsigned int cflag = termios->c_cflag;
+	u16 new_lcr = 0;
+	int status;
+
+	port_priv = usb_get_serial_port_data(port);
+
+	if (cflag & PARENB) {
+		if (cflag & PARODD)
+			new_lcr |= UART_LCR_PARITY;
+		else
+			new_lcr |= SERIAL_EVEN_PARITY;
+	}
+
+	switch (cflag & CSIZE) {
+	case CS5:
+		new_lcr |= UART_LCR_WLEN5;
+		break;
+	case CS6:
+		new_lcr |= UART_LCR_WLEN6;
+		break;
+	case CS7:
+		new_lcr |= UART_LCR_WLEN7;
+		break;
+	default:
+	case CS8:
+		new_lcr |= UART_LCR_WLEN8;
+		break;
+	}
+
+	baud = tty_get_baud_rate(tty);
+	if (!baud)
+		baud = 9600;
+
+	status = qt2_set_port_config(dev, port_priv->device_port, baud,
+				     new_lcr);
+	if (status < 0)
+		dev_err(&port->dev, "%s - qt2_set_port_config failed: %i\n",
+			__func__, status);
+
+	if (cflag & CRTSCTS)
+		status = qt2_control_msg(dev, QT_HW_FLOW_CONTROL_MASK,
+					 SERIAL_CRTSCTS,
+					 port_priv->device_port);
+	else
+		status = qt2_control_msg(dev, QT_HW_FLOW_CONTROL_MASK,
+					 0, port_priv->device_port);
+	if (status < 0)
+		dev_err(&port->dev, "%s - set HW flow control failed: %i\n",
+			__func__, status);
+
+	if (I_IXOFF(tty) || I_IXON(tty)) {
+		u16 x = ((u16) (START_CHAR(tty) << 8) | (u16) (STOP_CHAR(tty)));
+
+		status = qt2_control_msg(dev, QT_SW_FLOW_CONTROL_MASK,
+					 x, port_priv->device_port);
+	} else
+		status = qt2_control_msg(dev, QT_SW_FLOW_CONTROL_MASK,
+					 0, port_priv->device_port);
+
+	if (status < 0)
+		dev_err(&port->dev, "%s - set SW flow control failed: %i\n",
+			__func__, status);
+
+}
+
+static int qt2_open(struct tty_struct *tty, struct usb_serial_port *port)
+{
+	struct usb_serial *serial;
+	struct qt2_serial_private *serial_priv;
+	struct qt2_port_private *port_priv;
+	u8 *data;
+	u16 device_port;
+	int status;
+	unsigned long flags;
+
+	device_port = (u16) (port->number - port->serial->minor);
+
+	serial = port->serial;
+
+	port_priv = usb_get_serial_port_data(port);
+	serial_priv = usb_get_serial_data(serial);
+
+	/* set the port to RS232 mode */
+	status = qt2_control_msg(serial->dev, QT2_GET_SET_QMCR,
+				 QT2_QMCR_RS232, device_port);
+	if (status < 0) {
+		dev_err(&port->dev,
+			"%s failed to set RS232 mode for port %i error %i\n",
+			__func__, device_port, status);
+		return status;
+	}
+
+	data = kzalloc(2, GFP_KERNEL);
+	if (!data)
+		return -ENOMEM;
+
+	/* open the port */
+	status = usb_control_msg(serial->dev,
+				 usb_rcvctrlpipe(serial->dev, 0),
+				 QT_OPEN_CLOSE_CHANNEL,
+				 0xc0, 0,
+				 device_port, data, 2, QT2_USB_TIMEOUT);
+
+	if (status < 0) {
+		dev_err(&port->dev, "%s - open port failed %i", __func__,
+			status);
+		kfree(data);
+		return status;
+	}
+
+	spin_lock_irqsave(&port_priv->lock, flags);
+	port_priv->shadowLSR = data[0];
+	port_priv->shadowMSR = data[1];
+	spin_unlock_irqrestore(&port_priv->lock, flags);
+
+	kfree(data);
+
+	/* set to default speed and 8bit word size */
+	status = qt2_set_port_config(serial->dev, device_port,
+				     DEFAULT_BAUD_RATE, UART_LCR_WLEN8);
+	if (status < 0) {
+		dev_err(&port->dev,
+			"%s - initial setup failed for port %i (%i)\n",
+			__func__, port->number, device_port);
+		return status;
+	}
+
+	port_priv->is_open = true;
+	port_priv->device_port = (u8) device_port;
+
+	if (tty)
+		qt2_set_termios(tty, port, tty->termios);
+
+	return 0;
+
+}
+
+static void qt2_close(struct usb_serial_port *port)
+{
+	struct usb_serial *serial;
+	struct qt2_serial_private *serial_priv;
+	struct qt2_port_private *port_priv;
+	unsigned long flags;
+	int i;
+
+	serial = port->serial;
+	serial_priv = usb_get_serial_data(serial);
+	port_priv = usb_get_serial_port_data(port);
+
+	port_priv->is_open = false;
+
+	spin_lock_irqsave(&port_priv->urb_lock, flags);
+	if (port_priv->write_urb->status == -EINPROGRESS)
+		usb_kill_urb(port_priv->write_urb);
+	port_priv->urb_in_use = false;
+	spin_unlock_irqrestore(&port_priv->urb_lock, flags);
+
+	/* flush the port transmit buffer */
+	i = usb_control_msg(serial->dev,
+			    usb_rcvctrlpipe(serial->dev, 0),
+			    QT2_FLUSH_DEVICE, 0x40, 1,
+			    port_priv->device_port, NULL, 0, QT2_USB_TIMEOUT);
+
+	if (i < 0)
+		dev_err(&port->dev, "%s - transmit buffer flush failed: %i\n",
+			__func__, i);
+
+	/* flush the port receive buffer */
+	i = usb_control_msg(serial->dev,
+			    usb_rcvctrlpipe(serial->dev, 0),
+			    QT2_FLUSH_DEVICE, 0x40, 0,
+			    port_priv->device_port, NULL, 0, QT2_USB_TIMEOUT);
+
+	if (i < 0)
+		dev_err(&port->dev, "%s - receive buffer flush failed: %i\n",
+			__func__, i);
+
+	/* close the port */
+	i = usb_control_msg(serial->dev,
+			    usb_sndctrlpipe(serial->dev, 0),
+			    QT_OPEN_CLOSE_CHANNEL,
+			    0x40, 0,
+			    port_priv->device_port, NULL, 0, QT2_USB_TIMEOUT);
+
+	if (i < 0)
+		dev_err(&port->dev, "%s - close port failed %i\n",
+			__func__, i);
+
+}
+
+static void qt2_disconnect(struct usb_serial *serial)
+{
+	struct qt2_serial_private *serial_priv = usb_get_serial_data(serial);
+	struct qt2_port_private *port_priv;
+	int i;
+
+	if (serial_priv->read_urb->status == -EINPROGRESS)
+		usb_kill_urb(serial_priv->read_urb);
+
+	usb_free_urb(serial_priv->read_urb);
+
+	for (i = 0; i < serial->num_ports; i++) {
+		port_priv = usb_get_serial_port_data(serial->port[i]);
+
+		if (port_priv->write_urb->status == -EINPROGRESS)
+			usb_kill_urb(port_priv->write_urb);
+		usb_free_urb(port_priv->write_urb);
+	}
+}
+
+static int get_serial_info(struct usb_serial_port *port,
+			   struct serial_struct __user *retinfo)
+{
+	struct serial_struct tmp;
+
+	if (!retinfo)
+		return -EFAULT;
+
+	memset(&tmp, 0, sizeof(tmp));
+	tmp.line		= port->serial->minor;
+	tmp.port		= 0;
+	tmp.irq			= 0;
+	tmp.flags		= ASYNC_SKIP_TEST | ASYNC_AUTO_IRQ;
+	tmp.xmit_fifo_size	= port->bulk_out_size;
+	tmp.baud_base		= 9600;
+	tmp.close_delay		= 5*HZ;
+	tmp.closing_wait	= 30*HZ;
+
+	if (copy_to_user(retinfo, &tmp, sizeof(*retinfo)))
+		return -EFAULT;
+	return 0;
+}
+
+static int wait_modem_info(struct usb_serial_port *port, unsigned int arg)
+{
+	struct qt2_port_private *priv = usb_get_serial_port_data(port);
+	struct async_icount prev, cur;
+	unsigned long flags;
+
+	spin_lock_irqsave(&priv->lock, flags);
+	prev = priv->icount;
+	spin_unlock_irqrestore(&priv->lock, flags);
+
+	while (1) {
+		wait_event_interruptible(priv->delta_msr_wait,
+					 ((priv->icount.rng != prev.rng) ||
+					  (priv->icount.dsr != prev.dsr) ||
+					  (priv->icount.dcd != prev.dcd) ||
+					  (priv->icount.cts != prev.cts)));
+
+		if (signal_pending(current))
+			return -ERESTARTSYS;
+
+		spin_lock_irqsave(&priv->lock, flags);
+		cur = priv->icount;
+		spin_unlock_irqrestore(&priv->lock, flags);
+
+		if ((prev.rng == cur.rng) &&
+		    (prev.dsr == cur.dsr) &&
+		    (prev.dcd == cur.dcd) &&
+		    (prev.cts == cur.cts))
+			return -EIO;
+
+		if ((arg & TIOCM_RNG && (prev.rng != cur.rng)) ||
+		    (arg & TIOCM_DSR && (prev.dsr != cur.dsr)) ||
+		    (arg & TIOCM_CD && (prev.dcd != cur.dcd)) ||
+		    (arg & TIOCM_CTS && (prev.cts != cur.cts)))
+			return 0;
+	}
+	return 0;
+}
+
+static int qt2_get_icount(struct tty_struct *tty,
+			  struct serial_icounter_struct *icount)
+{
+	struct usb_serial_port *port = tty->driver_data;
+	struct qt2_port_private *priv = usb_get_serial_port_data(port);
+	struct async_icount cnow = priv->icount;
+
+	icount->cts = cnow.cts;
+	icount->dsr = cnow.dsr;
+	icount->rng = cnow.rng;
+	icount->dcd = cnow.dcd;
+	icount->rx = cnow.rx;
+	icount->tx = cnow.tx;
+	icount->frame = cnow.frame;
+	icount->overrun = cnow.overrun;
+	icount->parity = cnow.parity;
+	icount->brk = cnow.brk;
+	icount->buf_overrun = cnow.buf_overrun;
+
+	return 0;
+}
+
+static int qt2_ioctl(struct tty_struct *tty,
+		     unsigned int cmd, unsigned long arg)
+{
+	struct usb_serial_port *port = tty->driver_data;
+
+	switch (cmd) {
+	case TIOCGSERIAL:
+		return get_serial_info(port,
+				       (struct serial_struct __user *)arg);
+
+	case TIOCMIWAIT:
+		return wait_modem_info(port, arg);
+
+	default:
+		break;
+	}
+
+	return -ENOIOCTLCMD;
+}
+
+static void qt2_process_status(struct usb_serial_port *port, unsigned char *ch)
+{
+	switch (*ch) {
+	case QT2_LINE_STATUS:
+		qt2_update_lsr(port, ch + 1);
+		break;
+	case QT2_MODEM_STATUS:
+		qt2_update_msr(port, ch + 1);
+		break;
+	}
+}
+
+/* not needed, kept to document functionality */
+static void qt2_process_xmit_empty(struct usb_serial_port *port,
+				   unsigned char *ch)
+{
+	int bytes_written;
+
+	bytes_written = (int)(*ch) + (int)(*(ch + 1) << 4);
+}
+
+/* not needed, kept to document functionality */
+static void qt2_process_flush(struct usb_serial_port *port, unsigned char *ch)
+{
+	return;
+}
+
+void qt2_process_read_urb(struct urb *urb)
+{
+	struct usb_serial *serial;
+	struct qt2_serial_private *serial_priv;
+	struct usb_serial_port *port;
+	struct qt2_port_private *port_priv;
+	struct tty_struct *tty;
+	bool escapeflag;
+	unsigned char *ch;
+	int i;
+	unsigned char newport;
+	int len = urb->actual_length;
+
+	if (!len)
+		return;
+
+	ch = urb->transfer_buffer;
+	tty = NULL;
+	serial = urb->context;
+	serial_priv = usb_get_serial_data(serial);
+	port = serial->port[serial_priv->current_port];
+	port_priv = usb_get_serial_port_data(port);
+
+	if (port_priv->is_open)
+		tty = tty_port_tty_get(&port->port);
+
+	for (i = 0; i < urb->actual_length; i++) {
+		ch = (unsigned char *)urb->transfer_buffer + i;
+		if ((i <= (len - 3)) &&
+		    (*ch == QT2_CONTROL_BYTE) &&
+		    (*(ch + 1) == QT2_CONTROL_BYTE)) {
+			escapeflag = false;
+			switch (*(ch + 2)) {
+			case QT2_LINE_STATUS:
+			case QT2_MODEM_STATUS:
+				if (i > (len - 4)) {
+					dev_warn(&port->dev,
+						 "%s - status message too short\n",
+						__func__);
+					break;
+				}
+				qt2_process_status(port, ch + 2);
+				i += 3;
+				escapeflag = true;
+				break;
+			case QT2_XMIT_HOLD:
+				if (i > (len - 5)) {
+					dev_warn(&port->dev,
+						 "%s - xmit_empty message too short\n",
+						 __func__);
+					break;
+				}
+				qt2_process_xmit_empty(port, ch + 3);
+				i += 4;
+				escapeflag = true;
+				break;
+			case QT2_CHANGE_PORT:
+				if (i > (len - 4)) {
+					dev_warn(&port->dev,
+						 "%s - change_port message too short\n",
+						 __func__);
+					break;
+				}
+				if (tty) {
+					tty_flip_buffer_push(tty);
+					tty_kref_put(tty);
+				}
+
+				newport = *(ch + 3);
+
+				if (newport > serial->num_ports) {
+					dev_err(&port->dev,
+						"%s - port change to invalid port: %i\n",
+						__func__, newport);
+					break;
+				}
+
+				serial_priv->current_port = newport;
+				port = serial->port[serial_priv->current_port];
+				port_priv = usb_get_serial_port_data(port);
+				if (port_priv->is_open)
+					tty = tty_port_tty_get(&port->port);
+				else
+					tty = NULL;
+				i += 3;
+				escapeflag = true;
+				break;
+			case QT2_REC_FLUSH:
+			case QT2_XMIT_FLUSH:
+				qt2_process_flush(port, ch + 2);
+				i += 2;
+				escapeflag = true;
+				break;
+			case QT2_CONTROL_ESCAPE:
+				tty_buffer_request_room(tty, 2);
+				tty_insert_flip_string(tty, ch, 2);
+				i += 2;
+				escapeflag = true;
+				break;
+			default:
+				dev_warn(&port->dev,
+					 "%s - unsupported command %i\n",
+					 __func__, *(ch + 2));
+				break;
+			}
+			if (escapeflag)
+				continue;
+		}
+
+		if (tty) {
+			tty_buffer_request_room(tty, 1);
+			tty_insert_flip_string(tty, ch, 1);
+		}
+	}
+
+	if (tty) {
+		tty_flip_buffer_push(tty);
+		tty_kref_put(tty);
+	}
+}
+
+static void qt2_write_bulk_callback(struct urb *urb)
+{
+	struct usb_serial_port *port;
+	struct qt2_port_private *port_priv;
+
+	port = urb->context;
+	port_priv = usb_get_serial_port_data(port);
+
+	spin_lock(&port_priv->urb_lock);
+
+	port_priv->urb_in_use = false;
+	usb_serial_port_softint(port);
+
+	spin_unlock(&port_priv->urb_lock);
+
+}
+
+static void qt2_read_bulk_callback(struct urb *urb)
+{
+	struct usb_serial *serial = urb->context;
+	int status;
+
+	if (urb->status) {
+		dev_warn(&serial->dev->dev,
+			 "%s - non-zero urb status: %i\n", __func__,
+			 urb->status);
+		return;
+	}
+
+	qt2_process_read_urb(urb);
+
+	status = usb_submit_urb(urb, GFP_ATOMIC);
+	if (status != 0)
+		dev_err(&serial->dev->dev,
+			"%s - resubmit read urb failed: %i\n",
+			__func__, status);
+}
+
+static int qt2_setup_urbs(struct usb_serial *serial)
+{
+	struct usb_serial_port *port;
+	struct usb_serial_port *port0;
+	struct qt2_serial_private *serial_priv;
+	struct qt2_port_private *port_priv;
+	int pcount, status;
+
+	port0 = serial->port[0];
+
+	serial_priv = usb_get_serial_data(serial);
+	serial_priv->read_urb = usb_alloc_urb(0, GFP_KERNEL);
+	if (!serial_priv->read_urb) {
+		dev_err(&serial->dev->dev, "No free urbs available\n");
+		return -ENOMEM;
+	}
+
+	usb_fill_bulk_urb(serial_priv->read_urb, serial->dev,
+			  usb_rcvbulkpipe(serial->dev,
+					  port0->bulk_in_endpointAddress),
+			  serial_priv->read_buffer,
+			  sizeof(serial_priv->read_buffer),
+			  qt2_read_bulk_callback, serial);
+
+	/* setup write_urb for each port */
+	for (pcount = 0; pcount < serial->num_ports; pcount++) {
+
+		port = serial->port[pcount];
+		port_priv = usb_get_serial_port_data(port);
+
+		port_priv->write_urb = usb_alloc_urb(0, GFP_KERNEL);
+		if (!port_priv->write_urb) {
+			dev_err(&serial->dev->dev,
+				"failed to alloc write_urb for port %i\n",
+				pcount);
+			return -ENOMEM;
+		}
+
+		usb_fill_bulk_urb(port_priv->write_urb,
+				  serial->dev,
+				  usb_sndbulkpipe(serial->dev,
+						  port0->
+						  bulk_out_endpointAddress),
+				  port_priv->write_buffer,
+				  sizeof(port_priv->write_buffer),
+				  qt2_write_bulk_callback, port);
+	}
+
+	status = usb_submit_urb(serial_priv->read_urb, GFP_KERNEL);
+	if (status != 0) {
+		dev_err(&serial->dev->dev,
+			"%s - submit read urb failed %i\n", __func__, status);
+		return status;
+	}
+
+	return 0;
+
+}
+
+static int qt2_attach(struct usb_serial *serial)
+{
+	struct qt2_serial_private *serial_priv;
+	struct qt2_port_private *port_priv;
+	int status, pcount;
+
+	/* power on unit */
+	status = usb_control_msg(serial->dev, usb_rcvctrlpipe(serial->dev, 0),
+				 0xc2, 0x40, 0x8000, 0, NULL, 0,
+				 QT2_USB_TIMEOUT);
+	if (status < 0) {
+		dev_err(&serial->dev->dev,
+			"%s - failed to power on unit: %i\n", __func__, status);
+		return status;
+	}
+
+	serial_priv = kzalloc(sizeof(*serial_priv), GFP_KERNEL);
+	if (!serial_priv) {
+		dev_err(&serial->dev->dev, "%s - Out of memory\n", __func__);
+		return -ENOMEM;
+	}
+
+	usb_set_serial_data(serial, serial_priv);
+
+	for (pcount = 0; pcount < serial->num_ports; pcount++) {
+		port_priv = kzalloc(sizeof(*port_priv), GFP_KERNEL);
+		if (!port_priv) {
+			dev_err(&serial->dev->dev,
+				"%s- kmalloc(%Zd) failed.\n", __func__,
+				sizeof(*port_priv));
+			pcount--;
+			status = -ENOMEM;
+			goto attach_failed;
+		}
+
+		spin_lock_init(&port_priv->lock);
+		spin_lock_init(&port_priv->urb_lock);
+		init_waitqueue_head(&port_priv->delta_msr_wait);
+
+		port_priv->port = serial->port[pcount];
+
+		usb_set_serial_port_data(serial->port[pcount], port_priv);
+	}
+
+	status = qt2_setup_urbs(serial);
+	if (status != 0)
+		goto attach_failed;
+
+	return 0;
+
+attach_failed:
+	for (/* empty */; pcount >= 0; pcount--) {
+		port_priv = usb_get_serial_port_data(serial->port[pcount]);
+		kfree(port_priv);
+	}
+	kfree(serial_priv);
+	return status;
+}
+
+static int qt2_tiocmget(struct tty_struct *tty)
+{
+	struct usb_serial_port *port = tty->driver_data;
+	struct usb_device *dev = port->serial->dev;
+	struct qt2_port_private *port_priv = usb_get_serial_port_data(port);
+	u8 *d;
+	int r;
+
+	d = kzalloc(2, GFP_KERNEL);
+	if (!d)
+		return -ENOMEM;
+
+	r = qt2_getregister(dev, port_priv->device_port, UART_MCR, d);
+	if (r < 0)
+		goto mget_out;
+
+	r = qt2_getregister(dev, port_priv->device_port, UART_MSR, d + 1);
+	if (r < 0)
+		goto mget_out;
+
+	r = (d[0] & UART_MCR_DTR ? TIOCM_DTR : 0) |
+	    (d[0] & UART_MCR_RTS ? TIOCM_RTS : 0) |
+	    (d[1] & UART_MSR_CTS ? TIOCM_CTS : 0) |
+	    (d[1] & UART_MSR_DCD ? TIOCM_CAR : 0) |
+	    (d[1] & UART_MSR_RI ? TIOCM_RI : 0) |
+	    (d[1] & UART_MSR_DSR ? TIOCM_DSR : 0);
+
+mget_out:
+	kfree(d);
+	return r;
+}
+
+static int qt2_tiocmset(struct tty_struct *tty,
+			unsigned int set, unsigned int clear)
+{
+	struct qt2_port_private *port_priv;
+
+	port_priv = usb_get_serial_port_data(tty->driver_data);
+	return update_mctrl(port_priv, set, clear);
+}
+
+static void qt2_break_ctl(struct tty_struct *tty, int break_state)
+{
+	struct usb_serial_port *port = tty->driver_data;
+	struct qt2_port_private *port_priv;
+	int status;
+	u16 val;
+
+	port_priv = usb_get_serial_port_data(port);
+
+	if (!port_priv->is_open) {
+		dev_err(&port->dev,
+			"%s - port is not open\n", __func__);
+		return;
+	}
+
+	val = (break_state == -1) ? 1 : 0;
+
+	status = qt2_control_msg(port->serial->dev, QT2_BREAK_CONTROL,
+				 val, port_priv->device_port);
+	if (status < 0)
+		dev_warn(&port->dev,
+			 "%s - failed to send control message: %i\n", __func__,
+			 status);
+}
+
+
+
+static void qt2_dtr_rts(struct usb_serial_port *port, int on)
+{
+	struct usb_device *dev = port->serial->dev;
+	struct qt2_port_private *port_priv = usb_get_serial_port_data(port);
+
+	mutex_lock(&port->serial->disc_mutex);
+	if (!port->serial->disconnected) {
+		/* Disable flow control */
+		if (!on && qt2_setregister(dev, port_priv->device_port,
+					   UART_MCR, 0) < 0)
+			dev_warn(&port->dev, "error from flowcontrol urb\n");
+		/* drop RTS and DTR */
+		if (on)
+			update_mctrl(port_priv, TIOCM_DTR | TIOCM_RTS, 0);
+		else
+			update_mctrl(port_priv, 0, TIOCM_DTR | TIOCM_RTS);
+	}
+	mutex_unlock(&port->serial->disc_mutex);
+}
+
+static void qt2_update_msr(struct usb_serial_port *port, unsigned char *ch)
+{
+	struct qt2_port_private *port_priv;
+	u8 newMSR = (u8) *ch;
+	unsigned long flags;
+
+	port_priv = usb_get_serial_port_data(port);
+
+	spin_lock_irqsave(&port_priv->lock, flags);
+	port_priv->shadowMSR = newMSR;
+	spin_unlock_irqrestore(&port_priv->lock, flags);
+
+	if (newMSR & UART_MSR_ANY_DELTA) {
+		/* update input line counters */
+		if (newMSR & UART_MSR_DCTS)
+			port_priv->icount.cts++;
+
+		if (newMSR & UART_MSR_DDSR)
+			port_priv->icount.dsr++;
+
+		if (newMSR & UART_MSR_DDCD)
+			port_priv->icount.dcd++;
+
+		if (newMSR & UART_MSR_TERI)
+			port_priv->icount.rng++;
+
+		wake_up_interruptible(&port_priv->delta_msr_wait);
+	}
+}
+
+static void qt2_update_lsr(struct usb_serial_port *port, unsigned char *ch)
+{
+	struct qt2_port_private *port_priv;
+	struct async_icount *icount;
+	unsigned long flags;
+	u8 newLSR = (u8) *ch;
+
+	port_priv = usb_get_serial_port_data(port);
+
+	if (newLSR & UART_LSR_BI)
+		newLSR &= (u8) (UART_LSR_OE | UART_LSR_BI);
+
+	spin_lock_irqsave(&port_priv->lock, flags);
+	port_priv->shadowLSR = newLSR;
+	spin_unlock_irqrestore(&port_priv->lock, flags);
+
+	icount = &port_priv->icount;
+
+	if (newLSR & UART_LSR_BRK_ERROR_BITS) {
+
+		if (newLSR & UART_LSR_BI)
+			icount->brk++;
+
+		if (newLSR & UART_LSR_OE)
+			icount->overrun++;
+
+		if (newLSR & UART_LSR_PE)
+			icount->parity++;
+
+		if (newLSR & UART_LSR_FE)
+			icount->frame++;
+	}
+
+}
+
+static int qt2_write_room(struct tty_struct *tty)
+{
+	struct usb_serial_port *port = tty->driver_data;
+	struct qt2_port_private *port_priv;
+	unsigned long flags = 0;
+	int r;
+
+	port_priv = usb_get_serial_port_data(port);
+
+	spin_lock_irqsave(&port_priv->urb_lock, flags);
+
+	if (port_priv->urb_in_use)
+		r = 0;
+	else
+		r = QT2_WRITE_BUFFER_SIZE - QT2_WRITE_CONTROL_SIZE;
+
+	spin_unlock_irqrestore(&port_priv->urb_lock, flags);
+
+	return r;
+}
+
+static int qt2_write(struct tty_struct *tty,
+		     struct usb_serial_port *port,
+		     const unsigned char *buf, int count)
+{
+	struct qt2_port_private *port_priv;
+	struct urb *write_urb;
+	unsigned char *data;
+	unsigned long flags;
+	int status;
+	int bytes_out = 0;
+
+	port_priv = usb_get_serial_port_data(port);
+
+	if (port_priv->write_urb == NULL) {
+		dev_err(&port->dev, "%s - no output urb\n", __func__);
+		return 0;
+	}
+	write_urb = port_priv->write_urb;
+
+	count = min(count, QT2_WRITE_BUFFER_SIZE - QT2_WRITE_CONTROL_SIZE);
+
+	data = write_urb->transfer_buffer;
+	spin_lock_irqsave(&port_priv->urb_lock, flags);
+	if (port_priv->urb_in_use == true) {
+		printk(KERN_INFO "qt2_write - urb is in use\n");
+		goto write_out;
+	}
+
+	*data++ = QT2_CONTROL_BYTE;
+	*data++ = QT2_CONTROL_BYTE;
+	*data++ = port_priv->device_port;
+	put_unaligned_le16(count, data);
+	data += 2;
+	memcpy(data, buf, count);
+
+	write_urb->transfer_buffer_length = count + QT2_WRITE_CONTROL_SIZE;
+
+	status = usb_submit_urb(write_urb, GFP_ATOMIC);
+	if (status == 0) {
+		port_priv->urb_in_use = true;
+		bytes_out += count;
+	}
+
+write_out:
+	spin_unlock_irqrestore(&port_priv->urb_lock, flags);
+	return bytes_out;
+}
+
+
+static struct usb_serial_driver qt2_device = {
+	.driver = {
+		.owner = THIS_MODULE,
+		.name = "quatech-serial",
+	},
+	.description	     = DRIVER_DESC,
+	.id_table	     = id_table,
+	.open		     = qt2_open,
+	.close		     = qt2_close,
+	.write               = qt2_write,
+	.write_room          = qt2_write_room,
+	.calc_num_ports      = qt2_calc_num_ports,
+	.attach              = qt2_attach,
+	.release             = qt2_release,
+	.disconnect          = qt2_disconnect,
+	.dtr_rts             = qt2_dtr_rts,
+	.break_ctl           = qt2_break_ctl,
+	.tiocmget            = qt2_tiocmget,
+	.tiocmset            = qt2_tiocmset,
+	.get_icount	     = qt2_get_icount,
+	.ioctl               = qt2_ioctl,
+	.set_termios         = qt2_set_termios,
+};
+
+static struct usb_serial_driver *const serial_drivers[] = {
+	&qt2_device, NULL
+};
+
+module_usb_serial_driver(serial_drivers, id_table);
+
+MODULE_DESCRIPTION(DRIVER_DESC);
+MODULE_LICENSE("GPL");
+
+module_param(debug, bool, S_IRUGO | S_IWUSR);
+MODULE_PARM_DESC(debug, "Debug enabled or not");
diff --git a/drivers/usb/serial/safe_serial.c b/drivers/usb/serial/safe_serial.c
index ae4ee30c741..36e9d9fc061 100644
--- a/drivers/usb/serial/safe_serial.c
+++ b/drivers/usb/serial/safe_serial.c
@@ -151,13 +151,6 @@ static struct usb_device_id id_table[] = {
 
 MODULE_DEVICE_TABLE(usb, id_table);
 
-static struct usb_driver safe_driver = {
-	.name =		"safe_serial",
-	.probe =	usb_serial_probe,
-	.disconnect =	usb_serial_disconnect,
-	.id_table =	id_table,
-};
-
 static const __u16 crc10_table[256] = {
 	0x000, 0x233, 0x255, 0x066, 0x299, 0x0aa, 0x0cc, 0x2ff,
 	0x301, 0x132, 0x154, 0x367, 0x198, 0x3ab, 0x3cd, 0x1fe,
@@ -339,12 +332,12 @@ static int __init safe_init(void)
 		}
 	}
 
-	return usb_serial_register_drivers(&safe_driver, serial_drivers);
+	return usb_serial_register_drivers(serial_drivers, KBUILD_MODNAME, id_table);
 }
 
 static void __exit safe_exit(void)
 {
-	usb_serial_deregister_drivers(&safe_driver, serial_drivers);
+	usb_serial_deregister_drivers(serial_drivers);
 }
 
 module_init(safe_init);
diff --git a/drivers/usb/serial/siemens_mpi.c b/drivers/usb/serial/siemens_mpi.c
index 46c0430fd38..e4a1787cdba 100644
--- a/drivers/usb/serial/siemens_mpi.c
+++ b/drivers/usb/serial/siemens_mpi.c
@@ -29,13 +29,6 @@ static const struct usb_device_id id_table[] = {
 };
 MODULE_DEVICE_TABLE(usb, id_table);
 
-static struct usb_driver siemens_usb_mpi_driver = {
-	.name =		"siemens_mpi",
-	.probe =	usb_serial_probe,
-	.disconnect =	usb_serial_disconnect,
-	.id_table =	id_table,
-};
-
 static struct usb_serial_driver siemens_usb_mpi_device = {
 	.driver = {
 		.owner =	THIS_MODULE,
@@ -49,7 +42,7 @@ static struct usb_serial_driver * const serial_drivers[] = {
 	&siemens_usb_mpi_device, NULL
 };
 
-module_usb_serial_driver(siemens_usb_mpi_driver, serial_drivers);
+module_usb_serial_driver(serial_drivers, id_table);
 
 MODULE_AUTHOR(DRIVER_AUTHOR);
 MODULE_DESCRIPTION(DRIVER_DESC);
diff --git a/drivers/usb/serial/sierra.c b/drivers/usb/serial/sierra.c
index 8c8bf806f6f..ba54a0a8235 100644
--- a/drivers/usb/serial/sierra.c
+++ b/drivers/usb/serial/sierra.c
@@ -63,9 +63,7 @@ struct sierra_intf_private {
 
 static int sierra_set_power_state(struct usb_device *udev, __u16 swiState)
 {
-	int result;
-	dev_dbg(&udev->dev, "%s\n", __func__);
-	result = usb_control_msg(udev, usb_sndctrlpipe(udev, 0),
+	return usb_control_msg(udev, usb_sndctrlpipe(udev, 0),
 			SWIMS_USB_REQUEST_SetPower,	/* __u8 request      */
 			USB_TYPE_VENDOR,		/* __u8 request type */
 			swiState,			/* __u16 value       */
@@ -73,14 +71,11 @@ static int sierra_set_power_state(struct usb_device *udev, __u16 swiState)
 			NULL,				/* void *data        */
 			0,				/* __u16 size 	     */
 			USB_CTRL_SET_TIMEOUT);		/* int timeout 	     */
-	return result;
 }
 
 static int sierra_vsc_set_nmea(struct usb_device *udev, __u16 enable)
 {
-	int result;
-	dev_dbg(&udev->dev, "%s\n", __func__);
-	result = usb_control_msg(udev, usb_sndctrlpipe(udev, 0),
+	return usb_control_msg(udev, usb_sndctrlpipe(udev, 0),
 			SWIMS_USB_REQUEST_SetNmea,	/* __u8 request      */
 			USB_TYPE_VENDOR,		/* __u8 request type */
 			enable,				/* __u16 value       */
@@ -88,7 +83,6 @@ static int sierra_vsc_set_nmea(struct usb_device *udev, __u16 enable)
 			NULL,				/* void *data        */
 			0,				/* __u16 size 	     */
 			USB_CTRL_SET_TIMEOUT);		/* int timeout       */
-	return result;
 }
 
 static int sierra_calc_num_ports(struct usb_serial *serial)
@@ -96,8 +90,6 @@ static int sierra_calc_num_ports(struct usb_serial *serial)
 	int num_ports = 0;
 	u8 ifnum, numendpoints;
 
-	dev_dbg(&serial->dev->dev, "%s\n", __func__);
-
 	ifnum = serial->interface->cur_altsetting->desc.bInterfaceNumber;
 	numendpoints = serial->interface->cur_altsetting->desc.bNumEndpoints;
 
@@ -150,7 +142,6 @@ static int sierra_calc_interface(struct usb_serial *serial)
 	int interface;
 	struct usb_interface *p_interface;
 	struct usb_host_interface *p_host_interface;
-	dev_dbg(&serial->dev->dev, "%s\n", __func__);
 
 	/* Get the interface structure pointer from the serial struct */
 	p_interface = serial->interface;
@@ -175,9 +166,8 @@ static int sierra_probe(struct usb_serial *serial,
 	u8 ifnum;
 
 	udev = serial->dev;
-	dev_dbg(&udev->dev, "%s\n", __func__);
-
 	ifnum = sierra_calc_interface(serial);
+
 	/*
 	 * If this interface supports more than 1 alternate
 	 * select the 2nd one
@@ -344,8 +334,6 @@ static int sierra_send_setup(struct usb_serial_port *port)
 	int do_send = 0;
 	int retval;
 
-	dev_dbg(&port->dev, "%s\n", __func__);
-
 	portdata = usb_get_serial_port_data(port);
 
 	if (portdata->dtr_state)
@@ -393,7 +381,6 @@ static int sierra_send_setup(struct usb_serial_port *port)
 static void sierra_set_termios(struct tty_struct *tty,
 		struct usb_serial_port *port, struct ktermios *old_termios)
 {
-	dev_dbg(&port->dev, "%s\n", __func__);
 	tty_termios_copy_hw(tty->termios, old_termios);
 	sierra_send_setup(port);
 }
@@ -404,7 +391,6 @@ static int sierra_tiocmget(struct tty_struct *tty)
 	unsigned int value;
 	struct sierra_port_private *portdata;
 
-	dev_dbg(&port->dev, "%s\n", __func__);
 	portdata = usb_get_serial_port_data(port);
 
 	value = ((portdata->rts_state) ? TIOCM_RTS : 0) |
@@ -441,8 +427,7 @@ static void sierra_release_urb(struct urb *urb)
 {
 	struct usb_serial_port *port;
 	if (urb) {
-		port =  urb->context;
-		dev_dbg(&port->dev, "%s: %p\n", __func__, urb);
+		port = urb->context;
 		kfree(urb->transfer_buffer);
 		usb_free_urb(urb);
 	}
@@ -455,7 +440,6 @@ static void sierra_outdat_callback(struct urb *urb)
 	struct sierra_intf_private *intfdata;
 	int status = urb->status;
 
-	dev_dbg(&port->dev, "%s - port %d\n", __func__, port->number);
 	intfdata = port->serial->private;
 
 	/* free up the transfer buffer, as usb_free_urb() does not do this */
@@ -598,8 +582,6 @@ static void sierra_indat_callback(struct urb *urb)
 	endpoint = usb_pipeendpoint(urb->pipe);
 	port = urb->context;
 
-	dev_dbg(&port->dev, "%s: %p\n", __func__, urb);
-
 	if (status) {
 		dev_dbg(&port->dev, "%s: nonzero status: %d on"
 			" endpoint %02x\n", __func__, status, endpoint);
@@ -697,8 +679,6 @@ static int sierra_write_room(struct tty_struct *tty)
 	struct sierra_port_private *portdata = usb_get_serial_port_data(port);
 	unsigned long flags;
 
-	dev_dbg(&port->dev, "%s - port %d\n", __func__, port->number);
-
 	/* try to give a good number back based on if we have any free urbs at
 	 * this point in time */
 	spin_lock_irqsave(&portdata->lock, flags);
@@ -805,8 +785,6 @@ static void sierra_close(struct usb_serial_port *port)
 	struct sierra_port_private *portdata;
 	struct sierra_intf_private *intfdata = port->serial->private;
 
-
-	dev_dbg(&port->dev, "%s\n", __func__);
 	portdata = usb_get_serial_port_data(port);
 
 	portdata->rts_state = 0;
@@ -851,8 +829,6 @@ static int sierra_open(struct tty_struct *tty, struct usb_serial_port *port)
 
 	portdata = usb_get_serial_port_data(port);
 
-	dev_dbg(&port->dev, "%s\n", __func__);
-
 	/* Set some sane defaults */
 	portdata->rts_state = 1;
 	portdata->dtr_state = 1;
@@ -915,8 +891,6 @@ static int sierra_startup(struct usb_serial *serial)
 	int i;
 	u8 ifnum;
 
-	dev_dbg(&serial->dev->dev, "%s\n", __func__);
-
 	/* Set Device mode to D0 */
 	sierra_set_power_state(serial->dev, 0x0000);
 
@@ -977,8 +951,6 @@ static void sierra_release(struct usb_serial *serial)
 	struct usb_serial_port *port;
 	struct sierra_port_private *portdata;
 
-	dev_dbg(&serial->dev->dev, "%s\n", __func__);
-
 	for (i = 0; i < serial->num_ports; ++i) {
 		port = serial->port[i];
 		if (!port)
@@ -1067,29 +1039,11 @@ static int sierra_resume(struct usb_serial *serial)
 	return ec ? -EIO : 0;
 }
 
-static int sierra_reset_resume(struct usb_interface *intf)
-{
-	struct usb_serial *serial = usb_get_intfdata(intf);
-	dev_err(&serial->dev->dev, "%s\n", __func__);
-	return usb_serial_resume(intf);
-}
 #else
 #define sierra_suspend NULL
 #define sierra_resume NULL
-#define sierra_reset_resume NULL
 #endif
 
-static struct usb_driver sierra_driver = {
-	.name       = "sierra",
-	.probe      = usb_serial_probe,
-	.disconnect = usb_serial_disconnect,
-	.suspend    = usb_serial_suspend,
-	.resume     = usb_serial_resume,
-	.reset_resume = sierra_reset_resume,
-	.id_table   = id_table,
-	.supports_autosuspend =	1,
-};
-
 static struct usb_serial_driver sierra_device = {
 	.driver = {
 		.owner =	THIS_MODULE,
@@ -1118,7 +1072,7 @@ static struct usb_serial_driver * const serial_drivers[] = {
 	&sierra_device, NULL
 };
 
-module_usb_serial_driver(sierra_driver, serial_drivers);
+module_usb_serial_driver(serial_drivers, id_table);
 
 MODULE_AUTHOR(DRIVER_AUTHOR);
 MODULE_DESCRIPTION(DRIVER_DESC);
diff --git a/drivers/usb/serial/spcp8x5.c b/drivers/usb/serial/spcp8x5.c
index f06c9a8f3d3..cad60898471 100644
--- a/drivers/usb/serial/spcp8x5.c
+++ b/drivers/usb/serial/spcp8x5.c
@@ -151,14 +151,6 @@ enum spcp8x5_type {
 	SPCP835_TYPE,
 };
 
-static struct usb_driver spcp8x5_driver = {
-	.name =			"spcp8x5",
-	.probe =		usb_serial_probe,
-	.disconnect =		usb_serial_disconnect,
-	.id_table =		id_table,
-};
-
-
 struct spcp8x5_private {
 	spinlock_t 	lock;
 	enum spcp8x5_type	type;
@@ -433,7 +425,7 @@ static void spcp8x5_set_termios(struct tty_struct *tty,
 	if (i < 0)
 		dev_err(&port->dev, "Set UART format %#x failed (error = %d)\n",
 			uartdata, i);
-	dbg("0x21:0x40:0:0  %d", i);
+	dev_dbg(&port->dev, "0x21:0x40:0:0  %d\n", i);
 
 	if (cflag & CRTSCTS) {
 		/* enable hardware flow control */
@@ -454,8 +446,6 @@ static int spcp8x5_open(struct tty_struct *tty, struct usb_serial_port *port)
 	u8 status = 0x30;
 	/* status 0x30 means DSR and CTS = 1 other CDC RI and delta = 0 */
 
-	dbg("%s -  port %d", __func__, port->number);
-
 	usb_clear_halt(serial->dev, port->write_urb->pipe);
 	usb_clear_halt(serial->dev, port->read_urb->pipe);
 
@@ -579,15 +569,19 @@ static int spcp8x5_ioctl(struct tty_struct *tty,
 			 unsigned int cmd, unsigned long arg)
 {
 	struct usb_serial_port *port = tty->driver_data;
-	dbg("%s (%d) cmd = 0x%04x", __func__, port->number, cmd);
+
+	dev_dbg(&port->dev, "%s (%d) cmd = 0x%04x\n", __func__,
+		port->number, cmd);
 
 	switch (cmd) {
 	case TIOCMIWAIT:
-		dbg("%s (%d) TIOCMIWAIT", __func__,  port->number);
+		dev_dbg(&port->dev, "%s (%d) TIOCMIWAIT\n", __func__,
+			port->number);
 		return spcp8x5_wait_modem_info(port, arg);
 
 	default:
-		dbg("%s not supported = 0x%04x", __func__, cmd);
+		dev_dbg(&port->dev, "%s not supported = 0x%04x", __func__,
+			cmd);
 		break;
 	}
 
@@ -666,7 +660,7 @@ static struct usb_serial_driver * const serial_drivers[] = {
 	&spcp8x5_device, NULL
 };
 
-module_usb_serial_driver(spcp8x5_driver, serial_drivers);
+module_usb_serial_driver(serial_drivers, id_table);
 
 MODULE_DESCRIPTION(DRIVER_DESC);
 MODULE_VERSION(DRIVER_VERSION);
diff --git a/drivers/usb/serial/ssu100.c b/drivers/usb/serial/ssu100.c
index 3cdc8a52de4..3fee23bf0c1 100644
--- a/drivers/usb/serial/ssu100.c
+++ b/drivers/usb/serial/ssu100.c
@@ -59,20 +59,8 @@ static const struct usb_device_id id_table[] = {
 	{USB_DEVICE(USB_VENDOR_ID_QUATECH, QUATECH_SSU100)},
 	{}			/* Terminating entry */
 };
-
 MODULE_DEVICE_TABLE(usb, id_table);
 
-
-static struct usb_driver ssu100_driver = {
-	.name			       = "ssu100",
-	.probe			       = usb_serial_probe,
-	.disconnect		       = usb_serial_disconnect,
-	.id_table		       = id_table,
-	.suspend		       = usb_serial_suspend,
-	.resume			       = usb_serial_resume,
-	.supports_autosuspend	       = 1,
-};
-
 struct ssu100_port_private {
 	spinlock_t status_lock;
 	u8 shadowLSR;
@@ -85,7 +73,6 @@ static void ssu100_release(struct usb_serial *serial)
 {
 	struct ssu100_port_private *priv = usb_get_serial_port_data(*serial->port);
 
-	dbg("%s", __func__);
 	kfree(priv);
 }
 
@@ -171,8 +158,6 @@ static int ssu100_initdevice(struct usb_device *dev)
 	u8 *data;
 	int result = 0;
 
-	dbg("%s", __func__);
-
 	data = kzalloc(3, GFP_KERNEL);
 	if (!data)
 		return -ENOMEM;
@@ -237,8 +222,6 @@ static void ssu100_set_termios(struct tty_struct *tty,
 	u16 urb_value = 0; /* will hold the new flags */
 	int result;
 
-	dbg("%s", __func__);
-
 	if (cflag & PARENB) {
 		if (cflag & PARODD)
 			urb_value |= UART_LCR_PARITY;
@@ -312,8 +295,6 @@ static int ssu100_open(struct tty_struct *tty, struct usb_serial_port *port)
 	int result;
 	unsigned long flags;
 
-	dbg("%s - port %d", __func__, port->number);
-
 	data = kzalloc(2, GFP_KERNEL);
 	if (!data)
 		return -ENOMEM;
@@ -348,7 +329,6 @@ static int ssu100_open(struct tty_struct *tty, struct usb_serial_port *port)
 
 static void ssu100_close(struct usb_serial_port *port)
 {
-	dbg("%s", __func__);
 	usb_serial_generic_close(port);
 }
 
@@ -467,8 +447,6 @@ static int ssu100_attach(struct usb_serial *serial)
 	struct ssu100_port_private *priv;
 	struct usb_serial_port *port = *serial->port;
 
-	dbg("%s", __func__);
-
 	priv = kzalloc(sizeof(*priv), GFP_KERNEL);
 	if (!priv) {
 		dev_err(&port->dev, "%s- kmalloc(%Zd) failed.\n", __func__,
@@ -490,8 +468,6 @@ static int ssu100_tiocmget(struct tty_struct *tty)
 	u8 *d;
 	int r;
 
-	dbg("%s\n", __func__);
-
 	d = kzalloc(2, GFP_KERNEL);
 	if (!d)
 		return -ENOMEM;
@@ -522,7 +498,6 @@ static int ssu100_tiocmset(struct tty_struct *tty,
 	struct usb_serial_port *port = tty->driver_data;
 	struct usb_device *dev = port->serial->dev;
 
-	dbg("%s\n", __func__);
 	return update_mctrl(dev, set, clear);
 }
 
@@ -530,8 +505,6 @@ static void ssu100_dtr_rts(struct usb_serial_port *port, int on)
 {
 	struct usb_device *dev = port->serial->dev;
 
-	dbg("%s\n", __func__);
-
 	mutex_lock(&port->serial->disc_mutex);
 	if (!port->serial->disconnected) {
 		/* Disable flow control */
@@ -618,8 +591,6 @@ static int ssu100_process_packet(struct urb *urb,
 	int i;
 	char *ch;
 
-	dbg("%s - port %d", __func__, port->number);
-
 	if ((len >= 4) &&
 	    (packet[0] == 0x1b) && (packet[1] == 0x1b) &&
 	    ((packet[2] == 0x00) || (packet[2] == 0x01))) {
@@ -656,8 +627,6 @@ static void ssu100_process_read_urb(struct urb *urb)
 	struct tty_struct *tty;
 	int count;
 
-	dbg("%s", __func__);
-
 	tty = tty_port_tty_get(&port->port);
 	if (!tty)
 		return;
@@ -695,7 +664,7 @@ static struct usb_serial_driver * const serial_drivers[] = {
 	&ssu100_device, NULL
 };
 
-module_usb_serial_driver(ssu100_driver, serial_drivers);
+module_usb_serial_driver(serial_drivers, id_table);
 
 MODULE_DESCRIPTION(DRIVER_DESC);
 MODULE_LICENSE("GPL");
diff --git a/drivers/usb/serial/symbolserial.c b/drivers/usb/serial/symbolserial.c
index 1a5be136e6c..e53d2aac35c 100644
--- a/drivers/usb/serial/symbolserial.c
+++ b/drivers/usb/serial/symbolserial.c
@@ -54,8 +54,6 @@ static void symbol_int_callback(struct urb *urb)
 	int result;
 	int data_length;
 
-	dbg("%s - port %d", __func__, port->number);
-
 	switch (status) {
 	case 0:
 		/* success */
@@ -64,12 +62,12 @@ static void symbol_int_callback(struct urb *urb)
 	case -ENOENT:
 	case -ESHUTDOWN:
 		/* this urb is terminated, clean up */
-		dbg("%s - urb shutting down with status: %d",
-		    __func__, status);
+		dev_dbg(&port->dev, "%s - urb shutting down with status: %d\n",
+			__func__, status);
 		return;
 	default:
-		dbg("%s - nonzero urb status received: %d",
-		    __func__, status);
+		dev_dbg(&port->dev, "%s - nonzero urb status received: %d\n",
+			__func__, status);
 		goto exit;
 	}
 
@@ -125,8 +123,6 @@ static int symbol_open(struct tty_struct *tty, struct usb_serial_port *port)
 	unsigned long flags;
 	int result = 0;
 
-	dbg("%s - port %d", __func__, port->number);
-
 	spin_lock_irqsave(&priv->lock, flags);
 	priv->throttled = false;
 	priv->actually_throttled = false;
@@ -150,8 +146,6 @@ static void symbol_close(struct usb_serial_port *port)
 {
 	struct symbol_private *priv = usb_get_serial_data(port->serial);
 
-	dbg("%s - port %d", __func__, port->number);
-
 	/* shutdown our urbs */
 	usb_kill_urb(priv->int_urb);
 }
@@ -161,7 +155,6 @@ static void symbol_throttle(struct tty_struct *tty)
 	struct usb_serial_port *port = tty->driver_data;
 	struct symbol_private *priv = usb_get_serial_data(port->serial);
 
-	dbg("%s - port %d", __func__, port->number);
 	spin_lock_irq(&priv->lock);
 	priv->throttled = true;
 	spin_unlock_irq(&priv->lock);
@@ -174,8 +167,6 @@ static void symbol_unthrottle(struct tty_struct *tty)
 	int result;
 	bool was_throttled;
 
-	dbg("%s - port %d", __func__, port->number);
-
 	spin_lock_irq(&priv->lock);
 	priv->throttled = false;
 	was_throttled = priv->actually_throttled;
@@ -266,8 +257,6 @@ static void symbol_disconnect(struct usb_serial *serial)
 {
 	struct symbol_private *priv = usb_get_serial_data(serial);
 
-	dbg("%s", __func__);
-
 	usb_kill_urb(priv->int_urb);
 	usb_free_urb(priv->int_urb);
 }
@@ -276,19 +265,10 @@ static void symbol_release(struct usb_serial *serial)
 {
 	struct symbol_private *priv = usb_get_serial_data(serial);
 
-	dbg("%s", __func__);
-
 	kfree(priv->int_buffer);
 	kfree(priv);
 }
 
-static struct usb_driver symbol_driver = {
-	.name =			"symbol",
-	.probe =		usb_serial_probe,
-	.disconnect =		usb_serial_disconnect,
-	.id_table =		id_table,
-};
-
 static struct usb_serial_driver symbol_device = {
 	.driver = {
 		.owner =	THIS_MODULE,
@@ -309,7 +289,7 @@ static struct usb_serial_driver * const serial_drivers[] = {
 	&symbol_device, NULL
 };
 
-module_usb_serial_driver(symbol_driver, serial_drivers);
+module_usb_serial_driver(serial_drivers, id_table);
 
 MODULE_LICENSE("GPL");
 
diff --git a/drivers/usb/serial/ti_usb_3410_5052.c b/drivers/usb/serial/ti_usb_3410_5052.c
index ab74123d658..a4404f5ad68 100644
--- a/drivers/usb/serial/ti_usb_3410_5052.c
+++ b/drivers/usb/serial/ti_usb_3410_5052.c
@@ -165,7 +165,7 @@ static unsigned int product_5052_count;
 /* the array dimension is the number of default entries plus */
 /* TI_EXTRA_VID_PID_COUNT user defined entries plus 1 terminating */
 /* null entry */
-static struct usb_device_id ti_id_table_3410[14+TI_EXTRA_VID_PID_COUNT+1] = {
+static struct usb_device_id ti_id_table_3410[15+TI_EXTRA_VID_PID_COUNT+1] = {
 	{ USB_DEVICE(TI_VENDOR_ID, TI_3410_PRODUCT_ID) },
 	{ USB_DEVICE(TI_VENDOR_ID, TI_3410_EZ430_ID) },
 	{ USB_DEVICE(MTS_VENDOR_ID, MTS_GSM_NO_FW_PRODUCT_ID) },
@@ -180,6 +180,7 @@ static struct usb_device_id ti_id_table_3410[14+TI_EXTRA_VID_PID_COUNT+1] = {
 	{ USB_DEVICE(IBM_VENDOR_ID, IBM_454B_PRODUCT_ID) },
 	{ USB_DEVICE(IBM_VENDOR_ID, IBM_454C_PRODUCT_ID) },
 	{ USB_DEVICE(ABBOTT_VENDOR_ID, ABBOTT_PRODUCT_ID) },
+	{ USB_DEVICE(TI_VENDOR_ID, FRI2_PRODUCT_ID) },
 };
 
 static struct usb_device_id ti_id_table_5052[5+TI_EXTRA_VID_PID_COUNT+1] = {
@@ -189,7 +190,7 @@ static struct usb_device_id ti_id_table_5052[5+TI_EXTRA_VID_PID_COUNT+1] = {
 	{ USB_DEVICE(TI_VENDOR_ID, TI_5052_FIRMWARE_PRODUCT_ID) },
 };
 
-static struct usb_device_id ti_id_table_combined[18+2*TI_EXTRA_VID_PID_COUNT+1] = {
+static struct usb_device_id ti_id_table_combined[19+2*TI_EXTRA_VID_PID_COUNT+1] = {
 	{ USB_DEVICE(TI_VENDOR_ID, TI_3410_PRODUCT_ID) },
 	{ USB_DEVICE(TI_VENDOR_ID, TI_3410_EZ430_ID) },
 	{ USB_DEVICE(MTS_VENDOR_ID, MTS_GSM_NO_FW_PRODUCT_ID) },
@@ -208,16 +209,10 @@ static struct usb_device_id ti_id_table_combined[18+2*TI_EXTRA_VID_PID_COUNT+1]
 	{ USB_DEVICE(IBM_VENDOR_ID, IBM_454B_PRODUCT_ID) },
 	{ USB_DEVICE(IBM_VENDOR_ID, IBM_454C_PRODUCT_ID) },
 	{ USB_DEVICE(ABBOTT_VENDOR_ID, ABBOTT_PRODUCT_ID) },
+	{ USB_DEVICE(TI_VENDOR_ID, FRI2_PRODUCT_ID) },
 	{ }
 };
 
-static struct usb_driver ti_usb_driver = {
-	.name			= "ti_usb_3410_5052",
-	.probe			= usb_serial_probe,
-	.disconnect		= usb_serial_disconnect,
-	.id_table		= ti_id_table_combined,
-};
-
 static struct usb_serial_driver ti_1port_device = {
 	.driver = {
 		.owner		= THIS_MODULE,
@@ -344,20 +339,18 @@ static int __init ti_init(void)
 		ti_id_table_combined[c].match_flags = USB_DEVICE_ID_MATCH_DEVICE;
 	}
 
-	ret = usb_serial_register_drivers(&ti_usb_driver, serial_drivers);
+	ret = usb_serial_register_drivers(serial_drivers, KBUILD_MODNAME, ti_id_table_combined);
 	if (ret == 0)
 		printk(KERN_INFO KBUILD_MODNAME ": " TI_DRIVER_VERSION ":"
 			       TI_DRIVER_DESC "\n");
 	return ret;
 }
 
-
 static void __exit ti_exit(void)
 {
-	usb_serial_deregister_drivers(&ti_usb_driver, serial_drivers);
+	usb_serial_deregister_drivers(serial_drivers);
 }
 
-
 module_init(ti_init);
 module_exit(ti_exit);
 
@@ -394,7 +387,9 @@ static int ti_startup(struct usb_serial *serial)
 
 	/* if we have only 1 configuration, download firmware */
 	if (dev->descriptor.bNumConfigurations == 1) {
-		if ((status = ti_download_firmware(tdev)) != 0)
+		status = ti_download_firmware(tdev);
+
+		if (status != 0)
 			goto free_tdev;
 
 		/* 3410 must be reset, 5052 resets itself */
@@ -463,8 +458,6 @@ static void ti_release(struct usb_serial *serial)
 	struct ti_device *tdev = usb_get_serial_data(serial);
 	struct ti_port *tport;
 
-	dbg("%s", __func__);
-
 	for (i = 0; i < serial->num_ports; ++i) {
 		tport = usb_get_serial_port_data(serial->port[i]);
 		if (tport) {
@@ -489,8 +482,6 @@ static int ti_open(struct tty_struct *tty, struct usb_serial_port *port)
 			     TI_PIPE_TIMEOUT_ENABLE |
 			     (TI_TRANSFER_TIMEOUT << 2));
 
-	dbg("%s - port %d", __func__, port->number);
-
 	if (tport == NULL)
 		return -ENODEV;
 
@@ -631,8 +622,6 @@ static void ti_close(struct usb_serial_port *port)
 	int status;
 	int do_unlock;
 
-	dbg("%s - port %d", __func__, port->number);
-
 	tdev = usb_get_serial_data(port->serial);
 	tport = usb_get_serial_port_data(port);
 	if (tdev == NULL || tport == NULL)
@@ -666,8 +655,6 @@ static void ti_close(struct usb_serial_port *port)
 	}
 	if (do_unlock)
 		mutex_unlock(&tdev->td_open_close_lock);
-
-	dbg("%s - exit", __func__);
 }
 
 
@@ -676,8 +663,6 @@ static int ti_write(struct tty_struct *tty, struct usb_serial_port *port,
 {
 	struct ti_port *tport = usb_get_serial_port_data(port);
 
-	dbg("%s - port %d", __func__, port->number);
-
 	if (count == 0) {
 		dbg("%s - write request of 0 bytes", __func__);
 		return 0;
@@ -701,8 +686,6 @@ static int ti_write_room(struct tty_struct *tty)
 	int room = 0;
 	unsigned long flags;
 
-	dbg("%s - port %d", __func__, port->number);
-
 	if (tport == NULL)
 		return 0;
 
@@ -722,8 +705,6 @@ static int ti_chars_in_buffer(struct tty_struct *tty)
 	int chars = 0;
 	unsigned long flags;
 
-	dbg("%s - port %d", __func__, port->number);
-
 	if (tport == NULL)
 		return 0;
 
@@ -741,8 +722,6 @@ static void ti_throttle(struct tty_struct *tty)
 	struct usb_serial_port *port = tty->driver_data;
 	struct ti_port *tport = usb_get_serial_port_data(port);
 
-	dbg("%s - port %d", __func__, port->number);
-
 	if (tport == NULL)
 		return;
 
@@ -758,8 +737,6 @@ static void ti_unthrottle(struct tty_struct *tty)
 	struct ti_port *tport = usb_get_serial_port_data(port);
 	int status;
 
-	dbg("%s - port %d", __func__, port->number);
-
 	if (tport == NULL)
 		return;
 
@@ -854,8 +831,6 @@ static void ti_set_termios(struct tty_struct *tty,
 	int port_number = port->number - port->serial->minor;
 	unsigned int mcr;
 
-	dbg("%s - port %d", __func__, port->number);
-
 	cflag = tty->termios->c_cflag;
 	iflag = tty->termios->c_iflag;
 
@@ -988,8 +963,6 @@ static int ti_tiocmget(struct tty_struct *tty)
 	unsigned int mcr;
 	unsigned long flags;
 
-	dbg("%s - port %d", __func__, port->number);
-
 	if (tport == NULL)
 		return -ENODEV;
 
@@ -1020,8 +993,6 @@ static int ti_tiocmset(struct tty_struct *tty,
 	unsigned int mcr;
 	unsigned long flags;
 
-	dbg("%s - port %d", __func__, port->number);
-
 	if (tport == NULL)
 		return -ENODEV;
 
@@ -1084,8 +1055,6 @@ static void ti_interrupt_callback(struct urb *urb)
 	int retval;
 	__u8 msr;
 
-	dbg("%s", __func__);
-
 	switch (status) {
 	case 0:
 		break;
@@ -1165,8 +1134,6 @@ static void ti_bulk_in_callback(struct urb *urb)
 	int retval = 0;
 	struct tty_struct *tty;
 
-	dbg("%s", __func__);
-
 	switch (status) {
 	case 0:
 		break;
@@ -1233,8 +1200,6 @@ static void ti_bulk_out_callback(struct urb *urb)
 	struct usb_serial_port *port = tport->tp_port;
 	int status = urb->status;
 
-	dbg("%s - port %d", __func__, port->number);
-
 	tport->tp_write_urb_in_use = 0;
 
 	switch (status) {
@@ -1287,9 +1252,6 @@ static void ti_send(struct ti_port *tport)
 	struct tty_struct *tty = tty_port_tty_get(&port->port);	/* FIXME */
 	unsigned long flags;
 
-
-	dbg("%s - port %d", __func__, port->number);
-
 	spin_lock_irqsave(&tport->tp_lock, flags);
 
 	if (tport->tp_write_urb_in_use)
@@ -1366,8 +1328,6 @@ static int ti_get_lsr(struct ti_port *tport)
 	int port_number = port->number - port->serial->minor;
 	struct ti_port_status *data;
 
-	dbg("%s - port %d", __func__, port->number);
-
 	size = sizeof(struct ti_port_status);
 	data = kmalloc(size, GFP_KERNEL);
 	if (!data) {
@@ -1480,8 +1440,6 @@ static void ti_drain(struct ti_port *tport, unsigned long timeout, int flush)
 	struct usb_serial_port *port = tport->tp_port;
 	wait_queue_t wait;
 
-	dbg("%s - port %d", __func__, port->number);
-
 	spin_lock_irq(&tport->tp_lock);
 
 	/* wait for data to drain from the buffer */
@@ -1679,11 +1637,12 @@ static int ti_download_firmware(struct ti_device *tdev)
 	const struct firmware *fw_p;
 	char buf[32];
 
-	dbg("%s\n", __func__);
 	/* try ID specific firmware first, then try generic firmware */
 	sprintf(buf, "ti_usb-v%04x-p%04x.fw", dev->descriptor.idVendor,
 	    dev->descriptor.idProduct);
-	if ((status = request_firmware(&fw_p, buf, &dev->dev)) != 0) {
+	status = request_firmware(&fw_p, buf, &dev->dev);
+
+	if (status != 0) {
 		buf[0] = '\0';
 		if (dev->descriptor.idVendor == MTS_VENDOR_ID) {
 			switch (dev->descriptor.idProduct) {
diff --git a/drivers/usb/serial/ti_usb_3410_5052.h b/drivers/usb/serial/ti_usb_3410_5052.h
index f140f1b9d5c..b353e7e3d48 100644
--- a/drivers/usb/serial/ti_usb_3410_5052.h
+++ b/drivers/usb/serial/ti_usb_3410_5052.h
@@ -37,6 +37,7 @@
 #define TI_5152_BOOT_PRODUCT_ID		0x5152	/* no EEPROM, no firmware */
 #define TI_5052_EEPROM_PRODUCT_ID	0x505A	/* EEPROM, no firmware */
 #define TI_5052_FIRMWARE_PRODUCT_ID	0x505F	/* firmware is running */
+#define FRI2_PRODUCT_ID			0x5053  /* Fish River Island II */
 
 /* Multi-Tech vendor and product ids */
 #define MTS_VENDOR_ID			0x06E0
diff --git a/drivers/usb/serial/usb-serial.c b/drivers/usb/serial/usb-serial.c
index 97355a15bbe..6a1b609a0d9 100644
--- a/drivers/usb/serial/usb-serial.c
+++ b/drivers/usb/serial/usb-serial.c
@@ -1,7 +1,7 @@
 /*
  * USB Serial Converter driver
  *
- * Copyright (C) 1999 - 2005 Greg Kroah-Hartman (greg@kroah.com)
+ * Copyright (C) 1999 - 2012 Greg Kroah-Hartman (greg@kroah.com)
  * Copyright (C) 2000 Peter Berger (pberger@brimson.com)
  * Copyright (C) 2000 Al Borchers (borchers@steinerpoint.com)
  *
@@ -43,17 +43,6 @@
 #define DRIVER_AUTHOR "Greg Kroah-Hartman, greg@kroah.com, http://www.kroah.com/linux/"
 #define DRIVER_DESC "USB Serial Driver core"
 
-/* Driver structure we register with the USB core */
-static struct usb_driver usb_serial_driver = {
-	.name =		"usbserial",
-	.probe =	usb_serial_probe,
-	.disconnect =	usb_serial_disconnect,
-	.suspend =	usb_serial_suspend,
-	.resume =	usb_serial_resume,
-	.no_dynamic_id =	1,
-	.supports_autosuspend =	1,
-};
-
 /* There is no MODULE_DEVICE_TABLE for usbserial.c.  Instead
    the MODULE_DEVICE_TABLE declarations in each serial driver
    cause the "hotplug" program to pull in whatever module is necessary
@@ -710,7 +699,7 @@ static const struct tty_port_operations serial_port_ops = {
 	.shutdown = serial_down,
 };
 
-int usb_serial_probe(struct usb_interface *interface,
+static int usb_serial_probe(struct usb_interface *interface,
 			       const struct usb_device_id *id)
 {
 	struct usb_device *dev = interface_to_usbdev(interface);
@@ -856,6 +845,8 @@ int usb_serial_probe(struct usb_interface *interface,
 			module_put(type->driver.owner);
 			return -EIO;
 		}
+		dev_info(&interface->dev, "The \"generic\" usb-serial driver is only for testing and one-off prototypes.\n");
+		dev_info(&interface->dev, "Tell linux-usb@vger.kernel.org to add your device to a proper driver.\n");
 	}
 #endif
 	if (!num_ports) {
@@ -1043,6 +1034,8 @@ int usb_serial_probe(struct usb_interface *interface,
 		dbg("the device claims to support interrupt out transfers, but write_int_callback is not defined");
 	}
 
+	usb_set_intfdata(interface, serial);
+
 	/* if this device type has an attach function, call it */
 	if (type->attach) {
 		retval = type->attach(serial);
@@ -1087,10 +1080,7 @@ int usb_serial_probe(struct usb_interface *interface,
 	serial->disconnected = 0;
 
 	usb_serial_console_init(debug, minor);
-
 exit:
-	/* success */
-	usb_set_intfdata(interface, serial);
 	module_put(type->driver.owner);
 	return 0;
 
@@ -1099,9 +1089,8 @@ probe_error:
 	module_put(type->driver.owner);
 	return -EIO;
 }
-EXPORT_SYMBOL_GPL(usb_serial_probe);
 
-void usb_serial_disconnect(struct usb_interface *interface)
+static void usb_serial_disconnect(struct usb_interface *interface)
 {
 	int i;
 	struct usb_serial *serial = usb_get_intfdata(interface);
@@ -1112,7 +1101,6 @@ void usb_serial_disconnect(struct usb_interface *interface)
 	dbg("%s", __func__);
 
 	mutex_lock(&serial->disc_mutex);
-	usb_set_intfdata(interface, NULL);
 	/* must set a flag, to signal subdrivers */
 	serial->disconnected = 1;
 	mutex_unlock(&serial->disc_mutex);
@@ -1137,7 +1125,6 @@ void usb_serial_disconnect(struct usb_interface *interface)
 	usb_serial_put(serial);
 	dev_info(dev, "device disconnected\n");
 }
-EXPORT_SYMBOL_GPL(usb_serial_disconnect);
 
 int usb_serial_suspend(struct usb_interface *intf, pm_message_t message)
 {
@@ -1181,6 +1168,22 @@ int usb_serial_resume(struct usb_interface *intf)
 }
 EXPORT_SYMBOL(usb_serial_resume);
 
+static int usb_serial_reset_resume(struct usb_interface *intf)
+{
+	struct usb_serial *serial = usb_get_intfdata(intf);
+	int rv;
+
+	serial->suspending = 0;
+	if (serial->type->reset_resume)
+		rv = serial->type->reset_resume(serial);
+	else {
+		rv = -EOPNOTSUPP;
+		intf->needs_binding = 1;
+	}
+
+	return rv;
+}
+
 static const struct tty_operations serial_ops = {
 	.open =			serial_open,
 	.close =		serial_close,
@@ -1204,6 +1207,17 @@ static const struct tty_operations serial_ops = {
 
 struct tty_driver *usb_serial_tty_driver;
 
+/* Driver structure we register with the USB core */
+static struct usb_driver usb_serial_driver = {
+	.name =		"usbserial",
+	.probe =	usb_serial_probe,
+	.disconnect =	usb_serial_disconnect,
+	.suspend =	usb_serial_suspend,
+	.resume =	usb_serial_resume,
+	.no_dynamic_id =	1,
+	.supports_autosuspend =	1,
+};
+
 static int __init usb_serial_init(void)
 {
 	int i;
@@ -1338,7 +1352,6 @@ static int usb_serial_register(struct usb_serial_driver *driver)
 				driver->description);
 		return -EINVAL;
 	}
-	driver->usb_driver->supports_autosuspend = 1;
 
 	/* Add this device to our list of devices */
 	mutex_lock(&table_lock);
@@ -1369,18 +1382,19 @@ static void usb_serial_deregister(struct usb_serial_driver *device)
 
 /**
  * usb_serial_register_drivers - register drivers for a usb-serial module
- * @udriver: usb_driver used for matching devices/interfaces
  * @serial_drivers: NULL-terminated array of pointers to drivers to be registered
+ * @name: name of the usb_driver for this set of @serial_drivers
+ * @id_table: list of all devices this @serial_drivers set binds to
  *
- * Registers @udriver and all the drivers in the @serial_drivers array.
- * Automatically fills in the .no_dynamic_id field in @udriver and
- * the .usb_driver field in each serial driver.
+ * Registers all the drivers in the @serial_drivers array, and dynamically
+ * creates a struct usb_driver with the name @name and id_table of @id_table.
  */
-int usb_serial_register_drivers(struct usb_driver *udriver,
-		struct usb_serial_driver * const serial_drivers[])
+int usb_serial_register_drivers(struct usb_serial_driver *const serial_drivers[],
+				const char *name,
+				const struct usb_device_id *id_table)
 {
 	int rc;
-	const struct usb_device_id *saved_id_table;
+	struct usb_driver *udriver;
 	struct usb_serial_driver * const *sd;
 
 	/*
@@ -1391,12 +1405,30 @@ int usb_serial_register_drivers(struct usb_driver *udriver,
 	 * Performance hack: We don't want udriver to be probed until
 	 * the serial drivers are registered, because the probe would
 	 * simply fail for lack of a matching serial driver.
-	 * Therefore save off udriver's id_table until we are all set.
+	 * So we leave udriver's id_table set to NULL until we are all set.
+	 *
+	 * Suspend/resume support is implemented in the usb-serial core,
+	 * so fill in the PM-related fields in udriver.
 	 */
-	saved_id_table = udriver->id_table;
-	udriver->id_table = NULL;
+	udriver = kzalloc(sizeof(*udriver), GFP_KERNEL);
+	if (!udriver)
+		return -ENOMEM;
 
+	udriver->name = name;
 	udriver->no_dynamic_id = 1;
+	udriver->supports_autosuspend = 1;
+	udriver->suspend = usb_serial_suspend;
+	udriver->resume = usb_serial_resume;
+	udriver->probe = usb_serial_probe;
+	udriver->disconnect = usb_serial_disconnect;
+
+	/* we only set the reset_resume field if the serial_driver has one */
+	for (sd = serial_drivers; *sd; ++sd) {
+		if ((*sd)->reset_resume)
+			udriver->reset_resume = usb_serial_reset_resume;
+			break;
+	}
+
 	rc = usb_register(udriver);
 	if (rc)
 		return rc;
@@ -1408,8 +1440,8 @@ int usb_serial_register_drivers(struct usb_driver *udriver,
 			goto failed;
 	}
 
-	/* Now restore udriver's id_table and look for matches */
-	udriver->id_table = saved_id_table;
+	/* Now set udriver's id_table and look for matches */
+	udriver->id_table = id_table;
 	rc = driver_attach(&udriver->drvwrap.driver);
 	return 0;
 
@@ -1423,17 +1455,20 @@ EXPORT_SYMBOL_GPL(usb_serial_register_drivers);
 
 /**
  * usb_serial_deregister_drivers - deregister drivers for a usb-serial module
- * @udriver: usb_driver to unregister
  * @serial_drivers: NULL-terminated array of pointers to drivers to be deregistered
  *
- * Deregisters @udriver and all the drivers in the @serial_drivers array.
+ * Deregisters all the drivers in the @serial_drivers array and deregisters and
+ * frees the struct usb_driver that was created by the call to
+ * usb_serial_register_drivers().
  */
-void usb_serial_deregister_drivers(struct usb_driver *udriver,
-		struct usb_serial_driver * const serial_drivers[])
+void usb_serial_deregister_drivers(struct usb_serial_driver *const serial_drivers[])
 {
+	struct usb_driver *udriver = (*serial_drivers)->usb_driver;
+
 	for (; *serial_drivers; ++serial_drivers)
 		usb_serial_deregister(*serial_drivers);
 	usb_deregister(udriver);
+	kfree(udriver);
 }
 EXPORT_SYMBOL_GPL(usb_serial_deregister_drivers);
 
diff --git a/drivers/usb/serial/usb_debug.c b/drivers/usb/serial/usb_debug.c
index e3e8995a473..5760f97ee50 100644
--- a/drivers/usb/serial/usb_debug.c
+++ b/drivers/usb/serial/usb_debug.c
@@ -35,13 +35,6 @@ static const struct usb_device_id id_table[] = {
 };
 MODULE_DEVICE_TABLE(usb, id_table);
 
-static struct usb_driver debug_driver = {
-	.name =		"debug",
-	.probe =	usb_serial_probe,
-	.disconnect =	usb_serial_disconnect,
-	.id_table =	id_table,
-};
-
 /* This HW really does not support a serial break, so one will be
  * emulated when ever the break state is set to true.
  */
@@ -83,5 +76,5 @@ static struct usb_serial_driver * const serial_drivers[] = {
 	&debug_device, NULL
 };
 
-module_usb_serial_driver(debug_driver, serial_drivers);
+module_usb_serial_driver(serial_drivers, id_table);
 MODULE_LICENSE("GPL");
diff --git a/drivers/usb/serial/usb_wwan.c b/drivers/usb/serial/usb_wwan.c
index c88657dd31c..f35971dff4a 100644
--- a/drivers/usb/serial/usb_wwan.c
+++ b/drivers/usb/serial/usb_wwan.c
@@ -43,11 +43,8 @@ void usb_wwan_dtr_rts(struct usb_serial_port *port, int on)
 {
 	struct usb_serial *serial = port->serial;
 	struct usb_wwan_port_private *portdata;
-
 	struct usb_wwan_intf_private *intfdata;
 
-	dbg("%s", __func__);
-
 	intfdata = port->serial->private;
 
 	if (!intfdata->send_setup)
@@ -69,8 +66,6 @@ void usb_wwan_set_termios(struct tty_struct *tty,
 {
 	struct usb_wwan_intf_private *intfdata = port->serial->private;
 
-	dbg("%s", __func__);
-
 	/* Doesn't support option setting */
 	tty_termios_copy_hw(tty->termios, old_termios);
 
@@ -286,8 +281,6 @@ static void usb_wwan_indat_callback(struct urb *urb)
 	unsigned char *data = urb->transfer_buffer;
 	int status = urb->status;
 
-	dbg("%s: %p", __func__, urb);
-
 	endpoint = usb_pipeendpoint(urb->pipe);
 	port = urb->context;
 
@@ -307,20 +300,17 @@ static void usb_wwan_indat_callback(struct urb *urb)
 		}
 
 		/* Resubmit urb so we continue receiving */
-		if (status != -ESHUTDOWN) {
-			err = usb_submit_urb(urb, GFP_ATOMIC);
-			if (err) {
-				if (err != -EPERM) {
-					printk(KERN_ERR "%s: resubmit read urb failed. "
-						"(%d)", __func__, err);
-					/* busy also in error unless we are killed */
-					usb_mark_last_busy(port->serial->dev);
-				}
-			} else {
+		err = usb_submit_urb(urb, GFP_ATOMIC);
+		if (err) {
+			if (err != -EPERM) {
+				printk(KERN_ERR "%s: resubmit read urb failed. "
+					"(%d)", __func__, err);
+				/* busy also in error unless we are killed */
 				usb_mark_last_busy(port->serial->dev);
 			}
+		} else {
+			usb_mark_last_busy(port->serial->dev);
 		}
-
 	}
 }
 
@@ -331,8 +321,6 @@ static void usb_wwan_outdat_callback(struct urb *urb)
 	struct usb_wwan_intf_private *intfdata;
 	int i;
 
-	dbg("%s", __func__);
-
 	port = urb->context;
 	intfdata = port->serial->private;
 
@@ -406,8 +394,6 @@ int usb_wwan_open(struct tty_struct *tty, struct usb_serial_port *port)
 	portdata = usb_get_serial_port_data(port);
 	intfdata = serial->private;
 
-	dbg("%s", __func__);
-
 	/* Start reading from the IN endpoint */
 	for (i = 0; i < N_IN_URB; i++) {
 		urb = portdata->in_urbs[i];
@@ -441,7 +427,6 @@ void usb_wwan_close(struct usb_serial_port *port)
 	struct usb_wwan_port_private *portdata;
 	struct usb_wwan_intf_private *intfdata = port->serial->private;
 
-	dbg("%s", __func__);
 	portdata = usb_get_serial_port_data(port);
 
 	if (serial->dev) {
@@ -492,8 +477,6 @@ static void usb_wwan_setup_urbs(struct usb_serial *serial)
 	struct usb_serial_port *port;
 	struct usb_wwan_port_private *portdata;
 
-	dbg("%s", __func__);
-
 	for (i = 0; i < serial->num_ports; i++) {
 		port = serial->port[i];
 		portdata = usb_get_serial_port_data(port);
@@ -534,8 +517,6 @@ int usb_wwan_startup(struct usb_serial *serial)
 	struct usb_wwan_port_private *portdata;
 	u8 *buffer;
 
-	dbg("%s", __func__);
-
 	/* Now setup per port private data */
 	for (i = 0; i < serial->num_ports; i++) {
 		port = serial->port[i];
@@ -603,8 +584,6 @@ static void stop_read_write_urbs(struct usb_serial *serial)
 
 void usb_wwan_disconnect(struct usb_serial *serial)
 {
-	dbg("%s", __func__);
-
 	stop_read_write_urbs(serial);
 }
 EXPORT_SYMBOL(usb_wwan_disconnect);
@@ -615,8 +594,6 @@ void usb_wwan_release(struct usb_serial *serial)
 	struct usb_serial_port *port;
 	struct usb_wwan_port_private *portdata;
 
-	dbg("%s", __func__);
-
 	/* Now free them */
 	for (i = 0; i < serial->num_ports; ++i) {
 		port = serial->port[i];
@@ -649,8 +626,6 @@ int usb_wwan_suspend(struct usb_serial *serial, pm_message_t message)
 	struct usb_wwan_intf_private *intfdata = serial->private;
 	int b;
 
-	dbg("%s entered", __func__);
-
 	if (PMSG_IS_AUTO(message)) {
 		spin_lock_irq(&intfdata->susp_lock);
 		b = intfdata->in_flight;
@@ -714,7 +689,6 @@ int usb_wwan_resume(struct usb_serial *serial)
 	struct urb *urb;
 	int err = 0;
 
-	dbg("%s entered", __func__);
 	/* get the interrupt URBs resubmitted unconditionally */
 	for (i = 0; i < serial->num_ports; i++) {
 		port = serial->port[i];
@@ -725,8 +699,8 @@ int usb_wwan_resume(struct usb_serial *serial)
 		err = usb_submit_urb(port->interrupt_in_urb, GFP_NOIO);
 		dbg("Submitted interrupt URB for port %d (result %d)", i, err);
 		if (err < 0) {
-			err("%s: Error %d for interrupt URB of port%d",
-			    __func__, err, i);
+			dev_err(&port->dev, "%s: Error %d for interrupt URB\n",
+				__func__, err);
 			goto err_out;
 		}
 	}
@@ -747,8 +721,8 @@ int usb_wwan_resume(struct usb_serial *serial)
 			urb = portdata->in_urbs[j];
 			err = usb_submit_urb(urb, GFP_ATOMIC);
 			if (err < 0) {
-				err("%s: Error %d for bulk URB %d",
-				    __func__, err, i);
+				dev_err(&port->dev, "%s: Error %d for bulk URB %d\n",
+					__func__, err, i);
 				spin_unlock_irq(&intfdata->susp_lock);
 				goto err_out;
 			}
diff --git a/drivers/usb/serial/visor.c b/drivers/usb/serial/visor.c
index 71d696474f2..f253c91383d 100644
--- a/drivers/usb/serial/visor.c
+++ b/drivers/usb/serial/visor.c
@@ -53,8 +53,6 @@ static int palm_os_4_probe(struct usb_serial *serial,
 
 /* Parameters that may be passed into the module. */
 static bool debug;
-static __u16 vendor;
-static __u16 product;
 
 static struct usb_device_id id_table [] = {
 	{ USB_DEVICE(HANDSPRING_VENDOR_ID, HANDSPRING_VISOR_ID),
@@ -115,14 +113,12 @@ static struct usb_device_id id_table [] = {
 		.driver_info = (kernel_ulong_t)&palm_os_4_probe },
 	{ USB_DEVICE(FOSSIL_VENDOR_ID, FOSSIL_ABACUS_ID),
 		.driver_info = (kernel_ulong_t)&palm_os_4_probe },
-	{ },					/* optional parameter entry */
 	{ }					/* Terminating entry */
 };
 
 static struct usb_device_id clie_id_5_table [] = {
 	{ USB_DEVICE(SONY_VENDOR_ID, SONY_CLIE_UX50_ID),
 		.driver_info = (kernel_ulong_t)&palm_os_4_probe },
-	{ },					/* optional parameter entry */
 	{ }					/* Terminating entry */
 };
 
@@ -162,19 +158,11 @@ static struct usb_device_id id_table_combined [] = {
 	{ USB_DEVICE(ACEECA_VENDOR_ID, ACEECA_MEZ1000_ID) },
 	{ USB_DEVICE(KYOCERA_VENDOR_ID, KYOCERA_7135_ID) },
 	{ USB_DEVICE(FOSSIL_VENDOR_ID, FOSSIL_ABACUS_ID) },
-	{ },					/* optional parameter entry */
 	{ }					/* Terminating entry */
 };
 
 MODULE_DEVICE_TABLE(usb, id_table_combined);
 
-static struct usb_driver visor_driver = {
-	.name =		"visor",
-	.probe =	usb_serial_probe,
-	.disconnect =	usb_serial_disconnect,
-	.id_table =	id_table_combined,
-};
-
 /* All of the device info needed for the Handspring Visor,
    and Palm 4.0 devices */
 static struct usb_serial_driver handspring_device = {
@@ -244,8 +232,6 @@ static int visor_open(struct tty_struct *tty, struct usb_serial_port *port)
 {
 	int result = 0;
 
-	dbg("%s - port %d", __func__, port->number);
-
 	if (!port->read_urb) {
 		/* this is needed for some brain dead Sony devices */
 		dev_err(&port->dev, "Device lied about number of ports, please use a lower one.\n");
@@ -258,7 +244,7 @@ static int visor_open(struct tty_struct *tty, struct usb_serial_port *port)
 		goto exit;
 
 	if (port->interrupt_in_urb) {
-		dbg("%s - adding interrupt input for treo", __func__);
+		dev_dbg(&port->dev, "adding interrupt input for treo\n");
 		result = usb_submit_urb(port->interrupt_in_urb, GFP_KERNEL);
 		if (result)
 			dev_err(&port->dev,
@@ -274,8 +260,6 @@ static void visor_close(struct usb_serial_port *port)
 {
 	unsigned char *transfer_buffer;
 
-	dbg("%s - port %d", __func__, port->number);
-
 	/* shutdown our urbs */
 	usb_serial_generic_close(port);
 	usb_kill_urb(port->interrupt_in_urb);
@@ -310,12 +294,12 @@ static void visor_read_int_callback(struct urb *urb)
 	case -ENOENT:
 	case -ESHUTDOWN:
 		/* this urb is terminated, clean up */
-		dbg("%s - urb shutting down with status: %d",
-		    __func__, status);
+		dev_dbg(&port->dev, "%s - urb shutting down with status: %d\n",
+			__func__, status);
 		return;
 	default:
-		dbg("%s - nonzero urb status received: %d",
-		    __func__, status);
+		dev_dbg(&port->dev, "%s - nonzero urb status received: %d\n",
+			__func__, status);
 		goto exit;
 	}
 
@@ -348,8 +332,6 @@ static int palm_os_3_probe(struct usb_serial *serial,
 	int i;
 	int num_ports = 0;
 
-	dbg("%s", __func__);
-
 	transfer_buffer = kmalloc(sizeof(*connection_info), GFP_KERNEL);
 	if (!transfer_buffer) {
 		dev_err(dev, "%s - kmalloc(%Zd) failed.\n", __func__,
@@ -445,8 +427,6 @@ static int palm_os_4_probe(struct usb_serial *serial,
 	unsigned char *transfer_buffer;
 	int retval;
 
-	dbg("%s", __func__);
-
 	transfer_buffer =  kmalloc(sizeof(*connection_info), GFP_KERNEL);
 	if (!transfer_buffer) {
 		dev_err(dev, "%s - kmalloc(%Zd) failed.\n", __func__,
@@ -478,8 +458,6 @@ static int visor_probe(struct usb_serial *serial,
 	int (*startup)(struct usb_serial *serial,
 					const struct usb_device_id *id);
 
-	dbg("%s", __func__);
-
 	/*
 	 * some Samsung Android phones in modem mode have the same ID
 	 * as SPH-I500, but they are ACM devices, so dont bind to them
@@ -521,8 +499,6 @@ static int clie_3_5_startup(struct usb_serial *serial)
 	int result;
 	u8 *data;
 
-	dbg("%s", __func__);
-
 	data = kmalloc(1, GFP_KERNEL);
 	if (!data)
 		return -ENOMEM;
@@ -585,8 +561,6 @@ static int treo_attach(struct usb_serial *serial)
 		(serial->num_interrupt_in == 0))
 		return 0;
 
-	dbg("%s", __func__);
-
 	/*
 	* It appears that Treos and Kyoceras want to use the
 	* 1st bulk in endpoint to communicate with the 2nd bulk out endpoint,
@@ -622,8 +596,6 @@ static int clie_5_attach(struct usb_serial *serial)
 	unsigned int pipe;
 	int j;
 
-	dbg("%s", __func__);
-
 	/* TH55 registers 2 ports.
 	   Communication in from the UX50/TH55 uses bulk_in_endpointAddress
 	   from port 0. Communication out to the UX50/TH55 uses
@@ -648,59 +620,7 @@ static int clie_5_attach(struct usb_serial *serial)
 	return 0;
 }
 
-static int __init visor_init(void)
-{
-	int i, retval;
-	/* Only if parameters were passed to us */
-	if (vendor > 0 && product > 0) {
-		struct usb_device_id usb_dev_temp[] = {
-			{
-				USB_DEVICE(vendor, product),
-				.driver_info =
-					(kernel_ulong_t) &palm_os_4_probe
-			}
-		};
-
-		/* Find the last entry in id_table */
-		for (i = 0;; i++) {
-			if (id_table[i].idVendor == 0) {
-				id_table[i] = usb_dev_temp[0];
-				break;
-			}
-		}
-		/* Find the last entry in id_table_combined */
-		for (i = 0;; i++) {
-			if (id_table_combined[i].idVendor == 0) {
-				id_table_combined[i] = usb_dev_temp[0];
-				break;
-			}
-		}
-		printk(KERN_INFO KBUILD_MODNAME
-		       ": Untested USB device specified at time of module insertion\n");
-		printk(KERN_INFO KBUILD_MODNAME
-		       ": Warning: This is not guaranteed to work\n");
-		printk(KERN_INFO KBUILD_MODNAME
-		       ": Using a newer kernel is preferred to this method\n");
-		printk(KERN_INFO KBUILD_MODNAME
-		       ": Adding Palm OS protocol 4.x support for unknown device: 0x%x/0x%x\n",
-			vendor, product);
-	}
-
-	retval = usb_serial_register_drivers(&visor_driver, serial_drivers);
-	if (retval == 0)
-		printk(KERN_INFO KBUILD_MODNAME ": " DRIVER_DESC "\n");
-	return retval;
-}
-
-
-static void __exit visor_exit (void)
-{
-	usb_serial_deregister_drivers(&visor_driver, serial_drivers);
-}
-
-
-module_init(visor_init);
-module_exit(visor_exit);
+module_usb_serial_driver(serial_drivers, id_table_combined);
 
 MODULE_AUTHOR(DRIVER_AUTHOR);
 MODULE_DESCRIPTION(DRIVER_DESC);
@@ -708,9 +628,3 @@ MODULE_LICENSE("GPL");
 
 module_param(debug, bool, S_IRUGO | S_IWUSR);
 MODULE_PARM_DESC(debug, "Debug enabled or not");
-
-module_param(vendor, ushort, 0);
-MODULE_PARM_DESC(vendor, "User specified vendor ID");
-module_param(product, ushort, 0);
-MODULE_PARM_DESC(product, "User specified product ID");
-
diff --git a/drivers/usb/serial/vivopay-serial.c b/drivers/usb/serial/vivopay-serial.c
index 078f338b5fe..0c0aa876c20 100644
--- a/drivers/usb/serial/vivopay-serial.c
+++ b/drivers/usb/serial/vivopay-serial.c
@@ -25,13 +25,6 @@ static struct usb_device_id id_table [] = {
 
 MODULE_DEVICE_TABLE(usb, id_table);
 
-static struct usb_driver vivopay_serial_driver = {
-	.name =			"vivopay-serial",
-	.probe =		usb_serial_probe,
-	.disconnect =		usb_serial_disconnect,
-	.id_table =		id_table,
-};
-
 static struct usb_serial_driver vivopay_serial_device = {
 	.driver = {
 		.owner =	THIS_MODULE,
@@ -45,7 +38,7 @@ static struct usb_serial_driver * const serial_drivers[] = {
 	&vivopay_serial_device, NULL
 };
 
-module_usb_serial_driver(vivopay_serial_driver, serial_drivers);
+module_usb_serial_driver(serial_drivers, id_table);
 
 MODULE_AUTHOR("Forest Bond <forest.bond@outpostembedded.com>");
 MODULE_DESCRIPTION(DRIVER_DESC);
diff --git a/drivers/usb/serial/whiteheat.c b/drivers/usb/serial/whiteheat.c
index 407e23c8794..473635e7f5d 100644
--- a/drivers/usb/serial/whiteheat.c
+++ b/drivers/usb/serial/whiteheat.c
@@ -45,7 +45,6 @@ static bool debug;
 /*
  * Version Information
  */
-#define DRIVER_VERSION "v2.0"
 #define DRIVER_AUTHOR "Greg Kroah-Hartman <greg@kroah.com>, Stuart MacDonald <stuartm@connecttech.com>"
 #define DRIVER_DESC "USB ConnectTech WhiteHEAT driver"
 
@@ -78,12 +77,6 @@ static const struct usb_device_id id_table_combined[] = {
 
 MODULE_DEVICE_TABLE(usb, id_table_combined);
 
-static struct usb_driver whiteheat_driver = {
-	.name =		"whiteheat",
-	.probe =	usb_serial_probe,
-	.disconnect =	usb_serial_disconnect,
-	.id_table =	id_table_combined,
-};
 
 /* function prototypes for the Connect Tech WhiteHEAT prerenumeration device */
 static int  whiteheat_firmware_download(struct usb_serial *serial,
@@ -96,10 +89,6 @@ static void whiteheat_release(struct usb_serial *serial);
 static int  whiteheat_open(struct tty_struct *tty,
 			struct usb_serial_port *port);
 static void whiteheat_close(struct usb_serial_port *port);
-static int  whiteheat_write(struct tty_struct *tty,
-			struct usb_serial_port *port,
-			const unsigned char *buf, int count);
-static int  whiteheat_write_room(struct tty_struct *tty);
 static int  whiteheat_ioctl(struct tty_struct *tty,
 			unsigned int cmd, unsigned long arg);
 static void whiteheat_set_termios(struct tty_struct *tty,
@@ -108,11 +97,6 @@ static int  whiteheat_tiocmget(struct tty_struct *tty);
 static int  whiteheat_tiocmset(struct tty_struct *tty,
 			unsigned int set, unsigned int clear);
 static void whiteheat_break_ctl(struct tty_struct *tty, int break_state);
-static int  whiteheat_chars_in_buffer(struct tty_struct *tty);
-static void whiteheat_throttle(struct tty_struct *tty);
-static void whiteheat_unthrottle(struct tty_struct *tty);
-static void whiteheat_read_callback(struct urb *urb);
-static void whiteheat_write_callback(struct urb *urb);
 
 static struct usb_serial_driver whiteheat_fake_device = {
 	.driver = {
@@ -138,18 +122,13 @@ static struct usb_serial_driver whiteheat_device = {
 	.release =		whiteheat_release,
 	.open =			whiteheat_open,
 	.close =		whiteheat_close,
-	.write =		whiteheat_write,
-	.write_room =		whiteheat_write_room,
 	.ioctl =		whiteheat_ioctl,
 	.set_termios =		whiteheat_set_termios,
 	.break_ctl =		whiteheat_break_ctl,
 	.tiocmget =		whiteheat_tiocmget,
 	.tiocmset =		whiteheat_tiocmset,
-	.chars_in_buffer =	whiteheat_chars_in_buffer,
-	.throttle =		whiteheat_throttle,
-	.unthrottle =		whiteheat_unthrottle,
-	.read_bulk_callback =	whiteheat_read_callback,
-	.write_bulk_callback =	whiteheat_write_callback,
+	.throttle =		usb_serial_generic_throttle,
+	.unthrottle =		usb_serial_generic_unthrottle,
 };
 
 static struct usb_serial_driver * const serial_drivers[] = {
@@ -166,29 +145,8 @@ struct whiteheat_command_private {
 	__u8			result_buffer[64];
 };
 
-
-#define THROTTLED		0x01
-#define ACTUALLY_THROTTLED	0x02
-
-static int urb_pool_size = 8;
-
-struct whiteheat_urb_wrap {
-	struct list_head	list;
-	struct urb		*urb;
-};
-
 struct whiteheat_private {
-	spinlock_t		lock;
-	__u8			flags;
 	__u8			mcr;		/* FIXME: no locking on mcr */
-	struct list_head	rx_urbs_free;
-	struct list_head	rx_urbs_submitted;
-	struct list_head	rx_urb_q;
-	struct work_struct	rx_work;
-	struct usb_serial_port	*port;
-	struct list_head	tx_urbs_free;
-	struct list_head	tx_urbs_submitted;
-	struct mutex		deathwarrant;
 };
 
 
@@ -198,12 +156,6 @@ static void stop_command_port(struct usb_serial *serial);
 static void command_port_write_callback(struct urb *urb);
 static void command_port_read_callback(struct urb *urb);
 
-static int start_port_read(struct usb_serial_port *port);
-static struct whiteheat_urb_wrap *urb_to_wrap(struct urb *urb,
-						struct list_head *head);
-static struct list_head *list_first(struct list_head *head);
-static void rx_data_softint(struct work_struct *work);
-
 static int firm_send_command(struct usb_serial_port *port, __u8 command,
 						__u8 *data, __u8 datasize);
 static int firm_open(struct usb_serial_port *port);
@@ -247,8 +199,6 @@ static int whiteheat_firmware_download(struct usb_serial *serial,
 	const struct firmware *loader_fw = NULL, *firmware_fw = NULL;
 	const struct ihex_binrec *record;
 
-	dbg("%s", __func__);
-
 	if (request_ihex_firmware(&firmware_fw, "whiteheat.fw",
 				  &serial->dev->dev)) {
 		dev_err(&serial->dev->dev,
@@ -349,11 +299,6 @@ static int whiteheat_attach(struct usb_serial *serial)
 	__u8 *command;
 	__u8 *result;
 	int i;
-	int j;
-	struct urb *urb;
-	int buf_size;
-	struct whiteheat_urb_wrap *wrap;
-	struct list_head *tmp;
 
 	command_port = serial->port[COMMAND_PORT];
 
@@ -408,8 +353,8 @@ static int whiteheat_attach(struct usb_serial *serial)
 
 	hw_info = (struct whiteheat_hw_info *)&result[1];
 
-	dev_info(&serial->dev->dev, "%s: Driver %s: Firmware v%d.%02d\n",
-		 serial->type->description, DRIVER_VERSION,
+	dev_info(&serial->dev->dev, "%s: Firmware v%d.%02d\n",
+		 serial->type->description,
 		 hw_info->sw_major_rev, hw_info->sw_minor_rev);
 
 	for (i = 0; i < serial->num_ports; i++) {
@@ -423,72 +368,7 @@ static int whiteheat_attach(struct usb_serial *serial)
 			goto no_private;
 		}
 
-		spin_lock_init(&info->lock);
-		mutex_init(&info->deathwarrant);
-		info->flags = 0;
 		info->mcr = 0;
-		INIT_WORK(&info->rx_work, rx_data_softint);
-		info->port = port;
-
-		INIT_LIST_HEAD(&info->rx_urbs_free);
-		INIT_LIST_HEAD(&info->rx_urbs_submitted);
-		INIT_LIST_HEAD(&info->rx_urb_q);
-		INIT_LIST_HEAD(&info->tx_urbs_free);
-		INIT_LIST_HEAD(&info->tx_urbs_submitted);
-
-		for (j = 0; j < urb_pool_size; j++) {
-			urb = usb_alloc_urb(0, GFP_KERNEL);
-			if (!urb) {
-				dev_err(&port->dev, "No free urbs available\n");
-				goto no_rx_urb;
-			}
-			buf_size = port->read_urb->transfer_buffer_length;
-			urb->transfer_buffer = kmalloc(buf_size, GFP_KERNEL);
-			if (!urb->transfer_buffer) {
-				dev_err(&port->dev,
-					"Couldn't allocate urb buffer\n");
-				goto no_rx_buf;
-			}
-			wrap = kmalloc(sizeof(*wrap), GFP_KERNEL);
-			if (!wrap) {
-				dev_err(&port->dev,
-					"Couldn't allocate urb wrapper\n");
-				goto no_rx_wrap;
-			}
-			usb_fill_bulk_urb(urb, serial->dev,
-					usb_rcvbulkpipe(serial->dev,
-						port->bulk_in_endpointAddress),
-					urb->transfer_buffer, buf_size,
-					whiteheat_read_callback, port);
-			wrap->urb = urb;
-			list_add(&wrap->list, &info->rx_urbs_free);
-
-			urb = usb_alloc_urb(0, GFP_KERNEL);
-			if (!urb) {
-				dev_err(&port->dev, "No free urbs available\n");
-				goto no_tx_urb;
-			}
-			buf_size = port->write_urb->transfer_buffer_length;
-			urb->transfer_buffer = kmalloc(buf_size, GFP_KERNEL);
-			if (!urb->transfer_buffer) {
-				dev_err(&port->dev,
-					"Couldn't allocate urb buffer\n");
-				goto no_tx_buf;
-			}
-			wrap = kmalloc(sizeof(*wrap), GFP_KERNEL);
-			if (!wrap) {
-				dev_err(&port->dev,
-					"Couldn't allocate urb wrapper\n");
-				goto no_tx_wrap;
-			}
-			usb_fill_bulk_urb(urb, serial->dev,
-					usb_sndbulkpipe(serial->dev,
-						port->bulk_out_endpointAddress),
-					urb->transfer_buffer, buf_size,
-					whiteheat_write_callback, port);
-			wrap->urb = urb;
-			list_add(&wrap->list, &info->tx_urbs_free);
-		}
 
 		usb_set_serial_port_data(port, info);
 	}
@@ -531,29 +411,6 @@ no_command_private:
 	for (i = serial->num_ports - 1; i >= 0; i--) {
 		port = serial->port[i];
 		info = usb_get_serial_port_data(port);
-		for (j = urb_pool_size - 1; j >= 0; j--) {
-			tmp = list_first(&info->tx_urbs_free);
-			list_del(tmp);
-			wrap = list_entry(tmp, struct whiteheat_urb_wrap, list);
-			urb = wrap->urb;
-			kfree(wrap);
-no_tx_wrap:
-			kfree(urb->transfer_buffer);
-no_tx_buf:
-			usb_free_urb(urb);
-no_tx_urb:
-			tmp = list_first(&info->rx_urbs_free);
-			list_del(tmp);
-			wrap = list_entry(tmp, struct whiteheat_urb_wrap, list);
-			urb = wrap->urb;
-			kfree(wrap);
-no_rx_wrap:
-			kfree(urb->transfer_buffer);
-no_rx_buf:
-			usb_free_urb(urb);
-no_rx_urb:
-			;
-		}
 		kfree(info);
 no_private:
 		;
@@ -569,56 +426,27 @@ no_command_buffer:
 static void whiteheat_release(struct usb_serial *serial)
 {
 	struct usb_serial_port *command_port;
-	struct usb_serial_port *port;
 	struct whiteheat_private *info;
-	struct whiteheat_urb_wrap *wrap;
-	struct urb *urb;
-	struct list_head *tmp;
-	struct list_head *tmp2;
 	int i;
 
-	dbg("%s", __func__);
-
 	/* free up our private data for our command port */
 	command_port = serial->port[COMMAND_PORT];
 	kfree(usb_get_serial_port_data(command_port));
 
 	for (i = 0; i < serial->num_ports; i++) {
-		port = serial->port[i];
-		info = usb_get_serial_port_data(port);
-		list_for_each_safe(tmp, tmp2, &info->rx_urbs_free) {
-			list_del(tmp);
-			wrap = list_entry(tmp, struct whiteheat_urb_wrap, list);
-			urb = wrap->urb;
-			kfree(wrap);
-			kfree(urb->transfer_buffer);
-			usb_free_urb(urb);
-		}
-		list_for_each_safe(tmp, tmp2, &info->tx_urbs_free) {
-			list_del(tmp);
-			wrap = list_entry(tmp, struct whiteheat_urb_wrap, list);
-			urb = wrap->urb;
-			kfree(wrap);
-			kfree(urb->transfer_buffer);
-			usb_free_urb(urb);
-		}
+		info = usb_get_serial_port_data(serial->port[i]);
 		kfree(info);
 	}
 }
 
 static int whiteheat_open(struct tty_struct *tty, struct usb_serial_port *port)
 {
-	int		retval = 0;
-
-	dbg("%s - port %d", __func__, port->number);
+	int retval;
 
 	retval = start_command_port(port->serial);
 	if (retval)
 		goto exit;
 
-	if (tty)
-		tty->low_latency = 1;
-
 	/* send an open port command */
 	retval = firm_open(port);
 	if (retval) {
@@ -640,144 +468,25 @@ static int whiteheat_open(struct tty_struct *tty, struct usb_serial_port *port)
 	usb_clear_halt(port->serial->dev, port->read_urb->pipe);
 	usb_clear_halt(port->serial->dev, port->write_urb->pipe);
 
-	/* Start reading from the device */
-	retval = start_port_read(port);
+	retval = usb_serial_generic_open(tty, port);
 	if (retval) {
-		dev_err(&port->dev,
-			"%s - failed submitting read urb, error %d\n",
-			__func__, retval);
 		firm_close(port);
 		stop_command_port(port->serial);
 		goto exit;
 	}
-
 exit:
-	dbg("%s - exit, retval = %d", __func__, retval);
 	return retval;
 }
 
 
 static void whiteheat_close(struct usb_serial_port *port)
 {
-	struct whiteheat_private *info = usb_get_serial_port_data(port);
-	struct whiteheat_urb_wrap *wrap;
-	struct urb *urb;
-	struct list_head *tmp;
-	struct list_head *tmp2;
-
-	dbg("%s - port %d", __func__, port->number);
-
 	firm_report_tx_done(port);
 	firm_close(port);
 
-	/* shutdown our bulk reads and writes */
-	mutex_lock(&info->deathwarrant);
-	spin_lock_irq(&info->lock);
-	list_for_each_safe(tmp, tmp2, &info->rx_urbs_submitted) {
-		wrap = list_entry(tmp, struct whiteheat_urb_wrap, list);
-		urb = wrap->urb;
-		list_del(tmp);
-		spin_unlock_irq(&info->lock);
-		usb_kill_urb(urb);
-		spin_lock_irq(&info->lock);
-		list_add(tmp, &info->rx_urbs_free);
-	}
-	list_for_each_safe(tmp, tmp2, &info->rx_urb_q)
-		list_move(tmp, &info->rx_urbs_free);
-	list_for_each_safe(tmp, tmp2, &info->tx_urbs_submitted) {
-		wrap = list_entry(tmp, struct whiteheat_urb_wrap, list);
-		urb = wrap->urb;
-		list_del(tmp);
-		spin_unlock_irq(&info->lock);
-		usb_kill_urb(urb);
-		spin_lock_irq(&info->lock);
-		list_add(tmp, &info->tx_urbs_free);
-	}
-	spin_unlock_irq(&info->lock);
-	mutex_unlock(&info->deathwarrant);
-	stop_command_port(port->serial);
-}
-
-
-static int whiteheat_write(struct tty_struct *tty,
-	struct usb_serial_port *port, const unsigned char *buf, int count)
-{
-	struct whiteheat_private *info = usb_get_serial_port_data(port);
-	struct whiteheat_urb_wrap *wrap;
-	struct urb *urb;
-	int result;
-	int bytes;
-	int sent = 0;
-	unsigned long flags;
-	struct list_head *tmp;
-
-	dbg("%s - port %d", __func__, port->number);
-
-	if (count == 0) {
-		dbg("%s - write request of 0 bytes", __func__);
-		return (0);
-	}
-
-	while (count) {
-		spin_lock_irqsave(&info->lock, flags);
-		if (list_empty(&info->tx_urbs_free)) {
-			spin_unlock_irqrestore(&info->lock, flags);
-			break;
-		}
-		tmp = list_first(&info->tx_urbs_free);
-		list_del(tmp);
-		spin_unlock_irqrestore(&info->lock, flags);
-
-		wrap = list_entry(tmp, struct whiteheat_urb_wrap, list);
-		urb = wrap->urb;
-		bytes = (count > port->bulk_out_size) ?
-					port->bulk_out_size : count;
-		memcpy(urb->transfer_buffer, buf + sent, bytes);
-
-		usb_serial_debug_data(debug, &port->dev,
-				__func__, bytes, urb->transfer_buffer);
-
-		urb->transfer_buffer_length = bytes;
-		result = usb_submit_urb(urb, GFP_ATOMIC);
-		if (result) {
-			dev_err_console(port,
-				"%s - failed submitting write urb, error %d\n",
-				__func__, result);
-			sent = result;
-			spin_lock_irqsave(&info->lock, flags);
-			list_add(tmp, &info->tx_urbs_free);
-			spin_unlock_irqrestore(&info->lock, flags);
-			break;
-		} else {
-			sent += bytes;
-			count -= bytes;
-			spin_lock_irqsave(&info->lock, flags);
-			list_add(tmp, &info->tx_urbs_submitted);
-			spin_unlock_irqrestore(&info->lock, flags);
-		}
-	}
-
-	return sent;
-}
-
-static int whiteheat_write_room(struct tty_struct *tty)
-{
-	struct usb_serial_port *port = tty->driver_data;
-	struct whiteheat_private *info = usb_get_serial_port_data(port);
-	struct list_head *tmp;
-	int room = 0;
-	unsigned long flags;
-
-	dbg("%s - port %d", __func__, port->number);
+	usb_serial_generic_close(port);
 
-	spin_lock_irqsave(&info->lock, flags);
-	list_for_each(tmp, &info->tx_urbs_free)
-		room++;
-	spin_unlock_irqrestore(&info->lock, flags);
-	room *= port->bulk_out_size;
-
-	dbg("%s - returns %d", __func__, room);
-	return (room);
+	stop_command_port(port->serial);
 }
 
 static int whiteheat_tiocmget(struct tty_struct *tty)
@@ -786,8 +495,6 @@ static int whiteheat_tiocmget(struct tty_struct *tty)
 	struct whiteheat_private *info = usb_get_serial_port_data(port);
 	unsigned int modem_signals = 0;
 
-	dbg("%s - port %d", __func__, port->number);
-
 	firm_get_dtr_rts(port);
 	if (info->mcr & UART_MCR_DTR)
 		modem_signals |= TIOCM_DTR;
@@ -803,8 +510,6 @@ static int whiteheat_tiocmset(struct tty_struct *tty,
 	struct usb_serial_port *port = tty->driver_data;
 	struct whiteheat_private *info = usb_get_serial_port_data(port);
 
-	dbg("%s - port %d", __func__, port->number);
-
 	if (set & TIOCM_RTS)
 		info->mcr |= UART_MCR_RTS;
 	if (set & TIOCM_DTR)
@@ -837,7 +542,7 @@ static int whiteheat_ioctl(struct tty_struct *tty,
 		serstruct.line = port->serial->minor;
 		serstruct.port = port->number;
 		serstruct.flags = ASYNC_SKIP_TEST | ASYNC_AUTO_IRQ;
-		serstruct.xmit_fifo_size = port->bulk_out_size;
+		serstruct.xmit_fifo_size = kfifo_size(&port->write_fifo);
 		serstruct.custom_divisor = 0;
 		serstruct.baud_base = 460800;
 		serstruct.close_delay = CLOSING_DELAY;
@@ -867,60 +572,6 @@ static void whiteheat_break_ctl(struct tty_struct *tty, int break_state)
 }
 
 
-static int whiteheat_chars_in_buffer(struct tty_struct *tty)
-{
-	struct usb_serial_port *port = tty->driver_data;
-	struct whiteheat_private *info = usb_get_serial_port_data(port);
-	struct list_head *tmp;
-	struct whiteheat_urb_wrap *wrap;
-	int chars = 0;
-	unsigned long flags;
-
-	dbg("%s - port %d", __func__, port->number);
-
-	spin_lock_irqsave(&info->lock, flags);
-	list_for_each(tmp, &info->tx_urbs_submitted) {
-		wrap = list_entry(tmp, struct whiteheat_urb_wrap, list);
-		chars += wrap->urb->transfer_buffer_length;
-	}
-	spin_unlock_irqrestore(&info->lock, flags);
-
-	dbg("%s - returns %d", __func__, chars);
-	return chars;
-}
-
-
-static void whiteheat_throttle(struct tty_struct *tty)
-{
-	struct usb_serial_port *port = tty->driver_data;
-	struct whiteheat_private *info = usb_get_serial_port_data(port);
-
-	dbg("%s - port %d", __func__, port->number);
-
-	spin_lock_irq(&info->lock);
-	info->flags |= THROTTLED;
-	spin_unlock_irq(&info->lock);
-}
-
-
-static void whiteheat_unthrottle(struct tty_struct *tty)
-{
-	struct usb_serial_port *port = tty->driver_data;
-	struct whiteheat_private *info = usb_get_serial_port_data(port);
-	int actually_throttled;
-
-	dbg("%s - port %d", __func__, port->number);
-
-	spin_lock_irq(&info->lock);
-	actually_throttled = info->flags & ACTUALLY_THROTTLED;
-	info->flags &= ~(THROTTLED | ACTUALLY_THROTTLED);
-	spin_unlock_irq(&info->lock);
-
-	if (actually_throttled)
-		rx_data_softint(&info->rx_work);
-}
-
-
 /*****************************************************************************
  * Connect Tech's White Heat callback routines
  *****************************************************************************/
@@ -928,8 +579,6 @@ static void command_port_write_callback(struct urb *urb)
 {
 	int status = urb->status;
 
-	dbg("%s", __func__);
-
 	if (status) {
 		dbg("nonzero urb status: %d", status);
 		return;
@@ -945,8 +594,6 @@ static void command_port_read_callback(struct urb *urb)
 	unsigned char *data = urb->transfer_buffer;
 	int result;
 
-	dbg("%s", __func__);
-
 	command_info = usb_get_serial_port_data(command_port);
 	if (!command_info) {
 		dbg("%s - command_info is NULL, exiting.", __func__);
@@ -989,80 +636,6 @@ static void command_port_read_callback(struct urb *urb)
 }
 
 
-static void whiteheat_read_callback(struct urb *urb)
-{
-	struct usb_serial_port *port = urb->context;
-	struct whiteheat_urb_wrap *wrap;
-	unsigned char *data = urb->transfer_buffer;
-	struct whiteheat_private *info = usb_get_serial_port_data(port);
-	int status = urb->status;
-
-	dbg("%s - port %d", __func__, port->number);
-
-	spin_lock(&info->lock);
-	wrap = urb_to_wrap(urb, &info->rx_urbs_submitted);
-	if (!wrap) {
-		spin_unlock(&info->lock);
-		dev_err(&port->dev, "%s - Not my urb!\n", __func__);
-		return;
-	}
-	list_del(&wrap->list);
-	spin_unlock(&info->lock);
-
-	if (status) {
-		dbg("%s - nonzero read bulk status received: %d",
-		    __func__, status);
-		spin_lock(&info->lock);
-		list_add(&wrap->list, &info->rx_urbs_free);
-		spin_unlock(&info->lock);
-		return;
-	}
-
-	usb_serial_debug_data(debug, &port->dev,
-				__func__, urb->actual_length, data);
-
-	spin_lock(&info->lock);
-	list_add_tail(&wrap->list, &info->rx_urb_q);
-	if (info->flags & THROTTLED) {
-		info->flags |= ACTUALLY_THROTTLED;
-		spin_unlock(&info->lock);
-		return;
-	}
-	spin_unlock(&info->lock);
-
-	schedule_work(&info->rx_work);
-}
-
-
-static void whiteheat_write_callback(struct urb *urb)
-{
-	struct usb_serial_port *port = urb->context;
-	struct whiteheat_private *info = usb_get_serial_port_data(port);
-	struct whiteheat_urb_wrap *wrap;
-	int status = urb->status;
-
-	dbg("%s - port %d", __func__, port->number);
-
-	spin_lock(&info->lock);
-	wrap = urb_to_wrap(urb, &info->tx_urbs_submitted);
-	if (!wrap) {
-		spin_unlock(&info->lock);
-		dev_err(&port->dev, "%s - Not my urb!\n", __func__);
-		return;
-	}
-	list_move(&wrap->list, &info->tx_urbs_free);
-	spin_unlock(&info->lock);
-
-	if (status) {
-		dbg("%s - nonzero write bulk status received: %d",
-		    __func__, status);
-		return;
-	}
-
-	usb_serial_port_softint(port);
-}
-
-
 /*****************************************************************************
  * Connect Tech's White Heat firmware interface
  *****************************************************************************/
@@ -1337,124 +910,7 @@ static void stop_command_port(struct usb_serial *serial)
 	mutex_unlock(&command_info->mutex);
 }
 
-
-static int start_port_read(struct usb_serial_port *port)
-{
-	struct whiteheat_private *info = usb_get_serial_port_data(port);
-	struct whiteheat_urb_wrap *wrap;
-	struct urb *urb;
-	int retval = 0;
-	unsigned long flags;
-	struct list_head *tmp;
-	struct list_head *tmp2;
-
-	spin_lock_irqsave(&info->lock, flags);
-
-	list_for_each_safe(tmp, tmp2, &info->rx_urbs_free) {
-		list_del(tmp);
-		wrap = list_entry(tmp, struct whiteheat_urb_wrap, list);
-		urb = wrap->urb;
-		spin_unlock_irqrestore(&info->lock, flags);
-		retval = usb_submit_urb(urb, GFP_KERNEL);
-		if (retval) {
-			spin_lock_irqsave(&info->lock, flags);
-			list_add(tmp, &info->rx_urbs_free);
-			list_for_each_safe(tmp, tmp2, &info->rx_urbs_submitted) {
-				wrap = list_entry(tmp, struct whiteheat_urb_wrap, list);
-				urb = wrap->urb;
-				list_del(tmp);
-				spin_unlock_irqrestore(&info->lock, flags);
-				usb_kill_urb(urb);
-				spin_lock_irqsave(&info->lock, flags);
-				list_add(tmp, &info->rx_urbs_free);
-			}
-			break;
-		}
-		spin_lock_irqsave(&info->lock, flags);
-		list_add(tmp, &info->rx_urbs_submitted);
-	}
-
-	spin_unlock_irqrestore(&info->lock, flags);
-
-	return retval;
-}
-
-
-static struct whiteheat_urb_wrap *urb_to_wrap(struct urb *urb,
-						struct list_head *head)
-{
-	struct whiteheat_urb_wrap *wrap;
-	struct list_head *tmp;
-
-	list_for_each(tmp, head) {
-		wrap = list_entry(tmp, struct whiteheat_urb_wrap, list);
-		if (wrap->urb == urb)
-			return wrap;
-	}
-
-	return NULL;
-}
-
-
-static struct list_head *list_first(struct list_head *head)
-{
-	return head->next;
-}
-
-
-static void rx_data_softint(struct work_struct *work)
-{
-	struct whiteheat_private *info =
-		container_of(work, struct whiteheat_private, rx_work);
-	struct usb_serial_port *port = info->port;
-	struct tty_struct *tty = tty_port_tty_get(&port->port);
-	struct whiteheat_urb_wrap *wrap;
-	struct urb *urb;
-	unsigned long flags;
-	struct list_head *tmp;
-	struct list_head *tmp2;
-	int result;
-	int sent = 0;
-
-	spin_lock_irqsave(&info->lock, flags);
-	if (info->flags & THROTTLED) {
-		spin_unlock_irqrestore(&info->lock, flags);
-		goto out;
-	}
-
-	list_for_each_safe(tmp, tmp2, &info->rx_urb_q) {
-		list_del(tmp);
-		spin_unlock_irqrestore(&info->lock, flags);
-
-		wrap = list_entry(tmp, struct whiteheat_urb_wrap, list);
-		urb = wrap->urb;
-
-		if (tty && urb->actual_length)
-			sent += tty_insert_flip_string(tty,
-				urb->transfer_buffer, urb->actual_length);
-
-		result = usb_submit_urb(urb, GFP_ATOMIC);
-		if (result) {
-			dev_err(&port->dev,
-				"%s - failed resubmitting read urb, error %d\n",
-				__func__, result);
-			spin_lock_irqsave(&info->lock, flags);
-			list_add(tmp, &info->rx_urbs_free);
-			continue;
-		}
-
-		spin_lock_irqsave(&info->lock, flags);
-		list_add(tmp, &info->rx_urbs_submitted);
-	}
-	spin_unlock_irqrestore(&info->lock, flags);
-
-	if (sent)
-		tty_flip_buffer_push(tty);
-out:
-	tty_kref_put(tty);
-}
-
-module_usb_serial_driver(whiteheat_driver, serial_drivers);
+module_usb_serial_driver(serial_drivers, id_table_combined);
 
 MODULE_AUTHOR(DRIVER_AUTHOR);
 MODULE_DESCRIPTION(DRIVER_DESC);
@@ -1463,8 +919,5 @@ MODULE_LICENSE("GPL");
 MODULE_FIRMWARE("whiteheat.fw");
 MODULE_FIRMWARE("whiteheat_loader.fw");
 
-module_param(urb_pool_size, int, 0);
-MODULE_PARM_DESC(urb_pool_size, "Number of urbs to use for buffering");
-
 module_param(debug, bool, S_IRUGO | S_IWUSR);
 MODULE_PARM_DESC(debug, "Debug enabled or not");
diff --git a/drivers/usb/serial/zio.c b/drivers/usb/serial/zio.c
index 9d0bb3752cd..c043aa84a0b 100644
--- a/drivers/usb/serial/zio.c
+++ b/drivers/usb/serial/zio.c
@@ -22,13 +22,6 @@ static const struct usb_device_id id_table[] = {
 };
 MODULE_DEVICE_TABLE(usb, id_table);
 
-static struct usb_driver zio_driver = {
-	.name =		"zio",
-	.probe =	usb_serial_probe,
-	.disconnect =	usb_serial_disconnect,
-	.id_table =	id_table,
-};
-
 static struct usb_serial_driver zio_device = {
 	.driver = {
 		.owner =	THIS_MODULE,
@@ -42,5 +35,5 @@ static struct usb_serial_driver * const serial_drivers[] = {
 	&zio_device, NULL
 };
 
-module_usb_serial_driver(zio_driver, serial_drivers);
+module_usb_serial_driver(serial_drivers, id_table);
 MODULE_LICENSE("GPL");
diff --git a/drivers/usb/storage/ene_ub6250.c b/drivers/usb/storage/ene_ub6250.c
index e7e67810950..b28f2ad127d 100644
--- a/drivers/usb/storage/ene_ub6250.c
+++ b/drivers/usb/storage/ene_ub6250.c
@@ -1933,11 +1933,7 @@ static int ene_load_bincode(struct us_data *us, unsigned char flag)
 	kfree(buf);
 
 nofw:
-	if (sd_fw != NULL) {
-		release_firmware(sd_fw);
-		sd_fw = NULL;
-	}
-
+	release_firmware(sd_fw);
 	return result;
 }
 
diff --git a/drivers/usb/storage/unusual_devs.h b/drivers/usb/storage/unusual_devs.h
index 856ad92c40d..1719886bb9b 100644
--- a/drivers/usb/storage/unusual_devs.h
+++ b/drivers/usb/storage/unusual_devs.h
@@ -110,7 +110,7 @@ UNUSUAL_DEV(  0x040d, 0x6205, 0x0003, 0x0003,
 		USB_SC_DEVICE, USB_PR_DEVICE, NULL,
 		US_FL_IGNORE_RESIDUE ),
 
-/* Deduced by Jonathan Woithe <jwoithe@physics.adelaide.edu.au>
+/* Deduced by Jonathan Woithe <jwoithe@just42.net>
  * Entry needed for flags: US_FL_FIX_INQUIRY because initial inquiry message
  * always fails and confuses drive.
  */
@@ -1885,6 +1885,13 @@ UNUSUAL_DEV(  0x1652, 0x6600, 0x0201, 0x0201,
 		USB_SC_DEVICE, USB_PR_DEVICE, NULL,
 		US_FL_IGNORE_RESIDUE ),
 
+/* Reported by Jesse Feddema <jdfeddema@gmail.com> */
+UNUSUAL_DEV(  0x177f, 0x0400, 0x0000, 0x0000,
+		"Yarvik",
+		"PMP400",
+		USB_SC_DEVICE, USB_PR_DEVICE, NULL,
+		US_FL_BULK_IGNORE_TAG | US_FL_MAX_SECTORS_64 ),
+
 /* Reported by Hans de Goede <hdegoede@redhat.com>
  * These Appotech controllers are found in Picture Frames, they provide a
  * (buggy) emulation of a cdrom drive which contains the windows software
diff --git a/drivers/usb/storage/usb.c b/drivers/usb/storage/usb.c
index 2653e73db62..e23c30ab66d 100644
--- a/drivers/usb/storage/usb.c
+++ b/drivers/usb/storage/usb.c
@@ -121,7 +121,7 @@ MODULE_PARM_DESC(quirks, "supplemental list of device IDs and their quirks");
 }
 
 static struct us_unusual_dev us_unusual_dev_list[] = {
-#	include "unusual_devs.h" 
+#	include "unusual_devs.h"
 	{ }		/* Terminating entry */
 };
 
@@ -261,17 +261,17 @@ EXPORT_SYMBOL_GPL(usb_stor_post_reset);
 void fill_inquiry_response(struct us_data *us, unsigned char *data,
 		unsigned int data_len)
 {
-	if (data_len<36) // You lose.
+	if (data_len < 36) /* You lose. */
 		return;
 
 	memset(data+8, ' ', 28);
-	if(data[0]&0x20) { /* USB device currently not connected. Return
+	if (data[0]&0x20) { /* USB device currently not connected. Return
 			      peripheral qualifier 001b ("...however, the
 			      physical device is not currently connected
 			      to this logical unit") and leave vendor and
 			      product identification empty. ("If the target
 			      does store some of the INQUIRY data on the
-			      device, it may return zeros or ASCII spaces 
+			      device, it may return zeros or ASCII spaces
 			      (20h) in those fields until the data is
 			      available from the device."). */
 	} else {
@@ -298,7 +298,7 @@ static int usb_stor_control_thread(void * __us)
 	struct us_data *us = (struct us_data *)__us;
 	struct Scsi_Host *host = us_to_host(us);
 
-	for(;;) {
+	for (;;) {
 		US_DEBUGP("*** thread sleeping.\n");
 		if (wait_for_completion_interruptible(&us->cmnd_ready))
 			break;
@@ -327,7 +327,7 @@ static int usb_stor_control_thread(void * __us)
 
 		scsi_unlock(host);
 
-		/* reject the command if the direction indicator 
+		/* reject the command if the direction indicator
 		 * is UNKNOWN
 		 */
 		if (us->srb->sc_data_direction == DMA_BIDIRECTIONAL) {
@@ -338,7 +338,7 @@ static int usb_stor_control_thread(void * __us)
 		/* reject if target != 0 or if LUN is higher than
 		 * the maximum known LUN
 		 */
-		else if (us->srb->device->id && 
+		else if (us->srb->device->id &&
 				!(us->fflags & US_FL_SCM_MULT_TARG)) {
 			US_DEBUGP("Bad target number (%d:%d)\n",
 				  us->srb->device->id, us->srb->device->lun);
@@ -351,7 +351,7 @@ static int usb_stor_control_thread(void * __us)
 			us->srb->result = DID_BAD_TARGET << 16;
 		}
 
-		/* Handle those devices which need us to fake 
+		/* Handle those devices which need us to fake
 		 * their inquiry data */
 		else if ((us->srb->cmnd[0] == INQUIRY) &&
 			    (us->fflags & US_FL_FIX_INQUIRY)) {
@@ -376,7 +376,7 @@ static int usb_stor_control_thread(void * __us)
 
 		/* indicate that the command is done */
 		if (us->srb->result != DID_ABORT << 16) {
-			US_DEBUGP("scsi cmd done, result=0x%x\n", 
+			US_DEBUGP("scsi cmd done, result=0x%x\n",
 				   us->srb->result);
 			us->srb->scsi_done(us->srb);
 		} else {
@@ -414,7 +414,7 @@ SkipForAbort:
 	}
 	__set_current_state(TASK_RUNNING);
 	return 0;
-}	
+}
 
 /***********************************************************************
  * Device probing and disconnecting
@@ -732,7 +732,7 @@ static int get_pipes(struct us_data *us)
 	us->recv_ctrl_pipe = usb_rcvctrlpipe(us->pusb_dev, 0);
 	us->send_bulk_pipe = usb_sndbulkpipe(us->pusb_dev,
 		usb_endpoint_num(ep_out));
-	us->recv_bulk_pipe = usb_rcvbulkpipe(us->pusb_dev, 
+	us->recv_bulk_pipe = usb_rcvbulkpipe(us->pusb_dev,
 		usb_endpoint_num(ep_in));
 	if (ep_int) {
 		us->recv_intr_pipe = usb_rcvintpipe(us->pusb_dev,
diff --git a/drivers/usb/usb-skeleton.c b/drivers/usb/usb-skeleton.c
index b4a71679c93..0616f235bd6 100644
--- a/drivers/usb/usb-skeleton.c
+++ b/drivers/usb/usb-skeleton.c
@@ -93,8 +93,8 @@ static int skel_open(struct inode *inode, struct file *file)
 
 	interface = usb_find_interface(&skel_driver, subminor);
 	if (!interface) {
-		err("%s - error, can't find device for minor %d",
-		     __func__, subminor);
+		pr_err("%s - error, can't find device for minor %d\n",
+			__func__, subminor);
 		retval = -ENODEV;
 		goto exit;
 	}
@@ -179,8 +179,9 @@ static void skel_read_bulk_callback(struct urb *urb)
 		if (!(urb->status == -ENOENT ||
 		    urb->status == -ECONNRESET ||
 		    urb->status == -ESHUTDOWN))
-			err("%s - nonzero write bulk status received: %d",
-			    __func__, urb->status);
+			dev_err(&dev->interface->dev,
+				"%s - nonzero write bulk status received: %d\n",
+				__func__, urb->status);
 
 		dev->errors = urb->status;
 	} else {
@@ -213,7 +214,8 @@ static int skel_do_read_io(struct usb_skel *dev, size_t count)
 	/* do it */
 	rv = usb_submit_urb(dev->bulk_in_urb, GFP_KERNEL);
 	if (rv < 0) {
-		err("%s - failed submitting read urb, error %d",
+		dev_err(&dev->interface->dev,
+			"%s - failed submitting read urb, error %d\n",
 			__func__, rv);
 		dev->bulk_in_filled = 0;
 		rv = (rv == -ENOMEM) ? rv : -EIO;
@@ -364,8 +366,9 @@ static void skel_write_bulk_callback(struct urb *urb)
 		if (!(urb->status == -ENOENT ||
 		    urb->status == -ECONNRESET ||
 		    urb->status == -ESHUTDOWN))
-			err("%s - nonzero write bulk status received: %d",
-			    __func__, urb->status);
+			dev_err(&dev->interface->dev,
+				"%s - nonzero write bulk status received: %d\n",
+				__func__, urb->status);
 
 		spin_lock(&dev->err_lock);
 		dev->errors = urb->status;
@@ -459,8 +462,9 @@ static ssize_t skel_write(struct file *file, const char *user_buffer,
 	retval = usb_submit_urb(urb, GFP_KERNEL);
 	mutex_unlock(&dev->io_mutex);
 	if (retval) {
-		err("%s - failed submitting write urb, error %d", __func__,
-		    retval);
+		dev_err(&dev->interface->dev,
+			"%s - failed submitting write urb, error %d\n",
+			__func__, retval);
 		goto error_unanchor;
 	}
 
@@ -519,7 +523,7 @@ static int skel_probe(struct usb_interface *interface,
 	/* allocate memory for our device state and initialize it */
 	dev = kzalloc(sizeof(*dev), GFP_KERNEL);
 	if (!dev) {
-		err("Out of memory");
+		dev_err(&interface->dev, "Out of memory\n");
 		goto error;
 	}
 	kref_init(&dev->kref);
@@ -546,12 +550,14 @@ static int skel_probe(struct usb_interface *interface,
 			dev->bulk_in_endpointAddr = endpoint->bEndpointAddress;
 			dev->bulk_in_buffer = kmalloc(buffer_size, GFP_KERNEL);
 			if (!dev->bulk_in_buffer) {
-				err("Could not allocate bulk_in_buffer");
+				dev_err(&interface->dev,
+					"Could not allocate bulk_in_buffer\n");
 				goto error;
 			}
 			dev->bulk_in_urb = usb_alloc_urb(0, GFP_KERNEL);
 			if (!dev->bulk_in_urb) {
-				err("Could not allocate bulk_in_urb");
+				dev_err(&interface->dev,
+					"Could not allocate bulk_in_urb\n");
 				goto error;
 			}
 		}
@@ -563,7 +569,8 @@ static int skel_probe(struct usb_interface *interface,
 		}
 	}
 	if (!(dev->bulk_in_endpointAddr && dev->bulk_out_endpointAddr)) {
-		err("Could not find both bulk-in and bulk-out endpoints");
+		dev_err(&interface->dev,
+			"Could not find both bulk-in and bulk-out endpoints\n");
 		goto error;
 	}
 
@@ -574,7 +581,8 @@ static int skel_probe(struct usb_interface *interface,
 	retval = usb_register_dev(interface, &skel_class);
 	if (retval) {
 		/* something prevented us from registering this driver */
-		err("Not able to get a minor for this device.");
+		dev_err(&interface->dev,
+			"Not able to get a minor for this device.\n");
 		usb_set_intfdata(interface, NULL);
 		goto error;
 	}
diff --git a/drivers/vhost/net.c b/drivers/vhost/net.c
index 1f21d2a1e52..f82a7394756 100644
--- a/drivers/vhost/net.c
+++ b/drivers/vhost/net.c
@@ -24,6 +24,7 @@
 #include <linux/if_arp.h>
 #include <linux/if_tun.h>
 #include <linux/if_macvlan.h>
+#include <linux/if_vlan.h>
 
 #include <net/sock.h>
 
@@ -166,7 +167,7 @@ static void handle_tx(struct vhost_net *net)
 	if (wmem < sock->sk->sk_sndbuf / 2)
 		tx_poll_stop(net);
 	hdr_size = vq->vhost_hlen;
-	zcopy = vhost_sock_zcopy(sock);
+	zcopy = vq->ubufs;
 
 	for (;;) {
 		/* Release DMAs done buffers first */
@@ -257,7 +258,8 @@ static void handle_tx(struct vhost_net *net)
 					UIO_MAXIOV;
 			}
 			vhost_discard_vq_desc(vq, 1);
-			tx_poll_start(net, sock);
+			if (err == -EAGAIN || err == -ENOBUFS)
+				tx_poll_start(net, sock);
 			break;
 		}
 		if (err != len)
@@ -265,6 +267,8 @@ static void handle_tx(struct vhost_net *net)
 				 " len %d != %zd\n", err, len);
 		if (!zcopy)
 			vhost_add_used_and_signal(&net->dev, vq, head, 0);
+		else
+			vhost_zerocopy_signal_used(vq);
 		total_len += len;
 		if (unlikely(total_len >= VHOST_NET_WEIGHT)) {
 			vhost_poll_queue(&vq->poll);
@@ -283,8 +287,12 @@ static int peek_head_len(struct sock *sk)
 
 	spin_lock_irqsave(&sk->sk_receive_queue.lock, flags);
 	head = skb_peek(&sk->sk_receive_queue);
-	if (likely(head))
+	if (likely(head)) {
 		len = head->len;
+		if (vlan_tx_tag_present(head))
+			len += VLAN_HLEN;
+	}
+
 	spin_unlock_irqrestore(&sk->sk_receive_queue.lock, flags);
 	return len;
 }
diff --git a/drivers/vhost/vhost.c b/drivers/vhost/vhost.c
index 51e4c1eeec4..94dbd25caa3 100644
--- a/drivers/vhost/vhost.c
+++ b/drivers/vhost/vhost.c
@@ -1603,6 +1603,7 @@ void vhost_zerocopy_callback(struct ubuf_info *ubuf)
 	struct vhost_ubuf_ref *ubufs = ubuf->ctx;
 	struct vhost_virtqueue *vq = ubufs->vq;
 
+	vhost_poll_queue(&vq->poll);
 	/* set len = 1 to mark this desc buffers done DMA */
 	vq->heads[ubuf->desc].len = VHOST_DMA_DONE_LEN;
 	kref_put(&ubufs->kref, vhost_zerocopy_done_signal);
diff --git a/drivers/video/au1100fb.c b/drivers/video/au1100fb.c
index ffbce452546..fe3b6ec8712 100644
--- a/drivers/video/au1100fb.c
+++ b/drivers/video/au1100fb.c
@@ -536,7 +536,7 @@ static int __devinit au1100fb_drv_probe(struct platform_device *dev)
 	for (page = (unsigned long)fbdev->fb_mem;
 	     page < PAGE_ALIGN((unsigned long)fbdev->fb_mem + fbdev->fb_len);
 	     page += PAGE_SIZE) {
-#if CONFIG_DMA_NONCOHERENT
+#ifdef CONFIG_DMA_NONCOHERENT
 		SetPageReserved(virt_to_page(CAC_ADDR((void *)page)));
 #else
 		SetPageReserved(virt_to_page(page));
diff --git a/drivers/video/backlight/Kconfig b/drivers/video/backlight/Kconfig
index af16884491e..fa2b0375031 100644
--- a/drivers/video/backlight/Kconfig
+++ b/drivers/video/backlight/Kconfig
@@ -184,6 +184,18 @@ config BACKLIGHT_GENERIC
 	  known as the Corgi backlight driver. If you have a Sharp Zaurus
 	  SL-C7xx, SL-Cxx00 or SL-6000x say y.
 
+config BACKLIGHT_LM3533
+	tristate "Backlight Driver for LM3533"
+	depends on BACKLIGHT_CLASS_DEVICE
+	depends on MFD_LM3533
+	help
+	  Say Y to enable the backlight driver for National Semiconductor / TI
+	  LM3533 Lighting Power chips.
+
+	  The backlights can be controlled directly, through PWM input, or by
+	  the ambient-light-sensor interface. The chip supports 256 brightness
+	  levels.
+
 config BACKLIGHT_LOCOMO
 	tristate "Sharp LOCOMO LCD/Backlight Driver"
 	depends on SHARP_LOCOMO
diff --git a/drivers/video/backlight/Makefile b/drivers/video/backlight/Makefile
index 36855ae887d..a2ac9cfbaf6 100644
--- a/drivers/video/backlight/Makefile
+++ b/drivers/video/backlight/Makefile
@@ -21,6 +21,7 @@ obj-$(CONFIG_BACKLIGHT_EP93XX)	+= ep93xx_bl.o
 obj-$(CONFIG_BACKLIGHT_GENERIC)	+= generic_bl.o
 obj-$(CONFIG_BACKLIGHT_HP700)	+= jornada720_bl.o
 obj-$(CONFIG_BACKLIGHT_HP680)	+= hp680_bl.o
+obj-$(CONFIG_BACKLIGHT_LM3533)	+= lm3533_bl.o
 obj-$(CONFIG_BACKLIGHT_LOCOMO)	+= locomolcd.o
 obj-$(CONFIG_BACKLIGHT_LP855X)	+= lp855x_bl.o
 obj-$(CONFIG_BACKLIGHT_OMAP1)	+= omap1_bl.o
diff --git a/drivers/video/backlight/adp5520_bl.c b/drivers/video/backlight/adp5520_bl.c
index 4911ea7989c..df5db99af23 100644
--- a/drivers/video/backlight/adp5520_bl.c
+++ b/drivers/video/backlight/adp5520_bl.c
@@ -160,7 +160,7 @@ static ssize_t adp5520_store(struct device *dev, const char *buf,
 	unsigned long val;
 	int ret;
 
-	ret = strict_strtoul(buf, 10, &val);
+	ret = kstrtoul(buf, 10, &val);
 	if (ret)
 		return ret;
 
@@ -214,7 +214,7 @@ static ssize_t adp5520_bl_daylight_max_store(struct device *dev,
 	struct adp5520_bl *data = dev_get_drvdata(dev);
 	int ret;
 
-	ret = strict_strtoul(buf, 10, &data->cached_daylight_max);
+	ret = kstrtoul(buf, 10, &data->cached_daylight_max);
 	if (ret < 0)
 		return ret;
 
diff --git a/drivers/video/backlight/adp8860_bl.c b/drivers/video/backlight/adp8860_bl.c
index 550dbf0bb89..77d1fdba597 100644
--- a/drivers/video/backlight/adp8860_bl.c
+++ b/drivers/video/backlight/adp8860_bl.c
@@ -222,7 +222,8 @@ static int __devinit adp8860_led_probe(struct i2c_client *client)
 	struct led_info *cur_led;
 	int ret, i;
 
-	led = kzalloc(sizeof(*led) * pdata->num_leds, GFP_KERNEL);
+	led = devm_kzalloc(&client->dev, sizeof(*led) * pdata->num_leds,
+				GFP_KERNEL);
 	if (led == NULL) {
 		dev_err(&client->dev, "failed to alloc memory\n");
 		return -ENOMEM;
@@ -236,7 +237,7 @@ static int __devinit adp8860_led_probe(struct i2c_client *client)
 
 	if (ret) {
 		dev_err(&client->dev, "failed to write\n");
-		goto err_free;
+		return ret;
 	}
 
 	for (i = 0; i < pdata->num_leds; ++i) {
@@ -291,9 +292,6 @@ static int __devinit adp8860_led_probe(struct i2c_client *client)
 		cancel_work_sync(&led[i].work);
 	}
 
- err_free:
-	kfree(led);
-
 	return ret;
 }
 
@@ -309,7 +307,6 @@ static int __devexit adp8860_led_remove(struct i2c_client *client)
 		cancel_work_sync(&data->led[i].work);
 	}
 
-	kfree(data->led);
 	return 0;
 }
 #else
@@ -451,7 +448,7 @@ static ssize_t adp8860_store(struct device *dev, const char *buf,
 	unsigned long val;
 	int ret;
 
-	ret = strict_strtoul(buf, 10, &val);
+	ret = kstrtoul(buf, 10, &val);
 	if (ret)
 		return ret;
 
@@ -501,7 +498,7 @@ static ssize_t adp8860_bl_l1_daylight_max_store(struct device *dev,
 		struct device_attribute *attr, const char *buf, size_t count)
 {
 	struct adp8860_bl *data = dev_get_drvdata(dev);
-	int ret = strict_strtoul(buf, 10, &data->cached_daylight_max);
+	int ret = kstrtoul(buf, 10, &data->cached_daylight_max);
 	if (ret)
 		return ret;
 
@@ -608,7 +605,7 @@ static ssize_t adp8860_bl_ambient_light_zone_store(struct device *dev,
 	uint8_t reg_val;
 	int ret;
 
-	ret = strict_strtoul(buf, 10, &val);
+	ret = kstrtoul(buf, 10, &val);
 	if (ret)
 		return ret;
 
@@ -675,13 +672,13 @@ static int __devinit adp8860_probe(struct i2c_client *client,
 		return -EINVAL;
 	}
 
-	data = kzalloc(sizeof(*data), GFP_KERNEL);
+	data = devm_kzalloc(&client->dev, sizeof(*data), GFP_KERNEL);
 	if (data == NULL)
 		return -ENOMEM;
 
 	ret = adp8860_read(client, ADP8860_MFDVID, &reg_val);
 	if (ret < 0)
-		goto out2;
+		return ret;
 
 	switch (ADP8860_MANID(reg_val)) {
 	case ADP8863_MANUFID:
@@ -694,8 +691,7 @@ static int __devinit adp8860_probe(struct i2c_client *client,
 		break;
 	default:
 		dev_err(&client->dev, "failed to probe\n");
-		ret = -ENODEV;
-		goto out2;
+		return -ENODEV;
 	}
 
 	/* It's confirmed that the DEVID field is actually a REVID */
@@ -717,8 +713,7 @@ static int __devinit adp8860_probe(struct i2c_client *client,
 			&client->dev, data, &adp8860_bl_ops, &props);
 	if (IS_ERR(bl)) {
 		dev_err(&client->dev, "failed to register backlight\n");
-		ret = PTR_ERR(bl);
-		goto out2;
+		return PTR_ERR(bl);
 	}
 
 	bl->props.brightness = ADP8860_MAX_BRIGHTNESS;
@@ -756,8 +751,6 @@ out:
 			&adp8860_bl_attr_group);
 out1:
 	backlight_device_unregister(bl);
-out2:
-	kfree(data);
 
 	return ret;
 }
@@ -776,7 +769,6 @@ static int __devexit adp8860_remove(struct i2c_client *client)
 			&adp8860_bl_attr_group);
 
 	backlight_device_unregister(data->bl);
-	kfree(data);
 
 	return 0;
 }
diff --git a/drivers/video/backlight/adp8870_bl.c b/drivers/video/backlight/adp8870_bl.c
index 9be58c6f18f..edf7f91c8e6 100644
--- a/drivers/video/backlight/adp8870_bl.c
+++ b/drivers/video/backlight/adp8870_bl.c
@@ -244,8 +244,8 @@ static int __devinit adp8870_led_probe(struct i2c_client *client)
 	struct led_info *cur_led;
 	int ret, i;
 
-
-	led = kcalloc(pdata->num_leds, sizeof(*led), GFP_KERNEL);
+	led = devm_kzalloc(&client->dev, pdata->num_leds * sizeof(*led),
+				GFP_KERNEL);
 	if (led == NULL) {
 		dev_err(&client->dev, "failed to alloc memory\n");
 		return -ENOMEM;
@@ -253,17 +253,17 @@ static int __devinit adp8870_led_probe(struct i2c_client *client)
 
 	ret = adp8870_write(client, ADP8870_ISCLAW, pdata->led_fade_law);
 	if (ret)
-		goto err_free;
+		return ret;
 
 	ret = adp8870_write(client, ADP8870_ISCT1,
 			(pdata->led_on_time & 0x3) << 6);
 	if (ret)
-		goto err_free;
+		return ret;
 
 	ret = adp8870_write(client, ADP8870_ISCF,
 			FADE_VAL(pdata->led_fade_in, pdata->led_fade_out));
 	if (ret)
-		goto err_free;
+		return ret;
 
 	for (i = 0; i < pdata->num_leds; ++i) {
 		cur_led = &pdata->leds[i];
@@ -317,9 +317,6 @@ static int __devinit adp8870_led_probe(struct i2c_client *client)
 		cancel_work_sync(&led[i].work);
 	}
 
- err_free:
-	kfree(led);
-
 	return ret;
 }
 
@@ -335,7 +332,6 @@ static int __devexit adp8870_led_remove(struct i2c_client *client)
 		cancel_work_sync(&data->led[i].work);
 	}
 
-	kfree(data->led);
 	return 0;
 }
 #else
@@ -572,7 +568,7 @@ static ssize_t adp8870_store(struct device *dev, const char *buf,
 	unsigned long val;
 	int ret;
 
-	ret = strict_strtoul(buf, 10, &val);
+	ret = kstrtoul(buf, 10, &val);
 	if (ret)
 		return ret;
 
@@ -652,7 +648,7 @@ static ssize_t adp8870_bl_l1_daylight_max_store(struct device *dev,
 		struct device_attribute *attr, const char *buf, size_t count)
 {
 	struct adp8870_bl *data = dev_get_drvdata(dev);
-	int ret = strict_strtoul(buf, 10, &data->cached_daylight_max);
+	int ret = kstrtoul(buf, 10, &data->cached_daylight_max);
 	if (ret)
 		return ret;
 
@@ -794,7 +790,7 @@ static ssize_t adp8870_bl_ambient_light_zone_store(struct device *dev,
 	uint8_t reg_val;
 	int ret;
 
-	ret = strict_strtoul(buf, 10, &val);
+	ret = kstrtoul(buf, 10, &val);
 	if (ret)
 		return ret;
 
@@ -874,7 +870,7 @@ static int __devinit adp8870_probe(struct i2c_client *client,
 		return -ENODEV;
 	}
 
-	data = kzalloc(sizeof(*data), GFP_KERNEL);
+	data = devm_kzalloc(&client->dev, sizeof(*data), GFP_KERNEL);
 	if (data == NULL)
 		return -ENOMEM;
 
@@ -894,8 +890,7 @@ static int __devinit adp8870_probe(struct i2c_client *client,
 			&client->dev, data, &adp8870_bl_ops, &props);
 	if (IS_ERR(bl)) {
 		dev_err(&client->dev, "failed to register backlight\n");
-		ret = PTR_ERR(bl);
-		goto out2;
+		return PTR_ERR(bl);
 	}
 
 	data->bl = bl;
@@ -930,8 +925,6 @@ out:
 			&adp8870_bl_attr_group);
 out1:
 	backlight_device_unregister(bl);
-out2:
-	kfree(data);
 
 	return ret;
 }
@@ -950,7 +943,6 @@ static int __devexit adp8870_remove(struct i2c_client *client)
 			&adp8870_bl_attr_group);
 
 	backlight_device_unregister(data->bl);
-	kfree(data);
 
 	return 0;
 }
diff --git a/drivers/video/backlight/ams369fg06.c b/drivers/video/backlight/ams369fg06.c
index 7bdadc79011..3729238e709 100644
--- a/drivers/video/backlight/ams369fg06.c
+++ b/drivers/video/backlight/ams369fg06.c
@@ -482,7 +482,7 @@ static int __devinit ams369fg06_probe(struct spi_device *spi)
 	struct backlight_device *bd = NULL;
 	struct backlight_properties props;
 
-	lcd = kzalloc(sizeof(struct ams369fg06), GFP_KERNEL);
+	lcd = devm_kzalloc(&spi->dev, sizeof(struct ams369fg06), GFP_KERNEL);
 	if (!lcd)
 		return -ENOMEM;
 
@@ -492,7 +492,7 @@ static int __devinit ams369fg06_probe(struct spi_device *spi)
 	ret = spi_setup(spi);
 	if (ret < 0) {
 		dev_err(&spi->dev, "spi setup failed.\n");
-		goto out_free_lcd;
+		return ret;
 	}
 
 	lcd->spi = spi;
@@ -501,15 +501,13 @@ static int __devinit ams369fg06_probe(struct spi_device *spi)
 	lcd->lcd_pd = spi->dev.platform_data;
 	if (!lcd->lcd_pd) {
 		dev_err(&spi->dev, "platform data is NULL\n");
-		goto out_free_lcd;
+		return -EFAULT;
 	}
 
 	ld = lcd_device_register("ams369fg06", &spi->dev, lcd,
 		&ams369fg06_lcd_ops);
-	if (IS_ERR(ld)) {
-		ret = PTR_ERR(ld);
-		goto out_free_lcd;
-	}
+	if (IS_ERR(ld))
+		return PTR_ERR(ld);
 
 	lcd->ld = ld;
 
@@ -547,8 +545,6 @@ static int __devinit ams369fg06_probe(struct spi_device *spi)
 
 out_lcd_unregister:
 	lcd_device_unregister(ld);
-out_free_lcd:
-	kfree(lcd);
 	return ret;
 }
 
@@ -559,7 +555,6 @@ static int __devexit ams369fg06_remove(struct spi_device *spi)
 	ams369fg06_power(lcd, FB_BLANK_POWERDOWN);
 	backlight_device_unregister(lcd->bd);
 	lcd_device_unregister(lcd->ld);
-	kfree(lcd);
 
 	return 0;
 }
@@ -619,7 +614,6 @@ static void ams369fg06_shutdown(struct spi_device *spi)
 static struct spi_driver ams369fg06_driver = {
 	.driver = {
 		.name	= "ams369fg06",
-		.bus	= &spi_bus_type,
 		.owner	= THIS_MODULE,
 	},
 	.probe		= ams369fg06_probe,
diff --git a/drivers/video/backlight/apple_bl.c b/drivers/video/backlight/apple_bl.c
index a523b255e12..9dc73ac3709 100644
--- a/drivers/video/backlight/apple_bl.c
+++ b/drivers/video/backlight/apple_bl.c
@@ -16,6 +16,8 @@
  *  get at the firmware code in order to figure out what it's actually doing.
  */
 
+#define pr_fmt(fmt) KBUILD_MODNAME ": " fmt
+
 #include <linux/module.h>
 #include <linux/kernel.h>
 #include <linux/init.h>
@@ -25,6 +27,7 @@
 #include <linux/pci.h>
 #include <linux/acpi.h>
 #include <linux/atomic.h>
+#include <linux/apple_bl.h>
 
 static struct backlight_device *apple_backlight_device;
 
@@ -39,8 +42,6 @@ struct hw_data {
 
 static const struct hw_data *hw_data;
 
-#define DRIVER "apple_backlight: "
-
 /* Module parameters. */
 static int debug;
 module_param_named(debug, debug, int, 0644);
@@ -60,8 +61,7 @@ static int intel_chipset_send_intensity(struct backlight_device *bd)
 	int intensity = bd->props.brightness;
 
 	if (debug)
-		printk(KERN_DEBUG DRIVER "setting brightness to %d\n",
-		       intensity);
+		pr_debug("setting brightness to %d\n", intensity);
 
 	intel_chipset_set_brightness(intensity);
 	return 0;
@@ -76,8 +76,7 @@ static int intel_chipset_get_intensity(struct backlight_device *bd)
 	intensity = inb(0xb3) >> 4;
 
 	if (debug)
-		printk(KERN_DEBUG DRIVER "read brightness of %d\n",
-		       intensity);
+		pr_debug("read brightness of %d\n", intensity);
 
 	return intensity;
 }
@@ -107,8 +106,7 @@ static int nvidia_chipset_send_intensity(struct backlight_device *bd)
 	int intensity = bd->props.brightness;
 
 	if (debug)
-		printk(KERN_DEBUG DRIVER "setting brightness to %d\n",
-		       intensity);
+		pr_debug("setting brightness to %d\n", intensity);
 
 	nvidia_chipset_set_brightness(intensity);
 	return 0;
@@ -123,8 +121,7 @@ static int nvidia_chipset_get_intensity(struct backlight_device *bd)
 	intensity = inb(0x52f) >> 4;
 
 	if (debug)
-		printk(KERN_DEBUG DRIVER "read brightness of %d\n",
-		       intensity);
+		pr_debug("read brightness of %d\n", intensity);
 
 	return intensity;
 }
@@ -149,7 +146,7 @@ static int __devinit apple_bl_add(struct acpi_device *dev)
 	host = pci_get_bus_and_slot(0, 0);
 
 	if (!host) {
-		printk(KERN_ERR DRIVER "unable to find PCI host\n");
+		pr_err("unable to find PCI host\n");
 		return -ENODEV;
 	}
 
@@ -161,7 +158,7 @@ static int __devinit apple_bl_add(struct acpi_device *dev)
 	pci_dev_put(host);
 
 	if (!hw_data) {
-		printk(KERN_ERR DRIVER "unknown hardware\n");
+		pr_err("unknown hardware\n");
 		return -ENODEV;
 	}
 
diff --git a/drivers/video/backlight/backlight.c b/drivers/video/backlight/backlight.c
index bf5b1ece716..297db2fa91f 100644
--- a/drivers/video/backlight/backlight.c
+++ b/drivers/video/backlight/backlight.c
@@ -5,6 +5,8 @@
  *
  */
 
+#define pr_fmt(fmt) KBUILD_MODNAME ": " fmt
+
 #include <linux/module.h>
 #include <linux/init.h>
 #include <linux/device.h>
@@ -123,7 +125,7 @@ static ssize_t backlight_store_power(struct device *dev,
 	rc = -ENXIO;
 	mutex_lock(&bd->ops_lock);
 	if (bd->ops) {
-		pr_debug("backlight: set power to %lu\n", power);
+		pr_debug("set power to %lu\n", power);
 		if (bd->props.power != power) {
 			bd->props.power = power;
 			backlight_update_status(bd);
@@ -161,8 +163,7 @@ static ssize_t backlight_store_brightness(struct device *dev,
 		if (brightness > bd->props.max_brightness)
 			rc = -EINVAL;
 		else {
-			pr_debug("backlight: set brightness to %lu\n",
-				 brightness);
+			pr_debug("set brightness to %lu\n", brightness);
 			bd->props.brightness = brightness;
 			backlight_update_status(bd);
 			rc = count;
@@ -378,8 +379,8 @@ static int __init backlight_class_init(void)
 {
 	backlight_class = class_create(THIS_MODULE, "backlight");
 	if (IS_ERR(backlight_class)) {
-		printk(KERN_WARNING "Unable to create backlight class; errno = %ld\n",
-				PTR_ERR(backlight_class));
+		pr_warn("Unable to create backlight class; errno = %ld\n",
+			PTR_ERR(backlight_class));
 		return PTR_ERR(backlight_class);
 	}
 
diff --git a/drivers/video/backlight/corgi_lcd.c b/drivers/video/backlight/corgi_lcd.c
index 6dab13fe562..23d732677ba 100644
--- a/drivers/video/backlight/corgi_lcd.c
+++ b/drivers/video/backlight/corgi_lcd.c
@@ -544,7 +544,7 @@ static int __devinit corgi_lcd_probe(struct spi_device *spi)
 		return -EINVAL;
 	}
 
-	lcd = kzalloc(sizeof(struct corgi_lcd), GFP_KERNEL);
+	lcd = devm_kzalloc(&spi->dev, sizeof(struct corgi_lcd), GFP_KERNEL);
 	if (!lcd) {
 		dev_err(&spi->dev, "failed to allocate memory\n");
 		return -ENOMEM;
@@ -554,10 +554,9 @@ static int __devinit corgi_lcd_probe(struct spi_device *spi)
 
 	lcd->lcd_dev = lcd_device_register("corgi_lcd", &spi->dev,
 					lcd, &corgi_lcd_ops);
-	if (IS_ERR(lcd->lcd_dev)) {
-		ret = PTR_ERR(lcd->lcd_dev);
-		goto err_free_lcd;
-	}
+	if (IS_ERR(lcd->lcd_dev))
+		return PTR_ERR(lcd->lcd_dev);
+
 	lcd->power = FB_BLANK_POWERDOWN;
 	lcd->mode = (pdata) ? pdata->init_mode : CORGI_LCD_MODE_VGA;
 
@@ -591,8 +590,6 @@ err_unregister_bl:
 	backlight_device_unregister(lcd->bl_dev);
 err_unregister_lcd:
 	lcd_device_unregister(lcd->lcd_dev);
-err_free_lcd:
-	kfree(lcd);
 	return ret;
 }
 
@@ -613,7 +610,6 @@ static int __devexit corgi_lcd_remove(struct spi_device *spi)
 
 	corgi_lcd_set_power(lcd->lcd_dev, FB_BLANK_POWERDOWN);
 	lcd_device_unregister(lcd->lcd_dev);
-	kfree(lcd);
 
 	return 0;
 }
diff --git a/drivers/video/backlight/cr_bllcd.c b/drivers/video/backlight/cr_bllcd.c
index 22489eb5f3e..37bae801e23 100644
--- a/drivers/video/backlight/cr_bllcd.c
+++ b/drivers/video/backlight/cr_bllcd.c
@@ -27,6 +27,8 @@
  *   Alan Hourihane <alanh-at-tungstengraphics-dot-com>
  */
 
+#define pr_fmt(fmt) KBUILD_MODNAME ": " fmt
+
 #include <linux/module.h>
 #include <linux/kernel.h>
 #include <linux/init.h>
@@ -180,14 +182,13 @@ static int cr_backlight_probe(struct platform_device *pdev)
 	lpc_dev = pci_get_device(PCI_VENDOR_ID_INTEL,
 					CRVML_DEVICE_LPC, NULL);
 	if (!lpc_dev) {
-		printk("INTEL CARILLO RANCH LPC not found.\n");
+		pr_err("INTEL CARILLO RANCH LPC not found.\n");
 		return -ENODEV;
 	}
 
 	pci_read_config_byte(lpc_dev, CRVML_REG_GPIOEN, &dev_en);
 	if (!(dev_en & CRVML_GPIOEN_BIT)) {
-		printk(KERN_ERR
-		       "Carillo Ranch GPIO device was not enabled.\n");
+		pr_err("Carillo Ranch GPIO device was not enabled.\n");
 		pci_dev_put(lpc_dev);
 		return -ENODEV;
 	}
@@ -270,7 +271,7 @@ static int __init cr_backlight_init(void)
 		return PTR_ERR(crp);
 	}
 
-	printk("Carillo Ranch Backlight Driver Initialized.\n");
+	pr_info("Carillo Ranch Backlight Driver Initialized.\n");
 
 	return 0;
 }
diff --git a/drivers/video/backlight/da903x_bl.c b/drivers/video/backlight/da903x_bl.c
index 30e19681a30..573c7ece0fd 100644
--- a/drivers/video/backlight/da903x_bl.c
+++ b/drivers/video/backlight/da903x_bl.c
@@ -136,6 +136,7 @@ static int da903x_backlight_probe(struct platform_device *pdev)
 		da903x_write(data->da903x_dev, DA9034_WLED_CONTROL2,
 				DA9034_WLED_ISET(pdata->output_current));
 
+	memset(&props, 0, sizeof(props));
 	props.type = BACKLIGHT_RAW;
 	props.max_brightness = max_brightness;
 	bl = backlight_device_register(pdev->name, data->da903x_dev, data,
diff --git a/drivers/video/backlight/generic_bl.c b/drivers/video/backlight/generic_bl.c
index 9ce6170c186..8c660fcd250 100644
--- a/drivers/video/backlight/generic_bl.c
+++ b/drivers/video/backlight/generic_bl.c
@@ -9,6 +9,8 @@
  *
  */
 
+#define pr_fmt(fmt) KBUILD_MODNAME ": " fmt
+
 #include <linux/module.h>
 #include <linux/kernel.h>
 #include <linux/init.h>
@@ -106,7 +108,7 @@ static int genericbl_probe(struct platform_device *pdev)
 
 	generic_backlight_device = bd;
 
-	printk("Generic Backlight Driver Initialized.\n");
+	pr_info("Generic Backlight Driver Initialized.\n");
 	return 0;
 }
 
@@ -120,7 +122,7 @@ static int genericbl_remove(struct platform_device *pdev)
 
 	backlight_device_unregister(bd);
 
-	printk("Generic Backlight Driver Unloaded\n");
+	pr_info("Generic Backlight Driver Unloaded\n");
 	return 0;
 }
 
diff --git a/drivers/video/backlight/ili9320.c b/drivers/video/backlight/ili9320.c
index 5118a9f029a..6c9399341bc 100644
--- a/drivers/video/backlight/ili9320.c
+++ b/drivers/video/backlight/ili9320.c
@@ -220,7 +220,7 @@ int __devinit ili9320_probe_spi(struct spi_device *spi,
 
 	/* allocate and initialse our state */
 
-	ili = kzalloc(sizeof(struct ili9320), GFP_KERNEL);
+	ili = devm_kzalloc(&spi->dev, sizeof(struct ili9320), GFP_KERNEL);
 	if (ili == NULL) {
 		dev_err(dev, "no memory for device\n");
 		return -ENOMEM;
@@ -240,8 +240,7 @@ int __devinit ili9320_probe_spi(struct spi_device *spi,
 	lcd = lcd_device_register("ili9320", dev, ili, &ili9320_ops);
 	if (IS_ERR(lcd)) {
 		dev_err(dev, "failed to register lcd device\n");
-		ret = PTR_ERR(lcd);
-		goto err_free;
+		return PTR_ERR(lcd);
 	}
 
 	ili->lcd = lcd;
@@ -259,9 +258,6 @@ int __devinit ili9320_probe_spi(struct spi_device *spi,
  err_unregister:
 	lcd_device_unregister(lcd);
 
- err_free:
-	kfree(ili);
-
 	return ret;
 }
 
@@ -272,7 +268,6 @@ int __devexit ili9320_remove(struct ili9320 *ili)
 	ili9320_power(ili, FB_BLANK_POWERDOWN);
 
 	lcd_device_unregister(ili->lcd);
-	kfree(ili);
 
 	return 0;
 }
diff --git a/drivers/video/backlight/jornada720_bl.c b/drivers/video/backlight/jornada720_bl.c
index 2f8af5d786a..16f593b6442 100644
--- a/drivers/video/backlight/jornada720_bl.c
+++ b/drivers/video/backlight/jornada720_bl.c
@@ -9,6 +9,8 @@
  *
  */
 
+#define pr_fmt(fmt) KBUILD_MODNAME ": " fmt
+
 #include <linux/backlight.h>
 #include <linux/device.h>
 #include <linux/fb.h>
@@ -38,7 +40,7 @@ static int jornada_bl_get_brightness(struct backlight_device *bd)
 	ret = jornada_ssp_byte(GETBRIGHTNESS);
 
 	if (jornada_ssp_byte(GETBRIGHTNESS) != TXDUMMY) {
-		printk(KERN_ERR "bl : get brightness timeout\n");
+		pr_err("get brightness timeout\n");
 		jornada_ssp_end();
 		return -ETIMEDOUT;
 	} else /* exchange txdummy for value */
@@ -59,7 +61,7 @@ static int jornada_bl_update_status(struct backlight_device *bd)
 	if ((bd->props.power != FB_BLANK_UNBLANK) || (bd->props.fb_blank != FB_BLANK_UNBLANK)) {
 		ret = jornada_ssp_byte(BRIGHTNESSOFF);
 		if (ret != TXDUMMY) {
-			printk(KERN_INFO "bl : brightness off timeout\n");
+			pr_info("brightness off timeout\n");
 			/* turn off backlight */
 			PPSR &= ~PPC_LDD1;
 			PPDR |= PPC_LDD1;
@@ -70,7 +72,7 @@ static int jornada_bl_update_status(struct backlight_device *bd)
 
 		/* send command to our mcu */
 		if (jornada_ssp_byte(SETBRIGHTNESS) != TXDUMMY) {
-			printk(KERN_INFO "bl : failed to set brightness\n");
+			pr_info("failed to set brightness\n");
 			ret = -ETIMEDOUT;
 			goto out;
 		}
@@ -81,7 +83,7 @@ static int jornada_bl_update_status(struct backlight_device *bd)
 		   but due to physical layout it is equal to 0, so we simply
 		   invert the value (MAX VALUE - NEW VALUE). */
 		if (jornada_ssp_byte(BL_MAX_BRIGHT - bd->props.brightness) != TXDUMMY) {
-			printk(KERN_ERR "bl : set brightness failed\n");
+			pr_err("set brightness failed\n");
 			ret = -ETIMEDOUT;
 		}
 
@@ -113,7 +115,7 @@ static int jornada_bl_probe(struct platform_device *pdev)
 
 	if (IS_ERR(bd)) {
 		ret = PTR_ERR(bd);
-		printk(KERN_ERR "bl : failed to register device, err=%x\n", ret);
+		pr_err("failed to register device, err=%x\n", ret);
 		return ret;
 	}
 
@@ -125,7 +127,7 @@ static int jornada_bl_probe(struct platform_device *pdev)
 	jornada_bl_update_status(bd);
 
 	platform_set_drvdata(pdev, bd);
-	printk(KERN_INFO "HP Jornada 700 series backlight driver\n");
+	pr_info("HP Jornada 700 series backlight driver\n");
 
 	return 0;
 }
diff --git a/drivers/video/backlight/jornada720_lcd.c b/drivers/video/backlight/jornada720_lcd.c
index 22d231a17e3..635b30523fd 100644
--- a/drivers/video/backlight/jornada720_lcd.c
+++ b/drivers/video/backlight/jornada720_lcd.c
@@ -9,6 +9,8 @@
  *
  */
 
+#define pr_fmt(fmt) KBUILD_MODNAME ": " fmt
+
 #include <linux/device.h>
 #include <linux/fb.h>
 #include <linux/kernel.h>
@@ -44,7 +46,7 @@ static int jornada_lcd_get_contrast(struct lcd_device *dev)
 	jornada_ssp_start();
 
 	if (jornada_ssp_byte(GETCONTRAST) != TXDUMMY) {
-		printk(KERN_ERR "lcd: get contrast failed\n");
+		pr_err("get contrast failed\n");
 		jornada_ssp_end();
 		return -ETIMEDOUT;
 	} else {
@@ -65,7 +67,7 @@ static int jornada_lcd_set_contrast(struct lcd_device *dev, int value)
 
 	/* push the new value */
 	if (jornada_ssp_byte(value) != TXDUMMY) {
-		printk(KERN_ERR "lcd : set contrast failed\n");
+		pr_err("set contrast failed\n");
 		jornada_ssp_end();
 		return -ETIMEDOUT;
 	}
@@ -103,7 +105,7 @@ static int jornada_lcd_probe(struct platform_device *pdev)
 
 	if (IS_ERR(lcd_device)) {
 		ret = PTR_ERR(lcd_device);
-		printk(KERN_ERR "lcd : failed to register device\n");
+		pr_err("failed to register device\n");
 		return ret;
 	}
 
diff --git a/drivers/video/backlight/l4f00242t03.c b/drivers/video/backlight/l4f00242t03.c
index 6022b67285e..40f606a8609 100644
--- a/drivers/video/backlight/l4f00242t03.c
+++ b/drivers/video/backlight/l4f00242t03.c
@@ -11,6 +11,8 @@
  * published by the Free Software Foundation.
  */
 
+#define pr_fmt(fmt) KBUILD_MODNAME ": " fmt
+
 #include <linux/device.h>
 #include <linux/kernel.h>
 #include <linux/delay.h>
@@ -159,7 +161,8 @@ static int __devinit l4f00242t03_probe(struct spi_device *spi)
 		return -EINVAL;
 	}
 
-	priv = kzalloc(sizeof(struct l4f00242t03_priv), GFP_KERNEL);
+	priv = devm_kzalloc(&spi->dev, sizeof(struct l4f00242t03_priv),
+				GFP_KERNEL);
 
 	if (priv == NULL) {
 		dev_err(&spi->dev, "No memory for this device.\n");
@@ -177,7 +180,7 @@ static int __devinit l4f00242t03_probe(struct spi_device *spi)
 	if (ret) {
 		dev_err(&spi->dev,
 			"Unable to get the lcd l4f00242t03 reset gpio.\n");
-		goto err;
+		return ret;
 	}
 
 	ret = gpio_request_one(pdata->data_enable_gpio, GPIOF_OUT_INIT_LOW,
@@ -185,7 +188,7 @@ static int __devinit l4f00242t03_probe(struct spi_device *spi)
 	if (ret) {
 		dev_err(&spi->dev,
 			"Unable to get the lcd l4f00242t03 data en gpio.\n");
-		goto err2;
+		goto err;
 	}
 
 	priv->io_reg = regulator_get(&spi->dev, "vdd");
@@ -193,7 +196,7 @@ static int __devinit l4f00242t03_probe(struct spi_device *spi)
 		ret = PTR_ERR(priv->io_reg);
 		dev_err(&spi->dev, "%s: Unable to get the IO regulator\n",
 		       __func__);
-		goto err3;
+		goto err2;
 	}
 
 	priv->core_reg = regulator_get(&spi->dev, "vcore");
@@ -201,14 +204,14 @@ static int __devinit l4f00242t03_probe(struct spi_device *spi)
 		ret = PTR_ERR(priv->core_reg);
 		dev_err(&spi->dev, "%s: Unable to get the core regulator\n",
 		       __func__);
-		goto err4;
+		goto err3;
 	}
 
 	priv->ld = lcd_device_register("l4f00242t03",
 					&spi->dev, priv, &l4f_ops);
 	if (IS_ERR(priv->ld)) {
 		ret = PTR_ERR(priv->ld);
-		goto err5;
+		goto err4;
 	}
 
 	/* Init the LCD */
@@ -220,16 +223,14 @@ static int __devinit l4f00242t03_probe(struct spi_device *spi)
 
 	return 0;
 
-err5:
-	regulator_put(priv->core_reg);
 err4:
-	regulator_put(priv->io_reg);
+	regulator_put(priv->core_reg);
 err3:
-	gpio_free(pdata->data_enable_gpio);
+	regulator_put(priv->io_reg);
 err2:
-	gpio_free(pdata->reset_gpio);
+	gpio_free(pdata->data_enable_gpio);
 err:
-	kfree(priv);
+	gpio_free(pdata->reset_gpio);
 
 	return ret;
 }
@@ -250,8 +251,6 @@ static int __devexit l4f00242t03_remove(struct spi_device *spi)
 	regulator_put(priv->io_reg);
 	regulator_put(priv->core_reg);
 
-	kfree(priv);
-
 	return 0;
 }
 
diff --git a/drivers/video/backlight/lcd.c b/drivers/video/backlight/lcd.c
index 79c1b0d609a..a5d0d024bb9 100644
--- a/drivers/video/backlight/lcd.c
+++ b/drivers/video/backlight/lcd.c
@@ -5,6 +5,8 @@
  *
  */
 
+#define pr_fmt(fmt) KBUILD_MODNAME ": " fmt
+
 #include <linux/module.h>
 #include <linux/init.h>
 #include <linux/device.h>
@@ -32,6 +34,8 @@ static int fb_notifier_callback(struct notifier_block *self,
 	case FB_EVENT_BLANK:
 	case FB_EVENT_MODE_CHANGE:
 	case FB_EVENT_MODE_CHANGE_ALL:
+	case FB_EARLY_EVENT_BLANK:
+	case FB_R_EARLY_EVENT_BLANK:
 		break;
 	default:
 		return 0;
@@ -46,6 +50,14 @@ static int fb_notifier_callback(struct notifier_block *self,
 		if (event == FB_EVENT_BLANK) {
 			if (ld->ops->set_power)
 				ld->ops->set_power(ld, *(int *)evdata->data);
+		} else if (event == FB_EARLY_EVENT_BLANK) {
+			if (ld->ops->early_set_power)
+				ld->ops->early_set_power(ld,
+						*(int *)evdata->data);
+		} else if (event == FB_R_EARLY_EVENT_BLANK) {
+			if (ld->ops->r_early_set_power)
+				ld->ops->r_early_set_power(ld,
+						*(int *)evdata->data);
 		} else {
 			if (ld->ops->set_mode)
 				ld->ops->set_mode(ld, evdata->data);
@@ -106,7 +118,7 @@ static ssize_t lcd_store_power(struct device *dev,
 
 	mutex_lock(&ld->ops_lock);
 	if (ld->ops && ld->ops->set_power) {
-		pr_debug("lcd: set power to %lu\n", power);
+		pr_debug("set power to %lu\n", power);
 		ld->ops->set_power(ld, power);
 		rc = count;
 	}
@@ -142,7 +154,7 @@ static ssize_t lcd_store_contrast(struct device *dev,
 
 	mutex_lock(&ld->ops_lock);
 	if (ld->ops && ld->ops->set_contrast) {
-		pr_debug("lcd: set contrast to %lu\n", contrast);
+		pr_debug("set contrast to %lu\n", contrast);
 		ld->ops->set_contrast(ld, contrast);
 		rc = count;
 	}
@@ -253,8 +265,8 @@ static int __init lcd_class_init(void)
 {
 	lcd_class = class_create(THIS_MODULE, "lcd");
 	if (IS_ERR(lcd_class)) {
-		printk(KERN_WARNING "Unable to create backlight class; errno = %ld\n",
-				PTR_ERR(lcd_class));
+		pr_warn("Unable to create backlight class; errno = %ld\n",
+			PTR_ERR(lcd_class));
 		return PTR_ERR(lcd_class);
 	}
 
diff --git a/drivers/video/backlight/ld9040.c b/drivers/video/backlight/ld9040.c
index efd352be21a..58f517fb7d4 100644
--- a/drivers/video/backlight/ld9040.c
+++ b/drivers/video/backlight/ld9040.c
@@ -707,7 +707,7 @@ static int ld9040_probe(struct spi_device *spi)
 	struct backlight_device *bd = NULL;
 	struct backlight_properties props;
 
-	lcd = kzalloc(sizeof(struct ld9040), GFP_KERNEL);
+	lcd = devm_kzalloc(&spi->dev, sizeof(struct ld9040), GFP_KERNEL);
 	if (!lcd)
 		return -ENOMEM;
 
@@ -717,7 +717,7 @@ static int ld9040_probe(struct spi_device *spi)
 	ret = spi_setup(spi);
 	if (ret < 0) {
 		dev_err(&spi->dev, "spi setup failed.\n");
-		goto out_free_lcd;
+		return ret;
 	}
 
 	lcd->spi = spi;
@@ -726,7 +726,7 @@ static int ld9040_probe(struct spi_device *spi)
 	lcd->lcd_pd = spi->dev.platform_data;
 	if (!lcd->lcd_pd) {
 		dev_err(&spi->dev, "platform data is NULL.\n");
-		goto out_free_lcd;
+		return -EFAULT;
 	}
 
 	mutex_init(&lcd->lock);
@@ -734,13 +734,13 @@ static int ld9040_probe(struct spi_device *spi)
 	ret = regulator_bulk_get(lcd->dev, ARRAY_SIZE(supplies), supplies);
 	if (ret) {
 		dev_err(lcd->dev, "Failed to get regulators: %d\n", ret);
-		goto out_free_lcd;
+		return ret;
 	}
 
 	ld = lcd_device_register("ld9040", &spi->dev, lcd, &ld9040_lcd_ops);
 	if (IS_ERR(ld)) {
 		ret = PTR_ERR(ld);
-		goto out_free_lcd;
+		goto out_free_regulator;
 	}
 
 	lcd->ld = ld;
@@ -782,10 +782,9 @@ static int ld9040_probe(struct spi_device *spi)
 
 out_unregister_lcd:
 	lcd_device_unregister(lcd->ld);
-out_free_lcd:
+out_free_regulator:
 	regulator_bulk_free(ARRAY_SIZE(supplies), supplies);
 
-	kfree(lcd);
 	return ret;
 }
 
@@ -797,7 +796,6 @@ static int __devexit ld9040_remove(struct spi_device *spi)
 	backlight_device_unregister(lcd->bd);
 	lcd_device_unregister(lcd->ld);
 	regulator_bulk_free(ARRAY_SIZE(supplies), supplies);
-	kfree(lcd);
 
 	return 0;
 }
@@ -846,7 +844,6 @@ static void ld9040_shutdown(struct spi_device *spi)
 static struct spi_driver ld9040_driver = {
 	.driver = {
 		.name	= "ld9040",
-		.bus	= &spi_bus_type,
 		.owner	= THIS_MODULE,
 	},
 	.probe		= ld9040_probe,
diff --git a/drivers/video/backlight/lm3533_bl.c b/drivers/video/backlight/lm3533_bl.c
new file mode 100644
index 00000000000..bebeb63607d
--- /dev/null
+++ b/drivers/video/backlight/lm3533_bl.c
@@ -0,0 +1,423 @@
+/*
+ * lm3533-bl.c -- LM3533 Backlight driver
+ *
+ * Copyright (C) 2011-2012 Texas Instruments
+ *
+ * Author: Johan Hovold <jhovold@gmail.com>
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under  the terms of the GNU General  Public License as published by the
+ * Free Software Foundation;  either version 2 of the License, or (at your
+ * option) any later version.
+ */
+
+#include <linux/module.h>
+#include <linux/init.h>
+#include <linux/platform_device.h>
+#include <linux/backlight.h>
+#include <linux/fb.h>
+#include <linux/slab.h>
+
+#include <linux/mfd/lm3533.h>
+
+
+#define LM3533_HVCTRLBANK_COUNT		2
+#define LM3533_BL_MAX_BRIGHTNESS	255
+
+#define LM3533_REG_CTRLBANK_AB_BCONF	0x1a
+
+
+struct lm3533_bl {
+	struct lm3533 *lm3533;
+	struct lm3533_ctrlbank cb;
+	struct backlight_device *bd;
+	int id;
+};
+
+
+static inline int lm3533_bl_get_ctrlbank_id(struct lm3533_bl *bl)
+{
+	return bl->id;
+}
+
+static int lm3533_bl_update_status(struct backlight_device *bd)
+{
+	struct lm3533_bl *bl = bl_get_data(bd);
+	int brightness = bd->props.brightness;
+
+	if (bd->props.power != FB_BLANK_UNBLANK)
+		brightness = 0;
+	if (bd->props.fb_blank != FB_BLANK_UNBLANK)
+		brightness = 0;
+
+	return lm3533_ctrlbank_set_brightness(&bl->cb, (u8)brightness);
+}
+
+static int lm3533_bl_get_brightness(struct backlight_device *bd)
+{
+	struct lm3533_bl *bl = bl_get_data(bd);
+	u8 val;
+	int ret;
+
+	ret = lm3533_ctrlbank_get_brightness(&bl->cb, &val);
+	if (ret)
+		return ret;
+
+	return val;
+}
+
+static const struct backlight_ops lm3533_bl_ops = {
+	.get_brightness	= lm3533_bl_get_brightness,
+	.update_status	= lm3533_bl_update_status,
+};
+
+static ssize_t show_id(struct device *dev,
+				struct device_attribute *attr, char *buf)
+{
+	struct lm3533_bl *bl = dev_get_drvdata(dev);
+
+	return scnprintf(buf, PAGE_SIZE, "%d\n", bl->id);
+}
+
+static ssize_t show_als_channel(struct device *dev,
+				struct device_attribute *attr, char *buf)
+{
+	struct lm3533_bl *bl = dev_get_drvdata(dev);
+	unsigned channel = lm3533_bl_get_ctrlbank_id(bl);
+
+	return scnprintf(buf, PAGE_SIZE, "%u\n", channel);
+}
+
+static ssize_t show_als_en(struct device *dev,
+				struct device_attribute *attr, char *buf)
+{
+	struct lm3533_bl *bl = dev_get_drvdata(dev);
+	int ctrlbank = lm3533_bl_get_ctrlbank_id(bl);
+	u8 val;
+	u8 mask;
+	bool enable;
+	int ret;
+
+	ret = lm3533_read(bl->lm3533, LM3533_REG_CTRLBANK_AB_BCONF, &val);
+	if (ret)
+		return ret;
+
+	mask = 1 << (2 * ctrlbank);
+	enable = val & mask;
+
+	return scnprintf(buf, PAGE_SIZE, "%d\n", enable);
+}
+
+static ssize_t store_als_en(struct device *dev,
+					struct device_attribute *attr,
+					const char *buf, size_t len)
+{
+	struct lm3533_bl *bl = dev_get_drvdata(dev);
+	int ctrlbank = lm3533_bl_get_ctrlbank_id(bl);
+	int enable;
+	u8 val;
+	u8 mask;
+	int ret;
+
+	if (kstrtoint(buf, 0, &enable))
+		return -EINVAL;
+
+	mask = 1 << (2 * ctrlbank);
+
+	if (enable)
+		val = mask;
+	else
+		val = 0;
+
+	ret = lm3533_update(bl->lm3533, LM3533_REG_CTRLBANK_AB_BCONF, val,
+									mask);
+	if (ret)
+		return ret;
+
+	return len;
+}
+
+static ssize_t show_linear(struct device *dev,
+				struct device_attribute *attr, char *buf)
+{
+	struct lm3533_bl *bl = dev_get_drvdata(dev);
+	u8 val;
+	u8 mask;
+	int linear;
+	int ret;
+
+	ret = lm3533_read(bl->lm3533, LM3533_REG_CTRLBANK_AB_BCONF, &val);
+	if (ret)
+		return ret;
+
+	mask = 1 << (2 * lm3533_bl_get_ctrlbank_id(bl) + 1);
+
+	if (val & mask)
+		linear = 1;
+	else
+		linear = 0;
+
+	return scnprintf(buf, PAGE_SIZE, "%x\n", linear);
+}
+
+static ssize_t store_linear(struct device *dev,
+					struct device_attribute *attr,
+					const char *buf, size_t len)
+{
+	struct lm3533_bl *bl = dev_get_drvdata(dev);
+	unsigned long linear;
+	u8 mask;
+	u8 val;
+	int ret;
+
+	if (kstrtoul(buf, 0, &linear))
+		return -EINVAL;
+
+	mask = 1 << (2 * lm3533_bl_get_ctrlbank_id(bl) + 1);
+
+	if (linear)
+		val = mask;
+	else
+		val = 0;
+
+	ret = lm3533_update(bl->lm3533, LM3533_REG_CTRLBANK_AB_BCONF, val,
+									mask);
+	if (ret)
+		return ret;
+
+	return len;
+}
+
+static ssize_t show_pwm(struct device *dev,
+					struct device_attribute *attr,
+					char *buf)
+{
+	struct lm3533_bl *bl = dev_get_drvdata(dev);
+	u8 val;
+	int ret;
+
+	ret = lm3533_ctrlbank_get_pwm(&bl->cb, &val);
+	if (ret)
+		return ret;
+
+	return scnprintf(buf, PAGE_SIZE, "%u\n", val);
+}
+
+static ssize_t store_pwm(struct device *dev,
+					struct device_attribute *attr,
+					const char *buf, size_t len)
+{
+	struct lm3533_bl *bl = dev_get_drvdata(dev);
+	u8 val;
+	int ret;
+
+	if (kstrtou8(buf, 0, &val))
+		return -EINVAL;
+
+	ret = lm3533_ctrlbank_set_pwm(&bl->cb, val);
+	if (ret)
+		return ret;
+
+	return len;
+}
+
+static LM3533_ATTR_RO(als_channel);
+static LM3533_ATTR_RW(als_en);
+static LM3533_ATTR_RO(id);
+static LM3533_ATTR_RW(linear);
+static LM3533_ATTR_RW(pwm);
+
+static struct attribute *lm3533_bl_attributes[] = {
+	&dev_attr_als_channel.attr,
+	&dev_attr_als_en.attr,
+	&dev_attr_id.attr,
+	&dev_attr_linear.attr,
+	&dev_attr_pwm.attr,
+	NULL,
+};
+
+static umode_t lm3533_bl_attr_is_visible(struct kobject *kobj,
+					     struct attribute *attr, int n)
+{
+	struct device *dev = container_of(kobj, struct device, kobj);
+	struct lm3533_bl *bl = dev_get_drvdata(dev);
+	umode_t mode = attr->mode;
+
+	if (attr == &dev_attr_als_channel.attr ||
+					attr == &dev_attr_als_en.attr) {
+		if (!bl->lm3533->have_als)
+			mode = 0;
+	}
+
+	return mode;
+};
+
+static struct attribute_group lm3533_bl_attribute_group = {
+	.is_visible	= lm3533_bl_attr_is_visible,
+	.attrs		= lm3533_bl_attributes
+};
+
+static int __devinit lm3533_bl_setup(struct lm3533_bl *bl,
+					struct lm3533_bl_platform_data *pdata)
+{
+	int ret;
+
+	ret = lm3533_ctrlbank_set_max_current(&bl->cb, pdata->max_current);
+	if (ret)
+		return ret;
+
+	return lm3533_ctrlbank_set_pwm(&bl->cb, pdata->pwm);
+}
+
+static int __devinit lm3533_bl_probe(struct platform_device *pdev)
+{
+	struct lm3533 *lm3533;
+	struct lm3533_bl_platform_data *pdata;
+	struct lm3533_bl *bl;
+	struct backlight_device *bd;
+	struct backlight_properties props;
+	int ret;
+
+	dev_dbg(&pdev->dev, "%s\n", __func__);
+
+	lm3533 = dev_get_drvdata(pdev->dev.parent);
+	if (!lm3533)
+		return -EINVAL;
+
+	pdata = pdev->dev.platform_data;
+	if (!pdata) {
+		dev_err(&pdev->dev, "no platform data\n");
+		return -EINVAL;
+	}
+
+	if (pdev->id < 0 || pdev->id >= LM3533_HVCTRLBANK_COUNT) {
+		dev_err(&pdev->dev, "illegal backlight id %d\n", pdev->id);
+		return -EINVAL;
+	}
+
+	bl = kzalloc(sizeof(*bl), GFP_KERNEL);
+	if (!bl) {
+		dev_err(&pdev->dev,
+				"failed to allocate memory for backlight\n");
+		return -ENOMEM;
+	}
+
+	bl->lm3533 = lm3533;
+	bl->id = pdev->id;
+
+	bl->cb.lm3533 = lm3533;
+	bl->cb.id = lm3533_bl_get_ctrlbank_id(bl);
+	bl->cb.dev = NULL;			/* until registered */
+
+	memset(&props, 0, sizeof(props));
+	props.type = BACKLIGHT_RAW;
+	props.max_brightness = LM3533_BL_MAX_BRIGHTNESS;
+	props.brightness = pdata->default_brightness;
+	bd = backlight_device_register(pdata->name, pdev->dev.parent, bl,
+						&lm3533_bl_ops, &props);
+	if (IS_ERR(bd)) {
+		dev_err(&pdev->dev, "failed to register backlight device\n");
+		ret = PTR_ERR(bd);
+		goto err_free;
+	}
+
+	bl->bd = bd;
+	bl->cb.dev = &bl->bd->dev;
+
+	platform_set_drvdata(pdev, bl);
+
+	ret = sysfs_create_group(&bd->dev.kobj, &lm3533_bl_attribute_group);
+	if (ret < 0) {
+		dev_err(&pdev->dev, "failed to create sysfs attributes\n");
+		goto err_unregister;
+	}
+
+	backlight_update_status(bd);
+
+	ret = lm3533_bl_setup(bl, pdata);
+	if (ret)
+		goto err_sysfs_remove;
+
+	ret = lm3533_ctrlbank_enable(&bl->cb);
+	if (ret)
+		goto err_sysfs_remove;
+
+	return 0;
+
+err_sysfs_remove:
+	sysfs_remove_group(&bd->dev.kobj, &lm3533_bl_attribute_group);
+err_unregister:
+	backlight_device_unregister(bd);
+err_free:
+	kfree(bl);
+
+	return ret;
+}
+
+static int __devexit lm3533_bl_remove(struct platform_device *pdev)
+{
+	struct lm3533_bl *bl = platform_get_drvdata(pdev);
+	struct backlight_device *bd = bl->bd;
+
+	dev_dbg(&bd->dev, "%s\n", __func__);
+
+	bd->props.power = FB_BLANK_POWERDOWN;
+	bd->props.brightness = 0;
+
+	lm3533_ctrlbank_disable(&bl->cb);
+	sysfs_remove_group(&bd->dev.kobj, &lm3533_bl_attribute_group);
+	backlight_device_unregister(bd);
+	kfree(bl);
+
+	return 0;
+}
+
+#ifdef CONFIG_PM
+static int lm3533_bl_suspend(struct platform_device *pdev, pm_message_t state)
+{
+	struct lm3533_bl *bl = platform_get_drvdata(pdev);
+
+	dev_dbg(&pdev->dev, "%s\n", __func__);
+
+	return lm3533_ctrlbank_disable(&bl->cb);
+}
+
+static int lm3533_bl_resume(struct platform_device *pdev)
+{
+	struct lm3533_bl *bl = platform_get_drvdata(pdev);
+
+	dev_dbg(&pdev->dev, "%s\n", __func__);
+
+	return lm3533_ctrlbank_enable(&bl->cb);
+}
+#else
+#define lm3533_bl_suspend	NULL
+#define lm3533_bl_resume	NULL
+#endif
+
+static void lm3533_bl_shutdown(struct platform_device *pdev)
+{
+	struct lm3533_bl *bl = platform_get_drvdata(pdev);
+
+	dev_dbg(&pdev->dev, "%s\n", __func__);
+
+	lm3533_ctrlbank_disable(&bl->cb);
+}
+
+static struct platform_driver lm3533_bl_driver = {
+	.driver = {
+		.name	= "lm3533-backlight",
+		.owner	= THIS_MODULE,
+	},
+	.probe		= lm3533_bl_probe,
+	.remove		= __devexit_p(lm3533_bl_remove),
+	.shutdown	= lm3533_bl_shutdown,
+	.suspend	= lm3533_bl_suspend,
+	.resume		= lm3533_bl_resume,
+};
+module_platform_driver(lm3533_bl_driver);
+
+MODULE_AUTHOR("Johan Hovold <jhovold@gmail.com>");
+MODULE_DESCRIPTION("LM3533 Backlight driver");
+MODULE_LICENSE("GPL");
+MODULE_ALIAS("platform:lm3533-backlight");
diff --git a/drivers/video/backlight/lms283gf05.c b/drivers/video/backlight/lms283gf05.c
index 4161f9e3982..a9f2c36966f 100644
--- a/drivers/video/backlight/lms283gf05.c
+++ b/drivers/video/backlight/lms283gf05.c
@@ -168,7 +168,8 @@ static int __devinit lms283gf05_probe(struct spi_device *spi)
 			goto err;
 	}
 
-	st = kzalloc(sizeof(struct lms283gf05_state), GFP_KERNEL);
+	st = devm_kzalloc(&spi->dev, sizeof(struct lms283gf05_state),
+				GFP_KERNEL);
 	if (st == NULL) {
 		dev_err(&spi->dev, "No memory for device state\n");
 		ret = -ENOMEM;
@@ -178,7 +179,7 @@ static int __devinit lms283gf05_probe(struct spi_device *spi)
 	ld = lcd_device_register("lms283gf05", &spi->dev, st, &lms_ops);
 	if (IS_ERR(ld)) {
 		ret = PTR_ERR(ld);
-		goto err2;
+		goto err;
 	}
 
 	st->spi = spi;
@@ -193,8 +194,6 @@ static int __devinit lms283gf05_probe(struct spi_device *spi)
 
 	return 0;
 
-err2:
-	kfree(st);
 err:
 	if (pdata != NULL)
 		gpio_free(pdata->reset_gpio);
@@ -212,8 +211,6 @@ static int __devexit lms283gf05_remove(struct spi_device *spi)
 	if (pdata != NULL)
 		gpio_free(pdata->reset_gpio);
 
-	kfree(st);
-
 	return 0;
 }
 
diff --git a/drivers/video/backlight/ltv350qv.c b/drivers/video/backlight/ltv350qv.c
index 333949ff326..6c0f1ac0d32 100644
--- a/drivers/video/backlight/ltv350qv.c
+++ b/drivers/video/backlight/ltv350qv.c
@@ -232,23 +232,20 @@ static int __devinit ltv350qv_probe(struct spi_device *spi)
 	struct lcd_device *ld;
 	int ret;
 
-	lcd = kzalloc(sizeof(struct ltv350qv), GFP_KERNEL);
+	lcd = devm_kzalloc(&spi->dev, sizeof(struct ltv350qv), GFP_KERNEL);
 	if (!lcd)
 		return -ENOMEM;
 
 	lcd->spi = spi;
 	lcd->power = FB_BLANK_POWERDOWN;
-	lcd->buffer = kzalloc(8, GFP_KERNEL);
-	if (!lcd->buffer) {
-		ret = -ENOMEM;
-		goto out_free_lcd;
-	}
+	lcd->buffer = devm_kzalloc(&spi->dev, 8, GFP_KERNEL);
+	if (!lcd->buffer)
+		return -ENOMEM;
 
 	ld = lcd_device_register("ltv350qv", &spi->dev, lcd, &ltv_ops);
-	if (IS_ERR(ld)) {
-		ret = PTR_ERR(ld);
-		goto out_free_buffer;
-	}
+	if (IS_ERR(ld))
+		return PTR_ERR(ld);
+
 	lcd->ld = ld;
 
 	ret = ltv350qv_power(lcd, FB_BLANK_UNBLANK);
@@ -261,10 +258,6 @@ static int __devinit ltv350qv_probe(struct spi_device *spi)
 
 out_unregister:
 	lcd_device_unregister(ld);
-out_free_buffer:
-	kfree(lcd->buffer);
-out_free_lcd:
-	kfree(lcd);
 	return ret;
 }
 
@@ -274,8 +267,6 @@ static int __devexit ltv350qv_remove(struct spi_device *spi)
 
 	ltv350qv_power(lcd, FB_BLANK_POWERDOWN);
 	lcd_device_unregister(lcd->ld);
-	kfree(lcd->buffer);
-	kfree(lcd);
 
 	return 0;
 }
@@ -310,7 +301,6 @@ static void ltv350qv_shutdown(struct spi_device *spi)
 static struct spi_driver ltv350qv_driver = {
 	.driver = {
 		.name		= "ltv350qv",
-		.bus		= &spi_bus_type,
 		.owner		= THIS_MODULE,
 	},
 
diff --git a/drivers/video/backlight/omap1_bl.c b/drivers/video/backlight/omap1_bl.c
index 0175bfb08a1..bfdc5fbeaa1 100644
--- a/drivers/video/backlight/omap1_bl.c
+++ b/drivers/video/backlight/omap1_bl.c
@@ -18,6 +18,8 @@
  * Foundation, Inc., 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/init.h>
@@ -168,7 +170,7 @@ static int omapbl_probe(struct platform_device *pdev)
 	dev->props.brightness = pdata->default_intensity;
 	omapbl_update_status(dev);
 
-	printk(KERN_INFO "OMAP LCD backlight initialised\n");
+	pr_info("OMAP LCD backlight initialised\n");
 
 	return 0;
 }
diff --git a/drivers/video/backlight/pcf50633-backlight.c b/drivers/video/backlight/pcf50633-backlight.c
index c65853cb974..c092159f438 100644
--- a/drivers/video/backlight/pcf50633-backlight.c
+++ b/drivers/video/backlight/pcf50633-backlight.c
@@ -111,6 +111,7 @@ static int __devinit pcf50633_bl_probe(struct platform_device *pdev)
 	if (!pcf_bl)
 		return -ENOMEM;
 
+	memset(&bl_props, 0, sizeof(bl_props));
 	bl_props.type = BACKLIGHT_RAW;
 	bl_props.max_brightness = 0x3f;
 	bl_props.power = FB_BLANK_UNBLANK;
diff --git a/drivers/video/backlight/progear_bl.c b/drivers/video/backlight/progear_bl.c
index 6af183d6465..69b35f02929 100644
--- a/drivers/video/backlight/progear_bl.c
+++ b/drivers/video/backlight/progear_bl.c
@@ -15,6 +15,8 @@
  *
  */
 
+#define pr_fmt(fmt) KBUILD_MODNAME ": " fmt
+
 #include <linux/module.h>
 #include <linux/kernel.h>
 #include <linux/init.h>
@@ -68,13 +70,13 @@ static int progearbl_probe(struct platform_device *pdev)
 
 	pmu_dev = pci_get_device(PCI_VENDOR_ID_AL, PCI_DEVICE_ID_AL_M7101, NULL);
 	if (!pmu_dev) {
-		printk("ALI M7101 PMU not found.\n");
+		pr_err("ALI M7101 PMU not found.\n");
 		return -ENODEV;
 	}
 
 	sb_dev = pci_get_device(PCI_VENDOR_ID_AL, PCI_DEVICE_ID_AL_M1533, NULL);
 	if (!sb_dev) {
-		printk("ALI 1533 SB not found.\n");
+		pr_err("ALI 1533 SB not found.\n");
 		ret = -ENODEV;
 		goto put_pmu;
 	}
diff --git a/drivers/video/backlight/s6e63m0.c b/drivers/video/backlight/s6e63m0.c
index e264f55b257..6437ae474cf 100644
--- a/drivers/video/backlight/s6e63m0.c
+++ b/drivers/video/backlight/s6e63m0.c
@@ -741,7 +741,7 @@ static int __devinit s6e63m0_probe(struct spi_device *spi)
 	struct backlight_device *bd = NULL;
 	struct backlight_properties props;
 
-	lcd = kzalloc(sizeof(struct s6e63m0), GFP_KERNEL);
+	lcd = devm_kzalloc(&spi->dev, sizeof(struct s6e63m0), GFP_KERNEL);
 	if (!lcd)
 		return -ENOMEM;
 
@@ -751,7 +751,7 @@ static int __devinit s6e63m0_probe(struct spi_device *spi)
 	ret = spi_setup(spi);
 	if (ret < 0) {
 		dev_err(&spi->dev, "spi setup failed.\n");
-		goto out_free_lcd;
+		return ret;
 	}
 
 	lcd->spi = spi;
@@ -760,14 +760,12 @@ static int __devinit s6e63m0_probe(struct spi_device *spi)
 	lcd->lcd_pd = (struct lcd_platform_data *)spi->dev.platform_data;
 	if (!lcd->lcd_pd) {
 		dev_err(&spi->dev, "platform data is NULL.\n");
-		goto out_free_lcd;
+		return -EFAULT;
 	}
 
 	ld = lcd_device_register("s6e63m0", &spi->dev, lcd, &s6e63m0_lcd_ops);
-	if (IS_ERR(ld)) {
-		ret = PTR_ERR(ld);
-		goto out_free_lcd;
-	}
+	if (IS_ERR(ld))
+		return PTR_ERR(ld);
 
 	lcd->ld = ld;
 
@@ -824,8 +822,6 @@ static int __devinit s6e63m0_probe(struct spi_device *spi)
 
 out_lcd_unregister:
 	lcd_device_unregister(ld);
-out_free_lcd:
-	kfree(lcd);
 	return ret;
 }
 
@@ -838,7 +834,6 @@ static int __devexit s6e63m0_remove(struct spi_device *spi)
 	device_remove_file(&spi->dev, &dev_attr_gamma_mode);
 	backlight_device_unregister(lcd->bd);
 	lcd_device_unregister(lcd->ld);
-	kfree(lcd);
 
 	return 0;
 }
@@ -899,7 +894,6 @@ static void s6e63m0_shutdown(struct spi_device *spi)
 static struct spi_driver s6e63m0_driver = {
 	.driver = {
 		.name	= "s6e63m0",
-		.bus	= &spi_bus_type,
 		.owner	= THIS_MODULE,
 	},
 	.probe		= s6e63m0_probe,
diff --git a/drivers/video/backlight/tdo24m.c b/drivers/video/backlight/tdo24m.c
index 2368b8e5f89..02444d042cd 100644
--- a/drivers/video/backlight/tdo24m.c
+++ b/drivers/video/backlight/tdo24m.c
@@ -349,7 +349,7 @@ static int __devinit tdo24m_probe(struct spi_device *spi)
 	if (err)
 		return err;
 
-	lcd = kzalloc(sizeof(struct tdo24m), GFP_KERNEL);
+	lcd = devm_kzalloc(&spi->dev, sizeof(struct tdo24m), GFP_KERNEL);
 	if (!lcd)
 		return -ENOMEM;
 
@@ -357,11 +357,9 @@ static int __devinit tdo24m_probe(struct spi_device *spi)
 	lcd->power = FB_BLANK_POWERDOWN;
 	lcd->mode = MODE_VGA;	/* default to VGA */
 
-	lcd->buf = kmalloc(TDO24M_SPI_BUFF_SIZE, GFP_KERNEL);
-	if (lcd->buf == NULL) {
-		kfree(lcd);
+	lcd->buf = devm_kzalloc(&spi->dev, TDO24M_SPI_BUFF_SIZE, GFP_KERNEL);
+	if (lcd->buf == NULL)
 		return -ENOMEM;
-	}
 
 	m = &lcd->msg;
 	x = &lcd->xfer;
@@ -383,15 +381,13 @@ static int __devinit tdo24m_probe(struct spi_device *spi)
 		break;
 	default:
 		dev_err(&spi->dev, "Unsupported model");
-		goto out_free;
+		return -EINVAL;
 	}
 
 	lcd->lcd_dev = lcd_device_register("tdo24m", &spi->dev,
 					lcd, &tdo24m_ops);
-	if (IS_ERR(lcd->lcd_dev)) {
-		err = PTR_ERR(lcd->lcd_dev);
-		goto out_free;
-	}
+	if (IS_ERR(lcd->lcd_dev))
+		return PTR_ERR(lcd->lcd_dev);
 
 	dev_set_drvdata(&spi->dev, lcd);
 	err = tdo24m_power(lcd, FB_BLANK_UNBLANK);
@@ -402,9 +398,6 @@ static int __devinit tdo24m_probe(struct spi_device *spi)
 
 out_unregister:
 	lcd_device_unregister(lcd->lcd_dev);
-out_free:
-	kfree(lcd->buf);
-	kfree(lcd);
 	return err;
 }
 
@@ -414,8 +407,6 @@ static int __devexit tdo24m_remove(struct spi_device *spi)
 
 	tdo24m_power(lcd, FB_BLANK_POWERDOWN);
 	lcd_device_unregister(lcd->lcd_dev);
-	kfree(lcd->buf);
-	kfree(lcd);
 
 	return 0;
 }
diff --git a/drivers/video/backlight/tosa_bl.c b/drivers/video/backlight/tosa_bl.c
index 2b241abced4..0d54e607e82 100644
--- a/drivers/video/backlight/tosa_bl.c
+++ b/drivers/video/backlight/tosa_bl.c
@@ -82,8 +82,11 @@ static int __devinit tosa_bl_probe(struct i2c_client *client,
 		const struct i2c_device_id *id)
 {
 	struct backlight_properties props;
-	struct tosa_bl_data *data = kzalloc(sizeof(struct tosa_bl_data), GFP_KERNEL);
+	struct tosa_bl_data *data;
 	int ret = 0;
+
+	data = devm_kzalloc(&client->dev, sizeof(struct tosa_bl_data),
+				GFP_KERNEL);
 	if (!data)
 		return -ENOMEM;
 
@@ -92,7 +95,7 @@ static int __devinit tosa_bl_probe(struct i2c_client *client,
 	ret = gpio_request(TOSA_GPIO_BL_C20MA, "backlight");
 	if (ret) {
 		dev_dbg(&data->bl->dev, "Unable to request gpio!\n");
-		goto err_gpio_bl;
+		return ret;
 	}
 	ret = gpio_direction_output(TOSA_GPIO_BL_C20MA, 0);
 	if (ret)
@@ -122,8 +125,6 @@ err_reg:
 	data->bl = NULL;
 err_gpio_dir:
 	gpio_free(TOSA_GPIO_BL_C20MA);
-err_gpio_bl:
-	kfree(data);
 	return ret;
 }
 
@@ -136,8 +137,6 @@ static int __devexit tosa_bl_remove(struct i2c_client *client)
 
 	gpio_free(TOSA_GPIO_BL_C20MA);
 
-	kfree(data);
-
 	return 0;
 }
 
diff --git a/drivers/video/backlight/tosa_lcd.c b/drivers/video/backlight/tosa_lcd.c
index 2231aec2391..47823b8efff 100644
--- a/drivers/video/backlight/tosa_lcd.c
+++ b/drivers/video/backlight/tosa_lcd.c
@@ -174,7 +174,8 @@ static int __devinit tosa_lcd_probe(struct spi_device *spi)
 	int ret;
 	struct tosa_lcd_data *data;
 
-	data = kzalloc(sizeof(struct tosa_lcd_data), GFP_KERNEL);
+	data = devm_kzalloc(&spi->dev, sizeof(struct tosa_lcd_data),
+				GFP_KERNEL);
 	if (!data)
 		return -ENOMEM;
 
@@ -187,7 +188,7 @@ static int __devinit tosa_lcd_probe(struct spi_device *spi)
 
 	ret = spi_setup(spi);
 	if (ret < 0)
-		goto err_spi;
+		return ret;
 
 	data->spi = spi;
 	dev_set_drvdata(&spi->dev, data);
@@ -224,8 +225,6 @@ err_gpio_dir:
 	gpio_free(TOSA_GPIO_TG_ON);
 err_gpio_tg:
 	dev_set_drvdata(&spi->dev, NULL);
-err_spi:
-	kfree(data);
 	return ret;
 }
 
@@ -242,7 +241,6 @@ static int __devexit tosa_lcd_remove(struct spi_device *spi)
 
 	gpio_free(TOSA_GPIO_TG_ON);
 	dev_set_drvdata(&spi->dev, NULL);
-	kfree(data);
 
 	return 0;
 }
diff --git a/drivers/video/backlight/wm831x_bl.c b/drivers/video/backlight/wm831x_bl.c
index 5d365deb5f8..9e5517a3a52 100644
--- a/drivers/video/backlight/wm831x_bl.c
+++ b/drivers/video/backlight/wm831x_bl.c
@@ -194,6 +194,7 @@ static int wm831x_backlight_probe(struct platform_device *pdev)
 	data->current_brightness = 0;
 	data->isink_reg = isink_reg;
 
+	memset(&props, 0, sizeof(props));
 	props.type = BACKLIGHT_RAW;
 	props.max_brightness = max_isel;
 	bl = backlight_device_register("wm831x", &pdev->dev, data,
diff --git a/drivers/video/clps711xfb.c b/drivers/video/clps711xfb.c
index 99b354b8e25..f994c8b8f10 100644
--- a/drivers/video/clps711xfb.c
+++ b/drivers/video/clps711xfb.c
@@ -33,7 +33,6 @@
 #include <asm/mach-types.h>
 #include <linux/uaccess.h>
 
-#include <asm/hardware/clps7111.h>
 #include <mach/syspld.h>
 
 struct fb_info	*cfb;
diff --git a/drivers/video/console/sticore.c b/drivers/video/console/sticore.c
index 6468a297e34..39571f9e016 100644
--- a/drivers/video/console/sticore.c
+++ b/drivers/video/console/sticore.c
@@ -22,7 +22,9 @@
 #include <linux/font.h>
 
 #include <asm/hardware.h>
+#include <asm/page.h>
 #include <asm/parisc-device.h>
+#include <asm/pdc.h>
 #include <asm/cacheflush.h>
 #include <asm/grfioctl.h>
 
diff --git a/drivers/video/efifb.c b/drivers/video/efifb.c
index 784139aed07..b4a632ada40 100644
--- a/drivers/video/efifb.c
+++ b/drivers/video/efifb.c
@@ -18,6 +18,8 @@
 
 static bool request_mem_succeeded = false;
 
+static struct pci_dev *default_vga;
+
 static struct fb_var_screeninfo efifb_defined __devinitdata = {
 	.activate		= FB_ACTIVATE_NOW,
 	.height			= -1,
@@ -298,35 +300,72 @@ static struct fb_ops efifb_ops = {
 	.fb_imageblit	= cfb_imageblit,
 };
 
+struct pci_dev *vga_default_device(void)
+{
+	return default_vga;
+}
+
+EXPORT_SYMBOL_GPL(vga_default_device);
+
+void vga_set_default_device(struct pci_dev *pdev)
+{
+	default_vga = pdev;
+}
+
 static int __init efifb_setup(char *options)
 {
 	char *this_opt;
 	int i;
+	struct pci_dev *dev = NULL;
+
+	if (options && *options) {
+		while ((this_opt = strsep(&options, ",")) != NULL) {
+			if (!*this_opt) continue;
+
+			for (i = 0; i < M_UNKNOWN; i++) {
+				if (!strcmp(this_opt, dmi_list[i].optname) &&
+				    dmi_list[i].base != 0) {
+					screen_info.lfb_base = dmi_list[i].base;
+					screen_info.lfb_linelength = dmi_list[i].stride;
+					screen_info.lfb_width = dmi_list[i].width;
+					screen_info.lfb_height = dmi_list[i].height;
+				}
+			}
+			if (!strncmp(this_opt, "base:", 5))
+				screen_info.lfb_base = simple_strtoul(this_opt+5, NULL, 0);
+			else if (!strncmp(this_opt, "stride:", 7))
+				screen_info.lfb_linelength = simple_strtoul(this_opt+7, NULL, 0) * 4;
+			else if (!strncmp(this_opt, "height:", 7))
+				screen_info.lfb_height = simple_strtoul(this_opt+7, NULL, 0);
+			else if (!strncmp(this_opt, "width:", 6))
+				screen_info.lfb_width = simple_strtoul(this_opt+6, NULL, 0);
+		}
+	}
 
-	if (!options || !*options)
-		return 0;
+	for_each_pci_dev(dev) {
+		int i;
 
-	while ((this_opt = strsep(&options, ",")) != NULL) {
-		if (!*this_opt) continue;
+		if ((dev->class >> 8) != PCI_CLASS_DISPLAY_VGA)
+			continue;
 
-		for (i = 0; i < M_UNKNOWN; i++) {
-			if (!strcmp(this_opt, dmi_list[i].optname) &&
-					dmi_list[i].base != 0) {
-				screen_info.lfb_base = dmi_list[i].base;
-				screen_info.lfb_linelength = dmi_list[i].stride;
-				screen_info.lfb_width = dmi_list[i].width;
-				screen_info.lfb_height = dmi_list[i].height;
-			}
+		for (i=0; i < DEVICE_COUNT_RESOURCE; i++) {
+			resource_size_t start, end;
+
+			if (!(pci_resource_flags(dev, i) & IORESOURCE_MEM))
+				continue;
+
+			start = pci_resource_start(dev, i);
+			end  = pci_resource_end(dev, i);
+
+			if (!start || !end)
+				continue;
+
+			if (screen_info.lfb_base >= start &&
+			    (screen_info.lfb_base + screen_info.lfb_size) < end)
+				default_vga = dev;
 		}
-		if (!strncmp(this_opt, "base:", 5))
-			screen_info.lfb_base = simple_strtoul(this_opt+5, NULL, 0);
-		else if (!strncmp(this_opt, "stride:", 7))
-			screen_info.lfb_linelength = simple_strtoul(this_opt+7, NULL, 0) * 4;
-		else if (!strncmp(this_opt, "height:", 7))
-			screen_info.lfb_height = simple_strtoul(this_opt+7, NULL, 0);
-		else if (!strncmp(this_opt, "width:", 6))
-			screen_info.lfb_width = simple_strtoul(this_opt+6, NULL, 0);
 	}
+
 	return 0;
 }
 
diff --git a/drivers/video/fbmem.c b/drivers/video/fbmem.c
index c6ce416ab58..0dff12a1dae 100644
--- a/drivers/video/fbmem.c
+++ b/drivers/video/fbmem.c
@@ -1046,20 +1046,29 @@ fb_set_var(struct fb_info *info, struct fb_var_screeninfo *var)
 int
 fb_blank(struct fb_info *info, int blank)
 {	
- 	int ret = -EINVAL;
+	struct fb_event event;
+	int ret = -EINVAL, early_ret;
 
  	if (blank > FB_BLANK_POWERDOWN)
  		blank = FB_BLANK_POWERDOWN;
 
+	event.info = info;
+	event.data = &blank;
+
+	early_ret = fb_notifier_call_chain(FB_EARLY_EVENT_BLANK, &event);
+
 	if (info->fbops->fb_blank)
  		ret = info->fbops->fb_blank(blank, info);
 
- 	if (!ret) {
-		struct fb_event event;
-
-		event.info = info;
-		event.data = &blank;
+	if (!ret)
 		fb_notifier_call_chain(FB_EVENT_BLANK, &event);
+	else {
+		/*
+		 * if fb_blank is failed then revert effects of
+		 * the early blank event.
+		 */
+		if (!early_ret)
+			fb_notifier_call_chain(FB_R_EARLY_EVENT_BLANK, &event);
 	}
 
  	return ret;
diff --git a/drivers/video/imxfb.c b/drivers/video/imxfb.c
index f135dbead07..caad3689b4e 100644
--- a/drivers/video/imxfb.c
+++ b/drivers/video/imxfb.c
@@ -131,7 +131,9 @@ struct imxfb_rgb {
 struct imxfb_info {
 	struct platform_device  *pdev;
 	void __iomem		*regs;
-	struct clk		*clk;
+	struct clk		*clk_ipg;
+	struct clk		*clk_ahb;
+	struct clk		*clk_per;
 
 	/*
 	 * These are the addresses we mapped
@@ -340,7 +342,7 @@ static int imxfb_check_var(struct fb_var_screeninfo *var, struct fb_info *info)
 
 	pr_debug("var->bits_per_pixel=%d\n", var->bits_per_pixel);
 
-	lcd_clk = clk_get_rate(fbi->clk);
+	lcd_clk = clk_get_rate(fbi->clk_per);
 
 	tmp = var->pixclock * (unsigned long long)lcd_clk;
 
@@ -455,11 +457,17 @@ static int imxfb_bl_update_status(struct backlight_device *bl)
 
 	fbi->pwmr = (fbi->pwmr & ~0xFF) | brightness;
 
-	if (bl->props.fb_blank != FB_BLANK_UNBLANK)
-		clk_enable(fbi->clk);
+	if (bl->props.fb_blank != FB_BLANK_UNBLANK) {
+		clk_prepare_enable(fbi->clk_ipg);
+		clk_prepare_enable(fbi->clk_ahb);
+		clk_prepare_enable(fbi->clk_per);
+	}
 	writel(fbi->pwmr, fbi->regs + LCDC_PWMR);
-	if (bl->props.fb_blank != FB_BLANK_UNBLANK)
-		clk_disable(fbi->clk);
+	if (bl->props.fb_blank != FB_BLANK_UNBLANK) {
+		clk_disable_unprepare(fbi->clk_per);
+		clk_disable_unprepare(fbi->clk_ahb);
+		clk_disable_unprepare(fbi->clk_ipg);
+	}
 
 	return 0;
 }
@@ -522,7 +530,9 @@ static void imxfb_enable_controller(struct imxfb_info *fbi)
 	 */
 	writel(RMCR_LCDC_EN_MX1, fbi->regs + LCDC_RMCR);
 
-	clk_enable(fbi->clk);
+	clk_prepare_enable(fbi->clk_ipg);
+	clk_prepare_enable(fbi->clk_ahb);
+	clk_prepare_enable(fbi->clk_per);
 
 	if (fbi->backlight_power)
 		fbi->backlight_power(1);
@@ -539,7 +549,9 @@ static void imxfb_disable_controller(struct imxfb_info *fbi)
 	if (fbi->lcd_power)
 		fbi->lcd_power(0);
 
-	clk_disable(fbi->clk);
+	clk_disable_unprepare(fbi->clk_per);
+	clk_disable_unprepare(fbi->clk_ipg);
+	clk_disable_unprepare(fbi->clk_ahb);
 
 	writel(0, fbi->regs + LCDC_RMCR);
 }
@@ -770,10 +782,21 @@ static int __init imxfb_probe(struct platform_device *pdev)
 		goto failed_req;
 	}
 
-	fbi->clk = clk_get(&pdev->dev, NULL);
-	if (IS_ERR(fbi->clk)) {
-		ret = PTR_ERR(fbi->clk);
-		dev_err(&pdev->dev, "unable to get clock: %d\n", ret);
+	fbi->clk_ipg = devm_clk_get(&pdev->dev, "ipg");
+	if (IS_ERR(fbi->clk_ipg)) {
+		ret = PTR_ERR(fbi->clk_ipg);
+		goto failed_getclock;
+	}
+
+	fbi->clk_ahb = devm_clk_get(&pdev->dev, "ahb");
+	if (IS_ERR(fbi->clk_ahb)) {
+		ret = PTR_ERR(fbi->clk_ahb);
+		goto failed_getclock;
+	}
+
+	fbi->clk_per = devm_clk_get(&pdev->dev, "per");
+	if (IS_ERR(fbi->clk_per)) {
+		ret = PTR_ERR(fbi->clk_per);
 		goto failed_getclock;
 	}
 
@@ -858,7 +881,6 @@ failed_platform_init:
 failed_map:
 	iounmap(fbi->regs);
 failed_ioremap:
-	clk_put(fbi->clk);
 failed_getclock:
 	release_mem_region(res->start, resource_size(res));
 failed_req:
@@ -895,8 +917,6 @@ static int __devexit imxfb_remove(struct platform_device *pdev)
 
 	iounmap(fbi->regs);
 	release_mem_region(res->start, resource_size(res));
-	clk_disable(fbi->clk);
-	clk_put(fbi->clk);
 
 	platform_set_drvdata(pdev, NULL);
 
diff --git a/drivers/video/mxsfb.c b/drivers/video/mxsfb.c
index 4a89f889852..6c6bc578d0f 100644
--- a/drivers/video/mxsfb.c
+++ b/drivers/video/mxsfb.c
@@ -45,6 +45,7 @@
 #include <linux/clk.h>
 #include <linux/dma-mapping.h>
 #include <linux/io.h>
+#include <linux/pinctrl/consumer.h>
 #include <mach/mxsfb.h>
 
 #define REG_SET	4
@@ -756,6 +757,7 @@ static int __devinit mxsfb_probe(struct platform_device *pdev)
 	struct mxsfb_info *host;
 	struct fb_info *fb_info;
 	struct fb_modelist *modelist;
+	struct pinctrl *pinctrl;
 	int i, ret;
 
 	if (!pdata) {
@@ -793,6 +795,12 @@ static int __devinit mxsfb_probe(struct platform_device *pdev)
 
 	host->devdata = &mxsfb_devdata[pdev->id_entry->driver_data];
 
+	pinctrl = devm_pinctrl_get_select_default(&pdev->dev);
+	if (IS_ERR(pinctrl)) {
+		ret = PTR_ERR(pinctrl);
+		goto error_getpin;
+	}
+
 	host->clk = clk_get(&host->pdev->dev, NULL);
 	if (IS_ERR(host->clk)) {
 		ret = PTR_ERR(host->clk);
@@ -848,6 +856,7 @@ error_init_fb:
 error_pseudo_pallette:
 	clk_put(host->clk);
 error_getclock:
+error_getpin:
 	iounmap(host->base);
 error_ioremap:
 	framebuffer_release(fb_info);
diff --git a/drivers/video/omap2/displays/Kconfig b/drivers/video/omap2/displays/Kconfig
index 408a9927be9..c3853c92279 100644
--- a/drivers/video/omap2/displays/Kconfig
+++ b/drivers/video/omap2/displays/Kconfig
@@ -10,12 +10,12 @@ config PANEL_GENERIC_DPI
 	  Supports LCD Panel used in TI SDP3430 and EVM boards,
 	  OMAP3517 EVM boards and CM-T35.
 
-config PANEL_DVI
-	tristate "DVI output"
+config PANEL_TFP410
+	tristate "TFP410 DPI-to-DVI chip"
 	depends on OMAP2_DSS_DPI && I2C
 	help
-	  Driver for external monitors, connected via DVI. The driver uses i2c
-	  to read EDID information from the monitor.
+	  Driver for TFP410 DPI-to-DVI chip. The driver uses i2c to read EDID
+	  information from the monitor.
 
 config PANEL_LGPHILIPS_LB035Q02
 	tristate "LG.Philips LB035Q02 LCD Panel"
diff --git a/drivers/video/omap2/displays/Makefile b/drivers/video/omap2/displays/Makefile
index fbfafc6eebb..58a5176b07b 100644
--- a/drivers/video/omap2/displays/Makefile
+++ b/drivers/video/omap2/displays/Makefile
@@ -1,5 +1,5 @@
 obj-$(CONFIG_PANEL_GENERIC_DPI) += panel-generic-dpi.o
-obj-$(CONFIG_PANEL_DVI) += panel-dvi.o
+obj-$(CONFIG_PANEL_TFP410) += panel-tfp410.o
 obj-$(CONFIG_PANEL_LGPHILIPS_LB035Q02) += panel-lgphilips-lb035q02.o
 obj-$(CONFIG_PANEL_SHARP_LS037V7DW01) += panel-sharp-ls037v7dw01.o
 obj-$(CONFIG_PANEL_NEC_NL8048HL11_01B) += panel-nec-nl8048hl11-01b.o
diff --git a/drivers/video/omap2/displays/panel-acx565akm.c b/drivers/video/omap2/displays/panel-acx565akm.c
index d26f37ac69d..74e7cf07850 100644
--- a/drivers/video/omap2/displays/panel-acx565akm.c
+++ b/drivers/video/omap2/displays/panel-acx565akm.c
@@ -532,6 +532,7 @@ static int acx_panel_probe(struct omap_dss_device *dssdev)
 
 	/*------- Backlight control --------*/
 
+	memset(&props, 0, sizeof(props));
 	props.fb_blank = FB_BLANK_UNBLANK;
 	props.power = FB_BLANK_UNBLANK;
 	props.type = BACKLIGHT_RAW;
diff --git a/drivers/video/omap2/displays/panel-taal.c b/drivers/video/omap2/displays/panel-taal.c
index 0f21fa5a16a..b2dd88b4842 100644
--- a/drivers/video/omap2/displays/panel-taal.c
+++ b/drivers/video/omap2/displays/panel-taal.c
@@ -993,6 +993,15 @@ static int taal_probe(struct omap_dss_device *dssdev)
 
 	dev_set_drvdata(&dssdev->dev, td);
 
+	if (gpio_is_valid(panel_data->reset_gpio)) {
+		r = gpio_request_one(panel_data->reset_gpio, GPIOF_OUT_INIT_LOW,
+				"taal rst");
+		if (r) {
+			dev_err(&dssdev->dev, "failed to request reset gpio\n");
+			goto err_rst_gpio;
+		}
+	}
+
 	taal_hw_reset(dssdev);
 
 	if (panel_data->use_dsi_backlight) {
@@ -1073,6 +1082,9 @@ err_gpio:
 	if (bldev != NULL)
 		backlight_device_unregister(bldev);
 err_bl:
+	if (gpio_is_valid(panel_data->reset_gpio))
+		gpio_free(panel_data->reset_gpio);
+err_rst_gpio:
 	destroy_workqueue(td->workqueue);
 err_wq:
 	free_regulators(panel_config->regulators, panel_config->num_regulators);
@@ -1116,15 +1128,25 @@ static void __exit taal_remove(struct omap_dss_device *dssdev)
 	free_regulators(td->panel_config->regulators,
 			td->panel_config->num_regulators);
 
+	if (gpio_is_valid(panel_data->reset_gpio))
+		gpio_free(panel_data->reset_gpio);
+
 	kfree(td);
 }
 
 static int taal_power_on(struct omap_dss_device *dssdev)
 {
 	struct taal_data *td = dev_get_drvdata(&dssdev->dev);
+	struct nokia_dsi_panel_data *panel_data = get_panel_data(dssdev);
 	u8 id1, id2, id3;
 	int r;
 
+	r = omapdss_dsi_configure_pins(dssdev, &panel_data->pin_config);
+	if (r) {
+		dev_err(&dssdev->dev, "failed to configure DSI pins\n");
+		goto err0;
+	};
+
 	r = omapdss_dsi_display_enable(dssdev);
 	if (r) {
 		dev_err(&dssdev->dev, "failed to enable DSI\n");
diff --git a/drivers/video/omap2/displays/panel-dvi.c b/drivers/video/omap2/displays/panel-tfp410.c
index 03eb14af33e..52637fa8fda 100644
--- a/drivers/video/omap2/displays/panel-dvi.c
+++ b/drivers/video/omap2/displays/panel-tfp410.c
@@ -1,5 +1,5 @@
 /*
- * DVI output support
+ * TFP410 DPI-to-DVI chip
  *
  * Copyright (C) 2011 Texas Instruments Inc
  * Author: Tomi Valkeinen <tomi.valkeinen@ti.com>
@@ -21,11 +21,12 @@
 #include <linux/slab.h>
 #include <video/omapdss.h>
 #include <linux/i2c.h>
+#include <linux/gpio.h>
 #include <drm/drm_edid.h>
 
-#include <video/omap-panel-dvi.h>
+#include <video/omap-panel-tfp410.h>
 
-static const struct omap_video_timings panel_dvi_default_timings = {
+static const struct omap_video_timings tfp410_default_timings = {
 	.x_res		= 640,
 	.y_res		= 480,
 
@@ -44,17 +45,19 @@ struct panel_drv_data {
 	struct omap_dss_device *dssdev;
 
 	struct mutex lock;
+
+	int pd_gpio;
 };
 
-static inline struct panel_dvi_platform_data
+static inline struct tfp410_platform_data
 *get_pdata(const struct omap_dss_device *dssdev)
 {
 	return dssdev->data;
 }
 
-static int panel_dvi_power_on(struct omap_dss_device *dssdev)
+static int tfp410_power_on(struct omap_dss_device *dssdev)
 {
-	struct panel_dvi_platform_data *pdata = get_pdata(dssdev);
+	struct panel_drv_data *ddata = dev_get_drvdata(&dssdev->dev);
 	int r;
 
 	if (dssdev->state == OMAP_DSS_DISPLAY_ACTIVE)
@@ -64,57 +67,72 @@ static int panel_dvi_power_on(struct omap_dss_device *dssdev)
 	if (r)
 		goto err0;
 
-	if (pdata->platform_enable) {
-		r = pdata->platform_enable(dssdev);
-		if (r)
-			goto err1;
-	}
+	if (gpio_is_valid(ddata->pd_gpio))
+		gpio_set_value(ddata->pd_gpio, 1);
 
 	return 0;
-err1:
-	omapdss_dpi_display_disable(dssdev);
 err0:
 	return r;
 }
 
-static void panel_dvi_power_off(struct omap_dss_device *dssdev)
+static void tfp410_power_off(struct omap_dss_device *dssdev)
 {
-	struct panel_dvi_platform_data *pdata = get_pdata(dssdev);
+	struct panel_drv_data *ddata = dev_get_drvdata(&dssdev->dev);
 
 	if (dssdev->state != OMAP_DSS_DISPLAY_ACTIVE)
 		return;
 
-	if (pdata->platform_disable)
-		pdata->platform_disable(dssdev);
+	if (gpio_is_valid(ddata->pd_gpio))
+		gpio_set_value(ddata->pd_gpio, 0);
 
 	omapdss_dpi_display_disable(dssdev);
 }
 
-static int panel_dvi_probe(struct omap_dss_device *dssdev)
+static int tfp410_probe(struct omap_dss_device *dssdev)
 {
+	struct tfp410_platform_data *pdata = get_pdata(dssdev);
 	struct panel_drv_data *ddata;
+	int r;
 
 	ddata = kzalloc(sizeof(*ddata), GFP_KERNEL);
 	if (!ddata)
 		return -ENOMEM;
 
-	dssdev->panel.timings = panel_dvi_default_timings;
+	dssdev->panel.timings = tfp410_default_timings;
 	dssdev->panel.config = OMAP_DSS_LCD_TFT;
 
 	ddata->dssdev = dssdev;
 	mutex_init(&ddata->lock);
 
+	if (pdata)
+		ddata->pd_gpio = pdata->power_down_gpio;
+	else
+		ddata->pd_gpio = -1;
+
+	if (gpio_is_valid(ddata->pd_gpio)) {
+		r = gpio_request_one(ddata->pd_gpio, GPIOF_OUT_INIT_LOW,
+				"tfp410 pd");
+		if (r) {
+			dev_err(&dssdev->dev, "Failed to request PD GPIO %d\n",
+					ddata->pd_gpio);
+			ddata->pd_gpio = -1;
+		}
+	}
+
 	dev_set_drvdata(&dssdev->dev, ddata);
 
 	return 0;
 }
 
-static void __exit panel_dvi_remove(struct omap_dss_device *dssdev)
+static void __exit tfp410_remove(struct omap_dss_device *dssdev)
 {
 	struct panel_drv_data *ddata = dev_get_drvdata(&dssdev->dev);
 
 	mutex_lock(&ddata->lock);
 
+	if (gpio_is_valid(ddata->pd_gpio))
+		gpio_free(ddata->pd_gpio);
+
 	dev_set_drvdata(&dssdev->dev, NULL);
 
 	mutex_unlock(&ddata->lock);
@@ -122,14 +140,14 @@ static void __exit panel_dvi_remove(struct omap_dss_device *dssdev)
 	kfree(ddata);
 }
 
-static int panel_dvi_enable(struct omap_dss_device *dssdev)
+static int tfp410_enable(struct omap_dss_device *dssdev)
 {
 	struct panel_drv_data *ddata = dev_get_drvdata(&dssdev->dev);
 	int r;
 
 	mutex_lock(&ddata->lock);
 
-	r = panel_dvi_power_on(dssdev);
+	r = tfp410_power_on(dssdev);
 	if (r == 0)
 		dssdev->state = OMAP_DSS_DISPLAY_ACTIVE;
 
@@ -138,26 +156,26 @@ static int panel_dvi_enable(struct omap_dss_device *dssdev)
 	return r;
 }
 
-static void panel_dvi_disable(struct omap_dss_device *dssdev)
+static void tfp410_disable(struct omap_dss_device *dssdev)
 {
 	struct panel_drv_data *ddata = dev_get_drvdata(&dssdev->dev);
 
 	mutex_lock(&ddata->lock);
 
-	panel_dvi_power_off(dssdev);
+	tfp410_power_off(dssdev);
 
 	dssdev->state = OMAP_DSS_DISPLAY_DISABLED;
 
 	mutex_unlock(&ddata->lock);
 }
 
-static int panel_dvi_suspend(struct omap_dss_device *dssdev)
+static int tfp410_suspend(struct omap_dss_device *dssdev)
 {
 	struct panel_drv_data *ddata = dev_get_drvdata(&dssdev->dev);
 
 	mutex_lock(&ddata->lock);
 
-	panel_dvi_power_off(dssdev);
+	tfp410_power_off(dssdev);
 
 	dssdev->state = OMAP_DSS_DISPLAY_SUSPENDED;
 
@@ -166,14 +184,14 @@ static int panel_dvi_suspend(struct omap_dss_device *dssdev)
 	return 0;
 }
 
-static int panel_dvi_resume(struct omap_dss_device *dssdev)
+static int tfp410_resume(struct omap_dss_device *dssdev)
 {
 	struct panel_drv_data *ddata = dev_get_drvdata(&dssdev->dev);
 	int r;
 
 	mutex_lock(&ddata->lock);
 
-	r = panel_dvi_power_on(dssdev);
+	r = tfp410_power_on(dssdev);
 	if (r == 0)
 		dssdev->state = OMAP_DSS_DISPLAY_ACTIVE;
 
@@ -182,7 +200,7 @@ static int panel_dvi_resume(struct omap_dss_device *dssdev)
 	return r;
 }
 
-static void panel_dvi_set_timings(struct omap_dss_device *dssdev,
+static void tfp410_set_timings(struct omap_dss_device *dssdev,
 		struct omap_video_timings *timings)
 {
 	struct panel_drv_data *ddata = dev_get_drvdata(&dssdev->dev);
@@ -192,7 +210,7 @@ static void panel_dvi_set_timings(struct omap_dss_device *dssdev,
 	mutex_unlock(&ddata->lock);
 }
 
-static void panel_dvi_get_timings(struct omap_dss_device *dssdev,
+static void tfp410_get_timings(struct omap_dss_device *dssdev,
 		struct omap_video_timings *timings)
 {
 	struct panel_drv_data *ddata = dev_get_drvdata(&dssdev->dev);
@@ -202,7 +220,7 @@ static void panel_dvi_get_timings(struct omap_dss_device *dssdev,
 	mutex_unlock(&ddata->lock);
 }
 
-static int panel_dvi_check_timings(struct omap_dss_device *dssdev,
+static int tfp410_check_timings(struct omap_dss_device *dssdev,
 		struct omap_video_timings *timings)
 {
 	struct panel_drv_data *ddata = dev_get_drvdata(&dssdev->dev);
@@ -216,7 +234,7 @@ static int panel_dvi_check_timings(struct omap_dss_device *dssdev,
 }
 
 
-static int panel_dvi_ddc_read(struct i2c_adapter *adapter,
+static int tfp410_ddc_read(struct i2c_adapter *adapter,
 		unsigned char *buf, u16 count, u8 offset)
 {
 	int r, retries;
@@ -247,11 +265,11 @@ static int panel_dvi_ddc_read(struct i2c_adapter *adapter,
 	return r < 0 ? r : -EIO;
 }
 
-static int panel_dvi_read_edid(struct omap_dss_device *dssdev,
+static int tfp410_read_edid(struct omap_dss_device *dssdev,
 		u8 *edid, int len)
 {
 	struct panel_drv_data *ddata = dev_get_drvdata(&dssdev->dev);
-	struct panel_dvi_platform_data *pdata = get_pdata(dssdev);
+	struct tfp410_platform_data *pdata = get_pdata(dssdev);
 	struct i2c_adapter *adapter;
 	int r, l, bytes_read;
 
@@ -271,7 +289,7 @@ static int panel_dvi_read_edid(struct omap_dss_device *dssdev,
 	}
 
 	l = min(EDID_LENGTH, len);
-	r = panel_dvi_ddc_read(adapter, edid, l, 0);
+	r = tfp410_ddc_read(adapter, edid, l, 0);
 	if (r)
 		goto err;
 
@@ -281,7 +299,7 @@ static int panel_dvi_read_edid(struct omap_dss_device *dssdev,
 	if (len > EDID_LENGTH && edid[0x7e] > 0) {
 		l = min(EDID_LENGTH, len - EDID_LENGTH);
 
-		r = panel_dvi_ddc_read(adapter, edid + EDID_LENGTH,
+		r = tfp410_ddc_read(adapter, edid + EDID_LENGTH,
 				l, EDID_LENGTH);
 		if (r)
 			goto err;
@@ -298,10 +316,10 @@ err:
 	return r;
 }
 
-static bool panel_dvi_detect(struct omap_dss_device *dssdev)
+static bool tfp410_detect(struct omap_dss_device *dssdev)
 {
 	struct panel_drv_data *ddata = dev_get_drvdata(&dssdev->dev);
-	struct panel_dvi_platform_data *pdata = get_pdata(dssdev);
+	struct tfp410_platform_data *pdata = get_pdata(dssdev);
 	struct i2c_adapter *adapter;
 	unsigned char out;
 	int r;
@@ -315,7 +333,7 @@ static bool panel_dvi_detect(struct omap_dss_device *dssdev)
 	if (!adapter)
 		goto out;
 
-	r = panel_dvi_ddc_read(adapter, &out, 1, 0);
+	r = tfp410_ddc_read(adapter, &out, 1, 0);
 
 	mutex_unlock(&ddata->lock);
 
@@ -326,38 +344,38 @@ out:
 	return true;
 }
 
-static struct omap_dss_driver panel_dvi_driver = {
-	.probe		= panel_dvi_probe,
-	.remove		= __exit_p(panel_dvi_remove),
+static struct omap_dss_driver tfp410_driver = {
+	.probe		= tfp410_probe,
+	.remove		= __exit_p(tfp410_remove),
 
-	.enable		= panel_dvi_enable,
-	.disable	= panel_dvi_disable,
-	.suspend	= panel_dvi_suspend,
-	.resume		= panel_dvi_resume,
+	.enable		= tfp410_enable,
+	.disable	= tfp410_disable,
+	.suspend	= tfp410_suspend,
+	.resume		= tfp410_resume,
 
-	.set_timings	= panel_dvi_set_timings,
-	.get_timings	= panel_dvi_get_timings,
-	.check_timings	= panel_dvi_check_timings,
+	.set_timings	= tfp410_set_timings,
+	.get_timings	= tfp410_get_timings,
+	.check_timings	= tfp410_check_timings,
 
-	.read_edid	= panel_dvi_read_edid,
-	.detect		= panel_dvi_detect,
+	.read_edid	= tfp410_read_edid,
+	.detect		= tfp410_detect,
 
 	.driver         = {
-		.name   = "dvi",
+		.name   = "tfp410",
 		.owner  = THIS_MODULE,
 	},
 };
 
-static int __init panel_dvi_init(void)
+static int __init tfp410_init(void)
 {
-	return omap_dss_register_driver(&panel_dvi_driver);
+	return omap_dss_register_driver(&tfp410_driver);
 }
 
-static void __exit panel_dvi_exit(void)
+static void __exit tfp410_exit(void)
 {
-	omap_dss_unregister_driver(&panel_dvi_driver);
+	omap_dss_unregister_driver(&tfp410_driver);
 }
 
-module_init(panel_dvi_init);
-module_exit(panel_dvi_exit);
+module_init(tfp410_init);
+module_exit(tfp410_exit);
 MODULE_LICENSE("GPL");
diff --git a/drivers/video/omap2/dss/dsi.c b/drivers/video/omap2/dss/dsi.c
index 662d14f8c2c..210a3c4f615 100644
--- a/drivers/video/omap2/dss/dsi.c
+++ b/drivers/video/omap2/dss/dsi.c
@@ -2076,65 +2076,6 @@ static unsigned dsi_get_line_buf_size(struct platform_device *dsidev)
 	}
 }
 
-static int dsi_parse_lane_config(struct omap_dss_device *dssdev)
-{
-	struct platform_device *dsidev = dsi_get_dsidev_from_dssdev(dssdev);
-	struct dsi_data *dsi = dsi_get_dsidrv_data(dsidev);
-	u8 lanes[DSI_MAX_NR_LANES];
-	u8 polarities[DSI_MAX_NR_LANES];
-	int num_lanes, i;
-
-	static const enum dsi_lane_function functions[] = {
-		DSI_LANE_CLK,
-		DSI_LANE_DATA1,
-		DSI_LANE_DATA2,
-		DSI_LANE_DATA3,
-		DSI_LANE_DATA4,
-	};
-
-	lanes[0] = dssdev->phy.dsi.clk_lane;
-	lanes[1] = dssdev->phy.dsi.data1_lane;
-	lanes[2] = dssdev->phy.dsi.data2_lane;
-	lanes[3] = dssdev->phy.dsi.data3_lane;
-	lanes[4] = dssdev->phy.dsi.data4_lane;
-	polarities[0] = dssdev->phy.dsi.clk_pol;
-	polarities[1] = dssdev->phy.dsi.data1_pol;
-	polarities[2] = dssdev->phy.dsi.data2_pol;
-	polarities[3] = dssdev->phy.dsi.data3_pol;
-	polarities[4] = dssdev->phy.dsi.data4_pol;
-
-	num_lanes = 0;
-
-	for (i = 0; i < dsi->num_lanes_supported; ++i)
-		dsi->lanes[i].function = DSI_LANE_UNUSED;
-
-	for (i = 0; i < dsi->num_lanes_supported; ++i) {
-		int num;
-
-		if (lanes[i] == DSI_LANE_UNUSED)
-			break;
-
-		num = lanes[i] - 1;
-
-		if (num >= dsi->num_lanes_supported)
-			return -EINVAL;
-
-		if (dsi->lanes[num].function != DSI_LANE_UNUSED)
-			return -EINVAL;
-
-		dsi->lanes[num].function = functions[i];
-		dsi->lanes[num].polarity = polarities[i];
-		num_lanes++;
-	}
-
-	if (num_lanes < 2 || num_lanes > dsi->num_lanes_supported)
-		return -EINVAL;
-
-	dsi->num_lanes_used = num_lanes;
-
-	return 0;
-}
-
 static int dsi_set_lane_config(struct omap_dss_device *dssdev)
 {
 	struct platform_device *dsidev = dsi_get_dsidev_from_dssdev(dssdev);
@@ -3975,6 +3916,74 @@ static void dsi_proto_timings(struct omap_dss_device *dssdev)
 	}
 }
 
+int omapdss_dsi_configure_pins(struct omap_dss_device *dssdev,
+		const struct omap_dsi_pin_config *pin_cfg)
+{
+	struct platform_device *dsidev = dsi_get_dsidev_from_dssdev(dssdev);
+	struct dsi_data *dsi = dsi_get_dsidrv_data(dsidev);
+	int num_pins;
+	const int *pins;
+	struct dsi_lane_config lanes[DSI_MAX_NR_LANES];
+	int num_lanes;
+	int i;
+
+	static const enum dsi_lane_function functions[] = {
+		DSI_LANE_CLK,
+		DSI_LANE_DATA1,
+		DSI_LANE_DATA2,
+		DSI_LANE_DATA3,
+		DSI_LANE_DATA4,
+	};
+
+	num_pins = pin_cfg->num_pins;
+	pins = pin_cfg->pins;
+
+	if (num_pins < 4 || num_pins > dsi->num_lanes_supported * 2
+			|| num_pins % 2 != 0)
+		return -EINVAL;
+
+	for (i = 0; i < DSI_MAX_NR_LANES; ++i)
+		lanes[i].function = DSI_LANE_UNUSED;
+
+	num_lanes = 0;
+
+	for (i = 0; i < num_pins; i += 2) {
+		u8 lane, pol;
+		int dx, dy;
+
+		dx = pins[i];
+		dy = pins[i + 1];
+
+		if (dx < 0 || dx >= dsi->num_lanes_supported * 2)
+			return -EINVAL;
+
+		if (dy < 0 || dy >= dsi->num_lanes_supported * 2)
+			return -EINVAL;
+
+		if (dx & 1) {
+			if (dy != dx - 1)
+				return -EINVAL;
+			pol = 1;
+		} else {
+			if (dy != dx + 1)
+				return -EINVAL;
+			pol = 0;
+		}
+
+		lane = dx / 2;
+
+		lanes[lane].function = functions[i / 2];
+		lanes[lane].polarity = pol;
+		num_lanes++;
+	}
+
+	memcpy(dsi->lanes, lanes, sizeof(dsi->lanes));
+	dsi->num_lanes_used = num_lanes;
+
+	return 0;
+}
+EXPORT_SYMBOL(omapdss_dsi_configure_pins);
+
 int dsi_enable_video_output(struct omap_dss_device *dssdev, int channel)
 {
 	struct platform_device *dsidev = dsi_get_dsidev_from_dssdev(dssdev);
@@ -4339,12 +4348,6 @@ static int dsi_display_init_dsi(struct omap_dss_device *dssdev)
 	int dsi_module = dsi_get_dsidev_id(dsidev);
 	int r;
 
-	r = dsi_parse_lane_config(dssdev);
-	if (r) {
-		DSSERR("illegal lane config");
-		goto err0;
-	}
-
 	r = dsi_pll_init(dsidev, true, true);
 	if (r)
 		goto err0;
diff --git a/drivers/video/sh_mobile_lcdcfb.c b/drivers/video/sh_mobile_lcdcfb.c
index 7a0b301587f..e672698bd82 100644
--- a/drivers/video/sh_mobile_lcdcfb.c
+++ b/drivers/video/sh_mobile_lcdcfb.c
@@ -758,7 +758,7 @@ static void __sh_mobile_lcdc_start(struct sh_mobile_lcdc_priv *priv)
 		}
 
 		lcdc_write_chan(ch, LDDFR, tmp);
-		lcdc_write_chan(ch, LDMLSR, ch->pitch);
+		lcdc_write_chan(ch, LDMLSR, ch->line_size);
 		lcdc_write_chan(ch, LDSA1R, ch->base_addr_y);
 		if (ch->format->yuv)
 			lcdc_write_chan(ch, LDSA2R, ch->base_addr_c);
@@ -847,6 +847,7 @@ static int sh_mobile_lcdc_start(struct sh_mobile_lcdc_priv *priv)
 
 		ch->base_addr_y = ch->dma_handle;
 		ch->base_addr_c = ch->base_addr_y + ch->xres * ch->yres_virtual;
+		ch->line_size = ch->pitch;
 
 		/* Enable MERAM if possible. */
 		if (mdev == NULL || mdev->ops == NULL ||
@@ -882,7 +883,7 @@ static int sh_mobile_lcdc_start(struct sh_mobile_lcdc_priv *priv)
 
 		meram = mdev->ops->meram_register(mdev, ch->cfg->meram_cfg,
 					ch->pitch, ch->yres, pixelformat,
-					&ch->pitch);
+					&ch->line_size);
 		if (!IS_ERR(meram)) {
 			mdev->ops->meram_update(mdev, meram,
 					ch->base_addr_y, ch->base_addr_c,
diff --git a/drivers/video/sh_mobile_lcdcfb.h b/drivers/video/sh_mobile_lcdcfb.h
index da1c26e78a5..5c3bddd2cb7 100644
--- a/drivers/video/sh_mobile_lcdcfb.h
+++ b/drivers/video/sh_mobile_lcdcfb.h
@@ -84,6 +84,7 @@ struct sh_mobile_lcdc_chan {
 
 	unsigned long base_addr_y;
 	unsigned long base_addr_c;
+	unsigned int line_size;
 
 	int (*notify)(struct sh_mobile_lcdc_chan *ch,
 		      enum sh_mobile_lcdc_entity_event event,
diff --git a/drivers/video/udlfb.c b/drivers/video/udlfb.c
index a159b63e18b..7af1e816618 100644
--- a/drivers/video/udlfb.c
+++ b/drivers/video/udlfb.c
@@ -1594,7 +1594,7 @@ static int dlfb_usb_probe(struct usb_interface *interface,
 
 	dev = kzalloc(sizeof(*dev), GFP_KERNEL);
 	if (dev == NULL) {
-		err("dlfb_usb_probe: failed alloc of dev struct\n");
+		dev_err(&interface->dev, "dlfb_usb_probe: failed alloc of dev struct\n");
 		goto error;
 	}
 
diff --git a/drivers/video/uvesafb.c b/drivers/video/uvesafb.c
index 26e83d7fdd6..b0e2a4261af 100644
--- a/drivers/video/uvesafb.c
+++ b/drivers/video/uvesafb.c
@@ -73,7 +73,7 @@ static void uvesafb_cn_callback(struct cn_msg *msg, struct netlink_skb_parms *ns
 	struct uvesafb_task *utask;
 	struct uvesafb_ktask *task;
 
-	if (!cap_raised(current_cap(), CAP_SYS_ADMIN))
+	if (!capable(CAP_SYS_ADMIN))
 		return;
 
 	if (msg->seq >= UVESAFB_TASKS_MAX)
diff --git a/drivers/video/xen-fbfront.c b/drivers/video/xen-fbfront.c
index cb4529c40d7..b7f5173ff9e 100644
--- a/drivers/video/xen-fbfront.c
+++ b/drivers/video/xen-fbfront.c
@@ -365,7 +365,7 @@ static int __devinit xenfb_probe(struct xenbus_device *dev,
 	struct fb_info *fb_info;
 	int fb_size;
 	int val;
-	int ret;
+	int ret = 0;
 
 	info = kzalloc(sizeof(*info), GFP_KERNEL);
 	if (info == NULL) {
@@ -458,26 +458,31 @@ static int __devinit xenfb_probe(struct xenbus_device *dev,
 	xenfb_init_shared_page(info, fb_info);
 
 	ret = xenfb_connect_backend(dev, info);
-	if (ret < 0)
-		goto error;
+	if (ret < 0) {
+		xenbus_dev_fatal(dev, ret, "xenfb_connect_backend");
+		goto error_fb;
+	}
 
 	ret = register_framebuffer(fb_info);
 	if (ret) {
-		fb_deferred_io_cleanup(fb_info);
-		fb_dealloc_cmap(&fb_info->cmap);
-		framebuffer_release(fb_info);
 		xenbus_dev_fatal(dev, ret, "register_framebuffer");
-		goto error;
+		goto error_fb;
 	}
 	info->fb_info = fb_info;
 
 	xenfb_make_preferred_console();
 	return 0;
 
- error_nomem:
-	ret = -ENOMEM;
-	xenbus_dev_fatal(dev, ret, "allocating device memory");
- error:
+error_fb:
+	fb_deferred_io_cleanup(fb_info);
+	fb_dealloc_cmap(&fb_info->cmap);
+	framebuffer_release(fb_info);
+error_nomem:
+	if (!ret) {
+		ret = -ENOMEM;
+		xenbus_dev_fatal(dev, ret, "allocating device memory");
+	}
+error:
 	xenfb_remove(dev);
 	return ret;
 }
diff --git a/drivers/virtio/Kconfig b/drivers/virtio/Kconfig
index 1a61939b85f..f38b17a86c3 100644
--- a/drivers/virtio/Kconfig
+++ b/drivers/virtio/Kconfig
@@ -46,4 +46,15 @@ config VIRTIO_BALLOON
 
  	 If unsure, say N.
 
+config VIRTIO_MMIO_CMDLINE_DEVICES
+	bool "Memory mapped virtio devices parameter parsing"
+	depends on VIRTIO_MMIO
+	---help---
+	 Allow virtio-mmio devices instantiation via the kernel command line
+	 or module parameters. Be aware that using incorrect parameters (base
+	 address in particular) can crash your system - you have been warned.
+	 See Documentation/kernel-parameters.txt for details.
+
+	 If unsure, say 'N'.
+
 endmenu
diff --git a/drivers/virtio/virtio.c b/drivers/virtio/virtio.c
index 984c501c258..f3558070e37 100644
--- a/drivers/virtio/virtio.c
+++ b/drivers/virtio/virtio.c
@@ -2,9 +2,10 @@
 #include <linux/spinlock.h>
 #include <linux/virtio_config.h>
 #include <linux/module.h>
+#include <linux/idr.h>
 
 /* Unique numbering for virtio devices. */
-static unsigned int dev_index;
+static DEFINE_IDA(virtio_index_ida);
 
 static ssize_t device_show(struct device *_d,
 			   struct device_attribute *attr, char *buf)
@@ -193,7 +194,11 @@ int register_virtio_device(struct virtio_device *dev)
 	dev->dev.bus = &virtio_bus;
 
 	/* Assign a unique device index and hence name. */
-	dev->index = dev_index++;
+	err = ida_simple_get(&virtio_index_ida, 0, 0, GFP_KERNEL);
+	if (err < 0)
+		goto out;
+
+	dev->index = err;
 	dev_set_name(&dev->dev, "virtio%u", dev->index);
 
 	/* We always start by resetting the device, in case a previous
@@ -208,6 +213,7 @@ int register_virtio_device(struct virtio_device *dev)
 	/* device_register() causes the bus infrastructure to look for a
 	 * matching driver. */
 	err = device_register(&dev->dev);
+out:
 	if (err)
 		add_status(dev, VIRTIO_CONFIG_S_FAILED);
 	return err;
@@ -217,6 +223,7 @@ EXPORT_SYMBOL_GPL(register_virtio_device);
 void unregister_virtio_device(struct virtio_device *dev)
 {
 	device_unregister(&dev->dev);
+	ida_simple_remove(&virtio_index_ida, dev->index);
 }
 EXPORT_SYMBOL_GPL(unregister_virtio_device);
 
diff --git a/drivers/virtio/virtio_balloon.c b/drivers/virtio/virtio_balloon.c
index c2d05a8279f..bfbc15ca38d 100644
--- a/drivers/virtio/virtio_balloon.c
+++ b/drivers/virtio/virtio_balloon.c
@@ -381,20 +381,25 @@ out:
 	return err;
 }
 
-static void __devexit virtballoon_remove(struct virtio_device *vdev)
+static void remove_common(struct virtio_balloon *vb)
 {
-	struct virtio_balloon *vb = vdev->priv;
-
-	kthread_stop(vb->thread);
-
 	/* There might be pages left in the balloon: free them. */
 	while (vb->num_pages)
 		leak_balloon(vb, vb->num_pages);
+	update_balloon_size(vb);
 
 	/* Now we reset the device so we can clean up the queues. */
-	vdev->config->reset(vdev);
+	vb->vdev->config->reset(vb->vdev);
 
-	vdev->config->del_vqs(vdev);
+	vb->vdev->config->del_vqs(vb->vdev);
+}
+
+static void __devexit virtballoon_remove(struct virtio_device *vdev)
+{
+	struct virtio_balloon *vb = vdev->priv;
+
+	kthread_stop(vb->thread);
+	remove_common(vb);
 	kfree(vb);
 }
 
@@ -408,17 +413,11 @@ static int virtballoon_freeze(struct virtio_device *vdev)
 	 * function is called.
 	 */
 
-	while (vb->num_pages)
-		leak_balloon(vb, vb->num_pages);
-	update_balloon_size(vb);
-
-	/* Ensure we don't get any more requests from the host */
-	vdev->config->reset(vdev);
-	vdev->config->del_vqs(vdev);
+	remove_common(vb);
 	return 0;
 }
 
-static int restore_common(struct virtio_device *vdev)
+static int virtballoon_restore(struct virtio_device *vdev)
 {
 	struct virtio_balloon *vb = vdev->priv;
 	int ret;
@@ -431,11 +430,6 @@ static int restore_common(struct virtio_device *vdev)
 	update_balloon_size(vb);
 	return 0;
 }
-
-static int virtballoon_restore(struct virtio_device *vdev)
-{
-	return restore_common(vdev);
-}
 #endif
 
 static unsigned int features[] = {
diff --git a/drivers/virtio/virtio_mmio.c b/drivers/virtio/virtio_mmio.c
index 01d6dc250d5..453db0c403d 100644
--- a/drivers/virtio/virtio_mmio.c
+++ b/drivers/virtio/virtio_mmio.c
@@ -6,6 +6,50 @@
  * This module allows virtio devices to be used over a virtual, memory mapped
  * platform device.
  *
+ * The guest device(s) may be instantiated in one of three equivalent ways:
+ *
+ * 1. Static platform device in board's code, eg.:
+ *
+ *	static struct platform_device v2m_virtio_device = {
+ *		.name = "virtio-mmio",
+ *		.id = -1,
+ *		.num_resources = 2,
+ *		.resource = (struct resource []) {
+ *			{
+ *				.start = 0x1001e000,
+ *				.end = 0x1001e0ff,
+ *				.flags = IORESOURCE_MEM,
+ *			}, {
+ *				.start = 42 + 32,
+ *				.end = 42 + 32,
+ *				.flags = IORESOURCE_IRQ,
+ *			},
+ *		}
+ *	};
+ *
+ * 2. Device Tree node, eg.:
+ *
+ *		virtio_block@1e000 {
+ *			compatible = "virtio,mmio";
+ *			reg = <0x1e000 0x100>;
+ *			interrupts = <42>;
+ *		}
+ *
+ * 3. Kernel module (or command line) parameter. Can be used more than once -
+ *    one device will be created for each one. Syntax:
+ *
+ *		[virtio_mmio.]device=<size>@<baseaddr>:<irq>[:<id>]
+ *    where:
+ *		<size>     := size (can use standard suffixes like K, M or G)
+ *		<baseaddr> := physical base address
+ *		<irq>      := interrupt number (as passed to request_irq())
+ *		<id>       := (optional) platform device id
+ *    eg.:
+ *		virtio_mmio.device=0x100@0x100b0000:48 \
+ *				virtio_mmio.device=1K@0x1001e000:74
+ *
+ *
+ *
  * Registers layout (all 32-bit wide):
  *
  * offset d. name             description
@@ -42,6 +86,8 @@
  * See the COPYING file in the top-level directory.
  */
 
+#define pr_fmt(fmt) "virtio-mmio: " fmt
+
 #include <linux/highmem.h>
 #include <linux/interrupt.h>
 #include <linux/io.h>
@@ -449,6 +495,122 @@ static int __devexit virtio_mmio_remove(struct platform_device *pdev)
 
 
 
+/* Devices list parameter */
+
+#if defined(CONFIG_VIRTIO_MMIO_CMDLINE_DEVICES)
+
+static struct device vm_cmdline_parent = {
+	.init_name = "virtio-mmio-cmdline",
+};
+
+static int vm_cmdline_parent_registered;
+static int vm_cmdline_id;
+
+static int vm_cmdline_set(const char *device,
+		const struct kernel_param *kp)
+{
+	int err;
+	struct resource resources[2] = {};
+	char *str;
+	long long int base;
+	int processed, consumed = 0;
+	struct platform_device *pdev;
+
+	resources[0].flags = IORESOURCE_MEM;
+	resources[1].flags = IORESOURCE_IRQ;
+
+	resources[0].end = memparse(device, &str) - 1;
+
+	processed = sscanf(str, "@%lli:%u%n:%d%n",
+			&base, &resources[1].start, &consumed,
+			&vm_cmdline_id, &consumed);
+
+	if (processed < 2 || processed > 3 || str[consumed])
+		return -EINVAL;
+
+	resources[0].start = base;
+	resources[0].end += base;
+	resources[1].end = resources[1].start;
+
+	if (!vm_cmdline_parent_registered) {
+		err = device_register(&vm_cmdline_parent);
+		if (err) {
+			pr_err("Failed to register parent device!\n");
+			return err;
+		}
+		vm_cmdline_parent_registered = 1;
+	}
+
+	pr_info("Registering device virtio-mmio.%d at 0x%llx-0x%llx, IRQ %d.\n",
+		       vm_cmdline_id,
+		       (unsigned long long)resources[0].start,
+		       (unsigned long long)resources[0].end,
+		       (int)resources[1].start);
+
+	pdev = platform_device_register_resndata(&vm_cmdline_parent,
+			"virtio-mmio", vm_cmdline_id++,
+			resources, ARRAY_SIZE(resources), NULL, 0);
+	if (IS_ERR(pdev))
+		return PTR_ERR(pdev);
+
+	return 0;
+}
+
+static int vm_cmdline_get_device(struct device *dev, void *data)
+{
+	char *buffer = data;
+	unsigned int len = strlen(buffer);
+	struct platform_device *pdev = to_platform_device(dev);
+
+	snprintf(buffer + len, PAGE_SIZE - len, "0x%llx@0x%llx:%llu:%d\n",
+			pdev->resource[0].end - pdev->resource[0].start + 1ULL,
+			(unsigned long long)pdev->resource[0].start,
+			(unsigned long long)pdev->resource[1].start,
+			pdev->id);
+	return 0;
+}
+
+static int vm_cmdline_get(char *buffer, const struct kernel_param *kp)
+{
+	buffer[0] = '\0';
+	device_for_each_child(&vm_cmdline_parent, buffer,
+			vm_cmdline_get_device);
+	return strlen(buffer) + 1;
+}
+
+static struct kernel_param_ops vm_cmdline_param_ops = {
+	.set = vm_cmdline_set,
+	.get = vm_cmdline_get,
+};
+
+device_param_cb(device, &vm_cmdline_param_ops, NULL, S_IRUSR);
+
+static int vm_unregister_cmdline_device(struct device *dev,
+		void *data)
+{
+	platform_device_unregister(to_platform_device(dev));
+
+	return 0;
+}
+
+static void vm_unregister_cmdline_devices(void)
+{
+	if (vm_cmdline_parent_registered) {
+		device_for_each_child(&vm_cmdline_parent, NULL,
+				vm_unregister_cmdline_device);
+		device_unregister(&vm_cmdline_parent);
+		vm_cmdline_parent_registered = 0;
+	}
+}
+
+#else
+
+static void vm_unregister_cmdline_devices(void)
+{
+}
+
+#endif
+
 /* Platform driver */
 
 static struct of_device_id virtio_mmio_match[] = {
@@ -475,6 +637,7 @@ static int __init virtio_mmio_init(void)
 static void __exit virtio_mmio_exit(void)
 {
 	platform_driver_unregister(&virtio_mmio_driver);
+	vm_unregister_cmdline_devices();
 }
 
 module_init(virtio_mmio_init);
diff --git a/drivers/staging/vme/Kconfig b/drivers/vme/Kconfig
index 6411ae51ed3..c5c22465a80 100644
--- a/drivers/staging/vme/Kconfig
+++ b/drivers/vme/Kconfig
@@ -10,10 +10,10 @@ menuconfig VME_BUS
 
 if VME_BUS
 
-source "drivers/staging/vme/bridges/Kconfig"
+source "drivers/vme/bridges/Kconfig"
 
-source "drivers/staging/vme/devices/Kconfig"
+source "drivers/vme/boards/Kconfig"
 
-source "drivers/staging/vme/boards/Kconfig"
+source "drivers/staging/vme/devices/Kconfig"
 
 endif # VME
diff --git a/drivers/vme/Makefile b/drivers/vme/Makefile
new file mode 100644
index 00000000000..d7bfcb9fd5a
--- /dev/null
+++ b/drivers/vme/Makefile
@@ -0,0 +1,7 @@
+#
+# Makefile for the VME bridge device drivers.
+#
+obj-$(CONFIG_VME_BUS)		+= vme.o
+
+obj-y				+= bridges/
+obj-y				+= boards/
diff --git a/drivers/staging/vme/boards/Kconfig b/drivers/vme/boards/Kconfig
index 76163135352..76163135352 100644
--- a/drivers/staging/vme/boards/Kconfig
+++ b/drivers/vme/boards/Kconfig
diff --git a/drivers/staging/vme/boards/Makefile b/drivers/vme/boards/Makefile
index 43658340885..43658340885 100644
--- a/drivers/staging/vme/boards/Makefile
+++ b/drivers/vme/boards/Makefile
diff --git a/drivers/staging/vme/boards/vme_vmivme7805.c b/drivers/vme/boards/vme_vmivme7805.c
index 8e05bb4e135..8e05bb4e135 100644
--- a/drivers/staging/vme/boards/vme_vmivme7805.c
+++ b/drivers/vme/boards/vme_vmivme7805.c
diff --git a/drivers/staging/vme/boards/vme_vmivme7805.h b/drivers/vme/boards/vme_vmivme7805.h
index 44c2c449808..44c2c449808 100644
--- a/drivers/staging/vme/boards/vme_vmivme7805.h
+++ b/drivers/vme/boards/vme_vmivme7805.h
diff --git a/drivers/staging/vme/bridges/Kconfig b/drivers/vme/bridges/Kconfig
index 9331064e047..9331064e047 100644
--- a/drivers/staging/vme/bridges/Kconfig
+++ b/drivers/vme/bridges/Kconfig
diff --git a/drivers/staging/vme/bridges/Makefile b/drivers/vme/bridges/Makefile
index 59638afcd50..59638afcd50 100644
--- a/drivers/staging/vme/bridges/Makefile
+++ b/drivers/vme/bridges/Makefile
diff --git a/drivers/staging/vme/bridges/vme_ca91cx42.c b/drivers/vme/bridges/vme_ca91cx42.c
index 515b8b8e32a..e0df92ec44b 100644
--- a/drivers/staging/vme/bridges/vme_ca91cx42.c
+++ b/drivers/vme/bridges/vme_ca91cx42.c
@@ -29,8 +29,8 @@
 #include <linux/time.h>
 #include <linux/io.h>
 #include <linux/uaccess.h>
+#include <linux/vme.h>
 
-#include "../vme.h"
 #include "../vme_bridge.h"
 #include "vme_ca91cx42.h"
 
@@ -1501,7 +1501,7 @@ static int ca91cx42_slot_get(struct vme_bridge *ca91cx42_bridge)
 
 }
 
-void *ca91cx42_alloc_consistent(struct device *parent, size_t size,
+static void *ca91cx42_alloc_consistent(struct device *parent, size_t size,
 	dma_addr_t *dma)
 {
 	struct pci_dev *pdev;
@@ -1512,8 +1512,8 @@ void *ca91cx42_alloc_consistent(struct device *parent, size_t size,
 	return pci_alloc_consistent(pdev, size, dma);
 }
 
-void ca91cx42_free_consistent(struct device *parent, size_t size, void *vaddr,
-	dma_addr_t dma)
+static void ca91cx42_free_consistent(struct device *parent, size_t size,
+	void *vaddr, dma_addr_t dma)
 {
 	struct pci_dev *pdev;
 
diff --git a/drivers/staging/vme/bridges/vme_ca91cx42.h b/drivers/vme/bridges/vme_ca91cx42.h
index 02a7c794db0..02a7c794db0 100644
--- a/drivers/staging/vme/bridges/vme_ca91cx42.h
+++ b/drivers/vme/bridges/vme_ca91cx42.h
diff --git a/drivers/staging/vme/bridges/vme_tsi148.c b/drivers/vme/bridges/vme_tsi148.c
index f50582169b2..f6385f7a66d 100644
--- a/drivers/staging/vme/bridges/vme_tsi148.c
+++ b/drivers/vme/bridges/vme_tsi148.c
@@ -29,8 +29,9 @@
 #include <linux/time.h>
 #include <linux/io.h>
 #include <linux/uaccess.h>
+#include <linux/byteorder/generic.h>
+#include <linux/vme.h>
 
-#include "../vme.h"
 #include "../vme_bridge.h"
 #include "vme_tsi148.h"
 
@@ -1415,51 +1416,55 @@ static unsigned int tsi148_master_rmw(struct vme_master_resource *image,
 	return result;
 }
 
-static int tsi148_dma_set_vme_src_attributes(struct device *dev, u32 *attr,
+static int tsi148_dma_set_vme_src_attributes(struct device *dev, __be32 *attr,
 	u32 aspace, u32 cycle, u32 dwidth)
 {
+	u32 val;
+
+	val = be32_to_cpu(*attr);
+
 	/* Setup 2eSST speeds */
 	switch (cycle & (VME_2eSST160 | VME_2eSST267 | VME_2eSST320)) {
 	case VME_2eSST160:
-		*attr |= TSI148_LCSR_DSAT_2eSSTM_160;
+		val |= TSI148_LCSR_DSAT_2eSSTM_160;
 		break;
 	case VME_2eSST267:
-		*attr |= TSI148_LCSR_DSAT_2eSSTM_267;
+		val |= TSI148_LCSR_DSAT_2eSSTM_267;
 		break;
 	case VME_2eSST320:
-		*attr |= TSI148_LCSR_DSAT_2eSSTM_320;
+		val |= TSI148_LCSR_DSAT_2eSSTM_320;
 		break;
 	}
 
 	/* Setup cycle types */
 	if (cycle & VME_SCT)
-		*attr |= TSI148_LCSR_DSAT_TM_SCT;
+		val |= TSI148_LCSR_DSAT_TM_SCT;
 
 	if (cycle & VME_BLT)
-		*attr |= TSI148_LCSR_DSAT_TM_BLT;
+		val |= TSI148_LCSR_DSAT_TM_BLT;
 
 	if (cycle & VME_MBLT)
-		*attr |= TSI148_LCSR_DSAT_TM_MBLT;
+		val |= TSI148_LCSR_DSAT_TM_MBLT;
 
 	if (cycle & VME_2eVME)
-		*attr |= TSI148_LCSR_DSAT_TM_2eVME;
+		val |= TSI148_LCSR_DSAT_TM_2eVME;
 
 	if (cycle & VME_2eSST)
-		*attr |= TSI148_LCSR_DSAT_TM_2eSST;
+		val |= TSI148_LCSR_DSAT_TM_2eSST;
 
 	if (cycle & VME_2eSSTB) {
 		dev_err(dev, "Currently not setting Broadcast Select "
 			"Registers\n");
-		*attr |= TSI148_LCSR_DSAT_TM_2eSSTB;
+		val |= TSI148_LCSR_DSAT_TM_2eSSTB;
 	}
 
 	/* Setup data width */
 	switch (dwidth) {
 	case VME_D16:
-		*attr |= TSI148_LCSR_DSAT_DBW_16;
+		val |= TSI148_LCSR_DSAT_DBW_16;
 		break;
 	case VME_D32:
-		*attr |= TSI148_LCSR_DSAT_DBW_32;
+		val |= TSI148_LCSR_DSAT_DBW_32;
 		break;
 	default:
 		dev_err(dev, "Invalid data width\n");
@@ -1469,31 +1474,31 @@ static int tsi148_dma_set_vme_src_attributes(struct device *dev, u32 *attr,
 	/* Setup address space */
 	switch (aspace) {
 	case VME_A16:
-		*attr |= TSI148_LCSR_DSAT_AMODE_A16;
+		val |= TSI148_LCSR_DSAT_AMODE_A16;
 		break;
 	case VME_A24:
-		*attr |= TSI148_LCSR_DSAT_AMODE_A24;
+		val |= TSI148_LCSR_DSAT_AMODE_A24;
 		break;
 	case VME_A32:
-		*attr |= TSI148_LCSR_DSAT_AMODE_A32;
+		val |= TSI148_LCSR_DSAT_AMODE_A32;
 		break;
 	case VME_A64:
-		*attr |= TSI148_LCSR_DSAT_AMODE_A64;
+		val |= TSI148_LCSR_DSAT_AMODE_A64;
 		break;
 	case VME_CRCSR:
-		*attr |= TSI148_LCSR_DSAT_AMODE_CRCSR;
+		val |= TSI148_LCSR_DSAT_AMODE_CRCSR;
 		break;
 	case VME_USER1:
-		*attr |= TSI148_LCSR_DSAT_AMODE_USER1;
+		val |= TSI148_LCSR_DSAT_AMODE_USER1;
 		break;
 	case VME_USER2:
-		*attr |= TSI148_LCSR_DSAT_AMODE_USER2;
+		val |= TSI148_LCSR_DSAT_AMODE_USER2;
 		break;
 	case VME_USER3:
-		*attr |= TSI148_LCSR_DSAT_AMODE_USER3;
+		val |= TSI148_LCSR_DSAT_AMODE_USER3;
 		break;
 	case VME_USER4:
-		*attr |= TSI148_LCSR_DSAT_AMODE_USER4;
+		val |= TSI148_LCSR_DSAT_AMODE_USER4;
 		break;
 	default:
 		dev_err(dev, "Invalid address space\n");
@@ -1502,58 +1507,64 @@ static int tsi148_dma_set_vme_src_attributes(struct device *dev, u32 *attr,
 	}
 
 	if (cycle & VME_SUPER)
-		*attr |= TSI148_LCSR_DSAT_SUP;
+		val |= TSI148_LCSR_DSAT_SUP;
 	if (cycle & VME_PROG)
-		*attr |= TSI148_LCSR_DSAT_PGM;
+		val |= TSI148_LCSR_DSAT_PGM;
+
+	*attr = cpu_to_be32(val);
 
 	return 0;
 }
 
-static int tsi148_dma_set_vme_dest_attributes(struct device *dev, u32 *attr,
+static int tsi148_dma_set_vme_dest_attributes(struct device *dev, __be32 *attr,
 	u32 aspace, u32 cycle, u32 dwidth)
 {
+	u32 val;
+
+	val = be32_to_cpu(*attr);
+
 	/* Setup 2eSST speeds */
 	switch (cycle & (VME_2eSST160 | VME_2eSST267 | VME_2eSST320)) {
 	case VME_2eSST160:
-		*attr |= TSI148_LCSR_DDAT_2eSSTM_160;
+		val |= TSI148_LCSR_DDAT_2eSSTM_160;
 		break;
 	case VME_2eSST267:
-		*attr |= TSI148_LCSR_DDAT_2eSSTM_267;
+		val |= TSI148_LCSR_DDAT_2eSSTM_267;
 		break;
 	case VME_2eSST320:
-		*attr |= TSI148_LCSR_DDAT_2eSSTM_320;
+		val |= TSI148_LCSR_DDAT_2eSSTM_320;
 		break;
 	}
 
 	/* Setup cycle types */
 	if (cycle & VME_SCT)
-		*attr |= TSI148_LCSR_DDAT_TM_SCT;
+		val |= TSI148_LCSR_DDAT_TM_SCT;
 
 	if (cycle & VME_BLT)
-		*attr |= TSI148_LCSR_DDAT_TM_BLT;
+		val |= TSI148_LCSR_DDAT_TM_BLT;
 
 	if (cycle & VME_MBLT)
-		*attr |= TSI148_LCSR_DDAT_TM_MBLT;
+		val |= TSI148_LCSR_DDAT_TM_MBLT;
 
 	if (cycle & VME_2eVME)
-		*attr |= TSI148_LCSR_DDAT_TM_2eVME;
+		val |= TSI148_LCSR_DDAT_TM_2eVME;
 
 	if (cycle & VME_2eSST)
-		*attr |= TSI148_LCSR_DDAT_TM_2eSST;
+		val |= TSI148_LCSR_DDAT_TM_2eSST;
 
 	if (cycle & VME_2eSSTB) {
 		dev_err(dev, "Currently not setting Broadcast Select "
 			"Registers\n");
-		*attr |= TSI148_LCSR_DDAT_TM_2eSSTB;
+		val |= TSI148_LCSR_DDAT_TM_2eSSTB;
 	}
 
 	/* Setup data width */
 	switch (dwidth) {
 	case VME_D16:
-		*attr |= TSI148_LCSR_DDAT_DBW_16;
+		val |= TSI148_LCSR_DDAT_DBW_16;
 		break;
 	case VME_D32:
-		*attr |= TSI148_LCSR_DDAT_DBW_32;
+		val |= TSI148_LCSR_DDAT_DBW_32;
 		break;
 	default:
 		dev_err(dev, "Invalid data width\n");
@@ -1563,31 +1574,31 @@ static int tsi148_dma_set_vme_dest_attributes(struct device *dev, u32 *attr,
 	/* Setup address space */
 	switch (aspace) {
 	case VME_A16:
-		*attr |= TSI148_LCSR_DDAT_AMODE_A16;
+		val |= TSI148_LCSR_DDAT_AMODE_A16;
 		break;
 	case VME_A24:
-		*attr |= TSI148_LCSR_DDAT_AMODE_A24;
+		val |= TSI148_LCSR_DDAT_AMODE_A24;
 		break;
 	case VME_A32:
-		*attr |= TSI148_LCSR_DDAT_AMODE_A32;
+		val |= TSI148_LCSR_DDAT_AMODE_A32;
 		break;
 	case VME_A64:
-		*attr |= TSI148_LCSR_DDAT_AMODE_A64;
+		val |= TSI148_LCSR_DDAT_AMODE_A64;
 		break;
 	case VME_CRCSR:
-		*attr |= TSI148_LCSR_DDAT_AMODE_CRCSR;
+		val |= TSI148_LCSR_DDAT_AMODE_CRCSR;
 		break;
 	case VME_USER1:
-		*attr |= TSI148_LCSR_DDAT_AMODE_USER1;
+		val |= TSI148_LCSR_DDAT_AMODE_USER1;
 		break;
 	case VME_USER2:
-		*attr |= TSI148_LCSR_DDAT_AMODE_USER2;
+		val |= TSI148_LCSR_DDAT_AMODE_USER2;
 		break;
 	case VME_USER3:
-		*attr |= TSI148_LCSR_DDAT_AMODE_USER3;
+		val |= TSI148_LCSR_DDAT_AMODE_USER3;
 		break;
 	case VME_USER4:
-		*attr |= TSI148_LCSR_DDAT_AMODE_USER4;
+		val |= TSI148_LCSR_DDAT_AMODE_USER4;
 		break;
 	default:
 		dev_err(dev, "Invalid address space\n");
@@ -1596,25 +1607,28 @@ static int tsi148_dma_set_vme_dest_attributes(struct device *dev, u32 *attr,
 	}
 
 	if (cycle & VME_SUPER)
-		*attr |= TSI148_LCSR_DDAT_SUP;
+		val |= TSI148_LCSR_DDAT_SUP;
 	if (cycle & VME_PROG)
-		*attr |= TSI148_LCSR_DDAT_PGM;
+		val |= TSI148_LCSR_DDAT_PGM;
+
+	*attr = cpu_to_be32(val);
 
 	return 0;
 }
 
 /*
  * Add a link list descriptor to the list
+ *
+ * Note: DMA engine expects the DMA descriptor to be big endian.
  */
 static int tsi148_dma_list_add(struct vme_dma_list *list,
 	struct vme_dma_attr *src, struct vme_dma_attr *dest, size_t count)
 {
 	struct tsi148_dma_entry *entry, *prev;
-	u32 address_high, address_low;
+	u32 address_high, address_low, val;
 	struct vme_dma_pattern *pattern_attr;
 	struct vme_dma_pci *pci_attr;
 	struct vme_dma_vme *vme_attr;
-	dma_addr_t desc_ptr;
 	int retval = 0;
 	struct vme_bridge *tsi148_bridge;
 
@@ -1648,34 +1662,36 @@ static int tsi148_dma_list_add(struct vme_dma_list *list,
 	case VME_DMA_PATTERN:
 		pattern_attr = src->private;
 
-		entry->descriptor.dsal = pattern_attr->pattern;
-		entry->descriptor.dsat = TSI148_LCSR_DSAT_TYP_PAT;
+		entry->descriptor.dsal = cpu_to_be32(pattern_attr->pattern);
+
+		val = TSI148_LCSR_DSAT_TYP_PAT;
+
 		/* Default behaviour is 32 bit pattern */
 		if (pattern_attr->type & VME_DMA_PATTERN_BYTE)
-			entry->descriptor.dsat |= TSI148_LCSR_DSAT_PSZ;
+			val |= TSI148_LCSR_DSAT_PSZ;
 
 		/* It seems that the default behaviour is to increment */
 		if ((pattern_attr->type & VME_DMA_PATTERN_INCREMENT) == 0)
-			entry->descriptor.dsat |= TSI148_LCSR_DSAT_NIN;
-
+			val |= TSI148_LCSR_DSAT_NIN;
+		entry->descriptor.dsat = cpu_to_be32(val);
 		break;
 	case VME_DMA_PCI:
 		pci_attr = src->private;
 
 		reg_split((unsigned long long)pci_attr->address, &address_high,
 			&address_low);
-		entry->descriptor.dsau = address_high;
-		entry->descriptor.dsal = address_low;
-		entry->descriptor.dsat = TSI148_LCSR_DSAT_TYP_PCI;
+		entry->descriptor.dsau = cpu_to_be32(address_high);
+		entry->descriptor.dsal = cpu_to_be32(address_low);
+		entry->descriptor.dsat = cpu_to_be32(TSI148_LCSR_DSAT_TYP_PCI);
 		break;
 	case VME_DMA_VME:
 		vme_attr = src->private;
 
 		reg_split((unsigned long long)vme_attr->address, &address_high,
 			&address_low);
-		entry->descriptor.dsau = address_high;
-		entry->descriptor.dsal = address_low;
-		entry->descriptor.dsat = TSI148_LCSR_DSAT_TYP_VME;
+		entry->descriptor.dsau = cpu_to_be32(address_high);
+		entry->descriptor.dsal = cpu_to_be32(address_low);
+		entry->descriptor.dsat = cpu_to_be32(TSI148_LCSR_DSAT_TYP_VME);
 
 		retval = tsi148_dma_set_vme_src_attributes(
 			tsi148_bridge->parent, &entry->descriptor.dsat,
@@ -1691,9 +1707,8 @@ static int tsi148_dma_list_add(struct vme_dma_list *list,
 	}
 
 	/* Assume last link - this will be over-written by adding another */
-	entry->descriptor.dnlau = 0;
-	entry->descriptor.dnlal = TSI148_LCSR_DNLAL_LLA;
-
+	entry->descriptor.dnlau = cpu_to_be32(0);
+	entry->descriptor.dnlal = cpu_to_be32(TSI148_LCSR_DNLAL_LLA);
 
 	/* Fill out destination part */
 	switch (dest->type) {
@@ -1702,18 +1717,18 @@ static int tsi148_dma_list_add(struct vme_dma_list *list,
 
 		reg_split((unsigned long long)pci_attr->address, &address_high,
 			&address_low);
-		entry->descriptor.ddau = address_high;
-		entry->descriptor.ddal = address_low;
-		entry->descriptor.ddat = TSI148_LCSR_DDAT_TYP_PCI;
+		entry->descriptor.ddau = cpu_to_be32(address_high);
+		entry->descriptor.ddal = cpu_to_be32(address_low);
+		entry->descriptor.ddat = cpu_to_be32(TSI148_LCSR_DDAT_TYP_PCI);
 		break;
 	case VME_DMA_VME:
 		vme_attr = dest->private;
 
 		reg_split((unsigned long long)vme_attr->address, &address_high,
 			&address_low);
-		entry->descriptor.ddau = address_high;
-		entry->descriptor.ddal = address_low;
-		entry->descriptor.ddat = TSI148_LCSR_DDAT_TYP_VME;
+		entry->descriptor.ddau = cpu_to_be32(address_high);
+		entry->descriptor.ddal = cpu_to_be32(address_low);
+		entry->descriptor.ddat = cpu_to_be32(TSI148_LCSR_DDAT_TYP_VME);
 
 		retval = tsi148_dma_set_vme_dest_attributes(
 			tsi148_bridge->parent, &entry->descriptor.ddat,
@@ -1729,7 +1744,7 @@ static int tsi148_dma_list_add(struct vme_dma_list *list,
 	}
 
 	/* Fill out count */
-	entry->descriptor.dcnt = (u32)count;
+	entry->descriptor.dcnt = cpu_to_be32((u32)count);
 
 	/* Add to list */
 	list_add_tail(&entry->list, &list->entries);
@@ -1739,9 +1754,15 @@ static int tsi148_dma_list_add(struct vme_dma_list *list,
 		prev = list_entry(entry->list.prev, struct tsi148_dma_entry,
 			list);
 		/* We need the bus address for the pointer */
-		desc_ptr = virt_to_bus(&entry->descriptor);
-		reg_split(desc_ptr, &prev->descriptor.dnlau,
-			&prev->descriptor.dnlal);
+		entry->dma_handle = dma_map_single(tsi148_bridge->parent,
+			&entry->descriptor,
+			sizeof(struct tsi148_dma_descriptor), DMA_TO_DEVICE);
+
+		reg_split((unsigned long long)entry->dma_handle, &address_high,
+			&address_low);
+		entry->descriptor.dnlau = cpu_to_be32(address_high);
+		entry->descriptor.dnlal = cpu_to_be32(address_low);
+
 	}
 
 	return 0;
@@ -1784,7 +1805,6 @@ static int tsi148_dma_list_exec(struct vme_dma_list *list)
 	struct vme_dma_resource *ctrlr;
 	int channel, retval = 0;
 	struct tsi148_dma_entry *entry;
-	dma_addr_t bus_addr;
 	u32 bus_addr_high, bus_addr_low;
 	u32 val, dctlreg = 0;
 	struct vme_bridge *tsi148_bridge;
@@ -1817,23 +1837,29 @@ static int tsi148_dma_list_exec(struct vme_dma_list *list)
 	entry = list_first_entry(&list->entries, struct tsi148_dma_entry,
 		list);
 
-	bus_addr = virt_to_bus(&entry->descriptor);
+	entry->dma_handle = dma_map_single(tsi148_bridge->parent,
+		&entry->descriptor,
+		sizeof(struct tsi148_dma_descriptor), DMA_TO_DEVICE);
 
 	mutex_unlock(&ctrlr->mtx);
 
-	reg_split(bus_addr, &bus_addr_high, &bus_addr_low);
+	reg_split(entry->dma_handle, &bus_addr_high, &bus_addr_low);
 
 	iowrite32be(bus_addr_high, bridge->base +
 		TSI148_LCSR_DMA[channel] + TSI148_LCSR_OFFSET_DNLAU);
 	iowrite32be(bus_addr_low, bridge->base +
 		TSI148_LCSR_DMA[channel] + TSI148_LCSR_OFFSET_DNLAL);
 
+	dctlreg = ioread32be(bridge->base + TSI148_LCSR_DMA[channel] +
+		TSI148_LCSR_OFFSET_DCTL);
+
 	/* Start the operation */
 	iowrite32be(dctlreg | TSI148_LCSR_DCTL_DGO, bridge->base +
 		TSI148_LCSR_DMA[channel] + TSI148_LCSR_OFFSET_DCTL);
 
 	wait_event_interruptible(bridge->dma_queue[channel],
 		tsi148_dma_busy(ctrlr->parent, channel));
+
 	/*
 	 * Read status register, this register is valid until we kick off a
 	 * new transfer.
@@ -1864,10 +1890,15 @@ static int tsi148_dma_list_empty(struct vme_dma_list *list)
 	struct list_head *pos, *temp;
 	struct tsi148_dma_entry *entry;
 
+	struct vme_bridge *tsi148_bridge = list->parent->parent;
+
 	/* detach and free each entry */
 	list_for_each_safe(pos, temp, &list->entries) {
 		list_del(pos);
 		entry = list_entry(pos, struct tsi148_dma_entry, list);
+
+		dma_unmap_single(tsi148_bridge->parent, entry->dma_handle,
+			sizeof(struct tsi148_dma_descriptor), DMA_TO_DEVICE);
 		kfree(entry);
 	}
 
@@ -2110,7 +2141,7 @@ static int tsi148_slot_get(struct vme_bridge *tsi148_bridge)
 	return (int)slot;
 }
 
-void *tsi148_alloc_consistent(struct device *parent, size_t size,
+static void *tsi148_alloc_consistent(struct device *parent, size_t size,
 	dma_addr_t *dma)
 {
 	struct pci_dev *pdev;
@@ -2121,8 +2152,8 @@ void *tsi148_alloc_consistent(struct device *parent, size_t size,
 	return pci_alloc_consistent(pdev, size, dma);
 }
 
-void tsi148_free_consistent(struct device *parent, size_t size, void *vaddr,
-	dma_addr_t dma)
+static void tsi148_free_consistent(struct device *parent, size_t size,
+	void *vaddr, dma_addr_t dma)
 {
 	struct pci_dev *pdev;
 
diff --git a/drivers/staging/vme/bridges/vme_tsi148.h b/drivers/vme/bridges/vme_tsi148.h
index a3ac2fe9881..f5ed14382a8 100644
--- a/drivers/staging/vme/bridges/vme_tsi148.h
+++ b/drivers/vme/bridges/vme_tsi148.h
@@ -56,16 +56,16 @@ struct tsi148_driver {
  *       correctly laid out - It must also be aligned on 64-bit boundaries.
  */
 struct tsi148_dma_descriptor {
-	u32 dsau;      /* Source Address */
-	u32 dsal;
-	u32 ddau;      /* Destination Address */
-	u32 ddal;
-	u32 dsat;      /* Source attributes */
-	u32 ddat;      /* Destination attributes */
-	u32 dnlau;     /* Next link address */
-	u32 dnlal;
-	u32 dcnt;      /* Byte count */
-	u32 ddbs;      /* 2eSST Broadcast select */
+	__be32 dsau;      /* Source Address */
+	__be32 dsal;
+	__be32 ddau;      /* Destination Address */
+	__be32 ddal;
+	__be32 dsat;      /* Source attributes */
+	__be32 ddat;      /* Destination attributes */
+	__be32 dnlau;     /* Next link address */
+	__be32 dnlal;
+	__be32 dcnt;      /* Byte count */
+	__be32 ddbs;      /* 2eSST Broadcast select */
 };
 
 struct tsi148_dma_entry {
@@ -75,6 +75,7 @@ struct tsi148_dma_entry {
 	 */
 	struct tsi148_dma_descriptor descriptor;
 	struct list_head list;
+	dma_addr_t dma_handle;
 };
 
 /*
diff --git a/drivers/staging/vme/vme.c b/drivers/vme/vme.c
index 70722ae5232..95a9f71d793 100644
--- a/drivers/staging/vme/vme.c
+++ b/drivers/vme/vme.c
@@ -30,8 +30,8 @@
 #include <linux/mutex.h>
 #include <linux/spinlock.h>
 #include <linux/slab.h>
+#include <linux/vme.h>
 
-#include "vme.h"
 #include "vme_bridge.h"
 
 /* Bitmask and list of registered buses both protected by common mutex */
@@ -98,14 +98,13 @@ void *vme_alloc_consistent(struct vme_resource *resource, size_t size,
 	}
 
 	if (bridge->parent == NULL) {
-		printk(KERN_ERR "Dev entry NULL for"
-			" bridge %s\n", bridge->name);
+		printk(KERN_ERR "Dev entry NULL for bridge %s\n", bridge->name);
 		return NULL;
 	}
 
 	if (bridge->alloc_consistent == NULL) {
-		printk(KERN_ERR "alloc_consistent not supported by"
-			" bridge %s\n", bridge->name);
+		printk(KERN_ERR "alloc_consistent not supported by bridge %s\n",
+		       bridge->name);
 		return NULL;
 	}
 
@@ -133,14 +132,13 @@ void vme_free_consistent(struct vme_resource *resource, size_t size,
 	}
 
 	if (bridge->parent == NULL) {
-		printk(KERN_ERR "Dev entry NULL for"
-			" bridge %s\n", bridge->name);
+		printk(KERN_ERR "Dev entry NULL for bridge %s\n", bridge->name);
 		return;
 	}
 
 	if (bridge->free_consistent == NULL) {
-		printk(KERN_ERR "free_consistent not supported by"
-			" bridge %s\n", bridge->name);
+		printk(KERN_ERR "free_consistent not supported by bridge %s\n",
+		       bridge->name);
 		return;
 	}
 
@@ -747,15 +745,13 @@ struct vme_dma_attr *vme_dma_pattern_attribute(u32 pattern, u32 type)
 
 	attributes = kmalloc(sizeof(struct vme_dma_attr), GFP_KERNEL);
 	if (attributes == NULL) {
-		printk(KERN_ERR "Unable to allocate memory for attributes "
-			"structure\n");
+		printk(KERN_ERR "Unable to allocate memory for attributes structure\n");
 		goto err_attr;
 	}
 
 	pattern_attr = kmalloc(sizeof(struct vme_dma_pattern), GFP_KERNEL);
 	if (pattern_attr == NULL) {
-		printk(KERN_ERR "Unable to allocate memory for pattern "
-			"attributes\n");
+		printk(KERN_ERR "Unable to allocate memory for pattern attributes\n");
 		goto err_pat;
 	}
 
@@ -786,15 +782,13 @@ struct vme_dma_attr *vme_dma_pci_attribute(dma_addr_t address)
 
 	attributes = kmalloc(sizeof(struct vme_dma_attr), GFP_KERNEL);
 	if (attributes == NULL) {
-		printk(KERN_ERR "Unable to allocate memory for attributes "
-			"structure\n");
+		printk(KERN_ERR "Unable to allocate memory for attributes structure\n");
 		goto err_attr;
 	}
 
 	pci_attr = kmalloc(sizeof(struct vme_dma_pci), GFP_KERNEL);
 	if (pci_attr == NULL) {
-		printk(KERN_ERR "Unable to allocate memory for pci "
-			"attributes\n");
+		printk(KERN_ERR "Unable to allocate memory for pci attributes\n");
 		goto err_pci;
 	}
 
@@ -826,15 +820,13 @@ struct vme_dma_attr *vme_dma_vme_attribute(unsigned long long address,
 	attributes = kmalloc(
 		sizeof(struct vme_dma_attr), GFP_KERNEL);
 	if (attributes == NULL) {
-		printk(KERN_ERR "Unable to allocate memory for attributes "
-			"structure\n");
+		printk(KERN_ERR "Unable to allocate memory for attributes structure\n");
 		goto err_attr;
 	}
 
 	vme_attr = kmalloc(sizeof(struct vme_dma_vme), GFP_KERNEL);
 	if (vme_attr == NULL) {
-		printk(KERN_ERR "Unable to allocate memory for vme "
-			"attributes\n");
+		printk(KERN_ERR "Unable to allocate memory for vme attributes\n");
 		goto err_vme;
 	}
 
@@ -982,8 +974,8 @@ void vme_irq_handler(struct vme_bridge *bridge, int level, int statid)
 	if (call != NULL)
 		call(level, statid, priv_data);
 	else
-		printk(KERN_WARNING "Spurilous VME interrupt, level:%x, "
-			"vector:%x\n", level, statid);
+		printk(KERN_WARNING "Spurilous VME interrupt, level:%x, vector:%x\n",
+		       level, statid);
 }
 EXPORT_SYMBOL(vme_irq_handler);
 
@@ -1112,8 +1104,7 @@ struct vme_resource *vme_lm_request(struct vme_dev *vdev)
 			struct vme_lm_resource, list);
 
 		if (lm == NULL) {
-			printk(KERN_ERR "Registered NULL Location Monitor "
-				"resource\n");
+			printk(KERN_ERR "Registered NULL Location Monitor resource\n");
 			continue;
 		}
 
diff --git a/drivers/staging/vme/vme_bridge.h b/drivers/vme/vme_bridge.h
index 934949abd74..934949abd74 100644
--- a/drivers/staging/vme/vme_bridge.h
+++ b/drivers/vme/vme_bridge.h
diff --git a/drivers/w1/Kconfig b/drivers/w1/Kconfig
index fd2c7bd9dfb..6743bde038c 100644
--- a/drivers/w1/Kconfig
+++ b/drivers/w1/Kconfig
@@ -16,7 +16,7 @@ config W1_CON
 	depends on CONNECTOR
 	bool "Userspace communication over connector"
 	default y
-	--- help ---
+	---help---
 	  This allows to communicate with userspace using connector. For more
 	  information see <file:Documentation/connector/connector.txt>.
 	  There are three types of messages between w1 core and userspace:
diff --git a/drivers/w1/masters/mxc_w1.c b/drivers/w1/masters/mxc_w1.c
index a3b6a74c67a..1cc61a700fa 100644
--- a/drivers/w1/masters/mxc_w1.c
+++ b/drivers/w1/masters/mxc_w1.c
@@ -138,7 +138,7 @@ static int __devinit mxc_w1_probe(struct platform_device *pdev)
 		goto failed_ioremap;
 	}
 
-	clk_enable(mdev->clk);
+	clk_prepare_enable(mdev->clk);
 	__raw_writeb(mdev->clkdiv, mdev->regs + MXC_W1_TIME_DIVIDER);
 
 	mdev->bus_master.data = mdev;
@@ -178,7 +178,7 @@ static int __devexit mxc_w1_remove(struct platform_device *pdev)
 
 	iounmap(mdev->regs);
 	release_mem_region(res->start, resource_size(res));
-	clk_disable(mdev->clk);
+	clk_disable_unprepare(mdev->clk);
 	clk_put(mdev->clk);
 
 	platform_set_drvdata(pdev, NULL);
diff --git a/drivers/w1/slaves/w1_ds2408.c b/drivers/w1/slaves/w1_ds2408.c
index 7c8cdb8aed2..8e813eed0f0 100644
--- a/drivers/w1/slaves/w1_ds2408.c
+++ b/drivers/w1/slaves/w1_ds2408.c
@@ -332,7 +332,6 @@ static struct bin_attribute w1_f29_sysfs_bin_files[NB_SYSFS_BIN_FILES] = {
 		},
 		.size = 1,
 		.read = w1_f29_read_cond_search_mask,
-		.write = 0,
 	},
 	{
 		.attr =	{
@@ -341,7 +340,6 @@ static struct bin_attribute w1_f29_sysfs_bin_files[NB_SYSFS_BIN_FILES] = {
 		},
 		.size = 1,
 		.read = w1_f29_read_cond_search_polarity,
-		.write = 0,
 	},
 	{
 		.attr =	{
diff --git a/drivers/w1/w1.c b/drivers/w1/w1.c
index 9761950697b..2f2e894ea0c 100644
--- a/drivers/w1/w1.c
+++ b/drivers/w1/w1.c
@@ -1027,7 +1027,7 @@ static int __init w1_init(void)
 	retval = driver_register(&w1_slave_driver);
 	if (retval) {
 		printk(KERN_ERR
-			"Failed to register master driver. err=%d.\n",
+			"Failed to register slave driver. err=%d.\n",
 			retval);
 		goto err_out_master_unregister;
 	}
diff --git a/drivers/w1/w1_io.c b/drivers/w1/w1_io.c
index 3135b2c6399..e10acc23773 100644
--- a/drivers/w1/w1_io.c
+++ b/drivers/w1/w1_io.c
@@ -31,6 +31,9 @@
 static int w1_delay_parm = 1;
 module_param_named(delay_coef, w1_delay_parm, int, 0);
 
+static int w1_disable_irqs = 0;
+module_param_named(disable_irqs, w1_disable_irqs, int, 0);
+
 static u8 w1_crc8_table[] = {
 	0, 94, 188, 226, 97, 63, 221, 131, 194, 156, 126, 32, 163, 253, 31, 65,
 	157, 195, 33, 127, 252, 162, 64, 30, 95, 1, 227, 189, 62, 96, 130, 220,
@@ -79,6 +82,10 @@ static u8 w1_touch_bit(struct w1_master *dev, int bit)
  */
 static void w1_write_bit(struct w1_master *dev, int bit)
 {
+	unsigned long flags = 0;
+
+	if(w1_disable_irqs) local_irq_save(flags);
+
 	if (bit) {
 		dev->bus_master->write_bit(dev->bus_master->data, 0);
 		w1_delay(6);
@@ -90,6 +97,8 @@ static void w1_write_bit(struct w1_master *dev, int bit)
 		dev->bus_master->write_bit(dev->bus_master->data, 1);
 		w1_delay(10);
 	}
+
+	if(w1_disable_irqs) local_irq_restore(flags);
 }
 
 /**
@@ -158,7 +167,7 @@ EXPORT_SYMBOL_GPL(w1_write_8);
 static u8 w1_read_bit(struct w1_master *dev)
 {
 	int result;
-	unsigned long flags;
+	unsigned long flags = 0;
 
 	/* sample timing is critical here */
 	local_irq_save(flags);
@@ -318,6 +327,9 @@ EXPORT_SYMBOL_GPL(w1_read_block);
 int w1_reset_bus(struct w1_master *dev)
 {
 	int result;
+	unsigned long flags = 0;
+
+	if(w1_disable_irqs) local_irq_save(flags);
 
 	if (dev->bus_master->reset_bus)
 		result = dev->bus_master->reset_bus(dev->bus_master->data) & 0x1;
@@ -330,19 +342,21 @@ int w1_reset_bus(struct w1_master *dev)
 		 * cpu for such a short amount of time AND get it back in
 		 * the maximum amount of time.
 		 */
-		w1_delay(480);
+		w1_delay(500);
 		dev->bus_master->write_bit(dev->bus_master->data, 1);
 		w1_delay(70);
 
 		result = dev->bus_master->read_bit(dev->bus_master->data) & 0x1;
-		/* minmum 70 (above) + 410 = 480 us
+		/* minmum 70 (above) + 430 = 500 us
 		 * There aren't any timing requirements between a reset and
 		 * the following transactions.  Sleeping is safe here.
 		 */
-		/* w1_delay(410); min required time */
+		/* w1_delay(430); min required time */
 		msleep(1);
 	}
 
+	if(w1_disable_irqs) local_irq_restore(flags);
+
 	return result;
 }
 EXPORT_SYMBOL_GPL(w1_reset_bus);
diff --git a/drivers/watchdog/Kconfig b/drivers/watchdog/Kconfig
index 37096246c93..d92d7488be1 100644
--- a/drivers/watchdog/Kconfig
+++ b/drivers/watchdog/Kconfig
@@ -129,17 +129,6 @@ config 977_WATCHDOG
 
 	  Not sure? It's safe to say N.
 
-config IXP2000_WATCHDOG
-	tristate "IXP2000 Watchdog"
-	depends on ARCH_IXP2000
-	help
-	  Say Y here if to include support for the watchdog timer
-	  in the Intel IXP2000(2400, 2800, 2850) network processors.
-	  This driver can be built as a module by choosing M. The module
-	  will be called ixp2000_wdt.
-
-	  Say N if you are unsure.
-
 config IXP4XX_WATCHDOG
 	tristate "IXP4xx Watchdog"
 	depends on ARCH_IXP4XX
@@ -543,7 +532,7 @@ config WAFER_WDT
 
 config I6300ESB_WDT
 	tristate "Intel 6300ESB Timer/Watchdog"
-	depends on X86 && PCI
+	depends on PCI
 	---help---
 	  Hardware driver for the watchdog timer built into the Intel
 	  6300ESB controller hub.
@@ -551,6 +540,19 @@ config I6300ESB_WDT
 	  To compile this driver as a module, choose M here: the
 	  module will be called i6300esb.
 
+config IE6XX_WDT
+	tristate "Intel Atom E6xx Watchdog"
+	depends on X86 && PCI
+	select WATCHDOG_CORE
+	select MFD_CORE
+	select LPC_SCH
+	---help---
+	  Hardware driver for the watchdog timer built into the Intel
+	  Atom E6XX (TunnelCreek) processor.
+
+	  To compile this driver as a module, choose M here: the
+	  module will be called ie6xx_wdt.
+
 config INTEL_SCU_WATCHDOG
 	bool "Intel SCU Watchdog for Mobile Platforms"
 	depends on X86_MRST
@@ -563,6 +565,7 @@ config INTEL_SCU_WATCHDOG
 config ITCO_WDT
 	tristate "Intel TCO Timer/Watchdog"
 	depends on (X86 || IA64) && PCI
+	select LPC_ICH
 	---help---
 	  Hardware driver for the intel TCO timer based watchdog devices.
 	  These drivers are included in the Intel 82801 I/O Controller
@@ -607,7 +610,12 @@ config IT87_WDT
 	depends on X86 && EXPERIMENTAL
 	---help---
 	  This is the driver for the hardware watchdog on the ITE IT8702,
-	  IT8712, IT8716, IT8718, IT8720, IT8721, IT8726 Super I/O chips.
+	  IT8712, IT8716, IT8718, IT8720, IT8721, IT8726 and IT8728
+	  Super I/O chips.
+
+	  If the driver does not work, then make sure that the game port in
+	  the BIOS is enabled.
+
 	  This watchdog simply watches your kernel to make sure it doesn't
 	  freeze, and if it does, it reboots your computer after a certain
 	  amount of time.
@@ -780,7 +788,7 @@ config SMSC37B787_WDT
 
 config VIA_WDT
 	tristate "VIA Watchdog Timer"
-	depends on X86
+	depends on X86 && PCI
 	select WATCHDOG_CORE
 	---help---
 	This is the driver for the hardware watchdog timer on VIA
@@ -937,7 +945,7 @@ config BCM47XX_WDT
 	tristate "Broadcom BCM47xx Watchdog Timer"
 	depends on BCM47XX
 	help
-	  Hardware driver for the Broadcom BCM47xx Watchog Timer.
+	  Hardware driver for the Broadcom BCM47xx Watchdog Timer.
 
 config RC32434_WDT
 	tristate "IDT RC32434 SoC Watchdog Timer"
@@ -1138,6 +1146,7 @@ config ZVM_WATCHDOG
 config SH_WDT
 	tristate "SuperH Watchdog"
 	depends on SUPERH && (CPU_SH3 || CPU_SH4)
+	select WATCHDOG_CORE
 	help
 	  This driver adds watchdog support for the integrated watchdog in the
 	  SuperH processors. If you have one of these processors and wish
diff --git a/drivers/watchdog/Makefile b/drivers/watchdog/Makefile
index e8f479a1640..442bfbe0882 100644
--- a/drivers/watchdog/Makefile
+++ b/drivers/watchdog/Makefile
@@ -36,7 +36,6 @@ obj-$(CONFIG_OMAP_WATCHDOG) += omap_wdt.o
 obj-$(CONFIG_TWL4030_WATCHDOG) += twl4030_wdt.o
 obj-$(CONFIG_21285_WATCHDOG) += wdt285.o
 obj-$(CONFIG_977_WATCHDOG) += wdt977.o
-obj-$(CONFIG_IXP2000_WATCHDOG) += ixp2000_wdt.o
 obj-$(CONFIG_IXP4XX_WATCHDOG) += ixp4xx_wdt.o
 obj-$(CONFIG_KS8695_WATCHDOG) += ks8695_wdt.o
 obj-$(CONFIG_S3C2410_WATCHDOG) += s3c2410_wdt.o
@@ -81,6 +80,7 @@ obj-$(CONFIG_IB700_WDT) += ib700wdt.o
 obj-$(CONFIG_IBMASR) += ibmasr.o
 obj-$(CONFIG_WAFER_WDT) += wafer5823wdt.o
 obj-$(CONFIG_I6300ESB_WDT) += i6300esb.o
+obj-$(CONFIG_IE6XX_WDT) += ie6xx_wdt.o
 obj-$(CONFIG_ITCO_WDT) += iTCO_wdt.o
 ifeq ($(CONFIG_ITCO_VENDOR_SUPPORT),y)
 obj-$(CONFIG_ITCO_WDT) += iTCO_vendor_support.o
diff --git a/drivers/watchdog/ar7_wdt.c b/drivers/watchdog/ar7_wdt.c
index 639ae9a23fb..dc30dbd21cf 100644
--- a/drivers/watchdog/ar7_wdt.c
+++ b/drivers/watchdog/ar7_wdt.c
@@ -282,29 +282,19 @@ static int __devinit ar7_wdt_probe(struct platform_device *pdev)
 		platform_get_resource_byname(pdev, IORESOURCE_MEM, "regs");
 	if (!ar7_regs_wdt) {
 		pr_err("could not get registers resource\n");
-		rc = -ENODEV;
-		goto out;
-	}
-
-	if (!request_mem_region(ar7_regs_wdt->start,
-				resource_size(ar7_regs_wdt), LONGNAME)) {
-		pr_warn("watchdog I/O region busy\n");
-		rc = -EBUSY;
-		goto out;
+		return -ENODEV;
 	}
 
-	ar7_wdt = ioremap(ar7_regs_wdt->start, resource_size(ar7_regs_wdt));
+	ar7_wdt = devm_request_and_ioremap(&pdev->dev, ar7_regs_wdt);
 	if (!ar7_wdt) {
 		pr_err("could not ioremap registers\n");
-		rc = -ENXIO;
-		goto out_mem_region;
+		return -ENXIO;
 	}
 
 	vbus_clk = clk_get(NULL, "vbus");
 	if (IS_ERR(vbus_clk)) {
 		pr_err("could not get vbus clock\n");
-		rc = PTR_ERR(vbus_clk);
-		goto out_mem_region;
+		return PTR_ERR(vbus_clk);
 	}
 
 	ar7_wdt_disable_wdt();
@@ -314,24 +304,21 @@ static int __devinit ar7_wdt_probe(struct platform_device *pdev)
 	rc = misc_register(&ar7_wdt_miscdev);
 	if (rc) {
 		pr_err("unable to register misc device\n");
-		goto out_alloc;
+		goto out;
 	}
-	goto out;
+	return 0;
 
-out_alloc:
-	iounmap(ar7_wdt);
-out_mem_region:
-	release_mem_region(ar7_regs_wdt->start, resource_size(ar7_regs_wdt));
 out:
+	clk_put(vbus_clk);
+	vbus_clk = NULL;
 	return rc;
 }
 
 static int __devexit ar7_wdt_remove(struct platform_device *pdev)
 {
 	misc_deregister(&ar7_wdt_miscdev);
-	iounmap(ar7_wdt);
-	release_mem_region(ar7_regs_wdt->start, resource_size(ar7_regs_wdt));
-
+	clk_put(vbus_clk);
+	vbus_clk = NULL;
 	return 0;
 }
 
diff --git a/drivers/watchdog/hpwdt.c b/drivers/watchdog/hpwdt.c
index 9f13b897fd6..2b763815aee 100644
--- a/drivers/watchdog/hpwdt.c
+++ b/drivers/watchdog/hpwdt.c
@@ -147,7 +147,6 @@ struct cmn_registers {
 
 static unsigned int hpwdt_nmi_decoding;
 static unsigned int allow_kdump;
-static unsigned int priority;		/* hpwdt at end of die_notify list */
 static unsigned int is_icru;
 static DEFINE_SPINLOCK(rom_lock);
 static void *cru_rom_addr;
@@ -723,28 +722,35 @@ static int __devinit hpwdt_init_nmi_decoding(struct pci_dev *dev)
 	}
 
 	/*
-	 * If the priority is set to 1, then we will be put first on the
-	 * die notify list to handle a critical NMI. The default is to
-	 * be last so other users of the NMI signal can function.
+	 * Only one function can register for NMI_UNKNOWN
 	 */
-	retval = register_nmi_handler(NMI_UNKNOWN, hpwdt_pretimeout,
-					(priority) ? NMI_FLAG_FIRST : 0,
-					"hpwdt");
-	if (retval != 0) {
-		dev_warn(&dev->dev,
-			"Unable to register a die notifier (err=%d).\n",
-			retval);
-		if (cru_rom_addr)
-			iounmap(cru_rom_addr);
-	}
+	retval = register_nmi_handler(NMI_UNKNOWN, hpwdt_pretimeout, 0, "hpwdt");
+	if (retval)
+		goto error;
+	retval = register_nmi_handler(NMI_SERR, hpwdt_pretimeout, 0, "hpwdt");
+	if (retval)
+		goto error1;
+	retval = register_nmi_handler(NMI_IO_CHECK, hpwdt_pretimeout, 0, "hpwdt");
+	if (retval)
+		goto error2;
 
 	dev_info(&dev->dev,
 			"HP Watchdog Timer Driver: NMI decoding initialized"
-			", allow kernel dump: %s (default = 0/OFF)"
-			", priority: %s (default = 0/LAST).\n",
-			(allow_kdump == 0) ? "OFF" : "ON",
-			(priority == 0) ? "LAST" : "FIRST");
+			", allow kernel dump: %s (default = 0/OFF)\n",
+			(allow_kdump == 0) ? "OFF" : "ON");
 	return 0;
+
+error2:
+	unregister_nmi_handler(NMI_SERR, "hpwdt");
+error1:
+	unregister_nmi_handler(NMI_UNKNOWN, "hpwdt");
+error:
+	dev_warn(&dev->dev,
+		"Unable to register a die notifier (err=%d).\n",
+		retval);
+	if (cru_rom_addr)
+		iounmap(cru_rom_addr);
+	return retval;
 }
 
 static void hpwdt_exit_nmi_decoding(void)
@@ -855,16 +861,6 @@ static struct pci_driver hpwdt_driver = {
 	.remove = __devexit_p(hpwdt_exit),
 };
 
-static void __exit hpwdt_cleanup(void)
-{
-	pci_unregister_driver(&hpwdt_driver);
-}
-
-static int __init hpwdt_init(void)
-{
-	return pci_register_driver(&hpwdt_driver);
-}
-
 MODULE_AUTHOR("Tom Mingarelli");
 MODULE_DESCRIPTION("hp watchdog driver");
 MODULE_LICENSE("GPL");
@@ -881,11 +877,6 @@ MODULE_PARM_DESC(nowayout, "Watchdog cannot be stopped once started (default="
 #ifdef CONFIG_HPWDT_NMI_DECODING
 module_param(allow_kdump, int, 0);
 MODULE_PARM_DESC(allow_kdump, "Start a kernel dump after NMI occurs");
-
-module_param(priority, int, 0);
-MODULE_PARM_DESC(priority, "The hpwdt driver handles NMIs first or last"
-		" (default = 0/Last)\n");
 #endif /* !CONFIG_HPWDT_NMI_DECODING */
 
-module_init(hpwdt_init);
-module_exit(hpwdt_cleanup);
+module_pci_driver(hpwdt_driver);
diff --git a/drivers/watchdog/i6300esb.c b/drivers/watchdog/i6300esb.c
index 738032a36bc..276877d5b6a 100644
--- a/drivers/watchdog/i6300esb.c
+++ b/drivers/watchdog/i6300esb.c
@@ -492,19 +492,7 @@ static struct pci_driver esb_driver = {
 	.shutdown       = esb_shutdown,
 };
 
-static int __init watchdog_init(void)
-{
-	return pci_register_driver(&esb_driver);
-}
-
-static void __exit watchdog_cleanup(void)
-{
-	pci_unregister_driver(&esb_driver);
-	pr_info("Watchdog Module Unloaded\n");
-}
-
-module_init(watchdog_init);
-module_exit(watchdog_cleanup);
+module_pci_driver(esb_driver);
 
 MODULE_AUTHOR("Ross Biro and David Härdeman");
 MODULE_DESCRIPTION("Watchdog driver for Intel 6300ESB chipsets");
diff --git a/drivers/watchdog/iTCO_vendor.h b/drivers/watchdog/iTCO_vendor.h
index 9e27e6422f6..3c57b45537a 100644
--- a/drivers/watchdog/iTCO_vendor.h
+++ b/drivers/watchdog/iTCO_vendor.h
@@ -1,8 +1,8 @@
 /* iTCO Vendor Specific Support hooks */
 #ifdef CONFIG_ITCO_VENDOR_SUPPORT
-extern void iTCO_vendor_pre_start(unsigned long, unsigned int);
-extern void iTCO_vendor_pre_stop(unsigned long);
-extern void iTCO_vendor_pre_keepalive(unsigned long, unsigned int);
+extern void iTCO_vendor_pre_start(struct resource *, unsigned int);
+extern void iTCO_vendor_pre_stop(struct resource *);
+extern void iTCO_vendor_pre_keepalive(struct resource *, unsigned int);
 extern void iTCO_vendor_pre_set_heartbeat(unsigned int);
 extern int iTCO_vendor_check_noreboot_on(void);
 #else
diff --git a/drivers/watchdog/iTCO_vendor_support.c b/drivers/watchdog/iTCO_vendor_support.c
index 2721d29ce24..b6b2f90b5d4 100644
--- a/drivers/watchdog/iTCO_vendor_support.c
+++ b/drivers/watchdog/iTCO_vendor_support.c
@@ -35,11 +35,6 @@
 
 #include "iTCO_vendor.h"
 
-/* iTCO defines */
-#define	SMI_EN		(acpibase + 0x30) /* SMI Control and Enable Register */
-#define	TCOBASE		(acpibase + 0x60) /* TCO base address */
-#define	TCO1_STS	(TCOBASE + 0x04)  /* TCO1 Status Register */
-
 /* List of vendor support modes */
 /* SuperMicro Pentium 3 Era 370SSE+-OEM1/P3TSSE */
 #define SUPERMICRO_OLD_BOARD	1
@@ -82,24 +77,24 @@ MODULE_PARM_DESC(vendorsupport, "iTCO vendor specific support mode, default="
  *	    20.6 seconds.
  */
 
-static void supermicro_old_pre_start(unsigned long acpibase)
+static void supermicro_old_pre_start(struct resource *smires)
 {
 	unsigned long val32;
 
 	/* Bit 13: TCO_EN -> 0 = Disables TCO logic generating an SMI# */
-	val32 = inl(SMI_EN);
+	val32 = inl(smires->start);
 	val32 &= 0xffffdfff;	/* Turn off SMI clearing watchdog */
-	outl(val32, SMI_EN);	/* Needed to activate watchdog */
+	outl(val32, smires->start);	/* Needed to activate watchdog */
 }
 
-static void supermicro_old_pre_stop(unsigned long acpibase)
+static void supermicro_old_pre_stop(struct resource *smires)
 {
 	unsigned long val32;
 
 	/* Bit 13: TCO_EN -> 1 = Enables the TCO logic to generate SMI# */
-	val32 = inl(SMI_EN);
+	val32 = inl(smires->start);
 	val32 |= 0x00002000;	/* Turn on SMI clearing watchdog */
-	outl(val32, SMI_EN);	/* Needed to deactivate watchdog */
+	outl(val32, smires->start);	/* Needed to deactivate watchdog */
 }
 
 /*
@@ -270,66 +265,66 @@ static void supermicro_new_pre_set_heartbeat(unsigned int heartbeat)
  *	Don't use this fix if you don't need to!!!
  */
 
-static void broken_bios_start(unsigned long acpibase)
+static void broken_bios_start(struct resource *smires)
 {
 	unsigned long val32;
 
-	val32 = inl(SMI_EN);
+	val32 = inl(smires->start);
 	/* Bit 13: TCO_EN     -> 0 = Disables TCO logic generating an SMI#
 	   Bit  0: GBL_SMI_EN -> 0 = No SMI# will be generated by ICH. */
 	val32 &= 0xffffdffe;
-	outl(val32, SMI_EN);
+	outl(val32, smires->start);
 }
 
-static void broken_bios_stop(unsigned long acpibase)
+static void broken_bios_stop(struct resource *smires)
 {
 	unsigned long val32;
 
-	val32 = inl(SMI_EN);
+	val32 = inl(smires->start);
 	/* Bit 13: TCO_EN     -> 1 = Enables TCO logic generating an SMI#
 	   Bit  0: GBL_SMI_EN -> 1 = Turn global SMI on again. */
 	val32 |= 0x00002001;
-	outl(val32, SMI_EN);
+	outl(val32, smires->start);
 }
 
 /*
  *	Generic Support Functions
  */
 
-void iTCO_vendor_pre_start(unsigned long acpibase,
+void iTCO_vendor_pre_start(struct resource *smires,
 			   unsigned int heartbeat)
 {
 	switch (vendorsupport) {
 	case SUPERMICRO_OLD_BOARD:
-		supermicro_old_pre_start(acpibase);
+		supermicro_old_pre_start(smires);
 		break;
 	case SUPERMICRO_NEW_BOARD:
 		supermicro_new_pre_start(heartbeat);
 		break;
 	case BROKEN_BIOS:
-		broken_bios_start(acpibase);
+		broken_bios_start(smires);
 		break;
 	}
 }
 EXPORT_SYMBOL(iTCO_vendor_pre_start);
 
-void iTCO_vendor_pre_stop(unsigned long acpibase)
+void iTCO_vendor_pre_stop(struct resource *smires)
 {
 	switch (vendorsupport) {
 	case SUPERMICRO_OLD_BOARD:
-		supermicro_old_pre_stop(acpibase);
+		supermicro_old_pre_stop(smires);
 		break;
 	case SUPERMICRO_NEW_BOARD:
 		supermicro_new_pre_stop();
 		break;
 	case BROKEN_BIOS:
-		broken_bios_stop(acpibase);
+		broken_bios_stop(smires);
 		break;
 	}
 }
 EXPORT_SYMBOL(iTCO_vendor_pre_stop);
 
-void iTCO_vendor_pre_keepalive(unsigned long acpibase, unsigned int heartbeat)
+void iTCO_vendor_pre_keepalive(struct resource *smires, unsigned int heartbeat)
 {
 	if (vendorsupport == SUPERMICRO_NEW_BOARD)
 		supermicro_new_pre_set_heartbeat(heartbeat);
diff --git a/drivers/watchdog/iTCO_wdt.c b/drivers/watchdog/iTCO_wdt.c
index 9fecb95645a..741528b032e 100644
--- a/drivers/watchdog/iTCO_wdt.c
+++ b/drivers/watchdog/iTCO_wdt.c
@@ -66,316 +66,16 @@
 #include <linux/spinlock.h>		/* For spin_lock/spin_unlock/... */
 #include <linux/uaccess.h>		/* For copy_to_user/put_user/... */
 #include <linux/io.h>			/* For inb/outb/... */
+#include <linux/mfd/core.h>
+#include <linux/mfd/lpc_ich.h>
 
 #include "iTCO_vendor.h"
 
-/* TCO related info */
-enum iTCO_chipsets {
-	TCO_ICH = 0,	/* ICH */
-	TCO_ICH0,	/* ICH0 */
-	TCO_ICH2,	/* ICH2 */
-	TCO_ICH2M,	/* ICH2-M */
-	TCO_ICH3,	/* ICH3-S */
-	TCO_ICH3M,	/* ICH3-M */
-	TCO_ICH4,	/* ICH4 */
-	TCO_ICH4M,	/* ICH4-M */
-	TCO_CICH,	/* C-ICH */
-	TCO_ICH5,	/* ICH5 & ICH5R */
-	TCO_6300ESB,	/* 6300ESB */
-	TCO_ICH6,	/* ICH6 & ICH6R */
-	TCO_ICH6M,	/* ICH6-M */
-	TCO_ICH6W,	/* ICH6W & ICH6RW */
-	TCO_631XESB,	/* 631xESB/632xESB */
-	TCO_ICH7,	/* ICH7 & ICH7R */
-	TCO_ICH7DH,	/* ICH7DH */
-	TCO_ICH7M,	/* ICH7-M & ICH7-U */
-	TCO_ICH7MDH,	/* ICH7-M DH */
-	TCO_NM10,	/* NM10 */
-	TCO_ICH8,	/* ICH8 & ICH8R */
-	TCO_ICH8DH,	/* ICH8DH */
-	TCO_ICH8DO,	/* ICH8DO */
-	TCO_ICH8M,	/* ICH8M */
-	TCO_ICH8ME,	/* ICH8M-E */
-	TCO_ICH9,	/* ICH9 */
-	TCO_ICH9R,	/* ICH9R */
-	TCO_ICH9DH,	/* ICH9DH */
-	TCO_ICH9DO,	/* ICH9DO */
-	TCO_ICH9M,	/* ICH9M */
-	TCO_ICH9ME,	/* ICH9M-E */
-	TCO_ICH10,	/* ICH10 */
-	TCO_ICH10R,	/* ICH10R */
-	TCO_ICH10D,	/* ICH10D */
-	TCO_ICH10DO,	/* ICH10DO */
-	TCO_PCH,	/* PCH Desktop Full Featured */
-	TCO_PCHM,	/* PCH Mobile Full Featured */
-	TCO_P55,	/* P55 */
-	TCO_PM55,	/* PM55 */
-	TCO_H55,	/* H55 */
-	TCO_QM57,	/* QM57 */
-	TCO_H57,	/* H57 */
-	TCO_HM55,	/* HM55 */
-	TCO_Q57,	/* Q57 */
-	TCO_HM57,	/* HM57 */
-	TCO_PCHMSFF,	/* PCH Mobile SFF Full Featured */
-	TCO_QS57,	/* QS57 */
-	TCO_3400,	/* 3400 */
-	TCO_3420,	/* 3420 */
-	TCO_3450,	/* 3450 */
-	TCO_EP80579,	/* EP80579 */
-	TCO_CPT,	/* Cougar Point */
-	TCO_CPTD,	/* Cougar Point Desktop */
-	TCO_CPTM,	/* Cougar Point Mobile */
-	TCO_PBG,	/* Patsburg */
-	TCO_DH89XXCC,	/* DH89xxCC */
-	TCO_PPT,	/* Panther Point */
-	TCO_LPT,	/* Lynx Point */
-};
-
-static struct {
-	char *name;
-	unsigned int iTCO_version;
-} iTCO_chipset_info[] __devinitdata = {
-	{"ICH", 1},
-	{"ICH0", 1},
-	{"ICH2", 1},
-	{"ICH2-M", 1},
-	{"ICH3-S", 1},
-	{"ICH3-M", 1},
-	{"ICH4", 1},
-	{"ICH4-M", 1},
-	{"C-ICH", 1},
-	{"ICH5 or ICH5R", 1},
-	{"6300ESB", 1},
-	{"ICH6 or ICH6R", 2},
-	{"ICH6-M", 2},
-	{"ICH6W or ICH6RW", 2},
-	{"631xESB/632xESB", 2},
-	{"ICH7 or ICH7R", 2},
-	{"ICH7DH", 2},
-	{"ICH7-M or ICH7-U", 2},
-	{"ICH7-M DH", 2},
-	{"NM10", 2},
-	{"ICH8 or ICH8R", 2},
-	{"ICH8DH", 2},
-	{"ICH8DO", 2},
-	{"ICH8M", 2},
-	{"ICH8M-E", 2},
-	{"ICH9", 2},
-	{"ICH9R", 2},
-	{"ICH9DH", 2},
-	{"ICH9DO", 2},
-	{"ICH9M", 2},
-	{"ICH9M-E", 2},
-	{"ICH10", 2},
-	{"ICH10R", 2},
-	{"ICH10D", 2},
-	{"ICH10DO", 2},
-	{"PCH Desktop Full Featured", 2},
-	{"PCH Mobile Full Featured", 2},
-	{"P55", 2},
-	{"PM55", 2},
-	{"H55", 2},
-	{"QM57", 2},
-	{"H57", 2},
-	{"HM55", 2},
-	{"Q57", 2},
-	{"HM57", 2},
-	{"PCH Mobile SFF Full Featured", 2},
-	{"QS57", 2},
-	{"3400", 2},
-	{"3420", 2},
-	{"3450", 2},
-	{"EP80579", 2},
-	{"Cougar Point", 2},
-	{"Cougar Point Desktop", 2},
-	{"Cougar Point Mobile", 2},
-	{"Patsburg", 2},
-	{"DH89xxCC", 2},
-	{"Panther Point", 2},
-	{"Lynx Point", 2},
-	{NULL, 0}
-};
-
-/*
- * This data only exists for exporting the supported PCI ids
- * via MODULE_DEVICE_TABLE.  We do not actually register a
- * pci_driver, because the I/O Controller Hub has also other
- * functions that probably will be registered by other drivers.
- */
-static DEFINE_PCI_DEVICE_TABLE(iTCO_wdt_pci_tbl) = {
-	{ PCI_VDEVICE(INTEL, 0x2410), TCO_ICH},
-	{ PCI_VDEVICE(INTEL, 0x2420), TCO_ICH0},
-	{ PCI_VDEVICE(INTEL, 0x2440), TCO_ICH2},
-	{ PCI_VDEVICE(INTEL, 0x244c), TCO_ICH2M},
-	{ PCI_VDEVICE(INTEL, 0x2480), TCO_ICH3},
-	{ PCI_VDEVICE(INTEL, 0x248c), TCO_ICH3M},
-	{ PCI_VDEVICE(INTEL, 0x24c0), TCO_ICH4},
-	{ PCI_VDEVICE(INTEL, 0x24cc), TCO_ICH4M},
-	{ PCI_VDEVICE(INTEL, 0x2450), TCO_CICH},
-	{ PCI_VDEVICE(INTEL, 0x24d0), TCO_ICH5},
-	{ PCI_VDEVICE(INTEL, 0x25a1), TCO_6300ESB},
-	{ PCI_VDEVICE(INTEL, 0x2640), TCO_ICH6},
-	{ PCI_VDEVICE(INTEL, 0x2641), TCO_ICH6M},
-	{ PCI_VDEVICE(INTEL, 0x2642), TCO_ICH6W},
-	{ PCI_VDEVICE(INTEL, 0x2670), TCO_631XESB},
-	{ PCI_VDEVICE(INTEL, 0x2671), TCO_631XESB},
-	{ PCI_VDEVICE(INTEL, 0x2672), TCO_631XESB},
-	{ PCI_VDEVICE(INTEL, 0x2673), TCO_631XESB},
-	{ PCI_VDEVICE(INTEL, 0x2674), TCO_631XESB},
-	{ PCI_VDEVICE(INTEL, 0x2675), TCO_631XESB},
-	{ PCI_VDEVICE(INTEL, 0x2676), TCO_631XESB},
-	{ PCI_VDEVICE(INTEL, 0x2677), TCO_631XESB},
-	{ PCI_VDEVICE(INTEL, 0x2678), TCO_631XESB},
-	{ PCI_VDEVICE(INTEL, 0x2679), TCO_631XESB},
-	{ PCI_VDEVICE(INTEL, 0x267a), TCO_631XESB},
-	{ PCI_VDEVICE(INTEL, 0x267b), TCO_631XESB},
-	{ PCI_VDEVICE(INTEL, 0x267c), TCO_631XESB},
-	{ PCI_VDEVICE(INTEL, 0x267d), TCO_631XESB},
-	{ PCI_VDEVICE(INTEL, 0x267e), TCO_631XESB},
-	{ PCI_VDEVICE(INTEL, 0x267f), TCO_631XESB},
-	{ PCI_VDEVICE(INTEL, 0x27b8), TCO_ICH7},
-	{ PCI_VDEVICE(INTEL, 0x27b0), TCO_ICH7DH},
-	{ PCI_VDEVICE(INTEL, 0x27b9), TCO_ICH7M},
-	{ PCI_VDEVICE(INTEL, 0x27bd), TCO_ICH7MDH},
-	{ PCI_VDEVICE(INTEL, 0x27bc), TCO_NM10},
-	{ PCI_VDEVICE(INTEL, 0x2810), TCO_ICH8},
-	{ PCI_VDEVICE(INTEL, 0x2812), TCO_ICH8DH},
-	{ PCI_VDEVICE(INTEL, 0x2814), TCO_ICH8DO},
-	{ PCI_VDEVICE(INTEL, 0x2815), TCO_ICH8M},
-	{ PCI_VDEVICE(INTEL, 0x2811), TCO_ICH8ME},
-	{ PCI_VDEVICE(INTEL, 0x2918), TCO_ICH9},
-	{ PCI_VDEVICE(INTEL, 0x2916), TCO_ICH9R},
-	{ PCI_VDEVICE(INTEL, 0x2912), TCO_ICH9DH},
-	{ PCI_VDEVICE(INTEL, 0x2914), TCO_ICH9DO},
-	{ PCI_VDEVICE(INTEL, 0x2919), TCO_ICH9M},
-	{ PCI_VDEVICE(INTEL, 0x2917), TCO_ICH9ME},
-	{ PCI_VDEVICE(INTEL, 0x3a18), TCO_ICH10},
-	{ PCI_VDEVICE(INTEL, 0x3a16), TCO_ICH10R},
-	{ PCI_VDEVICE(INTEL, 0x3a1a), TCO_ICH10D},
-	{ PCI_VDEVICE(INTEL, 0x3a14), TCO_ICH10DO},
-	{ PCI_VDEVICE(INTEL, 0x3b00), TCO_PCH},
-	{ PCI_VDEVICE(INTEL, 0x3b01), TCO_PCHM},
-	{ PCI_VDEVICE(INTEL, 0x3b02), TCO_P55},
-	{ PCI_VDEVICE(INTEL, 0x3b03), TCO_PM55},
-	{ PCI_VDEVICE(INTEL, 0x3b06), TCO_H55},
-	{ PCI_VDEVICE(INTEL, 0x3b07), TCO_QM57},
-	{ PCI_VDEVICE(INTEL, 0x3b08), TCO_H57},
-	{ PCI_VDEVICE(INTEL, 0x3b09), TCO_HM55},
-	{ PCI_VDEVICE(INTEL, 0x3b0a), TCO_Q57},
-	{ PCI_VDEVICE(INTEL, 0x3b0b), TCO_HM57},
-	{ PCI_VDEVICE(INTEL, 0x3b0d), TCO_PCHMSFF},
-	{ PCI_VDEVICE(INTEL, 0x3b0f), TCO_QS57},
-	{ PCI_VDEVICE(INTEL, 0x3b12), TCO_3400},
-	{ PCI_VDEVICE(INTEL, 0x3b14), TCO_3420},
-	{ PCI_VDEVICE(INTEL, 0x3b16), TCO_3450},
-	{ PCI_VDEVICE(INTEL, 0x5031), TCO_EP80579},
-	{ PCI_VDEVICE(INTEL, 0x1c41), TCO_CPT},
-	{ PCI_VDEVICE(INTEL, 0x1c42), TCO_CPTD},
-	{ PCI_VDEVICE(INTEL, 0x1c43), TCO_CPTM},
-	{ PCI_VDEVICE(INTEL, 0x1c44), TCO_CPT},
-	{ PCI_VDEVICE(INTEL, 0x1c45), TCO_CPT},
-	{ PCI_VDEVICE(INTEL, 0x1c46), TCO_CPT},
-	{ PCI_VDEVICE(INTEL, 0x1c47), TCO_CPT},
-	{ PCI_VDEVICE(INTEL, 0x1c48), TCO_CPT},
-	{ PCI_VDEVICE(INTEL, 0x1c49), TCO_CPT},
-	{ PCI_VDEVICE(INTEL, 0x1c4a), TCO_CPT},
-	{ PCI_VDEVICE(INTEL, 0x1c4b), TCO_CPT},
-	{ PCI_VDEVICE(INTEL, 0x1c4c), TCO_CPT},
-	{ PCI_VDEVICE(INTEL, 0x1c4d), TCO_CPT},
-	{ PCI_VDEVICE(INTEL, 0x1c4e), TCO_CPT},
-	{ PCI_VDEVICE(INTEL, 0x1c4f), TCO_CPT},
-	{ PCI_VDEVICE(INTEL, 0x1c50), TCO_CPT},
-	{ PCI_VDEVICE(INTEL, 0x1c51), TCO_CPT},
-	{ PCI_VDEVICE(INTEL, 0x1c52), TCO_CPT},
-	{ PCI_VDEVICE(INTEL, 0x1c53), TCO_CPT},
-	{ PCI_VDEVICE(INTEL, 0x1c54), TCO_CPT},
-	{ PCI_VDEVICE(INTEL, 0x1c55), TCO_CPT},
-	{ PCI_VDEVICE(INTEL, 0x1c56), TCO_CPT},
-	{ PCI_VDEVICE(INTEL, 0x1c57), TCO_CPT},
-	{ PCI_VDEVICE(INTEL, 0x1c58), TCO_CPT},
-	{ PCI_VDEVICE(INTEL, 0x1c59), TCO_CPT},
-	{ PCI_VDEVICE(INTEL, 0x1c5a), TCO_CPT},
-	{ PCI_VDEVICE(INTEL, 0x1c5b), TCO_CPT},
-	{ PCI_VDEVICE(INTEL, 0x1c5c), TCO_CPT},
-	{ PCI_VDEVICE(INTEL, 0x1c5d), TCO_CPT},
-	{ PCI_VDEVICE(INTEL, 0x1c5e), TCO_CPT},
-	{ PCI_VDEVICE(INTEL, 0x1c5f), TCO_CPT},
-	{ PCI_VDEVICE(INTEL, 0x1d40), TCO_PBG},
-	{ PCI_VDEVICE(INTEL, 0x1d41), TCO_PBG},
-	{ PCI_VDEVICE(INTEL, 0x2310), TCO_DH89XXCC},
-	{ PCI_VDEVICE(INTEL, 0x1e40), TCO_PPT},
-	{ PCI_VDEVICE(INTEL, 0x1e41), TCO_PPT},
-	{ PCI_VDEVICE(INTEL, 0x1e42), TCO_PPT},
-	{ PCI_VDEVICE(INTEL, 0x1e43), TCO_PPT},
-	{ PCI_VDEVICE(INTEL, 0x1e44), TCO_PPT},
-	{ PCI_VDEVICE(INTEL, 0x1e45), TCO_PPT},
-	{ PCI_VDEVICE(INTEL, 0x1e46), TCO_PPT},
-	{ PCI_VDEVICE(INTEL, 0x1e47), TCO_PPT},
-	{ PCI_VDEVICE(INTEL, 0x1e48), TCO_PPT},
-	{ PCI_VDEVICE(INTEL, 0x1e49), TCO_PPT},
-	{ PCI_VDEVICE(INTEL, 0x1e4a), TCO_PPT},
-	{ PCI_VDEVICE(INTEL, 0x1e4b), TCO_PPT},
-	{ PCI_VDEVICE(INTEL, 0x1e4c), TCO_PPT},
-	{ PCI_VDEVICE(INTEL, 0x1e4d), TCO_PPT},
-	{ PCI_VDEVICE(INTEL, 0x1e4e), TCO_PPT},
-	{ PCI_VDEVICE(INTEL, 0x1e4f), TCO_PPT},
-	{ PCI_VDEVICE(INTEL, 0x1e50), TCO_PPT},
-	{ PCI_VDEVICE(INTEL, 0x1e51), TCO_PPT},
-	{ PCI_VDEVICE(INTEL, 0x1e52), TCO_PPT},
-	{ PCI_VDEVICE(INTEL, 0x1e53), TCO_PPT},
-	{ PCI_VDEVICE(INTEL, 0x1e54), TCO_PPT},
-	{ PCI_VDEVICE(INTEL, 0x1e55), TCO_PPT},
-	{ PCI_VDEVICE(INTEL, 0x1e56), TCO_PPT},
-	{ PCI_VDEVICE(INTEL, 0x1e57), TCO_PPT},
-	{ PCI_VDEVICE(INTEL, 0x1e58), TCO_PPT},
-	{ PCI_VDEVICE(INTEL, 0x1e59), TCO_PPT},
-	{ PCI_VDEVICE(INTEL, 0x1e5a), TCO_PPT},
-	{ PCI_VDEVICE(INTEL, 0x1e5b), TCO_PPT},
-	{ PCI_VDEVICE(INTEL, 0x1e5c), TCO_PPT},
-	{ PCI_VDEVICE(INTEL, 0x1e5d), TCO_PPT},
-	{ PCI_VDEVICE(INTEL, 0x1e5e), TCO_PPT},
-	{ PCI_VDEVICE(INTEL, 0x1e5f), TCO_PPT},
-	{ PCI_VDEVICE(INTEL, 0x8c40), TCO_LPT},
-	{ PCI_VDEVICE(INTEL, 0x8c41), TCO_LPT},
-	{ PCI_VDEVICE(INTEL, 0x8c42), TCO_LPT},
-	{ PCI_VDEVICE(INTEL, 0x8c43), TCO_LPT},
-	{ PCI_VDEVICE(INTEL, 0x8c44), TCO_LPT},
-	{ PCI_VDEVICE(INTEL, 0x8c45), TCO_LPT},
-	{ PCI_VDEVICE(INTEL, 0x8c46), TCO_LPT},
-	{ PCI_VDEVICE(INTEL, 0x8c47), TCO_LPT},
-	{ PCI_VDEVICE(INTEL, 0x8c48), TCO_LPT},
-	{ PCI_VDEVICE(INTEL, 0x8c49), TCO_LPT},
-	{ PCI_VDEVICE(INTEL, 0x8c4a), TCO_LPT},
-	{ PCI_VDEVICE(INTEL, 0x8c4b), TCO_LPT},
-	{ PCI_VDEVICE(INTEL, 0x8c4c), TCO_LPT},
-	{ PCI_VDEVICE(INTEL, 0x8c4d), TCO_LPT},
-	{ PCI_VDEVICE(INTEL, 0x8c4e), TCO_LPT},
-	{ PCI_VDEVICE(INTEL, 0x8c4f), TCO_LPT},
-	{ PCI_VDEVICE(INTEL, 0x8c50), TCO_LPT},
-	{ PCI_VDEVICE(INTEL, 0x8c51), TCO_LPT},
-	{ PCI_VDEVICE(INTEL, 0x8c52), TCO_LPT},
-	{ PCI_VDEVICE(INTEL, 0x8c53), TCO_LPT},
-	{ PCI_VDEVICE(INTEL, 0x8c54), TCO_LPT},
-	{ PCI_VDEVICE(INTEL, 0x8c55), TCO_LPT},
-	{ PCI_VDEVICE(INTEL, 0x8c56), TCO_LPT},
-	{ PCI_VDEVICE(INTEL, 0x8c57), TCO_LPT},
-	{ PCI_VDEVICE(INTEL, 0x8c58), TCO_LPT},
-	{ PCI_VDEVICE(INTEL, 0x8c59), TCO_LPT},
-	{ PCI_VDEVICE(INTEL, 0x8c5a), TCO_LPT},
-	{ PCI_VDEVICE(INTEL, 0x8c5b), TCO_LPT},
-	{ PCI_VDEVICE(INTEL, 0x8c5c), TCO_LPT},
-	{ PCI_VDEVICE(INTEL, 0x8c5d), TCO_LPT},
-	{ PCI_VDEVICE(INTEL, 0x8c5e), TCO_LPT},
-	{ PCI_VDEVICE(INTEL, 0x8c5f), TCO_LPT},
-	{ 0, },			/* End of list */
-};
-MODULE_DEVICE_TABLE(pci, iTCO_wdt_pci_tbl);
-
 /* Address definitions for the TCO */
 /* TCO base address */
-#define TCOBASE		(iTCO_wdt_private.ACPIBASE + 0x60)
+#define TCOBASE		(iTCO_wdt_private.tco_res->start)
 /* SMI Control and Enable Register */
-#define SMI_EN		(iTCO_wdt_private.ACPIBASE + 0x30)
+#define SMI_EN		(iTCO_wdt_private.smi_res->start)
 
 #define TCO_RLD		(TCOBASE + 0x00) /* TCO Timer Reload and Curr. Value */
 #define TCOv1_TMR	(TCOBASE + 0x01) /* TCOv1 Timer Initial Value	*/
@@ -393,19 +93,18 @@ static char expect_release;
 static struct {		/* this is private data for the iTCO_wdt device */
 	/* TCO version/generation */
 	unsigned int iTCO_version;
-	/* The device's ACPIBASE address (TCOBASE = ACPIBASE+0x60) */
-	unsigned long ACPIBASE;
+	struct resource *tco_res;
+	struct resource *smi_res;
+	struct resource *gcs_res;
 	/* NO_REBOOT flag is Memory-Mapped GCS register bit 5 (TCO version 2)*/
 	unsigned long __iomem *gcs;
 	/* the lock for io operations */
 	spinlock_t io_lock;
+	struct platform_device *dev;
 	/* the PCI-device */
 	struct pci_dev *pdev;
 } iTCO_wdt_private;
 
-/* the watchdog platform device */
-static struct platform_device *iTCO_wdt_platform_device;
-
 /* module parameters */
 #define WATCHDOG_HEARTBEAT 30	/* 30 sec default heartbeat */
 static int heartbeat = WATCHDOG_HEARTBEAT;  /* in seconds */
@@ -485,7 +184,7 @@ static int iTCO_wdt_start(void)
 
 	spin_lock(&iTCO_wdt_private.io_lock);
 
-	iTCO_vendor_pre_start(iTCO_wdt_private.ACPIBASE, heartbeat);
+	iTCO_vendor_pre_start(iTCO_wdt_private.smi_res, heartbeat);
 
 	/* disable chipset's NO_REBOOT bit */
 	if (iTCO_wdt_unset_NO_REBOOT_bit()) {
@@ -519,7 +218,7 @@ static int iTCO_wdt_stop(void)
 
 	spin_lock(&iTCO_wdt_private.io_lock);
 
-	iTCO_vendor_pre_stop(iTCO_wdt_private.ACPIBASE);
+	iTCO_vendor_pre_stop(iTCO_wdt_private.smi_res);
 
 	/* Bit 11: TCO Timer Halt -> 1 = The TCO timer is disabled */
 	val = inw(TCO1_CNT);
@@ -541,7 +240,7 @@ static int iTCO_wdt_keepalive(void)
 {
 	spin_lock(&iTCO_wdt_private.io_lock);
 
-	iTCO_vendor_pre_keepalive(iTCO_wdt_private.ACPIBASE, heartbeat);
+	iTCO_vendor_pre_keepalive(iTCO_wdt_private.smi_res, heartbeat);
 
 	/* Reload the timer by writing to the TCO Timer Counter register */
 	if (iTCO_wdt_private.iTCO_version == 2)
@@ -786,83 +485,120 @@ static struct miscdevice iTCO_wdt_miscdev = {
  *	Init & exit routines
  */
 
-static int __devinit iTCO_wdt_init(struct pci_dev *pdev,
-		const struct pci_device_id *ent, struct platform_device *dev)
+static void __devexit iTCO_wdt_cleanup(void)
+{
+	/* Stop the timer before we leave */
+	if (!nowayout)
+		iTCO_wdt_stop();
+
+	/* Deregister */
+	misc_deregister(&iTCO_wdt_miscdev);
+
+	/* release resources */
+	release_region(iTCO_wdt_private.tco_res->start,
+			resource_size(iTCO_wdt_private.tco_res));
+	release_region(iTCO_wdt_private.smi_res->start,
+			resource_size(iTCO_wdt_private.smi_res));
+	if (iTCO_wdt_private.iTCO_version == 2) {
+		iounmap(iTCO_wdt_private.gcs);
+		release_mem_region(iTCO_wdt_private.gcs_res->start,
+				resource_size(iTCO_wdt_private.gcs_res));
+	}
+
+	iTCO_wdt_private.tco_res = NULL;
+	iTCO_wdt_private.smi_res = NULL;
+	iTCO_wdt_private.gcs_res = NULL;
+	iTCO_wdt_private.gcs = NULL;
+}
+
+static int __devinit iTCO_wdt_probe(struct platform_device *dev)
 {
-	int ret;
-	u32 base_address;
-	unsigned long RCBA;
+	int ret = -ENODEV;
 	unsigned long val32;
+	struct lpc_ich_info *ich_info = dev->dev.platform_data;
+
+	if (!ich_info)
+		goto out;
+
+	spin_lock_init(&iTCO_wdt_private.io_lock);
+
+	iTCO_wdt_private.tco_res =
+		platform_get_resource(dev, IORESOURCE_IO, ICH_RES_IO_TCO);
+	if (!iTCO_wdt_private.tco_res)
+		goto out;
+
+	iTCO_wdt_private.smi_res =
+		platform_get_resource(dev, IORESOURCE_IO, ICH_RES_IO_SMI);
+	if (!iTCO_wdt_private.smi_res)
+		goto out;
+
+	iTCO_wdt_private.iTCO_version = ich_info->iTCO_version;
+	iTCO_wdt_private.dev = dev;
+	iTCO_wdt_private.pdev = to_pci_dev(dev->dev.parent);
 
 	/*
-	 *      Find the ACPI/PM base I/O address which is the base
-	 *      for the TCO registers (TCOBASE=ACPIBASE + 0x60)
-	 *      ACPIBASE is bits [15:7] from 0x40-0x43
+	 * Get the Memory-Mapped GCS register, we need it for the
+	 * NO_REBOOT flag (TCO v2).
 	 */
-	pci_read_config_dword(pdev, 0x40, &base_address);
-	base_address &= 0x0000ff80;
-	if (base_address == 0x00000000) {
-		/* Something's wrong here, ACPIBASE has to be set */
-		pr_err("failed to get TCOBASE address, device disabled by hardware/BIOS\n");
-		return -ENODEV;
-	}
-	iTCO_wdt_private.iTCO_version =
-			iTCO_chipset_info[ent->driver_data].iTCO_version;
-	iTCO_wdt_private.ACPIBASE = base_address;
-	iTCO_wdt_private.pdev = pdev;
-
-	/* Get the Memory-Mapped GCS register, we need it for the
-	   NO_REBOOT flag (TCO v2). To get access to it you have to
-	   read RCBA from PCI Config space 0xf0 and use it as base.
-	   GCS = RCBA + ICH6_GCS(0x3410). */
 	if (iTCO_wdt_private.iTCO_version == 2) {
-		pci_read_config_dword(pdev, 0xf0, &base_address);
-		if ((base_address & 1) == 0) {
-			pr_err("RCBA is disabled by hardware/BIOS, device disabled\n");
-			ret = -ENODEV;
+		iTCO_wdt_private.gcs_res = platform_get_resource(dev,
+							IORESOURCE_MEM,
+							ICH_RES_MEM_GCS);
+
+		if (!iTCO_wdt_private.gcs_res)
+			goto out;
+
+		if (!request_mem_region(iTCO_wdt_private.gcs_res->start,
+			resource_size(iTCO_wdt_private.gcs_res), dev->name)) {
+			ret = -EBUSY;
 			goto out;
 		}
-		RCBA = base_address & 0xffffc000;
-		iTCO_wdt_private.gcs = ioremap((RCBA + 0x3410), 4);
+		iTCO_wdt_private.gcs = ioremap(iTCO_wdt_private.gcs_res->start,
+			resource_size(iTCO_wdt_private.gcs_res));
+		if (!iTCO_wdt_private.gcs) {
+			ret = -EIO;
+			goto unreg_gcs;
+		}
 	}
 
 	/* Check chipset's NO_REBOOT bit */
 	if (iTCO_wdt_unset_NO_REBOOT_bit() && iTCO_vendor_check_noreboot_on()) {
 		pr_info("unable to reset NO_REBOOT flag, device disabled by hardware/BIOS\n");
 		ret = -ENODEV;	/* Cannot reset NO_REBOOT bit */
-		goto out_unmap;
+		goto unmap_gcs;
 	}
 
 	/* Set the NO_REBOOT bit to prevent later reboots, just for sure */
 	iTCO_wdt_set_NO_REBOOT_bit();
 
 	/* The TCO logic uses the TCO_EN bit in the SMI_EN register */
-	if (!request_region(SMI_EN, 4, "iTCO_wdt")) {
-		pr_err("I/O address 0x%04lx already in use, device disabled\n",
+	if (!request_region(iTCO_wdt_private.smi_res->start,
+			resource_size(iTCO_wdt_private.smi_res), dev->name)) {
+		pr_err("I/O address 0x%04llx already in use, device disabled\n",
 		       SMI_EN);
-		ret = -EIO;
-		goto out_unmap;
+		ret = -EBUSY;
+		goto unmap_gcs;
 	}
 	if (turn_SMI_watchdog_clear_off >= iTCO_wdt_private.iTCO_version) {
-		/* Bit 13: TCO_EN -> 0 = Disables TCO logic generating an SMI# */
+		/*
+		 * Bit 13: TCO_EN -> 0
+		 * Disables TCO logic generating an SMI#
+		 */
 		val32 = inl(SMI_EN);
 		val32 &= 0xffffdfff;	/* Turn off SMI clearing watchdog */
 		outl(val32, SMI_EN);
 	}
 
-	/* The TCO I/O registers reside in a 32-byte range pointed to
-	   by the TCOBASE value */
-	if (!request_region(TCOBASE, 0x20, "iTCO_wdt")) {
-		pr_err("I/O address 0x%04lx already in use, device disabled\n",
+	if (!request_region(iTCO_wdt_private.tco_res->start,
+			resource_size(iTCO_wdt_private.tco_res), dev->name)) {
+		pr_err("I/O address 0x%04llx already in use, device disabled\n",
 		       TCOBASE);
-		ret = -EIO;
-		goto unreg_smi_en;
+		ret = -EBUSY;
+		goto unreg_smi;
 	}
 
-	pr_info("Found a %s TCO device (Version=%d, TCOBASE=0x%04lx)\n",
-		iTCO_chipset_info[ent->driver_data].name,
-		iTCO_chipset_info[ent->driver_data].iTCO_version,
-		TCOBASE);
+	pr_info("Found a %s TCO device (Version=%d, TCOBASE=0x%04llx)\n",
+		ich_info->name, ich_info->iTCO_version, TCOBASE);
 
 	/* Clear out the (probably old) status */
 	outw(0x0008, TCO1_STS);	/* Clear the Time Out Status bit */
@@ -883,7 +619,7 @@ static int __devinit iTCO_wdt_init(struct pci_dev *pdev,
 	if (ret != 0) {
 		pr_err("cannot register miscdev on minor=%d (err=%d)\n",
 		       WATCHDOG_MINOR, ret);
-		goto unreg_region;
+		goto unreg_tco;
 	}
 
 	pr_info("initialized. heartbeat=%d sec (nowayout=%d)\n",
@@ -891,62 +627,31 @@ static int __devinit iTCO_wdt_init(struct pci_dev *pdev,
 
 	return 0;
 
-unreg_region:
-	release_region(TCOBASE, 0x20);
-unreg_smi_en:
-	release_region(SMI_EN, 4);
-out_unmap:
+unreg_tco:
+	release_region(iTCO_wdt_private.tco_res->start,
+			resource_size(iTCO_wdt_private.tco_res));
+unreg_smi:
+	release_region(iTCO_wdt_private.smi_res->start,
+			resource_size(iTCO_wdt_private.smi_res));
+unmap_gcs:
 	if (iTCO_wdt_private.iTCO_version == 2)
 		iounmap(iTCO_wdt_private.gcs);
-out:
-	iTCO_wdt_private.ACPIBASE = 0;
-	return ret;
-}
-
-static void __devexit iTCO_wdt_cleanup(void)
-{
-	/* Stop the timer before we leave */
-	if (!nowayout)
-		iTCO_wdt_stop();
-
-	/* Deregister */
-	misc_deregister(&iTCO_wdt_miscdev);
-	release_region(TCOBASE, 0x20);
-	release_region(SMI_EN, 4);
+unreg_gcs:
 	if (iTCO_wdt_private.iTCO_version == 2)
-		iounmap(iTCO_wdt_private.gcs);
-	pci_dev_put(iTCO_wdt_private.pdev);
-	iTCO_wdt_private.ACPIBASE = 0;
-}
-
-static int __devinit iTCO_wdt_probe(struct platform_device *dev)
-{
-	int ret = -ENODEV;
-	int found = 0;
-	struct pci_dev *pdev = NULL;
-	const struct pci_device_id *ent;
-
-	spin_lock_init(&iTCO_wdt_private.io_lock);
-
-	for_each_pci_dev(pdev) {
-		ent = pci_match_id(iTCO_wdt_pci_tbl, pdev);
-		if (ent) {
-			found++;
-			ret = iTCO_wdt_init(pdev, ent, dev);
-			if (!ret)
-				break;
-		}
-	}
-
-	if (!found)
-		pr_info("No device detected\n");
+		release_mem_region(iTCO_wdt_private.gcs_res->start,
+				resource_size(iTCO_wdt_private.gcs_res));
+out:
+	iTCO_wdt_private.tco_res = NULL;
+	iTCO_wdt_private.smi_res = NULL;
+	iTCO_wdt_private.gcs_res = NULL;
+	iTCO_wdt_private.gcs = NULL;
 
 	return ret;
 }
 
 static int __devexit iTCO_wdt_remove(struct platform_device *dev)
 {
-	if (iTCO_wdt_private.ACPIBASE)
+	if (iTCO_wdt_private.tco_res || iTCO_wdt_private.smi_res)
 		iTCO_wdt_cleanup();
 
 	return 0;
@@ -977,23 +682,11 @@ static int __init iTCO_wdt_init_module(void)
 	if (err)
 		return err;
 
-	iTCO_wdt_platform_device = platform_device_register_simple(DRV_NAME,
-								-1, NULL, 0);
-	if (IS_ERR(iTCO_wdt_platform_device)) {
-		err = PTR_ERR(iTCO_wdt_platform_device);
-		goto unreg_platform_driver;
-	}
-
 	return 0;
-
-unreg_platform_driver:
-	platform_driver_unregister(&iTCO_wdt_driver);
-	return err;
 }
 
 static void __exit iTCO_wdt_cleanup_module(void)
 {
-	platform_device_unregister(iTCO_wdt_platform_device);
 	platform_driver_unregister(&iTCO_wdt_driver);
 	pr_info("Watchdog Module Unloaded\n");
 }
diff --git a/drivers/watchdog/ie6xx_wdt.c b/drivers/watchdog/ie6xx_wdt.c
new file mode 100644
index 00000000000..5f0d776f902
--- /dev/null
+++ b/drivers/watchdog/ie6xx_wdt.c
@@ -0,0 +1,348 @@
+/*
+ *      Intel Atom E6xx Watchdog driver
+ *
+ *      Copyright (C) 2011 Alexander Stein
+ *                <alexander.stein@systec-electronic.com>
+ *
+ *      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., 59 Temple Place - Suite 330,
+ *      Boston, MA  02111-1307, USA.
+ *      The full GNU General Public License is included in this
+ *      distribution in the file called COPYING.
+ *
+ */
+
+#include <linux/module.h>
+#include <linux/moduleparam.h>
+#include <linux/platform_device.h>
+#include <linux/io.h>
+#include <linux/kernel.h>
+#include <linux/types.h>
+#include <linux/watchdog.h>
+#include <linux/miscdevice.h>
+#include <linux/seq_file.h>
+#include <linux/debugfs.h>
+#include <linux/uaccess.h>
+#include <linux/spinlock.h>
+
+#define DRIVER_NAME "ie6xx_wdt"
+
+#define PV1	0x00
+#define PV2	0x04
+
+#define RR0	0x0c
+#define RR1	0x0d
+#define WDT_RELOAD	0x01
+#define WDT_TOUT	0x02
+
+#define WDTCR	0x10
+#define WDT_PRE_SEL	0x04
+#define WDT_RESET_SEL	0x08
+#define WDT_RESET_EN	0x10
+#define WDT_TOUT_EN	0x20
+
+#define DCR	0x14
+
+#define WDTLR	0x18
+#define WDT_LOCK	0x01
+#define WDT_ENABLE	0x02
+#define WDT_TOUT_CNF	0x03
+
+#define MIN_TIME	1
+#define MAX_TIME	(10 * 60) /* 10 minutes */
+#define DEFAULT_TIME	60
+
+static unsigned int timeout = DEFAULT_TIME;
+module_param(timeout, uint, 0);
+MODULE_PARM_DESC(timeout,
+		"Default Watchdog timer setting ("
+		__MODULE_STRING(DEFAULT_TIME) "s)."
+		"The range is from 1 to 600");
+
+static bool nowayout = WATCHDOG_NOWAYOUT;
+module_param(nowayout, bool, 0);
+MODULE_PARM_DESC(nowayout,
+	"Watchdog cannot be stopped once started (default="
+		__MODULE_STRING(WATCHDOG_NOWAYOUT) ")");
+
+static u8 resetmode = 0x10;
+module_param(resetmode, byte, 0);
+MODULE_PARM_DESC(resetmode,
+	"Resetmode bits: 0x08 warm reset (cold reset otherwise), "
+	"0x10 reset enable, 0x20 disable toggle GPIO[4] (default=0x10)");
+
+static struct {
+	unsigned short sch_wdtba;
+	struct spinlock unlock_sequence;
+#ifdef CONFIG_DEBUG_FS
+	struct dentry *debugfs;
+#endif
+} ie6xx_wdt_data;
+
+/*
+ * This is needed to write to preload and reload registers
+ * struct ie6xx_wdt_data.unlock_sequence must be used
+ * to prevent sequence interrupts
+ */
+static void ie6xx_wdt_unlock_registers(void)
+{
+	outb(0x80, ie6xx_wdt_data.sch_wdtba + RR0);
+	outb(0x86, ie6xx_wdt_data.sch_wdtba + RR0);
+}
+
+static int ie6xx_wdt_ping(struct watchdog_device *wdd)
+{
+	spin_lock(&ie6xx_wdt_data.unlock_sequence);
+	ie6xx_wdt_unlock_registers();
+	outb(WDT_RELOAD, ie6xx_wdt_data.sch_wdtba + RR1);
+	spin_unlock(&ie6xx_wdt_data.unlock_sequence);
+	return 0;
+}
+
+static int ie6xx_wdt_set_timeout(struct watchdog_device *wdd, unsigned int t)
+{
+	u32 preload;
+	u64 clock;
+	u8 wdtcr;
+
+	/* Watchdog clock is PCI Clock (33MHz) */
+	clock = 33000000;
+	/* and the preload value is loaded into [34:15] of the down counter */
+	preload = (t * clock) >> 15;
+	/*
+	 * Manual states preload must be one less.
+	 * Does not wrap as t is at least 1
+	 */
+	preload -= 1;
+
+	spin_lock(&ie6xx_wdt_data.unlock_sequence);
+
+	/* Set ResetMode & Enable prescaler for range 10ms to 10 min */
+	wdtcr = resetmode & 0x38;
+	outb(wdtcr, ie6xx_wdt_data.sch_wdtba + WDTCR);
+
+	ie6xx_wdt_unlock_registers();
+	outl(0, ie6xx_wdt_data.sch_wdtba + PV1);
+
+	ie6xx_wdt_unlock_registers();
+	outl(preload, ie6xx_wdt_data.sch_wdtba + PV2);
+
+	ie6xx_wdt_unlock_registers();
+	outb(WDT_RELOAD | WDT_TOUT, ie6xx_wdt_data.sch_wdtba + RR1);
+
+	spin_unlock(&ie6xx_wdt_data.unlock_sequence);
+
+	wdd->timeout = t;
+	return 0;
+}
+
+static int ie6xx_wdt_start(struct watchdog_device *wdd)
+{
+	ie6xx_wdt_set_timeout(wdd, wdd->timeout);
+
+	/* Enable the watchdog timer */
+	spin_lock(&ie6xx_wdt_data.unlock_sequence);
+	outb(WDT_ENABLE, ie6xx_wdt_data.sch_wdtba + WDTLR);
+	spin_unlock(&ie6xx_wdt_data.unlock_sequence);
+
+	return 0;
+}
+
+static int ie6xx_wdt_stop(struct watchdog_device *wdd)
+{
+	if (inb(ie6xx_wdt_data.sch_wdtba + WDTLR) & WDT_LOCK)
+		return -1;
+
+	/* Disable the watchdog timer */
+	spin_lock(&ie6xx_wdt_data.unlock_sequence);
+	outb(0, ie6xx_wdt_data.sch_wdtba + WDTLR);
+	spin_unlock(&ie6xx_wdt_data.unlock_sequence);
+
+	return 0;
+}
+
+static const struct watchdog_info ie6xx_wdt_info = {
+	.identity =	"Intel Atom E6xx Watchdog",
+	.options =	WDIOF_SETTIMEOUT |
+			WDIOF_MAGICCLOSE |
+			WDIOF_KEEPALIVEPING,
+};
+
+static const struct watchdog_ops ie6xx_wdt_ops = {
+	.owner =	THIS_MODULE,
+	.start =	ie6xx_wdt_start,
+	.stop =		ie6xx_wdt_stop,
+	.ping =		ie6xx_wdt_ping,
+	.set_timeout =	ie6xx_wdt_set_timeout,
+};
+
+static struct watchdog_device ie6xx_wdt_dev = {
+	.info =		&ie6xx_wdt_info,
+	.ops =		&ie6xx_wdt_ops,
+	.min_timeout =	MIN_TIME,
+	.max_timeout =	MAX_TIME,
+};
+
+#ifdef CONFIG_DEBUG_FS
+
+static int ie6xx_wdt_dbg_show(struct seq_file *s, void *unused)
+{
+	seq_printf(s, "PV1   = 0x%08x\n",
+		inl(ie6xx_wdt_data.sch_wdtba + PV1));
+	seq_printf(s, "PV2   = 0x%08x\n",
+		inl(ie6xx_wdt_data.sch_wdtba + PV2));
+	seq_printf(s, "RR    = 0x%08x\n",
+		inw(ie6xx_wdt_data.sch_wdtba + RR0));
+	seq_printf(s, "WDTCR = 0x%08x\n",
+		inw(ie6xx_wdt_data.sch_wdtba + WDTCR));
+	seq_printf(s, "DCR   = 0x%08x\n",
+		inl(ie6xx_wdt_data.sch_wdtba + DCR));
+	seq_printf(s, "WDTLR = 0x%08x\n",
+		inw(ie6xx_wdt_data.sch_wdtba + WDTLR));
+
+	seq_printf(s, "\n");
+	return 0;
+}
+
+static int ie6xx_wdt_dbg_open(struct inode *inode, struct file *file)
+{
+	return single_open(file, ie6xx_wdt_dbg_show, NULL);
+}
+
+static const struct file_operations ie6xx_wdt_dbg_operations = {
+	.open		= ie6xx_wdt_dbg_open,
+	.read		= seq_read,
+	.llseek		= seq_lseek,
+	.release	= single_release,
+};
+
+static void __devinit ie6xx_wdt_debugfs_init(void)
+{
+	/* /sys/kernel/debug/ie6xx_wdt */
+	ie6xx_wdt_data.debugfs = debugfs_create_file("ie6xx_wdt",
+		S_IFREG | S_IRUGO, NULL, NULL, &ie6xx_wdt_dbg_operations);
+}
+
+static void __devexit ie6xx_wdt_debugfs_exit(void)
+{
+	debugfs_remove(ie6xx_wdt_data.debugfs);
+}
+
+#else
+static void __devinit ie6xx_wdt_debugfs_init(void)
+{
+}
+
+static void __devexit ie6xx_wdt_debugfs_exit(void)
+{
+}
+#endif
+
+static int __devinit ie6xx_wdt_probe(struct platform_device *pdev)
+{
+	struct resource *res;
+	u8 wdtlr;
+	int ret;
+
+	res = platform_get_resource(pdev, IORESOURCE_IO, 0);
+	if (!res)
+		return -ENODEV;
+
+	if (!request_region(res->start, resource_size(res), pdev->name)) {
+		dev_err(&pdev->dev, "Watchdog region 0x%llx already in use!\n",
+			(u64)res->start);
+		return -EBUSY;
+	}
+
+	ie6xx_wdt_data.sch_wdtba = res->start;
+	dev_dbg(&pdev->dev, "WDT = 0x%X\n", ie6xx_wdt_data.sch_wdtba);
+
+	ie6xx_wdt_dev.timeout = timeout;
+	watchdog_set_nowayout(&ie6xx_wdt_dev, nowayout);
+
+	spin_lock_init(&ie6xx_wdt_data.unlock_sequence);
+
+	wdtlr = inb(ie6xx_wdt_data.sch_wdtba + WDTLR);
+	if (wdtlr & WDT_LOCK)
+		dev_warn(&pdev->dev,
+			"Watchdog Timer is Locked (Reg=0x%x)\n", wdtlr);
+
+	ie6xx_wdt_debugfs_init();
+
+	ret = watchdog_register_device(&ie6xx_wdt_dev);
+	if (ret) {
+		dev_err(&pdev->dev,
+			"Watchdog timer: cannot register device (err =%d)\n",
+									ret);
+		goto misc_register_error;
+	}
+
+	return 0;
+
+misc_register_error:
+	ie6xx_wdt_debugfs_exit();
+	release_region(res->start, resource_size(res));
+	ie6xx_wdt_data.sch_wdtba = 0;
+	return ret;
+}
+
+static int __devexit ie6xx_wdt_remove(struct platform_device *pdev)
+{
+	struct resource *res;
+
+	res = platform_get_resource(pdev, IORESOURCE_IO, 0);
+	ie6xx_wdt_stop(NULL);
+	watchdog_unregister_device(&ie6xx_wdt_dev);
+	ie6xx_wdt_debugfs_exit();
+	release_region(res->start, resource_size(res));
+	ie6xx_wdt_data.sch_wdtba = 0;
+
+	return 0;
+}
+
+static struct platform_driver ie6xx_wdt_driver = {
+	.probe		= ie6xx_wdt_probe,
+	.remove		= __devexit_p(ie6xx_wdt_remove),
+	.driver		= {
+		.name	= DRIVER_NAME,
+		.owner	= THIS_MODULE,
+	},
+};
+
+static int __init ie6xx_wdt_init(void)
+{
+	/* Check boot parameters to verify that their initial values */
+	/* are in range. */
+	if ((timeout < MIN_TIME) ||
+	    (timeout > MAX_TIME)) {
+		pr_err("Watchdog timer: value of timeout %d (dec) "
+		  "is out of range from %d to %d (dec)\n",
+		  timeout, MIN_TIME, MAX_TIME);
+		return -EINVAL;
+	}
+
+	return platform_driver_register(&ie6xx_wdt_driver);
+}
+
+static void __exit ie6xx_wdt_exit(void)
+{
+	platform_driver_unregister(&ie6xx_wdt_driver);
+}
+
+late_initcall(ie6xx_wdt_init);
+module_exit(ie6xx_wdt_exit);
+
+MODULE_AUTHOR("Alexander Stein <alexander.stein@systec-electronic.com>");
+MODULE_DESCRIPTION("Intel Atom E6xx Watchdog Device Driver");
+MODULE_LICENSE("GPL");
+MODULE_ALIAS_MISCDEV(WATCHDOG_MINOR);
+MODULE_ALIAS("platform:" DRIVER_NAME);
diff --git a/drivers/watchdog/imx2_wdt.c b/drivers/watchdog/imx2_wdt.c
index 7a2b734fcdc..bcfab2b00ad 100644
--- a/drivers/watchdog/imx2_wdt.c
+++ b/drivers/watchdog/imx2_wdt.c
@@ -121,7 +121,7 @@ static void imx2_wdt_start(void)
 {
 	if (!test_and_set_bit(IMX2_WDT_STATUS_STARTED, &imx2_wdt.status)) {
 		/* at our first start we enable clock and do initialisations */
-		clk_enable(imx2_wdt.clk);
+		clk_prepare_enable(imx2_wdt.clk);
 
 		imx2_wdt_setup();
 	} else	/* delete the timer that pings the watchdog after close */
diff --git a/drivers/watchdog/it87_wdt.c b/drivers/watchdog/it87_wdt.c
index 8a741bcb512..d3dcc6988b5 100644
--- a/drivers/watchdog/it87_wdt.c
+++ b/drivers/watchdog/it87_wdt.c
@@ -12,7 +12,8 @@
  *		    http://www.ite.com.tw/
  *
  *	Support of the watchdog timers, which are available on
- *	IT8702, IT8712, IT8716, IT8718, IT8720, IT8721 and IT8726.
+ *	IT8702, IT8712, IT8716, IT8718, IT8720, IT8721, IT8726
+ *	and IT8728.
  *
  *	This program is free software; you can redistribute it and/or
  *	modify it under the terms of the GNU General Public License
@@ -84,6 +85,7 @@
 #define IT8720_ID	0x8720
 #define IT8721_ID	0x8721
 #define IT8726_ID	0x8726	/* the data sheet suggest wrongly 0x8716 */
+#define IT8728_ID	0x8728
 
 /* GPIO Configuration Registers LDN=0x07 */
 #define WDTCTRL		0x71
@@ -95,7 +97,7 @@
 #define WDT_CIRINT	0x80
 #define WDT_MOUSEINT	0x40
 #define WDT_KYBINT	0x20
-#define WDT_GAMEPORT	0x10 /* not in it8718, it8720, it8721 */
+#define WDT_GAMEPORT	0x10 /* not in it8718, it8720, it8721, it8728 */
 #define WDT_FORCE	0x02
 #define WDT_ZERO	0x01
 
@@ -616,6 +618,7 @@ static int __init it87_wdt_init(void)
 	case IT8718_ID:
 	case IT8720_ID:
 	case IT8721_ID:
+	case IT8728_ID:
 		max_units = 65535;
 		try_gameport = 0;
 		break;
diff --git a/drivers/watchdog/ixp2000_wdt.c b/drivers/watchdog/ixp2000_wdt.c
deleted file mode 100644
index 3f047a58d3a..00000000000
--- a/drivers/watchdog/ixp2000_wdt.c
+++ /dev/null
@@ -1,215 +0,0 @@
-/*
- * drivers/char/watchdog/ixp2000_wdt.c
- *
- * Watchdog driver for Intel IXP2000 network processors
- *
- * Adapted from the IXP4xx watchdog driver by Lennert Buytenhek.
- * The original version carries these notices:
- *
- * Author: Deepak Saxena <dsaxena@plexity.net>
- *
- * Copyright 2004 (c) MontaVista, Software, Inc.
- * Based on sa1100 driver, Copyright (C) 2000 Oleg Drokin <green@crimea.edu>
- *
- * 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.
- */
-
-#define pr_fmt(fmt) KBUILD_MODNAME ": " fmt
-
-#include <linux/module.h>
-#include <linux/moduleparam.h>
-#include <linux/types.h>
-#include <linux/timer.h>
-#include <linux/kernel.h>
-#include <linux/fs.h>
-#include <linux/miscdevice.h>
-#include <linux/watchdog.h>
-#include <linux/init.h>
-#include <linux/bitops.h>
-#include <linux/uaccess.h>
-#include <mach/hardware.h>
-
-static bool nowayout = WATCHDOG_NOWAYOUT;
-static unsigned int heartbeat = 60;	/* (secs) Default is 1 minute */
-static unsigned long wdt_status;
-static DEFINE_SPINLOCK(wdt_lock);
-
-#define	WDT_IN_USE		0
-#define	WDT_OK_TO_CLOSE		1
-
-static unsigned long wdt_tick_rate;
-
-static void wdt_enable(void)
-{
-	spin_lock(&wdt_lock);
-	ixp2000_reg_write(IXP2000_RESET0, *(IXP2000_RESET0) | WDT_RESET_ENABLE);
-	ixp2000_reg_write(IXP2000_TWDE, WDT_ENABLE);
-	ixp2000_reg_write(IXP2000_T4_CLD, heartbeat * wdt_tick_rate);
-	ixp2000_reg_write(IXP2000_T4_CTL, TIMER_DIVIDER_256 | TIMER_ENABLE);
-	spin_unlock(&wdt_lock);
-}
-
-static void wdt_disable(void)
-{
-	spin_lock(&wdt_lock);
-	ixp2000_reg_write(IXP2000_T4_CTL, 0);
-	spin_unlock(&wdt_lock);
-}
-
-static void wdt_keepalive(void)
-{
-	spin_lock(&wdt_lock);
-	ixp2000_reg_write(IXP2000_T4_CLD, heartbeat * wdt_tick_rate);
-	spin_unlock(&wdt_lock);
-}
-
-static int ixp2000_wdt_open(struct inode *inode, struct file *file)
-{
-	if (test_and_set_bit(WDT_IN_USE, &wdt_status))
-		return -EBUSY;
-
-	clear_bit(WDT_OK_TO_CLOSE, &wdt_status);
-
-	wdt_enable();
-
-	return nonseekable_open(inode, file);
-}
-
-static ssize_t ixp2000_wdt_write(struct file *file, const char *data,
-						size_t len, loff_t *ppos)
-{
-	if (len) {
-		if (!nowayout) {
-			size_t i;
-
-			clear_bit(WDT_OK_TO_CLOSE, &wdt_status);
-
-			for (i = 0; i != len; i++) {
-				char c;
-
-				if (get_user(c, data + i))
-					return -EFAULT;
-				if (c == 'V')
-					set_bit(WDT_OK_TO_CLOSE, &wdt_status);
-			}
-		}
-		wdt_keepalive();
-	}
-
-	return len;
-}
-
-
-static const struct watchdog_info ident = {
-	.options	= WDIOF_MAGICCLOSE | WDIOF_SETTIMEOUT |
-				WDIOF_KEEPALIVEPING,
-	.identity	= "IXP2000 Watchdog",
-};
-
-static long ixp2000_wdt_ioctl(struct file *file, unsigned int cmd,
-							unsigned long arg)
-{
-	int ret = -ENOTTY;
-	int time;
-
-	switch (cmd) {
-	case WDIOC_GETSUPPORT:
-		ret = copy_to_user((struct watchdog_info *)arg, &ident,
-				   sizeof(ident)) ? -EFAULT : 0;
-		break;
-
-	case WDIOC_GETSTATUS:
-		ret = put_user(0, (int *)arg);
-		break;
-
-	case WDIOC_GETBOOTSTATUS:
-		ret = put_user(0, (int *)arg);
-		break;
-
-	case WDIOC_KEEPALIVE:
-		wdt_enable();
-		ret = 0;
-		break;
-
-	case WDIOC_SETTIMEOUT:
-		ret = get_user(time, (int *)arg);
-		if (ret)
-			break;
-
-		if (time <= 0 || time > 60) {
-			ret = -EINVAL;
-			break;
-		}
-
-		heartbeat = time;
-		wdt_keepalive();
-		/* Fall through */
-
-	case WDIOC_GETTIMEOUT:
-		ret = put_user(heartbeat, (int *)arg);
-		break;
-	}
-
-	return ret;
-}
-
-static int ixp2000_wdt_release(struct inode *inode, struct file *file)
-{
-	if (test_bit(WDT_OK_TO_CLOSE, &wdt_status))
-		wdt_disable();
-	else
-		pr_crit("Device closed unexpectedly - timer will not stop\n");
-	clear_bit(WDT_IN_USE, &wdt_status);
-	clear_bit(WDT_OK_TO_CLOSE, &wdt_status);
-
-	return 0;
-}
-
-
-static const struct file_operations ixp2000_wdt_fops = {
-	.owner		= THIS_MODULE,
-	.llseek		= no_llseek,
-	.write		= ixp2000_wdt_write,
-	.unlocked_ioctl	= ixp2000_wdt_ioctl,
-	.open		= ixp2000_wdt_open,
-	.release	= ixp2000_wdt_release,
-};
-
-static struct miscdevice ixp2000_wdt_miscdev = {
-	.minor		= WATCHDOG_MINOR,
-	.name		= "watchdog",
-	.fops		= &ixp2000_wdt_fops,
-};
-
-static int __init ixp2000_wdt_init(void)
-{
-	if ((*IXP2000_PRODUCT_ID & 0x001ffef0) == 0x00000000) {
-		pr_info("Unable to use IXP2000 watchdog due to IXP2800 erratum #25\n");
-		return -EIO;
-	}
-	wdt_tick_rate = (*IXP2000_T1_CLD * HZ) / 256;
-	return misc_register(&ixp2000_wdt_miscdev);
-}
-
-static void __exit ixp2000_wdt_exit(void)
-{
-	misc_deregister(&ixp2000_wdt_miscdev);
-}
-
-module_init(ixp2000_wdt_init);
-module_exit(ixp2000_wdt_exit);
-
-MODULE_AUTHOR("Deepak Saxena <dsaxena@plexity.net>");
-MODULE_DESCRIPTION("IXP2000 Network Processor Watchdog");
-
-module_param(heartbeat, int, 0);
-MODULE_PARM_DESC(heartbeat, "Watchdog heartbeat in seconds (default 60s)");
-
-module_param(nowayout, bool, 0);
-MODULE_PARM_DESC(nowayout, "Watchdog cannot be stopped once started");
-
-MODULE_LICENSE("GPL");
-MODULE_ALIAS_MISCDEV(WATCHDOG_MINOR);
-
diff --git a/drivers/watchdog/lantiq_wdt.c b/drivers/watchdog/lantiq_wdt.c
index a9593a3a32a..2e74c3a8ee5 100644
--- a/drivers/watchdog/lantiq_wdt.c
+++ b/drivers/watchdog/lantiq_wdt.c
@@ -13,14 +13,15 @@
 #include <linux/fs.h>
 #include <linux/miscdevice.h>
 #include <linux/watchdog.h>
-#include <linux/platform_device.h>
+#include <linux/of_platform.h>
 #include <linux/uaccess.h>
 #include <linux/clk.h>
 #include <linux/io.h>
 
-#include <lantiq.h>
+#include <lantiq_soc.h>
 
-/* Section 3.4 of the datasheet
+/*
+ * Section 3.4 of the datasheet
  * The password sequence protects the WDT control register from unintended
  * write actions, which might cause malfunction of the WDT.
  *
@@ -70,7 +71,8 @@ ltq_wdt_disable(void)
 {
 	/* write the first password magic */
 	ltq_w32(LTQ_WDT_PW1, ltq_wdt_membase + LTQ_WDT_CR);
-	/* write the second password magic with no config
+	/*
+	 * write the second password magic with no config
 	 * this turns the watchdog off
 	 */
 	ltq_w32(LTQ_WDT_PW2, ltq_wdt_membase + LTQ_WDT_CR);
@@ -184,7 +186,7 @@ static struct miscdevice ltq_wdt_miscdev = {
 	.fops	= &ltq_wdt_fops,
 };
 
-static int __init
+static int __devinit
 ltq_wdt_probe(struct platform_device *pdev)
 {
 	struct resource *res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
@@ -194,28 +196,27 @@ ltq_wdt_probe(struct platform_device *pdev)
 		dev_err(&pdev->dev, "cannot obtain I/O memory region");
 		return -ENOENT;
 	}
-	res = devm_request_mem_region(&pdev->dev, res->start,
-		resource_size(res), dev_name(&pdev->dev));
-	if (!res) {
-		dev_err(&pdev->dev, "cannot request I/O memory region");
-		return -EBUSY;
-	}
-	ltq_wdt_membase = devm_ioremap_nocache(&pdev->dev, res->start,
-		resource_size(res));
+
+	ltq_wdt_membase = devm_request_and_ioremap(&pdev->dev, res);
 	if (!ltq_wdt_membase) {
 		dev_err(&pdev->dev, "cannot remap I/O memory region\n");
 		return -ENOMEM;
 	}
 
 	/* we do not need to enable the clock as it is always running */
-	clk = clk_get(&pdev->dev, "io");
-	WARN_ON(!clk);
+	clk = clk_get_io();
+	if (IS_ERR(clk)) {
+		dev_err(&pdev->dev, "Failed to get clock\n");
+		return -ENOENT;
+	}
 	ltq_io_region_clk_rate = clk_get_rate(clk);
 	clk_put(clk);
 
+	/* find out if the watchdog caused the last reboot */
 	if (ltq_reset_cause() == LTQ_RST_CAUSE_WDTRST)
 		ltq_wdt_bootstatus = WDIOF_CARDRESET;
 
+	dev_info(&pdev->dev, "Init done\n");
 	return misc_register(&ltq_wdt_miscdev);
 }
 
@@ -227,33 +228,26 @@ ltq_wdt_remove(struct platform_device *pdev)
 	return 0;
 }
 
+static const struct of_device_id ltq_wdt_match[] = {
+	{ .compatible = "lantiq,wdt" },
+	{},
+};
+MODULE_DEVICE_TABLE(of, ltq_wdt_match);
 
 static struct platform_driver ltq_wdt_driver = {
+	.probe = ltq_wdt_probe,
 	.remove = __devexit_p(ltq_wdt_remove),
 	.driver = {
-		.name = "ltq_wdt",
+		.name = "wdt",
 		.owner = THIS_MODULE,
+		.of_match_table = ltq_wdt_match,
 	},
 };
 
-static int __init
-init_ltq_wdt(void)
-{
-	return platform_driver_probe(&ltq_wdt_driver, ltq_wdt_probe);
-}
-
-static void __exit
-exit_ltq_wdt(void)
-{
-	return platform_driver_unregister(&ltq_wdt_driver);
-}
-
-module_init(init_ltq_wdt);
-module_exit(exit_ltq_wdt);
+module_platform_driver(ltq_wdt_driver);
 
 module_param(nowayout, bool, 0);
 MODULE_PARM_DESC(nowayout, "Watchdog cannot be stopped once started");
-
 MODULE_AUTHOR("John Crispin <blogic@openwrt.org>");
 MODULE_DESCRIPTION("Lantiq SoC Watchdog");
 MODULE_LICENSE("GPL");
diff --git a/drivers/watchdog/orion_wdt.c b/drivers/watchdog/orion_wdt.c
index 788aa158e78..0f5736949c6 100644
--- a/drivers/watchdog/orion_wdt.c
+++ b/drivers/watchdog/orion_wdt.c
@@ -24,8 +24,8 @@
 #include <linux/uaccess.h>
 #include <linux/io.h>
 #include <linux/spinlock.h>
+#include <linux/clk.h>
 #include <mach/bridge-regs.h>
-#include <plat/orion_wdt.h>
 
 /*
  * Watchdog timer block registers.
@@ -41,6 +41,7 @@
 static bool nowayout = WATCHDOG_NOWAYOUT;
 static int heartbeat = -1;		/* module parameter (seconds) */
 static unsigned int wdt_max_duration;	/* (seconds) */
+static struct clk *clk;
 static unsigned int wdt_tclk;
 static void __iomem *wdt_reg;
 static unsigned long wdt_status;
@@ -237,16 +238,16 @@ static struct miscdevice orion_wdt_miscdev = {
 
 static int __devinit orion_wdt_probe(struct platform_device *pdev)
 {
-	struct orion_wdt_platform_data *pdata = pdev->dev.platform_data;
 	struct resource *res;
 	int ret;
 
-	if (pdata) {
-		wdt_tclk = pdata->tclk;
-	} else {
-		pr_err("misses platform data\n");
+	clk = clk_get(&pdev->dev, NULL);
+	if (IS_ERR(clk)) {
+		printk(KERN_ERR "Orion Watchdog missing clock\n");
 		return -ENODEV;
 	}
+	clk_prepare_enable(clk);
+	wdt_tclk = clk_get_rate(clk);
 
 	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
 
@@ -282,6 +283,9 @@ static int __devexit orion_wdt_remove(struct platform_device *pdev)
 	if (!ret)
 		orion_wdt_miscdev.parent = NULL;
 
+	clk_disable_unprepare(clk);
+	clk_put(clk);
+
 	return ret;
 }
 
diff --git a/drivers/watchdog/pcwd_pci.c b/drivers/watchdog/pcwd_pci.c
index c891399bed6..ee6900da867 100644
--- a/drivers/watchdog/pcwd_pci.c
+++ b/drivers/watchdog/pcwd_pci.c
@@ -707,6 +707,7 @@ static int __devinit pcipcwd_card_init(struct pci_dev *pdev,
 		goto err_out_disable_device;
 	}
 
+	spin_lock_init(&pcipcwd_private.io_lock);
 	pcipcwd_private.pdev = pdev;
 	pcipcwd_private.io_addr = pci_resource_start(pdev, 0);
 
@@ -814,22 +815,7 @@ static struct pci_driver pcipcwd_driver = {
 	.remove		= __devexit_p(pcipcwd_card_exit),
 };
 
-static int __init pcipcwd_init_module(void)
-{
-	spin_lock_init(&pcipcwd_private.io_lock);
-
-	return pci_register_driver(&pcipcwd_driver);
-}
-
-static void __exit pcipcwd_cleanup_module(void)
-{
-	pci_unregister_driver(&pcipcwd_driver);
-
-	pr_info("Watchdog Module Unloaded\n");
-}
-
-module_init(pcipcwd_init_module);
-module_exit(pcipcwd_cleanup_module);
+module_pci_driver(pcipcwd_driver);
 
 MODULE_AUTHOR("Wim Van Sebroeck <wim@iguana.be>");
 MODULE_DESCRIPTION("Berkshire PCI-PC Watchdog driver");
diff --git a/drivers/watchdog/pnx4008_wdt.c b/drivers/watchdog/pnx4008_wdt.c
index 6b8432f61d0..87722e12605 100644
--- a/drivers/watchdog/pnx4008_wdt.c
+++ b/drivers/watchdog/pnx4008_wdt.c
@@ -32,6 +32,7 @@
 #include <linux/io.h>
 #include <linux/slab.h>
 #include <linux/err.h>
+#include <linux/of.h>
 #include <mach/hardware.h>
 
 /* WatchDog Timer - Chapter 23 Page 207 */
@@ -201,10 +202,19 @@ static int __devexit pnx4008_wdt_remove(struct platform_device *pdev)
 	return 0;
 }
 
+#ifdef CONFIG_OF
+static const struct of_device_id pnx4008_wdt_match[] = {
+	{ .compatible = "nxp,pnx4008-wdt" },
+	{ }
+};
+MODULE_DEVICE_TABLE(of, pnx4008_wdt_match);
+#endif
+
 static struct platform_driver platform_wdt_driver = {
 	.driver = {
 		.name = "pnx4008-watchdog",
 		.owner	= THIS_MODULE,
+		.of_match_table = of_match_ptr(pnx4008_wdt_match),
 	},
 	.probe = pnx4008_wdt_probe,
 	.remove = __devexit_p(pnx4008_wdt_remove),
diff --git a/drivers/watchdog/s3c2410_wdt.c b/drivers/watchdog/s3c2410_wdt.c
index 04e5a6de47d..200ece5e2a2 100644
--- a/drivers/watchdog/s3c2410_wdt.c
+++ b/drivers/watchdog/s3c2410_wdt.c
@@ -40,6 +40,7 @@
 #include <linux/cpufreq.h>
 #include <linux/slab.h>
 #include <linux/err.h>
+#include <linux/of.h>
 
 #include <mach/map.h>
 
@@ -201,7 +202,7 @@ static int s3c2410wdt_set_heartbeat(struct watchdog_device *wdd, unsigned timeou
 	writel(count, wdt_base + S3C2410_WTDAT);
 	writel(wtcon, wdt_base + S3C2410_WTCON);
 
-	wdd->timeout = timeout;
+	wdd->timeout = (count * divisor) / freq;
 
 	return 0;
 }
@@ -503,8 +504,6 @@ static const struct of_device_id s3c2410_wdt_match[] = {
 	{},
 };
 MODULE_DEVICE_TABLE(of, s3c2410_wdt_match);
-#else
-#define s3c2410_wdt_match NULL
 #endif
 
 static struct platform_driver s3c2410wdt_driver = {
@@ -516,7 +515,7 @@ static struct platform_driver s3c2410wdt_driver = {
 	.driver		= {
 		.owner	= THIS_MODULE,
 		.name	= "s3c2410-wdt",
-		.of_match_table	= s3c2410_wdt_match,
+		.of_match_table	= of_match_ptr(s3c2410_wdt_match),
 	},
 };
 
diff --git a/drivers/watchdog/sch311x_wdt.c b/drivers/watchdog/sch311x_wdt.c
index bd86f32d63a..f8477002b72 100644
--- a/drivers/watchdog/sch311x_wdt.c
+++ b/drivers/watchdog/sch311x_wdt.c
@@ -41,7 +41,6 @@
 #define DRV_NAME	"sch311x_wdt"
 
 /* Runtime registers */
-#define RESGEN			0x1d
 #define GP60			0x47
 #define WDT_TIME_OUT		0x65
 #define WDT_VAL			0x66
@@ -69,10 +68,6 @@ static unsigned short force_id;
 module_param(force_id, ushort, 0);
 MODULE_PARM_DESC(force_id, "Override the detected device ID");
 
-static unsigned short therm_trip;
-module_param(therm_trip, ushort, 0);
-MODULE_PARM_DESC(therm_trip, "Should a ThermTrip trigger the reset generator");
-
 #define WATCHDOG_TIMEOUT 60		/* 60 sec default timeout */
 static int timeout = WATCHDOG_TIMEOUT;	/* in seconds */
 module_param(timeout, int, 0);
@@ -358,26 +353,16 @@ static struct miscdevice sch311x_wdt_miscdev = {
 static int __devinit sch311x_wdt_probe(struct platform_device *pdev)
 {
 	struct device *dev = &pdev->dev;
-	unsigned char val;
 	int err;
 
 	spin_lock_init(&sch311x_wdt_data.io_lock);
 
-	if (!request_region(sch311x_wdt_data.runtime_reg + RESGEN, 1,
-								DRV_NAME)) {
-		dev_err(dev, "Failed to request region 0x%04x-0x%04x.\n",
-			sch311x_wdt_data.runtime_reg + RESGEN,
-			sch311x_wdt_data.runtime_reg + RESGEN);
-		err = -EBUSY;
-		goto exit;
-	}
-
 	if (!request_region(sch311x_wdt_data.runtime_reg + GP60, 1, DRV_NAME)) {
 		dev_err(dev, "Failed to request region 0x%04x-0x%04x.\n",
 			sch311x_wdt_data.runtime_reg + GP60,
 			sch311x_wdt_data.runtime_reg + GP60);
 		err = -EBUSY;
-		goto exit_release_region;
+		goto exit;
 	}
 
 	if (!request_region(sch311x_wdt_data.runtime_reg + WDT_TIME_OUT, 4,
@@ -386,7 +371,7 @@ static int __devinit sch311x_wdt_probe(struct platform_device *pdev)
 			sch311x_wdt_data.runtime_reg + WDT_TIME_OUT,
 			sch311x_wdt_data.runtime_reg + WDT_CTRL);
 		err = -EBUSY;
-		goto exit_release_region2;
+		goto exit_release_region;
 	}
 
 	/* Make sure that the watchdog is not running */
@@ -414,24 +399,13 @@ static int __devinit sch311x_wdt_probe(struct platform_device *pdev)
 	/* Get status at boot */
 	sch311x_wdt_get_status(&sch311x_wdt_data.boot_status);
 
-	/* enable watchdog */
-	/* -- Reset Generator --
-	 * Bit 0   Enable Watchdog Timer Generation: 0* = Enabled, 1 = Disabled
-	 * Bit 1   Thermtrip Source Select: O* = No Source, 1 = Source
-	 * Bit 2   WDT2_CTL: WDT input bit
-	 * Bit 3-7 Reserved
-	 */
-	outb(0, sch311x_wdt_data.runtime_reg + RESGEN);
-	val = therm_trip ? 0x06 : 0x04;
-	outb(val, sch311x_wdt_data.runtime_reg + RESGEN);
-
 	sch311x_wdt_miscdev.parent = dev;
 
 	err = misc_register(&sch311x_wdt_miscdev);
 	if (err != 0) {
 		dev_err(dev, "cannot register miscdev on minor=%d (err=%d)\n",
 							WATCHDOG_MINOR, err);
-		goto exit_release_region3;
+		goto exit_release_region2;
 	}
 
 	dev_info(dev,
@@ -440,12 +414,10 @@ static int __devinit sch311x_wdt_probe(struct platform_device *pdev)
 
 	return 0;
 
-exit_release_region3:
-	release_region(sch311x_wdt_data.runtime_reg + WDT_TIME_OUT, 4);
 exit_release_region2:
-	release_region(sch311x_wdt_data.runtime_reg + GP60, 1);
+	release_region(sch311x_wdt_data.runtime_reg + WDT_TIME_OUT, 4);
 exit_release_region:
-	release_region(sch311x_wdt_data.runtime_reg + RESGEN, 1);
+	release_region(sch311x_wdt_data.runtime_reg + GP60, 1);
 	sch311x_wdt_data.runtime_reg = 0;
 exit:
 	return err;
@@ -461,7 +433,6 @@ static int __devexit sch311x_wdt_remove(struct platform_device *pdev)
 	misc_deregister(&sch311x_wdt_miscdev);
 	release_region(sch311x_wdt_data.runtime_reg + WDT_TIME_OUT, 4);
 	release_region(sch311x_wdt_data.runtime_reg + GP60, 1);
-	release_region(sch311x_wdt_data.runtime_reg + RESGEN, 1);
 	sch311x_wdt_data.runtime_reg = 0;
 	return 0;
 }
diff --git a/drivers/watchdog/shwdt.c b/drivers/watchdog/shwdt.c
index 93958a7763e..e5b59bebcdb 100644
--- a/drivers/watchdog/shwdt.c
+++ b/drivers/watchdog/shwdt.c
@@ -3,7 +3,7 @@
  *
  * Watchdog driver for integrated watchdog in the SuperH processors.
  *
- * Copyright (C) 2001 - 2010  Paul Mundt <lethal@linux-sh.org>
+ * Copyright (C) 2001 - 2012  Paul Mundt <lethal@linux-sh.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
@@ -25,16 +25,15 @@
 #include <linux/platform_device.h>
 #include <linux/init.h>
 #include <linux/types.h>
+#include <linux/spinlock.h>
 #include <linux/miscdevice.h>
 #include <linux/watchdog.h>
-#include <linux/reboot.h>
-#include <linux/notifier.h>
-#include <linux/ioport.h>
+#include <linux/pm_runtime.h>
 #include <linux/fs.h>
 #include <linux/mm.h>
 #include <linux/slab.h>
 #include <linux/io.h>
-#include <linux/uaccess.h>
+#include <linux/clk.h>
 #include <asm/watchdog.h>
 
 #define DRV_NAME "sh-wdt"
@@ -69,10 +68,6 @@
 static int clock_division_ratio = WTCSR_CKS_4096;
 #define next_ping_period(cks)	(jiffies + msecs_to_jiffies(cks - 4))
 
-static const struct watchdog_info sh_wdt_info;
-static struct platform_device *sh_wdt_dev;
-static DEFINE_SPINLOCK(shwdt_lock);
-
 #define WATCHDOG_HEARTBEAT 30			/* 30 sec default heartbeat */
 static int heartbeat = WATCHDOG_HEARTBEAT;	/* in seconds */
 static bool nowayout = WATCHDOG_NOWAYOUT;
@@ -81,19 +76,22 @@ static unsigned long next_heartbeat;
 struct sh_wdt {
 	void __iomem		*base;
 	struct device		*dev;
+	struct clk		*clk;
+	spinlock_t		lock;
 
 	struct timer_list	timer;
-
-	unsigned long		enabled;
-	char			expect_close;
 };
 
-static void sh_wdt_start(struct sh_wdt *wdt)
+static int sh_wdt_start(struct watchdog_device *wdt_dev)
 {
+	struct sh_wdt *wdt = watchdog_get_drvdata(wdt_dev);
 	unsigned long flags;
 	u8 csr;
 
-	spin_lock_irqsave(&shwdt_lock, flags);
+	pm_runtime_get_sync(wdt->dev);
+	clk_enable(wdt->clk);
+
+	spin_lock_irqsave(&wdt->lock, flags);
 
 	next_heartbeat = jiffies + (heartbeat * HZ);
 	mod_timer(&wdt->timer, next_ping_period(clock_division_ratio));
@@ -122,15 +120,18 @@ static void sh_wdt_start(struct sh_wdt *wdt)
 	csr &= ~RSTCSR_RSTS;
 	sh_wdt_write_rstcsr(csr);
 #endif
-	spin_unlock_irqrestore(&shwdt_lock, flags);
+	spin_unlock_irqrestore(&wdt->lock, flags);
+
+	return 0;
 }
 
-static void sh_wdt_stop(struct sh_wdt *wdt)
+static int sh_wdt_stop(struct watchdog_device *wdt_dev)
 {
+	struct sh_wdt *wdt = watchdog_get_drvdata(wdt_dev);
 	unsigned long flags;
 	u8 csr;
 
-	spin_lock_irqsave(&shwdt_lock, flags);
+	spin_lock_irqsave(&wdt->lock, flags);
 
 	del_timer(&wdt->timer);
 
@@ -138,28 +139,39 @@ static void sh_wdt_stop(struct sh_wdt *wdt)
 	csr &= ~WTCSR_TME;
 	sh_wdt_write_csr(csr);
 
-	spin_unlock_irqrestore(&shwdt_lock, flags);
+	spin_unlock_irqrestore(&wdt->lock, flags);
+
+	clk_disable(wdt->clk);
+	pm_runtime_put_sync(wdt->dev);
+
+	return 0;
 }
 
-static inline void sh_wdt_keepalive(struct sh_wdt *wdt)
+static int sh_wdt_keepalive(struct watchdog_device *wdt_dev)
 {
+	struct sh_wdt *wdt = watchdog_get_drvdata(wdt_dev);
 	unsigned long flags;
 
-	spin_lock_irqsave(&shwdt_lock, flags);
+	spin_lock_irqsave(&wdt->lock, flags);
 	next_heartbeat = jiffies + (heartbeat * HZ);
-	spin_unlock_irqrestore(&shwdt_lock, flags);
+	spin_unlock_irqrestore(&wdt->lock, flags);
+
+	return 0;
 }
 
-static int sh_wdt_set_heartbeat(int t)
+static int sh_wdt_set_heartbeat(struct watchdog_device *wdt_dev, unsigned t)
 {
+	struct sh_wdt *wdt = watchdog_get_drvdata(wdt_dev);
 	unsigned long flags;
 
 	if (unlikely(t < 1 || t > 3600)) /* arbitrary upper limit */
 		return -EINVAL;
 
-	spin_lock_irqsave(&shwdt_lock, flags);
+	spin_lock_irqsave(&wdt->lock, flags);
 	heartbeat = t;
-	spin_unlock_irqrestore(&shwdt_lock, flags);
+	wdt_dev->timeout = t;
+	spin_unlock_irqrestore(&wdt->lock, flags);
+
 	return 0;
 }
 
@@ -168,7 +180,7 @@ static void sh_wdt_ping(unsigned long data)
 	struct sh_wdt *wdt = (struct sh_wdt *)data;
 	unsigned long flags;
 
-	spin_lock_irqsave(&shwdt_lock, flags);
+	spin_lock_irqsave(&wdt->lock, flags);
 	if (time_before(jiffies, next_heartbeat)) {
 		u8 csr;
 
@@ -182,137 +194,9 @@ static void sh_wdt_ping(unsigned long data)
 	} else
 		dev_warn(wdt->dev, "Heartbeat lost! Will not ping "
 		         "the watchdog\n");
-	spin_unlock_irqrestore(&shwdt_lock, flags);
-}
-
-static int sh_wdt_open(struct inode *inode, struct file *file)
-{
-	struct sh_wdt *wdt = platform_get_drvdata(sh_wdt_dev);
-
-	if (test_and_set_bit(0, &wdt->enabled))
-		return -EBUSY;
-	if (nowayout)
-		__module_get(THIS_MODULE);
-
-	file->private_data = wdt;
-
-	sh_wdt_start(wdt);
-
-	return nonseekable_open(inode, file);
-}
-
-static int sh_wdt_close(struct inode *inode, struct file *file)
-{
-	struct sh_wdt *wdt = file->private_data;
-
-	if (wdt->expect_close == 42) {
-		sh_wdt_stop(wdt);
-	} else {
-		dev_crit(wdt->dev, "Unexpected close, not "
-		         "stopping watchdog!\n");
-		sh_wdt_keepalive(wdt);
-	}
-
-	clear_bit(0, &wdt->enabled);
-	wdt->expect_close = 0;
-
-	return 0;
-}
-
-static ssize_t sh_wdt_write(struct file *file, const char *buf,
-			    size_t count, loff_t *ppos)
-{
-	struct sh_wdt *wdt = file->private_data;
-
-	if (count) {
-		if (!nowayout) {
-			size_t i;
-
-			wdt->expect_close = 0;
-
-			for (i = 0; i != count; i++) {
-				char c;
-				if (get_user(c, buf + i))
-					return -EFAULT;
-				if (c == 'V')
-					wdt->expect_close = 42;
-			}
-		}
-		sh_wdt_keepalive(wdt);
-	}
-
-	return count;
+	spin_unlock_irqrestore(&wdt->lock, flags);
 }
 
-static long sh_wdt_ioctl(struct file *file, unsigned int cmd,
-							unsigned long arg)
-{
-	struct sh_wdt *wdt = file->private_data;
-	int new_heartbeat;
-	int options, retval = -EINVAL;
-
-	switch (cmd) {
-	case WDIOC_GETSUPPORT:
-		return copy_to_user((struct watchdog_info *)arg,
-			  &sh_wdt_info, sizeof(sh_wdt_info)) ? -EFAULT : 0;
-	case WDIOC_GETSTATUS:
-	case WDIOC_GETBOOTSTATUS:
-		return put_user(0, (int *)arg);
-	case WDIOC_SETOPTIONS:
-		if (get_user(options, (int *)arg))
-			return -EFAULT;
-
-		if (options & WDIOS_DISABLECARD) {
-			sh_wdt_stop(wdt);
-			retval = 0;
-		}
-
-		if (options & WDIOS_ENABLECARD) {
-			sh_wdt_start(wdt);
-			retval = 0;
-		}
-
-		return retval;
-	case WDIOC_KEEPALIVE:
-		sh_wdt_keepalive(wdt);
-		return 0;
-	case WDIOC_SETTIMEOUT:
-		if (get_user(new_heartbeat, (int *)arg))
-			return -EFAULT;
-
-		if (sh_wdt_set_heartbeat(new_heartbeat))
-			return -EINVAL;
-
-		sh_wdt_keepalive(wdt);
-		/* Fall */
-	case WDIOC_GETTIMEOUT:
-		return put_user(heartbeat, (int *)arg);
-	default:
-		return -ENOTTY;
-	}
-	return 0;
-}
-
-static int sh_wdt_notify_sys(struct notifier_block *this,
-			     unsigned long code, void *unused)
-{
-	struct sh_wdt *wdt = platform_get_drvdata(sh_wdt_dev);
-
-	if (code == SYS_DOWN || code == SYS_HALT)
-		sh_wdt_stop(wdt);
-
-	return NOTIFY_DONE;
-}
-
-static const struct file_operations sh_wdt_fops = {
-	.owner		= THIS_MODULE,
-	.llseek		= no_llseek,
-	.write		= sh_wdt_write,
-	.unlocked_ioctl	= sh_wdt_ioctl,
-	.open		= sh_wdt_open,
-	.release	= sh_wdt_close,
-};
-
 static const struct watchdog_info sh_wdt_info = {
 	.options		= WDIOF_KEEPALIVEPING | WDIOF_SETTIMEOUT |
 				  WDIOF_MAGICCLOSE,
@@ -320,14 +204,17 @@ static const struct watchdog_info sh_wdt_info = {
 	.identity		= "SH WDT",
 };
 
-static struct notifier_block sh_wdt_notifier = {
-	.notifier_call		= sh_wdt_notify_sys,
+static const struct watchdog_ops sh_wdt_ops = {
+	.owner		= THIS_MODULE,
+	.start		= sh_wdt_start,
+	.stop		= sh_wdt_stop,
+	.ping		= sh_wdt_keepalive,
+	.set_timeout	= sh_wdt_set_heartbeat,
 };
 
-static struct miscdevice sh_wdt_miscdev = {
-	.minor		= WATCHDOG_MINOR,
-	.name		= "watchdog",
-	.fops		= &sh_wdt_fops,
+static struct watchdog_device sh_wdt_dev = {
+	.info	= &sh_wdt_info,
+	.ops	= &sh_wdt_ops,
 };
 
 static int __devinit sh_wdt_probe(struct platform_device *pdev)
@@ -347,39 +234,49 @@ static int __devinit sh_wdt_probe(struct platform_device *pdev)
 	if (unlikely(!res))
 		return -EINVAL;
 
-	if (!devm_request_mem_region(&pdev->dev, res->start,
-				     resource_size(res), DRV_NAME))
-		return -EBUSY;
-
 	wdt = devm_kzalloc(&pdev->dev, sizeof(struct sh_wdt), GFP_KERNEL);
-	if (unlikely(!wdt)) {
-		rc = -ENOMEM;
-		goto out_release;
-	}
+	if (unlikely(!wdt))
+		return -ENOMEM;
 
 	wdt->dev = &pdev->dev;
 
-	wdt->base = devm_ioremap(&pdev->dev, res->start, resource_size(res));
+	wdt->clk = clk_get(&pdev->dev, NULL);
+	if (IS_ERR(wdt->clk)) {
+		/*
+		 * Clock framework support is optional, continue on
+		 * anyways if we don't find a matching clock.
+		 */
+		wdt->clk = NULL;
+	}
+
+	wdt->base = devm_request_and_ioremap(wdt->dev, res);
 	if (unlikely(!wdt->base)) {
-		rc = -ENXIO;
-		goto out_err;
+		rc = -EADDRNOTAVAIL;
+		goto err;
 	}
 
-	rc = register_reboot_notifier(&sh_wdt_notifier);
+	watchdog_set_nowayout(&sh_wdt_dev, nowayout);
+	watchdog_set_drvdata(&sh_wdt_dev, wdt);
+
+	spin_lock_init(&wdt->lock);
+
+	rc = sh_wdt_set_heartbeat(&sh_wdt_dev, heartbeat);
 	if (unlikely(rc)) {
-		dev_err(&pdev->dev,
-			"Can't register reboot notifier (err=%d)\n", rc);
-		goto out_unmap;
+		/* Default timeout if invalid */
+		sh_wdt_set_heartbeat(&sh_wdt_dev, WATCHDOG_HEARTBEAT);
+
+		dev_warn(&pdev->dev,
+			 "heartbeat value must be 1<=x<=3600, using %d\n",
+			 sh_wdt_dev.timeout);
 	}
 
-	sh_wdt_miscdev.parent = wdt->dev;
+	dev_info(&pdev->dev, "configured with heartbeat=%d sec (nowayout=%d)\n",
+		 sh_wdt_dev.timeout, nowayout);
 
-	rc = misc_register(&sh_wdt_miscdev);
+	rc = watchdog_register_device(&sh_wdt_dev);
 	if (unlikely(rc)) {
-		dev_err(&pdev->dev,
-			"Can't register miscdev on minor=%d (err=%d)\n",
-						sh_wdt_miscdev.minor, rc);
-		goto out_unreg;
+		dev_err(&pdev->dev, "Can't register watchdog (err=%d)\n", rc);
+		goto err;
 	}
 
 	init_timer(&wdt->timer);
@@ -388,20 +285,15 @@ static int __devinit sh_wdt_probe(struct platform_device *pdev)
 	wdt->timer.expires	= next_ping_period(clock_division_ratio);
 
 	platform_set_drvdata(pdev, wdt);
-	sh_wdt_dev = pdev;
 
 	dev_info(&pdev->dev, "initialized.\n");
 
+	pm_runtime_enable(&pdev->dev);
+
 	return 0;
 
-out_unreg:
-	unregister_reboot_notifier(&sh_wdt_notifier);
-out_unmap:
-	devm_iounmap(&pdev->dev, wdt->base);
-out_err:
-	devm_kfree(&pdev->dev, wdt);
-out_release:
-	devm_release_mem_region(&pdev->dev, res->start, resource_size(res));
+err:
+	clk_put(wdt->clk);
 
 	return rc;
 }
@@ -409,36 +301,35 @@ out_release:
 static int __devexit sh_wdt_remove(struct platform_device *pdev)
 {
 	struct sh_wdt *wdt = platform_get_drvdata(pdev);
-	struct resource *res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
 
 	platform_set_drvdata(pdev, NULL);
 
-	misc_deregister(&sh_wdt_miscdev);
+	watchdog_unregister_device(&sh_wdt_dev);
 
-	sh_wdt_dev = NULL;
-
-	unregister_reboot_notifier(&sh_wdt_notifier);
-	devm_release_mem_region(&pdev->dev, res->start, resource_size(res));
-	devm_iounmap(&pdev->dev, wdt->base);
-	devm_kfree(&pdev->dev, wdt);
+	pm_runtime_disable(&pdev->dev);
+	clk_put(wdt->clk);
 
 	return 0;
 }
 
+static void sh_wdt_shutdown(struct platform_device *pdev)
+{
+	sh_wdt_stop(&sh_wdt_dev);
+}
+
 static struct platform_driver sh_wdt_driver = {
 	.driver		= {
 		.name	= DRV_NAME,
 		.owner	= THIS_MODULE,
 	},
 
-	.probe	= sh_wdt_probe,
-	.remove	= __devexit_p(sh_wdt_remove),
+	.probe		= sh_wdt_probe,
+	.remove		= __devexit_p(sh_wdt_remove),
+	.shutdown	= sh_wdt_shutdown,
 };
 
 static int __init sh_wdt_init(void)
 {
-	int rc;
-
 	if (unlikely(clock_division_ratio < 0x5 ||
 		     clock_division_ratio > 0x7)) {
 		clock_division_ratio = WTCSR_CKS_4096;
@@ -447,17 +338,6 @@ static int __init sh_wdt_init(void)
 			clock_division_ratio);
 	}
 
-	rc = sh_wdt_set_heartbeat(heartbeat);
-	if (unlikely(rc)) {
-		heartbeat = WATCHDOG_HEARTBEAT;
-
-		pr_info("heartbeat value must be 1<=x<=3600, using %d\n",
-			heartbeat);
-	}
-
-	pr_info("configured with heartbeat=%d sec (nowayout=%d)\n",
-		heartbeat, nowayout);
-
 	return platform_driver_register(&sh_wdt_driver);
 }
 
diff --git a/drivers/watchdog/sp5100_tco.c b/drivers/watchdog/sp5100_tco.c
index 59108e48ada..ae5e82cb83f 100644
--- a/drivers/watchdog/sp5100_tco.c
+++ b/drivers/watchdog/sp5100_tco.c
@@ -313,7 +313,7 @@ static unsigned char __devinit sp5100_tco_setupdevice(void)
 	tcobase_phys = val;
 
 	tcobase = ioremap(val, SP5100_WDT_MEM_MAP_SIZE);
-	if (tcobase == 0) {
+	if (!tcobase) {
 		pr_err("failed to get tcobase address\n");
 		goto unreg_mem_region;
 	}
diff --git a/drivers/watchdog/via_wdt.c b/drivers/watchdog/via_wdt.c
index 465e08273c9..5603e31afda 100644
--- a/drivers/watchdog/via_wdt.c
+++ b/drivers/watchdog/via_wdt.c
@@ -202,6 +202,9 @@ static int __devinit wdt_probe(struct pci_dev *pdev,
 		goto err_out_release;
 	}
 
+	if (timeout < 1 || timeout > WDT_TIMEOUT_MAX)
+		timeout = WDT_TIMEOUT;
+
 	wdt_dev.timeout = timeout;
 	watchdog_set_nowayout(&wdt_dev, nowayout);
 	if (readl(wdt_mem) & VIA_WDT_FIRED)
@@ -250,20 +253,7 @@ static struct pci_driver wdt_driver = {
 	.remove		= __devexit_p(wdt_remove),
 };
 
-static int __init wdt_init(void)
-{
-	if (timeout < 1 || timeout > WDT_TIMEOUT_MAX)
-		timeout = WDT_TIMEOUT;
-	return pci_register_driver(&wdt_driver);
-}
-
-static void __exit wdt_exit(void)
-{
-	pci_unregister_driver(&wdt_driver);
-}
-
-module_init(wdt_init);
-module_exit(wdt_exit);
+module_pci_driver(wdt_driver);
 
 MODULE_AUTHOR("Marc Vertes");
 MODULE_DESCRIPTION("Driver for watchdog timer on VIA chipset");
diff --git a/drivers/watchdog/wdt_pci.c b/drivers/watchdog/wdt_pci.c
index 1c888c7d4cc..e32654efdbb 100644
--- a/drivers/watchdog/wdt_pci.c
+++ b/drivers/watchdog/wdt_pci.c
@@ -739,39 +739,7 @@ static struct pci_driver wdtpci_driver = {
 	.remove		= __devexit_p(wdtpci_remove_one),
 };
 
-
-/**
- *	wdtpci_cleanup:
- *
- *	Unload the watchdog. You cannot do this with any file handles open.
- *	If your watchdog is set to continue ticking on close and you unload
- *	it, well it keeps ticking. We won't get the interrupt but the board
- *	will not touch PC memory so all is fine. You just have to load a new
- *	module in xx seconds or reboot.
- */
-
-static void __exit wdtpci_cleanup(void)
-{
-	pci_unregister_driver(&wdtpci_driver);
-}
-
-
-/**
- *	wdtpci_init:
- *
- *	Set up the WDT watchdog board. All we have to do is grab the
- *	resources we require and bitch if anyone beat us to them.
- *	The open() function will actually kick the board off.
- */
-
-static int __init wdtpci_init(void)
-{
-	return pci_register_driver(&wdtpci_driver);
-}
-
-
-module_init(wdtpci_init);
-module_exit(wdtpci_cleanup);
+module_pci_driver(wdtpci_driver);
 
 MODULE_AUTHOR("JP Nollmann, Alan Cox");
 MODULE_DESCRIPTION("Driver for the ICS PCI-WDT500/501 watchdog cards");
diff --git a/drivers/watchdog/wm831x_wdt.c b/drivers/watchdog/wm831x_wdt.c
index b1815c5ed7a..87d66d236c3 100644
--- a/drivers/watchdog/wm831x_wdt.c
+++ b/drivers/watchdog/wm831x_wdt.c
@@ -247,8 +247,9 @@ static int __devinit wm831x_wdt_probe(struct platform_device *pdev)
 		reg |= pdata->software << WM831X_WDOG_RST_SRC_SHIFT;
 
 		if (pdata->update_gpio) {
-			ret = gpio_request(pdata->update_gpio,
-					   "Watchdog update");
+			ret = gpio_request_one(pdata->update_gpio,
+					       GPIOF_DIR_OUT | GPIOF_INIT_LOW,
+					       "Watchdog update");
 			if (ret < 0) {
 				dev_err(wm831x->dev,
 					"Failed to request update GPIO: %d\n",
@@ -256,14 +257,6 @@ static int __devinit wm831x_wdt_probe(struct platform_device *pdev)
 				goto err;
 			}
 
-			ret = gpio_direction_output(pdata->update_gpio, 0);
-			if (ret != 0) {
-				dev_err(wm831x->dev,
-					"gpio_direction_output returned: %d\n",
-					ret);
-				goto err_gpio;
-			}
-
 			driver_data->update_gpio = pdata->update_gpio;
 
 			/* Make sure the watchdog takes hardware updates */
diff --git a/drivers/xen/Kconfig b/drivers/xen/Kconfig
index 94243136f6b..8d2501e604d 100644
--- a/drivers/xen/Kconfig
+++ b/drivers/xen/Kconfig
@@ -71,7 +71,7 @@ config XEN_DEV_EVTCHN
 	tristate "Xen /dev/xen/evtchn device"
 	default y
 	help
-	  The evtchn driver allows a userspace process to triger event
+	  The evtchn driver allows a userspace process to trigger event
 	  channels and to receive notification of an event channel
 	  firing.
 	  If in doubt, say yes.
@@ -183,15 +183,17 @@ config XEN_ACPI_PROCESSOR
 	depends on XEN && X86 && ACPI_PROCESSOR && CPU_FREQ
 	default m
 	help
-          This ACPI processor uploads Power Management information to the Xen hypervisor.
-
-	  To do that the driver parses the Power Management data and uploads said
-	  information to the Xen hypervisor. Then the Xen hypervisor can select the
-          proper Cx and Pxx states. It also registers itslef as the SMM so that
-          other drivers (such as ACPI cpufreq scaling driver) will not load.
-
-          To compile this driver as a module, choose M here: the
-          module will be called xen_acpi_processor  If you do not know what to choose,
-          select M here. If the CPUFREQ drivers are built in, select Y here.
+          This ACPI processor uploads Power Management information to the Xen
+	  hypervisor.
+
+	  To do that the driver parses the Power Management data and uploads
+	  said information to the Xen hypervisor. Then the Xen hypervisor can
+	  select the proper Cx and Pxx states. It also registers itslef as the
+	  SMM so that other drivers (such as ACPI cpufreq scaling driver) will
+	  not load.
+
+          To compile this driver as a module, choose M here: the module will be
+	  called xen_acpi_processor  If you do not know what to choose, select
+	  M here. If the CPUFREQ drivers are built in, select Y here.
 
 endmenu
diff --git a/drivers/xen/Makefile b/drivers/xen/Makefile
index 9adc5be57b1..fc348863113 100644
--- a/drivers/xen/Makefile
+++ b/drivers/xen/Makefile
@@ -17,7 +17,7 @@ obj-$(CONFIG_XEN_SYS_HYPERVISOR)	+= sys-hypervisor.o
 obj-$(CONFIG_XEN_PVHVM)			+= platform-pci.o
 obj-$(CONFIG_XEN_TMEM)			+= tmem.o
 obj-$(CONFIG_SWIOTLB_XEN)		+= swiotlb-xen.o
-obj-$(CONFIG_XEN_DOM0)			+= pci.o
+obj-$(CONFIG_XEN_DOM0)			+= pci.o acpi.o
 obj-$(CONFIG_XEN_PCIDEV_BACKEND)	+= xen-pciback/
 obj-$(CONFIG_XEN_PRIVCMD)		+= xen-privcmd.o
 obj-$(CONFIG_XEN_ACPI_PROCESSOR)	+= xen-acpi-processor.o
diff --git a/drivers/xen/acpi.c b/drivers/xen/acpi.c
new file mode 100644
index 00000000000..119d42a2bf5
--- /dev/null
+++ b/drivers/xen/acpi.c
@@ -0,0 +1,62 @@
+/******************************************************************************
+ * acpi.c
+ * acpi file for domain 0 kernel
+ *
+ * Copyright (c) 2011 Konrad Rzeszutek Wilk <konrad.wilk@oracle.com>
+ * Copyright (c) 2011 Yu Ke ke.yu@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; or, when distributed
+ * separately from the Linux kernel or incorporated into other
+ * software packages, subject to the following license:
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this source file (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
+ * 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 <xen/acpi.h>
+#include <xen/interface/platform.h>
+#include <asm/xen/hypercall.h>
+#include <asm/xen/hypervisor.h>
+
+int xen_acpi_notify_hypervisor_state(u8 sleep_state,
+				     u32 pm1a_cnt, u32 pm1b_cnt)
+{
+	struct xen_platform_op op = {
+		.cmd = XENPF_enter_acpi_sleep,
+		.interface_version = XENPF_INTERFACE_VERSION,
+		.u = {
+			.enter_acpi_sleep = {
+				.pm1a_cnt_val = (u16)pm1a_cnt,
+				.pm1b_cnt_val = (u16)pm1b_cnt,
+				.sleep_state = sleep_state,
+			},
+		},
+	};
+
+	if ((pm1a_cnt & 0xffff0000) || (pm1b_cnt & 0xffff0000)) {
+		WARN(1, "Using more than 16bits of PM1A/B 0x%x/0x%x!"
+		     "Email xen-devel@lists.xensource.com  Thank you.\n", \
+		     pm1a_cnt, pm1b_cnt);
+		return -1;
+	}
+
+	HYPERVISOR_dom0_op(&op);
+	return 1;
+}
diff --git a/drivers/xen/events.c b/drivers/xen/events.c
index 0a8a17cd80b..6908e4ce2a0 100644
--- a/drivers/xen/events.c
+++ b/drivers/xen/events.c
@@ -611,7 +611,7 @@ static void disable_pirq(struct irq_data *data)
 	disable_dynirq(data);
 }
 
-static int find_irq_by_gsi(unsigned gsi)
+int xen_irq_from_gsi(unsigned gsi)
 {
 	struct irq_info *info;
 
@@ -625,6 +625,7 @@ static int find_irq_by_gsi(unsigned gsi)
 
 	return -1;
 }
+EXPORT_SYMBOL_GPL(xen_irq_from_gsi);
 
 /*
  * Do not make any assumptions regarding the relationship between the
@@ -644,7 +645,7 @@ int xen_bind_pirq_gsi_to_irq(unsigned gsi,
 
 	mutex_lock(&irq_mapping_update_lock);
 
-	irq = find_irq_by_gsi(gsi);
+	irq = xen_irq_from_gsi(gsi);
 	if (irq != -1) {
 		printk(KERN_INFO "xen_map_pirq_gsi: returning irq %d for gsi %u\n",
 		       irq, gsi);
diff --git a/drivers/xen/grant-table.c b/drivers/xen/grant-table.c
index f100ce20b16..0bfc1ef1125 100644
--- a/drivers/xen/grant-table.c
+++ b/drivers/xen/grant-table.c
@@ -38,6 +38,7 @@
 #include <linux/vmalloc.h>
 #include <linux/uaccess.h>
 #include <linux/io.h>
+#include <linux/hardirq.h>
 
 #include <xen/xen.h>
 #include <xen/interface/xen.h>
@@ -426,10 +427,8 @@ static int gnttab_end_foreign_access_ref_v1(grant_ref_t ref, int readonly)
 	nflags = *pflags;
 	do {
 		flags = nflags;
-		if (flags & (GTF_reading|GTF_writing)) {
-			printk(KERN_ALERT "WARNING: g.e. still in use!\n");
+		if (flags & (GTF_reading|GTF_writing))
 			return 0;
-		}
 	} while ((nflags = sync_cmpxchg(pflags, flags, 0)) != flags);
 
 	return 1;
@@ -458,12 +457,103 @@ static int gnttab_end_foreign_access_ref_v2(grant_ref_t ref, int readonly)
 	return 1;
 }
 
-int gnttab_end_foreign_access_ref(grant_ref_t ref, int readonly)
+static inline int _gnttab_end_foreign_access_ref(grant_ref_t ref, int readonly)
 {
 	return gnttab_interface->end_foreign_access_ref(ref, readonly);
 }
+
+int gnttab_end_foreign_access_ref(grant_ref_t ref, int readonly)
+{
+	if (_gnttab_end_foreign_access_ref(ref, readonly))
+		return 1;
+	pr_warn("WARNING: g.e. %#x still in use!\n", ref);
+	return 0;
+}
 EXPORT_SYMBOL_GPL(gnttab_end_foreign_access_ref);
 
+struct deferred_entry {
+	struct list_head list;
+	grant_ref_t ref;
+	bool ro;
+	uint16_t warn_delay;
+	struct page *page;
+};
+static LIST_HEAD(deferred_list);
+static void gnttab_handle_deferred(unsigned long);
+static DEFINE_TIMER(deferred_timer, gnttab_handle_deferred, 0, 0);
+
+static void gnttab_handle_deferred(unsigned long unused)
+{
+	unsigned int nr = 10;
+	struct deferred_entry *first = NULL;
+	unsigned long flags;
+
+	spin_lock_irqsave(&gnttab_list_lock, flags);
+	while (nr--) {
+		struct deferred_entry *entry
+			= list_first_entry(&deferred_list,
+					   struct deferred_entry, list);
+
+		if (entry == first)
+			break;
+		list_del(&entry->list);
+		spin_unlock_irqrestore(&gnttab_list_lock, flags);
+		if (_gnttab_end_foreign_access_ref(entry->ref, entry->ro)) {
+			put_free_entry(entry->ref);
+			if (entry->page) {
+				pr_debug("freeing g.e. %#x (pfn %#lx)\n",
+					 entry->ref, page_to_pfn(entry->page));
+				__free_page(entry->page);
+			} else
+				pr_info("freeing g.e. %#x\n", entry->ref);
+			kfree(entry);
+			entry = NULL;
+		} else {
+			if (!--entry->warn_delay)
+				pr_info("g.e. %#x still pending\n",
+					entry->ref);
+			if (!first)
+				first = entry;
+		}
+		spin_lock_irqsave(&gnttab_list_lock, flags);
+		if (entry)
+			list_add_tail(&entry->list, &deferred_list);
+		else if (list_empty(&deferred_list))
+			break;
+	}
+	if (!list_empty(&deferred_list) && !timer_pending(&deferred_timer)) {
+		deferred_timer.expires = jiffies + HZ;
+		add_timer(&deferred_timer);
+	}
+	spin_unlock_irqrestore(&gnttab_list_lock, flags);
+}
+
+static void gnttab_add_deferred(grant_ref_t ref, bool readonly,
+				struct page *page)
+{
+	struct deferred_entry *entry = kmalloc(sizeof(*entry), GFP_ATOMIC);
+	const char *what = KERN_WARNING "leaking";
+
+	if (entry) {
+		unsigned long flags;
+
+		entry->ref = ref;
+		entry->ro = readonly;
+		entry->page = page;
+		entry->warn_delay = 60;
+		spin_lock_irqsave(&gnttab_list_lock, flags);
+		list_add_tail(&entry->list, &deferred_list);
+		if (!timer_pending(&deferred_timer)) {
+			deferred_timer.expires = jiffies + HZ;
+			add_timer(&deferred_timer);
+		}
+		spin_unlock_irqrestore(&gnttab_list_lock, flags);
+		what = KERN_DEBUG "deferring";
+	}
+	printk("%s g.e. %#x (pfn %#lx)\n",
+	       what, ref, page ? page_to_pfn(page) : -1);
+}
+
 void gnttab_end_foreign_access(grant_ref_t ref, int readonly,
 			       unsigned long page)
 {
@@ -471,12 +561,9 @@ void gnttab_end_foreign_access(grant_ref_t ref, int readonly,
 		put_free_entry(ref);
 		if (page != 0)
 			free_page(page);
-	} else {
-		/* XXX This needs to be fixed so that the ref and page are
-		   placed on a list to be freed up later. */
-		printk(KERN_WARNING
-		       "WARNING: leaking g.e. and page still in use!\n");
-	}
+	} else
+		gnttab_add_deferred(ref, readonly,
+				    page ? virt_to_page(page) : NULL);
 }
 EXPORT_SYMBOL_GPL(gnttab_end_foreign_access);
 
@@ -741,6 +828,7 @@ int gnttab_map_refs(struct gnttab_map_grant_ref *map_ops,
 		    struct page **pages, unsigned int count)
 {
 	int i, ret;
+	bool lazy = false;
 	pte_t *pte;
 	unsigned long mfn;
 
@@ -751,6 +839,11 @@ int gnttab_map_refs(struct gnttab_map_grant_ref *map_ops,
 	if (xen_feature(XENFEAT_auto_translated_physmap))
 		return ret;
 
+	if (!in_interrupt() && paravirt_get_lazy_mode() == PARAVIRT_LAZY_NONE) {
+		arch_enter_lazy_mmu_mode();
+		lazy = true;
+	}
+
 	for (i = 0; i < count; i++) {
 		/* Do not add to override if the map failed. */
 		if (map_ops[i].status)
@@ -769,6 +862,9 @@ int gnttab_map_refs(struct gnttab_map_grant_ref *map_ops,
 			return ret;
 	}
 
+	if (lazy)
+		arch_leave_lazy_mmu_mode();
+
 	return ret;
 }
 EXPORT_SYMBOL_GPL(gnttab_map_refs);
@@ -777,6 +873,7 @@ int gnttab_unmap_refs(struct gnttab_unmap_grant_ref *unmap_ops,
 		      struct page **pages, unsigned int count, bool clear_pte)
 {
 	int i, ret;
+	bool lazy = false;
 
 	ret = HYPERVISOR_grant_table_op(GNTTABOP_unmap_grant_ref, unmap_ops, count);
 	if (ret)
@@ -785,12 +882,20 @@ int gnttab_unmap_refs(struct gnttab_unmap_grant_ref *unmap_ops,
 	if (xen_feature(XENFEAT_auto_translated_physmap))
 		return ret;
 
+	if (!in_interrupt() && paravirt_get_lazy_mode() == PARAVIRT_LAZY_NONE) {
+		arch_enter_lazy_mmu_mode();
+		lazy = true;
+	}
+
 	for (i = 0; i < count; i++) {
 		ret = m2p_remove_override(pages[i], clear_pte);
 		if (ret)
 			return ret;
 	}
 
+	if (lazy)
+		arch_leave_lazy_mmu_mode();
+
 	return ret;
 }
 EXPORT_SYMBOL_GPL(gnttab_unmap_refs);
diff --git a/drivers/xen/xen-selfballoon.c b/drivers/xen/xen-selfballoon.c
index 146c9489701..7d041cb6da2 100644
--- a/drivers/xen/xen-selfballoon.c
+++ b/drivers/xen/xen-selfballoon.c
@@ -105,6 +105,12 @@ static unsigned int selfballoon_interval __read_mostly = 5;
  */
 static unsigned int selfballoon_min_usable_mb;
 
+/*
+ * Amount of RAM in MB to add to the target number of pages.
+ * Can be used to reserve some more room for caches and the like.
+ */
+static unsigned int selfballoon_reserved_mb;
+
 static void selfballoon_process(struct work_struct *work);
 static DECLARE_DELAYED_WORK(selfballoon_worker, selfballoon_process);
 
@@ -217,7 +223,8 @@ static void selfballoon_process(struct work_struct *work)
 		cur_pages = totalram_pages;
 		tgt_pages = cur_pages; /* default is no change */
 		goal_pages = percpu_counter_read_positive(&vm_committed_as) +
-				totalreserve_pages;
+				totalreserve_pages +
+				MB2PAGES(selfballoon_reserved_mb);
 #ifdef CONFIG_FRONTSWAP
 		/* allow space for frontswap pages to be repatriated */
 		if (frontswap_selfshrinking && frontswap_enabled)
@@ -397,6 +404,30 @@ static DEVICE_ATTR(selfballoon_min_usable_mb, S_IRUGO | S_IWUSR,
 		   show_selfballoon_min_usable_mb,
 		   store_selfballoon_min_usable_mb);
 
+SELFBALLOON_SHOW(selfballoon_reserved_mb, "%d\n",
+				selfballoon_reserved_mb);
+
+static ssize_t store_selfballoon_reserved_mb(struct device *dev,
+					     struct device_attribute *attr,
+					     const char *buf,
+					     size_t count)
+{
+	unsigned long val;
+	int err;
+
+	if (!capable(CAP_SYS_ADMIN))
+		return -EPERM;
+	err = strict_strtoul(buf, 10, &val);
+	if (err || val == 0)
+		return -EINVAL;
+	selfballoon_reserved_mb = val;
+	return count;
+}
+
+static DEVICE_ATTR(selfballoon_reserved_mb, S_IRUGO | S_IWUSR,
+		   show_selfballoon_reserved_mb,
+		   store_selfballoon_reserved_mb);
+
 
 #ifdef CONFIG_FRONTSWAP
 SELFBALLOON_SHOW(frontswap_selfshrinking, "%d\n", frontswap_selfshrinking);
@@ -480,6 +511,7 @@ static struct attribute *selfballoon_attrs[] = {
 	&dev_attr_selfballoon_downhysteresis.attr,
 	&dev_attr_selfballoon_uphysteresis.attr,
 	&dev_attr_selfballoon_min_usable_mb.attr,
+	&dev_attr_selfballoon_reserved_mb.attr,
 #ifdef CONFIG_FRONTSWAP
 	&dev_attr_frontswap_selfshrinking.attr,
 	&dev_attr_frontswap_hysteresis.attr,
diff --git a/drivers/xen/xenbus/xenbus_comms.c b/drivers/xen/xenbus/xenbus_comms.c
index 2eff7a6aaa2..52fe7ad0766 100644
--- a/drivers/xen/xenbus/xenbus_comms.c
+++ b/drivers/xen/xenbus/xenbus_comms.c
@@ -234,3 +234,9 @@ int xb_init_comms(void)
 
 	return 0;
 }
+
+void xb_deinit_comms(void)
+{
+	unbind_from_irqhandler(xenbus_irq, &xb_waitq);
+	xenbus_irq = 0;
+}
diff --git a/drivers/xen/xenbus/xenbus_comms.h b/drivers/xen/xenbus/xenbus_comms.h
index 6e42800fa49..c8abd3b8a6c 100644
--- a/drivers/xen/xenbus/xenbus_comms.h
+++ b/drivers/xen/xenbus/xenbus_comms.h
@@ -35,6 +35,7 @@
 
 int xs_init(void);
 int xb_init_comms(void);
+void xb_deinit_comms(void);
 
 /* Low level routines. */
 int xb_write(const void *data, unsigned len);
diff --git a/drivers/xen/xenbus/xenbus_dev_backend.c b/drivers/xen/xenbus/xenbus_dev_backend.c
index 3d3be78c109..be738c43104 100644
--- a/drivers/xen/xenbus/xenbus_dev_backend.c
+++ b/drivers/xen/xenbus/xenbus_dev_backend.c
@@ -8,7 +8,11 @@
 
 #include <xen/xen.h>
 #include <xen/page.h>
+#include <xen/xenbus.h>
 #include <xen/xenbus_dev.h>
+#include <xen/grant_table.h>
+#include <xen/events.h>
+#include <asm/xen/hypervisor.h>
 
 #include "xenbus_comms.h"
 
@@ -22,6 +26,50 @@ static int xenbus_backend_open(struct inode *inode, struct file *filp)
 	return nonseekable_open(inode, filp);
 }
 
+static long xenbus_alloc(domid_t domid)
+{
+	struct evtchn_alloc_unbound arg;
+	int err = -EEXIST;
+
+	xs_suspend();
+
+	/* If xenstored_ready is nonzero, that means we have already talked to
+	 * xenstore and set up watches. These watches will be restored by
+	 * xs_resume, but that requires communication over the port established
+	 * below that is not visible to anyone until the ioctl returns.
+	 *
+	 * This can be resolved by splitting the ioctl into two parts
+	 * (postponing the resume until xenstored is active) but this is
+	 * unnecessarily complex for the intended use where xenstored is only
+	 * started once - so return -EEXIST if it's already running.
+	 */
+	if (xenstored_ready)
+		goto out_err;
+
+	gnttab_grant_foreign_access_ref(GNTTAB_RESERVED_XENSTORE, domid,
+			virt_to_mfn(xen_store_interface), 0 /* writable */);
+
+	arg.dom = DOMID_SELF;
+	arg.remote_dom = domid;
+
+	err = HYPERVISOR_event_channel_op(EVTCHNOP_alloc_unbound, &arg);
+	if (err)
+		goto out_err;
+
+	if (xen_store_evtchn > 0)
+		xb_deinit_comms();
+
+	xen_store_evtchn = arg.port;
+
+	xs_resume();
+
+	return arg.port;
+
+ out_err:
+	xs_suspend_cancel();
+	return err;
+}
+
 static long xenbus_backend_ioctl(struct file *file, unsigned int cmd, unsigned long data)
 {
 	if (!capable(CAP_SYS_ADMIN))
@@ -33,6 +81,9 @@ static long xenbus_backend_ioctl(struct file *file, unsigned int cmd, unsigned l
 				return xen_store_evtchn;
 			return -ENODEV;
 
+		case IOCTL_XENBUS_BACKEND_SETUP:
+			return xenbus_alloc(data);
+
 		default:
 			return -ENOTTY;
 	}